From 5358b26c3e40e648e6cc7446791ca490eeda3d6a Mon Sep 17 00:00:00 2001
From: Awesomeplayer165 <dev@jacob2.dev>
Date: Thu, 8 Feb 2024 16:13:15 -0800
Subject: [PATCH 01/62] add AutoChooser class

---
 src/main/java/frc/robot/AutoChooser.java | 34 ++++++++++++++++++++++++
 1 file changed, 34 insertions(+)
 create mode 100644 src/main/java/frc/robot/AutoChooser.java

diff --git a/src/main/java/frc/robot/AutoChooser.java b/src/main/java/frc/robot/AutoChooser.java
new file mode 100644
index 00000000..9c64955c
--- /dev/null
+++ b/src/main/java/frc/robot/AutoChooser.java
@@ -0,0 +1,34 @@
+package frc.robot;
+
+import java.util.function.Supplier;
+import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
+import edu.wpi.first.wpilibj2.command.Command;
+
+import frc.robot.subsystems.elevator.ElevatorSubsystem;
+import frc.robot.subsystems.feeder.FeederSubsystem;
+import frc.robot.subsystems.intake.IntakeSubsystem;
+import frc.robot.subsystems.swerve.SwerveSubsystem;
+import frc.robot.subsystems.shooter.ShooterSubystem;
+
+public final class AutoChooser {
+    LoggedDashboardChooser<Supplier<Command>> chooser = new LoggedDashboardChooser<Supplier<Command>>(
+            "Auto Chooser");
+
+    SwerveSubsystem swerveSubsystem;
+    IntakeSubsystem intakeSubsystem;
+    ElevatorSubsystem elevatorSubsystem;
+    ShooterSubystem shooterSubsystem;
+    FeederSubsystem feederSubsystem;
+
+    public AutoChooser(SwerveSubsystem swerveSubsystem,
+                      IntakeSubsystem intakeSubsystem,
+                      ElevatorSubsystem elevatorSubsystem,
+                      ShooterSubystem shooterSubsystem,
+                      FeederSubsystem feederSubsystem) {
+        this.swerveSubsystem = swerveSubsystem;
+        this.intakeSubsystem = intakeSubsystem;
+        this.elevatorSubsystem = elevatorSubsystem;
+        this.shooterSubsystem = shooterSubsystem;
+        this.feederSubsystem = feederSubsystem;
+    }
+}

From 4c989804c0aae51b92b496be4fab1ce5fb592794 Mon Sep 17 00:00:00 2001
From: stoopipoopy <anotherguywithabeanie@gmail.com>
Date: Thu, 8 Feb 2024 16:30:32 -0800
Subject: [PATCH 02/62] Add PathPlanner's AutoChooser

---
 ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat | Bin 0 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat | Bin 0 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat | Bin 0 -> 2048 bytes
 simgui.json                                   |   9 +++++
 src/main/java/frc/robot/AutoChooser.java      |  34 ------------------
 src/main/java/frc/robot/AutoManager.java      |  34 ++++++++++++++++++
 src/main/java/frc/robot/Robot.java            |   7 ++++
 7 files changed, 50 insertions(+), 34 deletions(-)
 create mode 100644 ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat
 create mode 100644 ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat
 create mode 100644 ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat
 delete mode 100644 src/main/java/frc/robot/AutoChooser.java
 create mode 100644 src/main/java/frc/robot/AutoManager.java

diff --git a/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..8fe706c8b77d3cf5bd093459330c15f7d18eee60
GIT binary patch
literal 2048
zcmcgrPe_wt7=PU|6e2rF8Xhc&v?-;@vW#?t3OiT}23-;)azr-iU^ztS6DxMC4k6TG
z#|)uC9SVXzFvxPyr9w!;KrFfmo2Ihrd;a{~_C*I-^ul<*=XrmBp67YreJ~A{`R~i>
zFou4`uaCxO6|b^Ug@0H7w0W7mR<cq596n`UWMf6}Jt{Rh?!T~l-I$zIBgb%R)(?B2
zZEeyafw$WmO;H>wksbUN#ZyeWu}tmrwDZ01-RZE;pW`dcjLh18!ns$eCTOQ~_-^yc
z!A<Ya0v1YxKlHxcGX2C4B=&c!4x|_|I5sy(dHccVMe4`lw@`dT^}&Z9`OrD~J#t`i
zYkrJ6;NSP`3+taX6`UHy!O(G@r!g`=%(?5S;0Z?c*wqo9v(dhdk@%HI_Vkj>_qOWV
zTO>cHb-@lhaB)pI7P_R4m1WT@*X-*js7LE?^3*T*1f#f9@t6yRIc?|uzweE2c5=xT
z(GL(HZy{fXoc6nreS4Mk*Vp<SB(pPmlIQqd2Uy=<7*EvsgW%IUyc?e9`>09zxSpPw
zI_ASSJiLj^Zc?4(CDF6B?kSg_TOW0T-?>NpF)!!^yQ7(7Ka81L=w*LPb!U846;yBR
zviJtH4)}wQdcj9O@TYxr$DjIEtK1{{2HqWq^ztwd=wtVkdLPjO|8INsczeP9<j;$G
zAboo0@WH&?K6lQ^yx@bkb!b<b|9{9=>PKi*-dX6O_ycd&BoF-jfbhYio#u}ics#q2
zwrPHk*1a*7c-$cOTc4-fb-t$~arhMXeXvRJ@JrnnzfUJ7&IFz0(JK<W_2R16-k9P3
otnJIZ|I+_LATh-H(3@4g<VAT;cBSAGkqG0;zDoX0?Go$!2CFSodH?_b

literal 0
HcmV?d00001

diff --git a/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..8f800a805369a9ba570f676cbf1607af5edc8e99
GIT binary patch
literal 2048
zcmcgsO-Phc6ur)56oj^kjGz?6sVT}IrXM4nfdX4t6AeU=AW<RONDIp$f<CdrWwj8&
z7H*1+sDXkCf<914a==ZC2_lR{s+}<7FH?Q@-FxQei#AU5K;C!nyXTyH&wJxXs<G(5
zFRPh||74ZmV-<UneAX&pMMC~v|C97KbF~Nq67(h$D+2FPC&oqt=a+8~<*0U>7)I6U
zhuzb-GUkAQtM*0{9EVF}2fjh;IcDBidi(2?^Sx*4M8qG+;kAtVKG^}TbFWfO@XqJZ
z-RPBrTi%}qD6|HB@O`^Y-HZ>8>}^vXP??h6q3K@E+xI`u@jMQ`f#aLX4?Of(htDzZ
zq58S9{1|;ezw6Z(!+)B}O{yOU*Nz%LiNrtn-1<W5xs3a<D=r$JN#`b#c-$j<YF_-k
zqrCP8$xmxvki(9;0Zlj(x}=Y#1>sw-*>}&;kM`l@d0ybTjN=xqhc6uFxSjj|v2$Rp
zlZ!72e~=6E7V_(m(|Knz@2@a_c4d8#Ol9<#@$rKixTCu;e&P#+faiC3FEV59V{5{1
z`e{%43?90X>uXHgwaOFk6h2$~o^a{8`OzovEj6MKzrYu2i>8k(5>3orF#4AACjFbs
zx!>3&(G6-J&<7s<0*`s1&wKxtKl!cwa*voB_3k*Vmj^$<kKI%1eas8^zwH&HO$GOp
z?-$R&{QS<L1Harlch8Dn(81d}P?a*jKjb&*M|fG@S@@CYqn=hIkNTM&!2`!T`;Qp)
zcy@h_ljc2U`IM;4mj7=Yww`Nm9yxg2%)PHx>R)Oda6yd^Ulx69aL8ny?b7$+>J#<9
p^tTZ2kav?97W|pTrzb-WL+AH&fgh-wS~&If!SHzH!)^2C{5KY)LUjND

literal 0
HcmV?d00001

diff --git a/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..18ebfe414687be754b11316a3201722f03ad32fd
GIT binary patch
literal 2048
zcmdT@O-NKx6u!=A6hyX3jTRb2+9Wk4Q#p~&K*6-ICK_myU{VUzNDIrsf<~;EWwi*Q
z7PHI<5(+8^dSK9$11-vM5n&(_?SvUkWvX|6KIhG=Sp`8C9_OBO&-Z=joO|D->Ar0K
z`AW7ML!Yb)Y#cUYOr^suyZvZR{nzzRn3si1e;Pp9j!O@+=0zd#7kH1-jgJOSFI_dJ
zNY%_f)OP8{>JKiD+aTb!c;i{}qZML-Z_#}6i8qn2f1b1VJKi0Q`2t0}rkSBnR)F%{
zt4af8=^VQ2y>f8f`=bPf(x4B%uQyB|;X~=2oyr3$P8k@P9-z2&@ADk>W8+)Mzoz`a
zLyvUm9R2R!J-5#3qYmhwd-jF-Pnc>B4dY<&5YN*XnIGod@kH_|jq0)LdU(!8`!+`W
zS01ssd71A`<+avId|K;*94zuu8n7jFNgay|!na1FuOFixt;3E}zra%(`4P>>T*xhQ
z+xP!%TWXaZ8(S3q5G6Pk;;RwUerF4BFB1RCTA71H8sdj}jvsV@O?{>Q!^S`uczTDo
zV>5goo3cKxr)#Q_@z9N3spPW#$}@IW_$;kE<I;2UqfX!>TSXu90$;c@o<I2An9S@M
z)=w($l&`j$>P?&%-H_G+ec(|q@aPBnWQRBY&Y#>X_lUlc@Ajj5d6)<IVLese2eg1M
zXX{4WO76!wFY1B#>77Fd^K$FlIw|vl4&K(j%{l)65U<fqbV=S>^uFjL-z-ZU`7`~3
z2aYVwA3pN&?1q99{2r5cMwsH!cDdgs2X9ArS9`kk2=~3mFZoYZFnd?@T4UK`VLNu<
yg7|K}l=8JT=ea+tW`XTb{TD*%LFNZvQ2G3mJsh_-NPffnUiKr$#lESZXP#dXHB}k_

literal 0
HcmV?d00001

diff --git a/simgui.json b/simgui.json
index 147d7de3..d87b1e2a 100644
--- a/simgui.json
+++ b/simgui.json
@@ -1,14 +1,23 @@
 {
   "NTProvider": {
     "types": {
+      "/AdvantageKit/RealOutputs/Elevator/Mechanism2d": "Mechanism2d",
       "/AdvantageKit/RealOutputs/Shooter/Mechanism2d": "Mechanism2d",
       "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Auto Chooser": "String Chooser",
       "/SmartDashboard/VisionSystemSim-Left Camera Sim System/Sim Field": "Field2d",
       "/SmartDashboard/VisionSystemSim-Left Camera/Sim Field": "Field2d",
       "/SmartDashboard/VisionSystemSim-Right Camera Sim System/Sim Field": "Field2d",
       "/SmartDashboard/VisionSystemSim-Right Camera/Sim Field": "Field2d",
       "/SmartDashboard/reset Kicker value": "Command",
       "/SmartDashboard/reset Pivot value": "Command"
+    },
+    "windows": {
+      "/SmartDashboard/Auto Chooser": {
+        "window": {
+          "visible": true
+        }
+      }
     }
   },
   "NetworkTables": {
diff --git a/src/main/java/frc/robot/AutoChooser.java b/src/main/java/frc/robot/AutoChooser.java
deleted file mode 100644
index 9c64955c..00000000
--- a/src/main/java/frc/robot/AutoChooser.java
+++ /dev/null
@@ -1,34 +0,0 @@
-package frc.robot;
-
-import java.util.function.Supplier;
-import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
-import edu.wpi.first.wpilibj2.command.Command;
-
-import frc.robot.subsystems.elevator.ElevatorSubsystem;
-import frc.robot.subsystems.feeder.FeederSubsystem;
-import frc.robot.subsystems.intake.IntakeSubsystem;
-import frc.robot.subsystems.swerve.SwerveSubsystem;
-import frc.robot.subsystems.shooter.ShooterSubystem;
-
-public final class AutoChooser {
-    LoggedDashboardChooser<Supplier<Command>> chooser = new LoggedDashboardChooser<Supplier<Command>>(
-            "Auto Chooser");
-
-    SwerveSubsystem swerveSubsystem;
-    IntakeSubsystem intakeSubsystem;
-    ElevatorSubsystem elevatorSubsystem;
-    ShooterSubystem shooterSubsystem;
-    FeederSubsystem feederSubsystem;
-
-    public AutoChooser(SwerveSubsystem swerveSubsystem,
-                      IntakeSubsystem intakeSubsystem,
-                      ElevatorSubsystem elevatorSubsystem,
-                      ShooterSubystem shooterSubsystem,
-                      FeederSubsystem feederSubsystem) {
-        this.swerveSubsystem = swerveSubsystem;
-        this.intakeSubsystem = intakeSubsystem;
-        this.elevatorSubsystem = elevatorSubsystem;
-        this.shooterSubsystem = shooterSubsystem;
-        this.feederSubsystem = feederSubsystem;
-    }
-}
diff --git a/src/main/java/frc/robot/AutoManager.java b/src/main/java/frc/robot/AutoManager.java
new file mode 100644
index 00000000..8bff14e3
--- /dev/null
+++ b/src/main/java/frc/robot/AutoManager.java
@@ -0,0 +1,34 @@
+package frc.robot;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.elevator.ElevatorSubsystem;
+import frc.robot.subsystems.feeder.FeederSubsystem;
+import frc.robot.subsystems.intake.IntakeSubsystem;
+import frc.robot.subsystems.shooter.ShooterSubystem;
+import frc.robot.subsystems.swerve.SwerveSubsystem;
+import java.util.function.Supplier;
+import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
+
+public final class AutoManager {
+  LoggedDashboardChooser<Supplier<Command>> chooser =
+      new LoggedDashboardChooser<Supplier<Command>>("Auto Chooser");
+
+  SwerveSubsystem swerveSubsystem;
+  IntakeSubsystem intakeSubsystem;
+  ElevatorSubsystem elevatorSubsystem;
+  ShooterSubystem shooterSubsystem;
+  FeederSubsystem feederSubsystem;
+
+  public AutoManager(
+      SwerveSubsystem swerveSubsystem,
+      IntakeSubsystem intakeSubsystem,
+      ElevatorSubsystem elevatorSubsystem,
+      ShooterSubystem shooterSubsystem,
+      FeederSubsystem feederSubsystem) {
+    this.swerveSubsystem = swerveSubsystem;
+    this.intakeSubsystem = intakeSubsystem;
+    this.elevatorSubsystem = elevatorSubsystem;
+    this.shooterSubsystem = shooterSubsystem;
+    this.feederSubsystem = feederSubsystem;
+  }
+}
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 8097ad5f..e84dd903 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -4,12 +4,14 @@
 
 package frc.robot;
 
+import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.commands.PathPlannerAuto;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -136,10 +138,15 @@ public void robotInit() {
             swerve.pointTowardsTranslation(
                 () -> -controller.getLeftY() * SwerveSubsystem.MAX_LINEAR_SPEED,
                 () -> -controller.getLeftX() * SwerveSubsystem.MAX_LINEAR_SPEED));
+
     // Test binding for elevator
     controller.b().whileTrue(elevator.setExtensionCmd(() -> 1.0));
 
     NamedCommands.registerCommand("stop", swerve.stopWithXCmd().asProxy());
+
+    // Auto Chooser
+
+    SmartDashboard.putData("Auto Chooser", AutoBuilder.buildAutoChooser());
   }
 
   @Override

From b0d3ce85584ceb2ac28d1774e84c7e6c730a173a Mon Sep 17 00:00:00 2001
From: stoopipoopy <anotherguywithabeanie@gmail.com>
Date: Thu, 8 Feb 2024 17:49:55 -0800
Subject: [PATCH 03/62] abstract chooser to autoManager, add NamedCommand
 convert local 4 autos race to deadlines

---
 .vscode/settings.json                         |   3 +-
 ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 simgui.json                                   |   3 +-
 .../deploy/pathplanner/autos/local 4.auto     |  57 +++---------------
 src/main/deploy/pathplanner/navgrid.json      |   2 +-
 src/main/java/frc/robot/AutoManager.java      |  14 +++--
 src/main/java/frc/robot/Robot.java            |  22 ++-----
 .../subsystems/swerve/SwerveSubsystem.java    |   1 -
 10 files changed, 29 insertions(+), 73 deletions(-)

diff --git a/.vscode/settings.json b/.vscode/settings.json
index 6e590aac..c79a5a6c 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -27,5 +27,6 @@
   ],
   "java.test.defaultConfig": "WPIlibUnitTests",
   "java.compile.nullAnalysis.mode": "automatic",
-  "wpilib.autoStartRioLog": true
+  "wpilib.autoStartRioLog": true,
+  "java.debug.settings.onBuildFailureProceed": true
 }
diff --git a/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat
index 8fe706c8b77d3cf5bd093459330c15f7d18eee60..cf540f36cf43283c2e796672c8adcfe17aaebcfe 100644
GIT binary patch
delta 55
zcmZn=Xb@nz$)MgaQ80C)0`J5!FP2*jY6u?NQHG0Ovp6RTTCp8tIQKON#9G9}1^@x2
B62AZd

delta 55
zcmZn=Xb@nz%AnmaQ80C)0`J5!FP3WzS_mH7X@>oO%NQpLTCtsB*z>mn#9G9}1^@w_
B62AZd

diff --git a/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat
index 8f800a805369a9ba570f676cbf1607af5edc8e99..f73fac0924be3a7d77130abbe2ef5c3a9965013a 100644
GIT binary patch
delta 55
zcmZn=Xb@nz&Y;mSQ80C)0`J5!FP3Wzng|}-QHG0Ovp6RTTCp8tIQKON#9G9}1^@v^
B61@Nb

delta 55
zcmZn=Xb@nz$)MISQ80C)0`J5!FP0k&>Ifd&X@>oO%NQpLTCtsB*z>mn#9G9}1^@w9
B61@Nb

diff --git a/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat
index 18ebfe414687be754b11316a3201722f03ad32fd..aeead252ecff15156b97ea07266cd015f82ea35b 100644
GIT binary patch
delta 55
zcmZn=Xb@nz$q-jPQ80C)0`J5!FP2*ju?Qa9QHG0Ovp6RTTCp8tIQKON#9G9}1^^MW
B6C(fs

delta 55
zcmZn=Xb@nz%8*n&Q80C)0`J5!FP3Wzi3lFsX@>oO%NQpLTCtsB*z>mn#9G9}1^^MO
B6C(fs

diff --git a/simgui.json b/simgui.json
index d87b1e2a..e063301d 100644
--- a/simgui.json
+++ b/simgui.json
@@ -5,6 +5,7 @@
       "/AdvantageKit/RealOutputs/Shooter/Mechanism2d": "Mechanism2d",
       "/FMSInfo": "FMSInfo",
       "/SmartDashboard/Auto Chooser": "String Chooser",
+      "/SmartDashboard/SendableChooser[0]": "String Chooser",
       "/SmartDashboard/VisionSystemSim-Left Camera Sim System/Sim Field": "Field2d",
       "/SmartDashboard/VisionSystemSim-Left Camera/Sim Field": "Field2d",
       "/SmartDashboard/VisionSystemSim-Right Camera Sim System/Sim Field": "Field2d",
@@ -13,7 +14,7 @@
       "/SmartDashboard/reset Pivot value": "Command"
     },
     "windows": {
-      "/SmartDashboard/Auto Chooser": {
+      "/SmartDashboard/SendableChooser[0]": {
         "window": {
           "visible": true
         }
diff --git a/src/main/deploy/pathplanner/autos/local 4.auto b/src/main/deploy/pathplanner/autos/local 4.auto
index c5819b58..6279b00d 100644
--- a/src/main/deploy/pathplanner/autos/local 4.auto	
+++ b/src/main/deploy/pathplanner/autos/local 4.auto	
@@ -18,7 +18,7 @@
           }
         },
         {
-          "type": "race",
+          "type": "deadline",
           "data": {
             "commands": [
               {
@@ -28,22 +28,9 @@
                 }
               },
               {
-                "type": "race",
+                "type": "named",
                 "data": {
-                  "commands": [
-                    {
-                      "type": "named",
-                      "data": {
-                        "name": "intake"
-                      }
-                    },
-                    {
-                      "type": "wait",
-                      "data": {
-                        "waitTime": 2.0
-                      }
-                    }
-                  ]
+                  "name": "intake"
                 }
               }
             ]
@@ -56,7 +43,7 @@
           }
         },
         {
-          "type": "race",
+          "type": "deadline",
           "data": {
             "commands": [
               {
@@ -66,22 +53,9 @@
                 }
               },
               {
-                "type": "race",
+                "type": "named",
                 "data": {
-                  "commands": [
-                    {
-                      "type": "named",
-                      "data": {
-                        "name": "intake"
-                      }
-                    },
-                    {
-                      "type": "wait",
-                      "data": {
-                        "waitTime": 1.5
-                      }
-                    }
-                  ]
+                  "name": "intake"
                 }
               }
             ]
@@ -94,7 +68,7 @@
           }
         },
         {
-          "type": "race",
+          "type": "deadline",
           "data": {
             "commands": [
               {
@@ -104,22 +78,9 @@
                 }
               },
               {
-                "type": "race",
+                "type": "named",
                 "data": {
-                  "commands": [
-                    {
-                      "type": "named",
-                      "data": {
-                        "name": "intake"
-                      }
-                    },
-                    {
-                      "type": "wait",
-                      "data": {
-                        "waitTime": 1.5
-                      }
-                    }
-                  ]
+                  "name": "intake"
                 }
               }
             ]
diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
index bab0da93..893b9e28 100644
--- a/src/main/deploy/pathplanner/navgrid.json
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -1 +1 @@
-{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
+{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/AutoManager.java b/src/main/java/frc/robot/AutoManager.java
index 8bff14e3..7ccc489b 100644
--- a/src/main/java/frc/robot/AutoManager.java
+++ b/src/main/java/frc/robot/AutoManager.java
@@ -1,17 +1,18 @@
 package frc.robot;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
 import frc.robot.subsystems.feeder.FeederSubsystem;
 import frc.robot.subsystems.intake.IntakeSubsystem;
 import frc.robot.subsystems.shooter.ShooterSubystem;
 import frc.robot.subsystems.swerve.SwerveSubsystem;
-import java.util.function.Supplier;
-import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
 public final class AutoManager {
-  LoggedDashboardChooser<Supplier<Command>> chooser =
-      new LoggedDashboardChooser<Supplier<Command>>("Auto Chooser");
+  SendableChooser<Command> chooser = AutoBuilder.buildAutoChooser();
 
   SwerveSubsystem swerveSubsystem;
   IntakeSubsystem intakeSubsystem;
@@ -30,5 +31,10 @@ public AutoManager(
     this.elevatorSubsystem = elevatorSubsystem;
     this.shooterSubsystem = shooterSubsystem;
     this.feederSubsystem = feederSubsystem;
+
+    NamedCommands.registerCommand("intake", intakeSubsystem.getDefaultCommand());
+    NamedCommands.registerCommand("fender", shooterSubsystem.getDefaultCommand());
+
+    SmartDashboard.putData(chooser);
   }
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index e84dd903..758fad41 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -4,15 +4,11 @@
 
 package frc.robot;
 
-import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
-import com.pathplanner.lib.commands.PathPlannerAuto;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -43,7 +39,6 @@ public static enum RobotMode {
   }
 
   public static final RobotMode mode = Robot.isReal() ? RobotMode.REAL : RobotMode.SIM;
-  private Command autonomousCommand;
 
   private final CommandXboxController controller = new CommandXboxController(0);
 
@@ -68,6 +63,9 @@ public void setYaw(Rotation2d yaw) {}
   private final ShooterSubystem shooter =
       new ShooterSubystem(mode == RobotMode.REAL ? new ShooterIOReal() : new ShooterIOSim());
 
+  private final AutoManager autoManager =
+      new AutoManager(swerve, intake, elevator, shooter, feeder);
+
   @Override
   public void robotInit() {
     // Metadata about the current code running on the robot
@@ -143,10 +141,6 @@ public void robotInit() {
     controller.b().whileTrue(elevator.setExtensionCmd(() -> 1.0));
 
     NamedCommands.registerCommand("stop", swerve.stopWithXCmd().asProxy());
-
-    // Auto Chooser
-
-    SmartDashboard.putData("Auto Chooser", AutoBuilder.buildAutoChooser());
   }
 
   @Override
@@ -159,18 +153,12 @@ public void disabledPeriodic() {}
 
   @Override
   public void autonomousInit() {
-    autonomousCommand = new PathPlannerAuto("local 4");
-
-    if (autonomousCommand != null) {
-      autonomousCommand.schedule();
-    }
+    autoManager.chooser.getSelected().schedule();
   }
 
   @Override
   public void teleopInit() {
-    if (autonomousCommand != null) {
-      autonomousCommand.cancel();
-    }
+    autoManager.chooser.getSelected().cancel();
   }
 
   @Override
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
index a943ef2d..a991ce5d 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
@@ -128,7 +128,6 @@ public SwerveSubsystem(GyroIO gyroIO, ModuleIO... moduleIOs) {
     PathPlannerLogging.setLogActivePathCallback(
         (path) -> Logger.recordOutput("PathPlanner/Active Path", path.toArray(Pose2d[]::new)));
     Logger.recordOutput("PathPlanner/Target", new Pose2d());
-    Logger.recordOutput("PathPlanner/Absolute Translation Error", 0.0);
 
     odometry = new SwerveDriveOdometry(kinematics, getRotation(), getModulePositions());
 

From c49d46b0fe8a6cc5765fa069c6d79eb70480cdff Mon Sep 17 00:00:00 2001
From: Sneha-Narayan <59103434+Sneha-Narayan@users.noreply.github.com>
Date: Sat, 10 Feb 2024 17:19:37 -0800
Subject: [PATCH 04/62] added static auto choreo path

---
 auto1.chor                                    | 2018 +++++++++++++++++
 ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat |  Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat |  Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat |  Bin 2048 -> 2048 bytes
 deploy/choreo/NewPath.1.traj                  | 1876 +++++++++++++++
 deploy/choreo/NewPath.traj                    | 1876 +++++++++++++++
 simgui-ds.json                                |    7 +-
 simgui.json                                   |   12 +
 src/main/deploy/choreo/auto1.1.traj           | 1876 +++++++++++++++
 src/main/deploy/choreo/auto1.traj             | 1876 +++++++++++++++
 src/main/deploy/pathplanner/autos/auto1.auto  |  135 ++
 src/main/deploy/pathplanner/navgrid.json      |    2 +-
 12 files changed, 9673 insertions(+), 5 deletions(-)
 create mode 100644 auto1.chor
 create mode 100644 deploy/choreo/NewPath.1.traj
 create mode 100644 deploy/choreo/NewPath.traj
 create mode 100644 src/main/deploy/choreo/auto1.1.traj
 create mode 100644 src/main/deploy/choreo/auto1.traj
 create mode 100644 src/main/deploy/pathplanner/autos/auto1.auto

diff --git a/auto1.chor b/auto1.chor
new file mode 100644
index 00000000..0586a8bc
--- /dev/null
+++ b/auto1.chor
@@ -0,0 +1,2018 @@
+{
+    "version": "v0.2.1",
+    "robotConfiguration": {
+        "mass": 74.08797700309194,
+        "rotationalInertia": 6,
+        "motorMaxTorque": 1.162295081967213,
+        "motorMaxVelocity": 4800,
+        "gearing": 6.75,
+        "wheelbase": 0.5778496879611685,
+        "trackWidth": 0.5778496879611685,
+        "bumperLength": 0.8762995267982555,
+        "bumperWidth": 0.8762995267982555,
+        "wheelRadius": 0.050799972568014815
+    },
+    "paths": {
+        "auto1": {
+            "waypoints": [
+                {
+                    "x": 1.3321385383605957,
+                    "y": 5.547596454620361,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 10
+                },
+                {
+                    "x": 2.2676985263824463,
+                    "y": 4.124765396118164,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 25
+                },
+                {
+                    "x": 1.3321385383605957,
+                    "y": 5.52810525894165,
+                    "heading": -3.1411853071795863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 23
+                },
+                {
+                    "x": 2.2287168502807617,
+                    "y": 5.567087173461914,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 23
+                },
+                {
+                    "x": 1.3321385383605957,
+                    "y": 5.547596454620361,
+                    "heading": -3.1411853071795863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 25
+                },
+                {
+                    "x": 2.2676985263824463,
+                    "y": 7.028899669647217,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 25
+                },
+                {
+                    "x": 1.332138180732727,
+                    "y": 5.52810525894165,
+                    "heading": -3.1411853071795863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 38
+                },
+                {
+                    "x": 7.686149597167969,
+                    "y": 7.496679306030273,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 38
+                },
+                {
+                    "x": 1.3321380615234375,
+                    "y": 5.547595977783203,
+                    "heading": -3.1411853071795863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 1.3321385383605957,
+                    "y": 5.547596454620361,
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": -3.4724754769815887e-38,
+                    "velocityY": 7.030705743146232e-38,
+                    "timestamp": 0
+                },
+                {
+                    "x": 1.361410083208431,
+                    "y": 5.5023955382692575,
+                    "heading": 0.056119311202146825,
+                    "angularVelocity": 0.6842584112779871,
+                    "velocityX": 0.35690567728254624,
+                    "velocityY": -0.5511312692222103,
+                    "timestamp": 0.08201479189321614
+                },
+                {
+                    "x": 1.4202660843784345,
+                    "y": 5.411892088977607,
+                    "heading": 0.16553764559038536,
+                    "angularVelocity": 1.3341292693969398,
+                    "velocityX": 0.7176266599156221,
+                    "velocityY": -1.1035015416424676,
+                    "timestamp": 0.1640295837864323
+                },
+                {
+                    "x": 1.509321709738084,
+                    "y": 5.2758955282995315,
+                    "heading": 0.32143534479454616,
+                    "angularVelocity": 1.9008485616489859,
+                    "velocityX": 1.0858483366707872,
+                    "velocityY": -1.658195522280223,
+                    "timestamp": 0.24604437567964843
+                },
+                {
+                    "x": 1.6296766101338032,
+                    "y": 5.093983916449887,
+                    "heading": 0.5046862665772869,
+                    "angularVelocity": 2.2343642842054003,
+                    "velocityX": 1.4674779709545838,
+                    "velocityY": -2.2180341820106597,
+                    "timestamp": 0.3280591675728646
+                },
+                {
+                    "x": 1.782194657966092,
+                    "y": 4.869104821237669,
+                    "heading": 0.6276214122511846,
+                    "angularVelocity": 1.4989387016181202,
+                    "velocityX": 1.859640734452738,
+                    "velocityY": -2.7419333759331184,
+                    "timestamp": 0.4100739594660807
+                },
+                {
+                    "x": 1.9333417085303153,
+                    "y": 4.6354174904379,
+                    "heading": 0.6920907561549033,
+                    "angularVelocity": 0.7860697127374073,
+                    "velocityX": 1.8429242710390283,
+                    "velocityY": -2.849331509662689,
+                    "timestamp": 0.49208875135929686
+                },
+                {
+                    "x": 2.056917521349593,
+                    "y": 4.443493094854259,
+                    "heading": 0.6585466805605577,
+                    "angularVelocity": -0.40900031348028376,
+                    "velocityX": 1.5067503064590375,
+                    "velocityY": -2.340119277916704,
+                    "timestamp": 0.574103543252513
+                },
+                {
+                    "x": 2.1535962335212764,
+                    "y": 4.293727999952354,
+                    "heading": 0.5265725883978299,
+                    "angularVelocity": -1.609149875482936,
+                    "velocityX": 1.1787960432498548,
+                    "velocityY": -1.8260741927736632,
+                    "timestamp": 0.6561183351457291
+                },
+                {
+                    "x": 2.2240985983067136,
+                    "y": 4.187111119253089,
+                    "heading": 0.3017485783287344,
+                    "angularVelocity": -2.7412617270530677,
+                    "velocityX": 0.859629868685535,
+                    "velocityY": -1.2999713617280337,
+                    "timestamp": 0.7381331270389453
+                },
+                {
+                    "x": 2.2676985263824463,
+                    "y": 4.124765396118164,
+                    "heading": -1.6642026159430915e-35,
+                    "angularVelocity": -3.6791970248685537,
+                    "velocityX": 0.5316105432846836,
+                    "velocityY": -0.7601765693205621,
+                    "timestamp": 0.8201479189321614
+                },
+                {
+                    "x": 2.2826958672458773,
+                    "y": 4.104348331421266,
+                    "heading": -0.17441872129385144,
+                    "angularVelocity": -4.1236397648607985,
+                    "velocityX": 0.35456991481679806,
+                    "velocityY": -0.4827040310886087,
+                    "timestamp": 0.8624451930006055
+                },
+                {
+                    "x": 2.289905521300629,
+                    "y": 4.095598364764115,
+                    "heading": -0.366838925198976,
+                    "angularVelocity": -4.549234156171776,
+                    "velocityX": 0.17045197860942923,
+                    "velocityY": -0.20686833489535802,
+                    "timestamp": 0.9047424670690496
+                },
+                {
+                    "x": 2.289262823237601,
+                    "y": 4.0986035079624115,
+                    "heading": -0.5765637041805302,
+                    "angularVelocity": -4.958352130262211,
+                    "velocityX": -0.015194786831599966,
+                    "velocityY": 0.07104815297159969,
+                    "timestamp": 0.9470397411374937
+                },
+                {
+                    "x": 2.28071834356103,
+                    "y": 4.1136177509731775,
+                    "heading": -0.8009025976234118,
+                    "angularVelocity": -5.303861735388993,
+                    "velocityX": -0.20201017358104575,
+                    "velocityY": 0.3549695185195792,
+                    "timestamp": 0.9893370152059378
+                },
+                {
+                    "x": 2.264115058699565,
+                    "y": 4.140907151677903,
+                    "heading": -1.0347556438013104,
+                    "angularVelocity": -5.528797099299715,
+                    "velocityX": -0.39253794073343345,
+                    "velocityY": 0.6451810738575218,
+                    "timestamp": 1.0316342892743817
+                },
+                {
+                    "x": 2.2392828245472374,
+                    "y": 4.180616635647853,
+                    "heading": -1.271268857234422,
+                    "angularVelocity": -5.5916892670292055,
+                    "velocityX": -0.5870882864022154,
+                    "velocityY": 0.9388189864361984,
+                    "timestamp": 1.0739315633428257
+                },
+                {
+                    "x": 2.206219336908872,
+                    "y": 4.232592488850575,
+                    "heading": -1.500368652073653,
+                    "angularVelocity": -5.416419849385782,
+                    "velocityX": -0.7816931082807688,
+                    "velocityY": 1.228822763344421,
+                    "timestamp": 1.1162288374112697
+                },
+                {
+                    "x": 2.16560115012261,
+                    "y": 4.295896098840919,
+                    "heading": -1.7081840928578422,
+                    "angularVelocity": -4.913211202403,
+                    "velocityX": -0.9603027069908828,
+                    "velocityY": 1.496635690704529,
+                    "timestamp": 1.1585261114797136
+                },
+                {
+                    "x": 2.116418529309532,
+                    "y": 4.368321186816809,
+                    "heading": -1.8863054196825848,
+                    "angularVelocity": -4.211177451684291,
+                    "velocityX": -1.162784645022098,
+                    "velocityY": 1.7122873653439958,
+                    "timestamp": 1.2008233855481576
+                },
+                {
+                    "x": 2.058912682392385,
+                    "y": 4.450563139928545,
+                    "heading": -2.036216963298125,
+                    "angularVelocity": -3.54423652391779,
+                    "velocityX": -1.3595639005977715,
+                    "velocityY": 1.9443795119906613,
+                    "timestamp": 1.2431206596166016
+                },
+                {
+                    "x": 1.99337005684811,
+                    "y": 4.542961602948437,
+                    "heading": -2.1579434366118924,
+                    "angularVelocity": -2.877879863293171,
+                    "velocityX": -1.549570911785386,
+                    "velocityY": 2.184501603350988,
+                    "timestamp": 1.2854179336850455
+                },
+                {
+                    "x": 1.9201828437796038,
+                    "y": 4.645652670813241,
+                    "heading": -2.2501769997399386,
+                    "angularVelocity": -2.180603009517744,
+                    "velocityX": -1.7303056681637912,
+                    "velocityY": 2.4278412764527704,
+                    "timestamp": 1.3277152077534895
+                },
+                {
+                    "x": 1.8398838191629743,
+                    "y": 4.758671823925337,
+                    "heading": -2.3107451299036645,
+                    "angularVelocity": -1.4319629691907945,
+                    "velocityX": -1.8984444360809696,
+                    "velocityY": 2.6720197838095423,
+                    "timestamp": 1.3700124818219335
+                },
+                {
+                    "x": 1.753396216406846,
+                    "y": 4.882199443808802,
+                    "heading": -2.337574910152261,
+                    "angularVelocity": -0.6343146417705776,
+                    "velocityX": -2.0447559484844615,
+                    "velocityY": 2.920462904620659,
+                    "timestamp": 1.4123097558903774
+                },
+                {
+                    "x": 1.6751428992292787,
+                    "y": 4.996901240775032,
+                    "heading": -2.400008661282964,
+                    "angularVelocity": -1.4760703261792811,
+                    "velocityX": -1.8500794413119903,
+                    "velocityY": 2.7118011619525055,
+                    "timestamp": 1.4546070299588214
+                },
+                {
+                    "x": 1.604167639142011,
+                    "y": 5.102445488685979,
+                    "heading": -2.5005028957452873,
+                    "angularVelocity": -2.375903333621606,
+                    "velocityX": -1.6780102654468432,
+                    "velocityY": 2.4952966883908108,
+                    "timestamp": 1.4969043040272654
+                },
+                {
+                    "x": 1.5418087398499432,
+                    "y": 5.19740105341742,
+                    "heading": -2.6238103064581355,
+                    "angularVelocity": -2.9152566785584497,
+                    "velocityX": -1.4743006651246806,
+                    "velocityY": 2.2449570763776965,
+                    "timestamp": 1.5392015780957093
+                },
+                {
+                    "x": 1.488225747512813,
+                    "y": 5.280327915096259,
+                    "heading": -2.743877445785595,
+                    "angularVelocity": -2.8386495813694843,
+                    "velocityX": -1.2668190448969345,
+                    "velocityY": 1.960572247389977,
+                    "timestamp": 1.5814988521641533
+                },
+                {
+                    "x": 1.4429482417442723,
+                    "y": 5.35126150411709,
+                    "heading": -2.8526923539591857,
+                    "angularVelocity": -2.5726222450531995,
+                    "velocityX": -1.0704591907098815,
+                    "velocityY": 1.6770250703638487,
+                    "timestamp": 1.6237961262325973
+                },
+                {
+                    "x": 1.4056282761179824,
+                    "y": 5.410288137316586,
+                    "heading": -2.94638853961619,
+                    "angularVelocity": -2.215182602675249,
+                    "velocityX": -0.8823255504811011,
+                    "velocityY": 1.3955186120027623,
+                    "timestamp": 1.6660934003010412
+                },
+                {
+                    "x": 1.3760265384921018,
+                    "y": 5.457458867956814,
+                    "heading": -3.023001318784294,
+                    "angularVelocity": -1.8112935373596752,
+                    "velocityX": -0.6998497723040017,
+                    "velocityY": 1.115219164334259,
+                    "timestamp": 1.7083906743694852
+                },
+                {
+                    "x": 1.3539833473819511,
+                    "y": 5.492803685860998,
+                    "heading": -3.0814563447777146,
+                    "angularVelocity": -1.3820045683991566,
+                    "velocityX": -0.5211492134098515,
+                    "velocityY": 0.8356287416297835,
+                    "timestamp": 1.7506879484379292
+                },
+                {
+                    "x": 1.3393859043766252,
+                    "y": 5.516345418500216,
+                    "heading": -3.121051917122202,
+                    "angularVelocity": -0.9361258666554966,
+                    "velocityX": -0.3451154554713123,
+                    "velocityY": 0.5565780102311921,
+                    "timestamp": 1.7929852225063732
+                },
+                {
+                    "x": 1.3321385383605957,
+                    "y": 5.52810525894165,
+                    "heading": -3.1411853071795863,
+                    "angularVelocity": -0.47599734263737736,
+                    "velocityX": -0.17134357179382342,
+                    "velocityY": 0.2780283292584069,
+                    "timestamp": 1.8352824965748171
+                },
+                {
+                    "x": 1.3321385383605957,
+                    "y": 5.52810525894165,
+                    "heading": -3.1411853071795863,
+                    "angularVelocity": 1.0972891859651771e-35,
+                    "velocityX": 2.010813957988482e-36,
+                    "velocityY": -7.697026441410569e-35,
+                    "timestamp": 1.877579770643261
+                },
+                {
+                    "x": 1.337370959637994,
+                    "y": 5.5282378526253355,
+                    "heading": -3.1057781238593085,
+                    "angularVelocity": 1.0527517345397082,
+                    "velocityX": 0.1555740970920068,
+                    "velocityY": 0.003942370372304683,
+                    "timestamp": 1.9112127556658494
+                },
+                {
+                    "x": 1.3479554232342674,
+                    "y": 5.5287097500206475,
+                    "heading": -3.0355501553802027,
+                    "angularVelocity": 2.0880682589410555,
+                    "velocityX": 0.314704852666657,
+                    "velocityY": 0.014030791349476537,
+                    "timestamp": 1.9448457406884376
+                },
+                {
+                    "x": 1.364027106419543,
+                    "y": 5.529829382282991,
+                    "heading": -2.9313355116530078,
+                    "angularVelocity": 3.0985844300529233,
+                    "velocityX": 0.4778547956561592,
+                    "velocityY": 0.03328970835004707,
+                    "timestamp": 1.978478725711026
+                },
+                {
+                    "x": 1.3857940218160676,
+                    "y": 5.531991335292171,
+                    "heading": -2.7946612849381784,
+                    "angularVelocity": 4.06369600031153,
+                    "velocityX": 0.6471895189173892,
+                    "velocityY": 0.06428073534739039,
+                    "timestamp": 2.012111710733614
+                },
+                {
+                    "x": 1.4139333100804088,
+                    "y": 5.535564467259386,
+                    "heading": -2.6300339585531556,
+                    "angularVelocity": 4.89481758084981,
+                    "velocityX": 0.8366574731752962,
+                    "velocityY": 0.10623891887132897,
+                    "timestamp": 2.0457446957562024
+                },
+                {
+                    "x": 1.4504189067700664,
+                    "y": 5.5389912877920775,
+                    "heading": -2.4501533377113245,
+                    "angularVelocity": 5.348339456667967,
+                    "velocityX": 1.0848158932415233,
+                    "velocityY": 0.10188868250590417,
+                    "timestamp": 2.0793776807787907
+                },
+                {
+                    "x": 1.4957426020786804,
+                    "y": 5.541383162087915,
+                    "heading": -2.2591276091627437,
+                    "angularVelocity": 5.679713781583368,
+                    "velocityX": 1.3475965715851275,
+                    "velocityY": 0.07111691972124916,
+                    "timestamp": 2.113010665801379
+                },
+                {
+                    "x": 1.549887277450098,
+                    "y": 5.541604080123591,
+                    "heading": -2.077370178961034,
+                    "angularVelocity": 5.404142096802884,
+                    "velocityX": 1.6098682687562116,
+                    "velocityY": 0.006568493267186675,
+                    "timestamp": 2.1466436508239672
+                },
+                {
+                    "x": 1.612403659002375,
+                    "y": 5.539682953328213,
+                    "heading": -1.9100118188035742,
+                    "angularVelocity": 4.9760186330490965,
+                    "velocityX": 1.8587818330811392,
+                    "velocityY": -0.05712031786913091,
+                    "timestamp": 2.1802766358465555
+                },
+                {
+                    "x": 1.6835678333886066,
+                    "y": 5.53654594415181,
+                    "heading": -1.7561585727792173,
+                    "angularVelocity": 4.5744749067330135,
+                    "velocityX": 2.1159042035203575,
+                    "velocityY": -0.0932718036860474,
+                    "timestamp": 2.213909620869144
+                },
+                {
+                    "x": 1.7632443244228584,
+                    "y": 5.534877661418315,
+                    "heading": -1.616595649842294,
+                    "angularVelocity": 4.149584785388266,
+                    "velocityX": 2.3689984989658393,
+                    "velocityY": -0.04960257712408059,
+                    "timestamp": 2.247542605891732
+                },
+                {
+                    "x": 1.845517380604842,
+                    "y": 5.542129045770498,
+                    "heading": -1.4828842082324165,
+                    "angularVelocity": 3.9756043515042103,
+                    "velocityX": 2.4462014337035023,
+                    "velocityY": 0.21560335329479804,
+                    "timestamp": 2.2811755909143203
+                },
+                {
+                    "x": 1.9198078929781421,
+                    "y": 5.552688491598465,
+                    "heading": -1.3358148203184805,
+                    "angularVelocity": 4.372772378519569,
+                    "velocityX": 2.208858723761989,
+                    "velocityY": 0.31396100646060415,
+                    "timestamp": 2.3148085759369086
+                },
+                {
+                    "x": 1.9854932228404973,
+                    "y": 5.561629411594934,
+                    "heading": -1.1756047034069004,
+                    "angularVelocity": 4.763481945000775,
+                    "velocityX": 1.9530032739657306,
+                    "velocityY": 0.26583783718469556,
+                    "timestamp": 2.348441560959497
+                },
+                {
+                    "x": 2.0429500865174064,
+                    "y": 5.56779505699511,
+                    "heading": -1.0015843737719388,
+                    "angularVelocity": 5.1740970811270035,
+                    "velocityX": 1.7083486237787309,
+                    "velocityY": 0.18332138512343155,
+                    "timestamp": 2.382074545982085
+                },
+                {
+                    "x": 2.0926336267477486,
+                    "y": 5.570712245735625,
+                    "heading": -0.8117369336213589,
+                    "angularVelocity": 5.644680066996039,
+                    "velocityX": 1.477226603495769,
+                    "velocityY": 0.08673594504191894,
+                    "timestamp": 2.4157075310046734
+                },
+                {
+                    "x": 2.1331736827499013,
+                    "y": 5.571318223980459,
+                    "heading": -0.6223345825199257,
+                    "angularVelocity": 5.63144636059598,
+                    "velocityX": 1.2053659814888902,
+                    "velocityY": 0.01801737920159694,
+                    "timestamp": 2.4493405160272617
+                },
+                {
+                    "x": 2.1650842656051874,
+                    "y": 5.570653927593427,
+                    "heading": -0.4456863837750604,
+                    "angularVelocity": 5.252230767689132,
+                    "velocityX": 0.9487883051074633,
+                    "velocityY": -0.01975133597523799,
+                    "timestamp": 2.48297350104985
+                },
+                {
+                    "x": 2.1896037980440752,
+                    "y": 5.569969320542384,
+                    "heading": -0.29224766364008553,
+                    "angularVelocity": 4.56214992608965,
+                    "velocityX": 0.7290323003569462,
+                    "velocityY": -0.020355227184960373,
+                    "timestamp": 2.516606486072438
+                },
+                {
+                    "x": 2.20780773019257,
+                    "y": 5.569409848980837,
+                    "heading": -0.16876882632138132,
+                    "angularVelocity": 3.6713612317127033,
+                    "velocityX": 0.5412523490337908,
+                    "velocityY": -0.016634609184155776,
+                    "timestamp": 2.5502394710950265
+                },
+                {
+                    "x": 2.220213327560089,
+                    "y": 5.568812984785934,
+                    "heading": -0.07803807473080998,
+                    "angularVelocity": 2.697671691336225,
+                    "velocityX": 0.368852106323223,
+                    "velocityY": -0.017746393741217954,
+                    "timestamp": 2.5838724561176147
+                },
+                {
+                    "x": 2.227117258661317,
+                    "y": 5.568051315056596,
+                    "heading": -0.021486347391680306,
+                    "angularVelocity": 1.6814364618885036,
+                    "velocityX": 0.2052726243772784,
+                    "velocityY": -0.022646509931434324,
+                    "timestamp": 2.617505441140203
+                },
+                {
+                    "x": 2.2287168502807617,
+                    "y": 5.567087173461914,
+                    "heading": 6.38149991346766e-37,
+                    "angularVelocity": 0.6388474700431709,
+                    "velocityX": 0.047560203721742585,
+                    "velocityY": -0.02866654845040371,
+                    "timestamp": 2.6511384261627913
+                },
+                {
+                    "x": 2.2247758271699722,
+                    "y": 5.5659373744789455,
+                    "heading": -0.016384469566950412,
+                    "angularVelocity": -0.4672993433012807,
+                    "velocityX": -0.11240141184197158,
+                    "velocityY": -0.03279326849576676,
+                    "timestamp": 2.686200468945049
+                },
+                {
+                    "x": 2.215132755191586,
+                    "y": 5.564807940629173,
+                    "heading": -0.07111277017786721,
+                    "angularVelocity": -1.560898803038675,
+                    "velocityX": -0.27502881216225394,
+                    "velocityY": -0.03221243715853021,
+                    "timestamp": 2.7212625117273066
+                },
+                {
+                    "x": 2.199681422128445,
+                    "y": 5.563984363308005,
+                    "heading": -0.16355266498461823,
+                    "angularVelocity": -2.6364663171744263,
+                    "velocityX": -0.4406854774291637,
+                    "velocityY": -0.023489142554608954,
+                    "timestamp": 2.7563245545095643
+                },
+                {
+                    "x": 2.178275880729617,
+                    "y": 5.563860339253889,
+                    "heading": -0.2925810137194886,
+                    "angularVelocity": -3.6800008925938044,
+                    "velocityX": -0.6105046854160964,
+                    "velocityY": -0.0035372740511740697,
+                    "timestamp": 2.791386597291822
+                },
+                {
+                    "x": 2.150434792823077,
+                    "y": 5.56488104971689,
+                    "heading": -0.45485951159806126,
+                    "angularVelocity": -4.628324107820951,
+                    "velocityX": -0.7940520773258658,
+                    "velocityY": 0.029111551467175232,
+                    "timestamp": 2.8264486400740796
+                },
+                {
+                    "x": 2.1136927979170794,
+                    "y": 5.564806154665508,
+                    "heading": -0.6342616510080993,
+                    "angularVelocity": -5.116705279386068,
+                    "velocityX": -1.047913697846207,
+                    "velocityY": -0.0021360721007366726,
+                    "timestamp": 2.8615106828563373
+                },
+                {
+                    "x": 2.0677952991091333,
+                    "y": 5.562722960469238,
+                    "heading": -0.8285761603492735,
+                    "angularVelocity": -5.542019058841107,
+                    "velocityX": -1.3090366437853844,
+                    "velocityY": -0.05941451298794179,
+                    "timestamp": 2.896572725638595
+                },
+                {
+                    "x": 2.0120787764368706,
+                    "y": 5.557776466918295,
+                    "heading": -1.020468287532569,
+                    "angularVelocity": -5.472930609747536,
+                    "velocityX": -1.5890837569925207,
+                    "velocityY": -0.14107830458316797,
+                    "timestamp": 2.9316347684208526
+                },
+                {
+                    "x": 1.947346732087487,
+                    "y": 5.549990941392819,
+                    "heading": -1.1972983758508349,
+                    "angularVelocity": -5.043348141932757,
+                    "velocityX": -1.8462142879518657,
+                    "velocityY": -0.22204996935933435,
+                    "timestamp": 2.9666968112031102
+                },
+                {
+                    "x": 1.8731816133340355,
+                    "y": 5.541067205657786,
+                    "heading": -1.3595832736612468,
+                    "angularVelocity": -4.628506639451492,
+                    "velocityX": -2.1152537863818077,
+                    "velocityY": -0.25451271594333,
+                    "timestamp": 3.001758853985368
+                },
+                {
+                    "x": 1.7903438797008602,
+                    "y": 5.535778355661316,
+                    "heading": -1.5070368489944903,
+                    "angularVelocity": -4.2055044039778195,
+                    "velocityX": -2.362604316799635,
+                    "velocityY": -0.15084260860996357,
+                    "timestamp": 3.0368208967676256
+                },
+                {
+                    "x": 1.705207783905628,
+                    "y": 5.540259742780199,
+                    "heading": -1.6469795594362229,
+                    "angularVelocity": -3.991287995134942,
+                    "velocityX": -2.4281556075880766,
+                    "velocityY": 0.12781306402233675,
+                    "timestamp": 3.0718829395498832
+                },
+                {
+                    "x": 1.629345764073995,
+                    "y": 5.546176174189709,
+                    "heading": -1.802120558201536,
+                    "angularVelocity": -4.424756416184017,
+                    "velocityX": -2.1636508831716217,
+                    "velocityY": 0.16874177714778527,
+                    "timestamp": 3.106944982332141
+                },
+                {
+                    "x": 1.5628215543297233,
+                    "y": 5.550494777199676,
+                    "heading": -1.9719374235468123,
+                    "angularVelocity": -4.843324914063712,
+                    "velocityX": -1.8973284060315727,
+                    "velocityY": 0.12317031944734674,
+                    "timestamp": 3.1420070251143986
+                },
+                {
+                    "x": 1.5052956910676978,
+                    "y": 5.552468250746859,
+                    "heading": -2.1578472190724005,
+                    "angularVelocity": -5.302309300120524,
+                    "velocityX": -1.6406877265900472,
+                    "velocityY": 0.056285184506758255,
+                    "timestamp": 3.177069067896656
+                },
+                {
+                    "x": 1.4562451550739939,
+                    "y": 5.551766768072451,
+                    "heading": -2.3624196209535056,
+                    "angularVelocity": -5.834583088941523,
+                    "velocityX": -1.3989640106914938,
+                    "velocityY": -0.02000689688175958,
+                    "timestamp": 3.212131110678914
+                },
+                {
+                    "x": 1.4173942622657685,
+                    "y": 5.550106751106204,
+                    "heading": -2.5657566721184293,
+                    "angularVelocity": -5.799349810497013,
+                    "velocityX": -1.1080613029165842,
+                    "velocityY": -0.047345129790485885,
+                    "timestamp": 3.2471931534611715
+                },
+                {
+                    "x": 1.3874992845964569,
+                    "y": 5.548698391964879,
+                    "heading": -2.750369265628105,
+                    "angularVelocity": -5.26531196873374,
+                    "velocityX": -0.852630802345583,
+                    "velocityY": -0.0401676294239809,
+                    "timestamp": 3.282255196243429
+                },
+                {
+                    "x": 1.3647199881577643,
+                    "y": 5.548009174125998,
+                    "heading": -2.903871904539791,
+                    "angularVelocity": -4.378028966109259,
+                    "velocityX": -0.6496853757254475,
+                    "velocityY": -0.019657093089571077,
+                    "timestamp": 3.317317239025687
+                },
+                {
+                    "x": 1.3481785543906255,
+                    "y": 5.547743770300603,
+                    "heading": -3.0214740080658498,
+                    "angularVelocity": -3.354114426714698,
+                    "velocityX": -0.4717760990101023,
+                    "velocityY": -0.007569548273095167,
+                    "timestamp": 3.3523792818079445
+                },
+                {
+                    "x": 1.3374168488281284,
+                    "y": 5.547647789865521,
+                    "heading": -3.10100248389671,
+                    "angularVelocity": -2.268221401837531,
+                    "velocityX": -0.30693321633680876,
+                    "velocityY": -0.0027374456097331258,
+                    "timestamp": 3.387441324590202
+                },
+                {
+                    "x": 1.3321385383605957,
+                    "y": 5.547596454620361,
+                    "heading": -3.1411853071795863,
+                    "angularVelocity": -1.1460491201958698,
+                    "velocityX": -0.15054201206449694,
+                    "velocityY": -0.001464125906141093,
+                    "timestamp": 3.42250336737246
+                },
+                {
+                    "x": 1.3321385383605957,
+                    "y": 5.547596454620361,
+                    "heading": -3.1411853071795863,
+                    "angularVelocity": -1.1877010663188592e-33,
+                    "velocityX": -2.3472123166291743e-34,
+                    "velocityY": -4.828629969149868e-35,
+                    "timestamp": 3.4575654101547175
+                },
+                {
+                    "x": 1.3353773426968816,
+                    "y": 5.556976212978418,
+                    "heading": -3.1027759330153977,
+                    "angularVelocity": 0.9418411865422114,
+                    "velocityX": 0.07941913622507528,
+                    "velocityY": 0.2300022568362304,
+                    "timestamp": 3.4983465680749837
+                },
+                {
+                    "x": 1.342768335196551,
+                    "y": 5.576502155287014,
+                    "heading": -3.0330086785474775,
+                    "angularVelocity": 1.7107717883912714,
+                    "velocityX": 0.18123547433645554,
+                    "velocityY": 0.47879813385321396,
+                    "timestamp": 3.53912772599525
+                },
+                {
+                    "x": 1.357543783583743,
+                    "y": 5.605477021924564,
+                    "heading": -2.939156767804727,
+                    "angularVelocity": 2.301354731669143,
+                    "velocityX": 0.3623106635687124,
+                    "velocityY": 0.7104964183263536,
+                    "timestamp": 3.579908883915516
+                },
+                {
+                    "x": 1.3800861680237329,
+                    "y": 5.643206466545064,
+                    "heading": -2.8183134070613347,
+                    "angularVelocity": 2.9632155364411483,
+                    "velocityX": 0.5527646979535052,
+                    "velocityY": 0.9251685470595896,
+                    "timestamp": 3.620690041835782
+                },
+                {
+                    "x": 1.410132685953878,
+                    "y": 5.690126983565226,
+                    "heading": -2.6706561536339386,
+                    "angularVelocity": 3.6207224354956073,
+                    "velocityX": 0.7367745170181611,
+                    "velocityY": 1.1505440113274845,
+                    "timestamp": 3.6614711997560483
+                },
+                {
+                    "x": 1.447426110042683,
+                    "y": 5.747148126734536,
+                    "heading": -2.4998618512700794,
+                    "angularVelocity": 4.188068977781135,
+                    "velocityX": 0.9144768317201708,
+                    "velocityY": 1.3982227596576835,
+                    "timestamp": 3.7022523576763144
+                },
+                {
+                    "x": 1.4919514039971702,
+                    "y": 5.815443990853687,
+                    "heading": -2.3159167566388894,
+                    "angularVelocity": 4.510541240413857,
+                    "velocityX": 1.0918104395549988,
+                    "velocityY": 1.6746916370712401,
+                    "timestamp": 3.7430335155965806
+                },
+                {
+                    "x": 1.543306542439651,
+                    "y": 5.895557258888599,
+                    "heading": -2.140840879075951,
+                    "angularVelocity": 4.293058031977455,
+                    "velocityX": 1.259285931578716,
+                    "velocityY": 1.9644677130440258,
+                    "timestamp": 3.7838146735168467
+                },
+                {
+                    "x": 1.5991978574253154,
+                    "y": 5.9852989254543125,
+                    "heading": -2.0026162516135204,
+                    "angularVelocity": 3.389423805294663,
+                    "velocityX": 1.3705180979642946,
+                    "velocityY": 2.2005669074226377,
+                    "timestamp": 3.824595831437113
+                },
+                {
+                    "x": 1.6602838186413746,
+                    "y": 6.084712915583529,
+                    "heading": -1.8987034819220077,
+                    "angularVelocity": 2.5480583433820057,
+                    "velocityX": 1.497896683941456,
+                    "velocityY": 2.4377431931576705,
+                    "timestamp": 3.865376989357379
+                },
+                {
+                    "x": 1.7272241659282512,
+                    "y": 6.1936323149644315,
+                    "heading": -1.8270894144503607,
+                    "angularVelocity": 1.7560577267488233,
+                    "velocityX": 1.6414528350998836,
+                    "velocityY": 2.670826551660404,
+                    "timestamp": 3.906158147277645
+                },
+                {
+                    "x": 1.8006198616667228,
+                    "y": 6.311769110233653,
+                    "heading": -1.7863461946081067,
+                    "angularVelocity": 0.9990697155268056,
+                    "velocityX": 1.7997452618185266,
+                    "velocityY": 2.8968474975673533,
+                    "timestamp": 3.9469393051979114
+                },
+                {
+                    "x": 1.8749454151333007,
+                    "y": 6.423903525675366,
+                    "heading": -1.7351071665224345,
+                    "angularVelocity": 1.2564387746383576,
+                    "velocityX": 1.8225464223428127,
+                    "velocityY": 2.7496623725337597,
+                    "timestamp": 3.9877204631181775
+                },
+                {
+                    "x": 1.9436310828301862,
+                    "y": 6.525803671980358,
+                    "heading": -1.6558915421182487,
+                    "angularVelocity": 1.9424564785302534,
+                    "velocityX": 1.6842500605592803,
+                    "velocityY": 2.4987065473772168,
+                    "timestamp": 4.028501621038443
+                },
+                {
+                    "x": 2.0070825887496224,
+                    "y": 6.617181897860731,
+                    "heading": -1.5496488374577189,
+                    "angularVelocity": 2.6051909773688204,
+                    "velocityX": 1.5559025087883662,
+                    "velocityY": 2.240697188123809,
+                    "timestamp": 4.069282778958709
+                },
+                {
+                    "x": 2.0657895498392027,
+                    "y": 6.697945113539011,
+                    "heading": -1.416445908140072,
+                    "angularVelocity": 3.2662861014903286,
+                    "velocityX": 1.4395609169401762,
+                    "velocityY": 1.9804051625063046,
+                    "timestamp": 4.1100639368789755
+                },
+                {
+                    "x": 2.1168235029971925,
+                    "y": 6.770262031139991,
+                    "heading": -1.2545233571884964,
+                    "angularVelocity": 3.9705236243698976,
+                    "velocityX": 1.2514101060536296,
+                    "velocityY": 1.7732924048496255,
+                    "timestamp": 4.150845094799242
+                },
+                {
+                    "x": 2.160098891028687,
+                    "y": 6.834337445285054,
+                    "heading": -1.0613678161788878,
+                    "angularVelocity": 4.736391776497875,
+                    "velocityX": 1.0611613362255508,
+                    "velocityY": 1.571201442350923,
+                    "timestamp": 4.191626252719508
+                },
+                {
+                    "x": 2.1954531720383796,
+                    "y": 6.890659821255424,
+                    "heading": -0.8342118106482137,
+                    "angularVelocity": 5.570121524621797,
+                    "velocityX": 0.8669268557507981,
+                    "velocityY": 1.3810881996163518,
+                    "timestamp": 4.232407410639774
+                },
+                {
+                    "x": 2.2216225780243426,
+                    "y": 6.9367359795772305,
+                    "heading": -0.6132278215539244,
+                    "angularVelocity": 5.418776718560821,
+                    "velocityX": 0.6417033581324141,
+                    "velocityY": 1.1298393834695264,
+                    "timestamp": 4.27318856856004
+                },
+                {
+                    "x": 2.2400584030513464,
+                    "y": 6.973023548052357,
+                    "heading": -0.41519671142857095,
+                    "angularVelocity": 4.855946231652809,
+                    "velocityX": 0.4520672282785309,
+                    "velocityY": 0.889812117303642,
+                    "timestamp": 4.313969726480306
+                },
+                {
+                    "x": 2.252392360052711,
+                    "y": 6.999798504355172,
+                    "heading": -0.24838728557433673,
+                    "angularVelocity": 4.0903553101747345,
+                    "velocityX": 0.3024425403878878,
+                    "velocityY": 0.6565521350611045,
+                    "timestamp": 4.3547508844005725
+                },
+                {
+                    "x": 2.2601470415559044,
+                    "y": 7.01732009026677,
+                    "heading": -0.11829622013640562,
+                    "angularVelocity": 3.189979688469885,
+                    "velocityX": 0.19015353900334692,
+                    "velocityY": 0.4296490537580049,
+                    "timestamp": 4.395532042320839
+                },
+                {
+                    "x": 2.2649306384373356,
+                    "y": 7.026470873344902,
+                    "heading": -0.032599297392582494,
+                    "angularVelocity": 2.101385225779393,
+                    "velocityX": 0.11729919220988534,
+                    "velocityY": 0.2243875246510465,
+                    "timestamp": 4.436313200241105
+                },
+                {
+                    "x": 2.2676985263824463,
+                    "y": 7.028899669647217,
+                    "heading": 1.4623510884357687e-33,
+                    "angularVelocity": 0.799371549388559,
+                    "velocityX": 0.06787173504299782,
+                    "velocityY": 0.05955682541099079,
+                    "timestamp": 4.477094358161371
+                },
+                {
+                    "x": 2.268447419698579,
+                    "y": 7.024856278106883,
+                    "heading": -0.027481442707952487,
+                    "angularVelocity": -0.643111881448138,
+                    "velocityX": 0.017525360464520197,
+                    "velocityY": -0.09462214806441192,
+                    "timestamp": 4.5198263360655435
+                },
+                {
+                    "x": 2.2661447146026696,
+                    "y": 7.012526995839087,
+                    "heading": -0.10890493556394686,
+                    "angularVelocity": -1.9054463858094515,
+                    "velocityX": -0.05388716387229158,
+                    "velocityY": -0.28852589729042577,
+                    "timestamp": 4.562558313969716
+                },
+                {
+                    "x": 2.259237652200008,
+                    "y": 6.990956553960971,
+                    "heading": -0.23735745507492811,
+                    "angularVelocity": -3.0060045383117964,
+                    "velocityX": -0.16163685233926556,
+                    "velocityY": -0.504784541602271,
+                    "timestamp": 4.605290291873889
+                },
+                {
+                    "x": 2.2461382818263638,
+                    "y": 6.959794967044901,
+                    "heading": -0.4073842771005642,
+                    "angularVelocity": -3.9789129912714607,
+                    "velocityX": -0.30654725140549266,
+                    "velocityY": -0.7292334323009891,
+                    "timestamp": 4.648022269778061
+                },
+                {
+                    "x": 2.225216379141952,
+                    "y": 6.918625356851511,
+                    "heading": -0.6110307557476551,
+                    "angularVelocity": -4.765669380054774,
+                    "velocityX": -0.48960763602681573,
+                    "velocityY": -0.9634379734473075,
+                    "timestamp": 4.690754247682234
+                },
+                {
+                    "x": 2.1950172148844427,
+                    "y": 6.866427702804465,
+                    "heading": -0.8329684676174923,
+                    "angularVelocity": -5.193714935628281,
+                    "velocityX": -0.706711126857547,
+                    "velocityY": -1.2215127079794852,
+                    "timestamp": 4.7334862255864065
+                },
+                {
+                    "x": 2.1547332634682537,
+                    "y": 6.802939586567042,
+                    "heading": -1.0490897028487878,
+                    "angularVelocity": -5.057599620498584,
+                    "velocityX": -0.9427120716604103,
+                    "velocityY": -1.4857284720074833,
+                    "timestamp": 4.776218203490579
+                },
+                {
+                    "x": 2.1052130726477585,
+                    "y": 6.731031156240311,
+                    "heading": -1.2322360836735529,
+                    "angularVelocity": -4.285932685715518,
+                    "velocityX": -1.1588555748939446,
+                    "velocityY": -1.6827779535033027,
+                    "timestamp": 4.818950181394752
+                },
+                {
+                    "x": 2.046678257521749,
+                    "y": 6.650499615743051,
+                    "heading": -1.3842475177981566,
+                    "angularVelocity": -3.557322679177027,
+                    "velocityX": -1.3698129128793415,
+                    "velocityY": -1.8845732036521736,
+                    "timestamp": 4.861682159298924
+                },
+                {
+                    "x": 1.9824374188049456,
+                    "y": 6.55859622988028,
+                    "heading": -1.505962481707897,
+                    "angularVelocity": -2.848334429608893,
+                    "velocityX": -1.5033434413184743,
+                    "velocityY": -2.150693470563594,
+                    "timestamp": 4.904414137203097
+                },
+                {
+                    "x": 1.9120980242519563,
+                    "y": 6.455385209029271,
+                    "heading": -1.5975173206853661,
+                    "angularVelocity": -2.1425368884815836,
+                    "velocityX": -1.6460598830863262,
+                    "velocityY": -2.415311106882586,
+                    "timestamp": 4.9471461151072695
+                },
+                {
+                    "x": 1.835418260391466,
+                    "y": 6.341055354736124,
+                    "heading": -1.6582681711046532,
+                    "angularVelocity": -1.4216718579121885,
+                    "velocityX": -1.7944351659182955,
+                    "velocityY": -2.675510470157384,
+                    "timestamp": 4.989878093011442
+                },
+                {
+                    "x": 1.7526348309039128,
+                    "y": 6.215560469564215,
+                    "heading": -1.6873132093111056,
+                    "angularVelocity": -0.6797026402940408,
+                    "velocityX": -1.937271185368423,
+                    "velocityY": -2.9367909309822835,
+                    "timestamp": 5.032610070915615
+                },
+                {
+                    "x": 1.677441366215122,
+                    "y": 6.099853521630283,
+                    "heading": -1.7491066585439166,
+                    "angularVelocity": -1.4460704199413479,
+                    "velocityX": -1.7596532708458867,
+                    "velocityY": -2.70773677252684,
+                    "timestamp": 5.075342048819787
+                },
+                {
+                    "x": 1.6091252487579695,
+                    "y": 5.994372460119343,
+                    "heading": -1.8457597622108368,
+                    "angularVelocity": -2.2618448386280567,
+                    "velocityX": -1.598711803379506,
+                    "velocityY": -2.4684338681322977,
+                    "timestamp": 5.11807402672396
+                },
+                {
+                    "x": 1.547030840271468,
+                    "y": 5.899198607270202,
+                    "heading": -1.9803343034636756,
+                    "angularVelocity": -3.149270121655174,
+                    "velocityX": -1.4531133715773699,
+                    "velocityY": -2.2272278868666358,
+                    "timestamp": 5.1608060046281325
+                },
+                {
+                    "x": 1.4904311214521733,
+                    "y": 5.813988086882659,
+                    "heading": -2.1571135876817413,
+                    "angularVelocity": -4.136931939225903,
+                    "velocityX": -1.3245284116317142,
+                    "velocityY": -1.9940691858127988,
+                    "timestamp": 5.203537982532305
+                },
+                {
+                    "x": 1.4423633386018753,
+                    "y": 5.740159267538946,
+                    "heading": -2.35427402869071,
+                    "angularVelocity": -4.613885213811227,
+                    "velocityX": -1.1248667908162702,
+                    "velocityY": -1.7277182794879316,
+                    "timestamp": 5.246269960436478
+                },
+                {
+                    "x": 1.4038900802302197,
+                    "y": 5.6781134856885735,
+                    "heading": -2.5480008926422313,
+                    "angularVelocity": -4.533533748097493,
+                    "velocityX": -0.9003388155337229,
+                    "velocityY": -1.451975426681909,
+                    "timestamp": 5.28900193834065
+                },
+                {
+                    "x": 1.3744454738206087,
+                    "y": 5.627099122367314,
+                    "heading": -2.7223828634703597,
+                    "angularVelocity": -4.0808307824923435,
+                    "velocityX": -0.6890532068429254,
+                    "velocityY": -1.1938217190802813,
+                    "timestamp": 5.331733916244823
+                },
+                {
+                    "x": 1.3538849352129523,
+                    "y": 5.586407689469935,
+                    "heading": -2.872592229193493,
+                    "angularVelocity": -3.5151512541727503,
+                    "velocityX": -0.4811511101537121,
+                    "velocityY": -0.9522478221960695,
+                    "timestamp": 5.374465894148996
+                },
+                {
+                    "x": 1.341639273384249,
+                    "y": 5.556291662758613,
+                    "heading": -2.9981488971519656,
+                    "angularVelocity": -2.9382367518778816,
+                    "velocityX": -0.2865690386755447,
+                    "velocityY": -0.7047655687470963,
+                    "timestamp": 5.417197872053168
+                },
+                {
+                    "x": 1.3350584223241078,
+                    "y": 5.537231311212578,
+                    "heading": -3.091661499454674,
+                    "angularVelocity": -2.18835183600471,
+                    "velocityX": -0.15400295944407288,
+                    "velocityY": -0.4460442151491003,
+                    "timestamp": 5.459929849957341
+                },
+                {
+                    "x": 1.332138180732727,
+                    "y": 5.52810525894165,
+                    "heading": -3.1411853071795863,
+                    "angularVelocity": -1.1589402165275682,
+                    "velocityX": -0.06833855427730316,
+                    "velocityY": -0.21356493938549467,
+                    "timestamp": 5.502661827861513
+                },
+                {
+                    "x": 1.332138180732727,
+                    "y": 5.52810525894165,
+                    "heading": -3.1411853071795863,
+                    "angularVelocity": 0,
+                    "velocityX": 4.554923410438002e-36,
+                    "velocityY": -5.3517654956150865e-34,
+                    "timestamp": 5.545393805765686
+                },
+                {
+                    "x": 1.3511823216382217,
+                    "y": 5.531961266421083,
+                    "heading": -3.038798036456765,
+                    "angularVelocity": 1.6934181625027336,
+                    "velocityX": 0.3149775735885273,
+                    "velocityY": 0.06377582930300447,
+                    "timestamp": 5.60585570052386
+                },
+                {
+                    "x": 1.3908490268299478,
+                    "y": 5.542189708843604,
+                    "heading": -2.844785530962286,
+                    "angularVelocity": 3.2088393238494826,
+                    "velocityX": 0.6560612324568816,
+                    "velocityY": 0.16917171490292857,
+                    "timestamp": 5.666317595282035
+                },
+                {
+                    "x": 1.4539728254946123,
+                    "y": 5.56127979063472,
+                    "heading": -2.581938269596155,
+                    "angularVelocity": 4.34732094350375,
+                    "velocityX": 1.0440261410453116,
+                    "velocityY": 0.3157374056415134,
+                    "timestamp": 5.726779490040209
+                },
+                {
+                    "x": 1.5450944631009007,
+                    "y": 5.588995277628573,
+                    "heading": -2.2890493808444536,
+                    "angularVelocity": 4.8441897152438,
+                    "velocityX": 1.5070919952267785,
+                    "velocityY": 0.45839593854450544,
+                    "timestamp": 5.787241384798383
+                },
+                {
+                    "x": 1.6638598735645596,
+                    "y": 5.62434991056183,
+                    "heading": -2.0324634607383167,
+                    "angularVelocity": 4.24376247440454,
+                    "velocityX": 1.9643018290888397,
+                    "velocityY": 0.5847423914626325,
+                    "timestamp": 5.8477032795565576
+                },
+                {
+                    "x": 1.8083021892177116,
+                    "y": 5.6638944259582,
+                    "heading": -1.8350759652142852,
+                    "angularVelocity": 3.264659440685901,
+                    "velocityX": 2.3889809644714033,
+                    "velocityY": 0.6540402935523977,
+                    "timestamp": 5.908165174314732
+                },
+                {
+                    "x": 1.978235650109889,
+                    "y": 5.709112472816866,
+                    "heading": -1.6974261571036708,
+                    "angularVelocity": 2.276637354174287,
+                    "velocityX": 2.8105877523661738,
+                    "velocityY": 0.7478767749426485,
+                    "timestamp": 5.968627069072906
+                },
+                {
+                    "x": 2.1727729031197174,
+                    "y": 5.761282618111588,
+                    "heading": -1.6235540862695264,
+                    "angularVelocity": 1.2217954982986527,
+                    "velocityX": 3.217518302856812,
+                    "velocityY": 0.8628599137255631,
+                    "timestamp": 6.0290889638310805
+                },
+                {
+                    "x": 2.3909229420587215,
+                    "y": 5.821425961949498,
+                    "heading": -1.6168784459669248,
+                    "angularVelocity": 0.1104107029609625,
+                    "velocityX": 3.60805826234069,
+                    "velocityY": 0.994731377150211,
+                    "timestamp": 6.089550858589255
+                },
+                {
+                    "x": 2.6085958376613507,
+                    "y": 5.891661539060254,
+                    "heading": -1.616878243648269,
+                    "angularVelocity": 0.0000033462175924113523,
+                    "velocityX": 3.6001666251651883,
+                    "velocityY": 1.1616502822426138,
+                    "timestamp": 6.150012753347429
+                },
+                {
+                    "x": 2.826268683292055,
+                    "y": 5.961897271043331,
+                    "heading": -1.6168780413299741,
+                    "angularVelocity": 0.0000033462116195770168,
+                    "velocityX": 3.6001657986623896,
+                    "velocityY": 1.1616528437290037,
+                    "timestamp": 6.2104746481056035
+                },
+                {
+                    "x": 3.0439415289220437,
+                    "y": 6.032133003028614,
+                    "heading": -1.6168778390116512,
+                    "angularVelocity": 0.0000033462120828803855,
+                    "velocityX": 3.6001657986505493,
+                    "velocityY": 1.1616528437654758,
+                    "timestamp": 6.270936542863778
+                },
+                {
+                    "x": 3.2616143745520296,
+                    "y": 6.102368735013889,
+                    "heading": -1.6168776366933004,
+                    "angularVelocity": 0.000003346212545975565,
+                    "velocityX": 3.600165798650516,
+                    "velocityY": 1.1616528437653553,
+                    "timestamp": 6.331398437621952
+                },
+                {
+                    "x": 3.479287220182014,
+                    "y": 6.172604466999156,
+                    "heading": -1.6168774343749215,
+                    "angularVelocity": 0.0000033462130098357135,
+                    "velocityX": 3.6001657986504836,
+                    "velocityY": 1.1616528437652343,
+                    "timestamp": 6.391860332380126
+                },
+                {
+                    "x": 3.6969600658119965,
+                    "y": 6.242840198984417,
+                    "heading": -1.6168772320565146,
+                    "angularVelocity": 0.000003346213473053834,
+                    "velocityX": 3.6001657986504503,
+                    "velocityY": 1.1616528437651126,
+                    "timestamp": 6.452322227138301
+                },
+                {
+                    "x": 3.914632911441977,
+                    "y": 6.313075930969669,
+                    "heading": -1.61687702973808,
+                    "angularVelocity": 0.000003346213936555426,
+                    "velocityX": 3.6001657986504174,
+                    "velocityY": 1.1616528437649918,
+                    "timestamp": 6.512784121896475
+                },
+                {
+                    "x": 4.132305757071955,
+                    "y": 6.383311662954915,
+                    "heading": -1.616876827419617,
+                    "angularVelocity": 0.0000033462143992467285,
+                    "velocityX": 3.6001657986503846,
+                    "velocityY": 1.161652843764871,
+                    "timestamp": 6.573246016654649
+                },
+                {
+                    "x": 4.349978602701932,
+                    "y": 6.453547394940153,
+                    "heading": -1.6168766251011262,
+                    "angularVelocity": 0.000003346214863174556,
+                    "velocityX": 3.600165798650352,
+                    "velocityY": 1.1616528437647493,
+                    "timestamp": 6.633707911412824
+                },
+                {
+                    "x": 4.567651448331906,
+                    "y": 6.523783126925384,
+                    "heading": -1.6168764227826073,
+                    "angularVelocity": 0.000003346215326740712,
+                    "velocityX": 3.6001657986503193,
+                    "velocityY": 1.1616528437646283,
+                    "timestamp": 6.694169806170998
+                },
+                {
+                    "x": 4.785324293961879,
+                    "y": 6.594018858910608,
+                    "heading": -1.6168762204640603,
+                    "angularVelocity": 0.0000033462157905377784,
+                    "velocityX": 3.6001657986502864,
+                    "velocityY": 1.161652843764507,
+                    "timestamp": 6.754631700929172
+                },
+                {
+                    "x": 5.002997139591849,
+                    "y": 6.6642545908958235,
+                    "heading": -1.6168760181454853,
+                    "angularVelocity": 0.0000033462162531242337,
+                    "velocityX": 3.600165798650253,
+                    "velocityY": 1.1616528437643858,
+                    "timestamp": 6.815093595687347
+                },
+                {
+                    "x": 5.220669985221817,
+                    "y": 6.734490322881032,
+                    "heading": -1.6168758158268823,
+                    "angularVelocity": 0.000003346216717473229,
+                    "velocityX": 3.6001657986502207,
+                    "velocityY": 1.1616528437642646,
+                    "timestamp": 6.875555490445521
+                },
+                {
+                    "x": 5.438342830851783,
+                    "y": 6.804726054866234,
+                    "heading": -1.6168756135082514,
+                    "angularVelocity": 0.000003346217180660181,
+                    "velocityX": 3.600165798650188,
+                    "velocityY": 1.1616528437641434,
+                    "timestamp": 6.936017385203695
+                },
+                {
+                    "x": 5.656015676481748,
+                    "y": 6.874961786851428,
+                    "heading": -1.6168754111895922,
+                    "angularVelocity": 0.0000033462176438124147,
+                    "velocityX": 3.600165798650156,
+                    "velocityY": 1.1616528437640228,
+                    "timestamp": 6.99647927996187
+                },
+                {
+                    "x": 5.873688522111711,
+                    "y": 6.945197518836615,
+                    "heading": -1.6168752088709053,
+                    "angularVelocity": 0.0000033462181070326184,
+                    "velocityX": 3.6001657986501225,
+                    "velocityY": 1.161652843763901,
+                    "timestamp": 7.056941174720044
+                },
+                {
+                    "x": 6.091361367741672,
+                    "y": 7.015433250821794,
+                    "heading": -1.6168750065521902,
+                    "angularVelocity": 0.0000033462185712785595,
+                    "velocityX": 3.6001657986500892,
+                    "velocityY": 1.1616528437637803,
+                    "timestamp": 7.117403069478218
+                },
+                {
+                    "x": 6.30903421337163,
+                    "y": 7.0856689828069666,
+                    "heading": -1.616874804233447,
+                    "angularVelocity": 0.0000033462190342827524,
+                    "velocityX": 3.600165798650057,
+                    "velocityY": 1.1616528437636586,
+                    "timestamp": 7.1778649642363925
+                },
+                {
+                    "x": 6.526707059002331,
+                    "y": 7.155904714789823,
+                    "heading": -1.6168746019146758,
+                    "angularVelocity": 0.0000033462194986739715,
+                    "velocityX": 3.6001657986623425,
+                    "velocityY": 1.1616528437253608,
+                    "timestamp": 7.238326858994567
+                },
+                {
+                    "x": 6.744379956765645,
+                    "y": 7.22614028520157,
+                    "heading": -1.6168743995934785,
+                    "angularVelocity": 0.0000033462596273946,
+                    "velocityX": 3.600166660901485,
+                    "velocityY": 1.1616501714453904,
+                    "timestamp": 7.298788753752741
+                },
+                {
+                    "x": 6.948380246935694,
+                    "y": 7.286306890939545,
+                    "heading": -1.572684837353693,
+                    "angularVelocity": 0.7308663153301367,
+                    "velocityX": 3.37403071779302,
+                    "velocityY": 0.9951161136881308,
+                    "timestamp": 7.3592506485109155
+                },
+                {
+                    "x": 7.128326014252639,
+                    "y": 7.34087027160758,
+                    "heading": -1.456360393496805,
+                    "angularVelocity": 1.9239298457672127,
+                    "velocityX": 2.976184719924214,
+                    "velocityY": 0.9024424538177037,
+                    "timestamp": 7.41971254326909
+                },
+                {
+                    "x": 7.285215733567768,
+                    "y": 7.391288256354559,
+                    "heading": -1.2598604932432955,
+                    "angularVelocity": 3.249979198294029,
+                    "velocityX": 2.5948528398362565,
+                    "velocityY": 0.8338803298942721,
+                    "timestamp": 7.480174438027264
+                },
+                {
+                    "x": 7.415669802167391,
+                    "y": 7.433594551714448,
+                    "heading": -1.014978314704827,
+                    "angularVelocity": 4.050190281298798,
+                    "velocityX": 2.157624552147962,
+                    "velocityY": 0.6997183189362131,
+                    "timestamp": 7.540636332785438
+                },
+                {
+                    "x": 7.52162527778786,
+                    "y": 7.463369081831009,
+                    "heading": -0.7214685317692553,
+                    "angularVelocity": 4.854458896954922,
+                    "velocityX": 1.7524339262646644,
+                    "velocityY": 0.49245115846350496,
+                    "timestamp": 7.601098227543613
+                },
+                {
+                    "x": 7.599835823210802,
+                    "y": 7.482093660895173,
+                    "heading": -0.44961761921846705,
+                    "angularVelocity": 4.4962354163410385,
+                    "velocityX": 1.293551016483298,
+                    "velocityY": 0.30969223076874997,
+                    "timestamp": 7.661560122301787
+                },
+                {
+                    "x": 7.6517032735458015,
+                    "y": 7.492160163258041,
+                    "heading": -0.22315227789287845,
+                    "angularVelocity": 3.7455878984832975,
+                    "velocityX": 0.8578535380416231,
+                    "velocityY": 0.16649333275330752,
+                    "timestamp": 7.722022017059961
+                },
+                {
+                    "x": 7.67973874704759,
+                    "y": 7.4966228089126075,
+                    "heading": -0.06798236605855336,
+                    "angularVelocity": 2.5664083544676926,
+                    "velocityX": 0.46368830507082526,
+                    "velocityY": 0.07380922599953843,
+                    "timestamp": 7.782483911818136
+                },
+                {
+                    "x": 7.686149597167969,
+                    "y": 7.496679306030273,
+                    "heading": 0,
+                    "angularVelocity": 1.1243836523889659,
+                    "velocityX": 0.10603124738348535,
+                    "velocityY": 0.0009344251927952234,
+                    "timestamp": 7.84294580657631
+                },
+                {
+                    "x": 7.671082178881759,
+                    "y": 7.493118035056542,
+                    "heading": -0.029242529216573936,
+                    "angularVelocity": -0.471080674899979,
+                    "velocityX": -0.24272762190641095,
+                    "velocityY": -0.05737006951015711,
+                    "timestamp": 7.905021223324796
+                },
+                {
+                    "x": 7.633475390199317,
+                    "y": 7.486720174316825,
+                    "heading": -0.1516714089178372,
+                    "angularVelocity": -1.9722602942371796,
+                    "velocityX": -0.6058241837475781,
+                    "velocityY": -0.1030659329383018,
+                    "timestamp": 7.967096640073281
+                },
+                {
+                    "x": 7.569735009380553,
+                    "y": 7.475264417263962,
+                    "heading": -0.33932636884787254,
+                    "angularVelocity": -3.023015708301534,
+                    "velocityX": -1.0268216333854858,
+                    "velocityY": -0.1845457936960604,
+                    "timestamp": 8.029172056821766
+                },
+                {
+                    "x": 7.480376336028035,
+                    "y": 7.452059634472421,
+                    "heading": -0.582197860853168,
+                    "angularVelocity": -3.912522939464948,
+                    "velocityX": -1.4395178966671722,
+                    "velocityY": -0.37381598073777733,
+                    "timestamp": 8.09124747357025
+                },
+                {
+                    "x": 7.362878563951424,
+                    "y": 7.417380486057705,
+                    "heading": -0.863364111736863,
+                    "angularVelocity": -4.529429935571964,
+                    "velocityX": -1.8928229278376825,
+                    "velocityY": -0.5586615480203228,
+                    "timestamp": 8.153322890318735
+                },
+                {
+                    "x": 7.218864923659752,
+                    "y": 7.37005336377927,
+                    "heading": -1.0948198175456743,
+                    "angularVelocity": -3.7286210537516657,
+                    "velocityX": -2.319978629788013,
+                    "velocityY": -0.7624132830262488,
+                    "timestamp": 8.21539830706722
+                },
+                {
+                    "x": 7.047313606455814,
+                    "y": 7.316250565275482,
+                    "heading": -1.2668868462563043,
+                    "angularVelocity": -2.7719029162189006,
+                    "velocityX": -2.7635950943192666,
+                    "velocityY": -0.8667327796087775,
+                    "timestamp": 8.277473723815705
+                },
+                {
+                    "x": 6.850682482856188,
+                    "y": 7.25802494172709,
+                    "heading": -1.3587604988602537,
+                    "angularVelocity": -1.4800327958521704,
+                    "velocityX": -3.16761664921765,
+                    "velocityY": -0.9379819999325716,
+                    "timestamp": 8.33954914056419
+                },
+                {
+                    "x": 6.62975279957275,
+                    "y": 7.191687511601611,
+                    "heading": -1.3718894789288036,
+                    "angularVelocity": -0.21150047404023747,
+                    "velocityX": -3.5590527596228974,
+                    "velocityY": -1.0686586349353433,
+                    "timestamp": 8.401624557312674
+                },
+                {
+                    "x": 6.40649462388328,
+                    "y": 7.118888182289737,
+                    "heading": -1.3718898557853474,
+                    "angularVelocity": -0.000006070946652807348,
+                    "velocityX": -3.596563460766736,
+                    "velocityY": -1.1727561911801452,
+                    "timestamp": 8.463699974061159
+                },
+                {
+                    "x": 6.183236472249506,
+                    "y": 7.0460887792034885,
+                    "heading": -1.3718902326404057,
+                    "angularVelocity": -0.000006070922729380904,
+                    "velocityX": -3.5965630732430243,
+                    "velocityY": -1.1727573796437725,
+                    "timestamp": 8.525775390809644
+                },
+                {
+                    "x": 5.959978320616,
+                    "y": 6.973289376116265,
+                    "heading": -1.3718906094952235,
+                    "angularVelocity": -0.000006070918851907899,
+                    "velocityX": -3.5965630732387113,
+                    "velocityY": -1.172757379659484,
+                    "timestamp": 8.587850807558128
+                },
+                {
+                    "x": 5.736720168982443,
+                    "y": 6.900489973029042,
+                    "heading": -1.3718909863498006,
+                    "angularVelocity": -0.000006070914972563024,
+                    "velocityX": -3.596563073239526,
+                    "velocityY": -1.1727573796594701,
+                    "timestamp": 8.649926224306613
+                },
+                {
+                    "x": 5.513462017348836,
+                    "y": 6.8276905699418196,
+                    "heading": -1.3718913632041367,
+                    "angularVelocity": -0.000006070911095039567,
+                    "velocityX": -3.5965630732403415,
+                    "velocityY": -1.1727573796594566,
+                    "timestamp": 8.712001641055098
+                },
+                {
+                    "x": 5.290203865715178,
+                    "y": 6.7548911668545975,
+                    "heading": -1.3718917400582324,
+                    "angularVelocity": -0.0000060709072179749485,
+                    "velocityX": -3.596563073241156,
+                    "velocityY": -1.1727573796594426,
+                    "timestamp": 8.774077057803582
+                },
+                {
+                    "x": 5.066945714081469,
+                    "y": 6.682091763767377,
+                    "heading": -1.3718921169120872,
+                    "angularVelocity": -0.000006070903338953198,
+                    "velocityX": -3.596563073241971,
+                    "velocityY": -1.1727573796594288,
+                    "timestamp": 8.836152474552067
+                },
+                {
+                    "x": 4.84368756244771,
+                    "y": 6.609292360680157,
+                    "heading": -1.3718924937657013,
+                    "angularVelocity": -0.000006070899462290803,
+                    "velocityX": -3.596563073242785,
+                    "velocityY": -1.1727573796594153,
+                    "timestamp": 8.898227891300552
+                },
+                {
+                    "x": 4.620429410813901,
+                    "y": 6.536492957592937,
+                    "heading": -1.371892870619075,
+                    "angularVelocity": -0.000006070895585804876,
+                    "velocityX": -3.5965630732436003,
+                    "velocityY": -1.1727573796594017,
+                    "timestamp": 8.960303308049037
+                },
+                {
+                    "x": 4.39717125918004,
+                    "y": 6.46369355450572,
+                    "heading": -1.3718932474722079,
+                    "angularVelocity": -0.000006070891709350806,
+                    "velocityX": -3.5965630732444147,
+                    "velocityY": -1.172757379659388,
+                    "timestamp": 9.022378724797521
+                },
+                {
+                    "x": 4.173913107546129,
+                    "y": 6.390894151418502,
+                    "heading": -1.3718936243251003,
+                    "angularVelocity": -0.000006070887832958865,
+                    "velocityX": -3.5965630732452283,
+                    "velocityY": -1.1727573796593747,
+                    "timestamp": 9.084454141546006
+                },
+                {
+                    "x": 3.950654955912168,
+                    "y": 6.318094748331286,
+                    "heading": -1.3718940011777518,
+                    "angularVelocity": -0.000006070883956378075,
+                    "velocityX": -3.596563073246043,
+                    "velocityY": -1.1727573796593613,
+                    "timestamp": 9.14652955829449
+                },
+                {
+                    "x": 3.7273968042781562,
+                    "y": 6.24529534524407,
+                    "heading": -1.3718943780301627,
+                    "angularVelocity": -0.000006070880081532027,
+                    "velocityX": -3.5965630732468568,
+                    "velocityY": -1.1727573796593478,
+                    "timestamp": 9.208604975042975
+                },
+                {
+                    "x": 3.504138652644094,
+                    "y": 6.172495942156855,
+                    "heading": -1.3718947548823335,
+                    "angularVelocity": -0.000006070876206601658,
+                    "velocityX": -3.5965630732476703,
+                    "velocityY": -1.1727573796593347,
+                    "timestamp": 9.27068039179146
+                },
+                {
+                    "x": 3.2808805010099813,
+                    "y": 6.099696539069641,
+                    "heading": -1.3718951317342634,
+                    "angularVelocity": -0.000006070872330584258,
+                    "velocityX": -3.596563073248485,
+                    "velocityY": -1.1727573796593216,
+                    "timestamp": 9.332755808539945
+                },
+                {
+                    "x": 3.057622349375818,
+                    "y": 6.026897135982428,
+                    "heading": -1.371895508585953,
+                    "angularVelocity": -0.000006070868457801708,
+                    "velocityX": -3.5965630732492984,
+                    "velocityY": -1.1727573796593085,
+                    "timestamp": 9.39483122528843
+                },
+                {
+                    "x": 2.834364197741604,
+                    "y": 5.954097732895216,
+                    "heading": -1.371895885437402,
+                    "angularVelocity": -0.0000060708645841945965,
+                    "velocityX": -3.596563073250112,
+                    "velocityY": -1.1727573796592947,
+                    "timestamp": 9.456906642036914
+                },
+                {
+                    "x": 2.6111060461064435,
+                    "y": 5.881298329810753,
+                    "heading": -1.3718962622886106,
+                    "angularVelocity": -0.000006070860710280736,
+                    "velocityX": -3.5965630732653646,
+                    "velocityY": -1.1727573796150002,
+                    "timestamp": 9.518982058785399
+                },
+                {
+                    "x": 2.387847826730136,
+                    "y": 5.808499134475022,
+                    "heading": -1.37189663914262,
+                    "angularVelocity": -0.000006070905830150403,
+                    "velocityX": -3.5965641645370763,
+                    "velocityY": -1.1727540328999504,
+                    "timestamp": 9.581057475533884
+                },
+                {
+                    "x": 2.167695143748949,
+                    "y": 5.749324083617281,
+                    "heading": -1.3936267357407348,
+                    "angularVelocity": -0.3500596167748615,
+                    "velocityX": -3.546535722397048,
+                    "velocityY": -0.9532767391237054,
+                    "timestamp": 9.643132892282368
+                },
+                {
+                    "x": 1.9716359535895844,
+                    "y": 5.700085594404985,
+                    "heading": -1.4887110872196767,
+                    "angularVelocity": -1.5317553463104556,
+                    "velocityX": -3.1584031236350607,
+                    "velocityY": -0.7932043277582467,
+                    "timestamp": 9.705208309030853
+                },
+                {
+                    "x": 1.8021216559438722,
+                    "y": 5.6575856667748505,
+                    "heading": -1.6480423798805228,
+                    "angularVelocity": -2.5667373818273083,
+                    "velocityX": -2.73077985658225,
+                    "velocityY": -0.6846498961470444,
+                    "timestamp": 9.767283725779338
+                },
+                {
+                    "x": 1.6601304337176481,
+                    "y": 5.619404991825654,
+                    "heading": -1.8664040726402535,
+                    "angularVelocity": -3.5176838786999975,
+                    "velocityX": -2.2873986138754163,
+                    "velocityY": -0.6150691682650391,
+                    "timestamp": 9.829359142527823
+                },
+                {
+                    "x": 1.5437233022892554,
+                    "y": 5.5885120196672124,
+                    "heading": -2.146539077662866,
+                    "angularVelocity": -4.5128171455965,
+                    "velocityX": -1.8752533212953901,
+                    "velocityY": -0.49766838108574224,
+                    "timestamp": 9.891434559276307
+                },
+                {
+                    "x": 1.452238647018182,
+                    "y": 5.5675498489404625,
+                    "heading": -2.4956493740995165,
+                    "angularVelocity": -5.623970240121965,
+                    "velocityX": -1.4737662679212806,
+                    "velocityY": -0.33768876351944366,
+                    "timestamp": 9.953509976024792
+                },
+                {
+                    "x": 1.3894112242957146,
+                    "y": 5.555821056487952,
+                    "heading": -2.802567916985108,
+                    "angularVelocity": -4.94428485481059,
+                    "velocityX": -1.0121143926754297,
+                    "velocityY": -0.188944240197889,
+                    "timestamp": 10.015585392773277
+                },
+                {
+                    "x": 1.3504462657255363,
+                    "y": 5.550087016097331,
+                    "heading": -3.0243894075867264,
+                    "angularVelocity": -3.573419273210609,
+                    "velocityX": -0.6277035356533314,
+                    "velocityY": -0.09237216100947475,
+                    "timestamp": 10.077660809521761
+                },
+                {
+                    "x": 1.3321380615234375,
+                    "y": 5.547595977783203,
+                    "heading": -3.1411853071795863,
+                    "angularVelocity": -1.8815161574522963,
+                    "velocityX": -0.2949348576470964,
+                    "velocityY": -0.040129224169711215,
+                    "timestamp": 10.139736226270246
+                },
+                {
+                    "x": 1.3321380615234375,
+                    "y": 5.547595977783203,
+                    "heading": -3.1411853071795863,
+                    "angularVelocity": 7.133789284871957e-33,
+                    "velocityX": 0,
+                    "velocityY": 1.0797204950028118e-32,
+                    "timestamp": 10.201811643018731
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint",
+                    "uuid": "54940582-6fbe-4b4f-8291-c0747b2aece5"
+                },
+                {
+                    "scope": [
+                        2
+                    ],
+                    "type": "StopPoint",
+                    "uuid": "13f02721-6063-4019-8ee4-d455a26aa8d3"
+                },
+                {
+                    "scope": [
+                        4
+                    ],
+                    "type": "StopPoint",
+                    "uuid": "6c01d07d-0f28-46b2-a0e7-90f87c695873"
+                },
+                {
+                    "scope": [
+                        6
+                    ],
+                    "type": "StopPoint",
+                    "uuid": "fe0a509c-156e-4805-91a2-25eb8ebb28f9"
+                },
+                {
+                    "scope": [
+                        8
+                    ],
+                    "type": "StopPoint",
+                    "uuid": "45b66dc3-77d0-41da-b7d8-c5d876251180"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        }
+    },
+    "splitTrajectoriesAtStopPoints": false
+}
\ No newline at end of file
diff --git a/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat
index cf540f36cf43283c2e796672c8adcfe17aaebcfe..845bd633a464596ff2fc8b16816fd5ff93c4eb9a 100644
GIT binary patch
delta 30
jcmZn=Xb{-Yz{qxxLH4r}&qP5hwnGe3pH)DtMNDh}kLn1%

delta 30
kcmZn=Xb{-Yz{qx#;o{dU&WVCnY{wYRea!)}7BR5_0IbRiyZ`_I

diff --git a/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat
index f73fac0924be3a7d77130abbe2ef5c3a9965013a..be975d9a20ff795a6664c77dcf57cca3a1d42c1d 100644
GIT binary patch
delta 30
jcmZn=Xb{-Yz{qxxLH4r}&qP5hwnGe3pH)DtMNDh}kLn1%

delta 30
kcmZn=Xb{-Yz{qx#;o{dU&WVCnY{wYRea!)}7BR5_0IbRiyZ`_I

diff --git a/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat
index aeead252ecff15156b97ea07266cd015f82ea35b..c5ac962af08c491a8154630ad9826964b7d0124b 100644
GIT binary patch
delta 30
jcmZn=Xb{-Yz{qxxLH4r}&qP5hwnGe3pH)DtMNDh}kLn1%

delta 30
kcmZn=Xb{-Yz{qx#;o{dU&WVCnY{wYRea!)}7BR5_0IbRiyZ`_I

diff --git a/deploy/choreo/NewPath.1.traj b/deploy/choreo/NewPath.1.traj
new file mode 100644
index 00000000..92058ea3
--- /dev/null
+++ b/deploy/choreo/NewPath.1.traj
@@ -0,0 +1,1876 @@
+{
+    "samples": [
+        {
+            "x": 1.3321385383605957,
+            "y": 5.547596454620361,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 2.2261748232194093e-37,
+            "velocityY": 3.7157891353428806e-37,
+            "timestamp": 0
+        },
+        {
+            "x": 1.366635963151808,
+            "y": 5.509495705296941,
+            "heading": 0.06192923642991932,
+            "angularVelocity": 0.7673697851265452,
+            "velocityX": 0.4274601622031667,
+            "velocityY": -0.47210922509207903,
+            "timestamp": 0.08070325106650729
+        },
+        {
+            "x": 1.4355761410472572,
+            "y": 5.43294196238783,
+            "heading": 0.18410628088281034,
+            "angularVelocity": 1.5139048655202914,
+            "velocityX": 0.8542428834575204,
+            "velocityY": -0.9485831350960487,
+            "timestamp": 0.16140650213301458
+        },
+        {
+            "x": 1.5392732266735116,
+            "y": 5.317548823896549,
+            "heading": 0.36164871867370346,
+            "angularVelocity": 2.1999415815923076,
+            "velocityX": 1.2849183181083754,
+            "velocityY": -1.429844981042792,
+            "timestamp": 0.24210975319952188
+        },
+        {
+            "x": 1.67847556185222,
+            "y": 5.162242328350244,
+            "heading": 0.5745390252788012,
+            "angularVelocity": 2.637939659080841,
+            "velocityX": 1.724866512056522,
+            "velocityY": -1.9244143636582545,
+            "timestamp": 0.32281300426602916
+        },
+        {
+            "x": 1.8501216668381886,
+            "y": 4.967122032744555,
+            "heading": 0.7214821568069908,
+            "angularVelocity": 1.8207832966616715,
+            "velocityX": 2.1268796822635494,
+            "velocityY": -2.417750128119265,
+            "timestamp": 0.40351625533253643
+        },
+        {
+            "x": 1.999355457938739,
+            "y": 4.729284669626315,
+            "heading": 0.7742712340167793,
+            "angularVelocity": 0.6541133908754838,
+            "velocityX": 1.849167030180327,
+            "velocityY": -2.9470604960169275,
+            "timestamp": 0.4842195063990437
+        },
+        {
+            "x": 2.1155301056872418,
+            "y": 4.524468188873081,
+            "heading": 0.7178805227908414,
+            "angularVelocity": -0.6987415064538932,
+            "velocityX": 1.4395287205067313,
+            "velocityY": -2.537896281085937,
+            "timestamp": 0.564922757465551
+        },
+        {
+            "x": 2.1994300453605806,
+            "y": 4.354650955915648,
+            "heading": 0.5603443624430987,
+            "angularVelocity": -1.952042306423536,
+            "velocityX": 1.0396104068248448,
+            "velocityY": -2.104217992624444,
+            "timestamp": 0.6456260085320583
+        },
+        {
+            "x": 2.250899907873132,
+            "y": 4.221459437616645,
+            "heading": 0.3158282415144345,
+            "angularVelocity": -3.029817481914815,
+            "velocityX": 0.637766903220982,
+            "velocityY": -1.6503860320229218,
+            "timestamp": 0.7263292595985656
+        },
+        {
+            "x": 2.2676985263824463,
+            "y": 4.124765396118164,
+            "heading": 2.2216292217564286e-35,
+            "angularVelocity": -3.913451284064894,
+            "velocityX": 0.20815293420422745,
+            "velocityY": -1.1981430762782603,
+            "timestamp": 0.807032510665073
+        },
+        {
+            "x": 2.2676338555313906,
+            "y": 4.086690967677836,
+            "heading": -0.16755058667496922,
+            "angularVelocity": -4.348312733244185,
+            "velocityX": -0.0016783533301519722,
+            "velocityY": -0.9881166355999816,
+            "timestamp": 0.8455648327296736
+        },
+        {
+            "x": 2.259511405376126,
+            "y": 4.056750075909778,
+            "heading": -0.352256708092011,
+            "angularVelocity": -4.793537257042952,
+            "velocityX": -0.21079576106643969,
+            "velocityY": -0.7770331546035791,
+            "timestamp": 0.8840971547942744
+        },
+        {
+            "x": 2.2434268935883566,
+            "y": 4.035188434921252,
+            "heading": -0.5534507965852989,
+            "angularVelocity": -5.22143690577431,
+            "velocityX": -0.41742908098825227,
+            "velocityY": -0.5595728425704051,
+            "timestamp": 0.9226294768588752
+        },
+        {
+            "x": 2.2194223708397587,
+            "y": 4.022359555236857,
+            "heading": -0.7686869662578265,
+            "angularVelocity": -5.585860341135864,
+            "velocityX": -0.6229710918629213,
+            "velocityY": -0.33293814120224735,
+            "timestamp": 0.961161798923476
+        },
+        {
+            "x": 2.1874734502455304,
+            "y": 4.018617397576101,
+            "heading": -0.9939561131825075,
+            "angularVelocity": -5.846238556477593,
+            "velocityX": -0.8291459969805253,
+            "velocityY": -0.09711736693372348,
+            "timestamp": 0.9996941209880768
+        },
+        {
+            "x": 2.1476082595993153,
+            "y": 4.024282627294418,
+            "heading": -1.2244083666470835,
+            "angularVelocity": -5.980751771933577,
+            "velocityX": -1.0345909228979182,
+            "velocityY": 0.14702539101636564,
+            "timestamp": 1.0382264430526775
+        },
+        {
+            "x": 2.1000522078031225,
+            "y": 4.039643723439363,
+            "heading": -1.4537096411283452,
+            "angularVelocity": -5.950881291213917,
+            "velocityX": -1.2341859833015967,
+            "velocityY": 0.3986548259197214,
+            "timestamp": 1.0767587651172783
+        },
+        {
+            "x": 2.045480509029303,
+            "y": 4.064764963970464,
+            "heading": -1.6725096563971036,
+            "angularVelocity": -5.678350110899954,
+            "velocityX": -1.4162577246792443,
+            "velocityY": 0.6519524177386574,
+            "timestamp": 1.115291087181879
+        },
+        {
+            "x": 1.9845995907026053,
+            "y": 4.099193425968209,
+            "heading": -1.8731412390257827,
+            "angularVelocity": -5.206838619596132,
+            "velocityX": -1.5799960932701846,
+            "velocityY": 0.8934956460714911,
+            "timestamp": 1.1538234092464799
+        },
+        {
+            "x": 1.9178343766904495,
+            "y": 4.143331162612739,
+            "heading": -2.0560809001827813,
+            "angularVelocity": -4.747693659631898,
+            "velocityX": -1.7327067364437998,
+            "velocityY": 1.1454730542979317,
+            "timestamp": 1.1923557313110806
+        },
+        {
+            "x": 1.846280610428652,
+            "y": 4.197802019829692,
+            "heading": -2.22146373822619,
+            "angularVelocity": -4.2920548044350495,
+            "velocityX": -1.856980384982645,
+            "velocityY": 1.413640660576612,
+            "timestamp": 1.2308880533756814
+        },
+        {
+            "x": 1.770704211681186,
+            "y": 4.2612245864567395,
+            "heading": -2.3576115042271706,
+            "angularVelocity": -3.5333392514659345,
+            "velocityX": -1.961376701377084,
+            "velocityY": 1.6459575553406294,
+            "timestamp": 1.2694203754402822
+        },
+        {
+            "x": 1.6915488741102875,
+            "y": 4.3341463409601095,
+            "heading": -2.466232613363222,
+            "angularVelocity": -2.8189608961002843,
+            "velocityX": -2.0542581741684773,
+            "velocityY": 1.8924827416607193,
+            "timestamp": 1.307952697504883
+        },
+        {
+            "x": 1.609995812708591,
+            "y": 4.417230005205154,
+            "heading": -2.5495370955191317,
+            "angularVelocity": -2.1619377626982037,
+            "velocityX": -2.1164844741246154,
+            "velocityY": 2.156207043680162,
+            "timestamp": 1.3464850195694837
+        },
+        {
+            "x": 1.5308705698098255,
+            "y": 4.511360824889822,
+            "heading": -2.6152779027765387,
+            "angularVelocity": -1.7061210883473432,
+            "velocityX": -2.053477150069217,
+            "velocityY": 2.442905452904058,
+            "timestamp": 1.3850173416340845
+        },
+        {
+            "x": 1.4606506153508025,
+            "y": 4.614058209006809,
+            "heading": -2.6783194867011537,
+            "angularVelocity": -1.636070201503133,
+            "velocityX": -1.822364983384516,
+            "velocityY": 2.6652269734694745,
+            "timestamp": 1.4235496636986853
+        },
+        {
+            "x": 1.4019701962785118,
+            "y": 4.720456086702728,
+            "heading": -2.7505666696039515,
+            "angularVelocity": -1.874976098810575,
+            "velocityX": -1.5228882124962768,
+            "velocityY": 2.761263064227987,
+            "timestamp": 1.462081985763286
+        },
+        {
+            "x": 1.3551627373603712,
+            "y": 4.8282147910275475,
+            "heading": -2.8346512790115326,
+            "angularVelocity": -2.1821838109473566,
+            "velocityX": -1.2147583226275982,
+            "velocityY": 2.7965795610282194,
+            "timestamp": 1.5006143078278869
+        },
+        {
+            "x": 1.3203598068196198,
+            "y": 4.936382222496368,
+            "heading": -2.9296853375556533,
+            "angularVelocity": -2.466346522921523,
+            "velocityX": -0.9032139428919735,
+            "velocityY": 2.8071869452215745,
+            "timestamp": 1.5391466298924876
+        },
+        {
+            "x": 1.2978728176694776,
+            "y": 5.0435429055984535,
+            "heading": -3.0274097940467435,
+            "angularVelocity": -2.53616837125081,
+            "velocityX": -0.583587698463697,
+            "velocityY": 2.7810595718167943,
+            "timestamp": 1.5776789519570884
+        },
+        {
+            "x": 1.286514559723769,
+            "y": 5.147138897368402,
+            "heading": -3.1101525298443558,
+            "angularVelocity": -2.1473591874087323,
+            "velocityX": -0.2947722155614252,
+            "velocityY": 2.688547853313019,
+            "timestamp": 1.6162112740216892
+        },
+        {
+            "x": 1.2850446377120623,
+            "y": 5.2469545287393595,
+            "heading": -3.168448488726713,
+            "angularVelocity": -1.5129106100748941,
+            "velocityX": -0.038147766159596044,
+            "velocityY": 2.5904390398173645,
+            "timestamp": 1.65474359608629
+        },
+        {
+            "x": 1.2925718642931594,
+            "y": 5.343394761009087,
+            "heading": -3.1958165160103977,
+            "angularVelocity": -0.7102615626901808,
+            "velocityX": 0.1953483770969554,
+            "velocityY": 2.5028398783764474,
+            "timestamp": 1.6932759181508907
+        },
+        {
+            "x": 1.3084372785500542,
+            "y": 5.436937425716649,
+            "heading": -3.187781062808085,
+            "angularVelocity": 0.20853799542215204,
+            "velocityX": 0.4117430096814787,
+            "velocityY": 2.4276415148491566,
+            "timestamp": 1.7318082402154915
+        },
+        {
+            "x": 1.3321385383605957,
+            "y": 5.52810525894165,
+            "heading": -3.1411853071795863,
+            "angularVelocity": 1.2092641484304996,
+            "velocityX": 0.6151007398621232,
+            "velocityY": 2.366009322567054,
+            "timestamp": 1.7703405622800923
+        },
+        {
+            "x": 1.357521739751378,
+            "y": 5.604487249946148,
+            "heading": -3.0708796741945465,
+            "angularVelocity": 2.1424744850655806,
+            "velocityX": 0.7735206841904706,
+            "velocityY": 2.3276437448542358,
+            "timestamp": 1.8031557175947133
+        },
+        {
+            "x": 1.3882591250435519,
+            "y": 5.675559897172117,
+            "heading": -2.9834964247525093,
+            "angularVelocity": 2.6628930628008614,
+            "velocityX": 0.936682608918773,
+            "velocityY": 2.1658482656731963,
+            "timestamp": 1.8359708729093343
+        },
+        {
+            "x": 1.423715934589619,
+            "y": 5.741061467273705,
+            "heading": -2.8764865415416963,
+            "angularVelocity": 3.260989691648163,
+            "velocityX": 1.0805010430735036,
+            "velocityY": 1.9960767966380097,
+            "timestamp": 1.8687860282239552
+        },
+        {
+            "x": 1.4640545833350669,
+            "y": 5.800765385011703,
+            "heading": -2.7509368125913465,
+            "angularVelocity": 3.8259678415847733,
+            "velocityX": 1.2292688655194213,
+            "velocityY": 1.8194007362017863,
+            "timestamp": 1.9016011835385762
+        },
+        {
+            "x": 1.5093560477878591,
+            "y": 5.854065962524638,
+            "heading": -2.610542457820649,
+            "angularVelocity": 4.278338878016634,
+            "velocityX": 1.3805043437538693,
+            "velocityY": 1.6242671107878839,
+            "timestamp": 1.9344163388531972
+        },
+        {
+            "x": 1.5593454812172984,
+            "y": 5.900364561226932,
+            "heading": -2.4585168251320484,
+            "angularVelocity": 4.6327872359898405,
+            "velocityX": 1.5233642184581169,
+            "velocityY": 1.4108907380872764,
+            "timestamp": 1.9672314941678182
+        },
+        {
+            "x": 1.612840729658989,
+            "y": 5.938459321827602,
+            "heading": -2.302873266045166,
+            "angularVelocity": 4.74303892803867,
+            "velocityX": 1.6301994590242108,
+            "velocityY": 1.1608892365563526,
+            "timestamp": 2.000046649482439
+        },
+        {
+            "x": 1.6683469717714092,
+            "y": 5.967831924392925,
+            "heading": -2.1497463391084874,
+            "angularVelocity": 4.666347773415882,
+            "velocityX": 1.6914819259651337,
+            "velocityY": 0.89509259620163,
+            "timestamp": 2.03286180479706
+        },
+        {
+            "x": 1.7259927125934769,
+            "y": 5.988487486256733,
+            "heading": -1.9957888127587393,
+            "angularVelocity": 4.691659231036796,
+            "velocityX": 1.756680420049179,
+            "velocityY": 0.6294519000677786,
+            "timestamp": 2.065676960111681
+        },
+        {
+            "x": 1.7857676152473871,
+            "y": 6.000514617426619,
+            "heading": -1.8372643934887982,
+            "angularVelocity": 4.830829467362277,
+            "velocityX": 1.8215639109675918,
+            "velocityY": 0.36651148088664176,
+            "timestamp": 2.098492115426302
+        },
+        {
+            "x": 1.8473787764602114,
+            "y": 6.0041716221205474,
+            "heading": -1.6689363232818417,
+            "angularVelocity": 5.129583224369399,
+            "velocityX": 1.877521548263803,
+            "velocityY": 0.11144255326131712,
+            "timestamp": 2.131307270740923
+        },
+        {
+            "x": 1.9079302163194551,
+            "y": 5.9989341122190964,
+            "heading": -1.4983965075420516,
+            "angularVelocity": 5.196983348233757,
+            "velocityX": 1.8452278917681775,
+            "velocityY": -0.15960643340660682,
+            "timestamp": 2.164122426055544
+        },
+        {
+            "x": 1.9644851606047329,
+            "y": 5.9857778753833975,
+            "heading": -1.3328127647297614,
+            "angularVelocity": 5.045953347614136,
+            "velocityX": 1.7234397869840146,
+            "velocityY": -0.40091953579256995,
+            "timestamp": 2.196937581370165
+        },
+        {
+            "x": 2.0167542311341298,
+            "y": 5.964732325175136,
+            "heading": -1.1669030928960278,
+            "angularVelocity": 5.055885618795508,
+            "velocityX": 1.592833251229742,
+            "velocityY": -0.6413362974053785,
+            "timestamp": 2.229752736684786
+        },
+        {
+            "x": 2.0647189114008015,
+            "y": 5.935938401754757,
+            "heading": -0.9958716358792914,
+            "angularVelocity": 5.211965489023056,
+            "velocityX": 1.4616624485485903,
+            "velocityY": -0.8774580874084462,
+            "timestamp": 2.262567891999407
+        },
+        {
+            "x": 2.1085721264603117,
+            "y": 5.8997079709351175,
+            "heading": -0.8141851452694338,
+            "angularVelocity": 5.53666404647202,
+            "velocityX": 1.3363707908452565,
+            "velocityY": -1.1040761645731791,
+            "timestamp": 2.295383047314028
+        },
+        {
+            "x": 2.1457870640137746,
+            "y": 5.857488430688145,
+            "heading": -0.6354555413511925,
+            "angularVelocity": 5.446556696277672,
+            "velocityX": 1.134077751473621,
+            "velocityY": -1.2865866348089932,
+            "timestamp": 2.328198202628649
+        },
+        {
+            "x": 2.1756614189381724,
+            "y": 5.811166545923024,
+            "heading": -0.4679136928264175,
+            "angularVelocity": 5.105624121490153,
+            "velocityX": 0.9103828593213182,
+            "velocityY": -1.41160035114879,
+            "timestamp": 2.36101335794327
+        },
+        {
+            "x": 2.1979477640451273,
+            "y": 5.762830427028998,
+            "heading": -0.3159012603830297,
+            "angularVelocity": 4.632384975354892,
+            "velocityX": 0.6791479392152971,
+            "velocityY": -1.4729815669191495,
+            "timestamp": 2.393828513257891
+        },
+        {
+            "x": 2.213866689003856,
+            "y": 5.714154085995567,
+            "heading": -0.19000720732011314,
+            "angularVelocity": 3.8364606797037886,
+            "velocityX": 0.4851089323242057,
+            "velocityY": -1.483349402638423,
+            "timestamp": 2.426643668572512
+        },
+        {
+            "x": 2.2241033767575633,
+            "y": 5.665307000563528,
+            "heading": -0.09437057287174495,
+            "angularVelocity": 2.9144044430518514,
+            "velocityX": 0.31195000162458963,
+            "velocityY": -1.4885526203886283,
+            "timestamp": 2.459458823887133
+        },
+        {
+            "x": 2.228985536910567,
+            "y": 5.616279831781338,
+            "heading": -0.030711851451385126,
+            "angularVelocity": 1.9399183337704993,
+            "velocityX": 0.14877760309816687,
+            "velocityY": -1.4940404307745467,
+            "timestamp": 2.492273979201754
+        },
+        {
+            "x": 2.2287168502807617,
+            "y": 5.567087173461914,
+            "heading": -1.4593920868727723e-34,
+            "angularVelocity": 0.9359044976910779,
+            "velocityX": -0.008187882313191258,
+            "velocityY": -1.4990835133273062,
+            "timestamp": 2.525089134516375
+        },
+        {
+            "x": 2.2234617571142565,
+            "y": 5.5179127265043535,
+            "heading": -0.002801304121864836,
+            "angularVelocity": -0.08554590281020508,
+            "velocityX": -0.16047943019522937,
+            "velocityY": -1.5016836006283458,
+            "timestamp": 2.5578353447946145
+        },
+        {
+            "x": 2.213100127893157,
+            "y": 5.468804050064043,
+            "heading": -0.038527021013621016,
+            "angularVelocity": -1.090987829987043,
+            "velocityX": -0.31642224040761213,
+            "velocityY": -1.499675108143563,
+            "timestamp": 2.590581555072854
+        },
+        {
+            "x": 2.1974774857014916,
+            "y": 5.420028778568237,
+            "heading": -0.10642175719814694,
+            "angularVelocity": -2.0733616381142825,
+            "velocityX": -0.47708244889780627,
+            "velocityY": -1.4894936263271208,
+            "timestamp": 2.623327765351094
+        },
+        {
+            "x": 2.176386509447901,
+            "y": 5.371982692865446,
+            "heading": -0.2052939647176804,
+            "angularVelocity": -3.019348091868672,
+            "velocityX": -0.6440738050108257,
+            "velocityY": -1.4672258345179665,
+            "timestamp": 2.6560739756293335
+        },
+        {
+            "x": 2.1495039837265555,
+            "y": 5.325205721708642,
+            "heading": -0.3329902489476004,
+            "angularVelocity": -3.8995744284576235,
+            "velocityX": -0.8209354759811619,
+            "velocityY": -1.4284697606027426,
+            "timestamp": 2.688820185907573
+        },
+        {
+            "x": 2.1162335968543386,
+            "y": 5.280389038707846,
+            "heading": -0.4851442980051073,
+            "angularVelocity": -4.646462835383888,
+            "velocityX": -1.0160072444879351,
+            "velocityY": -1.368606706546959,
+            "timestamp": 2.7215663961858128
+        },
+        {
+            "x": 2.0756862755494496,
+            "y": 5.238416301489077,
+            "heading": -0.6539149158793239,
+            "angularVelocity": -5.153897701144575,
+            "velocityX": -1.2382294305314772,
+            "velocityY": -1.2817586176272708,
+            "timestamp": 2.7543126064640524
+        },
+        {
+            "x": 2.0277154786481417,
+            "y": 5.201197274104196,
+            "heading": -0.8280730192796384,
+            "angularVelocity": -5.318420113977116,
+            "velocityX": -1.4649266737649096,
+            "velocityY": -1.136590373928381,
+            "timestamp": 2.787058816742292
+        },
+        {
+            "x": 1.9735902732105297,
+            "y": 5.170441088651969,
+            "heading": -1.0002810429175644,
+            "angularVelocity": -5.258868802670591,
+            "velocityX": -1.6528692932012015,
+            "velocityY": -0.9392288509386596,
+            "timestamp": 2.8198050270205317
+        },
+        {
+            "x": 1.914416507688317,
+            "y": 5.147054029448202,
+            "heading": -1.1711019741635995,
+            "angularVelocity": -5.216509934877801,
+            "velocityX": -1.807041639915639,
+            "velocityY": -0.7141913218369506,
+            "timestamp": 2.8525512372987714
+        },
+        {
+            "x": 1.851848295060789,
+            "y": 5.131935997009135,
+            "heading": -1.3411756027953594,
+            "angularVelocity": -5.193688893666442,
+            "velocityX": -1.9107008748766678,
+            "velocityY": -0.46167273435953404,
+            "timestamp": 2.885297447577011
+        },
+        {
+            "x": 1.788148478011349,
+            "y": 5.125682164538362,
+            "heading": -1.5101885639025132,
+            "angularVelocity": -5.161298350895432,
+            "velocityX": -1.945257680451938,
+            "velocityY": -0.1909788161022263,
+            "timestamp": 2.9180436578552507
+        },
+        {
+            "x": 1.7253768027949503,
+            "y": 5.1283168367810115,
+            "heading": -1.680184610910991,
+            "angularVelocity": -5.191319714985216,
+            "velocityX": -1.9169141919946477,
+            "velocityY": 0.0804573176640312,
+            "timestamp": 2.9507898681334903
+        },
+        {
+            "x": 1.665253742824933,
+            "y": 5.13946824377609,
+            "heading": -1.8524223717430155,
+            "angularVelocity": -5.2597769136808665,
+            "velocityX": -1.83603108448766,
+            "velocityY": 0.3405403831565884,
+            "timestamp": 2.98353607841173
+        },
+        {
+            "x": 1.6088715232725108,
+            "y": 5.158717654167419,
+            "heading": -2.026839169725991,
+            "angularVelocity": -5.326320099363601,
+            "velocityX": -1.7217937304301978,
+            "velocityY": 0.5878362786951496,
+            "timestamp": 3.0162822886899696
+        },
+        {
+            "x": 1.5568081254735793,
+            "y": 5.1857922946114,
+            "heading": -2.2021042643709907,
+            "angularVelocity": -5.3522252851795065,
+            "velocityX": -1.5899060488696688,
+            "velocityY": 0.826802253266321,
+            "timestamp": 3.0490284989682093
+        },
+        {
+            "x": 1.5093538981441093,
+            "y": 5.2205186818889695,
+            "heading": -2.3759898286014502,
+            "angularVelocity": -5.31009734418057,
+            "velocityX": -1.4491517316434035,
+            "velocityY": 1.060470417263673,
+            "timestamp": 3.081774709246449
+        },
+        {
+            "x": 1.4666702915238679,
+            "y": 5.262723781048631,
+            "heading": -2.545870980641176,
+            "angularVelocity": -5.187811065655255,
+            "velocityX": -1.30346706557996,
+            "velocityY": 1.2888544598306537,
+            "timestamp": 3.1145209195246886
+        },
+        {
+            "x": 1.4288865927350196,
+            "y": 5.312181622313189,
+            "heading": -2.7092363373229658,
+            "angularVelocity": -4.988832457059862,
+            "velocityX": -1.1538342442623262,
+            "velocityY": 1.510337863353399,
+            "timestamp": 3.147267129802928
+        },
+        {
+            "x": 1.3968131737185872,
+            "y": 5.367362011920862,
+            "heading": -2.8587947946716725,
+            "angularVelocity": -4.567198954563906,
+            "velocityX": -0.9794543778931859,
+            "velocityY": 1.6850923859223195,
+            "timestamp": 3.180013340081168
+        },
+        {
+            "x": 1.3707084315183937,
+            "y": 5.426032663472122,
+            "heading": -2.986331521157274,
+            "angularVelocity": -3.8947018724286298,
+            "velocityX": -0.7971836123443037,
+            "velocityY": 1.7916776033850628,
+            "timestamp": 3.2127595503594075
+        },
+        {
+            "x": 1.3494796795233661,
+            "y": 5.486294372047085,
+            "heading": -3.0820270140723585,
+            "angularVelocity": -2.9223379469554978,
+            "velocityX": -0.6482811847432148,
+            "velocityY": 1.8402651196253896,
+            "timestamp": 3.245505760637647
+        },
+        {
+            "x": 1.3321385383605957,
+            "y": 5.547596454620361,
+            "heading": -3.1411853071795863,
+            "angularVelocity": -1.8065691450878867,
+            "velocityX": -0.529561772657824,
+            "velocityY": 1.8720359410264953,
+            "timestamp": 3.278251970915887
+        },
+        {
+            "x": 1.316411922217005,
+            "y": 5.622158239816111,
+            "heading": -3.1574444739447625,
+            "angularVelocity": -0.4153661551974852,
+            "velocityX": -0.4017613064785548,
+            "velocityY": 1.9047988429364744,
+            "timestamp": 3.317396149038551
+        },
+        {
+            "x": 1.30673758444171,
+            "y": 5.698077663390306,
+            "heading": -3.122874776057638,
+            "angularVelocity": 0.8831376604407247,
+            "velocityX": -0.24714627408906595,
+            "velocityY": 1.9394818646157501,
+            "timestamp": 3.356540327161215
+        },
+        {
+            "x": 1.3044116698031423,
+            "y": 5.775906190222514,
+            "heading": -3.0431020650652782,
+            "angularVelocity": 2.0379201919217973,
+            "velocityX": -0.05941917163975373,
+            "velocityY": 1.9882529296776088,
+            "timestamp": 3.395684505283879
+        },
+        {
+            "x": 1.310735543939778,
+            "y": 5.856467416837523,
+            "heading": -2.9252067960348933,
+            "angularVelocity": 3.011821289514447,
+            "velocityX": 0.16155337625991328,
+            "velocityY": 2.058064071815707,
+            "timestamp": 3.434828683406543
+        },
+        {
+            "x": 1.3277929402719157,
+            "y": 5.939930987776361,
+            "heading": -2.7826616721505166,
+            "angularVelocity": 3.6415408553908346,
+            "velocityX": 0.43575819317718334,
+            "velocityY": 2.132209052321722,
+            "timestamp": 3.473972861529207
+        },
+        {
+            "x": 1.3576022335544815,
+            "y": 6.023554053808955,
+            "heading": -2.637362279761064,
+            "angularVelocity": 3.711903004685308,
+            "velocityX": 0.7615255885346219,
+            "velocityY": 2.136283606991328,
+            "timestamp": 3.513117039651871
+        },
+        {
+            "x": 1.4000554524537352,
+            "y": 6.106306047220255,
+            "heading": -2.4978098785002194,
+            "angularVelocity": 3.5650870181393826,
+            "velocityX": 1.0845346852403,
+            "velocityY": 2.1140306778695206,
+            "timestamp": 3.552261217774535
+        },
+        {
+            "x": 1.455157655616598,
+            "y": 6.18915881243997,
+            "heading": -2.364648055922988,
+            "angularVelocity": 3.4018295686257636,
+            "velocityX": 1.407673013090022,
+            "velocityY": 2.1166050532491445,
+            "timestamp": 3.591405395897199
+        },
+        {
+            "x": 1.5225297792403976,
+            "y": 6.27399259313729,
+            "heading": -2.2419731152862643,
+            "angularVelocity": 3.1339255674829567,
+            "velocityX": 1.7211275559977186,
+            "velocityY": 2.16721322980599,
+            "timestamp": 3.630549574019863
+        },
+        {
+            "x": 1.596991898632741,
+            "y": 6.365541704289421,
+            "heading": -2.152688244347013,
+            "angularVelocity": 2.2809233766376438,
+            "velocityX": 1.9022527222057353,
+            "velocityY": 2.3387669774352777,
+            "timestamp": 3.669693752142527
+        },
+        {
+            "x": 1.679413821374646,
+            "y": 6.463137271673749,
+            "heading": -2.0939697752685182,
+            "angularVelocity": 1.5000562508808342,
+            "velocityX": 2.1055985000789685,
+            "velocityY": 2.4932332741410055,
+            "timestamp": 3.708837930265191
+        },
+        {
+            "x": 1.7662309748150582,
+            "y": 6.5526721858065455,
+            "heading": -2.030942574296911,
+            "angularVelocity": 1.6101296283218078,
+            "velocityX": 2.2178816264415735,
+            "velocityY": 2.2873111258646657,
+            "timestamp": 3.747982108387855
+        },
+        {
+            "x": 1.8463290740680989,
+            "y": 6.633589645928569,
+            "heading": -1.9424440862784471,
+            "angularVelocity": 2.260833979989085,
+            "velocityX": 2.0462327501689237,
+            "velocityY": 2.0671646207121097,
+            "timestamp": 3.787126286510519
+        },
+        {
+            "x": 1.9201306346515887,
+            "y": 6.705414092889608,
+            "heading": -1.8297336528989434,
+            "angularVelocity": 2.8793664546055737,
+            "velocityX": 1.88537770169097,
+            "velocityY": 1.8348692042011108,
+            "timestamp": 3.826270464633183
+        },
+        {
+            "x": 1.9881022552719625,
+            "y": 6.7678945659852445,
+            "heading": -1.6930072188425063,
+            "angularVelocity": 3.4928932120629863,
+            "velocityX": 1.73644265584974,
+            "velocityY": 1.596162599195345,
+            "timestamp": 3.865414642755847
+        },
+        {
+            "x": 2.0508860920717975,
+            "y": 6.821005360416637,
+            "heading": -1.5311822314903374,
+            "angularVelocity": 4.134075489976246,
+            "velocityX": 1.6039125052796561,
+            "velocityY": 1.3567993244094194,
+            "timestamp": 3.904558820878511
+        },
+        {
+            "x": 2.105765126126845,
+            "y": 6.86764701678895,
+            "heading": -1.3406160520225758,
+            "angularVelocity": 4.868314743270257,
+            "velocityX": 1.401971804927838,
+            "velocityY": 1.1915349512807134,
+            "timestamp": 3.943702999001175
+        },
+        {
+            "x": 2.1526116639678397,
+            "y": 6.908337111426706,
+            "heading": -1.1177858581420712,
+            "angularVelocity": 5.692550069188692,
+            "velocityX": 1.1967689727497768,
+            "velocityY": 1.039492884746424,
+            "timestamp": 3.982847177123839
+        },
+        {
+            "x": 2.1904354446918113,
+            "y": 6.943500988064586,
+            "heading": -0.8664176462137954,
+            "angularVelocity": 6.421598919271669,
+            "velocityX": 0.966268358105399,
+            "velocityY": 0.8983168972839031,
+            "timestamp": 4.021991355246503
+        },
+        {
+            "x": 2.218208118213364,
+            "y": 6.971474589606098,
+            "heading": -0.6268162193807357,
+            "angularVelocity": 6.120997765804011,
+            "velocityX": 0.7094969125299588,
+            "velocityY": 0.7146299368925976,
+            "timestamp": 4.061135533369167
+        },
+        {
+            "x": 2.2378592256547205,
+            "y": 6.992029926313173,
+            "heading": -0.4114319933826866,
+            "angularVelocity": 5.502331031784905,
+            "velocityX": 0.5020186496131576,
+            "velocityY": 0.5251186177076611,
+            "timestamp": 4.10027971149183
+        },
+        {
+            "x": 2.2511803788337867,
+            "y": 7.004798124284798,
+            "heading": -0.22575032024531463,
+            "angularVelocity": 4.743532296310119,
+            "velocityX": 0.3403099469178361,
+            "velocityY": 0.32618383075033625,
+            "timestamp": 4.139423889614494
+        },
+        {
+            "x": 2.261045669673148,
+            "y": 7.01480850997709,
+            "heading": -0.09556543349291266,
+            "angularVelocity": 3.32577903013953,
+            "velocityX": 0.25202447241187803,
+            "velocityY": 0.2557311501323063,
+            "timestamp": 4.1785680677371575
+        },
+        {
+            "x": 2.266437621623355,
+            "y": 7.022862452660783,
+            "heading": -0.01995585424474321,
+            "angularVelocity": 1.931566400787266,
+            "velocityX": 0.1377459486647134,
+            "velocityY": 0.2057507161972769,
+            "timestamp": 4.217712245859821
+        },
+        {
+            "x": 2.2676985263824463,
+            "y": 7.028899669647217,
+            "heading": -4.525227980572114e-34,
+            "angularVelocity": 0.509803889155846,
+            "velocityX": 0.03221180823213118,
+            "velocityY": 0.1542302655458861,
+            "timestamp": 4.256856423982485
+        },
+        {
+            "x": 2.265418134279513,
+            "y": 7.032677037086849,
+            "heading": -0.03924023540233394,
+            "angularVelocity": -0.9804537079645166,
+            "velocityX": -0.056977713563887346,
+            "velocityY": 0.09438103198308487,
+            "timestamp": 4.2968789513935945
+        },
+        {
+            "x": 2.260299136724811,
+            "y": 7.032742565757645,
+            "heading": -0.13692518133038253,
+            "angularVelocity": -2.440749054267192,
+            "velocityX": -0.1279029058340032,
+            "velocityY": 0.0016372946696696046,
+            "timestamp": 4.3369014788047044
+        },
+        {
+            "x": 2.251431996366295,
+            "y": 7.026515227476796,
+            "heading": -0.2837029990611584,
+            "angularVelocity": -3.667380028829281,
+            "velocityX": -0.22155373316216304,
+            "velocityY": -0.15559582774173908,
+            "timestamp": 4.376924006215814
+        },
+        {
+            "x": 2.2367988748775502,
+            "y": 7.013230050225817,
+            "heading": -0.4715484199670708,
+            "angularVelocity": -4.693492216930044,
+            "velocityX": -0.36562212422103535,
+            "velocityY": -0.3319424861532188,
+            "timestamp": 4.416946533626924
+        },
+        {
+            "x": 2.214572389947544,
+            "y": 6.992840585930019,
+            "heading": -0.6937250351049046,
+            "angularVelocity": -5.551288974223076,
+            "velocityX": -0.5553493586673437,
+            "velocityY": -0.5094496928282007,
+            "timestamp": 4.456969061038034
+        },
+        {
+            "x": 2.182927977382888,
+            "y": 6.964829235368663,
+            "heading": -0.9374735446804039,
+            "angularVelocity": -6.090282781787505,
+            "velocityX": -0.7906650232157135,
+            "velocityY": -0.6998895965171087,
+            "timestamp": 4.496991588449144
+        },
+        {
+            "x": 2.1407843072832162,
+            "y": 6.928562668933627,
+            "heading": -1.1813002792094054,
+            "angularVelocity": -6.092237304866431,
+            "velocityX": -1.0529987191157046,
+            "velocityY": -0.9061538283803887,
+            "timestamp": 4.537014115860254
+        },
+        {
+            "x": 2.0895949461282366,
+            "y": 6.88678871775435,
+            "heading": -1.3936812170017512,
+            "angularVelocity": -5.3065348824870116,
+            "velocityX": -1.27901370718457,
+            "velocityY": -1.0437609486821178,
+            "timestamp": 4.577036643271364
+        },
+        {
+            "x": 2.0327245845451865,
+            "y": 6.835876323976478,
+            "heading": -1.5775207555282802,
+            "angularVelocity": -4.593401527048441,
+            "velocityX": -1.420958776513053,
+            "velocityY": -1.2720934201606515,
+            "timestamp": 4.617059170682474
+        },
+        {
+            "x": 1.969383401794422,
+            "y": 6.775670280789337,
+            "heading": -1.7350606577956689,
+            "angularVelocity": -3.9362807013446637,
+            "velocityX": -1.582638250206598,
+            "velocityY": -1.5043038778812523,
+            "timestamp": 4.657081698093584
+        },
+        {
+            "x": 1.8991012602387887,
+            "y": 6.706204931476766,
+            "heading": -1.8673610734924142,
+            "angularVelocity": -3.305648699737559,
+            "velocityX": -1.7560645491899718,
+            "velocityY": -1.7356562367745094,
+            "timestamp": 4.6971042255046935
+        },
+        {
+            "x": 1.8215077032950175,
+            "y": 6.627747409048215,
+            "heading": -1.9742080738130303,
+            "angularVelocity": -2.6696714883367685,
+            "velocityX": -1.938747049798569,
+            "velocityY": -1.960334029448928,
+            "timestamp": 4.737126752915803
+        },
+        {
+            "x": 1.7362669499337922,
+            "y": 6.540782802917572,
+            "heading": -2.0543346433472873,
+            "angularVelocity": -2.002036720749795,
+            "velocityX": -2.1298193511278303,
+            "velocityY": -2.172891412812259,
+            "timestamp": 4.777149280326913
+        },
+        {
+            "x": 1.6430739434891832,
+            "y": 6.445951687040137,
+            "heading": -2.1059605165739685,
+            "angularVelocity": -1.2899203665071555,
+            "velocityX": -2.328513776437315,
+            "velocityY": -2.3694434612620463,
+            "timestamp": 4.817171807738023
+        },
+        {
+            "x": 1.55728497087458,
+            "y": 6.34602370365502,
+            "heading": -2.1651565461839404,
+            "angularVelocity": -1.4790677510671213,
+            "velocityX": -2.143517118081594,
+            "velocityY": -2.496793427328095,
+            "timestamp": 4.857194335149133
+        },
+        {
+            "x": 1.4798885640406532,
+            "y": 6.2518908184674045,
+            "heading": -2.25892978627056,
+            "angularVelocity": -2.3430114526097965,
+            "velocityX": -1.9338210712910326,
+            "velocityY": -2.351997519314229,
+            "timestamp": 4.897216862560243
+        },
+        {
+            "x": 1.4136471471462948,
+            "y": 6.162688939074268,
+            "heading": -2.372895153669848,
+            "angularVelocity": -2.847530497727978,
+            "velocityX": -1.6551032925514553,
+            "velocityY": -2.2287917621208337,
+            "timestamp": 4.937239389971353
+        },
+        {
+            "x": 1.3600641421628377,
+            "y": 6.075598413595313,
+            "heading": -2.5001562416185403,
+            "angularVelocity": -3.179736417979598,
+            "velocityX": -1.3388211202419749,
+            "velocityY": -2.1760376246199,
+            "timestamp": 4.977261917382463
+        },
+        {
+            "x": 1.319394826243984,
+            "y": 5.989620775668987,
+            "heading": -2.6390216185129676,
+            "angularVelocity": -3.4696803494694044,
+            "velocityX": -1.016160611275252,
+            "velocityY": -2.148231096031668,
+            "timestamp": 5.017284444793573
+        },
+        {
+            "x": 1.2917069915865356,
+            "y": 5.904139479101947,
+            "heading": -2.789119490409166,
+            "angularVelocity": -3.75033466413552,
+            "velocityX": -0.6918062513404106,
+            "velocityY": -2.1358295464196915,
+            "timestamp": 5.057306972204683
+        },
+        {
+            "x": 1.2763632236019438,
+            "y": 5.822310174000512,
+            "heading": -2.9261084847899674,
+            "angularVelocity": -3.422797190533623,
+            "velocityX": -0.3833782866079771,
+            "velocityY": -2.0445811495333315,
+            "timestamp": 5.0973294996157925
+        },
+        {
+            "x": 1.2717668608990054,
+            "y": 5.744491133376307,
+            "heading": -3.036631348807283,
+            "angularVelocity": -2.7615163550774677,
+            "velocityX": -0.11484438890439615,
+            "velocityY": -1.9443809688691047,
+            "timestamp": 5.137352027026902
+        },
+        {
+            "x": 1.2769543336934923,
+            "y": 5.67003274843766,
+            "heading": -3.1136420724595726,
+            "angularVelocity": -1.9241844189708244,
+            "velocityX": 0.12961382326511559,
+            "velocityY": -1.8604118668922167,
+            "timestamp": 5.177374554438012
+        },
+        {
+            "x": 1.290967939745204,
+            "y": 5.598089181810031,
+            "heading": -3.1506602139394917,
+            "angularVelocity": -0.9249326285586663,
+            "velocityX": 0.35014295593490485,
+            "velocityY": -1.7975767969031158,
+            "timestamp": 5.217397081849122
+        },
+        {
+            "x": 1.3126477003097534,
+            "y": 5.52810525894165,
+            "heading": -3.1411853071795863,
+            "angularVelocity": 0.23673934088618437,
+            "velocityX": 0.5416889428759926,
+            "velocityY": -1.748613278454628,
+            "timestamp": 5.257419609260232
+        },
+        {
+            "x": 1.3612675355497719,
+            "y": 5.427793675126224,
+            "heading": -3.0142432356702726,
+            "angularVelocity": 2.1028160607727164,
+            "velocityX": 0.8053954784197191,
+            "velocityY": -1.6616777008649257,
+            "timestamp": 5.317787262845048
+        },
+        {
+            "x": 1.4285233143505713,
+            "y": 5.3377057272616115,
+            "heading": -2.7956250008289785,
+            "angularVelocity": 3.6214466168399086,
+            "velocityX": 1.1141029145071253,
+            "velocityY": -1.4923215085383357,
+            "timestamp": 5.378154916429864
+        },
+        {
+            "x": 1.5158281848971034,
+            "y": 5.264153119824496,
+            "heading": -2.514690848185286,
+            "angularVelocity": 4.653719930475422,
+            "velocityX": 1.4462193801167051,
+            "velocityY": -1.218410905001898,
+            "timestamp": 5.43852257001468
+        },
+        {
+            "x": 1.6230211167401283,
+            "y": 5.213340726984806,
+            "heading": -2.222036602459031,
+            "angularVelocity": 4.847865178577475,
+            "velocityX": 1.7756683501441766,
+            "velocityY": -0.841715551662108,
+            "timestamp": 5.498890223599496
+        },
+        {
+            "x": 1.749062978630215,
+            "y": 5.185595086249832,
+            "heading": -1.9510224888468373,
+            "angularVelocity": 4.489392870495166,
+            "velocityX": 2.0879039420175545,
+            "velocityY": -0.45961105140506303,
+            "timestamp": 5.5592578771843115
+        },
+        {
+            "x": 1.894299674373998,
+            "y": 5.179254614564922,
+            "heading": -1.7150026509570102,
+            "angularVelocity": 3.9097070015852564,
+            "velocityX": 2.4058694867066146,
+            "velocityY": -0.10503094469295028,
+            "timestamp": 5.619625530769127
+        },
+        {
+            "x": 2.058119871692227,
+            "y": 5.193402237247618,
+            "heading": -1.5227916684845806,
+            "angularVelocity": 3.1840061863987503,
+            "velocityX": 2.713708212761054,
+            "velocityY": 0.2343576707485958,
+            "timestamp": 5.679993184353943
+        },
+        {
+            "x": 2.2389183972423563,
+            "y": 5.227931703588396,
+            "heading": -1.382086082537418,
+            "angularVelocity": 2.3308109159729624,
+            "velocityX": 2.9949569813262746,
+            "velocityY": 0.5719862259059945,
+            "timestamp": 5.740360837938759
+        },
+        {
+            "x": 2.4352711849705373,
+            "y": 5.282232257814921,
+            "heading": -1.300964814671627,
+            "angularVelocity": 1.3437869959914348,
+            "velocityX": 3.252615864095299,
+            "velocityY": 0.8994975123595437,
+            "timestamp": 5.800728491523575
+        },
+        {
+            "x": 2.6508698274310176,
+            "y": 5.350520182077314,
+            "heading": -1.2935554192187855,
+            "angularVelocity": 0.12273784076155059,
+            "velocityX": 3.5714265779366063,
+            "velocityY": 1.1312005719494984,
+            "timestamp": 5.861096145108391
+        },
+        {
+            "x": 2.8627491717000777,
+            "y": 5.435718232774483,
+            "heading": -1.2935551586148022,
+            "angularVelocity": 0.000004316947366860679,
+            "velocityX": 3.5098157984784524,
+            "velocityY": 1.4113195666528,
+            "timestamp": 5.921463798693207
+        },
+        {
+            "x": 3.0746283658136546,
+            "y": 5.520916656893941,
+            "heading": -1.2935548980122595,
+            "angularVelocity": 0.000004316923507170679,
+            "velocityX": 3.5098133111284437,
+            "velocityY": 1.4113257524537706,
+            "timestamp": 5.981831452278023
+        },
+        {
+            "x": 3.2865075599250586,
+            "y": 5.606115081018762,
+            "heading": -1.2935546374096347,
+            "angularVelocity": 0.000004316924865568706,
+            "velocityX": 3.509813311092456,
+            "velocityY": 1.4113257525426077,
+            "timestamp": 6.042199105862839
+        },
+        {
+            "x": 3.4983867540364426,
+            "y": 5.691313505143594,
+            "heading": -1.293554376806928,
+            "angularVelocity": 0.0000043169262224547675,
+            "velocityX": 3.5098133110921186,
+            "velocityY": 1.4113257525427836,
+            "timestamp": 6.1025667594476545
+        },
+        {
+            "x": 3.710265948147806,
+            "y": 5.776511929268437,
+            "heading": -1.293554116204139,
+            "angularVelocity": 0.000004316927581395022,
+            "velocityX": 3.5098133110917824,
+            "velocityY": 1.4113257525429583,
+            "timestamp": 6.16293441303247
+        },
+        {
+            "x": 3.922145142259149,
+            "y": 5.861710353393289,
+            "heading": -1.2935538556012682,
+            "angularVelocity": 0.000004316928938543054,
+            "velocityX": 3.509813311091446,
+            "velocityY": 1.4113257525431333,
+            "timestamp": 6.223302066617286
+        },
+        {
+            "x": 4.134024336370472,
+            "y": 5.9469087775181535,
+            "heading": -1.2935535949983157,
+            "angularVelocity": 0.000004316930294815631,
+            "velocityX": 3.5098133110911096,
+            "velocityY": 1.4113257525433083,
+            "timestamp": 6.283669720202102
+        },
+        {
+            "x": 4.345903530481774,
+            "y": 6.0321072016430275,
+            "heading": -1.293553334395281,
+            "angularVelocity": 0.000004316931652676853,
+            "velocityX": 3.509813311090773,
+            "velocityY": 1.4113257525434828,
+            "timestamp": 6.344037373786918
+        },
+        {
+            "x": 4.557782724593057,
+            "y": 6.117305625767913,
+            "heading": -1.2935530737921643,
+            "angularVelocity": 0.000004316933011809188,
+            "velocityX": 3.509813311090437,
+            "velocityY": 1.4113257525436576,
+            "timestamp": 6.404405027371734
+        },
+        {
+            "x": 4.769661918704319,
+            "y": 6.202504049892808,
+            "heading": -1.2935528131889658,
+            "angularVelocity": 0.000004316934368820573,
+            "velocityX": 3.5098133110901,
+            "velocityY": 1.4113257525438323,
+            "timestamp": 6.46477268095655
+        },
+        {
+            "x": 4.9815411128155604,
+            "y": 6.287702474017714,
+            "heading": -1.2935525525856855,
+            "angularVelocity": 0.000004316935725471939,
+            "velocityX": 3.5098133110897636,
+            "velocityY": 1.4113257525440075,
+            "timestamp": 6.525140334541366
+        },
+        {
+            "x": 5.193420306926782,
+            "y": 6.37290089814263,
+            "heading": -1.293552291982323,
+            "angularVelocity": 0.000004316937084312971,
+            "velocityX": 3.5098133110894274,
+            "velocityY": 1.4113257525441825,
+            "timestamp": 6.5855079881261815
+        },
+        {
+            "x": 5.405299501037983,
+            "y": 6.458099322267557,
+            "heading": -1.2935520313788786,
+            "angularVelocity": 0.000004316938442183061,
+            "velocityX": 3.509813311089091,
+            "velocityY": 1.411325752544357,
+            "timestamp": 6.645875641710997
+        },
+        {
+            "x": 5.617178695149163,
+            "y": 6.543297746392494,
+            "heading": -1.2935517707753523,
+            "angularVelocity": 0.000004316939799293205,
+            "velocityX": 3.509813311088755,
+            "velocityY": 1.411325752544532,
+            "timestamp": 6.706243295295813
+        },
+        {
+            "x": 5.829057889260324,
+            "y": 6.628496170517443,
+            "heading": -1.293551510171744,
+            "angularVelocity": 0.000004316941156612923,
+            "velocityX": 3.5098133110884175,
+            "velocityY": 1.4113257525447067,
+            "timestamp": 6.766610948880629
+        },
+        {
+            "x": 6.040937083371463,
+            "y": 6.713694594642401,
+            "heading": -1.293551249568054,
+            "angularVelocity": 0.0000043169425150049,
+            "velocityX": 3.5098133110880814,
+            "velocityY": 1.411325752544882,
+            "timestamp": 6.826978602465445
+        },
+        {
+            "x": 6.252816277482583,
+            "y": 6.798893018767371,
+            "heading": -1.2935509889642816,
+            "angularVelocity": 0.000004316943872175867,
+            "velocityX": 3.5098133110877447,
+            "velocityY": 1.4113257525450569,
+            "timestamp": 6.887346256050261
+        },
+        {
+            "x": 6.464695471593406,
+            "y": 6.884091442893039,
+            "heading": -1.293550728360427,
+            "angularVelocity": 0.000004316945230336038,
+            "velocityX": 3.509813311082821,
+            "velocityY": 1.4113257525566392,
+            "timestamp": 6.947713909635077
+        },
+        {
+            "x": 6.676574646384632,
+            "y": 6.969289915063695,
+            "heading": -1.2935504677556948,
+            "angularVelocity": 0.000004316959775515561,
+            "velocityX": 3.5098129910505658,
+            "velocityY": 1.41132654842967,
+            "timestamp": 7.008081563219893
+        },
+        {
+            "x": 6.8805324076089756,
+            "y": 7.05464814554522,
+            "heading": -1.2707475795927152,
+            "angularVelocity": 0.3777335511465864,
+            "velocityX": 3.3785934869538337,
+            "velocityY": 1.4139729708327582,
+            "timestamp": 7.068449216804709
+        },
+        {
+            "x": 7.061472859527864,
+            "y": 7.1337324563685955,
+            "heading": -1.1711733781498967,
+            "angularVelocity": 1.6494628419326967,
+            "velocityX": 2.997308014708086,
+            "velocityY": 1.3100444712873218,
+            "timestamp": 7.1288168703895245
+        },
+        {
+            "x": 7.217120419361732,
+            "y": 7.210021270677617,
+            "heading": -1.009883648548617,
+            "angularVelocity": 2.6717906034673087,
+            "velocityX": 2.5783271436115207,
+            "velocityY": 1.263736616859493,
+            "timestamp": 7.18918452397434
+        },
+        {
+            "x": 7.347594186911189,
+            "y": 7.2775930773892314,
+            "heading": -0.7947029660436775,
+            "angularVelocity": 3.5645030032948655,
+            "velocityX": 2.1613191800828018,
+            "velocityY": 1.119337968249467,
+            "timestamp": 7.249552177559156
+        },
+        {
+            "x": 7.451332936822743,
+            "y": 7.3349655169215735,
+            "heading": -0.5434124114773135,
+            "angularVelocity": 4.162668907005051,
+            "velocityX": 1.718449264651363,
+            "velocityY": 0.9503837920706217,
+            "timestamp": 7.309919831143972
+        },
+        {
+            "x": 7.527427510377051,
+            "y": 7.381739778721793,
+            "heading": -0.31640528763659864,
+            "angularVelocity": 3.7604099275081686,
+            "velocityX": 1.2605189871658125,
+            "velocityY": 0.7748232542201173,
+            "timestamp": 7.370287484728788
+        },
+        {
+            "x": 7.577573442921129,
+            "y": 7.419722379939866,
+            "heading": -0.13731969717190406,
+            "angularVelocity": 2.966581933039389,
+            "velocityX": 0.8306755284702909,
+            "velocityY": 0.6291879667760935,
+            "timestamp": 7.430655138313604
+        },
+        {
+            "x": 7.603931547937356,
+            "y": 7.45112329376933,
+            "heading": -0.02759055048880514,
+            "angularVelocity": 1.8176811614672275,
+            "velocityX": 0.4366262965512501,
+            "velocityY": 0.5201612447193243,
+            "timestamp": 7.49102279189842
+        },
+        {
+            "x": 7.608186721801758,
+            "y": 7.477188587188721,
+            "heading": 0,
+            "angularVelocity": 0.4570419562529598,
+            "velocityX": 0.07048764713743307,
+            "velocityY": 0.43177582482595267,
+            "timestamp": 7.551390445483236
+        },
+        {
+            "x": 7.590312649337569,
+            "y": 7.4994357327814924,
+            "heading": -0.062413166933384966,
+            "angularVelocity": -1.0141265252388012,
+            "velocityX": -0.2904286369464457,
+            "velocityY": 0.3614849488499569,
+            "timestamp": 7.612934212833851
+        },
+        {
+            "x": 7.548542604069605,
+            "y": 7.5172581696138145,
+            "heading": -0.20295351245592444,
+            "angularVelocity": -2.2835837254141333,
+            "velocityX": -0.6787047180585992,
+            "velocityY": 0.28958963026732126,
+            "timestamp": 7.6744779801844665
+        },
+        {
+            "x": 7.481084260938195,
+            "y": 7.5242062844896465,
+            "heading": -0.3946635775555263,
+            "angularVelocity": -3.1150199825666713,
+            "velocityX": -1.0961035704411768,
+            "velocityY": 0.11289713280385957,
+            "timestamp": 7.736021747535082
+        },
+        {
+            "x": 7.388848586007302,
+            "y": 7.518355172240059,
+            "heading": -0.6407957549264521,
+            "angularVelocity": -3.9993030645769183,
+            "velocityX": -1.4987005004978822,
+            "velocityY": -0.09507237696798558,
+            "timestamp": 7.797565514885697
+        },
+        {
+            "x": 7.269389686365513,
+            "y": 7.4992097780564855,
+            "heading": -0.9197330786516805,
+            "angularVelocity": -4.5323407346209255,
+            "velocityX": -1.941039763803056,
+            "velocityY": -0.31108583383434507,
+            "timestamp": 7.859109282236313
+        },
+        {
+            "x": 7.1251329311926535,
+            "y": 7.464894000432003,
+            "heading": -1.1518882810190125,
+            "angularVelocity": -3.772196801745026,
+            "velocityX": -2.343970175745461,
+            "velocityY": -0.5575833118727538,
+            "timestamp": 7.920653049586928
+        },
+        {
+            "x": 6.953983810333919,
+            "y": 7.422493842128991,
+            "heading": -1.327948941925082,
+            "angularVelocity": -2.8607390883799795,
+            "velocityX": -2.780933443409385,
+            "velocityY": -0.6889431721242942,
+            "timestamp": 7.9821968169375435
+        },
+        {
+            "x": 6.759450448912011,
+            "y": 7.372163465988786,
+            "heading": -1.4240765574561767,
+            "angularVelocity": -1.5619390828555313,
+            "velocityX": -3.1608945925206284,
+            "velocityY": -0.817798102177794,
+            "timestamp": 8.043740584288159
+        },
+        {
+            "x": 6.542371526128891,
+            "y": 7.310669881087202,
+            "heading": -1.4457611917111317,
+            "angularVelocity": -0.3523449276580275,
+            "velocityX": -3.5272283795435984,
+            "velocityY": -0.9991846055060891,
+            "timestamp": 8.105284351638774
+        },
+        {
+            "x": 6.323008319060415,
+            "y": 7.232674892951184,
+            "heading": -1.4457615219539912,
+            "angularVelocity": -0.000005365983815098308,
+            "velocityX": -3.564344799023447,
+            "velocityY": -1.2673092905034502,
+            "timestamp": 8.16682811898939
+        },
+        {
+            "x": 6.103645219000666,
+            "y": 7.154679603847749,
+            "heading": -1.4457618521936861,
+            "angularVelocity": -0.000005365932394056072,
+            "velocityX": -3.5643430602815576,
+            "velocityY": -1.267314180802329,
+            "timestamp": 8.228371886340005
+        },
+        {
+            "x": 5.884282118942346,
+            "y": 7.076684314740222,
+            "heading": -1.4457621824332476,
+            "angularVelocity": -0.000005365930225246195,
+            "velocityX": -3.5643430602583215,
+            "velocityY": -1.2673141808688406,
+            "timestamp": 8.28991565369062
+        },
+        {
+            "x": 5.66491901888401,
+            "y": 6.998689025632669,
+            "heading": -1.4457625126726754,
+            "angularVelocity": -0.000005365928056443698,
+            "velocityX": -3.564343060258591,
+            "velocityY": -1.2673141808692443,
+            "timestamp": 8.351459421041236
+        },
+        {
+            "x": 5.445555918825658,
+            "y": 6.920693736525091,
+            "heading": -1.44576284291197,
+            "angularVelocity": -0.000005365925887921031,
+            "velocityX": -3.5643430602588597,
+            "velocityY": -1.2673141808696466,
+            "timestamp": 8.413003188391851
+        },
+        {
+            "x": 5.226192818767289,
+            "y": 6.842698447417488,
+            "heading": -1.445763173151131,
+            "angularVelocity": -0.000005365923719151191,
+            "velocityX": -3.564343060259129,
+            "velocityY": -1.2673141808700494,
+            "timestamp": 8.474546955742467
+        },
+        {
+            "x": 5.006829718708903,
+            "y": 6.764703158309861,
+            "heading": -1.4457635033901584,
+            "angularVelocity": -0.000005365921549364942,
+            "velocityX": -3.5643430602593975,
+            "velocityY": -1.2673141808704518,
+            "timestamp": 8.536090723093082
+        },
+        {
+            "x": 4.787466618650501,
+            "y": 6.68670786920221,
+            "heading": -1.4457638336290524,
+            "angularVelocity": -0.000005365919381086304,
+            "velocityX": -3.564343060259667,
+            "velocityY": -1.2673141808708541,
+            "timestamp": 8.597634490443697
+        },
+        {
+            "x": 4.568103518592082,
+            "y": 6.608712580094533,
+            "heading": -1.4457641638678127,
+            "angularVelocity": -0.000005365917211650528,
+            "velocityX": -3.5643430602599366,
+            "velocityY": -1.2673141808712571,
+            "timestamp": 8.659178257794313
+        },
+        {
+            "x": 4.348740418533646,
+            "y": 6.530717290986832,
+            "heading": -1.4457644941064398,
+            "angularVelocity": -0.000005365915042310373,
+            "velocityX": -3.564343060260205,
+            "velocityY": -1.2673141808716597,
+            "timestamp": 8.720722025144928
+        },
+        {
+            "x": 4.1293773184751945,
+            "y": 6.452722001879106,
+            "heading": -1.4457648243449335,
+            "angularVelocity": -0.000005365912874367381,
+            "velocityX": -3.564343060260474,
+            "velocityY": -1.267314180872062,
+            "timestamp": 8.782265792495544
+        },
+        {
+            "x": 3.910014218416726,
+            "y": 6.374726712771355,
+            "heading": -1.4457651545832937,
+            "angularVelocity": -0.000005365910705198878,
+            "velocityX": -3.5643430602607435,
+            "velocityY": -1.2673141808724648,
+            "timestamp": 8.843809559846159
+        },
+        {
+            "x": 3.690651118358241,
+            "y": 6.296731423663579,
+            "heading": -1.4457654848215202,
+            "angularVelocity": -0.000005365908536510064,
+            "velocityX": -3.5643430602610127,
+            "velocityY": -1.2673141808728674,
+            "timestamp": 8.905353327196774
+        },
+        {
+            "x": 3.47128801829974,
+            "y": 6.218736134555778,
+            "heading": -1.4457658150596133,
+            "angularVelocity": -0.0000053659063672733,
+            "velocityX": -3.5643430602612813,
+            "velocityY": -1.2673141808732702,
+            "timestamp": 8.96689709454739
+        },
+        {
+            "x": 3.2519249182412215,
+            "y": 6.140740845447953,
+            "heading": -1.445766145297573,
+            "angularVelocity": -0.000005365904199203489,
+            "velocityX": -3.564343060261551,
+            "velocityY": -1.2673141808736725,
+            "timestamp": 9.028440861898005
+        },
+        {
+            "x": 3.0325618181826868,
+            "y": 6.062745556340103,
+            "heading": -1.4457664755353992,
+            "angularVelocity": -0.000005365902029653181,
+            "velocityX": -3.5643430602618196,
+            "velocityY": -1.267314180874075,
+            "timestamp": 9.08998462924862
+        },
+        {
+            "x": 2.8131987181241356,
+            "y": 5.984750267232228,
+            "heading": -1.445766805773092,
+            "angularVelocity": -0.000005365899860579176,
+            "velocityX": -3.5643430602620887,
+            "velocityY": -1.2673141808744774,
+            "timestamp": 9.151528396599236
+        },
+        {
+            "x": 2.5938356180647952,
+            "y": 5.906754978126503,
+            "heading": -1.4457671360106512,
+            "angularVelocity": -0.000005365897692675495,
+            "velocityX": -3.564343060274917,
+            "velocityY": -1.2673141808395556,
+            "timestamp": 9.213072163949851
+        },
+        {
+            "x": 2.374472460827342,
+            "y": 5.8287598498370325,
+            "heading": -1.4457674662499087,
+            "angularVelocity": -0.000005365925286950734,
+            "velocityX": -3.5643439893391378,
+            "velocityY": -1.2673115678006912,
+            "timestamp": 9.274615931300467
+        },
+        {
+            "x": 2.1579642030031834,
+            "y": 5.762355072468581,
+            "heading": -1.4644178853402519,
+            "angularVelocity": -0.30304318200235486,
+            "velocityX": -3.5179558734308305,
+            "velocityY": -1.0789846027810281,
+            "timestamp": 9.336159698651082
+        },
+        {
+            "x": 1.9655559601169903,
+            "y": 5.705313391080233,
+            "heading": -1.5528631085528386,
+            "angularVelocity": -1.4371109702906752,
+            "velocityX": -3.1263643934899394,
+            "velocityY": -0.9268474102890126,
+            "timestamp": 9.397703466001698
+        },
+        {
+            "x": 1.799063271019786,
+            "y": 5.655117829615961,
+            "heading": -1.7042345036546405,
+            "angularVelocity": -2.459573107369865,
+            "velocityX": -2.7052729506904143,
+            "velocityY": -0.8156075525618497,
+            "timestamp": 9.459247233352313
+        },
+        {
+            "x": 1.6591648707007773,
+            "y": 5.609591955542646,
+            "heading": -1.9150308855459262,
+            "angularVelocity": -3.425145891547008,
+            "velocityX": -2.273153015186835,
+            "velocityY": -0.739731674435107,
+            "timestamp": 9.520791000702928
+        },
+        {
+            "x": 1.5431948510849691,
+            "y": 5.574795624095963,
+            "heading": -2.189608491806892,
+            "angularVelocity": -4.461501433552055,
+            "velocityX": -1.884350351110707,
+            "velocityY": -0.5653916382539252,
+            "timestamp": 9.582334768053544
+        },
+        {
+            "x": 1.4525455234391338,
+            "y": 5.551032580854212,
+            "heading": -2.523374227659994,
+            "angularVelocity": -5.423225620096252,
+            "velocityX": -1.4729245795013663,
+            "velocityY": -0.3861161619563059,
+            "timestamp": 9.64387853540416
+        },
+        {
+            "x": 1.3896696664094041,
+            "y": 5.537613504449024,
+            "heading": -2.816143624187386,
+            "angularVelocity": -4.757092539679631,
+            "velocityX": -1.0216445911009162,
+            "velocityY": -0.21804119219317228,
+            "timestamp": 9.705422302754775
+        },
+        {
+            "x": 1.3505170646137672,
+            "y": 5.530954725925202,
+            "heading": -3.0286369343399735,
+            "angularVelocity": -3.4527185984896,
+            "velocityX": -0.6361749285282489,
+            "velocityY": -0.1081958224280782,
+            "timestamp": 9.76696607010539
+        },
+        {
+            "x": 1.332138180732727,
+            "y": 5.52810525894165,
+            "heading": -3.1411853071795863,
+            "angularVelocity": -1.828753384537267,
+            "velocityX": -0.2986311152571343,
+            "velocityY": -0.046299846535530764,
+            "timestamp": 9.828509837456005
+        },
+        {
+            "x": 1.332138180732727,
+            "y": 5.52810525894165,
+            "heading": -3.1411853071795863,
+            "angularVelocity": -8.15091951330742e-35,
+            "velocityX": 0,
+            "velocityY": 2.0839850321711294e-35,
+            "timestamp": 9.89005360480662
+        }
+    ]
+}
\ No newline at end of file
diff --git a/deploy/choreo/NewPath.traj b/deploy/choreo/NewPath.traj
new file mode 100644
index 00000000..92058ea3
--- /dev/null
+++ b/deploy/choreo/NewPath.traj
@@ -0,0 +1,1876 @@
+{
+    "samples": [
+        {
+            "x": 1.3321385383605957,
+            "y": 5.547596454620361,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 2.2261748232194093e-37,
+            "velocityY": 3.7157891353428806e-37,
+            "timestamp": 0
+        },
+        {
+            "x": 1.366635963151808,
+            "y": 5.509495705296941,
+            "heading": 0.06192923642991932,
+            "angularVelocity": 0.7673697851265452,
+            "velocityX": 0.4274601622031667,
+            "velocityY": -0.47210922509207903,
+            "timestamp": 0.08070325106650729
+        },
+        {
+            "x": 1.4355761410472572,
+            "y": 5.43294196238783,
+            "heading": 0.18410628088281034,
+            "angularVelocity": 1.5139048655202914,
+            "velocityX": 0.8542428834575204,
+            "velocityY": -0.9485831350960487,
+            "timestamp": 0.16140650213301458
+        },
+        {
+            "x": 1.5392732266735116,
+            "y": 5.317548823896549,
+            "heading": 0.36164871867370346,
+            "angularVelocity": 2.1999415815923076,
+            "velocityX": 1.2849183181083754,
+            "velocityY": -1.429844981042792,
+            "timestamp": 0.24210975319952188
+        },
+        {
+            "x": 1.67847556185222,
+            "y": 5.162242328350244,
+            "heading": 0.5745390252788012,
+            "angularVelocity": 2.637939659080841,
+            "velocityX": 1.724866512056522,
+            "velocityY": -1.9244143636582545,
+            "timestamp": 0.32281300426602916
+        },
+        {
+            "x": 1.8501216668381886,
+            "y": 4.967122032744555,
+            "heading": 0.7214821568069908,
+            "angularVelocity": 1.8207832966616715,
+            "velocityX": 2.1268796822635494,
+            "velocityY": -2.417750128119265,
+            "timestamp": 0.40351625533253643
+        },
+        {
+            "x": 1.999355457938739,
+            "y": 4.729284669626315,
+            "heading": 0.7742712340167793,
+            "angularVelocity": 0.6541133908754838,
+            "velocityX": 1.849167030180327,
+            "velocityY": -2.9470604960169275,
+            "timestamp": 0.4842195063990437
+        },
+        {
+            "x": 2.1155301056872418,
+            "y": 4.524468188873081,
+            "heading": 0.7178805227908414,
+            "angularVelocity": -0.6987415064538932,
+            "velocityX": 1.4395287205067313,
+            "velocityY": -2.537896281085937,
+            "timestamp": 0.564922757465551
+        },
+        {
+            "x": 2.1994300453605806,
+            "y": 4.354650955915648,
+            "heading": 0.5603443624430987,
+            "angularVelocity": -1.952042306423536,
+            "velocityX": 1.0396104068248448,
+            "velocityY": -2.104217992624444,
+            "timestamp": 0.6456260085320583
+        },
+        {
+            "x": 2.250899907873132,
+            "y": 4.221459437616645,
+            "heading": 0.3158282415144345,
+            "angularVelocity": -3.029817481914815,
+            "velocityX": 0.637766903220982,
+            "velocityY": -1.6503860320229218,
+            "timestamp": 0.7263292595985656
+        },
+        {
+            "x": 2.2676985263824463,
+            "y": 4.124765396118164,
+            "heading": 2.2216292217564286e-35,
+            "angularVelocity": -3.913451284064894,
+            "velocityX": 0.20815293420422745,
+            "velocityY": -1.1981430762782603,
+            "timestamp": 0.807032510665073
+        },
+        {
+            "x": 2.2676338555313906,
+            "y": 4.086690967677836,
+            "heading": -0.16755058667496922,
+            "angularVelocity": -4.348312733244185,
+            "velocityX": -0.0016783533301519722,
+            "velocityY": -0.9881166355999816,
+            "timestamp": 0.8455648327296736
+        },
+        {
+            "x": 2.259511405376126,
+            "y": 4.056750075909778,
+            "heading": -0.352256708092011,
+            "angularVelocity": -4.793537257042952,
+            "velocityX": -0.21079576106643969,
+            "velocityY": -0.7770331546035791,
+            "timestamp": 0.8840971547942744
+        },
+        {
+            "x": 2.2434268935883566,
+            "y": 4.035188434921252,
+            "heading": -0.5534507965852989,
+            "angularVelocity": -5.22143690577431,
+            "velocityX": -0.41742908098825227,
+            "velocityY": -0.5595728425704051,
+            "timestamp": 0.9226294768588752
+        },
+        {
+            "x": 2.2194223708397587,
+            "y": 4.022359555236857,
+            "heading": -0.7686869662578265,
+            "angularVelocity": -5.585860341135864,
+            "velocityX": -0.6229710918629213,
+            "velocityY": -0.33293814120224735,
+            "timestamp": 0.961161798923476
+        },
+        {
+            "x": 2.1874734502455304,
+            "y": 4.018617397576101,
+            "heading": -0.9939561131825075,
+            "angularVelocity": -5.846238556477593,
+            "velocityX": -0.8291459969805253,
+            "velocityY": -0.09711736693372348,
+            "timestamp": 0.9996941209880768
+        },
+        {
+            "x": 2.1476082595993153,
+            "y": 4.024282627294418,
+            "heading": -1.2244083666470835,
+            "angularVelocity": -5.980751771933577,
+            "velocityX": -1.0345909228979182,
+            "velocityY": 0.14702539101636564,
+            "timestamp": 1.0382264430526775
+        },
+        {
+            "x": 2.1000522078031225,
+            "y": 4.039643723439363,
+            "heading": -1.4537096411283452,
+            "angularVelocity": -5.950881291213917,
+            "velocityX": -1.2341859833015967,
+            "velocityY": 0.3986548259197214,
+            "timestamp": 1.0767587651172783
+        },
+        {
+            "x": 2.045480509029303,
+            "y": 4.064764963970464,
+            "heading": -1.6725096563971036,
+            "angularVelocity": -5.678350110899954,
+            "velocityX": -1.4162577246792443,
+            "velocityY": 0.6519524177386574,
+            "timestamp": 1.115291087181879
+        },
+        {
+            "x": 1.9845995907026053,
+            "y": 4.099193425968209,
+            "heading": -1.8731412390257827,
+            "angularVelocity": -5.206838619596132,
+            "velocityX": -1.5799960932701846,
+            "velocityY": 0.8934956460714911,
+            "timestamp": 1.1538234092464799
+        },
+        {
+            "x": 1.9178343766904495,
+            "y": 4.143331162612739,
+            "heading": -2.0560809001827813,
+            "angularVelocity": -4.747693659631898,
+            "velocityX": -1.7327067364437998,
+            "velocityY": 1.1454730542979317,
+            "timestamp": 1.1923557313110806
+        },
+        {
+            "x": 1.846280610428652,
+            "y": 4.197802019829692,
+            "heading": -2.22146373822619,
+            "angularVelocity": -4.2920548044350495,
+            "velocityX": -1.856980384982645,
+            "velocityY": 1.413640660576612,
+            "timestamp": 1.2308880533756814
+        },
+        {
+            "x": 1.770704211681186,
+            "y": 4.2612245864567395,
+            "heading": -2.3576115042271706,
+            "angularVelocity": -3.5333392514659345,
+            "velocityX": -1.961376701377084,
+            "velocityY": 1.6459575553406294,
+            "timestamp": 1.2694203754402822
+        },
+        {
+            "x": 1.6915488741102875,
+            "y": 4.3341463409601095,
+            "heading": -2.466232613363222,
+            "angularVelocity": -2.8189608961002843,
+            "velocityX": -2.0542581741684773,
+            "velocityY": 1.8924827416607193,
+            "timestamp": 1.307952697504883
+        },
+        {
+            "x": 1.609995812708591,
+            "y": 4.417230005205154,
+            "heading": -2.5495370955191317,
+            "angularVelocity": -2.1619377626982037,
+            "velocityX": -2.1164844741246154,
+            "velocityY": 2.156207043680162,
+            "timestamp": 1.3464850195694837
+        },
+        {
+            "x": 1.5308705698098255,
+            "y": 4.511360824889822,
+            "heading": -2.6152779027765387,
+            "angularVelocity": -1.7061210883473432,
+            "velocityX": -2.053477150069217,
+            "velocityY": 2.442905452904058,
+            "timestamp": 1.3850173416340845
+        },
+        {
+            "x": 1.4606506153508025,
+            "y": 4.614058209006809,
+            "heading": -2.6783194867011537,
+            "angularVelocity": -1.636070201503133,
+            "velocityX": -1.822364983384516,
+            "velocityY": 2.6652269734694745,
+            "timestamp": 1.4235496636986853
+        },
+        {
+            "x": 1.4019701962785118,
+            "y": 4.720456086702728,
+            "heading": -2.7505666696039515,
+            "angularVelocity": -1.874976098810575,
+            "velocityX": -1.5228882124962768,
+            "velocityY": 2.761263064227987,
+            "timestamp": 1.462081985763286
+        },
+        {
+            "x": 1.3551627373603712,
+            "y": 4.8282147910275475,
+            "heading": -2.8346512790115326,
+            "angularVelocity": -2.1821838109473566,
+            "velocityX": -1.2147583226275982,
+            "velocityY": 2.7965795610282194,
+            "timestamp": 1.5006143078278869
+        },
+        {
+            "x": 1.3203598068196198,
+            "y": 4.936382222496368,
+            "heading": -2.9296853375556533,
+            "angularVelocity": -2.466346522921523,
+            "velocityX": -0.9032139428919735,
+            "velocityY": 2.8071869452215745,
+            "timestamp": 1.5391466298924876
+        },
+        {
+            "x": 1.2978728176694776,
+            "y": 5.0435429055984535,
+            "heading": -3.0274097940467435,
+            "angularVelocity": -2.53616837125081,
+            "velocityX": -0.583587698463697,
+            "velocityY": 2.7810595718167943,
+            "timestamp": 1.5776789519570884
+        },
+        {
+            "x": 1.286514559723769,
+            "y": 5.147138897368402,
+            "heading": -3.1101525298443558,
+            "angularVelocity": -2.1473591874087323,
+            "velocityX": -0.2947722155614252,
+            "velocityY": 2.688547853313019,
+            "timestamp": 1.6162112740216892
+        },
+        {
+            "x": 1.2850446377120623,
+            "y": 5.2469545287393595,
+            "heading": -3.168448488726713,
+            "angularVelocity": -1.5129106100748941,
+            "velocityX": -0.038147766159596044,
+            "velocityY": 2.5904390398173645,
+            "timestamp": 1.65474359608629
+        },
+        {
+            "x": 1.2925718642931594,
+            "y": 5.343394761009087,
+            "heading": -3.1958165160103977,
+            "angularVelocity": -0.7102615626901808,
+            "velocityX": 0.1953483770969554,
+            "velocityY": 2.5028398783764474,
+            "timestamp": 1.6932759181508907
+        },
+        {
+            "x": 1.3084372785500542,
+            "y": 5.436937425716649,
+            "heading": -3.187781062808085,
+            "angularVelocity": 0.20853799542215204,
+            "velocityX": 0.4117430096814787,
+            "velocityY": 2.4276415148491566,
+            "timestamp": 1.7318082402154915
+        },
+        {
+            "x": 1.3321385383605957,
+            "y": 5.52810525894165,
+            "heading": -3.1411853071795863,
+            "angularVelocity": 1.2092641484304996,
+            "velocityX": 0.6151007398621232,
+            "velocityY": 2.366009322567054,
+            "timestamp": 1.7703405622800923
+        },
+        {
+            "x": 1.357521739751378,
+            "y": 5.604487249946148,
+            "heading": -3.0708796741945465,
+            "angularVelocity": 2.1424744850655806,
+            "velocityX": 0.7735206841904706,
+            "velocityY": 2.3276437448542358,
+            "timestamp": 1.8031557175947133
+        },
+        {
+            "x": 1.3882591250435519,
+            "y": 5.675559897172117,
+            "heading": -2.9834964247525093,
+            "angularVelocity": 2.6628930628008614,
+            "velocityX": 0.936682608918773,
+            "velocityY": 2.1658482656731963,
+            "timestamp": 1.8359708729093343
+        },
+        {
+            "x": 1.423715934589619,
+            "y": 5.741061467273705,
+            "heading": -2.8764865415416963,
+            "angularVelocity": 3.260989691648163,
+            "velocityX": 1.0805010430735036,
+            "velocityY": 1.9960767966380097,
+            "timestamp": 1.8687860282239552
+        },
+        {
+            "x": 1.4640545833350669,
+            "y": 5.800765385011703,
+            "heading": -2.7509368125913465,
+            "angularVelocity": 3.8259678415847733,
+            "velocityX": 1.2292688655194213,
+            "velocityY": 1.8194007362017863,
+            "timestamp": 1.9016011835385762
+        },
+        {
+            "x": 1.5093560477878591,
+            "y": 5.854065962524638,
+            "heading": -2.610542457820649,
+            "angularVelocity": 4.278338878016634,
+            "velocityX": 1.3805043437538693,
+            "velocityY": 1.6242671107878839,
+            "timestamp": 1.9344163388531972
+        },
+        {
+            "x": 1.5593454812172984,
+            "y": 5.900364561226932,
+            "heading": -2.4585168251320484,
+            "angularVelocity": 4.6327872359898405,
+            "velocityX": 1.5233642184581169,
+            "velocityY": 1.4108907380872764,
+            "timestamp": 1.9672314941678182
+        },
+        {
+            "x": 1.612840729658989,
+            "y": 5.938459321827602,
+            "heading": -2.302873266045166,
+            "angularVelocity": 4.74303892803867,
+            "velocityX": 1.6301994590242108,
+            "velocityY": 1.1608892365563526,
+            "timestamp": 2.000046649482439
+        },
+        {
+            "x": 1.6683469717714092,
+            "y": 5.967831924392925,
+            "heading": -2.1497463391084874,
+            "angularVelocity": 4.666347773415882,
+            "velocityX": 1.6914819259651337,
+            "velocityY": 0.89509259620163,
+            "timestamp": 2.03286180479706
+        },
+        {
+            "x": 1.7259927125934769,
+            "y": 5.988487486256733,
+            "heading": -1.9957888127587393,
+            "angularVelocity": 4.691659231036796,
+            "velocityX": 1.756680420049179,
+            "velocityY": 0.6294519000677786,
+            "timestamp": 2.065676960111681
+        },
+        {
+            "x": 1.7857676152473871,
+            "y": 6.000514617426619,
+            "heading": -1.8372643934887982,
+            "angularVelocity": 4.830829467362277,
+            "velocityX": 1.8215639109675918,
+            "velocityY": 0.36651148088664176,
+            "timestamp": 2.098492115426302
+        },
+        {
+            "x": 1.8473787764602114,
+            "y": 6.0041716221205474,
+            "heading": -1.6689363232818417,
+            "angularVelocity": 5.129583224369399,
+            "velocityX": 1.877521548263803,
+            "velocityY": 0.11144255326131712,
+            "timestamp": 2.131307270740923
+        },
+        {
+            "x": 1.9079302163194551,
+            "y": 5.9989341122190964,
+            "heading": -1.4983965075420516,
+            "angularVelocity": 5.196983348233757,
+            "velocityX": 1.8452278917681775,
+            "velocityY": -0.15960643340660682,
+            "timestamp": 2.164122426055544
+        },
+        {
+            "x": 1.9644851606047329,
+            "y": 5.9857778753833975,
+            "heading": -1.3328127647297614,
+            "angularVelocity": 5.045953347614136,
+            "velocityX": 1.7234397869840146,
+            "velocityY": -0.40091953579256995,
+            "timestamp": 2.196937581370165
+        },
+        {
+            "x": 2.0167542311341298,
+            "y": 5.964732325175136,
+            "heading": -1.1669030928960278,
+            "angularVelocity": 5.055885618795508,
+            "velocityX": 1.592833251229742,
+            "velocityY": -0.6413362974053785,
+            "timestamp": 2.229752736684786
+        },
+        {
+            "x": 2.0647189114008015,
+            "y": 5.935938401754757,
+            "heading": -0.9958716358792914,
+            "angularVelocity": 5.211965489023056,
+            "velocityX": 1.4616624485485903,
+            "velocityY": -0.8774580874084462,
+            "timestamp": 2.262567891999407
+        },
+        {
+            "x": 2.1085721264603117,
+            "y": 5.8997079709351175,
+            "heading": -0.8141851452694338,
+            "angularVelocity": 5.53666404647202,
+            "velocityX": 1.3363707908452565,
+            "velocityY": -1.1040761645731791,
+            "timestamp": 2.295383047314028
+        },
+        {
+            "x": 2.1457870640137746,
+            "y": 5.857488430688145,
+            "heading": -0.6354555413511925,
+            "angularVelocity": 5.446556696277672,
+            "velocityX": 1.134077751473621,
+            "velocityY": -1.2865866348089932,
+            "timestamp": 2.328198202628649
+        },
+        {
+            "x": 2.1756614189381724,
+            "y": 5.811166545923024,
+            "heading": -0.4679136928264175,
+            "angularVelocity": 5.105624121490153,
+            "velocityX": 0.9103828593213182,
+            "velocityY": -1.41160035114879,
+            "timestamp": 2.36101335794327
+        },
+        {
+            "x": 2.1979477640451273,
+            "y": 5.762830427028998,
+            "heading": -0.3159012603830297,
+            "angularVelocity": 4.632384975354892,
+            "velocityX": 0.6791479392152971,
+            "velocityY": -1.4729815669191495,
+            "timestamp": 2.393828513257891
+        },
+        {
+            "x": 2.213866689003856,
+            "y": 5.714154085995567,
+            "heading": -0.19000720732011314,
+            "angularVelocity": 3.8364606797037886,
+            "velocityX": 0.4851089323242057,
+            "velocityY": -1.483349402638423,
+            "timestamp": 2.426643668572512
+        },
+        {
+            "x": 2.2241033767575633,
+            "y": 5.665307000563528,
+            "heading": -0.09437057287174495,
+            "angularVelocity": 2.9144044430518514,
+            "velocityX": 0.31195000162458963,
+            "velocityY": -1.4885526203886283,
+            "timestamp": 2.459458823887133
+        },
+        {
+            "x": 2.228985536910567,
+            "y": 5.616279831781338,
+            "heading": -0.030711851451385126,
+            "angularVelocity": 1.9399183337704993,
+            "velocityX": 0.14877760309816687,
+            "velocityY": -1.4940404307745467,
+            "timestamp": 2.492273979201754
+        },
+        {
+            "x": 2.2287168502807617,
+            "y": 5.567087173461914,
+            "heading": -1.4593920868727723e-34,
+            "angularVelocity": 0.9359044976910779,
+            "velocityX": -0.008187882313191258,
+            "velocityY": -1.4990835133273062,
+            "timestamp": 2.525089134516375
+        },
+        {
+            "x": 2.2234617571142565,
+            "y": 5.5179127265043535,
+            "heading": -0.002801304121864836,
+            "angularVelocity": -0.08554590281020508,
+            "velocityX": -0.16047943019522937,
+            "velocityY": -1.5016836006283458,
+            "timestamp": 2.5578353447946145
+        },
+        {
+            "x": 2.213100127893157,
+            "y": 5.468804050064043,
+            "heading": -0.038527021013621016,
+            "angularVelocity": -1.090987829987043,
+            "velocityX": -0.31642224040761213,
+            "velocityY": -1.499675108143563,
+            "timestamp": 2.590581555072854
+        },
+        {
+            "x": 2.1974774857014916,
+            "y": 5.420028778568237,
+            "heading": -0.10642175719814694,
+            "angularVelocity": -2.0733616381142825,
+            "velocityX": -0.47708244889780627,
+            "velocityY": -1.4894936263271208,
+            "timestamp": 2.623327765351094
+        },
+        {
+            "x": 2.176386509447901,
+            "y": 5.371982692865446,
+            "heading": -0.2052939647176804,
+            "angularVelocity": -3.019348091868672,
+            "velocityX": -0.6440738050108257,
+            "velocityY": -1.4672258345179665,
+            "timestamp": 2.6560739756293335
+        },
+        {
+            "x": 2.1495039837265555,
+            "y": 5.325205721708642,
+            "heading": -0.3329902489476004,
+            "angularVelocity": -3.8995744284576235,
+            "velocityX": -0.8209354759811619,
+            "velocityY": -1.4284697606027426,
+            "timestamp": 2.688820185907573
+        },
+        {
+            "x": 2.1162335968543386,
+            "y": 5.280389038707846,
+            "heading": -0.4851442980051073,
+            "angularVelocity": -4.646462835383888,
+            "velocityX": -1.0160072444879351,
+            "velocityY": -1.368606706546959,
+            "timestamp": 2.7215663961858128
+        },
+        {
+            "x": 2.0756862755494496,
+            "y": 5.238416301489077,
+            "heading": -0.6539149158793239,
+            "angularVelocity": -5.153897701144575,
+            "velocityX": -1.2382294305314772,
+            "velocityY": -1.2817586176272708,
+            "timestamp": 2.7543126064640524
+        },
+        {
+            "x": 2.0277154786481417,
+            "y": 5.201197274104196,
+            "heading": -0.8280730192796384,
+            "angularVelocity": -5.318420113977116,
+            "velocityX": -1.4649266737649096,
+            "velocityY": -1.136590373928381,
+            "timestamp": 2.787058816742292
+        },
+        {
+            "x": 1.9735902732105297,
+            "y": 5.170441088651969,
+            "heading": -1.0002810429175644,
+            "angularVelocity": -5.258868802670591,
+            "velocityX": -1.6528692932012015,
+            "velocityY": -0.9392288509386596,
+            "timestamp": 2.8198050270205317
+        },
+        {
+            "x": 1.914416507688317,
+            "y": 5.147054029448202,
+            "heading": -1.1711019741635995,
+            "angularVelocity": -5.216509934877801,
+            "velocityX": -1.807041639915639,
+            "velocityY": -0.7141913218369506,
+            "timestamp": 2.8525512372987714
+        },
+        {
+            "x": 1.851848295060789,
+            "y": 5.131935997009135,
+            "heading": -1.3411756027953594,
+            "angularVelocity": -5.193688893666442,
+            "velocityX": -1.9107008748766678,
+            "velocityY": -0.46167273435953404,
+            "timestamp": 2.885297447577011
+        },
+        {
+            "x": 1.788148478011349,
+            "y": 5.125682164538362,
+            "heading": -1.5101885639025132,
+            "angularVelocity": -5.161298350895432,
+            "velocityX": -1.945257680451938,
+            "velocityY": -0.1909788161022263,
+            "timestamp": 2.9180436578552507
+        },
+        {
+            "x": 1.7253768027949503,
+            "y": 5.1283168367810115,
+            "heading": -1.680184610910991,
+            "angularVelocity": -5.191319714985216,
+            "velocityX": -1.9169141919946477,
+            "velocityY": 0.0804573176640312,
+            "timestamp": 2.9507898681334903
+        },
+        {
+            "x": 1.665253742824933,
+            "y": 5.13946824377609,
+            "heading": -1.8524223717430155,
+            "angularVelocity": -5.2597769136808665,
+            "velocityX": -1.83603108448766,
+            "velocityY": 0.3405403831565884,
+            "timestamp": 2.98353607841173
+        },
+        {
+            "x": 1.6088715232725108,
+            "y": 5.158717654167419,
+            "heading": -2.026839169725991,
+            "angularVelocity": -5.326320099363601,
+            "velocityX": -1.7217937304301978,
+            "velocityY": 0.5878362786951496,
+            "timestamp": 3.0162822886899696
+        },
+        {
+            "x": 1.5568081254735793,
+            "y": 5.1857922946114,
+            "heading": -2.2021042643709907,
+            "angularVelocity": -5.3522252851795065,
+            "velocityX": -1.5899060488696688,
+            "velocityY": 0.826802253266321,
+            "timestamp": 3.0490284989682093
+        },
+        {
+            "x": 1.5093538981441093,
+            "y": 5.2205186818889695,
+            "heading": -2.3759898286014502,
+            "angularVelocity": -5.31009734418057,
+            "velocityX": -1.4491517316434035,
+            "velocityY": 1.060470417263673,
+            "timestamp": 3.081774709246449
+        },
+        {
+            "x": 1.4666702915238679,
+            "y": 5.262723781048631,
+            "heading": -2.545870980641176,
+            "angularVelocity": -5.187811065655255,
+            "velocityX": -1.30346706557996,
+            "velocityY": 1.2888544598306537,
+            "timestamp": 3.1145209195246886
+        },
+        {
+            "x": 1.4288865927350196,
+            "y": 5.312181622313189,
+            "heading": -2.7092363373229658,
+            "angularVelocity": -4.988832457059862,
+            "velocityX": -1.1538342442623262,
+            "velocityY": 1.510337863353399,
+            "timestamp": 3.147267129802928
+        },
+        {
+            "x": 1.3968131737185872,
+            "y": 5.367362011920862,
+            "heading": -2.8587947946716725,
+            "angularVelocity": -4.567198954563906,
+            "velocityX": -0.9794543778931859,
+            "velocityY": 1.6850923859223195,
+            "timestamp": 3.180013340081168
+        },
+        {
+            "x": 1.3707084315183937,
+            "y": 5.426032663472122,
+            "heading": -2.986331521157274,
+            "angularVelocity": -3.8947018724286298,
+            "velocityX": -0.7971836123443037,
+            "velocityY": 1.7916776033850628,
+            "timestamp": 3.2127595503594075
+        },
+        {
+            "x": 1.3494796795233661,
+            "y": 5.486294372047085,
+            "heading": -3.0820270140723585,
+            "angularVelocity": -2.9223379469554978,
+            "velocityX": -0.6482811847432148,
+            "velocityY": 1.8402651196253896,
+            "timestamp": 3.245505760637647
+        },
+        {
+            "x": 1.3321385383605957,
+            "y": 5.547596454620361,
+            "heading": -3.1411853071795863,
+            "angularVelocity": -1.8065691450878867,
+            "velocityX": -0.529561772657824,
+            "velocityY": 1.8720359410264953,
+            "timestamp": 3.278251970915887
+        },
+        {
+            "x": 1.316411922217005,
+            "y": 5.622158239816111,
+            "heading": -3.1574444739447625,
+            "angularVelocity": -0.4153661551974852,
+            "velocityX": -0.4017613064785548,
+            "velocityY": 1.9047988429364744,
+            "timestamp": 3.317396149038551
+        },
+        {
+            "x": 1.30673758444171,
+            "y": 5.698077663390306,
+            "heading": -3.122874776057638,
+            "angularVelocity": 0.8831376604407247,
+            "velocityX": -0.24714627408906595,
+            "velocityY": 1.9394818646157501,
+            "timestamp": 3.356540327161215
+        },
+        {
+            "x": 1.3044116698031423,
+            "y": 5.775906190222514,
+            "heading": -3.0431020650652782,
+            "angularVelocity": 2.0379201919217973,
+            "velocityX": -0.05941917163975373,
+            "velocityY": 1.9882529296776088,
+            "timestamp": 3.395684505283879
+        },
+        {
+            "x": 1.310735543939778,
+            "y": 5.856467416837523,
+            "heading": -2.9252067960348933,
+            "angularVelocity": 3.011821289514447,
+            "velocityX": 0.16155337625991328,
+            "velocityY": 2.058064071815707,
+            "timestamp": 3.434828683406543
+        },
+        {
+            "x": 1.3277929402719157,
+            "y": 5.939930987776361,
+            "heading": -2.7826616721505166,
+            "angularVelocity": 3.6415408553908346,
+            "velocityX": 0.43575819317718334,
+            "velocityY": 2.132209052321722,
+            "timestamp": 3.473972861529207
+        },
+        {
+            "x": 1.3576022335544815,
+            "y": 6.023554053808955,
+            "heading": -2.637362279761064,
+            "angularVelocity": 3.711903004685308,
+            "velocityX": 0.7615255885346219,
+            "velocityY": 2.136283606991328,
+            "timestamp": 3.513117039651871
+        },
+        {
+            "x": 1.4000554524537352,
+            "y": 6.106306047220255,
+            "heading": -2.4978098785002194,
+            "angularVelocity": 3.5650870181393826,
+            "velocityX": 1.0845346852403,
+            "velocityY": 2.1140306778695206,
+            "timestamp": 3.552261217774535
+        },
+        {
+            "x": 1.455157655616598,
+            "y": 6.18915881243997,
+            "heading": -2.364648055922988,
+            "angularVelocity": 3.4018295686257636,
+            "velocityX": 1.407673013090022,
+            "velocityY": 2.1166050532491445,
+            "timestamp": 3.591405395897199
+        },
+        {
+            "x": 1.5225297792403976,
+            "y": 6.27399259313729,
+            "heading": -2.2419731152862643,
+            "angularVelocity": 3.1339255674829567,
+            "velocityX": 1.7211275559977186,
+            "velocityY": 2.16721322980599,
+            "timestamp": 3.630549574019863
+        },
+        {
+            "x": 1.596991898632741,
+            "y": 6.365541704289421,
+            "heading": -2.152688244347013,
+            "angularVelocity": 2.2809233766376438,
+            "velocityX": 1.9022527222057353,
+            "velocityY": 2.3387669774352777,
+            "timestamp": 3.669693752142527
+        },
+        {
+            "x": 1.679413821374646,
+            "y": 6.463137271673749,
+            "heading": -2.0939697752685182,
+            "angularVelocity": 1.5000562508808342,
+            "velocityX": 2.1055985000789685,
+            "velocityY": 2.4932332741410055,
+            "timestamp": 3.708837930265191
+        },
+        {
+            "x": 1.7662309748150582,
+            "y": 6.5526721858065455,
+            "heading": -2.030942574296911,
+            "angularVelocity": 1.6101296283218078,
+            "velocityX": 2.2178816264415735,
+            "velocityY": 2.2873111258646657,
+            "timestamp": 3.747982108387855
+        },
+        {
+            "x": 1.8463290740680989,
+            "y": 6.633589645928569,
+            "heading": -1.9424440862784471,
+            "angularVelocity": 2.260833979989085,
+            "velocityX": 2.0462327501689237,
+            "velocityY": 2.0671646207121097,
+            "timestamp": 3.787126286510519
+        },
+        {
+            "x": 1.9201306346515887,
+            "y": 6.705414092889608,
+            "heading": -1.8297336528989434,
+            "angularVelocity": 2.8793664546055737,
+            "velocityX": 1.88537770169097,
+            "velocityY": 1.8348692042011108,
+            "timestamp": 3.826270464633183
+        },
+        {
+            "x": 1.9881022552719625,
+            "y": 6.7678945659852445,
+            "heading": -1.6930072188425063,
+            "angularVelocity": 3.4928932120629863,
+            "velocityX": 1.73644265584974,
+            "velocityY": 1.596162599195345,
+            "timestamp": 3.865414642755847
+        },
+        {
+            "x": 2.0508860920717975,
+            "y": 6.821005360416637,
+            "heading": -1.5311822314903374,
+            "angularVelocity": 4.134075489976246,
+            "velocityX": 1.6039125052796561,
+            "velocityY": 1.3567993244094194,
+            "timestamp": 3.904558820878511
+        },
+        {
+            "x": 2.105765126126845,
+            "y": 6.86764701678895,
+            "heading": -1.3406160520225758,
+            "angularVelocity": 4.868314743270257,
+            "velocityX": 1.401971804927838,
+            "velocityY": 1.1915349512807134,
+            "timestamp": 3.943702999001175
+        },
+        {
+            "x": 2.1526116639678397,
+            "y": 6.908337111426706,
+            "heading": -1.1177858581420712,
+            "angularVelocity": 5.692550069188692,
+            "velocityX": 1.1967689727497768,
+            "velocityY": 1.039492884746424,
+            "timestamp": 3.982847177123839
+        },
+        {
+            "x": 2.1904354446918113,
+            "y": 6.943500988064586,
+            "heading": -0.8664176462137954,
+            "angularVelocity": 6.421598919271669,
+            "velocityX": 0.966268358105399,
+            "velocityY": 0.8983168972839031,
+            "timestamp": 4.021991355246503
+        },
+        {
+            "x": 2.218208118213364,
+            "y": 6.971474589606098,
+            "heading": -0.6268162193807357,
+            "angularVelocity": 6.120997765804011,
+            "velocityX": 0.7094969125299588,
+            "velocityY": 0.7146299368925976,
+            "timestamp": 4.061135533369167
+        },
+        {
+            "x": 2.2378592256547205,
+            "y": 6.992029926313173,
+            "heading": -0.4114319933826866,
+            "angularVelocity": 5.502331031784905,
+            "velocityX": 0.5020186496131576,
+            "velocityY": 0.5251186177076611,
+            "timestamp": 4.10027971149183
+        },
+        {
+            "x": 2.2511803788337867,
+            "y": 7.004798124284798,
+            "heading": -0.22575032024531463,
+            "angularVelocity": 4.743532296310119,
+            "velocityX": 0.3403099469178361,
+            "velocityY": 0.32618383075033625,
+            "timestamp": 4.139423889614494
+        },
+        {
+            "x": 2.261045669673148,
+            "y": 7.01480850997709,
+            "heading": -0.09556543349291266,
+            "angularVelocity": 3.32577903013953,
+            "velocityX": 0.25202447241187803,
+            "velocityY": 0.2557311501323063,
+            "timestamp": 4.1785680677371575
+        },
+        {
+            "x": 2.266437621623355,
+            "y": 7.022862452660783,
+            "heading": -0.01995585424474321,
+            "angularVelocity": 1.931566400787266,
+            "velocityX": 0.1377459486647134,
+            "velocityY": 0.2057507161972769,
+            "timestamp": 4.217712245859821
+        },
+        {
+            "x": 2.2676985263824463,
+            "y": 7.028899669647217,
+            "heading": -4.525227980572114e-34,
+            "angularVelocity": 0.509803889155846,
+            "velocityX": 0.03221180823213118,
+            "velocityY": 0.1542302655458861,
+            "timestamp": 4.256856423982485
+        },
+        {
+            "x": 2.265418134279513,
+            "y": 7.032677037086849,
+            "heading": -0.03924023540233394,
+            "angularVelocity": -0.9804537079645166,
+            "velocityX": -0.056977713563887346,
+            "velocityY": 0.09438103198308487,
+            "timestamp": 4.2968789513935945
+        },
+        {
+            "x": 2.260299136724811,
+            "y": 7.032742565757645,
+            "heading": -0.13692518133038253,
+            "angularVelocity": -2.440749054267192,
+            "velocityX": -0.1279029058340032,
+            "velocityY": 0.0016372946696696046,
+            "timestamp": 4.3369014788047044
+        },
+        {
+            "x": 2.251431996366295,
+            "y": 7.026515227476796,
+            "heading": -0.2837029990611584,
+            "angularVelocity": -3.667380028829281,
+            "velocityX": -0.22155373316216304,
+            "velocityY": -0.15559582774173908,
+            "timestamp": 4.376924006215814
+        },
+        {
+            "x": 2.2367988748775502,
+            "y": 7.013230050225817,
+            "heading": -0.4715484199670708,
+            "angularVelocity": -4.693492216930044,
+            "velocityX": -0.36562212422103535,
+            "velocityY": -0.3319424861532188,
+            "timestamp": 4.416946533626924
+        },
+        {
+            "x": 2.214572389947544,
+            "y": 6.992840585930019,
+            "heading": -0.6937250351049046,
+            "angularVelocity": -5.551288974223076,
+            "velocityX": -0.5553493586673437,
+            "velocityY": -0.5094496928282007,
+            "timestamp": 4.456969061038034
+        },
+        {
+            "x": 2.182927977382888,
+            "y": 6.964829235368663,
+            "heading": -0.9374735446804039,
+            "angularVelocity": -6.090282781787505,
+            "velocityX": -0.7906650232157135,
+            "velocityY": -0.6998895965171087,
+            "timestamp": 4.496991588449144
+        },
+        {
+            "x": 2.1407843072832162,
+            "y": 6.928562668933627,
+            "heading": -1.1813002792094054,
+            "angularVelocity": -6.092237304866431,
+            "velocityX": -1.0529987191157046,
+            "velocityY": -0.9061538283803887,
+            "timestamp": 4.537014115860254
+        },
+        {
+            "x": 2.0895949461282366,
+            "y": 6.88678871775435,
+            "heading": -1.3936812170017512,
+            "angularVelocity": -5.3065348824870116,
+            "velocityX": -1.27901370718457,
+            "velocityY": -1.0437609486821178,
+            "timestamp": 4.577036643271364
+        },
+        {
+            "x": 2.0327245845451865,
+            "y": 6.835876323976478,
+            "heading": -1.5775207555282802,
+            "angularVelocity": -4.593401527048441,
+            "velocityX": -1.420958776513053,
+            "velocityY": -1.2720934201606515,
+            "timestamp": 4.617059170682474
+        },
+        {
+            "x": 1.969383401794422,
+            "y": 6.775670280789337,
+            "heading": -1.7350606577956689,
+            "angularVelocity": -3.9362807013446637,
+            "velocityX": -1.582638250206598,
+            "velocityY": -1.5043038778812523,
+            "timestamp": 4.657081698093584
+        },
+        {
+            "x": 1.8991012602387887,
+            "y": 6.706204931476766,
+            "heading": -1.8673610734924142,
+            "angularVelocity": -3.305648699737559,
+            "velocityX": -1.7560645491899718,
+            "velocityY": -1.7356562367745094,
+            "timestamp": 4.6971042255046935
+        },
+        {
+            "x": 1.8215077032950175,
+            "y": 6.627747409048215,
+            "heading": -1.9742080738130303,
+            "angularVelocity": -2.6696714883367685,
+            "velocityX": -1.938747049798569,
+            "velocityY": -1.960334029448928,
+            "timestamp": 4.737126752915803
+        },
+        {
+            "x": 1.7362669499337922,
+            "y": 6.540782802917572,
+            "heading": -2.0543346433472873,
+            "angularVelocity": -2.002036720749795,
+            "velocityX": -2.1298193511278303,
+            "velocityY": -2.172891412812259,
+            "timestamp": 4.777149280326913
+        },
+        {
+            "x": 1.6430739434891832,
+            "y": 6.445951687040137,
+            "heading": -2.1059605165739685,
+            "angularVelocity": -1.2899203665071555,
+            "velocityX": -2.328513776437315,
+            "velocityY": -2.3694434612620463,
+            "timestamp": 4.817171807738023
+        },
+        {
+            "x": 1.55728497087458,
+            "y": 6.34602370365502,
+            "heading": -2.1651565461839404,
+            "angularVelocity": -1.4790677510671213,
+            "velocityX": -2.143517118081594,
+            "velocityY": -2.496793427328095,
+            "timestamp": 4.857194335149133
+        },
+        {
+            "x": 1.4798885640406532,
+            "y": 6.2518908184674045,
+            "heading": -2.25892978627056,
+            "angularVelocity": -2.3430114526097965,
+            "velocityX": -1.9338210712910326,
+            "velocityY": -2.351997519314229,
+            "timestamp": 4.897216862560243
+        },
+        {
+            "x": 1.4136471471462948,
+            "y": 6.162688939074268,
+            "heading": -2.372895153669848,
+            "angularVelocity": -2.847530497727978,
+            "velocityX": -1.6551032925514553,
+            "velocityY": -2.2287917621208337,
+            "timestamp": 4.937239389971353
+        },
+        {
+            "x": 1.3600641421628377,
+            "y": 6.075598413595313,
+            "heading": -2.5001562416185403,
+            "angularVelocity": -3.179736417979598,
+            "velocityX": -1.3388211202419749,
+            "velocityY": -2.1760376246199,
+            "timestamp": 4.977261917382463
+        },
+        {
+            "x": 1.319394826243984,
+            "y": 5.989620775668987,
+            "heading": -2.6390216185129676,
+            "angularVelocity": -3.4696803494694044,
+            "velocityX": -1.016160611275252,
+            "velocityY": -2.148231096031668,
+            "timestamp": 5.017284444793573
+        },
+        {
+            "x": 1.2917069915865356,
+            "y": 5.904139479101947,
+            "heading": -2.789119490409166,
+            "angularVelocity": -3.75033466413552,
+            "velocityX": -0.6918062513404106,
+            "velocityY": -2.1358295464196915,
+            "timestamp": 5.057306972204683
+        },
+        {
+            "x": 1.2763632236019438,
+            "y": 5.822310174000512,
+            "heading": -2.9261084847899674,
+            "angularVelocity": -3.422797190533623,
+            "velocityX": -0.3833782866079771,
+            "velocityY": -2.0445811495333315,
+            "timestamp": 5.0973294996157925
+        },
+        {
+            "x": 1.2717668608990054,
+            "y": 5.744491133376307,
+            "heading": -3.036631348807283,
+            "angularVelocity": -2.7615163550774677,
+            "velocityX": -0.11484438890439615,
+            "velocityY": -1.9443809688691047,
+            "timestamp": 5.137352027026902
+        },
+        {
+            "x": 1.2769543336934923,
+            "y": 5.67003274843766,
+            "heading": -3.1136420724595726,
+            "angularVelocity": -1.9241844189708244,
+            "velocityX": 0.12961382326511559,
+            "velocityY": -1.8604118668922167,
+            "timestamp": 5.177374554438012
+        },
+        {
+            "x": 1.290967939745204,
+            "y": 5.598089181810031,
+            "heading": -3.1506602139394917,
+            "angularVelocity": -0.9249326285586663,
+            "velocityX": 0.35014295593490485,
+            "velocityY": -1.7975767969031158,
+            "timestamp": 5.217397081849122
+        },
+        {
+            "x": 1.3126477003097534,
+            "y": 5.52810525894165,
+            "heading": -3.1411853071795863,
+            "angularVelocity": 0.23673934088618437,
+            "velocityX": 0.5416889428759926,
+            "velocityY": -1.748613278454628,
+            "timestamp": 5.257419609260232
+        },
+        {
+            "x": 1.3612675355497719,
+            "y": 5.427793675126224,
+            "heading": -3.0142432356702726,
+            "angularVelocity": 2.1028160607727164,
+            "velocityX": 0.8053954784197191,
+            "velocityY": -1.6616777008649257,
+            "timestamp": 5.317787262845048
+        },
+        {
+            "x": 1.4285233143505713,
+            "y": 5.3377057272616115,
+            "heading": -2.7956250008289785,
+            "angularVelocity": 3.6214466168399086,
+            "velocityX": 1.1141029145071253,
+            "velocityY": -1.4923215085383357,
+            "timestamp": 5.378154916429864
+        },
+        {
+            "x": 1.5158281848971034,
+            "y": 5.264153119824496,
+            "heading": -2.514690848185286,
+            "angularVelocity": 4.653719930475422,
+            "velocityX": 1.4462193801167051,
+            "velocityY": -1.218410905001898,
+            "timestamp": 5.43852257001468
+        },
+        {
+            "x": 1.6230211167401283,
+            "y": 5.213340726984806,
+            "heading": -2.222036602459031,
+            "angularVelocity": 4.847865178577475,
+            "velocityX": 1.7756683501441766,
+            "velocityY": -0.841715551662108,
+            "timestamp": 5.498890223599496
+        },
+        {
+            "x": 1.749062978630215,
+            "y": 5.185595086249832,
+            "heading": -1.9510224888468373,
+            "angularVelocity": 4.489392870495166,
+            "velocityX": 2.0879039420175545,
+            "velocityY": -0.45961105140506303,
+            "timestamp": 5.5592578771843115
+        },
+        {
+            "x": 1.894299674373998,
+            "y": 5.179254614564922,
+            "heading": -1.7150026509570102,
+            "angularVelocity": 3.9097070015852564,
+            "velocityX": 2.4058694867066146,
+            "velocityY": -0.10503094469295028,
+            "timestamp": 5.619625530769127
+        },
+        {
+            "x": 2.058119871692227,
+            "y": 5.193402237247618,
+            "heading": -1.5227916684845806,
+            "angularVelocity": 3.1840061863987503,
+            "velocityX": 2.713708212761054,
+            "velocityY": 0.2343576707485958,
+            "timestamp": 5.679993184353943
+        },
+        {
+            "x": 2.2389183972423563,
+            "y": 5.227931703588396,
+            "heading": -1.382086082537418,
+            "angularVelocity": 2.3308109159729624,
+            "velocityX": 2.9949569813262746,
+            "velocityY": 0.5719862259059945,
+            "timestamp": 5.740360837938759
+        },
+        {
+            "x": 2.4352711849705373,
+            "y": 5.282232257814921,
+            "heading": -1.300964814671627,
+            "angularVelocity": 1.3437869959914348,
+            "velocityX": 3.252615864095299,
+            "velocityY": 0.8994975123595437,
+            "timestamp": 5.800728491523575
+        },
+        {
+            "x": 2.6508698274310176,
+            "y": 5.350520182077314,
+            "heading": -1.2935554192187855,
+            "angularVelocity": 0.12273784076155059,
+            "velocityX": 3.5714265779366063,
+            "velocityY": 1.1312005719494984,
+            "timestamp": 5.861096145108391
+        },
+        {
+            "x": 2.8627491717000777,
+            "y": 5.435718232774483,
+            "heading": -1.2935551586148022,
+            "angularVelocity": 0.000004316947366860679,
+            "velocityX": 3.5098157984784524,
+            "velocityY": 1.4113195666528,
+            "timestamp": 5.921463798693207
+        },
+        {
+            "x": 3.0746283658136546,
+            "y": 5.520916656893941,
+            "heading": -1.2935548980122595,
+            "angularVelocity": 0.000004316923507170679,
+            "velocityX": 3.5098133111284437,
+            "velocityY": 1.4113257524537706,
+            "timestamp": 5.981831452278023
+        },
+        {
+            "x": 3.2865075599250586,
+            "y": 5.606115081018762,
+            "heading": -1.2935546374096347,
+            "angularVelocity": 0.000004316924865568706,
+            "velocityX": 3.509813311092456,
+            "velocityY": 1.4113257525426077,
+            "timestamp": 6.042199105862839
+        },
+        {
+            "x": 3.4983867540364426,
+            "y": 5.691313505143594,
+            "heading": -1.293554376806928,
+            "angularVelocity": 0.0000043169262224547675,
+            "velocityX": 3.5098133110921186,
+            "velocityY": 1.4113257525427836,
+            "timestamp": 6.1025667594476545
+        },
+        {
+            "x": 3.710265948147806,
+            "y": 5.776511929268437,
+            "heading": -1.293554116204139,
+            "angularVelocity": 0.000004316927581395022,
+            "velocityX": 3.5098133110917824,
+            "velocityY": 1.4113257525429583,
+            "timestamp": 6.16293441303247
+        },
+        {
+            "x": 3.922145142259149,
+            "y": 5.861710353393289,
+            "heading": -1.2935538556012682,
+            "angularVelocity": 0.000004316928938543054,
+            "velocityX": 3.509813311091446,
+            "velocityY": 1.4113257525431333,
+            "timestamp": 6.223302066617286
+        },
+        {
+            "x": 4.134024336370472,
+            "y": 5.9469087775181535,
+            "heading": -1.2935535949983157,
+            "angularVelocity": 0.000004316930294815631,
+            "velocityX": 3.5098133110911096,
+            "velocityY": 1.4113257525433083,
+            "timestamp": 6.283669720202102
+        },
+        {
+            "x": 4.345903530481774,
+            "y": 6.0321072016430275,
+            "heading": -1.293553334395281,
+            "angularVelocity": 0.000004316931652676853,
+            "velocityX": 3.509813311090773,
+            "velocityY": 1.4113257525434828,
+            "timestamp": 6.344037373786918
+        },
+        {
+            "x": 4.557782724593057,
+            "y": 6.117305625767913,
+            "heading": -1.2935530737921643,
+            "angularVelocity": 0.000004316933011809188,
+            "velocityX": 3.509813311090437,
+            "velocityY": 1.4113257525436576,
+            "timestamp": 6.404405027371734
+        },
+        {
+            "x": 4.769661918704319,
+            "y": 6.202504049892808,
+            "heading": -1.2935528131889658,
+            "angularVelocity": 0.000004316934368820573,
+            "velocityX": 3.5098133110901,
+            "velocityY": 1.4113257525438323,
+            "timestamp": 6.46477268095655
+        },
+        {
+            "x": 4.9815411128155604,
+            "y": 6.287702474017714,
+            "heading": -1.2935525525856855,
+            "angularVelocity": 0.000004316935725471939,
+            "velocityX": 3.5098133110897636,
+            "velocityY": 1.4113257525440075,
+            "timestamp": 6.525140334541366
+        },
+        {
+            "x": 5.193420306926782,
+            "y": 6.37290089814263,
+            "heading": -1.293552291982323,
+            "angularVelocity": 0.000004316937084312971,
+            "velocityX": 3.5098133110894274,
+            "velocityY": 1.4113257525441825,
+            "timestamp": 6.5855079881261815
+        },
+        {
+            "x": 5.405299501037983,
+            "y": 6.458099322267557,
+            "heading": -1.2935520313788786,
+            "angularVelocity": 0.000004316938442183061,
+            "velocityX": 3.509813311089091,
+            "velocityY": 1.411325752544357,
+            "timestamp": 6.645875641710997
+        },
+        {
+            "x": 5.617178695149163,
+            "y": 6.543297746392494,
+            "heading": -1.2935517707753523,
+            "angularVelocity": 0.000004316939799293205,
+            "velocityX": 3.509813311088755,
+            "velocityY": 1.411325752544532,
+            "timestamp": 6.706243295295813
+        },
+        {
+            "x": 5.829057889260324,
+            "y": 6.628496170517443,
+            "heading": -1.293551510171744,
+            "angularVelocity": 0.000004316941156612923,
+            "velocityX": 3.5098133110884175,
+            "velocityY": 1.4113257525447067,
+            "timestamp": 6.766610948880629
+        },
+        {
+            "x": 6.040937083371463,
+            "y": 6.713694594642401,
+            "heading": -1.293551249568054,
+            "angularVelocity": 0.0000043169425150049,
+            "velocityX": 3.5098133110880814,
+            "velocityY": 1.411325752544882,
+            "timestamp": 6.826978602465445
+        },
+        {
+            "x": 6.252816277482583,
+            "y": 6.798893018767371,
+            "heading": -1.2935509889642816,
+            "angularVelocity": 0.000004316943872175867,
+            "velocityX": 3.5098133110877447,
+            "velocityY": 1.4113257525450569,
+            "timestamp": 6.887346256050261
+        },
+        {
+            "x": 6.464695471593406,
+            "y": 6.884091442893039,
+            "heading": -1.293550728360427,
+            "angularVelocity": 0.000004316945230336038,
+            "velocityX": 3.509813311082821,
+            "velocityY": 1.4113257525566392,
+            "timestamp": 6.947713909635077
+        },
+        {
+            "x": 6.676574646384632,
+            "y": 6.969289915063695,
+            "heading": -1.2935504677556948,
+            "angularVelocity": 0.000004316959775515561,
+            "velocityX": 3.5098129910505658,
+            "velocityY": 1.41132654842967,
+            "timestamp": 7.008081563219893
+        },
+        {
+            "x": 6.8805324076089756,
+            "y": 7.05464814554522,
+            "heading": -1.2707475795927152,
+            "angularVelocity": 0.3777335511465864,
+            "velocityX": 3.3785934869538337,
+            "velocityY": 1.4139729708327582,
+            "timestamp": 7.068449216804709
+        },
+        {
+            "x": 7.061472859527864,
+            "y": 7.1337324563685955,
+            "heading": -1.1711733781498967,
+            "angularVelocity": 1.6494628419326967,
+            "velocityX": 2.997308014708086,
+            "velocityY": 1.3100444712873218,
+            "timestamp": 7.1288168703895245
+        },
+        {
+            "x": 7.217120419361732,
+            "y": 7.210021270677617,
+            "heading": -1.009883648548617,
+            "angularVelocity": 2.6717906034673087,
+            "velocityX": 2.5783271436115207,
+            "velocityY": 1.263736616859493,
+            "timestamp": 7.18918452397434
+        },
+        {
+            "x": 7.347594186911189,
+            "y": 7.2775930773892314,
+            "heading": -0.7947029660436775,
+            "angularVelocity": 3.5645030032948655,
+            "velocityX": 2.1613191800828018,
+            "velocityY": 1.119337968249467,
+            "timestamp": 7.249552177559156
+        },
+        {
+            "x": 7.451332936822743,
+            "y": 7.3349655169215735,
+            "heading": -0.5434124114773135,
+            "angularVelocity": 4.162668907005051,
+            "velocityX": 1.718449264651363,
+            "velocityY": 0.9503837920706217,
+            "timestamp": 7.309919831143972
+        },
+        {
+            "x": 7.527427510377051,
+            "y": 7.381739778721793,
+            "heading": -0.31640528763659864,
+            "angularVelocity": 3.7604099275081686,
+            "velocityX": 1.2605189871658125,
+            "velocityY": 0.7748232542201173,
+            "timestamp": 7.370287484728788
+        },
+        {
+            "x": 7.577573442921129,
+            "y": 7.419722379939866,
+            "heading": -0.13731969717190406,
+            "angularVelocity": 2.966581933039389,
+            "velocityX": 0.8306755284702909,
+            "velocityY": 0.6291879667760935,
+            "timestamp": 7.430655138313604
+        },
+        {
+            "x": 7.603931547937356,
+            "y": 7.45112329376933,
+            "heading": -0.02759055048880514,
+            "angularVelocity": 1.8176811614672275,
+            "velocityX": 0.4366262965512501,
+            "velocityY": 0.5201612447193243,
+            "timestamp": 7.49102279189842
+        },
+        {
+            "x": 7.608186721801758,
+            "y": 7.477188587188721,
+            "heading": 0,
+            "angularVelocity": 0.4570419562529598,
+            "velocityX": 0.07048764713743307,
+            "velocityY": 0.43177582482595267,
+            "timestamp": 7.551390445483236
+        },
+        {
+            "x": 7.590312649337569,
+            "y": 7.4994357327814924,
+            "heading": -0.062413166933384966,
+            "angularVelocity": -1.0141265252388012,
+            "velocityX": -0.2904286369464457,
+            "velocityY": 0.3614849488499569,
+            "timestamp": 7.612934212833851
+        },
+        {
+            "x": 7.548542604069605,
+            "y": 7.5172581696138145,
+            "heading": -0.20295351245592444,
+            "angularVelocity": -2.2835837254141333,
+            "velocityX": -0.6787047180585992,
+            "velocityY": 0.28958963026732126,
+            "timestamp": 7.6744779801844665
+        },
+        {
+            "x": 7.481084260938195,
+            "y": 7.5242062844896465,
+            "heading": -0.3946635775555263,
+            "angularVelocity": -3.1150199825666713,
+            "velocityX": -1.0961035704411768,
+            "velocityY": 0.11289713280385957,
+            "timestamp": 7.736021747535082
+        },
+        {
+            "x": 7.388848586007302,
+            "y": 7.518355172240059,
+            "heading": -0.6407957549264521,
+            "angularVelocity": -3.9993030645769183,
+            "velocityX": -1.4987005004978822,
+            "velocityY": -0.09507237696798558,
+            "timestamp": 7.797565514885697
+        },
+        {
+            "x": 7.269389686365513,
+            "y": 7.4992097780564855,
+            "heading": -0.9197330786516805,
+            "angularVelocity": -4.5323407346209255,
+            "velocityX": -1.941039763803056,
+            "velocityY": -0.31108583383434507,
+            "timestamp": 7.859109282236313
+        },
+        {
+            "x": 7.1251329311926535,
+            "y": 7.464894000432003,
+            "heading": -1.1518882810190125,
+            "angularVelocity": -3.772196801745026,
+            "velocityX": -2.343970175745461,
+            "velocityY": -0.5575833118727538,
+            "timestamp": 7.920653049586928
+        },
+        {
+            "x": 6.953983810333919,
+            "y": 7.422493842128991,
+            "heading": -1.327948941925082,
+            "angularVelocity": -2.8607390883799795,
+            "velocityX": -2.780933443409385,
+            "velocityY": -0.6889431721242942,
+            "timestamp": 7.9821968169375435
+        },
+        {
+            "x": 6.759450448912011,
+            "y": 7.372163465988786,
+            "heading": -1.4240765574561767,
+            "angularVelocity": -1.5619390828555313,
+            "velocityX": -3.1608945925206284,
+            "velocityY": -0.817798102177794,
+            "timestamp": 8.043740584288159
+        },
+        {
+            "x": 6.542371526128891,
+            "y": 7.310669881087202,
+            "heading": -1.4457611917111317,
+            "angularVelocity": -0.3523449276580275,
+            "velocityX": -3.5272283795435984,
+            "velocityY": -0.9991846055060891,
+            "timestamp": 8.105284351638774
+        },
+        {
+            "x": 6.323008319060415,
+            "y": 7.232674892951184,
+            "heading": -1.4457615219539912,
+            "angularVelocity": -0.000005365983815098308,
+            "velocityX": -3.564344799023447,
+            "velocityY": -1.2673092905034502,
+            "timestamp": 8.16682811898939
+        },
+        {
+            "x": 6.103645219000666,
+            "y": 7.154679603847749,
+            "heading": -1.4457618521936861,
+            "angularVelocity": -0.000005365932394056072,
+            "velocityX": -3.5643430602815576,
+            "velocityY": -1.267314180802329,
+            "timestamp": 8.228371886340005
+        },
+        {
+            "x": 5.884282118942346,
+            "y": 7.076684314740222,
+            "heading": -1.4457621824332476,
+            "angularVelocity": -0.000005365930225246195,
+            "velocityX": -3.5643430602583215,
+            "velocityY": -1.2673141808688406,
+            "timestamp": 8.28991565369062
+        },
+        {
+            "x": 5.66491901888401,
+            "y": 6.998689025632669,
+            "heading": -1.4457625126726754,
+            "angularVelocity": -0.000005365928056443698,
+            "velocityX": -3.564343060258591,
+            "velocityY": -1.2673141808692443,
+            "timestamp": 8.351459421041236
+        },
+        {
+            "x": 5.445555918825658,
+            "y": 6.920693736525091,
+            "heading": -1.44576284291197,
+            "angularVelocity": -0.000005365925887921031,
+            "velocityX": -3.5643430602588597,
+            "velocityY": -1.2673141808696466,
+            "timestamp": 8.413003188391851
+        },
+        {
+            "x": 5.226192818767289,
+            "y": 6.842698447417488,
+            "heading": -1.445763173151131,
+            "angularVelocity": -0.000005365923719151191,
+            "velocityX": -3.564343060259129,
+            "velocityY": -1.2673141808700494,
+            "timestamp": 8.474546955742467
+        },
+        {
+            "x": 5.006829718708903,
+            "y": 6.764703158309861,
+            "heading": -1.4457635033901584,
+            "angularVelocity": -0.000005365921549364942,
+            "velocityX": -3.5643430602593975,
+            "velocityY": -1.2673141808704518,
+            "timestamp": 8.536090723093082
+        },
+        {
+            "x": 4.787466618650501,
+            "y": 6.68670786920221,
+            "heading": -1.4457638336290524,
+            "angularVelocity": -0.000005365919381086304,
+            "velocityX": -3.564343060259667,
+            "velocityY": -1.2673141808708541,
+            "timestamp": 8.597634490443697
+        },
+        {
+            "x": 4.568103518592082,
+            "y": 6.608712580094533,
+            "heading": -1.4457641638678127,
+            "angularVelocity": -0.000005365917211650528,
+            "velocityX": -3.5643430602599366,
+            "velocityY": -1.2673141808712571,
+            "timestamp": 8.659178257794313
+        },
+        {
+            "x": 4.348740418533646,
+            "y": 6.530717290986832,
+            "heading": -1.4457644941064398,
+            "angularVelocity": -0.000005365915042310373,
+            "velocityX": -3.564343060260205,
+            "velocityY": -1.2673141808716597,
+            "timestamp": 8.720722025144928
+        },
+        {
+            "x": 4.1293773184751945,
+            "y": 6.452722001879106,
+            "heading": -1.4457648243449335,
+            "angularVelocity": -0.000005365912874367381,
+            "velocityX": -3.564343060260474,
+            "velocityY": -1.267314180872062,
+            "timestamp": 8.782265792495544
+        },
+        {
+            "x": 3.910014218416726,
+            "y": 6.374726712771355,
+            "heading": -1.4457651545832937,
+            "angularVelocity": -0.000005365910705198878,
+            "velocityX": -3.5643430602607435,
+            "velocityY": -1.2673141808724648,
+            "timestamp": 8.843809559846159
+        },
+        {
+            "x": 3.690651118358241,
+            "y": 6.296731423663579,
+            "heading": -1.4457654848215202,
+            "angularVelocity": -0.000005365908536510064,
+            "velocityX": -3.5643430602610127,
+            "velocityY": -1.2673141808728674,
+            "timestamp": 8.905353327196774
+        },
+        {
+            "x": 3.47128801829974,
+            "y": 6.218736134555778,
+            "heading": -1.4457658150596133,
+            "angularVelocity": -0.0000053659063672733,
+            "velocityX": -3.5643430602612813,
+            "velocityY": -1.2673141808732702,
+            "timestamp": 8.96689709454739
+        },
+        {
+            "x": 3.2519249182412215,
+            "y": 6.140740845447953,
+            "heading": -1.445766145297573,
+            "angularVelocity": -0.000005365904199203489,
+            "velocityX": -3.564343060261551,
+            "velocityY": -1.2673141808736725,
+            "timestamp": 9.028440861898005
+        },
+        {
+            "x": 3.0325618181826868,
+            "y": 6.062745556340103,
+            "heading": -1.4457664755353992,
+            "angularVelocity": -0.000005365902029653181,
+            "velocityX": -3.5643430602618196,
+            "velocityY": -1.267314180874075,
+            "timestamp": 9.08998462924862
+        },
+        {
+            "x": 2.8131987181241356,
+            "y": 5.984750267232228,
+            "heading": -1.445766805773092,
+            "angularVelocity": -0.000005365899860579176,
+            "velocityX": -3.5643430602620887,
+            "velocityY": -1.2673141808744774,
+            "timestamp": 9.151528396599236
+        },
+        {
+            "x": 2.5938356180647952,
+            "y": 5.906754978126503,
+            "heading": -1.4457671360106512,
+            "angularVelocity": -0.000005365897692675495,
+            "velocityX": -3.564343060274917,
+            "velocityY": -1.2673141808395556,
+            "timestamp": 9.213072163949851
+        },
+        {
+            "x": 2.374472460827342,
+            "y": 5.8287598498370325,
+            "heading": -1.4457674662499087,
+            "angularVelocity": -0.000005365925286950734,
+            "velocityX": -3.5643439893391378,
+            "velocityY": -1.2673115678006912,
+            "timestamp": 9.274615931300467
+        },
+        {
+            "x": 2.1579642030031834,
+            "y": 5.762355072468581,
+            "heading": -1.4644178853402519,
+            "angularVelocity": -0.30304318200235486,
+            "velocityX": -3.5179558734308305,
+            "velocityY": -1.0789846027810281,
+            "timestamp": 9.336159698651082
+        },
+        {
+            "x": 1.9655559601169903,
+            "y": 5.705313391080233,
+            "heading": -1.5528631085528386,
+            "angularVelocity": -1.4371109702906752,
+            "velocityX": -3.1263643934899394,
+            "velocityY": -0.9268474102890126,
+            "timestamp": 9.397703466001698
+        },
+        {
+            "x": 1.799063271019786,
+            "y": 5.655117829615961,
+            "heading": -1.7042345036546405,
+            "angularVelocity": -2.459573107369865,
+            "velocityX": -2.7052729506904143,
+            "velocityY": -0.8156075525618497,
+            "timestamp": 9.459247233352313
+        },
+        {
+            "x": 1.6591648707007773,
+            "y": 5.609591955542646,
+            "heading": -1.9150308855459262,
+            "angularVelocity": -3.425145891547008,
+            "velocityX": -2.273153015186835,
+            "velocityY": -0.739731674435107,
+            "timestamp": 9.520791000702928
+        },
+        {
+            "x": 1.5431948510849691,
+            "y": 5.574795624095963,
+            "heading": -2.189608491806892,
+            "angularVelocity": -4.461501433552055,
+            "velocityX": -1.884350351110707,
+            "velocityY": -0.5653916382539252,
+            "timestamp": 9.582334768053544
+        },
+        {
+            "x": 1.4525455234391338,
+            "y": 5.551032580854212,
+            "heading": -2.523374227659994,
+            "angularVelocity": -5.423225620096252,
+            "velocityX": -1.4729245795013663,
+            "velocityY": -0.3861161619563059,
+            "timestamp": 9.64387853540416
+        },
+        {
+            "x": 1.3896696664094041,
+            "y": 5.537613504449024,
+            "heading": -2.816143624187386,
+            "angularVelocity": -4.757092539679631,
+            "velocityX": -1.0216445911009162,
+            "velocityY": -0.21804119219317228,
+            "timestamp": 9.705422302754775
+        },
+        {
+            "x": 1.3505170646137672,
+            "y": 5.530954725925202,
+            "heading": -3.0286369343399735,
+            "angularVelocity": -3.4527185984896,
+            "velocityX": -0.6361749285282489,
+            "velocityY": -0.1081958224280782,
+            "timestamp": 9.76696607010539
+        },
+        {
+            "x": 1.332138180732727,
+            "y": 5.52810525894165,
+            "heading": -3.1411853071795863,
+            "angularVelocity": -1.828753384537267,
+            "velocityX": -0.2986311152571343,
+            "velocityY": -0.046299846535530764,
+            "timestamp": 9.828509837456005
+        },
+        {
+            "x": 1.332138180732727,
+            "y": 5.52810525894165,
+            "heading": -3.1411853071795863,
+            "angularVelocity": -8.15091951330742e-35,
+            "velocityX": 0,
+            "velocityY": 2.0839850321711294e-35,
+            "timestamp": 9.89005360480662
+        }
+    ]
+}
\ No newline at end of file
diff --git a/simgui-ds.json b/simgui-ds.json
index cf566929..aca5a327 100644
--- a/simgui-ds.json
+++ b/simgui-ds.json
@@ -93,12 +93,11 @@
   ],
   "robotJoysticks": [
     {
-      "guid": "78696e70757401000000000000000000",
-      "name": "Keyboard",
-      "useGamepad": true
+      "guid": "Keyboard0",
+      "name": "Keyboard"
     },
     {
-      "useGamepad": true
+      "guid": "Keyboard1"
     }
   ]
 }
diff --git a/simgui.json b/simgui.json
index e063301d..9594ddb6 100644
--- a/simgui.json
+++ b/simgui.json
@@ -1,4 +1,16 @@
 {
+  "HALProvider": {
+    "Other Devices": {
+      "Talon FX (v6)[20]/Rotor Sensor": {
+        "header": {
+          "open": true
+        }
+      },
+      "window": {
+        "visible": false
+      }
+    }
+  },
   "NTProvider": {
     "types": {
       "/AdvantageKit/RealOutputs/Elevator/Mechanism2d": "Mechanism2d",
diff --git a/src/main/deploy/choreo/auto1.1.traj b/src/main/deploy/choreo/auto1.1.traj
new file mode 100644
index 00000000..c037a4f6
--- /dev/null
+++ b/src/main/deploy/choreo/auto1.1.traj
@@ -0,0 +1,1876 @@
+{
+    "samples": [
+        {
+            "x": 1.3321385383605957,
+            "y": 5.547596454620361,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -3.4724754769815887e-38,
+            "velocityY": 7.030705743146232e-38,
+            "timestamp": 0
+        },
+        {
+            "x": 1.361410083208431,
+            "y": 5.5023955382692575,
+            "heading": 0.056119311202146825,
+            "angularVelocity": 0.6842584112779871,
+            "velocityX": 0.35690567728254624,
+            "velocityY": -0.5511312692222103,
+            "timestamp": 0.08201479189321614
+        },
+        {
+            "x": 1.4202660843784345,
+            "y": 5.411892088977607,
+            "heading": 0.16553764559038536,
+            "angularVelocity": 1.3341292693969398,
+            "velocityX": 0.7176266599156221,
+            "velocityY": -1.1035015416424676,
+            "timestamp": 0.1640295837864323
+        },
+        {
+            "x": 1.509321709738084,
+            "y": 5.2758955282995315,
+            "heading": 0.32143534479454616,
+            "angularVelocity": 1.9008485616489859,
+            "velocityX": 1.0858483366707872,
+            "velocityY": -1.658195522280223,
+            "timestamp": 0.24604437567964843
+        },
+        {
+            "x": 1.6296766101338032,
+            "y": 5.093983916449887,
+            "heading": 0.5046862665772869,
+            "angularVelocity": 2.2343642842054003,
+            "velocityX": 1.4674779709545838,
+            "velocityY": -2.2180341820106597,
+            "timestamp": 0.3280591675728646
+        },
+        {
+            "x": 1.782194657966092,
+            "y": 4.869104821237669,
+            "heading": 0.6276214122511846,
+            "angularVelocity": 1.4989387016181202,
+            "velocityX": 1.859640734452738,
+            "velocityY": -2.7419333759331184,
+            "timestamp": 0.4100739594660807
+        },
+        {
+            "x": 1.9333417085303153,
+            "y": 4.6354174904379,
+            "heading": 0.6920907561549033,
+            "angularVelocity": 0.7860697127374073,
+            "velocityX": 1.8429242710390283,
+            "velocityY": -2.849331509662689,
+            "timestamp": 0.49208875135929686
+        },
+        {
+            "x": 2.056917521349593,
+            "y": 4.443493094854259,
+            "heading": 0.6585466805605577,
+            "angularVelocity": -0.40900031348028376,
+            "velocityX": 1.5067503064590375,
+            "velocityY": -2.340119277916704,
+            "timestamp": 0.574103543252513
+        },
+        {
+            "x": 2.1535962335212764,
+            "y": 4.293727999952354,
+            "heading": 0.5265725883978299,
+            "angularVelocity": -1.609149875482936,
+            "velocityX": 1.1787960432498548,
+            "velocityY": -1.8260741927736632,
+            "timestamp": 0.6561183351457291
+        },
+        {
+            "x": 2.2240985983067136,
+            "y": 4.187111119253089,
+            "heading": 0.3017485783287344,
+            "angularVelocity": -2.7412617270530677,
+            "velocityX": 0.859629868685535,
+            "velocityY": -1.2999713617280337,
+            "timestamp": 0.7381331270389453
+        },
+        {
+            "x": 2.2676985263824463,
+            "y": 4.124765396118164,
+            "heading": -1.6642026159430915e-35,
+            "angularVelocity": -3.6791970248685537,
+            "velocityX": 0.5316105432846836,
+            "velocityY": -0.7601765693205621,
+            "timestamp": 0.8201479189321614
+        },
+        {
+            "x": 2.2826958672458773,
+            "y": 4.104348331421266,
+            "heading": -0.17441872129385144,
+            "angularVelocity": -4.1236397648607985,
+            "velocityX": 0.35456991481679806,
+            "velocityY": -0.4827040310886087,
+            "timestamp": 0.8624451930006055
+        },
+        {
+            "x": 2.289905521300629,
+            "y": 4.095598364764115,
+            "heading": -0.366838925198976,
+            "angularVelocity": -4.549234156171776,
+            "velocityX": 0.17045197860942923,
+            "velocityY": -0.20686833489535802,
+            "timestamp": 0.9047424670690496
+        },
+        {
+            "x": 2.289262823237601,
+            "y": 4.0986035079624115,
+            "heading": -0.5765637041805302,
+            "angularVelocity": -4.958352130262211,
+            "velocityX": -0.015194786831599966,
+            "velocityY": 0.07104815297159969,
+            "timestamp": 0.9470397411374937
+        },
+        {
+            "x": 2.28071834356103,
+            "y": 4.1136177509731775,
+            "heading": -0.8009025976234118,
+            "angularVelocity": -5.303861735388993,
+            "velocityX": -0.20201017358104575,
+            "velocityY": 0.3549695185195792,
+            "timestamp": 0.9893370152059378
+        },
+        {
+            "x": 2.264115058699565,
+            "y": 4.140907151677903,
+            "heading": -1.0347556438013104,
+            "angularVelocity": -5.528797099299715,
+            "velocityX": -0.39253794073343345,
+            "velocityY": 0.6451810738575218,
+            "timestamp": 1.0316342892743817
+        },
+        {
+            "x": 2.2392828245472374,
+            "y": 4.180616635647853,
+            "heading": -1.271268857234422,
+            "angularVelocity": -5.5916892670292055,
+            "velocityX": -0.5870882864022154,
+            "velocityY": 0.9388189864361984,
+            "timestamp": 1.0739315633428257
+        },
+        {
+            "x": 2.206219336908872,
+            "y": 4.232592488850575,
+            "heading": -1.500368652073653,
+            "angularVelocity": -5.416419849385782,
+            "velocityX": -0.7816931082807688,
+            "velocityY": 1.228822763344421,
+            "timestamp": 1.1162288374112697
+        },
+        {
+            "x": 2.16560115012261,
+            "y": 4.295896098840919,
+            "heading": -1.7081840928578422,
+            "angularVelocity": -4.913211202403,
+            "velocityX": -0.9603027069908828,
+            "velocityY": 1.496635690704529,
+            "timestamp": 1.1585261114797136
+        },
+        {
+            "x": 2.116418529309532,
+            "y": 4.368321186816809,
+            "heading": -1.8863054196825848,
+            "angularVelocity": -4.211177451684291,
+            "velocityX": -1.162784645022098,
+            "velocityY": 1.7122873653439958,
+            "timestamp": 1.2008233855481576
+        },
+        {
+            "x": 2.058912682392385,
+            "y": 4.450563139928545,
+            "heading": -2.036216963298125,
+            "angularVelocity": -3.54423652391779,
+            "velocityX": -1.3595639005977715,
+            "velocityY": 1.9443795119906613,
+            "timestamp": 1.2431206596166016
+        },
+        {
+            "x": 1.99337005684811,
+            "y": 4.542961602948437,
+            "heading": -2.1579434366118924,
+            "angularVelocity": -2.877879863293171,
+            "velocityX": -1.549570911785386,
+            "velocityY": 2.184501603350988,
+            "timestamp": 1.2854179336850455
+        },
+        {
+            "x": 1.9201828437796038,
+            "y": 4.645652670813241,
+            "heading": -2.2501769997399386,
+            "angularVelocity": -2.180603009517744,
+            "velocityX": -1.7303056681637912,
+            "velocityY": 2.4278412764527704,
+            "timestamp": 1.3277152077534895
+        },
+        {
+            "x": 1.8398838191629743,
+            "y": 4.758671823925337,
+            "heading": -2.3107451299036645,
+            "angularVelocity": -1.4319629691907945,
+            "velocityX": -1.8984444360809696,
+            "velocityY": 2.6720197838095423,
+            "timestamp": 1.3700124818219335
+        },
+        {
+            "x": 1.753396216406846,
+            "y": 4.882199443808802,
+            "heading": -2.337574910152261,
+            "angularVelocity": -0.6343146417705776,
+            "velocityX": -2.0447559484844615,
+            "velocityY": 2.920462904620659,
+            "timestamp": 1.4123097558903774
+        },
+        {
+            "x": 1.6751428992292787,
+            "y": 4.996901240775032,
+            "heading": -2.400008661282964,
+            "angularVelocity": -1.4760703261792811,
+            "velocityX": -1.8500794413119903,
+            "velocityY": 2.7118011619525055,
+            "timestamp": 1.4546070299588214
+        },
+        {
+            "x": 1.604167639142011,
+            "y": 5.102445488685979,
+            "heading": -2.5005028957452873,
+            "angularVelocity": -2.375903333621606,
+            "velocityX": -1.6780102654468432,
+            "velocityY": 2.4952966883908108,
+            "timestamp": 1.4969043040272654
+        },
+        {
+            "x": 1.5418087398499432,
+            "y": 5.19740105341742,
+            "heading": -2.6238103064581355,
+            "angularVelocity": -2.9152566785584497,
+            "velocityX": -1.4743006651246806,
+            "velocityY": 2.2449570763776965,
+            "timestamp": 1.5392015780957093
+        },
+        {
+            "x": 1.488225747512813,
+            "y": 5.280327915096259,
+            "heading": -2.743877445785595,
+            "angularVelocity": -2.8386495813694843,
+            "velocityX": -1.2668190448969345,
+            "velocityY": 1.960572247389977,
+            "timestamp": 1.5814988521641533
+        },
+        {
+            "x": 1.4429482417442723,
+            "y": 5.35126150411709,
+            "heading": -2.8526923539591857,
+            "angularVelocity": -2.5726222450531995,
+            "velocityX": -1.0704591907098815,
+            "velocityY": 1.6770250703638487,
+            "timestamp": 1.6237961262325973
+        },
+        {
+            "x": 1.4056282761179824,
+            "y": 5.410288137316586,
+            "heading": -2.94638853961619,
+            "angularVelocity": -2.215182602675249,
+            "velocityX": -0.8823255504811011,
+            "velocityY": 1.3955186120027623,
+            "timestamp": 1.6660934003010412
+        },
+        {
+            "x": 1.3760265384921018,
+            "y": 5.457458867956814,
+            "heading": -3.023001318784294,
+            "angularVelocity": -1.8112935373596752,
+            "velocityX": -0.6998497723040017,
+            "velocityY": 1.115219164334259,
+            "timestamp": 1.7083906743694852
+        },
+        {
+            "x": 1.3539833473819511,
+            "y": 5.492803685860998,
+            "heading": -3.0814563447777146,
+            "angularVelocity": -1.3820045683991566,
+            "velocityX": -0.5211492134098515,
+            "velocityY": 0.8356287416297835,
+            "timestamp": 1.7506879484379292
+        },
+        {
+            "x": 1.3393859043766252,
+            "y": 5.516345418500216,
+            "heading": -3.121051917122202,
+            "angularVelocity": -0.9361258666554966,
+            "velocityX": -0.3451154554713123,
+            "velocityY": 0.5565780102311921,
+            "timestamp": 1.7929852225063732
+        },
+        {
+            "x": 1.3321385383605957,
+            "y": 5.52810525894165,
+            "heading": -3.1411853071795863,
+            "angularVelocity": -0.47599734263737736,
+            "velocityX": -0.17134357179382342,
+            "velocityY": 0.2780283292584069,
+            "timestamp": 1.8352824965748171
+        },
+        {
+            "x": 1.3321385383605957,
+            "y": 5.52810525894165,
+            "heading": -3.1411853071795863,
+            "angularVelocity": 1.0972891859651771e-35,
+            "velocityX": 2.010813957988482e-36,
+            "velocityY": -7.697026441410569e-35,
+            "timestamp": 1.877579770643261
+        },
+        {
+            "x": 1.337370959637994,
+            "y": 5.5282378526253355,
+            "heading": -3.1057781238593085,
+            "angularVelocity": 1.0527517345397082,
+            "velocityX": 0.1555740970920068,
+            "velocityY": 0.003942370372304683,
+            "timestamp": 1.9112127556658494
+        },
+        {
+            "x": 1.3479554232342674,
+            "y": 5.5287097500206475,
+            "heading": -3.0355501553802027,
+            "angularVelocity": 2.0880682589410555,
+            "velocityX": 0.314704852666657,
+            "velocityY": 0.014030791349476537,
+            "timestamp": 1.9448457406884376
+        },
+        {
+            "x": 1.364027106419543,
+            "y": 5.529829382282991,
+            "heading": -2.9313355116530078,
+            "angularVelocity": 3.0985844300529233,
+            "velocityX": 0.4778547956561592,
+            "velocityY": 0.03328970835004707,
+            "timestamp": 1.978478725711026
+        },
+        {
+            "x": 1.3857940218160676,
+            "y": 5.531991335292171,
+            "heading": -2.7946612849381784,
+            "angularVelocity": 4.06369600031153,
+            "velocityX": 0.6471895189173892,
+            "velocityY": 0.06428073534739039,
+            "timestamp": 2.012111710733614
+        },
+        {
+            "x": 1.4139333100804088,
+            "y": 5.535564467259386,
+            "heading": -2.6300339585531556,
+            "angularVelocity": 4.89481758084981,
+            "velocityX": 0.8366574731752962,
+            "velocityY": 0.10623891887132897,
+            "timestamp": 2.0457446957562024
+        },
+        {
+            "x": 1.4504189067700664,
+            "y": 5.5389912877920775,
+            "heading": -2.4501533377113245,
+            "angularVelocity": 5.348339456667967,
+            "velocityX": 1.0848158932415233,
+            "velocityY": 0.10188868250590417,
+            "timestamp": 2.0793776807787907
+        },
+        {
+            "x": 1.4957426020786804,
+            "y": 5.541383162087915,
+            "heading": -2.2591276091627437,
+            "angularVelocity": 5.679713781583368,
+            "velocityX": 1.3475965715851275,
+            "velocityY": 0.07111691972124916,
+            "timestamp": 2.113010665801379
+        },
+        {
+            "x": 1.549887277450098,
+            "y": 5.541604080123591,
+            "heading": -2.077370178961034,
+            "angularVelocity": 5.404142096802884,
+            "velocityX": 1.6098682687562116,
+            "velocityY": 0.006568493267186675,
+            "timestamp": 2.1466436508239672
+        },
+        {
+            "x": 1.612403659002375,
+            "y": 5.539682953328213,
+            "heading": -1.9100118188035742,
+            "angularVelocity": 4.9760186330490965,
+            "velocityX": 1.8587818330811392,
+            "velocityY": -0.05712031786913091,
+            "timestamp": 2.1802766358465555
+        },
+        {
+            "x": 1.6835678333886066,
+            "y": 5.53654594415181,
+            "heading": -1.7561585727792173,
+            "angularVelocity": 4.5744749067330135,
+            "velocityX": 2.1159042035203575,
+            "velocityY": -0.0932718036860474,
+            "timestamp": 2.213909620869144
+        },
+        {
+            "x": 1.7632443244228584,
+            "y": 5.534877661418315,
+            "heading": -1.616595649842294,
+            "angularVelocity": 4.149584785388266,
+            "velocityX": 2.3689984989658393,
+            "velocityY": -0.04960257712408059,
+            "timestamp": 2.247542605891732
+        },
+        {
+            "x": 1.845517380604842,
+            "y": 5.542129045770498,
+            "heading": -1.4828842082324165,
+            "angularVelocity": 3.9756043515042103,
+            "velocityX": 2.4462014337035023,
+            "velocityY": 0.21560335329479804,
+            "timestamp": 2.2811755909143203
+        },
+        {
+            "x": 1.9198078929781421,
+            "y": 5.552688491598465,
+            "heading": -1.3358148203184805,
+            "angularVelocity": 4.372772378519569,
+            "velocityX": 2.208858723761989,
+            "velocityY": 0.31396100646060415,
+            "timestamp": 2.3148085759369086
+        },
+        {
+            "x": 1.9854932228404973,
+            "y": 5.561629411594934,
+            "heading": -1.1756047034069004,
+            "angularVelocity": 4.763481945000775,
+            "velocityX": 1.9530032739657306,
+            "velocityY": 0.26583783718469556,
+            "timestamp": 2.348441560959497
+        },
+        {
+            "x": 2.0429500865174064,
+            "y": 5.56779505699511,
+            "heading": -1.0015843737719388,
+            "angularVelocity": 5.1740970811270035,
+            "velocityX": 1.7083486237787309,
+            "velocityY": 0.18332138512343155,
+            "timestamp": 2.382074545982085
+        },
+        {
+            "x": 2.0926336267477486,
+            "y": 5.570712245735625,
+            "heading": -0.8117369336213589,
+            "angularVelocity": 5.644680066996039,
+            "velocityX": 1.477226603495769,
+            "velocityY": 0.08673594504191894,
+            "timestamp": 2.4157075310046734
+        },
+        {
+            "x": 2.1331736827499013,
+            "y": 5.571318223980459,
+            "heading": -0.6223345825199257,
+            "angularVelocity": 5.63144636059598,
+            "velocityX": 1.2053659814888902,
+            "velocityY": 0.01801737920159694,
+            "timestamp": 2.4493405160272617
+        },
+        {
+            "x": 2.1650842656051874,
+            "y": 5.570653927593427,
+            "heading": -0.4456863837750604,
+            "angularVelocity": 5.252230767689132,
+            "velocityX": 0.9487883051074633,
+            "velocityY": -0.01975133597523799,
+            "timestamp": 2.48297350104985
+        },
+        {
+            "x": 2.1896037980440752,
+            "y": 5.569969320542384,
+            "heading": -0.29224766364008553,
+            "angularVelocity": 4.56214992608965,
+            "velocityX": 0.7290323003569462,
+            "velocityY": -0.020355227184960373,
+            "timestamp": 2.516606486072438
+        },
+        {
+            "x": 2.20780773019257,
+            "y": 5.569409848980837,
+            "heading": -0.16876882632138132,
+            "angularVelocity": 3.6713612317127033,
+            "velocityX": 0.5412523490337908,
+            "velocityY": -0.016634609184155776,
+            "timestamp": 2.5502394710950265
+        },
+        {
+            "x": 2.220213327560089,
+            "y": 5.568812984785934,
+            "heading": -0.07803807473080998,
+            "angularVelocity": 2.697671691336225,
+            "velocityX": 0.368852106323223,
+            "velocityY": -0.017746393741217954,
+            "timestamp": 2.5838724561176147
+        },
+        {
+            "x": 2.227117258661317,
+            "y": 5.568051315056596,
+            "heading": -0.021486347391680306,
+            "angularVelocity": 1.6814364618885036,
+            "velocityX": 0.2052726243772784,
+            "velocityY": -0.022646509931434324,
+            "timestamp": 2.617505441140203
+        },
+        {
+            "x": 2.2287168502807617,
+            "y": 5.567087173461914,
+            "heading": 6.38149991346766e-37,
+            "angularVelocity": 0.6388474700431709,
+            "velocityX": 0.047560203721742585,
+            "velocityY": -0.02866654845040371,
+            "timestamp": 2.6511384261627913
+        },
+        {
+            "x": 2.2247758271699722,
+            "y": 5.5659373744789455,
+            "heading": -0.016384469566950412,
+            "angularVelocity": -0.4672993433012807,
+            "velocityX": -0.11240141184197158,
+            "velocityY": -0.03279326849576676,
+            "timestamp": 2.686200468945049
+        },
+        {
+            "x": 2.215132755191586,
+            "y": 5.564807940629173,
+            "heading": -0.07111277017786721,
+            "angularVelocity": -1.560898803038675,
+            "velocityX": -0.27502881216225394,
+            "velocityY": -0.03221243715853021,
+            "timestamp": 2.7212625117273066
+        },
+        {
+            "x": 2.199681422128445,
+            "y": 5.563984363308005,
+            "heading": -0.16355266498461823,
+            "angularVelocity": -2.6364663171744263,
+            "velocityX": -0.4406854774291637,
+            "velocityY": -0.023489142554608954,
+            "timestamp": 2.7563245545095643
+        },
+        {
+            "x": 2.178275880729617,
+            "y": 5.563860339253889,
+            "heading": -0.2925810137194886,
+            "angularVelocity": -3.6800008925938044,
+            "velocityX": -0.6105046854160964,
+            "velocityY": -0.0035372740511740697,
+            "timestamp": 2.791386597291822
+        },
+        {
+            "x": 2.150434792823077,
+            "y": 5.56488104971689,
+            "heading": -0.45485951159806126,
+            "angularVelocity": -4.628324107820951,
+            "velocityX": -0.7940520773258658,
+            "velocityY": 0.029111551467175232,
+            "timestamp": 2.8264486400740796
+        },
+        {
+            "x": 2.1136927979170794,
+            "y": 5.564806154665508,
+            "heading": -0.6342616510080993,
+            "angularVelocity": -5.116705279386068,
+            "velocityX": -1.047913697846207,
+            "velocityY": -0.0021360721007366726,
+            "timestamp": 2.8615106828563373
+        },
+        {
+            "x": 2.0677952991091333,
+            "y": 5.562722960469238,
+            "heading": -0.8285761603492735,
+            "angularVelocity": -5.542019058841107,
+            "velocityX": -1.3090366437853844,
+            "velocityY": -0.05941451298794179,
+            "timestamp": 2.896572725638595
+        },
+        {
+            "x": 2.0120787764368706,
+            "y": 5.557776466918295,
+            "heading": -1.020468287532569,
+            "angularVelocity": -5.472930609747536,
+            "velocityX": -1.5890837569925207,
+            "velocityY": -0.14107830458316797,
+            "timestamp": 2.9316347684208526
+        },
+        {
+            "x": 1.947346732087487,
+            "y": 5.549990941392819,
+            "heading": -1.1972983758508349,
+            "angularVelocity": -5.043348141932757,
+            "velocityX": -1.8462142879518657,
+            "velocityY": -0.22204996935933435,
+            "timestamp": 2.9666968112031102
+        },
+        {
+            "x": 1.8731816133340355,
+            "y": 5.541067205657786,
+            "heading": -1.3595832736612468,
+            "angularVelocity": -4.628506639451492,
+            "velocityX": -2.1152537863818077,
+            "velocityY": -0.25451271594333,
+            "timestamp": 3.001758853985368
+        },
+        {
+            "x": 1.7903438797008602,
+            "y": 5.535778355661316,
+            "heading": -1.5070368489944903,
+            "angularVelocity": -4.2055044039778195,
+            "velocityX": -2.362604316799635,
+            "velocityY": -0.15084260860996357,
+            "timestamp": 3.0368208967676256
+        },
+        {
+            "x": 1.705207783905628,
+            "y": 5.540259742780199,
+            "heading": -1.6469795594362229,
+            "angularVelocity": -3.991287995134942,
+            "velocityX": -2.4281556075880766,
+            "velocityY": 0.12781306402233675,
+            "timestamp": 3.0718829395498832
+        },
+        {
+            "x": 1.629345764073995,
+            "y": 5.546176174189709,
+            "heading": -1.802120558201536,
+            "angularVelocity": -4.424756416184017,
+            "velocityX": -2.1636508831716217,
+            "velocityY": 0.16874177714778527,
+            "timestamp": 3.106944982332141
+        },
+        {
+            "x": 1.5628215543297233,
+            "y": 5.550494777199676,
+            "heading": -1.9719374235468123,
+            "angularVelocity": -4.843324914063712,
+            "velocityX": -1.8973284060315727,
+            "velocityY": 0.12317031944734674,
+            "timestamp": 3.1420070251143986
+        },
+        {
+            "x": 1.5052956910676978,
+            "y": 5.552468250746859,
+            "heading": -2.1578472190724005,
+            "angularVelocity": -5.302309300120524,
+            "velocityX": -1.6406877265900472,
+            "velocityY": 0.056285184506758255,
+            "timestamp": 3.177069067896656
+        },
+        {
+            "x": 1.4562451550739939,
+            "y": 5.551766768072451,
+            "heading": -2.3624196209535056,
+            "angularVelocity": -5.834583088941523,
+            "velocityX": -1.3989640106914938,
+            "velocityY": -0.02000689688175958,
+            "timestamp": 3.212131110678914
+        },
+        {
+            "x": 1.4173942622657685,
+            "y": 5.550106751106204,
+            "heading": -2.5657566721184293,
+            "angularVelocity": -5.799349810497013,
+            "velocityX": -1.1080613029165842,
+            "velocityY": -0.047345129790485885,
+            "timestamp": 3.2471931534611715
+        },
+        {
+            "x": 1.3874992845964569,
+            "y": 5.548698391964879,
+            "heading": -2.750369265628105,
+            "angularVelocity": -5.26531196873374,
+            "velocityX": -0.852630802345583,
+            "velocityY": -0.0401676294239809,
+            "timestamp": 3.282255196243429
+        },
+        {
+            "x": 1.3647199881577643,
+            "y": 5.548009174125998,
+            "heading": -2.903871904539791,
+            "angularVelocity": -4.378028966109259,
+            "velocityX": -0.6496853757254475,
+            "velocityY": -0.019657093089571077,
+            "timestamp": 3.317317239025687
+        },
+        {
+            "x": 1.3481785543906255,
+            "y": 5.547743770300603,
+            "heading": -3.0214740080658498,
+            "angularVelocity": -3.354114426714698,
+            "velocityX": -0.4717760990101023,
+            "velocityY": -0.007569548273095167,
+            "timestamp": 3.3523792818079445
+        },
+        {
+            "x": 1.3374168488281284,
+            "y": 5.547647789865521,
+            "heading": -3.10100248389671,
+            "angularVelocity": -2.268221401837531,
+            "velocityX": -0.30693321633680876,
+            "velocityY": -0.0027374456097331258,
+            "timestamp": 3.387441324590202
+        },
+        {
+            "x": 1.3321385383605957,
+            "y": 5.547596454620361,
+            "heading": -3.1411853071795863,
+            "angularVelocity": -1.1460491201958698,
+            "velocityX": -0.15054201206449694,
+            "velocityY": -0.001464125906141093,
+            "timestamp": 3.42250336737246
+        },
+        {
+            "x": 1.3321385383605957,
+            "y": 5.547596454620361,
+            "heading": -3.1411853071795863,
+            "angularVelocity": -1.1877010663188592e-33,
+            "velocityX": -2.3472123166291743e-34,
+            "velocityY": -4.828629969149868e-35,
+            "timestamp": 3.4575654101547175
+        },
+        {
+            "x": 1.3353773426968816,
+            "y": 5.556976212978418,
+            "heading": -3.1027759330153977,
+            "angularVelocity": 0.9418411865422114,
+            "velocityX": 0.07941913622507528,
+            "velocityY": 0.2300022568362304,
+            "timestamp": 3.4983465680749837
+        },
+        {
+            "x": 1.342768335196551,
+            "y": 5.576502155287014,
+            "heading": -3.0330086785474775,
+            "angularVelocity": 1.7107717883912714,
+            "velocityX": 0.18123547433645554,
+            "velocityY": 0.47879813385321396,
+            "timestamp": 3.53912772599525
+        },
+        {
+            "x": 1.357543783583743,
+            "y": 5.605477021924564,
+            "heading": -2.939156767804727,
+            "angularVelocity": 2.301354731669143,
+            "velocityX": 0.3623106635687124,
+            "velocityY": 0.7104964183263536,
+            "timestamp": 3.579908883915516
+        },
+        {
+            "x": 1.3800861680237329,
+            "y": 5.643206466545064,
+            "heading": -2.8183134070613347,
+            "angularVelocity": 2.9632155364411483,
+            "velocityX": 0.5527646979535052,
+            "velocityY": 0.9251685470595896,
+            "timestamp": 3.620690041835782
+        },
+        {
+            "x": 1.410132685953878,
+            "y": 5.690126983565226,
+            "heading": -2.6706561536339386,
+            "angularVelocity": 3.6207224354956073,
+            "velocityX": 0.7367745170181611,
+            "velocityY": 1.1505440113274845,
+            "timestamp": 3.6614711997560483
+        },
+        {
+            "x": 1.447426110042683,
+            "y": 5.747148126734536,
+            "heading": -2.4998618512700794,
+            "angularVelocity": 4.188068977781135,
+            "velocityX": 0.9144768317201708,
+            "velocityY": 1.3982227596576835,
+            "timestamp": 3.7022523576763144
+        },
+        {
+            "x": 1.4919514039971702,
+            "y": 5.815443990853687,
+            "heading": -2.3159167566388894,
+            "angularVelocity": 4.510541240413857,
+            "velocityX": 1.0918104395549988,
+            "velocityY": 1.6746916370712401,
+            "timestamp": 3.7430335155965806
+        },
+        {
+            "x": 1.543306542439651,
+            "y": 5.895557258888599,
+            "heading": -2.140840879075951,
+            "angularVelocity": 4.293058031977455,
+            "velocityX": 1.259285931578716,
+            "velocityY": 1.9644677130440258,
+            "timestamp": 3.7838146735168467
+        },
+        {
+            "x": 1.5991978574253154,
+            "y": 5.9852989254543125,
+            "heading": -2.0026162516135204,
+            "angularVelocity": 3.389423805294663,
+            "velocityX": 1.3705180979642946,
+            "velocityY": 2.2005669074226377,
+            "timestamp": 3.824595831437113
+        },
+        {
+            "x": 1.6602838186413746,
+            "y": 6.084712915583529,
+            "heading": -1.8987034819220077,
+            "angularVelocity": 2.5480583433820057,
+            "velocityX": 1.497896683941456,
+            "velocityY": 2.4377431931576705,
+            "timestamp": 3.865376989357379
+        },
+        {
+            "x": 1.7272241659282512,
+            "y": 6.1936323149644315,
+            "heading": -1.8270894144503607,
+            "angularVelocity": 1.7560577267488233,
+            "velocityX": 1.6414528350998836,
+            "velocityY": 2.670826551660404,
+            "timestamp": 3.906158147277645
+        },
+        {
+            "x": 1.8006198616667228,
+            "y": 6.311769110233653,
+            "heading": -1.7863461946081067,
+            "angularVelocity": 0.9990697155268056,
+            "velocityX": 1.7997452618185266,
+            "velocityY": 2.8968474975673533,
+            "timestamp": 3.9469393051979114
+        },
+        {
+            "x": 1.8749454151333007,
+            "y": 6.423903525675366,
+            "heading": -1.7351071665224345,
+            "angularVelocity": 1.2564387746383576,
+            "velocityX": 1.8225464223428127,
+            "velocityY": 2.7496623725337597,
+            "timestamp": 3.9877204631181775
+        },
+        {
+            "x": 1.9436310828301862,
+            "y": 6.525803671980358,
+            "heading": -1.6558915421182487,
+            "angularVelocity": 1.9424564785302534,
+            "velocityX": 1.6842500605592803,
+            "velocityY": 2.4987065473772168,
+            "timestamp": 4.028501621038443
+        },
+        {
+            "x": 2.0070825887496224,
+            "y": 6.617181897860731,
+            "heading": -1.5496488374577189,
+            "angularVelocity": 2.6051909773688204,
+            "velocityX": 1.5559025087883662,
+            "velocityY": 2.240697188123809,
+            "timestamp": 4.069282778958709
+        },
+        {
+            "x": 2.0657895498392027,
+            "y": 6.697945113539011,
+            "heading": -1.416445908140072,
+            "angularVelocity": 3.2662861014903286,
+            "velocityX": 1.4395609169401762,
+            "velocityY": 1.9804051625063046,
+            "timestamp": 4.1100639368789755
+        },
+        {
+            "x": 2.1168235029971925,
+            "y": 6.770262031139991,
+            "heading": -1.2545233571884964,
+            "angularVelocity": 3.9705236243698976,
+            "velocityX": 1.2514101060536296,
+            "velocityY": 1.7732924048496255,
+            "timestamp": 4.150845094799242
+        },
+        {
+            "x": 2.160098891028687,
+            "y": 6.834337445285054,
+            "heading": -1.0613678161788878,
+            "angularVelocity": 4.736391776497875,
+            "velocityX": 1.0611613362255508,
+            "velocityY": 1.571201442350923,
+            "timestamp": 4.191626252719508
+        },
+        {
+            "x": 2.1954531720383796,
+            "y": 6.890659821255424,
+            "heading": -0.8342118106482137,
+            "angularVelocity": 5.570121524621797,
+            "velocityX": 0.8669268557507981,
+            "velocityY": 1.3810881996163518,
+            "timestamp": 4.232407410639774
+        },
+        {
+            "x": 2.2216225780243426,
+            "y": 6.9367359795772305,
+            "heading": -0.6132278215539244,
+            "angularVelocity": 5.418776718560821,
+            "velocityX": 0.6417033581324141,
+            "velocityY": 1.1298393834695264,
+            "timestamp": 4.27318856856004
+        },
+        {
+            "x": 2.2400584030513464,
+            "y": 6.973023548052357,
+            "heading": -0.41519671142857095,
+            "angularVelocity": 4.855946231652809,
+            "velocityX": 0.4520672282785309,
+            "velocityY": 0.889812117303642,
+            "timestamp": 4.313969726480306
+        },
+        {
+            "x": 2.252392360052711,
+            "y": 6.999798504355172,
+            "heading": -0.24838728557433673,
+            "angularVelocity": 4.0903553101747345,
+            "velocityX": 0.3024425403878878,
+            "velocityY": 0.6565521350611045,
+            "timestamp": 4.3547508844005725
+        },
+        {
+            "x": 2.2601470415559044,
+            "y": 7.01732009026677,
+            "heading": -0.11829622013640562,
+            "angularVelocity": 3.189979688469885,
+            "velocityX": 0.19015353900334692,
+            "velocityY": 0.4296490537580049,
+            "timestamp": 4.395532042320839
+        },
+        {
+            "x": 2.2649306384373356,
+            "y": 7.026470873344902,
+            "heading": -0.032599297392582494,
+            "angularVelocity": 2.101385225779393,
+            "velocityX": 0.11729919220988534,
+            "velocityY": 0.2243875246510465,
+            "timestamp": 4.436313200241105
+        },
+        {
+            "x": 2.2676985263824463,
+            "y": 7.028899669647217,
+            "heading": 1.4623510884357687e-33,
+            "angularVelocity": 0.799371549388559,
+            "velocityX": 0.06787173504299782,
+            "velocityY": 0.05955682541099079,
+            "timestamp": 4.477094358161371
+        },
+        {
+            "x": 2.268447419698579,
+            "y": 7.024856278106883,
+            "heading": -0.027481442707952487,
+            "angularVelocity": -0.643111881448138,
+            "velocityX": 0.017525360464520197,
+            "velocityY": -0.09462214806441192,
+            "timestamp": 4.5198263360655435
+        },
+        {
+            "x": 2.2661447146026696,
+            "y": 7.012526995839087,
+            "heading": -0.10890493556394686,
+            "angularVelocity": -1.9054463858094515,
+            "velocityX": -0.05388716387229158,
+            "velocityY": -0.28852589729042577,
+            "timestamp": 4.562558313969716
+        },
+        {
+            "x": 2.259237652200008,
+            "y": 6.990956553960971,
+            "heading": -0.23735745507492811,
+            "angularVelocity": -3.0060045383117964,
+            "velocityX": -0.16163685233926556,
+            "velocityY": -0.504784541602271,
+            "timestamp": 4.605290291873889
+        },
+        {
+            "x": 2.2461382818263638,
+            "y": 6.959794967044901,
+            "heading": -0.4073842771005642,
+            "angularVelocity": -3.9789129912714607,
+            "velocityX": -0.30654725140549266,
+            "velocityY": -0.7292334323009891,
+            "timestamp": 4.648022269778061
+        },
+        {
+            "x": 2.225216379141952,
+            "y": 6.918625356851511,
+            "heading": -0.6110307557476551,
+            "angularVelocity": -4.765669380054774,
+            "velocityX": -0.48960763602681573,
+            "velocityY": -0.9634379734473075,
+            "timestamp": 4.690754247682234
+        },
+        {
+            "x": 2.1950172148844427,
+            "y": 6.866427702804465,
+            "heading": -0.8329684676174923,
+            "angularVelocity": -5.193714935628281,
+            "velocityX": -0.706711126857547,
+            "velocityY": -1.2215127079794852,
+            "timestamp": 4.7334862255864065
+        },
+        {
+            "x": 2.1547332634682537,
+            "y": 6.802939586567042,
+            "heading": -1.0490897028487878,
+            "angularVelocity": -5.057599620498584,
+            "velocityX": -0.9427120716604103,
+            "velocityY": -1.4857284720074833,
+            "timestamp": 4.776218203490579
+        },
+        {
+            "x": 2.1052130726477585,
+            "y": 6.731031156240311,
+            "heading": -1.2322360836735529,
+            "angularVelocity": -4.285932685715518,
+            "velocityX": -1.1588555748939446,
+            "velocityY": -1.6827779535033027,
+            "timestamp": 4.818950181394752
+        },
+        {
+            "x": 2.046678257521749,
+            "y": 6.650499615743051,
+            "heading": -1.3842475177981566,
+            "angularVelocity": -3.557322679177027,
+            "velocityX": -1.3698129128793415,
+            "velocityY": -1.8845732036521736,
+            "timestamp": 4.861682159298924
+        },
+        {
+            "x": 1.9824374188049456,
+            "y": 6.55859622988028,
+            "heading": -1.505962481707897,
+            "angularVelocity": -2.848334429608893,
+            "velocityX": -1.5033434413184743,
+            "velocityY": -2.150693470563594,
+            "timestamp": 4.904414137203097
+        },
+        {
+            "x": 1.9120980242519563,
+            "y": 6.455385209029271,
+            "heading": -1.5975173206853661,
+            "angularVelocity": -2.1425368884815836,
+            "velocityX": -1.6460598830863262,
+            "velocityY": -2.415311106882586,
+            "timestamp": 4.9471461151072695
+        },
+        {
+            "x": 1.835418260391466,
+            "y": 6.341055354736124,
+            "heading": -1.6582681711046532,
+            "angularVelocity": -1.4216718579121885,
+            "velocityX": -1.7944351659182955,
+            "velocityY": -2.675510470157384,
+            "timestamp": 4.989878093011442
+        },
+        {
+            "x": 1.7526348309039128,
+            "y": 6.215560469564215,
+            "heading": -1.6873132093111056,
+            "angularVelocity": -0.6797026402940408,
+            "velocityX": -1.937271185368423,
+            "velocityY": -2.9367909309822835,
+            "timestamp": 5.032610070915615
+        },
+        {
+            "x": 1.677441366215122,
+            "y": 6.099853521630283,
+            "heading": -1.7491066585439166,
+            "angularVelocity": -1.4460704199413479,
+            "velocityX": -1.7596532708458867,
+            "velocityY": -2.70773677252684,
+            "timestamp": 5.075342048819787
+        },
+        {
+            "x": 1.6091252487579695,
+            "y": 5.994372460119343,
+            "heading": -1.8457597622108368,
+            "angularVelocity": -2.2618448386280567,
+            "velocityX": -1.598711803379506,
+            "velocityY": -2.4684338681322977,
+            "timestamp": 5.11807402672396
+        },
+        {
+            "x": 1.547030840271468,
+            "y": 5.899198607270202,
+            "heading": -1.9803343034636756,
+            "angularVelocity": -3.149270121655174,
+            "velocityX": -1.4531133715773699,
+            "velocityY": -2.2272278868666358,
+            "timestamp": 5.1608060046281325
+        },
+        {
+            "x": 1.4904311214521733,
+            "y": 5.813988086882659,
+            "heading": -2.1571135876817413,
+            "angularVelocity": -4.136931939225903,
+            "velocityX": -1.3245284116317142,
+            "velocityY": -1.9940691858127988,
+            "timestamp": 5.203537982532305
+        },
+        {
+            "x": 1.4423633386018753,
+            "y": 5.740159267538946,
+            "heading": -2.35427402869071,
+            "angularVelocity": -4.613885213811227,
+            "velocityX": -1.1248667908162702,
+            "velocityY": -1.7277182794879316,
+            "timestamp": 5.246269960436478
+        },
+        {
+            "x": 1.4038900802302197,
+            "y": 5.6781134856885735,
+            "heading": -2.5480008926422313,
+            "angularVelocity": -4.533533748097493,
+            "velocityX": -0.9003388155337229,
+            "velocityY": -1.451975426681909,
+            "timestamp": 5.28900193834065
+        },
+        {
+            "x": 1.3744454738206087,
+            "y": 5.627099122367314,
+            "heading": -2.7223828634703597,
+            "angularVelocity": -4.0808307824923435,
+            "velocityX": -0.6890532068429254,
+            "velocityY": -1.1938217190802813,
+            "timestamp": 5.331733916244823
+        },
+        {
+            "x": 1.3538849352129523,
+            "y": 5.586407689469935,
+            "heading": -2.872592229193493,
+            "angularVelocity": -3.5151512541727503,
+            "velocityX": -0.4811511101537121,
+            "velocityY": -0.9522478221960695,
+            "timestamp": 5.374465894148996
+        },
+        {
+            "x": 1.341639273384249,
+            "y": 5.556291662758613,
+            "heading": -2.9981488971519656,
+            "angularVelocity": -2.9382367518778816,
+            "velocityX": -0.2865690386755447,
+            "velocityY": -0.7047655687470963,
+            "timestamp": 5.417197872053168
+        },
+        {
+            "x": 1.3350584223241078,
+            "y": 5.537231311212578,
+            "heading": -3.091661499454674,
+            "angularVelocity": -2.18835183600471,
+            "velocityX": -0.15400295944407288,
+            "velocityY": -0.4460442151491003,
+            "timestamp": 5.459929849957341
+        },
+        {
+            "x": 1.332138180732727,
+            "y": 5.52810525894165,
+            "heading": -3.1411853071795863,
+            "angularVelocity": -1.1589402165275682,
+            "velocityX": -0.06833855427730316,
+            "velocityY": -0.21356493938549467,
+            "timestamp": 5.502661827861513
+        },
+        {
+            "x": 1.332138180732727,
+            "y": 5.52810525894165,
+            "heading": -3.1411853071795863,
+            "angularVelocity": 0,
+            "velocityX": 4.554923410438002e-36,
+            "velocityY": -5.3517654956150865e-34,
+            "timestamp": 5.545393805765686
+        },
+        {
+            "x": 1.3511823216382217,
+            "y": 5.531961266421083,
+            "heading": -3.038798036456765,
+            "angularVelocity": 1.6934181625027336,
+            "velocityX": 0.3149775735885273,
+            "velocityY": 0.06377582930300447,
+            "timestamp": 5.60585570052386
+        },
+        {
+            "x": 1.3908490268299478,
+            "y": 5.542189708843604,
+            "heading": -2.844785530962286,
+            "angularVelocity": 3.2088393238494826,
+            "velocityX": 0.6560612324568816,
+            "velocityY": 0.16917171490292857,
+            "timestamp": 5.666317595282035
+        },
+        {
+            "x": 1.4539728254946123,
+            "y": 5.56127979063472,
+            "heading": -2.581938269596155,
+            "angularVelocity": 4.34732094350375,
+            "velocityX": 1.0440261410453116,
+            "velocityY": 0.3157374056415134,
+            "timestamp": 5.726779490040209
+        },
+        {
+            "x": 1.5450944631009007,
+            "y": 5.588995277628573,
+            "heading": -2.2890493808444536,
+            "angularVelocity": 4.8441897152438,
+            "velocityX": 1.5070919952267785,
+            "velocityY": 0.45839593854450544,
+            "timestamp": 5.787241384798383
+        },
+        {
+            "x": 1.6638598735645596,
+            "y": 5.62434991056183,
+            "heading": -2.0324634607383167,
+            "angularVelocity": 4.24376247440454,
+            "velocityX": 1.9643018290888397,
+            "velocityY": 0.5847423914626325,
+            "timestamp": 5.8477032795565576
+        },
+        {
+            "x": 1.8083021892177116,
+            "y": 5.6638944259582,
+            "heading": -1.8350759652142852,
+            "angularVelocity": 3.264659440685901,
+            "velocityX": 2.3889809644714033,
+            "velocityY": 0.6540402935523977,
+            "timestamp": 5.908165174314732
+        },
+        {
+            "x": 1.978235650109889,
+            "y": 5.709112472816866,
+            "heading": -1.6974261571036708,
+            "angularVelocity": 2.276637354174287,
+            "velocityX": 2.8105877523661738,
+            "velocityY": 0.7478767749426485,
+            "timestamp": 5.968627069072906
+        },
+        {
+            "x": 2.1727729031197174,
+            "y": 5.761282618111588,
+            "heading": -1.6235540862695264,
+            "angularVelocity": 1.2217954982986527,
+            "velocityX": 3.217518302856812,
+            "velocityY": 0.8628599137255631,
+            "timestamp": 6.0290889638310805
+        },
+        {
+            "x": 2.3909229420587215,
+            "y": 5.821425961949498,
+            "heading": -1.6168784459669248,
+            "angularVelocity": 0.1104107029609625,
+            "velocityX": 3.60805826234069,
+            "velocityY": 0.994731377150211,
+            "timestamp": 6.089550858589255
+        },
+        {
+            "x": 2.6085958376613507,
+            "y": 5.891661539060254,
+            "heading": -1.616878243648269,
+            "angularVelocity": 0.0000033462175924113523,
+            "velocityX": 3.6001666251651883,
+            "velocityY": 1.1616502822426138,
+            "timestamp": 6.150012753347429
+        },
+        {
+            "x": 2.826268683292055,
+            "y": 5.961897271043331,
+            "heading": -1.6168780413299741,
+            "angularVelocity": 0.0000033462116195770168,
+            "velocityX": 3.6001657986623896,
+            "velocityY": 1.1616528437290037,
+            "timestamp": 6.2104746481056035
+        },
+        {
+            "x": 3.0439415289220437,
+            "y": 6.032133003028614,
+            "heading": -1.6168778390116512,
+            "angularVelocity": 0.0000033462120828803855,
+            "velocityX": 3.6001657986505493,
+            "velocityY": 1.1616528437654758,
+            "timestamp": 6.270936542863778
+        },
+        {
+            "x": 3.2616143745520296,
+            "y": 6.102368735013889,
+            "heading": -1.6168776366933004,
+            "angularVelocity": 0.000003346212545975565,
+            "velocityX": 3.600165798650516,
+            "velocityY": 1.1616528437653553,
+            "timestamp": 6.331398437621952
+        },
+        {
+            "x": 3.479287220182014,
+            "y": 6.172604466999156,
+            "heading": -1.6168774343749215,
+            "angularVelocity": 0.0000033462130098357135,
+            "velocityX": 3.6001657986504836,
+            "velocityY": 1.1616528437652343,
+            "timestamp": 6.391860332380126
+        },
+        {
+            "x": 3.6969600658119965,
+            "y": 6.242840198984417,
+            "heading": -1.6168772320565146,
+            "angularVelocity": 0.000003346213473053834,
+            "velocityX": 3.6001657986504503,
+            "velocityY": 1.1616528437651126,
+            "timestamp": 6.452322227138301
+        },
+        {
+            "x": 3.914632911441977,
+            "y": 6.313075930969669,
+            "heading": -1.61687702973808,
+            "angularVelocity": 0.000003346213936555426,
+            "velocityX": 3.6001657986504174,
+            "velocityY": 1.1616528437649918,
+            "timestamp": 6.512784121896475
+        },
+        {
+            "x": 4.132305757071955,
+            "y": 6.383311662954915,
+            "heading": -1.616876827419617,
+            "angularVelocity": 0.0000033462143992467285,
+            "velocityX": 3.6001657986503846,
+            "velocityY": 1.161652843764871,
+            "timestamp": 6.573246016654649
+        },
+        {
+            "x": 4.349978602701932,
+            "y": 6.453547394940153,
+            "heading": -1.6168766251011262,
+            "angularVelocity": 0.000003346214863174556,
+            "velocityX": 3.600165798650352,
+            "velocityY": 1.1616528437647493,
+            "timestamp": 6.633707911412824
+        },
+        {
+            "x": 4.567651448331906,
+            "y": 6.523783126925384,
+            "heading": -1.6168764227826073,
+            "angularVelocity": 0.000003346215326740712,
+            "velocityX": 3.6001657986503193,
+            "velocityY": 1.1616528437646283,
+            "timestamp": 6.694169806170998
+        },
+        {
+            "x": 4.785324293961879,
+            "y": 6.594018858910608,
+            "heading": -1.6168762204640603,
+            "angularVelocity": 0.0000033462157905377784,
+            "velocityX": 3.6001657986502864,
+            "velocityY": 1.161652843764507,
+            "timestamp": 6.754631700929172
+        },
+        {
+            "x": 5.002997139591849,
+            "y": 6.6642545908958235,
+            "heading": -1.6168760181454853,
+            "angularVelocity": 0.0000033462162531242337,
+            "velocityX": 3.600165798650253,
+            "velocityY": 1.1616528437643858,
+            "timestamp": 6.815093595687347
+        },
+        {
+            "x": 5.220669985221817,
+            "y": 6.734490322881032,
+            "heading": -1.6168758158268823,
+            "angularVelocity": 0.000003346216717473229,
+            "velocityX": 3.6001657986502207,
+            "velocityY": 1.1616528437642646,
+            "timestamp": 6.875555490445521
+        },
+        {
+            "x": 5.438342830851783,
+            "y": 6.804726054866234,
+            "heading": -1.6168756135082514,
+            "angularVelocity": 0.000003346217180660181,
+            "velocityX": 3.600165798650188,
+            "velocityY": 1.1616528437641434,
+            "timestamp": 6.936017385203695
+        },
+        {
+            "x": 5.656015676481748,
+            "y": 6.874961786851428,
+            "heading": -1.6168754111895922,
+            "angularVelocity": 0.0000033462176438124147,
+            "velocityX": 3.600165798650156,
+            "velocityY": 1.1616528437640228,
+            "timestamp": 6.99647927996187
+        },
+        {
+            "x": 5.873688522111711,
+            "y": 6.945197518836615,
+            "heading": -1.6168752088709053,
+            "angularVelocity": 0.0000033462181070326184,
+            "velocityX": 3.6001657986501225,
+            "velocityY": 1.161652843763901,
+            "timestamp": 7.056941174720044
+        },
+        {
+            "x": 6.091361367741672,
+            "y": 7.015433250821794,
+            "heading": -1.6168750065521902,
+            "angularVelocity": 0.0000033462185712785595,
+            "velocityX": 3.6001657986500892,
+            "velocityY": 1.1616528437637803,
+            "timestamp": 7.117403069478218
+        },
+        {
+            "x": 6.30903421337163,
+            "y": 7.0856689828069666,
+            "heading": -1.616874804233447,
+            "angularVelocity": 0.0000033462190342827524,
+            "velocityX": 3.600165798650057,
+            "velocityY": 1.1616528437636586,
+            "timestamp": 7.1778649642363925
+        },
+        {
+            "x": 6.526707059002331,
+            "y": 7.155904714789823,
+            "heading": -1.6168746019146758,
+            "angularVelocity": 0.0000033462194986739715,
+            "velocityX": 3.6001657986623425,
+            "velocityY": 1.1616528437253608,
+            "timestamp": 7.238326858994567
+        },
+        {
+            "x": 6.744379956765645,
+            "y": 7.22614028520157,
+            "heading": -1.6168743995934785,
+            "angularVelocity": 0.0000033462596273946,
+            "velocityX": 3.600166660901485,
+            "velocityY": 1.1616501714453904,
+            "timestamp": 7.298788753752741
+        },
+        {
+            "x": 6.948380246935694,
+            "y": 7.286306890939545,
+            "heading": -1.572684837353693,
+            "angularVelocity": 0.7308663153301367,
+            "velocityX": 3.37403071779302,
+            "velocityY": 0.9951161136881308,
+            "timestamp": 7.3592506485109155
+        },
+        {
+            "x": 7.128326014252639,
+            "y": 7.34087027160758,
+            "heading": -1.456360393496805,
+            "angularVelocity": 1.9239298457672127,
+            "velocityX": 2.976184719924214,
+            "velocityY": 0.9024424538177037,
+            "timestamp": 7.41971254326909
+        },
+        {
+            "x": 7.285215733567768,
+            "y": 7.391288256354559,
+            "heading": -1.2598604932432955,
+            "angularVelocity": 3.249979198294029,
+            "velocityX": 2.5948528398362565,
+            "velocityY": 0.8338803298942721,
+            "timestamp": 7.480174438027264
+        },
+        {
+            "x": 7.415669802167391,
+            "y": 7.433594551714448,
+            "heading": -1.014978314704827,
+            "angularVelocity": 4.050190281298798,
+            "velocityX": 2.157624552147962,
+            "velocityY": 0.6997183189362131,
+            "timestamp": 7.540636332785438
+        },
+        {
+            "x": 7.52162527778786,
+            "y": 7.463369081831009,
+            "heading": -0.7214685317692553,
+            "angularVelocity": 4.854458896954922,
+            "velocityX": 1.7524339262646644,
+            "velocityY": 0.49245115846350496,
+            "timestamp": 7.601098227543613
+        },
+        {
+            "x": 7.599835823210802,
+            "y": 7.482093660895173,
+            "heading": -0.44961761921846705,
+            "angularVelocity": 4.4962354163410385,
+            "velocityX": 1.293551016483298,
+            "velocityY": 0.30969223076874997,
+            "timestamp": 7.661560122301787
+        },
+        {
+            "x": 7.6517032735458015,
+            "y": 7.492160163258041,
+            "heading": -0.22315227789287845,
+            "angularVelocity": 3.7455878984832975,
+            "velocityX": 0.8578535380416231,
+            "velocityY": 0.16649333275330752,
+            "timestamp": 7.722022017059961
+        },
+        {
+            "x": 7.67973874704759,
+            "y": 7.4966228089126075,
+            "heading": -0.06798236605855336,
+            "angularVelocity": 2.5664083544676926,
+            "velocityX": 0.46368830507082526,
+            "velocityY": 0.07380922599953843,
+            "timestamp": 7.782483911818136
+        },
+        {
+            "x": 7.686149597167969,
+            "y": 7.496679306030273,
+            "heading": 0,
+            "angularVelocity": 1.1243836523889659,
+            "velocityX": 0.10603124738348535,
+            "velocityY": 0.0009344251927952234,
+            "timestamp": 7.84294580657631
+        },
+        {
+            "x": 7.671082178881759,
+            "y": 7.493118035056542,
+            "heading": -0.029242529216573936,
+            "angularVelocity": -0.471080674899979,
+            "velocityX": -0.24272762190641095,
+            "velocityY": -0.05737006951015711,
+            "timestamp": 7.905021223324796
+        },
+        {
+            "x": 7.633475390199317,
+            "y": 7.486720174316825,
+            "heading": -0.1516714089178372,
+            "angularVelocity": -1.9722602942371796,
+            "velocityX": -0.6058241837475781,
+            "velocityY": -0.1030659329383018,
+            "timestamp": 7.967096640073281
+        },
+        {
+            "x": 7.569735009380553,
+            "y": 7.475264417263962,
+            "heading": -0.33932636884787254,
+            "angularVelocity": -3.023015708301534,
+            "velocityX": -1.0268216333854858,
+            "velocityY": -0.1845457936960604,
+            "timestamp": 8.029172056821766
+        },
+        {
+            "x": 7.480376336028035,
+            "y": 7.452059634472421,
+            "heading": -0.582197860853168,
+            "angularVelocity": -3.912522939464948,
+            "velocityX": -1.4395178966671722,
+            "velocityY": -0.37381598073777733,
+            "timestamp": 8.09124747357025
+        },
+        {
+            "x": 7.362878563951424,
+            "y": 7.417380486057705,
+            "heading": -0.863364111736863,
+            "angularVelocity": -4.529429935571964,
+            "velocityX": -1.8928229278376825,
+            "velocityY": -0.5586615480203228,
+            "timestamp": 8.153322890318735
+        },
+        {
+            "x": 7.218864923659752,
+            "y": 7.37005336377927,
+            "heading": -1.0948198175456743,
+            "angularVelocity": -3.7286210537516657,
+            "velocityX": -2.319978629788013,
+            "velocityY": -0.7624132830262488,
+            "timestamp": 8.21539830706722
+        },
+        {
+            "x": 7.047313606455814,
+            "y": 7.316250565275482,
+            "heading": -1.2668868462563043,
+            "angularVelocity": -2.7719029162189006,
+            "velocityX": -2.7635950943192666,
+            "velocityY": -0.8667327796087775,
+            "timestamp": 8.277473723815705
+        },
+        {
+            "x": 6.850682482856188,
+            "y": 7.25802494172709,
+            "heading": -1.3587604988602537,
+            "angularVelocity": -1.4800327958521704,
+            "velocityX": -3.16761664921765,
+            "velocityY": -0.9379819999325716,
+            "timestamp": 8.33954914056419
+        },
+        {
+            "x": 6.62975279957275,
+            "y": 7.191687511601611,
+            "heading": -1.3718894789288036,
+            "angularVelocity": -0.21150047404023747,
+            "velocityX": -3.5590527596228974,
+            "velocityY": -1.0686586349353433,
+            "timestamp": 8.401624557312674
+        },
+        {
+            "x": 6.40649462388328,
+            "y": 7.118888182289737,
+            "heading": -1.3718898557853474,
+            "angularVelocity": -0.000006070946652807348,
+            "velocityX": -3.596563460766736,
+            "velocityY": -1.1727561911801452,
+            "timestamp": 8.463699974061159
+        },
+        {
+            "x": 6.183236472249506,
+            "y": 7.0460887792034885,
+            "heading": -1.3718902326404057,
+            "angularVelocity": -0.000006070922729380904,
+            "velocityX": -3.5965630732430243,
+            "velocityY": -1.1727573796437725,
+            "timestamp": 8.525775390809644
+        },
+        {
+            "x": 5.959978320616,
+            "y": 6.973289376116265,
+            "heading": -1.3718906094952235,
+            "angularVelocity": -0.000006070918851907899,
+            "velocityX": -3.5965630732387113,
+            "velocityY": -1.172757379659484,
+            "timestamp": 8.587850807558128
+        },
+        {
+            "x": 5.736720168982443,
+            "y": 6.900489973029042,
+            "heading": -1.3718909863498006,
+            "angularVelocity": -0.000006070914972563024,
+            "velocityX": -3.596563073239526,
+            "velocityY": -1.1727573796594701,
+            "timestamp": 8.649926224306613
+        },
+        {
+            "x": 5.513462017348836,
+            "y": 6.8276905699418196,
+            "heading": -1.3718913632041367,
+            "angularVelocity": -0.000006070911095039567,
+            "velocityX": -3.5965630732403415,
+            "velocityY": -1.1727573796594566,
+            "timestamp": 8.712001641055098
+        },
+        {
+            "x": 5.290203865715178,
+            "y": 6.7548911668545975,
+            "heading": -1.3718917400582324,
+            "angularVelocity": -0.0000060709072179749485,
+            "velocityX": -3.596563073241156,
+            "velocityY": -1.1727573796594426,
+            "timestamp": 8.774077057803582
+        },
+        {
+            "x": 5.066945714081469,
+            "y": 6.682091763767377,
+            "heading": -1.3718921169120872,
+            "angularVelocity": -0.000006070903338953198,
+            "velocityX": -3.596563073241971,
+            "velocityY": -1.1727573796594288,
+            "timestamp": 8.836152474552067
+        },
+        {
+            "x": 4.84368756244771,
+            "y": 6.609292360680157,
+            "heading": -1.3718924937657013,
+            "angularVelocity": -0.000006070899462290803,
+            "velocityX": -3.596563073242785,
+            "velocityY": -1.1727573796594153,
+            "timestamp": 8.898227891300552
+        },
+        {
+            "x": 4.620429410813901,
+            "y": 6.536492957592937,
+            "heading": -1.371892870619075,
+            "angularVelocity": -0.000006070895585804876,
+            "velocityX": -3.5965630732436003,
+            "velocityY": -1.1727573796594017,
+            "timestamp": 8.960303308049037
+        },
+        {
+            "x": 4.39717125918004,
+            "y": 6.46369355450572,
+            "heading": -1.3718932474722079,
+            "angularVelocity": -0.000006070891709350806,
+            "velocityX": -3.5965630732444147,
+            "velocityY": -1.172757379659388,
+            "timestamp": 9.022378724797521
+        },
+        {
+            "x": 4.173913107546129,
+            "y": 6.390894151418502,
+            "heading": -1.3718936243251003,
+            "angularVelocity": -0.000006070887832958865,
+            "velocityX": -3.5965630732452283,
+            "velocityY": -1.1727573796593747,
+            "timestamp": 9.084454141546006
+        },
+        {
+            "x": 3.950654955912168,
+            "y": 6.318094748331286,
+            "heading": -1.3718940011777518,
+            "angularVelocity": -0.000006070883956378075,
+            "velocityX": -3.596563073246043,
+            "velocityY": -1.1727573796593613,
+            "timestamp": 9.14652955829449
+        },
+        {
+            "x": 3.7273968042781562,
+            "y": 6.24529534524407,
+            "heading": -1.3718943780301627,
+            "angularVelocity": -0.000006070880081532027,
+            "velocityX": -3.5965630732468568,
+            "velocityY": -1.1727573796593478,
+            "timestamp": 9.208604975042975
+        },
+        {
+            "x": 3.504138652644094,
+            "y": 6.172495942156855,
+            "heading": -1.3718947548823335,
+            "angularVelocity": -0.000006070876206601658,
+            "velocityX": -3.5965630732476703,
+            "velocityY": -1.1727573796593347,
+            "timestamp": 9.27068039179146
+        },
+        {
+            "x": 3.2808805010099813,
+            "y": 6.099696539069641,
+            "heading": -1.3718951317342634,
+            "angularVelocity": -0.000006070872330584258,
+            "velocityX": -3.596563073248485,
+            "velocityY": -1.1727573796593216,
+            "timestamp": 9.332755808539945
+        },
+        {
+            "x": 3.057622349375818,
+            "y": 6.026897135982428,
+            "heading": -1.371895508585953,
+            "angularVelocity": -0.000006070868457801708,
+            "velocityX": -3.5965630732492984,
+            "velocityY": -1.1727573796593085,
+            "timestamp": 9.39483122528843
+        },
+        {
+            "x": 2.834364197741604,
+            "y": 5.954097732895216,
+            "heading": -1.371895885437402,
+            "angularVelocity": -0.0000060708645841945965,
+            "velocityX": -3.596563073250112,
+            "velocityY": -1.1727573796592947,
+            "timestamp": 9.456906642036914
+        },
+        {
+            "x": 2.6111060461064435,
+            "y": 5.881298329810753,
+            "heading": -1.3718962622886106,
+            "angularVelocity": -0.000006070860710280736,
+            "velocityX": -3.5965630732653646,
+            "velocityY": -1.1727573796150002,
+            "timestamp": 9.518982058785399
+        },
+        {
+            "x": 2.387847826730136,
+            "y": 5.808499134475022,
+            "heading": -1.37189663914262,
+            "angularVelocity": -0.000006070905830150403,
+            "velocityX": -3.5965641645370763,
+            "velocityY": -1.1727540328999504,
+            "timestamp": 9.581057475533884
+        },
+        {
+            "x": 2.167695143748949,
+            "y": 5.749324083617281,
+            "heading": -1.3936267357407348,
+            "angularVelocity": -0.3500596167748615,
+            "velocityX": -3.546535722397048,
+            "velocityY": -0.9532767391237054,
+            "timestamp": 9.643132892282368
+        },
+        {
+            "x": 1.9716359535895844,
+            "y": 5.700085594404985,
+            "heading": -1.4887110872196767,
+            "angularVelocity": -1.5317553463104556,
+            "velocityX": -3.1584031236350607,
+            "velocityY": -0.7932043277582467,
+            "timestamp": 9.705208309030853
+        },
+        {
+            "x": 1.8021216559438722,
+            "y": 5.6575856667748505,
+            "heading": -1.6480423798805228,
+            "angularVelocity": -2.5667373818273083,
+            "velocityX": -2.73077985658225,
+            "velocityY": -0.6846498961470444,
+            "timestamp": 9.767283725779338
+        },
+        {
+            "x": 1.6601304337176481,
+            "y": 5.619404991825654,
+            "heading": -1.8664040726402535,
+            "angularVelocity": -3.5176838786999975,
+            "velocityX": -2.2873986138754163,
+            "velocityY": -0.6150691682650391,
+            "timestamp": 9.829359142527823
+        },
+        {
+            "x": 1.5437233022892554,
+            "y": 5.5885120196672124,
+            "heading": -2.146539077662866,
+            "angularVelocity": -4.5128171455965,
+            "velocityX": -1.8752533212953901,
+            "velocityY": -0.49766838108574224,
+            "timestamp": 9.891434559276307
+        },
+        {
+            "x": 1.452238647018182,
+            "y": 5.5675498489404625,
+            "heading": -2.4956493740995165,
+            "angularVelocity": -5.623970240121965,
+            "velocityX": -1.4737662679212806,
+            "velocityY": -0.33768876351944366,
+            "timestamp": 9.953509976024792
+        },
+        {
+            "x": 1.3894112242957146,
+            "y": 5.555821056487952,
+            "heading": -2.802567916985108,
+            "angularVelocity": -4.94428485481059,
+            "velocityX": -1.0121143926754297,
+            "velocityY": -0.188944240197889,
+            "timestamp": 10.015585392773277
+        },
+        {
+            "x": 1.3504462657255363,
+            "y": 5.550087016097331,
+            "heading": -3.0243894075867264,
+            "angularVelocity": -3.573419273210609,
+            "velocityX": -0.6277035356533314,
+            "velocityY": -0.09237216100947475,
+            "timestamp": 10.077660809521761
+        },
+        {
+            "x": 1.3321380615234375,
+            "y": 5.547595977783203,
+            "heading": -3.1411853071795863,
+            "angularVelocity": -1.8815161574522963,
+            "velocityX": -0.2949348576470964,
+            "velocityY": -0.040129224169711215,
+            "timestamp": 10.139736226270246
+        },
+        {
+            "x": 1.3321380615234375,
+            "y": 5.547595977783203,
+            "heading": -3.1411853071795863,
+            "angularVelocity": 7.133789284871957e-33,
+            "velocityX": 0,
+            "velocityY": 1.0797204950028118e-32,
+            "timestamp": 10.201811643018731
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/auto1.traj b/src/main/deploy/choreo/auto1.traj
new file mode 100644
index 00000000..c037a4f6
--- /dev/null
+++ b/src/main/deploy/choreo/auto1.traj
@@ -0,0 +1,1876 @@
+{
+    "samples": [
+        {
+            "x": 1.3321385383605957,
+            "y": 5.547596454620361,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -3.4724754769815887e-38,
+            "velocityY": 7.030705743146232e-38,
+            "timestamp": 0
+        },
+        {
+            "x": 1.361410083208431,
+            "y": 5.5023955382692575,
+            "heading": 0.056119311202146825,
+            "angularVelocity": 0.6842584112779871,
+            "velocityX": 0.35690567728254624,
+            "velocityY": -0.5511312692222103,
+            "timestamp": 0.08201479189321614
+        },
+        {
+            "x": 1.4202660843784345,
+            "y": 5.411892088977607,
+            "heading": 0.16553764559038536,
+            "angularVelocity": 1.3341292693969398,
+            "velocityX": 0.7176266599156221,
+            "velocityY": -1.1035015416424676,
+            "timestamp": 0.1640295837864323
+        },
+        {
+            "x": 1.509321709738084,
+            "y": 5.2758955282995315,
+            "heading": 0.32143534479454616,
+            "angularVelocity": 1.9008485616489859,
+            "velocityX": 1.0858483366707872,
+            "velocityY": -1.658195522280223,
+            "timestamp": 0.24604437567964843
+        },
+        {
+            "x": 1.6296766101338032,
+            "y": 5.093983916449887,
+            "heading": 0.5046862665772869,
+            "angularVelocity": 2.2343642842054003,
+            "velocityX": 1.4674779709545838,
+            "velocityY": -2.2180341820106597,
+            "timestamp": 0.3280591675728646
+        },
+        {
+            "x": 1.782194657966092,
+            "y": 4.869104821237669,
+            "heading": 0.6276214122511846,
+            "angularVelocity": 1.4989387016181202,
+            "velocityX": 1.859640734452738,
+            "velocityY": -2.7419333759331184,
+            "timestamp": 0.4100739594660807
+        },
+        {
+            "x": 1.9333417085303153,
+            "y": 4.6354174904379,
+            "heading": 0.6920907561549033,
+            "angularVelocity": 0.7860697127374073,
+            "velocityX": 1.8429242710390283,
+            "velocityY": -2.849331509662689,
+            "timestamp": 0.49208875135929686
+        },
+        {
+            "x": 2.056917521349593,
+            "y": 4.443493094854259,
+            "heading": 0.6585466805605577,
+            "angularVelocity": -0.40900031348028376,
+            "velocityX": 1.5067503064590375,
+            "velocityY": -2.340119277916704,
+            "timestamp": 0.574103543252513
+        },
+        {
+            "x": 2.1535962335212764,
+            "y": 4.293727999952354,
+            "heading": 0.5265725883978299,
+            "angularVelocity": -1.609149875482936,
+            "velocityX": 1.1787960432498548,
+            "velocityY": -1.8260741927736632,
+            "timestamp": 0.6561183351457291
+        },
+        {
+            "x": 2.2240985983067136,
+            "y": 4.187111119253089,
+            "heading": 0.3017485783287344,
+            "angularVelocity": -2.7412617270530677,
+            "velocityX": 0.859629868685535,
+            "velocityY": -1.2999713617280337,
+            "timestamp": 0.7381331270389453
+        },
+        {
+            "x": 2.2676985263824463,
+            "y": 4.124765396118164,
+            "heading": -1.6642026159430915e-35,
+            "angularVelocity": -3.6791970248685537,
+            "velocityX": 0.5316105432846836,
+            "velocityY": -0.7601765693205621,
+            "timestamp": 0.8201479189321614
+        },
+        {
+            "x": 2.2826958672458773,
+            "y": 4.104348331421266,
+            "heading": -0.17441872129385144,
+            "angularVelocity": -4.1236397648607985,
+            "velocityX": 0.35456991481679806,
+            "velocityY": -0.4827040310886087,
+            "timestamp": 0.8624451930006055
+        },
+        {
+            "x": 2.289905521300629,
+            "y": 4.095598364764115,
+            "heading": -0.366838925198976,
+            "angularVelocity": -4.549234156171776,
+            "velocityX": 0.17045197860942923,
+            "velocityY": -0.20686833489535802,
+            "timestamp": 0.9047424670690496
+        },
+        {
+            "x": 2.289262823237601,
+            "y": 4.0986035079624115,
+            "heading": -0.5765637041805302,
+            "angularVelocity": -4.958352130262211,
+            "velocityX": -0.015194786831599966,
+            "velocityY": 0.07104815297159969,
+            "timestamp": 0.9470397411374937
+        },
+        {
+            "x": 2.28071834356103,
+            "y": 4.1136177509731775,
+            "heading": -0.8009025976234118,
+            "angularVelocity": -5.303861735388993,
+            "velocityX": -0.20201017358104575,
+            "velocityY": 0.3549695185195792,
+            "timestamp": 0.9893370152059378
+        },
+        {
+            "x": 2.264115058699565,
+            "y": 4.140907151677903,
+            "heading": -1.0347556438013104,
+            "angularVelocity": -5.528797099299715,
+            "velocityX": -0.39253794073343345,
+            "velocityY": 0.6451810738575218,
+            "timestamp": 1.0316342892743817
+        },
+        {
+            "x": 2.2392828245472374,
+            "y": 4.180616635647853,
+            "heading": -1.271268857234422,
+            "angularVelocity": -5.5916892670292055,
+            "velocityX": -0.5870882864022154,
+            "velocityY": 0.9388189864361984,
+            "timestamp": 1.0739315633428257
+        },
+        {
+            "x": 2.206219336908872,
+            "y": 4.232592488850575,
+            "heading": -1.500368652073653,
+            "angularVelocity": -5.416419849385782,
+            "velocityX": -0.7816931082807688,
+            "velocityY": 1.228822763344421,
+            "timestamp": 1.1162288374112697
+        },
+        {
+            "x": 2.16560115012261,
+            "y": 4.295896098840919,
+            "heading": -1.7081840928578422,
+            "angularVelocity": -4.913211202403,
+            "velocityX": -0.9603027069908828,
+            "velocityY": 1.496635690704529,
+            "timestamp": 1.1585261114797136
+        },
+        {
+            "x": 2.116418529309532,
+            "y": 4.368321186816809,
+            "heading": -1.8863054196825848,
+            "angularVelocity": -4.211177451684291,
+            "velocityX": -1.162784645022098,
+            "velocityY": 1.7122873653439958,
+            "timestamp": 1.2008233855481576
+        },
+        {
+            "x": 2.058912682392385,
+            "y": 4.450563139928545,
+            "heading": -2.036216963298125,
+            "angularVelocity": -3.54423652391779,
+            "velocityX": -1.3595639005977715,
+            "velocityY": 1.9443795119906613,
+            "timestamp": 1.2431206596166016
+        },
+        {
+            "x": 1.99337005684811,
+            "y": 4.542961602948437,
+            "heading": -2.1579434366118924,
+            "angularVelocity": -2.877879863293171,
+            "velocityX": -1.549570911785386,
+            "velocityY": 2.184501603350988,
+            "timestamp": 1.2854179336850455
+        },
+        {
+            "x": 1.9201828437796038,
+            "y": 4.645652670813241,
+            "heading": -2.2501769997399386,
+            "angularVelocity": -2.180603009517744,
+            "velocityX": -1.7303056681637912,
+            "velocityY": 2.4278412764527704,
+            "timestamp": 1.3277152077534895
+        },
+        {
+            "x": 1.8398838191629743,
+            "y": 4.758671823925337,
+            "heading": -2.3107451299036645,
+            "angularVelocity": -1.4319629691907945,
+            "velocityX": -1.8984444360809696,
+            "velocityY": 2.6720197838095423,
+            "timestamp": 1.3700124818219335
+        },
+        {
+            "x": 1.753396216406846,
+            "y": 4.882199443808802,
+            "heading": -2.337574910152261,
+            "angularVelocity": -0.6343146417705776,
+            "velocityX": -2.0447559484844615,
+            "velocityY": 2.920462904620659,
+            "timestamp": 1.4123097558903774
+        },
+        {
+            "x": 1.6751428992292787,
+            "y": 4.996901240775032,
+            "heading": -2.400008661282964,
+            "angularVelocity": -1.4760703261792811,
+            "velocityX": -1.8500794413119903,
+            "velocityY": 2.7118011619525055,
+            "timestamp": 1.4546070299588214
+        },
+        {
+            "x": 1.604167639142011,
+            "y": 5.102445488685979,
+            "heading": -2.5005028957452873,
+            "angularVelocity": -2.375903333621606,
+            "velocityX": -1.6780102654468432,
+            "velocityY": 2.4952966883908108,
+            "timestamp": 1.4969043040272654
+        },
+        {
+            "x": 1.5418087398499432,
+            "y": 5.19740105341742,
+            "heading": -2.6238103064581355,
+            "angularVelocity": -2.9152566785584497,
+            "velocityX": -1.4743006651246806,
+            "velocityY": 2.2449570763776965,
+            "timestamp": 1.5392015780957093
+        },
+        {
+            "x": 1.488225747512813,
+            "y": 5.280327915096259,
+            "heading": -2.743877445785595,
+            "angularVelocity": -2.8386495813694843,
+            "velocityX": -1.2668190448969345,
+            "velocityY": 1.960572247389977,
+            "timestamp": 1.5814988521641533
+        },
+        {
+            "x": 1.4429482417442723,
+            "y": 5.35126150411709,
+            "heading": -2.8526923539591857,
+            "angularVelocity": -2.5726222450531995,
+            "velocityX": -1.0704591907098815,
+            "velocityY": 1.6770250703638487,
+            "timestamp": 1.6237961262325973
+        },
+        {
+            "x": 1.4056282761179824,
+            "y": 5.410288137316586,
+            "heading": -2.94638853961619,
+            "angularVelocity": -2.215182602675249,
+            "velocityX": -0.8823255504811011,
+            "velocityY": 1.3955186120027623,
+            "timestamp": 1.6660934003010412
+        },
+        {
+            "x": 1.3760265384921018,
+            "y": 5.457458867956814,
+            "heading": -3.023001318784294,
+            "angularVelocity": -1.8112935373596752,
+            "velocityX": -0.6998497723040017,
+            "velocityY": 1.115219164334259,
+            "timestamp": 1.7083906743694852
+        },
+        {
+            "x": 1.3539833473819511,
+            "y": 5.492803685860998,
+            "heading": -3.0814563447777146,
+            "angularVelocity": -1.3820045683991566,
+            "velocityX": -0.5211492134098515,
+            "velocityY": 0.8356287416297835,
+            "timestamp": 1.7506879484379292
+        },
+        {
+            "x": 1.3393859043766252,
+            "y": 5.516345418500216,
+            "heading": -3.121051917122202,
+            "angularVelocity": -0.9361258666554966,
+            "velocityX": -0.3451154554713123,
+            "velocityY": 0.5565780102311921,
+            "timestamp": 1.7929852225063732
+        },
+        {
+            "x": 1.3321385383605957,
+            "y": 5.52810525894165,
+            "heading": -3.1411853071795863,
+            "angularVelocity": -0.47599734263737736,
+            "velocityX": -0.17134357179382342,
+            "velocityY": 0.2780283292584069,
+            "timestamp": 1.8352824965748171
+        },
+        {
+            "x": 1.3321385383605957,
+            "y": 5.52810525894165,
+            "heading": -3.1411853071795863,
+            "angularVelocity": 1.0972891859651771e-35,
+            "velocityX": 2.010813957988482e-36,
+            "velocityY": -7.697026441410569e-35,
+            "timestamp": 1.877579770643261
+        },
+        {
+            "x": 1.337370959637994,
+            "y": 5.5282378526253355,
+            "heading": -3.1057781238593085,
+            "angularVelocity": 1.0527517345397082,
+            "velocityX": 0.1555740970920068,
+            "velocityY": 0.003942370372304683,
+            "timestamp": 1.9112127556658494
+        },
+        {
+            "x": 1.3479554232342674,
+            "y": 5.5287097500206475,
+            "heading": -3.0355501553802027,
+            "angularVelocity": 2.0880682589410555,
+            "velocityX": 0.314704852666657,
+            "velocityY": 0.014030791349476537,
+            "timestamp": 1.9448457406884376
+        },
+        {
+            "x": 1.364027106419543,
+            "y": 5.529829382282991,
+            "heading": -2.9313355116530078,
+            "angularVelocity": 3.0985844300529233,
+            "velocityX": 0.4778547956561592,
+            "velocityY": 0.03328970835004707,
+            "timestamp": 1.978478725711026
+        },
+        {
+            "x": 1.3857940218160676,
+            "y": 5.531991335292171,
+            "heading": -2.7946612849381784,
+            "angularVelocity": 4.06369600031153,
+            "velocityX": 0.6471895189173892,
+            "velocityY": 0.06428073534739039,
+            "timestamp": 2.012111710733614
+        },
+        {
+            "x": 1.4139333100804088,
+            "y": 5.535564467259386,
+            "heading": -2.6300339585531556,
+            "angularVelocity": 4.89481758084981,
+            "velocityX": 0.8366574731752962,
+            "velocityY": 0.10623891887132897,
+            "timestamp": 2.0457446957562024
+        },
+        {
+            "x": 1.4504189067700664,
+            "y": 5.5389912877920775,
+            "heading": -2.4501533377113245,
+            "angularVelocity": 5.348339456667967,
+            "velocityX": 1.0848158932415233,
+            "velocityY": 0.10188868250590417,
+            "timestamp": 2.0793776807787907
+        },
+        {
+            "x": 1.4957426020786804,
+            "y": 5.541383162087915,
+            "heading": -2.2591276091627437,
+            "angularVelocity": 5.679713781583368,
+            "velocityX": 1.3475965715851275,
+            "velocityY": 0.07111691972124916,
+            "timestamp": 2.113010665801379
+        },
+        {
+            "x": 1.549887277450098,
+            "y": 5.541604080123591,
+            "heading": -2.077370178961034,
+            "angularVelocity": 5.404142096802884,
+            "velocityX": 1.6098682687562116,
+            "velocityY": 0.006568493267186675,
+            "timestamp": 2.1466436508239672
+        },
+        {
+            "x": 1.612403659002375,
+            "y": 5.539682953328213,
+            "heading": -1.9100118188035742,
+            "angularVelocity": 4.9760186330490965,
+            "velocityX": 1.8587818330811392,
+            "velocityY": -0.05712031786913091,
+            "timestamp": 2.1802766358465555
+        },
+        {
+            "x": 1.6835678333886066,
+            "y": 5.53654594415181,
+            "heading": -1.7561585727792173,
+            "angularVelocity": 4.5744749067330135,
+            "velocityX": 2.1159042035203575,
+            "velocityY": -0.0932718036860474,
+            "timestamp": 2.213909620869144
+        },
+        {
+            "x": 1.7632443244228584,
+            "y": 5.534877661418315,
+            "heading": -1.616595649842294,
+            "angularVelocity": 4.149584785388266,
+            "velocityX": 2.3689984989658393,
+            "velocityY": -0.04960257712408059,
+            "timestamp": 2.247542605891732
+        },
+        {
+            "x": 1.845517380604842,
+            "y": 5.542129045770498,
+            "heading": -1.4828842082324165,
+            "angularVelocity": 3.9756043515042103,
+            "velocityX": 2.4462014337035023,
+            "velocityY": 0.21560335329479804,
+            "timestamp": 2.2811755909143203
+        },
+        {
+            "x": 1.9198078929781421,
+            "y": 5.552688491598465,
+            "heading": -1.3358148203184805,
+            "angularVelocity": 4.372772378519569,
+            "velocityX": 2.208858723761989,
+            "velocityY": 0.31396100646060415,
+            "timestamp": 2.3148085759369086
+        },
+        {
+            "x": 1.9854932228404973,
+            "y": 5.561629411594934,
+            "heading": -1.1756047034069004,
+            "angularVelocity": 4.763481945000775,
+            "velocityX": 1.9530032739657306,
+            "velocityY": 0.26583783718469556,
+            "timestamp": 2.348441560959497
+        },
+        {
+            "x": 2.0429500865174064,
+            "y": 5.56779505699511,
+            "heading": -1.0015843737719388,
+            "angularVelocity": 5.1740970811270035,
+            "velocityX": 1.7083486237787309,
+            "velocityY": 0.18332138512343155,
+            "timestamp": 2.382074545982085
+        },
+        {
+            "x": 2.0926336267477486,
+            "y": 5.570712245735625,
+            "heading": -0.8117369336213589,
+            "angularVelocity": 5.644680066996039,
+            "velocityX": 1.477226603495769,
+            "velocityY": 0.08673594504191894,
+            "timestamp": 2.4157075310046734
+        },
+        {
+            "x": 2.1331736827499013,
+            "y": 5.571318223980459,
+            "heading": -0.6223345825199257,
+            "angularVelocity": 5.63144636059598,
+            "velocityX": 1.2053659814888902,
+            "velocityY": 0.01801737920159694,
+            "timestamp": 2.4493405160272617
+        },
+        {
+            "x": 2.1650842656051874,
+            "y": 5.570653927593427,
+            "heading": -0.4456863837750604,
+            "angularVelocity": 5.252230767689132,
+            "velocityX": 0.9487883051074633,
+            "velocityY": -0.01975133597523799,
+            "timestamp": 2.48297350104985
+        },
+        {
+            "x": 2.1896037980440752,
+            "y": 5.569969320542384,
+            "heading": -0.29224766364008553,
+            "angularVelocity": 4.56214992608965,
+            "velocityX": 0.7290323003569462,
+            "velocityY": -0.020355227184960373,
+            "timestamp": 2.516606486072438
+        },
+        {
+            "x": 2.20780773019257,
+            "y": 5.569409848980837,
+            "heading": -0.16876882632138132,
+            "angularVelocity": 3.6713612317127033,
+            "velocityX": 0.5412523490337908,
+            "velocityY": -0.016634609184155776,
+            "timestamp": 2.5502394710950265
+        },
+        {
+            "x": 2.220213327560089,
+            "y": 5.568812984785934,
+            "heading": -0.07803807473080998,
+            "angularVelocity": 2.697671691336225,
+            "velocityX": 0.368852106323223,
+            "velocityY": -0.017746393741217954,
+            "timestamp": 2.5838724561176147
+        },
+        {
+            "x": 2.227117258661317,
+            "y": 5.568051315056596,
+            "heading": -0.021486347391680306,
+            "angularVelocity": 1.6814364618885036,
+            "velocityX": 0.2052726243772784,
+            "velocityY": -0.022646509931434324,
+            "timestamp": 2.617505441140203
+        },
+        {
+            "x": 2.2287168502807617,
+            "y": 5.567087173461914,
+            "heading": 6.38149991346766e-37,
+            "angularVelocity": 0.6388474700431709,
+            "velocityX": 0.047560203721742585,
+            "velocityY": -0.02866654845040371,
+            "timestamp": 2.6511384261627913
+        },
+        {
+            "x": 2.2247758271699722,
+            "y": 5.5659373744789455,
+            "heading": -0.016384469566950412,
+            "angularVelocity": -0.4672993433012807,
+            "velocityX": -0.11240141184197158,
+            "velocityY": -0.03279326849576676,
+            "timestamp": 2.686200468945049
+        },
+        {
+            "x": 2.215132755191586,
+            "y": 5.564807940629173,
+            "heading": -0.07111277017786721,
+            "angularVelocity": -1.560898803038675,
+            "velocityX": -0.27502881216225394,
+            "velocityY": -0.03221243715853021,
+            "timestamp": 2.7212625117273066
+        },
+        {
+            "x": 2.199681422128445,
+            "y": 5.563984363308005,
+            "heading": -0.16355266498461823,
+            "angularVelocity": -2.6364663171744263,
+            "velocityX": -0.4406854774291637,
+            "velocityY": -0.023489142554608954,
+            "timestamp": 2.7563245545095643
+        },
+        {
+            "x": 2.178275880729617,
+            "y": 5.563860339253889,
+            "heading": -0.2925810137194886,
+            "angularVelocity": -3.6800008925938044,
+            "velocityX": -0.6105046854160964,
+            "velocityY": -0.0035372740511740697,
+            "timestamp": 2.791386597291822
+        },
+        {
+            "x": 2.150434792823077,
+            "y": 5.56488104971689,
+            "heading": -0.45485951159806126,
+            "angularVelocity": -4.628324107820951,
+            "velocityX": -0.7940520773258658,
+            "velocityY": 0.029111551467175232,
+            "timestamp": 2.8264486400740796
+        },
+        {
+            "x": 2.1136927979170794,
+            "y": 5.564806154665508,
+            "heading": -0.6342616510080993,
+            "angularVelocity": -5.116705279386068,
+            "velocityX": -1.047913697846207,
+            "velocityY": -0.0021360721007366726,
+            "timestamp": 2.8615106828563373
+        },
+        {
+            "x": 2.0677952991091333,
+            "y": 5.562722960469238,
+            "heading": -0.8285761603492735,
+            "angularVelocity": -5.542019058841107,
+            "velocityX": -1.3090366437853844,
+            "velocityY": -0.05941451298794179,
+            "timestamp": 2.896572725638595
+        },
+        {
+            "x": 2.0120787764368706,
+            "y": 5.557776466918295,
+            "heading": -1.020468287532569,
+            "angularVelocity": -5.472930609747536,
+            "velocityX": -1.5890837569925207,
+            "velocityY": -0.14107830458316797,
+            "timestamp": 2.9316347684208526
+        },
+        {
+            "x": 1.947346732087487,
+            "y": 5.549990941392819,
+            "heading": -1.1972983758508349,
+            "angularVelocity": -5.043348141932757,
+            "velocityX": -1.8462142879518657,
+            "velocityY": -0.22204996935933435,
+            "timestamp": 2.9666968112031102
+        },
+        {
+            "x": 1.8731816133340355,
+            "y": 5.541067205657786,
+            "heading": -1.3595832736612468,
+            "angularVelocity": -4.628506639451492,
+            "velocityX": -2.1152537863818077,
+            "velocityY": -0.25451271594333,
+            "timestamp": 3.001758853985368
+        },
+        {
+            "x": 1.7903438797008602,
+            "y": 5.535778355661316,
+            "heading": -1.5070368489944903,
+            "angularVelocity": -4.2055044039778195,
+            "velocityX": -2.362604316799635,
+            "velocityY": -0.15084260860996357,
+            "timestamp": 3.0368208967676256
+        },
+        {
+            "x": 1.705207783905628,
+            "y": 5.540259742780199,
+            "heading": -1.6469795594362229,
+            "angularVelocity": -3.991287995134942,
+            "velocityX": -2.4281556075880766,
+            "velocityY": 0.12781306402233675,
+            "timestamp": 3.0718829395498832
+        },
+        {
+            "x": 1.629345764073995,
+            "y": 5.546176174189709,
+            "heading": -1.802120558201536,
+            "angularVelocity": -4.424756416184017,
+            "velocityX": -2.1636508831716217,
+            "velocityY": 0.16874177714778527,
+            "timestamp": 3.106944982332141
+        },
+        {
+            "x": 1.5628215543297233,
+            "y": 5.550494777199676,
+            "heading": -1.9719374235468123,
+            "angularVelocity": -4.843324914063712,
+            "velocityX": -1.8973284060315727,
+            "velocityY": 0.12317031944734674,
+            "timestamp": 3.1420070251143986
+        },
+        {
+            "x": 1.5052956910676978,
+            "y": 5.552468250746859,
+            "heading": -2.1578472190724005,
+            "angularVelocity": -5.302309300120524,
+            "velocityX": -1.6406877265900472,
+            "velocityY": 0.056285184506758255,
+            "timestamp": 3.177069067896656
+        },
+        {
+            "x": 1.4562451550739939,
+            "y": 5.551766768072451,
+            "heading": -2.3624196209535056,
+            "angularVelocity": -5.834583088941523,
+            "velocityX": -1.3989640106914938,
+            "velocityY": -0.02000689688175958,
+            "timestamp": 3.212131110678914
+        },
+        {
+            "x": 1.4173942622657685,
+            "y": 5.550106751106204,
+            "heading": -2.5657566721184293,
+            "angularVelocity": -5.799349810497013,
+            "velocityX": -1.1080613029165842,
+            "velocityY": -0.047345129790485885,
+            "timestamp": 3.2471931534611715
+        },
+        {
+            "x": 1.3874992845964569,
+            "y": 5.548698391964879,
+            "heading": -2.750369265628105,
+            "angularVelocity": -5.26531196873374,
+            "velocityX": -0.852630802345583,
+            "velocityY": -0.0401676294239809,
+            "timestamp": 3.282255196243429
+        },
+        {
+            "x": 1.3647199881577643,
+            "y": 5.548009174125998,
+            "heading": -2.903871904539791,
+            "angularVelocity": -4.378028966109259,
+            "velocityX": -0.6496853757254475,
+            "velocityY": -0.019657093089571077,
+            "timestamp": 3.317317239025687
+        },
+        {
+            "x": 1.3481785543906255,
+            "y": 5.547743770300603,
+            "heading": -3.0214740080658498,
+            "angularVelocity": -3.354114426714698,
+            "velocityX": -0.4717760990101023,
+            "velocityY": -0.007569548273095167,
+            "timestamp": 3.3523792818079445
+        },
+        {
+            "x": 1.3374168488281284,
+            "y": 5.547647789865521,
+            "heading": -3.10100248389671,
+            "angularVelocity": -2.268221401837531,
+            "velocityX": -0.30693321633680876,
+            "velocityY": -0.0027374456097331258,
+            "timestamp": 3.387441324590202
+        },
+        {
+            "x": 1.3321385383605957,
+            "y": 5.547596454620361,
+            "heading": -3.1411853071795863,
+            "angularVelocity": -1.1460491201958698,
+            "velocityX": -0.15054201206449694,
+            "velocityY": -0.001464125906141093,
+            "timestamp": 3.42250336737246
+        },
+        {
+            "x": 1.3321385383605957,
+            "y": 5.547596454620361,
+            "heading": -3.1411853071795863,
+            "angularVelocity": -1.1877010663188592e-33,
+            "velocityX": -2.3472123166291743e-34,
+            "velocityY": -4.828629969149868e-35,
+            "timestamp": 3.4575654101547175
+        },
+        {
+            "x": 1.3353773426968816,
+            "y": 5.556976212978418,
+            "heading": -3.1027759330153977,
+            "angularVelocity": 0.9418411865422114,
+            "velocityX": 0.07941913622507528,
+            "velocityY": 0.2300022568362304,
+            "timestamp": 3.4983465680749837
+        },
+        {
+            "x": 1.342768335196551,
+            "y": 5.576502155287014,
+            "heading": -3.0330086785474775,
+            "angularVelocity": 1.7107717883912714,
+            "velocityX": 0.18123547433645554,
+            "velocityY": 0.47879813385321396,
+            "timestamp": 3.53912772599525
+        },
+        {
+            "x": 1.357543783583743,
+            "y": 5.605477021924564,
+            "heading": -2.939156767804727,
+            "angularVelocity": 2.301354731669143,
+            "velocityX": 0.3623106635687124,
+            "velocityY": 0.7104964183263536,
+            "timestamp": 3.579908883915516
+        },
+        {
+            "x": 1.3800861680237329,
+            "y": 5.643206466545064,
+            "heading": -2.8183134070613347,
+            "angularVelocity": 2.9632155364411483,
+            "velocityX": 0.5527646979535052,
+            "velocityY": 0.9251685470595896,
+            "timestamp": 3.620690041835782
+        },
+        {
+            "x": 1.410132685953878,
+            "y": 5.690126983565226,
+            "heading": -2.6706561536339386,
+            "angularVelocity": 3.6207224354956073,
+            "velocityX": 0.7367745170181611,
+            "velocityY": 1.1505440113274845,
+            "timestamp": 3.6614711997560483
+        },
+        {
+            "x": 1.447426110042683,
+            "y": 5.747148126734536,
+            "heading": -2.4998618512700794,
+            "angularVelocity": 4.188068977781135,
+            "velocityX": 0.9144768317201708,
+            "velocityY": 1.3982227596576835,
+            "timestamp": 3.7022523576763144
+        },
+        {
+            "x": 1.4919514039971702,
+            "y": 5.815443990853687,
+            "heading": -2.3159167566388894,
+            "angularVelocity": 4.510541240413857,
+            "velocityX": 1.0918104395549988,
+            "velocityY": 1.6746916370712401,
+            "timestamp": 3.7430335155965806
+        },
+        {
+            "x": 1.543306542439651,
+            "y": 5.895557258888599,
+            "heading": -2.140840879075951,
+            "angularVelocity": 4.293058031977455,
+            "velocityX": 1.259285931578716,
+            "velocityY": 1.9644677130440258,
+            "timestamp": 3.7838146735168467
+        },
+        {
+            "x": 1.5991978574253154,
+            "y": 5.9852989254543125,
+            "heading": -2.0026162516135204,
+            "angularVelocity": 3.389423805294663,
+            "velocityX": 1.3705180979642946,
+            "velocityY": 2.2005669074226377,
+            "timestamp": 3.824595831437113
+        },
+        {
+            "x": 1.6602838186413746,
+            "y": 6.084712915583529,
+            "heading": -1.8987034819220077,
+            "angularVelocity": 2.5480583433820057,
+            "velocityX": 1.497896683941456,
+            "velocityY": 2.4377431931576705,
+            "timestamp": 3.865376989357379
+        },
+        {
+            "x": 1.7272241659282512,
+            "y": 6.1936323149644315,
+            "heading": -1.8270894144503607,
+            "angularVelocity": 1.7560577267488233,
+            "velocityX": 1.6414528350998836,
+            "velocityY": 2.670826551660404,
+            "timestamp": 3.906158147277645
+        },
+        {
+            "x": 1.8006198616667228,
+            "y": 6.311769110233653,
+            "heading": -1.7863461946081067,
+            "angularVelocity": 0.9990697155268056,
+            "velocityX": 1.7997452618185266,
+            "velocityY": 2.8968474975673533,
+            "timestamp": 3.9469393051979114
+        },
+        {
+            "x": 1.8749454151333007,
+            "y": 6.423903525675366,
+            "heading": -1.7351071665224345,
+            "angularVelocity": 1.2564387746383576,
+            "velocityX": 1.8225464223428127,
+            "velocityY": 2.7496623725337597,
+            "timestamp": 3.9877204631181775
+        },
+        {
+            "x": 1.9436310828301862,
+            "y": 6.525803671980358,
+            "heading": -1.6558915421182487,
+            "angularVelocity": 1.9424564785302534,
+            "velocityX": 1.6842500605592803,
+            "velocityY": 2.4987065473772168,
+            "timestamp": 4.028501621038443
+        },
+        {
+            "x": 2.0070825887496224,
+            "y": 6.617181897860731,
+            "heading": -1.5496488374577189,
+            "angularVelocity": 2.6051909773688204,
+            "velocityX": 1.5559025087883662,
+            "velocityY": 2.240697188123809,
+            "timestamp": 4.069282778958709
+        },
+        {
+            "x": 2.0657895498392027,
+            "y": 6.697945113539011,
+            "heading": -1.416445908140072,
+            "angularVelocity": 3.2662861014903286,
+            "velocityX": 1.4395609169401762,
+            "velocityY": 1.9804051625063046,
+            "timestamp": 4.1100639368789755
+        },
+        {
+            "x": 2.1168235029971925,
+            "y": 6.770262031139991,
+            "heading": -1.2545233571884964,
+            "angularVelocity": 3.9705236243698976,
+            "velocityX": 1.2514101060536296,
+            "velocityY": 1.7732924048496255,
+            "timestamp": 4.150845094799242
+        },
+        {
+            "x": 2.160098891028687,
+            "y": 6.834337445285054,
+            "heading": -1.0613678161788878,
+            "angularVelocity": 4.736391776497875,
+            "velocityX": 1.0611613362255508,
+            "velocityY": 1.571201442350923,
+            "timestamp": 4.191626252719508
+        },
+        {
+            "x": 2.1954531720383796,
+            "y": 6.890659821255424,
+            "heading": -0.8342118106482137,
+            "angularVelocity": 5.570121524621797,
+            "velocityX": 0.8669268557507981,
+            "velocityY": 1.3810881996163518,
+            "timestamp": 4.232407410639774
+        },
+        {
+            "x": 2.2216225780243426,
+            "y": 6.9367359795772305,
+            "heading": -0.6132278215539244,
+            "angularVelocity": 5.418776718560821,
+            "velocityX": 0.6417033581324141,
+            "velocityY": 1.1298393834695264,
+            "timestamp": 4.27318856856004
+        },
+        {
+            "x": 2.2400584030513464,
+            "y": 6.973023548052357,
+            "heading": -0.41519671142857095,
+            "angularVelocity": 4.855946231652809,
+            "velocityX": 0.4520672282785309,
+            "velocityY": 0.889812117303642,
+            "timestamp": 4.313969726480306
+        },
+        {
+            "x": 2.252392360052711,
+            "y": 6.999798504355172,
+            "heading": -0.24838728557433673,
+            "angularVelocity": 4.0903553101747345,
+            "velocityX": 0.3024425403878878,
+            "velocityY": 0.6565521350611045,
+            "timestamp": 4.3547508844005725
+        },
+        {
+            "x": 2.2601470415559044,
+            "y": 7.01732009026677,
+            "heading": -0.11829622013640562,
+            "angularVelocity": 3.189979688469885,
+            "velocityX": 0.19015353900334692,
+            "velocityY": 0.4296490537580049,
+            "timestamp": 4.395532042320839
+        },
+        {
+            "x": 2.2649306384373356,
+            "y": 7.026470873344902,
+            "heading": -0.032599297392582494,
+            "angularVelocity": 2.101385225779393,
+            "velocityX": 0.11729919220988534,
+            "velocityY": 0.2243875246510465,
+            "timestamp": 4.436313200241105
+        },
+        {
+            "x": 2.2676985263824463,
+            "y": 7.028899669647217,
+            "heading": 1.4623510884357687e-33,
+            "angularVelocity": 0.799371549388559,
+            "velocityX": 0.06787173504299782,
+            "velocityY": 0.05955682541099079,
+            "timestamp": 4.477094358161371
+        },
+        {
+            "x": 2.268447419698579,
+            "y": 7.024856278106883,
+            "heading": -0.027481442707952487,
+            "angularVelocity": -0.643111881448138,
+            "velocityX": 0.017525360464520197,
+            "velocityY": -0.09462214806441192,
+            "timestamp": 4.5198263360655435
+        },
+        {
+            "x": 2.2661447146026696,
+            "y": 7.012526995839087,
+            "heading": -0.10890493556394686,
+            "angularVelocity": -1.9054463858094515,
+            "velocityX": -0.05388716387229158,
+            "velocityY": -0.28852589729042577,
+            "timestamp": 4.562558313969716
+        },
+        {
+            "x": 2.259237652200008,
+            "y": 6.990956553960971,
+            "heading": -0.23735745507492811,
+            "angularVelocity": -3.0060045383117964,
+            "velocityX": -0.16163685233926556,
+            "velocityY": -0.504784541602271,
+            "timestamp": 4.605290291873889
+        },
+        {
+            "x": 2.2461382818263638,
+            "y": 6.959794967044901,
+            "heading": -0.4073842771005642,
+            "angularVelocity": -3.9789129912714607,
+            "velocityX": -0.30654725140549266,
+            "velocityY": -0.7292334323009891,
+            "timestamp": 4.648022269778061
+        },
+        {
+            "x": 2.225216379141952,
+            "y": 6.918625356851511,
+            "heading": -0.6110307557476551,
+            "angularVelocity": -4.765669380054774,
+            "velocityX": -0.48960763602681573,
+            "velocityY": -0.9634379734473075,
+            "timestamp": 4.690754247682234
+        },
+        {
+            "x": 2.1950172148844427,
+            "y": 6.866427702804465,
+            "heading": -0.8329684676174923,
+            "angularVelocity": -5.193714935628281,
+            "velocityX": -0.706711126857547,
+            "velocityY": -1.2215127079794852,
+            "timestamp": 4.7334862255864065
+        },
+        {
+            "x": 2.1547332634682537,
+            "y": 6.802939586567042,
+            "heading": -1.0490897028487878,
+            "angularVelocity": -5.057599620498584,
+            "velocityX": -0.9427120716604103,
+            "velocityY": -1.4857284720074833,
+            "timestamp": 4.776218203490579
+        },
+        {
+            "x": 2.1052130726477585,
+            "y": 6.731031156240311,
+            "heading": -1.2322360836735529,
+            "angularVelocity": -4.285932685715518,
+            "velocityX": -1.1588555748939446,
+            "velocityY": -1.6827779535033027,
+            "timestamp": 4.818950181394752
+        },
+        {
+            "x": 2.046678257521749,
+            "y": 6.650499615743051,
+            "heading": -1.3842475177981566,
+            "angularVelocity": -3.557322679177027,
+            "velocityX": -1.3698129128793415,
+            "velocityY": -1.8845732036521736,
+            "timestamp": 4.861682159298924
+        },
+        {
+            "x": 1.9824374188049456,
+            "y": 6.55859622988028,
+            "heading": -1.505962481707897,
+            "angularVelocity": -2.848334429608893,
+            "velocityX": -1.5033434413184743,
+            "velocityY": -2.150693470563594,
+            "timestamp": 4.904414137203097
+        },
+        {
+            "x": 1.9120980242519563,
+            "y": 6.455385209029271,
+            "heading": -1.5975173206853661,
+            "angularVelocity": -2.1425368884815836,
+            "velocityX": -1.6460598830863262,
+            "velocityY": -2.415311106882586,
+            "timestamp": 4.9471461151072695
+        },
+        {
+            "x": 1.835418260391466,
+            "y": 6.341055354736124,
+            "heading": -1.6582681711046532,
+            "angularVelocity": -1.4216718579121885,
+            "velocityX": -1.7944351659182955,
+            "velocityY": -2.675510470157384,
+            "timestamp": 4.989878093011442
+        },
+        {
+            "x": 1.7526348309039128,
+            "y": 6.215560469564215,
+            "heading": -1.6873132093111056,
+            "angularVelocity": -0.6797026402940408,
+            "velocityX": -1.937271185368423,
+            "velocityY": -2.9367909309822835,
+            "timestamp": 5.032610070915615
+        },
+        {
+            "x": 1.677441366215122,
+            "y": 6.099853521630283,
+            "heading": -1.7491066585439166,
+            "angularVelocity": -1.4460704199413479,
+            "velocityX": -1.7596532708458867,
+            "velocityY": -2.70773677252684,
+            "timestamp": 5.075342048819787
+        },
+        {
+            "x": 1.6091252487579695,
+            "y": 5.994372460119343,
+            "heading": -1.8457597622108368,
+            "angularVelocity": -2.2618448386280567,
+            "velocityX": -1.598711803379506,
+            "velocityY": -2.4684338681322977,
+            "timestamp": 5.11807402672396
+        },
+        {
+            "x": 1.547030840271468,
+            "y": 5.899198607270202,
+            "heading": -1.9803343034636756,
+            "angularVelocity": -3.149270121655174,
+            "velocityX": -1.4531133715773699,
+            "velocityY": -2.2272278868666358,
+            "timestamp": 5.1608060046281325
+        },
+        {
+            "x": 1.4904311214521733,
+            "y": 5.813988086882659,
+            "heading": -2.1571135876817413,
+            "angularVelocity": -4.136931939225903,
+            "velocityX": -1.3245284116317142,
+            "velocityY": -1.9940691858127988,
+            "timestamp": 5.203537982532305
+        },
+        {
+            "x": 1.4423633386018753,
+            "y": 5.740159267538946,
+            "heading": -2.35427402869071,
+            "angularVelocity": -4.613885213811227,
+            "velocityX": -1.1248667908162702,
+            "velocityY": -1.7277182794879316,
+            "timestamp": 5.246269960436478
+        },
+        {
+            "x": 1.4038900802302197,
+            "y": 5.6781134856885735,
+            "heading": -2.5480008926422313,
+            "angularVelocity": -4.533533748097493,
+            "velocityX": -0.9003388155337229,
+            "velocityY": -1.451975426681909,
+            "timestamp": 5.28900193834065
+        },
+        {
+            "x": 1.3744454738206087,
+            "y": 5.627099122367314,
+            "heading": -2.7223828634703597,
+            "angularVelocity": -4.0808307824923435,
+            "velocityX": -0.6890532068429254,
+            "velocityY": -1.1938217190802813,
+            "timestamp": 5.331733916244823
+        },
+        {
+            "x": 1.3538849352129523,
+            "y": 5.586407689469935,
+            "heading": -2.872592229193493,
+            "angularVelocity": -3.5151512541727503,
+            "velocityX": -0.4811511101537121,
+            "velocityY": -0.9522478221960695,
+            "timestamp": 5.374465894148996
+        },
+        {
+            "x": 1.341639273384249,
+            "y": 5.556291662758613,
+            "heading": -2.9981488971519656,
+            "angularVelocity": -2.9382367518778816,
+            "velocityX": -0.2865690386755447,
+            "velocityY": -0.7047655687470963,
+            "timestamp": 5.417197872053168
+        },
+        {
+            "x": 1.3350584223241078,
+            "y": 5.537231311212578,
+            "heading": -3.091661499454674,
+            "angularVelocity": -2.18835183600471,
+            "velocityX": -0.15400295944407288,
+            "velocityY": -0.4460442151491003,
+            "timestamp": 5.459929849957341
+        },
+        {
+            "x": 1.332138180732727,
+            "y": 5.52810525894165,
+            "heading": -3.1411853071795863,
+            "angularVelocity": -1.1589402165275682,
+            "velocityX": -0.06833855427730316,
+            "velocityY": -0.21356493938549467,
+            "timestamp": 5.502661827861513
+        },
+        {
+            "x": 1.332138180732727,
+            "y": 5.52810525894165,
+            "heading": -3.1411853071795863,
+            "angularVelocity": 0,
+            "velocityX": 4.554923410438002e-36,
+            "velocityY": -5.3517654956150865e-34,
+            "timestamp": 5.545393805765686
+        },
+        {
+            "x": 1.3511823216382217,
+            "y": 5.531961266421083,
+            "heading": -3.038798036456765,
+            "angularVelocity": 1.6934181625027336,
+            "velocityX": 0.3149775735885273,
+            "velocityY": 0.06377582930300447,
+            "timestamp": 5.60585570052386
+        },
+        {
+            "x": 1.3908490268299478,
+            "y": 5.542189708843604,
+            "heading": -2.844785530962286,
+            "angularVelocity": 3.2088393238494826,
+            "velocityX": 0.6560612324568816,
+            "velocityY": 0.16917171490292857,
+            "timestamp": 5.666317595282035
+        },
+        {
+            "x": 1.4539728254946123,
+            "y": 5.56127979063472,
+            "heading": -2.581938269596155,
+            "angularVelocity": 4.34732094350375,
+            "velocityX": 1.0440261410453116,
+            "velocityY": 0.3157374056415134,
+            "timestamp": 5.726779490040209
+        },
+        {
+            "x": 1.5450944631009007,
+            "y": 5.588995277628573,
+            "heading": -2.2890493808444536,
+            "angularVelocity": 4.8441897152438,
+            "velocityX": 1.5070919952267785,
+            "velocityY": 0.45839593854450544,
+            "timestamp": 5.787241384798383
+        },
+        {
+            "x": 1.6638598735645596,
+            "y": 5.62434991056183,
+            "heading": -2.0324634607383167,
+            "angularVelocity": 4.24376247440454,
+            "velocityX": 1.9643018290888397,
+            "velocityY": 0.5847423914626325,
+            "timestamp": 5.8477032795565576
+        },
+        {
+            "x": 1.8083021892177116,
+            "y": 5.6638944259582,
+            "heading": -1.8350759652142852,
+            "angularVelocity": 3.264659440685901,
+            "velocityX": 2.3889809644714033,
+            "velocityY": 0.6540402935523977,
+            "timestamp": 5.908165174314732
+        },
+        {
+            "x": 1.978235650109889,
+            "y": 5.709112472816866,
+            "heading": -1.6974261571036708,
+            "angularVelocity": 2.276637354174287,
+            "velocityX": 2.8105877523661738,
+            "velocityY": 0.7478767749426485,
+            "timestamp": 5.968627069072906
+        },
+        {
+            "x": 2.1727729031197174,
+            "y": 5.761282618111588,
+            "heading": -1.6235540862695264,
+            "angularVelocity": 1.2217954982986527,
+            "velocityX": 3.217518302856812,
+            "velocityY": 0.8628599137255631,
+            "timestamp": 6.0290889638310805
+        },
+        {
+            "x": 2.3909229420587215,
+            "y": 5.821425961949498,
+            "heading": -1.6168784459669248,
+            "angularVelocity": 0.1104107029609625,
+            "velocityX": 3.60805826234069,
+            "velocityY": 0.994731377150211,
+            "timestamp": 6.089550858589255
+        },
+        {
+            "x": 2.6085958376613507,
+            "y": 5.891661539060254,
+            "heading": -1.616878243648269,
+            "angularVelocity": 0.0000033462175924113523,
+            "velocityX": 3.6001666251651883,
+            "velocityY": 1.1616502822426138,
+            "timestamp": 6.150012753347429
+        },
+        {
+            "x": 2.826268683292055,
+            "y": 5.961897271043331,
+            "heading": -1.6168780413299741,
+            "angularVelocity": 0.0000033462116195770168,
+            "velocityX": 3.6001657986623896,
+            "velocityY": 1.1616528437290037,
+            "timestamp": 6.2104746481056035
+        },
+        {
+            "x": 3.0439415289220437,
+            "y": 6.032133003028614,
+            "heading": -1.6168778390116512,
+            "angularVelocity": 0.0000033462120828803855,
+            "velocityX": 3.6001657986505493,
+            "velocityY": 1.1616528437654758,
+            "timestamp": 6.270936542863778
+        },
+        {
+            "x": 3.2616143745520296,
+            "y": 6.102368735013889,
+            "heading": -1.6168776366933004,
+            "angularVelocity": 0.000003346212545975565,
+            "velocityX": 3.600165798650516,
+            "velocityY": 1.1616528437653553,
+            "timestamp": 6.331398437621952
+        },
+        {
+            "x": 3.479287220182014,
+            "y": 6.172604466999156,
+            "heading": -1.6168774343749215,
+            "angularVelocity": 0.0000033462130098357135,
+            "velocityX": 3.6001657986504836,
+            "velocityY": 1.1616528437652343,
+            "timestamp": 6.391860332380126
+        },
+        {
+            "x": 3.6969600658119965,
+            "y": 6.242840198984417,
+            "heading": -1.6168772320565146,
+            "angularVelocity": 0.000003346213473053834,
+            "velocityX": 3.6001657986504503,
+            "velocityY": 1.1616528437651126,
+            "timestamp": 6.452322227138301
+        },
+        {
+            "x": 3.914632911441977,
+            "y": 6.313075930969669,
+            "heading": -1.61687702973808,
+            "angularVelocity": 0.000003346213936555426,
+            "velocityX": 3.6001657986504174,
+            "velocityY": 1.1616528437649918,
+            "timestamp": 6.512784121896475
+        },
+        {
+            "x": 4.132305757071955,
+            "y": 6.383311662954915,
+            "heading": -1.616876827419617,
+            "angularVelocity": 0.0000033462143992467285,
+            "velocityX": 3.6001657986503846,
+            "velocityY": 1.161652843764871,
+            "timestamp": 6.573246016654649
+        },
+        {
+            "x": 4.349978602701932,
+            "y": 6.453547394940153,
+            "heading": -1.6168766251011262,
+            "angularVelocity": 0.000003346214863174556,
+            "velocityX": 3.600165798650352,
+            "velocityY": 1.1616528437647493,
+            "timestamp": 6.633707911412824
+        },
+        {
+            "x": 4.567651448331906,
+            "y": 6.523783126925384,
+            "heading": -1.6168764227826073,
+            "angularVelocity": 0.000003346215326740712,
+            "velocityX": 3.6001657986503193,
+            "velocityY": 1.1616528437646283,
+            "timestamp": 6.694169806170998
+        },
+        {
+            "x": 4.785324293961879,
+            "y": 6.594018858910608,
+            "heading": -1.6168762204640603,
+            "angularVelocity": 0.0000033462157905377784,
+            "velocityX": 3.6001657986502864,
+            "velocityY": 1.161652843764507,
+            "timestamp": 6.754631700929172
+        },
+        {
+            "x": 5.002997139591849,
+            "y": 6.6642545908958235,
+            "heading": -1.6168760181454853,
+            "angularVelocity": 0.0000033462162531242337,
+            "velocityX": 3.600165798650253,
+            "velocityY": 1.1616528437643858,
+            "timestamp": 6.815093595687347
+        },
+        {
+            "x": 5.220669985221817,
+            "y": 6.734490322881032,
+            "heading": -1.6168758158268823,
+            "angularVelocity": 0.000003346216717473229,
+            "velocityX": 3.6001657986502207,
+            "velocityY": 1.1616528437642646,
+            "timestamp": 6.875555490445521
+        },
+        {
+            "x": 5.438342830851783,
+            "y": 6.804726054866234,
+            "heading": -1.6168756135082514,
+            "angularVelocity": 0.000003346217180660181,
+            "velocityX": 3.600165798650188,
+            "velocityY": 1.1616528437641434,
+            "timestamp": 6.936017385203695
+        },
+        {
+            "x": 5.656015676481748,
+            "y": 6.874961786851428,
+            "heading": -1.6168754111895922,
+            "angularVelocity": 0.0000033462176438124147,
+            "velocityX": 3.600165798650156,
+            "velocityY": 1.1616528437640228,
+            "timestamp": 6.99647927996187
+        },
+        {
+            "x": 5.873688522111711,
+            "y": 6.945197518836615,
+            "heading": -1.6168752088709053,
+            "angularVelocity": 0.0000033462181070326184,
+            "velocityX": 3.6001657986501225,
+            "velocityY": 1.161652843763901,
+            "timestamp": 7.056941174720044
+        },
+        {
+            "x": 6.091361367741672,
+            "y": 7.015433250821794,
+            "heading": -1.6168750065521902,
+            "angularVelocity": 0.0000033462185712785595,
+            "velocityX": 3.6001657986500892,
+            "velocityY": 1.1616528437637803,
+            "timestamp": 7.117403069478218
+        },
+        {
+            "x": 6.30903421337163,
+            "y": 7.0856689828069666,
+            "heading": -1.616874804233447,
+            "angularVelocity": 0.0000033462190342827524,
+            "velocityX": 3.600165798650057,
+            "velocityY": 1.1616528437636586,
+            "timestamp": 7.1778649642363925
+        },
+        {
+            "x": 6.526707059002331,
+            "y": 7.155904714789823,
+            "heading": -1.6168746019146758,
+            "angularVelocity": 0.0000033462194986739715,
+            "velocityX": 3.6001657986623425,
+            "velocityY": 1.1616528437253608,
+            "timestamp": 7.238326858994567
+        },
+        {
+            "x": 6.744379956765645,
+            "y": 7.22614028520157,
+            "heading": -1.6168743995934785,
+            "angularVelocity": 0.0000033462596273946,
+            "velocityX": 3.600166660901485,
+            "velocityY": 1.1616501714453904,
+            "timestamp": 7.298788753752741
+        },
+        {
+            "x": 6.948380246935694,
+            "y": 7.286306890939545,
+            "heading": -1.572684837353693,
+            "angularVelocity": 0.7308663153301367,
+            "velocityX": 3.37403071779302,
+            "velocityY": 0.9951161136881308,
+            "timestamp": 7.3592506485109155
+        },
+        {
+            "x": 7.128326014252639,
+            "y": 7.34087027160758,
+            "heading": -1.456360393496805,
+            "angularVelocity": 1.9239298457672127,
+            "velocityX": 2.976184719924214,
+            "velocityY": 0.9024424538177037,
+            "timestamp": 7.41971254326909
+        },
+        {
+            "x": 7.285215733567768,
+            "y": 7.391288256354559,
+            "heading": -1.2598604932432955,
+            "angularVelocity": 3.249979198294029,
+            "velocityX": 2.5948528398362565,
+            "velocityY": 0.8338803298942721,
+            "timestamp": 7.480174438027264
+        },
+        {
+            "x": 7.415669802167391,
+            "y": 7.433594551714448,
+            "heading": -1.014978314704827,
+            "angularVelocity": 4.050190281298798,
+            "velocityX": 2.157624552147962,
+            "velocityY": 0.6997183189362131,
+            "timestamp": 7.540636332785438
+        },
+        {
+            "x": 7.52162527778786,
+            "y": 7.463369081831009,
+            "heading": -0.7214685317692553,
+            "angularVelocity": 4.854458896954922,
+            "velocityX": 1.7524339262646644,
+            "velocityY": 0.49245115846350496,
+            "timestamp": 7.601098227543613
+        },
+        {
+            "x": 7.599835823210802,
+            "y": 7.482093660895173,
+            "heading": -0.44961761921846705,
+            "angularVelocity": 4.4962354163410385,
+            "velocityX": 1.293551016483298,
+            "velocityY": 0.30969223076874997,
+            "timestamp": 7.661560122301787
+        },
+        {
+            "x": 7.6517032735458015,
+            "y": 7.492160163258041,
+            "heading": -0.22315227789287845,
+            "angularVelocity": 3.7455878984832975,
+            "velocityX": 0.8578535380416231,
+            "velocityY": 0.16649333275330752,
+            "timestamp": 7.722022017059961
+        },
+        {
+            "x": 7.67973874704759,
+            "y": 7.4966228089126075,
+            "heading": -0.06798236605855336,
+            "angularVelocity": 2.5664083544676926,
+            "velocityX": 0.46368830507082526,
+            "velocityY": 0.07380922599953843,
+            "timestamp": 7.782483911818136
+        },
+        {
+            "x": 7.686149597167969,
+            "y": 7.496679306030273,
+            "heading": 0,
+            "angularVelocity": 1.1243836523889659,
+            "velocityX": 0.10603124738348535,
+            "velocityY": 0.0009344251927952234,
+            "timestamp": 7.84294580657631
+        },
+        {
+            "x": 7.671082178881759,
+            "y": 7.493118035056542,
+            "heading": -0.029242529216573936,
+            "angularVelocity": -0.471080674899979,
+            "velocityX": -0.24272762190641095,
+            "velocityY": -0.05737006951015711,
+            "timestamp": 7.905021223324796
+        },
+        {
+            "x": 7.633475390199317,
+            "y": 7.486720174316825,
+            "heading": -0.1516714089178372,
+            "angularVelocity": -1.9722602942371796,
+            "velocityX": -0.6058241837475781,
+            "velocityY": -0.1030659329383018,
+            "timestamp": 7.967096640073281
+        },
+        {
+            "x": 7.569735009380553,
+            "y": 7.475264417263962,
+            "heading": -0.33932636884787254,
+            "angularVelocity": -3.023015708301534,
+            "velocityX": -1.0268216333854858,
+            "velocityY": -0.1845457936960604,
+            "timestamp": 8.029172056821766
+        },
+        {
+            "x": 7.480376336028035,
+            "y": 7.452059634472421,
+            "heading": -0.582197860853168,
+            "angularVelocity": -3.912522939464948,
+            "velocityX": -1.4395178966671722,
+            "velocityY": -0.37381598073777733,
+            "timestamp": 8.09124747357025
+        },
+        {
+            "x": 7.362878563951424,
+            "y": 7.417380486057705,
+            "heading": -0.863364111736863,
+            "angularVelocity": -4.529429935571964,
+            "velocityX": -1.8928229278376825,
+            "velocityY": -0.5586615480203228,
+            "timestamp": 8.153322890318735
+        },
+        {
+            "x": 7.218864923659752,
+            "y": 7.37005336377927,
+            "heading": -1.0948198175456743,
+            "angularVelocity": -3.7286210537516657,
+            "velocityX": -2.319978629788013,
+            "velocityY": -0.7624132830262488,
+            "timestamp": 8.21539830706722
+        },
+        {
+            "x": 7.047313606455814,
+            "y": 7.316250565275482,
+            "heading": -1.2668868462563043,
+            "angularVelocity": -2.7719029162189006,
+            "velocityX": -2.7635950943192666,
+            "velocityY": -0.8667327796087775,
+            "timestamp": 8.277473723815705
+        },
+        {
+            "x": 6.850682482856188,
+            "y": 7.25802494172709,
+            "heading": -1.3587604988602537,
+            "angularVelocity": -1.4800327958521704,
+            "velocityX": -3.16761664921765,
+            "velocityY": -0.9379819999325716,
+            "timestamp": 8.33954914056419
+        },
+        {
+            "x": 6.62975279957275,
+            "y": 7.191687511601611,
+            "heading": -1.3718894789288036,
+            "angularVelocity": -0.21150047404023747,
+            "velocityX": -3.5590527596228974,
+            "velocityY": -1.0686586349353433,
+            "timestamp": 8.401624557312674
+        },
+        {
+            "x": 6.40649462388328,
+            "y": 7.118888182289737,
+            "heading": -1.3718898557853474,
+            "angularVelocity": -0.000006070946652807348,
+            "velocityX": -3.596563460766736,
+            "velocityY": -1.1727561911801452,
+            "timestamp": 8.463699974061159
+        },
+        {
+            "x": 6.183236472249506,
+            "y": 7.0460887792034885,
+            "heading": -1.3718902326404057,
+            "angularVelocity": -0.000006070922729380904,
+            "velocityX": -3.5965630732430243,
+            "velocityY": -1.1727573796437725,
+            "timestamp": 8.525775390809644
+        },
+        {
+            "x": 5.959978320616,
+            "y": 6.973289376116265,
+            "heading": -1.3718906094952235,
+            "angularVelocity": -0.000006070918851907899,
+            "velocityX": -3.5965630732387113,
+            "velocityY": -1.172757379659484,
+            "timestamp": 8.587850807558128
+        },
+        {
+            "x": 5.736720168982443,
+            "y": 6.900489973029042,
+            "heading": -1.3718909863498006,
+            "angularVelocity": -0.000006070914972563024,
+            "velocityX": -3.596563073239526,
+            "velocityY": -1.1727573796594701,
+            "timestamp": 8.649926224306613
+        },
+        {
+            "x": 5.513462017348836,
+            "y": 6.8276905699418196,
+            "heading": -1.3718913632041367,
+            "angularVelocity": -0.000006070911095039567,
+            "velocityX": -3.5965630732403415,
+            "velocityY": -1.1727573796594566,
+            "timestamp": 8.712001641055098
+        },
+        {
+            "x": 5.290203865715178,
+            "y": 6.7548911668545975,
+            "heading": -1.3718917400582324,
+            "angularVelocity": -0.0000060709072179749485,
+            "velocityX": -3.596563073241156,
+            "velocityY": -1.1727573796594426,
+            "timestamp": 8.774077057803582
+        },
+        {
+            "x": 5.066945714081469,
+            "y": 6.682091763767377,
+            "heading": -1.3718921169120872,
+            "angularVelocity": -0.000006070903338953198,
+            "velocityX": -3.596563073241971,
+            "velocityY": -1.1727573796594288,
+            "timestamp": 8.836152474552067
+        },
+        {
+            "x": 4.84368756244771,
+            "y": 6.609292360680157,
+            "heading": -1.3718924937657013,
+            "angularVelocity": -0.000006070899462290803,
+            "velocityX": -3.596563073242785,
+            "velocityY": -1.1727573796594153,
+            "timestamp": 8.898227891300552
+        },
+        {
+            "x": 4.620429410813901,
+            "y": 6.536492957592937,
+            "heading": -1.371892870619075,
+            "angularVelocity": -0.000006070895585804876,
+            "velocityX": -3.5965630732436003,
+            "velocityY": -1.1727573796594017,
+            "timestamp": 8.960303308049037
+        },
+        {
+            "x": 4.39717125918004,
+            "y": 6.46369355450572,
+            "heading": -1.3718932474722079,
+            "angularVelocity": -0.000006070891709350806,
+            "velocityX": -3.5965630732444147,
+            "velocityY": -1.172757379659388,
+            "timestamp": 9.022378724797521
+        },
+        {
+            "x": 4.173913107546129,
+            "y": 6.390894151418502,
+            "heading": -1.3718936243251003,
+            "angularVelocity": -0.000006070887832958865,
+            "velocityX": -3.5965630732452283,
+            "velocityY": -1.1727573796593747,
+            "timestamp": 9.084454141546006
+        },
+        {
+            "x": 3.950654955912168,
+            "y": 6.318094748331286,
+            "heading": -1.3718940011777518,
+            "angularVelocity": -0.000006070883956378075,
+            "velocityX": -3.596563073246043,
+            "velocityY": -1.1727573796593613,
+            "timestamp": 9.14652955829449
+        },
+        {
+            "x": 3.7273968042781562,
+            "y": 6.24529534524407,
+            "heading": -1.3718943780301627,
+            "angularVelocity": -0.000006070880081532027,
+            "velocityX": -3.5965630732468568,
+            "velocityY": -1.1727573796593478,
+            "timestamp": 9.208604975042975
+        },
+        {
+            "x": 3.504138652644094,
+            "y": 6.172495942156855,
+            "heading": -1.3718947548823335,
+            "angularVelocity": -0.000006070876206601658,
+            "velocityX": -3.5965630732476703,
+            "velocityY": -1.1727573796593347,
+            "timestamp": 9.27068039179146
+        },
+        {
+            "x": 3.2808805010099813,
+            "y": 6.099696539069641,
+            "heading": -1.3718951317342634,
+            "angularVelocity": -0.000006070872330584258,
+            "velocityX": -3.596563073248485,
+            "velocityY": -1.1727573796593216,
+            "timestamp": 9.332755808539945
+        },
+        {
+            "x": 3.057622349375818,
+            "y": 6.026897135982428,
+            "heading": -1.371895508585953,
+            "angularVelocity": -0.000006070868457801708,
+            "velocityX": -3.5965630732492984,
+            "velocityY": -1.1727573796593085,
+            "timestamp": 9.39483122528843
+        },
+        {
+            "x": 2.834364197741604,
+            "y": 5.954097732895216,
+            "heading": -1.371895885437402,
+            "angularVelocity": -0.0000060708645841945965,
+            "velocityX": -3.596563073250112,
+            "velocityY": -1.1727573796592947,
+            "timestamp": 9.456906642036914
+        },
+        {
+            "x": 2.6111060461064435,
+            "y": 5.881298329810753,
+            "heading": -1.3718962622886106,
+            "angularVelocity": -0.000006070860710280736,
+            "velocityX": -3.5965630732653646,
+            "velocityY": -1.1727573796150002,
+            "timestamp": 9.518982058785399
+        },
+        {
+            "x": 2.387847826730136,
+            "y": 5.808499134475022,
+            "heading": -1.37189663914262,
+            "angularVelocity": -0.000006070905830150403,
+            "velocityX": -3.5965641645370763,
+            "velocityY": -1.1727540328999504,
+            "timestamp": 9.581057475533884
+        },
+        {
+            "x": 2.167695143748949,
+            "y": 5.749324083617281,
+            "heading": -1.3936267357407348,
+            "angularVelocity": -0.3500596167748615,
+            "velocityX": -3.546535722397048,
+            "velocityY": -0.9532767391237054,
+            "timestamp": 9.643132892282368
+        },
+        {
+            "x": 1.9716359535895844,
+            "y": 5.700085594404985,
+            "heading": -1.4887110872196767,
+            "angularVelocity": -1.5317553463104556,
+            "velocityX": -3.1584031236350607,
+            "velocityY": -0.7932043277582467,
+            "timestamp": 9.705208309030853
+        },
+        {
+            "x": 1.8021216559438722,
+            "y": 5.6575856667748505,
+            "heading": -1.6480423798805228,
+            "angularVelocity": -2.5667373818273083,
+            "velocityX": -2.73077985658225,
+            "velocityY": -0.6846498961470444,
+            "timestamp": 9.767283725779338
+        },
+        {
+            "x": 1.6601304337176481,
+            "y": 5.619404991825654,
+            "heading": -1.8664040726402535,
+            "angularVelocity": -3.5176838786999975,
+            "velocityX": -2.2873986138754163,
+            "velocityY": -0.6150691682650391,
+            "timestamp": 9.829359142527823
+        },
+        {
+            "x": 1.5437233022892554,
+            "y": 5.5885120196672124,
+            "heading": -2.146539077662866,
+            "angularVelocity": -4.5128171455965,
+            "velocityX": -1.8752533212953901,
+            "velocityY": -0.49766838108574224,
+            "timestamp": 9.891434559276307
+        },
+        {
+            "x": 1.452238647018182,
+            "y": 5.5675498489404625,
+            "heading": -2.4956493740995165,
+            "angularVelocity": -5.623970240121965,
+            "velocityX": -1.4737662679212806,
+            "velocityY": -0.33768876351944366,
+            "timestamp": 9.953509976024792
+        },
+        {
+            "x": 1.3894112242957146,
+            "y": 5.555821056487952,
+            "heading": -2.802567916985108,
+            "angularVelocity": -4.94428485481059,
+            "velocityX": -1.0121143926754297,
+            "velocityY": -0.188944240197889,
+            "timestamp": 10.015585392773277
+        },
+        {
+            "x": 1.3504462657255363,
+            "y": 5.550087016097331,
+            "heading": -3.0243894075867264,
+            "angularVelocity": -3.573419273210609,
+            "velocityX": -0.6277035356533314,
+            "velocityY": -0.09237216100947475,
+            "timestamp": 10.077660809521761
+        },
+        {
+            "x": 1.3321380615234375,
+            "y": 5.547595977783203,
+            "heading": -3.1411853071795863,
+            "angularVelocity": -1.8815161574522963,
+            "velocityX": -0.2949348576470964,
+            "velocityY": -0.040129224169711215,
+            "timestamp": 10.139736226270246
+        },
+        {
+            "x": 1.3321380615234375,
+            "y": 5.547595977783203,
+            "heading": -3.1411853071795863,
+            "angularVelocity": 7.133789284871957e-33,
+            "velocityX": 0,
+            "velocityY": 1.0797204950028118e-32,
+            "timestamp": 10.201811643018731
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/autos/auto1.auto b/src/main/deploy/pathplanner/autos/auto1.auto
new file mode 100644
index 00000000..3c131b0f
--- /dev/null
+++ b/src/main/deploy/pathplanner/autos/auto1.auto
@@ -0,0 +1,135 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 1.3321385383605957,
+      "y": 5.547596454620361
+    },
+    "rotation": 0.0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "deadline",
+          "data": {
+            "commands": [
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "auto1.1"
+                }
+              },
+              {
+                "type": "sequential",
+                "data": {
+                  "commands": [
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.0
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "intake"
+                      }
+                    },
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.0
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "fender"
+                      }
+                    },
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.0
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "intake"
+                      }
+                    },
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.0
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "fender"
+                      }
+                    },
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.0
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "intake"
+                      }
+                    },
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 1.0
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "fender"
+                      }
+                    },
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 2.1
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "intake"
+                      }
+                    },
+                    {
+                      "type": "wait",
+                      "data": {
+                        "waitTime": 2.1
+                      }
+                    },
+                    {
+                      "type": "named",
+                      "data": {
+                        "name": "fender"
+                      }
+                    }
+                  ]
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
index 893b9e28..bab0da93 100644
--- a/src/main/deploy/pathplanner/navgrid.json
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -1 +1 @@
-{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
+{"field_size":{"x":16.54,"y":8.21},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true],[true,true,true,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,true,true,true,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file

From c440621a2f02e0d5c3cc5da1da7f0502bc753743 Mon Sep 17 00:00:00 2001
From: Lewis-Seiden <lewy@seiden.us>
Date: Sat, 10 Feb 2024 17:47:41 -0800
Subject: [PATCH 05/62] added teleop command bindings

---
 src/main/java/frc/robot/Robot.java            | 102 +++++++++++++++---
 .../carriage/CarriageSubsystem.java           |  27 ++++-
 .../elevator/ElevatorSubsystem.java           |  22 ++--
 .../subsystems/feeder/FeederSubsystem.java    |   4 +
 .../utils/CommandXboxControllerSubsystem.java |  27 +++++
 5 files changed, 156 insertions(+), 26 deletions(-)
 create mode 100644 src/main/java/frc/robot/utils/CommandXboxControllerSubsystem.java

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 0004206c..f2f59212 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -6,16 +6,17 @@
 
 import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.commands.PathPlannerAuto;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.subsystems.carriage.CarriageIOReal;
 import frc.robot.subsystems.carriage.CarriageSubsystem;
 import frc.robot.subsystems.elevator.ElevatorIOSim;
@@ -30,6 +31,7 @@
 import frc.robot.subsystems.swerve.GyroIO;
 import frc.robot.subsystems.swerve.GyroIOPigeon2;
 import frc.robot.subsystems.swerve.SwerveSubsystem;
+import frc.robot.utils.CommandXboxControllerSubsystem;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
@@ -44,10 +46,18 @@ public static enum RobotMode {
     REAL
   }
 
+  public static enum Target {
+    AMP,
+    SPEAKER
+  }
+
   public static final RobotMode mode = Robot.isReal() ? RobotMode.REAL : RobotMode.SIM;
   private Command autonomousCommand;
 
-  private final CommandXboxController controller = new CommandXboxController(0);
+  private final CommandXboxControllerSubsystem controller = new CommandXboxControllerSubsystem(0);
+  private final CommandXboxControllerSubsystem operator = new CommandXboxControllerSubsystem(1);
+
+  private Target currentTarget = Target.SPEAKER;
 
   private final SwerveSubsystem swerve =
       new SwerveSubsystem(
@@ -120,30 +130,85 @@ public void robotInit() {
         swerve.runVelocityFieldRelative(
             () ->
                 new ChassisSpeeds(
-                    -controller.getLeftY() * SwerveSubsystem.MAX_LINEAR_SPEED,
-                    -controller.getLeftX() * SwerveSubsystem.MAX_LINEAR_SPEED,
-                    -controller.getRightX() * SwerveSubsystem.MAX_ANGULAR_SPEED)));
+                    -teleopAxisAdjustment(controller.getLeftY()) * SwerveSubsystem.MAX_LINEAR_SPEED,
+                    -teleopAxisAdjustment(controller.getLeftX()) * SwerveSubsystem.MAX_LINEAR_SPEED,
+                    -teleopAxisAdjustment(controller.getRightX())
+                        * SwerveSubsystem.MAX_ANGULAR_SPEED)));
     elevator.setDefaultCommand(elevator.setExtensionCmd(() -> 0.0));
     feeder.setDefaultCommand(feeder.runVoltageCmd(0.0));
+    carriage.setDefaultCommand(carriage.runVoltageCmd(0.0));
     intake.setDefaultCommand(intake.runVoltageCmd(10.0));
     shooter.setDefaultCommand(shooter.runStateCmd(Rotation2d.fromDegrees(0.0), 0.0, 0.0));
 
-    // Controller bindings here
+    // Robot State management bindings
+    new Trigger(() -> carriage.getBeambreak() || feeder.getFirstBeambreak())
+        .whileTrue(
+            intake.runVoltageCmd(0.0).withInterruptBehavior(InterruptionBehavior.kCancelSelf));
+    new Trigger(() -> currentTarget == Target.SPEAKER)
+        .whileTrue(Commands.parallel(carriage.runVoltageCmd(5.0), feeder.indexCmd()));
+    new Trigger(() -> currentTarget == Target.AMP)
+        .whileTrue(
+            Commands.either(
+                Commands.parallel(carriage.indexBackwardsCmd(), feeder.runVoltageCmd(-5.0)),
+                Commands.parallel(carriage.indexForwardsCmd(), feeder.runVoltageCmd(0.0)),
+                () -> feeder.getFirstBeambreak()));
+
+    // ---- Controller bindings here ----
+    controller.leftTrigger().whileTrue(intake.runVoltageCmd(10.0));
     controller
         .rightTrigger()
-        .whileTrue(shooter.runStateCmd(Rotation2d.fromDegrees(45.0), 50.0, 40.0));
-    controller.start().onTrue(Commands.runOnce(() -> swerve.setYaw(Rotation2d.fromDegrees(0))));
-
-    // Test binding for autoaim
+        .and(() -> currentTarget == Target.SPEAKER)
+        .whileTrue(
+            Commands.parallel(
+                shooter.runStateCmd(Rotation2d.fromDegrees(80.0), 50.0, 40.0),
+                Commands.waitSeconds(0.5).andThen(feeder.runVoltageCmd(3.0))));
+    controller
+        .rightTrigger()
+        .and(() -> currentTarget == Target.AMP)
+        .whileTrue(
+            elevator.setExtensionCmd(() -> ElevatorSubsystem.AMP_EXTENSION_METERS)
+            )
+        .onFalse(Commands.parallel(
+          carriage.runVoltageCmd(-3.0),
+          elevator.setExtensionCmd(() -> ElevatorSubsystem.AMP_EXTENSION_METERS)
+        ).withTimeout(0.5));
+    controller.rightBumper().whileTrue(swerve.stopWithXCmd());
+    // Heading reset
+    controller
+        .leftStick()
+        .and(controller.rightStick())
+        .onTrue(Commands.runOnce(() -> swerve.setYaw(new Rotation2d())));
     controller
         .a()
         .whileTrue(
             swerve.pointTowardsTranslation(
-                () -> -controller.getLeftY() * SwerveSubsystem.MAX_LINEAR_SPEED,
-                () -> -controller.getLeftX() * SwerveSubsystem.MAX_LINEAR_SPEED));
-    // Test binding for elevator
-    controller.b().whileTrue(elevator.setExtensionCmd(() -> 0.5));
-    controller.x().whileTrue(elevator.setExtensionCmd(() -> Units.inchesToMeters(30.0)));
+                () ->
+                    -teleopAxisAdjustment(controller.getLeftY()) * SwerveSubsystem.MAX_LINEAR_SPEED,
+                () ->
+                    -teleopAxisAdjustment(controller.getLeftX())
+                        * SwerveSubsystem.MAX_LINEAR_SPEED));
+    controller
+        .y()
+        .and(() -> elevator.getExtensionMeters() > 0.9 * ElevatorSubsystem.CLIMB_EXTENSION_METERS)
+        .onTrue(
+            Commands.sequence(
+                elevator
+                    .setExtensionCmd(() -> 0.0)
+                    .until(() -> elevator.getExtensionMeters() < 0.05),
+                Commands.parallel(
+                    elevator.setExtensionCmd(() -> ElevatorSubsystem.TRAP_EXTENSION_METERS),
+                    Commands.waitUntil(() -> elevator.getExtensionMeters() > 0.95 * ElevatorSubsystem.TRAP_EXTENSION_METERS)
+                        .andThen(carriage.runVoltageCmd(-3.0)))));
+
+    // Prep climb
+    operator
+        .rightTrigger(0.5)
+        .and(operator.rightBumper())
+        .toggleOnFalse(
+            Commands.parallel(
+                elevator.setExtensionCmd(() -> ElevatorSubsystem.CLIMB_EXTENSION_METERS)));
+    operator.leftTrigger().onTrue(Commands.runOnce(() -> currentTarget = Target.SPEAKER));
+    operator.leftBumper().onTrue(Commands.runOnce(() -> currentTarget = Target.AMP));
 
     NamedCommands.registerCommand("stop", swerve.stopWithXCmd().asProxy());
   }
@@ -185,4 +250,9 @@ public void teleopPeriodic() {}
   public void testInit() {
     CommandScheduler.getInstance().cancelAll();
   }
+
+  /** Modifies the given joystick axis value to make teleop driving smoother. */
+  private static double teleopAxisAdjustment(double x) {
+    return MathUtil.applyDeadband(Math.abs(Math.pow(x, 2)) * Math.signum(x), 0.02);
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/carriage/CarriageSubsystem.java b/src/main/java/frc/robot/subsystems/carriage/CarriageSubsystem.java
index 08a156ca..1b031de4 100644
--- a/src/main/java/frc/robot/subsystems/carriage/CarriageSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/carriage/CarriageSubsystem.java
@@ -5,11 +5,14 @@
 package frc.robot.subsystems.carriage;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.littletonrobotics.junction.Logger;
 
 /** Drainpipe style amp/trap mechanism on the elevator */
 public class CarriageSubsystem extends SubsystemBase {
+  public static final double INDEXING_VOLTAGE = 3.0;
+
   final CarriageIO io;
   final CarriageIOInputsAutoLogged inputs = new CarriageIOInputsAutoLogged();
 
@@ -29,8 +32,26 @@ public Command runVoltageCmd(double voltage) {
     return this.run(() -> io.setVoltage(voltage));
   }
 
-  /** Run the amp mech until the note is triggering the beambreak */
-  public Command index() {
-    return this.run(() -> runVoltageCmd(3.0)).until(() -> inputs.beambreak);
+  /**
+   * Run the amp mech forward until the note is triggering the beambreak, then jog it a little
+   * further forward.
+   */
+  public Command indexForwardsCmd() {
+    return runVoltageCmd(INDEXING_VOLTAGE)
+        .until(() -> inputs.beambreak)
+        .andThen(runVoltageCmd(INDEXING_VOLTAGE).withTimeout(0.25));
+  }
+
+  /** Run the amp mech backwards until the beambreak cycles, then forward index. */
+  public Command indexBackwardsCmd() {
+    return Commands.sequence(
+        runVoltageCmd(-INDEXING_VOLTAGE).until(() -> inputs.beambreak),
+        runVoltageCmd(-INDEXING_VOLTAGE).withTimeout(0.25),
+        runVoltageCmd(-INDEXING_VOLTAGE).until(() -> !inputs.beambreak),
+        indexForwardsCmd());
+  }
+
+  public boolean getBeambreak() {
+    return inputs.beambreak;
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java
index 8b482993..8d7f6a5b 100644
--- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java
@@ -22,7 +22,11 @@ public class ElevatorSubsystem extends SubsystemBase {
   // TODO find real values
   public static final double GEAR_RATIO = 12.5 / 1.0;
   public static final double DRUM_RADIUS_METERS = Units.inchesToMeters(1.751 / 2.0);
-  public static Rotation2d ELEVATOR_ANGLE = Rotation2d.fromDegrees(65.0);
+  public static final Rotation2d ELEVATOR_ANGLE = Rotation2d.fromDegrees(65.0);
+
+  public static final double CLIMB_EXTENSION_METERS = 0.6;
+  public static final double TRAP_EXTENSION_METERS = 0.9;
+  public static final double AMP_EXTENSION_METERS = 0.6;
 
   private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged();
   private final ElevatorIO io;
@@ -54,6 +58,14 @@ public void periodic() {
     Logger.recordOutput("Elevator/Carriage Pose", getCarriagePose());
   }
 
+  public Command setExtensionCmd(DoubleSupplier meters) {
+    return this.run(
+        () -> {
+          io.setTarget(meters.getAsDouble());
+          Logger.recordOutput("Elevator/Setpoint", meters.getAsDouble());
+        });
+  }
+
   public Pose3d getCarriagePose() {
     return new Pose3d(
         Units.inchesToMeters(4.5) + carriage.getLength() * Math.cos(ELEVATOR_ANGLE.getRadians()),
@@ -72,11 +84,7 @@ public Pose3d getFirstStagePose() {
         new Rotation3d());
   }
 
-  public Command setExtensionCmd(DoubleSupplier meters) {
-    return this.run(
-        () -> {
-          io.setTarget(meters.getAsDouble());
-          Logger.recordOutput("Elevator/Setpoint", meters.getAsDouble());
-        });
+  public double getExtensionMeters() {
+    return inputs.elevatorPositionMeters;
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederSubsystem.java b/src/main/java/frc/robot/subsystems/feeder/FeederSubsystem.java
index 6b1bfbb9..ea77b741 100644
--- a/src/main/java/frc/robot/subsystems/feeder/FeederSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/feeder/FeederSubsystem.java
@@ -42,4 +42,8 @@ public Command indexCmd() {
           }
         });
   }
+
+  public boolean getFirstBeambreak() {
+    return inputs.firstBeambreak;
+  }
 }
diff --git a/src/main/java/frc/robot/utils/CommandXboxControllerSubsystem.java b/src/main/java/frc/robot/utils/CommandXboxControllerSubsystem.java
new file mode 100644
index 00000000..c2144a88
--- /dev/null
+++ b/src/main/java/frc/robot/utils/CommandXboxControllerSubsystem.java
@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.utils;
+
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import java.util.function.DoubleSupplier;
+
+/** A CommandXboxController that implements Subsystem to allow the rumble to be mutexed. */
+public class CommandXboxControllerSubsystem extends CommandXboxController implements Subsystem {
+
+  public CommandXboxControllerSubsystem(int port) {
+    super(port);
+  }
+
+  public Command rumble(DoubleSupplier left, DoubleSupplier right) {
+    return this.run(
+        () -> {
+          super.getHID().setRumble(RumbleType.kLeftRumble, left.getAsDouble());
+          super.getHID().setRumble(RumbleType.kRightRumble, right.getAsDouble());
+        });
+  }
+}

From 638ee32a2334e08e8abad2ed5ee42db20bef15f9 Mon Sep 17 00:00:00 2001
From: Lewis-Seiden <lewy@seiden.us>
Date: Sat, 10 Feb 2024 18:13:00 -0800
Subject: [PATCH 06/62] added rumble on pickup

---
 src/main/java/frc/robot/Robot.java            | 28 ++++++++++++-------
 .../utils/CommandXboxControllerSubsystem.java |  8 +++++-
 2 files changed, 25 insertions(+), 11 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index f2f59212..87bb80e5 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -140,10 +140,16 @@ public void robotInit() {
     intake.setDefaultCommand(intake.runVoltageCmd(10.0));
     shooter.setDefaultCommand(shooter.runStateCmd(Rotation2d.fromDegrees(0.0), 0.0, 0.0));
 
-    // Robot State management bindings
+    controller.setDefaultCommand(controller.rumbleCmd(0.0, 0.0));
+    operator.setDefaultCommand(operator.rumbleCmd(0.0, 0.0));
+
+    // Robot state management bindings
     new Trigger(() -> carriage.getBeambreak() || feeder.getFirstBeambreak())
+        .debounce(0.25)
         .whileTrue(
-            intake.runVoltageCmd(0.0).withInterruptBehavior(InterruptionBehavior.kCancelSelf));
+            Commands.parallel(
+                intake.runVoltageCmd(0.0).withInterruptBehavior(InterruptionBehavior.kCancelSelf),
+                controller.rumbleCmd(1.0, 1.0).withTimeout(0.25)));
     new Trigger(() -> currentTarget == Target.SPEAKER)
         .whileTrue(Commands.parallel(carriage.runVoltageCmd(5.0), feeder.indexCmd()));
     new Trigger(() -> currentTarget == Target.AMP)
@@ -165,13 +171,12 @@ public void robotInit() {
     controller
         .rightTrigger()
         .and(() -> currentTarget == Target.AMP)
-        .whileTrue(
-            elevator.setExtensionCmd(() -> ElevatorSubsystem.AMP_EXTENSION_METERS)
-            )
-        .onFalse(Commands.parallel(
-          carriage.runVoltageCmd(-3.0),
-          elevator.setExtensionCmd(() -> ElevatorSubsystem.AMP_EXTENSION_METERS)
-        ).withTimeout(0.5));
+        .whileTrue(elevator.setExtensionCmd(() -> ElevatorSubsystem.AMP_EXTENSION_METERS))
+        .onFalse(
+            Commands.parallel(
+                    carriage.runVoltageCmd(-3.0),
+                    elevator.setExtensionCmd(() -> ElevatorSubsystem.AMP_EXTENSION_METERS))
+                .withTimeout(0.5));
     controller.rightBumper().whileTrue(swerve.stopWithXCmd());
     // Heading reset
     controller
@@ -197,7 +202,10 @@ public void robotInit() {
                     .until(() -> elevator.getExtensionMeters() < 0.05),
                 Commands.parallel(
                     elevator.setExtensionCmd(() -> ElevatorSubsystem.TRAP_EXTENSION_METERS),
-                    Commands.waitUntil(() -> elevator.getExtensionMeters() > 0.95 * ElevatorSubsystem.TRAP_EXTENSION_METERS)
+                    Commands.waitUntil(
+                            () ->
+                                elevator.getExtensionMeters()
+                                    > 0.95 * ElevatorSubsystem.TRAP_EXTENSION_METERS)
                         .andThen(carriage.runVoltageCmd(-3.0)))));
 
     // Prep climb
diff --git a/src/main/java/frc/robot/utils/CommandXboxControllerSubsystem.java b/src/main/java/frc/robot/utils/CommandXboxControllerSubsystem.java
index c2144a88..417ad83a 100644
--- a/src/main/java/frc/robot/utils/CommandXboxControllerSubsystem.java
+++ b/src/main/java/frc/robot/utils/CommandXboxControllerSubsystem.java
@@ -17,11 +17,17 @@ public CommandXboxControllerSubsystem(int port) {
     super(port);
   }
 
-  public Command rumble(DoubleSupplier left, DoubleSupplier right) {
+  /** Rumble the controller at the specified power. */
+  public Command rumbleCmd(DoubleSupplier left, DoubleSupplier right) {
     return this.run(
         () -> {
           super.getHID().setRumble(RumbleType.kLeftRumble, left.getAsDouble());
           super.getHID().setRumble(RumbleType.kRightRumble, right.getAsDouble());
         });
   }
+
+  /** Rumble the controller at the specified power. */
+  public Command rumbleCmd(double left, double right) {
+    return rumbleCmd(() -> left, () -> right);
+  }
 }

From 7a8ba88b9a6bdef3d49f9fe132ea62e7558826c0 Mon Sep 17 00:00:00 2001
From: Lewis-Seiden <lewy@seiden.us>
Date: Sun, 11 Feb 2024 12:04:50 -0800
Subject: [PATCH 07/62] added flywheel idle

---
 src/main/java/frc/robot/Robot.java            |  7 ++++++-
 .../subsystems/shooter/ShooterSubystem.java   | 21 +++++++++++++------
 2 files changed, 21 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 87bb80e5..de709507 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -58,6 +58,7 @@ public static enum Target {
   private final CommandXboxControllerSubsystem operator = new CommandXboxControllerSubsystem(1);
 
   private Target currentTarget = Target.SPEAKER;
+  private double flywheelIdleSpeed = 1.0;
 
   private final SwerveSubsystem swerve =
       new SwerveSubsystem(
@@ -138,7 +139,7 @@ public void robotInit() {
     feeder.setDefaultCommand(feeder.runVoltageCmd(0.0));
     carriage.setDefaultCommand(carriage.runVoltageCmd(0.0));
     intake.setDefaultCommand(intake.runVoltageCmd(10.0));
-    shooter.setDefaultCommand(shooter.runStateCmd(Rotation2d.fromDegrees(0.0), 0.0, 0.0));
+    shooter.setDefaultCommand(shooter.runStateCmd(() -> Rotation2d.fromDegrees(0.0), () -> flywheelIdleSpeed, () -> flywheelIdleSpeed));
 
     controller.setDefaultCommand(controller.rumbleCmd(0.0, 0.0));
     operator.setDefaultCommand(operator.rumbleCmd(0.0, 0.0));
@@ -217,6 +218,10 @@ public void robotInit() {
                 elevator.setExtensionCmd(() -> ElevatorSubsystem.CLIMB_EXTENSION_METERS)));
     operator.leftTrigger().onTrue(Commands.runOnce(() -> currentTarget = Target.SPEAKER));
     operator.leftBumper().onTrue(Commands.runOnce(() -> currentTarget = Target.AMP));
+    operator.a().onTrue(Commands.runOnce(() -> flywheelIdleSpeed = 1.0));
+    operator.b().onTrue(Commands.runOnce(() -> flywheelIdleSpeed = 20.0));
+    operator.x().onTrue(Commands.runOnce(() -> flywheelIdleSpeed = 20.0));
+    operator.y().onTrue(Commands.runOnce(() -> flywheelIdleSpeed = 80.0));
 
     NamedCommands.registerCommand("stop", swerve.stopWithXCmd().asProxy());
   }
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java
index fcb2b523..fb168910 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java
@@ -9,8 +9,13 @@
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+import java.util.function.DoubleSupplier;
+
 import org.littletonrobotics.junction.Logger;
 
+import com.google.common.base.Supplier;
+
 public class ShooterSubystem extends SubsystemBase {
   public static final double PIVOT_RATIO = (27.0 / 1.0) * (48.0 / 22.0);
   public static final double FLYWHEEL_RATIO = 18.0 / 24.0;
@@ -45,17 +50,21 @@ public Pose3d getMechanismPose() {
         0.0437896, 0.0, 0.3274568, new Rotation3d(0.0, inputs.pivotRotation.getRadians(), 0.0));
   }
 
-  public Command runStateCmd(Rotation2d rotation, double left, double right) {
+  public Command runStateCmd(Supplier<Rotation2d> rotation, DoubleSupplier left, DoubleSupplier right) {
     return this.run(
         () -> {
-          Logger.recordOutput("Shooter/Left Velocity Setpoint", left);
-          Logger.recordOutput("Shooter/Right Velocity Setpoint", right);
-          Logger.recordOutput("Shooter/Rotation Setpoint", rotation);
-          io.setFlywheelVelocity(left, right);
-          io.setPivotSetpoint(rotation);
+          Logger.recordOutput("Shooter/Left Velocity Setpoint", left.getAsDouble());
+          Logger.recordOutput("Shooter/Right Velocity Setpoint", right.getAsDouble());
+          Logger.recordOutput("Shooter/Rotation Setpoint", rotation.get());
+          io.setFlywheelVelocity(left.getAsDouble(), right.getAsDouble());
+          io.setPivotSetpoint(rotation.get());
         });
   }
 
+  public Command runStateCmd(Rotation2d rotation, double left, double right) {
+    return runStateCmd(() -> rotation, () -> left, () -> right);
+  }
+
   public Command runFlywheelVoltageCmd(Rotation2d rotation, double voltage) {
     return this.run(
         () -> {

From ff81c2cbc3a128e4fab2ab28fc7601a472877183 Mon Sep 17 00:00:00 2001
From: Lewis-Seiden <lewy@seiden.us>
Date: Sun, 11 Feb 2024 12:42:14 -0800
Subject: [PATCH 08/62] added ledsubsystem

---
 src/main/java/frc/robot/Robot.java            |  9 ++-
 .../java/frc/robot/subsystems/leds/LEDIO.java | 25 ++++++
 .../frc/robot/subsystems/leds/LEDIOReal.java  | 32 ++++++++
 .../robot/subsystems/leds/LEDSubsystem.java   | 81 +++++++++++++++++++
 .../subsystems/shooter/ShooterSubystem.java   |  8 +-
 5 files changed, 149 insertions(+), 6 deletions(-)
 create mode 100644 src/main/java/frc/robot/subsystems/leds/LEDIO.java
 create mode 100644 src/main/java/frc/robot/subsystems/leds/LEDIOReal.java
 create mode 100644 src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 9bb750af..1a28c168 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -12,6 +12,7 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
+import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -25,6 +26,8 @@
 import frc.robot.subsystems.feeder.FeederSubsystem;
 import frc.robot.subsystems.intake.IntakeIOReal;
 import frc.robot.subsystems.intake.IntakeSubsystem;
+import frc.robot.subsystems.leds.LEDIOReal;
+import frc.robot.subsystems.leds.LEDSubsystem;
 import frc.robot.subsystems.reaction_bar_release.ReactionBarReleaseIOReal;
 import frc.robot.subsystems.reaction_bar_release.ReactionBarReleaseSubsystem;
 import frc.robot.subsystems.shooter.ShooterIOReal;
@@ -85,6 +88,7 @@ public void setYaw(Rotation2d yaw) {}
   private final CarriageSubsystem carriage = new CarriageSubsystem(new CarriageIOReal());
   private final ReactionBarReleaseSubsystem reactionBarRelease =
       new ReactionBarReleaseSubsystem(new ReactionBarReleaseIOReal());
+  private final LEDSubsystem leds = new LEDSubsystem(new LEDIOReal());
 
   @Override
   public void robotInit() {
@@ -143,9 +147,12 @@ public void robotInit() {
     feeder.setDefaultCommand(feeder.runVoltageCmd(0.0));
     carriage.setDefaultCommand(carriage.runVoltageCmd(0.0));
     intake.setDefaultCommand(intake.runVoltageCmd(10.0));
-    shooter.setDefaultCommand(shooter.runStateCmd(() -> Rotation2d.fromDegrees(0.0), () -> flywheelIdleSpeed, () -> flywheelIdleSpeed));
+    shooter.setDefaultCommand(
+        shooter.runStateCmd(
+            () -> Rotation2d.fromDegrees(0.0), () -> flywheelIdleSpeed, () -> flywheelIdleSpeed));
     reactionBarRelease.setDefaultCommand(
         reactionBarRelease.setRotationCmd(Rotation2d.fromDegrees(0.0)));
+    leds.setDefaultCommand(leds.setSolidCmd(new Color("#350868")));
 
     controller.setDefaultCommand(controller.rumbleCmd(0.0, 0.0));
     operator.setDefaultCommand(operator.rumbleCmd(0.0, 0.0));
diff --git a/src/main/java/frc/robot/subsystems/leds/LEDIO.java b/src/main/java/frc/robot/subsystems/leds/LEDIO.java
new file mode 100644
index 00000000..7049e6d6
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/leds/LEDIO.java
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems.leds;
+
+import edu.wpi.first.wpilibj.util.Color;
+import org.littletonrobotics.junction.AutoLog;
+
+public interface LEDIO {
+  @AutoLog
+  public static class LEDIOInputs {
+    // No sensors so no inputs
+  }
+
+  public void updateInputs(LEDIOInputs inputs);
+
+  public void set(int i, Color color);
+
+  public default void solid(Color color) {
+    for (int i = 0; i < LEDSubsystem.LED_LENGTH; i++) {
+      set(i, color);
+    }
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/leds/LEDIOReal.java b/src/main/java/frc/robot/subsystems/leds/LEDIOReal.java
new file mode 100644
index 00000000..dfee71b1
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/leds/LEDIOReal.java
@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems.leds;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.util.Color;
+
+public class LEDIOReal implements LEDIO {
+  AddressableLED led;
+  AddressableLEDBuffer buffer;
+
+  public LEDIOReal() {
+    led = new AddressableLED(3);
+    buffer = new AddressableLEDBuffer(LEDSubsystem.LED_LENGTH);
+    led.setLength(buffer.getLength());
+    led.start();
+  }
+
+  /** Write data from buffer to leds */
+  @Override
+  public void updateInputs(LEDIOInputs inputs) {
+    led.setData(buffer);
+  }
+
+  @Override
+  public void set(int i, Color color) {
+    buffer.setLED(i, color);
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java
new file mode 100644
index 00000000..cb06f73a
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java
@@ -0,0 +1,81 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems.leds;
+
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
+
+public class LEDSubsystem extends SubsystemBase {
+  public static final int LED_LENGTH = 80;
+  LEDIO io;
+  LEDIOInputsAutoLogged inputs = new LEDIOInputsAutoLogged();
+
+  Color[] colors = new Color[LED_LENGTH];
+
+  int rainbowStart = 0;
+  int dashStart = 0;
+
+  /** Creates a new LEDSubsystem. */
+  public LEDSubsystem(LEDIO io) {
+    this.io = io;
+  }
+
+  @Override
+  public void periodic() {
+    io.updateInputs(inputs);
+    Logger.processInputs("LED", inputs);
+  }
+
+  private void setIndex(int i, Color color) {
+    io.set(i, color);
+  }
+
+  private void setSolid(Color color) {
+    io.solid(color);
+  }
+
+  public Command setSolidCmd(Color color) {
+    return this.run(() -> setSolid(color));
+  }
+
+  public Command setBlinkingCmd(Color onColor, Color offColor, double frequency) {
+    return Commands.repeatingSequence(
+        setSolidCmd(onColor).withTimeout(1.0 / frequency),
+        setSolidCmd(onColor).withTimeout(1.0 / frequency));
+  }
+
+  public void setProgress(Color color, double progress) {
+    for (int i = 0; i < LED_LENGTH; i++) {
+      if (i < progress) {
+        setIndex(i, color);
+      } else {
+        setIndex(i, Color.kBlack);
+      }
+    }
+  }
+
+  public Command setRainbowCmd() {
+    return this.run(
+        () -> {
+          for (int i = 0; i < LED_LENGTH; i++) {
+            setIndex(i, Color.fromHSV(rainbowStart % 180 + i, 255, 255));
+          }
+          rainbowStart += 6;
+        });
+  }
+
+  public void runColorAlong(Color colorDash, Color colorBg, int dashLength, double frequency) {
+    setSolid(colorBg);
+    for (int i = dashStart; i < dashStart + dashLength; i++) {
+      setIndex(i % LED_LENGTH, colorDash);
+    }
+
+    dashStart += (int) (LED_LENGTH / frequency);
+    dashStart %= LED_LENGTH;
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java
index fb168910..7ced7717 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java
@@ -1,5 +1,6 @@
 package frc.robot.subsystems.shooter;
 
+import com.google.common.base.Supplier;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
@@ -9,13 +10,9 @@
 import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
 import java.util.function.DoubleSupplier;
-
 import org.littletonrobotics.junction.Logger;
 
-import com.google.common.base.Supplier;
-
 public class ShooterSubystem extends SubsystemBase {
   public static final double PIVOT_RATIO = (27.0 / 1.0) * (48.0 / 22.0);
   public static final double FLYWHEEL_RATIO = 18.0 / 24.0;
@@ -50,7 +47,8 @@ public Pose3d getMechanismPose() {
         0.0437896, 0.0, 0.3274568, new Rotation3d(0.0, inputs.pivotRotation.getRadians(), 0.0));
   }
 
-  public Command runStateCmd(Supplier<Rotation2d> rotation, DoubleSupplier left, DoubleSupplier right) {
+  public Command runStateCmd(
+      Supplier<Rotation2d> rotation, DoubleSupplier left, DoubleSupplier right) {
     return this.run(
         () -> {
           Logger.recordOutput("Shooter/Left Velocity Setpoint", left.getAsDouble());

From 69efb468f4efd5fe2957e8f4cae38c3413fcb6ee Mon Sep 17 00:00:00 2001
From: Lewis-Seiden <lewy@seiden.us>
Date: Sun, 11 Feb 2024 14:33:14 -0800
Subject: [PATCH 09/62] added leds to indicate when piece picked up

---
 src/main/java/frc/robot/Robot.java            | 34 +++++++++++++++++--
 .../robot/subsystems/leds/LEDSubsystem.java   | 27 +++++++++------
 2 files changed, 47 insertions(+), 14 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 1a28c168..ceb3af82 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -10,6 +10,8 @@
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj.util.Color;
@@ -152,7 +154,29 @@ public void robotInit() {
             () -> Rotation2d.fromDegrees(0.0), () -> flywheelIdleSpeed, () -> flywheelIdleSpeed));
     reactionBarRelease.setDefaultCommand(
         reactionBarRelease.setRotationCmd(Rotation2d.fromDegrees(0.0)));
-    leds.setDefaultCommand(leds.setSolidCmd(new Color("#350868")));
+    leds.setDefaultCommand(
+        Commands.either(
+                leds.setBlinkingCmd(new Color("#00ff00"), new Color("#000000"), 10.0)
+                    .withInterruptBehavior(InterruptionBehavior.kCancelSelf),
+                leds.setRunAlongCmd(
+                    // Set color to be purple with a moving dash corresponding to alliance color
+                    () -> {
+                      if (DriverStation.getAlliance().isEmpty()) {
+                        return new Color("#e0e0e0");
+                      } else if (DriverStation.getAlliance().get() == Alliance.Red) {
+                        return new Color("#ff0000");
+                      } else { // Blue
+                        return new Color("#0000ff");
+                      }
+                    },
+                    () -> new Color("#350868"),
+                    10,
+                    1.0),
+                () -> DriverStation.isEnabled())
+            .until(() -> true)
+            .repeatedly()
+            .ignoringDisable(true)
+            .withInterruptBehavior(InterruptionBehavior.kCancelSelf));
 
     controller.setDefaultCommand(controller.rumbleCmd(0.0, 0.0));
     operator.setDefaultCommand(operator.rumbleCmd(0.0, 0.0));
@@ -163,9 +187,13 @@ public void robotInit() {
         .whileTrue(
             Commands.parallel(
                 intake.runVoltageCmd(0.0).withInterruptBehavior(InterruptionBehavior.kCancelSelf),
-                controller.rumbleCmd(1.0, 1.0).withTimeout(0.25)));
+                controller.rumbleCmd(1.0, 1.0).withTimeout(0.25),
+                leds.setBlinkingCmd(new Color("#ff8000"), new Color("#000000"), 25.0)));
     new Trigger(() -> currentTarget == Target.SPEAKER)
-        .whileTrue(Commands.parallel(carriage.runVoltageCmd(5.0), feeder.indexCmd()));
+        .whileTrue(
+            Commands.parallel(
+                carriage.runVoltageCmd(5.0),
+                feeder.indexCmd()));
     new Trigger(() -> currentTarget == Target.AMP)
         .whileTrue(
             Commands.either(
diff --git a/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java
index cb06f73a..86f408b0 100644
--- a/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java
@@ -4,6 +4,7 @@
 
 package frc.robot.subsystems.leds;
 
+import com.google.common.base.Supplier;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -17,8 +18,8 @@ public class LEDSubsystem extends SubsystemBase {
 
   Color[] colors = new Color[LED_LENGTH];
 
-  int rainbowStart = 0;
-  int dashStart = 0;
+  double rainbowStart = 0;
+  double dashStart = 0;
 
   /** Creates a new LEDSubsystem. */
   public LEDSubsystem(LEDIO io) {
@@ -46,7 +47,7 @@ public Command setSolidCmd(Color color) {
   public Command setBlinkingCmd(Color onColor, Color offColor, double frequency) {
     return Commands.repeatingSequence(
         setSolidCmd(onColor).withTimeout(1.0 / frequency),
-        setSolidCmd(onColor).withTimeout(1.0 / frequency));
+        setSolidCmd(offColor).withTimeout(1.0 / frequency));
   }
 
   public void setProgress(Color color, double progress) {
@@ -63,19 +64,23 @@ public Command setRainbowCmd() {
     return this.run(
         () -> {
           for (int i = 0; i < LED_LENGTH; i++) {
-            setIndex(i, Color.fromHSV(rainbowStart % 180 + i, 255, 255));
+            setIndex(i, Color.fromHSV((int) rainbowStart % 180 + i, 255, 255));
           }
           rainbowStart += 6;
         });
   }
 
-  public void runColorAlong(Color colorDash, Color colorBg, int dashLength, double frequency) {
-    setSolid(colorBg);
-    for (int i = dashStart; i < dashStart + dashLength; i++) {
-      setIndex(i % LED_LENGTH, colorDash);
-    }
+  public Command setRunAlongCmd(
+      Supplier<Color> colorDash, Supplier<Color> colorBg, int dashLength, double frequency) {
+    return this.run(
+        () -> {
+          setSolid(colorBg.get());
+          for (int i = (int) dashStart; i < dashStart + dashLength; i++) {
+            setIndex(i % LED_LENGTH, colorDash.get());
+          }
 
-    dashStart += (int) (LED_LENGTH / frequency);
-    dashStart %= LED_LENGTH;
+          dashStart += LED_LENGTH * frequency * 0.020;
+          dashStart %= LED_LENGTH;
+        });
   }
 }

From 0ecc5c0aa4510c4ba5af5f84723309b0b4a7e6cd Mon Sep 17 00:00:00 2001
From: Awesomeplayer165 <dev@jacob2.dev>
Date: Sun, 11 Feb 2024 15:46:39 -0800
Subject: [PATCH 10/62] add custom auto chooser local 4 doesn't work due to an
 issue with commands

---
 ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat |  Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat |  Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat |  Bin 2048 -> 2048 bytes
 simgui.json                                   |    2 +-
 src/main/deploy/choreo/auto1.1.traj           | 1876 -----------------
 src/main/deploy/choreo/auto1.traj             | 1876 -----------------
 src/main/java/frc/robot/AutoManager.java      |   39 +-
 src/main/java/frc/robot/Robot.java            |   11 +-
 8 files changed, 40 insertions(+), 3764 deletions(-)
 delete mode 100644 src/main/deploy/choreo/auto1.1.traj
 delete mode 100644 src/main/deploy/choreo/auto1.traj

diff --git a/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat
index 845bd633a464596ff2fc8b16816fd5ff93c4eb9a..1083f416a8f7b26f24efff87c938db4eadc83ed3 100644
GIT binary patch
delta 55
zcmZn=Xb@nz$e`aaQ80C)0`J5!FP2LTdI%odNe0#TLIM*7t=LX5XuKByu@*700RZIc
B5WxTd

delta 55
zcmZn=Xb@nz$)MgaQ80C)0`J5!FP2*jY6u?NK?d2+N<0$<t=JARNPSiTu@*700RZH-
B5WxTd

diff --git a/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat
index be975d9a20ff795a6664c77dcf57cca3a1d42c1d..b76d1bce23a0681d7dea259a941318ffc318d90b 100644
GIT binary patch
delta 55
zcmZn=Xb@nz&!EsSQ80C)0`J5!FP3`@iU=OtNe0#TLIM*7t=LX5XuKByu@*700RZI8
B5WfHb

delta 55
zcmZn=Xb@nz&Y;mSQ80C)0`J5!FP3Wzng|}-K?d2+N<0$<t=JARNPSiTu@*700RZG!
B5WfHb

diff --git a/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat
index c5ac962af08c491a8154630ad9826964b7d0124b..b476eeb8d54b9f4746257b1d392e0b9461c76eae 100644
GIT binary patch
delta 55
zcmZn=Xb@nz$dFb&Q80C)0`J5!FP2LTsR$n1Ne0#TLIM*7t=LX5XuKByu@*700RZ&)
B5hVZs

delta 55
zcmZn=Xb@nz$q-jPQ80C)0`J5!FP2*ju?Qa9K?d2+N<0$<t=JARNPSiTu@*700RZ&G
B5hVZs

diff --git a/simgui.json b/simgui.json
index 9594ddb6..181ebf07 100644
--- a/simgui.json
+++ b/simgui.json
@@ -26,7 +26,7 @@
       "/SmartDashboard/reset Pivot value": "Command"
     },
     "windows": {
-      "/SmartDashboard/SendableChooser[0]": {
+      "/SmartDashboard/Auto Chooser": {
         "window": {
           "visible": true
         }
diff --git a/src/main/deploy/choreo/auto1.1.traj b/src/main/deploy/choreo/auto1.1.traj
deleted file mode 100644
index c037a4f6..00000000
--- a/src/main/deploy/choreo/auto1.1.traj
+++ /dev/null
@@ -1,1876 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.3321385383605957,
-            "y": 5.547596454620361,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -3.4724754769815887e-38,
-            "velocityY": 7.030705743146232e-38,
-            "timestamp": 0
-        },
-        {
-            "x": 1.361410083208431,
-            "y": 5.5023955382692575,
-            "heading": 0.056119311202146825,
-            "angularVelocity": 0.6842584112779871,
-            "velocityX": 0.35690567728254624,
-            "velocityY": -0.5511312692222103,
-            "timestamp": 0.08201479189321614
-        },
-        {
-            "x": 1.4202660843784345,
-            "y": 5.411892088977607,
-            "heading": 0.16553764559038536,
-            "angularVelocity": 1.3341292693969398,
-            "velocityX": 0.7176266599156221,
-            "velocityY": -1.1035015416424676,
-            "timestamp": 0.1640295837864323
-        },
-        {
-            "x": 1.509321709738084,
-            "y": 5.2758955282995315,
-            "heading": 0.32143534479454616,
-            "angularVelocity": 1.9008485616489859,
-            "velocityX": 1.0858483366707872,
-            "velocityY": -1.658195522280223,
-            "timestamp": 0.24604437567964843
-        },
-        {
-            "x": 1.6296766101338032,
-            "y": 5.093983916449887,
-            "heading": 0.5046862665772869,
-            "angularVelocity": 2.2343642842054003,
-            "velocityX": 1.4674779709545838,
-            "velocityY": -2.2180341820106597,
-            "timestamp": 0.3280591675728646
-        },
-        {
-            "x": 1.782194657966092,
-            "y": 4.869104821237669,
-            "heading": 0.6276214122511846,
-            "angularVelocity": 1.4989387016181202,
-            "velocityX": 1.859640734452738,
-            "velocityY": -2.7419333759331184,
-            "timestamp": 0.4100739594660807
-        },
-        {
-            "x": 1.9333417085303153,
-            "y": 4.6354174904379,
-            "heading": 0.6920907561549033,
-            "angularVelocity": 0.7860697127374073,
-            "velocityX": 1.8429242710390283,
-            "velocityY": -2.849331509662689,
-            "timestamp": 0.49208875135929686
-        },
-        {
-            "x": 2.056917521349593,
-            "y": 4.443493094854259,
-            "heading": 0.6585466805605577,
-            "angularVelocity": -0.40900031348028376,
-            "velocityX": 1.5067503064590375,
-            "velocityY": -2.340119277916704,
-            "timestamp": 0.574103543252513
-        },
-        {
-            "x": 2.1535962335212764,
-            "y": 4.293727999952354,
-            "heading": 0.5265725883978299,
-            "angularVelocity": -1.609149875482936,
-            "velocityX": 1.1787960432498548,
-            "velocityY": -1.8260741927736632,
-            "timestamp": 0.6561183351457291
-        },
-        {
-            "x": 2.2240985983067136,
-            "y": 4.187111119253089,
-            "heading": 0.3017485783287344,
-            "angularVelocity": -2.7412617270530677,
-            "velocityX": 0.859629868685535,
-            "velocityY": -1.2999713617280337,
-            "timestamp": 0.7381331270389453
-        },
-        {
-            "x": 2.2676985263824463,
-            "y": 4.124765396118164,
-            "heading": -1.6642026159430915e-35,
-            "angularVelocity": -3.6791970248685537,
-            "velocityX": 0.5316105432846836,
-            "velocityY": -0.7601765693205621,
-            "timestamp": 0.8201479189321614
-        },
-        {
-            "x": 2.2826958672458773,
-            "y": 4.104348331421266,
-            "heading": -0.17441872129385144,
-            "angularVelocity": -4.1236397648607985,
-            "velocityX": 0.35456991481679806,
-            "velocityY": -0.4827040310886087,
-            "timestamp": 0.8624451930006055
-        },
-        {
-            "x": 2.289905521300629,
-            "y": 4.095598364764115,
-            "heading": -0.366838925198976,
-            "angularVelocity": -4.549234156171776,
-            "velocityX": 0.17045197860942923,
-            "velocityY": -0.20686833489535802,
-            "timestamp": 0.9047424670690496
-        },
-        {
-            "x": 2.289262823237601,
-            "y": 4.0986035079624115,
-            "heading": -0.5765637041805302,
-            "angularVelocity": -4.958352130262211,
-            "velocityX": -0.015194786831599966,
-            "velocityY": 0.07104815297159969,
-            "timestamp": 0.9470397411374937
-        },
-        {
-            "x": 2.28071834356103,
-            "y": 4.1136177509731775,
-            "heading": -0.8009025976234118,
-            "angularVelocity": -5.303861735388993,
-            "velocityX": -0.20201017358104575,
-            "velocityY": 0.3549695185195792,
-            "timestamp": 0.9893370152059378
-        },
-        {
-            "x": 2.264115058699565,
-            "y": 4.140907151677903,
-            "heading": -1.0347556438013104,
-            "angularVelocity": -5.528797099299715,
-            "velocityX": -0.39253794073343345,
-            "velocityY": 0.6451810738575218,
-            "timestamp": 1.0316342892743817
-        },
-        {
-            "x": 2.2392828245472374,
-            "y": 4.180616635647853,
-            "heading": -1.271268857234422,
-            "angularVelocity": -5.5916892670292055,
-            "velocityX": -0.5870882864022154,
-            "velocityY": 0.9388189864361984,
-            "timestamp": 1.0739315633428257
-        },
-        {
-            "x": 2.206219336908872,
-            "y": 4.232592488850575,
-            "heading": -1.500368652073653,
-            "angularVelocity": -5.416419849385782,
-            "velocityX": -0.7816931082807688,
-            "velocityY": 1.228822763344421,
-            "timestamp": 1.1162288374112697
-        },
-        {
-            "x": 2.16560115012261,
-            "y": 4.295896098840919,
-            "heading": -1.7081840928578422,
-            "angularVelocity": -4.913211202403,
-            "velocityX": -0.9603027069908828,
-            "velocityY": 1.496635690704529,
-            "timestamp": 1.1585261114797136
-        },
-        {
-            "x": 2.116418529309532,
-            "y": 4.368321186816809,
-            "heading": -1.8863054196825848,
-            "angularVelocity": -4.211177451684291,
-            "velocityX": -1.162784645022098,
-            "velocityY": 1.7122873653439958,
-            "timestamp": 1.2008233855481576
-        },
-        {
-            "x": 2.058912682392385,
-            "y": 4.450563139928545,
-            "heading": -2.036216963298125,
-            "angularVelocity": -3.54423652391779,
-            "velocityX": -1.3595639005977715,
-            "velocityY": 1.9443795119906613,
-            "timestamp": 1.2431206596166016
-        },
-        {
-            "x": 1.99337005684811,
-            "y": 4.542961602948437,
-            "heading": -2.1579434366118924,
-            "angularVelocity": -2.877879863293171,
-            "velocityX": -1.549570911785386,
-            "velocityY": 2.184501603350988,
-            "timestamp": 1.2854179336850455
-        },
-        {
-            "x": 1.9201828437796038,
-            "y": 4.645652670813241,
-            "heading": -2.2501769997399386,
-            "angularVelocity": -2.180603009517744,
-            "velocityX": -1.7303056681637912,
-            "velocityY": 2.4278412764527704,
-            "timestamp": 1.3277152077534895
-        },
-        {
-            "x": 1.8398838191629743,
-            "y": 4.758671823925337,
-            "heading": -2.3107451299036645,
-            "angularVelocity": -1.4319629691907945,
-            "velocityX": -1.8984444360809696,
-            "velocityY": 2.6720197838095423,
-            "timestamp": 1.3700124818219335
-        },
-        {
-            "x": 1.753396216406846,
-            "y": 4.882199443808802,
-            "heading": -2.337574910152261,
-            "angularVelocity": -0.6343146417705776,
-            "velocityX": -2.0447559484844615,
-            "velocityY": 2.920462904620659,
-            "timestamp": 1.4123097558903774
-        },
-        {
-            "x": 1.6751428992292787,
-            "y": 4.996901240775032,
-            "heading": -2.400008661282964,
-            "angularVelocity": -1.4760703261792811,
-            "velocityX": -1.8500794413119903,
-            "velocityY": 2.7118011619525055,
-            "timestamp": 1.4546070299588214
-        },
-        {
-            "x": 1.604167639142011,
-            "y": 5.102445488685979,
-            "heading": -2.5005028957452873,
-            "angularVelocity": -2.375903333621606,
-            "velocityX": -1.6780102654468432,
-            "velocityY": 2.4952966883908108,
-            "timestamp": 1.4969043040272654
-        },
-        {
-            "x": 1.5418087398499432,
-            "y": 5.19740105341742,
-            "heading": -2.6238103064581355,
-            "angularVelocity": -2.9152566785584497,
-            "velocityX": -1.4743006651246806,
-            "velocityY": 2.2449570763776965,
-            "timestamp": 1.5392015780957093
-        },
-        {
-            "x": 1.488225747512813,
-            "y": 5.280327915096259,
-            "heading": -2.743877445785595,
-            "angularVelocity": -2.8386495813694843,
-            "velocityX": -1.2668190448969345,
-            "velocityY": 1.960572247389977,
-            "timestamp": 1.5814988521641533
-        },
-        {
-            "x": 1.4429482417442723,
-            "y": 5.35126150411709,
-            "heading": -2.8526923539591857,
-            "angularVelocity": -2.5726222450531995,
-            "velocityX": -1.0704591907098815,
-            "velocityY": 1.6770250703638487,
-            "timestamp": 1.6237961262325973
-        },
-        {
-            "x": 1.4056282761179824,
-            "y": 5.410288137316586,
-            "heading": -2.94638853961619,
-            "angularVelocity": -2.215182602675249,
-            "velocityX": -0.8823255504811011,
-            "velocityY": 1.3955186120027623,
-            "timestamp": 1.6660934003010412
-        },
-        {
-            "x": 1.3760265384921018,
-            "y": 5.457458867956814,
-            "heading": -3.023001318784294,
-            "angularVelocity": -1.8112935373596752,
-            "velocityX": -0.6998497723040017,
-            "velocityY": 1.115219164334259,
-            "timestamp": 1.7083906743694852
-        },
-        {
-            "x": 1.3539833473819511,
-            "y": 5.492803685860998,
-            "heading": -3.0814563447777146,
-            "angularVelocity": -1.3820045683991566,
-            "velocityX": -0.5211492134098515,
-            "velocityY": 0.8356287416297835,
-            "timestamp": 1.7506879484379292
-        },
-        {
-            "x": 1.3393859043766252,
-            "y": 5.516345418500216,
-            "heading": -3.121051917122202,
-            "angularVelocity": -0.9361258666554966,
-            "velocityX": -0.3451154554713123,
-            "velocityY": 0.5565780102311921,
-            "timestamp": 1.7929852225063732
-        },
-        {
-            "x": 1.3321385383605957,
-            "y": 5.52810525894165,
-            "heading": -3.1411853071795863,
-            "angularVelocity": -0.47599734263737736,
-            "velocityX": -0.17134357179382342,
-            "velocityY": 0.2780283292584069,
-            "timestamp": 1.8352824965748171
-        },
-        {
-            "x": 1.3321385383605957,
-            "y": 5.52810525894165,
-            "heading": -3.1411853071795863,
-            "angularVelocity": 1.0972891859651771e-35,
-            "velocityX": 2.010813957988482e-36,
-            "velocityY": -7.697026441410569e-35,
-            "timestamp": 1.877579770643261
-        },
-        {
-            "x": 1.337370959637994,
-            "y": 5.5282378526253355,
-            "heading": -3.1057781238593085,
-            "angularVelocity": 1.0527517345397082,
-            "velocityX": 0.1555740970920068,
-            "velocityY": 0.003942370372304683,
-            "timestamp": 1.9112127556658494
-        },
-        {
-            "x": 1.3479554232342674,
-            "y": 5.5287097500206475,
-            "heading": -3.0355501553802027,
-            "angularVelocity": 2.0880682589410555,
-            "velocityX": 0.314704852666657,
-            "velocityY": 0.014030791349476537,
-            "timestamp": 1.9448457406884376
-        },
-        {
-            "x": 1.364027106419543,
-            "y": 5.529829382282991,
-            "heading": -2.9313355116530078,
-            "angularVelocity": 3.0985844300529233,
-            "velocityX": 0.4778547956561592,
-            "velocityY": 0.03328970835004707,
-            "timestamp": 1.978478725711026
-        },
-        {
-            "x": 1.3857940218160676,
-            "y": 5.531991335292171,
-            "heading": -2.7946612849381784,
-            "angularVelocity": 4.06369600031153,
-            "velocityX": 0.6471895189173892,
-            "velocityY": 0.06428073534739039,
-            "timestamp": 2.012111710733614
-        },
-        {
-            "x": 1.4139333100804088,
-            "y": 5.535564467259386,
-            "heading": -2.6300339585531556,
-            "angularVelocity": 4.89481758084981,
-            "velocityX": 0.8366574731752962,
-            "velocityY": 0.10623891887132897,
-            "timestamp": 2.0457446957562024
-        },
-        {
-            "x": 1.4504189067700664,
-            "y": 5.5389912877920775,
-            "heading": -2.4501533377113245,
-            "angularVelocity": 5.348339456667967,
-            "velocityX": 1.0848158932415233,
-            "velocityY": 0.10188868250590417,
-            "timestamp": 2.0793776807787907
-        },
-        {
-            "x": 1.4957426020786804,
-            "y": 5.541383162087915,
-            "heading": -2.2591276091627437,
-            "angularVelocity": 5.679713781583368,
-            "velocityX": 1.3475965715851275,
-            "velocityY": 0.07111691972124916,
-            "timestamp": 2.113010665801379
-        },
-        {
-            "x": 1.549887277450098,
-            "y": 5.541604080123591,
-            "heading": -2.077370178961034,
-            "angularVelocity": 5.404142096802884,
-            "velocityX": 1.6098682687562116,
-            "velocityY": 0.006568493267186675,
-            "timestamp": 2.1466436508239672
-        },
-        {
-            "x": 1.612403659002375,
-            "y": 5.539682953328213,
-            "heading": -1.9100118188035742,
-            "angularVelocity": 4.9760186330490965,
-            "velocityX": 1.8587818330811392,
-            "velocityY": -0.05712031786913091,
-            "timestamp": 2.1802766358465555
-        },
-        {
-            "x": 1.6835678333886066,
-            "y": 5.53654594415181,
-            "heading": -1.7561585727792173,
-            "angularVelocity": 4.5744749067330135,
-            "velocityX": 2.1159042035203575,
-            "velocityY": -0.0932718036860474,
-            "timestamp": 2.213909620869144
-        },
-        {
-            "x": 1.7632443244228584,
-            "y": 5.534877661418315,
-            "heading": -1.616595649842294,
-            "angularVelocity": 4.149584785388266,
-            "velocityX": 2.3689984989658393,
-            "velocityY": -0.04960257712408059,
-            "timestamp": 2.247542605891732
-        },
-        {
-            "x": 1.845517380604842,
-            "y": 5.542129045770498,
-            "heading": -1.4828842082324165,
-            "angularVelocity": 3.9756043515042103,
-            "velocityX": 2.4462014337035023,
-            "velocityY": 0.21560335329479804,
-            "timestamp": 2.2811755909143203
-        },
-        {
-            "x": 1.9198078929781421,
-            "y": 5.552688491598465,
-            "heading": -1.3358148203184805,
-            "angularVelocity": 4.372772378519569,
-            "velocityX": 2.208858723761989,
-            "velocityY": 0.31396100646060415,
-            "timestamp": 2.3148085759369086
-        },
-        {
-            "x": 1.9854932228404973,
-            "y": 5.561629411594934,
-            "heading": -1.1756047034069004,
-            "angularVelocity": 4.763481945000775,
-            "velocityX": 1.9530032739657306,
-            "velocityY": 0.26583783718469556,
-            "timestamp": 2.348441560959497
-        },
-        {
-            "x": 2.0429500865174064,
-            "y": 5.56779505699511,
-            "heading": -1.0015843737719388,
-            "angularVelocity": 5.1740970811270035,
-            "velocityX": 1.7083486237787309,
-            "velocityY": 0.18332138512343155,
-            "timestamp": 2.382074545982085
-        },
-        {
-            "x": 2.0926336267477486,
-            "y": 5.570712245735625,
-            "heading": -0.8117369336213589,
-            "angularVelocity": 5.644680066996039,
-            "velocityX": 1.477226603495769,
-            "velocityY": 0.08673594504191894,
-            "timestamp": 2.4157075310046734
-        },
-        {
-            "x": 2.1331736827499013,
-            "y": 5.571318223980459,
-            "heading": -0.6223345825199257,
-            "angularVelocity": 5.63144636059598,
-            "velocityX": 1.2053659814888902,
-            "velocityY": 0.01801737920159694,
-            "timestamp": 2.4493405160272617
-        },
-        {
-            "x": 2.1650842656051874,
-            "y": 5.570653927593427,
-            "heading": -0.4456863837750604,
-            "angularVelocity": 5.252230767689132,
-            "velocityX": 0.9487883051074633,
-            "velocityY": -0.01975133597523799,
-            "timestamp": 2.48297350104985
-        },
-        {
-            "x": 2.1896037980440752,
-            "y": 5.569969320542384,
-            "heading": -0.29224766364008553,
-            "angularVelocity": 4.56214992608965,
-            "velocityX": 0.7290323003569462,
-            "velocityY": -0.020355227184960373,
-            "timestamp": 2.516606486072438
-        },
-        {
-            "x": 2.20780773019257,
-            "y": 5.569409848980837,
-            "heading": -0.16876882632138132,
-            "angularVelocity": 3.6713612317127033,
-            "velocityX": 0.5412523490337908,
-            "velocityY": -0.016634609184155776,
-            "timestamp": 2.5502394710950265
-        },
-        {
-            "x": 2.220213327560089,
-            "y": 5.568812984785934,
-            "heading": -0.07803807473080998,
-            "angularVelocity": 2.697671691336225,
-            "velocityX": 0.368852106323223,
-            "velocityY": -0.017746393741217954,
-            "timestamp": 2.5838724561176147
-        },
-        {
-            "x": 2.227117258661317,
-            "y": 5.568051315056596,
-            "heading": -0.021486347391680306,
-            "angularVelocity": 1.6814364618885036,
-            "velocityX": 0.2052726243772784,
-            "velocityY": -0.022646509931434324,
-            "timestamp": 2.617505441140203
-        },
-        {
-            "x": 2.2287168502807617,
-            "y": 5.567087173461914,
-            "heading": 6.38149991346766e-37,
-            "angularVelocity": 0.6388474700431709,
-            "velocityX": 0.047560203721742585,
-            "velocityY": -0.02866654845040371,
-            "timestamp": 2.6511384261627913
-        },
-        {
-            "x": 2.2247758271699722,
-            "y": 5.5659373744789455,
-            "heading": -0.016384469566950412,
-            "angularVelocity": -0.4672993433012807,
-            "velocityX": -0.11240141184197158,
-            "velocityY": -0.03279326849576676,
-            "timestamp": 2.686200468945049
-        },
-        {
-            "x": 2.215132755191586,
-            "y": 5.564807940629173,
-            "heading": -0.07111277017786721,
-            "angularVelocity": -1.560898803038675,
-            "velocityX": -0.27502881216225394,
-            "velocityY": -0.03221243715853021,
-            "timestamp": 2.7212625117273066
-        },
-        {
-            "x": 2.199681422128445,
-            "y": 5.563984363308005,
-            "heading": -0.16355266498461823,
-            "angularVelocity": -2.6364663171744263,
-            "velocityX": -0.4406854774291637,
-            "velocityY": -0.023489142554608954,
-            "timestamp": 2.7563245545095643
-        },
-        {
-            "x": 2.178275880729617,
-            "y": 5.563860339253889,
-            "heading": -0.2925810137194886,
-            "angularVelocity": -3.6800008925938044,
-            "velocityX": -0.6105046854160964,
-            "velocityY": -0.0035372740511740697,
-            "timestamp": 2.791386597291822
-        },
-        {
-            "x": 2.150434792823077,
-            "y": 5.56488104971689,
-            "heading": -0.45485951159806126,
-            "angularVelocity": -4.628324107820951,
-            "velocityX": -0.7940520773258658,
-            "velocityY": 0.029111551467175232,
-            "timestamp": 2.8264486400740796
-        },
-        {
-            "x": 2.1136927979170794,
-            "y": 5.564806154665508,
-            "heading": -0.6342616510080993,
-            "angularVelocity": -5.116705279386068,
-            "velocityX": -1.047913697846207,
-            "velocityY": -0.0021360721007366726,
-            "timestamp": 2.8615106828563373
-        },
-        {
-            "x": 2.0677952991091333,
-            "y": 5.562722960469238,
-            "heading": -0.8285761603492735,
-            "angularVelocity": -5.542019058841107,
-            "velocityX": -1.3090366437853844,
-            "velocityY": -0.05941451298794179,
-            "timestamp": 2.896572725638595
-        },
-        {
-            "x": 2.0120787764368706,
-            "y": 5.557776466918295,
-            "heading": -1.020468287532569,
-            "angularVelocity": -5.472930609747536,
-            "velocityX": -1.5890837569925207,
-            "velocityY": -0.14107830458316797,
-            "timestamp": 2.9316347684208526
-        },
-        {
-            "x": 1.947346732087487,
-            "y": 5.549990941392819,
-            "heading": -1.1972983758508349,
-            "angularVelocity": -5.043348141932757,
-            "velocityX": -1.8462142879518657,
-            "velocityY": -0.22204996935933435,
-            "timestamp": 2.9666968112031102
-        },
-        {
-            "x": 1.8731816133340355,
-            "y": 5.541067205657786,
-            "heading": -1.3595832736612468,
-            "angularVelocity": -4.628506639451492,
-            "velocityX": -2.1152537863818077,
-            "velocityY": -0.25451271594333,
-            "timestamp": 3.001758853985368
-        },
-        {
-            "x": 1.7903438797008602,
-            "y": 5.535778355661316,
-            "heading": -1.5070368489944903,
-            "angularVelocity": -4.2055044039778195,
-            "velocityX": -2.362604316799635,
-            "velocityY": -0.15084260860996357,
-            "timestamp": 3.0368208967676256
-        },
-        {
-            "x": 1.705207783905628,
-            "y": 5.540259742780199,
-            "heading": -1.6469795594362229,
-            "angularVelocity": -3.991287995134942,
-            "velocityX": -2.4281556075880766,
-            "velocityY": 0.12781306402233675,
-            "timestamp": 3.0718829395498832
-        },
-        {
-            "x": 1.629345764073995,
-            "y": 5.546176174189709,
-            "heading": -1.802120558201536,
-            "angularVelocity": -4.424756416184017,
-            "velocityX": -2.1636508831716217,
-            "velocityY": 0.16874177714778527,
-            "timestamp": 3.106944982332141
-        },
-        {
-            "x": 1.5628215543297233,
-            "y": 5.550494777199676,
-            "heading": -1.9719374235468123,
-            "angularVelocity": -4.843324914063712,
-            "velocityX": -1.8973284060315727,
-            "velocityY": 0.12317031944734674,
-            "timestamp": 3.1420070251143986
-        },
-        {
-            "x": 1.5052956910676978,
-            "y": 5.552468250746859,
-            "heading": -2.1578472190724005,
-            "angularVelocity": -5.302309300120524,
-            "velocityX": -1.6406877265900472,
-            "velocityY": 0.056285184506758255,
-            "timestamp": 3.177069067896656
-        },
-        {
-            "x": 1.4562451550739939,
-            "y": 5.551766768072451,
-            "heading": -2.3624196209535056,
-            "angularVelocity": -5.834583088941523,
-            "velocityX": -1.3989640106914938,
-            "velocityY": -0.02000689688175958,
-            "timestamp": 3.212131110678914
-        },
-        {
-            "x": 1.4173942622657685,
-            "y": 5.550106751106204,
-            "heading": -2.5657566721184293,
-            "angularVelocity": -5.799349810497013,
-            "velocityX": -1.1080613029165842,
-            "velocityY": -0.047345129790485885,
-            "timestamp": 3.2471931534611715
-        },
-        {
-            "x": 1.3874992845964569,
-            "y": 5.548698391964879,
-            "heading": -2.750369265628105,
-            "angularVelocity": -5.26531196873374,
-            "velocityX": -0.852630802345583,
-            "velocityY": -0.0401676294239809,
-            "timestamp": 3.282255196243429
-        },
-        {
-            "x": 1.3647199881577643,
-            "y": 5.548009174125998,
-            "heading": -2.903871904539791,
-            "angularVelocity": -4.378028966109259,
-            "velocityX": -0.6496853757254475,
-            "velocityY": -0.019657093089571077,
-            "timestamp": 3.317317239025687
-        },
-        {
-            "x": 1.3481785543906255,
-            "y": 5.547743770300603,
-            "heading": -3.0214740080658498,
-            "angularVelocity": -3.354114426714698,
-            "velocityX": -0.4717760990101023,
-            "velocityY": -0.007569548273095167,
-            "timestamp": 3.3523792818079445
-        },
-        {
-            "x": 1.3374168488281284,
-            "y": 5.547647789865521,
-            "heading": -3.10100248389671,
-            "angularVelocity": -2.268221401837531,
-            "velocityX": -0.30693321633680876,
-            "velocityY": -0.0027374456097331258,
-            "timestamp": 3.387441324590202
-        },
-        {
-            "x": 1.3321385383605957,
-            "y": 5.547596454620361,
-            "heading": -3.1411853071795863,
-            "angularVelocity": -1.1460491201958698,
-            "velocityX": -0.15054201206449694,
-            "velocityY": -0.001464125906141093,
-            "timestamp": 3.42250336737246
-        },
-        {
-            "x": 1.3321385383605957,
-            "y": 5.547596454620361,
-            "heading": -3.1411853071795863,
-            "angularVelocity": -1.1877010663188592e-33,
-            "velocityX": -2.3472123166291743e-34,
-            "velocityY": -4.828629969149868e-35,
-            "timestamp": 3.4575654101547175
-        },
-        {
-            "x": 1.3353773426968816,
-            "y": 5.556976212978418,
-            "heading": -3.1027759330153977,
-            "angularVelocity": 0.9418411865422114,
-            "velocityX": 0.07941913622507528,
-            "velocityY": 0.2300022568362304,
-            "timestamp": 3.4983465680749837
-        },
-        {
-            "x": 1.342768335196551,
-            "y": 5.576502155287014,
-            "heading": -3.0330086785474775,
-            "angularVelocity": 1.7107717883912714,
-            "velocityX": 0.18123547433645554,
-            "velocityY": 0.47879813385321396,
-            "timestamp": 3.53912772599525
-        },
-        {
-            "x": 1.357543783583743,
-            "y": 5.605477021924564,
-            "heading": -2.939156767804727,
-            "angularVelocity": 2.301354731669143,
-            "velocityX": 0.3623106635687124,
-            "velocityY": 0.7104964183263536,
-            "timestamp": 3.579908883915516
-        },
-        {
-            "x": 1.3800861680237329,
-            "y": 5.643206466545064,
-            "heading": -2.8183134070613347,
-            "angularVelocity": 2.9632155364411483,
-            "velocityX": 0.5527646979535052,
-            "velocityY": 0.9251685470595896,
-            "timestamp": 3.620690041835782
-        },
-        {
-            "x": 1.410132685953878,
-            "y": 5.690126983565226,
-            "heading": -2.6706561536339386,
-            "angularVelocity": 3.6207224354956073,
-            "velocityX": 0.7367745170181611,
-            "velocityY": 1.1505440113274845,
-            "timestamp": 3.6614711997560483
-        },
-        {
-            "x": 1.447426110042683,
-            "y": 5.747148126734536,
-            "heading": -2.4998618512700794,
-            "angularVelocity": 4.188068977781135,
-            "velocityX": 0.9144768317201708,
-            "velocityY": 1.3982227596576835,
-            "timestamp": 3.7022523576763144
-        },
-        {
-            "x": 1.4919514039971702,
-            "y": 5.815443990853687,
-            "heading": -2.3159167566388894,
-            "angularVelocity": 4.510541240413857,
-            "velocityX": 1.0918104395549988,
-            "velocityY": 1.6746916370712401,
-            "timestamp": 3.7430335155965806
-        },
-        {
-            "x": 1.543306542439651,
-            "y": 5.895557258888599,
-            "heading": -2.140840879075951,
-            "angularVelocity": 4.293058031977455,
-            "velocityX": 1.259285931578716,
-            "velocityY": 1.9644677130440258,
-            "timestamp": 3.7838146735168467
-        },
-        {
-            "x": 1.5991978574253154,
-            "y": 5.9852989254543125,
-            "heading": -2.0026162516135204,
-            "angularVelocity": 3.389423805294663,
-            "velocityX": 1.3705180979642946,
-            "velocityY": 2.2005669074226377,
-            "timestamp": 3.824595831437113
-        },
-        {
-            "x": 1.6602838186413746,
-            "y": 6.084712915583529,
-            "heading": -1.8987034819220077,
-            "angularVelocity": 2.5480583433820057,
-            "velocityX": 1.497896683941456,
-            "velocityY": 2.4377431931576705,
-            "timestamp": 3.865376989357379
-        },
-        {
-            "x": 1.7272241659282512,
-            "y": 6.1936323149644315,
-            "heading": -1.8270894144503607,
-            "angularVelocity": 1.7560577267488233,
-            "velocityX": 1.6414528350998836,
-            "velocityY": 2.670826551660404,
-            "timestamp": 3.906158147277645
-        },
-        {
-            "x": 1.8006198616667228,
-            "y": 6.311769110233653,
-            "heading": -1.7863461946081067,
-            "angularVelocity": 0.9990697155268056,
-            "velocityX": 1.7997452618185266,
-            "velocityY": 2.8968474975673533,
-            "timestamp": 3.9469393051979114
-        },
-        {
-            "x": 1.8749454151333007,
-            "y": 6.423903525675366,
-            "heading": -1.7351071665224345,
-            "angularVelocity": 1.2564387746383576,
-            "velocityX": 1.8225464223428127,
-            "velocityY": 2.7496623725337597,
-            "timestamp": 3.9877204631181775
-        },
-        {
-            "x": 1.9436310828301862,
-            "y": 6.525803671980358,
-            "heading": -1.6558915421182487,
-            "angularVelocity": 1.9424564785302534,
-            "velocityX": 1.6842500605592803,
-            "velocityY": 2.4987065473772168,
-            "timestamp": 4.028501621038443
-        },
-        {
-            "x": 2.0070825887496224,
-            "y": 6.617181897860731,
-            "heading": -1.5496488374577189,
-            "angularVelocity": 2.6051909773688204,
-            "velocityX": 1.5559025087883662,
-            "velocityY": 2.240697188123809,
-            "timestamp": 4.069282778958709
-        },
-        {
-            "x": 2.0657895498392027,
-            "y": 6.697945113539011,
-            "heading": -1.416445908140072,
-            "angularVelocity": 3.2662861014903286,
-            "velocityX": 1.4395609169401762,
-            "velocityY": 1.9804051625063046,
-            "timestamp": 4.1100639368789755
-        },
-        {
-            "x": 2.1168235029971925,
-            "y": 6.770262031139991,
-            "heading": -1.2545233571884964,
-            "angularVelocity": 3.9705236243698976,
-            "velocityX": 1.2514101060536296,
-            "velocityY": 1.7732924048496255,
-            "timestamp": 4.150845094799242
-        },
-        {
-            "x": 2.160098891028687,
-            "y": 6.834337445285054,
-            "heading": -1.0613678161788878,
-            "angularVelocity": 4.736391776497875,
-            "velocityX": 1.0611613362255508,
-            "velocityY": 1.571201442350923,
-            "timestamp": 4.191626252719508
-        },
-        {
-            "x": 2.1954531720383796,
-            "y": 6.890659821255424,
-            "heading": -0.8342118106482137,
-            "angularVelocity": 5.570121524621797,
-            "velocityX": 0.8669268557507981,
-            "velocityY": 1.3810881996163518,
-            "timestamp": 4.232407410639774
-        },
-        {
-            "x": 2.2216225780243426,
-            "y": 6.9367359795772305,
-            "heading": -0.6132278215539244,
-            "angularVelocity": 5.418776718560821,
-            "velocityX": 0.6417033581324141,
-            "velocityY": 1.1298393834695264,
-            "timestamp": 4.27318856856004
-        },
-        {
-            "x": 2.2400584030513464,
-            "y": 6.973023548052357,
-            "heading": -0.41519671142857095,
-            "angularVelocity": 4.855946231652809,
-            "velocityX": 0.4520672282785309,
-            "velocityY": 0.889812117303642,
-            "timestamp": 4.313969726480306
-        },
-        {
-            "x": 2.252392360052711,
-            "y": 6.999798504355172,
-            "heading": -0.24838728557433673,
-            "angularVelocity": 4.0903553101747345,
-            "velocityX": 0.3024425403878878,
-            "velocityY": 0.6565521350611045,
-            "timestamp": 4.3547508844005725
-        },
-        {
-            "x": 2.2601470415559044,
-            "y": 7.01732009026677,
-            "heading": -0.11829622013640562,
-            "angularVelocity": 3.189979688469885,
-            "velocityX": 0.19015353900334692,
-            "velocityY": 0.4296490537580049,
-            "timestamp": 4.395532042320839
-        },
-        {
-            "x": 2.2649306384373356,
-            "y": 7.026470873344902,
-            "heading": -0.032599297392582494,
-            "angularVelocity": 2.101385225779393,
-            "velocityX": 0.11729919220988534,
-            "velocityY": 0.2243875246510465,
-            "timestamp": 4.436313200241105
-        },
-        {
-            "x": 2.2676985263824463,
-            "y": 7.028899669647217,
-            "heading": 1.4623510884357687e-33,
-            "angularVelocity": 0.799371549388559,
-            "velocityX": 0.06787173504299782,
-            "velocityY": 0.05955682541099079,
-            "timestamp": 4.477094358161371
-        },
-        {
-            "x": 2.268447419698579,
-            "y": 7.024856278106883,
-            "heading": -0.027481442707952487,
-            "angularVelocity": -0.643111881448138,
-            "velocityX": 0.017525360464520197,
-            "velocityY": -0.09462214806441192,
-            "timestamp": 4.5198263360655435
-        },
-        {
-            "x": 2.2661447146026696,
-            "y": 7.012526995839087,
-            "heading": -0.10890493556394686,
-            "angularVelocity": -1.9054463858094515,
-            "velocityX": -0.05388716387229158,
-            "velocityY": -0.28852589729042577,
-            "timestamp": 4.562558313969716
-        },
-        {
-            "x": 2.259237652200008,
-            "y": 6.990956553960971,
-            "heading": -0.23735745507492811,
-            "angularVelocity": -3.0060045383117964,
-            "velocityX": -0.16163685233926556,
-            "velocityY": -0.504784541602271,
-            "timestamp": 4.605290291873889
-        },
-        {
-            "x": 2.2461382818263638,
-            "y": 6.959794967044901,
-            "heading": -0.4073842771005642,
-            "angularVelocity": -3.9789129912714607,
-            "velocityX": -0.30654725140549266,
-            "velocityY": -0.7292334323009891,
-            "timestamp": 4.648022269778061
-        },
-        {
-            "x": 2.225216379141952,
-            "y": 6.918625356851511,
-            "heading": -0.6110307557476551,
-            "angularVelocity": -4.765669380054774,
-            "velocityX": -0.48960763602681573,
-            "velocityY": -0.9634379734473075,
-            "timestamp": 4.690754247682234
-        },
-        {
-            "x": 2.1950172148844427,
-            "y": 6.866427702804465,
-            "heading": -0.8329684676174923,
-            "angularVelocity": -5.193714935628281,
-            "velocityX": -0.706711126857547,
-            "velocityY": -1.2215127079794852,
-            "timestamp": 4.7334862255864065
-        },
-        {
-            "x": 2.1547332634682537,
-            "y": 6.802939586567042,
-            "heading": -1.0490897028487878,
-            "angularVelocity": -5.057599620498584,
-            "velocityX": -0.9427120716604103,
-            "velocityY": -1.4857284720074833,
-            "timestamp": 4.776218203490579
-        },
-        {
-            "x": 2.1052130726477585,
-            "y": 6.731031156240311,
-            "heading": -1.2322360836735529,
-            "angularVelocity": -4.285932685715518,
-            "velocityX": -1.1588555748939446,
-            "velocityY": -1.6827779535033027,
-            "timestamp": 4.818950181394752
-        },
-        {
-            "x": 2.046678257521749,
-            "y": 6.650499615743051,
-            "heading": -1.3842475177981566,
-            "angularVelocity": -3.557322679177027,
-            "velocityX": -1.3698129128793415,
-            "velocityY": -1.8845732036521736,
-            "timestamp": 4.861682159298924
-        },
-        {
-            "x": 1.9824374188049456,
-            "y": 6.55859622988028,
-            "heading": -1.505962481707897,
-            "angularVelocity": -2.848334429608893,
-            "velocityX": -1.5033434413184743,
-            "velocityY": -2.150693470563594,
-            "timestamp": 4.904414137203097
-        },
-        {
-            "x": 1.9120980242519563,
-            "y": 6.455385209029271,
-            "heading": -1.5975173206853661,
-            "angularVelocity": -2.1425368884815836,
-            "velocityX": -1.6460598830863262,
-            "velocityY": -2.415311106882586,
-            "timestamp": 4.9471461151072695
-        },
-        {
-            "x": 1.835418260391466,
-            "y": 6.341055354736124,
-            "heading": -1.6582681711046532,
-            "angularVelocity": -1.4216718579121885,
-            "velocityX": -1.7944351659182955,
-            "velocityY": -2.675510470157384,
-            "timestamp": 4.989878093011442
-        },
-        {
-            "x": 1.7526348309039128,
-            "y": 6.215560469564215,
-            "heading": -1.6873132093111056,
-            "angularVelocity": -0.6797026402940408,
-            "velocityX": -1.937271185368423,
-            "velocityY": -2.9367909309822835,
-            "timestamp": 5.032610070915615
-        },
-        {
-            "x": 1.677441366215122,
-            "y": 6.099853521630283,
-            "heading": -1.7491066585439166,
-            "angularVelocity": -1.4460704199413479,
-            "velocityX": -1.7596532708458867,
-            "velocityY": -2.70773677252684,
-            "timestamp": 5.075342048819787
-        },
-        {
-            "x": 1.6091252487579695,
-            "y": 5.994372460119343,
-            "heading": -1.8457597622108368,
-            "angularVelocity": -2.2618448386280567,
-            "velocityX": -1.598711803379506,
-            "velocityY": -2.4684338681322977,
-            "timestamp": 5.11807402672396
-        },
-        {
-            "x": 1.547030840271468,
-            "y": 5.899198607270202,
-            "heading": -1.9803343034636756,
-            "angularVelocity": -3.149270121655174,
-            "velocityX": -1.4531133715773699,
-            "velocityY": -2.2272278868666358,
-            "timestamp": 5.1608060046281325
-        },
-        {
-            "x": 1.4904311214521733,
-            "y": 5.813988086882659,
-            "heading": -2.1571135876817413,
-            "angularVelocity": -4.136931939225903,
-            "velocityX": -1.3245284116317142,
-            "velocityY": -1.9940691858127988,
-            "timestamp": 5.203537982532305
-        },
-        {
-            "x": 1.4423633386018753,
-            "y": 5.740159267538946,
-            "heading": -2.35427402869071,
-            "angularVelocity": -4.613885213811227,
-            "velocityX": -1.1248667908162702,
-            "velocityY": -1.7277182794879316,
-            "timestamp": 5.246269960436478
-        },
-        {
-            "x": 1.4038900802302197,
-            "y": 5.6781134856885735,
-            "heading": -2.5480008926422313,
-            "angularVelocity": -4.533533748097493,
-            "velocityX": -0.9003388155337229,
-            "velocityY": -1.451975426681909,
-            "timestamp": 5.28900193834065
-        },
-        {
-            "x": 1.3744454738206087,
-            "y": 5.627099122367314,
-            "heading": -2.7223828634703597,
-            "angularVelocity": -4.0808307824923435,
-            "velocityX": -0.6890532068429254,
-            "velocityY": -1.1938217190802813,
-            "timestamp": 5.331733916244823
-        },
-        {
-            "x": 1.3538849352129523,
-            "y": 5.586407689469935,
-            "heading": -2.872592229193493,
-            "angularVelocity": -3.5151512541727503,
-            "velocityX": -0.4811511101537121,
-            "velocityY": -0.9522478221960695,
-            "timestamp": 5.374465894148996
-        },
-        {
-            "x": 1.341639273384249,
-            "y": 5.556291662758613,
-            "heading": -2.9981488971519656,
-            "angularVelocity": -2.9382367518778816,
-            "velocityX": -0.2865690386755447,
-            "velocityY": -0.7047655687470963,
-            "timestamp": 5.417197872053168
-        },
-        {
-            "x": 1.3350584223241078,
-            "y": 5.537231311212578,
-            "heading": -3.091661499454674,
-            "angularVelocity": -2.18835183600471,
-            "velocityX": -0.15400295944407288,
-            "velocityY": -0.4460442151491003,
-            "timestamp": 5.459929849957341
-        },
-        {
-            "x": 1.332138180732727,
-            "y": 5.52810525894165,
-            "heading": -3.1411853071795863,
-            "angularVelocity": -1.1589402165275682,
-            "velocityX": -0.06833855427730316,
-            "velocityY": -0.21356493938549467,
-            "timestamp": 5.502661827861513
-        },
-        {
-            "x": 1.332138180732727,
-            "y": 5.52810525894165,
-            "heading": -3.1411853071795863,
-            "angularVelocity": 0,
-            "velocityX": 4.554923410438002e-36,
-            "velocityY": -5.3517654956150865e-34,
-            "timestamp": 5.545393805765686
-        },
-        {
-            "x": 1.3511823216382217,
-            "y": 5.531961266421083,
-            "heading": -3.038798036456765,
-            "angularVelocity": 1.6934181625027336,
-            "velocityX": 0.3149775735885273,
-            "velocityY": 0.06377582930300447,
-            "timestamp": 5.60585570052386
-        },
-        {
-            "x": 1.3908490268299478,
-            "y": 5.542189708843604,
-            "heading": -2.844785530962286,
-            "angularVelocity": 3.2088393238494826,
-            "velocityX": 0.6560612324568816,
-            "velocityY": 0.16917171490292857,
-            "timestamp": 5.666317595282035
-        },
-        {
-            "x": 1.4539728254946123,
-            "y": 5.56127979063472,
-            "heading": -2.581938269596155,
-            "angularVelocity": 4.34732094350375,
-            "velocityX": 1.0440261410453116,
-            "velocityY": 0.3157374056415134,
-            "timestamp": 5.726779490040209
-        },
-        {
-            "x": 1.5450944631009007,
-            "y": 5.588995277628573,
-            "heading": -2.2890493808444536,
-            "angularVelocity": 4.8441897152438,
-            "velocityX": 1.5070919952267785,
-            "velocityY": 0.45839593854450544,
-            "timestamp": 5.787241384798383
-        },
-        {
-            "x": 1.6638598735645596,
-            "y": 5.62434991056183,
-            "heading": -2.0324634607383167,
-            "angularVelocity": 4.24376247440454,
-            "velocityX": 1.9643018290888397,
-            "velocityY": 0.5847423914626325,
-            "timestamp": 5.8477032795565576
-        },
-        {
-            "x": 1.8083021892177116,
-            "y": 5.6638944259582,
-            "heading": -1.8350759652142852,
-            "angularVelocity": 3.264659440685901,
-            "velocityX": 2.3889809644714033,
-            "velocityY": 0.6540402935523977,
-            "timestamp": 5.908165174314732
-        },
-        {
-            "x": 1.978235650109889,
-            "y": 5.709112472816866,
-            "heading": -1.6974261571036708,
-            "angularVelocity": 2.276637354174287,
-            "velocityX": 2.8105877523661738,
-            "velocityY": 0.7478767749426485,
-            "timestamp": 5.968627069072906
-        },
-        {
-            "x": 2.1727729031197174,
-            "y": 5.761282618111588,
-            "heading": -1.6235540862695264,
-            "angularVelocity": 1.2217954982986527,
-            "velocityX": 3.217518302856812,
-            "velocityY": 0.8628599137255631,
-            "timestamp": 6.0290889638310805
-        },
-        {
-            "x": 2.3909229420587215,
-            "y": 5.821425961949498,
-            "heading": -1.6168784459669248,
-            "angularVelocity": 0.1104107029609625,
-            "velocityX": 3.60805826234069,
-            "velocityY": 0.994731377150211,
-            "timestamp": 6.089550858589255
-        },
-        {
-            "x": 2.6085958376613507,
-            "y": 5.891661539060254,
-            "heading": -1.616878243648269,
-            "angularVelocity": 0.0000033462175924113523,
-            "velocityX": 3.6001666251651883,
-            "velocityY": 1.1616502822426138,
-            "timestamp": 6.150012753347429
-        },
-        {
-            "x": 2.826268683292055,
-            "y": 5.961897271043331,
-            "heading": -1.6168780413299741,
-            "angularVelocity": 0.0000033462116195770168,
-            "velocityX": 3.6001657986623896,
-            "velocityY": 1.1616528437290037,
-            "timestamp": 6.2104746481056035
-        },
-        {
-            "x": 3.0439415289220437,
-            "y": 6.032133003028614,
-            "heading": -1.6168778390116512,
-            "angularVelocity": 0.0000033462120828803855,
-            "velocityX": 3.6001657986505493,
-            "velocityY": 1.1616528437654758,
-            "timestamp": 6.270936542863778
-        },
-        {
-            "x": 3.2616143745520296,
-            "y": 6.102368735013889,
-            "heading": -1.6168776366933004,
-            "angularVelocity": 0.000003346212545975565,
-            "velocityX": 3.600165798650516,
-            "velocityY": 1.1616528437653553,
-            "timestamp": 6.331398437621952
-        },
-        {
-            "x": 3.479287220182014,
-            "y": 6.172604466999156,
-            "heading": -1.6168774343749215,
-            "angularVelocity": 0.0000033462130098357135,
-            "velocityX": 3.6001657986504836,
-            "velocityY": 1.1616528437652343,
-            "timestamp": 6.391860332380126
-        },
-        {
-            "x": 3.6969600658119965,
-            "y": 6.242840198984417,
-            "heading": -1.6168772320565146,
-            "angularVelocity": 0.000003346213473053834,
-            "velocityX": 3.6001657986504503,
-            "velocityY": 1.1616528437651126,
-            "timestamp": 6.452322227138301
-        },
-        {
-            "x": 3.914632911441977,
-            "y": 6.313075930969669,
-            "heading": -1.61687702973808,
-            "angularVelocity": 0.000003346213936555426,
-            "velocityX": 3.6001657986504174,
-            "velocityY": 1.1616528437649918,
-            "timestamp": 6.512784121896475
-        },
-        {
-            "x": 4.132305757071955,
-            "y": 6.383311662954915,
-            "heading": -1.616876827419617,
-            "angularVelocity": 0.0000033462143992467285,
-            "velocityX": 3.6001657986503846,
-            "velocityY": 1.161652843764871,
-            "timestamp": 6.573246016654649
-        },
-        {
-            "x": 4.349978602701932,
-            "y": 6.453547394940153,
-            "heading": -1.6168766251011262,
-            "angularVelocity": 0.000003346214863174556,
-            "velocityX": 3.600165798650352,
-            "velocityY": 1.1616528437647493,
-            "timestamp": 6.633707911412824
-        },
-        {
-            "x": 4.567651448331906,
-            "y": 6.523783126925384,
-            "heading": -1.6168764227826073,
-            "angularVelocity": 0.000003346215326740712,
-            "velocityX": 3.6001657986503193,
-            "velocityY": 1.1616528437646283,
-            "timestamp": 6.694169806170998
-        },
-        {
-            "x": 4.785324293961879,
-            "y": 6.594018858910608,
-            "heading": -1.6168762204640603,
-            "angularVelocity": 0.0000033462157905377784,
-            "velocityX": 3.6001657986502864,
-            "velocityY": 1.161652843764507,
-            "timestamp": 6.754631700929172
-        },
-        {
-            "x": 5.002997139591849,
-            "y": 6.6642545908958235,
-            "heading": -1.6168760181454853,
-            "angularVelocity": 0.0000033462162531242337,
-            "velocityX": 3.600165798650253,
-            "velocityY": 1.1616528437643858,
-            "timestamp": 6.815093595687347
-        },
-        {
-            "x": 5.220669985221817,
-            "y": 6.734490322881032,
-            "heading": -1.6168758158268823,
-            "angularVelocity": 0.000003346216717473229,
-            "velocityX": 3.6001657986502207,
-            "velocityY": 1.1616528437642646,
-            "timestamp": 6.875555490445521
-        },
-        {
-            "x": 5.438342830851783,
-            "y": 6.804726054866234,
-            "heading": -1.6168756135082514,
-            "angularVelocity": 0.000003346217180660181,
-            "velocityX": 3.600165798650188,
-            "velocityY": 1.1616528437641434,
-            "timestamp": 6.936017385203695
-        },
-        {
-            "x": 5.656015676481748,
-            "y": 6.874961786851428,
-            "heading": -1.6168754111895922,
-            "angularVelocity": 0.0000033462176438124147,
-            "velocityX": 3.600165798650156,
-            "velocityY": 1.1616528437640228,
-            "timestamp": 6.99647927996187
-        },
-        {
-            "x": 5.873688522111711,
-            "y": 6.945197518836615,
-            "heading": -1.6168752088709053,
-            "angularVelocity": 0.0000033462181070326184,
-            "velocityX": 3.6001657986501225,
-            "velocityY": 1.161652843763901,
-            "timestamp": 7.056941174720044
-        },
-        {
-            "x": 6.091361367741672,
-            "y": 7.015433250821794,
-            "heading": -1.6168750065521902,
-            "angularVelocity": 0.0000033462185712785595,
-            "velocityX": 3.6001657986500892,
-            "velocityY": 1.1616528437637803,
-            "timestamp": 7.117403069478218
-        },
-        {
-            "x": 6.30903421337163,
-            "y": 7.0856689828069666,
-            "heading": -1.616874804233447,
-            "angularVelocity": 0.0000033462190342827524,
-            "velocityX": 3.600165798650057,
-            "velocityY": 1.1616528437636586,
-            "timestamp": 7.1778649642363925
-        },
-        {
-            "x": 6.526707059002331,
-            "y": 7.155904714789823,
-            "heading": -1.6168746019146758,
-            "angularVelocity": 0.0000033462194986739715,
-            "velocityX": 3.6001657986623425,
-            "velocityY": 1.1616528437253608,
-            "timestamp": 7.238326858994567
-        },
-        {
-            "x": 6.744379956765645,
-            "y": 7.22614028520157,
-            "heading": -1.6168743995934785,
-            "angularVelocity": 0.0000033462596273946,
-            "velocityX": 3.600166660901485,
-            "velocityY": 1.1616501714453904,
-            "timestamp": 7.298788753752741
-        },
-        {
-            "x": 6.948380246935694,
-            "y": 7.286306890939545,
-            "heading": -1.572684837353693,
-            "angularVelocity": 0.7308663153301367,
-            "velocityX": 3.37403071779302,
-            "velocityY": 0.9951161136881308,
-            "timestamp": 7.3592506485109155
-        },
-        {
-            "x": 7.128326014252639,
-            "y": 7.34087027160758,
-            "heading": -1.456360393496805,
-            "angularVelocity": 1.9239298457672127,
-            "velocityX": 2.976184719924214,
-            "velocityY": 0.9024424538177037,
-            "timestamp": 7.41971254326909
-        },
-        {
-            "x": 7.285215733567768,
-            "y": 7.391288256354559,
-            "heading": -1.2598604932432955,
-            "angularVelocity": 3.249979198294029,
-            "velocityX": 2.5948528398362565,
-            "velocityY": 0.8338803298942721,
-            "timestamp": 7.480174438027264
-        },
-        {
-            "x": 7.415669802167391,
-            "y": 7.433594551714448,
-            "heading": -1.014978314704827,
-            "angularVelocity": 4.050190281298798,
-            "velocityX": 2.157624552147962,
-            "velocityY": 0.6997183189362131,
-            "timestamp": 7.540636332785438
-        },
-        {
-            "x": 7.52162527778786,
-            "y": 7.463369081831009,
-            "heading": -0.7214685317692553,
-            "angularVelocity": 4.854458896954922,
-            "velocityX": 1.7524339262646644,
-            "velocityY": 0.49245115846350496,
-            "timestamp": 7.601098227543613
-        },
-        {
-            "x": 7.599835823210802,
-            "y": 7.482093660895173,
-            "heading": -0.44961761921846705,
-            "angularVelocity": 4.4962354163410385,
-            "velocityX": 1.293551016483298,
-            "velocityY": 0.30969223076874997,
-            "timestamp": 7.661560122301787
-        },
-        {
-            "x": 7.6517032735458015,
-            "y": 7.492160163258041,
-            "heading": -0.22315227789287845,
-            "angularVelocity": 3.7455878984832975,
-            "velocityX": 0.8578535380416231,
-            "velocityY": 0.16649333275330752,
-            "timestamp": 7.722022017059961
-        },
-        {
-            "x": 7.67973874704759,
-            "y": 7.4966228089126075,
-            "heading": -0.06798236605855336,
-            "angularVelocity": 2.5664083544676926,
-            "velocityX": 0.46368830507082526,
-            "velocityY": 0.07380922599953843,
-            "timestamp": 7.782483911818136
-        },
-        {
-            "x": 7.686149597167969,
-            "y": 7.496679306030273,
-            "heading": 0,
-            "angularVelocity": 1.1243836523889659,
-            "velocityX": 0.10603124738348535,
-            "velocityY": 0.0009344251927952234,
-            "timestamp": 7.84294580657631
-        },
-        {
-            "x": 7.671082178881759,
-            "y": 7.493118035056542,
-            "heading": -0.029242529216573936,
-            "angularVelocity": -0.471080674899979,
-            "velocityX": -0.24272762190641095,
-            "velocityY": -0.05737006951015711,
-            "timestamp": 7.905021223324796
-        },
-        {
-            "x": 7.633475390199317,
-            "y": 7.486720174316825,
-            "heading": -0.1516714089178372,
-            "angularVelocity": -1.9722602942371796,
-            "velocityX": -0.6058241837475781,
-            "velocityY": -0.1030659329383018,
-            "timestamp": 7.967096640073281
-        },
-        {
-            "x": 7.569735009380553,
-            "y": 7.475264417263962,
-            "heading": -0.33932636884787254,
-            "angularVelocity": -3.023015708301534,
-            "velocityX": -1.0268216333854858,
-            "velocityY": -0.1845457936960604,
-            "timestamp": 8.029172056821766
-        },
-        {
-            "x": 7.480376336028035,
-            "y": 7.452059634472421,
-            "heading": -0.582197860853168,
-            "angularVelocity": -3.912522939464948,
-            "velocityX": -1.4395178966671722,
-            "velocityY": -0.37381598073777733,
-            "timestamp": 8.09124747357025
-        },
-        {
-            "x": 7.362878563951424,
-            "y": 7.417380486057705,
-            "heading": -0.863364111736863,
-            "angularVelocity": -4.529429935571964,
-            "velocityX": -1.8928229278376825,
-            "velocityY": -0.5586615480203228,
-            "timestamp": 8.153322890318735
-        },
-        {
-            "x": 7.218864923659752,
-            "y": 7.37005336377927,
-            "heading": -1.0948198175456743,
-            "angularVelocity": -3.7286210537516657,
-            "velocityX": -2.319978629788013,
-            "velocityY": -0.7624132830262488,
-            "timestamp": 8.21539830706722
-        },
-        {
-            "x": 7.047313606455814,
-            "y": 7.316250565275482,
-            "heading": -1.2668868462563043,
-            "angularVelocity": -2.7719029162189006,
-            "velocityX": -2.7635950943192666,
-            "velocityY": -0.8667327796087775,
-            "timestamp": 8.277473723815705
-        },
-        {
-            "x": 6.850682482856188,
-            "y": 7.25802494172709,
-            "heading": -1.3587604988602537,
-            "angularVelocity": -1.4800327958521704,
-            "velocityX": -3.16761664921765,
-            "velocityY": -0.9379819999325716,
-            "timestamp": 8.33954914056419
-        },
-        {
-            "x": 6.62975279957275,
-            "y": 7.191687511601611,
-            "heading": -1.3718894789288036,
-            "angularVelocity": -0.21150047404023747,
-            "velocityX": -3.5590527596228974,
-            "velocityY": -1.0686586349353433,
-            "timestamp": 8.401624557312674
-        },
-        {
-            "x": 6.40649462388328,
-            "y": 7.118888182289737,
-            "heading": -1.3718898557853474,
-            "angularVelocity": -0.000006070946652807348,
-            "velocityX": -3.596563460766736,
-            "velocityY": -1.1727561911801452,
-            "timestamp": 8.463699974061159
-        },
-        {
-            "x": 6.183236472249506,
-            "y": 7.0460887792034885,
-            "heading": -1.3718902326404057,
-            "angularVelocity": -0.000006070922729380904,
-            "velocityX": -3.5965630732430243,
-            "velocityY": -1.1727573796437725,
-            "timestamp": 8.525775390809644
-        },
-        {
-            "x": 5.959978320616,
-            "y": 6.973289376116265,
-            "heading": -1.3718906094952235,
-            "angularVelocity": -0.000006070918851907899,
-            "velocityX": -3.5965630732387113,
-            "velocityY": -1.172757379659484,
-            "timestamp": 8.587850807558128
-        },
-        {
-            "x": 5.736720168982443,
-            "y": 6.900489973029042,
-            "heading": -1.3718909863498006,
-            "angularVelocity": -0.000006070914972563024,
-            "velocityX": -3.596563073239526,
-            "velocityY": -1.1727573796594701,
-            "timestamp": 8.649926224306613
-        },
-        {
-            "x": 5.513462017348836,
-            "y": 6.8276905699418196,
-            "heading": -1.3718913632041367,
-            "angularVelocity": -0.000006070911095039567,
-            "velocityX": -3.5965630732403415,
-            "velocityY": -1.1727573796594566,
-            "timestamp": 8.712001641055098
-        },
-        {
-            "x": 5.290203865715178,
-            "y": 6.7548911668545975,
-            "heading": -1.3718917400582324,
-            "angularVelocity": -0.0000060709072179749485,
-            "velocityX": -3.596563073241156,
-            "velocityY": -1.1727573796594426,
-            "timestamp": 8.774077057803582
-        },
-        {
-            "x": 5.066945714081469,
-            "y": 6.682091763767377,
-            "heading": -1.3718921169120872,
-            "angularVelocity": -0.000006070903338953198,
-            "velocityX": -3.596563073241971,
-            "velocityY": -1.1727573796594288,
-            "timestamp": 8.836152474552067
-        },
-        {
-            "x": 4.84368756244771,
-            "y": 6.609292360680157,
-            "heading": -1.3718924937657013,
-            "angularVelocity": -0.000006070899462290803,
-            "velocityX": -3.596563073242785,
-            "velocityY": -1.1727573796594153,
-            "timestamp": 8.898227891300552
-        },
-        {
-            "x": 4.620429410813901,
-            "y": 6.536492957592937,
-            "heading": -1.371892870619075,
-            "angularVelocity": -0.000006070895585804876,
-            "velocityX": -3.5965630732436003,
-            "velocityY": -1.1727573796594017,
-            "timestamp": 8.960303308049037
-        },
-        {
-            "x": 4.39717125918004,
-            "y": 6.46369355450572,
-            "heading": -1.3718932474722079,
-            "angularVelocity": -0.000006070891709350806,
-            "velocityX": -3.5965630732444147,
-            "velocityY": -1.172757379659388,
-            "timestamp": 9.022378724797521
-        },
-        {
-            "x": 4.173913107546129,
-            "y": 6.390894151418502,
-            "heading": -1.3718936243251003,
-            "angularVelocity": -0.000006070887832958865,
-            "velocityX": -3.5965630732452283,
-            "velocityY": -1.1727573796593747,
-            "timestamp": 9.084454141546006
-        },
-        {
-            "x": 3.950654955912168,
-            "y": 6.318094748331286,
-            "heading": -1.3718940011777518,
-            "angularVelocity": -0.000006070883956378075,
-            "velocityX": -3.596563073246043,
-            "velocityY": -1.1727573796593613,
-            "timestamp": 9.14652955829449
-        },
-        {
-            "x": 3.7273968042781562,
-            "y": 6.24529534524407,
-            "heading": -1.3718943780301627,
-            "angularVelocity": -0.000006070880081532027,
-            "velocityX": -3.5965630732468568,
-            "velocityY": -1.1727573796593478,
-            "timestamp": 9.208604975042975
-        },
-        {
-            "x": 3.504138652644094,
-            "y": 6.172495942156855,
-            "heading": -1.3718947548823335,
-            "angularVelocity": -0.000006070876206601658,
-            "velocityX": -3.5965630732476703,
-            "velocityY": -1.1727573796593347,
-            "timestamp": 9.27068039179146
-        },
-        {
-            "x": 3.2808805010099813,
-            "y": 6.099696539069641,
-            "heading": -1.3718951317342634,
-            "angularVelocity": -0.000006070872330584258,
-            "velocityX": -3.596563073248485,
-            "velocityY": -1.1727573796593216,
-            "timestamp": 9.332755808539945
-        },
-        {
-            "x": 3.057622349375818,
-            "y": 6.026897135982428,
-            "heading": -1.371895508585953,
-            "angularVelocity": -0.000006070868457801708,
-            "velocityX": -3.5965630732492984,
-            "velocityY": -1.1727573796593085,
-            "timestamp": 9.39483122528843
-        },
-        {
-            "x": 2.834364197741604,
-            "y": 5.954097732895216,
-            "heading": -1.371895885437402,
-            "angularVelocity": -0.0000060708645841945965,
-            "velocityX": -3.596563073250112,
-            "velocityY": -1.1727573796592947,
-            "timestamp": 9.456906642036914
-        },
-        {
-            "x": 2.6111060461064435,
-            "y": 5.881298329810753,
-            "heading": -1.3718962622886106,
-            "angularVelocity": -0.000006070860710280736,
-            "velocityX": -3.5965630732653646,
-            "velocityY": -1.1727573796150002,
-            "timestamp": 9.518982058785399
-        },
-        {
-            "x": 2.387847826730136,
-            "y": 5.808499134475022,
-            "heading": -1.37189663914262,
-            "angularVelocity": -0.000006070905830150403,
-            "velocityX": -3.5965641645370763,
-            "velocityY": -1.1727540328999504,
-            "timestamp": 9.581057475533884
-        },
-        {
-            "x": 2.167695143748949,
-            "y": 5.749324083617281,
-            "heading": -1.3936267357407348,
-            "angularVelocity": -0.3500596167748615,
-            "velocityX": -3.546535722397048,
-            "velocityY": -0.9532767391237054,
-            "timestamp": 9.643132892282368
-        },
-        {
-            "x": 1.9716359535895844,
-            "y": 5.700085594404985,
-            "heading": -1.4887110872196767,
-            "angularVelocity": -1.5317553463104556,
-            "velocityX": -3.1584031236350607,
-            "velocityY": -0.7932043277582467,
-            "timestamp": 9.705208309030853
-        },
-        {
-            "x": 1.8021216559438722,
-            "y": 5.6575856667748505,
-            "heading": -1.6480423798805228,
-            "angularVelocity": -2.5667373818273083,
-            "velocityX": -2.73077985658225,
-            "velocityY": -0.6846498961470444,
-            "timestamp": 9.767283725779338
-        },
-        {
-            "x": 1.6601304337176481,
-            "y": 5.619404991825654,
-            "heading": -1.8664040726402535,
-            "angularVelocity": -3.5176838786999975,
-            "velocityX": -2.2873986138754163,
-            "velocityY": -0.6150691682650391,
-            "timestamp": 9.829359142527823
-        },
-        {
-            "x": 1.5437233022892554,
-            "y": 5.5885120196672124,
-            "heading": -2.146539077662866,
-            "angularVelocity": -4.5128171455965,
-            "velocityX": -1.8752533212953901,
-            "velocityY": -0.49766838108574224,
-            "timestamp": 9.891434559276307
-        },
-        {
-            "x": 1.452238647018182,
-            "y": 5.5675498489404625,
-            "heading": -2.4956493740995165,
-            "angularVelocity": -5.623970240121965,
-            "velocityX": -1.4737662679212806,
-            "velocityY": -0.33768876351944366,
-            "timestamp": 9.953509976024792
-        },
-        {
-            "x": 1.3894112242957146,
-            "y": 5.555821056487952,
-            "heading": -2.802567916985108,
-            "angularVelocity": -4.94428485481059,
-            "velocityX": -1.0121143926754297,
-            "velocityY": -0.188944240197889,
-            "timestamp": 10.015585392773277
-        },
-        {
-            "x": 1.3504462657255363,
-            "y": 5.550087016097331,
-            "heading": -3.0243894075867264,
-            "angularVelocity": -3.573419273210609,
-            "velocityX": -0.6277035356533314,
-            "velocityY": -0.09237216100947475,
-            "timestamp": 10.077660809521761
-        },
-        {
-            "x": 1.3321380615234375,
-            "y": 5.547595977783203,
-            "heading": -3.1411853071795863,
-            "angularVelocity": -1.8815161574522963,
-            "velocityX": -0.2949348576470964,
-            "velocityY": -0.040129224169711215,
-            "timestamp": 10.139736226270246
-        },
-        {
-            "x": 1.3321380615234375,
-            "y": 5.547595977783203,
-            "heading": -3.1411853071795863,
-            "angularVelocity": 7.133789284871957e-33,
-            "velocityX": 0,
-            "velocityY": 1.0797204950028118e-32,
-            "timestamp": 10.201811643018731
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/auto1.traj b/src/main/deploy/choreo/auto1.traj
deleted file mode 100644
index c037a4f6..00000000
--- a/src/main/deploy/choreo/auto1.traj
+++ /dev/null
@@ -1,1876 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.3321385383605957,
-            "y": 5.547596454620361,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -3.4724754769815887e-38,
-            "velocityY": 7.030705743146232e-38,
-            "timestamp": 0
-        },
-        {
-            "x": 1.361410083208431,
-            "y": 5.5023955382692575,
-            "heading": 0.056119311202146825,
-            "angularVelocity": 0.6842584112779871,
-            "velocityX": 0.35690567728254624,
-            "velocityY": -0.5511312692222103,
-            "timestamp": 0.08201479189321614
-        },
-        {
-            "x": 1.4202660843784345,
-            "y": 5.411892088977607,
-            "heading": 0.16553764559038536,
-            "angularVelocity": 1.3341292693969398,
-            "velocityX": 0.7176266599156221,
-            "velocityY": -1.1035015416424676,
-            "timestamp": 0.1640295837864323
-        },
-        {
-            "x": 1.509321709738084,
-            "y": 5.2758955282995315,
-            "heading": 0.32143534479454616,
-            "angularVelocity": 1.9008485616489859,
-            "velocityX": 1.0858483366707872,
-            "velocityY": -1.658195522280223,
-            "timestamp": 0.24604437567964843
-        },
-        {
-            "x": 1.6296766101338032,
-            "y": 5.093983916449887,
-            "heading": 0.5046862665772869,
-            "angularVelocity": 2.2343642842054003,
-            "velocityX": 1.4674779709545838,
-            "velocityY": -2.2180341820106597,
-            "timestamp": 0.3280591675728646
-        },
-        {
-            "x": 1.782194657966092,
-            "y": 4.869104821237669,
-            "heading": 0.6276214122511846,
-            "angularVelocity": 1.4989387016181202,
-            "velocityX": 1.859640734452738,
-            "velocityY": -2.7419333759331184,
-            "timestamp": 0.4100739594660807
-        },
-        {
-            "x": 1.9333417085303153,
-            "y": 4.6354174904379,
-            "heading": 0.6920907561549033,
-            "angularVelocity": 0.7860697127374073,
-            "velocityX": 1.8429242710390283,
-            "velocityY": -2.849331509662689,
-            "timestamp": 0.49208875135929686
-        },
-        {
-            "x": 2.056917521349593,
-            "y": 4.443493094854259,
-            "heading": 0.6585466805605577,
-            "angularVelocity": -0.40900031348028376,
-            "velocityX": 1.5067503064590375,
-            "velocityY": -2.340119277916704,
-            "timestamp": 0.574103543252513
-        },
-        {
-            "x": 2.1535962335212764,
-            "y": 4.293727999952354,
-            "heading": 0.5265725883978299,
-            "angularVelocity": -1.609149875482936,
-            "velocityX": 1.1787960432498548,
-            "velocityY": -1.8260741927736632,
-            "timestamp": 0.6561183351457291
-        },
-        {
-            "x": 2.2240985983067136,
-            "y": 4.187111119253089,
-            "heading": 0.3017485783287344,
-            "angularVelocity": -2.7412617270530677,
-            "velocityX": 0.859629868685535,
-            "velocityY": -1.2999713617280337,
-            "timestamp": 0.7381331270389453
-        },
-        {
-            "x": 2.2676985263824463,
-            "y": 4.124765396118164,
-            "heading": -1.6642026159430915e-35,
-            "angularVelocity": -3.6791970248685537,
-            "velocityX": 0.5316105432846836,
-            "velocityY": -0.7601765693205621,
-            "timestamp": 0.8201479189321614
-        },
-        {
-            "x": 2.2826958672458773,
-            "y": 4.104348331421266,
-            "heading": -0.17441872129385144,
-            "angularVelocity": -4.1236397648607985,
-            "velocityX": 0.35456991481679806,
-            "velocityY": -0.4827040310886087,
-            "timestamp": 0.8624451930006055
-        },
-        {
-            "x": 2.289905521300629,
-            "y": 4.095598364764115,
-            "heading": -0.366838925198976,
-            "angularVelocity": -4.549234156171776,
-            "velocityX": 0.17045197860942923,
-            "velocityY": -0.20686833489535802,
-            "timestamp": 0.9047424670690496
-        },
-        {
-            "x": 2.289262823237601,
-            "y": 4.0986035079624115,
-            "heading": -0.5765637041805302,
-            "angularVelocity": -4.958352130262211,
-            "velocityX": -0.015194786831599966,
-            "velocityY": 0.07104815297159969,
-            "timestamp": 0.9470397411374937
-        },
-        {
-            "x": 2.28071834356103,
-            "y": 4.1136177509731775,
-            "heading": -0.8009025976234118,
-            "angularVelocity": -5.303861735388993,
-            "velocityX": -0.20201017358104575,
-            "velocityY": 0.3549695185195792,
-            "timestamp": 0.9893370152059378
-        },
-        {
-            "x": 2.264115058699565,
-            "y": 4.140907151677903,
-            "heading": -1.0347556438013104,
-            "angularVelocity": -5.528797099299715,
-            "velocityX": -0.39253794073343345,
-            "velocityY": 0.6451810738575218,
-            "timestamp": 1.0316342892743817
-        },
-        {
-            "x": 2.2392828245472374,
-            "y": 4.180616635647853,
-            "heading": -1.271268857234422,
-            "angularVelocity": -5.5916892670292055,
-            "velocityX": -0.5870882864022154,
-            "velocityY": 0.9388189864361984,
-            "timestamp": 1.0739315633428257
-        },
-        {
-            "x": 2.206219336908872,
-            "y": 4.232592488850575,
-            "heading": -1.500368652073653,
-            "angularVelocity": -5.416419849385782,
-            "velocityX": -0.7816931082807688,
-            "velocityY": 1.228822763344421,
-            "timestamp": 1.1162288374112697
-        },
-        {
-            "x": 2.16560115012261,
-            "y": 4.295896098840919,
-            "heading": -1.7081840928578422,
-            "angularVelocity": -4.913211202403,
-            "velocityX": -0.9603027069908828,
-            "velocityY": 1.496635690704529,
-            "timestamp": 1.1585261114797136
-        },
-        {
-            "x": 2.116418529309532,
-            "y": 4.368321186816809,
-            "heading": -1.8863054196825848,
-            "angularVelocity": -4.211177451684291,
-            "velocityX": -1.162784645022098,
-            "velocityY": 1.7122873653439958,
-            "timestamp": 1.2008233855481576
-        },
-        {
-            "x": 2.058912682392385,
-            "y": 4.450563139928545,
-            "heading": -2.036216963298125,
-            "angularVelocity": -3.54423652391779,
-            "velocityX": -1.3595639005977715,
-            "velocityY": 1.9443795119906613,
-            "timestamp": 1.2431206596166016
-        },
-        {
-            "x": 1.99337005684811,
-            "y": 4.542961602948437,
-            "heading": -2.1579434366118924,
-            "angularVelocity": -2.877879863293171,
-            "velocityX": -1.549570911785386,
-            "velocityY": 2.184501603350988,
-            "timestamp": 1.2854179336850455
-        },
-        {
-            "x": 1.9201828437796038,
-            "y": 4.645652670813241,
-            "heading": -2.2501769997399386,
-            "angularVelocity": -2.180603009517744,
-            "velocityX": -1.7303056681637912,
-            "velocityY": 2.4278412764527704,
-            "timestamp": 1.3277152077534895
-        },
-        {
-            "x": 1.8398838191629743,
-            "y": 4.758671823925337,
-            "heading": -2.3107451299036645,
-            "angularVelocity": -1.4319629691907945,
-            "velocityX": -1.8984444360809696,
-            "velocityY": 2.6720197838095423,
-            "timestamp": 1.3700124818219335
-        },
-        {
-            "x": 1.753396216406846,
-            "y": 4.882199443808802,
-            "heading": -2.337574910152261,
-            "angularVelocity": -0.6343146417705776,
-            "velocityX": -2.0447559484844615,
-            "velocityY": 2.920462904620659,
-            "timestamp": 1.4123097558903774
-        },
-        {
-            "x": 1.6751428992292787,
-            "y": 4.996901240775032,
-            "heading": -2.400008661282964,
-            "angularVelocity": -1.4760703261792811,
-            "velocityX": -1.8500794413119903,
-            "velocityY": 2.7118011619525055,
-            "timestamp": 1.4546070299588214
-        },
-        {
-            "x": 1.604167639142011,
-            "y": 5.102445488685979,
-            "heading": -2.5005028957452873,
-            "angularVelocity": -2.375903333621606,
-            "velocityX": -1.6780102654468432,
-            "velocityY": 2.4952966883908108,
-            "timestamp": 1.4969043040272654
-        },
-        {
-            "x": 1.5418087398499432,
-            "y": 5.19740105341742,
-            "heading": -2.6238103064581355,
-            "angularVelocity": -2.9152566785584497,
-            "velocityX": -1.4743006651246806,
-            "velocityY": 2.2449570763776965,
-            "timestamp": 1.5392015780957093
-        },
-        {
-            "x": 1.488225747512813,
-            "y": 5.280327915096259,
-            "heading": -2.743877445785595,
-            "angularVelocity": -2.8386495813694843,
-            "velocityX": -1.2668190448969345,
-            "velocityY": 1.960572247389977,
-            "timestamp": 1.5814988521641533
-        },
-        {
-            "x": 1.4429482417442723,
-            "y": 5.35126150411709,
-            "heading": -2.8526923539591857,
-            "angularVelocity": -2.5726222450531995,
-            "velocityX": -1.0704591907098815,
-            "velocityY": 1.6770250703638487,
-            "timestamp": 1.6237961262325973
-        },
-        {
-            "x": 1.4056282761179824,
-            "y": 5.410288137316586,
-            "heading": -2.94638853961619,
-            "angularVelocity": -2.215182602675249,
-            "velocityX": -0.8823255504811011,
-            "velocityY": 1.3955186120027623,
-            "timestamp": 1.6660934003010412
-        },
-        {
-            "x": 1.3760265384921018,
-            "y": 5.457458867956814,
-            "heading": -3.023001318784294,
-            "angularVelocity": -1.8112935373596752,
-            "velocityX": -0.6998497723040017,
-            "velocityY": 1.115219164334259,
-            "timestamp": 1.7083906743694852
-        },
-        {
-            "x": 1.3539833473819511,
-            "y": 5.492803685860998,
-            "heading": -3.0814563447777146,
-            "angularVelocity": -1.3820045683991566,
-            "velocityX": -0.5211492134098515,
-            "velocityY": 0.8356287416297835,
-            "timestamp": 1.7506879484379292
-        },
-        {
-            "x": 1.3393859043766252,
-            "y": 5.516345418500216,
-            "heading": -3.121051917122202,
-            "angularVelocity": -0.9361258666554966,
-            "velocityX": -0.3451154554713123,
-            "velocityY": 0.5565780102311921,
-            "timestamp": 1.7929852225063732
-        },
-        {
-            "x": 1.3321385383605957,
-            "y": 5.52810525894165,
-            "heading": -3.1411853071795863,
-            "angularVelocity": -0.47599734263737736,
-            "velocityX": -0.17134357179382342,
-            "velocityY": 0.2780283292584069,
-            "timestamp": 1.8352824965748171
-        },
-        {
-            "x": 1.3321385383605957,
-            "y": 5.52810525894165,
-            "heading": -3.1411853071795863,
-            "angularVelocity": 1.0972891859651771e-35,
-            "velocityX": 2.010813957988482e-36,
-            "velocityY": -7.697026441410569e-35,
-            "timestamp": 1.877579770643261
-        },
-        {
-            "x": 1.337370959637994,
-            "y": 5.5282378526253355,
-            "heading": -3.1057781238593085,
-            "angularVelocity": 1.0527517345397082,
-            "velocityX": 0.1555740970920068,
-            "velocityY": 0.003942370372304683,
-            "timestamp": 1.9112127556658494
-        },
-        {
-            "x": 1.3479554232342674,
-            "y": 5.5287097500206475,
-            "heading": -3.0355501553802027,
-            "angularVelocity": 2.0880682589410555,
-            "velocityX": 0.314704852666657,
-            "velocityY": 0.014030791349476537,
-            "timestamp": 1.9448457406884376
-        },
-        {
-            "x": 1.364027106419543,
-            "y": 5.529829382282991,
-            "heading": -2.9313355116530078,
-            "angularVelocity": 3.0985844300529233,
-            "velocityX": 0.4778547956561592,
-            "velocityY": 0.03328970835004707,
-            "timestamp": 1.978478725711026
-        },
-        {
-            "x": 1.3857940218160676,
-            "y": 5.531991335292171,
-            "heading": -2.7946612849381784,
-            "angularVelocity": 4.06369600031153,
-            "velocityX": 0.6471895189173892,
-            "velocityY": 0.06428073534739039,
-            "timestamp": 2.012111710733614
-        },
-        {
-            "x": 1.4139333100804088,
-            "y": 5.535564467259386,
-            "heading": -2.6300339585531556,
-            "angularVelocity": 4.89481758084981,
-            "velocityX": 0.8366574731752962,
-            "velocityY": 0.10623891887132897,
-            "timestamp": 2.0457446957562024
-        },
-        {
-            "x": 1.4504189067700664,
-            "y": 5.5389912877920775,
-            "heading": -2.4501533377113245,
-            "angularVelocity": 5.348339456667967,
-            "velocityX": 1.0848158932415233,
-            "velocityY": 0.10188868250590417,
-            "timestamp": 2.0793776807787907
-        },
-        {
-            "x": 1.4957426020786804,
-            "y": 5.541383162087915,
-            "heading": -2.2591276091627437,
-            "angularVelocity": 5.679713781583368,
-            "velocityX": 1.3475965715851275,
-            "velocityY": 0.07111691972124916,
-            "timestamp": 2.113010665801379
-        },
-        {
-            "x": 1.549887277450098,
-            "y": 5.541604080123591,
-            "heading": -2.077370178961034,
-            "angularVelocity": 5.404142096802884,
-            "velocityX": 1.6098682687562116,
-            "velocityY": 0.006568493267186675,
-            "timestamp": 2.1466436508239672
-        },
-        {
-            "x": 1.612403659002375,
-            "y": 5.539682953328213,
-            "heading": -1.9100118188035742,
-            "angularVelocity": 4.9760186330490965,
-            "velocityX": 1.8587818330811392,
-            "velocityY": -0.05712031786913091,
-            "timestamp": 2.1802766358465555
-        },
-        {
-            "x": 1.6835678333886066,
-            "y": 5.53654594415181,
-            "heading": -1.7561585727792173,
-            "angularVelocity": 4.5744749067330135,
-            "velocityX": 2.1159042035203575,
-            "velocityY": -0.0932718036860474,
-            "timestamp": 2.213909620869144
-        },
-        {
-            "x": 1.7632443244228584,
-            "y": 5.534877661418315,
-            "heading": -1.616595649842294,
-            "angularVelocity": 4.149584785388266,
-            "velocityX": 2.3689984989658393,
-            "velocityY": -0.04960257712408059,
-            "timestamp": 2.247542605891732
-        },
-        {
-            "x": 1.845517380604842,
-            "y": 5.542129045770498,
-            "heading": -1.4828842082324165,
-            "angularVelocity": 3.9756043515042103,
-            "velocityX": 2.4462014337035023,
-            "velocityY": 0.21560335329479804,
-            "timestamp": 2.2811755909143203
-        },
-        {
-            "x": 1.9198078929781421,
-            "y": 5.552688491598465,
-            "heading": -1.3358148203184805,
-            "angularVelocity": 4.372772378519569,
-            "velocityX": 2.208858723761989,
-            "velocityY": 0.31396100646060415,
-            "timestamp": 2.3148085759369086
-        },
-        {
-            "x": 1.9854932228404973,
-            "y": 5.561629411594934,
-            "heading": -1.1756047034069004,
-            "angularVelocity": 4.763481945000775,
-            "velocityX": 1.9530032739657306,
-            "velocityY": 0.26583783718469556,
-            "timestamp": 2.348441560959497
-        },
-        {
-            "x": 2.0429500865174064,
-            "y": 5.56779505699511,
-            "heading": -1.0015843737719388,
-            "angularVelocity": 5.1740970811270035,
-            "velocityX": 1.7083486237787309,
-            "velocityY": 0.18332138512343155,
-            "timestamp": 2.382074545982085
-        },
-        {
-            "x": 2.0926336267477486,
-            "y": 5.570712245735625,
-            "heading": -0.8117369336213589,
-            "angularVelocity": 5.644680066996039,
-            "velocityX": 1.477226603495769,
-            "velocityY": 0.08673594504191894,
-            "timestamp": 2.4157075310046734
-        },
-        {
-            "x": 2.1331736827499013,
-            "y": 5.571318223980459,
-            "heading": -0.6223345825199257,
-            "angularVelocity": 5.63144636059598,
-            "velocityX": 1.2053659814888902,
-            "velocityY": 0.01801737920159694,
-            "timestamp": 2.4493405160272617
-        },
-        {
-            "x": 2.1650842656051874,
-            "y": 5.570653927593427,
-            "heading": -0.4456863837750604,
-            "angularVelocity": 5.252230767689132,
-            "velocityX": 0.9487883051074633,
-            "velocityY": -0.01975133597523799,
-            "timestamp": 2.48297350104985
-        },
-        {
-            "x": 2.1896037980440752,
-            "y": 5.569969320542384,
-            "heading": -0.29224766364008553,
-            "angularVelocity": 4.56214992608965,
-            "velocityX": 0.7290323003569462,
-            "velocityY": -0.020355227184960373,
-            "timestamp": 2.516606486072438
-        },
-        {
-            "x": 2.20780773019257,
-            "y": 5.569409848980837,
-            "heading": -0.16876882632138132,
-            "angularVelocity": 3.6713612317127033,
-            "velocityX": 0.5412523490337908,
-            "velocityY": -0.016634609184155776,
-            "timestamp": 2.5502394710950265
-        },
-        {
-            "x": 2.220213327560089,
-            "y": 5.568812984785934,
-            "heading": -0.07803807473080998,
-            "angularVelocity": 2.697671691336225,
-            "velocityX": 0.368852106323223,
-            "velocityY": -0.017746393741217954,
-            "timestamp": 2.5838724561176147
-        },
-        {
-            "x": 2.227117258661317,
-            "y": 5.568051315056596,
-            "heading": -0.021486347391680306,
-            "angularVelocity": 1.6814364618885036,
-            "velocityX": 0.2052726243772784,
-            "velocityY": -0.022646509931434324,
-            "timestamp": 2.617505441140203
-        },
-        {
-            "x": 2.2287168502807617,
-            "y": 5.567087173461914,
-            "heading": 6.38149991346766e-37,
-            "angularVelocity": 0.6388474700431709,
-            "velocityX": 0.047560203721742585,
-            "velocityY": -0.02866654845040371,
-            "timestamp": 2.6511384261627913
-        },
-        {
-            "x": 2.2247758271699722,
-            "y": 5.5659373744789455,
-            "heading": -0.016384469566950412,
-            "angularVelocity": -0.4672993433012807,
-            "velocityX": -0.11240141184197158,
-            "velocityY": -0.03279326849576676,
-            "timestamp": 2.686200468945049
-        },
-        {
-            "x": 2.215132755191586,
-            "y": 5.564807940629173,
-            "heading": -0.07111277017786721,
-            "angularVelocity": -1.560898803038675,
-            "velocityX": -0.27502881216225394,
-            "velocityY": -0.03221243715853021,
-            "timestamp": 2.7212625117273066
-        },
-        {
-            "x": 2.199681422128445,
-            "y": 5.563984363308005,
-            "heading": -0.16355266498461823,
-            "angularVelocity": -2.6364663171744263,
-            "velocityX": -0.4406854774291637,
-            "velocityY": -0.023489142554608954,
-            "timestamp": 2.7563245545095643
-        },
-        {
-            "x": 2.178275880729617,
-            "y": 5.563860339253889,
-            "heading": -0.2925810137194886,
-            "angularVelocity": -3.6800008925938044,
-            "velocityX": -0.6105046854160964,
-            "velocityY": -0.0035372740511740697,
-            "timestamp": 2.791386597291822
-        },
-        {
-            "x": 2.150434792823077,
-            "y": 5.56488104971689,
-            "heading": -0.45485951159806126,
-            "angularVelocity": -4.628324107820951,
-            "velocityX": -0.7940520773258658,
-            "velocityY": 0.029111551467175232,
-            "timestamp": 2.8264486400740796
-        },
-        {
-            "x": 2.1136927979170794,
-            "y": 5.564806154665508,
-            "heading": -0.6342616510080993,
-            "angularVelocity": -5.116705279386068,
-            "velocityX": -1.047913697846207,
-            "velocityY": -0.0021360721007366726,
-            "timestamp": 2.8615106828563373
-        },
-        {
-            "x": 2.0677952991091333,
-            "y": 5.562722960469238,
-            "heading": -0.8285761603492735,
-            "angularVelocity": -5.542019058841107,
-            "velocityX": -1.3090366437853844,
-            "velocityY": -0.05941451298794179,
-            "timestamp": 2.896572725638595
-        },
-        {
-            "x": 2.0120787764368706,
-            "y": 5.557776466918295,
-            "heading": -1.020468287532569,
-            "angularVelocity": -5.472930609747536,
-            "velocityX": -1.5890837569925207,
-            "velocityY": -0.14107830458316797,
-            "timestamp": 2.9316347684208526
-        },
-        {
-            "x": 1.947346732087487,
-            "y": 5.549990941392819,
-            "heading": -1.1972983758508349,
-            "angularVelocity": -5.043348141932757,
-            "velocityX": -1.8462142879518657,
-            "velocityY": -0.22204996935933435,
-            "timestamp": 2.9666968112031102
-        },
-        {
-            "x": 1.8731816133340355,
-            "y": 5.541067205657786,
-            "heading": -1.3595832736612468,
-            "angularVelocity": -4.628506639451492,
-            "velocityX": -2.1152537863818077,
-            "velocityY": -0.25451271594333,
-            "timestamp": 3.001758853985368
-        },
-        {
-            "x": 1.7903438797008602,
-            "y": 5.535778355661316,
-            "heading": -1.5070368489944903,
-            "angularVelocity": -4.2055044039778195,
-            "velocityX": -2.362604316799635,
-            "velocityY": -0.15084260860996357,
-            "timestamp": 3.0368208967676256
-        },
-        {
-            "x": 1.705207783905628,
-            "y": 5.540259742780199,
-            "heading": -1.6469795594362229,
-            "angularVelocity": -3.991287995134942,
-            "velocityX": -2.4281556075880766,
-            "velocityY": 0.12781306402233675,
-            "timestamp": 3.0718829395498832
-        },
-        {
-            "x": 1.629345764073995,
-            "y": 5.546176174189709,
-            "heading": -1.802120558201536,
-            "angularVelocity": -4.424756416184017,
-            "velocityX": -2.1636508831716217,
-            "velocityY": 0.16874177714778527,
-            "timestamp": 3.106944982332141
-        },
-        {
-            "x": 1.5628215543297233,
-            "y": 5.550494777199676,
-            "heading": -1.9719374235468123,
-            "angularVelocity": -4.843324914063712,
-            "velocityX": -1.8973284060315727,
-            "velocityY": 0.12317031944734674,
-            "timestamp": 3.1420070251143986
-        },
-        {
-            "x": 1.5052956910676978,
-            "y": 5.552468250746859,
-            "heading": -2.1578472190724005,
-            "angularVelocity": -5.302309300120524,
-            "velocityX": -1.6406877265900472,
-            "velocityY": 0.056285184506758255,
-            "timestamp": 3.177069067896656
-        },
-        {
-            "x": 1.4562451550739939,
-            "y": 5.551766768072451,
-            "heading": -2.3624196209535056,
-            "angularVelocity": -5.834583088941523,
-            "velocityX": -1.3989640106914938,
-            "velocityY": -0.02000689688175958,
-            "timestamp": 3.212131110678914
-        },
-        {
-            "x": 1.4173942622657685,
-            "y": 5.550106751106204,
-            "heading": -2.5657566721184293,
-            "angularVelocity": -5.799349810497013,
-            "velocityX": -1.1080613029165842,
-            "velocityY": -0.047345129790485885,
-            "timestamp": 3.2471931534611715
-        },
-        {
-            "x": 1.3874992845964569,
-            "y": 5.548698391964879,
-            "heading": -2.750369265628105,
-            "angularVelocity": -5.26531196873374,
-            "velocityX": -0.852630802345583,
-            "velocityY": -0.0401676294239809,
-            "timestamp": 3.282255196243429
-        },
-        {
-            "x": 1.3647199881577643,
-            "y": 5.548009174125998,
-            "heading": -2.903871904539791,
-            "angularVelocity": -4.378028966109259,
-            "velocityX": -0.6496853757254475,
-            "velocityY": -0.019657093089571077,
-            "timestamp": 3.317317239025687
-        },
-        {
-            "x": 1.3481785543906255,
-            "y": 5.547743770300603,
-            "heading": -3.0214740080658498,
-            "angularVelocity": -3.354114426714698,
-            "velocityX": -0.4717760990101023,
-            "velocityY": -0.007569548273095167,
-            "timestamp": 3.3523792818079445
-        },
-        {
-            "x": 1.3374168488281284,
-            "y": 5.547647789865521,
-            "heading": -3.10100248389671,
-            "angularVelocity": -2.268221401837531,
-            "velocityX": -0.30693321633680876,
-            "velocityY": -0.0027374456097331258,
-            "timestamp": 3.387441324590202
-        },
-        {
-            "x": 1.3321385383605957,
-            "y": 5.547596454620361,
-            "heading": -3.1411853071795863,
-            "angularVelocity": -1.1460491201958698,
-            "velocityX": -0.15054201206449694,
-            "velocityY": -0.001464125906141093,
-            "timestamp": 3.42250336737246
-        },
-        {
-            "x": 1.3321385383605957,
-            "y": 5.547596454620361,
-            "heading": -3.1411853071795863,
-            "angularVelocity": -1.1877010663188592e-33,
-            "velocityX": -2.3472123166291743e-34,
-            "velocityY": -4.828629969149868e-35,
-            "timestamp": 3.4575654101547175
-        },
-        {
-            "x": 1.3353773426968816,
-            "y": 5.556976212978418,
-            "heading": -3.1027759330153977,
-            "angularVelocity": 0.9418411865422114,
-            "velocityX": 0.07941913622507528,
-            "velocityY": 0.2300022568362304,
-            "timestamp": 3.4983465680749837
-        },
-        {
-            "x": 1.342768335196551,
-            "y": 5.576502155287014,
-            "heading": -3.0330086785474775,
-            "angularVelocity": 1.7107717883912714,
-            "velocityX": 0.18123547433645554,
-            "velocityY": 0.47879813385321396,
-            "timestamp": 3.53912772599525
-        },
-        {
-            "x": 1.357543783583743,
-            "y": 5.605477021924564,
-            "heading": -2.939156767804727,
-            "angularVelocity": 2.301354731669143,
-            "velocityX": 0.3623106635687124,
-            "velocityY": 0.7104964183263536,
-            "timestamp": 3.579908883915516
-        },
-        {
-            "x": 1.3800861680237329,
-            "y": 5.643206466545064,
-            "heading": -2.8183134070613347,
-            "angularVelocity": 2.9632155364411483,
-            "velocityX": 0.5527646979535052,
-            "velocityY": 0.9251685470595896,
-            "timestamp": 3.620690041835782
-        },
-        {
-            "x": 1.410132685953878,
-            "y": 5.690126983565226,
-            "heading": -2.6706561536339386,
-            "angularVelocity": 3.6207224354956073,
-            "velocityX": 0.7367745170181611,
-            "velocityY": 1.1505440113274845,
-            "timestamp": 3.6614711997560483
-        },
-        {
-            "x": 1.447426110042683,
-            "y": 5.747148126734536,
-            "heading": -2.4998618512700794,
-            "angularVelocity": 4.188068977781135,
-            "velocityX": 0.9144768317201708,
-            "velocityY": 1.3982227596576835,
-            "timestamp": 3.7022523576763144
-        },
-        {
-            "x": 1.4919514039971702,
-            "y": 5.815443990853687,
-            "heading": -2.3159167566388894,
-            "angularVelocity": 4.510541240413857,
-            "velocityX": 1.0918104395549988,
-            "velocityY": 1.6746916370712401,
-            "timestamp": 3.7430335155965806
-        },
-        {
-            "x": 1.543306542439651,
-            "y": 5.895557258888599,
-            "heading": -2.140840879075951,
-            "angularVelocity": 4.293058031977455,
-            "velocityX": 1.259285931578716,
-            "velocityY": 1.9644677130440258,
-            "timestamp": 3.7838146735168467
-        },
-        {
-            "x": 1.5991978574253154,
-            "y": 5.9852989254543125,
-            "heading": -2.0026162516135204,
-            "angularVelocity": 3.389423805294663,
-            "velocityX": 1.3705180979642946,
-            "velocityY": 2.2005669074226377,
-            "timestamp": 3.824595831437113
-        },
-        {
-            "x": 1.6602838186413746,
-            "y": 6.084712915583529,
-            "heading": -1.8987034819220077,
-            "angularVelocity": 2.5480583433820057,
-            "velocityX": 1.497896683941456,
-            "velocityY": 2.4377431931576705,
-            "timestamp": 3.865376989357379
-        },
-        {
-            "x": 1.7272241659282512,
-            "y": 6.1936323149644315,
-            "heading": -1.8270894144503607,
-            "angularVelocity": 1.7560577267488233,
-            "velocityX": 1.6414528350998836,
-            "velocityY": 2.670826551660404,
-            "timestamp": 3.906158147277645
-        },
-        {
-            "x": 1.8006198616667228,
-            "y": 6.311769110233653,
-            "heading": -1.7863461946081067,
-            "angularVelocity": 0.9990697155268056,
-            "velocityX": 1.7997452618185266,
-            "velocityY": 2.8968474975673533,
-            "timestamp": 3.9469393051979114
-        },
-        {
-            "x": 1.8749454151333007,
-            "y": 6.423903525675366,
-            "heading": -1.7351071665224345,
-            "angularVelocity": 1.2564387746383576,
-            "velocityX": 1.8225464223428127,
-            "velocityY": 2.7496623725337597,
-            "timestamp": 3.9877204631181775
-        },
-        {
-            "x": 1.9436310828301862,
-            "y": 6.525803671980358,
-            "heading": -1.6558915421182487,
-            "angularVelocity": 1.9424564785302534,
-            "velocityX": 1.6842500605592803,
-            "velocityY": 2.4987065473772168,
-            "timestamp": 4.028501621038443
-        },
-        {
-            "x": 2.0070825887496224,
-            "y": 6.617181897860731,
-            "heading": -1.5496488374577189,
-            "angularVelocity": 2.6051909773688204,
-            "velocityX": 1.5559025087883662,
-            "velocityY": 2.240697188123809,
-            "timestamp": 4.069282778958709
-        },
-        {
-            "x": 2.0657895498392027,
-            "y": 6.697945113539011,
-            "heading": -1.416445908140072,
-            "angularVelocity": 3.2662861014903286,
-            "velocityX": 1.4395609169401762,
-            "velocityY": 1.9804051625063046,
-            "timestamp": 4.1100639368789755
-        },
-        {
-            "x": 2.1168235029971925,
-            "y": 6.770262031139991,
-            "heading": -1.2545233571884964,
-            "angularVelocity": 3.9705236243698976,
-            "velocityX": 1.2514101060536296,
-            "velocityY": 1.7732924048496255,
-            "timestamp": 4.150845094799242
-        },
-        {
-            "x": 2.160098891028687,
-            "y": 6.834337445285054,
-            "heading": -1.0613678161788878,
-            "angularVelocity": 4.736391776497875,
-            "velocityX": 1.0611613362255508,
-            "velocityY": 1.571201442350923,
-            "timestamp": 4.191626252719508
-        },
-        {
-            "x": 2.1954531720383796,
-            "y": 6.890659821255424,
-            "heading": -0.8342118106482137,
-            "angularVelocity": 5.570121524621797,
-            "velocityX": 0.8669268557507981,
-            "velocityY": 1.3810881996163518,
-            "timestamp": 4.232407410639774
-        },
-        {
-            "x": 2.2216225780243426,
-            "y": 6.9367359795772305,
-            "heading": -0.6132278215539244,
-            "angularVelocity": 5.418776718560821,
-            "velocityX": 0.6417033581324141,
-            "velocityY": 1.1298393834695264,
-            "timestamp": 4.27318856856004
-        },
-        {
-            "x": 2.2400584030513464,
-            "y": 6.973023548052357,
-            "heading": -0.41519671142857095,
-            "angularVelocity": 4.855946231652809,
-            "velocityX": 0.4520672282785309,
-            "velocityY": 0.889812117303642,
-            "timestamp": 4.313969726480306
-        },
-        {
-            "x": 2.252392360052711,
-            "y": 6.999798504355172,
-            "heading": -0.24838728557433673,
-            "angularVelocity": 4.0903553101747345,
-            "velocityX": 0.3024425403878878,
-            "velocityY": 0.6565521350611045,
-            "timestamp": 4.3547508844005725
-        },
-        {
-            "x": 2.2601470415559044,
-            "y": 7.01732009026677,
-            "heading": -0.11829622013640562,
-            "angularVelocity": 3.189979688469885,
-            "velocityX": 0.19015353900334692,
-            "velocityY": 0.4296490537580049,
-            "timestamp": 4.395532042320839
-        },
-        {
-            "x": 2.2649306384373356,
-            "y": 7.026470873344902,
-            "heading": -0.032599297392582494,
-            "angularVelocity": 2.101385225779393,
-            "velocityX": 0.11729919220988534,
-            "velocityY": 0.2243875246510465,
-            "timestamp": 4.436313200241105
-        },
-        {
-            "x": 2.2676985263824463,
-            "y": 7.028899669647217,
-            "heading": 1.4623510884357687e-33,
-            "angularVelocity": 0.799371549388559,
-            "velocityX": 0.06787173504299782,
-            "velocityY": 0.05955682541099079,
-            "timestamp": 4.477094358161371
-        },
-        {
-            "x": 2.268447419698579,
-            "y": 7.024856278106883,
-            "heading": -0.027481442707952487,
-            "angularVelocity": -0.643111881448138,
-            "velocityX": 0.017525360464520197,
-            "velocityY": -0.09462214806441192,
-            "timestamp": 4.5198263360655435
-        },
-        {
-            "x": 2.2661447146026696,
-            "y": 7.012526995839087,
-            "heading": -0.10890493556394686,
-            "angularVelocity": -1.9054463858094515,
-            "velocityX": -0.05388716387229158,
-            "velocityY": -0.28852589729042577,
-            "timestamp": 4.562558313969716
-        },
-        {
-            "x": 2.259237652200008,
-            "y": 6.990956553960971,
-            "heading": -0.23735745507492811,
-            "angularVelocity": -3.0060045383117964,
-            "velocityX": -0.16163685233926556,
-            "velocityY": -0.504784541602271,
-            "timestamp": 4.605290291873889
-        },
-        {
-            "x": 2.2461382818263638,
-            "y": 6.959794967044901,
-            "heading": -0.4073842771005642,
-            "angularVelocity": -3.9789129912714607,
-            "velocityX": -0.30654725140549266,
-            "velocityY": -0.7292334323009891,
-            "timestamp": 4.648022269778061
-        },
-        {
-            "x": 2.225216379141952,
-            "y": 6.918625356851511,
-            "heading": -0.6110307557476551,
-            "angularVelocity": -4.765669380054774,
-            "velocityX": -0.48960763602681573,
-            "velocityY": -0.9634379734473075,
-            "timestamp": 4.690754247682234
-        },
-        {
-            "x": 2.1950172148844427,
-            "y": 6.866427702804465,
-            "heading": -0.8329684676174923,
-            "angularVelocity": -5.193714935628281,
-            "velocityX": -0.706711126857547,
-            "velocityY": -1.2215127079794852,
-            "timestamp": 4.7334862255864065
-        },
-        {
-            "x": 2.1547332634682537,
-            "y": 6.802939586567042,
-            "heading": -1.0490897028487878,
-            "angularVelocity": -5.057599620498584,
-            "velocityX": -0.9427120716604103,
-            "velocityY": -1.4857284720074833,
-            "timestamp": 4.776218203490579
-        },
-        {
-            "x": 2.1052130726477585,
-            "y": 6.731031156240311,
-            "heading": -1.2322360836735529,
-            "angularVelocity": -4.285932685715518,
-            "velocityX": -1.1588555748939446,
-            "velocityY": -1.6827779535033027,
-            "timestamp": 4.818950181394752
-        },
-        {
-            "x": 2.046678257521749,
-            "y": 6.650499615743051,
-            "heading": -1.3842475177981566,
-            "angularVelocity": -3.557322679177027,
-            "velocityX": -1.3698129128793415,
-            "velocityY": -1.8845732036521736,
-            "timestamp": 4.861682159298924
-        },
-        {
-            "x": 1.9824374188049456,
-            "y": 6.55859622988028,
-            "heading": -1.505962481707897,
-            "angularVelocity": -2.848334429608893,
-            "velocityX": -1.5033434413184743,
-            "velocityY": -2.150693470563594,
-            "timestamp": 4.904414137203097
-        },
-        {
-            "x": 1.9120980242519563,
-            "y": 6.455385209029271,
-            "heading": -1.5975173206853661,
-            "angularVelocity": -2.1425368884815836,
-            "velocityX": -1.6460598830863262,
-            "velocityY": -2.415311106882586,
-            "timestamp": 4.9471461151072695
-        },
-        {
-            "x": 1.835418260391466,
-            "y": 6.341055354736124,
-            "heading": -1.6582681711046532,
-            "angularVelocity": -1.4216718579121885,
-            "velocityX": -1.7944351659182955,
-            "velocityY": -2.675510470157384,
-            "timestamp": 4.989878093011442
-        },
-        {
-            "x": 1.7526348309039128,
-            "y": 6.215560469564215,
-            "heading": -1.6873132093111056,
-            "angularVelocity": -0.6797026402940408,
-            "velocityX": -1.937271185368423,
-            "velocityY": -2.9367909309822835,
-            "timestamp": 5.032610070915615
-        },
-        {
-            "x": 1.677441366215122,
-            "y": 6.099853521630283,
-            "heading": -1.7491066585439166,
-            "angularVelocity": -1.4460704199413479,
-            "velocityX": -1.7596532708458867,
-            "velocityY": -2.70773677252684,
-            "timestamp": 5.075342048819787
-        },
-        {
-            "x": 1.6091252487579695,
-            "y": 5.994372460119343,
-            "heading": -1.8457597622108368,
-            "angularVelocity": -2.2618448386280567,
-            "velocityX": -1.598711803379506,
-            "velocityY": -2.4684338681322977,
-            "timestamp": 5.11807402672396
-        },
-        {
-            "x": 1.547030840271468,
-            "y": 5.899198607270202,
-            "heading": -1.9803343034636756,
-            "angularVelocity": -3.149270121655174,
-            "velocityX": -1.4531133715773699,
-            "velocityY": -2.2272278868666358,
-            "timestamp": 5.1608060046281325
-        },
-        {
-            "x": 1.4904311214521733,
-            "y": 5.813988086882659,
-            "heading": -2.1571135876817413,
-            "angularVelocity": -4.136931939225903,
-            "velocityX": -1.3245284116317142,
-            "velocityY": -1.9940691858127988,
-            "timestamp": 5.203537982532305
-        },
-        {
-            "x": 1.4423633386018753,
-            "y": 5.740159267538946,
-            "heading": -2.35427402869071,
-            "angularVelocity": -4.613885213811227,
-            "velocityX": -1.1248667908162702,
-            "velocityY": -1.7277182794879316,
-            "timestamp": 5.246269960436478
-        },
-        {
-            "x": 1.4038900802302197,
-            "y": 5.6781134856885735,
-            "heading": -2.5480008926422313,
-            "angularVelocity": -4.533533748097493,
-            "velocityX": -0.9003388155337229,
-            "velocityY": -1.451975426681909,
-            "timestamp": 5.28900193834065
-        },
-        {
-            "x": 1.3744454738206087,
-            "y": 5.627099122367314,
-            "heading": -2.7223828634703597,
-            "angularVelocity": -4.0808307824923435,
-            "velocityX": -0.6890532068429254,
-            "velocityY": -1.1938217190802813,
-            "timestamp": 5.331733916244823
-        },
-        {
-            "x": 1.3538849352129523,
-            "y": 5.586407689469935,
-            "heading": -2.872592229193493,
-            "angularVelocity": -3.5151512541727503,
-            "velocityX": -0.4811511101537121,
-            "velocityY": -0.9522478221960695,
-            "timestamp": 5.374465894148996
-        },
-        {
-            "x": 1.341639273384249,
-            "y": 5.556291662758613,
-            "heading": -2.9981488971519656,
-            "angularVelocity": -2.9382367518778816,
-            "velocityX": -0.2865690386755447,
-            "velocityY": -0.7047655687470963,
-            "timestamp": 5.417197872053168
-        },
-        {
-            "x": 1.3350584223241078,
-            "y": 5.537231311212578,
-            "heading": -3.091661499454674,
-            "angularVelocity": -2.18835183600471,
-            "velocityX": -0.15400295944407288,
-            "velocityY": -0.4460442151491003,
-            "timestamp": 5.459929849957341
-        },
-        {
-            "x": 1.332138180732727,
-            "y": 5.52810525894165,
-            "heading": -3.1411853071795863,
-            "angularVelocity": -1.1589402165275682,
-            "velocityX": -0.06833855427730316,
-            "velocityY": -0.21356493938549467,
-            "timestamp": 5.502661827861513
-        },
-        {
-            "x": 1.332138180732727,
-            "y": 5.52810525894165,
-            "heading": -3.1411853071795863,
-            "angularVelocity": 0,
-            "velocityX": 4.554923410438002e-36,
-            "velocityY": -5.3517654956150865e-34,
-            "timestamp": 5.545393805765686
-        },
-        {
-            "x": 1.3511823216382217,
-            "y": 5.531961266421083,
-            "heading": -3.038798036456765,
-            "angularVelocity": 1.6934181625027336,
-            "velocityX": 0.3149775735885273,
-            "velocityY": 0.06377582930300447,
-            "timestamp": 5.60585570052386
-        },
-        {
-            "x": 1.3908490268299478,
-            "y": 5.542189708843604,
-            "heading": -2.844785530962286,
-            "angularVelocity": 3.2088393238494826,
-            "velocityX": 0.6560612324568816,
-            "velocityY": 0.16917171490292857,
-            "timestamp": 5.666317595282035
-        },
-        {
-            "x": 1.4539728254946123,
-            "y": 5.56127979063472,
-            "heading": -2.581938269596155,
-            "angularVelocity": 4.34732094350375,
-            "velocityX": 1.0440261410453116,
-            "velocityY": 0.3157374056415134,
-            "timestamp": 5.726779490040209
-        },
-        {
-            "x": 1.5450944631009007,
-            "y": 5.588995277628573,
-            "heading": -2.2890493808444536,
-            "angularVelocity": 4.8441897152438,
-            "velocityX": 1.5070919952267785,
-            "velocityY": 0.45839593854450544,
-            "timestamp": 5.787241384798383
-        },
-        {
-            "x": 1.6638598735645596,
-            "y": 5.62434991056183,
-            "heading": -2.0324634607383167,
-            "angularVelocity": 4.24376247440454,
-            "velocityX": 1.9643018290888397,
-            "velocityY": 0.5847423914626325,
-            "timestamp": 5.8477032795565576
-        },
-        {
-            "x": 1.8083021892177116,
-            "y": 5.6638944259582,
-            "heading": -1.8350759652142852,
-            "angularVelocity": 3.264659440685901,
-            "velocityX": 2.3889809644714033,
-            "velocityY": 0.6540402935523977,
-            "timestamp": 5.908165174314732
-        },
-        {
-            "x": 1.978235650109889,
-            "y": 5.709112472816866,
-            "heading": -1.6974261571036708,
-            "angularVelocity": 2.276637354174287,
-            "velocityX": 2.8105877523661738,
-            "velocityY": 0.7478767749426485,
-            "timestamp": 5.968627069072906
-        },
-        {
-            "x": 2.1727729031197174,
-            "y": 5.761282618111588,
-            "heading": -1.6235540862695264,
-            "angularVelocity": 1.2217954982986527,
-            "velocityX": 3.217518302856812,
-            "velocityY": 0.8628599137255631,
-            "timestamp": 6.0290889638310805
-        },
-        {
-            "x": 2.3909229420587215,
-            "y": 5.821425961949498,
-            "heading": -1.6168784459669248,
-            "angularVelocity": 0.1104107029609625,
-            "velocityX": 3.60805826234069,
-            "velocityY": 0.994731377150211,
-            "timestamp": 6.089550858589255
-        },
-        {
-            "x": 2.6085958376613507,
-            "y": 5.891661539060254,
-            "heading": -1.616878243648269,
-            "angularVelocity": 0.0000033462175924113523,
-            "velocityX": 3.6001666251651883,
-            "velocityY": 1.1616502822426138,
-            "timestamp": 6.150012753347429
-        },
-        {
-            "x": 2.826268683292055,
-            "y": 5.961897271043331,
-            "heading": -1.6168780413299741,
-            "angularVelocity": 0.0000033462116195770168,
-            "velocityX": 3.6001657986623896,
-            "velocityY": 1.1616528437290037,
-            "timestamp": 6.2104746481056035
-        },
-        {
-            "x": 3.0439415289220437,
-            "y": 6.032133003028614,
-            "heading": -1.6168778390116512,
-            "angularVelocity": 0.0000033462120828803855,
-            "velocityX": 3.6001657986505493,
-            "velocityY": 1.1616528437654758,
-            "timestamp": 6.270936542863778
-        },
-        {
-            "x": 3.2616143745520296,
-            "y": 6.102368735013889,
-            "heading": -1.6168776366933004,
-            "angularVelocity": 0.000003346212545975565,
-            "velocityX": 3.600165798650516,
-            "velocityY": 1.1616528437653553,
-            "timestamp": 6.331398437621952
-        },
-        {
-            "x": 3.479287220182014,
-            "y": 6.172604466999156,
-            "heading": -1.6168774343749215,
-            "angularVelocity": 0.0000033462130098357135,
-            "velocityX": 3.6001657986504836,
-            "velocityY": 1.1616528437652343,
-            "timestamp": 6.391860332380126
-        },
-        {
-            "x": 3.6969600658119965,
-            "y": 6.242840198984417,
-            "heading": -1.6168772320565146,
-            "angularVelocity": 0.000003346213473053834,
-            "velocityX": 3.6001657986504503,
-            "velocityY": 1.1616528437651126,
-            "timestamp": 6.452322227138301
-        },
-        {
-            "x": 3.914632911441977,
-            "y": 6.313075930969669,
-            "heading": -1.61687702973808,
-            "angularVelocity": 0.000003346213936555426,
-            "velocityX": 3.6001657986504174,
-            "velocityY": 1.1616528437649918,
-            "timestamp": 6.512784121896475
-        },
-        {
-            "x": 4.132305757071955,
-            "y": 6.383311662954915,
-            "heading": -1.616876827419617,
-            "angularVelocity": 0.0000033462143992467285,
-            "velocityX": 3.6001657986503846,
-            "velocityY": 1.161652843764871,
-            "timestamp": 6.573246016654649
-        },
-        {
-            "x": 4.349978602701932,
-            "y": 6.453547394940153,
-            "heading": -1.6168766251011262,
-            "angularVelocity": 0.000003346214863174556,
-            "velocityX": 3.600165798650352,
-            "velocityY": 1.1616528437647493,
-            "timestamp": 6.633707911412824
-        },
-        {
-            "x": 4.567651448331906,
-            "y": 6.523783126925384,
-            "heading": -1.6168764227826073,
-            "angularVelocity": 0.000003346215326740712,
-            "velocityX": 3.6001657986503193,
-            "velocityY": 1.1616528437646283,
-            "timestamp": 6.694169806170998
-        },
-        {
-            "x": 4.785324293961879,
-            "y": 6.594018858910608,
-            "heading": -1.6168762204640603,
-            "angularVelocity": 0.0000033462157905377784,
-            "velocityX": 3.6001657986502864,
-            "velocityY": 1.161652843764507,
-            "timestamp": 6.754631700929172
-        },
-        {
-            "x": 5.002997139591849,
-            "y": 6.6642545908958235,
-            "heading": -1.6168760181454853,
-            "angularVelocity": 0.0000033462162531242337,
-            "velocityX": 3.600165798650253,
-            "velocityY": 1.1616528437643858,
-            "timestamp": 6.815093595687347
-        },
-        {
-            "x": 5.220669985221817,
-            "y": 6.734490322881032,
-            "heading": -1.6168758158268823,
-            "angularVelocity": 0.000003346216717473229,
-            "velocityX": 3.6001657986502207,
-            "velocityY": 1.1616528437642646,
-            "timestamp": 6.875555490445521
-        },
-        {
-            "x": 5.438342830851783,
-            "y": 6.804726054866234,
-            "heading": -1.6168756135082514,
-            "angularVelocity": 0.000003346217180660181,
-            "velocityX": 3.600165798650188,
-            "velocityY": 1.1616528437641434,
-            "timestamp": 6.936017385203695
-        },
-        {
-            "x": 5.656015676481748,
-            "y": 6.874961786851428,
-            "heading": -1.6168754111895922,
-            "angularVelocity": 0.0000033462176438124147,
-            "velocityX": 3.600165798650156,
-            "velocityY": 1.1616528437640228,
-            "timestamp": 6.99647927996187
-        },
-        {
-            "x": 5.873688522111711,
-            "y": 6.945197518836615,
-            "heading": -1.6168752088709053,
-            "angularVelocity": 0.0000033462181070326184,
-            "velocityX": 3.6001657986501225,
-            "velocityY": 1.161652843763901,
-            "timestamp": 7.056941174720044
-        },
-        {
-            "x": 6.091361367741672,
-            "y": 7.015433250821794,
-            "heading": -1.6168750065521902,
-            "angularVelocity": 0.0000033462185712785595,
-            "velocityX": 3.6001657986500892,
-            "velocityY": 1.1616528437637803,
-            "timestamp": 7.117403069478218
-        },
-        {
-            "x": 6.30903421337163,
-            "y": 7.0856689828069666,
-            "heading": -1.616874804233447,
-            "angularVelocity": 0.0000033462190342827524,
-            "velocityX": 3.600165798650057,
-            "velocityY": 1.1616528437636586,
-            "timestamp": 7.1778649642363925
-        },
-        {
-            "x": 6.526707059002331,
-            "y": 7.155904714789823,
-            "heading": -1.6168746019146758,
-            "angularVelocity": 0.0000033462194986739715,
-            "velocityX": 3.6001657986623425,
-            "velocityY": 1.1616528437253608,
-            "timestamp": 7.238326858994567
-        },
-        {
-            "x": 6.744379956765645,
-            "y": 7.22614028520157,
-            "heading": -1.6168743995934785,
-            "angularVelocity": 0.0000033462596273946,
-            "velocityX": 3.600166660901485,
-            "velocityY": 1.1616501714453904,
-            "timestamp": 7.298788753752741
-        },
-        {
-            "x": 6.948380246935694,
-            "y": 7.286306890939545,
-            "heading": -1.572684837353693,
-            "angularVelocity": 0.7308663153301367,
-            "velocityX": 3.37403071779302,
-            "velocityY": 0.9951161136881308,
-            "timestamp": 7.3592506485109155
-        },
-        {
-            "x": 7.128326014252639,
-            "y": 7.34087027160758,
-            "heading": -1.456360393496805,
-            "angularVelocity": 1.9239298457672127,
-            "velocityX": 2.976184719924214,
-            "velocityY": 0.9024424538177037,
-            "timestamp": 7.41971254326909
-        },
-        {
-            "x": 7.285215733567768,
-            "y": 7.391288256354559,
-            "heading": -1.2598604932432955,
-            "angularVelocity": 3.249979198294029,
-            "velocityX": 2.5948528398362565,
-            "velocityY": 0.8338803298942721,
-            "timestamp": 7.480174438027264
-        },
-        {
-            "x": 7.415669802167391,
-            "y": 7.433594551714448,
-            "heading": -1.014978314704827,
-            "angularVelocity": 4.050190281298798,
-            "velocityX": 2.157624552147962,
-            "velocityY": 0.6997183189362131,
-            "timestamp": 7.540636332785438
-        },
-        {
-            "x": 7.52162527778786,
-            "y": 7.463369081831009,
-            "heading": -0.7214685317692553,
-            "angularVelocity": 4.854458896954922,
-            "velocityX": 1.7524339262646644,
-            "velocityY": 0.49245115846350496,
-            "timestamp": 7.601098227543613
-        },
-        {
-            "x": 7.599835823210802,
-            "y": 7.482093660895173,
-            "heading": -0.44961761921846705,
-            "angularVelocity": 4.4962354163410385,
-            "velocityX": 1.293551016483298,
-            "velocityY": 0.30969223076874997,
-            "timestamp": 7.661560122301787
-        },
-        {
-            "x": 7.6517032735458015,
-            "y": 7.492160163258041,
-            "heading": -0.22315227789287845,
-            "angularVelocity": 3.7455878984832975,
-            "velocityX": 0.8578535380416231,
-            "velocityY": 0.16649333275330752,
-            "timestamp": 7.722022017059961
-        },
-        {
-            "x": 7.67973874704759,
-            "y": 7.4966228089126075,
-            "heading": -0.06798236605855336,
-            "angularVelocity": 2.5664083544676926,
-            "velocityX": 0.46368830507082526,
-            "velocityY": 0.07380922599953843,
-            "timestamp": 7.782483911818136
-        },
-        {
-            "x": 7.686149597167969,
-            "y": 7.496679306030273,
-            "heading": 0,
-            "angularVelocity": 1.1243836523889659,
-            "velocityX": 0.10603124738348535,
-            "velocityY": 0.0009344251927952234,
-            "timestamp": 7.84294580657631
-        },
-        {
-            "x": 7.671082178881759,
-            "y": 7.493118035056542,
-            "heading": -0.029242529216573936,
-            "angularVelocity": -0.471080674899979,
-            "velocityX": -0.24272762190641095,
-            "velocityY": -0.05737006951015711,
-            "timestamp": 7.905021223324796
-        },
-        {
-            "x": 7.633475390199317,
-            "y": 7.486720174316825,
-            "heading": -0.1516714089178372,
-            "angularVelocity": -1.9722602942371796,
-            "velocityX": -0.6058241837475781,
-            "velocityY": -0.1030659329383018,
-            "timestamp": 7.967096640073281
-        },
-        {
-            "x": 7.569735009380553,
-            "y": 7.475264417263962,
-            "heading": -0.33932636884787254,
-            "angularVelocity": -3.023015708301534,
-            "velocityX": -1.0268216333854858,
-            "velocityY": -0.1845457936960604,
-            "timestamp": 8.029172056821766
-        },
-        {
-            "x": 7.480376336028035,
-            "y": 7.452059634472421,
-            "heading": -0.582197860853168,
-            "angularVelocity": -3.912522939464948,
-            "velocityX": -1.4395178966671722,
-            "velocityY": -0.37381598073777733,
-            "timestamp": 8.09124747357025
-        },
-        {
-            "x": 7.362878563951424,
-            "y": 7.417380486057705,
-            "heading": -0.863364111736863,
-            "angularVelocity": -4.529429935571964,
-            "velocityX": -1.8928229278376825,
-            "velocityY": -0.5586615480203228,
-            "timestamp": 8.153322890318735
-        },
-        {
-            "x": 7.218864923659752,
-            "y": 7.37005336377927,
-            "heading": -1.0948198175456743,
-            "angularVelocity": -3.7286210537516657,
-            "velocityX": -2.319978629788013,
-            "velocityY": -0.7624132830262488,
-            "timestamp": 8.21539830706722
-        },
-        {
-            "x": 7.047313606455814,
-            "y": 7.316250565275482,
-            "heading": -1.2668868462563043,
-            "angularVelocity": -2.7719029162189006,
-            "velocityX": -2.7635950943192666,
-            "velocityY": -0.8667327796087775,
-            "timestamp": 8.277473723815705
-        },
-        {
-            "x": 6.850682482856188,
-            "y": 7.25802494172709,
-            "heading": -1.3587604988602537,
-            "angularVelocity": -1.4800327958521704,
-            "velocityX": -3.16761664921765,
-            "velocityY": -0.9379819999325716,
-            "timestamp": 8.33954914056419
-        },
-        {
-            "x": 6.62975279957275,
-            "y": 7.191687511601611,
-            "heading": -1.3718894789288036,
-            "angularVelocity": -0.21150047404023747,
-            "velocityX": -3.5590527596228974,
-            "velocityY": -1.0686586349353433,
-            "timestamp": 8.401624557312674
-        },
-        {
-            "x": 6.40649462388328,
-            "y": 7.118888182289737,
-            "heading": -1.3718898557853474,
-            "angularVelocity": -0.000006070946652807348,
-            "velocityX": -3.596563460766736,
-            "velocityY": -1.1727561911801452,
-            "timestamp": 8.463699974061159
-        },
-        {
-            "x": 6.183236472249506,
-            "y": 7.0460887792034885,
-            "heading": -1.3718902326404057,
-            "angularVelocity": -0.000006070922729380904,
-            "velocityX": -3.5965630732430243,
-            "velocityY": -1.1727573796437725,
-            "timestamp": 8.525775390809644
-        },
-        {
-            "x": 5.959978320616,
-            "y": 6.973289376116265,
-            "heading": -1.3718906094952235,
-            "angularVelocity": -0.000006070918851907899,
-            "velocityX": -3.5965630732387113,
-            "velocityY": -1.172757379659484,
-            "timestamp": 8.587850807558128
-        },
-        {
-            "x": 5.736720168982443,
-            "y": 6.900489973029042,
-            "heading": -1.3718909863498006,
-            "angularVelocity": -0.000006070914972563024,
-            "velocityX": -3.596563073239526,
-            "velocityY": -1.1727573796594701,
-            "timestamp": 8.649926224306613
-        },
-        {
-            "x": 5.513462017348836,
-            "y": 6.8276905699418196,
-            "heading": -1.3718913632041367,
-            "angularVelocity": -0.000006070911095039567,
-            "velocityX": -3.5965630732403415,
-            "velocityY": -1.1727573796594566,
-            "timestamp": 8.712001641055098
-        },
-        {
-            "x": 5.290203865715178,
-            "y": 6.7548911668545975,
-            "heading": -1.3718917400582324,
-            "angularVelocity": -0.0000060709072179749485,
-            "velocityX": -3.596563073241156,
-            "velocityY": -1.1727573796594426,
-            "timestamp": 8.774077057803582
-        },
-        {
-            "x": 5.066945714081469,
-            "y": 6.682091763767377,
-            "heading": -1.3718921169120872,
-            "angularVelocity": -0.000006070903338953198,
-            "velocityX": -3.596563073241971,
-            "velocityY": -1.1727573796594288,
-            "timestamp": 8.836152474552067
-        },
-        {
-            "x": 4.84368756244771,
-            "y": 6.609292360680157,
-            "heading": -1.3718924937657013,
-            "angularVelocity": -0.000006070899462290803,
-            "velocityX": -3.596563073242785,
-            "velocityY": -1.1727573796594153,
-            "timestamp": 8.898227891300552
-        },
-        {
-            "x": 4.620429410813901,
-            "y": 6.536492957592937,
-            "heading": -1.371892870619075,
-            "angularVelocity": -0.000006070895585804876,
-            "velocityX": -3.5965630732436003,
-            "velocityY": -1.1727573796594017,
-            "timestamp": 8.960303308049037
-        },
-        {
-            "x": 4.39717125918004,
-            "y": 6.46369355450572,
-            "heading": -1.3718932474722079,
-            "angularVelocity": -0.000006070891709350806,
-            "velocityX": -3.5965630732444147,
-            "velocityY": -1.172757379659388,
-            "timestamp": 9.022378724797521
-        },
-        {
-            "x": 4.173913107546129,
-            "y": 6.390894151418502,
-            "heading": -1.3718936243251003,
-            "angularVelocity": -0.000006070887832958865,
-            "velocityX": -3.5965630732452283,
-            "velocityY": -1.1727573796593747,
-            "timestamp": 9.084454141546006
-        },
-        {
-            "x": 3.950654955912168,
-            "y": 6.318094748331286,
-            "heading": -1.3718940011777518,
-            "angularVelocity": -0.000006070883956378075,
-            "velocityX": -3.596563073246043,
-            "velocityY": -1.1727573796593613,
-            "timestamp": 9.14652955829449
-        },
-        {
-            "x": 3.7273968042781562,
-            "y": 6.24529534524407,
-            "heading": -1.3718943780301627,
-            "angularVelocity": -0.000006070880081532027,
-            "velocityX": -3.5965630732468568,
-            "velocityY": -1.1727573796593478,
-            "timestamp": 9.208604975042975
-        },
-        {
-            "x": 3.504138652644094,
-            "y": 6.172495942156855,
-            "heading": -1.3718947548823335,
-            "angularVelocity": -0.000006070876206601658,
-            "velocityX": -3.5965630732476703,
-            "velocityY": -1.1727573796593347,
-            "timestamp": 9.27068039179146
-        },
-        {
-            "x": 3.2808805010099813,
-            "y": 6.099696539069641,
-            "heading": -1.3718951317342634,
-            "angularVelocity": -0.000006070872330584258,
-            "velocityX": -3.596563073248485,
-            "velocityY": -1.1727573796593216,
-            "timestamp": 9.332755808539945
-        },
-        {
-            "x": 3.057622349375818,
-            "y": 6.026897135982428,
-            "heading": -1.371895508585953,
-            "angularVelocity": -0.000006070868457801708,
-            "velocityX": -3.5965630732492984,
-            "velocityY": -1.1727573796593085,
-            "timestamp": 9.39483122528843
-        },
-        {
-            "x": 2.834364197741604,
-            "y": 5.954097732895216,
-            "heading": -1.371895885437402,
-            "angularVelocity": -0.0000060708645841945965,
-            "velocityX": -3.596563073250112,
-            "velocityY": -1.1727573796592947,
-            "timestamp": 9.456906642036914
-        },
-        {
-            "x": 2.6111060461064435,
-            "y": 5.881298329810753,
-            "heading": -1.3718962622886106,
-            "angularVelocity": -0.000006070860710280736,
-            "velocityX": -3.5965630732653646,
-            "velocityY": -1.1727573796150002,
-            "timestamp": 9.518982058785399
-        },
-        {
-            "x": 2.387847826730136,
-            "y": 5.808499134475022,
-            "heading": -1.37189663914262,
-            "angularVelocity": -0.000006070905830150403,
-            "velocityX": -3.5965641645370763,
-            "velocityY": -1.1727540328999504,
-            "timestamp": 9.581057475533884
-        },
-        {
-            "x": 2.167695143748949,
-            "y": 5.749324083617281,
-            "heading": -1.3936267357407348,
-            "angularVelocity": -0.3500596167748615,
-            "velocityX": -3.546535722397048,
-            "velocityY": -0.9532767391237054,
-            "timestamp": 9.643132892282368
-        },
-        {
-            "x": 1.9716359535895844,
-            "y": 5.700085594404985,
-            "heading": -1.4887110872196767,
-            "angularVelocity": -1.5317553463104556,
-            "velocityX": -3.1584031236350607,
-            "velocityY": -0.7932043277582467,
-            "timestamp": 9.705208309030853
-        },
-        {
-            "x": 1.8021216559438722,
-            "y": 5.6575856667748505,
-            "heading": -1.6480423798805228,
-            "angularVelocity": -2.5667373818273083,
-            "velocityX": -2.73077985658225,
-            "velocityY": -0.6846498961470444,
-            "timestamp": 9.767283725779338
-        },
-        {
-            "x": 1.6601304337176481,
-            "y": 5.619404991825654,
-            "heading": -1.8664040726402535,
-            "angularVelocity": -3.5176838786999975,
-            "velocityX": -2.2873986138754163,
-            "velocityY": -0.6150691682650391,
-            "timestamp": 9.829359142527823
-        },
-        {
-            "x": 1.5437233022892554,
-            "y": 5.5885120196672124,
-            "heading": -2.146539077662866,
-            "angularVelocity": -4.5128171455965,
-            "velocityX": -1.8752533212953901,
-            "velocityY": -0.49766838108574224,
-            "timestamp": 9.891434559276307
-        },
-        {
-            "x": 1.452238647018182,
-            "y": 5.5675498489404625,
-            "heading": -2.4956493740995165,
-            "angularVelocity": -5.623970240121965,
-            "velocityX": -1.4737662679212806,
-            "velocityY": -0.33768876351944366,
-            "timestamp": 9.953509976024792
-        },
-        {
-            "x": 1.3894112242957146,
-            "y": 5.555821056487952,
-            "heading": -2.802567916985108,
-            "angularVelocity": -4.94428485481059,
-            "velocityX": -1.0121143926754297,
-            "velocityY": -0.188944240197889,
-            "timestamp": 10.015585392773277
-        },
-        {
-            "x": 1.3504462657255363,
-            "y": 5.550087016097331,
-            "heading": -3.0243894075867264,
-            "angularVelocity": -3.573419273210609,
-            "velocityX": -0.6277035356533314,
-            "velocityY": -0.09237216100947475,
-            "timestamp": 10.077660809521761
-        },
-        {
-            "x": 1.3321380615234375,
-            "y": 5.547595977783203,
-            "heading": -3.1411853071795863,
-            "angularVelocity": -1.8815161574522963,
-            "velocityX": -0.2949348576470964,
-            "velocityY": -0.040129224169711215,
-            "timestamp": 10.139736226270246
-        },
-        {
-            "x": 1.3321380615234375,
-            "y": 5.547595977783203,
-            "heading": -3.1411853071795863,
-            "angularVelocity": 7.133789284871957e-33,
-            "velocityX": 0,
-            "velocityY": 1.0797204950028118e-32,
-            "timestamp": 10.201811643018731
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/AutoManager.java b/src/main/java/frc/robot/AutoManager.java
index 7ccc489b..f2d0dd7a 100644
--- a/src/main/java/frc/robot/AutoManager.java
+++ b/src/main/java/frc/robot/AutoManager.java
@@ -2,7 +2,8 @@
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import com.pathplanner.lib.commands.PathPlannerAuto;
+
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
@@ -11,8 +12,13 @@
 import frc.robot.subsystems.shooter.ShooterSubystem;
 import frc.robot.subsystems.swerve.SwerveSubsystem;
 
+import java.util.ArrayList;
+import java.util.List;
+
+import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
+
 public final class AutoManager {
-  SendableChooser<Command> chooser = AutoBuilder.buildAutoChooser();
+  LoggedDashboardChooser<Command> chooser = new LoggedDashboardChooser<Command>("Auto Chooser");
 
   SwerveSubsystem swerveSubsystem;
   IntakeSubsystem intakeSubsystem;
@@ -20,6 +26,27 @@ public final class AutoManager {
   ShooterSubystem shooterSubsystem;
   FeederSubsystem feederSubsystem;
 
+  private void setUpChooser(String defaultAutoName) {
+    List<String> autoNames = AutoBuilder.getAllAutoNames();
+
+    List<PathPlannerAuto> options = new ArrayList<>();
+
+    for (String autoName : autoNames) {
+      try {
+        options.add(new PathPlannerAuto(autoName));
+      } catch (Exception e) {
+        System.out.println("Error loading auto: " + autoName);
+        e.printStackTrace();
+      }
+    }
+
+    if (!defaultAutoName.isEmpty()) {
+      chooser.addDefaultOption(defaultAutoName, new PathPlannerAuto(defaultAutoName));
+    }
+
+    options.forEach(auto -> chooser.addOption(auto.getName(), auto));
+  }
+
   public AutoManager(
       SwerveSubsystem swerveSubsystem,
       IntakeSubsystem intakeSubsystem,
@@ -32,9 +59,13 @@ public AutoManager(
     this.shooterSubsystem = shooterSubsystem;
     this.feederSubsystem = feederSubsystem;
 
+    registerNamedCommands();
+    setUpChooser("single");
+  }
+
+  public void registerNamedCommands() {
     NamedCommands.registerCommand("intake", intakeSubsystem.getDefaultCommand());
     NamedCommands.registerCommand("fender", shooterSubsystem.getDefaultCommand());
-
-    SmartDashboard.putData(chooser);
+    NamedCommands.registerCommand("stop", swerveSubsystem.stopWithXCmd().asProxy());
   }
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 758fad41..bc88d410 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -4,7 +4,6 @@
 
 package frc.robot;
 
-import com.pathplanner.lib.auto.NamedCommands;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.PowerDistribution;
@@ -63,8 +62,8 @@ public void setYaw(Rotation2d yaw) {}
   private final ShooterSubystem shooter =
       new ShooterSubystem(mode == RobotMode.REAL ? new ShooterIOReal() : new ShooterIOSim());
 
-  private final AutoManager autoManager =
-      new AutoManager(swerve, intake, elevator, shooter, feeder);
+  private AutoManager autoManager = new AutoManager(swerve, intake, elevator, shooter, feeder);
+  ;
 
   @Override
   public void robotInit() {
@@ -139,8 +138,6 @@ public void robotInit() {
 
     // Test binding for elevator
     controller.b().whileTrue(elevator.setExtensionCmd(() -> 1.0));
-
-    NamedCommands.registerCommand("stop", swerve.stopWithXCmd().asProxy());
   }
 
   @Override
@@ -153,12 +150,12 @@ public void disabledPeriodic() {}
 
   @Override
   public void autonomousInit() {
-    autoManager.chooser.getSelected().schedule();
+    autoManager.chooser.get().schedule();
   }
 
   @Override
   public void teleopInit() {
-    autoManager.chooser.getSelected().cancel();
+    autoManager.chooser.get().cancel();
   }
 
   @Override

From 1d09aca22c7bb9ec26133e92a6aabafee09dfc5d Mon Sep 17 00:00:00 2001
From: Lewis-Seiden <lewy@seiden.us>
Date: Tue, 13 Feb 2024 16:37:37 -0800
Subject: [PATCH 11/62] added trap sequence leds

---
 src/main/java/frc/robot/Robot.java            | 50 ++++++++---------
 .../robot/subsystems/leds/LEDSubsystem.java   | 54 ++++++++++++++++---
 2 files changed, 68 insertions(+), 36 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index ceb3af82..5e594439 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -11,7 +11,6 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj.util.Color;
@@ -155,28 +154,8 @@ public void robotInit() {
     reactionBarRelease.setDefaultCommand(
         reactionBarRelease.setRotationCmd(Rotation2d.fromDegrees(0.0)));
     leds.setDefaultCommand(
-        Commands.either(
-                leds.setBlinkingCmd(new Color("#00ff00"), new Color("#000000"), 10.0)
-                    .withInterruptBehavior(InterruptionBehavior.kCancelSelf),
-                leds.setRunAlongCmd(
-                    // Set color to be purple with a moving dash corresponding to alliance color
-                    () -> {
-                      if (DriverStation.getAlliance().isEmpty()) {
-                        return new Color("#e0e0e0");
-                      } else if (DriverStation.getAlliance().get() == Alliance.Red) {
-                        return new Color("#ff0000");
-                      } else { // Blue
-                        return new Color("#0000ff");
-                      }
-                    },
-                    () -> new Color("#350868"),
-                    10,
-                    1.0),
-                () -> DriverStation.isEnabled())
-            .until(() -> true)
-            .repeatedly()
-            .ignoringDisable(true)
-            .withInterruptBehavior(InterruptionBehavior.kCancelSelf));
+        leds.defaultStateDisplay(
+            () -> DriverStation.isEnabled(), () -> currentTarget == Target.SPEAKER));
 
     controller.setDefaultCommand(controller.rumbleCmd(0.0, 0.0));
     operator.setDefaultCommand(operator.rumbleCmd(0.0, 0.0));
@@ -190,10 +169,7 @@ public void robotInit() {
                 controller.rumbleCmd(1.0, 1.0).withTimeout(0.25),
                 leds.setBlinkingCmd(new Color("#ff8000"), new Color("#000000"), 25.0)));
     new Trigger(() -> currentTarget == Target.SPEAKER)
-        .whileTrue(
-            Commands.parallel(
-                carriage.runVoltageCmd(5.0),
-                feeder.indexCmd()));
+        .whileTrue(Commands.parallel(carriage.runVoltageCmd(5.0), feeder.indexCmd()));
     new Trigger(() -> currentTarget == Target.AMP)
         .whileTrue(
             Commands.either(
@@ -241,8 +217,17 @@ public void robotInit() {
             Commands.sequence(
                 elevator
                     .setExtensionCmd(() -> 0.0)
-                    .until(() -> elevator.getExtensionMeters() < 0.05),
+                    .until(() -> elevator.getExtensionMeters() < 0.05)
+                    .raceWith(
+                        leds.setProgressCmd(
+                            new Color("#b59aff"),
+                            () ->
+                                1.0
+                                    - (elevator.getExtensionMeters()
+                                        / ElevatorSubsystem.CLIMB_EXTENSION_METERS))),
+                Commands.waitUntil(() -> controller.y().getAsBoolean()),
                 Commands.parallel(
+                    leds.setRainbowCmd(),
                     elevator.setExtensionCmd(() -> ElevatorSubsystem.TRAP_EXTENSION_METERS),
                     Commands.waitUntil(
                             () ->
@@ -256,6 +241,15 @@ public void robotInit() {
         .and(operator.rightBumper())
         .toggleOnFalse(
             Commands.parallel(
+                Commands.sequence(
+                    leds.setProgressCmd(new Color("#ff0000"), () ->  elevator.getExtensionMeters()
+                                        / ElevatorSubsystem.CLIMB_EXTENSION_METERS)
+                        .until(
+                            () ->
+                                elevator.getExtensionMeters()
+                                    > 0.9 * ElevatorSubsystem.CLIMB_EXTENSION_METERS),
+                    leds.setBlinkingCmd(new Color("#ffffff"), new Color("#00ff00"), 10.0)),
+                reactionBarRelease.setRotationCmd(Rotation2d.fromDegrees(90.0)),
                 elevator.setExtensionCmd(() -> ElevatorSubsystem.CLIMB_EXTENSION_METERS)));
     operator.leftTrigger().onTrue(Commands.runOnce(() -> currentTarget = Target.SPEAKER));
     operator.leftBumper().onTrue(Commands.runOnce(() -> currentTarget = Target.AMP));
diff --git a/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java
index 86f408b0..f70652d9 100644
--- a/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java
@@ -5,10 +5,15 @@
 package frc.robot.subsystems.leds;
 
 import com.google.common.base.Supplier;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
 import org.littletonrobotics.junction.Logger;
 
 public class LEDSubsystem extends SubsystemBase {
@@ -50,14 +55,18 @@ public Command setBlinkingCmd(Color onColor, Color offColor, double frequency) {
         setSolidCmd(offColor).withTimeout(1.0 / frequency));
   }
 
-  public void setProgress(Color color, double progress) {
-    for (int i = 0; i < LED_LENGTH; i++) {
-      if (i < progress) {
-        setIndex(i, color);
-      } else {
-        setIndex(i, Color.kBlack);
-      }
-    }
+  /** Sets the first portion of the leds to a color, and the rest off */
+  public Command setProgressCmd(Color color, DoubleSupplier progress) {
+    return this.run(
+        () -> {
+          for (int i = 0; i < LED_LENGTH; i++) {
+            if (i < progress.getAsDouble() * LED_LENGTH) {
+              setIndex(i, color);
+            } else {
+              setIndex(i, Color.kBlack);
+            }
+          }
+        });
   }
 
   public Command setRainbowCmd() {
@@ -83,4 +92,33 @@ public Command setRunAlongCmd(
           dashStart %= LED_LENGTH;
         });
   }
+
+  public Command defaultStateDisplay(BooleanSupplier enabled, BooleanSupplier targetIsSpeaker) {
+    return Commands.either(
+            Commands.either(
+                this.setBlinkingCmd(new Color("#ffff00"), new Color(), 10.0)
+                    .until(() -> !targetIsSpeaker.getAsBoolean() || !enabled.getAsBoolean()),
+                this.setBlinkingCmd(new Color("#ff7777"), new Color(), 10.0)
+                    .until(() -> targetIsSpeaker.getAsBoolean() || !enabled.getAsBoolean()),
+                targetIsSpeaker),
+            this.setRunAlongCmd(
+                    // Set color to be purple with a moving dash corresponding to alliance color
+                    () -> {
+                      if (DriverStation.getAlliance().isEmpty()) {
+                        return new Color("#350868");
+                      } else if (DriverStation.getAlliance().get() == Alliance.Red) {
+                        return new Color("#ff0000");
+                      } else { // Blue
+                        return new Color("#0000ff");
+                      }
+                    },
+                    () -> new Color("#b59aff"),
+                    10,
+                    1.0)
+                .until(enabled),
+            enabled)
+        .ignoringDisable(true)
+        .withInterruptBehavior(InterruptionBehavior.kCancelSelf)
+        .repeatedly();
+  }
 }

From 50f91f452d0ff6d47ce215e0cc4f85d0e1930783 Mon Sep 17 00:00:00 2001
From: Lewis-Seiden <lewy@seiden.us>
Date: Sat, 17 Feb 2024 13:47:03 -0800
Subject: [PATCH 12/62] removed extraneous controller binding

---
 simgui-ds.json                     |  3 +--
 simgui.json                        |  5 +++++
 src/main/java/frc/robot/Robot.java | 32 ++++--------------------------
 3 files changed, 10 insertions(+), 30 deletions(-)

diff --git a/simgui-ds.json b/simgui-ds.json
index c4b7efd3..69b1a3cb 100644
--- a/simgui-ds.json
+++ b/simgui-ds.json
@@ -91,8 +91,7 @@
   ],
   "robotJoysticks": [
     {
-      "guid": "78696e70757401000000000000000000",
-      "useGamepad": true
+      "guid": "Keyboard0"
     }
   ]
 }
diff --git a/simgui.json b/simgui.json
index 1d91652d..3f0177eb 100644
--- a/simgui.json
+++ b/simgui.json
@@ -4,6 +4,11 @@
       "/AdvantageKit/RealOutputs/Elevator/Mechanism2d": "Mechanism2d",
       "/AdvantageKit/RealOutputs/Shooter/Mechanism2d": "Mechanism2d",
       "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Run Elevator Sysid": "Command",
+      "/SmartDashboard/Run Flywheel Sysid": "Command",
+      "/SmartDashboard/Run Pivot Sysid": "Command",
+      "/SmartDashboard/Run Swerve Azimuth Sysid": "Command",
+      "/SmartDashboard/Run Swerve Drive Sysid": "Command",
       "/SmartDashboard/Shooter shoot": "Command"
     }
   },
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 8d50c41d..d455f0fd 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -187,7 +187,7 @@ public void robotInit() {
                     () -> {
                       double vx = swerve.getVelocity().vxMetersPerSecond;
                       double vy = swerve.getVelocity().vyMetersPerSecond;
-                      double vTheta = swerve.getVelocity().omegaRadiansPerSecond;
+                      double omega = swerve.getVelocity().omegaRadiansPerSecond;
 
                       double polarVelocity =
                           MathUtil.clamp(
@@ -199,7 +199,7 @@ public void robotInit() {
                           new ChassisSpeeds(
                               polarVelocity * Math.cos(polarRadians),
                               polarVelocity * Math.sin(polarRadians),
-                              vTheta);
+                              omega);
                       Logger.recordOutput("AutoAim/Polar Speeds", polarSpeeds);
                       return polarSpeeds;
                     }),
@@ -264,37 +264,13 @@ public void robotInit() {
     operator.x().onTrue(Commands.runOnce(() -> flywheelIdleSpeed = 20.0));
     operator.y().onTrue(Commands.runOnce(() -> flywheelIdleSpeed = 80.0));
 
-    SmartDashboard.putData("Shooter shoot", shootWithDashboard());
-
+    
     NamedCommands.registerCommand("stop", swerve.stopWithXCmd().asProxy());
     NamedCommands.registerCommand(
         "auto aim amp 4 local sgmt 1", autonomousAutoAim("amp 4 local sgmt 1"));
 
-    controller
-        .leftBumper()
-        .whileTrue(
-            teleopAutoAim(
-                () -> {
-                  double vx = swerve.getVelocity().vxMetersPerSecond;
-                  double vy = swerve.getVelocity().vyMetersPerSecond;
-                  double vTheta = swerve.getVelocity().omegaRadiansPerSecond;
-
-                  double polarVelocity =
-                      MathUtil.clamp(
-                          Math.sqrt(Math.pow(vx, 2) + Math.pow(vy, 2)),
-                          -SwerveSubsystem.MAX_LINEAR_SPEED / 2,
-                          SwerveSubsystem.MAX_LINEAR_SPEED / 2);
-                  double polarRadians = Math.atan2(vy, vx);
-                  ChassisSpeeds polarSpeeds =
-                      new ChassisSpeeds(
-                          polarVelocity * Math.cos(polarRadians),
-                          polarVelocity * Math.sin(polarRadians),
-                          vTheta);
-                  Logger.recordOutput("AutoAim/Polar Sppeeds", polarSpeeds);
-                  return polarSpeeds;
-                }));
-
     // Dashboard command buttons
+    SmartDashboard.putData("Shooter shoot", shootWithDashboard());
     SmartDashboard.putData("Run Swerve Azimuth Sysid", swerve.runModuleSteerCharacterizationCmd());
     SmartDashboard.putData("Run Swerve Drive Sysid", swerve.runDriveCharacterizationCmd());
     SmartDashboard.putData("Run Elevator Sysid", elevator.runSysidCmd());

From 1e480151650eadd8b3b17c51fda1cd459403f429 Mon Sep 17 00:00:00 2001
From: Lewis-Seiden <lewy@seiden.us>
Date: Sat, 17 Feb 2024 13:48:16 -0800
Subject: [PATCH 13/62] added second led strip to ledioreal

---
 .../java/frc/robot/subsystems/leds/LEDIOReal.java | 15 ++++++++++-----
 .../frc/robot/subsystems/leds/LEDSubsystem.java   |  2 +-
 2 files changed, 11 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/leds/LEDIOReal.java b/src/main/java/frc/robot/subsystems/leds/LEDIOReal.java
index dfee71b1..b43ec6ac 100644
--- a/src/main/java/frc/robot/subsystems/leds/LEDIOReal.java
+++ b/src/main/java/frc/robot/subsystems/leds/LEDIOReal.java
@@ -9,20 +9,25 @@
 import edu.wpi.first.wpilibj.util.Color;
 
 public class LEDIOReal implements LEDIO {
-  AddressableLED led;
+  AddressableLED leftLed;
+  AddressableLED rightLed;
   AddressableLEDBuffer buffer;
 
   public LEDIOReal() {
-    led = new AddressableLED(3);
+    leftLed = new AddressableLED(3);
+    rightLed = new AddressableLED(4);
     buffer = new AddressableLEDBuffer(LEDSubsystem.LED_LENGTH);
-    led.setLength(buffer.getLength());
-    led.start();
+    leftLed.setLength(buffer.getLength());
+    leftLed.start();
+    rightLed.setLength(buffer.getLength());
+    rightLed.start();
   }
 
   /** Write data from buffer to leds */
   @Override
   public void updateInputs(LEDIOInputs inputs) {
-    led.setData(buffer);
+    leftLed.setData(buffer);
+    rightLed.setData(buffer);
   }
 
   @Override
diff --git a/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java
index f70652d9..17be9b80 100644
--- a/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java
@@ -17,7 +17,7 @@
 import org.littletonrobotics.junction.Logger;
 
 public class LEDSubsystem extends SubsystemBase {
-  public static final int LED_LENGTH = 80;
+  public static final int LED_LENGTH = 36;
   LEDIO io;
   LEDIOInputsAutoLogged inputs = new LEDIOInputsAutoLogged();
 

From 4c5f5af87167abc51e3277d33d4f36f3ad931988 Mon Sep 17 00:00:00 2001
From: Lewis-Seiden <lewy@seiden.us>
Date: Sat, 17 Feb 2024 13:54:04 -0800
Subject: [PATCH 14/62] added lediosim

---
 .../frc/robot/subsystems/leds/LEDIOSim.java   | 32 +++++++++++++++++++
 1 file changed, 32 insertions(+)
 create mode 100644 src/main/java/frc/robot/subsystems/leds/LEDIOSim.java

diff --git a/src/main/java/frc/robot/subsystems/leds/LEDIOSim.java b/src/main/java/frc/robot/subsystems/leds/LEDIOSim.java
new file mode 100644
index 00000000..5ff56e8c
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/leds/LEDIOSim.java
@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems.leds;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.util.Color;
+
+public class LEDIOSim implements LEDIO {
+  AddressableLED led;
+  AddressableLEDBuffer buffer;
+
+  public LEDIOSim() {
+    led = new AddressableLED(3);
+    buffer = new AddressableLEDBuffer(LEDSubsystem.LED_LENGTH);
+    led.setLength(buffer.getLength());
+    led.start();
+  }
+
+  /** Write data from buffer to leds */
+  @Override
+  public void updateInputs(LEDIOInputs inputs) {
+    led.setData(buffer);
+  }
+
+  @Override
+  public void set(int i, Color color) {
+    buffer.setLED(i, color);
+  }
+}

From 62bbb88095b40cc85b34d535614e57a21f2f3431 Mon Sep 17 00:00:00 2001
From: Lewis-Seiden <Lewis-Seiden@users.noreply.github.com>
Date: Sat, 17 Feb 2024 21:58:02 +0000
Subject: [PATCH 15/62] run spotless

---
 src/main/java/frc/robot/Robot.java                              | 1 -
 src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java | 2 --
 2 files changed, 3 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 57a8fc84..7ab1ca6b 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -44,7 +44,6 @@
 import frc.robot.utils.CommandXboxControllerSubsystem;
 import frc.robot.utils.autoaim.AutoAim;
 import java.util.function.Supplier;
-import frc.robot.utils.CommandXboxControllerSubsystem;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java
index 5da1baf5..07feb35e 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java
@@ -4,7 +4,6 @@
 
 import com.ctre.phoenix6.SignalLogger;
 import com.google.common.base.Supplier;
-import com.google.common.base.Supplier;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
@@ -20,7 +19,6 @@
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
 import java.util.function.DoubleSupplier;
-import java.util.function.DoubleSupplier;
 import org.littletonrobotics.junction.Logger;
 
 public class ShooterSubystem extends SubsystemBase {

From bd3b37383e7a44d08cccfd199f5b4b83e41bbcf0 Mon Sep 17 00:00:00 2001
From: Lewis-Seiden <lewy@seiden.us>
Date: Sat, 17 Feb 2024 13:58:46 -0800
Subject: [PATCH 16/62] added sim ledio

---
 simgui.json                                   |  5 +++
 src/main/java/frc/robot/Robot.java            |  6 ++--
 .../frc/robot/subsystems/leds/LEDIOSim.java   | 32 +++++++++++++++++++
 .../subsystems/shooter/ShooterSubystem.java   |  2 --
 4 files changed, 41 insertions(+), 4 deletions(-)
 create mode 100644 src/main/java/frc/robot/subsystems/leds/LEDIOSim.java

diff --git a/simgui.json b/simgui.json
index 1d91652d..3f0177eb 100644
--- a/simgui.json
+++ b/simgui.json
@@ -4,6 +4,11 @@
       "/AdvantageKit/RealOutputs/Elevator/Mechanism2d": "Mechanism2d",
       "/AdvantageKit/RealOutputs/Shooter/Mechanism2d": "Mechanism2d",
       "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Run Elevator Sysid": "Command",
+      "/SmartDashboard/Run Flywheel Sysid": "Command",
+      "/SmartDashboard/Run Pivot Sysid": "Command",
+      "/SmartDashboard/Run Swerve Azimuth Sysid": "Command",
+      "/SmartDashboard/Run Swerve Drive Sysid": "Command",
       "/SmartDashboard/Shooter shoot": "Command"
     }
   },
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 57a8fc84..be54ad31 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -11,6 +11,7 @@
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
@@ -31,6 +32,7 @@
 import frc.robot.subsystems.intake.IntakeIOReal;
 import frc.robot.subsystems.intake.IntakeSubsystem;
 import frc.robot.subsystems.leds.LEDIOReal;
+import frc.robot.subsystems.leds.LEDIOSim;
 import frc.robot.subsystems.leds.LEDSubsystem;
 import frc.robot.subsystems.reaction_bar_release.ReactionBarReleaseIOReal;
 import frc.robot.subsystems.reaction_bar_release.ReactionBarReleaseSubsystem;
@@ -44,7 +46,6 @@
 import frc.robot.utils.CommandXboxControllerSubsystem;
 import frc.robot.utils.autoaim.AutoAim;
 import java.util.function.Supplier;
-import frc.robot.utils.CommandXboxControllerSubsystem;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
@@ -97,7 +98,8 @@ public void setYaw(Rotation2d yaw) {}
   private final CarriageSubsystem carriage = new CarriageSubsystem(new CarriageIOReal());
   private final ReactionBarReleaseSubsystem reactionBarRelease =
       new ReactionBarReleaseSubsystem(new ReactionBarReleaseIOReal());
-  private final LEDSubsystem leds = new LEDSubsystem(new LEDIOReal());
+  private final LEDSubsystem leds =
+      new LEDSubsystem(mode == RobotMode.REAL ? new LEDIOReal() : new LEDIOSim());
 
   @Override
   public void robotInit() {
diff --git a/src/main/java/frc/robot/subsystems/leds/LEDIOSim.java b/src/main/java/frc/robot/subsystems/leds/LEDIOSim.java
new file mode 100644
index 00000000..5ff56e8c
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/leds/LEDIOSim.java
@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems.leds;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.util.Color;
+
+public class LEDIOSim implements LEDIO {
+  AddressableLED led;
+  AddressableLEDBuffer buffer;
+
+  public LEDIOSim() {
+    led = new AddressableLED(3);
+    buffer = new AddressableLEDBuffer(LEDSubsystem.LED_LENGTH);
+    led.setLength(buffer.getLength());
+    led.start();
+  }
+
+  /** Write data from buffer to leds */
+  @Override
+  public void updateInputs(LEDIOInputs inputs) {
+    led.setData(buffer);
+  }
+
+  @Override
+  public void set(int i, Color color) {
+    buffer.setLED(i, color);
+  }
+}
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java
index 5da1baf5..07feb35e 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java
@@ -4,7 +4,6 @@
 
 import com.ctre.phoenix6.SignalLogger;
 import com.google.common.base.Supplier;
-import com.google.common.base.Supplier;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
@@ -20,7 +19,6 @@
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
 import java.util.function.DoubleSupplier;
-import java.util.function.DoubleSupplier;
 import org.littletonrobotics.junction.Logger;
 
 public class ShooterSubystem extends SubsystemBase {

From 105d1e2d3a5079d2b14b384ad14195b5b9f6232c Mon Sep 17 00:00:00 2001
From: Lewis-Seiden <lewy@seiden.us>
Date: Sat, 17 Feb 2024 14:52:07 -0800
Subject: [PATCH 17/62] updated ledioreal to reflect wiring changes

---
 .../java/frc/robot/subsystems/leds/LEDIOReal.java | 15 +++++----------
 .../java/frc/robot/subsystems/leds/LEDIOSim.java  |  1 +
 2 files changed, 6 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/leds/LEDIOReal.java b/src/main/java/frc/robot/subsystems/leds/LEDIOReal.java
index b43ec6ac..dfee71b1 100644
--- a/src/main/java/frc/robot/subsystems/leds/LEDIOReal.java
+++ b/src/main/java/frc/robot/subsystems/leds/LEDIOReal.java
@@ -9,25 +9,20 @@
 import edu.wpi.first.wpilibj.util.Color;
 
 public class LEDIOReal implements LEDIO {
-  AddressableLED leftLed;
-  AddressableLED rightLed;
+  AddressableLED led;
   AddressableLEDBuffer buffer;
 
   public LEDIOReal() {
-    leftLed = new AddressableLED(3);
-    rightLed = new AddressableLED(4);
+    led = new AddressableLED(3);
     buffer = new AddressableLEDBuffer(LEDSubsystem.LED_LENGTH);
-    leftLed.setLength(buffer.getLength());
-    leftLed.start();
-    rightLed.setLength(buffer.getLength());
-    rightLed.start();
+    led.setLength(buffer.getLength());
+    led.start();
   }
 
   /** Write data from buffer to leds */
   @Override
   public void updateInputs(LEDIOInputs inputs) {
-    leftLed.setData(buffer);
-    rightLed.setData(buffer);
+    led.setData(buffer);
   }
 
   @Override
diff --git a/src/main/java/frc/robot/subsystems/leds/LEDIOSim.java b/src/main/java/frc/robot/subsystems/leds/LEDIOSim.java
index 5ff56e8c..e0d3634b 100644
--- a/src/main/java/frc/robot/subsystems/leds/LEDIOSim.java
+++ b/src/main/java/frc/robot/subsystems/leds/LEDIOSim.java
@@ -8,6 +8,7 @@
 import edu.wpi.first.wpilibj.AddressableLEDBuffer;
 import edu.wpi.first.wpilibj.util.Color;
 
+// This is a copy of LEDIOReal right now, but if we move leds to separate ports sim breaks w ledioreal
 public class LEDIOSim implements LEDIO {
   AddressableLED led;
   AddressableLEDBuffer buffer;

From 1d82fd6420932451d04ad4c2a84587bd60b75256 Mon Sep 17 00:00:00 2001
From: Lewis-Seiden <Lewis-Seiden@users.noreply.github.com>
Date: Sat, 17 Feb 2024 22:53:15 +0000
Subject: [PATCH 18/62] run spotless

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

diff --git a/src/main/java/frc/robot/subsystems/leds/LEDIOSim.java b/src/main/java/frc/robot/subsystems/leds/LEDIOSim.java
index e0d3634b..db5dcb3a 100644
--- a/src/main/java/frc/robot/subsystems/leds/LEDIOSim.java
+++ b/src/main/java/frc/robot/subsystems/leds/LEDIOSim.java
@@ -8,7 +8,8 @@
 import edu.wpi.first.wpilibj.AddressableLEDBuffer;
 import edu.wpi.first.wpilibj.util.Color;
 
-// This is a copy of LEDIOReal right now, but if we move leds to separate ports sim breaks w ledioreal
+// This is a copy of LEDIOReal right now, but if we move leds to separate ports sim breaks w
+// ledioreal
 public class LEDIOSim implements LEDIO {
   AddressableLED led;
   AddressableLEDBuffer buffer;

From adb521c0b685e76c875506767305ffb0ca89fe86 Mon Sep 17 00:00:00 2001
From: Lewis-Seiden <lewy@seiden.us>
Date: Sat, 17 Feb 2024 15:31:04 -0800
Subject: [PATCH 19/62] response to pr feedback

---
 src/main/java/frc/robot/Robot.java                  |  2 +-
 .../java/frc/robot/subsystems/leds/LEDIOReal.java   |  4 ++--
 .../java/frc/robot/subsystems/leds/LEDIOSim.java    |  6 +++---
 .../frc/robot/subsystems/leds/LEDSubsystem.java     | 13 ++++++-------
 4 files changed, 12 insertions(+), 13 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index be54ad31..412fbeaa 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -164,7 +164,7 @@ public void robotInit() {
     reactionBarRelease.setDefaultCommand(
         reactionBarRelease.setRotationCmd(Rotation2d.fromDegrees(0.0)));
     leds.setDefaultCommand(
-        leds.defaultStateDisplay(
+        leds.defaultStateDisplayCmd(
             () -> DriverStation.isEnabled(), () -> currentTarget == Target.SPEAKER));
 
     controller.setDefaultCommand(controller.rumbleCmd(0.0, 0.0));
diff --git a/src/main/java/frc/robot/subsystems/leds/LEDIOReal.java b/src/main/java/frc/robot/subsystems/leds/LEDIOReal.java
index dfee71b1..df5d365c 100644
--- a/src/main/java/frc/robot/subsystems/leds/LEDIOReal.java
+++ b/src/main/java/frc/robot/subsystems/leds/LEDIOReal.java
@@ -9,8 +9,8 @@
 import edu.wpi.first.wpilibj.util.Color;
 
 public class LEDIOReal implements LEDIO {
-  AddressableLED led;
-  AddressableLEDBuffer buffer;
+  private final AddressableLED led;
+  private final AddressableLEDBuffer buffer;
 
   public LEDIOReal() {
     led = new AddressableLED(3);
diff --git a/src/main/java/frc/robot/subsystems/leds/LEDIOSim.java b/src/main/java/frc/robot/subsystems/leds/LEDIOSim.java
index e0d3634b..0c8ffebf 100644
--- a/src/main/java/frc/robot/subsystems/leds/LEDIOSim.java
+++ b/src/main/java/frc/robot/subsystems/leds/LEDIOSim.java
@@ -8,10 +8,10 @@
 import edu.wpi.first.wpilibj.AddressableLEDBuffer;
 import edu.wpi.first.wpilibj.util.Color;
 
-// This is a copy of LEDIOReal right now, but if we move leds to separate ports sim breaks w ledioreal
+// This is a copy of LEDIOReal right now, but if we move leds to separate ports sim breaks w LEDIOReal
 public class LEDIOSim implements LEDIO {
-  AddressableLED led;
-  AddressableLEDBuffer buffer;
+  private final AddressableLED led;
+  private final AddressableLEDBuffer buffer;
 
   public LEDIOSim() {
     led = new AddressableLED(3);
diff --git a/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java
index 17be9b80..c30cc07f 100644
--- a/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java
@@ -18,13 +18,12 @@
 
 public class LEDSubsystem extends SubsystemBase {
   public static final int LED_LENGTH = 36;
-  LEDIO io;
-  LEDIOInputsAutoLogged inputs = new LEDIOInputsAutoLogged();
+  
+  private final LEDIO io;
+  private final LEDIOInputsAutoLogged inputs = new LEDIOInputsAutoLogged();
 
-  Color[] colors = new Color[LED_LENGTH];
-
-  double rainbowStart = 0;
-  double dashStart = 0;
+  private double rainbowStart = 0;
+  private double dashStart = 0;
 
   /** Creates a new LEDSubsystem. */
   public LEDSubsystem(LEDIO io) {
@@ -93,7 +92,7 @@ public Command setRunAlongCmd(
         });
   }
 
-  public Command defaultStateDisplay(BooleanSupplier enabled, BooleanSupplier targetIsSpeaker) {
+  public Command defaultStateDisplayCmd(BooleanSupplier enabled, BooleanSupplier targetIsSpeaker) {
     return Commands.either(
             Commands.either(
                 this.setBlinkingCmd(new Color("#ffff00"), new Color(), 10.0)

From 82f22bc408ae3e77cfa8d27c083c6dcf01752189 Mon Sep 17 00:00:00 2001
From: Stoopipoopy <anotherguywithabeanie@gmail.com>
Date: Sat, 17 Feb 2024 17:55:54 -0800
Subject: [PATCH 20/62] added dynamicAuto folder, and can determine closest
 note by distane, priority, and blacklist

---
 ctre_sim/Talon FX vers. C - 00 - 0 - ext.dat  | Bin 0 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat | Bin 0 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat | Bin 0 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat | Bin 0 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 018 - 0 - ext.dat | Bin 0 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 simgui-ds.json                                |   5 +-
 src/main/java/frc/robot/AutoManager.java      |   4 --
 src/main/java/frc/robot/Robot.java            |   3 +
 .../robot/utils/dynamicauto/DynamicAuto.java  |  54 ++++++++++++++++++
 .../frc/robot/utils/dynamicauto/Note.java     |  47 +++++++++++++++
 vendordeps/ChoreoLib.json                     |  43 ++++++++++++++
 14 files changed, 150 insertions(+), 6 deletions(-)
 create mode 100644 ctre_sim/Talon FX vers. C - 00 - 0 - ext.dat
 create mode 100644 ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat
 create mode 100644 ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat
 create mode 100644 ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat
 create mode 100644 ctre_sim/Talon FX vers. C - 018 - 0 - ext.dat
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/Note.java
 create mode 100644 vendordeps/ChoreoLib.json

diff --git a/ctre_sim/Talon FX vers. C - 00 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 00 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..520f0aea026407f7abe1db48e50fd73d745ec3f4
GIT binary patch
literal 2048
zcmcgqOK4M35S@HlqjnK76uYXT*+?<8iy%beGnQ7w>O+f2-2{X#RBASY6oU^%aOV%C
zqGE6rK`^L`fKgJdl!_FKilRc}LN@_v(bkPt@BI2+0xn9#3(0*m_nb55&fE_Wb>P1*
zXDa~y8ebn_gVVUmQWgH4|53P6y|sF&^<8`!a@AY|e4j`?y}eNSc?v*dfH{iYN|Uef
zTvt{<WS`mhel+Tm7j-I<Pa52M52x49vF$rNg}hEVQ{CSIPAI?0gjnAZTv*P}1{OT7
zT>pEAS+_9sgnX&)meZWqx*`XU<-<Pnc^>`D##ZI2p5T?Q^)6pcz4rDi(ZzUA3sjo+
zBgO56KME@-ex~)VUzEPp_Mz_q;}M2_oVG*MCy;vsa6IPcJ@Rcjf&IN8y5$zmSHzd3
z7dOS7lZ+XMEcVg%o%NPWcKp8j5g#UxzLbwJejm&a=%j)Gt~S8iiEZaw4ff?6>sv_l
zEX{{BC-?pG>&+DF^Fe6OLF!w7Z;<4}HpdmwjmApDUVMIcE&ft}56xrnPKx}L9~~z>
zOX{j_T67Ci?{E6r2lcP^m4CB~{S-G1jP=9;D!bT^)^}P%^xm}2yN%-m=wE&7*fV-h
z;j3Tes~+O@{_O|M`>E5nk~~MfH~PhS*2lICPy3<zx*J*=xn^12Ya;w*{xiq;ewlfm
zbNxIFB#)g(pO*QdKGxSd+AniNE3vYf`g3n*iRKjf!MaZ3A;p{5HPN}^Q@&+2@yrnW
zQT$rrBYrl}2N7U?cK_tpVvbd`e;9SgS+9kUKz%q@Mkq5K=lSmN=n9m!Y6tV_U7rm4
zUtHsQgW=~RpA4cP_o~;+i(ayW?{CQ*o3k8ije5OwC-+6?ZpXN9C+>27qMiNx0dZnJ
AKL7v#

literal 0
HcmV?d00001

diff --git a/ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..51223a6e646f55e0270d60c47f7cc29513ee6652
GIT binary patch
literal 2048
zcmdT^O=uHQ5T2w#8fZffMS7}H(1Rd?ROv<BL#y^61}X&wL2sfd#=nyX_Y%}oLlr4h
zP^$Id#fm)$B5pBi4uYW<MbIuP^<b)nmTrG2`sU4i-EF7}3W5X6d-L9W-+XUol6Q3I
zJpFTXcM<U?7Q-g8mWdJ(_ov(Ubnw5{KT7KsGja1g{{Fx`+OXL27kE^8s!o!Jz4%tH
z8plyh`$nElE?i(o&g0|~`@m_~@^W=%ysO7}%*WItXIc-lpKD59K3*j%>b)DAuzAeY
zsy5s(6|j!n?NOV*q*LPzy;eO$?Mq{2@7`M*M;W0x@eq3D{^TveJAIG8GT*7flkFT?
zim%M?q*nUK?_Qj?{7IpwtgazmaKHnH0zywW<t(y8@!&10kNA+6$o2ucOfk9LM3N72
zDaC`wYmj@lXWo;(X+TZfCG~}q9qDf}kpAJlFCL(tVSShvct*0nmp8_^@34@YsIY1G
zAm4d&!tWqRSDe@5T<3CPY0H=BJC4=O*naHald$ypg!mQZ-MIBpAsSy_6W@2>xt*Wl
z-A=0~f4XY<pa;C3@}_^ok2*j<<O6?Wo9G#vY`&5*Gwj4fFY>!<7i^!9=gsEpH(5V)
zYZ<7bI)H~>;K7G@)^T)=`M0g%_d0Z5JLUI<emOOMR-1=<fFG-&_c;e!wz)|*ybnIx
zdLEgVKG*icgbZ@!S*K$@=I1#_9@H!TLF#2feDj$h6TXD5!@0IO#<i^vR-N-k3nHq_
ziXOzL6|dLy>8#)p&nwp-ek9%pCi-5S{bIH6bxZ)i5x)uc*>>h7zk99U`r|ufUiww%
zG(EHP>eySN??xM&-`i;4|8hX^=Vtxxv^j6Rzg!QW<Jeg5tDx)S&_F43r$zo3egM|O
Bc31!a

literal 0
HcmV?d00001

diff --git a/ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..61891ad70817b1de2afb92ea92508dec7d27cab4
GIT binary patch
literal 2048
zcmcgsO-NK>6ut9iiW#zD`dQRQrXm+XCQ%YW_=t+2i~*xvU_lV2H2o}E2)!0VwMtAv
zFeoZrv?<bs5YdY!a}hbXC<)4_u%NLFr8grC_3n4?nbAQAndk$#-tRl--1F|e`i|B)
z^xu!uK*T?lpZB!Zsa$2D3jfakFs&$5TMMmU!n+hN#4F%aNVFz{5(7o)A46F`u{Ce(
ze>O35ffKp6$RW<bQ&=%{PLDOU`Wfr7bpP2chdFOq8M$&IOVqCKPGH>B@t@{B52_|T
z_R(_psi~jVt+9LFs2`$Tg^?lm{#%2ixX7FwfM0n(bw~Ji$K&s;cRKZCD<jj^-1>HO
zZjSoSr75Ey5qU~zEb)a0K6q##{2ZfwhWSeQ&~4Wo_>wM>nFD<J&6IIVJ>a6shmKn%
z?@nvpmARpSMBpy#wP<o8^Nn~iKYZ_tKJ>HHAJzq*nVj$CU;M*!$0=zOrE2!v<~KJd
z`ZhosaUaIL?&Wx4<G0!0aXdI}=5e4cWaN_}$up|Ee$%7WY<##3-*Irz?9b1f_(`70
z8a>p7uB*DSzv!b6@DKao-`p&IMkZEWi~2ik2gEPx5A-h2@tjd-bi<8X>>s|Nmr)<!
z!!P*I1D@lN#YA|AzPnrIi0=lzJWuHG&=2S%T8o1B_<)a?@V=OSHs@u|<?{fJBYy+y
z^Y^0;`el8TeiiG?h+aq|c5B-jZ{9t}-$mw^_yInqeE9f$PWZrc<o+WEUe}Q>Zm%`_
z2tDZRUoHRNIBz`K+)&q+H+`tD6FhL>*?O@fDfOM}UFK%2UiPI+eNNGHv#-Ga_Pz1>
vaCNEKo0r|fKOY`E6!Y)<I$zg(T~fOYdosH7q46>7HGS&oPRDzz<^RGTheUz&

literal 0
HcmV?d00001

diff --git a/ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..3822b64e3b6fd0d64a8a778ffc7b280d49751f7a
GIT binary patch
literal 2048
zcmcgsO-NK>6ut9i3J2^pWQ41n7Dd>fM2Q6DTT}#L3>57G3nVF}v04hf7DKg&#6$#x
zq`yU*f`o*KUL>UuHAqpkF(PcySVq$Pz)<gg_a5Ik2*DVAAb0Nj&N=sd_g;KU^)CJQ
z<2DoVpPUeWR1n|L2Dbv*A}RmQ|7)r#uC^+-<59dAUj&~bQhO#WF_^{ov5cQMYOQ7X
z*>vF?C-U7QmpBJcVRNBxcJg4SpRpcG51-j~g!7J-(Mu;XL_K=%hNes%|7yW=zj`Li
zK00naHT6ezYn<Ef)DO|_;>4JD?}NcnTx3p;z^~k&x+#38_whH@>q<S@&d5<~etpND
zpQFBe;iu7$iaaGW27KXx4;~r_KW((%FkdMjx;>f$ALtU9Il!0SOc}S-1I|`Hbi8W0
zcRTZ*%nb!30(VhAizX*B-)L6mhxfjCfPM!3VO{W<$@xD1;UAtmPM}ScTDSKG-+4LF
zcL36e?^XD&dpT9y^tJRI?crH7j|1HiBcF^&o>twpTOOrK@l|E`-b2sL{`|;^pXBL`
z(L-J6da4`yi$3}Q|F94K^{wJ(V!HOK?eDM?62GWFa%V-3=Zre<>#q&5fB1%8T77^I
zzu-d;c#cPx65$^D-X57F-VJ<tp3vc;AJ9j%mW1!}0Ut4u{bKsrnwL43&jUD){07$N
z_oEK_Wqp!<&DAN0UPL2yt7%Pk!98EDh0?#o5AZSN>ot8oD}3NNa{rM7uj@$XcWgHM
z==Kj7iOEKpZ&QhG%dytxhVH!SLt}&BA8NsugHrE!+xX#_|7~Yd@^ZZa)!Vf(XWr+o
mDwz1&_r_;vz3dJ2+Ewr4m1Sn19L1+E+a|BIC63q0|Ajvz2#(4C

literal 0
HcmV?d00001

diff --git a/ctre_sim/Talon FX vers. C - 018 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 018 - 0 - ext.dat
new file mode 100644
index 0000000000000000000000000000000000000000..e585805a23469e4c37ea917b60e9f6f246c48d42
GIT binary patch
literal 2048
zcmcgsO=y!r5S@I9(m)NN_Rv#_iXH?Jq%DGo-@!j9YOvz15D$WAOQZED_+5f}vQ&{m
z1x0NSUR3O%AnGRu?LlnlMG>^3(jH8;#bVMz(4C!m`O;9(5^*6+X7{}}Z)Y}nPa8b?
z@5|do#Gl&N2U_pduCh{vf9HRk)|Q%`mDUgNJ|#+t8u%0ut=X`|P+9u>Sl%SA<_AWe
z&n%wjMD`u>h;#51Rt{I^Cp)@L#(FG0(%aI>c~@if(#bqgpPqZ6DO<-JExYd5&lcE6
z*PUm!epX+NJM>2V5bZ8ajQRK8SsW!q=F}+s%6ZRi;k*5hzp>uw%+ppzW}T(=-PqC`
z^}P!}tbSDFDXB5w3lDto&_MV(PVJWYTKUlJ(;WCfm&ncmzDzS^+)@v?nDU|H*UP!r
zz2M8-P(UJZ7j;`SIg$BB3o<|4_vJ(MGw2WNg3nCO_wyICc<xw%Hc@8N-dlX;7e(I%
zNF(;^u&?iOs<h>6<vmV>=j}WWo{L!dbVTx;>TcZnC{u~AtHSpmd12qrTv7a_&*ZHh
z>O$97-S{8$(Fgd4eeiE=6F(C(O;=*(4Z9)ni~6I(4Mm<a>b%{2{U-Z|Z|LRJ2l(&{
zKJ<X+cx*Kh&Y|z`kvZbtz*px<9Ul4teMDzf_%1K-5tD^!+t1X3%(;3Vz;Wa|us+`(
zb<i*8lk}@xXHoPb8k3znY~Qx8D=hoY8?;dFr}zOru6#YFFK2}hJV)+7@(O%PIeuGH
zBj|q<54D?@?;7OceGv!03C?-#OiO+5+JMa`+vL3rRQYu6DbX+Ejn~J^YwZ1B4GRCs
g^xV<7*>8DJ?njDoW?8@Eg^tfdgW1Gzv-~go2Jf?WRR910

literal 0
HcmV?d00001

diff --git a/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat
index 1083f416a8f7b26f24efff87c938db4eadc83ed3..fb779e1b29808fcabcb28a363e030d92bdae2afa 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx#VdJANnTdi{Y{wYZJ?a6m7BR5_0IU8AyZ`_I

delta 30
jcmZn=Xb{-Yz{qxzLG`_mz(heSwi65*?*%}tMNDh}kUj{$

diff --git a/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat
index b76d1bce23a0681d7dea259a941318ffc318d90b..06a7fad6dcd8de57429919a57e1202cfa7316840 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx#VdJANnTdi{Y{wYZJ?a6m7BR5_0IU8AyZ`_I

delta 30
jcmZn=Xb{-Yz{qxzLG`_mz(heSwi65*?*%}tMNDh}kUj{$

diff --git a/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat
index b476eeb8d54b9f4746257b1d392e0b9461c76eae..56843e473a87865c3712a51395166e93b345cb02 100644
GIT binary patch
delta 36
ncmZn=Xb{-Yz{q)&Vez9583qP{iGt>w#~2no>Vk3>F|h#v*D?yc

delta 30
jcmZn=Xb{-Yz{qxzLG`_mz(heSwi65*?*%}tMNDh}kUj{$

diff --git a/simgui-ds.json b/simgui-ds.json
index aca5a327..4c9bde9a 100644
--- a/simgui-ds.json
+++ b/simgui-ds.json
@@ -93,8 +93,9 @@
   ],
   "robotJoysticks": [
     {
-      "guid": "Keyboard0",
-      "name": "Keyboard"
+      "guid": "78696e70757401000000000000000000",
+      "name": "Keyboard",
+      "useGamepad": true
     },
     {
       "guid": "Keyboard1"
diff --git a/src/main/java/frc/robot/AutoManager.java b/src/main/java/frc/robot/AutoManager.java
index f2d0dd7a..df87d69f 100644
--- a/src/main/java/frc/robot/AutoManager.java
+++ b/src/main/java/frc/robot/AutoManager.java
@@ -3,18 +3,14 @@
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.commands.PathPlannerAuto;
-
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
 import frc.robot.subsystems.feeder.FeederSubsystem;
 import frc.robot.subsystems.intake.IntakeSubsystem;
 import frc.robot.subsystems.shooter.ShooterSubystem;
 import frc.robot.subsystems.swerve.SwerveSubsystem;
-
 import java.util.ArrayList;
 import java.util.List;
-
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
 public final class AutoManager {
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index bc88d410..c4b80745 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -23,6 +23,7 @@
 import frc.robot.subsystems.swerve.GyroIO;
 import frc.robot.subsystems.swerve.GyroIOPigeon2;
 import frc.robot.subsystems.swerve.SwerveSubsystem;
+import frc.robot.utils.dynamicauto.DynamicAuto;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
@@ -143,6 +144,8 @@ public void robotInit() {
   @Override
   public void robotPeriodic() {
     CommandScheduler.getInstance().run();
+    Logger.recordOutput(
+        "DynamicAuto/Closest Note", DynamicAuto.getClosestNote(swerve.getPose()).getPose());
   }
 
   @Override
diff --git a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
new file mode 100644
index 00000000..d84c9c33
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
@@ -0,0 +1,54 @@
+package frc.robot.utils.dynamicauto;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+
+public class DynamicAuto {
+
+  public static final Note[] notes = {
+    new Note(new Pose2d(2.204, 7.0, Rotation2d.fromRadians(-Math.PI)), true, 0), // w1
+    new Note(new Pose2d(2.204, 5.5, Rotation2d.fromRadians(-Math.PI)), false, 1), // w2
+    new Note(new Pose2d(2.204, 4.1, Rotation2d.fromRadians(-Math.PI)), false, 2), // w3
+    new Note(new Pose2d(7.538, 7.3, Rotation2d.fromRadians(-Math.PI)), true, 0), // c1
+    new Note(new Pose2d(7.538, 5.7, Rotation2d.fromRadians(-Math.PI)), false, 0), // c2
+    new Note(new Pose2d(7.538, 4.1, Rotation2d.fromRadians(-Math.PI)), false, 1), // c3
+    new Note(new Pose2d(7.538, 2.5, Rotation2d.fromRadians(-Math.PI)), false, 2), // c4
+    new Note(new Pose2d(7.538, 0.7, Rotation2d.fromRadians(-Math.PI)), true, 3), // c5
+  };
+
+  ///  public static final Pose2d
+
+  //  public static PathPlannerPath makePath(){
+  ///    List<Translation2d> fgishf = PathPlannerPath.bezierFromPoses();
+  ///  }
+
+  public static Note getClosestNote(Pose2d curPose) {
+    Note closestNote = new Note();
+    double shortestDistance = 99999999;
+    for (Note note : notes) {
+      System.out.println(note.getPriority());
+      System.out.println(closestNote.getPriority());
+      if (note.getBlacklist()) {
+        System.out.println("iojfeioeoi");
+        continue;
+
+      } else if (note.getPriority() > closestNote.getPriority()) {
+        shortestDistance = curPose.minus(note.getPose()).getTranslation().getNorm();
+        closestNote = note;
+        if (shortestDistance > curPose.minus(note.getPose()).getTranslation().getNorm()) {
+          shortestDistance = curPose.minus(note.getPose()).getTranslation().getNorm();
+          closestNote = note;
+          System.out.println("ghoghsghuhguhuihi");
+        }
+      } else if (note.getPriority() == closestNote.getPriority()) {
+        if (shortestDistance > curPose.minus(note.getPose()).getTranslation().getNorm()) {
+          shortestDistance = curPose.minus(note.getPose()).getTranslation().getNorm();
+          closestNote = note;
+          System.out.println("ghoghsghuhguhuihi");
+        }
+      }
+    }
+
+    return closestNote;
+  }
+}
diff --git a/src/main/java/frc/robot/utils/dynamicauto/Note.java b/src/main/java/frc/robot/utils/dynamicauto/Note.java
new file mode 100644
index 00000000..250db29f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/Note.java
@@ -0,0 +1,47 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.utils.dynamicauto;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+/** Add your docs here. */
+public class Note {
+
+  private boolean blacklisted;
+  private int priority;
+  private final Pose2d pose;
+
+  public Note(Pose2d pose, boolean blacklisted, int priority) {
+    this.pose = pose;
+    this.blacklisted = blacklisted;
+    this.priority = priority;
+  }
+
+  public Note() {
+    this.pose = new Pose2d();
+    this.blacklisted = true;
+    this.priority = -1;
+  }
+
+  public void setBlacklist(boolean blacklist) {
+    this.blacklisted = blacklist;
+  }
+
+  public void setPriority(Integer priority) {
+    this.priority = priority;
+  }
+
+  public boolean getBlacklist() {
+    return blacklisted;
+  }
+
+  public int getPriority() {
+    return priority;
+  }
+
+  public Pose2d getPose() {
+    return pose;
+  }
+}
diff --git a/vendordeps/ChoreoLib.json b/vendordeps/ChoreoLib.json
new file mode 100644
index 00000000..387b2de5
--- /dev/null
+++ b/vendordeps/ChoreoLib.json
@@ -0,0 +1,43 @@
+{
+    "fileName": "ChoreoLib.json",
+    "name": "ChoreoLib",
+    "version": "2024.1.3",
+    "uuid": "287cff6e-1b60-4412-8059-f6834fb30e30",
+    "frcYear": "2024",
+    "mavenUrls": [
+        "https://SleipnirGroup.github.io/ChoreoLib/dep"
+    ],
+    "jsonUrl": "https://SleipnirGroup.github.io/ChoreoLib/dep/ChoreoLib.json",
+    "javaDependencies": [
+        {
+            "groupId": "com.choreo.lib",
+            "artifactId": "ChoreoLib-java",
+            "version": "2024.1.3"
+        },
+        {
+            "groupId": "com.google.code.gson",
+            "artifactId": "gson",
+            "version": "2.10.1"
+        }
+    ],
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "com.choreo.lib",
+            "artifactId": "ChoreoLib-cpp",
+            "version": "2024.1.3",
+            "libName": "ChoreoLib",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "osxuniversal",
+                "linuxathena",
+                "linuxarm32",
+                "linuxarm64"
+            ]
+        }
+    ]
+}
\ No newline at end of file

From 87759b344185f06c946e559577261190c39db038 Mon Sep 17 00:00:00 2001
From: Lewis-Seiden <lewy@seiden.us>
Date: Sun, 18 Feb 2024 11:49:54 -0800
Subject: [PATCH 21/62] put talons on canivore

---
 .../java/frc/robot/subsystems/carriage/CarriageIOReal.java  | 2 +-
 .../java/frc/robot/subsystems/elevator/ElevatorIOReal.java  | 4 ++--
 src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java | 2 +-
 src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java | 4 ++--
 src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java   | 4 ++--
 .../java/frc/robot/subsystems/shooter/ShooterIOReal.java    | 6 +++---
 src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java | 4 ++--
 7 files changed, 13 insertions(+), 13 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java b/src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java
index 7b73aa1e..bc9af0b1 100644
--- a/src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java
+++ b/src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java
@@ -14,7 +14,7 @@
 
 /** Create a CarriageIO that uses a real TalonFX. */
 public class CarriageIOReal implements CarriageIO {
-  final TalonFX motor = new TalonFX(18);
+  final TalonFX motor = new TalonFX(18, "canivore");
 
   final DigitalInput beambreak = new DigitalInput(2);
 
diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java
index 2c87f3de..255801bc 100644
--- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java
+++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java
@@ -16,8 +16,8 @@
 
 /** Elevator IO using TalonFXs. */
 public class ElevatorIOReal implements ElevatorIO {
-  private final TalonFX motor = new TalonFX(16);
-  private final TalonFX follower = new TalonFX(17);
+  private final TalonFX motor = new TalonFX(16, "canivore");
+  private final TalonFX follower = new TalonFX(17, "canivore");
 
   private final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true);
   private final MotionMagicVoltage positionVoltage =
diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java
index c8afed61..a6d41133 100644
--- a/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java
+++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java
@@ -14,7 +14,7 @@
 
 /** Feeder IO using a TalonFX. */
 public class FeederIOReal implements FeederIO {
-  private final TalonFX motor = new TalonFX(13);
+  private final TalonFX motor = new TalonFX(13, "canivore");
 
   DigitalInput firstBeambreak = new DigitalInput(0);
   DigitalInput lastBeambreak = new DigitalInput(1);
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java
index 297503a4..16bfbaf1 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java
@@ -13,8 +13,8 @@
 
 /** Intake IO implementation for TalonFX motors. */
 public class IntakeIOReal implements IntakeIO {
-  private final TalonFX intakeMotor = new TalonFX(14);
-  private final TalonFX centeringMotor = new TalonFX(15);
+  private final TalonFX intakeMotor = new TalonFX(14, "canivore");
+  private final TalonFX centeringMotor = new TalonFX(15, "canivore");
 
   private final VoltageOut intakeVoltageOut = new VoltageOut(0.0).withEnableFOC(true);
   private final VoltageOut centeringVoltageOut = new VoltageOut(0.0).withEnableFOC(true);
diff --git a/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java
index 17be9b80..762a3a9b 100644
--- a/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java
@@ -105,14 +105,14 @@ public Command defaultStateDisplay(BooleanSupplier enabled, BooleanSupplier targ
                     // Set color to be purple with a moving dash corresponding to alliance color
                     () -> {
                       if (DriverStation.getAlliance().isEmpty()) {
-                        return new Color("#350868");
+                        return new Color("#b59aff");
                       } else if (DriverStation.getAlliance().get() == Alliance.Red) {
                         return new Color("#ff0000");
                       } else { // Blue
                         return new Color("#0000ff");
                       }
                     },
-                    () -> new Color("#b59aff"),
+                    () -> new Color("#350868"),
                     10,
                     1.0)
                 .until(enabled),
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java
index 4ed9283f..9c76c464 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java
@@ -12,9 +12,9 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 
 public class ShooterIOReal implements ShooterIO {
-  private final TalonFX pivotMotor = new TalonFX(10);
-  private final TalonFX flywheelLeftMotor = new TalonFX(11);
-  private final TalonFX flywheelRightMotor = new TalonFX(12);
+  private final TalonFX pivotMotor = new TalonFX(10, "canivore");
+  private final TalonFX flywheelLeftMotor = new TalonFX(11, "canivore");
+  private final TalonFX flywheelRightMotor = new TalonFX(12, "canivore");
 
   private final StatusSignal<Double> pivotVelocity = pivotMotor.getVelocity();
   private final StatusSignal<Double> pivotVoltage = pivotMotor.getMotorVoltage();
diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java
index a5f7c88e..2cd9b928 100644
--- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java
+++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java
@@ -77,8 +77,8 @@ public class ModuleIOReal implements ModuleIO {
   public ModuleIOReal(ModuleConstants constants) {
     name = constants.prefix();
 
-    driveTalon = new TalonFX(constants.driveID());
-    turnTalon = new TalonFX(constants.turnID());
+    driveTalon = new TalonFX(constants.driveID(), "canivore");
+    turnTalon = new TalonFX(constants.turnID(), "canivore");
     cancoder = new CANcoder(constants.cancoderID());
 
     var driveConfig = new TalonFXConfiguration();

From 43de39985f83be2232629a7cbd46ef05059e3f80 Mon Sep 17 00:00:00 2001
From: Lewis-Seiden <lewy@seiden.us>
Date: Sun, 18 Feb 2024 12:02:20 -0800
Subject: [PATCH 22/62] removed automatic logging of virtual target bc of loop
 times

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

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 469c532b..95a77191 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -34,8 +34,6 @@
 import frc.robot.subsystems.leds.LEDIOReal;
 import frc.robot.subsystems.leds.LEDIOSim;
 import frc.robot.subsystems.leds.LEDSubsystem;
-import frc.robot.subsystems.reaction_bar_release.ReactionBarReleaseIOReal;
-import frc.robot.subsystems.reaction_bar_release.ReactionBarReleaseSubsystem;
 import frc.robot.subsystems.shooter.ShooterIOReal;
 import frc.robot.subsystems.shooter.ShooterIOSim;
 import frc.robot.subsystems.shooter.ShooterSubystem;
@@ -96,8 +94,8 @@ public void setYaw(Rotation2d yaw) {}
   private final ShooterSubystem shooter =
       new ShooterSubystem(mode == RobotMode.REAL ? new ShooterIOReal() : new ShooterIOSim());
   private final CarriageSubsystem carriage = new CarriageSubsystem(new CarriageIOReal());
-  private final ReactionBarReleaseSubsystem reactionBarRelease =
-      new ReactionBarReleaseSubsystem(new ReactionBarReleaseIOReal());
+  // private final ReactionBarReleaseSubsystem reactionBarRelease =
+  //     new ReactionBarReleaseSubsystem(new ReactionBarReleaseIOReal());
   private final LEDSubsystem leds =
       new LEDSubsystem(mode == RobotMode.REAL ? new LEDIOReal() : new LEDIOSim());
 
@@ -123,7 +121,7 @@ public void robotInit() {
 
     switch (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
         new PowerDistribution(1, ModuleType.kRev); // Enables power distribution logging
         break;
@@ -161,8 +159,8 @@ public void robotInit() {
     shooter.setDefaultCommand(
         shooter.runStateCmd(
             () -> Rotation2d.fromDegrees(0.0), () -> flywheelIdleSpeed, () -> flywheelIdleSpeed));
-    reactionBarRelease.setDefaultCommand(
-        reactionBarRelease.setRotationCmd(Rotation2d.fromDegrees(0.0)));
+    // reactionBarRelease.setDefaultCommand(
+    //     reactionBarRelease.setRotationCmd(Rotation2d.fromDegrees(0.0)));
     leds.setDefaultCommand(
         leds.defaultStateDisplay(
             () -> DriverStation.isEnabled(), () -> currentTarget == Target.SPEAKER));
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
index 51993813..bfac133a 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
@@ -450,7 +450,6 @@ public Pose2d getVirtualTarget(ChassisSpeeds speedsRobotRelative) {
             .inverse());
   }
 
-  @AutoLogOutput(key = "AutoAim/Virtual Target")
   public Pose2d getVirtualTarget() {
     return getVirtualTarget(getVelocity());
   }

From 17f36d5c309cc6c0dc88c70f112e10b025208c6d Mon Sep 17 00:00:00 2001
From: Stoopipoopy <anotherguywithabeanie@gmail.com>
Date: Sun, 18 Feb 2024 12:08:10 -0800
Subject: [PATCH 23/62] robot can determine the closest shooting location based
 on its pose

---
 ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 src/main/java/frc/robot/Robot.java            |   4 +++
 .../robot/utils/dynamicauto/DynamicAuto.java  |  28 +++++++++++++++---
 5 files changed, 28 insertions(+), 4 deletions(-)

diff --git a/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat
index fb779e1b29808fcabcb28a363e030d92bdae2afa..8412ca3c645ee62940c463aae705fed7d2d207fe 100644
GIT binary patch
delta 54
zcmZn=Xb@nz%%IyaQ80C)0`J5!FP19|I#AwZGe#el!bhiMCJI`y9bqVWbOOX$#KZ;w
D-e?iM

delta 54
zcmZn=Xb@nz$e`aaQ80C)0`J5!FP2LTdQjeEGe#eljgPuyCJI`y9b;Jcs0YMa#KZ;w
D-&_&B

diff --git a/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat
index 06a7fad6dcd8de57429919a57e1202cfa7316840..7d12321016677911d1053a82d8606e787685b6ed 100644
GIT binary patch
delta 54
zcmZn=Xb@nz$e`CSQ80C)0`J5!FP2LTx=`L^Ge#el!bhiMCJI`y9bqVWbOOX$#KZ;w
D-VhPH

delta 54
zcmZn=Xb@nz&!EsSQ80C)0`J5!FP3`@icsEUGe#eljgPuyCJI`y9b;Jcs0YMa#KZ;w
D-#`(&

diff --git a/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat
index 56843e473a87865c3712a51395166e93b345cb02..fab79fa0d3173c1b8d61b0657c419717a2bd3b02 100644
GIT binary patch
delta 60
zcmZn=Xb@nz%#cz&Q80C)0`J5!FP19|$xz;8Ge#fw<VPoD7#IX53Yv2sVMu#)49Z!=
G#0CIGtr8&s

delta 60
zcmZn=Xb@nz$dFb&Q80C)0`J5!FP2LTsZiczGe#fw#g95<7#IX53Yv2sV_5L03(8r<
G#0CIHp%Nhg

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index c4b80745..4d990c25 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -146,6 +146,10 @@ public void robotPeriodic() {
     CommandScheduler.getInstance().run();
     Logger.recordOutput(
         "DynamicAuto/Closest Note", DynamicAuto.getClosestNote(swerve.getPose()).getPose());
+
+    Logger.recordOutput(
+        "DynamicAuto/Closest Shooting Location",
+        DynamicAuto.closestShootingLocation(swerve.getPose()));
   }
 
   @Override
diff --git a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
index d84c9c33..4d457e88 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
@@ -1,7 +1,10 @@
 package frc.robot.utils.dynamicauto;
 
+import com.pathplanner.lib.path.PathPlannerPath;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import java.util.List;
 
 public class DynamicAuto {
 
@@ -16,11 +19,28 @@ public class DynamicAuto {
     new Note(new Pose2d(7.538, 0.7, Rotation2d.fromRadians(-Math.PI)), true, 3), // c5
   };
 
-  ///  public static final Pose2d
+  public static final Pose2d[] shootingLocations = {
+    new Pose2d(5.639, 6.463, Rotation2d.fromRadians(0.1)), // Left
+    new Pose2d(4.216, 5.216, Rotation2d.fromRadians(0)), // Middle
+    new Pose2d(6.048, 1.746, Rotation2d.fromRadians(-0.179)) // Right
+  };
+
+  public static Pose2d closestShootingLocation(Pose2d curPose) {
+    Pose2d closestPosition = new Pose2d();
+    double shortestDistance = 9999999;
+    for (Pose2d location : shootingLocations) {
+      if (shortestDistance > curPose.minus(location).getTranslation().getNorm()) {
+        closestPosition = location;
+        shortestDistance = curPose.minus(location).getTranslation().getNorm();
+      }
+    }
+    return closestPosition;
+  }
 
-  //  public static PathPlannerPath makePath(){
-  ///    List<Translation2d> fgishf = PathPlannerPath.bezierFromPoses();
-  ///  }
+  public static PathPlannerPath makePath() {
+    List<Translation2d> fgishf = PathPlannerPath.bezierFromPoses();
+    return new PathPlannerPath(fgishf, null, null, null, null, null, false, null);
+  }
 
   public static Note getClosestNote(Pose2d curPose) {
     Note closestNote = new Note();

From 4cca5e2bdec5cef23c013df9d8bd3229f9d3b8e1 Mon Sep 17 00:00:00 2001
From: Lewis-Seiden <lewy@seiden.us>
Date: Sun, 18 Feb 2024 12:27:20 -0800
Subject: [PATCH 24/62] added swerve offsets and put cancoders and gyro on
 canivore

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

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 95a77191..febcfd46 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -104,6 +104,7 @@ public void robotInit() {
     // Metadata about the current code running on the robot
     Logger.recordMetadata("Codebase", "Comp2024");
     Logger.recordMetadata("RuntimeType", getRuntimeType().toString());
+    Logger.recordMetadata("Robot Mode", mode.toString());
     Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA);
     Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE);
     Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH);
@@ -155,7 +156,7 @@ public void robotInit() {
     elevator.setDefaultCommand(elevator.setExtensionCmd(() -> 0.0));
     feeder.setDefaultCommand(feeder.runVoltageCmd(0.0));
     carriage.setDefaultCommand(carriage.runVoltageCmd(0.0));
-    intake.setDefaultCommand(intake.runVoltageCmd(10.0));
+    intake.setDefaultCommand(intake.runVoltageCmd(0.0));
     shooter.setDefaultCommand(
         shooter.runStateCmd(
             () -> Rotation2d.fromDegrees(0.0), () -> flywheelIdleSpeed, () -> flywheelIdleSpeed));
diff --git a/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java
index 1d91209e..27b8d877 100644
--- a/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java
+++ b/src/main/java/frc/robot/subsystems/swerve/GyroIOPigeon2.java
@@ -24,7 +24,7 @@
 
 /** IO implementation for Pigeon2 */
 public class GyroIOPigeon2 implements GyroIO {
-  private final Pigeon2 pigeon = new Pigeon2(SwerveSubsystem.PIGEON_ID);
+  private final Pigeon2 pigeon = new Pigeon2(SwerveSubsystem.PIGEON_ID, "canivore");
   private final StatusSignal<Double> yaw = pigeon.getYaw();
   private final Queue<Double> yawPositionQueue;
   private final StatusSignal<Double> yawVelocity = pigeon.getAngularVelocityZWorld();
diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java
index 2cd9b928..95a991e6 100644
--- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java
+++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java
@@ -25,6 +25,7 @@
 import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
 import com.ctre.phoenix6.signals.InvertedValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.ctre.phoenix6.signals.SensorDirectionValue;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 import frc.robot.subsystems.swerve.Module.ModuleConstants;
@@ -79,7 +80,7 @@ public ModuleIOReal(ModuleConstants constants) {
 
     driveTalon = new TalonFX(constants.driveID(), "canivore");
     turnTalon = new TalonFX(constants.turnID(), "canivore");
-    cancoder = new CANcoder(constants.cancoderID());
+    cancoder = new CANcoder(constants.cancoderID(), "canivore");
 
     var driveConfig = new TalonFXConfiguration();
     // Current limits
@@ -101,6 +102,8 @@ public ModuleIOReal(ModuleConstants constants) {
     driveConfig.Slot0.kS = 0.0;
     driveConfig.Slot0.kP = 0.25; // Guess
     driveConfig.Slot0.kD = 0.0;
+    driveConfig.MotionMagic.MotionMagicCruiseVelocity = 40.0;
+    driveConfig.MotionMagic.MotionMagicAcceleration = 100.0;
 
     driveTalon.getConfigurator().apply(driveConfig);
 
@@ -135,12 +138,15 @@ public ModuleIOReal(ModuleConstants constants) {
     turnConfig.Slot0.kS = 0.0; // TODO: Find empirically
     turnConfig.Slot0.kP = 50.0;
     turnConfig.Slot0.kD = 0.0;
+    turnConfig.MotionMagic.MotionMagicCruiseVelocity = 40.0;
+    turnConfig.MotionMagic.MotionMagicAcceleration = 100.0;
     turnConfig.ClosedLoopGeneral.ContinuousWrap = true;
 
     turnTalon.getConfigurator().apply(turnConfig);
 
     var cancoderConfig = new CANcoderConfiguration();
     cancoderConfig.MagnetSensor.MagnetOffset = constants.cancoderOffset().getRotations();
+    cancoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive;
     cancoder.getConfigurator().apply(cancoderConfig);
 
     drivePosition = driveTalon.getPosition();
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
index bfac133a..aa464130 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
@@ -90,13 +90,13 @@ public class AutoAimStates {
   public static final int PIGEON_ID = 0;
 
   public static final ModuleConstants frontLeft =
-      new ModuleConstants("Front Left", 0, 1, 0, Rotation2d.fromRotations(0.0));
+      new ModuleConstants("Front Left", 0, 1, 0, Rotation2d.fromRotations(0.374023));
   public static final ModuleConstants frontRight =
-      new ModuleConstants("Front Right", 2, 3, 1, Rotation2d.fromRotations(0.0));
+      new ModuleConstants("Front Right", 2, 3, 1, Rotation2d.fromRotations(-0.077148));
   public static final ModuleConstants backLeft =
-      new ModuleConstants("Back Left", 4, 5, 2, Rotation2d.fromRotations(0.0));
+      new ModuleConstants("Back Left", 4, 5, 2, Rotation2d.fromRotations(0.134277));
   public static final ModuleConstants backRight =
-      new ModuleConstants("Back Right", 6, 7, 3, Rotation2d.fromRotations(0.0));
+      new ModuleConstants("Back Right", 6, 7, 3, Rotation2d.fromRotations(0.511963));
 
   public static final Lock odometryLock = new ReentrantLock();
   private final GyroIO gyroIO;

From 99e789c0e3b9539b3ea7c0917bb6b545f239dd2e Mon Sep 17 00:00:00 2001
From: Lewis-Seiden <lewy@seiden.us>
Date: Sun, 18 Feb 2024 14:28:03 -0800
Subject: [PATCH 25/62] run swerve sysids, fix heading reset

---
 src/main/java/frc/robot/Robot.java            |  4 ++
 .../frc/robot/subsystems/swerve/Module.java   |  4 +-
 .../robot/subsystems/swerve/ModuleIOReal.java | 44 ++++++++-----------
 .../subsystems/swerve/SwerveSubsystem.java    |  8 ++--
 4 files changed, 29 insertions(+), 31 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index febcfd46..abf66ecc 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -5,6 +5,8 @@
 package frc.robot;
 
 import com.choreo.lib.Choreo;
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.SignalLogger;
 import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.commands.PathPlannerAuto;
 import edu.wpi.first.math.MathUtil;
@@ -143,6 +145,7 @@ public void robotInit() {
 
     Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may
     // be added.
+    SignalLogger.setPath("/media/sda1/");
 
     // Default Commands here
     swerve.setDefaultCommand(
@@ -296,6 +299,7 @@ public void robotPeriodic() {
         new Pose3d[] {
           shooter.getMechanismPose(), elevator.getCarriagePose(), elevator.getFirstStagePose()
         });
+    Logger.recordOutput("Canivore Util", CANBus.getStatus("canivore").BusUtilization);
   }
 
   private LoggedDashboardNumber rotation = new LoggedDashboardNumber("Rotation (Rotations)");
diff --git a/src/main/java/frc/robot/subsystems/swerve/Module.java b/src/main/java/frc/robot/subsystems/swerve/Module.java
index 7be37170..8a21bbee 100644
--- a/src/main/java/frc/robot/subsystems/swerve/Module.java
+++ b/src/main/java/frc/robot/subsystems/swerve/Module.java
@@ -34,7 +34,7 @@ public record ModuleConstants(
   public static final double DRIVE_GEAR_RATIO = (50.0 / 16.0) * (16.0 / 28.0) * (45.0 / 15.0);
   public static final double TURN_GEAR_RATIO = 150.0 / 7.0;
 
-  public static final double DRIVE_STATOR_CURRENT_LIMIT = 50.0; // TODO bump as needed
+  public static final double DRIVE_STATOR_CURRENT_LIMIT = 80.0; // TODO bump as needed
   public static final double TURN_STATOR_CURRENT_LIMIT = 40.0;
 
   private final ModuleIO io;
@@ -73,7 +73,7 @@ public void periodic() {
   /** Runs the module closed loop with the specified setpoint state. Returns the optimized state. */
   public SwerveModuleState runSetpoint(SwerveModuleState state) {
     // Optimize state based on current angle
-    final var optimizedState = SwerveModuleState.optimize(state, getAngle());
+    final var optimizedState = state; // SwerveModuleState.optimize(state, getAngle());
 
     io.setTurnSetpoint(optimizedState.angle);
     io.setDriveSetpoint(optimizedState.speedMetersPerSecond);
diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java
index 95a991e6..038b1293 100644
--- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java
+++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java
@@ -45,7 +45,7 @@
  */
 public class ModuleIOReal implements ModuleIO {
   // Constants
-  private static final boolean IS_TURN_MOTOR_INVERTED = false;
+  private static final boolean IS_TURN_MOTOR_INVERTED = true;
 
   private final String name;
 
@@ -95,15 +95,14 @@ public ModuleIOReal(ModuleConstants constants) {
     driveConfig.Feedback.SensorToMechanismRatio =
         (Module.DRIVE_GEAR_RATIO) * (1.0 / (Module.WHEEL_RADIUS * 2 * Math.PI));
     // Controls Gains
-    driveConfig.Slot0.kV = 2.5;
-    // (5800.0 * (Module.DRIVE_GEAR_RATIO) * (1.0 / (Module.WHEEL_RADIUS * 2 * Math.PI)))
-    //     / 12.0; // Hypothetical based on free speed
-    driveConfig.Slot0.kA = 0.0; // TODO: Find using sysid or hand tuning
-    driveConfig.Slot0.kS = 0.0;
-    driveConfig.Slot0.kP = 0.25; // Guess
+    driveConfig.Slot0.kV = 2.0733;
+    driveConfig.Slot0.kA = 0.6;
+    driveConfig.Slot0.kS = 0.052218;
+    driveConfig.Slot0.kP = 1.9855;
     driveConfig.Slot0.kD = 0.0;
-    driveConfig.MotionMagic.MotionMagicCruiseVelocity = 40.0;
-    driveConfig.MotionMagic.MotionMagicAcceleration = 100.0;
+
+    driveConfig.MotionMagic.MotionMagicCruiseVelocity = SwerveSubsystem.MAX_LINEAR_SPEED;
+    driveConfig.MotionMagic.MotionMagicAcceleration = SwerveSubsystem.MAX_LINEAR_SPEED / 0.75;
 
     driveTalon.getConfigurator().apply(driveConfig);
 
@@ -125,28 +124,23 @@ public ModuleIOReal(ModuleConstants constants) {
     turnConfig.Feedback.FeedbackRotorOffset =
         0.0; // Is this correct? Cancoder config should handle it
     // Controls Gains
-    turnConfig.Slot0.kV = 0.0;
-    // (5800.0 / Module.TURN_GEAR_RATIO)
-    //     / 12.0; // Free speed over voltage, should find empirically
-    turnConfig.Slot0.kA = 0.0;
-    // Module.TURN_GEAR_RATIO
-    //     * (9.37 / 483.0)
-    //     / (0.004 * (12.0 / 483.0)); // Based on motor dynamics math, should find in real life
-    // gearing * Kt (torque per amp) / (intertia * resistance (nominal voltage / stall current))
-    // Yeah its messy and should be found using sysid later but its worth trying as a first guess
-    // If this works we can use a similar technique on future mechanisms
-    turnConfig.Slot0.kS = 0.0; // TODO: Find empirically
-    turnConfig.Slot0.kP = 50.0;
-    turnConfig.Slot0.kD = 0.0;
-    turnConfig.MotionMagic.MotionMagicCruiseVelocity = 40.0;
-    turnConfig.MotionMagic.MotionMagicAcceleration = 100.0;
+    turnConfig.Slot0.kV = 2.7935;
+    turnConfig.Slot0.kA = 0.031543;
+    turnConfig.Slot0.kS = 0.34822;
+    turnConfig.Slot0.kP = 28.579;
+    turnConfig.Slot0.kD = 0.68275;
+    turnConfig.MotionMagic.MotionMagicCruiseVelocity = 5500 / Module.TURN_GEAR_RATIO;
+    turnConfig.MotionMagic.MotionMagicAcceleration = (5500 * 0.1) / Module.TURN_GEAR_RATIO;
     turnConfig.ClosedLoopGeneral.ContinuousWrap = true;
 
     turnTalon.getConfigurator().apply(turnConfig);
 
     var cancoderConfig = new CANcoderConfiguration();
     cancoderConfig.MagnetSensor.MagnetOffset = constants.cancoderOffset().getRotations();
-    cancoderConfig.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive;
+    cancoderConfig.MagnetSensor.SensorDirection =
+        IS_TURN_MOTOR_INVERTED
+            ? SensorDirectionValue.CounterClockwise_Positive
+            : SensorDirectionValue.Clockwise_Positive;
     cancoder.getConfigurator().apply(cancoderConfig);
 
     drivePosition = driveTalon.getPosition();
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
index aa464130..33da39e1 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
@@ -80,7 +80,7 @@ public class AutoAimStates {
   }
 
   // Drivebase constants
-  public static final double MAX_LINEAR_SPEED = Units.feetToMeters(12.5);
+  public static final double MAX_LINEAR_SPEED = Units.feetToMeters(18.9);
   public static final double TRACK_WIDTH_X = Units.inchesToMeters(21.75);
   public static final double TRACK_WIDTH_Y = Units.inchesToMeters(21.25);
   public static final double DRIVE_BASE_RADIUS =
@@ -171,8 +171,8 @@ public SwerveSubsystem(GyroIO gyroIO, ModuleIO... moduleIOs) {
         new SysIdRoutine(
             new SysIdRoutine.Config(
                 null, // Default ramp rate is acceptable
-                Volts.of(4), // Reduce dynamic voltage to 4 to prevent motor brownout
-                Seconds.of(5),
+                Volts.of(3.5),
+                Seconds.of(3.5),
                 // Log state with Phoenix SignalLogger class
                 (state) -> SignalLogger.writeString("state", state.toString())),
             new SysIdRoutine.Mechanism(
@@ -307,7 +307,7 @@ public Command stopCmd() {
 
   public Command runVelocityFieldRelative(Supplier<ChassisSpeeds> speeds) {
     return this.runVelocityCmd(
-        () -> ChassisSpeeds.fromFieldRelativeSpeeds(speeds.get(), getRotation()));
+        () -> ChassisSpeeds.fromFieldRelativeSpeeds(speeds.get(), gyroInputs.yawPosition));
   }
 
   /**

From b2b46904facd9c9154c106913fa15731be03c7ef Mon Sep 17 00:00:00 2001
From: Lewis-Seiden <lewy@seiden.us>
Date: Sun, 18 Feb 2024 15:19:11 -0800
Subject: [PATCH 26/62] tuned carriage and shooter handoffs

---
 src/main/java/frc/robot/Robot.java            | 43 +++++++++++++------
 .../subsystems/carriage/CarriageIOReal.java   |  6 +--
 .../carriage/CarriageSubsystem.java           |  5 ++-
 .../robot/subsystems/feeder/FeederIOReal.java |  5 ++-
 .../subsystems/feeder/FeederSubsystem.java    |  6 ++-
 .../robot/subsystems/intake/IntakeIOReal.java |  6 ++-
 .../frc/robot/subsystems/swerve/Module.java   |  2 +-
 .../components/InvertedDigitalInput.java      |  6 +--
 8 files changed, 51 insertions(+), 28 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index abf66ecc..22d554e6 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -5,7 +5,6 @@
 package frc.robot;
 
 import com.choreo.lib.Choreo;
-import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.SignalLogger;
 import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.commands.PathPlannerAuto;
@@ -157,8 +156,13 @@ public void robotInit() {
                     -teleopAxisAdjustment(controller.getRightX())
                         * SwerveSubsystem.MAX_ANGULAR_SPEED)));
     elevator.setDefaultCommand(elevator.setExtensionCmd(() -> 0.0));
-    feeder.setDefaultCommand(feeder.runVoltageCmd(0.0));
-    carriage.setDefaultCommand(carriage.runVoltageCmd(0.0));
+    feeder.setDefaultCommand(
+        Commands.either(
+            feeder.indexCmd(), feeder.runVoltageCmd(0.0), () -> currentTarget == Target.SPEAKER));
+    carriage.setDefaultCommand(
+        carriage
+            .runVoltageCmd(CarriageSubsystem.INDEXING_VOLTAGE)
+            .withInterruptBehavior(InterruptionBehavior.kCancelSelf));
     intake.setDefaultCommand(intake.runVoltageCmd(0.0));
     shooter.setDefaultCommand(
         shooter.runStateCmd(
@@ -177,23 +181,28 @@ public void robotInit() {
         .debounce(0.25)
         .whileTrue(
             Commands.parallel(
-                intake.runVoltageCmd(0.0).withInterruptBehavior(InterruptionBehavior.kCancelSelf),
-                controller.rumbleCmd(1.0, 1.0).withTimeout(0.25),
-                leds.setBlinkingCmd(new Color("#ff8000"), new Color("#000000"), 25.0)));
-    new Trigger(() -> currentTarget == Target.SPEAKER)
-        .whileTrue(Commands.parallel(carriage.runVoltageCmd(5.0), feeder.indexCmd()));
+                    controller.rumbleCmd(1.0, 1.0),
+                    leds.setBlinkingCmd(new Color("#ff8000"), new Color("#000000"), 25.0))
+                .withTimeout(0.5));
+    // Handled by default commands
+    // new Trigger(() -> currentTarget == Target.SPEAKER)
+    //     .whileTrue(feeder.indexCmd())
+    //     .whileTrue(carriage.runVoltageCmd(5.0));
     new Trigger(() -> currentTarget == Target.AMP)
         .whileTrue(
             Commands.either(
-                Commands.parallel(carriage.indexBackwardsCmd(), feeder.runVoltageCmd(-5.0)),
+                Commands.parallel(
+                    carriage.indexBackwardsCmd(),
+                    feeder.runVoltageCmd(-FeederSubsystem.INDEXING_VOLTAGE).withTimeout(1.0)),
                 Commands.parallel(carriage.indexForwardsCmd(), feeder.runVoltageCmd(0.0)),
                 () -> feeder.getFirstBeambreak()));
 
     // ---- Controller bindings here ----
-    controller.leftTrigger().whileTrue(intake.runVoltageCmd(10.0));
+    controller.leftTrigger().whileTrue(intake.runVoltageCmd(8.0));
     controller
         .rightTrigger()
         .and(() -> currentTarget == Target.SPEAKER)
+        .and(() -> false)
         .whileTrue(
             Commands.parallel(
                 teleopAutoAim(
@@ -216,7 +225,17 @@ public void robotInit() {
                       Logger.recordOutput("AutoAim/Polar Speeds", polarSpeeds);
                       return polarSpeeds;
                     }),
-                Commands.waitSeconds(0.5).andThen(feeder.runVoltageCmd(3.0))));
+                Commands.waitSeconds(0.5)
+                    .andThen(feeder.runVoltageCmd(FeederSubsystem.INDEXING_VOLTAGE))));
+    controller
+        .rightTrigger()
+        .and(() -> currentTarget == Target.SPEAKER)
+        .and(() -> true)
+        .whileTrue(
+            Commands.parallel(
+                shooter.runStateCmd(Rotation2d.fromDegrees(75.0), 80.0, 60.0),
+                Commands.waitSeconds(1.0)
+                    .andThen(feeder.runVoltageCmd(FeederSubsystem.INDEXING_VOLTAGE))));
     controller
         .rightTrigger()
         .and(() -> currentTarget == Target.AMP)
@@ -299,7 +318,7 @@ public void robotPeriodic() {
         new Pose3d[] {
           shooter.getMechanismPose(), elevator.getCarriagePose(), elevator.getFirstStagePose()
         });
-    Logger.recordOutput("Canivore Util", CANBus.getStatus("canivore").BusUtilization);
+    // Logger.recordOutput("Canivore Util", CANBus.getStatus("canivore").BusUtilization);
   }
 
   private LoggedDashboardNumber rotation = new LoggedDashboardNumber("Rotation (Rotations)");
diff --git a/src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java b/src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java
index bc9af0b1..0a5fc5c4 100644
--- a/src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java
+++ b/src/main/java/frc/robot/subsystems/carriage/CarriageIOReal.java
@@ -10,13 +10,13 @@
 import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.InvertedValue;
-import edu.wpi.first.wpilibj.DigitalInput;
+import frc.robot.utils.components.InvertedDigitalInput;
 
 /** Create a CarriageIO that uses a real TalonFX. */
 public class CarriageIOReal implements CarriageIO {
   final TalonFX motor = new TalonFX(18, "canivore");
 
-  final DigitalInput beambreak = new DigitalInput(2);
+  final InvertedDigitalInput beambreak = new InvertedDigitalInput(2);
 
   final VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true);
 
@@ -28,7 +28,7 @@ public class CarriageIOReal implements CarriageIO {
   public CarriageIOReal() {
     var config = new TalonFXConfiguration();
 
-    config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+    config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
 
     motor.getConfigurator().apply(config);
     BaseStatusSignal.setUpdateFrequencyForAll(50.0, velocity, voltage, amperage, temp);
diff --git a/src/main/java/frc/robot/subsystems/carriage/CarriageSubsystem.java b/src/main/java/frc/robot/subsystems/carriage/CarriageSubsystem.java
index 1b031de4..b0b1c3db 100644
--- a/src/main/java/frc/robot/subsystems/carriage/CarriageSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/carriage/CarriageSubsystem.java
@@ -24,7 +24,7 @@ public CarriageSubsystem(CarriageIO io) {
   @Override
   public void periodic() {
     io.updateInputs(inputs);
-    Logger.processInputs("Intake", inputs);
+    Logger.processInputs("Carriage", inputs);
   }
 
   /** Run the carriage roller at the specified voltage */
@@ -39,7 +39,7 @@ public Command runVoltageCmd(double voltage) {
   public Command indexForwardsCmd() {
     return runVoltageCmd(INDEXING_VOLTAGE)
         .until(() -> inputs.beambreak)
-        .andThen(runVoltageCmd(INDEXING_VOLTAGE).withTimeout(0.25));
+        .andThen(runVoltageCmd(INDEXING_VOLTAGE / 2).withTimeout(0.12), runVoltageCmd(0.0));
   }
 
   /** Run the amp mech backwards until the beambreak cycles, then forward index. */
@@ -48,6 +48,7 @@ public Command indexBackwardsCmd() {
         runVoltageCmd(-INDEXING_VOLTAGE).until(() -> inputs.beambreak),
         runVoltageCmd(-INDEXING_VOLTAGE).withTimeout(0.25),
         runVoltageCmd(-INDEXING_VOLTAGE).until(() -> !inputs.beambreak),
+        runVoltageCmd(-INDEXING_VOLTAGE).withTimeout(0.25),
         indexForwardsCmd());
   }
 
diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java
index a6d41133..8170b741 100644
--- a/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java
+++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOReal.java
@@ -11,12 +11,13 @@
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.InvertedValue;
 import edu.wpi.first.wpilibj.DigitalInput;
+import frc.robot.utils.components.InvertedDigitalInput;
 
 /** Feeder IO using a TalonFX. */
 public class FeederIOReal implements FeederIO {
   private final TalonFX motor = new TalonFX(13, "canivore");
 
-  DigitalInput firstBeambreak = new DigitalInput(0);
+  InvertedDigitalInput firstBeambreak = new InvertedDigitalInput(0);
   DigitalInput lastBeambreak = new DigitalInput(1);
 
   private final StatusSignal<Double> velocity = motor.getVelocity();
@@ -32,7 +33,7 @@ public FeederIOReal() {
     config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
 
     config.CurrentLimits.StatorCurrentLimitEnable = true;
-    config.CurrentLimits.StatorCurrentLimit = 20.0;
+    config.CurrentLimits.StatorCurrentLimit = 40.0;
 
     motor.getConfigurator().apply(config);
 
diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederSubsystem.java b/src/main/java/frc/robot/subsystems/feeder/FeederSubsystem.java
index ea77b741..89b8a99e 100644
--- a/src/main/java/frc/robot/subsystems/feeder/FeederSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/feeder/FeederSubsystem.java
@@ -10,6 +10,8 @@
 
 /** Feeder motor for shooter and associated beambreaks for indexing */
 public class FeederSubsystem extends SubsystemBase {
+  public static final double INDEXING_VOLTAGE = 4.0;
+
   private final FeederIO io;
   private final FeederIOInputsAutoLogged inputs = new FeederIOInputsAutoLogged();
 
@@ -34,11 +36,11 @@ public Command indexCmd() {
     return this.run(
         () -> {
           if (inputs.lastBeambreak) {
-            io.setVoltage(-3.0);
+            io.setVoltage(-INDEXING_VOLTAGE);
           } else if (inputs.firstBeambreak) {
             io.setVoltage(0.0);
           } else {
-            io.setVoltage(3.0);
+            io.setVoltage(INDEXING_VOLTAGE);
           }
         });
   }
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java
index 16bfbaf1..f9b98b5e 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java
@@ -31,11 +31,15 @@ public class IntakeIOReal implements IntakeIO {
 
   public IntakeIOReal() {
     var intakeConfig = new TalonFXConfiguration();
+    intakeConfig.CurrentLimits.SupplyCurrentLimit = 20.0;
+    intakeConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
     intakeConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
     intakeMotor.getConfigurator().apply(intakeConfig);
 
     var centeringConfig = new TalonFXConfiguration();
-    centeringConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+    centeringConfig.CurrentLimits.SupplyCurrentLimit = 20.0;
+    centeringConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
+    centeringConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
     centeringMotor.getConfigurator().apply(centeringConfig);
 
     BaseStatusSignal.setUpdateFrequencyForAll(
diff --git a/src/main/java/frc/robot/subsystems/swerve/Module.java b/src/main/java/frc/robot/subsystems/swerve/Module.java
index 8a21bbee..186fc8a4 100644
--- a/src/main/java/frc/robot/subsystems/swerve/Module.java
+++ b/src/main/java/frc/robot/subsystems/swerve/Module.java
@@ -35,7 +35,7 @@ public record ModuleConstants(
   public static final double TURN_GEAR_RATIO = 150.0 / 7.0;
 
   public static final double DRIVE_STATOR_CURRENT_LIMIT = 80.0; // TODO bump as needed
-  public static final double TURN_STATOR_CURRENT_LIMIT = 40.0;
+  public static final double TURN_STATOR_CURRENT_LIMIT = 20.0;
 
   private final ModuleIO io;
   private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged();
diff --git a/src/main/java/frc/robot/utils/components/InvertedDigitalInput.java b/src/main/java/frc/robot/utils/components/InvertedDigitalInput.java
index 1e3e8ff3..72c52ab3 100644
--- a/src/main/java/frc/robot/utils/components/InvertedDigitalInput.java
+++ b/src/main/java/frc/robot/utils/components/InvertedDigitalInput.java
@@ -4,14 +4,10 @@
 
 /** A digital input (ie a beambreak or limit switch) whose normal state is reversed */
 public class InvertedDigitalInput extends DigitalInput {
-  private InvertedDigitalInput(int channel) {
+  public InvertedDigitalInput(int channel) {
     super(channel);
   }
 
-  public static InvertedDigitalInput from(final DigitalInput input) {
-    return new InvertedDigitalInput(input.getChannel());
-  }
-
   public boolean get() {
     return !super.get();
   }

From 8db0105e92afb07396ea3ad2cb441e43a9796d3f Mon Sep 17 00:00:00 2001
From: Stoopipoopy <anotherguywithabeanie@gmail.com>
Date: Sun, 18 Feb 2024 15:19:22 -0800
Subject: [PATCH 27/62] made shootingLocation class and can create paths using
 choreo

---
 .vscode/launch.json                           |     1 +
 ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat |   Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat |   Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat |   Bin 2048 -> 2048 bytes
 src/main/java/frc/robot/Robot.java            |     9 +-
 .../subsystems/swerve/SwerveSubsystem.java    |    21 +
 .../robot/utils/dynamicauto/ChoreoBuilder.py  |   117 +
 .../robot/utils/dynamicauto/DynamicAuto.java  |    61 +-
 .../frc/robot/utils/dynamicauto/Note.java     |     9 +-
 .../utils/dynamicauto/ShootingLocation.java   |    31 +
 .../frc/robot/utils/dynamicauto/data.chor     | 26859 ++++++++++++++++
 .../deploy/choreo/Amp Side To W1.1.traj       |   121 +
 .../deploy/choreo/Amp Side To W1.traj         |   121 +
 .../dynamicauto/deploy/choreo/New Path.1.traj |   265 +
 .../dynamicauto/deploy/choreo/New Path.traj   |   265 +
 .../deploy/choreo/S1 To C1 To S1.1.traj       |   247 +
 .../deploy/choreo/S1 To C1 To S1.traj         |   247 +
 .../deploy/choreo/S1 To C1 To S2.1.traj       |   265 +
 .../deploy/choreo/S1 To C1 To S2.traj         |   265 +
 .../deploy/choreo/S1 To C2 To S1.1.traj       |   247 +
 .../deploy/choreo/S1 To C2 To S1.traj         |   247 +
 .../deploy/choreo/S1 To C2 To S2.1.traj       |   256 +
 .../deploy/choreo/S1 To C2 To S2.traj         |   256 +
 .../deploy/choreo/S1 To C3 To S1.1.traj       |   301 +
 .../deploy/choreo/S1 To C3 To S1.traj         |   301 +
 .../deploy/choreo/S1 To C3 To S2.1.traj       |   292 +
 .../deploy/choreo/S1 To C3 To S2.traj         |   292 +
 .../deploy/choreo/S1 To C3 To S3.1.traj       |   310 +
 .../deploy/choreo/S1 To C3 To S3.traj         |   310 +
 .../deploy/choreo/S1 To C3 To S4.1.traj       |   319 +
 .../deploy/choreo/S1 To C3 To S4.traj         |   319 +
 .../dynamicauto/deploy/choreo/S1 To W1.1.traj |   130 +
 .../dynamicauto/deploy/choreo/S1 To W1.traj   |   130 +
 .../dynamicauto/deploy/choreo/S1 To W2.1.traj |   148 +
 .../dynamicauto/deploy/choreo/S1 To W2.traj   |   148 +
 .../deploy/choreo/S2 To C1 To S1.1.traj       |   265 +
 .../deploy/choreo/S2 To C1 To S1.traj         |   265 +
 .../deploy/choreo/S2 To C1 To S2.1.traj       |   283 +
 .../deploy/choreo/S2 To C1 To S2.traj         |   283 +
 .../deploy/choreo/S2 To C2 To S1.1.traj       |   256 +
 .../deploy/choreo/S2 To C2 To S1.traj         |   256 +
 .../deploy/choreo/S2 To C2 To S2.1.traj       |   265 +
 .../deploy/choreo/S2 To C2 To S2.traj         |   265 +
 .../deploy/choreo/S2 To C3 To S1.1.traj       |   292 +
 .../deploy/choreo/S2 To C3 To S1.traj         |   292 +
 .../deploy/choreo/S2 To C3 To S2.1.traj       |   283 +
 .../deploy/choreo/S2 To C3 To S2.traj         |   283 +
 .../deploy/choreo/S2 To C3 To S3.1.traj       |   301 +
 .../deploy/choreo/S2 To C3 To S3.traj         |   301 +
 .../deploy/choreo/S2 To C3 To S4.1.traj       |   310 +
 .../deploy/choreo/S2 To C3 To S4.traj         |   310 +
 .../dynamicauto/deploy/choreo/S2 To W1.1.traj |   130 +
 .../dynamicauto/deploy/choreo/S2 To W1.traj   |   130 +
 .../dynamicauto/deploy/choreo/S2 To W2.1.traj |   103 +
 .../dynamicauto/deploy/choreo/S2 To W2.traj   |   103 +
 .../dynamicauto/deploy/choreo/S2 To W3.1.traj |   130 +
 .../dynamicauto/deploy/choreo/S2 To W3.traj   |   130 +
 .../deploy/choreo/S3 To C3 To S1.1.traj       |   310 +
 .../deploy/choreo/S3 To C3 To S1.traj         |   310 +
 .../deploy/choreo/S3 To C3 To S2.1.traj       |   301 +
 .../deploy/choreo/S3 To C3 To S2.traj         |   301 +
 .../deploy/choreo/S3 To C3 To S3.1.traj       |   319 +
 .../deploy/choreo/S3 To C3 To S3.traj         |   319 +
 .../deploy/choreo/S3 To C3 To S4.1.traj       |   328 +
 .../deploy/choreo/S3 To C3 To S4.traj         |   328 +
 .../deploy/choreo/S3 To C4 To S3.1.traj       |   319 +
 .../deploy/choreo/S3 To C4 To S3.traj         |   319 +
 .../deploy/choreo/S3 To C4 To S4.1.traj       |   301 +
 .../deploy/choreo/S3 To C4 To S4.traj         |   301 +
 .../deploy/choreo/S3 To C5 To S3.1.traj       |   355 +
 .../deploy/choreo/S3 To C5 To S3.traj         |   355 +
 .../deploy/choreo/S3 To C5 To S4.1.traj       |   319 +
 .../deploy/choreo/S3 To C5 To S4.traj         |   319 +
 .../dynamicauto/deploy/choreo/S3 To W2.1.traj |   157 +
 .../dynamicauto/deploy/choreo/S3 To W2.traj   |   157 +
 .../dynamicauto/deploy/choreo/S3 To W3.1.traj |   121 +
 .../dynamicauto/deploy/choreo/S3 To W3.traj   |   121 +
 .../deploy/choreo/S4 To C3 To S1.1.traj       |   319 +
 .../deploy/choreo/S4 To C3 To S1.traj         |   319 +
 .../deploy/choreo/S4 To C3 To S2.1.traj       |   310 +
 .../deploy/choreo/S4 To C3 To S2.traj         |   310 +
 .../deploy/choreo/S4 To C3 To S3.1.traj       |   328 +
 .../deploy/choreo/S4 To C3 To S3.traj         |   328 +
 .../deploy/choreo/S4 To C3 To S4.1.traj       |   337 +
 .../deploy/choreo/S4 To C3 To S4.traj         |   337 +
 .../deploy/choreo/S4 To C4 To S3.1.traj       |   301 +
 .../deploy/choreo/S4 To C4 To S3.traj         |   301 +
 .../deploy/choreo/S4 To C4 To S4.1.traj       |   283 +
 .../deploy/choreo/S4 To C4 To S4.traj         |   283 +
 .../deploy/choreo/S4 To C5 To S3.1.traj       |   319 +
 .../deploy/choreo/S4 To C5 To S3.traj         |   319 +
 .../deploy/choreo/S4 To C5 To S4.1.traj       |   283 +
 .../deploy/choreo/S4 To C5 To S4.traj         |   283 +
 .../dynamicauto/deploy/choreo/S4 To W3.1.traj |   166 +
 .../dynamicauto/deploy/choreo/S4 To W3.traj   |   166 +
 .../deploy/choreo/W1 To C1 To S1.1.traj       |   319 +
 .../deploy/choreo/W1 To C1 To S1.traj         |   319 +
 .../deploy/choreo/W1 To C1 To S2.1.traj       |   337 +
 .../deploy/choreo/W1 To C1 To S2.traj         |   337 +
 .../deploy/choreo/W1 To C2 To S1.1.traj       |   319 +
 .../deploy/choreo/W1 To C2 To S1.traj         |   319 +
 .../deploy/choreo/W1 To C2 To S2.1.traj       |   328 +
 .../deploy/choreo/W1 To C2 To S2.traj         |   328 +
 .../deploy/choreo/W1 To C3 To S1.1.traj       |   364 +
 .../deploy/choreo/W1 To C3 To S1.traj         |   364 +
 .../deploy/choreo/W1 To C3 To S2.1.traj       |   355 +
 .../deploy/choreo/W1 To C3 To S2.traj         |   355 +
 .../deploy/choreo/W1 To C3 To S3.1.traj       |   373 +
 .../deploy/choreo/W1 To C3 To S3.traj         |   373 +
 .../deploy/choreo/W1 To C3 To S4.1.traj       |   382 +
 .../deploy/choreo/W1 To C3 To S4.traj         |   382 +
 .../dynamicauto/deploy/choreo/W1 To W2.1.traj |   103 +
 .../dynamicauto/deploy/choreo/W1 To W2.traj   |   103 +
 .../deploy/choreo/W2 To C1 To S1.1.traj       |   310 +
 .../deploy/choreo/W2 To C1 To S1.traj         |   310 +
 .../deploy/choreo/W2 To C1 To S2.1.traj       |   328 +
 .../deploy/choreo/W2 To C1 To S2.traj         |   328 +
 .../deploy/choreo/W2 To C2 To S1.1.traj       |   301 +
 .../deploy/choreo/W2 To C2 To S1.traj         |   301 +
 .../deploy/choreo/W2 To C2 To S2.1.traj       |   310 +
 .../deploy/choreo/W2 To C2 To S2.traj         |   310 +
 .../deploy/choreo/W2 To C3 To S1.1.traj       |   337 +
 .../deploy/choreo/W2 To C3 To S1.traj         |   337 +
 .../deploy/choreo/W2 To C3 To S2.1.traj       |   328 +
 .../deploy/choreo/W2 To C3 To S2.traj         |   328 +
 .../deploy/choreo/W2 To C3 To S3.1.traj       |   346 +
 .../deploy/choreo/W2 To C3 To S3.traj         |   346 +
 .../deploy/choreo/W2 To C3 To S4.1.traj       |   355 +
 .../deploy/choreo/W2 To C3 To S4.traj         |   355 +
 .../deploy/choreo/W2 To C4 To S3.1.traj       |   364 +
 .../deploy/choreo/W2 To C4 To S3.traj         |   364 +
 .../deploy/choreo/W2 To C4 To S4.1.traj       |   346 +
 .../deploy/choreo/W2 To C4 To S4.traj         |   346 +
 .../dynamicauto/deploy/choreo/W2 To W1.1.traj |   103 +
 .../dynamicauto/deploy/choreo/W2 To W1.traj   |   103 +
 .../dynamicauto/deploy/choreo/W2 To W3.1.traj |   103 +
 .../dynamicauto/deploy/choreo/W2 To W3.traj   |   103 +
 .../deploy/choreo/W3 To C1 To S1.1.traj       |   337 +
 .../deploy/choreo/W3 To C1 To S1.traj         |   337 +
 .../deploy/choreo/W3 To C1 To S2.1.traj       |   355 +
 .../deploy/choreo/W3 To C1 To S2.traj         |   355 +
 .../deploy/choreo/W3 To C2 To S1.1.traj       |   328 +
 .../deploy/choreo/W3 To C2 To S1.traj         |   328 +
 .../deploy/choreo/W3 To C2 To S2.1.traj       |   337 +
 .../deploy/choreo/W3 To C2 To S2.traj         |   337 +
 .../deploy/choreo/W3 To C3 To S1.1.traj       |   346 +
 .../deploy/choreo/W3 To C3 To S1.traj         |   346 +
 .../deploy/choreo/W3 To C3 To S2.1.traj       |   337 +
 .../deploy/choreo/W3 To C3 To S2.traj         |   337 +
 .../deploy/choreo/W3 To C3 To S3.1.traj       |   355 +
 .../deploy/choreo/W3 To C3 To S3.traj         |   355 +
 .../deploy/choreo/W3 To C3 To S4.1.traj       |   364 +
 .../deploy/choreo/W3 To C3 To S4.traj         |   364 +
 .../deploy/choreo/W3 To C4 To S3.1.traj       |   364 +
 .../deploy/choreo/W3 To C4 To S3.traj         |   364 +
 .../deploy/choreo/W3 To C4 To S4.1.traj       |   346 +
 .../deploy/choreo/W3 To C4 To S4.traj         |   346 +
 .../deploy/choreo/W3 To C5 To S3.1.traj       |   400 +
 .../deploy/choreo/W3 To C5 To S3.traj         |   400 +
 .../deploy/choreo/W3 To C5 To S4.1.traj       |   364 +
 .../deploy/choreo/W3 To C5 To S4.traj         |   364 +
 .../dynamicauto/deploy/choreo/W3 To W2.1.traj |   103 +
 .../dynamicauto/deploy/choreo/W3 To W2.traj   |   103 +
 163 files changed, 70558 insertions(+), 34 deletions(-)
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/ChoreoBuilder.py
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/ShootingLocation.java
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/data.chor
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.traj

diff --git a/.vscode/launch.json b/.vscode/launch.json
index c9c9713d..a11af841 100644
--- a/.vscode/launch.json
+++ b/.vscode/launch.json
@@ -4,6 +4,7 @@
   // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
   "version": "0.2.0",
   "configurations": [
+  
 
     {
       "type": "wpilib",
diff --git a/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat
index 8412ca3c645ee62940c463aae705fed7d2d207fe..58cf25bfb864839b250fb122dad2f781a946d8c6 100644
GIT binary patch
delta 30
jcmZn=Xb{-Yz{qx*LH@Fs&O|{gwo?pBmqkFVMNDh}kH!eT

delta 30
kcmZn=Xb{-Yz{qx#q43cunTdi{Y)2SM9-RQO7BR5_0ILEDyZ`_I

diff --git a/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat
index 7d12321016677911d1053a82d8606e787685b6ed..d1edf7c053d5cc7de88b0525f853807a5df621ab 100644
GIT binary patch
delta 30
jcmZn=Xb{-Yz{qx*LH@Fs&O|{gwo?pBmqkFVMNDh}kH!eT

delta 30
kcmZn=Xb{-Yz{qx#q43cunTdi{Y)2SM9-RQO7BR5_0ILEDyZ`_I

diff --git a/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat
index fab79fa0d3173c1b8d61b0657c419717a2bd3b02..141bbf23eafd1a07bb2e79e2d670a3b140dbea9b 100644
GIT binary patch
delta 36
mcmZn=Xb{-Yz{q)$LHx3S4g-V0L_u@TQw$=Pg`k{8Ol$zM2ME3Z

delta 36
ncmZn=Xb{-Yz{q)&A^Fh>83qP{iGt>wM;Ou`9fNWfF|h#v);0>f

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 4d990c25..5cf3fa12 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -139,6 +139,12 @@ public void robotInit() {
 
     // Test binding for elevator
     controller.b().whileTrue(elevator.setExtensionCmd(() -> 1.0));
+
+    controller
+        .x()
+        .toggleOnTrue(
+            Commands.run(
+                () -> swerve.runChoreoTraj(DynamicAuto.makePath(swerve::getPose)), swerve));
   }
 
   @Override
@@ -146,10 +152,9 @@ public void robotPeriodic() {
     CommandScheduler.getInstance().run();
     Logger.recordOutput(
         "DynamicAuto/Closest Note", DynamicAuto.getClosestNote(swerve.getPose()).getPose());
-
     Logger.recordOutput(
         "DynamicAuto/Closest Shooting Location",
-        DynamicAuto.closestShootingLocation(swerve.getPose()));
+        DynamicAuto.closestShootingLocation(() -> swerve.getPose()).getPose());
   }
 
   @Override
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
index a991ce5d..26e27c48 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
@@ -16,6 +16,8 @@
 import static edu.wpi.first.units.Units.Seconds;
 import static edu.wpi.first.units.Units.Volts;
 
+import com.choreo.lib.Choreo;
+import com.choreo.lib.ChoreoTrajectory;
 import com.ctre.phoenix6.SignalLogger;
 import com.google.common.collect.Streams;
 import com.pathplanner.lib.auto.AutoBuilder;
@@ -23,6 +25,7 @@
 import com.pathplanner.lib.util.PIDConstants;
 import com.pathplanner.lib.util.PathPlannerLogging;
 import com.pathplanner.lib.util.ReplanningConfig;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -39,6 +42,7 @@
 import edu.wpi.first.units.Measure;
 import edu.wpi.first.units.Voltage;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -48,6 +52,7 @@
 import frc.robot.subsystems.swerve.Module.ModuleConstants;
 import frc.robot.utils.autoaim.AutoAim;
 import java.util.Arrays;
+import java.util.Optional;
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
 import java.util.function.DoubleSupplier;
@@ -411,6 +416,22 @@ public Command pointTowardsTranslation(DoubleSupplier x, DoubleSupplier y) {
                         getVelocity().omegaRadiansPerSecond)));
   }
 
+  public Command runChoreoTraj(ChoreoTrajectory traj) {
+    return Choreo.choreoSwerveCommand(
+        traj,
+        this::getPose,
+        new PIDController(5.0, 0.0, 0.0),
+        new PIDController(5.0, 0.0, 0.0),
+        new PIDController(5.0, 0.0, 0.0),
+        (ChassisSpeeds speeds) -> //
+        this.runVelocity(speeds),
+        () -> {
+          Optional<Alliance> alliance = DriverStation.getAlliance();
+          return alliance.isPresent() && alliance.get() == Alliance.Red;
+        },
+        this);
+  }
+
   public Command runModuleSteerCharacterizationCmd() {
     return Commands.sequence(
         this.runOnce(() -> SignalLogger.start()),
diff --git a/src/main/java/frc/robot/utils/dynamicauto/ChoreoBuilder.py b/src/main/java/frc/robot/utils/dynamicauto/ChoreoBuilder.py
new file mode 100644
index 00000000..ff2c8a03
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/ChoreoBuilder.py
@@ -0,0 +1,117 @@
+import json
+
+# Assuming locations is a list of dictionaries where each dictionary represents a location
+# L,M, and R were change to S1, S2, and S4 with an S3 being added.
+locations = [
+    {"name": "Amp Side", "x": 0.71, "y": 6.72, "angle": -2.10, "allowed_destinations": ["W1", "W2", "C1", "C2", "C3"]},
+    {"name": "Center", "x": 1.35, "y": 5.56, "angle": 3.14, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]},
+    {"name": "Stage Side", "x": 0.71, "y": 4.36, "angle": 2.10, "allowed_destinations": ["W2", "W3", "C3", "C4", "C5"]},
+    {"name": "W1", "x": 2.3, "y": 6.757, "angle": -2.75, "allowed_destinations": ["W2", "C1", "C2", "C3"]},
+    {"name": "W2", "x": 2.25, "y": 5.56, "angle": 3.14, "allowed_destinations": ["W1", "W3", "C1", "C2", "C3", "C4"]},
+    {"name": "W3", "x": 2.3, "y": 4.36, "angle": 2.75, "allowed_destinations": ["W2", "C1", "C2", "C3", "C4", "C5"]},
+    {"name": "S1", "x": 5.176, "y": 6.63, "angle": -2.88, "allowed_destinations": ["W1", "W2", "C1", "C2", "C3"]},
+    {"name": "S2", "x": 4.263, "y": 5.56, "angle": 3.14, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3"]},
+    {"name": "S3", "x": 4.263, "y": 3, "angle": 2.574, "allowed_destinations": ["W2", "W3", "C3", "C4", "C5"]},
+    {"name": "S4", "x": 5.176, "y": 1.62, "angle": 2.5, "allowed_destinations": ["W3", "C3", "C4", "C5"]},
+]
+
+
+midPoints = [
+    {"name": "C1", "x": 7.68, "y": 7.467, "angle": 3.14, "allowed_destinations": ["S1", "S2"]},
+    {"name": "C2", "x": 7.68, "y": 5.797, "angle": 3.14, "allowed_destinations": ["S1", "S2"]},
+    {"name": "C3", "x": 7.68, "y": 4.127, "angle": 3.14, "allowed_destinations": ["S1", "S2", "S3", "S4"]},
+    {"name": "C4", "x": 7.68, "y": 2.457, "angle": 3.14, "allowed_destinations": ["S3", "S4"]},
+    {"name": "C5", "x": 7.68, "y": 0.787, "angle": 3.14, "allowed_destinations": ["S3", "S4"]},
+]
+
+def create_waypoint(point):
+    return {
+        "x": point["x"],
+        "y": point["y"],
+        "heading": point["angle"],
+        "isInitialGuess": False,
+        "translationConstrained": True,
+        "headingConstrained": True,
+        "controlIntervalCount": 40
+    }
+
+def create_path(allPoints):
+    return {
+        "waypoints": [create_waypoint(point) for point in allPoints],
+        "trajectory": [],
+        "constraints": [
+            {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+        ],
+        "usesControlIntervalGuessing": True,
+        "defaultControlIntervalCount": 40,
+        "usesDefaultFieldObstacles": True,
+        "circleObstacles": []
+    }
+
+
+paths = {}
+def find_path(start, end_name, newPath, path_name):
+    end = next((loc for loc in locations if loc["name"] == end_name), None)
+    if end:
+        newPath.append(end)
+        path_name += f" To {end['name']}"
+        paths[path_name] = create_path(newPath)
+        return
+    else:
+        midPoint = next((mid for mid in midPoints if mid["name"] == end_name), None)
+        if midPoint:
+            newPath.append(midPoint)
+            path_name += f" To {midPoint['name']}"
+            for end_name in midPoint["allowed_destinations"]:
+                find_path(midPoint, end_name, newPath.copy(), path_name)
+        else:
+            print("No midpoint found")
+
+for start in locations:
+    newPath = [start]
+    for end_name in start["allowed_destinations"]:
+        path_name = start['name']
+        find_path(start, end_name, newPath.copy(), path_name)
+
+# Wrap the paths in another dictionary with the specified format
+data = {
+    "version": "v0.2.1",
+    "robotConfiguration": {
+        "mass": 74.08797700309194,
+        "rotationalInertia": 6,
+        "motorMaxTorque": 1.162295081967213,
+        "motorMaxVelocity": 4800,
+        "gearing": 6.75,
+        "wheelbase": 0.5778496879611685,
+        "trackWidth": 0.5778496879611685,
+        "bumperLength": 0.8762995267982555,
+        "bumperWidth": 0.8762995267982555,
+        "wheelRadius": 0.050799972568014815
+    },
+    "paths": paths,
+    "splitTrajectoriesAtStopPoints": False
+}
+
+#Prints names of all paths
+#for i in paths:
+#print(i)
+print(paths["Amp Side To C3 To S4"])
+print(len(paths["Amp Side To C3 To S4"]['waypoints']))
+
+#Total Number of paths
+print(len(paths))
+
+# Write the data to a JSON file
+with open('C:\\Users\\Robotics\\Desktop\\Crescendo\\src\\main\\java\\frc\\robot\\utils\\dynamicauto\\data.chor', 'w') as f:
+    json.dump(data, f, indent=4)
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
index 4d457e88..4117eaf1 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
@@ -1,70 +1,69 @@
 package frc.robot.utils.dynamicauto;
 
-import com.pathplanner.lib.path.PathPlannerPath;
+import com.choreo.lib.Choreo;
+import com.choreo.lib.ChoreoTrajectory;
+import com.google.common.base.Supplier;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import java.util.List;
 
 public class DynamicAuto {
 
   public static final Note[] notes = {
-    new Note(new Pose2d(2.204, 7.0, Rotation2d.fromRadians(-Math.PI)), true, 0), // w1
-    new Note(new Pose2d(2.204, 5.5, Rotation2d.fromRadians(-Math.PI)), false, 1), // w2
-    new Note(new Pose2d(2.204, 4.1, Rotation2d.fromRadians(-Math.PI)), false, 2), // w3
-    new Note(new Pose2d(7.538, 7.3, Rotation2d.fromRadians(-Math.PI)), true, 0), // c1
-    new Note(new Pose2d(7.538, 5.7, Rotation2d.fromRadians(-Math.PI)), false, 0), // c2
-    new Note(new Pose2d(7.538, 4.1, Rotation2d.fromRadians(-Math.PI)), false, 1), // c3
-    new Note(new Pose2d(7.538, 2.5, Rotation2d.fromRadians(-Math.PI)), false, 2), // c4
-    new Note(new Pose2d(7.538, 0.7, Rotation2d.fromRadians(-Math.PI)), true, 3), // c5
+    new Note(new Pose2d(2.204, 7.0, Rotation2d.fromRadians(-Math.PI)), true, 0, "W1"), // w1
+    new Note(new Pose2d(2.204, 5.5, Rotation2d.fromRadians(-Math.PI)), false, 1, "W2"), // w2
+    new Note(new Pose2d(2.204, 4.1, Rotation2d.fromRadians(-Math.PI)), false, 2, "W3"), // w3
+    new Note(new Pose2d(7.538, 7.3, Rotation2d.fromRadians(-Math.PI)), true, 0, "C1"), // c1
+    new Note(new Pose2d(7.538, 5.7, Rotation2d.fromRadians(-Math.PI)), false, 0, "C2"), // c2
+    new Note(new Pose2d(7.538, 4.1, Rotation2d.fromRadians(-Math.PI)), false, 1, "C3"), // c3
+    new Note(new Pose2d(7.538, 2.5, Rotation2d.fromRadians(-Math.PI)), false, 2, "C4"), // c4
+    new Note(new Pose2d(7.538, 0.7, Rotation2d.fromRadians(-Math.PI)), true, 3, "C5") // c5
   };
 
-  public static final Pose2d[] shootingLocations = {
-    new Pose2d(5.639, 6.463, Rotation2d.fromRadians(0.1)), // Left
-    new Pose2d(4.216, 5.216, Rotation2d.fromRadians(0)), // Middle
-    new Pose2d(6.048, 1.746, Rotation2d.fromRadians(-0.179)) // Right
+  public static final ShootingLocation[] shootingLocations = {
+    new ShootingLocation(new Pose2d(5.639, 6.463, Rotation2d.fromRadians(0.1)), "S1"), // Left
+    new ShootingLocation(new Pose2d(4.216, 5.216, Rotation2d.fromRadians(0)), "S2"),
+    new ShootingLocation(new Pose2d(4.263, 3.000, Rotation2d.fromRadians(2.574)), "S3"), // Middle
+    new ShootingLocation(new Pose2d(5.176, 1.620, Rotation2d.fromRadians(-0.179)), "S4") // Right
   };
 
-  public static Pose2d closestShootingLocation(Pose2d curPose) {
-    Pose2d closestPosition = new Pose2d();
+  public static ShootingLocation closestShootingLocation(Supplier<Pose2d> curPose) {
+
+    ShootingLocation closestLocation = new ShootingLocation();
     double shortestDistance = 9999999;
-    for (Pose2d location : shootingLocations) {
-      if (shortestDistance > curPose.minus(location).getTranslation().getNorm()) {
-        closestPosition = location;
-        shortestDistance = curPose.minus(location).getTranslation().getNorm();
+    for (ShootingLocation location : shootingLocations) {
+      if (shortestDistance > curPose.get().minus(location.getPose()).getTranslation().getNorm()) {
+        closestLocation = location;
+        shortestDistance = curPose.get().minus(location.getPose()).getTranslation().getNorm();
       }
     }
-    return closestPosition;
+    return closestLocation;
   }
 
-  public static PathPlannerPath makePath() {
-    List<Translation2d> fgishf = PathPlannerPath.bezierFromPoses();
-    return new PathPlannerPath(fgishf, null, null, null, null, null, false, null);
+  public static ChoreoTrajectory makePath(Supplier<Pose2d> startingPose) {
+    return Choreo.getTrajectory(
+        getClosestNote(startingPose.get()).getName()
+            + " To "
+            + closestShootingLocation(startingPose).getName());
   }
 
   public static Note getClosestNote(Pose2d curPose) {
     Note closestNote = new Note();
     double shortestDistance = 99999999;
     for (Note note : notes) {
-      System.out.println(note.getPriority());
-      System.out.println(closestNote.getPriority());
       if (note.getBlacklist()) {
-        System.out.println("iojfeioeoi");
-        continue;
 
+        continue;
       } else if (note.getPriority() > closestNote.getPriority()) {
         shortestDistance = curPose.minus(note.getPose()).getTranslation().getNorm();
         closestNote = note;
         if (shortestDistance > curPose.minus(note.getPose()).getTranslation().getNorm()) {
           shortestDistance = curPose.minus(note.getPose()).getTranslation().getNorm();
           closestNote = note;
-          System.out.println("ghoghsghuhguhuihi");
         }
       } else if (note.getPriority() == closestNote.getPriority()) {
         if (shortestDistance > curPose.minus(note.getPose()).getTranslation().getNorm()) {
           shortestDistance = curPose.minus(note.getPose()).getTranslation().getNorm();
           closestNote = note;
-          System.out.println("ghoghsghuhguhuihi");
         }
       }
     }
diff --git a/src/main/java/frc/robot/utils/dynamicauto/Note.java b/src/main/java/frc/robot/utils/dynamicauto/Note.java
index 250db29f..b827ba29 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/Note.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/Note.java
@@ -12,17 +12,20 @@ public class Note {
   private boolean blacklisted;
   private int priority;
   private final Pose2d pose;
+  private final String name;
 
-  public Note(Pose2d pose, boolean blacklisted, int priority) {
+  public Note(Pose2d pose, boolean blacklisted, int priority, String name) {
     this.pose = pose;
     this.blacklisted = blacklisted;
     this.priority = priority;
+    this.name = name;
   }
 
   public Note() {
     this.pose = new Pose2d();
     this.blacklisted = true;
     this.priority = -1;
+    this.name = "Uninitialized";
   }
 
   public void setBlacklist(boolean blacklist) {
@@ -44,4 +47,8 @@ public int getPriority() {
   public Pose2d getPose() {
     return pose;
   }
+
+  public String getName() {
+    return name;
+  }
 }
diff --git a/src/main/java/frc/robot/utils/dynamicauto/ShootingLocation.java b/src/main/java/frc/robot/utils/dynamicauto/ShootingLocation.java
new file mode 100644
index 00000000..4d95a1ad
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/ShootingLocation.java
@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.utils.dynamicauto;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+/** Add your docs here. */
+public class ShootingLocation {
+  private Pose2d pose;
+  private String name;
+
+  public ShootingLocation(Pose2d pose, String name) {
+    this.pose = pose;
+    this.name = name;
+  }
+
+  public ShootingLocation() {
+    this.pose = new Pose2d();
+    this.name = "Uninitialized";
+  }
+
+  public Pose2d getPose() {
+    return pose;
+  }
+
+  public String getName() {
+    return name;
+  }
+}
diff --git a/src/main/java/frc/robot/utils/dynamicauto/data.chor b/src/main/java/frc/robot/utils/dynamicauto/data.chor
new file mode 100644
index 00000000..f162506e
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/data.chor
@@ -0,0 +1,26859 @@
+{
+    "version": "v0.2.2",
+    "robotConfiguration": {
+        "mass": 74.08797700309194,
+        "rotationalInertia": 6,
+        "motorMaxTorque": 1.162295081967213,
+        "motorMaxVelocity": 4800,
+        "gearing": 6.75,
+        "wheelbase": 0.5778496879611685,
+        "trackWidth": 0.5778496879611685,
+        "bumperLength": 0.8762995267982555,
+        "bumperWidth": 0.8762995267982555,
+        "wheelRadius": 0.050799972568014815
+    },
+    "paths": {
+        "Amp Side To W1": {
+            "waypoints": [
+                {
+                    "x": 0.71,
+                    "y": 6.72,
+                    "heading": -2.1,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 12
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 0.71,
+                    "y": 6.72,
+                    "heading": -2.1,
+                    "angularVelocity": 1.1712645267224225e-32,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 0.754119446225283,
+                    "y": 6.720987363814453,
+                    "heading": -2.1237071001932817,
+                    "angularVelocity": -0.324046398407224,
+                    "velocityX": 0.6030576296752822,
+                    "velocityY": 0.013496027999351609,
+                    "timestamp": 0.07315958550933178
+                },
+                {
+                    "x": 0.8424134953068435,
+                    "y": 6.722973003308925,
+                    "heading": -2.1697540075127364,
+                    "angularVelocity": -0.6294036112814133,
+                    "velocityX": 1.2068691814868098,
+                    "velocityY": 0.027141207548542687,
+                    "timestamp": 0.14631917101866357
+                },
+                {
+                    "x": 0.9749625172883624,
+                    "y": 6.725970405382453,
+                    "heading": -2.236002046597228,
+                    "angularVelocity": -0.9055278077813215,
+                    "velocityX": 1.8117792912401405,
+                    "velocityY": 0.04097073613339593,
+                    "timestamp": 0.21947875652799537
+                },
+                {
+                    "x": 1.15189309679064,
+                    "y": 6.729994604425149,
+                    "heading": -2.318545180808417,
+                    "angularVelocity": -1.128261370488901,
+                    "velocityX": 2.4184196543829377,
+                    "velocityY": 0.05500576602073325,
+                    "timestamp": 0.29263834203732714
+                },
+                {
+                    "x": 1.3734035805681073,
+                    "y": 6.735067453562316,
+                    "heading": -2.4074941689331384,
+                    "angularVelocity": -1.2158213787772831,
+                    "velocityX": 3.0277711694964378,
+                    "velocityY": 0.06933950078936708,
+                    "timestamp": 0.3657979275466589
+                },
+                {
+                    "x": 1.6366126990046324,
+                    "y": 6.741789664093089,
+                    "heading": -2.440634607899988,
+                    "angularVelocity": -0.45298833688286777,
+                    "velocityX": 3.5977393338693906,
+                    "velocityY": 0.09188420743425116,
+                    "timestamp": 0.4389575130559907
+                },
+                {
+                    "x": 1.8581204107797031,
+                    "y": 6.746837438064149,
+                    "heading": -2.53012067379602,
+                    "angularVelocity": -1.2231625599444977,
+                    "velocityX": 3.0277332796918652,
+                    "velocityY": 0.06899675464152406,
+                    "timestamp": 0.5121170985653225
+                },
+                {
+                    "x": 2.035048200585451,
+                    "y": 6.750889717758592,
+                    "heading": -2.613093680081622,
+                    "angularVelocity": -1.1341371839104541,
+                    "velocityX": 2.4183815227216985,
+                    "velocityY": 0.05538959339687604,
+                    "timestamp": 0.5852766840746543
+                },
+                {
+                    "x": 2.1675926710077147,
+                    "y": 6.75393921231849,
+                    "heading": -2.679769909140043,
+                    "angularVelocity": -0.9113806289937514,
+                    "velocityX": 1.8117170771198088,
+                    "velocityY": 0.04168277524633505,
+                    "timestamp": 0.658436269583986
+                },
+                {
+                    "x": 2.255882530144482,
+                    "y": 6.755978072079856,
+                    "heading": -2.726139004333996,
+                    "angularVelocity": -0.6338075164194407,
+                    "velocityX": 1.206811910183738,
+                    "velocityY": 0.027868662010218256,
+                    "timestamp": 0.7315958550933178
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "angularVelocity": -0.32614995697255006,
+                    "velocityX": 0.6030306151733326,
+                    "velocityY": 0.013968476079118136,
+                    "timestamp": 0.8047554406026496
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "angularVelocity": 2.3711039345986376e-33,
+                    "velocityX": -1.8060489353873473e-34,
+                    "velocityY": 7.872914025869492e-32,
+                    "timestamp": 0.8779150261119814
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Amp Side To W2": {
+            "waypoints": [
+                {
+                    "x": 0.71,
+                    "y": 6.72,
+                    "heading": -2.1,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Amp Side To C1 To S1": {
+            "waypoints": [
+                {
+                    "x": 0.71,
+                    "y": 6.72,
+                    "heading": -2.1,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Amp Side To C1 To S2": {
+            "waypoints": [
+                {
+                    "x": 0.71,
+                    "y": 6.72,
+                    "heading": -2.1,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Amp Side To C2 To S1": {
+            "waypoints": [
+                {
+                    "x": 0.71,
+                    "y": 6.72,
+                    "heading": -2.1,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Amp Side To C2 To S2": {
+            "waypoints": [
+                {
+                    "x": 0.71,
+                    "y": 6.72,
+                    "heading": -2.1,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Amp Side To C3 To S1": {
+            "waypoints": [
+                {
+                    "x": 0.71,
+                    "y": 6.72,
+                    "heading": -2.1,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Amp Side To C3 To S2": {
+            "waypoints": [
+                {
+                    "x": 0.71,
+                    "y": 6.72,
+                    "heading": -2.1,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Amp Side To C3 To S3": {
+            "waypoints": [
+                {
+                    "x": 0.71,
+                    "y": 6.72,
+                    "heading": -2.1,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Amp Side To C3 To S4": {
+            "waypoints": [
+                {
+                    "x": 0.71,
+                    "y": 6.72,
+                    "heading": -2.1,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Center To W1": {
+            "waypoints": [
+                {
+                    "x": 1.35,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Center To W2": {
+            "waypoints": [
+                {
+                    "x": 1.35,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Center To W3": {
+            "waypoints": [
+                {
+                    "x": 1.35,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Center To C1 To S1": {
+            "waypoints": [
+                {
+                    "x": 1.35,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Center To C1 To S2": {
+            "waypoints": [
+                {
+                    "x": 1.35,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Center To C2 To S1": {
+            "waypoints": [
+                {
+                    "x": 1.35,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Center To C2 To S2": {
+            "waypoints": [
+                {
+                    "x": 1.35,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Center To C3 To S1": {
+            "waypoints": [
+                {
+                    "x": 1.35,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Center To C3 To S2": {
+            "waypoints": [
+                {
+                    "x": 1.35,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Center To C3 To S3": {
+            "waypoints": [
+                {
+                    "x": 1.35,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Center To C3 To S4": {
+            "waypoints": [
+                {
+                    "x": 1.35,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Center To C4 To S3": {
+            "waypoints": [
+                {
+                    "x": 1.35,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Center To C4 To S4": {
+            "waypoints": [
+                {
+                    "x": 1.35,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Center To C5 To S3": {
+            "waypoints": [
+                {
+                    "x": 1.35,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Center To C5 To S4": {
+            "waypoints": [
+                {
+                    "x": 1.35,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Stage Side To W2": {
+            "waypoints": [
+                {
+                    "x": 0.71,
+                    "y": 4.36,
+                    "heading": 2.1,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Stage Side To W3": {
+            "waypoints": [
+                {
+                    "x": 0.71,
+                    "y": 4.36,
+                    "heading": 2.1,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Stage Side To C3 To S1": {
+            "waypoints": [
+                {
+                    "x": 0.71,
+                    "y": 4.36,
+                    "heading": 2.1,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Stage Side To C3 To S2": {
+            "waypoints": [
+                {
+                    "x": 0.71,
+                    "y": 4.36,
+                    "heading": 2.1,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Stage Side To C3 To S3": {
+            "waypoints": [
+                {
+                    "x": 0.71,
+                    "y": 4.36,
+                    "heading": 2.1,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Stage Side To C3 To S4": {
+            "waypoints": [
+                {
+                    "x": 0.71,
+                    "y": 4.36,
+                    "heading": 2.1,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Stage Side To C4 To S3": {
+            "waypoints": [
+                {
+                    "x": 0.71,
+                    "y": 4.36,
+                    "heading": 2.1,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Stage Side To C4 To S4": {
+            "waypoints": [
+                {
+                    "x": 0.71,
+                    "y": 4.36,
+                    "heading": 2.1,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Stage Side To C5 To S3": {
+            "waypoints": [
+                {
+                    "x": 0.71,
+                    "y": 4.36,
+                    "heading": 2.1,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "Stage Side To C5 To S4": {
+            "waypoints": [
+                {
+                    "x": 0.71,
+                    "y": 4.36,
+                    "heading": 2.1,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W1 To W2": {
+            "waypoints": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 10
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.2980013304879616,
+                    "y": 6.709118488011682,
+                    "heading": -2.76564785007466,
+                    "angularVelocity": -0.2059742190979395,
+                    "velocityX": -0.026308687136742944,
+                    "velocityY": -0.6302691420205845,
+                    "timestamp": 0.07596994489499287
+                },
+                {
+                    "x": 2.294004010549845,
+                    "y": 6.6133557271698225,
+                    "heading": -2.796958855018623,
+                    "angularVelocity": -0.4121498967419566,
+                    "velocityX": -0.05261712304308625,
+                    "velocityY": -1.2605348203717124,
+                    "timestamp": 0.15193988978998574
+                },
+                {
+                    "x": 2.2880079544710785,
+                    "y": 6.469712199120552,
+                    "heading": -2.8439591282551087,
+                    "angularVelocity": -0.618669307993449,
+                    "velocityX": -0.07892668721893806,
+                    "velocityY": -1.8907941587665607,
+                    "timestamp": 0.2279098346849786
+                },
+                {
+                    "x": 2.2800128266474267,
+                    "y": 6.278188582998367,
+                    "heading": -2.906683998413123,
+                    "angularVelocity": -0.8256537535299532,
+                    "velocityX": -0.10524066898696208,
+                    "velocityY": -2.5210445576459737,
+                    "timestamp": 0.3038797795799715
+                },
+                {
+                    "x": 2.27001785393274,
+                    "y": 6.038785706839934,
+                    "heading": -2.985174219418862,
+                    "angularVelocity": -1.0331746470822023,
+                    "velocityX": -0.13156482775526773,
+                    "velocityY": -3.151284056995696,
+                    "timestamp": 0.37984972447496435
+                },
+                {
+                    "x": 2.2620112641506833,
+                    "y": 5.847271276792759,
+                    "heading": -3.048371938278793,
+                    "angularVelocity": -0.8318779083923785,
+                    "velocityX": -0.10539154389440716,
+                    "velocityY": -2.5209236404197743,
+                    "timestamp": 0.4558196693699572
+                },
+                {
+                    "x": 2.256005772930322,
+                    "y": 5.703635612537123,
+                    "heading": -3.0957774424626403,
+                    "angularVelocity": -0.6240034035745593,
+                    "velocityX": -0.07905088293353696,
+                    "velocityY": -1.8906906468626767,
+                    "timestamp": 0.5317896142649501
+                },
+                {
+                    "x": 2.25200192771716,
+                    "y": 5.607878538669833,
+                    "heading": -3.127382702013982,
+                    "angularVelocity": -0.4160231996354264,
+                    "velocityX": -0.05270301589261432,
+                    "velocityY": -1.2604599621553931,
+                    "timestamp": 0.607759559159943
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -0.20801127587293633,
+                    "velocityX": -0.026351575217374957,
+                    "velocityY": -0.6302300039313168,
+                    "timestamp": 0.6837295040549358
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": 2.0416042206069368e-34,
+                    "velocityX": 0,
+                    "velocityY": 6.898965298259951e-35,
+                    "timestamp": 0.7596994489499287
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W1 To C1 To S1": {
+            "waypoints": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 21
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": -3.143185307179586,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.363517540312363,
+                    "y": 6.769205991625322,
+                    "heading": -2.7639736107792703,
+                    "angularVelocity": -0.15851225932569585,
+                    "velocityX": 0.7205230617028869,
+                    "velocityY": 0.13846094187126756,
+                    "timestamp": 0.08815476379374498
+                },
+                {
+                    "x": 2.4905727294218147,
+                    "y": 6.793631120419202,
+                    "heading": -2.790535805480005,
+                    "angularVelocity": -0.30131320824454183,
+                    "velocityX": 1.4412742277515702,
+                    "velocityY": 0.2770710026632852,
+                    "timestamp": 0.17630952758748997
+                },
+                {
+                    "x": 2.6811943332190395,
+                    "y": 6.830296876066563,
+                    "heading": -2.8273459830802556,
+                    "angularVelocity": -0.4175631130539097,
+                    "velocityX": 2.162351705044798,
+                    "velocityY": 0.4159248357030905,
+                    "timestamp": 0.26446429138123495
+                },
+                {
+                    "x": 2.935420930881945,
+                    "y": 6.8792439916128805,
+                    "heading": -2.869603740492264,
+                    "angularVelocity": -0.47935875037764497,
+                    "velocityX": 2.8838668124358944,
+                    "velocityY": 0.5552407316391297,
+                    "timestamp": 0.35261905517497993
+                },
+                {
+                    "x": 3.253189180048851,
+                    "y": 6.9405688545954956,
+                    "heading": -2.9017584747192204,
+                    "angularVelocity": -0.364753223117765,
+                    "velocityX": 3.604663383936769,
+                    "velocityY": 0.695650017576943,
+                    "timestamp": 0.4407738189687249
+                },
+                {
+                    "x": 3.5807176480413454,
+                    "y": 7.003313040154224,
+                    "heading": -2.901758484999235,
+                    "angularVelocity": -1.1661326594690383e-7,
+                    "velocityX": 3.7153802460274443,
+                    "velocityY": 0.7117503678590205,
+                    "timestamp": 0.5289285827624699
+                },
+                {
+                    "x": 3.9082461164396207,
+                    "y": 7.066057223594786,
+                    "heading": -2.901758495279238,
+                    "angularVelocity": -1.1661313280225847e-7,
+                    "velocityX": 3.7153802506304894,
+                    "velocityY": 0.7117503438312037,
+                    "timestamp": 0.6170833465562149
+                },
+                {
+                    "x": 4.235774584837897,
+                    "y": 7.128801407035338,
+                    "heading": -2.901758505559241,
+                    "angularVelocity": -1.1661313256198823e-7,
+                    "velocityX": 3.7153802506305107,
+                    "velocityY": 0.711750343831093,
+                    "timestamp": 0.7052381103499599
+                },
+                {
+                    "x": 4.563303053236174,
+                    "y": 7.19154559047589,
+                    "heading": -2.9017585158392443,
+                    "angularVelocity": -1.166131334044927e-7,
+                    "velocityX": 3.7153802506305107,
+                    "velocityY": 0.7117503438310929,
+                    "timestamp": 0.7933928741437049
+                },
+                {
+                    "x": 4.890831521634451,
+                    "y": 7.254289773916442,
+                    "heading": -2.9017585261192473,
+                    "angularVelocity": -1.1661313263433965e-7,
+                    "velocityX": 3.7153802506305107,
+                    "velocityY": 0.711750343831093,
+                    "timestamp": 0.8815476379374498
+                },
+                {
+                    "x": 5.218359990032727,
+                    "y": 7.317033957356994,
+                    "heading": -2.9017585363992504,
+                    "angularVelocity": -1.1661313267352917e-7,
+                    "velocityX": 3.7153802506305107,
+                    "velocityY": 0.7117503438310931,
+                    "timestamp": 0.9697024017311948
+                },
+                {
+                    "x": 5.545888458431004,
+                    "y": 7.379778140797546,
+                    "heading": -2.9017585466792535,
+                    "angularVelocity": -1.1661313271580576e-7,
+                    "velocityX": 3.7153802506305107,
+                    "velocityY": 0.7117503438310933,
+                    "timestamp": 1.0578571655249398
+                },
+                {
+                    "x": 5.873416926829281,
+                    "y": 7.442522324238098,
+                    "heading": -2.901758556959257,
+                    "angularVelocity": -1.166131330429824e-7,
+                    "velocityX": 3.7153802506305103,
+                    "velocityY": 0.7117503438310929,
+                    "timestamp": 1.1460119293186848
+                },
+                {
+                    "x": 6.200945395227635,
+                    "y": 7.505266507678241,
+                    "heading": -2.90175856723926,
+                    "angularVelocity": -1.1661313350589332e-7,
+                    "velocityX": 3.7153802506314,
+                    "velocityY": 0.7117503438264522,
+                    "timestamp": 1.2341666931124298
+                },
+                {
+                    "x": 6.528473880635578,
+                    "y": 7.568010602327234,
+                    "heading": -2.9017585775192636,
+                    "angularVelocity": -1.1661313512046063e-7,
+                    "velocityX": 3.715380443582822,
+                    "velocityY": 0.7117493366074876,
+                    "timestamp": 1.3223214569061748
+                },
+                {
+                    "x": 6.858875530524355,
+                    "y": 7.613248618194527,
+                    "heading": -2.901758593513828,
+                    "angularVelocity": -1.8143731961725824e-7,
+                    "velocityX": 3.747972720587331,
+                    "velocityY": 0.51316586784965,
+                    "timestamp": 1.4104762206999197
+                },
+                {
+                    "x": 7.1493370822627105,
+                    "y": 7.612204494163831,
+                    "heading": -2.9774836288017847,
+                    "angularVelocity": -0.859001057108326,
+                    "velocityX": 3.294904770182843,
+                    "velocityY": -0.011844215624353746,
+                    "timestamp": 1.4986309844936647
+                },
+                {
+                    "x": 7.3768731997980925,
+                    "y": 7.596010582302943,
+                    "heading": -3.0421265478344597,
+                    "angularVelocity": -0.7332890050493631,
+                    "velocityX": 2.581098374533033,
+                    "velocityY": -0.18369865863138202,
+                    "timestamp": 1.5867857482874097
+                },
+                {
+                    "x": 7.541210779550857,
+                    "y": 7.566116365480278,
+                    "heading": -3.09203212161725,
+                    "angularVelocity": -0.566113181354048,
+                    "velocityX": 1.8641939775059564,
+                    "velocityY": -0.33911062245705376,
+                    "timestamp": 1.6749405120811547
+                },
+                {
+                    "x": 7.642266411565545,
+                    "y": 7.52302718020075,
+                    "heading": -3.125932818808563,
+                    "angularVelocity": -0.3845588795476813,
+                    "velocityX": 1.146343404096945,
+                    "velocityY": -0.4887902074166845,
+                    "timestamp": 1.7630952758748997
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -0.19570681865122413,
+                    "velocityX": 0.4280379960263239,
+                    "velocityY": -0.6355547651607396,
+                    "timestamp": 1.8512500396686447
+                },
+                {
+                    "x": 7.65149635610838,
+                    "y": 7.395583820775626,
+                    "heading": -3.1428442524727873,
+                    "angularVelocity": 0.003749626131397279,
+                    "velocityX": -0.3133749684294368,
+                    "velocityY": -0.7851642756589506,
+                    "timestamp": 1.9422070297386083
+                },
+                {
+                    "x": 7.55553660648891,
+                    "y": 7.310596065504526,
+                    "heading": -3.1249452939328513,
+                    "angularVelocity": 0.19678485981305774,
+                    "velocityX": -1.0550013753274867,
+                    "velocityY": -0.9343729954757892,
+                    "timestamp": 2.033164019808572
+                },
+                {
+                    "x": 7.392089155098869,
+                    "y": 7.212098072056303,
+                    "heading": -3.09046475805122,
+                    "angularVelocity": 0.37908615770051013,
+                    "velocityX": -1.796975155667623,
+                    "velocityY": -1.082907354041688,
+                    "timestamp": 2.1241210098785355
+                },
+                {
+                    "x": 7.1610935040737775,
+                    "y": 7.100214825167467,
+                    "heading": -3.041377794518818,
+                    "angularVelocity": 0.5396722505288368,
+                    "velocityX": -2.539614062068222,
+                    "velocityY": -1.2300676045109389,
+                    "timestamp": 2.215077999948499
+                },
+                {
+                    "x": 6.862392163588639,
+                    "y": 6.975338305116689,
+                    "heading": -2.9837366917531454,
+                    "angularVelocity": 0.6337182301358967,
+                    "velocityX": -3.2839844442451716,
+                    "velocityY": -1.3729183425572538,
+                    "timestamp": 2.3060349900184627
+                },
+                {
+                    "x": 6.525454088685491,
+                    "y": 6.905573407470138,
+                    "heading": -2.9837366810920996,
+                    "angularVelocity": 1.1720974856117963e-7,
+                    "velocityX": -3.704367027141189,
+                    "velocityY": -0.7670097437560687,
+                    "timestamp": 2.3969919800884263
+                },
+                {
+                    "x": 6.1883268460675325,
+                    "y": 6.836728439791378,
+                    "heading": -2.983736670548346,
+                    "angularVelocity": 1.1592021231646331e-7,
+                    "velocityX": -3.7064467762031894,
+                    "velocityY": -0.7568958430329531,
+                    "timestamp": 2.48794897015839
+                },
+                {
+                    "x": 5.851199548758337,
+                    "y": 6.767884093073121,
+                    "heading": -2.9837365285519053,
+                    "angularVelocity": 0.0000015611382957440025,
+                    "velocityX": -3.706447377489981,
+                    "velocityY": -0.7568890160645022,
+                    "timestamp": 2.5789059602283535
+                },
+                {
+                    "x": 5.5810538404288925,
+                    "y": 6.712717106073741,
+                    "heading": -2.9505527658877098,
+                    "angularVelocity": 0.3648291641873475,
+                    "velocityX": -2.970037906065856,
+                    "velocityY": -0.6065172886320637,
+                    "timestamp": 2.669862950298317
+                },
+                {
+                    "x": 5.3784959131966685,
+                    "y": 6.671352294274059,
+                    "heading": -2.9178931983006144,
+                    "angularVelocity": 0.3590660548682879,
+                    "velocityX": -2.226963832866719,
+                    "velocityY": -0.45477331393503795,
+                    "timestamp": 2.7608199403682807
+                },
+                {
+                    "x": 5.2434899940019335,
+                    "y": 6.6437823583535645,
+                    "heading": -2.8932620140402783,
+                    "angularVelocity": 0.2708003446618372,
+                    "velocityX": -1.4842830561003668,
+                    "velocityY": -0.30310958948050626,
+                    "timestamp": 2.8517769304382443
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "angularVelocity": 0.14580533096009823,
+                    "velocityX": -0.7419989816068217,
+                    "velocityY": -0.15152610418353574,
+                    "timestamp": 2.942733920508208
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "angularVelocity": 0,
+                    "velocityX": 1.1797533379852482e-32,
+                    "velocityY": 0,
+                    "timestamp": 3.0336909105781715
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W1 To C1 To S2": {
+            "waypoints": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 21
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": -3.143185307179586,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.360726976398386,
+                    "y": 6.773087618158843,
+                    "heading": -2.760701916982345,
+                    "angularVelocity": -0.1232236665180751,
+                    "velocityX": 0.699220588306308,
+                    "velocityY": 0.18523553288193312,
+                    "timestamp": 0.08684952562036656
+                },
+                {
+                    "x": 2.4821843215925723,
+                    "y": 6.805261248290758,
+                    "heading": -2.781888481642216,
+                    "angularVelocity": -0.243945657831127,
+                    "velocityX": 1.3984802372452498,
+                    "velocityY": 0.37045257187160163,
+                    "timestamp": 0.17369905124073312
+                },
+                {
+                    "x": 2.664377574952537,
+                    "y": 6.853518187805784,
+                    "heading": -2.813196351929266,
+                    "angularVelocity": -0.3604840678552547,
+                    "velocityX": 2.0978036674189,
+                    "velocityY": 0.5556384927872844,
+                    "timestamp": 0.26054857686109967
+                },
+                {
+                    "x": 2.9073173781912582,
+                    "y": 6.917852930781389,
+                    "heading": -2.8538946648532777,
+                    "angularVelocity": -0.4686071988668809,
+                    "velocityX": 2.797249628060051,
+                    "velocityY": 0.7407610175878006,
+                    "timestamp": 0.34739810248146624
+                },
+                {
+                    "x": 3.211032053030438,
+                    "y": 6.99824783750104,
+                    "heading": -2.901766372941813,
+                    "angularVelocity": -0.5512028735505646,
+                    "velocityX": 3.4970216897535775,
+                    "velocityY": 0.9256804357344971,
+                    "timestamp": 0.4342476281018328
+                },
+                {
+                    "x": 3.5286107194849063,
+                    "y": 7.082430089360801,
+                    "heading": -2.9017663796929978,
+                    "angularVelocity": -7.773427725096788e-8,
+                    "velocityX": 3.656654013778666,
+                    "velocityY": 0.9692885626994009,
+                    "timestamp": 0.5210971537221993
+                },
+                {
+                    "x": 3.846189385784725,
+                    "y": 7.166612341804015,
+                    "heading": -2.9017663864441645,
+                    "angularVelocity": -7.773406554275384e-8,
+                    "velocityX": 3.6566540119980107,
+                    "velocityY": 0.9692885694173843,
+                    "timestamp": 0.6079466793425659
+                },
+                {
+                    "x": 4.163768052084543,
+                    "y": 7.250794594247232,
+                    "heading": -2.9017663931953313,
+                    "angularVelocity": -7.77340646408651e-8,
+                    "velocityX": 3.656654011998002,
+                    "velocityY": 0.969288569417417,
+                    "timestamp": 0.6947962049629324
+                },
+                {
+                    "x": 4.481346718384361,
+                    "y": 7.334976846690449,
+                    "heading": -2.901766399946498,
+                    "angularVelocity": -7.773406451460054e-8,
+                    "velocityX": 3.656654011998002,
+                    "velocityY": 0.9692885694174173,
+                    "timestamp": 0.7816457305832989
+                },
+                {
+                    "x": 4.798925384684178,
+                    "y": 7.419159099133665,
+                    "heading": -2.9017664066976647,
+                    "angularVelocity": -7.773406529246814e-8,
+                    "velocityX": 3.656654011998002,
+                    "velocityY": 0.9692885694174173,
+                    "timestamp": 0.8684952562036654
+                },
+                {
+                    "x": 5.1165040509839965,
+                    "y": 7.503341351576882,
+                    "heading": -2.9017664134488315,
+                    "angularVelocity": -7.773406535280122e-8,
+                    "velocityX": 3.656654011998002,
+                    "velocityY": 0.969288569417417,
+                    "timestamp": 0.9553447818240319
+                },
+                {
+                    "x": 5.434082717283814,
+                    "y": 7.5875236040201,
+                    "heading": -2.901766420199998,
+                    "angularVelocity": -7.773406538713727e-8,
+                    "velocityX": 3.6566540119980018,
+                    "velocityY": 0.9692885694174171,
+                    "timestamp": 1.0421943074443984
+                },
+                {
+                    "x": 5.751661383583632,
+                    "y": 7.6717058564633165,
+                    "heading": -2.901766426951165,
+                    "angularVelocity": -7.773406530909276e-8,
+                    "velocityX": 3.6566540119980027,
+                    "velocityY": 0.969288569417416,
+                    "timestamp": 1.129043833064765
+                },
+                {
+                    "x": 6.0692400498888315,
+                    "y": 7.755888108886232,
+                    "heading": -2.9017664337023317,
+                    "angularVelocity": -7.773406498550699e-8,
+                    "velocityX": 3.6566540120599655,
+                    "velocityY": 0.9692885691836598,
+                    "timestamp": 1.2158933586851315
+                },
+                {
+                    "x": 6.386819824167639,
+                    "y": 7.840066181352823,
+                    "heading": -2.901766440453515,
+                    "angularVelocity": -7.773425363968975e-8,
+                    "velocityX": 3.6566667694538877,
+                    "velocityY": 0.9692404404655809,
+                    "timestamp": 1.302742884305498
+                },
+                {
+                    "x": 6.714420971530167,
+                    "y": 7.864972939151579,
+                    "heading": -2.901766454732528,
+                    "angularVelocity": -1.6441095007226412e-7,
+                    "velocityX": 3.7720545394171228,
+                    "velocityY": 0.28678058539594425,
+                    "timestamp": 1.3895924099258645
+                },
+                {
+                    "x": 7.024629196469154,
+                    "y": 7.83076867028296,
+                    "heading": -2.946892502448731,
+                    "angularVelocity": -0.5195888796655828,
+                    "velocityX": 3.57178951437027,
+                    "velocityY": -0.39383368676145797,
+                    "timestamp": 1.476441935546231
+                },
+                {
+                    "x": 7.277681557115376,
+                    "y": 7.7704292121587075,
+                    "heading": -3.000549099570889,
+                    "angularVelocity": -0.6178110558347718,
+                    "velocityX": 2.9136873096139366,
+                    "velocityY": -0.6947586379213355,
+                    "timestamp": 1.5632914611665976
+                },
+                {
+                    "x": 7.471595029086891,
+                    "y": 7.688687529495423,
+                    "heading": -3.0531246016667453,
+                    "angularVelocity": -0.6053631464343129,
+                    "velocityX": 2.232752229631644,
+                    "velocityY": -0.941187439761266,
+                    "timestamp": 1.650140986786964
+                },
+                {
+                    "x": 7.605791298108634,
+                    "y": 7.5872519075766585,
+                    "heading": -3.101243004619086,
+                    "angularVelocity": -0.5540433595763132,
+                    "velocityX": 1.5451583421207176,
+                    "velocityY": -1.1679467584193457,
+                    "timestamp": 1.7369905124073306
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -0.4829307041215213,
+                    "velocityX": 0.854451436102516,
+                    "velocityY": -1.3846006263992148,
+                    "timestamp": 1.8238400380276971
+                },
+                {
+                    "x": 7.684759443310988,
+                    "y": 7.301931264284962,
+                    "heading": -3.1822787877008336,
+                    "angularVelocity": -0.3860471334464062,
+                    "velocityX": 0.046999382572933714,
+                    "velocityY": -1.6300495989311972,
+                    "timestamp": 1.925106119638729
+                },
+                {
+                    "x": 7.607446461395855,
+                    "y": 7.1130338870077985,
+                    "heading": -3.2115348858230854,
+                    "angularVelocity": -0.2889032305471994,
+                    "velocityX": -0.7634637450650118,
+                    "velocityY": -1.8653568329339223,
+                    "timestamp": 2.0263722012497607
+                },
+                {
+                    "x": 7.447519490138273,
+                    "y": 6.902260922095987,
+                    "heading": -3.2309059414306347,
+                    "angularVelocity": -0.1912886852083134,
+                    "velocityX": -1.5792748046828227,
+                    "velocityY": -2.0813777086972793,
+                    "timestamp": 2.1276382828607927
+                },
+                {
+                    "x": 7.203787473069244,
+                    "y": 6.674749702005586,
+                    "heading": -3.240291639650134,
+                    "angularVelocity": -0.09268353302700813,
+                    "velocityX": -2.406847516870182,
+                    "velocityY": -2.246667556121621,
+                    "timestamp": 2.2289043644718247
+                },
+                {
+                    "x": 6.8784205377457015,
+                    "y": 6.472538498060418,
+                    "heading": -3.240291634989942,
+                    "angularVelocity": 4.6019273769549756e-8,
+                    "velocityX": -3.212990274205886,
+                    "velocityY": -1.996830535247213,
+                    "timestamp": 2.3301704460828567
+                },
+                {
+                    "x": 6.516724017217928,
+                    "y": 6.34632974848032,
+                    "heading": -3.2402916309896224,
+                    "angularVelocity": 3.950305172889797e-8,
+                    "velocityX": -3.5717440111670684,
+                    "velocityY": -1.2463082166308144,
+                    "timestamp": 2.4314365276938887
+                },
+                {
+                    "x": 6.155023833001519,
+                    "y": 6.220131499152302,
+                    "heading": -3.240291626989329,
+                    "angularVelocity": 3.950279970360729e-8,
+                    "velocityX": -3.571780189992463,
+                    "velocityY": -1.2462045269279236,
+                    "timestamp": 2.5327026093049207
+                },
+                {
+                    "x": 5.793323648774168,
+                    "y": 6.093933249855646,
+                    "heading": -3.240291622989035,
+                    "angularVelocity": 3.950280005859099e-8,
+                    "velocityX": -3.571780190100518,
+                    "velocityY": -1.2462045266182225,
+                    "timestamp": 2.6339686909159528
+                },
+                {
+                    "x": 5.431623464546816,
+                    "y": 5.96773500055899,
+                    "heading": -3.240291618988741,
+                    "angularVelocity": 3.9502799442138326e-8,
+                    "velocityX": -3.5717801901005184,
+                    "velocityY": -1.246204526618221,
+                    "timestamp": 2.7352347725269848
+                },
+                {
+                    "x": 5.0699232803112935,
+                    "y": 5.841536751285806,
+                    "heading": -3.240291614988434,
+                    "angularVelocity": 3.9502931091413385e-8,
+                    "velocityX": -3.571780190181203,
+                    "velocityY": -1.2462045263864454,
+                    "timestamp": 2.836500854138017
+                },
+                {
+                    "x": 4.747153979068025,
+                    "y": 5.72892205607683,
+                    "heading": -3.2014500073341168,
+                    "angularVelocity": 0.383559895242649,
+                    "velocityX": -3.1873387032305858,
+                    "velocityY": -1.11206727284526,
+                    "timestamp": 2.937766935749049
+                },
+                {
+                    "x": 4.505076992705577,
+                    "y": 5.644461029601604,
+                    "heading": -3.172317846735081,
+                    "angularVelocity": 0.2876793506335712,
+                    "velocityX": -2.3905041304188885,
+                    "velocityY": -0.8340505046852051,
+                    "timestamp": 3.039033017360081
+                },
+                {
+                    "x": 4.343692332074418,
+                    "y": 5.588153676944576,
+                    "heading": -3.1528961309698516,
+                    "angularVelocity": 0.19178895298675563,
+                    "velocityX": -1.5936694504591111,
+                    "velocityY": -0.5560336863163278,
+                    "timestamp": 3.140299098971113
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": 0.0958941398322912,
+                    "velocityX": -0.796834742598023,
+                    "velocityY": -0.2780168492417652,
+                    "timestamp": 3.241565180582145
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": 0,
+                    "velocityX": 6.013328716711935e-32,
+                    "velocityY": 0,
+                    "timestamp": 3.342831262193177
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W1 To C2 To S1": {
+            "waypoints": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 21
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": -3.143185307179586,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.3639970489597717,
+                    "y": 6.742603274185352,
+                    "heading": -2.7625259389239276,
+                    "angularVelocity": -0.14112178255834015,
+                    "velocityX": 0.7210140239806111,
+                    "velocityY": -0.16219874791866862,
+                    "timestamp": 0.08875978390330586
+                },
+                {
+                    "x": 2.4920132643077064,
+                    "y": 6.7138069605767345,
+                    "heading": -2.7859988261252213,
+                    "angularVelocity": -0.26445408234505485,
+                    "velocityX": 1.4422772309517358,
+                    "velocityY": -0.3244297399370449,
+                    "timestamp": 0.1775195678066117
+                },
+                {
+                    "x": 2.684079448734256,
+                    "y": 6.670607796290334,
+                    "heading": -2.8177443395453605,
+                    "angularVelocity": -0.3576564973921373,
+                    "velocityX": 2.163887472233876,
+                    "velocityY": -0.4866974927908953,
+                    "timestamp": 0.26627935170991757
+                },
+                {
+                    "x": 2.9402327242018096,
+                    "y": 6.613005243899921,
+                    "heading": -2.852261622202671,
+                    "angularVelocity": -0.3888842574798656,
+                    "velocityX": 2.8859159430424604,
+                    "velocityY": -0.6489713004840627,
+                    "timestamp": 0.3550391356132234
+                },
+                {
+                    "x": 3.260353111631952,
+                    "y": 6.541052974820016,
+                    "heading": -2.8718469111116423,
+                    "angularVelocity": -0.22065498638783787,
+                    "velocityX": 3.6065926859272044,
+                    "velocityY": -0.8106404265054099,
+                    "timestamp": 0.4437989195165293
+                },
+                {
+                    "x": 3.5879291031343383,
+                    "y": 6.467313799666466,
+                    "heading": -2.87184692127097,
+                    "angularVelocity": -1.1445867602426544e-7,
+                    "velocityX": 3.690590232387729,
+                    "velocityY": -0.83077236008012,
+                    "timestamp": 0.5325587034198351
+                },
+                {
+                    "x": 3.915505094528652,
+                    "y": 6.3935746240328,
+                    "heading": -2.87184693143029,
+                    "angularVelocity": -1.1445859480672728e-7,
+                    "velocityX": 3.6905902311701486,
+                    "velocityY": -0.8307723654892717,
+                    "timestamp": 0.621318487323141
+                },
+                {
+                    "x": 4.243081085922966,
+                    "y": 6.3198354483991315,
+                    "heading": -2.87184694158961,
+                    "angularVelocity": -1.1445859516237229e-7,
+                    "velocityX": 3.6905902311701433,
+                    "velocityY": -0.8307723654892962,
+                    "timestamp": 0.7100782712264468
+                },
+                {
+                    "x": 4.5706570773172785,
+                    "y": 6.246096272765464,
+                    "heading": -2.87184695174893,
+                    "angularVelocity": -1.1445859514834922e-7,
+                    "velocityX": 3.6905902311701433,
+                    "velocityY": -0.830772365489296,
+                    "timestamp": 0.7988380551297527
+                },
+                {
+                    "x": 4.898233068711592,
+                    "y": 6.172357097131795,
+                    "heading": -2.8718469619082505,
+                    "angularVelocity": -1.1445859490669181e-7,
+                    "velocityX": 3.6905902311701437,
+                    "velocityY": -0.830772365489296,
+                    "timestamp": 0.8875978390330586
+                },
+                {
+                    "x": 5.225809060105905,
+                    "y": 6.0986179214981275,
+                    "heading": -2.8718469720675706,
+                    "angularVelocity": -1.1445859563440553e-7,
+                    "velocityX": 3.6905902311701433,
+                    "velocityY": -0.830772365489296,
+                    "timestamp": 0.9763576229363644
+                },
+                {
+                    "x": 5.553385051500219,
+                    "y": 6.024878745864458,
+                    "heading": -2.871846982226891,
+                    "angularVelocity": -1.1445859523892014e-7,
+                    "velocityX": 3.6905902311701437,
+                    "velocityY": -0.830772365489296,
+                    "timestamp": 1.0651174068396703
+                },
+                {
+                    "x": 5.8809610428945325,
+                    "y": 5.951139570230791,
+                    "heading": -2.871846992386211,
+                    "angularVelocity": -1.1445859598937227e-7,
+                    "velocityX": 3.6905902311701433,
+                    "velocityY": -0.8307723654892962,
+                    "timestamp": 1.1538771907429761
+                },
+                {
+                    "x": 6.208537034288882,
+                    "y": 5.877400394597284,
+                    "heading": -2.871847002545531,
+                    "angularVelocity": -1.1445859482620514e-7,
+                    "velocityX": 3.6905902311705536,
+                    "velocityY": -0.830772365487475,
+                    "timestamp": 1.242636974646282
+                },
+                {
+                    "x": 6.536113033734776,
+                    "y": 5.80366125473165,
+                    "heading": -2.8718470127048517,
+                    "angularVelocity": -1.1445859560539411e-7,
+                    "velocityX": 3.6905903218821825,
+                    "velocityY": -0.8307719625136204,
+                    "timestamp": 1.3313967585495878
+                },
+                {
+                    "x": 6.865344430510141,
+                    "y": 5.737705340336343,
+                    "heading": -2.8718470255455273,
+                    "angularVelocity": -1.4466772317254716e-7,
+                    "velocityX": 3.7092406301262124,
+                    "velocityY": -0.7430833142536372,
+                    "timestamp": 1.4201565424528937
+                },
+                {
+                    "x": 7.155715453065551,
+                    "y": 5.718947060788103,
+                    "heading": -2.9583296823858576,
+                    "angularVelocity": -0.974345058506937,
+                    "velocityX": 3.2714255238807013,
+                    "velocityY": -0.21133759821538473,
+                    "timestamp": 1.5089163263561995
+                },
+                {
+                    "x": 7.382461638266762,
+                    "y": 5.716006595883618,
+                    "heading": -3.030136340606869,
+                    "angularVelocity": -0.8089999216225757,
+                    "velocityX": 2.5546049711908463,
+                    "velocityY": -0.0331283468162716,
+                    "timestamp": 1.5976761102595054
+                },
+                {
+                    "x": 7.54544918703291,
+                    "y": 5.728176149550433,
+                    "heading": -3.085282134377374,
+                    "angularVelocity": -0.6212925645535633,
+                    "velocityX": 1.836276989404401,
+                    "velocityY": 0.13710661666404886,
+                    "timestamp": 1.6864358941628113
+                },
+                {
+                    "x": 7.644635592677741,
+                    "y": 5.7552144852221,
+                    "heading": -3.1230806073530184,
+                    "angularVelocity": -0.4258513407020198,
+                    "velocityX": 1.117470111834477,
+                    "velocityY": 0.3046237212691066,
+                    "timestamp": 1.775195678066117
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -0.2265068586519956,
+                    "velocityX": 0.39842827198390307,
+                    "velocityY": 0.4707708034014574,
+                    "timestamp": 1.863955461969423
+                },
+                {
+                    "x": 7.649158607972273,
+                    "y": 5.85526478345475,
+                    "heading": -3.144984703286767,
+                    "angularVelocity": -0.019773386750234018,
+                    "velocityX": -0.3389130220112019,
+                    "velocityY": 0.640265971773451,
+                    "timestamp": 1.9549563678794555
+                },
+                {
+                    "x": 7.551198365445194,
+                    "y": 5.928933141126823,
+                    "heading": -3.12869095344792,
+                    "angularVelocity": 0.17905041357452225,
+                    "velocityX": -1.076475465243452,
+                    "velocityY": 0.8095343330417561,
+                    "timestamp": 2.045957273789488
+                },
+                {
+                    "x": 7.386086795598405,
+                    "y": 6.0179701455014545,
+                    "heading": -3.0955113223095805,
+                    "angularVelocity": 0.3646077015007144,
+                    "velocityX": -1.814394793058725,
+                    "velocityY": 0.9784188792874091,
+                    "timestamp": 2.1369581796995205
+                },
+                {
+                    "x": 7.153763389046842,
+                    "y": 6.12230392350914,
+                    "heading": -3.047867696260251,
+                    "angularVelocity": 0.5235511182320745,
+                    "velocityX": -2.5529790525519296,
+                    "velocityY": 1.1465136194450147,
+                    "timestamp": 2.227959085609553
+                },
+                {
+                    "x": 6.854082785517415,
+                    "y": 6.241701438550574,
+                    "heading": -2.993162545915138,
+                    "angularVelocity": 0.6011495138212849,
+                    "velocityX": -3.29316066178182,
+                    "velocityY": 1.3120475433450827,
+                    "timestamp": 2.3189599915195855
+                },
+                {
+                    "x": 6.518695058786276,
+                    "y": 6.319314800630408,
+                    "heading": -2.9931625344725843,
+                    "angularVelocity": 1.257410975035093e-7,
+                    "velocityX": -3.685542724846253,
+                    "velocityY": 0.8528855982660521,
+                    "timestamp": 2.409960897429618
+                },
+                {
+                    "x": 6.18330715393461,
+                    "y": 6.396927393000049,
+                    "heading": -2.993162523030137,
+                    "angularVelocity": 1.2573992931558898e-7,
+                    "velocityX": -3.6855446821952005,
+                    "velocityY": 0.8528771399965199,
+                    "timestamp": 2.5009618033396506
+                },
+                {
+                    "x": 5.8479121197525,
+                    "y": 6.474508873089667,
+                    "heading": -2.9931623885517284,
+                    "angularVelocity": 0.0000014777699956967743,
+                    "velocityX": -3.685623025705882,
+                    "velocityY": 0.852535250212982,
+                    "timestamp": 2.591962709249683
+                },
+                {
+                    "x": 5.579069881205996,
+                    "y": 6.536724652078007,
+                    "heading": -2.956924011318997,
+                    "angularVelocity": 0.3982199613326688,
+                    "velocityX": -2.954280903668077,
+                    "velocityY": 0.6836830728899436,
+                    "timestamp": 2.6829636151597156
+                },
+                {
+                    "x": 5.377498755107576,
+                    "y": 6.583371353425055,
+                    "heading": -2.9212816182082424,
+                    "angularVelocity": 0.391670750464755,
+                    "velocityX": -2.2150452688647118,
+                    "velocityY": 0.5125960107821789,
+                    "timestamp": 2.773964521069748
+                },
+                {
+                    "x": 5.243156221242747,
+                    "y": 6.614459642803244,
+                    "heading": -2.894437298256596,
+                    "angularVelocity": 0.29498959030348726,
+                    "velocityX": -1.4762768845141638,
+                    "velocityY": 0.341626152699224,
+                    "timestamp": 2.8649654269797806
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "angularVelocity": 0.15865004982334352,
+                    "velocityX": -0.7379731066539015,
+                    "velocityY": 0.17077145596902096,
+                    "timestamp": 2.955966332889813
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "angularVelocity": 0,
+                    "velocityX": 2.398158538123596e-33,
+                    "velocityY": 0,
+                    "timestamp": 3.0469672387998457
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W1 To C2 To S2": {
+            "waypoints": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 21
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": -3.143185307179586,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 14
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.36406618503168,
+                    "y": 6.750841975187211,
+                    "heading": -2.7578611838826945,
+                    "angularVelocity": -0.0894504040603958,
+                    "velocityX": 0.7289927602771729,
+                    "velocityY": -0.07007059190289512,
+                    "timestamp": 0.08788315676457649
+                },
+                {
+                    "x": 2.4922026900458016,
+                    "y": 6.738526737153379,
+                    "heading": -2.773150566031247,
+                    "angularVelocity": -0.17397397534899775,
+                    "velocityX": 1.458032571103211,
+                    "velocityY": -0.1401319489105608,
+                    "timestamp": 0.17576631352915298
+                },
+                {
+                    "x": 2.6844159261129263,
+                    "y": 6.720055698035347,
+                    "heading": -2.7951433359678384,
+                    "angularVelocity": -0.25025011328969643,
+                    "velocityX": 2.187145331864103,
+                    "velocityY": -0.21017723757365894,
+                    "timestamp": 0.2636494702937295
+                },
+                {
+                    "x": 2.940716876843772,
+                    "y": 6.6954319085352205,
+                    "heading": -2.8223782521748206,
+                    "angularVelocity": -0.3098991571267751,
+                    "velocityX": 2.9163830723267137,
+                    "velocityY": -0.2801878130765108,
+                    "timestamp": 0.35153262705830596
+                },
+                {
+                    "x": 3.261123048295386,
+                    "y": 6.664666336504878,
+                    "heading": -2.850384657388346,
+                    "angularVelocity": -0.318677733533739,
+                    "velocityX": 3.645820009742318,
+                    "velocityY": -0.35007358819345447,
+                    "timestamp": 0.43941578382288243
+                },
+                {
+                    "x": 3.5920521261120903,
+                    "y": 6.632831781454392,
+                    "heading": -2.8503846627195863,
+                    "angularVelocity": -6.066282061484616e-8,
+                    "velocityX": 3.7655574742633027,
+                    "velocityY": -0.3622372730165362,
+                    "timestamp": 0.527298940587459
+                },
+                {
+                    "x": 3.922981203901898,
+                    "y": 6.6009972261242424,
+                    "heading": -2.850384668050817,
+                    "angularVelocity": -6.066271266149535e-8,
+                    "velocityX": 3.7655574739572506,
+                    "velocityY": -0.3622372761987803,
+                    "timestamp": 0.6151820973520354
+                },
+                {
+                    "x": 4.253910281691706,
+                    "y": 6.56916267079409,
+                    "heading": -2.8503846733820475,
+                    "angularVelocity": -6.06627128107855e-8,
+                    "velocityX": 3.765557473957249,
+                    "velocityY": -0.36223727619879537,
+                    "timestamp": 0.7030652541166119
+                },
+                {
+                    "x": 4.5848393594815136,
+                    "y": 6.537328115463938,
+                    "heading": -2.850384678713278,
+                    "angularVelocity": -6.066271279819625e-8,
+                    "velocityX": 3.765557473957249,
+                    "velocityY": -0.3622372761987953,
+                    "timestamp": 0.7909484108811884
+                },
+                {
+                    "x": 4.915768437271321,
+                    "y": 6.505493560133786,
+                    "heading": -2.850384684044509,
+                    "angularVelocity": -6.066271362462515e-8,
+                    "velocityX": 3.7655574739572493,
+                    "velocityY": -0.3622372761987953,
+                    "timestamp": 0.8788315676457649
+                },
+                {
+                    "x": 5.24669751506113,
+                    "y": 6.473659004803634,
+                    "heading": -2.8503846893757396,
+                    "angularVelocity": -6.066271352975027e-8,
+                    "velocityX": 3.765557473957249,
+                    "velocityY": -0.36223727619879525,
+                    "timestamp": 0.9667147244103413
+                },
+                {
+                    "x": 5.577626592850938,
+                    "y": 6.441824449473482,
+                    "heading": -2.85038469470697,
+                    "angularVelocity": -6.066271274322426e-8,
+                    "velocityX": 3.765557473957249,
+                    "velocityY": -0.3622372761987953,
+                    "timestamp": 1.054597881174918
+                },
+                {
+                    "x": 5.908555670640745,
+                    "y": 6.40998989414333,
+                    "heading": -2.8503847000382008,
+                    "angularVelocity": -6.066271341664136e-8,
+                    "velocityX": 3.7655574739572493,
+                    "velocityY": -0.36223727619879537,
+                    "timestamp": 1.1424810379394945
+                },
+                {
+                    "x": 6.239484748430477,
+                    "y": 6.37815533881239,
+                    "heading": -2.8503847053694313,
+                    "angularVelocity": -6.066271364618301e-8,
+                    "velocityX": 3.7655574739563864,
+                    "velocityY": -0.36223727620776275,
+                    "timestamp": 1.230364194704071
+                },
+                {
+                    "x": 6.5704138100618055,
+                    "y": 6.346320615510802,
+                    "heading": -2.8503847107006623,
+                    "angularVelocity": -6.066271546575477e-8,
+                    "velocityX": 3.7655572900940437,
+                    "velocityY": -0.3622391875028854,
+                    "timestamp": 1.3182473514686477
+                },
+                {
+                    "x": 6.897540531836615,
+                    "y": 6.287028122680583,
+                    "heading": -2.8503847211917432,
+                    "angularVelocity": -1.1937532942487347e-7,
+                    "velocityX": 3.722291435788134,
+                    "velocityY": -0.6746741356714466,
+                    "timestamp": 1.4061305082332243
+                },
+                {
+                    "x": 7.182619204890035,
+                    "y": 6.1828768490539545,
+                    "heading": -2.9185193386552397,
+                    "angularVelocity": -0.7752864140510717,
+                    "velocityX": 3.243837426289712,
+                    "velocityY": -1.1851107477355587,
+                    "timestamp": 1.4940136649978009
+                },
+                {
+                    "x": 7.403317140168432,
+                    "y": 6.080132379818418,
+                    "heading": -2.984427246863809,
+                    "angularVelocity": -0.749949258025913,
+                    "velocityX": 2.511265450666594,
+                    "velocityY": -1.1691030797945798,
+                    "timestamp": 1.5818968217623774
+                },
+                {
+                    "x": 7.559743583293149,
+                    "y": 5.981103818181206,
+                    "heading": -3.044561457345504,
+                    "angularVelocity": -0.6842518258963323,
+                    "velocityX": 1.7799365530730349,
+                    "velocityY": -1.1268207160843378,
+                    "timestamp": 1.669779978526954
+                },
+                {
+                    "x": 7.6519593983078185,
+                    "y": 5.886591133773424,
+                    "heading": -3.097689544379845,
+                    "angularVelocity": -0.6045309361913614,
+                    "velocityX": 1.0493002118904227,
+                    "velocityY": -1.0754357022127217,
+                    "timestamp": 1.7576631352915306
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -0.5176846676276772,
+                    "velocityX": 0.3190668465323417,
+                    "velocityY": -1.0194346342544611,
+                    "timestamp": 1.8455462920561072
+                },
+                {
+                    "x": 7.628959612668781,
+                    "y": 5.701821981186599,
+                    "heading": -3.184554593580224,
+                    "angularVelocity": -0.41404680845855485,
+                    "velocityX": -0.5108405610944724,
+                    "velocityY": -0.9525945055820302,
+                    "timestamp": 1.9454608078922528
+                },
+                {
+                    "x": 7.495051570255288,
+                    "y": 5.613939825179433,
+                    "heading": -3.215560246622884,
+                    "angularVelocity": -0.3103218064280789,
+                    "velocityX": -1.3402261052147335,
+                    "velocityY": -0.8795734560860885,
+                    "timestamp": 2.0453753237283987
+                },
+                {
+                    "x": 7.278385319183743,
+                    "y": 5.534503531789237,
+                    "heading": -3.2361878862694873,
+                    "angularVelocity": -0.2064528809850976,
+                    "velocityX": -2.1685162486986918,
+                    "velocityY": -0.7950425693946834,
+                    "timestamp": 2.1452898395645446
+                },
+                {
+                    "x": 6.979307104157413,
+                    "y": 5.4664019474569185,
+                    "heading": -3.246410388494101,
+                    "angularVelocity": -0.10231248321693356,
+                    "velocityX": -2.9933409827737334,
+                    "velocityY": -0.6815985021035508,
+                    "timestamp": 2.2452043554006904
+                },
+                {
+                    "x": 6.603136214940544,
+                    "y": 5.429560638127869,
+                    "heading": -3.2464103824694632,
+                    "angularVelocity": 6.029792436770947e-8,
+                    "velocityX": -3.7649273087983395,
+                    "velocityY": -0.368728297592585,
+                    "timestamp": 2.3451188712368363
+                },
+                {
+                    "x": 6.225751326953569,
+                    "y": 5.450595811881877,
+                    "heading": -3.2464103781263827,
+                    "angularVelocity": 4.346796393735324e-8,
+                    "velocityX": -3.777077683145315,
+                    "velocityY": 0.2105317088110093,
+                    "timestamp": 2.445033387072982
+                },
+                {
+                    "x": 5.848366461127548,
+                    "y": 5.47163138321431,
+                    "heading": -3.2464103737833025,
+                    "angularVelocity": 4.346795775537738e-8,
+                    "velocityX": -3.777077461346165,
+                    "velocityY": 0.21053568799683506,
+                    "timestamp": 2.544947902909128
+                },
+                {
+                    "x": 5.470981595301597,
+                    "y": 5.492666954547974,
+                    "heading": -3.246410369440223,
+                    "angularVelocity": 4.346795748687552e-8,
+                    "velocityX": -3.777077461345479,
+                    "velocityY": 0.21053568800914468,
+                    "timestamp": 2.644862418745274
+                },
+                {
+                    "x": 5.09359672947572,
+                    "y": 5.513702525882575,
+                    "heading": -3.246410365097125,
+                    "angularVelocity": 4.346813479097949e-8,
+                    "velocityX": -3.777077461344731,
+                    "velocityY": 0.21053568801853617,
+                    "timestamp": 2.7447769345814197
+                },
+                {
+                    "x": 4.761358057173187,
+                    "y": 5.532221526743186,
+                    "heading": -3.20512020544725,
+                    "angularVelocity": 0.4132548639637959,
+                    "velocityX": -3.3252292674608595,
+                    "velocityY": 0.18534845218066273,
+                    "timestamp": 2.8446914504175655
+                },
+                {
+                    "x": 4.5121790336148475,
+                    "y": 5.546110766147867,
+                    "heading": -3.174152719009984,
+                    "angularVelocity": 0.3099398138309664,
+                    "velocityX": -2.4939221440754293,
+                    "velocityY": 0.13901122663156756,
+                    "timestamp": 2.9446059662537114
+                },
+                {
+                    "x": 4.346059678819821,
+                    "y": 5.555370255329891,
+                    "heading": -3.153507786492084,
+                    "angularVelocity": 0.20662595765119826,
+                    "velocityX": -1.6626148203275395,
+                    "velocityY": 0.09267411351128947,
+                    "timestamp": 3.0445204820898573
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": 0.10331310947276948,
+                    "velocityX": -0.8313074243990123,
+                    "velocityY": 0.04633705754727876,
+                    "timestamp": 3.144434997926003
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": 0,
+                    "velocityX": 1.3822008000428574e-34,
+                    "velocityY": 0,
+                    "timestamp": 3.244349513762149
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W1 To C3 To S1": {
+            "waypoints": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 23
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": -3.143185307179586,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.352643709641578,
+                    "y": 6.724306544750454,
+                    "heading": -2.7643107246230096,
+                    "angularVelocity": -0.16582604780361404,
+                    "velocityX": 0.6100109212961704,
+                    "velocityY": -0.37883661491424325,
+                    "timestamp": 0.08629961825889286
+                },
+                {
+                    "x": 2.457948690141224,
+                    "y": 6.658909154192408,
+                    "heading": -2.791757157392081,
+                    "angularVelocity": -0.31803654897277683,
+                    "velocityX": 1.2202253338341789,
+                    "velocityY": -0.7577946678969769,
+                    "timestamp": 0.17259923651778572
+                },
+                {
+                    "x": 2.6159410271495345,
+                    "y": 6.560792064019538,
+                    "heading": -2.830360502782766,
+                    "angularVelocity": -0.44731768423791834,
+                    "velocityX": 1.8307420148076843,
+                    "velocityY": -1.136935390358925,
+                    "timestamp": 0.25889885477667857
+                },
+                {
+                    "x": 2.826660648973102,
+                    "y": 6.4299308538855104,
+                    "heading": -2.87607731089876,
+                    "angularVelocity": -0.5297451951435223,
+                    "velocityX": 2.4417213665020787,
+                    "velocityY": -1.516359084482017,
+                    "timestamp": 0.34519847303557144
+                },
+                {
+                    "x": 3.0901174136120235,
+                    "y": 6.266320048368765,
+                    "heading": -2.9160689628763303,
+                    "angularVelocity": -0.46340473786276914,
+                    "velocityX": 3.052814948135086,
+                    "velocityY": -1.8958462252537605,
+                    "timestamp": 0.4314980912944643
+                },
+                {
+                    "x": 3.3674552739873977,
+                    "y": 6.094087733168023,
+                    "heading": -2.916068973852836,
+                    "angularVelocity": -1.2719066550138966e-7,
+                    "velocityX": 3.213662655416737,
+                    "velocityY": -1.995748285747182,
+                    "timestamp": 0.5177977095533571
+                },
+                {
+                    "x": 3.6447931343598987,
+                    "y": 5.921855417962633,
+                    "heading": -2.9160689848293226,
+                    "angularVelocity": -1.2719044142347595e-7,
+                    "velocityX": 3.213662655383448,
+                    "velocityY": -1.9957482858010414,
+                    "timestamp": 0.60409732781225
+                },
+                {
+                    "x": 3.9221309947324,
+                    "y": 5.749623102757243,
+                    "heading": -2.9160689958058095,
+                    "angularVelocity": -1.2719044074317096e-7,
+                    "velocityX": 3.2136626553834478,
+                    "velocityY": -1.9957482858010414,
+                    "timestamp": 0.6903969460711429
+                },
+                {
+                    "x": 4.199468855104901,
+                    "y": 5.577390787551853,
+                    "heading": -2.916069006782296,
+                    "angularVelocity": -1.2719044064772273e-7,
+                    "velocityX": 3.213662655383448,
+                    "velocityY": -1.995748285801041,
+                    "timestamp": 0.7766965643300358
+                },
+                {
+                    "x": 4.476806715477402,
+                    "y": 5.405158472346463,
+                    "heading": -2.9160690177587827,
+                    "angularVelocity": -1.271904417367868e-7,
+                    "velocityX": 3.213662655383448,
+                    "velocityY": -1.9957482858010416,
+                    "timestamp": 0.8629961825889286
+                },
+                {
+                    "x": 4.754144575849903,
+                    "y": 5.2329261571410735,
+                    "heading": -2.9160690287352695,
+                    "angularVelocity": -1.2719044128845816e-7,
+                    "velocityX": 3.2136626553834478,
+                    "velocityY": -1.9957482858010416,
+                    "timestamp": 0.9492958008478215
+                },
+                {
+                    "x": 5.031482436222404,
+                    "y": 5.0606938419356835,
+                    "heading": -2.9160690397117563,
+                    "angularVelocity": -1.271904407298397e-7,
+                    "velocityX": 3.2136626553834486,
+                    "velocityY": -1.995748285801041,
+                    "timestamp": 1.0355954191067143
+                },
+                {
+                    "x": 5.308820296594906,
+                    "y": 4.8884615267302935,
+                    "heading": -2.9160690506882427,
+                    "angularVelocity": -1.2719044158933254e-7,
+                    "velocityX": 3.2136626553834478,
+                    "velocityY": -1.995748285801042,
+                    "timestamp": 1.1218950373656071
+                },
+                {
+                    "x": 5.586158156967406,
+                    "y": 4.716229211524904,
+                    "heading": -2.9160690616647296,
+                    "angularVelocity": -1.271904410743158e-7,
+                    "velocityX": 3.2136626553834478,
+                    "velocityY": -1.995748285801042,
+                    "timestamp": 1.2081946556245
+                },
+                {
+                    "x": 5.863496017339908,
+                    "y": 4.543996896319514,
+                    "heading": -2.9160690726412164,
+                    "angularVelocity": -1.2719044055646472e-7,
+                    "velocityX": 3.213662655383448,
+                    "velocityY": -1.9957482858010414,
+                    "timestamp": 1.2944942738833929
+                },
+                {
+                    "x": 6.140833877716964,
+                    "y": 4.371764581121458,
+                    "heading": -2.916069083617703,
+                    "angularVelocity": -1.2719044131717392e-7,
+                    "velocityX": 3.213662655436225,
+                    "velocityY": -1.9957482857160567,
+                    "timestamp": 1.3807938921422858
+                },
+                {
+                    "x": 6.418172664892274,
+                    "y": 4.19953375831344,
+                    "heading": -2.9160690945941994,
+                    "angularVelocity": -1.2719055821939482e-7,
+                    "velocityX": 3.21367339474496,
+                    "velocityY": -1.9957309925918116,
+                    "timestamp": 1.4670935104011786
+                },
+                {
+                    "x": 6.7207652632777295,
+                    "y": 4.0769859125864,
+                    "heading": -2.916069118255731,
+                    "angularVelocity": -2.741788669167458e-7,
+                    "velocityX": 3.5063028607839835,
+                    "velocityY": -1.4200276687058784,
+                    "timestamp": 1.5533931286600715
+                },
+                {
+                    "x": 7.011920641126383,
+                    "y": 4.012803875379112,
+                    "heading": -2.9821933696709393,
+                    "angularVelocity": -0.7662171948025339,
+                    "velocityX": 3.3737736472290445,
+                    "velocityY": -0.7437117162469018,
+                    "timestamp": 1.6396927469189644
+                },
+                {
+                    "x": 7.255025953055537,
+                    "y": 3.987925238494502,
+                    "heading": -3.0423989255025554,
+                    "angularVelocity": -0.6976340920894686,
+                    "velocityX": 2.8169917414808885,
+                    "velocityY": -0.28828211973553797,
+                    "timestamp": 1.7259923651778573
+                },
+                {
+                    "x": 7.447850515704627,
+                    "y": 3.9993347769870677,
+                    "heading": -3.0905963113163932,
+                    "angularVelocity": -0.5584889804459283,
+                    "velocityX": 2.234361710265075,
+                    "velocityY": 0.13220844684348906,
+                    "timestamp": 1.8122919834367501
+                },
+                {
+                    "x": 7.5896354595479805,
+                    "y": 4.045892337030357,
+                    "heading": -3.124573318920613,
+                    "angularVelocity": -0.3937098250312271,
+                    "velocityX": 1.6429382505152235,
+                    "velocityY": 0.539487439032544,
+                    "timestamp": 1.898591601695643
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -0.21566709836621822,
+                    "velocityX": 1.047102435377472,
+                    "velocityY": 0.9398380271749195,
+                    "timestamp": 1.9848912199545359
+                },
+                {
+                    "x": 7.718344137255122,
+                    "y": 4.246200601691832,
+                    "heading": -3.145373267573727,
+                    "angularVelocity": -0.024710376357260012,
+                    "velocityX": 0.43305082947346774,
+                    "velocityY": 1.34622717133274,
+                    "timestamp": 2.073435415339313
+                },
+                {
+                    "x": 7.702075707426083,
+                    "y": 4.401035710087067,
+                    "heading": -3.1311779977271272,
+                    "angularVelocity": 0.16031846906115368,
+                    "velocityX": -0.183732313080062,
+                    "velocityY": 1.7486759885425491,
+                    "timestamp": 2.16197961072409
+                },
+                {
+                    "x": 7.630801467153219,
+                    "y": 4.590926342055076,
+                    "heading": -3.10146947436901,
+                    "angularVelocity": 0.33552197554576096,
+                    "velocityX": -0.804956665562769,
+                    "velocityY": 2.1445858889079887,
+                    "timestamp": 2.250523806108867
+                },
+                {
+                    "x": 7.503771169060967,
+                    "y": 4.814724539953898,
+                    "heading": -3.057954484569731,
+                    "angularVelocity": 0.49144937855435067,
+                    "velocityX": -1.4346541581807906,
+                    "velocityY": 2.527530990879987,
+                    "timestamp": 2.339068001493644
+                },
+                {
+                    "x": 7.31900648308238,
+                    "y": 5.069082680437073,
+                    "heading": -3.0055052448244375,
+                    "angularVelocity": 0.5923509667139643,
+                    "velocityX": -2.086694505218799,
+                    "velocityY": 2.872668720699063,
+                    "timestamp": 2.427612196878421
+                },
+                {
+                    "x": 7.075378945667608,
+                    "y": 5.298956768781039,
+                    "heading": -3.0055052272028773,
+                    "angularVelocity": 1.990142925919268e-7,
+                    "velocityX": -2.75147948837462,
+                    "velocityY": 2.5961508526209696,
+                    "timestamp": 2.516156392263198
+                },
+                {
+                    "x": 6.801072699384408,
+                    "y": 5.491187253021094,
+                    "heading": -3.005505216579771,
+                    "angularVelocity": 1.1997518482429124e-7,
+                    "velocityX": -3.097958540262374,
+                    "velocityY": 2.1710117010503502,
+                    "timestamp": 2.604700587647975
+                },
+                {
+                    "x": 6.5267661173867575,
+                    "y": 5.683417258207369,
+                    "heading": -3.005505205956667,
+                    "angularVelocity": 1.199751543060781e-7,
+                    "velocityX": -3.0979623317525853,
+                    "velocityY": 2.1710062907151193,
+                    "timestamp": 2.693244783032752
+                },
+                {
+                    "x": 6.252459535387558,
+                    "y": 5.875647263391433,
+                    "heading": -3.005505195333564,
+                    "angularVelocity": 1.199751539565171e-7,
+                    "velocityX": -3.0979623317700846,
+                    "velocityY": 2.1710062906901486,
+                    "timestamp": 2.781788978417529
+                },
+                {
+                    "x": 5.978152953372341,
+                    "y": 6.067877268552632,
+                    "heading": -3.0055051847104526,
+                    "angularVelocity": 1.1997524424086504e-7,
+                    "velocityX": -3.0979623319509813,
+                    "velocityY": 2.171006290431916,
+                    "timestamp": 2.870333173802306
+                },
+                {
+                    "x": 5.71076650313837,
+                    "y": 6.255250599668089,
+                    "heading": -2.984811425443856,
+                    "angularVelocity": 0.23371107700143318,
+                    "velocityX": -3.0198077815501345,
+                    "velocityY": 2.1161560088926668,
+                    "timestamp": 2.958877369187083
+                },
+                {
+                    "x": 5.496808427326641,
+                    "y": 6.405185542172583,
+                    "heading": -2.9493217674847187,
+                    "angularVelocity": 0.40081292520855316,
+                    "velocityX": -2.4163986682814493,
+                    "velocityY": 1.6933345190325848,
+                    "timestamp": 3.04742156457186
+                },
+                {
+                    "x": 5.33638152944714,
+                    "y": 6.517608311818503,
+                    "heading": -2.9168038375204985,
+                    "angularVelocity": 0.3672508381444,
+                    "velocityX": -1.811828513234718,
+                    "velocityY": 1.269679724997829,
+                    "timestamp": 3.135965759956637
+                },
+                {
+                    "x": 5.229454180923455,
+                    "y": 6.592540440066129,
+                    "heading": -2.8927984833645977,
+                    "angularVelocity": 0.27111155118061186,
+                    "velocityX": -1.2076155648493634,
+                    "velocityY": 0.8462681028637682,
+                    "timestamp": 3.224509955341414
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "angularVelocity": 0.144543448733038,
+                    "velocityX": -0.6037005666060847,
+                    "velocityY": 0.4230605944433498,
+                    "timestamp": 3.313054150726191
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "angularVelocity": 0,
+                    "velocityX": 1.048960687291352e-31,
+                    "velocityY": 0,
+                    "timestamp": 3.401598346110968
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W1 To C3 To S2": {
+            "waypoints": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 23
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": -3.143185307179586,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.357227124880225,
+                    "y": 6.730410634555629,
+                    "heading": -2.7571910501011128,
+                    "angularVelocity": -0.08264039553460875,
+                    "velocityX": 0.6576608657862654,
+                    "velocityY": -0.30556812237992936,
+                    "timestamp": 0.08701616267193764
+                },
+                {
+                    "x": 2.4716821810223912,
+                    "y": 6.677231589623697,
+                    "heading": -2.771475854118176,
+                    "angularVelocity": -0.1641626518388192,
+                    "velocityX": 1.3153309986062809,
+                    "velocityY": -0.6111398537812316,
+                    "timestamp": 0.17403232534387528
+                },
+                {
+                    "x": 2.6433664887429877,
+                    "y": 6.597462350883563,
+                    "heading": -2.792692156200411,
+                    "angularVelocity": -0.2438202447770986,
+                    "velocityX": 1.973016304659045,
+                    "velocityY": -0.916717495815976,
+                    "timestamp": 0.26104848801581293
+                },
+                {
+                    "x": 2.8722826002398385,
+                    "y": 6.491101923690649,
+                    "heading": -2.820514979829771,
+                    "angularVelocity": -0.319743169257603,
+                    "velocityX": 2.630730940870083,
+                    "velocityY": -1.222306568423469,
+                    "timestamp": 0.34806465068775055
+                },
+                {
+                    "x": 3.1584374767810863,
+                    "y": 6.358147629132494,
+                    "heading": -2.8539648898748324,
+                    "angularVelocity": -0.38441030973950396,
+                    "velocityX": 3.288525576795303,
+                    "velocityY": -1.5279264274087752,
+                    "timestamp": 0.4350808133596882
+                },
+                {
+                    "x": 3.456964402226087,
+                    "y": 6.219442363897474,
+                    "heading": -2.853964894160219,
+                    "angularVelocity": -4.924816792193532e-8,
+                    "velocityX": 3.4307066213720128,
+                    "velocityY": -1.5940172604250262,
+                    "timestamp": 0.5220969760316259
+                },
+                {
+                    "x": 3.755491327665543,
+                    "y": 6.0807370986505,
+                    "heading": -2.853964898445594,
+                    "angularVelocity": -4.924803343036069e-8,
+                    "velocityX": 3.4307066213082953,
+                    "velocityY": -1.5940172605623917,
+                    "timestamp": 0.6091131387035635
+                },
+                {
+                    "x": 4.054018253104999,
+                    "y": 5.942031833403527,
+                    "heading": -2.853964902730969,
+                    "angularVelocity": -4.924803431145499e-8,
+                    "velocityX": 3.4307066213082953,
+                    "velocityY": -1.594017260562392,
+                    "timestamp": 0.6961293013755012
+                },
+                {
+                    "x": 4.352545178544456,
+                    "y": 5.803326568156554,
+                    "heading": -2.8539649070163438,
+                    "angularVelocity": -4.9248033877501264e-8,
+                    "velocityX": 3.4307066213082953,
+                    "velocityY": -1.5940172605623917,
+                    "timestamp": 0.7831454640474389
+                },
+                {
+                    "x": 4.651072103983911,
+                    "y": 5.66462130290958,
+                    "heading": -2.8539649113017185,
+                    "angularVelocity": -4.924803325887112e-8,
+                    "velocityX": 3.430706621308295,
+                    "velocityY": -1.5940172605623921,
+                    "timestamp": 0.8701616267193766
+                },
+                {
+                    "x": 4.949599029423368,
+                    "y": 5.525916037662607,
+                    "heading": -2.853964915587093,
+                    "angularVelocity": -4.924803367441165e-8,
+                    "velocityX": 3.430706621308295,
+                    "velocityY": -1.5940172605623926,
+                    "timestamp": 0.9571777893913143
+                },
+                {
+                    "x": 5.248125954862823,
+                    "y": 5.387210772415633,
+                    "heading": -2.853964919872468,
+                    "angularVelocity": -4.924803391655779e-8,
+                    "velocityX": 3.4307066213082953,
+                    "velocityY": -1.594017260562392,
+                    "timestamp": 1.044193952063252
+                },
+                {
+                    "x": 5.54665288030228,
+                    "y": 5.24850550716866,
+                    "heading": -2.8539649241578426,
+                    "angularVelocity": -4.9248034279126105e-8,
+                    "velocityX": 3.4307066213082953,
+                    "velocityY": -1.5940172605623923,
+                    "timestamp": 1.1312101147351896
+                },
+                {
+                    "x": 5.8451798057417355,
+                    "y": 5.109800241921686,
+                    "heading": -2.8539649284432174,
+                    "angularVelocity": -4.9248034369336404e-8,
+                    "velocityX": 3.4307066213082953,
+                    "velocityY": -1.594017260562393,
+                    "timestamp": 1.2182262774071273
+                },
+                {
+                    "x": 6.143706731181192,
+                    "y": 4.9710949766747135,
+                    "heading": -2.853964932728592,
+                    "angularVelocity": -4.9248033497376594e-8,
+                    "velocityX": 3.4307066213082953,
+                    "velocityY": -1.594017260562392,
+                    "timestamp": 1.305242440079065
+                },
+                {
+                    "x": 6.4422336566206475,
+                    "y": 4.83238971142774,
+                    "heading": -2.8539649370139673,
+                    "angularVelocity": -4.924803317735332e-8,
+                    "velocityX": 3.4307066213082953,
+                    "velocityY": -1.5940172605623926,
+                    "timestamp": 1.3922586027510027
+                },
+                {
+                    "x": 6.740760582057679,
+                    "y": 4.693684446175547,
+                    "heading": -2.853964941299342,
+                    "angularVelocity": -4.9248033793238546e-8,
+                    "velocityX": 3.4307066212804305,
+                    "velocityY": -1.5940172606223628,
+                    "timestamp": 1.4792747654229403
+                },
+                {
+                    "x": 7.039286998892109,
+                    "y": 4.554978086294829,
+                    "heading": -2.8539649455851244,
+                    "angularVelocity": -4.925271638681629e-8,
+                    "velocityX": 3.4307007763593735,
+                    "velocityY": -1.59402984022244,
+                    "timestamp": 1.566290928094878
+                },
+                {
+                    "x": 7.281877390378217,
+                    "y": 4.416206845653086,
+                    "heading": -2.926969050497618,
+                    "angularVelocity": -0.8389717803085356,
+                    "velocityX": 2.7878773786049877,
+                    "velocityY": -1.594775457577107,
+                    "timestamp": 1.6533070907668157
+                },
+                {
+                    "x": 7.4672438532140255,
+                    "y": 4.304023352017119,
+                    "heading": -2.9923953501231186,
+                    "angularVelocity": -0.7518867485821702,
+                    "velocityX": 2.130253244269864,
+                    "velocityY": -1.2892259344843033,
+                    "timestamp": 1.7403232534387534
+                },
+                {
+                    "x": 7.595386429544574,
+                    "y": 4.218427699418672,
+                    "heading": -3.050241491364391,
+                    "angularVelocity": -0.6647746747849547,
+                    "velocityX": 1.4726295942704664,
+                    "velocityY": -0.9836753307676307,
+                    "timestamp": 1.827339416110691
+                },
+                {
+                    "x": 7.666305140070344,
+                    "y": 4.159419912093254,
+                    "heading": -3.100505408798514,
+                    "angularVelocity": -0.5776388649040337,
+                    "velocityX": 0.8150061821634523,
+                    "velocityY": -0.6781244485335891,
+                    "timestamp": 1.9143555787826287
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -0.4904824238455614,
+                    "velocityX": 0.15738294483620113,
+                    "velocityY": -0.37257345184803875,
+                    "timestamp": 2.0013717414545664
+                },
+                {
+                    "x": 7.62294760973909,
+                    "y": 4.124186267487049,
+                    "heading": -3.1816074637236196,
+                    "angularVelocity": -0.39238397540812464,
+                    "velocityX": -0.5826441228371791,
+                    "velocityY": -0.02873507497949264,
+                    "timestamp": 2.099291531639857
+                },
+                {
+                    "x": 7.4934981410182715,
+                    "y": 4.155183222691002,
+                    "heading": -3.2104217744560906,
+                    "angularVelocity": -0.29426442476997366,
+                    "velocityX": -1.3219949560335675,
+                    "velocityY": 0.31655455087575607,
+                    "timestamp": 2.1972113218251477
+                },
+                {
+                    "x": 7.291774718335501,
+                    "y": 4.220253220991955,
+                    "heading": -3.2296266134452343,
+                    "angularVelocity": -0.19612826940094027,
+                    "velocityX": -2.06008838765949,
+                    "velocityY": 0.6645234653569294,
+                    "timestamp": 2.2951311120104383
+                },
+                {
+                    "x": 7.018085982963682,
+                    "y": 4.320043342217767,
+                    "heading": -3.2392208692548645,
+                    "angularVelocity": -0.09798076355633002,
+                    "velocityX": -2.795029838747879,
+                    "velocityY": 1.0191006438737447,
+                    "timestamp": 2.393050902195729
+                },
+                {
+                    "x": 6.674609525490048,
+                    "y": 4.458745905991077,
+                    "heading": -3.2392208627403147,
+                    "angularVelocity": 6.652945395302414e-8,
+                    "velocityX": -3.5077327762210677,
+                    "velocityY": 1.4164916357648132,
+                    "timestamp": 2.4909706923810195
+                },
+                {
+                    "x": 6.337654568768723,
+                    "y": 4.612615494980773,
+                    "heading": -3.239220858536144,
+                    "angularVelocity": 4.2934841578218656e-8,
+                    "velocityX": -3.4411323398846756,
+                    "velocityY": 1.5713839735413375,
+                    "timestamp": 2.58889048256631
+                },
+                {
+                    "x": 6.000699635853928,
+                    "y": 4.766485136103752,
+                    "heading": -3.239220854331973,
+                    "angularVelocity": 4.293484065869374e-8,
+                    "velocityX": -3.441132096761908,
+                    "velocityY": 1.5713845059493727,
+                    "timestamp": 2.686810272751601
+                },
+                {
+                    "x": 5.66374470293921,
+                    "y": 4.920354777226902,
+                    "heading": -3.239220850127803,
+                    "angularVelocity": 4.293484113596361e-8,
+                    "velocityX": -3.4411320967611103,
+                    "velocityY": 1.5713845059511196,
+                    "timestamp": 2.7847300629368914
+                },
+                {
+                    "x": 5.326789770024494,
+                    "y": 5.074224418350052,
+                    "heading": -3.239220845923632,
+                    "angularVelocity": 4.293484003264988e-8,
+                    "velocityX": -3.441132096761111,
+                    "velocityY": 1.571384505951117,
+                    "timestamp": 2.882649853122182
+                },
+                {
+                    "x": 4.989834837078673,
+                    "y": 5.228094059405012,
+                    "heading": -3.2392208417194372,
+                    "angularVelocity": 4.293508551285566e-8,
+                    "velocityX": -3.4411320970787416,
+                    "velocityY": 1.571384505254732,
+                    "timestamp": 2.9805696433074726
+                },
+                {
+                    "x": 4.699100923806666,
+                    "y": 5.360856424917345,
+                    "heading": -3.200805721128661,
+                    "angularVelocity": 0.39231212115635317,
+                    "velocityX": -2.9691026984622892,
+                    "velocityY": 1.3558277163493837,
+                    "timestamp": 3.0784894334927633
+                },
+                {
+                    "x": 4.481050467012654,
+                    "y": 5.460428209814717,
+                    "heading": -3.171995332755761,
+                    "angularVelocity": 0.2942243679074833,
+                    "velocityX": -2.2268272468864767,
+                    "velocityY": 1.016870897179783,
+                    "timestamp": 3.176409223678054
+                },
+                {
+                    "x": 4.335683489671324,
+                    "y": 5.5268094029690245,
+                    "heading": -3.152788680819891,
+                    "angularVelocity": 0.19614678401093785,
+                    "velocityX": -1.4845515606830426,
+                    "velocityY": 0.6779139643650731,
+                    "timestamp": 3.2743290138633445
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": 0.09807387885669308,
+                    "velocityX": -0.7422757905606995,
+                    "velocityY": 0.3389569868171628,
+                    "timestamp": 3.372248804048635
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": 0,
+                    "velocityX": 2.220393944714812e-34,
+                    "velocityY": 0,
+                    "timestamp": 3.4701685942339258
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W1 To C3 To S3": {
+            "waypoints": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 23
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": -3.143185307179586,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 17
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": -3.7091853071795864,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.3574291714983375,
+                    "y": 6.741815654710436,
+                    "heading": -2.758007728773179,
+                    "angularVelocity": -0.09483878465079744,
+                    "velocityX": 0.6801570061474024,
+                    "velocityY": -0.17983436924136462,
+                    "timestamp": 0.08443516861442411
+                },
+                {
+                    "x": 2.47228751228047,
+                    "y": 6.711446972989247,
+                    "heading": -2.7740235521881464,
+                    "angularVelocity": -0.18968190243216043,
+                    "velocityX": 1.3603139860670759,
+                    "velocityY": -0.35966863357457735,
+                    "timestamp": 0.16887033722884823
+                },
+                {
+                    "x": 2.6445750170696067,
+                    "y": 6.665893967918211,
+                    "heading": -2.798048184372895,
+                    "angularVelocity": -0.2845334779214143,
+                    "velocityX": 2.0404709034915762,
+                    "velocityY": -0.5395027429749605,
+                    "timestamp": 0.2533055058432723
+                },
+                {
+                    "x": 2.8742916749054777,
+                    "y": 6.605156656477843,
+                    "heading": -2.830082761948688,
+                    "angularVelocity": -0.37939851487799736,
+                    "velocityX": 2.720627691109149,
+                    "velocityY": -0.7193366512682123,
+                    "timestamp": 0.33774067445769645
+                },
+                {
+                    "x": 3.161437459702618,
+                    "y": 6.529235060521681,
+                    "heading": -2.870128852530637,
+                    "angularVelocity": -0.4742821177371166,
+                    "velocityX": 3.400784169785955,
+                    "velocityY": -0.8991703007411442,
+                    "timestamp": 0.4221758430721206
+                },
+                {
+                    "x": 3.470239032500689,
+                    "y": 6.44758707798969,
+                    "heading": -2.8701288573799584,
+                    "angularVelocity": -5.743248864696825e-8,
+                    "velocityX": 3.6572624637989906,
+                    "velocityY": -0.9669902230531149,
+                    "timestamp": 0.5066110116865447
+                },
+                {
+                    "x": 3.77904060528322,
+                    "y": 6.3659390953988515,
+                    "heading": -2.8701288622292584,
+                    "angularVelocity": -5.743222732502049e-8,
+                    "velocityX": 3.6572624636149422,
+                    "velocityY": -0.9669902237500612,
+                    "timestamp": 0.5910461803009688
+                },
+                {
+                    "x": 4.087842178065752,
+                    "y": 6.284291112808013,
+                    "heading": -2.870128867078558,
+                    "angularVelocity": -5.743222733813293e-8,
+                    "velocityX": 3.6572624636149413,
+                    "velocityY": -0.9669902237500647,
+                    "timestamp": 0.6754813489153929
+                },
+                {
+                    "x": 4.396643750848283,
+                    "y": 6.202643130217175,
+                    "heading": -2.870128871927858,
+                    "angularVelocity": -5.7432227678581676e-8,
+                    "velocityX": 3.657262463614942,
+                    "velocityY": -0.9669902237500649,
+                    "timestamp": 0.759916517529817
+                },
+                {
+                    "x": 4.705445323630815,
+                    "y": 6.120995147626337,
+                    "heading": -2.870128876777158,
+                    "angularVelocity": -5.743222718130824e-8,
+                    "velocityX": 3.657262463614942,
+                    "velocityY": -0.9669902237500648,
+                    "timestamp": 0.8443516861442412
+                },
+                {
+                    "x": 5.014246896413346,
+                    "y": 6.039347165035499,
+                    "heading": -2.870128881626458,
+                    "angularVelocity": -5.743222751764232e-8,
+                    "velocityX": 3.657262463614942,
+                    "velocityY": -0.9669902237500648,
+                    "timestamp": 0.9287868547586653
+                },
+                {
+                    "x": 5.323048469195878,
+                    "y": 5.957699182444661,
+                    "heading": -2.8701288864757575,
+                    "angularVelocity": -5.74322277347238e-8,
+                    "velocityX": 3.657262463614942,
+                    "velocityY": -0.9669902237500648,
+                    "timestamp": 1.0132220233730893
+                },
+                {
+                    "x": 5.631850041978409,
+                    "y": 5.876051199853823,
+                    "heading": -2.8701288913250576,
+                    "angularVelocity": -5.743222678749957e-8,
+                    "velocityX": 3.657262463614942,
+                    "velocityY": -0.9669902237500648,
+                    "timestamp": 1.0976571919875133
+                },
+                {
+                    "x": 5.940651614760942,
+                    "y": 5.7944032172629845,
+                    "heading": -2.870128896174357,
+                    "angularVelocity": -5.7432226879414726e-8,
+                    "velocityX": 3.6572624636149413,
+                    "velocityY": -0.9669902237500653,
+                    "timestamp": 1.1820923606019373
+                },
+                {
+                    "x": 6.249453187542404,
+                    "y": 5.712755234668102,
+                    "heading": -2.8701289010236573,
+                    "angularVelocity": -5.743222723099526e-8,
+                    "velocityX": 3.6572624636022777,
+                    "velocityY": -0.9669902237979594,
+                    "timestamp": 1.2665275292163614
+                },
+                {
+                    "x": 6.5582545569659105,
+                    "y": 5.631106482955165,
+                    "heading": -2.8701289058729587,
+                    "angularVelocity": -5.743224882801054e-8,
+                    "velocityX": 3.6572600551514123,
+                    "velocityY": -0.9669993327755135,
+                    "timestamp": 1.3509626978307854
+                },
+                {
+                    "x": 6.8516839479397085,
+                    "y": 5.504916165148498,
+                    "heading": -2.8701289109689694,
+                    "angularVelocity": -6.035412197409506e-8,
+                    "velocityX": 3.4752034701766337,
+                    "velocityY": -1.4945231930893996,
+                    "timestamp": 1.4353978664452094
+                },
+                {
+                    "x": 7.116648885902659,
+                    "y": 5.326539084420994,
+                    "heading": -2.8701289171366793,
+                    "angularVelocity": -7.304669371510495e-8,
+                    "velocityX": 3.1380873907283835,
+                    "velocityY": -2.1125922249539495,
+                    "timestamp": 1.5198330350596334
+                },
+                {
+                    "x": 7.343971949437644,
+                    "y": 5.1021533761176485,
+                    "heading": -2.8701292296822887,
+                    "angularVelocity": -0.0000037016046138767138,
+                    "velocityX": 2.6922793815105543,
+                    "velocityY": -2.6574910903300615,
+                    "timestamp": 1.6042682036740574
+                },
+                {
+                    "x": 7.516721651510428,
+                    "y": 4.857947803192162,
+                    "heading": -2.9165540487463932,
+                    "angularVelocity": -0.5498279902312642,
+                    "velocityX": 2.0459448936692075,
+                    "velocityY": -2.8922257980043913,
+                    "timestamp": 1.6887033722884814
+                },
+                {
+                    "x": 7.630386965757564,
+                    "y": 4.610372175146774,
+                    "heading": -2.9830505381288734,
+                    "angularVelocity": -0.7875449350512241,
+                    "velocityX": 1.3461844881981369,
+                    "velocityY": -2.932138729727077,
+                    "timestamp": 1.7731385409029055
+                },
+                {
+                    "x": 7.684751488226614,
+                    "y": 4.365674345538311,
+                    "heading": -3.0601706707306313,
+                    "angularVelocity": -0.9133650570882078,
+                    "velocityX": 0.6438611228136838,
+                    "velocityY": -2.8980557938586533,
+                    "timestamp": 1.8575737095173295
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -0.9831760605353209,
+                    "velocityX": -0.05627380515240752,
+                    "velocityY": -2.8267172252385406,
+                    "timestamp": 1.9420088781317535
+                },
+                {
+                    "x": 7.621934708150364,
+                    "y": 3.906654914920449,
+                    "heading": -3.2251079869587973,
+                    "angularVelocity": -1.0177912145669754,
+                    "velocityX": -0.7213917327302901,
+                    "velocityY": -2.737523874602733,
+                    "timestamp": 2.022499531427526
+                },
+                {
+                    "x": 7.510577058905177,
+                    "y": 3.6951455922988243,
+                    "heading": -3.30599509113983,
+                    "angularVelocity": -1.0049254275993986,
+                    "velocityX": -1.3834854692504928,
+                    "velocityY": -2.627750104653823,
+                    "timestamp": 2.102990184723298
+                },
+                {
+                    "x": 7.346480542958668,
+                    "y": 3.4951541921560745,
+                    "heading": -3.379583459687032,
+                    "angularVelocity": -0.9142473757388557,
+                    "velocityX": -2.0387027465600407,
+                    "velocityY": -2.484653707652887,
+                    "timestamp": 2.1834808380190704
+                },
+                {
+                    "x": 7.131234653225788,
+                    "y": 3.3116699228744952,
+                    "heading": -3.433702808915206,
+                    "angularVelocity": -0.6723681199262506,
+                    "velocityX": -2.67417247741705,
+                    "velocityY": -2.279572369817109,
+                    "timestamp": 2.2639714913148428
+                },
+                {
+                    "x": 6.871582405242626,
+                    "y": 3.1556914599677013,
+                    "heading": -3.4376019485164777,
+                    "angularVelocity": -0.04844214131228369,
+                    "velocityX": -3.2258683132939807,
+                    "velocityY": -1.9378456568569487,
+                    "timestamp": 2.344462144610615
+                },
+                {
+                    "x": 6.586927593692301,
+                    "y": 3.0475868041755487,
+                    "heading": -3.4376019671352136,
+                    "angularVelocity": -2.3131550136324309e-7,
+                    "velocityX": -3.5364952313696496,
+                    "velocityY": -1.3430709202335072,
+                    "timestamp": 2.4249527979063874
+                },
+                {
+                    "x": 6.287649067705066,
+                    "y": 2.9914855953111847,
+                    "heading": -3.4376019800205944,
+                    "angularVelocity": -1.6008543147444885e-7,
+                    "velocityX": -3.718177375048765,
+                    "velocityY": -0.6969903531309354,
+                    "timestamp": 2.5054434512021597
+                },
+                {
+                    "x": 5.983166666187018,
+                    "y": 2.9891511130138855,
+                    "heading": -3.437601992016166,
+                    "angularVelocity": -1.4903061473211454e-7,
+                    "velocityX": -3.7828292981942986,
+                    "velocityY": -0.029003147591768797,
+                    "timestamp": 2.585934104497932
+                },
+                {
+                    "x": 5.678681298356483,
+                    "y": 2.9910598954241565,
+                    "heading": -3.437602003982072,
+                    "angularVelocity": -1.4866205580605315e-7,
+                    "velocityX": -3.782866151075542,
+                    "velocityY": 0.02371433616337754,
+                    "timestamp": 2.6664247577937044
+                },
+                {
+                    "x": 5.374195930686038,
+                    "y": 2.9929687033714,
+                    "heading": -3.437602015947978,
+                    "angularVelocity": -1.4866205622499977e-7,
+                    "velocityX": -3.7828661490866247,
+                    "velocityY": 0.02371465342968995,
+                    "timestamp": 2.7469154110894767
+                },
+                {
+                    "x": 5.06971056314094,
+                    "y": 2.9948775312983553,
+                    "heading": -3.4376020279140773,
+                    "angularVelocity": -1.4866445697347623e-7,
+                    "velocityX": -3.7828661475293295,
+                    "velocityY": 0.023714901653687247,
+                    "timestamp": 2.827406064385249
+                },
+                {
+                    "x": 4.800808155197524,
+                    "y": 2.9965858943831902,
+                    "heading": -3.5281868157365524,
+                    "angularVelocity": -1.1254075363217084,
+                    "velocityX": -3.340790476073066,
+                    "velocityY": 0.021224365996295316,
+                    "timestamp": 2.9078967176810213
+                },
+                {
+                    "x": 4.585685258881781,
+                    "y": 2.9979519483478,
+                    "heading": -3.6006057670766456,
+                    "angularVelocity": -0.8997187670227613,
+                    "velocityX": -2.6726444314627344,
+                    "velocityY": 0.016971585006104423,
+                    "timestamp": 2.9883873709767936
+                },
+                {
+                    "x": 4.424342674038891,
+                    "y": 2.9989760866788195,
+                    "heading": -3.6548985624477908,
+                    "angularVelocity": -0.6745229805958503,
+                    "velocityX": -2.0044884497336284,
+                    "velocityY": 0.012723692616254528,
+                    "timestamp": 3.068878024272566
+                },
+                {
+                    "x": 4.316780863688961,
+                    "y": 2.9996586949819566,
+                    "heading": -3.6910886560266527,
+                    "angularVelocity": -0.44961858423356516,
+                    "velocityX": -1.33632671553406,
+                    "velocityY": 0.008480590915651792,
+                    "timestamp": 3.1493686775683383
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": -3.7091853071795864,
+                    "angularVelocity": -0.22482922441248102,
+                    "velocityX": -0.6681628423530913,
+                    "velocityY": 0.004240306222743686,
+                    "timestamp": 3.2298593308641106
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": -3.7091853071795864,
+                    "angularVelocity": 0,
+                    "velocityX": -1.1582419131838594e-33,
+                    "velocityY": 0,
+                    "timestamp": 3.310349984159883
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W1 To C3 To S4": {
+            "waypoints": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 23
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": -3.143185307179586,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 18
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": -3.7831853071795862,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.352388442136136,
+                    "y": 6.742171711440638,
+                    "heading": -2.7776238211060527,
+                    "angularVelocity": -0.340136208728432,
+                    "velocityX": 0.6450666626176952,
+                    "velocityY": -0.18258291758985748,
+                    "timestamp": 0.08121399720716439
+                },
+                {
+                    "x": 2.4572309209740357,
+                    "y": 6.712518353067644,
+                    "heading": -2.831291417681962,
+                    "angularVelocity": -0.6608170810552816,
+                    "velocityX": 1.290941000856364,
+                    "velocityY": -0.3651262022858861,
+                    "timestamp": 0.16242799441432879
+                },
+                {
+                    "x": 2.614630940945627,
+                    "y": 6.668043884281434,
+                    "heading": -2.9082589809802553,
+                    "angularVelocity": -0.9477130290857405,
+                    "velocityX": 1.9380898045256882,
+                    "velocityY": -0.5476207342969771,
+                    "timestamp": 0.2436419916214932
+                },
+                {
+                    "x": 2.824773268720236,
+                    "y": 6.608764345103305,
+                    "heading": -3.0025843751568475,
+                    "angularVelocity": -1.1614425766389054,
+                    "velocityX": 2.587513667614254,
+                    "velocityY": -0.7299177631454756,
+                    "timestamp": 0.32485598882865757
+                },
+                {
+                    "x": 3.087924526957624,
+                    "y": 6.534856635997473,
+                    "heading": -3.094100877088787,
+                    "angularVelocity": -1.1268562695832722,
+                    "velocityX": 3.2402204950799356,
+                    "velocityY": -0.9100365903255841,
+                    "timestamp": 0.40606998603582195
+                },
+                {
+                    "x": 3.383391154718911,
+                    "y": 6.450664344423451,
+                    "heading": -3.094100898976011,
+                    "angularVelocity": -2.695006379530874e-7,
+                    "velocityX": 3.638124435716056,
+                    "velocityY": -1.036672180514684,
+                    "timestamp": 0.48728398324298633
+                },
+                {
+                    "x": 3.6788577741864907,
+                    "y": 6.366472023742971,
+                    "heading": -3.0941009208630392,
+                    "angularVelocity": -2.694982209366716e-7,
+                    "velocityX": 3.638124333594396,
+                    "velocityY": -1.036672538906832,
+                    "timestamp": 0.5684979804501508
+                },
+                {
+                    "x": 3.9743243936540216,
+                    "y": 6.282279703062319,
+                    "heading": -3.094100942750067,
+                    "angularVelocity": -2.694982225892601e-7,
+                    "velocityX": 3.638124333593797,
+                    "velocityY": -1.036672538908934,
+                    "timestamp": 0.6497119776573151
+                },
+                {
+                    "x": 4.269791013121552,
+                    "y": 6.198087382381668,
+                    "heading": -3.0941009646370947,
+                    "angularVelocity": -2.694982224333276e-7,
+                    "velocityX": 3.6381243335937974,
+                    "velocityY": -1.0366725389089342,
+                    "timestamp": 0.7309259748644795
+                },
+                {
+                    "x": 4.565257632589082,
+                    "y": 6.113895061701016,
+                    "heading": -3.094100986524123,
+                    "angularVelocity": -2.694982222034585e-7,
+                    "velocityX": 3.6381243335937974,
+                    "velocityY": -1.036672538908934,
+                    "timestamp": 0.8121399720716439
+                },
+                {
+                    "x": 4.860724252056613,
+                    "y": 6.0297027410203645,
+                    "heading": -3.0941010084111507,
+                    "angularVelocity": -2.694982230736536e-7,
+                    "velocityX": 3.638124333593797,
+                    "velocityY": -1.0366725389089342,
+                    "timestamp": 0.8933539692788083
+                },
+                {
+                    "x": 5.156190871524144,
+                    "y": 5.945510420339713,
+                    "heading": -3.094101030298179,
+                    "angularVelocity": -2.6949822300117995e-7,
+                    "velocityX": 3.638124333593797,
+                    "velocityY": -1.0366725389089342,
+                    "timestamp": 0.9745679664859727
+                },
+                {
+                    "x": 5.451657490991675,
+                    "y": 5.861318099659061,
+                    "heading": -3.0941010521852066,
+                    "angularVelocity": -2.694982228196196e-7,
+                    "velocityX": 3.638124333593797,
+                    "velocityY": -1.0366725389089342,
+                    "timestamp": 1.0557819636931371
+                },
+                {
+                    "x": 5.747124110459206,
+                    "y": 5.77712577897841,
+                    "heading": -3.0941010740722343,
+                    "angularVelocity": -2.694982236260836e-7,
+                    "velocityX": 3.638124333593797,
+                    "velocityY": -1.0366725389089344,
+                    "timestamp": 1.1369959609003015
+                },
+                {
+                    "x": 6.042590729926668,
+                    "y": 5.69293345829752,
+                    "heading": -3.0941010959592625,
+                    "angularVelocity": -2.694982236351797e-7,
+                    "velocityX": 3.6381243335929603,
+                    "velocityY": -1.0366725389118696,
+                    "timestamp": 1.218209958107466
+                },
+                {
+                    "x": 6.338057337809956,
+                    "y": 5.6087410969628495,
+                    "heading": -3.0941011178462903,
+                    "angularVelocity": -2.6949822192516605e-7,
+                    "velocityX": 3.6381241909552937,
+                    "velocityY": -1.0366730394879038,
+                    "timestamp": 1.2994239553146303
+                },
+                {
+                    "x": 6.63151320525297,
+                    "y": 5.517787680923723,
+                    "heading": -3.0941011398106846,
+                    "angularVelocity": -2.704508467508134e-7,
+                    "velocityX": 3.6133656455032592,
+                    "velocityY": -1.1199229094611813,
+                    "timestamp": 1.3806379525217947
+                },
+                {
+                    "x": 6.904052184030547,
+                    "y": 5.375973104754023,
+                    "heading": -3.09410116345768,
+                    "angularVelocity": -2.911689606888355e-7,
+                    "velocityX": 3.355812891235069,
+                    "velocityY": -1.7461839220887416,
+                    "timestamp": 1.461851949728959
+                },
+                {
+                    "x": 7.14694076900972,
+                    "y": 5.1878401702780765,
+                    "heading": -3.094101192989132,
+                    "angularVelocity": -3.636251518218712e-7,
+                    "velocityX": 2.9907231921877617,
+                    "velocityY": -2.3165087416877084,
+                    "timestamp": 1.5430659469361234
+                },
+                {
+                    "x": 7.352395911467515,
+                    "y": 4.959417222897554,
+                    "heading": -3.0941012381435535,
+                    "angularVelocity": -5.559930935407798e-7,
+                    "velocityX": 2.529799659188236,
+                    "velocityY": -2.812605649731368,
+                    "timestamp": 1.6242799441432878
+                },
+                {
+                    "x": 7.513833646479666,
+                    "y": 4.698024306067166,
+                    "heading": -3.094103186189668,
+                    "angularVelocity": -0.00002398658095119272,
+                    "velocityX": 1.9878067889975632,
+                    "velocityY": -3.2185697764010213,
+                    "timestamp": 1.7054939413504522
+                },
+                {
+                    "x": 7.6238137890728055,
+                    "y": 4.417795537290539,
+                    "heading": -3.1108263182271356,
+                    "angularVelocity": -0.20591440655693208,
+                    "velocityX": 1.35420181708456,
+                    "velocityY": -3.4504984166977444,
+                    "timestamp": 1.7867079385576166
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -0.3984410331631951,
+                    "velocityX": 0.6918291533072239,
+                    "velocityY": -3.5806086055413933,
+                    "timestamp": 1.867921935764781
+                },
+                {
+                    "x": 7.6870591404547595,
+                    "y": 3.8674277894608684,
+                    "heading": -3.1773151030340356,
+                    "angularVelocity": -0.47768106939784993,
+                    "velocityX": 0.09879982223438938,
+                    "velocityY": -3.63297605573316,
+                    "timestamp": 1.9393708543807355
+                },
+                {
+                    "x": 7.651588574491949,
+                    "y": 3.608358714127091,
+                    "heading": -3.205456863771594,
+                    "angularVelocity": -0.39387245154381195,
+                    "velocityX": -0.4964465053735158,
+                    "velocityY": -3.625934168821804,
+                    "timestamp": 2.0108197729966903
+                },
+                {
+                    "x": 7.574047848742119,
+                    "y": 3.354318466237311,
+                    "heading": -3.2189736743217594,
+                    "angularVelocity": -0.1891814573314478,
+                    "velocityX": -1.0852610123268396,
+                    "velocityY": -3.5555506341084104,
+                    "timestamp": 2.082268691612645
+                },
+                {
+                    "x": 7.455586952035965,
+                    "y": 3.1113741995114603,
+                    "heading": -3.2189745116430255,
+                    "angularVelocity": -0.00001171915938609546,
+                    "velocityX": -1.6579802605551721,
+                    "velocityY": -3.4002511365232904,
+                    "timestamp": 2.1537176102285995
+                },
+                {
+                    "x": 7.300453960582251,
+                    "y": 2.8900400447662866,
+                    "heading": -3.2189745885172623,
+                    "angularVelocity": -0.0000010759328215916604,
+                    "velocityX": -2.171243378574256,
+                    "velocityY": -3.097795726435581,
+                    "timestamp": 2.225166528844554
+                },
+                {
+                    "x": 7.11249649568723,
+                    "y": 2.6958055218428347,
+                    "heading": -3.2189746440183837,
+                    "angularVelocity": -7.767944182730611e-7,
+                    "velocityX": -2.6306551384378265,
+                    "velocityY": -2.718508924810283,
+                    "timestamp": 2.2966154474605087
+                },
+                {
+                    "x": 6.896376183100963,
+                    "y": 2.533487712060768,
+                    "heading": -3.2189746912030057,
+                    "angularVelocity": -6.603965892997474e-7,
+                    "velocityX": -3.024822723297278,
+                    "velocityY": -2.2718021899927887,
+                    "timestamp": 2.3680643660764633
+                },
+                {
+                    "x": 6.6576888713401265,
+                    "y": 2.4066672511412732,
+                    "heading": -3.218974736751788,
+                    "angularVelocity": -6.375013452668414e-7,
+                    "velocityX": -3.340670739366957,
+                    "velocityY": -1.7749808302282493,
+                    "timestamp": 2.439513284692418
+                },
+                {
+                    "x": 6.418991689499422,
+                    "y": 2.2798653705517777,
+                    "heading": -3.2189747823005193,
+                    "angularVelocity": -6.375006445255001e-7,
+                    "velocityX": -3.3408088808494862,
+                    "velocityY": -1.7747207802229084,
+                    "timestamp": 2.5109622033083725
+                },
+                {
+                    "x": 6.180294507575015,
+                    "y": 2.1530634901198433,
+                    "heading": -3.2189748278492507,
+                    "angularVelocity": -6.375006422464704e-7,
+                    "velocityX": -3.340808882020978,
+                    "velocityY": -1.7747207780176886,
+                    "timestamp": 2.582411121924327
+                },
+                {
+                    "x": 5.941597302835271,
+                    "y": 2.026261652636718,
+                    "heading": -3.21897487339828,
+                    "angularVelocity": -6.375048107159433e-7,
+                    "velocityX": -3.3408092013447153,
+                    "velocityY": -1.774720176905592,
+                    "timestamp": 2.6538600405402817
+                },
+                {
+                    "x": 5.72404161684181,
+                    "y": 1.9132773125800981,
+                    "heading": -3.2812904960777876,
+                    "angularVelocity": -0.8721702705344045,
+                    "velocityX": -3.0449122283008005,
+                    "velocityY": -1.5813303020905036,
+                    "timestamp": 2.7253089591562363
+                },
+                {
+                    "x": 5.540083719687043,
+                    "y": 1.8151536354630486,
+                    "heading": -3.418088909630078,
+                    "angularVelocity": -1.9146323863713337,
+                    "velocityX": -2.574677136023417,
+                    "velocityY": -1.3733402691069942,
+                    "timestamp": 2.796757877772191
+                },
+                {
+                    "x": 5.393751164472601,
+                    "y": 1.7369217513952075,
+                    "heading": -3.5523873708884453,
+                    "angularVelocity": -1.8796430213546256,
+                    "velocityX": -2.048072357826006,
+                    "velocityY": -1.0949344732596382,
+                    "timestamp": 2.8682067963881455
+                },
+                {
+                    "x": 5.284579981216215,
+                    "y": 1.6784003858088632,
+                    "heading": -3.6634662967687484,
+                    "angularVelocity": -1.554662100328298,
+                    "velocityX": -1.5279613095723332,
+                    "velocityY": -0.8190657986344941,
+                    "timestamp": 2.9396557150041
+                },
+                {
+                    "x": 5.21211226324901,
+                    "y": 1.6394523869599669,
+                    "heading": -3.742196708881707,
+                    "angularVelocity": -1.1019118782914785,
+                    "velocityX": -1.0142591290490286,
+                    "velocityY": -0.5451167016141149,
+                    "timestamp": 3.0111046336200546
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": -3.7831853071795862,
+                    "angularVelocity": -0.5736769582023385,
+                    "velocityX": -0.505427709031773,
+                    "velocityY": -0.2722558624712981,
+                    "timestamp": 3.0825535522360092
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": -3.7831853071795862,
+                    "angularVelocity": 0,
+                    "velocityX": -1.863217485402065e-29,
+                    "velocityY": -1.5751272024067454e-29,
+                    "timestamp": 3.154002470851964
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W2 To W1": {
+            "waypoints": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 10
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 3.5331853071795862,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": -6.980040471932584e-34,
+                    "velocityX": -4.489498521627733e-35,
+                    "velocityY": 1.1490136335521518e-35,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.252002036509344,
+                    "y": 5.607878031980728,
+                    "heading": 3.1558282188080855,
+                    "angularVelocity": 0.20834807697002747,
+                    "velocityX": 0.026352962503434416,
+                    "velocityY": 0.6302222639984295,
+                    "timestamp": 0.07597007391799691
+                },
+                {
+                    "x": 2.2560056075028405,
+                    "y": 5.703634203308701,
+                    "heading": 3.187479727503087,
+                    "angularVelocity": 0.41663127416680434,
+                    "velocityX": 0.05269931681016954,
+                    "velocityY": 1.260445941270677,
+                    "timestamp": 0.15194014783599383
+                },
+                {
+                    "x": 2.262010006101527,
+                    "y": 5.847268776725278,
+                    "heading": 3.2349419015403043,
+                    "angularVelocity": 0.6247482934984211,
+                    "velocityX": 0.07903636641406601,
+                    "velocityY": 1.8906730770279163,
+                    "timestamp": 0.22791022175399073
+                },
+                {
+                    "x": 2.2700144000703353,
+                    "y": 6.038782280998214,
+                    "heading": 3.298188503077313,
+                    "angularVelocity": 0.8325199420666319,
+                    "velocityX": 0.105362461242945,
+                    "velocityY": 2.520907172996284,
+                    "timestamp": 0.30388029567198765
+                },
+                {
+                    "x": 2.280009617993533,
+                    "y": 6.278187235561798,
+                    "heading": 3.376569762958043,
+                    "angularVelocity": 1.0317386286254728,
+                    "velocityX": 0.13156783201220376,
+                    "velocityY": 3.151306063253281,
+                    "timestamp": 0.3798503695899846
+                },
+                {
+                    "x": 2.28800573619677,
+                    "y": 6.469712012711673,
+                    "heading": 3.4392336798072036,
+                    "angularVelocity": 0.8248500181374163,
+                    "velocityX": 0.10525352669615307,
+                    "velocityY": 2.5210555587534333,
+                    "timestamp": 0.4558204435079815
+                },
+                {
+                    "x": 2.2940028560112626,
+                    "y": 6.613355954230437,
+                    "heading": 3.486212722152842,
+                    "angularVelocity": 0.6183887934128925,
+                    "velocityX": 0.07894055521080842,
+                    "velocityY": 1.890796390086646,
+                    "timestamp": 0.5317905174259784
+                },
+                {
+                    "x": 2.2980009548636073,
+                    "y": 6.709118670121978,
+                    "heading": 3.517527021603037,
+                    "angularVelocity": 0.41219256261348647,
+                    "velocityX": 0.052627286589986035,
+                    "velocityY": 1.2605320878706525,
+                    "timestamp": 0.6077605913439753
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 3.5331853071795862,
+                    "angularVelocity": 0.20611123260786593,
+                    "velocityX": 0.02631358682828647,
+                    "velocityY": 0.6302656744773738,
+                    "timestamp": 0.6837306652619722
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 3.5331853071795862,
+                    "angularVelocity": 2.252106897096389e-33,
+                    "velocityX": 0,
+                    "velocityY": 2.1844888905645107e-35,
+                    "timestamp": 0.7597007391799692
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W2 To W3": {
+            "waypoints": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 10
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": -7.990908752759601e-36,
+                    "velocityX": 0,
+                    "velocityY": 3.147004019802851e-35,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.252001928264259,
+                    "y": 5.512001885302468,
+                    "heading": 3.1243023137364228,
+                    "angularVelocity": -0.2063808903009179,
+                    "velocityX": 0.026319785639686007,
+                    "velocityY": -0.6310416374562365,
+                    "timestamp": 0.07606172374142242
+                },
+                {
+                    "x": 2.2560053075353443,
+                    "y": 5.416005550394565,
+                    "heading": 3.0929118769402844,
+                    "angularVelocity": -0.41269688947430483,
+                    "velocityX": 0.052633296672250224,
+                    "velocityY": -1.2620846621127049,
+                    "timestamp": 0.15212344748284484
+                },
+                {
+                    "x": 2.262009466656741,
+                    "y": 5.272010740000972,
+                    "heading": 3.0458411253223887,
+                    "angularVelocity": -0.6188493936571329,
+                    "velocityX": 0.0789379838643735,
+                    "velocityY": -1.8931310429292179,
+                    "timestamp": 0.22818517122426726
+                },
+                {
+                    "x": 2.2700136159756155,
+                    "y": 5.080016943812723,
+                    "heading": 2.983115684252823,
+                    "angularVelocity": -0.8246649955344898,
+                    "velocityX": 0.10523228931919847,
+                    "velocityY": -2.524184132888531,
+                    "timestamp": 0.3042468949656897
+                },
+                {
+                    "x": 2.280009094575526,
+                    "y": 4.840012208292782,
+                    "heading": 2.9053630904797005,
+                    "angularVelocity": -1.0222302355051571,
+                    "velocityX": 0.1314127278247149,
+                    "velocityY": -3.155394378595143,
+                    "timestamp": 0.3803086187071121
+                },
+                {
+                    "x": 2.2880054239679986,
+                    "y": 4.64800763986969,
+                    "heading": 2.8432007503387635,
+                    "angularVelocity": -0.8172617853397972,
+                    "velocityX": 0.10512947904857922,
+                    "velocityY": -2.524325757799354,
+                    "timestamp": 0.45637034244853447
+                },
+                {
+                    "x": 2.294002700573331,
+                    "y": 4.504003870137798,
+                    "heading": 2.7965972482938968,
+                    "angularVelocity": -0.6127063620501001,
+                    "velocityX": 0.07884749793103833,
+                    "velocityY": -1.8932488332954869,
+                    "timestamp": 0.5324320661899569
+                },
+                {
+                    "x": 2.2980009029345383,
+                    "y": 4.408001272313515,
+                    "heading": 2.7655331423967997,
+                    "angularVelocity": -0.40840654627682993,
+                    "velocityX": 0.05256523471383641,
+                    "velocityY": -1.2621670020344438,
+                    "timestamp": 0.6084937899313793
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": -0.20421759635116843,
+                    "velocityX": 0.026282563254256014,
+                    "velocityY": -0.631083151319306,
+                    "timestamp": 0.6845555136728017
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": -4.352426400818003e-34,
+                    "velocityX": -2.6471819427760516e-34,
+                    "velocityY": 2.5922629666231517e-36,
+                    "timestamp": 0.760617237414224
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W2 To C1 To S1": {
+            "waypoints": [
+                {
+                    "x": 2.329045295715332,
+                    "y": 5.559675216674805,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 20
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 5.233179569244385,
+                    "y": 6.553707599639893,
+                    "heading": 3.4031853071795863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.329045295715332,
+                    "y": 5.559675216674805,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.4000279527631365,
+                    "y": 5.585359375509262,
+                    "heading": 3.1338545708828756,
+                    "angularVelocity": -0.06457961815077011,
+                    "velocityX": 0.745925597725292,
+                    "velocityY": 0.2699035557060694,
+                    "timestamp": 0.09516050563794304
+                },
+                {
+                    "x": 2.5419932646198635,
+                    "y": 5.636727692199652,
+                    "heading": 3.1215638736263545,
+                    "angularVelocity": -0.12915754465735657,
+                    "velocityX": 1.4918511719231908,
+                    "velocityY": 0.539807101129259,
+                    "timestamp": 0.19032101127588608
+                },
+                {
+                    "x": 2.7549412265998368,
+                    "y": 5.713780164921229,
+                    "heading": 3.103128182719956,
+                    "angularVelocity": -0.1937325866743065,
+                    "velocityX": 2.2377766968814634,
+                    "velocityY": 0.8097106273770546,
+                    "timestamp": 0.2854815169138291
+                },
+                {
+                    "x": 3.038871822724849,
+                    "y": 5.81651678795237,
+                    "heading": 3.078547903585146,
+                    "angularVelocity": -0.258303368295786,
+                    "velocityX": 2.9837020539320758,
+                    "velocityY": 1.0796140934988998,
+                    "timestamp": 0.38064202255177215
+                },
+                {
+                    "x": 3.3773799452774673,
+                    "y": 5.939001713582426,
+                    "heading": 3.0785479006839798,
+                    "angularVelocity": -3.048712788248211e-8,
+                    "velocityX": 3.5572333320775313,
+                    "velocityY": 1.2871403405116655,
+                    "timestamp": 0.4758025281897152
+                },
+                {
+                    "x": 3.715888067817609,
+                    "y": 6.0614866392475255,
+                    "heading": 3.0785478977827805,
+                    "angularVelocity": -3.0487407415354604e-8,
+                    "velocityX": 3.5572333319465472,
+                    "velocityY": 1.287140340879956,
+                    "timestamp": 0.5709630338276582
+                },
+                {
+                    "x": 4.054396190357754,
+                    "y": 6.183971564912614,
+                    "heading": 3.0785478948815816,
+                    "angularVelocity": -3.0487407316957414e-8,
+                    "velocityX": 3.5572333319465868,
+                    "velocityY": 1.28714034087985,
+                    "timestamp": 0.6661235394656013
+                },
+                {
+                    "x": 4.392904312897899,
+                    "y": 6.306456490577704,
+                    "heading": 3.0785478919803824,
+                    "angularVelocity": -3.048740785433711e-8,
+                    "velocityX": 3.557233331946584,
+                    "velocityY": 1.287140340879858,
+                    "timestamp": 0.7612840451035443
+                },
+                {
+                    "x": 4.7314124354380445,
+                    "y": 6.428941416242792,
+                    "heading": 3.0785478890791835,
+                    "angularVelocity": -3.0487408354838206e-8,
+                    "velocityX": 3.557233331946584,
+                    "velocityY": 1.2871403408798578,
+                    "timestamp": 0.8564445507414873
+                },
+                {
+                    "x": 5.06992055797819,
+                    "y": 6.551426341907881,
+                    "heading": 3.0785478861779842,
+                    "angularVelocity": -3.04874074079248e-8,
+                    "velocityX": 3.557233331946584,
+                    "velocityY": 1.2871403408798578,
+                    "timestamp": 0.9516050563794303
+                },
+                {
+                    "x": 5.408428680518335,
+                    "y": 6.67391126757297,
+                    "heading": 3.0785478832767854,
+                    "angularVelocity": -3.048740819605906e-8,
+                    "velocityX": 3.557233331946584,
+                    "velocityY": 1.2871403408798576,
+                    "timestamp": 1.0467655620173733
+                },
+                {
+                    "x": 5.746936803058481,
+                    "y": 6.796396193238057,
+                    "heading": 3.078547880375586,
+                    "angularVelocity": -3.0487408175601846e-8,
+                    "velocityX": 3.5572333319465934,
+                    "velocityY": 1.2871403408798332,
+                    "timestamp": 1.1419260676553162
+                },
+                {
+                    "x": 6.085444925598784,
+                    "y": 6.918881118902711,
+                    "heading": 3.0785478774743873,
+                    "angularVelocity": -3.0487407710602e-8,
+                    "velocityX": 3.55723333194824,
+                    "velocityY": 1.2871403408752817,
+                    "timestamp": 1.2370865732932592
+                },
+                {
+                    "x": 6.4239530481377205,
+                    "y": 7.041366044571132,
+                    "heading": 3.078547874573188,
+                    "angularVelocity": -3.0487407747049345e-8,
+                    "velocityX": 3.55723333193393,
+                    "velocityY": 1.287140340914726,
+                    "timestamp": 1.3322470789312022
+                },
+                {
+                    "x": 6.762461177860979,
+                    "y": 7.163850950384819,
+                    "heading": 3.078547871671963,
+                    "angularVelocity": -3.0487683312908684e-8,
+                    "velocityX": 3.5572334074304637,
+                    "velocityY": 1.2871401322709841,
+                    "timestamp": 1.4274075845691452
+                },
+                {
+                    "x": 7.087934228336467,
+                    "y": 7.275849077547301,
+                    "heading": 3.0785482003586213,
+                    "angularVelocity": 0.000003454027869296417,
+                    "velocityX": 3.420253479039086,
+                    "velocityY": 1.1769391767257837,
+                    "timestamp": 1.5225680902070882
+                },
+                {
+                    "x": 7.34242464824946,
+                    "y": 7.362163048068745,
+                    "heading": 3.0846933316902727,
+                    "angularVelocity": 0.06457648885540108,
+                    "velocityX": 2.6743281596359925,
+                    "velocityY": 0.9070356440694888,
+                    "timestamp": 1.6177285958450311
+                },
+                {
+                    "x": 7.525932419508426,
+                    "y": 7.422792858981067,
+                    "heading": 3.0969834578057935,
+                    "angularVelocity": 0.12915154278707255,
+                    "velocityX": 1.9284026501171228,
+                    "velocityY": 0.6371320802228948,
+                    "timestamp": 1.7128891014829741
+                },
+                {
+                    "x": 7.638457537165259,
+                    "y": 7.457738509668789,
+                    "heading": 3.1154188790536077,
+                    "angularVelocity": 0.1937297529488966,
+                    "velocityX": 1.1824770886008305,
+                    "velocityY": 0.3672285099084715,
+                    "timestamp": 1.8080496071209171
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "angularVelocity": 0.2583122145204678,
+                    "velocityX": 0.4365515142645689,
+                    "velocityY": 0.09732493820788266,
+                    "timestamp": 1.90321011275886
+                },
+                {
+                    "x": 7.653654383343035,
+                    "y": 7.451878922022455,
+                    "heading": 3.169571245622926,
+                    "angularVelocity": 0.3208641076607671,
+                    "velocityX": -0.28586427797403835,
+                    "velocityY": -0.16407192492517308,
+                    "timestamp": 1.9953713888811428
+                },
+                {
+                    "x": 7.560730009307251,
+                    "y": 7.412667176131766,
+                    "heading": 3.204907797708199,
+                    "angularVelocity": 0.3834208202531959,
+                    "velocityX": -1.0082800276276813,
+                    "velocityY": -0.42546878190023074,
+                    "timestamp": 2.0875326650034256
+                },
+                {
+                    "x": 7.4012268833190475,
+                    "y": 7.349364762932986,
+                    "heading": 3.2460101421492427,
+                    "angularVelocity": 0.4459828050518832,
+                    "velocityX": -1.7306957184018388,
+                    "velocityY": -0.6868656323100164,
+                    "timestamp": 2.1796939411257084
+                },
+                {
+                    "x": 7.175145013744201,
+                    "y": 7.261971683800118,
+                    "heading": 3.292878761166739,
+                    "angularVelocity": 0.5085500222039014,
+                    "velocityX": -2.4531113184025837,
+                    "velocityY": -0.9482624678108277,
+                    "timestamp": 2.271855217247991
+                },
+                {
+                    "x": 6.8824844194459525,
+                    "y": 7.150487945420426,
+                    "heading": 3.3455140401053294,
+                    "angularVelocity": 0.5711214205450821,
+                    "velocityX": -3.1755267137264047,
+                    "velocityY": -1.2096592307504261,
+                    "timestamp": 2.364016493370274
+                },
+                {
+                    "x": 6.55464527272117,
+                    "y": 7.031863403145239,
+                    "heading": 3.3455140429153563,
+                    "angularVelocity": 3.049000281588377e-8,
+                    "velocityX": -3.557233151692005,
+                    "velocityY": -1.2871408390477628,
+                    "timestamp": 2.4561777694925566
+                },
+                {
+                    "x": 6.226806115184277,
+                    "y": 6.913238890751769,
+                    "heading": 3.3455140457253494,
+                    "angularVelocity": 3.0489828937045393e-8,
+                    "velocityX": -3.557233269011331,
+                    "velocityY": -1.287140514808609,
+                    "timestamp": 2.5483390456148394
+                },
+                {
+                    "x": 5.898966957623017,
+                    "y": 6.79461437842809,
+                    "heading": 3.345514048537748,
+                    "angularVelocity": 3.0514995916000634e-8,
+                    "velocityX": -3.557233269277337,
+                    "velocityY": -1.287140514052891,
+                    "timestamp": 2.640500321737122
+                },
+                {
+                    "x": 5.632652017742471,
+                    "y": 6.698251673595034,
+                    "heading": 3.3685823941434916,
+                    "angularVelocity": 0.2503041035574726,
+                    "velocityX": -2.889662026024368,
+                    "velocityY": -1.045587787926248,
+                    "timestamp": 2.732661597859405
+                },
+                {
+                    "x": 5.432915797517009,
+                    "y": 6.625979638346594,
+                    "heading": 3.3858838135535563,
+                    "angularVelocity": 0.18772981600595268,
+                    "velocityX": -2.167246685696753,
+                    "velocityY": -0.7841909128155133,
+                    "timestamp": 2.8248228739816876
+                },
+                {
+                    "x": 5.299758312935604,
+                    "y": 6.577798279546128,
+                    "heading": 3.3974181385441695,
+                    "angularVelocity": 0.1251537031151884,
+                    "velocityX": -1.4448311718780615,
+                    "velocityY": -0.5227939632329148,
+                    "timestamp": 2.9169841501039704
+                },
+                {
+                    "x": 5.233179569244385,
+                    "y": 6.553707599639893,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": 0.06257691818239948,
+                    "velocityX": -0.7224156011355166,
+                    "velocityY": -0.2613969871068907,
+                    "timestamp": 3.009145426226253
+                },
+                {
+                    "x": 5.233179569244385,
+                    "y": 6.553707599639893,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": 0,
+                    "velocityX": 1.5847972099366162e-31,
+                    "velocityY": 0,
+                    "timestamp": 3.101306702348536
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W2 To C1 To S2": {
+            "waypoints": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 20
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.3199317069050815,
+                    "y": 5.589221584324863,
+                    "heading": 3.14,
+                    "angularVelocity": -4.40794531804863e-17,
+                    "velocityX": 0.7334971919039711,
+                    "velocityY": 0.3064983109073157,
+                    "timestamp": 0.09534011537734256
+                },
+                {
+                    "x": 2.4597951178135684,
+                    "y": 5.647664751762101,
+                    "heading": 3.14,
+                    "angularVelocity": -2.0051811551543008e-16,
+                    "velocityX": 1.4669943533729506,
+                    "velocityY": 0.6129966090971003,
+                    "timestamp": 0.19068023075468513
+                },
+                {
+                    "x": 2.6695902269221126,
+                    "y": 5.73532949988673,
+                    "heading": 3.14,
+                    "angularVelocity": -1.6298734307861892e-16,
+                    "velocityX": 2.200491453971973,
+                    "velocityY": 0.9194948818518327,
+                    "timestamp": 0.2860203461320277
+                },
+                {
+                    "x": 2.9493170168206886,
+                    "y": 5.852215821423819,
+                    "heading": 3.14,
+                    "angularVelocity": -2.718821323245932e-16,
+                    "velocityX": 2.933988371961343,
+                    "velocityY": 1.2259930783015016,
+                    "timestamp": 0.38136046150937025
+                },
+                {
+                    "x": 3.2820984406189933,
+                    "y": 5.991271492558365,
+                    "heading": 3.14,
+                    "angularVelocity": -1.791056538237388e-18,
+                    "velocityX": 3.4904659227776635,
+                    "velocityY": 1.45852216125588,
+                    "timestamp": 0.4767005768867128
+                },
+                {
+                    "x": 3.6148798644173543,
+                    "y": 6.130327163692935,
+                    "heading": 3.14,
+                    "angularVelocity": 2.7598941425139603e-16,
+                    "velocityX": 3.4904659227782475,
+                    "velocityY": 1.4585221612561248,
+                    "timestamp": 0.5720406922640554
+                },
+                {
+                    "x": 3.947661288215715,
+                    "y": 6.269382834827505,
+                    "heading": 3.14,
+                    "angularVelocity": -1.359472794095135e-16,
+                    "velocityX": 3.4904659227782475,
+                    "velocityY": 1.4585221612561248,
+                    "timestamp": 0.667380807641398
+                },
+                {
+                    "x": 4.280442712014075,
+                    "y": 6.408438505962075,
+                    "heading": 3.14,
+                    "angularVelocity": -3.500332184235888e-16,
+                    "velocityX": 3.4904659227782475,
+                    "velocityY": 1.4585221612561245,
+                    "timestamp": 0.7627209230187405
+                },
+                {
+                    "x": 4.613224135812436,
+                    "y": 6.547494177096645,
+                    "heading": 3.14,
+                    "angularVelocity": -3.2849392470850763e-16,
+                    "velocityX": 3.4904659227782475,
+                    "velocityY": 1.4585221612561245,
+                    "timestamp": 0.858061038396083
+                },
+                {
+                    "x": 4.9460055596107955,
+                    "y": 6.686549848231215,
+                    "heading": 3.14,
+                    "angularVelocity": -3.264382961483678e-16,
+                    "velocityX": 3.4904659227782475,
+                    "velocityY": 1.4585221612561245,
+                    "timestamp": 0.9534011537734256
+                },
+                {
+                    "x": 5.278786983409157,
+                    "y": 6.825605519365785,
+                    "heading": 3.14,
+                    "angularVelocity": -2.8870332355952577e-16,
+                    "velocityX": 3.4904659227782475,
+                    "velocityY": 1.4585221612561245,
+                    "timestamp": 1.0487412691507683
+                },
+                {
+                    "x": 5.611568407207518,
+                    "y": 6.9646611905003555,
+                    "heading": 3.14,
+                    "angularVelocity": -2.714509339961268e-16,
+                    "velocityX": 3.4904659227782475,
+                    "velocityY": 1.4585221612561245,
+                    "timestamp": 1.1440813845281108
+                },
+                {
+                    "x": 5.944349831005879,
+                    "y": 7.1037168616349256,
+                    "heading": 3.14,
+                    "angularVelocity": 3.236789006032357e-16,
+                    "velocityX": 3.4904659227782475,
+                    "velocityY": 1.4585221612561248,
+                    "timestamp": 1.2394214999054534
+                },
+                {
+                    "x": 6.2771312548052975,
+                    "y": 7.242772532766961,
+                    "heading": 3.14,
+                    "angularVelocity": 1.392859578179056e-16,
+                    "velocityX": 3.4904659227893537,
+                    "velocityY": 1.458522161229545,
+                    "timestamp": 1.334761615282796
+                },
+                {
+                    "x": 6.609912972036511,
+                    "y": 7.381827501669921,
+                    "heading": 3.14,
+                    "angularVelocity": 7.280552315335093e-17,
+                    "velocityX": 3.4904690005262737,
+                    "velocityY": 1.4585147957142848,
+                    "timestamp": 1.4301017306601385
+                },
+                {
+                    "x": 6.961412726724582,
+                    "y": 7.462622346034388,
+                    "heading": 3.14,
+                    "angularVelocity": -1.0172483638053557e-16,
+                    "velocityX": 3.6867980838588856,
+                    "velocityY": 0.8474380804417183,
+                    "timestamp": 1.525441846037481
+                },
+                {
+                    "x": 7.244997116389008,
+                    "y": 7.50977433975784,
+                    "heading": 3.14,
+                    "angularVelocity": -1.1360407734774875e-16,
+                    "velocityX": 2.974449826728655,
+                    "velocityY": 0.4945661491684764,
+                    "timestamp": 1.6207819614148236
+                },
+                {
+                    "x": 7.459465375503974,
+                    "y": 7.525824834495841,
+                    "heading": 3.14,
+                    "angularVelocity": -6.302038440751775e-17,
+                    "velocityX": 2.249507023000035,
+                    "velocityY": 0.16834985645314604,
+                    "timestamp": 1.716122076792166
+                },
+                {
+                    "x": 7.6045126721499186,
+                    "y": 7.511460301300605,
+                    "heading": 3.14,
+                    "angularVelocity": 8.943638136140749e-17,
+                    "velocityX": 1.5213669091113222,
+                    "velocityY": -0.15066620318617746,
+                    "timestamp": 1.8114621921695087
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "angularVelocity": 1.2339640787414218e-16,
+                    "velocityX": 0.7917687906220042,
+                    "velocityY": -0.4663336217356026,
+                    "timestamp": 1.9068023075468512
+                },
+                {
+                    "x": 7.680079036283902,
+                    "y": 7.383775130350334,
+                    "heading": 3.14,
+                    "angularVelocity": 3.1298666285147066e-16,
+                    "velocityX": 0.0007655130286299905,
+                    "velocityY": -0.8060819521991853,
+                    "timestamp": 2.010048471814154
+                },
+                {
+                    "x": 7.598307382340287,
+                    "y": 7.265900859151241,
+                    "heading": 3.14,
+                    "angularVelocity": 4.0403721611171334e-16,
+                    "velocityX": -0.792006700916374,
+                    "velocityY": -1.141681843927082,
+                    "timestamp": 2.113294636081456
+                },
+                {
+                    "x": 7.434354633787184,
+                    "y": 7.114168304333743,
+                    "heading": 3.14,
+                    "angularVelocity": 4.565397791635157e-16,
+                    "velocityX": -1.5879790761875838,
+                    "velocityY": -1.4696192918573159,
+                    "timestamp": 2.2165408003487586
+                },
+                {
+                    "x": 7.187443357098086,
+                    "y": 6.930529696196354,
+                    "heading": 3.14,
+                    "angularVelocity": 3.88515705379733e-16,
+                    "velocityX": -2.3914813537270896,
+                    "velocityY": -1.7786482378362456,
+                    "timestamp": 2.319786964616061
+                },
+                {
+                    "x": 6.853793099378443,
+                    "y": 6.7274887650449,
+                    "heading": 3.14,
+                    "angularVelocity": -3.674797460353125e-16,
+                    "velocityX": -3.2315995474255925,
+                    "velocityY": -1.9665711805601358,
+                    "timestamp": 2.4230331288833633
+                },
+                {
+                    "x": 6.49770423144866,
+                    "y": 6.567024283806904,
+                    "heading": 3.14,
+                    "angularVelocity": 2.249424709718662e-16,
+                    "velocityX": -3.448930722577495,
+                    "velocityY": -1.5541931497092332,
+                    "timestamp": 2.5262792931506657
+                },
+                {
+                    "x": 6.1416152766915,
+                    "y": 6.406559995248834,
+                    "heading": 3.14,
+                    "angularVelocity": 2.526804739134875e-16,
+                    "velocityX": -3.4489315635518585,
+                    "velocityY": -1.5541912834905027,
+                    "timestamp": 2.629525457417968
+                },
+                {
+                    "x": 5.785526321934093,
+                    "y": 6.246095706691313,
+                    "heading": 3.14,
+                    "angularVelocity": 2.2036218085246878e-16,
+                    "velocityX": -3.4489315635542535,
+                    "velocityY": -1.5541912834851872,
+                    "timestamp": 2.7327716216852704
+                },
+                {
+                    "x": 5.429437367176686,
+                    "y": 6.085631418133792,
+                    "heading": 3.14,
+                    "angularVelocity": 7.127301298549232e-19,
+                    "velocityX": -3.4489315635542535,
+                    "velocityY": -1.5541912834851876,
+                    "timestamp": 2.836017785952573
+                },
+                {
+                    "x": 5.07334841241929,
+                    "y": 5.925167129576275,
+                    "heading": 3.14,
+                    "angularVelocity": 1.0539129254381463e-16,
+                    "velocityX": -3.4489315635541473,
+                    "velocityY": -1.5541912834851401,
+                    "timestamp": 2.939263950219875
+                },
+                {
+                    "x": 4.749209064120322,
+                    "y": 5.7791002852572,
+                    "heading": 3.14,
+                    "angularVelocity": 8.727825696429895e-16,
+                    "velocityX": -3.1394807797389603,
+                    "velocityY": -1.4147435437980136,
+                    "timestamp": 3.0425101144871776
+                },
+                {
+                    "x": 4.506104536371044,
+                    "y": 5.669550144571212,
+                    "heading": 3.14,
+                    "angularVelocity": 4.949097028202336e-16,
+                    "velocityX": -2.3546107448590976,
+                    "velocityY": -1.061057729974005,
+                    "timestamp": 3.14575627875448
+                },
+                {
+                    "x": 4.3440348464149885,
+                    "y": 5.596516715288762,
+                    "heading": 3.14,
+                    "angularVelocity": 3.5626049667024716e-16,
+                    "velocityX": -1.5697405429654518,
+                    "velocityY": -0.7073718408886114,
+                    "timestamp": 3.2490024430217823
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 2.245803154155974e-16,
+                    "velocityX": -0.7848702854005415,
+                    "velocityY": -0.35368592671608795,
+                    "timestamp": 3.3522486072890847
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": -4.241491761729664e-33,
+                    "velocityY": 0,
+                    "timestamp": 3.455494771556387
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W2 To C2 To S1": {
+            "waypoints": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 19
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.327391358066629,
+                    "y": 5.555067935622319,
+                    "heading": 3.133430176934097,
+                    "angularVelocity": -0.0681146690773455,
+                    "velocityX": 0.8023787994403143,
+                    "velocityY": -0.05113470021702577,
+                    "timestamp": 0.09645239644992105
+                },
+                {
+                    "x": 2.4821740708193567,
+                    "y": 5.545203805497392,
+                    "heading": 3.12029049964152,
+                    "angularVelocity": -0.13622966122359603,
+                    "velocityX": 1.6047575638319433,
+                    "velocityY": -0.10226941463344474,
+                    "timestamp": 0.1929047928998421
+                },
+                {
+                    "x": 2.7143481316892246,
+                    "y": 5.5304076076409565,
+                    "heading": 3.1005809392040384,
+                    "angularVelocity": -0.20434495318853443,
+                    "velocityX": 2.4071362601178543,
+                    "velocityY": -0.15340414962232987,
+                    "timestamp": 0.2893571893497632
+                },
+                {
+                    "x": 3.023913521714313,
+                    "y": 5.510679340033408,
+                    "heading": 3.0743015138082646,
+                    "angularVelocity": -0.2724600565981605,
+                    "velocityX": 3.2095147598102214,
+                    "velocityY": -0.2045389055500683,
+                    "timestamp": 0.3858095857996842
+                },
+                {
+                    "x": 3.388048508086598,
+                    "y": 5.487473394252625,
+                    "heading": 3.0743015107885725,
+                    "angularVelocity": -3.130758650216746e-8,
+                    "velocityX": 3.7752818983750913,
+                    "velocityY": -0.24059480774884917,
+                    "timestamp": 0.48226198224960526
+                },
+                {
+                    "x": 3.752183494458947,
+                    "y": 5.464267448472115,
+                    "heading": 3.0743015077689044,
+                    "angularVelocity": -3.130734274282633e-8,
+                    "velocityX": 3.7752818983757597,
+                    "velocityY": -0.24059480774599656,
+                    "timestamp": 0.5787143786995264
+                },
+                {
+                    "x": 4.116318480831297,
+                    "y": 5.441061502691605,
+                    "heading": 3.0743015047492364,
+                    "angularVelocity": -3.1307342782181776e-8,
+                    "velocityX": 3.77528189837576,
+                    "velocityY": -0.24059480774599656,
+                    "timestamp": 0.6751667751494475
+                },
+                {
+                    "x": 4.480453467203646,
+                    "y": 5.417855556911095,
+                    "heading": 3.0743015017295683,
+                    "angularVelocity": -3.130734248880116e-8,
+                    "velocityX": 3.7752818983757597,
+                    "velocityY": -0.2405948077459966,
+                    "timestamp": 0.7716191715993685
+                },
+                {
+                    "x": 4.844588453575996,
+                    "y": 5.3946496111305855,
+                    "heading": 3.0743014987098998,
+                    "angularVelocity": -3.130734337287564e-8,
+                    "velocityX": 3.77528189837576,
+                    "velocityY": -0.24059480774599656,
+                    "timestamp": 0.8680715680492896
+                },
+                {
+                    "x": 5.208723439948346,
+                    "y": 5.371443665350077,
+                    "heading": 3.0743014956902317,
+                    "angularVelocity": -3.130734341157956e-8,
+                    "velocityX": 3.7752818983757606,
+                    "velocityY": -0.24059480774599654,
+                    "timestamp": 0.9645239644992107
+                },
+                {
+                    "x": 5.572858426320695,
+                    "y": 5.348237719569567,
+                    "heading": 3.0743014926705636,
+                    "angularVelocity": -3.130734268712419e-8,
+                    "velocityX": 3.7752818983757597,
+                    "velocityY": -0.24059480774599656,
+                    "timestamp": 1.0609763609491318
+                },
+                {
+                    "x": 5.936993412693067,
+                    "y": 5.325031773789395,
+                    "heading": 3.0743014896508956,
+                    "angularVelocity": -3.130734257132029e-8,
+                    "velocityX": 3.7752818983759835,
+                    "velocityY": -0.24059480774249117,
+                    "timestamp": 1.157428757399053
+                },
+                {
+                    "x": 6.301128405222732,
+                    "y": 5.301825924626314,
+                    "heading": 3.0743014866312275,
+                    "angularVelocity": -3.1307343059822334e-8,
+                    "velocityX": 3.7752819622136293,
+                    "velocityY": -0.24059380603497976,
+                    "timestamp": 1.253881153848974
+                },
+                {
+                    "x": 6.666000662308113,
+                    "y": 5.302845713467377,
+                    "heading": 3.0743014835545788,
+                    "angularVelocity": -3.1898104709200575e-8,
+                    "velocityX": 3.7829257801264253,
+                    "velocityY": 0.010572975670877716,
+                    "timestamp": 1.3503335502988951
+                },
+                {
+                    "x": 7.022411728114523,
+                    "y": 5.380972301482392,
+                    "heading": 3.0743014635013113,
+                    "angularVelocity": -2.0790844162753672e-7,
+                    "velocityX": 3.6952017671376614,
+                    "velocityY": 0.8100015229334367,
+                    "timestamp": 1.4467859467488162
+                },
+                {
+                    "x": 7.302470785429282,
+                    "y": 5.472668060315495,
+                    "heading": 3.0808623231023553,
+                    "angularVelocity": 0.06802173758793187,
+                    "velocityX": 2.903598745316483,
+                    "velocityY": 0.9506840908894665,
+                    "timestamp": 1.5432383431987373
+                },
+                {
+                    "x": 7.505508952457786,
+                    "y": 5.5733927696291925,
+                    "heading": 3.0939992329731627,
+                    "angularVelocity": 0.1362009691239587,
+                    "velocityX": 2.1050608849716217,
+                    "velocityY": 1.0442945226974727,
+                    "timestamp": 1.6396907396486584
+                },
+                {
+                    "x": 7.631373643549882,
+                    "y": 5.681732940772809,
+                    "heading": 3.113712106449711,
+                    "angularVelocity": 0.20437930214396433,
+                    "velocityX": 1.3049410457877573,
+                    "velocityY": 1.1232501745031058,
+                    "timestamp": 1.7361431360985795
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": 0.27254785280465055,
+                    "velocityX": 0.5041487639486981,
+                    "velocityY": 1.1950668253954515,
+                    "timestamp": 1.8325955325485006
+                },
+                {
+                    "x": 7.657241486683139,
+                    "y": 5.911359552858986,
+                    "heading": 3.1705946317800286,
+                    "angularVelocity": 0.3367496839490721,
+                    "velocityX": -0.25049891829736254,
+                    "velocityY": 1.2587353088184159,
+                    "timestamp": 1.923448273436763
+                },
+                {
+                    "x": 7.565890794694817,
+                    "y": 6.031156159947816,
+                    "heading": 3.2067228278300997,
+                    "angularVelocity": 0.39765664411275126,
+                    "velocityX": -1.0054808594126166,
+                    "velocityY": 1.318580000092289,
+                    "timestamp": 2.0143010143250253
+                },
+                {
+                    "x": 7.40590178880199,
+                    "y": 6.1558149457566955,
+                    "heading": 3.247889559674286,
+                    "angularVelocity": 0.453114913669111,
+                    "velocityX": -1.7609706028527334,
+                    "velocityY": 1.3720971386234067,
+                    "timestamp": 2.1051537552132875
+                },
+                {
+                    "x": 7.1771984711004135,
+                    "y": 6.284201834322083,
+                    "heading": 3.293119082899787,
+                    "angularVelocity": 0.49783333758889264,
+                    "velocityX": -2.517296841741443,
+                    "velocityY": 1.4131317042298905,
+                    "timestamp": 2.1960064961015497
+                },
+                {
+                    "x": 6.879672767630686,
+                    "y": 6.413043079334037,
+                    "heading": 3.3395968476460443,
+                    "angularVelocity": 0.5115725105467077,
+                    "velocityX": -3.274812631526949,
+                    "velocityY": 1.4181327250260163,
+                    "timestamp": 2.286859236989812
+                },
+                {
+                    "x": 6.543860581996508,
+                    "y": 6.486209643935175,
+                    "heading": 3.33959685479452,
+                    "angularVelocity": 7.868200264443885e-8,
+                    "velocityX": -3.6962251479807913,
+                    "velocityY": 0.805331395462515,
+                    "timestamp": 2.377711977878074
+                },
+                {
+                    "x": 6.202053460852504,
+                    "y": 6.52214101663038,
+                    "heading": 3.33959685834771,
+                    "angularVelocity": 3.910933561720104e-8,
+                    "velocityX": -3.7622103395249686,
+                    "velocityY": 0.39549024436583907,
+                    "timestamp": 2.4685647187663364
+                },
+                {
+                    "x": 5.860246134397577,
+                    "y": 6.558070415270777,
+                    "heading": 3.339596863101522,
+                    "angularVelocity": 5.232436753390925e-8,
+                    "velocityX": -3.762212599345888,
+                    "velocityY": 0.39546851629479235,
+                    "timestamp": 2.5594174596545987
+                },
+                {
+                    "x": 5.5865423033177715,
+                    "y": 6.586842981129828,
+                    "heading": 3.3642400407233666,
+                    "angularVelocity": 0.2712430839279776,
+                    "velocityX": -3.0126095085719746,
+                    "velocityY": 0.31669452762507067,
+                    "timestamp": 2.650270200542861
+                },
+                {
+                    "x": 5.381269635001315,
+                    "y": 6.608421695048074,
+                    "heading": 3.3835025789166386,
+                    "angularVelocity": 0.2120193403626775,
+                    "velocityX": -2.2593998409901115,
+                    "velocityY": 0.23751307563506308,
+                    "timestamp": 2.741122941431123
+                },
+                {
+                    "x": 5.24442286400993,
+                    "y": 6.622807278395225,
+                    "heading": 3.3965772666434986,
+                    "angularVelocity": 0.14391076811804343,
+                    "velocityX": -1.5062481291531917,
+                    "velocityY": 0.15833956363345458,
+                    "timestamp": 2.8319756823193853
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": 0.07273352979206758,
+                    "velocityX": -0.753118324675337,
+                    "velocityY": 0.07916901058187684,
+                    "timestamp": 2.9228284232076476
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": -1.0239181929178098e-32,
+                    "velocityX": 1.4788561432464288e-34,
+                    "velocityY": 0,
+                    "timestamp": 3.01368116409591
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W2 To C2 To S2": {
+            "waypoints": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 19
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 14
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.3282521020736953,
+                    "y": 5.564172492511594,
+                    "heading": 3.14,
+                    "angularVelocity": 1.232216948997824e-17,
+                    "velocityX": 0.8071870820603332,
+                    "velocityY": 0.043040148021588745,
+                    "timestamp": 0.09694419527026792
+                },
+                {
+                    "x": 2.484756303050172,
+                    "y": 5.5725174773657065,
+                    "heading": 3.14,
+                    "angularVelocity": 2.4529935765970837e-17,
+                    "velocityX": 1.6143741314120201,
+                    "velocityY": 0.08608029429911494,
+                    "timestamp": 0.19388839054053583
+                },
+                {
+                    "x": 2.7195125965876055,
+                    "y": 5.5850349542241835,
+                    "heading": 3.14,
+                    "angularVelocity": 3.650998333784998e-17,
+                    "velocityX": 2.421561115346432,
+                    "velocityY": 0.12912043708851692,
+                    "timestamp": 0.29083258581080373
+                },
+                {
+                    "x": 3.032520963660537,
+                    "y": 5.601724922072566,
+                    "heading": 3.14,
+                    "angularVelocity": 4.7804313451185203e-17,
+                    "velocityX": 3.2287479030291797,
+                    "velocityY": 0.17216056941355468,
+                    "timestamp": 0.38777678108107166
+                },
+                {
+                    "x": 3.398734861706694,
+                    "y": 5.621251870322576,
+                    "heading": 3.14,
+                    "angularVelocity": 4.271471690686359e-16,
+                    "velocityX": 3.777574273786295,
+                    "velocityY": 0.20142462579390313,
+                    "timestamp": 0.4847209763513396
+                },
+                {
+                    "x": 3.7649487597528926,
+                    "y": 5.6407788185725884,
+                    "heading": 3.14,
+                    "angularVelocity": 3.505027380693511e-16,
+                    "velocityX": 3.7775742737867146,
+                    "velocityY": 0.20142462579393414,
+                    "timestamp": 0.5816651716216075
+                },
+                {
+                    "x": 4.1311626577990905,
+                    "y": 5.660305766822602,
+                    "heading": 3.14,
+                    "angularVelocity": 3.7529805795054796e-16,
+                    "velocityX": 3.7775742737867146,
+                    "velocityY": 0.20142462579393414,
+                    "timestamp": 0.6786093668918753
+                },
+                {
+                    "x": 4.497376555845288,
+                    "y": 5.679832715072616,
+                    "heading": 3.14,
+                    "angularVelocity": 4.2292762480360005e-16,
+                    "velocityX": 3.7775742737867146,
+                    "velocityY": 0.20142462579393414,
+                    "timestamp": 0.7755535621621432
+                },
+                {
+                    "x": 4.8635904538914865,
+                    "y": 5.699359663322629,
+                    "heading": 3.14,
+                    "angularVelocity": -1.0631316332366903e-16,
+                    "velocityX": 3.7775742737867146,
+                    "velocityY": 0.20142462579393414,
+                    "timestamp": 0.8724977574324111
+                },
+                {
+                    "x": 5.229804351937685,
+                    "y": 5.718886611572643,
+                    "heading": 3.14,
+                    "angularVelocity": -2.15507046342417e-16,
+                    "velocityX": 3.7775742737867146,
+                    "velocityY": 0.2014246257939341,
+                    "timestamp": 0.969441952702679
+                },
+                {
+                    "x": 5.596018249983883,
+                    "y": 5.738413559822655,
+                    "heading": 3.14,
+                    "angularVelocity": 2.7855582274724927e-16,
+                    "velocityX": 3.7775742737867146,
+                    "velocityY": 0.20142462579393414,
+                    "timestamp": 1.0663861479729468
+                },
+                {
+                    "x": 5.962232148030081,
+                    "y": 5.757940508072669,
+                    "heading": 3.14,
+                    "angularVelocity": 3.1548114792697624e-17,
+                    "velocityX": 3.7775742737867146,
+                    "velocityY": 0.201424625793935,
+                    "timestamp": 1.1633303432432147
+                },
+                {
+                    "x": 6.3284460460762855,
+                    "y": 5.777467456322548,
+                    "heading": 3.14,
+                    "angularVelocity": 5.639488659636306e-17,
+                    "velocityX": 3.777574273786789,
+                    "velocityY": 0.20142462579255413,
+                    "timestamp": 1.2602745385134826
+                },
+                {
+                    "x": 6.694659946182267,
+                    "y": 5.796994365942685,
+                    "heading": 3.14,
+                    "angularVelocity": 1.8236235813910642e-16,
+                    "velocityX": 3.7775742950338076,
+                    "velocityY": 0.20142422731870196,
+                    "timestamp": 1.3572187337837505
+                },
+                {
+                    "x": 7.048232130231478,
+                    "y": 5.805340495974463,
+                    "heading": 3.14,
+                    "angularVelocity": 9.880815779139735e-16,
+                    "velocityX": 3.6471723000895206,
+                    "velocityY": 0.08609210670542117,
+                    "timestamp": 1.4541629290540183
+                },
+                {
+                    "x": 7.323552241539944,
+                    "y": 5.8095141201136,
+                    "heading": 3.14,
+                    "angularVelocity": 1.0046882743013462e-15,
+                    "velocityX": 2.839985520604805,
+                    "velocityY": 0.04305182065552602,
+                    "timestamp": 1.5511071243242862
+                },
+                {
+                    "x": 7.520620260713227,
+                    "y": 5.809515244263857,
+                    "heading": 3.14,
+                    "angularVelocity": 1.1309282599454547e-15,
+                    "velocityX": 2.032798541062352,
+                    "velocityY": 0.000011595504179921879,
+                    "timestamp": 1.648051319594554
+                },
+                {
+                    "x": 7.639436181279198,
+                    "y": 5.805343870530581,
+                    "heading": 3.14,
+                    "angularVelocity": 1.1596973638353238e-15,
+                    "velocityX": 1.2256114947584758,
+                    "velocityY": -0.043028607930100944,
+                    "timestamp": 1.744995514864822
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": 1.1762980395606437e-15,
+                    "velocityX": 0.41842441505544703,
+                    "velocityY": -0.08606880015967894,
+                    "timestamp": 1.8419397101350898
+                },
+                {
+                    "x": 7.637810963282883,
+                    "y": 5.783845940195137,
+                    "heading": 3.14,
+                    "angularVelocity": 1.2081191600227442e-15,
+                    "velocityX": -0.41931671098523704,
+                    "velocityY": -0.1307381629639976,
+                    "timestamp": 1.94255348238971
+                },
+                {
+                    "x": 7.511333424065826,
+                    "y": 5.766201461544033,
+                    "heading": 3.14,
+                    "angularVelocity": 1.221716047001012e-15,
+                    "velocityX": -1.257059901260624,
+                    "velocityY": -0.1753684232696404,
+                    "timestamp": 2.04316725464433
+                },
+                {
+                    "x": 7.3005670350867495,
+                    "y": 5.744073149987429,
+                    "heading": 3.14,
+                    "angularVelocity": 9.674484857548948e-16,
+                    "velocityX": -2.0948065429741827,
+                    "velocityY": -0.2199332259036739,
+                    "timestamp": 2.14378102689895
+                },
+                {
+                    "x": 7.005511097627955,
+                    "y": 5.717474286699145,
+                    "heading": 3.14,
+                    "angularVelocity": 6.103023189064465e-16,
+                    "velocityX": -2.9325601292436843,
+                    "velocityY": -0.2643660269377714,
+                    "timestamp": 2.2443947991535698
+                },
+                {
+                    "x": 6.626163481456156,
+                    "y": 5.686445611767655,
+                    "heading": 3.14,
+                    "angularVelocity": -3.898195701943848e-16,
+                    "velocityX": -3.7703348879039833,
+                    "velocityY": -0.30839391219134293,
+                    "timestamp": 2.3450085714081896
+                },
+                {
+                    "x": 6.24609124574338,
+                    "y": 5.666109085648216,
+                    "heading": 3.14,
+                    "angularVelocity": 5.28249015807614e-17,
+                    "velocityX": -3.7775368838161545,
+                    "velocityY": -0.2021246762112827,
+                    "timestamp": 2.4456223436628095
+                },
+                {
+                    "x": 5.866019008161075,
+                    "y": 5.6457725944687756,
+                    "heading": 3.14,
+                    "angularVelocity": 1.0714715731827337e-16,
+                    "velocityX": -3.777536902397429,
+                    "velocityY": -0.20212432894236246,
+                    "timestamp": 2.5462361159174294
+                },
+                {
+                    "x": 5.485946770578765,
+                    "y": 5.625436103289443,
+                    "heading": 3.14,
+                    "angularVelocity": 4.4105778021487463e-16,
+                    "velocityX": -3.7775369023974865,
+                    "velocityY": -0.2021243289413037,
+                    "timestamp": 2.6468498881720492
+                },
+                {
+                    "x": 5.105874532996473,
+                    "y": 5.60509961211012,
+                    "heading": 3.14,
+                    "angularVelocity": 4.563638430357427e-16,
+                    "velocityX": -3.7775369023972933,
+                    "velocityY": -0.20212432894121146,
+                    "timestamp": 2.747463660426669
+                },
+                {
+                    "x": 4.768724738760508,
+                    "y": 5.587059768280702,
+                    "heading": 3.14,
+                    "angularVelocity": 7.096592098456484e-16,
+                    "velocityX": -3.3509308606209816,
+                    "velocityY": -0.1792979574700793,
+                    "timestamp": 2.848077432681289
+                },
+                {
+                    "x": 4.5158623742843815,
+                    "y": 5.573529884402755,
+                    "heading": 3.14,
+                    "angularVelocity": 5.58698579821863e-16,
+                    "velocityX": -2.5131983323104805,
+                    "velocityY": -0.13447347810003318,
+                    "timestamp": 2.948691204935909
+                },
+                {
+                    "x": 4.3472874591846,
+                    "y": 5.564509961525897,
+                    "heading": 3.14,
+                    "angularVelocity": 2.9765197430594465e-16,
+                    "velocityX": -1.6754656090315643,
+                    "velocityY": -0.08964898829784126,
+                    "timestamp": 3.0493049771905287
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 1.413897346526725e-16,
+                    "velocityX": -0.8377328207631561,
+                    "velocityY": -0.04482449501826641,
+                    "timestamp": 3.1499187494451486
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": -1.608253361776519e-31,
+                    "velocityY": 0,
+                    "timestamp": 3.2505325216997685
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W2 To C3 To S1": {
+            "waypoints": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 20
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.3161821396104223,
+                    "y": 5.531175019358262,
+                    "heading": 3.133818954660017,
+                    "angularVelocity": -0.0664208658906851,
+                    "velocityX": 0.711186341084945,
+                    "velocityY": -0.3097502231737145,
+                    "timestamp": 0.09305878893773292
+                },
+                {
+                    "x": 2.448546415336416,
+                    "y": 5.473525059620157,
+                    "heading": 3.121456701812491,
+                    "angularVelocity": -0.13284347441699146,
+                    "velocityX": 1.4223726446145823,
+                    "velocityY": -0.6195004297410188,
+                    "timestamp": 0.18611757787546584
+                },
+                {
+                    "x": 2.647092820298068,
+                    "y": 5.387050123667136,
+                    "heading": 3.1029129720677258,
+                    "angularVelocity": -0.19926897777675723,
+                    "velocityX": 2.1335588742134064,
+                    "velocityY": -0.9292506053445697,
+                    "timestamp": 0.2791763668131988
+                },
+                {
+                    "x": 2.911821335852912,
+                    "y": 5.271750219180522,
+                    "heading": 3.078187424791321,
+                    "angularVelocity": -0.26569814155811494,
+                    "velocityX": 2.844744903482224,
+                    "velocityY": -1.2390006984054311,
+                    "timestamp": 0.3722351557509317
+                },
+                {
+                    "x": 3.2345733580792335,
+                    "y": 5.131178588369686,
+                    "heading": 3.07818742179356,
+                    "angularVelocity": -3.221363014134022e-8,
+                    "velocityX": 3.468259429448196,
+                    "velocityY": -1.5105680228107692,
+                    "timestamp": 0.4652939446886646
+                },
+                {
+                    "x": 3.557325380260581,
+                    "y": 4.990606957455228,
+                    "heading": 3.078187418795868,
+                    "angularVelocity": -3.2212882318357466e-8,
+                    "velocityX": 3.4682594289649007,
+                    "velocityY": -1.510568023924279,
+                    "timestamp": 0.5583527336263975
+                },
+                {
+                    "x": 3.8800774024419273,
+                    "y": 4.85003532654077,
+                    "heading": 3.0781874157981757,
+                    "angularVelocity": -3.221288214692397e-8,
+                    "velocityX": 3.468259428964899,
+                    "velocityY": -1.5105680239242834,
+                    "timestamp": 0.6514115225641304
+                },
+                {
+                    "x": 4.202829424623274,
+                    "y": 4.709463695626312,
+                    "heading": 3.078187412800484,
+                    "angularVelocity": -3.221288189984943e-8,
+                    "velocityX": 3.468259428964899,
+                    "velocityY": -1.5105680239242838,
+                    "timestamp": 0.7444703115018633
+                },
+                {
+                    "x": 4.525581446804621,
+                    "y": 4.568892064711854,
+                    "heading": 3.078187409802792,
+                    "angularVelocity": -3.2212882353617496e-8,
+                    "velocityX": 3.468259428964899,
+                    "velocityY": -1.5105680239242834,
+                    "timestamp": 0.8375291004395962
+                },
+                {
+                    "x": 4.848333468985968,
+                    "y": 4.428320433797395,
+                    "heading": 3.0781874068051,
+                    "angularVelocity": -3.221288275372931e-8,
+                    "velocityX": 3.468259428964899,
+                    "velocityY": -1.5105680239242836,
+                    "timestamp": 0.9305878893773291
+                },
+                {
+                    "x": 5.1710854911673145,
+                    "y": 4.287748802882937,
+                    "heading": 3.0781874038074077,
+                    "angularVelocity": -3.221288266203546e-8,
+                    "velocityX": 3.4682594289648985,
+                    "velocityY": -1.5105680239242834,
+                    "timestamp": 1.023646678315062
+                },
+                {
+                    "x": 5.493837513348661,
+                    "y": 4.147177171968479,
+                    "heading": 3.078187400809716,
+                    "angularVelocity": -3.221288308232409e-8,
+                    "velocityX": 3.4682594289648994,
+                    "velocityY": -1.5105680239242831,
+                    "timestamp": 1.116705467252795
+                },
+                {
+                    "x": 5.816589535530807,
+                    "y": 4.006605541055855,
+                    "heading": 3.078187397812024,
+                    "angularVelocity": -3.221288143740925e-8,
+                    "velocityX": 3.4682594289734867,
+                    "velocityY": -1.510568023904566,
+                    "timestamp": 1.209764256190528
+                },
+                {
+                    "x": 6.139341766628845,
+                    "y": 3.866034389814445,
+                    "heading": 3.0781873948143317,
+                    "angularVelocity": -3.2212889577605074e-8,
+                    "velocityX": 3.4682616739617793,
+                    "velocityY": -1.5105628694079405,
+                    "timestamp": 1.302823045128261
+                },
+                {
+                    "x": 6.479618734001887,
+                    "y": 3.7758078084398288,
+                    "heading": 3.078187391647213,
+                    "angularVelocity": -3.403352419183157e-8,
+                    "velocityX": 3.656580654630348,
+                    "velocityY": -0.9695653941401335,
+                    "timestamp": 1.395881834065994
+                },
+                {
+                    "x": 6.831146782632028,
+                    "y": 3.7569067855053886,
+                    "heading": 3.078187386672884,
+                    "angularVelocity": -5.3453615233680875e-8,
+                    "velocityX": 3.777483595508151,
+                    "velocityY": -0.20310841297416227,
+                    "timestamp": 1.488940623003727
+                },
+                {
+                    "x": 7.135392516475817,
+                    "y": 3.7925535020762564,
+                    "heading": 3.084237894820064,
+                    "angularVelocity": 0.06501812688781647,
+                    "velocityX": 3.269392792628807,
+                    "velocityY": 0.3830558830366858,
+                    "timestamp": 1.58199941194146
+                },
+                {
+                    "x": 7.379778215429103,
+                    "y": 3.868544177421427,
+                    "heading": 3.09656532372019,
+                    "angularVelocity": 0.1324692599252942,
+                    "velocityX": 2.6261431267583744,
+                    "velocityY": 0.8165878388554683,
+                    "timestamp": 1.675058200879193
+                },
+                {
+                    "x": 7.561679450282434,
+                    "y": 3.980681271949268,
+                    "heading": 3.1151578247073277,
+                    "angularVelocity": 0.1997930684395467,
+                    "velocityX": 1.954691619456221,
+                    "velocityY": 1.2050134738253881,
+                    "timestamp": 1.768116989816926
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": 0.26695141400662953,
+                    "velocityX": 1.2714602357090166,
+                    "velocityY": 1.572325727864734,
+                    "timestamp": 1.861175778754659
+                },
+                {
+                    "x": 7.734511016491924,
+                    "y": 4.293948601996246,
+                    "heading": 3.168900193264706,
+                    "angularVelocity": 0.3300287650543787,
+                    "velocityX": 0.6224942265925568,
+                    "velocityY": 1.9064869372920104,
+                    "timestamp": 1.948744488894043
+                },
+                {
+                    "x": 7.731795753069985,
+                    "y": 4.489388700342548,
+                    "heading": 3.202314872595517,
+                    "angularVelocity": 0.38158240857521825,
+                    "velocityX": -0.031007233264223562,
+                    "velocityY": 2.2318485453904535,
+                    "timestamp": 2.0363131990334273
+                },
+                {
+                    "x": 7.671228890668297,
+                    "y": 4.712045759892295,
+                    "heading": 3.2385899184542097,
+                    "angularVelocity": 0.41424666186076586,
+                    "velocityX": -0.6916495892800459,
+                    "velocityY": 2.5426554667225383,
+                    "timestamp": 2.1238819091728116
+                },
+                {
+                    "x": 7.551691509705012,
+                    "y": 4.959414788228928,
+                    "heading": 3.2745248408990206,
+                    "angularVelocity": 0.4103625871342999,
+                    "velocityX": -1.3650695639231716,
+                    "velocityY": 2.824856366422358,
+                    "timestamp": 2.211450619312196
+                },
+                {
+                    "x": 7.370754702688456,
+                    "y": 5.224445119410505,
+                    "heading": 3.301432833728465,
+                    "angularVelocity": 0.30727862482631746,
+                    "velocityX": -2.066226700479633,
+                    "velocityY": 3.0265414525316805,
+                    "timestamp": 2.29901932945158
+                },
+                {
+                    "x": 7.135099709363178,
+                    "y": 5.457264131297548,
+                    "heading": 3.3014328467429856,
+                    "angularVelocity": 1.4862067021464798e-7,
+                    "velocityX": -2.6910867243583128,
+                    "velocityY": 2.658700939142107,
+                    "timestamp": 2.3865880395909644
+                },
+                {
+                    "x": 6.859107156435953,
+                    "y": 5.640474617463791,
+                    "heading": 3.3014328522641176,
+                    "angularVelocity": 6.304914289451061e-8,
+                    "velocityX": -3.1517256847557116,
+                    "velocityY": 2.0921912161846987,
+                    "timestamp": 2.4741567497303487
+                },
+                {
+                    "x": 6.573538412185887,
+                    "y": 5.808368822234562,
+                    "heading": 3.3014328577109833,
+                    "angularVelocity": 6.220104875802801e-8,
+                    "velocityX": -3.26108199830194,
+                    "velocityY": 1.9172853465984772,
+                    "timestamp": 2.561725459869733
+                },
+                {
+                    "x": 6.287969619272061,
+                    "y": 5.976262944233841,
+                    "heading": 3.301432863157849,
+                    "angularVelocity": 6.220104736843505e-8,
+                    "velocityX": -3.2610825540228303,
+                    "velocityY": 1.9172844013808124,
+                    "timestamp": 2.6492941700091173
+                },
+                {
+                    "x": 6.002400826335374,
+                    "y": 6.144157066194225,
+                    "heading": 3.3014328686047243,
+                    "angularVelocity": 6.2201164432783e-8,
+                    "velocityX": -3.2610825542838673,
+                    "velocityY": 1.9172844009366552,
+                    "timestamp": 2.7368628801485015
+                },
+                {
+                    "x": 5.726906238126056,
+                    "y": 6.306118202865447,
+                    "heading": 3.330104518338368,
+                    "angularVelocity": 0.3274188884135285,
+                    "velocityX": -3.14603912482906,
+                    "velocityY": 1.849532057894034,
+                    "timestamp": 2.824431590287886
+                },
+                {
+                    "x": 5.5065316534228295,
+                    "y": 6.435677156249956,
+                    "heading": 3.357685145070299,
+                    "angularVelocity": 0.31495983768665414,
+                    "velocityX": -2.5165905076419874,
+                    "velocityY": 1.47951195327976,
+                    "timestamp": 2.91200030042727
+                },
+                {
+                    "x": 5.341261355658222,
+                    "y": 6.532840911354814,
+                    "heading": 3.379877882623065,
+                    "angularVelocity": 0.2534322764083417,
+                    "velocityX": -1.8873213674330116,
+                    "velocityY": 1.1095716146806525,
+                    "timestamp": 2.9995690105666544
+                },
+                {
+                    "x": 5.231085952105995,
+                    "y": 6.597614249467722,
+                    "heading": 3.395277171030749,
+                    "angularVelocity": 0.17585377680193118,
+                    "velocityX": -1.2581594884389657,
+                    "velocityY": 0.7396858764940984,
+                    "timestamp": 3.0871377207060386
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": 0.09030778386766043,
+                    "velocityX": -0.6290597636794472,
+                    "velocityY": 0.3698324490646197,
+                    "timestamp": 3.174706430845423
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": 0,
+                    "velocityX": -1.4368373863628223e-33,
+                    "velocityY": 0,
+                    "timestamp": 3.262275140984807
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W2 To C3 To S2": {
+            "waypoints": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 20
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.3211310801785023,
+                    "y": 5.537078029621854,
+                    "heading": 3.14,
+                    "angularVelocity": 6.136916649287371e-17,
+                    "velocityX": 0.7513410861003985,
+                    "velocityY": -0.24211945152891107,
+                    "timestamp": 0.09467215555545593
+                },
+                {
+                    "x": 2.4633932375532455,
+                    "y": 5.491234089826592,
+                    "heading": 3.1399999999999997,
+                    "angularVelocity": -6.918142497868558e-18,
+                    "velocityX": 1.5026821406998672,
+                    "velocityY": -0.48423889290665645,
+                    "timestamp": 0.18934431111091185
+                },
+                {
+                    "x": 2.676786466159711,
+                    "y": 5.422468182536282,
+                    "heading": 3.14,
+                    "angularVelocity": -1.447784282504661e-16,
+                    "velocityX": 2.254023132297508,
+                    "velocityY": -0.7263583139820807,
+                    "timestamp": 0.28401646666636776
+                },
+                {
+                    "x": 2.9613107481043692,
+                    "y": 5.330780313517108,
+                    "heading": 3.14,
+                    "angularVelocity": -2.19469189514941e-16,
+                    "velocityX": 3.005363934889942,
+                    "velocityY": -0.9684776741506306,
+                    "timestamp": 0.3786886222218237
+                },
+                {
+                    "x": 3.30218779492529,
+                    "y": 5.220932782027199,
+                    "heading": 3.14,
+                    "angularVelocity": 3.7716439529503614e-16,
+                    "velocityX": 3.6006051073934406,
+                    "velocityY": -1.160293972873203,
+                    "timestamp": 0.47336077777727964
+                },
+                {
+                    "x": 3.643064841746281,
+                    "y": 5.111085250537267,
+                    "heading": 3.14,
+                    "angularVelocity": 1.8926244658706998e-16,
+                    "velocityX": 3.600605107394178,
+                    "velocityY": -1.1602939728734405,
+                    "timestamp": 0.5680329333327355
+                },
+                {
+                    "x": 3.9839418885672715,
+                    "y": 5.001237719047335,
+                    "heading": 3.14,
+                    "angularVelocity": 3.4257793215779713e-16,
+                    "velocityX": 3.6006051073941783,
+                    "velocityY": -1.1602939728734407,
+                    "timestamp": 0.6627050888881915
+                },
+                {
+                    "x": 4.3248189353882625,
+                    "y": 4.891390187557403,
+                    "heading": 3.14,
+                    "angularVelocity": 4.383897524805657e-17,
+                    "velocityX": 3.6006051073941783,
+                    "velocityY": -1.1602939728734407,
+                    "timestamp": 0.7573772444436474
+                },
+                {
+                    "x": 4.665695982209253,
+                    "y": 4.7815426560674705,
+                    "heading": 3.14,
+                    "angularVelocity": 1.5398680786578701e-19,
+                    "velocityX": 3.600605107394178,
+                    "velocityY": -1.160293972873441,
+                    "timestamp": 0.8520493999991033
+                },
+                {
+                    "x": 5.0065730290302435,
+                    "y": 4.671695124577538,
+                    "heading": 3.14,
+                    "angularVelocity": 4.356845547140055e-17,
+                    "velocityX": 3.600605107394178,
+                    "velocityY": -1.1602939728734414,
+                    "timestamp": 0.9467215555545593
+                },
+                {
+                    "x": 5.347450075851235,
+                    "y": 4.561847593087606,
+                    "heading": 3.14,
+                    "angularVelocity": 2.68380689151133e-16,
+                    "velocityX": 3.600605107394178,
+                    "velocityY": -1.160293972873442,
+                    "timestamp": 1.041393711110015
+                },
+                {
+                    "x": 5.688327122672225,
+                    "y": 4.452000061597674,
+                    "heading": 3.14,
+                    "angularVelocity": 3.726757040891467e-16,
+                    "velocityX": 3.600605107394178,
+                    "velocityY": -1.1602939728734432,
+                    "timestamp": 1.136065866665471
+                },
+                {
+                    "x": 6.029204169493216,
+                    "y": 4.342152530107741,
+                    "heading": 3.14,
+                    "angularVelocity": 1.8883338122910111e-16,
+                    "velocityX": 3.6006051073941774,
+                    "velocityY": -1.1602939728734463,
+                    "timestamp": 1.230738022220927
+                },
+                {
+                    "x": 6.370081216318914,
+                    "y": 4.2323049986209496,
+                    "heading": 3.14,
+                    "angularVelocity": 2.0987063063623018e-16,
+                    "velocityX": 3.600605107443935,
+                    "velocityY": -1.1602939728401256,
+                    "timestamp": 1.325410177776383
+                },
+                {
+                    "x": 6.710958611265409,
+                    "y": 4.122458547431971,
+                    "heading": 3.14,
+                    "angularVelocity": 1.3456713754312597e-16,
+                    "velocityX": 3.600608784563045,
+                    "velocityY": -1.1602825619053307,
+                    "timestamp": 1.4200823333318389
+                },
+                {
+                    "x": 7.047028916950751,
+                    "y": 4.077522582297829,
+                    "heading": 3.14,
+                    "angularVelocity": 3.8640318477602567e-16,
+                    "velocityX": 3.549832616713609,
+                    "velocityY": -0.4746481673570916,
+                    "timestamp": 1.5147544888872948
+                },
+                {
+                    "x": 7.31196824975774,
+                    "y": 4.055508828572456,
+                    "heading": 3.14,
+                    "angularVelocity": 5.274988476955795e-16,
+                    "velocityX": 2.7984926639997774,
+                    "velocityY": -0.23252616987768301,
+                    "timestamp": 1.6094266444427507
+                },
+                {
+                    "x": 7.5057765524653535,
+                    "y": 4.056417169982893,
+                    "heading": 3.1400000000000006,
+                    "angularVelocity": 4.746542936809208e-16,
+                    "velocityX": 2.0471521068735608,
+                    "velocityY": 0.009594599437474344,
+                    "timestamp": 1.7040987999982067
+                },
+                {
+                    "x": 7.628453805780265,
+                    "y": 4.080247567089719,
+                    "heading": 3.14,
+                    "angularVelocity": 5.453879442658063e-16,
+                    "velocityX": 1.295811345956418,
+                    "velocityY": 0.2517149521631642,
+                    "timestamp": 1.7987709555536626
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": 4.116008282611605e-16,
+                    "velocityX": 0.5444704825542928,
+                    "velocityY": 0.49383509476441706,
+                    "timestamp": 1.8934431111091186
+                },
+                {
+                    "x": 7.6568603335776855,
+                    "y": 4.200231008500336,
+                    "heading": 3.14,
+                    "angularVelocity": 4.411427948098232e-16,
+                    "velocityX": -0.23545951737158613,
+                    "velocityY": 0.7451679554678371,
+                    "timestamp": 1.9917176114589898
+                },
+                {
+                    "x": 7.557063724755448,
+                    "y": 4.2981314655224265,
+                    "heading": 3.14,
+                    "angularVelocity": 5.768827951780328e-16,
+                    "velocityX": -1.0154883359055142,
+                    "velocityY": 0.9961938923479718,
+                    "timestamp": 2.0899921118088614
+                },
+                {
+                    "x": 7.380593913469942,
+                    "y": 4.42065077709202,
+                    "heading": 3.14,
+                    "angularVelocity": 6.878777879965837e-16,
+                    "velocityX": -1.7956826100081775,
+                    "velocityY": 1.246705006216337,
+                    "timestamp": 2.1882666121587326
+                },
+                {
+                    "x": 7.127418097854458,
+                    "y": 4.5676865279586805,
+                    "heading": 3.1400000000000006,
+                    "angularVelocity": 5.966587967087216e-16,
+                    "velocityX": -2.576210662116218,
+                    "velocityY": 1.4961739855526293,
+                    "timestamp": 2.286541112508604
+                },
+                {
+                    "x": 6.7974355684839916,
+                    "y": 4.73892129278293,
+                    "heading": 3.1400000000000006,
+                    "angularVelocity": 9.996236488627517e-17,
+                    "velocityX": -3.357763490993921,
+                    "velocityY": 1.7424129780831201,
+                    "timestamp": 2.384815612858475
+                },
+                {
+                    "x": 6.443766002914483,
+                    "y": 4.853499817181314,
+                    "heading": 3.1400000000000006,
+                    "angularVelocity": -3.2402717865838093e-16,
+                    "velocityX": -3.5987928130938722,
+                    "velocityY": 1.1659028943466263,
+                    "timestamp": 2.4830901132083465
+                },
+                {
+                    "x": 6.090096257463813,
+                    "y": 4.968077786339023,
+                    "heading": 3.1400000000000006,
+                    "angularVelocity": -3.4427620459908776e-16,
+                    "velocityX": -3.5987946434889486,
+                    "velocityY": 1.165897244450957,
+                    "timestamp": 2.581364613558218
+                },
+                {
+                    "x": 5.736426512012546,
+                    "y": 5.082655755494858,
+                    "heading": 3.1400000000000006,
+                    "angularVelocity": -3.8345847210221107e-16,
+                    "velocityX": -3.598794643495036,
+                    "velocityY": 1.165897244431872,
+                    "timestamp": 2.679639113908089
+                },
+                {
+                    "x": 5.382756766561277,
+                    "y": 5.197233724650691,
+                    "heading": 3.1400000000000006,
+                    "angularVelocity": -2.673892854874729e-16,
+                    "velocityX": -3.5987946434950357,
+                    "velocityY": 1.165897244431872,
+                    "timestamp": 2.7779136142579604
+                },
+                {
+                    "x": 5.02908702111004,
+                    "y": 5.311811693806516,
+                    "heading": 3.1400000000000006,
+                    "angularVelocity": -2.8589912756820243e-16,
+                    "velocityX": -3.598794643494731,
+                    "velocityY": 1.1658972444317732,
+                    "timestamp": 2.8761881146078316
+                },
+                {
+                    "x": 4.722652230446586,
+                    "y": 5.411087010523561,
+                    "heading": 3.14,
+                    "angularVelocity": 1.1044854210040432e-15,
+                    "velocityX": -3.1181516016108115,
+                    "velocityY": 1.0101838865993877,
+                    "timestamp": 2.974462614957703
+                },
+                {
+                    "x": 4.492826119821717,
+                    "y": 5.485543503772035,
+                    "heading": 3.14,
+                    "angularVelocity": 7.043876441844827e-16,
+                    "velocityX": -2.338613880575899,
+                    "velocityY": 0.7576379730591117,
+                    "timestamp": 3.072737115307574
+                },
+                {
+                    "x": 4.33960870762911,
+                    "y": 5.535181167592957,
+                    "heading": 3.14,
+                    "angularVelocity": 5.590882375321651e-16,
+                    "velocityX": -1.5590759723746228,
+                    "velocityY": 0.5050919988827703,
+                    "timestamp": 3.1710116156574455
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 3.3318762312703263e-16,
+                    "velocityX": -0.7795380017845207,
+                    "velocityY": 0.2525460044943946,
+                    "timestamp": 3.269286116007317
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": -3.7380886634717777e-32,
+                    "velocityY": 0,
+                    "timestamp": 3.367560616357188
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W2 To C3 To S3": {
+            "waypoints": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 20
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 17
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": -2.1257809849356417e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.321508847678769,
+                    "y": 5.553282811958262,
+                    "heading": 3.1537553335344852,
+                    "angularVelocity": 0.14810177732022164,
+                    "velocityX": 0.7699258915674908,
+                    "velocityY": -0.07232303637577411,
+                    "timestamp": 0.09287757232476541
+                },
+                {
+                    "x": 2.4645265668919087,
+                    "y": 5.539848505148353,
+                    "heading": 3.1812640229350246,
+                    "angularVelocity": 0.2961822613574502,
+                    "velocityX": 1.53985203998495,
+                    "velocityY": -0.14464532689263399,
+                    "timestamp": 0.18575514464953083
+                },
+                {
+                    "x": 2.679053206199872,
+                    "y": 5.519697173536265,
+                    "heading": 3.2225218511092746,
+                    "angularVelocity": 0.4442173405435694,
+                    "velocityX": 2.309778711246101,
+                    "velocityY": -0.21696660569059611,
+                    "timestamp": 0.27863271697429626
+                },
+                {
+                    "x": 2.965088840346323,
+                    "y": 5.49282892094042,
+                    "heading": 3.277521318880573,
+                    "angularVelocity": 0.5921716771297765,
+                    "velocityX": 3.079706187261968,
+                    "velocityY": -0.2892867666899661,
+                    "timestamp": 0.37151028929906166
+                },
+                {
+                    "x": 3.3148991000673105,
+                    "y": 5.459968004407915,
+                    "heading": 3.277521325558456,
+                    "angularVelocity": 7.189983956229139e-8,
+                    "velocityX": 3.7663587770985667,
+                    "velocityY": -0.35380895204280205,
+                    "timestamp": 0.46438786162382706
+                },
+                {
+                    "x": 3.664709359511986,
+                    "y": 5.427107084931802,
+                    "heading": 3.2775213322361236,
+                    "angularVelocity": 7.18975223410498e-8,
+                    "velocityX": 3.7663587741235602,
+                    "velocityY": -0.3538089837362208,
+                    "timestamp": 0.5572654339485925
+                },
+                {
+                    "x": 4.014519618956661,
+                    "y": 5.394246165455678,
+                    "heading": 3.277521338913791,
+                    "angularVelocity": 7.189752294324541e-8,
+                    "velocityX": 3.7663587741235487,
+                    "velocityY": -0.3538089837363427,
+                    "timestamp": 0.650143006273358
+                },
+                {
+                    "x": 4.364329878401335,
+                    "y": 5.361385245979554,
+                    "heading": 3.2775213455914582,
+                    "angularVelocity": 7.189752233099723e-8,
+                    "velocityX": 3.7663587741235482,
+                    "velocityY": -0.3538089837363424,
+                    "timestamp": 0.7430205785981234
+                },
+                {
+                    "x": 4.714140137846011,
+                    "y": 5.32852432650343,
+                    "heading": 3.277521352269126,
+                    "angularVelocity": 7.189752328963948e-8,
+                    "velocityX": 3.7663587741235482,
+                    "velocityY": -0.35380898373634245,
+                    "timestamp": 0.8358981509228889
+                },
+                {
+                    "x": 5.063950397290685,
+                    "y": 5.295663407027306,
+                    "heading": 3.2775213589467933,
+                    "angularVelocity": 7.189752245277794e-8,
+                    "velocityX": 3.7663587741235482,
+                    "velocityY": -0.3538089837363426,
+                    "timestamp": 0.9287757232476543
+                },
+                {
+                    "x": 5.41376065673536,
+                    "y": 5.262802487551181,
+                    "heading": 3.2775213656244606,
+                    "angularVelocity": 7.189752262506364e-8,
+                    "velocityX": 3.7663587741235482,
+                    "velocityY": -0.3538089837363426,
+                    "timestamp": 1.0216532955724198
+                },
+                {
+                    "x": 5.7635709161800355,
+                    "y": 5.229941568075057,
+                    "heading": 3.277521372302128,
+                    "angularVelocity": 7.189752282304632e-8,
+                    "velocityX": 3.7663587741235482,
+                    "velocityY": -0.3538089837363426,
+                    "timestamp": 1.1145308678971853
+                },
+                {
+                    "x": 6.113381175624392,
+                    "y": 5.19708064859555,
+                    "heading": 3.2775213789797957,
+                    "angularVelocity": 7.189752355462406e-8,
+                    "velocityX": 3.766358774120127,
+                    "velocityY": -0.3538089837727691,
+                    "timestamp": 1.2074084402219507
+                },
+                {
+                    "x": 6.463191352381069,
+                    "y": 5.164218848902877,
+                    "heading": 3.277521385657466,
+                    "angularVelocity": 7.189755308023614e-8,
+                    "velocityX": 3.766357883833297,
+                    "velocityY": -0.35381846090640534,
+                    "timestamp": 1.3002860125467162
+                },
+                {
+                    "x": 6.802417095902647,
+                    "y": 5.072715014503353,
+                    "heading": 3.277521392775139,
+                    "angularVelocity": 7.663500778773157e-8,
+                    "velocityX": 3.6523967523118,
+                    "velocityY": -0.9852091533956361,
+                    "timestamp": 1.3931635848714816
+                },
+                {
+                    "x": 7.115900861455289,
+                    "y": 4.914048826005074,
+                    "heading": 3.277521402122426,
+                    "angularVelocity": 1.006409437934784e-7,
+                    "velocityX": 3.375236429053956,
+                    "velocityY": -1.7083369485958357,
+                    "timestamp": 1.486041157196247
+                },
+                {
+                    "x": 7.364244703869027,
+                    "y": 4.725111728374696,
+                    "heading": 3.264201738164946,
+                    "angularVelocity": -0.1434109831263146,
+                    "velocityX": 2.673883868813388,
+                    "velocityY": -2.034259648494277,
+                    "timestamp": 1.5789187295210125
+                },
+                {
+                    "x": 7.541281052388914,
+                    "y": 4.527619244510334,
+                    "heading": 3.236812571923802,
+                    "angularVelocity": -0.29489537199973426,
+                    "velocityX": 1.9061259256523482,
+                    "velocityY": -2.1263743110531466,
+                    "timestamp": 1.671796301845778
+                },
+                {
+                    "x": 7.646550613403749,
+                    "y": 4.327374608371355,
+                    "heading": 3.1953955958101785,
+                    "angularVelocity": -0.44593086443733326,
+                    "velocityX": 1.1334228315824013,
+                    "velocityY": -2.156006354674992,
+                    "timestamp": 1.7646738741705434
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": -0.5964367330411746,
+                    "velocityX": 0.3601449279842051,
+                    "velocityY": -2.1574057477590434,
+                    "timestamp": 1.857551446495309
+                },
+                {
+                    "x": 7.652622542952261,
+                    "y": 3.949180878788953,
+                    "heading": 3.079427144579805,
+                    "angularVelocity": -0.7304577329243706,
+                    "velocityX": -0.33014912487777537,
+                    "velocityY": -2.1443491684415465,
+                    "timestamp": 1.940475964492665
+                },
+                {
+                    "x": 7.56799601367521,
+                    "y": 3.773464984873132,
+                    "heading": 3.0103677863230933,
+                    "angularVelocity": -0.832797825353816,
+                    "velocityX": -1.0205248257186026,
+                    "velocityY": -2.1189860147444324,
+                    "timestamp": 2.0234004824900214
+                },
+                {
+                    "x": 7.426167818552031,
+                    "y": 3.601500026216296,
+                    "heading": 2.937043425915028,
+                    "angularVelocity": -0.8842301671310652,
+                    "velocityX": -1.7103288454169892,
+                    "velocityY": -2.0737528876841793,
+                    "timestamp": 2.1063250004873777
+                },
+                {
+                    "x": 7.22742147527628,
+                    "y": 3.436376202929032,
+                    "heading": 2.8673776999748726,
+                    "angularVelocity": -0.8401101100446183,
+                    "velocityX": -2.3967138799900867,
+                    "velocityY": -1.9912545441931753,
+                    "timestamp": 2.189249518484734
+                },
+                {
+                    "x": 6.9736090926507135,
+                    "y": 3.285744578395889,
+                    "heading": 2.8218659539774595,
+                    "angularVelocity": -0.5488334101485464,
+                    "velocityX": -3.0607640388534922,
+                    "velocityY": -1.8164908059875007,
+                    "timestamp": 2.2721740364820904
+                },
+                {
+                    "x": 6.6802459764564,
+                    "y": 3.1746372799457343,
+                    "heading": 2.8218659065870306,
+                    "angularVelocity": -5.714887481984483e-7,
+                    "velocityX": -3.5377126485518553,
+                    "velocityY": -1.3398606483753976,
+                    "timestamp": 2.3550985544794467
+                },
+                {
+                    "x": 6.371560257590401,
+                    "y": 3.1187813398722177,
+                    "heading": 2.821865892471116,
+                    "angularVelocity": -1.702260676188045e-7,
+                    "velocityX": -3.722490360159113,
+                    "velocityY": -0.6735756977845532,
+                    "timestamp": 2.438023072476803
+                },
+                {
+                    "x": 6.0583631867424375,
+                    "y": 3.1010512989609436,
+                    "heading": 2.821865878845265,
+                    "angularVelocity": -1.6431631700415249e-7,
+                    "velocityX": -3.7768934738692015,
+                    "velocityY": -0.21380939364446908,
+                    "timestamp": 2.5209475904741594
+                },
+                {
+                    "x": 5.745166091358163,
+                    "y": 3.0833216914831887,
+                    "heading": 2.821865865219415,
+                    "angularVelocity": -1.6431629349932233e-7,
+                    "velocityX": -3.7768937697564957,
+                    "velocityY": -0.2138041668004641,
+                    "timestamp": 2.6038721084715157
+                },
+                {
+                    "x": 5.431968995973753,
+                    "y": 3.065592084007832,
+                    "heading": 2.8218658515935657,
+                    "angularVelocity": -1.6431629385226094e-7,
+                    "velocityX": -3.7768937697581326,
+                    "velocityY": -0.213804166771547,
+                    "timestamp": 2.686796626468872
+                },
+                {
+                    "x": 5.118771900355572,
+                    "y": 3.047862480662691,
+                    "heading": 2.821865837967647,
+                    "angularVelocity": -1.6431712462133498e-7,
+                    "velocityX": -3.7768937725772025,
+                    "velocityY": -0.21380411696461904,
+                    "timestamp": 2.7697211444662284
+                },
+                {
+                    "x": 4.833327806211659,
+                    "y": 3.0319657734270002,
+                    "heading": 2.7501020518876387,
+                    "angularVelocity": -0.8654109521902384,
+                    "velocityX": -3.4422158975107173,
+                    "velocityY": -0.1917009301904812,
+                    "timestamp": 2.8526456624635848
+                },
+                {
+                    "x": 4.6051204329349895,
+                    "y": 3.019195659985863,
+                    "heading": 2.6830003866678815,
+                    "angularVelocity": -0.809189692509236,
+                    "velocityX": -2.7519891437167656,
+                    "velocityY": -0.15399683651516197,
+                    "timestamp": 2.935570180460941
+                },
+                {
+                    "x": 4.434032594221504,
+                    "y": 3.009603052392577,
+                    "heading": 2.6296182647389936,
+                    "angularVelocity": -0.6437435298760438,
+                    "velocityX": -2.063175558270236,
+                    "velocityY": -0.11567878626188524,
+                    "timestamp": 3.0184946984582974
+                },
+                {
+                    "x": 4.320003715449931,
+                    "y": 3.0032022577019606,
+                    "heading": 2.5928181043840297,
+                    "angularVelocity": -0.44377900823176664,
+                    "velocityX": -1.3750924518513103,
+                    "velocityY": -0.07718820495068371,
+                    "timestamp": 3.1014192164556538
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": -0.22693052475300574,
+                    "velocityX": -0.6874169042712922,
+                    "velocityY": -0.03861653681318734,
+                    "timestamp": 3.18434373445301
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 3.2672682524503665
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W2 To C3 To S4": {
+            "waypoints": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 20
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 18
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": -4.116881309267156e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.3172206852139876,
+                    "y": 5.557554237561194,
+                    "heading": 3.1526693404609274,
+                    "angularVelocity": 0.14095873188728006,
+                    "velocityX": 0.747895486240753,
+                    "velocityY": -0.02721148531256728,
+                    "timestamp": 0.08987978460983724
+                },
+                {
+                    "x": 2.4516878784162954,
+                    "y": 5.552665336111155,
+                    "heading": 3.1761213228598115,
+                    "angularVelocity": 0.2609261081415128,
+                    "velocityX": 1.4960782759551832,
+                    "velocityY": -0.054393782442455275,
+                    "timestamp": 0.17975956921967448
+                },
+                {
+                    "x": 2.6534358807362874,
+                    "y": 5.545338085116866,
+                    "heading": 3.2071661677578067,
+                    "angularVelocity": 0.34540408649958604,
+                    "velocityX": 2.2446426990871093,
+                    "velocityY": -0.08152279209504047,
+                    "timestamp": 0.2696393538295117
+                },
+                {
+                    "x": 2.9224981140685604,
+                    "y": 5.535583877021325,
+                    "heading": 3.239227837145004,
+                    "angularVelocity": 0.3567172476694272,
+                    "velocityX": 2.9935789732947984,
+                    "velocityY": -0.10852504974159909,
+                    "timestamp": 0.35951913843934896
+                },
+                {
+                    "x": 3.258623258854089,
+                    "y": 5.523455945202495,
+                    "heading": 3.2503847245896496,
+                    "angularVelocity": 0.1241312214205911,
+                    "velocityX": 3.739719073033272,
+                    "velocityY": -0.13493503429591652,
+                    "timestamp": 0.4493989230491862
+                },
+                {
+                    "x": 3.598405730984745,
+                    "y": 5.5110225450854795,
+                    "heading": 3.2503847355595754,
+                    "angularVelocity": 1.2205109450182653e-7,
+                    "velocityX": 3.7804103960154256,
+                    "velocityY": -0.13833366614078543,
+                    "timestamp": 0.5392787076590234
+                },
+                {
+                    "x": 3.938188203087489,
+                    "y": 5.498589144205611,
+                    "heading": 3.2503847465294977,
+                    "angularVelocity": 1.22051053234482e-7,
+                    "velocityX": 3.7804103957048714,
+                    "velocityY": -0.13833367462827542,
+                    "timestamp": 0.6291584922688607
+                },
+                {
+                    "x": 4.277970675190232,
+                    "y": 5.486155743325738,
+                    "heading": 3.25038475749942,
+                    "angularVelocity": 1.22051052798414e-7,
+                    "velocityX": 3.7804103957048705,
+                    "velocityY": -0.13833367462831278,
+                    "timestamp": 0.7190382768786979
+                },
+                {
+                    "x": 4.617753147292976,
+                    "y": 5.473722342445865,
+                    "heading": 3.2503847684693423,
+                    "angularVelocity": 1.2205105381924992e-7,
+                    "velocityX": 3.7804103957048705,
+                    "velocityY": -0.1383336746283128,
+                    "timestamp": 0.8089180614885352
+                },
+                {
+                    "x": 4.957535619395719,
+                    "y": 5.461288941565993,
+                    "heading": 3.2503847794392646,
+                    "angularVelocity": 1.2205105317325576e-7,
+                    "velocityX": 3.78041039570487,
+                    "velocityY": -0.13833367462831272,
+                    "timestamp": 0.8987978460983724
+                },
+                {
+                    "x": 5.297318091498462,
+                    "y": 5.44885554068612,
+                    "heading": 3.2503847904091874,
+                    "angularVelocity": 1.2205105386911006e-7,
+                    "velocityX": 3.78041039570487,
+                    "velocityY": -0.13833367462831295,
+                    "timestamp": 0.9886776307082097
+                },
+                {
+                    "x": 5.637100563601198,
+                    "y": 5.436422139806048,
+                    "heading": 3.2503848013791092,
+                    "angularVelocity": 1.2205105333670814e-7,
+                    "velocityX": 3.7804103957047883,
+                    "velocityY": -0.13833367463053942,
+                    "timestamp": 1.0785574153180468
+                },
+                {
+                    "x": 5.976883034047419,
+                    "y": 5.423988693656475,
+                    "heading": 3.2503848123490315,
+                    "angularVelocity": 1.2205105297878886e-7,
+                    "velocityX": 3.7804103772744444,
+                    "velocityY": -0.1383341782977477,
+                    "timestamp": 1.168437199927884
+                },
+                {
+                    "x": 6.3161557778942825,
+                    "y": 5.401611852989575,
+                    "heading": 3.2503848233905317,
+                    "angularVelocity": 1.2284742555590528e-7,
+                    "velocityX": 3.774739173214837,
+                    "velocityY": -0.24896411094026674,
+                    "timestamp": 1.2583169845377211
+                },
+                {
+                    "x": 6.6443599765864345,
+                    "y": 5.312793385156624,
+                    "heading": 3.2503848356259346,
+                    "angularVelocity": 1.3613075412636898e-7,
+                    "velocityX": 3.6515908456709,
+                    "velocityY": -0.9881918188666926,
+                    "timestamp": 1.3481967691475583
+                },
+                {
+                    "x": 6.948614681552646,
+                    "y": 5.161017938085189,
+                    "heading": 3.2503848519237777,
+                    "angularVelocity": 1.8132935219655535e-7,
+                    "velocityX": 3.3851294402514114,
+                    "velocityY": -1.6886494302392412,
+                    "timestamp": 1.4380765537573954
+                },
+                {
+                    "x": 7.216978880977283,
+                    "y": 4.952242107786858,
+                    "heading": 3.2503848494896554,
+                    "angularVelocity": -2.7081974161956603e-8,
+                    "velocityX": 2.985812667326646,
+                    "velocityY": -2.322834118980229,
+                    "timestamp": 1.5279563383672325
+                },
+                {
+                    "x": 7.434432646263598,
+                    "y": 4.699605663740004,
+                    "heading": 3.233827697906493,
+                    "angularVelocity": -0.18421441100386476,
+                    "velocityX": 2.4193845838667345,
+                    "velocityY": -2.8108260955843263,
+                    "timestamp": 1.6178361229770697
+                },
+                {
+                    "x": 7.589713042543029,
+                    "y": 4.421668707909288,
+                    "heading": 3.1971997410199715,
+                    "angularVelocity": -0.40752163621075216,
+                    "velocityX": 1.7276453982782098,
+                    "velocityY": -3.0923188905848704,
+                    "timestamp": 1.7077159075869068
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": -0.6364027380377996,
+                    "velocityX": 1.0045301938460758,
+                    "velocityY": -3.2784759018773304,
+                    "timestamp": 1.797595692196744
+                },
+                {
+                    "x": 7.709767191117991,
+                    "y": 3.879341169574254,
+                    "heading": 3.0798110703023642,
+                    "angularVelocity": -0.8233834909878134,
+                    "velocityX": 0.40721464665943,
+                    "velocityY": -3.3879684087134527,
+                    "timestamp": 1.8706952021534504
+                },
+                {
+                    "x": 7.695293406413974,
+                    "y": 3.6265437637575832,
+                    "heading": 3.0152640841936,
+                    "angularVelocity": -0.8830016254143865,
+                    "velocityX": -0.19800111810029022,
+                    "velocityY": -3.4582640289435425,
+                    "timestamp": 1.9437947121101569
+                },
+                {
+                    "x": 7.636370047387183,
+                    "y": 3.3734118596165046,
+                    "heading": 2.9616083332383942,
+                    "angularVelocity": -0.7340097216379668,
+                    "velocityX": -0.8060705066515889,
+                    "velocityY": -3.46283996009006,
+                    "timestamp": 2.0168942220668633
+                },
+                {
+                    "x": 7.533938934527094,
+                    "y": 3.12599493210629,
+                    "heading": 2.935877856016127,
+                    "angularVelocity": -0.3519924721450196,
+                    "velocityX": -1.4012558076073192,
+                    "velocityY": -3.3846591811184132,
+                    "timestamp": 2.08999373202357
+                },
+                {
+                    "x": 7.389145536880011,
+                    "y": 2.8904033965878195,
+                    "heading": 2.9358731972146472,
+                    "angularVelocity": -0.00006373232119512069,
+                    "velocityX": -1.9807711123211746,
+                    "velocityY": -3.2228880283604715,
+                    "timestamp": 2.1630932419802766
+                },
+                {
+                    "x": 7.2083948751557925,
+                    "y": 2.6811221610888363,
+                    "heading": 2.935873140353821,
+                    "angularVelocity": -7.778550956603191e-7,
+                    "velocityX": -2.4726658472985523,
+                    "velocityY": -2.8629635906306268,
+                    "timestamp": 2.2361927519369833
+                },
+                {
+                    "x": 6.996380296591032,
+                    "y": 2.5035855179171795,
+                    "heading": 2.9358730970284697,
+                    "angularVelocity": -5.92690040120521e-7,
+                    "velocityX": -2.9003556753023303,
+                    "velocityY": -2.428698130491067,
+                    "timestamp": 2.30929226189369
+                },
+                {
+                    "x": 6.758605859634312,
+                    "y": 2.362402091220058,
+                    "heading": 2.935873058757806,
+                    "angularVelocity": -5.235419952154551e-7,
+                    "velocityX": -3.252750081327982,
+                    "velocityY": -1.931386773738272,
+                    "timestamp": 2.3823917718503966
+                },
+                {
+                    "x": 6.50822939770154,
+                    "y": 2.2450092870168694,
+                    "heading": 2.935873021282274,
+                    "angularVelocity": -5.126646128379785e-7,
+                    "velocityX": -3.4251455595402076,
+                    "velocityY": -1.6059314798786082,
+                    "timestamp": 2.4554912818071033
+                },
+                {
+                    "x": 6.2578527752872475,
+                    "y": 2.12761682509076,
+                    "heading": 2.935872983806744,
+                    "angularVelocity": -5.126645909112232e-7,
+                    "velocityX": -3.4251477549245686,
+                    "velocityY": -1.6059267975342413,
+                    "timestamp": 2.52859079176381
+                },
+                {
+                    "x": 6.007476152294899,
+                    "y": 2.010224364397617,
+                    "heading": 2.9358729463311364,
+                    "angularVelocity": -5.126656467056276e-7,
+                    "velocityX": -3.425147762832367,
+                    "velocityY": -1.6059267806672737,
+                    "timestamp": 2.6016903017205166
+                },
+                {
+                    "x": 5.771581826202875,
+                    "y": 1.8997576718980689,
+                    "heading": 2.8963919451713847,
+                    "angularVelocity": -0.540099395784409,
+                    "velocityX": -3.227030198037426,
+                    "velocityY": -1.5111823945872924,
+                    "timestamp": 2.6747898116772233
+                },
+                {
+                    "x": 5.572260449403434,
+                    "y": 1.8062657885588822,
+                    "heading": 2.7909821338342944,
+                    "angularVelocity": -1.4420043499543171,
+                    "velocityX": -2.7267129002300594,
+                    "velocityY": -1.2789673062725992,
+                    "timestamp": 2.74788932163393
+                },
+                {
+                    "x": 5.413306384281217,
+                    "y": 1.7316311280227,
+                    "heading": 2.684821280424371,
+                    "angularVelocity": -1.4522785921931365,
+                    "velocityX": -2.1744887922827503,
+                    "velocityY": -1.0210008327056677,
+                    "timestamp": 2.8209888315906366
+                },
+                {
+                    "x": 5.294461986790537,
+                    "y": 1.6757665506891541,
+                    "heading": 2.596152824094934,
+                    "angularVelocity": -1.2129829103089709,
+                    "velocityX": -1.6257892503119276,
+                    "velocityY": -0.7642264273266058,
+                    "timestamp": 2.8940883415473433
+                },
+                {
+                    "x": 5.2154339068234155,
+                    "y": 1.6385760788874908,
+                    "heading": 2.5330095835299122,
+                    "angularVelocity": -0.8637984112673935,
+                    "velocityX": -1.0811027326165235,
+                    "velocityY": -0.5087649947815693,
+                    "timestamp": 2.96718785150405
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": -0.45157051736003884,
+                    "velocityX": -0.5394551460983961,
+                    "velocityY": -0.2541204297879725,
+                    "timestamp": 3.0402873614607566
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": 0,
+                    "velocityX": 1.8516151332127108e-31,
+                    "velocityY": 3.1624508891409975e-32,
+                    "timestamp": 3.1133868714174633
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W2 To C4 To S3": {
+            "waypoints": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 22
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 17
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 2.1305523573234936e-32,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.3154171108625845,
+                    "y": 5.531249621704393,
+                    "heading": 3.152034306361113,
+                    "angularVelocity": 0.12992542777148963,
+                    "velocityX": 0.7062597425523925,
+                    "velocityY": -0.3103963856794681,
+                    "timestamp": 0.09262471994534183
+                },
+                {
+                    "x": 2.4462513478934307,
+                    "y": 5.473748850345723,
+                    "heading": 3.1761003812465023,
+                    "angularVelocity": 0.2598234564119746,
+                    "velocityX": 1.4125196503487585,
+                    "velocityY": -0.6207929307921308,
+                    "timestamp": 0.18524943989068365
+                },
+                {
+                    "x": 2.642502734215126,
+                    "y": 5.387497655476605,
+                    "heading": 3.2121940698357436,
+                    "angularVelocity": 0.38967662855597096,
+                    "velocityX": 2.1187798077824542,
+                    "velocityY": -0.931189804622509,
+                    "timestamp": 0.2778741598360255
+                },
+                {
+                    "x": 2.9041712871426104,
+                    "y": 5.2724959877699655,
+                    "heading": 3.2603098134687913,
+                    "angularVelocity": 0.5194697879944143,
+                    "velocityX": 2.8250401521526483,
+                    "velocityY": -1.2415872110005046,
+                    "timestamp": 0.3704988797813673
+                },
+                {
+                    "x": 3.22495104061126,
+                    "y": 5.131513113466323,
+                    "heading": 3.260309819428016,
+                    "angularVelocity": 6.433730075600531e-8,
+                    "velocityX": 3.4632196853922204,
+                    "velocityY": -1.5220869157481582,
+                    "timestamp": 0.46312359972670913
+                },
+                {
+                    "x": 3.545730793052991,
+                    "y": 4.99053023682554,
+                    "heading": 3.2603098253870204,
+                    "angularVelocity": 6.433492130788198e-8,
+                    "velocityX": 3.4632196743053516,
+                    "velocityY": -1.5220869409805253,
+                    "timestamp": 0.555748319672051
+                },
+                {
+                    "x": 3.866510545494718,
+                    "y": 4.849547360184748,
+                    "heading": 3.2603098313460244,
+                    "angularVelocity": 6.433492218112887e-8,
+                    "velocityX": 3.463219674305308,
+                    "velocityY": -1.5220869409806246,
+                    "timestamp": 0.6483730396173928
+                },
+                {
+                    "x": 4.187290297936445,
+                    "y": 4.708564483543956,
+                    "heading": 3.2603098373050288,
+                    "angularVelocity": 6.433492230011589e-8,
+                    "velocityX": 3.463219674305308,
+                    "velocityY": -1.5220869409806246,
+                    "timestamp": 0.7409977595627346
+                },
+                {
+                    "x": 4.508070050378173,
+                    "y": 4.567581606903164,
+                    "heading": 3.260309843264033,
+                    "angularVelocity": 6.433492243162183e-8,
+                    "velocityX": 3.463219674305308,
+                    "velocityY": -1.5220869409806246,
+                    "timestamp": 0.8336224795080764
+                },
+                {
+                    "x": 4.828849802819899,
+                    "y": 4.426598730262372,
+                    "heading": 3.260309849223037,
+                    "angularVelocity": 6.433492147964013e-8,
+                    "velocityX": 3.463219674305308,
+                    "velocityY": -1.5220869409806246,
+                    "timestamp": 0.9262471994534183
+                },
+                {
+                    "x": 5.149629555261627,
+                    "y": 4.28561585362158,
+                    "heading": 3.2603098551820415,
+                    "angularVelocity": 6.433492220525221e-8,
+                    "velocityX": 3.4632196743053076,
+                    "velocityY": -1.5220869409806248,
+                    "timestamp": 1.01887191939876
+                },
+                {
+                    "x": 5.470409307703354,
+                    "y": 4.1446329769807875,
+                    "heading": 3.260309861141046,
+                    "angularVelocity": 6.433492177225282e-8,
+                    "velocityX": 3.463219674305308,
+                    "velocityY": -1.5220869409806244,
+                    "timestamp": 1.111496639344102
+                },
+                {
+                    "x": 5.791189060145081,
+                    "y": 4.003650100339995,
+                    "heading": 3.2603098671000503,
+                    "angularVelocity": 6.433492264273842e-8,
+                    "velocityX": 3.463219674305308,
+                    "velocityY": -1.5220869409806244,
+                    "timestamp": 1.2041213592894437
+                },
+                {
+                    "x": 6.111968812586809,
+                    "y": 3.8626672236992023,
+                    "heading": 3.2603098730590543,
+                    "angularVelocity": 6.433492200729343e-8,
+                    "velocityX": 3.463219674305308,
+                    "velocityY": -1.5220869409806248,
+                    "timestamp": 1.2967460792347856
+                },
+                {
+                    "x": 6.432748565028133,
+                    "y": 3.7216843470574936,
+                    "heading": 3.2603098790180582,
+                    "angularVelocity": 6.43349214222261e-8,
+                    "velocityX": 3.4632196743009604,
+                    "velocityY": -1.5220869409905164,
+                    "timestamp": 1.3893707991801274
+                },
+                {
+                    "x": 6.753528214821051,
+                    "y": 3.5807012368590723,
+                    "heading": 3.260309884977063,
+                    "angularVelocity": 6.433492684215086e-8,
+                    "velocityX": 3.4632185660826984,
+                    "velocityY": -1.5220894625281032,
+                    "timestamp": 1.4819955191254692
+                },
+                {
+                    "x": 7.055963078223925,
+                    "y": 3.403757816572982,
+                    "heading": 3.260309891160044,
+                    "angularVelocity": 6.675302824117912e-8,
+                    "velocityX": 3.2651635932755534,
+                    "velocityY": -1.9103261029075773,
+                    "timestamp": 1.574620239070811
+                },
+                {
+                    "x": 7.316159274039154,
+                    "y": 3.1690804957678615,
+                    "heading": 3.2603099328635268,
+                    "angularVelocity": 4.502413954005259e-7,
+                    "velocityX": 2.809144210841027,
+                    "velocityY": -2.533635955321695,
+                    "timestamp": 1.6672449590161529
+                },
+                {
+                    "x": 7.507506683295118,
+                    "y": 2.9535313290497767,
+                    "heading": 3.2483076831663804,
+                    "angularVelocity": -0.12957933588602658,
+                    "velocityX": 2.0658352259405257,
+                    "velocityY": -2.3271235459095725,
+                    "timestamp": 1.7598696789614947
+                },
+                {
+                    "x": 7.631583047935508,
+                    "y": 2.7620770453961936,
+                    "heading": 3.2242582782979583,
+                    "angularVelocity": -0.25964348267518605,
+                    "velocityX": 1.3395599437559367,
+                    "velocityY": -2.066989069079623,
+                    "timestamp": 1.8524943989068365
+                },
+                {
+                    "x": 7.68897880573514,
+                    "y": 2.5963063443763943,
+                    "heading": 3.1881570664078476,
+                    "angularVelocity": -0.3897578520228113,
+                    "velocityX": 0.6196591777389554,
+                    "velocityY": -1.7897025882250568,
+                    "timestamp": 1.9451191188521784
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": -0.5199159191656997,
+                    "velocityX": -0.09693746702217505,
+                    "velocityY": -1.5039866728730655,
+                    "timestamp": 2.03774383879752
+                },
+                {
+                    "x": 7.6191836827180115,
+                    "y": 2.3540612532810705,
+                    "heading": 3.087368523551757,
+                    "angularVelocity": -0.6362125445554702,
+                    "velocityX": -0.73515140709603,
+                    "velocityY": -1.244329940996524,
+                    "timestamp": 2.1204700863364088
+                },
+                {
+                    "x": 7.505717742673261,
+                    "y": 2.2730171165833535,
+                    "heading": 3.0268817588529657,
+                    "angularVelocity": -0.7311677550750295,
+                    "velocityX": -1.3715833054244395,
+                    "velocityY": -0.979666540049693,
+                    "timestamp": 2.2031963338752973
+                },
+                {
+                    "x": 7.339845145495756,
+                    "y": 2.2145049491373583,
+                    "heading": 2.9612826526647904,
+                    "angularVelocity": -0.7929660553905423,
+                    "velocityX": -2.005078220180729,
+                    "velocityY": -0.7072987012796481,
+                    "timestamp": 2.285922581414186
+                },
+                {
+                    "x": 7.122035574070675,
+                    "y": 2.1796305402319347,
+                    "heading": 2.895398132602207,
+                    "angularVelocity": -0.7964161559680392,
+                    "velocityX": -2.6328955791532933,
+                    "velocityY": -0.4215640131510867,
+                    "timestamp": 2.3686488289530745
+                },
+                {
+                    "x": 6.853520994844272,
+                    "y": 2.1707672940791287,
+                    "heading": 2.839999953502189,
+                    "angularVelocity": -0.6696566174354214,
+                    "velocityX": -3.245820851479772,
+                    "velocityY": -0.1071394680223975,
+                    "timestamp": 2.451375076491963
+                },
+                {
+                    "x": 6.54155924782903,
+                    "y": 2.195597775343439,
+                    "heading": 2.83999969260268,
+                    "angularVelocity": -0.0000031537694148635814,
+                    "velocityX": -3.7710129045632463,
+                    "velocityY": 0.30015239422817563,
+                    "timestamp": 2.5341013240308516
+                },
+                {
+                    "x": 6.239292092495854,
+                    "y": 2.276661405774626,
+                    "heading": 2.8399996811990342,
+                    "angularVelocity": -1.3784796655157707e-7,
+                    "velocityX": -3.6538240803329365,
+                    "velocityY": 0.9799021815062957,
+                    "timestamp": 2.61682757156974
+                },
+                {
+                    "x": 5.945407443635962,
+                    "y": 2.3842186409280646,
+                    "heading": 2.8399996700710783,
+                    "angularVelocity": -1.345154198919157e-7,
+                    "velocityX": -3.5524958233085573,
+                    "velocityY": 1.3001585150212127,
+                    "timestamp": 2.699553819108629
+                },
+                {
+                    "x": 5.651522869698407,
+                    "y": 2.491776080795775,
+                    "heading": 2.8399996589431233,
+                    "angularVelocity": -1.3451541092672365e-7,
+                    "velocityX": -3.5524949176427194,
+                    "velocityY": 1.3001609896200028,
+                    "timestamp": 2.7822800666475174
+                },
+                {
+                    "x": 5.35763829576127,
+                    "y": 2.5993335206646324,
+                    "heading": 2.839999647815168,
+                    "angularVelocity": -1.3451540998350566e-7,
+                    "velocityX": -3.5524949176376506,
+                    "velocityY": 1.3001609896338533,
+                    "timestamp": 2.865006314186406
+                },
+                {
+                    "x": 5.063753722106806,
+                    "y": 2.7068909613057275,
+                    "heading": 2.8399996366871263,
+                    "angularVelocity": -1.3451645746164066e-7,
+                    "velocityX": -3.5524949142206896,
+                    "velocityY": 1.3001609989687195,
+                    "timestamp": 2.9477325617252945
+                },
+                {
+                    "x": 4.796835976566279,
+                    "y": 2.804592771493776,
+                    "heading": 2.7513017358564444,
+                    "angularVelocity": -1.0721857145640414,
+                    "velocityX": -3.226518227060295,
+                    "velocityY": 1.1810255281084718,
+                    "timestamp": 3.030458809264183
+                },
+                {
+                    "x": 4.583301664019943,
+                    "y": 2.882755208599232,
+                    "heading": 2.680367864718666,
+                    "angularVelocity": -0.8574530242585245,
+                    "velocityX": -2.581215985240433,
+                    "velocityY": 0.9448323770362348,
+                    "timestamp": 3.1131850568030717
+                },
+                {
+                    "x": 4.42315083623892,
+                    "y": 2.9413775026442943,
+                    "heading": 2.627181361737275,
+                    "angularVelocity": -0.6429217396375844,
+                    "velocityX": -1.9359131176080184,
+                    "velocityY": 0.7086299184246748,
+                    "timestamp": 3.1959113043419602
+                },
+                {
+                    "x": 4.316383597066322,
+                    "y": 2.980459172951351,
+                    "heading": 2.5917278056853306,
+                    "angularVelocity": -0.4285647797003965,
+                    "velocityX": -1.2906089947136565,
+                    "velocityY": 0.47242164935239944,
+                    "timestamp": 3.278637551880849
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": -0.21429481226012745,
+                    "velocityX": -0.645304225133951,
+                    "velocityY": 0.23621072670391596,
+                    "timestamp": 3.3613637994197374
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": 0,
+                    "velocityX": 2.7848791215143805e-32,
+                    "velocityY": -1.0794258279039968e-32,
+                    "timestamp": 3.444090046958626
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W2 To C4 To S4": {
+            "waypoints": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 22
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.3154271184850326,
+                    "y": 5.536626406530092,
+                    "heading": 3.157504964629553,
+                    "angularVelocity": 0.19152994653168934,
+                    "velocityX": 0.7158684847619835,
+                    "velocityY": -0.25574133980197417,
+                    "timestamp": 0.09139544466297708
+                },
+                {
+                    "x": 2.4462968906270097,
+                    "y": 5.489871788853177,
+                    "heading": 3.1916196887834967,
+                    "angularVelocity": 0.3732650383145345,
+                    "velocityX": 1.4319069470536998,
+                    "velocityY": -0.5115639827490647,
+                    "timestamp": 0.18279088932595416
+                },
+                {
+                    "x": 2.6426368598266095,
+                    "y": 5.419722773289121,
+                    "heading": 3.240625955812914,
+                    "angularVelocity": 0.536200323878483,
+                    "velocityX": 2.148246774482153,
+                    "velocityY": -0.7675329533406525,
+                    "timestamp": 0.2741863339889312
+                },
+                {
+                    "x": 2.9045066521861433,
+                    "y": 5.326149233940284,
+                    "heading": 3.2998389732795825,
+                    "angularVelocity": 0.6478771199673835,
+                    "velocityX": 2.8652389987837097,
+                    "velocityY": -1.023831545367414,
+                    "timestamp": 0.36558177865190833
+                },
+                {
+                    "x": 3.230136445009879,
+                    "y": 5.209943279570635,
+                    "heading": 3.2998389932105203,
+                    "angularVelocity": 2.1807364619110316e-7,
+                    "velocityX": 3.5628667711449338,
+                    "velocityY": -1.2714633075877884,
+                    "timestamp": 0.45697722331488544
+                },
+                {
+                    "x": 3.555730781054687,
+                    "y": 5.093637996752385,
+                    "heading": 3.29983900348091,
+                    "angularVelocity": 1.1237311095674653e-7,
+                    "velocityX": 3.5624788220621486,
+                    "velocityY": -1.2725501062676312,
+                    "timestamp": 0.5483726679778625
+                },
+                {
+                    "x": 3.881325116955394,
+                    "y": 4.977332713530725,
+                    "heading": 3.2998390137513005,
+                    "angularVelocity": 1.123731101511981e-7,
+                    "velocityX": 3.562478820485468,
+                    "velocityY": -1.2725501106815165,
+                    "timestamp": 0.6397681126408397
+                },
+                {
+                    "x": 4.2069194528561,
+                    "y": 4.861027430309063,
+                    "heading": 3.2998390240216913,
+                    "angularVelocity": 1.1237311074592376e-7,
+                    "velocityX": 3.562478820485462,
+                    "velocityY": -1.2725501106815353,
+                    "timestamp": 0.7311635573038168
+                },
+                {
+                    "x": 4.532513788756806,
+                    "y": 4.744722147087402,
+                    "heading": 3.2998390342920816,
+                    "angularVelocity": 1.1237311063100021e-7,
+                    "velocityX": 3.562478820485462,
+                    "velocityY": -1.272550110681535,
+                    "timestamp": 0.8225590019667939
+                },
+                {
+                    "x": 4.858108124657512,
+                    "y": 4.62841686386574,
+                    "heading": 3.299839044562472,
+                    "angularVelocity": 1.1237311003038478e-7,
+                    "velocityX": 3.5624788204854614,
+                    "velocityY": -1.272550110681535,
+                    "timestamp": 0.913954446629771
+                },
+                {
+                    "x": 5.183702460558219,
+                    "y": 4.5121115806440795,
+                    "heading": 3.2998390548328627,
+                    "angularVelocity": 1.1237311094761516e-7,
+                    "velocityX": 3.5624788204854614,
+                    "velocityY": -1.272550110681535,
+                    "timestamp": 1.005349891292748
+                },
+                {
+                    "x": 5.509296796458925,
+                    "y": 4.395806297422418,
+                    "heading": 3.299839065103253,
+                    "angularVelocity": 1.1237310968464263e-7,
+                    "velocityX": 3.562478820485462,
+                    "velocityY": -1.2725501106815353,
+                    "timestamp": 1.096745335955725
+                },
+                {
+                    "x": 5.834891132359631,
+                    "y": 4.279501014200756,
+                    "heading": 3.2998390753736433,
+                    "angularVelocity": 1.123731107718446e-7,
+                    "velocityX": 3.562478820485462,
+                    "velocityY": -1.2725501106815351,
+                    "timestamp": 1.188140780618702
+                },
+                {
+                    "x": 6.160485468260297,
+                    "y": 4.16319573097898,
+                    "heading": 3.299839085644034,
+                    "angularVelocity": 1.1237310986388625e-7,
+                    "velocityX": 3.562478820485015,
+                    "velocityY": -1.2725501106827837,
+                    "timestamp": 1.279536225281679
+                },
+                {
+                    "x": 6.4860797940952795,
+                    "y": 4.046890419578527,
+                    "heading": 3.2998390959144244,
+                    "angularVelocity": 1.1237311101180894e-7,
+                    "velocityX": 3.5624787103517215,
+                    "velocityY": -1.2725504189987695,
+                    "timestamp": 1.370931669944656
+                },
+                {
+                    "x": 6.809159340398316,
+                    "y": 3.9237717890604613,
+                    "heading": 3.299839106230886,
+                    "angularVelocity": 1.1287719938305865e-7,
+                    "velocityX": 3.5349633397413522,
+                    "velocityY": -1.3470981072657797,
+                    "timestamp": 1.462327114607633
+                },
+                {
+                    "x": 7.101008389413158,
+                    "y": 3.738399942122118,
+                    "heading": 3.299839117623808,
+                    "angularVelocity": 1.246552472151185e-7,
+                    "velocityX": 3.1932559668706793,
+                    "velocityY": -2.0282394557178747,
+                    "timestamp": 1.55372255927061
+                },
+                {
+                    "x": 7.349782731781937,
+                    "y": 3.498296000909773,
+                    "heading": 3.299839132467128,
+                    "angularVelocity": 1.6240765120244838e-7,
+                    "velocityX": 2.7219555994956917,
+                    "velocityY": -2.6270887142977735,
+                    "timestamp": 1.645118003933587
+                },
+                {
+                    "x": 7.5357710820320545,
+                    "y": 3.2282223692532166,
+                    "heading": 3.2881158704604148,
+                    "angularVelocity": -0.12826965337097124,
+                    "velocityX": 2.0349849047286135,
+                    "velocityY": -2.955001014026776,
+                    "timestamp": 1.736513448596564
+                },
+                {
+                    "x": 7.652314357234611,
+                    "y": 2.959723163992482,
+                    "heading": 3.2581736625054765,
+                    "angularVelocity": -0.32761160105108156,
+                    "velocityX": 1.2751540914573078,
+                    "velocityY": -2.9377744837371598,
+                    "timestamp": 1.827908893259541
+                },
+                {
+                    "x": 7.700165626970879,
+                    "y": 2.701389747494563,
+                    "heading": 3.208935276055406,
+                    "angularVelocity": -0.5387400502456196,
+                    "velocityX": 0.5235629621664141,
+                    "velocityY": -2.826545868347203,
+                    "timestamp": 1.919304337922518
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": -0.7542528657692632,
+                    "velocityX": -0.2206414887004817,
+                    "velocityY": -2.6739817109674653,
+                    "timestamp": 2.0106997825854953
+                },
+                {
+                    "x": 7.619243737534899,
+                    "y": 2.269397277536314,
+                    "heading": 3.071018168568731,
+                    "angularVelocity": -0.9308752015312701,
+                    "velocityX": -0.8198752757491441,
+                    "velocityY": -2.531604604539154,
+                    "timestamp": 2.08480405705527
+                },
+                {
+                    "x": 7.514288335022884,
+                    "y": 2.093479496062301,
+                    "heading": 2.9929449451014944,
+                    "angularVelocity": -1.0535589751865693,
+                    "velocityX": -1.4163204924814625,
+                    "velocityY": -2.373922189141621,
+                    "timestamp": 2.1589083315250446
+                },
+                {
+                    "x": 7.365556927410938,
+                    "y": 1.9309603366701282,
+                    "heading": 2.911854592478602,
+                    "angularVelocity": -1.0942736192096787,
+                    "velocityX": -2.0070557154244444,
+                    "velocityY": -2.193114507294498,
+                    "timestamp": 2.233012605994819
+                },
+                {
+                    "x": 7.174068652738112,
+                    "y": 1.784693946040825,
+                    "heading": 2.8381473535529165,
+                    "angularVelocity": -0.994642204556963,
+                    "velocityX": -2.5840381819125056,
+                    "velocityY": -1.973791548137795,
+                    "timestamp": 2.307116880464594
+                },
+                {
+                    "x": 6.9430736895852,
+                    "y": 1.6600434204320091,
+                    "heading": 2.7943942143969487,
+                    "angularVelocity": -0.5904266585027068,
+                    "velocityX": -3.117161119324147,
+                    "velocityY": -1.6820962960737031,
+                    "timestamp": 2.3812211549343685
+                },
+                {
+                    "x": 6.679335807473864,
+                    "y": 1.5650263715806954,
+                    "heading": 2.794393527099141,
+                    "angularVelocity": -0.000009274738771364558,
+                    "velocityX": -3.5590103809586133,
+                    "velocityY": -1.2822073966879628,
+                    "timestamp": 2.455325429404143
+                },
+                {
+                    "x": 6.403647884167786,
+                    "y": 1.5142107470678074,
+                    "heading": 2.794393497300607,
+                    "angularVelocity": -4.021162732159612e-7,
+                    "velocityX": -3.720270190601743,
+                    "velocityY": -0.6857313545879138,
+                    "timestamp": 2.5294297038739177
+                },
+                {
+                    "x": 6.123365156631299,
+                    "y": 1.5089521104338168,
+                    "heading": 2.7943934713550327,
+                    "angularVelocity": -3.501225017023653e-7,
+                    "velocityX": -3.782274768114868,
+                    "velocityY": -0.07096266270252378,
+                    "timestamp": 2.6035339783436924
+                },
+                {
+                    "x": 5.853614932127584,
+                    "y": 1.5409906174836003,
+                    "heading": 2.7698594509818024,
+                    "angularVelocity": -0.33107429427995533,
+                    "velocityX": -3.6401439246765253,
+                    "velocityY": 0.43234357638746057,
+                    "timestamp": 2.677638252813467
+                },
+                {
+                    "x": 5.6273847181633965,
+                    "y": 1.5674202114884777,
+                    "heading": 2.697593854639993,
+                    "angularVelocity": -0.9751879612723551,
+                    "velocityX": -3.0528632198735024,
+                    "velocityY": 0.3566541092809186,
+                    "timestamp": 2.7517425272832416
+                },
+                {
+                    "x": 5.446628200819817,
+                    "y": 1.588490932967887,
+                    "heading": 2.6254372460611823,
+                    "angularVelocity": -0.9737172260984721,
+                    "velocityX": -2.4392185017249606,
+                    "velocityY": 0.284338813519285,
+                    "timestamp": 2.8258468017530163
+                },
+                {
+                    "x": 5.311228363745466,
+                    "y": 1.6042594522545108,
+                    "heading": 2.5652453697807402,
+                    "angularVelocity": -0.8122591673836874,
+                    "velocityX": -1.8271528605219525,
+                    "velocityY": 0.21278825546128166,
+                    "timestamp": 2.899951076222791
+                },
+                {
+                    "x": 5.22105179084387,
+                    "y": 1.614756634238219,
+                    "heading": 2.5224127669101155,
+                    "angularVelocity": -0.578004483237462,
+                    "velocityX": -1.2168876025954114,
+                    "velocityY": 0.14165420360473746,
+                    "timestamp": 2.9740553506925655
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": -0.302449043195724,
+                    "velocityX": -0.6079513113949444,
+                    "velocityY": 0.07075658994444238,
+                    "timestamp": 3.04815962516234
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": 0,
+                    "velocityX": 8.84173108800433e-33,
+                    "velocityY": 0,
+                    "timestamp": 3.122263899632115
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W3 To W2": {
+            "waypoints": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 10
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 8.35160049146123e-34,
+                    "velocityX": 7.119134799605741e-35,
+                    "velocityY": 1.2756128975400497e-35,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.298001258966314,
+                    "y": 4.408001448921345,
+                    "heading": 2.7655229314430416,
+                    "angularVelocity": 0.204083682327543,
+                    "velocityX": -0.026277925124547522,
+                    "velocityY": 0.6310864986340488,
+                    "timestamp": 0.07606160015345173
+                },
+                {
+                    "x": 2.294003795304181,
+                    "y": 4.504004095385984,
+                    "heading": 2.796583558711994,
+                    "angularVelocity": 0.408361475518381,
+                    "velocityX": -0.0525556082710341,
+                    "velocityY": 1.262169692340901,
+                    "timestamp": 0.15212320030690346
+                },
+                {
+                    "x": 2.2880075283432446,
+                    "y": 4.6480074792263695,
+                    "heading": 2.8432070800984706,
+                    "angularVelocity": 0.6129705566595356,
+                    "velocityX": -0.07883435200994282,
+                    "velocityY": 1.8932468361152464,
+                    "timestamp": 0.2281848004603552
+                },
+                {
+                    "x": 2.2800121406869254,
+                    "y": 4.840010950636843,
+                    "heading": 2.905427634623498,
+                    "angularVelocity": 0.8180284716532374,
+                    "velocityX": -0.10511726863738108,
+                    "velocityY": 2.5243154367396032,
+                    "timestamp": 0.3042464006138069
+                },
+                {
+                    "x": 2.2700168989019116,
+                    "y": 5.080013713319674,
+                    "heading": 2.983284780095673,
+                    "angularVelocity": 1.0236064625921717,
+                    "velocityX": -0.13140982788751254,
+                    "velocityY": 3.1553735682477444,
+                    "timestamp": 0.3803080007672587
+                },
+                {
+                    "x": 2.262010665765912,
+                    "y": 5.27200837395213,
+                    "heading": 3.04596409402067,
+                    "angularVelocity": 0.8240598909113609,
+                    "velocityX": -0.10525985674568247,
+                    "velocityY": 2.5241995993394015,
+                    "timestamp": 0.45636960092071044
+                },
+                {
+                    "x": 2.2560054673554846,
+                    "y": 5.416004212856887,
+                    "heading": 3.092980839154268,
+                    "angularVelocity": 0.6181403630576198,
+                    "velocityX": -0.07895177590678841,
+                    "velocityY": 1.8931476410468593,
+                    "timestamp": 0.5324312010741622
+                },
+                {
+                    "x": 2.2520018258057144,
+                    "y": 5.512001403135216,
+                    "heading": 3.1243269625502275,
+                    "angularVelocity": 0.4121149611988094,
+                    "velocityX": -0.05263683043340117,
+                    "velocityY": 1.2620979585580252,
+                    "timestamp": 0.608492801227614
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0.20605716180244424,
+                    "velocityX": -0.026318481358210057,
+                    "velocityY": 0.6310490019661509,
+                    "timestamp": 0.6845544013810657
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": -5.838549689746647e-34,
+                    "velocityX": 5.793975279736999e-33,
+                    "velocityY": -3.8925630164914687e-35,
+                    "timestamp": 0.7606160015345175
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W3 To C1 To S1": {
+            "waypoints": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 23
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 0,
+                    "velocityX": -8.850231580467738e-33,
+                    "velocityY": -9.949674184637553e-35,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.3596219168979733,
+                    "y": 4.390117696915229,
+                    "heading": 2.7532876341018757,
+                    "angularVelocity": 0.0367297993475745,
+                    "velocityX": 0.6661024239682877,
+                    "velocityY": 0.3364781268925937,
+                    "timestamp": 0.08950863223523108
+                },
+                {
+                    "x": 2.478866409519412,
+                    "y": 4.45035344252589,
+                    "heading": 2.759663263066401,
+                    "angularVelocity": 0.07122920779048274,
+                    "velocityX": 1.3322122084053396,
+                    "velocityY": 0.6729601839112106,
+                    "timestamp": 0.17901726447046215
+                },
+                {
+                    "x": 2.6577344843676225,
+                    "y": 4.540707777094465,
+                    "heading": 2.7687936152184336,
+                    "angularVelocity": 0.10200526948103018,
+                    "velocityX": 1.998333237604842,
+                    "velocityY": 1.009448276799944,
+                    "timestamp": 0.2685258967056932
+                },
+                {
+                    "x": 2.896227808747147,
+                    "y": 4.661181607047397,
+                    "heading": 2.7800101234260337,
+                    "angularVelocity": 0.12531202776199954,
+                    "velocityX": 2.664472894108777,
+                    "velocityY": 1.3459464963817573,
+                    "timestamp": 0.3580345289409243
+                },
+                {
+                    "x": 3.194348569009026,
+                    "y": 4.811776231628672,
+                    "heading": 2.7912909680084645,
+                    "angularVelocity": 0.126030800613561,
+                    "velocityX": 3.3306369767600654,
+                    "velocityY": 1.6824592312561388,
+                    "timestamp": 0.4475431611761554
+                },
+                {
+                    "x": 3.4965824721893735,
+                    "y": 4.964447700014571,
+                    "heading": 2.791290970219591,
+                    "angularVelocity": 2.4702941902196968e-8,
+                    "velocityX": 3.3765894487815307,
+                    "velocityY": 1.7056619520749001,
+                    "timestamp": 0.5370517934113864
+                },
+                {
+                    "x": 3.798816375371316,
+                    "y": 5.117119168397318,
+                    "heading": 2.7912909724307147,
+                    "angularVelocity": 2.470290930957206e-8,
+                    "velocityX": 3.3765894487993435,
+                    "velocityY": 1.7056619520396954,
+                    "timestamp": 0.6265604256466175
+                },
+                {
+                    "x": 4.101050278553258,
+                    "y": 5.2697906367800655,
+                    "heading": 2.7912909746418384,
+                    "angularVelocity": 2.4702908438395345e-8,
+                    "velocityX": 3.376589448799344,
+                    "velocityY": 1.705661952039695,
+                    "timestamp": 0.7160690578818486
+                },
+                {
+                    "x": 4.4032841817351995,
+                    "y": 5.422462105162813,
+                    "heading": 2.791290976852962,
+                    "angularVelocity": 2.470290834028317e-8,
+                    "velocityX": 3.376589448799344,
+                    "velocityY": 1.7056619520396945,
+                    "timestamp": 0.8055776901170797
+                },
+                {
+                    "x": 4.705518084917142,
+                    "y": 5.5751335735455605,
+                    "heading": 2.791290979064086,
+                    "angularVelocity": 2.470290893539666e-8,
+                    "velocityX": 3.376589448799343,
+                    "velocityY": 1.7056619520396956,
+                    "timestamp": 0.8950863223523108
+                },
+                {
+                    "x": 5.007751988099084,
+                    "y": 5.727805041928308,
+                    "heading": 2.791290981275209,
+                    "angularVelocity": 2.470290833665101e-8,
+                    "velocityX": 3.376589448799344,
+                    "velocityY": 1.705661952039695,
+                    "timestamp": 0.9845949545875419
+                },
+                {
+                    "x": 5.309985891281026,
+                    "y": 5.8804765103110554,
+                    "heading": 2.791290983486333,
+                    "angularVelocity": 2.4702909020592533e-8,
+                    "velocityX": 3.376589448799343,
+                    "velocityY": 1.705661952039695,
+                    "timestamp": 1.0741035868227728
+                },
+                {
+                    "x": 5.612219794462969,
+                    "y": 6.033147978693803,
+                    "heading": 2.7912909856974566,
+                    "angularVelocity": 2.470290845419301e-8,
+                    "velocityX": 3.3765894487993435,
+                    "velocityY": 1.7056619520396947,
+                    "timestamp": 1.1636122190580038
+                },
+                {
+                    "x": 5.914453697644911,
+                    "y": 6.18581944707655,
+                    "heading": 2.7912909879085803,
+                    "angularVelocity": 2.4702909294524738e-8,
+                    "velocityX": 3.3765894487993435,
+                    "velocityY": 1.705661952039695,
+                    "timestamp": 1.2531208512932348
+                },
+                {
+                    "x": 6.216687600826853,
+                    "y": 6.338490915459298,
+                    "heading": 2.791290990119704,
+                    "angularVelocity": 2.4702908192380297e-8,
+                    "velocityX": 3.3765894487993426,
+                    "velocityY": 1.705661952039695,
+                    "timestamp": 1.3426294835284658
+                },
+                {
+                    "x": 6.518921504008634,
+                    "y": 6.491162383842364,
+                    "heading": 2.791290992330828,
+                    "angularVelocity": 2.470290883873948e-8,
+                    "velocityX": 3.3765894487975445,
+                    "velocityY": 1.705661952043257,
+                    "timestamp": 1.4321381157636968
+                },
+                {
+                    "x": 6.821155370898669,
+                    "y": 6.643833924069847,
+                    "heading": 2.7912909945419515,
+                    "angularVelocity": 2.4702908942452264e-8,
+                    "velocityX": 3.3765890433422854,
+                    "velocityY": 1.705662754696757,
+                    "timestamp": 1.5216467479989277
+                },
+                {
+                    "x": 7.115501994166732,
+                    "y": 6.811207678318402,
+                    "heading": 2.7912909995251494,
+                    "angularVelocity": 5.5672818987002594e-8,
+                    "velocityX": 3.2884719151390045,
+                    "velocityY": 1.8699174601248647,
+                    "timestamp": 1.6111553802341587
+                },
+                {
+                    "x": 7.349346251698842,
+                    "y": 6.9991016785605416,
+                    "heading": 2.863630358302155,
+                    "angularVelocity": 0.8081830430264627,
+                    "velocityX": 2.6125330227095924,
+                    "velocityY": 2.099171840190225,
+                    "timestamp": 1.7006640124693897
+                },
+                {
+                    "x": 7.522269801890552,
+                    "y": 7.159584774540021,
+                    "heading": 2.9361878922250697,
+                    "angularVelocity": 0.8106205190604594,
+                    "velocityX": 1.9319203731910752,
+                    "velocityY": 1.792934289932232,
+                    "timestamp": 1.7901726447046207
+                },
+                {
+                    "x": 7.634890963268431,
+                    "y": 7.291326986197087,
+                    "heading": 3.0068580812297814,
+                    "angularVelocity": 0.789534900041686,
+                    "velocityX": 1.258215644295718,
+                    "velocityY": 1.4718380603877852,
+                    "timestamp": 1.8796812769398517
+                },
+                {
+                    "x": 7.687430049075347,
+                    "y": 7.393874919842459,
+                    "heading": 3.074917441639189,
+                    "angularVelocity": 0.7603664441050307,
+                    "velocityX": 0.5869722784819492,
+                    "velocityY": 1.1456764681184401,
+                    "timestamp": 1.9691899091750826
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "angularVelocity": 0.7271092936575372,
+                    "velocityX": -0.08300930189416394,
+                    "velocityY": 0.8169612062149058,
+                    "timestamp": 2.0586985414103136
+                },
+                {
+                    "x": 7.610902849690699,
+                    "y": 7.510752049010022,
+                    "heading": 3.20269374092293,
+                    "angularVelocity": 0.6908898775103415,
+                    "velocityX": -0.7614559445765343,
+                    "velocityY": 0.4821509665298148,
+                    "timestamp": 2.149442005692712
+                },
+                {
+                    "x": 7.480264531664604,
+                    "y": 7.524075152445223,
+                    "heading": 3.2620627183539166,
+                    "angularVelocity": 0.6542507264900841,
+                    "velocityX": -1.4396443761452817,
+                    "velocityY": 0.1468216310734938,
+                    "timestamp": 2.2401854699751107
+                },
+                {
+                    "x": 7.2881241468833045,
+                    "y": 7.506890911811968,
+                    "heading": 3.31804363381791,
+                    "angularVelocity": 0.6169140213754432,
+                    "velocityX": -2.117401912090868,
+                    "velocityY": -0.18937166185078083,
+                    "timestamp": 2.3309289342575092
+                },
+                {
+                    "x": 7.034560081128849,
+                    "y": 7.459042940332114,
+                    "heading": 3.3705102766509376,
+                    "angularVelocity": 0.5781864649750256,
+                    "velocityX": -2.7942956306511584,
+                    "velocityY": -0.5272883491746447,
+                    "timestamp": 2.421672398539908
+                },
+                {
+                    "x": 6.71980893194678,
+                    "y": 7.380065366153176,
+                    "heading": 3.4190860492179427,
+                    "angularVelocity": 0.5353087734873627,
+                    "velocityX": -3.468582025946757,
+                    "velocityY": -0.8703389803716955,
+                    "timestamp": 2.5124158628223063
+                },
+                {
+                    "x": 6.409212684326435,
+                    "y": 7.233883432324963,
+                    "heading": 3.4190860479548357,
+                    "angularVelocity": -1.3919538885721925e-8,
+                    "velocityX": -3.4227946891442476,
+                    "velocityY": -1.6109362253714221,
+                    "timestamp": 2.603159327104705
+                },
+                {
+                    "x": 6.100914553727412,
+                    "y": 7.0829150593165995,
+                    "heading": 3.4190860462953716,
+                    "angularVelocity": -1.8287422949592592e-8,
+                    "velocityX": -3.397469261693411,
+                    "velocityY": -1.663683155610435,
+                    "timestamp": 2.6939027913871034
+                },
+                {
+                    "x": 5.79261648458282,
+                    "y": 6.9319467005164075,
+                    "heading": 3.4190860242893293,
+                    "angularVelocity": -2.4250829002858187e-7,
+                    "velocityX": -3.3974685844608317,
+                    "velocityY": -1.6636829990352793,
+                    "timestamp": 2.784646255669502
+                },
+                {
+                    "x": 5.545968445126592,
+                    "y": 6.811167311993174,
+                    "heading": 3.4140434484907587,
+                    "angularVelocity": -0.05556957559916094,
+                    "velocityX": -2.7180804855393776,
+                    "velocityY": -1.330998209935673,
+                    "timestamp": 2.8753897199519005
+                },
+                {
+                    "x": 5.3609835283362095,
+                    "y": 6.720583315840053,
+                    "heading": 3.409029867698435,
+                    "angularVelocity": -0.05525004838608659,
+                    "velocityX": -2.0385481009927036,
+                    "velocityY": -0.9982426488724173,
+                    "timestamp": 2.966133184234299
+                },
+                {
+                    "x": 5.237660981460967,
+                    "y": 6.6601943432298505,
+                    "heading": 3.4052336568932624,
+                    "angularVelocity": -0.041834536902387906,
+                    "velocityX": -1.3590240118170522,
+                    "velocityY": -0.6654911523133865,
+                    "timestamp": 3.0568766485166976
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": -0.022572972388422655,
+                    "velocityX": -0.6795087883031954,
+                    "velocityY": -0.3327439994563607,
+                    "timestamp": 3.147620112799096
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": 0,
+                    "velocityX": -1.5756247890906886e-35,
+                    "velocityY": 0,
+                    "timestamp": 3.2383635770814947
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W3 To C1 To S2": {
+            "waypoints": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 23
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.3566378052654167,
+                    "y": 4.392308940927803,
+                    "heading": 2.7577639052389085,
+                    "angularVelocity": 0.08777137611433758,
+                    "velocityX": 0.6402935063310357,
+                    "velocityY": 0.3652543557357081,
+                    "timestamp": 0.08845600448138136
+                },
+                {
+                    "x": 2.469918670668774,
+                    "y": 4.456929940384967,
+                    "heading": 2.772608563363178,
+                    "angularVelocity": 0.16781967726556357,
+                    "velocityX": 1.2806464192852178,
+                    "velocityY": 0.7305439561286646,
+                    "timestamp": 0.1769120089627627
+                },
+                {
+                    "x": 2.639850302646433,
+                    "y": 4.553867610725804,
+                    "heading": 2.793388618859665,
+                    "angularVelocity": 0.2349196712910238,
+                    "velocityX": 1.92108645392669,
+                    "velocityY": 1.0958856994410031,
+                    "timestamp": 0.26536801344414407
+                },
+                {
+                    "x": 2.8664440757683463,
+                    "y": 4.683128905196596,
+                    "heading": 2.8177875607011336,
+                    "angularVelocity": 0.27583138063398066,
+                    "velocityX": 2.561655078707626,
+                    "velocityY": 1.4613060495853287,
+                    "timestamp": 0.3538240179255254
+                },
+                {
+                    "x": 3.1496996536335864,
+                    "y": 4.844715100104656,
+                    "heading": 2.8386513493911516,
+                    "angularVelocity": 0.23586627965327242,
+                    "velocityX": 3.2022199004575587,
+                    "velocityY": 1.8267408284540865,
+                    "timestamp": 0.4422800224069068
+                },
+                {
+                    "x": 3.44035847389222,
+                    "y": 5.010517813781029,
+                    "heading": 2.83865135522876,
+                    "angularVelocity": 6.599448670952606e-8,
+                    "velocityX": 3.28591396324954,
+                    "velocityY": 1.8744088052413612,
+                    "timestamp": 0.5307360268882881
+                },
+                {
+                    "x": 3.731017294885288,
+                    "y": 5.17632052617049,
+                    "heading": 2.8386513610659576,
+                    "angularVelocity": 6.598983540705142e-8,
+                    "velocityX": 3.285913971552369,
+                    "velocityY": 1.8744087906927347,
+                    "timestamp": 0.6191920313696695
+                },
+                {
+                    "x": 4.021676115881456,
+                    "y": 5.3421232385545165,
+                    "heading": 2.8386513669031546,
+                    "angularVelocity": 6.598983544244374e-8,
+                    "velocityX": 3.285913971587413,
+                    "velocityY": 1.8744087906313014,
+                    "timestamp": 0.7076480358510508
+                },
+                {
+                    "x": 4.31233493687768,
+                    "y": 5.507925950938444,
+                    "heading": 2.838651372740352,
+                    "angularVelocity": 6.598983514854984e-8,
+                    "velocityX": 3.2859139715880556,
+                    "velocityY": 1.8744087906301747,
+                    "timestamp": 0.7961040403324322
+                },
+                {
+                    "x": 4.602993757873904,
+                    "y": 5.673728663322372,
+                    "heading": 2.8386513785775485,
+                    "angularVelocity": 6.598983509017798e-8,
+                    "velocityX": 3.2859139715880414,
+                    "velocityY": 1.8744087906301996,
+                    "timestamp": 0.8845600448138136
+                },
+                {
+                    "x": 4.893652578870126,
+                    "y": 5.839531375706303,
+                    "heading": 2.8386513844147463,
+                    "angularVelocity": 6.598983636221009e-8,
+                    "velocityX": 3.2859139715880246,
+                    "velocityY": 1.8744087906302287,
+                    "timestamp": 0.9730160492951949
+                },
+                {
+                    "x": 5.184311399866346,
+                    "y": 6.005334088090239,
+                    "heading": 2.8386513902519432,
+                    "angularVelocity": 6.598983502831849e-8,
+                    "velocityX": 3.285913971588007,
+                    "velocityY": 1.8744087906302613,
+                    "timestamp": 1.0614720537765763
+                },
+                {
+                    "x": 5.474970220862562,
+                    "y": 6.171136800474182,
+                    "heading": 2.83865139608914,
+                    "angularVelocity": 6.598983586934159e-8,
+                    "velocityX": 3.2859139715879397,
+                    "velocityY": 1.8744087906303781,
+                    "timestamp": 1.1499280582579576
+                },
+                {
+                    "x": 5.765629041858637,
+                    "y": 6.336939512858371,
+                    "heading": 2.8386514019263376,
+                    "angularVelocity": 6.598983523621195e-8,
+                    "velocityX": 3.2859139715863668,
+                    "velocityY": 1.8744087906331355,
+                    "timestamp": 1.238384062739339
+                },
+                {
+                    "x": 6.0562878628501755,
+                    "y": 6.502742225250514,
+                    "heading": 2.8386514077635345,
+                    "angularVelocity": 6.598983511287735e-8,
+                    "velocityX": 3.285913971535071,
+                    "velocityY": 1.87440879072306,
+                    "timestamp": 1.3268400672207203
+                },
+                {
+                    "x": 6.3469466835037505,
+                    "y": 6.6685449382351205,
+                    "heading": 2.8386514136007315,
+                    "angularVelocity": 6.598983592798126e-8,
+                    "velocityX": 3.2859139677143814,
+                    "velocityY": 1.8744087974208865,
+                    "timestamp": 1.4152960717021017
+                },
+                {
+                    "x": 6.6376054603552355,
+                    "y": 6.834347728006541,
+                    "heading": 2.838651419437929,
+                    "angularVelocity": 6.598983535546771e-8,
+                    "velocityX": 3.2859134725293426,
+                    "velocityY": 1.8744096655000533,
+                    "timestamp": 1.503752076183483
+                },
+                {
+                    "x": 6.928282728448998,
+                    "y": 7.000118097747795,
+                    "heading": 2.8386514252461743,
+                    "angularVelocity": 6.56625327771565e-8,
+                    "velocityX": 3.286122517041433,
+                    "velocityY": 1.8740431552739683,
+                    "timestamp": 1.5922080806648644
+                },
+                {
+                    "x": 7.191968889959837,
+                    "y": 7.158086456014882,
+                    "heading": 2.9053215778244037,
+                    "angularVelocity": 0.7537097449644049,
+                    "velocityX": 2.980986571311004,
+                    "velocityY": 1.7858409860730329,
+                    "timestamp": 1.6806640851462458
+                },
+                {
+                    "x": 7.39896469669124,
+                    "y": 7.2837659035610995,
+                    "heading": 2.9729435938200757,
+                    "angularVelocity": 0.7644706132968896,
+                    "velocityX": 2.3400989898312643,
+                    "velocityY": 1.4208130729292534,
+                    "timestamp": 1.7691200896276271
+                },
+                {
+                    "x": 7.549292841201835,
+                    "y": 7.377144263475716,
+                    "heading": 3.03570050164413,
+                    "angularVelocity": 0.7094702976016349,
+                    "velocityX": 1.6994679489759839,
+                    "velocityY": 1.0556474991393678,
+                    "timestamp": 1.8575760941090085
+                },
+                {
+                    "x": 7.642968334049115,
+                    "y": 7.4382219539742405,
+                    "heading": 3.0916990951391803,
+                    "angularVelocity": 0.6330671820796432,
+                    "velocityX": 1.0590066032994798,
+                    "velocityY": 0.6904866532988578,
+                    "timestamp": 1.9460320985903898
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "angularVelocity": 0.546044388326283,
+                    "velocityX": 0.41864502209875154,
+                    "velocityY": 0.32533739449908106,
+                    "timestamp": 2.034488103071771
+                },
+                {
+                    "x": 7.645871150286324,
+                    "y": 7.456493783242831,
+                    "heading": 3.185181806591133,
+                    "angularVelocity": 0.43686385232963987,
+                    "velocityX": -0.329992576357443,
+                    "velocityY": -0.10158483407911743,
+                    "timestamp": 2.1379111863737847
+                },
+                {
+                    "x": 7.534332014927772,
+                    "y": 7.401805741681953,
+                    "heading": 3.219069089269022,
+                    "angularVelocity": 0.32765685953225704,
+                    "velocityX": -1.078474280570675,
+                    "velocityY": -0.5287798411617203,
+                    "timestamp": 2.2413342696757983
+                },
+                {
+                    "x": 7.345414230675068,
+                    "y": 7.30288053546568,
+                    "heading": 3.2416598317701943,
+                    "angularVelocity": 0.21843037144032681,
+                    "velocityX": -1.826650088365854,
+                    "velocityY": -0.9565099304515025,
+                    "timestamp": 2.344757352977812
+                },
+                {
+                    "x": 7.079208201344681,
+                    "y": 7.159560464323527,
+                    "heading": 3.2529526054228244,
+                    "angularVelocity": 0.1091900694900192,
+                    "velocityX": -2.573951779729956,
+                    "velocityY": -1.3857648270226828,
+                    "timestamp": 2.4481804362798254
+                },
+                {
+                    "x": 6.738167350146867,
+                    "y": 6.967824923059757,
+                    "heading": 3.25295260062049,
+                    "angularVelocity": -4.6433873545046094e-8,
+                    "velocityX": -3.29753126970623,
+                    "velocityY": -1.853895041050502,
+                    "timestamp": 2.551603519581839
+                },
+                {
+                    "x": 6.398085530846708,
+                    "y": 6.774393439892416,
+                    "heading": 3.252952596202531,
+                    "angularVelocity": -4.2717337144488564e-8,
+                    "velocityX": -3.2882583698173717,
+                    "velocityY": -1.8702931395158464,
+                    "timestamp": 2.6550266028838525
+                },
+                {
+                    "x": 6.058003908621006,
+                    "y": 6.580961610238813,
+                    "heading": 3.252952591784573,
+                    "angularVelocity": -4.2717336082734796e-8,
+                    "velocityX": -3.288256464300223,
+                    "velocityY": -1.8702964896989283,
+                    "timestamp": 2.758449686185866
+                },
+                {
+                    "x": 5.717922287179122,
+                    "y": 6.387529779207143,
+                    "heading": 3.252952587366614,
+                    "angularVelocity": -4.271733673837129e-8,
+                    "velocityX": -3.28825645672147,
+                    "velocityY": -1.8702965030234862,
+                    "timestamp": 2.8618727694878796
+                },
+                {
+                    "x": 5.377840665722721,
+                    "y": 6.194097948200997,
+                    "heading": 3.2529525829486556,
+                    "angularVelocity": -4.271733567219934e-8,
+                    "velocityX": -3.2882564568618378,
+                    "velocityY": -1.8702965027766985,
+                    "timestamp": 2.965295852789893
+                },
+                {
+                    "x": 5.037759049752838,
+                    "y": 6.000666107536036,
+                    "heading": 3.252952578536604,
+                    "angularVelocity": -4.266021621617093e-8,
+                    "velocityX": -3.288256403812571,
+                    "velocityY": -1.8702965961679845,
+                    "timestamp": 3.0687189360919067
+                },
+                {
+                    "x": 4.7278555042556665,
+                    "y": 5.824399716323421,
+                    "heading": 3.2077639144902763,
+                    "angularVelocity": -0.4369301572101497,
+                    "velocityX": -2.996463996264777,
+                    "velocityY": -1.7043234990188791,
+                    "timestamp": 3.1721420193939203
+                },
+                {
+                    "x": 4.495427770658729,
+                    "y": 5.69219987124691,
+                    "heading": 3.1738800653247865,
+                    "angularVelocity": -0.32762366082757177,
+                    "velocityX": -2.2473487172898445,
+                    "velocityY": -1.278243123833972,
+                    "timestamp": 3.275565102695934
+                },
+                {
+                    "x": 4.340475927186926,
+                    "y": 5.604066625906228,
+                    "heading": 3.1512930339800644,
+                    "angularVelocity": -0.2183944881894502,
+                    "velocityX": -1.4982326819566802,
+                    "velocityY": -0.8521622303921732,
+                    "timestamp": 3.3789881859979474
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": -0.10919258660163363,
+                    "velocityX": -0.7491163936844148,
+                    "velocityY": -0.4260811464839441,
+                    "timestamp": 3.482411269299961
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 8.84233842596168e-24,
+                    "velocityY": 0,
+                    "timestamp": 3.5858343526019745
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W3 To C2 To S1": {
+            "waypoints": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 22
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.3590330484085675,
+                    "y": 4.366649962138697,
+                    "heading": 2.7576232354660704,
+                    "angularVelocity": 0.09028523567993728,
+                    "velocityX": 0.6991536221325528,
+                    "velocityY": 0.07875834370159086,
+                    "timestamp": 0.08443501762674911
+                },
+                {
+                    "x": 2.4770991403474683,
+                    "y": 4.379949883012471,
+                    "heading": 2.7728702101731937,
+                    "angularVelocity": 0.18057643778228707,
+                    "velocityX": 1.3983071864900902,
+                    "velocityY": 0.15751664709264293,
+                    "timestamp": 0.16887003525349822
+                },
+                {
+                    "x": 2.6541982667626414,
+                    "y": 4.399899758130858,
+                    "heading": 2.7957418198907438,
+                    "angularVelocity": 0.27087824886418327,
+                    "velocityX": 2.097460643616519,
+                    "velocityY": 0.23627489730122597,
+                    "timestamp": 0.25330505288024735
+                },
+                {
+                    "x": 2.8903304124163527,
+                    "y": 4.426499582699937,
+                    "heading": 2.8262393577196585,
+                    "angularVelocity": 0.361195374693129,
+                    "velocityX": 2.796613920275944,
+                    "velocityY": 0.31503309073333907,
+                    "timestamp": 0.33774007050699645
+                },
+                {
+                    "x": 3.1854955469851514,
+                    "y": 4.459749351941674,
+                    "heading": 2.864364443644879,
+                    "angularVelocity": 0.4515316866960985,
+                    "velocityX": 3.4957668378017823,
+                    "velocityY": 0.3937912275771662,
+                    "timestamp": 0.42217508813374555
+                },
+                {
+                    "x": 3.5029006029698784,
+                    "y": 4.495505051079763,
+                    "heading": 2.8643644482492165,
+                    "angularVelocity": 5.453113642276571e-8,
+                    "velocityX": 3.7591637321358573,
+                    "velocityY": 0.4234700263361048,
+                    "timestamp": 0.5066101057604947
+                },
+                {
+                    "x": 3.820305658948058,
+                    "y": 4.531260750276139,
+                    "heading": 2.864364452853533,
+                    "angularVelocity": 5.453089037976801e-8,
+                    "velocityX": 3.759163732058311,
+                    "velocityY": 0.4234700270264227,
+                    "timestamp": 0.5910451233872438
+                },
+                {
+                    "x": 4.1377107149262375,
+                    "y": 4.567016449472515,
+                    "heading": 2.8643644574578495,
+                    "angularVelocity": 5.4530890155242205e-8,
+                    "velocityX": 3.75916373205831,
+                    "velocityY": 0.4234700270264262,
+                    "timestamp": 0.6754801410139929
+                },
+                {
+                    "x": 4.455115770904417,
+                    "y": 4.602772148668891,
+                    "heading": 2.864364462062166,
+                    "angularVelocity": 5.453089038095968e-8,
+                    "velocityX": 3.7591637320583104,
+                    "velocityY": 0.4234700270264258,
+                    "timestamp": 0.759915158640742
+                },
+                {
+                    "x": 4.772520826882595,
+                    "y": 4.638527847865267,
+                    "heading": 2.864364466666483,
+                    "angularVelocity": 5.4530891020586436e-8,
+                    "velocityX": 3.7591637320583104,
+                    "velocityY": 0.4234700270264263,
+                    "timestamp": 0.8443501762674911
+                },
+                {
+                    "x": 5.089925882860775,
+                    "y": 4.674283547061643,
+                    "heading": 2.8643644712708,
+                    "angularVelocity": 5.4530890812107064e-8,
+                    "velocityX": 3.75916373205831,
+                    "velocityY": 0.4234700270264263,
+                    "timestamp": 0.9287851938942402
+                },
+                {
+                    "x": 5.407330938838954,
+                    "y": 4.7100392462580185,
+                    "heading": 2.8643644758751163,
+                    "angularVelocity": 5.453088997566471e-8,
+                    "velocityX": 3.7591637320583104,
+                    "velocityY": 0.4234700270264265,
+                    "timestamp": 1.0132202115209894
+                },
+                {
+                    "x": 5.7247359948171335,
+                    "y": 4.745794945454395,
+                    "heading": 2.864364480479433,
+                    "angularVelocity": 5.453089022113266e-8,
+                    "velocityX": 3.7591637320583104,
+                    "velocityY": 0.42347002702642605,
+                    "timestamp": 1.0976552291477386
+                },
+                {
+                    "x": 6.04214105079524,
+                    "y": 4.7815506446514116,
+                    "heading": 2.8643644850837493,
+                    "angularVelocity": 5.453089025367266e-8,
+                    "velocityX": 3.759163732057455,
+                    "velocityY": 0.42347002703401615,
+                    "timestamp": 1.1820902467744878
+                },
+                {
+                    "x": 6.359546093014576,
+                    "y": 4.817306465985485,
+                    "heading": 2.8643644896880662,
+                    "angularVelocity": 5.4530891813953255e-8,
+                    "velocityX": 3.7591635691064473,
+                    "velocityY": 0.42347147355537545,
+                    "timestamp": 1.266525264401237
+                },
+                {
+                    "x": 6.6740817237038605,
+                    "y": 4.872910210638354,
+                    "heading": 2.8643644943647257,
+                    "angularVelocity": 5.53876754719752e-8,
+                    "velocityX": 3.7251799020130547,
+                    "velocityY": 0.6585389121213792,
+                    "timestamp": 1.3509602820279862
+                },
+                {
+                    "x": 6.972867149310035,
+                    "y": 4.985833654432599,
+                    "heading": 2.8643645123115293,
+                    "angularVelocity": 2.1255166363796299e-7,
+                    "velocityX": 3.5386434918149345,
+                    "velocityY": 1.337400606623087,
+                    "timestamp": 1.4353952996547354
+                },
+                {
+                    "x": 7.232618211035156,
+                    "y": 5.1421799444134875,
+                    "heading": 2.9041055246483745,
+                    "angularVelocity": 0.4706697938114134,
+                    "velocityX": 3.0763428376762785,
+                    "velocityY": 1.8516759322776348,
+                    "timestamp": 1.5198303172814847
+                },
+                {
+                    "x": 7.433588885971066,
+                    "y": 5.306272472318755,
+                    "heading": 2.9592505290938482,
+                    "angularVelocity": 0.6531058557865896,
+                    "velocityX": 2.3801815950855185,
+                    "velocityY": 1.9434179386406838,
+                    "timestamp": 1.6042653349082339
+                },
+                {
+                    "x": 7.575142359826725,
+                    "y": 5.471558749180071,
+                    "heading": 3.0192595634287165,
+                    "angularVelocity": 0.7107126405793248,
+                    "velocityX": 1.676478288681205,
+                    "velocityY": 1.9575560177173887,
+                    "timestamp": 1.688700352534983
+                },
+                {
+                    "x": 7.657265359417412,
+                    "y": 5.635564900043681,
+                    "heading": 3.0801944156150345,
+                    "angularVelocity": 0.7216774970745534,
+                    "velocityX": 0.9726177822774612,
+                    "velocityY": 1.9423949384201076,
+                    "timestamp": 1.7731353701617323
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": 0.7083030958711996,
+                    "velocityX": 0.26925606486028053,
+                    "velocityY": 1.9119448836968793,
+                    "timestamp": 1.8575703877884815
+                },
+                {
+                    "x": 7.638446013209911,
+                    "y": 5.962794429881767,
+                    "heading": 3.200158410841702,
+                    "angularVelocity": 0.6785760380077615,
+                    "velocityX": -0.46872148590566404,
+                    "velocityY": 1.870131304647713,
+                    "timestamp": 1.9462242857633076
+                },
+                {
+                    "x": 7.531523237847712,
+                    "y": 6.124072395344993,
+                    "heading": 3.2564305280159678,
+                    "angularVelocity": 0.6347393454740654,
+                    "velocityX": -1.206069646171223,
+                    "velocityY": 1.8191863995537148,
+                    "timestamp": 2.034878183738134
+                },
+                {
+                    "x": 7.359349891233686,
+                    "y": 6.279498639853745,
+                    "heading": 3.30674894531401,
+                    "angularVelocity": 0.5675826833054816,
+                    "velocityX": -1.9420843363584046,
+                    "velocityY": 1.7531800412530831,
+                    "timestamp": 2.1235320817129604
+                },
+                {
+                    "x": 7.122252404274228,
+                    "y": 6.426460665486359,
+                    "heading": 3.3470467672011193,
+                    "angularVelocity": 0.45455217207203097,
+                    "velocityX": -2.6744169447211816,
+                    "velocityY": 1.6577051769832414,
+                    "timestamp": 2.2121859796877867
+                },
+                {
+                    "x": 6.821828497038046,
+                    "y": 6.5576759176562405,
+                    "heading": 3.365711055371459,
+                    "angularVelocity": 0.2105298085780703,
+                    "velocityX": -3.388727558504971,
+                    "velocityY": 1.4800844087773837,
+                    "timestamp": 2.300839877662613
+                },
+                {
+                    "x": 6.494234201783015,
+                    "y": 6.629485644322343,
+                    "heading": 3.365711060189302,
+                    "angularVelocity": 5.434439690558349e-8,
+                    "velocityX": -3.6952046411772423,
+                    "velocityY": 0.8100007817647606,
+                    "timestamp": 2.3894937756374395
+                },
+                {
+                    "x": 6.158929367283791,
+                    "y": 6.636218351220065,
+                    "heading": 3.3657110616036774,
+                    "angularVelocity": 1.5953902696988204e-8,
+                    "velocityX": -3.782178134958452,
+                    "velocityY": 0.07594372104916906,
+                    "timestamp": 2.478147673612266
+                },
+                {
+                    "x": 5.8312862675507695,
+                    "y": 6.634145572711449,
+                    "heading": 3.3782020006814046,
+                    "angularVelocity": 0.14089554281384958,
+                    "velocityX": -3.6957551468978513,
+                    "velocityY": -0.023380568209240477,
+                    "timestamp": 2.566801571587092
+                },
+                {
+                    "x": 5.569171767766223,
+                    "y": 6.6324873452973785,
+                    "heading": 3.388195171082853,
+                    "angularVelocity": 0.11272116206650896,
+                    "velocityX": -2.9566043430935838,
+                    "velocityY": -0.018704506535536753,
+                    "timestamp": 2.6554554695619186
+                },
+                {
+                    "x": 5.372585886333595,
+                    "y": 6.631243673213895,
+                    "heading": 3.395690186633039,
+                    "angularVelocity": 0.08454242533492021,
+                    "velocityX": -2.2174533317017797,
+                    "velocityY": -0.014028397080035612,
+                    "timestamp": 2.744109367536745
+                },
+                {
+                    "x": 5.241528629389324,
+                    "y": 6.630414557878557,
+                    "heading": 3.4006869200557808,
+                    "angularVelocity": 0.05636225295091492,
+                    "velocityX": -1.4783022510920478,
+                    "velocityY": -0.00935227163472713,
+                    "timestamp": 2.8327632655115713
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": 0.02818135672404778,
+                    "velocityX": -0.7391511358917447,
+                    "velocityY": -0.00467613819613763,
+                    "timestamp": 2.9214171634863977
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": 0,
+                    "velocityX": -2.6920195969080552e-36,
+                    "velocityY": 0,
+                    "timestamp": 3.010071061461224
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W3 To C2 To S2": {
+            "waypoints": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 22
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 14
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.3595382644510017,
+                    "y": 4.372038786355543,
+                    "heading": 2.7583958335298844,
+                    "angularVelocity": 0.09833011550569279,
+                    "velocityX": 0.6972987732113981,
+                    "velocityY": 0.140995560318735,
+                    "timestamp": 0.08538415201391997
+                },
+                {
+                    "x": 2.478614789358208,
+                    "y": 4.396116350384182,
+                    "heading": 2.7751880425894457,
+                    "angularVelocity": 0.19666657879115437,
+                    "velocityX": 1.3945974996366264,
+                    "velocityY": 0.28199101895061424,
+                    "timestamp": 0.17076830402783993
+                },
+                {
+                    "x": 2.657229566273028,
+                    "y": 4.432232679689,
+                    "heading": 2.800377642298772,
+                    "angularVelocity": 0.2950149309349538,
+                    "velocityX": 2.0918961271138583,
+                    "velocityY": 0.4229863323925762,
+                    "timestamp": 0.2561524560417599
+                },
+                {
+                    "x": 2.8953825799844894,
+                    "y": 4.480387759130421,
+                    "heading": 2.8339661569504204,
+                    "angularVelocity": 0.39338113525063506,
+                    "velocityX": 2.7891945764435975,
+                    "velocityY": 0.5639814685232187,
+                    "timestamp": 0.34153660805567987
+                },
+                {
+                    "x": 3.1930738006682495,
+                    "y": 4.5405815710050295,
+                    "heading": 2.8759554693739764,
+                    "angularVelocity": 0.49176939084328164,
+                    "velocityX": 3.486492676477345,
+                    "velocityY": 0.7049763973154618,
+                    "timestamp": 0.42692076006959984
+                },
+                {
+                    "x": 3.5096695268696414,
+                    "y": 4.604598695680213,
+                    "heading": 2.8759554743130047,
+                    "angularVelocity": 5.784479190315687e-8,
+                    "velocityX": 3.707897996688871,
+                    "velocityY": 0.7497541776224135,
+                    "timestamp": 0.5123049120835198
+                },
+                {
+                    "x": 3.8262652530617234,
+                    "y": 4.6686158204015085,
+                    "heading": 2.875955479252014,
+                    "angularVelocity": 5.784456729975194e-8,
+                    "velocityX": 3.7078979965798418,
+                    "velocityY": 0.7497541781624566,
+                    "timestamp": 0.5976890640974397
+                },
+                {
+                    "x": 4.142860979253806,
+                    "y": 4.732632945122804,
+                    "heading": 2.875955484191023,
+                    "angularVelocity": 5.784456771643219e-8,
+                    "velocityX": 3.707897996579841,
+                    "velocityY": 0.7497541781624594,
+                    "timestamp": 0.6830732161113597
+                },
+                {
+                    "x": 4.459456705445889,
+                    "y": 4.796650069844099,
+                    "heading": 2.8759554891300323,
+                    "angularVelocity": 5.7844567227510466e-8,
+                    "velocityX": 3.707897996579841,
+                    "velocityY": 0.7497541781624593,
+                    "timestamp": 0.7684573681252798
+                },
+                {
+                    "x": 4.7760524316379716,
+                    "y": 4.860667194565394,
+                    "heading": 2.8759554940690415,
+                    "angularVelocity": 5.784456749923784e-8,
+                    "velocityX": 3.707897996579841,
+                    "velocityY": 0.7497541781624593,
+                    "timestamp": 0.8538415201391998
+                },
+                {
+                    "x": 5.092648157830054,
+                    "y": 4.92468431928669,
+                    "heading": 2.8759554990080507,
+                    "angularVelocity": 5.784456784777939e-8,
+                    "velocityX": 3.707897996579841,
+                    "velocityY": 0.749754178162459,
+                    "timestamp": 0.9392256721531198
+                },
+                {
+                    "x": 5.409243884022137,
+                    "y": 4.988701444007985,
+                    "heading": 2.87595550394706,
+                    "angularVelocity": 5.784456724140334e-8,
+                    "velocityX": 3.707897996579841,
+                    "velocityY": 0.749754178162459,
+                    "timestamp": 1.0246098241670398
+                },
+                {
+                    "x": 5.72583961021422,
+                    "y": 5.05271856872928,
+                    "heading": 2.875955508886069,
+                    "angularVelocity": 5.7844566753037437e-8,
+                    "velocityX": 3.7078979965798413,
+                    "velocityY": 0.7497541781624589,
+                    "timestamp": 1.1099939761809599
+                },
+                {
+                    "x": 6.042435336406303,
+                    "y": 5.116735693450575,
+                    "heading": 2.8759555138250783,
+                    "angularVelocity": 5.784456768545098e-8,
+                    "velocityX": 3.7078979965798413,
+                    "velocityY": 0.749754178162459,
+                    "timestamp": 1.1953781281948799
+                },
+                {
+                    "x": 6.35903106259824,
+                    "y": 5.180752818172588,
+                    "heading": 2.8759555187640875,
+                    "angularVelocity": 5.784456775109183e-8,
+                    "velocityX": 3.707897996578142,
+                    "velocityY": 0.7497541781708625,
+                    "timestamp": 1.2807622802088
+                },
+                {
+                    "x": 6.67562676006639,
+                    "y": 5.244770084947493,
+                    "heading": 2.875955523703097,
+                    "angularVelocity": 5.7844569052705035e-8,
+                    "velocityX": 3.7078976601715947,
+                    "velocityY": 0.7497558418624185,
+                    "timestamp": 1.36614643222272
+                },
+                {
+                    "x": 6.986762276730635,
+                    "y": 5.331521073675251,
+                    "heading": 2.875955533952509,
+                    "angularVelocity": 1.2003881364523766e-7,
+                    "velocityX": 3.643949249663132,
+                    "velocityY": 1.0160080844231525,
+                    "timestamp": 1.45153058423664
+                },
+                {
+                    "x": 7.245394684994352,
+                    "y": 5.443936340264843,
+                    "heading": 2.937208027811149,
+                    "angularVelocity": 0.717375442794747,
+                    "velocityX": 3.0290446431036937,
+                    "velocityY": 1.3165823392058056,
+                    "timestamp": 1.53691473625056
+                },
+                {
+                    "x": 7.443822925582165,
+                    "y": 5.547959848416938,
+                    "heading": 2.996476692643064,
+                    "angularVelocity": 0.694141283059797,
+                    "velocityX": 2.3239469609707415,
+                    "velocityY": 1.2182999502663732,
+                    "timestamp": 1.62229888826448
+                },
+                {
+                    "x": 7.58233184253966,
+                    "y": 5.641838110863567,
+                    "heading": 3.05066970535842,
+                    "angularVelocity": 0.6346963861223477,
+                    "velocityX": 1.6221853082866564,
+                    "velocityY": 1.0994811125058057,
+                    "timestamp": 1.7076830402784
+                },
+                {
+                    "x": 7.661036876366553,
+                    "y": 5.724955999091069,
+                    "heading": 3.0986966592522744,
+                    "angularVelocity": 0.5624808909037848,
+                    "velocityX": 0.9217756687922859,
+                    "velocityY": 0.9734580278311056,
+                    "timestamp": 1.79306719229232
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": 0.4837354447344324,
+                    "velocityX": 0.22209184241070226,
+                    "velocityY": 0.8437631481915462,
+                    "timestamp": 1.87845134430624
+                },
+                {
+                    "x": 7.620399591023171,
+                    "y": 5.865894759292055,
+                    "heading": 3.178669357704941,
+                    "angularVelocity": 0.3869200928483079,
+                    "velocityX": -0.5963532146324005,
+                    "velocityY": 0.6893511618538622,
+                    "timestamp": 1.978392801004126
+                },
+                {
+                    "x": 7.479095963903043,
+                    "y": 5.918870767960299,
+                    "heading": 3.2076554876039776,
+                    "angularVelocity": 0.29003109276922934,
+                    "velocityX": -1.413863993870693,
+                    "velocityY": 0.5300704074024576,
+                    "timestamp": 2.078334257702012
+                },
+                {
+                    "x": 7.256273297326838,
+                    "y": 5.955010806465711,
+                    "heading": 3.2269465299619338,
+                    "angularVelocity": 0.19302342586691915,
+                    "velocityX": -2.229531907362301,
+                    "velocityY": 0.36161208470935113,
+                    "timestamp": 2.178275714399898
+                },
+                {
+                    "x": 6.95245768893108,
+                    "y": 5.971943859159761,
+                    "heading": 3.236518587853394,
+                    "angularVelocity": 0.09577664972800967,
+                    "velocityX": -3.0399357627352264,
+                    "velocityY": 0.16942971669141613,
+                    "timestamp": 2.278217171097784
+                },
+                {
+                    "x": 6.575020543317634,
+                    "y": 5.950033200191844,
+                    "heading": 3.236518582605277,
+                    "angularVelocity": -5.2511914101363804e-8,
+                    "velocityX": -3.7765823921738897,
+                    "velocityY": -0.21923493705070526,
+                    "timestamp": 2.37815862779567
+                },
+                {
+                    "x": 6.202215559947926,
+                    "y": 5.88714179095661,
+                    "heading": 3.2365185785582873,
+                    "angularVelocity": -4.049360572502445e-8,
+                    "velocityX": -3.730233635643953,
+                    "velocityY": -0.6292824951045971,
+                    "timestamp": 2.478100084493556
+                },
+                {
+                    "x": 5.829410606689244,
+                    "y": 5.824250203230788,
+                    "heading": 3.2365185745112974,
+                    "angularVelocity": -4.049360338896351e-8,
+                    "velocityX": -3.7302333343573055,
+                    "velocityY": -0.6292842810560431,
+                    "timestamp": 2.578041541191442
+                },
+                {
+                    "x": 5.456605653430655,
+                    "y": 5.761358615504415,
+                    "heading": 3.2365185704643076,
+                    "angularVelocity": -4.049360329558589e-8,
+                    "velocityX": -3.730233334356378,
+                    "velocityY": -0.6292842810615451,
+                    "timestamp": 2.677982997889328
+                },
+                {
+                    "x": 5.0838007001693555,
+                    "y": 5.698467027794241,
+                    "heading": 3.2365185664173017,
+                    "angularVelocity": -4.0493766096106506e-8,
+                    "velocityX": -3.7302333343834975,
+                    "velocityY": -0.6292842808994649,
+                    "timestamp": 2.777924454587214
+                },
+                {
+                    "x": 4.755480441979736,
+                    "y": 5.643080213031107,
+                    "heading": 3.197910664693085,
+                    "angularVelocity": -0.38630517304670825,
+                    "velocityX": -3.28512580301988,
+                    "velocityY": -0.5541925902737567,
+                    "timestamp": 2.8778659112851
+                },
+                {
+                    "x": 4.50924022643339,
+                    "y": 5.601540105644793,
+                    "heading": 3.1689552309170463,
+                    "angularVelocity": -0.2897239517287447,
+                    "velocityX": -2.4638445714345307,
+                    "velocityY": -0.41564440582335266,
+                    "timestamp": 2.977807367982986
+                },
+                {
+                    "x": 4.345080076326452,
+                    "y": 5.573846702024279,
+                    "heading": 3.149651762381054,
+                    "angularVelocity": -0.19314776043684534,
+                    "velocityX": -1.6425631117543122,
+                    "velocityY": -0.2770962575043274,
+                    "timestamp": 3.077748824680872
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": -0.09657416151367515,
+                    "velocityX": -0.8212815686144453,
+                    "velocityY": -0.1385481308936372,
+                    "timestamp": 3.177690281378758
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": -1.245204860871434e-36,
+                    "velocityY": 0,
+                    "timestamp": 3.277631738076644
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W3 To C3 To S1": {
+            "waypoints": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 21
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.358164523227949,
+                    "y": 4.34513375016413,
+                    "heading": 2.762567979975215,
+                    "angularVelocity": 0.1479885663322048,
+                    "velocityX": 0.6848900476349593,
+                    "velocityY": -0.17505080405008622,
+                    "timestamp": 0.08492534448237501
+                },
+                {
+                    "x": 2.474493569181997,
+                    "y": 4.315401331546784,
+                    "heading": 2.787705678078292,
+                    "angularVelocity": 0.2959975994950906,
+                    "velocityX": 1.3697800893606054,
+                    "velocityY": -0.35010065368078286,
+                    "timestamp": 0.16985068896475003
+                },
+                {
+                    "x": 2.6489871423142275,
+                    "y": 4.270802870950086,
+                    "heading": 2.825415221133538,
+                    "angularVelocity": 0.4440316761160991,
+                    "velocityX": 2.054670183509755,
+                    "velocityY": -0.525149010210423,
+                    "timestamp": 0.25477603344712507
+                },
+                {
+                    "x": 2.881645266961739,
+                    "y": 4.211338540895288,
+                    "heading": 2.8756978223326772,
+                    "angularVelocity": 0.5920800381277782,
+                    "velocityX": 2.739560564229402,
+                    "velocityY": -0.7001953352940375,
+                    "timestamp": 0.33970137792950006
+                },
+                {
+                    "x": 3.172468005146889,
+                    "y": 4.137008553769027,
+                    "heading": 2.9385516414717108,
+                    "angularVelocity": 0.7401067316492073,
+                    "velocityX": 3.4244516752652747,
+                    "velocityY": -0.8752391595148364,
+                    "timestamp": 0.42462672241187505
+                },
+                {
+                    "x": 3.48373017288665,
+                    "y": 4.0574555172899185,
+                    "heading": 2.9385516489955585,
+                    "angularVelocity": 8.859366728426556e-8,
+                    "velocityX": 3.6651269375110873,
+                    "velocityY": -0.936740815877624,
+                    "timestamp": 0.50955206689425
+                },
+                {
+                    "x": 3.7949923406136996,
+                    "y": 3.977902480761,
+                    "heading": 2.9385516565193726,
+                    "angularVelocity": 8.859327100912678e-8,
+                    "velocityX": 3.665126937361409,
+                    "velocityY": -0.9367408164641361,
+                    "timestamp": 0.594477411376625
+                },
+                {
+                    "x": 4.10625450834075,
+                    "y": 3.898349444232082,
+                    "heading": 2.938551664043187,
+                    "angularVelocity": 8.859327019599772e-8,
+                    "velocityX": 3.665126937361408,
+                    "velocityY": -0.9367408164641391,
+                    "timestamp": 0.679402755859
+                },
+                {
+                    "x": 4.417516676067799,
+                    "y": 3.818796407703164,
+                    "heading": 2.938551671567001,
+                    "angularVelocity": 8.859327006115765e-8,
+                    "velocityX": 3.6651269373614084,
+                    "velocityY": -0.9367408164641391,
+                    "timestamp": 0.764328100341375
+                },
+                {
+                    "x": 4.728778843794849,
+                    "y": 3.7392433711742457,
+                    "heading": 2.938551679090815,
+                    "angularVelocity": 8.859327000476347e-8,
+                    "velocityX": 3.665126937361408,
+                    "velocityY": -0.9367408164641391,
+                    "timestamp": 0.84925344482375
+                },
+                {
+                    "x": 5.0400410115218985,
+                    "y": 3.6596903346453273,
+                    "heading": 2.9385516866146295,
+                    "angularVelocity": 8.85932714022833e-8,
+                    "velocityX": 3.665126937361408,
+                    "velocityY": -0.9367408164641392,
+                    "timestamp": 0.934178789306125
+                },
+                {
+                    "x": 5.351303179248949,
+                    "y": 3.580137298116409,
+                    "heading": 2.9385516941384435,
+                    "angularVelocity": 8.859327127858633e-8,
+                    "velocityX": 3.665126937361408,
+                    "velocityY": -0.936740816464139,
+                    "timestamp": 1.0191041337885
+                },
+                {
+                    "x": 5.662565346976546,
+                    "y": 3.5005842615896343,
+                    "heading": 2.938551701662258,
+                    "angularVelocity": 8.859327107695318e-8,
+                    "velocityX": 3.6651269373678588,
+                    "velocityY": -0.9367408164388998,
+                    "timestamp": 1.104029478270875
+                },
+                {
+                    "x": 5.973827620415735,
+                    "y": 3.4210316386751205,
+                    "heading": 2.9385517091860733,
+                    "angularVelocity": 8.859328561882092e-8,
+                    "velocityX": 3.6651281821269137,
+                    "velocityY": -0.9367359461347136,
+                    "timestamp": 1.18895482275325
+                },
+                {
+                    "x": 6.292581444856613,
+                    "y": 3.380921520397279,
+                    "heading": 2.9385517170132656,
+                    "angularVelocity": 9.216556583241828e-8,
+                    "velocityX": 3.753341554087312,
+                    "velocityY": -0.4722985643722118,
+                    "timestamp": 1.273880167235625
+                },
+                {
+                    "x": 6.61322600810305,
+                    "y": 3.400918618969079,
+                    "heading": 2.938551726316265,
+                    "angularVelocity": 1.0954326272813381e-7,
+                    "velocityX": 3.7756050941068793,
+                    "velocityY": 0.23546679373140608,
+                    "timestamp": 1.358805511718
+                },
+                {
+                    "x": 6.924526035610863,
+                    "y": 3.4803224362243568,
+                    "heading": 2.9385522977985694,
+                    "angularVelocity": 0.000006729231518347491,
+                    "velocityX": 3.665572738093738,
+                    "velocityY": 0.9349837523680175,
+                    "timestamp": 1.443730856200375
+                },
+                {
+                    "x": 7.193617949641073,
+                    "y": 3.601275977700439,
+                    "heading": 2.9756713722126755,
+                    "angularVelocity": 0.43707887957768976,
+                    "velocityX": 3.1685701797306898,
+                    "velocityY": 1.4242337456892449,
+                    "timestamp": 1.52865620068275
+                },
+                {
+                    "x": 7.4107131876465635,
+                    "y": 3.752187582369628,
+                    "heading": 3.026776803352479,
+                    "angularVelocity": 0.6017689001003675,
+                    "velocityX": 2.5563068284114614,
+                    "velocityY": 1.7769913750601078,
+                    "timestamp": 1.613581545165125
+                },
+                {
+                    "x": 7.573210012728862,
+                    "y": 3.928266853326757,
+                    "heading": 3.0830399478590267,
+                    "angularVelocity": 0.6625012220965879,
+                    "velocityX": 1.9134079004651277,
+                    "velocityY": 2.0733418513677266,
+                    "timestamp": 1.6985068896475
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": 0.6707073428803676,
+                    "velocityX": 1.257457216359011,
+                    "velocityY": 2.340092323257951,
+                    "timestamp": 1.783432234129875
+                },
+                {
+                    "x": 7.730461589944827,
+                    "y": 4.347803549557207,
+                    "heading": 3.1951904179502595,
+                    "angularVelocity": 0.647312922727555,
+                    "velocityX": 0.5918498262162951,
+                    "velocityY": 2.5897428633591493,
+                    "timestamp": 1.868693034605951
+                },
+                {
+                    "x": 7.723612364676548,
+                    "y": 4.588311365928096,
+                    "heading": 3.24650445053595,
+                    "angularVelocity": 0.6018478867095445,
+                    "velocityX": -0.0803326409092394,
+                    "velocityY": 2.820848678735727,
+                    "timestamp": 1.953953835082027
+                },
+                {
+                    "x": 7.6586227331171335,
+                    "y": 4.845842471404587,
+                    "heading": 3.2908004330134384,
+                    "angularVelocity": 0.5195351466342113,
+                    "velocityX": -0.762245148960926,
+                    "velocityY": 3.020510059001304,
+                    "timestamp": 2.039214635558103
+                },
+                {
+                    "x": 7.534247896600953,
+                    "y": 5.114930519867729,
+                    "heading": 3.3218318995468055,
+                    "angularVelocity": 0.3639593618649453,
+                    "velocityX": -1.4587575512040862,
+                    "velocityY": 3.1560581997895576,
+                    "timestamp": 2.124475436034179
+                },
+                {
+                    "x": 7.350032329869969,
+                    "y": 5.3796839143357555,
+                    "heading": 3.321832164657738,
+                    "angularVelocity": 0.0000031094117170733377,
+                    "velocityX": -2.160612681353773,
+                    "velocityY": 3.1052182596188063,
+                    "timestamp": 2.209736236510255
+                },
+                {
+                    "x": 7.119535512814417,
+                    "y": 5.605296483491154,
+                    "heading": 3.3218321686163597,
+                    "angularVelocity": 4.6429567622162366e-8,
+                    "velocityX": -2.70343247739305,
+                    "velocityY": 2.6461465045558032,
+                    "timestamp": 2.294997036986331
+                },
+                {
+                    "x": 6.850897968160264,
+                    "y": 5.7838005054369255,
+                    "heading": 3.321832171960481,
+                    "angularVelocity": 3.9222265908183896e-8,
+                    "velocityX": -3.1507743670496304,
+                    "velocityY": 2.0936235755358563,
+                    "timestamp": 2.380257837462407
+                },
+                {
+                    "x": 6.563016789640927,
+                    "y": 5.929245490779759,
+                    "heading": 3.321832175184079,
+                    "angularVelocity": 3.780866961118305e-8,
+                    "velocityX": -3.376477547851724,
+                    "velocityY": 1.7058834133705343,
+                    "timestamp": 2.465518637938483
+                },
+                {
+                    "x": 6.275135446510561,
+                    "y": 6.074690150305356,
+                    "heading": 3.321832178407676,
+                    "angularVelocity": 3.780866514458699e-8,
+                    "velocityX": -3.3764794785282892,
+                    "velocityY": 1.7058795919516283,
+                    "timestamp": 2.550779438414559
+                },
+                {
+                    "x": 5.987254103377649,
+                    "y": 6.2201348098258835,
+                    "heading": 3.3218321816312852,
+                    "angularVelocity": 3.780881195826611e-8,
+                    "velocityX": -3.376479478558147,
+                    "velocityY": 1.705879591892162,
+                    "timestamp": 2.6360402388906348
+                },
+                {
+                    "x": 5.716836088038681,
+                    "y": 6.356756526684127,
+                    "heading": 3.3489498411718635,
+                    "angularVelocity": 0.3180554180720732,
+                    "velocityX": -3.171657008015607,
+                    "velocityY": 1.6023977736002932,
+                    "timestamp": 2.7213010393667108
+                },
+                {
+                    "x": 5.500501658783529,
+                    "y": 6.4660539115745435,
+                    "heading": 3.370644010379382,
+                    "angularVelocity": 0.2544448220798278,
+                    "velocityX": -2.537325805612817,
+                    "velocityY": 1.2819183526324525,
+                    "timestamp": 2.8065618398427867
+                },
+                {
+                    "x": 5.33825083142265,
+                    "y": 6.5480269544022365,
+                    "heading": 3.3869146454118173,
+                    "angularVelocity": 0.19083371187678314,
+                    "velocityX": -1.9029944177735691,
+                    "velocityY": 0.9614388132644309,
+                    "timestamp": 2.8918226403188627
+                },
+                {
+                    "x": 5.2300836110004285,
+                    "y": 6.602675651200572,
+                    "heading": 3.3977617464965464,
+                    "angularVelocity": 0.12722260434057955,
+                    "velocityX": -1.2686629707701718,
+                    "velocityY": 0.6409592273728405,
+                    "timestamp": 2.9770834407949387
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": 0.0636114211074268,
+                    "velocityX": -0.6343314946427733,
+                    "velocityY": 0.3204796183809554,
+                    "timestamp": 3.0623442412710147
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": 0,
+                    "velocityX": -1.2693090039370116e-34,
+                    "velocityY": 0,
+                    "timestamp": 3.1476050417470907
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W3 To C3 To S2": {
+            "waypoints": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 21
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.361838792645819,
+                    "y": 4.349925567383164,
+                    "heading": 2.7594883701133286,
+                    "angularVelocity": 0.10940981634945876,
+                    "velocityX": 0.7130593416825138,
+                    "velocityY": -0.11616766728823658,
+                    "timestamp": 0.08672320665473099
+                },
+                {
+                    "x": 2.485516776956774,
+                    "y": 4.329776279587658,
+                    "heading": 2.7784262463080607,
+                    "angularVelocity": 0.21837149392012115,
+                    "velocityX": 1.4261232844324094,
+                    "velocityY": -0.2323402071111933,
+                    "timestamp": 0.17344641330946198
+                },
+                {
+                    "x": 2.671034614420037,
+                    "y": 4.299551431366552,
+                    "heading": 2.806748673224486,
+                    "angularVelocity": 0.3265841752046417,
+                    "velocityX": 2.1391948547504818,
+                    "velocityY": -0.34852087909340246,
+                    "timestamp": 0.26016961996419297
+                },
+                {
+                    "x": 2.9183936206497236,
+                    "y": 4.259249598753385,
+                    "heading": 2.8443244197137543,
+                    "angularVelocity": 0.4332836381257234,
+                    "velocityX": 2.852281595334577,
+                    "velocityY": -0.46471797074593596,
+                    "timestamp": 0.34689282661892396
+                },
+                {
+                    "x": 3.2275976397156323,
+                    "y": 4.208866459419414,
+                    "heading": 2.8907555208713065,
+                    "angularVelocity": 0.5353941920351031,
+                    "velocityX": 3.5654126616527764,
+                    "velocityY": -0.5809649029072455,
+                    "timestamp": 0.43361603327365494
+                },
+                {
+                    "x": 3.5513989095488143,
+                    "y": 4.156123400332019,
+                    "heading": 2.8907555264025566,
+                    "angularVelocity": 6.378050766718983e-8,
+                    "velocityX": 3.7337326688382726,
+                    "velocityY": -0.6081769934704949,
+                    "timestamp": 0.5203392399283859
+                },
+                {
+                    "x": 3.8752001793990973,
+                    "y": 4.103380341349552,
+                    "heading": 2.8907555319337894,
+                    "angularVelocity": 6.378030392692821e-8,
+                    "velocityX": 3.7337326690354664,
+                    "velocityY": -0.6081769922605936,
+                    "timestamp": 0.6070624465831169
+                },
+                {
+                    "x": 4.19900144924938,
+                    "y": 4.050637282367084,
+                    "heading": 2.890755537465022,
+                    "angularVelocity": 6.378030384357619e-8,
+                    "velocityX": 3.7337326690354677,
+                    "velocityY": -0.6081769922605877,
+                    "timestamp": 0.6937856532378479
+                },
+                {
+                    "x": 4.522802719099664,
+                    "y": 3.9978942233846166,
+                    "heading": 2.8907555429962546,
+                    "angularVelocity": 6.378030394841341e-8,
+                    "velocityX": 3.7337326690354673,
+                    "velocityY": -0.6081769922605876,
+                    "timestamp": 0.7805088598925789
+                },
+                {
+                    "x": 4.846603988949947,
+                    "y": 3.9451511644021493,
+                    "heading": 2.8907555485274874,
+                    "angularVelocity": 6.37803046219163e-8,
+                    "velocityX": 3.7337326690354673,
+                    "velocityY": -0.6081769922605877,
+                    "timestamp": 0.8672320665473099
+                },
+                {
+                    "x": 5.170405258800231,
+                    "y": 3.8924081054196815,
+                    "heading": 2.8907555540587198,
+                    "angularVelocity": 6.37803035811712e-8,
+                    "velocityX": 3.7337326690354673,
+                    "velocityY": -0.6081769922605876,
+                    "timestamp": 0.9539552732020409
+                },
+                {
+                    "x": 5.494206528650514,
+                    "y": 3.839665046437214,
+                    "heading": 2.8907555595899526,
+                    "angularVelocity": 6.378030493154673e-8,
+                    "velocityX": 3.7337326690354673,
+                    "velocityY": -0.6081769922605876,
+                    "timestamp": 1.0406784798567719
+                },
+                {
+                    "x": 5.818007798500798,
+                    "y": 3.7869219874547464,
+                    "heading": 2.8907555651211854,
+                    "angularVelocity": 6.378030454431603e-8,
+                    "velocityX": 3.7337326690354677,
+                    "velocityY": -0.6081769922605872,
+                    "timestamp": 1.1274016865115029
+                },
+                {
+                    "x": 6.141809068352602,
+                    "y": 3.734178928481622,
+                    "heading": 2.8907555706524177,
+                    "angularVelocity": 6.378030476763932e-8,
+                    "velocityX": 3.7337326690530155,
+                    "velocityY": -0.608176992152858,
+                    "timestamp": 1.2141248931662338
+                },
+                {
+                    "x": 6.4656106506759485,
+                    "y": 3.6814377878761273,
+                    "heading": 2.890755576183656,
+                    "angularVelocity": 6.378036992489158e-8,
+                    "velocityX": 3.733736272143269,
+                    "velocityY": -0.6081548715728616,
+                    "timestamp": 1.3008480998209648
+                },
+                {
+                    "x": 6.793655691240644,
+                    "y": 3.6853799559796676,
+                    "heading": 2.8907555914201803,
+                    "angularVelocity": 1.756914361676573e-7,
+                    "velocityX": 3.7826673299885396,
+                    "velocityY": 0.045456899665096306,
+                    "timestamp": 1.3875713064756958
+                },
+                {
+                    "x": 7.092289943250509,
+                    "y": 3.7425375783401047,
+                    "heading": 2.945166528243714,
+                    "angularVelocity": 0.6274091897932109,
+                    "velocityX": 3.4435333231946736,
+                    "velocityY": 0.6590810529873269,
+                    "timestamp": 1.4742945131304268
+                },
+                {
+                    "x": 7.330977190762065,
+                    "y": 3.8181008616818835,
+                    "heading": 3.0010867643989263,
+                    "angularVelocity": 0.6448128282182443,
+                    "velocityX": 2.7522880751150582,
+                    "velocityY": 0.8713156057826263,
+                    "timestamp": 1.5610177197851578
+                },
+                {
+                    "x": 7.508670206250799,
+                    "y": 3.908197537676412,
+                    "heading": 3.053304413172006,
+                    "angularVelocity": 0.6021185192211955,
+                    "velocityX": 2.0489673104014514,
+                    "velocityY": 1.0388992689491814,
+                    "timestamp": 1.6477409264398888
+                },
+                {
+                    "x": 7.6250594852295395,
+                    "y": 4.011408797557566,
+                    "heading": 3.099932944450418,
+                    "angularVelocity": 0.5376707467014636,
+                    "velocityX": 1.342077668346812,
+                    "velocityY": 1.1901227348761163,
+                    "timestamp": 1.7344641330946198
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": 0.4620107707627286,
+                    "velocityX": 0.6335157207596422,
+                    "velocityY": 1.332875096542895,
+                    "timestamp": 1.8211873397493508
+                },
+                {
+                    "x": 7.66415878383622,
+                    "y": 4.271751344750917,
+                    "heading": 3.1759557228740287,
+                    "angularVelocity": 0.3694329307480838,
+                    "velocityX": -0.16276315552044252,
+                    "velocityY": 1.487271267174521,
+                    "timestamp": 1.918514134091254
+                },
+                {
+                    "x": 7.570652845333858,
+                    "y": 4.430649041463004,
+                    "heading": 3.2028903885273214,
+                    "angularVelocity": 0.2767446090814936,
+                    "velocityX": -0.9607419943770155,
+                    "velocityY": 1.6326202644041503,
+                    "timestamp": 2.015840928433157
+                },
+                {
+                    "x": 7.399206149109808,
+                    "y": 4.602082311613017,
+                    "heading": 3.2207860785976568,
+                    "angularVelocity": 0.1838721822839176,
+                    "velocityX": -1.761557003734937,
+                    "velocityY": 1.7614190553505542,
+                    "timestamp": 2.11316772277506
+                },
+                {
+                    "x": 7.149293090301517,
+                    "y": 4.782169514035782,
+                    "heading": 3.229614246279924,
+                    "angularVelocity": 0.09070644668783194,
+                    "velocityX": -2.5677724258579846,
+                    "velocityY": 1.8503352919456928,
+                    "timestamp": 2.210494517116963
+                },
+                {
+                    "x": 6.821439370412565,
+                    "y": 4.949709126664123,
+                    "heading": 3.229614240242725,
+                    "angularVelocity": -6.203018521599257e-8,
+                    "velocityX": -3.3685864422619463,
+                    "velocityY": 1.7214130369873843,
+                    "timestamp": 2.3078213114588664
+                },
+                {
+                    "x": 6.465396251393308,
+                    "y": 5.04346891931761,
+                    "heading": 3.2296142362544686,
+                    "angularVelocity": -4.0977992187262516e-8,
+                    "velocityX": -3.658223014810281,
+                    "velocityY": 0.9633502601977828,
+                    "timestamp": 2.4051481058007695
+                },
+                {
+                    "x": 6.106941254304619,
+                    "y": 5.12753803654929,
+                    "heading": 3.2296142322922576,
+                    "angularVelocity": -4.071038102398141e-8,
+                    "velocityX": -3.6830042488552306,
+                    "velocityY": 0.8637818372641555,
+                    "timestamp": 2.5024749001426727
+                },
+                {
+                    "x": 5.7484862494466205,
+                    "y": 5.211607120654073,
+                    "heading": 3.2296142283300466,
+                    "angularVelocity": -4.071038072165154e-8,
+                    "velocityX": -3.6830043286822827,
+                    "velocityY": 0.8637814968964618,
+                    "timestamp": 2.599801694484576
+                },
+                {
+                    "x": 5.390031244588595,
+                    "y": 5.295676204758746,
+                    "heading": 3.229614224367836,
+                    "angularVelocity": -4.0710380903709755e-8,
+                    "velocityX": -3.683004328682549,
+                    "velocityY": 0.8637814968953244,
+                    "timestamp": 2.697128488826479
+                },
+                {
+                    "x": 5.031576239716237,
+                    "y": 5.3797452888021455,
+                    "heading": 3.2296142204055998,
+                    "angularVelocity": -4.071064154144793e-8,
+                    "velocityX": -3.683004328829807,
+                    "velocityY": 0.8637814962657624,
+                    "timestamp": 2.794455283168382
+                },
+                {
+                    "x": 4.7241457561516045,
+                    "y": 5.451847166184253,
+                    "heading": 3.193769184032689,
+                    "angularVelocity": -0.3682956642649294,
+                    "velocityX": -3.1587445743322107,
+                    "velocityY": 0.740822482335317,
+                    "timestamp": 2.8917820775102854
+                },
+                {
+                    "x": 4.493572881554858,
+                    "y": 5.505923581174423,
+                    "heading": 3.1668847241449476,
+                    "angularVelocity": -0.27622876176636874,
+                    "velocityX": -2.3690585532567625,
+                    "velocityY": 0.5556169331972818,
+                    "timestamp": 2.9891088718521885
+                },
+                {
+                    "x": 4.3398576283261585,
+                    "y": 5.541974526790158,
+                    "heading": 3.1489615588617923,
+                    "angularVelocity": -0.1841544808328986,
+                    "velocityX": -1.5793724047738293,
+                    "velocityY": 0.37041131231641944,
+                    "timestamp": 3.0864356661940917
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": -0.09207699608717179,
+                    "velocityX": -0.7896862199751749,
+                    "velocityY": 0.18520566028835728,
+                    "timestamp": 3.183762460535995
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": -4.104369359528437e-34,
+                    "velocityY": 0,
+                    "timestamp": 3.281089254877898
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W3 To C3 To S3": {
+            "waypoints": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 21
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 17
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.362347004549993,
+                    "y": 4.364661246321825,
+                    "heading": 2.7702286217513103,
+                    "angularVelocity": 0.23313109188524597,
+                    "velocityX": 0.7185375961450525,
+                    "velocityY": 0.05371999426914206,
+                    "timestamp": 0.0867693004297674
+                },
+                {
+                    "x": 2.4870912612086284,
+                    "y": 4.373976445805142,
+                    "heading": 2.8086286090699177,
+                    "angularVelocity": 0.4425526900460507,
+                    "velocityX": 1.4376542860294879,
+                    "velocityY": 0.10735593622605764,
+                    "timestamp": 0.1735386008595348
+                },
+                {
+                    "x": 2.674306466073838,
+                    "y": 4.3879313037176075,
+                    "heading": 2.8616727700685,
+                    "angularVelocity": 0.6113240597291352,
+                    "velocityX": 2.1576203096940354,
+                    "velocityY": 0.16082713405947677,
+                    "timestamp": 0.2603079012893022
+                },
+                {
+                    "x": 2.9240965513714046,
+                    "y": 4.40648950041988,
+                    "heading": 2.921990092412666,
+                    "angularVelocity": 0.6951458873750875,
+                    "velocityX": 2.878784132871411,
+                    "velocityY": 0.2138797548251989,
+                    "timestamp": 0.3470772017190696
+                },
+                {
+                    "x": 3.236315453938978,
+                    "y": 4.429486199889588,
+                    "heading": 2.9648285300542776,
+                    "angularVelocity": 0.493705001993024,
+                    "velocityX": 3.598264605351849,
+                    "velocityY": 0.26503267118445795,
+                    "timestamp": 0.43384650214883697
+                },
+                {
+                    "x": 3.563623741097209,
+                    "y": 4.454241375249833,
+                    "heading": 2.9648285457681323,
+                    "angularVelocity": 1.810992388928579e-7,
+                    "velocityX": 3.7721669477231816,
+                    "velocityY": 0.28529877776624385,
+                    "timestamp": 0.5206158025786044
+                },
+                {
+                    "x": 3.8909320279988893,
+                    "y": 4.478996554002297,
+                    "heading": 2.9648285614819643,
+                    "angularVelocity": 1.8109898446503704e-7,
+                    "velocityX": 3.7721669447664756,
+                    "velocityY": 0.28529881686094194,
+                    "timestamp": 0.6073851030083718
+                },
+                {
+                    "x": 4.218240314900568,
+                    "y": 4.503751732754777,
+                    "heading": 2.9648285771957963,
+                    "angularVelocity": 1.810989844949235e-7,
+                    "velocityX": 3.772166944766461,
+                    "velocityY": 0.2852988168611311,
+                    "timestamp": 0.6941544034381392
+                },
+                {
+                    "x": 4.545548601802246,
+                    "y": 4.528506911507257,
+                    "heading": 2.9648285929096283,
+                    "angularVelocity": 1.810989846493643e-7,
+                    "velocityX": 3.7721669447664614,
+                    "velocityY": 0.2852988168611311,
+                    "timestamp": 0.7809237038679065
+                },
+                {
+                    "x": 4.872856888703925,
+                    "y": 4.5532620902597385,
+                    "heading": 2.9648286086234608,
+                    "angularVelocity": 1.8109898537942274e-7,
+                    "velocityX": 3.7721669447664614,
+                    "velocityY": 0.28529881686113107,
+                    "timestamp": 0.8676930042976739
+                },
+                {
+                    "x": 5.200165175605603,
+                    "y": 4.578017269012219,
+                    "heading": 2.964828624337293,
+                    "angularVelocity": 1.8109898550822278e-7,
+                    "velocityX": 3.7721669447664614,
+                    "velocityY": 0.28529881686113095,
+                    "timestamp": 0.9544623047274413
+                },
+                {
+                    "x": 5.527473462507282,
+                    "y": 4.602772447764699,
+                    "heading": 2.9648286400511257,
+                    "angularVelocity": 1.8109898581520642e-7,
+                    "velocityX": 3.772166944766462,
+                    "velocityY": 0.2852988168611311,
+                    "timestamp": 1.0412316051572088
+                },
+                {
+                    "x": 5.854781749408961,
+                    "y": 4.627527626517179,
+                    "heading": 2.964828655764958,
+                    "angularVelocity": 1.8109898550546543e-7,
+                    "velocityX": 3.772166944766461,
+                    "velocityY": 0.28529881686113034,
+                    "timestamp": 1.1280009055869762
+                },
+                {
+                    "x": 6.182090036311611,
+                    "y": 4.652282805256812,
+                    "heading": 2.9648286714787906,
+                    "angularVelocity": 1.810989852504463e-7,
+                    "velocityX": 3.772166944777661,
+                    "velocityY": 0.2852988167130518,
+                    "timestamp": 1.2147702060167436
+                },
+                {
+                    "x": 6.509398523986194,
+                    "y": 4.6770353292870315,
+                    "heading": 2.964828687192649,
+                    "angularVelocity": 1.8109928558732558e-7,
+                    "velocityX": 3.7721692586367297,
+                    "velocityY": 0.2852682216823196,
+                    "timestamp": 1.301539506446511
+                },
+                {
+                    "x": 6.835912518328979,
+                    "y": 4.643388028658291,
+                    "heading": 2.964828718989663,
+                    "angularVelocity": 3.6645465598978293e-7,
+                    "velocityX": 3.763012871206354,
+                    "velocityY": -0.38777886259409694,
+                    "timestamp": 1.3883088068762783
+                },
+                {
+                    "x": 7.128079325417914,
+                    "y": 4.560807160262635,
+                    "heading": 3.033554953244841,
+                    "angularVelocity": 0.7920570284049364,
+                    "velocityX": 3.3671679458268646,
+                    "velocityY": -0.9517291021897464,
+                    "timestamp": 1.4750781073060457
+                },
+                {
+                    "x": 7.358963687834594,
+                    "y": 4.4649781472961925,
+                    "heading": 3.091042362636633,
+                    "angularVelocity": 0.66253166854011,
+                    "velocityX": 2.660899203671262,
+                    "velocityY": -1.1044114968289644,
+                    "timestamp": 1.561847407735813
+                },
+                {
+                    "x": 7.527984669479397,
+                    "y": 4.3598798773022285,
+                    "heading": 3.1296058219303373,
+                    "angularVelocity": 0.44443667406213694,
+                    "velocityX": 1.947935281345401,
+                    "velocityY": -1.211237954822889,
+                    "timestamp": 1.6486167081655805
+                },
+                {
+                    "x": 7.635013269633329,
+                    "y": 4.246969115663087,
+                    "heading": 3.1464129858106427,
+                    "angularVelocity": 0.19369942822011468,
+                    "velocityX": 1.2334846497991885,
+                    "velocityY": -1.3012754635555983,
+                    "timestamp": 1.7353860085953479
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": -0.07390846507784325,
+                    "velocityX": 0.5184636748694783,
+                    "velocityY": -1.3826216768935726,
+                    "timestamp": 1.8221553090251152
+                },
+                {
+                    "x": 7.665105053681803,
+                    "y": 4.004188436329708,
+                    "heading": 3.1110105191844553,
+                    "angularVelocity": -0.3438049921625736,
+                    "velocityX": -0.17664879667123173,
+                    "velocityY": -1.4565017205308959,
+                    "timestamp": 1.9064748549200063
+                },
+                {
+                    "x": 7.591499034942275,
+                    "y": 3.875650129704607,
+                    "heading": 3.0609451613914906,
+                    "angularVelocity": -0.5937574409541295,
+                    "velocityX": -0.8729413561036249,
+                    "velocityY": -1.524418867071828,
+                    "timestamp": 1.9907944008148974
+                },
+                {
+                    "x": 7.459038327956553,
+                    "y": 3.7422011819159797,
+                    "heading": 2.9924948430687532,
+                    "angularVelocity": -0.8117965721502389,
+                    "velocityX": -1.5709371484381591,
+                    "velocityY": -1.582657334931321,
+                    "timestamp": 2.0751139467097883
+                },
+                {
+                    "x": 7.267508984071126,
+                    "y": 3.6053961445739597,
+                    "heading": 2.9106651455038604,
+                    "angularVelocity": -0.9704712791847574,
+                    "velocityX": -2.271470296153853,
+                    "velocityY": -1.6224593703641987,
+                    "timestamp": 2.159433492604679
+                },
+                {
+                    "x": 7.016701982610521,
+                    "y": 3.4693216018432858,
+                    "heading": 2.828330372603407,
+                    "angularVelocity": -0.9764612940763276,
+                    "velocityX": -2.9744823551736075,
+                    "velocityY": -1.6137959625671907,
+                    "timestamp": 2.24375303849957
+                },
+                {
+                    "x": 6.717326936237698,
+                    "y": 3.359230054214691,
+                    "heading": 2.828330320326688,
+                    "angularVelocity": -6.199834052808441e-7,
+                    "velocityX": -3.5504821947927914,
+                    "velocityY": -1.3056468279114077,
+                    "timestamp": 2.328072584394461
+                },
+                {
+                    "x": 6.402750279383998,
+                    "y": 3.3064370272518535,
+                    "heading": 2.8283303027221653,
+                    "angularVelocity": -2.0878341406433196e-7,
+                    "velocityX": -3.730767920000857,
+                    "velocityY": -0.6261066328398724,
+                    "timestamp": 2.412392130289352
+                },
+                {
+                    "x": 6.0870128630404565,
+                    "y": 3.261099838136822,
+                    "heading": 2.8283302851918997,
+                    "angularVelocity": -2.0790275245783378e-7,
+                    "velocityX": -3.7445341171206636,
+                    "velocityY": -0.5376830322538348,
+                    "timestamp": 2.4967116761842427
+                },
+                {
+                    "x": 5.771275440975781,
+                    "y": 3.215762688864957,
+                    "heading": 2.8283302676616344,
+                    "angularVelocity": -2.079027527224642e-7,
+                    "velocityX": -3.7445341849713016,
+                    "velocityY": -0.5376825597279692,
+                    "timestamp": 2.5810312220791336
+                },
+                {
+                    "x": 5.455538018911073,
+                    "y": 3.1704255395933156,
+                    "heading": 2.8283302501313687,
+                    "angularVelocity": -2.079027528417884e-7,
+                    "velocityX": -3.7445341849716827,
+                    "velocityY": -0.5376825597253148,
+                    "timestamp": 2.6653507679740245
+                },
+                {
+                    "x": 5.13980059641411,
+                    "y": 3.1250883933321525,
+                    "heading": 2.8283302326010555,
+                    "angularVelocity": -2.0790331306647263e-7,
+                    "velocityX": -3.7445341900980704,
+                    "velocityY": -0.5376825240221068,
+                    "timestamp": 2.7496703138689154
+                },
+                {
+                    "x": 4.847289842857696,
+                    "y": 3.0834618283006696,
+                    "heading": 2.7687586646684967,
+                    "angularVelocity": -0.706497731935343,
+                    "velocityX": -3.469074109116353,
+                    "velocityY": -0.4936763426521838,
+                    "timestamp": 2.8339898597638062
+                },
+                {
+                    "x": 4.613405710202319,
+                    "y": 3.0500832751446354,
+                    "heading": 2.6983183223854565,
+                    "angularVelocity": -0.8353975526724018,
+                    "velocityX": -2.773783114853639,
+                    "velocityY": -0.39585783820090814,
+                    "timestamp": 2.918309405658697
+                },
+                {
+                    "x": 4.43813741968374,
+                    "y": 3.025042089065369,
+                    "heading": 2.6386185119236694,
+                    "angularVelocity": -0.7080186429870714,
+                    "velocityX": -2.0786199529236167,
+                    "velocityY": -0.29697961265685946,
+                    "timestamp": 3.002628951553588
+                },
+                {
+                    "x": 4.321361759336474,
+                    "y": 3.008347212158301,
+                    "heading": 2.596145764785128,
+                    "angularVelocity": -0.5037117632427286,
+                    "velocityX": -1.3849180413380506,
+                    "velocityY": -0.19799533702279348,
+                    "timestamp": 3.086948497448479
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": -0.26264093989232323,
+                    "velocityX": -0.6921498297586374,
+                    "velocityY": -0.0989949847299469,
+                    "timestamp": 3.1712680433433698
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": 0,
+                    "velocityX": -1.507480653475374e-32,
+                    "velocityY": 0,
+                    "timestamp": 3.2555875892382606
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W3 To C3 To S4": {
+            "waypoints": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 21
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 18
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.35775083125341,
+                    "y": 4.369912807689986,
+                    "heading": 2.783102260210542,
+                    "angularVelocity": 0.39244175739179554,
+                    "velocityX": 0.6846613362282545,
+                    "velocityY": 0.11752066613584591,
+                    "timestamp": 0.08434948520907419
+                },
+                {
+                    "x": 2.473379945358936,
+                    "y": 4.389732628016986,
+                    "heading": 2.846401171320743,
+                    "angularVelocity": 0.7504362469231927,
+                    "velocityX": 1.3708336668436103,
+                    "velocityY": 0.2349726293868104,
+                    "timestamp": 0.16869897041814838
+                },
+                {
+                    "x": 2.647085500332804,
+                    "y": 4.419444254302336,
+                    "heading": 2.9346614354688145,
+                    "angularVelocity": 1.0463639929669244,
+                    "velocityX": 2.0593552473178725,
+                    "velocityY": 0.35224431081829954,
+                    "timestamp": 0.25304845562722256
+                },
+                {
+                    "x": 2.8791780514976093,
+                    "y": 4.45897416371866,
+                    "heading": 3.036264025920344,
+                    "angularVelocity": 1.2045431006447933,
+                    "velocityX": 2.7515585968251655,
+                    "velocityY": 0.4686443470086779,
+                    "timestamp": 0.33739794083629676
+                },
+                {
+                    "x": 3.1692885380051594,
+                    "y": 4.507734938420023,
+                    "heading": 3.1081945167682674,
+                    "angularVelocity": 0.8527673959080092,
+                    "velocityX": 3.439386568732019,
+                    "velocityY": 0.5780802879887197,
+                    "timestamp": 0.42174742604537097
+                },
+                {
+                    "x": 3.483614233052985,
+                    "y": 4.562663983733042,
+                    "heading": 3.108194542936824,
+                    "angularVelocity": 3.1023966932602597e-7,
+                    "velocityX": 3.726468445761316,
+                    "velocityY": 0.651207831048041,
+                    "timestamp": 0.5060969112544451
+                },
+                {
+                    "x": 3.7979399253430755,
+                    "y": 4.61759304482709,
+                    "heading": 3.108194569105307,
+                    "angularVelocity": 3.102387994950786e-7,
+                    "velocityX": 3.726468413067156,
+                    "velocityY": 0.6512080181390185,
+                    "timestamp": 0.5904463964635193
+                },
+                {
+                    "x": 4.112265617633151,
+                    "y": 4.67252210592122,
+                    "heading": 3.1081945952737904,
+                    "angularVelocity": 3.1023880025122276e-7,
+                    "velocityX": 3.7264684130669847,
+                    "velocityY": 0.6512080181399977,
+                    "timestamp": 0.6747958816725934
+                },
+                {
+                    "x": 4.426591309923227,
+                    "y": 4.72745116701535,
+                    "heading": 3.1081946214422733,
+                    "angularVelocity": 3.102388010308895e-7,
+                    "velocityX": 3.7264684130669847,
+                    "velocityY": 0.6512080181399976,
+                    "timestamp": 0.7591453668816676
+                },
+                {
+                    "x": 4.740917002213303,
+                    "y": 4.7823802281094805,
+                    "heading": 3.1081946476107567,
+                    "angularVelocity": 3.1023880026526175e-7,
+                    "velocityX": 3.726468413066985,
+                    "velocityY": 0.6512080181399978,
+                    "timestamp": 0.8434948520907417
+                },
+                {
+                    "x": 5.055242694503379,
+                    "y": 4.837309289203611,
+                    "heading": 3.1081946737792396,
+                    "angularVelocity": 3.1023880190764424e-7,
+                    "velocityX": 3.7264684130669847,
+                    "velocityY": 0.6512080181399977,
+                    "timestamp": 0.9278443372998159
+                },
+                {
+                    "x": 5.369568386793454,
+                    "y": 4.892238350297741,
+                    "heading": 3.108194699947723,
+                    "angularVelocity": 3.102388017253904e-7,
+                    "velocityX": 3.726468413066985,
+                    "velocityY": 0.6512080181399975,
+                    "timestamp": 1.01219382250889
+                },
+                {
+                    "x": 5.683894079083919,
+                    "y": 4.947167411389647,
+                    "heading": 3.108194726116206,
+                    "angularVelocity": 3.102388024033471e-7,
+                    "velocityX": 3.7264684130715926,
+                    "velocityY": 0.6512080181136309,
+                    "timestamp": 1.0965433077179643
+                },
+                {
+                    "x": 5.9982198456255675,
+                    "y": 5.002096047585362,
+                    "heading": 3.108194752284696,
+                    "angularVelocity": 3.102388798208338e-7,
+                    "velocityX": 3.7264692933518155,
+                    "velocityY": 0.6512029807836425,
+                    "timestamp": 1.1808927929270385
+                },
+                {
+                    "x": 6.316938354394085,
+                    "y": 5.017469863405169,
+                    "heading": 3.1081947796852947,
+                    "angularVelocity": 3.2484607308932857e-7,
+                    "velocityX": 3.7785471716694046,
+                    "velocityY": 0.18226330346533962,
+                    "timestamp": 1.2652422781361128
+                },
+                {
+                    "x": 6.6329944526576226,
+                    "y": 4.973579291611239,
+                    "heading": 3.1081948124098697,
+                    "angularVelocity": 3.8796413400864556e-7,
+                    "velocityX": 3.7469831319081606,
+                    "velocityY": -0.5203419046973292,
+                    "timestamp": 1.349591763345187
+                },
+                {
+                    "x": 6.935463453156901,
+                    "y": 4.87194123143387,
+                    "heading": 3.1081950470159,
+                    "angularVelocity": 0.0000027813569896549863,
+                    "velocityX": 3.5859021516202274,
+                    "velocityY": -1.2049636097415746,
+                    "timestamp": 1.4339412485542613
+                },
+                {
+                    "x": 7.201083494587787,
+                    "y": 4.724560137748792,
+                    "heading": 3.137800042082307,
+                    "angularVelocity": 0.3509801511298589,
+                    "velocityX": 3.1490416423111984,
+                    "velocityY": -1.7472672574084946,
+                    "timestamp": 1.5182907337633356
+                },
+                {
+                    "x": 7.415442295317108,
+                    "y": 4.547383804871535,
+                    "heading": 3.160342409540337,
+                    "angularVelocity": 0.26724961512396933,
+                    "velocityX": 2.541317237419968,
+                    "velocityY": -2.1005028357683178,
+                    "timestamp": 1.6026402189724098
+                },
+                {
+                    "x": 7.575455349602933,
+                    "y": 4.347068945452764,
+                    "heading": 3.163134577298841,
+                    "angularVelocity": 0.033102368693571485,
+                    "velocityX": 1.8970246693172619,
+                    "velocityY": -2.374820177292801,
+                    "timestamp": 1.686989704181484
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": -0.27427052152716735,
+                    "velocityX": 1.2394225067045201,
+                    "velocityY": -2.6090134979162847,
+                    "timestamp": 1.7713391893905583
+                },
+                {
+                    "x": 7.728694415127363,
+                    "y": 3.916223066041241,
+                    "heading": 3.095787658836112,
+                    "angularVelocity": -0.5866618963302697,
+                    "velocityX": 0.6461353813727732,
+                    "velocityY": -2.796838903430975,
+                    "timestamp": 1.84670174734847
+                },
+                {
+                    "x": 7.732075801393433,
+                    "y": 3.692700126888283,
+                    "heading": 3.0330227964139005,
+                    "angularVelocity": -0.8328388011625617,
+                    "velocityX": 0.04486825232179016,
+                    "velocityY": -2.9659680511082303,
+                    "timestamp": 1.9220643053063815
+                },
+                {
+                    "x": 7.689365986032336,
+                    "y": 3.4587309949886484,
+                    "heading": 2.9594753921163566,
+                    "angularVelocity": -0.9759143836202951,
+                    "velocityX": -0.5667245979754308,
+                    "velocityY": -3.10458055351971,
+                    "timestamp": 1.997426863264293
+                },
+                {
+                    "x": 7.599710660115244,
+                    "y": 3.218597864956733,
+                    "heading": 2.8890529754773646,
+                    "angularVelocity": -0.9344483327957235,
+                    "velocityX": -1.1896534346294716,
+                    "velocityY": -3.1863718076823337,
+                    "timestamp": 2.072789421222205
+                },
+                {
+                    "x": 7.463852101408951,
+                    "y": 2.9820453977091343,
+                    "heading": 2.853986095526745,
+                    "angularVelocity": -0.4653090460411922,
+                    "velocityX": -1.8027328475523154,
+                    "velocityY": -3.138859317642961,
+                    "timestamp": 2.1481519791801165
+                },
+                {
+                    "x": 7.284189485763313,
+                    "y": 2.760688689378167,
+                    "heading": 2.8539858842251613,
+                    "angularVelocity": -0.000002803800583333773,
+                    "velocityX": -2.3839771434772157,
+                    "velocityY": -2.9372239256341386,
+                    "timestamp": 2.223514537138028
+                },
+                {
+                    "x": 7.070363087296056,
+                    "y": 2.5721263513481594,
+                    "heading": 2.853985845147589,
+                    "angularVelocity": -5.185276832708053e-7,
+                    "velocityX": -2.8373028233287183,
+                    "velocityY": -2.502069238882734,
+                    "timestamp": 2.2988770950959396
+                },
+                {
+                    "x": 6.82827305330887,
+                    "y": 2.421561078917411,
+                    "heading": 2.8539858112509897,
+                    "angularVelocity": -4.4978037103687734e-7,
+                    "velocityX": -3.212338335468772,
+                    "velocityY": -1.9978790066392913,
+                    "timestamp": 2.374239653053851
+                },
+                {
+                    "x": 6.571710658920333,
+                    "y": 2.2972500819001187,
+                    "heading": 2.8539857783310936,
+                    "angularVelocity": -4.368203133600748e-7,
+                    "velocityX": -3.4043748160966305,
+                    "velocityY": -1.6495060728527418,
+                    "timestamp": 2.449602211011763
+                },
+                {
+                    "x": 6.3151480830563145,
+                    "y": 2.172939459426277,
+                    "heading": 2.8539857454112014,
+                    "angularVelocity": -4.3682025848078035e-7,
+                    "velocityX": -3.4043772241290333,
+                    "velocityY": -1.6495011029650315,
+                    "timestamp": 2.5249647689696744
+                },
+                {
+                    "x": 6.058585509566717,
+                    "y": 2.048628832051934,
+                    "heading": 2.853985712491288,
+                    "angularVelocity": -4.3682054129494706e-7,
+                    "velocityX": -3.404377192622409,
+                    "velocityY": -1.6495011679907159,
+                    "timestamp": 2.600327326927586
+                },
+                {
+                    "x": 5.808645132247433,
+                    "y": 1.9267432640802056,
+                    "heading": 2.8368087867037386,
+                    "angularVelocity": -0.22792386900060174,
+                    "velocityX": -3.31650602224609,
+                    "velocityY": -1.6173225972478102,
+                    "timestamp": 2.6756898848854975
+                },
+                {
+                    "x": 5.597200041398206,
+                    "y": 1.824292282290654,
+                    "heading": 2.751245197802903,
+                    "angularVelocity": -1.1353594041834565,
+                    "velocityX": -2.805704803270014,
+                    "velocityY": -1.3594414065240215,
+                    "timestamp": 2.751052442843409
+                },
+                {
+                    "x": 5.4283718675083765,
+                    "y": 1.7424505925946856,
+                    "heading": 2.6610334034240704,
+                    "angularVelocity": -1.1970373196357535,
+                    "velocityX": -2.2402128917136284,
+                    "velocityY": -1.0859728214330957,
+                    "timestamp": 2.8264150008013207
+                },
+                {
+                    "x": 5.302034408244249,
+                    "y": 1.6811743111050435,
+                    "heading": 2.5842866606936457,
+                    "angularVelocity": -1.0183670088969912,
+                    "velocityX": -1.6763955827333314,
+                    "velocityY": -0.8130865399216356,
+                    "timestamp": 2.9017775587592323
+                },
+                {
+                    "x": 5.217968301319177,
+                    "y": 1.6403775914720826,
+                    "heading": 2.5290716090988563,
+                    "angularVelocity": -0.7326589368904639,
+                    "velocityX": -1.1154890333210488,
+                    "velocityY": -0.5413393698200221,
+                    "timestamp": 2.977140116717144
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": -0.38575666599708336,
+                    "velocityX": -0.5568853082536673,
+                    "velocityY": -0.27039410582988466,
+                    "timestamp": 3.0525026746750554
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": 0,
+                    "velocityX": 5.6652938345374496e-33,
+                    "velocityY": 3.5857679285094535e-32,
+                    "timestamp": 3.127865232632967
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W3 To C4 To S3": {
+            "waypoints": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 22
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 17
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": -1.5074616592069685e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.3610343967602465,
+                    "y": 4.342403579285033,
+                    "heading": 2.7671031550368004,
+                    "angularVelocity": 0.1956775196968839,
+                    "velocityX": 0.6982956857107938,
+                    "velocityY": -0.20132098163403012,
+                    "timestamp": 0.08740480287819585
+                },
+                {
+                    "x": 2.4831385894662814,
+                    "y": 4.307214947818553,
+                    "heading": 2.799662518084232,
+                    "angularVelocity": 0.3725122873717296,
+                    "velocityX": 1.3969963741717348,
+                    "velocityY": -0.4025937969966943,
+                    "timestamp": 0.1748096057563917
+                },
+                {
+                    "x": 2.6663651072482595,
+                    "y": 4.254442931030586,
+                    "heading": 2.8448933899423676,
+                    "angularVelocity": 0.5174872589229184,
+                    "velocityX": 2.0962980494025705,
+                    "velocityY": -0.6037656404477811,
+                    "timestamp": 0.26221440863458756
+                },
+                {
+                    "x": 2.9107932372110517,
+                    "y": 4.184113644093446,
+                    "heading": 2.897077048059956,
+                    "angularVelocity": 0.597034217791321,
+                    "velocityX": 2.796506849897233,
+                    "velocityY": -0.8046386997193782,
+                    "timestamp": 0.3496192115127834
+                },
+                {
+                    "x": 3.2164117287156215,
+                    "y": 4.096391426129405,
+                    "heading": 2.937460118957845,
+                    "angularVelocity": 0.46202347660649606,
+                    "velocityX": 3.4965869316182627,
+                    "velocityY": -1.0036315519901937,
+                    "timestamp": 0.43702401439097927
+                },
+                {
+                    "x": 3.5340454438791253,
+                    "y": 4.004541690732033,
+                    "heading": 2.9374601327514616,
+                    "angularVelocity": 1.5781302417851277e-7,
+                    "velocityX": 3.6340533323568787,
+                    "velocityY": -1.0508545568756669,
+                    "timestamp": 0.5244288172691751
+                },
+                {
+                    "x": 3.8516791581377654,
+                    "y": 3.9126919522054426,
+                    "heading": 2.93746014654506,
+                    "angularVelocity": 1.5781282325664268e-7,
+                    "velocityX": 3.634053322004319,
+                    "velocityY": -1.050854592677122,
+                    "timestamp": 0.611833620147371
+                },
+                {
+                    "x": 4.169312872396402,
+                    "y": 3.820842213678837,
+                    "heading": 2.9374601603386585,
+                    "angularVelocity": 1.5781282237025435e-7,
+                    "velocityX": 3.63405332200427,
+                    "velocityY": -1.0508545926772908,
+                    "timestamp": 0.6992384230255668
+                },
+                {
+                    "x": 4.486946586655037,
+                    "y": 3.7289924751522316,
+                    "heading": 2.9374601741322572,
+                    "angularVelocity": 1.5781282238426364e-7,
+                    "velocityX": 3.63405332200427,
+                    "velocityY": -1.0508545926772912,
+                    "timestamp": 0.7866432259037627
+                },
+                {
+                    "x": 4.804580300913673,
+                    "y": 3.637142736625626,
+                    "heading": 2.937460187925856,
+                    "angularVelocity": 1.578128226198401e-7,
+                    "velocityX": 3.63405332200427,
+                    "velocityY": -1.0508545926772912,
+                    "timestamp": 0.8740480287819585
+                },
+                {
+                    "x": 5.122214015172308,
+                    "y": 3.545292998099021,
+                    "heading": 2.9374602017194547,
+                    "angularVelocity": 1.5781282274310823e-7,
+                    "velocityX": 3.63405332200427,
+                    "velocityY": -1.050854592677291,
+                    "timestamp": 0.9614528316601544
+                },
+                {
+                    "x": 5.439847729430944,
+                    "y": 3.4534432595724156,
+                    "heading": 2.937460215513054,
+                    "angularVelocity": 1.5781282350457181e-7,
+                    "velocityX": 3.63405332200427,
+                    "velocityY": -1.050854592677291,
+                    "timestamp": 1.0488576345383502
+                },
+                {
+                    "x": 5.757481443689581,
+                    "y": 3.3615935210458106,
+                    "heading": 2.9374602293066525,
+                    "angularVelocity": 1.5781282308668676e-7,
+                    "velocityX": 3.6340533220042697,
+                    "velocityY": -1.0508545926772905,
+                    "timestamp": 1.136262437416546
+                },
+                {
+                    "x": 6.075115157948217,
+                    "y": 3.269743782519205,
+                    "heading": 2.9374602431002512,
+                    "angularVelocity": 1.578128234236369e-7,
+                    "velocityX": 3.63405332200427,
+                    "velocityY": -1.050854592677291,
+                    "timestamp": 1.2236672402947417
+                },
+                {
+                    "x": 6.3927488722066945,
+                    "y": 3.1778940439920533,
+                    "heading": 2.93746025689385,
+                    "angularVelocity": 1.5781282293850814e-7,
+                    "velocityX": 3.6340533220024622,
+                    "velocityY": -1.0508545926835444,
+                    "timestamp": 1.3110720431729375
+                },
+                {
+                    "x": 6.710382552956458,
+                    "y": 3.0860441895855293,
+                    "heading": 2.9374602706874486,
+                    "angularVelocity": 1.5781282618344319e-7,
+                    "velocityX": 3.634052938628626,
+                    "velocityY": -1.0508559184616288,
+                    "timestamp": 1.3984768460511332
+                },
+                {
+                    "x": 7.02127514506585,
+                    "y": 2.9734682388937714,
+                    "heading": 2.9374602922500688,
+                    "angularVelocity": 2.466983432002081e-7,
+                    "velocityX": 3.5569280162171513,
+                    "velocityY": -1.2879835773857788,
+                    "timestamp": 1.485881648929329
+                },
+                {
+                    "x": 7.2761933316047935,
+                    "y": 2.8394289044438907,
+                    "heading": 3.0149898604295666,
+                    "angularVelocity": 0.8870172533600954,
+                    "velocityX": 2.9165237852453965,
+                    "velocityY": -1.533546556207826,
+                    "timestamp": 1.5732864518075247
+                },
+                {
+                    "x": 7.469158921958533,
+                    "y": 2.7195716127256344,
+                    "heading": 3.0771718399112147,
+                    "angularVelocity": 0.711425201293604,
+                    "velocityX": 2.207722962577334,
+                    "velocityY": -1.3712895375473233,
+                    "timestamp": 1.6606912546857204
+                },
+                {
+                    "x": 7.6006701652701585,
+                    "y": 2.6155462997255583,
+                    "heading": 3.1196274586402084,
+                    "angularVelocity": 0.48573553547347,
+                    "velocityX": 1.5046226177627207,
+                    "velocityY": -1.1901555701125741,
+                    "timestamp": 1.7480960575639162
+                },
+                {
+                    "x": 7.670917138983036,
+                    "y": 2.527925545784097,
+                    "heading": 3.140832038047466,
+                    "angularVelocity": 0.24260199335736132,
+                    "velocityX": 0.8036969525664591,
+                    "velocityY": -1.0024707001921451,
+                    "timestamp": 1.835500860442112
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": -0.009519363010585313,
+                    "velocityX": 0.10391718438655147,
+                    "velocityY": -0.8114605084452422,
+                    "timestamp": 1.9229056633203077
+                },
+                {
+                    "x": 7.63404119577111,
+                    "y": 2.4050502975891916,
+                    "heading": 3.1190998231163127,
+                    "angularVelocity": -0.25309519096022687,
+                    "velocityX": -0.5565480329352274,
+                    "velocityY": -0.6290961040742543,
+                    "timestamp": 2.005483988117367
+                },
+                {
+                    "x": 7.533566509755774,
+                    "y": 2.3683786594075307,
+                    "heading": 3.079403507657828,
+                    "angularVelocity": -0.4807110771021385,
+                    "velocityX": -1.216719838556405,
+                    "velocityY": -0.4440830965242228,
+                    "timestamp": 2.0880623129144262
+                },
+                {
+                    "x": 7.378621026771135,
+                    "y": 2.3473334555907552,
+                    "heading": 3.0230319998393305,
+                    "angularVelocity": -0.6826429084997041,
+                    "velocityX": -1.876345679879389,
+                    "velocityY": -0.2548514258250533,
+                    "timestamp": 2.1706406377114855
+                },
+                {
+                    "x": 7.169309740069539,
+                    "y": 2.342556806639995,
+                    "heading": 2.953940328458666,
+                    "angularVelocity": -0.8366804673073884,
+                    "velocityX": -2.5347000828121677,
+                    "velocityY": -0.05784385869415158,
+                    "timestamp": 2.253218962508545
+                },
+                {
+                    "x": 6.906019202766067,
+                    "y": 2.3556284163077987,
+                    "heading": 2.881970584756294,
+                    "angularVelocity": -0.8715331036230265,
+                    "velocityX": -3.188373437588175,
+                    "velocityY": 0.15829347107643843,
+                    "timestamp": 2.335797287305604
+                },
+                {
+                    "x": 6.596097301196444,
+                    "y": 2.394810276985141,
+                    "heading": 2.8819705158229665,
+                    "angularVelocity": -8.347629724920641e-7,
+                    "velocityX": -3.753065981070365,
+                    "velocityY": 0.47448117618799124,
+                    "timestamp": 2.4183756121026634
+                },
+                {
+                    "x": 6.2936576837370914,
+                    "y": 2.4730221394670577,
+                    "heading": 2.881970499784504,
+                    "angularVelocity": -1.942212104248679e-7,
+                    "velocityX": -3.6624576509951607,
+                    "velocityY": 0.9471233846669401,
+                    "timestamp": 2.5009539368997227
+                },
+                {
+                    "x": 5.991218190130788,
+                    "y": 2.5512344808782137,
+                    "heading": 2.8819704837460454,
+                    "angularVelocity": -1.9422117139312037e-7,
+                    "velocityX": -3.662456151169988,
+                    "velocityY": 0.9471291843636568,
+                    "timestamp": 2.583532261696782
+                },
+                {
+                    "x": 5.688778696525171,
+                    "y": 2.6294468222920195,
+                    "heading": 2.8819704677075864,
+                    "angularVelocity": -1.9422117079756682e-7,
+                    "velocityX": -3.662456151161689,
+                    "velocityY": 0.9471291843957483,
+                    "timestamp": 2.6661105864938413
+                },
+                {
+                    "x": 5.38633920291957,
+                    "y": 2.7076591637058867,
+                    "heading": 2.881970451669127,
+                    "angularVelocity": -1.9422117106487922e-7,
+                    "velocityX": -3.6624561511614977,
+                    "velocityY": 0.947129184396488,
+                    "timestamp": 2.7486889112909005
+                },
+                {
+                    "x": 5.0838997121673595,
+                    "y": 2.7858715161533545,
+                    "heading": 2.881970435630565,
+                    "angularVelocity": -1.9422242240409168e-7,
+                    "velocityX": -3.662456116607741,
+                    "velocityY": 0.9471293180102566,
+                    "timestamp": 2.83126723608796
+                },
+                {
+                    "x": 4.810061938834441,
+                    "y": 2.8575158595499386,
+                    "heading": 2.7916066716165844,
+                    "angularVelocity": -1.094279452096593,
+                    "velocityX": -3.3160974626924187,
+                    "velocityY": 0.8675925985741851,
+                    "timestamp": 2.913845560885019
+                },
+                {
+                    "x": 4.591150287853899,
+                    "y": 2.914595495450747,
+                    "heading": 2.7083456685941125,
+                    "angularVelocity": -1.0082670389244417,
+                    "velocityX": -2.650957760629438,
+                    "velocityY": 0.6912181379446108,
+                    "timestamp": 2.9964238856820784
+                },
+                {
+                    "x": 4.427043317855732,
+                    "y": 2.957326813953426,
+                    "heading": 2.64244137634817,
+                    "angularVelocity": -0.7980822135579249,
+                    "velocityX": -1.98728867897803,
+                    "velocityY": 0.5174640997827658,
+                    "timestamp": 3.0790022104791377
+                },
+                {
+                    "x": 4.317672771154393,
+                    "y": 2.985782903772343,
+                    "heading": 2.597132660112387,
+                    "angularVelocity": -0.5486756524442865,
+                    "velocityX": -1.3244461784629789,
+                    "velocityY": 0.34459514514068673,
+                    "timestamp": 3.161580535276197
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": -0.28012992718412955,
+                    "velocityX": -0.6620716911944379,
+                    "velocityY": 0.1721649871512405,
+                    "timestamp": 3.2441588600732563
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": 0,
+                    "velocityX": -8.552238423258885e-34,
+                    "velocityY": -9.381987056731701e-33,
+                    "timestamp": 3.3267371848703156
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W3 To C4 To S4": {
+            "waypoints": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 22
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.360128743568809,
+                    "y": 4.349177185639382,
+                    "heading": 2.7782206485451093,
+                    "angularVelocity": 0.3283351874090942,
+                    "velocityX": 0.6995722389859838,
+                    "velocityY": -0.1259188206007136,
+                    "timestamp": 0.0859507284851176
+                },
+                {
+                    "x": 2.4804864804971807,
+                    "y": 4.327553542642656,
+                    "heading": 2.831856423124361,
+                    "angularVelocity": 0.6240293191760308,
+                    "velocityX": 1.4003108414515997,
+                    "velocityY": -0.2515818466910346,
+                    "timestamp": 0.1719014569702352
+                },
+                {
+                    "x": 2.6612218913219765,
+                    "y": 4.295165149158174,
+                    "heading": 2.9060715845563863,
+                    "angularVelocity": 0.863461691832829,
+                    "velocityX": 2.102779278433835,
+                    "velocityY": -0.3768251189411322,
+                    "timestamp": 0.2578521854553528
+                },
+                {
+                    "x": 2.902552042800783,
+                    "y": 4.252097385240196,
+                    "heading": 2.990739532355289,
+                    "angularVelocity": 0.9850753948357858,
+                    "velocityX": 2.8077731943900375,
+                    "velocityY": -0.5010750307419894,
+                    "timestamp": 0.3438029139404704
+                },
+                {
+                    "x": 3.2042136582065344,
+                    "y": 4.198837394002331,
+                    "heading": 3.04987623940461,
+                    "angularVelocity": 0.6880303179693488,
+                    "velocityX": 3.5097039981224545,
+                    "velocityY": -0.6196572405676055,
+                    "timestamp": 0.429753642425588
+                },
+                {
+                    "x": 3.524112842751923,
+                    "y": 4.1406587673803905,
+                    "heading": 3.0498762636795202,
+                    "angularVelocity": 2.824282081012085e-7,
+                    "velocityX": 3.7218903223232087,
+                    "velocityY": -0.6768834615754469,
+                    "timestamp": 0.5157043709107056
+                },
+                {
+                    "x": 3.844012025527027,
+                    "y": 4.082480131024319,
+                    "heading": 3.0498762879543886,
+                    "angularVelocity": 2.824277225814376e-7,
+                    "velocityX": 3.7218903017266984,
+                    "velocityY": -0.6768835748279012,
+                    "timestamp": 0.6016550993958233
+                },
+                {
+                    "x": 4.163911208302122,
+                    "y": 4.0243014946682,
+                    "heading": 3.049876312229257,
+                    "angularVelocity": 2.824277213255298e-7,
+                    "velocityX": 3.721890301726596,
+                    "velocityY": -0.6768835748284626,
+                    "timestamp": 0.6876058278809409
+                },
+                {
+                    "x": 4.483810391077216,
+                    "y": 3.9661228583120804,
+                    "heading": 3.0498763365041253,
+                    "angularVelocity": 2.8242772165714766e-7,
+                    "velocityX": 3.7218903017265963,
+                    "velocityY": -0.6768835748284627,
+                    "timestamp": 0.7735565563660586
+                },
+                {
+                    "x": 4.80370957385231,
+                    "y": 3.9079442219559613,
+                    "heading": 3.0498763607789936,
+                    "angularVelocity": 2.8242772140625194e-7,
+                    "velocityX": 3.7218903017265963,
+                    "velocityY": -0.6768835748284627,
+                    "timestamp": 0.8595072848511762
+                },
+                {
+                    "x": 5.123608756627404,
+                    "y": 3.8497655855998416,
+                    "heading": 3.049876385053862,
+                    "angularVelocity": 2.824277226048516e-7,
+                    "velocityX": 3.7218903017265963,
+                    "velocityY": -0.6768835748284628,
+                    "timestamp": 0.9454580133362939
+                },
+                {
+                    "x": 5.443507939402499,
+                    "y": 3.7915869492437224,
+                    "heading": 3.0498764093287303,
+                    "angularVelocity": 2.8242772163119933e-7,
+                    "velocityX": 3.7218903017265963,
+                    "velocityY": -0.6768835748284626,
+                    "timestamp": 1.0314087418214115
+                },
+                {
+                    "x": 5.763407122177593,
+                    "y": 3.733408312887603,
+                    "heading": 3.0498764336035986,
+                    "angularVelocity": 2.824277219712686e-7,
+                    "velocityX": 3.7218903017265963,
+                    "velocityY": -0.6768835748284628,
+                    "timestamp": 1.117359470306529
+                },
+                {
+                    "x": 6.083306304952576,
+                    "y": 3.675229676530871,
+                    "heading": 3.049876457878467,
+                    "angularVelocity": 2.8242772161077964e-7,
+                    "velocityX": 3.7218903017253,
+                    "velocityY": -0.6768835748355908,
+                    "timestamp": 1.2033101987916466
+                },
+                {
+                    "x": 6.403205465262658,
+                    "y": 3.6170509166494913,
+                    "heading": 3.0498764821533357,
+                    "angularVelocity": 2.8242772554820356e-7,
+                    "velocityX": 3.721890040355771,
+                    "velocityY": -0.6768850119920853,
+                    "timestamp": 1.2892609272767641
+                },
+                {
+                    "x": 6.718573965768454,
+                    "y": 3.5379121175247272,
+                    "heading": 3.0498765068050417,
+                    "angularVelocity": 2.868120651983924e-7,
+                    "velocityX": 3.6691777494404794,
+                    "velocityY": -0.9207461125646453,
+                    "timestamp": 1.3752116557618816
+                },
+                {
+                    "x": 7.013358117396929,
+                    "y": 3.4007168540694233,
+                    "heading": 3.0498765454511387,
+                    "angularVelocity": 4.49630829762963e-7,
+                    "velocityX": 3.4296876457482686,
+                    "velocityY": -1.5962082680784608,
+                    "timestamp": 1.4611623842469992
+                },
+                {
+                    "x": 7.268807462845475,
+                    "y": 3.2166766255285517,
+                    "heading": 3.0757384072562206,
+                    "angularVelocity": 0.30089171157615124,
+                    "velocityX": 2.9720439832312224,
+                    "velocityY": -2.1412294204433464,
+                    "timestamp": 1.5471131127321167
+                },
+                {
+                    "x": 7.46355872560419,
+                    "y": 3.0231641785099956,
+                    "heading": 3.117732513124605,
+                    "angularVelocity": 0.48858347809886377,
+                    "velocityX": 2.2658477268453225,
+                    "velocityY": -2.2514346350428145,
+                    "timestamp": 1.6330638412172342
+                },
+                {
+                    "x": 7.596796563191941,
+                    "y": 2.830012972060279,
+                    "heading": 3.1482585374873016,
+                    "angularVelocity": 0.3551572499816586,
+                    "velocityX": 1.5501653090794745,
+                    "velocityY": -2.2472317553790035,
+                    "timestamp": 1.7190145697023518
+                },
+                {
+                    "x": 7.66884686477411,
+                    "y": 2.640707793733853,
+                    "heading": 3.1573927530357144,
+                    "angularVelocity": 0.10627269494275392,
+                    "velocityX": 0.8382744725035164,
+                    "velocityY": -2.202484861535554,
+                    "timestamp": 1.8049652981874693
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": -0.2023572498136116,
+                    "velocityX": 0.12976196272503107,
+                    "velocityY": -2.1373616835098828,
+                    "timestamp": 1.8909160266725868
+                },
+                {
+                    "x": 7.6422705837740335,
+                    "y": 2.299493905398916,
+                    "heading": 3.1013004562411446,
+                    "angularVelocity": -0.5083194372878264,
+                    "velocityX": -0.49557678882949796,
+                    "velocityY": -2.068846337723125,
+                    "timestamp": 1.9670483578801472
+                },
+                {
+                    "x": 7.556907286255345,
+                    "y": 2.147926635640613,
+                    "heading": 3.0424682290459026,
+                    "angularVelocity": -0.7727627180470553,
+                    "velocityX": -1.121248964332173,
+                    "velocityY": -1.9908397306931678,
+                    "timestamp": 2.043180689087708
+                },
+                {
+                    "x": 7.423923039902345,
+                    "y": 2.003393138237165,
+                    "heading": 2.9682494555138628,
+                    "angularVelocity": -0.974865373945302,
+                    "velocityX": -1.7467512716828084,
+                    "velocityY": -1.8984509617786702,
+                    "timestamp": 2.119313020295268
+                },
+                {
+                    "x": 7.2434803504733996,
+                    "y": 1.8677477086842909,
+                    "heading": 2.8865548866835007,
+                    "angularVelocity": -1.07306012484416,
+                    "velocityX": -2.3701190619920354,
+                    "velocityY": -1.7817059769660042,
+                    "timestamp": 2.1954453515028285
+                },
+                {
+                    "x": 7.01645692191088,
+                    "y": 1.7447501516568382,
+                    "heading": 2.813573416858664,
+                    "angularVelocity": -0.9586133600147891,
+                    "velocityX": -2.9819581899254453,
+                    "velocityY": -1.6155758673946832,
+                    "timestamp": 2.271577682710389
+                },
+                {
+                    "x": 6.750046151807222,
+                    "y": 1.6436496548008426,
+                    "heading": 2.8060686181878967,
+                    "angularVelocity": -0.09857571089344144,
+                    "velocityX": -3.499311867613085,
+                    "velocityY": -1.327957455819293,
+                    "timestamp": 2.347710013917949
+                },
+                {
+                    "x": 6.467461314020777,
+                    "y": 1.5880425214231262,
+                    "heading": 2.8060685831707377,
+                    "angularVelocity": -4.5995122509551036e-7,
+                    "velocityX": -3.711758635316633,
+                    "velocityY": -0.7304010332495747,
+                    "timestamp": 2.4238423451255096
+                },
+                {
+                    "x": 6.17955680980973,
+                    "y": 1.580470049320341,
+                    "heading": 2.806068554276105,
+                    "angularVelocity": -3.7953169924465946e-7,
+                    "velocityX": -3.7816325816443177,
+                    "velocityY": -0.09946460304917487,
+                    "timestamp": 2.49997467633307
+                },
+                {
+                    "x": 5.8949085342429335,
+                    "y": 1.5924545384965154,
+                    "heading": 2.7975026231592013,
+                    "angularVelocity": -0.11251371107452125,
+                    "velocityX": -3.738861940149357,
+                    "velocityY": 0.1574165533364508,
+                    "timestamp": 2.5761070075406303
+                },
+                {
+                    "x": 5.65482422655778,
+                    "y": 1.6016450496652885,
+                    "heading": 2.72071509082511,
+                    "angularVelocity": -1.008606082543986,
+                    "velocityX": -3.153513151076552,
+                    "velocityY": 0.12071758506519875,
+                    "timestamp": 2.6522393387481906
+                },
+                {
+                    "x": 5.463026554407694,
+                    "y": 1.608988353775347,
+                    "heading": 2.6410025465355518,
+                    "angularVelocity": -1.0470261848719398,
+                    "velocityX": -2.5192670329138944,
+                    "velocityY": 0.09645447595750602,
+                    "timestamp": 2.728371669955751
+                },
+                {
+                    "x": 5.319399038502622,
+                    "y": 1.614493352033255,
+                    "heading": 2.573619865722977,
+                    "angularVelocity": -0.8850731317940332,
+                    "velocityX": -1.8865508730252805,
+                    "velocityY": 0.07230828441193854,
+                    "timestamp": 2.8045040011633113
+                },
+                {
+                    "x": 5.223767247762021,
+                    "y": 1.6181637826708264,
+                    "heading": 2.525354405173392,
+                    "angularVelocity": -0.6339679842201956,
+                    "velocityX": -1.2561258695714954,
+                    "velocityY": 0.04821119463109347,
+                    "timestamp": 2.8806363323708717
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": -0.33303072126212585,
+                    "velocityX": -0.6274239472819125,
+                    "velocityY": 0.02411875874607297,
+                    "timestamp": 2.956768663578432
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": 0,
+                    "velocityX": -1.0690007247060788e-32,
+                    "velocityY": 0,
+                    "timestamp": 3.0329009947859924
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W3 To C5 To S3": {
+            "waypoints": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 24
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 19
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.35465792702043,
+                    "y": 4.32401597789768,
+                    "heading": 2.7661167341638526,
+                    "angularVelocity": 0.1817092285961366,
+                    "velocityX": 0.6162445601306747,
+                    "velocityY": -0.4057043338633825,
+                    "timestamp": 0.08869518784691614
+                },
+                {
+                    "x": 2.464008141320373,
+                    "y": 4.252030892508781,
+                    "heading": 2.796228115996352,
+                    "angularVelocity": 0.3394928469453156,
+                    "velocityX": 1.2328765173672827,
+                    "velocityY": -0.8116007997315744,
+                    "timestamp": 0.1773903756938323
+                },
+                {
+                    "x": 2.6280987448252096,
+                    "y": 4.144022899680214,
+                    "heading": 2.836753181719006,
+                    "angularVelocity": 0.4569026427070467,
+                    "velocityX": 1.8500508030723093,
+                    "velocityY": -1.2177435490072333,
+                    "timestamp": 0.2660855635407484
+                },
+                {
+                    "x": 2.8469891593833587,
+                    "y": 3.9999743075055525,
+                    "heading": 2.8803143946879453,
+                    "angularVelocity": 0.49113389380407046,
+                    "velocityX": 2.467895044497164,
+                    "velocityY": -1.624085766899582,
+                    "timestamp": 0.3547807513876646
+                },
+                {
+                    "x": 3.12048957146799,
+                    "y": 3.8200884895179508,
+                    "heading": 2.9023929043889884,
+                    "angularVelocity": 0.248925677221065,
+                    "velocityX": 3.083599220249474,
+                    "velocityY": -2.028135035894805,
+                    "timestamp": 0.44347593923458073
+                },
+                {
+                    "x": 3.4006775749010125,
+                    "y": 3.6354966761760505,
+                    "heading": 2.9023929178552135,
+                    "angularVelocity": 1.5182588146629277e-7,
+                    "velocityX": 3.1589989291934804,
+                    "velocityY": -2.081193104416185,
+                    "timestamp": 0.5321711270814968
+                },
+                {
+                    "x": 3.680865577691327,
+                    "y": 3.450904861858589,
+                    "heading": 2.9023929313214296,
+                    "angularVelocity": 1.518257825170986e-7,
+                    "velocityX": 3.1589989219472225,
+                    "velocityY": -2.081193115415224,
+                    "timestamp": 0.6208663149284129
+                },
+                {
+                    "x": 3.961053580481639,
+                    "y": 3.2663130475411224,
+                    "heading": 2.902392944787646,
+                    "angularVelocity": 1.5182578291151429e-7,
+                    "velocityX": 3.1589989219471897,
+                    "velocityY": -2.081193115415273,
+                    "timestamp": 0.709561502775329
+                },
+                {
+                    "x": 4.24124158327195,
+                    "y": 3.0817212332236563,
+                    "heading": 2.9023929582538623,
+                    "angularVelocity": 1.518257833819606e-7,
+                    "velocityX": 3.1589989219471897,
+                    "velocityY": -2.0811931154152736,
+                    "timestamp": 0.7982566906222451
+                },
+                {
+                    "x": 4.521429586062262,
+                    "y": 2.89712941890619,
+                    "heading": 2.9023929717200785,
+                    "angularVelocity": 1.5182578356012397e-7,
+                    "velocityX": 3.1589989219471892,
+                    "velocityY": -2.081193115415273,
+                    "timestamp": 0.8869518784691612
+                },
+                {
+                    "x": 4.801617588852573,
+                    "y": 2.7125376045887237,
+                    "heading": 2.902392985186295,
+                    "angularVelocity": 1.518257838228889e-7,
+                    "velocityX": 3.15899892194719,
+                    "velocityY": -2.0811931154152736,
+                    "timestamp": 0.9756470663160773
+                },
+                {
+                    "x": 5.081805591642885,
+                    "y": 2.5279457902712577,
+                    "heading": 2.902392998652511,
+                    "angularVelocity": 1.5182578286793036e-7,
+                    "velocityX": 3.1589989219471897,
+                    "velocityY": -2.081193115415273,
+                    "timestamp": 1.0643422541629934
+                },
+                {
+                    "x": 5.361993594433196,
+                    "y": 2.343353975953791,
+                    "heading": 2.9023930121187274,
+                    "angularVelocity": 1.5182578369194972e-7,
+                    "velocityX": 3.1589989219471897,
+                    "velocityY": -2.0811931154152736,
+                    "timestamp": 1.1530374420099097
+                },
+                {
+                    "x": 5.642181597223507,
+                    "y": 2.158762161636325,
+                    "heading": 2.902393025584944,
+                    "angularVelocity": 1.5182578337777967e-7,
+                    "velocityX": 3.1589989219471897,
+                    "velocityY": -2.081193115415273,
+                    "timestamp": 1.2417326298568259
+                },
+                {
+                    "x": 5.922369600013819,
+                    "y": 1.9741703473188588,
+                    "heading": 2.90239303905116,
+                    "angularVelocity": 1.5182578259222556e-7,
+                    "velocityX": 3.15899892194719,
+                    "velocityY": -2.081193115415273,
+                    "timestamp": 1.330427817703742
+                },
+                {
+                    "x": 6.202557602804131,
+                    "y": 1.7895785330013922,
+                    "heading": 2.9023930525173767,
+                    "angularVelocity": 1.5182578252273246e-7,
+                    "velocityX": 3.1589989219471892,
+                    "velocityY": -2.0811931154152736,
+                    "timestamp": 1.4191230055506583
+                },
+                {
+                    "x": 6.482745605594443,
+                    "y": 1.604986718683926,
+                    "heading": 2.902393065983593,
+                    "angularVelocity": 1.5182578277552693e-7,
+                    "velocityX": 3.1589989219471897,
+                    "velocityY": -2.081193115415273,
+                    "timestamp": 1.5078181933975745
+                },
+                {
+                    "x": 6.762933608384558,
+                    "y": 1.4203949043661621,
+                    "heading": 2.902393079449809,
+                    "angularVelocity": 1.5182578233984683e-7,
+                    "velocityX": 3.1589989219449803,
+                    "velocityY": -2.081193115418628,
+                    "timestamp": 1.5965133812444907
+                },
+                {
+                    "x": 7.043121567747365,
+                    "y": 1.2358030241312514,
+                    "heading": 2.902393092916261,
+                    "angularVelocity": 1.5182844252968712e-7,
+                    "velocityX": 3.158998432320794,
+                    "velocityY": -2.08119385860604,
+                    "timestamp": 1.685208569091407
+                },
+                {
+                    "x": 7.279459556862412,
+                    "y": 1.0742756485961233,
+                    "heading": 2.998760727595811,
+                    "angularVelocity": 1.0865035298857004,
+                    "velocityX": 2.6646089247023874,
+                    "velocityY": -1.8211515129086453,
+                    "timestamp": 1.7739037569383231
+                },
+                {
+                    "x": 7.461316655805292,
+                    "y": 0.9486305467963247,
+                    "heading": 3.0706677283735453,
+                    "angularVelocity": 0.8107204294086697,
+                    "velocityX": 2.0503603787023565,
+                    "velocityY": -1.416594348011941,
+                    "timestamp": 1.8625989447852394
+                },
+                {
+                    "x": 7.588692434846267,
+                    "y": 0.8588692195479983,
+                    "heading": 3.1181671121508234,
+                    "angularVelocity": 0.5355350716350028,
+                    "velocityX": 1.4361069876848378,
+                    "velocityY": -1.0120202620603307,
+                    "timestamp": 1.9512941326321556
+                },
+                {
+                    "x": 7.661586847053616,
+                    "y": 0.8049924095541465,
+                    "heading": 3.141279756163227,
+                    "angularVelocity": 0.26058509569081495,
+                    "velocityX": 0.8218530675323843,
+                    "velocityY": -0.6074378024526055,
+                    "timestamp": 2.0399893204790716
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": -0.01442869894402112,
+                    "velocityX": 0.20760036021529943,
+                    "velocityY": -0.20285665988103574,
+                    "timestamp": 2.1286845083259878
+                },
+                {
+                    "x": 7.650032668413058,
+                    "y": 0.8012856501840878,
+                    "heading": 3.117704293378248,
+                    "angularVelocity": -0.2703970639421689,
+                    "velocityX": -0.3634367195783461,
+                    "velocityY": 0.17325299134110608,
+                    "timestamp": 2.211139939966771
+                },
+                {
+                    "x": 7.572958138860234,
+                    "y": 0.846621328160437,
+                    "heading": 3.075672962840878,
+                    "angularVelocity": -0.509746049483795,
+                    "velocityX": -0.9347416903788446,
+                    "velocityY": 0.5498203947782849,
+                    "timestamp": 2.293595371607554
+                },
+                {
+                    "x": 7.448745482136049,
+                    "y": 0.9230647936190245,
+                    "heading": 3.016123238078663,
+                    "angularVelocity": -0.7222049970175785,
+                    "velocityX": -1.5064217632783534,
+                    "velocityY": 0.927088294093387,
+                    "timestamp": 2.376050803248337
+                },
+                {
+                    "x": 7.277351379823213,
+                    "y": 1.0307130669975626,
+                    "heading": 2.9432415686566307,
+                    "angularVelocity": -0.8838916730136303,
+                    "velocityX": -2.0786271917114503,
+                    "velocityY": 1.305532834362055,
+                    "timestamp": 2.45850623488912
+                },
+                {
+                    "x": 7.058753992164297,
+                    "y": 1.1697461046151478,
+                    "heading": 2.8675628117721894,
+                    "angularVelocity": -0.9178140891207157,
+                    "velocityX": -2.6510974875643654,
+                    "velocityY": 1.6861598423652984,
+                    "timestamp": 2.540961666529903
+                },
+                {
+                    "x": 6.796121093379507,
+                    "y": 1.3380331209180303,
+                    "heading": 2.867562732679127,
+                    "angularVelocity": -9.592219802669244e-7,
+                    "velocityX": -3.1851497658632115,
+                    "velocityY": 2.0409451864375003,
+                    "timestamp": 2.623417098170686
+                },
+                {
+                    "x": 6.5354030679592325,
+                    "y": 1.509271805879804,
+                    "heading": 2.8675627155127748,
+                    "angularVelocity": -2.0818946417191608e-7,
+                    "velocityX": -3.161926633967453,
+                    "velocityY": 2.076742326785391,
+                    "timestamp": 2.705872529811469
+                },
+                {
+                    "x": 6.274685053353285,
+                    "y": 1.680510507306839,
+                    "heading": 2.867562698346423,
+                    "angularVelocity": -2.0818946436511562e-7,
+                    "velocityX": -3.161926502813852,
+                    "velocityY": 2.0767425264721937,
+                    "timestamp": 2.7883279614522523
+                },
+                {
+                    "x": 6.013967038747397,
+                    "y": 1.851749208733966,
+                    "heading": 2.8675626811800705,
+                    "angularVelocity": -2.0818946408004615e-7,
+                    "velocityX": -3.1619265028131216,
+                    "velocityY": 2.076742526473307,
+                    "timestamp": 2.8707833930930353
+                },
+                {
+                    "x": 5.75324902414151,
+                    "y": 2.022987910161093,
+                    "heading": 2.8675626640137186,
+                    "angularVelocity": -2.081894642016249e-7,
+                    "velocityX": -3.1619265028131216,
+                    "velocityY": 2.076742526473306,
+                    "timestamp": 2.9532388247338184
+                },
+                {
+                    "x": 5.492531009535623,
+                    "y": 2.194226611588219,
+                    "heading": 2.8675626468473663,
+                    "angularVelocity": -2.0818946425512402e-7,
+                    "velocityX": -3.1619265028131216,
+                    "velocityY": 2.076742526473306,
+                    "timestamp": 3.0356942563746014
+                },
+                {
+                    "x": 5.231812994929702,
+                    "y": 2.3654653130152945,
+                    "heading": 2.8675626296810144,
+                    "angularVelocity": -2.0818946468474427e-7,
+                    "velocityX": -3.1619265028135324,
+                    "velocityY": 2.0767425264726813,
+                    "timestamp": 3.1181496880153845
+                },
+                {
+                    "x": 4.971094974250145,
+                    "y": 2.536704005194925,
+                    "heading": 2.8675626125145612,
+                    "angularVelocity": -2.0819068604958381e-7,
+                    "velocityX": -3.16192657647314,
+                    "velocityY": 2.076742414321854,
+                    "timestamp": 3.2006051196561676
+                },
+                {
+                    "x": 4.734733232154558,
+                    "y": 2.6911196483021613,
+                    "heading": 2.78339324120731,
+                    "angularVelocity": -1.020786255463861,
+                    "velocityX": -2.8665393824544814,
+                    "velocityY": 1.8727164485651855,
+                    "timestamp": 3.2830605512969506
+                },
+                {
+                    "x": 4.545913318554367,
+                    "y": 2.8146834617063132,
+                    "heading": 2.703884624892798,
+                    "angularVelocity": -0.9642617197238292,
+                    "velocityX": -2.289963315246283,
+                    "velocityY": 1.4985527447416418,
+                    "timestamp": 3.3655159829377337
+                },
+                {
+                    "x": 4.404411416841804,
+                    "y": 2.9073475116185867,
+                    "heading": 2.6403705813576916,
+                    "angularVelocity": -0.7702833187728019,
+                    "velocityX": -1.7161016429944431,
+                    "velocityY": 1.1238077112489557,
+                    "timestamp": 3.4479714145785167
+                },
+                {
+                    "x": 4.3101255761487,
+                    "y": 2.9691175811007473,
+                    "heading": 2.5964788856355536,
+                    "angularVelocity": -0.5323081190497229,
+                    "velocityX": -1.1434764067922174,
+                    "velocityY": 0.7491328133634895,
+                    "timestamp": 3.5304268462193
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": -0.27261861575697227,
+                    "velocityX": -0.5715278570610401,
+                    "velocityY": 0.37453468236989096,
+                    "timestamp": 3.612882277860083
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": 0,
+                    "velocityX": -3.9375732354803004e-35,
+                    "velocityY": 0,
+                    "timestamp": 3.695337709500866
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W3 To C5 To S4": {
+            "waypoints": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 24
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.3565686386495837,
+                    "y": 4.327692083019332,
+                    "heading": 2.7686204392526563,
+                    "angularVelocity": 0.21032675786146612,
+                    "velocityX": 0.6389698009998809,
+                    "velocityY": -0.36493335842385927,
+                    "timestamp": 0.08853100500377892
+                },
+                {
+                    "x": 2.4697534077972008,
+                    "y": 4.26305693150059,
+                    "heading": 2.803412029343583,
+                    "angularVelocity": 0.39298763285756855,
+                    "velocityX": 1.2784760451187218,
+                    "velocityY": -0.7300849178881736,
+                    "timestamp": 0.17706201000755784
+                },
+                {
+                    "x": 2.639620733022083,
+                    "y": 4.166070131606458,
+                    "heading": 2.850239105466856,
+                    "angularVelocity": 0.5289341979263338,
+                    "velocityX": 1.9187325978919179,
+                    "velocityY": -1.095512243309488,
+                    "timestamp": 0.26559301501133675
+                },
+                {
+                    "x": 2.8662525015467275,
+                    "y": 4.036714523350174,
+                    "heading": 2.9005551954880358,
+                    "angularVelocity": 0.5683442768897115,
+                    "velocityX": 2.559914105967419,
+                    "velocityY": -1.4611333989799566,
+                    "timestamp": 0.35412402001511567
+                },
+                {
+                    "x": 3.1493645211146295,
+                    "y": 3.8752655198055206,
+                    "heading": 2.9253298956108997,
+                    "angularVelocity": 0.2798420747827705,
+                    "velocityX": 3.197885526723865,
+                    "velocityY": -1.8236436323948029,
+                    "timestamp": 0.4426550250188946
+                },
+                {
+                    "x": 3.4401071732873256,
+                    "y": 3.709037480706115,
+                    "heading": 2.9253299112862043,
+                    "angularVelocity": 1.770600526020634e-7,
+                    "velocityX": 3.28407716777061,
+                    "velocityY": -1.877625122320825,
+                    "timestamp": 0.5311860300226735
+                },
+                {
+                    "x": 3.730849824609712,
+                    "y": 3.5428094401194565,
+                    "heading": 2.925329926961498,
+                    "angularVelocity": 1.7705993300653618e-7,
+                    "velocityX": 3.2840771581659602,
+                    "velocityY": -1.87762513912005,
+                    "timestamp": 0.6197170350264525
+                },
+                {
+                    "x": 4.021592475932095,
+                    "y": 3.376581399532791,
+                    "heading": 2.9253299426367922,
+                    "angularVelocity": 1.7705993370243028e-7,
+                    "velocityX": 3.2840771581659167,
+                    "velocityY": -1.8776251391201264,
+                    "timestamp": 0.7082480400302313
+                },
+                {
+                    "x": 4.312335127254477,
+                    "y": 3.2103533589461257,
+                    "heading": 2.925329958312086,
+                    "angularVelocity": 1.7705993363835285e-7,
+                    "velocityX": 3.2840771581659163,
+                    "velocityY": -1.8776251391201266,
+                    "timestamp": 0.7967790450340102
+                },
+                {
+                    "x": 4.60307777857686,
+                    "y": 3.0441253183594608,
+                    "heading": 2.9253299739873797,
+                    "angularVelocity": 1.7705993334445259e-7,
+                    "velocityX": 3.2840771581659163,
+                    "velocityY": -1.8776251391201269,
+                    "timestamp": 0.8853100500377891
+                },
+                {
+                    "x": 4.893820429899242,
+                    "y": 2.877897277772796,
+                    "heading": 2.9253299896626737,
+                    "angularVelocity": 1.7705993259163212e-7,
+                    "velocityX": 3.2840771581659163,
+                    "velocityY": -1.877625139120127,
+                    "timestamp": 0.9738410550415679
+                },
+                {
+                    "x": 5.184563081221626,
+                    "y": 2.7116692371861304,
+                    "heading": 2.925330005337967,
+                    "angularVelocity": 1.7705993390699708e-7,
+                    "velocityX": 3.2840771581659163,
+                    "velocityY": -1.8776251391201266,
+                    "timestamp": 1.0623720600453468
+                },
+                {
+                    "x": 5.475305732544008,
+                    "y": 2.5454411965994654,
+                    "heading": 2.925330021013261,
+                    "angularVelocity": 1.770599334284566e-7,
+                    "velocityX": 3.2840771581659163,
+                    "velocityY": -1.8776251391201269,
+                    "timestamp": 1.1509030650491257
+                },
+                {
+                    "x": 5.766048383866392,
+                    "y": 2.3792131560128005,
+                    "heading": 2.925330036688555,
+                    "angularVelocity": 1.7705993332802181e-7,
+                    "velocityX": 3.2840771581659163,
+                    "velocityY": -1.8776251391201264,
+                    "timestamp": 1.2394340700529045
+                },
+                {
+                    "x": 6.056791035188774,
+                    "y": 2.212985115426135,
+                    "heading": 2.9253300523638495,
+                    "angularVelocity": 1.7705993348595005e-7,
+                    "velocityX": 3.2840771581659163,
+                    "velocityY": -1.8776251391201266,
+                    "timestamp": 1.3279650750566834
+                },
+                {
+                    "x": 6.347533686511157,
+                    "y": 2.04675707483947,
+                    "heading": 2.925330068039143,
+                    "angularVelocity": 1.7705993292383098e-7,
+                    "velocityX": 3.2840771581659163,
+                    "velocityY": -1.8776251391201269,
+                    "timestamp": 1.4164960800604622
+                },
+                {
+                    "x": 6.638276337833025,
+                    "y": 1.8805290342519057,
+                    "heading": 2.925330083714437,
+                    "angularVelocity": 1.7705993325721192e-7,
+                    "velocityX": 3.284077158160109,
+                    "velocityY": -1.877625139130284,
+                    "timestamp": 1.505027085064241
+                },
+                {
+                    "x": 6.929018876242265,
+                    "y": 1.7143007961736503,
+                    "heading": 2.9253300993897313,
+                    "angularVelocity": 1.770599420488751e-7,
+                    "velocityX": 3.2840758827580245,
+                    "velocityY": -1.8776273698819952,
+                    "timestamp": 1.59355809006802
+                },
+                {
+                    "x": 7.201522264704821,
+                    "y": 1.5196088586386673,
+                    "heading": 2.925330126495543,
+                    "angularVelocity": 3.061730945539565e-7,
+                    "velocityX": 3.078055969780553,
+                    "velocityY": -2.1991384546767203,
+                    "timestamp": 1.6820890950717988
+                },
+                {
+                    "x": 7.413524241952794,
+                    "y": 1.3146281632272387,
+                    "heading": 3.008225358482996,
+                    "angularVelocity": 0.9363412511121879,
+                    "velocityX": 2.394663623652733,
+                    "velocityY": -2.3153548906699357,
+                    "timestamp": 1.7706201000755777
+                },
+                {
+                    "x": 7.566471934487395,
+                    "y": 1.1372904255598042,
+                    "heading": 3.0754099127352705,
+                    "angularVelocity": 0.758881639821055,
+                    "velocityX": 1.727617262766506,
+                    "velocityY": -2.003114475655854,
+                    "timestamp": 1.8591511050793565
+                },
+                {
+                    "x": 7.66159309798317,
+                    "y": 0.9897915834005926,
+                    "heading": 3.120981260630991,
+                    "angularVelocity": 0.5147501476321013,
+                    "velocityX": 1.0744389888232937,
+                    "velocityY": -1.6660698944162209,
+                    "timestamp": 1.9476821100831354
+                },
+                {
+                    "x": 7.6993540657193265,
+                    "y": 0.8728983057701601,
+                    "heading": 3.1428690494072535,
+                    "angularVelocity": 0.2472330318097442,
+                    "velocityX": 0.4265281720741691,
+                    "velocityY": -1.3203654202890747,
+                    "timestamp": 2.0362131150869143
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": -0.03240728383410146,
+                    "velocityX": -0.21861341931571387,
+                    "velocityY": -0.9702624043010712,
+                    "timestamp": 2.124744120090693
+                },
+                {
+                    "x": 7.618086573966619,
+                    "y": 0.7353968639365192,
+                    "heading": 3.1174668267835224,
+                    "angularVelocity": -0.28713837638984974,
+                    "velocityX": -0.7889577050333905,
+                    "velocityY": -0.6575745263915584,
+                    "timestamp": 2.203219082231727
+                },
+                {
+                    "x": 7.5114563039412285,
+                    "y": 0.7085217505748593,
+                    "heading": 3.0768465482235205,
+                    "angularVelocity": -0.5176208748841893,
+                    "velocityX": -1.3587807769024405,
+                    "velocityY": -0.34246736320004995,
+                    "timestamp": 2.281694044372761
+                },
+                {
+                    "x": 7.360182308394819,
+                    "y": 0.7066609452418267,
+                    "heading": 3.0210994010584384,
+                    "angularVelocity": -0.7103813196472814,
+                    "velocityX": -1.9276721060984434,
+                    "velocityY": -0.02371208959223001,
+                    "timestamp": 2.360169006513795
+                },
+                {
+                    "x": 7.164421971622539,
+                    "y": 0.7302939539488027,
+                    "heading": 2.9554195577269637,
+                    "angularVelocity": -0.8369528514504796,
+                    "velocityX": -2.494557900142165,
+                    "velocityY": 0.3011534897526061,
+                    "timestamp": 2.4386439686548287
+                },
+                {
+                    "x": 6.9246535759772865,
+                    "y": 0.7803818201714893,
+                    "heading": 2.891069702848338,
+                    "angularVelocity": -0.8200049177840428,
+                    "velocityX": -3.055348981428448,
+                    "velocityY": 0.638265567209492,
+                    "timestamp": 2.5171189307958626
+                },
+                {
+                    "x": 6.644255242545633,
+                    "y": 0.8592678201395815,
+                    "heading": 2.8720123676443365,
+                    "angularVelocity": -0.24284605795368294,
+                    "velocityX": -3.573092943042356,
+                    "velocityY": 1.0052378212854063,
+                    "timestamp": 2.5955938929368965
+                },
+                {
+                    "x": 6.376569654247088,
+                    "y": 0.9876185603343286,
+                    "heading": 2.872012347067197,
+                    "angularVelocity": -2.622127987829542e-7,
+                    "velocityX": -3.411095475490091,
+                    "velocityY": 1.635562945084242,
+                    "timestamp": 2.6740688550779304
+                },
+                {
+                    "x": 6.113906618540549,
+                    "y": 1.125957335049975,
+                    "heading": 2.872012327384056,
+                    "angularVelocity": -2.5082064696072804e-7,
+                    "velocityX": -3.347093500147104,
+                    "velocityY": 1.762839648995542,
+                    "timestamp": 2.7525438172189642
+                },
+                {
+                    "x": 5.851058985303004,
+                    "y": 1.2639450359763313,
+                    "heading": 2.8720122952205234,
+                    "angularVelocity": -4.098572586075927e-7,
+                    "velocityX": -3.3494458113298133,
+                    "velocityY": 1.758365944520708,
+                    "timestamp": 2.831018779359998
+                },
+                {
+                    "x": 5.625853496223335,
+                    "y": 1.3827528651043908,
+                    "heading": 2.754606631455948,
+                    "angularVelocity": -1.4960907346931063,
+                    "velocityX": -2.8697750586351334,
+                    "velocityY": 1.5139584128060528,
+                    "timestamp": 2.909493741501032
+                },
+                {
+                    "x": 5.445842564985353,
+                    "y": 1.4776937741923677,
+                    "heading": 2.6549514330576893,
+                    "angularVelocity": -1.2698980117917973,
+                    "velocityX": -2.2938645183985233,
+                    "velocityY": 1.2098242101391037,
+                    "timestamp": 2.987968703642066
+                },
+                {
+                    "x": 5.310896371700566,
+                    "y": 1.5488618130458742,
+                    "heading": 2.5782320494228896,
+                    "angularVelocity": -0.9776288072239107,
+                    "velocityX": -1.7196082623431492,
+                    "velocityY": 0.9068884764238665,
+                    "timestamp": 3.0664436657831
+                },
+                {
+                    "x": 5.220959019114767,
+                    "y": 1.5962913343665528,
+                    "heading": 2.5262728715572207,
+                    "angularVelocity": -0.6621115378476069,
+                    "velocityX": -1.146064300409159,
+                    "velocityY": 0.604390496365404,
+                    "timestamp": 3.1449186279241337
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": -0.33479304532875703,
+                    "velocityX": -0.5729090895764638,
+                    "velocityY": 0.30211757975540204,
+                    "timestamp": 3.2233935900651676
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": 0,
+                    "velocityX": 1.304014234910508e-32,
+                    "velocityY": 0,
+                    "timestamp": 3.3018685522062015
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S1 To W1": {
+            "waypoints": [
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "angularVelocity": 1.76189972977325e-30,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.102979050763036,
+                    "y": 6.633224483540667,
+                    "heading": -2.873498740443943,
+                    "angularVelocity": 0.06942713881135329,
+                    "velocityX": -0.779792828619582,
+                    "velocityY": 0.034434352980739984,
+                    "timestamp": 0.09364147316695418
+                },
+                {
+                    "x": 4.956937154228187,
+                    "y": 6.639673450002842,
+                    "heading": -2.8604964293774233,
+                    "angularVelocity": 0.138852055897772,
+                    "velocityX": -1.559585636531679,
+                    "velocityY": 0.06886869934946069,
+                    "timestamp": 0.18728294633390835
+                },
+                {
+                    "x": 4.73787431448253,
+                    "y": 6.649346898579915,
+                    "heading": -2.840993426039055,
+                    "angularVelocity": 0.20827313666458666,
+                    "velocityX": -2.339378400797736,
+                    "velocityY": 0.10330303710435738,
+                    "timestamp": 0.28092441950086255
+                },
+                {
+                    "x": 4.4457905464926135,
+                    "y": 6.6622448281102296,
+                    "heading": -2.814990271784832,
+                    "angularVelocity": 0.27768843627487766,
+                    "velocityX": -3.1191710052355526,
+                    "velocityY": 0.13773736245389714,
+                    "timestamp": 0.3745658926678167
+                },
+                {
+                    "x": 4.091895298236837,
+                    "y": 6.6778724148973385,
+                    "heading": -2.8149902687925503,
+                    "angularVelocity": 3.195462281879478e-8,
+                    "velocityX": -3.779257590542345,
+                    "velocityY": 0.16688745123911564,
+                    "timestamp": 0.46820736583477085
+                },
+                {
+                    "x": 3.7380000499851023,
+                    "y": 6.693500001778431,
+                    "heading": -2.814990265800417,
+                    "angularVelocity": 3.1953066955491674e-8,
+                    "velocityX": -3.779257590499122,
+                    "velocityY": 0.16688745224276955,
+                    "timestamp": 0.561848839001725
+                },
+                {
+                    "x": 3.3841048017333675,
+                    "y": 6.709127588659523,
+                    "heading": -2.8149902628082843,
+                    "angularVelocity": 3.1953067022179835e-8,
+                    "velocityX": -3.779257590499122,
+                    "velocityY": 0.1668874522427702,
+                    "timestamp": 0.6554903121686791
+                },
+                {
+                    "x": 3.030209553477566,
+                    "y": 6.724755175446062,
+                    "heading": -2.8149902598160015,
+                    "angularVelocity": 3.195463264613959e-8,
+                    "velocityX": -3.7792575905426142,
+                    "velocityY": 0.16688745123301843,
+                    "timestamp": 0.7491317853356333
+                },
+                {
+                    "x": 2.7381257501719745,
+                    "y": 6.737653104232043,
+                    "heading": -2.7889939719160233,
+                    "angularVelocity": 0.27761511027933256,
+                    "velocityX": -3.1191713823726643,
+                    "velocityY": 0.13773735450514524,
+                    "timestamp": 0.8427732585025874
+                },
+                {
+                    "x": 2.5190628796730365,
+                    "y": 6.747326551843937,
+                    "heading": -2.769496951872836,
+                    "angularVelocity": 0.2082092408823168,
+                    "velocityX": -2.3393787292129176,
+                    "velocityY": 0.10330302679718467,
+                    "timestamp": 0.9364147316695416
+                },
+                {
+                    "x": 2.373020960810499,
+                    "y": 6.753775517241458,
+                    "heading": -2.7564989943923397,
+                    "angularVelocity": 0.13880556382672637,
+                    "velocityX": -1.5595858749697047,
+                    "velocityY": 0.06886868797998212,
+                    "timestamp": 1.0300562048364958
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "angularVelocity": 0.06940294906251296,
+                    "velocityX": -0.779792952213697,
+                    "velocityY": 0.03443434462839962,
+                    "timestamp": 1.12369767800345
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "angularVelocity": 1.276683149554956e-30,
+                    "velocityX": 2.9209786111644144e-32,
+                    "velocityY": 0,
+                    "timestamp": 1.2173391511704041
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S1 To W2": {
+            "waypoints": [
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": -1.6791892318677926e-35,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.11892498821706,
+                    "y": 6.609128402762145,
+                    "heading": -2.8887795529976006,
+                    "angularVelocity": -0.10279668380897901,
+                    "velocityX": -0.6682711456093472,
+                    "velocityY": -0.24437815711512118,
+                    "timestamp": 0.08540696715387537
+                },
+                {
+                    "x": 5.004774958946586,
+                    "y": 6.5673852200358445,
+                    "heading": -2.90633824182768,
+                    "angularVelocity": -0.2055884831789443,
+                    "velocityX": -1.3365423580117657,
+                    "velocityY": -0.48875617666053534,
+                    "timestamp": 0.17081393430775074
+                },
+                {
+                    "x": 4.833549902172449,
+                    "y": 6.504770468021476,
+                    "heading": -2.93267521126463,
+                    "angularVelocity": -0.30837026901446724,
+                    "velocityX": -2.004813687689499,
+                    "velocityY": -0.7331340065214745,
+                    "timestamp": 0.2562209014616261
+                },
+                {
+                    "x": 4.605249804128149,
+                    "y": 6.4212841662523426,
+                    "heading": -2.9677889698411977,
+                    "angularVelocity": -0.4111345917869177,
+                    "velocityX": -2.6730851785543184,
+                    "velocityY": -0.9775116076738672,
+                    "timestamp": 0.3416278686155015
+                },
+                {
+                    "x": 4.319874653666952,
+                    "y": 6.316926338018247,
+                    "heading": -3.0116770321088655,
+                    "angularVelocity": -0.5138698133209222,
+                    "velocityX": -3.341356799932311,
+                    "velocityY": -1.2218889361341865,
+                    "timestamp": 0.42703483576937684
+                },
+                {
+                    "x": 4.016437784224315,
+                    "y": 6.205962942906548,
+                    "heading": -3.011677037359934,
+                    "angularVelocity": -6.148290543535782e-8,
+                    "velocityX": -3.5528350854087023,
+                    "velocityY": -1.2992311846383482,
+                    "timestamp": 0.5124418029232523
+                },
+                {
+                    "x": 3.713000914796963,
+                    "y": 6.094999547753008,
+                    "heading": -3.0116770426109816,
+                    "angularVelocity": -6.148266047339023e-8,
+                    "velocityX": -3.55283508522973,
+                    "velocityY": -1.299231185128267,
+                    "timestamp": 0.5978487700771277
+                },
+                {
+                    "x": 3.4095640453696117,
+                    "y": 5.984036152599468,
+                    "heading": -3.011677047862029,
+                    "angularVelocity": -6.148266099885158e-8,
+                    "velocityX": -3.5528350852297295,
+                    "velocityY": -1.2992311851282667,
+                    "timestamp": 0.6832557372310031
+                },
+                {
+                    "x": 3.1061271759243305,
+                    "y": 5.873072757495,
+                    "heading": -3.011677053113097,
+                    "angularVelocity": -6.1482904284003e-8,
+                    "velocityX": -3.5528350854396615,
+                    "velocityY": -1.2992311845536897,
+                    "timestamp": 0.7686627043848785
+                },
+                {
+                    "x": 2.820751491800317,
+                    "y": 5.768715178575412,
+                    "heading": -3.055515411809828,
+                    "angularVelocity": -0.513287851771491,
+                    "velocityX": -3.341363048401663,
+                    "velocityY": -1.2218860169986983,
+                    "timestamp": 0.8540696715387539
+                },
+                {
+                    "x": 2.592450909011727,
+                    "y": 5.685229109517301,
+                    "heading": -3.0905842313047507,
+                    "angularVelocity": -0.41060841595908354,
+                    "velocityX": -2.673090854253924,
+                    "velocityY": -0.977508882942736,
+                    "timestamp": 0.9394766386926293
+                },
+                {
+                    "x": 2.421225457633144,
+                    "y": 5.622614555370758,
+                    "heading": -3.116884896329009,
+                    "angularVelocity": -0.30794519347436133,
+                    "velocityX": -2.00481830797353,
+                    "velocityY": -0.7331316897570141,
+                    "timestamp": 1.0248836058465047
+                },
+                {
+                    "x": 2.3070751524559587,
+                    "y": 5.5808715184580695,
+                    "heading": -3.134418439279544,
+                    "angularVelocity": -0.2052940589606165,
+                    "velocityX": -1.336545588505958,
+                    "velocityY": -0.4887544693804859,
+                    "timestamp": 1.11029057300038
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -0.1026481584839197,
+                    "velocityX": -0.668272792700015,
+                    "velocityY": -0.24437723471044392,
+                    "timestamp": 1.1956975401542553
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -2.233003674732857e-32,
+                    "velocityX": 0,
+                    "velocityY": 6.615380928603251e-34,
+                    "timestamp": 1.2811045073081306
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S1 To C1 To S1": {
+            "waypoints": [
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": -3.143185307179586,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "angularVelocity": 2.29574628206475e-30,
+                    "velocityX": 4.692945968016159e-32,
+                    "velocityY": -1.566641075737276e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.235048481309196,
+                    "y": 6.649731386951801,
+                    "heading": -2.8906451335254117,
+                    "angularVelocity": -0.12312313043682141,
+                    "velocityX": 0.6829631445217772,
+                    "velocityY": 0.2282160316336707,
+                    "timestamp": 0.08645925008229886
+                },
+                {
+                    "x": 5.3531499211746905,
+                    "y": 6.689193367474774,
+                    "heading": -2.911632072054118,
+                    "angularVelocity": -0.24273791998680166,
+                    "velocityX": 1.3659780735212093,
+                    "velocityY": 0.45642288691397265,
+                    "timestamp": 0.17291850016459773
+                },
+                {
+                    "x": 5.530311551906127,
+                    "y": 6.748384549002882,
+                    "heading": -2.9424540818551694,
+                    "angularVelocity": -0.3564917550356453,
+                    "velocityX": 2.0490766524439916,
+                    "velocityY": 0.6846136355771774,
+                    "timestamp": 0.2593777502468966
+                },
+                {
+                    "x": 5.7665469637988895,
+                    "y": 6.827301853965155,
+                    "heading": -2.9820943384526712,
+                    "angularVelocity": -0.4584848533802071,
+                    "velocityX": 2.732332418658699,
+                    "velocityY": 0.9127687886162316,
+                    "timestamp": 0.34583700032919545
+                },
+                {
+                    "x": 6.061889894006546,
+                    "y": 6.925933356393463,
+                    "heading": -3.027468756644365,
+                    "angularVelocity": -0.5248069830402681,
+                    "velocityX": 3.415978393596438,
+                    "velocityY": 1.1407860042093994,
+                    "timestamp": 0.43229625041149433
+                },
+                {
+                    "x": 6.3720848481132855,
+                    "y": 7.029634881790706,
+                    "heading": -3.0274687634280455,
+                    "angularVelocity": -7.846101114866984e-8,
+                    "velocityX": 3.587759017241877,
+                    "velocityY": 1.1994266119419068,
+                    "timestamp": 0.5187555004937932
+                },
+                {
+                    "x": 6.682279802039258,
+                    "y": 7.133336407728698,
+                    "heading": -3.027468770211708,
+                    "angularVelocity": -7.846080505503267e-8,
+                    "velocityX": 3.5877590151510974,
+                    "velocityY": 1.1994266181962872,
+                    "timestamp": 0.605214750576092
+                },
+                {
+                    "x": 6.992474757407683,
+                    "y": 7.237037929350706,
+                    "heading": -3.027468776995789,
+                    "angularVelocity": -7.846565119739109e-8,
+                    "velocityX": 3.5877590318346937,
+                    "velocityY": 1.1994265682769871,
+                    "timestamp": 0.6916740006583908
+                },
+                {
+                    "x": 7.248034956342824,
+                    "y": 7.322496159466712,
+                    "heading": -3.07460707499528,
+                    "angularVelocity": -0.5452082681093516,
+                    "velocityX": 2.9558456578310177,
+                    "velocityY": 0.9884220605040753,
+                    "timestamp": 0.7781332507406896
+                },
+                {
+                    "x": 7.444567623191234,
+                    "y": 7.388221479482509,
+                    "heading": -3.109745402875848,
+                    "angularVelocity": -0.406414904676237,
+                    "velocityX": 2.2731248149998473,
+                    "velocityY": 0.7601884119189827,
+                    "timestamp": 0.8645925008229884
+                },
+                {
+                    "x": 7.58207269769946,
+                    "y": 7.434213892330609,
+                    "heading": -3.1328871823753137,
+                    "angularVelocity": -0.2676611175500936,
+                    "velocityX": 1.5904032752686705,
+                    "velocityY": 0.5319547972517508,
+                    "timestamp": 0.9510517509052873
+                },
+                {
+                    "x": 7.660550155243088,
+                    "y": 7.460473398860823,
+                    "heading": -3.1440339918196103,
+                    "angularVelocity": -0.12892558556646783,
+                    "velocityX": 0.907681450727999,
+                    "velocityY": 0.3037211924135578,
+                    "timestamp": 1.037511000987586
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": 0.009816007412950826,
+                    "velocityX": 0.22495967450928966,
+                    "velocityY": 0.07548759829543861,
+                    "timestamp": 1.123970251069885
+                },
+                {
+                    "x": 7.635308945850804,
+                    "y": 7.452085548446669,
+                    "heading": -3.129057152990018,
+                    "angularVelocity": 0.1555765979823413,
+                    "velocityX": -0.49212955006536996,
+                    "velocityY": -0.1642351578439422,
+                    "timestamp": 1.2147818151045664
+                },
+                {
+                    "x": 7.525497368411343,
+                    "y": 7.41540168270092,
+                    "heading": -3.1017205952496476,
+                    "angularVelocity": 0.301025073527845,
+                    "velocityX": -1.2092246026888915,
+                    "velocityY": -0.4039558853066335,
+                    "timestamp": 1.3055933791392478
+                },
+                {
+                    "x": 7.350564387959683,
+                    "y": 7.356948703524257,
+                    "heading": -3.061223374709373,
+                    "angularVelocity": 0.44594783682873773,
+                    "velocityX": -1.9263293426462433,
+                    "velocityY": -0.6436733008397082,
+                    "timestamp": 1.3964049431739292
+                },
+                {
+                    "x": 7.110508199108376,
+                    "y": 7.276727219900285,
+                    "heading": -3.007665771604216,
+                    "angularVelocity": 0.5897663328854458,
+                    "velocityX": -2.6434539631934633,
+                    "velocityY": -0.8833840103589294,
+                    "timestamp": 1.4872165072086105
+                },
+                {
+                    "x": 6.80532324732103,
+                    "y": 7.174739191350757,
+                    "heading": -2.9413637265725296,
+                    "angularVelocity": 0.7301057495614385,
+                    "velocityX": -3.3606397492593936,
+                    "velocityY": -1.1230731419900883,
+                    "timestamp": 1.578028071243292
+                },
+                {
+                    "x": 6.479515641287707,
+                    "y": 7.065810200947661,
+                    "heading": -2.9413637193977467,
+                    "angularVelocity": 7.900737542269723e-8,
+                    "velocityX": -3.587732570148064,
+                    "velocityY": -1.1995057189140004,
+                    "timestamp": 1.6688396352779733
+                },
+                {
+                    "x": 6.153708035353397,
+                    "y": 6.956881210248373,
+                    "heading": -2.9413637122229894,
+                    "angularVelocity": 7.900708560619817e-8,
+                    "velocityX": -3.5877325690577564,
+                    "velocityY": -1.1995057221756227,
+                    "timestamp": 1.7596511993126547
+                },
+                {
+                    "x": 5.827900259692805,
+                    "y": 6.847953284193167,
+                    "heading": -2.9413633662901284,
+                    "angularVelocity": 0.0000038093481209652,
+                    "velocityX": -3.587734438051908,
+                    "velocityY": -1.1994939985136122,
+                    "timestamp": 1.850462763347336
+                },
+                {
+                    "x": 5.5671157361716075,
+                    "y": 6.7607637759552865,
+                    "heading": -2.922540068712598,
+                    "angularVelocity": 0.2072786409826639,
+                    "velocityX": -2.8717105171937094,
+                    "velocityY": -0.9601145973503112,
+                    "timestamp": 1.9412743273820174
+                },
+                {
+                    "x": 5.371545145894187,
+                    "y": 6.69537761850095,
+                    "heading": -2.9031356081339226,
+                    "angularVelocity": 0.2136782995204373,
+                    "velocityX": -2.1535868515901284,
+                    "velocityY": -0.7200201664777647,
+                    "timestamp": 2.0320858914166986
+                },
+                {
+                    "x": 5.241178014496252,
+                    "y": 6.651791296591565,
+                    "heading": -2.888169171521839,
+                    "angularVelocity": 0.1648076076208368,
+                    "velocityX": -1.4355785277318724,
+                    "velocityY": -0.47996444475676003,
+                    "timestamp": 2.1228974554513798
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "angularVelocity": 0.08995739264147623,
+                    "velocityX": -0.717728135057343,
+                    "velocityY": -0.23996169235946166,
+                    "timestamp": 2.213709019486061
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": -2.956634078664977e-32,
+                    "timestamp": 2.304520583520742
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S1 To C1 To S2": {
+            "waypoints": [
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": -3.143185307179586,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.230255854586943,
+                    "y": 6.654457619561188,
+                    "heading": -2.8852614761787456,
+                    "angularVelocity": -0.06226832690913654,
+                    "velocityX": 0.6421052144646586,
+                    "velocityY": 0.2894501464070739,
+                    "timestamp": 0.08449682912507997
+                },
+                {
+                    "x": 5.338767562811348,
+                    "y": 6.703372857257978,
+                    "heading": -2.8957843391178932,
+                    "angularVelocity": -0.12453559557330285,
+                    "velocityX": 1.2842104176924247,
+                    "velocityY": 0.5789002759426788,
+                    "timestamp": 0.16899365825015994
+                },
+                {
+                    "x": 5.501535123103558,
+                    "y": 6.776745710953194,
+                    "heading": -2.911568425934469,
+                    "angularVelocity": -0.1868009365559807,
+                    "velocityX": 1.9263156023436965,
+                    "velocityY": 0.8683503801853032,
+                    "timestamp": 0.25349048737523994
+                },
+                {
+                    "x": 5.718558531629796,
+                    "y": 6.87457617714364,
+                    "heading": -2.9326134806355157,
+                    "angularVelocity": -0.24906324792252543,
+                    "velocityX": 2.568420741623094,
+                    "velocityY": 1.1578004429684545,
+                    "timestamp": 0.3379873165003199
+                },
+                {
+                    "x": 5.9898377739629405,
+                    "y": 6.996864247325266,
+                    "heading": -2.9589191023342685,
+                    "angularVelocity": -0.31132081488895513,
+                    "velocityX": 3.210525710160934,
+                    "velocityY": 1.4472504051081343,
+                    "timestamp": 0.4224841456253998
+                },
+                {
+                    "x": 6.281244778996208,
+                    "y": 7.128225687635927,
+                    "heading": -2.958919105469154,
+                    "angularVelocity": -3.710063015796767e-8,
+                    "velocityX": 3.448733024074779,
+                    "velocityY": 1.5546315958934869,
+                    "timestamp": 0.5069809747504798
+                },
+                {
+                    "x": 6.572651810300593,
+                    "y": 7.259587069667809,
+                    "heading": -2.9589191086040265,
+                    "angularVelocity": -3.7100470021746385e-8,
+                    "velocityX": 3.448733334987257,
+                    "velocityY": 1.5546309061778836,
+                    "timestamp": 0.5914778038755597
+                },
+                {
+                    "x": 6.868681501268645,
+                    "y": 7.380170200918755,
+                    "heading": -2.958919113683838,
+                    "angularVelocity": -6.01183729074805e-8,
+                    "velocityX": 3.5034414194388437,
+                    "velocityY": 1.4270728558635946,
+                    "timestamp": 0.6759746330006396
+                },
+                {
+                    "x": 7.138377967809157,
+                    "y": 7.448808221216772,
+                    "heading": -3.0028485661720943,
+                    "angularVelocity": -0.5198946865003425,
+                    "velocityX": 3.1917939327792095,
+                    "velocityY": 0.8123147461121022,
+                    "timestamp": 0.7604714621257196
+                },
+                {
+                    "x": 7.3546590217483025,
+                    "y": 7.491172600361784,
+                    "heading": -3.044173791439457,
+                    "angularVelocity": -0.4890742729077964,
+                    "velocityX": 2.5596351505567982,
+                    "velocityY": 0.5013724134227592,
+                    "timestamp": 0.8449682912507995
+                },
+                {
+                    "x": 7.517104833144657,
+                    "y": 7.508147678254589,
+                    "heading": -3.0815979795537904,
+                    "angularVelocity": -0.4429064202981498,
+                    "velocityX": 1.9225077802137127,
+                    "velocityY": 0.200896034426051,
+                    "timestamp": 0.9294651203758795
+                },
+                {
+                    "x": 7.625575199280341,
+                    "y": 7.500038616852461,
+                    "heading": -3.1146766365119762,
+                    "angularVelocity": -0.39147808622758395,
+                    "velocityX": 1.2837211438445335,
+                    "velocityY": -0.09596882493808385,
+                    "timestamp": 1.0139619495009595
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -0.3373933786960057,
+                    "velocityX": 0.6441046520111882,
+                    "velocityY": -0.39100422104069327,
+                    "timestamp": 1.0984587786260396
+                },
+                {
+                    "x": 7.666478172550486,
+                    "y": 7.390434189445638,
+                    "heading": -3.1708333738054395,
+                    "angularVelocity": -0.26987816293521383,
+                    "velocityX": -0.13198919117872188,
+                    "velocityY": -0.7473737883984474,
+                    "timestamp": 1.200905254195427
+                },
+                {
+                    "x": 7.573314043125627,
+                    "y": 7.277653381548422,
+                    "heading": -3.1915604539305784,
+                    "angularVelocity": -0.2023210658047483,
+                    "velocityX": -0.909393211499577,
+                    "velocityY": -1.1008754305152135,
+                    "timestamp": 1.3033517297648145
+                },
+                {
+                    "x": 7.400248960624901,
+                    "y": 7.129232012070322,
+                    "heading": -3.205357963605193,
+                    "angularVelocity": -0.13468017906843224,
+                    "velocityX": -1.6893219755862467,
+                    "velocityY": -1.4487698932851405,
+                    "timestamp": 1.405798205334202
+                },
+                {
+                    "x": 7.146578363694586,
+                    "y": 6.946793949790035,
+                    "heading": -3.2122031163802927,
+                    "angularVelocity": -0.06681686936573578,
+                    "velocityX": -2.476128100263464,
+                    "velocityY": -1.780813456649585,
+                    "timestamp": 1.5082446809035894
+                },
+                {
+                    "x": 6.805429935311203,
+                    "y": 6.762913771007078,
+                    "heading": -3.2122031133395477,
+                    "angularVelocity": 2.9681302827383053e-8,
+                    "velocityX": -3.330016249825229,
+                    "velocityY": -1.7948902366915849,
+                    "timestamp": 1.610691156472977
+                },
+                {
+                    "x": 6.4551130042311256,
+                    "y": 6.597166308184442,
+                    "heading": -3.2122031105786535,
+                    "angularVelocity": 2.694963002504427e-8,
+                    "velocityX": -3.4195117902597523,
+                    "velocityY": -1.6178932647651112,
+                    "timestamp": 1.7131376320423644
+                },
+                {
+                    "x": 6.104796046388704,
+                    "y": 6.431418901925591,
+                    "heading": -3.212203107817759,
+                    "angularVelocity": 2.6949630458066062e-8,
+                    "velocityX": -3.419512051492193,
+                    "velocityY": -1.617892712634998,
+                    "timestamp": 1.8155841076117518
+                },
+                {
+                    "x": 5.754479088546207,
+                    "y": 6.265671495666902,
+                    "heading": -3.2122031050568642,
+                    "angularVelocity": 2.694963045994546e-8,
+                    "velocityX": -3.4195120514929416,
+                    "velocityY": -1.617892712633416,
+                    "timestamp": 1.9180305831811393
+                },
+                {
+                    "x": 5.404162130703709,
+                    "y": 6.099924089408213,
+                    "heading": -3.212203102295969,
+                    "angularVelocity": 2.6949631214728657e-8,
+                    "velocityX": -3.419512051492942,
+                    "velocityY": -1.6178927126334155,
+                    "timestamp": 2.0204770587505267
+                },
+                {
+                    "x": 5.05384517285956,
+                    "y": 5.9341766831530425,
+                    "heading": -3.212203099535067,
+                    "angularVelocity": 2.6949706103007404e-8,
+                    "velocityX": -3.4195120515090505,
+                    "velocityY": -1.6178927125990576,
+                    "timestamp": 2.122923534319914
+                },
+                {
+                    "x": 4.737507119027181,
+                    "y": 5.784506016757487,
+                    "heading": -3.184596232441734,
+                    "angularVelocity": 0.2694760062744644,
+                    "velocityX": -3.087837351887456,
+                    "velocityY": -1.4609645238032911,
+                    "timestamp": 2.2253700098893017
+                },
+                {
+                    "x": 4.500253563523173,
+                    "y": 5.67225301019093,
+                    "heading": -3.163890824246715,
+                    "angularVelocity": 0.20210952187413175,
+                    "velocityX": -2.315878161599758,
+                    "velocityY": -1.0957234589346887,
+                    "timestamp": 2.327816485458689
+                },
+                {
+                    "x": 4.3420845221283875,
+                    "y": 5.597417670515286,
+                    "heading": -3.150087142577917,
+                    "angularVelocity": 0.1347404251056839,
+                    "velocityX": -1.5439188172721183,
+                    "velocityY": -0.7304823251333565,
+                    "timestamp": 2.4302629610280766
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": 0.06737015949032035,
+                    "velocityX": -0.771959422604275,
+                    "velocityY": -0.3652411691795466,
+                    "timestamp": 2.532709436597464
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -1.9737281778417947e-31,
+                    "velocityX": -1.0240473596982798e-33,
+                    "velocityY": -4.498532055712875e-34,
+                    "timestamp": 2.6351559121668515
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S1 To C2 To S1": {
+            "waypoints": [
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": -3.143185307179586,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "angularVelocity": 1.4108683409597047e-30,
+                    "velocityX": 3.332067657020877e-32,
+                    "velocityY": 1.6252866310843352e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.234960531442942,
+                    "y": 6.610392010976446,
+                    "heading": -2.8906900859192084,
+                    "angularVelocity": -0.12376379247862908,
+                    "velocityX": 0.6826118174441478,
+                    "velocityY": -0.22701025069196354,
+                    "timestamp": 0.08637490581940685
+                },
+                {
+                    "x": 5.352887916547719,
+                    "y": 6.571175678219841,
+                    "heading": -2.91160851046633,
+                    "angularVelocity": -0.24218173494536643,
+                    "velocityX": 1.3652968299769979,
+                    "velocityY": -0.45402460801062294,
+                    "timestamp": 0.1727498116388137
+                },
+                {
+                    "x": 5.529792169405666,
+                    "y": 6.5123505926970395,
+                    "heading": -2.941981384820343,
+                    "angularVelocity": -0.35164002861494675,
+                    "velocityX": 2.0480977800178555,
+                    "velocityY": -0.6810437008846502,
+                    "timestamp": 0.2591247174582205
+                },
+                {
+                    "x": 5.765691321896416,
+                    "y": 6.43391666324999,
+                    "heading": -2.9802445174117826,
+                    "angularVelocity": -0.4429889934853137,
+                    "velocityX": 2.7311074930027868,
+                    "velocityY": -0.9080638491352222,
+                    "timestamp": 0.3454996232776274
+                },
+                {
+                    "x": 6.060622443246375,
+                    "y": 6.335879897503487,
+                    "heading": -3.0215937804128985,
+                    "angularVelocity": -0.47871847278597873,
+                    "velocityX": 3.4145463726079974,
+                    "velocityY": -1.135014444490357,
+                    "timestamp": 0.4318745290970343
+                },
+                {
+                    "x": 6.370665856130874,
+                    "y": 6.2327325092534815,
+                    "heading": -3.021593787446907,
+                    "angularVelocity": -8.143578577450647e-8,
+                    "velocityX": 3.5895079704369173,
+                    "velocityY": -1.1941823527503022,
+                    "timestamp": 0.5182494349164412
+                },
+                {
+                    "x": 6.680709268872309,
+                    "y": 6.129585120573416,
+                    "heading": -3.021593794480898,
+                    "angularVelocity": -8.143559184955701e-8,
+                    "velocityX": 3.589507968780596,
+                    "velocityY": -1.1941823577293083,
+                    "timestamp": 0.604624340735848
+                },
+                {
+                    "x": 6.990752683844134,
+                    "y": 6.026437738598777,
+                    "heading": -3.0215938015153188,
+                    "angularVelocity": -8.144055599607105e-8,
+                    "velocityX": 3.5895079946028012,
+                    "velocityY": -1.1941822800976392,
+                    "timestamp": 0.6909992465552549
+                },
+                {
+                    "x": 7.246471127975024,
+                    "y": 5.941338278881442,
+                    "heading": -3.0707797962131482,
+                    "angularVelocity": -0.5694477375264861,
+                    "velocityX": 2.96056408634992,
+                    "velocityY": -0.9852336035564434,
+                    "timestamp": 0.7773741523746618
+                },
+                {
+                    "x": 7.443255050635665,
+                    "y": 5.875844773804915,
+                    "heading": -3.107535505632415,
+                    "angularVelocity": -0.4255368972118188,
+                    "velocityX": 2.2782533977168096,
+                    "velocityY": -0.7582469058018522,
+                    "timestamp": 0.8637490581940687
+                },
+                {
+                    "x": 7.581104494826472,
+                    "y": 5.829957223632905,
+                    "heading": -3.1318559851891097,
+                    "angularVelocity": -0.2815688112890415,
+                    "velocityX": 1.5959432069182882,
+                    "velocityY": -0.531260204994182,
+                    "timestamp": 0.9501239640134755
+                },
+                {
+                    "x": 7.660019481037545,
+                    "y": 5.803675631519179,
+                    "heading": -3.1437391060728066,
+                    "angularVelocity": -0.13757607919844067,
+                    "velocityX": 0.9136332533425802,
+                    "velocityY": -0.3042734676739749,
+                    "timestamp": 1.0364988698328823
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": 0.0064115716005977265,
+                    "velocityX": 0.2313231924569808,
+                    "velocityY": -0.07728670099033955,
+                    "timestamp": 1.122873775652289
+                },
+                {
+                    "x": 7.635819311328683,
+                    "y": 5.811667633489592,
+                    "heading": -3.128848165121335,
+                    "angularVelocity": 0.15781563504855214,
+                    "velocityX": -0.48631752487124336,
+                    "velocityY": 0.1614535089628058,
+                    "timestamp": 1.2137211879121816
+                },
+                {
+                    "x": 7.526434828772679,
+                    "y": 5.848024979672397,
+                    "heading": -3.1012666400693027,
+                    "angularVelocity": 0.30360275946215987,
+                    "velocityX": -1.2040462114976394,
+                    "velocityY": 0.4002023313416631,
+                    "timestamp": 1.3045686001720742
+                },
+                {
+                    "x": 7.351833825159198,
+                    "y": 5.906073128317211,
+                    "heading": -3.0612921137460094,
+                    "angularVelocity": 0.44001832665334883,
+                    "velocityX": -1.921914992073542,
+                    "velocityY": 0.6389631493158797,
+                    "timestamp": 1.3954160124319668
+                },
+                {
+                    "x": 7.111993081438883,
+                    "y": 5.98581338993347,
+                    "heading": -3.010630767411968,
+                    "angularVelocity": 0.5576531579045794,
+                    "velocityX": -2.640039355596495,
+                    "velocityY": 0.8777383926810057,
+                    "timestamp": 1.4862634246918593
+                },
+                {
+                    "x": 6.806861000464256,
+                    "y": 6.08724274158431,
+                    "heading": -2.954485863102405,
+                    "angularVelocity": 0.6180132478540051,
+                    "velocityX": -3.358731673078681,
+                    "velocityY": 1.116480361164798,
+                    "timestamp": 1.577110836951752
+                },
+                {
+                    "x": 6.480775270198975,
+                    "y": 6.195766521383185,
+                    "heading": -2.9544858545944934,
+                    "angularVelocity": 9.365056825417393e-8,
+                    "velocityX": -3.5893783009738223,
+                    "velocityY": 1.1945720532843969,
+                    "timestamp": 1.6679582492116445
+                },
+                {
+                    "x": 6.154689540144831,
+                    "y": 6.30429030181652,
+                    "heading": -2.954485846086605,
+                    "angularVelocity": 9.365030974191779e-8,
+                    "velocityX": -3.589378298649725,
+                    "velocityY": 1.1945720602682006,
+                    "timestamp": 1.758805661471537
+                },
+                {
+                    "x": 5.828602321664721,
+                    "y": 6.412809153986278,
+                    "heading": -2.954485580357569,
+                    "angularVelocity": 0.0000029250024503863607,
+                    "velocityX": -3.5893946824518035,
+                    "velocityY": 1.194517812563883,
+                    "timestamp": 1.8496530737314296
+                },
+                {
+                    "x": 5.567525835725181,
+                    "y": 6.4996975117043,
+                    "heading": -2.9315080017864195,
+                    "angularVelocity": 0.25292496505229944,
+                    "velocityX": -2.8737911124282145,
+                    "velocityY": 0.9564208330939352,
+                    "timestamp": 1.9405004859913222
+                },
+                {
+                    "x": 5.371744680904255,
+                    "y": 6.564854924968385,
+                    "heading": -2.907956180338786,
+                    "angularVelocity": 0.2592459252452843,
+                    "velocityX": -2.155054832611796,
+                    "velocityY": 0.7172181534207646,
+                    "timestamp": 2.0313478982512145
+                },
+                {
+                    "x": 5.241242968677622,
+                    "y": 6.608286759589549,
+                    "heading": -2.8898557615846676,
+                    "angularVelocity": 0.19923978354186256,
+                    "velocityX": -1.4364934452213165,
+                    "velocityY": 0.478074537741632,
+                    "timestamp": 2.122195310511107
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "angularVelocity": 0.10848698206630299,
+                    "velocityX": -0.7181599019131975,
+                    "velocityY": 0.2390078029777863,
+                    "timestamp": 2.2130427227709997
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": -3.247565811207596e-32,
+                    "timestamp": 2.3038901350308922
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S1 To C2 To S2": {
+            "waypoints": [
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": -3.143185307179586,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 14
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.2343405973213555,
+                    "y": 6.622412668725056,
+                    "heading": -2.8862658883257666,
+                    "angularVelocity": -0.07457855479400594,
+                    "velocityX": 0.6943879634997097,
+                    "velocityY": -0.0903067804291561,
+                    "timestamp": 0.08401729348434916
+                },
+                {
+                    "x": 5.351021788709109,
+                    "y": 6.607238006430742,
+                    "heading": -2.898797916255787,
+                    "angularVelocity": -0.14916010038283942,
+                    "velocityX": 1.3887758882579295,
+                    "velocityY": -0.18061355781642813,
+                    "timestamp": 0.16803458696869833
+                },
+                {
+                    "x": 5.52604356848206,
+                    "y": 6.584476013331715,
+                    "heading": -2.917596514461126,
+                    "angularVelocity": -0.22374677195285786,
+                    "velocityX": 2.083163745396705,
+                    "velocityY": -0.27092033264873905,
+                    "timestamp": 0.2520518804530475
+                },
+                {
+                    "x": 5.759405926747038,
+                    "y": 6.554126689512535,
+                    "heading": -2.9426622755752048,
+                    "angularVelocity": -0.2983404972304698,
+                    "velocityX": 2.7775514847838854,
+                    "velocityY": -0.3612271064746121,
+                    "timestamp": 0.33606917393739666
+                },
+                {
+                    "x": 6.051108840456095,
+                    "y": 6.516190035692408,
+                    "heading": -2.97399589297024,
+                    "angularVelocity": -0.37294247524019475,
+                    "velocityX": 3.4719389498472166,
+                    "velocityY": -0.45153387174027243,
+                    "timestamp": 0.4200864674217458
+                },
+                {
+                    "x": 6.366286997596586,
+                    "y": 6.47520006127383,
+                    "heading": -2.973995896754836,
+                    "angularVelocity": -4.504543806754251e-8,
+                    "velocityX": 3.751348610143015,
+                    "velocityY": -0.4878754446691907,
+                    "timestamp": 0.504103760906095
+                },
+                {
+                    "x": 6.681465049565767,
+                    "y": 6.434209278184785,
+                    "heading": -2.973995900539415,
+                    "angularVelocity": -4.5045241068948784e-8,
+                    "velocityX": 3.751347358361316,
+                    "velocityY": -0.48788506971698453,
+                    "timestamp": 0.5881210543904442
+                },
+                {
+                    "x": 6.987163031152994,
+                    "y": 6.347225437890881,
+                    "heading": -2.9739959119301913,
+                    "angularVelocity": -1.3557656915583158e-7,
+                    "velocityX": 3.6385126074570944,
+                    "velocityY": -1.0353087642618135,
+                    "timestamp": 0.6721383478747933
+                },
+                {
+                    "x": 7.243185278067537,
+                    "y": 6.230387598842041,
+                    "heading": -3.0107958850128846,
+                    "angularVelocity": -0.4380047435061505,
+                    "velocityX": 3.047256538467705,
+                    "velocityY": -1.3906403575188384,
+                    "timestamp": 0.7561556413591425
+                },
+                {
+                    "x": 7.4403635227243035,
+                    "y": 6.1148085109072055,
+                    "heading": -3.048556145231695,
+                    "angularVelocity": -0.44943438014751774,
+                    "velocityX": 2.3468768925946972,
+                    "velocityY": -1.3756583096355752,
+                    "timestamp": 0.8401729348434916
+                },
+                {
+                    "x": 7.578836794258849,
+                    "y": 6.003419207644454,
+                    "heading": -3.0839307394839546,
+                    "angularVelocity": -0.42103944063431625,
+                    "velocityX": 1.648152014803231,
+                    "velocityY": -1.3257901872726057,
+                    "timestamp": 0.9241902283278408
+                },
+                {
+                    "x": 7.658696859900876,
+                    "y": 5.8972967644784475,
+                    "heading": -3.115687244057696,
+                    "angularVelocity": -0.37797581017838017,
+                    "velocityX": 0.9505193791669008,
+                    "velocityY": -1.2631023776762744,
+                    "timestamp": 1.00820752181219
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -0.32729051343474413,
+                    "velocityX": 0.25355660978405975,
+                    "velocityY": -1.1937633351296981,
+                    "timestamp": 1.092224815296539
+                },
+                {
+                    "x": 7.6231583877847156,
+                    "y": 5.686873944343296,
+                    "heading": -3.169227404734073,
+                    "angularVelocity": -0.2617377186372048,
+                    "velocityX": -0.5712901533281343,
+                    "velocityY": -1.1068287610009868,
+                    "timestamp": 1.1917217379796003
+                },
+                {
+                    "x": 7.484327538641781,
+                    "y": 5.5861293490761845,
+                    "heading": -3.18873884204171,
+                    "angularVelocity": -0.1961009122843807,
+                    "velocityX": -1.3953280704486528,
+                    "velocityY": -1.0125398107841346,
+                    "timestamp": 1.2912186606626617
+                },
+                {
+                    "x": 7.263675847711948,
+                    "y": 5.496135627515027,
+                    "heading": -3.2017054758509116,
+                    "angularVelocity": -0.13032195830322837,
+                    "velocityX": -2.2176735217500094,
+                    "velocityY": -0.9044874869931846,
+                    "timestamp": 1.390715583345723
+                },
+                {
+                    "x": 6.9617360146088005,
+                    "y": 5.420370006770522,
+                    "heading": -3.2080997246142733,
+                    "angularVelocity": -0.0642657942671267,
+                    "velocityX": -3.034665042505381,
+                    "velocityY": -0.7614870761968194,
+                    "timestamp": 1.4902125060287843
+                },
+                {
+                    "x": 6.587133153221649,
+                    "y": 5.383725358359533,
+                    "heading": -3.208099721013608,
+                    "angularVelocity": 3.618871368220605e-8,
+                    "velocityX": -3.7649693205127126,
+                    "velocityY": -0.3682993144191623,
+                    "timestamp": 1.5897094287118456
+                },
+                {
+                    "x": 6.211820115708755,
+                    "y": 5.412190570741109,
+                    "heading": -3.2080997182657316,
+                    "angularVelocity": 2.761770262617581e-8,
+                    "velocityX": -3.7721069897651076,
+                    "velocityY": 0.2860913846777879,
+                    "timestamp": 1.689206351394907
+                },
+                {
+                    "x": 5.836507126679018,
+                    "y": 5.440656422364601,
+                    "heading": -3.208099715517856,
+                    "angularVelocity": 2.761769551512973e-8,
+                    "velocityX": -3.7721065024821283,
+                    "velocityY": 0.28609780941835966,
+                    "timestamp": 1.7887032740779683
+                },
+                {
+                    "x": 5.461194137649431,
+                    "y": 5.46912227399009,
+                    "heading": -3.20809971276998,
+                    "angularVelocity": 2.7617694565057146e-8,
+                    "velocityX": -3.772106502480606,
+                    "velocityY": 0.28609780943843166,
+                    "timestamp": 1.8882001967610296
+                },
+                {
+                    "x": 5.08588114861977,
+                    "y": 5.497588125614274,
+                    "heading": -3.2080997100220925,
+                    "angularVelocity": 2.7617811191359224e-8,
+                    "velocityX": -3.7721065024813654,
+                    "velocityY": 0.2860978094253166,
+                    "timestamp": 1.9876971194440909
+                },
+                {
+                    "x": 4.756728707190495,
+                    "y": 5.522552875223879,
+                    "heading": -3.182133930988408,
+                    "angularVelocity": 0.26097067460464174,
+                    "velocityX": -3.3081670523395013,
+                    "velocityY": 0.2509097662158744,
+                    "timestamp": 2.087194042127152
+                },
+                {
+                    "x": 4.509864358241751,
+                    "y": 5.541276437563115,
+                    "heading": -3.1626596163193277,
+                    "angularVelocity": 0.19572780890032443,
+                    "velocityX": -2.4811254689263977,
+                    "velocityY": 0.18818232598889303,
+                    "timestamp": 2.1866909648102135
+                },
+                {
+                    "x": 4.3452881204218,
+                    "y": 5.553758812444608,
+                    "heading": -3.1496767469198765,
+                    "angularVelocity": 0.13048513511123086,
+                    "velocityX": -1.654083698087773,
+                    "velocityY": 0.12545488387870263,
+                    "timestamp": 2.286187887493275
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": 0.06524261821612926,
+                    "velocityX": -0.8270418642385756,
+                    "velocityY": 0.06272744309160665,
+                    "timestamp": 2.385684810176336
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -3.6755949365770895e-31,
+                    "velocityX": -3.301266761312006e-32,
+                    "velocityY": -7.495504571617436e-31,
+                    "timestamp": 2.4851817328593975
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S1 To C3 To S1": {
+            "waypoints": [
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": -3.143185307179586,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.218507748835192,
+                    "y": 6.587508953337151,
+                    "heading": -2.890727584630965,
+                    "angularVelocity": -0.12627463531080194,
+                    "velocityX": 0.5003596491390202,
+                    "velocityY": -0.5001630474999694,
+                    "timestamp": 0.08495439012388621
+                },
+                {
+                    "x": 5.303524107208959,
+                    "y": 6.502525872657889,
+                    "heading": -2.9120884337900024,
+                    "angularVelocity": -0.25143902661031875,
+                    "velocityX": 1.0007294296365374,
+                    "velocityY": -1.0003377171605015,
+                    "timestamp": 0.16990878024777242
+                },
+                {
+                    "x": 5.43105048877714,
+                    "y": 6.375049128446974,
+                    "heading": -2.9439256135386342,
+                    "angularVelocity": -0.37475614505901766,
+                    "velocityX": 1.5011158503081115,
+                    "velocityY": -1.5005315678808802,
+                    "timestamp": 0.2548631703716586
+                },
+                {
+                    "x": 5.601089619523365,
+                    "y": 6.205075520007896,
+                    "heading": -2.985927322319944,
+                    "angularVelocity": -0.49440304050481265,
+                    "velocityX": 2.001534358593136,
+                    "velocityY": -2.0007630940704417,
+                    "timestamp": 0.33981756049554485
+                },
+                {
+                    "x": 5.813649033766866,
+                    "y": 5.992595894100207,
+                    "heading": -3.037161099536011,
+                    "angularVelocity": -0.6030739216881468,
+                    "velocityX": 2.502041553515969,
+                    "velocityY": -2.501102363255205,
+                    "timestamp": 0.42477195061943107
+                },
+                {
+                    "x": 6.040945217563345,
+                    "y": 5.7653958045642115,
+                    "heading": -3.03716110602298,
+                    "angularVelocity": -7.635824927871784e-8,
+                    "velocityX": 2.6755083929744035,
+                    "velocityY": -2.6743772653146864,
+                    "timestamp": 0.5097263407433172
+                },
+                {
+                    "x": 6.268241401429681,
+                    "y": 5.53819571509808,
+                    "heading": -3.037161112509923,
+                    "angularVelocity": -7.635794871774561e-8,
+                    "velocityX": 2.675508393796696,
+                    "velocityY": -2.674377264492316,
+                    "timestamp": 0.5946807308672034
+                },
+                {
+                    "x": 6.495537585296019,
+                    "y": 5.310995625631949,
+                    "heading": -3.037161118996866,
+                    "angularVelocity": -7.635794841586156e-8,
+                    "velocityX": 2.675508393796701,
+                    "velocityY": -2.6743772644923114,
+                    "timestamp": 0.6796351209910896
+                },
+                {
+                    "x": 6.722833769162356,
+                    "y": 5.083795536165818,
+                    "heading": -3.037161125483809,
+                    "angularVelocity": -7.635794938585852e-8,
+                    "velocityX": 2.6755083937967,
+                    "velocityY": -2.6743772644923127,
+                    "timestamp": 0.7645895111149758
+                },
+                {
+                    "x": 6.950129953013132,
+                    "y": 4.856595446684119,
+                    "heading": -3.037161131970752,
+                    "angularVelocity": -7.635794903516443e-8,
+                    "velocityX": 2.67550839361353,
+                    "velocityY": -2.6743772646755604,
+                    "timestamp": 0.8495439012388619
+                },
+                {
+                    "x": 7.177423048635347,
+                    "y": 4.629392269264043,
+                    "heading": -3.0371611395742963,
+                    "angularVelocity": -8.950149400601411e-8,
+                    "velocityX": 2.6754720420070424,
+                    "velocityY": -2.674413612866287,
+                    "timestamp": 0.9344982913627481
+                },
+                {
+                    "x": 7.362966742660802,
+                    "y": 4.443922816476664,
+                    "heading": -3.078668671836781,
+                    "angularVelocity": -0.48858607779956104,
+                    "velocityX": 2.1840389149365182,
+                    "velocityY": -2.183165019685597,
+                    "timestamp": 1.0194526814866343
+                },
+                {
+                    "x": 7.505991293187966,
+                    "y": 4.3009534921360935,
+                    "heading": -3.1105416993817094,
+                    "angularVelocity": -0.37517811025716574,
+                    "velocityX": 1.6835451389668437,
+                    "velocityY": -1.6828950703063337,
+                    "timestamp": 1.1044070716105205
+                },
+                {
+                    "x": 7.6065035905836105,
+                    "y": 4.200477915848541,
+                    "heading": -3.1320612562280816,
+                    "angularVelocity": -0.25330717829595906,
+                    "velocityX": 1.1831324696564203,
+                    "velocityY": -1.1827002246831095,
+                    "timestamp": 1.1893614617344068
+                },
+                {
+                    "x": 7.664506074481624,
+                    "y": 4.142493806219386,
+                    "heading": -3.1429833930528956,
+                    "angularVelocity": -0.12856471347350074,
+                    "velocityX": 0.6827485173302748,
+                    "velocityY": -0.6825322334062814,
+                    "timestamp": 1.274315851858293
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -0.002376735636840867,
+                    "velocityX": 0.1823793390257534,
+                    "velocityY": -0.18237793475472894,
+                    "timestamp": 1.3592702419821794
+                },
+                {
+                    "x": 7.6496885572239846,
+                    "y": 4.157291685194411,
+                    "heading": -3.131583473376336,
+                    "angularVelocity": 0.13054330158990457,
+                    "velocityX": -0.3410629632388108,
+                    "velocityY": 0.3408406518372109,
+                    "timestamp": 1.44814369364031
+                },
+                {
+                    "x": 7.572857027601893,
+                    "y": 4.234083644862719,
+                    "heading": -3.108168408493633,
+                    "angularVelocity": 0.26346523563343455,
+                    "velocityX": -0.8645048458050324,
+                    "velocityY": 0.8640596065007748,
+                    "timestamp": 1.5370171452984407
+                },
+                {
+                    "x": 7.449505471220409,
+                    "y": 4.357375922418621,
+                    "heading": -3.0729392389653802,
+                    "angularVelocity": 0.39639699900261377,
+                    "velocityX": -1.387946052278649,
+                    "velocityY": 1.3872790496532752,
+                    "timestamp": 1.6258905969565713
+                },
+                {
+                    "x": 7.279633974763547,
+                    "y": 4.52716856280588,
+                    "heading": -3.0258936977546345,
+                    "angularVelocity": 0.5293542709674363,
+                    "velocityX": -1.911386283387714,
+                    "velocityY": 1.9104989985109575,
+                    "timestamp": 1.714764048614702
+                },
+                {
+                    "x": 7.063242657628,
+                    "y": 4.743461594393602,
+                    "heading": -2.9670273926808983,
+                    "angularVelocity": 0.6623609635456799,
+                    "velocityX": -2.434825171050669,
+                    "velocityY": 2.4337192665764835,
+                    "timestamp": 1.8036375002728326
+                },
+                {
+                    "x": 6.825460366769834,
+                    "y": 4.981142108011423,
+                    "heading": -2.967027386113265,
+                    "angularVelocity": 7.38987079974334e-8,
+                    "velocityX": -2.675515425830906,
+                    "velocityY": 2.6743702329927124,
+                    "timestamp": 1.8925109519309633
+                },
+                {
+                    "x": 6.587678075764525,
+                    "y": 5.218822621482074,
+                    "heading": -2.9670273795456663,
+                    "angularVelocity": 7.389832008297281e-8,
+                    "velocityX": -2.6755154274865576,
+                    "velocityY": 2.6743702313367446,
+                    "timestamp": 1.981384403589094
+                },
+                {
+                    "x": 6.349895784759215,
+                    "y": 5.456503134952723,
+                    "heading": -2.967027372978067,
+                    "angularVelocity": 7.389832049238788e-8,
+                    "velocityX": -2.675515427486565,
+                    "velocityY": 2.6743702313367375,
+                    "timestamp": 2.0702578552472244
+                },
+                {
+                    "x": 6.112113493753906,
+                    "y": 5.6941836484233725,
+                    "heading": -2.967027366410468,
+                    "angularVelocity": 7.389831996958896e-8,
+                    "velocityX": -2.67551542748656,
+                    "velocityY": 2.6743702313367437,
+                    "timestamp": 2.159131306905355
+                },
+                {
+                    "x": 5.874331202859884,
+                    "y": 5.931864162005351,
+                    "heading": -2.9670273598428634,
+                    "angularVelocity": 7.389838540319935e-8,
+                    "velocityX": -2.6755154262343623,
+                    "velocityY": 2.6743702325894136,
+                    "timestamp": 2.2480047585634853
+                },
+                {
+                    "x": 5.641547676468134,
+                    "y": 6.164595563213349,
+                    "heading": -2.949497080521501,
+                    "angularVelocity": 0.1972498985246826,
+                    "velocityX": -2.619269557429503,
+                    "velocityY": 2.6186830472537257,
+                    "timestamp": 2.3368782102216157
+                },
+                {
+                    "x": 5.455313702452088,
+                    "y": 6.350776206364247,
+                    "heading": -2.9252668204425896,
+                    "angularVelocity": 0.2726377745748803,
+                    "velocityX": -2.0954961300759565,
+                    "velocityY": 2.0948960536284607,
+                    "timestamp": 2.425751661879746
+                },
+                {
+                    "x": 5.3156503816730245,
+                    "y": 6.490395881310486,
+                    "heading": -2.9038323021536026,
+                    "angularVelocity": 0.2411802162410647,
+                    "velocityX": -1.571485276799544,
+                    "velocityY": 1.5709941758913435,
+                    "timestamp": 2.5146251135378765
+                },
+                {
+                    "x": 5.222548326123915,
+                    "y": 6.583467415924721,
+                    "heading": -2.888242371448522,
+                    "angularVelocity": 0.17541718493253458,
+                    "velocityX": -1.0475800569474265,
+                    "velocityY": 1.047236636788705,
+                    "timestamp": 2.603498565196007
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "angularVelocity": 0.09274278532860386,
+                    "velocityX": -0.5237596296246885,
+                    "velocityY": 0.5235825008157663,
+                    "timestamp": 2.6923720168541374
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "angularVelocity": 3.731495977364104e-28,
+                    "velocityX": 5.297862503022749e-31,
+                    "velocityY": -3.827373628676626e-30,
+                    "timestamp": 2.781245468512268
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S1 To C3 To S2": {
+            "waypoints": [
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": -3.143185307179586,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.2230421692043185,
+                    "y": 6.596479510791831,
+                    "heading": -2.8866360691814505,
+                    "angularVelocity": -0.07970895698970118,
+                    "velocityX": 0.5650456828103535,
+                    "velocityY": -0.40263040657207605,
+                    "timestamp": 0.08325374502526237
+                },
+                {
+                    "x": 5.3171265089770285,
+                    "y": 6.529438536200997,
+                    "heading": -2.8999080848900367,
+                    "angularVelocity": -0.15941644072056302,
+                    "velocityX": 1.130091382005238,
+                    "velocityY": -0.8052607671941977,
+                    "timestamp": 0.16650749005052473
+                },
+                {
+                    "x": 5.458253021148553,
+                    "y": 6.428877082265555,
+                    "heading": -2.919815877976496,
+                    "angularVelocity": -0.23912189271985748,
+                    "velocityX": 1.6951371031862086,
+                    "velocityY": -1.207891055290405,
+                    "timestamp": 0.2497612350757871
+                },
+                {
+                    "x": 5.646421706294536,
+                    "y": 6.294795158564417,
+                    "heading": -2.946359278217123,
+                    "angularVelocity": -0.31882530008192794,
+                    "velocityX": 2.260182831281474,
+                    "velocityY": -1.6105212283297445,
+                    "timestamp": 0.33301498010104946
+                },
+                {
+                    "x": 5.881632555955815,
+                    "y": 6.127192783772945,
+                    "heading": -2.9795381480937526,
+                    "angularVelocity": -0.3985270556484735,
+                    "velocityX": 2.825228457769793,
+                    "velocityY": -2.01315117705052,
+                    "timestamp": 0.41626872512631186
+                },
+                {
+                    "x": 6.138121957560345,
+                    "y": 5.944428216164069,
+                    "heading": -2.9795381521700137,
+                    "angularVelocity": -4.8961898041700444e-8,
+                    "velocityX": 3.080815181667819,
+                    "velocityY": -2.195271426569656,
+                    "timestamp": 0.49952247015157425
+                },
+                {
+                    "x": 6.3946113591646245,
+                    "y": 5.761663648554803,
+                    "heading": -2.979538156246253,
+                    "angularVelocity": -4.896162950841139e-8,
+                    "velocityX": 3.080815181664829,
+                    "velocityY": -2.1952714265743465,
+                    "timestamp": 0.5827762151768366
+                },
+                {
+                    "x": 6.651100760768856,
+                    "y": 5.57889908094547,
+                    "heading": -2.979538160322492,
+                    "angularVelocity": -4.8961628925618056e-8,
+                    "velocityX": 3.080815181664257,
+                    "velocityY": -2.1952714265751494,
+                    "timestamp": 0.666029960202099
+                },
+                {
+                    "x": 6.907590153574081,
+                    "y": 5.396134500987725,
+                    "heading": -2.979538164398731,
+                    "angularVelocity": -4.8961630365108595e-8,
+                    "velocityX": 3.080815075975223,
+                    "velocityY": -2.195271574897749,
+                    "timestamp": 0.7492837052273614
+                },
+                {
+                    "x": 7.162448400773746,
+                    "y": 5.211102141322406,
+                    "heading": -2.979538168481962,
+                    "angularVelocity": -4.904561353756505e-8,
+                    "velocityX": 3.06122261674062,
+                    "velocityY": -2.2225109466143445,
+                    "timestamp": 0.8325374502526238
+                },
+                {
+                    "x": 7.379207979156498,
+                    "y": 4.982618051640636,
+                    "heading": -2.979538183621576,
+                    "angularVelocity": -1.8184904794867305e-7,
+                    "velocityX": 2.6036015354861264,
+                    "velocityY": -2.7444301708283056,
+                    "timestamp": 0.9157911952778862
+                },
+                {
+                    "x": 7.538979437885237,
+                    "y": 4.753039236440615,
+                    "heading": -3.0140329583631105,
+                    "angularVelocity": -0.4143330096570687,
+                    "velocityX": 1.919090350593248,
+                    "velocityY": -2.7575794353799603,
+                    "timestamp": 0.9990449403031486
+                },
+                {
+                    "x": 7.647684941731154,
+                    "y": 4.550516222432533,
+                    "heading": -3.0504775725311664,
+                    "angularVelocity": -0.43775345069461125,
+                    "velocityX": 1.3057130800893728,
+                    "velocityY": -2.432599445786314,
+                    "timestamp": 1.082298685328411
+                },
+                {
+                    "x": 7.7070137148586175,
+                    "y": 4.378021543776967,
+                    "heading": -3.0850144113956173,
+                    "angularVelocity": -0.4148382616778724,
+                    "velocityX": 0.7126258777842882,
+                    "velocityY": -2.071914946327394,
+                    "timestamp": 1.1655524303536733
+                },
+                {
+                    "x": 7.717659875456922,
+                    "y": 4.236658143123209,
+                    "heading": -3.1161746508326322,
+                    "angularVelocity": -0.3742803333058527,
+                    "velocityX": 0.12787605644728667,
+                    "velocityY": -1.6979824824801262,
+                    "timestamp": 1.2488061753789357
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -0.32443773356692346,
+                    "velocityX": -0.4523505272405549,
+                    "velocityY": -1.3171556797827937,
+                    "timestamp": 1.332059920404198
+                },
+                {
+                    "x": 7.570205120538345,
+                    "y": 4.042598994966427,
+                    "heading": -3.1684471523328126,
+                    "angularVelocity": -0.2594470408705627,
+                    "velocityX": -1.1276277091512135,
+                    "velocityY": -0.8668246863853641,
+                    "timestamp": 1.4294279422882057
+                },
+                {
+                    "x": 7.395116825349968,
+                    "y": 4.002723380018642,
+                    "heading": -3.1873733598366223,
+                    "angularVelocity": -0.19437806312174594,
+                    "velocityX": -1.7982114846386814,
+                    "velocityY": -0.4095350216243075,
+                    "timestamp": 1.5267959641722133
+                },
+                {
+                    "x": 7.155591929265255,
+                    "y": 4.008604211117546,
+                    "heading": -3.1999520195770272,
+                    "angularVelocity": -0.1291867647818698,
+                    "velocityX": -2.4599955041712693,
+                    "velocityY": 0.06039797240528109,
+                    "timestamp": 1.624163986056221
+                },
+                {
+                    "x": 6.8538087913529315,
+                    "y": 4.063163767369359,
+                    "heading": -3.206164046421751,
+                    "angularVelocity": -0.06379945617179969,
+                    "velocityX": -3.0994070956050406,
+                    "velocityY": 0.5603436857001141,
+                    "timestamp": 1.7215320079402285
+                },
+                {
+                    "x": 6.5048799215848865,
+                    "y": 4.181151469294334,
+                    "heading": -3.206164042303272,
+                    "angularVelocity": 4.229807303696183e-8,
+                    "velocityX": -3.5836084888703787,
+                    "velocityY": 1.2117705550753732,
+                    "timestamp": 1.8189000298242362
+                },
+                {
+                    "x": 6.189160518675962,
+                    "y": 4.3708716014574716,
+                    "heading": -3.2061640395158935,
+                    "angularVelocity": 2.862724812638766e-8,
+                    "velocityX": -3.2425368904488527,
+                    "velocityY": 1.9484850209768463,
+                    "timestamp": 1.9162680517082438
+                },
+                {
+                    "x": 5.875739114062726,
+                    "y": 4.564364477524865,
+                    "heading": -3.2061640367361566,
+                    "angularVelocity": 2.854876387634348e-8,
+                    "velocityX": -3.218935730116917,
+                    "velocityY": 1.987232279381165,
+                    "timestamp": 2.0136360735922514
+                },
+                {
+                    "x": 5.562317717200868,
+                    "y": 4.757857366148008,
+                    "heading": -3.20616403395642,
+                    "angularVelocity": 2.854876379789868e-8,
+                    "velocityX": -3.21893565050783,
+                    "velocityY": 1.9872324083326358,
+                    "timestamp": 2.111004095476259
+                },
+                {
+                    "x": 5.248896320339037,
+                    "y": 4.951350254771193,
+                    "heading": -3.2061640311766837,
+                    "angularVelocity": 2.8548763614821095e-8,
+                    "velocityX": -3.218935650507565,
+                    "velocityY": 1.9872324083330646,
+                    "timestamp": 2.208372117360266
+                },
+                {
+                    "x": 4.935474923469446,
+                    "y": 5.14484314338174,
+                    "heading": -3.2061640283969295,
+                    "angularVelocity": 2.8548942066572532e-8,
+                    "velocityX": -3.2189356505872584,
+                    "velocityY": 1.9872324082032717,
+                    "timestamp": 2.3057401392442736
+                },
+                {
+                    "x": 4.666484969375691,
+                    "y": 5.310905875767285,
+                    "heading": -3.180972420287378,
+                    "angularVelocity": 0.2587256844917891,
+                    "velocityX": -2.76261085404609,
+                    "velocityY": 1.7055161353012933,
+                    "timestamp": 2.403108161128281
+                },
+                {
+                    "x": 4.464742488596807,
+                    "y": 5.435452935258762,
+                    "heading": -3.1620788420473156,
+                    "angularVelocity": 0.19404295039052252,
+                    "velocityX": -2.071958296731311,
+                    "velocityY": 1.279137206256954,
+                    "timestamp": 2.5004761830122884
+                },
+                {
+                    "x": 4.330247497040812,
+                    "y": 5.518484311233343,
+                    "heading": -3.149483158535891,
+                    "angularVelocity": 0.12936160422802073,
+                    "velocityX": -1.3813055760362147,
+                    "velocityY": 0.8527581681128775,
+                    "timestamp": 2.5978442048962958
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": 0.0646809007150999,
+                    "velocityX": -0.6906528009876122,
+                    "velocityY": 0.4263790920607684,
+                    "timestamp": 2.695212226780303
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -3.400041679826015e-29,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 2.7925802486643105
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S1 To C3 To S3": {
+            "waypoints": [
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": -3.143185307179586,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 17
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": -3.7091853071795864,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "angularVelocity": 2.6904386896052926e-33,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.221431258578075,
+                    "y": 6.6088996918922325,
+                    "heading": -2.8866746127276346,
+                    "angularVelocity": -0.08608437747729661,
+                    "velocityX": 0.5859398548340703,
+                    "velocityY": -0.2721366710185261,
+                    "timestamp": 0.07753570303037556
+                },
+                {
+                    "x": 5.31229377552925,
+                    "y": 6.566699082350374,
+                    "heading": -2.90002400824425,
+                    "angularVelocity": -0.17217094828412036,
+                    "velocityX": 1.1718797070244922,
+                    "velocityY": -0.5442732559647441,
+                    "timestamp": 0.15507140606075112
+                },
+                {
+                    "x": 5.448587549407382,
+                    "y": 6.503398181602668,
+                    "heading": -2.920048533381972,
+                    "angularVelocity": -0.2582619922834353,
+                    "velocityX": 1.7578195405636137,
+                    "velocityY": -0.8164097089944291,
+                    "timestamp": 0.2326071090911267
+                },
+                {
+                    "x": 5.630312575309777,
+                    "y": 6.418997003981843,
+                    "heading": -2.946748763466125,
+                    "angularVelocity": -0.34436045641700236,
+                    "velocityX": 2.3437593108713033,
+                    "velocityY": -1.0885459771707946,
+                    "timestamp": 0.31014281212150224
+                },
+                {
+                    "x": 5.85746883610922,
+                    "y": 6.313495571165909,
+                    "heading": -2.9801255206908306,
+                    "angularVelocity": -0.43046952462183896,
+                    "velocityX": 2.929698860284435,
+                    "velocityY": -1.3606819657597289,
+                    "timestamp": 0.3876785151518778
+                },
+                {
+                    "x": 6.123490041046321,
+                    "y": 6.189943134833017,
+                    "heading": -2.9801255253705285,
+                    "angularVelocity": -6.035539216471427e-8,
+                    "velocityX": 3.4309510914331183,
+                    "velocityY": -1.5934908887649881,
+                    "timestamp": 0.46521421818225334
+                },
+                {
+                    "x": 6.3895112454442975,
+                    "y": 6.066390697338873,
+                    "heading": -2.980125530050055,
+                    "angularVelocity": -6.035318736040994e-8,
+                    "velocityX": 3.4309510844798714,
+                    "velocityY": -1.5934909037419929,
+                    "timestamp": 0.5427499212126289
+                },
+                {
+                    "x": 6.655532416724182,
+                    "y": 5.9428381885380706,
+                    "heading": -2.980125534729582,
+                    "angularVelocity": -6.035318830910377e-8,
+                    "velocityX": 3.4309506573464303,
+                    "velocityY": -1.5934918234042366,
+                    "timestamp": 0.6202856242430045
+                },
+                {
+                    "x": 6.916684100099049,
+                    "y": 5.8093006126159725,
+                    "heading": -2.9801255394461488,
+                    "angularVelocity": -6.083090321698073e-8,
+                    "velocityX": 3.368147487778084,
+                    "velocityY": -1.722272072128935,
+                    "timestamp": 0.6978213272733801
+                },
+                {
+                    "x": 7.151284630214051,
+                    "y": 5.6332464271795715,
+                    "heading": -2.980125544587418,
+                    "angularVelocity": -6.630841173330857e-8,
+                    "velocityX": 3.0257097175361216,
+                    "velocityY": -2.2706208695551444,
+                    "timestamp": 0.7753570303037557
+                },
+                {
+                    "x": 7.352481762263237,
+                    "y": 5.41981682210962,
+                    "heading": -2.9801255509932516,
+                    "angularVelocity": -8.261785689390399e-8,
+                    "velocityX": 2.594896598414324,
+                    "velocityY": -2.7526622798059472,
+                    "timestamp": 0.8528927333341313
+                },
+                {
+                    "x": 7.514399141163384,
+                    "y": 5.175245297922962,
+                    "heading": -2.9801256025732923,
+                    "angularVelocity": -6.652424424834864e-7,
+                    "velocityX": 2.0882944575444746,
+                    "velocityY": -3.1543084621396242,
+                    "timestamp": 0.9304284363645069
+                },
+                {
+                    "x": 7.630164379984533,
+                    "y": 4.911565821303128,
+                    "heading": -2.995124870089402,
+                    "angularVelocity": -0.19344981640565623,
+                    "velocityX": 1.4930571890964461,
+                    "velocityY": -3.4007491557345486,
+                    "timestamp": 1.0079641393948824
+                },
+                {
+                    "x": 7.696376897938806,
+                    "y": 4.643893289420577,
+                    "heading": -3.0332372938699343,
+                    "angularVelocity": -0.49154676221354465,
+                    "velocityX": 0.8539616636781268,
+                    "velocityY": -3.452248724406194,
+                    "timestamp": 1.085499842425258
+                },
+                {
+                    "x": 7.712814001135022,
+                    "y": 4.380911639847311,
+                    "heading": -3.084305372604304,
+                    "angularVelocity": -0.6586395265464133,
+                    "velocityX": 0.2119939918488342,
+                    "velocityY": -3.3917490819711635,
+                    "timestamp": 1.1630355454556336
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -0.7593912516949146,
+                    "velocityX": -0.4232114993806923,
+                    "velocityY": -3.274770588561481,
+                    "timestamp": 1.2405712484860092
+                },
+                {
+                    "x": 7.5963416463862465,
+                    "y": 3.881029306471945,
+                    "heading": -3.207914317977363,
+                    "angularVelocity": -0.8216221979368662,
+                    "velocityX": -1.061897277970947,
+                    "velocityY": -3.1221700958161,
+                    "timestamp": 1.3193532129464565
+                },
+                {
+                    "x": 7.463114472360868,
+                    "y": 3.64992032438806,
+                    "heading": -3.2707770828701133,
+                    "angularVelocity": -0.7979334524504146,
+                    "velocityX": -1.6910872296446224,
+                    "velocityY": -2.933526520526327,
+                    "timestamp": 1.3981351774069037
+                },
+                {
+                    "x": 7.282107143226341,
+                    "y": 3.438334424773196,
+                    "heading": -3.3202070228970326,
+                    "angularVelocity": -0.627427106768014,
+                    "velocityX": -2.297573186632101,
+                    "velocityY": -2.6857149483888696,
+                    "timestamp": 1.476917141867351
+                },
+                {
+                    "x": 7.057858991686637,
+                    "y": 3.2538746449727443,
+                    "heading": -3.3390490142356852,
+                    "angularVelocity": -0.23916630497468983,
+                    "velocityX": -2.846440210973517,
+                    "velocityY": -2.341396042403339,
+                    "timestamp": 1.5556991063277983
+                },
+                {
+                    "x": 6.798499862682402,
+                    "y": 3.1070645806670387,
+                    "heading": -3.339049130726268,
+                    "angularVelocity": -0.0000014786453189461519,
+                    "velocityX": -3.2921129954108745,
+                    "velocityY": -1.8634983947298314,
+                    "timestamp": 1.6344810707882456
+                },
+                {
+                    "x": 6.517654165139062,
+                    "y": 3.0073345934329407,
+                    "heading": -3.339049157320925,
+                    "angularVelocity": -3.3757290550660805e-7,
+                    "velocityX": -3.564847607783876,
+                    "velocityY": -1.26589871066451,
+                    "timestamp": 1.7132630352486928
+                },
+                {
+                    "x": 6.223790307061648,
+                    "y": 2.9576917507277325,
+                    "heading": -3.339049179064471,
+                    "angularVelocity": -2.7599649426865103e-7,
+                    "velocityX": -3.730090511070579,
+                    "velocityY": -0.6301295359311817,
+                    "timestamp": 1.79204499970914
+                },
+                {
+                    "x": 5.925769134173891,
+                    "y": 2.9596309032298342,
+                    "heading": -3.3390491994000007,
+                    "angularVelocity": -2.581241798261079e-7,
+                    "velocityX": -3.7828603910655114,
+                    "velocityY": 0.024614167917522748,
+                    "timestamp": 1.8708269641695874
+                },
+                {
+                    "x": 5.627822525147453,
+                    "y": 2.9665733818436117,
+                    "heading": -3.339049219680213,
+                    "angularVelocity": -2.574220136164825e-7,
+                    "velocityX": -3.781913932547634,
+                    "velocityY": 0.08812268977200981,
+                    "timestamp": 1.9496089286300347
+                },
+                {
+                    "x": 5.329875916862242,
+                    "y": 2.9735158922681233,
+                    "heading": -3.3390492399604246,
+                    "angularVelocity": -2.5742201457831597e-7,
+                    "velocityX": -3.7819139231390566,
+                    "velocityY": 0.08812309355394705,
+                    "timestamp": 2.028390893090482
+                },
+                {
+                    "x": 5.031929325692052,
+                    "y": 2.9804591371189906,
+                    "heading": -3.3390492602433595,
+                    "angularVelocity": -2.5745657158705766e-7,
+                    "velocityX": -3.781913705893629,
+                    "velocityY": 0.08813241581901742,
+                    "timestamp": 2.107172857550929
+                },
+                {
+                    "x": 4.775295960622385,
+                    "y": 2.98713582308431,
+                    "heading": -3.452004590816206,
+                    "angularVelocity": -1.4337714392683885,
+                    "velocityX": -3.2575141636447755,
+                    "velocityY": 0.08474891443804398,
+                    "timestamp": 2.1859548220113765
+                },
+                {
+                    "x": 4.570259992018404,
+                    "y": 2.9923299087295048,
+                    "heading": -3.5517317371257473,
+                    "angularVelocity": -1.2658626500689703,
+                    "velocityX": -2.6025749676110213,
+                    "velocityY": 0.06592988231212166,
+                    "timestamp": 2.2647367864718237
+                },
+                {
+                    "x": 4.416588409818771,
+                    "y": 2.996180871589336,
+                    "heading": -3.6294028651739794,
+                    "angularVelocity": -0.9858998640130027,
+                    "velocityX": -1.9505934289920475,
+                    "velocityY": 0.04888127487306719,
+                    "timestamp": 2.343518750932271
+                },
+                {
+                    "x": 4.314185287460311,
+                    "y": 2.998730700935922,
+                    "heading": -3.6823213519430347,
+                    "angularVelocity": -0.6717081394387356,
+                    "velocityX": -1.2998295112312361,
+                    "velocityY": 0.03236564820448933,
+                    "timestamp": 2.4223007153927183
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": -3.7091853071795864,
+                    "angularVelocity": -0.3409911826968791,
+                    "velocityX": -0.6497081890615813,
+                    "velocityY": 0.01611154371144828,
+                    "timestamp": 2.5010826798531656
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": -3.7091853071795864,
+                    "angularVelocity": 0,
+                    "velocityX": -2.823998043085552e-35,
+                    "velocityY": 0,
+                    "timestamp": 2.579864644313613
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S1 To C3 To S4": {
+            "waypoints": [
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": -3.143185307179586,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 18
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": -3.7831853071795862,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "angularVelocity": -4.965302317163387e-33,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.214448464194176,
+                    "y": 6.6065493619277555,
+                    "heading": -2.899983532805956,
+                    "angularVelocity": -0.2708663369562031,
+                    "velocityX": 0.5211488258354376,
+                    "velocityY": -0.31786113574578073,
+                    "timestamp": 0.0737763615461288
+                },
+                {
+                    "x": 5.29140014559419,
+                    "y": 6.559619691972876,
+                    "heading": -2.9381129725152273,
+                    "angularVelocity": -0.5168246157738562,
+                    "velocityX": 1.0430398001113212,
+                    "velocityY": -0.636107134743106,
+                    "timestamp": 0.1475527230922576
+                },
+                {
+                    "x": 5.4069285579459345,
+                    "y": 6.489171539636289,
+                    "heading": -2.991543589570423,
+                    "angularVelocity": -0.7242240730696432,
+                    "velocityX": 1.5659272147693208,
+                    "velocityY": -0.9548878646250154,
+                    "timestamp": 0.22132908463838638
+                },
+                {
+                    "x": 5.5611322195100845,
+                    "y": 6.395152264966145,
+                    "heading": -3.0552740925010653,
+                    "angularVelocity": -0.8638336398684346,
+                    "velocityX": 2.090149992931468,
+                    "velocityY": -1.274382101526624,
+                    "timestamp": 0.2951054461845152
+                },
+                {
+                    "x": 5.754097394020428,
+                    "y": 6.277527348774539,
+                    "heading": -3.118382886347054,
+                    "angularVelocity": -0.8554066983437507,
+                    "velocityX": 2.615542030894161,
+                    "velocityY": -1.5943442279687556,
+                    "timestamp": 0.36888180773064394
+                },
+                {
+                    "x": 5.984744637231227,
+                    "y": 6.137241174757932,
+                    "heading": -3.141361477565195,
+                    "angularVelocity": -0.3114627874915312,
+                    "velocityX": 3.126302766592624,
+                    "velocityY": -1.9015057272632323,
+                    "timestamp": 0.4426581692767727
+                },
+                {
+                    "x": 6.222972456839802,
+                    "y": 5.99184648556835,
+                    "heading": -3.141361501066067,
+                    "angularVelocity": -3.1854203464323085e-7,
+                    "velocityX": 3.2290535154627107,
+                    "velocityY": -1.9707489789757882,
+                    "timestamp": 0.5164345308229015
+                },
+                {
+                    "x": 6.461200274584459,
+                    "y": 5.84645179332473,
+                    "heading": -3.141361524566912,
+                    "angularVelocity": -3.1854166338718743e-7,
+                    "velocityX": 3.229053490198278,
+                    "velocityY": -1.9707490203716698,
+                    "timestamp": 0.5902108923690302
+                },
+                {
+                    "x": 6.699428092323813,
+                    "y": 5.701057101072421,
+                    "heading": -3.141361548067757,
+                    "angularVelocity": -3.1854166408242085e-7,
+                    "velocityX": 3.229053490126394,
+                    "velocityY": -1.9707490204894516,
+                    "timestamp": 0.663987253915159
+                },
+                {
+                    "x": 6.937655250211389,
+                    "y": 5.555661327664803,
+                    "heading": -3.1413615715686114,
+                    "angularVelocity": -3.1854179676885927e-7,
+                    "velocityX": 3.229044546180602,
+                    "velocityY": -1.9707636749842845,
+                    "timestamp": 0.7377636154612878
+                },
+                {
+                    "x": 7.1538386137488,
+                    "y": 5.379148261525069,
+                    "heading": -3.141361595947866,
+                    "angularVelocity": -3.304480485706385e-7,
+                    "velocityX": 2.9302524413899382,
+                    "velocityY": -2.3925423054289428,
+                    "timestamp": 0.8115399770074165
+                },
+                {
+                    "x": 7.338555522796062,
+                    "y": 5.1699312191681095,
+                    "heading": -3.1413616242387703,
+                    "angularVelocity": -3.834684085405442e-7,
+                    "velocityX": 2.5037411059064913,
+                    "velocityY": -2.835827600770833,
+                    "timestamp": 0.8853163385535453
+                },
+                {
+                    "x": 7.486921372318085,
+                    "y": 4.933542391468771,
+                    "heading": -3.14136166220073,
+                    "angularVelocity": -5.145545098256939e-7,
+                    "velocityX": 2.011021503537517,
+                    "velocityY": -3.2041269418191103,
+                    "timestamp": 0.9590927000996741
+                },
+                {
+                    "x": 7.595012870290821,
+                    "y": 4.676232612674377,
+                    "heading": -3.1413617253677457,
+                    "angularVelocity": -8.561958656088552e-7,
+                    "velocityX": 1.4651237294367088,
+                    "velocityY": -3.4876994934686665,
+                    "timestamp": 1.032869061645803
+                },
+                {
+                    "x": 7.659971709721358,
+                    "y": 4.404806001492882,
+                    "heading": -3.141361954512953,
+                    "angularVelocity": -0.000003105943453461285,
+                    "velocityX": 0.8804830987757537,
+                    "velocityY": -3.679045774191308,
+                    "timestamp": 1.1066454231919318
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -0.02471459188852694,
+                    "velocityX": 0.271473001092883,
+                    "velocityY": -3.7655150738110477,
+                    "timestamp": 1.1804217847380607
+                },
+                {
+                    "x": 7.658280036401298,
+                    "y": 3.864604229609803,
+                    "heading": -3.146329645491869,
+                    "angularVelocity": -0.04501004755691485,
+                    "velocityX": -0.3109132979403334,
+                    "velocityY": -3.756099036118669,
+                    "timestamp": 1.2502803714970885
+                },
+                {
+                    "x": 7.5962523104298665,
+                    "y": 3.6077169181356963,
+                    "heading": -3.1463327455058754,
+                    "angularVelocity": -0.00004437556140498802,
+                    "velocityX": -0.8879041052661434,
+                    "velocityY": -3.677247470811584,
+                    "timestamp": 1.3201389582561163
+                },
+                {
+                    "x": 7.495521811257821,
+                    "y": 3.363396565109804,
+                    "heading": -3.1463328962340493,
+                    "angularVelocity": -0.0000021576184275855436,
+                    "velocityX": -1.4419200823444587,
+                    "velocityY": -3.4973560783394904,
+                    "timestamp": 1.389997545015144
+                },
+                {
+                    "x": 7.358477102372377,
+                    "y": 3.1374367675877326,
+                    "heading": -3.146332979999805,
+                    "angularVelocity": -0.000001199076011915671,
+                    "velocityX": -1.9617446507781449,
+                    "velocityY": -3.234531472866801,
+                    "timestamp": 1.4598561317741718
+                },
+                {
+                    "x": 7.188367445952169,
+                    "y": 2.9351948516310506,
+                    "heading": -3.1463330407624492,
+                    "angularVelocity": -8.697949218469567e-7,
+                    "velocityX": -2.4350572250622284,
+                    "velocityY": -2.895018713365922,
+                    "timestamp": 1.5297147185331996
+                },
+                {
+                    "x": 6.989226108875858,
+                    "y": 2.761465700266129,
+                    "heading": -3.146333092561237,
+                    "angularVelocity": -7.414806172090105e-7,
+                    "velocityX": -2.8506350659974578,
+                    "velocityY": -2.486868965216661,
+                    "timestamp": 1.5995733052922274
+                },
+                {
+                    "x": 6.765782539563676,
+                    "y": 2.620355593747368,
+                    "heading": -3.14633314264619,
+                    "angularVelocity": -7.169476958064807e-7,
+                    "velocityX": -3.19851259062737,
+                    "velocityY": -2.0199393240734156,
+                    "timestamp": 1.6694318920512552
+                },
+                {
+                    "x": 6.5419983548650125,
+                    "y": 2.479786288735506,
+                    "heading": -3.1463331927321803,
+                    "angularVelocity": -7.169625493020222e-7,
+                    "velocityX": -3.203388374726096,
+                    "velocityY": -2.012197949219134,
+                    "timestamp": 1.739290478810283
+                },
+                {
+                    "x": 6.318214167123251,
+                    "y": 2.339216988568208,
+                    "heading": -3.1463332428181707,
+                    "angularVelocity": -7.16962547152982e-7,
+                    "velocityX": -3.2033884182869192,
+                    "velocityY": -2.012197879870968,
+                    "timestamp": 1.8091490655693108
+                },
+                {
+                    "x": 6.094429979379188,
+                    "y": 2.198647688404573,
+                    "heading": -3.1463332929041607,
+                    "angularVelocity": -7.1696254162218e-7,
+                    "velocityX": -3.2033884183198493,
+                    "velocityY": -2.0121978798185434,
+                    "timestamp": 1.8790076523283386
+                },
+                {
+                    "x": 5.870645537599717,
+                    "y": 2.05807879266491,
+                    "heading": -3.1463333429939095,
+                    "angularVelocity": -7.170163458750797e-7,
+                    "velocityX": -3.203392054743378,
+                    "velocityY": -2.012192090637989,
+                    "timestamp": 1.9488662390873663
+                },
+                {
+                    "x": 5.673381673927696,
+                    "y": 1.935533968246103,
+                    "heading": -3.225487827303808,
+                    "angularVelocity": -1.1330673576740993,
+                    "velocityX": -2.8237597240904497,
+                    "velocityY": -1.754184132603728,
+                    "timestamp": 2.018724825846394
+                },
+                {
+                    "x": 5.505993389711308,
+                    "y": 1.8298743063416731,
+                    "heading": -3.3794217717080413,
+                    "angularVelocity": -2.2035078512999076,
+                    "velocityX": -2.396101781929577,
+                    "velocityY": -1.512479235643509,
+                    "timestamp": 2.088583412605422
+                },
+                {
+                    "x": 5.373136379336833,
+                    "y": 1.745695395482932,
+                    "heading": -3.5283087444702135,
+                    "angularVelocity": -2.1312623067475838,
+                    "velocityX": -1.9017992853585055,
+                    "velocityY": -1.2049901775011624,
+                    "timestamp": 2.15844199936445
+                },
+                {
+                    "x": 5.274205775679982,
+                    "y": 1.6827613854161347,
+                    "heading": -3.651053741383974,
+                    "angularVelocity": -1.7570495283158336,
+                    "velocityX": -1.4161552394139385,
+                    "velocityY": -0.9008772290782794,
+                    "timestamp": 2.228300586123478
+                },
+                {
+                    "x": 5.2086365166677675,
+                    "y": 1.6409004652612855,
+                    "heading": -3.73798062119163,
+                    "angularVelocity": -1.2443263432669185,
+                    "velocityX": -0.9385998494128075,
+                    "velocityY": -0.5992236902708281,
+                    "timestamp": 2.298159172882506
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": -3.7831853071795862,
+                    "angularVelocity": -0.6470884695089293,
+                    "velocityX": -0.46717974384944866,
+                    "velocityY": -0.29918248036394063,
+                    "timestamp": 2.368017759641534
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": -3.7831853071795862,
+                    "angularVelocity": 0,
+                    "velocityX": -3.585209340915008e-32,
+                    "velocityY": 0,
+                    "timestamp": 2.437876346400562
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S2 To W1": {
+            "waypoints": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 3.5331853071795862,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": -6.652842580111428e-35,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.215224473336601,
+                    "y": 5.589131466417677,
+                    "heading": 3.1531161748208096,
+                    "angularVelocity": 0.15993673271356004,
+                    "velocityX": -0.5825678402891398,
+                    "velocityY": 0.3552248747558748,
+                    "timestamp": 0.08200852048353235
+                },
+                {
+                    "x": 4.119673422721879,
+                    "y": 5.647394502413024,
+                    "heading": 3.179344235647322,
+                    "angularVelocity": 0.3198211682379922,
+                    "velocityX": -1.1651356475076162,
+                    "velocityY": 0.7104510074297316,
+                    "timestamp": 0.1640170409670647
+                },
+                {
+                    "x": 3.9763468590229047,
+                    "y": 5.73478928117174,
+                    "heading": 3.2186777846940484,
+                    "angularVelocity": 0.47962759009443284,
+                    "velocityX": -1.7477033222146137,
+                    "velocityY": 1.065679251904866,
+                    "timestamp": 0.24602556145059706
+                },
+                {
+                    "x": 3.785244828834372,
+                    "y": 5.85131604858519,
+                    "heading": 3.2711097941834586,
+                    "angularVelocity": 0.6393483162513433,
+                    "velocityX": -2.3302704287526703,
+                    "velocityY": 1.420910494743635,
+                    "timestamp": 0.3280340819341294
+                },
+                {
+                    "x": 3.546367452965707,
+                    "y": 5.996975111850807,
+                    "heading": 3.3366347863626045,
+                    "angularVelocity": 0.7990022474835781,
+                    "velocityX": -2.9128360621581053,
+                    "velocityY": 1.7761454835033377,
+                    "timestamp": 0.4100426024176618
+                },
+                {
+                    "x": 3.281494477712196,
+                    "y": 6.158489941563481,
+                    "heading": 3.336634794646938,
+                    "angularVelocity": 1.0101796436143703e-7,
+                    "velocityX": -3.2298226293047043,
+                    "velocityY": 1.9694883990146768,
+                    "timestamp": 0.4920511229011941
+                },
+                {
+                    "x": 3.016621502775785,
+                    "y": 6.320004771796177,
+                    "heading": 3.336634802931272,
+                    "angularVelocity": 1.0101796370003125e-7,
+                    "velocityX": -3.2298226254380333,
+                    "velocityY": 1.969488405355746,
+                    "timestamp": 0.5740596433847265
+                },
+                {
+                    "x": 2.7777474341519097,
+                    "y": 6.465669636134063,
+                    "heading": 3.402143395936758,
+                    "angularVelocity": 0.7988022783393678,
+                    "velocityX": -2.912795734094993,
+                    "velocityY": 1.7762162209368904,
+                    "timestamp": 0.6560681638682588
+                },
+                {
+                    "x": 2.586648366744574,
+                    "y": 6.582201701925324,
+                    "heading": 3.4545567132875594,
+                    "angularVelocity": 0.6391203870252214,
+                    "velocityX": -2.3302343010286215,
+                    "velocityY": 1.4209751023939097,
+                    "timestamp": 0.7380766843517912
+                },
+                {
+                    "x": 2.4433241632679117,
+                    "y": 6.669600830029101,
+                    "heading": 3.4938702766455694,
+                    "angularVelocity": 0.4793838875059847,
+                    "velocityX": -1.7476745420061834,
+                    "velocityY": 1.0657322871875106,
+                    "timestamp": 0.8200852048353235
+                },
+                {
+                    "x": 2.3477747237681617,
+                    "y": 6.727866941749082,
+                    "heading": 3.5200804322455714,
+                    "angularVelocity": 0.3196028345038231,
+                    "velocityX": -1.1651160018054092,
+                    "velocityY": 0.7104885123696424,
+                    "timestamp": 0.9020937253188559
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 3.5331853071795862,
+                    "angularVelocity": 0.15979894353351334,
+                    "velocityX": -0.5825580499011068,
+                    "velocityY": 0.35524428533945085,
+                    "timestamp": 0.9841022458023883
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 3.5331853071795862,
+                    "angularVelocity": 2.6358633635199364e-33,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 1.0661107662859206
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S2 To W2": {
+            "waypoints": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 10
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": -4.0059408516052846e-40,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.181096344253034,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 1.5012541318382292e-16,
+                    "velocityX": -0.8263919813283321,
+                    "velocityY": -1.2317462001949155e-21,
+                    "timestamp": 0.09910993523353699
+                },
+                {
+                    "x": 4.017289035836238,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 3.002363652724146e-16,
+                    "velocityX": -1.6527839316089836,
+                    "velocityY": -2.463492548222639e-21,
+                    "timestamp": 0.19821987046707398
+                },
+                {
+                    "x": 3.771578080903875,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 4.503226484774511e-16,
+                    "velocityX": -2.4791758197942855,
+                    "velocityY": -3.695244043691227e-21,
+                    "timestamp": 0.29732980570061096
+                },
+                {
+                    "x": 3.4439634979187304,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 6.003547774343467e-16,
+                    "velocityX": -3.305567521693684,
+                    "velocityY": -4.926945384758097e-21,
+                    "timestamp": 0.39643974093414797
+                },
+                {
+                    "x": 3.0690365020812695,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 4.535737431947664e-16,
+                    "velocityX": -3.7829405796099507,
+                    "velocityY": 2.4634613990171587e-20,
+                    "timestamp": 0.49554967616768497
+                },
+                {
+                    "x": 2.741421919096125,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 5.990674751034296e-16,
+                    "velocityX": -3.305567521693684,
+                    "velocityY": -4.926861067570722e-21,
+                    "timestamp": 0.5946596114012219
+                },
+                {
+                    "x": 2.4957109641637625,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 4.492129826743599e-16,
+                    "velocityX": -2.479175819794286,
+                    "velocityY": -3.695159726389389e-21,
+                    "timestamp": 0.6937695466347589
+                },
+                {
+                    "x": 2.3319036557469652,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 2.9945516157260173e-16,
+                    "velocityX": -1.6527839316089836,
+                    "velocityY": -2.4634409378754245e-21,
+                    "timestamp": 0.7928794818682958
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 1.4972226131077696e-16,
+                    "velocityX": -0.8263919813283321,
+                    "velocityY": -1.2317240814696432e-21,
+                    "timestamp": 0.8919894171018328
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 2.546248641381176e-40,
+                    "timestamp": 0.9910993523353697
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S2 To W3": {
+            "waypoints": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": -1.339537382865945e-32,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.215219135134553,
+                    "y": 5.5307922107130985,
+                    "heading": 3.1269908374623143,
+                    "angularVelocity": -0.1585720754034668,
+                    "velocityX": -0.582413424718004,
+                    "velocityY": -0.3560213620019152,
+                    "timestamp": 0.08203942910241563
+                },
+                {
+                    "x": 4.119657409619225,
+                    "y": 5.472376532354226,
+                    "heading": 3.100976599590504,
+                    "angularVelocity": -0.3170943308166476,
+                    "velocityX": -1.1648267980513523,
+                    "velocityY": -0.7120439403100625,
+                    "timestamp": 0.16407885820483126
+                },
+                {
+                    "x": 3.976314836365869,
+                    "y": 5.384752798145142,
+                    "heading": 3.0619633632091103,
+                    "angularVelocity": -0.4755425142304533,
+                    "velocityX": -1.7472400139987678,
+                    "velocityY": -1.0680685515217063,
+                    "timestamp": 0.24611828730724689
+                },
+                {
+                    "x": 3.785191463205027,
+                    "y": 5.267920772243767,
+                    "heading": 3.009957765442303,
+                    "angularVelocity": -0.6339097960065634,
+                    "velocityX": -2.3296526469272343,
+                    "velocityY": -1.4240960374739504,
+                    "timestamp": 0.3281577164096625
+                },
+                {
+                    "x": 3.546287409634579,
+                    "y": 5.121880161282017,
+                    "heading": 2.944964910348519,
+                    "angularVelocity": -0.7922148630830776,
+                    "velocityX": -2.9120638232648712,
+                    "velocityY": -1.780127099366288,
+                    "timestamp": 0.41019714551207814
+                },
+                {
+                    "x": 3.281494622858476,
+                    "y": 4.960009610093287,
+                    "heading": 2.9449649021355118,
+                    "angularVelocity": -1.0011048865301711e-7,
+                    "velocityX": -3.227628344969881,
+                    "velocityY": -1.9730823697792366,
+                    "timestamp": 0.49223657461449377
+                },
+                {
+                    "x": 3.0167018363816,
+                    "y": 4.798139058415072,
+                    "heading": 2.9449648939225046,
+                    "angularVelocity": -1.0011048873886247e-7,
+                    "velocityX": -3.2276283413225157,
+                    "velocityY": -1.9730823757457077,
+                    "timestamp": 0.5742760037169095
+                },
+                {
+                    "x": 2.777800999846863,
+                    "y": 4.652092907401912,
+                    "heading": 2.879984644909019,
+                    "angularVelocity": -0.7920612042822246,
+                    "velocityX": -2.912024609977479,
+                    "velocityY": -1.7801946284979562,
+                    "timestamp": 0.656315432819325
+                },
+                {
+                    "x": 2.58668051019359,
+                    "y": 4.5352558204412405,
+                    "heading": 2.827994144000888,
+                    "angularVelocity": -0.6337257764583869,
+                    "velocityX": -2.329617499101367,
+                    "velocityY": -1.424157728045816,
+                    "timestamp": 0.7383548619217406
+                },
+                {
+                    "x": 2.443340235924958,
+                    "y": 4.447627930217013,
+                    "heading": 2.788997784467408,
+                    "angularVelocity": -0.4753367979291705,
+                    "velocityX": -1.7472119910743198,
+                    "velocityY": -1.0681192102743058,
+                    "timestamp": 0.8203942910241562
+                },
+                {
+                    "x": 2.3477800812496348,
+                    "y": 4.389209311628481,
+                    "heading": 2.7629991294364147,
+                    "angularVelocity": -0.31690438750539845,
+                    "velocityX": -1.1648076506728071,
+                    "velocityY": -0.7120797795363016,
+                    "timestamp": 0.9024337201265717
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": -0.1584497793150997,
+                    "velocityX": -0.5824038730204719,
+                    "velocityY": -0.35603991822050945,
+                    "timestamp": 0.9844731492289873
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": -7.396671024736362e-34,
+                    "velocityX": -1.1837806823878881e-33,
+                    "velocityY": 0,
+                    "timestamp": 1.066512578331403
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S2 To C1 To S1": {
+            "waypoints": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": -8.153039521410402e-32,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.331992841648679,
+                    "y": 5.5918650858232235,
+                    "heading": 3.133856142863887,
+                    "angularVelocity": -0.06434641956265956,
+                    "velocityX": 0.7225822862075633,
+                    "velocityY": 0.3337323991032911,
+                    "timestamp": 0.09548094793574066
+                },
+                {
+                    "x": 4.469978522807239,
+                    "y": 5.655595256777472,
+                    "heading": 3.121568622421363,
+                    "angularVelocity": -0.12869080908995395,
+                    "velocityX": 1.445164550014868,
+                    "velocityY": 0.6674647909569773,
+                    "timestamp": 0.19096189587148132
+                },
+                {
+                    "x": 4.676957039051013,
+                    "y": 5.751190511387417,
+                    "heading": 3.1031377745555058,
+                    "angularVelocity": -0.19303168081514768,
+                    "velocityX": 2.1677467674813493,
+                    "velocityY": 1.0011971673591054,
+                    "timestamp": 0.28644284380722196
+                },
+                {
+                    "x": 4.9529283757083125,
+                    "y": 5.878650843878014,
+                    "heading": 3.078564198952369,
+                    "angularVelocity": -0.2573662718522563,
+                    "velocityX": 2.890328831286909,
+                    "velocityY": 1.334929483277519,
+                    "timestamp": 0.38192379174296265
+                },
+                {
+                    "x": 5.280841898732695,
+                    "y": 6.030101358943919,
+                    "heading": 3.0785641960647663,
+                    "angularVelocity": -3.024270852224024e-8,
+                    "velocityX": 3.4343345988250538,
+                    "velocityY": 1.5861857086708926,
+                    "timestamp": 0.47740473967870334
+                },
+                {
+                    "x": 5.608755421663104,
+                    "y": 6.181551874214712,
+                    "heading": 3.078564193177445,
+                    "angularVelocity": -3.023976799325844e-8,
+                    "velocityX": 3.434334597840841,
+                    "velocityY": 1.5861857108167463,
+                    "timestamp": 0.572885687614444
+                },
+                {
+                    "x": 5.936668944593511,
+                    "y": 6.333002389485506,
+                    "heading": 3.0785641902901233,
+                    "angularVelocity": -3.023976796721421e-8,
+                    "velocityX": 3.434334597840832,
+                    "velocityY": 1.5861857108167654,
+                    "timestamp": 0.6683666355501847
+                },
+                {
+                    "x": 6.26458246752392,
+                    "y": 6.484452904756295,
+                    "heading": 3.078564187402802,
+                    "angularVelocity": -3.023976775003668e-8,
+                    "velocityX": 3.434334597840855,
+                    "velocityY": 1.5861857108167163,
+                    "timestamp": 0.7638475834859254
+                },
+                {
+                    "x": 6.592495990360559,
+                    "y": 6.635903420230111,
+                    "heading": 3.0785641845154803,
+                    "angularVelocity": -3.02397677343441e-8,
+                    "velocityX": 3.434334596858777,
+                    "velocityY": 1.586185712943067,
+                    "timestamp": 0.8593285314216661
+                },
+                {
+                    "x": 6.920387522031495,
+                    "y": 6.787401540858459,
+                    "heading": 3.0785641816279576,
+                    "angularVelocity": -3.0241868882472936e-8,
+                    "velocityX": 3.4341042769240095,
+                    "velocityY": 1.5866842957027985,
+                    "timestamp": 0.9548094793574068
+                },
+                {
+                    "x": 7.210301641874205,
+                    "y": 6.987038475991185,
+                    "heading": 3.078564386301996,
+                    "angularVelocity": 0.000002143611293414107,
+                    "velocityX": 3.036355693052319,
+                    "velocityY": 2.0908562330876395,
+                    "timestamp": 1.0502904272931475
+                },
+                {
+                    "x": 7.431218296435779,
+                    "y": 7.154820398489436,
+                    "heading": 3.084707768304825,
+                    "angularVelocity": 0.06434144335228988,
+                    "velocityX": 2.3137249821846093,
+                    "velocityY": 1.757229333447492,
+                    "timestamp": 1.145771375228888
+                },
+                {
+                    "x": 7.58313979167575,
+                    "y": 7.2907422724167805,
+                    "heading": 3.096994605047932,
+                    "angularVelocity": 0.12868364850522696,
+                    "velocityX": 1.5911184223080599,
+                    "velocityY": 1.4235496909665903,
+                    "timestamp": 1.2412523231646286
+                },
+                {
+                    "x": 7.666066897511305,
+                    "y": 7.39480241917028,
+                    "heading": 3.11542521266587,
+                    "angularVelocity": 0.19302916462815903,
+                    "velocityX": 0.8685199260002638,
+                    "velocityY": 1.0898524679843054,
+                    "timestamp": 1.3367332711003692
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "angularVelocity": 0.2573789626666942,
+                    "velocityX": 0.145925472986578,
+                    "velocityY": 0.756146460527717,
+                    "timestamp": 1.4322142190361098
+                },
+                {
+                    "x": 7.628765464766867,
+                    "y": 7.5070371939069025,
+                    "heading": 3.1695683378543595,
+                    "angularVelocity": 0.3197138219543324,
+                    "velocityX": -0.5539841013770347,
+                    "velocityY": 0.4329105121655262,
+                    "timestamp": 1.5246979836528902
+                },
+                {
+                    "x": 7.512802856979312,
+                    "y": 7.517175485453599,
+                    "heading": 3.204897367018404,
+                    "angularVelocity": 0.3820024986055476,
+                    "velocityX": -1.25386989022506,
+                    "velocityY": 0.10962239252574857,
+                    "timestamp": 1.6171817482696706
+                },
+                {
+                    "x": 7.3321158425333,
+                    "y": 7.497406833459743,
+                    "heading": 3.2459797295297097,
+                    "angularVelocity": 0.44421161575322776,
+                    "velocityX": -1.9537160408082612,
+                    "velocityY": -0.21375267405386408,
+                    "timestamp": 1.709665512886451
+                },
+                {
+                    "x": 7.086711752516261,
+                    "y": 7.447715159830384,
+                    "heading": 3.292800204439529,
+                    "angularVelocity": 0.5062561532175872,
+                    "velocityX": -2.6534829224793675,
+                    "velocityY": -0.5373015883889657,
+                    "timestamp": 1.8021492775032315
+                },
+                {
+                    "x": 6.7766125854356485,
+                    "y": 7.3680522721143955,
+                    "heading": 3.345311930276191,
+                    "angularVelocity": 0.5677939912444252,
+                    "velocityX": -3.3530119406998606,
+                    "velocityY": -0.8613715936546359,
+                    "timestamp": 1.894633042120012
+                },
+                {
+                    "x": 6.4587265246711105,
+                    "y": 7.22193286725247,
+                    "heading": 3.3453119331318786,
+                    "angularVelocity": 3.087771911901371e-8,
+                    "velocityX": -3.437209353240929,
+                    "velocityY": -1.5799465502853158,
+                    "timestamp": 1.9871168067367924
+                },
+                {
+                    "x": 6.1410584209016115,
+                    "y": 7.075340220973225,
+                    "heading": 3.3453119359389976,
+                    "angularVelocity": 3.035256115645747e-8,
+                    "velocityX": -3.434852647767093,
+                    "velocityY": -1.5850635718263661,
+                    "timestamp": 2.079600571353573
+                },
+                {
+                    "x": 5.823390305014834,
+                    "y": 6.928747600950052,
+                    "heading": 3.3453119387456907,
+                    "angularVelocity": 3.034795349843415e-8,
+                    "velocityX": -3.4348527787877012,
+                    "velocityY": -1.5850632879271098,
+                    "timestamp": 2.1720843359703537
+                },
+                {
+                    "x": 5.564434200353795,
+                    "y": 6.809248569525128,
+                    "heading": 3.3684615293972238,
+                    "angularVelocity": 0.2503097786643198,
+                    "velocityX": -2.8000169082002904,
+                    "velocityY": -1.292108208616342,
+                    "timestamp": 2.2645681005871343
+                },
+                {
+                    "x": 5.3702171046964775,
+                    "y": 6.719624287090389,
+                    "heading": 3.3858234872314386,
+                    "angularVelocity": 0.18772979134186565,
+                    "velocityX": -2.1000128667129405,
+                    "velocityY": -0.9690812523282153,
+                    "timestamp": 2.357051865203915
+                },
+                {
+                    "x": 5.240739035958368,
+                    "y": 6.659874762854457,
+                    "heading": 3.3973980568576057,
+                    "angularVelocity": 0.1251524489081645,
+                    "velocityX": -1.4000086315106373,
+                    "velocityY": -0.6460541964699718,
+                    "timestamp": 2.4495356298206956
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": 0.06257585151184072,
+                    "velocityX": -0.7000043329401022,
+                    "velocityY": -0.3230271061984462,
+                    "timestamp": 2.5420193944374763
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": 1.89050013704423e-29,
+                    "velocityX": -9.26196849914815e-31,
+                    "velocityY": -1.067270908871219e-31,
+                    "timestamp": 2.634503159054257
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S2 To C1 To S2": {
+            "waypoints": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": -6.721620698039908e-29,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.32871710747576,
+                    "y": 5.596676184944768,
+                    "heading": 3.14,
+                    "angularVelocity": -1.6840159888455038e-16,
+                    "velocityX": 0.6917264182881007,
+                    "velocityY": 0.3860469065482612,
+                    "timestamp": 0.09500447827150149
+                },
+                {
+                    "x": 4.460151319654045,
+                    "y": 5.670028553286586,
+                    "heading": 3.14,
+                    "angularVelocity": -1.2818077527297718e-16,
+                    "velocityX": 1.3834528073858645,
+                    "velocityY": 0.772093796805632,
+                    "timestamp": 0.19000895654300298
+                },
+                {
+                    "x": 4.657302630988413,
+                    "y": 5.780057101930027,
+                    "heading": 3.14,
+                    "angularVelocity": -1.8176327661301372e-16,
+                    "velocityX": 2.0751791381032545,
+                    "velocityY": 1.1581406544813895,
+                    "timestamp": 0.2850134348145045
+                },
+                {
+                    "x": 4.920171024839782,
+                    "y": 5.926761821588956,
+                    "heading": 3.14,
+                    "angularVelocity": -2.1864698969841676e-16,
+                    "velocityX": 2.766905293682386,
+                    "velocityY": 1.5441874144139043,
+                    "timestamp": 0.38001791308600597
+                },
+                {
+                    "x": 5.23400140491651,
+                    "y": 6.101908012635582,
+                    "heading": 3.14,
+                    "angularVelocity": 2.4726801201282114e-16,
+                    "velocityX": 3.3033219674134005,
+                    "velocityY": 1.8435572115473686,
+                    "timestamp": 0.47502239135750746
+                },
+                {
+                    "x": 5.547831784993484,
+                    "y": 6.277054203682345,
+                    "heading": 3.14,
+                    "angularVelocity": 3.7708427520389947e-17,
+                    "velocityX": 3.303321967416651,
+                    "velocityY": 1.843557211549182,
+                    "timestamp": 0.570026869629009
+                },
+                {
+                    "x": 5.861662165070458,
+                    "y": 6.452200394729108,
+                    "heading": 3.14,
+                    "angularVelocity": 2.1976269787785873e-16,
+                    "velocityX": 3.3033219674166516,
+                    "velocityY": 1.843557211549183,
+                    "timestamp": 0.6650313479005104
+                },
+                {
+                    "x": 6.175492545147431,
+                    "y": 6.627346585775871,
+                    "heading": 3.14,
+                    "angularVelocity": 1.9941780105068909e-16,
+                    "velocityX": 3.3033219674166516,
+                    "velocityY": 1.843557211549183,
+                    "timestamp": 0.7600358261720119
+                },
+                {
+                    "x": 6.489322925224406,
+                    "y": 6.802492776822635,
+                    "heading": 3.14,
+                    "angularVelocity": 1.915977022732522e-16,
+                    "velocityX": 3.3033219674166516,
+                    "velocityY": 1.8435572115491827,
+                    "timestamp": 0.8550403044435134
+                },
+                {
+                    "x": 6.803153305301396,
+                    "y": 6.977638967869407,
+                    "heading": 3.14,
+                    "angularVelocity": 1.0071749090943876e-16,
+                    "velocityX": 3.3033219674167866,
+                    "velocityY": 1.8435572115492582,
+                    "timestamp": 0.9500447827150149
+                },
+                {
+                    "x": 7.109956833123924,
+                    "y": 7.148863529636325,
+                    "heading": 3.14,
+                    "angularVelocity": 7.28120504001254e-16,
+                    "velocityX": 3.2293585881581666,
+                    "velocityY": 1.802278849171093,
+                    "timestamp": 1.0450492609865165
+                },
+                {
+                    "x": 7.351043278430091,
+                    "y": 7.283411920388113,
+                    "heading": 3.14,
+                    "angularVelocity": 8.519808203444012e-16,
+                    "velocityX": 2.5376324325879027,
+                    "velocityY": 1.4162320892435267,
+                    "timestamp": 1.140053739258018
+                },
+                {
+                    "x": 7.526412624580293,
+                    "y": 7.381284130838343,
+                    "heading": 3.14,
+                    "angularVelocity": 9.198517694714364e-16,
+                    "velocityX": 1.8459061018722946,
+                    "velocityY": 1.0301852315687638,
+                    "timestamp": 1.2350582175295193
+                },
+                {
+                    "x": 7.636064866028015,
+                    "y": 7.442480157891549,
+                    "heading": 3.14,
+                    "angularVelocity": 7.057693969312242e-16,
+                    "velocityX": 1.1541797127752547,
+                    "velocityY": 0.6441383413117968,
+                    "timestamp": 1.3300626958010207
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "angularVelocity": 6.521841549416907e-16,
+                    "velocityX": 0.4624532944875416,
+                    "velocityY": 0.25809143476375196,
+                    "timestamp": 1.425067174072522
+                },
+                {
+                    "x": 7.649929424023421,
+                    "y": 7.450217855315383,
+                    "heading": 3.14,
+                    "angularVelocity": 4.173997569351088e-16,
+                    "velocityX": -0.2907017552877216,
+                    "velocityY": -0.16223829304468454,
+                    "timestamp": 1.5285085032844143
+                },
+                {
+                    "x": 7.5419514905723615,
+                    "y": 7.389956245982292,
+                    "heading": 3.14,
+                    "angularVelocity": 3.561755142449842e-16,
+                    "velocityX": -1.043856785955406,
+                    "velocityY": -0.5825680101893362,
+                    "timestamp": 1.6319498324963067
+                },
+                {
+                    "x": 7.356066203105843,
+                    "y": 7.286215173931179,
+                    "heading": 3.14,
+                    "angularVelocity": 4.0492399811570283e-16,
+                    "velocityX": -1.7970117831835097,
+                    "velocityY": -1.0028977086716289,
+                    "timestamp": 1.735391161708199
+                },
+                {
+                    "x": 7.092273569234474,
+                    "y": 7.138994643409464,
+                    "heading": 3.14,
+                    "angularVelocity": 3.5744526417986045e-16,
+                    "velocityX": -2.550166706837388,
+                    "velocityY": -1.4232273660927433,
+                    "timestamp": 1.8388324909200913
+                },
+                {
+                    "x": 6.7505736194163815,
+                    "y": 6.94829467141558,
+                    "heading": 3.14,
+                    "angularVelocity": 3.486084398579742e-18,
+                    "velocityX": -3.3033213360452667,
+                    "velocityY": -1.8435568591859304,
+                    "timestamp": 1.9422738201319836
+                },
+                {
+                    "x": 6.4088736018749985,
+                    "y": 6.757594661625877,
+                    "heading": 3.14,
+                    "angularVelocity": 3.295553774982102e-16,
+                    "velocityX": -3.303321990779068,
+                    "velocityY": -1.8435572245875573,
+                    "timestamp": 2.045715149343876
+                },
+                {
+                    "x": 6.0671735843335775,
+                    "y": 6.566894651836152,
+                    "heading": 3.14,
+                    "angularVelocity": -4.7007888222967745e-17,
+                    "velocityX": -3.303321990779443,
+                    "velocityY": -1.8435572245877663,
+                    "timestamp": 2.1491564785557684
+                },
+                {
+                    "x": 5.725473566792156,
+                    "y": 6.376194642046427,
+                    "heading": 3.14,
+                    "angularVelocity": -1.693931270443523e-16,
+                    "velocityX": -3.3033219907794424,
+                    "velocityY": -1.843557224587766,
+                    "timestamp": 2.252597807767661
+                },
+                {
+                    "x": 5.383773549250734,
+                    "y": 6.185494632256702,
+                    "heading": 3.14,
+                    "angularVelocity": -1.7179108161052583e-16,
+                    "velocityX": -3.3033219907794424,
+                    "velocityY": -1.8435572245877665,
+                    "timestamp": 2.3560391369795535
+                },
+                {
+                    "x": 5.042073531709293,
+                    "y": 5.994794622466965,
+                    "heading": 3.14,
+                    "angularVelocity": -2.8062613243046787e-16,
+                    "velocityX": -3.3033219907797844,
+                    "velocityY": -1.8435572245879575,
+                    "timestamp": 2.459480466191446
+                },
+                {
+                    "x": 4.730444135370463,
+                    "y": 5.820876782602128,
+                    "heading": 3.14,
+                    "angularVelocity": -4.299065406379285e-16,
+                    "velocityX": -3.0126197982407033,
+                    "velocityY": -1.6813186875168347,
+                    "timestamp": 2.5629217954033385
+                },
+                {
+                    "x": 4.496722071912365,
+                    "y": 5.690438393660192,
+                    "heading": 3.14,
+                    "angularVelocity": -4.936527887438678e-16,
+                    "velocityX": -2.2594650053308842,
+                    "velocityY": -1.2609891030629203,
+                    "timestamp": 2.666363124615231
+                },
+                {
+                    "x": 4.340907358243485,
+                    "y": 5.6034794650776485,
+                    "heading": 3.14,
+                    "angularVelocity": -3.0491715981392867e-16,
+                    "velocityX": -1.50631004895991,
+                    "velocityY": -0.8406594273826603,
+                    "timestamp": 2.7698044538271236
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": -6.545212108880601e-17,
+                    "velocityX": -0.7531550381017611,
+                    "velocityY": -0.42032972129354973,
+                    "timestamp": 2.873245783039016
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": -8.479397572029164e-30,
+                    "velocityY": 1.1992173369619903e-31,
+                    "timestamp": 2.9766871122509087
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S2 To C2 To S1": {
+            "waypoints": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 14
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": -3.0990371742085845e-31,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.334488852780742,
+                    "y": 5.55372681466254,
+                    "heading": 3.133953491828579,
+                    "angularVelocity": -0.06516682898121992,
+                    "velocityX": 0.7704780529773917,
+                    "velocityY": -0.06760986415037558,
+                    "timestamp": 0.09278506052766491
+                },
+                {
+                    "x": 4.477466555007721,
+                    "y": 5.541180442989367,
+                    "heading": 3.121860441564243,
+                    "angularVelocity": -0.13033402355464482,
+                    "velocityX": 1.5409560700168046,
+                    "velocityY": -0.1352197390595365,
+                    "timestamp": 0.18557012105532983
+                },
+                {
+                    "x": 4.691933100191895,
+                    "y": 5.522360883581774,
+                    "heading": 3.1037208124155655,
+                    "angularVelocity": -0.19550161465122112,
+                    "velocityX": 2.311434017119952,
+                    "velocityY": -0.20282962904337912,
+                    "timestamp": 0.27835518158299477
+                },
+                {
+                    "x": 4.977888469750385,
+                    "y": 5.497268135402371,
+                    "heading": 3.079534623451408,
+                    "angularVelocity": -0.26066900023130835,
+                    "velocityX": 3.0819117639443303,
+                    "velocityY": -0.2704395302078152,
+                    "timestamp": 0.37114024211065966
+                },
+                {
+                    "x": 5.327545209579434,
+                    "y": 5.4665855253125155,
+                    "heading": 3.079534620495457,
+                    "angularVelocity": -3.185804655621534e-8,
+                    "velocityX": 3.768459467941971,
+                    "velocityY": -0.33068480976750836,
+                    "timestamp": 0.46392530263832454
+                },
+                {
+                    "x": 5.677201949407512,
+                    "y": 5.435902915209702,
+                    "heading": 3.0795346175395855,
+                    "angularVelocity": -3.185719286228054e-8,
+                    "velocityX": 3.768459467931527,
+                    "velocityY": -0.3306848099071595,
+                    "timestamp": 0.5567103631659894
+                },
+                {
+                    "x": 6.026858689235635,
+                    "y": 5.405220305107383,
+                    "heading": 3.0795346145837144,
+                    "angularVelocity": -3.185719241382707e-8,
+                    "velocityX": 3.7684594679319945,
+                    "velocityY": -0.3306848099018315,
+                    "timestamp": 0.6494954236936543
+                },
+                {
+                    "x": 6.376515440276988,
+                    "y": 5.37453782279048,
+                    "heading": 3.079534611627843,
+                    "angularVelocity": -3.1857194092858966e-8,
+                    "velocityX": 3.7684595887836787,
+                    "velocityY": -0.3306834326820888,
+                    "timestamp": 0.7422804842213192
+                },
+                {
+                    "x": 6.7274981570931205,
+                    "y": 5.371017863275757,
+                    "heading": 3.079534608531603,
+                    "angularVelocity": -3.337002331102787e-8,
+                    "velocityX": 3.782750313683133,
+                    "velocityY": -0.03793670548581862,
+                    "timestamp": 0.8350655447489841
+                },
+                {
+                    "x": 7.060187879890856,
+                    "y": 5.436910997452384,
+                    "heading": 3.079737215344131,
+                    "angularVelocity": 0.0021836145967468554,
+                    "velocityX": 3.585595794255326,
+                    "velocityY": 0.7101696523329738,
+                    "timestamp": 0.927850605276649
+                },
+                {
+                    "x": 7.322040445742604,
+                    "y": 5.514304234937123,
+                    "heading": 3.085826008048483,
+                    "angularVelocity": 0.065622554641053,
+                    "velocityX": 2.82214145642183,
+                    "velocityY": 0.834113132487145,
+                    "timestamp": 1.0206356658043139
+                },
+                {
+                    "x": 7.51268687185891,
+                    "y": 5.600628661192279,
+                    "heading": 3.097884063831068,
+                    "angularVelocity": 0.12995686712932528,
+                    "velocityX": 2.054710370743838,
+                    "velocityY": 0.9303698867493723,
+                    "timestamp": 1.1134207263319789
+                },
+                {
+                    "x": 7.632022870387607,
+                    "y": 5.695007866387598,
+                    "heading": 3.115937777513367,
+                    "angularVelocity": 0.19457565236926141,
+                    "velocityX": 1.2861553126119536,
+                    "velocityY": 1.0171810489597053,
+                    "timestamp": 1.2062057868596439
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": 0.25933293948166103,
+                    "velocityX": 0.517078173356003,
+                    "velocityY": 1.0992301242503861,
+                    "timestamp": 1.2989908473873089
+                },
+                {
+                    "x": 7.658324369192282,
+                    "y": 5.904176742987938,
+                    "heading": 3.16941072815558,
+                    "angularVelocity": 0.32296823834349714,
+                    "velocityX": -0.23802675880415525,
+                    "velocityY": 1.176940730303702,
+                    "timestamp": 1.3900546879260585
+                },
+                {
+                    "x": 7.56785439743373,
+                    "y": 6.01813279809926,
+                    "heading": 3.204362801369678,
+                    "angularVelocity": 0.3838194502596341,
+                    "velocityX": -0.9934785445386147,
+                    "velocityY": 1.2513864387569726,
+                    "timestamp": 1.481118528464808
+                },
+                {
+                    "x": 7.4085407741930185,
+                    "y": 6.138375580441298,
+                    "heading": 3.2444365070983814,
+                    "angularVelocity": 0.44006166983116235,
+                    "velocityX": -1.7494718243615388,
+                    "velocityY": 1.3204229212238012,
+                    "timestamp": 1.5721823690035577
+                },
+                {
+                    "x": 7.180297052881674,
+                    "y": 6.26393107037867,
+                    "heading": 3.2888027958474897,
+                    "angularVelocity": 0.48719984229357544,
+                    "velocityX": -2.5064144007215123,
+                    "velocityY": 1.3787633949387568,
+                    "timestamp": 1.6632462095423073
+                },
+                {
+                    "x": 6.882954936764855,
+                    "y": 6.39196860518662,
+                    "heading": 3.335055491791258,
+                    "angularVelocity": 0.5079150590414778,
+                    "velocityX": -3.2652050952131715,
+                    "velocityY": 1.406019491935018,
+                    "timestamp": 1.754310050081057
+                },
+                {
+                    "x": 6.546228290367871,
+                    "y": 6.464686559433002,
+                    "heading": 3.335055498171187,
+                    "angularVelocity": 7.005996108772343e-8,
+                    "velocityX": -3.697698717787989,
+                    "velocityY": 0.7985381883321813,
+                    "timestamp": 1.8453738906198065
+                },
+                {
+                    "x": 6.204219309221898,
+                    "y": 6.505949051941506,
+                    "heading": 3.3350555015810333,
+                    "angularVelocity": 3.7444564221531034e-8,
+                    "velocityX": -3.7557056579492913,
+                    "velocityY": 0.45311610255384965,
+                    "timestamp": 1.936437731158556
+                },
+                {
+                    "x": 5.862210306846827,
+                    "y": 6.547211368473936,
+                    "heading": 3.335055504991925,
+                    "angularVelocity": 3.7456047151170976e-8,
+                    "velocityX": -3.7557058910725254,
+                    "velocityY": 0.45311417010650673,
+                    "timestamp": 2.0275015716973055
+                },
+                {
+                    "x": 5.587726197097608,
+                    "y": 6.580326820073676,
+                    "heading": 3.3623069003974067,
+                    "angularVelocity": 0.29925594225114466,
+                    "velocityX": -3.0141943072610453,
+                    "velocityY": 0.3636509442587018,
+                    "timestamp": 2.118565412236055
+                },
+                {
+                    "x": 5.381863101775985,
+                    "y": 6.605163409773074,
+                    "heading": 3.382745950256302,
+                    "angularVelocity": 0.22444748363294062,
+                    "velocityX": -2.2606458733093975,
+                    "velocityY": 0.2727382191708659,
+                    "timestamp": 2.2096292527748043
+                },
+                {
+                    "x": 5.244621034490526,
+                    "y": 6.621721136542047,
+                    "heading": 3.3963721295427387,
+                    "angularVelocity": 0.1496332595442014,
+                    "velocityX": -1.5070972899178572,
+                    "velocityY": 0.18182548277135752,
+                    "timestamp": 2.3006930933135536
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": 0.07481759605718756,
+                    "velocityX": -0.7535486542688541,
+                    "velocityY": 0.09091274219244924,
+                    "timestamp": 2.391756933852303
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": 0,
+                    "velocityX": -1.5663528740094776e-30,
+                    "velocityY": 4.877949363390491e-30,
+                    "timestamp": 2.4828207743910524
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S2 To C2 To S2": {
+            "waypoints": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 14
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 14
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": -2.9840248880147764e-34,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.335913751201177,
+                    "y": 5.5650572312071045,
+                    "heading": 3.14,
+                    "angularVelocity": -1.4550376605668876e-16,
+                    "velocityX": 0.7787859499921702,
+                    "velocityY": 0.0540158823962963,
+                    "timestamp": 0.09362489295282081
+                },
+                {
+                    "x": 4.481741250496669,
+                    "y": 5.575171693405826,
+                    "heading": 3.14,
+                    "angularVelocity": -2.6552600483679955e-16,
+                    "velocityX": 1.5575718668001757,
+                    "velocityY": 0.10803176249096925,
+                    "timestamp": 0.18724978590564162
+                },
+                {
+                    "x": 4.7004824916727515,
+                    "y": 5.590343386165186,
+                    "heading": 3.14,
+                    "angularVelocity": -3.921189080478138e-16,
+                    "velocityX": 2.3363577172399013,
+                    "velocityY": 0.1620476379823989,
+                    "timestamp": 0.2808746788584624
+                },
+                {
+                    "x": 4.992137456088294,
+                    "y": 5.6105723081922525,
+                    "heading": 3.14,
+                    "angularVelocity": -4.0854142114404067e-16,
+                    "velocityX": 3.1151433685752,
+                    "velocityY": 0.2160634996641273,
+                    "timestamp": 0.37449957181128324
+                },
+                {
+                    "x": 5.345466004271564,
+                    "y": 5.63507885367643,
+                    "heading": 3.14,
+                    "angularVelocity": 5.235558245992657e-16,
+                    "velocityX": 3.773873988420134,
+                    "velocityY": 0.2617524539817304,
+                    "timestamp": 0.4681244647641041
+                },
+                {
+                    "x": 5.69879455245494,
+                    "y": 5.6595853991606155,
+                    "heading": 3.14,
+                    "angularVelocity": -5.535576803898805e-16,
+                    "velocityX": 3.773873988421262,
+                    "velocityY": 0.2617524539818086,
+                    "timestamp": 0.5617493577169249
+                },
+                {
+                    "x": 6.052123100638315,
+                    "y": 5.6840919446448,
+                    "heading": 3.14,
+                    "angularVelocity": -2.4126594725597773e-16,
+                    "velocityX": 3.7738739884212626,
+                    "velocityY": 0.2617524539818086,
+                    "timestamp": 0.6553742506697458
+                },
+                {
+                    "x": 6.405451648821692,
+                    "y": 5.708598490128986,
+                    "heading": 3.14,
+                    "angularVelocity": 3.1966951315745e-16,
+                    "velocityX": 3.773873988421262,
+                    "velocityY": 0.26175245398180863,
+                    "timestamp": 0.7489991436225666
+                },
+                {
+                    "x": 6.7587801970050565,
+                    "y": 5.733105035613169,
+                    "heading": 3.14,
+                    "angularVelocity": -3.2948712369984396e-16,
+                    "velocityX": 3.773873988421149,
+                    "velocityY": 0.26175245398180075,
+                    "timestamp": 0.8426240365753874
+                },
+                {
+                    "x": 7.088851630801794,
+                    "y": 5.755998488879142,
+                    "heading": 3.14,
+                    "angularVelocity": -9.417571312555162e-16,
+                    "velocityX": 3.5254666081494603,
+                    "velocityY": 0.24452314490237434,
+                    "timestamp": 0.9362489295282083
+                },
+                {
+                    "x": 7.3460093413592045,
+                    "y": 5.773834712877417,
+                    "heading": 3.14,
+                    "angularVelocity": -8.090853196926974e-16,
+                    "velocityX": 2.746680956815585,
+                    "velocityY": 0.19050728322074467,
+                    "timestamp": 1.029873822481029
+                },
+                {
+                    "x": 7.530253310036057,
+                    "y": 5.786613706315055,
+                    "heading": 3.14,
+                    "angularVelocity": -6.531335051646392e-16,
+                    "velocityX": 1.9678951063762244,
+                    "velocityY": 0.13649140772934035,
+                    "timestamp": 1.1234987154338498
+                },
+                {
+                    "x": 7.641583530618614,
+                    "y": 5.79433546876108,
+                    "heading": 3.14,
+                    "angularVelocity": -6.271183503983396e-16,
+                    "velocityX": 1.1891091895683832,
+                    "velocityY": 0.08247552763467879,
+                    "timestamp": 1.2171236083866706
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": -6.05510209215834e-16,
+                    "velocityX": 0.41032323957630595,
+                    "velocityY": 0.028459645238388953,
+                    "timestamp": 1.3107485013394913
+                },
+                {
+                    "x": 7.6370908002198155,
+                    "y": 5.794023857082849,
+                    "heading": 3.14,
+                    "angularVelocity": -4.789725529265609e-16,
+                    "velocityX": -0.42651589559076464,
+                    "velocityY": -0.02958275307434032,
+                    "timestamp": 1.4113524887260582
+                },
+                {
+                    "x": 7.509992248723152,
+                    "y": 5.785208417602395,
+                    "heading": 3.14,
+                    "angularVelocity": -2.32893689231122e-16,
+                    "velocityX": -1.2633550100583235,
+                    "velocityY": -0.08762514995137034,
+                    "timestamp": 1.5119564761126252
+                },
+                {
+                    "x": 7.298704349055096,
+                    "y": 5.770553681804523,
+                    "heading": 3.14,
+                    "angularVelocity": -1.4093989000106693e-18,
+                    "velocityX": -2.1001940892878346,
+                    "velocityY": -0.14566754438432153,
+                    "timestamp": 1.6125604634991921
+                },
+                {
+                    "x": 7.003227108598371,
+                    "y": 5.75005965020129,
+                    "heading": 3.14,
+                    "angularVelocity": 2.7928007738396434e-16,
+                    "velocityX": -2.9370330951333345,
+                    "velocityY": -0.2037099337274247,
+                    "timestamp": 1.713164450885759
+                },
+                {
+                    "x": 6.623560552157282,
+                    "y": 5.723726324513103,
+                    "heading": 3.14,
+                    "angularVelocity": 4.2079673901381875e-16,
+                    "velocityX": -3.773871854424956,
+                    "velocityY": -0.2617523059697759,
+                    "timestamp": 1.813768438272326
+                },
+                {
+                    "x": 6.243893779806084,
+                    "y": 5.6973929838495865,
+                    "heading": 3.14,
+                    "angularVelocity": -4.939929195074519e-16,
+                    "velocityX": -3.773874000563654,
+                    "velocityY": -0.2617524548239938,
+                    "timestamp": 1.914372425658893
+                },
+                {
+                    "x": 5.864227007454884,
+                    "y": 5.67105964318607,
+                    "heading": 3.14,
+                    "angularVelocity": 2.0250011365009936e-16,
+                    "velocityX": -3.773874000563654,
+                    "velocityY": -0.26175245482399373,
+                    "timestamp": 2.01497641304546
+                },
+                {
+                    "x": 5.484560235103686,
+                    "y": 5.644726302522554,
+                    "heading": 3.14,
+                    "angularVelocity": -5.77312332256685e-16,
+                    "velocityX": -3.773874000563654,
+                    "velocityY": -0.2617524548239938,
+                    "timestamp": 2.115580400432027
+                },
+                {
+                    "x": 5.104893462752507,
+                    "y": 5.61839296185904,
+                    "heading": 3.14,
+                    "angularVelocity": -1.6679194482656825e-16,
+                    "velocityX": -3.773874000563452,
+                    "velocityY": -0.26175245482397974,
+                    "timestamp": 2.2161843878185943
+                },
+                {
+                    "x": 4.768136096424167,
+                    "y": 5.595035778417478,
+                    "heading": 3.14,
+                    "angularVelocity": 9.060703447293421e-16,
+                    "velocityX": -3.3473560549281536,
+                    "velocityY": -0.2321695595604253,
+                    "timestamp": 2.3167883752051615
+                },
+                {
+                    "x": 4.515568053067083,
+                    "y": 5.577517889545478,
+                    "heading": 3.14,
+                    "angularVelocity": 6.700681499162007e-16,
+                    "velocityX": -2.51051722618707,
+                    "velocityY": -0.1741271825011227,
+                    "timestamp": 2.4173923625917286
+                },
+                {
+                    "x": 4.34718935210125,
+                    "y": 5.565839296589989,
+                    "heading": 3.14,
+                    "angularVelocity": 4.365333326959493e-16,
+                    "velocityX": -1.6736782044119611,
+                    "velocityY": -0.11608479205315635,
+                    "timestamp": 2.517996349978296
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 2.0339545203820728e-16,
+                    "velocityX": -0.836839118292156,
+                    "velocityY": -0.05804239714230061,
+                    "timestamp": 2.618600337364863
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 2.71920432475143
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S2 To C3 To S1": {
+            "waypoints": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": -4.804532357029023e-32,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.321076149980653,
+                    "y": 5.523337393191978,
+                    "heading": 3.1351500057236232,
+                    "angularVelocity": -0.05343391557798715,
+                    "velocityX": 0.6398432489449108,
+                    "velocityY": -0.40392349462987753,
+                    "timestamp": 0.09076621512600157
+                },
+                {
+                    "x": 4.4372316676864765,
+                    "y": 5.45001014084527,
+                    "heading": 3.1261940931811654,
+                    "angularVelocity": -0.098670111230538,
+                    "velocityX": 1.2797219487953386,
+                    "velocityY": -0.807869450598055,
+                    "timestamp": 0.18153243025200314
+                },
+                {
+                    "x": 4.611470902956015,
+                    "y": 5.340015489147945,
+                    "heading": 3.1144364232978923,
+                    "angularVelocity": -0.1295379549202309,
+                    "velocityX": 1.9196485721880063,
+                    "velocityY": -1.2118457461802494,
+                    "timestamp": 0.2722986453780047
+                },
+                {
+                    "x": 4.843797638177826,
+                    "y": 5.193351042771614,
+                    "heading": 3.102754458759954,
+                    "angularVelocity": -0.1287038852696506,
+                    "velocityX": 2.5596168673475677,
+                    "velocityY": -1.6158484318502528,
+                    "timestamp": 0.36306486050400627
+                },
+                {
+                    "x": 5.1341460210960115,
+                    "y": 5.0100588028009865,
+                    "heading": 3.102754388459234,
+                    "angularVelocity": -7.745251732579077e-7,
+                    "velocityX": 3.1988596474483897,
+                    "velocityY": -2.0193883783319753,
+                    "timestamp": 0.45383107563000785
+                },
+                {
+                    "x": 5.424494292461022,
+                    "y": 4.826766239115272,
+                    "heading": 3.1027543844284144,
+                    "angularVelocity": -4.440880652615192e-8,
+                    "velocityX": 3.1988584184318825,
+                    "velocityY": -2.019391944803125,
+                    "timestamp": 0.5445972907560094
+                },
+                {
+                    "x": 5.714842563825266,
+                    "y": 4.643473675428344,
+                    "heading": 3.102754380397595,
+                    "angularVelocity": -4.440880657505379e-8,
+                    "velocityX": 3.1988584184234474,
+                    "velocityY": -2.019391944816487,
+                    "timestamp": 0.635363505882011
+                },
+                {
+                    "x": 6.005190835190169,
+                    "y": 4.460181111742461,
+                    "heading": 3.102754376366776,
+                    "angularVelocity": -4.4408806414514103e-8,
+                    "velocityX": 3.198858418430713,
+                    "velocityY": -2.019391944804976,
+                    "timestamp": 0.7261297210080125
+                },
+                {
+                    "x": 6.295539259817851,
+                    "y": 4.276888790835792,
+                    "heading": 3.1027543723359563,
+                    "angularVelocity": -4.440881138366432e-8,
+                    "velocityX": 3.1988601069750424,
+                    "velocityY": -2.019389270029866,
+                    "timestamp": 0.8168959361340141
+                },
+                {
+                    "x": 6.605220322395041,
+                    "y": 4.128578571418711,
+                    "heading": 3.1027543676059754,
+                    "angularVelocity": -5.211168523884132e-8,
+                    "velocityX": 3.4118538725812453,
+                    "velocityY": -1.633980432159657,
+                    "timestamp": 0.9076621512600157
+                },
+                {
+                    "x": 6.930843742926924,
+                    "y": 4.047062548737473,
+                    "heading": 3.096403794352487,
+                    "angularVelocity": -0.0699662671256465,
+                    "velocityX": 3.5874958549263174,
+                    "velocityY": -0.8980877143337577,
+                    "timestamp": 0.9984283663860173
+                },
+                {
+                    "x": 7.20268686077656,
+                    "y": 4.00824503573056,
+                    "heading": 3.096579244871637,
+                    "angularVelocity": 0.0019329936684692615,
+                    "velocityX": 2.9949813096454783,
+                    "velocityY": -0.4276647754125783,
+                    "timestamp": 1.0891945815120188
+                },
+                {
+                    "x": 7.418574834249914,
+                    "y": 4.009226418838573,
+                    "heading": 3.1038020925835856,
+                    "angularVelocity": 0.07957638976044962,
+                    "velocityX": 2.37850584794857,
+                    "velocityY": 0.010812207016131005,
+                    "timestamp": 1.1799607966380203
+                },
+                {
+                    "x": 7.577796617923462,
+                    "y": 4.04898509700118,
+                    "heading": 3.1182467242773515,
+                    "angularVelocity": 0.15914106007078685,
+                    "velocityX": 1.7541965747113617,
+                    "velocityY": 0.43803388857203507,
+                    "timestamp": 1.270727011764022
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": 0.23966269489643754,
+                    "velocityX": 1.1260068730932273,
+                    "velocityY": 0.8595147752996188,
+                    "timestamp": 1.3614932268900235
+                },
+                {
+                    "x": 7.725157318651882,
+                    "y": 4.240486437949111,
+                    "heading": 3.168528121789897,
+                    "angularVelocity": 0.31944830036475336,
+                    "velocityX": 0.505656446597267,
+                    "velocityY": 1.2707829132351232,
+                    "timestamp": 1.4507975736570093
+                },
+                {
+                    "x": 7.714677077614687,
+                    "y": 4.390346197703396,
+                    "heading": 3.203767814945465,
+                    "angularVelocity": 0.39460221625624564,
+                    "velocityX": -0.11735420969529425,
+                    "velocityY": 1.6780791213366064,
+                    "timestamp": 1.5401019204239952
+                },
+                {
+                    "x": 7.648174313953831,
+                    "y": 4.575993018943536,
+                    "heading": 3.2450393487188225,
+                    "angularVelocity": 0.4621447361464423,
+                    "velocityX": -0.7446755512850395,
+                    "velocityY": 2.0788105838177287,
+                    "timestamp": 1.629406267190981
+                },
+                {
+                    "x": 7.524918843988051,
+                    "y": 4.7962734130800495,
+                    "heading": 3.2910178393067144,
+                    "angularVelocity": 0.5148516533899635,
+                    "velocityX": -1.3801732438330052,
+                    "velocityY": 2.4666256695351096,
+                    "timestamp": 1.7187106139579669
+                },
+                {
+                    "x": 7.343009789801653,
+                    "y": 5.047887037555876,
+                    "heading": 3.3379953942154983,
+                    "angularVelocity": 0.5260388391996106,
+                    "velocityX": -2.0369563271207567,
+                    "velocityY": 2.817484630757546,
+                    "timestamp": 1.8080149607249527
+                },
+                {
+                    "x": 7.098537759361482,
+                    "y": 5.281049171644365,
+                    "heading": 3.337995405344767,
+                    "angularVelocity": 1.2462180234431456e-7,
+                    "velocityX": -2.7375154658266756,
+                    "velocityY": 2.6108710553234187,
+                    "timestamp": 1.8973193074919386
+                },
+                {
+                    "x": 6.821988530708413,
+                    "y": 5.475089583706822,
+                    "heading": 3.337995410181252,
+                    "angularVelocity": 5.415732950343717e-8,
+                    "velocityX": -3.096705128750813,
+                    "velocityY": 2.1727991871297463,
+                    "timestamp": 1.9866236542589244
+                },
+                {
+                    "x": 6.5454389358432925,
+                    "y": 5.669129473837467,
+                    "heading": 3.3379954150177356,
+                    "angularVelocity": 5.4157314847566374e-8,
+                    "velocityX": -3.0967092294700773,
+                    "velocityY": 2.172793342713053,
+                    "timestamp": 2.0759280010259102
+                },
+                {
+                    "x": 6.268889340976521,
+                    "y": 5.86316936396576,
+                    "heading": 3.337995419854219,
+                    "angularVelocity": 5.415731402245697e-8,
+                    "velocityX": -3.096709229488555,
+                    "velocityY": 2.172793342686719,
+                    "timestamp": 2.165232347792896
+                },
+                {
+                    "x": 5.9923397461188825,
+                    "y": 6.057209254107063,
+                    "heading": 3.337995424690707,
+                    "angularVelocity": 5.415735914657007e-8,
+                    "velocityX": -3.0967092293862986,
+                    "velocityY": 2.1727933428323993,
+                    "timestamp": 2.2545366945598815
+                },
+                {
+                    "x": 5.720218360303881,
+                    "y": 6.248146363272753,
+                    "heading": 3.35123648127501,
+                    "angularVelocity": 0.14826889242971145,
+                    "velocityX": -3.04712363581837,
+                    "velocityY": 2.1380494463935373,
+                    "timestamp": 2.343841041326867
+                },
+                {
+                    "x": 5.5025203930179725,
+                    "y": 6.400895625132601,
+                    "heading": 3.369358291202865,
+                    "angularVelocity": 0.20292192467559766,
+                    "velocityX": -2.437708523348032,
+                    "velocityY": 1.7104347928147705,
+                    "timestamp": 2.4331453880938527
+                },
+                {
+                    "x": 5.339255695066747,
+                    "y": 6.515451087573219,
+                    "heading": 3.385376867292448,
+                    "angularVelocity": 0.17937062046238467,
+                    "velocityX": -1.828183104874154,
+                    "velocityY": 1.28275348947479,
+                    "timestamp": 2.5224497348608383
+                },
+                {
+                    "x": 5.230417322474398,
+                    "y": 6.591817929626251,
+                    "heading": 3.3970262318270823,
+                    "angularVelocity": 0.1304456608929641,
+                    "velocityX": -1.218735442702819,
+                    "velocityY": 0.8551301791870324,
+                    "timestamp": 2.611754081627824
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": 0.06896725160058048,
+                    "velocityX": -0.6093468509028436,
+                    "velocityY": 0.42754996543868296,
+                    "timestamp": 2.7010584283948096
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": 2.6480923788288046e-29,
+                    "velocityX": -1.5869679624644047e-31,
+                    "velocityY": 0,
+                    "timestamp": 2.790362775161795
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S2 To C3 To S2": {
+            "waypoints": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": -2.7122542067664996e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.329648409276986,
+                    "y": 5.532049408693613,
+                    "heading": 3.14,
+                    "angularVelocity": 3.6127527846474837e-17,
+                    "velocityX": 0.715878164292546,
+                    "velocityY": -0.3002204885663498,
+                    "timestamp": 0.09310021257998485
+                },
+                {
+                    "x": 4.462945224981569,
+                    "y": 5.4761482272758,
+                    "heading": 3.14,
+                    "angularVelocity": 1.058102916611751e-16,
+                    "velocityX": 1.4317562979794687,
+                    "velocityY": -0.6004409642975056,
+                    "timestamp": 0.1862004251599697
+                },
+                {
+                    "x": 4.662890441407609,
+                    "y": 5.392296458139565,
+                    "heading": 3.14,
+                    "angularVelocity": 1.669202321487025e-16,
+                    "velocityX": 2.147634370376114,
+                    "velocityY": -0.9006614143251297,
+                    "timestamp": 0.27930063773995456
+                },
+                {
+                    "x": 4.929484041376741,
+                    "y": 5.280494108489063,
+                    "heading": 3.14,
+                    "angularVelocity": 1.646239395597301e-16,
+                    "velocityX": 2.8635122582579458,
+                    "velocityY": -1.2008817869720905,
+                    "timestamp": 0.3724008503199394
+                },
+                {
+                    "x": 5.2542718637391,
+                    "y": 5.1442866313321245,
+                    "heading": 3.14,
+                    "angularVelocity": -2.6454281962617153e-17,
+                    "velocityX": 3.4885830371581816,
+                    "velocityY": -1.4630200445559467,
+                    "timestamp": 0.46550106289992427
+                },
+                {
+                    "x": 5.579059685941281,
+                    "y": 5.008079154242361,
+                    "heading": 3.14,
+                    "angularVelocity": -9.771658175099026e-17,
+                    "velocityX": 3.4885830354377245,
+                    "velocityY": -1.463020043834432,
+                    "timestamp": 0.5586012754799091
+                },
+                {
+                    "x": 5.903847508143462,
+                    "y": 4.871871677152596,
+                    "heading": 3.14,
+                    "angularVelocity": -3.1326214825920436e-16,
+                    "velocityX": 3.488583035437725,
+                    "velocityY": -1.4630200438344323,
+                    "timestamp": 0.6517014880598939
+                },
+                {
+                    "x": 6.228635330345643,
+                    "y": 4.735664200062831,
+                    "heading": 3.14,
+                    "angularVelocity": -1.2137930063882497e-16,
+                    "velocityX": 3.488583035437725,
+                    "velocityY": -1.4630200438344325,
+                    "timestamp": 0.7448017006398788
+                },
+                {
+                    "x": 6.553423152547824,
+                    "y": 4.599456722973066,
+                    "heading": 3.14,
+                    "angularVelocity": 1.3394779600477518e-16,
+                    "velocityX": 3.488583035437725,
+                    "velocityY": -1.4630200438344325,
+                    "timestamp": 0.8379019132198637
+                },
+                {
+                    "x": 6.878210974643575,
+                    "y": 4.463249245927935,
+                    "heading": 3.14,
+                    "angularVelocity": 2.91910120342859e-17,
+                    "velocityX": 3.488583034294527,
+                    "velocityY": -1.4630200433550054,
+                    "timestamp": 0.9310021257998486
+                },
+                {
+                    "x": 7.17186557168986,
+                    "y": 4.3400982252760985,
+                    "heading": 3.14,
+                    "angularVelocity": -6.102921228742647e-16,
+                    "velocityX": 3.1541775137602737,
+                    "velocityY": -1.3227791563413718,
+                    "timestamp": 1.0241023383798336
+                },
+                {
+                    "x": 7.3988717849138865,
+                    "y": 4.2448977852556045,
+                    "heading": 3.14,
+                    "angularVelocity": -6.383129451815479e-16,
+                    "velocityX": 2.4382996228798555,
+                    "velocityY": -1.0225587824368827,
+                    "timestamp": 1.1172025509598185
+                },
+                {
+                    "x": 7.559229597346586,
+                    "y": 4.1776479329828335,
+                    "heading": 3.14,
+                    "angularVelocity": -7.263779621185202e-16,
+                    "velocityX": 1.7224215497327395,
+                    "velocityY": -0.7223383320945309,
+                    "timestamp": 1.2103027635398034
+                },
+                {
+                    "x": 7.65293900332055,
+                    "y": 4.138348670834548,
+                    "heading": 3.14,
+                    "angularVelocity": -7.150088563233795e-16,
+                    "velocityX": 1.0065434157113666,
+                    "velocityY": -0.42211785622311543,
+                    "timestamp": 1.3034029761197883
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": -7.092042046536786e-16,
+                    "velocityX": 0.2906652512305344,
+                    "velocityY": -0.12189736757780324,
+                    "timestamp": 1.3965031886997732
+                },
+                {
+                    "x": 7.634550444733744,
+                    "y": 4.146060349047862,
+                    "heading": 3.14,
+                    "angularVelocity": -8.252913531228688e-16,
+                    "velocityX": -0.4634351183891671,
+                    "velocityY": 0.19435250940932913,
+                    "timestamp": 1.4945742097824644
+                },
+                {
+                    "x": 7.515145498014414,
+                    "y": 4.196135645696618,
+                    "heading": 3.14,
+                    "angularVelocity": -8.640334878143468e-16,
+                    "velocityX": -1.217535469712484,
+                    "velocityY": 0.5106023787234387,
+                    "timestamp": 1.5926452308651555
+                },
+                {
+                    "x": 7.321785162832602,
+                    "y": 4.277225888692093,
+                    "heading": 3.14,
+                    "angularVelocity": -7.792870163094326e-16,
+                    "velocityX": -1.971635790541653,
+                    "velocityY": 0.826852235249104,
+                    "timestamp": 1.6907162519478467
+                },
+                {
+                    "x": 7.054469445169555,
+                    "y": 4.389331075525908,
+                    "heading": 3.14,
+                    "angularVelocity": -9.085136126677818e-16,
+                    "velocityX": -2.725736050381882,
+                    "velocityY": 1.1431020661976106,
+                    "timestamp": 1.7887872730305379
+                },
+                {
+                    "x": 6.713198362969528,
+                    "y": 4.532451198672714,
+                    "heading": 3.14,
+                    "angularVelocity": -7.684534164287992e-16,
+                    "velocityX": -3.4798361272500533,
+                    "velocityY": 1.4593518204124447,
+                    "timestamp": 1.886858294113229
+                },
+                {
+                    "x": 6.371069462531013,
+                    "y": 4.675931068244968,
+                    "heading": 3.14,
+                    "angularVelocity": -1.8367347430029744e-16,
+                    "velocityX": -3.4885830356557705,
+                    "velocityY": 1.4630200439258751,
+                    "timestamp": 1.9849293151959202
+                },
+                {
+                    "x": 6.028940562092558,
+                    "y": 4.819410937817198,
+                    "heading": 3.14,
+                    "angularVelocity": 1.6315422621813397e-16,
+                    "velocityX": -3.48858303565516,
+                    "velocityY": 1.463020043925619,
+                    "timestamp": 2.0830003362786114
+                },
+                {
+                    "x": 5.686811661654102,
+                    "y": 4.962890807389427,
+                    "heading": 3.14,
+                    "angularVelocity": -1.6415107007869627e-16,
+                    "velocityX": -3.4885830356551595,
+                    "velocityY": 1.4630200439256191,
+                    "timestamp": 2.1810713573613025
+                },
+                {
+                    "x": 5.344682761215647,
+                    "y": 5.106370676961656,
+                    "heading": 3.14,
+                    "angularVelocity": -2.6663466601394013e-16,
+                    "velocityX": -3.48858303565516,
+                    "velocityY": 1.4630200439256194,
+                    "timestamp": 2.2791423784439937
+                },
+                {
+                    "x": 5.002553860860093,
+                    "y": 5.249850546499118,
+                    "heading": 3.14,
+                    "angularVelocity": -2.7119156877069447e-16,
+                    "velocityX": -3.4885830348098277,
+                    "velocityY": 1.4630200435711092,
+                    "timestamp": 2.377213399526685
+                },
+                {
+                    "x": 4.706732333820954,
+                    "y": 5.373910320642252,
+                    "heading": 3.14,
+                    "angularVelocity": 2.3228605057035087e-16,
+                    "velocityX": -3.01640100993532,
+                    "velocityY": 1.264999311453705,
+                    "timestamp": 2.475284420609376
+                },
+                {
+                    "x": 4.4848661713885285,
+                    "y": 5.466955158443148,
+                    "heading": 3.14,
+                    "angularVelocity": 1.3635627120067326e-16,
+                    "velocityX": -2.262300932356051,
+                    "velocityY": 0.9487495569406552,
+                    "timestamp": 2.573355441692067
+                },
+                {
+                    "x": 4.336955391458343,
+                    "y": 5.528985052396896,
+                    "heading": 3.14,
+                    "angularVelocity": 8.074174127249248e-18,
+                    "velocityX": -1.5082006723016521,
+                    "velocityY": 0.6324997259023314,
+                    "timestamp": 2.6714264627747584
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 3.321022798774287e-17,
+                    "velocityX": -0.7541003513770345,
+                    "velocityY": 0.31624986933663735,
+                    "timestamp": 2.7694974838574495
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": -1.5033553036563547e-32,
+                    "velocityY": 0,
+                    "timestamp": 2.8675685049401407
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S2 To C3 To S3": {
+            "waypoints": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 17
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": -2.3834052223810005e-28,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.3290756213238994,
+                    "y": 5.551919291811216,
+                    "heading": 3.150896767326327,
+                    "angularVelocity": 0.12189104355668522,
+                    "velocityX": 0.7391207131092994,
+                    "velocityY": -0.09039065663175272,
+                    "timestamp": 0.08939760468350924
+                },
+                {
+                    "x": 4.461244485010799,
+                    "y": 5.535758895626619,
+                    "heading": 3.171238121679265,
+                    "angularVelocity": 0.22753802436807322,
+                    "velocityX": 1.478438534845478,
+                    "velocityY": -0.18076990140321392,
+                    "timestamp": 0.17879520936701848
+                },
+                {
+                    "x": 4.65953073652644,
+                    "y": 5.511521259157241,
+                    "heading": 3.1985778553739457,
+                    "angularVelocity": 0.30582176996086496,
+                    "velocityX": 2.2180264473277496,
+                    "velocityY": -0.2711217661229951,
+                    "timestamp": 0.26819281405052775
+                },
+                {
+                    "x": 4.923962030908283,
+                    "y": 5.4792142049190105,
+                    "heading": 3.2279129750090267,
+                    "angularVelocity": 0.328142121241728,
+                    "velocityX": 2.9579237085624133,
+                    "velocityY": -0.3613861283199711,
+                    "timestamp": 0.35759041873403696
+                },
+                {
+                    "x": 5.25442830298904,
+                    "y": 5.438888092999425,
+                    "heading": 3.243107062504638,
+                    "angularVelocity": 0.1699607897635962,
+                    "velocityX": 3.6965897828133016,
+                    "velocityY": -0.4510871634824905,
+                    "timestamp": 0.4469880234175462
+                },
+                {
+                    "x": 5.590105329368368,
+                    "y": 5.397771389697725,
+                    "heading": 3.243107071754486,
+                    "angularVelocity": 1.0346835398617793e-7,
+                    "velocityX": 3.7548771867867403,
+                    "velocityY": -0.45993070448202833,
+                    "timestamp": 0.5363856281010554
+                },
+                {
+                    "x": 5.925782355662538,
+                    "y": 5.356654685708362,
+                    "heading": 3.2431070810044496,
+                    "angularVelocity": 1.0346963637170158e-7,
+                    "velocityX": 3.7548771858341166,
+                    "velocityY": -0.4599307121748044,
+                    "timestamp": 0.6257832327845646
+                },
+                {
+                    "x": 6.26145937114871,
+                    "y": 5.315537893473946,
+                    "heading": 3.243107090254437,
+                    "angularVelocity": 1.0346990400921205e-7,
+                    "velocityX": 3.754877064936017,
+                    "velocityY": -0.45993169928168404,
+                    "timestamp": 0.7151808374680738
+                },
+                {
+                    "x": 6.594482534239531,
+                    "y": 5.256671755131022,
+                    "heading": 3.24310709962475,
+                    "angularVelocity": 1.0481586200532189e-7,
+                    "velocityX": 3.725191119725111,
+                    "velocityY": -0.658475565944433,
+                    "timestamp": 0.804578442151583
+                },
+                {
+                    "x": 6.90949802384657,
+                    "y": 5.13364781945348,
+                    "heading": 3.243107110210723,
+                    "angularVelocity": 1.1841422044120688e-7,
+                    "velocityX": 3.523757607622681,
+                    "velocityY": -1.3761435344273285,
+                    "timestamp": 0.8939760468350922
+                },
+                {
+                    "x": 7.194274658905514,
+                    "y": 4.9512421336811645,
+                    "heading": 3.243107206435074,
+                    "angularVelocity": 0.0000010763582143626502,
+                    "velocityX": 3.185506323875142,
+                    "velocityY": -2.0403867242241884,
+                    "timestamp": 0.9833736515186015
+                },
+                {
+                    "x": 7.415334413339818,
+                    "y": 4.749404498288232,
+                    "heading": 3.236772739824744,
+                    "angularVelocity": -0.07085722970314587,
+                    "velocityX": 2.472770441853857,
+                    "velocityY": -2.2577521635961224,
+                    "timestamp": 1.0727712562021108
+                },
+                {
+                    "x": 7.5700702382409935,
+                    "y": 4.542039922027483,
+                    "heading": 3.21849203872718,
+                    "angularVelocity": -0.20448759406641112,
+                    "velocityX": 1.7308721575766997,
+                    "velocityY": -2.3195764248545423,
+                    "timestamp": 1.16216886088562
+                },
+                {
+                    "x": 7.658284258545904,
+                    "y": 4.333806888908036,
+                    "heading": 3.1865144497864955,
+                    "angularVelocity": -0.3577007354214622,
+                    "velocityX": 0.9867604463982368,
+                    "velocityY": -2.3292909676746256,
+                    "timestamp": 1.2515664655691292
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": -0.5203097996961199,
+                    "velocityX": 0.24291189378586123,
+                    "velocityY": -2.3133381441461327,
+                    "timestamp": 1.3409640702526384
+                },
+                {
+                    "x": 7.643813431477439,
+                    "y": 3.93924818499591,
+                    "heading": 3.084542602133154,
+                    "angularVelocity": -0.6748386526461468,
+                    "velocityX": -0.4403397217071775,
+                    "velocityY": -2.284675926642036,
+                    "timestamp": 1.4231428216475348
+                },
+                {
+                    "x": 7.551490590699835,
+                    "y": 3.7550289645599473,
+                    "heading": 3.0195743357345632,
+                    "angularVelocity": -0.7905725664570535,
+                    "velocityX": -1.1234393223010666,
+                    "velocityY": -2.2416892116036538,
+                    "timestamp": 1.5053215730424312
+                },
+                {
+                    "x": 7.403134940422789,
+                    "y": 3.5762585586759936,
+                    "heading": 2.9502561501390385,
+                    "angularVelocity": -0.8435049744515962,
+                    "velocityX": -1.8052799264521386,
+                    "velocityY": -2.175384790327918,
+                    "timestamp": 1.5875003244373276
+                },
+                {
+                    "x": 7.199227882598224,
+                    "y": 3.4065730339632285,
+                    "heading": 2.886416818113751,
+                    "angularVelocity": -0.7768350205485519,
+                    "velocityX": -2.4812625448848915,
+                    "velocityY": -2.064834544462633,
+                    "timestamp": 1.669679075832224
+                },
+                {
+                    "x": 6.942760679940155,
+                    "y": 3.255041432853574,
+                    "heading": 2.8546081791353424,
+                    "angularVelocity": -0.3870664673889389,
+                    "velocityX": -3.1208456967576708,
+                    "velocityY": -1.843926787962309,
+                    "timestamp": 1.7518578272271204
+                },
+                {
+                    "x": 6.651351957860763,
+                    "y": 3.1467563415044117,
+                    "heading": 2.854608138694005,
+                    "angularVelocity": -4.921174528263849e-7,
+                    "velocityX": -3.5460349194222314,
+                    "velocityY": -1.3176774957700823,
+                    "timestamp": 1.8340365786220167
+                },
+                {
+                    "x": 6.345190283970336,
+                    "y": 3.092814329077674,
+                    "heading": 2.85460812188226,
+                    "angularVelocity": -2.045787704737326e-7,
+                    "velocityX": -3.7255576252046376,
+                    "velocityY": -0.6563985399585123,
+                    "timestamp": 1.9162153300169131
+                },
+                {
+                    "x": 6.034627693227853,
+                    "y": 3.0788290052051352,
+                    "heading": 2.8546081056797767,
+                    "angularVelocity": -1.9716489468637527e-7,
+                    "velocityX": -3.7791106030377564,
+                    "velocityY": -0.17018175224969478,
+                    "timestamp": 1.9983940814118095
+                },
+                {
+                    "x": 5.724065078469444,
+                    "y": 3.064844225150663,
+                    "heading": 2.8546080894768826,
+                    "angularVelocity": -1.9716993391114644e-7,
+                    "velocityX": -3.7791108952905743,
+                    "velocityY": -0.17017513428705025,
+                    "timestamp": 2.080572832806706
+                },
+                {
+                    "x": 5.413502463745052,
+                    "y": 3.050859444277176,
+                    "heading": 2.8546080732739703,
+                    "angularVelocity": -1.9717015240502906e-7,
+                    "velocityX": -3.7791108948748344,
+                    "velocityY": -0.170175144292348,
+                    "timestamp": 2.1627515842016023
+                },
+                {
+                    "x": 5.102939848618815,
+                    "y": 3.0368746723382456,
+                    "heading": 2.854608057073056,
+                    "angularVelocity": -1.9714443134713148e-7,
+                    "velocityX": -3.779110899765686,
+                    "velocityY": -0.17017503556443397,
+                    "timestamp": 2.2449303355964987
+                },
+                {
+                    "x": 4.82269393470373,
+                    "y": 3.0246815634320945,
+                    "heading": 2.775318198682859,
+                    "angularVelocity": -0.9648462289850903,
+                    "velocityX": -3.4101992200919007,
+                    "velocityY": -0.1483730125178458,
+                    "timestamp": 2.327109086991395
+                },
+                {
+                    "x": 4.59870268568745,
+                    "y": 3.0148363143730834,
+                    "heading": 2.699114427610376,
+                    "angularVelocity": -0.927292880108085,
+                    "velocityX": -2.725658947290856,
+                    "velocityY": -0.11980285529234247,
+                    "timestamp": 2.4092878383862915
+                },
+                {
+                    "x": 4.43080987518015,
+                    "y": 3.007426917155393,
+                    "heading": 2.637997904363483,
+                    "angularVelocity": -0.7437022612594317,
+                    "velocityX": -2.043019730258355,
+                    "velocityY": -0.09016195906753964,
+                    "timestamp": 2.491466589781188
+                },
+                {
+                    "x": 4.318925850575796,
+                    "y": 3.002477699207252,
+                    "heading": 2.595691429442944,
+                    "angularVelocity": -0.5148103882976692,
+                    "velocityX": -1.361471459539352,
+                    "velocityY": -0.06022503220498214,
+                    "timestamp": 2.5736453411760842
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": -0.26395423478763,
+                    "velocityX": -0.6805390642559688,
+                    "velocityY": -0.03015011990058865,
+                    "timestamp": 2.6558240925709806
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": 0,
+                    "velocityX": 2.4341219671984428e-29,
+                    "velocityY": 0,
+                    "timestamp": 2.738002843965877
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S2 To C3 To S4": {
+            "waypoints": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 18
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": -2.6674512845646027e-30,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.322765898095933,
+                    "y": 5.556151172121073,
+                    "heading": 3.1479530411377072,
+                    "angularVelocity": 0.09380834863319021,
+                    "velocityX": 0.7049555142252463,
+                    "velocityY": -0.04539800325896039,
+                    "timestamp": 0.08477967317075337
+                },
+                {
+                    "x": 4.442297694457063,
+                    "y": 5.548453524989103,
+                    "heading": 3.163858908065189,
+                    "angularVelocity": 0.18761415717719215,
+                    "velocityX": 1.4099110304470142,
+                    "velocityY": -0.09079590477320977,
+                    "timestamp": 0.16955934634150674
+                },
+                {
+                    "x": 4.621595390179887,
+                    "y": 5.536907071113832,
+                    "heading": 3.1877171391651413,
+                    "angularVelocity": 0.28141452081573415,
+                    "velocityX": 2.1148665596022633,
+                    "velocityY": -0.13619365873154282,
+                    "timestamp": 0.2543390195122601
+                },
+                {
+                    "x": 4.860658986379666,
+                    "y": 5.521511826336959,
+                    "heading": 3.2195268977958102,
+                    "angularVelocity": 0.3752050160303855,
+                    "velocityX": 2.819822101912331,
+                    "velocityY": -0.1815912258326068,
+                    "timestamp": 0.3391186926830135
+                },
+                {
+                    "x": 5.159488476433141,
+                    "y": 5.5022678093145005,
+                    "heading": 3.2592867547687217,
+                    "angularVelocity": 0.46897865356654944,
+                    "velocityX": 3.524777566099216,
+                    "velocityY": -0.22698857288071755,
+                    "timestamp": 0.42389836585376683
+                },
+                {
+                    "x": 5.47954195294372,
+                    "y": 5.481656666167451,
+                    "heading": 3.2592867595783463,
+                    "angularVelocity": 5.673087575653179e-8,
+                    "velocityX": 3.7751204332431567,
+                    "velocityY": -0.24311420858382377,
+                    "timestamp": 0.5086780390245202
+                },
+                {
+                    "x": 5.799595429451853,
+                    "y": 5.461045522982243,
+                    "heading": 3.2592867643879506,
+                    "angularVelocity": 5.6730628986964756e-8,
+                    "velocityX": 3.7751204332143167,
+                    "velocityY": -0.2431142090339428,
+                    "timestamp": 0.5934577121952737
+                },
+                {
+                    "x": 6.1196488941034595,
+                    "y": 5.440434195687293,
+                    "heading": 3.259286769197555,
+                    "angularVelocity": 5.673063176342314e-8,
+                    "velocityX": 3.7751202933632646,
+                    "velocityY": -0.24311638065988433,
+                    "timestamp": 0.6782373853660271
+                },
+                {
+                    "x": 6.4368892365574455,
+                    "y": 5.393342668998585,
+                    "heading": 3.2592867741163385,
+                    "angularVelocity": 5.801842614465255e-8,
+                    "velocityX": 3.741938728815606,
+                    "velocityY": -0.5554577521660955,
+                    "timestamp": 0.7630170585367805
+                },
+                {
+                    "x": 6.7398295350074555,
+                    "y": 5.288051546129373,
+                    "heading": 3.2592867797575464,
+                    "angularVelocity": 6.653963355876342e-8,
+                    "velocityX": 3.5732657029690613,
+                    "velocityY": -1.2419382964267203,
+                    "timestamp": 0.8477967317075339
+                },
+                {
+                    "x": 7.017891386263095,
+                    "y": 5.128237130868345,
+                    "heading": 3.259286787454074,
+                    "angularVelocity": 9.07826943070083e-8,
+                    "velocityX": 3.2798174474623285,
+                    "velocityY": -1.8850558074130808,
+                    "timestamp": 0.9325764048782873
+                },
+                {
+                    "x": 7.261365099007722,
+                    "y": 4.919479947434011,
+                    "heading": 3.259286723898943,
+                    "angularVelocity": -7.496505777325929e-7,
+                    "velocityX": 2.8718406622562314,
+                    "velocityY": -2.462349471599812,
+                    "timestamp": 1.0173560780490407
+                },
+                {
+                    "x": 7.457139881435657,
+                    "y": 4.674807078885688,
+                    "heading": 3.2408811636287,
+                    "angularVelocity": -0.21709874055771505,
+                    "velocityX": 2.309218414106716,
+                    "velocityY": -2.885985040961276,
+                    "timestamp": 1.102135751219794
+                },
+                {
+                    "x": 7.597538484149158,
+                    "y": 4.408115178432638,
+                    "heading": 3.1997348588691503,
+                    "angularVelocity": -0.48533219368821684,
+                    "velocityX": 1.6560408581747834,
+                    "velocityY": -3.1457056919299244,
+                    "timestamp": 1.1869154243905473
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": -0.7045893978594314,
+                    "velocityX": 0.9726566848724733,
+                    "velocityY": -3.315832297049189,
+                    "timestamp": 1.2716950975613006
+                },
+                {
+                    "x": 7.707365632796218,
+                    "y": 3.8777713806946585,
+                    "heading": 3.0766920768105654,
+                    "angularVelocity": -0.8684660682637303,
+                    "velocityX": 0.37540519927184846,
+                    "velocityY": -3.418949607021354,
+                    "timestamp": 1.344591351660309
+                },
+                {
+                    "x": 7.690646428306815,
+                    "y": 3.624138174667908,
+                    "heading": 3.0114310872436763,
+                    "angularVelocity": -0.8952584789497043,
+                    "velocityX": -0.229356154113369,
+                    "velocityY": -3.4793722827239795,
+                    "timestamp": 1.4174876057593173
+                },
+                {
+                    "x": 7.629817161501398,
+                    "y": 3.371341042658395,
+                    "heading": 2.9608367604127053,
+                    "angularVelocity": -0.6940593512835794,
+                    "velocityX": -0.8344635476447348,
+                    "velocityY": -3.4679029139977815,
+                    "timestamp": 1.4903838598583257
+                },
+                {
+                    "x": 7.52574877175356,
+                    "y": 3.1246562301261127,
+                    "heading": 2.9373487413645845,
+                    "angularVelocity": -0.32221160522605474,
+                    "velocityX": -1.4276232850943003,
+                    "velocityY": -3.3840533451439727,
+                    "timestamp": 1.563280113957334
+                },
+                {
+                    "x": 7.379748340613123,
+                    "y": 2.890715038062695,
+                    "heading": 2.9373478462882416,
+                    "angularVelocity": -0.00001227877368205191,
+                    "velocityX": -2.002852313126858,
+                    "velocityY": -3.2092347536203247,
+                    "timestamp": 1.6361763680563424
+                },
+                {
+                    "x": 7.198165609339985,
+                    "y": 2.6831759130730863,
+                    "heading": 2.937347790914402,
+                    "angularVelocity": -7.596252972489614e-7,
+                    "velocityX": -2.4909747903727215,
+                    "velocityY": -2.847047870356197,
+                    "timestamp": 1.7090726221553507
+                },
+                {
+                    "x": 6.985688529633763,
+                    "y": 2.5073968727951343,
+                    "heading": 2.9373477481889543,
+                    "angularVelocity": -5.861130796388339e-7,
+                    "velocityX": -2.91478735543415,
+                    "velocityY": -2.4113590259261937,
+                    "timestamp": 1.781968876254359
+                },
+                {
+                    "x": 6.7478025524071645,
+                    "y": 2.367915534392956,
+                    "heading": 2.937347710107927,
+                    "angularVelocity": -5.224003305629458e-7,
+                    "velocityX": -3.263349813607291,
+                    "velocityY": -1.9134225774154996,
+                    "timestamp": 1.8548651303533674
+                },
+                {
+                    "x": 6.4987639591558635,
+                    "y": 2.249490193910499,
+                    "heading": 2.937347672689208,
+                    "angularVelocity": -5.133147047667194e-7,
+                    "velocityX": -3.4163428056670964,
+                    "velocityY": -1.624573744513203,
+                    "timestamp": 1.9277613844523758
+                },
+                {
+                    "x": 6.249725239429151,
+                    "y": 2.1310651193958834,
+                    "heading": 2.937347635270487,
+                    "angularVelocity": -5.133147287283584e-7,
+                    "velocityX": -3.4163445406727813,
+                    "velocityY": -1.6245700959327491,
+                    "timestamp": 2.000657638551384
+                },
+                {
+                    "x": 6.0006865195043355,
+                    "y": 2.012640045297908,
+                    "heading": 2.937347597851683,
+                    "angularVelocity": -5.133158656564982e-7,
+                    "velocityX": -3.4163445433903696,
+                    "velocityY": -1.6245700902172364,
+                    "timestamp": 2.0735538926503922
+                },
+                {
+                    "x": 5.766671175118509,
+                    "y": 1.901395758932124,
+                    "heading": 2.8964169231036223,
+                    "angularVelocity": -0.5614921542139502,
+                    "velocityX": -3.2102519845241893,
+                    "velocityY": -1.5260631391951232,
+                    "timestamp": 2.1464501467494004
+                },
+                {
+                    "x": 5.568987668741884,
+                    "y": 1.8073555304166649,
+                    "heading": 2.790644028809865,
+                    "angularVelocity": -1.4510058932587933,
+                    "velocityX": -2.7118472522341635,
+                    "velocityY": -1.2900557055764181,
+                    "timestamp": 2.2193464008484085
+                },
+                {
+                    "x": 5.41134573614175,
+                    "y": 1.7322845721056195,
+                    "heading": 2.684499308002556,
+                    "angularVelocity": -1.456106656228851,
+                    "velocityX": -2.162551897193064,
+                    "velocityY": -1.029832866434731,
+                    "timestamp": 2.2922426549474166
+                },
+                {
+                    "x": 5.293483125084031,
+                    "y": 1.6760930732996284,
+                    "heading": 2.5959515176489267,
+                    "angularVelocity": -1.2147097467272172,
+                    "velocityX": -1.6168541513476236,
+                    "velocityY": -0.7708420617846653,
+                    "timestamp": 2.3651389090464248
+                },
+                {
+                    "x": 5.2151080549458655,
+                    "y": 1.6386848703250345,
+                    "heading": 2.5329322459715486,
+                    "angularVelocity": -0.8645063104618199,
+                    "velocityX": -1.075159088856404,
+                    "velocityY": -0.5131704425282329,
+                    "timestamp": 2.438035163145433
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": -0.45176870030861394,
+                    "velocityX": -0.536489226081922,
+                    "velocityY": -0.25632140575671664,
+                    "timestamp": 2.510931417244441
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": 0,
+                    "velocityX": 2.4074157612584747e-31,
+                    "velocityY": 0,
+                    "timestamp": 2.583827671343449
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S3 To W2": {
+            "waypoints": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.228111844192817,
+                    "y": 3.0443410125088586,
+                    "heading": 2.5974492034990555,
+                    "angularVelocity": 0.2841110410157656,
+                    "velocityX": -0.42270562690532876,
+                    "velocityY": 0.5372366368048371,
+                    "timestamp": 0.08253534750081891
+                },
+                {
+                    "x": 4.158301873890454,
+                    "y": 3.133062319081129,
+                    "heading": 2.642673247648486,
+                    "angularVelocity": 0.5479354666675527,
+                    "velocityX": -0.8458190631798037,
+                    "velocityY": 1.0749492097478566,
+                    "timestamp": 0.16507069500163782
+                },
+                {
+                    "x": 4.053518280244317,
+                    "y": 3.266222365985673,
+                    "heading": 2.7068891131091086,
+                    "angularVelocity": 0.7780407716825238,
+                    "velocityX": -1.2695602165496185,
+                    "velocityY": 1.6133699189092645,
+                    "timestamp": 0.2476060425024567
+                },
+                {
+                    "x": 3.9136724453244414,
+                    "y": 3.4439114508470956,
+                    "heading": 2.784478333107965,
+                    "angularVelocity": 0.9400726155310244,
+                    "velocityX": -1.694375066616006,
+                    "velocityY": 2.152884675982738,
+                    "timestamp": 0.33014139000327564
+                },
+                {
+                    "x": 3.738653008239037,
+                    "y": 3.6661781083911804,
+                    "heading": 2.8571480043531183,
+                    "angularVelocity": 0.8804672597329689,
+                    "velocityX": -2.1205391675811076,
+                    "velocityY": 2.6929874808109258,
+                    "timestamp": 0.41267673750409456
+                },
+                {
+                    "x": 3.545725477812246,
+                    "y": 3.911665858291861,
+                    "heading": 2.8571480229647763,
+                    "angularVelocity": 2.254992345614167e-7,
+                    "velocityX": -2.3375140018024174,
+                    "velocityY": 2.9743347224441523,
+                    "timestamp": 0.4952120850049135
+                },
+                {
+                    "x": 3.352797950657087,
+                    "y": 4.157153610763737,
+                    "heading": 2.8571480415763673,
+                    "angularVelocity": 2.2549842301439466e-7,
+                    "velocityX": -2.3375139621632335,
+                    "velocityY": 2.9743347535968137,
+                    "timestamp": 0.5777474325057324
+                },
+                {
+                    "x": 3.1598704235019452,
+                    "y": 4.402641363235625,
+                    "heading": 2.8571480601879577,
+                    "angularVelocity": 2.2549842390160243e-7,
+                    "velocityX": -2.3375139621630274,
+                    "velocityY": 2.974334753596974,
+                    "timestamp": 0.6602827800065513
+                },
+                {
+                    "x": 2.966942896346795,
+                    "y": 4.648129115707507,
+                    "heading": 2.8571480787995487,
+                    "angularVelocity": 2.2549842409910227e-7,
+                    "velocityX": -2.337513962163132,
+                    "velocityY": 2.974334753596892,
+                    "timestamp": 0.7428181275073702
+                },
+                {
+                    "x": 2.7740153675253203,
+                    "y": 4.8936168668697935,
+                    "heading": 2.857148097411205,
+                    "angularVelocity": 2.2549920897733148e-7,
+                    "velocityX": -2.3375139823523616,
+                    "velocityY": 2.9743347377297993,
+                    "timestamp": 0.8253534750081891
+                },
+                {
+                    "x": 2.599116566256145,
+                    "y": 5.1159598004085165,
+                    "heading": 2.929233412420125,
+                    "angularVelocity": 0.873387187328517,
+                    "velocityX": -2.1190775415035294,
+                    "velocityY": 2.693911642360469,
+                    "timestamp": 0.907888822509008
+                },
+                {
+                    "x": 2.459351765780957,
+                    "y": 5.293700413500499,
+                    "heading": 3.0069352217627348,
+                    "angularVelocity": 0.9414367503794493,
+                    "velocityX": -1.6933932515859442,
+                    "velocityY": 2.1535089931040625,
+                    "timestamp": 0.990424170009827
+                },
+                {
+                    "x": 2.354631169439892,
+                    "y": 5.426897486256648,
+                    "heading": 3.071284016075413,
+                    "angularVelocity": 0.7796513404397946,
+                    "velocityX": -1.268796939881139,
+                    "velocityY": 1.6138185249032515,
+                    "timestamp": 1.0729595175106459
+                },
+                {
+                    "x": 2.284865378797646,
+                    "y": 5.515644940856661,
+                    "heading": 3.116557551514127,
+                    "angularVelocity": 0.5485351041656966,
+                    "velocityX": -0.8452837814919688,
+                    "velocityY": 1.0752660197999702,
+                    "timestamp": 1.1554948650114647
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0.2840291971344776,
+                    "velocityX": -0.4224296601804454,
+                    "velocityY": 0.5374068261225751,
+                    "timestamp": 1.2380302125122835
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 2.192310159207654e-31,
+                    "velocityX": 0,
+                    "velocityY": 7.442690740363628e-33,
+                    "timestamp": 1.3205655600131023
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S3 To W3": {
+            "waypoints": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 12
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 2.097905783412131e-34,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.206954796561596,
+                    "y": 3.0388324489204006,
+                    "heading": 2.584970820102585,
+                    "angularVelocity": 0.12125870375097321,
+                    "velocityX": -0.619458587129609,
+                    "velocityY": 0.42920878982002253,
+                    "timestamp": 0.0904744959596105
+                },
+                {
+                    "x": 4.094846273424844,
+                    "y": 3.1165082311139285,
+                    "heading": 2.6050859488479765,
+                    "angularVelocity": 0.2223292711613592,
+                    "velocityX": -1.2391174103561666,
+                    "velocityY": 0.858537882633839,
+                    "timestamp": 0.180948991919221
+                },
+                {
+                    "x": 3.9266509261170293,
+                    "y": 3.2330407963022716,
+                    "heading": 2.631256329120159,
+                    "angularVelocity": 0.2892569888851994,
+                    "velocityX": -1.8590360247256863,
+                    "velocityY": 1.2880156330505033,
+                    "timestamp": 0.2714234878788315
+                },
+                {
+                    "x": 3.702348687884298,
+                    "y": 3.38843810676698,
+                    "heading": 2.65711164443334,
+                    "angularVelocity": 0.2857746267492099,
+                    "velocityX": -2.479176433686503,
+                    "velocityY": 1.7175813892799228,
+                    "timestamp": 0.361897983838442
+                },
+                {
+                    "x": 3.4221471625561564,
+                    "y": 3.582533883045857,
+                    "heading": 2.6614389879992997,
+                    "angularVelocity": 0.04782942994113533,
+                    "velocityX": -3.0970222310299356,
+                    "velocityY": 2.1453092854534943,
+                    "timestamp": 0.4523724797980525
+                },
+                {
+                    "x": 3.1408345803954827,
+                    "y": 3.7774823101067563,
+                    "heading": 2.6614389980641375,
+                    "angularVelocity": 1.1124502748712501e-7,
+                    "velocityX": -3.1093025628598916,
+                    "velocityY": 2.154733496917496,
+                    "timestamp": 0.542846975757663
+                },
+                {
+                    "x": 2.8606418991642766,
+                    "y": 3.9715702772190298,
+                    "heading": 2.6657880248697206,
+                    "angularVelocity": 0.04806909128871381,
+                    "velocityX": -3.096924478654065,
+                    "velocityY": 2.1452229719955302,
+                    "timestamp": 0.6333214717172736
+                },
+                {
+                    "x": 2.6363424534806392,
+                    "y": 4.126965038945093,
+                    "heading": 2.691984108325433,
+                    "angularVelocity": 0.28954108202389867,
+                    "velocityX": -2.479145568092125,
+                    "velocityY": 1.7175532184830777,
+                    "timestamp": 0.7237959676768841
+                },
+                {
+                    "x": 2.46814994135131,
+                    "y": 4.24349512219199,
+                    "heading": 2.7185033740588933,
+                    "angularVelocity": 0.2931131635737346,
+                    "velocityX": -1.8590046879555269,
+                    "velocityY": 1.2879882005523255,
+                    "timestamp": 0.8142704636364947
+                },
+                {
+                    "x": 2.3560438127968273,
+                    "y": 4.3211688040728875,
+                    "heading": 2.738885032189894,
+                    "angularVelocity": 0.22527517743895228,
+                    "velocityX": -1.2390909434248611,
+                    "velocityY": 0.8585146682173574,
+                    "timestamp": 0.9047449595961052
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 0.12285194509475847,
+                    "velocityX": -0.6194432165927374,
+                    "velocityY": 0.42919494068745534,
+                    "timestamp": 0.9952194555557158
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 6.588066214768088e-31,
+                    "velocityX": -4.3339190939808694e-33,
+                    "velocityY": -1.4815259513582048e-34,
+                    "timestamp": 1.0856939515153263
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S3 To C3 To S1": {
+            "waypoints": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 17
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.308593242288023,
+                    "y": 2.998690970742917,
+                    "heading": 2.600069907206834,
+                    "angularVelocity": 0.35025874949099595,
+                    "velocityX": 0.6125619052781636,
+                    "velocityY": -0.017587287403649503,
+                    "timestamp": 0.07443042392151207
+                },
+                {
+                    "x": 4.399870755267574,
+                    "y": 2.9960680712021444,
+                    "heading": 2.65011196683443,
+                    "angularVelocity": 0.6723333952842253,
+                    "velocityX": 1.2263468104898216,
+                    "velocityY": -0.03523961577241709,
+                    "timestamp": 0.14886084784302414
+                },
+                {
+                    "x": 4.536960438371983,
+                    "y": 2.992122797800661,
+                    "heading": 2.720779852914046,
+                    "angularVelocity": 0.9494489263440683,
+                    "velocityX": 1.8418500914219216,
+                    "velocityY": -0.05300619281227453,
+                    "timestamp": 0.22329127176453623
+                },
+                {
+                    "x": 4.72004322969291,
+                    "y": 2.9868327872754663,
+                    "heading": 2.805997286445448,
+                    "angularVelocity": 1.1449274240499019,
+                    "velocityX": 2.4597843418706042,
+                    "velocityY": -0.07107322847942685,
+                    "timestamp": 0.2977216956860483
+                },
+                {
+                    "x": 4.949295920013874,
+                    "y": 2.980134342239005,
+                    "heading": 2.892264553695649,
+                    "angularVelocity": 1.1590323244855822,
+                    "velocityX": 3.0800938412323915,
+                    "velocityY": -0.08999606187282569,
+                    "timestamp": 0.37215211960756034
+                },
+                {
+                    "x": 5.2218046444028055,
+                    "y": 2.971482120946463,
+                    "heading": 2.9184186664707665,
+                    "angularVelocity": 0.35139008213490475,
+                    "velocityX": 3.661254498245178,
+                    "velocityY": -0.11624576129869893,
+                    "timestamp": 0.4465825435290724
+                },
+                {
+                    "x": 5.503268186944086,
+                    "y": 2.9638921982075033,
+                    "heading": 2.918418695115671,
+                    "angularVelocity": 3.8485478250287795e-7,
+                    "velocityX": 3.7815657591590193,
+                    "velocityY": -0.10197339124338625,
+                    "timestamp": 0.5210129674505845
+                },
+                {
+                    "x": 5.784731729781038,
+                    "y": 2.9563022864327206,
+                    "heading": 2.9184187237605435,
+                    "angularVelocity": 3.8485435121714565e-7,
+                    "velocityX": 3.781565763131487,
+                    "velocityY": -0.10197324393565109,
+                    "timestamp": 0.5954433913720966
+                },
+                {
+                    "x": 6.066195282639446,
+                    "y": 2.9487127463017835,
+                    "heading": 2.9184187524054175,
+                    "angularVelocity": 3.848543706133971e-7,
+                    "velocityX": 3.7815658977733997,
+                    "velocityY": -0.10196825076449405,
+                    "timestamp": 0.6698738152936087
+                },
+                {
+                    "x": 6.347042051479281,
+                    "y": 2.968823062647617,
+                    "heading": 2.918418781616621,
+                    "angularVelocity": 3.924632205757564e-7,
+                    "velocityX": 3.773279178632548,
+                    "velocityY": 0.2701894640159414,
+                    "timestamp": 0.7443042392151208
+                },
+                {
+                    "x": 6.6208214562671275,
+                    "y": 3.0345814714160126,
+                    "heading": 2.918418814269471,
+                    "angularVelocity": 4.3870299176986663e-7,
+                    "velocityX": 3.678326554696914,
+                    "velocityY": 0.8834883009364279,
+                    "timestamp": 0.8187346631366329
+                },
+                {
+                    "x": 6.88016498053821,
+                    "y": 3.1442184338201953,
+                    "heading": 2.9184188559848527,
+                    "angularVelocity": 5.604614269784904e-7,
+                    "velocityX": 3.4843752138851785,
+                    "velocityY": 1.4730127362944399,
+                    "timestamp": 0.893165087058145
+                },
+                {
+                    "x": 7.118091210487748,
+                    "y": 3.294782489019261,
+                    "heading": 2.918422820305874,
+                    "angularVelocity": 0.00005326210462723065,
+                    "velocityX": 3.196626022181901,
+                    "velocityY": 2.022883214501594,
+                    "timestamp": 0.9675955109796571
+                },
+                {
+                    "x": 7.320693053454344,
+                    "y": 3.474127369690306,
+                    "heading": 2.9455538073395324,
+                    "angularVelocity": 0.36451474550610485,
+                    "velocityX": 2.722029948133077,
+                    "velocityY": 2.4095641435572857,
+                    "timestamp": 1.0420259349011691
+                },
+                {
+                    "x": 7.483696807015107,
+                    "y": 3.6758684658318805,
+                    "heading": 3.001437207311255,
+                    "angularVelocity": 0.7508139417646484,
+                    "velocityX": 2.1900151171065767,
+                    "velocityY": 2.710465499354305,
+                    "timestamp": 1.1164563588226812
+                },
+                {
+                    "x": 7.603947516595235,
+                    "y": 3.8947228146234494,
+                    "heading": 3.0702288107606313,
+                    "angularVelocity": 0.9242403821576988,
+                    "velocityX": 1.6156123160998233,
+                    "velocityY": 2.940388315164694,
+                    "timestamp": 1.1908867827441934
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": 0.9374014759467769,
+                    "velocityX": 1.0217929631162956,
+                    "velocityY": 3.12072903980085,
+                    "timestamp": 1.2653172066657055
+                },
+                {
+                    "x": 7.709300887372812,
+                    "y": 4.397378653394831,
+                    "heading": 3.2093959242930556,
+                    "angularVelocity": 0.8431459366282483,
+                    "velocityX": 0.355999640896236,
+                    "velocityY": 3.2850439746024853,
+                    "timestamp": 1.3476231530960114
+                },
+                {
+                    "x": 7.683172834427827,
+                    "y": 4.6781135002756455,
+                    "heading": 3.266996168893978,
+                    "angularVelocity": 0.6998309004282659,
+                    "velocityX": -0.31745036753947053,
+                    "velocityY": 3.4108695550756014,
+                    "timestamp": 1.4299290995263174
+                },
+                {
+                    "x": 7.601002938928111,
+                    "y": 4.963519420188029,
+                    "heading": 3.305651937027416,
+                    "angularVelocity": 0.4696594816046569,
+                    "velocityX": -0.9983470097059609,
+                    "velocityY": 3.467622113477001,
+                    "timestamp": 1.5122350459566234
+                },
+                {
+                    "x": 7.463684437134381,
+                    "y": 5.241326726377331,
+                    "heading": 3.3097255928301426,
+                    "angularVelocity": 0.049494064273674344,
+                    "velocityX": -1.6683910185032116,
+                    "velocityY": 3.3753005492080685,
+                    "timestamp": 1.5945409923869294
+                },
+                {
+                    "x": 7.277476070304,
+                    "y": 5.490867404030893,
+                    "heading": 3.3097256042381358,
+                    "angularVelocity": 1.3860472723422523e-7,
+                    "velocityX": -2.2623926326885195,
+                    "velocityY": 3.031866936429266,
+                    "timestamp": 1.6768469388172353
+                },
+                {
+                    "x": 7.049248371933673,
+                    "y": 5.702660296381284,
+                    "heading": 3.3097256090357283,
+                    "angularVelocity": 5.8289739150802425e-8,
+                    "velocityX": -2.7729187047691926,
+                    "velocityY": 2.573239255923398,
+                    "timestamp": 1.7591528852475413
+                },
+                {
+                    "x": 6.786512642610724,
+                    "y": 5.869735280017231,
+                    "heading": 3.309725613101849,
+                    "angularVelocity": 4.940251326866522e-8,
+                    "velocityX": -3.192184048881866,
+                    "velocityY": 2.029926036722274,
+                    "timestamp": 1.8414588316778473
+                },
+                {
+                    "x": 6.5049501154842,
+                    "y": 6.00265125873641,
+                    "heading": 3.309725617013439,
+                    "angularVelocity": 4.7524998408670815e-8,
+                    "velocityX": -3.4209256966013024,
+                    "velocityY": 1.6149012857986877,
+                    "timestamp": 1.9237647781081533
+                },
+                {
+                    "x": 6.223387384136782,
+                    "y": 6.135566804843462,
+                    "heading": 3.309725620925028,
+                    "angularVelocity": 4.75249886637903e-8,
+                    "velocityX": -3.4209281778423657,
+                    "velocityY": 1.6148960296520045,
+                    "timestamp": 2.0060707245384592
+                },
+                {
+                    "x": 5.941824652769856,
+                    "y": 6.268482350909119,
+                    "heading": 3.3097256248366427,
+                    "angularVelocity": 4.752530281025494e-8,
+                    "velocityX": -3.42092817807938,
+                    "velocityY": 1.6148960291490546,
+                    "timestamp": 2.0883766709687652
+                },
+                {
+                    "x": 5.686549787544516,
+                    "y": 6.388988216738383,
+                    "heading": 3.340879096998464,
+                    "angularVelocity": 0.3785081578303866,
+                    "velocityX": -3.1015361136937893,
+                    "velocityY": 1.464121015014444,
+                    "timestamp": 2.170682617399071
+                },
+                {
+                    "x": 5.482329878299888,
+                    "y": 6.485392923986267,
+                    "heading": 3.365801660864199,
+                    "angularVelocity": 0.30280392786490506,
+                    "velocityX": -2.4812290982833782,
+                    "velocityY": 1.1712969892098404,
+                    "timestamp": 2.252988563829377
+                },
+                {
+                    "x": 5.32916494108436,
+                    "y": 6.557696460239907,
+                    "heading": 3.3844934905089157,
+                    "angularVelocity": 0.22710181287501843,
+                    "velocityX": -1.8609218878884095,
+                    "velocityY": 0.8784728125915443,
+                    "timestamp": 2.335294510259683
+                },
+                {
+                    "x": 5.227054980864034,
+                    "y": 6.6058988198426265,
+                    "heading": 3.396954693375837,
+                    "angularVelocity": 0.1514010032977761,
+                    "velocityX": -1.2406146171563652,
+                    "velocityY": 0.5856485672458114,
+                    "timestamp": 2.417600456689989
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": 0.07570065194530232,
+                    "velocityX": -0.6203073177375487,
+                    "velocityY": 0.29282428794839654,
+                    "timestamp": 2.499906403120295
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": 0,
+                    "velocityX": 1.5419247738054587e-33,
+                    "velocityY": 0,
+                    "timestamp": 2.582212349550601
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S3 To C3 To S2": {
+            "waypoints": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 17
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.312496748515021,
+                    "y": 3.002119555669804,
+                    "heading": 2.5925232531186833,
+                    "angularVelocity": 0.239650083468562,
+                    "velocityX": 0.6403788706575965,
+                    "velocityY": 0.027422380395612005,
+                    "timestamp": 0.07729291327834321
+                },
+                {
+                    "x": 4.411499664032465,
+                    "y": 3.0063632003056355,
+                    "heading": 2.629246894113083,
+                    "angularVelocity": 0.4751230020552096,
+                    "velocityX": 1.2808795957906263,
+                    "velocityY": 0.05490341165625346,
+                    "timestamp": 0.15458582655668643
+                },
+                {
+                    "x": 4.560023871242694,
+                    "y": 3.012738860842861,
+                    "heading": 2.6836373496883423,
+                    "angularVelocity": 0.7036926578170436,
+                    "velocityX": 1.9215760010930336,
+                    "velocityY": 0.08248699999527603,
+                    "timestamp": 0.23187873983502966
+                },
+                {
+                    "x": 4.758098277209683,
+                    "y": 3.021263020053228,
+                    "heading": 2.7546133652741296,
+                    "angularVelocity": 0.9182732617438291,
+                    "velocityX": 2.5626464001129405,
+                    "velocityY": 0.11028383908458261,
+                    "timestamp": 0.30917165311337286
+                },
+                {
+                    "x": 5.005797064956168,
+                    "y": 3.0319826680841246,
+                    "heading": 2.8389548559272906,
+                    "angularVelocity": 1.0911930612504541,
+                    "velocityX": 3.2046765640012143,
+                    "velocityY": 0.13868862714869012,
+                    "timestamp": 0.38646456639171606
+                },
+                {
+                    "x": 5.297951167759167,
+                    "y": 3.043836589819136,
+                    "heading": 2.838954877461829,
+                    "angularVelocity": 2.786094794708315e-7,
+                    "velocityX": 3.7798303933881865,
+                    "velocityY": 0.15336362975896514,
+                    "timestamp": 0.46375747967005926
+                },
+                {
+                    "x": 5.590081141172345,
+                    "y": 3.0562710547940686,
+                    "heading": 2.838954891655661,
+                    "angularVelocity": 1.83636908600502e-7,
+                    "velocityX": 3.7795182122476696,
+                    "velocityY": 0.1608745801850421,
+                    "timestamp": 0.5410503929484025
+                },
+                {
+                    "x": 5.882211114419822,
+                    "y": 3.0687055236618757,
+                    "heading": 2.8389549058494925,
+                    "angularVelocity": 1.8363690785211144e-7,
+                    "velocityX": 3.779518210103882,
+                    "velocityY": 0.16087463055026802,
+                    "timestamp": 0.6183433062267457
+                },
+                {
+                    "x": 6.17434108478517,
+                    "y": 3.0811400602410006,
+                    "heading": 2.838954920043324,
+                    "angularVelocity": 1.8363690859499724e-7,
+                    "velocityX": 3.779518172815467,
+                    "velocityY": 0.1608755065855243,
+                    "timestamp": 0.6956362195050889
+                },
+                {
+                    "x": 6.465903802710874,
+                    "y": 3.1031790875652407,
+                    "heading": 2.8389549343224108,
+                    "angularVelocity": 1.8473992278083087e-7,
+                    "velocityX": 3.772179176061626,
+                    "velocityY": 0.28513645545840094,
+                    "timestamp": 0.7729291327834321
+                },
+                {
+                    "x": 6.7494946065782635,
+                    "y": 3.174388582838956,
+                    "heading": 2.838954949771126,
+                    "angularVelocity": 1.998723373980939e-7,
+                    "velocityX": 3.669040172494696,
+                    "velocityY": 0.9212939744848142,
+                    "timestamp": 0.8502220460617753
+                },
+                {
+                    "x": 7.016882485558567,
+                    "y": 3.2927027326639142,
+                    "heading": 2.838955049089198,
+                    "angularVelocity": 0.000001284957026149309,
+                    "velocityX": 3.4594100240134633,
+                    "velocityY": 1.5307244197005683,
+                    "timestamp": 0.9275149593401185
+                },
+                {
+                    "x": 7.246855043486079,
+                    "y": 3.4413903121844696,
+                    "heading": 2.884377696826009,
+                    "angularVelocity": 0.5876689829666233,
+                    "velocityX": 2.9753382059665197,
+                    "velocityY": 1.9236896788339406,
+                    "timestamp": 1.0048078726184617
+                },
+                {
+                    "x": 7.429257242099238,
+                    "y": 3.603847510115455,
+                    "heading": 2.9490702967114695,
+                    "angularVelocity": 0.8369797066969458,
+                    "velocityX": 2.359882567193478,
+                    "velocityY": 2.101838202759324,
+                    "timestamp": 1.082100785896805
+                },
+                {
+                    "x": 7.562439804570718,
+                    "y": 3.7739114806600167,
+                    "heading": 3.01726271663618,
+                    "angularVelocity": 0.8822596669262517,
+                    "velocityX": 1.7230889200911585,
+                    "velocityY": 2.2002530805396896,
+                    "timestamp": 1.1593936991751483
+                },
+                {
+                    "x": 7.646052714432655,
+                    "y": 3.9488127153581356,
+                    "heading": 3.0821685087673805,
+                    "angularVelocity": 0.8397379446349181,
+                    "velocityX": 1.0817668310784703,
+                    "velocityY": 2.2628366208461252,
+                    "timestamp": 1.2366866124534917
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": 0.7482120776630553,
+                    "velocityX": 0.43920307991361807,
+                    "velocityY": 2.305350867034668,
+                    "timestamp": 1.313979525731835
+                },
+                {
+                    "x": 7.646672814687191,
+                    "y": 4.349453153850708,
+                    "heading": 3.196736252005302,
+                    "angularVelocity": 0.5972944316963834,
+                    "velocityX": -0.35085402203857,
+                    "velocityY": 2.34188945184307,
+                    "timestamp": 1.4089682766635396
+                },
+                {
+                    "x": 7.538236721836807,
+                    "y": 4.57348977092197,
+                    "heading": 3.2390603114191148,
+                    "angularVelocity": 0.44556917528311457,
+                    "velocityX": -1.1415677307763321,
+                    "velocityY": 2.3585594596600363,
+                    "timestamp": 1.5039570275952443
+                },
+                {
+                    "x": 7.354699862509888,
+                    "y": 4.7956671896432965,
+                    "heading": 3.2668511132107456,
+                    "angularVelocity": 0.2925693992082484,
+                    "velocityX": -1.932195733986223,
+                    "velocityY": 2.3389866330705615,
+                    "timestamp": 1.598945778526949
+                },
+                {
+                    "x": 7.096712135037422,
+                    "y": 5.0078309608342675,
+                    "heading": 3.2799432590323385,
+                    "angularVelocity": 0.13782838171022743,
+                    "velocityX": -2.7159818919816576,
+                    "velocityY": 2.2335673341310986,
+                    "timestamp": 1.6939345294586536
+                },
+                {
+                    "x": 6.778591882911988,
+                    "y": 5.174931015852947,
+                    "heading": 3.2799432523824414,
+                    "angularVelocity": -7.000720422097915e-8,
+                    "velocityX": -3.349030795806095,
+                    "velocityY": 1.7591562514473036,
+                    "timestamp": 1.7889232803903583
+                },
+                {
+                    "x": 6.432650840523452,
+                    "y": 5.272130447349719,
+                    "heading": 3.2799432455857884,
+                    "angularVelocity": -7.155218790875671e-8,
+                    "velocityX": -3.6419159005181747,
+                    "velocityY": 1.0232730775316359,
+                    "timestamp": 1.883912031322063
+                },
+                {
+                    "x": 6.076435890184285,
+                    "y": 5.319394007657195,
+                    "heading": 3.2799432391201027,
+                    "angularVelocity": -6.806791276827721e-8,
+                    "velocityX": -3.7500751072648586,
+                    "velocityY": 0.4975700790239653,
+                    "timestamp": 1.9789007822537676
+                },
+                {
+                    "x": 5.72022088924427,
+                    "y": 5.36665718659578,
+                    "heading": 3.279943232654418,
+                    "angularVelocity": -6.806790136756466e-8,
+                    "velocityX": -3.7500756399684434,
+                    "velocityY": 0.49756606413917676,
+                    "timestamp": 2.073889533185472
+                },
+                {
+                    "x": 5.364005888304073,
+                    "y": 5.413920365532998,
+                    "heading": 3.279943226188733,
+                    "angularVelocity": -6.806790152018608e-8,
+                    "velocityX": -3.750075639970353,
+                    "velocityY": 0.4975660641247859,
+                    "timestamp": 2.1688782841171768
+                },
+                {
+                    "x": 5.007790887255383,
+                    "y": 5.4611835436519325,
+                    "heading": 3.2799432197229685,
+                    "angularVelocity": -6.806874126926854e-8,
+                    "velocityX": -3.750075641112522,
+                    "velocityY": 0.4975660555102637,
+                    "timestamp": 2.2638670350488814
+                },
+                {
+                    "x": 4.7098745121627585,
+                    "y": 5.500710065242911,
+                    "heading": 3.22396872446115,
+                    "angularVelocity": -0.5892749900676477,
+                    "velocityX": -3.1363332202022756,
+                    "velocityY": 0.416117921367316,
+                    "timestamp": 2.358855785980586
+                },
+                {
+                    "x": 4.4864372535551835,
+                    "y": 5.530355016492048,
+                    "heading": 3.181984877578949,
+                    "angularVelocity": -0.44198756663709243,
+                    "velocityX": -2.352249676050825,
+                    "velocityY": 0.3120890732677509,
+                    "timestamp": 2.4538445369122908
+                },
+                {
+                    "x": 4.337479086660562,
+                    "y": 5.550118338543265,
+                    "heading": 3.1539948813381518,
+                    "angularVelocity": -0.2946664311958542,
+                    "velocityX": -1.5681663926891658,
+                    "velocityY": 0.20805960555714925,
+                    "timestamp": 2.5488332878439954
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": -0.147331986165536,
+                    "velocityX": -0.7840832301722918,
+                    "velocityY": 0.10402980731729136,
+                    "timestamp": 2.6438220387757
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": -1.2414335719227057e-31,
+                    "velocityX": 3.5860623226783847e-34,
+                    "velocityY": 0,
+                    "timestamp": 2.7388107897074048
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S3 To C3 To S3": {
+            "waypoints": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 17
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 17
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.312741719551674,
+                    "y": 3.016403262503484,
+                    "heading": 2.6023238516402656,
+                    "angularVelocity": 0.35533614833193405,
+                    "velocityX": 0.624033456373989,
+                    "velocityY": 0.2057867055686534,
+                    "timestamp": 0.07971002042214732
+                },
+                {
+                    "x": 4.412293799902437,
+                    "y": 3.049211318978086,
+                    "heading": 2.6573890862312104,
+                    "angularVelocity": 0.6908194766394113,
+                    "velocityX": 1.248928049742448,
+                    "velocityY": 0.41159262412491066,
+                    "timestamp": 0.15942004084429465
+                },
+                {
+                    "x": 4.5617613818375675,
+                    "y": 3.0984309719087952,
+                    "heading": 2.7365422108568223,
+                    "angularVelocity": 0.9930134781852262,
+                    "velocityX": 1.875141684113798,
+                    "velocityY": 0.6174838830806988,
+                    "timestamp": 0.23913006126644198
+                },
+                {
+                    "x": 4.761327239793144,
+                    "y": 3.1640795845539236,
+                    "heading": 2.8341908945522007,
+                    "angularVelocity": 1.225049036221883,
+                    "velocityX": 2.503648310446647,
+                    "velocityY": 0.8235929723446413,
+                    "timestamp": 0.3188400816885893
+                },
+                {
+                    "x": 5.011274516078503,
+                    "y": 3.2461089339964913,
+                    "heading": 2.9311433561003417,
+                    "angularVelocity": 1.2163145992771967,
+                    "velocityX": 3.1357070912995426,
+                    "velocityY": 1.0290970822506025,
+                    "timestamp": 0.3985501021107366
+                },
+                {
+                    "x": 5.297580410210689,
+                    "y": 3.3407358757733725,
+                    "heading": 2.9311433795557584,
+                    "angularVelocity": 2.942593217144604e-7,
+                    "velocityX": 3.5918431913064333,
+                    "velocityY": 1.1871398511220184,
+                    "timestamp": 0.4782601225328839
+                },
+                {
+                    "x": 5.583886278072779,
+                    "y": 3.4353628970344365,
+                    "heading": 2.931143403010656,
+                    "angularVelocity": 2.942528124810342e-7,
+                    "velocityX": 3.5918428617356097,
+                    "velocityY": 1.187140848288782,
+                    "timestamp": 0.5579701429550312
+                },
+                {
+                    "x": 5.8701921459347055,
+                    "y": 3.529989918295993,
+                    "heading": 2.9311434264655536,
+                    "angularVelocity": 2.942528130233853e-7,
+                    "velocityX": 3.591842861733567,
+                    "velocityY": 1.1871408482949637,
+                    "timestamp": 0.6376801633771786
+                },
+                {
+                    "x": 6.156498013796632,
+                    "y": 3.62461693955755,
+                    "heading": 2.931143449920451,
+                    "angularVelocity": 2.9425281250003654e-7,
+                    "velocityX": 3.5918428617335665,
+                    "velocityY": 1.1871408482949637,
+                    "timestamp": 0.717390183799326
+                },
+                {
+                    "x": 6.442803881658564,
+                    "y": 3.7192439608190906,
+                    "heading": 2.9311434733753488,
+                    "angularVelocity": 2.9425281349790425e-7,
+                    "velocityX": 3.5918428617336366,
+                    "velocityY": 1.1871408482947532,
+                    "timestamp": 0.7971002042214733
+                },
+                {
+                    "x": 6.729109750413145,
+                    "y": 3.813870979379799,
+                    "heading": 2.9311434968302543,
+                    "angularVelocity": 2.942529087425497e-7,
+                    "velocityX": 3.591842872932335,
+                    "velocityY": 1.1871408144115423,
+                    "timestamp": 0.8768102246436207
+                },
+                {
+                    "x": 7.011880136025942,
+                    "y": 3.9068513947536463,
+                    "heading": 2.940966575440456,
+                    "angularVelocity": 0.12323517869119305,
+                    "velocityX": 3.547488560600464,
+                    "velocityY": 1.1664833967099661,
+                    "timestamp": 0.956520245065768
+                },
+                {
+                    "x": 7.2458966997348915,
+                    "y": 3.984032577425885,
+                    "heading": 2.999027591028601,
+                    "angularVelocity": 0.7284029696724628,
+                    "velocityX": 2.935848748621424,
+                    "velocityY": 0.9682745314012468,
+                    "timestamp": 1.0362302654879154
+                },
+                {
+                    "x": 7.429606919081267,
+                    "y": 4.044603850649925,
+                    "heading": 3.057792091345064,
+                    "angularVelocity": 0.7372285191403096,
+                    "velocityX": 2.304731806282817,
+                    "velocityY": 0.7598953419313301,
+                    "timestamp": 1.1159402859100627
+                },
+                {
+                    "x": 7.563114805498745,
+                    "y": 4.088594555269305,
+                    "heading": 3.1044636591164667,
+                    "angularVelocity": 0.5855169466050564,
+                    "velocityX": 1.674919736695776,
+                    "velocityY": 0.55188424725527,
+                    "timestamp": 1.19565030633221
+                },
+                {
+                    "x": 7.646547133770953,
+                    "y": 4.116048088709135,
+                    "heading": 3.133033734418083,
+                    "angularVelocity": 0.35842514090835725,
+                    "velocityX": 1.046698116878499,
+                    "velocityY": 0.34441759385375514,
+                    "timestamp": 1.2753603267543572
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": 0.0873951047186249,
+                    "velocityX": 0.4196820682252798,
+                    "velocityY": 0.13739691989618263,
+                    "timestamp": 1.3550703471765044
+                },
+                {
+                    "x": 7.658055452006423,
+                    "y": 4.119659499210501,
+                    "heading": 3.1197811319835678,
+                    "angularVelocity": -0.235277452949756,
+                    "velocityX": -0.2553583788106411,
+                    "velocityY": -0.08541795355335377,
+                    "timestamp": 1.4410066226799219
+                },
+                {
+                    "x": 7.578036609646341,
+                    "y": 4.0931397899338755,
+                    "heading": 3.0739363035782867,
+                    "angularVelocity": -0.5334746954847726,
+                    "velocityX": -0.9311416150086622,
+                    "velocityY": -0.30859737778105817,
+                    "timestamp": 1.5269428981833393
+                },
+                {
+                    "x": 7.439853098092205,
+                    "y": 4.047395776011725,
+                    "heading": 3.0057619833378117,
+                    "angularVelocity": -0.7933124846417565,
+                    "velocityX": -1.6079765005482662,
+                    "velocityY": -0.5323015647836847,
+                    "timestamp": 1.6128791736867567
+                },
+                {
+                    "x": 7.243372469557272,
+                    "y": 3.982360857227892,
+                    "heading": 2.9212012505051064,
+                    "angularVelocity": -0.9839934572140336,
+                    "velocityX": -2.2863526186577556,
+                    "velocityY": -0.7567807471624278,
+                    "timestamp": 1.6988154491901741
+                },
+                {
+                    "x": 6.9884466912904,
+                    "y": 3.897942797791821,
+                    "heading": 2.8348834977989155,
+                    "angularVelocity": -1.0044390707013913,
+                    "velocityX": -2.9664513242342614,
+                    "velocityY": -0.9823332340335614,
+                    "timestamp": 1.7847517246935916
+                },
+                {
+                    "x": 6.679412229111129,
+                    "y": 3.7970350761603098,
+                    "heading": 2.8348833945193865,
+                    "angularVelocity": -0.000001201815274119231,
+                    "velocityX": -3.596088617629014,
+                    "velocityY": -1.1742156736534226,
+                    "timestamp": 1.870688000197009
+                },
+                {
+                    "x": 6.370694817722953,
+                    "y": 3.695161383669821,
+                    "heading": 2.8348833730448595,
+                    "angularVelocity": -2.4988896913294006e-7,
+                    "velocityX": -3.5923992467639496,
+                    "velocityY": -1.1854562219936775,
+                    "timestamp": 1.9566242757004264
+                },
+                {
+                    "x": 6.061977407921374,
+                    "y": 3.59328768637132,
+                    "heading": 2.834883351570332,
+                    "angularVelocity": -2.498889692815502e-7,
+                    "velocityX": -3.592399228301472,
+                    "velocityY": -1.185456277942254,
+                    "timestamp": 2.042560551203844
+                },
+                {
+                    "x": 5.753259998119802,
+                    "y": 3.4914139890727944,
+                    "heading": 2.834883330095805,
+                    "angularVelocity": -2.4988896886019487e-7,
+                    "velocityX": -3.59239922830138,
+                    "velocityY": -1.1854562779425326,
+                    "timestamp": 2.128496826707261
+                },
+                {
+                    "x": 5.444542588318229,
+                    "y": 3.3895402917742743,
+                    "heading": 2.8348833086212775,
+                    "angularVelocity": -2.4988896799949907e-7,
+                    "velocityX": -3.5923992283013995,
+                    "velocityY": -1.1854562779424735,
+                    "timestamp": 2.2144331022106782
+                },
+                {
+                    "x": 5.135825178181493,
+                    "y": 3.28766659549148,
+                    "heading": 2.8348832871467184,
+                    "angularVelocity": -2.498893376295858e-7,
+                    "velocityX": -3.5923992322015317,
+                    "velocityY": -1.1854562661229522,
+                    "timestamp": 2.3003693777140954
+                },
+                {
+                    "x": 4.844823310807952,
+                    "y": 3.1918227901947045,
+                    "heading": 2.7888021821480646,
+                    "angularVelocity": -0.5362241350199245,
+                    "velocityX": -3.386251797263078,
+                    "velocityY": -1.1152892621342902,
+                    "timestamp": 2.3863056532175126
+                },
+                {
+                    "x": 4.611844119098646,
+                    "y": 3.115032101312665,
+                    "heading": 2.714656158607103,
+                    "angularVelocity": -0.8628023859145838,
+                    "velocityX": -2.7110692236137233,
+                    "velocityY": -0.8935771120193101,
+                    "timestamp": 2.47224192872093
+                },
+                {
+                    "x": 4.43731744566658,
+                    "y": 3.0574895143251735,
+                    "heading": 2.6481597119281033,
+                    "angularVelocity": -0.7737878595443164,
+                    "velocityX": -2.030884773742906,
+                    "velocityY": -0.669596007627809,
+                    "timestamp": 2.558178204224347
+                },
+                {
+                    "x": 4.321077385755849,
+                    "y": 3.019155902534149,
+                    "heading": 2.5996556036782517,
+                    "angularVelocity": -0.5644194836896647,
+                    "velocityX": -1.3526308794486799,
+                    "velocityY": -0.44607020221047544,
+                    "timestamp": 2.6441144797277643
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": -0.2985421875449052,
+                    "velocityX": -0.675819209241155,
+                    "velocityY": -0.2229082238197189,
+                    "timestamp": 2.7300507552311815
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": 0,
+                    "velocityX": -1.2326632641143652e-35,
+                    "velocityY": 0,
+                    "timestamp": 2.8159870307345987
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S3 To C3 To S4": {
+            "waypoints": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 17
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 18
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": -2.99079667903027e-32,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.307911912995487,
+                    "y": 3.0252386980683,
+                    "heading": 2.6037905893528444,
+                    "angularVelocity": 0.3765521075910298,
+                    "velocityX": 0.5676851603736718,
+                    "velocityY": 0.31901634566234616,
+                    "timestamp": 0.07911412192970564
+                },
+                {
+                    "x": 4.397813948925716,
+                    "y": 3.0757408190390847,
+                    "heading": 2.6615026890848092,
+                    "angularVelocity": 0.7294791160450911,
+                    "velocityX": 1.1363588919069176,
+                    "velocityY": 0.6383452124471162,
+                    "timestamp": 0.15822824385941128
+                },
+                {
+                    "x": 4.532822684228442,
+                    "y": 3.151547007950623,
+                    "heading": 2.744086644451416,
+                    "angularVelocity": 1.0438585849437199,
+                    "velocityX": 1.7065061459278321,
+                    "velocityY": 0.9581878312306107,
+                    "timestamp": 0.23734236578911694
+                },
+                {
+                    "x": 4.713127943842242,
+                    "y": 3.2527364142046724,
+                    "heading": 2.845383620284061,
+                    "angularVelocity": 1.2803905720226456,
+                    "velocityX": 2.2790527811710266,
+                    "velocityY": 1.2790308959500052,
+                    "timestamp": 0.31645648771882257
+                },
+                {
+                    "x": 4.938980637634121,
+                    "y": 3.3793948659075563,
+                    "heading": 2.944365706531666,
+                    "angularVelocity": 1.251130440852921,
+                    "velocityX": 2.854770909200696,
+                    "velocityY": 1.6009588252198856,
+                    "timestamp": 0.3955706096485282
+                },
+                {
+                    "x": 5.199953974250007,
+                    "y": 3.5259002853585115,
+                    "heading": 2.944365731476778,
+                    "angularVelocity": 3.1530542976951733e-7,
+                    "velocityX": 3.2986947241576456,
+                    "velocityY": 1.8518238700939902,
+                    "timestamp": 0.4746847315782338
+                },
+                {
+                    "x": 5.460927260915445,
+                    "y": 3.6724057937882604,
+                    "heading": 2.944365756420666,
+                    "angularVelocity": 3.152899521547411e-7,
+                    "velocityX": 3.2986940927855755,
+                    "velocityY": 1.8518249947831291,
+                    "timestamp": 0.5537988535079394
+                },
+                {
+                    "x": 5.721900547580569,
+                    "y": 3.818911302218567,
+                    "heading": 2.9443657813645534,
+                    "angularVelocity": 3.1528995198652365e-7,
+                    "velocityX": 3.2986940927816133,
+                    "velocityY": 1.851824994790188,
+                    "timestamp": 0.6329129754376451
+                },
+                {
+                    "x": 5.982873834247296,
+                    "y": 3.965416810646019,
+                    "heading": 2.9443658063084412,
+                    "angularVelocity": 3.1528995220649985e-7,
+                    "velocityX": 3.2986940928018726,
+                    "velocityY": 1.851824994754099,
+                    "timestamp": 0.7120270973673508
+                },
+                {
+                    "x": 6.243847376304821,
+                    "y": 4.1119218641396325,
+                    "heading": 2.944365831252335,
+                    "angularVelocity": 3.152900317708399e-7,
+                    "velocityX": 3.298697320933481,
+                    "velocityY": 1.8518192444047645,
+                    "timestamp": 0.7911412192970565
+                },
+                {
+                    "x": 6.520761156875476,
+                    "y": 4.225454581965132,
+                    "heading": 2.9443658572464595,
+                    "angularVelocity": 3.2856491270029264e-7,
+                    "velocityX": 3.5001814317891196,
+                    "velocityY": 1.4350499639795518,
+                    "timestamp": 0.8702553412267622
+                },
+                {
+                    "x": 6.81318632649156,
+                    "y": 4.28915727346349,
+                    "heading": 2.9443676715667886,
+                    "angularVelocity": 0.000022932951594388774,
+                    "velocityX": 3.696244898931049,
+                    "velocityY": 0.8052000065798415,
+                    "timestamp": 0.9493694631564679
+                },
+                {
+                    "x": 7.072106261164272,
+                    "y": 4.3159902610395875,
+                    "heading": 2.999666770720341,
+                    "angularVelocity": 0.6989788650209022,
+                    "velocityX": 3.2727397885142184,
+                    "velocityY": 0.3391681146374708,
+                    "timestamp": 1.0284835850861735
+                },
+                {
+                    "x": 7.2892635874980165,
+                    "y": 4.311538961400137,
+                    "heading": 3.058239945124217,
+                    "angularVelocity": 0.7403630726751829,
+                    "velocityX": 2.7448617394337207,
+                    "velocityY": -0.05626428671490304,
+                    "timestamp": 1.1075977070158791
+                },
+                {
+                    "x": 7.463265555506274,
+                    "y": 4.277921305765472,
+                    "heading": 3.1051985992516418,
+                    "angularVelocity": 0.593555903573676,
+                    "velocityX": 2.1993793745554173,
+                    "velocityY": -0.4249261044006207,
+                    "timestamp": 1.1867118289455847
+                },
+                {
+                    "x": 7.593597601326297,
+                    "y": 4.21619741378488,
+                    "heading": 3.1337465954664303,
+                    "angularVelocity": 0.3608457696105646,
+                    "velocityX": 1.6473929387198967,
+                    "velocityY": -0.7801880432350068,
+                    "timestamp": 1.2658259508752903
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": 0.07904283560305705,
+                    "velocityX": 1.0921235876253772,
+                    "velocityY": -1.1274524902663325,
+                    "timestamp": 1.3449400728049958
+                },
+                {
+                    "x": 7.722328891064707,
+                    "y": 4.011878570496831,
+                    "heading": 3.1218227549328446,
+                    "angularVelocity": -0.23159012596074638,
+                    "velocityX": 0.539298071695534,
+                    "velocityY": -1.4667231618939216,
+                    "timestamp": 1.423428933207607
+                },
+                {
+                    "x": 7.720975293339473,
+                    "y": 3.870431716581803,
+                    "heading": 3.0818439654671956,
+                    "angularVelocity": -0.5093562227885172,
+                    "velocityX": -0.01724573039142055,
+                    "velocityY": -1.8021264825284895,
+                    "timestamp": 1.5019177936102184
+                },
+                {
+                    "x": 7.675510236414314,
+                    "y": 3.703145056798568,
+                    "heading": 3.02401855987363,
+                    "angularVelocity": -0.7367339173603616,
+                    "velocityX": -0.5792548992550559,
+                    "velocityY": -2.131342701692084,
+                    "timestamp": 1.5804066540128296
+                },
+                {
+                    "x": 7.585250814368908,
+                    "y": 3.51090702633694,
+                    "heading": 2.955217915344013,
+                    "angularVelocity": -0.8765657212590741,
+                    "velocityX": -1.149964741269228,
+                    "velocityY": -2.4492396688591995,
+                    "timestamp": 1.658895514415441
+                },
+                {
+                    "x": 7.449012873441734,
+                    "y": 3.2957924623075137,
+                    "heading": 2.8903514658300926,
+                    "angularVelocity": -0.8264414743848527,
+                    "velocityX": -1.7357614854940644,
+                    "velocityY": -2.7407018387830946,
+                    "timestamp": 1.7373843748180522
+                },
+                {
+                    "x": 7.265754194056636,
+                    "y": 3.066909845502957,
+                    "heading": 2.8806156888219427,
+                    "angularVelocity": -0.12404023906336885,
+                    "velocityX": -2.334836796522032,
+                    "velocityY": -2.916115938370047,
+                    "timestamp": 1.8158732352206635
+                },
+                {
+                    "x": 7.0429291564371885,
+                    "y": 2.8706709413875,
+                    "heading": 2.8806156606364275,
+                    "angularVelocity": -3.5910210875012317e-7,
+                    "velocityX": -2.838938372610575,
+                    "velocityY": -2.5002134456895257,
+                    "timestamp": 1.8943620956232747
+                },
+                {
+                    "x": 6.796142716598087,
+                    "y": 2.7055735191439014,
+                    "heading": 2.8806156351575583,
+                    "angularVelocity": -3.246176492669905e-7,
+                    "velocityX": -3.1442224867732946,
+                    "velocityY": -2.103450367309771,
+                    "timestamp": 1.972850956025886
+                },
+                {
+                    "x": 6.549355877169833,
+                    "y": 2.5404766942048504,
+                    "heading": 2.880615609678698,
+                    "angularVelocity": -3.246175317472835e-7,
+                    "velocityX": -3.1442275778034396,
+                    "velocityY": -2.10344275725474,
+                    "timestamp": 2.0513398164284973
+                },
+                {
+                    "x": 6.302569037739046,
+                    "y": 2.375379869269585,
+                    "heading": 2.8806155841998384,
+                    "angularVelocity": -3.24617530454395e-7,
+                    "velocityX": -3.144227577835705,
+                    "velocityY": -2.103442757206509,
+                    "timestamp": 2.1298286768311088
+                },
+                {
+                    "x": 6.055782203816465,
+                    "y": 2.2102830361006496,
+                    "heading": 2.8806155587209785,
+                    "angularVelocity": -3.246175312544412e-7,
+                    "velocityX": -3.1442275076575137,
+                    "velocityY": -2.103442862108911,
+                    "timestamp": 2.2083175372337203
+                },
+                {
+                    "x": 5.809854810532114,
+                    "y": 2.043908693921413,
+                    "heading": 2.8806155277577266,
+                    "angularVelocity": -3.944923106855428e-7,
+                    "velocityX": -3.1332776654274115,
+                    "velocityY": -2.1197191719412425,
+                    "timestamp": 2.2868063976363318
+                },
+                {
+                    "x": 5.598037560183058,
+                    "y": 1.9023806628591073,
+                    "heading": 2.776131644051983,
+                    "angularVelocity": -1.3311937919570889,
+                    "velocityX": -2.6986918814024308,
+                    "velocityY": -1.8031607330815247,
+                    "timestamp": 2.3652952580389432
+                },
+                {
+                    "x": 5.428977486755044,
+                    "y": 1.7893186281455802,
+                    "heading": 2.67268813100655,
+                    "angularVelocity": -1.3179387815648507,
+                    "velocityX": -2.1539371645965275,
+                    "velocityY": -1.4404851100343643,
+                    "timestamp": 2.4437841184415547
+                },
+                {
+                    "x": 5.302396782822421,
+                    "y": 1.7046186401543708,
+                    "heading": 2.5887102200267456,
+                    "angularVelocity": -1.0699341352267786,
+                    "velocityX": -1.6127218981562843,
+                    "velocityY": -1.0791338739884617,
+                    "timestamp": 2.5222729788441662
+                },
+                {
+                    "x": 5.218108357184172,
+                    "y": 1.6481959100495345,
+                    "heading": 2.5301489625141826,
+                    "angularVelocity": -0.7461091575565081,
+                    "velocityX": -1.0738902973732658,
+                    "velocityY": -0.7188629037982504,
+                    "timestamp": 2.6007618392467777
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": -0.3841177252355578,
+                    "velocityX": -0.5364883241797085,
+                    "velocityY": -0.35923454493927354,
+                    "timestamp": 2.6792506996493892
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": 0,
+                    "velocityX": -9.31070457943821e-33,
+                    "velocityY": 1.510534612718486e-32,
+                    "timestamp": 2.7577395600520007
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S3 To C4 To S3": {
+            "waypoints": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 17
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 17
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": -2.4689012164756437e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.313159042990802,
+                    "y": 2.9920825634147805,
+                    "heading": 2.597885779471472,
+                    "angularVelocity": 0.30476728851208423,
+                    "velocityX": 0.6399973484188943,
+                    "velocityY": -0.10102143339823635,
+                    "timestamp": 0.07837382938338545
+                },
+                {
+                    "x": 4.4134829053058295,
+                    "y": 2.976248991975609,
+                    "heading": 2.6455288467924127,
+                    "angularVelocity": 0.6078951060041105,
+                    "velocityX": 1.2800683991625412,
+                    "velocityY": -0.20202625753700879,
+                    "timestamp": 0.1567476587667709
+                },
+                {
+                    "x": 4.563981380382106,
+                    "y": 2.952502286743102,
+                    "heading": 2.7167039498074406,
+                    "angularVelocity": 0.9081488498779698,
+                    "velocityX": 1.9202644079068125,
+                    "velocityY": -0.3029927900593087,
+                    "timestamp": 0.23512148815015638
+                },
+                {
+                    "x": 4.764676150331059,
+                    "y": 2.9208479584714695,
+                    "heading": 2.8108575648883316,
+                    "angularVelocity": 1.2013399858304654,
+                    "velocityX": 2.560737066543005,
+                    "velocityY": -0.40388900887800433,
+                    "timestamp": 0.3134953175335418
+                },
+                {
+                    "x": 5.015633164055166,
+                    "y": 2.881293970992892,
+                    "heading": 2.92608193331875,
+                    "angularVelocity": 1.47018933918326,
+                    "velocityX": 3.2020511910485845,
+                    "velocityY": -0.5046836142851278,
+                    "timestamp": 0.39186914691692726
+                },
+                {
+                    "x": 5.308481077006102,
+                    "y": 2.835005929387577,
+                    "heading": 2.926081954236969,
+                    "angularVelocity": 2.6690310524294676e-7,
+                    "velocityX": 3.7365523064899215,
+                    "velocityY": -0.5906058434235878,
+                    "timestamp": 0.4702429763003127
+                },
+                {
+                    "x": 5.601350966367862,
+                    "y": 2.7888571291196405,
+                    "heading": 2.92608197144068,
+                    "angularVelocity": 2.1950836119943446e-7,
+                    "velocityX": 3.7368327114541455,
+                    "velocityY": -0.5888292129019572,
+                    "timestamp": 0.5486168056836982
+                },
+                {
+                    "x": 5.8942208558716445,
+                    "y": 2.7427083297530035,
+                    "heading": 2.9260819886443907,
+                    "angularVelocity": 2.1950836114981294e-7,
+                    "velocityX": 3.736832713266251,
+                    "velocityY": -0.5888292014019535,
+                    "timestamp": 0.6269906350670836
+                },
+                {
+                    "x": 6.187090745375428,
+                    "y": 2.6965595303863723,
+                    "heading": 2.9260820058481016,
+                    "angularVelocity": 2.195083613443253e-7,
+                    "velocityX": 3.736832713266263,
+                    "velocityY": -0.5888292014018791,
+                    "timestamp": 0.7053644644504691
+                },
+                {
+                    "x": 6.479960634879199,
+                    "y": 2.6504107310196727,
+                    "heading": 2.9260820230518125,
+                    "angularVelocity": 2.1950836077924463e-7,
+                    "velocityX": 3.736832713266126,
+                    "velocityY": -0.5888292014027512,
+                    "timestamp": 0.7837382938338545
+                },
+                {
+                    "x": 6.772830522718861,
+                    "y": 2.604261921092308,
+                    "heading": 2.926082040255567,
+                    "angularVelocity": 2.1950891675310117e-7,
+                    "velocityX": 3.736832692033143,
+                    "velocityY": -0.5888293361500981,
+                    "timestamp": 0.86211212321724
+                },
+                {
+                    "x": 7.050151670300153,
+                    "y": 2.559252271073386,
+                    "heading": 2.978396963955354,
+                    "angularVelocity": 0.6675050091514937,
+                    "velocityX": 3.5384406984211854,
+                    "velocityY": -0.5742943833809976,
+                    "timestamp": 0.9404859526006254
+                },
+                {
+                    "x": 7.276980321576378,
+                    "y": 2.5226261995289687,
+                    "heading": 3.0372586376427035,
+                    "angularVelocity": 0.7510373571182644,
+                    "velocityX": 2.894188698712594,
+                    "velocityY": -0.4673252772333,
+                    "timestamp": 1.0188597819840108
+                },
+                {
+                    "x": 7.453315564542908,
+                    "y": 2.49415241035015,
+                    "heading": 3.0866031396716647,
+                    "angularVelocity": 0.6296043260507225,
+                    "velocityX": 2.2499250624075064,
+                    "velocityY": -0.3633073616900632,
+                    "timestamp": 1.0972336113673964
+                },
+                {
+                    "x": 7.579233655925443,
+                    "y": 2.473748793081471,
+                    "heading": 3.1215813110163633,
+                    "angularVelocity": 0.446299123315424,
+                    "velocityX": 1.6066344132117514,
+                    "velocityY": -0.2603371231087117,
+                    "timestamp": 1.175607440750782
+                },
+                {
+                    "x": 7.654784082070509,
+                    "y": 2.4613731780152506,
+                    "heading": 3.1398442445430734,
+                    "angularVelocity": 0.2330233659676447,
+                    "velocityX": 0.9639751781872319,
+                    "velocityY": -0.1579049430605922,
+                    "timestamp": 1.2539812701341675
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": 0.0019873401380948097,
+                    "velocityX": 0.32173900558233287,
+                    "velocityY": -0.05579895801519462,
+                    "timestamp": 1.332355099517553
+                },
+                {
+                    "x": 7.650333485517468,
+                    "y": 2.4613165476638152,
+                    "heading": 3.1188382258181035,
+                    "angularVelocity": -0.255122622507773,
+                    "velocityX": -0.3576542737105397,
+                    "velocityY": 0.05203953840108144,
+                    "timestamp": 1.4153025620901392
+                },
+                {
+                    "x": 7.564253765684918,
+                    "y": 2.4745901696035957,
+                    "heading": 3.07855569322522,
+                    "angularVelocity": -0.4856391183470325,
+                    "velocityX": -1.0377619418703934,
+                    "velocityY": 0.16002444834484145,
+                    "timestamp": 1.4982500246627253
+                },
+                {
+                    "x": 7.42168176124644,
+                    "y": 2.4968358287790196,
+                    "heading": 3.0225573774767818,
+                    "angularVelocity": -0.6751058321938196,
+                    "velocityX": -1.7188229756119993,
+                    "velocityY": 0.2681897490952097,
+                    "timestamp": 1.5811974872353114
+                },
+                {
+                    "x": 7.222516189064582,
+                    "y": 2.528071510677467,
+                    "heading": 2.9567671784015253,
+                    "angularVelocity": -0.7931550530274372,
+                    "velocityX": -2.401105061020643,
+                    "velocityY": 0.37657187971380685,
+                    "timestamp": 1.6641449498078975
+                },
+                {
+                    "x": 6.966699685219415,
+                    "y": 2.5683095941313905,
+                    "heading": 2.8941605042172336,
+                    "angularVelocity": -0.7547750376269705,
+                    "velocityX": -3.0840787157449774,
+                    "velocityY": 0.48510324735627736,
+                    "timestamp": 1.7470924123804836
+                },
+                {
+                    "x": 6.656762628445554,
+                    "y": 2.6172997543511465,
+                    "heading": 2.894159452148734,
+                    "angularVelocity": -0.000012683552539851008,
+                    "velocityX": -3.736546570097186,
+                    "velocityY": 0.5906167434222167,
+                    "timestamp": 1.8300398749530697
+                },
+                {
+                    "x": 6.3468583527209415,
+                    "y": 2.666498972584932,
+                    "heading": 2.8941594322307855,
+                    "angularVelocity": -2.401272821235818e-7,
+                    "velocityX": -3.736151367540867,
+                    "velocityY": 0.5931371100198551,
+                    "timestamp": 1.9129873375256559
+                },
+                {
+                    "x": 6.036954077181096,
+                    "y": 2.715698191982551,
+                    "heading": 2.894159412312837,
+                    "angularVelocity": -2.4012728242851975e-7,
+                    "velocityX": -3.736151365313363,
+                    "velocityY": 0.5931371240508304,
+                    "timestamp": 1.995934800098242
+                },
+                {
+                    "x": 5.72704980164125,
+                    "y": 2.7648974113801765,
+                    "heading": 2.8941593923948883,
+                    "angularVelocity": -2.401272814815578e-7,
+                    "velocityX": -3.7361513653133502,
+                    "velocityY": 0.5931371240509082,
+                    "timestamp": 2.078882262670828
+                },
+                {
+                    "x": 5.417145526101417,
+                    "y": 2.8140966307778834,
+                    "heading": 2.8941593724769397,
+                    "angularVelocity": -2.4012728169619825e-7,
+                    "velocityX": -3.7361513653131957,
+                    "velocityY": 0.5931371240518842,
+                    "timestamp": 2.1618297252434138
+                },
+                {
+                    "x": 5.10724125286934,
+                    "y": 2.8632958647117834,
+                    "heading": 2.894159352558893,
+                    "angularVelocity": -2.401284654616401e-7,
+                    "velocityX": -3.7361513374913162,
+                    "velocityY": 0.59313729929767,
+                    "timestamp": 2.2447771878159997
+                },
+                {
+                    "x": 4.825498631830447,
+                    "y": 2.909240953390356,
+                    "heading": 2.811052225227297,
+                    "angularVelocity": -1.0019248902143632,
+                    "velocityX": -3.3966394185035327,
+                    "velocityY": 0.5539058972221842,
+                    "timestamp": 2.3277246503885856
+                },
+                {
+                    "x": 4.60031373082884,
+                    "y": 2.9456643222027448,
+                    "heading": 2.723257007302943,
+                    "angularVelocity": -1.0584436847304166,
+                    "velocityX": -2.7147895067260346,
+                    "velocityY": 0.43911371948857386,
+                    "timestamp": 2.4106721129611715
+                },
+                {
+                    "x": 4.431586205140753,
+                    "y": 2.9728717926897983,
+                    "heading": 2.6509541339197864,
+                    "angularVelocity": -0.871670707466155,
+                    "velocityX": -2.0341493332654417,
+                    "velocityY": 0.3280084723899037,
+                    "timestamp": 2.4936195755337573
+                },
+                {
+                    "x": 4.3191766366235065,
+                    "y": 2.9909670138264888,
+                    "heading": 2.600230923186435,
+                    "angularVelocity": -0.6115100951879456,
+                    "velocityX": -1.3551899603785846,
+                    "velocityY": 0.21815279907876073,
+                    "timestamp": 2.5765670381063432
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": -0.31623539012403434,
+                    "velocityX": -0.6772556372577037,
+                    "velocityY": 0.10890009041091805,
+                    "timestamp": 2.659514500678929
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": 0,
+                    "velocityX": -4.6317137057442136e-32,
+                    "velocityY": 0,
+                    "timestamp": 2.742461963251515
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S3 To C4 To S4": {
+            "waypoints": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 17
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.312461905031287,
+                    "y": 3.0009727246573483,
+                    "heading": 2.5993071037453466,
+                    "angularVelocity": 0.3268807464808282,
+                    "velocityX": 0.6388777080808309,
+                    "velocityY": 0.012564257266019683,
+                    "timestamp": 0.07741998884241831
+                },
+                {
+                    "x": 4.411454270150545,
+                    "y": 3.0029274539385553,
+                    "heading": 2.648169844004936,
+                    "angularVelocity": 0.6311385598239377,
+                    "velocityX": 1.2786409117256237,
+                    "velocityY": 0.025248379784522566,
+                    "timestamp": 0.15483997768483662
+                },
+                {
+                    "x": 4.560077070415931,
+                    "y": 3.0058795423873246,
+                    "heading": 2.717741930312934,
+                    "angularVelocity": 0.8986320890539764,
+                    "velocityX": 1.9196954492966272,
+                    "velocityY": 0.03813083020172101,
+                    "timestamp": 0.23225996652725495
+                },
+                {
+                    "x": 4.758482787325499,
+                    "y": 3.0098532742147075,
+                    "heading": 2.8026323249857676,
+                    "angularVelocity": 1.0964919517828993,
+                    "velocityX": 2.562719523421868,
+                    "velocityY": 0.05132694910962975,
+                    "timestamp": 0.30967995536967324
+                },
+                {
+                    "x": 5.006851286997014,
+                    "y": 3.0148884552327933,
+                    "heading": 2.889629605764704,
+                    "angularVelocity": 1.1237056744610465,
+                    "velocityX": 3.2080668492092808,
+                    "velocityY": 0.06503722221317501,
+                    "timestamp": 0.38709994421209154
+                },
+                {
+                    "x": 5.299689191212239,
+                    "y": 3.01955932024386,
+                    "heading": 2.889629778554125,
+                    "angularVelocity": 0.000002231845078236991,
+                    "velocityX": 3.7824586207480784,
+                    "velocityY": 0.060331512325246216,
+                    "timestamp": 0.46451993305450984
+                },
+                {
+                    "x": 5.592507772821644,
+                    "y": 3.025318229635612,
+                    "heading": 2.8896298031470753,
+                    "angularVelocity": 3.176563362313136e-7,
+                    "velocityX": 3.782209039133448,
+                    "velocityY": 0.07438530381958017,
+                    "timestamp": 0.5419399218969282
+                },
+                {
+                    "x": 5.8853263542850165,
+                    "y": 3.0310771464526267,
+                    "heading": 2.889629827740025,
+                    "angularVelocity": 3.176563361839336e-7,
+                    "velocityX": 3.782209037247192,
+                    "velocityY": 0.07438539972844682,
+                    "timestamp": 0.6193599107393465
+                },
+                {
+                    "x": 6.178144935748446,
+                    "y": 3.0368360632666453,
+                    "heading": 2.8896298523329755,
+                    "angularVelocity": 3.1765633664876355e-7,
+                    "velocityX": 3.782209037247953,
+                    "velocityY": 0.07438539968973952,
+                    "timestamp": 0.6967798995817648
+                },
+                {
+                    "x": 6.4709635259990606,
+                    "y": 3.0425945332689603,
+                    "heading": 2.889629876925932,
+                    "angularVelocity": 3.176564212988765e-7,
+                    "velocityX": 3.7822091507481463,
+                    "velocityY": 0.07437962841916478,
+                    "timestamp": 0.7741998884241831
+                },
+                {
+                    "x": 6.762585697804425,
+                    "y": 3.0155317439126783,
+                    "heading": 2.889629906398516,
+                    "angularVelocity": 3.80684423008717e-7,
+                    "velocityX": 3.766755539049949,
+                    "velocityY": -0.3495581665784765,
+                    "timestamp": 0.8516198772666014
+                },
+                {
+                    "x": 7.037113234733603,
+                    "y": 2.9421863256805914,
+                    "heading": 2.9183632792459075,
+                    "angularVelocity": 0.37113635996351935,
+                    "velocityX": 3.5459516467763192,
+                    "velocityY": -0.9473705606103254,
+                    "timestamp": 0.9290398661090197
+                },
+                {
+                    "x": 7.2647684874925345,
+                    "y": 2.8553520162741264,
+                    "heading": 2.9862391901434,
+                    "angularVelocity": 0.8767233360837648,
+                    "velocityX": 2.9405229342296155,
+                    "velocityY": -1.1216006448051647,
+                    "timestamp": 1.006459854951438
+                },
+                {
+                    "x": 7.442903065756583,
+                    "y": 2.761816317140275,
+                    "heading": 3.0514695730897063,
+                    "angularVelocity": 0.8425522132155971,
+                    "velocityX": 2.3008861267937584,
+                    "velocityY": -1.208159553267764,
+                    "timestamp": 1.0838798437938564
+                },
+                {
+                    "x": 7.571438256526523,
+                    "y": 2.6636606141768233,
+                    "heading": 3.1022478511869496,
+                    "angularVelocity": 0.6558807209414358,
+                    "velocityX": 1.660232618110589,
+                    "velocityY": -1.2678341140456517,
+                    "timestamp": 1.1612998326362747
+                },
+                {
+                    "x": 7.650445589096316,
+                    "y": 2.561865725059965,
+                    "heading": 3.1328810502986992,
+                    "angularVelocity": 0.39567558158786426,
+                    "velocityX": 1.0205030219082787,
+                    "velocityY": -1.3148398835868218,
+                    "timestamp": 1.238719821478693
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": 0.09195234729096653,
+                    "velocityX": 0.38174134801077375,
+                    "velocityY": -1.354504523029707,
+                    "timestamp": 1.3161398103211113
+                },
+                {
+                    "x": 7.659642368469709,
+                    "y": 2.34860980319894,
+                    "heading": 3.1210552256706268,
+                    "angularVelocity": -0.24288242827145334,
+                    "velocityX": -0.26099603478867267,
+                    "velocityY": -1.3896219475702174,
+                    "timestamp": 1.3941395835620152
+                },
+                {
+                    "x": 7.589031675721614,
+                    "y": 2.237822483491401,
+                    "heading": 3.078729120037355,
+                    "angularVelocity": -0.5426439574708317,
+                    "velocityX": -0.9052679234080746,
+                    "velocityY": -1.4203543818694149,
+                    "timestamp": 1.472139356802919
+                },
+                {
+                    "x": 7.468012328059872,
+                    "y": 2.1251640911848773,
+                    "heading": 3.017134343801584,
+                    "angularVelocity": -0.7896789141365163,
+                    "velocityX": -1.5515346087990811,
+                    "velocityY": -1.4443425618504895,
+                    "timestamp": 1.550139130043823
+                },
+                {
+                    "x": 7.29639145154735,
+                    "y": 2.0115501518980783,
+                    "heading": 2.9431614871389193,
+                    "angularVelocity": -0.948372714292366,
+                    "velocityX": -2.2002740441625033,
+                    "velocityY": -1.4565931997763661,
+                    "timestamp": 1.6281389032847269
+                },
+                {
+                    "x": 7.0740549155451475,
+                    "y": 1.8989556561262921,
+                    "heading": 2.8711554567434443,
+                    "angularVelocity": -0.9231569196115829,
+                    "velocityX": -2.8504766970989883,
+                    "velocityY": -1.44352337312617,
+                    "timestamp": 1.7061386765256308
+                },
+                {
+                    "x": 6.804126889371604,
+                    "y": 1.794205881873725,
+                    "heading": 2.8575112203208874,
+                    "angularVelocity": -0.17492661652254402,
+                    "velocityX": -3.460625780793808,
+                    "velocityY": -1.3429497279312002,
+                    "timestamp": 1.7841384497665347
+                },
+                {
+                    "x": 6.514819933259115,
+                    "y": 1.7361808711845228,
+                    "heading": 2.8575111902020094,
+                    "angularVelocity": -3.8614058154759053e-7,
+                    "velocityX": -3.7090743228055154,
+                    "velocityY": -0.7439125561300094,
+                    "timestamp": 1.8621382230074386
+                },
+                {
+                    "x": 6.220884578970619,
+                    "y": 1.7103463006817032,
+                    "heading": 2.8575111626758107,
+                    "angularVelocity": -3.52901004822605e-7,
+                    "velocityX": -3.768412933466744,
+                    "velocityY": -0.3312134052368198,
+                    "timestamp": 1.9401379962483425
+                },
+                {
+                    "x": 5.926733464556045,
+                    "y": 1.687097421621547,
+                    "heading": 2.857511000804422,
+                    "angularVelocity": -0.000002075280251147508,
+                    "velocityX": -3.771179096970985,
+                    "velocityY": -0.2980634185736858,
+                    "timestamp": 2.0181377694892464
+                },
+                {
+                    "x": 5.675971267912864,
+                    "y": 1.6645564964621176,
+                    "heading": 2.7589225755051734,
+                    "angularVelocity": -1.2639578450408617,
+                    "velocityX": -3.214909303244982,
+                    "velocityY": -0.28898706012659137,
+                    "timestamp": 2.0961375427301503
+                },
+                {
+                    "x": 5.475720463425886,
+                    "y": 1.6466626572874457,
+                    "heading": 2.662658583499747,
+                    "angularVelocity": -1.2341573315618837,
+                    "velocityX": -2.5673254698895036,
+                    "velocityY": -0.22940886147715536,
+                    "timestamp": 2.174137315971054
+                },
+                {
+                    "x": 5.325756154484835,
+                    "y": 1.6333048659006117,
+                    "heading": 2.583901126299631,
+                    "angularVelocity": -1.009713925152988,
+                    "velocityX": -1.9226249347915934,
+                    "velocityY": -0.17125423359344924,
+                    "timestamp": 2.252137089211958
+                },
+                {
+                    "x": 5.225890415283439,
+                    "y": 1.6244276229064765,
+                    "heading": 2.5286254494455984,
+                    "angularVelocity": -0.7086645839765799,
+                    "velocityX": -1.2803337119065468,
+                    "velocityY": -0.11381113848520645,
+                    "timestamp": 2.330136862452862
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": -0.3669940085234364,
+                    "velocityX": -0.6396225682522365,
+                    "velocityY": -0.05676456126098677,
+                    "timestamp": 2.408136635693766
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": -3.495963808080905e-32,
+                    "velocityX": 2.431919697102105e-35,
+                    "velocityY": 0,
+                    "timestamp": 2.48613640893467
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S3 To C5 To S3": {
+            "waypoints": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 19
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 19
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": -4.85982063591045e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.306024668622343,
+                    "y": 2.9721182741266654,
+                    "heading": 2.598597610192154,
+                    "angularVelocity": 0.31231028150986523,
+                    "velocityX": 0.5462744658665957,
+                    "velocityY": -0.3540079539621319,
+                    "timestamp": 0.0787601678472939
+                },
+                {
+                    "x": 4.392095194160992,
+                    "y": 2.9163526825964556,
+                    "heading": 2.647262656420547,
+                    "angularVelocity": 0.6178890619271524,
+                    "velocityX": 1.0928179546992538,
+                    "velocityY": -0.7080430762709009,
+                    "timestamp": 0.1575203356945878
+                },
+                {
+                    "x": 4.521245672207201,
+                    "y": 2.83269821600786,
+                    "heading": 2.7190885011917048,
+                    "angularVelocity": 0.911956471581102,
+                    "velocityX": 1.6397943475262466,
+                    "velocityY": -1.0621418018152553,
+                    "timestamp": 0.23628050354188168
+                },
+                {
+                    "x": 4.693538421890133,
+                    "y": 2.721142508781873,
+                    "heading": 2.812276483563172,
+                    "angularVelocity": 1.1831866909189597,
+                    "velocityX": 2.1875619922114358,
+                    "velocityY": -1.4163975303135559,
+                    "timestamp": 0.3150406713891756
+                },
+                {
+                    "x": 4.909132383686271,
+                    "y": 2.581660366752094,
+                    "heading": 2.9215937215973726,
+                    "angularVelocity": 1.387976194339083,
+                    "velocityX": 2.7373476681023576,
+                    "velocityY": -1.7709731434323392,
+                    "timestamp": 0.39380083923646947
+                },
+                {
+                    "x": 5.159074984583097,
+                    "y": 2.4194878202605934,
+                    "heading": 2.9215937398089027,
+                    "angularVelocity": 2.3122767087752481e-7,
+                    "velocityX": 3.1734645535727406,
+                    "velocityY": -2.059068066055073,
+                    "timestamp": 0.47256100708376336
+                },
+                {
+                    "x": 5.409017280295355,
+                    "y": 2.257314803413765,
+                    "heading": 2.921593758018432,
+                    "angularVelocity": 2.3120226821213244e-7,
+                    "velocityX": 3.173460678713437,
+                    "velocityY": -2.059074038050069,
+                    "timestamp": 0.5513211749310573
+                },
+                {
+                    "x": 5.658959576005671,
+                    "y": 2.0951417865639432,
+                    "heading": 2.9215937762279616,
+                    "angularVelocity": 2.312022681051112e-7,
+                    "velocityX": 3.1734606786887793,
+                    "velocityY": -2.0590740380880725,
+                    "timestamp": 0.6300813427783512
+                },
+                {
+                    "x": 5.908901871715987,
+                    "y": 1.9329687697141213,
+                    "heading": 2.9215937944374906,
+                    "angularVelocity": 2.3120226848439114e-7,
+                    "velocityX": 3.173460678688779,
+                    "velocityY": -2.0590740380880725,
+                    "timestamp": 0.708841510625645
+                },
+                {
+                    "x": 6.158844167426302,
+                    "y": 1.7707957528642995,
+                    "heading": 2.92159381264702,
+                    "angularVelocity": 2.3120226852070691e-7,
+                    "velocityX": 3.1734606786887785,
+                    "velocityY": -2.059074038088073,
+                    "timestamp": 0.7876016784729389
+                },
+                {
+                    "x": 6.408786463136619,
+                    "y": 1.6086227360144778,
+                    "heading": 2.921593830856549,
+                    "angularVelocity": 2.3120226809334475e-7,
+                    "velocityX": 3.173460678688779,
+                    "velocityY": -2.059074038088073,
+                    "timestamp": 0.8663618463202328
+                },
+                {
+                    "x": 6.658728758846954,
+                    "y": 1.4464497191646866,
+                    "heading": 2.9215938490660784,
+                    "angularVelocity": 2.31202268371391e-7,
+                    "velocityX": 3.173460678689032,
+                    "velocityY": -2.0590740380876835,
+                    "timestamp": 0.9451220141675267
+                },
+                {
+                    "x": 6.908671057688978,
+                    "y": 1.2842767071415118,
+                    "heading": 2.9215938672756456,
+                    "angularVelocity": 2.3120274459298332e-7,
+                    "velocityX": 3.173460718451369,
+                    "velocityY": -2.05907397680523,
+                    "timestamp": 1.0238821820148205
+                },
+                {
+                    "x": 7.1455069915896265,
+                    "y": 1.1314725250685531,
+                    "heading": 2.963985587877118,
+                    "angularVelocity": 0.5382380683048028,
+                    "velocityX": 3.007052173375748,
+                    "velocityY": -1.9401200663922304,
+                    "timestamp": 1.1026423498621143
+                },
+                {
+                    "x": 7.339036455456064,
+                    "y": 1.0065280886493264,
+                    "heading": 3.0237371798128563,
+                    "angularVelocity": 0.7586524199845985,
+                    "velocityX": 2.457199738853539,
+                    "velocityY": -1.5863911902965262,
+                    "timestamp": 1.181402517709408
+                },
+                {
+                    "x": 7.489157100180206,
+                    "y": 0.9096422022009641,
+                    "heading": 3.07668127081849,
+                    "angularVelocity": 0.6722191236093652,
+                    "velocityX": 1.9060478009036246,
+                    "velocityY": -1.2301381408456074,
+                    "timestamp": 1.2601626855567019
+                },
+                {
+                    "x": 7.5959791241645505,
+                    "y": 0.8407792473093463,
+                    "heading": 3.115718535259042,
+                    "angularVelocity": 0.4956472987238782,
+                    "velocityX": 1.3562950270935392,
+                    "velocityY": -0.8743373303257803,
+                    "timestamp": 1.3389228534039956
+                },
+                {
+                    "x": 7.659576823404001,
+                    "y": 0.7999065653267177,
+                    "heading": 3.1374811645909735,
+                    "angularVelocity": 0.2763151720819586,
+                    "velocityX": 0.8074855726914214,
+                    "velocityY": -0.5189511792543529,
+                    "timestamp": 1.4176830212512894
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": 0.03198108228905446,
+                    "velocityX": 0.2593084442835197,
+                    "velocityY": -0.16387173465319213,
+                    "timestamp": 1.4964431890985832
+                },
+                {
+                    "x": 7.653968415649204,
+                    "y": 0.8042046719225767,
+                    "heading": 3.120030327667123,
+                    "angularVelocity": -0.24175586185815795,
+                    "velocityX": -0.3151422820242312,
+                    "velocityY": 0.20828235032084352,
+                    "timestamp": 1.5790458287176843
+                },
+                {
+                    "x": 7.58042792526076,
+                    "y": 0.8521912576149779,
+                    "heading": 3.0798198381250614,
+                    "angularVelocity": -0.4867942444391582,
+                    "velocityX": -0.8902922561258279,
+                    "velocityY": 0.5809328359683593,
+                    "timestamp": 1.6616484683367854
+                },
+                {
+                    "x": 7.459301398894465,
+                    "y": 0.9310142655562479,
+                    "heading": 3.0230678620569518,
+                    "angularVelocity": -0.6870479724353368,
+                    "velocityX": -1.466375976903756,
+                    "velocityY": 0.9542432094753368,
+                    "timestamp": 1.7442511079558864
+                },
+                {
+                    "x": 7.290490307507352,
+                    "y": 1.0407418073187003,
+                    "heading": 2.956300453743236,
+                    "angularVelocity": -0.808296304108442,
+                    "velocityX": -2.0436525051200767,
+                    "velocityY": 1.3283781519383162,
+                    "timestamp": 1.8268537475749875
+                },
+                {
+                    "x": 7.073948364025268,
+                    "y": 1.1814068761754821,
+                    "heading": 2.8937785154835103,
+                    "angularVelocity": -0.756900004019734,
+                    "velocityX": -2.621489391629628,
+                    "velocityY": 1.7029125159367249,
+                    "timestamp": 1.9094563871940886
+                },
+                {
+                    "x": 6.811621234855711,
+                    "y": 1.3511947989186324,
+                    "heading": 2.893774866555063,
+                    "angularVelocity": -0.000044174477718030994,
+                    "velocityX": -3.1757717474792755,
+                    "velocityY": 2.0554781726840226,
+                    "timestamp": 1.9920590268131897
+                },
+                {
+                    "x": 6.5493872326198215,
+                    "y": 1.5211289667970835,
+                    "heading": 2.893774844652908,
+                    "angularVelocity": -2.6515078170743926e-7,
+                    "velocityX": -3.174644338789979,
+                    "velocityY": 2.057248638325058,
+                    "timestamp": 2.074661666432291
+                },
+                {
+                    "x": 6.287153230916077,
+                    "y": 1.691063135496712,
+                    "heading": 2.893774822750754,
+                    "angularVelocity": -2.651507828628349e-7,
+                    "velocityX": -3.1746443323477713,
+                    "velocityY": 2.057248648266354,
+                    "timestamp": 2.1572643060513923
+                },
+                {
+                    "x": 6.024919229212335,
+                    "y": 1.860997304196345,
+                    "heading": 2.893774800848599,
+                    "angularVelocity": -2.651507825022106e-7,
+                    "velocityX": -3.1746443323477354,
+                    "velocityY": 2.0572486482664103,
+                    "timestamp": 2.2398669456704936
+                },
+                {
+                    "x": 5.762685227508594,
+                    "y": 2.030931472895978,
+                    "heading": 2.8937747789464447,
+                    "angularVelocity": -2.651507826588173e-7,
+                    "velocityX": -3.174644332347735,
+                    "velocityY": 2.0572486482664103,
+                    "timestamp": 2.322469585289595
+                },
+                {
+                    "x": 5.500451225804852,
+                    "y": 2.2008656415956116,
+                    "heading": 2.89377475704429,
+                    "angularVelocity": -2.651507831859411e-7,
+                    "velocityX": -3.1746443323477345,
+                    "velocityY": 2.05724864826641,
+                    "timestamp": 2.405072224908696
+                },
+                {
+                    "x": 5.238217224101064,
+                    "y": 2.3707998102951717,
+                    "heading": 2.8937747351421352,
+                    "angularVelocity": -2.6515078363410014e-7,
+                    "velocityX": -3.1746443323483056,
+                    "velocityY": 2.057248648265529,
+                    "timestamp": 2.4876748645277975
+                },
+                {
+                    "x": 4.975983214041022,
+                    "y": 2.5407339660996877,
+                    "heading": 2.89377471323987,
+                    "angularVelocity": -2.651521251593368e-7,
+                    "velocityX": -3.1746444335103496,
+                    "velocityY": 2.0572484921561784,
+                    "timestamp": 2.570277504146899
+                },
+                {
+                    "x": 4.737824648225833,
+                    "y": 2.6937649564980313,
+                    "heading": 2.811064183589652,
+                    "angularVelocity": -1.0013061329711461,
+                    "velocityX": -2.883183478317317,
+                    "velocityY": 1.8526162251473437,
+                    "timestamp": 2.652880143766
+                },
+                {
+                    "x": 4.54767021844827,
+                    "y": 2.816291569523401,
+                    "heading": 2.7234816518817766,
+                    "angularVelocity": -1.0602873238885226,
+                    "velocityX": -2.302038175225493,
+                    "velocityY": 1.48332563693325,
+                    "timestamp": 2.7354827833851014
+                },
+                {
+                    "x": 4.405252215667174,
+                    "y": 2.908161976795727,
+                    "heading": 2.651165155434109,
+                    "angularVelocity": -0.875474401073941,
+                    "velocityX": -1.724133798118536,
+                    "velocityY": 1.1121969914758385,
+                    "timestamp": 2.8180854230042027
+                },
+                {
+                    "x": 4.3103959059570425,
+                    "y": 2.9693921451036718,
+                    "heading": 2.600323851470343,
+                    "angularVelocity": -0.6154924854483996,
+                    "velocityX": -1.1483447762388197,
+                    "velocityY": 0.7412616423674994,
+                    "timestamp": 2.900688062623304
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": -0.3186805118059785,
+                    "velocityX": -0.5737819781013825,
+                    "velocityY": 0.3705433027015611,
+                    "timestamp": 2.9832907022424053
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": 0,
+                    "velocityX": 5.015233372101362e-32,
+                    "velocityY": -2.828100691709421e-32,
+                    "timestamp": 3.0658933418615066
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S3 To C5 To S4": {
+            "waypoints": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 19
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.307906047793953,
+                    "y": 2.9763559763722562,
+                    "heading": 2.599832328454837,
+                    "angularVelocity": 0.3294814998044202,
+                    "velocityX": 0.5727595173353921,
+                    "velocityY": -0.3015705061160393,
+                    "timestamp": 0.07840297094122421
+                },
+                {
+                    "x": 4.397737480320486,
+                    "y": 2.929052986687658,
+                    "heading": 2.6508685642334093,
+                    "angularVelocity": 0.6509477276930885,
+                    "velocityX": 1.1457656699499514,
+                    "velocityY": -0.6033315972179728,
+                    "timestamp": 0.15680594188244842
+                },
+                {
+                    "x": 4.532524605763972,
+                    "y": 2.858064592924414,
+                    "heading": 2.7260427369404674,
+                    "angularVelocity": 0.95881790963547,
+                    "velocityX": 1.7191583918999656,
+                    "velocityY": -0.9054298952072739,
+                    "timestamp": 0.23520891282367262
+                },
+                {
+                    "x": 4.712319110504002,
+                    "y": 2.7633347501885597,
+                    "heading": 2.823264198017844,
+                    "angularVelocity": 1.2400226663643632,
+                    "velocityX": 2.293210353914077,
+                    "velocityY": -1.20824302444969,
+                    "timestamp": 0.31361188376489685
+                },
+                {
+                    "x": 4.937226049144337,
+                    "y": 2.644712434074336,
+                    "heading": 2.936900931840023,
+                    "angularVelocity": 1.4493932112280175,
+                    "velocityX": 2.8686022473427424,
+                    "velocityY": -1.5129824124033164,
+                    "timestamp": 0.3920148547061211
+                },
+                {
+                    "x": 5.200333937862815,
+                    "y": 2.5078106422289963,
+                    "heading": 2.936900964883958,
+                    "angularVelocity": 4.214627997065615e-7,
+                    "velocityX": 3.355840799907034,
+                    "velocityY": -1.7461301555518065,
+                    "timestamp": 0.4704178256473453
+                },
+                {
+                    "x": 5.462885368981394,
+                    "y": 2.3698446646000946,
+                    "heading": 2.936900984463062,
+                    "angularVelocity": 2.4972400074747634e-7,
+                    "velocityX": 3.3487433953925927,
+                    "velocityY": -1.7597034394566977,
+                    "timestamp": 0.5488207965885695
+                },
+                {
+                    "x": 5.725436796470205,
+                    "y": 2.2318786800636885,
+                    "heading": 2.9369010040421655,
+                    "angularVelocity": 2.4972400141004584e-7,
+                    "velocityX": 3.3487433490962957,
+                    "velocityY": -1.759703527559279,
+                    "timestamp": 0.6272237675297937
+                },
+                {
+                    "x": 5.987988223958993,
+                    "y": 2.0939126955272376,
+                    "heading": 2.936901023621269,
+                    "angularVelocity": 2.497240000885312e-7,
+                    "velocityX": 3.3487433490959946,
+                    "velocityY": -1.7597035275598514,
+                    "timestamp": 0.7056267384710179
+                },
+                {
+                    "x": 6.250539651447782,
+                    "y": 1.9559467109907867,
+                    "heading": 2.9369010432003724,
+                    "angularVelocity": 2.497240001698141e-7,
+                    "velocityX": 3.348743349095995,
+                    "velocityY": -1.759703527559851,
+                    "timestamp": 0.784029709412242
+                },
+                {
+                    "x": 6.513091078936429,
+                    "y": 1.8179807264540686,
+                    "heading": 2.936901062779476,
+                    "angularVelocity": 2.497239999907855e-7,
+                    "velocityX": 3.3487433490942036,
+                    "velocityY": -1.7597035275632584,
+                    "timestamp": 0.8624326803534662
+                },
+                {
+                    "x": 6.775642484801246,
+                    "y": 1.6800147007668695,
+                    "heading": 2.9369010823585797,
+                    "angularVelocity": 2.4972399921138707e-7,
+                    "velocityX": 3.348743073290482,
+                    "velocityY": -1.759704052421956,
+                    "timestamp": 0.9408356512946904
+                },
+                {
+                    "x": 7.03487156699359,
+                    "y": 1.5359031200183482,
+                    "heading": 2.936901102721687,
+                    "angularVelocity": 2.5972367283965845e-7,
+                    "velocityX": 3.3063681016204165,
+                    "velocityY": -1.8380882639839122,
+                    "timestamp": 1.0192386222359147
+                },
+                {
+                    "x": 7.257601032453945,
+                    "y": 1.356628823714497,
+                    "heading": 2.964092637940216,
+                    "angularVelocity": 0.34681766382323054,
+                    "velocityX": 2.8408294071826354,
+                    "velocityY": -2.286575293661087,
+                    "timestamp": 1.0976415931771388
+                },
+                {
+                    "x": 7.433803163736967,
+                    "y": 1.1974021851496213,
+                    "heading": 3.0191989559887835,
+                    "angularVelocity": 0.7028600751601236,
+                    "velocityX": 2.2473910002092454,
+                    "velocityY": -2.030875063193899,
+                    "timestamp": 1.176044564118363
+                },
+                {
+                    "x": 7.563846036619734,
+                    "y": 1.0604378478062424,
+                    "heading": 3.0711979216149987,
+                    "angularVelocity": 0.6632269798184063,
+                    "velocityX": 1.6586472594285269,
+                    "velocityY": -1.74692789953109,
+                    "timestamp": 1.2544475350595872
+                },
+                {
+                    "x": 7.648096830602628,
+                    "y": 0.946251036791659,
+                    "heading": 3.1111882105248365,
+                    "angularVelocity": 0.5100608871025,
+                    "velocityX": 1.0745867531729072,
+                    "velocityY": -1.4564092360754384,
+                    "timestamp": 1.3328505060008113
+                },
+                {
+                    "x": 7.6867694062848315,
+                    "y": 0.855064254512633,
+                    "heading": 3.1349445229229786,
+                    "angularVelocity": 0.3030027065666845,
+                    "velocityX": 0.49325395731671434,
+                    "velocityY": -1.1630526392597296,
+                    "timestamp": 1.4112534769420355
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": 0.06448068250805035,
+                    "velocityX": -0.08634119604942615,
+                    "velocityY": -0.8681336140135004,
+                    "timestamp": 1.4896564478832597
+                },
+                {
+                    "x": 7.627073807612577,
+                    "y": 0.7419574324370846,
+                    "heading": 3.1244199952297187,
+                    "angularVelocity": -0.1970875494622002,
+                    "velocityX": -0.6695179952660093,
+                    "velocityY": -0.5697898937387053,
+                    "timestamp": 1.568707635097908
+                },
+                {
+                    "x": 7.5280560417619835,
+                    "y": 0.7206319042387348,
+                    "heading": 3.0899060052252425,
+                    "angularVelocity": -0.4366030570890143,
+                    "velocityX": -1.2525778465750235,
+                    "velocityY": -0.26976860120365537,
+                    "timestamp": 1.6477588223125563
+                },
+                {
+                    "x": 7.382968853200293,
+                    "y": 0.7232223421332327,
+                    "heading": 3.03915644307554,
+                    "angularVelocity": -0.6419835544263846,
+                    "velocityX": -1.8353574901749392,
+                    "velocityY": 0.0327691206892746,
+                    "timestamp": 1.7268100095272048
+                },
+                {
+                    "x": 7.191873936786201,
+                    "y": 0.7500593523651382,
+                    "heading": 2.9768884847472514,
+                    "angularVelocity": -0.7876916276946772,
+                    "velocityX": -2.417356691875479,
+                    "velocityY": 0.3394890219549461,
+                    "timestamp": 1.8058611967418532
+                },
+                {
+                    "x": 6.955009615423507,
+                    "y": 0.8017966281088876,
+                    "heading": 2.913255262792046,
+                    "angularVelocity": -0.8049622554368411,
+                    "velocityX": -2.9963411013610584,
+                    "velocityY": 0.6544781623992936,
+                    "timestamp": 1.8849123839565016
+                },
+                {
+                    "x": 6.67449530614552,
+                    "y": 0.8800439513487449,
+                    "heading": 2.8869556643049132,
+                    "angularVelocity": -0.33269074651445785,
+                    "velocityX": -3.5485148188383207,
+                    "velocityY": 0.9898310954821422,
+                    "timestamp": 1.96396357117115
+                },
+                {
+                    "x": 6.4045779974291,
+                    "y": 1.0087805726115764,
+                    "heading": 2.886955644734896,
+                    "angularVelocity": -2.475613351225265e-7,
+                    "velocityX": -3.4144624290526653,
+                    "velocityY": 1.6285223005349794,
+                    "timestamp": 2.0430147583857985
+                },
+                {
+                    "x": 6.136902414581452,
+                    "y": 1.1421156905122876,
+                    "heading": 2.8869556256683206,
+                    "angularVelocity": -2.4119277641286314e-7,
+                    "velocityX": -3.386104526435805,
+                    "velocityY": 1.6866934273696874,
+                    "timestamp": 2.122065945600447
+                },
+                {
+                    "x": 5.869226659738861,
+                    "y": 1.2754504631222496,
+                    "heading": 2.8869556065980015,
+                    "angularVelocity": -2.4124013985780433e-7,
+                    "velocityX": -3.3861067021774174,
+                    "velocityY": 1.6866890594304795,
+                    "timestamp": 2.2011171328150954
+                },
+                {
+                    "x": 5.637978571315548,
+                    "y": 1.3902481295023497,
+                    "heading": 2.7615946389842474,
+                    "angularVelocity": -1.5858201758975898,
+                    "velocityX": -2.9252955783626895,
+                    "velocityY": 1.4521940836692195,
+                    "timestamp": 2.280168320029744
+                },
+                {
+                    "x": 5.453130511537557,
+                    "y": 1.4821316832792537,
+                    "heading": 2.658075983788866,
+                    "angularVelocity": -1.309514238087925,
+                    "velocityX": -2.3383337593158178,
+                    "velocityY": 1.162329839872467,
+                    "timestamp": 2.3592195072443922
+                },
+                {
+                    "x": 5.3145458591491295,
+                    "y": 1.5510603964907557,
+                    "heading": 2.5794202908528,
+                    "angularVelocity": -0.9949969849595353,
+                    "velocityX": -1.753100203443375,
+                    "velocityY": 0.8719503860752608,
+                    "timestamp": 2.4382706944590407
+                },
+                {
+                    "x": 5.222177020265271,
+                    "y": 1.597019066974222,
+                    "heading": 2.5265746527994444,
+                    "angularVelocity": -0.668498980411977,
+                    "velocityX": -1.1684687117102668,
+                    "velocityY": 0.5813786244434977,
+                    "timestamp": 2.517321881673689
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": -0.336170192198179,
+                    "velocityX": -0.5841407560381138,
+                    "velocityY": 0.29070952424007707,
+                    "timestamp": 2.5963730688883375
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": 0,
+                    "velocityX": 3.1840213941352295e-31,
+                    "velocityY": 0,
+                    "timestamp": 2.675424256102986
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S4 To W3": {
+            "waypoints": [
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 17
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": 0,
+                    "velocityX": -2.991539680273006e-28,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.128677687485332,
+                    "y": 1.6650804220486217,
+                    "heading": 2.5120089352888275,
+                    "angularVelocity": 0.13555115758601896,
+                    "velocityX": -0.5341517867092611,
+                    "velocityY": 0.5088463919723994,
+                    "timestamp": 0.0885933805561246
+                },
+                {
+                    "x": 5.034016646343104,
+                    "y": 1.7552508281834698,
+                    "heading": 2.534662979379905,
+                    "angularVelocity": 0.2557080895761949,
+                    "velocityX": -1.0684888707035958,
+                    "velocityY": 1.017800715683549,
+                    "timestamp": 0.1771867611122492
+                },
+                {
+                    "x": 4.891992835259765,
+                    "y": 1.890523822602506,
+                    "heading": 2.5656675717312063,
+                    "angularVelocity": 0.3499651120288192,
+                    "velocityX": -1.6030973216262263,
+                    "velocityY": 1.5268973095946068,
+                    "timestamp": 0.2657801416683738
+                },
+                {
+                    "x": 4.702571182201446,
+                    "y": 2.070911500413953,
+                    "heading": 2.6003407126574074,
+                    "angularVelocity": 0.3913739458702845,
+                    "velocityX": -2.1381016490087936,
+                    "velocityY": 2.036130427342408,
+                    "timestamp": 0.3543735222244984
+                },
+                {
+                    "x": 4.465767136473608,
+                    "y": 2.2963300542080516,
+                    "heading": 2.6236809711316846,
+                    "angularVelocity": 0.2634537515983315,
+                    "velocityX": -2.672931591969363,
+                    "velocityY": 2.5444175668553926,
+                    "timestamp": 0.442966902780623
+                },
+                {
+                    "x": 4.223169514205327,
+                    "y": 2.5275605696083283,
+                    "heading": 2.6236809810890134,
+                    "angularVelocity": 1.1239359640247893e-7,
+                    "velocityX": -2.7383267321489955,
+                    "velocityY": 2.610020228924234,
+                    "timestamp": 0.5315602833367477
+                },
+                {
+                    "x": 3.980571892636886,
+                    "y": 2.7587910857428417,
+                    "heading": 2.623680991046373,
+                    "angularVelocity": 1.1239395834581428e-7,
+                    "velocityX": -2.7383267242494984,
+                    "velocityY": 2.6100202372119665,
+                    "timestamp": 0.6201536638928723
+                },
+                {
+                    "x": 3.737974271068317,
+                    "y": 2.9900216018772205,
+                    "heading": 2.623681001003734,
+                    "angularVelocity": 1.1239395847018201e-7,
+                    "velocityX": -2.7383267242509515,
+                    "velocityY": 2.610020237210452,
+                    "timestamp": 0.7087470444489969
+                },
+                {
+                    "x": 3.4953766494997485,
+                    "y": 3.2212521180116,
+                    "heading": 2.623681010961094,
+                    "angularVelocity": 1.1239395711385692e-7,
+                    "velocityX": -2.73832672425095,
+                    "velocityY": 2.610020237210453,
+                    "timestamp": 0.7973404250051216
+                },
+                {
+                    "x": 3.252779027931301,
+                    "y": 3.4524826341461052,
+                    "heading": 2.6236810209184545,
+                    "angularVelocity": 1.1239395822212682e-7,
+                    "velocityX": -2.7383267242495783,
+                    "velocityY": 2.6100202372118835,
+                    "timestamp": 0.8859338055612462
+                },
+                {
+                    "x": 3.0101814056711773,
+                    "y": 3.68371314955494,
+                    "heading": 2.6236810308757845,
+                    "angularVelocity": 1.1239361401031205e-7,
+                    "velocityX": -2.7383267320569105,
+                    "velocityY": 2.6100202290208365,
+                    "timestamp": 0.9745271861173709
+                },
+                {
+                    "x": 2.773398473292677,
+                    "y": 3.9091148718156945,
+                    "heading": 2.6467919550399244,
+                    "angularVelocity": 0.2608651348337584,
+                    "velocityX": -2.6726932745104093,
+                    "velocityY": 2.5442275805015666,
+                    "timestamp": 1.0631205666734955
+                },
+                {
+                    "x": 2.58398624574228,
+                    "y": 4.089495303792956,
+                    "heading": 2.682294638537745,
+                    "angularVelocity": 0.40073742840518267,
+                    "velocityX": -2.1379952583522854,
+                    "velocityY": 2.036048639807706,
+                    "timestamp": 1.15171394722962
+                },
+                {
+                    "x": 2.441971645222967,
+                    "y": 4.224760268869052,
+                    "heading": 2.7142172786134493,
+                    "angularVelocity": 0.36032759869046743,
+                    "velocityX": -1.6029933571543074,
+                    "velocityY": 1.5268066781852525,
+                    "timestamp": 1.2403073277857446
+                },
+                {
+                    "x": 2.3473180655703776,
+                    "y": 4.314923716116089,
+                    "heading": 2.737593550901004,
+                    "angularVelocity": 0.263860258416364,
+                    "velocityX": -1.0684046489525902,
+                    "velocityY": 1.0177221670632457,
+                    "timestamp": 1.3289007083418691
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 0.1400381046653499,
+                    "velocityX": -0.5341038492193103,
+                    "velocityY": 0.5087996823346718,
+                    "timestamp": 1.4174940888979937
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 0,
+                    "velocityX": -3.3897612225154066e-29,
+                    "velocityY": 0,
+                    "timestamp": 1.5060874694541182
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S4 To C3 To S1": {
+            "waypoints": [
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 18
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.205167733325116,
+                    "y": 1.6385570868106298,
+                    "heading": 2.543052954613419,
+                    "angularVelocity": 0.6503304289456182,
+                    "velocityX": 0.44058914643605,
+                    "velocityY": 0.2803114985693042,
+                    "timestamp": 0.0662016610283172
+                },
+                {
+                    "x": 5.263930522170129,
+                    "y": 1.6757412702349752,
+                    "heading": 2.6242045140781594,
+                    "angularVelocity": 1.225823615363673,
+                    "velocityX": 0.8876331489609754,
+                    "velocityY": 0.5616805204997857,
+                    "timestamp": 0.1324033220566344
+                },
+                {
+                    "x": 5.3527880828329195,
+                    "y": 1.7316217896984443,
+                    "heading": 2.7372039865474704,
+                    "angularVelocity": 1.706897843863404,
+                    "velocityX": 1.3422255466488056,
+                    "velocityY": 0.8440954289583611,
+                    "timestamp": 0.19860498308495161
+                },
+                {
+                    "x": 5.472262183244267,
+                    "y": 1.806361940098468,
+                    "heading": 2.873328504655769,
+                    "angularVelocity": 2.056210010353016,
+                    "velocityX": 1.804699437378857,
+                    "velocityY": 1.1289769658201285,
+                    "timestamp": 0.2648066441132688
+                },
+                {
+                    "x": 5.622851983789364,
+                    "y": 1.9002058058145468,
+                    "heading": 3.0148229359693395,
+                    "angularVelocity": 2.137324488779513,
+                    "velocityX": 2.2747133260099135,
+                    "velocityY": 1.4175454853906315,
+                    "timestamp": 0.331008305141586
+                },
+                {
+                    "x": 5.803032153079139,
+                    "y": 2.011379887463594,
+                    "heading": 3.1112408203645807,
+                    "angularVelocity": 1.456426967203298,
+                    "velocityX": 2.721686533102414,
+                    "velocityY": 1.6793246562416593,
+                    "timestamp": 0.39720996616990323
+                },
+                {
+                    "x": 6.010803138038067,
+                    "y": 2.135461093514753,
+                    "heading": 3.1324618293668918,
+                    "angularVelocity": 0.32055100540867,
+                    "velocityX": 3.138455768807129,
+                    "velocityY": 1.8742914320237858,
+                    "timestamp": 0.46341162719822043
+                },
+                {
+                    "x": 6.223708618139815,
+                    "y": 2.267331907058253,
+                    "heading": 3.132461891539073,
+                    "angularVelocity": 9.391332530924462e-7,
+                    "velocityX": 3.2160141723739923,
+                    "velocityY": 1.9919562665816168,
+                    "timestamp": 0.5296132882265376
+                },
+                {
+                    "x": 6.436614072470765,
+                    "y": 2.3992027622087693,
+                    "heading": 3.1324619537111715,
+                    "angularVelocity": 9.391320033179244e-7,
+                    "velocityX": 3.2160137830967814,
+                    "velocityY": 1.991956895070635,
+                    "timestamp": 0.5958149492548548
+                },
+                {
+                    "x": 6.649519526799896,
+                    "y": 2.5310736173622237,
+                    "heading": 3.1324620158832706,
+                    "angularVelocity": 9.391320117961031e-7,
+                    "velocityX": 3.2160137830692905,
+                    "velocityY": 1.991956895115017,
+                    "timestamp": 0.6620166102831719
+                },
+                {
+                    "x": 6.862424842072437,
+                    "y": 2.6629446970222306,
+                    "heading": 3.132462078055361,
+                    "angularVelocity": 9.391318749443678e-7,
+                    "velocityX": 3.216011682569308,
+                    "velocityY": 1.991960286367572,
+                    "timestamp": 0.7282182713114891
+                },
+                {
+                    "x": 7.065700834244428,
+                    "y": 2.8092236283210066,
+                    "heading": 3.1324621404036685,
+                    "angularVelocity": 9.417936977148102e-7,
+                    "velocityX": 3.070557279295834,
+                    "velocityY": 2.209596089080417,
+                    "timestamp": 0.7944199323398062
+                },
+                {
+                    "x": 7.245525316742551,
+                    "y": 2.9835274116090518,
+                    "heading": 3.132462207800352,
+                    "angularVelocity": 0.000001018051251212501,
+                    "velocityX": 2.7163137556501122,
+                    "velocityY": 2.632921600162078,
+                    "timestamp": 0.8606215933681234
+                },
+                {
+                    "x": 7.3980692436855175,
+                    "y": 3.182145135413928,
+                    "heading": 3.132462290722476,
+                    "angularVelocity": 0.0000012525686368484786,
+                    "velocityX": 2.304231111024479,
+                    "velocityY": 3.0001924531762993,
+                    "timestamp": 0.9268232543964405
+                },
+                {
+                    "x": 7.520084601366705,
+                    "y": 3.4008478887342375,
+                    "heading": 3.132462412903275,
+                    "angularVelocity": 0.0000018455850946514453,
+                    "velocityX": 1.843086046269626,
+                    "velocityY": 3.3035840781513723,
+                    "timestamp": 0.9930249154247577
+                },
+                {
+                    "x": 7.608973390349284,
+                    "y": 3.6349790028149913,
+                    "heading": 3.1324627892694235,
+                    "angularVelocity": 0.000005685146605966413,
+                    "velocityX": 1.3426972616976043,
+                    "velocityY": 3.536635039725298,
+                    "timestamp": 1.0592265764530748
+                },
+                {
+                    "x": 7.662614259939079,
+                    "y": 3.8787278318222707,
+                    "heading": 3.1349569914257884,
+                    "angularVelocity": 0.03767582440419081,
+                    "velocityX": 0.8102647087184026,
+                    "velocityY": 3.6819140973379696,
+                    "timestamp": 1.125428237481392
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": 0.07617646589996442,
+                    "velocityX": 0.26261788285761967,
+                    "velocityY": 3.7502407692104156,
+                    "timestamp": 1.1916298985097091
+                },
+                {
+                    "x": 7.6496343984109,
+                    "y": 4.420930599340373,
+                    "heading": 3.140398780477199,
+                    "angularVelocity": 0.005103020206742044,
+                    "velocityX": -0.38857538706933203,
+                    "velocityY": 3.7613019480289434,
+                    "timestamp": 1.2697758712078666
+                },
+                {
+                    "x": 7.569234672947985,
+                    "y": 4.705409013581856,
+                    "heading": 3.140398835230542,
+                    "angularVelocity": 7.00654698073033e-7,
+                    "velocityX": -1.0288402931970753,
+                    "velocityY": 3.640346449333809,
+                    "timestamp": 1.347921843906024
+                },
+                {
+                    "x": 7.441209757564435,
+                    "y": 4.9718704928955155,
+                    "heading": 3.1403988601468993,
+                    "angularVelocity": 3.1884377743325344e-7,
+                    "velocityX": -1.6382791200012203,
+                    "velocityY": 3.409791574837347,
+                    "timestamp": 1.4260678166041816
+                },
+                {
+                    "x": 7.269357950164967,
+                    "y": 5.212409598206138,
+                    "heading": 3.1403988766618367,
+                    "angularVelocity": 2.1133446439744448e-7,
+                    "velocityX": -2.199112781707609,
+                    "velocityY": 3.07807423729561,
+                    "timestamp": 1.504213789302339
+                },
+                {
+                    "x": 7.058777847289789,
+                    "y": 5.4198900260822835,
+                    "heading": 3.140398889837091,
+                    "angularVelocity": 1.685979934733241e-7,
+                    "velocityX": -2.6947019226259408,
+                    "velocityY": 2.6550367307797926,
+                    "timestamp": 1.5823597620004966
+                },
+                {
+                    "x": 6.8157174042290976,
+                    "y": 5.588156866874436,
+                    "heading": 3.1403989019071075,
+                    "angularVelocity": 1.5445475097502255e-7,
+                    "velocityX": -3.110338699084864,
+                    "velocityY": 2.1532375243707835,
+                    "timestamp": 1.660505734698654
+                },
+                {
+                    "x": 6.566201090125382,
+                    "y": 5.746693038711312,
+                    "heading": 3.1403989138819113,
+                    "angularVelocity": 1.5323635026193936e-7,
+                    "velocityX": -3.1929516709384456,
+                    "velocityY": 2.0287183889726994,
+                    "timestamp": 1.7386517073968115
+                },
+                {
+                    "x": 6.316684733354014,
+                    "y": 5.905229143394562,
+                    "heading": 3.140398925856715,
+                    "angularVelocity": 1.532363481603047e-7,
+                    "velocityX": -3.1929522169378273,
+                    "velocityY": 2.0287175296369964,
+                    "timestamp": 1.816797680094969
+                },
+                {
+                    "x": 6.067168376581658,
+                    "y": 6.063765248076258,
+                    "heading": 3.1403989378315185,
+                    "angularVelocity": 1.5323634796892713e-7,
+                    "velocityX": -3.1929522169504736,
+                    "velocityY": 2.028717529617091,
+                    "timestamp": 1.8949436527931265
+                },
+                {
+                    "x": 5.817651906877993,
+                    "y": 6.222301175016583,
+                    "heading": 3.1403989498077705,
+                    "angularVelocity": 1.5325488550289546e-7,
+                    "velocityX": -3.1929536620832875,
+                    "velocityY": 2.0287152551381347,
+                    "timestamp": 1.973089625491284
+                },
+                {
+                    "x": 5.6037668941620185,
+                    "y": 6.358199133155801,
+                    "heading": 3.2279940884560396,
+                    "angularVelocity": 1.1209168639620608,
+                    "velocityX": -2.736993415413761,
+                    "velocityY": 1.7390270214456762,
+                    "timestamp": 2.0512355981894417
+                },
+                {
+                    "x": 5.432659669200931,
+                    "y": 6.4669188463641385,
+                    "heading": 3.29806779388705,
+                    "angularVelocity": 0.8967027091917401,
+                    "velocityX": -2.189584684318749,
+                    "velocityY": 1.3912388502514341,
+                    "timestamp": 2.1293815708875994
+                },
+                {
+                    "x": 5.304329710503707,
+                    "y": 6.548459271235515,
+                    "heading": 3.3506253655164633,
+                    "angularVelocity": 0.6725563687371077,
+                    "velocityX": -1.6421826265173798,
+                    "velocityY": 1.0434373270437258,
+                    "timestamp": 2.207527543585757
+                },
+                {
+                    "x": 5.218776570774893,
+                    "y": 6.602819757390003,
+                    "heading": 3.3856653169356727,
+                    "angularVelocity": 0.4483910073584796,
+                    "velocityX": -1.0947862925612357,
+                    "velocityY": 0.6956274812071462,
+                    "timestamp": 2.285673516283915
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": 0.2241956896684635,
+                    "velocityX": -0.5473931579317236,
+                    "velocityY": 0.34781373462440857,
+                    "timestamp": 2.3638194889820725
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": 0,
+                    "velocityX": -1.468222292761223e-31,
+                    "velocityY": 0,
+                    "timestamp": 2.44196546168023
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S4 To C3 To S2": {
+            "waypoints": [
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 18
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.210251836832851,
+                    "y": 1.636837042886923,
+                    "heading": 2.532387023792424,
+                    "angularVelocity": 0.47220005236446794,
+                    "velocityX": 0.4993888678908201,
+                    "velocityY": 0.24548265329423538,
+                    "timestamp": 0.06858750571977071
+                },
+                {
+                    "x": 5.278911164181792,
+                    "y": 1.6705273462343695,
+                    "heading": 2.5947497489204117,
+                    "angularVelocity": 0.909243228391831,
+                    "velocityX": 1.0010471532448244,
+                    "velocityY": 0.4912017574322581,
+                    "timestamp": 0.13717501143954142
+                },
+                {
+                    "x": 5.382181986675021,
+                    "y": 1.7210991538184346,
+                    "heading": 2.6836393944616295,
+                    "angularVelocity": 1.2960034718917393,
+                    "velocityX": 1.5056798087273409,
+                    "velocityY": 0.7373326534236055,
+                    "timestamp": 0.20576251715931212
+                },
+                {
+                    "x": 5.520335944035756,
+                    "y": 1.7886220906689136,
+                    "heading": 2.793394080133565,
+                    "angularVelocity": 1.6002139824177626,
+                    "velocityX": 2.0142729482712705,
+                    "velocityY": 0.9844786764277269,
+                    "timestamp": 0.27435002287908283
+                },
+                {
+                    "x": 5.693737584114165,
+                    "y": 1.8732111914756295,
+                    "heading": 2.9110576970550115,
+                    "angularVelocity": 1.7155255273779335,
+                    "velocityX": 2.5281811644657237,
+                    "velocityY": 1.2333018954257229,
+                    "timestamp": 0.3429375285988535
+                },
+                {
+                    "x": 5.900852745156476,
+                    "y": 1.9734126829741925,
+                    "heading": 2.983977883284084,
+                    "angularVelocity": 1.0631701133294391,
+                    "velocityX": 3.0197214327712314,
+                    "velocityY": 1.4609292238729052,
+                    "timestamp": 0.4115250343186242
+                },
+                {
+                    "x": 6.133968719369523,
+                    "y": 2.087332400713016,
+                    "heading": 2.9839779219365523,
+                    "angularVelocity": 5.635497037494653e-7,
+                    "velocityX": 3.3988110774212195,
+                    "velocityY": 1.6609397957153824,
+                    "timestamp": 0.4801125400383949
+                },
+                {
+                    "x": 6.367084623966495,
+                    "y": 2.2012522609094827,
+                    "heading": 2.9839779605877594,
+                    "angularVelocity": 5.635313124243683e-7,
+                    "velocityX": 3.3988100624247526,
+                    "velocityY": 1.660941872735705,
+                    "timestamp": 0.5487000457581656
+                },
+                {
+                    "x": 6.600200528555652,
+                    "y": 2.315172121121942,
+                    "heading": 2.9839779992389666,
+                    "angularVelocity": 5.635313147177628e-7,
+                    "velocityX": 3.3988100623108077,
+                    "velocityY": 1.6609418729688719,
+                    "timestamp": 0.6172875514779362
+                },
+                {
+                    "x": 6.833315692249175,
+                    "y": 2.4290934974276186,
+                    "heading": 2.983978037890185,
+                    "angularVelocity": 5.635314723100833e-7,
+                    "velocityX": 3.398799260115481,
+                    "velocityY": 1.6609639774790368,
+                    "timestamp": 0.6858750571977069
+                },
+                {
+                    "x": 7.049439158690477,
+                    "y": 2.572660228674406,
+                    "heading": 2.9839780777366456,
+                    "angularVelocity": 5.809580085952453e-7,
+                    "velocityX": 3.151061759328619,
+                    "velocityY": 2.093190731171372,
+                    "timestamp": 0.7544625629174776
+                },
+                {
+                    "x": 7.241451301040205,
+                    "y": 2.747165513732945,
+                    "heading": 2.983978123402154,
+                    "angularVelocity": 6.657992258942737e-7,
+                    "velocityX": 2.7995207047510204,
+                    "velocityY": 2.5442722144105754,
+                    "timestamp": 0.8230500686372483
+                },
+                {
+                    "x": 7.4049637163298945,
+                    "y": 2.9486213241330868,
+                    "heading": 2.98397818333491,
+                    "angularVelocity": 8.738144839700177e-7,
+                    "velocityX": 2.383997108128638,
+                    "velocityY": 2.9372085817383917,
+                    "timestamp": 0.891637574357019
+                },
+                {
+                    "x": 7.536239247186927,
+                    "y": 3.1724233234500354,
+                    "heading": 2.983979044678588,
+                    "angularVelocity": 0.000012558317568342283,
+                    "velocityX": 1.9139860748601447,
+                    "velocityY": 3.263014115593316,
+                    "timestamp": 0.9602250800767896
+                },
+                {
+                    "x": 7.630290050662344,
+                    "y": 3.4075679677071538,
+                    "heading": 3.001663168154554,
+                    "angularVelocity": 0.25783301623794963,
+                    "velocityX": 1.371252715613873,
+                    "velocityY": 3.428388914124583,
+                    "timestamp": 1.0288125857965604
+                },
+                {
+                    "x": 7.68591505099028,
+                    "y": 3.648060804187146,
+                    "heading": 3.0401905596367595,
+                    "angularVelocity": 0.5617260910408102,
+                    "velocityX": 0.8110077738532104,
+                    "velocityY": 3.5063651018682354,
+                    "timestamp": 1.097400091516331
+                },
+                {
+                    "x": 7.702477054476245,
+                    "y": 3.8894159117324456,
+                    "heading": 3.0910773123993565,
+                    "angularVelocity": 0.7419245273403817,
+                    "velocityX": 0.24147260222047945,
+                    "velocityY": 3.5189369406639295,
+                    "timestamp": 1.1659875972361018
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": 0.7132886243236253,
+                    "velocityX": -0.3277135425813607,
+                    "velocityY": 3.463955800321066,
+                    "timestamp": 1.2345751029558725
+                },
+                {
+                    "x": 7.5839717767270045,
+                    "y": 4.427707701571831,
+                    "heading": 3.18571906595995,
+                    "angularVelocity": 0.5075163501765089,
+                    "velocityX": -1.0659861999835865,
+                    "velocityY": 3.338083838051171,
+                    "timestamp": 1.3246590301910224
+                },
+                {
+                    "x": 7.4226682910577315,
+                    "y": 4.711490758148744,
+                    "heading": 3.2114421388245056,
+                    "angularVelocity": 0.28554564231429624,
+                    "velocityX": -1.7905911811350714,
+                    "velocityY": 3.1502074264162947,
+                    "timestamp": 1.4147429574261723
+                },
+                {
+                    "x": 7.199929791587311,
+                    "y": 4.9676132378837865,
+                    "heading": 3.2147906679837175,
+                    "angularVelocity": 0.03717121646428904,
+                    "velocityX": -2.472566486682988,
+                    "velocityY": 2.8431540186572435,
+                    "timestamp": 1.5048268846613222
+                },
+                {
+                    "x": 6.929834673893736,
+                    "y": 5.175413812947385,
+                    "heading": 3.214790655255866,
+                    "angularVelocity": -1.4128881851850989e-7,
+                    "velocityX": -2.998260910501095,
+                    "velocityY": 2.306744182246487,
+                    "timestamp": 1.594910811896472
+                },
+                {
+                    "x": 6.624007279414812,
+                    "y": 5.325753649029949,
+                    "heading": 3.2147906451916457,
+                    "angularVelocity": -1.1172048505786767e-7,
+                    "velocityX": -3.394916317098503,
+                    "velocityY": 1.6688863451759368,
+                    "timestamp": 1.684994739131622
+                },
+                {
+                    "x": 6.294504922277233,
+                    "y": 5.412705714996136,
+                    "heading": 3.2147906373147057,
+                    "angularVelocity": -8.744001355884246e-8,
+                    "velocityX": -3.6577263808388842,
+                    "velocityY": 0.9652339616501584,
+                    "timestamp": 1.775078666366772
+                },
+                {
+                    "x": 5.954617378458506,
+                    "y": 5.437382093793352,
+                    "heading": 3.214790629982673,
+                    "angularVelocity": -8.139113095546486e-8,
+                    "velocityX": -3.773009839274702,
+                    "velocityY": 0.273926543331081,
+                    "timestamp": 1.8651625936019218
+                },
+                {
+                    "x": 5.614729638884886,
+                    "y": 5.46205577615271,
+                    "heading": 3.214790622650651,
+                    "angularVelocity": -8.139101337806881e-8,
+                    "velocityX": -3.7730120123026722,
+                    "velocityY": 0.273896610823274,
+                    "timestamp": 1.9552465208370717
+                },
+                {
+                    "x": 5.27484189928346,
+                    "y": 5.486729458129014,
+                    "heading": 3.214790615318626,
+                    "angularVelocity": -8.139103977990715e-8,
+                    "velocityX": -3.7730120126113382,
+                    "velocityY": 0.27389660657108617,
+                    "timestamp": 2.0453304480722214
+                },
+                {
+                    "x": 4.937591000410069,
+                    "y": 5.511126660727889,
+                    "heading": 3.206674021661731,
+                    "angularVelocity": -0.09010035314854972,
+                    "velocityX": -3.7437410781731515,
+                    "velocityY": 0.27082747552945735,
+                    "timestamp": 2.1354143753073713
+                },
+                {
+                    "x": 4.66772599134931,
+                    "y": 5.5306707771302355,
+                    "heading": 3.1851297651856467,
+                    "angularVelocity": -0.23915760710394238,
+                    "velocityX": -2.9957065299375607,
+                    "velocityY": 0.21695453342447346,
+                    "timestamp": 2.2254983025425212
+                },
+                {
+                    "x": 4.465349331515348,
+                    "y": 5.545333951302465,
+                    "heading": 3.164279125670826,
+                    "angularVelocity": -0.23145793211693713,
+                    "velocityX": -2.246534604399402,
+                    "velocityY": 0.16277236819342672,
+                    "timestamp": 2.315582229777671
+                },
+                {
+                    "x": 4.330445855865838,
+                    "y": 5.555111001202187,
+                    "heading": 3.1485187249188784,
+                    "angularVelocity": -0.17495241643725573,
+                    "velocityX": -1.4975310223472684,
+                    "velocityY": 0.10853267835671068,
+                    "timestamp": 2.405666157012821
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": -0.09456431552591144,
+                    "velocityX": -0.7487002169630299,
+                    "velocityY": 0.054271599250453016,
+                    "timestamp": 2.495750084247971
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": -3.2898689544721607e-31,
+                    "velocityX": 6.680323432143241e-33,
+                    "velocityY": 0,
+                    "timestamp": 2.585834011483121
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S4 To C3 To S3": {
+            "waypoints": [
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 18
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 17
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": -5.457496407482876e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.213718688496592,
+                    "y": 1.6454507753697194,
+                    "heading": 2.523944374487045,
+                    "angularVelocity": 0.3224093526452332,
+                    "velocityX": 0.5078795417016481,
+                    "velocityY": 0.3426929367359288,
+                    "timestamp": 0.07426699719034811
+                },
+                {
+                    "x": 5.289237393773012,
+                    "y": 1.696398991795747,
+                    "heading": 2.5693854471889703,
+                    "angularVelocity": 0.6118609129363125,
+                    "velocityX": 1.0168541631333754,
+                    "velocityY": 0.6860142237264,
+                    "timestamp": 0.14853399438069623
+                },
+                {
+                    "x": 5.402661733248807,
+                    "y": 1.772907618565081,
+                    "heading": 2.632642220499669,
+                    "angularVelocity": 0.8517480940904348,
+                    "velocityX": 1.5272509158420084,
+                    "velocityY": 1.0301833878275757,
+                    "timestamp": 0.22280099157104433
+                },
+                {
+                    "x": 5.554123710239057,
+                    "y": 1.8750622586328765,
+                    "heading": 2.707444867040942,
+                    "angularVelocity": 1.007212481602717,
+                    "velocityX": 2.039425084092856,
+                    "velocityY": 1.3755051898216737,
+                    "timestamp": 0.29706798876139245
+                },
+                {
+                    "x": 5.743710865909146,
+                    "y": 2.002930120234068,
+                    "heading": 2.7801295579879124,
+                    "angularVelocity": 0.9786943554574798,
+                    "velocityX": 2.552777988103829,
+                    "velocityY": 1.7217319460683576,
+                    "timestamp": 0.3713349859517406
+                },
+                {
+                    "x": 5.969240710205324,
+                    "y": 2.1555656332559185,
+                    "heading": 2.8014617866621823,
+                    "angularVelocity": 0.2872369892591052,
+                    "velocityX": 3.036743813919657,
+                    "velocityY": 2.055226665899016,
+                    "timestamp": 0.4456019831420887
+                },
+                {
+                    "x": 6.202237157849221,
+                    "y": 2.312550420967927,
+                    "heading": 2.8014618141625047,
+                    "angularVelocity": 3.702899476409415e-7,
+                    "velocityX": 3.137281113530435,
+                    "velocityY": 2.1137893499268903,
+                    "timestamp": 0.5198689803324368
+                },
+                {
+                    "x": 6.435233608100515,
+                    "y": 2.4695352048100654,
+                    "heading": 2.8014618416628014,
+                    "angularVelocity": 3.7028959815407443e-7,
+                    "velocityX": 3.137281148638853,
+                    "velocityY": 2.1137892978193555,
+                    "timestamp": 0.5941359775227849
+                },
+                {
+                    "x": 6.6682300583517184,
+                    "y": 2.6265199886523383,
+                    "heading": 2.801461869163098,
+                    "angularVelocity": 3.702895988133056e-7,
+                    "velocityX": 3.137281148637628,
+                    "velocityY": 2.1137892978211736,
+                    "timestamp": 0.668402974713133
+                },
+                {
+                    "x": 6.901226494182671,
+                    "y": 2.7835047938971096,
+                    "heading": 2.801461896663395,
+                    "angularVelocity": 3.702895996177554e-7,
+                    "velocityX": 3.137280954469964,
+                    "velocityY": 2.1137895860043394,
+                    "timestamp": 0.7426699719034812
+                },
+                {
+                    "x": 7.132347068567706,
+                    "y": 2.9432384730568093,
+                    "heading": 2.801461924236189,
+                    "angularVelocity": 3.712657746616385e-7,
+                    "velocityX": 3.1120226093518935,
+                    "velocityY": 2.1508029838650415,
+                    "timestamp": 0.8169369690938293
+                },
+                {
+                    "x": 7.334313444212558,
+                    "y": 3.1385360610663486,
+                    "heading": 2.8014619668447454,
+                    "angularVelocity": 5.737212724741241e-7,
+                    "velocityX": 2.7194633321070794,
+                    "velocityY": 2.6296685660925183,
+                    "timestamp": 0.8912039662841774
+                },
+                {
+                    "x": 7.4948116486140535,
+                    "y": 3.3502854712542645,
+                    "heading": 2.8423857101264045,
+                    "angularVelocity": 0.5510353835468871,
+                    "velocityX": 2.161097263568294,
+                    "velocityY": 2.8511912181557233,
+                    "timestamp": 0.9654709634745255
+                },
+                {
+                    "x": 7.612584040336913,
+                    "y": 3.548974342070663,
+                    "heading": 2.9223254670447334,
+                    "angularVelocity": 1.0763833188709784,
+                    "velocityX": 1.5857971397578559,
+                    "velocityY": 2.675331955419639,
+                    "timestamp": 1.0397379606648736
+                },
+                {
+                    "x": 7.689064457271449,
+                    "y": 3.727414986995987,
+                    "heading": 3.0016093061540334,
+                    "angularVelocity": 1.0675514307666671,
+                    "velocityX": 1.0298035443457485,
+                    "velocityY": 2.402690988945958,
+                    "timestamp": 1.1140049578552216
+                },
+                {
+                    "x": 7.725394752128812,
+                    "y": 3.8837274225386396,
+                    "heading": 3.06785364542737,
+                    "angularVelocity": 0.8919754639271454,
+                    "velocityX": 0.4891849170129639,
+                    "velocityY": 2.1047361742931416,
+                    "timestamp": 1.1882719550455696
+                },
+                {
+                    "x": 7.722234181622283,
+                    "y": 4.017077206893078,
+                    "heading": 3.1151383358166123,
+                    "angularVelocity": 0.6366850980665145,
+                    "velocityX": -0.04255686409979938,
+                    "velocityY": 1.7955456582236569,
+                    "timestamp": 1.2625389522359176
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": 0.33476059520309653,
+                    "velocityX": -0.5686803455111581,
+                    "velocityY": 1.4801028352497836,
+                    "timestamp": 1.3368059494262656
+                },
+                {
+                    "x": 7.583164521123183,
+                    "y": 4.220739525524416,
+                    "heading": 3.1367175382385155,
+                    "angularVelocity": -0.03922526988843187,
+                    "velocityX": -1.1571796016903517,
+                    "velocityY": 1.1201830988719972,
+                    "timestamp": 1.4204882738773397
+                },
+                {
+                    "x": 7.437331010349068,
+                    "y": 4.2837766667768715,
+                    "heading": 3.1044919064382244,
+                    "angularVelocity": -0.38509484543694944,
+                    "velocityX": -1.742703871226464,
+                    "velocityY": 0.753290992643391,
+                    "timestamp": 1.5041705983284137
+                },
+                {
+                    "x": 7.242933099071244,
+                    "y": 4.315173048252524,
+                    "heading": 3.047144336604056,
+                    "angularVelocity": -0.6853008710064864,
+                    "velocityX": -2.3230462651820925,
+                    "velocityY": 0.3751853414876064,
+                    "timestamp": 1.5878529227794878
+                },
+                {
+                    "x": 7.00089933731331,
+                    "y": 4.3131727597182845,
+                    "heading": 2.9720298467353206,
+                    "angularVelocity": -0.8976147634695787,
+                    "velocityX": -2.8922925282679244,
+                    "velocityY": -0.023903357696631062,
+                    "timestamp": 1.6715352472305618
+                },
+                {
+                    "x": 6.7143889772576815,
+                    "y": 4.273350582816976,
+                    "heading": 2.8998851691901093,
+                    "angularVelocity": -0.8621256402526404,
+                    "velocityX": -3.4237858703738695,
+                    "velocityY": -0.47587321650692566,
+                    "timestamp": 1.7552175716816358
+                },
+                {
+                    "x": 6.411635278119781,
+                    "y": 4.1808641012010535,
+                    "heading": 2.8998851130559236,
+                    "angularVelocity": -6.708009886588717e-7,
+                    "velocityX": -3.6178930392272837,
+                    "velocityY": -1.1052092807244585,
+                    "timestamp": 1.8388998961327099
+                },
+                {
+                    "x": 6.131017053711007,
+                    "y": 4.034347987511598,
+                    "heading": 2.8998850873919646,
+                    "angularVelocity": -3.066831498282175e-7,
+                    "velocityX": -3.3533751153487676,
+                    "velocityY": -1.7508609452539547,
+                    "timestamp": 1.922582220583784
+                },
+                {
+                    "x": 5.854044563819553,
+                    "y": 3.8810515447858025,
+                    "heading": 2.8998850618676295,
+                    "angularVelocity": -3.050146543457835e-7,
+                    "velocityX": -3.3098087524252473,
+                    "velocityY": -1.8318855711927797,
+                    "timestamp": 2.006264545034858
+                },
+                {
+                    "x": 5.577072094171527,
+                    "y": 3.7277550654846525,
+                    "heading": 2.8998850363432944,
+                    "angularVelocity": -3.0501465285690934e-7,
+                    "velocityX": -3.3098085105171973,
+                    "velocityY": -1.8318860082666197,
+                    "timestamp": 2.089946869485932
+                },
+                {
+                    "x": 5.300099624523611,
+                    "y": 3.5744585861833027,
+                    "heading": 2.8998850108189593,
+                    "angularVelocity": -3.050146519273835e-7,
+                    "velocityX": -3.3098085105158774,
+                    "velocityY": -1.8318860082690038,
+                    "timestamp": 2.173629193937006
+                },
+                {
+                    "x": 5.0231271554381145,
+                    "y": 3.421162105865856,
+                    "heading": 2.899884985294552,
+                    "angularVelocity": -3.050155161997765e-7,
+                    "velocityX": -3.3098085037949927,
+                    "velocityY": -1.831886020411314,
+                    "timestamp": 2.25731151838808
+                },
+                {
+                    "x": 4.7695419048400876,
+                    "y": 3.2806617941163765,
+                    "heading": 2.834618611781297,
+                    "angularVelocity": -0.7799302175385181,
+                    "velocityX": -3.030332298504562,
+                    "velocityY": -1.678972383608021,
+                    "timestamp": 2.340993842839154
+                },
+                {
+                    "x": 4.566605904742026,
+                    "y": 3.1682378317487854,
+                    "heading": 2.7427589462823403,
+                    "angularVelocity": -1.0977188564194784,
+                    "velocityX": -2.4250760411980474,
+                    "velocityY": -1.3434612757837647,
+                    "timestamp": 2.424676167290228
+                },
+                {
+                    "x": 4.414672096834118,
+                    "y": 3.08405727220976,
+                    "heading": 2.6624278185804466,
+                    "angularVelocity": -0.959953350111109,
+                    "velocityX": -1.8156021466246228,
+                    "velocityY": -1.0059538868120508,
+                    "timestamp": 2.508358491741302
+                },
+                {
+                    "x": 4.313522085490217,
+                    "y": 3.0280030162190834,
+                    "heading": 2.6044623353701315,
+                    "angularVelocity": -0.6926849079605295,
+                    "velocityX": -1.2087380699258492,
+                    "velocityY": -0.6698458289533948,
+                    "timestamp": 2.5920408161923763
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": -0.3640235326869004,
+                    "velocityX": -0.6037366411798936,
+                    "velocityY": -0.3346347798387892,
+                    "timestamp": 2.6757231406434503
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": 0,
+                    "velocityX": 3.2974830734555563e-32,
+                    "velocityY": 2.8302357269459625e-32,
+                    "timestamp": 2.7594054650945243
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S4 To C3 To S4": {
+            "waypoints": [
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 18
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 18
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 6.749906626357912e-35,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.210246643603351,
+                    "y": 1.6542137133955805,
+                    "heading": 2.5206016461454785,
+                    "angularVelocity": 0.26943757558327686,
+                    "velocityX": 0.4478929770559344,
+                    "velocityY": 0.44746230101760404,
+                    "timestamp": 0.07646166686617559
+                },
+                {
+                    "x": 5.278793169653249,
+                    "y": 1.7226907202744617,
+                    "heading": 2.5594414345510907,
+                    "angularVelocity": 0.5079641864673213,
+                    "velocityX": 0.8964822356001046,
+                    "velocityY": 0.8955730326770261,
+                    "timestamp": 0.15292333373235117
+                },
+                {
+                    "x": 5.381708599884549,
+                    "y": 1.8254946394710425,
+                    "heading": 2.61294205757613,
+                    "angularVelocity": 0.6997051623093229,
+                    "velocityX": 1.3459741913738752,
+                    "velocityY": 1.344515800008778,
+                    "timestamp": 0.22938500059852676
+                },
+                {
+                    "x": 5.519076425443274,
+                    "y": 1.9627026546285364,
+                    "heading": 2.6750057017308966,
+                    "angularVelocity": 0.8116961962572822,
+                    "velocityX": 1.796558081831356,
+                    "velocityY": 1.7944680096712713,
+                    "timestamp": 0.30584666746470235
+                },
+                {
+                    "x": 5.690930173156719,
+                    "y": 2.1343411411965687,
+                    "heading": 2.7325200708925137,
+                    "angularVelocity": 0.7521987359009454,
+                    "velocityX": 2.2475804511851214,
+                    "velocityY": 2.2447651693028954,
+                    "timestamp": 0.38230833433087796
+                },
+                {
+                    "x": 5.895075735242969,
+                    "y": 2.338989079985295,
+                    "heading": 2.7330245204544825,
+                    "angularVelocity": 0.006597417799580435,
+                    "velocityX": 2.6699072941157542,
+                    "velocityY": 2.6764776021284593,
+                    "timestamp": 0.4587700011970536
+                },
+                {
+                    "x": 6.099717519184054,
+                    "y": 2.543408411889471,
+                    "heading": 2.7330245436350364,
+                    "angularVelocity": 3.031656877658216e-7,
+                    "velocityX": 2.676397106268319,
+                    "velocityY": 2.673487778679394,
+                    "timestamp": 0.5352316680632292
+                },
+                {
+                    "x": 6.3043593056762255,
+                    "y": 2.7478277412397842,
+                    "heading": 2.7330245668155904,
+                    "angularVelocity": 3.03165682232601e-7,
+                    "velocityX": 2.676397139632574,
+                    "velocityY": 2.673487745278835,
+                    "timestamp": 0.6116933349294048
+                },
+                {
+                    "x": 6.509001092168415,
+                    "y": 2.9522470705900794,
+                    "heading": 2.733024589996144,
+                    "angularVelocity": 3.031656817444859e-7,
+                    "velocityX": 2.676397139632809,
+                    "velocityY": 2.6734877452786003,
+                    "timestamp": 0.6881550017955804
+                },
+                {
+                    "x": 6.713642878660604,
+                    "y": 3.1566663999403746,
+                    "heading": 2.733024613176697,
+                    "angularVelocity": 3.0316568050462653e-7,
+                    "velocityX": 2.676397139632809,
+                    "velocityY": 2.6734877452786003,
+                    "timestamp": 0.764616668661756
+                },
+                {
+                    "x": 6.918284665152036,
+                    "y": 3.3610857292914273,
+                    "heading": 2.7330246363572503,
+                    "angularVelocity": 3.0316568069769535e-7,
+                    "velocityX": 2.676397139622915,
+                    "velocityY": 2.6734877452885044,
+                    "timestamp": 0.8410783355279317
+                },
+                {
+                    "x": 7.122926344234402,
+                    "y": 3.565505166167973,
+                    "heading": 2.733024659538502,
+                    "angularVelocity": 3.031748161580926e-7,
+                    "velocityX": 2.6763957348789207,
+                    "velocityY": 2.67348915155517,
+                    "timestamp": 0.9175400023941073
+                },
+                {
+                    "x": 7.301295269977114,
+                    "y": 3.7451643880510668,
+                    "heading": 2.8254238510952887,
+                    "angularVelocity": 1.2084381016504042,
+                    "velocityX": 2.332788873866637,
+                    "velocityY": 2.349663945955243,
+                    "timestamp": 0.9940016692602829
+                },
+                {
+                    "x": 7.445441288033757,
+                    "y": 3.8901748417746975,
+                    "heading": 2.925487102833734,
+                    "angularVelocity": 1.308672120810262,
+                    "velocityX": 1.8852063257910538,
+                    "velocityY": 1.8965118034561068,
+                    "timestamp": 1.0704633361264584
+                },
+                {
+                    "x": 7.555314221872798,
+                    "y": 4.0007360886631185,
+                    "heading": 3.011754015083649,
+                    "angularVelocity": 1.1282374003289979,
+                    "velocityX": 1.436967546513772,
+                    "velocityY": 1.445969613531011,
+                    "timestamp": 1.146925002992634
+                },
+                {
+                    "x": 7.6309987143881095,
+                    "y": 4.077005222028419,
+                    "heading": 3.0780078899884065,
+                    "angularVelocity": 0.8664979148403406,
+                    "velocityX": 0.98983576499549,
+                    "velocityY": 0.997481960454622,
+                    "timestamp": 1.2233866698588094
+                },
+                {
+                    "x": 7.672548767265181,
+                    "y": 4.1190736771641765,
+                    "heading": 3.121324471245367,
+                    "angularVelocity": 0.5665136928387217,
+                    "velocityX": 0.5434102417593452,
+                    "velocityY": 0.5501901391894305,
+                    "timestamp": 1.299848336724985
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": 0.24424694778521738,
+                    "velocityX": 0.09745056627996526,
+                    "velocityY": 0.10366400787072177,
+                    "timestamp": 1.3763100035911604
+                },
+                {
+                    "x": 7.651764263007655,
+                    "y": 4.099219122472668,
+                    "heading": 3.13204050140682,
+                    "angularVelocity": -0.10143830296235676,
+                    "velocityX": -0.35984493368073844,
+                    "velocityY": -0.35404806448388143,
+                    "timestamp": 1.4547764049477947
+                },
+                {
+                    "x": 7.587580185818596,
+                    "y": 4.035451706139359,
+                    "heading": 3.0993558868019444,
+                    "angularVelocity": -0.41654280099226804,
+                    "velocityX": -0.8179816594027142,
+                    "velocityY": -0.8126716050540244,
+                    "timestamp": 1.533242806304429
+                },
+                {
+                    "x": 7.487355398269796,
+                    "y": 3.9356017103716967,
+                    "heading": 3.045691872479544,
+                    "angularVelocity": -0.683910736246133,
+                    "velocityX": -1.2772955789481495,
+                    "velocityY": -1.272519116989166,
+                    "timestamp": 1.6117092076610633
+                },
+                {
+                    "x": 7.35096170591853,
+                    "y": 3.7995397339434045,
+                    "heading": 2.9777353222524336,
+                    "angularVelocity": -0.8660592183684257,
+                    "velocityX": -1.7382432479775494,
+                    "velocityY": -1.7340157580297684,
+                    "timestamp": 1.6901756090176976
+                },
+                {
+                    "x": 7.178293181957013,
+                    "y": 3.6271506268794376,
+                    "heading": 2.9104315316891,
+                    "angularVelocity": -0.8577402480513148,
+                    "velocityX": -2.2005408808889824,
+                    "velocityY": -2.1969799058382784,
+                    "timestamp": 1.7686420103743319
+                },
+                {
+                    "x": 6.971558922924333,
+                    "y": 3.4198249259322475,
+                    "heading": 2.9002273959835922,
+                    "angularVelocity": -0.13004465005511517,
+                    "velocityX": -2.6346851067256227,
+                    "velocityY": -2.6422226247497056,
+                    "timestamp": 1.8471084117309662
+                },
+                {
+                    "x": 6.761730033100129,
+                    "y": 3.2098675579352265,
+                    "heading": 2.900227368387608,
+                    "angularVelocity": -3.5169172941648496e-7,
+                    "velocityX": -2.6741240352099296,
+                    "velocityY": -2.6757614006376103,
+                    "timestamp": 1.9255748130876005
+                },
+                {
+                    "x": 6.551901141744289,
+                    "y": 2.9999101914688975,
+                    "heading": 2.900227340791633,
+                    "angularVelocity": -3.5169161244263704e-7,
+                    "velocityX": -2.674124054729561,
+                    "velocityY": -2.6757613811300027,
+                    "timestamp": 2.0040412144442348
+                },
+                {
+                    "x": 6.342072250388439,
+                    "y": 2.7899528250025782,
+                    "heading": 2.9002273131956575,
+                    "angularVelocity": -3.5169161245965383e-7,
+                    "velocityX": -2.674124054729688,
+                    "velocityY": -2.6757613811298757,
+                    "timestamp": 2.082507615800869
+                },
+                {
+                    "x": 6.132243359032647,
+                    "y": 2.579995458536201,
+                    "heading": 2.9002272855996822,
+                    "angularVelocity": -3.5169161351505577e-7,
+                    "velocityX": -2.674124054728939,
+                    "velocityY": -2.675761381130624,
+                    "timestamp": 2.1609740171575034
+                },
+                {
+                    "x": 5.922414476696474,
+                    "y": 2.3700380830557237,
+                    "heading": 2.900227258003707,
+                    "angularVelocity": -3.51691614830637e-7,
+                    "velocityX": -2.6741239397801397,
+                    "velocityY": -2.67576149600908,
+                    "timestamp": 2.2394404185141377
+                },
+                {
+                    "x": 5.71397323769833,
+                    "y": 2.1587030259315876,
+                    "heading": 2.9002272179386366,
+                    "angularVelocity": -5.106016050290645e-7,
+                    "velocityX": -2.6564393854481323,
+                    "velocityY": -2.6933190954381305,
+                    "timestamp": 2.317906819870772
+                },
+                {
+                    "x": 5.5341856675792425,
+                    "y": 1.9787047842990844,
+                    "heading": 2.7906748594245325,
+                    "angularVelocity": -1.39616901782181,
+                    "velocityX": -2.2912681990084183,
+                    "velocityY": -2.2939530616983537,
+                    "timestamp": 2.3963732212274063
+                },
+                {
+                    "x": 5.390687970791945,
+                    "y": 1.8350178958205943,
+                    "heading": 2.6821705547459764,
+                    "angularVelocity": -1.3828122967612335,
+                    "velocityX": -1.8287788697622565,
+                    "velocityY": -1.8311899869783075,
+                    "timestamp": 2.4748396225840406
+                },
+                {
+                    "x": 5.2832586949423685,
+                    "y": 1.7274312477093827,
+                    "heading": 2.5937450000537616,
+                    "angularVelocity": -1.1269225192361696,
+                    "velocityX": -1.369111798071441,
+                    "velocityY": -1.371117398671364,
+                    "timestamp": 2.553306023940675
+                },
+                {
+                    "x": 5.211730147578831,
+                    "y": 1.6557903820034874,
+                    "heading": 2.5319094528254147,
+                    "angularVelocity": -0.7880512698333239,
+                    "velocityX": -0.9115818506628853,
+                    "velocityY": -0.913013270231208,
+                    "timestamp": 2.631772425297309
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": -0.4066639003920201,
+                    "velocityX": -0.4553560117589102,
+                    "velocityY": -0.45612365782926495,
+                    "timestamp": 2.7102388266539434
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": 0,
+                    "velocityX": -6.900806950408652e-34,
+                    "velocityY": 1.697639013664865e-32,
+                    "timestamp": 2.7887052280105777
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S4 To C4 To S3": {
+            "waypoints": [
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 17
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": 2.0168042378406012e-32,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.220318940234659,
+                    "y": 1.6240042188799209,
+                    "heading": 2.522514839920101,
+                    "angularVelocity": 0.3066580902466342,
+                    "velocityX": 0.6036357185902667,
+                    "velocityY": 0.05453852299210067,
+                    "timestamp": 0.07342000956828917
+                },
+                {
+                    "x": 5.309030400883647,
+                    "y": 1.632013165892952,
+                    "heading": 2.5656393100423864,
+                    "angularVelocity": 0.5873667189075205,
+                    "velocityX": 1.208273618739824,
+                    "velocityY": 0.10908398214768973,
+                    "timestamp": 0.14684001913657835
+                },
+                {
+                    "x": 5.442235678671135,
+                    "y": 1.6440289719213574,
+                    "heading": 2.6263948924042286,
+                    "angularVelocity": 0.8275071430674761,
+                    "velocityX": 1.8142912071346369,
+                    "velocityY": 0.1636584644847863,
+                    "timestamp": 0.22026002870486752
+                },
+                {
+                    "x": 5.6200752852220655,
+                    "y": 1.6600527803178184,
+                    "heading": 2.6994681964366594,
+                    "angularVelocity": 0.9952777786614565,
+                    "velocityX": 2.4222226011223906,
+                    "velocityY": 0.21824851958859787,
+                    "timestamp": 0.2936800382731567
+                },
+                {
+                    "x": 5.842683997650443,
+                    "y": 1.680064789350092,
+                    "heading": 2.773109972156216,
+                    "angularVelocity": 1.0030205137886674,
+                    "velocityX": 3.031989695143347,
+                    "velocityY": 0.2725688698481895,
+                    "timestamp": 0.3671000478414459
+                },
+                {
+                    "x": 6.108354667125484,
+                    "y": 1.7034775424781843,
+                    "heading": 2.8018843070789763,
+                    "angularVelocity": 0.39191407208947715,
+                    "velocityX": 3.6185049693835545,
+                    "velocityY": 0.31888790625005003,
+                    "timestamp": 0.4405200574097351
+                },
+                {
+                    "x": 6.384952370737624,
+                    "y": 1.7286802043771023,
+                    "heading": 2.8018843326053964,
+                    "angularVelocity": 3.476766132233899e-7,
+                    "velocityX": 3.7673340719858177,
+                    "velocityY": 0.34326693836064737,
+                    "timestamp": 0.5139400669780243
+                },
+                {
+                    "x": 6.661294346947145,
+                    "y": 1.7565473171945905,
+                    "heading": 2.8018843581575315,
+                    "angularVelocity": 3.48026857889212e-7,
+                    "velocityX": 3.7638509969478653,
+                    "velocityY": 0.37955746643565225,
+                    "timestamp": 0.5873600765463135
+                },
+                {
+                    "x": 6.9295239726753675,
+                    "y": 1.8286189976607798,
+                    "heading": 2.8018844037308708,
+                    "angularVelocity": 6.207209630445694e-7,
+                    "velocityX": 3.6533586321414817,
+                    "velocityY": 0.9816354000767032,
+                    "timestamp": 0.6607800861146027
+                },
+                {
+                    "x": 7.165852331775521,
+                    "y": 1.9289604111011978,
+                    "heading": 2.8534870271763118,
+                    "angularVelocity": 0.7028414154241911,
+                    "velocityX": 3.218854921019001,
+                    "velocityY": 1.3666766598157905,
+                    "timestamp": 0.7342000956828919
+                },
+                {
+                    "x": 7.3582678693157275,
+                    "y": 2.0352947515499316,
+                    "heading": 2.9321246763669695,
+                    "angularVelocity": 1.0710656352818668,
+                    "velocityX": 2.620750646473749,
+                    "velocityY": 1.4483019148865994,
+                    "timestamp": 0.8076201052511811
+                },
+                {
+                    "x": 7.505754872565929,
+                    "y": 2.142580812119965,
+                    "heading": 3.007723504913372,
+                    "angularVelocity": 1.0296760922659598,
+                    "velocityX": 2.0088120951961708,
+                    "velocityY": 1.4612645953179093,
+                    "timestamp": 0.8810401148194703
+                },
+                {
+                    "x": 7.608443195664495,
+                    "y": 2.2491262386362876,
+                    "heading": 3.070552698416666,
+                    "angularVelocity": 0.855750276698862,
+                    "velocityX": 1.3986421917181704,
+                    "velocityY": 1.4511769631033729,
+                    "timestamp": 0.9544601243877595
+                },
+                {
+                    "x": 7.666484933948513,
+                    "y": 2.354097419514004,
+                    "heading": 3.115652624171886,
+                    "angularVelocity": 0.6142729484838055,
+                    "velocityX": 0.7905438670643103,
+                    "velocityY": 1.4297353200436178,
+                    "timestamp": 1.0278801339560486
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": 0.331617715269703,
+                    "velocityX": 0.18407878357617766,
+                    "velocityY": 1.4015604341523722,
+                    "timestamp": 1.1013001435243377
+                },
+                {
+                    "x": 7.640202623194362,
+                    "y": 2.56833046361043,
+                    "heading": 3.1389376214813485,
+                    "angularVelocity": -0.013028904924243205,
+                    "velocityX": -0.48807108721701464,
+                    "velocityY": 1.3653457784439293,
+                    "timestamp": 1.1828402671824687
+                },
+                {
+                    "x": 7.5455521042399925,
+                    "y": 2.676247019412303,
+                    "heading": 3.111715108217083,
+                    "angularVelocity": -0.3338542062850063,
+                    "velocityX": -1.1607845893293163,
+                    "velocityY": 1.3234779512271038,
+                    "timestamp": 1.2643803908406
+                },
+                {
+                    "x": 7.395994092884954,
+                    "y": 2.780007432926005,
+                    "heading": 3.061424395012115,
+                    "angularVelocity": -0.616760325454245,
+                    "velocityX": -1.8341646375480058,
+                    "velocityY": 1.2725074338705782,
+                    "timestamp": 1.3459205144987312
+                },
+                {
+                    "x": 7.191491634258091,
+                    "y": 2.8782237282133547,
+                    "heading": 2.9936811867966338,
+                    "angularVelocity": -0.8307959955948208,
+                    "velocityX": -2.5079978966461463,
+                    "velocityY": 1.2045149170871063,
+                    "timestamp": 1.4274606381568624
+                },
+                {
+                    "x": 6.932297950713545,
+                    "y": 2.9674313510808874,
+                    "heading": 2.9221270442336884,
+                    "angularVelocity": -0.8775329169593383,
+                    "velocityX": -3.178725661874757,
+                    "velocityY": 1.0940334508388325,
+                    "timestamp": 1.5090007618149937
+                },
+                {
+                    "x": 6.629938780171593,
+                    "y": 3.0284835582667458,
+                    "heading": 2.9221269432503374,
+                    "angularVelocity": -0.0000012384498122465325,
+                    "velocityX": -3.7081029188726404,
+                    "velocityY": 0.7487382217106987,
+                    "timestamp": 1.5905408854731249
+                },
+                {
+                    "x": 6.3215346451204795,
+                    "y": 3.034428801743739,
+                    "heading": 2.922126920201386,
+                    "angularVelocity": -2.8267005270994496e-7,
+                    "velocityX": -3.7822377648596976,
+                    "velocityY": 0.07291187712592015,
+                    "timestamp": 1.6720810091312561
+                },
+                {
+                    "x": 6.01312270862256,
+                    "y": 3.0289030469499765,
+                    "heading": 2.9221268973831047,
+                    "angularVelocity": -2.7984114359464816e-7,
+                    "velocityX": -3.7823334410305938,
+                    "velocityY": -0.06776730946506612,
+                    "timestamp": 1.7536211327893874
+                },
+                {
+                    "x": 5.704710773369897,
+                    "y": 3.023377222654451,
+                    "heading": 2.9221268745648237,
+                    "angularVelocity": -2.7984113775105834e-7,
+                    "velocityX": -3.782333425758887,
+                    "velocityY": -0.06776816182780301,
+                    "timestamp": 1.8351612564475186
+                },
+                {
+                    "x": 5.396298838117239,
+                    "y": 3.0178513983586757,
+                    "heading": 2.922126851746543,
+                    "angularVelocity": -2.7984113682317286e-7,
+                    "velocityX": -3.7823334257588326,
+                    "velocityY": -0.06776816183086862,
+                    "timestamp": 1.9167013801056498
+                },
+                {
+                    "x": 5.087886902401733,
+                    "y": 3.012325599899855,
+                    "heading": 2.922126828928091,
+                    "angularVelocity": -2.798432358955337e-7,
+                    "velocityX": -3.7823334314351498,
+                    "velocityY": -0.06776784496901485,
+                    "timestamp": 1.998241503763781
+                },
+                {
+                    "x": 4.812446473600025,
+                    "y": 3.0084984766412206,
+                    "heading": 2.8330493298750046,
+                    "angularVelocity": -1.0924376252672112,
+                    "velocityX": -3.3779741364697116,
+                    "velocityY": -0.04693546056758059,
+                    "timestamp": 2.0797816274219123
+                },
+                {
+                    "x": 4.592410263299027,
+                    "y": 3.0051727297391726,
+                    "heading": 2.737251875069825,
+                    "angularVelocity": -1.1748504969997702,
+                    "velocityX": -2.6985022885608188,
+                    "velocityY": -0.040786630591711566,
+                    "timestamp": 2.1613217510800435
+                },
+                {
+                    "x": 4.427608751414137,
+                    "y": 3.0026089111743004,
+                    "heading": 2.658186226973151,
+                    "angularVelocity": -0.9696532768109137,
+                    "velocityX": -2.0211094181785265,
+                    "velocityY": -0.03144241693353635,
+                    "timestamp": 2.2428618747381748
+                },
+                {
+                    "x": 4.3178442485460256,
+                    "y": 3.0008747293604148,
+                    "heading": 2.602696579769804,
+                    "angularVelocity": -0.6805195370563186,
+                    "velocityX": -1.346140991008498,
+                    "velocityY": -0.021267833994898407,
+                    "timestamp": 2.324401998396306
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": -0.3519320119027408,
+                    "velocityX": -0.6726044318496349,
+                    "velocityY": -0.010727594234238386,
+                    "timestamp": 2.4059421220544372
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": 0,
+                    "velocityX": -1.5467932880075213e-32,
+                    "velocityY": 0,
+                    "timestamp": 2.4874822457125685
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S4 To C4 To S4": {
+            "waypoints": [
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.2208311166528825,
+                    "y": 1.6350333822394376,
+                    "heading": 2.5207239283142147,
+                    "angularVelocity": 0.274168379737783,
+                    "velocityX": 0.5930957889931558,
+                    "velocityY": 0.1988849791445399,
+                    "timestamp": 0.07558832398555491
+                },
+                {
+                    "x": 5.31056444260237,
+                    "y": 1.6651195189308639,
+                    "heading": 2.5599126898589697,
+                    "angularVelocity": 0.5184499335141133,
+                    "velocityX": 1.1871321021304377,
+                    "velocityY": 0.39802624406880477,
+                    "timestamp": 0.15117664797110983
+                },
+                {
+                    "x": 5.445292613790804,
+                    "y": 1.7102861706342203,
+                    "heading": 2.6140890806736206,
+                    "angularVelocity": 0.716729621164803,
+                    "velocityX": 1.7823939476973087,
+                    "velocityY": 0.5975347688882264,
+                    "timestamp": 0.22676497195666473
+                },
+                {
+                    "x": 5.625130316677607,
+                    "y": 1.7705705454307188,
+                    "heading": 2.677161642931129,
+                    "angularVelocity": 0.834422023559608,
+                    "velocityX": 2.379173044254461,
+                    "velocityY": 0.79753554011886,
+                    "timestamp": 0.30235329594221966
+                },
+                {
+                    "x": 5.850123959727638,
+                    "y": 1.8459876978345418,
+                    "heading": 2.735754513087534,
+                    "angularVelocity": 0.7751576839777234,
+                    "velocityX": 2.97656610421776,
+                    "velocityY": 0.9977354758948757,
+                    "timestamp": 0.3779416199277746
+                },
+                {
+                    "x": 6.118241955907835,
+                    "y": 1.9356767665379448,
+                    "heading": 2.743662980850177,
+                    "angularVelocity": 0.10462552079005556,
+                    "velocityX": 3.547082168820654,
+                    "velocityY": 1.1865465983945254,
+                    "timestamp": 0.4535299439133295
+                },
+                {
+                    "x": 6.3893659801779465,
+                    "y": 2.026544533897615,
+                    "heading": 2.7436630046141004,
+                    "angularVelocity": 3.143861609924561e-7,
+                    "velocityX": 3.5868505871611136,
+                    "velocityY": 1.2021402588187382,
+                    "timestamp": 0.5291182678988844
+                },
+                {
+                    "x": 6.660490004070523,
+                    "y": 2.1174123023837605,
+                    "heading": 2.7436630283780157,
+                    "angularVelocity": 3.1438606173533106e-7,
+                    "velocityX": 3.5868505821664702,
+                    "velocityY": 1.2021402737215172,
+                    "timestamp": 0.6047065918844393
+                },
+                {
+                    "x": 6.931614067804288,
+                    "y": 2.2082799519937164,
+                    "heading": 2.743663052142803,
+                    "angularVelocity": 3.1439759977679495e-7,
+                    "velocityX": 3.5868511092477546,
+                    "velocityY": 1.2021387010422189,
+                    "timestamp": 0.6802949158699942
+                },
+                {
+                    "x": 7.168848548798668,
+                    "y": 2.287192936383632,
+                    "heading": 2.8325335746925835,
+                    "angularVelocity": 1.1757175958389672,
+                    "velocityX": 3.1385069609389844,
+                    "velocityY": 1.0439837825349283,
+                    "timestamp": 0.7558832398555492
+                },
+                {
+                    "x": 7.360921839643199,
+                    "y": 2.3511354933788766,
+                    "heading": 2.9301346596996316,
+                    "angularVelocity": 1.2912190648082638,
+                    "velocityX": 2.541044445981228,
+                    "velocityY": 0.8459316680637452,
+                    "timestamp": 0.8314715638411041
+                },
+                {
+                    "x": 7.507932695990372,
+                    "y": 2.4000526423462776,
+                    "heading": 3.014739800319359,
+                    "angularVelocity": 1.119288484765134,
+                    "velocityX": 1.944888424504045,
+                    "velocityY": 0.6471521842017212,
+                    "timestamp": 0.907059887826659
+                },
+                {
+                    "x": 7.6100514108149255,
+                    "y": 2.4339833202352414,
+                    "heading": 3.0798385166063986,
+                    "angularVelocity": 0.8612271426930879,
+                    "velocityX": 1.3509853035511394,
+                    "velocityY": 0.4488878189103197,
+                    "timestamp": 0.982648211812214
+                },
+                {
+                    "x": 7.667383998050378,
+                    "y": 2.452958398052505,
+                    "heading": 3.1222248732691185,
+                    "angularVelocity": 0.5607526986683374,
+                    "velocityX": 0.7584846999174467,
+                    "velocityY": 0.25103186334555994,
+                    "timestamp": 1.0582365357977688
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": 0.23515704269716123,
+                    "velocityX": 0.16690411011130135,
+                    "velocityY": 0.05346860115942382,
+                    "timestamp": 1.1338248597833236
+                },
+                {
+                    "x": 7.644913557520251,
+                    "y": 2.4451050214750283,
+                    "heading": 3.130617959515128,
+                    "angularVelocity": -0.11950075188579921,
+                    "velocityX": -0.44690238377135916,
+                    "velocityY": -0.1515084996373644,
+                    "timestamp": 1.212335164838424
+                },
+                {
+                    "x": 7.561539712456839,
+                    "y": 2.4170767124271264,
+                    "heading": 3.096030936228231,
+                    "angularVelocity": -0.44054119089994853,
+                    "velocityX": -1.0619477915019166,
+                    "velocityY": -0.3570016576579214,
+                    "timestamp": 1.2908454698935243
+                },
+                {
+                    "x": 7.429747941085825,
+                    "y": 2.3728628641773573,
+                    "heading": 3.040236463304404,
+                    "angularVelocity": -0.7106643246980755,
+                    "velocityX": -1.6786557035858298,
+                    "velocityY": -0.5631598070945942,
+                    "timestamp": 1.3693557749486247
+                },
+                {
+                    "x": 7.249364854543017,
+                    "y": 2.3124007511137603,
+                    "heading": 2.9700149395216235,
+                    "angularVelocity": -0.8944242890598059,
+                    "velocityX": -2.2975721010917782,
+                    "velocityY": -0.7701169040314122,
+                    "timestamp": 1.447866080003725
+                },
+                {
+                    "x": 7.020237112848181,
+                    "y": 2.2356470641616397,
+                    "heading": 2.9000211501626434,
+                    "angularVelocity": -0.8915235943849231,
+                    "velocityX": -2.9184416177471677,
+                    "velocityY": -0.977625636510412,
+                    "timestamp": 1.5263763850588254
+                },
+                {
+                    "x": 6.744411982227483,
+                    "y": 2.1437703180815415,
+                    "heading": 2.8843371221032816,
+                    "angularVelocity": -0.19977031102303894,
+                    "velocityX": -3.5132347330342664,
+                    "velocityY": -1.1702507844749341,
+                    "timestamp": 1.6048866901139258
+                },
+                {
+                    "x": 6.462777076833342,
+                    "y": 2.049479989397194,
+                    "heading": 2.8843370939243,
+                    "angularVelocity": -3.589208009765681e-7,
+                    "velocityX": -3.5872348884198493,
+                    "velocityY": -1.2009930240135072,
+                    "timestamp": 1.6833969951690262
+                },
+                {
+                    "x": 6.181142164450445,
+                    "y": 1.955189681587478,
+                    "heading": 2.884337065745333,
+                    "angularVelocity": -3.589206129086503e-7,
+                    "velocityX": -3.5872349774369066,
+                    "velocityY": -1.200992758129539,
+                    "timestamp": 1.7619073002241266
+                },
+                {
+                    "x": 5.898328744228412,
+                    "y": 1.8644954419041788,
+                    "heading": 2.8843369910470673,
+                    "angularVelocity": -9.514453601861041e-7,
+                    "velocityX": -3.6022458456064737,
+                    "velocityY": -1.1551889859509814,
+                    "timestamp": 1.840417605279227
+                },
+                {
+                    "x": 5.657030939387785,
+                    "y": 1.782595874715138,
+                    "heading": 2.7806821628594123,
+                    "angularVelocity": -1.3202703532347235,
+                    "velocityX": -3.0734539201099995,
+                    "velocityY": -1.043169646730562,
+                    "timestamp": 1.9189279103343273
+                },
+                {
+                    "x": 5.464337892157779,
+                    "y": 1.7174002123662173,
+                    "heading": 2.6765644069162446,
+                    "angularVelocity": -1.3261667480478392,
+                    "velocityX": -2.4543663038217995,
+                    "velocityY": -0.8304089801098533,
+                    "timestamp": 1.9974382153894277
+                },
+                {
+                    "x": 5.320057899798496,
+                    "y": 1.6686446362166114,
+                    "heading": 2.5910880570386063,
+                    "angularVelocity": -1.0887277767886236,
+                    "velocityX": -1.8377204401132547,
+                    "velocityY": -0.6210086193829906,
+                    "timestamp": 2.075948520444528
+                },
+                {
+                    "x": 5.223989702910862,
+                    "y": 1.6362005448482675,
+                    "heading": 2.5310608538348376,
+                    "angularVelocity": -0.7645773782389272,
+                    "velocityX": -1.2236380538862024,
+                    "velocityY": -0.41324627825065113,
+                    "timestamp": 2.154458825499628
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": -0.3956277308187441,
+                    "velocityX": -0.6112535529849511,
+                    "velocityY": -0.2063492790774072,
+                    "timestamp": 2.2329691305547286
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": 3.766832812159788e-31,
+                    "velocityX": 0,
+                    "velocityY": 1.2430862122144045e-32,
+                    "timestamp": 2.311479435609829
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S4 To C5 To S3": {
+            "waypoints": [
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 19
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.21689679817953,
+                    "y": 1.6000159633135866,
+                    "heading": 2.520043259980684,
+                    "angularVelocity": 0.2702645151942541,
+                    "velocityX": 0.5514548702977145,
+                    "velocityY": -0.2694659447557082,
+                    "timestamp": 0.0741616411102716
+                },
+                {
+                    "x": 5.298748395600195,
+                    "y": 1.5600344658293337,
+                    "heading": 2.558338172376498,
+                    "angularVelocity": 0.5163708869233576,
+                    "velocityX": 1.1036918303757857,
+                    "velocityY": -0.5391129010319948,
+                    "timestamp": 0.1483232822205432
+                },
+                {
+                    "x": 5.421633479629326,
+                    "y": 1.5000375028373136,
+                    "heading": 2.612131147144223,
+                    "angularVelocity": 0.7253476859785596,
+                    "velocityX": 1.656989815616613,
+                    "velocityY": -0.8090026338927551,
+                    "timestamp": 0.2224849233308148
+                },
+                {
+                    "x": 5.585659490393147,
+                    "y": 1.420004250322953,
+                    "heading": 2.6766780389547544,
+                    "angularVelocity": 0.8703541459463568,
+                    "velocityX": 2.211736529939155,
+                    "velocityY": -1.0791731590102869,
+                    "timestamp": 0.2966465644410864
+                },
+                {
+                    "x": 5.790940733567372,
+                    "y": 1.3199448136082716,
+                    "heading": 2.7418726481321705,
+                    "angularVelocity": 0.8790880056234819,
+                    "velocityX": 2.7680245488229662,
+                    "velocityY": -1.3492074233619007,
+                    "timestamp": 0.37080820555135796
+                },
+                {
+                    "x": 6.036365145395861,
+                    "y": 1.2006925884246085,
+                    "heading": 2.766396446394458,
+                    "angularVelocity": 0.3306803611021658,
+                    "velocityX": 3.3093174335714934,
+                    "velocityY": -1.6080041298754981,
+                    "timestamp": 0.44496984666162953
+                },
+                {
+                    "x": 6.288321832391407,
+                    "y": 1.0773004957458903,
+                    "heading": 2.7663964696788734,
+                    "angularVelocity": 3.1396844397951453e-7,
+                    "velocityX": 3.3973990222372876,
+                    "velocityY": -1.6638263505420645,
+                    "timestamp": 0.5191314877719011
+                },
+                {
+                    "x": 6.540278624834678,
+                    "y": 0.9539086183831178,
+                    "heading": 2.766396492963265,
+                    "angularVelocity": 3.1396812929995246e-7,
+                    "velocityX": 3.397400444100689,
+                    "velocityY": -1.663823447208988,
+                    "timestamp": 0.5932931288821727
+                },
+                {
+                    "x": 6.801316019800969,
+                    "y": 0.8511114616545457,
+                    "heading": 2.7663965374148853,
+                    "angularVelocity": 5.993883039125867e-7,
+                    "velocityX": 3.519843831100697,
+                    "velocityY": -1.3861230036120238,
+                    "timestamp": 0.6674547699924442
+                },
+                {
+                    "x": 7.047191253658378,
+                    "y": 0.7842163440856953,
+                    "heading": 2.8499152738316673,
+                    "angularVelocity": 1.1261716322139188,
+                    "velocityX": 3.3153963447464685,
+                    "velocityY": -0.9020177623819695,
+                    "timestamp": 0.7416164111027158
+                },
+                {
+                    "x": 7.2541679092956945,
+                    "y": 0.7415676707832506,
+                    "heading": 2.93858588461799,
+                    "angularVelocity": 1.1956398140448308,
+                    "velocityX": 2.790885591778509,
+                    "velocityY": -0.575077259132303,
+                    "timestamp": 0.8157780522129874
+                },
+                {
+                    "x": 7.421132571302565,
+                    "y": 0.7211096612789919,
+                    "heading": 3.016140310820358,
+                    "angularVelocity": 1.045748516905977,
+                    "velocityX": 2.251361478889076,
+                    "velocityY": -0.2758570225521344,
+                    "timestamp": 0.889939693323259
+                },
+                {
+                    "x": 7.547824539966462,
+                    "y": 0.722094092716002,
+                    "heading": 3.0771627100726597,
+                    "angularVelocity": 0.8228296776978581,
+                    "velocityX": 1.7083220754986685,
+                    "velocityY": 0.013274132317907437,
+                    "timestamp": 0.9641013344335305
+                },
+                {
+                    "x": 7.634133595568631,
+                    "y": 0.7441357582804435,
+                    "heading": 3.1189807676901022,
+                    "angularVelocity": 0.5638771876052062,
+                    "velocityX": 1.1637964628348345,
+                    "velocityY": 0.29721113549347455,
+                    "timestamp": 1.0382629755438022
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": 0.28342458439730217,
+                    "velocityX": 0.6184653379387841,
+                    "velocityY": 0.5779839965491583,
+                    "timestamp": 1.1124246166540739
+                },
+                {
+                    "x": 7.681059166008756,
+                    "y": 0.8599952012380814,
+                    "heading": 3.136373480041968,
+                    "angularVelocity": -0.044073969444744315,
+                    "velocityX": 0.012872299297217518,
+                    "velocityY": 0.8871282403539436,
+                    "timestamp": 1.1947071975906878
+                },
+                {
+                    "x": 7.6321422839644315,
+                    "y": 0.9582569372062664,
+                    "heading": 3.107236664084856,
+                    "angularVelocity": -0.3541067334720519,
+                    "velocityX": -0.5944986349176513,
+                    "velocityY": 1.1941985150402432,
+                    "timestamp": 1.2769897785273017
+                },
+                {
+                    "x": 7.533021561059945,
+                    "y": 1.0815072200967695,
+                    "heading": 3.0548999267391683,
+                    "angularVelocity": -0.6360609590747484,
+                    "velocityX": -1.204637989914761,
+                    "velocityY": 1.4978903370258465,
+                    "timestamp": 1.3592723594639156
+                },
+                {
+                    "x": 7.383292536414222,
+                    "y": 1.229213843710233,
+                    "heading": 2.9837724589347805,
+                    "angularVelocity": -0.8644292266327552,
+                    "velocityX": -1.819692855296544,
+                    "velocityY": 1.7951141290432664,
+                    "timestamp": 1.4415549404005294
+                },
+                {
+                    "x": 7.182069766586899,
+                    "y": 1.399958151778736,
+                    "heading": 2.9050185961423023,
+                    "angularVelocity": -0.9571146395266165,
+                    "velocityX": -2.445508727811161,
+                    "velocityY": 2.0750966501650514,
+                    "timestamp": 1.5238375213371433
+                },
+                {
+                    "x": 6.9296160896416765,
+                    "y": 1.5820466019910795,
+                    "heading": 2.9050185123503516,
+                    "angularVelocity": -0.0000010183437364658426,
+                    "velocityX": -3.068130266109342,
+                    "velocityY": 2.2129647385832873,
+                    "timestamp": 1.6061201022737572
+                },
+                {
+                    "x": 6.6549097990315635,
+                    "y": 1.728421234696809,
+                    "heading": 2.9050184910214054,
+                    "angularVelocity": -2.59215813112527e-7,
+                    "velocityX": -3.338571633062085,
+                    "velocityY": 1.7789261231181734,
+                    "timestamp": 1.6884026832103711
+                },
+                {
+                    "x": 6.380203216821482,
+                    "y": 1.8747953201455823,
+                    "heading": 2.905018469692463,
+                    "angularVelocity": -2.59215765603584e-7,
+                    "velocityX": -3.338575176946638,
+                    "velocityY": 1.7789194721727335,
+                    "timestamp": 1.770685264146985
+                },
+                {
+                    "x": 6.105496634609771,
+                    "y": 2.0211694055912957,
+                    "heading": 2.9050184483635206,
+                    "angularVelocity": -2.592157647120513e-7,
+                    "velocityX": -3.3385751769664513,
+                    "velocityY": 1.7789194721355484,
+                    "timestamp": 1.852967845083599
+                },
+                {
+                    "x": 5.83079005239806,
+                    "y": 2.167543491037009,
+                    "heading": 2.905018427034578,
+                    "angularVelocity": -2.592157660778243e-7,
+                    "velocityX": -3.3385751769664513,
+                    "velocityY": 1.778919472135548,
+                    "timestamp": 1.9352504260202128
+                },
+                {
+                    "x": 5.556083470186349,
+                    "y": 2.3139175764827224,
+                    "heading": 2.905018405705636,
+                    "angularVelocity": -2.592157651075657e-7,
+                    "velocityX": -3.338575176966452,
+                    "velocityY": 1.7789194721355481,
+                    "timestamp": 2.0175330069568265
+                },
+                {
+                    "x": 5.281376887974587,
+                    "y": 2.460291661928342,
+                    "heading": 2.9050183843766937,
+                    "angularVelocity": -2.5921576594087425e-7,
+                    "velocityX": -3.3385751769670593,
+                    "velocityY": 1.7789194721344066,
+                    "timestamp": 2.09981558789344
+                },
+                {
+                    "x": 5.006670296810042,
+                    "y": 2.6066657305717675,
+                    "heading": 2.905018363047624,
+                    "angularVelocity": -2.5921731220751964e-7,
+                    "velocityX": -3.338575285772395,
+                    "velocityY": 1.778919267933304,
+                    "timestamp": 2.182098168830054
+                },
+                {
+                    "x": 4.758260828306463,
+                    "y": 2.7376390899942815,
+                    "heading": 2.815277658009563,
+                    "angularVelocity": -1.0906403763293109,
+                    "velocityX": -3.0189800280443744,
+                    "velocityY": 1.591750743980768,
+                    "timestamp": 2.2643807497666675
+                },
+                {
+                    "x": 4.559939575601031,
+                    "y": 2.842572976973571,
+                    "heading": 2.72503756976798,
+                    "angularVelocity": -1.0967095005333054,
+                    "velocityX": -2.4102458922406798,
+                    "velocityY": 1.2752867713292104,
+                    "timestamp": 2.346663330703281
+                },
+                {
+                    "x": 4.4113908880653625,
+                    "y": 2.9212861863102546,
+                    "heading": 2.651616705628037,
+                    "angularVelocity": -0.8923014239977028,
+                    "velocityX": -1.8053479344565475,
+                    "velocityY": 0.9566205682988939,
+                    "timestamp": 2.428945911639895
+                },
+                {
+                    "x": 4.31244325419048,
+                    "y": 2.973762547349605,
+                    "heading": 2.6003949192739833,
+                    "angularVelocity": -0.6225106914611183,
+                    "velocityX": -1.202534397299805,
+                    "velocityY": 0.6377578394116608,
+                    "timestamp": 2.5112284925765085
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": -0.3207838034920002,
+                    "velocityX": -0.6008957622339189,
+                    "velocityY": 0.31887007373537113,
+                    "timestamp": 2.593511073513122
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": 0,
+                    "velocityX": -9.140912570286281e-33,
+                    "velocityY": 0,
+                    "timestamp": 2.675793654449736
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S4 To C5 To S4": {
+            "waypoints": [
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.220564813130112,
+                    "y": 1.6051202900758712,
+                    "heading": 2.517168506154745,
+                    "angularVelocity": 0.22812300403374605,
+                    "velocityX": 0.5921458136084756,
+                    "velocityY": -0.19771109358311004,
+                    "timestamp": 0.07525986354364067
+                },
+                {
+                    "x": 5.309738195223409,
+                    "y": 1.5753445036804834,
+                    "heading": 2.5497776530467964,
+                    "angularVelocity": 0.433287350742303,
+                    "velocityX": 1.1848730238739802,
+                    "velocityY": -0.3956396543042072,
+                    "timestamp": 0.15051972708728134
+                },
+                {
+                    "x": 5.443577884042542,
+                    "y": 1.5306492621981627,
+                    "heading": 2.5951663634005544,
+                    "angularVelocity": 0.6030931789749864,
+                    "velocityX": 1.7783674128179001,
+                    "velocityY": -0.593878853585797,
+                    "timestamp": 0.225779590630922
+                },
+                {
+                    "x": 5.622156845349364,
+                    "y": 1.4710006417461063,
+                    "heading": 2.6487285344970175,
+                    "angularVelocity": 0.7116963621041407,
+                    "velocityX": 2.372831319356166,
+                    "velocityY": -0.7925688095018658,
+                    "timestamp": 0.3010394541745627
+                },
+                {
+                    "x": 5.845518423751245,
+                    "y": 1.3963607866160492,
+                    "heading": 2.700604108231769,
+                    "angularVelocity": 0.6892860456047826,
+                    "velocityX": 2.967871158474283,
+                    "velocityY": -0.9917617653767852,
+                    "timestamp": 0.37629931771820335
+                },
+                {
+                    "x": 6.112314775127604,
+                    "y": 1.3070273852513161,
+                    "heading": 2.7110861192214117,
+                    "angularVelocity": 0.13927757101984,
+                    "velocityX": 3.5450017953016038,
+                    "velocityY": -1.186999247121034,
+                    "timestamp": 0.451559181261844
+                },
+                {
+                    "x": 6.382409273163311,
+                    "y": 1.216999105128909,
+                    "heading": 2.7110861382363836,
+                    "angularVelocity": 2.526575375097703e-7,
+                    "velocityX": 3.588825242542295,
+                    "velocityY": -1.196232306084407,
+                    "timestamp": 0.5268190448054847
+                },
+                {
+                    "x": 6.652503772105593,
+                    "y": 1.1269708277263142,
+                    "heading": 2.711086157251347,
+                    "angularVelocity": 2.5265742250097044e-7,
+                    "velocityX": 3.5888252545882353,
+                    "velocityY": -1.19623226994546,
+                    "timestamp": 0.6020789083491254
+                },
+                {
+                    "x": 6.9225982993416295,
+                    "y": 1.0369426352093585,
+                    "heading": 2.7110861762673086,
+                    "angularVelocity": 2.5267068695978854e-7,
+                    "velocityX": 3.5888256305356965,
+                    "velocityY": -1.19623114204494,
+                    "timestamp": 0.677338771892766
+                },
+                {
+                    "x": 7.1604160781282875,
+                    "y": 0.9586172665469176,
+                    "heading": 2.823441998219159,
+                    "angularVelocity": 1.4929049384563269,
+                    "velocityX": 3.1599549559208913,
+                    "velocityY": -1.040732270488674,
+                    "timestamp": 0.7525986354364067
+                },
+                {
+                    "x": 7.353386220004786,
+                    "y": 0.8948816690648573,
+                    "heading": 2.9268528477366496,
+                    "angularVelocity": 1.3740504519719978,
+                    "velocityX": 2.5640511793461145,
+                    "velocityY": -0.8468736784926819,
+                    "timestamp": 0.8278584989800474
+                },
+                {
+                    "x": 7.501750789212216,
+                    "y": 0.8458382793290063,
+                    "heading": 3.012370231908866,
+                    "angularVelocity": 1.136294701393235,
+                    "velocityX": 1.9713637817240808,
+                    "velocityY": -0.6516539816393957,
+                    "timestamp": 0.903118362523688
+                },
+                {
+                    "x": 7.605609860307225,
+                    "y": 0.8115063107648604,
+                    "heading": 3.0771995592704795,
+                    "angularVelocity": 0.8614063899281612,
+                    "velocityX": 1.3800061042468479,
+                    "velocityY": -0.45617899033577186,
+                    "timestamp": 0.9783782260673287
+                },
+                {
+                    "x": 7.665015516622623,
+                    "y": 0.7918924721730815,
+                    "heading": 3.1200072835634023,
+                    "angularVelocity": 0.5687988560874772,
+                    "velocityX": 0.7893404733713316,
+                    "velocityY": -0.2606148572194197,
+                    "timestamp": 1.0536380896109694
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": 0.26564911886937803,
+                    "velocityX": 0.19910324935265278,
+                    "velocityY": -0.06500772048629082,
+                    "timestamp": 1.1288979531546102
+                },
+                {
+                    "x": 7.647079450231064,
+                    "y": 0.7979971975497204,
+                    "heading": 3.1353264592199808,
+                    "angularVelocity": -0.059354936296623806,
+                    "velocityX": -0.418097803433937,
+                    "velocityY": 0.13966668757779366,
+                    "timestamp": 1.2076368259698977
+                },
+                {
+                    "x": 7.565488841826375,
+                    "y": 0.8251307377838253,
+                    "heading": 3.107083972632397,
+                    "angularVelocity": -0.3586854317033129,
+                    "velocityX": -1.0362176328849728,
+                    "velocityY": 0.3446015832326818,
+                    "timestamp": 1.2863756987851853
+                },
+                {
+                    "x": 7.435127415733675,
+                    "y": 0.8684254377651182,
+                    "heading": 3.05838611435167,
+                    "angularVelocity": -0.6184728907024979,
+                    "velocityX": -1.6556171231776886,
+                    "velocityY": 0.5498516607274432,
+                    "timestamp": 1.365114571600473
+                },
+                {
+                    "x": 7.25585035840985,
+                    "y": 0.927906970601061,
+                    "heading": 2.99467735542164,
+                    "angularVelocity": -0.809114439312372,
+                    "velocityX": -2.2768557754742957,
+                    "velocityY": 0.75542779200662,
+                    "timestamp": 1.4438534444157605
+                },
+                {
+                    "x": 7.027485658601358,
+                    "y": 1.0035719757661283,
+                    "heading": 2.9276702363354157,
+                    "angularVelocity": -0.8510042967393713,
+                    "velocityX": -2.900279006332858,
+                    "velocityY": 0.9609612439152991,
+                    "timestamp": 1.522592317231048
+                },
+                {
+                    "x": 6.751324119312769,
+                    "y": 1.0947095363284396,
+                    "heading": 2.9052334045847372,
+                    "angularVelocity": -0.28495241230227925,
+                    "velocityX": -3.507308771569951,
+                    "velocityY": 1.1574659034821269,
+                    "timestamp": 1.6013311900463356
+                },
+                {
+                    "x": 6.468725179881112,
+                    "y": 1.1888428468535348,
+                    "heading": 2.9052333806250203,
+                    "angularVelocity": -3.0429336907534885e-7,
+                    "velocityX": -3.5890650872613086,
+                    "velocityY": 1.195512548749856,
+                    "timestamp": 1.6800700628616232
+                },
+                {
+                    "x": 6.186126242787232,
+                    "y": 1.2829761643969295,
+                    "heading": 2.9052333566653217,
+                    "angularVelocity": -3.0429313853702755e-7,
+                    "velocityX": -3.5890650575710485,
+                    "velocityY": 1.1955126378837118,
+                    "timestamp": 1.7588089356769108
+                },
+                {
+                    "x": 5.9036205043534045,
+                    "y": 1.377388777995483,
+                    "heading": 2.905233301444114,
+                    "angularVelocity": -7.013207784930713e-7,
+                    "velocityX": -3.5878814152769594,
+                    "velocityY": 1.1990597556563287,
+                    "timestamp": 1.8375478084921983
+                },
+                {
+                    "x": 5.660687795689049,
+                    "y": 1.4583340101651068,
+                    "heading": 2.781705854564737,
+                    "angularVelocity": -1.5688241711201327,
+                    "velocityX": -3.0852957373957746,
+                    "velocityY": 1.0280212209731776,
+                    "timestamp": 1.916286681307486
+                },
+                {
+                    "x": 5.4666551877353555,
+                    "y": 1.5230274173690972,
+                    "heading": 2.6729354985546605,
+                    "angularVelocity": -1.3814060593074367,
+                    "velocityX": -2.464254325927075,
+                    "velocityY": 0.8216196764176925,
+                    "timestamp": 1.9950255541227735
+                },
+                {
+                    "x": 5.321269342371624,
+                    "y": 1.571523398115741,
+                    "heading": 2.5878352744293056,
+                    "angularVelocity": -1.080790479754395,
+                    "velocityX": -1.8464303610846597,
+                    "velocityY": 0.6159090041891974,
+                    "timestamp": 2.073764426938061
+                },
+                {
+                    "x": 5.2244075328588835,
+                    "y": 1.6038438553412442,
+                    "heading": 2.5296373366742775,
+                    "angularVelocity": -0.7391258685091143,
+                    "velocityX": -1.2301650512570566,
+                    "velocityY": 0.4104765037889631,
+                    "timestamp": 2.1525032997533486
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": -0.3764003168270292,
+                    "velocityX": -0.6147856976876184,
+                    "velocityY": 0.20518638483251173,
+                    "timestamp": 2.231242172568636
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": 3.3983560967726525e-33,
+                    "velocityX": 0,
+                    "velocityY": -1.4165312907158555e-34,
+                    "timestamp": 2.309981045383924
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "New Path": {
+            "waypoints": [
+                {
+                    "x": 2.002868175506592,
+                    "y": 7.052158832550049,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 21
+                },
+                {
+                    "x": 7.3945817947387695,
+                    "y": 4.159608364105225,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.002868175506592,
+                    "y": 7.052158832550049,
+                    "heading": 0,
+                    "angularVelocity": 8.788933310419519e-34,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.0607038716342068,
+                    "y": 7.050473549314507,
+                    "heading": 1.1142678013967149e-17,
+                    "angularVelocity": 1.3376235580902032e-16,
+                    "velocityX": 0.6942889931584871,
+                    "velocityY": -0.020230993644643338,
+                    "timestamp": 0.0833020495752158
+                },
+                {
+                    "x": 2.1763752619443593,
+                    "y": 7.0471029829001015,
+                    "heading": 3.3885337626879046e-17,
+                    "angularVelocity": 2.730144097029411e-16,
+                    "velocityX": 1.3885779629672816,
+                    "velocityY": -0.040461986608896404,
+                    "timestamp": 0.1666040991504316
+                },
+                {
+                    "x": 2.349882343195254,
+                    "y": 7.0420471334012955,
+                    "heading": 6.768693556041358e-17,
+                    "angularVelocity": 4.057715055994475e-16,
+                    "velocityX": 2.0828668938599213,
+                    "velocityY": -0.06069297843916565,
+                    "timestamp": 0.24990614872564737
+                },
+                {
+                    "x": 2.5812251089033005,
+                    "y": 7.035306001007014,
+                    "heading": 1.1310438234374613e-16,
+                    "angularVelocity": 5.452140379341678e-16,
+                    "velocityX": 2.7771557469202537,
+                    "velocityY": -0.08092396800146733,
+                    "timestamp": 0.3332081983008632
+                },
+                {
+                    "x": 2.8704035396177314,
+                    "y": 7.026879586284038,
+                    "heading": 1.7011242610247892e-16,
+                    "angularVelocity": 6.843534341468776e-16,
+                    "velocityX": 3.4714443664837273,
+                    "velocityY": -0.10115495075986815,
+                    "timestamp": 0.416510247876079
+                },
+                {
+                    "x": 3.1853965350688225,
+                    "y": 7.01770095699296,
+                    "heading": 1.5081927746826734e-16,
+                    "angularVelocity": -2.3160472920646563e-16,
+                    "velocityX": 3.7813354780265604,
+                    "velocityY": -0.11018491547189069,
+                    "timestamp": 0.4998122974512948
+                },
+                {
+                    "x": 3.5003895305199366,
+                    "y": 7.00852232770188,
+                    "heading": 1.0198438674694206e-16,
+                    "angularVelocity": -5.862387656768365e-16,
+                    "velocityX": 3.7813354780268433,
+                    "velocityY": -0.11018491547189893,
+                    "timestamp": 0.5831143470265105
+                },
+                {
+                    "x": 3.815382525971051,
+                    "y": 6.999343698410801,
+                    "heading": 1.4640214038258707e-16,
+                    "angularVelocity": 5.332132145865036e-16,
+                    "velocityX": 3.7813354780268433,
+                    "velocityY": -0.11018491547189894,
+                    "timestamp": 0.6664163966017264
+                },
+                {
+                    "x": 4.130375521422166,
+                    "y": 6.990165069119721,
+                    "heading": 9.202484523489465e-17,
+                    "angularVelocity": -6.527725959322717e-16,
+                    "velocityX": 3.7813354780268433,
+                    "velocityY": -0.11018491547189893,
+                    "timestamp": 0.7497184461769422
+                },
+                {
+                    "x": 4.44536851687328,
+                    "y": 6.980986439828642,
+                    "heading": 8.683300340325528e-17,
+                    "angularVelocity": -6.232549928980416e-17,
+                    "velocityX": 3.7813354780268433,
+                    "velocityY": -0.11018491547189896,
+                    "timestamp": 0.833020495752158
+                },
+                {
+                    "x": 4.760361512324395,
+                    "y": 6.971807810537562,
+                    "heading": 4.9895892426490756e-17,
+                    "angularVelocity": -4.4341179076768094e-16,
+                    "velocityX": 3.7813354780268433,
+                    "velocityY": -0.1101849154718995,
+                    "timestamp": 0.9163225453273738
+                },
+                {
+                    "x": 5.075354507775272,
+                    "y": 6.962629181238361,
+                    "heading": 7.658568949420125e-17,
+                    "angularVelocity": 3.203978437962892e-16,
+                    "velocityX": 3.781335478024002,
+                    "velocityY": -0.11018491556939063,
+                    "timestamp": 0.9996245949025896
+                },
+                {
+                    "x": 5.390347460810252,
+                    "y": 6.953449096380609,
+                    "heading": 6.207074204602565e-17,
+                    "angularVelocity": -1.7424478175737502e-16,
+                    "velocityX": 3.7813349688420876,
+                    "velocityY": -0.11020238883121884,
+                    "timestamp": 1.0829266444778054
+                },
+                {
+                    "x": 5.699832756918085,
+                    "y": 6.89408845944864,
+                    "heading": 1.1352912563557224e-17,
+                    "angularVelocity": -6.088425140747107e-16,
+                    "velocityX": 3.715218265168718,
+                    "velocityY": -0.7125951550372694,
+                    "timestamp": 1.166228694053021
+                },
+                {
+                    "x": 5.993248235249296,
+                    "y": 6.779144176535828,
+                    "heading": -2.0188379939239748e-17,
+                    "angularVelocity": -3.786376525383924e-16,
+                    "velocityX": 3.5223080323645073,
+                    "velocityY": -1.3798493974512511,
+                    "timestamp": 1.2495307436282368
+                },
+                {
+                    "x": 6.260702133178711,
+                    "y": 6.612491130828857,
+                    "heading": -1.9922096402182107e-17,
+                    "angularVelocity": 3.1966024655343145e-18,
+                    "velocityX": 3.210652070306157,
+                    "velocityY": -2.000587579258764,
+                    "timestamp": 1.3328327932034525
+                },
+                {
+                    "x": 6.492446929695756,
+                    "y": 6.400713070229776,
+                    "heading": -6.399527557587997e-17,
+                    "angularVelocity": -5.310840097207522e-16,
+                    "velocityX": 2.792536369594054,
+                    "velocityY": -2.5519362047963385,
+                    "timestamp": 1.4158200011912707
+                },
+                {
+                    "x": 6.6817397771824,
+                    "y": 6.15026582896043,
+                    "heading": -8.43162270708746e-17,
+                    "angularVelocity": -2.4486848018766247e-16,
+                    "velocityX": 2.2809882640519046,
+                    "velocityY": -3.0179017627163183,
+                    "timestamp": 1.498807209179089
+                },
+                {
+                    "x": 6.822247473161292,
+                    "y": 5.869528955070651,
+                    "heading": -6.616259576550212e-17,
+                    "angularVelocity": 2.1875216368331533e-16,
+                    "velocityX": 1.6931247524259796,
+                    "velocityY": -3.382893348225265,
+                    "timestamp": 1.5817944171669072
+                },
+                {
+                    "x": 6.921893026221615,
+                    "y": 5.5718271423734045,
+                    "heading": -4.4342219960097724e-17,
+                    "angularVelocity": 2.629366179978876e-16,
+                    "velocityX": 1.2007338899140572,
+                    "velocityY": -3.5873217079546538,
+                    "timestamp": 1.6647816251547254
+                },
+                {
+                    "x": 7.021537931073347,
+                    "y": 5.2741251127111655,
+                    "heading": -6.875165326695044e-17,
+                    "angularVelocity": -2.94134890168089e-16,
+                    "velocityX": 1.2007260789681786,
+                    "velocityY": -3.5873243223936337,
+                    "timestamp": 1.7477688331425436
+                },
+                {
+                    "x": 7.1211828359216325,
+                    "y": 4.976423083047797,
+                    "heading": -9.319435110608268e-17,
+                    "angularVelocity": -2.945357294430311e-16,
+                    "velocityX": 1.2007260789266394,
+                    "velocityY": -3.587324322407249,
+                    "timestamp": 1.8307560411303618
+                },
+                {
+                    "x": 7.212315815029836,
+                    "y": 4.704151531470839,
+                    "heading": -5.872326423531157e-17,
+                    "angularVelocity": 4.1537832644440137e-16,
+                    "velocityX": 1.098156948738158,
+                    "velocityY": -3.2808857916623166,
+                    "timestamp": 1.91374324911818
+                },
+                {
+                    "x": 7.285222204628618,
+                    "y": 4.486334271350703,
+                    "heading": -3.7707698468715826e-17,
+                    "angularVelocity": 2.5323862035767555e-16,
+                    "velocityX": 0.8785256350530809,
+                    "velocityY": -2.624708860576564,
+                    "timestamp": 1.9967304571059983
+                },
+                {
+                    "x": 7.339901998909188,
+                    "y": 4.3229713200418995,
+                    "heading": -2.239413365263319e-17,
+                    "angularVelocity": 1.8452922091102423e-16,
+                    "velocityX": 0.6588942513717942,
+                    "velocityY": -1.96853172036809,
+                    "timestamp": 2.0797176650938165
+                },
+                {
+                    "x": 7.376355195935282,
+                    "y": 4.214062683329266,
+                    "heading": -7.106347444201965e-18,
+                    "angularVelocity": 1.8421858583516529e-16,
+                    "velocityX": 0.43926284435843027,
+                    "velocityY": -1.31235451045202,
+                    "timestamp": 2.1627048730816347
+                },
+                {
+                    "x": 7.3945817947387695,
+                    "y": 4.159608364105225,
+                    "heading": 0,
+                    "angularVelocity": 8.563184520521882e-17,
+                    "velocityX": 0.21963142567902694,
+                    "velocityY": -0.6561772656821501,
+                    "timestamp": 2.245692081069453
+                },
+                {
+                    "x": 7.3945817947387695,
+                    "y": 4.159608364105225,
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": -1.513321847281751e-32,
+                    "velocityY": -2.5829360962249507e-33,
+                    "timestamp": 2.328679289057271
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": [
+                {
+                    "x": 4.849137783050537,
+                    "y": 4.090187072753906,
+                    "radius": 0.8600171478139426
+                }
+            ]
+        }
+    },
+    "splitTrajectoriesAtStopPoints": false,
+    "usesObstacles": true
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj
new file mode 100644
index 00000000..2328dc93
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj	
@@ -0,0 +1,121 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": -2.1,
+            "angularVelocity": 1.1712645267224225e-32,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.754119446225283,
+            "y": 6.720987363814453,
+            "heading": -2.1237071001932817,
+            "angularVelocity": -0.324046398407224,
+            "velocityX": 0.6030576296752822,
+            "velocityY": 0.013496027999351609,
+            "timestamp": 0.07315958550933178
+        },
+        {
+            "x": 0.8424134953068435,
+            "y": 6.722973003308925,
+            "heading": -2.1697540075127364,
+            "angularVelocity": -0.6294036112814133,
+            "velocityX": 1.2068691814868098,
+            "velocityY": 0.027141207548542687,
+            "timestamp": 0.14631917101866357
+        },
+        {
+            "x": 0.9749625172883624,
+            "y": 6.725970405382453,
+            "heading": -2.236002046597228,
+            "angularVelocity": -0.9055278077813215,
+            "velocityX": 1.8117792912401405,
+            "velocityY": 0.04097073613339593,
+            "timestamp": 0.21947875652799537
+        },
+        {
+            "x": 1.15189309679064,
+            "y": 6.729994604425149,
+            "heading": -2.318545180808417,
+            "angularVelocity": -1.128261370488901,
+            "velocityX": 2.4184196543829377,
+            "velocityY": 0.05500576602073325,
+            "timestamp": 0.29263834203732714
+        },
+        {
+            "x": 1.3734035805681073,
+            "y": 6.735067453562316,
+            "heading": -2.4074941689331384,
+            "angularVelocity": -1.2158213787772831,
+            "velocityX": 3.0277711694964378,
+            "velocityY": 0.06933950078936708,
+            "timestamp": 0.3657979275466589
+        },
+        {
+            "x": 1.6366126990046324,
+            "y": 6.741789664093089,
+            "heading": -2.440634607899988,
+            "angularVelocity": -0.45298833688286777,
+            "velocityX": 3.5977393338693906,
+            "velocityY": 0.09188420743425116,
+            "timestamp": 0.4389575130559907
+        },
+        {
+            "x": 1.8581204107797031,
+            "y": 6.746837438064149,
+            "heading": -2.53012067379602,
+            "angularVelocity": -1.2231625599444977,
+            "velocityX": 3.0277332796918652,
+            "velocityY": 0.06899675464152406,
+            "timestamp": 0.5121170985653225
+        },
+        {
+            "x": 2.035048200585451,
+            "y": 6.750889717758592,
+            "heading": -2.613093680081622,
+            "angularVelocity": -1.1341371839104541,
+            "velocityX": 2.4183815227216985,
+            "velocityY": 0.05538959339687604,
+            "timestamp": 0.5852766840746543
+        },
+        {
+            "x": 2.1675926710077147,
+            "y": 6.75393921231849,
+            "heading": -2.679769909140043,
+            "angularVelocity": -0.9113806289937514,
+            "velocityX": 1.8117170771198088,
+            "velocityY": 0.04168277524633505,
+            "timestamp": 0.658436269583986
+        },
+        {
+            "x": 2.255882530144482,
+            "y": 6.755978072079856,
+            "heading": -2.726139004333996,
+            "angularVelocity": -0.6338075164194407,
+            "velocityX": 1.206811910183738,
+            "velocityY": 0.027868662010218256,
+            "timestamp": 0.7315958550933178
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": -0.32614995697255006,
+            "velocityX": 0.6030306151733326,
+            "velocityY": 0.013968476079118136,
+            "timestamp": 0.8047554406026496
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 2.3711039345986376e-33,
+            "velocityX": -1.8060489353873473e-34,
+            "velocityY": 7.872914025869492e-32,
+            "timestamp": 0.8779150261119814
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.traj
new file mode 100644
index 00000000..2328dc93
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.traj	
@@ -0,0 +1,121 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": -2.1,
+            "angularVelocity": 1.1712645267224225e-32,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.754119446225283,
+            "y": 6.720987363814453,
+            "heading": -2.1237071001932817,
+            "angularVelocity": -0.324046398407224,
+            "velocityX": 0.6030576296752822,
+            "velocityY": 0.013496027999351609,
+            "timestamp": 0.07315958550933178
+        },
+        {
+            "x": 0.8424134953068435,
+            "y": 6.722973003308925,
+            "heading": -2.1697540075127364,
+            "angularVelocity": -0.6294036112814133,
+            "velocityX": 1.2068691814868098,
+            "velocityY": 0.027141207548542687,
+            "timestamp": 0.14631917101866357
+        },
+        {
+            "x": 0.9749625172883624,
+            "y": 6.725970405382453,
+            "heading": -2.236002046597228,
+            "angularVelocity": -0.9055278077813215,
+            "velocityX": 1.8117792912401405,
+            "velocityY": 0.04097073613339593,
+            "timestamp": 0.21947875652799537
+        },
+        {
+            "x": 1.15189309679064,
+            "y": 6.729994604425149,
+            "heading": -2.318545180808417,
+            "angularVelocity": -1.128261370488901,
+            "velocityX": 2.4184196543829377,
+            "velocityY": 0.05500576602073325,
+            "timestamp": 0.29263834203732714
+        },
+        {
+            "x": 1.3734035805681073,
+            "y": 6.735067453562316,
+            "heading": -2.4074941689331384,
+            "angularVelocity": -1.2158213787772831,
+            "velocityX": 3.0277711694964378,
+            "velocityY": 0.06933950078936708,
+            "timestamp": 0.3657979275466589
+        },
+        {
+            "x": 1.6366126990046324,
+            "y": 6.741789664093089,
+            "heading": -2.440634607899988,
+            "angularVelocity": -0.45298833688286777,
+            "velocityX": 3.5977393338693906,
+            "velocityY": 0.09188420743425116,
+            "timestamp": 0.4389575130559907
+        },
+        {
+            "x": 1.8581204107797031,
+            "y": 6.746837438064149,
+            "heading": -2.53012067379602,
+            "angularVelocity": -1.2231625599444977,
+            "velocityX": 3.0277332796918652,
+            "velocityY": 0.06899675464152406,
+            "timestamp": 0.5121170985653225
+        },
+        {
+            "x": 2.035048200585451,
+            "y": 6.750889717758592,
+            "heading": -2.613093680081622,
+            "angularVelocity": -1.1341371839104541,
+            "velocityX": 2.4183815227216985,
+            "velocityY": 0.05538959339687604,
+            "timestamp": 0.5852766840746543
+        },
+        {
+            "x": 2.1675926710077147,
+            "y": 6.75393921231849,
+            "heading": -2.679769909140043,
+            "angularVelocity": -0.9113806289937514,
+            "velocityX": 1.8117170771198088,
+            "velocityY": 0.04168277524633505,
+            "timestamp": 0.658436269583986
+        },
+        {
+            "x": 2.255882530144482,
+            "y": 6.755978072079856,
+            "heading": -2.726139004333996,
+            "angularVelocity": -0.6338075164194407,
+            "velocityX": 1.206811910183738,
+            "velocityY": 0.027868662010218256,
+            "timestamp": 0.7315958550933178
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": -0.32614995697255006,
+            "velocityX": 0.6030306151733326,
+            "velocityY": 0.013968476079118136,
+            "timestamp": 0.8047554406026496
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 2.3711039345986376e-33,
+            "velocityX": -1.8060489353873473e-34,
+            "velocityY": 7.872914025869492e-32,
+            "timestamp": 0.8779150261119814
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.1.traj
new file mode 100644
index 00000000..a21af086
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.1.traj	
@@ -0,0 +1,265 @@
+{
+    "samples": [
+        {
+            "x": 2.002868175506592,
+            "y": 7.052158832550049,
+            "heading": 0,
+            "angularVelocity": 8.788933310419519e-34,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.0607038716342068,
+            "y": 7.050473549314507,
+            "heading": 1.1142678013967149e-17,
+            "angularVelocity": 1.3376235580902032e-16,
+            "velocityX": 0.6942889931584871,
+            "velocityY": -0.020230993644643338,
+            "timestamp": 0.0833020495752158
+        },
+        {
+            "x": 2.1763752619443593,
+            "y": 7.0471029829001015,
+            "heading": 3.3885337626879046e-17,
+            "angularVelocity": 2.730144097029411e-16,
+            "velocityX": 1.3885779629672816,
+            "velocityY": -0.040461986608896404,
+            "timestamp": 0.1666040991504316
+        },
+        {
+            "x": 2.349882343195254,
+            "y": 7.0420471334012955,
+            "heading": 6.768693556041358e-17,
+            "angularVelocity": 4.057715055994475e-16,
+            "velocityX": 2.0828668938599213,
+            "velocityY": -0.06069297843916565,
+            "timestamp": 0.24990614872564737
+        },
+        {
+            "x": 2.5812251089033005,
+            "y": 7.035306001007014,
+            "heading": 1.1310438234374613e-16,
+            "angularVelocity": 5.452140379341678e-16,
+            "velocityX": 2.7771557469202537,
+            "velocityY": -0.08092396800146733,
+            "timestamp": 0.3332081983008632
+        },
+        {
+            "x": 2.8704035396177314,
+            "y": 7.026879586284038,
+            "heading": 1.7011242610247892e-16,
+            "angularVelocity": 6.843534341468776e-16,
+            "velocityX": 3.4714443664837273,
+            "velocityY": -0.10115495075986815,
+            "timestamp": 0.416510247876079
+        },
+        {
+            "x": 3.1853965350688225,
+            "y": 7.01770095699296,
+            "heading": 1.5081927746826734e-16,
+            "angularVelocity": -2.3160472920646563e-16,
+            "velocityX": 3.7813354780265604,
+            "velocityY": -0.11018491547189069,
+            "timestamp": 0.4998122974512948
+        },
+        {
+            "x": 3.5003895305199366,
+            "y": 7.00852232770188,
+            "heading": 1.0198438674694206e-16,
+            "angularVelocity": -5.862387656768365e-16,
+            "velocityX": 3.7813354780268433,
+            "velocityY": -0.11018491547189893,
+            "timestamp": 0.5831143470265105
+        },
+        {
+            "x": 3.815382525971051,
+            "y": 6.999343698410801,
+            "heading": 1.4640214038258707e-16,
+            "angularVelocity": 5.332132145865036e-16,
+            "velocityX": 3.7813354780268433,
+            "velocityY": -0.11018491547189894,
+            "timestamp": 0.6664163966017264
+        },
+        {
+            "x": 4.130375521422166,
+            "y": 6.990165069119721,
+            "heading": 9.202484523489465e-17,
+            "angularVelocity": -6.527725959322717e-16,
+            "velocityX": 3.7813354780268433,
+            "velocityY": -0.11018491547189893,
+            "timestamp": 0.7497184461769422
+        },
+        {
+            "x": 4.44536851687328,
+            "y": 6.980986439828642,
+            "heading": 8.683300340325528e-17,
+            "angularVelocity": -6.232549928980416e-17,
+            "velocityX": 3.7813354780268433,
+            "velocityY": -0.11018491547189896,
+            "timestamp": 0.833020495752158
+        },
+        {
+            "x": 4.760361512324395,
+            "y": 6.971807810537562,
+            "heading": 4.9895892426490756e-17,
+            "angularVelocity": -4.4341179076768094e-16,
+            "velocityX": 3.7813354780268433,
+            "velocityY": -0.1101849154718995,
+            "timestamp": 0.9163225453273738
+        },
+        {
+            "x": 5.075354507775272,
+            "y": 6.962629181238361,
+            "heading": 7.658568949420125e-17,
+            "angularVelocity": 3.203978437962892e-16,
+            "velocityX": 3.781335478024002,
+            "velocityY": -0.11018491556939063,
+            "timestamp": 0.9996245949025896
+        },
+        {
+            "x": 5.390347460810252,
+            "y": 6.953449096380609,
+            "heading": 6.207074204602565e-17,
+            "angularVelocity": -1.7424478175737502e-16,
+            "velocityX": 3.7813349688420876,
+            "velocityY": -0.11020238883121884,
+            "timestamp": 1.0829266444778054
+        },
+        {
+            "x": 5.699832756918085,
+            "y": 6.89408845944864,
+            "heading": 1.1352912563557224e-17,
+            "angularVelocity": -6.088425140747107e-16,
+            "velocityX": 3.715218265168718,
+            "velocityY": -0.7125951550372694,
+            "timestamp": 1.166228694053021
+        },
+        {
+            "x": 5.993248235249296,
+            "y": 6.779144176535828,
+            "heading": -2.0188379939239748e-17,
+            "angularVelocity": -3.786376525383924e-16,
+            "velocityX": 3.5223080323645073,
+            "velocityY": -1.3798493974512511,
+            "timestamp": 1.2495307436282368
+        },
+        {
+            "x": 6.260702133178711,
+            "y": 6.612491130828857,
+            "heading": -1.9922096402182107e-17,
+            "angularVelocity": 3.1966024655343145e-18,
+            "velocityX": 3.210652070306157,
+            "velocityY": -2.000587579258764,
+            "timestamp": 1.3328327932034525
+        },
+        {
+            "x": 6.492446929695756,
+            "y": 6.400713070229776,
+            "heading": -6.399527557587997e-17,
+            "angularVelocity": -5.310840097207522e-16,
+            "velocityX": 2.792536369594054,
+            "velocityY": -2.5519362047963385,
+            "timestamp": 1.4158200011912707
+        },
+        {
+            "x": 6.6817397771824,
+            "y": 6.15026582896043,
+            "heading": -8.43162270708746e-17,
+            "angularVelocity": -2.4486848018766247e-16,
+            "velocityX": 2.2809882640519046,
+            "velocityY": -3.0179017627163183,
+            "timestamp": 1.498807209179089
+        },
+        {
+            "x": 6.822247473161292,
+            "y": 5.869528955070651,
+            "heading": -6.616259576550212e-17,
+            "angularVelocity": 2.1875216368331533e-16,
+            "velocityX": 1.6931247524259796,
+            "velocityY": -3.382893348225265,
+            "timestamp": 1.5817944171669072
+        },
+        {
+            "x": 6.921893026221615,
+            "y": 5.5718271423734045,
+            "heading": -4.4342219960097724e-17,
+            "angularVelocity": 2.629366179978876e-16,
+            "velocityX": 1.2007338899140572,
+            "velocityY": -3.5873217079546538,
+            "timestamp": 1.6647816251547254
+        },
+        {
+            "x": 7.021537931073347,
+            "y": 5.2741251127111655,
+            "heading": -6.875165326695044e-17,
+            "angularVelocity": -2.94134890168089e-16,
+            "velocityX": 1.2007260789681786,
+            "velocityY": -3.5873243223936337,
+            "timestamp": 1.7477688331425436
+        },
+        {
+            "x": 7.1211828359216325,
+            "y": 4.976423083047797,
+            "heading": -9.319435110608268e-17,
+            "angularVelocity": -2.945357294430311e-16,
+            "velocityX": 1.2007260789266394,
+            "velocityY": -3.587324322407249,
+            "timestamp": 1.8307560411303618
+        },
+        {
+            "x": 7.212315815029836,
+            "y": 4.704151531470839,
+            "heading": -5.872326423531157e-17,
+            "angularVelocity": 4.1537832644440137e-16,
+            "velocityX": 1.098156948738158,
+            "velocityY": -3.2808857916623166,
+            "timestamp": 1.91374324911818
+        },
+        {
+            "x": 7.285222204628618,
+            "y": 4.486334271350703,
+            "heading": -3.7707698468715826e-17,
+            "angularVelocity": 2.5323862035767555e-16,
+            "velocityX": 0.8785256350530809,
+            "velocityY": -2.624708860576564,
+            "timestamp": 1.9967304571059983
+        },
+        {
+            "x": 7.339901998909188,
+            "y": 4.3229713200418995,
+            "heading": -2.239413365263319e-17,
+            "angularVelocity": 1.8452922091102423e-16,
+            "velocityX": 0.6588942513717942,
+            "velocityY": -1.96853172036809,
+            "timestamp": 2.0797176650938165
+        },
+        {
+            "x": 7.376355195935282,
+            "y": 4.214062683329266,
+            "heading": -7.106347444201965e-18,
+            "angularVelocity": 1.8421858583516529e-16,
+            "velocityX": 0.43926284435843027,
+            "velocityY": -1.31235451045202,
+            "timestamp": 2.1627048730816347
+        },
+        {
+            "x": 7.3945817947387695,
+            "y": 4.159608364105225,
+            "heading": 0,
+            "angularVelocity": 8.563184520521882e-17,
+            "velocityX": 0.21963142567902694,
+            "velocityY": -0.6561772656821501,
+            "timestamp": 2.245692081069453
+        },
+        {
+            "x": 7.3945817947387695,
+            "y": 4.159608364105225,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -1.513321847281751e-32,
+            "velocityY": -2.5829360962249507e-33,
+            "timestamp": 2.328679289057271
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.traj
new file mode 100644
index 00000000..a21af086
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.traj	
@@ -0,0 +1,265 @@
+{
+    "samples": [
+        {
+            "x": 2.002868175506592,
+            "y": 7.052158832550049,
+            "heading": 0,
+            "angularVelocity": 8.788933310419519e-34,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.0607038716342068,
+            "y": 7.050473549314507,
+            "heading": 1.1142678013967149e-17,
+            "angularVelocity": 1.3376235580902032e-16,
+            "velocityX": 0.6942889931584871,
+            "velocityY": -0.020230993644643338,
+            "timestamp": 0.0833020495752158
+        },
+        {
+            "x": 2.1763752619443593,
+            "y": 7.0471029829001015,
+            "heading": 3.3885337626879046e-17,
+            "angularVelocity": 2.730144097029411e-16,
+            "velocityX": 1.3885779629672816,
+            "velocityY": -0.040461986608896404,
+            "timestamp": 0.1666040991504316
+        },
+        {
+            "x": 2.349882343195254,
+            "y": 7.0420471334012955,
+            "heading": 6.768693556041358e-17,
+            "angularVelocity": 4.057715055994475e-16,
+            "velocityX": 2.0828668938599213,
+            "velocityY": -0.06069297843916565,
+            "timestamp": 0.24990614872564737
+        },
+        {
+            "x": 2.5812251089033005,
+            "y": 7.035306001007014,
+            "heading": 1.1310438234374613e-16,
+            "angularVelocity": 5.452140379341678e-16,
+            "velocityX": 2.7771557469202537,
+            "velocityY": -0.08092396800146733,
+            "timestamp": 0.3332081983008632
+        },
+        {
+            "x": 2.8704035396177314,
+            "y": 7.026879586284038,
+            "heading": 1.7011242610247892e-16,
+            "angularVelocity": 6.843534341468776e-16,
+            "velocityX": 3.4714443664837273,
+            "velocityY": -0.10115495075986815,
+            "timestamp": 0.416510247876079
+        },
+        {
+            "x": 3.1853965350688225,
+            "y": 7.01770095699296,
+            "heading": 1.5081927746826734e-16,
+            "angularVelocity": -2.3160472920646563e-16,
+            "velocityX": 3.7813354780265604,
+            "velocityY": -0.11018491547189069,
+            "timestamp": 0.4998122974512948
+        },
+        {
+            "x": 3.5003895305199366,
+            "y": 7.00852232770188,
+            "heading": 1.0198438674694206e-16,
+            "angularVelocity": -5.862387656768365e-16,
+            "velocityX": 3.7813354780268433,
+            "velocityY": -0.11018491547189893,
+            "timestamp": 0.5831143470265105
+        },
+        {
+            "x": 3.815382525971051,
+            "y": 6.999343698410801,
+            "heading": 1.4640214038258707e-16,
+            "angularVelocity": 5.332132145865036e-16,
+            "velocityX": 3.7813354780268433,
+            "velocityY": -0.11018491547189894,
+            "timestamp": 0.6664163966017264
+        },
+        {
+            "x": 4.130375521422166,
+            "y": 6.990165069119721,
+            "heading": 9.202484523489465e-17,
+            "angularVelocity": -6.527725959322717e-16,
+            "velocityX": 3.7813354780268433,
+            "velocityY": -0.11018491547189893,
+            "timestamp": 0.7497184461769422
+        },
+        {
+            "x": 4.44536851687328,
+            "y": 6.980986439828642,
+            "heading": 8.683300340325528e-17,
+            "angularVelocity": -6.232549928980416e-17,
+            "velocityX": 3.7813354780268433,
+            "velocityY": -0.11018491547189896,
+            "timestamp": 0.833020495752158
+        },
+        {
+            "x": 4.760361512324395,
+            "y": 6.971807810537562,
+            "heading": 4.9895892426490756e-17,
+            "angularVelocity": -4.4341179076768094e-16,
+            "velocityX": 3.7813354780268433,
+            "velocityY": -0.1101849154718995,
+            "timestamp": 0.9163225453273738
+        },
+        {
+            "x": 5.075354507775272,
+            "y": 6.962629181238361,
+            "heading": 7.658568949420125e-17,
+            "angularVelocity": 3.203978437962892e-16,
+            "velocityX": 3.781335478024002,
+            "velocityY": -0.11018491556939063,
+            "timestamp": 0.9996245949025896
+        },
+        {
+            "x": 5.390347460810252,
+            "y": 6.953449096380609,
+            "heading": 6.207074204602565e-17,
+            "angularVelocity": -1.7424478175737502e-16,
+            "velocityX": 3.7813349688420876,
+            "velocityY": -0.11020238883121884,
+            "timestamp": 1.0829266444778054
+        },
+        {
+            "x": 5.699832756918085,
+            "y": 6.89408845944864,
+            "heading": 1.1352912563557224e-17,
+            "angularVelocity": -6.088425140747107e-16,
+            "velocityX": 3.715218265168718,
+            "velocityY": -0.7125951550372694,
+            "timestamp": 1.166228694053021
+        },
+        {
+            "x": 5.993248235249296,
+            "y": 6.779144176535828,
+            "heading": -2.0188379939239748e-17,
+            "angularVelocity": -3.786376525383924e-16,
+            "velocityX": 3.5223080323645073,
+            "velocityY": -1.3798493974512511,
+            "timestamp": 1.2495307436282368
+        },
+        {
+            "x": 6.260702133178711,
+            "y": 6.612491130828857,
+            "heading": -1.9922096402182107e-17,
+            "angularVelocity": 3.1966024655343145e-18,
+            "velocityX": 3.210652070306157,
+            "velocityY": -2.000587579258764,
+            "timestamp": 1.3328327932034525
+        },
+        {
+            "x": 6.492446929695756,
+            "y": 6.400713070229776,
+            "heading": -6.399527557587997e-17,
+            "angularVelocity": -5.310840097207522e-16,
+            "velocityX": 2.792536369594054,
+            "velocityY": -2.5519362047963385,
+            "timestamp": 1.4158200011912707
+        },
+        {
+            "x": 6.6817397771824,
+            "y": 6.15026582896043,
+            "heading": -8.43162270708746e-17,
+            "angularVelocity": -2.4486848018766247e-16,
+            "velocityX": 2.2809882640519046,
+            "velocityY": -3.0179017627163183,
+            "timestamp": 1.498807209179089
+        },
+        {
+            "x": 6.822247473161292,
+            "y": 5.869528955070651,
+            "heading": -6.616259576550212e-17,
+            "angularVelocity": 2.1875216368331533e-16,
+            "velocityX": 1.6931247524259796,
+            "velocityY": -3.382893348225265,
+            "timestamp": 1.5817944171669072
+        },
+        {
+            "x": 6.921893026221615,
+            "y": 5.5718271423734045,
+            "heading": -4.4342219960097724e-17,
+            "angularVelocity": 2.629366179978876e-16,
+            "velocityX": 1.2007338899140572,
+            "velocityY": -3.5873217079546538,
+            "timestamp": 1.6647816251547254
+        },
+        {
+            "x": 7.021537931073347,
+            "y": 5.2741251127111655,
+            "heading": -6.875165326695044e-17,
+            "angularVelocity": -2.94134890168089e-16,
+            "velocityX": 1.2007260789681786,
+            "velocityY": -3.5873243223936337,
+            "timestamp": 1.7477688331425436
+        },
+        {
+            "x": 7.1211828359216325,
+            "y": 4.976423083047797,
+            "heading": -9.319435110608268e-17,
+            "angularVelocity": -2.945357294430311e-16,
+            "velocityX": 1.2007260789266394,
+            "velocityY": -3.587324322407249,
+            "timestamp": 1.8307560411303618
+        },
+        {
+            "x": 7.212315815029836,
+            "y": 4.704151531470839,
+            "heading": -5.872326423531157e-17,
+            "angularVelocity": 4.1537832644440137e-16,
+            "velocityX": 1.098156948738158,
+            "velocityY": -3.2808857916623166,
+            "timestamp": 1.91374324911818
+        },
+        {
+            "x": 7.285222204628618,
+            "y": 4.486334271350703,
+            "heading": -3.7707698468715826e-17,
+            "angularVelocity": 2.5323862035767555e-16,
+            "velocityX": 0.8785256350530809,
+            "velocityY": -2.624708860576564,
+            "timestamp": 1.9967304571059983
+        },
+        {
+            "x": 7.339901998909188,
+            "y": 4.3229713200418995,
+            "heading": -2.239413365263319e-17,
+            "angularVelocity": 1.8452922091102423e-16,
+            "velocityX": 0.6588942513717942,
+            "velocityY": -1.96853172036809,
+            "timestamp": 2.0797176650938165
+        },
+        {
+            "x": 7.376355195935282,
+            "y": 4.214062683329266,
+            "heading": -7.106347444201965e-18,
+            "angularVelocity": 1.8421858583516529e-16,
+            "velocityX": 0.43926284435843027,
+            "velocityY": -1.31235451045202,
+            "timestamp": 2.1627048730816347
+        },
+        {
+            "x": 7.3945817947387695,
+            "y": 4.159608364105225,
+            "heading": 0,
+            "angularVelocity": 8.563184520521882e-17,
+            "velocityX": 0.21963142567902694,
+            "velocityY": -0.6561772656821501,
+            "timestamp": 2.245692081069453
+        },
+        {
+            "x": 7.3945817947387695,
+            "y": 4.159608364105225,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -1.513321847281751e-32,
+            "velocityY": -2.5829360962249507e-33,
+            "timestamp": 2.328679289057271
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S1.1.traj
new file mode 100644
index 00000000..7ba7fd1a
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S1.1.traj	
@@ -0,0 +1,247 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 2.29574628206475e-30,
+            "velocityX": 4.692945968016159e-32,
+            "velocityY": -1.566641075737276e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 5.235048481309196,
+            "y": 6.649731386951801,
+            "heading": -2.8906451335254117,
+            "angularVelocity": -0.12312313043682141,
+            "velocityX": 0.6829631445217772,
+            "velocityY": 0.2282160316336707,
+            "timestamp": 0.08645925008229886
+        },
+        {
+            "x": 5.3531499211746905,
+            "y": 6.689193367474774,
+            "heading": -2.911632072054118,
+            "angularVelocity": -0.24273791998680166,
+            "velocityX": 1.3659780735212093,
+            "velocityY": 0.45642288691397265,
+            "timestamp": 0.17291850016459773
+        },
+        {
+            "x": 5.530311551906127,
+            "y": 6.748384549002882,
+            "heading": -2.9424540818551694,
+            "angularVelocity": -0.3564917550356453,
+            "velocityX": 2.0490766524439916,
+            "velocityY": 0.6846136355771774,
+            "timestamp": 0.2593777502468966
+        },
+        {
+            "x": 5.7665469637988895,
+            "y": 6.827301853965155,
+            "heading": -2.9820943384526712,
+            "angularVelocity": -0.4584848533802071,
+            "velocityX": 2.732332418658699,
+            "velocityY": 0.9127687886162316,
+            "timestamp": 0.34583700032919545
+        },
+        {
+            "x": 6.061889894006546,
+            "y": 6.925933356393463,
+            "heading": -3.027468756644365,
+            "angularVelocity": -0.5248069830402681,
+            "velocityX": 3.415978393596438,
+            "velocityY": 1.1407860042093994,
+            "timestamp": 0.43229625041149433
+        },
+        {
+            "x": 6.3720848481132855,
+            "y": 7.029634881790706,
+            "heading": -3.0274687634280455,
+            "angularVelocity": -7.846101114866984e-8,
+            "velocityX": 3.587759017241877,
+            "velocityY": 1.1994266119419068,
+            "timestamp": 0.5187555004937932
+        },
+        {
+            "x": 6.682279802039258,
+            "y": 7.133336407728698,
+            "heading": -3.027468770211708,
+            "angularVelocity": -7.846080505503267e-8,
+            "velocityX": 3.5877590151510974,
+            "velocityY": 1.1994266181962872,
+            "timestamp": 0.605214750576092
+        },
+        {
+            "x": 6.992474757407683,
+            "y": 7.237037929350706,
+            "heading": -3.027468776995789,
+            "angularVelocity": -7.846565119739109e-8,
+            "velocityX": 3.5877590318346937,
+            "velocityY": 1.1994265682769871,
+            "timestamp": 0.6916740006583908
+        },
+        {
+            "x": 7.248034956342824,
+            "y": 7.322496159466712,
+            "heading": -3.07460707499528,
+            "angularVelocity": -0.5452082681093516,
+            "velocityX": 2.9558456578310177,
+            "velocityY": 0.9884220605040753,
+            "timestamp": 0.7781332507406896
+        },
+        {
+            "x": 7.444567623191234,
+            "y": 7.388221479482509,
+            "heading": -3.109745402875848,
+            "angularVelocity": -0.406414904676237,
+            "velocityX": 2.2731248149998473,
+            "velocityY": 0.7601884119189827,
+            "timestamp": 0.8645925008229884
+        },
+        {
+            "x": 7.58207269769946,
+            "y": 7.434213892330609,
+            "heading": -3.1328871823753137,
+            "angularVelocity": -0.2676611175500936,
+            "velocityX": 1.5904032752686705,
+            "velocityY": 0.5319547972517508,
+            "timestamp": 0.9510517509052873
+        },
+        {
+            "x": 7.660550155243088,
+            "y": 7.460473398860823,
+            "heading": -3.1440339918196103,
+            "angularVelocity": -0.12892558556646783,
+            "velocityX": 0.907681450727999,
+            "velocityY": 0.3037211924135578,
+            "timestamp": 1.037511000987586
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0.009816007412950826,
+            "velocityX": 0.22495967450928966,
+            "velocityY": 0.07548759829543861,
+            "timestamp": 1.123970251069885
+        },
+        {
+            "x": 7.635308945850804,
+            "y": 7.452085548446669,
+            "heading": -3.129057152990018,
+            "angularVelocity": 0.1555765979823413,
+            "velocityX": -0.49212955006536996,
+            "velocityY": -0.1642351578439422,
+            "timestamp": 1.2147818151045664
+        },
+        {
+            "x": 7.525497368411343,
+            "y": 7.41540168270092,
+            "heading": -3.1017205952496476,
+            "angularVelocity": 0.301025073527845,
+            "velocityX": -1.2092246026888915,
+            "velocityY": -0.4039558853066335,
+            "timestamp": 1.3055933791392478
+        },
+        {
+            "x": 7.350564387959683,
+            "y": 7.356948703524257,
+            "heading": -3.061223374709373,
+            "angularVelocity": 0.44594783682873773,
+            "velocityX": -1.9263293426462433,
+            "velocityY": -0.6436733008397082,
+            "timestamp": 1.3964049431739292
+        },
+        {
+            "x": 7.110508199108376,
+            "y": 7.276727219900285,
+            "heading": -3.007665771604216,
+            "angularVelocity": 0.5897663328854458,
+            "velocityX": -2.6434539631934633,
+            "velocityY": -0.8833840103589294,
+            "timestamp": 1.4872165072086105
+        },
+        {
+            "x": 6.80532324732103,
+            "y": 7.174739191350757,
+            "heading": -2.9413637265725296,
+            "angularVelocity": 0.7301057495614385,
+            "velocityX": -3.3606397492593936,
+            "velocityY": -1.1230731419900883,
+            "timestamp": 1.578028071243292
+        },
+        {
+            "x": 6.479515641287707,
+            "y": 7.065810200947661,
+            "heading": -2.9413637193977467,
+            "angularVelocity": 7.900737542269723e-8,
+            "velocityX": -3.587732570148064,
+            "velocityY": -1.1995057189140004,
+            "timestamp": 1.6688396352779733
+        },
+        {
+            "x": 6.153708035353397,
+            "y": 6.956881210248373,
+            "heading": -2.9413637122229894,
+            "angularVelocity": 7.900708560619817e-8,
+            "velocityX": -3.5877325690577564,
+            "velocityY": -1.1995057221756227,
+            "timestamp": 1.7596511993126547
+        },
+        {
+            "x": 5.827900259692805,
+            "y": 6.847953284193167,
+            "heading": -2.9413633662901284,
+            "angularVelocity": 0.0000038093481209652,
+            "velocityX": -3.587734438051908,
+            "velocityY": -1.1994939985136122,
+            "timestamp": 1.850462763347336
+        },
+        {
+            "x": 5.5671157361716075,
+            "y": 6.7607637759552865,
+            "heading": -2.922540068712598,
+            "angularVelocity": 0.2072786409826639,
+            "velocityX": -2.8717105171937094,
+            "velocityY": -0.9601145973503112,
+            "timestamp": 1.9412743273820174
+        },
+        {
+            "x": 5.371545145894187,
+            "y": 6.69537761850095,
+            "heading": -2.9031356081339226,
+            "angularVelocity": 0.2136782995204373,
+            "velocityX": -2.1535868515901284,
+            "velocityY": -0.7200201664777647,
+            "timestamp": 2.0320858914166986
+        },
+        {
+            "x": 5.241178014496252,
+            "y": 6.651791296591565,
+            "heading": -2.888169171521839,
+            "angularVelocity": 0.1648076076208368,
+            "velocityX": -1.4355785277318724,
+            "velocityY": -0.47996444475676003,
+            "timestamp": 2.1228974554513798
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0.08995739264147623,
+            "velocityX": -0.717728135057343,
+            "velocityY": -0.23996169235946166,
+            "timestamp": 2.213709019486061
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -2.956634078664977e-32,
+            "timestamp": 2.304520583520742
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S1.traj
new file mode 100644
index 00000000..7ba7fd1a
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S1.traj	
@@ -0,0 +1,247 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 2.29574628206475e-30,
+            "velocityX": 4.692945968016159e-32,
+            "velocityY": -1.566641075737276e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 5.235048481309196,
+            "y": 6.649731386951801,
+            "heading": -2.8906451335254117,
+            "angularVelocity": -0.12312313043682141,
+            "velocityX": 0.6829631445217772,
+            "velocityY": 0.2282160316336707,
+            "timestamp": 0.08645925008229886
+        },
+        {
+            "x": 5.3531499211746905,
+            "y": 6.689193367474774,
+            "heading": -2.911632072054118,
+            "angularVelocity": -0.24273791998680166,
+            "velocityX": 1.3659780735212093,
+            "velocityY": 0.45642288691397265,
+            "timestamp": 0.17291850016459773
+        },
+        {
+            "x": 5.530311551906127,
+            "y": 6.748384549002882,
+            "heading": -2.9424540818551694,
+            "angularVelocity": -0.3564917550356453,
+            "velocityX": 2.0490766524439916,
+            "velocityY": 0.6846136355771774,
+            "timestamp": 0.2593777502468966
+        },
+        {
+            "x": 5.7665469637988895,
+            "y": 6.827301853965155,
+            "heading": -2.9820943384526712,
+            "angularVelocity": -0.4584848533802071,
+            "velocityX": 2.732332418658699,
+            "velocityY": 0.9127687886162316,
+            "timestamp": 0.34583700032919545
+        },
+        {
+            "x": 6.061889894006546,
+            "y": 6.925933356393463,
+            "heading": -3.027468756644365,
+            "angularVelocity": -0.5248069830402681,
+            "velocityX": 3.415978393596438,
+            "velocityY": 1.1407860042093994,
+            "timestamp": 0.43229625041149433
+        },
+        {
+            "x": 6.3720848481132855,
+            "y": 7.029634881790706,
+            "heading": -3.0274687634280455,
+            "angularVelocity": -7.846101114866984e-8,
+            "velocityX": 3.587759017241877,
+            "velocityY": 1.1994266119419068,
+            "timestamp": 0.5187555004937932
+        },
+        {
+            "x": 6.682279802039258,
+            "y": 7.133336407728698,
+            "heading": -3.027468770211708,
+            "angularVelocity": -7.846080505503267e-8,
+            "velocityX": 3.5877590151510974,
+            "velocityY": 1.1994266181962872,
+            "timestamp": 0.605214750576092
+        },
+        {
+            "x": 6.992474757407683,
+            "y": 7.237037929350706,
+            "heading": -3.027468776995789,
+            "angularVelocity": -7.846565119739109e-8,
+            "velocityX": 3.5877590318346937,
+            "velocityY": 1.1994265682769871,
+            "timestamp": 0.6916740006583908
+        },
+        {
+            "x": 7.248034956342824,
+            "y": 7.322496159466712,
+            "heading": -3.07460707499528,
+            "angularVelocity": -0.5452082681093516,
+            "velocityX": 2.9558456578310177,
+            "velocityY": 0.9884220605040753,
+            "timestamp": 0.7781332507406896
+        },
+        {
+            "x": 7.444567623191234,
+            "y": 7.388221479482509,
+            "heading": -3.109745402875848,
+            "angularVelocity": -0.406414904676237,
+            "velocityX": 2.2731248149998473,
+            "velocityY": 0.7601884119189827,
+            "timestamp": 0.8645925008229884
+        },
+        {
+            "x": 7.58207269769946,
+            "y": 7.434213892330609,
+            "heading": -3.1328871823753137,
+            "angularVelocity": -0.2676611175500936,
+            "velocityX": 1.5904032752686705,
+            "velocityY": 0.5319547972517508,
+            "timestamp": 0.9510517509052873
+        },
+        {
+            "x": 7.660550155243088,
+            "y": 7.460473398860823,
+            "heading": -3.1440339918196103,
+            "angularVelocity": -0.12892558556646783,
+            "velocityX": 0.907681450727999,
+            "velocityY": 0.3037211924135578,
+            "timestamp": 1.037511000987586
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0.009816007412950826,
+            "velocityX": 0.22495967450928966,
+            "velocityY": 0.07548759829543861,
+            "timestamp": 1.123970251069885
+        },
+        {
+            "x": 7.635308945850804,
+            "y": 7.452085548446669,
+            "heading": -3.129057152990018,
+            "angularVelocity": 0.1555765979823413,
+            "velocityX": -0.49212955006536996,
+            "velocityY": -0.1642351578439422,
+            "timestamp": 1.2147818151045664
+        },
+        {
+            "x": 7.525497368411343,
+            "y": 7.41540168270092,
+            "heading": -3.1017205952496476,
+            "angularVelocity": 0.301025073527845,
+            "velocityX": -1.2092246026888915,
+            "velocityY": -0.4039558853066335,
+            "timestamp": 1.3055933791392478
+        },
+        {
+            "x": 7.350564387959683,
+            "y": 7.356948703524257,
+            "heading": -3.061223374709373,
+            "angularVelocity": 0.44594783682873773,
+            "velocityX": -1.9263293426462433,
+            "velocityY": -0.6436733008397082,
+            "timestamp": 1.3964049431739292
+        },
+        {
+            "x": 7.110508199108376,
+            "y": 7.276727219900285,
+            "heading": -3.007665771604216,
+            "angularVelocity": 0.5897663328854458,
+            "velocityX": -2.6434539631934633,
+            "velocityY": -0.8833840103589294,
+            "timestamp": 1.4872165072086105
+        },
+        {
+            "x": 6.80532324732103,
+            "y": 7.174739191350757,
+            "heading": -2.9413637265725296,
+            "angularVelocity": 0.7301057495614385,
+            "velocityX": -3.3606397492593936,
+            "velocityY": -1.1230731419900883,
+            "timestamp": 1.578028071243292
+        },
+        {
+            "x": 6.479515641287707,
+            "y": 7.065810200947661,
+            "heading": -2.9413637193977467,
+            "angularVelocity": 7.900737542269723e-8,
+            "velocityX": -3.587732570148064,
+            "velocityY": -1.1995057189140004,
+            "timestamp": 1.6688396352779733
+        },
+        {
+            "x": 6.153708035353397,
+            "y": 6.956881210248373,
+            "heading": -2.9413637122229894,
+            "angularVelocity": 7.900708560619817e-8,
+            "velocityX": -3.5877325690577564,
+            "velocityY": -1.1995057221756227,
+            "timestamp": 1.7596511993126547
+        },
+        {
+            "x": 5.827900259692805,
+            "y": 6.847953284193167,
+            "heading": -2.9413633662901284,
+            "angularVelocity": 0.0000038093481209652,
+            "velocityX": -3.587734438051908,
+            "velocityY": -1.1994939985136122,
+            "timestamp": 1.850462763347336
+        },
+        {
+            "x": 5.5671157361716075,
+            "y": 6.7607637759552865,
+            "heading": -2.922540068712598,
+            "angularVelocity": 0.2072786409826639,
+            "velocityX": -2.8717105171937094,
+            "velocityY": -0.9601145973503112,
+            "timestamp": 1.9412743273820174
+        },
+        {
+            "x": 5.371545145894187,
+            "y": 6.69537761850095,
+            "heading": -2.9031356081339226,
+            "angularVelocity": 0.2136782995204373,
+            "velocityX": -2.1535868515901284,
+            "velocityY": -0.7200201664777647,
+            "timestamp": 2.0320858914166986
+        },
+        {
+            "x": 5.241178014496252,
+            "y": 6.651791296591565,
+            "heading": -2.888169171521839,
+            "angularVelocity": 0.1648076076208368,
+            "velocityX": -1.4355785277318724,
+            "velocityY": -0.47996444475676003,
+            "timestamp": 2.1228974554513798
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0.08995739264147623,
+            "velocityX": -0.717728135057343,
+            "velocityY": -0.23996169235946166,
+            "timestamp": 2.213709019486061
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -2.956634078664977e-32,
+            "timestamp": 2.304520583520742
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S2.1.traj
new file mode 100644
index 00000000..98b1af2d
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S2.1.traj	
@@ -0,0 +1,265 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.230255854586943,
+            "y": 6.654457619561188,
+            "heading": -2.8852614761787456,
+            "angularVelocity": -0.06226832690913654,
+            "velocityX": 0.6421052144646586,
+            "velocityY": 0.2894501464070739,
+            "timestamp": 0.08449682912507997
+        },
+        {
+            "x": 5.338767562811348,
+            "y": 6.703372857257978,
+            "heading": -2.8957843391178932,
+            "angularVelocity": -0.12453559557330285,
+            "velocityX": 1.2842104176924247,
+            "velocityY": 0.5789002759426788,
+            "timestamp": 0.16899365825015994
+        },
+        {
+            "x": 5.501535123103558,
+            "y": 6.776745710953194,
+            "heading": -2.911568425934469,
+            "angularVelocity": -0.1868009365559807,
+            "velocityX": 1.9263156023436965,
+            "velocityY": 0.8683503801853032,
+            "timestamp": 0.25349048737523994
+        },
+        {
+            "x": 5.718558531629796,
+            "y": 6.87457617714364,
+            "heading": -2.9326134806355157,
+            "angularVelocity": -0.24906324792252543,
+            "velocityX": 2.568420741623094,
+            "velocityY": 1.1578004429684545,
+            "timestamp": 0.3379873165003199
+        },
+        {
+            "x": 5.9898377739629405,
+            "y": 6.996864247325266,
+            "heading": -2.9589191023342685,
+            "angularVelocity": -0.31132081488895513,
+            "velocityX": 3.210525710160934,
+            "velocityY": 1.4472504051081343,
+            "timestamp": 0.4224841456253998
+        },
+        {
+            "x": 6.281244778996208,
+            "y": 7.128225687635927,
+            "heading": -2.958919105469154,
+            "angularVelocity": -3.710063015796767e-8,
+            "velocityX": 3.448733024074779,
+            "velocityY": 1.5546315958934869,
+            "timestamp": 0.5069809747504798
+        },
+        {
+            "x": 6.572651810300593,
+            "y": 7.259587069667809,
+            "heading": -2.9589191086040265,
+            "angularVelocity": -3.7100470021746385e-8,
+            "velocityX": 3.448733334987257,
+            "velocityY": 1.5546309061778836,
+            "timestamp": 0.5914778038755597
+        },
+        {
+            "x": 6.868681501268645,
+            "y": 7.380170200918755,
+            "heading": -2.958919113683838,
+            "angularVelocity": -6.01183729074805e-8,
+            "velocityX": 3.5034414194388437,
+            "velocityY": 1.4270728558635946,
+            "timestamp": 0.6759746330006396
+        },
+        {
+            "x": 7.138377967809157,
+            "y": 7.448808221216772,
+            "heading": -3.0028485661720943,
+            "angularVelocity": -0.5198946865003425,
+            "velocityX": 3.1917939327792095,
+            "velocityY": 0.8123147461121022,
+            "timestamp": 0.7604714621257196
+        },
+        {
+            "x": 7.3546590217483025,
+            "y": 7.491172600361784,
+            "heading": -3.044173791439457,
+            "angularVelocity": -0.4890742729077964,
+            "velocityX": 2.5596351505567982,
+            "velocityY": 0.5013724134227592,
+            "timestamp": 0.8449682912507995
+        },
+        {
+            "x": 7.517104833144657,
+            "y": 7.508147678254589,
+            "heading": -3.0815979795537904,
+            "angularVelocity": -0.4429064202981498,
+            "velocityX": 1.9225077802137127,
+            "velocityY": 0.200896034426051,
+            "timestamp": 0.9294651203758795
+        },
+        {
+            "x": 7.625575199280341,
+            "y": 7.500038616852461,
+            "heading": -3.1146766365119762,
+            "angularVelocity": -0.39147808622758395,
+            "velocityX": 1.2837211438445335,
+            "velocityY": -0.09596882493808385,
+            "timestamp": 1.0139619495009595
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.3373933786960057,
+            "velocityX": 0.6441046520111882,
+            "velocityY": -0.39100422104069327,
+            "timestamp": 1.0984587786260396
+        },
+        {
+            "x": 7.666478172550486,
+            "y": 7.390434189445638,
+            "heading": -3.1708333738054395,
+            "angularVelocity": -0.26987816293521383,
+            "velocityX": -0.13198919117872188,
+            "velocityY": -0.7473737883984474,
+            "timestamp": 1.200905254195427
+        },
+        {
+            "x": 7.573314043125627,
+            "y": 7.277653381548422,
+            "heading": -3.1915604539305784,
+            "angularVelocity": -0.2023210658047483,
+            "velocityX": -0.909393211499577,
+            "velocityY": -1.1008754305152135,
+            "timestamp": 1.3033517297648145
+        },
+        {
+            "x": 7.400248960624901,
+            "y": 7.129232012070322,
+            "heading": -3.205357963605193,
+            "angularVelocity": -0.13468017906843224,
+            "velocityX": -1.6893219755862467,
+            "velocityY": -1.4487698932851405,
+            "timestamp": 1.405798205334202
+        },
+        {
+            "x": 7.146578363694586,
+            "y": 6.946793949790035,
+            "heading": -3.2122031163802927,
+            "angularVelocity": -0.06681686936573578,
+            "velocityX": -2.476128100263464,
+            "velocityY": -1.780813456649585,
+            "timestamp": 1.5082446809035894
+        },
+        {
+            "x": 6.805429935311203,
+            "y": 6.762913771007078,
+            "heading": -3.2122031133395477,
+            "angularVelocity": 2.9681302827383053e-8,
+            "velocityX": -3.330016249825229,
+            "velocityY": -1.7948902366915849,
+            "timestamp": 1.610691156472977
+        },
+        {
+            "x": 6.4551130042311256,
+            "y": 6.597166308184442,
+            "heading": -3.2122031105786535,
+            "angularVelocity": 2.694963002504427e-8,
+            "velocityX": -3.4195117902597523,
+            "velocityY": -1.6178932647651112,
+            "timestamp": 1.7131376320423644
+        },
+        {
+            "x": 6.104796046388704,
+            "y": 6.431418901925591,
+            "heading": -3.212203107817759,
+            "angularVelocity": 2.6949630458066062e-8,
+            "velocityX": -3.419512051492193,
+            "velocityY": -1.617892712634998,
+            "timestamp": 1.8155841076117518
+        },
+        {
+            "x": 5.754479088546207,
+            "y": 6.265671495666902,
+            "heading": -3.2122031050568642,
+            "angularVelocity": 2.694963045994546e-8,
+            "velocityX": -3.4195120514929416,
+            "velocityY": -1.617892712633416,
+            "timestamp": 1.9180305831811393
+        },
+        {
+            "x": 5.404162130703709,
+            "y": 6.099924089408213,
+            "heading": -3.212203102295969,
+            "angularVelocity": 2.6949631214728657e-8,
+            "velocityX": -3.419512051492942,
+            "velocityY": -1.6178927126334155,
+            "timestamp": 2.0204770587505267
+        },
+        {
+            "x": 5.05384517285956,
+            "y": 5.9341766831530425,
+            "heading": -3.212203099535067,
+            "angularVelocity": 2.6949706103007404e-8,
+            "velocityX": -3.4195120515090505,
+            "velocityY": -1.6178927125990576,
+            "timestamp": 2.122923534319914
+        },
+        {
+            "x": 4.737507119027181,
+            "y": 5.784506016757487,
+            "heading": -3.184596232441734,
+            "angularVelocity": 0.2694760062744644,
+            "velocityX": -3.087837351887456,
+            "velocityY": -1.4609645238032911,
+            "timestamp": 2.2253700098893017
+        },
+        {
+            "x": 4.500253563523173,
+            "y": 5.67225301019093,
+            "heading": -3.163890824246715,
+            "angularVelocity": 0.20210952187413175,
+            "velocityX": -2.315878161599758,
+            "velocityY": -1.0957234589346887,
+            "timestamp": 2.327816485458689
+        },
+        {
+            "x": 4.3420845221283875,
+            "y": 5.597417670515286,
+            "heading": -3.150087142577917,
+            "angularVelocity": 0.1347404251056839,
+            "velocityX": -1.5439188172721183,
+            "velocityY": -0.7304823251333565,
+            "timestamp": 2.4302629610280766
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0.06737015949032035,
+            "velocityX": -0.771959422604275,
+            "velocityY": -0.3652411691795466,
+            "timestamp": 2.532709436597464
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": -1.9737281778417947e-31,
+            "velocityX": -1.0240473596982798e-33,
+            "velocityY": -4.498532055712875e-34,
+            "timestamp": 2.6351559121668515
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S2.traj
new file mode 100644
index 00000000..98b1af2d
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S2.traj	
@@ -0,0 +1,265 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.230255854586943,
+            "y": 6.654457619561188,
+            "heading": -2.8852614761787456,
+            "angularVelocity": -0.06226832690913654,
+            "velocityX": 0.6421052144646586,
+            "velocityY": 0.2894501464070739,
+            "timestamp": 0.08449682912507997
+        },
+        {
+            "x": 5.338767562811348,
+            "y": 6.703372857257978,
+            "heading": -2.8957843391178932,
+            "angularVelocity": -0.12453559557330285,
+            "velocityX": 1.2842104176924247,
+            "velocityY": 0.5789002759426788,
+            "timestamp": 0.16899365825015994
+        },
+        {
+            "x": 5.501535123103558,
+            "y": 6.776745710953194,
+            "heading": -2.911568425934469,
+            "angularVelocity": -0.1868009365559807,
+            "velocityX": 1.9263156023436965,
+            "velocityY": 0.8683503801853032,
+            "timestamp": 0.25349048737523994
+        },
+        {
+            "x": 5.718558531629796,
+            "y": 6.87457617714364,
+            "heading": -2.9326134806355157,
+            "angularVelocity": -0.24906324792252543,
+            "velocityX": 2.568420741623094,
+            "velocityY": 1.1578004429684545,
+            "timestamp": 0.3379873165003199
+        },
+        {
+            "x": 5.9898377739629405,
+            "y": 6.996864247325266,
+            "heading": -2.9589191023342685,
+            "angularVelocity": -0.31132081488895513,
+            "velocityX": 3.210525710160934,
+            "velocityY": 1.4472504051081343,
+            "timestamp": 0.4224841456253998
+        },
+        {
+            "x": 6.281244778996208,
+            "y": 7.128225687635927,
+            "heading": -2.958919105469154,
+            "angularVelocity": -3.710063015796767e-8,
+            "velocityX": 3.448733024074779,
+            "velocityY": 1.5546315958934869,
+            "timestamp": 0.5069809747504798
+        },
+        {
+            "x": 6.572651810300593,
+            "y": 7.259587069667809,
+            "heading": -2.9589191086040265,
+            "angularVelocity": -3.7100470021746385e-8,
+            "velocityX": 3.448733334987257,
+            "velocityY": 1.5546309061778836,
+            "timestamp": 0.5914778038755597
+        },
+        {
+            "x": 6.868681501268645,
+            "y": 7.380170200918755,
+            "heading": -2.958919113683838,
+            "angularVelocity": -6.01183729074805e-8,
+            "velocityX": 3.5034414194388437,
+            "velocityY": 1.4270728558635946,
+            "timestamp": 0.6759746330006396
+        },
+        {
+            "x": 7.138377967809157,
+            "y": 7.448808221216772,
+            "heading": -3.0028485661720943,
+            "angularVelocity": -0.5198946865003425,
+            "velocityX": 3.1917939327792095,
+            "velocityY": 0.8123147461121022,
+            "timestamp": 0.7604714621257196
+        },
+        {
+            "x": 7.3546590217483025,
+            "y": 7.491172600361784,
+            "heading": -3.044173791439457,
+            "angularVelocity": -0.4890742729077964,
+            "velocityX": 2.5596351505567982,
+            "velocityY": 0.5013724134227592,
+            "timestamp": 0.8449682912507995
+        },
+        {
+            "x": 7.517104833144657,
+            "y": 7.508147678254589,
+            "heading": -3.0815979795537904,
+            "angularVelocity": -0.4429064202981498,
+            "velocityX": 1.9225077802137127,
+            "velocityY": 0.200896034426051,
+            "timestamp": 0.9294651203758795
+        },
+        {
+            "x": 7.625575199280341,
+            "y": 7.500038616852461,
+            "heading": -3.1146766365119762,
+            "angularVelocity": -0.39147808622758395,
+            "velocityX": 1.2837211438445335,
+            "velocityY": -0.09596882493808385,
+            "timestamp": 1.0139619495009595
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.3373933786960057,
+            "velocityX": 0.6441046520111882,
+            "velocityY": -0.39100422104069327,
+            "timestamp": 1.0984587786260396
+        },
+        {
+            "x": 7.666478172550486,
+            "y": 7.390434189445638,
+            "heading": -3.1708333738054395,
+            "angularVelocity": -0.26987816293521383,
+            "velocityX": -0.13198919117872188,
+            "velocityY": -0.7473737883984474,
+            "timestamp": 1.200905254195427
+        },
+        {
+            "x": 7.573314043125627,
+            "y": 7.277653381548422,
+            "heading": -3.1915604539305784,
+            "angularVelocity": -0.2023210658047483,
+            "velocityX": -0.909393211499577,
+            "velocityY": -1.1008754305152135,
+            "timestamp": 1.3033517297648145
+        },
+        {
+            "x": 7.400248960624901,
+            "y": 7.129232012070322,
+            "heading": -3.205357963605193,
+            "angularVelocity": -0.13468017906843224,
+            "velocityX": -1.6893219755862467,
+            "velocityY": -1.4487698932851405,
+            "timestamp": 1.405798205334202
+        },
+        {
+            "x": 7.146578363694586,
+            "y": 6.946793949790035,
+            "heading": -3.2122031163802927,
+            "angularVelocity": -0.06681686936573578,
+            "velocityX": -2.476128100263464,
+            "velocityY": -1.780813456649585,
+            "timestamp": 1.5082446809035894
+        },
+        {
+            "x": 6.805429935311203,
+            "y": 6.762913771007078,
+            "heading": -3.2122031133395477,
+            "angularVelocity": 2.9681302827383053e-8,
+            "velocityX": -3.330016249825229,
+            "velocityY": -1.7948902366915849,
+            "timestamp": 1.610691156472977
+        },
+        {
+            "x": 6.4551130042311256,
+            "y": 6.597166308184442,
+            "heading": -3.2122031105786535,
+            "angularVelocity": 2.694963002504427e-8,
+            "velocityX": -3.4195117902597523,
+            "velocityY": -1.6178932647651112,
+            "timestamp": 1.7131376320423644
+        },
+        {
+            "x": 6.104796046388704,
+            "y": 6.431418901925591,
+            "heading": -3.212203107817759,
+            "angularVelocity": 2.6949630458066062e-8,
+            "velocityX": -3.419512051492193,
+            "velocityY": -1.617892712634998,
+            "timestamp": 1.8155841076117518
+        },
+        {
+            "x": 5.754479088546207,
+            "y": 6.265671495666902,
+            "heading": -3.2122031050568642,
+            "angularVelocity": 2.694963045994546e-8,
+            "velocityX": -3.4195120514929416,
+            "velocityY": -1.617892712633416,
+            "timestamp": 1.9180305831811393
+        },
+        {
+            "x": 5.404162130703709,
+            "y": 6.099924089408213,
+            "heading": -3.212203102295969,
+            "angularVelocity": 2.6949631214728657e-8,
+            "velocityX": -3.419512051492942,
+            "velocityY": -1.6178927126334155,
+            "timestamp": 2.0204770587505267
+        },
+        {
+            "x": 5.05384517285956,
+            "y": 5.9341766831530425,
+            "heading": -3.212203099535067,
+            "angularVelocity": 2.6949706103007404e-8,
+            "velocityX": -3.4195120515090505,
+            "velocityY": -1.6178927125990576,
+            "timestamp": 2.122923534319914
+        },
+        {
+            "x": 4.737507119027181,
+            "y": 5.784506016757487,
+            "heading": -3.184596232441734,
+            "angularVelocity": 0.2694760062744644,
+            "velocityX": -3.087837351887456,
+            "velocityY": -1.4609645238032911,
+            "timestamp": 2.2253700098893017
+        },
+        {
+            "x": 4.500253563523173,
+            "y": 5.67225301019093,
+            "heading": -3.163890824246715,
+            "angularVelocity": 0.20210952187413175,
+            "velocityX": -2.315878161599758,
+            "velocityY": -1.0957234589346887,
+            "timestamp": 2.327816485458689
+        },
+        {
+            "x": 4.3420845221283875,
+            "y": 5.597417670515286,
+            "heading": -3.150087142577917,
+            "angularVelocity": 0.1347404251056839,
+            "velocityX": -1.5439188172721183,
+            "velocityY": -0.7304823251333565,
+            "timestamp": 2.4302629610280766
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0.06737015949032035,
+            "velocityX": -0.771959422604275,
+            "velocityY": -0.3652411691795466,
+            "timestamp": 2.532709436597464
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": -1.9737281778417947e-31,
+            "velocityX": -1.0240473596982798e-33,
+            "velocityY": -4.498532055712875e-34,
+            "timestamp": 2.6351559121668515
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S1.1.traj
new file mode 100644
index 00000000..50078b64
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S1.1.traj	
@@ -0,0 +1,247 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 1.4108683409597047e-30,
+            "velocityX": 3.332067657020877e-32,
+            "velocityY": 1.6252866310843352e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 5.234960531442942,
+            "y": 6.610392010976446,
+            "heading": -2.8906900859192084,
+            "angularVelocity": -0.12376379247862908,
+            "velocityX": 0.6826118174441478,
+            "velocityY": -0.22701025069196354,
+            "timestamp": 0.08637490581940685
+        },
+        {
+            "x": 5.352887916547719,
+            "y": 6.571175678219841,
+            "heading": -2.91160851046633,
+            "angularVelocity": -0.24218173494536643,
+            "velocityX": 1.3652968299769979,
+            "velocityY": -0.45402460801062294,
+            "timestamp": 0.1727498116388137
+        },
+        {
+            "x": 5.529792169405666,
+            "y": 6.5123505926970395,
+            "heading": -2.941981384820343,
+            "angularVelocity": -0.35164002861494675,
+            "velocityX": 2.0480977800178555,
+            "velocityY": -0.6810437008846502,
+            "timestamp": 0.2591247174582205
+        },
+        {
+            "x": 5.765691321896416,
+            "y": 6.43391666324999,
+            "heading": -2.9802445174117826,
+            "angularVelocity": -0.4429889934853137,
+            "velocityX": 2.7311074930027868,
+            "velocityY": -0.9080638491352222,
+            "timestamp": 0.3454996232776274
+        },
+        {
+            "x": 6.060622443246375,
+            "y": 6.335879897503487,
+            "heading": -3.0215937804128985,
+            "angularVelocity": -0.47871847278597873,
+            "velocityX": 3.4145463726079974,
+            "velocityY": -1.135014444490357,
+            "timestamp": 0.4318745290970343
+        },
+        {
+            "x": 6.370665856130874,
+            "y": 6.2327325092534815,
+            "heading": -3.021593787446907,
+            "angularVelocity": -8.143578577450647e-8,
+            "velocityX": 3.5895079704369173,
+            "velocityY": -1.1941823527503022,
+            "timestamp": 0.5182494349164412
+        },
+        {
+            "x": 6.680709268872309,
+            "y": 6.129585120573416,
+            "heading": -3.021593794480898,
+            "angularVelocity": -8.143559184955701e-8,
+            "velocityX": 3.589507968780596,
+            "velocityY": -1.1941823577293083,
+            "timestamp": 0.604624340735848
+        },
+        {
+            "x": 6.990752683844134,
+            "y": 6.026437738598777,
+            "heading": -3.0215938015153188,
+            "angularVelocity": -8.144055599607105e-8,
+            "velocityX": 3.5895079946028012,
+            "velocityY": -1.1941822800976392,
+            "timestamp": 0.6909992465552549
+        },
+        {
+            "x": 7.246471127975024,
+            "y": 5.941338278881442,
+            "heading": -3.0707797962131482,
+            "angularVelocity": -0.5694477375264861,
+            "velocityX": 2.96056408634992,
+            "velocityY": -0.9852336035564434,
+            "timestamp": 0.7773741523746618
+        },
+        {
+            "x": 7.443255050635665,
+            "y": 5.875844773804915,
+            "heading": -3.107535505632415,
+            "angularVelocity": -0.4255368972118188,
+            "velocityX": 2.2782533977168096,
+            "velocityY": -0.7582469058018522,
+            "timestamp": 0.8637490581940687
+        },
+        {
+            "x": 7.581104494826472,
+            "y": 5.829957223632905,
+            "heading": -3.1318559851891097,
+            "angularVelocity": -0.2815688112890415,
+            "velocityX": 1.5959432069182882,
+            "velocityY": -0.531260204994182,
+            "timestamp": 0.9501239640134755
+        },
+        {
+            "x": 7.660019481037545,
+            "y": 5.803675631519179,
+            "heading": -3.1437391060728066,
+            "angularVelocity": -0.13757607919844067,
+            "velocityX": 0.9136332533425802,
+            "velocityY": -0.3042734676739749,
+            "timestamp": 1.0364988698328823
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0.0064115716005977265,
+            "velocityX": 0.2313231924569808,
+            "velocityY": -0.07728670099033955,
+            "timestamp": 1.122873775652289
+        },
+        {
+            "x": 7.635819311328683,
+            "y": 5.811667633489592,
+            "heading": -3.128848165121335,
+            "angularVelocity": 0.15781563504855214,
+            "velocityX": -0.48631752487124336,
+            "velocityY": 0.1614535089628058,
+            "timestamp": 1.2137211879121816
+        },
+        {
+            "x": 7.526434828772679,
+            "y": 5.848024979672397,
+            "heading": -3.1012666400693027,
+            "angularVelocity": 0.30360275946215987,
+            "velocityX": -1.2040462114976394,
+            "velocityY": 0.4002023313416631,
+            "timestamp": 1.3045686001720742
+        },
+        {
+            "x": 7.351833825159198,
+            "y": 5.906073128317211,
+            "heading": -3.0612921137460094,
+            "angularVelocity": 0.44001832665334883,
+            "velocityX": -1.921914992073542,
+            "velocityY": 0.6389631493158797,
+            "timestamp": 1.3954160124319668
+        },
+        {
+            "x": 7.111993081438883,
+            "y": 5.98581338993347,
+            "heading": -3.010630767411968,
+            "angularVelocity": 0.5576531579045794,
+            "velocityX": -2.640039355596495,
+            "velocityY": 0.8777383926810057,
+            "timestamp": 1.4862634246918593
+        },
+        {
+            "x": 6.806861000464256,
+            "y": 6.08724274158431,
+            "heading": -2.954485863102405,
+            "angularVelocity": 0.6180132478540051,
+            "velocityX": -3.358731673078681,
+            "velocityY": 1.116480361164798,
+            "timestamp": 1.577110836951752
+        },
+        {
+            "x": 6.480775270198975,
+            "y": 6.195766521383185,
+            "heading": -2.9544858545944934,
+            "angularVelocity": 9.365056825417393e-8,
+            "velocityX": -3.5893783009738223,
+            "velocityY": 1.1945720532843969,
+            "timestamp": 1.6679582492116445
+        },
+        {
+            "x": 6.154689540144831,
+            "y": 6.30429030181652,
+            "heading": -2.954485846086605,
+            "angularVelocity": 9.365030974191779e-8,
+            "velocityX": -3.589378298649725,
+            "velocityY": 1.1945720602682006,
+            "timestamp": 1.758805661471537
+        },
+        {
+            "x": 5.828602321664721,
+            "y": 6.412809153986278,
+            "heading": -2.954485580357569,
+            "angularVelocity": 0.0000029250024503863607,
+            "velocityX": -3.5893946824518035,
+            "velocityY": 1.194517812563883,
+            "timestamp": 1.8496530737314296
+        },
+        {
+            "x": 5.567525835725181,
+            "y": 6.4996975117043,
+            "heading": -2.9315080017864195,
+            "angularVelocity": 0.25292496505229944,
+            "velocityX": -2.8737911124282145,
+            "velocityY": 0.9564208330939352,
+            "timestamp": 1.9405004859913222
+        },
+        {
+            "x": 5.371744680904255,
+            "y": 6.564854924968385,
+            "heading": -2.907956180338786,
+            "angularVelocity": 0.2592459252452843,
+            "velocityX": -2.155054832611796,
+            "velocityY": 0.7172181534207646,
+            "timestamp": 2.0313478982512145
+        },
+        {
+            "x": 5.241242968677622,
+            "y": 6.608286759589549,
+            "heading": -2.8898557615846676,
+            "angularVelocity": 0.19923978354186256,
+            "velocityX": -1.4364934452213165,
+            "velocityY": 0.478074537741632,
+            "timestamp": 2.122195310511107
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0.10848698206630299,
+            "velocityX": -0.7181599019131975,
+            "velocityY": 0.2390078029777863,
+            "timestamp": 2.2130427227709997
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -3.247565811207596e-32,
+            "timestamp": 2.3038901350308922
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S1.traj
new file mode 100644
index 00000000..50078b64
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S1.traj	
@@ -0,0 +1,247 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 1.4108683409597047e-30,
+            "velocityX": 3.332067657020877e-32,
+            "velocityY": 1.6252866310843352e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 5.234960531442942,
+            "y": 6.610392010976446,
+            "heading": -2.8906900859192084,
+            "angularVelocity": -0.12376379247862908,
+            "velocityX": 0.6826118174441478,
+            "velocityY": -0.22701025069196354,
+            "timestamp": 0.08637490581940685
+        },
+        {
+            "x": 5.352887916547719,
+            "y": 6.571175678219841,
+            "heading": -2.91160851046633,
+            "angularVelocity": -0.24218173494536643,
+            "velocityX": 1.3652968299769979,
+            "velocityY": -0.45402460801062294,
+            "timestamp": 0.1727498116388137
+        },
+        {
+            "x": 5.529792169405666,
+            "y": 6.5123505926970395,
+            "heading": -2.941981384820343,
+            "angularVelocity": -0.35164002861494675,
+            "velocityX": 2.0480977800178555,
+            "velocityY": -0.6810437008846502,
+            "timestamp": 0.2591247174582205
+        },
+        {
+            "x": 5.765691321896416,
+            "y": 6.43391666324999,
+            "heading": -2.9802445174117826,
+            "angularVelocity": -0.4429889934853137,
+            "velocityX": 2.7311074930027868,
+            "velocityY": -0.9080638491352222,
+            "timestamp": 0.3454996232776274
+        },
+        {
+            "x": 6.060622443246375,
+            "y": 6.335879897503487,
+            "heading": -3.0215937804128985,
+            "angularVelocity": -0.47871847278597873,
+            "velocityX": 3.4145463726079974,
+            "velocityY": -1.135014444490357,
+            "timestamp": 0.4318745290970343
+        },
+        {
+            "x": 6.370665856130874,
+            "y": 6.2327325092534815,
+            "heading": -3.021593787446907,
+            "angularVelocity": -8.143578577450647e-8,
+            "velocityX": 3.5895079704369173,
+            "velocityY": -1.1941823527503022,
+            "timestamp": 0.5182494349164412
+        },
+        {
+            "x": 6.680709268872309,
+            "y": 6.129585120573416,
+            "heading": -3.021593794480898,
+            "angularVelocity": -8.143559184955701e-8,
+            "velocityX": 3.589507968780596,
+            "velocityY": -1.1941823577293083,
+            "timestamp": 0.604624340735848
+        },
+        {
+            "x": 6.990752683844134,
+            "y": 6.026437738598777,
+            "heading": -3.0215938015153188,
+            "angularVelocity": -8.144055599607105e-8,
+            "velocityX": 3.5895079946028012,
+            "velocityY": -1.1941822800976392,
+            "timestamp": 0.6909992465552549
+        },
+        {
+            "x": 7.246471127975024,
+            "y": 5.941338278881442,
+            "heading": -3.0707797962131482,
+            "angularVelocity": -0.5694477375264861,
+            "velocityX": 2.96056408634992,
+            "velocityY": -0.9852336035564434,
+            "timestamp": 0.7773741523746618
+        },
+        {
+            "x": 7.443255050635665,
+            "y": 5.875844773804915,
+            "heading": -3.107535505632415,
+            "angularVelocity": -0.4255368972118188,
+            "velocityX": 2.2782533977168096,
+            "velocityY": -0.7582469058018522,
+            "timestamp": 0.8637490581940687
+        },
+        {
+            "x": 7.581104494826472,
+            "y": 5.829957223632905,
+            "heading": -3.1318559851891097,
+            "angularVelocity": -0.2815688112890415,
+            "velocityX": 1.5959432069182882,
+            "velocityY": -0.531260204994182,
+            "timestamp": 0.9501239640134755
+        },
+        {
+            "x": 7.660019481037545,
+            "y": 5.803675631519179,
+            "heading": -3.1437391060728066,
+            "angularVelocity": -0.13757607919844067,
+            "velocityX": 0.9136332533425802,
+            "velocityY": -0.3042734676739749,
+            "timestamp": 1.0364988698328823
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0.0064115716005977265,
+            "velocityX": 0.2313231924569808,
+            "velocityY": -0.07728670099033955,
+            "timestamp": 1.122873775652289
+        },
+        {
+            "x": 7.635819311328683,
+            "y": 5.811667633489592,
+            "heading": -3.128848165121335,
+            "angularVelocity": 0.15781563504855214,
+            "velocityX": -0.48631752487124336,
+            "velocityY": 0.1614535089628058,
+            "timestamp": 1.2137211879121816
+        },
+        {
+            "x": 7.526434828772679,
+            "y": 5.848024979672397,
+            "heading": -3.1012666400693027,
+            "angularVelocity": 0.30360275946215987,
+            "velocityX": -1.2040462114976394,
+            "velocityY": 0.4002023313416631,
+            "timestamp": 1.3045686001720742
+        },
+        {
+            "x": 7.351833825159198,
+            "y": 5.906073128317211,
+            "heading": -3.0612921137460094,
+            "angularVelocity": 0.44001832665334883,
+            "velocityX": -1.921914992073542,
+            "velocityY": 0.6389631493158797,
+            "timestamp": 1.3954160124319668
+        },
+        {
+            "x": 7.111993081438883,
+            "y": 5.98581338993347,
+            "heading": -3.010630767411968,
+            "angularVelocity": 0.5576531579045794,
+            "velocityX": -2.640039355596495,
+            "velocityY": 0.8777383926810057,
+            "timestamp": 1.4862634246918593
+        },
+        {
+            "x": 6.806861000464256,
+            "y": 6.08724274158431,
+            "heading": -2.954485863102405,
+            "angularVelocity": 0.6180132478540051,
+            "velocityX": -3.358731673078681,
+            "velocityY": 1.116480361164798,
+            "timestamp": 1.577110836951752
+        },
+        {
+            "x": 6.480775270198975,
+            "y": 6.195766521383185,
+            "heading": -2.9544858545944934,
+            "angularVelocity": 9.365056825417393e-8,
+            "velocityX": -3.5893783009738223,
+            "velocityY": 1.1945720532843969,
+            "timestamp": 1.6679582492116445
+        },
+        {
+            "x": 6.154689540144831,
+            "y": 6.30429030181652,
+            "heading": -2.954485846086605,
+            "angularVelocity": 9.365030974191779e-8,
+            "velocityX": -3.589378298649725,
+            "velocityY": 1.1945720602682006,
+            "timestamp": 1.758805661471537
+        },
+        {
+            "x": 5.828602321664721,
+            "y": 6.412809153986278,
+            "heading": -2.954485580357569,
+            "angularVelocity": 0.0000029250024503863607,
+            "velocityX": -3.5893946824518035,
+            "velocityY": 1.194517812563883,
+            "timestamp": 1.8496530737314296
+        },
+        {
+            "x": 5.567525835725181,
+            "y": 6.4996975117043,
+            "heading": -2.9315080017864195,
+            "angularVelocity": 0.25292496505229944,
+            "velocityX": -2.8737911124282145,
+            "velocityY": 0.9564208330939352,
+            "timestamp": 1.9405004859913222
+        },
+        {
+            "x": 5.371744680904255,
+            "y": 6.564854924968385,
+            "heading": -2.907956180338786,
+            "angularVelocity": 0.2592459252452843,
+            "velocityX": -2.155054832611796,
+            "velocityY": 0.7172181534207646,
+            "timestamp": 2.0313478982512145
+        },
+        {
+            "x": 5.241242968677622,
+            "y": 6.608286759589549,
+            "heading": -2.8898557615846676,
+            "angularVelocity": 0.19923978354186256,
+            "velocityX": -1.4364934452213165,
+            "velocityY": 0.478074537741632,
+            "timestamp": 2.122195310511107
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0.10848698206630299,
+            "velocityX": -0.7181599019131975,
+            "velocityY": 0.2390078029777863,
+            "timestamp": 2.2130427227709997
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -3.247565811207596e-32,
+            "timestamp": 2.3038901350308922
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S2.1.traj
new file mode 100644
index 00000000..526be462
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S2.1.traj	
@@ -0,0 +1,256 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.2343405973213555,
+            "y": 6.622412668725056,
+            "heading": -2.8862658883257666,
+            "angularVelocity": -0.07457855479400594,
+            "velocityX": 0.6943879634997097,
+            "velocityY": -0.0903067804291561,
+            "timestamp": 0.08401729348434916
+        },
+        {
+            "x": 5.351021788709109,
+            "y": 6.607238006430742,
+            "heading": -2.898797916255787,
+            "angularVelocity": -0.14916010038283942,
+            "velocityX": 1.3887758882579295,
+            "velocityY": -0.18061355781642813,
+            "timestamp": 0.16803458696869833
+        },
+        {
+            "x": 5.52604356848206,
+            "y": 6.584476013331715,
+            "heading": -2.917596514461126,
+            "angularVelocity": -0.22374677195285786,
+            "velocityX": 2.083163745396705,
+            "velocityY": -0.27092033264873905,
+            "timestamp": 0.2520518804530475
+        },
+        {
+            "x": 5.759405926747038,
+            "y": 6.554126689512535,
+            "heading": -2.9426622755752048,
+            "angularVelocity": -0.2983404972304698,
+            "velocityX": 2.7775514847838854,
+            "velocityY": -0.3612271064746121,
+            "timestamp": 0.33606917393739666
+        },
+        {
+            "x": 6.051108840456095,
+            "y": 6.516190035692408,
+            "heading": -2.97399589297024,
+            "angularVelocity": -0.37294247524019475,
+            "velocityX": 3.4719389498472166,
+            "velocityY": -0.45153387174027243,
+            "timestamp": 0.4200864674217458
+        },
+        {
+            "x": 6.366286997596586,
+            "y": 6.47520006127383,
+            "heading": -2.973995896754836,
+            "angularVelocity": -4.504543806754251e-8,
+            "velocityX": 3.751348610143015,
+            "velocityY": -0.4878754446691907,
+            "timestamp": 0.504103760906095
+        },
+        {
+            "x": 6.681465049565767,
+            "y": 6.434209278184785,
+            "heading": -2.973995900539415,
+            "angularVelocity": -4.5045241068948784e-8,
+            "velocityX": 3.751347358361316,
+            "velocityY": -0.48788506971698453,
+            "timestamp": 0.5881210543904442
+        },
+        {
+            "x": 6.987163031152994,
+            "y": 6.347225437890881,
+            "heading": -2.9739959119301913,
+            "angularVelocity": -1.3557656915583158e-7,
+            "velocityX": 3.6385126074570944,
+            "velocityY": -1.0353087642618135,
+            "timestamp": 0.6721383478747933
+        },
+        {
+            "x": 7.243185278067537,
+            "y": 6.230387598842041,
+            "heading": -3.0107958850128846,
+            "angularVelocity": -0.4380047435061505,
+            "velocityX": 3.047256538467705,
+            "velocityY": -1.3906403575188384,
+            "timestamp": 0.7561556413591425
+        },
+        {
+            "x": 7.4403635227243035,
+            "y": 6.1148085109072055,
+            "heading": -3.048556145231695,
+            "angularVelocity": -0.44943438014751774,
+            "velocityX": 2.3468768925946972,
+            "velocityY": -1.3756583096355752,
+            "timestamp": 0.8401729348434916
+        },
+        {
+            "x": 7.578836794258849,
+            "y": 6.003419207644454,
+            "heading": -3.0839307394839546,
+            "angularVelocity": -0.42103944063431625,
+            "velocityX": 1.648152014803231,
+            "velocityY": -1.3257901872726057,
+            "timestamp": 0.9241902283278408
+        },
+        {
+            "x": 7.658696859900876,
+            "y": 5.8972967644784475,
+            "heading": -3.115687244057696,
+            "angularVelocity": -0.37797581017838017,
+            "velocityX": 0.9505193791669008,
+            "velocityY": -1.2631023776762744,
+            "timestamp": 1.00820752181219
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.32729051343474413,
+            "velocityX": 0.25355660978405975,
+            "velocityY": -1.1937633351296981,
+            "timestamp": 1.092224815296539
+        },
+        {
+            "x": 7.6231583877847156,
+            "y": 5.686873944343296,
+            "heading": -3.169227404734073,
+            "angularVelocity": -0.2617377186372048,
+            "velocityX": -0.5712901533281343,
+            "velocityY": -1.1068287610009868,
+            "timestamp": 1.1917217379796003
+        },
+        {
+            "x": 7.484327538641781,
+            "y": 5.5861293490761845,
+            "heading": -3.18873884204171,
+            "angularVelocity": -0.1961009122843807,
+            "velocityX": -1.3953280704486528,
+            "velocityY": -1.0125398107841346,
+            "timestamp": 1.2912186606626617
+        },
+        {
+            "x": 7.263675847711948,
+            "y": 5.496135627515027,
+            "heading": -3.2017054758509116,
+            "angularVelocity": -0.13032195830322837,
+            "velocityX": -2.2176735217500094,
+            "velocityY": -0.9044874869931846,
+            "timestamp": 1.390715583345723
+        },
+        {
+            "x": 6.9617360146088005,
+            "y": 5.420370006770522,
+            "heading": -3.2080997246142733,
+            "angularVelocity": -0.0642657942671267,
+            "velocityX": -3.034665042505381,
+            "velocityY": -0.7614870761968194,
+            "timestamp": 1.4902125060287843
+        },
+        {
+            "x": 6.587133153221649,
+            "y": 5.383725358359533,
+            "heading": -3.208099721013608,
+            "angularVelocity": 3.618871368220605e-8,
+            "velocityX": -3.7649693205127126,
+            "velocityY": -0.3682993144191623,
+            "timestamp": 1.5897094287118456
+        },
+        {
+            "x": 6.211820115708755,
+            "y": 5.412190570741109,
+            "heading": -3.2080997182657316,
+            "angularVelocity": 2.761770262617581e-8,
+            "velocityX": -3.7721069897651076,
+            "velocityY": 0.2860913846777879,
+            "timestamp": 1.689206351394907
+        },
+        {
+            "x": 5.836507126679018,
+            "y": 5.440656422364601,
+            "heading": -3.208099715517856,
+            "angularVelocity": 2.761769551512973e-8,
+            "velocityX": -3.7721065024821283,
+            "velocityY": 0.28609780941835966,
+            "timestamp": 1.7887032740779683
+        },
+        {
+            "x": 5.461194137649431,
+            "y": 5.46912227399009,
+            "heading": -3.20809971276998,
+            "angularVelocity": 2.7617694565057146e-8,
+            "velocityX": -3.772106502480606,
+            "velocityY": 0.28609780943843166,
+            "timestamp": 1.8882001967610296
+        },
+        {
+            "x": 5.08588114861977,
+            "y": 5.497588125614274,
+            "heading": -3.2080997100220925,
+            "angularVelocity": 2.7617811191359224e-8,
+            "velocityX": -3.7721065024813654,
+            "velocityY": 0.2860978094253166,
+            "timestamp": 1.9876971194440909
+        },
+        {
+            "x": 4.756728707190495,
+            "y": 5.522552875223879,
+            "heading": -3.182133930988408,
+            "angularVelocity": 0.26097067460464174,
+            "velocityX": -3.3081670523395013,
+            "velocityY": 0.2509097662158744,
+            "timestamp": 2.087194042127152
+        },
+        {
+            "x": 4.509864358241751,
+            "y": 5.541276437563115,
+            "heading": -3.1626596163193277,
+            "angularVelocity": 0.19572780890032443,
+            "velocityX": -2.4811254689263977,
+            "velocityY": 0.18818232598889303,
+            "timestamp": 2.1866909648102135
+        },
+        {
+            "x": 4.3452881204218,
+            "y": 5.553758812444608,
+            "heading": -3.1496767469198765,
+            "angularVelocity": 0.13048513511123086,
+            "velocityX": -1.654083698087773,
+            "velocityY": 0.12545488387870263,
+            "timestamp": 2.286187887493275
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0.06524261821612926,
+            "velocityX": -0.8270418642385756,
+            "velocityY": 0.06272744309160665,
+            "timestamp": 2.385684810176336
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": -3.6755949365770895e-31,
+            "velocityX": -3.301266761312006e-32,
+            "velocityY": -7.495504571617436e-31,
+            "timestamp": 2.4851817328593975
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S2.traj
new file mode 100644
index 00000000..526be462
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S2.traj	
@@ -0,0 +1,256 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.2343405973213555,
+            "y": 6.622412668725056,
+            "heading": -2.8862658883257666,
+            "angularVelocity": -0.07457855479400594,
+            "velocityX": 0.6943879634997097,
+            "velocityY": -0.0903067804291561,
+            "timestamp": 0.08401729348434916
+        },
+        {
+            "x": 5.351021788709109,
+            "y": 6.607238006430742,
+            "heading": -2.898797916255787,
+            "angularVelocity": -0.14916010038283942,
+            "velocityX": 1.3887758882579295,
+            "velocityY": -0.18061355781642813,
+            "timestamp": 0.16803458696869833
+        },
+        {
+            "x": 5.52604356848206,
+            "y": 6.584476013331715,
+            "heading": -2.917596514461126,
+            "angularVelocity": -0.22374677195285786,
+            "velocityX": 2.083163745396705,
+            "velocityY": -0.27092033264873905,
+            "timestamp": 0.2520518804530475
+        },
+        {
+            "x": 5.759405926747038,
+            "y": 6.554126689512535,
+            "heading": -2.9426622755752048,
+            "angularVelocity": -0.2983404972304698,
+            "velocityX": 2.7775514847838854,
+            "velocityY": -0.3612271064746121,
+            "timestamp": 0.33606917393739666
+        },
+        {
+            "x": 6.051108840456095,
+            "y": 6.516190035692408,
+            "heading": -2.97399589297024,
+            "angularVelocity": -0.37294247524019475,
+            "velocityX": 3.4719389498472166,
+            "velocityY": -0.45153387174027243,
+            "timestamp": 0.4200864674217458
+        },
+        {
+            "x": 6.366286997596586,
+            "y": 6.47520006127383,
+            "heading": -2.973995896754836,
+            "angularVelocity": -4.504543806754251e-8,
+            "velocityX": 3.751348610143015,
+            "velocityY": -0.4878754446691907,
+            "timestamp": 0.504103760906095
+        },
+        {
+            "x": 6.681465049565767,
+            "y": 6.434209278184785,
+            "heading": -2.973995900539415,
+            "angularVelocity": -4.5045241068948784e-8,
+            "velocityX": 3.751347358361316,
+            "velocityY": -0.48788506971698453,
+            "timestamp": 0.5881210543904442
+        },
+        {
+            "x": 6.987163031152994,
+            "y": 6.347225437890881,
+            "heading": -2.9739959119301913,
+            "angularVelocity": -1.3557656915583158e-7,
+            "velocityX": 3.6385126074570944,
+            "velocityY": -1.0353087642618135,
+            "timestamp": 0.6721383478747933
+        },
+        {
+            "x": 7.243185278067537,
+            "y": 6.230387598842041,
+            "heading": -3.0107958850128846,
+            "angularVelocity": -0.4380047435061505,
+            "velocityX": 3.047256538467705,
+            "velocityY": -1.3906403575188384,
+            "timestamp": 0.7561556413591425
+        },
+        {
+            "x": 7.4403635227243035,
+            "y": 6.1148085109072055,
+            "heading": -3.048556145231695,
+            "angularVelocity": -0.44943438014751774,
+            "velocityX": 2.3468768925946972,
+            "velocityY": -1.3756583096355752,
+            "timestamp": 0.8401729348434916
+        },
+        {
+            "x": 7.578836794258849,
+            "y": 6.003419207644454,
+            "heading": -3.0839307394839546,
+            "angularVelocity": -0.42103944063431625,
+            "velocityX": 1.648152014803231,
+            "velocityY": -1.3257901872726057,
+            "timestamp": 0.9241902283278408
+        },
+        {
+            "x": 7.658696859900876,
+            "y": 5.8972967644784475,
+            "heading": -3.115687244057696,
+            "angularVelocity": -0.37797581017838017,
+            "velocityX": 0.9505193791669008,
+            "velocityY": -1.2631023776762744,
+            "timestamp": 1.00820752181219
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.32729051343474413,
+            "velocityX": 0.25355660978405975,
+            "velocityY": -1.1937633351296981,
+            "timestamp": 1.092224815296539
+        },
+        {
+            "x": 7.6231583877847156,
+            "y": 5.686873944343296,
+            "heading": -3.169227404734073,
+            "angularVelocity": -0.2617377186372048,
+            "velocityX": -0.5712901533281343,
+            "velocityY": -1.1068287610009868,
+            "timestamp": 1.1917217379796003
+        },
+        {
+            "x": 7.484327538641781,
+            "y": 5.5861293490761845,
+            "heading": -3.18873884204171,
+            "angularVelocity": -0.1961009122843807,
+            "velocityX": -1.3953280704486528,
+            "velocityY": -1.0125398107841346,
+            "timestamp": 1.2912186606626617
+        },
+        {
+            "x": 7.263675847711948,
+            "y": 5.496135627515027,
+            "heading": -3.2017054758509116,
+            "angularVelocity": -0.13032195830322837,
+            "velocityX": -2.2176735217500094,
+            "velocityY": -0.9044874869931846,
+            "timestamp": 1.390715583345723
+        },
+        {
+            "x": 6.9617360146088005,
+            "y": 5.420370006770522,
+            "heading": -3.2080997246142733,
+            "angularVelocity": -0.0642657942671267,
+            "velocityX": -3.034665042505381,
+            "velocityY": -0.7614870761968194,
+            "timestamp": 1.4902125060287843
+        },
+        {
+            "x": 6.587133153221649,
+            "y": 5.383725358359533,
+            "heading": -3.208099721013608,
+            "angularVelocity": 3.618871368220605e-8,
+            "velocityX": -3.7649693205127126,
+            "velocityY": -0.3682993144191623,
+            "timestamp": 1.5897094287118456
+        },
+        {
+            "x": 6.211820115708755,
+            "y": 5.412190570741109,
+            "heading": -3.2080997182657316,
+            "angularVelocity": 2.761770262617581e-8,
+            "velocityX": -3.7721069897651076,
+            "velocityY": 0.2860913846777879,
+            "timestamp": 1.689206351394907
+        },
+        {
+            "x": 5.836507126679018,
+            "y": 5.440656422364601,
+            "heading": -3.208099715517856,
+            "angularVelocity": 2.761769551512973e-8,
+            "velocityX": -3.7721065024821283,
+            "velocityY": 0.28609780941835966,
+            "timestamp": 1.7887032740779683
+        },
+        {
+            "x": 5.461194137649431,
+            "y": 5.46912227399009,
+            "heading": -3.20809971276998,
+            "angularVelocity": 2.7617694565057146e-8,
+            "velocityX": -3.772106502480606,
+            "velocityY": 0.28609780943843166,
+            "timestamp": 1.8882001967610296
+        },
+        {
+            "x": 5.08588114861977,
+            "y": 5.497588125614274,
+            "heading": -3.2080997100220925,
+            "angularVelocity": 2.7617811191359224e-8,
+            "velocityX": -3.7721065024813654,
+            "velocityY": 0.2860978094253166,
+            "timestamp": 1.9876971194440909
+        },
+        {
+            "x": 4.756728707190495,
+            "y": 5.522552875223879,
+            "heading": -3.182133930988408,
+            "angularVelocity": 0.26097067460464174,
+            "velocityX": -3.3081670523395013,
+            "velocityY": 0.2509097662158744,
+            "timestamp": 2.087194042127152
+        },
+        {
+            "x": 4.509864358241751,
+            "y": 5.541276437563115,
+            "heading": -3.1626596163193277,
+            "angularVelocity": 0.19572780890032443,
+            "velocityX": -2.4811254689263977,
+            "velocityY": 0.18818232598889303,
+            "timestamp": 2.1866909648102135
+        },
+        {
+            "x": 4.3452881204218,
+            "y": 5.553758812444608,
+            "heading": -3.1496767469198765,
+            "angularVelocity": 0.13048513511123086,
+            "velocityX": -1.654083698087773,
+            "velocityY": 0.12545488387870263,
+            "timestamp": 2.286187887493275
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0.06524261821612926,
+            "velocityX": -0.8270418642385756,
+            "velocityY": 0.06272744309160665,
+            "timestamp": 2.385684810176336
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": -3.6755949365770895e-31,
+            "velocityX": -3.301266761312006e-32,
+            "velocityY": -7.495504571617436e-31,
+            "timestamp": 2.4851817328593975
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S1.1.traj
new file mode 100644
index 00000000..41e8c0c3
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S1.1.traj	
@@ -0,0 +1,301 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.218507748835192,
+            "y": 6.587508953337151,
+            "heading": -2.890727584630965,
+            "angularVelocity": -0.12627463531080194,
+            "velocityX": 0.5003596491390202,
+            "velocityY": -0.5001630474999694,
+            "timestamp": 0.08495439012388621
+        },
+        {
+            "x": 5.303524107208959,
+            "y": 6.502525872657889,
+            "heading": -2.9120884337900024,
+            "angularVelocity": -0.25143902661031875,
+            "velocityX": 1.0007294296365374,
+            "velocityY": -1.0003377171605015,
+            "timestamp": 0.16990878024777242
+        },
+        {
+            "x": 5.43105048877714,
+            "y": 6.375049128446974,
+            "heading": -2.9439256135386342,
+            "angularVelocity": -0.37475614505901766,
+            "velocityX": 1.5011158503081115,
+            "velocityY": -1.5005315678808802,
+            "timestamp": 0.2548631703716586
+        },
+        {
+            "x": 5.601089619523365,
+            "y": 6.205075520007896,
+            "heading": -2.985927322319944,
+            "angularVelocity": -0.49440304050481265,
+            "velocityX": 2.001534358593136,
+            "velocityY": -2.0007630940704417,
+            "timestamp": 0.33981756049554485
+        },
+        {
+            "x": 5.813649033766866,
+            "y": 5.992595894100207,
+            "heading": -3.037161099536011,
+            "angularVelocity": -0.6030739216881468,
+            "velocityX": 2.502041553515969,
+            "velocityY": -2.501102363255205,
+            "timestamp": 0.42477195061943107
+        },
+        {
+            "x": 6.040945217563345,
+            "y": 5.7653958045642115,
+            "heading": -3.03716110602298,
+            "angularVelocity": -7.635824927871784e-8,
+            "velocityX": 2.6755083929744035,
+            "velocityY": -2.6743772653146864,
+            "timestamp": 0.5097263407433172
+        },
+        {
+            "x": 6.268241401429681,
+            "y": 5.53819571509808,
+            "heading": -3.037161112509923,
+            "angularVelocity": -7.635794871774561e-8,
+            "velocityX": 2.675508393796696,
+            "velocityY": -2.674377264492316,
+            "timestamp": 0.5946807308672034
+        },
+        {
+            "x": 6.495537585296019,
+            "y": 5.310995625631949,
+            "heading": -3.037161118996866,
+            "angularVelocity": -7.635794841586156e-8,
+            "velocityX": 2.675508393796701,
+            "velocityY": -2.6743772644923114,
+            "timestamp": 0.6796351209910896
+        },
+        {
+            "x": 6.722833769162356,
+            "y": 5.083795536165818,
+            "heading": -3.037161125483809,
+            "angularVelocity": -7.635794938585852e-8,
+            "velocityX": 2.6755083937967,
+            "velocityY": -2.6743772644923127,
+            "timestamp": 0.7645895111149758
+        },
+        {
+            "x": 6.950129953013132,
+            "y": 4.856595446684119,
+            "heading": -3.037161131970752,
+            "angularVelocity": -7.635794903516443e-8,
+            "velocityX": 2.67550839361353,
+            "velocityY": -2.6743772646755604,
+            "timestamp": 0.8495439012388619
+        },
+        {
+            "x": 7.177423048635347,
+            "y": 4.629392269264043,
+            "heading": -3.0371611395742963,
+            "angularVelocity": -8.950149400601411e-8,
+            "velocityX": 2.6754720420070424,
+            "velocityY": -2.674413612866287,
+            "timestamp": 0.9344982913627481
+        },
+        {
+            "x": 7.362966742660802,
+            "y": 4.443922816476664,
+            "heading": -3.078668671836781,
+            "angularVelocity": -0.48858607779956104,
+            "velocityX": 2.1840389149365182,
+            "velocityY": -2.183165019685597,
+            "timestamp": 1.0194526814866343
+        },
+        {
+            "x": 7.505991293187966,
+            "y": 4.3009534921360935,
+            "heading": -3.1105416993817094,
+            "angularVelocity": -0.37517811025716574,
+            "velocityX": 1.6835451389668437,
+            "velocityY": -1.6828950703063337,
+            "timestamp": 1.1044070716105205
+        },
+        {
+            "x": 7.6065035905836105,
+            "y": 4.200477915848541,
+            "heading": -3.1320612562280816,
+            "angularVelocity": -0.25330717829595906,
+            "velocityX": 1.1831324696564203,
+            "velocityY": -1.1827002246831095,
+            "timestamp": 1.1893614617344068
+        },
+        {
+            "x": 7.664506074481624,
+            "y": 4.142493806219386,
+            "heading": -3.1429833930528956,
+            "angularVelocity": -0.12856471347350074,
+            "velocityX": 0.6827485173302748,
+            "velocityY": -0.6825322334062814,
+            "timestamp": 1.274315851858293
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.002376735636840867,
+            "velocityX": 0.1823793390257534,
+            "velocityY": -0.18237793475472894,
+            "timestamp": 1.3592702419821794
+        },
+        {
+            "x": 7.6496885572239846,
+            "y": 4.157291685194411,
+            "heading": -3.131583473376336,
+            "angularVelocity": 0.13054330158990457,
+            "velocityX": -0.3410629632388108,
+            "velocityY": 0.3408406518372109,
+            "timestamp": 1.44814369364031
+        },
+        {
+            "x": 7.572857027601893,
+            "y": 4.234083644862719,
+            "heading": -3.108168408493633,
+            "angularVelocity": 0.26346523563343455,
+            "velocityX": -0.8645048458050324,
+            "velocityY": 0.8640596065007748,
+            "timestamp": 1.5370171452984407
+        },
+        {
+            "x": 7.449505471220409,
+            "y": 4.357375922418621,
+            "heading": -3.0729392389653802,
+            "angularVelocity": 0.39639699900261377,
+            "velocityX": -1.387946052278649,
+            "velocityY": 1.3872790496532752,
+            "timestamp": 1.6258905969565713
+        },
+        {
+            "x": 7.279633974763547,
+            "y": 4.52716856280588,
+            "heading": -3.0258936977546345,
+            "angularVelocity": 0.5293542709674363,
+            "velocityX": -1.911386283387714,
+            "velocityY": 1.9104989985109575,
+            "timestamp": 1.714764048614702
+        },
+        {
+            "x": 7.063242657628,
+            "y": 4.743461594393602,
+            "heading": -2.9670273926808983,
+            "angularVelocity": 0.6623609635456799,
+            "velocityX": -2.434825171050669,
+            "velocityY": 2.4337192665764835,
+            "timestamp": 1.8036375002728326
+        },
+        {
+            "x": 6.825460366769834,
+            "y": 4.981142108011423,
+            "heading": -2.967027386113265,
+            "angularVelocity": 7.38987079974334e-8,
+            "velocityX": -2.675515425830906,
+            "velocityY": 2.6743702329927124,
+            "timestamp": 1.8925109519309633
+        },
+        {
+            "x": 6.587678075764525,
+            "y": 5.218822621482074,
+            "heading": -2.9670273795456663,
+            "angularVelocity": 7.389832008297281e-8,
+            "velocityX": -2.6755154274865576,
+            "velocityY": 2.6743702313367446,
+            "timestamp": 1.981384403589094
+        },
+        {
+            "x": 6.349895784759215,
+            "y": 5.456503134952723,
+            "heading": -2.967027372978067,
+            "angularVelocity": 7.389832049238788e-8,
+            "velocityX": -2.675515427486565,
+            "velocityY": 2.6743702313367375,
+            "timestamp": 2.0702578552472244
+        },
+        {
+            "x": 6.112113493753906,
+            "y": 5.6941836484233725,
+            "heading": -2.967027366410468,
+            "angularVelocity": 7.389831996958896e-8,
+            "velocityX": -2.67551542748656,
+            "velocityY": 2.6743702313367437,
+            "timestamp": 2.159131306905355
+        },
+        {
+            "x": 5.874331202859884,
+            "y": 5.931864162005351,
+            "heading": -2.9670273598428634,
+            "angularVelocity": 7.389838540319935e-8,
+            "velocityX": -2.6755154262343623,
+            "velocityY": 2.6743702325894136,
+            "timestamp": 2.2480047585634853
+        },
+        {
+            "x": 5.641547676468134,
+            "y": 6.164595563213349,
+            "heading": -2.949497080521501,
+            "angularVelocity": 0.1972498985246826,
+            "velocityX": -2.619269557429503,
+            "velocityY": 2.6186830472537257,
+            "timestamp": 2.3368782102216157
+        },
+        {
+            "x": 5.455313702452088,
+            "y": 6.350776206364247,
+            "heading": -2.9252668204425896,
+            "angularVelocity": 0.2726377745748803,
+            "velocityX": -2.0954961300759565,
+            "velocityY": 2.0948960536284607,
+            "timestamp": 2.425751661879746
+        },
+        {
+            "x": 5.3156503816730245,
+            "y": 6.490395881310486,
+            "heading": -2.9038323021536026,
+            "angularVelocity": 0.2411802162410647,
+            "velocityX": -1.571485276799544,
+            "velocityY": 1.5709941758913435,
+            "timestamp": 2.5146251135378765
+        },
+        {
+            "x": 5.222548326123915,
+            "y": 6.583467415924721,
+            "heading": -2.888242371448522,
+            "angularVelocity": 0.17541718493253458,
+            "velocityX": -1.0475800569474265,
+            "velocityY": 1.047236636788705,
+            "timestamp": 2.603498565196007
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0.09274278532860386,
+            "velocityX": -0.5237596296246885,
+            "velocityY": 0.5235825008157663,
+            "timestamp": 2.6923720168541374
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 3.731495977364104e-28,
+            "velocityX": 5.297862503022749e-31,
+            "velocityY": -3.827373628676626e-30,
+            "timestamp": 2.781245468512268
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S1.traj
new file mode 100644
index 00000000..41e8c0c3
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S1.traj	
@@ -0,0 +1,301 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.218507748835192,
+            "y": 6.587508953337151,
+            "heading": -2.890727584630965,
+            "angularVelocity": -0.12627463531080194,
+            "velocityX": 0.5003596491390202,
+            "velocityY": -0.5001630474999694,
+            "timestamp": 0.08495439012388621
+        },
+        {
+            "x": 5.303524107208959,
+            "y": 6.502525872657889,
+            "heading": -2.9120884337900024,
+            "angularVelocity": -0.25143902661031875,
+            "velocityX": 1.0007294296365374,
+            "velocityY": -1.0003377171605015,
+            "timestamp": 0.16990878024777242
+        },
+        {
+            "x": 5.43105048877714,
+            "y": 6.375049128446974,
+            "heading": -2.9439256135386342,
+            "angularVelocity": -0.37475614505901766,
+            "velocityX": 1.5011158503081115,
+            "velocityY": -1.5005315678808802,
+            "timestamp": 0.2548631703716586
+        },
+        {
+            "x": 5.601089619523365,
+            "y": 6.205075520007896,
+            "heading": -2.985927322319944,
+            "angularVelocity": -0.49440304050481265,
+            "velocityX": 2.001534358593136,
+            "velocityY": -2.0007630940704417,
+            "timestamp": 0.33981756049554485
+        },
+        {
+            "x": 5.813649033766866,
+            "y": 5.992595894100207,
+            "heading": -3.037161099536011,
+            "angularVelocity": -0.6030739216881468,
+            "velocityX": 2.502041553515969,
+            "velocityY": -2.501102363255205,
+            "timestamp": 0.42477195061943107
+        },
+        {
+            "x": 6.040945217563345,
+            "y": 5.7653958045642115,
+            "heading": -3.03716110602298,
+            "angularVelocity": -7.635824927871784e-8,
+            "velocityX": 2.6755083929744035,
+            "velocityY": -2.6743772653146864,
+            "timestamp": 0.5097263407433172
+        },
+        {
+            "x": 6.268241401429681,
+            "y": 5.53819571509808,
+            "heading": -3.037161112509923,
+            "angularVelocity": -7.635794871774561e-8,
+            "velocityX": 2.675508393796696,
+            "velocityY": -2.674377264492316,
+            "timestamp": 0.5946807308672034
+        },
+        {
+            "x": 6.495537585296019,
+            "y": 5.310995625631949,
+            "heading": -3.037161118996866,
+            "angularVelocity": -7.635794841586156e-8,
+            "velocityX": 2.675508393796701,
+            "velocityY": -2.6743772644923114,
+            "timestamp": 0.6796351209910896
+        },
+        {
+            "x": 6.722833769162356,
+            "y": 5.083795536165818,
+            "heading": -3.037161125483809,
+            "angularVelocity": -7.635794938585852e-8,
+            "velocityX": 2.6755083937967,
+            "velocityY": -2.6743772644923127,
+            "timestamp": 0.7645895111149758
+        },
+        {
+            "x": 6.950129953013132,
+            "y": 4.856595446684119,
+            "heading": -3.037161131970752,
+            "angularVelocity": -7.635794903516443e-8,
+            "velocityX": 2.67550839361353,
+            "velocityY": -2.6743772646755604,
+            "timestamp": 0.8495439012388619
+        },
+        {
+            "x": 7.177423048635347,
+            "y": 4.629392269264043,
+            "heading": -3.0371611395742963,
+            "angularVelocity": -8.950149400601411e-8,
+            "velocityX": 2.6754720420070424,
+            "velocityY": -2.674413612866287,
+            "timestamp": 0.9344982913627481
+        },
+        {
+            "x": 7.362966742660802,
+            "y": 4.443922816476664,
+            "heading": -3.078668671836781,
+            "angularVelocity": -0.48858607779956104,
+            "velocityX": 2.1840389149365182,
+            "velocityY": -2.183165019685597,
+            "timestamp": 1.0194526814866343
+        },
+        {
+            "x": 7.505991293187966,
+            "y": 4.3009534921360935,
+            "heading": -3.1105416993817094,
+            "angularVelocity": -0.37517811025716574,
+            "velocityX": 1.6835451389668437,
+            "velocityY": -1.6828950703063337,
+            "timestamp": 1.1044070716105205
+        },
+        {
+            "x": 7.6065035905836105,
+            "y": 4.200477915848541,
+            "heading": -3.1320612562280816,
+            "angularVelocity": -0.25330717829595906,
+            "velocityX": 1.1831324696564203,
+            "velocityY": -1.1827002246831095,
+            "timestamp": 1.1893614617344068
+        },
+        {
+            "x": 7.664506074481624,
+            "y": 4.142493806219386,
+            "heading": -3.1429833930528956,
+            "angularVelocity": -0.12856471347350074,
+            "velocityX": 0.6827485173302748,
+            "velocityY": -0.6825322334062814,
+            "timestamp": 1.274315851858293
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.002376735636840867,
+            "velocityX": 0.1823793390257534,
+            "velocityY": -0.18237793475472894,
+            "timestamp": 1.3592702419821794
+        },
+        {
+            "x": 7.6496885572239846,
+            "y": 4.157291685194411,
+            "heading": -3.131583473376336,
+            "angularVelocity": 0.13054330158990457,
+            "velocityX": -0.3410629632388108,
+            "velocityY": 0.3408406518372109,
+            "timestamp": 1.44814369364031
+        },
+        {
+            "x": 7.572857027601893,
+            "y": 4.234083644862719,
+            "heading": -3.108168408493633,
+            "angularVelocity": 0.26346523563343455,
+            "velocityX": -0.8645048458050324,
+            "velocityY": 0.8640596065007748,
+            "timestamp": 1.5370171452984407
+        },
+        {
+            "x": 7.449505471220409,
+            "y": 4.357375922418621,
+            "heading": -3.0729392389653802,
+            "angularVelocity": 0.39639699900261377,
+            "velocityX": -1.387946052278649,
+            "velocityY": 1.3872790496532752,
+            "timestamp": 1.6258905969565713
+        },
+        {
+            "x": 7.279633974763547,
+            "y": 4.52716856280588,
+            "heading": -3.0258936977546345,
+            "angularVelocity": 0.5293542709674363,
+            "velocityX": -1.911386283387714,
+            "velocityY": 1.9104989985109575,
+            "timestamp": 1.714764048614702
+        },
+        {
+            "x": 7.063242657628,
+            "y": 4.743461594393602,
+            "heading": -2.9670273926808983,
+            "angularVelocity": 0.6623609635456799,
+            "velocityX": -2.434825171050669,
+            "velocityY": 2.4337192665764835,
+            "timestamp": 1.8036375002728326
+        },
+        {
+            "x": 6.825460366769834,
+            "y": 4.981142108011423,
+            "heading": -2.967027386113265,
+            "angularVelocity": 7.38987079974334e-8,
+            "velocityX": -2.675515425830906,
+            "velocityY": 2.6743702329927124,
+            "timestamp": 1.8925109519309633
+        },
+        {
+            "x": 6.587678075764525,
+            "y": 5.218822621482074,
+            "heading": -2.9670273795456663,
+            "angularVelocity": 7.389832008297281e-8,
+            "velocityX": -2.6755154274865576,
+            "velocityY": 2.6743702313367446,
+            "timestamp": 1.981384403589094
+        },
+        {
+            "x": 6.349895784759215,
+            "y": 5.456503134952723,
+            "heading": -2.967027372978067,
+            "angularVelocity": 7.389832049238788e-8,
+            "velocityX": -2.675515427486565,
+            "velocityY": 2.6743702313367375,
+            "timestamp": 2.0702578552472244
+        },
+        {
+            "x": 6.112113493753906,
+            "y": 5.6941836484233725,
+            "heading": -2.967027366410468,
+            "angularVelocity": 7.389831996958896e-8,
+            "velocityX": -2.67551542748656,
+            "velocityY": 2.6743702313367437,
+            "timestamp": 2.159131306905355
+        },
+        {
+            "x": 5.874331202859884,
+            "y": 5.931864162005351,
+            "heading": -2.9670273598428634,
+            "angularVelocity": 7.389838540319935e-8,
+            "velocityX": -2.6755154262343623,
+            "velocityY": 2.6743702325894136,
+            "timestamp": 2.2480047585634853
+        },
+        {
+            "x": 5.641547676468134,
+            "y": 6.164595563213349,
+            "heading": -2.949497080521501,
+            "angularVelocity": 0.1972498985246826,
+            "velocityX": -2.619269557429503,
+            "velocityY": 2.6186830472537257,
+            "timestamp": 2.3368782102216157
+        },
+        {
+            "x": 5.455313702452088,
+            "y": 6.350776206364247,
+            "heading": -2.9252668204425896,
+            "angularVelocity": 0.2726377745748803,
+            "velocityX": -2.0954961300759565,
+            "velocityY": 2.0948960536284607,
+            "timestamp": 2.425751661879746
+        },
+        {
+            "x": 5.3156503816730245,
+            "y": 6.490395881310486,
+            "heading": -2.9038323021536026,
+            "angularVelocity": 0.2411802162410647,
+            "velocityX": -1.571485276799544,
+            "velocityY": 1.5709941758913435,
+            "timestamp": 2.5146251135378765
+        },
+        {
+            "x": 5.222548326123915,
+            "y": 6.583467415924721,
+            "heading": -2.888242371448522,
+            "angularVelocity": 0.17541718493253458,
+            "velocityX": -1.0475800569474265,
+            "velocityY": 1.047236636788705,
+            "timestamp": 2.603498565196007
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0.09274278532860386,
+            "velocityX": -0.5237596296246885,
+            "velocityY": 0.5235825008157663,
+            "timestamp": 2.6923720168541374
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 3.731495977364104e-28,
+            "velocityX": 5.297862503022749e-31,
+            "velocityY": -3.827373628676626e-30,
+            "timestamp": 2.781245468512268
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S2.1.traj
new file mode 100644
index 00000000..3bf6de58
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S2.1.traj	
@@ -0,0 +1,292 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.2230421692043185,
+            "y": 6.596479510791831,
+            "heading": -2.8866360691814505,
+            "angularVelocity": -0.07970895698970118,
+            "velocityX": 0.5650456828103535,
+            "velocityY": -0.40263040657207605,
+            "timestamp": 0.08325374502526237
+        },
+        {
+            "x": 5.3171265089770285,
+            "y": 6.529438536200997,
+            "heading": -2.8999080848900367,
+            "angularVelocity": -0.15941644072056302,
+            "velocityX": 1.130091382005238,
+            "velocityY": -0.8052607671941977,
+            "timestamp": 0.16650749005052473
+        },
+        {
+            "x": 5.458253021148553,
+            "y": 6.428877082265555,
+            "heading": -2.919815877976496,
+            "angularVelocity": -0.23912189271985748,
+            "velocityX": 1.6951371031862086,
+            "velocityY": -1.207891055290405,
+            "timestamp": 0.2497612350757871
+        },
+        {
+            "x": 5.646421706294536,
+            "y": 6.294795158564417,
+            "heading": -2.946359278217123,
+            "angularVelocity": -0.31882530008192794,
+            "velocityX": 2.260182831281474,
+            "velocityY": -1.6105212283297445,
+            "timestamp": 0.33301498010104946
+        },
+        {
+            "x": 5.881632555955815,
+            "y": 6.127192783772945,
+            "heading": -2.9795381480937526,
+            "angularVelocity": -0.3985270556484735,
+            "velocityX": 2.825228457769793,
+            "velocityY": -2.01315117705052,
+            "timestamp": 0.41626872512631186
+        },
+        {
+            "x": 6.138121957560345,
+            "y": 5.944428216164069,
+            "heading": -2.9795381521700137,
+            "angularVelocity": -4.8961898041700444e-8,
+            "velocityX": 3.080815181667819,
+            "velocityY": -2.195271426569656,
+            "timestamp": 0.49952247015157425
+        },
+        {
+            "x": 6.3946113591646245,
+            "y": 5.761663648554803,
+            "heading": -2.979538156246253,
+            "angularVelocity": -4.896162950841139e-8,
+            "velocityX": 3.080815181664829,
+            "velocityY": -2.1952714265743465,
+            "timestamp": 0.5827762151768366
+        },
+        {
+            "x": 6.651100760768856,
+            "y": 5.57889908094547,
+            "heading": -2.979538160322492,
+            "angularVelocity": -4.8961628925618056e-8,
+            "velocityX": 3.080815181664257,
+            "velocityY": -2.1952714265751494,
+            "timestamp": 0.666029960202099
+        },
+        {
+            "x": 6.907590153574081,
+            "y": 5.396134500987725,
+            "heading": -2.979538164398731,
+            "angularVelocity": -4.8961630365108595e-8,
+            "velocityX": 3.080815075975223,
+            "velocityY": -2.195271574897749,
+            "timestamp": 0.7492837052273614
+        },
+        {
+            "x": 7.162448400773746,
+            "y": 5.211102141322406,
+            "heading": -2.979538168481962,
+            "angularVelocity": -4.904561353756505e-8,
+            "velocityX": 3.06122261674062,
+            "velocityY": -2.2225109466143445,
+            "timestamp": 0.8325374502526238
+        },
+        {
+            "x": 7.379207979156498,
+            "y": 4.982618051640636,
+            "heading": -2.979538183621576,
+            "angularVelocity": -1.8184904794867305e-7,
+            "velocityX": 2.6036015354861264,
+            "velocityY": -2.7444301708283056,
+            "timestamp": 0.9157911952778862
+        },
+        {
+            "x": 7.538979437885237,
+            "y": 4.753039236440615,
+            "heading": -3.0140329583631105,
+            "angularVelocity": -0.4143330096570687,
+            "velocityX": 1.919090350593248,
+            "velocityY": -2.7575794353799603,
+            "timestamp": 0.9990449403031486
+        },
+        {
+            "x": 7.647684941731154,
+            "y": 4.550516222432533,
+            "heading": -3.0504775725311664,
+            "angularVelocity": -0.43775345069461125,
+            "velocityX": 1.3057130800893728,
+            "velocityY": -2.432599445786314,
+            "timestamp": 1.082298685328411
+        },
+        {
+            "x": 7.7070137148586175,
+            "y": 4.378021543776967,
+            "heading": -3.0850144113956173,
+            "angularVelocity": -0.4148382616778724,
+            "velocityX": 0.7126258777842882,
+            "velocityY": -2.071914946327394,
+            "timestamp": 1.1655524303536733
+        },
+        {
+            "x": 7.717659875456922,
+            "y": 4.236658143123209,
+            "heading": -3.1161746508326322,
+            "angularVelocity": -0.3742803333058527,
+            "velocityX": 0.12787605644728667,
+            "velocityY": -1.6979824824801262,
+            "timestamp": 1.2488061753789357
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.32443773356692346,
+            "velocityX": -0.4523505272405549,
+            "velocityY": -1.3171556797827937,
+            "timestamp": 1.332059920404198
+        },
+        {
+            "x": 7.570205120538345,
+            "y": 4.042598994966427,
+            "heading": -3.1684471523328126,
+            "angularVelocity": -0.2594470408705627,
+            "velocityX": -1.1276277091512135,
+            "velocityY": -0.8668246863853641,
+            "timestamp": 1.4294279422882057
+        },
+        {
+            "x": 7.395116825349968,
+            "y": 4.002723380018642,
+            "heading": -3.1873733598366223,
+            "angularVelocity": -0.19437806312174594,
+            "velocityX": -1.7982114846386814,
+            "velocityY": -0.4095350216243075,
+            "timestamp": 1.5267959641722133
+        },
+        {
+            "x": 7.155591929265255,
+            "y": 4.008604211117546,
+            "heading": -3.1999520195770272,
+            "angularVelocity": -0.1291867647818698,
+            "velocityX": -2.4599955041712693,
+            "velocityY": 0.06039797240528109,
+            "timestamp": 1.624163986056221
+        },
+        {
+            "x": 6.8538087913529315,
+            "y": 4.063163767369359,
+            "heading": -3.206164046421751,
+            "angularVelocity": -0.06379945617179969,
+            "velocityX": -3.0994070956050406,
+            "velocityY": 0.5603436857001141,
+            "timestamp": 1.7215320079402285
+        },
+        {
+            "x": 6.5048799215848865,
+            "y": 4.181151469294334,
+            "heading": -3.206164042303272,
+            "angularVelocity": 4.229807303696183e-8,
+            "velocityX": -3.5836084888703787,
+            "velocityY": 1.2117705550753732,
+            "timestamp": 1.8189000298242362
+        },
+        {
+            "x": 6.189160518675962,
+            "y": 4.3708716014574716,
+            "heading": -3.2061640395158935,
+            "angularVelocity": 2.862724812638766e-8,
+            "velocityX": -3.2425368904488527,
+            "velocityY": 1.9484850209768463,
+            "timestamp": 1.9162680517082438
+        },
+        {
+            "x": 5.875739114062726,
+            "y": 4.564364477524865,
+            "heading": -3.2061640367361566,
+            "angularVelocity": 2.854876387634348e-8,
+            "velocityX": -3.218935730116917,
+            "velocityY": 1.987232279381165,
+            "timestamp": 2.0136360735922514
+        },
+        {
+            "x": 5.562317717200868,
+            "y": 4.757857366148008,
+            "heading": -3.20616403395642,
+            "angularVelocity": 2.854876379789868e-8,
+            "velocityX": -3.21893565050783,
+            "velocityY": 1.9872324083326358,
+            "timestamp": 2.111004095476259
+        },
+        {
+            "x": 5.248896320339037,
+            "y": 4.951350254771193,
+            "heading": -3.2061640311766837,
+            "angularVelocity": 2.8548763614821095e-8,
+            "velocityX": -3.218935650507565,
+            "velocityY": 1.9872324083330646,
+            "timestamp": 2.208372117360266
+        },
+        {
+            "x": 4.935474923469446,
+            "y": 5.14484314338174,
+            "heading": -3.2061640283969295,
+            "angularVelocity": 2.8548942066572532e-8,
+            "velocityX": -3.2189356505872584,
+            "velocityY": 1.9872324082032717,
+            "timestamp": 2.3057401392442736
+        },
+        {
+            "x": 4.666484969375691,
+            "y": 5.310905875767285,
+            "heading": -3.180972420287378,
+            "angularVelocity": 0.2587256844917891,
+            "velocityX": -2.76261085404609,
+            "velocityY": 1.7055161353012933,
+            "timestamp": 2.403108161128281
+        },
+        {
+            "x": 4.464742488596807,
+            "y": 5.435452935258762,
+            "heading": -3.1620788420473156,
+            "angularVelocity": 0.19404295039052252,
+            "velocityX": -2.071958296731311,
+            "velocityY": 1.279137206256954,
+            "timestamp": 2.5004761830122884
+        },
+        {
+            "x": 4.330247497040812,
+            "y": 5.518484311233343,
+            "heading": -3.149483158535891,
+            "angularVelocity": 0.12936160422802073,
+            "velocityX": -1.3813055760362147,
+            "velocityY": 0.8527581681128775,
+            "timestamp": 2.5978442048962958
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0.0646809007150999,
+            "velocityX": -0.6906528009876122,
+            "velocityY": 0.4263790920607684,
+            "timestamp": 2.695212226780303
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": -3.400041679826015e-29,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 2.7925802486643105
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S2.traj
new file mode 100644
index 00000000..3bf6de58
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S2.traj	
@@ -0,0 +1,292 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.2230421692043185,
+            "y": 6.596479510791831,
+            "heading": -2.8866360691814505,
+            "angularVelocity": -0.07970895698970118,
+            "velocityX": 0.5650456828103535,
+            "velocityY": -0.40263040657207605,
+            "timestamp": 0.08325374502526237
+        },
+        {
+            "x": 5.3171265089770285,
+            "y": 6.529438536200997,
+            "heading": -2.8999080848900367,
+            "angularVelocity": -0.15941644072056302,
+            "velocityX": 1.130091382005238,
+            "velocityY": -0.8052607671941977,
+            "timestamp": 0.16650749005052473
+        },
+        {
+            "x": 5.458253021148553,
+            "y": 6.428877082265555,
+            "heading": -2.919815877976496,
+            "angularVelocity": -0.23912189271985748,
+            "velocityX": 1.6951371031862086,
+            "velocityY": -1.207891055290405,
+            "timestamp": 0.2497612350757871
+        },
+        {
+            "x": 5.646421706294536,
+            "y": 6.294795158564417,
+            "heading": -2.946359278217123,
+            "angularVelocity": -0.31882530008192794,
+            "velocityX": 2.260182831281474,
+            "velocityY": -1.6105212283297445,
+            "timestamp": 0.33301498010104946
+        },
+        {
+            "x": 5.881632555955815,
+            "y": 6.127192783772945,
+            "heading": -2.9795381480937526,
+            "angularVelocity": -0.3985270556484735,
+            "velocityX": 2.825228457769793,
+            "velocityY": -2.01315117705052,
+            "timestamp": 0.41626872512631186
+        },
+        {
+            "x": 6.138121957560345,
+            "y": 5.944428216164069,
+            "heading": -2.9795381521700137,
+            "angularVelocity": -4.8961898041700444e-8,
+            "velocityX": 3.080815181667819,
+            "velocityY": -2.195271426569656,
+            "timestamp": 0.49952247015157425
+        },
+        {
+            "x": 6.3946113591646245,
+            "y": 5.761663648554803,
+            "heading": -2.979538156246253,
+            "angularVelocity": -4.896162950841139e-8,
+            "velocityX": 3.080815181664829,
+            "velocityY": -2.1952714265743465,
+            "timestamp": 0.5827762151768366
+        },
+        {
+            "x": 6.651100760768856,
+            "y": 5.57889908094547,
+            "heading": -2.979538160322492,
+            "angularVelocity": -4.8961628925618056e-8,
+            "velocityX": 3.080815181664257,
+            "velocityY": -2.1952714265751494,
+            "timestamp": 0.666029960202099
+        },
+        {
+            "x": 6.907590153574081,
+            "y": 5.396134500987725,
+            "heading": -2.979538164398731,
+            "angularVelocity": -4.8961630365108595e-8,
+            "velocityX": 3.080815075975223,
+            "velocityY": -2.195271574897749,
+            "timestamp": 0.7492837052273614
+        },
+        {
+            "x": 7.162448400773746,
+            "y": 5.211102141322406,
+            "heading": -2.979538168481962,
+            "angularVelocity": -4.904561353756505e-8,
+            "velocityX": 3.06122261674062,
+            "velocityY": -2.2225109466143445,
+            "timestamp": 0.8325374502526238
+        },
+        {
+            "x": 7.379207979156498,
+            "y": 4.982618051640636,
+            "heading": -2.979538183621576,
+            "angularVelocity": -1.8184904794867305e-7,
+            "velocityX": 2.6036015354861264,
+            "velocityY": -2.7444301708283056,
+            "timestamp": 0.9157911952778862
+        },
+        {
+            "x": 7.538979437885237,
+            "y": 4.753039236440615,
+            "heading": -3.0140329583631105,
+            "angularVelocity": -0.4143330096570687,
+            "velocityX": 1.919090350593248,
+            "velocityY": -2.7575794353799603,
+            "timestamp": 0.9990449403031486
+        },
+        {
+            "x": 7.647684941731154,
+            "y": 4.550516222432533,
+            "heading": -3.0504775725311664,
+            "angularVelocity": -0.43775345069461125,
+            "velocityX": 1.3057130800893728,
+            "velocityY": -2.432599445786314,
+            "timestamp": 1.082298685328411
+        },
+        {
+            "x": 7.7070137148586175,
+            "y": 4.378021543776967,
+            "heading": -3.0850144113956173,
+            "angularVelocity": -0.4148382616778724,
+            "velocityX": 0.7126258777842882,
+            "velocityY": -2.071914946327394,
+            "timestamp": 1.1655524303536733
+        },
+        {
+            "x": 7.717659875456922,
+            "y": 4.236658143123209,
+            "heading": -3.1161746508326322,
+            "angularVelocity": -0.3742803333058527,
+            "velocityX": 0.12787605644728667,
+            "velocityY": -1.6979824824801262,
+            "timestamp": 1.2488061753789357
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.32443773356692346,
+            "velocityX": -0.4523505272405549,
+            "velocityY": -1.3171556797827937,
+            "timestamp": 1.332059920404198
+        },
+        {
+            "x": 7.570205120538345,
+            "y": 4.042598994966427,
+            "heading": -3.1684471523328126,
+            "angularVelocity": -0.2594470408705627,
+            "velocityX": -1.1276277091512135,
+            "velocityY": -0.8668246863853641,
+            "timestamp": 1.4294279422882057
+        },
+        {
+            "x": 7.395116825349968,
+            "y": 4.002723380018642,
+            "heading": -3.1873733598366223,
+            "angularVelocity": -0.19437806312174594,
+            "velocityX": -1.7982114846386814,
+            "velocityY": -0.4095350216243075,
+            "timestamp": 1.5267959641722133
+        },
+        {
+            "x": 7.155591929265255,
+            "y": 4.008604211117546,
+            "heading": -3.1999520195770272,
+            "angularVelocity": -0.1291867647818698,
+            "velocityX": -2.4599955041712693,
+            "velocityY": 0.06039797240528109,
+            "timestamp": 1.624163986056221
+        },
+        {
+            "x": 6.8538087913529315,
+            "y": 4.063163767369359,
+            "heading": -3.206164046421751,
+            "angularVelocity": -0.06379945617179969,
+            "velocityX": -3.0994070956050406,
+            "velocityY": 0.5603436857001141,
+            "timestamp": 1.7215320079402285
+        },
+        {
+            "x": 6.5048799215848865,
+            "y": 4.181151469294334,
+            "heading": -3.206164042303272,
+            "angularVelocity": 4.229807303696183e-8,
+            "velocityX": -3.5836084888703787,
+            "velocityY": 1.2117705550753732,
+            "timestamp": 1.8189000298242362
+        },
+        {
+            "x": 6.189160518675962,
+            "y": 4.3708716014574716,
+            "heading": -3.2061640395158935,
+            "angularVelocity": 2.862724812638766e-8,
+            "velocityX": -3.2425368904488527,
+            "velocityY": 1.9484850209768463,
+            "timestamp": 1.9162680517082438
+        },
+        {
+            "x": 5.875739114062726,
+            "y": 4.564364477524865,
+            "heading": -3.2061640367361566,
+            "angularVelocity": 2.854876387634348e-8,
+            "velocityX": -3.218935730116917,
+            "velocityY": 1.987232279381165,
+            "timestamp": 2.0136360735922514
+        },
+        {
+            "x": 5.562317717200868,
+            "y": 4.757857366148008,
+            "heading": -3.20616403395642,
+            "angularVelocity": 2.854876379789868e-8,
+            "velocityX": -3.21893565050783,
+            "velocityY": 1.9872324083326358,
+            "timestamp": 2.111004095476259
+        },
+        {
+            "x": 5.248896320339037,
+            "y": 4.951350254771193,
+            "heading": -3.2061640311766837,
+            "angularVelocity": 2.8548763614821095e-8,
+            "velocityX": -3.218935650507565,
+            "velocityY": 1.9872324083330646,
+            "timestamp": 2.208372117360266
+        },
+        {
+            "x": 4.935474923469446,
+            "y": 5.14484314338174,
+            "heading": -3.2061640283969295,
+            "angularVelocity": 2.8548942066572532e-8,
+            "velocityX": -3.2189356505872584,
+            "velocityY": 1.9872324082032717,
+            "timestamp": 2.3057401392442736
+        },
+        {
+            "x": 4.666484969375691,
+            "y": 5.310905875767285,
+            "heading": -3.180972420287378,
+            "angularVelocity": 0.2587256844917891,
+            "velocityX": -2.76261085404609,
+            "velocityY": 1.7055161353012933,
+            "timestamp": 2.403108161128281
+        },
+        {
+            "x": 4.464742488596807,
+            "y": 5.435452935258762,
+            "heading": -3.1620788420473156,
+            "angularVelocity": 0.19404295039052252,
+            "velocityX": -2.071958296731311,
+            "velocityY": 1.279137206256954,
+            "timestamp": 2.5004761830122884
+        },
+        {
+            "x": 4.330247497040812,
+            "y": 5.518484311233343,
+            "heading": -3.149483158535891,
+            "angularVelocity": 0.12936160422802073,
+            "velocityX": -1.3813055760362147,
+            "velocityY": 0.8527581681128775,
+            "timestamp": 2.5978442048962958
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0.0646809007150999,
+            "velocityX": -0.6906528009876122,
+            "velocityY": 0.4263790920607684,
+            "timestamp": 2.695212226780303
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": -3.400041679826015e-29,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 2.7925802486643105
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S3.1.traj
new file mode 100644
index 00000000..77d8e571
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S3.1.traj	
@@ -0,0 +1,310 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 2.6904386896052926e-33,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.221431258578075,
+            "y": 6.6088996918922325,
+            "heading": -2.8866746127276346,
+            "angularVelocity": -0.08608437747729661,
+            "velocityX": 0.5859398548340703,
+            "velocityY": -0.2721366710185261,
+            "timestamp": 0.07753570303037556
+        },
+        {
+            "x": 5.31229377552925,
+            "y": 6.566699082350374,
+            "heading": -2.90002400824425,
+            "angularVelocity": -0.17217094828412036,
+            "velocityX": 1.1718797070244922,
+            "velocityY": -0.5442732559647441,
+            "timestamp": 0.15507140606075112
+        },
+        {
+            "x": 5.448587549407382,
+            "y": 6.503398181602668,
+            "heading": -2.920048533381972,
+            "angularVelocity": -0.2582619922834353,
+            "velocityX": 1.7578195405636137,
+            "velocityY": -0.8164097089944291,
+            "timestamp": 0.2326071090911267
+        },
+        {
+            "x": 5.630312575309777,
+            "y": 6.418997003981843,
+            "heading": -2.946748763466125,
+            "angularVelocity": -0.34436045641700236,
+            "velocityX": 2.3437593108713033,
+            "velocityY": -1.0885459771707946,
+            "timestamp": 0.31014281212150224
+        },
+        {
+            "x": 5.85746883610922,
+            "y": 6.313495571165909,
+            "heading": -2.9801255206908306,
+            "angularVelocity": -0.43046952462183896,
+            "velocityX": 2.929698860284435,
+            "velocityY": -1.3606819657597289,
+            "timestamp": 0.3876785151518778
+        },
+        {
+            "x": 6.123490041046321,
+            "y": 6.189943134833017,
+            "heading": -2.9801255253705285,
+            "angularVelocity": -6.035539216471427e-8,
+            "velocityX": 3.4309510914331183,
+            "velocityY": -1.5934908887649881,
+            "timestamp": 0.46521421818225334
+        },
+        {
+            "x": 6.3895112454442975,
+            "y": 6.066390697338873,
+            "heading": -2.980125530050055,
+            "angularVelocity": -6.035318736040994e-8,
+            "velocityX": 3.4309510844798714,
+            "velocityY": -1.5934909037419929,
+            "timestamp": 0.5427499212126289
+        },
+        {
+            "x": 6.655532416724182,
+            "y": 5.9428381885380706,
+            "heading": -2.980125534729582,
+            "angularVelocity": -6.035318830910377e-8,
+            "velocityX": 3.4309506573464303,
+            "velocityY": -1.5934918234042366,
+            "timestamp": 0.6202856242430045
+        },
+        {
+            "x": 6.916684100099049,
+            "y": 5.8093006126159725,
+            "heading": -2.9801255394461488,
+            "angularVelocity": -6.083090321698073e-8,
+            "velocityX": 3.368147487778084,
+            "velocityY": -1.722272072128935,
+            "timestamp": 0.6978213272733801
+        },
+        {
+            "x": 7.151284630214051,
+            "y": 5.6332464271795715,
+            "heading": -2.980125544587418,
+            "angularVelocity": -6.630841173330857e-8,
+            "velocityX": 3.0257097175361216,
+            "velocityY": -2.2706208695551444,
+            "timestamp": 0.7753570303037557
+        },
+        {
+            "x": 7.352481762263237,
+            "y": 5.41981682210962,
+            "heading": -2.9801255509932516,
+            "angularVelocity": -8.261785689390399e-8,
+            "velocityX": 2.594896598414324,
+            "velocityY": -2.7526622798059472,
+            "timestamp": 0.8528927333341313
+        },
+        {
+            "x": 7.514399141163384,
+            "y": 5.175245297922962,
+            "heading": -2.9801256025732923,
+            "angularVelocity": -6.652424424834864e-7,
+            "velocityX": 2.0882944575444746,
+            "velocityY": -3.1543084621396242,
+            "timestamp": 0.9304284363645069
+        },
+        {
+            "x": 7.630164379984533,
+            "y": 4.911565821303128,
+            "heading": -2.995124870089402,
+            "angularVelocity": -0.19344981640565623,
+            "velocityX": 1.4930571890964461,
+            "velocityY": -3.4007491557345486,
+            "timestamp": 1.0079641393948824
+        },
+        {
+            "x": 7.696376897938806,
+            "y": 4.643893289420577,
+            "heading": -3.0332372938699343,
+            "angularVelocity": -0.49154676221354465,
+            "velocityX": 0.8539616636781268,
+            "velocityY": -3.452248724406194,
+            "timestamp": 1.085499842425258
+        },
+        {
+            "x": 7.712814001135022,
+            "y": 4.380911639847311,
+            "heading": -3.084305372604304,
+            "angularVelocity": -0.6586395265464133,
+            "velocityX": 0.2119939918488342,
+            "velocityY": -3.3917490819711635,
+            "timestamp": 1.1630355454556336
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.7593912516949146,
+            "velocityX": -0.4232114993806923,
+            "velocityY": -3.274770588561481,
+            "timestamp": 1.2405712484860092
+        },
+        {
+            "x": 7.5963416463862465,
+            "y": 3.881029306471945,
+            "heading": -3.207914317977363,
+            "angularVelocity": -0.8216221979368662,
+            "velocityX": -1.061897277970947,
+            "velocityY": -3.1221700958161,
+            "timestamp": 1.3193532129464565
+        },
+        {
+            "x": 7.463114472360868,
+            "y": 3.64992032438806,
+            "heading": -3.2707770828701133,
+            "angularVelocity": -0.7979334524504146,
+            "velocityX": -1.6910872296446224,
+            "velocityY": -2.933526520526327,
+            "timestamp": 1.3981351774069037
+        },
+        {
+            "x": 7.282107143226341,
+            "y": 3.438334424773196,
+            "heading": -3.3202070228970326,
+            "angularVelocity": -0.627427106768014,
+            "velocityX": -2.297573186632101,
+            "velocityY": -2.6857149483888696,
+            "timestamp": 1.476917141867351
+        },
+        {
+            "x": 7.057858991686637,
+            "y": 3.2538746449727443,
+            "heading": -3.3390490142356852,
+            "angularVelocity": -0.23916630497468983,
+            "velocityX": -2.846440210973517,
+            "velocityY": -2.341396042403339,
+            "timestamp": 1.5556991063277983
+        },
+        {
+            "x": 6.798499862682402,
+            "y": 3.1070645806670387,
+            "heading": -3.339049130726268,
+            "angularVelocity": -0.0000014786453189461519,
+            "velocityX": -3.2921129954108745,
+            "velocityY": -1.8634983947298314,
+            "timestamp": 1.6344810707882456
+        },
+        {
+            "x": 6.517654165139062,
+            "y": 3.0073345934329407,
+            "heading": -3.339049157320925,
+            "angularVelocity": -3.3757290550660805e-7,
+            "velocityX": -3.564847607783876,
+            "velocityY": -1.26589871066451,
+            "timestamp": 1.7132630352486928
+        },
+        {
+            "x": 6.223790307061648,
+            "y": 2.9576917507277325,
+            "heading": -3.339049179064471,
+            "angularVelocity": -2.7599649426865103e-7,
+            "velocityX": -3.730090511070579,
+            "velocityY": -0.6301295359311817,
+            "timestamp": 1.79204499970914
+        },
+        {
+            "x": 5.925769134173891,
+            "y": 2.9596309032298342,
+            "heading": -3.3390491994000007,
+            "angularVelocity": -2.581241798261079e-7,
+            "velocityX": -3.7828603910655114,
+            "velocityY": 0.024614167917522748,
+            "timestamp": 1.8708269641695874
+        },
+        {
+            "x": 5.627822525147453,
+            "y": 2.9665733818436117,
+            "heading": -3.339049219680213,
+            "angularVelocity": -2.574220136164825e-7,
+            "velocityX": -3.781913932547634,
+            "velocityY": 0.08812268977200981,
+            "timestamp": 1.9496089286300347
+        },
+        {
+            "x": 5.329875916862242,
+            "y": 2.9735158922681233,
+            "heading": -3.3390492399604246,
+            "angularVelocity": -2.5742201457831597e-7,
+            "velocityX": -3.7819139231390566,
+            "velocityY": 0.08812309355394705,
+            "timestamp": 2.028390893090482
+        },
+        {
+            "x": 5.031929325692052,
+            "y": 2.9804591371189906,
+            "heading": -3.3390492602433595,
+            "angularVelocity": -2.5745657158705766e-7,
+            "velocityX": -3.781913705893629,
+            "velocityY": 0.08813241581901742,
+            "timestamp": 2.107172857550929
+        },
+        {
+            "x": 4.775295960622385,
+            "y": 2.98713582308431,
+            "heading": -3.452004590816206,
+            "angularVelocity": -1.4337714392683885,
+            "velocityX": -3.2575141636447755,
+            "velocityY": 0.08474891443804398,
+            "timestamp": 2.1859548220113765
+        },
+        {
+            "x": 4.570259992018404,
+            "y": 2.9923299087295048,
+            "heading": -3.5517317371257473,
+            "angularVelocity": -1.2658626500689703,
+            "velocityX": -2.6025749676110213,
+            "velocityY": 0.06592988231212166,
+            "timestamp": 2.2647367864718237
+        },
+        {
+            "x": 4.416588409818771,
+            "y": 2.996180871589336,
+            "heading": -3.6294028651739794,
+            "angularVelocity": -0.9858998640130027,
+            "velocityX": -1.9505934289920475,
+            "velocityY": 0.04888127487306719,
+            "timestamp": 2.343518750932271
+        },
+        {
+            "x": 4.314185287460311,
+            "y": 2.998730700935922,
+            "heading": -3.6823213519430347,
+            "angularVelocity": -0.6717081394387356,
+            "velocityX": -1.2998295112312361,
+            "velocityY": 0.03236564820448933,
+            "timestamp": 2.4223007153927183
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -3.7091853071795864,
+            "angularVelocity": -0.3409911826968791,
+            "velocityX": -0.6497081890615813,
+            "velocityY": 0.01611154371144828,
+            "timestamp": 2.5010826798531656
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -3.7091853071795864,
+            "angularVelocity": 0,
+            "velocityX": -2.823998043085552e-35,
+            "velocityY": 0,
+            "timestamp": 2.579864644313613
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S3.traj
new file mode 100644
index 00000000..77d8e571
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S3.traj	
@@ -0,0 +1,310 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 2.6904386896052926e-33,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.221431258578075,
+            "y": 6.6088996918922325,
+            "heading": -2.8866746127276346,
+            "angularVelocity": -0.08608437747729661,
+            "velocityX": 0.5859398548340703,
+            "velocityY": -0.2721366710185261,
+            "timestamp": 0.07753570303037556
+        },
+        {
+            "x": 5.31229377552925,
+            "y": 6.566699082350374,
+            "heading": -2.90002400824425,
+            "angularVelocity": -0.17217094828412036,
+            "velocityX": 1.1718797070244922,
+            "velocityY": -0.5442732559647441,
+            "timestamp": 0.15507140606075112
+        },
+        {
+            "x": 5.448587549407382,
+            "y": 6.503398181602668,
+            "heading": -2.920048533381972,
+            "angularVelocity": -0.2582619922834353,
+            "velocityX": 1.7578195405636137,
+            "velocityY": -0.8164097089944291,
+            "timestamp": 0.2326071090911267
+        },
+        {
+            "x": 5.630312575309777,
+            "y": 6.418997003981843,
+            "heading": -2.946748763466125,
+            "angularVelocity": -0.34436045641700236,
+            "velocityX": 2.3437593108713033,
+            "velocityY": -1.0885459771707946,
+            "timestamp": 0.31014281212150224
+        },
+        {
+            "x": 5.85746883610922,
+            "y": 6.313495571165909,
+            "heading": -2.9801255206908306,
+            "angularVelocity": -0.43046952462183896,
+            "velocityX": 2.929698860284435,
+            "velocityY": -1.3606819657597289,
+            "timestamp": 0.3876785151518778
+        },
+        {
+            "x": 6.123490041046321,
+            "y": 6.189943134833017,
+            "heading": -2.9801255253705285,
+            "angularVelocity": -6.035539216471427e-8,
+            "velocityX": 3.4309510914331183,
+            "velocityY": -1.5934908887649881,
+            "timestamp": 0.46521421818225334
+        },
+        {
+            "x": 6.3895112454442975,
+            "y": 6.066390697338873,
+            "heading": -2.980125530050055,
+            "angularVelocity": -6.035318736040994e-8,
+            "velocityX": 3.4309510844798714,
+            "velocityY": -1.5934909037419929,
+            "timestamp": 0.5427499212126289
+        },
+        {
+            "x": 6.655532416724182,
+            "y": 5.9428381885380706,
+            "heading": -2.980125534729582,
+            "angularVelocity": -6.035318830910377e-8,
+            "velocityX": 3.4309506573464303,
+            "velocityY": -1.5934918234042366,
+            "timestamp": 0.6202856242430045
+        },
+        {
+            "x": 6.916684100099049,
+            "y": 5.8093006126159725,
+            "heading": -2.9801255394461488,
+            "angularVelocity": -6.083090321698073e-8,
+            "velocityX": 3.368147487778084,
+            "velocityY": -1.722272072128935,
+            "timestamp": 0.6978213272733801
+        },
+        {
+            "x": 7.151284630214051,
+            "y": 5.6332464271795715,
+            "heading": -2.980125544587418,
+            "angularVelocity": -6.630841173330857e-8,
+            "velocityX": 3.0257097175361216,
+            "velocityY": -2.2706208695551444,
+            "timestamp": 0.7753570303037557
+        },
+        {
+            "x": 7.352481762263237,
+            "y": 5.41981682210962,
+            "heading": -2.9801255509932516,
+            "angularVelocity": -8.261785689390399e-8,
+            "velocityX": 2.594896598414324,
+            "velocityY": -2.7526622798059472,
+            "timestamp": 0.8528927333341313
+        },
+        {
+            "x": 7.514399141163384,
+            "y": 5.175245297922962,
+            "heading": -2.9801256025732923,
+            "angularVelocity": -6.652424424834864e-7,
+            "velocityX": 2.0882944575444746,
+            "velocityY": -3.1543084621396242,
+            "timestamp": 0.9304284363645069
+        },
+        {
+            "x": 7.630164379984533,
+            "y": 4.911565821303128,
+            "heading": -2.995124870089402,
+            "angularVelocity": -0.19344981640565623,
+            "velocityX": 1.4930571890964461,
+            "velocityY": -3.4007491557345486,
+            "timestamp": 1.0079641393948824
+        },
+        {
+            "x": 7.696376897938806,
+            "y": 4.643893289420577,
+            "heading": -3.0332372938699343,
+            "angularVelocity": -0.49154676221354465,
+            "velocityX": 0.8539616636781268,
+            "velocityY": -3.452248724406194,
+            "timestamp": 1.085499842425258
+        },
+        {
+            "x": 7.712814001135022,
+            "y": 4.380911639847311,
+            "heading": -3.084305372604304,
+            "angularVelocity": -0.6586395265464133,
+            "velocityX": 0.2119939918488342,
+            "velocityY": -3.3917490819711635,
+            "timestamp": 1.1630355454556336
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.7593912516949146,
+            "velocityX": -0.4232114993806923,
+            "velocityY": -3.274770588561481,
+            "timestamp": 1.2405712484860092
+        },
+        {
+            "x": 7.5963416463862465,
+            "y": 3.881029306471945,
+            "heading": -3.207914317977363,
+            "angularVelocity": -0.8216221979368662,
+            "velocityX": -1.061897277970947,
+            "velocityY": -3.1221700958161,
+            "timestamp": 1.3193532129464565
+        },
+        {
+            "x": 7.463114472360868,
+            "y": 3.64992032438806,
+            "heading": -3.2707770828701133,
+            "angularVelocity": -0.7979334524504146,
+            "velocityX": -1.6910872296446224,
+            "velocityY": -2.933526520526327,
+            "timestamp": 1.3981351774069037
+        },
+        {
+            "x": 7.282107143226341,
+            "y": 3.438334424773196,
+            "heading": -3.3202070228970326,
+            "angularVelocity": -0.627427106768014,
+            "velocityX": -2.297573186632101,
+            "velocityY": -2.6857149483888696,
+            "timestamp": 1.476917141867351
+        },
+        {
+            "x": 7.057858991686637,
+            "y": 3.2538746449727443,
+            "heading": -3.3390490142356852,
+            "angularVelocity": -0.23916630497468983,
+            "velocityX": -2.846440210973517,
+            "velocityY": -2.341396042403339,
+            "timestamp": 1.5556991063277983
+        },
+        {
+            "x": 6.798499862682402,
+            "y": 3.1070645806670387,
+            "heading": -3.339049130726268,
+            "angularVelocity": -0.0000014786453189461519,
+            "velocityX": -3.2921129954108745,
+            "velocityY": -1.8634983947298314,
+            "timestamp": 1.6344810707882456
+        },
+        {
+            "x": 6.517654165139062,
+            "y": 3.0073345934329407,
+            "heading": -3.339049157320925,
+            "angularVelocity": -3.3757290550660805e-7,
+            "velocityX": -3.564847607783876,
+            "velocityY": -1.26589871066451,
+            "timestamp": 1.7132630352486928
+        },
+        {
+            "x": 6.223790307061648,
+            "y": 2.9576917507277325,
+            "heading": -3.339049179064471,
+            "angularVelocity": -2.7599649426865103e-7,
+            "velocityX": -3.730090511070579,
+            "velocityY": -0.6301295359311817,
+            "timestamp": 1.79204499970914
+        },
+        {
+            "x": 5.925769134173891,
+            "y": 2.9596309032298342,
+            "heading": -3.3390491994000007,
+            "angularVelocity": -2.581241798261079e-7,
+            "velocityX": -3.7828603910655114,
+            "velocityY": 0.024614167917522748,
+            "timestamp": 1.8708269641695874
+        },
+        {
+            "x": 5.627822525147453,
+            "y": 2.9665733818436117,
+            "heading": -3.339049219680213,
+            "angularVelocity": -2.574220136164825e-7,
+            "velocityX": -3.781913932547634,
+            "velocityY": 0.08812268977200981,
+            "timestamp": 1.9496089286300347
+        },
+        {
+            "x": 5.329875916862242,
+            "y": 2.9735158922681233,
+            "heading": -3.3390492399604246,
+            "angularVelocity": -2.5742201457831597e-7,
+            "velocityX": -3.7819139231390566,
+            "velocityY": 0.08812309355394705,
+            "timestamp": 2.028390893090482
+        },
+        {
+            "x": 5.031929325692052,
+            "y": 2.9804591371189906,
+            "heading": -3.3390492602433595,
+            "angularVelocity": -2.5745657158705766e-7,
+            "velocityX": -3.781913705893629,
+            "velocityY": 0.08813241581901742,
+            "timestamp": 2.107172857550929
+        },
+        {
+            "x": 4.775295960622385,
+            "y": 2.98713582308431,
+            "heading": -3.452004590816206,
+            "angularVelocity": -1.4337714392683885,
+            "velocityX": -3.2575141636447755,
+            "velocityY": 0.08474891443804398,
+            "timestamp": 2.1859548220113765
+        },
+        {
+            "x": 4.570259992018404,
+            "y": 2.9923299087295048,
+            "heading": -3.5517317371257473,
+            "angularVelocity": -1.2658626500689703,
+            "velocityX": -2.6025749676110213,
+            "velocityY": 0.06592988231212166,
+            "timestamp": 2.2647367864718237
+        },
+        {
+            "x": 4.416588409818771,
+            "y": 2.996180871589336,
+            "heading": -3.6294028651739794,
+            "angularVelocity": -0.9858998640130027,
+            "velocityX": -1.9505934289920475,
+            "velocityY": 0.04888127487306719,
+            "timestamp": 2.343518750932271
+        },
+        {
+            "x": 4.314185287460311,
+            "y": 2.998730700935922,
+            "heading": -3.6823213519430347,
+            "angularVelocity": -0.6717081394387356,
+            "velocityX": -1.2998295112312361,
+            "velocityY": 0.03236564820448933,
+            "timestamp": 2.4223007153927183
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -3.7091853071795864,
+            "angularVelocity": -0.3409911826968791,
+            "velocityX": -0.6497081890615813,
+            "velocityY": 0.01611154371144828,
+            "timestamp": 2.5010826798531656
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -3.7091853071795864,
+            "angularVelocity": 0,
+            "velocityX": -2.823998043085552e-35,
+            "velocityY": 0,
+            "timestamp": 2.579864644313613
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S4.1.traj
new file mode 100644
index 00000000..4d0afc34
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S4.1.traj	
@@ -0,0 +1,319 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": -4.965302317163387e-33,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.214448464194176,
+            "y": 6.6065493619277555,
+            "heading": -2.899983532805956,
+            "angularVelocity": -0.2708663369562031,
+            "velocityX": 0.5211488258354376,
+            "velocityY": -0.31786113574578073,
+            "timestamp": 0.0737763615461288
+        },
+        {
+            "x": 5.29140014559419,
+            "y": 6.559619691972876,
+            "heading": -2.9381129725152273,
+            "angularVelocity": -0.5168246157738562,
+            "velocityX": 1.0430398001113212,
+            "velocityY": -0.636107134743106,
+            "timestamp": 0.1475527230922576
+        },
+        {
+            "x": 5.4069285579459345,
+            "y": 6.489171539636289,
+            "heading": -2.991543589570423,
+            "angularVelocity": -0.7242240730696432,
+            "velocityX": 1.5659272147693208,
+            "velocityY": -0.9548878646250154,
+            "timestamp": 0.22132908463838638
+        },
+        {
+            "x": 5.5611322195100845,
+            "y": 6.395152264966145,
+            "heading": -3.0552740925010653,
+            "angularVelocity": -0.8638336398684346,
+            "velocityX": 2.090149992931468,
+            "velocityY": -1.274382101526624,
+            "timestamp": 0.2951054461845152
+        },
+        {
+            "x": 5.754097394020428,
+            "y": 6.277527348774539,
+            "heading": -3.118382886347054,
+            "angularVelocity": -0.8554066983437507,
+            "velocityX": 2.615542030894161,
+            "velocityY": -1.5943442279687556,
+            "timestamp": 0.36888180773064394
+        },
+        {
+            "x": 5.984744637231227,
+            "y": 6.137241174757932,
+            "heading": -3.141361477565195,
+            "angularVelocity": -0.3114627874915312,
+            "velocityX": 3.126302766592624,
+            "velocityY": -1.9015057272632323,
+            "timestamp": 0.4426581692767727
+        },
+        {
+            "x": 6.222972456839802,
+            "y": 5.99184648556835,
+            "heading": -3.141361501066067,
+            "angularVelocity": -3.1854203464323085e-7,
+            "velocityX": 3.2290535154627107,
+            "velocityY": -1.9707489789757882,
+            "timestamp": 0.5164345308229015
+        },
+        {
+            "x": 6.461200274584459,
+            "y": 5.84645179332473,
+            "heading": -3.141361524566912,
+            "angularVelocity": -3.1854166338718743e-7,
+            "velocityX": 3.229053490198278,
+            "velocityY": -1.9707490203716698,
+            "timestamp": 0.5902108923690302
+        },
+        {
+            "x": 6.699428092323813,
+            "y": 5.701057101072421,
+            "heading": -3.141361548067757,
+            "angularVelocity": -3.1854166408242085e-7,
+            "velocityX": 3.229053490126394,
+            "velocityY": -1.9707490204894516,
+            "timestamp": 0.663987253915159
+        },
+        {
+            "x": 6.937655250211389,
+            "y": 5.555661327664803,
+            "heading": -3.1413615715686114,
+            "angularVelocity": -3.1854179676885927e-7,
+            "velocityX": 3.229044546180602,
+            "velocityY": -1.9707636749842845,
+            "timestamp": 0.7377636154612878
+        },
+        {
+            "x": 7.1538386137488,
+            "y": 5.379148261525069,
+            "heading": -3.141361595947866,
+            "angularVelocity": -3.304480485706385e-7,
+            "velocityX": 2.9302524413899382,
+            "velocityY": -2.3925423054289428,
+            "timestamp": 0.8115399770074165
+        },
+        {
+            "x": 7.338555522796062,
+            "y": 5.1699312191681095,
+            "heading": -3.1413616242387703,
+            "angularVelocity": -3.834684085405442e-7,
+            "velocityX": 2.5037411059064913,
+            "velocityY": -2.835827600770833,
+            "timestamp": 0.8853163385535453
+        },
+        {
+            "x": 7.486921372318085,
+            "y": 4.933542391468771,
+            "heading": -3.14136166220073,
+            "angularVelocity": -5.145545098256939e-7,
+            "velocityX": 2.011021503537517,
+            "velocityY": -3.2041269418191103,
+            "timestamp": 0.9590927000996741
+        },
+        {
+            "x": 7.595012870290821,
+            "y": 4.676232612674377,
+            "heading": -3.1413617253677457,
+            "angularVelocity": -8.561958656088552e-7,
+            "velocityX": 1.4651237294367088,
+            "velocityY": -3.4876994934686665,
+            "timestamp": 1.032869061645803
+        },
+        {
+            "x": 7.659971709721358,
+            "y": 4.404806001492882,
+            "heading": -3.141361954512953,
+            "angularVelocity": -0.000003105943453461285,
+            "velocityX": 0.8804830987757537,
+            "velocityY": -3.679045774191308,
+            "timestamp": 1.1066454231919318
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.02471459188852694,
+            "velocityX": 0.271473001092883,
+            "velocityY": -3.7655150738110477,
+            "timestamp": 1.1804217847380607
+        },
+        {
+            "x": 7.658280036401298,
+            "y": 3.864604229609803,
+            "heading": -3.146329645491869,
+            "angularVelocity": -0.04501004755691485,
+            "velocityX": -0.3109132979403334,
+            "velocityY": -3.756099036118669,
+            "timestamp": 1.2502803714970885
+        },
+        {
+            "x": 7.5962523104298665,
+            "y": 3.6077169181356963,
+            "heading": -3.1463327455058754,
+            "angularVelocity": -0.00004437556140498802,
+            "velocityX": -0.8879041052661434,
+            "velocityY": -3.677247470811584,
+            "timestamp": 1.3201389582561163
+        },
+        {
+            "x": 7.495521811257821,
+            "y": 3.363396565109804,
+            "heading": -3.1463328962340493,
+            "angularVelocity": -0.0000021576184275855436,
+            "velocityX": -1.4419200823444587,
+            "velocityY": -3.4973560783394904,
+            "timestamp": 1.389997545015144
+        },
+        {
+            "x": 7.358477102372377,
+            "y": 3.1374367675877326,
+            "heading": -3.146332979999805,
+            "angularVelocity": -0.000001199076011915671,
+            "velocityX": -1.9617446507781449,
+            "velocityY": -3.234531472866801,
+            "timestamp": 1.4598561317741718
+        },
+        {
+            "x": 7.188367445952169,
+            "y": 2.9351948516310506,
+            "heading": -3.1463330407624492,
+            "angularVelocity": -8.697949218469567e-7,
+            "velocityX": -2.4350572250622284,
+            "velocityY": -2.895018713365922,
+            "timestamp": 1.5297147185331996
+        },
+        {
+            "x": 6.989226108875858,
+            "y": 2.761465700266129,
+            "heading": -3.146333092561237,
+            "angularVelocity": -7.414806172090105e-7,
+            "velocityX": -2.8506350659974578,
+            "velocityY": -2.486868965216661,
+            "timestamp": 1.5995733052922274
+        },
+        {
+            "x": 6.765782539563676,
+            "y": 2.620355593747368,
+            "heading": -3.14633314264619,
+            "angularVelocity": -7.169476958064807e-7,
+            "velocityX": -3.19851259062737,
+            "velocityY": -2.0199393240734156,
+            "timestamp": 1.6694318920512552
+        },
+        {
+            "x": 6.5419983548650125,
+            "y": 2.479786288735506,
+            "heading": -3.1463331927321803,
+            "angularVelocity": -7.169625493020222e-7,
+            "velocityX": -3.203388374726096,
+            "velocityY": -2.012197949219134,
+            "timestamp": 1.739290478810283
+        },
+        {
+            "x": 6.318214167123251,
+            "y": 2.339216988568208,
+            "heading": -3.1463332428181707,
+            "angularVelocity": -7.16962547152982e-7,
+            "velocityX": -3.2033884182869192,
+            "velocityY": -2.012197879870968,
+            "timestamp": 1.8091490655693108
+        },
+        {
+            "x": 6.094429979379188,
+            "y": 2.198647688404573,
+            "heading": -3.1463332929041607,
+            "angularVelocity": -7.1696254162218e-7,
+            "velocityX": -3.2033884183198493,
+            "velocityY": -2.0121978798185434,
+            "timestamp": 1.8790076523283386
+        },
+        {
+            "x": 5.870645537599717,
+            "y": 2.05807879266491,
+            "heading": -3.1463333429939095,
+            "angularVelocity": -7.170163458750797e-7,
+            "velocityX": -3.203392054743378,
+            "velocityY": -2.012192090637989,
+            "timestamp": 1.9488662390873663
+        },
+        {
+            "x": 5.673381673927696,
+            "y": 1.935533968246103,
+            "heading": -3.225487827303808,
+            "angularVelocity": -1.1330673576740993,
+            "velocityX": -2.8237597240904497,
+            "velocityY": -1.754184132603728,
+            "timestamp": 2.018724825846394
+        },
+        {
+            "x": 5.505993389711308,
+            "y": 1.8298743063416731,
+            "heading": -3.3794217717080413,
+            "angularVelocity": -2.2035078512999076,
+            "velocityX": -2.396101781929577,
+            "velocityY": -1.512479235643509,
+            "timestamp": 2.088583412605422
+        },
+        {
+            "x": 5.373136379336833,
+            "y": 1.745695395482932,
+            "heading": -3.5283087444702135,
+            "angularVelocity": -2.1312623067475838,
+            "velocityX": -1.9017992853585055,
+            "velocityY": -1.2049901775011624,
+            "timestamp": 2.15844199936445
+        },
+        {
+            "x": 5.274205775679982,
+            "y": 1.6827613854161347,
+            "heading": -3.651053741383974,
+            "angularVelocity": -1.7570495283158336,
+            "velocityX": -1.4161552394139385,
+            "velocityY": -0.9008772290782794,
+            "timestamp": 2.228300586123478
+        },
+        {
+            "x": 5.2086365166677675,
+            "y": 1.6409004652612855,
+            "heading": -3.73798062119163,
+            "angularVelocity": -1.2443263432669185,
+            "velocityX": -0.9385998494128075,
+            "velocityY": -0.5992236902708281,
+            "timestamp": 2.298159172882506
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -3.7831853071795862,
+            "angularVelocity": -0.6470884695089293,
+            "velocityX": -0.46717974384944866,
+            "velocityY": -0.29918248036394063,
+            "timestamp": 2.368017759641534
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -3.7831853071795862,
+            "angularVelocity": 0,
+            "velocityX": -3.585209340915008e-32,
+            "velocityY": 0,
+            "timestamp": 2.437876346400562
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S4.traj
new file mode 100644
index 00000000..4d0afc34
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S4.traj	
@@ -0,0 +1,319 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": -4.965302317163387e-33,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.214448464194176,
+            "y": 6.6065493619277555,
+            "heading": -2.899983532805956,
+            "angularVelocity": -0.2708663369562031,
+            "velocityX": 0.5211488258354376,
+            "velocityY": -0.31786113574578073,
+            "timestamp": 0.0737763615461288
+        },
+        {
+            "x": 5.29140014559419,
+            "y": 6.559619691972876,
+            "heading": -2.9381129725152273,
+            "angularVelocity": -0.5168246157738562,
+            "velocityX": 1.0430398001113212,
+            "velocityY": -0.636107134743106,
+            "timestamp": 0.1475527230922576
+        },
+        {
+            "x": 5.4069285579459345,
+            "y": 6.489171539636289,
+            "heading": -2.991543589570423,
+            "angularVelocity": -0.7242240730696432,
+            "velocityX": 1.5659272147693208,
+            "velocityY": -0.9548878646250154,
+            "timestamp": 0.22132908463838638
+        },
+        {
+            "x": 5.5611322195100845,
+            "y": 6.395152264966145,
+            "heading": -3.0552740925010653,
+            "angularVelocity": -0.8638336398684346,
+            "velocityX": 2.090149992931468,
+            "velocityY": -1.274382101526624,
+            "timestamp": 0.2951054461845152
+        },
+        {
+            "x": 5.754097394020428,
+            "y": 6.277527348774539,
+            "heading": -3.118382886347054,
+            "angularVelocity": -0.8554066983437507,
+            "velocityX": 2.615542030894161,
+            "velocityY": -1.5943442279687556,
+            "timestamp": 0.36888180773064394
+        },
+        {
+            "x": 5.984744637231227,
+            "y": 6.137241174757932,
+            "heading": -3.141361477565195,
+            "angularVelocity": -0.3114627874915312,
+            "velocityX": 3.126302766592624,
+            "velocityY": -1.9015057272632323,
+            "timestamp": 0.4426581692767727
+        },
+        {
+            "x": 6.222972456839802,
+            "y": 5.99184648556835,
+            "heading": -3.141361501066067,
+            "angularVelocity": -3.1854203464323085e-7,
+            "velocityX": 3.2290535154627107,
+            "velocityY": -1.9707489789757882,
+            "timestamp": 0.5164345308229015
+        },
+        {
+            "x": 6.461200274584459,
+            "y": 5.84645179332473,
+            "heading": -3.141361524566912,
+            "angularVelocity": -3.1854166338718743e-7,
+            "velocityX": 3.229053490198278,
+            "velocityY": -1.9707490203716698,
+            "timestamp": 0.5902108923690302
+        },
+        {
+            "x": 6.699428092323813,
+            "y": 5.701057101072421,
+            "heading": -3.141361548067757,
+            "angularVelocity": -3.1854166408242085e-7,
+            "velocityX": 3.229053490126394,
+            "velocityY": -1.9707490204894516,
+            "timestamp": 0.663987253915159
+        },
+        {
+            "x": 6.937655250211389,
+            "y": 5.555661327664803,
+            "heading": -3.1413615715686114,
+            "angularVelocity": -3.1854179676885927e-7,
+            "velocityX": 3.229044546180602,
+            "velocityY": -1.9707636749842845,
+            "timestamp": 0.7377636154612878
+        },
+        {
+            "x": 7.1538386137488,
+            "y": 5.379148261525069,
+            "heading": -3.141361595947866,
+            "angularVelocity": -3.304480485706385e-7,
+            "velocityX": 2.9302524413899382,
+            "velocityY": -2.3925423054289428,
+            "timestamp": 0.8115399770074165
+        },
+        {
+            "x": 7.338555522796062,
+            "y": 5.1699312191681095,
+            "heading": -3.1413616242387703,
+            "angularVelocity": -3.834684085405442e-7,
+            "velocityX": 2.5037411059064913,
+            "velocityY": -2.835827600770833,
+            "timestamp": 0.8853163385535453
+        },
+        {
+            "x": 7.486921372318085,
+            "y": 4.933542391468771,
+            "heading": -3.14136166220073,
+            "angularVelocity": -5.145545098256939e-7,
+            "velocityX": 2.011021503537517,
+            "velocityY": -3.2041269418191103,
+            "timestamp": 0.9590927000996741
+        },
+        {
+            "x": 7.595012870290821,
+            "y": 4.676232612674377,
+            "heading": -3.1413617253677457,
+            "angularVelocity": -8.561958656088552e-7,
+            "velocityX": 1.4651237294367088,
+            "velocityY": -3.4876994934686665,
+            "timestamp": 1.032869061645803
+        },
+        {
+            "x": 7.659971709721358,
+            "y": 4.404806001492882,
+            "heading": -3.141361954512953,
+            "angularVelocity": -0.000003105943453461285,
+            "velocityX": 0.8804830987757537,
+            "velocityY": -3.679045774191308,
+            "timestamp": 1.1066454231919318
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.02471459188852694,
+            "velocityX": 0.271473001092883,
+            "velocityY": -3.7655150738110477,
+            "timestamp": 1.1804217847380607
+        },
+        {
+            "x": 7.658280036401298,
+            "y": 3.864604229609803,
+            "heading": -3.146329645491869,
+            "angularVelocity": -0.04501004755691485,
+            "velocityX": -0.3109132979403334,
+            "velocityY": -3.756099036118669,
+            "timestamp": 1.2502803714970885
+        },
+        {
+            "x": 7.5962523104298665,
+            "y": 3.6077169181356963,
+            "heading": -3.1463327455058754,
+            "angularVelocity": -0.00004437556140498802,
+            "velocityX": -0.8879041052661434,
+            "velocityY": -3.677247470811584,
+            "timestamp": 1.3201389582561163
+        },
+        {
+            "x": 7.495521811257821,
+            "y": 3.363396565109804,
+            "heading": -3.1463328962340493,
+            "angularVelocity": -0.0000021576184275855436,
+            "velocityX": -1.4419200823444587,
+            "velocityY": -3.4973560783394904,
+            "timestamp": 1.389997545015144
+        },
+        {
+            "x": 7.358477102372377,
+            "y": 3.1374367675877326,
+            "heading": -3.146332979999805,
+            "angularVelocity": -0.000001199076011915671,
+            "velocityX": -1.9617446507781449,
+            "velocityY": -3.234531472866801,
+            "timestamp": 1.4598561317741718
+        },
+        {
+            "x": 7.188367445952169,
+            "y": 2.9351948516310506,
+            "heading": -3.1463330407624492,
+            "angularVelocity": -8.697949218469567e-7,
+            "velocityX": -2.4350572250622284,
+            "velocityY": -2.895018713365922,
+            "timestamp": 1.5297147185331996
+        },
+        {
+            "x": 6.989226108875858,
+            "y": 2.761465700266129,
+            "heading": -3.146333092561237,
+            "angularVelocity": -7.414806172090105e-7,
+            "velocityX": -2.8506350659974578,
+            "velocityY": -2.486868965216661,
+            "timestamp": 1.5995733052922274
+        },
+        {
+            "x": 6.765782539563676,
+            "y": 2.620355593747368,
+            "heading": -3.14633314264619,
+            "angularVelocity": -7.169476958064807e-7,
+            "velocityX": -3.19851259062737,
+            "velocityY": -2.0199393240734156,
+            "timestamp": 1.6694318920512552
+        },
+        {
+            "x": 6.5419983548650125,
+            "y": 2.479786288735506,
+            "heading": -3.1463331927321803,
+            "angularVelocity": -7.169625493020222e-7,
+            "velocityX": -3.203388374726096,
+            "velocityY": -2.012197949219134,
+            "timestamp": 1.739290478810283
+        },
+        {
+            "x": 6.318214167123251,
+            "y": 2.339216988568208,
+            "heading": -3.1463332428181707,
+            "angularVelocity": -7.16962547152982e-7,
+            "velocityX": -3.2033884182869192,
+            "velocityY": -2.012197879870968,
+            "timestamp": 1.8091490655693108
+        },
+        {
+            "x": 6.094429979379188,
+            "y": 2.198647688404573,
+            "heading": -3.1463332929041607,
+            "angularVelocity": -7.1696254162218e-7,
+            "velocityX": -3.2033884183198493,
+            "velocityY": -2.0121978798185434,
+            "timestamp": 1.8790076523283386
+        },
+        {
+            "x": 5.870645537599717,
+            "y": 2.05807879266491,
+            "heading": -3.1463333429939095,
+            "angularVelocity": -7.170163458750797e-7,
+            "velocityX": -3.203392054743378,
+            "velocityY": -2.012192090637989,
+            "timestamp": 1.9488662390873663
+        },
+        {
+            "x": 5.673381673927696,
+            "y": 1.935533968246103,
+            "heading": -3.225487827303808,
+            "angularVelocity": -1.1330673576740993,
+            "velocityX": -2.8237597240904497,
+            "velocityY": -1.754184132603728,
+            "timestamp": 2.018724825846394
+        },
+        {
+            "x": 5.505993389711308,
+            "y": 1.8298743063416731,
+            "heading": -3.3794217717080413,
+            "angularVelocity": -2.2035078512999076,
+            "velocityX": -2.396101781929577,
+            "velocityY": -1.512479235643509,
+            "timestamp": 2.088583412605422
+        },
+        {
+            "x": 5.373136379336833,
+            "y": 1.745695395482932,
+            "heading": -3.5283087444702135,
+            "angularVelocity": -2.1312623067475838,
+            "velocityX": -1.9017992853585055,
+            "velocityY": -1.2049901775011624,
+            "timestamp": 2.15844199936445
+        },
+        {
+            "x": 5.274205775679982,
+            "y": 1.6827613854161347,
+            "heading": -3.651053741383974,
+            "angularVelocity": -1.7570495283158336,
+            "velocityX": -1.4161552394139385,
+            "velocityY": -0.9008772290782794,
+            "timestamp": 2.228300586123478
+        },
+        {
+            "x": 5.2086365166677675,
+            "y": 1.6409004652612855,
+            "heading": -3.73798062119163,
+            "angularVelocity": -1.2443263432669185,
+            "velocityX": -0.9385998494128075,
+            "velocityY": -0.5992236902708281,
+            "timestamp": 2.298159172882506
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -3.7831853071795862,
+            "angularVelocity": -0.6470884695089293,
+            "velocityX": -0.46717974384944866,
+            "velocityY": -0.29918248036394063,
+            "timestamp": 2.368017759641534
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -3.7831853071795862,
+            "angularVelocity": 0,
+            "velocityX": -3.585209340915008e-32,
+            "velocityY": 0,
+            "timestamp": 2.437876346400562
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj
new file mode 100644
index 00000000..7043ffc8
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 1.76189972977325e-30,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.102979050763036,
+            "y": 6.633224483540667,
+            "heading": -2.873498740443943,
+            "angularVelocity": 0.06942713881135329,
+            "velocityX": -0.779792828619582,
+            "velocityY": 0.034434352980739984,
+            "timestamp": 0.09364147316695418
+        },
+        {
+            "x": 4.956937154228187,
+            "y": 6.639673450002842,
+            "heading": -2.8604964293774233,
+            "angularVelocity": 0.138852055897772,
+            "velocityX": -1.559585636531679,
+            "velocityY": 0.06886869934946069,
+            "timestamp": 0.18728294633390835
+        },
+        {
+            "x": 4.73787431448253,
+            "y": 6.649346898579915,
+            "heading": -2.840993426039055,
+            "angularVelocity": 0.20827313666458666,
+            "velocityX": -2.339378400797736,
+            "velocityY": 0.10330303710435738,
+            "timestamp": 0.28092441950086255
+        },
+        {
+            "x": 4.4457905464926135,
+            "y": 6.6622448281102296,
+            "heading": -2.814990271784832,
+            "angularVelocity": 0.27768843627487766,
+            "velocityX": -3.1191710052355526,
+            "velocityY": 0.13773736245389714,
+            "timestamp": 0.3745658926678167
+        },
+        {
+            "x": 4.091895298236837,
+            "y": 6.6778724148973385,
+            "heading": -2.8149902687925503,
+            "angularVelocity": 3.195462281879478e-8,
+            "velocityX": -3.779257590542345,
+            "velocityY": 0.16688745123911564,
+            "timestamp": 0.46820736583477085
+        },
+        {
+            "x": 3.7380000499851023,
+            "y": 6.693500001778431,
+            "heading": -2.814990265800417,
+            "angularVelocity": 3.1953066955491674e-8,
+            "velocityX": -3.779257590499122,
+            "velocityY": 0.16688745224276955,
+            "timestamp": 0.561848839001725
+        },
+        {
+            "x": 3.3841048017333675,
+            "y": 6.709127588659523,
+            "heading": -2.8149902628082843,
+            "angularVelocity": 3.1953067022179835e-8,
+            "velocityX": -3.779257590499122,
+            "velocityY": 0.1668874522427702,
+            "timestamp": 0.6554903121686791
+        },
+        {
+            "x": 3.030209553477566,
+            "y": 6.724755175446062,
+            "heading": -2.8149902598160015,
+            "angularVelocity": 3.195463264613959e-8,
+            "velocityX": -3.7792575905426142,
+            "velocityY": 0.16688745123301843,
+            "timestamp": 0.7491317853356333
+        },
+        {
+            "x": 2.7381257501719745,
+            "y": 6.737653104232043,
+            "heading": -2.7889939719160233,
+            "angularVelocity": 0.27761511027933256,
+            "velocityX": -3.1191713823726643,
+            "velocityY": 0.13773735450514524,
+            "timestamp": 0.8427732585025874
+        },
+        {
+            "x": 2.5190628796730365,
+            "y": 6.747326551843937,
+            "heading": -2.769496951872836,
+            "angularVelocity": 0.2082092408823168,
+            "velocityX": -2.3393787292129176,
+            "velocityY": 0.10330302679718467,
+            "timestamp": 0.9364147316695416
+        },
+        {
+            "x": 2.373020960810499,
+            "y": 6.753775517241458,
+            "heading": -2.7564989943923397,
+            "angularVelocity": 0.13880556382672637,
+            "velocityX": -1.5595858749697047,
+            "velocityY": 0.06886868797998212,
+            "timestamp": 1.0300562048364958
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0.06940294906251296,
+            "velocityX": -0.779792952213697,
+            "velocityY": 0.03443434462839962,
+            "timestamp": 1.12369767800345
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 1.276683149554956e-30,
+            "velocityX": 2.9209786111644144e-32,
+            "velocityY": 0,
+            "timestamp": 1.2173391511704041
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.traj
new file mode 100644
index 00000000..7043ffc8
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 1.76189972977325e-30,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.102979050763036,
+            "y": 6.633224483540667,
+            "heading": -2.873498740443943,
+            "angularVelocity": 0.06942713881135329,
+            "velocityX": -0.779792828619582,
+            "velocityY": 0.034434352980739984,
+            "timestamp": 0.09364147316695418
+        },
+        {
+            "x": 4.956937154228187,
+            "y": 6.639673450002842,
+            "heading": -2.8604964293774233,
+            "angularVelocity": 0.138852055897772,
+            "velocityX": -1.559585636531679,
+            "velocityY": 0.06886869934946069,
+            "timestamp": 0.18728294633390835
+        },
+        {
+            "x": 4.73787431448253,
+            "y": 6.649346898579915,
+            "heading": -2.840993426039055,
+            "angularVelocity": 0.20827313666458666,
+            "velocityX": -2.339378400797736,
+            "velocityY": 0.10330303710435738,
+            "timestamp": 0.28092441950086255
+        },
+        {
+            "x": 4.4457905464926135,
+            "y": 6.6622448281102296,
+            "heading": -2.814990271784832,
+            "angularVelocity": 0.27768843627487766,
+            "velocityX": -3.1191710052355526,
+            "velocityY": 0.13773736245389714,
+            "timestamp": 0.3745658926678167
+        },
+        {
+            "x": 4.091895298236837,
+            "y": 6.6778724148973385,
+            "heading": -2.8149902687925503,
+            "angularVelocity": 3.195462281879478e-8,
+            "velocityX": -3.779257590542345,
+            "velocityY": 0.16688745123911564,
+            "timestamp": 0.46820736583477085
+        },
+        {
+            "x": 3.7380000499851023,
+            "y": 6.693500001778431,
+            "heading": -2.814990265800417,
+            "angularVelocity": 3.1953066955491674e-8,
+            "velocityX": -3.779257590499122,
+            "velocityY": 0.16688745224276955,
+            "timestamp": 0.561848839001725
+        },
+        {
+            "x": 3.3841048017333675,
+            "y": 6.709127588659523,
+            "heading": -2.8149902628082843,
+            "angularVelocity": 3.1953067022179835e-8,
+            "velocityX": -3.779257590499122,
+            "velocityY": 0.1668874522427702,
+            "timestamp": 0.6554903121686791
+        },
+        {
+            "x": 3.030209553477566,
+            "y": 6.724755175446062,
+            "heading": -2.8149902598160015,
+            "angularVelocity": 3.195463264613959e-8,
+            "velocityX": -3.7792575905426142,
+            "velocityY": 0.16688745123301843,
+            "timestamp": 0.7491317853356333
+        },
+        {
+            "x": 2.7381257501719745,
+            "y": 6.737653104232043,
+            "heading": -2.7889939719160233,
+            "angularVelocity": 0.27761511027933256,
+            "velocityX": -3.1191713823726643,
+            "velocityY": 0.13773735450514524,
+            "timestamp": 0.8427732585025874
+        },
+        {
+            "x": 2.5190628796730365,
+            "y": 6.747326551843937,
+            "heading": -2.769496951872836,
+            "angularVelocity": 0.2082092408823168,
+            "velocityX": -2.3393787292129176,
+            "velocityY": 0.10330302679718467,
+            "timestamp": 0.9364147316695416
+        },
+        {
+            "x": 2.373020960810499,
+            "y": 6.753775517241458,
+            "heading": -2.7564989943923397,
+            "angularVelocity": 0.13880556382672637,
+            "velocityX": -1.5595858749697047,
+            "velocityY": 0.06886868797998212,
+            "timestamp": 1.0300562048364958
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0.06940294906251296,
+            "velocityX": -0.779792952213697,
+            "velocityY": 0.03443434462839962,
+            "timestamp": 1.12369767800345
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 1.276683149554956e-30,
+            "velocityX": 2.9209786111644144e-32,
+            "velocityY": 0,
+            "timestamp": 1.2173391511704041
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj
new file mode 100644
index 00000000..051fce47
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -1.6791892318677926e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 5.11892498821706,
+            "y": 6.609128402762145,
+            "heading": -2.8887795529976006,
+            "angularVelocity": -0.10279668380897901,
+            "velocityX": -0.6682711456093472,
+            "velocityY": -0.24437815711512118,
+            "timestamp": 0.08540696715387537
+        },
+        {
+            "x": 5.004774958946586,
+            "y": 6.5673852200358445,
+            "heading": -2.90633824182768,
+            "angularVelocity": -0.2055884831789443,
+            "velocityX": -1.3365423580117657,
+            "velocityY": -0.48875617666053534,
+            "timestamp": 0.17081393430775074
+        },
+        {
+            "x": 4.833549902172449,
+            "y": 6.504770468021476,
+            "heading": -2.93267521126463,
+            "angularVelocity": -0.30837026901446724,
+            "velocityX": -2.004813687689499,
+            "velocityY": -0.7331340065214745,
+            "timestamp": 0.2562209014616261
+        },
+        {
+            "x": 4.605249804128149,
+            "y": 6.4212841662523426,
+            "heading": -2.9677889698411977,
+            "angularVelocity": -0.4111345917869177,
+            "velocityX": -2.6730851785543184,
+            "velocityY": -0.9775116076738672,
+            "timestamp": 0.3416278686155015
+        },
+        {
+            "x": 4.319874653666952,
+            "y": 6.316926338018247,
+            "heading": -3.0116770321088655,
+            "angularVelocity": -0.5138698133209222,
+            "velocityX": -3.341356799932311,
+            "velocityY": -1.2218889361341865,
+            "timestamp": 0.42703483576937684
+        },
+        {
+            "x": 4.016437784224315,
+            "y": 6.205962942906548,
+            "heading": -3.011677037359934,
+            "angularVelocity": -6.148290543535782e-8,
+            "velocityX": -3.5528350854087023,
+            "velocityY": -1.2992311846383482,
+            "timestamp": 0.5124418029232523
+        },
+        {
+            "x": 3.713000914796963,
+            "y": 6.094999547753008,
+            "heading": -3.0116770426109816,
+            "angularVelocity": -6.148266047339023e-8,
+            "velocityX": -3.55283508522973,
+            "velocityY": -1.299231185128267,
+            "timestamp": 0.5978487700771277
+        },
+        {
+            "x": 3.4095640453696117,
+            "y": 5.984036152599468,
+            "heading": -3.011677047862029,
+            "angularVelocity": -6.148266099885158e-8,
+            "velocityX": -3.5528350852297295,
+            "velocityY": -1.2992311851282667,
+            "timestamp": 0.6832557372310031
+        },
+        {
+            "x": 3.1061271759243305,
+            "y": 5.873072757495,
+            "heading": -3.011677053113097,
+            "angularVelocity": -6.1482904284003e-8,
+            "velocityX": -3.5528350854396615,
+            "velocityY": -1.2992311845536897,
+            "timestamp": 0.7686627043848785
+        },
+        {
+            "x": 2.820751491800317,
+            "y": 5.768715178575412,
+            "heading": -3.055515411809828,
+            "angularVelocity": -0.513287851771491,
+            "velocityX": -3.341363048401663,
+            "velocityY": -1.2218860169986983,
+            "timestamp": 0.8540696715387539
+        },
+        {
+            "x": 2.592450909011727,
+            "y": 5.685229109517301,
+            "heading": -3.0905842313047507,
+            "angularVelocity": -0.41060841595908354,
+            "velocityX": -2.673090854253924,
+            "velocityY": -0.977508882942736,
+            "timestamp": 0.9394766386926293
+        },
+        {
+            "x": 2.421225457633144,
+            "y": 5.622614555370758,
+            "heading": -3.116884896329009,
+            "angularVelocity": -0.30794519347436133,
+            "velocityX": -2.00481830797353,
+            "velocityY": -0.7331316897570141,
+            "timestamp": 1.0248836058465047
+        },
+        {
+            "x": 2.3070751524559587,
+            "y": 5.5808715184580695,
+            "heading": -3.134418439279544,
+            "angularVelocity": -0.2052940589606165,
+            "velocityX": -1.336545588505958,
+            "velocityY": -0.4887544693804859,
+            "timestamp": 1.11029057300038
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.1026481584839197,
+            "velocityX": -0.668272792700015,
+            "velocityY": -0.24437723471044392,
+            "timestamp": 1.1956975401542553
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": -2.233003674732857e-32,
+            "velocityX": 0,
+            "velocityY": 6.615380928603251e-34,
+            "timestamp": 1.2811045073081306
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.traj
new file mode 100644
index 00000000..051fce47
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -1.6791892318677926e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 5.11892498821706,
+            "y": 6.609128402762145,
+            "heading": -2.8887795529976006,
+            "angularVelocity": -0.10279668380897901,
+            "velocityX": -0.6682711456093472,
+            "velocityY": -0.24437815711512118,
+            "timestamp": 0.08540696715387537
+        },
+        {
+            "x": 5.004774958946586,
+            "y": 6.5673852200358445,
+            "heading": -2.90633824182768,
+            "angularVelocity": -0.2055884831789443,
+            "velocityX": -1.3365423580117657,
+            "velocityY": -0.48875617666053534,
+            "timestamp": 0.17081393430775074
+        },
+        {
+            "x": 4.833549902172449,
+            "y": 6.504770468021476,
+            "heading": -2.93267521126463,
+            "angularVelocity": -0.30837026901446724,
+            "velocityX": -2.004813687689499,
+            "velocityY": -0.7331340065214745,
+            "timestamp": 0.2562209014616261
+        },
+        {
+            "x": 4.605249804128149,
+            "y": 6.4212841662523426,
+            "heading": -2.9677889698411977,
+            "angularVelocity": -0.4111345917869177,
+            "velocityX": -2.6730851785543184,
+            "velocityY": -0.9775116076738672,
+            "timestamp": 0.3416278686155015
+        },
+        {
+            "x": 4.319874653666952,
+            "y": 6.316926338018247,
+            "heading": -3.0116770321088655,
+            "angularVelocity": -0.5138698133209222,
+            "velocityX": -3.341356799932311,
+            "velocityY": -1.2218889361341865,
+            "timestamp": 0.42703483576937684
+        },
+        {
+            "x": 4.016437784224315,
+            "y": 6.205962942906548,
+            "heading": -3.011677037359934,
+            "angularVelocity": -6.148290543535782e-8,
+            "velocityX": -3.5528350854087023,
+            "velocityY": -1.2992311846383482,
+            "timestamp": 0.5124418029232523
+        },
+        {
+            "x": 3.713000914796963,
+            "y": 6.094999547753008,
+            "heading": -3.0116770426109816,
+            "angularVelocity": -6.148266047339023e-8,
+            "velocityX": -3.55283508522973,
+            "velocityY": -1.299231185128267,
+            "timestamp": 0.5978487700771277
+        },
+        {
+            "x": 3.4095640453696117,
+            "y": 5.984036152599468,
+            "heading": -3.011677047862029,
+            "angularVelocity": -6.148266099885158e-8,
+            "velocityX": -3.5528350852297295,
+            "velocityY": -1.2992311851282667,
+            "timestamp": 0.6832557372310031
+        },
+        {
+            "x": 3.1061271759243305,
+            "y": 5.873072757495,
+            "heading": -3.011677053113097,
+            "angularVelocity": -6.1482904284003e-8,
+            "velocityX": -3.5528350854396615,
+            "velocityY": -1.2992311845536897,
+            "timestamp": 0.7686627043848785
+        },
+        {
+            "x": 2.820751491800317,
+            "y": 5.768715178575412,
+            "heading": -3.055515411809828,
+            "angularVelocity": -0.513287851771491,
+            "velocityX": -3.341363048401663,
+            "velocityY": -1.2218860169986983,
+            "timestamp": 0.8540696715387539
+        },
+        {
+            "x": 2.592450909011727,
+            "y": 5.685229109517301,
+            "heading": -3.0905842313047507,
+            "angularVelocity": -0.41060841595908354,
+            "velocityX": -2.673090854253924,
+            "velocityY": -0.977508882942736,
+            "timestamp": 0.9394766386926293
+        },
+        {
+            "x": 2.421225457633144,
+            "y": 5.622614555370758,
+            "heading": -3.116884896329009,
+            "angularVelocity": -0.30794519347436133,
+            "velocityX": -2.00481830797353,
+            "velocityY": -0.7331316897570141,
+            "timestamp": 1.0248836058465047
+        },
+        {
+            "x": 2.3070751524559587,
+            "y": 5.5808715184580695,
+            "heading": -3.134418439279544,
+            "angularVelocity": -0.2052940589606165,
+            "velocityX": -1.336545588505958,
+            "velocityY": -0.4887544693804859,
+            "timestamp": 1.11029057300038
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.1026481584839197,
+            "velocityX": -0.668272792700015,
+            "velocityY": -0.24437723471044392,
+            "timestamp": 1.1956975401542553
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": -2.233003674732857e-32,
+            "velocityX": 0,
+            "velocityY": 6.615380928603251e-34,
+            "timestamp": 1.2811045073081306
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S1.1.traj
new file mode 100644
index 00000000..eb6a515e
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S1.1.traj	
@@ -0,0 +1,265 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -8.153039521410402e-32,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.331992841648679,
+            "y": 5.5918650858232235,
+            "heading": 3.133856142863887,
+            "angularVelocity": -0.06434641956265956,
+            "velocityX": 0.7225822862075633,
+            "velocityY": 0.3337323991032911,
+            "timestamp": 0.09548094793574066
+        },
+        {
+            "x": 4.469978522807239,
+            "y": 5.655595256777472,
+            "heading": 3.121568622421363,
+            "angularVelocity": -0.12869080908995395,
+            "velocityX": 1.445164550014868,
+            "velocityY": 0.6674647909569773,
+            "timestamp": 0.19096189587148132
+        },
+        {
+            "x": 4.676957039051013,
+            "y": 5.751190511387417,
+            "heading": 3.1031377745555058,
+            "angularVelocity": -0.19303168081514768,
+            "velocityX": 2.1677467674813493,
+            "velocityY": 1.0011971673591054,
+            "timestamp": 0.28644284380722196
+        },
+        {
+            "x": 4.9529283757083125,
+            "y": 5.878650843878014,
+            "heading": 3.078564198952369,
+            "angularVelocity": -0.2573662718522563,
+            "velocityX": 2.890328831286909,
+            "velocityY": 1.334929483277519,
+            "timestamp": 0.38192379174296265
+        },
+        {
+            "x": 5.280841898732695,
+            "y": 6.030101358943919,
+            "heading": 3.0785641960647663,
+            "angularVelocity": -3.024270852224024e-8,
+            "velocityX": 3.4343345988250538,
+            "velocityY": 1.5861857086708926,
+            "timestamp": 0.47740473967870334
+        },
+        {
+            "x": 5.608755421663104,
+            "y": 6.181551874214712,
+            "heading": 3.078564193177445,
+            "angularVelocity": -3.023976799325844e-8,
+            "velocityX": 3.434334597840841,
+            "velocityY": 1.5861857108167463,
+            "timestamp": 0.572885687614444
+        },
+        {
+            "x": 5.936668944593511,
+            "y": 6.333002389485506,
+            "heading": 3.0785641902901233,
+            "angularVelocity": -3.023976796721421e-8,
+            "velocityX": 3.434334597840832,
+            "velocityY": 1.5861857108167654,
+            "timestamp": 0.6683666355501847
+        },
+        {
+            "x": 6.26458246752392,
+            "y": 6.484452904756295,
+            "heading": 3.078564187402802,
+            "angularVelocity": -3.023976775003668e-8,
+            "velocityX": 3.434334597840855,
+            "velocityY": 1.5861857108167163,
+            "timestamp": 0.7638475834859254
+        },
+        {
+            "x": 6.592495990360559,
+            "y": 6.635903420230111,
+            "heading": 3.0785641845154803,
+            "angularVelocity": -3.02397677343441e-8,
+            "velocityX": 3.434334596858777,
+            "velocityY": 1.586185712943067,
+            "timestamp": 0.8593285314216661
+        },
+        {
+            "x": 6.920387522031495,
+            "y": 6.787401540858459,
+            "heading": 3.0785641816279576,
+            "angularVelocity": -3.0241868882472936e-8,
+            "velocityX": 3.4341042769240095,
+            "velocityY": 1.5866842957027985,
+            "timestamp": 0.9548094793574068
+        },
+        {
+            "x": 7.210301641874205,
+            "y": 6.987038475991185,
+            "heading": 3.078564386301996,
+            "angularVelocity": 0.000002143611293414107,
+            "velocityX": 3.036355693052319,
+            "velocityY": 2.0908562330876395,
+            "timestamp": 1.0502904272931475
+        },
+        {
+            "x": 7.431218296435779,
+            "y": 7.154820398489436,
+            "heading": 3.084707768304825,
+            "angularVelocity": 0.06434144335228988,
+            "velocityX": 2.3137249821846093,
+            "velocityY": 1.757229333447492,
+            "timestamp": 1.145771375228888
+        },
+        {
+            "x": 7.58313979167575,
+            "y": 7.2907422724167805,
+            "heading": 3.096994605047932,
+            "angularVelocity": 0.12868364850522696,
+            "velocityX": 1.5911184223080599,
+            "velocityY": 1.4235496909665903,
+            "timestamp": 1.2412523231646286
+        },
+        {
+            "x": 7.666066897511305,
+            "y": 7.39480241917028,
+            "heading": 3.11542521266587,
+            "angularVelocity": 0.19302916462815903,
+            "velocityX": 0.8685199260002638,
+            "velocityY": 1.0898524679843054,
+            "timestamp": 1.3367332711003692
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 0.2573789626666942,
+            "velocityX": 0.145925472986578,
+            "velocityY": 0.756146460527717,
+            "timestamp": 1.4322142190361098
+        },
+        {
+            "x": 7.628765464766867,
+            "y": 7.5070371939069025,
+            "heading": 3.1695683378543595,
+            "angularVelocity": 0.3197138219543324,
+            "velocityX": -0.5539841013770347,
+            "velocityY": 0.4329105121655262,
+            "timestamp": 1.5246979836528902
+        },
+        {
+            "x": 7.512802856979312,
+            "y": 7.517175485453599,
+            "heading": 3.204897367018404,
+            "angularVelocity": 0.3820024986055476,
+            "velocityX": -1.25386989022506,
+            "velocityY": 0.10962239252574857,
+            "timestamp": 1.6171817482696706
+        },
+        {
+            "x": 7.3321158425333,
+            "y": 7.497406833459743,
+            "heading": 3.2459797295297097,
+            "angularVelocity": 0.44421161575322776,
+            "velocityX": -1.9537160408082612,
+            "velocityY": -0.21375267405386408,
+            "timestamp": 1.709665512886451
+        },
+        {
+            "x": 7.086711752516261,
+            "y": 7.447715159830384,
+            "heading": 3.292800204439529,
+            "angularVelocity": 0.5062561532175872,
+            "velocityX": -2.6534829224793675,
+            "velocityY": -0.5373015883889657,
+            "timestamp": 1.8021492775032315
+        },
+        {
+            "x": 6.7766125854356485,
+            "y": 7.3680522721143955,
+            "heading": 3.345311930276191,
+            "angularVelocity": 0.5677939912444252,
+            "velocityX": -3.3530119406998606,
+            "velocityY": -0.8613715936546359,
+            "timestamp": 1.894633042120012
+        },
+        {
+            "x": 6.4587265246711105,
+            "y": 7.22193286725247,
+            "heading": 3.3453119331318786,
+            "angularVelocity": 3.087771911901371e-8,
+            "velocityX": -3.437209353240929,
+            "velocityY": -1.5799465502853158,
+            "timestamp": 1.9871168067367924
+        },
+        {
+            "x": 6.1410584209016115,
+            "y": 7.075340220973225,
+            "heading": 3.3453119359389976,
+            "angularVelocity": 3.035256115645747e-8,
+            "velocityX": -3.434852647767093,
+            "velocityY": -1.5850635718263661,
+            "timestamp": 2.079600571353573
+        },
+        {
+            "x": 5.823390305014834,
+            "y": 6.928747600950052,
+            "heading": 3.3453119387456907,
+            "angularVelocity": 3.034795349843415e-8,
+            "velocityX": -3.4348527787877012,
+            "velocityY": -1.5850632879271098,
+            "timestamp": 2.1720843359703537
+        },
+        {
+            "x": 5.564434200353795,
+            "y": 6.809248569525128,
+            "heading": 3.3684615293972238,
+            "angularVelocity": 0.2503097786643198,
+            "velocityX": -2.8000169082002904,
+            "velocityY": -1.292108208616342,
+            "timestamp": 2.2645681005871343
+        },
+        {
+            "x": 5.3702171046964775,
+            "y": 6.719624287090389,
+            "heading": 3.3858234872314386,
+            "angularVelocity": 0.18772979134186565,
+            "velocityX": -2.1000128667129405,
+            "velocityY": -0.9690812523282153,
+            "timestamp": 2.357051865203915
+        },
+        {
+            "x": 5.240739035958368,
+            "y": 6.659874762854457,
+            "heading": 3.3973980568576057,
+            "angularVelocity": 0.1251524489081645,
+            "velocityX": -1.4000086315106373,
+            "velocityY": -0.6460541964699718,
+            "timestamp": 2.4495356298206956
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.06257585151184072,
+            "velocityX": -0.7000043329401022,
+            "velocityY": -0.3230271061984462,
+            "timestamp": 2.5420193944374763
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 1.89050013704423e-29,
+            "velocityX": -9.26196849914815e-31,
+            "velocityY": -1.067270908871219e-31,
+            "timestamp": 2.634503159054257
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S1.traj
new file mode 100644
index 00000000..eb6a515e
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S1.traj	
@@ -0,0 +1,265 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -8.153039521410402e-32,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.331992841648679,
+            "y": 5.5918650858232235,
+            "heading": 3.133856142863887,
+            "angularVelocity": -0.06434641956265956,
+            "velocityX": 0.7225822862075633,
+            "velocityY": 0.3337323991032911,
+            "timestamp": 0.09548094793574066
+        },
+        {
+            "x": 4.469978522807239,
+            "y": 5.655595256777472,
+            "heading": 3.121568622421363,
+            "angularVelocity": -0.12869080908995395,
+            "velocityX": 1.445164550014868,
+            "velocityY": 0.6674647909569773,
+            "timestamp": 0.19096189587148132
+        },
+        {
+            "x": 4.676957039051013,
+            "y": 5.751190511387417,
+            "heading": 3.1031377745555058,
+            "angularVelocity": -0.19303168081514768,
+            "velocityX": 2.1677467674813493,
+            "velocityY": 1.0011971673591054,
+            "timestamp": 0.28644284380722196
+        },
+        {
+            "x": 4.9529283757083125,
+            "y": 5.878650843878014,
+            "heading": 3.078564198952369,
+            "angularVelocity": -0.2573662718522563,
+            "velocityX": 2.890328831286909,
+            "velocityY": 1.334929483277519,
+            "timestamp": 0.38192379174296265
+        },
+        {
+            "x": 5.280841898732695,
+            "y": 6.030101358943919,
+            "heading": 3.0785641960647663,
+            "angularVelocity": -3.024270852224024e-8,
+            "velocityX": 3.4343345988250538,
+            "velocityY": 1.5861857086708926,
+            "timestamp": 0.47740473967870334
+        },
+        {
+            "x": 5.608755421663104,
+            "y": 6.181551874214712,
+            "heading": 3.078564193177445,
+            "angularVelocity": -3.023976799325844e-8,
+            "velocityX": 3.434334597840841,
+            "velocityY": 1.5861857108167463,
+            "timestamp": 0.572885687614444
+        },
+        {
+            "x": 5.936668944593511,
+            "y": 6.333002389485506,
+            "heading": 3.0785641902901233,
+            "angularVelocity": -3.023976796721421e-8,
+            "velocityX": 3.434334597840832,
+            "velocityY": 1.5861857108167654,
+            "timestamp": 0.6683666355501847
+        },
+        {
+            "x": 6.26458246752392,
+            "y": 6.484452904756295,
+            "heading": 3.078564187402802,
+            "angularVelocity": -3.023976775003668e-8,
+            "velocityX": 3.434334597840855,
+            "velocityY": 1.5861857108167163,
+            "timestamp": 0.7638475834859254
+        },
+        {
+            "x": 6.592495990360559,
+            "y": 6.635903420230111,
+            "heading": 3.0785641845154803,
+            "angularVelocity": -3.02397677343441e-8,
+            "velocityX": 3.434334596858777,
+            "velocityY": 1.586185712943067,
+            "timestamp": 0.8593285314216661
+        },
+        {
+            "x": 6.920387522031495,
+            "y": 6.787401540858459,
+            "heading": 3.0785641816279576,
+            "angularVelocity": -3.0241868882472936e-8,
+            "velocityX": 3.4341042769240095,
+            "velocityY": 1.5866842957027985,
+            "timestamp": 0.9548094793574068
+        },
+        {
+            "x": 7.210301641874205,
+            "y": 6.987038475991185,
+            "heading": 3.078564386301996,
+            "angularVelocity": 0.000002143611293414107,
+            "velocityX": 3.036355693052319,
+            "velocityY": 2.0908562330876395,
+            "timestamp": 1.0502904272931475
+        },
+        {
+            "x": 7.431218296435779,
+            "y": 7.154820398489436,
+            "heading": 3.084707768304825,
+            "angularVelocity": 0.06434144335228988,
+            "velocityX": 2.3137249821846093,
+            "velocityY": 1.757229333447492,
+            "timestamp": 1.145771375228888
+        },
+        {
+            "x": 7.58313979167575,
+            "y": 7.2907422724167805,
+            "heading": 3.096994605047932,
+            "angularVelocity": 0.12868364850522696,
+            "velocityX": 1.5911184223080599,
+            "velocityY": 1.4235496909665903,
+            "timestamp": 1.2412523231646286
+        },
+        {
+            "x": 7.666066897511305,
+            "y": 7.39480241917028,
+            "heading": 3.11542521266587,
+            "angularVelocity": 0.19302916462815903,
+            "velocityX": 0.8685199260002638,
+            "velocityY": 1.0898524679843054,
+            "timestamp": 1.3367332711003692
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 0.2573789626666942,
+            "velocityX": 0.145925472986578,
+            "velocityY": 0.756146460527717,
+            "timestamp": 1.4322142190361098
+        },
+        {
+            "x": 7.628765464766867,
+            "y": 7.5070371939069025,
+            "heading": 3.1695683378543595,
+            "angularVelocity": 0.3197138219543324,
+            "velocityX": -0.5539841013770347,
+            "velocityY": 0.4329105121655262,
+            "timestamp": 1.5246979836528902
+        },
+        {
+            "x": 7.512802856979312,
+            "y": 7.517175485453599,
+            "heading": 3.204897367018404,
+            "angularVelocity": 0.3820024986055476,
+            "velocityX": -1.25386989022506,
+            "velocityY": 0.10962239252574857,
+            "timestamp": 1.6171817482696706
+        },
+        {
+            "x": 7.3321158425333,
+            "y": 7.497406833459743,
+            "heading": 3.2459797295297097,
+            "angularVelocity": 0.44421161575322776,
+            "velocityX": -1.9537160408082612,
+            "velocityY": -0.21375267405386408,
+            "timestamp": 1.709665512886451
+        },
+        {
+            "x": 7.086711752516261,
+            "y": 7.447715159830384,
+            "heading": 3.292800204439529,
+            "angularVelocity": 0.5062561532175872,
+            "velocityX": -2.6534829224793675,
+            "velocityY": -0.5373015883889657,
+            "timestamp": 1.8021492775032315
+        },
+        {
+            "x": 6.7766125854356485,
+            "y": 7.3680522721143955,
+            "heading": 3.345311930276191,
+            "angularVelocity": 0.5677939912444252,
+            "velocityX": -3.3530119406998606,
+            "velocityY": -0.8613715936546359,
+            "timestamp": 1.894633042120012
+        },
+        {
+            "x": 6.4587265246711105,
+            "y": 7.22193286725247,
+            "heading": 3.3453119331318786,
+            "angularVelocity": 3.087771911901371e-8,
+            "velocityX": -3.437209353240929,
+            "velocityY": -1.5799465502853158,
+            "timestamp": 1.9871168067367924
+        },
+        {
+            "x": 6.1410584209016115,
+            "y": 7.075340220973225,
+            "heading": 3.3453119359389976,
+            "angularVelocity": 3.035256115645747e-8,
+            "velocityX": -3.434852647767093,
+            "velocityY": -1.5850635718263661,
+            "timestamp": 2.079600571353573
+        },
+        {
+            "x": 5.823390305014834,
+            "y": 6.928747600950052,
+            "heading": 3.3453119387456907,
+            "angularVelocity": 3.034795349843415e-8,
+            "velocityX": -3.4348527787877012,
+            "velocityY": -1.5850632879271098,
+            "timestamp": 2.1720843359703537
+        },
+        {
+            "x": 5.564434200353795,
+            "y": 6.809248569525128,
+            "heading": 3.3684615293972238,
+            "angularVelocity": 0.2503097786643198,
+            "velocityX": -2.8000169082002904,
+            "velocityY": -1.292108208616342,
+            "timestamp": 2.2645681005871343
+        },
+        {
+            "x": 5.3702171046964775,
+            "y": 6.719624287090389,
+            "heading": 3.3858234872314386,
+            "angularVelocity": 0.18772979134186565,
+            "velocityX": -2.1000128667129405,
+            "velocityY": -0.9690812523282153,
+            "timestamp": 2.357051865203915
+        },
+        {
+            "x": 5.240739035958368,
+            "y": 6.659874762854457,
+            "heading": 3.3973980568576057,
+            "angularVelocity": 0.1251524489081645,
+            "velocityX": -1.4000086315106373,
+            "velocityY": -0.6460541964699718,
+            "timestamp": 2.4495356298206956
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.06257585151184072,
+            "velocityX": -0.7000043329401022,
+            "velocityY": -0.3230271061984462,
+            "timestamp": 2.5420193944374763
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 1.89050013704423e-29,
+            "velocityX": -9.26196849914815e-31,
+            "velocityY": -1.067270908871219e-31,
+            "timestamp": 2.634503159054257
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S2.1.traj
new file mode 100644
index 00000000..1b1b6343
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S2.1.traj	
@@ -0,0 +1,283 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -6.721620698039908e-29,
+            "timestamp": 0
+        },
+        {
+            "x": 4.32871710747576,
+            "y": 5.596676184944768,
+            "heading": 3.14,
+            "angularVelocity": -1.6840159888455038e-16,
+            "velocityX": 0.6917264182881007,
+            "velocityY": 0.3860469065482612,
+            "timestamp": 0.09500447827150149
+        },
+        {
+            "x": 4.460151319654045,
+            "y": 5.670028553286586,
+            "heading": 3.14,
+            "angularVelocity": -1.2818077527297718e-16,
+            "velocityX": 1.3834528073858645,
+            "velocityY": 0.772093796805632,
+            "timestamp": 0.19000895654300298
+        },
+        {
+            "x": 4.657302630988413,
+            "y": 5.780057101930027,
+            "heading": 3.14,
+            "angularVelocity": -1.8176327661301372e-16,
+            "velocityX": 2.0751791381032545,
+            "velocityY": 1.1581406544813895,
+            "timestamp": 0.2850134348145045
+        },
+        {
+            "x": 4.920171024839782,
+            "y": 5.926761821588956,
+            "heading": 3.14,
+            "angularVelocity": -2.1864698969841676e-16,
+            "velocityX": 2.766905293682386,
+            "velocityY": 1.5441874144139043,
+            "timestamp": 0.38001791308600597
+        },
+        {
+            "x": 5.23400140491651,
+            "y": 6.101908012635582,
+            "heading": 3.14,
+            "angularVelocity": 2.4726801201282114e-16,
+            "velocityX": 3.3033219674134005,
+            "velocityY": 1.8435572115473686,
+            "timestamp": 0.47502239135750746
+        },
+        {
+            "x": 5.547831784993484,
+            "y": 6.277054203682345,
+            "heading": 3.14,
+            "angularVelocity": 3.7708427520389947e-17,
+            "velocityX": 3.303321967416651,
+            "velocityY": 1.843557211549182,
+            "timestamp": 0.570026869629009
+        },
+        {
+            "x": 5.861662165070458,
+            "y": 6.452200394729108,
+            "heading": 3.14,
+            "angularVelocity": 2.1976269787785873e-16,
+            "velocityX": 3.3033219674166516,
+            "velocityY": 1.843557211549183,
+            "timestamp": 0.6650313479005104
+        },
+        {
+            "x": 6.175492545147431,
+            "y": 6.627346585775871,
+            "heading": 3.14,
+            "angularVelocity": 1.9941780105068909e-16,
+            "velocityX": 3.3033219674166516,
+            "velocityY": 1.843557211549183,
+            "timestamp": 0.7600358261720119
+        },
+        {
+            "x": 6.489322925224406,
+            "y": 6.802492776822635,
+            "heading": 3.14,
+            "angularVelocity": 1.915977022732522e-16,
+            "velocityX": 3.3033219674166516,
+            "velocityY": 1.8435572115491827,
+            "timestamp": 0.8550403044435134
+        },
+        {
+            "x": 6.803153305301396,
+            "y": 6.977638967869407,
+            "heading": 3.14,
+            "angularVelocity": 1.0071749090943876e-16,
+            "velocityX": 3.3033219674167866,
+            "velocityY": 1.8435572115492582,
+            "timestamp": 0.9500447827150149
+        },
+        {
+            "x": 7.109956833123924,
+            "y": 7.148863529636325,
+            "heading": 3.14,
+            "angularVelocity": 7.28120504001254e-16,
+            "velocityX": 3.2293585881581666,
+            "velocityY": 1.802278849171093,
+            "timestamp": 1.0450492609865165
+        },
+        {
+            "x": 7.351043278430091,
+            "y": 7.283411920388113,
+            "heading": 3.14,
+            "angularVelocity": 8.519808203444012e-16,
+            "velocityX": 2.5376324325879027,
+            "velocityY": 1.4162320892435267,
+            "timestamp": 1.140053739258018
+        },
+        {
+            "x": 7.526412624580293,
+            "y": 7.381284130838343,
+            "heading": 3.14,
+            "angularVelocity": 9.198517694714364e-16,
+            "velocityX": 1.8459061018722946,
+            "velocityY": 1.0301852315687638,
+            "timestamp": 1.2350582175295193
+        },
+        {
+            "x": 7.636064866028015,
+            "y": 7.442480157891549,
+            "heading": 3.14,
+            "angularVelocity": 7.057693969312242e-16,
+            "velocityX": 1.1541797127752547,
+            "velocityY": 0.6441383413117968,
+            "timestamp": 1.3300626958010207
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 6.521841549416907e-16,
+            "velocityX": 0.4624532944875416,
+            "velocityY": 0.25809143476375196,
+            "timestamp": 1.425067174072522
+        },
+        {
+            "x": 7.649929424023421,
+            "y": 7.450217855315383,
+            "heading": 3.14,
+            "angularVelocity": 4.173997569351088e-16,
+            "velocityX": -0.2907017552877216,
+            "velocityY": -0.16223829304468454,
+            "timestamp": 1.5285085032844143
+        },
+        {
+            "x": 7.5419514905723615,
+            "y": 7.389956245982292,
+            "heading": 3.14,
+            "angularVelocity": 3.561755142449842e-16,
+            "velocityX": -1.043856785955406,
+            "velocityY": -0.5825680101893362,
+            "timestamp": 1.6319498324963067
+        },
+        {
+            "x": 7.356066203105843,
+            "y": 7.286215173931179,
+            "heading": 3.14,
+            "angularVelocity": 4.0492399811570283e-16,
+            "velocityX": -1.7970117831835097,
+            "velocityY": -1.0028977086716289,
+            "timestamp": 1.735391161708199
+        },
+        {
+            "x": 7.092273569234474,
+            "y": 7.138994643409464,
+            "heading": 3.14,
+            "angularVelocity": 3.5744526417986045e-16,
+            "velocityX": -2.550166706837388,
+            "velocityY": -1.4232273660927433,
+            "timestamp": 1.8388324909200913
+        },
+        {
+            "x": 6.7505736194163815,
+            "y": 6.94829467141558,
+            "heading": 3.14,
+            "angularVelocity": 3.486084398579742e-18,
+            "velocityX": -3.3033213360452667,
+            "velocityY": -1.8435568591859304,
+            "timestamp": 1.9422738201319836
+        },
+        {
+            "x": 6.4088736018749985,
+            "y": 6.757594661625877,
+            "heading": 3.14,
+            "angularVelocity": 3.295553774982102e-16,
+            "velocityX": -3.303321990779068,
+            "velocityY": -1.8435572245875573,
+            "timestamp": 2.045715149343876
+        },
+        {
+            "x": 6.0671735843335775,
+            "y": 6.566894651836152,
+            "heading": 3.14,
+            "angularVelocity": -4.7007888222967745e-17,
+            "velocityX": -3.303321990779443,
+            "velocityY": -1.8435572245877663,
+            "timestamp": 2.1491564785557684
+        },
+        {
+            "x": 5.725473566792156,
+            "y": 6.376194642046427,
+            "heading": 3.14,
+            "angularVelocity": -1.693931270443523e-16,
+            "velocityX": -3.3033219907794424,
+            "velocityY": -1.843557224587766,
+            "timestamp": 2.252597807767661
+        },
+        {
+            "x": 5.383773549250734,
+            "y": 6.185494632256702,
+            "heading": 3.14,
+            "angularVelocity": -1.7179108161052583e-16,
+            "velocityX": -3.3033219907794424,
+            "velocityY": -1.8435572245877665,
+            "timestamp": 2.3560391369795535
+        },
+        {
+            "x": 5.042073531709293,
+            "y": 5.994794622466965,
+            "heading": 3.14,
+            "angularVelocity": -2.8062613243046787e-16,
+            "velocityX": -3.3033219907797844,
+            "velocityY": -1.8435572245879575,
+            "timestamp": 2.459480466191446
+        },
+        {
+            "x": 4.730444135370463,
+            "y": 5.820876782602128,
+            "heading": 3.14,
+            "angularVelocity": -4.299065406379285e-16,
+            "velocityX": -3.0126197982407033,
+            "velocityY": -1.6813186875168347,
+            "timestamp": 2.5629217954033385
+        },
+        {
+            "x": 4.496722071912365,
+            "y": 5.690438393660192,
+            "heading": 3.14,
+            "angularVelocity": -4.936527887438678e-16,
+            "velocityX": -2.2594650053308842,
+            "velocityY": -1.2609891030629203,
+            "timestamp": 2.666363124615231
+        },
+        {
+            "x": 4.340907358243485,
+            "y": 5.6034794650776485,
+            "heading": 3.14,
+            "angularVelocity": -3.0491715981392867e-16,
+            "velocityX": -1.50631004895991,
+            "velocityY": -0.8406594273826603,
+            "timestamp": 2.7698044538271236
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -6.545212108880601e-17,
+            "velocityX": -0.7531550381017611,
+            "velocityY": -0.42032972129354973,
+            "timestamp": 2.873245783039016
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -8.479397572029164e-30,
+            "velocityY": 1.1992173369619903e-31,
+            "timestamp": 2.9766871122509087
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S2.traj
new file mode 100644
index 00000000..1b1b6343
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S2.traj	
@@ -0,0 +1,283 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -6.721620698039908e-29,
+            "timestamp": 0
+        },
+        {
+            "x": 4.32871710747576,
+            "y": 5.596676184944768,
+            "heading": 3.14,
+            "angularVelocity": -1.6840159888455038e-16,
+            "velocityX": 0.6917264182881007,
+            "velocityY": 0.3860469065482612,
+            "timestamp": 0.09500447827150149
+        },
+        {
+            "x": 4.460151319654045,
+            "y": 5.670028553286586,
+            "heading": 3.14,
+            "angularVelocity": -1.2818077527297718e-16,
+            "velocityX": 1.3834528073858645,
+            "velocityY": 0.772093796805632,
+            "timestamp": 0.19000895654300298
+        },
+        {
+            "x": 4.657302630988413,
+            "y": 5.780057101930027,
+            "heading": 3.14,
+            "angularVelocity": -1.8176327661301372e-16,
+            "velocityX": 2.0751791381032545,
+            "velocityY": 1.1581406544813895,
+            "timestamp": 0.2850134348145045
+        },
+        {
+            "x": 4.920171024839782,
+            "y": 5.926761821588956,
+            "heading": 3.14,
+            "angularVelocity": -2.1864698969841676e-16,
+            "velocityX": 2.766905293682386,
+            "velocityY": 1.5441874144139043,
+            "timestamp": 0.38001791308600597
+        },
+        {
+            "x": 5.23400140491651,
+            "y": 6.101908012635582,
+            "heading": 3.14,
+            "angularVelocity": 2.4726801201282114e-16,
+            "velocityX": 3.3033219674134005,
+            "velocityY": 1.8435572115473686,
+            "timestamp": 0.47502239135750746
+        },
+        {
+            "x": 5.547831784993484,
+            "y": 6.277054203682345,
+            "heading": 3.14,
+            "angularVelocity": 3.7708427520389947e-17,
+            "velocityX": 3.303321967416651,
+            "velocityY": 1.843557211549182,
+            "timestamp": 0.570026869629009
+        },
+        {
+            "x": 5.861662165070458,
+            "y": 6.452200394729108,
+            "heading": 3.14,
+            "angularVelocity": 2.1976269787785873e-16,
+            "velocityX": 3.3033219674166516,
+            "velocityY": 1.843557211549183,
+            "timestamp": 0.6650313479005104
+        },
+        {
+            "x": 6.175492545147431,
+            "y": 6.627346585775871,
+            "heading": 3.14,
+            "angularVelocity": 1.9941780105068909e-16,
+            "velocityX": 3.3033219674166516,
+            "velocityY": 1.843557211549183,
+            "timestamp": 0.7600358261720119
+        },
+        {
+            "x": 6.489322925224406,
+            "y": 6.802492776822635,
+            "heading": 3.14,
+            "angularVelocity": 1.915977022732522e-16,
+            "velocityX": 3.3033219674166516,
+            "velocityY": 1.8435572115491827,
+            "timestamp": 0.8550403044435134
+        },
+        {
+            "x": 6.803153305301396,
+            "y": 6.977638967869407,
+            "heading": 3.14,
+            "angularVelocity": 1.0071749090943876e-16,
+            "velocityX": 3.3033219674167866,
+            "velocityY": 1.8435572115492582,
+            "timestamp": 0.9500447827150149
+        },
+        {
+            "x": 7.109956833123924,
+            "y": 7.148863529636325,
+            "heading": 3.14,
+            "angularVelocity": 7.28120504001254e-16,
+            "velocityX": 3.2293585881581666,
+            "velocityY": 1.802278849171093,
+            "timestamp": 1.0450492609865165
+        },
+        {
+            "x": 7.351043278430091,
+            "y": 7.283411920388113,
+            "heading": 3.14,
+            "angularVelocity": 8.519808203444012e-16,
+            "velocityX": 2.5376324325879027,
+            "velocityY": 1.4162320892435267,
+            "timestamp": 1.140053739258018
+        },
+        {
+            "x": 7.526412624580293,
+            "y": 7.381284130838343,
+            "heading": 3.14,
+            "angularVelocity": 9.198517694714364e-16,
+            "velocityX": 1.8459061018722946,
+            "velocityY": 1.0301852315687638,
+            "timestamp": 1.2350582175295193
+        },
+        {
+            "x": 7.636064866028015,
+            "y": 7.442480157891549,
+            "heading": 3.14,
+            "angularVelocity": 7.057693969312242e-16,
+            "velocityX": 1.1541797127752547,
+            "velocityY": 0.6441383413117968,
+            "timestamp": 1.3300626958010207
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 6.521841549416907e-16,
+            "velocityX": 0.4624532944875416,
+            "velocityY": 0.25809143476375196,
+            "timestamp": 1.425067174072522
+        },
+        {
+            "x": 7.649929424023421,
+            "y": 7.450217855315383,
+            "heading": 3.14,
+            "angularVelocity": 4.173997569351088e-16,
+            "velocityX": -0.2907017552877216,
+            "velocityY": -0.16223829304468454,
+            "timestamp": 1.5285085032844143
+        },
+        {
+            "x": 7.5419514905723615,
+            "y": 7.389956245982292,
+            "heading": 3.14,
+            "angularVelocity": 3.561755142449842e-16,
+            "velocityX": -1.043856785955406,
+            "velocityY": -0.5825680101893362,
+            "timestamp": 1.6319498324963067
+        },
+        {
+            "x": 7.356066203105843,
+            "y": 7.286215173931179,
+            "heading": 3.14,
+            "angularVelocity": 4.0492399811570283e-16,
+            "velocityX": -1.7970117831835097,
+            "velocityY": -1.0028977086716289,
+            "timestamp": 1.735391161708199
+        },
+        {
+            "x": 7.092273569234474,
+            "y": 7.138994643409464,
+            "heading": 3.14,
+            "angularVelocity": 3.5744526417986045e-16,
+            "velocityX": -2.550166706837388,
+            "velocityY": -1.4232273660927433,
+            "timestamp": 1.8388324909200913
+        },
+        {
+            "x": 6.7505736194163815,
+            "y": 6.94829467141558,
+            "heading": 3.14,
+            "angularVelocity": 3.486084398579742e-18,
+            "velocityX": -3.3033213360452667,
+            "velocityY": -1.8435568591859304,
+            "timestamp": 1.9422738201319836
+        },
+        {
+            "x": 6.4088736018749985,
+            "y": 6.757594661625877,
+            "heading": 3.14,
+            "angularVelocity": 3.295553774982102e-16,
+            "velocityX": -3.303321990779068,
+            "velocityY": -1.8435572245875573,
+            "timestamp": 2.045715149343876
+        },
+        {
+            "x": 6.0671735843335775,
+            "y": 6.566894651836152,
+            "heading": 3.14,
+            "angularVelocity": -4.7007888222967745e-17,
+            "velocityX": -3.303321990779443,
+            "velocityY": -1.8435572245877663,
+            "timestamp": 2.1491564785557684
+        },
+        {
+            "x": 5.725473566792156,
+            "y": 6.376194642046427,
+            "heading": 3.14,
+            "angularVelocity": -1.693931270443523e-16,
+            "velocityX": -3.3033219907794424,
+            "velocityY": -1.843557224587766,
+            "timestamp": 2.252597807767661
+        },
+        {
+            "x": 5.383773549250734,
+            "y": 6.185494632256702,
+            "heading": 3.14,
+            "angularVelocity": -1.7179108161052583e-16,
+            "velocityX": -3.3033219907794424,
+            "velocityY": -1.8435572245877665,
+            "timestamp": 2.3560391369795535
+        },
+        {
+            "x": 5.042073531709293,
+            "y": 5.994794622466965,
+            "heading": 3.14,
+            "angularVelocity": -2.8062613243046787e-16,
+            "velocityX": -3.3033219907797844,
+            "velocityY": -1.8435572245879575,
+            "timestamp": 2.459480466191446
+        },
+        {
+            "x": 4.730444135370463,
+            "y": 5.820876782602128,
+            "heading": 3.14,
+            "angularVelocity": -4.299065406379285e-16,
+            "velocityX": -3.0126197982407033,
+            "velocityY": -1.6813186875168347,
+            "timestamp": 2.5629217954033385
+        },
+        {
+            "x": 4.496722071912365,
+            "y": 5.690438393660192,
+            "heading": 3.14,
+            "angularVelocity": -4.936527887438678e-16,
+            "velocityX": -2.2594650053308842,
+            "velocityY": -1.2609891030629203,
+            "timestamp": 2.666363124615231
+        },
+        {
+            "x": 4.340907358243485,
+            "y": 5.6034794650776485,
+            "heading": 3.14,
+            "angularVelocity": -3.0491715981392867e-16,
+            "velocityX": -1.50631004895991,
+            "velocityY": -0.8406594273826603,
+            "timestamp": 2.7698044538271236
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -6.545212108880601e-17,
+            "velocityX": -0.7531550381017611,
+            "velocityY": -0.42032972129354973,
+            "timestamp": 2.873245783039016
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -8.479397572029164e-30,
+            "velocityY": 1.1992173369619903e-31,
+            "timestamp": 2.9766871122509087
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S1.1.traj
new file mode 100644
index 00000000..505b3c8d
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S1.1.traj	
@@ -0,0 +1,256 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -3.0990371742085845e-31,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.334488852780742,
+            "y": 5.55372681466254,
+            "heading": 3.133953491828579,
+            "angularVelocity": -0.06516682898121992,
+            "velocityX": 0.7704780529773917,
+            "velocityY": -0.06760986415037558,
+            "timestamp": 0.09278506052766491
+        },
+        {
+            "x": 4.477466555007721,
+            "y": 5.541180442989367,
+            "heading": 3.121860441564243,
+            "angularVelocity": -0.13033402355464482,
+            "velocityX": 1.5409560700168046,
+            "velocityY": -0.1352197390595365,
+            "timestamp": 0.18557012105532983
+        },
+        {
+            "x": 4.691933100191895,
+            "y": 5.522360883581774,
+            "heading": 3.1037208124155655,
+            "angularVelocity": -0.19550161465122112,
+            "velocityX": 2.311434017119952,
+            "velocityY": -0.20282962904337912,
+            "timestamp": 0.27835518158299477
+        },
+        {
+            "x": 4.977888469750385,
+            "y": 5.497268135402371,
+            "heading": 3.079534623451408,
+            "angularVelocity": -0.26066900023130835,
+            "velocityX": 3.0819117639443303,
+            "velocityY": -0.2704395302078152,
+            "timestamp": 0.37114024211065966
+        },
+        {
+            "x": 5.327545209579434,
+            "y": 5.4665855253125155,
+            "heading": 3.079534620495457,
+            "angularVelocity": -3.185804655621534e-8,
+            "velocityX": 3.768459467941971,
+            "velocityY": -0.33068480976750836,
+            "timestamp": 0.46392530263832454
+        },
+        {
+            "x": 5.677201949407512,
+            "y": 5.435902915209702,
+            "heading": 3.0795346175395855,
+            "angularVelocity": -3.185719286228054e-8,
+            "velocityX": 3.768459467931527,
+            "velocityY": -0.3306848099071595,
+            "timestamp": 0.5567103631659894
+        },
+        {
+            "x": 6.026858689235635,
+            "y": 5.405220305107383,
+            "heading": 3.0795346145837144,
+            "angularVelocity": -3.185719241382707e-8,
+            "velocityX": 3.7684594679319945,
+            "velocityY": -0.3306848099018315,
+            "timestamp": 0.6494954236936543
+        },
+        {
+            "x": 6.376515440276988,
+            "y": 5.37453782279048,
+            "heading": 3.079534611627843,
+            "angularVelocity": -3.1857194092858966e-8,
+            "velocityX": 3.7684595887836787,
+            "velocityY": -0.3306834326820888,
+            "timestamp": 0.7422804842213192
+        },
+        {
+            "x": 6.7274981570931205,
+            "y": 5.371017863275757,
+            "heading": 3.079534608531603,
+            "angularVelocity": -3.337002331102787e-8,
+            "velocityX": 3.782750313683133,
+            "velocityY": -0.03793670548581862,
+            "timestamp": 0.8350655447489841
+        },
+        {
+            "x": 7.060187879890856,
+            "y": 5.436910997452384,
+            "heading": 3.079737215344131,
+            "angularVelocity": 0.0021836145967468554,
+            "velocityX": 3.585595794255326,
+            "velocityY": 0.7101696523329738,
+            "timestamp": 0.927850605276649
+        },
+        {
+            "x": 7.322040445742604,
+            "y": 5.514304234937123,
+            "heading": 3.085826008048483,
+            "angularVelocity": 0.065622554641053,
+            "velocityX": 2.82214145642183,
+            "velocityY": 0.834113132487145,
+            "timestamp": 1.0206356658043139
+        },
+        {
+            "x": 7.51268687185891,
+            "y": 5.600628661192279,
+            "heading": 3.097884063831068,
+            "angularVelocity": 0.12995686712932528,
+            "velocityX": 2.054710370743838,
+            "velocityY": 0.9303698867493723,
+            "timestamp": 1.1134207263319789
+        },
+        {
+            "x": 7.632022870387607,
+            "y": 5.695007866387598,
+            "heading": 3.115937777513367,
+            "angularVelocity": 0.19457565236926141,
+            "velocityX": 1.2861553126119536,
+            "velocityY": 1.0171810489597053,
+            "timestamp": 1.2062057868596439
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 0.25933293948166103,
+            "velocityX": 0.517078173356003,
+            "velocityY": 1.0992301242503861,
+            "timestamp": 1.2989908473873089
+        },
+        {
+            "x": 7.658324369192282,
+            "y": 5.904176742987938,
+            "heading": 3.16941072815558,
+            "angularVelocity": 0.32296823834349714,
+            "velocityX": -0.23802675880415525,
+            "velocityY": 1.176940730303702,
+            "timestamp": 1.3900546879260585
+        },
+        {
+            "x": 7.56785439743373,
+            "y": 6.01813279809926,
+            "heading": 3.204362801369678,
+            "angularVelocity": 0.3838194502596341,
+            "velocityX": -0.9934785445386147,
+            "velocityY": 1.2513864387569726,
+            "timestamp": 1.481118528464808
+        },
+        {
+            "x": 7.4085407741930185,
+            "y": 6.138375580441298,
+            "heading": 3.2444365070983814,
+            "angularVelocity": 0.44006166983116235,
+            "velocityX": -1.7494718243615388,
+            "velocityY": 1.3204229212238012,
+            "timestamp": 1.5721823690035577
+        },
+        {
+            "x": 7.180297052881674,
+            "y": 6.26393107037867,
+            "heading": 3.2888027958474897,
+            "angularVelocity": 0.48719984229357544,
+            "velocityX": -2.5064144007215123,
+            "velocityY": 1.3787633949387568,
+            "timestamp": 1.6632462095423073
+        },
+        {
+            "x": 6.882954936764855,
+            "y": 6.39196860518662,
+            "heading": 3.335055491791258,
+            "angularVelocity": 0.5079150590414778,
+            "velocityX": -3.2652050952131715,
+            "velocityY": 1.406019491935018,
+            "timestamp": 1.754310050081057
+        },
+        {
+            "x": 6.546228290367871,
+            "y": 6.464686559433002,
+            "heading": 3.335055498171187,
+            "angularVelocity": 7.005996108772343e-8,
+            "velocityX": -3.697698717787989,
+            "velocityY": 0.7985381883321813,
+            "timestamp": 1.8453738906198065
+        },
+        {
+            "x": 6.204219309221898,
+            "y": 6.505949051941506,
+            "heading": 3.3350555015810333,
+            "angularVelocity": 3.7444564221531034e-8,
+            "velocityX": -3.7557056579492913,
+            "velocityY": 0.45311610255384965,
+            "timestamp": 1.936437731158556
+        },
+        {
+            "x": 5.862210306846827,
+            "y": 6.547211368473936,
+            "heading": 3.335055504991925,
+            "angularVelocity": 3.7456047151170976e-8,
+            "velocityX": -3.7557058910725254,
+            "velocityY": 0.45311417010650673,
+            "timestamp": 2.0275015716973055
+        },
+        {
+            "x": 5.587726197097608,
+            "y": 6.580326820073676,
+            "heading": 3.3623069003974067,
+            "angularVelocity": 0.29925594225114466,
+            "velocityX": -3.0141943072610453,
+            "velocityY": 0.3636509442587018,
+            "timestamp": 2.118565412236055
+        },
+        {
+            "x": 5.381863101775985,
+            "y": 6.605163409773074,
+            "heading": 3.382745950256302,
+            "angularVelocity": 0.22444748363294062,
+            "velocityX": -2.2606458733093975,
+            "velocityY": 0.2727382191708659,
+            "timestamp": 2.2096292527748043
+        },
+        {
+            "x": 5.244621034490526,
+            "y": 6.621721136542047,
+            "heading": 3.3963721295427387,
+            "angularVelocity": 0.1496332595442014,
+            "velocityX": -1.5070972899178572,
+            "velocityY": 0.18182548277135752,
+            "timestamp": 2.3006930933135536
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.07481759605718756,
+            "velocityX": -0.7535486542688541,
+            "velocityY": 0.09091274219244924,
+            "timestamp": 2.391756933852303
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0,
+            "velocityX": -1.5663528740094776e-30,
+            "velocityY": 4.877949363390491e-30,
+            "timestamp": 2.4828207743910524
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S1.traj
new file mode 100644
index 00000000..505b3c8d
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S1.traj	
@@ -0,0 +1,256 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -3.0990371742085845e-31,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.334488852780742,
+            "y": 5.55372681466254,
+            "heading": 3.133953491828579,
+            "angularVelocity": -0.06516682898121992,
+            "velocityX": 0.7704780529773917,
+            "velocityY": -0.06760986415037558,
+            "timestamp": 0.09278506052766491
+        },
+        {
+            "x": 4.477466555007721,
+            "y": 5.541180442989367,
+            "heading": 3.121860441564243,
+            "angularVelocity": -0.13033402355464482,
+            "velocityX": 1.5409560700168046,
+            "velocityY": -0.1352197390595365,
+            "timestamp": 0.18557012105532983
+        },
+        {
+            "x": 4.691933100191895,
+            "y": 5.522360883581774,
+            "heading": 3.1037208124155655,
+            "angularVelocity": -0.19550161465122112,
+            "velocityX": 2.311434017119952,
+            "velocityY": -0.20282962904337912,
+            "timestamp": 0.27835518158299477
+        },
+        {
+            "x": 4.977888469750385,
+            "y": 5.497268135402371,
+            "heading": 3.079534623451408,
+            "angularVelocity": -0.26066900023130835,
+            "velocityX": 3.0819117639443303,
+            "velocityY": -0.2704395302078152,
+            "timestamp": 0.37114024211065966
+        },
+        {
+            "x": 5.327545209579434,
+            "y": 5.4665855253125155,
+            "heading": 3.079534620495457,
+            "angularVelocity": -3.185804655621534e-8,
+            "velocityX": 3.768459467941971,
+            "velocityY": -0.33068480976750836,
+            "timestamp": 0.46392530263832454
+        },
+        {
+            "x": 5.677201949407512,
+            "y": 5.435902915209702,
+            "heading": 3.0795346175395855,
+            "angularVelocity": -3.185719286228054e-8,
+            "velocityX": 3.768459467931527,
+            "velocityY": -0.3306848099071595,
+            "timestamp": 0.5567103631659894
+        },
+        {
+            "x": 6.026858689235635,
+            "y": 5.405220305107383,
+            "heading": 3.0795346145837144,
+            "angularVelocity": -3.185719241382707e-8,
+            "velocityX": 3.7684594679319945,
+            "velocityY": -0.3306848099018315,
+            "timestamp": 0.6494954236936543
+        },
+        {
+            "x": 6.376515440276988,
+            "y": 5.37453782279048,
+            "heading": 3.079534611627843,
+            "angularVelocity": -3.1857194092858966e-8,
+            "velocityX": 3.7684595887836787,
+            "velocityY": -0.3306834326820888,
+            "timestamp": 0.7422804842213192
+        },
+        {
+            "x": 6.7274981570931205,
+            "y": 5.371017863275757,
+            "heading": 3.079534608531603,
+            "angularVelocity": -3.337002331102787e-8,
+            "velocityX": 3.782750313683133,
+            "velocityY": -0.03793670548581862,
+            "timestamp": 0.8350655447489841
+        },
+        {
+            "x": 7.060187879890856,
+            "y": 5.436910997452384,
+            "heading": 3.079737215344131,
+            "angularVelocity": 0.0021836145967468554,
+            "velocityX": 3.585595794255326,
+            "velocityY": 0.7101696523329738,
+            "timestamp": 0.927850605276649
+        },
+        {
+            "x": 7.322040445742604,
+            "y": 5.514304234937123,
+            "heading": 3.085826008048483,
+            "angularVelocity": 0.065622554641053,
+            "velocityX": 2.82214145642183,
+            "velocityY": 0.834113132487145,
+            "timestamp": 1.0206356658043139
+        },
+        {
+            "x": 7.51268687185891,
+            "y": 5.600628661192279,
+            "heading": 3.097884063831068,
+            "angularVelocity": 0.12995686712932528,
+            "velocityX": 2.054710370743838,
+            "velocityY": 0.9303698867493723,
+            "timestamp": 1.1134207263319789
+        },
+        {
+            "x": 7.632022870387607,
+            "y": 5.695007866387598,
+            "heading": 3.115937777513367,
+            "angularVelocity": 0.19457565236926141,
+            "velocityX": 1.2861553126119536,
+            "velocityY": 1.0171810489597053,
+            "timestamp": 1.2062057868596439
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 0.25933293948166103,
+            "velocityX": 0.517078173356003,
+            "velocityY": 1.0992301242503861,
+            "timestamp": 1.2989908473873089
+        },
+        {
+            "x": 7.658324369192282,
+            "y": 5.904176742987938,
+            "heading": 3.16941072815558,
+            "angularVelocity": 0.32296823834349714,
+            "velocityX": -0.23802675880415525,
+            "velocityY": 1.176940730303702,
+            "timestamp": 1.3900546879260585
+        },
+        {
+            "x": 7.56785439743373,
+            "y": 6.01813279809926,
+            "heading": 3.204362801369678,
+            "angularVelocity": 0.3838194502596341,
+            "velocityX": -0.9934785445386147,
+            "velocityY": 1.2513864387569726,
+            "timestamp": 1.481118528464808
+        },
+        {
+            "x": 7.4085407741930185,
+            "y": 6.138375580441298,
+            "heading": 3.2444365070983814,
+            "angularVelocity": 0.44006166983116235,
+            "velocityX": -1.7494718243615388,
+            "velocityY": 1.3204229212238012,
+            "timestamp": 1.5721823690035577
+        },
+        {
+            "x": 7.180297052881674,
+            "y": 6.26393107037867,
+            "heading": 3.2888027958474897,
+            "angularVelocity": 0.48719984229357544,
+            "velocityX": -2.5064144007215123,
+            "velocityY": 1.3787633949387568,
+            "timestamp": 1.6632462095423073
+        },
+        {
+            "x": 6.882954936764855,
+            "y": 6.39196860518662,
+            "heading": 3.335055491791258,
+            "angularVelocity": 0.5079150590414778,
+            "velocityX": -3.2652050952131715,
+            "velocityY": 1.406019491935018,
+            "timestamp": 1.754310050081057
+        },
+        {
+            "x": 6.546228290367871,
+            "y": 6.464686559433002,
+            "heading": 3.335055498171187,
+            "angularVelocity": 7.005996108772343e-8,
+            "velocityX": -3.697698717787989,
+            "velocityY": 0.7985381883321813,
+            "timestamp": 1.8453738906198065
+        },
+        {
+            "x": 6.204219309221898,
+            "y": 6.505949051941506,
+            "heading": 3.3350555015810333,
+            "angularVelocity": 3.7444564221531034e-8,
+            "velocityX": -3.7557056579492913,
+            "velocityY": 0.45311610255384965,
+            "timestamp": 1.936437731158556
+        },
+        {
+            "x": 5.862210306846827,
+            "y": 6.547211368473936,
+            "heading": 3.335055504991925,
+            "angularVelocity": 3.7456047151170976e-8,
+            "velocityX": -3.7557058910725254,
+            "velocityY": 0.45311417010650673,
+            "timestamp": 2.0275015716973055
+        },
+        {
+            "x": 5.587726197097608,
+            "y": 6.580326820073676,
+            "heading": 3.3623069003974067,
+            "angularVelocity": 0.29925594225114466,
+            "velocityX": -3.0141943072610453,
+            "velocityY": 0.3636509442587018,
+            "timestamp": 2.118565412236055
+        },
+        {
+            "x": 5.381863101775985,
+            "y": 6.605163409773074,
+            "heading": 3.382745950256302,
+            "angularVelocity": 0.22444748363294062,
+            "velocityX": -2.2606458733093975,
+            "velocityY": 0.2727382191708659,
+            "timestamp": 2.2096292527748043
+        },
+        {
+            "x": 5.244621034490526,
+            "y": 6.621721136542047,
+            "heading": 3.3963721295427387,
+            "angularVelocity": 0.1496332595442014,
+            "velocityX": -1.5070972899178572,
+            "velocityY": 0.18182548277135752,
+            "timestamp": 2.3006930933135536
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.07481759605718756,
+            "velocityX": -0.7535486542688541,
+            "velocityY": 0.09091274219244924,
+            "timestamp": 2.391756933852303
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0,
+            "velocityX": -1.5663528740094776e-30,
+            "velocityY": 4.877949363390491e-30,
+            "timestamp": 2.4828207743910524
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S2.1.traj
new file mode 100644
index 00000000..e9fe1d34
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S2.1.traj	
@@ -0,0 +1,265 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -2.9840248880147764e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 4.335913751201177,
+            "y": 5.5650572312071045,
+            "heading": 3.14,
+            "angularVelocity": -1.4550376605668876e-16,
+            "velocityX": 0.7787859499921702,
+            "velocityY": 0.0540158823962963,
+            "timestamp": 0.09362489295282081
+        },
+        {
+            "x": 4.481741250496669,
+            "y": 5.575171693405826,
+            "heading": 3.14,
+            "angularVelocity": -2.6552600483679955e-16,
+            "velocityX": 1.5575718668001757,
+            "velocityY": 0.10803176249096925,
+            "timestamp": 0.18724978590564162
+        },
+        {
+            "x": 4.7004824916727515,
+            "y": 5.590343386165186,
+            "heading": 3.14,
+            "angularVelocity": -3.921189080478138e-16,
+            "velocityX": 2.3363577172399013,
+            "velocityY": 0.1620476379823989,
+            "timestamp": 0.2808746788584624
+        },
+        {
+            "x": 4.992137456088294,
+            "y": 5.6105723081922525,
+            "heading": 3.14,
+            "angularVelocity": -4.0854142114404067e-16,
+            "velocityX": 3.1151433685752,
+            "velocityY": 0.2160634996641273,
+            "timestamp": 0.37449957181128324
+        },
+        {
+            "x": 5.345466004271564,
+            "y": 5.63507885367643,
+            "heading": 3.14,
+            "angularVelocity": 5.235558245992657e-16,
+            "velocityX": 3.773873988420134,
+            "velocityY": 0.2617524539817304,
+            "timestamp": 0.4681244647641041
+        },
+        {
+            "x": 5.69879455245494,
+            "y": 5.6595853991606155,
+            "heading": 3.14,
+            "angularVelocity": -5.535576803898805e-16,
+            "velocityX": 3.773873988421262,
+            "velocityY": 0.2617524539818086,
+            "timestamp": 0.5617493577169249
+        },
+        {
+            "x": 6.052123100638315,
+            "y": 5.6840919446448,
+            "heading": 3.14,
+            "angularVelocity": -2.4126594725597773e-16,
+            "velocityX": 3.7738739884212626,
+            "velocityY": 0.2617524539818086,
+            "timestamp": 0.6553742506697458
+        },
+        {
+            "x": 6.405451648821692,
+            "y": 5.708598490128986,
+            "heading": 3.14,
+            "angularVelocity": 3.1966951315745e-16,
+            "velocityX": 3.773873988421262,
+            "velocityY": 0.26175245398180863,
+            "timestamp": 0.7489991436225666
+        },
+        {
+            "x": 6.7587801970050565,
+            "y": 5.733105035613169,
+            "heading": 3.14,
+            "angularVelocity": -3.2948712369984396e-16,
+            "velocityX": 3.773873988421149,
+            "velocityY": 0.26175245398180075,
+            "timestamp": 0.8426240365753874
+        },
+        {
+            "x": 7.088851630801794,
+            "y": 5.755998488879142,
+            "heading": 3.14,
+            "angularVelocity": -9.417571312555162e-16,
+            "velocityX": 3.5254666081494603,
+            "velocityY": 0.24452314490237434,
+            "timestamp": 0.9362489295282083
+        },
+        {
+            "x": 7.3460093413592045,
+            "y": 5.773834712877417,
+            "heading": 3.14,
+            "angularVelocity": -8.090853196926974e-16,
+            "velocityX": 2.746680956815585,
+            "velocityY": 0.19050728322074467,
+            "timestamp": 1.029873822481029
+        },
+        {
+            "x": 7.530253310036057,
+            "y": 5.786613706315055,
+            "heading": 3.14,
+            "angularVelocity": -6.531335051646392e-16,
+            "velocityX": 1.9678951063762244,
+            "velocityY": 0.13649140772934035,
+            "timestamp": 1.1234987154338498
+        },
+        {
+            "x": 7.641583530618614,
+            "y": 5.79433546876108,
+            "heading": 3.14,
+            "angularVelocity": -6.271183503983396e-16,
+            "velocityX": 1.1891091895683832,
+            "velocityY": 0.08247552763467879,
+            "timestamp": 1.2171236083866706
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": -6.05510209215834e-16,
+            "velocityX": 0.41032323957630595,
+            "velocityY": 0.028459645238388953,
+            "timestamp": 1.3107485013394913
+        },
+        {
+            "x": 7.6370908002198155,
+            "y": 5.794023857082849,
+            "heading": 3.14,
+            "angularVelocity": -4.789725529265609e-16,
+            "velocityX": -0.42651589559076464,
+            "velocityY": -0.02958275307434032,
+            "timestamp": 1.4113524887260582
+        },
+        {
+            "x": 7.509992248723152,
+            "y": 5.785208417602395,
+            "heading": 3.14,
+            "angularVelocity": -2.32893689231122e-16,
+            "velocityX": -1.2633550100583235,
+            "velocityY": -0.08762514995137034,
+            "timestamp": 1.5119564761126252
+        },
+        {
+            "x": 7.298704349055096,
+            "y": 5.770553681804523,
+            "heading": 3.14,
+            "angularVelocity": -1.4093989000106693e-18,
+            "velocityX": -2.1001940892878346,
+            "velocityY": -0.14566754438432153,
+            "timestamp": 1.6125604634991921
+        },
+        {
+            "x": 7.003227108598371,
+            "y": 5.75005965020129,
+            "heading": 3.14,
+            "angularVelocity": 2.7928007738396434e-16,
+            "velocityX": -2.9370330951333345,
+            "velocityY": -0.2037099337274247,
+            "timestamp": 1.713164450885759
+        },
+        {
+            "x": 6.623560552157282,
+            "y": 5.723726324513103,
+            "heading": 3.14,
+            "angularVelocity": 4.2079673901381875e-16,
+            "velocityX": -3.773871854424956,
+            "velocityY": -0.2617523059697759,
+            "timestamp": 1.813768438272326
+        },
+        {
+            "x": 6.243893779806084,
+            "y": 5.6973929838495865,
+            "heading": 3.14,
+            "angularVelocity": -4.939929195074519e-16,
+            "velocityX": -3.773874000563654,
+            "velocityY": -0.2617524548239938,
+            "timestamp": 1.914372425658893
+        },
+        {
+            "x": 5.864227007454884,
+            "y": 5.67105964318607,
+            "heading": 3.14,
+            "angularVelocity": 2.0250011365009936e-16,
+            "velocityX": -3.773874000563654,
+            "velocityY": -0.26175245482399373,
+            "timestamp": 2.01497641304546
+        },
+        {
+            "x": 5.484560235103686,
+            "y": 5.644726302522554,
+            "heading": 3.14,
+            "angularVelocity": -5.77312332256685e-16,
+            "velocityX": -3.773874000563654,
+            "velocityY": -0.2617524548239938,
+            "timestamp": 2.115580400432027
+        },
+        {
+            "x": 5.104893462752507,
+            "y": 5.61839296185904,
+            "heading": 3.14,
+            "angularVelocity": -1.6679194482656825e-16,
+            "velocityX": -3.773874000563452,
+            "velocityY": -0.26175245482397974,
+            "timestamp": 2.2161843878185943
+        },
+        {
+            "x": 4.768136096424167,
+            "y": 5.595035778417478,
+            "heading": 3.14,
+            "angularVelocity": 9.060703447293421e-16,
+            "velocityX": -3.3473560549281536,
+            "velocityY": -0.2321695595604253,
+            "timestamp": 2.3167883752051615
+        },
+        {
+            "x": 4.515568053067083,
+            "y": 5.577517889545478,
+            "heading": 3.14,
+            "angularVelocity": 6.700681499162007e-16,
+            "velocityX": -2.51051722618707,
+            "velocityY": -0.1741271825011227,
+            "timestamp": 2.4173923625917286
+        },
+        {
+            "x": 4.34718935210125,
+            "y": 5.565839296589989,
+            "heading": 3.14,
+            "angularVelocity": 4.365333326959493e-16,
+            "velocityX": -1.6736782044119611,
+            "velocityY": -0.11608479205315635,
+            "timestamp": 2.517996349978296
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 2.0339545203820728e-16,
+            "velocityX": -0.836839118292156,
+            "velocityY": -0.05804239714230061,
+            "timestamp": 2.618600337364863
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 2.71920432475143
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S2.traj
new file mode 100644
index 00000000..e9fe1d34
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S2.traj	
@@ -0,0 +1,265 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -2.9840248880147764e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 4.335913751201177,
+            "y": 5.5650572312071045,
+            "heading": 3.14,
+            "angularVelocity": -1.4550376605668876e-16,
+            "velocityX": 0.7787859499921702,
+            "velocityY": 0.0540158823962963,
+            "timestamp": 0.09362489295282081
+        },
+        {
+            "x": 4.481741250496669,
+            "y": 5.575171693405826,
+            "heading": 3.14,
+            "angularVelocity": -2.6552600483679955e-16,
+            "velocityX": 1.5575718668001757,
+            "velocityY": 0.10803176249096925,
+            "timestamp": 0.18724978590564162
+        },
+        {
+            "x": 4.7004824916727515,
+            "y": 5.590343386165186,
+            "heading": 3.14,
+            "angularVelocity": -3.921189080478138e-16,
+            "velocityX": 2.3363577172399013,
+            "velocityY": 0.1620476379823989,
+            "timestamp": 0.2808746788584624
+        },
+        {
+            "x": 4.992137456088294,
+            "y": 5.6105723081922525,
+            "heading": 3.14,
+            "angularVelocity": -4.0854142114404067e-16,
+            "velocityX": 3.1151433685752,
+            "velocityY": 0.2160634996641273,
+            "timestamp": 0.37449957181128324
+        },
+        {
+            "x": 5.345466004271564,
+            "y": 5.63507885367643,
+            "heading": 3.14,
+            "angularVelocity": 5.235558245992657e-16,
+            "velocityX": 3.773873988420134,
+            "velocityY": 0.2617524539817304,
+            "timestamp": 0.4681244647641041
+        },
+        {
+            "x": 5.69879455245494,
+            "y": 5.6595853991606155,
+            "heading": 3.14,
+            "angularVelocity": -5.535576803898805e-16,
+            "velocityX": 3.773873988421262,
+            "velocityY": 0.2617524539818086,
+            "timestamp": 0.5617493577169249
+        },
+        {
+            "x": 6.052123100638315,
+            "y": 5.6840919446448,
+            "heading": 3.14,
+            "angularVelocity": -2.4126594725597773e-16,
+            "velocityX": 3.7738739884212626,
+            "velocityY": 0.2617524539818086,
+            "timestamp": 0.6553742506697458
+        },
+        {
+            "x": 6.405451648821692,
+            "y": 5.708598490128986,
+            "heading": 3.14,
+            "angularVelocity": 3.1966951315745e-16,
+            "velocityX": 3.773873988421262,
+            "velocityY": 0.26175245398180863,
+            "timestamp": 0.7489991436225666
+        },
+        {
+            "x": 6.7587801970050565,
+            "y": 5.733105035613169,
+            "heading": 3.14,
+            "angularVelocity": -3.2948712369984396e-16,
+            "velocityX": 3.773873988421149,
+            "velocityY": 0.26175245398180075,
+            "timestamp": 0.8426240365753874
+        },
+        {
+            "x": 7.088851630801794,
+            "y": 5.755998488879142,
+            "heading": 3.14,
+            "angularVelocity": -9.417571312555162e-16,
+            "velocityX": 3.5254666081494603,
+            "velocityY": 0.24452314490237434,
+            "timestamp": 0.9362489295282083
+        },
+        {
+            "x": 7.3460093413592045,
+            "y": 5.773834712877417,
+            "heading": 3.14,
+            "angularVelocity": -8.090853196926974e-16,
+            "velocityX": 2.746680956815585,
+            "velocityY": 0.19050728322074467,
+            "timestamp": 1.029873822481029
+        },
+        {
+            "x": 7.530253310036057,
+            "y": 5.786613706315055,
+            "heading": 3.14,
+            "angularVelocity": -6.531335051646392e-16,
+            "velocityX": 1.9678951063762244,
+            "velocityY": 0.13649140772934035,
+            "timestamp": 1.1234987154338498
+        },
+        {
+            "x": 7.641583530618614,
+            "y": 5.79433546876108,
+            "heading": 3.14,
+            "angularVelocity": -6.271183503983396e-16,
+            "velocityX": 1.1891091895683832,
+            "velocityY": 0.08247552763467879,
+            "timestamp": 1.2171236083866706
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": -6.05510209215834e-16,
+            "velocityX": 0.41032323957630595,
+            "velocityY": 0.028459645238388953,
+            "timestamp": 1.3107485013394913
+        },
+        {
+            "x": 7.6370908002198155,
+            "y": 5.794023857082849,
+            "heading": 3.14,
+            "angularVelocity": -4.789725529265609e-16,
+            "velocityX": -0.42651589559076464,
+            "velocityY": -0.02958275307434032,
+            "timestamp": 1.4113524887260582
+        },
+        {
+            "x": 7.509992248723152,
+            "y": 5.785208417602395,
+            "heading": 3.14,
+            "angularVelocity": -2.32893689231122e-16,
+            "velocityX": -1.2633550100583235,
+            "velocityY": -0.08762514995137034,
+            "timestamp": 1.5119564761126252
+        },
+        {
+            "x": 7.298704349055096,
+            "y": 5.770553681804523,
+            "heading": 3.14,
+            "angularVelocity": -1.4093989000106693e-18,
+            "velocityX": -2.1001940892878346,
+            "velocityY": -0.14566754438432153,
+            "timestamp": 1.6125604634991921
+        },
+        {
+            "x": 7.003227108598371,
+            "y": 5.75005965020129,
+            "heading": 3.14,
+            "angularVelocity": 2.7928007738396434e-16,
+            "velocityX": -2.9370330951333345,
+            "velocityY": -0.2037099337274247,
+            "timestamp": 1.713164450885759
+        },
+        {
+            "x": 6.623560552157282,
+            "y": 5.723726324513103,
+            "heading": 3.14,
+            "angularVelocity": 4.2079673901381875e-16,
+            "velocityX": -3.773871854424956,
+            "velocityY": -0.2617523059697759,
+            "timestamp": 1.813768438272326
+        },
+        {
+            "x": 6.243893779806084,
+            "y": 5.6973929838495865,
+            "heading": 3.14,
+            "angularVelocity": -4.939929195074519e-16,
+            "velocityX": -3.773874000563654,
+            "velocityY": -0.2617524548239938,
+            "timestamp": 1.914372425658893
+        },
+        {
+            "x": 5.864227007454884,
+            "y": 5.67105964318607,
+            "heading": 3.14,
+            "angularVelocity": 2.0250011365009936e-16,
+            "velocityX": -3.773874000563654,
+            "velocityY": -0.26175245482399373,
+            "timestamp": 2.01497641304546
+        },
+        {
+            "x": 5.484560235103686,
+            "y": 5.644726302522554,
+            "heading": 3.14,
+            "angularVelocity": -5.77312332256685e-16,
+            "velocityX": -3.773874000563654,
+            "velocityY": -0.2617524548239938,
+            "timestamp": 2.115580400432027
+        },
+        {
+            "x": 5.104893462752507,
+            "y": 5.61839296185904,
+            "heading": 3.14,
+            "angularVelocity": -1.6679194482656825e-16,
+            "velocityX": -3.773874000563452,
+            "velocityY": -0.26175245482397974,
+            "timestamp": 2.2161843878185943
+        },
+        {
+            "x": 4.768136096424167,
+            "y": 5.595035778417478,
+            "heading": 3.14,
+            "angularVelocity": 9.060703447293421e-16,
+            "velocityX": -3.3473560549281536,
+            "velocityY": -0.2321695595604253,
+            "timestamp": 2.3167883752051615
+        },
+        {
+            "x": 4.515568053067083,
+            "y": 5.577517889545478,
+            "heading": 3.14,
+            "angularVelocity": 6.700681499162007e-16,
+            "velocityX": -2.51051722618707,
+            "velocityY": -0.1741271825011227,
+            "timestamp": 2.4173923625917286
+        },
+        {
+            "x": 4.34718935210125,
+            "y": 5.565839296589989,
+            "heading": 3.14,
+            "angularVelocity": 4.365333326959493e-16,
+            "velocityX": -1.6736782044119611,
+            "velocityY": -0.11608479205315635,
+            "timestamp": 2.517996349978296
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 2.0339545203820728e-16,
+            "velocityX": -0.836839118292156,
+            "velocityY": -0.05804239714230061,
+            "timestamp": 2.618600337364863
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 2.71920432475143
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S1.1.traj
new file mode 100644
index 00000000..28f6b2dd
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S1.1.traj	
@@ -0,0 +1,292 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -4.804532357029023e-32,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.321076149980653,
+            "y": 5.523337393191978,
+            "heading": 3.1351500057236232,
+            "angularVelocity": -0.05343391557798715,
+            "velocityX": 0.6398432489449108,
+            "velocityY": -0.40392349462987753,
+            "timestamp": 0.09076621512600157
+        },
+        {
+            "x": 4.4372316676864765,
+            "y": 5.45001014084527,
+            "heading": 3.1261940931811654,
+            "angularVelocity": -0.098670111230538,
+            "velocityX": 1.2797219487953386,
+            "velocityY": -0.807869450598055,
+            "timestamp": 0.18153243025200314
+        },
+        {
+            "x": 4.611470902956015,
+            "y": 5.340015489147945,
+            "heading": 3.1144364232978923,
+            "angularVelocity": -0.1295379549202309,
+            "velocityX": 1.9196485721880063,
+            "velocityY": -1.2118457461802494,
+            "timestamp": 0.2722986453780047
+        },
+        {
+            "x": 4.843797638177826,
+            "y": 5.193351042771614,
+            "heading": 3.102754458759954,
+            "angularVelocity": -0.1287038852696506,
+            "velocityX": 2.5596168673475677,
+            "velocityY": -1.6158484318502528,
+            "timestamp": 0.36306486050400627
+        },
+        {
+            "x": 5.1341460210960115,
+            "y": 5.0100588028009865,
+            "heading": 3.102754388459234,
+            "angularVelocity": -7.745251732579077e-7,
+            "velocityX": 3.1988596474483897,
+            "velocityY": -2.0193883783319753,
+            "timestamp": 0.45383107563000785
+        },
+        {
+            "x": 5.424494292461022,
+            "y": 4.826766239115272,
+            "heading": 3.1027543844284144,
+            "angularVelocity": -4.440880652615192e-8,
+            "velocityX": 3.1988584184318825,
+            "velocityY": -2.019391944803125,
+            "timestamp": 0.5445972907560094
+        },
+        {
+            "x": 5.714842563825266,
+            "y": 4.643473675428344,
+            "heading": 3.102754380397595,
+            "angularVelocity": -4.440880657505379e-8,
+            "velocityX": 3.1988584184234474,
+            "velocityY": -2.019391944816487,
+            "timestamp": 0.635363505882011
+        },
+        {
+            "x": 6.005190835190169,
+            "y": 4.460181111742461,
+            "heading": 3.102754376366776,
+            "angularVelocity": -4.4408806414514103e-8,
+            "velocityX": 3.198858418430713,
+            "velocityY": -2.019391944804976,
+            "timestamp": 0.7261297210080125
+        },
+        {
+            "x": 6.295539259817851,
+            "y": 4.276888790835792,
+            "heading": 3.1027543723359563,
+            "angularVelocity": -4.440881138366432e-8,
+            "velocityX": 3.1988601069750424,
+            "velocityY": -2.019389270029866,
+            "timestamp": 0.8168959361340141
+        },
+        {
+            "x": 6.605220322395041,
+            "y": 4.128578571418711,
+            "heading": 3.1027543676059754,
+            "angularVelocity": -5.211168523884132e-8,
+            "velocityX": 3.4118538725812453,
+            "velocityY": -1.633980432159657,
+            "timestamp": 0.9076621512600157
+        },
+        {
+            "x": 6.930843742926924,
+            "y": 4.047062548737473,
+            "heading": 3.096403794352487,
+            "angularVelocity": -0.0699662671256465,
+            "velocityX": 3.5874958549263174,
+            "velocityY": -0.8980877143337577,
+            "timestamp": 0.9984283663860173
+        },
+        {
+            "x": 7.20268686077656,
+            "y": 4.00824503573056,
+            "heading": 3.096579244871637,
+            "angularVelocity": 0.0019329936684692615,
+            "velocityX": 2.9949813096454783,
+            "velocityY": -0.4276647754125783,
+            "timestamp": 1.0891945815120188
+        },
+        {
+            "x": 7.418574834249914,
+            "y": 4.009226418838573,
+            "heading": 3.1038020925835856,
+            "angularVelocity": 0.07957638976044962,
+            "velocityX": 2.37850584794857,
+            "velocityY": 0.010812207016131005,
+            "timestamp": 1.1799607966380203
+        },
+        {
+            "x": 7.577796617923462,
+            "y": 4.04898509700118,
+            "heading": 3.1182467242773515,
+            "angularVelocity": 0.15914106007078685,
+            "velocityX": 1.7541965747113617,
+            "velocityY": 0.43803388857203507,
+            "timestamp": 1.270727011764022
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.23966269489643754,
+            "velocityX": 1.1260068730932273,
+            "velocityY": 0.8595147752996188,
+            "timestamp": 1.3614932268900235
+        },
+        {
+            "x": 7.725157318651882,
+            "y": 4.240486437949111,
+            "heading": 3.168528121789897,
+            "angularVelocity": 0.31944830036475336,
+            "velocityX": 0.505656446597267,
+            "velocityY": 1.2707829132351232,
+            "timestamp": 1.4507975736570093
+        },
+        {
+            "x": 7.714677077614687,
+            "y": 4.390346197703396,
+            "heading": 3.203767814945465,
+            "angularVelocity": 0.39460221625624564,
+            "velocityX": -0.11735420969529425,
+            "velocityY": 1.6780791213366064,
+            "timestamp": 1.5401019204239952
+        },
+        {
+            "x": 7.648174313953831,
+            "y": 4.575993018943536,
+            "heading": 3.2450393487188225,
+            "angularVelocity": 0.4621447361464423,
+            "velocityX": -0.7446755512850395,
+            "velocityY": 2.0788105838177287,
+            "timestamp": 1.629406267190981
+        },
+        {
+            "x": 7.524918843988051,
+            "y": 4.7962734130800495,
+            "heading": 3.2910178393067144,
+            "angularVelocity": 0.5148516533899635,
+            "velocityX": -1.3801732438330052,
+            "velocityY": 2.4666256695351096,
+            "timestamp": 1.7187106139579669
+        },
+        {
+            "x": 7.343009789801653,
+            "y": 5.047887037555876,
+            "heading": 3.3379953942154983,
+            "angularVelocity": 0.5260388391996106,
+            "velocityX": -2.0369563271207567,
+            "velocityY": 2.817484630757546,
+            "timestamp": 1.8080149607249527
+        },
+        {
+            "x": 7.098537759361482,
+            "y": 5.281049171644365,
+            "heading": 3.337995405344767,
+            "angularVelocity": 1.2462180234431456e-7,
+            "velocityX": -2.7375154658266756,
+            "velocityY": 2.6108710553234187,
+            "timestamp": 1.8973193074919386
+        },
+        {
+            "x": 6.821988530708413,
+            "y": 5.475089583706822,
+            "heading": 3.337995410181252,
+            "angularVelocity": 5.415732950343717e-8,
+            "velocityX": -3.096705128750813,
+            "velocityY": 2.1727991871297463,
+            "timestamp": 1.9866236542589244
+        },
+        {
+            "x": 6.5454389358432925,
+            "y": 5.669129473837467,
+            "heading": 3.3379954150177356,
+            "angularVelocity": 5.4157314847566374e-8,
+            "velocityX": -3.0967092294700773,
+            "velocityY": 2.172793342713053,
+            "timestamp": 2.0759280010259102
+        },
+        {
+            "x": 6.268889340976521,
+            "y": 5.86316936396576,
+            "heading": 3.337995419854219,
+            "angularVelocity": 5.415731402245697e-8,
+            "velocityX": -3.096709229488555,
+            "velocityY": 2.172793342686719,
+            "timestamp": 2.165232347792896
+        },
+        {
+            "x": 5.9923397461188825,
+            "y": 6.057209254107063,
+            "heading": 3.337995424690707,
+            "angularVelocity": 5.415735914657007e-8,
+            "velocityX": -3.0967092293862986,
+            "velocityY": 2.1727933428323993,
+            "timestamp": 2.2545366945598815
+        },
+        {
+            "x": 5.720218360303881,
+            "y": 6.248146363272753,
+            "heading": 3.35123648127501,
+            "angularVelocity": 0.14826889242971145,
+            "velocityX": -3.04712363581837,
+            "velocityY": 2.1380494463935373,
+            "timestamp": 2.343841041326867
+        },
+        {
+            "x": 5.5025203930179725,
+            "y": 6.400895625132601,
+            "heading": 3.369358291202865,
+            "angularVelocity": 0.20292192467559766,
+            "velocityX": -2.437708523348032,
+            "velocityY": 1.7104347928147705,
+            "timestamp": 2.4331453880938527
+        },
+        {
+            "x": 5.339255695066747,
+            "y": 6.515451087573219,
+            "heading": 3.385376867292448,
+            "angularVelocity": 0.17937062046238467,
+            "velocityX": -1.828183104874154,
+            "velocityY": 1.28275348947479,
+            "timestamp": 2.5224497348608383
+        },
+        {
+            "x": 5.230417322474398,
+            "y": 6.591817929626251,
+            "heading": 3.3970262318270823,
+            "angularVelocity": 0.1304456608929641,
+            "velocityX": -1.218735442702819,
+            "velocityY": 0.8551301791870324,
+            "timestamp": 2.611754081627824
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.06896725160058048,
+            "velocityX": -0.6093468509028436,
+            "velocityY": 0.42754996543868296,
+            "timestamp": 2.7010584283948096
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 2.6480923788288046e-29,
+            "velocityX": -1.5869679624644047e-31,
+            "velocityY": 0,
+            "timestamp": 2.790362775161795
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S1.traj
new file mode 100644
index 00000000..28f6b2dd
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S1.traj	
@@ -0,0 +1,292 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -4.804532357029023e-32,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.321076149980653,
+            "y": 5.523337393191978,
+            "heading": 3.1351500057236232,
+            "angularVelocity": -0.05343391557798715,
+            "velocityX": 0.6398432489449108,
+            "velocityY": -0.40392349462987753,
+            "timestamp": 0.09076621512600157
+        },
+        {
+            "x": 4.4372316676864765,
+            "y": 5.45001014084527,
+            "heading": 3.1261940931811654,
+            "angularVelocity": -0.098670111230538,
+            "velocityX": 1.2797219487953386,
+            "velocityY": -0.807869450598055,
+            "timestamp": 0.18153243025200314
+        },
+        {
+            "x": 4.611470902956015,
+            "y": 5.340015489147945,
+            "heading": 3.1144364232978923,
+            "angularVelocity": -0.1295379549202309,
+            "velocityX": 1.9196485721880063,
+            "velocityY": -1.2118457461802494,
+            "timestamp": 0.2722986453780047
+        },
+        {
+            "x": 4.843797638177826,
+            "y": 5.193351042771614,
+            "heading": 3.102754458759954,
+            "angularVelocity": -0.1287038852696506,
+            "velocityX": 2.5596168673475677,
+            "velocityY": -1.6158484318502528,
+            "timestamp": 0.36306486050400627
+        },
+        {
+            "x": 5.1341460210960115,
+            "y": 5.0100588028009865,
+            "heading": 3.102754388459234,
+            "angularVelocity": -7.745251732579077e-7,
+            "velocityX": 3.1988596474483897,
+            "velocityY": -2.0193883783319753,
+            "timestamp": 0.45383107563000785
+        },
+        {
+            "x": 5.424494292461022,
+            "y": 4.826766239115272,
+            "heading": 3.1027543844284144,
+            "angularVelocity": -4.440880652615192e-8,
+            "velocityX": 3.1988584184318825,
+            "velocityY": -2.019391944803125,
+            "timestamp": 0.5445972907560094
+        },
+        {
+            "x": 5.714842563825266,
+            "y": 4.643473675428344,
+            "heading": 3.102754380397595,
+            "angularVelocity": -4.440880657505379e-8,
+            "velocityX": 3.1988584184234474,
+            "velocityY": -2.019391944816487,
+            "timestamp": 0.635363505882011
+        },
+        {
+            "x": 6.005190835190169,
+            "y": 4.460181111742461,
+            "heading": 3.102754376366776,
+            "angularVelocity": -4.4408806414514103e-8,
+            "velocityX": 3.198858418430713,
+            "velocityY": -2.019391944804976,
+            "timestamp": 0.7261297210080125
+        },
+        {
+            "x": 6.295539259817851,
+            "y": 4.276888790835792,
+            "heading": 3.1027543723359563,
+            "angularVelocity": -4.440881138366432e-8,
+            "velocityX": 3.1988601069750424,
+            "velocityY": -2.019389270029866,
+            "timestamp": 0.8168959361340141
+        },
+        {
+            "x": 6.605220322395041,
+            "y": 4.128578571418711,
+            "heading": 3.1027543676059754,
+            "angularVelocity": -5.211168523884132e-8,
+            "velocityX": 3.4118538725812453,
+            "velocityY": -1.633980432159657,
+            "timestamp": 0.9076621512600157
+        },
+        {
+            "x": 6.930843742926924,
+            "y": 4.047062548737473,
+            "heading": 3.096403794352487,
+            "angularVelocity": -0.0699662671256465,
+            "velocityX": 3.5874958549263174,
+            "velocityY": -0.8980877143337577,
+            "timestamp": 0.9984283663860173
+        },
+        {
+            "x": 7.20268686077656,
+            "y": 4.00824503573056,
+            "heading": 3.096579244871637,
+            "angularVelocity": 0.0019329936684692615,
+            "velocityX": 2.9949813096454783,
+            "velocityY": -0.4276647754125783,
+            "timestamp": 1.0891945815120188
+        },
+        {
+            "x": 7.418574834249914,
+            "y": 4.009226418838573,
+            "heading": 3.1038020925835856,
+            "angularVelocity": 0.07957638976044962,
+            "velocityX": 2.37850584794857,
+            "velocityY": 0.010812207016131005,
+            "timestamp": 1.1799607966380203
+        },
+        {
+            "x": 7.577796617923462,
+            "y": 4.04898509700118,
+            "heading": 3.1182467242773515,
+            "angularVelocity": 0.15914106007078685,
+            "velocityX": 1.7541965747113617,
+            "velocityY": 0.43803388857203507,
+            "timestamp": 1.270727011764022
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.23966269489643754,
+            "velocityX": 1.1260068730932273,
+            "velocityY": 0.8595147752996188,
+            "timestamp": 1.3614932268900235
+        },
+        {
+            "x": 7.725157318651882,
+            "y": 4.240486437949111,
+            "heading": 3.168528121789897,
+            "angularVelocity": 0.31944830036475336,
+            "velocityX": 0.505656446597267,
+            "velocityY": 1.2707829132351232,
+            "timestamp": 1.4507975736570093
+        },
+        {
+            "x": 7.714677077614687,
+            "y": 4.390346197703396,
+            "heading": 3.203767814945465,
+            "angularVelocity": 0.39460221625624564,
+            "velocityX": -0.11735420969529425,
+            "velocityY": 1.6780791213366064,
+            "timestamp": 1.5401019204239952
+        },
+        {
+            "x": 7.648174313953831,
+            "y": 4.575993018943536,
+            "heading": 3.2450393487188225,
+            "angularVelocity": 0.4621447361464423,
+            "velocityX": -0.7446755512850395,
+            "velocityY": 2.0788105838177287,
+            "timestamp": 1.629406267190981
+        },
+        {
+            "x": 7.524918843988051,
+            "y": 4.7962734130800495,
+            "heading": 3.2910178393067144,
+            "angularVelocity": 0.5148516533899635,
+            "velocityX": -1.3801732438330052,
+            "velocityY": 2.4666256695351096,
+            "timestamp": 1.7187106139579669
+        },
+        {
+            "x": 7.343009789801653,
+            "y": 5.047887037555876,
+            "heading": 3.3379953942154983,
+            "angularVelocity": 0.5260388391996106,
+            "velocityX": -2.0369563271207567,
+            "velocityY": 2.817484630757546,
+            "timestamp": 1.8080149607249527
+        },
+        {
+            "x": 7.098537759361482,
+            "y": 5.281049171644365,
+            "heading": 3.337995405344767,
+            "angularVelocity": 1.2462180234431456e-7,
+            "velocityX": -2.7375154658266756,
+            "velocityY": 2.6108710553234187,
+            "timestamp": 1.8973193074919386
+        },
+        {
+            "x": 6.821988530708413,
+            "y": 5.475089583706822,
+            "heading": 3.337995410181252,
+            "angularVelocity": 5.415732950343717e-8,
+            "velocityX": -3.096705128750813,
+            "velocityY": 2.1727991871297463,
+            "timestamp": 1.9866236542589244
+        },
+        {
+            "x": 6.5454389358432925,
+            "y": 5.669129473837467,
+            "heading": 3.3379954150177356,
+            "angularVelocity": 5.4157314847566374e-8,
+            "velocityX": -3.0967092294700773,
+            "velocityY": 2.172793342713053,
+            "timestamp": 2.0759280010259102
+        },
+        {
+            "x": 6.268889340976521,
+            "y": 5.86316936396576,
+            "heading": 3.337995419854219,
+            "angularVelocity": 5.415731402245697e-8,
+            "velocityX": -3.096709229488555,
+            "velocityY": 2.172793342686719,
+            "timestamp": 2.165232347792896
+        },
+        {
+            "x": 5.9923397461188825,
+            "y": 6.057209254107063,
+            "heading": 3.337995424690707,
+            "angularVelocity": 5.415735914657007e-8,
+            "velocityX": -3.0967092293862986,
+            "velocityY": 2.1727933428323993,
+            "timestamp": 2.2545366945598815
+        },
+        {
+            "x": 5.720218360303881,
+            "y": 6.248146363272753,
+            "heading": 3.35123648127501,
+            "angularVelocity": 0.14826889242971145,
+            "velocityX": -3.04712363581837,
+            "velocityY": 2.1380494463935373,
+            "timestamp": 2.343841041326867
+        },
+        {
+            "x": 5.5025203930179725,
+            "y": 6.400895625132601,
+            "heading": 3.369358291202865,
+            "angularVelocity": 0.20292192467559766,
+            "velocityX": -2.437708523348032,
+            "velocityY": 1.7104347928147705,
+            "timestamp": 2.4331453880938527
+        },
+        {
+            "x": 5.339255695066747,
+            "y": 6.515451087573219,
+            "heading": 3.385376867292448,
+            "angularVelocity": 0.17937062046238467,
+            "velocityX": -1.828183104874154,
+            "velocityY": 1.28275348947479,
+            "timestamp": 2.5224497348608383
+        },
+        {
+            "x": 5.230417322474398,
+            "y": 6.591817929626251,
+            "heading": 3.3970262318270823,
+            "angularVelocity": 0.1304456608929641,
+            "velocityX": -1.218735442702819,
+            "velocityY": 0.8551301791870324,
+            "timestamp": 2.611754081627824
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.06896725160058048,
+            "velocityX": -0.6093468509028436,
+            "velocityY": 0.42754996543868296,
+            "timestamp": 2.7010584283948096
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 2.6480923788288046e-29,
+            "velocityX": -1.5869679624644047e-31,
+            "velocityY": 0,
+            "timestamp": 2.790362775161795
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S2.1.traj
new file mode 100644
index 00000000..698e9db0
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S2.1.traj	
@@ -0,0 +1,283 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -2.7122542067664996e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 4.329648409276986,
+            "y": 5.532049408693613,
+            "heading": 3.14,
+            "angularVelocity": 3.6127527846474837e-17,
+            "velocityX": 0.715878164292546,
+            "velocityY": -0.3002204885663498,
+            "timestamp": 0.09310021257998485
+        },
+        {
+            "x": 4.462945224981569,
+            "y": 5.4761482272758,
+            "heading": 3.14,
+            "angularVelocity": 1.058102916611751e-16,
+            "velocityX": 1.4317562979794687,
+            "velocityY": -0.6004409642975056,
+            "timestamp": 0.1862004251599697
+        },
+        {
+            "x": 4.662890441407609,
+            "y": 5.392296458139565,
+            "heading": 3.14,
+            "angularVelocity": 1.669202321487025e-16,
+            "velocityX": 2.147634370376114,
+            "velocityY": -0.9006614143251297,
+            "timestamp": 0.27930063773995456
+        },
+        {
+            "x": 4.929484041376741,
+            "y": 5.280494108489063,
+            "heading": 3.14,
+            "angularVelocity": 1.646239395597301e-16,
+            "velocityX": 2.8635122582579458,
+            "velocityY": -1.2008817869720905,
+            "timestamp": 0.3724008503199394
+        },
+        {
+            "x": 5.2542718637391,
+            "y": 5.1442866313321245,
+            "heading": 3.14,
+            "angularVelocity": -2.6454281962617153e-17,
+            "velocityX": 3.4885830371581816,
+            "velocityY": -1.4630200445559467,
+            "timestamp": 0.46550106289992427
+        },
+        {
+            "x": 5.579059685941281,
+            "y": 5.008079154242361,
+            "heading": 3.14,
+            "angularVelocity": -9.771658175099026e-17,
+            "velocityX": 3.4885830354377245,
+            "velocityY": -1.463020043834432,
+            "timestamp": 0.5586012754799091
+        },
+        {
+            "x": 5.903847508143462,
+            "y": 4.871871677152596,
+            "heading": 3.14,
+            "angularVelocity": -3.1326214825920436e-16,
+            "velocityX": 3.488583035437725,
+            "velocityY": -1.4630200438344323,
+            "timestamp": 0.6517014880598939
+        },
+        {
+            "x": 6.228635330345643,
+            "y": 4.735664200062831,
+            "heading": 3.14,
+            "angularVelocity": -1.2137930063882497e-16,
+            "velocityX": 3.488583035437725,
+            "velocityY": -1.4630200438344325,
+            "timestamp": 0.7448017006398788
+        },
+        {
+            "x": 6.553423152547824,
+            "y": 4.599456722973066,
+            "heading": 3.14,
+            "angularVelocity": 1.3394779600477518e-16,
+            "velocityX": 3.488583035437725,
+            "velocityY": -1.4630200438344325,
+            "timestamp": 0.8379019132198637
+        },
+        {
+            "x": 6.878210974643575,
+            "y": 4.463249245927935,
+            "heading": 3.14,
+            "angularVelocity": 2.91910120342859e-17,
+            "velocityX": 3.488583034294527,
+            "velocityY": -1.4630200433550054,
+            "timestamp": 0.9310021257998486
+        },
+        {
+            "x": 7.17186557168986,
+            "y": 4.3400982252760985,
+            "heading": 3.14,
+            "angularVelocity": -6.102921228742647e-16,
+            "velocityX": 3.1541775137602737,
+            "velocityY": -1.3227791563413718,
+            "timestamp": 1.0241023383798336
+        },
+        {
+            "x": 7.3988717849138865,
+            "y": 4.2448977852556045,
+            "heading": 3.14,
+            "angularVelocity": -6.383129451815479e-16,
+            "velocityX": 2.4382996228798555,
+            "velocityY": -1.0225587824368827,
+            "timestamp": 1.1172025509598185
+        },
+        {
+            "x": 7.559229597346586,
+            "y": 4.1776479329828335,
+            "heading": 3.14,
+            "angularVelocity": -7.263779621185202e-16,
+            "velocityX": 1.7224215497327395,
+            "velocityY": -0.7223383320945309,
+            "timestamp": 1.2103027635398034
+        },
+        {
+            "x": 7.65293900332055,
+            "y": 4.138348670834548,
+            "heading": 3.14,
+            "angularVelocity": -7.150088563233795e-16,
+            "velocityX": 1.0065434157113666,
+            "velocityY": -0.42211785622311543,
+            "timestamp": 1.3034029761197883
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": -7.092042046536786e-16,
+            "velocityX": 0.2906652512305344,
+            "velocityY": -0.12189736757780324,
+            "timestamp": 1.3965031886997732
+        },
+        {
+            "x": 7.634550444733744,
+            "y": 4.146060349047862,
+            "heading": 3.14,
+            "angularVelocity": -8.252913531228688e-16,
+            "velocityX": -0.4634351183891671,
+            "velocityY": 0.19435250940932913,
+            "timestamp": 1.4945742097824644
+        },
+        {
+            "x": 7.515145498014414,
+            "y": 4.196135645696618,
+            "heading": 3.14,
+            "angularVelocity": -8.640334878143468e-16,
+            "velocityX": -1.217535469712484,
+            "velocityY": 0.5106023787234387,
+            "timestamp": 1.5926452308651555
+        },
+        {
+            "x": 7.321785162832602,
+            "y": 4.277225888692093,
+            "heading": 3.14,
+            "angularVelocity": -7.792870163094326e-16,
+            "velocityX": -1.971635790541653,
+            "velocityY": 0.826852235249104,
+            "timestamp": 1.6907162519478467
+        },
+        {
+            "x": 7.054469445169555,
+            "y": 4.389331075525908,
+            "heading": 3.14,
+            "angularVelocity": -9.085136126677818e-16,
+            "velocityX": -2.725736050381882,
+            "velocityY": 1.1431020661976106,
+            "timestamp": 1.7887872730305379
+        },
+        {
+            "x": 6.713198362969528,
+            "y": 4.532451198672714,
+            "heading": 3.14,
+            "angularVelocity": -7.684534164287992e-16,
+            "velocityX": -3.4798361272500533,
+            "velocityY": 1.4593518204124447,
+            "timestamp": 1.886858294113229
+        },
+        {
+            "x": 6.371069462531013,
+            "y": 4.675931068244968,
+            "heading": 3.14,
+            "angularVelocity": -1.8367347430029744e-16,
+            "velocityX": -3.4885830356557705,
+            "velocityY": 1.4630200439258751,
+            "timestamp": 1.9849293151959202
+        },
+        {
+            "x": 6.028940562092558,
+            "y": 4.819410937817198,
+            "heading": 3.14,
+            "angularVelocity": 1.6315422621813397e-16,
+            "velocityX": -3.48858303565516,
+            "velocityY": 1.463020043925619,
+            "timestamp": 2.0830003362786114
+        },
+        {
+            "x": 5.686811661654102,
+            "y": 4.962890807389427,
+            "heading": 3.14,
+            "angularVelocity": -1.6415107007869627e-16,
+            "velocityX": -3.4885830356551595,
+            "velocityY": 1.4630200439256191,
+            "timestamp": 2.1810713573613025
+        },
+        {
+            "x": 5.344682761215647,
+            "y": 5.106370676961656,
+            "heading": 3.14,
+            "angularVelocity": -2.6663466601394013e-16,
+            "velocityX": -3.48858303565516,
+            "velocityY": 1.4630200439256194,
+            "timestamp": 2.2791423784439937
+        },
+        {
+            "x": 5.002553860860093,
+            "y": 5.249850546499118,
+            "heading": 3.14,
+            "angularVelocity": -2.7119156877069447e-16,
+            "velocityX": -3.4885830348098277,
+            "velocityY": 1.4630200435711092,
+            "timestamp": 2.377213399526685
+        },
+        {
+            "x": 4.706732333820954,
+            "y": 5.373910320642252,
+            "heading": 3.14,
+            "angularVelocity": 2.3228605057035087e-16,
+            "velocityX": -3.01640100993532,
+            "velocityY": 1.264999311453705,
+            "timestamp": 2.475284420609376
+        },
+        {
+            "x": 4.4848661713885285,
+            "y": 5.466955158443148,
+            "heading": 3.14,
+            "angularVelocity": 1.3635627120067326e-16,
+            "velocityX": -2.262300932356051,
+            "velocityY": 0.9487495569406552,
+            "timestamp": 2.573355441692067
+        },
+        {
+            "x": 4.336955391458343,
+            "y": 5.528985052396896,
+            "heading": 3.14,
+            "angularVelocity": 8.074174127249248e-18,
+            "velocityX": -1.5082006723016521,
+            "velocityY": 0.6324997259023314,
+            "timestamp": 2.6714264627747584
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 3.321022798774287e-17,
+            "velocityX": -0.7541003513770345,
+            "velocityY": 0.31624986933663735,
+            "timestamp": 2.7694974838574495
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -1.5033553036563547e-32,
+            "velocityY": 0,
+            "timestamp": 2.8675685049401407
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S2.traj
new file mode 100644
index 00000000..698e9db0
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S2.traj	
@@ -0,0 +1,283 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -2.7122542067664996e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 4.329648409276986,
+            "y": 5.532049408693613,
+            "heading": 3.14,
+            "angularVelocity": 3.6127527846474837e-17,
+            "velocityX": 0.715878164292546,
+            "velocityY": -0.3002204885663498,
+            "timestamp": 0.09310021257998485
+        },
+        {
+            "x": 4.462945224981569,
+            "y": 5.4761482272758,
+            "heading": 3.14,
+            "angularVelocity": 1.058102916611751e-16,
+            "velocityX": 1.4317562979794687,
+            "velocityY": -0.6004409642975056,
+            "timestamp": 0.1862004251599697
+        },
+        {
+            "x": 4.662890441407609,
+            "y": 5.392296458139565,
+            "heading": 3.14,
+            "angularVelocity": 1.669202321487025e-16,
+            "velocityX": 2.147634370376114,
+            "velocityY": -0.9006614143251297,
+            "timestamp": 0.27930063773995456
+        },
+        {
+            "x": 4.929484041376741,
+            "y": 5.280494108489063,
+            "heading": 3.14,
+            "angularVelocity": 1.646239395597301e-16,
+            "velocityX": 2.8635122582579458,
+            "velocityY": -1.2008817869720905,
+            "timestamp": 0.3724008503199394
+        },
+        {
+            "x": 5.2542718637391,
+            "y": 5.1442866313321245,
+            "heading": 3.14,
+            "angularVelocity": -2.6454281962617153e-17,
+            "velocityX": 3.4885830371581816,
+            "velocityY": -1.4630200445559467,
+            "timestamp": 0.46550106289992427
+        },
+        {
+            "x": 5.579059685941281,
+            "y": 5.008079154242361,
+            "heading": 3.14,
+            "angularVelocity": -9.771658175099026e-17,
+            "velocityX": 3.4885830354377245,
+            "velocityY": -1.463020043834432,
+            "timestamp": 0.5586012754799091
+        },
+        {
+            "x": 5.903847508143462,
+            "y": 4.871871677152596,
+            "heading": 3.14,
+            "angularVelocity": -3.1326214825920436e-16,
+            "velocityX": 3.488583035437725,
+            "velocityY": -1.4630200438344323,
+            "timestamp": 0.6517014880598939
+        },
+        {
+            "x": 6.228635330345643,
+            "y": 4.735664200062831,
+            "heading": 3.14,
+            "angularVelocity": -1.2137930063882497e-16,
+            "velocityX": 3.488583035437725,
+            "velocityY": -1.4630200438344325,
+            "timestamp": 0.7448017006398788
+        },
+        {
+            "x": 6.553423152547824,
+            "y": 4.599456722973066,
+            "heading": 3.14,
+            "angularVelocity": 1.3394779600477518e-16,
+            "velocityX": 3.488583035437725,
+            "velocityY": -1.4630200438344325,
+            "timestamp": 0.8379019132198637
+        },
+        {
+            "x": 6.878210974643575,
+            "y": 4.463249245927935,
+            "heading": 3.14,
+            "angularVelocity": 2.91910120342859e-17,
+            "velocityX": 3.488583034294527,
+            "velocityY": -1.4630200433550054,
+            "timestamp": 0.9310021257998486
+        },
+        {
+            "x": 7.17186557168986,
+            "y": 4.3400982252760985,
+            "heading": 3.14,
+            "angularVelocity": -6.102921228742647e-16,
+            "velocityX": 3.1541775137602737,
+            "velocityY": -1.3227791563413718,
+            "timestamp": 1.0241023383798336
+        },
+        {
+            "x": 7.3988717849138865,
+            "y": 4.2448977852556045,
+            "heading": 3.14,
+            "angularVelocity": -6.383129451815479e-16,
+            "velocityX": 2.4382996228798555,
+            "velocityY": -1.0225587824368827,
+            "timestamp": 1.1172025509598185
+        },
+        {
+            "x": 7.559229597346586,
+            "y": 4.1776479329828335,
+            "heading": 3.14,
+            "angularVelocity": -7.263779621185202e-16,
+            "velocityX": 1.7224215497327395,
+            "velocityY": -0.7223383320945309,
+            "timestamp": 1.2103027635398034
+        },
+        {
+            "x": 7.65293900332055,
+            "y": 4.138348670834548,
+            "heading": 3.14,
+            "angularVelocity": -7.150088563233795e-16,
+            "velocityX": 1.0065434157113666,
+            "velocityY": -0.42211785622311543,
+            "timestamp": 1.3034029761197883
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": -7.092042046536786e-16,
+            "velocityX": 0.2906652512305344,
+            "velocityY": -0.12189736757780324,
+            "timestamp": 1.3965031886997732
+        },
+        {
+            "x": 7.634550444733744,
+            "y": 4.146060349047862,
+            "heading": 3.14,
+            "angularVelocity": -8.252913531228688e-16,
+            "velocityX": -0.4634351183891671,
+            "velocityY": 0.19435250940932913,
+            "timestamp": 1.4945742097824644
+        },
+        {
+            "x": 7.515145498014414,
+            "y": 4.196135645696618,
+            "heading": 3.14,
+            "angularVelocity": -8.640334878143468e-16,
+            "velocityX": -1.217535469712484,
+            "velocityY": 0.5106023787234387,
+            "timestamp": 1.5926452308651555
+        },
+        {
+            "x": 7.321785162832602,
+            "y": 4.277225888692093,
+            "heading": 3.14,
+            "angularVelocity": -7.792870163094326e-16,
+            "velocityX": -1.971635790541653,
+            "velocityY": 0.826852235249104,
+            "timestamp": 1.6907162519478467
+        },
+        {
+            "x": 7.054469445169555,
+            "y": 4.389331075525908,
+            "heading": 3.14,
+            "angularVelocity": -9.085136126677818e-16,
+            "velocityX": -2.725736050381882,
+            "velocityY": 1.1431020661976106,
+            "timestamp": 1.7887872730305379
+        },
+        {
+            "x": 6.713198362969528,
+            "y": 4.532451198672714,
+            "heading": 3.14,
+            "angularVelocity": -7.684534164287992e-16,
+            "velocityX": -3.4798361272500533,
+            "velocityY": 1.4593518204124447,
+            "timestamp": 1.886858294113229
+        },
+        {
+            "x": 6.371069462531013,
+            "y": 4.675931068244968,
+            "heading": 3.14,
+            "angularVelocity": -1.8367347430029744e-16,
+            "velocityX": -3.4885830356557705,
+            "velocityY": 1.4630200439258751,
+            "timestamp": 1.9849293151959202
+        },
+        {
+            "x": 6.028940562092558,
+            "y": 4.819410937817198,
+            "heading": 3.14,
+            "angularVelocity": 1.6315422621813397e-16,
+            "velocityX": -3.48858303565516,
+            "velocityY": 1.463020043925619,
+            "timestamp": 2.0830003362786114
+        },
+        {
+            "x": 5.686811661654102,
+            "y": 4.962890807389427,
+            "heading": 3.14,
+            "angularVelocity": -1.6415107007869627e-16,
+            "velocityX": -3.4885830356551595,
+            "velocityY": 1.4630200439256191,
+            "timestamp": 2.1810713573613025
+        },
+        {
+            "x": 5.344682761215647,
+            "y": 5.106370676961656,
+            "heading": 3.14,
+            "angularVelocity": -2.6663466601394013e-16,
+            "velocityX": -3.48858303565516,
+            "velocityY": 1.4630200439256194,
+            "timestamp": 2.2791423784439937
+        },
+        {
+            "x": 5.002553860860093,
+            "y": 5.249850546499118,
+            "heading": 3.14,
+            "angularVelocity": -2.7119156877069447e-16,
+            "velocityX": -3.4885830348098277,
+            "velocityY": 1.4630200435711092,
+            "timestamp": 2.377213399526685
+        },
+        {
+            "x": 4.706732333820954,
+            "y": 5.373910320642252,
+            "heading": 3.14,
+            "angularVelocity": 2.3228605057035087e-16,
+            "velocityX": -3.01640100993532,
+            "velocityY": 1.264999311453705,
+            "timestamp": 2.475284420609376
+        },
+        {
+            "x": 4.4848661713885285,
+            "y": 5.466955158443148,
+            "heading": 3.14,
+            "angularVelocity": 1.3635627120067326e-16,
+            "velocityX": -2.262300932356051,
+            "velocityY": 0.9487495569406552,
+            "timestamp": 2.573355441692067
+        },
+        {
+            "x": 4.336955391458343,
+            "y": 5.528985052396896,
+            "heading": 3.14,
+            "angularVelocity": 8.074174127249248e-18,
+            "velocityX": -1.5082006723016521,
+            "velocityY": 0.6324997259023314,
+            "timestamp": 2.6714264627747584
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 3.321022798774287e-17,
+            "velocityX": -0.7541003513770345,
+            "velocityY": 0.31624986933663735,
+            "timestamp": 2.7694974838574495
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -1.5033553036563547e-32,
+            "velocityY": 0,
+            "timestamp": 2.8675685049401407
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S3.1.traj
new file mode 100644
index 00000000..13a23ce8
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S3.1.traj	
@@ -0,0 +1,301 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -2.3834052223810005e-28,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.3290756213238994,
+            "y": 5.551919291811216,
+            "heading": 3.150896767326327,
+            "angularVelocity": 0.12189104355668522,
+            "velocityX": 0.7391207131092994,
+            "velocityY": -0.09039065663175272,
+            "timestamp": 0.08939760468350924
+        },
+        {
+            "x": 4.461244485010799,
+            "y": 5.535758895626619,
+            "heading": 3.171238121679265,
+            "angularVelocity": 0.22753802436807322,
+            "velocityX": 1.478438534845478,
+            "velocityY": -0.18076990140321392,
+            "timestamp": 0.17879520936701848
+        },
+        {
+            "x": 4.65953073652644,
+            "y": 5.511521259157241,
+            "heading": 3.1985778553739457,
+            "angularVelocity": 0.30582176996086496,
+            "velocityX": 2.2180264473277496,
+            "velocityY": -0.2711217661229951,
+            "timestamp": 0.26819281405052775
+        },
+        {
+            "x": 4.923962030908283,
+            "y": 5.4792142049190105,
+            "heading": 3.2279129750090267,
+            "angularVelocity": 0.328142121241728,
+            "velocityX": 2.9579237085624133,
+            "velocityY": -0.3613861283199711,
+            "timestamp": 0.35759041873403696
+        },
+        {
+            "x": 5.25442830298904,
+            "y": 5.438888092999425,
+            "heading": 3.243107062504638,
+            "angularVelocity": 0.1699607897635962,
+            "velocityX": 3.6965897828133016,
+            "velocityY": -0.4510871634824905,
+            "timestamp": 0.4469880234175462
+        },
+        {
+            "x": 5.590105329368368,
+            "y": 5.397771389697725,
+            "heading": 3.243107071754486,
+            "angularVelocity": 1.0346835398617793e-7,
+            "velocityX": 3.7548771867867403,
+            "velocityY": -0.45993070448202833,
+            "timestamp": 0.5363856281010554
+        },
+        {
+            "x": 5.925782355662538,
+            "y": 5.356654685708362,
+            "heading": 3.2431070810044496,
+            "angularVelocity": 1.0346963637170158e-7,
+            "velocityX": 3.7548771858341166,
+            "velocityY": -0.4599307121748044,
+            "timestamp": 0.6257832327845646
+        },
+        {
+            "x": 6.26145937114871,
+            "y": 5.315537893473946,
+            "heading": 3.243107090254437,
+            "angularVelocity": 1.0346990400921205e-7,
+            "velocityX": 3.754877064936017,
+            "velocityY": -0.45993169928168404,
+            "timestamp": 0.7151808374680738
+        },
+        {
+            "x": 6.594482534239531,
+            "y": 5.256671755131022,
+            "heading": 3.24310709962475,
+            "angularVelocity": 1.0481586200532189e-7,
+            "velocityX": 3.725191119725111,
+            "velocityY": -0.658475565944433,
+            "timestamp": 0.804578442151583
+        },
+        {
+            "x": 6.90949802384657,
+            "y": 5.13364781945348,
+            "heading": 3.243107110210723,
+            "angularVelocity": 1.1841422044120688e-7,
+            "velocityX": 3.523757607622681,
+            "velocityY": -1.3761435344273285,
+            "timestamp": 0.8939760468350922
+        },
+        {
+            "x": 7.194274658905514,
+            "y": 4.9512421336811645,
+            "heading": 3.243107206435074,
+            "angularVelocity": 0.0000010763582143626502,
+            "velocityX": 3.185506323875142,
+            "velocityY": -2.0403867242241884,
+            "timestamp": 0.9833736515186015
+        },
+        {
+            "x": 7.415334413339818,
+            "y": 4.749404498288232,
+            "heading": 3.236772739824744,
+            "angularVelocity": -0.07085722970314587,
+            "velocityX": 2.472770441853857,
+            "velocityY": -2.2577521635961224,
+            "timestamp": 1.0727712562021108
+        },
+        {
+            "x": 7.5700702382409935,
+            "y": 4.542039922027483,
+            "heading": 3.21849203872718,
+            "angularVelocity": -0.20448759406641112,
+            "velocityX": 1.7308721575766997,
+            "velocityY": -2.3195764248545423,
+            "timestamp": 1.16216886088562
+        },
+        {
+            "x": 7.658284258545904,
+            "y": 4.333806888908036,
+            "heading": 3.1865144497864955,
+            "angularVelocity": -0.3577007354214622,
+            "velocityX": 0.9867604463982368,
+            "velocityY": -2.3292909676746256,
+            "timestamp": 1.2515664655691292
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": -0.5203097996961199,
+            "velocityX": 0.24291189378586123,
+            "velocityY": -2.3133381441461327,
+            "timestamp": 1.3409640702526384
+        },
+        {
+            "x": 7.643813431477439,
+            "y": 3.93924818499591,
+            "heading": 3.084542602133154,
+            "angularVelocity": -0.6748386526461468,
+            "velocityX": -0.4403397217071775,
+            "velocityY": -2.284675926642036,
+            "timestamp": 1.4231428216475348
+        },
+        {
+            "x": 7.551490590699835,
+            "y": 3.7550289645599473,
+            "heading": 3.0195743357345632,
+            "angularVelocity": -0.7905725664570535,
+            "velocityX": -1.1234393223010666,
+            "velocityY": -2.2416892116036538,
+            "timestamp": 1.5053215730424312
+        },
+        {
+            "x": 7.403134940422789,
+            "y": 3.5762585586759936,
+            "heading": 2.9502561501390385,
+            "angularVelocity": -0.8435049744515962,
+            "velocityX": -1.8052799264521386,
+            "velocityY": -2.175384790327918,
+            "timestamp": 1.5875003244373276
+        },
+        {
+            "x": 7.199227882598224,
+            "y": 3.4065730339632285,
+            "heading": 2.886416818113751,
+            "angularVelocity": -0.7768350205485519,
+            "velocityX": -2.4812625448848915,
+            "velocityY": -2.064834544462633,
+            "timestamp": 1.669679075832224
+        },
+        {
+            "x": 6.942760679940155,
+            "y": 3.255041432853574,
+            "heading": 2.8546081791353424,
+            "angularVelocity": -0.3870664673889389,
+            "velocityX": -3.1208456967576708,
+            "velocityY": -1.843926787962309,
+            "timestamp": 1.7518578272271204
+        },
+        {
+            "x": 6.651351957860763,
+            "y": 3.1467563415044117,
+            "heading": 2.854608138694005,
+            "angularVelocity": -4.921174528263849e-7,
+            "velocityX": -3.5460349194222314,
+            "velocityY": -1.3176774957700823,
+            "timestamp": 1.8340365786220167
+        },
+        {
+            "x": 6.345190283970336,
+            "y": 3.092814329077674,
+            "heading": 2.85460812188226,
+            "angularVelocity": -2.045787704737326e-7,
+            "velocityX": -3.7255576252046376,
+            "velocityY": -0.6563985399585123,
+            "timestamp": 1.9162153300169131
+        },
+        {
+            "x": 6.034627693227853,
+            "y": 3.0788290052051352,
+            "heading": 2.8546081056797767,
+            "angularVelocity": -1.9716489468637527e-7,
+            "velocityX": -3.7791106030377564,
+            "velocityY": -0.17018175224969478,
+            "timestamp": 1.9983940814118095
+        },
+        {
+            "x": 5.724065078469444,
+            "y": 3.064844225150663,
+            "heading": 2.8546080894768826,
+            "angularVelocity": -1.9716993391114644e-7,
+            "velocityX": -3.7791108952905743,
+            "velocityY": -0.17017513428705025,
+            "timestamp": 2.080572832806706
+        },
+        {
+            "x": 5.413502463745052,
+            "y": 3.050859444277176,
+            "heading": 2.8546080732739703,
+            "angularVelocity": -1.9717015240502906e-7,
+            "velocityX": -3.7791108948748344,
+            "velocityY": -0.170175144292348,
+            "timestamp": 2.1627515842016023
+        },
+        {
+            "x": 5.102939848618815,
+            "y": 3.0368746723382456,
+            "heading": 2.854608057073056,
+            "angularVelocity": -1.9714443134713148e-7,
+            "velocityX": -3.779110899765686,
+            "velocityY": -0.17017503556443397,
+            "timestamp": 2.2449303355964987
+        },
+        {
+            "x": 4.82269393470373,
+            "y": 3.0246815634320945,
+            "heading": 2.775318198682859,
+            "angularVelocity": -0.9648462289850903,
+            "velocityX": -3.4101992200919007,
+            "velocityY": -0.1483730125178458,
+            "timestamp": 2.327109086991395
+        },
+        {
+            "x": 4.59870268568745,
+            "y": 3.0148363143730834,
+            "heading": 2.699114427610376,
+            "angularVelocity": -0.927292880108085,
+            "velocityX": -2.725658947290856,
+            "velocityY": -0.11980285529234247,
+            "timestamp": 2.4092878383862915
+        },
+        {
+            "x": 4.43080987518015,
+            "y": 3.007426917155393,
+            "heading": 2.637997904363483,
+            "angularVelocity": -0.7437022612594317,
+            "velocityX": -2.043019730258355,
+            "velocityY": -0.09016195906753964,
+            "timestamp": 2.491466589781188
+        },
+        {
+            "x": 4.318925850575796,
+            "y": 3.002477699207252,
+            "heading": 2.595691429442944,
+            "angularVelocity": -0.5148103882976692,
+            "velocityX": -1.361471459539352,
+            "velocityY": -0.06022503220498214,
+            "timestamp": 2.5736453411760842
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -0.26395423478763,
+            "velocityX": -0.6805390642559688,
+            "velocityY": -0.03015011990058865,
+            "timestamp": 2.6558240925709806
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 2.4341219671984428e-29,
+            "velocityY": 0,
+            "timestamp": 2.738002843965877
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S3.traj
new file mode 100644
index 00000000..13a23ce8
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S3.traj	
@@ -0,0 +1,301 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -2.3834052223810005e-28,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.3290756213238994,
+            "y": 5.551919291811216,
+            "heading": 3.150896767326327,
+            "angularVelocity": 0.12189104355668522,
+            "velocityX": 0.7391207131092994,
+            "velocityY": -0.09039065663175272,
+            "timestamp": 0.08939760468350924
+        },
+        {
+            "x": 4.461244485010799,
+            "y": 5.535758895626619,
+            "heading": 3.171238121679265,
+            "angularVelocity": 0.22753802436807322,
+            "velocityX": 1.478438534845478,
+            "velocityY": -0.18076990140321392,
+            "timestamp": 0.17879520936701848
+        },
+        {
+            "x": 4.65953073652644,
+            "y": 5.511521259157241,
+            "heading": 3.1985778553739457,
+            "angularVelocity": 0.30582176996086496,
+            "velocityX": 2.2180264473277496,
+            "velocityY": -0.2711217661229951,
+            "timestamp": 0.26819281405052775
+        },
+        {
+            "x": 4.923962030908283,
+            "y": 5.4792142049190105,
+            "heading": 3.2279129750090267,
+            "angularVelocity": 0.328142121241728,
+            "velocityX": 2.9579237085624133,
+            "velocityY": -0.3613861283199711,
+            "timestamp": 0.35759041873403696
+        },
+        {
+            "x": 5.25442830298904,
+            "y": 5.438888092999425,
+            "heading": 3.243107062504638,
+            "angularVelocity": 0.1699607897635962,
+            "velocityX": 3.6965897828133016,
+            "velocityY": -0.4510871634824905,
+            "timestamp": 0.4469880234175462
+        },
+        {
+            "x": 5.590105329368368,
+            "y": 5.397771389697725,
+            "heading": 3.243107071754486,
+            "angularVelocity": 1.0346835398617793e-7,
+            "velocityX": 3.7548771867867403,
+            "velocityY": -0.45993070448202833,
+            "timestamp": 0.5363856281010554
+        },
+        {
+            "x": 5.925782355662538,
+            "y": 5.356654685708362,
+            "heading": 3.2431070810044496,
+            "angularVelocity": 1.0346963637170158e-7,
+            "velocityX": 3.7548771858341166,
+            "velocityY": -0.4599307121748044,
+            "timestamp": 0.6257832327845646
+        },
+        {
+            "x": 6.26145937114871,
+            "y": 5.315537893473946,
+            "heading": 3.243107090254437,
+            "angularVelocity": 1.0346990400921205e-7,
+            "velocityX": 3.754877064936017,
+            "velocityY": -0.45993169928168404,
+            "timestamp": 0.7151808374680738
+        },
+        {
+            "x": 6.594482534239531,
+            "y": 5.256671755131022,
+            "heading": 3.24310709962475,
+            "angularVelocity": 1.0481586200532189e-7,
+            "velocityX": 3.725191119725111,
+            "velocityY": -0.658475565944433,
+            "timestamp": 0.804578442151583
+        },
+        {
+            "x": 6.90949802384657,
+            "y": 5.13364781945348,
+            "heading": 3.243107110210723,
+            "angularVelocity": 1.1841422044120688e-7,
+            "velocityX": 3.523757607622681,
+            "velocityY": -1.3761435344273285,
+            "timestamp": 0.8939760468350922
+        },
+        {
+            "x": 7.194274658905514,
+            "y": 4.9512421336811645,
+            "heading": 3.243107206435074,
+            "angularVelocity": 0.0000010763582143626502,
+            "velocityX": 3.185506323875142,
+            "velocityY": -2.0403867242241884,
+            "timestamp": 0.9833736515186015
+        },
+        {
+            "x": 7.415334413339818,
+            "y": 4.749404498288232,
+            "heading": 3.236772739824744,
+            "angularVelocity": -0.07085722970314587,
+            "velocityX": 2.472770441853857,
+            "velocityY": -2.2577521635961224,
+            "timestamp": 1.0727712562021108
+        },
+        {
+            "x": 7.5700702382409935,
+            "y": 4.542039922027483,
+            "heading": 3.21849203872718,
+            "angularVelocity": -0.20448759406641112,
+            "velocityX": 1.7308721575766997,
+            "velocityY": -2.3195764248545423,
+            "timestamp": 1.16216886088562
+        },
+        {
+            "x": 7.658284258545904,
+            "y": 4.333806888908036,
+            "heading": 3.1865144497864955,
+            "angularVelocity": -0.3577007354214622,
+            "velocityX": 0.9867604463982368,
+            "velocityY": -2.3292909676746256,
+            "timestamp": 1.2515664655691292
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": -0.5203097996961199,
+            "velocityX": 0.24291189378586123,
+            "velocityY": -2.3133381441461327,
+            "timestamp": 1.3409640702526384
+        },
+        {
+            "x": 7.643813431477439,
+            "y": 3.93924818499591,
+            "heading": 3.084542602133154,
+            "angularVelocity": -0.6748386526461468,
+            "velocityX": -0.4403397217071775,
+            "velocityY": -2.284675926642036,
+            "timestamp": 1.4231428216475348
+        },
+        {
+            "x": 7.551490590699835,
+            "y": 3.7550289645599473,
+            "heading": 3.0195743357345632,
+            "angularVelocity": -0.7905725664570535,
+            "velocityX": -1.1234393223010666,
+            "velocityY": -2.2416892116036538,
+            "timestamp": 1.5053215730424312
+        },
+        {
+            "x": 7.403134940422789,
+            "y": 3.5762585586759936,
+            "heading": 2.9502561501390385,
+            "angularVelocity": -0.8435049744515962,
+            "velocityX": -1.8052799264521386,
+            "velocityY": -2.175384790327918,
+            "timestamp": 1.5875003244373276
+        },
+        {
+            "x": 7.199227882598224,
+            "y": 3.4065730339632285,
+            "heading": 2.886416818113751,
+            "angularVelocity": -0.7768350205485519,
+            "velocityX": -2.4812625448848915,
+            "velocityY": -2.064834544462633,
+            "timestamp": 1.669679075832224
+        },
+        {
+            "x": 6.942760679940155,
+            "y": 3.255041432853574,
+            "heading": 2.8546081791353424,
+            "angularVelocity": -0.3870664673889389,
+            "velocityX": -3.1208456967576708,
+            "velocityY": -1.843926787962309,
+            "timestamp": 1.7518578272271204
+        },
+        {
+            "x": 6.651351957860763,
+            "y": 3.1467563415044117,
+            "heading": 2.854608138694005,
+            "angularVelocity": -4.921174528263849e-7,
+            "velocityX": -3.5460349194222314,
+            "velocityY": -1.3176774957700823,
+            "timestamp": 1.8340365786220167
+        },
+        {
+            "x": 6.345190283970336,
+            "y": 3.092814329077674,
+            "heading": 2.85460812188226,
+            "angularVelocity": -2.045787704737326e-7,
+            "velocityX": -3.7255576252046376,
+            "velocityY": -0.6563985399585123,
+            "timestamp": 1.9162153300169131
+        },
+        {
+            "x": 6.034627693227853,
+            "y": 3.0788290052051352,
+            "heading": 2.8546081056797767,
+            "angularVelocity": -1.9716489468637527e-7,
+            "velocityX": -3.7791106030377564,
+            "velocityY": -0.17018175224969478,
+            "timestamp": 1.9983940814118095
+        },
+        {
+            "x": 5.724065078469444,
+            "y": 3.064844225150663,
+            "heading": 2.8546080894768826,
+            "angularVelocity": -1.9716993391114644e-7,
+            "velocityX": -3.7791108952905743,
+            "velocityY": -0.17017513428705025,
+            "timestamp": 2.080572832806706
+        },
+        {
+            "x": 5.413502463745052,
+            "y": 3.050859444277176,
+            "heading": 2.8546080732739703,
+            "angularVelocity": -1.9717015240502906e-7,
+            "velocityX": -3.7791108948748344,
+            "velocityY": -0.170175144292348,
+            "timestamp": 2.1627515842016023
+        },
+        {
+            "x": 5.102939848618815,
+            "y": 3.0368746723382456,
+            "heading": 2.854608057073056,
+            "angularVelocity": -1.9714443134713148e-7,
+            "velocityX": -3.779110899765686,
+            "velocityY": -0.17017503556443397,
+            "timestamp": 2.2449303355964987
+        },
+        {
+            "x": 4.82269393470373,
+            "y": 3.0246815634320945,
+            "heading": 2.775318198682859,
+            "angularVelocity": -0.9648462289850903,
+            "velocityX": -3.4101992200919007,
+            "velocityY": -0.1483730125178458,
+            "timestamp": 2.327109086991395
+        },
+        {
+            "x": 4.59870268568745,
+            "y": 3.0148363143730834,
+            "heading": 2.699114427610376,
+            "angularVelocity": -0.927292880108085,
+            "velocityX": -2.725658947290856,
+            "velocityY": -0.11980285529234247,
+            "timestamp": 2.4092878383862915
+        },
+        {
+            "x": 4.43080987518015,
+            "y": 3.007426917155393,
+            "heading": 2.637997904363483,
+            "angularVelocity": -0.7437022612594317,
+            "velocityX": -2.043019730258355,
+            "velocityY": -0.09016195906753964,
+            "timestamp": 2.491466589781188
+        },
+        {
+            "x": 4.318925850575796,
+            "y": 3.002477699207252,
+            "heading": 2.595691429442944,
+            "angularVelocity": -0.5148103882976692,
+            "velocityX": -1.361471459539352,
+            "velocityY": -0.06022503220498214,
+            "timestamp": 2.5736453411760842
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -0.26395423478763,
+            "velocityX": -0.6805390642559688,
+            "velocityY": -0.03015011990058865,
+            "timestamp": 2.6558240925709806
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 2.4341219671984428e-29,
+            "velocityY": 0,
+            "timestamp": 2.738002843965877
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S4.1.traj
new file mode 100644
index 00000000..20dd60b7
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S4.1.traj	
@@ -0,0 +1,310 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -2.6674512845646027e-30,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.322765898095933,
+            "y": 5.556151172121073,
+            "heading": 3.1479530411377072,
+            "angularVelocity": 0.09380834863319021,
+            "velocityX": 0.7049555142252463,
+            "velocityY": -0.04539800325896039,
+            "timestamp": 0.08477967317075337
+        },
+        {
+            "x": 4.442297694457063,
+            "y": 5.548453524989103,
+            "heading": 3.163858908065189,
+            "angularVelocity": 0.18761415717719215,
+            "velocityX": 1.4099110304470142,
+            "velocityY": -0.09079590477320977,
+            "timestamp": 0.16955934634150674
+        },
+        {
+            "x": 4.621595390179887,
+            "y": 5.536907071113832,
+            "heading": 3.1877171391651413,
+            "angularVelocity": 0.28141452081573415,
+            "velocityX": 2.1148665596022633,
+            "velocityY": -0.13619365873154282,
+            "timestamp": 0.2543390195122601
+        },
+        {
+            "x": 4.860658986379666,
+            "y": 5.521511826336959,
+            "heading": 3.2195268977958102,
+            "angularVelocity": 0.3752050160303855,
+            "velocityX": 2.819822101912331,
+            "velocityY": -0.1815912258326068,
+            "timestamp": 0.3391186926830135
+        },
+        {
+            "x": 5.159488476433141,
+            "y": 5.5022678093145005,
+            "heading": 3.2592867547687217,
+            "angularVelocity": 0.46897865356654944,
+            "velocityX": 3.524777566099216,
+            "velocityY": -0.22698857288071755,
+            "timestamp": 0.42389836585376683
+        },
+        {
+            "x": 5.47954195294372,
+            "y": 5.481656666167451,
+            "heading": 3.2592867595783463,
+            "angularVelocity": 5.673087575653179e-8,
+            "velocityX": 3.7751204332431567,
+            "velocityY": -0.24311420858382377,
+            "timestamp": 0.5086780390245202
+        },
+        {
+            "x": 5.799595429451853,
+            "y": 5.461045522982243,
+            "heading": 3.2592867643879506,
+            "angularVelocity": 5.6730628986964756e-8,
+            "velocityX": 3.7751204332143167,
+            "velocityY": -0.2431142090339428,
+            "timestamp": 0.5934577121952737
+        },
+        {
+            "x": 6.1196488941034595,
+            "y": 5.440434195687293,
+            "heading": 3.259286769197555,
+            "angularVelocity": 5.673063176342314e-8,
+            "velocityX": 3.7751202933632646,
+            "velocityY": -0.24311638065988433,
+            "timestamp": 0.6782373853660271
+        },
+        {
+            "x": 6.4368892365574455,
+            "y": 5.393342668998585,
+            "heading": 3.2592867741163385,
+            "angularVelocity": 5.801842614465255e-8,
+            "velocityX": 3.741938728815606,
+            "velocityY": -0.5554577521660955,
+            "timestamp": 0.7630170585367805
+        },
+        {
+            "x": 6.7398295350074555,
+            "y": 5.288051546129373,
+            "heading": 3.2592867797575464,
+            "angularVelocity": 6.653963355876342e-8,
+            "velocityX": 3.5732657029690613,
+            "velocityY": -1.2419382964267203,
+            "timestamp": 0.8477967317075339
+        },
+        {
+            "x": 7.017891386263095,
+            "y": 5.128237130868345,
+            "heading": 3.259286787454074,
+            "angularVelocity": 9.07826943070083e-8,
+            "velocityX": 3.2798174474623285,
+            "velocityY": -1.8850558074130808,
+            "timestamp": 0.9325764048782873
+        },
+        {
+            "x": 7.261365099007722,
+            "y": 4.919479947434011,
+            "heading": 3.259286723898943,
+            "angularVelocity": -7.496505777325929e-7,
+            "velocityX": 2.8718406622562314,
+            "velocityY": -2.462349471599812,
+            "timestamp": 1.0173560780490407
+        },
+        {
+            "x": 7.457139881435657,
+            "y": 4.674807078885688,
+            "heading": 3.2408811636287,
+            "angularVelocity": -0.21709874055771505,
+            "velocityX": 2.309218414106716,
+            "velocityY": -2.885985040961276,
+            "timestamp": 1.102135751219794
+        },
+        {
+            "x": 7.597538484149158,
+            "y": 4.408115178432638,
+            "heading": 3.1997348588691503,
+            "angularVelocity": -0.48533219368821684,
+            "velocityX": 1.6560408581747834,
+            "velocityY": -3.1457056919299244,
+            "timestamp": 1.1869154243905473
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": -0.7045893978594314,
+            "velocityX": 0.9726566848724733,
+            "velocityY": -3.315832297049189,
+            "timestamp": 1.2716950975613006
+        },
+        {
+            "x": 7.707365632796218,
+            "y": 3.8777713806946585,
+            "heading": 3.0766920768105654,
+            "angularVelocity": -0.8684660682637303,
+            "velocityX": 0.37540519927184846,
+            "velocityY": -3.418949607021354,
+            "timestamp": 1.344591351660309
+        },
+        {
+            "x": 7.690646428306815,
+            "y": 3.624138174667908,
+            "heading": 3.0114310872436763,
+            "angularVelocity": -0.8952584789497043,
+            "velocityX": -0.229356154113369,
+            "velocityY": -3.4793722827239795,
+            "timestamp": 1.4174876057593173
+        },
+        {
+            "x": 7.629817161501398,
+            "y": 3.371341042658395,
+            "heading": 2.9608367604127053,
+            "angularVelocity": -0.6940593512835794,
+            "velocityX": -0.8344635476447348,
+            "velocityY": -3.4679029139977815,
+            "timestamp": 1.4903838598583257
+        },
+        {
+            "x": 7.52574877175356,
+            "y": 3.1246562301261127,
+            "heading": 2.9373487413645845,
+            "angularVelocity": -0.32221160522605474,
+            "velocityX": -1.4276232850943003,
+            "velocityY": -3.3840533451439727,
+            "timestamp": 1.563280113957334
+        },
+        {
+            "x": 7.379748340613123,
+            "y": 2.890715038062695,
+            "heading": 2.9373478462882416,
+            "angularVelocity": -0.00001227877368205191,
+            "velocityX": -2.002852313126858,
+            "velocityY": -3.2092347536203247,
+            "timestamp": 1.6361763680563424
+        },
+        {
+            "x": 7.198165609339985,
+            "y": 2.6831759130730863,
+            "heading": 2.937347790914402,
+            "angularVelocity": -7.596252972489614e-7,
+            "velocityX": -2.4909747903727215,
+            "velocityY": -2.847047870356197,
+            "timestamp": 1.7090726221553507
+        },
+        {
+            "x": 6.985688529633763,
+            "y": 2.5073968727951343,
+            "heading": 2.9373477481889543,
+            "angularVelocity": -5.861130796388339e-7,
+            "velocityX": -2.91478735543415,
+            "velocityY": -2.4113590259261937,
+            "timestamp": 1.781968876254359
+        },
+        {
+            "x": 6.7478025524071645,
+            "y": 2.367915534392956,
+            "heading": 2.937347710107927,
+            "angularVelocity": -5.224003305629458e-7,
+            "velocityX": -3.263349813607291,
+            "velocityY": -1.9134225774154996,
+            "timestamp": 1.8548651303533674
+        },
+        {
+            "x": 6.4987639591558635,
+            "y": 2.249490193910499,
+            "heading": 2.937347672689208,
+            "angularVelocity": -5.133147047667194e-7,
+            "velocityX": -3.4163428056670964,
+            "velocityY": -1.624573744513203,
+            "timestamp": 1.9277613844523758
+        },
+        {
+            "x": 6.249725239429151,
+            "y": 2.1310651193958834,
+            "heading": 2.937347635270487,
+            "angularVelocity": -5.133147287283584e-7,
+            "velocityX": -3.4163445406727813,
+            "velocityY": -1.6245700959327491,
+            "timestamp": 2.000657638551384
+        },
+        {
+            "x": 6.0006865195043355,
+            "y": 2.012640045297908,
+            "heading": 2.937347597851683,
+            "angularVelocity": -5.133158656564982e-7,
+            "velocityX": -3.4163445433903696,
+            "velocityY": -1.6245700902172364,
+            "timestamp": 2.0735538926503922
+        },
+        {
+            "x": 5.766671175118509,
+            "y": 1.901395758932124,
+            "heading": 2.8964169231036223,
+            "angularVelocity": -0.5614921542139502,
+            "velocityX": -3.2102519845241893,
+            "velocityY": -1.5260631391951232,
+            "timestamp": 2.1464501467494004
+        },
+        {
+            "x": 5.568987668741884,
+            "y": 1.8073555304166649,
+            "heading": 2.790644028809865,
+            "angularVelocity": -1.4510058932587933,
+            "velocityX": -2.7118472522341635,
+            "velocityY": -1.2900557055764181,
+            "timestamp": 2.2193464008484085
+        },
+        {
+            "x": 5.41134573614175,
+            "y": 1.7322845721056195,
+            "heading": 2.684499308002556,
+            "angularVelocity": -1.456106656228851,
+            "velocityX": -2.162551897193064,
+            "velocityY": -1.029832866434731,
+            "timestamp": 2.2922426549474166
+        },
+        {
+            "x": 5.293483125084031,
+            "y": 1.6760930732996284,
+            "heading": 2.5959515176489267,
+            "angularVelocity": -1.2147097467272172,
+            "velocityX": -1.6168541513476236,
+            "velocityY": -0.7708420617846653,
+            "timestamp": 2.3651389090464248
+        },
+        {
+            "x": 5.2151080549458655,
+            "y": 1.6386848703250345,
+            "heading": 2.5329322459715486,
+            "angularVelocity": -0.8645063104618199,
+            "velocityX": -1.075159088856404,
+            "velocityY": -0.5131704425282329,
+            "timestamp": 2.438035163145433
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.45176870030861394,
+            "velocityX": -0.536489226081922,
+            "velocityY": -0.25632140575671664,
+            "timestamp": 2.510931417244441
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": 2.4074157612584747e-31,
+            "velocityY": 0,
+            "timestamp": 2.583827671343449
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S4.traj
new file mode 100644
index 00000000..20dd60b7
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S4.traj	
@@ -0,0 +1,310 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -2.6674512845646027e-30,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.322765898095933,
+            "y": 5.556151172121073,
+            "heading": 3.1479530411377072,
+            "angularVelocity": 0.09380834863319021,
+            "velocityX": 0.7049555142252463,
+            "velocityY": -0.04539800325896039,
+            "timestamp": 0.08477967317075337
+        },
+        {
+            "x": 4.442297694457063,
+            "y": 5.548453524989103,
+            "heading": 3.163858908065189,
+            "angularVelocity": 0.18761415717719215,
+            "velocityX": 1.4099110304470142,
+            "velocityY": -0.09079590477320977,
+            "timestamp": 0.16955934634150674
+        },
+        {
+            "x": 4.621595390179887,
+            "y": 5.536907071113832,
+            "heading": 3.1877171391651413,
+            "angularVelocity": 0.28141452081573415,
+            "velocityX": 2.1148665596022633,
+            "velocityY": -0.13619365873154282,
+            "timestamp": 0.2543390195122601
+        },
+        {
+            "x": 4.860658986379666,
+            "y": 5.521511826336959,
+            "heading": 3.2195268977958102,
+            "angularVelocity": 0.3752050160303855,
+            "velocityX": 2.819822101912331,
+            "velocityY": -0.1815912258326068,
+            "timestamp": 0.3391186926830135
+        },
+        {
+            "x": 5.159488476433141,
+            "y": 5.5022678093145005,
+            "heading": 3.2592867547687217,
+            "angularVelocity": 0.46897865356654944,
+            "velocityX": 3.524777566099216,
+            "velocityY": -0.22698857288071755,
+            "timestamp": 0.42389836585376683
+        },
+        {
+            "x": 5.47954195294372,
+            "y": 5.481656666167451,
+            "heading": 3.2592867595783463,
+            "angularVelocity": 5.673087575653179e-8,
+            "velocityX": 3.7751204332431567,
+            "velocityY": -0.24311420858382377,
+            "timestamp": 0.5086780390245202
+        },
+        {
+            "x": 5.799595429451853,
+            "y": 5.461045522982243,
+            "heading": 3.2592867643879506,
+            "angularVelocity": 5.6730628986964756e-8,
+            "velocityX": 3.7751204332143167,
+            "velocityY": -0.2431142090339428,
+            "timestamp": 0.5934577121952737
+        },
+        {
+            "x": 6.1196488941034595,
+            "y": 5.440434195687293,
+            "heading": 3.259286769197555,
+            "angularVelocity": 5.673063176342314e-8,
+            "velocityX": 3.7751202933632646,
+            "velocityY": -0.24311638065988433,
+            "timestamp": 0.6782373853660271
+        },
+        {
+            "x": 6.4368892365574455,
+            "y": 5.393342668998585,
+            "heading": 3.2592867741163385,
+            "angularVelocity": 5.801842614465255e-8,
+            "velocityX": 3.741938728815606,
+            "velocityY": -0.5554577521660955,
+            "timestamp": 0.7630170585367805
+        },
+        {
+            "x": 6.7398295350074555,
+            "y": 5.288051546129373,
+            "heading": 3.2592867797575464,
+            "angularVelocity": 6.653963355876342e-8,
+            "velocityX": 3.5732657029690613,
+            "velocityY": -1.2419382964267203,
+            "timestamp": 0.8477967317075339
+        },
+        {
+            "x": 7.017891386263095,
+            "y": 5.128237130868345,
+            "heading": 3.259286787454074,
+            "angularVelocity": 9.07826943070083e-8,
+            "velocityX": 3.2798174474623285,
+            "velocityY": -1.8850558074130808,
+            "timestamp": 0.9325764048782873
+        },
+        {
+            "x": 7.261365099007722,
+            "y": 4.919479947434011,
+            "heading": 3.259286723898943,
+            "angularVelocity": -7.496505777325929e-7,
+            "velocityX": 2.8718406622562314,
+            "velocityY": -2.462349471599812,
+            "timestamp": 1.0173560780490407
+        },
+        {
+            "x": 7.457139881435657,
+            "y": 4.674807078885688,
+            "heading": 3.2408811636287,
+            "angularVelocity": -0.21709874055771505,
+            "velocityX": 2.309218414106716,
+            "velocityY": -2.885985040961276,
+            "timestamp": 1.102135751219794
+        },
+        {
+            "x": 7.597538484149158,
+            "y": 4.408115178432638,
+            "heading": 3.1997348588691503,
+            "angularVelocity": -0.48533219368821684,
+            "velocityX": 1.6560408581747834,
+            "velocityY": -3.1457056919299244,
+            "timestamp": 1.1869154243905473
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": -0.7045893978594314,
+            "velocityX": 0.9726566848724733,
+            "velocityY": -3.315832297049189,
+            "timestamp": 1.2716950975613006
+        },
+        {
+            "x": 7.707365632796218,
+            "y": 3.8777713806946585,
+            "heading": 3.0766920768105654,
+            "angularVelocity": -0.8684660682637303,
+            "velocityX": 0.37540519927184846,
+            "velocityY": -3.418949607021354,
+            "timestamp": 1.344591351660309
+        },
+        {
+            "x": 7.690646428306815,
+            "y": 3.624138174667908,
+            "heading": 3.0114310872436763,
+            "angularVelocity": -0.8952584789497043,
+            "velocityX": -0.229356154113369,
+            "velocityY": -3.4793722827239795,
+            "timestamp": 1.4174876057593173
+        },
+        {
+            "x": 7.629817161501398,
+            "y": 3.371341042658395,
+            "heading": 2.9608367604127053,
+            "angularVelocity": -0.6940593512835794,
+            "velocityX": -0.8344635476447348,
+            "velocityY": -3.4679029139977815,
+            "timestamp": 1.4903838598583257
+        },
+        {
+            "x": 7.52574877175356,
+            "y": 3.1246562301261127,
+            "heading": 2.9373487413645845,
+            "angularVelocity": -0.32221160522605474,
+            "velocityX": -1.4276232850943003,
+            "velocityY": -3.3840533451439727,
+            "timestamp": 1.563280113957334
+        },
+        {
+            "x": 7.379748340613123,
+            "y": 2.890715038062695,
+            "heading": 2.9373478462882416,
+            "angularVelocity": -0.00001227877368205191,
+            "velocityX": -2.002852313126858,
+            "velocityY": -3.2092347536203247,
+            "timestamp": 1.6361763680563424
+        },
+        {
+            "x": 7.198165609339985,
+            "y": 2.6831759130730863,
+            "heading": 2.937347790914402,
+            "angularVelocity": -7.596252972489614e-7,
+            "velocityX": -2.4909747903727215,
+            "velocityY": -2.847047870356197,
+            "timestamp": 1.7090726221553507
+        },
+        {
+            "x": 6.985688529633763,
+            "y": 2.5073968727951343,
+            "heading": 2.9373477481889543,
+            "angularVelocity": -5.861130796388339e-7,
+            "velocityX": -2.91478735543415,
+            "velocityY": -2.4113590259261937,
+            "timestamp": 1.781968876254359
+        },
+        {
+            "x": 6.7478025524071645,
+            "y": 2.367915534392956,
+            "heading": 2.937347710107927,
+            "angularVelocity": -5.224003305629458e-7,
+            "velocityX": -3.263349813607291,
+            "velocityY": -1.9134225774154996,
+            "timestamp": 1.8548651303533674
+        },
+        {
+            "x": 6.4987639591558635,
+            "y": 2.249490193910499,
+            "heading": 2.937347672689208,
+            "angularVelocity": -5.133147047667194e-7,
+            "velocityX": -3.4163428056670964,
+            "velocityY": -1.624573744513203,
+            "timestamp": 1.9277613844523758
+        },
+        {
+            "x": 6.249725239429151,
+            "y": 2.1310651193958834,
+            "heading": 2.937347635270487,
+            "angularVelocity": -5.133147287283584e-7,
+            "velocityX": -3.4163445406727813,
+            "velocityY": -1.6245700959327491,
+            "timestamp": 2.000657638551384
+        },
+        {
+            "x": 6.0006865195043355,
+            "y": 2.012640045297908,
+            "heading": 2.937347597851683,
+            "angularVelocity": -5.133158656564982e-7,
+            "velocityX": -3.4163445433903696,
+            "velocityY": -1.6245700902172364,
+            "timestamp": 2.0735538926503922
+        },
+        {
+            "x": 5.766671175118509,
+            "y": 1.901395758932124,
+            "heading": 2.8964169231036223,
+            "angularVelocity": -0.5614921542139502,
+            "velocityX": -3.2102519845241893,
+            "velocityY": -1.5260631391951232,
+            "timestamp": 2.1464501467494004
+        },
+        {
+            "x": 5.568987668741884,
+            "y": 1.8073555304166649,
+            "heading": 2.790644028809865,
+            "angularVelocity": -1.4510058932587933,
+            "velocityX": -2.7118472522341635,
+            "velocityY": -1.2900557055764181,
+            "timestamp": 2.2193464008484085
+        },
+        {
+            "x": 5.41134573614175,
+            "y": 1.7322845721056195,
+            "heading": 2.684499308002556,
+            "angularVelocity": -1.456106656228851,
+            "velocityX": -2.162551897193064,
+            "velocityY": -1.029832866434731,
+            "timestamp": 2.2922426549474166
+        },
+        {
+            "x": 5.293483125084031,
+            "y": 1.6760930732996284,
+            "heading": 2.5959515176489267,
+            "angularVelocity": -1.2147097467272172,
+            "velocityX": -1.6168541513476236,
+            "velocityY": -0.7708420617846653,
+            "timestamp": 2.3651389090464248
+        },
+        {
+            "x": 5.2151080549458655,
+            "y": 1.6386848703250345,
+            "heading": 2.5329322459715486,
+            "angularVelocity": -0.8645063104618199,
+            "velocityX": -1.075159088856404,
+            "velocityY": -0.5131704425282329,
+            "timestamp": 2.438035163145433
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.45176870030861394,
+            "velocityX": -0.536489226081922,
+            "velocityY": -0.25632140575671664,
+            "timestamp": 2.510931417244441
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": 2.4074157612584747e-31,
+            "velocityY": 0,
+            "timestamp": 2.583827671343449
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj
new file mode 100644
index 00000000..f00ff892
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -6.652842580111428e-35,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.215224473336601,
+            "y": 5.589131466417677,
+            "heading": 3.1531161748208096,
+            "angularVelocity": 0.15993673271356004,
+            "velocityX": -0.5825678402891398,
+            "velocityY": 0.3552248747558748,
+            "timestamp": 0.08200852048353235
+        },
+        {
+            "x": 4.119673422721879,
+            "y": 5.647394502413024,
+            "heading": 3.179344235647322,
+            "angularVelocity": 0.3198211682379922,
+            "velocityX": -1.1651356475076162,
+            "velocityY": 0.7104510074297316,
+            "timestamp": 0.1640170409670647
+        },
+        {
+            "x": 3.9763468590229047,
+            "y": 5.73478928117174,
+            "heading": 3.2186777846940484,
+            "angularVelocity": 0.47962759009443284,
+            "velocityX": -1.7477033222146137,
+            "velocityY": 1.065679251904866,
+            "timestamp": 0.24602556145059706
+        },
+        {
+            "x": 3.785244828834372,
+            "y": 5.85131604858519,
+            "heading": 3.2711097941834586,
+            "angularVelocity": 0.6393483162513433,
+            "velocityX": -2.3302704287526703,
+            "velocityY": 1.420910494743635,
+            "timestamp": 0.3280340819341294
+        },
+        {
+            "x": 3.546367452965707,
+            "y": 5.996975111850807,
+            "heading": 3.3366347863626045,
+            "angularVelocity": 0.7990022474835781,
+            "velocityX": -2.9128360621581053,
+            "velocityY": 1.7761454835033377,
+            "timestamp": 0.4100426024176618
+        },
+        {
+            "x": 3.281494477712196,
+            "y": 6.158489941563481,
+            "heading": 3.336634794646938,
+            "angularVelocity": 1.0101796436143703e-7,
+            "velocityX": -3.2298226293047043,
+            "velocityY": 1.9694883990146768,
+            "timestamp": 0.4920511229011941
+        },
+        {
+            "x": 3.016621502775785,
+            "y": 6.320004771796177,
+            "heading": 3.336634802931272,
+            "angularVelocity": 1.0101796370003125e-7,
+            "velocityX": -3.2298226254380333,
+            "velocityY": 1.969488405355746,
+            "timestamp": 0.5740596433847265
+        },
+        {
+            "x": 2.7777474341519097,
+            "y": 6.465669636134063,
+            "heading": 3.402143395936758,
+            "angularVelocity": 0.7988022783393678,
+            "velocityX": -2.912795734094993,
+            "velocityY": 1.7762162209368904,
+            "timestamp": 0.6560681638682588
+        },
+        {
+            "x": 2.586648366744574,
+            "y": 6.582201701925324,
+            "heading": 3.4545567132875594,
+            "angularVelocity": 0.6391203870252214,
+            "velocityX": -2.3302343010286215,
+            "velocityY": 1.4209751023939097,
+            "timestamp": 0.7380766843517912
+        },
+        {
+            "x": 2.4433241632679117,
+            "y": 6.669600830029101,
+            "heading": 3.4938702766455694,
+            "angularVelocity": 0.4793838875059847,
+            "velocityX": -1.7476745420061834,
+            "velocityY": 1.0657322871875106,
+            "timestamp": 0.8200852048353235
+        },
+        {
+            "x": 2.3477747237681617,
+            "y": 6.727866941749082,
+            "heading": 3.5200804322455714,
+            "angularVelocity": 0.3196028345038231,
+            "velocityX": -1.1651160018054092,
+            "velocityY": 0.7104885123696424,
+            "timestamp": 0.9020937253188559
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 0.15979894353351334,
+            "velocityX": -0.5825580499011068,
+            "velocityY": 0.35524428533945085,
+            "timestamp": 0.9841022458023883
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 2.6358633635199364e-33,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 1.0661107662859206
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.traj
new file mode 100644
index 00000000..f00ff892
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -6.652842580111428e-35,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.215224473336601,
+            "y": 5.589131466417677,
+            "heading": 3.1531161748208096,
+            "angularVelocity": 0.15993673271356004,
+            "velocityX": -0.5825678402891398,
+            "velocityY": 0.3552248747558748,
+            "timestamp": 0.08200852048353235
+        },
+        {
+            "x": 4.119673422721879,
+            "y": 5.647394502413024,
+            "heading": 3.179344235647322,
+            "angularVelocity": 0.3198211682379922,
+            "velocityX": -1.1651356475076162,
+            "velocityY": 0.7104510074297316,
+            "timestamp": 0.1640170409670647
+        },
+        {
+            "x": 3.9763468590229047,
+            "y": 5.73478928117174,
+            "heading": 3.2186777846940484,
+            "angularVelocity": 0.47962759009443284,
+            "velocityX": -1.7477033222146137,
+            "velocityY": 1.065679251904866,
+            "timestamp": 0.24602556145059706
+        },
+        {
+            "x": 3.785244828834372,
+            "y": 5.85131604858519,
+            "heading": 3.2711097941834586,
+            "angularVelocity": 0.6393483162513433,
+            "velocityX": -2.3302704287526703,
+            "velocityY": 1.420910494743635,
+            "timestamp": 0.3280340819341294
+        },
+        {
+            "x": 3.546367452965707,
+            "y": 5.996975111850807,
+            "heading": 3.3366347863626045,
+            "angularVelocity": 0.7990022474835781,
+            "velocityX": -2.9128360621581053,
+            "velocityY": 1.7761454835033377,
+            "timestamp": 0.4100426024176618
+        },
+        {
+            "x": 3.281494477712196,
+            "y": 6.158489941563481,
+            "heading": 3.336634794646938,
+            "angularVelocity": 1.0101796436143703e-7,
+            "velocityX": -3.2298226293047043,
+            "velocityY": 1.9694883990146768,
+            "timestamp": 0.4920511229011941
+        },
+        {
+            "x": 3.016621502775785,
+            "y": 6.320004771796177,
+            "heading": 3.336634802931272,
+            "angularVelocity": 1.0101796370003125e-7,
+            "velocityX": -3.2298226254380333,
+            "velocityY": 1.969488405355746,
+            "timestamp": 0.5740596433847265
+        },
+        {
+            "x": 2.7777474341519097,
+            "y": 6.465669636134063,
+            "heading": 3.402143395936758,
+            "angularVelocity": 0.7988022783393678,
+            "velocityX": -2.912795734094993,
+            "velocityY": 1.7762162209368904,
+            "timestamp": 0.6560681638682588
+        },
+        {
+            "x": 2.586648366744574,
+            "y": 6.582201701925324,
+            "heading": 3.4545567132875594,
+            "angularVelocity": 0.6391203870252214,
+            "velocityX": -2.3302343010286215,
+            "velocityY": 1.4209751023939097,
+            "timestamp": 0.7380766843517912
+        },
+        {
+            "x": 2.4433241632679117,
+            "y": 6.669600830029101,
+            "heading": 3.4938702766455694,
+            "angularVelocity": 0.4793838875059847,
+            "velocityX": -1.7476745420061834,
+            "velocityY": 1.0657322871875106,
+            "timestamp": 0.8200852048353235
+        },
+        {
+            "x": 2.3477747237681617,
+            "y": 6.727866941749082,
+            "heading": 3.5200804322455714,
+            "angularVelocity": 0.3196028345038231,
+            "velocityX": -1.1651160018054092,
+            "velocityY": 0.7104885123696424,
+            "timestamp": 0.9020937253188559
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 0.15979894353351334,
+            "velocityX": -0.5825580499011068,
+            "velocityY": 0.35524428533945085,
+            "timestamp": 0.9841022458023883
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 2.6358633635199364e-33,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 1.0661107662859206
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj
new file mode 100644
index 00000000..d7ef3c12
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -4.0059408516052846e-40,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.181096344253034,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 1.5012541318382292e-16,
+            "velocityX": -0.8263919813283321,
+            "velocityY": -1.2317462001949155e-21,
+            "timestamp": 0.09910993523353699
+        },
+        {
+            "x": 4.017289035836238,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 3.002363652724146e-16,
+            "velocityX": -1.6527839316089836,
+            "velocityY": -2.463492548222639e-21,
+            "timestamp": 0.19821987046707398
+        },
+        {
+            "x": 3.771578080903875,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 4.503226484774511e-16,
+            "velocityX": -2.4791758197942855,
+            "velocityY": -3.695244043691227e-21,
+            "timestamp": 0.29732980570061096
+        },
+        {
+            "x": 3.4439634979187304,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 6.003547774343467e-16,
+            "velocityX": -3.305567521693684,
+            "velocityY": -4.926945384758097e-21,
+            "timestamp": 0.39643974093414797
+        },
+        {
+            "x": 3.0690365020812695,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 4.535737431947664e-16,
+            "velocityX": -3.7829405796099507,
+            "velocityY": 2.4634613990171587e-20,
+            "timestamp": 0.49554967616768497
+        },
+        {
+            "x": 2.741421919096125,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 5.990674751034296e-16,
+            "velocityX": -3.305567521693684,
+            "velocityY": -4.926861067570722e-21,
+            "timestamp": 0.5946596114012219
+        },
+        {
+            "x": 2.4957109641637625,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 4.492129826743599e-16,
+            "velocityX": -2.479175819794286,
+            "velocityY": -3.695159726389389e-21,
+            "timestamp": 0.6937695466347589
+        },
+        {
+            "x": 2.3319036557469652,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 2.9945516157260173e-16,
+            "velocityX": -1.6527839316089836,
+            "velocityY": -2.4634409378754245e-21,
+            "timestamp": 0.7928794818682958
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 1.4972226131077696e-16,
+            "velocityX": -0.8263919813283321,
+            "velocityY": -1.2317240814696432e-21,
+            "timestamp": 0.8919894171018328
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.546248641381176e-40,
+            "timestamp": 0.9910993523353697
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.traj
new file mode 100644
index 00000000..d7ef3c12
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -4.0059408516052846e-40,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.181096344253034,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 1.5012541318382292e-16,
+            "velocityX": -0.8263919813283321,
+            "velocityY": -1.2317462001949155e-21,
+            "timestamp": 0.09910993523353699
+        },
+        {
+            "x": 4.017289035836238,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 3.002363652724146e-16,
+            "velocityX": -1.6527839316089836,
+            "velocityY": -2.463492548222639e-21,
+            "timestamp": 0.19821987046707398
+        },
+        {
+            "x": 3.771578080903875,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 4.503226484774511e-16,
+            "velocityX": -2.4791758197942855,
+            "velocityY": -3.695244043691227e-21,
+            "timestamp": 0.29732980570061096
+        },
+        {
+            "x": 3.4439634979187304,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 6.003547774343467e-16,
+            "velocityX": -3.305567521693684,
+            "velocityY": -4.926945384758097e-21,
+            "timestamp": 0.39643974093414797
+        },
+        {
+            "x": 3.0690365020812695,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 4.535737431947664e-16,
+            "velocityX": -3.7829405796099507,
+            "velocityY": 2.4634613990171587e-20,
+            "timestamp": 0.49554967616768497
+        },
+        {
+            "x": 2.741421919096125,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 5.990674751034296e-16,
+            "velocityX": -3.305567521693684,
+            "velocityY": -4.926861067570722e-21,
+            "timestamp": 0.5946596114012219
+        },
+        {
+            "x": 2.4957109641637625,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 4.492129826743599e-16,
+            "velocityX": -2.479175819794286,
+            "velocityY": -3.695159726389389e-21,
+            "timestamp": 0.6937695466347589
+        },
+        {
+            "x": 2.3319036557469652,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 2.9945516157260173e-16,
+            "velocityX": -1.6527839316089836,
+            "velocityY": -2.4634409378754245e-21,
+            "timestamp": 0.7928794818682958
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 1.4972226131077696e-16,
+            "velocityX": -0.8263919813283321,
+            "velocityY": -1.2317240814696432e-21,
+            "timestamp": 0.8919894171018328
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.546248641381176e-40,
+            "timestamp": 0.9910993523353697
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj
new file mode 100644
index 00000000..e124574f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -1.339537382865945e-32,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.215219135134553,
+            "y": 5.5307922107130985,
+            "heading": 3.1269908374623143,
+            "angularVelocity": -0.1585720754034668,
+            "velocityX": -0.582413424718004,
+            "velocityY": -0.3560213620019152,
+            "timestamp": 0.08203942910241563
+        },
+        {
+            "x": 4.119657409619225,
+            "y": 5.472376532354226,
+            "heading": 3.100976599590504,
+            "angularVelocity": -0.3170943308166476,
+            "velocityX": -1.1648267980513523,
+            "velocityY": -0.7120439403100625,
+            "timestamp": 0.16407885820483126
+        },
+        {
+            "x": 3.976314836365869,
+            "y": 5.384752798145142,
+            "heading": 3.0619633632091103,
+            "angularVelocity": -0.4755425142304533,
+            "velocityX": -1.7472400139987678,
+            "velocityY": -1.0680685515217063,
+            "timestamp": 0.24611828730724689
+        },
+        {
+            "x": 3.785191463205027,
+            "y": 5.267920772243767,
+            "heading": 3.009957765442303,
+            "angularVelocity": -0.6339097960065634,
+            "velocityX": -2.3296526469272343,
+            "velocityY": -1.4240960374739504,
+            "timestamp": 0.3281577164096625
+        },
+        {
+            "x": 3.546287409634579,
+            "y": 5.121880161282017,
+            "heading": 2.944964910348519,
+            "angularVelocity": -0.7922148630830776,
+            "velocityX": -2.9120638232648712,
+            "velocityY": -1.780127099366288,
+            "timestamp": 0.41019714551207814
+        },
+        {
+            "x": 3.281494622858476,
+            "y": 4.960009610093287,
+            "heading": 2.9449649021355118,
+            "angularVelocity": -1.0011048865301711e-7,
+            "velocityX": -3.227628344969881,
+            "velocityY": -1.9730823697792366,
+            "timestamp": 0.49223657461449377
+        },
+        {
+            "x": 3.0167018363816,
+            "y": 4.798139058415072,
+            "heading": 2.9449648939225046,
+            "angularVelocity": -1.0011048873886247e-7,
+            "velocityX": -3.2276283413225157,
+            "velocityY": -1.9730823757457077,
+            "timestamp": 0.5742760037169095
+        },
+        {
+            "x": 2.777800999846863,
+            "y": 4.652092907401912,
+            "heading": 2.879984644909019,
+            "angularVelocity": -0.7920612042822246,
+            "velocityX": -2.912024609977479,
+            "velocityY": -1.7801946284979562,
+            "timestamp": 0.656315432819325
+        },
+        {
+            "x": 2.58668051019359,
+            "y": 4.5352558204412405,
+            "heading": 2.827994144000888,
+            "angularVelocity": -0.6337257764583869,
+            "velocityX": -2.329617499101367,
+            "velocityY": -1.424157728045816,
+            "timestamp": 0.7383548619217406
+        },
+        {
+            "x": 2.443340235924958,
+            "y": 4.447627930217013,
+            "heading": 2.788997784467408,
+            "angularVelocity": -0.4753367979291705,
+            "velocityX": -1.7472119910743198,
+            "velocityY": -1.0681192102743058,
+            "timestamp": 0.8203942910241562
+        },
+        {
+            "x": 2.3477800812496348,
+            "y": 4.389209311628481,
+            "heading": 2.7629991294364147,
+            "angularVelocity": -0.31690438750539845,
+            "velocityX": -1.1648076506728071,
+            "velocityY": -0.7120797795363016,
+            "timestamp": 0.9024337201265717
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": -0.1584497793150997,
+            "velocityX": -0.5824038730204719,
+            "velocityY": -0.35603991822050945,
+            "timestamp": 0.9844731492289873
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": -7.396671024736362e-34,
+            "velocityX": -1.1837806823878881e-33,
+            "velocityY": 0,
+            "timestamp": 1.066512578331403
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.traj
new file mode 100644
index 00000000..e124574f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -1.339537382865945e-32,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.215219135134553,
+            "y": 5.5307922107130985,
+            "heading": 3.1269908374623143,
+            "angularVelocity": -0.1585720754034668,
+            "velocityX": -0.582413424718004,
+            "velocityY": -0.3560213620019152,
+            "timestamp": 0.08203942910241563
+        },
+        {
+            "x": 4.119657409619225,
+            "y": 5.472376532354226,
+            "heading": 3.100976599590504,
+            "angularVelocity": -0.3170943308166476,
+            "velocityX": -1.1648267980513523,
+            "velocityY": -0.7120439403100625,
+            "timestamp": 0.16407885820483126
+        },
+        {
+            "x": 3.976314836365869,
+            "y": 5.384752798145142,
+            "heading": 3.0619633632091103,
+            "angularVelocity": -0.4755425142304533,
+            "velocityX": -1.7472400139987678,
+            "velocityY": -1.0680685515217063,
+            "timestamp": 0.24611828730724689
+        },
+        {
+            "x": 3.785191463205027,
+            "y": 5.267920772243767,
+            "heading": 3.009957765442303,
+            "angularVelocity": -0.6339097960065634,
+            "velocityX": -2.3296526469272343,
+            "velocityY": -1.4240960374739504,
+            "timestamp": 0.3281577164096625
+        },
+        {
+            "x": 3.546287409634579,
+            "y": 5.121880161282017,
+            "heading": 2.944964910348519,
+            "angularVelocity": -0.7922148630830776,
+            "velocityX": -2.9120638232648712,
+            "velocityY": -1.780127099366288,
+            "timestamp": 0.41019714551207814
+        },
+        {
+            "x": 3.281494622858476,
+            "y": 4.960009610093287,
+            "heading": 2.9449649021355118,
+            "angularVelocity": -1.0011048865301711e-7,
+            "velocityX": -3.227628344969881,
+            "velocityY": -1.9730823697792366,
+            "timestamp": 0.49223657461449377
+        },
+        {
+            "x": 3.0167018363816,
+            "y": 4.798139058415072,
+            "heading": 2.9449648939225046,
+            "angularVelocity": -1.0011048873886247e-7,
+            "velocityX": -3.2276283413225157,
+            "velocityY": -1.9730823757457077,
+            "timestamp": 0.5742760037169095
+        },
+        {
+            "x": 2.777800999846863,
+            "y": 4.652092907401912,
+            "heading": 2.879984644909019,
+            "angularVelocity": -0.7920612042822246,
+            "velocityX": -2.912024609977479,
+            "velocityY": -1.7801946284979562,
+            "timestamp": 0.656315432819325
+        },
+        {
+            "x": 2.58668051019359,
+            "y": 4.5352558204412405,
+            "heading": 2.827994144000888,
+            "angularVelocity": -0.6337257764583869,
+            "velocityX": -2.329617499101367,
+            "velocityY": -1.424157728045816,
+            "timestamp": 0.7383548619217406
+        },
+        {
+            "x": 2.443340235924958,
+            "y": 4.447627930217013,
+            "heading": 2.788997784467408,
+            "angularVelocity": -0.4753367979291705,
+            "velocityX": -1.7472119910743198,
+            "velocityY": -1.0681192102743058,
+            "timestamp": 0.8203942910241562
+        },
+        {
+            "x": 2.3477800812496348,
+            "y": 4.389209311628481,
+            "heading": 2.7629991294364147,
+            "angularVelocity": -0.31690438750539845,
+            "velocityX": -1.1648076506728071,
+            "velocityY": -0.7120797795363016,
+            "timestamp": 0.9024337201265717
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": -0.1584497793150997,
+            "velocityX": -0.5824038730204719,
+            "velocityY": -0.35603991822050945,
+            "timestamp": 0.9844731492289873
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": -7.396671024736362e-34,
+            "velocityX": -1.1837806823878881e-33,
+            "velocityY": 0,
+            "timestamp": 1.066512578331403
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S1.1.traj
new file mode 100644
index 00000000..c69cb9ab
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S1.1.traj	
@@ -0,0 +1,310 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.308593242288023,
+            "y": 2.998690970742917,
+            "heading": 2.600069907206834,
+            "angularVelocity": 0.35025874949099595,
+            "velocityX": 0.6125619052781636,
+            "velocityY": -0.017587287403649503,
+            "timestamp": 0.07443042392151207
+        },
+        {
+            "x": 4.399870755267574,
+            "y": 2.9960680712021444,
+            "heading": 2.65011196683443,
+            "angularVelocity": 0.6723333952842253,
+            "velocityX": 1.2263468104898216,
+            "velocityY": -0.03523961577241709,
+            "timestamp": 0.14886084784302414
+        },
+        {
+            "x": 4.536960438371983,
+            "y": 2.992122797800661,
+            "heading": 2.720779852914046,
+            "angularVelocity": 0.9494489263440683,
+            "velocityX": 1.8418500914219216,
+            "velocityY": -0.05300619281227453,
+            "timestamp": 0.22329127176453623
+        },
+        {
+            "x": 4.72004322969291,
+            "y": 2.9868327872754663,
+            "heading": 2.805997286445448,
+            "angularVelocity": 1.1449274240499019,
+            "velocityX": 2.4597843418706042,
+            "velocityY": -0.07107322847942685,
+            "timestamp": 0.2977216956860483
+        },
+        {
+            "x": 4.949295920013874,
+            "y": 2.980134342239005,
+            "heading": 2.892264553695649,
+            "angularVelocity": 1.1590323244855822,
+            "velocityX": 3.0800938412323915,
+            "velocityY": -0.08999606187282569,
+            "timestamp": 0.37215211960756034
+        },
+        {
+            "x": 5.2218046444028055,
+            "y": 2.971482120946463,
+            "heading": 2.9184186664707665,
+            "angularVelocity": 0.35139008213490475,
+            "velocityX": 3.661254498245178,
+            "velocityY": -0.11624576129869893,
+            "timestamp": 0.4465825435290724
+        },
+        {
+            "x": 5.503268186944086,
+            "y": 2.9638921982075033,
+            "heading": 2.918418695115671,
+            "angularVelocity": 3.8485478250287795e-7,
+            "velocityX": 3.7815657591590193,
+            "velocityY": -0.10197339124338625,
+            "timestamp": 0.5210129674505845
+        },
+        {
+            "x": 5.784731729781038,
+            "y": 2.9563022864327206,
+            "heading": 2.9184187237605435,
+            "angularVelocity": 3.8485435121714565e-7,
+            "velocityX": 3.781565763131487,
+            "velocityY": -0.10197324393565109,
+            "timestamp": 0.5954433913720966
+        },
+        {
+            "x": 6.066195282639446,
+            "y": 2.9487127463017835,
+            "heading": 2.9184187524054175,
+            "angularVelocity": 3.848543706133971e-7,
+            "velocityX": 3.7815658977733997,
+            "velocityY": -0.10196825076449405,
+            "timestamp": 0.6698738152936087
+        },
+        {
+            "x": 6.347042051479281,
+            "y": 2.968823062647617,
+            "heading": 2.918418781616621,
+            "angularVelocity": 3.924632205757564e-7,
+            "velocityX": 3.773279178632548,
+            "velocityY": 0.2701894640159414,
+            "timestamp": 0.7443042392151208
+        },
+        {
+            "x": 6.6208214562671275,
+            "y": 3.0345814714160126,
+            "heading": 2.918418814269471,
+            "angularVelocity": 4.3870299176986663e-7,
+            "velocityX": 3.678326554696914,
+            "velocityY": 0.8834883009364279,
+            "timestamp": 0.8187346631366329
+        },
+        {
+            "x": 6.88016498053821,
+            "y": 3.1442184338201953,
+            "heading": 2.9184188559848527,
+            "angularVelocity": 5.604614269784904e-7,
+            "velocityX": 3.4843752138851785,
+            "velocityY": 1.4730127362944399,
+            "timestamp": 0.893165087058145
+        },
+        {
+            "x": 7.118091210487748,
+            "y": 3.294782489019261,
+            "heading": 2.918422820305874,
+            "angularVelocity": 0.00005326210462723065,
+            "velocityX": 3.196626022181901,
+            "velocityY": 2.022883214501594,
+            "timestamp": 0.9675955109796571
+        },
+        {
+            "x": 7.320693053454344,
+            "y": 3.474127369690306,
+            "heading": 2.9455538073395324,
+            "angularVelocity": 0.36451474550610485,
+            "velocityX": 2.722029948133077,
+            "velocityY": 2.4095641435572857,
+            "timestamp": 1.0420259349011691
+        },
+        {
+            "x": 7.483696807015107,
+            "y": 3.6758684658318805,
+            "heading": 3.001437207311255,
+            "angularVelocity": 0.7508139417646484,
+            "velocityX": 2.1900151171065767,
+            "velocityY": 2.710465499354305,
+            "timestamp": 1.1164563588226812
+        },
+        {
+            "x": 7.603947516595235,
+            "y": 3.8947228146234494,
+            "heading": 3.0702288107606313,
+            "angularVelocity": 0.9242403821576988,
+            "velocityX": 1.6156123160998233,
+            "velocityY": 2.940388315164694,
+            "timestamp": 1.1908867827441934
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.9374014759467769,
+            "velocityX": 1.0217929631162956,
+            "velocityY": 3.12072903980085,
+            "timestamp": 1.2653172066657055
+        },
+        {
+            "x": 7.709300887372812,
+            "y": 4.397378653394831,
+            "heading": 3.2093959242930556,
+            "angularVelocity": 0.8431459366282483,
+            "velocityX": 0.355999640896236,
+            "velocityY": 3.2850439746024853,
+            "timestamp": 1.3476231530960114
+        },
+        {
+            "x": 7.683172834427827,
+            "y": 4.6781135002756455,
+            "heading": 3.266996168893978,
+            "angularVelocity": 0.6998309004282659,
+            "velocityX": -0.31745036753947053,
+            "velocityY": 3.4108695550756014,
+            "timestamp": 1.4299290995263174
+        },
+        {
+            "x": 7.601002938928111,
+            "y": 4.963519420188029,
+            "heading": 3.305651937027416,
+            "angularVelocity": 0.4696594816046569,
+            "velocityX": -0.9983470097059609,
+            "velocityY": 3.467622113477001,
+            "timestamp": 1.5122350459566234
+        },
+        {
+            "x": 7.463684437134381,
+            "y": 5.241326726377331,
+            "heading": 3.3097255928301426,
+            "angularVelocity": 0.049494064273674344,
+            "velocityX": -1.6683910185032116,
+            "velocityY": 3.3753005492080685,
+            "timestamp": 1.5945409923869294
+        },
+        {
+            "x": 7.277476070304,
+            "y": 5.490867404030893,
+            "heading": 3.3097256042381358,
+            "angularVelocity": 1.3860472723422523e-7,
+            "velocityX": -2.2623926326885195,
+            "velocityY": 3.031866936429266,
+            "timestamp": 1.6768469388172353
+        },
+        {
+            "x": 7.049248371933673,
+            "y": 5.702660296381284,
+            "heading": 3.3097256090357283,
+            "angularVelocity": 5.8289739150802425e-8,
+            "velocityX": -2.7729187047691926,
+            "velocityY": 2.573239255923398,
+            "timestamp": 1.7591528852475413
+        },
+        {
+            "x": 6.786512642610724,
+            "y": 5.869735280017231,
+            "heading": 3.309725613101849,
+            "angularVelocity": 4.940251326866522e-8,
+            "velocityX": -3.192184048881866,
+            "velocityY": 2.029926036722274,
+            "timestamp": 1.8414588316778473
+        },
+        {
+            "x": 6.5049501154842,
+            "y": 6.00265125873641,
+            "heading": 3.309725617013439,
+            "angularVelocity": 4.7524998408670815e-8,
+            "velocityX": -3.4209256966013024,
+            "velocityY": 1.6149012857986877,
+            "timestamp": 1.9237647781081533
+        },
+        {
+            "x": 6.223387384136782,
+            "y": 6.135566804843462,
+            "heading": 3.309725620925028,
+            "angularVelocity": 4.75249886637903e-8,
+            "velocityX": -3.4209281778423657,
+            "velocityY": 1.6148960296520045,
+            "timestamp": 2.0060707245384592
+        },
+        {
+            "x": 5.941824652769856,
+            "y": 6.268482350909119,
+            "heading": 3.3097256248366427,
+            "angularVelocity": 4.752530281025494e-8,
+            "velocityX": -3.42092817807938,
+            "velocityY": 1.6148960291490546,
+            "timestamp": 2.0883766709687652
+        },
+        {
+            "x": 5.686549787544516,
+            "y": 6.388988216738383,
+            "heading": 3.340879096998464,
+            "angularVelocity": 0.3785081578303866,
+            "velocityX": -3.1015361136937893,
+            "velocityY": 1.464121015014444,
+            "timestamp": 2.170682617399071
+        },
+        {
+            "x": 5.482329878299888,
+            "y": 6.485392923986267,
+            "heading": 3.365801660864199,
+            "angularVelocity": 0.30280392786490506,
+            "velocityX": -2.4812290982833782,
+            "velocityY": 1.1712969892098404,
+            "timestamp": 2.252988563829377
+        },
+        {
+            "x": 5.32916494108436,
+            "y": 6.557696460239907,
+            "heading": 3.3844934905089157,
+            "angularVelocity": 0.22710181287501843,
+            "velocityX": -1.8609218878884095,
+            "velocityY": 0.8784728125915443,
+            "timestamp": 2.335294510259683
+        },
+        {
+            "x": 5.227054980864034,
+            "y": 6.6058988198426265,
+            "heading": 3.396954693375837,
+            "angularVelocity": 0.1514010032977761,
+            "velocityX": -1.2406146171563652,
+            "velocityY": 0.5856485672458114,
+            "timestamp": 2.417600456689989
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.07570065194530232,
+            "velocityX": -0.6203073177375487,
+            "velocityY": 0.29282428794839654,
+            "timestamp": 2.499906403120295
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0,
+            "velocityX": 1.5419247738054587e-33,
+            "velocityY": 0,
+            "timestamp": 2.582212349550601
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S1.traj
new file mode 100644
index 00000000..c69cb9ab
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S1.traj	
@@ -0,0 +1,310 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.308593242288023,
+            "y": 2.998690970742917,
+            "heading": 2.600069907206834,
+            "angularVelocity": 0.35025874949099595,
+            "velocityX": 0.6125619052781636,
+            "velocityY": -0.017587287403649503,
+            "timestamp": 0.07443042392151207
+        },
+        {
+            "x": 4.399870755267574,
+            "y": 2.9960680712021444,
+            "heading": 2.65011196683443,
+            "angularVelocity": 0.6723333952842253,
+            "velocityX": 1.2263468104898216,
+            "velocityY": -0.03523961577241709,
+            "timestamp": 0.14886084784302414
+        },
+        {
+            "x": 4.536960438371983,
+            "y": 2.992122797800661,
+            "heading": 2.720779852914046,
+            "angularVelocity": 0.9494489263440683,
+            "velocityX": 1.8418500914219216,
+            "velocityY": -0.05300619281227453,
+            "timestamp": 0.22329127176453623
+        },
+        {
+            "x": 4.72004322969291,
+            "y": 2.9868327872754663,
+            "heading": 2.805997286445448,
+            "angularVelocity": 1.1449274240499019,
+            "velocityX": 2.4597843418706042,
+            "velocityY": -0.07107322847942685,
+            "timestamp": 0.2977216956860483
+        },
+        {
+            "x": 4.949295920013874,
+            "y": 2.980134342239005,
+            "heading": 2.892264553695649,
+            "angularVelocity": 1.1590323244855822,
+            "velocityX": 3.0800938412323915,
+            "velocityY": -0.08999606187282569,
+            "timestamp": 0.37215211960756034
+        },
+        {
+            "x": 5.2218046444028055,
+            "y": 2.971482120946463,
+            "heading": 2.9184186664707665,
+            "angularVelocity": 0.35139008213490475,
+            "velocityX": 3.661254498245178,
+            "velocityY": -0.11624576129869893,
+            "timestamp": 0.4465825435290724
+        },
+        {
+            "x": 5.503268186944086,
+            "y": 2.9638921982075033,
+            "heading": 2.918418695115671,
+            "angularVelocity": 3.8485478250287795e-7,
+            "velocityX": 3.7815657591590193,
+            "velocityY": -0.10197339124338625,
+            "timestamp": 0.5210129674505845
+        },
+        {
+            "x": 5.784731729781038,
+            "y": 2.9563022864327206,
+            "heading": 2.9184187237605435,
+            "angularVelocity": 3.8485435121714565e-7,
+            "velocityX": 3.781565763131487,
+            "velocityY": -0.10197324393565109,
+            "timestamp": 0.5954433913720966
+        },
+        {
+            "x": 6.066195282639446,
+            "y": 2.9487127463017835,
+            "heading": 2.9184187524054175,
+            "angularVelocity": 3.848543706133971e-7,
+            "velocityX": 3.7815658977733997,
+            "velocityY": -0.10196825076449405,
+            "timestamp": 0.6698738152936087
+        },
+        {
+            "x": 6.347042051479281,
+            "y": 2.968823062647617,
+            "heading": 2.918418781616621,
+            "angularVelocity": 3.924632205757564e-7,
+            "velocityX": 3.773279178632548,
+            "velocityY": 0.2701894640159414,
+            "timestamp": 0.7443042392151208
+        },
+        {
+            "x": 6.6208214562671275,
+            "y": 3.0345814714160126,
+            "heading": 2.918418814269471,
+            "angularVelocity": 4.3870299176986663e-7,
+            "velocityX": 3.678326554696914,
+            "velocityY": 0.8834883009364279,
+            "timestamp": 0.8187346631366329
+        },
+        {
+            "x": 6.88016498053821,
+            "y": 3.1442184338201953,
+            "heading": 2.9184188559848527,
+            "angularVelocity": 5.604614269784904e-7,
+            "velocityX": 3.4843752138851785,
+            "velocityY": 1.4730127362944399,
+            "timestamp": 0.893165087058145
+        },
+        {
+            "x": 7.118091210487748,
+            "y": 3.294782489019261,
+            "heading": 2.918422820305874,
+            "angularVelocity": 0.00005326210462723065,
+            "velocityX": 3.196626022181901,
+            "velocityY": 2.022883214501594,
+            "timestamp": 0.9675955109796571
+        },
+        {
+            "x": 7.320693053454344,
+            "y": 3.474127369690306,
+            "heading": 2.9455538073395324,
+            "angularVelocity": 0.36451474550610485,
+            "velocityX": 2.722029948133077,
+            "velocityY": 2.4095641435572857,
+            "timestamp": 1.0420259349011691
+        },
+        {
+            "x": 7.483696807015107,
+            "y": 3.6758684658318805,
+            "heading": 3.001437207311255,
+            "angularVelocity": 0.7508139417646484,
+            "velocityX": 2.1900151171065767,
+            "velocityY": 2.710465499354305,
+            "timestamp": 1.1164563588226812
+        },
+        {
+            "x": 7.603947516595235,
+            "y": 3.8947228146234494,
+            "heading": 3.0702288107606313,
+            "angularVelocity": 0.9242403821576988,
+            "velocityX": 1.6156123160998233,
+            "velocityY": 2.940388315164694,
+            "timestamp": 1.1908867827441934
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.9374014759467769,
+            "velocityX": 1.0217929631162956,
+            "velocityY": 3.12072903980085,
+            "timestamp": 1.2653172066657055
+        },
+        {
+            "x": 7.709300887372812,
+            "y": 4.397378653394831,
+            "heading": 3.2093959242930556,
+            "angularVelocity": 0.8431459366282483,
+            "velocityX": 0.355999640896236,
+            "velocityY": 3.2850439746024853,
+            "timestamp": 1.3476231530960114
+        },
+        {
+            "x": 7.683172834427827,
+            "y": 4.6781135002756455,
+            "heading": 3.266996168893978,
+            "angularVelocity": 0.6998309004282659,
+            "velocityX": -0.31745036753947053,
+            "velocityY": 3.4108695550756014,
+            "timestamp": 1.4299290995263174
+        },
+        {
+            "x": 7.601002938928111,
+            "y": 4.963519420188029,
+            "heading": 3.305651937027416,
+            "angularVelocity": 0.4696594816046569,
+            "velocityX": -0.9983470097059609,
+            "velocityY": 3.467622113477001,
+            "timestamp": 1.5122350459566234
+        },
+        {
+            "x": 7.463684437134381,
+            "y": 5.241326726377331,
+            "heading": 3.3097255928301426,
+            "angularVelocity": 0.049494064273674344,
+            "velocityX": -1.6683910185032116,
+            "velocityY": 3.3753005492080685,
+            "timestamp": 1.5945409923869294
+        },
+        {
+            "x": 7.277476070304,
+            "y": 5.490867404030893,
+            "heading": 3.3097256042381358,
+            "angularVelocity": 1.3860472723422523e-7,
+            "velocityX": -2.2623926326885195,
+            "velocityY": 3.031866936429266,
+            "timestamp": 1.6768469388172353
+        },
+        {
+            "x": 7.049248371933673,
+            "y": 5.702660296381284,
+            "heading": 3.3097256090357283,
+            "angularVelocity": 5.8289739150802425e-8,
+            "velocityX": -2.7729187047691926,
+            "velocityY": 2.573239255923398,
+            "timestamp": 1.7591528852475413
+        },
+        {
+            "x": 6.786512642610724,
+            "y": 5.869735280017231,
+            "heading": 3.309725613101849,
+            "angularVelocity": 4.940251326866522e-8,
+            "velocityX": -3.192184048881866,
+            "velocityY": 2.029926036722274,
+            "timestamp": 1.8414588316778473
+        },
+        {
+            "x": 6.5049501154842,
+            "y": 6.00265125873641,
+            "heading": 3.309725617013439,
+            "angularVelocity": 4.7524998408670815e-8,
+            "velocityX": -3.4209256966013024,
+            "velocityY": 1.6149012857986877,
+            "timestamp": 1.9237647781081533
+        },
+        {
+            "x": 6.223387384136782,
+            "y": 6.135566804843462,
+            "heading": 3.309725620925028,
+            "angularVelocity": 4.75249886637903e-8,
+            "velocityX": -3.4209281778423657,
+            "velocityY": 1.6148960296520045,
+            "timestamp": 2.0060707245384592
+        },
+        {
+            "x": 5.941824652769856,
+            "y": 6.268482350909119,
+            "heading": 3.3097256248366427,
+            "angularVelocity": 4.752530281025494e-8,
+            "velocityX": -3.42092817807938,
+            "velocityY": 1.6148960291490546,
+            "timestamp": 2.0883766709687652
+        },
+        {
+            "x": 5.686549787544516,
+            "y": 6.388988216738383,
+            "heading": 3.340879096998464,
+            "angularVelocity": 0.3785081578303866,
+            "velocityX": -3.1015361136937893,
+            "velocityY": 1.464121015014444,
+            "timestamp": 2.170682617399071
+        },
+        {
+            "x": 5.482329878299888,
+            "y": 6.485392923986267,
+            "heading": 3.365801660864199,
+            "angularVelocity": 0.30280392786490506,
+            "velocityX": -2.4812290982833782,
+            "velocityY": 1.1712969892098404,
+            "timestamp": 2.252988563829377
+        },
+        {
+            "x": 5.32916494108436,
+            "y": 6.557696460239907,
+            "heading": 3.3844934905089157,
+            "angularVelocity": 0.22710181287501843,
+            "velocityX": -1.8609218878884095,
+            "velocityY": 0.8784728125915443,
+            "timestamp": 2.335294510259683
+        },
+        {
+            "x": 5.227054980864034,
+            "y": 6.6058988198426265,
+            "heading": 3.396954693375837,
+            "angularVelocity": 0.1514010032977761,
+            "velocityX": -1.2406146171563652,
+            "velocityY": 0.5856485672458114,
+            "timestamp": 2.417600456689989
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.07570065194530232,
+            "velocityX": -0.6203073177375487,
+            "velocityY": 0.29282428794839654,
+            "timestamp": 2.499906403120295
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0,
+            "velocityX": 1.5419247738054587e-33,
+            "velocityY": 0,
+            "timestamp": 2.582212349550601
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S2.1.traj
new file mode 100644
index 00000000..9ab4bfc6
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S2.1.traj	
@@ -0,0 +1,301 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.312496748515021,
+            "y": 3.002119555669804,
+            "heading": 2.5925232531186833,
+            "angularVelocity": 0.239650083468562,
+            "velocityX": 0.6403788706575965,
+            "velocityY": 0.027422380395612005,
+            "timestamp": 0.07729291327834321
+        },
+        {
+            "x": 4.411499664032465,
+            "y": 3.0063632003056355,
+            "heading": 2.629246894113083,
+            "angularVelocity": 0.4751230020552096,
+            "velocityX": 1.2808795957906263,
+            "velocityY": 0.05490341165625346,
+            "timestamp": 0.15458582655668643
+        },
+        {
+            "x": 4.560023871242694,
+            "y": 3.012738860842861,
+            "heading": 2.6836373496883423,
+            "angularVelocity": 0.7036926578170436,
+            "velocityX": 1.9215760010930336,
+            "velocityY": 0.08248699999527603,
+            "timestamp": 0.23187873983502966
+        },
+        {
+            "x": 4.758098277209683,
+            "y": 3.021263020053228,
+            "heading": 2.7546133652741296,
+            "angularVelocity": 0.9182732617438291,
+            "velocityX": 2.5626464001129405,
+            "velocityY": 0.11028383908458261,
+            "timestamp": 0.30917165311337286
+        },
+        {
+            "x": 5.005797064956168,
+            "y": 3.0319826680841246,
+            "heading": 2.8389548559272906,
+            "angularVelocity": 1.0911930612504541,
+            "velocityX": 3.2046765640012143,
+            "velocityY": 0.13868862714869012,
+            "timestamp": 0.38646456639171606
+        },
+        {
+            "x": 5.297951167759167,
+            "y": 3.043836589819136,
+            "heading": 2.838954877461829,
+            "angularVelocity": 2.786094794708315e-7,
+            "velocityX": 3.7798303933881865,
+            "velocityY": 0.15336362975896514,
+            "timestamp": 0.46375747967005926
+        },
+        {
+            "x": 5.590081141172345,
+            "y": 3.0562710547940686,
+            "heading": 2.838954891655661,
+            "angularVelocity": 1.83636908600502e-7,
+            "velocityX": 3.7795182122476696,
+            "velocityY": 0.1608745801850421,
+            "timestamp": 0.5410503929484025
+        },
+        {
+            "x": 5.882211114419822,
+            "y": 3.0687055236618757,
+            "heading": 2.8389549058494925,
+            "angularVelocity": 1.8363690785211144e-7,
+            "velocityX": 3.779518210103882,
+            "velocityY": 0.16087463055026802,
+            "timestamp": 0.6183433062267457
+        },
+        {
+            "x": 6.17434108478517,
+            "y": 3.0811400602410006,
+            "heading": 2.838954920043324,
+            "angularVelocity": 1.8363690859499724e-7,
+            "velocityX": 3.779518172815467,
+            "velocityY": 0.1608755065855243,
+            "timestamp": 0.6956362195050889
+        },
+        {
+            "x": 6.465903802710874,
+            "y": 3.1031790875652407,
+            "heading": 2.8389549343224108,
+            "angularVelocity": 1.8473992278083087e-7,
+            "velocityX": 3.772179176061626,
+            "velocityY": 0.28513645545840094,
+            "timestamp": 0.7729291327834321
+        },
+        {
+            "x": 6.7494946065782635,
+            "y": 3.174388582838956,
+            "heading": 2.838954949771126,
+            "angularVelocity": 1.998723373980939e-7,
+            "velocityX": 3.669040172494696,
+            "velocityY": 0.9212939744848142,
+            "timestamp": 0.8502220460617753
+        },
+        {
+            "x": 7.016882485558567,
+            "y": 3.2927027326639142,
+            "heading": 2.838955049089198,
+            "angularVelocity": 0.000001284957026149309,
+            "velocityX": 3.4594100240134633,
+            "velocityY": 1.5307244197005683,
+            "timestamp": 0.9275149593401185
+        },
+        {
+            "x": 7.246855043486079,
+            "y": 3.4413903121844696,
+            "heading": 2.884377696826009,
+            "angularVelocity": 0.5876689829666233,
+            "velocityX": 2.9753382059665197,
+            "velocityY": 1.9236896788339406,
+            "timestamp": 1.0048078726184617
+        },
+        {
+            "x": 7.429257242099238,
+            "y": 3.603847510115455,
+            "heading": 2.9490702967114695,
+            "angularVelocity": 0.8369797066969458,
+            "velocityX": 2.359882567193478,
+            "velocityY": 2.101838202759324,
+            "timestamp": 1.082100785896805
+        },
+        {
+            "x": 7.562439804570718,
+            "y": 3.7739114806600167,
+            "heading": 3.01726271663618,
+            "angularVelocity": 0.8822596669262517,
+            "velocityX": 1.7230889200911585,
+            "velocityY": 2.2002530805396896,
+            "timestamp": 1.1593936991751483
+        },
+        {
+            "x": 7.646052714432655,
+            "y": 3.9488127153581356,
+            "heading": 3.0821685087673805,
+            "angularVelocity": 0.8397379446349181,
+            "velocityX": 1.0817668310784703,
+            "velocityY": 2.2628366208461252,
+            "timestamp": 1.2366866124534917
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.7482120776630553,
+            "velocityX": 0.43920307991361807,
+            "velocityY": 2.305350867034668,
+            "timestamp": 1.313979525731835
+        },
+        {
+            "x": 7.646672814687191,
+            "y": 4.349453153850708,
+            "heading": 3.196736252005302,
+            "angularVelocity": 0.5972944316963834,
+            "velocityX": -0.35085402203857,
+            "velocityY": 2.34188945184307,
+            "timestamp": 1.4089682766635396
+        },
+        {
+            "x": 7.538236721836807,
+            "y": 4.57348977092197,
+            "heading": 3.2390603114191148,
+            "angularVelocity": 0.44556917528311457,
+            "velocityX": -1.1415677307763321,
+            "velocityY": 2.3585594596600363,
+            "timestamp": 1.5039570275952443
+        },
+        {
+            "x": 7.354699862509888,
+            "y": 4.7956671896432965,
+            "heading": 3.2668511132107456,
+            "angularVelocity": 0.2925693992082484,
+            "velocityX": -1.932195733986223,
+            "velocityY": 2.3389866330705615,
+            "timestamp": 1.598945778526949
+        },
+        {
+            "x": 7.096712135037422,
+            "y": 5.0078309608342675,
+            "heading": 3.2799432590323385,
+            "angularVelocity": 0.13782838171022743,
+            "velocityX": -2.7159818919816576,
+            "velocityY": 2.2335673341310986,
+            "timestamp": 1.6939345294586536
+        },
+        {
+            "x": 6.778591882911988,
+            "y": 5.174931015852947,
+            "heading": 3.2799432523824414,
+            "angularVelocity": -7.000720422097915e-8,
+            "velocityX": -3.349030795806095,
+            "velocityY": 1.7591562514473036,
+            "timestamp": 1.7889232803903583
+        },
+        {
+            "x": 6.432650840523452,
+            "y": 5.272130447349719,
+            "heading": 3.2799432455857884,
+            "angularVelocity": -7.155218790875671e-8,
+            "velocityX": -3.6419159005181747,
+            "velocityY": 1.0232730775316359,
+            "timestamp": 1.883912031322063
+        },
+        {
+            "x": 6.076435890184285,
+            "y": 5.319394007657195,
+            "heading": 3.2799432391201027,
+            "angularVelocity": -6.806791276827721e-8,
+            "velocityX": -3.7500751072648586,
+            "velocityY": 0.4975700790239653,
+            "timestamp": 1.9789007822537676
+        },
+        {
+            "x": 5.72022088924427,
+            "y": 5.36665718659578,
+            "heading": 3.279943232654418,
+            "angularVelocity": -6.806790136756466e-8,
+            "velocityX": -3.7500756399684434,
+            "velocityY": 0.49756606413917676,
+            "timestamp": 2.073889533185472
+        },
+        {
+            "x": 5.364005888304073,
+            "y": 5.413920365532998,
+            "heading": 3.279943226188733,
+            "angularVelocity": -6.806790152018608e-8,
+            "velocityX": -3.750075639970353,
+            "velocityY": 0.4975660641247859,
+            "timestamp": 2.1688782841171768
+        },
+        {
+            "x": 5.007790887255383,
+            "y": 5.4611835436519325,
+            "heading": 3.2799432197229685,
+            "angularVelocity": -6.806874126926854e-8,
+            "velocityX": -3.750075641112522,
+            "velocityY": 0.4975660555102637,
+            "timestamp": 2.2638670350488814
+        },
+        {
+            "x": 4.7098745121627585,
+            "y": 5.500710065242911,
+            "heading": 3.22396872446115,
+            "angularVelocity": -0.5892749900676477,
+            "velocityX": -3.1363332202022756,
+            "velocityY": 0.416117921367316,
+            "timestamp": 2.358855785980586
+        },
+        {
+            "x": 4.4864372535551835,
+            "y": 5.530355016492048,
+            "heading": 3.181984877578949,
+            "angularVelocity": -0.44198756663709243,
+            "velocityX": -2.352249676050825,
+            "velocityY": 0.3120890732677509,
+            "timestamp": 2.4538445369122908
+        },
+        {
+            "x": 4.337479086660562,
+            "y": 5.550118338543265,
+            "heading": 3.1539948813381518,
+            "angularVelocity": -0.2946664311958542,
+            "velocityX": -1.5681663926891658,
+            "velocityY": 0.20805960555714925,
+            "timestamp": 2.5488332878439954
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -0.147331986165536,
+            "velocityX": -0.7840832301722918,
+            "velocityY": 0.10402980731729136,
+            "timestamp": 2.6438220387757
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -1.2414335719227057e-31,
+            "velocityX": 3.5860623226783847e-34,
+            "velocityY": 0,
+            "timestamp": 2.7388107897074048
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S2.traj
new file mode 100644
index 00000000..9ab4bfc6
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S2.traj	
@@ -0,0 +1,301 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.312496748515021,
+            "y": 3.002119555669804,
+            "heading": 2.5925232531186833,
+            "angularVelocity": 0.239650083468562,
+            "velocityX": 0.6403788706575965,
+            "velocityY": 0.027422380395612005,
+            "timestamp": 0.07729291327834321
+        },
+        {
+            "x": 4.411499664032465,
+            "y": 3.0063632003056355,
+            "heading": 2.629246894113083,
+            "angularVelocity": 0.4751230020552096,
+            "velocityX": 1.2808795957906263,
+            "velocityY": 0.05490341165625346,
+            "timestamp": 0.15458582655668643
+        },
+        {
+            "x": 4.560023871242694,
+            "y": 3.012738860842861,
+            "heading": 2.6836373496883423,
+            "angularVelocity": 0.7036926578170436,
+            "velocityX": 1.9215760010930336,
+            "velocityY": 0.08248699999527603,
+            "timestamp": 0.23187873983502966
+        },
+        {
+            "x": 4.758098277209683,
+            "y": 3.021263020053228,
+            "heading": 2.7546133652741296,
+            "angularVelocity": 0.9182732617438291,
+            "velocityX": 2.5626464001129405,
+            "velocityY": 0.11028383908458261,
+            "timestamp": 0.30917165311337286
+        },
+        {
+            "x": 5.005797064956168,
+            "y": 3.0319826680841246,
+            "heading": 2.8389548559272906,
+            "angularVelocity": 1.0911930612504541,
+            "velocityX": 3.2046765640012143,
+            "velocityY": 0.13868862714869012,
+            "timestamp": 0.38646456639171606
+        },
+        {
+            "x": 5.297951167759167,
+            "y": 3.043836589819136,
+            "heading": 2.838954877461829,
+            "angularVelocity": 2.786094794708315e-7,
+            "velocityX": 3.7798303933881865,
+            "velocityY": 0.15336362975896514,
+            "timestamp": 0.46375747967005926
+        },
+        {
+            "x": 5.590081141172345,
+            "y": 3.0562710547940686,
+            "heading": 2.838954891655661,
+            "angularVelocity": 1.83636908600502e-7,
+            "velocityX": 3.7795182122476696,
+            "velocityY": 0.1608745801850421,
+            "timestamp": 0.5410503929484025
+        },
+        {
+            "x": 5.882211114419822,
+            "y": 3.0687055236618757,
+            "heading": 2.8389549058494925,
+            "angularVelocity": 1.8363690785211144e-7,
+            "velocityX": 3.779518210103882,
+            "velocityY": 0.16087463055026802,
+            "timestamp": 0.6183433062267457
+        },
+        {
+            "x": 6.17434108478517,
+            "y": 3.0811400602410006,
+            "heading": 2.838954920043324,
+            "angularVelocity": 1.8363690859499724e-7,
+            "velocityX": 3.779518172815467,
+            "velocityY": 0.1608755065855243,
+            "timestamp": 0.6956362195050889
+        },
+        {
+            "x": 6.465903802710874,
+            "y": 3.1031790875652407,
+            "heading": 2.8389549343224108,
+            "angularVelocity": 1.8473992278083087e-7,
+            "velocityX": 3.772179176061626,
+            "velocityY": 0.28513645545840094,
+            "timestamp": 0.7729291327834321
+        },
+        {
+            "x": 6.7494946065782635,
+            "y": 3.174388582838956,
+            "heading": 2.838954949771126,
+            "angularVelocity": 1.998723373980939e-7,
+            "velocityX": 3.669040172494696,
+            "velocityY": 0.9212939744848142,
+            "timestamp": 0.8502220460617753
+        },
+        {
+            "x": 7.016882485558567,
+            "y": 3.2927027326639142,
+            "heading": 2.838955049089198,
+            "angularVelocity": 0.000001284957026149309,
+            "velocityX": 3.4594100240134633,
+            "velocityY": 1.5307244197005683,
+            "timestamp": 0.9275149593401185
+        },
+        {
+            "x": 7.246855043486079,
+            "y": 3.4413903121844696,
+            "heading": 2.884377696826009,
+            "angularVelocity": 0.5876689829666233,
+            "velocityX": 2.9753382059665197,
+            "velocityY": 1.9236896788339406,
+            "timestamp": 1.0048078726184617
+        },
+        {
+            "x": 7.429257242099238,
+            "y": 3.603847510115455,
+            "heading": 2.9490702967114695,
+            "angularVelocity": 0.8369797066969458,
+            "velocityX": 2.359882567193478,
+            "velocityY": 2.101838202759324,
+            "timestamp": 1.082100785896805
+        },
+        {
+            "x": 7.562439804570718,
+            "y": 3.7739114806600167,
+            "heading": 3.01726271663618,
+            "angularVelocity": 0.8822596669262517,
+            "velocityX": 1.7230889200911585,
+            "velocityY": 2.2002530805396896,
+            "timestamp": 1.1593936991751483
+        },
+        {
+            "x": 7.646052714432655,
+            "y": 3.9488127153581356,
+            "heading": 3.0821685087673805,
+            "angularVelocity": 0.8397379446349181,
+            "velocityX": 1.0817668310784703,
+            "velocityY": 2.2628366208461252,
+            "timestamp": 1.2366866124534917
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.7482120776630553,
+            "velocityX": 0.43920307991361807,
+            "velocityY": 2.305350867034668,
+            "timestamp": 1.313979525731835
+        },
+        {
+            "x": 7.646672814687191,
+            "y": 4.349453153850708,
+            "heading": 3.196736252005302,
+            "angularVelocity": 0.5972944316963834,
+            "velocityX": -0.35085402203857,
+            "velocityY": 2.34188945184307,
+            "timestamp": 1.4089682766635396
+        },
+        {
+            "x": 7.538236721836807,
+            "y": 4.57348977092197,
+            "heading": 3.2390603114191148,
+            "angularVelocity": 0.44556917528311457,
+            "velocityX": -1.1415677307763321,
+            "velocityY": 2.3585594596600363,
+            "timestamp": 1.5039570275952443
+        },
+        {
+            "x": 7.354699862509888,
+            "y": 4.7956671896432965,
+            "heading": 3.2668511132107456,
+            "angularVelocity": 0.2925693992082484,
+            "velocityX": -1.932195733986223,
+            "velocityY": 2.3389866330705615,
+            "timestamp": 1.598945778526949
+        },
+        {
+            "x": 7.096712135037422,
+            "y": 5.0078309608342675,
+            "heading": 3.2799432590323385,
+            "angularVelocity": 0.13782838171022743,
+            "velocityX": -2.7159818919816576,
+            "velocityY": 2.2335673341310986,
+            "timestamp": 1.6939345294586536
+        },
+        {
+            "x": 6.778591882911988,
+            "y": 5.174931015852947,
+            "heading": 3.2799432523824414,
+            "angularVelocity": -7.000720422097915e-8,
+            "velocityX": -3.349030795806095,
+            "velocityY": 1.7591562514473036,
+            "timestamp": 1.7889232803903583
+        },
+        {
+            "x": 6.432650840523452,
+            "y": 5.272130447349719,
+            "heading": 3.2799432455857884,
+            "angularVelocity": -7.155218790875671e-8,
+            "velocityX": -3.6419159005181747,
+            "velocityY": 1.0232730775316359,
+            "timestamp": 1.883912031322063
+        },
+        {
+            "x": 6.076435890184285,
+            "y": 5.319394007657195,
+            "heading": 3.2799432391201027,
+            "angularVelocity": -6.806791276827721e-8,
+            "velocityX": -3.7500751072648586,
+            "velocityY": 0.4975700790239653,
+            "timestamp": 1.9789007822537676
+        },
+        {
+            "x": 5.72022088924427,
+            "y": 5.36665718659578,
+            "heading": 3.279943232654418,
+            "angularVelocity": -6.806790136756466e-8,
+            "velocityX": -3.7500756399684434,
+            "velocityY": 0.49756606413917676,
+            "timestamp": 2.073889533185472
+        },
+        {
+            "x": 5.364005888304073,
+            "y": 5.413920365532998,
+            "heading": 3.279943226188733,
+            "angularVelocity": -6.806790152018608e-8,
+            "velocityX": -3.750075639970353,
+            "velocityY": 0.4975660641247859,
+            "timestamp": 2.1688782841171768
+        },
+        {
+            "x": 5.007790887255383,
+            "y": 5.4611835436519325,
+            "heading": 3.2799432197229685,
+            "angularVelocity": -6.806874126926854e-8,
+            "velocityX": -3.750075641112522,
+            "velocityY": 0.4975660555102637,
+            "timestamp": 2.2638670350488814
+        },
+        {
+            "x": 4.7098745121627585,
+            "y": 5.500710065242911,
+            "heading": 3.22396872446115,
+            "angularVelocity": -0.5892749900676477,
+            "velocityX": -3.1363332202022756,
+            "velocityY": 0.416117921367316,
+            "timestamp": 2.358855785980586
+        },
+        {
+            "x": 4.4864372535551835,
+            "y": 5.530355016492048,
+            "heading": 3.181984877578949,
+            "angularVelocity": -0.44198756663709243,
+            "velocityX": -2.352249676050825,
+            "velocityY": 0.3120890732677509,
+            "timestamp": 2.4538445369122908
+        },
+        {
+            "x": 4.337479086660562,
+            "y": 5.550118338543265,
+            "heading": 3.1539948813381518,
+            "angularVelocity": -0.2946664311958542,
+            "velocityX": -1.5681663926891658,
+            "velocityY": 0.20805960555714925,
+            "timestamp": 2.5488332878439954
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -0.147331986165536,
+            "velocityX": -0.7840832301722918,
+            "velocityY": 0.10402980731729136,
+            "timestamp": 2.6438220387757
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -1.2414335719227057e-31,
+            "velocityX": 3.5860623226783847e-34,
+            "velocityY": 0,
+            "timestamp": 2.7388107897074048
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S3.1.traj
new file mode 100644
index 00000000..49c85196
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S3.1.traj	
@@ -0,0 +1,319 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.312741719551674,
+            "y": 3.016403262503484,
+            "heading": 2.6023238516402656,
+            "angularVelocity": 0.35533614833193405,
+            "velocityX": 0.624033456373989,
+            "velocityY": 0.2057867055686534,
+            "timestamp": 0.07971002042214732
+        },
+        {
+            "x": 4.412293799902437,
+            "y": 3.049211318978086,
+            "heading": 2.6573890862312104,
+            "angularVelocity": 0.6908194766394113,
+            "velocityX": 1.248928049742448,
+            "velocityY": 0.41159262412491066,
+            "timestamp": 0.15942004084429465
+        },
+        {
+            "x": 4.5617613818375675,
+            "y": 3.0984309719087952,
+            "heading": 2.7365422108568223,
+            "angularVelocity": 0.9930134781852262,
+            "velocityX": 1.875141684113798,
+            "velocityY": 0.6174838830806988,
+            "timestamp": 0.23913006126644198
+        },
+        {
+            "x": 4.761327239793144,
+            "y": 3.1640795845539236,
+            "heading": 2.8341908945522007,
+            "angularVelocity": 1.225049036221883,
+            "velocityX": 2.503648310446647,
+            "velocityY": 0.8235929723446413,
+            "timestamp": 0.3188400816885893
+        },
+        {
+            "x": 5.011274516078503,
+            "y": 3.2461089339964913,
+            "heading": 2.9311433561003417,
+            "angularVelocity": 1.2163145992771967,
+            "velocityX": 3.1357070912995426,
+            "velocityY": 1.0290970822506025,
+            "timestamp": 0.3985501021107366
+        },
+        {
+            "x": 5.297580410210689,
+            "y": 3.3407358757733725,
+            "heading": 2.9311433795557584,
+            "angularVelocity": 2.942593217144604e-7,
+            "velocityX": 3.5918431913064333,
+            "velocityY": 1.1871398511220184,
+            "timestamp": 0.4782601225328839
+        },
+        {
+            "x": 5.583886278072779,
+            "y": 3.4353628970344365,
+            "heading": 2.931143403010656,
+            "angularVelocity": 2.942528124810342e-7,
+            "velocityX": 3.5918428617356097,
+            "velocityY": 1.187140848288782,
+            "timestamp": 0.5579701429550312
+        },
+        {
+            "x": 5.8701921459347055,
+            "y": 3.529989918295993,
+            "heading": 2.9311434264655536,
+            "angularVelocity": 2.942528130233853e-7,
+            "velocityX": 3.591842861733567,
+            "velocityY": 1.1871408482949637,
+            "timestamp": 0.6376801633771786
+        },
+        {
+            "x": 6.156498013796632,
+            "y": 3.62461693955755,
+            "heading": 2.931143449920451,
+            "angularVelocity": 2.9425281250003654e-7,
+            "velocityX": 3.5918428617335665,
+            "velocityY": 1.1871408482949637,
+            "timestamp": 0.717390183799326
+        },
+        {
+            "x": 6.442803881658564,
+            "y": 3.7192439608190906,
+            "heading": 2.9311434733753488,
+            "angularVelocity": 2.9425281349790425e-7,
+            "velocityX": 3.5918428617336366,
+            "velocityY": 1.1871408482947532,
+            "timestamp": 0.7971002042214733
+        },
+        {
+            "x": 6.729109750413145,
+            "y": 3.813870979379799,
+            "heading": 2.9311434968302543,
+            "angularVelocity": 2.942529087425497e-7,
+            "velocityX": 3.591842872932335,
+            "velocityY": 1.1871408144115423,
+            "timestamp": 0.8768102246436207
+        },
+        {
+            "x": 7.011880136025942,
+            "y": 3.9068513947536463,
+            "heading": 2.940966575440456,
+            "angularVelocity": 0.12323517869119305,
+            "velocityX": 3.547488560600464,
+            "velocityY": 1.1664833967099661,
+            "timestamp": 0.956520245065768
+        },
+        {
+            "x": 7.2458966997348915,
+            "y": 3.984032577425885,
+            "heading": 2.999027591028601,
+            "angularVelocity": 0.7284029696724628,
+            "velocityX": 2.935848748621424,
+            "velocityY": 0.9682745314012468,
+            "timestamp": 1.0362302654879154
+        },
+        {
+            "x": 7.429606919081267,
+            "y": 4.044603850649925,
+            "heading": 3.057792091345064,
+            "angularVelocity": 0.7372285191403096,
+            "velocityX": 2.304731806282817,
+            "velocityY": 0.7598953419313301,
+            "timestamp": 1.1159402859100627
+        },
+        {
+            "x": 7.563114805498745,
+            "y": 4.088594555269305,
+            "heading": 3.1044636591164667,
+            "angularVelocity": 0.5855169466050564,
+            "velocityX": 1.674919736695776,
+            "velocityY": 0.55188424725527,
+            "timestamp": 1.19565030633221
+        },
+        {
+            "x": 7.646547133770953,
+            "y": 4.116048088709135,
+            "heading": 3.133033734418083,
+            "angularVelocity": 0.35842514090835725,
+            "velocityX": 1.046698116878499,
+            "velocityY": 0.34441759385375514,
+            "timestamp": 1.2753603267543572
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.0873951047186249,
+            "velocityX": 0.4196820682252798,
+            "velocityY": 0.13739691989618263,
+            "timestamp": 1.3550703471765044
+        },
+        {
+            "x": 7.658055452006423,
+            "y": 4.119659499210501,
+            "heading": 3.1197811319835678,
+            "angularVelocity": -0.235277452949756,
+            "velocityX": -0.2553583788106411,
+            "velocityY": -0.08541795355335377,
+            "timestamp": 1.4410066226799219
+        },
+        {
+            "x": 7.578036609646341,
+            "y": 4.0931397899338755,
+            "heading": 3.0739363035782867,
+            "angularVelocity": -0.5334746954847726,
+            "velocityX": -0.9311416150086622,
+            "velocityY": -0.30859737778105817,
+            "timestamp": 1.5269428981833393
+        },
+        {
+            "x": 7.439853098092205,
+            "y": 4.047395776011725,
+            "heading": 3.0057619833378117,
+            "angularVelocity": -0.7933124846417565,
+            "velocityX": -1.6079765005482662,
+            "velocityY": -0.5323015647836847,
+            "timestamp": 1.6128791736867567
+        },
+        {
+            "x": 7.243372469557272,
+            "y": 3.982360857227892,
+            "heading": 2.9212012505051064,
+            "angularVelocity": -0.9839934572140336,
+            "velocityX": -2.2863526186577556,
+            "velocityY": -0.7567807471624278,
+            "timestamp": 1.6988154491901741
+        },
+        {
+            "x": 6.9884466912904,
+            "y": 3.897942797791821,
+            "heading": 2.8348834977989155,
+            "angularVelocity": -1.0044390707013913,
+            "velocityX": -2.9664513242342614,
+            "velocityY": -0.9823332340335614,
+            "timestamp": 1.7847517246935916
+        },
+        {
+            "x": 6.679412229111129,
+            "y": 3.7970350761603098,
+            "heading": 2.8348833945193865,
+            "angularVelocity": -0.000001201815274119231,
+            "velocityX": -3.596088617629014,
+            "velocityY": -1.1742156736534226,
+            "timestamp": 1.870688000197009
+        },
+        {
+            "x": 6.370694817722953,
+            "y": 3.695161383669821,
+            "heading": 2.8348833730448595,
+            "angularVelocity": -2.4988896913294006e-7,
+            "velocityX": -3.5923992467639496,
+            "velocityY": -1.1854562219936775,
+            "timestamp": 1.9566242757004264
+        },
+        {
+            "x": 6.061977407921374,
+            "y": 3.59328768637132,
+            "heading": 2.834883351570332,
+            "angularVelocity": -2.498889692815502e-7,
+            "velocityX": -3.592399228301472,
+            "velocityY": -1.185456277942254,
+            "timestamp": 2.042560551203844
+        },
+        {
+            "x": 5.753259998119802,
+            "y": 3.4914139890727944,
+            "heading": 2.834883330095805,
+            "angularVelocity": -2.4988896886019487e-7,
+            "velocityX": -3.59239922830138,
+            "velocityY": -1.1854562779425326,
+            "timestamp": 2.128496826707261
+        },
+        {
+            "x": 5.444542588318229,
+            "y": 3.3895402917742743,
+            "heading": 2.8348833086212775,
+            "angularVelocity": -2.4988896799949907e-7,
+            "velocityX": -3.5923992283013995,
+            "velocityY": -1.1854562779424735,
+            "timestamp": 2.2144331022106782
+        },
+        {
+            "x": 5.135825178181493,
+            "y": 3.28766659549148,
+            "heading": 2.8348832871467184,
+            "angularVelocity": -2.498893376295858e-7,
+            "velocityX": -3.5923992322015317,
+            "velocityY": -1.1854562661229522,
+            "timestamp": 2.3003693777140954
+        },
+        {
+            "x": 4.844823310807952,
+            "y": 3.1918227901947045,
+            "heading": 2.7888021821480646,
+            "angularVelocity": -0.5362241350199245,
+            "velocityX": -3.386251797263078,
+            "velocityY": -1.1152892621342902,
+            "timestamp": 2.3863056532175126
+        },
+        {
+            "x": 4.611844119098646,
+            "y": 3.115032101312665,
+            "heading": 2.714656158607103,
+            "angularVelocity": -0.8628023859145838,
+            "velocityX": -2.7110692236137233,
+            "velocityY": -0.8935771120193101,
+            "timestamp": 2.47224192872093
+        },
+        {
+            "x": 4.43731744566658,
+            "y": 3.0574895143251735,
+            "heading": 2.6481597119281033,
+            "angularVelocity": -0.7737878595443164,
+            "velocityX": -2.030884773742906,
+            "velocityY": -0.669596007627809,
+            "timestamp": 2.558178204224347
+        },
+        {
+            "x": 4.321077385755849,
+            "y": 3.019155902534149,
+            "heading": 2.5996556036782517,
+            "angularVelocity": -0.5644194836896647,
+            "velocityX": -1.3526308794486799,
+            "velocityY": -0.44607020221047544,
+            "timestamp": 2.6441144797277643
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -0.2985421875449052,
+            "velocityX": -0.675819209241155,
+            "velocityY": -0.2229082238197189,
+            "timestamp": 2.7300507552311815
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": -1.2326632641143652e-35,
+            "velocityY": 0,
+            "timestamp": 2.8159870307345987
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S3.traj
new file mode 100644
index 00000000..49c85196
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S3.traj	
@@ -0,0 +1,319 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.312741719551674,
+            "y": 3.016403262503484,
+            "heading": 2.6023238516402656,
+            "angularVelocity": 0.35533614833193405,
+            "velocityX": 0.624033456373989,
+            "velocityY": 0.2057867055686534,
+            "timestamp": 0.07971002042214732
+        },
+        {
+            "x": 4.412293799902437,
+            "y": 3.049211318978086,
+            "heading": 2.6573890862312104,
+            "angularVelocity": 0.6908194766394113,
+            "velocityX": 1.248928049742448,
+            "velocityY": 0.41159262412491066,
+            "timestamp": 0.15942004084429465
+        },
+        {
+            "x": 4.5617613818375675,
+            "y": 3.0984309719087952,
+            "heading": 2.7365422108568223,
+            "angularVelocity": 0.9930134781852262,
+            "velocityX": 1.875141684113798,
+            "velocityY": 0.6174838830806988,
+            "timestamp": 0.23913006126644198
+        },
+        {
+            "x": 4.761327239793144,
+            "y": 3.1640795845539236,
+            "heading": 2.8341908945522007,
+            "angularVelocity": 1.225049036221883,
+            "velocityX": 2.503648310446647,
+            "velocityY": 0.8235929723446413,
+            "timestamp": 0.3188400816885893
+        },
+        {
+            "x": 5.011274516078503,
+            "y": 3.2461089339964913,
+            "heading": 2.9311433561003417,
+            "angularVelocity": 1.2163145992771967,
+            "velocityX": 3.1357070912995426,
+            "velocityY": 1.0290970822506025,
+            "timestamp": 0.3985501021107366
+        },
+        {
+            "x": 5.297580410210689,
+            "y": 3.3407358757733725,
+            "heading": 2.9311433795557584,
+            "angularVelocity": 2.942593217144604e-7,
+            "velocityX": 3.5918431913064333,
+            "velocityY": 1.1871398511220184,
+            "timestamp": 0.4782601225328839
+        },
+        {
+            "x": 5.583886278072779,
+            "y": 3.4353628970344365,
+            "heading": 2.931143403010656,
+            "angularVelocity": 2.942528124810342e-7,
+            "velocityX": 3.5918428617356097,
+            "velocityY": 1.187140848288782,
+            "timestamp": 0.5579701429550312
+        },
+        {
+            "x": 5.8701921459347055,
+            "y": 3.529989918295993,
+            "heading": 2.9311434264655536,
+            "angularVelocity": 2.942528130233853e-7,
+            "velocityX": 3.591842861733567,
+            "velocityY": 1.1871408482949637,
+            "timestamp": 0.6376801633771786
+        },
+        {
+            "x": 6.156498013796632,
+            "y": 3.62461693955755,
+            "heading": 2.931143449920451,
+            "angularVelocity": 2.9425281250003654e-7,
+            "velocityX": 3.5918428617335665,
+            "velocityY": 1.1871408482949637,
+            "timestamp": 0.717390183799326
+        },
+        {
+            "x": 6.442803881658564,
+            "y": 3.7192439608190906,
+            "heading": 2.9311434733753488,
+            "angularVelocity": 2.9425281349790425e-7,
+            "velocityX": 3.5918428617336366,
+            "velocityY": 1.1871408482947532,
+            "timestamp": 0.7971002042214733
+        },
+        {
+            "x": 6.729109750413145,
+            "y": 3.813870979379799,
+            "heading": 2.9311434968302543,
+            "angularVelocity": 2.942529087425497e-7,
+            "velocityX": 3.591842872932335,
+            "velocityY": 1.1871408144115423,
+            "timestamp": 0.8768102246436207
+        },
+        {
+            "x": 7.011880136025942,
+            "y": 3.9068513947536463,
+            "heading": 2.940966575440456,
+            "angularVelocity": 0.12323517869119305,
+            "velocityX": 3.547488560600464,
+            "velocityY": 1.1664833967099661,
+            "timestamp": 0.956520245065768
+        },
+        {
+            "x": 7.2458966997348915,
+            "y": 3.984032577425885,
+            "heading": 2.999027591028601,
+            "angularVelocity": 0.7284029696724628,
+            "velocityX": 2.935848748621424,
+            "velocityY": 0.9682745314012468,
+            "timestamp": 1.0362302654879154
+        },
+        {
+            "x": 7.429606919081267,
+            "y": 4.044603850649925,
+            "heading": 3.057792091345064,
+            "angularVelocity": 0.7372285191403096,
+            "velocityX": 2.304731806282817,
+            "velocityY": 0.7598953419313301,
+            "timestamp": 1.1159402859100627
+        },
+        {
+            "x": 7.563114805498745,
+            "y": 4.088594555269305,
+            "heading": 3.1044636591164667,
+            "angularVelocity": 0.5855169466050564,
+            "velocityX": 1.674919736695776,
+            "velocityY": 0.55188424725527,
+            "timestamp": 1.19565030633221
+        },
+        {
+            "x": 7.646547133770953,
+            "y": 4.116048088709135,
+            "heading": 3.133033734418083,
+            "angularVelocity": 0.35842514090835725,
+            "velocityX": 1.046698116878499,
+            "velocityY": 0.34441759385375514,
+            "timestamp": 1.2753603267543572
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.0873951047186249,
+            "velocityX": 0.4196820682252798,
+            "velocityY": 0.13739691989618263,
+            "timestamp": 1.3550703471765044
+        },
+        {
+            "x": 7.658055452006423,
+            "y": 4.119659499210501,
+            "heading": 3.1197811319835678,
+            "angularVelocity": -0.235277452949756,
+            "velocityX": -0.2553583788106411,
+            "velocityY": -0.08541795355335377,
+            "timestamp": 1.4410066226799219
+        },
+        {
+            "x": 7.578036609646341,
+            "y": 4.0931397899338755,
+            "heading": 3.0739363035782867,
+            "angularVelocity": -0.5334746954847726,
+            "velocityX": -0.9311416150086622,
+            "velocityY": -0.30859737778105817,
+            "timestamp": 1.5269428981833393
+        },
+        {
+            "x": 7.439853098092205,
+            "y": 4.047395776011725,
+            "heading": 3.0057619833378117,
+            "angularVelocity": -0.7933124846417565,
+            "velocityX": -1.6079765005482662,
+            "velocityY": -0.5323015647836847,
+            "timestamp": 1.6128791736867567
+        },
+        {
+            "x": 7.243372469557272,
+            "y": 3.982360857227892,
+            "heading": 2.9212012505051064,
+            "angularVelocity": -0.9839934572140336,
+            "velocityX": -2.2863526186577556,
+            "velocityY": -0.7567807471624278,
+            "timestamp": 1.6988154491901741
+        },
+        {
+            "x": 6.9884466912904,
+            "y": 3.897942797791821,
+            "heading": 2.8348834977989155,
+            "angularVelocity": -1.0044390707013913,
+            "velocityX": -2.9664513242342614,
+            "velocityY": -0.9823332340335614,
+            "timestamp": 1.7847517246935916
+        },
+        {
+            "x": 6.679412229111129,
+            "y": 3.7970350761603098,
+            "heading": 2.8348833945193865,
+            "angularVelocity": -0.000001201815274119231,
+            "velocityX": -3.596088617629014,
+            "velocityY": -1.1742156736534226,
+            "timestamp": 1.870688000197009
+        },
+        {
+            "x": 6.370694817722953,
+            "y": 3.695161383669821,
+            "heading": 2.8348833730448595,
+            "angularVelocity": -2.4988896913294006e-7,
+            "velocityX": -3.5923992467639496,
+            "velocityY": -1.1854562219936775,
+            "timestamp": 1.9566242757004264
+        },
+        {
+            "x": 6.061977407921374,
+            "y": 3.59328768637132,
+            "heading": 2.834883351570332,
+            "angularVelocity": -2.498889692815502e-7,
+            "velocityX": -3.592399228301472,
+            "velocityY": -1.185456277942254,
+            "timestamp": 2.042560551203844
+        },
+        {
+            "x": 5.753259998119802,
+            "y": 3.4914139890727944,
+            "heading": 2.834883330095805,
+            "angularVelocity": -2.4988896886019487e-7,
+            "velocityX": -3.59239922830138,
+            "velocityY": -1.1854562779425326,
+            "timestamp": 2.128496826707261
+        },
+        {
+            "x": 5.444542588318229,
+            "y": 3.3895402917742743,
+            "heading": 2.8348833086212775,
+            "angularVelocity": -2.4988896799949907e-7,
+            "velocityX": -3.5923992283013995,
+            "velocityY": -1.1854562779424735,
+            "timestamp": 2.2144331022106782
+        },
+        {
+            "x": 5.135825178181493,
+            "y": 3.28766659549148,
+            "heading": 2.8348832871467184,
+            "angularVelocity": -2.498893376295858e-7,
+            "velocityX": -3.5923992322015317,
+            "velocityY": -1.1854562661229522,
+            "timestamp": 2.3003693777140954
+        },
+        {
+            "x": 4.844823310807952,
+            "y": 3.1918227901947045,
+            "heading": 2.7888021821480646,
+            "angularVelocity": -0.5362241350199245,
+            "velocityX": -3.386251797263078,
+            "velocityY": -1.1152892621342902,
+            "timestamp": 2.3863056532175126
+        },
+        {
+            "x": 4.611844119098646,
+            "y": 3.115032101312665,
+            "heading": 2.714656158607103,
+            "angularVelocity": -0.8628023859145838,
+            "velocityX": -2.7110692236137233,
+            "velocityY": -0.8935771120193101,
+            "timestamp": 2.47224192872093
+        },
+        {
+            "x": 4.43731744566658,
+            "y": 3.0574895143251735,
+            "heading": 2.6481597119281033,
+            "angularVelocity": -0.7737878595443164,
+            "velocityX": -2.030884773742906,
+            "velocityY": -0.669596007627809,
+            "timestamp": 2.558178204224347
+        },
+        {
+            "x": 4.321077385755849,
+            "y": 3.019155902534149,
+            "heading": 2.5996556036782517,
+            "angularVelocity": -0.5644194836896647,
+            "velocityX": -1.3526308794486799,
+            "velocityY": -0.44607020221047544,
+            "timestamp": 2.6441144797277643
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -0.2985421875449052,
+            "velocityX": -0.675819209241155,
+            "velocityY": -0.2229082238197189,
+            "timestamp": 2.7300507552311815
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": -1.2326632641143652e-35,
+            "velocityY": 0,
+            "timestamp": 2.8159870307345987
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S4.1.traj
new file mode 100644
index 00000000..13cbda64
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S4.1.traj	
@@ -0,0 +1,328 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -2.99079667903027e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 4.307911912995487,
+            "y": 3.0252386980683,
+            "heading": 2.6037905893528444,
+            "angularVelocity": 0.3765521075910298,
+            "velocityX": 0.5676851603736718,
+            "velocityY": 0.31901634566234616,
+            "timestamp": 0.07911412192970564
+        },
+        {
+            "x": 4.397813948925716,
+            "y": 3.0757408190390847,
+            "heading": 2.6615026890848092,
+            "angularVelocity": 0.7294791160450911,
+            "velocityX": 1.1363588919069176,
+            "velocityY": 0.6383452124471162,
+            "timestamp": 0.15822824385941128
+        },
+        {
+            "x": 4.532822684228442,
+            "y": 3.151547007950623,
+            "heading": 2.744086644451416,
+            "angularVelocity": 1.0438585849437199,
+            "velocityX": 1.7065061459278321,
+            "velocityY": 0.9581878312306107,
+            "timestamp": 0.23734236578911694
+        },
+        {
+            "x": 4.713127943842242,
+            "y": 3.2527364142046724,
+            "heading": 2.845383620284061,
+            "angularVelocity": 1.2803905720226456,
+            "velocityX": 2.2790527811710266,
+            "velocityY": 1.2790308959500052,
+            "timestamp": 0.31645648771882257
+        },
+        {
+            "x": 4.938980637634121,
+            "y": 3.3793948659075563,
+            "heading": 2.944365706531666,
+            "angularVelocity": 1.251130440852921,
+            "velocityX": 2.854770909200696,
+            "velocityY": 1.6009588252198856,
+            "timestamp": 0.3955706096485282
+        },
+        {
+            "x": 5.199953974250007,
+            "y": 3.5259002853585115,
+            "heading": 2.944365731476778,
+            "angularVelocity": 3.1530542976951733e-7,
+            "velocityX": 3.2986947241576456,
+            "velocityY": 1.8518238700939902,
+            "timestamp": 0.4746847315782338
+        },
+        {
+            "x": 5.460927260915445,
+            "y": 3.6724057937882604,
+            "heading": 2.944365756420666,
+            "angularVelocity": 3.152899521547411e-7,
+            "velocityX": 3.2986940927855755,
+            "velocityY": 1.8518249947831291,
+            "timestamp": 0.5537988535079394
+        },
+        {
+            "x": 5.721900547580569,
+            "y": 3.818911302218567,
+            "heading": 2.9443657813645534,
+            "angularVelocity": 3.1528995198652365e-7,
+            "velocityX": 3.2986940927816133,
+            "velocityY": 1.851824994790188,
+            "timestamp": 0.6329129754376451
+        },
+        {
+            "x": 5.982873834247296,
+            "y": 3.965416810646019,
+            "heading": 2.9443658063084412,
+            "angularVelocity": 3.1528995220649985e-7,
+            "velocityX": 3.2986940928018726,
+            "velocityY": 1.851824994754099,
+            "timestamp": 0.7120270973673508
+        },
+        {
+            "x": 6.243847376304821,
+            "y": 4.1119218641396325,
+            "heading": 2.944365831252335,
+            "angularVelocity": 3.152900317708399e-7,
+            "velocityX": 3.298697320933481,
+            "velocityY": 1.8518192444047645,
+            "timestamp": 0.7911412192970565
+        },
+        {
+            "x": 6.520761156875476,
+            "y": 4.225454581965132,
+            "heading": 2.9443658572464595,
+            "angularVelocity": 3.2856491270029264e-7,
+            "velocityX": 3.5001814317891196,
+            "velocityY": 1.4350499639795518,
+            "timestamp": 0.8702553412267622
+        },
+        {
+            "x": 6.81318632649156,
+            "y": 4.28915727346349,
+            "heading": 2.9443676715667886,
+            "angularVelocity": 0.000022932951594388774,
+            "velocityX": 3.696244898931049,
+            "velocityY": 0.8052000065798415,
+            "timestamp": 0.9493694631564679
+        },
+        {
+            "x": 7.072106261164272,
+            "y": 4.3159902610395875,
+            "heading": 2.999666770720341,
+            "angularVelocity": 0.6989788650209022,
+            "velocityX": 3.2727397885142184,
+            "velocityY": 0.3391681146374708,
+            "timestamp": 1.0284835850861735
+        },
+        {
+            "x": 7.2892635874980165,
+            "y": 4.311538961400137,
+            "heading": 3.058239945124217,
+            "angularVelocity": 0.7403630726751829,
+            "velocityX": 2.7448617394337207,
+            "velocityY": -0.05626428671490304,
+            "timestamp": 1.1075977070158791
+        },
+        {
+            "x": 7.463265555506274,
+            "y": 4.277921305765472,
+            "heading": 3.1051985992516418,
+            "angularVelocity": 0.593555903573676,
+            "velocityX": 2.1993793745554173,
+            "velocityY": -0.4249261044006207,
+            "timestamp": 1.1867118289455847
+        },
+        {
+            "x": 7.593597601326297,
+            "y": 4.21619741378488,
+            "heading": 3.1337465954664303,
+            "angularVelocity": 0.3608457696105646,
+            "velocityX": 1.6473929387198967,
+            "velocityY": -0.7801880432350068,
+            "timestamp": 1.2658259508752903
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.07904283560305705,
+            "velocityX": 1.0921235876253772,
+            "velocityY": -1.1274524902663325,
+            "timestamp": 1.3449400728049958
+        },
+        {
+            "x": 7.722328891064707,
+            "y": 4.011878570496831,
+            "heading": 3.1218227549328446,
+            "angularVelocity": -0.23159012596074638,
+            "velocityX": 0.539298071695534,
+            "velocityY": -1.4667231618939216,
+            "timestamp": 1.423428933207607
+        },
+        {
+            "x": 7.720975293339473,
+            "y": 3.870431716581803,
+            "heading": 3.0818439654671956,
+            "angularVelocity": -0.5093562227885172,
+            "velocityX": -0.01724573039142055,
+            "velocityY": -1.8021264825284895,
+            "timestamp": 1.5019177936102184
+        },
+        {
+            "x": 7.675510236414314,
+            "y": 3.703145056798568,
+            "heading": 3.02401855987363,
+            "angularVelocity": -0.7367339173603616,
+            "velocityX": -0.5792548992550559,
+            "velocityY": -2.131342701692084,
+            "timestamp": 1.5804066540128296
+        },
+        {
+            "x": 7.585250814368908,
+            "y": 3.51090702633694,
+            "heading": 2.955217915344013,
+            "angularVelocity": -0.8765657212590741,
+            "velocityX": -1.149964741269228,
+            "velocityY": -2.4492396688591995,
+            "timestamp": 1.658895514415441
+        },
+        {
+            "x": 7.449012873441734,
+            "y": 3.2957924623075137,
+            "heading": 2.8903514658300926,
+            "angularVelocity": -0.8264414743848527,
+            "velocityX": -1.7357614854940644,
+            "velocityY": -2.7407018387830946,
+            "timestamp": 1.7373843748180522
+        },
+        {
+            "x": 7.265754194056636,
+            "y": 3.066909845502957,
+            "heading": 2.8806156888219427,
+            "angularVelocity": -0.12404023906336885,
+            "velocityX": -2.334836796522032,
+            "velocityY": -2.916115938370047,
+            "timestamp": 1.8158732352206635
+        },
+        {
+            "x": 7.0429291564371885,
+            "y": 2.8706709413875,
+            "heading": 2.8806156606364275,
+            "angularVelocity": -3.5910210875012317e-7,
+            "velocityX": -2.838938372610575,
+            "velocityY": -2.5002134456895257,
+            "timestamp": 1.8943620956232747
+        },
+        {
+            "x": 6.796142716598087,
+            "y": 2.7055735191439014,
+            "heading": 2.8806156351575583,
+            "angularVelocity": -3.246176492669905e-7,
+            "velocityX": -3.1442224867732946,
+            "velocityY": -2.103450367309771,
+            "timestamp": 1.972850956025886
+        },
+        {
+            "x": 6.549355877169833,
+            "y": 2.5404766942048504,
+            "heading": 2.880615609678698,
+            "angularVelocity": -3.246175317472835e-7,
+            "velocityX": -3.1442275778034396,
+            "velocityY": -2.10344275725474,
+            "timestamp": 2.0513398164284973
+        },
+        {
+            "x": 6.302569037739046,
+            "y": 2.375379869269585,
+            "heading": 2.8806155841998384,
+            "angularVelocity": -3.24617530454395e-7,
+            "velocityX": -3.144227577835705,
+            "velocityY": -2.103442757206509,
+            "timestamp": 2.1298286768311088
+        },
+        {
+            "x": 6.055782203816465,
+            "y": 2.2102830361006496,
+            "heading": 2.8806155587209785,
+            "angularVelocity": -3.246175312544412e-7,
+            "velocityX": -3.1442275076575137,
+            "velocityY": -2.103442862108911,
+            "timestamp": 2.2083175372337203
+        },
+        {
+            "x": 5.809854810532114,
+            "y": 2.043908693921413,
+            "heading": 2.8806155277577266,
+            "angularVelocity": -3.944923106855428e-7,
+            "velocityX": -3.1332776654274115,
+            "velocityY": -2.1197191719412425,
+            "timestamp": 2.2868063976363318
+        },
+        {
+            "x": 5.598037560183058,
+            "y": 1.9023806628591073,
+            "heading": 2.776131644051983,
+            "angularVelocity": -1.3311937919570889,
+            "velocityX": -2.6986918814024308,
+            "velocityY": -1.8031607330815247,
+            "timestamp": 2.3652952580389432
+        },
+        {
+            "x": 5.428977486755044,
+            "y": 1.7893186281455802,
+            "heading": 2.67268813100655,
+            "angularVelocity": -1.3179387815648507,
+            "velocityX": -2.1539371645965275,
+            "velocityY": -1.4404851100343643,
+            "timestamp": 2.4437841184415547
+        },
+        {
+            "x": 5.302396782822421,
+            "y": 1.7046186401543708,
+            "heading": 2.5887102200267456,
+            "angularVelocity": -1.0699341352267786,
+            "velocityX": -1.6127218981562843,
+            "velocityY": -1.0791338739884617,
+            "timestamp": 2.5222729788441662
+        },
+        {
+            "x": 5.218108357184172,
+            "y": 1.6481959100495345,
+            "heading": 2.5301489625141826,
+            "angularVelocity": -0.7461091575565081,
+            "velocityX": -1.0738902973732658,
+            "velocityY": -0.7188629037982504,
+            "timestamp": 2.6007618392467777
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.3841177252355578,
+            "velocityX": -0.5364883241797085,
+            "velocityY": -0.35923454493927354,
+            "timestamp": 2.6792506996493892
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": -9.31070457943821e-33,
+            "velocityY": 1.510534612718486e-32,
+            "timestamp": 2.7577395600520007
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S4.traj
new file mode 100644
index 00000000..13cbda64
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S4.traj	
@@ -0,0 +1,328 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -2.99079667903027e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 4.307911912995487,
+            "y": 3.0252386980683,
+            "heading": 2.6037905893528444,
+            "angularVelocity": 0.3765521075910298,
+            "velocityX": 0.5676851603736718,
+            "velocityY": 0.31901634566234616,
+            "timestamp": 0.07911412192970564
+        },
+        {
+            "x": 4.397813948925716,
+            "y": 3.0757408190390847,
+            "heading": 2.6615026890848092,
+            "angularVelocity": 0.7294791160450911,
+            "velocityX": 1.1363588919069176,
+            "velocityY": 0.6383452124471162,
+            "timestamp": 0.15822824385941128
+        },
+        {
+            "x": 4.532822684228442,
+            "y": 3.151547007950623,
+            "heading": 2.744086644451416,
+            "angularVelocity": 1.0438585849437199,
+            "velocityX": 1.7065061459278321,
+            "velocityY": 0.9581878312306107,
+            "timestamp": 0.23734236578911694
+        },
+        {
+            "x": 4.713127943842242,
+            "y": 3.2527364142046724,
+            "heading": 2.845383620284061,
+            "angularVelocity": 1.2803905720226456,
+            "velocityX": 2.2790527811710266,
+            "velocityY": 1.2790308959500052,
+            "timestamp": 0.31645648771882257
+        },
+        {
+            "x": 4.938980637634121,
+            "y": 3.3793948659075563,
+            "heading": 2.944365706531666,
+            "angularVelocity": 1.251130440852921,
+            "velocityX": 2.854770909200696,
+            "velocityY": 1.6009588252198856,
+            "timestamp": 0.3955706096485282
+        },
+        {
+            "x": 5.199953974250007,
+            "y": 3.5259002853585115,
+            "heading": 2.944365731476778,
+            "angularVelocity": 3.1530542976951733e-7,
+            "velocityX": 3.2986947241576456,
+            "velocityY": 1.8518238700939902,
+            "timestamp": 0.4746847315782338
+        },
+        {
+            "x": 5.460927260915445,
+            "y": 3.6724057937882604,
+            "heading": 2.944365756420666,
+            "angularVelocity": 3.152899521547411e-7,
+            "velocityX": 3.2986940927855755,
+            "velocityY": 1.8518249947831291,
+            "timestamp": 0.5537988535079394
+        },
+        {
+            "x": 5.721900547580569,
+            "y": 3.818911302218567,
+            "heading": 2.9443657813645534,
+            "angularVelocity": 3.1528995198652365e-7,
+            "velocityX": 3.2986940927816133,
+            "velocityY": 1.851824994790188,
+            "timestamp": 0.6329129754376451
+        },
+        {
+            "x": 5.982873834247296,
+            "y": 3.965416810646019,
+            "heading": 2.9443658063084412,
+            "angularVelocity": 3.1528995220649985e-7,
+            "velocityX": 3.2986940928018726,
+            "velocityY": 1.851824994754099,
+            "timestamp": 0.7120270973673508
+        },
+        {
+            "x": 6.243847376304821,
+            "y": 4.1119218641396325,
+            "heading": 2.944365831252335,
+            "angularVelocity": 3.152900317708399e-7,
+            "velocityX": 3.298697320933481,
+            "velocityY": 1.8518192444047645,
+            "timestamp": 0.7911412192970565
+        },
+        {
+            "x": 6.520761156875476,
+            "y": 4.225454581965132,
+            "heading": 2.9443658572464595,
+            "angularVelocity": 3.2856491270029264e-7,
+            "velocityX": 3.5001814317891196,
+            "velocityY": 1.4350499639795518,
+            "timestamp": 0.8702553412267622
+        },
+        {
+            "x": 6.81318632649156,
+            "y": 4.28915727346349,
+            "heading": 2.9443676715667886,
+            "angularVelocity": 0.000022932951594388774,
+            "velocityX": 3.696244898931049,
+            "velocityY": 0.8052000065798415,
+            "timestamp": 0.9493694631564679
+        },
+        {
+            "x": 7.072106261164272,
+            "y": 4.3159902610395875,
+            "heading": 2.999666770720341,
+            "angularVelocity": 0.6989788650209022,
+            "velocityX": 3.2727397885142184,
+            "velocityY": 0.3391681146374708,
+            "timestamp": 1.0284835850861735
+        },
+        {
+            "x": 7.2892635874980165,
+            "y": 4.311538961400137,
+            "heading": 3.058239945124217,
+            "angularVelocity": 0.7403630726751829,
+            "velocityX": 2.7448617394337207,
+            "velocityY": -0.05626428671490304,
+            "timestamp": 1.1075977070158791
+        },
+        {
+            "x": 7.463265555506274,
+            "y": 4.277921305765472,
+            "heading": 3.1051985992516418,
+            "angularVelocity": 0.593555903573676,
+            "velocityX": 2.1993793745554173,
+            "velocityY": -0.4249261044006207,
+            "timestamp": 1.1867118289455847
+        },
+        {
+            "x": 7.593597601326297,
+            "y": 4.21619741378488,
+            "heading": 3.1337465954664303,
+            "angularVelocity": 0.3608457696105646,
+            "velocityX": 1.6473929387198967,
+            "velocityY": -0.7801880432350068,
+            "timestamp": 1.2658259508752903
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.07904283560305705,
+            "velocityX": 1.0921235876253772,
+            "velocityY": -1.1274524902663325,
+            "timestamp": 1.3449400728049958
+        },
+        {
+            "x": 7.722328891064707,
+            "y": 4.011878570496831,
+            "heading": 3.1218227549328446,
+            "angularVelocity": -0.23159012596074638,
+            "velocityX": 0.539298071695534,
+            "velocityY": -1.4667231618939216,
+            "timestamp": 1.423428933207607
+        },
+        {
+            "x": 7.720975293339473,
+            "y": 3.870431716581803,
+            "heading": 3.0818439654671956,
+            "angularVelocity": -0.5093562227885172,
+            "velocityX": -0.01724573039142055,
+            "velocityY": -1.8021264825284895,
+            "timestamp": 1.5019177936102184
+        },
+        {
+            "x": 7.675510236414314,
+            "y": 3.703145056798568,
+            "heading": 3.02401855987363,
+            "angularVelocity": -0.7367339173603616,
+            "velocityX": -0.5792548992550559,
+            "velocityY": -2.131342701692084,
+            "timestamp": 1.5804066540128296
+        },
+        {
+            "x": 7.585250814368908,
+            "y": 3.51090702633694,
+            "heading": 2.955217915344013,
+            "angularVelocity": -0.8765657212590741,
+            "velocityX": -1.149964741269228,
+            "velocityY": -2.4492396688591995,
+            "timestamp": 1.658895514415441
+        },
+        {
+            "x": 7.449012873441734,
+            "y": 3.2957924623075137,
+            "heading": 2.8903514658300926,
+            "angularVelocity": -0.8264414743848527,
+            "velocityX": -1.7357614854940644,
+            "velocityY": -2.7407018387830946,
+            "timestamp": 1.7373843748180522
+        },
+        {
+            "x": 7.265754194056636,
+            "y": 3.066909845502957,
+            "heading": 2.8806156888219427,
+            "angularVelocity": -0.12404023906336885,
+            "velocityX": -2.334836796522032,
+            "velocityY": -2.916115938370047,
+            "timestamp": 1.8158732352206635
+        },
+        {
+            "x": 7.0429291564371885,
+            "y": 2.8706709413875,
+            "heading": 2.8806156606364275,
+            "angularVelocity": -3.5910210875012317e-7,
+            "velocityX": -2.838938372610575,
+            "velocityY": -2.5002134456895257,
+            "timestamp": 1.8943620956232747
+        },
+        {
+            "x": 6.796142716598087,
+            "y": 2.7055735191439014,
+            "heading": 2.8806156351575583,
+            "angularVelocity": -3.246176492669905e-7,
+            "velocityX": -3.1442224867732946,
+            "velocityY": -2.103450367309771,
+            "timestamp": 1.972850956025886
+        },
+        {
+            "x": 6.549355877169833,
+            "y": 2.5404766942048504,
+            "heading": 2.880615609678698,
+            "angularVelocity": -3.246175317472835e-7,
+            "velocityX": -3.1442275778034396,
+            "velocityY": -2.10344275725474,
+            "timestamp": 2.0513398164284973
+        },
+        {
+            "x": 6.302569037739046,
+            "y": 2.375379869269585,
+            "heading": 2.8806155841998384,
+            "angularVelocity": -3.24617530454395e-7,
+            "velocityX": -3.144227577835705,
+            "velocityY": -2.103442757206509,
+            "timestamp": 2.1298286768311088
+        },
+        {
+            "x": 6.055782203816465,
+            "y": 2.2102830361006496,
+            "heading": 2.8806155587209785,
+            "angularVelocity": -3.246175312544412e-7,
+            "velocityX": -3.1442275076575137,
+            "velocityY": -2.103442862108911,
+            "timestamp": 2.2083175372337203
+        },
+        {
+            "x": 5.809854810532114,
+            "y": 2.043908693921413,
+            "heading": 2.8806155277577266,
+            "angularVelocity": -3.944923106855428e-7,
+            "velocityX": -3.1332776654274115,
+            "velocityY": -2.1197191719412425,
+            "timestamp": 2.2868063976363318
+        },
+        {
+            "x": 5.598037560183058,
+            "y": 1.9023806628591073,
+            "heading": 2.776131644051983,
+            "angularVelocity": -1.3311937919570889,
+            "velocityX": -2.6986918814024308,
+            "velocityY": -1.8031607330815247,
+            "timestamp": 2.3652952580389432
+        },
+        {
+            "x": 5.428977486755044,
+            "y": 1.7893186281455802,
+            "heading": 2.67268813100655,
+            "angularVelocity": -1.3179387815648507,
+            "velocityX": -2.1539371645965275,
+            "velocityY": -1.4404851100343643,
+            "timestamp": 2.4437841184415547
+        },
+        {
+            "x": 5.302396782822421,
+            "y": 1.7046186401543708,
+            "heading": 2.5887102200267456,
+            "angularVelocity": -1.0699341352267786,
+            "velocityX": -1.6127218981562843,
+            "velocityY": -1.0791338739884617,
+            "timestamp": 2.5222729788441662
+        },
+        {
+            "x": 5.218108357184172,
+            "y": 1.6481959100495345,
+            "heading": 2.5301489625141826,
+            "angularVelocity": -0.7461091575565081,
+            "velocityX": -1.0738902973732658,
+            "velocityY": -0.7188629037982504,
+            "timestamp": 2.6007618392467777
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.3841177252355578,
+            "velocityX": -0.5364883241797085,
+            "velocityY": -0.35923454493927354,
+            "timestamp": 2.6792506996493892
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": -9.31070457943821e-33,
+            "velocityY": 1.510534612718486e-32,
+            "timestamp": 2.7577395600520007
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S3.1.traj
new file mode 100644
index 00000000..219e0c08
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S3.1.traj	
@@ -0,0 +1,319 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -2.4689012164756437e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 4.313159042990802,
+            "y": 2.9920825634147805,
+            "heading": 2.597885779471472,
+            "angularVelocity": 0.30476728851208423,
+            "velocityX": 0.6399973484188943,
+            "velocityY": -0.10102143339823635,
+            "timestamp": 0.07837382938338545
+        },
+        {
+            "x": 4.4134829053058295,
+            "y": 2.976248991975609,
+            "heading": 2.6455288467924127,
+            "angularVelocity": 0.6078951060041105,
+            "velocityX": 1.2800683991625412,
+            "velocityY": -0.20202625753700879,
+            "timestamp": 0.1567476587667709
+        },
+        {
+            "x": 4.563981380382106,
+            "y": 2.952502286743102,
+            "heading": 2.7167039498074406,
+            "angularVelocity": 0.9081488498779698,
+            "velocityX": 1.9202644079068125,
+            "velocityY": -0.3029927900593087,
+            "timestamp": 0.23512148815015638
+        },
+        {
+            "x": 4.764676150331059,
+            "y": 2.9208479584714695,
+            "heading": 2.8108575648883316,
+            "angularVelocity": 1.2013399858304654,
+            "velocityX": 2.560737066543005,
+            "velocityY": -0.40388900887800433,
+            "timestamp": 0.3134953175335418
+        },
+        {
+            "x": 5.015633164055166,
+            "y": 2.881293970992892,
+            "heading": 2.92608193331875,
+            "angularVelocity": 1.47018933918326,
+            "velocityX": 3.2020511910485845,
+            "velocityY": -0.5046836142851278,
+            "timestamp": 0.39186914691692726
+        },
+        {
+            "x": 5.308481077006102,
+            "y": 2.835005929387577,
+            "heading": 2.926081954236969,
+            "angularVelocity": 2.6690310524294676e-7,
+            "velocityX": 3.7365523064899215,
+            "velocityY": -0.5906058434235878,
+            "timestamp": 0.4702429763003127
+        },
+        {
+            "x": 5.601350966367862,
+            "y": 2.7888571291196405,
+            "heading": 2.92608197144068,
+            "angularVelocity": 2.1950836119943446e-7,
+            "velocityX": 3.7368327114541455,
+            "velocityY": -0.5888292129019572,
+            "timestamp": 0.5486168056836982
+        },
+        {
+            "x": 5.8942208558716445,
+            "y": 2.7427083297530035,
+            "heading": 2.9260819886443907,
+            "angularVelocity": 2.1950836114981294e-7,
+            "velocityX": 3.736832713266251,
+            "velocityY": -0.5888292014019535,
+            "timestamp": 0.6269906350670836
+        },
+        {
+            "x": 6.187090745375428,
+            "y": 2.6965595303863723,
+            "heading": 2.9260820058481016,
+            "angularVelocity": 2.195083613443253e-7,
+            "velocityX": 3.736832713266263,
+            "velocityY": -0.5888292014018791,
+            "timestamp": 0.7053644644504691
+        },
+        {
+            "x": 6.479960634879199,
+            "y": 2.6504107310196727,
+            "heading": 2.9260820230518125,
+            "angularVelocity": 2.1950836077924463e-7,
+            "velocityX": 3.736832713266126,
+            "velocityY": -0.5888292014027512,
+            "timestamp": 0.7837382938338545
+        },
+        {
+            "x": 6.772830522718861,
+            "y": 2.604261921092308,
+            "heading": 2.926082040255567,
+            "angularVelocity": 2.1950891675310117e-7,
+            "velocityX": 3.736832692033143,
+            "velocityY": -0.5888293361500981,
+            "timestamp": 0.86211212321724
+        },
+        {
+            "x": 7.050151670300153,
+            "y": 2.559252271073386,
+            "heading": 2.978396963955354,
+            "angularVelocity": 0.6675050091514937,
+            "velocityX": 3.5384406984211854,
+            "velocityY": -0.5742943833809976,
+            "timestamp": 0.9404859526006254
+        },
+        {
+            "x": 7.276980321576378,
+            "y": 2.5226261995289687,
+            "heading": 3.0372586376427035,
+            "angularVelocity": 0.7510373571182644,
+            "velocityX": 2.894188698712594,
+            "velocityY": -0.4673252772333,
+            "timestamp": 1.0188597819840108
+        },
+        {
+            "x": 7.453315564542908,
+            "y": 2.49415241035015,
+            "heading": 3.0866031396716647,
+            "angularVelocity": 0.6296043260507225,
+            "velocityX": 2.2499250624075064,
+            "velocityY": -0.3633073616900632,
+            "timestamp": 1.0972336113673964
+        },
+        {
+            "x": 7.579233655925443,
+            "y": 2.473748793081471,
+            "heading": 3.1215813110163633,
+            "angularVelocity": 0.446299123315424,
+            "velocityX": 1.6066344132117514,
+            "velocityY": -0.2603371231087117,
+            "timestamp": 1.175607440750782
+        },
+        {
+            "x": 7.654784082070509,
+            "y": 2.4613731780152506,
+            "heading": 3.1398442445430734,
+            "angularVelocity": 0.2330233659676447,
+            "velocityX": 0.9639751781872319,
+            "velocityY": -0.1579049430605922,
+            "timestamp": 1.2539812701341675
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 0.0019873401380948097,
+            "velocityX": 0.32173900558233287,
+            "velocityY": -0.05579895801519462,
+            "timestamp": 1.332355099517553
+        },
+        {
+            "x": 7.650333485517468,
+            "y": 2.4613165476638152,
+            "heading": 3.1188382258181035,
+            "angularVelocity": -0.255122622507773,
+            "velocityX": -0.3576542737105397,
+            "velocityY": 0.05203953840108144,
+            "timestamp": 1.4153025620901392
+        },
+        {
+            "x": 7.564253765684918,
+            "y": 2.4745901696035957,
+            "heading": 3.07855569322522,
+            "angularVelocity": -0.4856391183470325,
+            "velocityX": -1.0377619418703934,
+            "velocityY": 0.16002444834484145,
+            "timestamp": 1.4982500246627253
+        },
+        {
+            "x": 7.42168176124644,
+            "y": 2.4968358287790196,
+            "heading": 3.0225573774767818,
+            "angularVelocity": -0.6751058321938196,
+            "velocityX": -1.7188229756119993,
+            "velocityY": 0.2681897490952097,
+            "timestamp": 1.5811974872353114
+        },
+        {
+            "x": 7.222516189064582,
+            "y": 2.528071510677467,
+            "heading": 2.9567671784015253,
+            "angularVelocity": -0.7931550530274372,
+            "velocityX": -2.401105061020643,
+            "velocityY": 0.37657187971380685,
+            "timestamp": 1.6641449498078975
+        },
+        {
+            "x": 6.966699685219415,
+            "y": 2.5683095941313905,
+            "heading": 2.8941605042172336,
+            "angularVelocity": -0.7547750376269705,
+            "velocityX": -3.0840787157449774,
+            "velocityY": 0.48510324735627736,
+            "timestamp": 1.7470924123804836
+        },
+        {
+            "x": 6.656762628445554,
+            "y": 2.6172997543511465,
+            "heading": 2.894159452148734,
+            "angularVelocity": -0.000012683552539851008,
+            "velocityX": -3.736546570097186,
+            "velocityY": 0.5906167434222167,
+            "timestamp": 1.8300398749530697
+        },
+        {
+            "x": 6.3468583527209415,
+            "y": 2.666498972584932,
+            "heading": 2.8941594322307855,
+            "angularVelocity": -2.401272821235818e-7,
+            "velocityX": -3.736151367540867,
+            "velocityY": 0.5931371100198551,
+            "timestamp": 1.9129873375256559
+        },
+        {
+            "x": 6.036954077181096,
+            "y": 2.715698191982551,
+            "heading": 2.894159412312837,
+            "angularVelocity": -2.4012728242851975e-7,
+            "velocityX": -3.736151365313363,
+            "velocityY": 0.5931371240508304,
+            "timestamp": 1.995934800098242
+        },
+        {
+            "x": 5.72704980164125,
+            "y": 2.7648974113801765,
+            "heading": 2.8941593923948883,
+            "angularVelocity": -2.401272814815578e-7,
+            "velocityX": -3.7361513653133502,
+            "velocityY": 0.5931371240509082,
+            "timestamp": 2.078882262670828
+        },
+        {
+            "x": 5.417145526101417,
+            "y": 2.8140966307778834,
+            "heading": 2.8941593724769397,
+            "angularVelocity": -2.4012728169619825e-7,
+            "velocityX": -3.7361513653131957,
+            "velocityY": 0.5931371240518842,
+            "timestamp": 2.1618297252434138
+        },
+        {
+            "x": 5.10724125286934,
+            "y": 2.8632958647117834,
+            "heading": 2.894159352558893,
+            "angularVelocity": -2.401284654616401e-7,
+            "velocityX": -3.7361513374913162,
+            "velocityY": 0.59313729929767,
+            "timestamp": 2.2447771878159997
+        },
+        {
+            "x": 4.825498631830447,
+            "y": 2.909240953390356,
+            "heading": 2.811052225227297,
+            "angularVelocity": -1.0019248902143632,
+            "velocityX": -3.3966394185035327,
+            "velocityY": 0.5539058972221842,
+            "timestamp": 2.3277246503885856
+        },
+        {
+            "x": 4.60031373082884,
+            "y": 2.9456643222027448,
+            "heading": 2.723257007302943,
+            "angularVelocity": -1.0584436847304166,
+            "velocityX": -2.7147895067260346,
+            "velocityY": 0.43911371948857386,
+            "timestamp": 2.4106721129611715
+        },
+        {
+            "x": 4.431586205140753,
+            "y": 2.9728717926897983,
+            "heading": 2.6509541339197864,
+            "angularVelocity": -0.871670707466155,
+            "velocityX": -2.0341493332654417,
+            "velocityY": 0.3280084723899037,
+            "timestamp": 2.4936195755337573
+        },
+        {
+            "x": 4.3191766366235065,
+            "y": 2.9909670138264888,
+            "heading": 2.600230923186435,
+            "angularVelocity": -0.6115100951879456,
+            "velocityX": -1.3551899603785846,
+            "velocityY": 0.21815279907876073,
+            "timestamp": 2.5765670381063432
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -0.31623539012403434,
+            "velocityX": -0.6772556372577037,
+            "velocityY": 0.10890009041091805,
+            "timestamp": 2.659514500678929
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": -4.6317137057442136e-32,
+            "velocityY": 0,
+            "timestamp": 2.742461963251515
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S3.traj
new file mode 100644
index 00000000..219e0c08
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S3.traj	
@@ -0,0 +1,319 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -2.4689012164756437e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 4.313159042990802,
+            "y": 2.9920825634147805,
+            "heading": 2.597885779471472,
+            "angularVelocity": 0.30476728851208423,
+            "velocityX": 0.6399973484188943,
+            "velocityY": -0.10102143339823635,
+            "timestamp": 0.07837382938338545
+        },
+        {
+            "x": 4.4134829053058295,
+            "y": 2.976248991975609,
+            "heading": 2.6455288467924127,
+            "angularVelocity": 0.6078951060041105,
+            "velocityX": 1.2800683991625412,
+            "velocityY": -0.20202625753700879,
+            "timestamp": 0.1567476587667709
+        },
+        {
+            "x": 4.563981380382106,
+            "y": 2.952502286743102,
+            "heading": 2.7167039498074406,
+            "angularVelocity": 0.9081488498779698,
+            "velocityX": 1.9202644079068125,
+            "velocityY": -0.3029927900593087,
+            "timestamp": 0.23512148815015638
+        },
+        {
+            "x": 4.764676150331059,
+            "y": 2.9208479584714695,
+            "heading": 2.8108575648883316,
+            "angularVelocity": 1.2013399858304654,
+            "velocityX": 2.560737066543005,
+            "velocityY": -0.40388900887800433,
+            "timestamp": 0.3134953175335418
+        },
+        {
+            "x": 5.015633164055166,
+            "y": 2.881293970992892,
+            "heading": 2.92608193331875,
+            "angularVelocity": 1.47018933918326,
+            "velocityX": 3.2020511910485845,
+            "velocityY": -0.5046836142851278,
+            "timestamp": 0.39186914691692726
+        },
+        {
+            "x": 5.308481077006102,
+            "y": 2.835005929387577,
+            "heading": 2.926081954236969,
+            "angularVelocity": 2.6690310524294676e-7,
+            "velocityX": 3.7365523064899215,
+            "velocityY": -0.5906058434235878,
+            "timestamp": 0.4702429763003127
+        },
+        {
+            "x": 5.601350966367862,
+            "y": 2.7888571291196405,
+            "heading": 2.92608197144068,
+            "angularVelocity": 2.1950836119943446e-7,
+            "velocityX": 3.7368327114541455,
+            "velocityY": -0.5888292129019572,
+            "timestamp": 0.5486168056836982
+        },
+        {
+            "x": 5.8942208558716445,
+            "y": 2.7427083297530035,
+            "heading": 2.9260819886443907,
+            "angularVelocity": 2.1950836114981294e-7,
+            "velocityX": 3.736832713266251,
+            "velocityY": -0.5888292014019535,
+            "timestamp": 0.6269906350670836
+        },
+        {
+            "x": 6.187090745375428,
+            "y": 2.6965595303863723,
+            "heading": 2.9260820058481016,
+            "angularVelocity": 2.195083613443253e-7,
+            "velocityX": 3.736832713266263,
+            "velocityY": -0.5888292014018791,
+            "timestamp": 0.7053644644504691
+        },
+        {
+            "x": 6.479960634879199,
+            "y": 2.6504107310196727,
+            "heading": 2.9260820230518125,
+            "angularVelocity": 2.1950836077924463e-7,
+            "velocityX": 3.736832713266126,
+            "velocityY": -0.5888292014027512,
+            "timestamp": 0.7837382938338545
+        },
+        {
+            "x": 6.772830522718861,
+            "y": 2.604261921092308,
+            "heading": 2.926082040255567,
+            "angularVelocity": 2.1950891675310117e-7,
+            "velocityX": 3.736832692033143,
+            "velocityY": -0.5888293361500981,
+            "timestamp": 0.86211212321724
+        },
+        {
+            "x": 7.050151670300153,
+            "y": 2.559252271073386,
+            "heading": 2.978396963955354,
+            "angularVelocity": 0.6675050091514937,
+            "velocityX": 3.5384406984211854,
+            "velocityY": -0.5742943833809976,
+            "timestamp": 0.9404859526006254
+        },
+        {
+            "x": 7.276980321576378,
+            "y": 2.5226261995289687,
+            "heading": 3.0372586376427035,
+            "angularVelocity": 0.7510373571182644,
+            "velocityX": 2.894188698712594,
+            "velocityY": -0.4673252772333,
+            "timestamp": 1.0188597819840108
+        },
+        {
+            "x": 7.453315564542908,
+            "y": 2.49415241035015,
+            "heading": 3.0866031396716647,
+            "angularVelocity": 0.6296043260507225,
+            "velocityX": 2.2499250624075064,
+            "velocityY": -0.3633073616900632,
+            "timestamp": 1.0972336113673964
+        },
+        {
+            "x": 7.579233655925443,
+            "y": 2.473748793081471,
+            "heading": 3.1215813110163633,
+            "angularVelocity": 0.446299123315424,
+            "velocityX": 1.6066344132117514,
+            "velocityY": -0.2603371231087117,
+            "timestamp": 1.175607440750782
+        },
+        {
+            "x": 7.654784082070509,
+            "y": 2.4613731780152506,
+            "heading": 3.1398442445430734,
+            "angularVelocity": 0.2330233659676447,
+            "velocityX": 0.9639751781872319,
+            "velocityY": -0.1579049430605922,
+            "timestamp": 1.2539812701341675
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 0.0019873401380948097,
+            "velocityX": 0.32173900558233287,
+            "velocityY": -0.05579895801519462,
+            "timestamp": 1.332355099517553
+        },
+        {
+            "x": 7.650333485517468,
+            "y": 2.4613165476638152,
+            "heading": 3.1188382258181035,
+            "angularVelocity": -0.255122622507773,
+            "velocityX": -0.3576542737105397,
+            "velocityY": 0.05203953840108144,
+            "timestamp": 1.4153025620901392
+        },
+        {
+            "x": 7.564253765684918,
+            "y": 2.4745901696035957,
+            "heading": 3.07855569322522,
+            "angularVelocity": -0.4856391183470325,
+            "velocityX": -1.0377619418703934,
+            "velocityY": 0.16002444834484145,
+            "timestamp": 1.4982500246627253
+        },
+        {
+            "x": 7.42168176124644,
+            "y": 2.4968358287790196,
+            "heading": 3.0225573774767818,
+            "angularVelocity": -0.6751058321938196,
+            "velocityX": -1.7188229756119993,
+            "velocityY": 0.2681897490952097,
+            "timestamp": 1.5811974872353114
+        },
+        {
+            "x": 7.222516189064582,
+            "y": 2.528071510677467,
+            "heading": 2.9567671784015253,
+            "angularVelocity": -0.7931550530274372,
+            "velocityX": -2.401105061020643,
+            "velocityY": 0.37657187971380685,
+            "timestamp": 1.6641449498078975
+        },
+        {
+            "x": 6.966699685219415,
+            "y": 2.5683095941313905,
+            "heading": 2.8941605042172336,
+            "angularVelocity": -0.7547750376269705,
+            "velocityX": -3.0840787157449774,
+            "velocityY": 0.48510324735627736,
+            "timestamp": 1.7470924123804836
+        },
+        {
+            "x": 6.656762628445554,
+            "y": 2.6172997543511465,
+            "heading": 2.894159452148734,
+            "angularVelocity": -0.000012683552539851008,
+            "velocityX": -3.736546570097186,
+            "velocityY": 0.5906167434222167,
+            "timestamp": 1.8300398749530697
+        },
+        {
+            "x": 6.3468583527209415,
+            "y": 2.666498972584932,
+            "heading": 2.8941594322307855,
+            "angularVelocity": -2.401272821235818e-7,
+            "velocityX": -3.736151367540867,
+            "velocityY": 0.5931371100198551,
+            "timestamp": 1.9129873375256559
+        },
+        {
+            "x": 6.036954077181096,
+            "y": 2.715698191982551,
+            "heading": 2.894159412312837,
+            "angularVelocity": -2.4012728242851975e-7,
+            "velocityX": -3.736151365313363,
+            "velocityY": 0.5931371240508304,
+            "timestamp": 1.995934800098242
+        },
+        {
+            "x": 5.72704980164125,
+            "y": 2.7648974113801765,
+            "heading": 2.8941593923948883,
+            "angularVelocity": -2.401272814815578e-7,
+            "velocityX": -3.7361513653133502,
+            "velocityY": 0.5931371240509082,
+            "timestamp": 2.078882262670828
+        },
+        {
+            "x": 5.417145526101417,
+            "y": 2.8140966307778834,
+            "heading": 2.8941593724769397,
+            "angularVelocity": -2.4012728169619825e-7,
+            "velocityX": -3.7361513653131957,
+            "velocityY": 0.5931371240518842,
+            "timestamp": 2.1618297252434138
+        },
+        {
+            "x": 5.10724125286934,
+            "y": 2.8632958647117834,
+            "heading": 2.894159352558893,
+            "angularVelocity": -2.401284654616401e-7,
+            "velocityX": -3.7361513374913162,
+            "velocityY": 0.59313729929767,
+            "timestamp": 2.2447771878159997
+        },
+        {
+            "x": 4.825498631830447,
+            "y": 2.909240953390356,
+            "heading": 2.811052225227297,
+            "angularVelocity": -1.0019248902143632,
+            "velocityX": -3.3966394185035327,
+            "velocityY": 0.5539058972221842,
+            "timestamp": 2.3277246503885856
+        },
+        {
+            "x": 4.60031373082884,
+            "y": 2.9456643222027448,
+            "heading": 2.723257007302943,
+            "angularVelocity": -1.0584436847304166,
+            "velocityX": -2.7147895067260346,
+            "velocityY": 0.43911371948857386,
+            "timestamp": 2.4106721129611715
+        },
+        {
+            "x": 4.431586205140753,
+            "y": 2.9728717926897983,
+            "heading": 2.6509541339197864,
+            "angularVelocity": -0.871670707466155,
+            "velocityX": -2.0341493332654417,
+            "velocityY": 0.3280084723899037,
+            "timestamp": 2.4936195755337573
+        },
+        {
+            "x": 4.3191766366235065,
+            "y": 2.9909670138264888,
+            "heading": 2.600230923186435,
+            "angularVelocity": -0.6115100951879456,
+            "velocityX": -1.3551899603785846,
+            "velocityY": 0.21815279907876073,
+            "timestamp": 2.5765670381063432
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -0.31623539012403434,
+            "velocityX": -0.6772556372577037,
+            "velocityY": 0.10890009041091805,
+            "timestamp": 2.659514500678929
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": -4.6317137057442136e-32,
+            "velocityY": 0,
+            "timestamp": 2.742461963251515
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S4.1.traj
new file mode 100644
index 00000000..1cf75a8d
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S4.1.traj	
@@ -0,0 +1,301 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.312461905031287,
+            "y": 3.0009727246573483,
+            "heading": 2.5993071037453466,
+            "angularVelocity": 0.3268807464808282,
+            "velocityX": 0.6388777080808309,
+            "velocityY": 0.012564257266019683,
+            "timestamp": 0.07741998884241831
+        },
+        {
+            "x": 4.411454270150545,
+            "y": 3.0029274539385553,
+            "heading": 2.648169844004936,
+            "angularVelocity": 0.6311385598239377,
+            "velocityX": 1.2786409117256237,
+            "velocityY": 0.025248379784522566,
+            "timestamp": 0.15483997768483662
+        },
+        {
+            "x": 4.560077070415931,
+            "y": 3.0058795423873246,
+            "heading": 2.717741930312934,
+            "angularVelocity": 0.8986320890539764,
+            "velocityX": 1.9196954492966272,
+            "velocityY": 0.03813083020172101,
+            "timestamp": 0.23225996652725495
+        },
+        {
+            "x": 4.758482787325499,
+            "y": 3.0098532742147075,
+            "heading": 2.8026323249857676,
+            "angularVelocity": 1.0964919517828993,
+            "velocityX": 2.562719523421868,
+            "velocityY": 0.05132694910962975,
+            "timestamp": 0.30967995536967324
+        },
+        {
+            "x": 5.006851286997014,
+            "y": 3.0148884552327933,
+            "heading": 2.889629605764704,
+            "angularVelocity": 1.1237056744610465,
+            "velocityX": 3.2080668492092808,
+            "velocityY": 0.06503722221317501,
+            "timestamp": 0.38709994421209154
+        },
+        {
+            "x": 5.299689191212239,
+            "y": 3.01955932024386,
+            "heading": 2.889629778554125,
+            "angularVelocity": 0.000002231845078236991,
+            "velocityX": 3.7824586207480784,
+            "velocityY": 0.060331512325246216,
+            "timestamp": 0.46451993305450984
+        },
+        {
+            "x": 5.592507772821644,
+            "y": 3.025318229635612,
+            "heading": 2.8896298031470753,
+            "angularVelocity": 3.176563362313136e-7,
+            "velocityX": 3.782209039133448,
+            "velocityY": 0.07438530381958017,
+            "timestamp": 0.5419399218969282
+        },
+        {
+            "x": 5.8853263542850165,
+            "y": 3.0310771464526267,
+            "heading": 2.889629827740025,
+            "angularVelocity": 3.176563361839336e-7,
+            "velocityX": 3.782209037247192,
+            "velocityY": 0.07438539972844682,
+            "timestamp": 0.6193599107393465
+        },
+        {
+            "x": 6.178144935748446,
+            "y": 3.0368360632666453,
+            "heading": 2.8896298523329755,
+            "angularVelocity": 3.1765633664876355e-7,
+            "velocityX": 3.782209037247953,
+            "velocityY": 0.07438539968973952,
+            "timestamp": 0.6967798995817648
+        },
+        {
+            "x": 6.4709635259990606,
+            "y": 3.0425945332689603,
+            "heading": 2.889629876925932,
+            "angularVelocity": 3.176564212988765e-7,
+            "velocityX": 3.7822091507481463,
+            "velocityY": 0.07437962841916478,
+            "timestamp": 0.7741998884241831
+        },
+        {
+            "x": 6.762585697804425,
+            "y": 3.0155317439126783,
+            "heading": 2.889629906398516,
+            "angularVelocity": 3.80684423008717e-7,
+            "velocityX": 3.766755539049949,
+            "velocityY": -0.3495581665784765,
+            "timestamp": 0.8516198772666014
+        },
+        {
+            "x": 7.037113234733603,
+            "y": 2.9421863256805914,
+            "heading": 2.9183632792459075,
+            "angularVelocity": 0.37113635996351935,
+            "velocityX": 3.5459516467763192,
+            "velocityY": -0.9473705606103254,
+            "timestamp": 0.9290398661090197
+        },
+        {
+            "x": 7.2647684874925345,
+            "y": 2.8553520162741264,
+            "heading": 2.9862391901434,
+            "angularVelocity": 0.8767233360837648,
+            "velocityX": 2.9405229342296155,
+            "velocityY": -1.1216006448051647,
+            "timestamp": 1.006459854951438
+        },
+        {
+            "x": 7.442903065756583,
+            "y": 2.761816317140275,
+            "heading": 3.0514695730897063,
+            "angularVelocity": 0.8425522132155971,
+            "velocityX": 2.3008861267937584,
+            "velocityY": -1.208159553267764,
+            "timestamp": 1.0838798437938564
+        },
+        {
+            "x": 7.571438256526523,
+            "y": 2.6636606141768233,
+            "heading": 3.1022478511869496,
+            "angularVelocity": 0.6558807209414358,
+            "velocityX": 1.660232618110589,
+            "velocityY": -1.2678341140456517,
+            "timestamp": 1.1612998326362747
+        },
+        {
+            "x": 7.650445589096316,
+            "y": 2.561865725059965,
+            "heading": 3.1328810502986992,
+            "angularVelocity": 0.39567558158786426,
+            "velocityX": 1.0205030219082787,
+            "velocityY": -1.3148398835868218,
+            "timestamp": 1.238719821478693
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 0.09195234729096653,
+            "velocityX": 0.38174134801077375,
+            "velocityY": -1.354504523029707,
+            "timestamp": 1.3161398103211113
+        },
+        {
+            "x": 7.659642368469709,
+            "y": 2.34860980319894,
+            "heading": 3.1210552256706268,
+            "angularVelocity": -0.24288242827145334,
+            "velocityX": -0.26099603478867267,
+            "velocityY": -1.3896219475702174,
+            "timestamp": 1.3941395835620152
+        },
+        {
+            "x": 7.589031675721614,
+            "y": 2.237822483491401,
+            "heading": 3.078729120037355,
+            "angularVelocity": -0.5426439574708317,
+            "velocityX": -0.9052679234080746,
+            "velocityY": -1.4203543818694149,
+            "timestamp": 1.472139356802919
+        },
+        {
+            "x": 7.468012328059872,
+            "y": 2.1251640911848773,
+            "heading": 3.017134343801584,
+            "angularVelocity": -0.7896789141365163,
+            "velocityX": -1.5515346087990811,
+            "velocityY": -1.4443425618504895,
+            "timestamp": 1.550139130043823
+        },
+        {
+            "x": 7.29639145154735,
+            "y": 2.0115501518980783,
+            "heading": 2.9431614871389193,
+            "angularVelocity": -0.948372714292366,
+            "velocityX": -2.2002740441625033,
+            "velocityY": -1.4565931997763661,
+            "timestamp": 1.6281389032847269
+        },
+        {
+            "x": 7.0740549155451475,
+            "y": 1.8989556561262921,
+            "heading": 2.8711554567434443,
+            "angularVelocity": -0.9231569196115829,
+            "velocityX": -2.8504766970989883,
+            "velocityY": -1.44352337312617,
+            "timestamp": 1.7061386765256308
+        },
+        {
+            "x": 6.804126889371604,
+            "y": 1.794205881873725,
+            "heading": 2.8575112203208874,
+            "angularVelocity": -0.17492661652254402,
+            "velocityX": -3.460625780793808,
+            "velocityY": -1.3429497279312002,
+            "timestamp": 1.7841384497665347
+        },
+        {
+            "x": 6.514819933259115,
+            "y": 1.7361808711845228,
+            "heading": 2.8575111902020094,
+            "angularVelocity": -3.8614058154759053e-7,
+            "velocityX": -3.7090743228055154,
+            "velocityY": -0.7439125561300094,
+            "timestamp": 1.8621382230074386
+        },
+        {
+            "x": 6.220884578970619,
+            "y": 1.7103463006817032,
+            "heading": 2.8575111626758107,
+            "angularVelocity": -3.52901004822605e-7,
+            "velocityX": -3.768412933466744,
+            "velocityY": -0.3312134052368198,
+            "timestamp": 1.9401379962483425
+        },
+        {
+            "x": 5.926733464556045,
+            "y": 1.687097421621547,
+            "heading": 2.857511000804422,
+            "angularVelocity": -0.000002075280251147508,
+            "velocityX": -3.771179096970985,
+            "velocityY": -0.2980634185736858,
+            "timestamp": 2.0181377694892464
+        },
+        {
+            "x": 5.675971267912864,
+            "y": 1.6645564964621176,
+            "heading": 2.7589225755051734,
+            "angularVelocity": -1.2639578450408617,
+            "velocityX": -3.214909303244982,
+            "velocityY": -0.28898706012659137,
+            "timestamp": 2.0961375427301503
+        },
+        {
+            "x": 5.475720463425886,
+            "y": 1.6466626572874457,
+            "heading": 2.662658583499747,
+            "angularVelocity": -1.2341573315618837,
+            "velocityX": -2.5673254698895036,
+            "velocityY": -0.22940886147715536,
+            "timestamp": 2.174137315971054
+        },
+        {
+            "x": 5.325756154484835,
+            "y": 1.6333048659006117,
+            "heading": 2.583901126299631,
+            "angularVelocity": -1.009713925152988,
+            "velocityX": -1.9226249347915934,
+            "velocityY": -0.17125423359344924,
+            "timestamp": 2.252137089211958
+        },
+        {
+            "x": 5.225890415283439,
+            "y": 1.6244276229064765,
+            "heading": 2.5286254494455984,
+            "angularVelocity": -0.7086645839765799,
+            "velocityX": -1.2803337119065468,
+            "velocityY": -0.11381113848520645,
+            "timestamp": 2.330136862452862
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.3669940085234364,
+            "velocityX": -0.6396225682522365,
+            "velocityY": -0.05676456126098677,
+            "timestamp": 2.408136635693766
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -3.495963808080905e-32,
+            "velocityX": 2.431919697102105e-35,
+            "velocityY": 0,
+            "timestamp": 2.48613640893467
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S4.traj
new file mode 100644
index 00000000..1cf75a8d
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S4.traj	
@@ -0,0 +1,301 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.312461905031287,
+            "y": 3.0009727246573483,
+            "heading": 2.5993071037453466,
+            "angularVelocity": 0.3268807464808282,
+            "velocityX": 0.6388777080808309,
+            "velocityY": 0.012564257266019683,
+            "timestamp": 0.07741998884241831
+        },
+        {
+            "x": 4.411454270150545,
+            "y": 3.0029274539385553,
+            "heading": 2.648169844004936,
+            "angularVelocity": 0.6311385598239377,
+            "velocityX": 1.2786409117256237,
+            "velocityY": 0.025248379784522566,
+            "timestamp": 0.15483997768483662
+        },
+        {
+            "x": 4.560077070415931,
+            "y": 3.0058795423873246,
+            "heading": 2.717741930312934,
+            "angularVelocity": 0.8986320890539764,
+            "velocityX": 1.9196954492966272,
+            "velocityY": 0.03813083020172101,
+            "timestamp": 0.23225996652725495
+        },
+        {
+            "x": 4.758482787325499,
+            "y": 3.0098532742147075,
+            "heading": 2.8026323249857676,
+            "angularVelocity": 1.0964919517828993,
+            "velocityX": 2.562719523421868,
+            "velocityY": 0.05132694910962975,
+            "timestamp": 0.30967995536967324
+        },
+        {
+            "x": 5.006851286997014,
+            "y": 3.0148884552327933,
+            "heading": 2.889629605764704,
+            "angularVelocity": 1.1237056744610465,
+            "velocityX": 3.2080668492092808,
+            "velocityY": 0.06503722221317501,
+            "timestamp": 0.38709994421209154
+        },
+        {
+            "x": 5.299689191212239,
+            "y": 3.01955932024386,
+            "heading": 2.889629778554125,
+            "angularVelocity": 0.000002231845078236991,
+            "velocityX": 3.7824586207480784,
+            "velocityY": 0.060331512325246216,
+            "timestamp": 0.46451993305450984
+        },
+        {
+            "x": 5.592507772821644,
+            "y": 3.025318229635612,
+            "heading": 2.8896298031470753,
+            "angularVelocity": 3.176563362313136e-7,
+            "velocityX": 3.782209039133448,
+            "velocityY": 0.07438530381958017,
+            "timestamp": 0.5419399218969282
+        },
+        {
+            "x": 5.8853263542850165,
+            "y": 3.0310771464526267,
+            "heading": 2.889629827740025,
+            "angularVelocity": 3.176563361839336e-7,
+            "velocityX": 3.782209037247192,
+            "velocityY": 0.07438539972844682,
+            "timestamp": 0.6193599107393465
+        },
+        {
+            "x": 6.178144935748446,
+            "y": 3.0368360632666453,
+            "heading": 2.8896298523329755,
+            "angularVelocity": 3.1765633664876355e-7,
+            "velocityX": 3.782209037247953,
+            "velocityY": 0.07438539968973952,
+            "timestamp": 0.6967798995817648
+        },
+        {
+            "x": 6.4709635259990606,
+            "y": 3.0425945332689603,
+            "heading": 2.889629876925932,
+            "angularVelocity": 3.176564212988765e-7,
+            "velocityX": 3.7822091507481463,
+            "velocityY": 0.07437962841916478,
+            "timestamp": 0.7741998884241831
+        },
+        {
+            "x": 6.762585697804425,
+            "y": 3.0155317439126783,
+            "heading": 2.889629906398516,
+            "angularVelocity": 3.80684423008717e-7,
+            "velocityX": 3.766755539049949,
+            "velocityY": -0.3495581665784765,
+            "timestamp": 0.8516198772666014
+        },
+        {
+            "x": 7.037113234733603,
+            "y": 2.9421863256805914,
+            "heading": 2.9183632792459075,
+            "angularVelocity": 0.37113635996351935,
+            "velocityX": 3.5459516467763192,
+            "velocityY": -0.9473705606103254,
+            "timestamp": 0.9290398661090197
+        },
+        {
+            "x": 7.2647684874925345,
+            "y": 2.8553520162741264,
+            "heading": 2.9862391901434,
+            "angularVelocity": 0.8767233360837648,
+            "velocityX": 2.9405229342296155,
+            "velocityY": -1.1216006448051647,
+            "timestamp": 1.006459854951438
+        },
+        {
+            "x": 7.442903065756583,
+            "y": 2.761816317140275,
+            "heading": 3.0514695730897063,
+            "angularVelocity": 0.8425522132155971,
+            "velocityX": 2.3008861267937584,
+            "velocityY": -1.208159553267764,
+            "timestamp": 1.0838798437938564
+        },
+        {
+            "x": 7.571438256526523,
+            "y": 2.6636606141768233,
+            "heading": 3.1022478511869496,
+            "angularVelocity": 0.6558807209414358,
+            "velocityX": 1.660232618110589,
+            "velocityY": -1.2678341140456517,
+            "timestamp": 1.1612998326362747
+        },
+        {
+            "x": 7.650445589096316,
+            "y": 2.561865725059965,
+            "heading": 3.1328810502986992,
+            "angularVelocity": 0.39567558158786426,
+            "velocityX": 1.0205030219082787,
+            "velocityY": -1.3148398835868218,
+            "timestamp": 1.238719821478693
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 0.09195234729096653,
+            "velocityX": 0.38174134801077375,
+            "velocityY": -1.354504523029707,
+            "timestamp": 1.3161398103211113
+        },
+        {
+            "x": 7.659642368469709,
+            "y": 2.34860980319894,
+            "heading": 3.1210552256706268,
+            "angularVelocity": -0.24288242827145334,
+            "velocityX": -0.26099603478867267,
+            "velocityY": -1.3896219475702174,
+            "timestamp": 1.3941395835620152
+        },
+        {
+            "x": 7.589031675721614,
+            "y": 2.237822483491401,
+            "heading": 3.078729120037355,
+            "angularVelocity": -0.5426439574708317,
+            "velocityX": -0.9052679234080746,
+            "velocityY": -1.4203543818694149,
+            "timestamp": 1.472139356802919
+        },
+        {
+            "x": 7.468012328059872,
+            "y": 2.1251640911848773,
+            "heading": 3.017134343801584,
+            "angularVelocity": -0.7896789141365163,
+            "velocityX": -1.5515346087990811,
+            "velocityY": -1.4443425618504895,
+            "timestamp": 1.550139130043823
+        },
+        {
+            "x": 7.29639145154735,
+            "y": 2.0115501518980783,
+            "heading": 2.9431614871389193,
+            "angularVelocity": -0.948372714292366,
+            "velocityX": -2.2002740441625033,
+            "velocityY": -1.4565931997763661,
+            "timestamp": 1.6281389032847269
+        },
+        {
+            "x": 7.0740549155451475,
+            "y": 1.8989556561262921,
+            "heading": 2.8711554567434443,
+            "angularVelocity": -0.9231569196115829,
+            "velocityX": -2.8504766970989883,
+            "velocityY": -1.44352337312617,
+            "timestamp": 1.7061386765256308
+        },
+        {
+            "x": 6.804126889371604,
+            "y": 1.794205881873725,
+            "heading": 2.8575112203208874,
+            "angularVelocity": -0.17492661652254402,
+            "velocityX": -3.460625780793808,
+            "velocityY": -1.3429497279312002,
+            "timestamp": 1.7841384497665347
+        },
+        {
+            "x": 6.514819933259115,
+            "y": 1.7361808711845228,
+            "heading": 2.8575111902020094,
+            "angularVelocity": -3.8614058154759053e-7,
+            "velocityX": -3.7090743228055154,
+            "velocityY": -0.7439125561300094,
+            "timestamp": 1.8621382230074386
+        },
+        {
+            "x": 6.220884578970619,
+            "y": 1.7103463006817032,
+            "heading": 2.8575111626758107,
+            "angularVelocity": -3.52901004822605e-7,
+            "velocityX": -3.768412933466744,
+            "velocityY": -0.3312134052368198,
+            "timestamp": 1.9401379962483425
+        },
+        {
+            "x": 5.926733464556045,
+            "y": 1.687097421621547,
+            "heading": 2.857511000804422,
+            "angularVelocity": -0.000002075280251147508,
+            "velocityX": -3.771179096970985,
+            "velocityY": -0.2980634185736858,
+            "timestamp": 2.0181377694892464
+        },
+        {
+            "x": 5.675971267912864,
+            "y": 1.6645564964621176,
+            "heading": 2.7589225755051734,
+            "angularVelocity": -1.2639578450408617,
+            "velocityX": -3.214909303244982,
+            "velocityY": -0.28898706012659137,
+            "timestamp": 2.0961375427301503
+        },
+        {
+            "x": 5.475720463425886,
+            "y": 1.6466626572874457,
+            "heading": 2.662658583499747,
+            "angularVelocity": -1.2341573315618837,
+            "velocityX": -2.5673254698895036,
+            "velocityY": -0.22940886147715536,
+            "timestamp": 2.174137315971054
+        },
+        {
+            "x": 5.325756154484835,
+            "y": 1.6333048659006117,
+            "heading": 2.583901126299631,
+            "angularVelocity": -1.009713925152988,
+            "velocityX": -1.9226249347915934,
+            "velocityY": -0.17125423359344924,
+            "timestamp": 2.252137089211958
+        },
+        {
+            "x": 5.225890415283439,
+            "y": 1.6244276229064765,
+            "heading": 2.5286254494455984,
+            "angularVelocity": -0.7086645839765799,
+            "velocityX": -1.2803337119065468,
+            "velocityY": -0.11381113848520645,
+            "timestamp": 2.330136862452862
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.3669940085234364,
+            "velocityX": -0.6396225682522365,
+            "velocityY": -0.05676456126098677,
+            "timestamp": 2.408136635693766
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -3.495963808080905e-32,
+            "velocityX": 2.431919697102105e-35,
+            "velocityY": 0,
+            "timestamp": 2.48613640893467
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S3.1.traj
new file mode 100644
index 00000000..ba2da1d0
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S3.1.traj	
@@ -0,0 +1,355 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -4.85982063591045e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 4.306024668622343,
+            "y": 2.9721182741266654,
+            "heading": 2.598597610192154,
+            "angularVelocity": 0.31231028150986523,
+            "velocityX": 0.5462744658665957,
+            "velocityY": -0.3540079539621319,
+            "timestamp": 0.0787601678472939
+        },
+        {
+            "x": 4.392095194160992,
+            "y": 2.9163526825964556,
+            "heading": 2.647262656420547,
+            "angularVelocity": 0.6178890619271524,
+            "velocityX": 1.0928179546992538,
+            "velocityY": -0.7080430762709009,
+            "timestamp": 0.1575203356945878
+        },
+        {
+            "x": 4.521245672207201,
+            "y": 2.83269821600786,
+            "heading": 2.7190885011917048,
+            "angularVelocity": 0.911956471581102,
+            "velocityX": 1.6397943475262466,
+            "velocityY": -1.0621418018152553,
+            "timestamp": 0.23628050354188168
+        },
+        {
+            "x": 4.693538421890133,
+            "y": 2.721142508781873,
+            "heading": 2.812276483563172,
+            "angularVelocity": 1.1831866909189597,
+            "velocityX": 2.1875619922114358,
+            "velocityY": -1.4163975303135559,
+            "timestamp": 0.3150406713891756
+        },
+        {
+            "x": 4.909132383686271,
+            "y": 2.581660366752094,
+            "heading": 2.9215937215973726,
+            "angularVelocity": 1.387976194339083,
+            "velocityX": 2.7373476681023576,
+            "velocityY": -1.7709731434323392,
+            "timestamp": 0.39380083923646947
+        },
+        {
+            "x": 5.159074984583097,
+            "y": 2.4194878202605934,
+            "heading": 2.9215937398089027,
+            "angularVelocity": 2.3122767087752481e-7,
+            "velocityX": 3.1734645535727406,
+            "velocityY": -2.059068066055073,
+            "timestamp": 0.47256100708376336
+        },
+        {
+            "x": 5.409017280295355,
+            "y": 2.257314803413765,
+            "heading": 2.921593758018432,
+            "angularVelocity": 2.3120226821213244e-7,
+            "velocityX": 3.173460678713437,
+            "velocityY": -2.059074038050069,
+            "timestamp": 0.5513211749310573
+        },
+        {
+            "x": 5.658959576005671,
+            "y": 2.0951417865639432,
+            "heading": 2.9215937762279616,
+            "angularVelocity": 2.312022681051112e-7,
+            "velocityX": 3.1734606786887793,
+            "velocityY": -2.0590740380880725,
+            "timestamp": 0.6300813427783512
+        },
+        {
+            "x": 5.908901871715987,
+            "y": 1.9329687697141213,
+            "heading": 2.9215937944374906,
+            "angularVelocity": 2.3120226848439114e-7,
+            "velocityX": 3.173460678688779,
+            "velocityY": -2.0590740380880725,
+            "timestamp": 0.708841510625645
+        },
+        {
+            "x": 6.158844167426302,
+            "y": 1.7707957528642995,
+            "heading": 2.92159381264702,
+            "angularVelocity": 2.3120226852070691e-7,
+            "velocityX": 3.1734606786887785,
+            "velocityY": -2.059074038088073,
+            "timestamp": 0.7876016784729389
+        },
+        {
+            "x": 6.408786463136619,
+            "y": 1.6086227360144778,
+            "heading": 2.921593830856549,
+            "angularVelocity": 2.3120226809334475e-7,
+            "velocityX": 3.173460678688779,
+            "velocityY": -2.059074038088073,
+            "timestamp": 0.8663618463202328
+        },
+        {
+            "x": 6.658728758846954,
+            "y": 1.4464497191646866,
+            "heading": 2.9215938490660784,
+            "angularVelocity": 2.31202268371391e-7,
+            "velocityX": 3.173460678689032,
+            "velocityY": -2.0590740380876835,
+            "timestamp": 0.9451220141675267
+        },
+        {
+            "x": 6.908671057688978,
+            "y": 1.2842767071415118,
+            "heading": 2.9215938672756456,
+            "angularVelocity": 2.3120274459298332e-7,
+            "velocityX": 3.173460718451369,
+            "velocityY": -2.05907397680523,
+            "timestamp": 1.0238821820148205
+        },
+        {
+            "x": 7.1455069915896265,
+            "y": 1.1314725250685531,
+            "heading": 2.963985587877118,
+            "angularVelocity": 0.5382380683048028,
+            "velocityX": 3.007052173375748,
+            "velocityY": -1.9401200663922304,
+            "timestamp": 1.1026423498621143
+        },
+        {
+            "x": 7.339036455456064,
+            "y": 1.0065280886493264,
+            "heading": 3.0237371798128563,
+            "angularVelocity": 0.7586524199845985,
+            "velocityX": 2.457199738853539,
+            "velocityY": -1.5863911902965262,
+            "timestamp": 1.181402517709408
+        },
+        {
+            "x": 7.489157100180206,
+            "y": 0.9096422022009641,
+            "heading": 3.07668127081849,
+            "angularVelocity": 0.6722191236093652,
+            "velocityX": 1.9060478009036246,
+            "velocityY": -1.2301381408456074,
+            "timestamp": 1.2601626855567019
+        },
+        {
+            "x": 7.5959791241645505,
+            "y": 0.8407792473093463,
+            "heading": 3.115718535259042,
+            "angularVelocity": 0.4956472987238782,
+            "velocityX": 1.3562950270935392,
+            "velocityY": -0.8743373303257803,
+            "timestamp": 1.3389228534039956
+        },
+        {
+            "x": 7.659576823404001,
+            "y": 0.7999065653267177,
+            "heading": 3.1374811645909735,
+            "angularVelocity": 0.2763151720819586,
+            "velocityX": 0.8074855726914214,
+            "velocityY": -0.5189511792543529,
+            "timestamp": 1.4176830212512894
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 0.03198108228905446,
+            "velocityX": 0.2593084442835197,
+            "velocityY": -0.16387173465319213,
+            "timestamp": 1.4964431890985832
+        },
+        {
+            "x": 7.653968415649204,
+            "y": 0.8042046719225767,
+            "heading": 3.120030327667123,
+            "angularVelocity": -0.24175586185815795,
+            "velocityX": -0.3151422820242312,
+            "velocityY": 0.20828235032084352,
+            "timestamp": 1.5790458287176843
+        },
+        {
+            "x": 7.58042792526076,
+            "y": 0.8521912576149779,
+            "heading": 3.0798198381250614,
+            "angularVelocity": -0.4867942444391582,
+            "velocityX": -0.8902922561258279,
+            "velocityY": 0.5809328359683593,
+            "timestamp": 1.6616484683367854
+        },
+        {
+            "x": 7.459301398894465,
+            "y": 0.9310142655562479,
+            "heading": 3.0230678620569518,
+            "angularVelocity": -0.6870479724353368,
+            "velocityX": -1.466375976903756,
+            "velocityY": 0.9542432094753368,
+            "timestamp": 1.7442511079558864
+        },
+        {
+            "x": 7.290490307507352,
+            "y": 1.0407418073187003,
+            "heading": 2.956300453743236,
+            "angularVelocity": -0.808296304108442,
+            "velocityX": -2.0436525051200767,
+            "velocityY": 1.3283781519383162,
+            "timestamp": 1.8268537475749875
+        },
+        {
+            "x": 7.073948364025268,
+            "y": 1.1814068761754821,
+            "heading": 2.8937785154835103,
+            "angularVelocity": -0.756900004019734,
+            "velocityX": -2.621489391629628,
+            "velocityY": 1.7029125159367249,
+            "timestamp": 1.9094563871940886
+        },
+        {
+            "x": 6.811621234855711,
+            "y": 1.3511947989186324,
+            "heading": 2.893774866555063,
+            "angularVelocity": -0.000044174477718030994,
+            "velocityX": -3.1757717474792755,
+            "velocityY": 2.0554781726840226,
+            "timestamp": 1.9920590268131897
+        },
+        {
+            "x": 6.5493872326198215,
+            "y": 1.5211289667970835,
+            "heading": 2.893774844652908,
+            "angularVelocity": -2.6515078170743926e-7,
+            "velocityX": -3.174644338789979,
+            "velocityY": 2.057248638325058,
+            "timestamp": 2.074661666432291
+        },
+        {
+            "x": 6.287153230916077,
+            "y": 1.691063135496712,
+            "heading": 2.893774822750754,
+            "angularVelocity": -2.651507828628349e-7,
+            "velocityX": -3.1746443323477713,
+            "velocityY": 2.057248648266354,
+            "timestamp": 2.1572643060513923
+        },
+        {
+            "x": 6.024919229212335,
+            "y": 1.860997304196345,
+            "heading": 2.893774800848599,
+            "angularVelocity": -2.651507825022106e-7,
+            "velocityX": -3.1746443323477354,
+            "velocityY": 2.0572486482664103,
+            "timestamp": 2.2398669456704936
+        },
+        {
+            "x": 5.762685227508594,
+            "y": 2.030931472895978,
+            "heading": 2.8937747789464447,
+            "angularVelocity": -2.651507826588173e-7,
+            "velocityX": -3.174644332347735,
+            "velocityY": 2.0572486482664103,
+            "timestamp": 2.322469585289595
+        },
+        {
+            "x": 5.500451225804852,
+            "y": 2.2008656415956116,
+            "heading": 2.89377475704429,
+            "angularVelocity": -2.651507831859411e-7,
+            "velocityX": -3.1746443323477345,
+            "velocityY": 2.05724864826641,
+            "timestamp": 2.405072224908696
+        },
+        {
+            "x": 5.238217224101064,
+            "y": 2.3707998102951717,
+            "heading": 2.8937747351421352,
+            "angularVelocity": -2.6515078363410014e-7,
+            "velocityX": -3.1746443323483056,
+            "velocityY": 2.057248648265529,
+            "timestamp": 2.4876748645277975
+        },
+        {
+            "x": 4.975983214041022,
+            "y": 2.5407339660996877,
+            "heading": 2.89377471323987,
+            "angularVelocity": -2.651521251593368e-7,
+            "velocityX": -3.1746444335103496,
+            "velocityY": 2.0572484921561784,
+            "timestamp": 2.570277504146899
+        },
+        {
+            "x": 4.737824648225833,
+            "y": 2.6937649564980313,
+            "heading": 2.811064183589652,
+            "angularVelocity": -1.0013061329711461,
+            "velocityX": -2.883183478317317,
+            "velocityY": 1.8526162251473437,
+            "timestamp": 2.652880143766
+        },
+        {
+            "x": 4.54767021844827,
+            "y": 2.816291569523401,
+            "heading": 2.7234816518817766,
+            "angularVelocity": -1.0602873238885226,
+            "velocityX": -2.302038175225493,
+            "velocityY": 1.48332563693325,
+            "timestamp": 2.7354827833851014
+        },
+        {
+            "x": 4.405252215667174,
+            "y": 2.908161976795727,
+            "heading": 2.651165155434109,
+            "angularVelocity": -0.875474401073941,
+            "velocityX": -1.724133798118536,
+            "velocityY": 1.1121969914758385,
+            "timestamp": 2.8180854230042027
+        },
+        {
+            "x": 4.3103959059570425,
+            "y": 2.9693921451036718,
+            "heading": 2.600323851470343,
+            "angularVelocity": -0.6154924854483996,
+            "velocityX": -1.1483447762388197,
+            "velocityY": 0.7412616423674994,
+            "timestamp": 2.900688062623304
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -0.3186805118059785,
+            "velocityX": -0.5737819781013825,
+            "velocityY": 0.3705433027015611,
+            "timestamp": 2.9832907022424053
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 5.015233372101362e-32,
+            "velocityY": -2.828100691709421e-32,
+            "timestamp": 3.0658933418615066
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S3.traj
new file mode 100644
index 00000000..ba2da1d0
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S3.traj	
@@ -0,0 +1,355 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -4.85982063591045e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 4.306024668622343,
+            "y": 2.9721182741266654,
+            "heading": 2.598597610192154,
+            "angularVelocity": 0.31231028150986523,
+            "velocityX": 0.5462744658665957,
+            "velocityY": -0.3540079539621319,
+            "timestamp": 0.0787601678472939
+        },
+        {
+            "x": 4.392095194160992,
+            "y": 2.9163526825964556,
+            "heading": 2.647262656420547,
+            "angularVelocity": 0.6178890619271524,
+            "velocityX": 1.0928179546992538,
+            "velocityY": -0.7080430762709009,
+            "timestamp": 0.1575203356945878
+        },
+        {
+            "x": 4.521245672207201,
+            "y": 2.83269821600786,
+            "heading": 2.7190885011917048,
+            "angularVelocity": 0.911956471581102,
+            "velocityX": 1.6397943475262466,
+            "velocityY": -1.0621418018152553,
+            "timestamp": 0.23628050354188168
+        },
+        {
+            "x": 4.693538421890133,
+            "y": 2.721142508781873,
+            "heading": 2.812276483563172,
+            "angularVelocity": 1.1831866909189597,
+            "velocityX": 2.1875619922114358,
+            "velocityY": -1.4163975303135559,
+            "timestamp": 0.3150406713891756
+        },
+        {
+            "x": 4.909132383686271,
+            "y": 2.581660366752094,
+            "heading": 2.9215937215973726,
+            "angularVelocity": 1.387976194339083,
+            "velocityX": 2.7373476681023576,
+            "velocityY": -1.7709731434323392,
+            "timestamp": 0.39380083923646947
+        },
+        {
+            "x": 5.159074984583097,
+            "y": 2.4194878202605934,
+            "heading": 2.9215937398089027,
+            "angularVelocity": 2.3122767087752481e-7,
+            "velocityX": 3.1734645535727406,
+            "velocityY": -2.059068066055073,
+            "timestamp": 0.47256100708376336
+        },
+        {
+            "x": 5.409017280295355,
+            "y": 2.257314803413765,
+            "heading": 2.921593758018432,
+            "angularVelocity": 2.3120226821213244e-7,
+            "velocityX": 3.173460678713437,
+            "velocityY": -2.059074038050069,
+            "timestamp": 0.5513211749310573
+        },
+        {
+            "x": 5.658959576005671,
+            "y": 2.0951417865639432,
+            "heading": 2.9215937762279616,
+            "angularVelocity": 2.312022681051112e-7,
+            "velocityX": 3.1734606786887793,
+            "velocityY": -2.0590740380880725,
+            "timestamp": 0.6300813427783512
+        },
+        {
+            "x": 5.908901871715987,
+            "y": 1.9329687697141213,
+            "heading": 2.9215937944374906,
+            "angularVelocity": 2.3120226848439114e-7,
+            "velocityX": 3.173460678688779,
+            "velocityY": -2.0590740380880725,
+            "timestamp": 0.708841510625645
+        },
+        {
+            "x": 6.158844167426302,
+            "y": 1.7707957528642995,
+            "heading": 2.92159381264702,
+            "angularVelocity": 2.3120226852070691e-7,
+            "velocityX": 3.1734606786887785,
+            "velocityY": -2.059074038088073,
+            "timestamp": 0.7876016784729389
+        },
+        {
+            "x": 6.408786463136619,
+            "y": 1.6086227360144778,
+            "heading": 2.921593830856549,
+            "angularVelocity": 2.3120226809334475e-7,
+            "velocityX": 3.173460678688779,
+            "velocityY": -2.059074038088073,
+            "timestamp": 0.8663618463202328
+        },
+        {
+            "x": 6.658728758846954,
+            "y": 1.4464497191646866,
+            "heading": 2.9215938490660784,
+            "angularVelocity": 2.31202268371391e-7,
+            "velocityX": 3.173460678689032,
+            "velocityY": -2.0590740380876835,
+            "timestamp": 0.9451220141675267
+        },
+        {
+            "x": 6.908671057688978,
+            "y": 1.2842767071415118,
+            "heading": 2.9215938672756456,
+            "angularVelocity": 2.3120274459298332e-7,
+            "velocityX": 3.173460718451369,
+            "velocityY": -2.05907397680523,
+            "timestamp": 1.0238821820148205
+        },
+        {
+            "x": 7.1455069915896265,
+            "y": 1.1314725250685531,
+            "heading": 2.963985587877118,
+            "angularVelocity": 0.5382380683048028,
+            "velocityX": 3.007052173375748,
+            "velocityY": -1.9401200663922304,
+            "timestamp": 1.1026423498621143
+        },
+        {
+            "x": 7.339036455456064,
+            "y": 1.0065280886493264,
+            "heading": 3.0237371798128563,
+            "angularVelocity": 0.7586524199845985,
+            "velocityX": 2.457199738853539,
+            "velocityY": -1.5863911902965262,
+            "timestamp": 1.181402517709408
+        },
+        {
+            "x": 7.489157100180206,
+            "y": 0.9096422022009641,
+            "heading": 3.07668127081849,
+            "angularVelocity": 0.6722191236093652,
+            "velocityX": 1.9060478009036246,
+            "velocityY": -1.2301381408456074,
+            "timestamp": 1.2601626855567019
+        },
+        {
+            "x": 7.5959791241645505,
+            "y": 0.8407792473093463,
+            "heading": 3.115718535259042,
+            "angularVelocity": 0.4956472987238782,
+            "velocityX": 1.3562950270935392,
+            "velocityY": -0.8743373303257803,
+            "timestamp": 1.3389228534039956
+        },
+        {
+            "x": 7.659576823404001,
+            "y": 0.7999065653267177,
+            "heading": 3.1374811645909735,
+            "angularVelocity": 0.2763151720819586,
+            "velocityX": 0.8074855726914214,
+            "velocityY": -0.5189511792543529,
+            "timestamp": 1.4176830212512894
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 0.03198108228905446,
+            "velocityX": 0.2593084442835197,
+            "velocityY": -0.16387173465319213,
+            "timestamp": 1.4964431890985832
+        },
+        {
+            "x": 7.653968415649204,
+            "y": 0.8042046719225767,
+            "heading": 3.120030327667123,
+            "angularVelocity": -0.24175586185815795,
+            "velocityX": -0.3151422820242312,
+            "velocityY": 0.20828235032084352,
+            "timestamp": 1.5790458287176843
+        },
+        {
+            "x": 7.58042792526076,
+            "y": 0.8521912576149779,
+            "heading": 3.0798198381250614,
+            "angularVelocity": -0.4867942444391582,
+            "velocityX": -0.8902922561258279,
+            "velocityY": 0.5809328359683593,
+            "timestamp": 1.6616484683367854
+        },
+        {
+            "x": 7.459301398894465,
+            "y": 0.9310142655562479,
+            "heading": 3.0230678620569518,
+            "angularVelocity": -0.6870479724353368,
+            "velocityX": -1.466375976903756,
+            "velocityY": 0.9542432094753368,
+            "timestamp": 1.7442511079558864
+        },
+        {
+            "x": 7.290490307507352,
+            "y": 1.0407418073187003,
+            "heading": 2.956300453743236,
+            "angularVelocity": -0.808296304108442,
+            "velocityX": -2.0436525051200767,
+            "velocityY": 1.3283781519383162,
+            "timestamp": 1.8268537475749875
+        },
+        {
+            "x": 7.073948364025268,
+            "y": 1.1814068761754821,
+            "heading": 2.8937785154835103,
+            "angularVelocity": -0.756900004019734,
+            "velocityX": -2.621489391629628,
+            "velocityY": 1.7029125159367249,
+            "timestamp": 1.9094563871940886
+        },
+        {
+            "x": 6.811621234855711,
+            "y": 1.3511947989186324,
+            "heading": 2.893774866555063,
+            "angularVelocity": -0.000044174477718030994,
+            "velocityX": -3.1757717474792755,
+            "velocityY": 2.0554781726840226,
+            "timestamp": 1.9920590268131897
+        },
+        {
+            "x": 6.5493872326198215,
+            "y": 1.5211289667970835,
+            "heading": 2.893774844652908,
+            "angularVelocity": -2.6515078170743926e-7,
+            "velocityX": -3.174644338789979,
+            "velocityY": 2.057248638325058,
+            "timestamp": 2.074661666432291
+        },
+        {
+            "x": 6.287153230916077,
+            "y": 1.691063135496712,
+            "heading": 2.893774822750754,
+            "angularVelocity": -2.651507828628349e-7,
+            "velocityX": -3.1746443323477713,
+            "velocityY": 2.057248648266354,
+            "timestamp": 2.1572643060513923
+        },
+        {
+            "x": 6.024919229212335,
+            "y": 1.860997304196345,
+            "heading": 2.893774800848599,
+            "angularVelocity": -2.651507825022106e-7,
+            "velocityX": -3.1746443323477354,
+            "velocityY": 2.0572486482664103,
+            "timestamp": 2.2398669456704936
+        },
+        {
+            "x": 5.762685227508594,
+            "y": 2.030931472895978,
+            "heading": 2.8937747789464447,
+            "angularVelocity": -2.651507826588173e-7,
+            "velocityX": -3.174644332347735,
+            "velocityY": 2.0572486482664103,
+            "timestamp": 2.322469585289595
+        },
+        {
+            "x": 5.500451225804852,
+            "y": 2.2008656415956116,
+            "heading": 2.89377475704429,
+            "angularVelocity": -2.651507831859411e-7,
+            "velocityX": -3.1746443323477345,
+            "velocityY": 2.05724864826641,
+            "timestamp": 2.405072224908696
+        },
+        {
+            "x": 5.238217224101064,
+            "y": 2.3707998102951717,
+            "heading": 2.8937747351421352,
+            "angularVelocity": -2.6515078363410014e-7,
+            "velocityX": -3.1746443323483056,
+            "velocityY": 2.057248648265529,
+            "timestamp": 2.4876748645277975
+        },
+        {
+            "x": 4.975983214041022,
+            "y": 2.5407339660996877,
+            "heading": 2.89377471323987,
+            "angularVelocity": -2.651521251593368e-7,
+            "velocityX": -3.1746444335103496,
+            "velocityY": 2.0572484921561784,
+            "timestamp": 2.570277504146899
+        },
+        {
+            "x": 4.737824648225833,
+            "y": 2.6937649564980313,
+            "heading": 2.811064183589652,
+            "angularVelocity": -1.0013061329711461,
+            "velocityX": -2.883183478317317,
+            "velocityY": 1.8526162251473437,
+            "timestamp": 2.652880143766
+        },
+        {
+            "x": 4.54767021844827,
+            "y": 2.816291569523401,
+            "heading": 2.7234816518817766,
+            "angularVelocity": -1.0602873238885226,
+            "velocityX": -2.302038175225493,
+            "velocityY": 1.48332563693325,
+            "timestamp": 2.7354827833851014
+        },
+        {
+            "x": 4.405252215667174,
+            "y": 2.908161976795727,
+            "heading": 2.651165155434109,
+            "angularVelocity": -0.875474401073941,
+            "velocityX": -1.724133798118536,
+            "velocityY": 1.1121969914758385,
+            "timestamp": 2.8180854230042027
+        },
+        {
+            "x": 4.3103959059570425,
+            "y": 2.9693921451036718,
+            "heading": 2.600323851470343,
+            "angularVelocity": -0.6154924854483996,
+            "velocityX": -1.1483447762388197,
+            "velocityY": 0.7412616423674994,
+            "timestamp": 2.900688062623304
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -0.3186805118059785,
+            "velocityX": -0.5737819781013825,
+            "velocityY": 0.3705433027015611,
+            "timestamp": 2.9832907022424053
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 5.015233372101362e-32,
+            "velocityY": -2.828100691709421e-32,
+            "timestamp": 3.0658933418615066
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S4.1.traj
new file mode 100644
index 00000000..15d6e495
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S4.1.traj	
@@ -0,0 +1,319 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.307906047793953,
+            "y": 2.9763559763722562,
+            "heading": 2.599832328454837,
+            "angularVelocity": 0.3294814998044202,
+            "velocityX": 0.5727595173353921,
+            "velocityY": -0.3015705061160393,
+            "timestamp": 0.07840297094122421
+        },
+        {
+            "x": 4.397737480320486,
+            "y": 2.929052986687658,
+            "heading": 2.6508685642334093,
+            "angularVelocity": 0.6509477276930885,
+            "velocityX": 1.1457656699499514,
+            "velocityY": -0.6033315972179728,
+            "timestamp": 0.15680594188244842
+        },
+        {
+            "x": 4.532524605763972,
+            "y": 2.858064592924414,
+            "heading": 2.7260427369404674,
+            "angularVelocity": 0.95881790963547,
+            "velocityX": 1.7191583918999656,
+            "velocityY": -0.9054298952072739,
+            "timestamp": 0.23520891282367262
+        },
+        {
+            "x": 4.712319110504002,
+            "y": 2.7633347501885597,
+            "heading": 2.823264198017844,
+            "angularVelocity": 1.2400226663643632,
+            "velocityX": 2.293210353914077,
+            "velocityY": -1.20824302444969,
+            "timestamp": 0.31361188376489685
+        },
+        {
+            "x": 4.937226049144337,
+            "y": 2.644712434074336,
+            "heading": 2.936900931840023,
+            "angularVelocity": 1.4493932112280175,
+            "velocityX": 2.8686022473427424,
+            "velocityY": -1.5129824124033164,
+            "timestamp": 0.3920148547061211
+        },
+        {
+            "x": 5.200333937862815,
+            "y": 2.5078106422289963,
+            "heading": 2.936900964883958,
+            "angularVelocity": 4.214627997065615e-7,
+            "velocityX": 3.355840799907034,
+            "velocityY": -1.7461301555518065,
+            "timestamp": 0.4704178256473453
+        },
+        {
+            "x": 5.462885368981394,
+            "y": 2.3698446646000946,
+            "heading": 2.936900984463062,
+            "angularVelocity": 2.4972400074747634e-7,
+            "velocityX": 3.3487433953925927,
+            "velocityY": -1.7597034394566977,
+            "timestamp": 0.5488207965885695
+        },
+        {
+            "x": 5.725436796470205,
+            "y": 2.2318786800636885,
+            "heading": 2.9369010040421655,
+            "angularVelocity": 2.4972400141004584e-7,
+            "velocityX": 3.3487433490962957,
+            "velocityY": -1.759703527559279,
+            "timestamp": 0.6272237675297937
+        },
+        {
+            "x": 5.987988223958993,
+            "y": 2.0939126955272376,
+            "heading": 2.936901023621269,
+            "angularVelocity": 2.497240000885312e-7,
+            "velocityX": 3.3487433490959946,
+            "velocityY": -1.7597035275598514,
+            "timestamp": 0.7056267384710179
+        },
+        {
+            "x": 6.250539651447782,
+            "y": 1.9559467109907867,
+            "heading": 2.9369010432003724,
+            "angularVelocity": 2.497240001698141e-7,
+            "velocityX": 3.348743349095995,
+            "velocityY": -1.759703527559851,
+            "timestamp": 0.784029709412242
+        },
+        {
+            "x": 6.513091078936429,
+            "y": 1.8179807264540686,
+            "heading": 2.936901062779476,
+            "angularVelocity": 2.497239999907855e-7,
+            "velocityX": 3.3487433490942036,
+            "velocityY": -1.7597035275632584,
+            "timestamp": 0.8624326803534662
+        },
+        {
+            "x": 6.775642484801246,
+            "y": 1.6800147007668695,
+            "heading": 2.9369010823585797,
+            "angularVelocity": 2.4972399921138707e-7,
+            "velocityX": 3.348743073290482,
+            "velocityY": -1.759704052421956,
+            "timestamp": 0.9408356512946904
+        },
+        {
+            "x": 7.03487156699359,
+            "y": 1.5359031200183482,
+            "heading": 2.936901102721687,
+            "angularVelocity": 2.5972367283965845e-7,
+            "velocityX": 3.3063681016204165,
+            "velocityY": -1.8380882639839122,
+            "timestamp": 1.0192386222359147
+        },
+        {
+            "x": 7.257601032453945,
+            "y": 1.356628823714497,
+            "heading": 2.964092637940216,
+            "angularVelocity": 0.34681766382323054,
+            "velocityX": 2.8408294071826354,
+            "velocityY": -2.286575293661087,
+            "timestamp": 1.0976415931771388
+        },
+        {
+            "x": 7.433803163736967,
+            "y": 1.1974021851496213,
+            "heading": 3.0191989559887835,
+            "angularVelocity": 0.7028600751601236,
+            "velocityX": 2.2473910002092454,
+            "velocityY": -2.030875063193899,
+            "timestamp": 1.176044564118363
+        },
+        {
+            "x": 7.563846036619734,
+            "y": 1.0604378478062424,
+            "heading": 3.0711979216149987,
+            "angularVelocity": 0.6632269798184063,
+            "velocityX": 1.6586472594285269,
+            "velocityY": -1.74692789953109,
+            "timestamp": 1.2544475350595872
+        },
+        {
+            "x": 7.648096830602628,
+            "y": 0.946251036791659,
+            "heading": 3.1111882105248365,
+            "angularVelocity": 0.5100608871025,
+            "velocityX": 1.0745867531729072,
+            "velocityY": -1.4564092360754384,
+            "timestamp": 1.3328505060008113
+        },
+        {
+            "x": 7.6867694062848315,
+            "y": 0.855064254512633,
+            "heading": 3.1349445229229786,
+            "angularVelocity": 0.3030027065666845,
+            "velocityX": 0.49325395731671434,
+            "velocityY": -1.1630526392597296,
+            "timestamp": 1.4112534769420355
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 0.06448068250805035,
+            "velocityX": -0.08634119604942615,
+            "velocityY": -0.8681336140135004,
+            "timestamp": 1.4896564478832597
+        },
+        {
+            "x": 7.627073807612577,
+            "y": 0.7419574324370846,
+            "heading": 3.1244199952297187,
+            "angularVelocity": -0.1970875494622002,
+            "velocityX": -0.6695179952660093,
+            "velocityY": -0.5697898937387053,
+            "timestamp": 1.568707635097908
+        },
+        {
+            "x": 7.5280560417619835,
+            "y": 0.7206319042387348,
+            "heading": 3.0899060052252425,
+            "angularVelocity": -0.4366030570890143,
+            "velocityX": -1.2525778465750235,
+            "velocityY": -0.26976860120365537,
+            "timestamp": 1.6477588223125563
+        },
+        {
+            "x": 7.382968853200293,
+            "y": 0.7232223421332327,
+            "heading": 3.03915644307554,
+            "angularVelocity": -0.6419835544263846,
+            "velocityX": -1.8353574901749392,
+            "velocityY": 0.0327691206892746,
+            "timestamp": 1.7268100095272048
+        },
+        {
+            "x": 7.191873936786201,
+            "y": 0.7500593523651382,
+            "heading": 2.9768884847472514,
+            "angularVelocity": -0.7876916276946772,
+            "velocityX": -2.417356691875479,
+            "velocityY": 0.3394890219549461,
+            "timestamp": 1.8058611967418532
+        },
+        {
+            "x": 6.955009615423507,
+            "y": 0.8017966281088876,
+            "heading": 2.913255262792046,
+            "angularVelocity": -0.8049622554368411,
+            "velocityX": -2.9963411013610584,
+            "velocityY": 0.6544781623992936,
+            "timestamp": 1.8849123839565016
+        },
+        {
+            "x": 6.67449530614552,
+            "y": 0.8800439513487449,
+            "heading": 2.8869556643049132,
+            "angularVelocity": -0.33269074651445785,
+            "velocityX": -3.5485148188383207,
+            "velocityY": 0.9898310954821422,
+            "timestamp": 1.96396357117115
+        },
+        {
+            "x": 6.4045779974291,
+            "y": 1.0087805726115764,
+            "heading": 2.886955644734896,
+            "angularVelocity": -2.475613351225265e-7,
+            "velocityX": -3.4144624290526653,
+            "velocityY": 1.6285223005349794,
+            "timestamp": 2.0430147583857985
+        },
+        {
+            "x": 6.136902414581452,
+            "y": 1.1421156905122876,
+            "heading": 2.8869556256683206,
+            "angularVelocity": -2.4119277641286314e-7,
+            "velocityX": -3.386104526435805,
+            "velocityY": 1.6866934273696874,
+            "timestamp": 2.122065945600447
+        },
+        {
+            "x": 5.869226659738861,
+            "y": 1.2754504631222496,
+            "heading": 2.8869556065980015,
+            "angularVelocity": -2.4124013985780433e-7,
+            "velocityX": -3.3861067021774174,
+            "velocityY": 1.6866890594304795,
+            "timestamp": 2.2011171328150954
+        },
+        {
+            "x": 5.637978571315548,
+            "y": 1.3902481295023497,
+            "heading": 2.7615946389842474,
+            "angularVelocity": -1.5858201758975898,
+            "velocityX": -2.9252955783626895,
+            "velocityY": 1.4521940836692195,
+            "timestamp": 2.280168320029744
+        },
+        {
+            "x": 5.453130511537557,
+            "y": 1.4821316832792537,
+            "heading": 2.658075983788866,
+            "angularVelocity": -1.309514238087925,
+            "velocityX": -2.3383337593158178,
+            "velocityY": 1.162329839872467,
+            "timestamp": 2.3592195072443922
+        },
+        {
+            "x": 5.3145458591491295,
+            "y": 1.5510603964907557,
+            "heading": 2.5794202908528,
+            "angularVelocity": -0.9949969849595353,
+            "velocityX": -1.753100203443375,
+            "velocityY": 0.8719503860752608,
+            "timestamp": 2.4382706944590407
+        },
+        {
+            "x": 5.222177020265271,
+            "y": 1.597019066974222,
+            "heading": 2.5265746527994444,
+            "angularVelocity": -0.668498980411977,
+            "velocityX": -1.1684687117102668,
+            "velocityY": 0.5813786244434977,
+            "timestamp": 2.517321881673689
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.336170192198179,
+            "velocityX": -0.5841407560381138,
+            "velocityY": 0.29070952424007707,
+            "timestamp": 2.5963730688883375
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": 3.1840213941352295e-31,
+            "velocityY": 0,
+            "timestamp": 2.675424256102986
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S4.traj
new file mode 100644
index 00000000..15d6e495
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S4.traj	
@@ -0,0 +1,319 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.307906047793953,
+            "y": 2.9763559763722562,
+            "heading": 2.599832328454837,
+            "angularVelocity": 0.3294814998044202,
+            "velocityX": 0.5727595173353921,
+            "velocityY": -0.3015705061160393,
+            "timestamp": 0.07840297094122421
+        },
+        {
+            "x": 4.397737480320486,
+            "y": 2.929052986687658,
+            "heading": 2.6508685642334093,
+            "angularVelocity": 0.6509477276930885,
+            "velocityX": 1.1457656699499514,
+            "velocityY": -0.6033315972179728,
+            "timestamp": 0.15680594188244842
+        },
+        {
+            "x": 4.532524605763972,
+            "y": 2.858064592924414,
+            "heading": 2.7260427369404674,
+            "angularVelocity": 0.95881790963547,
+            "velocityX": 1.7191583918999656,
+            "velocityY": -0.9054298952072739,
+            "timestamp": 0.23520891282367262
+        },
+        {
+            "x": 4.712319110504002,
+            "y": 2.7633347501885597,
+            "heading": 2.823264198017844,
+            "angularVelocity": 1.2400226663643632,
+            "velocityX": 2.293210353914077,
+            "velocityY": -1.20824302444969,
+            "timestamp": 0.31361188376489685
+        },
+        {
+            "x": 4.937226049144337,
+            "y": 2.644712434074336,
+            "heading": 2.936900931840023,
+            "angularVelocity": 1.4493932112280175,
+            "velocityX": 2.8686022473427424,
+            "velocityY": -1.5129824124033164,
+            "timestamp": 0.3920148547061211
+        },
+        {
+            "x": 5.200333937862815,
+            "y": 2.5078106422289963,
+            "heading": 2.936900964883958,
+            "angularVelocity": 4.214627997065615e-7,
+            "velocityX": 3.355840799907034,
+            "velocityY": -1.7461301555518065,
+            "timestamp": 0.4704178256473453
+        },
+        {
+            "x": 5.462885368981394,
+            "y": 2.3698446646000946,
+            "heading": 2.936900984463062,
+            "angularVelocity": 2.4972400074747634e-7,
+            "velocityX": 3.3487433953925927,
+            "velocityY": -1.7597034394566977,
+            "timestamp": 0.5488207965885695
+        },
+        {
+            "x": 5.725436796470205,
+            "y": 2.2318786800636885,
+            "heading": 2.9369010040421655,
+            "angularVelocity": 2.4972400141004584e-7,
+            "velocityX": 3.3487433490962957,
+            "velocityY": -1.759703527559279,
+            "timestamp": 0.6272237675297937
+        },
+        {
+            "x": 5.987988223958993,
+            "y": 2.0939126955272376,
+            "heading": 2.936901023621269,
+            "angularVelocity": 2.497240000885312e-7,
+            "velocityX": 3.3487433490959946,
+            "velocityY": -1.7597035275598514,
+            "timestamp": 0.7056267384710179
+        },
+        {
+            "x": 6.250539651447782,
+            "y": 1.9559467109907867,
+            "heading": 2.9369010432003724,
+            "angularVelocity": 2.497240001698141e-7,
+            "velocityX": 3.348743349095995,
+            "velocityY": -1.759703527559851,
+            "timestamp": 0.784029709412242
+        },
+        {
+            "x": 6.513091078936429,
+            "y": 1.8179807264540686,
+            "heading": 2.936901062779476,
+            "angularVelocity": 2.497239999907855e-7,
+            "velocityX": 3.3487433490942036,
+            "velocityY": -1.7597035275632584,
+            "timestamp": 0.8624326803534662
+        },
+        {
+            "x": 6.775642484801246,
+            "y": 1.6800147007668695,
+            "heading": 2.9369010823585797,
+            "angularVelocity": 2.4972399921138707e-7,
+            "velocityX": 3.348743073290482,
+            "velocityY": -1.759704052421956,
+            "timestamp": 0.9408356512946904
+        },
+        {
+            "x": 7.03487156699359,
+            "y": 1.5359031200183482,
+            "heading": 2.936901102721687,
+            "angularVelocity": 2.5972367283965845e-7,
+            "velocityX": 3.3063681016204165,
+            "velocityY": -1.8380882639839122,
+            "timestamp": 1.0192386222359147
+        },
+        {
+            "x": 7.257601032453945,
+            "y": 1.356628823714497,
+            "heading": 2.964092637940216,
+            "angularVelocity": 0.34681766382323054,
+            "velocityX": 2.8408294071826354,
+            "velocityY": -2.286575293661087,
+            "timestamp": 1.0976415931771388
+        },
+        {
+            "x": 7.433803163736967,
+            "y": 1.1974021851496213,
+            "heading": 3.0191989559887835,
+            "angularVelocity": 0.7028600751601236,
+            "velocityX": 2.2473910002092454,
+            "velocityY": -2.030875063193899,
+            "timestamp": 1.176044564118363
+        },
+        {
+            "x": 7.563846036619734,
+            "y": 1.0604378478062424,
+            "heading": 3.0711979216149987,
+            "angularVelocity": 0.6632269798184063,
+            "velocityX": 1.6586472594285269,
+            "velocityY": -1.74692789953109,
+            "timestamp": 1.2544475350595872
+        },
+        {
+            "x": 7.648096830602628,
+            "y": 0.946251036791659,
+            "heading": 3.1111882105248365,
+            "angularVelocity": 0.5100608871025,
+            "velocityX": 1.0745867531729072,
+            "velocityY": -1.4564092360754384,
+            "timestamp": 1.3328505060008113
+        },
+        {
+            "x": 7.6867694062848315,
+            "y": 0.855064254512633,
+            "heading": 3.1349445229229786,
+            "angularVelocity": 0.3030027065666845,
+            "velocityX": 0.49325395731671434,
+            "velocityY": -1.1630526392597296,
+            "timestamp": 1.4112534769420355
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 0.06448068250805035,
+            "velocityX": -0.08634119604942615,
+            "velocityY": -0.8681336140135004,
+            "timestamp": 1.4896564478832597
+        },
+        {
+            "x": 7.627073807612577,
+            "y": 0.7419574324370846,
+            "heading": 3.1244199952297187,
+            "angularVelocity": -0.1970875494622002,
+            "velocityX": -0.6695179952660093,
+            "velocityY": -0.5697898937387053,
+            "timestamp": 1.568707635097908
+        },
+        {
+            "x": 7.5280560417619835,
+            "y": 0.7206319042387348,
+            "heading": 3.0899060052252425,
+            "angularVelocity": -0.4366030570890143,
+            "velocityX": -1.2525778465750235,
+            "velocityY": -0.26976860120365537,
+            "timestamp": 1.6477588223125563
+        },
+        {
+            "x": 7.382968853200293,
+            "y": 0.7232223421332327,
+            "heading": 3.03915644307554,
+            "angularVelocity": -0.6419835544263846,
+            "velocityX": -1.8353574901749392,
+            "velocityY": 0.0327691206892746,
+            "timestamp": 1.7268100095272048
+        },
+        {
+            "x": 7.191873936786201,
+            "y": 0.7500593523651382,
+            "heading": 2.9768884847472514,
+            "angularVelocity": -0.7876916276946772,
+            "velocityX": -2.417356691875479,
+            "velocityY": 0.3394890219549461,
+            "timestamp": 1.8058611967418532
+        },
+        {
+            "x": 6.955009615423507,
+            "y": 0.8017966281088876,
+            "heading": 2.913255262792046,
+            "angularVelocity": -0.8049622554368411,
+            "velocityX": -2.9963411013610584,
+            "velocityY": 0.6544781623992936,
+            "timestamp": 1.8849123839565016
+        },
+        {
+            "x": 6.67449530614552,
+            "y": 0.8800439513487449,
+            "heading": 2.8869556643049132,
+            "angularVelocity": -0.33269074651445785,
+            "velocityX": -3.5485148188383207,
+            "velocityY": 0.9898310954821422,
+            "timestamp": 1.96396357117115
+        },
+        {
+            "x": 6.4045779974291,
+            "y": 1.0087805726115764,
+            "heading": 2.886955644734896,
+            "angularVelocity": -2.475613351225265e-7,
+            "velocityX": -3.4144624290526653,
+            "velocityY": 1.6285223005349794,
+            "timestamp": 2.0430147583857985
+        },
+        {
+            "x": 6.136902414581452,
+            "y": 1.1421156905122876,
+            "heading": 2.8869556256683206,
+            "angularVelocity": -2.4119277641286314e-7,
+            "velocityX": -3.386104526435805,
+            "velocityY": 1.6866934273696874,
+            "timestamp": 2.122065945600447
+        },
+        {
+            "x": 5.869226659738861,
+            "y": 1.2754504631222496,
+            "heading": 2.8869556065980015,
+            "angularVelocity": -2.4124013985780433e-7,
+            "velocityX": -3.3861067021774174,
+            "velocityY": 1.6866890594304795,
+            "timestamp": 2.2011171328150954
+        },
+        {
+            "x": 5.637978571315548,
+            "y": 1.3902481295023497,
+            "heading": 2.7615946389842474,
+            "angularVelocity": -1.5858201758975898,
+            "velocityX": -2.9252955783626895,
+            "velocityY": 1.4521940836692195,
+            "timestamp": 2.280168320029744
+        },
+        {
+            "x": 5.453130511537557,
+            "y": 1.4821316832792537,
+            "heading": 2.658075983788866,
+            "angularVelocity": -1.309514238087925,
+            "velocityX": -2.3383337593158178,
+            "velocityY": 1.162329839872467,
+            "timestamp": 2.3592195072443922
+        },
+        {
+            "x": 5.3145458591491295,
+            "y": 1.5510603964907557,
+            "heading": 2.5794202908528,
+            "angularVelocity": -0.9949969849595353,
+            "velocityX": -1.753100203443375,
+            "velocityY": 0.8719503860752608,
+            "timestamp": 2.4382706944590407
+        },
+        {
+            "x": 5.222177020265271,
+            "y": 1.597019066974222,
+            "heading": 2.5265746527994444,
+            "angularVelocity": -0.668498980411977,
+            "velocityX": -1.1684687117102668,
+            "velocityY": 0.5813786244434977,
+            "timestamp": 2.517321881673689
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.336170192198179,
+            "velocityX": -0.5841407560381138,
+            "velocityY": 0.29070952424007707,
+            "timestamp": 2.5963730688883375
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": 3.1840213941352295e-31,
+            "velocityY": 0,
+            "timestamp": 2.675424256102986
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.1.traj
new file mode 100644
index 00000000..0702b196
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.1.traj	
@@ -0,0 +1,157 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.228111844192817,
+            "y": 3.0443410125088586,
+            "heading": 2.5974492034990555,
+            "angularVelocity": 0.2841110410157656,
+            "velocityX": -0.42270562690532876,
+            "velocityY": 0.5372366368048371,
+            "timestamp": 0.08253534750081891
+        },
+        {
+            "x": 4.158301873890454,
+            "y": 3.133062319081129,
+            "heading": 2.642673247648486,
+            "angularVelocity": 0.5479354666675527,
+            "velocityX": -0.8458190631798037,
+            "velocityY": 1.0749492097478566,
+            "timestamp": 0.16507069500163782
+        },
+        {
+            "x": 4.053518280244317,
+            "y": 3.266222365985673,
+            "heading": 2.7068891131091086,
+            "angularVelocity": 0.7780407716825238,
+            "velocityX": -1.2695602165496185,
+            "velocityY": 1.6133699189092645,
+            "timestamp": 0.2476060425024567
+        },
+        {
+            "x": 3.9136724453244414,
+            "y": 3.4439114508470956,
+            "heading": 2.784478333107965,
+            "angularVelocity": 0.9400726155310244,
+            "velocityX": -1.694375066616006,
+            "velocityY": 2.152884675982738,
+            "timestamp": 0.33014139000327564
+        },
+        {
+            "x": 3.738653008239037,
+            "y": 3.6661781083911804,
+            "heading": 2.8571480043531183,
+            "angularVelocity": 0.8804672597329689,
+            "velocityX": -2.1205391675811076,
+            "velocityY": 2.6929874808109258,
+            "timestamp": 0.41267673750409456
+        },
+        {
+            "x": 3.545725477812246,
+            "y": 3.911665858291861,
+            "heading": 2.8571480229647763,
+            "angularVelocity": 2.254992345614167e-7,
+            "velocityX": -2.3375140018024174,
+            "velocityY": 2.9743347224441523,
+            "timestamp": 0.4952120850049135
+        },
+        {
+            "x": 3.352797950657087,
+            "y": 4.157153610763737,
+            "heading": 2.8571480415763673,
+            "angularVelocity": 2.2549842301439466e-7,
+            "velocityX": -2.3375139621632335,
+            "velocityY": 2.9743347535968137,
+            "timestamp": 0.5777474325057324
+        },
+        {
+            "x": 3.1598704235019452,
+            "y": 4.402641363235625,
+            "heading": 2.8571480601879577,
+            "angularVelocity": 2.2549842390160243e-7,
+            "velocityX": -2.3375139621630274,
+            "velocityY": 2.974334753596974,
+            "timestamp": 0.6602827800065513
+        },
+        {
+            "x": 2.966942896346795,
+            "y": 4.648129115707507,
+            "heading": 2.8571480787995487,
+            "angularVelocity": 2.2549842409910227e-7,
+            "velocityX": -2.337513962163132,
+            "velocityY": 2.974334753596892,
+            "timestamp": 0.7428181275073702
+        },
+        {
+            "x": 2.7740153675253203,
+            "y": 4.8936168668697935,
+            "heading": 2.857148097411205,
+            "angularVelocity": 2.2549920897733148e-7,
+            "velocityX": -2.3375139823523616,
+            "velocityY": 2.9743347377297993,
+            "timestamp": 0.8253534750081891
+        },
+        {
+            "x": 2.599116566256145,
+            "y": 5.1159598004085165,
+            "heading": 2.929233412420125,
+            "angularVelocity": 0.873387187328517,
+            "velocityX": -2.1190775415035294,
+            "velocityY": 2.693911642360469,
+            "timestamp": 0.907888822509008
+        },
+        {
+            "x": 2.459351765780957,
+            "y": 5.293700413500499,
+            "heading": 3.0069352217627348,
+            "angularVelocity": 0.9414367503794493,
+            "velocityX": -1.6933932515859442,
+            "velocityY": 2.1535089931040625,
+            "timestamp": 0.990424170009827
+        },
+        {
+            "x": 2.354631169439892,
+            "y": 5.426897486256648,
+            "heading": 3.071284016075413,
+            "angularVelocity": 0.7796513404397946,
+            "velocityX": -1.268796939881139,
+            "velocityY": 1.6138185249032515,
+            "timestamp": 1.0729595175106459
+        },
+        {
+            "x": 2.284865378797646,
+            "y": 5.515644940856661,
+            "heading": 3.116557551514127,
+            "angularVelocity": 0.5485351041656966,
+            "velocityX": -0.8452837814919688,
+            "velocityY": 1.0752660197999702,
+            "timestamp": 1.1554948650114647
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0.2840291971344776,
+            "velocityX": -0.4224296601804454,
+            "velocityY": 0.5374068261225751,
+            "timestamp": 1.2380302125122835
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 2.192310159207654e-31,
+            "velocityX": 0,
+            "velocityY": 7.442690740363628e-33,
+            "timestamp": 1.3205655600131023
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.traj
new file mode 100644
index 00000000..0702b196
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.traj	
@@ -0,0 +1,157 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.228111844192817,
+            "y": 3.0443410125088586,
+            "heading": 2.5974492034990555,
+            "angularVelocity": 0.2841110410157656,
+            "velocityX": -0.42270562690532876,
+            "velocityY": 0.5372366368048371,
+            "timestamp": 0.08253534750081891
+        },
+        {
+            "x": 4.158301873890454,
+            "y": 3.133062319081129,
+            "heading": 2.642673247648486,
+            "angularVelocity": 0.5479354666675527,
+            "velocityX": -0.8458190631798037,
+            "velocityY": 1.0749492097478566,
+            "timestamp": 0.16507069500163782
+        },
+        {
+            "x": 4.053518280244317,
+            "y": 3.266222365985673,
+            "heading": 2.7068891131091086,
+            "angularVelocity": 0.7780407716825238,
+            "velocityX": -1.2695602165496185,
+            "velocityY": 1.6133699189092645,
+            "timestamp": 0.2476060425024567
+        },
+        {
+            "x": 3.9136724453244414,
+            "y": 3.4439114508470956,
+            "heading": 2.784478333107965,
+            "angularVelocity": 0.9400726155310244,
+            "velocityX": -1.694375066616006,
+            "velocityY": 2.152884675982738,
+            "timestamp": 0.33014139000327564
+        },
+        {
+            "x": 3.738653008239037,
+            "y": 3.6661781083911804,
+            "heading": 2.8571480043531183,
+            "angularVelocity": 0.8804672597329689,
+            "velocityX": -2.1205391675811076,
+            "velocityY": 2.6929874808109258,
+            "timestamp": 0.41267673750409456
+        },
+        {
+            "x": 3.545725477812246,
+            "y": 3.911665858291861,
+            "heading": 2.8571480229647763,
+            "angularVelocity": 2.254992345614167e-7,
+            "velocityX": -2.3375140018024174,
+            "velocityY": 2.9743347224441523,
+            "timestamp": 0.4952120850049135
+        },
+        {
+            "x": 3.352797950657087,
+            "y": 4.157153610763737,
+            "heading": 2.8571480415763673,
+            "angularVelocity": 2.2549842301439466e-7,
+            "velocityX": -2.3375139621632335,
+            "velocityY": 2.9743347535968137,
+            "timestamp": 0.5777474325057324
+        },
+        {
+            "x": 3.1598704235019452,
+            "y": 4.402641363235625,
+            "heading": 2.8571480601879577,
+            "angularVelocity": 2.2549842390160243e-7,
+            "velocityX": -2.3375139621630274,
+            "velocityY": 2.974334753596974,
+            "timestamp": 0.6602827800065513
+        },
+        {
+            "x": 2.966942896346795,
+            "y": 4.648129115707507,
+            "heading": 2.8571480787995487,
+            "angularVelocity": 2.2549842409910227e-7,
+            "velocityX": -2.337513962163132,
+            "velocityY": 2.974334753596892,
+            "timestamp": 0.7428181275073702
+        },
+        {
+            "x": 2.7740153675253203,
+            "y": 4.8936168668697935,
+            "heading": 2.857148097411205,
+            "angularVelocity": 2.2549920897733148e-7,
+            "velocityX": -2.3375139823523616,
+            "velocityY": 2.9743347377297993,
+            "timestamp": 0.8253534750081891
+        },
+        {
+            "x": 2.599116566256145,
+            "y": 5.1159598004085165,
+            "heading": 2.929233412420125,
+            "angularVelocity": 0.873387187328517,
+            "velocityX": -2.1190775415035294,
+            "velocityY": 2.693911642360469,
+            "timestamp": 0.907888822509008
+        },
+        {
+            "x": 2.459351765780957,
+            "y": 5.293700413500499,
+            "heading": 3.0069352217627348,
+            "angularVelocity": 0.9414367503794493,
+            "velocityX": -1.6933932515859442,
+            "velocityY": 2.1535089931040625,
+            "timestamp": 0.990424170009827
+        },
+        {
+            "x": 2.354631169439892,
+            "y": 5.426897486256648,
+            "heading": 3.071284016075413,
+            "angularVelocity": 0.7796513404397946,
+            "velocityX": -1.268796939881139,
+            "velocityY": 1.6138185249032515,
+            "timestamp": 1.0729595175106459
+        },
+        {
+            "x": 2.284865378797646,
+            "y": 5.515644940856661,
+            "heading": 3.116557551514127,
+            "angularVelocity": 0.5485351041656966,
+            "velocityX": -0.8452837814919688,
+            "velocityY": 1.0752660197999702,
+            "timestamp": 1.1554948650114647
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0.2840291971344776,
+            "velocityX": -0.4224296601804454,
+            "velocityY": 0.5374068261225751,
+            "timestamp": 1.2380302125122835
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 2.192310159207654e-31,
+            "velocityX": 0,
+            "velocityY": 7.442690740363628e-33,
+            "timestamp": 1.3205655600131023
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.1.traj
new file mode 100644
index 00000000..a2adec3d
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.1.traj	
@@ -0,0 +1,121 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.097905783412131e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 4.206954796561596,
+            "y": 3.0388324489204006,
+            "heading": 2.584970820102585,
+            "angularVelocity": 0.12125870375097321,
+            "velocityX": -0.619458587129609,
+            "velocityY": 0.42920878982002253,
+            "timestamp": 0.0904744959596105
+        },
+        {
+            "x": 4.094846273424844,
+            "y": 3.1165082311139285,
+            "heading": 2.6050859488479765,
+            "angularVelocity": 0.2223292711613592,
+            "velocityX": -1.2391174103561666,
+            "velocityY": 0.858537882633839,
+            "timestamp": 0.180948991919221
+        },
+        {
+            "x": 3.9266509261170293,
+            "y": 3.2330407963022716,
+            "heading": 2.631256329120159,
+            "angularVelocity": 0.2892569888851994,
+            "velocityX": -1.8590360247256863,
+            "velocityY": 1.2880156330505033,
+            "timestamp": 0.2714234878788315
+        },
+        {
+            "x": 3.702348687884298,
+            "y": 3.38843810676698,
+            "heading": 2.65711164443334,
+            "angularVelocity": 0.2857746267492099,
+            "velocityX": -2.479176433686503,
+            "velocityY": 1.7175813892799228,
+            "timestamp": 0.361897983838442
+        },
+        {
+            "x": 3.4221471625561564,
+            "y": 3.582533883045857,
+            "heading": 2.6614389879992997,
+            "angularVelocity": 0.04782942994113533,
+            "velocityX": -3.0970222310299356,
+            "velocityY": 2.1453092854534943,
+            "timestamp": 0.4523724797980525
+        },
+        {
+            "x": 3.1408345803954827,
+            "y": 3.7774823101067563,
+            "heading": 2.6614389980641375,
+            "angularVelocity": 1.1124502748712501e-7,
+            "velocityX": -3.1093025628598916,
+            "velocityY": 2.154733496917496,
+            "timestamp": 0.542846975757663
+        },
+        {
+            "x": 2.8606418991642766,
+            "y": 3.9715702772190298,
+            "heading": 2.6657880248697206,
+            "angularVelocity": 0.04806909128871381,
+            "velocityX": -3.096924478654065,
+            "velocityY": 2.1452229719955302,
+            "timestamp": 0.6333214717172736
+        },
+        {
+            "x": 2.6363424534806392,
+            "y": 4.126965038945093,
+            "heading": 2.691984108325433,
+            "angularVelocity": 0.28954108202389867,
+            "velocityX": -2.479145568092125,
+            "velocityY": 1.7175532184830777,
+            "timestamp": 0.7237959676768841
+        },
+        {
+            "x": 2.46814994135131,
+            "y": 4.24349512219199,
+            "heading": 2.7185033740588933,
+            "angularVelocity": 0.2931131635737346,
+            "velocityX": -1.8590046879555269,
+            "velocityY": 1.2879882005523255,
+            "timestamp": 0.8142704636364947
+        },
+        {
+            "x": 2.3560438127968273,
+            "y": 4.3211688040728875,
+            "heading": 2.738885032189894,
+            "angularVelocity": 0.22527517743895228,
+            "velocityX": -1.2390909434248611,
+            "velocityY": 0.8585146682173574,
+            "timestamp": 0.9047449595961052
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0.12285194509475847,
+            "velocityX": -0.6194432165927374,
+            "velocityY": 0.42919494068745534,
+            "timestamp": 0.9952194555557158
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 6.588066214768088e-31,
+            "velocityX": -4.3339190939808694e-33,
+            "velocityY": -1.4815259513582048e-34,
+            "timestamp": 1.0856939515153263
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.traj
new file mode 100644
index 00000000..a2adec3d
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.traj	
@@ -0,0 +1,121 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.097905783412131e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 4.206954796561596,
+            "y": 3.0388324489204006,
+            "heading": 2.584970820102585,
+            "angularVelocity": 0.12125870375097321,
+            "velocityX": -0.619458587129609,
+            "velocityY": 0.42920878982002253,
+            "timestamp": 0.0904744959596105
+        },
+        {
+            "x": 4.094846273424844,
+            "y": 3.1165082311139285,
+            "heading": 2.6050859488479765,
+            "angularVelocity": 0.2223292711613592,
+            "velocityX": -1.2391174103561666,
+            "velocityY": 0.858537882633839,
+            "timestamp": 0.180948991919221
+        },
+        {
+            "x": 3.9266509261170293,
+            "y": 3.2330407963022716,
+            "heading": 2.631256329120159,
+            "angularVelocity": 0.2892569888851994,
+            "velocityX": -1.8590360247256863,
+            "velocityY": 1.2880156330505033,
+            "timestamp": 0.2714234878788315
+        },
+        {
+            "x": 3.702348687884298,
+            "y": 3.38843810676698,
+            "heading": 2.65711164443334,
+            "angularVelocity": 0.2857746267492099,
+            "velocityX": -2.479176433686503,
+            "velocityY": 1.7175813892799228,
+            "timestamp": 0.361897983838442
+        },
+        {
+            "x": 3.4221471625561564,
+            "y": 3.582533883045857,
+            "heading": 2.6614389879992997,
+            "angularVelocity": 0.04782942994113533,
+            "velocityX": -3.0970222310299356,
+            "velocityY": 2.1453092854534943,
+            "timestamp": 0.4523724797980525
+        },
+        {
+            "x": 3.1408345803954827,
+            "y": 3.7774823101067563,
+            "heading": 2.6614389980641375,
+            "angularVelocity": 1.1124502748712501e-7,
+            "velocityX": -3.1093025628598916,
+            "velocityY": 2.154733496917496,
+            "timestamp": 0.542846975757663
+        },
+        {
+            "x": 2.8606418991642766,
+            "y": 3.9715702772190298,
+            "heading": 2.6657880248697206,
+            "angularVelocity": 0.04806909128871381,
+            "velocityX": -3.096924478654065,
+            "velocityY": 2.1452229719955302,
+            "timestamp": 0.6333214717172736
+        },
+        {
+            "x": 2.6363424534806392,
+            "y": 4.126965038945093,
+            "heading": 2.691984108325433,
+            "angularVelocity": 0.28954108202389867,
+            "velocityX": -2.479145568092125,
+            "velocityY": 1.7175532184830777,
+            "timestamp": 0.7237959676768841
+        },
+        {
+            "x": 2.46814994135131,
+            "y": 4.24349512219199,
+            "heading": 2.7185033740588933,
+            "angularVelocity": 0.2931131635737346,
+            "velocityX": -1.8590046879555269,
+            "velocityY": 1.2879882005523255,
+            "timestamp": 0.8142704636364947
+        },
+        {
+            "x": 2.3560438127968273,
+            "y": 4.3211688040728875,
+            "heading": 2.738885032189894,
+            "angularVelocity": 0.22527517743895228,
+            "velocityX": -1.2390909434248611,
+            "velocityY": 0.8585146682173574,
+            "timestamp": 0.9047449595961052
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0.12285194509475847,
+            "velocityX": -0.6194432165927374,
+            "velocityY": 0.42919494068745534,
+            "timestamp": 0.9952194555557158
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 6.588066214768088e-31,
+            "velocityX": -4.3339190939808694e-33,
+            "velocityY": -1.4815259513582048e-34,
+            "timestamp": 1.0856939515153263
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S1.1.traj
new file mode 100644
index 00000000..a99f9c86
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S1.1.traj	
@@ -0,0 +1,319 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.205167733325116,
+            "y": 1.6385570868106298,
+            "heading": 2.543052954613419,
+            "angularVelocity": 0.6503304289456182,
+            "velocityX": 0.44058914643605,
+            "velocityY": 0.2803114985693042,
+            "timestamp": 0.0662016610283172
+        },
+        {
+            "x": 5.263930522170129,
+            "y": 1.6757412702349752,
+            "heading": 2.6242045140781594,
+            "angularVelocity": 1.225823615363673,
+            "velocityX": 0.8876331489609754,
+            "velocityY": 0.5616805204997857,
+            "timestamp": 0.1324033220566344
+        },
+        {
+            "x": 5.3527880828329195,
+            "y": 1.7316217896984443,
+            "heading": 2.7372039865474704,
+            "angularVelocity": 1.706897843863404,
+            "velocityX": 1.3422255466488056,
+            "velocityY": 0.8440954289583611,
+            "timestamp": 0.19860498308495161
+        },
+        {
+            "x": 5.472262183244267,
+            "y": 1.806361940098468,
+            "heading": 2.873328504655769,
+            "angularVelocity": 2.056210010353016,
+            "velocityX": 1.804699437378857,
+            "velocityY": 1.1289769658201285,
+            "timestamp": 0.2648066441132688
+        },
+        {
+            "x": 5.622851983789364,
+            "y": 1.9002058058145468,
+            "heading": 3.0148229359693395,
+            "angularVelocity": 2.137324488779513,
+            "velocityX": 2.2747133260099135,
+            "velocityY": 1.4175454853906315,
+            "timestamp": 0.331008305141586
+        },
+        {
+            "x": 5.803032153079139,
+            "y": 2.011379887463594,
+            "heading": 3.1112408203645807,
+            "angularVelocity": 1.456426967203298,
+            "velocityX": 2.721686533102414,
+            "velocityY": 1.6793246562416593,
+            "timestamp": 0.39720996616990323
+        },
+        {
+            "x": 6.010803138038067,
+            "y": 2.135461093514753,
+            "heading": 3.1324618293668918,
+            "angularVelocity": 0.32055100540867,
+            "velocityX": 3.138455768807129,
+            "velocityY": 1.8742914320237858,
+            "timestamp": 0.46341162719822043
+        },
+        {
+            "x": 6.223708618139815,
+            "y": 2.267331907058253,
+            "heading": 3.132461891539073,
+            "angularVelocity": 9.391332530924462e-7,
+            "velocityX": 3.2160141723739923,
+            "velocityY": 1.9919562665816168,
+            "timestamp": 0.5296132882265376
+        },
+        {
+            "x": 6.436614072470765,
+            "y": 2.3992027622087693,
+            "heading": 3.1324619537111715,
+            "angularVelocity": 9.391320033179244e-7,
+            "velocityX": 3.2160137830967814,
+            "velocityY": 1.991956895070635,
+            "timestamp": 0.5958149492548548
+        },
+        {
+            "x": 6.649519526799896,
+            "y": 2.5310736173622237,
+            "heading": 3.1324620158832706,
+            "angularVelocity": 9.391320117961031e-7,
+            "velocityX": 3.2160137830692905,
+            "velocityY": 1.991956895115017,
+            "timestamp": 0.6620166102831719
+        },
+        {
+            "x": 6.862424842072437,
+            "y": 2.6629446970222306,
+            "heading": 3.132462078055361,
+            "angularVelocity": 9.391318749443678e-7,
+            "velocityX": 3.216011682569308,
+            "velocityY": 1.991960286367572,
+            "timestamp": 0.7282182713114891
+        },
+        {
+            "x": 7.065700834244428,
+            "y": 2.8092236283210066,
+            "heading": 3.1324621404036685,
+            "angularVelocity": 9.417936977148102e-7,
+            "velocityX": 3.070557279295834,
+            "velocityY": 2.209596089080417,
+            "timestamp": 0.7944199323398062
+        },
+        {
+            "x": 7.245525316742551,
+            "y": 2.9835274116090518,
+            "heading": 3.132462207800352,
+            "angularVelocity": 0.000001018051251212501,
+            "velocityX": 2.7163137556501122,
+            "velocityY": 2.632921600162078,
+            "timestamp": 0.8606215933681234
+        },
+        {
+            "x": 7.3980692436855175,
+            "y": 3.182145135413928,
+            "heading": 3.132462290722476,
+            "angularVelocity": 0.0000012525686368484786,
+            "velocityX": 2.304231111024479,
+            "velocityY": 3.0001924531762993,
+            "timestamp": 0.9268232543964405
+        },
+        {
+            "x": 7.520084601366705,
+            "y": 3.4008478887342375,
+            "heading": 3.132462412903275,
+            "angularVelocity": 0.0000018455850946514453,
+            "velocityX": 1.843086046269626,
+            "velocityY": 3.3035840781513723,
+            "timestamp": 0.9930249154247577
+        },
+        {
+            "x": 7.608973390349284,
+            "y": 3.6349790028149913,
+            "heading": 3.1324627892694235,
+            "angularVelocity": 0.000005685146605966413,
+            "velocityX": 1.3426972616976043,
+            "velocityY": 3.536635039725298,
+            "timestamp": 1.0592265764530748
+        },
+        {
+            "x": 7.662614259939079,
+            "y": 3.8787278318222707,
+            "heading": 3.1349569914257884,
+            "angularVelocity": 0.03767582440419081,
+            "velocityX": 0.8102647087184026,
+            "velocityY": 3.6819140973379696,
+            "timestamp": 1.125428237481392
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.07617646589996442,
+            "velocityX": 0.26261788285761967,
+            "velocityY": 3.7502407692104156,
+            "timestamp": 1.1916298985097091
+        },
+        {
+            "x": 7.6496343984109,
+            "y": 4.420930599340373,
+            "heading": 3.140398780477199,
+            "angularVelocity": 0.005103020206742044,
+            "velocityX": -0.38857538706933203,
+            "velocityY": 3.7613019480289434,
+            "timestamp": 1.2697758712078666
+        },
+        {
+            "x": 7.569234672947985,
+            "y": 4.705409013581856,
+            "heading": 3.140398835230542,
+            "angularVelocity": 7.00654698073033e-7,
+            "velocityX": -1.0288402931970753,
+            "velocityY": 3.640346449333809,
+            "timestamp": 1.347921843906024
+        },
+        {
+            "x": 7.441209757564435,
+            "y": 4.9718704928955155,
+            "heading": 3.1403988601468993,
+            "angularVelocity": 3.1884377743325344e-7,
+            "velocityX": -1.6382791200012203,
+            "velocityY": 3.409791574837347,
+            "timestamp": 1.4260678166041816
+        },
+        {
+            "x": 7.269357950164967,
+            "y": 5.212409598206138,
+            "heading": 3.1403988766618367,
+            "angularVelocity": 2.1133446439744448e-7,
+            "velocityX": -2.199112781707609,
+            "velocityY": 3.07807423729561,
+            "timestamp": 1.504213789302339
+        },
+        {
+            "x": 7.058777847289789,
+            "y": 5.4198900260822835,
+            "heading": 3.140398889837091,
+            "angularVelocity": 1.685979934733241e-7,
+            "velocityX": -2.6947019226259408,
+            "velocityY": 2.6550367307797926,
+            "timestamp": 1.5823597620004966
+        },
+        {
+            "x": 6.8157174042290976,
+            "y": 5.588156866874436,
+            "heading": 3.1403989019071075,
+            "angularVelocity": 1.5445475097502255e-7,
+            "velocityX": -3.110338699084864,
+            "velocityY": 2.1532375243707835,
+            "timestamp": 1.660505734698654
+        },
+        {
+            "x": 6.566201090125382,
+            "y": 5.746693038711312,
+            "heading": 3.1403989138819113,
+            "angularVelocity": 1.5323635026193936e-7,
+            "velocityX": -3.1929516709384456,
+            "velocityY": 2.0287183889726994,
+            "timestamp": 1.7386517073968115
+        },
+        {
+            "x": 6.316684733354014,
+            "y": 5.905229143394562,
+            "heading": 3.140398925856715,
+            "angularVelocity": 1.532363481603047e-7,
+            "velocityX": -3.1929522169378273,
+            "velocityY": 2.0287175296369964,
+            "timestamp": 1.816797680094969
+        },
+        {
+            "x": 6.067168376581658,
+            "y": 6.063765248076258,
+            "heading": 3.1403989378315185,
+            "angularVelocity": 1.5323634796892713e-7,
+            "velocityX": -3.1929522169504736,
+            "velocityY": 2.028717529617091,
+            "timestamp": 1.8949436527931265
+        },
+        {
+            "x": 5.817651906877993,
+            "y": 6.222301175016583,
+            "heading": 3.1403989498077705,
+            "angularVelocity": 1.5325488550289546e-7,
+            "velocityX": -3.1929536620832875,
+            "velocityY": 2.0287152551381347,
+            "timestamp": 1.973089625491284
+        },
+        {
+            "x": 5.6037668941620185,
+            "y": 6.358199133155801,
+            "heading": 3.2279940884560396,
+            "angularVelocity": 1.1209168639620608,
+            "velocityX": -2.736993415413761,
+            "velocityY": 1.7390270214456762,
+            "timestamp": 2.0512355981894417
+        },
+        {
+            "x": 5.432659669200931,
+            "y": 6.4669188463641385,
+            "heading": 3.29806779388705,
+            "angularVelocity": 0.8967027091917401,
+            "velocityX": -2.189584684318749,
+            "velocityY": 1.3912388502514341,
+            "timestamp": 2.1293815708875994
+        },
+        {
+            "x": 5.304329710503707,
+            "y": 6.548459271235515,
+            "heading": 3.3506253655164633,
+            "angularVelocity": 0.6725563687371077,
+            "velocityX": -1.6421826265173798,
+            "velocityY": 1.0434373270437258,
+            "timestamp": 2.207527543585757
+        },
+        {
+            "x": 5.218776570774893,
+            "y": 6.602819757390003,
+            "heading": 3.3856653169356727,
+            "angularVelocity": 0.4483910073584796,
+            "velocityX": -1.0947862925612357,
+            "velocityY": 0.6956274812071462,
+            "timestamp": 2.285673516283915
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.2241956896684635,
+            "velocityX": -0.5473931579317236,
+            "velocityY": 0.34781373462440857,
+            "timestamp": 2.3638194889820725
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0,
+            "velocityX": -1.468222292761223e-31,
+            "velocityY": 0,
+            "timestamp": 2.44196546168023
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S1.traj
new file mode 100644
index 00000000..a99f9c86
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S1.traj	
@@ -0,0 +1,319 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.205167733325116,
+            "y": 1.6385570868106298,
+            "heading": 2.543052954613419,
+            "angularVelocity": 0.6503304289456182,
+            "velocityX": 0.44058914643605,
+            "velocityY": 0.2803114985693042,
+            "timestamp": 0.0662016610283172
+        },
+        {
+            "x": 5.263930522170129,
+            "y": 1.6757412702349752,
+            "heading": 2.6242045140781594,
+            "angularVelocity": 1.225823615363673,
+            "velocityX": 0.8876331489609754,
+            "velocityY": 0.5616805204997857,
+            "timestamp": 0.1324033220566344
+        },
+        {
+            "x": 5.3527880828329195,
+            "y": 1.7316217896984443,
+            "heading": 2.7372039865474704,
+            "angularVelocity": 1.706897843863404,
+            "velocityX": 1.3422255466488056,
+            "velocityY": 0.8440954289583611,
+            "timestamp": 0.19860498308495161
+        },
+        {
+            "x": 5.472262183244267,
+            "y": 1.806361940098468,
+            "heading": 2.873328504655769,
+            "angularVelocity": 2.056210010353016,
+            "velocityX": 1.804699437378857,
+            "velocityY": 1.1289769658201285,
+            "timestamp": 0.2648066441132688
+        },
+        {
+            "x": 5.622851983789364,
+            "y": 1.9002058058145468,
+            "heading": 3.0148229359693395,
+            "angularVelocity": 2.137324488779513,
+            "velocityX": 2.2747133260099135,
+            "velocityY": 1.4175454853906315,
+            "timestamp": 0.331008305141586
+        },
+        {
+            "x": 5.803032153079139,
+            "y": 2.011379887463594,
+            "heading": 3.1112408203645807,
+            "angularVelocity": 1.456426967203298,
+            "velocityX": 2.721686533102414,
+            "velocityY": 1.6793246562416593,
+            "timestamp": 0.39720996616990323
+        },
+        {
+            "x": 6.010803138038067,
+            "y": 2.135461093514753,
+            "heading": 3.1324618293668918,
+            "angularVelocity": 0.32055100540867,
+            "velocityX": 3.138455768807129,
+            "velocityY": 1.8742914320237858,
+            "timestamp": 0.46341162719822043
+        },
+        {
+            "x": 6.223708618139815,
+            "y": 2.267331907058253,
+            "heading": 3.132461891539073,
+            "angularVelocity": 9.391332530924462e-7,
+            "velocityX": 3.2160141723739923,
+            "velocityY": 1.9919562665816168,
+            "timestamp": 0.5296132882265376
+        },
+        {
+            "x": 6.436614072470765,
+            "y": 2.3992027622087693,
+            "heading": 3.1324619537111715,
+            "angularVelocity": 9.391320033179244e-7,
+            "velocityX": 3.2160137830967814,
+            "velocityY": 1.991956895070635,
+            "timestamp": 0.5958149492548548
+        },
+        {
+            "x": 6.649519526799896,
+            "y": 2.5310736173622237,
+            "heading": 3.1324620158832706,
+            "angularVelocity": 9.391320117961031e-7,
+            "velocityX": 3.2160137830692905,
+            "velocityY": 1.991956895115017,
+            "timestamp": 0.6620166102831719
+        },
+        {
+            "x": 6.862424842072437,
+            "y": 2.6629446970222306,
+            "heading": 3.132462078055361,
+            "angularVelocity": 9.391318749443678e-7,
+            "velocityX": 3.216011682569308,
+            "velocityY": 1.991960286367572,
+            "timestamp": 0.7282182713114891
+        },
+        {
+            "x": 7.065700834244428,
+            "y": 2.8092236283210066,
+            "heading": 3.1324621404036685,
+            "angularVelocity": 9.417936977148102e-7,
+            "velocityX": 3.070557279295834,
+            "velocityY": 2.209596089080417,
+            "timestamp": 0.7944199323398062
+        },
+        {
+            "x": 7.245525316742551,
+            "y": 2.9835274116090518,
+            "heading": 3.132462207800352,
+            "angularVelocity": 0.000001018051251212501,
+            "velocityX": 2.7163137556501122,
+            "velocityY": 2.632921600162078,
+            "timestamp": 0.8606215933681234
+        },
+        {
+            "x": 7.3980692436855175,
+            "y": 3.182145135413928,
+            "heading": 3.132462290722476,
+            "angularVelocity": 0.0000012525686368484786,
+            "velocityX": 2.304231111024479,
+            "velocityY": 3.0001924531762993,
+            "timestamp": 0.9268232543964405
+        },
+        {
+            "x": 7.520084601366705,
+            "y": 3.4008478887342375,
+            "heading": 3.132462412903275,
+            "angularVelocity": 0.0000018455850946514453,
+            "velocityX": 1.843086046269626,
+            "velocityY": 3.3035840781513723,
+            "timestamp": 0.9930249154247577
+        },
+        {
+            "x": 7.608973390349284,
+            "y": 3.6349790028149913,
+            "heading": 3.1324627892694235,
+            "angularVelocity": 0.000005685146605966413,
+            "velocityX": 1.3426972616976043,
+            "velocityY": 3.536635039725298,
+            "timestamp": 1.0592265764530748
+        },
+        {
+            "x": 7.662614259939079,
+            "y": 3.8787278318222707,
+            "heading": 3.1349569914257884,
+            "angularVelocity": 0.03767582440419081,
+            "velocityX": 0.8102647087184026,
+            "velocityY": 3.6819140973379696,
+            "timestamp": 1.125428237481392
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.07617646589996442,
+            "velocityX": 0.26261788285761967,
+            "velocityY": 3.7502407692104156,
+            "timestamp": 1.1916298985097091
+        },
+        {
+            "x": 7.6496343984109,
+            "y": 4.420930599340373,
+            "heading": 3.140398780477199,
+            "angularVelocity": 0.005103020206742044,
+            "velocityX": -0.38857538706933203,
+            "velocityY": 3.7613019480289434,
+            "timestamp": 1.2697758712078666
+        },
+        {
+            "x": 7.569234672947985,
+            "y": 4.705409013581856,
+            "heading": 3.140398835230542,
+            "angularVelocity": 7.00654698073033e-7,
+            "velocityX": -1.0288402931970753,
+            "velocityY": 3.640346449333809,
+            "timestamp": 1.347921843906024
+        },
+        {
+            "x": 7.441209757564435,
+            "y": 4.9718704928955155,
+            "heading": 3.1403988601468993,
+            "angularVelocity": 3.1884377743325344e-7,
+            "velocityX": -1.6382791200012203,
+            "velocityY": 3.409791574837347,
+            "timestamp": 1.4260678166041816
+        },
+        {
+            "x": 7.269357950164967,
+            "y": 5.212409598206138,
+            "heading": 3.1403988766618367,
+            "angularVelocity": 2.1133446439744448e-7,
+            "velocityX": -2.199112781707609,
+            "velocityY": 3.07807423729561,
+            "timestamp": 1.504213789302339
+        },
+        {
+            "x": 7.058777847289789,
+            "y": 5.4198900260822835,
+            "heading": 3.140398889837091,
+            "angularVelocity": 1.685979934733241e-7,
+            "velocityX": -2.6947019226259408,
+            "velocityY": 2.6550367307797926,
+            "timestamp": 1.5823597620004966
+        },
+        {
+            "x": 6.8157174042290976,
+            "y": 5.588156866874436,
+            "heading": 3.1403989019071075,
+            "angularVelocity": 1.5445475097502255e-7,
+            "velocityX": -3.110338699084864,
+            "velocityY": 2.1532375243707835,
+            "timestamp": 1.660505734698654
+        },
+        {
+            "x": 6.566201090125382,
+            "y": 5.746693038711312,
+            "heading": 3.1403989138819113,
+            "angularVelocity": 1.5323635026193936e-7,
+            "velocityX": -3.1929516709384456,
+            "velocityY": 2.0287183889726994,
+            "timestamp": 1.7386517073968115
+        },
+        {
+            "x": 6.316684733354014,
+            "y": 5.905229143394562,
+            "heading": 3.140398925856715,
+            "angularVelocity": 1.532363481603047e-7,
+            "velocityX": -3.1929522169378273,
+            "velocityY": 2.0287175296369964,
+            "timestamp": 1.816797680094969
+        },
+        {
+            "x": 6.067168376581658,
+            "y": 6.063765248076258,
+            "heading": 3.1403989378315185,
+            "angularVelocity": 1.5323634796892713e-7,
+            "velocityX": -3.1929522169504736,
+            "velocityY": 2.028717529617091,
+            "timestamp": 1.8949436527931265
+        },
+        {
+            "x": 5.817651906877993,
+            "y": 6.222301175016583,
+            "heading": 3.1403989498077705,
+            "angularVelocity": 1.5325488550289546e-7,
+            "velocityX": -3.1929536620832875,
+            "velocityY": 2.0287152551381347,
+            "timestamp": 1.973089625491284
+        },
+        {
+            "x": 5.6037668941620185,
+            "y": 6.358199133155801,
+            "heading": 3.2279940884560396,
+            "angularVelocity": 1.1209168639620608,
+            "velocityX": -2.736993415413761,
+            "velocityY": 1.7390270214456762,
+            "timestamp": 2.0512355981894417
+        },
+        {
+            "x": 5.432659669200931,
+            "y": 6.4669188463641385,
+            "heading": 3.29806779388705,
+            "angularVelocity": 0.8967027091917401,
+            "velocityX": -2.189584684318749,
+            "velocityY": 1.3912388502514341,
+            "timestamp": 2.1293815708875994
+        },
+        {
+            "x": 5.304329710503707,
+            "y": 6.548459271235515,
+            "heading": 3.3506253655164633,
+            "angularVelocity": 0.6725563687371077,
+            "velocityX": -1.6421826265173798,
+            "velocityY": 1.0434373270437258,
+            "timestamp": 2.207527543585757
+        },
+        {
+            "x": 5.218776570774893,
+            "y": 6.602819757390003,
+            "heading": 3.3856653169356727,
+            "angularVelocity": 0.4483910073584796,
+            "velocityX": -1.0947862925612357,
+            "velocityY": 0.6956274812071462,
+            "timestamp": 2.285673516283915
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.2241956896684635,
+            "velocityX": -0.5473931579317236,
+            "velocityY": 0.34781373462440857,
+            "timestamp": 2.3638194889820725
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0,
+            "velocityX": -1.468222292761223e-31,
+            "velocityY": 0,
+            "timestamp": 2.44196546168023
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S2.1.traj
new file mode 100644
index 00000000..5b390158
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S2.1.traj	
@@ -0,0 +1,310 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.210251836832851,
+            "y": 1.636837042886923,
+            "heading": 2.532387023792424,
+            "angularVelocity": 0.47220005236446794,
+            "velocityX": 0.4993888678908201,
+            "velocityY": 0.24548265329423538,
+            "timestamp": 0.06858750571977071
+        },
+        {
+            "x": 5.278911164181792,
+            "y": 1.6705273462343695,
+            "heading": 2.5947497489204117,
+            "angularVelocity": 0.909243228391831,
+            "velocityX": 1.0010471532448244,
+            "velocityY": 0.4912017574322581,
+            "timestamp": 0.13717501143954142
+        },
+        {
+            "x": 5.382181986675021,
+            "y": 1.7210991538184346,
+            "heading": 2.6836393944616295,
+            "angularVelocity": 1.2960034718917393,
+            "velocityX": 1.5056798087273409,
+            "velocityY": 0.7373326534236055,
+            "timestamp": 0.20576251715931212
+        },
+        {
+            "x": 5.520335944035756,
+            "y": 1.7886220906689136,
+            "heading": 2.793394080133565,
+            "angularVelocity": 1.6002139824177626,
+            "velocityX": 2.0142729482712705,
+            "velocityY": 0.9844786764277269,
+            "timestamp": 0.27435002287908283
+        },
+        {
+            "x": 5.693737584114165,
+            "y": 1.8732111914756295,
+            "heading": 2.9110576970550115,
+            "angularVelocity": 1.7155255273779335,
+            "velocityX": 2.5281811644657237,
+            "velocityY": 1.2333018954257229,
+            "timestamp": 0.3429375285988535
+        },
+        {
+            "x": 5.900852745156476,
+            "y": 1.9734126829741925,
+            "heading": 2.983977883284084,
+            "angularVelocity": 1.0631701133294391,
+            "velocityX": 3.0197214327712314,
+            "velocityY": 1.4609292238729052,
+            "timestamp": 0.4115250343186242
+        },
+        {
+            "x": 6.133968719369523,
+            "y": 2.087332400713016,
+            "heading": 2.9839779219365523,
+            "angularVelocity": 5.635497037494653e-7,
+            "velocityX": 3.3988110774212195,
+            "velocityY": 1.6609397957153824,
+            "timestamp": 0.4801125400383949
+        },
+        {
+            "x": 6.367084623966495,
+            "y": 2.2012522609094827,
+            "heading": 2.9839779605877594,
+            "angularVelocity": 5.635313124243683e-7,
+            "velocityX": 3.3988100624247526,
+            "velocityY": 1.660941872735705,
+            "timestamp": 0.5487000457581656
+        },
+        {
+            "x": 6.600200528555652,
+            "y": 2.315172121121942,
+            "heading": 2.9839779992389666,
+            "angularVelocity": 5.635313147177628e-7,
+            "velocityX": 3.3988100623108077,
+            "velocityY": 1.6609418729688719,
+            "timestamp": 0.6172875514779362
+        },
+        {
+            "x": 6.833315692249175,
+            "y": 2.4290934974276186,
+            "heading": 2.983978037890185,
+            "angularVelocity": 5.635314723100833e-7,
+            "velocityX": 3.398799260115481,
+            "velocityY": 1.6609639774790368,
+            "timestamp": 0.6858750571977069
+        },
+        {
+            "x": 7.049439158690477,
+            "y": 2.572660228674406,
+            "heading": 2.9839780777366456,
+            "angularVelocity": 5.809580085952453e-7,
+            "velocityX": 3.151061759328619,
+            "velocityY": 2.093190731171372,
+            "timestamp": 0.7544625629174776
+        },
+        {
+            "x": 7.241451301040205,
+            "y": 2.747165513732945,
+            "heading": 2.983978123402154,
+            "angularVelocity": 6.657992258942737e-7,
+            "velocityX": 2.7995207047510204,
+            "velocityY": 2.5442722144105754,
+            "timestamp": 0.8230500686372483
+        },
+        {
+            "x": 7.4049637163298945,
+            "y": 2.9486213241330868,
+            "heading": 2.98397818333491,
+            "angularVelocity": 8.738144839700177e-7,
+            "velocityX": 2.383997108128638,
+            "velocityY": 2.9372085817383917,
+            "timestamp": 0.891637574357019
+        },
+        {
+            "x": 7.536239247186927,
+            "y": 3.1724233234500354,
+            "heading": 2.983979044678588,
+            "angularVelocity": 0.000012558317568342283,
+            "velocityX": 1.9139860748601447,
+            "velocityY": 3.263014115593316,
+            "timestamp": 0.9602250800767896
+        },
+        {
+            "x": 7.630290050662344,
+            "y": 3.4075679677071538,
+            "heading": 3.001663168154554,
+            "angularVelocity": 0.25783301623794963,
+            "velocityX": 1.371252715613873,
+            "velocityY": 3.428388914124583,
+            "timestamp": 1.0288125857965604
+        },
+        {
+            "x": 7.68591505099028,
+            "y": 3.648060804187146,
+            "heading": 3.0401905596367595,
+            "angularVelocity": 0.5617260910408102,
+            "velocityX": 0.8110077738532104,
+            "velocityY": 3.5063651018682354,
+            "timestamp": 1.097400091516331
+        },
+        {
+            "x": 7.702477054476245,
+            "y": 3.8894159117324456,
+            "heading": 3.0910773123993565,
+            "angularVelocity": 0.7419245273403817,
+            "velocityX": 0.24147260222047945,
+            "velocityY": 3.5189369406639295,
+            "timestamp": 1.1659875972361018
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.7132886243236253,
+            "velocityX": -0.3277135425813607,
+            "velocityY": 3.463955800321066,
+            "timestamp": 1.2345751029558725
+        },
+        {
+            "x": 7.5839717767270045,
+            "y": 4.427707701571831,
+            "heading": 3.18571906595995,
+            "angularVelocity": 0.5075163501765089,
+            "velocityX": -1.0659861999835865,
+            "velocityY": 3.338083838051171,
+            "timestamp": 1.3246590301910224
+        },
+        {
+            "x": 7.4226682910577315,
+            "y": 4.711490758148744,
+            "heading": 3.2114421388245056,
+            "angularVelocity": 0.28554564231429624,
+            "velocityX": -1.7905911811350714,
+            "velocityY": 3.1502074264162947,
+            "timestamp": 1.4147429574261723
+        },
+        {
+            "x": 7.199929791587311,
+            "y": 4.9676132378837865,
+            "heading": 3.2147906679837175,
+            "angularVelocity": 0.03717121646428904,
+            "velocityX": -2.472566486682988,
+            "velocityY": 2.8431540186572435,
+            "timestamp": 1.5048268846613222
+        },
+        {
+            "x": 6.929834673893736,
+            "y": 5.175413812947385,
+            "heading": 3.214790655255866,
+            "angularVelocity": -1.4128881851850989e-7,
+            "velocityX": -2.998260910501095,
+            "velocityY": 2.306744182246487,
+            "timestamp": 1.594910811896472
+        },
+        {
+            "x": 6.624007279414812,
+            "y": 5.325753649029949,
+            "heading": 3.2147906451916457,
+            "angularVelocity": -1.1172048505786767e-7,
+            "velocityX": -3.394916317098503,
+            "velocityY": 1.6688863451759368,
+            "timestamp": 1.684994739131622
+        },
+        {
+            "x": 6.294504922277233,
+            "y": 5.412705714996136,
+            "heading": 3.2147906373147057,
+            "angularVelocity": -8.744001355884246e-8,
+            "velocityX": -3.6577263808388842,
+            "velocityY": 0.9652339616501584,
+            "timestamp": 1.775078666366772
+        },
+        {
+            "x": 5.954617378458506,
+            "y": 5.437382093793352,
+            "heading": 3.214790629982673,
+            "angularVelocity": -8.139113095546486e-8,
+            "velocityX": -3.773009839274702,
+            "velocityY": 0.273926543331081,
+            "timestamp": 1.8651625936019218
+        },
+        {
+            "x": 5.614729638884886,
+            "y": 5.46205577615271,
+            "heading": 3.214790622650651,
+            "angularVelocity": -8.139101337806881e-8,
+            "velocityX": -3.7730120123026722,
+            "velocityY": 0.273896610823274,
+            "timestamp": 1.9552465208370717
+        },
+        {
+            "x": 5.27484189928346,
+            "y": 5.486729458129014,
+            "heading": 3.214790615318626,
+            "angularVelocity": -8.139103977990715e-8,
+            "velocityX": -3.7730120126113382,
+            "velocityY": 0.27389660657108617,
+            "timestamp": 2.0453304480722214
+        },
+        {
+            "x": 4.937591000410069,
+            "y": 5.511126660727889,
+            "heading": 3.206674021661731,
+            "angularVelocity": -0.09010035314854972,
+            "velocityX": -3.7437410781731515,
+            "velocityY": 0.27082747552945735,
+            "timestamp": 2.1354143753073713
+        },
+        {
+            "x": 4.66772599134931,
+            "y": 5.5306707771302355,
+            "heading": 3.1851297651856467,
+            "angularVelocity": -0.23915760710394238,
+            "velocityX": -2.9957065299375607,
+            "velocityY": 0.21695453342447346,
+            "timestamp": 2.2254983025425212
+        },
+        {
+            "x": 4.465349331515348,
+            "y": 5.545333951302465,
+            "heading": 3.164279125670826,
+            "angularVelocity": -0.23145793211693713,
+            "velocityX": -2.246534604399402,
+            "velocityY": 0.16277236819342672,
+            "timestamp": 2.315582229777671
+        },
+        {
+            "x": 4.330445855865838,
+            "y": 5.555111001202187,
+            "heading": 3.1485187249188784,
+            "angularVelocity": -0.17495241643725573,
+            "velocityX": -1.4975310223472684,
+            "velocityY": 0.10853267835671068,
+            "timestamp": 2.405666157012821
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -0.09456431552591144,
+            "velocityX": -0.7487002169630299,
+            "velocityY": 0.054271599250453016,
+            "timestamp": 2.495750084247971
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -3.2898689544721607e-31,
+            "velocityX": 6.680323432143241e-33,
+            "velocityY": 0,
+            "timestamp": 2.585834011483121
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S2.traj
new file mode 100644
index 00000000..5b390158
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S2.traj	
@@ -0,0 +1,310 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.210251836832851,
+            "y": 1.636837042886923,
+            "heading": 2.532387023792424,
+            "angularVelocity": 0.47220005236446794,
+            "velocityX": 0.4993888678908201,
+            "velocityY": 0.24548265329423538,
+            "timestamp": 0.06858750571977071
+        },
+        {
+            "x": 5.278911164181792,
+            "y": 1.6705273462343695,
+            "heading": 2.5947497489204117,
+            "angularVelocity": 0.909243228391831,
+            "velocityX": 1.0010471532448244,
+            "velocityY": 0.4912017574322581,
+            "timestamp": 0.13717501143954142
+        },
+        {
+            "x": 5.382181986675021,
+            "y": 1.7210991538184346,
+            "heading": 2.6836393944616295,
+            "angularVelocity": 1.2960034718917393,
+            "velocityX": 1.5056798087273409,
+            "velocityY": 0.7373326534236055,
+            "timestamp": 0.20576251715931212
+        },
+        {
+            "x": 5.520335944035756,
+            "y": 1.7886220906689136,
+            "heading": 2.793394080133565,
+            "angularVelocity": 1.6002139824177626,
+            "velocityX": 2.0142729482712705,
+            "velocityY": 0.9844786764277269,
+            "timestamp": 0.27435002287908283
+        },
+        {
+            "x": 5.693737584114165,
+            "y": 1.8732111914756295,
+            "heading": 2.9110576970550115,
+            "angularVelocity": 1.7155255273779335,
+            "velocityX": 2.5281811644657237,
+            "velocityY": 1.2333018954257229,
+            "timestamp": 0.3429375285988535
+        },
+        {
+            "x": 5.900852745156476,
+            "y": 1.9734126829741925,
+            "heading": 2.983977883284084,
+            "angularVelocity": 1.0631701133294391,
+            "velocityX": 3.0197214327712314,
+            "velocityY": 1.4609292238729052,
+            "timestamp": 0.4115250343186242
+        },
+        {
+            "x": 6.133968719369523,
+            "y": 2.087332400713016,
+            "heading": 2.9839779219365523,
+            "angularVelocity": 5.635497037494653e-7,
+            "velocityX": 3.3988110774212195,
+            "velocityY": 1.6609397957153824,
+            "timestamp": 0.4801125400383949
+        },
+        {
+            "x": 6.367084623966495,
+            "y": 2.2012522609094827,
+            "heading": 2.9839779605877594,
+            "angularVelocity": 5.635313124243683e-7,
+            "velocityX": 3.3988100624247526,
+            "velocityY": 1.660941872735705,
+            "timestamp": 0.5487000457581656
+        },
+        {
+            "x": 6.600200528555652,
+            "y": 2.315172121121942,
+            "heading": 2.9839779992389666,
+            "angularVelocity": 5.635313147177628e-7,
+            "velocityX": 3.3988100623108077,
+            "velocityY": 1.6609418729688719,
+            "timestamp": 0.6172875514779362
+        },
+        {
+            "x": 6.833315692249175,
+            "y": 2.4290934974276186,
+            "heading": 2.983978037890185,
+            "angularVelocity": 5.635314723100833e-7,
+            "velocityX": 3.398799260115481,
+            "velocityY": 1.6609639774790368,
+            "timestamp": 0.6858750571977069
+        },
+        {
+            "x": 7.049439158690477,
+            "y": 2.572660228674406,
+            "heading": 2.9839780777366456,
+            "angularVelocity": 5.809580085952453e-7,
+            "velocityX": 3.151061759328619,
+            "velocityY": 2.093190731171372,
+            "timestamp": 0.7544625629174776
+        },
+        {
+            "x": 7.241451301040205,
+            "y": 2.747165513732945,
+            "heading": 2.983978123402154,
+            "angularVelocity": 6.657992258942737e-7,
+            "velocityX": 2.7995207047510204,
+            "velocityY": 2.5442722144105754,
+            "timestamp": 0.8230500686372483
+        },
+        {
+            "x": 7.4049637163298945,
+            "y": 2.9486213241330868,
+            "heading": 2.98397818333491,
+            "angularVelocity": 8.738144839700177e-7,
+            "velocityX": 2.383997108128638,
+            "velocityY": 2.9372085817383917,
+            "timestamp": 0.891637574357019
+        },
+        {
+            "x": 7.536239247186927,
+            "y": 3.1724233234500354,
+            "heading": 2.983979044678588,
+            "angularVelocity": 0.000012558317568342283,
+            "velocityX": 1.9139860748601447,
+            "velocityY": 3.263014115593316,
+            "timestamp": 0.9602250800767896
+        },
+        {
+            "x": 7.630290050662344,
+            "y": 3.4075679677071538,
+            "heading": 3.001663168154554,
+            "angularVelocity": 0.25783301623794963,
+            "velocityX": 1.371252715613873,
+            "velocityY": 3.428388914124583,
+            "timestamp": 1.0288125857965604
+        },
+        {
+            "x": 7.68591505099028,
+            "y": 3.648060804187146,
+            "heading": 3.0401905596367595,
+            "angularVelocity": 0.5617260910408102,
+            "velocityX": 0.8110077738532104,
+            "velocityY": 3.5063651018682354,
+            "timestamp": 1.097400091516331
+        },
+        {
+            "x": 7.702477054476245,
+            "y": 3.8894159117324456,
+            "heading": 3.0910773123993565,
+            "angularVelocity": 0.7419245273403817,
+            "velocityX": 0.24147260222047945,
+            "velocityY": 3.5189369406639295,
+            "timestamp": 1.1659875972361018
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.7132886243236253,
+            "velocityX": -0.3277135425813607,
+            "velocityY": 3.463955800321066,
+            "timestamp": 1.2345751029558725
+        },
+        {
+            "x": 7.5839717767270045,
+            "y": 4.427707701571831,
+            "heading": 3.18571906595995,
+            "angularVelocity": 0.5075163501765089,
+            "velocityX": -1.0659861999835865,
+            "velocityY": 3.338083838051171,
+            "timestamp": 1.3246590301910224
+        },
+        {
+            "x": 7.4226682910577315,
+            "y": 4.711490758148744,
+            "heading": 3.2114421388245056,
+            "angularVelocity": 0.28554564231429624,
+            "velocityX": -1.7905911811350714,
+            "velocityY": 3.1502074264162947,
+            "timestamp": 1.4147429574261723
+        },
+        {
+            "x": 7.199929791587311,
+            "y": 4.9676132378837865,
+            "heading": 3.2147906679837175,
+            "angularVelocity": 0.03717121646428904,
+            "velocityX": -2.472566486682988,
+            "velocityY": 2.8431540186572435,
+            "timestamp": 1.5048268846613222
+        },
+        {
+            "x": 6.929834673893736,
+            "y": 5.175413812947385,
+            "heading": 3.214790655255866,
+            "angularVelocity": -1.4128881851850989e-7,
+            "velocityX": -2.998260910501095,
+            "velocityY": 2.306744182246487,
+            "timestamp": 1.594910811896472
+        },
+        {
+            "x": 6.624007279414812,
+            "y": 5.325753649029949,
+            "heading": 3.2147906451916457,
+            "angularVelocity": -1.1172048505786767e-7,
+            "velocityX": -3.394916317098503,
+            "velocityY": 1.6688863451759368,
+            "timestamp": 1.684994739131622
+        },
+        {
+            "x": 6.294504922277233,
+            "y": 5.412705714996136,
+            "heading": 3.2147906373147057,
+            "angularVelocity": -8.744001355884246e-8,
+            "velocityX": -3.6577263808388842,
+            "velocityY": 0.9652339616501584,
+            "timestamp": 1.775078666366772
+        },
+        {
+            "x": 5.954617378458506,
+            "y": 5.437382093793352,
+            "heading": 3.214790629982673,
+            "angularVelocity": -8.139113095546486e-8,
+            "velocityX": -3.773009839274702,
+            "velocityY": 0.273926543331081,
+            "timestamp": 1.8651625936019218
+        },
+        {
+            "x": 5.614729638884886,
+            "y": 5.46205577615271,
+            "heading": 3.214790622650651,
+            "angularVelocity": -8.139101337806881e-8,
+            "velocityX": -3.7730120123026722,
+            "velocityY": 0.273896610823274,
+            "timestamp": 1.9552465208370717
+        },
+        {
+            "x": 5.27484189928346,
+            "y": 5.486729458129014,
+            "heading": 3.214790615318626,
+            "angularVelocity": -8.139103977990715e-8,
+            "velocityX": -3.7730120126113382,
+            "velocityY": 0.27389660657108617,
+            "timestamp": 2.0453304480722214
+        },
+        {
+            "x": 4.937591000410069,
+            "y": 5.511126660727889,
+            "heading": 3.206674021661731,
+            "angularVelocity": -0.09010035314854972,
+            "velocityX": -3.7437410781731515,
+            "velocityY": 0.27082747552945735,
+            "timestamp": 2.1354143753073713
+        },
+        {
+            "x": 4.66772599134931,
+            "y": 5.5306707771302355,
+            "heading": 3.1851297651856467,
+            "angularVelocity": -0.23915760710394238,
+            "velocityX": -2.9957065299375607,
+            "velocityY": 0.21695453342447346,
+            "timestamp": 2.2254983025425212
+        },
+        {
+            "x": 4.465349331515348,
+            "y": 5.545333951302465,
+            "heading": 3.164279125670826,
+            "angularVelocity": -0.23145793211693713,
+            "velocityX": -2.246534604399402,
+            "velocityY": 0.16277236819342672,
+            "timestamp": 2.315582229777671
+        },
+        {
+            "x": 4.330445855865838,
+            "y": 5.555111001202187,
+            "heading": 3.1485187249188784,
+            "angularVelocity": -0.17495241643725573,
+            "velocityX": -1.4975310223472684,
+            "velocityY": 0.10853267835671068,
+            "timestamp": 2.405666157012821
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -0.09456431552591144,
+            "velocityX": -0.7487002169630299,
+            "velocityY": 0.054271599250453016,
+            "timestamp": 2.495750084247971
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -3.2898689544721607e-31,
+            "velocityX": 6.680323432143241e-33,
+            "velocityY": 0,
+            "timestamp": 2.585834011483121
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S3.1.traj
new file mode 100644
index 00000000..06ec0a62
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S3.1.traj	
@@ -0,0 +1,328 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -5.457496407482876e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 5.213718688496592,
+            "y": 1.6454507753697194,
+            "heading": 2.523944374487045,
+            "angularVelocity": 0.3224093526452332,
+            "velocityX": 0.5078795417016481,
+            "velocityY": 0.3426929367359288,
+            "timestamp": 0.07426699719034811
+        },
+        {
+            "x": 5.289237393773012,
+            "y": 1.696398991795747,
+            "heading": 2.5693854471889703,
+            "angularVelocity": 0.6118609129363125,
+            "velocityX": 1.0168541631333754,
+            "velocityY": 0.6860142237264,
+            "timestamp": 0.14853399438069623
+        },
+        {
+            "x": 5.402661733248807,
+            "y": 1.772907618565081,
+            "heading": 2.632642220499669,
+            "angularVelocity": 0.8517480940904348,
+            "velocityX": 1.5272509158420084,
+            "velocityY": 1.0301833878275757,
+            "timestamp": 0.22280099157104433
+        },
+        {
+            "x": 5.554123710239057,
+            "y": 1.8750622586328765,
+            "heading": 2.707444867040942,
+            "angularVelocity": 1.007212481602717,
+            "velocityX": 2.039425084092856,
+            "velocityY": 1.3755051898216737,
+            "timestamp": 0.29706798876139245
+        },
+        {
+            "x": 5.743710865909146,
+            "y": 2.002930120234068,
+            "heading": 2.7801295579879124,
+            "angularVelocity": 0.9786943554574798,
+            "velocityX": 2.552777988103829,
+            "velocityY": 1.7217319460683576,
+            "timestamp": 0.3713349859517406
+        },
+        {
+            "x": 5.969240710205324,
+            "y": 2.1555656332559185,
+            "heading": 2.8014617866621823,
+            "angularVelocity": 0.2872369892591052,
+            "velocityX": 3.036743813919657,
+            "velocityY": 2.055226665899016,
+            "timestamp": 0.4456019831420887
+        },
+        {
+            "x": 6.202237157849221,
+            "y": 2.312550420967927,
+            "heading": 2.8014618141625047,
+            "angularVelocity": 3.702899476409415e-7,
+            "velocityX": 3.137281113530435,
+            "velocityY": 2.1137893499268903,
+            "timestamp": 0.5198689803324368
+        },
+        {
+            "x": 6.435233608100515,
+            "y": 2.4695352048100654,
+            "heading": 2.8014618416628014,
+            "angularVelocity": 3.7028959815407443e-7,
+            "velocityX": 3.137281148638853,
+            "velocityY": 2.1137892978193555,
+            "timestamp": 0.5941359775227849
+        },
+        {
+            "x": 6.6682300583517184,
+            "y": 2.6265199886523383,
+            "heading": 2.801461869163098,
+            "angularVelocity": 3.702895988133056e-7,
+            "velocityX": 3.137281148637628,
+            "velocityY": 2.1137892978211736,
+            "timestamp": 0.668402974713133
+        },
+        {
+            "x": 6.901226494182671,
+            "y": 2.7835047938971096,
+            "heading": 2.801461896663395,
+            "angularVelocity": 3.702895996177554e-7,
+            "velocityX": 3.137280954469964,
+            "velocityY": 2.1137895860043394,
+            "timestamp": 0.7426699719034812
+        },
+        {
+            "x": 7.132347068567706,
+            "y": 2.9432384730568093,
+            "heading": 2.801461924236189,
+            "angularVelocity": 3.712657746616385e-7,
+            "velocityX": 3.1120226093518935,
+            "velocityY": 2.1508029838650415,
+            "timestamp": 0.8169369690938293
+        },
+        {
+            "x": 7.334313444212558,
+            "y": 3.1385360610663486,
+            "heading": 2.8014619668447454,
+            "angularVelocity": 5.737212724741241e-7,
+            "velocityX": 2.7194633321070794,
+            "velocityY": 2.6296685660925183,
+            "timestamp": 0.8912039662841774
+        },
+        {
+            "x": 7.4948116486140535,
+            "y": 3.3502854712542645,
+            "heading": 2.8423857101264045,
+            "angularVelocity": 0.5510353835468871,
+            "velocityX": 2.161097263568294,
+            "velocityY": 2.8511912181557233,
+            "timestamp": 0.9654709634745255
+        },
+        {
+            "x": 7.612584040336913,
+            "y": 3.548974342070663,
+            "heading": 2.9223254670447334,
+            "angularVelocity": 1.0763833188709784,
+            "velocityX": 1.5857971397578559,
+            "velocityY": 2.675331955419639,
+            "timestamp": 1.0397379606648736
+        },
+        {
+            "x": 7.689064457271449,
+            "y": 3.727414986995987,
+            "heading": 3.0016093061540334,
+            "angularVelocity": 1.0675514307666671,
+            "velocityX": 1.0298035443457485,
+            "velocityY": 2.402690988945958,
+            "timestamp": 1.1140049578552216
+        },
+        {
+            "x": 7.725394752128812,
+            "y": 3.8837274225386396,
+            "heading": 3.06785364542737,
+            "angularVelocity": 0.8919754639271454,
+            "velocityX": 0.4891849170129639,
+            "velocityY": 2.1047361742931416,
+            "timestamp": 1.1882719550455696
+        },
+        {
+            "x": 7.722234181622283,
+            "y": 4.017077206893078,
+            "heading": 3.1151383358166123,
+            "angularVelocity": 0.6366850980665145,
+            "velocityX": -0.04255686409979938,
+            "velocityY": 1.7955456582236569,
+            "timestamp": 1.2625389522359176
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.33476059520309653,
+            "velocityX": -0.5686803455111581,
+            "velocityY": 1.4801028352497836,
+            "timestamp": 1.3368059494262656
+        },
+        {
+            "x": 7.583164521123183,
+            "y": 4.220739525524416,
+            "heading": 3.1367175382385155,
+            "angularVelocity": -0.03922526988843187,
+            "velocityX": -1.1571796016903517,
+            "velocityY": 1.1201830988719972,
+            "timestamp": 1.4204882738773397
+        },
+        {
+            "x": 7.437331010349068,
+            "y": 4.2837766667768715,
+            "heading": 3.1044919064382244,
+            "angularVelocity": -0.38509484543694944,
+            "velocityX": -1.742703871226464,
+            "velocityY": 0.753290992643391,
+            "timestamp": 1.5041705983284137
+        },
+        {
+            "x": 7.242933099071244,
+            "y": 4.315173048252524,
+            "heading": 3.047144336604056,
+            "angularVelocity": -0.6853008710064864,
+            "velocityX": -2.3230462651820925,
+            "velocityY": 0.3751853414876064,
+            "timestamp": 1.5878529227794878
+        },
+        {
+            "x": 7.00089933731331,
+            "y": 4.3131727597182845,
+            "heading": 2.9720298467353206,
+            "angularVelocity": -0.8976147634695787,
+            "velocityX": -2.8922925282679244,
+            "velocityY": -0.023903357696631062,
+            "timestamp": 1.6715352472305618
+        },
+        {
+            "x": 6.7143889772576815,
+            "y": 4.273350582816976,
+            "heading": 2.8998851691901093,
+            "angularVelocity": -0.8621256402526404,
+            "velocityX": -3.4237858703738695,
+            "velocityY": -0.47587321650692566,
+            "timestamp": 1.7552175716816358
+        },
+        {
+            "x": 6.411635278119781,
+            "y": 4.1808641012010535,
+            "heading": 2.8998851130559236,
+            "angularVelocity": -6.708009886588717e-7,
+            "velocityX": -3.6178930392272837,
+            "velocityY": -1.1052092807244585,
+            "timestamp": 1.8388998961327099
+        },
+        {
+            "x": 6.131017053711007,
+            "y": 4.034347987511598,
+            "heading": 2.8998850873919646,
+            "angularVelocity": -3.066831498282175e-7,
+            "velocityX": -3.3533751153487676,
+            "velocityY": -1.7508609452539547,
+            "timestamp": 1.922582220583784
+        },
+        {
+            "x": 5.854044563819553,
+            "y": 3.8810515447858025,
+            "heading": 2.8998850618676295,
+            "angularVelocity": -3.050146543457835e-7,
+            "velocityX": -3.3098087524252473,
+            "velocityY": -1.8318855711927797,
+            "timestamp": 2.006264545034858
+        },
+        {
+            "x": 5.577072094171527,
+            "y": 3.7277550654846525,
+            "heading": 2.8998850363432944,
+            "angularVelocity": -3.0501465285690934e-7,
+            "velocityX": -3.3098085105171973,
+            "velocityY": -1.8318860082666197,
+            "timestamp": 2.089946869485932
+        },
+        {
+            "x": 5.300099624523611,
+            "y": 3.5744585861833027,
+            "heading": 2.8998850108189593,
+            "angularVelocity": -3.050146519273835e-7,
+            "velocityX": -3.3098085105158774,
+            "velocityY": -1.8318860082690038,
+            "timestamp": 2.173629193937006
+        },
+        {
+            "x": 5.0231271554381145,
+            "y": 3.421162105865856,
+            "heading": 2.899884985294552,
+            "angularVelocity": -3.050155161997765e-7,
+            "velocityX": -3.3098085037949927,
+            "velocityY": -1.831886020411314,
+            "timestamp": 2.25731151838808
+        },
+        {
+            "x": 4.7695419048400876,
+            "y": 3.2806617941163765,
+            "heading": 2.834618611781297,
+            "angularVelocity": -0.7799302175385181,
+            "velocityX": -3.030332298504562,
+            "velocityY": -1.678972383608021,
+            "timestamp": 2.340993842839154
+        },
+        {
+            "x": 4.566605904742026,
+            "y": 3.1682378317487854,
+            "heading": 2.7427589462823403,
+            "angularVelocity": -1.0977188564194784,
+            "velocityX": -2.4250760411980474,
+            "velocityY": -1.3434612757837647,
+            "timestamp": 2.424676167290228
+        },
+        {
+            "x": 4.414672096834118,
+            "y": 3.08405727220976,
+            "heading": 2.6624278185804466,
+            "angularVelocity": -0.959953350111109,
+            "velocityX": -1.8156021466246228,
+            "velocityY": -1.0059538868120508,
+            "timestamp": 2.508358491741302
+        },
+        {
+            "x": 4.313522085490217,
+            "y": 3.0280030162190834,
+            "heading": 2.6044623353701315,
+            "angularVelocity": -0.6926849079605295,
+            "velocityX": -1.2087380699258492,
+            "velocityY": -0.6698458289533948,
+            "timestamp": 2.5920408161923763
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -0.3640235326869004,
+            "velocityX": -0.6037366411798936,
+            "velocityY": -0.3346347798387892,
+            "timestamp": 2.6757231406434503
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 3.2974830734555563e-32,
+            "velocityY": 2.8302357269459625e-32,
+            "timestamp": 2.7594054650945243
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S3.traj
new file mode 100644
index 00000000..06ec0a62
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S3.traj	
@@ -0,0 +1,328 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -5.457496407482876e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 5.213718688496592,
+            "y": 1.6454507753697194,
+            "heading": 2.523944374487045,
+            "angularVelocity": 0.3224093526452332,
+            "velocityX": 0.5078795417016481,
+            "velocityY": 0.3426929367359288,
+            "timestamp": 0.07426699719034811
+        },
+        {
+            "x": 5.289237393773012,
+            "y": 1.696398991795747,
+            "heading": 2.5693854471889703,
+            "angularVelocity": 0.6118609129363125,
+            "velocityX": 1.0168541631333754,
+            "velocityY": 0.6860142237264,
+            "timestamp": 0.14853399438069623
+        },
+        {
+            "x": 5.402661733248807,
+            "y": 1.772907618565081,
+            "heading": 2.632642220499669,
+            "angularVelocity": 0.8517480940904348,
+            "velocityX": 1.5272509158420084,
+            "velocityY": 1.0301833878275757,
+            "timestamp": 0.22280099157104433
+        },
+        {
+            "x": 5.554123710239057,
+            "y": 1.8750622586328765,
+            "heading": 2.707444867040942,
+            "angularVelocity": 1.007212481602717,
+            "velocityX": 2.039425084092856,
+            "velocityY": 1.3755051898216737,
+            "timestamp": 0.29706798876139245
+        },
+        {
+            "x": 5.743710865909146,
+            "y": 2.002930120234068,
+            "heading": 2.7801295579879124,
+            "angularVelocity": 0.9786943554574798,
+            "velocityX": 2.552777988103829,
+            "velocityY": 1.7217319460683576,
+            "timestamp": 0.3713349859517406
+        },
+        {
+            "x": 5.969240710205324,
+            "y": 2.1555656332559185,
+            "heading": 2.8014617866621823,
+            "angularVelocity": 0.2872369892591052,
+            "velocityX": 3.036743813919657,
+            "velocityY": 2.055226665899016,
+            "timestamp": 0.4456019831420887
+        },
+        {
+            "x": 6.202237157849221,
+            "y": 2.312550420967927,
+            "heading": 2.8014618141625047,
+            "angularVelocity": 3.702899476409415e-7,
+            "velocityX": 3.137281113530435,
+            "velocityY": 2.1137893499268903,
+            "timestamp": 0.5198689803324368
+        },
+        {
+            "x": 6.435233608100515,
+            "y": 2.4695352048100654,
+            "heading": 2.8014618416628014,
+            "angularVelocity": 3.7028959815407443e-7,
+            "velocityX": 3.137281148638853,
+            "velocityY": 2.1137892978193555,
+            "timestamp": 0.5941359775227849
+        },
+        {
+            "x": 6.6682300583517184,
+            "y": 2.6265199886523383,
+            "heading": 2.801461869163098,
+            "angularVelocity": 3.702895988133056e-7,
+            "velocityX": 3.137281148637628,
+            "velocityY": 2.1137892978211736,
+            "timestamp": 0.668402974713133
+        },
+        {
+            "x": 6.901226494182671,
+            "y": 2.7835047938971096,
+            "heading": 2.801461896663395,
+            "angularVelocity": 3.702895996177554e-7,
+            "velocityX": 3.137280954469964,
+            "velocityY": 2.1137895860043394,
+            "timestamp": 0.7426699719034812
+        },
+        {
+            "x": 7.132347068567706,
+            "y": 2.9432384730568093,
+            "heading": 2.801461924236189,
+            "angularVelocity": 3.712657746616385e-7,
+            "velocityX": 3.1120226093518935,
+            "velocityY": 2.1508029838650415,
+            "timestamp": 0.8169369690938293
+        },
+        {
+            "x": 7.334313444212558,
+            "y": 3.1385360610663486,
+            "heading": 2.8014619668447454,
+            "angularVelocity": 5.737212724741241e-7,
+            "velocityX": 2.7194633321070794,
+            "velocityY": 2.6296685660925183,
+            "timestamp": 0.8912039662841774
+        },
+        {
+            "x": 7.4948116486140535,
+            "y": 3.3502854712542645,
+            "heading": 2.8423857101264045,
+            "angularVelocity": 0.5510353835468871,
+            "velocityX": 2.161097263568294,
+            "velocityY": 2.8511912181557233,
+            "timestamp": 0.9654709634745255
+        },
+        {
+            "x": 7.612584040336913,
+            "y": 3.548974342070663,
+            "heading": 2.9223254670447334,
+            "angularVelocity": 1.0763833188709784,
+            "velocityX": 1.5857971397578559,
+            "velocityY": 2.675331955419639,
+            "timestamp": 1.0397379606648736
+        },
+        {
+            "x": 7.689064457271449,
+            "y": 3.727414986995987,
+            "heading": 3.0016093061540334,
+            "angularVelocity": 1.0675514307666671,
+            "velocityX": 1.0298035443457485,
+            "velocityY": 2.402690988945958,
+            "timestamp": 1.1140049578552216
+        },
+        {
+            "x": 7.725394752128812,
+            "y": 3.8837274225386396,
+            "heading": 3.06785364542737,
+            "angularVelocity": 0.8919754639271454,
+            "velocityX": 0.4891849170129639,
+            "velocityY": 2.1047361742931416,
+            "timestamp": 1.1882719550455696
+        },
+        {
+            "x": 7.722234181622283,
+            "y": 4.017077206893078,
+            "heading": 3.1151383358166123,
+            "angularVelocity": 0.6366850980665145,
+            "velocityX": -0.04255686409979938,
+            "velocityY": 1.7955456582236569,
+            "timestamp": 1.2625389522359176
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.33476059520309653,
+            "velocityX": -0.5686803455111581,
+            "velocityY": 1.4801028352497836,
+            "timestamp": 1.3368059494262656
+        },
+        {
+            "x": 7.583164521123183,
+            "y": 4.220739525524416,
+            "heading": 3.1367175382385155,
+            "angularVelocity": -0.03922526988843187,
+            "velocityX": -1.1571796016903517,
+            "velocityY": 1.1201830988719972,
+            "timestamp": 1.4204882738773397
+        },
+        {
+            "x": 7.437331010349068,
+            "y": 4.2837766667768715,
+            "heading": 3.1044919064382244,
+            "angularVelocity": -0.38509484543694944,
+            "velocityX": -1.742703871226464,
+            "velocityY": 0.753290992643391,
+            "timestamp": 1.5041705983284137
+        },
+        {
+            "x": 7.242933099071244,
+            "y": 4.315173048252524,
+            "heading": 3.047144336604056,
+            "angularVelocity": -0.6853008710064864,
+            "velocityX": -2.3230462651820925,
+            "velocityY": 0.3751853414876064,
+            "timestamp": 1.5878529227794878
+        },
+        {
+            "x": 7.00089933731331,
+            "y": 4.3131727597182845,
+            "heading": 2.9720298467353206,
+            "angularVelocity": -0.8976147634695787,
+            "velocityX": -2.8922925282679244,
+            "velocityY": -0.023903357696631062,
+            "timestamp": 1.6715352472305618
+        },
+        {
+            "x": 6.7143889772576815,
+            "y": 4.273350582816976,
+            "heading": 2.8998851691901093,
+            "angularVelocity": -0.8621256402526404,
+            "velocityX": -3.4237858703738695,
+            "velocityY": -0.47587321650692566,
+            "timestamp": 1.7552175716816358
+        },
+        {
+            "x": 6.411635278119781,
+            "y": 4.1808641012010535,
+            "heading": 2.8998851130559236,
+            "angularVelocity": -6.708009886588717e-7,
+            "velocityX": -3.6178930392272837,
+            "velocityY": -1.1052092807244585,
+            "timestamp": 1.8388998961327099
+        },
+        {
+            "x": 6.131017053711007,
+            "y": 4.034347987511598,
+            "heading": 2.8998850873919646,
+            "angularVelocity": -3.066831498282175e-7,
+            "velocityX": -3.3533751153487676,
+            "velocityY": -1.7508609452539547,
+            "timestamp": 1.922582220583784
+        },
+        {
+            "x": 5.854044563819553,
+            "y": 3.8810515447858025,
+            "heading": 2.8998850618676295,
+            "angularVelocity": -3.050146543457835e-7,
+            "velocityX": -3.3098087524252473,
+            "velocityY": -1.8318855711927797,
+            "timestamp": 2.006264545034858
+        },
+        {
+            "x": 5.577072094171527,
+            "y": 3.7277550654846525,
+            "heading": 2.8998850363432944,
+            "angularVelocity": -3.0501465285690934e-7,
+            "velocityX": -3.3098085105171973,
+            "velocityY": -1.8318860082666197,
+            "timestamp": 2.089946869485932
+        },
+        {
+            "x": 5.300099624523611,
+            "y": 3.5744585861833027,
+            "heading": 2.8998850108189593,
+            "angularVelocity": -3.050146519273835e-7,
+            "velocityX": -3.3098085105158774,
+            "velocityY": -1.8318860082690038,
+            "timestamp": 2.173629193937006
+        },
+        {
+            "x": 5.0231271554381145,
+            "y": 3.421162105865856,
+            "heading": 2.899884985294552,
+            "angularVelocity": -3.050155161997765e-7,
+            "velocityX": -3.3098085037949927,
+            "velocityY": -1.831886020411314,
+            "timestamp": 2.25731151838808
+        },
+        {
+            "x": 4.7695419048400876,
+            "y": 3.2806617941163765,
+            "heading": 2.834618611781297,
+            "angularVelocity": -0.7799302175385181,
+            "velocityX": -3.030332298504562,
+            "velocityY": -1.678972383608021,
+            "timestamp": 2.340993842839154
+        },
+        {
+            "x": 4.566605904742026,
+            "y": 3.1682378317487854,
+            "heading": 2.7427589462823403,
+            "angularVelocity": -1.0977188564194784,
+            "velocityX": -2.4250760411980474,
+            "velocityY": -1.3434612757837647,
+            "timestamp": 2.424676167290228
+        },
+        {
+            "x": 4.414672096834118,
+            "y": 3.08405727220976,
+            "heading": 2.6624278185804466,
+            "angularVelocity": -0.959953350111109,
+            "velocityX": -1.8156021466246228,
+            "velocityY": -1.0059538868120508,
+            "timestamp": 2.508358491741302
+        },
+        {
+            "x": 4.313522085490217,
+            "y": 3.0280030162190834,
+            "heading": 2.6044623353701315,
+            "angularVelocity": -0.6926849079605295,
+            "velocityX": -1.2087380699258492,
+            "velocityY": -0.6698458289533948,
+            "timestamp": 2.5920408161923763
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -0.3640235326869004,
+            "velocityX": -0.6037366411798936,
+            "velocityY": -0.3346347798387892,
+            "timestamp": 2.6757231406434503
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 3.2974830734555563e-32,
+            "velocityY": 2.8302357269459625e-32,
+            "timestamp": 2.7594054650945243
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S4.1.traj
new file mode 100644
index 00000000..752a3083
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S4.1.traj	
@@ -0,0 +1,337 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 6.749906626357912e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 5.210246643603351,
+            "y": 1.6542137133955805,
+            "heading": 2.5206016461454785,
+            "angularVelocity": 0.26943757558327686,
+            "velocityX": 0.4478929770559344,
+            "velocityY": 0.44746230101760404,
+            "timestamp": 0.07646166686617559
+        },
+        {
+            "x": 5.278793169653249,
+            "y": 1.7226907202744617,
+            "heading": 2.5594414345510907,
+            "angularVelocity": 0.5079641864673213,
+            "velocityX": 0.8964822356001046,
+            "velocityY": 0.8955730326770261,
+            "timestamp": 0.15292333373235117
+        },
+        {
+            "x": 5.381708599884549,
+            "y": 1.8254946394710425,
+            "heading": 2.61294205757613,
+            "angularVelocity": 0.6997051623093229,
+            "velocityX": 1.3459741913738752,
+            "velocityY": 1.344515800008778,
+            "timestamp": 0.22938500059852676
+        },
+        {
+            "x": 5.519076425443274,
+            "y": 1.9627026546285364,
+            "heading": 2.6750057017308966,
+            "angularVelocity": 0.8116961962572822,
+            "velocityX": 1.796558081831356,
+            "velocityY": 1.7944680096712713,
+            "timestamp": 0.30584666746470235
+        },
+        {
+            "x": 5.690930173156719,
+            "y": 2.1343411411965687,
+            "heading": 2.7325200708925137,
+            "angularVelocity": 0.7521987359009454,
+            "velocityX": 2.2475804511851214,
+            "velocityY": 2.2447651693028954,
+            "timestamp": 0.38230833433087796
+        },
+        {
+            "x": 5.895075735242969,
+            "y": 2.338989079985295,
+            "heading": 2.7330245204544825,
+            "angularVelocity": 0.006597417799580435,
+            "velocityX": 2.6699072941157542,
+            "velocityY": 2.6764776021284593,
+            "timestamp": 0.4587700011970536
+        },
+        {
+            "x": 6.099717519184054,
+            "y": 2.543408411889471,
+            "heading": 2.7330245436350364,
+            "angularVelocity": 3.031656877658216e-7,
+            "velocityX": 2.676397106268319,
+            "velocityY": 2.673487778679394,
+            "timestamp": 0.5352316680632292
+        },
+        {
+            "x": 6.3043593056762255,
+            "y": 2.7478277412397842,
+            "heading": 2.7330245668155904,
+            "angularVelocity": 3.03165682232601e-7,
+            "velocityX": 2.676397139632574,
+            "velocityY": 2.673487745278835,
+            "timestamp": 0.6116933349294048
+        },
+        {
+            "x": 6.509001092168415,
+            "y": 2.9522470705900794,
+            "heading": 2.733024589996144,
+            "angularVelocity": 3.031656817444859e-7,
+            "velocityX": 2.676397139632809,
+            "velocityY": 2.6734877452786003,
+            "timestamp": 0.6881550017955804
+        },
+        {
+            "x": 6.713642878660604,
+            "y": 3.1566663999403746,
+            "heading": 2.733024613176697,
+            "angularVelocity": 3.0316568050462653e-7,
+            "velocityX": 2.676397139632809,
+            "velocityY": 2.6734877452786003,
+            "timestamp": 0.764616668661756
+        },
+        {
+            "x": 6.918284665152036,
+            "y": 3.3610857292914273,
+            "heading": 2.7330246363572503,
+            "angularVelocity": 3.0316568069769535e-7,
+            "velocityX": 2.676397139622915,
+            "velocityY": 2.6734877452885044,
+            "timestamp": 0.8410783355279317
+        },
+        {
+            "x": 7.122926344234402,
+            "y": 3.565505166167973,
+            "heading": 2.733024659538502,
+            "angularVelocity": 3.031748161580926e-7,
+            "velocityX": 2.6763957348789207,
+            "velocityY": 2.67348915155517,
+            "timestamp": 0.9175400023941073
+        },
+        {
+            "x": 7.301295269977114,
+            "y": 3.7451643880510668,
+            "heading": 2.8254238510952887,
+            "angularVelocity": 1.2084381016504042,
+            "velocityX": 2.332788873866637,
+            "velocityY": 2.349663945955243,
+            "timestamp": 0.9940016692602829
+        },
+        {
+            "x": 7.445441288033757,
+            "y": 3.8901748417746975,
+            "heading": 2.925487102833734,
+            "angularVelocity": 1.308672120810262,
+            "velocityX": 1.8852063257910538,
+            "velocityY": 1.8965118034561068,
+            "timestamp": 1.0704633361264584
+        },
+        {
+            "x": 7.555314221872798,
+            "y": 4.0007360886631185,
+            "heading": 3.011754015083649,
+            "angularVelocity": 1.1282374003289979,
+            "velocityX": 1.436967546513772,
+            "velocityY": 1.445969613531011,
+            "timestamp": 1.146925002992634
+        },
+        {
+            "x": 7.6309987143881095,
+            "y": 4.077005222028419,
+            "heading": 3.0780078899884065,
+            "angularVelocity": 0.8664979148403406,
+            "velocityX": 0.98983576499549,
+            "velocityY": 0.997481960454622,
+            "timestamp": 1.2233866698588094
+        },
+        {
+            "x": 7.672548767265181,
+            "y": 4.1190736771641765,
+            "heading": 3.121324471245367,
+            "angularVelocity": 0.5665136928387217,
+            "velocityX": 0.5434102417593452,
+            "velocityY": 0.5501901391894305,
+            "timestamp": 1.299848336724985
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.24424694778521738,
+            "velocityX": 0.09745056627996526,
+            "velocityY": 0.10366400787072177,
+            "timestamp": 1.3763100035911604
+        },
+        {
+            "x": 7.651764263007655,
+            "y": 4.099219122472668,
+            "heading": 3.13204050140682,
+            "angularVelocity": -0.10143830296235676,
+            "velocityX": -0.35984493368073844,
+            "velocityY": -0.35404806448388143,
+            "timestamp": 1.4547764049477947
+        },
+        {
+            "x": 7.587580185818596,
+            "y": 4.035451706139359,
+            "heading": 3.0993558868019444,
+            "angularVelocity": -0.41654280099226804,
+            "velocityX": -0.8179816594027142,
+            "velocityY": -0.8126716050540244,
+            "timestamp": 1.533242806304429
+        },
+        {
+            "x": 7.487355398269796,
+            "y": 3.9356017103716967,
+            "heading": 3.045691872479544,
+            "angularVelocity": -0.683910736246133,
+            "velocityX": -1.2772955789481495,
+            "velocityY": -1.272519116989166,
+            "timestamp": 1.6117092076610633
+        },
+        {
+            "x": 7.35096170591853,
+            "y": 3.7995397339434045,
+            "heading": 2.9777353222524336,
+            "angularVelocity": -0.8660592183684257,
+            "velocityX": -1.7382432479775494,
+            "velocityY": -1.7340157580297684,
+            "timestamp": 1.6901756090176976
+        },
+        {
+            "x": 7.178293181957013,
+            "y": 3.6271506268794376,
+            "heading": 2.9104315316891,
+            "angularVelocity": -0.8577402480513148,
+            "velocityX": -2.2005408808889824,
+            "velocityY": -2.1969799058382784,
+            "timestamp": 1.7686420103743319
+        },
+        {
+            "x": 6.971558922924333,
+            "y": 3.4198249259322475,
+            "heading": 2.9002273959835922,
+            "angularVelocity": -0.13004465005511517,
+            "velocityX": -2.6346851067256227,
+            "velocityY": -2.6422226247497056,
+            "timestamp": 1.8471084117309662
+        },
+        {
+            "x": 6.761730033100129,
+            "y": 3.2098675579352265,
+            "heading": 2.900227368387608,
+            "angularVelocity": -3.5169172941648496e-7,
+            "velocityX": -2.6741240352099296,
+            "velocityY": -2.6757614006376103,
+            "timestamp": 1.9255748130876005
+        },
+        {
+            "x": 6.551901141744289,
+            "y": 2.9999101914688975,
+            "heading": 2.900227340791633,
+            "angularVelocity": -3.5169161244263704e-7,
+            "velocityX": -2.674124054729561,
+            "velocityY": -2.6757613811300027,
+            "timestamp": 2.0040412144442348
+        },
+        {
+            "x": 6.342072250388439,
+            "y": 2.7899528250025782,
+            "heading": 2.9002273131956575,
+            "angularVelocity": -3.5169161245965383e-7,
+            "velocityX": -2.674124054729688,
+            "velocityY": -2.6757613811298757,
+            "timestamp": 2.082507615800869
+        },
+        {
+            "x": 6.132243359032647,
+            "y": 2.579995458536201,
+            "heading": 2.9002272855996822,
+            "angularVelocity": -3.5169161351505577e-7,
+            "velocityX": -2.674124054728939,
+            "velocityY": -2.675761381130624,
+            "timestamp": 2.1609740171575034
+        },
+        {
+            "x": 5.922414476696474,
+            "y": 2.3700380830557237,
+            "heading": 2.900227258003707,
+            "angularVelocity": -3.51691614830637e-7,
+            "velocityX": -2.6741239397801397,
+            "velocityY": -2.67576149600908,
+            "timestamp": 2.2394404185141377
+        },
+        {
+            "x": 5.71397323769833,
+            "y": 2.1587030259315876,
+            "heading": 2.9002272179386366,
+            "angularVelocity": -5.106016050290645e-7,
+            "velocityX": -2.6564393854481323,
+            "velocityY": -2.6933190954381305,
+            "timestamp": 2.317906819870772
+        },
+        {
+            "x": 5.5341856675792425,
+            "y": 1.9787047842990844,
+            "heading": 2.7906748594245325,
+            "angularVelocity": -1.39616901782181,
+            "velocityX": -2.2912681990084183,
+            "velocityY": -2.2939530616983537,
+            "timestamp": 2.3963732212274063
+        },
+        {
+            "x": 5.390687970791945,
+            "y": 1.8350178958205943,
+            "heading": 2.6821705547459764,
+            "angularVelocity": -1.3828122967612335,
+            "velocityX": -1.8287788697622565,
+            "velocityY": -1.8311899869783075,
+            "timestamp": 2.4748396225840406
+        },
+        {
+            "x": 5.2832586949423685,
+            "y": 1.7274312477093827,
+            "heading": 2.5937450000537616,
+            "angularVelocity": -1.1269225192361696,
+            "velocityX": -1.369111798071441,
+            "velocityY": -1.371117398671364,
+            "timestamp": 2.553306023940675
+        },
+        {
+            "x": 5.211730147578831,
+            "y": 1.6557903820034874,
+            "heading": 2.5319094528254147,
+            "angularVelocity": -0.7880512698333239,
+            "velocityX": -0.9115818506628853,
+            "velocityY": -0.913013270231208,
+            "timestamp": 2.631772425297309
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.4066639003920201,
+            "velocityX": -0.4553560117589102,
+            "velocityY": -0.45612365782926495,
+            "timestamp": 2.7102388266539434
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": -6.900806950408652e-34,
+            "velocityY": 1.697639013664865e-32,
+            "timestamp": 2.7887052280105777
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S4.traj
new file mode 100644
index 00000000..752a3083
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S4.traj	
@@ -0,0 +1,337 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 6.749906626357912e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 5.210246643603351,
+            "y": 1.6542137133955805,
+            "heading": 2.5206016461454785,
+            "angularVelocity": 0.26943757558327686,
+            "velocityX": 0.4478929770559344,
+            "velocityY": 0.44746230101760404,
+            "timestamp": 0.07646166686617559
+        },
+        {
+            "x": 5.278793169653249,
+            "y": 1.7226907202744617,
+            "heading": 2.5594414345510907,
+            "angularVelocity": 0.5079641864673213,
+            "velocityX": 0.8964822356001046,
+            "velocityY": 0.8955730326770261,
+            "timestamp": 0.15292333373235117
+        },
+        {
+            "x": 5.381708599884549,
+            "y": 1.8254946394710425,
+            "heading": 2.61294205757613,
+            "angularVelocity": 0.6997051623093229,
+            "velocityX": 1.3459741913738752,
+            "velocityY": 1.344515800008778,
+            "timestamp": 0.22938500059852676
+        },
+        {
+            "x": 5.519076425443274,
+            "y": 1.9627026546285364,
+            "heading": 2.6750057017308966,
+            "angularVelocity": 0.8116961962572822,
+            "velocityX": 1.796558081831356,
+            "velocityY": 1.7944680096712713,
+            "timestamp": 0.30584666746470235
+        },
+        {
+            "x": 5.690930173156719,
+            "y": 2.1343411411965687,
+            "heading": 2.7325200708925137,
+            "angularVelocity": 0.7521987359009454,
+            "velocityX": 2.2475804511851214,
+            "velocityY": 2.2447651693028954,
+            "timestamp": 0.38230833433087796
+        },
+        {
+            "x": 5.895075735242969,
+            "y": 2.338989079985295,
+            "heading": 2.7330245204544825,
+            "angularVelocity": 0.006597417799580435,
+            "velocityX": 2.6699072941157542,
+            "velocityY": 2.6764776021284593,
+            "timestamp": 0.4587700011970536
+        },
+        {
+            "x": 6.099717519184054,
+            "y": 2.543408411889471,
+            "heading": 2.7330245436350364,
+            "angularVelocity": 3.031656877658216e-7,
+            "velocityX": 2.676397106268319,
+            "velocityY": 2.673487778679394,
+            "timestamp": 0.5352316680632292
+        },
+        {
+            "x": 6.3043593056762255,
+            "y": 2.7478277412397842,
+            "heading": 2.7330245668155904,
+            "angularVelocity": 3.03165682232601e-7,
+            "velocityX": 2.676397139632574,
+            "velocityY": 2.673487745278835,
+            "timestamp": 0.6116933349294048
+        },
+        {
+            "x": 6.509001092168415,
+            "y": 2.9522470705900794,
+            "heading": 2.733024589996144,
+            "angularVelocity": 3.031656817444859e-7,
+            "velocityX": 2.676397139632809,
+            "velocityY": 2.6734877452786003,
+            "timestamp": 0.6881550017955804
+        },
+        {
+            "x": 6.713642878660604,
+            "y": 3.1566663999403746,
+            "heading": 2.733024613176697,
+            "angularVelocity": 3.0316568050462653e-7,
+            "velocityX": 2.676397139632809,
+            "velocityY": 2.6734877452786003,
+            "timestamp": 0.764616668661756
+        },
+        {
+            "x": 6.918284665152036,
+            "y": 3.3610857292914273,
+            "heading": 2.7330246363572503,
+            "angularVelocity": 3.0316568069769535e-7,
+            "velocityX": 2.676397139622915,
+            "velocityY": 2.6734877452885044,
+            "timestamp": 0.8410783355279317
+        },
+        {
+            "x": 7.122926344234402,
+            "y": 3.565505166167973,
+            "heading": 2.733024659538502,
+            "angularVelocity": 3.031748161580926e-7,
+            "velocityX": 2.6763957348789207,
+            "velocityY": 2.67348915155517,
+            "timestamp": 0.9175400023941073
+        },
+        {
+            "x": 7.301295269977114,
+            "y": 3.7451643880510668,
+            "heading": 2.8254238510952887,
+            "angularVelocity": 1.2084381016504042,
+            "velocityX": 2.332788873866637,
+            "velocityY": 2.349663945955243,
+            "timestamp": 0.9940016692602829
+        },
+        {
+            "x": 7.445441288033757,
+            "y": 3.8901748417746975,
+            "heading": 2.925487102833734,
+            "angularVelocity": 1.308672120810262,
+            "velocityX": 1.8852063257910538,
+            "velocityY": 1.8965118034561068,
+            "timestamp": 1.0704633361264584
+        },
+        {
+            "x": 7.555314221872798,
+            "y": 4.0007360886631185,
+            "heading": 3.011754015083649,
+            "angularVelocity": 1.1282374003289979,
+            "velocityX": 1.436967546513772,
+            "velocityY": 1.445969613531011,
+            "timestamp": 1.146925002992634
+        },
+        {
+            "x": 7.6309987143881095,
+            "y": 4.077005222028419,
+            "heading": 3.0780078899884065,
+            "angularVelocity": 0.8664979148403406,
+            "velocityX": 0.98983576499549,
+            "velocityY": 0.997481960454622,
+            "timestamp": 1.2233866698588094
+        },
+        {
+            "x": 7.672548767265181,
+            "y": 4.1190736771641765,
+            "heading": 3.121324471245367,
+            "angularVelocity": 0.5665136928387217,
+            "velocityX": 0.5434102417593452,
+            "velocityY": 0.5501901391894305,
+            "timestamp": 1.299848336724985
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.24424694778521738,
+            "velocityX": 0.09745056627996526,
+            "velocityY": 0.10366400787072177,
+            "timestamp": 1.3763100035911604
+        },
+        {
+            "x": 7.651764263007655,
+            "y": 4.099219122472668,
+            "heading": 3.13204050140682,
+            "angularVelocity": -0.10143830296235676,
+            "velocityX": -0.35984493368073844,
+            "velocityY": -0.35404806448388143,
+            "timestamp": 1.4547764049477947
+        },
+        {
+            "x": 7.587580185818596,
+            "y": 4.035451706139359,
+            "heading": 3.0993558868019444,
+            "angularVelocity": -0.41654280099226804,
+            "velocityX": -0.8179816594027142,
+            "velocityY": -0.8126716050540244,
+            "timestamp": 1.533242806304429
+        },
+        {
+            "x": 7.487355398269796,
+            "y": 3.9356017103716967,
+            "heading": 3.045691872479544,
+            "angularVelocity": -0.683910736246133,
+            "velocityX": -1.2772955789481495,
+            "velocityY": -1.272519116989166,
+            "timestamp": 1.6117092076610633
+        },
+        {
+            "x": 7.35096170591853,
+            "y": 3.7995397339434045,
+            "heading": 2.9777353222524336,
+            "angularVelocity": -0.8660592183684257,
+            "velocityX": -1.7382432479775494,
+            "velocityY": -1.7340157580297684,
+            "timestamp": 1.6901756090176976
+        },
+        {
+            "x": 7.178293181957013,
+            "y": 3.6271506268794376,
+            "heading": 2.9104315316891,
+            "angularVelocity": -0.8577402480513148,
+            "velocityX": -2.2005408808889824,
+            "velocityY": -2.1969799058382784,
+            "timestamp": 1.7686420103743319
+        },
+        {
+            "x": 6.971558922924333,
+            "y": 3.4198249259322475,
+            "heading": 2.9002273959835922,
+            "angularVelocity": -0.13004465005511517,
+            "velocityX": -2.6346851067256227,
+            "velocityY": -2.6422226247497056,
+            "timestamp": 1.8471084117309662
+        },
+        {
+            "x": 6.761730033100129,
+            "y": 3.2098675579352265,
+            "heading": 2.900227368387608,
+            "angularVelocity": -3.5169172941648496e-7,
+            "velocityX": -2.6741240352099296,
+            "velocityY": -2.6757614006376103,
+            "timestamp": 1.9255748130876005
+        },
+        {
+            "x": 6.551901141744289,
+            "y": 2.9999101914688975,
+            "heading": 2.900227340791633,
+            "angularVelocity": -3.5169161244263704e-7,
+            "velocityX": -2.674124054729561,
+            "velocityY": -2.6757613811300027,
+            "timestamp": 2.0040412144442348
+        },
+        {
+            "x": 6.342072250388439,
+            "y": 2.7899528250025782,
+            "heading": 2.9002273131956575,
+            "angularVelocity": -3.5169161245965383e-7,
+            "velocityX": -2.674124054729688,
+            "velocityY": -2.6757613811298757,
+            "timestamp": 2.082507615800869
+        },
+        {
+            "x": 6.132243359032647,
+            "y": 2.579995458536201,
+            "heading": 2.9002272855996822,
+            "angularVelocity": -3.5169161351505577e-7,
+            "velocityX": -2.674124054728939,
+            "velocityY": -2.675761381130624,
+            "timestamp": 2.1609740171575034
+        },
+        {
+            "x": 5.922414476696474,
+            "y": 2.3700380830557237,
+            "heading": 2.900227258003707,
+            "angularVelocity": -3.51691614830637e-7,
+            "velocityX": -2.6741239397801397,
+            "velocityY": -2.67576149600908,
+            "timestamp": 2.2394404185141377
+        },
+        {
+            "x": 5.71397323769833,
+            "y": 2.1587030259315876,
+            "heading": 2.9002272179386366,
+            "angularVelocity": -5.106016050290645e-7,
+            "velocityX": -2.6564393854481323,
+            "velocityY": -2.6933190954381305,
+            "timestamp": 2.317906819870772
+        },
+        {
+            "x": 5.5341856675792425,
+            "y": 1.9787047842990844,
+            "heading": 2.7906748594245325,
+            "angularVelocity": -1.39616901782181,
+            "velocityX": -2.2912681990084183,
+            "velocityY": -2.2939530616983537,
+            "timestamp": 2.3963732212274063
+        },
+        {
+            "x": 5.390687970791945,
+            "y": 1.8350178958205943,
+            "heading": 2.6821705547459764,
+            "angularVelocity": -1.3828122967612335,
+            "velocityX": -1.8287788697622565,
+            "velocityY": -1.8311899869783075,
+            "timestamp": 2.4748396225840406
+        },
+        {
+            "x": 5.2832586949423685,
+            "y": 1.7274312477093827,
+            "heading": 2.5937450000537616,
+            "angularVelocity": -1.1269225192361696,
+            "velocityX": -1.369111798071441,
+            "velocityY": -1.371117398671364,
+            "timestamp": 2.553306023940675
+        },
+        {
+            "x": 5.211730147578831,
+            "y": 1.6557903820034874,
+            "heading": 2.5319094528254147,
+            "angularVelocity": -0.7880512698333239,
+            "velocityX": -0.9115818506628853,
+            "velocityY": -0.913013270231208,
+            "timestamp": 2.631772425297309
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.4066639003920201,
+            "velocityX": -0.4553560117589102,
+            "velocityY": -0.45612365782926495,
+            "timestamp": 2.7102388266539434
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": -6.900806950408652e-34,
+            "velocityY": 1.697639013664865e-32,
+            "timestamp": 2.7887052280105777
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S3.1.traj
new file mode 100644
index 00000000..0baf5ca9
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S3.1.traj	
@@ -0,0 +1,301 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 2.0168042378406012e-32,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.220318940234659,
+            "y": 1.6240042188799209,
+            "heading": 2.522514839920101,
+            "angularVelocity": 0.3066580902466342,
+            "velocityX": 0.6036357185902667,
+            "velocityY": 0.05453852299210067,
+            "timestamp": 0.07342000956828917
+        },
+        {
+            "x": 5.309030400883647,
+            "y": 1.632013165892952,
+            "heading": 2.5656393100423864,
+            "angularVelocity": 0.5873667189075205,
+            "velocityX": 1.208273618739824,
+            "velocityY": 0.10908398214768973,
+            "timestamp": 0.14684001913657835
+        },
+        {
+            "x": 5.442235678671135,
+            "y": 1.6440289719213574,
+            "heading": 2.6263948924042286,
+            "angularVelocity": 0.8275071430674761,
+            "velocityX": 1.8142912071346369,
+            "velocityY": 0.1636584644847863,
+            "timestamp": 0.22026002870486752
+        },
+        {
+            "x": 5.6200752852220655,
+            "y": 1.6600527803178184,
+            "heading": 2.6994681964366594,
+            "angularVelocity": 0.9952777786614565,
+            "velocityX": 2.4222226011223906,
+            "velocityY": 0.21824851958859787,
+            "timestamp": 0.2936800382731567
+        },
+        {
+            "x": 5.842683997650443,
+            "y": 1.680064789350092,
+            "heading": 2.773109972156216,
+            "angularVelocity": 1.0030205137886674,
+            "velocityX": 3.031989695143347,
+            "velocityY": 0.2725688698481895,
+            "timestamp": 0.3671000478414459
+        },
+        {
+            "x": 6.108354667125484,
+            "y": 1.7034775424781843,
+            "heading": 2.8018843070789763,
+            "angularVelocity": 0.39191407208947715,
+            "velocityX": 3.6185049693835545,
+            "velocityY": 0.31888790625005003,
+            "timestamp": 0.4405200574097351
+        },
+        {
+            "x": 6.384952370737624,
+            "y": 1.7286802043771023,
+            "heading": 2.8018843326053964,
+            "angularVelocity": 3.476766132233899e-7,
+            "velocityX": 3.7673340719858177,
+            "velocityY": 0.34326693836064737,
+            "timestamp": 0.5139400669780243
+        },
+        {
+            "x": 6.661294346947145,
+            "y": 1.7565473171945905,
+            "heading": 2.8018843581575315,
+            "angularVelocity": 3.48026857889212e-7,
+            "velocityX": 3.7638509969478653,
+            "velocityY": 0.37955746643565225,
+            "timestamp": 0.5873600765463135
+        },
+        {
+            "x": 6.9295239726753675,
+            "y": 1.8286189976607798,
+            "heading": 2.8018844037308708,
+            "angularVelocity": 6.207209630445694e-7,
+            "velocityX": 3.6533586321414817,
+            "velocityY": 0.9816354000767032,
+            "timestamp": 0.6607800861146027
+        },
+        {
+            "x": 7.165852331775521,
+            "y": 1.9289604111011978,
+            "heading": 2.8534870271763118,
+            "angularVelocity": 0.7028414154241911,
+            "velocityX": 3.218854921019001,
+            "velocityY": 1.3666766598157905,
+            "timestamp": 0.7342000956828919
+        },
+        {
+            "x": 7.3582678693157275,
+            "y": 2.0352947515499316,
+            "heading": 2.9321246763669695,
+            "angularVelocity": 1.0710656352818668,
+            "velocityX": 2.620750646473749,
+            "velocityY": 1.4483019148865994,
+            "timestamp": 0.8076201052511811
+        },
+        {
+            "x": 7.505754872565929,
+            "y": 2.142580812119965,
+            "heading": 3.007723504913372,
+            "angularVelocity": 1.0296760922659598,
+            "velocityX": 2.0088120951961708,
+            "velocityY": 1.4612645953179093,
+            "timestamp": 0.8810401148194703
+        },
+        {
+            "x": 7.608443195664495,
+            "y": 2.2491262386362876,
+            "heading": 3.070552698416666,
+            "angularVelocity": 0.855750276698862,
+            "velocityX": 1.3986421917181704,
+            "velocityY": 1.4511769631033729,
+            "timestamp": 0.9544601243877595
+        },
+        {
+            "x": 7.666484933948513,
+            "y": 2.354097419514004,
+            "heading": 3.115652624171886,
+            "angularVelocity": 0.6142729484838055,
+            "velocityX": 0.7905438670643103,
+            "velocityY": 1.4297353200436178,
+            "timestamp": 1.0278801339560486
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 0.331617715269703,
+            "velocityX": 0.18407878357617766,
+            "velocityY": 1.4015604341523722,
+            "timestamp": 1.1013001435243377
+        },
+        {
+            "x": 7.640202623194362,
+            "y": 2.56833046361043,
+            "heading": 3.1389376214813485,
+            "angularVelocity": -0.013028904924243205,
+            "velocityX": -0.48807108721701464,
+            "velocityY": 1.3653457784439293,
+            "timestamp": 1.1828402671824687
+        },
+        {
+            "x": 7.5455521042399925,
+            "y": 2.676247019412303,
+            "heading": 3.111715108217083,
+            "angularVelocity": -0.3338542062850063,
+            "velocityX": -1.1607845893293163,
+            "velocityY": 1.3234779512271038,
+            "timestamp": 1.2643803908406
+        },
+        {
+            "x": 7.395994092884954,
+            "y": 2.780007432926005,
+            "heading": 3.061424395012115,
+            "angularVelocity": -0.616760325454245,
+            "velocityX": -1.8341646375480058,
+            "velocityY": 1.2725074338705782,
+            "timestamp": 1.3459205144987312
+        },
+        {
+            "x": 7.191491634258091,
+            "y": 2.8782237282133547,
+            "heading": 2.9936811867966338,
+            "angularVelocity": -0.8307959955948208,
+            "velocityX": -2.5079978966461463,
+            "velocityY": 1.2045149170871063,
+            "timestamp": 1.4274606381568624
+        },
+        {
+            "x": 6.932297950713545,
+            "y": 2.9674313510808874,
+            "heading": 2.9221270442336884,
+            "angularVelocity": -0.8775329169593383,
+            "velocityX": -3.178725661874757,
+            "velocityY": 1.0940334508388325,
+            "timestamp": 1.5090007618149937
+        },
+        {
+            "x": 6.629938780171593,
+            "y": 3.0284835582667458,
+            "heading": 2.9221269432503374,
+            "angularVelocity": -0.0000012384498122465325,
+            "velocityX": -3.7081029188726404,
+            "velocityY": 0.7487382217106987,
+            "timestamp": 1.5905408854731249
+        },
+        {
+            "x": 6.3215346451204795,
+            "y": 3.034428801743739,
+            "heading": 2.922126920201386,
+            "angularVelocity": -2.8267005270994496e-7,
+            "velocityX": -3.7822377648596976,
+            "velocityY": 0.07291187712592015,
+            "timestamp": 1.6720810091312561
+        },
+        {
+            "x": 6.01312270862256,
+            "y": 3.0289030469499765,
+            "heading": 2.9221268973831047,
+            "angularVelocity": -2.7984114359464816e-7,
+            "velocityX": -3.7823334410305938,
+            "velocityY": -0.06776730946506612,
+            "timestamp": 1.7536211327893874
+        },
+        {
+            "x": 5.704710773369897,
+            "y": 3.023377222654451,
+            "heading": 2.9221268745648237,
+            "angularVelocity": -2.7984113775105834e-7,
+            "velocityX": -3.782333425758887,
+            "velocityY": -0.06776816182780301,
+            "timestamp": 1.8351612564475186
+        },
+        {
+            "x": 5.396298838117239,
+            "y": 3.0178513983586757,
+            "heading": 2.922126851746543,
+            "angularVelocity": -2.7984113682317286e-7,
+            "velocityX": -3.7823334257588326,
+            "velocityY": -0.06776816183086862,
+            "timestamp": 1.9167013801056498
+        },
+        {
+            "x": 5.087886902401733,
+            "y": 3.012325599899855,
+            "heading": 2.922126828928091,
+            "angularVelocity": -2.798432358955337e-7,
+            "velocityX": -3.7823334314351498,
+            "velocityY": -0.06776784496901485,
+            "timestamp": 1.998241503763781
+        },
+        {
+            "x": 4.812446473600025,
+            "y": 3.0084984766412206,
+            "heading": 2.8330493298750046,
+            "angularVelocity": -1.0924376252672112,
+            "velocityX": -3.3779741364697116,
+            "velocityY": -0.04693546056758059,
+            "timestamp": 2.0797816274219123
+        },
+        {
+            "x": 4.592410263299027,
+            "y": 3.0051727297391726,
+            "heading": 2.737251875069825,
+            "angularVelocity": -1.1748504969997702,
+            "velocityX": -2.6985022885608188,
+            "velocityY": -0.040786630591711566,
+            "timestamp": 2.1613217510800435
+        },
+        {
+            "x": 4.427608751414137,
+            "y": 3.0026089111743004,
+            "heading": 2.658186226973151,
+            "angularVelocity": -0.9696532768109137,
+            "velocityX": -2.0211094181785265,
+            "velocityY": -0.03144241693353635,
+            "timestamp": 2.2428618747381748
+        },
+        {
+            "x": 4.3178442485460256,
+            "y": 3.0008747293604148,
+            "heading": 2.602696579769804,
+            "angularVelocity": -0.6805195370563186,
+            "velocityX": -1.346140991008498,
+            "velocityY": -0.021267833994898407,
+            "timestamp": 2.324401998396306
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -0.3519320119027408,
+            "velocityX": -0.6726044318496349,
+            "velocityY": -0.010727594234238386,
+            "timestamp": 2.4059421220544372
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": -1.5467932880075213e-32,
+            "velocityY": 0,
+            "timestamp": 2.4874822457125685
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S3.traj
new file mode 100644
index 00000000..0baf5ca9
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S3.traj	
@@ -0,0 +1,301 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 2.0168042378406012e-32,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.220318940234659,
+            "y": 1.6240042188799209,
+            "heading": 2.522514839920101,
+            "angularVelocity": 0.3066580902466342,
+            "velocityX": 0.6036357185902667,
+            "velocityY": 0.05453852299210067,
+            "timestamp": 0.07342000956828917
+        },
+        {
+            "x": 5.309030400883647,
+            "y": 1.632013165892952,
+            "heading": 2.5656393100423864,
+            "angularVelocity": 0.5873667189075205,
+            "velocityX": 1.208273618739824,
+            "velocityY": 0.10908398214768973,
+            "timestamp": 0.14684001913657835
+        },
+        {
+            "x": 5.442235678671135,
+            "y": 1.6440289719213574,
+            "heading": 2.6263948924042286,
+            "angularVelocity": 0.8275071430674761,
+            "velocityX": 1.8142912071346369,
+            "velocityY": 0.1636584644847863,
+            "timestamp": 0.22026002870486752
+        },
+        {
+            "x": 5.6200752852220655,
+            "y": 1.6600527803178184,
+            "heading": 2.6994681964366594,
+            "angularVelocity": 0.9952777786614565,
+            "velocityX": 2.4222226011223906,
+            "velocityY": 0.21824851958859787,
+            "timestamp": 0.2936800382731567
+        },
+        {
+            "x": 5.842683997650443,
+            "y": 1.680064789350092,
+            "heading": 2.773109972156216,
+            "angularVelocity": 1.0030205137886674,
+            "velocityX": 3.031989695143347,
+            "velocityY": 0.2725688698481895,
+            "timestamp": 0.3671000478414459
+        },
+        {
+            "x": 6.108354667125484,
+            "y": 1.7034775424781843,
+            "heading": 2.8018843070789763,
+            "angularVelocity": 0.39191407208947715,
+            "velocityX": 3.6185049693835545,
+            "velocityY": 0.31888790625005003,
+            "timestamp": 0.4405200574097351
+        },
+        {
+            "x": 6.384952370737624,
+            "y": 1.7286802043771023,
+            "heading": 2.8018843326053964,
+            "angularVelocity": 3.476766132233899e-7,
+            "velocityX": 3.7673340719858177,
+            "velocityY": 0.34326693836064737,
+            "timestamp": 0.5139400669780243
+        },
+        {
+            "x": 6.661294346947145,
+            "y": 1.7565473171945905,
+            "heading": 2.8018843581575315,
+            "angularVelocity": 3.48026857889212e-7,
+            "velocityX": 3.7638509969478653,
+            "velocityY": 0.37955746643565225,
+            "timestamp": 0.5873600765463135
+        },
+        {
+            "x": 6.9295239726753675,
+            "y": 1.8286189976607798,
+            "heading": 2.8018844037308708,
+            "angularVelocity": 6.207209630445694e-7,
+            "velocityX": 3.6533586321414817,
+            "velocityY": 0.9816354000767032,
+            "timestamp": 0.6607800861146027
+        },
+        {
+            "x": 7.165852331775521,
+            "y": 1.9289604111011978,
+            "heading": 2.8534870271763118,
+            "angularVelocity": 0.7028414154241911,
+            "velocityX": 3.218854921019001,
+            "velocityY": 1.3666766598157905,
+            "timestamp": 0.7342000956828919
+        },
+        {
+            "x": 7.3582678693157275,
+            "y": 2.0352947515499316,
+            "heading": 2.9321246763669695,
+            "angularVelocity": 1.0710656352818668,
+            "velocityX": 2.620750646473749,
+            "velocityY": 1.4483019148865994,
+            "timestamp": 0.8076201052511811
+        },
+        {
+            "x": 7.505754872565929,
+            "y": 2.142580812119965,
+            "heading": 3.007723504913372,
+            "angularVelocity": 1.0296760922659598,
+            "velocityX": 2.0088120951961708,
+            "velocityY": 1.4612645953179093,
+            "timestamp": 0.8810401148194703
+        },
+        {
+            "x": 7.608443195664495,
+            "y": 2.2491262386362876,
+            "heading": 3.070552698416666,
+            "angularVelocity": 0.855750276698862,
+            "velocityX": 1.3986421917181704,
+            "velocityY": 1.4511769631033729,
+            "timestamp": 0.9544601243877595
+        },
+        {
+            "x": 7.666484933948513,
+            "y": 2.354097419514004,
+            "heading": 3.115652624171886,
+            "angularVelocity": 0.6142729484838055,
+            "velocityX": 0.7905438670643103,
+            "velocityY": 1.4297353200436178,
+            "timestamp": 1.0278801339560486
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 0.331617715269703,
+            "velocityX": 0.18407878357617766,
+            "velocityY": 1.4015604341523722,
+            "timestamp": 1.1013001435243377
+        },
+        {
+            "x": 7.640202623194362,
+            "y": 2.56833046361043,
+            "heading": 3.1389376214813485,
+            "angularVelocity": -0.013028904924243205,
+            "velocityX": -0.48807108721701464,
+            "velocityY": 1.3653457784439293,
+            "timestamp": 1.1828402671824687
+        },
+        {
+            "x": 7.5455521042399925,
+            "y": 2.676247019412303,
+            "heading": 3.111715108217083,
+            "angularVelocity": -0.3338542062850063,
+            "velocityX": -1.1607845893293163,
+            "velocityY": 1.3234779512271038,
+            "timestamp": 1.2643803908406
+        },
+        {
+            "x": 7.395994092884954,
+            "y": 2.780007432926005,
+            "heading": 3.061424395012115,
+            "angularVelocity": -0.616760325454245,
+            "velocityX": -1.8341646375480058,
+            "velocityY": 1.2725074338705782,
+            "timestamp": 1.3459205144987312
+        },
+        {
+            "x": 7.191491634258091,
+            "y": 2.8782237282133547,
+            "heading": 2.9936811867966338,
+            "angularVelocity": -0.8307959955948208,
+            "velocityX": -2.5079978966461463,
+            "velocityY": 1.2045149170871063,
+            "timestamp": 1.4274606381568624
+        },
+        {
+            "x": 6.932297950713545,
+            "y": 2.9674313510808874,
+            "heading": 2.9221270442336884,
+            "angularVelocity": -0.8775329169593383,
+            "velocityX": -3.178725661874757,
+            "velocityY": 1.0940334508388325,
+            "timestamp": 1.5090007618149937
+        },
+        {
+            "x": 6.629938780171593,
+            "y": 3.0284835582667458,
+            "heading": 2.9221269432503374,
+            "angularVelocity": -0.0000012384498122465325,
+            "velocityX": -3.7081029188726404,
+            "velocityY": 0.7487382217106987,
+            "timestamp": 1.5905408854731249
+        },
+        {
+            "x": 6.3215346451204795,
+            "y": 3.034428801743739,
+            "heading": 2.922126920201386,
+            "angularVelocity": -2.8267005270994496e-7,
+            "velocityX": -3.7822377648596976,
+            "velocityY": 0.07291187712592015,
+            "timestamp": 1.6720810091312561
+        },
+        {
+            "x": 6.01312270862256,
+            "y": 3.0289030469499765,
+            "heading": 2.9221268973831047,
+            "angularVelocity": -2.7984114359464816e-7,
+            "velocityX": -3.7823334410305938,
+            "velocityY": -0.06776730946506612,
+            "timestamp": 1.7536211327893874
+        },
+        {
+            "x": 5.704710773369897,
+            "y": 3.023377222654451,
+            "heading": 2.9221268745648237,
+            "angularVelocity": -2.7984113775105834e-7,
+            "velocityX": -3.782333425758887,
+            "velocityY": -0.06776816182780301,
+            "timestamp": 1.8351612564475186
+        },
+        {
+            "x": 5.396298838117239,
+            "y": 3.0178513983586757,
+            "heading": 2.922126851746543,
+            "angularVelocity": -2.7984113682317286e-7,
+            "velocityX": -3.7823334257588326,
+            "velocityY": -0.06776816183086862,
+            "timestamp": 1.9167013801056498
+        },
+        {
+            "x": 5.087886902401733,
+            "y": 3.012325599899855,
+            "heading": 2.922126828928091,
+            "angularVelocity": -2.798432358955337e-7,
+            "velocityX": -3.7823334314351498,
+            "velocityY": -0.06776784496901485,
+            "timestamp": 1.998241503763781
+        },
+        {
+            "x": 4.812446473600025,
+            "y": 3.0084984766412206,
+            "heading": 2.8330493298750046,
+            "angularVelocity": -1.0924376252672112,
+            "velocityX": -3.3779741364697116,
+            "velocityY": -0.04693546056758059,
+            "timestamp": 2.0797816274219123
+        },
+        {
+            "x": 4.592410263299027,
+            "y": 3.0051727297391726,
+            "heading": 2.737251875069825,
+            "angularVelocity": -1.1748504969997702,
+            "velocityX": -2.6985022885608188,
+            "velocityY": -0.040786630591711566,
+            "timestamp": 2.1613217510800435
+        },
+        {
+            "x": 4.427608751414137,
+            "y": 3.0026089111743004,
+            "heading": 2.658186226973151,
+            "angularVelocity": -0.9696532768109137,
+            "velocityX": -2.0211094181785265,
+            "velocityY": -0.03144241693353635,
+            "timestamp": 2.2428618747381748
+        },
+        {
+            "x": 4.3178442485460256,
+            "y": 3.0008747293604148,
+            "heading": 2.602696579769804,
+            "angularVelocity": -0.6805195370563186,
+            "velocityX": -1.346140991008498,
+            "velocityY": -0.021267833994898407,
+            "timestamp": 2.324401998396306
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -0.3519320119027408,
+            "velocityX": -0.6726044318496349,
+            "velocityY": -0.010727594234238386,
+            "timestamp": 2.4059421220544372
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": -1.5467932880075213e-32,
+            "velocityY": 0,
+            "timestamp": 2.4874822457125685
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S4.1.traj
new file mode 100644
index 00000000..7a429afb
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S4.1.traj	
@@ -0,0 +1,283 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.2208311166528825,
+            "y": 1.6350333822394376,
+            "heading": 2.5207239283142147,
+            "angularVelocity": 0.274168379737783,
+            "velocityX": 0.5930957889931558,
+            "velocityY": 0.1988849791445399,
+            "timestamp": 0.07558832398555491
+        },
+        {
+            "x": 5.31056444260237,
+            "y": 1.6651195189308639,
+            "heading": 2.5599126898589697,
+            "angularVelocity": 0.5184499335141133,
+            "velocityX": 1.1871321021304377,
+            "velocityY": 0.39802624406880477,
+            "timestamp": 0.15117664797110983
+        },
+        {
+            "x": 5.445292613790804,
+            "y": 1.7102861706342203,
+            "heading": 2.6140890806736206,
+            "angularVelocity": 0.716729621164803,
+            "velocityX": 1.7823939476973087,
+            "velocityY": 0.5975347688882264,
+            "timestamp": 0.22676497195666473
+        },
+        {
+            "x": 5.625130316677607,
+            "y": 1.7705705454307188,
+            "heading": 2.677161642931129,
+            "angularVelocity": 0.834422023559608,
+            "velocityX": 2.379173044254461,
+            "velocityY": 0.79753554011886,
+            "timestamp": 0.30235329594221966
+        },
+        {
+            "x": 5.850123959727638,
+            "y": 1.8459876978345418,
+            "heading": 2.735754513087534,
+            "angularVelocity": 0.7751576839777234,
+            "velocityX": 2.97656610421776,
+            "velocityY": 0.9977354758948757,
+            "timestamp": 0.3779416199277746
+        },
+        {
+            "x": 6.118241955907835,
+            "y": 1.9356767665379448,
+            "heading": 2.743662980850177,
+            "angularVelocity": 0.10462552079005556,
+            "velocityX": 3.547082168820654,
+            "velocityY": 1.1865465983945254,
+            "timestamp": 0.4535299439133295
+        },
+        {
+            "x": 6.3893659801779465,
+            "y": 2.026544533897615,
+            "heading": 2.7436630046141004,
+            "angularVelocity": 3.143861609924561e-7,
+            "velocityX": 3.5868505871611136,
+            "velocityY": 1.2021402588187382,
+            "timestamp": 0.5291182678988844
+        },
+        {
+            "x": 6.660490004070523,
+            "y": 2.1174123023837605,
+            "heading": 2.7436630283780157,
+            "angularVelocity": 3.1438606173533106e-7,
+            "velocityX": 3.5868505821664702,
+            "velocityY": 1.2021402737215172,
+            "timestamp": 0.6047065918844393
+        },
+        {
+            "x": 6.931614067804288,
+            "y": 2.2082799519937164,
+            "heading": 2.743663052142803,
+            "angularVelocity": 3.1439759977679495e-7,
+            "velocityX": 3.5868511092477546,
+            "velocityY": 1.2021387010422189,
+            "timestamp": 0.6802949158699942
+        },
+        {
+            "x": 7.168848548798668,
+            "y": 2.287192936383632,
+            "heading": 2.8325335746925835,
+            "angularVelocity": 1.1757175958389672,
+            "velocityX": 3.1385069609389844,
+            "velocityY": 1.0439837825349283,
+            "timestamp": 0.7558832398555492
+        },
+        {
+            "x": 7.360921839643199,
+            "y": 2.3511354933788766,
+            "heading": 2.9301346596996316,
+            "angularVelocity": 1.2912190648082638,
+            "velocityX": 2.541044445981228,
+            "velocityY": 0.8459316680637452,
+            "timestamp": 0.8314715638411041
+        },
+        {
+            "x": 7.507932695990372,
+            "y": 2.4000526423462776,
+            "heading": 3.014739800319359,
+            "angularVelocity": 1.119288484765134,
+            "velocityX": 1.944888424504045,
+            "velocityY": 0.6471521842017212,
+            "timestamp": 0.907059887826659
+        },
+        {
+            "x": 7.6100514108149255,
+            "y": 2.4339833202352414,
+            "heading": 3.0798385166063986,
+            "angularVelocity": 0.8612271426930879,
+            "velocityX": 1.3509853035511394,
+            "velocityY": 0.4488878189103197,
+            "timestamp": 0.982648211812214
+        },
+        {
+            "x": 7.667383998050378,
+            "y": 2.452958398052505,
+            "heading": 3.1222248732691185,
+            "angularVelocity": 0.5607526986683374,
+            "velocityX": 0.7584846999174467,
+            "velocityY": 0.25103186334555994,
+            "timestamp": 1.0582365357977688
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 0.23515704269716123,
+            "velocityX": 0.16690411011130135,
+            "velocityY": 0.05346860115942382,
+            "timestamp": 1.1338248597833236
+        },
+        {
+            "x": 7.644913557520251,
+            "y": 2.4451050214750283,
+            "heading": 3.130617959515128,
+            "angularVelocity": -0.11950075188579921,
+            "velocityX": -0.44690238377135916,
+            "velocityY": -0.1515084996373644,
+            "timestamp": 1.212335164838424
+        },
+        {
+            "x": 7.561539712456839,
+            "y": 2.4170767124271264,
+            "heading": 3.096030936228231,
+            "angularVelocity": -0.44054119089994853,
+            "velocityX": -1.0619477915019166,
+            "velocityY": -0.3570016576579214,
+            "timestamp": 1.2908454698935243
+        },
+        {
+            "x": 7.429747941085825,
+            "y": 2.3728628641773573,
+            "heading": 3.040236463304404,
+            "angularVelocity": -0.7106643246980755,
+            "velocityX": -1.6786557035858298,
+            "velocityY": -0.5631598070945942,
+            "timestamp": 1.3693557749486247
+        },
+        {
+            "x": 7.249364854543017,
+            "y": 2.3124007511137603,
+            "heading": 2.9700149395216235,
+            "angularVelocity": -0.8944242890598059,
+            "velocityX": -2.2975721010917782,
+            "velocityY": -0.7701169040314122,
+            "timestamp": 1.447866080003725
+        },
+        {
+            "x": 7.020237112848181,
+            "y": 2.2356470641616397,
+            "heading": 2.9000211501626434,
+            "angularVelocity": -0.8915235943849231,
+            "velocityX": -2.9184416177471677,
+            "velocityY": -0.977625636510412,
+            "timestamp": 1.5263763850588254
+        },
+        {
+            "x": 6.744411982227483,
+            "y": 2.1437703180815415,
+            "heading": 2.8843371221032816,
+            "angularVelocity": -0.19977031102303894,
+            "velocityX": -3.5132347330342664,
+            "velocityY": -1.1702507844749341,
+            "timestamp": 1.6048866901139258
+        },
+        {
+            "x": 6.462777076833342,
+            "y": 2.049479989397194,
+            "heading": 2.8843370939243,
+            "angularVelocity": -3.589208009765681e-7,
+            "velocityX": -3.5872348884198493,
+            "velocityY": -1.2009930240135072,
+            "timestamp": 1.6833969951690262
+        },
+        {
+            "x": 6.181142164450445,
+            "y": 1.955189681587478,
+            "heading": 2.884337065745333,
+            "angularVelocity": -3.589206129086503e-7,
+            "velocityX": -3.5872349774369066,
+            "velocityY": -1.200992758129539,
+            "timestamp": 1.7619073002241266
+        },
+        {
+            "x": 5.898328744228412,
+            "y": 1.8644954419041788,
+            "heading": 2.8843369910470673,
+            "angularVelocity": -9.514453601861041e-7,
+            "velocityX": -3.6022458456064737,
+            "velocityY": -1.1551889859509814,
+            "timestamp": 1.840417605279227
+        },
+        {
+            "x": 5.657030939387785,
+            "y": 1.782595874715138,
+            "heading": 2.7806821628594123,
+            "angularVelocity": -1.3202703532347235,
+            "velocityX": -3.0734539201099995,
+            "velocityY": -1.043169646730562,
+            "timestamp": 1.9189279103343273
+        },
+        {
+            "x": 5.464337892157779,
+            "y": 1.7174002123662173,
+            "heading": 2.6765644069162446,
+            "angularVelocity": -1.3261667480478392,
+            "velocityX": -2.4543663038217995,
+            "velocityY": -0.8304089801098533,
+            "timestamp": 1.9974382153894277
+        },
+        {
+            "x": 5.320057899798496,
+            "y": 1.6686446362166114,
+            "heading": 2.5910880570386063,
+            "angularVelocity": -1.0887277767886236,
+            "velocityX": -1.8377204401132547,
+            "velocityY": -0.6210086193829906,
+            "timestamp": 2.075948520444528
+        },
+        {
+            "x": 5.223989702910862,
+            "y": 1.6362005448482675,
+            "heading": 2.5310608538348376,
+            "angularVelocity": -0.7645773782389272,
+            "velocityX": -1.2236380538862024,
+            "velocityY": -0.41324627825065113,
+            "timestamp": 2.154458825499628
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.3956277308187441,
+            "velocityX": -0.6112535529849511,
+            "velocityY": -0.2063492790774072,
+            "timestamp": 2.2329691305547286
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 3.766832812159788e-31,
+            "velocityX": 0,
+            "velocityY": 1.2430862122144045e-32,
+            "timestamp": 2.311479435609829
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S4.traj
new file mode 100644
index 00000000..7a429afb
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S4.traj	
@@ -0,0 +1,283 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.2208311166528825,
+            "y": 1.6350333822394376,
+            "heading": 2.5207239283142147,
+            "angularVelocity": 0.274168379737783,
+            "velocityX": 0.5930957889931558,
+            "velocityY": 0.1988849791445399,
+            "timestamp": 0.07558832398555491
+        },
+        {
+            "x": 5.31056444260237,
+            "y": 1.6651195189308639,
+            "heading": 2.5599126898589697,
+            "angularVelocity": 0.5184499335141133,
+            "velocityX": 1.1871321021304377,
+            "velocityY": 0.39802624406880477,
+            "timestamp": 0.15117664797110983
+        },
+        {
+            "x": 5.445292613790804,
+            "y": 1.7102861706342203,
+            "heading": 2.6140890806736206,
+            "angularVelocity": 0.716729621164803,
+            "velocityX": 1.7823939476973087,
+            "velocityY": 0.5975347688882264,
+            "timestamp": 0.22676497195666473
+        },
+        {
+            "x": 5.625130316677607,
+            "y": 1.7705705454307188,
+            "heading": 2.677161642931129,
+            "angularVelocity": 0.834422023559608,
+            "velocityX": 2.379173044254461,
+            "velocityY": 0.79753554011886,
+            "timestamp": 0.30235329594221966
+        },
+        {
+            "x": 5.850123959727638,
+            "y": 1.8459876978345418,
+            "heading": 2.735754513087534,
+            "angularVelocity": 0.7751576839777234,
+            "velocityX": 2.97656610421776,
+            "velocityY": 0.9977354758948757,
+            "timestamp": 0.3779416199277746
+        },
+        {
+            "x": 6.118241955907835,
+            "y": 1.9356767665379448,
+            "heading": 2.743662980850177,
+            "angularVelocity": 0.10462552079005556,
+            "velocityX": 3.547082168820654,
+            "velocityY": 1.1865465983945254,
+            "timestamp": 0.4535299439133295
+        },
+        {
+            "x": 6.3893659801779465,
+            "y": 2.026544533897615,
+            "heading": 2.7436630046141004,
+            "angularVelocity": 3.143861609924561e-7,
+            "velocityX": 3.5868505871611136,
+            "velocityY": 1.2021402588187382,
+            "timestamp": 0.5291182678988844
+        },
+        {
+            "x": 6.660490004070523,
+            "y": 2.1174123023837605,
+            "heading": 2.7436630283780157,
+            "angularVelocity": 3.1438606173533106e-7,
+            "velocityX": 3.5868505821664702,
+            "velocityY": 1.2021402737215172,
+            "timestamp": 0.6047065918844393
+        },
+        {
+            "x": 6.931614067804288,
+            "y": 2.2082799519937164,
+            "heading": 2.743663052142803,
+            "angularVelocity": 3.1439759977679495e-7,
+            "velocityX": 3.5868511092477546,
+            "velocityY": 1.2021387010422189,
+            "timestamp": 0.6802949158699942
+        },
+        {
+            "x": 7.168848548798668,
+            "y": 2.287192936383632,
+            "heading": 2.8325335746925835,
+            "angularVelocity": 1.1757175958389672,
+            "velocityX": 3.1385069609389844,
+            "velocityY": 1.0439837825349283,
+            "timestamp": 0.7558832398555492
+        },
+        {
+            "x": 7.360921839643199,
+            "y": 2.3511354933788766,
+            "heading": 2.9301346596996316,
+            "angularVelocity": 1.2912190648082638,
+            "velocityX": 2.541044445981228,
+            "velocityY": 0.8459316680637452,
+            "timestamp": 0.8314715638411041
+        },
+        {
+            "x": 7.507932695990372,
+            "y": 2.4000526423462776,
+            "heading": 3.014739800319359,
+            "angularVelocity": 1.119288484765134,
+            "velocityX": 1.944888424504045,
+            "velocityY": 0.6471521842017212,
+            "timestamp": 0.907059887826659
+        },
+        {
+            "x": 7.6100514108149255,
+            "y": 2.4339833202352414,
+            "heading": 3.0798385166063986,
+            "angularVelocity": 0.8612271426930879,
+            "velocityX": 1.3509853035511394,
+            "velocityY": 0.4488878189103197,
+            "timestamp": 0.982648211812214
+        },
+        {
+            "x": 7.667383998050378,
+            "y": 2.452958398052505,
+            "heading": 3.1222248732691185,
+            "angularVelocity": 0.5607526986683374,
+            "velocityX": 0.7584846999174467,
+            "velocityY": 0.25103186334555994,
+            "timestamp": 1.0582365357977688
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 0.23515704269716123,
+            "velocityX": 0.16690411011130135,
+            "velocityY": 0.05346860115942382,
+            "timestamp": 1.1338248597833236
+        },
+        {
+            "x": 7.644913557520251,
+            "y": 2.4451050214750283,
+            "heading": 3.130617959515128,
+            "angularVelocity": -0.11950075188579921,
+            "velocityX": -0.44690238377135916,
+            "velocityY": -0.1515084996373644,
+            "timestamp": 1.212335164838424
+        },
+        {
+            "x": 7.561539712456839,
+            "y": 2.4170767124271264,
+            "heading": 3.096030936228231,
+            "angularVelocity": -0.44054119089994853,
+            "velocityX": -1.0619477915019166,
+            "velocityY": -0.3570016576579214,
+            "timestamp": 1.2908454698935243
+        },
+        {
+            "x": 7.429747941085825,
+            "y": 2.3728628641773573,
+            "heading": 3.040236463304404,
+            "angularVelocity": -0.7106643246980755,
+            "velocityX": -1.6786557035858298,
+            "velocityY": -0.5631598070945942,
+            "timestamp": 1.3693557749486247
+        },
+        {
+            "x": 7.249364854543017,
+            "y": 2.3124007511137603,
+            "heading": 2.9700149395216235,
+            "angularVelocity": -0.8944242890598059,
+            "velocityX": -2.2975721010917782,
+            "velocityY": -0.7701169040314122,
+            "timestamp": 1.447866080003725
+        },
+        {
+            "x": 7.020237112848181,
+            "y": 2.2356470641616397,
+            "heading": 2.9000211501626434,
+            "angularVelocity": -0.8915235943849231,
+            "velocityX": -2.9184416177471677,
+            "velocityY": -0.977625636510412,
+            "timestamp": 1.5263763850588254
+        },
+        {
+            "x": 6.744411982227483,
+            "y": 2.1437703180815415,
+            "heading": 2.8843371221032816,
+            "angularVelocity": -0.19977031102303894,
+            "velocityX": -3.5132347330342664,
+            "velocityY": -1.1702507844749341,
+            "timestamp": 1.6048866901139258
+        },
+        {
+            "x": 6.462777076833342,
+            "y": 2.049479989397194,
+            "heading": 2.8843370939243,
+            "angularVelocity": -3.589208009765681e-7,
+            "velocityX": -3.5872348884198493,
+            "velocityY": -1.2009930240135072,
+            "timestamp": 1.6833969951690262
+        },
+        {
+            "x": 6.181142164450445,
+            "y": 1.955189681587478,
+            "heading": 2.884337065745333,
+            "angularVelocity": -3.589206129086503e-7,
+            "velocityX": -3.5872349774369066,
+            "velocityY": -1.200992758129539,
+            "timestamp": 1.7619073002241266
+        },
+        {
+            "x": 5.898328744228412,
+            "y": 1.8644954419041788,
+            "heading": 2.8843369910470673,
+            "angularVelocity": -9.514453601861041e-7,
+            "velocityX": -3.6022458456064737,
+            "velocityY": -1.1551889859509814,
+            "timestamp": 1.840417605279227
+        },
+        {
+            "x": 5.657030939387785,
+            "y": 1.782595874715138,
+            "heading": 2.7806821628594123,
+            "angularVelocity": -1.3202703532347235,
+            "velocityX": -3.0734539201099995,
+            "velocityY": -1.043169646730562,
+            "timestamp": 1.9189279103343273
+        },
+        {
+            "x": 5.464337892157779,
+            "y": 1.7174002123662173,
+            "heading": 2.6765644069162446,
+            "angularVelocity": -1.3261667480478392,
+            "velocityX": -2.4543663038217995,
+            "velocityY": -0.8304089801098533,
+            "timestamp": 1.9974382153894277
+        },
+        {
+            "x": 5.320057899798496,
+            "y": 1.6686446362166114,
+            "heading": 2.5910880570386063,
+            "angularVelocity": -1.0887277767886236,
+            "velocityX": -1.8377204401132547,
+            "velocityY": -0.6210086193829906,
+            "timestamp": 2.075948520444528
+        },
+        {
+            "x": 5.223989702910862,
+            "y": 1.6362005448482675,
+            "heading": 2.5310608538348376,
+            "angularVelocity": -0.7645773782389272,
+            "velocityX": -1.2236380538862024,
+            "velocityY": -0.41324627825065113,
+            "timestamp": 2.154458825499628
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.3956277308187441,
+            "velocityX": -0.6112535529849511,
+            "velocityY": -0.2063492790774072,
+            "timestamp": 2.2329691305547286
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 3.766832812159788e-31,
+            "velocityX": 0,
+            "velocityY": 1.2430862122144045e-32,
+            "timestamp": 2.311479435609829
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S3.1.traj
new file mode 100644
index 00000000..19b7b304
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S3.1.traj	
@@ -0,0 +1,319 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.21689679817953,
+            "y": 1.6000159633135866,
+            "heading": 2.520043259980684,
+            "angularVelocity": 0.2702645151942541,
+            "velocityX": 0.5514548702977145,
+            "velocityY": -0.2694659447557082,
+            "timestamp": 0.0741616411102716
+        },
+        {
+            "x": 5.298748395600195,
+            "y": 1.5600344658293337,
+            "heading": 2.558338172376498,
+            "angularVelocity": 0.5163708869233576,
+            "velocityX": 1.1036918303757857,
+            "velocityY": -0.5391129010319948,
+            "timestamp": 0.1483232822205432
+        },
+        {
+            "x": 5.421633479629326,
+            "y": 1.5000375028373136,
+            "heading": 2.612131147144223,
+            "angularVelocity": 0.7253476859785596,
+            "velocityX": 1.656989815616613,
+            "velocityY": -0.8090026338927551,
+            "timestamp": 0.2224849233308148
+        },
+        {
+            "x": 5.585659490393147,
+            "y": 1.420004250322953,
+            "heading": 2.6766780389547544,
+            "angularVelocity": 0.8703541459463568,
+            "velocityX": 2.211736529939155,
+            "velocityY": -1.0791731590102869,
+            "timestamp": 0.2966465644410864
+        },
+        {
+            "x": 5.790940733567372,
+            "y": 1.3199448136082716,
+            "heading": 2.7418726481321705,
+            "angularVelocity": 0.8790880056234819,
+            "velocityX": 2.7680245488229662,
+            "velocityY": -1.3492074233619007,
+            "timestamp": 0.37080820555135796
+        },
+        {
+            "x": 6.036365145395861,
+            "y": 1.2006925884246085,
+            "heading": 2.766396446394458,
+            "angularVelocity": 0.3306803611021658,
+            "velocityX": 3.3093174335714934,
+            "velocityY": -1.6080041298754981,
+            "timestamp": 0.44496984666162953
+        },
+        {
+            "x": 6.288321832391407,
+            "y": 1.0773004957458903,
+            "heading": 2.7663964696788734,
+            "angularVelocity": 3.1396844397951453e-7,
+            "velocityX": 3.3973990222372876,
+            "velocityY": -1.6638263505420645,
+            "timestamp": 0.5191314877719011
+        },
+        {
+            "x": 6.540278624834678,
+            "y": 0.9539086183831178,
+            "heading": 2.766396492963265,
+            "angularVelocity": 3.1396812929995246e-7,
+            "velocityX": 3.397400444100689,
+            "velocityY": -1.663823447208988,
+            "timestamp": 0.5932931288821727
+        },
+        {
+            "x": 6.801316019800969,
+            "y": 0.8511114616545457,
+            "heading": 2.7663965374148853,
+            "angularVelocity": 5.993883039125867e-7,
+            "velocityX": 3.519843831100697,
+            "velocityY": -1.3861230036120238,
+            "timestamp": 0.6674547699924442
+        },
+        {
+            "x": 7.047191253658378,
+            "y": 0.7842163440856953,
+            "heading": 2.8499152738316673,
+            "angularVelocity": 1.1261716322139188,
+            "velocityX": 3.3153963447464685,
+            "velocityY": -0.9020177623819695,
+            "timestamp": 0.7416164111027158
+        },
+        {
+            "x": 7.2541679092956945,
+            "y": 0.7415676707832506,
+            "heading": 2.93858588461799,
+            "angularVelocity": 1.1956398140448308,
+            "velocityX": 2.790885591778509,
+            "velocityY": -0.575077259132303,
+            "timestamp": 0.8157780522129874
+        },
+        {
+            "x": 7.421132571302565,
+            "y": 0.7211096612789919,
+            "heading": 3.016140310820358,
+            "angularVelocity": 1.045748516905977,
+            "velocityX": 2.251361478889076,
+            "velocityY": -0.2758570225521344,
+            "timestamp": 0.889939693323259
+        },
+        {
+            "x": 7.547824539966462,
+            "y": 0.722094092716002,
+            "heading": 3.0771627100726597,
+            "angularVelocity": 0.8228296776978581,
+            "velocityX": 1.7083220754986685,
+            "velocityY": 0.013274132317907437,
+            "timestamp": 0.9641013344335305
+        },
+        {
+            "x": 7.634133595568631,
+            "y": 0.7441357582804435,
+            "heading": 3.1189807676901022,
+            "angularVelocity": 0.5638771876052062,
+            "velocityX": 1.1637964628348345,
+            "velocityY": 0.29721113549347455,
+            "timestamp": 1.0382629755438022
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 0.28342458439730217,
+            "velocityX": 0.6184653379387841,
+            "velocityY": 0.5779839965491583,
+            "timestamp": 1.1124246166540739
+        },
+        {
+            "x": 7.681059166008756,
+            "y": 0.8599952012380814,
+            "heading": 3.136373480041968,
+            "angularVelocity": -0.044073969444744315,
+            "velocityX": 0.012872299297217518,
+            "velocityY": 0.8871282403539436,
+            "timestamp": 1.1947071975906878
+        },
+        {
+            "x": 7.6321422839644315,
+            "y": 0.9582569372062664,
+            "heading": 3.107236664084856,
+            "angularVelocity": -0.3541067334720519,
+            "velocityX": -0.5944986349176513,
+            "velocityY": 1.1941985150402432,
+            "timestamp": 1.2769897785273017
+        },
+        {
+            "x": 7.533021561059945,
+            "y": 1.0815072200967695,
+            "heading": 3.0548999267391683,
+            "angularVelocity": -0.6360609590747484,
+            "velocityX": -1.204637989914761,
+            "velocityY": 1.4978903370258465,
+            "timestamp": 1.3592723594639156
+        },
+        {
+            "x": 7.383292536414222,
+            "y": 1.229213843710233,
+            "heading": 2.9837724589347805,
+            "angularVelocity": -0.8644292266327552,
+            "velocityX": -1.819692855296544,
+            "velocityY": 1.7951141290432664,
+            "timestamp": 1.4415549404005294
+        },
+        {
+            "x": 7.182069766586899,
+            "y": 1.399958151778736,
+            "heading": 2.9050185961423023,
+            "angularVelocity": -0.9571146395266165,
+            "velocityX": -2.445508727811161,
+            "velocityY": 2.0750966501650514,
+            "timestamp": 1.5238375213371433
+        },
+        {
+            "x": 6.9296160896416765,
+            "y": 1.5820466019910795,
+            "heading": 2.9050185123503516,
+            "angularVelocity": -0.0000010183437364658426,
+            "velocityX": -3.068130266109342,
+            "velocityY": 2.2129647385832873,
+            "timestamp": 1.6061201022737572
+        },
+        {
+            "x": 6.6549097990315635,
+            "y": 1.728421234696809,
+            "heading": 2.9050184910214054,
+            "angularVelocity": -2.59215813112527e-7,
+            "velocityX": -3.338571633062085,
+            "velocityY": 1.7789261231181734,
+            "timestamp": 1.6884026832103711
+        },
+        {
+            "x": 6.380203216821482,
+            "y": 1.8747953201455823,
+            "heading": 2.905018469692463,
+            "angularVelocity": -2.59215765603584e-7,
+            "velocityX": -3.338575176946638,
+            "velocityY": 1.7789194721727335,
+            "timestamp": 1.770685264146985
+        },
+        {
+            "x": 6.105496634609771,
+            "y": 2.0211694055912957,
+            "heading": 2.9050184483635206,
+            "angularVelocity": -2.592157647120513e-7,
+            "velocityX": -3.3385751769664513,
+            "velocityY": 1.7789194721355484,
+            "timestamp": 1.852967845083599
+        },
+        {
+            "x": 5.83079005239806,
+            "y": 2.167543491037009,
+            "heading": 2.905018427034578,
+            "angularVelocity": -2.592157660778243e-7,
+            "velocityX": -3.3385751769664513,
+            "velocityY": 1.778919472135548,
+            "timestamp": 1.9352504260202128
+        },
+        {
+            "x": 5.556083470186349,
+            "y": 2.3139175764827224,
+            "heading": 2.905018405705636,
+            "angularVelocity": -2.592157651075657e-7,
+            "velocityX": -3.338575176966452,
+            "velocityY": 1.7789194721355481,
+            "timestamp": 2.0175330069568265
+        },
+        {
+            "x": 5.281376887974587,
+            "y": 2.460291661928342,
+            "heading": 2.9050183843766937,
+            "angularVelocity": -2.5921576594087425e-7,
+            "velocityX": -3.3385751769670593,
+            "velocityY": 1.7789194721344066,
+            "timestamp": 2.09981558789344
+        },
+        {
+            "x": 5.006670296810042,
+            "y": 2.6066657305717675,
+            "heading": 2.905018363047624,
+            "angularVelocity": -2.5921731220751964e-7,
+            "velocityX": -3.338575285772395,
+            "velocityY": 1.778919267933304,
+            "timestamp": 2.182098168830054
+        },
+        {
+            "x": 4.758260828306463,
+            "y": 2.7376390899942815,
+            "heading": 2.815277658009563,
+            "angularVelocity": -1.0906403763293109,
+            "velocityX": -3.0189800280443744,
+            "velocityY": 1.591750743980768,
+            "timestamp": 2.2643807497666675
+        },
+        {
+            "x": 4.559939575601031,
+            "y": 2.842572976973571,
+            "heading": 2.72503756976798,
+            "angularVelocity": -1.0967095005333054,
+            "velocityX": -2.4102458922406798,
+            "velocityY": 1.2752867713292104,
+            "timestamp": 2.346663330703281
+        },
+        {
+            "x": 4.4113908880653625,
+            "y": 2.9212861863102546,
+            "heading": 2.651616705628037,
+            "angularVelocity": -0.8923014239977028,
+            "velocityX": -1.8053479344565475,
+            "velocityY": 0.9566205682988939,
+            "timestamp": 2.428945911639895
+        },
+        {
+            "x": 4.31244325419048,
+            "y": 2.973762547349605,
+            "heading": 2.6003949192739833,
+            "angularVelocity": -0.6225106914611183,
+            "velocityX": -1.202534397299805,
+            "velocityY": 0.6377578394116608,
+            "timestamp": 2.5112284925765085
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -0.3207838034920002,
+            "velocityX": -0.6008957622339189,
+            "velocityY": 0.31887007373537113,
+            "timestamp": 2.593511073513122
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": -9.140912570286281e-33,
+            "velocityY": 0,
+            "timestamp": 2.675793654449736
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S3.traj
new file mode 100644
index 00000000..19b7b304
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S3.traj	
@@ -0,0 +1,319 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.21689679817953,
+            "y": 1.6000159633135866,
+            "heading": 2.520043259980684,
+            "angularVelocity": 0.2702645151942541,
+            "velocityX": 0.5514548702977145,
+            "velocityY": -0.2694659447557082,
+            "timestamp": 0.0741616411102716
+        },
+        {
+            "x": 5.298748395600195,
+            "y": 1.5600344658293337,
+            "heading": 2.558338172376498,
+            "angularVelocity": 0.5163708869233576,
+            "velocityX": 1.1036918303757857,
+            "velocityY": -0.5391129010319948,
+            "timestamp": 0.1483232822205432
+        },
+        {
+            "x": 5.421633479629326,
+            "y": 1.5000375028373136,
+            "heading": 2.612131147144223,
+            "angularVelocity": 0.7253476859785596,
+            "velocityX": 1.656989815616613,
+            "velocityY": -0.8090026338927551,
+            "timestamp": 0.2224849233308148
+        },
+        {
+            "x": 5.585659490393147,
+            "y": 1.420004250322953,
+            "heading": 2.6766780389547544,
+            "angularVelocity": 0.8703541459463568,
+            "velocityX": 2.211736529939155,
+            "velocityY": -1.0791731590102869,
+            "timestamp": 0.2966465644410864
+        },
+        {
+            "x": 5.790940733567372,
+            "y": 1.3199448136082716,
+            "heading": 2.7418726481321705,
+            "angularVelocity": 0.8790880056234819,
+            "velocityX": 2.7680245488229662,
+            "velocityY": -1.3492074233619007,
+            "timestamp": 0.37080820555135796
+        },
+        {
+            "x": 6.036365145395861,
+            "y": 1.2006925884246085,
+            "heading": 2.766396446394458,
+            "angularVelocity": 0.3306803611021658,
+            "velocityX": 3.3093174335714934,
+            "velocityY": -1.6080041298754981,
+            "timestamp": 0.44496984666162953
+        },
+        {
+            "x": 6.288321832391407,
+            "y": 1.0773004957458903,
+            "heading": 2.7663964696788734,
+            "angularVelocity": 3.1396844397951453e-7,
+            "velocityX": 3.3973990222372876,
+            "velocityY": -1.6638263505420645,
+            "timestamp": 0.5191314877719011
+        },
+        {
+            "x": 6.540278624834678,
+            "y": 0.9539086183831178,
+            "heading": 2.766396492963265,
+            "angularVelocity": 3.1396812929995246e-7,
+            "velocityX": 3.397400444100689,
+            "velocityY": -1.663823447208988,
+            "timestamp": 0.5932931288821727
+        },
+        {
+            "x": 6.801316019800969,
+            "y": 0.8511114616545457,
+            "heading": 2.7663965374148853,
+            "angularVelocity": 5.993883039125867e-7,
+            "velocityX": 3.519843831100697,
+            "velocityY": -1.3861230036120238,
+            "timestamp": 0.6674547699924442
+        },
+        {
+            "x": 7.047191253658378,
+            "y": 0.7842163440856953,
+            "heading": 2.8499152738316673,
+            "angularVelocity": 1.1261716322139188,
+            "velocityX": 3.3153963447464685,
+            "velocityY": -0.9020177623819695,
+            "timestamp": 0.7416164111027158
+        },
+        {
+            "x": 7.2541679092956945,
+            "y": 0.7415676707832506,
+            "heading": 2.93858588461799,
+            "angularVelocity": 1.1956398140448308,
+            "velocityX": 2.790885591778509,
+            "velocityY": -0.575077259132303,
+            "timestamp": 0.8157780522129874
+        },
+        {
+            "x": 7.421132571302565,
+            "y": 0.7211096612789919,
+            "heading": 3.016140310820358,
+            "angularVelocity": 1.045748516905977,
+            "velocityX": 2.251361478889076,
+            "velocityY": -0.2758570225521344,
+            "timestamp": 0.889939693323259
+        },
+        {
+            "x": 7.547824539966462,
+            "y": 0.722094092716002,
+            "heading": 3.0771627100726597,
+            "angularVelocity": 0.8228296776978581,
+            "velocityX": 1.7083220754986685,
+            "velocityY": 0.013274132317907437,
+            "timestamp": 0.9641013344335305
+        },
+        {
+            "x": 7.634133595568631,
+            "y": 0.7441357582804435,
+            "heading": 3.1189807676901022,
+            "angularVelocity": 0.5638771876052062,
+            "velocityX": 1.1637964628348345,
+            "velocityY": 0.29721113549347455,
+            "timestamp": 1.0382629755438022
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 0.28342458439730217,
+            "velocityX": 0.6184653379387841,
+            "velocityY": 0.5779839965491583,
+            "timestamp": 1.1124246166540739
+        },
+        {
+            "x": 7.681059166008756,
+            "y": 0.8599952012380814,
+            "heading": 3.136373480041968,
+            "angularVelocity": -0.044073969444744315,
+            "velocityX": 0.012872299297217518,
+            "velocityY": 0.8871282403539436,
+            "timestamp": 1.1947071975906878
+        },
+        {
+            "x": 7.6321422839644315,
+            "y": 0.9582569372062664,
+            "heading": 3.107236664084856,
+            "angularVelocity": -0.3541067334720519,
+            "velocityX": -0.5944986349176513,
+            "velocityY": 1.1941985150402432,
+            "timestamp": 1.2769897785273017
+        },
+        {
+            "x": 7.533021561059945,
+            "y": 1.0815072200967695,
+            "heading": 3.0548999267391683,
+            "angularVelocity": -0.6360609590747484,
+            "velocityX": -1.204637989914761,
+            "velocityY": 1.4978903370258465,
+            "timestamp": 1.3592723594639156
+        },
+        {
+            "x": 7.383292536414222,
+            "y": 1.229213843710233,
+            "heading": 2.9837724589347805,
+            "angularVelocity": -0.8644292266327552,
+            "velocityX": -1.819692855296544,
+            "velocityY": 1.7951141290432664,
+            "timestamp": 1.4415549404005294
+        },
+        {
+            "x": 7.182069766586899,
+            "y": 1.399958151778736,
+            "heading": 2.9050185961423023,
+            "angularVelocity": -0.9571146395266165,
+            "velocityX": -2.445508727811161,
+            "velocityY": 2.0750966501650514,
+            "timestamp": 1.5238375213371433
+        },
+        {
+            "x": 6.9296160896416765,
+            "y": 1.5820466019910795,
+            "heading": 2.9050185123503516,
+            "angularVelocity": -0.0000010183437364658426,
+            "velocityX": -3.068130266109342,
+            "velocityY": 2.2129647385832873,
+            "timestamp": 1.6061201022737572
+        },
+        {
+            "x": 6.6549097990315635,
+            "y": 1.728421234696809,
+            "heading": 2.9050184910214054,
+            "angularVelocity": -2.59215813112527e-7,
+            "velocityX": -3.338571633062085,
+            "velocityY": 1.7789261231181734,
+            "timestamp": 1.6884026832103711
+        },
+        {
+            "x": 6.380203216821482,
+            "y": 1.8747953201455823,
+            "heading": 2.905018469692463,
+            "angularVelocity": -2.59215765603584e-7,
+            "velocityX": -3.338575176946638,
+            "velocityY": 1.7789194721727335,
+            "timestamp": 1.770685264146985
+        },
+        {
+            "x": 6.105496634609771,
+            "y": 2.0211694055912957,
+            "heading": 2.9050184483635206,
+            "angularVelocity": -2.592157647120513e-7,
+            "velocityX": -3.3385751769664513,
+            "velocityY": 1.7789194721355484,
+            "timestamp": 1.852967845083599
+        },
+        {
+            "x": 5.83079005239806,
+            "y": 2.167543491037009,
+            "heading": 2.905018427034578,
+            "angularVelocity": -2.592157660778243e-7,
+            "velocityX": -3.3385751769664513,
+            "velocityY": 1.778919472135548,
+            "timestamp": 1.9352504260202128
+        },
+        {
+            "x": 5.556083470186349,
+            "y": 2.3139175764827224,
+            "heading": 2.905018405705636,
+            "angularVelocity": -2.592157651075657e-7,
+            "velocityX": -3.338575176966452,
+            "velocityY": 1.7789194721355481,
+            "timestamp": 2.0175330069568265
+        },
+        {
+            "x": 5.281376887974587,
+            "y": 2.460291661928342,
+            "heading": 2.9050183843766937,
+            "angularVelocity": -2.5921576594087425e-7,
+            "velocityX": -3.3385751769670593,
+            "velocityY": 1.7789194721344066,
+            "timestamp": 2.09981558789344
+        },
+        {
+            "x": 5.006670296810042,
+            "y": 2.6066657305717675,
+            "heading": 2.905018363047624,
+            "angularVelocity": -2.5921731220751964e-7,
+            "velocityX": -3.338575285772395,
+            "velocityY": 1.778919267933304,
+            "timestamp": 2.182098168830054
+        },
+        {
+            "x": 4.758260828306463,
+            "y": 2.7376390899942815,
+            "heading": 2.815277658009563,
+            "angularVelocity": -1.0906403763293109,
+            "velocityX": -3.0189800280443744,
+            "velocityY": 1.591750743980768,
+            "timestamp": 2.2643807497666675
+        },
+        {
+            "x": 4.559939575601031,
+            "y": 2.842572976973571,
+            "heading": 2.72503756976798,
+            "angularVelocity": -1.0967095005333054,
+            "velocityX": -2.4102458922406798,
+            "velocityY": 1.2752867713292104,
+            "timestamp": 2.346663330703281
+        },
+        {
+            "x": 4.4113908880653625,
+            "y": 2.9212861863102546,
+            "heading": 2.651616705628037,
+            "angularVelocity": -0.8923014239977028,
+            "velocityX": -1.8053479344565475,
+            "velocityY": 0.9566205682988939,
+            "timestamp": 2.428945911639895
+        },
+        {
+            "x": 4.31244325419048,
+            "y": 2.973762547349605,
+            "heading": 2.6003949192739833,
+            "angularVelocity": -0.6225106914611183,
+            "velocityX": -1.202534397299805,
+            "velocityY": 0.6377578394116608,
+            "timestamp": 2.5112284925765085
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -0.3207838034920002,
+            "velocityX": -0.6008957622339189,
+            "velocityY": 0.31887007373537113,
+            "timestamp": 2.593511073513122
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": -9.140912570286281e-33,
+            "velocityY": 0,
+            "timestamp": 2.675793654449736
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S4.1.traj
new file mode 100644
index 00000000..1dfddc31
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S4.1.traj	
@@ -0,0 +1,283 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.220564813130112,
+            "y": 1.6051202900758712,
+            "heading": 2.517168506154745,
+            "angularVelocity": 0.22812300403374605,
+            "velocityX": 0.5921458136084756,
+            "velocityY": -0.19771109358311004,
+            "timestamp": 0.07525986354364067
+        },
+        {
+            "x": 5.309738195223409,
+            "y": 1.5753445036804834,
+            "heading": 2.5497776530467964,
+            "angularVelocity": 0.433287350742303,
+            "velocityX": 1.1848730238739802,
+            "velocityY": -0.3956396543042072,
+            "timestamp": 0.15051972708728134
+        },
+        {
+            "x": 5.443577884042542,
+            "y": 1.5306492621981627,
+            "heading": 2.5951663634005544,
+            "angularVelocity": 0.6030931789749864,
+            "velocityX": 1.7783674128179001,
+            "velocityY": -0.593878853585797,
+            "timestamp": 0.225779590630922
+        },
+        {
+            "x": 5.622156845349364,
+            "y": 1.4710006417461063,
+            "heading": 2.6487285344970175,
+            "angularVelocity": 0.7116963621041407,
+            "velocityX": 2.372831319356166,
+            "velocityY": -0.7925688095018658,
+            "timestamp": 0.3010394541745627
+        },
+        {
+            "x": 5.845518423751245,
+            "y": 1.3963607866160492,
+            "heading": 2.700604108231769,
+            "angularVelocity": 0.6892860456047826,
+            "velocityX": 2.967871158474283,
+            "velocityY": -0.9917617653767852,
+            "timestamp": 0.37629931771820335
+        },
+        {
+            "x": 6.112314775127604,
+            "y": 1.3070273852513161,
+            "heading": 2.7110861192214117,
+            "angularVelocity": 0.13927757101984,
+            "velocityX": 3.5450017953016038,
+            "velocityY": -1.186999247121034,
+            "timestamp": 0.451559181261844
+        },
+        {
+            "x": 6.382409273163311,
+            "y": 1.216999105128909,
+            "heading": 2.7110861382363836,
+            "angularVelocity": 2.526575375097703e-7,
+            "velocityX": 3.588825242542295,
+            "velocityY": -1.196232306084407,
+            "timestamp": 0.5268190448054847
+        },
+        {
+            "x": 6.652503772105593,
+            "y": 1.1269708277263142,
+            "heading": 2.711086157251347,
+            "angularVelocity": 2.5265742250097044e-7,
+            "velocityX": 3.5888252545882353,
+            "velocityY": -1.19623226994546,
+            "timestamp": 0.6020789083491254
+        },
+        {
+            "x": 6.9225982993416295,
+            "y": 1.0369426352093585,
+            "heading": 2.7110861762673086,
+            "angularVelocity": 2.5267068695978854e-7,
+            "velocityX": 3.5888256305356965,
+            "velocityY": -1.19623114204494,
+            "timestamp": 0.677338771892766
+        },
+        {
+            "x": 7.1604160781282875,
+            "y": 0.9586172665469176,
+            "heading": 2.823441998219159,
+            "angularVelocity": 1.4929049384563269,
+            "velocityX": 3.1599549559208913,
+            "velocityY": -1.040732270488674,
+            "timestamp": 0.7525986354364067
+        },
+        {
+            "x": 7.353386220004786,
+            "y": 0.8948816690648573,
+            "heading": 2.9268528477366496,
+            "angularVelocity": 1.3740504519719978,
+            "velocityX": 2.5640511793461145,
+            "velocityY": -0.8468736784926819,
+            "timestamp": 0.8278584989800474
+        },
+        {
+            "x": 7.501750789212216,
+            "y": 0.8458382793290063,
+            "heading": 3.012370231908866,
+            "angularVelocity": 1.136294701393235,
+            "velocityX": 1.9713637817240808,
+            "velocityY": -0.6516539816393957,
+            "timestamp": 0.903118362523688
+        },
+        {
+            "x": 7.605609860307225,
+            "y": 0.8115063107648604,
+            "heading": 3.0771995592704795,
+            "angularVelocity": 0.8614063899281612,
+            "velocityX": 1.3800061042468479,
+            "velocityY": -0.45617899033577186,
+            "timestamp": 0.9783782260673287
+        },
+        {
+            "x": 7.665015516622623,
+            "y": 0.7918924721730815,
+            "heading": 3.1200072835634023,
+            "angularVelocity": 0.5687988560874772,
+            "velocityX": 0.7893404733713316,
+            "velocityY": -0.2606148572194197,
+            "timestamp": 1.0536380896109694
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 0.26564911886937803,
+            "velocityX": 0.19910324935265278,
+            "velocityY": -0.06500772048629082,
+            "timestamp": 1.1288979531546102
+        },
+        {
+            "x": 7.647079450231064,
+            "y": 0.7979971975497204,
+            "heading": 3.1353264592199808,
+            "angularVelocity": -0.059354936296623806,
+            "velocityX": -0.418097803433937,
+            "velocityY": 0.13966668757779366,
+            "timestamp": 1.2076368259698977
+        },
+        {
+            "x": 7.565488841826375,
+            "y": 0.8251307377838253,
+            "heading": 3.107083972632397,
+            "angularVelocity": -0.3586854317033129,
+            "velocityX": -1.0362176328849728,
+            "velocityY": 0.3446015832326818,
+            "timestamp": 1.2863756987851853
+        },
+        {
+            "x": 7.435127415733675,
+            "y": 0.8684254377651182,
+            "heading": 3.05838611435167,
+            "angularVelocity": -0.6184728907024979,
+            "velocityX": -1.6556171231776886,
+            "velocityY": 0.5498516607274432,
+            "timestamp": 1.365114571600473
+        },
+        {
+            "x": 7.25585035840985,
+            "y": 0.927906970601061,
+            "heading": 2.99467735542164,
+            "angularVelocity": -0.809114439312372,
+            "velocityX": -2.2768557754742957,
+            "velocityY": 0.75542779200662,
+            "timestamp": 1.4438534444157605
+        },
+        {
+            "x": 7.027485658601358,
+            "y": 1.0035719757661283,
+            "heading": 2.9276702363354157,
+            "angularVelocity": -0.8510042967393713,
+            "velocityX": -2.900279006332858,
+            "velocityY": 0.9609612439152991,
+            "timestamp": 1.522592317231048
+        },
+        {
+            "x": 6.751324119312769,
+            "y": 1.0947095363284396,
+            "heading": 2.9052334045847372,
+            "angularVelocity": -0.28495241230227925,
+            "velocityX": -3.507308771569951,
+            "velocityY": 1.1574659034821269,
+            "timestamp": 1.6013311900463356
+        },
+        {
+            "x": 6.468725179881112,
+            "y": 1.1888428468535348,
+            "heading": 2.9052333806250203,
+            "angularVelocity": -3.0429336907534885e-7,
+            "velocityX": -3.5890650872613086,
+            "velocityY": 1.195512548749856,
+            "timestamp": 1.6800700628616232
+        },
+        {
+            "x": 6.186126242787232,
+            "y": 1.2829761643969295,
+            "heading": 2.9052333566653217,
+            "angularVelocity": -3.0429313853702755e-7,
+            "velocityX": -3.5890650575710485,
+            "velocityY": 1.1955126378837118,
+            "timestamp": 1.7588089356769108
+        },
+        {
+            "x": 5.9036205043534045,
+            "y": 1.377388777995483,
+            "heading": 2.905233301444114,
+            "angularVelocity": -7.013207784930713e-7,
+            "velocityX": -3.5878814152769594,
+            "velocityY": 1.1990597556563287,
+            "timestamp": 1.8375478084921983
+        },
+        {
+            "x": 5.660687795689049,
+            "y": 1.4583340101651068,
+            "heading": 2.781705854564737,
+            "angularVelocity": -1.5688241711201327,
+            "velocityX": -3.0852957373957746,
+            "velocityY": 1.0280212209731776,
+            "timestamp": 1.916286681307486
+        },
+        {
+            "x": 5.4666551877353555,
+            "y": 1.5230274173690972,
+            "heading": 2.6729354985546605,
+            "angularVelocity": -1.3814060593074367,
+            "velocityX": -2.464254325927075,
+            "velocityY": 0.8216196764176925,
+            "timestamp": 1.9950255541227735
+        },
+        {
+            "x": 5.321269342371624,
+            "y": 1.571523398115741,
+            "heading": 2.5878352744293056,
+            "angularVelocity": -1.080790479754395,
+            "velocityX": -1.8464303610846597,
+            "velocityY": 0.6159090041891974,
+            "timestamp": 2.073764426938061
+        },
+        {
+            "x": 5.2244075328588835,
+            "y": 1.6038438553412442,
+            "heading": 2.5296373366742775,
+            "angularVelocity": -0.7391258685091143,
+            "velocityX": -1.2301650512570566,
+            "velocityY": 0.4104765037889631,
+            "timestamp": 2.1525032997533486
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.3764003168270292,
+            "velocityX": -0.6147856976876184,
+            "velocityY": 0.20518638483251173,
+            "timestamp": 2.231242172568636
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 3.3983560967726525e-33,
+            "velocityX": 0,
+            "velocityY": -1.4165312907158555e-34,
+            "timestamp": 2.309981045383924
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S4.traj
new file mode 100644
index 00000000..1dfddc31
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S4.traj	
@@ -0,0 +1,283 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.220564813130112,
+            "y": 1.6051202900758712,
+            "heading": 2.517168506154745,
+            "angularVelocity": 0.22812300403374605,
+            "velocityX": 0.5921458136084756,
+            "velocityY": -0.19771109358311004,
+            "timestamp": 0.07525986354364067
+        },
+        {
+            "x": 5.309738195223409,
+            "y": 1.5753445036804834,
+            "heading": 2.5497776530467964,
+            "angularVelocity": 0.433287350742303,
+            "velocityX": 1.1848730238739802,
+            "velocityY": -0.3956396543042072,
+            "timestamp": 0.15051972708728134
+        },
+        {
+            "x": 5.443577884042542,
+            "y": 1.5306492621981627,
+            "heading": 2.5951663634005544,
+            "angularVelocity": 0.6030931789749864,
+            "velocityX": 1.7783674128179001,
+            "velocityY": -0.593878853585797,
+            "timestamp": 0.225779590630922
+        },
+        {
+            "x": 5.622156845349364,
+            "y": 1.4710006417461063,
+            "heading": 2.6487285344970175,
+            "angularVelocity": 0.7116963621041407,
+            "velocityX": 2.372831319356166,
+            "velocityY": -0.7925688095018658,
+            "timestamp": 0.3010394541745627
+        },
+        {
+            "x": 5.845518423751245,
+            "y": 1.3963607866160492,
+            "heading": 2.700604108231769,
+            "angularVelocity": 0.6892860456047826,
+            "velocityX": 2.967871158474283,
+            "velocityY": -0.9917617653767852,
+            "timestamp": 0.37629931771820335
+        },
+        {
+            "x": 6.112314775127604,
+            "y": 1.3070273852513161,
+            "heading": 2.7110861192214117,
+            "angularVelocity": 0.13927757101984,
+            "velocityX": 3.5450017953016038,
+            "velocityY": -1.186999247121034,
+            "timestamp": 0.451559181261844
+        },
+        {
+            "x": 6.382409273163311,
+            "y": 1.216999105128909,
+            "heading": 2.7110861382363836,
+            "angularVelocity": 2.526575375097703e-7,
+            "velocityX": 3.588825242542295,
+            "velocityY": -1.196232306084407,
+            "timestamp": 0.5268190448054847
+        },
+        {
+            "x": 6.652503772105593,
+            "y": 1.1269708277263142,
+            "heading": 2.711086157251347,
+            "angularVelocity": 2.5265742250097044e-7,
+            "velocityX": 3.5888252545882353,
+            "velocityY": -1.19623226994546,
+            "timestamp": 0.6020789083491254
+        },
+        {
+            "x": 6.9225982993416295,
+            "y": 1.0369426352093585,
+            "heading": 2.7110861762673086,
+            "angularVelocity": 2.5267068695978854e-7,
+            "velocityX": 3.5888256305356965,
+            "velocityY": -1.19623114204494,
+            "timestamp": 0.677338771892766
+        },
+        {
+            "x": 7.1604160781282875,
+            "y": 0.9586172665469176,
+            "heading": 2.823441998219159,
+            "angularVelocity": 1.4929049384563269,
+            "velocityX": 3.1599549559208913,
+            "velocityY": -1.040732270488674,
+            "timestamp": 0.7525986354364067
+        },
+        {
+            "x": 7.353386220004786,
+            "y": 0.8948816690648573,
+            "heading": 2.9268528477366496,
+            "angularVelocity": 1.3740504519719978,
+            "velocityX": 2.5640511793461145,
+            "velocityY": -0.8468736784926819,
+            "timestamp": 0.8278584989800474
+        },
+        {
+            "x": 7.501750789212216,
+            "y": 0.8458382793290063,
+            "heading": 3.012370231908866,
+            "angularVelocity": 1.136294701393235,
+            "velocityX": 1.9713637817240808,
+            "velocityY": -0.6516539816393957,
+            "timestamp": 0.903118362523688
+        },
+        {
+            "x": 7.605609860307225,
+            "y": 0.8115063107648604,
+            "heading": 3.0771995592704795,
+            "angularVelocity": 0.8614063899281612,
+            "velocityX": 1.3800061042468479,
+            "velocityY": -0.45617899033577186,
+            "timestamp": 0.9783782260673287
+        },
+        {
+            "x": 7.665015516622623,
+            "y": 0.7918924721730815,
+            "heading": 3.1200072835634023,
+            "angularVelocity": 0.5687988560874772,
+            "velocityX": 0.7893404733713316,
+            "velocityY": -0.2606148572194197,
+            "timestamp": 1.0536380896109694
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 0.26564911886937803,
+            "velocityX": 0.19910324935265278,
+            "velocityY": -0.06500772048629082,
+            "timestamp": 1.1288979531546102
+        },
+        {
+            "x": 7.647079450231064,
+            "y": 0.7979971975497204,
+            "heading": 3.1353264592199808,
+            "angularVelocity": -0.059354936296623806,
+            "velocityX": -0.418097803433937,
+            "velocityY": 0.13966668757779366,
+            "timestamp": 1.2076368259698977
+        },
+        {
+            "x": 7.565488841826375,
+            "y": 0.8251307377838253,
+            "heading": 3.107083972632397,
+            "angularVelocity": -0.3586854317033129,
+            "velocityX": -1.0362176328849728,
+            "velocityY": 0.3446015832326818,
+            "timestamp": 1.2863756987851853
+        },
+        {
+            "x": 7.435127415733675,
+            "y": 0.8684254377651182,
+            "heading": 3.05838611435167,
+            "angularVelocity": -0.6184728907024979,
+            "velocityX": -1.6556171231776886,
+            "velocityY": 0.5498516607274432,
+            "timestamp": 1.365114571600473
+        },
+        {
+            "x": 7.25585035840985,
+            "y": 0.927906970601061,
+            "heading": 2.99467735542164,
+            "angularVelocity": -0.809114439312372,
+            "velocityX": -2.2768557754742957,
+            "velocityY": 0.75542779200662,
+            "timestamp": 1.4438534444157605
+        },
+        {
+            "x": 7.027485658601358,
+            "y": 1.0035719757661283,
+            "heading": 2.9276702363354157,
+            "angularVelocity": -0.8510042967393713,
+            "velocityX": -2.900279006332858,
+            "velocityY": 0.9609612439152991,
+            "timestamp": 1.522592317231048
+        },
+        {
+            "x": 6.751324119312769,
+            "y": 1.0947095363284396,
+            "heading": 2.9052334045847372,
+            "angularVelocity": -0.28495241230227925,
+            "velocityX": -3.507308771569951,
+            "velocityY": 1.1574659034821269,
+            "timestamp": 1.6013311900463356
+        },
+        {
+            "x": 6.468725179881112,
+            "y": 1.1888428468535348,
+            "heading": 2.9052333806250203,
+            "angularVelocity": -3.0429336907534885e-7,
+            "velocityX": -3.5890650872613086,
+            "velocityY": 1.195512548749856,
+            "timestamp": 1.6800700628616232
+        },
+        {
+            "x": 6.186126242787232,
+            "y": 1.2829761643969295,
+            "heading": 2.9052333566653217,
+            "angularVelocity": -3.0429313853702755e-7,
+            "velocityX": -3.5890650575710485,
+            "velocityY": 1.1955126378837118,
+            "timestamp": 1.7588089356769108
+        },
+        {
+            "x": 5.9036205043534045,
+            "y": 1.377388777995483,
+            "heading": 2.905233301444114,
+            "angularVelocity": -7.013207784930713e-7,
+            "velocityX": -3.5878814152769594,
+            "velocityY": 1.1990597556563287,
+            "timestamp": 1.8375478084921983
+        },
+        {
+            "x": 5.660687795689049,
+            "y": 1.4583340101651068,
+            "heading": 2.781705854564737,
+            "angularVelocity": -1.5688241711201327,
+            "velocityX": -3.0852957373957746,
+            "velocityY": 1.0280212209731776,
+            "timestamp": 1.916286681307486
+        },
+        {
+            "x": 5.4666551877353555,
+            "y": 1.5230274173690972,
+            "heading": 2.6729354985546605,
+            "angularVelocity": -1.3814060593074367,
+            "velocityX": -2.464254325927075,
+            "velocityY": 0.8216196764176925,
+            "timestamp": 1.9950255541227735
+        },
+        {
+            "x": 5.321269342371624,
+            "y": 1.571523398115741,
+            "heading": 2.5878352744293056,
+            "angularVelocity": -1.080790479754395,
+            "velocityX": -1.8464303610846597,
+            "velocityY": 0.6159090041891974,
+            "timestamp": 2.073764426938061
+        },
+        {
+            "x": 5.2244075328588835,
+            "y": 1.6038438553412442,
+            "heading": 2.5296373366742775,
+            "angularVelocity": -0.7391258685091143,
+            "velocityX": -1.2301650512570566,
+            "velocityY": 0.4104765037889631,
+            "timestamp": 2.1525032997533486
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.3764003168270292,
+            "velocityX": -0.6147856976876184,
+            "velocityY": 0.20518638483251173,
+            "timestamp": 2.231242172568636
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 3.3983560967726525e-33,
+            "velocityX": 0,
+            "velocityY": -1.4165312907158555e-34,
+            "timestamp": 2.309981045383924
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.1.traj
new file mode 100644
index 00000000..8867d64b
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.1.traj	
@@ -0,0 +1,166 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": -2.991539680273006e-28,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.128677687485332,
+            "y": 1.6650804220486217,
+            "heading": 2.5120089352888275,
+            "angularVelocity": 0.13555115758601896,
+            "velocityX": -0.5341517867092611,
+            "velocityY": 0.5088463919723994,
+            "timestamp": 0.0885933805561246
+        },
+        {
+            "x": 5.034016646343104,
+            "y": 1.7552508281834698,
+            "heading": 2.534662979379905,
+            "angularVelocity": 0.2557080895761949,
+            "velocityX": -1.0684888707035958,
+            "velocityY": 1.017800715683549,
+            "timestamp": 0.1771867611122492
+        },
+        {
+            "x": 4.891992835259765,
+            "y": 1.890523822602506,
+            "heading": 2.5656675717312063,
+            "angularVelocity": 0.3499651120288192,
+            "velocityX": -1.6030973216262263,
+            "velocityY": 1.5268973095946068,
+            "timestamp": 0.2657801416683738
+        },
+        {
+            "x": 4.702571182201446,
+            "y": 2.070911500413953,
+            "heading": 2.6003407126574074,
+            "angularVelocity": 0.3913739458702845,
+            "velocityX": -2.1381016490087936,
+            "velocityY": 2.036130427342408,
+            "timestamp": 0.3543735222244984
+        },
+        {
+            "x": 4.465767136473608,
+            "y": 2.2963300542080516,
+            "heading": 2.6236809711316846,
+            "angularVelocity": 0.2634537515983315,
+            "velocityX": -2.672931591969363,
+            "velocityY": 2.5444175668553926,
+            "timestamp": 0.442966902780623
+        },
+        {
+            "x": 4.223169514205327,
+            "y": 2.5275605696083283,
+            "heading": 2.6236809810890134,
+            "angularVelocity": 1.1239359640247893e-7,
+            "velocityX": -2.7383267321489955,
+            "velocityY": 2.610020228924234,
+            "timestamp": 0.5315602833367477
+        },
+        {
+            "x": 3.980571892636886,
+            "y": 2.7587910857428417,
+            "heading": 2.623680991046373,
+            "angularVelocity": 1.1239395834581428e-7,
+            "velocityX": -2.7383267242494984,
+            "velocityY": 2.6100202372119665,
+            "timestamp": 0.6201536638928723
+        },
+        {
+            "x": 3.737974271068317,
+            "y": 2.9900216018772205,
+            "heading": 2.623681001003734,
+            "angularVelocity": 1.1239395847018201e-7,
+            "velocityX": -2.7383267242509515,
+            "velocityY": 2.610020237210452,
+            "timestamp": 0.7087470444489969
+        },
+        {
+            "x": 3.4953766494997485,
+            "y": 3.2212521180116,
+            "heading": 2.623681010961094,
+            "angularVelocity": 1.1239395711385692e-7,
+            "velocityX": -2.73832672425095,
+            "velocityY": 2.610020237210453,
+            "timestamp": 0.7973404250051216
+        },
+        {
+            "x": 3.252779027931301,
+            "y": 3.4524826341461052,
+            "heading": 2.6236810209184545,
+            "angularVelocity": 1.1239395822212682e-7,
+            "velocityX": -2.7383267242495783,
+            "velocityY": 2.6100202372118835,
+            "timestamp": 0.8859338055612462
+        },
+        {
+            "x": 3.0101814056711773,
+            "y": 3.68371314955494,
+            "heading": 2.6236810308757845,
+            "angularVelocity": 1.1239361401031205e-7,
+            "velocityX": -2.7383267320569105,
+            "velocityY": 2.6100202290208365,
+            "timestamp": 0.9745271861173709
+        },
+        {
+            "x": 2.773398473292677,
+            "y": 3.9091148718156945,
+            "heading": 2.6467919550399244,
+            "angularVelocity": 0.2608651348337584,
+            "velocityX": -2.6726932745104093,
+            "velocityY": 2.5442275805015666,
+            "timestamp": 1.0631205666734955
+        },
+        {
+            "x": 2.58398624574228,
+            "y": 4.089495303792956,
+            "heading": 2.682294638537745,
+            "angularVelocity": 0.40073742840518267,
+            "velocityX": -2.1379952583522854,
+            "velocityY": 2.036048639807706,
+            "timestamp": 1.15171394722962
+        },
+        {
+            "x": 2.441971645222967,
+            "y": 4.224760268869052,
+            "heading": 2.7142172786134493,
+            "angularVelocity": 0.36032759869046743,
+            "velocityX": -1.6029933571543074,
+            "velocityY": 1.5268066781852525,
+            "timestamp": 1.2403073277857446
+        },
+        {
+            "x": 2.3473180655703776,
+            "y": 4.314923716116089,
+            "heading": 2.737593550901004,
+            "angularVelocity": 0.263860258416364,
+            "velocityX": -1.0684046489525902,
+            "velocityY": 1.0177221670632457,
+            "timestamp": 1.3289007083418691
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0.1400381046653499,
+            "velocityX": -0.5341038492193103,
+            "velocityY": 0.5087996823346718,
+            "timestamp": 1.4174940888979937
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": -3.3897612225154066e-29,
+            "velocityY": 0,
+            "timestamp": 1.5060874694541182
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.traj
new file mode 100644
index 00000000..8867d64b
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.traj	
@@ -0,0 +1,166 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": -2.991539680273006e-28,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.128677687485332,
+            "y": 1.6650804220486217,
+            "heading": 2.5120089352888275,
+            "angularVelocity": 0.13555115758601896,
+            "velocityX": -0.5341517867092611,
+            "velocityY": 0.5088463919723994,
+            "timestamp": 0.0885933805561246
+        },
+        {
+            "x": 5.034016646343104,
+            "y": 1.7552508281834698,
+            "heading": 2.534662979379905,
+            "angularVelocity": 0.2557080895761949,
+            "velocityX": -1.0684888707035958,
+            "velocityY": 1.017800715683549,
+            "timestamp": 0.1771867611122492
+        },
+        {
+            "x": 4.891992835259765,
+            "y": 1.890523822602506,
+            "heading": 2.5656675717312063,
+            "angularVelocity": 0.3499651120288192,
+            "velocityX": -1.6030973216262263,
+            "velocityY": 1.5268973095946068,
+            "timestamp": 0.2657801416683738
+        },
+        {
+            "x": 4.702571182201446,
+            "y": 2.070911500413953,
+            "heading": 2.6003407126574074,
+            "angularVelocity": 0.3913739458702845,
+            "velocityX": -2.1381016490087936,
+            "velocityY": 2.036130427342408,
+            "timestamp": 0.3543735222244984
+        },
+        {
+            "x": 4.465767136473608,
+            "y": 2.2963300542080516,
+            "heading": 2.6236809711316846,
+            "angularVelocity": 0.2634537515983315,
+            "velocityX": -2.672931591969363,
+            "velocityY": 2.5444175668553926,
+            "timestamp": 0.442966902780623
+        },
+        {
+            "x": 4.223169514205327,
+            "y": 2.5275605696083283,
+            "heading": 2.6236809810890134,
+            "angularVelocity": 1.1239359640247893e-7,
+            "velocityX": -2.7383267321489955,
+            "velocityY": 2.610020228924234,
+            "timestamp": 0.5315602833367477
+        },
+        {
+            "x": 3.980571892636886,
+            "y": 2.7587910857428417,
+            "heading": 2.623680991046373,
+            "angularVelocity": 1.1239395834581428e-7,
+            "velocityX": -2.7383267242494984,
+            "velocityY": 2.6100202372119665,
+            "timestamp": 0.6201536638928723
+        },
+        {
+            "x": 3.737974271068317,
+            "y": 2.9900216018772205,
+            "heading": 2.623681001003734,
+            "angularVelocity": 1.1239395847018201e-7,
+            "velocityX": -2.7383267242509515,
+            "velocityY": 2.610020237210452,
+            "timestamp": 0.7087470444489969
+        },
+        {
+            "x": 3.4953766494997485,
+            "y": 3.2212521180116,
+            "heading": 2.623681010961094,
+            "angularVelocity": 1.1239395711385692e-7,
+            "velocityX": -2.73832672425095,
+            "velocityY": 2.610020237210453,
+            "timestamp": 0.7973404250051216
+        },
+        {
+            "x": 3.252779027931301,
+            "y": 3.4524826341461052,
+            "heading": 2.6236810209184545,
+            "angularVelocity": 1.1239395822212682e-7,
+            "velocityX": -2.7383267242495783,
+            "velocityY": 2.6100202372118835,
+            "timestamp": 0.8859338055612462
+        },
+        {
+            "x": 3.0101814056711773,
+            "y": 3.68371314955494,
+            "heading": 2.6236810308757845,
+            "angularVelocity": 1.1239361401031205e-7,
+            "velocityX": -2.7383267320569105,
+            "velocityY": 2.6100202290208365,
+            "timestamp": 0.9745271861173709
+        },
+        {
+            "x": 2.773398473292677,
+            "y": 3.9091148718156945,
+            "heading": 2.6467919550399244,
+            "angularVelocity": 0.2608651348337584,
+            "velocityX": -2.6726932745104093,
+            "velocityY": 2.5442275805015666,
+            "timestamp": 1.0631205666734955
+        },
+        {
+            "x": 2.58398624574228,
+            "y": 4.089495303792956,
+            "heading": 2.682294638537745,
+            "angularVelocity": 0.40073742840518267,
+            "velocityX": -2.1379952583522854,
+            "velocityY": 2.036048639807706,
+            "timestamp": 1.15171394722962
+        },
+        {
+            "x": 2.441971645222967,
+            "y": 4.224760268869052,
+            "heading": 2.7142172786134493,
+            "angularVelocity": 0.36032759869046743,
+            "velocityX": -1.6029933571543074,
+            "velocityY": 1.5268066781852525,
+            "timestamp": 1.2403073277857446
+        },
+        {
+            "x": 2.3473180655703776,
+            "y": 4.314923716116089,
+            "heading": 2.737593550901004,
+            "angularVelocity": 0.263860258416364,
+            "velocityX": -1.0684046489525902,
+            "velocityY": 1.0177221670632457,
+            "timestamp": 1.3289007083418691
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0.1400381046653499,
+            "velocityX": -0.5341038492193103,
+            "velocityY": 0.5087996823346718,
+            "timestamp": 1.4174940888979937
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": -3.3897612225154066e-29,
+            "velocityY": 0,
+            "timestamp": 1.5060874694541182
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S1.1.traj
new file mode 100644
index 00000000..f17937c5
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S1.1.traj	
@@ -0,0 +1,319 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.363517540312363,
+            "y": 6.769205991625322,
+            "heading": -2.7639736107792703,
+            "angularVelocity": -0.15851225932569585,
+            "velocityX": 0.7205230617028869,
+            "velocityY": 0.13846094187126756,
+            "timestamp": 0.08815476379374498
+        },
+        {
+            "x": 2.4905727294218147,
+            "y": 6.793631120419202,
+            "heading": -2.790535805480005,
+            "angularVelocity": -0.30131320824454183,
+            "velocityX": 1.4412742277515702,
+            "velocityY": 0.2770710026632852,
+            "timestamp": 0.17630952758748997
+        },
+        {
+            "x": 2.6811943332190395,
+            "y": 6.830296876066563,
+            "heading": -2.8273459830802556,
+            "angularVelocity": -0.4175631130539097,
+            "velocityX": 2.162351705044798,
+            "velocityY": 0.4159248357030905,
+            "timestamp": 0.26446429138123495
+        },
+        {
+            "x": 2.935420930881945,
+            "y": 6.8792439916128805,
+            "heading": -2.869603740492264,
+            "angularVelocity": -0.47935875037764497,
+            "velocityX": 2.8838668124358944,
+            "velocityY": 0.5552407316391297,
+            "timestamp": 0.35261905517497993
+        },
+        {
+            "x": 3.253189180048851,
+            "y": 6.9405688545954956,
+            "heading": -2.9017584747192204,
+            "angularVelocity": -0.364753223117765,
+            "velocityX": 3.604663383936769,
+            "velocityY": 0.695650017576943,
+            "timestamp": 0.4407738189687249
+        },
+        {
+            "x": 3.5807176480413454,
+            "y": 7.003313040154224,
+            "heading": -2.901758484999235,
+            "angularVelocity": -1.1661326594690383e-7,
+            "velocityX": 3.7153802460274443,
+            "velocityY": 0.7117503678590205,
+            "timestamp": 0.5289285827624699
+        },
+        {
+            "x": 3.9082461164396207,
+            "y": 7.066057223594786,
+            "heading": -2.901758495279238,
+            "angularVelocity": -1.1661313280225847e-7,
+            "velocityX": 3.7153802506304894,
+            "velocityY": 0.7117503438312037,
+            "timestamp": 0.6170833465562149
+        },
+        {
+            "x": 4.235774584837897,
+            "y": 7.128801407035338,
+            "heading": -2.901758505559241,
+            "angularVelocity": -1.1661313256198823e-7,
+            "velocityX": 3.7153802506305107,
+            "velocityY": 0.711750343831093,
+            "timestamp": 0.7052381103499599
+        },
+        {
+            "x": 4.563303053236174,
+            "y": 7.19154559047589,
+            "heading": -2.9017585158392443,
+            "angularVelocity": -1.166131334044927e-7,
+            "velocityX": 3.7153802506305107,
+            "velocityY": 0.7117503438310929,
+            "timestamp": 0.7933928741437049
+        },
+        {
+            "x": 4.890831521634451,
+            "y": 7.254289773916442,
+            "heading": -2.9017585261192473,
+            "angularVelocity": -1.1661313263433965e-7,
+            "velocityX": 3.7153802506305107,
+            "velocityY": 0.711750343831093,
+            "timestamp": 0.8815476379374498
+        },
+        {
+            "x": 5.218359990032727,
+            "y": 7.317033957356994,
+            "heading": -2.9017585363992504,
+            "angularVelocity": -1.1661313267352917e-7,
+            "velocityX": 3.7153802506305107,
+            "velocityY": 0.7117503438310931,
+            "timestamp": 0.9697024017311948
+        },
+        {
+            "x": 5.545888458431004,
+            "y": 7.379778140797546,
+            "heading": -2.9017585466792535,
+            "angularVelocity": -1.1661313271580576e-7,
+            "velocityX": 3.7153802506305107,
+            "velocityY": 0.7117503438310933,
+            "timestamp": 1.0578571655249398
+        },
+        {
+            "x": 5.873416926829281,
+            "y": 7.442522324238098,
+            "heading": -2.901758556959257,
+            "angularVelocity": -1.166131330429824e-7,
+            "velocityX": 3.7153802506305103,
+            "velocityY": 0.7117503438310929,
+            "timestamp": 1.1460119293186848
+        },
+        {
+            "x": 6.200945395227635,
+            "y": 7.505266507678241,
+            "heading": -2.90175856723926,
+            "angularVelocity": -1.1661313350589332e-7,
+            "velocityX": 3.7153802506314,
+            "velocityY": 0.7117503438264522,
+            "timestamp": 1.2341666931124298
+        },
+        {
+            "x": 6.528473880635578,
+            "y": 7.568010602327234,
+            "heading": -2.9017585775192636,
+            "angularVelocity": -1.1661313512046063e-7,
+            "velocityX": 3.715380443582822,
+            "velocityY": 0.7117493366074876,
+            "timestamp": 1.3223214569061748
+        },
+        {
+            "x": 6.858875530524355,
+            "y": 7.613248618194527,
+            "heading": -2.901758593513828,
+            "angularVelocity": -1.8143731961725824e-7,
+            "velocityX": 3.747972720587331,
+            "velocityY": 0.51316586784965,
+            "timestamp": 1.4104762206999197
+        },
+        {
+            "x": 7.1493370822627105,
+            "y": 7.612204494163831,
+            "heading": -2.9774836288017847,
+            "angularVelocity": -0.859001057108326,
+            "velocityX": 3.294904770182843,
+            "velocityY": -0.011844215624353746,
+            "timestamp": 1.4986309844936647
+        },
+        {
+            "x": 7.3768731997980925,
+            "y": 7.596010582302943,
+            "heading": -3.0421265478344597,
+            "angularVelocity": -0.7332890050493631,
+            "velocityX": 2.581098374533033,
+            "velocityY": -0.18369865863138202,
+            "timestamp": 1.5867857482874097
+        },
+        {
+            "x": 7.541210779550857,
+            "y": 7.566116365480278,
+            "heading": -3.09203212161725,
+            "angularVelocity": -0.566113181354048,
+            "velocityX": 1.8641939775059564,
+            "velocityY": -0.33911062245705376,
+            "timestamp": 1.6749405120811547
+        },
+        {
+            "x": 7.642266411565545,
+            "y": 7.52302718020075,
+            "heading": -3.125932818808563,
+            "angularVelocity": -0.3845588795476813,
+            "velocityX": 1.146343404096945,
+            "velocityY": -0.4887902074166845,
+            "timestamp": 1.7630952758748997
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.19570681865122413,
+            "velocityX": 0.4280379960263239,
+            "velocityY": -0.6355547651607396,
+            "timestamp": 1.8512500396686447
+        },
+        {
+            "x": 7.65149635610838,
+            "y": 7.395583820775626,
+            "heading": -3.1428442524727873,
+            "angularVelocity": 0.003749626131397279,
+            "velocityX": -0.3133749684294368,
+            "velocityY": -0.7851642756589506,
+            "timestamp": 1.9422070297386083
+        },
+        {
+            "x": 7.55553660648891,
+            "y": 7.310596065504526,
+            "heading": -3.1249452939328513,
+            "angularVelocity": 0.19678485981305774,
+            "velocityX": -1.0550013753274867,
+            "velocityY": -0.9343729954757892,
+            "timestamp": 2.033164019808572
+        },
+        {
+            "x": 7.392089155098869,
+            "y": 7.212098072056303,
+            "heading": -3.09046475805122,
+            "angularVelocity": 0.37908615770051013,
+            "velocityX": -1.796975155667623,
+            "velocityY": -1.082907354041688,
+            "timestamp": 2.1241210098785355
+        },
+        {
+            "x": 7.1610935040737775,
+            "y": 7.100214825167467,
+            "heading": -3.041377794518818,
+            "angularVelocity": 0.5396722505288368,
+            "velocityX": -2.539614062068222,
+            "velocityY": -1.2300676045109389,
+            "timestamp": 2.215077999948499
+        },
+        {
+            "x": 6.862392163588639,
+            "y": 6.975338305116689,
+            "heading": -2.9837366917531454,
+            "angularVelocity": 0.6337182301358967,
+            "velocityX": -3.2839844442451716,
+            "velocityY": -1.3729183425572538,
+            "timestamp": 2.3060349900184627
+        },
+        {
+            "x": 6.525454088685491,
+            "y": 6.905573407470138,
+            "heading": -2.9837366810920996,
+            "angularVelocity": 1.1720974856117963e-7,
+            "velocityX": -3.704367027141189,
+            "velocityY": -0.7670097437560687,
+            "timestamp": 2.3969919800884263
+        },
+        {
+            "x": 6.1883268460675325,
+            "y": 6.836728439791378,
+            "heading": -2.983736670548346,
+            "angularVelocity": 1.1592021231646331e-7,
+            "velocityX": -3.7064467762031894,
+            "velocityY": -0.7568958430329531,
+            "timestamp": 2.48794897015839
+        },
+        {
+            "x": 5.851199548758337,
+            "y": 6.767884093073121,
+            "heading": -2.9837365285519053,
+            "angularVelocity": 0.0000015611382957440025,
+            "velocityX": -3.706447377489981,
+            "velocityY": -0.7568890160645022,
+            "timestamp": 2.5789059602283535
+        },
+        {
+            "x": 5.5810538404288925,
+            "y": 6.712717106073741,
+            "heading": -2.9505527658877098,
+            "angularVelocity": 0.3648291641873475,
+            "velocityX": -2.970037906065856,
+            "velocityY": -0.6065172886320637,
+            "timestamp": 2.669862950298317
+        },
+        {
+            "x": 5.3784959131966685,
+            "y": 6.671352294274059,
+            "heading": -2.9178931983006144,
+            "angularVelocity": 0.3590660548682879,
+            "velocityX": -2.226963832866719,
+            "velocityY": -0.45477331393503795,
+            "timestamp": 2.7608199403682807
+        },
+        {
+            "x": 5.2434899940019335,
+            "y": 6.6437823583535645,
+            "heading": -2.8932620140402783,
+            "angularVelocity": 0.2708003446618372,
+            "velocityX": -1.4842830561003668,
+            "velocityY": -0.30310958948050626,
+            "timestamp": 2.8517769304382443
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0.14580533096009823,
+            "velocityX": -0.7419989816068217,
+            "velocityY": -0.15152610418353574,
+            "timestamp": 2.942733920508208
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0,
+            "velocityX": 1.1797533379852482e-32,
+            "velocityY": 0,
+            "timestamp": 3.0336909105781715
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S1.traj
new file mode 100644
index 00000000..f17937c5
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S1.traj	
@@ -0,0 +1,319 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.363517540312363,
+            "y": 6.769205991625322,
+            "heading": -2.7639736107792703,
+            "angularVelocity": -0.15851225932569585,
+            "velocityX": 0.7205230617028869,
+            "velocityY": 0.13846094187126756,
+            "timestamp": 0.08815476379374498
+        },
+        {
+            "x": 2.4905727294218147,
+            "y": 6.793631120419202,
+            "heading": -2.790535805480005,
+            "angularVelocity": -0.30131320824454183,
+            "velocityX": 1.4412742277515702,
+            "velocityY": 0.2770710026632852,
+            "timestamp": 0.17630952758748997
+        },
+        {
+            "x": 2.6811943332190395,
+            "y": 6.830296876066563,
+            "heading": -2.8273459830802556,
+            "angularVelocity": -0.4175631130539097,
+            "velocityX": 2.162351705044798,
+            "velocityY": 0.4159248357030905,
+            "timestamp": 0.26446429138123495
+        },
+        {
+            "x": 2.935420930881945,
+            "y": 6.8792439916128805,
+            "heading": -2.869603740492264,
+            "angularVelocity": -0.47935875037764497,
+            "velocityX": 2.8838668124358944,
+            "velocityY": 0.5552407316391297,
+            "timestamp": 0.35261905517497993
+        },
+        {
+            "x": 3.253189180048851,
+            "y": 6.9405688545954956,
+            "heading": -2.9017584747192204,
+            "angularVelocity": -0.364753223117765,
+            "velocityX": 3.604663383936769,
+            "velocityY": 0.695650017576943,
+            "timestamp": 0.4407738189687249
+        },
+        {
+            "x": 3.5807176480413454,
+            "y": 7.003313040154224,
+            "heading": -2.901758484999235,
+            "angularVelocity": -1.1661326594690383e-7,
+            "velocityX": 3.7153802460274443,
+            "velocityY": 0.7117503678590205,
+            "timestamp": 0.5289285827624699
+        },
+        {
+            "x": 3.9082461164396207,
+            "y": 7.066057223594786,
+            "heading": -2.901758495279238,
+            "angularVelocity": -1.1661313280225847e-7,
+            "velocityX": 3.7153802506304894,
+            "velocityY": 0.7117503438312037,
+            "timestamp": 0.6170833465562149
+        },
+        {
+            "x": 4.235774584837897,
+            "y": 7.128801407035338,
+            "heading": -2.901758505559241,
+            "angularVelocity": -1.1661313256198823e-7,
+            "velocityX": 3.7153802506305107,
+            "velocityY": 0.711750343831093,
+            "timestamp": 0.7052381103499599
+        },
+        {
+            "x": 4.563303053236174,
+            "y": 7.19154559047589,
+            "heading": -2.9017585158392443,
+            "angularVelocity": -1.166131334044927e-7,
+            "velocityX": 3.7153802506305107,
+            "velocityY": 0.7117503438310929,
+            "timestamp": 0.7933928741437049
+        },
+        {
+            "x": 4.890831521634451,
+            "y": 7.254289773916442,
+            "heading": -2.9017585261192473,
+            "angularVelocity": -1.1661313263433965e-7,
+            "velocityX": 3.7153802506305107,
+            "velocityY": 0.711750343831093,
+            "timestamp": 0.8815476379374498
+        },
+        {
+            "x": 5.218359990032727,
+            "y": 7.317033957356994,
+            "heading": -2.9017585363992504,
+            "angularVelocity": -1.1661313267352917e-7,
+            "velocityX": 3.7153802506305107,
+            "velocityY": 0.7117503438310931,
+            "timestamp": 0.9697024017311948
+        },
+        {
+            "x": 5.545888458431004,
+            "y": 7.379778140797546,
+            "heading": -2.9017585466792535,
+            "angularVelocity": -1.1661313271580576e-7,
+            "velocityX": 3.7153802506305107,
+            "velocityY": 0.7117503438310933,
+            "timestamp": 1.0578571655249398
+        },
+        {
+            "x": 5.873416926829281,
+            "y": 7.442522324238098,
+            "heading": -2.901758556959257,
+            "angularVelocity": -1.166131330429824e-7,
+            "velocityX": 3.7153802506305103,
+            "velocityY": 0.7117503438310929,
+            "timestamp": 1.1460119293186848
+        },
+        {
+            "x": 6.200945395227635,
+            "y": 7.505266507678241,
+            "heading": -2.90175856723926,
+            "angularVelocity": -1.1661313350589332e-7,
+            "velocityX": 3.7153802506314,
+            "velocityY": 0.7117503438264522,
+            "timestamp": 1.2341666931124298
+        },
+        {
+            "x": 6.528473880635578,
+            "y": 7.568010602327234,
+            "heading": -2.9017585775192636,
+            "angularVelocity": -1.1661313512046063e-7,
+            "velocityX": 3.715380443582822,
+            "velocityY": 0.7117493366074876,
+            "timestamp": 1.3223214569061748
+        },
+        {
+            "x": 6.858875530524355,
+            "y": 7.613248618194527,
+            "heading": -2.901758593513828,
+            "angularVelocity": -1.8143731961725824e-7,
+            "velocityX": 3.747972720587331,
+            "velocityY": 0.51316586784965,
+            "timestamp": 1.4104762206999197
+        },
+        {
+            "x": 7.1493370822627105,
+            "y": 7.612204494163831,
+            "heading": -2.9774836288017847,
+            "angularVelocity": -0.859001057108326,
+            "velocityX": 3.294904770182843,
+            "velocityY": -0.011844215624353746,
+            "timestamp": 1.4986309844936647
+        },
+        {
+            "x": 7.3768731997980925,
+            "y": 7.596010582302943,
+            "heading": -3.0421265478344597,
+            "angularVelocity": -0.7332890050493631,
+            "velocityX": 2.581098374533033,
+            "velocityY": -0.18369865863138202,
+            "timestamp": 1.5867857482874097
+        },
+        {
+            "x": 7.541210779550857,
+            "y": 7.566116365480278,
+            "heading": -3.09203212161725,
+            "angularVelocity": -0.566113181354048,
+            "velocityX": 1.8641939775059564,
+            "velocityY": -0.33911062245705376,
+            "timestamp": 1.6749405120811547
+        },
+        {
+            "x": 7.642266411565545,
+            "y": 7.52302718020075,
+            "heading": -3.125932818808563,
+            "angularVelocity": -0.3845588795476813,
+            "velocityX": 1.146343404096945,
+            "velocityY": -0.4887902074166845,
+            "timestamp": 1.7630952758748997
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.19570681865122413,
+            "velocityX": 0.4280379960263239,
+            "velocityY": -0.6355547651607396,
+            "timestamp": 1.8512500396686447
+        },
+        {
+            "x": 7.65149635610838,
+            "y": 7.395583820775626,
+            "heading": -3.1428442524727873,
+            "angularVelocity": 0.003749626131397279,
+            "velocityX": -0.3133749684294368,
+            "velocityY": -0.7851642756589506,
+            "timestamp": 1.9422070297386083
+        },
+        {
+            "x": 7.55553660648891,
+            "y": 7.310596065504526,
+            "heading": -3.1249452939328513,
+            "angularVelocity": 0.19678485981305774,
+            "velocityX": -1.0550013753274867,
+            "velocityY": -0.9343729954757892,
+            "timestamp": 2.033164019808572
+        },
+        {
+            "x": 7.392089155098869,
+            "y": 7.212098072056303,
+            "heading": -3.09046475805122,
+            "angularVelocity": 0.37908615770051013,
+            "velocityX": -1.796975155667623,
+            "velocityY": -1.082907354041688,
+            "timestamp": 2.1241210098785355
+        },
+        {
+            "x": 7.1610935040737775,
+            "y": 7.100214825167467,
+            "heading": -3.041377794518818,
+            "angularVelocity": 0.5396722505288368,
+            "velocityX": -2.539614062068222,
+            "velocityY": -1.2300676045109389,
+            "timestamp": 2.215077999948499
+        },
+        {
+            "x": 6.862392163588639,
+            "y": 6.975338305116689,
+            "heading": -2.9837366917531454,
+            "angularVelocity": 0.6337182301358967,
+            "velocityX": -3.2839844442451716,
+            "velocityY": -1.3729183425572538,
+            "timestamp": 2.3060349900184627
+        },
+        {
+            "x": 6.525454088685491,
+            "y": 6.905573407470138,
+            "heading": -2.9837366810920996,
+            "angularVelocity": 1.1720974856117963e-7,
+            "velocityX": -3.704367027141189,
+            "velocityY": -0.7670097437560687,
+            "timestamp": 2.3969919800884263
+        },
+        {
+            "x": 6.1883268460675325,
+            "y": 6.836728439791378,
+            "heading": -2.983736670548346,
+            "angularVelocity": 1.1592021231646331e-7,
+            "velocityX": -3.7064467762031894,
+            "velocityY": -0.7568958430329531,
+            "timestamp": 2.48794897015839
+        },
+        {
+            "x": 5.851199548758337,
+            "y": 6.767884093073121,
+            "heading": -2.9837365285519053,
+            "angularVelocity": 0.0000015611382957440025,
+            "velocityX": -3.706447377489981,
+            "velocityY": -0.7568890160645022,
+            "timestamp": 2.5789059602283535
+        },
+        {
+            "x": 5.5810538404288925,
+            "y": 6.712717106073741,
+            "heading": -2.9505527658877098,
+            "angularVelocity": 0.3648291641873475,
+            "velocityX": -2.970037906065856,
+            "velocityY": -0.6065172886320637,
+            "timestamp": 2.669862950298317
+        },
+        {
+            "x": 5.3784959131966685,
+            "y": 6.671352294274059,
+            "heading": -2.9178931983006144,
+            "angularVelocity": 0.3590660548682879,
+            "velocityX": -2.226963832866719,
+            "velocityY": -0.45477331393503795,
+            "timestamp": 2.7608199403682807
+        },
+        {
+            "x": 5.2434899940019335,
+            "y": 6.6437823583535645,
+            "heading": -2.8932620140402783,
+            "angularVelocity": 0.2708003446618372,
+            "velocityX": -1.4842830561003668,
+            "velocityY": -0.30310958948050626,
+            "timestamp": 2.8517769304382443
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0.14580533096009823,
+            "velocityX": -0.7419989816068217,
+            "velocityY": -0.15152610418353574,
+            "timestamp": 2.942733920508208
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0,
+            "velocityX": 1.1797533379852482e-32,
+            "velocityY": 0,
+            "timestamp": 3.0336909105781715
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S2.1.traj
new file mode 100644
index 00000000..7e79decb
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S2.1.traj	
@@ -0,0 +1,337 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.360726976398386,
+            "y": 6.773087618158843,
+            "heading": -2.760701916982345,
+            "angularVelocity": -0.1232236665180751,
+            "velocityX": 0.699220588306308,
+            "velocityY": 0.18523553288193312,
+            "timestamp": 0.08684952562036656
+        },
+        {
+            "x": 2.4821843215925723,
+            "y": 6.805261248290758,
+            "heading": -2.781888481642216,
+            "angularVelocity": -0.243945657831127,
+            "velocityX": 1.3984802372452498,
+            "velocityY": 0.37045257187160163,
+            "timestamp": 0.17369905124073312
+        },
+        {
+            "x": 2.664377574952537,
+            "y": 6.853518187805784,
+            "heading": -2.813196351929266,
+            "angularVelocity": -0.3604840678552547,
+            "velocityX": 2.0978036674189,
+            "velocityY": 0.5556384927872844,
+            "timestamp": 0.26054857686109967
+        },
+        {
+            "x": 2.9073173781912582,
+            "y": 6.917852930781389,
+            "heading": -2.8538946648532777,
+            "angularVelocity": -0.4686071988668809,
+            "velocityX": 2.797249628060051,
+            "velocityY": 0.7407610175878006,
+            "timestamp": 0.34739810248146624
+        },
+        {
+            "x": 3.211032053030438,
+            "y": 6.99824783750104,
+            "heading": -2.901766372941813,
+            "angularVelocity": -0.5512028735505646,
+            "velocityX": 3.4970216897535775,
+            "velocityY": 0.9256804357344971,
+            "timestamp": 0.4342476281018328
+        },
+        {
+            "x": 3.5286107194849063,
+            "y": 7.082430089360801,
+            "heading": -2.9017663796929978,
+            "angularVelocity": -7.773427725096788e-8,
+            "velocityX": 3.656654013778666,
+            "velocityY": 0.9692885626994009,
+            "timestamp": 0.5210971537221993
+        },
+        {
+            "x": 3.846189385784725,
+            "y": 7.166612341804015,
+            "heading": -2.9017663864441645,
+            "angularVelocity": -7.773406554275384e-8,
+            "velocityX": 3.6566540119980107,
+            "velocityY": 0.9692885694173843,
+            "timestamp": 0.6079466793425659
+        },
+        {
+            "x": 4.163768052084543,
+            "y": 7.250794594247232,
+            "heading": -2.9017663931953313,
+            "angularVelocity": -7.77340646408651e-8,
+            "velocityX": 3.656654011998002,
+            "velocityY": 0.969288569417417,
+            "timestamp": 0.6947962049629324
+        },
+        {
+            "x": 4.481346718384361,
+            "y": 7.334976846690449,
+            "heading": -2.901766399946498,
+            "angularVelocity": -7.773406451460054e-8,
+            "velocityX": 3.656654011998002,
+            "velocityY": 0.9692885694174173,
+            "timestamp": 0.7816457305832989
+        },
+        {
+            "x": 4.798925384684178,
+            "y": 7.419159099133665,
+            "heading": -2.9017664066976647,
+            "angularVelocity": -7.773406529246814e-8,
+            "velocityX": 3.656654011998002,
+            "velocityY": 0.9692885694174173,
+            "timestamp": 0.8684952562036654
+        },
+        {
+            "x": 5.1165040509839965,
+            "y": 7.503341351576882,
+            "heading": -2.9017664134488315,
+            "angularVelocity": -7.773406535280122e-8,
+            "velocityX": 3.656654011998002,
+            "velocityY": 0.969288569417417,
+            "timestamp": 0.9553447818240319
+        },
+        {
+            "x": 5.434082717283814,
+            "y": 7.5875236040201,
+            "heading": -2.901766420199998,
+            "angularVelocity": -7.773406538713727e-8,
+            "velocityX": 3.6566540119980018,
+            "velocityY": 0.9692885694174171,
+            "timestamp": 1.0421943074443984
+        },
+        {
+            "x": 5.751661383583632,
+            "y": 7.6717058564633165,
+            "heading": -2.901766426951165,
+            "angularVelocity": -7.773406530909276e-8,
+            "velocityX": 3.6566540119980027,
+            "velocityY": 0.969288569417416,
+            "timestamp": 1.129043833064765
+        },
+        {
+            "x": 6.0692400498888315,
+            "y": 7.755888108886232,
+            "heading": -2.9017664337023317,
+            "angularVelocity": -7.773406498550699e-8,
+            "velocityX": 3.6566540120599655,
+            "velocityY": 0.9692885691836598,
+            "timestamp": 1.2158933586851315
+        },
+        {
+            "x": 6.386819824167639,
+            "y": 7.840066181352823,
+            "heading": -2.901766440453515,
+            "angularVelocity": -7.773425363968975e-8,
+            "velocityX": 3.6566667694538877,
+            "velocityY": 0.9692404404655809,
+            "timestamp": 1.302742884305498
+        },
+        {
+            "x": 6.714420971530167,
+            "y": 7.864972939151579,
+            "heading": -2.901766454732528,
+            "angularVelocity": -1.6441095007226412e-7,
+            "velocityX": 3.7720545394171228,
+            "velocityY": 0.28678058539594425,
+            "timestamp": 1.3895924099258645
+        },
+        {
+            "x": 7.024629196469154,
+            "y": 7.83076867028296,
+            "heading": -2.946892502448731,
+            "angularVelocity": -0.5195888796655828,
+            "velocityX": 3.57178951437027,
+            "velocityY": -0.39383368676145797,
+            "timestamp": 1.476441935546231
+        },
+        {
+            "x": 7.277681557115376,
+            "y": 7.7704292121587075,
+            "heading": -3.000549099570889,
+            "angularVelocity": -0.6178110558347718,
+            "velocityX": 2.9136873096139366,
+            "velocityY": -0.6947586379213355,
+            "timestamp": 1.5632914611665976
+        },
+        {
+            "x": 7.471595029086891,
+            "y": 7.688687529495423,
+            "heading": -3.0531246016667453,
+            "angularVelocity": -0.6053631464343129,
+            "velocityX": 2.232752229631644,
+            "velocityY": -0.941187439761266,
+            "timestamp": 1.650140986786964
+        },
+        {
+            "x": 7.605791298108634,
+            "y": 7.5872519075766585,
+            "heading": -3.101243004619086,
+            "angularVelocity": -0.5540433595763132,
+            "velocityX": 1.5451583421207176,
+            "velocityY": -1.1679467584193457,
+            "timestamp": 1.7369905124073306
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.4829307041215213,
+            "velocityX": 0.854451436102516,
+            "velocityY": -1.3846006263992148,
+            "timestamp": 1.8238400380276971
+        },
+        {
+            "x": 7.684759443310988,
+            "y": 7.301931264284962,
+            "heading": -3.1822787877008336,
+            "angularVelocity": -0.3860471334464062,
+            "velocityX": 0.046999382572933714,
+            "velocityY": -1.6300495989311972,
+            "timestamp": 1.925106119638729
+        },
+        {
+            "x": 7.607446461395855,
+            "y": 7.1130338870077985,
+            "heading": -3.2115348858230854,
+            "angularVelocity": -0.2889032305471994,
+            "velocityX": -0.7634637450650118,
+            "velocityY": -1.8653568329339223,
+            "timestamp": 2.0263722012497607
+        },
+        {
+            "x": 7.447519490138273,
+            "y": 6.902260922095987,
+            "heading": -3.2309059414306347,
+            "angularVelocity": -0.1912886852083134,
+            "velocityX": -1.5792748046828227,
+            "velocityY": -2.0813777086972793,
+            "timestamp": 2.1276382828607927
+        },
+        {
+            "x": 7.203787473069244,
+            "y": 6.674749702005586,
+            "heading": -3.240291639650134,
+            "angularVelocity": -0.09268353302700813,
+            "velocityX": -2.406847516870182,
+            "velocityY": -2.246667556121621,
+            "timestamp": 2.2289043644718247
+        },
+        {
+            "x": 6.8784205377457015,
+            "y": 6.472538498060418,
+            "heading": -3.240291634989942,
+            "angularVelocity": 4.6019273769549756e-8,
+            "velocityX": -3.212990274205886,
+            "velocityY": -1.996830535247213,
+            "timestamp": 2.3301704460828567
+        },
+        {
+            "x": 6.516724017217928,
+            "y": 6.34632974848032,
+            "heading": -3.2402916309896224,
+            "angularVelocity": 3.950305172889797e-8,
+            "velocityX": -3.5717440111670684,
+            "velocityY": -1.2463082166308144,
+            "timestamp": 2.4314365276938887
+        },
+        {
+            "x": 6.155023833001519,
+            "y": 6.220131499152302,
+            "heading": -3.240291626989329,
+            "angularVelocity": 3.950279970360729e-8,
+            "velocityX": -3.571780189992463,
+            "velocityY": -1.2462045269279236,
+            "timestamp": 2.5327026093049207
+        },
+        {
+            "x": 5.793323648774168,
+            "y": 6.093933249855646,
+            "heading": -3.240291622989035,
+            "angularVelocity": 3.950280005859099e-8,
+            "velocityX": -3.571780190100518,
+            "velocityY": -1.2462045266182225,
+            "timestamp": 2.6339686909159528
+        },
+        {
+            "x": 5.431623464546816,
+            "y": 5.96773500055899,
+            "heading": -3.240291618988741,
+            "angularVelocity": 3.9502799442138326e-8,
+            "velocityX": -3.5717801901005184,
+            "velocityY": -1.246204526618221,
+            "timestamp": 2.7352347725269848
+        },
+        {
+            "x": 5.0699232803112935,
+            "y": 5.841536751285806,
+            "heading": -3.240291614988434,
+            "angularVelocity": 3.9502931091413385e-8,
+            "velocityX": -3.571780190181203,
+            "velocityY": -1.2462045263864454,
+            "timestamp": 2.836500854138017
+        },
+        {
+            "x": 4.747153979068025,
+            "y": 5.72892205607683,
+            "heading": -3.2014500073341168,
+            "angularVelocity": 0.383559895242649,
+            "velocityX": -3.1873387032305858,
+            "velocityY": -1.11206727284526,
+            "timestamp": 2.937766935749049
+        },
+        {
+            "x": 4.505076992705577,
+            "y": 5.644461029601604,
+            "heading": -3.172317846735081,
+            "angularVelocity": 0.2876793506335712,
+            "velocityX": -2.3905041304188885,
+            "velocityY": -0.8340505046852051,
+            "timestamp": 3.039033017360081
+        },
+        {
+            "x": 4.343692332074418,
+            "y": 5.588153676944576,
+            "heading": -3.1528961309698516,
+            "angularVelocity": 0.19178895298675563,
+            "velocityX": -1.5936694504591111,
+            "velocityY": -0.5560336863163278,
+            "timestamp": 3.140299098971113
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0.0958941398322912,
+            "velocityX": -0.796834742598023,
+            "velocityY": -0.2780168492417652,
+            "timestamp": 3.241565180582145
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0,
+            "velocityX": 6.013328716711935e-32,
+            "velocityY": 0,
+            "timestamp": 3.342831262193177
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S2.traj
new file mode 100644
index 00000000..7e79decb
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S2.traj	
@@ -0,0 +1,337 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.360726976398386,
+            "y": 6.773087618158843,
+            "heading": -2.760701916982345,
+            "angularVelocity": -0.1232236665180751,
+            "velocityX": 0.699220588306308,
+            "velocityY": 0.18523553288193312,
+            "timestamp": 0.08684952562036656
+        },
+        {
+            "x": 2.4821843215925723,
+            "y": 6.805261248290758,
+            "heading": -2.781888481642216,
+            "angularVelocity": -0.243945657831127,
+            "velocityX": 1.3984802372452498,
+            "velocityY": 0.37045257187160163,
+            "timestamp": 0.17369905124073312
+        },
+        {
+            "x": 2.664377574952537,
+            "y": 6.853518187805784,
+            "heading": -2.813196351929266,
+            "angularVelocity": -0.3604840678552547,
+            "velocityX": 2.0978036674189,
+            "velocityY": 0.5556384927872844,
+            "timestamp": 0.26054857686109967
+        },
+        {
+            "x": 2.9073173781912582,
+            "y": 6.917852930781389,
+            "heading": -2.8538946648532777,
+            "angularVelocity": -0.4686071988668809,
+            "velocityX": 2.797249628060051,
+            "velocityY": 0.7407610175878006,
+            "timestamp": 0.34739810248146624
+        },
+        {
+            "x": 3.211032053030438,
+            "y": 6.99824783750104,
+            "heading": -2.901766372941813,
+            "angularVelocity": -0.5512028735505646,
+            "velocityX": 3.4970216897535775,
+            "velocityY": 0.9256804357344971,
+            "timestamp": 0.4342476281018328
+        },
+        {
+            "x": 3.5286107194849063,
+            "y": 7.082430089360801,
+            "heading": -2.9017663796929978,
+            "angularVelocity": -7.773427725096788e-8,
+            "velocityX": 3.656654013778666,
+            "velocityY": 0.9692885626994009,
+            "timestamp": 0.5210971537221993
+        },
+        {
+            "x": 3.846189385784725,
+            "y": 7.166612341804015,
+            "heading": -2.9017663864441645,
+            "angularVelocity": -7.773406554275384e-8,
+            "velocityX": 3.6566540119980107,
+            "velocityY": 0.9692885694173843,
+            "timestamp": 0.6079466793425659
+        },
+        {
+            "x": 4.163768052084543,
+            "y": 7.250794594247232,
+            "heading": -2.9017663931953313,
+            "angularVelocity": -7.77340646408651e-8,
+            "velocityX": 3.656654011998002,
+            "velocityY": 0.969288569417417,
+            "timestamp": 0.6947962049629324
+        },
+        {
+            "x": 4.481346718384361,
+            "y": 7.334976846690449,
+            "heading": -2.901766399946498,
+            "angularVelocity": -7.773406451460054e-8,
+            "velocityX": 3.656654011998002,
+            "velocityY": 0.9692885694174173,
+            "timestamp": 0.7816457305832989
+        },
+        {
+            "x": 4.798925384684178,
+            "y": 7.419159099133665,
+            "heading": -2.9017664066976647,
+            "angularVelocity": -7.773406529246814e-8,
+            "velocityX": 3.656654011998002,
+            "velocityY": 0.9692885694174173,
+            "timestamp": 0.8684952562036654
+        },
+        {
+            "x": 5.1165040509839965,
+            "y": 7.503341351576882,
+            "heading": -2.9017664134488315,
+            "angularVelocity": -7.773406535280122e-8,
+            "velocityX": 3.656654011998002,
+            "velocityY": 0.969288569417417,
+            "timestamp": 0.9553447818240319
+        },
+        {
+            "x": 5.434082717283814,
+            "y": 7.5875236040201,
+            "heading": -2.901766420199998,
+            "angularVelocity": -7.773406538713727e-8,
+            "velocityX": 3.6566540119980018,
+            "velocityY": 0.9692885694174171,
+            "timestamp": 1.0421943074443984
+        },
+        {
+            "x": 5.751661383583632,
+            "y": 7.6717058564633165,
+            "heading": -2.901766426951165,
+            "angularVelocity": -7.773406530909276e-8,
+            "velocityX": 3.6566540119980027,
+            "velocityY": 0.969288569417416,
+            "timestamp": 1.129043833064765
+        },
+        {
+            "x": 6.0692400498888315,
+            "y": 7.755888108886232,
+            "heading": -2.9017664337023317,
+            "angularVelocity": -7.773406498550699e-8,
+            "velocityX": 3.6566540120599655,
+            "velocityY": 0.9692885691836598,
+            "timestamp": 1.2158933586851315
+        },
+        {
+            "x": 6.386819824167639,
+            "y": 7.840066181352823,
+            "heading": -2.901766440453515,
+            "angularVelocity": -7.773425363968975e-8,
+            "velocityX": 3.6566667694538877,
+            "velocityY": 0.9692404404655809,
+            "timestamp": 1.302742884305498
+        },
+        {
+            "x": 6.714420971530167,
+            "y": 7.864972939151579,
+            "heading": -2.901766454732528,
+            "angularVelocity": -1.6441095007226412e-7,
+            "velocityX": 3.7720545394171228,
+            "velocityY": 0.28678058539594425,
+            "timestamp": 1.3895924099258645
+        },
+        {
+            "x": 7.024629196469154,
+            "y": 7.83076867028296,
+            "heading": -2.946892502448731,
+            "angularVelocity": -0.5195888796655828,
+            "velocityX": 3.57178951437027,
+            "velocityY": -0.39383368676145797,
+            "timestamp": 1.476441935546231
+        },
+        {
+            "x": 7.277681557115376,
+            "y": 7.7704292121587075,
+            "heading": -3.000549099570889,
+            "angularVelocity": -0.6178110558347718,
+            "velocityX": 2.9136873096139366,
+            "velocityY": -0.6947586379213355,
+            "timestamp": 1.5632914611665976
+        },
+        {
+            "x": 7.471595029086891,
+            "y": 7.688687529495423,
+            "heading": -3.0531246016667453,
+            "angularVelocity": -0.6053631464343129,
+            "velocityX": 2.232752229631644,
+            "velocityY": -0.941187439761266,
+            "timestamp": 1.650140986786964
+        },
+        {
+            "x": 7.605791298108634,
+            "y": 7.5872519075766585,
+            "heading": -3.101243004619086,
+            "angularVelocity": -0.5540433595763132,
+            "velocityX": 1.5451583421207176,
+            "velocityY": -1.1679467584193457,
+            "timestamp": 1.7369905124073306
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.4829307041215213,
+            "velocityX": 0.854451436102516,
+            "velocityY": -1.3846006263992148,
+            "timestamp": 1.8238400380276971
+        },
+        {
+            "x": 7.684759443310988,
+            "y": 7.301931264284962,
+            "heading": -3.1822787877008336,
+            "angularVelocity": -0.3860471334464062,
+            "velocityX": 0.046999382572933714,
+            "velocityY": -1.6300495989311972,
+            "timestamp": 1.925106119638729
+        },
+        {
+            "x": 7.607446461395855,
+            "y": 7.1130338870077985,
+            "heading": -3.2115348858230854,
+            "angularVelocity": -0.2889032305471994,
+            "velocityX": -0.7634637450650118,
+            "velocityY": -1.8653568329339223,
+            "timestamp": 2.0263722012497607
+        },
+        {
+            "x": 7.447519490138273,
+            "y": 6.902260922095987,
+            "heading": -3.2309059414306347,
+            "angularVelocity": -0.1912886852083134,
+            "velocityX": -1.5792748046828227,
+            "velocityY": -2.0813777086972793,
+            "timestamp": 2.1276382828607927
+        },
+        {
+            "x": 7.203787473069244,
+            "y": 6.674749702005586,
+            "heading": -3.240291639650134,
+            "angularVelocity": -0.09268353302700813,
+            "velocityX": -2.406847516870182,
+            "velocityY": -2.246667556121621,
+            "timestamp": 2.2289043644718247
+        },
+        {
+            "x": 6.8784205377457015,
+            "y": 6.472538498060418,
+            "heading": -3.240291634989942,
+            "angularVelocity": 4.6019273769549756e-8,
+            "velocityX": -3.212990274205886,
+            "velocityY": -1.996830535247213,
+            "timestamp": 2.3301704460828567
+        },
+        {
+            "x": 6.516724017217928,
+            "y": 6.34632974848032,
+            "heading": -3.2402916309896224,
+            "angularVelocity": 3.950305172889797e-8,
+            "velocityX": -3.5717440111670684,
+            "velocityY": -1.2463082166308144,
+            "timestamp": 2.4314365276938887
+        },
+        {
+            "x": 6.155023833001519,
+            "y": 6.220131499152302,
+            "heading": -3.240291626989329,
+            "angularVelocity": 3.950279970360729e-8,
+            "velocityX": -3.571780189992463,
+            "velocityY": -1.2462045269279236,
+            "timestamp": 2.5327026093049207
+        },
+        {
+            "x": 5.793323648774168,
+            "y": 6.093933249855646,
+            "heading": -3.240291622989035,
+            "angularVelocity": 3.950280005859099e-8,
+            "velocityX": -3.571780190100518,
+            "velocityY": -1.2462045266182225,
+            "timestamp": 2.6339686909159528
+        },
+        {
+            "x": 5.431623464546816,
+            "y": 5.96773500055899,
+            "heading": -3.240291618988741,
+            "angularVelocity": 3.9502799442138326e-8,
+            "velocityX": -3.5717801901005184,
+            "velocityY": -1.246204526618221,
+            "timestamp": 2.7352347725269848
+        },
+        {
+            "x": 5.0699232803112935,
+            "y": 5.841536751285806,
+            "heading": -3.240291614988434,
+            "angularVelocity": 3.9502931091413385e-8,
+            "velocityX": -3.571780190181203,
+            "velocityY": -1.2462045263864454,
+            "timestamp": 2.836500854138017
+        },
+        {
+            "x": 4.747153979068025,
+            "y": 5.72892205607683,
+            "heading": -3.2014500073341168,
+            "angularVelocity": 0.383559895242649,
+            "velocityX": -3.1873387032305858,
+            "velocityY": -1.11206727284526,
+            "timestamp": 2.937766935749049
+        },
+        {
+            "x": 4.505076992705577,
+            "y": 5.644461029601604,
+            "heading": -3.172317846735081,
+            "angularVelocity": 0.2876793506335712,
+            "velocityX": -2.3905041304188885,
+            "velocityY": -0.8340505046852051,
+            "timestamp": 3.039033017360081
+        },
+        {
+            "x": 4.343692332074418,
+            "y": 5.588153676944576,
+            "heading": -3.1528961309698516,
+            "angularVelocity": 0.19178895298675563,
+            "velocityX": -1.5936694504591111,
+            "velocityY": -0.5560336863163278,
+            "timestamp": 3.140299098971113
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0.0958941398322912,
+            "velocityX": -0.796834742598023,
+            "velocityY": -0.2780168492417652,
+            "timestamp": 3.241565180582145
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0,
+            "velocityX": 6.013328716711935e-32,
+            "velocityY": 0,
+            "timestamp": 3.342831262193177
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S1.1.traj
new file mode 100644
index 00000000..a6012f8c
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S1.1.traj	
@@ -0,0 +1,319 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3639970489597717,
+            "y": 6.742603274185352,
+            "heading": -2.7625259389239276,
+            "angularVelocity": -0.14112178255834015,
+            "velocityX": 0.7210140239806111,
+            "velocityY": -0.16219874791866862,
+            "timestamp": 0.08875978390330586
+        },
+        {
+            "x": 2.4920132643077064,
+            "y": 6.7138069605767345,
+            "heading": -2.7859988261252213,
+            "angularVelocity": -0.26445408234505485,
+            "velocityX": 1.4422772309517358,
+            "velocityY": -0.3244297399370449,
+            "timestamp": 0.1775195678066117
+        },
+        {
+            "x": 2.684079448734256,
+            "y": 6.670607796290334,
+            "heading": -2.8177443395453605,
+            "angularVelocity": -0.3576564973921373,
+            "velocityX": 2.163887472233876,
+            "velocityY": -0.4866974927908953,
+            "timestamp": 0.26627935170991757
+        },
+        {
+            "x": 2.9402327242018096,
+            "y": 6.613005243899921,
+            "heading": -2.852261622202671,
+            "angularVelocity": -0.3888842574798656,
+            "velocityX": 2.8859159430424604,
+            "velocityY": -0.6489713004840627,
+            "timestamp": 0.3550391356132234
+        },
+        {
+            "x": 3.260353111631952,
+            "y": 6.541052974820016,
+            "heading": -2.8718469111116423,
+            "angularVelocity": -0.22065498638783787,
+            "velocityX": 3.6065926859272044,
+            "velocityY": -0.8106404265054099,
+            "timestamp": 0.4437989195165293
+        },
+        {
+            "x": 3.5879291031343383,
+            "y": 6.467313799666466,
+            "heading": -2.87184692127097,
+            "angularVelocity": -1.1445867602426544e-7,
+            "velocityX": 3.690590232387729,
+            "velocityY": -0.83077236008012,
+            "timestamp": 0.5325587034198351
+        },
+        {
+            "x": 3.915505094528652,
+            "y": 6.3935746240328,
+            "heading": -2.87184693143029,
+            "angularVelocity": -1.1445859480672728e-7,
+            "velocityX": 3.6905902311701486,
+            "velocityY": -0.8307723654892717,
+            "timestamp": 0.621318487323141
+        },
+        {
+            "x": 4.243081085922966,
+            "y": 6.3198354483991315,
+            "heading": -2.87184694158961,
+            "angularVelocity": -1.1445859516237229e-7,
+            "velocityX": 3.6905902311701433,
+            "velocityY": -0.8307723654892962,
+            "timestamp": 0.7100782712264468
+        },
+        {
+            "x": 4.5706570773172785,
+            "y": 6.246096272765464,
+            "heading": -2.87184695174893,
+            "angularVelocity": -1.1445859514834922e-7,
+            "velocityX": 3.6905902311701433,
+            "velocityY": -0.830772365489296,
+            "timestamp": 0.7988380551297527
+        },
+        {
+            "x": 4.898233068711592,
+            "y": 6.172357097131795,
+            "heading": -2.8718469619082505,
+            "angularVelocity": -1.1445859490669181e-7,
+            "velocityX": 3.6905902311701437,
+            "velocityY": -0.830772365489296,
+            "timestamp": 0.8875978390330586
+        },
+        {
+            "x": 5.225809060105905,
+            "y": 6.0986179214981275,
+            "heading": -2.8718469720675706,
+            "angularVelocity": -1.1445859563440553e-7,
+            "velocityX": 3.6905902311701433,
+            "velocityY": -0.830772365489296,
+            "timestamp": 0.9763576229363644
+        },
+        {
+            "x": 5.553385051500219,
+            "y": 6.024878745864458,
+            "heading": -2.871846982226891,
+            "angularVelocity": -1.1445859523892014e-7,
+            "velocityX": 3.6905902311701437,
+            "velocityY": -0.830772365489296,
+            "timestamp": 1.0651174068396703
+        },
+        {
+            "x": 5.8809610428945325,
+            "y": 5.951139570230791,
+            "heading": -2.871846992386211,
+            "angularVelocity": -1.1445859598937227e-7,
+            "velocityX": 3.6905902311701433,
+            "velocityY": -0.8307723654892962,
+            "timestamp": 1.1538771907429761
+        },
+        {
+            "x": 6.208537034288882,
+            "y": 5.877400394597284,
+            "heading": -2.871847002545531,
+            "angularVelocity": -1.1445859482620514e-7,
+            "velocityX": 3.6905902311705536,
+            "velocityY": -0.830772365487475,
+            "timestamp": 1.242636974646282
+        },
+        {
+            "x": 6.536113033734776,
+            "y": 5.80366125473165,
+            "heading": -2.8718470127048517,
+            "angularVelocity": -1.1445859560539411e-7,
+            "velocityX": 3.6905903218821825,
+            "velocityY": -0.8307719625136204,
+            "timestamp": 1.3313967585495878
+        },
+        {
+            "x": 6.865344430510141,
+            "y": 5.737705340336343,
+            "heading": -2.8718470255455273,
+            "angularVelocity": -1.4466772317254716e-7,
+            "velocityX": 3.7092406301262124,
+            "velocityY": -0.7430833142536372,
+            "timestamp": 1.4201565424528937
+        },
+        {
+            "x": 7.155715453065551,
+            "y": 5.718947060788103,
+            "heading": -2.9583296823858576,
+            "angularVelocity": -0.974345058506937,
+            "velocityX": 3.2714255238807013,
+            "velocityY": -0.21133759821538473,
+            "timestamp": 1.5089163263561995
+        },
+        {
+            "x": 7.382461638266762,
+            "y": 5.716006595883618,
+            "heading": -3.030136340606869,
+            "angularVelocity": -0.8089999216225757,
+            "velocityX": 2.5546049711908463,
+            "velocityY": -0.0331283468162716,
+            "timestamp": 1.5976761102595054
+        },
+        {
+            "x": 7.54544918703291,
+            "y": 5.728176149550433,
+            "heading": -3.085282134377374,
+            "angularVelocity": -0.6212925645535633,
+            "velocityX": 1.836276989404401,
+            "velocityY": 0.13710661666404886,
+            "timestamp": 1.6864358941628113
+        },
+        {
+            "x": 7.644635592677741,
+            "y": 5.7552144852221,
+            "heading": -3.1230806073530184,
+            "angularVelocity": -0.4258513407020198,
+            "velocityX": 1.117470111834477,
+            "velocityY": 0.3046237212691066,
+            "timestamp": 1.775195678066117
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.2265068586519956,
+            "velocityX": 0.39842827198390307,
+            "velocityY": 0.4707708034014574,
+            "timestamp": 1.863955461969423
+        },
+        {
+            "x": 7.649158607972273,
+            "y": 5.85526478345475,
+            "heading": -3.144984703286767,
+            "angularVelocity": -0.019773386750234018,
+            "velocityX": -0.3389130220112019,
+            "velocityY": 0.640265971773451,
+            "timestamp": 1.9549563678794555
+        },
+        {
+            "x": 7.551198365445194,
+            "y": 5.928933141126823,
+            "heading": -3.12869095344792,
+            "angularVelocity": 0.17905041357452225,
+            "velocityX": -1.076475465243452,
+            "velocityY": 0.8095343330417561,
+            "timestamp": 2.045957273789488
+        },
+        {
+            "x": 7.386086795598405,
+            "y": 6.0179701455014545,
+            "heading": -3.0955113223095805,
+            "angularVelocity": 0.3646077015007144,
+            "velocityX": -1.814394793058725,
+            "velocityY": 0.9784188792874091,
+            "timestamp": 2.1369581796995205
+        },
+        {
+            "x": 7.153763389046842,
+            "y": 6.12230392350914,
+            "heading": -3.047867696260251,
+            "angularVelocity": 0.5235511182320745,
+            "velocityX": -2.5529790525519296,
+            "velocityY": 1.1465136194450147,
+            "timestamp": 2.227959085609553
+        },
+        {
+            "x": 6.854082785517415,
+            "y": 6.241701438550574,
+            "heading": -2.993162545915138,
+            "angularVelocity": 0.6011495138212849,
+            "velocityX": -3.29316066178182,
+            "velocityY": 1.3120475433450827,
+            "timestamp": 2.3189599915195855
+        },
+        {
+            "x": 6.518695058786276,
+            "y": 6.319314800630408,
+            "heading": -2.9931625344725843,
+            "angularVelocity": 1.257410975035093e-7,
+            "velocityX": -3.685542724846253,
+            "velocityY": 0.8528855982660521,
+            "timestamp": 2.409960897429618
+        },
+        {
+            "x": 6.18330715393461,
+            "y": 6.396927393000049,
+            "heading": -2.993162523030137,
+            "angularVelocity": 1.2573992931558898e-7,
+            "velocityX": -3.6855446821952005,
+            "velocityY": 0.8528771399965199,
+            "timestamp": 2.5009618033396506
+        },
+        {
+            "x": 5.8479121197525,
+            "y": 6.474508873089667,
+            "heading": -2.9931623885517284,
+            "angularVelocity": 0.0000014777699956967743,
+            "velocityX": -3.685623025705882,
+            "velocityY": 0.852535250212982,
+            "timestamp": 2.591962709249683
+        },
+        {
+            "x": 5.579069881205996,
+            "y": 6.536724652078007,
+            "heading": -2.956924011318997,
+            "angularVelocity": 0.3982199613326688,
+            "velocityX": -2.954280903668077,
+            "velocityY": 0.6836830728899436,
+            "timestamp": 2.6829636151597156
+        },
+        {
+            "x": 5.377498755107576,
+            "y": 6.583371353425055,
+            "heading": -2.9212816182082424,
+            "angularVelocity": 0.391670750464755,
+            "velocityX": -2.2150452688647118,
+            "velocityY": 0.5125960107821789,
+            "timestamp": 2.773964521069748
+        },
+        {
+            "x": 5.243156221242747,
+            "y": 6.614459642803244,
+            "heading": -2.894437298256596,
+            "angularVelocity": 0.29498959030348726,
+            "velocityX": -1.4762768845141638,
+            "velocityY": 0.341626152699224,
+            "timestamp": 2.8649654269797806
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0.15865004982334352,
+            "velocityX": -0.7379731066539015,
+            "velocityY": 0.17077145596902096,
+            "timestamp": 2.955966332889813
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0,
+            "velocityX": 2.398158538123596e-33,
+            "velocityY": 0,
+            "timestamp": 3.0469672387998457
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S1.traj
new file mode 100644
index 00000000..a6012f8c
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S1.traj	
@@ -0,0 +1,319 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3639970489597717,
+            "y": 6.742603274185352,
+            "heading": -2.7625259389239276,
+            "angularVelocity": -0.14112178255834015,
+            "velocityX": 0.7210140239806111,
+            "velocityY": -0.16219874791866862,
+            "timestamp": 0.08875978390330586
+        },
+        {
+            "x": 2.4920132643077064,
+            "y": 6.7138069605767345,
+            "heading": -2.7859988261252213,
+            "angularVelocity": -0.26445408234505485,
+            "velocityX": 1.4422772309517358,
+            "velocityY": -0.3244297399370449,
+            "timestamp": 0.1775195678066117
+        },
+        {
+            "x": 2.684079448734256,
+            "y": 6.670607796290334,
+            "heading": -2.8177443395453605,
+            "angularVelocity": -0.3576564973921373,
+            "velocityX": 2.163887472233876,
+            "velocityY": -0.4866974927908953,
+            "timestamp": 0.26627935170991757
+        },
+        {
+            "x": 2.9402327242018096,
+            "y": 6.613005243899921,
+            "heading": -2.852261622202671,
+            "angularVelocity": -0.3888842574798656,
+            "velocityX": 2.8859159430424604,
+            "velocityY": -0.6489713004840627,
+            "timestamp": 0.3550391356132234
+        },
+        {
+            "x": 3.260353111631952,
+            "y": 6.541052974820016,
+            "heading": -2.8718469111116423,
+            "angularVelocity": -0.22065498638783787,
+            "velocityX": 3.6065926859272044,
+            "velocityY": -0.8106404265054099,
+            "timestamp": 0.4437989195165293
+        },
+        {
+            "x": 3.5879291031343383,
+            "y": 6.467313799666466,
+            "heading": -2.87184692127097,
+            "angularVelocity": -1.1445867602426544e-7,
+            "velocityX": 3.690590232387729,
+            "velocityY": -0.83077236008012,
+            "timestamp": 0.5325587034198351
+        },
+        {
+            "x": 3.915505094528652,
+            "y": 6.3935746240328,
+            "heading": -2.87184693143029,
+            "angularVelocity": -1.1445859480672728e-7,
+            "velocityX": 3.6905902311701486,
+            "velocityY": -0.8307723654892717,
+            "timestamp": 0.621318487323141
+        },
+        {
+            "x": 4.243081085922966,
+            "y": 6.3198354483991315,
+            "heading": -2.87184694158961,
+            "angularVelocity": -1.1445859516237229e-7,
+            "velocityX": 3.6905902311701433,
+            "velocityY": -0.8307723654892962,
+            "timestamp": 0.7100782712264468
+        },
+        {
+            "x": 4.5706570773172785,
+            "y": 6.246096272765464,
+            "heading": -2.87184695174893,
+            "angularVelocity": -1.1445859514834922e-7,
+            "velocityX": 3.6905902311701433,
+            "velocityY": -0.830772365489296,
+            "timestamp": 0.7988380551297527
+        },
+        {
+            "x": 4.898233068711592,
+            "y": 6.172357097131795,
+            "heading": -2.8718469619082505,
+            "angularVelocity": -1.1445859490669181e-7,
+            "velocityX": 3.6905902311701437,
+            "velocityY": -0.830772365489296,
+            "timestamp": 0.8875978390330586
+        },
+        {
+            "x": 5.225809060105905,
+            "y": 6.0986179214981275,
+            "heading": -2.8718469720675706,
+            "angularVelocity": -1.1445859563440553e-7,
+            "velocityX": 3.6905902311701433,
+            "velocityY": -0.830772365489296,
+            "timestamp": 0.9763576229363644
+        },
+        {
+            "x": 5.553385051500219,
+            "y": 6.024878745864458,
+            "heading": -2.871846982226891,
+            "angularVelocity": -1.1445859523892014e-7,
+            "velocityX": 3.6905902311701437,
+            "velocityY": -0.830772365489296,
+            "timestamp": 1.0651174068396703
+        },
+        {
+            "x": 5.8809610428945325,
+            "y": 5.951139570230791,
+            "heading": -2.871846992386211,
+            "angularVelocity": -1.1445859598937227e-7,
+            "velocityX": 3.6905902311701433,
+            "velocityY": -0.8307723654892962,
+            "timestamp": 1.1538771907429761
+        },
+        {
+            "x": 6.208537034288882,
+            "y": 5.877400394597284,
+            "heading": -2.871847002545531,
+            "angularVelocity": -1.1445859482620514e-7,
+            "velocityX": 3.6905902311705536,
+            "velocityY": -0.830772365487475,
+            "timestamp": 1.242636974646282
+        },
+        {
+            "x": 6.536113033734776,
+            "y": 5.80366125473165,
+            "heading": -2.8718470127048517,
+            "angularVelocity": -1.1445859560539411e-7,
+            "velocityX": 3.6905903218821825,
+            "velocityY": -0.8307719625136204,
+            "timestamp": 1.3313967585495878
+        },
+        {
+            "x": 6.865344430510141,
+            "y": 5.737705340336343,
+            "heading": -2.8718470255455273,
+            "angularVelocity": -1.4466772317254716e-7,
+            "velocityX": 3.7092406301262124,
+            "velocityY": -0.7430833142536372,
+            "timestamp": 1.4201565424528937
+        },
+        {
+            "x": 7.155715453065551,
+            "y": 5.718947060788103,
+            "heading": -2.9583296823858576,
+            "angularVelocity": -0.974345058506937,
+            "velocityX": 3.2714255238807013,
+            "velocityY": -0.21133759821538473,
+            "timestamp": 1.5089163263561995
+        },
+        {
+            "x": 7.382461638266762,
+            "y": 5.716006595883618,
+            "heading": -3.030136340606869,
+            "angularVelocity": -0.8089999216225757,
+            "velocityX": 2.5546049711908463,
+            "velocityY": -0.0331283468162716,
+            "timestamp": 1.5976761102595054
+        },
+        {
+            "x": 7.54544918703291,
+            "y": 5.728176149550433,
+            "heading": -3.085282134377374,
+            "angularVelocity": -0.6212925645535633,
+            "velocityX": 1.836276989404401,
+            "velocityY": 0.13710661666404886,
+            "timestamp": 1.6864358941628113
+        },
+        {
+            "x": 7.644635592677741,
+            "y": 5.7552144852221,
+            "heading": -3.1230806073530184,
+            "angularVelocity": -0.4258513407020198,
+            "velocityX": 1.117470111834477,
+            "velocityY": 0.3046237212691066,
+            "timestamp": 1.775195678066117
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.2265068586519956,
+            "velocityX": 0.39842827198390307,
+            "velocityY": 0.4707708034014574,
+            "timestamp": 1.863955461969423
+        },
+        {
+            "x": 7.649158607972273,
+            "y": 5.85526478345475,
+            "heading": -3.144984703286767,
+            "angularVelocity": -0.019773386750234018,
+            "velocityX": -0.3389130220112019,
+            "velocityY": 0.640265971773451,
+            "timestamp": 1.9549563678794555
+        },
+        {
+            "x": 7.551198365445194,
+            "y": 5.928933141126823,
+            "heading": -3.12869095344792,
+            "angularVelocity": 0.17905041357452225,
+            "velocityX": -1.076475465243452,
+            "velocityY": 0.8095343330417561,
+            "timestamp": 2.045957273789488
+        },
+        {
+            "x": 7.386086795598405,
+            "y": 6.0179701455014545,
+            "heading": -3.0955113223095805,
+            "angularVelocity": 0.3646077015007144,
+            "velocityX": -1.814394793058725,
+            "velocityY": 0.9784188792874091,
+            "timestamp": 2.1369581796995205
+        },
+        {
+            "x": 7.153763389046842,
+            "y": 6.12230392350914,
+            "heading": -3.047867696260251,
+            "angularVelocity": 0.5235511182320745,
+            "velocityX": -2.5529790525519296,
+            "velocityY": 1.1465136194450147,
+            "timestamp": 2.227959085609553
+        },
+        {
+            "x": 6.854082785517415,
+            "y": 6.241701438550574,
+            "heading": -2.993162545915138,
+            "angularVelocity": 0.6011495138212849,
+            "velocityX": -3.29316066178182,
+            "velocityY": 1.3120475433450827,
+            "timestamp": 2.3189599915195855
+        },
+        {
+            "x": 6.518695058786276,
+            "y": 6.319314800630408,
+            "heading": -2.9931625344725843,
+            "angularVelocity": 1.257410975035093e-7,
+            "velocityX": -3.685542724846253,
+            "velocityY": 0.8528855982660521,
+            "timestamp": 2.409960897429618
+        },
+        {
+            "x": 6.18330715393461,
+            "y": 6.396927393000049,
+            "heading": -2.993162523030137,
+            "angularVelocity": 1.2573992931558898e-7,
+            "velocityX": -3.6855446821952005,
+            "velocityY": 0.8528771399965199,
+            "timestamp": 2.5009618033396506
+        },
+        {
+            "x": 5.8479121197525,
+            "y": 6.474508873089667,
+            "heading": -2.9931623885517284,
+            "angularVelocity": 0.0000014777699956967743,
+            "velocityX": -3.685623025705882,
+            "velocityY": 0.852535250212982,
+            "timestamp": 2.591962709249683
+        },
+        {
+            "x": 5.579069881205996,
+            "y": 6.536724652078007,
+            "heading": -2.956924011318997,
+            "angularVelocity": 0.3982199613326688,
+            "velocityX": -2.954280903668077,
+            "velocityY": 0.6836830728899436,
+            "timestamp": 2.6829636151597156
+        },
+        {
+            "x": 5.377498755107576,
+            "y": 6.583371353425055,
+            "heading": -2.9212816182082424,
+            "angularVelocity": 0.391670750464755,
+            "velocityX": -2.2150452688647118,
+            "velocityY": 0.5125960107821789,
+            "timestamp": 2.773964521069748
+        },
+        {
+            "x": 5.243156221242747,
+            "y": 6.614459642803244,
+            "heading": -2.894437298256596,
+            "angularVelocity": 0.29498959030348726,
+            "velocityX": -1.4762768845141638,
+            "velocityY": 0.341626152699224,
+            "timestamp": 2.8649654269797806
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0.15865004982334352,
+            "velocityX": -0.7379731066539015,
+            "velocityY": 0.17077145596902096,
+            "timestamp": 2.955966332889813
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0,
+            "velocityX": 2.398158538123596e-33,
+            "velocityY": 0,
+            "timestamp": 3.0469672387998457
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S2.1.traj
new file mode 100644
index 00000000..84327156
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S2.1.traj	
@@ -0,0 +1,328 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.36406618503168,
+            "y": 6.750841975187211,
+            "heading": -2.7578611838826945,
+            "angularVelocity": -0.0894504040603958,
+            "velocityX": 0.7289927602771729,
+            "velocityY": -0.07007059190289512,
+            "timestamp": 0.08788315676457649
+        },
+        {
+            "x": 2.4922026900458016,
+            "y": 6.738526737153379,
+            "heading": -2.773150566031247,
+            "angularVelocity": -0.17397397534899775,
+            "velocityX": 1.458032571103211,
+            "velocityY": -0.1401319489105608,
+            "timestamp": 0.17576631352915298
+        },
+        {
+            "x": 2.6844159261129263,
+            "y": 6.720055698035347,
+            "heading": -2.7951433359678384,
+            "angularVelocity": -0.25025011328969643,
+            "velocityX": 2.187145331864103,
+            "velocityY": -0.21017723757365894,
+            "timestamp": 0.2636494702937295
+        },
+        {
+            "x": 2.940716876843772,
+            "y": 6.6954319085352205,
+            "heading": -2.8223782521748206,
+            "angularVelocity": -0.3098991571267751,
+            "velocityX": 2.9163830723267137,
+            "velocityY": -0.2801878130765108,
+            "timestamp": 0.35153262705830596
+        },
+        {
+            "x": 3.261123048295386,
+            "y": 6.664666336504878,
+            "heading": -2.850384657388346,
+            "angularVelocity": -0.318677733533739,
+            "velocityX": 3.645820009742318,
+            "velocityY": -0.35007358819345447,
+            "timestamp": 0.43941578382288243
+        },
+        {
+            "x": 3.5920521261120903,
+            "y": 6.632831781454392,
+            "heading": -2.8503846627195863,
+            "angularVelocity": -6.066282061484616e-8,
+            "velocityX": 3.7655574742633027,
+            "velocityY": -0.3622372730165362,
+            "timestamp": 0.527298940587459
+        },
+        {
+            "x": 3.922981203901898,
+            "y": 6.6009972261242424,
+            "heading": -2.850384668050817,
+            "angularVelocity": -6.066271266149535e-8,
+            "velocityX": 3.7655574739572506,
+            "velocityY": -0.3622372761987803,
+            "timestamp": 0.6151820973520354
+        },
+        {
+            "x": 4.253910281691706,
+            "y": 6.56916267079409,
+            "heading": -2.8503846733820475,
+            "angularVelocity": -6.06627128107855e-8,
+            "velocityX": 3.765557473957249,
+            "velocityY": -0.36223727619879537,
+            "timestamp": 0.7030652541166119
+        },
+        {
+            "x": 4.5848393594815136,
+            "y": 6.537328115463938,
+            "heading": -2.850384678713278,
+            "angularVelocity": -6.066271279819625e-8,
+            "velocityX": 3.765557473957249,
+            "velocityY": -0.3622372761987953,
+            "timestamp": 0.7909484108811884
+        },
+        {
+            "x": 4.915768437271321,
+            "y": 6.505493560133786,
+            "heading": -2.850384684044509,
+            "angularVelocity": -6.066271362462515e-8,
+            "velocityX": 3.7655574739572493,
+            "velocityY": -0.3622372761987953,
+            "timestamp": 0.8788315676457649
+        },
+        {
+            "x": 5.24669751506113,
+            "y": 6.473659004803634,
+            "heading": -2.8503846893757396,
+            "angularVelocity": -6.066271352975027e-8,
+            "velocityX": 3.765557473957249,
+            "velocityY": -0.36223727619879525,
+            "timestamp": 0.9667147244103413
+        },
+        {
+            "x": 5.577626592850938,
+            "y": 6.441824449473482,
+            "heading": -2.85038469470697,
+            "angularVelocity": -6.066271274322426e-8,
+            "velocityX": 3.765557473957249,
+            "velocityY": -0.3622372761987953,
+            "timestamp": 1.054597881174918
+        },
+        {
+            "x": 5.908555670640745,
+            "y": 6.40998989414333,
+            "heading": -2.8503847000382008,
+            "angularVelocity": -6.066271341664136e-8,
+            "velocityX": 3.7655574739572493,
+            "velocityY": -0.36223727619879537,
+            "timestamp": 1.1424810379394945
+        },
+        {
+            "x": 6.239484748430477,
+            "y": 6.37815533881239,
+            "heading": -2.8503847053694313,
+            "angularVelocity": -6.066271364618301e-8,
+            "velocityX": 3.7655574739563864,
+            "velocityY": -0.36223727620776275,
+            "timestamp": 1.230364194704071
+        },
+        {
+            "x": 6.5704138100618055,
+            "y": 6.346320615510802,
+            "heading": -2.8503847107006623,
+            "angularVelocity": -6.066271546575477e-8,
+            "velocityX": 3.7655572900940437,
+            "velocityY": -0.3622391875028854,
+            "timestamp": 1.3182473514686477
+        },
+        {
+            "x": 6.897540531836615,
+            "y": 6.287028122680583,
+            "heading": -2.8503847211917432,
+            "angularVelocity": -1.1937532942487347e-7,
+            "velocityX": 3.722291435788134,
+            "velocityY": -0.6746741356714466,
+            "timestamp": 1.4061305082332243
+        },
+        {
+            "x": 7.182619204890035,
+            "y": 6.1828768490539545,
+            "heading": -2.9185193386552397,
+            "angularVelocity": -0.7752864140510717,
+            "velocityX": 3.243837426289712,
+            "velocityY": -1.1851107477355587,
+            "timestamp": 1.4940136649978009
+        },
+        {
+            "x": 7.403317140168432,
+            "y": 6.080132379818418,
+            "heading": -2.984427246863809,
+            "angularVelocity": -0.749949258025913,
+            "velocityX": 2.511265450666594,
+            "velocityY": -1.1691030797945798,
+            "timestamp": 1.5818968217623774
+        },
+        {
+            "x": 7.559743583293149,
+            "y": 5.981103818181206,
+            "heading": -3.044561457345504,
+            "angularVelocity": -0.6842518258963323,
+            "velocityX": 1.7799365530730349,
+            "velocityY": -1.1268207160843378,
+            "timestamp": 1.669779978526954
+        },
+        {
+            "x": 7.6519593983078185,
+            "y": 5.886591133773424,
+            "heading": -3.097689544379845,
+            "angularVelocity": -0.6045309361913614,
+            "velocityX": 1.0493002118904227,
+            "velocityY": -1.0754357022127217,
+            "timestamp": 1.7576631352915306
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.5176846676276772,
+            "velocityX": 0.3190668465323417,
+            "velocityY": -1.0194346342544611,
+            "timestamp": 1.8455462920561072
+        },
+        {
+            "x": 7.628959612668781,
+            "y": 5.701821981186599,
+            "heading": -3.184554593580224,
+            "angularVelocity": -0.41404680845855485,
+            "velocityX": -0.5108405610944724,
+            "velocityY": -0.9525945055820302,
+            "timestamp": 1.9454608078922528
+        },
+        {
+            "x": 7.495051570255288,
+            "y": 5.613939825179433,
+            "heading": -3.215560246622884,
+            "angularVelocity": -0.3103218064280789,
+            "velocityX": -1.3402261052147335,
+            "velocityY": -0.8795734560860885,
+            "timestamp": 2.0453753237283987
+        },
+        {
+            "x": 7.278385319183743,
+            "y": 5.534503531789237,
+            "heading": -3.2361878862694873,
+            "angularVelocity": -0.2064528809850976,
+            "velocityX": -2.1685162486986918,
+            "velocityY": -0.7950425693946834,
+            "timestamp": 2.1452898395645446
+        },
+        {
+            "x": 6.979307104157413,
+            "y": 5.4664019474569185,
+            "heading": -3.246410388494101,
+            "angularVelocity": -0.10231248321693356,
+            "velocityX": -2.9933409827737334,
+            "velocityY": -0.6815985021035508,
+            "timestamp": 2.2452043554006904
+        },
+        {
+            "x": 6.603136214940544,
+            "y": 5.429560638127869,
+            "heading": -3.2464103824694632,
+            "angularVelocity": 6.029792436770947e-8,
+            "velocityX": -3.7649273087983395,
+            "velocityY": -0.368728297592585,
+            "timestamp": 2.3451188712368363
+        },
+        {
+            "x": 6.225751326953569,
+            "y": 5.450595811881877,
+            "heading": -3.2464103781263827,
+            "angularVelocity": 4.346796393735324e-8,
+            "velocityX": -3.777077683145315,
+            "velocityY": 0.2105317088110093,
+            "timestamp": 2.445033387072982
+        },
+        {
+            "x": 5.848366461127548,
+            "y": 5.47163138321431,
+            "heading": -3.2464103737833025,
+            "angularVelocity": 4.346795775537738e-8,
+            "velocityX": -3.777077461346165,
+            "velocityY": 0.21053568799683506,
+            "timestamp": 2.544947902909128
+        },
+        {
+            "x": 5.470981595301597,
+            "y": 5.492666954547974,
+            "heading": -3.246410369440223,
+            "angularVelocity": 4.346795748687552e-8,
+            "velocityX": -3.777077461345479,
+            "velocityY": 0.21053568800914468,
+            "timestamp": 2.644862418745274
+        },
+        {
+            "x": 5.09359672947572,
+            "y": 5.513702525882575,
+            "heading": -3.246410365097125,
+            "angularVelocity": 4.346813479097949e-8,
+            "velocityX": -3.777077461344731,
+            "velocityY": 0.21053568801853617,
+            "timestamp": 2.7447769345814197
+        },
+        {
+            "x": 4.761358057173187,
+            "y": 5.532221526743186,
+            "heading": -3.20512020544725,
+            "angularVelocity": 0.4132548639637959,
+            "velocityX": -3.3252292674608595,
+            "velocityY": 0.18534845218066273,
+            "timestamp": 2.8446914504175655
+        },
+        {
+            "x": 4.5121790336148475,
+            "y": 5.546110766147867,
+            "heading": -3.174152719009984,
+            "angularVelocity": 0.3099398138309664,
+            "velocityX": -2.4939221440754293,
+            "velocityY": 0.13901122663156756,
+            "timestamp": 2.9446059662537114
+        },
+        {
+            "x": 4.346059678819821,
+            "y": 5.555370255329891,
+            "heading": -3.153507786492084,
+            "angularVelocity": 0.20662595765119826,
+            "velocityX": -1.6626148203275395,
+            "velocityY": 0.09267411351128947,
+            "timestamp": 3.0445204820898573
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0.10331310947276948,
+            "velocityX": -0.8313074243990123,
+            "velocityY": 0.04633705754727876,
+            "timestamp": 3.144434997926003
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0,
+            "velocityX": 1.3822008000428574e-34,
+            "velocityY": 0,
+            "timestamp": 3.244349513762149
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S2.traj
new file mode 100644
index 00000000..84327156
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S2.traj	
@@ -0,0 +1,328 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.36406618503168,
+            "y": 6.750841975187211,
+            "heading": -2.7578611838826945,
+            "angularVelocity": -0.0894504040603958,
+            "velocityX": 0.7289927602771729,
+            "velocityY": -0.07007059190289512,
+            "timestamp": 0.08788315676457649
+        },
+        {
+            "x": 2.4922026900458016,
+            "y": 6.738526737153379,
+            "heading": -2.773150566031247,
+            "angularVelocity": -0.17397397534899775,
+            "velocityX": 1.458032571103211,
+            "velocityY": -0.1401319489105608,
+            "timestamp": 0.17576631352915298
+        },
+        {
+            "x": 2.6844159261129263,
+            "y": 6.720055698035347,
+            "heading": -2.7951433359678384,
+            "angularVelocity": -0.25025011328969643,
+            "velocityX": 2.187145331864103,
+            "velocityY": -0.21017723757365894,
+            "timestamp": 0.2636494702937295
+        },
+        {
+            "x": 2.940716876843772,
+            "y": 6.6954319085352205,
+            "heading": -2.8223782521748206,
+            "angularVelocity": -0.3098991571267751,
+            "velocityX": 2.9163830723267137,
+            "velocityY": -0.2801878130765108,
+            "timestamp": 0.35153262705830596
+        },
+        {
+            "x": 3.261123048295386,
+            "y": 6.664666336504878,
+            "heading": -2.850384657388346,
+            "angularVelocity": -0.318677733533739,
+            "velocityX": 3.645820009742318,
+            "velocityY": -0.35007358819345447,
+            "timestamp": 0.43941578382288243
+        },
+        {
+            "x": 3.5920521261120903,
+            "y": 6.632831781454392,
+            "heading": -2.8503846627195863,
+            "angularVelocity": -6.066282061484616e-8,
+            "velocityX": 3.7655574742633027,
+            "velocityY": -0.3622372730165362,
+            "timestamp": 0.527298940587459
+        },
+        {
+            "x": 3.922981203901898,
+            "y": 6.6009972261242424,
+            "heading": -2.850384668050817,
+            "angularVelocity": -6.066271266149535e-8,
+            "velocityX": 3.7655574739572506,
+            "velocityY": -0.3622372761987803,
+            "timestamp": 0.6151820973520354
+        },
+        {
+            "x": 4.253910281691706,
+            "y": 6.56916267079409,
+            "heading": -2.8503846733820475,
+            "angularVelocity": -6.06627128107855e-8,
+            "velocityX": 3.765557473957249,
+            "velocityY": -0.36223727619879537,
+            "timestamp": 0.7030652541166119
+        },
+        {
+            "x": 4.5848393594815136,
+            "y": 6.537328115463938,
+            "heading": -2.850384678713278,
+            "angularVelocity": -6.066271279819625e-8,
+            "velocityX": 3.765557473957249,
+            "velocityY": -0.3622372761987953,
+            "timestamp": 0.7909484108811884
+        },
+        {
+            "x": 4.915768437271321,
+            "y": 6.505493560133786,
+            "heading": -2.850384684044509,
+            "angularVelocity": -6.066271362462515e-8,
+            "velocityX": 3.7655574739572493,
+            "velocityY": -0.3622372761987953,
+            "timestamp": 0.8788315676457649
+        },
+        {
+            "x": 5.24669751506113,
+            "y": 6.473659004803634,
+            "heading": -2.8503846893757396,
+            "angularVelocity": -6.066271352975027e-8,
+            "velocityX": 3.765557473957249,
+            "velocityY": -0.36223727619879525,
+            "timestamp": 0.9667147244103413
+        },
+        {
+            "x": 5.577626592850938,
+            "y": 6.441824449473482,
+            "heading": -2.85038469470697,
+            "angularVelocity": -6.066271274322426e-8,
+            "velocityX": 3.765557473957249,
+            "velocityY": -0.3622372761987953,
+            "timestamp": 1.054597881174918
+        },
+        {
+            "x": 5.908555670640745,
+            "y": 6.40998989414333,
+            "heading": -2.8503847000382008,
+            "angularVelocity": -6.066271341664136e-8,
+            "velocityX": 3.7655574739572493,
+            "velocityY": -0.36223727619879537,
+            "timestamp": 1.1424810379394945
+        },
+        {
+            "x": 6.239484748430477,
+            "y": 6.37815533881239,
+            "heading": -2.8503847053694313,
+            "angularVelocity": -6.066271364618301e-8,
+            "velocityX": 3.7655574739563864,
+            "velocityY": -0.36223727620776275,
+            "timestamp": 1.230364194704071
+        },
+        {
+            "x": 6.5704138100618055,
+            "y": 6.346320615510802,
+            "heading": -2.8503847107006623,
+            "angularVelocity": -6.066271546575477e-8,
+            "velocityX": 3.7655572900940437,
+            "velocityY": -0.3622391875028854,
+            "timestamp": 1.3182473514686477
+        },
+        {
+            "x": 6.897540531836615,
+            "y": 6.287028122680583,
+            "heading": -2.8503847211917432,
+            "angularVelocity": -1.1937532942487347e-7,
+            "velocityX": 3.722291435788134,
+            "velocityY": -0.6746741356714466,
+            "timestamp": 1.4061305082332243
+        },
+        {
+            "x": 7.182619204890035,
+            "y": 6.1828768490539545,
+            "heading": -2.9185193386552397,
+            "angularVelocity": -0.7752864140510717,
+            "velocityX": 3.243837426289712,
+            "velocityY": -1.1851107477355587,
+            "timestamp": 1.4940136649978009
+        },
+        {
+            "x": 7.403317140168432,
+            "y": 6.080132379818418,
+            "heading": -2.984427246863809,
+            "angularVelocity": -0.749949258025913,
+            "velocityX": 2.511265450666594,
+            "velocityY": -1.1691030797945798,
+            "timestamp": 1.5818968217623774
+        },
+        {
+            "x": 7.559743583293149,
+            "y": 5.981103818181206,
+            "heading": -3.044561457345504,
+            "angularVelocity": -0.6842518258963323,
+            "velocityX": 1.7799365530730349,
+            "velocityY": -1.1268207160843378,
+            "timestamp": 1.669779978526954
+        },
+        {
+            "x": 7.6519593983078185,
+            "y": 5.886591133773424,
+            "heading": -3.097689544379845,
+            "angularVelocity": -0.6045309361913614,
+            "velocityX": 1.0493002118904227,
+            "velocityY": -1.0754357022127217,
+            "timestamp": 1.7576631352915306
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.5176846676276772,
+            "velocityX": 0.3190668465323417,
+            "velocityY": -1.0194346342544611,
+            "timestamp": 1.8455462920561072
+        },
+        {
+            "x": 7.628959612668781,
+            "y": 5.701821981186599,
+            "heading": -3.184554593580224,
+            "angularVelocity": -0.41404680845855485,
+            "velocityX": -0.5108405610944724,
+            "velocityY": -0.9525945055820302,
+            "timestamp": 1.9454608078922528
+        },
+        {
+            "x": 7.495051570255288,
+            "y": 5.613939825179433,
+            "heading": -3.215560246622884,
+            "angularVelocity": -0.3103218064280789,
+            "velocityX": -1.3402261052147335,
+            "velocityY": -0.8795734560860885,
+            "timestamp": 2.0453753237283987
+        },
+        {
+            "x": 7.278385319183743,
+            "y": 5.534503531789237,
+            "heading": -3.2361878862694873,
+            "angularVelocity": -0.2064528809850976,
+            "velocityX": -2.1685162486986918,
+            "velocityY": -0.7950425693946834,
+            "timestamp": 2.1452898395645446
+        },
+        {
+            "x": 6.979307104157413,
+            "y": 5.4664019474569185,
+            "heading": -3.246410388494101,
+            "angularVelocity": -0.10231248321693356,
+            "velocityX": -2.9933409827737334,
+            "velocityY": -0.6815985021035508,
+            "timestamp": 2.2452043554006904
+        },
+        {
+            "x": 6.603136214940544,
+            "y": 5.429560638127869,
+            "heading": -3.2464103824694632,
+            "angularVelocity": 6.029792436770947e-8,
+            "velocityX": -3.7649273087983395,
+            "velocityY": -0.368728297592585,
+            "timestamp": 2.3451188712368363
+        },
+        {
+            "x": 6.225751326953569,
+            "y": 5.450595811881877,
+            "heading": -3.2464103781263827,
+            "angularVelocity": 4.346796393735324e-8,
+            "velocityX": -3.777077683145315,
+            "velocityY": 0.2105317088110093,
+            "timestamp": 2.445033387072982
+        },
+        {
+            "x": 5.848366461127548,
+            "y": 5.47163138321431,
+            "heading": -3.2464103737833025,
+            "angularVelocity": 4.346795775537738e-8,
+            "velocityX": -3.777077461346165,
+            "velocityY": 0.21053568799683506,
+            "timestamp": 2.544947902909128
+        },
+        {
+            "x": 5.470981595301597,
+            "y": 5.492666954547974,
+            "heading": -3.246410369440223,
+            "angularVelocity": 4.346795748687552e-8,
+            "velocityX": -3.777077461345479,
+            "velocityY": 0.21053568800914468,
+            "timestamp": 2.644862418745274
+        },
+        {
+            "x": 5.09359672947572,
+            "y": 5.513702525882575,
+            "heading": -3.246410365097125,
+            "angularVelocity": 4.346813479097949e-8,
+            "velocityX": -3.777077461344731,
+            "velocityY": 0.21053568801853617,
+            "timestamp": 2.7447769345814197
+        },
+        {
+            "x": 4.761358057173187,
+            "y": 5.532221526743186,
+            "heading": -3.20512020544725,
+            "angularVelocity": 0.4132548639637959,
+            "velocityX": -3.3252292674608595,
+            "velocityY": 0.18534845218066273,
+            "timestamp": 2.8446914504175655
+        },
+        {
+            "x": 4.5121790336148475,
+            "y": 5.546110766147867,
+            "heading": -3.174152719009984,
+            "angularVelocity": 0.3099398138309664,
+            "velocityX": -2.4939221440754293,
+            "velocityY": 0.13901122663156756,
+            "timestamp": 2.9446059662537114
+        },
+        {
+            "x": 4.346059678819821,
+            "y": 5.555370255329891,
+            "heading": -3.153507786492084,
+            "angularVelocity": 0.20662595765119826,
+            "velocityX": -1.6626148203275395,
+            "velocityY": 0.09267411351128947,
+            "timestamp": 3.0445204820898573
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0.10331310947276948,
+            "velocityX": -0.8313074243990123,
+            "velocityY": 0.04633705754727876,
+            "timestamp": 3.144434997926003
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0,
+            "velocityX": 1.3822008000428574e-34,
+            "velocityY": 0,
+            "timestamp": 3.244349513762149
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S1.1.traj
new file mode 100644
index 00000000..6ed764e0
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S1.1.traj	
@@ -0,0 +1,364 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.352643709641578,
+            "y": 6.724306544750454,
+            "heading": -2.7643107246230096,
+            "angularVelocity": -0.16582604780361404,
+            "velocityX": 0.6100109212961704,
+            "velocityY": -0.37883661491424325,
+            "timestamp": 0.08629961825889286
+        },
+        {
+            "x": 2.457948690141224,
+            "y": 6.658909154192408,
+            "heading": -2.791757157392081,
+            "angularVelocity": -0.31803654897277683,
+            "velocityX": 1.2202253338341789,
+            "velocityY": -0.7577946678969769,
+            "timestamp": 0.17259923651778572
+        },
+        {
+            "x": 2.6159410271495345,
+            "y": 6.560792064019538,
+            "heading": -2.830360502782766,
+            "angularVelocity": -0.44731768423791834,
+            "velocityX": 1.8307420148076843,
+            "velocityY": -1.136935390358925,
+            "timestamp": 0.25889885477667857
+        },
+        {
+            "x": 2.826660648973102,
+            "y": 6.4299308538855104,
+            "heading": -2.87607731089876,
+            "angularVelocity": -0.5297451951435223,
+            "velocityX": 2.4417213665020787,
+            "velocityY": -1.516359084482017,
+            "timestamp": 0.34519847303557144
+        },
+        {
+            "x": 3.0901174136120235,
+            "y": 6.266320048368765,
+            "heading": -2.9160689628763303,
+            "angularVelocity": -0.46340473786276914,
+            "velocityX": 3.052814948135086,
+            "velocityY": -1.8958462252537605,
+            "timestamp": 0.4314980912944643
+        },
+        {
+            "x": 3.3674552739873977,
+            "y": 6.094087733168023,
+            "heading": -2.916068973852836,
+            "angularVelocity": -1.2719066550138966e-7,
+            "velocityX": 3.213662655416737,
+            "velocityY": -1.995748285747182,
+            "timestamp": 0.5177977095533571
+        },
+        {
+            "x": 3.6447931343598987,
+            "y": 5.921855417962633,
+            "heading": -2.9160689848293226,
+            "angularVelocity": -1.2719044142347595e-7,
+            "velocityX": 3.213662655383448,
+            "velocityY": -1.9957482858010414,
+            "timestamp": 0.60409732781225
+        },
+        {
+            "x": 3.9221309947324,
+            "y": 5.749623102757243,
+            "heading": -2.9160689958058095,
+            "angularVelocity": -1.2719044074317096e-7,
+            "velocityX": 3.2136626553834478,
+            "velocityY": -1.9957482858010414,
+            "timestamp": 0.6903969460711429
+        },
+        {
+            "x": 4.199468855104901,
+            "y": 5.577390787551853,
+            "heading": -2.916069006782296,
+            "angularVelocity": -1.2719044064772273e-7,
+            "velocityX": 3.213662655383448,
+            "velocityY": -1.995748285801041,
+            "timestamp": 0.7766965643300358
+        },
+        {
+            "x": 4.476806715477402,
+            "y": 5.405158472346463,
+            "heading": -2.9160690177587827,
+            "angularVelocity": -1.271904417367868e-7,
+            "velocityX": 3.213662655383448,
+            "velocityY": -1.9957482858010416,
+            "timestamp": 0.8629961825889286
+        },
+        {
+            "x": 4.754144575849903,
+            "y": 5.2329261571410735,
+            "heading": -2.9160690287352695,
+            "angularVelocity": -1.2719044128845816e-7,
+            "velocityX": 3.2136626553834478,
+            "velocityY": -1.9957482858010416,
+            "timestamp": 0.9492958008478215
+        },
+        {
+            "x": 5.031482436222404,
+            "y": 5.0606938419356835,
+            "heading": -2.9160690397117563,
+            "angularVelocity": -1.271904407298397e-7,
+            "velocityX": 3.2136626553834486,
+            "velocityY": -1.995748285801041,
+            "timestamp": 1.0355954191067143
+        },
+        {
+            "x": 5.308820296594906,
+            "y": 4.8884615267302935,
+            "heading": -2.9160690506882427,
+            "angularVelocity": -1.2719044158933254e-7,
+            "velocityX": 3.2136626553834478,
+            "velocityY": -1.995748285801042,
+            "timestamp": 1.1218950373656071
+        },
+        {
+            "x": 5.586158156967406,
+            "y": 4.716229211524904,
+            "heading": -2.9160690616647296,
+            "angularVelocity": -1.271904410743158e-7,
+            "velocityX": 3.2136626553834478,
+            "velocityY": -1.995748285801042,
+            "timestamp": 1.2081946556245
+        },
+        {
+            "x": 5.863496017339908,
+            "y": 4.543996896319514,
+            "heading": -2.9160690726412164,
+            "angularVelocity": -1.2719044055646472e-7,
+            "velocityX": 3.213662655383448,
+            "velocityY": -1.9957482858010414,
+            "timestamp": 1.2944942738833929
+        },
+        {
+            "x": 6.140833877716964,
+            "y": 4.371764581121458,
+            "heading": -2.916069083617703,
+            "angularVelocity": -1.2719044131717392e-7,
+            "velocityX": 3.213662655436225,
+            "velocityY": -1.9957482857160567,
+            "timestamp": 1.3807938921422858
+        },
+        {
+            "x": 6.418172664892274,
+            "y": 4.19953375831344,
+            "heading": -2.9160690945941994,
+            "angularVelocity": -1.2719055821939482e-7,
+            "velocityX": 3.21367339474496,
+            "velocityY": -1.9957309925918116,
+            "timestamp": 1.4670935104011786
+        },
+        {
+            "x": 6.7207652632777295,
+            "y": 4.0769859125864,
+            "heading": -2.916069118255731,
+            "angularVelocity": -2.741788669167458e-7,
+            "velocityX": 3.5063028607839835,
+            "velocityY": -1.4200276687058784,
+            "timestamp": 1.5533931286600715
+        },
+        {
+            "x": 7.011920641126383,
+            "y": 4.012803875379112,
+            "heading": -2.9821933696709393,
+            "angularVelocity": -0.7662171948025339,
+            "velocityX": 3.3737736472290445,
+            "velocityY": -0.7437117162469018,
+            "timestamp": 1.6396927469189644
+        },
+        {
+            "x": 7.255025953055537,
+            "y": 3.987925238494502,
+            "heading": -3.0423989255025554,
+            "angularVelocity": -0.6976340920894686,
+            "velocityX": 2.8169917414808885,
+            "velocityY": -0.28828211973553797,
+            "timestamp": 1.7259923651778573
+        },
+        {
+            "x": 7.447850515704627,
+            "y": 3.9993347769870677,
+            "heading": -3.0905963113163932,
+            "angularVelocity": -0.5584889804459283,
+            "velocityX": 2.234361710265075,
+            "velocityY": 0.13220844684348906,
+            "timestamp": 1.8122919834367501
+        },
+        {
+            "x": 7.5896354595479805,
+            "y": 4.045892337030357,
+            "heading": -3.124573318920613,
+            "angularVelocity": -0.3937098250312271,
+            "velocityX": 1.6429382505152235,
+            "velocityY": 0.539487439032544,
+            "timestamp": 1.898591601695643
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.21566709836621822,
+            "velocityX": 1.047102435377472,
+            "velocityY": 0.9398380271749195,
+            "timestamp": 1.9848912199545359
+        },
+        {
+            "x": 7.718344137255122,
+            "y": 4.246200601691832,
+            "heading": -3.145373267573727,
+            "angularVelocity": -0.024710376357260012,
+            "velocityX": 0.43305082947346774,
+            "velocityY": 1.34622717133274,
+            "timestamp": 2.073435415339313
+        },
+        {
+            "x": 7.702075707426083,
+            "y": 4.401035710087067,
+            "heading": -3.1311779977271272,
+            "angularVelocity": 0.16031846906115368,
+            "velocityX": -0.183732313080062,
+            "velocityY": 1.7486759885425491,
+            "timestamp": 2.16197961072409
+        },
+        {
+            "x": 7.630801467153219,
+            "y": 4.590926342055076,
+            "heading": -3.10146947436901,
+            "angularVelocity": 0.33552197554576096,
+            "velocityX": -0.804956665562769,
+            "velocityY": 2.1445858889079887,
+            "timestamp": 2.250523806108867
+        },
+        {
+            "x": 7.503771169060967,
+            "y": 4.814724539953898,
+            "heading": -3.057954484569731,
+            "angularVelocity": 0.49144937855435067,
+            "velocityX": -1.4346541581807906,
+            "velocityY": 2.527530990879987,
+            "timestamp": 2.339068001493644
+        },
+        {
+            "x": 7.31900648308238,
+            "y": 5.069082680437073,
+            "heading": -3.0055052448244375,
+            "angularVelocity": 0.5923509667139643,
+            "velocityX": -2.086694505218799,
+            "velocityY": 2.872668720699063,
+            "timestamp": 2.427612196878421
+        },
+        {
+            "x": 7.075378945667608,
+            "y": 5.298956768781039,
+            "heading": -3.0055052272028773,
+            "angularVelocity": 1.990142925919268e-7,
+            "velocityX": -2.75147948837462,
+            "velocityY": 2.5961508526209696,
+            "timestamp": 2.516156392263198
+        },
+        {
+            "x": 6.801072699384408,
+            "y": 5.491187253021094,
+            "heading": -3.005505216579771,
+            "angularVelocity": 1.1997518482429124e-7,
+            "velocityX": -3.097958540262374,
+            "velocityY": 2.1710117010503502,
+            "timestamp": 2.604700587647975
+        },
+        {
+            "x": 6.5267661173867575,
+            "y": 5.683417258207369,
+            "heading": -3.005505205956667,
+            "angularVelocity": 1.199751543060781e-7,
+            "velocityX": -3.0979623317525853,
+            "velocityY": 2.1710062907151193,
+            "timestamp": 2.693244783032752
+        },
+        {
+            "x": 6.252459535387558,
+            "y": 5.875647263391433,
+            "heading": -3.005505195333564,
+            "angularVelocity": 1.199751539565171e-7,
+            "velocityX": -3.0979623317700846,
+            "velocityY": 2.1710062906901486,
+            "timestamp": 2.781788978417529
+        },
+        {
+            "x": 5.978152953372341,
+            "y": 6.067877268552632,
+            "heading": -3.0055051847104526,
+            "angularVelocity": 1.1997524424086504e-7,
+            "velocityX": -3.0979623319509813,
+            "velocityY": 2.171006290431916,
+            "timestamp": 2.870333173802306
+        },
+        {
+            "x": 5.71076650313837,
+            "y": 6.255250599668089,
+            "heading": -2.984811425443856,
+            "angularVelocity": 0.23371107700143318,
+            "velocityX": -3.0198077815501345,
+            "velocityY": 2.1161560088926668,
+            "timestamp": 2.958877369187083
+        },
+        {
+            "x": 5.496808427326641,
+            "y": 6.405185542172583,
+            "heading": -2.9493217674847187,
+            "angularVelocity": 0.40081292520855316,
+            "velocityX": -2.4163986682814493,
+            "velocityY": 1.6933345190325848,
+            "timestamp": 3.04742156457186
+        },
+        {
+            "x": 5.33638152944714,
+            "y": 6.517608311818503,
+            "heading": -2.9168038375204985,
+            "angularVelocity": 0.3672508381444,
+            "velocityX": -1.811828513234718,
+            "velocityY": 1.269679724997829,
+            "timestamp": 3.135965759956637
+        },
+        {
+            "x": 5.229454180923455,
+            "y": 6.592540440066129,
+            "heading": -2.8927984833645977,
+            "angularVelocity": 0.27111155118061186,
+            "velocityX": -1.2076155648493634,
+            "velocityY": 0.8462681028637682,
+            "timestamp": 3.224509955341414
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0.144543448733038,
+            "velocityX": -0.6037005666060847,
+            "velocityY": 0.4230605944433498,
+            "timestamp": 3.313054150726191
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0,
+            "velocityX": 1.048960687291352e-31,
+            "velocityY": 0,
+            "timestamp": 3.401598346110968
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S1.traj
new file mode 100644
index 00000000..6ed764e0
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S1.traj	
@@ -0,0 +1,364 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.352643709641578,
+            "y": 6.724306544750454,
+            "heading": -2.7643107246230096,
+            "angularVelocity": -0.16582604780361404,
+            "velocityX": 0.6100109212961704,
+            "velocityY": -0.37883661491424325,
+            "timestamp": 0.08629961825889286
+        },
+        {
+            "x": 2.457948690141224,
+            "y": 6.658909154192408,
+            "heading": -2.791757157392081,
+            "angularVelocity": -0.31803654897277683,
+            "velocityX": 1.2202253338341789,
+            "velocityY": -0.7577946678969769,
+            "timestamp": 0.17259923651778572
+        },
+        {
+            "x": 2.6159410271495345,
+            "y": 6.560792064019538,
+            "heading": -2.830360502782766,
+            "angularVelocity": -0.44731768423791834,
+            "velocityX": 1.8307420148076843,
+            "velocityY": -1.136935390358925,
+            "timestamp": 0.25889885477667857
+        },
+        {
+            "x": 2.826660648973102,
+            "y": 6.4299308538855104,
+            "heading": -2.87607731089876,
+            "angularVelocity": -0.5297451951435223,
+            "velocityX": 2.4417213665020787,
+            "velocityY": -1.516359084482017,
+            "timestamp": 0.34519847303557144
+        },
+        {
+            "x": 3.0901174136120235,
+            "y": 6.266320048368765,
+            "heading": -2.9160689628763303,
+            "angularVelocity": -0.46340473786276914,
+            "velocityX": 3.052814948135086,
+            "velocityY": -1.8958462252537605,
+            "timestamp": 0.4314980912944643
+        },
+        {
+            "x": 3.3674552739873977,
+            "y": 6.094087733168023,
+            "heading": -2.916068973852836,
+            "angularVelocity": -1.2719066550138966e-7,
+            "velocityX": 3.213662655416737,
+            "velocityY": -1.995748285747182,
+            "timestamp": 0.5177977095533571
+        },
+        {
+            "x": 3.6447931343598987,
+            "y": 5.921855417962633,
+            "heading": -2.9160689848293226,
+            "angularVelocity": -1.2719044142347595e-7,
+            "velocityX": 3.213662655383448,
+            "velocityY": -1.9957482858010414,
+            "timestamp": 0.60409732781225
+        },
+        {
+            "x": 3.9221309947324,
+            "y": 5.749623102757243,
+            "heading": -2.9160689958058095,
+            "angularVelocity": -1.2719044074317096e-7,
+            "velocityX": 3.2136626553834478,
+            "velocityY": -1.9957482858010414,
+            "timestamp": 0.6903969460711429
+        },
+        {
+            "x": 4.199468855104901,
+            "y": 5.577390787551853,
+            "heading": -2.916069006782296,
+            "angularVelocity": -1.2719044064772273e-7,
+            "velocityX": 3.213662655383448,
+            "velocityY": -1.995748285801041,
+            "timestamp": 0.7766965643300358
+        },
+        {
+            "x": 4.476806715477402,
+            "y": 5.405158472346463,
+            "heading": -2.9160690177587827,
+            "angularVelocity": -1.271904417367868e-7,
+            "velocityX": 3.213662655383448,
+            "velocityY": -1.9957482858010416,
+            "timestamp": 0.8629961825889286
+        },
+        {
+            "x": 4.754144575849903,
+            "y": 5.2329261571410735,
+            "heading": -2.9160690287352695,
+            "angularVelocity": -1.2719044128845816e-7,
+            "velocityX": 3.2136626553834478,
+            "velocityY": -1.9957482858010416,
+            "timestamp": 0.9492958008478215
+        },
+        {
+            "x": 5.031482436222404,
+            "y": 5.0606938419356835,
+            "heading": -2.9160690397117563,
+            "angularVelocity": -1.271904407298397e-7,
+            "velocityX": 3.2136626553834486,
+            "velocityY": -1.995748285801041,
+            "timestamp": 1.0355954191067143
+        },
+        {
+            "x": 5.308820296594906,
+            "y": 4.8884615267302935,
+            "heading": -2.9160690506882427,
+            "angularVelocity": -1.2719044158933254e-7,
+            "velocityX": 3.2136626553834478,
+            "velocityY": -1.995748285801042,
+            "timestamp": 1.1218950373656071
+        },
+        {
+            "x": 5.586158156967406,
+            "y": 4.716229211524904,
+            "heading": -2.9160690616647296,
+            "angularVelocity": -1.271904410743158e-7,
+            "velocityX": 3.2136626553834478,
+            "velocityY": -1.995748285801042,
+            "timestamp": 1.2081946556245
+        },
+        {
+            "x": 5.863496017339908,
+            "y": 4.543996896319514,
+            "heading": -2.9160690726412164,
+            "angularVelocity": -1.2719044055646472e-7,
+            "velocityX": 3.213662655383448,
+            "velocityY": -1.9957482858010414,
+            "timestamp": 1.2944942738833929
+        },
+        {
+            "x": 6.140833877716964,
+            "y": 4.371764581121458,
+            "heading": -2.916069083617703,
+            "angularVelocity": -1.2719044131717392e-7,
+            "velocityX": 3.213662655436225,
+            "velocityY": -1.9957482857160567,
+            "timestamp": 1.3807938921422858
+        },
+        {
+            "x": 6.418172664892274,
+            "y": 4.19953375831344,
+            "heading": -2.9160690945941994,
+            "angularVelocity": -1.2719055821939482e-7,
+            "velocityX": 3.21367339474496,
+            "velocityY": -1.9957309925918116,
+            "timestamp": 1.4670935104011786
+        },
+        {
+            "x": 6.7207652632777295,
+            "y": 4.0769859125864,
+            "heading": -2.916069118255731,
+            "angularVelocity": -2.741788669167458e-7,
+            "velocityX": 3.5063028607839835,
+            "velocityY": -1.4200276687058784,
+            "timestamp": 1.5533931286600715
+        },
+        {
+            "x": 7.011920641126383,
+            "y": 4.012803875379112,
+            "heading": -2.9821933696709393,
+            "angularVelocity": -0.7662171948025339,
+            "velocityX": 3.3737736472290445,
+            "velocityY": -0.7437117162469018,
+            "timestamp": 1.6396927469189644
+        },
+        {
+            "x": 7.255025953055537,
+            "y": 3.987925238494502,
+            "heading": -3.0423989255025554,
+            "angularVelocity": -0.6976340920894686,
+            "velocityX": 2.8169917414808885,
+            "velocityY": -0.28828211973553797,
+            "timestamp": 1.7259923651778573
+        },
+        {
+            "x": 7.447850515704627,
+            "y": 3.9993347769870677,
+            "heading": -3.0905963113163932,
+            "angularVelocity": -0.5584889804459283,
+            "velocityX": 2.234361710265075,
+            "velocityY": 0.13220844684348906,
+            "timestamp": 1.8122919834367501
+        },
+        {
+            "x": 7.5896354595479805,
+            "y": 4.045892337030357,
+            "heading": -3.124573318920613,
+            "angularVelocity": -0.3937098250312271,
+            "velocityX": 1.6429382505152235,
+            "velocityY": 0.539487439032544,
+            "timestamp": 1.898591601695643
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.21566709836621822,
+            "velocityX": 1.047102435377472,
+            "velocityY": 0.9398380271749195,
+            "timestamp": 1.9848912199545359
+        },
+        {
+            "x": 7.718344137255122,
+            "y": 4.246200601691832,
+            "heading": -3.145373267573727,
+            "angularVelocity": -0.024710376357260012,
+            "velocityX": 0.43305082947346774,
+            "velocityY": 1.34622717133274,
+            "timestamp": 2.073435415339313
+        },
+        {
+            "x": 7.702075707426083,
+            "y": 4.401035710087067,
+            "heading": -3.1311779977271272,
+            "angularVelocity": 0.16031846906115368,
+            "velocityX": -0.183732313080062,
+            "velocityY": 1.7486759885425491,
+            "timestamp": 2.16197961072409
+        },
+        {
+            "x": 7.630801467153219,
+            "y": 4.590926342055076,
+            "heading": -3.10146947436901,
+            "angularVelocity": 0.33552197554576096,
+            "velocityX": -0.804956665562769,
+            "velocityY": 2.1445858889079887,
+            "timestamp": 2.250523806108867
+        },
+        {
+            "x": 7.503771169060967,
+            "y": 4.814724539953898,
+            "heading": -3.057954484569731,
+            "angularVelocity": 0.49144937855435067,
+            "velocityX": -1.4346541581807906,
+            "velocityY": 2.527530990879987,
+            "timestamp": 2.339068001493644
+        },
+        {
+            "x": 7.31900648308238,
+            "y": 5.069082680437073,
+            "heading": -3.0055052448244375,
+            "angularVelocity": 0.5923509667139643,
+            "velocityX": -2.086694505218799,
+            "velocityY": 2.872668720699063,
+            "timestamp": 2.427612196878421
+        },
+        {
+            "x": 7.075378945667608,
+            "y": 5.298956768781039,
+            "heading": -3.0055052272028773,
+            "angularVelocity": 1.990142925919268e-7,
+            "velocityX": -2.75147948837462,
+            "velocityY": 2.5961508526209696,
+            "timestamp": 2.516156392263198
+        },
+        {
+            "x": 6.801072699384408,
+            "y": 5.491187253021094,
+            "heading": -3.005505216579771,
+            "angularVelocity": 1.1997518482429124e-7,
+            "velocityX": -3.097958540262374,
+            "velocityY": 2.1710117010503502,
+            "timestamp": 2.604700587647975
+        },
+        {
+            "x": 6.5267661173867575,
+            "y": 5.683417258207369,
+            "heading": -3.005505205956667,
+            "angularVelocity": 1.199751543060781e-7,
+            "velocityX": -3.0979623317525853,
+            "velocityY": 2.1710062907151193,
+            "timestamp": 2.693244783032752
+        },
+        {
+            "x": 6.252459535387558,
+            "y": 5.875647263391433,
+            "heading": -3.005505195333564,
+            "angularVelocity": 1.199751539565171e-7,
+            "velocityX": -3.0979623317700846,
+            "velocityY": 2.1710062906901486,
+            "timestamp": 2.781788978417529
+        },
+        {
+            "x": 5.978152953372341,
+            "y": 6.067877268552632,
+            "heading": -3.0055051847104526,
+            "angularVelocity": 1.1997524424086504e-7,
+            "velocityX": -3.0979623319509813,
+            "velocityY": 2.171006290431916,
+            "timestamp": 2.870333173802306
+        },
+        {
+            "x": 5.71076650313837,
+            "y": 6.255250599668089,
+            "heading": -2.984811425443856,
+            "angularVelocity": 0.23371107700143318,
+            "velocityX": -3.0198077815501345,
+            "velocityY": 2.1161560088926668,
+            "timestamp": 2.958877369187083
+        },
+        {
+            "x": 5.496808427326641,
+            "y": 6.405185542172583,
+            "heading": -2.9493217674847187,
+            "angularVelocity": 0.40081292520855316,
+            "velocityX": -2.4163986682814493,
+            "velocityY": 1.6933345190325848,
+            "timestamp": 3.04742156457186
+        },
+        {
+            "x": 5.33638152944714,
+            "y": 6.517608311818503,
+            "heading": -2.9168038375204985,
+            "angularVelocity": 0.3672508381444,
+            "velocityX": -1.811828513234718,
+            "velocityY": 1.269679724997829,
+            "timestamp": 3.135965759956637
+        },
+        {
+            "x": 5.229454180923455,
+            "y": 6.592540440066129,
+            "heading": -2.8927984833645977,
+            "angularVelocity": 0.27111155118061186,
+            "velocityX": -1.2076155648493634,
+            "velocityY": 0.8462681028637682,
+            "timestamp": 3.224509955341414
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0.144543448733038,
+            "velocityX": -0.6037005666060847,
+            "velocityY": 0.4230605944433498,
+            "timestamp": 3.313054150726191
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0,
+            "velocityX": 1.048960687291352e-31,
+            "velocityY": 0,
+            "timestamp": 3.401598346110968
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S2.1.traj
new file mode 100644
index 00000000..9420bca5
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S2.1.traj	
@@ -0,0 +1,355 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.357227124880225,
+            "y": 6.730410634555629,
+            "heading": -2.7571910501011128,
+            "angularVelocity": -0.08264039553460875,
+            "velocityX": 0.6576608657862654,
+            "velocityY": -0.30556812237992936,
+            "timestamp": 0.08701616267193764
+        },
+        {
+            "x": 2.4716821810223912,
+            "y": 6.677231589623697,
+            "heading": -2.771475854118176,
+            "angularVelocity": -0.1641626518388192,
+            "velocityX": 1.3153309986062809,
+            "velocityY": -0.6111398537812316,
+            "timestamp": 0.17403232534387528
+        },
+        {
+            "x": 2.6433664887429877,
+            "y": 6.597462350883563,
+            "heading": -2.792692156200411,
+            "angularVelocity": -0.2438202447770986,
+            "velocityX": 1.973016304659045,
+            "velocityY": -0.916717495815976,
+            "timestamp": 0.26104848801581293
+        },
+        {
+            "x": 2.8722826002398385,
+            "y": 6.491101923690649,
+            "heading": -2.820514979829771,
+            "angularVelocity": -0.319743169257603,
+            "velocityX": 2.630730940870083,
+            "velocityY": -1.222306568423469,
+            "timestamp": 0.34806465068775055
+        },
+        {
+            "x": 3.1584374767810863,
+            "y": 6.358147629132494,
+            "heading": -2.8539648898748324,
+            "angularVelocity": -0.38441030973950396,
+            "velocityX": 3.288525576795303,
+            "velocityY": -1.5279264274087752,
+            "timestamp": 0.4350808133596882
+        },
+        {
+            "x": 3.456964402226087,
+            "y": 6.219442363897474,
+            "heading": -2.853964894160219,
+            "angularVelocity": -4.924816792193532e-8,
+            "velocityX": 3.4307066213720128,
+            "velocityY": -1.5940172604250262,
+            "timestamp": 0.5220969760316259
+        },
+        {
+            "x": 3.755491327665543,
+            "y": 6.0807370986505,
+            "heading": -2.853964898445594,
+            "angularVelocity": -4.924803343036069e-8,
+            "velocityX": 3.4307066213082953,
+            "velocityY": -1.5940172605623917,
+            "timestamp": 0.6091131387035635
+        },
+        {
+            "x": 4.054018253104999,
+            "y": 5.942031833403527,
+            "heading": -2.853964902730969,
+            "angularVelocity": -4.924803431145499e-8,
+            "velocityX": 3.4307066213082953,
+            "velocityY": -1.594017260562392,
+            "timestamp": 0.6961293013755012
+        },
+        {
+            "x": 4.352545178544456,
+            "y": 5.803326568156554,
+            "heading": -2.8539649070163438,
+            "angularVelocity": -4.9248033877501264e-8,
+            "velocityX": 3.4307066213082953,
+            "velocityY": -1.5940172605623917,
+            "timestamp": 0.7831454640474389
+        },
+        {
+            "x": 4.651072103983911,
+            "y": 5.66462130290958,
+            "heading": -2.8539649113017185,
+            "angularVelocity": -4.924803325887112e-8,
+            "velocityX": 3.430706621308295,
+            "velocityY": -1.5940172605623921,
+            "timestamp": 0.8701616267193766
+        },
+        {
+            "x": 4.949599029423368,
+            "y": 5.525916037662607,
+            "heading": -2.853964915587093,
+            "angularVelocity": -4.924803367441165e-8,
+            "velocityX": 3.430706621308295,
+            "velocityY": -1.5940172605623926,
+            "timestamp": 0.9571777893913143
+        },
+        {
+            "x": 5.248125954862823,
+            "y": 5.387210772415633,
+            "heading": -2.853964919872468,
+            "angularVelocity": -4.924803391655779e-8,
+            "velocityX": 3.4307066213082953,
+            "velocityY": -1.594017260562392,
+            "timestamp": 1.044193952063252
+        },
+        {
+            "x": 5.54665288030228,
+            "y": 5.24850550716866,
+            "heading": -2.8539649241578426,
+            "angularVelocity": -4.9248034279126105e-8,
+            "velocityX": 3.4307066213082953,
+            "velocityY": -1.5940172605623923,
+            "timestamp": 1.1312101147351896
+        },
+        {
+            "x": 5.8451798057417355,
+            "y": 5.109800241921686,
+            "heading": -2.8539649284432174,
+            "angularVelocity": -4.9248034369336404e-8,
+            "velocityX": 3.4307066213082953,
+            "velocityY": -1.594017260562393,
+            "timestamp": 1.2182262774071273
+        },
+        {
+            "x": 6.143706731181192,
+            "y": 4.9710949766747135,
+            "heading": -2.853964932728592,
+            "angularVelocity": -4.9248033497376594e-8,
+            "velocityX": 3.4307066213082953,
+            "velocityY": -1.594017260562392,
+            "timestamp": 1.305242440079065
+        },
+        {
+            "x": 6.4422336566206475,
+            "y": 4.83238971142774,
+            "heading": -2.8539649370139673,
+            "angularVelocity": -4.924803317735332e-8,
+            "velocityX": 3.4307066213082953,
+            "velocityY": -1.5940172605623926,
+            "timestamp": 1.3922586027510027
+        },
+        {
+            "x": 6.740760582057679,
+            "y": 4.693684446175547,
+            "heading": -2.853964941299342,
+            "angularVelocity": -4.9248033793238546e-8,
+            "velocityX": 3.4307066212804305,
+            "velocityY": -1.5940172606223628,
+            "timestamp": 1.4792747654229403
+        },
+        {
+            "x": 7.039286998892109,
+            "y": 4.554978086294829,
+            "heading": -2.8539649455851244,
+            "angularVelocity": -4.925271638681629e-8,
+            "velocityX": 3.4307007763593735,
+            "velocityY": -1.59402984022244,
+            "timestamp": 1.566290928094878
+        },
+        {
+            "x": 7.281877390378217,
+            "y": 4.416206845653086,
+            "heading": -2.926969050497618,
+            "angularVelocity": -0.8389717803085356,
+            "velocityX": 2.7878773786049877,
+            "velocityY": -1.594775457577107,
+            "timestamp": 1.6533070907668157
+        },
+        {
+            "x": 7.4672438532140255,
+            "y": 4.304023352017119,
+            "heading": -2.9923953501231186,
+            "angularVelocity": -0.7518867485821702,
+            "velocityX": 2.130253244269864,
+            "velocityY": -1.2892259344843033,
+            "timestamp": 1.7403232534387534
+        },
+        {
+            "x": 7.595386429544574,
+            "y": 4.218427699418672,
+            "heading": -3.050241491364391,
+            "angularVelocity": -0.6647746747849547,
+            "velocityX": 1.4726295942704664,
+            "velocityY": -0.9836753307676307,
+            "timestamp": 1.827339416110691
+        },
+        {
+            "x": 7.666305140070344,
+            "y": 4.159419912093254,
+            "heading": -3.100505408798514,
+            "angularVelocity": -0.5776388649040337,
+            "velocityX": 0.8150061821634523,
+            "velocityY": -0.6781244485335891,
+            "timestamp": 1.9143555787826287
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.4904824238455614,
+            "velocityX": 0.15738294483620113,
+            "velocityY": -0.37257345184803875,
+            "timestamp": 2.0013717414545664
+        },
+        {
+            "x": 7.62294760973909,
+            "y": 4.124186267487049,
+            "heading": -3.1816074637236196,
+            "angularVelocity": -0.39238397540812464,
+            "velocityX": -0.5826441228371791,
+            "velocityY": -0.02873507497949264,
+            "timestamp": 2.099291531639857
+        },
+        {
+            "x": 7.4934981410182715,
+            "y": 4.155183222691002,
+            "heading": -3.2104217744560906,
+            "angularVelocity": -0.29426442476997366,
+            "velocityX": -1.3219949560335675,
+            "velocityY": 0.31655455087575607,
+            "timestamp": 2.1972113218251477
+        },
+        {
+            "x": 7.291774718335501,
+            "y": 4.220253220991955,
+            "heading": -3.2296266134452343,
+            "angularVelocity": -0.19612826940094027,
+            "velocityX": -2.06008838765949,
+            "velocityY": 0.6645234653569294,
+            "timestamp": 2.2951311120104383
+        },
+        {
+            "x": 7.018085982963682,
+            "y": 4.320043342217767,
+            "heading": -3.2392208692548645,
+            "angularVelocity": -0.09798076355633002,
+            "velocityX": -2.795029838747879,
+            "velocityY": 1.0191006438737447,
+            "timestamp": 2.393050902195729
+        },
+        {
+            "x": 6.674609525490048,
+            "y": 4.458745905991077,
+            "heading": -3.2392208627403147,
+            "angularVelocity": 6.652945395302414e-8,
+            "velocityX": -3.5077327762210677,
+            "velocityY": 1.4164916357648132,
+            "timestamp": 2.4909706923810195
+        },
+        {
+            "x": 6.337654568768723,
+            "y": 4.612615494980773,
+            "heading": -3.239220858536144,
+            "angularVelocity": 4.2934841578218656e-8,
+            "velocityX": -3.4411323398846756,
+            "velocityY": 1.5713839735413375,
+            "timestamp": 2.58889048256631
+        },
+        {
+            "x": 6.000699635853928,
+            "y": 4.766485136103752,
+            "heading": -3.239220854331973,
+            "angularVelocity": 4.293484065869374e-8,
+            "velocityX": -3.441132096761908,
+            "velocityY": 1.5713845059493727,
+            "timestamp": 2.686810272751601
+        },
+        {
+            "x": 5.66374470293921,
+            "y": 4.920354777226902,
+            "heading": -3.239220850127803,
+            "angularVelocity": 4.293484113596361e-8,
+            "velocityX": -3.4411320967611103,
+            "velocityY": 1.5713845059511196,
+            "timestamp": 2.7847300629368914
+        },
+        {
+            "x": 5.326789770024494,
+            "y": 5.074224418350052,
+            "heading": -3.239220845923632,
+            "angularVelocity": 4.293484003264988e-8,
+            "velocityX": -3.441132096761111,
+            "velocityY": 1.571384505951117,
+            "timestamp": 2.882649853122182
+        },
+        {
+            "x": 4.989834837078673,
+            "y": 5.228094059405012,
+            "heading": -3.2392208417194372,
+            "angularVelocity": 4.293508551285566e-8,
+            "velocityX": -3.4411320970787416,
+            "velocityY": 1.571384505254732,
+            "timestamp": 2.9805696433074726
+        },
+        {
+            "x": 4.699100923806666,
+            "y": 5.360856424917345,
+            "heading": -3.200805721128661,
+            "angularVelocity": 0.39231212115635317,
+            "velocityX": -2.9691026984622892,
+            "velocityY": 1.3558277163493837,
+            "timestamp": 3.0784894334927633
+        },
+        {
+            "x": 4.481050467012654,
+            "y": 5.460428209814717,
+            "heading": -3.171995332755761,
+            "angularVelocity": 0.2942243679074833,
+            "velocityX": -2.2268272468864767,
+            "velocityY": 1.016870897179783,
+            "timestamp": 3.176409223678054
+        },
+        {
+            "x": 4.335683489671324,
+            "y": 5.5268094029690245,
+            "heading": -3.152788680819891,
+            "angularVelocity": 0.19614678401093785,
+            "velocityX": -1.4845515606830426,
+            "velocityY": 0.6779139643650731,
+            "timestamp": 3.2743290138633445
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0.09807387885669308,
+            "velocityX": -0.7422757905606995,
+            "velocityY": 0.3389569868171628,
+            "timestamp": 3.372248804048635
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0,
+            "velocityX": 2.220393944714812e-34,
+            "velocityY": 0,
+            "timestamp": 3.4701685942339258
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S2.traj
new file mode 100644
index 00000000..9420bca5
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S2.traj	
@@ -0,0 +1,355 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.357227124880225,
+            "y": 6.730410634555629,
+            "heading": -2.7571910501011128,
+            "angularVelocity": -0.08264039553460875,
+            "velocityX": 0.6576608657862654,
+            "velocityY": -0.30556812237992936,
+            "timestamp": 0.08701616267193764
+        },
+        {
+            "x": 2.4716821810223912,
+            "y": 6.677231589623697,
+            "heading": -2.771475854118176,
+            "angularVelocity": -0.1641626518388192,
+            "velocityX": 1.3153309986062809,
+            "velocityY": -0.6111398537812316,
+            "timestamp": 0.17403232534387528
+        },
+        {
+            "x": 2.6433664887429877,
+            "y": 6.597462350883563,
+            "heading": -2.792692156200411,
+            "angularVelocity": -0.2438202447770986,
+            "velocityX": 1.973016304659045,
+            "velocityY": -0.916717495815976,
+            "timestamp": 0.26104848801581293
+        },
+        {
+            "x": 2.8722826002398385,
+            "y": 6.491101923690649,
+            "heading": -2.820514979829771,
+            "angularVelocity": -0.319743169257603,
+            "velocityX": 2.630730940870083,
+            "velocityY": -1.222306568423469,
+            "timestamp": 0.34806465068775055
+        },
+        {
+            "x": 3.1584374767810863,
+            "y": 6.358147629132494,
+            "heading": -2.8539648898748324,
+            "angularVelocity": -0.38441030973950396,
+            "velocityX": 3.288525576795303,
+            "velocityY": -1.5279264274087752,
+            "timestamp": 0.4350808133596882
+        },
+        {
+            "x": 3.456964402226087,
+            "y": 6.219442363897474,
+            "heading": -2.853964894160219,
+            "angularVelocity": -4.924816792193532e-8,
+            "velocityX": 3.4307066213720128,
+            "velocityY": -1.5940172604250262,
+            "timestamp": 0.5220969760316259
+        },
+        {
+            "x": 3.755491327665543,
+            "y": 6.0807370986505,
+            "heading": -2.853964898445594,
+            "angularVelocity": -4.924803343036069e-8,
+            "velocityX": 3.4307066213082953,
+            "velocityY": -1.5940172605623917,
+            "timestamp": 0.6091131387035635
+        },
+        {
+            "x": 4.054018253104999,
+            "y": 5.942031833403527,
+            "heading": -2.853964902730969,
+            "angularVelocity": -4.924803431145499e-8,
+            "velocityX": 3.4307066213082953,
+            "velocityY": -1.594017260562392,
+            "timestamp": 0.6961293013755012
+        },
+        {
+            "x": 4.352545178544456,
+            "y": 5.803326568156554,
+            "heading": -2.8539649070163438,
+            "angularVelocity": -4.9248033877501264e-8,
+            "velocityX": 3.4307066213082953,
+            "velocityY": -1.5940172605623917,
+            "timestamp": 0.7831454640474389
+        },
+        {
+            "x": 4.651072103983911,
+            "y": 5.66462130290958,
+            "heading": -2.8539649113017185,
+            "angularVelocity": -4.924803325887112e-8,
+            "velocityX": 3.430706621308295,
+            "velocityY": -1.5940172605623921,
+            "timestamp": 0.8701616267193766
+        },
+        {
+            "x": 4.949599029423368,
+            "y": 5.525916037662607,
+            "heading": -2.853964915587093,
+            "angularVelocity": -4.924803367441165e-8,
+            "velocityX": 3.430706621308295,
+            "velocityY": -1.5940172605623926,
+            "timestamp": 0.9571777893913143
+        },
+        {
+            "x": 5.248125954862823,
+            "y": 5.387210772415633,
+            "heading": -2.853964919872468,
+            "angularVelocity": -4.924803391655779e-8,
+            "velocityX": 3.4307066213082953,
+            "velocityY": -1.594017260562392,
+            "timestamp": 1.044193952063252
+        },
+        {
+            "x": 5.54665288030228,
+            "y": 5.24850550716866,
+            "heading": -2.8539649241578426,
+            "angularVelocity": -4.9248034279126105e-8,
+            "velocityX": 3.4307066213082953,
+            "velocityY": -1.5940172605623923,
+            "timestamp": 1.1312101147351896
+        },
+        {
+            "x": 5.8451798057417355,
+            "y": 5.109800241921686,
+            "heading": -2.8539649284432174,
+            "angularVelocity": -4.9248034369336404e-8,
+            "velocityX": 3.4307066213082953,
+            "velocityY": -1.594017260562393,
+            "timestamp": 1.2182262774071273
+        },
+        {
+            "x": 6.143706731181192,
+            "y": 4.9710949766747135,
+            "heading": -2.853964932728592,
+            "angularVelocity": -4.9248033497376594e-8,
+            "velocityX": 3.4307066213082953,
+            "velocityY": -1.594017260562392,
+            "timestamp": 1.305242440079065
+        },
+        {
+            "x": 6.4422336566206475,
+            "y": 4.83238971142774,
+            "heading": -2.8539649370139673,
+            "angularVelocity": -4.924803317735332e-8,
+            "velocityX": 3.4307066213082953,
+            "velocityY": -1.5940172605623926,
+            "timestamp": 1.3922586027510027
+        },
+        {
+            "x": 6.740760582057679,
+            "y": 4.693684446175547,
+            "heading": -2.853964941299342,
+            "angularVelocity": -4.9248033793238546e-8,
+            "velocityX": 3.4307066212804305,
+            "velocityY": -1.5940172606223628,
+            "timestamp": 1.4792747654229403
+        },
+        {
+            "x": 7.039286998892109,
+            "y": 4.554978086294829,
+            "heading": -2.8539649455851244,
+            "angularVelocity": -4.925271638681629e-8,
+            "velocityX": 3.4307007763593735,
+            "velocityY": -1.59402984022244,
+            "timestamp": 1.566290928094878
+        },
+        {
+            "x": 7.281877390378217,
+            "y": 4.416206845653086,
+            "heading": -2.926969050497618,
+            "angularVelocity": -0.8389717803085356,
+            "velocityX": 2.7878773786049877,
+            "velocityY": -1.594775457577107,
+            "timestamp": 1.6533070907668157
+        },
+        {
+            "x": 7.4672438532140255,
+            "y": 4.304023352017119,
+            "heading": -2.9923953501231186,
+            "angularVelocity": -0.7518867485821702,
+            "velocityX": 2.130253244269864,
+            "velocityY": -1.2892259344843033,
+            "timestamp": 1.7403232534387534
+        },
+        {
+            "x": 7.595386429544574,
+            "y": 4.218427699418672,
+            "heading": -3.050241491364391,
+            "angularVelocity": -0.6647746747849547,
+            "velocityX": 1.4726295942704664,
+            "velocityY": -0.9836753307676307,
+            "timestamp": 1.827339416110691
+        },
+        {
+            "x": 7.666305140070344,
+            "y": 4.159419912093254,
+            "heading": -3.100505408798514,
+            "angularVelocity": -0.5776388649040337,
+            "velocityX": 0.8150061821634523,
+            "velocityY": -0.6781244485335891,
+            "timestamp": 1.9143555787826287
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.4904824238455614,
+            "velocityX": 0.15738294483620113,
+            "velocityY": -0.37257345184803875,
+            "timestamp": 2.0013717414545664
+        },
+        {
+            "x": 7.62294760973909,
+            "y": 4.124186267487049,
+            "heading": -3.1816074637236196,
+            "angularVelocity": -0.39238397540812464,
+            "velocityX": -0.5826441228371791,
+            "velocityY": -0.02873507497949264,
+            "timestamp": 2.099291531639857
+        },
+        {
+            "x": 7.4934981410182715,
+            "y": 4.155183222691002,
+            "heading": -3.2104217744560906,
+            "angularVelocity": -0.29426442476997366,
+            "velocityX": -1.3219949560335675,
+            "velocityY": 0.31655455087575607,
+            "timestamp": 2.1972113218251477
+        },
+        {
+            "x": 7.291774718335501,
+            "y": 4.220253220991955,
+            "heading": -3.2296266134452343,
+            "angularVelocity": -0.19612826940094027,
+            "velocityX": -2.06008838765949,
+            "velocityY": 0.6645234653569294,
+            "timestamp": 2.2951311120104383
+        },
+        {
+            "x": 7.018085982963682,
+            "y": 4.320043342217767,
+            "heading": -3.2392208692548645,
+            "angularVelocity": -0.09798076355633002,
+            "velocityX": -2.795029838747879,
+            "velocityY": 1.0191006438737447,
+            "timestamp": 2.393050902195729
+        },
+        {
+            "x": 6.674609525490048,
+            "y": 4.458745905991077,
+            "heading": -3.2392208627403147,
+            "angularVelocity": 6.652945395302414e-8,
+            "velocityX": -3.5077327762210677,
+            "velocityY": 1.4164916357648132,
+            "timestamp": 2.4909706923810195
+        },
+        {
+            "x": 6.337654568768723,
+            "y": 4.612615494980773,
+            "heading": -3.239220858536144,
+            "angularVelocity": 4.2934841578218656e-8,
+            "velocityX": -3.4411323398846756,
+            "velocityY": 1.5713839735413375,
+            "timestamp": 2.58889048256631
+        },
+        {
+            "x": 6.000699635853928,
+            "y": 4.766485136103752,
+            "heading": -3.239220854331973,
+            "angularVelocity": 4.293484065869374e-8,
+            "velocityX": -3.441132096761908,
+            "velocityY": 1.5713845059493727,
+            "timestamp": 2.686810272751601
+        },
+        {
+            "x": 5.66374470293921,
+            "y": 4.920354777226902,
+            "heading": -3.239220850127803,
+            "angularVelocity": 4.293484113596361e-8,
+            "velocityX": -3.4411320967611103,
+            "velocityY": 1.5713845059511196,
+            "timestamp": 2.7847300629368914
+        },
+        {
+            "x": 5.326789770024494,
+            "y": 5.074224418350052,
+            "heading": -3.239220845923632,
+            "angularVelocity": 4.293484003264988e-8,
+            "velocityX": -3.441132096761111,
+            "velocityY": 1.571384505951117,
+            "timestamp": 2.882649853122182
+        },
+        {
+            "x": 4.989834837078673,
+            "y": 5.228094059405012,
+            "heading": -3.2392208417194372,
+            "angularVelocity": 4.293508551285566e-8,
+            "velocityX": -3.4411320970787416,
+            "velocityY": 1.571384505254732,
+            "timestamp": 2.9805696433074726
+        },
+        {
+            "x": 4.699100923806666,
+            "y": 5.360856424917345,
+            "heading": -3.200805721128661,
+            "angularVelocity": 0.39231212115635317,
+            "velocityX": -2.9691026984622892,
+            "velocityY": 1.3558277163493837,
+            "timestamp": 3.0784894334927633
+        },
+        {
+            "x": 4.481050467012654,
+            "y": 5.460428209814717,
+            "heading": -3.171995332755761,
+            "angularVelocity": 0.2942243679074833,
+            "velocityX": -2.2268272468864767,
+            "velocityY": 1.016870897179783,
+            "timestamp": 3.176409223678054
+        },
+        {
+            "x": 4.335683489671324,
+            "y": 5.5268094029690245,
+            "heading": -3.152788680819891,
+            "angularVelocity": 0.19614678401093785,
+            "velocityX": -1.4845515606830426,
+            "velocityY": 0.6779139643650731,
+            "timestamp": 3.2743290138633445
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0.09807387885669308,
+            "velocityX": -0.7422757905606995,
+            "velocityY": 0.3389569868171628,
+            "timestamp": 3.372248804048635
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0,
+            "velocityX": 2.220393944714812e-34,
+            "velocityY": 0,
+            "timestamp": 3.4701685942339258
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S3.1.traj
new file mode 100644
index 00000000..c8a9e073
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S3.1.traj	
@@ -0,0 +1,373 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3574291714983375,
+            "y": 6.741815654710436,
+            "heading": -2.758007728773179,
+            "angularVelocity": -0.09483878465079744,
+            "velocityX": 0.6801570061474024,
+            "velocityY": -0.17983436924136462,
+            "timestamp": 0.08443516861442411
+        },
+        {
+            "x": 2.47228751228047,
+            "y": 6.711446972989247,
+            "heading": -2.7740235521881464,
+            "angularVelocity": -0.18968190243216043,
+            "velocityX": 1.3603139860670759,
+            "velocityY": -0.35966863357457735,
+            "timestamp": 0.16887033722884823
+        },
+        {
+            "x": 2.6445750170696067,
+            "y": 6.665893967918211,
+            "heading": -2.798048184372895,
+            "angularVelocity": -0.2845334779214143,
+            "velocityX": 2.0404709034915762,
+            "velocityY": -0.5395027429749605,
+            "timestamp": 0.2533055058432723
+        },
+        {
+            "x": 2.8742916749054777,
+            "y": 6.605156656477843,
+            "heading": -2.830082761948688,
+            "angularVelocity": -0.37939851487799736,
+            "velocityX": 2.720627691109149,
+            "velocityY": -0.7193366512682123,
+            "timestamp": 0.33774067445769645
+        },
+        {
+            "x": 3.161437459702618,
+            "y": 6.529235060521681,
+            "heading": -2.870128852530637,
+            "angularVelocity": -0.4742821177371166,
+            "velocityX": 3.400784169785955,
+            "velocityY": -0.8991703007411442,
+            "timestamp": 0.4221758430721206
+        },
+        {
+            "x": 3.470239032500689,
+            "y": 6.44758707798969,
+            "heading": -2.8701288573799584,
+            "angularVelocity": -5.743248864696825e-8,
+            "velocityX": 3.6572624637989906,
+            "velocityY": -0.9669902230531149,
+            "timestamp": 0.5066110116865447
+        },
+        {
+            "x": 3.77904060528322,
+            "y": 6.3659390953988515,
+            "heading": -2.8701288622292584,
+            "angularVelocity": -5.743222732502049e-8,
+            "velocityX": 3.6572624636149422,
+            "velocityY": -0.9669902237500612,
+            "timestamp": 0.5910461803009688
+        },
+        {
+            "x": 4.087842178065752,
+            "y": 6.284291112808013,
+            "heading": -2.870128867078558,
+            "angularVelocity": -5.743222733813293e-8,
+            "velocityX": 3.6572624636149413,
+            "velocityY": -0.9669902237500647,
+            "timestamp": 0.6754813489153929
+        },
+        {
+            "x": 4.396643750848283,
+            "y": 6.202643130217175,
+            "heading": -2.870128871927858,
+            "angularVelocity": -5.7432227678581676e-8,
+            "velocityX": 3.657262463614942,
+            "velocityY": -0.9669902237500649,
+            "timestamp": 0.759916517529817
+        },
+        {
+            "x": 4.705445323630815,
+            "y": 6.120995147626337,
+            "heading": -2.870128876777158,
+            "angularVelocity": -5.743222718130824e-8,
+            "velocityX": 3.657262463614942,
+            "velocityY": -0.9669902237500648,
+            "timestamp": 0.8443516861442412
+        },
+        {
+            "x": 5.014246896413346,
+            "y": 6.039347165035499,
+            "heading": -2.870128881626458,
+            "angularVelocity": -5.743222751764232e-8,
+            "velocityX": 3.657262463614942,
+            "velocityY": -0.9669902237500648,
+            "timestamp": 0.9287868547586653
+        },
+        {
+            "x": 5.323048469195878,
+            "y": 5.957699182444661,
+            "heading": -2.8701288864757575,
+            "angularVelocity": -5.74322277347238e-8,
+            "velocityX": 3.657262463614942,
+            "velocityY": -0.9669902237500648,
+            "timestamp": 1.0132220233730893
+        },
+        {
+            "x": 5.631850041978409,
+            "y": 5.876051199853823,
+            "heading": -2.8701288913250576,
+            "angularVelocity": -5.743222678749957e-8,
+            "velocityX": 3.657262463614942,
+            "velocityY": -0.9669902237500648,
+            "timestamp": 1.0976571919875133
+        },
+        {
+            "x": 5.940651614760942,
+            "y": 5.7944032172629845,
+            "heading": -2.870128896174357,
+            "angularVelocity": -5.7432226879414726e-8,
+            "velocityX": 3.6572624636149413,
+            "velocityY": -0.9669902237500653,
+            "timestamp": 1.1820923606019373
+        },
+        {
+            "x": 6.249453187542404,
+            "y": 5.712755234668102,
+            "heading": -2.8701289010236573,
+            "angularVelocity": -5.743222723099526e-8,
+            "velocityX": 3.6572624636022777,
+            "velocityY": -0.9669902237979594,
+            "timestamp": 1.2665275292163614
+        },
+        {
+            "x": 6.5582545569659105,
+            "y": 5.631106482955165,
+            "heading": -2.8701289058729587,
+            "angularVelocity": -5.743224882801054e-8,
+            "velocityX": 3.6572600551514123,
+            "velocityY": -0.9669993327755135,
+            "timestamp": 1.3509626978307854
+        },
+        {
+            "x": 6.8516839479397085,
+            "y": 5.504916165148498,
+            "heading": -2.8701289109689694,
+            "angularVelocity": -6.035412197409506e-8,
+            "velocityX": 3.4752034701766337,
+            "velocityY": -1.4945231930893996,
+            "timestamp": 1.4353978664452094
+        },
+        {
+            "x": 7.116648885902659,
+            "y": 5.326539084420994,
+            "heading": -2.8701289171366793,
+            "angularVelocity": -7.304669371510495e-8,
+            "velocityX": 3.1380873907283835,
+            "velocityY": -2.1125922249539495,
+            "timestamp": 1.5198330350596334
+        },
+        {
+            "x": 7.343971949437644,
+            "y": 5.1021533761176485,
+            "heading": -2.8701292296822887,
+            "angularVelocity": -0.0000037016046138767138,
+            "velocityX": 2.6922793815105543,
+            "velocityY": -2.6574910903300615,
+            "timestamp": 1.6042682036740574
+        },
+        {
+            "x": 7.516721651510428,
+            "y": 4.857947803192162,
+            "heading": -2.9165540487463932,
+            "angularVelocity": -0.5498279902312642,
+            "velocityX": 2.0459448936692075,
+            "velocityY": -2.8922257980043913,
+            "timestamp": 1.6887033722884814
+        },
+        {
+            "x": 7.630386965757564,
+            "y": 4.610372175146774,
+            "heading": -2.9830505381288734,
+            "angularVelocity": -0.7875449350512241,
+            "velocityX": 1.3461844881981369,
+            "velocityY": -2.932138729727077,
+            "timestamp": 1.7731385409029055
+        },
+        {
+            "x": 7.684751488226614,
+            "y": 4.365674345538311,
+            "heading": -3.0601706707306313,
+            "angularVelocity": -0.9133650570882078,
+            "velocityX": 0.6438611228136838,
+            "velocityY": -2.8980557938586533,
+            "timestamp": 1.8575737095173295
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.9831760605353209,
+            "velocityX": -0.05627380515240752,
+            "velocityY": -2.8267172252385406,
+            "timestamp": 1.9420088781317535
+        },
+        {
+            "x": 7.621934708150364,
+            "y": 3.906654914920449,
+            "heading": -3.2251079869587973,
+            "angularVelocity": -1.0177912145669754,
+            "velocityX": -0.7213917327302901,
+            "velocityY": -2.737523874602733,
+            "timestamp": 2.022499531427526
+        },
+        {
+            "x": 7.510577058905177,
+            "y": 3.6951455922988243,
+            "heading": -3.30599509113983,
+            "angularVelocity": -1.0049254275993986,
+            "velocityX": -1.3834854692504928,
+            "velocityY": -2.627750104653823,
+            "timestamp": 2.102990184723298
+        },
+        {
+            "x": 7.346480542958668,
+            "y": 3.4951541921560745,
+            "heading": -3.379583459687032,
+            "angularVelocity": -0.9142473757388557,
+            "velocityX": -2.0387027465600407,
+            "velocityY": -2.484653707652887,
+            "timestamp": 2.1834808380190704
+        },
+        {
+            "x": 7.131234653225788,
+            "y": 3.3116699228744952,
+            "heading": -3.433702808915206,
+            "angularVelocity": -0.6723681199262506,
+            "velocityX": -2.67417247741705,
+            "velocityY": -2.279572369817109,
+            "timestamp": 2.2639714913148428
+        },
+        {
+            "x": 6.871582405242626,
+            "y": 3.1556914599677013,
+            "heading": -3.4376019485164777,
+            "angularVelocity": -0.04844214131228369,
+            "velocityX": -3.2258683132939807,
+            "velocityY": -1.9378456568569487,
+            "timestamp": 2.344462144610615
+        },
+        {
+            "x": 6.586927593692301,
+            "y": 3.0475868041755487,
+            "heading": -3.4376019671352136,
+            "angularVelocity": -2.3131550136324309e-7,
+            "velocityX": -3.5364952313696496,
+            "velocityY": -1.3430709202335072,
+            "timestamp": 2.4249527979063874
+        },
+        {
+            "x": 6.287649067705066,
+            "y": 2.9914855953111847,
+            "heading": -3.4376019800205944,
+            "angularVelocity": -1.6008543147444885e-7,
+            "velocityX": -3.718177375048765,
+            "velocityY": -0.6969903531309354,
+            "timestamp": 2.5054434512021597
+        },
+        {
+            "x": 5.983166666187018,
+            "y": 2.9891511130138855,
+            "heading": -3.437601992016166,
+            "angularVelocity": -1.4903061473211454e-7,
+            "velocityX": -3.7828292981942986,
+            "velocityY": -0.029003147591768797,
+            "timestamp": 2.585934104497932
+        },
+        {
+            "x": 5.678681298356483,
+            "y": 2.9910598954241565,
+            "heading": -3.437602003982072,
+            "angularVelocity": -1.4866205580605315e-7,
+            "velocityX": -3.782866151075542,
+            "velocityY": 0.02371433616337754,
+            "timestamp": 2.6664247577937044
+        },
+        {
+            "x": 5.374195930686038,
+            "y": 2.9929687033714,
+            "heading": -3.437602015947978,
+            "angularVelocity": -1.4866205622499977e-7,
+            "velocityX": -3.7828661490866247,
+            "velocityY": 0.02371465342968995,
+            "timestamp": 2.7469154110894767
+        },
+        {
+            "x": 5.06971056314094,
+            "y": 2.9948775312983553,
+            "heading": -3.4376020279140773,
+            "angularVelocity": -1.4866445697347623e-7,
+            "velocityX": -3.7828661475293295,
+            "velocityY": 0.023714901653687247,
+            "timestamp": 2.827406064385249
+        },
+        {
+            "x": 4.800808155197524,
+            "y": 2.9965858943831902,
+            "heading": -3.5281868157365524,
+            "angularVelocity": -1.1254075363217084,
+            "velocityX": -3.340790476073066,
+            "velocityY": 0.021224365996295316,
+            "timestamp": 2.9078967176810213
+        },
+        {
+            "x": 4.585685258881781,
+            "y": 2.9979519483478,
+            "heading": -3.6006057670766456,
+            "angularVelocity": -0.8997187670227613,
+            "velocityX": -2.6726444314627344,
+            "velocityY": 0.016971585006104423,
+            "timestamp": 2.9883873709767936
+        },
+        {
+            "x": 4.424342674038891,
+            "y": 2.9989760866788195,
+            "heading": -3.6548985624477908,
+            "angularVelocity": -0.6745229805958503,
+            "velocityX": -2.0044884497336284,
+            "velocityY": 0.012723692616254528,
+            "timestamp": 3.068878024272566
+        },
+        {
+            "x": 4.316780863688961,
+            "y": 2.9996586949819566,
+            "heading": -3.6910886560266527,
+            "angularVelocity": -0.44961858423356516,
+            "velocityX": -1.33632671553406,
+            "velocityY": 0.008480590915651792,
+            "timestamp": 3.1493686775683383
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -3.7091853071795864,
+            "angularVelocity": -0.22482922441248102,
+            "velocityX": -0.6681628423530913,
+            "velocityY": 0.004240306222743686,
+            "timestamp": 3.2298593308641106
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -3.7091853071795864,
+            "angularVelocity": 0,
+            "velocityX": -1.1582419131838594e-33,
+            "velocityY": 0,
+            "timestamp": 3.310349984159883
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S3.traj
new file mode 100644
index 00000000..c8a9e073
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S3.traj	
@@ -0,0 +1,373 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3574291714983375,
+            "y": 6.741815654710436,
+            "heading": -2.758007728773179,
+            "angularVelocity": -0.09483878465079744,
+            "velocityX": 0.6801570061474024,
+            "velocityY": -0.17983436924136462,
+            "timestamp": 0.08443516861442411
+        },
+        {
+            "x": 2.47228751228047,
+            "y": 6.711446972989247,
+            "heading": -2.7740235521881464,
+            "angularVelocity": -0.18968190243216043,
+            "velocityX": 1.3603139860670759,
+            "velocityY": -0.35966863357457735,
+            "timestamp": 0.16887033722884823
+        },
+        {
+            "x": 2.6445750170696067,
+            "y": 6.665893967918211,
+            "heading": -2.798048184372895,
+            "angularVelocity": -0.2845334779214143,
+            "velocityX": 2.0404709034915762,
+            "velocityY": -0.5395027429749605,
+            "timestamp": 0.2533055058432723
+        },
+        {
+            "x": 2.8742916749054777,
+            "y": 6.605156656477843,
+            "heading": -2.830082761948688,
+            "angularVelocity": -0.37939851487799736,
+            "velocityX": 2.720627691109149,
+            "velocityY": -0.7193366512682123,
+            "timestamp": 0.33774067445769645
+        },
+        {
+            "x": 3.161437459702618,
+            "y": 6.529235060521681,
+            "heading": -2.870128852530637,
+            "angularVelocity": -0.4742821177371166,
+            "velocityX": 3.400784169785955,
+            "velocityY": -0.8991703007411442,
+            "timestamp": 0.4221758430721206
+        },
+        {
+            "x": 3.470239032500689,
+            "y": 6.44758707798969,
+            "heading": -2.8701288573799584,
+            "angularVelocity": -5.743248864696825e-8,
+            "velocityX": 3.6572624637989906,
+            "velocityY": -0.9669902230531149,
+            "timestamp": 0.5066110116865447
+        },
+        {
+            "x": 3.77904060528322,
+            "y": 6.3659390953988515,
+            "heading": -2.8701288622292584,
+            "angularVelocity": -5.743222732502049e-8,
+            "velocityX": 3.6572624636149422,
+            "velocityY": -0.9669902237500612,
+            "timestamp": 0.5910461803009688
+        },
+        {
+            "x": 4.087842178065752,
+            "y": 6.284291112808013,
+            "heading": -2.870128867078558,
+            "angularVelocity": -5.743222733813293e-8,
+            "velocityX": 3.6572624636149413,
+            "velocityY": -0.9669902237500647,
+            "timestamp": 0.6754813489153929
+        },
+        {
+            "x": 4.396643750848283,
+            "y": 6.202643130217175,
+            "heading": -2.870128871927858,
+            "angularVelocity": -5.7432227678581676e-8,
+            "velocityX": 3.657262463614942,
+            "velocityY": -0.9669902237500649,
+            "timestamp": 0.759916517529817
+        },
+        {
+            "x": 4.705445323630815,
+            "y": 6.120995147626337,
+            "heading": -2.870128876777158,
+            "angularVelocity": -5.743222718130824e-8,
+            "velocityX": 3.657262463614942,
+            "velocityY": -0.9669902237500648,
+            "timestamp": 0.8443516861442412
+        },
+        {
+            "x": 5.014246896413346,
+            "y": 6.039347165035499,
+            "heading": -2.870128881626458,
+            "angularVelocity": -5.743222751764232e-8,
+            "velocityX": 3.657262463614942,
+            "velocityY": -0.9669902237500648,
+            "timestamp": 0.9287868547586653
+        },
+        {
+            "x": 5.323048469195878,
+            "y": 5.957699182444661,
+            "heading": -2.8701288864757575,
+            "angularVelocity": -5.74322277347238e-8,
+            "velocityX": 3.657262463614942,
+            "velocityY": -0.9669902237500648,
+            "timestamp": 1.0132220233730893
+        },
+        {
+            "x": 5.631850041978409,
+            "y": 5.876051199853823,
+            "heading": -2.8701288913250576,
+            "angularVelocity": -5.743222678749957e-8,
+            "velocityX": 3.657262463614942,
+            "velocityY": -0.9669902237500648,
+            "timestamp": 1.0976571919875133
+        },
+        {
+            "x": 5.940651614760942,
+            "y": 5.7944032172629845,
+            "heading": -2.870128896174357,
+            "angularVelocity": -5.7432226879414726e-8,
+            "velocityX": 3.6572624636149413,
+            "velocityY": -0.9669902237500653,
+            "timestamp": 1.1820923606019373
+        },
+        {
+            "x": 6.249453187542404,
+            "y": 5.712755234668102,
+            "heading": -2.8701289010236573,
+            "angularVelocity": -5.743222723099526e-8,
+            "velocityX": 3.6572624636022777,
+            "velocityY": -0.9669902237979594,
+            "timestamp": 1.2665275292163614
+        },
+        {
+            "x": 6.5582545569659105,
+            "y": 5.631106482955165,
+            "heading": -2.8701289058729587,
+            "angularVelocity": -5.743224882801054e-8,
+            "velocityX": 3.6572600551514123,
+            "velocityY": -0.9669993327755135,
+            "timestamp": 1.3509626978307854
+        },
+        {
+            "x": 6.8516839479397085,
+            "y": 5.504916165148498,
+            "heading": -2.8701289109689694,
+            "angularVelocity": -6.035412197409506e-8,
+            "velocityX": 3.4752034701766337,
+            "velocityY": -1.4945231930893996,
+            "timestamp": 1.4353978664452094
+        },
+        {
+            "x": 7.116648885902659,
+            "y": 5.326539084420994,
+            "heading": -2.8701289171366793,
+            "angularVelocity": -7.304669371510495e-8,
+            "velocityX": 3.1380873907283835,
+            "velocityY": -2.1125922249539495,
+            "timestamp": 1.5198330350596334
+        },
+        {
+            "x": 7.343971949437644,
+            "y": 5.1021533761176485,
+            "heading": -2.8701292296822887,
+            "angularVelocity": -0.0000037016046138767138,
+            "velocityX": 2.6922793815105543,
+            "velocityY": -2.6574910903300615,
+            "timestamp": 1.6042682036740574
+        },
+        {
+            "x": 7.516721651510428,
+            "y": 4.857947803192162,
+            "heading": -2.9165540487463932,
+            "angularVelocity": -0.5498279902312642,
+            "velocityX": 2.0459448936692075,
+            "velocityY": -2.8922257980043913,
+            "timestamp": 1.6887033722884814
+        },
+        {
+            "x": 7.630386965757564,
+            "y": 4.610372175146774,
+            "heading": -2.9830505381288734,
+            "angularVelocity": -0.7875449350512241,
+            "velocityX": 1.3461844881981369,
+            "velocityY": -2.932138729727077,
+            "timestamp": 1.7731385409029055
+        },
+        {
+            "x": 7.684751488226614,
+            "y": 4.365674345538311,
+            "heading": -3.0601706707306313,
+            "angularVelocity": -0.9133650570882078,
+            "velocityX": 0.6438611228136838,
+            "velocityY": -2.8980557938586533,
+            "timestamp": 1.8575737095173295
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.9831760605353209,
+            "velocityX": -0.05627380515240752,
+            "velocityY": -2.8267172252385406,
+            "timestamp": 1.9420088781317535
+        },
+        {
+            "x": 7.621934708150364,
+            "y": 3.906654914920449,
+            "heading": -3.2251079869587973,
+            "angularVelocity": -1.0177912145669754,
+            "velocityX": -0.7213917327302901,
+            "velocityY": -2.737523874602733,
+            "timestamp": 2.022499531427526
+        },
+        {
+            "x": 7.510577058905177,
+            "y": 3.6951455922988243,
+            "heading": -3.30599509113983,
+            "angularVelocity": -1.0049254275993986,
+            "velocityX": -1.3834854692504928,
+            "velocityY": -2.627750104653823,
+            "timestamp": 2.102990184723298
+        },
+        {
+            "x": 7.346480542958668,
+            "y": 3.4951541921560745,
+            "heading": -3.379583459687032,
+            "angularVelocity": -0.9142473757388557,
+            "velocityX": -2.0387027465600407,
+            "velocityY": -2.484653707652887,
+            "timestamp": 2.1834808380190704
+        },
+        {
+            "x": 7.131234653225788,
+            "y": 3.3116699228744952,
+            "heading": -3.433702808915206,
+            "angularVelocity": -0.6723681199262506,
+            "velocityX": -2.67417247741705,
+            "velocityY": -2.279572369817109,
+            "timestamp": 2.2639714913148428
+        },
+        {
+            "x": 6.871582405242626,
+            "y": 3.1556914599677013,
+            "heading": -3.4376019485164777,
+            "angularVelocity": -0.04844214131228369,
+            "velocityX": -3.2258683132939807,
+            "velocityY": -1.9378456568569487,
+            "timestamp": 2.344462144610615
+        },
+        {
+            "x": 6.586927593692301,
+            "y": 3.0475868041755487,
+            "heading": -3.4376019671352136,
+            "angularVelocity": -2.3131550136324309e-7,
+            "velocityX": -3.5364952313696496,
+            "velocityY": -1.3430709202335072,
+            "timestamp": 2.4249527979063874
+        },
+        {
+            "x": 6.287649067705066,
+            "y": 2.9914855953111847,
+            "heading": -3.4376019800205944,
+            "angularVelocity": -1.6008543147444885e-7,
+            "velocityX": -3.718177375048765,
+            "velocityY": -0.6969903531309354,
+            "timestamp": 2.5054434512021597
+        },
+        {
+            "x": 5.983166666187018,
+            "y": 2.9891511130138855,
+            "heading": -3.437601992016166,
+            "angularVelocity": -1.4903061473211454e-7,
+            "velocityX": -3.7828292981942986,
+            "velocityY": -0.029003147591768797,
+            "timestamp": 2.585934104497932
+        },
+        {
+            "x": 5.678681298356483,
+            "y": 2.9910598954241565,
+            "heading": -3.437602003982072,
+            "angularVelocity": -1.4866205580605315e-7,
+            "velocityX": -3.782866151075542,
+            "velocityY": 0.02371433616337754,
+            "timestamp": 2.6664247577937044
+        },
+        {
+            "x": 5.374195930686038,
+            "y": 2.9929687033714,
+            "heading": -3.437602015947978,
+            "angularVelocity": -1.4866205622499977e-7,
+            "velocityX": -3.7828661490866247,
+            "velocityY": 0.02371465342968995,
+            "timestamp": 2.7469154110894767
+        },
+        {
+            "x": 5.06971056314094,
+            "y": 2.9948775312983553,
+            "heading": -3.4376020279140773,
+            "angularVelocity": -1.4866445697347623e-7,
+            "velocityX": -3.7828661475293295,
+            "velocityY": 0.023714901653687247,
+            "timestamp": 2.827406064385249
+        },
+        {
+            "x": 4.800808155197524,
+            "y": 2.9965858943831902,
+            "heading": -3.5281868157365524,
+            "angularVelocity": -1.1254075363217084,
+            "velocityX": -3.340790476073066,
+            "velocityY": 0.021224365996295316,
+            "timestamp": 2.9078967176810213
+        },
+        {
+            "x": 4.585685258881781,
+            "y": 2.9979519483478,
+            "heading": -3.6006057670766456,
+            "angularVelocity": -0.8997187670227613,
+            "velocityX": -2.6726444314627344,
+            "velocityY": 0.016971585006104423,
+            "timestamp": 2.9883873709767936
+        },
+        {
+            "x": 4.424342674038891,
+            "y": 2.9989760866788195,
+            "heading": -3.6548985624477908,
+            "angularVelocity": -0.6745229805958503,
+            "velocityX": -2.0044884497336284,
+            "velocityY": 0.012723692616254528,
+            "timestamp": 3.068878024272566
+        },
+        {
+            "x": 4.316780863688961,
+            "y": 2.9996586949819566,
+            "heading": -3.6910886560266527,
+            "angularVelocity": -0.44961858423356516,
+            "velocityX": -1.33632671553406,
+            "velocityY": 0.008480590915651792,
+            "timestamp": 3.1493686775683383
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -3.7091853071795864,
+            "angularVelocity": -0.22482922441248102,
+            "velocityX": -0.6681628423530913,
+            "velocityY": 0.004240306222743686,
+            "timestamp": 3.2298593308641106
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -3.7091853071795864,
+            "angularVelocity": 0,
+            "velocityX": -1.1582419131838594e-33,
+            "velocityY": 0,
+            "timestamp": 3.310349984159883
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S4.1.traj
new file mode 100644
index 00000000..f8080ad6
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S4.1.traj	
@@ -0,0 +1,382 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.352388442136136,
+            "y": 6.742171711440638,
+            "heading": -2.7776238211060527,
+            "angularVelocity": -0.340136208728432,
+            "velocityX": 0.6450666626176952,
+            "velocityY": -0.18258291758985748,
+            "timestamp": 0.08121399720716439
+        },
+        {
+            "x": 2.4572309209740357,
+            "y": 6.712518353067644,
+            "heading": -2.831291417681962,
+            "angularVelocity": -0.6608170810552816,
+            "velocityX": 1.290941000856364,
+            "velocityY": -0.3651262022858861,
+            "timestamp": 0.16242799441432879
+        },
+        {
+            "x": 2.614630940945627,
+            "y": 6.668043884281434,
+            "heading": -2.9082589809802553,
+            "angularVelocity": -0.9477130290857405,
+            "velocityX": 1.9380898045256882,
+            "velocityY": -0.5476207342969771,
+            "timestamp": 0.2436419916214932
+        },
+        {
+            "x": 2.824773268720236,
+            "y": 6.608764345103305,
+            "heading": -3.0025843751568475,
+            "angularVelocity": -1.1614425766389054,
+            "velocityX": 2.587513667614254,
+            "velocityY": -0.7299177631454756,
+            "timestamp": 0.32485598882865757
+        },
+        {
+            "x": 3.087924526957624,
+            "y": 6.534856635997473,
+            "heading": -3.094100877088787,
+            "angularVelocity": -1.1268562695832722,
+            "velocityX": 3.2402204950799356,
+            "velocityY": -0.9100365903255841,
+            "timestamp": 0.40606998603582195
+        },
+        {
+            "x": 3.383391154718911,
+            "y": 6.450664344423451,
+            "heading": -3.094100898976011,
+            "angularVelocity": -2.695006379530874e-7,
+            "velocityX": 3.638124435716056,
+            "velocityY": -1.036672180514684,
+            "timestamp": 0.48728398324298633
+        },
+        {
+            "x": 3.6788577741864907,
+            "y": 6.366472023742971,
+            "heading": -3.0941009208630392,
+            "angularVelocity": -2.694982209366716e-7,
+            "velocityX": 3.638124333594396,
+            "velocityY": -1.036672538906832,
+            "timestamp": 0.5684979804501508
+        },
+        {
+            "x": 3.9743243936540216,
+            "y": 6.282279703062319,
+            "heading": -3.094100942750067,
+            "angularVelocity": -2.694982225892601e-7,
+            "velocityX": 3.638124333593797,
+            "velocityY": -1.036672538908934,
+            "timestamp": 0.6497119776573151
+        },
+        {
+            "x": 4.269791013121552,
+            "y": 6.198087382381668,
+            "heading": -3.0941009646370947,
+            "angularVelocity": -2.694982224333276e-7,
+            "velocityX": 3.6381243335937974,
+            "velocityY": -1.0366725389089342,
+            "timestamp": 0.7309259748644795
+        },
+        {
+            "x": 4.565257632589082,
+            "y": 6.113895061701016,
+            "heading": -3.094100986524123,
+            "angularVelocity": -2.694982222034585e-7,
+            "velocityX": 3.6381243335937974,
+            "velocityY": -1.036672538908934,
+            "timestamp": 0.8121399720716439
+        },
+        {
+            "x": 4.860724252056613,
+            "y": 6.0297027410203645,
+            "heading": -3.0941010084111507,
+            "angularVelocity": -2.694982230736536e-7,
+            "velocityX": 3.638124333593797,
+            "velocityY": -1.0366725389089342,
+            "timestamp": 0.8933539692788083
+        },
+        {
+            "x": 5.156190871524144,
+            "y": 5.945510420339713,
+            "heading": -3.094101030298179,
+            "angularVelocity": -2.6949822300117995e-7,
+            "velocityX": 3.638124333593797,
+            "velocityY": -1.0366725389089342,
+            "timestamp": 0.9745679664859727
+        },
+        {
+            "x": 5.451657490991675,
+            "y": 5.861318099659061,
+            "heading": -3.0941010521852066,
+            "angularVelocity": -2.694982228196196e-7,
+            "velocityX": 3.638124333593797,
+            "velocityY": -1.0366725389089342,
+            "timestamp": 1.0557819636931371
+        },
+        {
+            "x": 5.747124110459206,
+            "y": 5.77712577897841,
+            "heading": -3.0941010740722343,
+            "angularVelocity": -2.694982236260836e-7,
+            "velocityX": 3.638124333593797,
+            "velocityY": -1.0366725389089344,
+            "timestamp": 1.1369959609003015
+        },
+        {
+            "x": 6.042590729926668,
+            "y": 5.69293345829752,
+            "heading": -3.0941010959592625,
+            "angularVelocity": -2.694982236351797e-7,
+            "velocityX": 3.6381243335929603,
+            "velocityY": -1.0366725389118696,
+            "timestamp": 1.218209958107466
+        },
+        {
+            "x": 6.338057337809956,
+            "y": 5.6087410969628495,
+            "heading": -3.0941011178462903,
+            "angularVelocity": -2.6949822192516605e-7,
+            "velocityX": 3.6381241909552937,
+            "velocityY": -1.0366730394879038,
+            "timestamp": 1.2994239553146303
+        },
+        {
+            "x": 6.63151320525297,
+            "y": 5.517787680923723,
+            "heading": -3.0941011398106846,
+            "angularVelocity": -2.704508467508134e-7,
+            "velocityX": 3.6133656455032592,
+            "velocityY": -1.1199229094611813,
+            "timestamp": 1.3806379525217947
+        },
+        {
+            "x": 6.904052184030547,
+            "y": 5.375973104754023,
+            "heading": -3.09410116345768,
+            "angularVelocity": -2.911689606888355e-7,
+            "velocityX": 3.355812891235069,
+            "velocityY": -1.7461839220887416,
+            "timestamp": 1.461851949728959
+        },
+        {
+            "x": 7.14694076900972,
+            "y": 5.1878401702780765,
+            "heading": -3.094101192989132,
+            "angularVelocity": -3.636251518218712e-7,
+            "velocityX": 2.9907231921877617,
+            "velocityY": -2.3165087416877084,
+            "timestamp": 1.5430659469361234
+        },
+        {
+            "x": 7.352395911467515,
+            "y": 4.959417222897554,
+            "heading": -3.0941012381435535,
+            "angularVelocity": -5.559930935407798e-7,
+            "velocityX": 2.529799659188236,
+            "velocityY": -2.812605649731368,
+            "timestamp": 1.6242799441432878
+        },
+        {
+            "x": 7.513833646479666,
+            "y": 4.698024306067166,
+            "heading": -3.094103186189668,
+            "angularVelocity": -0.00002398658095119272,
+            "velocityX": 1.9878067889975632,
+            "velocityY": -3.2185697764010213,
+            "timestamp": 1.7054939413504522
+        },
+        {
+            "x": 7.6238137890728055,
+            "y": 4.417795537290539,
+            "heading": -3.1108263182271356,
+            "angularVelocity": -0.20591440655693208,
+            "velocityX": 1.35420181708456,
+            "velocityY": -3.4504984166977444,
+            "timestamp": 1.7867079385576166
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.3984410331631951,
+            "velocityX": 0.6918291533072239,
+            "velocityY": -3.5806086055413933,
+            "timestamp": 1.867921935764781
+        },
+        {
+            "x": 7.6870591404547595,
+            "y": 3.8674277894608684,
+            "heading": -3.1773151030340356,
+            "angularVelocity": -0.47768106939784993,
+            "velocityX": 0.09879982223438938,
+            "velocityY": -3.63297605573316,
+            "timestamp": 1.9393708543807355
+        },
+        {
+            "x": 7.651588574491949,
+            "y": 3.608358714127091,
+            "heading": -3.205456863771594,
+            "angularVelocity": -0.39387245154381195,
+            "velocityX": -0.4964465053735158,
+            "velocityY": -3.625934168821804,
+            "timestamp": 2.0108197729966903
+        },
+        {
+            "x": 7.574047848742119,
+            "y": 3.354318466237311,
+            "heading": -3.2189736743217594,
+            "angularVelocity": -0.1891814573314478,
+            "velocityX": -1.0852610123268396,
+            "velocityY": -3.5555506341084104,
+            "timestamp": 2.082268691612645
+        },
+        {
+            "x": 7.455586952035965,
+            "y": 3.1113741995114603,
+            "heading": -3.2189745116430255,
+            "angularVelocity": -0.00001171915938609546,
+            "velocityX": -1.6579802605551721,
+            "velocityY": -3.4002511365232904,
+            "timestamp": 2.1537176102285995
+        },
+        {
+            "x": 7.300453960582251,
+            "y": 2.8900400447662866,
+            "heading": -3.2189745885172623,
+            "angularVelocity": -0.0000010759328215916604,
+            "velocityX": -2.171243378574256,
+            "velocityY": -3.097795726435581,
+            "timestamp": 2.225166528844554
+        },
+        {
+            "x": 7.11249649568723,
+            "y": 2.6958055218428347,
+            "heading": -3.2189746440183837,
+            "angularVelocity": -7.767944182730611e-7,
+            "velocityX": -2.6306551384378265,
+            "velocityY": -2.718508924810283,
+            "timestamp": 2.2966154474605087
+        },
+        {
+            "x": 6.896376183100963,
+            "y": 2.533487712060768,
+            "heading": -3.2189746912030057,
+            "angularVelocity": -6.603965892997474e-7,
+            "velocityX": -3.024822723297278,
+            "velocityY": -2.2718021899927887,
+            "timestamp": 2.3680643660764633
+        },
+        {
+            "x": 6.6576888713401265,
+            "y": 2.4066672511412732,
+            "heading": -3.218974736751788,
+            "angularVelocity": -6.375013452668414e-7,
+            "velocityX": -3.340670739366957,
+            "velocityY": -1.7749808302282493,
+            "timestamp": 2.439513284692418
+        },
+        {
+            "x": 6.418991689499422,
+            "y": 2.2798653705517777,
+            "heading": -3.2189747823005193,
+            "angularVelocity": -6.375006445255001e-7,
+            "velocityX": -3.3408088808494862,
+            "velocityY": -1.7747207802229084,
+            "timestamp": 2.5109622033083725
+        },
+        {
+            "x": 6.180294507575015,
+            "y": 2.1530634901198433,
+            "heading": -3.2189748278492507,
+            "angularVelocity": -6.375006422464704e-7,
+            "velocityX": -3.340808882020978,
+            "velocityY": -1.7747207780176886,
+            "timestamp": 2.582411121924327
+        },
+        {
+            "x": 5.941597302835271,
+            "y": 2.026261652636718,
+            "heading": -3.21897487339828,
+            "angularVelocity": -6.375048107159433e-7,
+            "velocityX": -3.3408092013447153,
+            "velocityY": -1.774720176905592,
+            "timestamp": 2.6538600405402817
+        },
+        {
+            "x": 5.72404161684181,
+            "y": 1.9132773125800981,
+            "heading": -3.2812904960777876,
+            "angularVelocity": -0.8721702705344045,
+            "velocityX": -3.0449122283008005,
+            "velocityY": -1.5813303020905036,
+            "timestamp": 2.7253089591562363
+        },
+        {
+            "x": 5.540083719687043,
+            "y": 1.8151536354630486,
+            "heading": -3.418088909630078,
+            "angularVelocity": -1.9146323863713337,
+            "velocityX": -2.574677136023417,
+            "velocityY": -1.3733402691069942,
+            "timestamp": 2.796757877772191
+        },
+        {
+            "x": 5.393751164472601,
+            "y": 1.7369217513952075,
+            "heading": -3.5523873708884453,
+            "angularVelocity": -1.8796430213546256,
+            "velocityX": -2.048072357826006,
+            "velocityY": -1.0949344732596382,
+            "timestamp": 2.8682067963881455
+        },
+        {
+            "x": 5.284579981216215,
+            "y": 1.6784003858088632,
+            "heading": -3.6634662967687484,
+            "angularVelocity": -1.554662100328298,
+            "velocityX": -1.5279613095723332,
+            "velocityY": -0.8190657986344941,
+            "timestamp": 2.9396557150041
+        },
+        {
+            "x": 5.21211226324901,
+            "y": 1.6394523869599669,
+            "heading": -3.742196708881707,
+            "angularVelocity": -1.1019118782914785,
+            "velocityX": -1.0142591290490286,
+            "velocityY": -0.5451167016141149,
+            "timestamp": 3.0111046336200546
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -3.7831853071795862,
+            "angularVelocity": -0.5736769582023385,
+            "velocityX": -0.505427709031773,
+            "velocityY": -0.2722558624712981,
+            "timestamp": 3.0825535522360092
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -3.7831853071795862,
+            "angularVelocity": 0,
+            "velocityX": -1.863217485402065e-29,
+            "velocityY": -1.5751272024067454e-29,
+            "timestamp": 3.154002470851964
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S4.traj
new file mode 100644
index 00000000..f8080ad6
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S4.traj	
@@ -0,0 +1,382 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.352388442136136,
+            "y": 6.742171711440638,
+            "heading": -2.7776238211060527,
+            "angularVelocity": -0.340136208728432,
+            "velocityX": 0.6450666626176952,
+            "velocityY": -0.18258291758985748,
+            "timestamp": 0.08121399720716439
+        },
+        {
+            "x": 2.4572309209740357,
+            "y": 6.712518353067644,
+            "heading": -2.831291417681962,
+            "angularVelocity": -0.6608170810552816,
+            "velocityX": 1.290941000856364,
+            "velocityY": -0.3651262022858861,
+            "timestamp": 0.16242799441432879
+        },
+        {
+            "x": 2.614630940945627,
+            "y": 6.668043884281434,
+            "heading": -2.9082589809802553,
+            "angularVelocity": -0.9477130290857405,
+            "velocityX": 1.9380898045256882,
+            "velocityY": -0.5476207342969771,
+            "timestamp": 0.2436419916214932
+        },
+        {
+            "x": 2.824773268720236,
+            "y": 6.608764345103305,
+            "heading": -3.0025843751568475,
+            "angularVelocity": -1.1614425766389054,
+            "velocityX": 2.587513667614254,
+            "velocityY": -0.7299177631454756,
+            "timestamp": 0.32485598882865757
+        },
+        {
+            "x": 3.087924526957624,
+            "y": 6.534856635997473,
+            "heading": -3.094100877088787,
+            "angularVelocity": -1.1268562695832722,
+            "velocityX": 3.2402204950799356,
+            "velocityY": -0.9100365903255841,
+            "timestamp": 0.40606998603582195
+        },
+        {
+            "x": 3.383391154718911,
+            "y": 6.450664344423451,
+            "heading": -3.094100898976011,
+            "angularVelocity": -2.695006379530874e-7,
+            "velocityX": 3.638124435716056,
+            "velocityY": -1.036672180514684,
+            "timestamp": 0.48728398324298633
+        },
+        {
+            "x": 3.6788577741864907,
+            "y": 6.366472023742971,
+            "heading": -3.0941009208630392,
+            "angularVelocity": -2.694982209366716e-7,
+            "velocityX": 3.638124333594396,
+            "velocityY": -1.036672538906832,
+            "timestamp": 0.5684979804501508
+        },
+        {
+            "x": 3.9743243936540216,
+            "y": 6.282279703062319,
+            "heading": -3.094100942750067,
+            "angularVelocity": -2.694982225892601e-7,
+            "velocityX": 3.638124333593797,
+            "velocityY": -1.036672538908934,
+            "timestamp": 0.6497119776573151
+        },
+        {
+            "x": 4.269791013121552,
+            "y": 6.198087382381668,
+            "heading": -3.0941009646370947,
+            "angularVelocity": -2.694982224333276e-7,
+            "velocityX": 3.6381243335937974,
+            "velocityY": -1.0366725389089342,
+            "timestamp": 0.7309259748644795
+        },
+        {
+            "x": 4.565257632589082,
+            "y": 6.113895061701016,
+            "heading": -3.094100986524123,
+            "angularVelocity": -2.694982222034585e-7,
+            "velocityX": 3.6381243335937974,
+            "velocityY": -1.036672538908934,
+            "timestamp": 0.8121399720716439
+        },
+        {
+            "x": 4.860724252056613,
+            "y": 6.0297027410203645,
+            "heading": -3.0941010084111507,
+            "angularVelocity": -2.694982230736536e-7,
+            "velocityX": 3.638124333593797,
+            "velocityY": -1.0366725389089342,
+            "timestamp": 0.8933539692788083
+        },
+        {
+            "x": 5.156190871524144,
+            "y": 5.945510420339713,
+            "heading": -3.094101030298179,
+            "angularVelocity": -2.6949822300117995e-7,
+            "velocityX": 3.638124333593797,
+            "velocityY": -1.0366725389089342,
+            "timestamp": 0.9745679664859727
+        },
+        {
+            "x": 5.451657490991675,
+            "y": 5.861318099659061,
+            "heading": -3.0941010521852066,
+            "angularVelocity": -2.694982228196196e-7,
+            "velocityX": 3.638124333593797,
+            "velocityY": -1.0366725389089342,
+            "timestamp": 1.0557819636931371
+        },
+        {
+            "x": 5.747124110459206,
+            "y": 5.77712577897841,
+            "heading": -3.0941010740722343,
+            "angularVelocity": -2.694982236260836e-7,
+            "velocityX": 3.638124333593797,
+            "velocityY": -1.0366725389089344,
+            "timestamp": 1.1369959609003015
+        },
+        {
+            "x": 6.042590729926668,
+            "y": 5.69293345829752,
+            "heading": -3.0941010959592625,
+            "angularVelocity": -2.694982236351797e-7,
+            "velocityX": 3.6381243335929603,
+            "velocityY": -1.0366725389118696,
+            "timestamp": 1.218209958107466
+        },
+        {
+            "x": 6.338057337809956,
+            "y": 5.6087410969628495,
+            "heading": -3.0941011178462903,
+            "angularVelocity": -2.6949822192516605e-7,
+            "velocityX": 3.6381241909552937,
+            "velocityY": -1.0366730394879038,
+            "timestamp": 1.2994239553146303
+        },
+        {
+            "x": 6.63151320525297,
+            "y": 5.517787680923723,
+            "heading": -3.0941011398106846,
+            "angularVelocity": -2.704508467508134e-7,
+            "velocityX": 3.6133656455032592,
+            "velocityY": -1.1199229094611813,
+            "timestamp": 1.3806379525217947
+        },
+        {
+            "x": 6.904052184030547,
+            "y": 5.375973104754023,
+            "heading": -3.09410116345768,
+            "angularVelocity": -2.911689606888355e-7,
+            "velocityX": 3.355812891235069,
+            "velocityY": -1.7461839220887416,
+            "timestamp": 1.461851949728959
+        },
+        {
+            "x": 7.14694076900972,
+            "y": 5.1878401702780765,
+            "heading": -3.094101192989132,
+            "angularVelocity": -3.636251518218712e-7,
+            "velocityX": 2.9907231921877617,
+            "velocityY": -2.3165087416877084,
+            "timestamp": 1.5430659469361234
+        },
+        {
+            "x": 7.352395911467515,
+            "y": 4.959417222897554,
+            "heading": -3.0941012381435535,
+            "angularVelocity": -5.559930935407798e-7,
+            "velocityX": 2.529799659188236,
+            "velocityY": -2.812605649731368,
+            "timestamp": 1.6242799441432878
+        },
+        {
+            "x": 7.513833646479666,
+            "y": 4.698024306067166,
+            "heading": -3.094103186189668,
+            "angularVelocity": -0.00002398658095119272,
+            "velocityX": 1.9878067889975632,
+            "velocityY": -3.2185697764010213,
+            "timestamp": 1.7054939413504522
+        },
+        {
+            "x": 7.6238137890728055,
+            "y": 4.417795537290539,
+            "heading": -3.1108263182271356,
+            "angularVelocity": -0.20591440655693208,
+            "velocityX": 1.35420181708456,
+            "velocityY": -3.4504984166977444,
+            "timestamp": 1.7867079385576166
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.3984410331631951,
+            "velocityX": 0.6918291533072239,
+            "velocityY": -3.5806086055413933,
+            "timestamp": 1.867921935764781
+        },
+        {
+            "x": 7.6870591404547595,
+            "y": 3.8674277894608684,
+            "heading": -3.1773151030340356,
+            "angularVelocity": -0.47768106939784993,
+            "velocityX": 0.09879982223438938,
+            "velocityY": -3.63297605573316,
+            "timestamp": 1.9393708543807355
+        },
+        {
+            "x": 7.651588574491949,
+            "y": 3.608358714127091,
+            "heading": -3.205456863771594,
+            "angularVelocity": -0.39387245154381195,
+            "velocityX": -0.4964465053735158,
+            "velocityY": -3.625934168821804,
+            "timestamp": 2.0108197729966903
+        },
+        {
+            "x": 7.574047848742119,
+            "y": 3.354318466237311,
+            "heading": -3.2189736743217594,
+            "angularVelocity": -0.1891814573314478,
+            "velocityX": -1.0852610123268396,
+            "velocityY": -3.5555506341084104,
+            "timestamp": 2.082268691612645
+        },
+        {
+            "x": 7.455586952035965,
+            "y": 3.1113741995114603,
+            "heading": -3.2189745116430255,
+            "angularVelocity": -0.00001171915938609546,
+            "velocityX": -1.6579802605551721,
+            "velocityY": -3.4002511365232904,
+            "timestamp": 2.1537176102285995
+        },
+        {
+            "x": 7.300453960582251,
+            "y": 2.8900400447662866,
+            "heading": -3.2189745885172623,
+            "angularVelocity": -0.0000010759328215916604,
+            "velocityX": -2.171243378574256,
+            "velocityY": -3.097795726435581,
+            "timestamp": 2.225166528844554
+        },
+        {
+            "x": 7.11249649568723,
+            "y": 2.6958055218428347,
+            "heading": -3.2189746440183837,
+            "angularVelocity": -7.767944182730611e-7,
+            "velocityX": -2.6306551384378265,
+            "velocityY": -2.718508924810283,
+            "timestamp": 2.2966154474605087
+        },
+        {
+            "x": 6.896376183100963,
+            "y": 2.533487712060768,
+            "heading": -3.2189746912030057,
+            "angularVelocity": -6.603965892997474e-7,
+            "velocityX": -3.024822723297278,
+            "velocityY": -2.2718021899927887,
+            "timestamp": 2.3680643660764633
+        },
+        {
+            "x": 6.6576888713401265,
+            "y": 2.4066672511412732,
+            "heading": -3.218974736751788,
+            "angularVelocity": -6.375013452668414e-7,
+            "velocityX": -3.340670739366957,
+            "velocityY": -1.7749808302282493,
+            "timestamp": 2.439513284692418
+        },
+        {
+            "x": 6.418991689499422,
+            "y": 2.2798653705517777,
+            "heading": -3.2189747823005193,
+            "angularVelocity": -6.375006445255001e-7,
+            "velocityX": -3.3408088808494862,
+            "velocityY": -1.7747207802229084,
+            "timestamp": 2.5109622033083725
+        },
+        {
+            "x": 6.180294507575015,
+            "y": 2.1530634901198433,
+            "heading": -3.2189748278492507,
+            "angularVelocity": -6.375006422464704e-7,
+            "velocityX": -3.340808882020978,
+            "velocityY": -1.7747207780176886,
+            "timestamp": 2.582411121924327
+        },
+        {
+            "x": 5.941597302835271,
+            "y": 2.026261652636718,
+            "heading": -3.21897487339828,
+            "angularVelocity": -6.375048107159433e-7,
+            "velocityX": -3.3408092013447153,
+            "velocityY": -1.774720176905592,
+            "timestamp": 2.6538600405402817
+        },
+        {
+            "x": 5.72404161684181,
+            "y": 1.9132773125800981,
+            "heading": -3.2812904960777876,
+            "angularVelocity": -0.8721702705344045,
+            "velocityX": -3.0449122283008005,
+            "velocityY": -1.5813303020905036,
+            "timestamp": 2.7253089591562363
+        },
+        {
+            "x": 5.540083719687043,
+            "y": 1.8151536354630486,
+            "heading": -3.418088909630078,
+            "angularVelocity": -1.9146323863713337,
+            "velocityX": -2.574677136023417,
+            "velocityY": -1.3733402691069942,
+            "timestamp": 2.796757877772191
+        },
+        {
+            "x": 5.393751164472601,
+            "y": 1.7369217513952075,
+            "heading": -3.5523873708884453,
+            "angularVelocity": -1.8796430213546256,
+            "velocityX": -2.048072357826006,
+            "velocityY": -1.0949344732596382,
+            "timestamp": 2.8682067963881455
+        },
+        {
+            "x": 5.284579981216215,
+            "y": 1.6784003858088632,
+            "heading": -3.6634662967687484,
+            "angularVelocity": -1.554662100328298,
+            "velocityX": -1.5279613095723332,
+            "velocityY": -0.8190657986344941,
+            "timestamp": 2.9396557150041
+        },
+        {
+            "x": 5.21211226324901,
+            "y": 1.6394523869599669,
+            "heading": -3.742196708881707,
+            "angularVelocity": -1.1019118782914785,
+            "velocityX": -1.0142591290490286,
+            "velocityY": -0.5451167016141149,
+            "timestamp": 3.0111046336200546
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -3.7831853071795862,
+            "angularVelocity": -0.5736769582023385,
+            "velocityX": -0.505427709031773,
+            "velocityY": -0.2722558624712981,
+            "timestamp": 3.0825535522360092
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -3.7831853071795862,
+            "angularVelocity": 0,
+            "velocityX": -1.863217485402065e-29,
+            "velocityY": -1.5751272024067454e-29,
+            "timestamp": 3.154002470851964
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.1.traj
new file mode 100644
index 00000000..8e3875a1
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.1.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2980013304879616,
+            "y": 6.709118488011682,
+            "heading": -2.76564785007466,
+            "angularVelocity": -0.2059742190979395,
+            "velocityX": -0.026308687136742944,
+            "velocityY": -0.6302691420205845,
+            "timestamp": 0.07596994489499287
+        },
+        {
+            "x": 2.294004010549845,
+            "y": 6.6133557271698225,
+            "heading": -2.796958855018623,
+            "angularVelocity": -0.4121498967419566,
+            "velocityX": -0.05261712304308625,
+            "velocityY": -1.2605348203717124,
+            "timestamp": 0.15193988978998574
+        },
+        {
+            "x": 2.2880079544710785,
+            "y": 6.469712199120552,
+            "heading": -2.8439591282551087,
+            "angularVelocity": -0.618669307993449,
+            "velocityX": -0.07892668721893806,
+            "velocityY": -1.8907941587665607,
+            "timestamp": 0.2279098346849786
+        },
+        {
+            "x": 2.2800128266474267,
+            "y": 6.278188582998367,
+            "heading": -2.906683998413123,
+            "angularVelocity": -0.8256537535299532,
+            "velocityX": -0.10524066898696208,
+            "velocityY": -2.5210445576459737,
+            "timestamp": 0.3038797795799715
+        },
+        {
+            "x": 2.27001785393274,
+            "y": 6.038785706839934,
+            "heading": -2.985174219418862,
+            "angularVelocity": -1.0331746470822023,
+            "velocityX": -0.13156482775526773,
+            "velocityY": -3.151284056995696,
+            "timestamp": 0.37984972447496435
+        },
+        {
+            "x": 2.2620112641506833,
+            "y": 5.847271276792759,
+            "heading": -3.048371938278793,
+            "angularVelocity": -0.8318779083923785,
+            "velocityX": -0.10539154389440716,
+            "velocityY": -2.5209236404197743,
+            "timestamp": 0.4558196693699572
+        },
+        {
+            "x": 2.256005772930322,
+            "y": 5.703635612537123,
+            "heading": -3.0957774424626403,
+            "angularVelocity": -0.6240034035745593,
+            "velocityX": -0.07905088293353696,
+            "velocityY": -1.8906906468626767,
+            "timestamp": 0.5317896142649501
+        },
+        {
+            "x": 2.25200192771716,
+            "y": 5.607878538669833,
+            "heading": -3.127382702013982,
+            "angularVelocity": -0.4160231996354264,
+            "velocityX": -0.05270301589261432,
+            "velocityY": -1.2604599621553931,
+            "timestamp": 0.607759559159943
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.20801127587293633,
+            "velocityX": -0.026351575217374957,
+            "velocityY": -0.6302300039313168,
+            "timestamp": 0.6837295040549358
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": 2.0416042206069368e-34,
+            "velocityX": 0,
+            "velocityY": 6.898965298259951e-35,
+            "timestamp": 0.7596994489499287
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.traj
new file mode 100644
index 00000000..8e3875a1
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2980013304879616,
+            "y": 6.709118488011682,
+            "heading": -2.76564785007466,
+            "angularVelocity": -0.2059742190979395,
+            "velocityX": -0.026308687136742944,
+            "velocityY": -0.6302691420205845,
+            "timestamp": 0.07596994489499287
+        },
+        {
+            "x": 2.294004010549845,
+            "y": 6.6133557271698225,
+            "heading": -2.796958855018623,
+            "angularVelocity": -0.4121498967419566,
+            "velocityX": -0.05261712304308625,
+            "velocityY": -1.2605348203717124,
+            "timestamp": 0.15193988978998574
+        },
+        {
+            "x": 2.2880079544710785,
+            "y": 6.469712199120552,
+            "heading": -2.8439591282551087,
+            "angularVelocity": -0.618669307993449,
+            "velocityX": -0.07892668721893806,
+            "velocityY": -1.8907941587665607,
+            "timestamp": 0.2279098346849786
+        },
+        {
+            "x": 2.2800128266474267,
+            "y": 6.278188582998367,
+            "heading": -2.906683998413123,
+            "angularVelocity": -0.8256537535299532,
+            "velocityX": -0.10524066898696208,
+            "velocityY": -2.5210445576459737,
+            "timestamp": 0.3038797795799715
+        },
+        {
+            "x": 2.27001785393274,
+            "y": 6.038785706839934,
+            "heading": -2.985174219418862,
+            "angularVelocity": -1.0331746470822023,
+            "velocityX": -0.13156482775526773,
+            "velocityY": -3.151284056995696,
+            "timestamp": 0.37984972447496435
+        },
+        {
+            "x": 2.2620112641506833,
+            "y": 5.847271276792759,
+            "heading": -3.048371938278793,
+            "angularVelocity": -0.8318779083923785,
+            "velocityX": -0.10539154389440716,
+            "velocityY": -2.5209236404197743,
+            "timestamp": 0.4558196693699572
+        },
+        {
+            "x": 2.256005772930322,
+            "y": 5.703635612537123,
+            "heading": -3.0957774424626403,
+            "angularVelocity": -0.6240034035745593,
+            "velocityX": -0.07905088293353696,
+            "velocityY": -1.8906906468626767,
+            "timestamp": 0.5317896142649501
+        },
+        {
+            "x": 2.25200192771716,
+            "y": 5.607878538669833,
+            "heading": -3.127382702013982,
+            "angularVelocity": -0.4160231996354264,
+            "velocityX": -0.05270301589261432,
+            "velocityY": -1.2604599621553931,
+            "timestamp": 0.607759559159943
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.20801127587293633,
+            "velocityX": -0.026351575217374957,
+            "velocityY": -0.6302300039313168,
+            "timestamp": 0.6837295040549358
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": 2.0416042206069368e-34,
+            "velocityX": 0,
+            "velocityY": 6.898965298259951e-35,
+            "timestamp": 0.7596994489499287
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S1.1.traj
new file mode 100644
index 00000000..ce6e1914
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S1.1.traj	
@@ -0,0 +1,310 @@
+{
+    "samples": [
+        {
+            "x": 2.329045295715332,
+            "y": 5.559675216674805,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.4000279527631365,
+            "y": 5.585359375509262,
+            "heading": 3.1338545708828756,
+            "angularVelocity": -0.06457961815077011,
+            "velocityX": 0.745925597725292,
+            "velocityY": 0.2699035557060694,
+            "timestamp": 0.09516050563794304
+        },
+        {
+            "x": 2.5419932646198635,
+            "y": 5.636727692199652,
+            "heading": 3.1215638736263545,
+            "angularVelocity": -0.12915754465735657,
+            "velocityX": 1.4918511719231908,
+            "velocityY": 0.539807101129259,
+            "timestamp": 0.19032101127588608
+        },
+        {
+            "x": 2.7549412265998368,
+            "y": 5.713780164921229,
+            "heading": 3.103128182719956,
+            "angularVelocity": -0.1937325866743065,
+            "velocityX": 2.2377766968814634,
+            "velocityY": 0.8097106273770546,
+            "timestamp": 0.2854815169138291
+        },
+        {
+            "x": 3.038871822724849,
+            "y": 5.81651678795237,
+            "heading": 3.078547903585146,
+            "angularVelocity": -0.258303368295786,
+            "velocityX": 2.9837020539320758,
+            "velocityY": 1.0796140934988998,
+            "timestamp": 0.38064202255177215
+        },
+        {
+            "x": 3.3773799452774673,
+            "y": 5.939001713582426,
+            "heading": 3.0785479006839798,
+            "angularVelocity": -3.048712788248211e-8,
+            "velocityX": 3.5572333320775313,
+            "velocityY": 1.2871403405116655,
+            "timestamp": 0.4758025281897152
+        },
+        {
+            "x": 3.715888067817609,
+            "y": 6.0614866392475255,
+            "heading": 3.0785478977827805,
+            "angularVelocity": -3.0487407415354604e-8,
+            "velocityX": 3.5572333319465472,
+            "velocityY": 1.287140340879956,
+            "timestamp": 0.5709630338276582
+        },
+        {
+            "x": 4.054396190357754,
+            "y": 6.183971564912614,
+            "heading": 3.0785478948815816,
+            "angularVelocity": -3.0487407316957414e-8,
+            "velocityX": 3.5572333319465868,
+            "velocityY": 1.28714034087985,
+            "timestamp": 0.6661235394656013
+        },
+        {
+            "x": 4.392904312897899,
+            "y": 6.306456490577704,
+            "heading": 3.0785478919803824,
+            "angularVelocity": -3.048740785433711e-8,
+            "velocityX": 3.557233331946584,
+            "velocityY": 1.287140340879858,
+            "timestamp": 0.7612840451035443
+        },
+        {
+            "x": 4.7314124354380445,
+            "y": 6.428941416242792,
+            "heading": 3.0785478890791835,
+            "angularVelocity": -3.0487408354838206e-8,
+            "velocityX": 3.557233331946584,
+            "velocityY": 1.2871403408798578,
+            "timestamp": 0.8564445507414873
+        },
+        {
+            "x": 5.06992055797819,
+            "y": 6.551426341907881,
+            "heading": 3.0785478861779842,
+            "angularVelocity": -3.04874074079248e-8,
+            "velocityX": 3.557233331946584,
+            "velocityY": 1.2871403408798578,
+            "timestamp": 0.9516050563794303
+        },
+        {
+            "x": 5.408428680518335,
+            "y": 6.67391126757297,
+            "heading": 3.0785478832767854,
+            "angularVelocity": -3.048740819605906e-8,
+            "velocityX": 3.557233331946584,
+            "velocityY": 1.2871403408798576,
+            "timestamp": 1.0467655620173733
+        },
+        {
+            "x": 5.746936803058481,
+            "y": 6.796396193238057,
+            "heading": 3.078547880375586,
+            "angularVelocity": -3.0487408175601846e-8,
+            "velocityX": 3.5572333319465934,
+            "velocityY": 1.2871403408798332,
+            "timestamp": 1.1419260676553162
+        },
+        {
+            "x": 6.085444925598784,
+            "y": 6.918881118902711,
+            "heading": 3.0785478774743873,
+            "angularVelocity": -3.0487407710602e-8,
+            "velocityX": 3.55723333194824,
+            "velocityY": 1.2871403408752817,
+            "timestamp": 1.2370865732932592
+        },
+        {
+            "x": 6.4239530481377205,
+            "y": 7.041366044571132,
+            "heading": 3.078547874573188,
+            "angularVelocity": -3.0487407747049345e-8,
+            "velocityX": 3.55723333193393,
+            "velocityY": 1.287140340914726,
+            "timestamp": 1.3322470789312022
+        },
+        {
+            "x": 6.762461177860979,
+            "y": 7.163850950384819,
+            "heading": 3.078547871671963,
+            "angularVelocity": -3.0487683312908684e-8,
+            "velocityX": 3.5572334074304637,
+            "velocityY": 1.2871401322709841,
+            "timestamp": 1.4274075845691452
+        },
+        {
+            "x": 7.087934228336467,
+            "y": 7.275849077547301,
+            "heading": 3.0785482003586213,
+            "angularVelocity": 0.000003454027869296417,
+            "velocityX": 3.420253479039086,
+            "velocityY": 1.1769391767257837,
+            "timestamp": 1.5225680902070882
+        },
+        {
+            "x": 7.34242464824946,
+            "y": 7.362163048068745,
+            "heading": 3.0846933316902727,
+            "angularVelocity": 0.06457648885540108,
+            "velocityX": 2.6743281596359925,
+            "velocityY": 0.9070356440694888,
+            "timestamp": 1.6177285958450311
+        },
+        {
+            "x": 7.525932419508426,
+            "y": 7.422792858981067,
+            "heading": 3.0969834578057935,
+            "angularVelocity": 0.12915154278707255,
+            "velocityX": 1.9284026501171228,
+            "velocityY": 0.6371320802228948,
+            "timestamp": 1.7128891014829741
+        },
+        {
+            "x": 7.638457537165259,
+            "y": 7.457738509668789,
+            "heading": 3.1154188790536077,
+            "angularVelocity": 0.1937297529488966,
+            "velocityX": 1.1824770886008305,
+            "velocityY": 0.3672285099084715,
+            "timestamp": 1.8080496071209171
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 0.2583122145204678,
+            "velocityX": 0.4365515142645689,
+            "velocityY": 0.09732493820788266,
+            "timestamp": 1.90321011275886
+        },
+        {
+            "x": 7.653654383343035,
+            "y": 7.451878922022455,
+            "heading": 3.169571245622926,
+            "angularVelocity": 0.3208641076607671,
+            "velocityX": -0.28586427797403835,
+            "velocityY": -0.16407192492517308,
+            "timestamp": 1.9953713888811428
+        },
+        {
+            "x": 7.560730009307251,
+            "y": 7.412667176131766,
+            "heading": 3.204907797708199,
+            "angularVelocity": 0.3834208202531959,
+            "velocityX": -1.0082800276276813,
+            "velocityY": -0.42546878190023074,
+            "timestamp": 2.0875326650034256
+        },
+        {
+            "x": 7.4012268833190475,
+            "y": 7.349364762932986,
+            "heading": 3.2460101421492427,
+            "angularVelocity": 0.4459828050518832,
+            "velocityX": -1.7306957184018388,
+            "velocityY": -0.6868656323100164,
+            "timestamp": 2.1796939411257084
+        },
+        {
+            "x": 7.175145013744201,
+            "y": 7.261971683800118,
+            "heading": 3.292878761166739,
+            "angularVelocity": 0.5085500222039014,
+            "velocityX": -2.4531113184025837,
+            "velocityY": -0.9482624678108277,
+            "timestamp": 2.271855217247991
+        },
+        {
+            "x": 6.8824844194459525,
+            "y": 7.150487945420426,
+            "heading": 3.3455140401053294,
+            "angularVelocity": 0.5711214205450821,
+            "velocityX": -3.1755267137264047,
+            "velocityY": -1.2096592307504261,
+            "timestamp": 2.364016493370274
+        },
+        {
+            "x": 6.55464527272117,
+            "y": 7.031863403145239,
+            "heading": 3.3455140429153563,
+            "angularVelocity": 3.049000281588377e-8,
+            "velocityX": -3.557233151692005,
+            "velocityY": -1.2871408390477628,
+            "timestamp": 2.4561777694925566
+        },
+        {
+            "x": 6.226806115184277,
+            "y": 6.913238890751769,
+            "heading": 3.3455140457253494,
+            "angularVelocity": 3.0489828937045393e-8,
+            "velocityX": -3.557233269011331,
+            "velocityY": -1.287140514808609,
+            "timestamp": 2.5483390456148394
+        },
+        {
+            "x": 5.898966957623017,
+            "y": 6.79461437842809,
+            "heading": 3.345514048537748,
+            "angularVelocity": 3.0514995916000634e-8,
+            "velocityX": -3.557233269277337,
+            "velocityY": -1.287140514052891,
+            "timestamp": 2.640500321737122
+        },
+        {
+            "x": 5.632652017742471,
+            "y": 6.698251673595034,
+            "heading": 3.3685823941434916,
+            "angularVelocity": 0.2503041035574726,
+            "velocityX": -2.889662026024368,
+            "velocityY": -1.045587787926248,
+            "timestamp": 2.732661597859405
+        },
+        {
+            "x": 5.432915797517009,
+            "y": 6.625979638346594,
+            "heading": 3.3858838135535563,
+            "angularVelocity": 0.18772981600595268,
+            "velocityX": -2.167246685696753,
+            "velocityY": -0.7841909128155133,
+            "timestamp": 2.8248228739816876
+        },
+        {
+            "x": 5.299758312935604,
+            "y": 6.577798279546128,
+            "heading": 3.3974181385441695,
+            "angularVelocity": 0.1251537031151884,
+            "velocityX": -1.4448311718780615,
+            "velocityY": -0.5227939632329148,
+            "timestamp": 2.9169841501039704
+        },
+        {
+            "x": 5.233179569244385,
+            "y": 6.553707599639893,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.06257691818239948,
+            "velocityX": -0.7224156011355166,
+            "velocityY": -0.2613969871068907,
+            "timestamp": 3.009145426226253
+        },
+        {
+            "x": 5.233179569244385,
+            "y": 6.553707599639893,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0,
+            "velocityX": 1.5847972099366162e-31,
+            "velocityY": 0,
+            "timestamp": 3.101306702348536
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S1.traj
new file mode 100644
index 00000000..ce6e1914
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S1.traj	
@@ -0,0 +1,310 @@
+{
+    "samples": [
+        {
+            "x": 2.329045295715332,
+            "y": 5.559675216674805,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.4000279527631365,
+            "y": 5.585359375509262,
+            "heading": 3.1338545708828756,
+            "angularVelocity": -0.06457961815077011,
+            "velocityX": 0.745925597725292,
+            "velocityY": 0.2699035557060694,
+            "timestamp": 0.09516050563794304
+        },
+        {
+            "x": 2.5419932646198635,
+            "y": 5.636727692199652,
+            "heading": 3.1215638736263545,
+            "angularVelocity": -0.12915754465735657,
+            "velocityX": 1.4918511719231908,
+            "velocityY": 0.539807101129259,
+            "timestamp": 0.19032101127588608
+        },
+        {
+            "x": 2.7549412265998368,
+            "y": 5.713780164921229,
+            "heading": 3.103128182719956,
+            "angularVelocity": -0.1937325866743065,
+            "velocityX": 2.2377766968814634,
+            "velocityY": 0.8097106273770546,
+            "timestamp": 0.2854815169138291
+        },
+        {
+            "x": 3.038871822724849,
+            "y": 5.81651678795237,
+            "heading": 3.078547903585146,
+            "angularVelocity": -0.258303368295786,
+            "velocityX": 2.9837020539320758,
+            "velocityY": 1.0796140934988998,
+            "timestamp": 0.38064202255177215
+        },
+        {
+            "x": 3.3773799452774673,
+            "y": 5.939001713582426,
+            "heading": 3.0785479006839798,
+            "angularVelocity": -3.048712788248211e-8,
+            "velocityX": 3.5572333320775313,
+            "velocityY": 1.2871403405116655,
+            "timestamp": 0.4758025281897152
+        },
+        {
+            "x": 3.715888067817609,
+            "y": 6.0614866392475255,
+            "heading": 3.0785478977827805,
+            "angularVelocity": -3.0487407415354604e-8,
+            "velocityX": 3.5572333319465472,
+            "velocityY": 1.287140340879956,
+            "timestamp": 0.5709630338276582
+        },
+        {
+            "x": 4.054396190357754,
+            "y": 6.183971564912614,
+            "heading": 3.0785478948815816,
+            "angularVelocity": -3.0487407316957414e-8,
+            "velocityX": 3.5572333319465868,
+            "velocityY": 1.28714034087985,
+            "timestamp": 0.6661235394656013
+        },
+        {
+            "x": 4.392904312897899,
+            "y": 6.306456490577704,
+            "heading": 3.0785478919803824,
+            "angularVelocity": -3.048740785433711e-8,
+            "velocityX": 3.557233331946584,
+            "velocityY": 1.287140340879858,
+            "timestamp": 0.7612840451035443
+        },
+        {
+            "x": 4.7314124354380445,
+            "y": 6.428941416242792,
+            "heading": 3.0785478890791835,
+            "angularVelocity": -3.0487408354838206e-8,
+            "velocityX": 3.557233331946584,
+            "velocityY": 1.2871403408798578,
+            "timestamp": 0.8564445507414873
+        },
+        {
+            "x": 5.06992055797819,
+            "y": 6.551426341907881,
+            "heading": 3.0785478861779842,
+            "angularVelocity": -3.04874074079248e-8,
+            "velocityX": 3.557233331946584,
+            "velocityY": 1.2871403408798578,
+            "timestamp": 0.9516050563794303
+        },
+        {
+            "x": 5.408428680518335,
+            "y": 6.67391126757297,
+            "heading": 3.0785478832767854,
+            "angularVelocity": -3.048740819605906e-8,
+            "velocityX": 3.557233331946584,
+            "velocityY": 1.2871403408798576,
+            "timestamp": 1.0467655620173733
+        },
+        {
+            "x": 5.746936803058481,
+            "y": 6.796396193238057,
+            "heading": 3.078547880375586,
+            "angularVelocity": -3.0487408175601846e-8,
+            "velocityX": 3.5572333319465934,
+            "velocityY": 1.2871403408798332,
+            "timestamp": 1.1419260676553162
+        },
+        {
+            "x": 6.085444925598784,
+            "y": 6.918881118902711,
+            "heading": 3.0785478774743873,
+            "angularVelocity": -3.0487407710602e-8,
+            "velocityX": 3.55723333194824,
+            "velocityY": 1.2871403408752817,
+            "timestamp": 1.2370865732932592
+        },
+        {
+            "x": 6.4239530481377205,
+            "y": 7.041366044571132,
+            "heading": 3.078547874573188,
+            "angularVelocity": -3.0487407747049345e-8,
+            "velocityX": 3.55723333193393,
+            "velocityY": 1.287140340914726,
+            "timestamp": 1.3322470789312022
+        },
+        {
+            "x": 6.762461177860979,
+            "y": 7.163850950384819,
+            "heading": 3.078547871671963,
+            "angularVelocity": -3.0487683312908684e-8,
+            "velocityX": 3.5572334074304637,
+            "velocityY": 1.2871401322709841,
+            "timestamp": 1.4274075845691452
+        },
+        {
+            "x": 7.087934228336467,
+            "y": 7.275849077547301,
+            "heading": 3.0785482003586213,
+            "angularVelocity": 0.000003454027869296417,
+            "velocityX": 3.420253479039086,
+            "velocityY": 1.1769391767257837,
+            "timestamp": 1.5225680902070882
+        },
+        {
+            "x": 7.34242464824946,
+            "y": 7.362163048068745,
+            "heading": 3.0846933316902727,
+            "angularVelocity": 0.06457648885540108,
+            "velocityX": 2.6743281596359925,
+            "velocityY": 0.9070356440694888,
+            "timestamp": 1.6177285958450311
+        },
+        {
+            "x": 7.525932419508426,
+            "y": 7.422792858981067,
+            "heading": 3.0969834578057935,
+            "angularVelocity": 0.12915154278707255,
+            "velocityX": 1.9284026501171228,
+            "velocityY": 0.6371320802228948,
+            "timestamp": 1.7128891014829741
+        },
+        {
+            "x": 7.638457537165259,
+            "y": 7.457738509668789,
+            "heading": 3.1154188790536077,
+            "angularVelocity": 0.1937297529488966,
+            "velocityX": 1.1824770886008305,
+            "velocityY": 0.3672285099084715,
+            "timestamp": 1.8080496071209171
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 0.2583122145204678,
+            "velocityX": 0.4365515142645689,
+            "velocityY": 0.09732493820788266,
+            "timestamp": 1.90321011275886
+        },
+        {
+            "x": 7.653654383343035,
+            "y": 7.451878922022455,
+            "heading": 3.169571245622926,
+            "angularVelocity": 0.3208641076607671,
+            "velocityX": -0.28586427797403835,
+            "velocityY": -0.16407192492517308,
+            "timestamp": 1.9953713888811428
+        },
+        {
+            "x": 7.560730009307251,
+            "y": 7.412667176131766,
+            "heading": 3.204907797708199,
+            "angularVelocity": 0.3834208202531959,
+            "velocityX": -1.0082800276276813,
+            "velocityY": -0.42546878190023074,
+            "timestamp": 2.0875326650034256
+        },
+        {
+            "x": 7.4012268833190475,
+            "y": 7.349364762932986,
+            "heading": 3.2460101421492427,
+            "angularVelocity": 0.4459828050518832,
+            "velocityX": -1.7306957184018388,
+            "velocityY": -0.6868656323100164,
+            "timestamp": 2.1796939411257084
+        },
+        {
+            "x": 7.175145013744201,
+            "y": 7.261971683800118,
+            "heading": 3.292878761166739,
+            "angularVelocity": 0.5085500222039014,
+            "velocityX": -2.4531113184025837,
+            "velocityY": -0.9482624678108277,
+            "timestamp": 2.271855217247991
+        },
+        {
+            "x": 6.8824844194459525,
+            "y": 7.150487945420426,
+            "heading": 3.3455140401053294,
+            "angularVelocity": 0.5711214205450821,
+            "velocityX": -3.1755267137264047,
+            "velocityY": -1.2096592307504261,
+            "timestamp": 2.364016493370274
+        },
+        {
+            "x": 6.55464527272117,
+            "y": 7.031863403145239,
+            "heading": 3.3455140429153563,
+            "angularVelocity": 3.049000281588377e-8,
+            "velocityX": -3.557233151692005,
+            "velocityY": -1.2871408390477628,
+            "timestamp": 2.4561777694925566
+        },
+        {
+            "x": 6.226806115184277,
+            "y": 6.913238890751769,
+            "heading": 3.3455140457253494,
+            "angularVelocity": 3.0489828937045393e-8,
+            "velocityX": -3.557233269011331,
+            "velocityY": -1.287140514808609,
+            "timestamp": 2.5483390456148394
+        },
+        {
+            "x": 5.898966957623017,
+            "y": 6.79461437842809,
+            "heading": 3.345514048537748,
+            "angularVelocity": 3.0514995916000634e-8,
+            "velocityX": -3.557233269277337,
+            "velocityY": -1.287140514052891,
+            "timestamp": 2.640500321737122
+        },
+        {
+            "x": 5.632652017742471,
+            "y": 6.698251673595034,
+            "heading": 3.3685823941434916,
+            "angularVelocity": 0.2503041035574726,
+            "velocityX": -2.889662026024368,
+            "velocityY": -1.045587787926248,
+            "timestamp": 2.732661597859405
+        },
+        {
+            "x": 5.432915797517009,
+            "y": 6.625979638346594,
+            "heading": 3.3858838135535563,
+            "angularVelocity": 0.18772981600595268,
+            "velocityX": -2.167246685696753,
+            "velocityY": -0.7841909128155133,
+            "timestamp": 2.8248228739816876
+        },
+        {
+            "x": 5.299758312935604,
+            "y": 6.577798279546128,
+            "heading": 3.3974181385441695,
+            "angularVelocity": 0.1251537031151884,
+            "velocityX": -1.4448311718780615,
+            "velocityY": -0.5227939632329148,
+            "timestamp": 2.9169841501039704
+        },
+        {
+            "x": 5.233179569244385,
+            "y": 6.553707599639893,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.06257691818239948,
+            "velocityX": -0.7224156011355166,
+            "velocityY": -0.2613969871068907,
+            "timestamp": 3.009145426226253
+        },
+        {
+            "x": 5.233179569244385,
+            "y": 6.553707599639893,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0,
+            "velocityX": 1.5847972099366162e-31,
+            "velocityY": 0,
+            "timestamp": 3.101306702348536
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S2.1.traj
new file mode 100644
index 00000000..42ce41bc
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S2.1.traj	
@@ -0,0 +1,328 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3199317069050815,
+            "y": 5.589221584324863,
+            "heading": 3.14,
+            "angularVelocity": -4.40794531804863e-17,
+            "velocityX": 0.7334971919039711,
+            "velocityY": 0.3064983109073157,
+            "timestamp": 0.09534011537734256
+        },
+        {
+            "x": 2.4597951178135684,
+            "y": 5.647664751762101,
+            "heading": 3.14,
+            "angularVelocity": -2.0051811551543008e-16,
+            "velocityX": 1.4669943533729506,
+            "velocityY": 0.6129966090971003,
+            "timestamp": 0.19068023075468513
+        },
+        {
+            "x": 2.6695902269221126,
+            "y": 5.73532949988673,
+            "heading": 3.14,
+            "angularVelocity": -1.6298734307861892e-16,
+            "velocityX": 2.200491453971973,
+            "velocityY": 0.9194948818518327,
+            "timestamp": 0.2860203461320277
+        },
+        {
+            "x": 2.9493170168206886,
+            "y": 5.852215821423819,
+            "heading": 3.14,
+            "angularVelocity": -2.718821323245932e-16,
+            "velocityX": 2.933988371961343,
+            "velocityY": 1.2259930783015016,
+            "timestamp": 0.38136046150937025
+        },
+        {
+            "x": 3.2820984406189933,
+            "y": 5.991271492558365,
+            "heading": 3.14,
+            "angularVelocity": -1.791056538237388e-18,
+            "velocityX": 3.4904659227776635,
+            "velocityY": 1.45852216125588,
+            "timestamp": 0.4767005768867128
+        },
+        {
+            "x": 3.6148798644173543,
+            "y": 6.130327163692935,
+            "heading": 3.14,
+            "angularVelocity": 2.7598941425139603e-16,
+            "velocityX": 3.4904659227782475,
+            "velocityY": 1.4585221612561248,
+            "timestamp": 0.5720406922640554
+        },
+        {
+            "x": 3.947661288215715,
+            "y": 6.269382834827505,
+            "heading": 3.14,
+            "angularVelocity": -1.359472794095135e-16,
+            "velocityX": 3.4904659227782475,
+            "velocityY": 1.4585221612561248,
+            "timestamp": 0.667380807641398
+        },
+        {
+            "x": 4.280442712014075,
+            "y": 6.408438505962075,
+            "heading": 3.14,
+            "angularVelocity": -3.500332184235888e-16,
+            "velocityX": 3.4904659227782475,
+            "velocityY": 1.4585221612561245,
+            "timestamp": 0.7627209230187405
+        },
+        {
+            "x": 4.613224135812436,
+            "y": 6.547494177096645,
+            "heading": 3.14,
+            "angularVelocity": -3.2849392470850763e-16,
+            "velocityX": 3.4904659227782475,
+            "velocityY": 1.4585221612561245,
+            "timestamp": 0.858061038396083
+        },
+        {
+            "x": 4.9460055596107955,
+            "y": 6.686549848231215,
+            "heading": 3.14,
+            "angularVelocity": -3.264382961483678e-16,
+            "velocityX": 3.4904659227782475,
+            "velocityY": 1.4585221612561245,
+            "timestamp": 0.9534011537734256
+        },
+        {
+            "x": 5.278786983409157,
+            "y": 6.825605519365785,
+            "heading": 3.14,
+            "angularVelocity": -2.8870332355952577e-16,
+            "velocityX": 3.4904659227782475,
+            "velocityY": 1.4585221612561245,
+            "timestamp": 1.0487412691507683
+        },
+        {
+            "x": 5.611568407207518,
+            "y": 6.9646611905003555,
+            "heading": 3.14,
+            "angularVelocity": -2.714509339961268e-16,
+            "velocityX": 3.4904659227782475,
+            "velocityY": 1.4585221612561245,
+            "timestamp": 1.1440813845281108
+        },
+        {
+            "x": 5.944349831005879,
+            "y": 7.1037168616349256,
+            "heading": 3.14,
+            "angularVelocity": 3.236789006032357e-16,
+            "velocityX": 3.4904659227782475,
+            "velocityY": 1.4585221612561248,
+            "timestamp": 1.2394214999054534
+        },
+        {
+            "x": 6.2771312548052975,
+            "y": 7.242772532766961,
+            "heading": 3.14,
+            "angularVelocity": 1.392859578179056e-16,
+            "velocityX": 3.4904659227893537,
+            "velocityY": 1.458522161229545,
+            "timestamp": 1.334761615282796
+        },
+        {
+            "x": 6.609912972036511,
+            "y": 7.381827501669921,
+            "heading": 3.14,
+            "angularVelocity": 7.280552315335093e-17,
+            "velocityX": 3.4904690005262737,
+            "velocityY": 1.4585147957142848,
+            "timestamp": 1.4301017306601385
+        },
+        {
+            "x": 6.961412726724582,
+            "y": 7.462622346034388,
+            "heading": 3.14,
+            "angularVelocity": -1.0172483638053557e-16,
+            "velocityX": 3.6867980838588856,
+            "velocityY": 0.8474380804417183,
+            "timestamp": 1.525441846037481
+        },
+        {
+            "x": 7.244997116389008,
+            "y": 7.50977433975784,
+            "heading": 3.14,
+            "angularVelocity": -1.1360407734774875e-16,
+            "velocityX": 2.974449826728655,
+            "velocityY": 0.4945661491684764,
+            "timestamp": 1.6207819614148236
+        },
+        {
+            "x": 7.459465375503974,
+            "y": 7.525824834495841,
+            "heading": 3.14,
+            "angularVelocity": -6.302038440751775e-17,
+            "velocityX": 2.249507023000035,
+            "velocityY": 0.16834985645314604,
+            "timestamp": 1.716122076792166
+        },
+        {
+            "x": 7.6045126721499186,
+            "y": 7.511460301300605,
+            "heading": 3.14,
+            "angularVelocity": 8.943638136140749e-17,
+            "velocityX": 1.5213669091113222,
+            "velocityY": -0.15066620318617746,
+            "timestamp": 1.8114621921695087
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 1.2339640787414218e-16,
+            "velocityX": 0.7917687906220042,
+            "velocityY": -0.4663336217356026,
+            "timestamp": 1.9068023075468512
+        },
+        {
+            "x": 7.680079036283902,
+            "y": 7.383775130350334,
+            "heading": 3.14,
+            "angularVelocity": 3.1298666285147066e-16,
+            "velocityX": 0.0007655130286299905,
+            "velocityY": -0.8060819521991853,
+            "timestamp": 2.010048471814154
+        },
+        {
+            "x": 7.598307382340287,
+            "y": 7.265900859151241,
+            "heading": 3.14,
+            "angularVelocity": 4.0403721611171334e-16,
+            "velocityX": -0.792006700916374,
+            "velocityY": -1.141681843927082,
+            "timestamp": 2.113294636081456
+        },
+        {
+            "x": 7.434354633787184,
+            "y": 7.114168304333743,
+            "heading": 3.14,
+            "angularVelocity": 4.565397791635157e-16,
+            "velocityX": -1.5879790761875838,
+            "velocityY": -1.4696192918573159,
+            "timestamp": 2.2165408003487586
+        },
+        {
+            "x": 7.187443357098086,
+            "y": 6.930529696196354,
+            "heading": 3.14,
+            "angularVelocity": 3.88515705379733e-16,
+            "velocityX": -2.3914813537270896,
+            "velocityY": -1.7786482378362456,
+            "timestamp": 2.319786964616061
+        },
+        {
+            "x": 6.853793099378443,
+            "y": 6.7274887650449,
+            "heading": 3.14,
+            "angularVelocity": -3.674797460353125e-16,
+            "velocityX": -3.2315995474255925,
+            "velocityY": -1.9665711805601358,
+            "timestamp": 2.4230331288833633
+        },
+        {
+            "x": 6.49770423144866,
+            "y": 6.567024283806904,
+            "heading": 3.14,
+            "angularVelocity": 2.249424709718662e-16,
+            "velocityX": -3.448930722577495,
+            "velocityY": -1.5541931497092332,
+            "timestamp": 2.5262792931506657
+        },
+        {
+            "x": 6.1416152766915,
+            "y": 6.406559995248834,
+            "heading": 3.14,
+            "angularVelocity": 2.526804739134875e-16,
+            "velocityX": -3.4489315635518585,
+            "velocityY": -1.5541912834905027,
+            "timestamp": 2.629525457417968
+        },
+        {
+            "x": 5.785526321934093,
+            "y": 6.246095706691313,
+            "heading": 3.14,
+            "angularVelocity": 2.2036218085246878e-16,
+            "velocityX": -3.4489315635542535,
+            "velocityY": -1.5541912834851872,
+            "timestamp": 2.7327716216852704
+        },
+        {
+            "x": 5.429437367176686,
+            "y": 6.085631418133792,
+            "heading": 3.14,
+            "angularVelocity": 7.127301298549232e-19,
+            "velocityX": -3.4489315635542535,
+            "velocityY": -1.5541912834851876,
+            "timestamp": 2.836017785952573
+        },
+        {
+            "x": 5.07334841241929,
+            "y": 5.925167129576275,
+            "heading": 3.14,
+            "angularVelocity": 1.0539129254381463e-16,
+            "velocityX": -3.4489315635541473,
+            "velocityY": -1.5541912834851401,
+            "timestamp": 2.939263950219875
+        },
+        {
+            "x": 4.749209064120322,
+            "y": 5.7791002852572,
+            "heading": 3.14,
+            "angularVelocity": 8.727825696429895e-16,
+            "velocityX": -3.1394807797389603,
+            "velocityY": -1.4147435437980136,
+            "timestamp": 3.0425101144871776
+        },
+        {
+            "x": 4.506104536371044,
+            "y": 5.669550144571212,
+            "heading": 3.14,
+            "angularVelocity": 4.949097028202336e-16,
+            "velocityX": -2.3546107448590976,
+            "velocityY": -1.061057729974005,
+            "timestamp": 3.14575627875448
+        },
+        {
+            "x": 4.3440348464149885,
+            "y": 5.596516715288762,
+            "heading": 3.14,
+            "angularVelocity": 3.5626049667024716e-16,
+            "velocityX": -1.5697405429654518,
+            "velocityY": -0.7073718408886114,
+            "timestamp": 3.2490024430217823
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 2.245803154155974e-16,
+            "velocityX": -0.7848702854005415,
+            "velocityY": -0.35368592671608795,
+            "timestamp": 3.3522486072890847
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -4.241491761729664e-33,
+            "velocityY": 0,
+            "timestamp": 3.455494771556387
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S2.traj
new file mode 100644
index 00000000..42ce41bc
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S2.traj	
@@ -0,0 +1,328 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3199317069050815,
+            "y": 5.589221584324863,
+            "heading": 3.14,
+            "angularVelocity": -4.40794531804863e-17,
+            "velocityX": 0.7334971919039711,
+            "velocityY": 0.3064983109073157,
+            "timestamp": 0.09534011537734256
+        },
+        {
+            "x": 2.4597951178135684,
+            "y": 5.647664751762101,
+            "heading": 3.14,
+            "angularVelocity": -2.0051811551543008e-16,
+            "velocityX": 1.4669943533729506,
+            "velocityY": 0.6129966090971003,
+            "timestamp": 0.19068023075468513
+        },
+        {
+            "x": 2.6695902269221126,
+            "y": 5.73532949988673,
+            "heading": 3.14,
+            "angularVelocity": -1.6298734307861892e-16,
+            "velocityX": 2.200491453971973,
+            "velocityY": 0.9194948818518327,
+            "timestamp": 0.2860203461320277
+        },
+        {
+            "x": 2.9493170168206886,
+            "y": 5.852215821423819,
+            "heading": 3.14,
+            "angularVelocity": -2.718821323245932e-16,
+            "velocityX": 2.933988371961343,
+            "velocityY": 1.2259930783015016,
+            "timestamp": 0.38136046150937025
+        },
+        {
+            "x": 3.2820984406189933,
+            "y": 5.991271492558365,
+            "heading": 3.14,
+            "angularVelocity": -1.791056538237388e-18,
+            "velocityX": 3.4904659227776635,
+            "velocityY": 1.45852216125588,
+            "timestamp": 0.4767005768867128
+        },
+        {
+            "x": 3.6148798644173543,
+            "y": 6.130327163692935,
+            "heading": 3.14,
+            "angularVelocity": 2.7598941425139603e-16,
+            "velocityX": 3.4904659227782475,
+            "velocityY": 1.4585221612561248,
+            "timestamp": 0.5720406922640554
+        },
+        {
+            "x": 3.947661288215715,
+            "y": 6.269382834827505,
+            "heading": 3.14,
+            "angularVelocity": -1.359472794095135e-16,
+            "velocityX": 3.4904659227782475,
+            "velocityY": 1.4585221612561248,
+            "timestamp": 0.667380807641398
+        },
+        {
+            "x": 4.280442712014075,
+            "y": 6.408438505962075,
+            "heading": 3.14,
+            "angularVelocity": -3.500332184235888e-16,
+            "velocityX": 3.4904659227782475,
+            "velocityY": 1.4585221612561245,
+            "timestamp": 0.7627209230187405
+        },
+        {
+            "x": 4.613224135812436,
+            "y": 6.547494177096645,
+            "heading": 3.14,
+            "angularVelocity": -3.2849392470850763e-16,
+            "velocityX": 3.4904659227782475,
+            "velocityY": 1.4585221612561245,
+            "timestamp": 0.858061038396083
+        },
+        {
+            "x": 4.9460055596107955,
+            "y": 6.686549848231215,
+            "heading": 3.14,
+            "angularVelocity": -3.264382961483678e-16,
+            "velocityX": 3.4904659227782475,
+            "velocityY": 1.4585221612561245,
+            "timestamp": 0.9534011537734256
+        },
+        {
+            "x": 5.278786983409157,
+            "y": 6.825605519365785,
+            "heading": 3.14,
+            "angularVelocity": -2.8870332355952577e-16,
+            "velocityX": 3.4904659227782475,
+            "velocityY": 1.4585221612561245,
+            "timestamp": 1.0487412691507683
+        },
+        {
+            "x": 5.611568407207518,
+            "y": 6.9646611905003555,
+            "heading": 3.14,
+            "angularVelocity": -2.714509339961268e-16,
+            "velocityX": 3.4904659227782475,
+            "velocityY": 1.4585221612561245,
+            "timestamp": 1.1440813845281108
+        },
+        {
+            "x": 5.944349831005879,
+            "y": 7.1037168616349256,
+            "heading": 3.14,
+            "angularVelocity": 3.236789006032357e-16,
+            "velocityX": 3.4904659227782475,
+            "velocityY": 1.4585221612561248,
+            "timestamp": 1.2394214999054534
+        },
+        {
+            "x": 6.2771312548052975,
+            "y": 7.242772532766961,
+            "heading": 3.14,
+            "angularVelocity": 1.392859578179056e-16,
+            "velocityX": 3.4904659227893537,
+            "velocityY": 1.458522161229545,
+            "timestamp": 1.334761615282796
+        },
+        {
+            "x": 6.609912972036511,
+            "y": 7.381827501669921,
+            "heading": 3.14,
+            "angularVelocity": 7.280552315335093e-17,
+            "velocityX": 3.4904690005262737,
+            "velocityY": 1.4585147957142848,
+            "timestamp": 1.4301017306601385
+        },
+        {
+            "x": 6.961412726724582,
+            "y": 7.462622346034388,
+            "heading": 3.14,
+            "angularVelocity": -1.0172483638053557e-16,
+            "velocityX": 3.6867980838588856,
+            "velocityY": 0.8474380804417183,
+            "timestamp": 1.525441846037481
+        },
+        {
+            "x": 7.244997116389008,
+            "y": 7.50977433975784,
+            "heading": 3.14,
+            "angularVelocity": -1.1360407734774875e-16,
+            "velocityX": 2.974449826728655,
+            "velocityY": 0.4945661491684764,
+            "timestamp": 1.6207819614148236
+        },
+        {
+            "x": 7.459465375503974,
+            "y": 7.525824834495841,
+            "heading": 3.14,
+            "angularVelocity": -6.302038440751775e-17,
+            "velocityX": 2.249507023000035,
+            "velocityY": 0.16834985645314604,
+            "timestamp": 1.716122076792166
+        },
+        {
+            "x": 7.6045126721499186,
+            "y": 7.511460301300605,
+            "heading": 3.14,
+            "angularVelocity": 8.943638136140749e-17,
+            "velocityX": 1.5213669091113222,
+            "velocityY": -0.15066620318617746,
+            "timestamp": 1.8114621921695087
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 1.2339640787414218e-16,
+            "velocityX": 0.7917687906220042,
+            "velocityY": -0.4663336217356026,
+            "timestamp": 1.9068023075468512
+        },
+        {
+            "x": 7.680079036283902,
+            "y": 7.383775130350334,
+            "heading": 3.14,
+            "angularVelocity": 3.1298666285147066e-16,
+            "velocityX": 0.0007655130286299905,
+            "velocityY": -0.8060819521991853,
+            "timestamp": 2.010048471814154
+        },
+        {
+            "x": 7.598307382340287,
+            "y": 7.265900859151241,
+            "heading": 3.14,
+            "angularVelocity": 4.0403721611171334e-16,
+            "velocityX": -0.792006700916374,
+            "velocityY": -1.141681843927082,
+            "timestamp": 2.113294636081456
+        },
+        {
+            "x": 7.434354633787184,
+            "y": 7.114168304333743,
+            "heading": 3.14,
+            "angularVelocity": 4.565397791635157e-16,
+            "velocityX": -1.5879790761875838,
+            "velocityY": -1.4696192918573159,
+            "timestamp": 2.2165408003487586
+        },
+        {
+            "x": 7.187443357098086,
+            "y": 6.930529696196354,
+            "heading": 3.14,
+            "angularVelocity": 3.88515705379733e-16,
+            "velocityX": -2.3914813537270896,
+            "velocityY": -1.7786482378362456,
+            "timestamp": 2.319786964616061
+        },
+        {
+            "x": 6.853793099378443,
+            "y": 6.7274887650449,
+            "heading": 3.14,
+            "angularVelocity": -3.674797460353125e-16,
+            "velocityX": -3.2315995474255925,
+            "velocityY": -1.9665711805601358,
+            "timestamp": 2.4230331288833633
+        },
+        {
+            "x": 6.49770423144866,
+            "y": 6.567024283806904,
+            "heading": 3.14,
+            "angularVelocity": 2.249424709718662e-16,
+            "velocityX": -3.448930722577495,
+            "velocityY": -1.5541931497092332,
+            "timestamp": 2.5262792931506657
+        },
+        {
+            "x": 6.1416152766915,
+            "y": 6.406559995248834,
+            "heading": 3.14,
+            "angularVelocity": 2.526804739134875e-16,
+            "velocityX": -3.4489315635518585,
+            "velocityY": -1.5541912834905027,
+            "timestamp": 2.629525457417968
+        },
+        {
+            "x": 5.785526321934093,
+            "y": 6.246095706691313,
+            "heading": 3.14,
+            "angularVelocity": 2.2036218085246878e-16,
+            "velocityX": -3.4489315635542535,
+            "velocityY": -1.5541912834851872,
+            "timestamp": 2.7327716216852704
+        },
+        {
+            "x": 5.429437367176686,
+            "y": 6.085631418133792,
+            "heading": 3.14,
+            "angularVelocity": 7.127301298549232e-19,
+            "velocityX": -3.4489315635542535,
+            "velocityY": -1.5541912834851876,
+            "timestamp": 2.836017785952573
+        },
+        {
+            "x": 5.07334841241929,
+            "y": 5.925167129576275,
+            "heading": 3.14,
+            "angularVelocity": 1.0539129254381463e-16,
+            "velocityX": -3.4489315635541473,
+            "velocityY": -1.5541912834851401,
+            "timestamp": 2.939263950219875
+        },
+        {
+            "x": 4.749209064120322,
+            "y": 5.7791002852572,
+            "heading": 3.14,
+            "angularVelocity": 8.727825696429895e-16,
+            "velocityX": -3.1394807797389603,
+            "velocityY": -1.4147435437980136,
+            "timestamp": 3.0425101144871776
+        },
+        {
+            "x": 4.506104536371044,
+            "y": 5.669550144571212,
+            "heading": 3.14,
+            "angularVelocity": 4.949097028202336e-16,
+            "velocityX": -2.3546107448590976,
+            "velocityY": -1.061057729974005,
+            "timestamp": 3.14575627875448
+        },
+        {
+            "x": 4.3440348464149885,
+            "y": 5.596516715288762,
+            "heading": 3.14,
+            "angularVelocity": 3.5626049667024716e-16,
+            "velocityX": -1.5697405429654518,
+            "velocityY": -0.7073718408886114,
+            "timestamp": 3.2490024430217823
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 2.245803154155974e-16,
+            "velocityX": -0.7848702854005415,
+            "velocityY": -0.35368592671608795,
+            "timestamp": 3.3522486072890847
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -4.241491761729664e-33,
+            "velocityY": 0,
+            "timestamp": 3.455494771556387
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S1.1.traj
new file mode 100644
index 00000000..e9a814a4
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S1.1.traj	
@@ -0,0 +1,301 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.327391358066629,
+            "y": 5.555067935622319,
+            "heading": 3.133430176934097,
+            "angularVelocity": -0.0681146690773455,
+            "velocityX": 0.8023787994403143,
+            "velocityY": -0.05113470021702577,
+            "timestamp": 0.09645239644992105
+        },
+        {
+            "x": 2.4821740708193567,
+            "y": 5.545203805497392,
+            "heading": 3.12029049964152,
+            "angularVelocity": -0.13622966122359603,
+            "velocityX": 1.6047575638319433,
+            "velocityY": -0.10226941463344474,
+            "timestamp": 0.1929047928998421
+        },
+        {
+            "x": 2.7143481316892246,
+            "y": 5.5304076076409565,
+            "heading": 3.1005809392040384,
+            "angularVelocity": -0.20434495318853443,
+            "velocityX": 2.4071362601178543,
+            "velocityY": -0.15340414962232987,
+            "timestamp": 0.2893571893497632
+        },
+        {
+            "x": 3.023913521714313,
+            "y": 5.510679340033408,
+            "heading": 3.0743015138082646,
+            "angularVelocity": -0.2724600565981605,
+            "velocityX": 3.2095147598102214,
+            "velocityY": -0.2045389055500683,
+            "timestamp": 0.3858095857996842
+        },
+        {
+            "x": 3.388048508086598,
+            "y": 5.487473394252625,
+            "heading": 3.0743015107885725,
+            "angularVelocity": -3.130758650216746e-8,
+            "velocityX": 3.7752818983750913,
+            "velocityY": -0.24059480774884917,
+            "timestamp": 0.48226198224960526
+        },
+        {
+            "x": 3.752183494458947,
+            "y": 5.464267448472115,
+            "heading": 3.0743015077689044,
+            "angularVelocity": -3.130734274282633e-8,
+            "velocityX": 3.7752818983757597,
+            "velocityY": -0.24059480774599656,
+            "timestamp": 0.5787143786995264
+        },
+        {
+            "x": 4.116318480831297,
+            "y": 5.441061502691605,
+            "heading": 3.0743015047492364,
+            "angularVelocity": -3.1307342782181776e-8,
+            "velocityX": 3.77528189837576,
+            "velocityY": -0.24059480774599656,
+            "timestamp": 0.6751667751494475
+        },
+        {
+            "x": 4.480453467203646,
+            "y": 5.417855556911095,
+            "heading": 3.0743015017295683,
+            "angularVelocity": -3.130734248880116e-8,
+            "velocityX": 3.7752818983757597,
+            "velocityY": -0.2405948077459966,
+            "timestamp": 0.7716191715993685
+        },
+        {
+            "x": 4.844588453575996,
+            "y": 5.3946496111305855,
+            "heading": 3.0743014987098998,
+            "angularVelocity": -3.130734337287564e-8,
+            "velocityX": 3.77528189837576,
+            "velocityY": -0.24059480774599656,
+            "timestamp": 0.8680715680492896
+        },
+        {
+            "x": 5.208723439948346,
+            "y": 5.371443665350077,
+            "heading": 3.0743014956902317,
+            "angularVelocity": -3.130734341157956e-8,
+            "velocityX": 3.7752818983757606,
+            "velocityY": -0.24059480774599654,
+            "timestamp": 0.9645239644992107
+        },
+        {
+            "x": 5.572858426320695,
+            "y": 5.348237719569567,
+            "heading": 3.0743014926705636,
+            "angularVelocity": -3.130734268712419e-8,
+            "velocityX": 3.7752818983757597,
+            "velocityY": -0.24059480774599656,
+            "timestamp": 1.0609763609491318
+        },
+        {
+            "x": 5.936993412693067,
+            "y": 5.325031773789395,
+            "heading": 3.0743014896508956,
+            "angularVelocity": -3.130734257132029e-8,
+            "velocityX": 3.7752818983759835,
+            "velocityY": -0.24059480774249117,
+            "timestamp": 1.157428757399053
+        },
+        {
+            "x": 6.301128405222732,
+            "y": 5.301825924626314,
+            "heading": 3.0743014866312275,
+            "angularVelocity": -3.1307343059822334e-8,
+            "velocityX": 3.7752819622136293,
+            "velocityY": -0.24059380603497976,
+            "timestamp": 1.253881153848974
+        },
+        {
+            "x": 6.666000662308113,
+            "y": 5.302845713467377,
+            "heading": 3.0743014835545788,
+            "angularVelocity": -3.1898104709200575e-8,
+            "velocityX": 3.7829257801264253,
+            "velocityY": 0.010572975670877716,
+            "timestamp": 1.3503335502988951
+        },
+        {
+            "x": 7.022411728114523,
+            "y": 5.380972301482392,
+            "heading": 3.0743014635013113,
+            "angularVelocity": -2.0790844162753672e-7,
+            "velocityX": 3.6952017671376614,
+            "velocityY": 0.8100015229334367,
+            "timestamp": 1.4467859467488162
+        },
+        {
+            "x": 7.302470785429282,
+            "y": 5.472668060315495,
+            "heading": 3.0808623231023553,
+            "angularVelocity": 0.06802173758793187,
+            "velocityX": 2.903598745316483,
+            "velocityY": 0.9506840908894665,
+            "timestamp": 1.5432383431987373
+        },
+        {
+            "x": 7.505508952457786,
+            "y": 5.5733927696291925,
+            "heading": 3.0939992329731627,
+            "angularVelocity": 0.1362009691239587,
+            "velocityX": 2.1050608849716217,
+            "velocityY": 1.0442945226974727,
+            "timestamp": 1.6396907396486584
+        },
+        {
+            "x": 7.631373643549882,
+            "y": 5.681732940772809,
+            "heading": 3.113712106449711,
+            "angularVelocity": 0.20437930214396433,
+            "velocityX": 1.3049410457877573,
+            "velocityY": 1.1232501745031058,
+            "timestamp": 1.7361431360985795
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 0.27254785280465055,
+            "velocityX": 0.5041487639486981,
+            "velocityY": 1.1950668253954515,
+            "timestamp": 1.8325955325485006
+        },
+        {
+            "x": 7.657241486683139,
+            "y": 5.911359552858986,
+            "heading": 3.1705946317800286,
+            "angularVelocity": 0.3367496839490721,
+            "velocityX": -0.25049891829736254,
+            "velocityY": 1.2587353088184159,
+            "timestamp": 1.923448273436763
+        },
+        {
+            "x": 7.565890794694817,
+            "y": 6.031156159947816,
+            "heading": 3.2067228278300997,
+            "angularVelocity": 0.39765664411275126,
+            "velocityX": -1.0054808594126166,
+            "velocityY": 1.318580000092289,
+            "timestamp": 2.0143010143250253
+        },
+        {
+            "x": 7.40590178880199,
+            "y": 6.1558149457566955,
+            "heading": 3.247889559674286,
+            "angularVelocity": 0.453114913669111,
+            "velocityX": -1.7609706028527334,
+            "velocityY": 1.3720971386234067,
+            "timestamp": 2.1051537552132875
+        },
+        {
+            "x": 7.1771984711004135,
+            "y": 6.284201834322083,
+            "heading": 3.293119082899787,
+            "angularVelocity": 0.49783333758889264,
+            "velocityX": -2.517296841741443,
+            "velocityY": 1.4131317042298905,
+            "timestamp": 2.1960064961015497
+        },
+        {
+            "x": 6.879672767630686,
+            "y": 6.413043079334037,
+            "heading": 3.3395968476460443,
+            "angularVelocity": 0.5115725105467077,
+            "velocityX": -3.274812631526949,
+            "velocityY": 1.4181327250260163,
+            "timestamp": 2.286859236989812
+        },
+        {
+            "x": 6.543860581996508,
+            "y": 6.486209643935175,
+            "heading": 3.33959685479452,
+            "angularVelocity": 7.868200264443885e-8,
+            "velocityX": -3.6962251479807913,
+            "velocityY": 0.805331395462515,
+            "timestamp": 2.377711977878074
+        },
+        {
+            "x": 6.202053460852504,
+            "y": 6.52214101663038,
+            "heading": 3.33959685834771,
+            "angularVelocity": 3.910933561720104e-8,
+            "velocityX": -3.7622103395249686,
+            "velocityY": 0.39549024436583907,
+            "timestamp": 2.4685647187663364
+        },
+        {
+            "x": 5.860246134397577,
+            "y": 6.558070415270777,
+            "heading": 3.339596863101522,
+            "angularVelocity": 5.232436753390925e-8,
+            "velocityX": -3.762212599345888,
+            "velocityY": 0.39546851629479235,
+            "timestamp": 2.5594174596545987
+        },
+        {
+            "x": 5.5865423033177715,
+            "y": 6.586842981129828,
+            "heading": 3.3642400407233666,
+            "angularVelocity": 0.2712430839279776,
+            "velocityX": -3.0126095085719746,
+            "velocityY": 0.31669452762507067,
+            "timestamp": 2.650270200542861
+        },
+        {
+            "x": 5.381269635001315,
+            "y": 6.608421695048074,
+            "heading": 3.3835025789166386,
+            "angularVelocity": 0.2120193403626775,
+            "velocityX": -2.2593998409901115,
+            "velocityY": 0.23751307563506308,
+            "timestamp": 2.741122941431123
+        },
+        {
+            "x": 5.24442286400993,
+            "y": 6.622807278395225,
+            "heading": 3.3965772666434986,
+            "angularVelocity": 0.14391076811804343,
+            "velocityX": -1.5062481291531917,
+            "velocityY": 0.15833956363345458,
+            "timestamp": 2.8319756823193853
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.07273352979206758,
+            "velocityX": -0.753118324675337,
+            "velocityY": 0.07916901058187684,
+            "timestamp": 2.9228284232076476
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": -1.0239181929178098e-32,
+            "velocityX": 1.4788561432464288e-34,
+            "velocityY": 0,
+            "timestamp": 3.01368116409591
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S1.traj
new file mode 100644
index 00000000..e9a814a4
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S1.traj	
@@ -0,0 +1,301 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.327391358066629,
+            "y": 5.555067935622319,
+            "heading": 3.133430176934097,
+            "angularVelocity": -0.0681146690773455,
+            "velocityX": 0.8023787994403143,
+            "velocityY": -0.05113470021702577,
+            "timestamp": 0.09645239644992105
+        },
+        {
+            "x": 2.4821740708193567,
+            "y": 5.545203805497392,
+            "heading": 3.12029049964152,
+            "angularVelocity": -0.13622966122359603,
+            "velocityX": 1.6047575638319433,
+            "velocityY": -0.10226941463344474,
+            "timestamp": 0.1929047928998421
+        },
+        {
+            "x": 2.7143481316892246,
+            "y": 5.5304076076409565,
+            "heading": 3.1005809392040384,
+            "angularVelocity": -0.20434495318853443,
+            "velocityX": 2.4071362601178543,
+            "velocityY": -0.15340414962232987,
+            "timestamp": 0.2893571893497632
+        },
+        {
+            "x": 3.023913521714313,
+            "y": 5.510679340033408,
+            "heading": 3.0743015138082646,
+            "angularVelocity": -0.2724600565981605,
+            "velocityX": 3.2095147598102214,
+            "velocityY": -0.2045389055500683,
+            "timestamp": 0.3858095857996842
+        },
+        {
+            "x": 3.388048508086598,
+            "y": 5.487473394252625,
+            "heading": 3.0743015107885725,
+            "angularVelocity": -3.130758650216746e-8,
+            "velocityX": 3.7752818983750913,
+            "velocityY": -0.24059480774884917,
+            "timestamp": 0.48226198224960526
+        },
+        {
+            "x": 3.752183494458947,
+            "y": 5.464267448472115,
+            "heading": 3.0743015077689044,
+            "angularVelocity": -3.130734274282633e-8,
+            "velocityX": 3.7752818983757597,
+            "velocityY": -0.24059480774599656,
+            "timestamp": 0.5787143786995264
+        },
+        {
+            "x": 4.116318480831297,
+            "y": 5.441061502691605,
+            "heading": 3.0743015047492364,
+            "angularVelocity": -3.1307342782181776e-8,
+            "velocityX": 3.77528189837576,
+            "velocityY": -0.24059480774599656,
+            "timestamp": 0.6751667751494475
+        },
+        {
+            "x": 4.480453467203646,
+            "y": 5.417855556911095,
+            "heading": 3.0743015017295683,
+            "angularVelocity": -3.130734248880116e-8,
+            "velocityX": 3.7752818983757597,
+            "velocityY": -0.2405948077459966,
+            "timestamp": 0.7716191715993685
+        },
+        {
+            "x": 4.844588453575996,
+            "y": 5.3946496111305855,
+            "heading": 3.0743014987098998,
+            "angularVelocity": -3.130734337287564e-8,
+            "velocityX": 3.77528189837576,
+            "velocityY": -0.24059480774599656,
+            "timestamp": 0.8680715680492896
+        },
+        {
+            "x": 5.208723439948346,
+            "y": 5.371443665350077,
+            "heading": 3.0743014956902317,
+            "angularVelocity": -3.130734341157956e-8,
+            "velocityX": 3.7752818983757606,
+            "velocityY": -0.24059480774599654,
+            "timestamp": 0.9645239644992107
+        },
+        {
+            "x": 5.572858426320695,
+            "y": 5.348237719569567,
+            "heading": 3.0743014926705636,
+            "angularVelocity": -3.130734268712419e-8,
+            "velocityX": 3.7752818983757597,
+            "velocityY": -0.24059480774599656,
+            "timestamp": 1.0609763609491318
+        },
+        {
+            "x": 5.936993412693067,
+            "y": 5.325031773789395,
+            "heading": 3.0743014896508956,
+            "angularVelocity": -3.130734257132029e-8,
+            "velocityX": 3.7752818983759835,
+            "velocityY": -0.24059480774249117,
+            "timestamp": 1.157428757399053
+        },
+        {
+            "x": 6.301128405222732,
+            "y": 5.301825924626314,
+            "heading": 3.0743014866312275,
+            "angularVelocity": -3.1307343059822334e-8,
+            "velocityX": 3.7752819622136293,
+            "velocityY": -0.24059380603497976,
+            "timestamp": 1.253881153848974
+        },
+        {
+            "x": 6.666000662308113,
+            "y": 5.302845713467377,
+            "heading": 3.0743014835545788,
+            "angularVelocity": -3.1898104709200575e-8,
+            "velocityX": 3.7829257801264253,
+            "velocityY": 0.010572975670877716,
+            "timestamp": 1.3503335502988951
+        },
+        {
+            "x": 7.022411728114523,
+            "y": 5.380972301482392,
+            "heading": 3.0743014635013113,
+            "angularVelocity": -2.0790844162753672e-7,
+            "velocityX": 3.6952017671376614,
+            "velocityY": 0.8100015229334367,
+            "timestamp": 1.4467859467488162
+        },
+        {
+            "x": 7.302470785429282,
+            "y": 5.472668060315495,
+            "heading": 3.0808623231023553,
+            "angularVelocity": 0.06802173758793187,
+            "velocityX": 2.903598745316483,
+            "velocityY": 0.9506840908894665,
+            "timestamp": 1.5432383431987373
+        },
+        {
+            "x": 7.505508952457786,
+            "y": 5.5733927696291925,
+            "heading": 3.0939992329731627,
+            "angularVelocity": 0.1362009691239587,
+            "velocityX": 2.1050608849716217,
+            "velocityY": 1.0442945226974727,
+            "timestamp": 1.6396907396486584
+        },
+        {
+            "x": 7.631373643549882,
+            "y": 5.681732940772809,
+            "heading": 3.113712106449711,
+            "angularVelocity": 0.20437930214396433,
+            "velocityX": 1.3049410457877573,
+            "velocityY": 1.1232501745031058,
+            "timestamp": 1.7361431360985795
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 0.27254785280465055,
+            "velocityX": 0.5041487639486981,
+            "velocityY": 1.1950668253954515,
+            "timestamp": 1.8325955325485006
+        },
+        {
+            "x": 7.657241486683139,
+            "y": 5.911359552858986,
+            "heading": 3.1705946317800286,
+            "angularVelocity": 0.3367496839490721,
+            "velocityX": -0.25049891829736254,
+            "velocityY": 1.2587353088184159,
+            "timestamp": 1.923448273436763
+        },
+        {
+            "x": 7.565890794694817,
+            "y": 6.031156159947816,
+            "heading": 3.2067228278300997,
+            "angularVelocity": 0.39765664411275126,
+            "velocityX": -1.0054808594126166,
+            "velocityY": 1.318580000092289,
+            "timestamp": 2.0143010143250253
+        },
+        {
+            "x": 7.40590178880199,
+            "y": 6.1558149457566955,
+            "heading": 3.247889559674286,
+            "angularVelocity": 0.453114913669111,
+            "velocityX": -1.7609706028527334,
+            "velocityY": 1.3720971386234067,
+            "timestamp": 2.1051537552132875
+        },
+        {
+            "x": 7.1771984711004135,
+            "y": 6.284201834322083,
+            "heading": 3.293119082899787,
+            "angularVelocity": 0.49783333758889264,
+            "velocityX": -2.517296841741443,
+            "velocityY": 1.4131317042298905,
+            "timestamp": 2.1960064961015497
+        },
+        {
+            "x": 6.879672767630686,
+            "y": 6.413043079334037,
+            "heading": 3.3395968476460443,
+            "angularVelocity": 0.5115725105467077,
+            "velocityX": -3.274812631526949,
+            "velocityY": 1.4181327250260163,
+            "timestamp": 2.286859236989812
+        },
+        {
+            "x": 6.543860581996508,
+            "y": 6.486209643935175,
+            "heading": 3.33959685479452,
+            "angularVelocity": 7.868200264443885e-8,
+            "velocityX": -3.6962251479807913,
+            "velocityY": 0.805331395462515,
+            "timestamp": 2.377711977878074
+        },
+        {
+            "x": 6.202053460852504,
+            "y": 6.52214101663038,
+            "heading": 3.33959685834771,
+            "angularVelocity": 3.910933561720104e-8,
+            "velocityX": -3.7622103395249686,
+            "velocityY": 0.39549024436583907,
+            "timestamp": 2.4685647187663364
+        },
+        {
+            "x": 5.860246134397577,
+            "y": 6.558070415270777,
+            "heading": 3.339596863101522,
+            "angularVelocity": 5.232436753390925e-8,
+            "velocityX": -3.762212599345888,
+            "velocityY": 0.39546851629479235,
+            "timestamp": 2.5594174596545987
+        },
+        {
+            "x": 5.5865423033177715,
+            "y": 6.586842981129828,
+            "heading": 3.3642400407233666,
+            "angularVelocity": 0.2712430839279776,
+            "velocityX": -3.0126095085719746,
+            "velocityY": 0.31669452762507067,
+            "timestamp": 2.650270200542861
+        },
+        {
+            "x": 5.381269635001315,
+            "y": 6.608421695048074,
+            "heading": 3.3835025789166386,
+            "angularVelocity": 0.2120193403626775,
+            "velocityX": -2.2593998409901115,
+            "velocityY": 0.23751307563506308,
+            "timestamp": 2.741122941431123
+        },
+        {
+            "x": 5.24442286400993,
+            "y": 6.622807278395225,
+            "heading": 3.3965772666434986,
+            "angularVelocity": 0.14391076811804343,
+            "velocityX": -1.5062481291531917,
+            "velocityY": 0.15833956363345458,
+            "timestamp": 2.8319756823193853
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.07273352979206758,
+            "velocityX": -0.753118324675337,
+            "velocityY": 0.07916901058187684,
+            "timestamp": 2.9228284232076476
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": -1.0239181929178098e-32,
+            "velocityX": 1.4788561432464288e-34,
+            "velocityY": 0,
+            "timestamp": 3.01368116409591
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S2.1.traj
new file mode 100644
index 00000000..9ae1f0d7
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S2.1.traj	
@@ -0,0 +1,310 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3282521020736953,
+            "y": 5.564172492511594,
+            "heading": 3.14,
+            "angularVelocity": 1.232216948997824e-17,
+            "velocityX": 0.8071870820603332,
+            "velocityY": 0.043040148021588745,
+            "timestamp": 0.09694419527026792
+        },
+        {
+            "x": 2.484756303050172,
+            "y": 5.5725174773657065,
+            "heading": 3.14,
+            "angularVelocity": 2.4529935765970837e-17,
+            "velocityX": 1.6143741314120201,
+            "velocityY": 0.08608029429911494,
+            "timestamp": 0.19388839054053583
+        },
+        {
+            "x": 2.7195125965876055,
+            "y": 5.5850349542241835,
+            "heading": 3.14,
+            "angularVelocity": 3.650998333784998e-17,
+            "velocityX": 2.421561115346432,
+            "velocityY": 0.12912043708851692,
+            "timestamp": 0.29083258581080373
+        },
+        {
+            "x": 3.032520963660537,
+            "y": 5.601724922072566,
+            "heading": 3.14,
+            "angularVelocity": 4.7804313451185203e-17,
+            "velocityX": 3.2287479030291797,
+            "velocityY": 0.17216056941355468,
+            "timestamp": 0.38777678108107166
+        },
+        {
+            "x": 3.398734861706694,
+            "y": 5.621251870322576,
+            "heading": 3.14,
+            "angularVelocity": 4.271471690686359e-16,
+            "velocityX": 3.777574273786295,
+            "velocityY": 0.20142462579390313,
+            "timestamp": 0.4847209763513396
+        },
+        {
+            "x": 3.7649487597528926,
+            "y": 5.6407788185725884,
+            "heading": 3.14,
+            "angularVelocity": 3.505027380693511e-16,
+            "velocityX": 3.7775742737867146,
+            "velocityY": 0.20142462579393414,
+            "timestamp": 0.5816651716216075
+        },
+        {
+            "x": 4.1311626577990905,
+            "y": 5.660305766822602,
+            "heading": 3.14,
+            "angularVelocity": 3.7529805795054796e-16,
+            "velocityX": 3.7775742737867146,
+            "velocityY": 0.20142462579393414,
+            "timestamp": 0.6786093668918753
+        },
+        {
+            "x": 4.497376555845288,
+            "y": 5.679832715072616,
+            "heading": 3.14,
+            "angularVelocity": 4.2292762480360005e-16,
+            "velocityX": 3.7775742737867146,
+            "velocityY": 0.20142462579393414,
+            "timestamp": 0.7755535621621432
+        },
+        {
+            "x": 4.8635904538914865,
+            "y": 5.699359663322629,
+            "heading": 3.14,
+            "angularVelocity": -1.0631316332366903e-16,
+            "velocityX": 3.7775742737867146,
+            "velocityY": 0.20142462579393414,
+            "timestamp": 0.8724977574324111
+        },
+        {
+            "x": 5.229804351937685,
+            "y": 5.718886611572643,
+            "heading": 3.14,
+            "angularVelocity": -2.15507046342417e-16,
+            "velocityX": 3.7775742737867146,
+            "velocityY": 0.2014246257939341,
+            "timestamp": 0.969441952702679
+        },
+        {
+            "x": 5.596018249983883,
+            "y": 5.738413559822655,
+            "heading": 3.14,
+            "angularVelocity": 2.7855582274724927e-16,
+            "velocityX": 3.7775742737867146,
+            "velocityY": 0.20142462579393414,
+            "timestamp": 1.0663861479729468
+        },
+        {
+            "x": 5.962232148030081,
+            "y": 5.757940508072669,
+            "heading": 3.14,
+            "angularVelocity": 3.1548114792697624e-17,
+            "velocityX": 3.7775742737867146,
+            "velocityY": 0.201424625793935,
+            "timestamp": 1.1633303432432147
+        },
+        {
+            "x": 6.3284460460762855,
+            "y": 5.777467456322548,
+            "heading": 3.14,
+            "angularVelocity": 5.639488659636306e-17,
+            "velocityX": 3.777574273786789,
+            "velocityY": 0.20142462579255413,
+            "timestamp": 1.2602745385134826
+        },
+        {
+            "x": 6.694659946182267,
+            "y": 5.796994365942685,
+            "heading": 3.14,
+            "angularVelocity": 1.8236235813910642e-16,
+            "velocityX": 3.7775742950338076,
+            "velocityY": 0.20142422731870196,
+            "timestamp": 1.3572187337837505
+        },
+        {
+            "x": 7.048232130231478,
+            "y": 5.805340495974463,
+            "heading": 3.14,
+            "angularVelocity": 9.880815779139735e-16,
+            "velocityX": 3.6471723000895206,
+            "velocityY": 0.08609210670542117,
+            "timestamp": 1.4541629290540183
+        },
+        {
+            "x": 7.323552241539944,
+            "y": 5.8095141201136,
+            "heading": 3.14,
+            "angularVelocity": 1.0046882743013462e-15,
+            "velocityX": 2.839985520604805,
+            "velocityY": 0.04305182065552602,
+            "timestamp": 1.5511071243242862
+        },
+        {
+            "x": 7.520620260713227,
+            "y": 5.809515244263857,
+            "heading": 3.14,
+            "angularVelocity": 1.1309282599454547e-15,
+            "velocityX": 2.032798541062352,
+            "velocityY": 0.000011595504179921879,
+            "timestamp": 1.648051319594554
+        },
+        {
+            "x": 7.639436181279198,
+            "y": 5.805343870530581,
+            "heading": 3.14,
+            "angularVelocity": 1.1596973638353238e-15,
+            "velocityX": 1.2256114947584758,
+            "velocityY": -0.043028607930100944,
+            "timestamp": 1.744995514864822
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 1.1762980395606437e-15,
+            "velocityX": 0.41842441505544703,
+            "velocityY": -0.08606880015967894,
+            "timestamp": 1.8419397101350898
+        },
+        {
+            "x": 7.637810963282883,
+            "y": 5.783845940195137,
+            "heading": 3.14,
+            "angularVelocity": 1.2081191600227442e-15,
+            "velocityX": -0.41931671098523704,
+            "velocityY": -0.1307381629639976,
+            "timestamp": 1.94255348238971
+        },
+        {
+            "x": 7.511333424065826,
+            "y": 5.766201461544033,
+            "heading": 3.14,
+            "angularVelocity": 1.221716047001012e-15,
+            "velocityX": -1.257059901260624,
+            "velocityY": -0.1753684232696404,
+            "timestamp": 2.04316725464433
+        },
+        {
+            "x": 7.3005670350867495,
+            "y": 5.744073149987429,
+            "heading": 3.14,
+            "angularVelocity": 9.674484857548948e-16,
+            "velocityX": -2.0948065429741827,
+            "velocityY": -0.2199332259036739,
+            "timestamp": 2.14378102689895
+        },
+        {
+            "x": 7.005511097627955,
+            "y": 5.717474286699145,
+            "heading": 3.14,
+            "angularVelocity": 6.103023189064465e-16,
+            "velocityX": -2.9325601292436843,
+            "velocityY": -0.2643660269377714,
+            "timestamp": 2.2443947991535698
+        },
+        {
+            "x": 6.626163481456156,
+            "y": 5.686445611767655,
+            "heading": 3.14,
+            "angularVelocity": -3.898195701943848e-16,
+            "velocityX": -3.7703348879039833,
+            "velocityY": -0.30839391219134293,
+            "timestamp": 2.3450085714081896
+        },
+        {
+            "x": 6.24609124574338,
+            "y": 5.666109085648216,
+            "heading": 3.14,
+            "angularVelocity": 5.28249015807614e-17,
+            "velocityX": -3.7775368838161545,
+            "velocityY": -0.2021246762112827,
+            "timestamp": 2.4456223436628095
+        },
+        {
+            "x": 5.866019008161075,
+            "y": 5.6457725944687756,
+            "heading": 3.14,
+            "angularVelocity": 1.0714715731827337e-16,
+            "velocityX": -3.777536902397429,
+            "velocityY": -0.20212432894236246,
+            "timestamp": 2.5462361159174294
+        },
+        {
+            "x": 5.485946770578765,
+            "y": 5.625436103289443,
+            "heading": 3.14,
+            "angularVelocity": 4.4105778021487463e-16,
+            "velocityX": -3.7775369023974865,
+            "velocityY": -0.2021243289413037,
+            "timestamp": 2.6468498881720492
+        },
+        {
+            "x": 5.105874532996473,
+            "y": 5.60509961211012,
+            "heading": 3.14,
+            "angularVelocity": 4.563638430357427e-16,
+            "velocityX": -3.7775369023972933,
+            "velocityY": -0.20212432894121146,
+            "timestamp": 2.747463660426669
+        },
+        {
+            "x": 4.768724738760508,
+            "y": 5.587059768280702,
+            "heading": 3.14,
+            "angularVelocity": 7.096592098456484e-16,
+            "velocityX": -3.3509308606209816,
+            "velocityY": -0.1792979574700793,
+            "timestamp": 2.848077432681289
+        },
+        {
+            "x": 4.5158623742843815,
+            "y": 5.573529884402755,
+            "heading": 3.14,
+            "angularVelocity": 5.58698579821863e-16,
+            "velocityX": -2.5131983323104805,
+            "velocityY": -0.13447347810003318,
+            "timestamp": 2.948691204935909
+        },
+        {
+            "x": 4.3472874591846,
+            "y": 5.564509961525897,
+            "heading": 3.14,
+            "angularVelocity": 2.9765197430594465e-16,
+            "velocityX": -1.6754656090315643,
+            "velocityY": -0.08964898829784126,
+            "timestamp": 3.0493049771905287
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 1.413897346526725e-16,
+            "velocityX": -0.8377328207631561,
+            "velocityY": -0.04482449501826641,
+            "timestamp": 3.1499187494451486
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -1.608253361776519e-31,
+            "velocityY": 0,
+            "timestamp": 3.2505325216997685
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S2.traj
new file mode 100644
index 00000000..9ae1f0d7
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S2.traj	
@@ -0,0 +1,310 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3282521020736953,
+            "y": 5.564172492511594,
+            "heading": 3.14,
+            "angularVelocity": 1.232216948997824e-17,
+            "velocityX": 0.8071870820603332,
+            "velocityY": 0.043040148021588745,
+            "timestamp": 0.09694419527026792
+        },
+        {
+            "x": 2.484756303050172,
+            "y": 5.5725174773657065,
+            "heading": 3.14,
+            "angularVelocity": 2.4529935765970837e-17,
+            "velocityX": 1.6143741314120201,
+            "velocityY": 0.08608029429911494,
+            "timestamp": 0.19388839054053583
+        },
+        {
+            "x": 2.7195125965876055,
+            "y": 5.5850349542241835,
+            "heading": 3.14,
+            "angularVelocity": 3.650998333784998e-17,
+            "velocityX": 2.421561115346432,
+            "velocityY": 0.12912043708851692,
+            "timestamp": 0.29083258581080373
+        },
+        {
+            "x": 3.032520963660537,
+            "y": 5.601724922072566,
+            "heading": 3.14,
+            "angularVelocity": 4.7804313451185203e-17,
+            "velocityX": 3.2287479030291797,
+            "velocityY": 0.17216056941355468,
+            "timestamp": 0.38777678108107166
+        },
+        {
+            "x": 3.398734861706694,
+            "y": 5.621251870322576,
+            "heading": 3.14,
+            "angularVelocity": 4.271471690686359e-16,
+            "velocityX": 3.777574273786295,
+            "velocityY": 0.20142462579390313,
+            "timestamp": 0.4847209763513396
+        },
+        {
+            "x": 3.7649487597528926,
+            "y": 5.6407788185725884,
+            "heading": 3.14,
+            "angularVelocity": 3.505027380693511e-16,
+            "velocityX": 3.7775742737867146,
+            "velocityY": 0.20142462579393414,
+            "timestamp": 0.5816651716216075
+        },
+        {
+            "x": 4.1311626577990905,
+            "y": 5.660305766822602,
+            "heading": 3.14,
+            "angularVelocity": 3.7529805795054796e-16,
+            "velocityX": 3.7775742737867146,
+            "velocityY": 0.20142462579393414,
+            "timestamp": 0.6786093668918753
+        },
+        {
+            "x": 4.497376555845288,
+            "y": 5.679832715072616,
+            "heading": 3.14,
+            "angularVelocity": 4.2292762480360005e-16,
+            "velocityX": 3.7775742737867146,
+            "velocityY": 0.20142462579393414,
+            "timestamp": 0.7755535621621432
+        },
+        {
+            "x": 4.8635904538914865,
+            "y": 5.699359663322629,
+            "heading": 3.14,
+            "angularVelocity": -1.0631316332366903e-16,
+            "velocityX": 3.7775742737867146,
+            "velocityY": 0.20142462579393414,
+            "timestamp": 0.8724977574324111
+        },
+        {
+            "x": 5.229804351937685,
+            "y": 5.718886611572643,
+            "heading": 3.14,
+            "angularVelocity": -2.15507046342417e-16,
+            "velocityX": 3.7775742737867146,
+            "velocityY": 0.2014246257939341,
+            "timestamp": 0.969441952702679
+        },
+        {
+            "x": 5.596018249983883,
+            "y": 5.738413559822655,
+            "heading": 3.14,
+            "angularVelocity": 2.7855582274724927e-16,
+            "velocityX": 3.7775742737867146,
+            "velocityY": 0.20142462579393414,
+            "timestamp": 1.0663861479729468
+        },
+        {
+            "x": 5.962232148030081,
+            "y": 5.757940508072669,
+            "heading": 3.14,
+            "angularVelocity": 3.1548114792697624e-17,
+            "velocityX": 3.7775742737867146,
+            "velocityY": 0.201424625793935,
+            "timestamp": 1.1633303432432147
+        },
+        {
+            "x": 6.3284460460762855,
+            "y": 5.777467456322548,
+            "heading": 3.14,
+            "angularVelocity": 5.639488659636306e-17,
+            "velocityX": 3.777574273786789,
+            "velocityY": 0.20142462579255413,
+            "timestamp": 1.2602745385134826
+        },
+        {
+            "x": 6.694659946182267,
+            "y": 5.796994365942685,
+            "heading": 3.14,
+            "angularVelocity": 1.8236235813910642e-16,
+            "velocityX": 3.7775742950338076,
+            "velocityY": 0.20142422731870196,
+            "timestamp": 1.3572187337837505
+        },
+        {
+            "x": 7.048232130231478,
+            "y": 5.805340495974463,
+            "heading": 3.14,
+            "angularVelocity": 9.880815779139735e-16,
+            "velocityX": 3.6471723000895206,
+            "velocityY": 0.08609210670542117,
+            "timestamp": 1.4541629290540183
+        },
+        {
+            "x": 7.323552241539944,
+            "y": 5.8095141201136,
+            "heading": 3.14,
+            "angularVelocity": 1.0046882743013462e-15,
+            "velocityX": 2.839985520604805,
+            "velocityY": 0.04305182065552602,
+            "timestamp": 1.5511071243242862
+        },
+        {
+            "x": 7.520620260713227,
+            "y": 5.809515244263857,
+            "heading": 3.14,
+            "angularVelocity": 1.1309282599454547e-15,
+            "velocityX": 2.032798541062352,
+            "velocityY": 0.000011595504179921879,
+            "timestamp": 1.648051319594554
+        },
+        {
+            "x": 7.639436181279198,
+            "y": 5.805343870530581,
+            "heading": 3.14,
+            "angularVelocity": 1.1596973638353238e-15,
+            "velocityX": 1.2256114947584758,
+            "velocityY": -0.043028607930100944,
+            "timestamp": 1.744995514864822
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 1.1762980395606437e-15,
+            "velocityX": 0.41842441505544703,
+            "velocityY": -0.08606880015967894,
+            "timestamp": 1.8419397101350898
+        },
+        {
+            "x": 7.637810963282883,
+            "y": 5.783845940195137,
+            "heading": 3.14,
+            "angularVelocity": 1.2081191600227442e-15,
+            "velocityX": -0.41931671098523704,
+            "velocityY": -0.1307381629639976,
+            "timestamp": 1.94255348238971
+        },
+        {
+            "x": 7.511333424065826,
+            "y": 5.766201461544033,
+            "heading": 3.14,
+            "angularVelocity": 1.221716047001012e-15,
+            "velocityX": -1.257059901260624,
+            "velocityY": -0.1753684232696404,
+            "timestamp": 2.04316725464433
+        },
+        {
+            "x": 7.3005670350867495,
+            "y": 5.744073149987429,
+            "heading": 3.14,
+            "angularVelocity": 9.674484857548948e-16,
+            "velocityX": -2.0948065429741827,
+            "velocityY": -0.2199332259036739,
+            "timestamp": 2.14378102689895
+        },
+        {
+            "x": 7.005511097627955,
+            "y": 5.717474286699145,
+            "heading": 3.14,
+            "angularVelocity": 6.103023189064465e-16,
+            "velocityX": -2.9325601292436843,
+            "velocityY": -0.2643660269377714,
+            "timestamp": 2.2443947991535698
+        },
+        {
+            "x": 6.626163481456156,
+            "y": 5.686445611767655,
+            "heading": 3.14,
+            "angularVelocity": -3.898195701943848e-16,
+            "velocityX": -3.7703348879039833,
+            "velocityY": -0.30839391219134293,
+            "timestamp": 2.3450085714081896
+        },
+        {
+            "x": 6.24609124574338,
+            "y": 5.666109085648216,
+            "heading": 3.14,
+            "angularVelocity": 5.28249015807614e-17,
+            "velocityX": -3.7775368838161545,
+            "velocityY": -0.2021246762112827,
+            "timestamp": 2.4456223436628095
+        },
+        {
+            "x": 5.866019008161075,
+            "y": 5.6457725944687756,
+            "heading": 3.14,
+            "angularVelocity": 1.0714715731827337e-16,
+            "velocityX": -3.777536902397429,
+            "velocityY": -0.20212432894236246,
+            "timestamp": 2.5462361159174294
+        },
+        {
+            "x": 5.485946770578765,
+            "y": 5.625436103289443,
+            "heading": 3.14,
+            "angularVelocity": 4.4105778021487463e-16,
+            "velocityX": -3.7775369023974865,
+            "velocityY": -0.2021243289413037,
+            "timestamp": 2.6468498881720492
+        },
+        {
+            "x": 5.105874532996473,
+            "y": 5.60509961211012,
+            "heading": 3.14,
+            "angularVelocity": 4.563638430357427e-16,
+            "velocityX": -3.7775369023972933,
+            "velocityY": -0.20212432894121146,
+            "timestamp": 2.747463660426669
+        },
+        {
+            "x": 4.768724738760508,
+            "y": 5.587059768280702,
+            "heading": 3.14,
+            "angularVelocity": 7.096592098456484e-16,
+            "velocityX": -3.3509308606209816,
+            "velocityY": -0.1792979574700793,
+            "timestamp": 2.848077432681289
+        },
+        {
+            "x": 4.5158623742843815,
+            "y": 5.573529884402755,
+            "heading": 3.14,
+            "angularVelocity": 5.58698579821863e-16,
+            "velocityX": -2.5131983323104805,
+            "velocityY": -0.13447347810003318,
+            "timestamp": 2.948691204935909
+        },
+        {
+            "x": 4.3472874591846,
+            "y": 5.564509961525897,
+            "heading": 3.14,
+            "angularVelocity": 2.9765197430594465e-16,
+            "velocityX": -1.6754656090315643,
+            "velocityY": -0.08964898829784126,
+            "timestamp": 3.0493049771905287
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 1.413897346526725e-16,
+            "velocityX": -0.8377328207631561,
+            "velocityY": -0.04482449501826641,
+            "timestamp": 3.1499187494451486
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -1.608253361776519e-31,
+            "velocityY": 0,
+            "timestamp": 3.2505325216997685
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S1.1.traj
new file mode 100644
index 00000000..b581bfce
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S1.1.traj	
@@ -0,0 +1,337 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3161821396104223,
+            "y": 5.531175019358262,
+            "heading": 3.133818954660017,
+            "angularVelocity": -0.0664208658906851,
+            "velocityX": 0.711186341084945,
+            "velocityY": -0.3097502231737145,
+            "timestamp": 0.09305878893773292
+        },
+        {
+            "x": 2.448546415336416,
+            "y": 5.473525059620157,
+            "heading": 3.121456701812491,
+            "angularVelocity": -0.13284347441699146,
+            "velocityX": 1.4223726446145823,
+            "velocityY": -0.6195004297410188,
+            "timestamp": 0.18611757787546584
+        },
+        {
+            "x": 2.647092820298068,
+            "y": 5.387050123667136,
+            "heading": 3.1029129720677258,
+            "angularVelocity": -0.19926897777675723,
+            "velocityX": 2.1335588742134064,
+            "velocityY": -0.9292506053445697,
+            "timestamp": 0.2791763668131988
+        },
+        {
+            "x": 2.911821335852912,
+            "y": 5.271750219180522,
+            "heading": 3.078187424791321,
+            "angularVelocity": -0.26569814155811494,
+            "velocityX": 2.844744903482224,
+            "velocityY": -1.2390006984054311,
+            "timestamp": 0.3722351557509317
+        },
+        {
+            "x": 3.2345733580792335,
+            "y": 5.131178588369686,
+            "heading": 3.07818742179356,
+            "angularVelocity": -3.221363014134022e-8,
+            "velocityX": 3.468259429448196,
+            "velocityY": -1.5105680228107692,
+            "timestamp": 0.4652939446886646
+        },
+        {
+            "x": 3.557325380260581,
+            "y": 4.990606957455228,
+            "heading": 3.078187418795868,
+            "angularVelocity": -3.2212882318357466e-8,
+            "velocityX": 3.4682594289649007,
+            "velocityY": -1.510568023924279,
+            "timestamp": 0.5583527336263975
+        },
+        {
+            "x": 3.8800774024419273,
+            "y": 4.85003532654077,
+            "heading": 3.0781874157981757,
+            "angularVelocity": -3.221288214692397e-8,
+            "velocityX": 3.468259428964899,
+            "velocityY": -1.5105680239242834,
+            "timestamp": 0.6514115225641304
+        },
+        {
+            "x": 4.202829424623274,
+            "y": 4.709463695626312,
+            "heading": 3.078187412800484,
+            "angularVelocity": -3.221288189984943e-8,
+            "velocityX": 3.468259428964899,
+            "velocityY": -1.5105680239242838,
+            "timestamp": 0.7444703115018633
+        },
+        {
+            "x": 4.525581446804621,
+            "y": 4.568892064711854,
+            "heading": 3.078187409802792,
+            "angularVelocity": -3.2212882353617496e-8,
+            "velocityX": 3.468259428964899,
+            "velocityY": -1.5105680239242834,
+            "timestamp": 0.8375291004395962
+        },
+        {
+            "x": 4.848333468985968,
+            "y": 4.428320433797395,
+            "heading": 3.0781874068051,
+            "angularVelocity": -3.221288275372931e-8,
+            "velocityX": 3.468259428964899,
+            "velocityY": -1.5105680239242836,
+            "timestamp": 0.9305878893773291
+        },
+        {
+            "x": 5.1710854911673145,
+            "y": 4.287748802882937,
+            "heading": 3.0781874038074077,
+            "angularVelocity": -3.221288266203546e-8,
+            "velocityX": 3.4682594289648985,
+            "velocityY": -1.5105680239242834,
+            "timestamp": 1.023646678315062
+        },
+        {
+            "x": 5.493837513348661,
+            "y": 4.147177171968479,
+            "heading": 3.078187400809716,
+            "angularVelocity": -3.221288308232409e-8,
+            "velocityX": 3.4682594289648994,
+            "velocityY": -1.5105680239242831,
+            "timestamp": 1.116705467252795
+        },
+        {
+            "x": 5.816589535530807,
+            "y": 4.006605541055855,
+            "heading": 3.078187397812024,
+            "angularVelocity": -3.221288143740925e-8,
+            "velocityX": 3.4682594289734867,
+            "velocityY": -1.510568023904566,
+            "timestamp": 1.209764256190528
+        },
+        {
+            "x": 6.139341766628845,
+            "y": 3.866034389814445,
+            "heading": 3.0781873948143317,
+            "angularVelocity": -3.2212889577605074e-8,
+            "velocityX": 3.4682616739617793,
+            "velocityY": -1.5105628694079405,
+            "timestamp": 1.302823045128261
+        },
+        {
+            "x": 6.479618734001887,
+            "y": 3.7758078084398288,
+            "heading": 3.078187391647213,
+            "angularVelocity": -3.403352419183157e-8,
+            "velocityX": 3.656580654630348,
+            "velocityY": -0.9695653941401335,
+            "timestamp": 1.395881834065994
+        },
+        {
+            "x": 6.831146782632028,
+            "y": 3.7569067855053886,
+            "heading": 3.078187386672884,
+            "angularVelocity": -5.3453615233680875e-8,
+            "velocityX": 3.777483595508151,
+            "velocityY": -0.20310841297416227,
+            "timestamp": 1.488940623003727
+        },
+        {
+            "x": 7.135392516475817,
+            "y": 3.7925535020762564,
+            "heading": 3.084237894820064,
+            "angularVelocity": 0.06501812688781647,
+            "velocityX": 3.269392792628807,
+            "velocityY": 0.3830558830366858,
+            "timestamp": 1.58199941194146
+        },
+        {
+            "x": 7.379778215429103,
+            "y": 3.868544177421427,
+            "heading": 3.09656532372019,
+            "angularVelocity": 0.1324692599252942,
+            "velocityX": 2.6261431267583744,
+            "velocityY": 0.8165878388554683,
+            "timestamp": 1.675058200879193
+        },
+        {
+            "x": 7.561679450282434,
+            "y": 3.980681271949268,
+            "heading": 3.1151578247073277,
+            "angularVelocity": 0.1997930684395467,
+            "velocityX": 1.954691619456221,
+            "velocityY": 1.2050134738253881,
+            "timestamp": 1.768116989816926
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.26695141400662953,
+            "velocityX": 1.2714602357090166,
+            "velocityY": 1.572325727864734,
+            "timestamp": 1.861175778754659
+        },
+        {
+            "x": 7.734511016491924,
+            "y": 4.293948601996246,
+            "heading": 3.168900193264706,
+            "angularVelocity": 0.3300287650543787,
+            "velocityX": 0.6224942265925568,
+            "velocityY": 1.9064869372920104,
+            "timestamp": 1.948744488894043
+        },
+        {
+            "x": 7.731795753069985,
+            "y": 4.489388700342548,
+            "heading": 3.202314872595517,
+            "angularVelocity": 0.38158240857521825,
+            "velocityX": -0.031007233264223562,
+            "velocityY": 2.2318485453904535,
+            "timestamp": 2.0363131990334273
+        },
+        {
+            "x": 7.671228890668297,
+            "y": 4.712045759892295,
+            "heading": 3.2385899184542097,
+            "angularVelocity": 0.41424666186076586,
+            "velocityX": -0.6916495892800459,
+            "velocityY": 2.5426554667225383,
+            "timestamp": 2.1238819091728116
+        },
+        {
+            "x": 7.551691509705012,
+            "y": 4.959414788228928,
+            "heading": 3.2745248408990206,
+            "angularVelocity": 0.4103625871342999,
+            "velocityX": -1.3650695639231716,
+            "velocityY": 2.824856366422358,
+            "timestamp": 2.211450619312196
+        },
+        {
+            "x": 7.370754702688456,
+            "y": 5.224445119410505,
+            "heading": 3.301432833728465,
+            "angularVelocity": 0.30727862482631746,
+            "velocityX": -2.066226700479633,
+            "velocityY": 3.0265414525316805,
+            "timestamp": 2.29901932945158
+        },
+        {
+            "x": 7.135099709363178,
+            "y": 5.457264131297548,
+            "heading": 3.3014328467429856,
+            "angularVelocity": 1.4862067021464798e-7,
+            "velocityX": -2.6910867243583128,
+            "velocityY": 2.658700939142107,
+            "timestamp": 2.3865880395909644
+        },
+        {
+            "x": 6.859107156435953,
+            "y": 5.640474617463791,
+            "heading": 3.3014328522641176,
+            "angularVelocity": 6.304914289451061e-8,
+            "velocityX": -3.1517256847557116,
+            "velocityY": 2.0921912161846987,
+            "timestamp": 2.4741567497303487
+        },
+        {
+            "x": 6.573538412185887,
+            "y": 5.808368822234562,
+            "heading": 3.3014328577109833,
+            "angularVelocity": 6.220104875802801e-8,
+            "velocityX": -3.26108199830194,
+            "velocityY": 1.9172853465984772,
+            "timestamp": 2.561725459869733
+        },
+        {
+            "x": 6.287969619272061,
+            "y": 5.976262944233841,
+            "heading": 3.301432863157849,
+            "angularVelocity": 6.220104736843505e-8,
+            "velocityX": -3.2610825540228303,
+            "velocityY": 1.9172844013808124,
+            "timestamp": 2.6492941700091173
+        },
+        {
+            "x": 6.002400826335374,
+            "y": 6.144157066194225,
+            "heading": 3.3014328686047243,
+            "angularVelocity": 6.2201164432783e-8,
+            "velocityX": -3.2610825542838673,
+            "velocityY": 1.9172844009366552,
+            "timestamp": 2.7368628801485015
+        },
+        {
+            "x": 5.726906238126056,
+            "y": 6.306118202865447,
+            "heading": 3.330104518338368,
+            "angularVelocity": 0.3274188884135285,
+            "velocityX": -3.14603912482906,
+            "velocityY": 1.849532057894034,
+            "timestamp": 2.824431590287886
+        },
+        {
+            "x": 5.5065316534228295,
+            "y": 6.435677156249956,
+            "heading": 3.357685145070299,
+            "angularVelocity": 0.31495983768665414,
+            "velocityX": -2.5165905076419874,
+            "velocityY": 1.47951195327976,
+            "timestamp": 2.91200030042727
+        },
+        {
+            "x": 5.341261355658222,
+            "y": 6.532840911354814,
+            "heading": 3.379877882623065,
+            "angularVelocity": 0.2534322764083417,
+            "velocityX": -1.8873213674330116,
+            "velocityY": 1.1095716146806525,
+            "timestamp": 2.9995690105666544
+        },
+        {
+            "x": 5.231085952105995,
+            "y": 6.597614249467722,
+            "heading": 3.395277171030749,
+            "angularVelocity": 0.17585377680193118,
+            "velocityX": -1.2581594884389657,
+            "velocityY": 0.7396858764940984,
+            "timestamp": 3.0871377207060386
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.09030778386766043,
+            "velocityX": -0.6290597636794472,
+            "velocityY": 0.3698324490646197,
+            "timestamp": 3.174706430845423
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0,
+            "velocityX": -1.4368373863628223e-33,
+            "velocityY": 0,
+            "timestamp": 3.262275140984807
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S1.traj
new file mode 100644
index 00000000..b581bfce
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S1.traj	
@@ -0,0 +1,337 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3161821396104223,
+            "y": 5.531175019358262,
+            "heading": 3.133818954660017,
+            "angularVelocity": -0.0664208658906851,
+            "velocityX": 0.711186341084945,
+            "velocityY": -0.3097502231737145,
+            "timestamp": 0.09305878893773292
+        },
+        {
+            "x": 2.448546415336416,
+            "y": 5.473525059620157,
+            "heading": 3.121456701812491,
+            "angularVelocity": -0.13284347441699146,
+            "velocityX": 1.4223726446145823,
+            "velocityY": -0.6195004297410188,
+            "timestamp": 0.18611757787546584
+        },
+        {
+            "x": 2.647092820298068,
+            "y": 5.387050123667136,
+            "heading": 3.1029129720677258,
+            "angularVelocity": -0.19926897777675723,
+            "velocityX": 2.1335588742134064,
+            "velocityY": -0.9292506053445697,
+            "timestamp": 0.2791763668131988
+        },
+        {
+            "x": 2.911821335852912,
+            "y": 5.271750219180522,
+            "heading": 3.078187424791321,
+            "angularVelocity": -0.26569814155811494,
+            "velocityX": 2.844744903482224,
+            "velocityY": -1.2390006984054311,
+            "timestamp": 0.3722351557509317
+        },
+        {
+            "x": 3.2345733580792335,
+            "y": 5.131178588369686,
+            "heading": 3.07818742179356,
+            "angularVelocity": -3.221363014134022e-8,
+            "velocityX": 3.468259429448196,
+            "velocityY": -1.5105680228107692,
+            "timestamp": 0.4652939446886646
+        },
+        {
+            "x": 3.557325380260581,
+            "y": 4.990606957455228,
+            "heading": 3.078187418795868,
+            "angularVelocity": -3.2212882318357466e-8,
+            "velocityX": 3.4682594289649007,
+            "velocityY": -1.510568023924279,
+            "timestamp": 0.5583527336263975
+        },
+        {
+            "x": 3.8800774024419273,
+            "y": 4.85003532654077,
+            "heading": 3.0781874157981757,
+            "angularVelocity": -3.221288214692397e-8,
+            "velocityX": 3.468259428964899,
+            "velocityY": -1.5105680239242834,
+            "timestamp": 0.6514115225641304
+        },
+        {
+            "x": 4.202829424623274,
+            "y": 4.709463695626312,
+            "heading": 3.078187412800484,
+            "angularVelocity": -3.221288189984943e-8,
+            "velocityX": 3.468259428964899,
+            "velocityY": -1.5105680239242838,
+            "timestamp": 0.7444703115018633
+        },
+        {
+            "x": 4.525581446804621,
+            "y": 4.568892064711854,
+            "heading": 3.078187409802792,
+            "angularVelocity": -3.2212882353617496e-8,
+            "velocityX": 3.468259428964899,
+            "velocityY": -1.5105680239242834,
+            "timestamp": 0.8375291004395962
+        },
+        {
+            "x": 4.848333468985968,
+            "y": 4.428320433797395,
+            "heading": 3.0781874068051,
+            "angularVelocity": -3.221288275372931e-8,
+            "velocityX": 3.468259428964899,
+            "velocityY": -1.5105680239242836,
+            "timestamp": 0.9305878893773291
+        },
+        {
+            "x": 5.1710854911673145,
+            "y": 4.287748802882937,
+            "heading": 3.0781874038074077,
+            "angularVelocity": -3.221288266203546e-8,
+            "velocityX": 3.4682594289648985,
+            "velocityY": -1.5105680239242834,
+            "timestamp": 1.023646678315062
+        },
+        {
+            "x": 5.493837513348661,
+            "y": 4.147177171968479,
+            "heading": 3.078187400809716,
+            "angularVelocity": -3.221288308232409e-8,
+            "velocityX": 3.4682594289648994,
+            "velocityY": -1.5105680239242831,
+            "timestamp": 1.116705467252795
+        },
+        {
+            "x": 5.816589535530807,
+            "y": 4.006605541055855,
+            "heading": 3.078187397812024,
+            "angularVelocity": -3.221288143740925e-8,
+            "velocityX": 3.4682594289734867,
+            "velocityY": -1.510568023904566,
+            "timestamp": 1.209764256190528
+        },
+        {
+            "x": 6.139341766628845,
+            "y": 3.866034389814445,
+            "heading": 3.0781873948143317,
+            "angularVelocity": -3.2212889577605074e-8,
+            "velocityX": 3.4682616739617793,
+            "velocityY": -1.5105628694079405,
+            "timestamp": 1.302823045128261
+        },
+        {
+            "x": 6.479618734001887,
+            "y": 3.7758078084398288,
+            "heading": 3.078187391647213,
+            "angularVelocity": -3.403352419183157e-8,
+            "velocityX": 3.656580654630348,
+            "velocityY": -0.9695653941401335,
+            "timestamp": 1.395881834065994
+        },
+        {
+            "x": 6.831146782632028,
+            "y": 3.7569067855053886,
+            "heading": 3.078187386672884,
+            "angularVelocity": -5.3453615233680875e-8,
+            "velocityX": 3.777483595508151,
+            "velocityY": -0.20310841297416227,
+            "timestamp": 1.488940623003727
+        },
+        {
+            "x": 7.135392516475817,
+            "y": 3.7925535020762564,
+            "heading": 3.084237894820064,
+            "angularVelocity": 0.06501812688781647,
+            "velocityX": 3.269392792628807,
+            "velocityY": 0.3830558830366858,
+            "timestamp": 1.58199941194146
+        },
+        {
+            "x": 7.379778215429103,
+            "y": 3.868544177421427,
+            "heading": 3.09656532372019,
+            "angularVelocity": 0.1324692599252942,
+            "velocityX": 2.6261431267583744,
+            "velocityY": 0.8165878388554683,
+            "timestamp": 1.675058200879193
+        },
+        {
+            "x": 7.561679450282434,
+            "y": 3.980681271949268,
+            "heading": 3.1151578247073277,
+            "angularVelocity": 0.1997930684395467,
+            "velocityX": 1.954691619456221,
+            "velocityY": 1.2050134738253881,
+            "timestamp": 1.768116989816926
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.26695141400662953,
+            "velocityX": 1.2714602357090166,
+            "velocityY": 1.572325727864734,
+            "timestamp": 1.861175778754659
+        },
+        {
+            "x": 7.734511016491924,
+            "y": 4.293948601996246,
+            "heading": 3.168900193264706,
+            "angularVelocity": 0.3300287650543787,
+            "velocityX": 0.6224942265925568,
+            "velocityY": 1.9064869372920104,
+            "timestamp": 1.948744488894043
+        },
+        {
+            "x": 7.731795753069985,
+            "y": 4.489388700342548,
+            "heading": 3.202314872595517,
+            "angularVelocity": 0.38158240857521825,
+            "velocityX": -0.031007233264223562,
+            "velocityY": 2.2318485453904535,
+            "timestamp": 2.0363131990334273
+        },
+        {
+            "x": 7.671228890668297,
+            "y": 4.712045759892295,
+            "heading": 3.2385899184542097,
+            "angularVelocity": 0.41424666186076586,
+            "velocityX": -0.6916495892800459,
+            "velocityY": 2.5426554667225383,
+            "timestamp": 2.1238819091728116
+        },
+        {
+            "x": 7.551691509705012,
+            "y": 4.959414788228928,
+            "heading": 3.2745248408990206,
+            "angularVelocity": 0.4103625871342999,
+            "velocityX": -1.3650695639231716,
+            "velocityY": 2.824856366422358,
+            "timestamp": 2.211450619312196
+        },
+        {
+            "x": 7.370754702688456,
+            "y": 5.224445119410505,
+            "heading": 3.301432833728465,
+            "angularVelocity": 0.30727862482631746,
+            "velocityX": -2.066226700479633,
+            "velocityY": 3.0265414525316805,
+            "timestamp": 2.29901932945158
+        },
+        {
+            "x": 7.135099709363178,
+            "y": 5.457264131297548,
+            "heading": 3.3014328467429856,
+            "angularVelocity": 1.4862067021464798e-7,
+            "velocityX": -2.6910867243583128,
+            "velocityY": 2.658700939142107,
+            "timestamp": 2.3865880395909644
+        },
+        {
+            "x": 6.859107156435953,
+            "y": 5.640474617463791,
+            "heading": 3.3014328522641176,
+            "angularVelocity": 6.304914289451061e-8,
+            "velocityX": -3.1517256847557116,
+            "velocityY": 2.0921912161846987,
+            "timestamp": 2.4741567497303487
+        },
+        {
+            "x": 6.573538412185887,
+            "y": 5.808368822234562,
+            "heading": 3.3014328577109833,
+            "angularVelocity": 6.220104875802801e-8,
+            "velocityX": -3.26108199830194,
+            "velocityY": 1.9172853465984772,
+            "timestamp": 2.561725459869733
+        },
+        {
+            "x": 6.287969619272061,
+            "y": 5.976262944233841,
+            "heading": 3.301432863157849,
+            "angularVelocity": 6.220104736843505e-8,
+            "velocityX": -3.2610825540228303,
+            "velocityY": 1.9172844013808124,
+            "timestamp": 2.6492941700091173
+        },
+        {
+            "x": 6.002400826335374,
+            "y": 6.144157066194225,
+            "heading": 3.3014328686047243,
+            "angularVelocity": 6.2201164432783e-8,
+            "velocityX": -3.2610825542838673,
+            "velocityY": 1.9172844009366552,
+            "timestamp": 2.7368628801485015
+        },
+        {
+            "x": 5.726906238126056,
+            "y": 6.306118202865447,
+            "heading": 3.330104518338368,
+            "angularVelocity": 0.3274188884135285,
+            "velocityX": -3.14603912482906,
+            "velocityY": 1.849532057894034,
+            "timestamp": 2.824431590287886
+        },
+        {
+            "x": 5.5065316534228295,
+            "y": 6.435677156249956,
+            "heading": 3.357685145070299,
+            "angularVelocity": 0.31495983768665414,
+            "velocityX": -2.5165905076419874,
+            "velocityY": 1.47951195327976,
+            "timestamp": 2.91200030042727
+        },
+        {
+            "x": 5.341261355658222,
+            "y": 6.532840911354814,
+            "heading": 3.379877882623065,
+            "angularVelocity": 0.2534322764083417,
+            "velocityX": -1.8873213674330116,
+            "velocityY": 1.1095716146806525,
+            "timestamp": 2.9995690105666544
+        },
+        {
+            "x": 5.231085952105995,
+            "y": 6.597614249467722,
+            "heading": 3.395277171030749,
+            "angularVelocity": 0.17585377680193118,
+            "velocityX": -1.2581594884389657,
+            "velocityY": 0.7396858764940984,
+            "timestamp": 3.0871377207060386
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.09030778386766043,
+            "velocityX": -0.6290597636794472,
+            "velocityY": 0.3698324490646197,
+            "timestamp": 3.174706430845423
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0,
+            "velocityX": -1.4368373863628223e-33,
+            "velocityY": 0,
+            "timestamp": 3.262275140984807
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S2.1.traj
new file mode 100644
index 00000000..9d250387
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S2.1.traj	
@@ -0,0 +1,328 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3211310801785023,
+            "y": 5.537078029621854,
+            "heading": 3.14,
+            "angularVelocity": 6.136916649287371e-17,
+            "velocityX": 0.7513410861003985,
+            "velocityY": -0.24211945152891107,
+            "timestamp": 0.09467215555545593
+        },
+        {
+            "x": 2.4633932375532455,
+            "y": 5.491234089826592,
+            "heading": 3.1399999999999997,
+            "angularVelocity": -6.918142497868558e-18,
+            "velocityX": 1.5026821406998672,
+            "velocityY": -0.48423889290665645,
+            "timestamp": 0.18934431111091185
+        },
+        {
+            "x": 2.676786466159711,
+            "y": 5.422468182536282,
+            "heading": 3.14,
+            "angularVelocity": -1.447784282504661e-16,
+            "velocityX": 2.254023132297508,
+            "velocityY": -0.7263583139820807,
+            "timestamp": 0.28401646666636776
+        },
+        {
+            "x": 2.9613107481043692,
+            "y": 5.330780313517108,
+            "heading": 3.14,
+            "angularVelocity": -2.19469189514941e-16,
+            "velocityX": 3.005363934889942,
+            "velocityY": -0.9684776741506306,
+            "timestamp": 0.3786886222218237
+        },
+        {
+            "x": 3.30218779492529,
+            "y": 5.220932782027199,
+            "heading": 3.14,
+            "angularVelocity": 3.7716439529503614e-16,
+            "velocityX": 3.6006051073934406,
+            "velocityY": -1.160293972873203,
+            "timestamp": 0.47336077777727964
+        },
+        {
+            "x": 3.643064841746281,
+            "y": 5.111085250537267,
+            "heading": 3.14,
+            "angularVelocity": 1.8926244658706998e-16,
+            "velocityX": 3.600605107394178,
+            "velocityY": -1.1602939728734405,
+            "timestamp": 0.5680329333327355
+        },
+        {
+            "x": 3.9839418885672715,
+            "y": 5.001237719047335,
+            "heading": 3.14,
+            "angularVelocity": 3.4257793215779713e-16,
+            "velocityX": 3.6006051073941783,
+            "velocityY": -1.1602939728734407,
+            "timestamp": 0.6627050888881915
+        },
+        {
+            "x": 4.3248189353882625,
+            "y": 4.891390187557403,
+            "heading": 3.14,
+            "angularVelocity": 4.383897524805657e-17,
+            "velocityX": 3.6006051073941783,
+            "velocityY": -1.1602939728734407,
+            "timestamp": 0.7573772444436474
+        },
+        {
+            "x": 4.665695982209253,
+            "y": 4.7815426560674705,
+            "heading": 3.14,
+            "angularVelocity": 1.5398680786578701e-19,
+            "velocityX": 3.600605107394178,
+            "velocityY": -1.160293972873441,
+            "timestamp": 0.8520493999991033
+        },
+        {
+            "x": 5.0065730290302435,
+            "y": 4.671695124577538,
+            "heading": 3.14,
+            "angularVelocity": 4.356845547140055e-17,
+            "velocityX": 3.600605107394178,
+            "velocityY": -1.1602939728734414,
+            "timestamp": 0.9467215555545593
+        },
+        {
+            "x": 5.347450075851235,
+            "y": 4.561847593087606,
+            "heading": 3.14,
+            "angularVelocity": 2.68380689151133e-16,
+            "velocityX": 3.600605107394178,
+            "velocityY": -1.160293972873442,
+            "timestamp": 1.041393711110015
+        },
+        {
+            "x": 5.688327122672225,
+            "y": 4.452000061597674,
+            "heading": 3.14,
+            "angularVelocity": 3.726757040891467e-16,
+            "velocityX": 3.600605107394178,
+            "velocityY": -1.1602939728734432,
+            "timestamp": 1.136065866665471
+        },
+        {
+            "x": 6.029204169493216,
+            "y": 4.342152530107741,
+            "heading": 3.14,
+            "angularVelocity": 1.8883338122910111e-16,
+            "velocityX": 3.6006051073941774,
+            "velocityY": -1.1602939728734463,
+            "timestamp": 1.230738022220927
+        },
+        {
+            "x": 6.370081216318914,
+            "y": 4.2323049986209496,
+            "heading": 3.14,
+            "angularVelocity": 2.0987063063623018e-16,
+            "velocityX": 3.600605107443935,
+            "velocityY": -1.1602939728401256,
+            "timestamp": 1.325410177776383
+        },
+        {
+            "x": 6.710958611265409,
+            "y": 4.122458547431971,
+            "heading": 3.14,
+            "angularVelocity": 1.3456713754312597e-16,
+            "velocityX": 3.600608784563045,
+            "velocityY": -1.1602825619053307,
+            "timestamp": 1.4200823333318389
+        },
+        {
+            "x": 7.047028916950751,
+            "y": 4.077522582297829,
+            "heading": 3.14,
+            "angularVelocity": 3.8640318477602567e-16,
+            "velocityX": 3.549832616713609,
+            "velocityY": -0.4746481673570916,
+            "timestamp": 1.5147544888872948
+        },
+        {
+            "x": 7.31196824975774,
+            "y": 4.055508828572456,
+            "heading": 3.14,
+            "angularVelocity": 5.274988476955795e-16,
+            "velocityX": 2.7984926639997774,
+            "velocityY": -0.23252616987768301,
+            "timestamp": 1.6094266444427507
+        },
+        {
+            "x": 7.5057765524653535,
+            "y": 4.056417169982893,
+            "heading": 3.1400000000000006,
+            "angularVelocity": 4.746542936809208e-16,
+            "velocityX": 2.0471521068735608,
+            "velocityY": 0.009594599437474344,
+            "timestamp": 1.7040987999982067
+        },
+        {
+            "x": 7.628453805780265,
+            "y": 4.080247567089719,
+            "heading": 3.14,
+            "angularVelocity": 5.453879442658063e-16,
+            "velocityX": 1.295811345956418,
+            "velocityY": 0.2517149521631642,
+            "timestamp": 1.7987709555536626
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 4.116008282611605e-16,
+            "velocityX": 0.5444704825542928,
+            "velocityY": 0.49383509476441706,
+            "timestamp": 1.8934431111091186
+        },
+        {
+            "x": 7.6568603335776855,
+            "y": 4.200231008500336,
+            "heading": 3.14,
+            "angularVelocity": 4.411427948098232e-16,
+            "velocityX": -0.23545951737158613,
+            "velocityY": 0.7451679554678371,
+            "timestamp": 1.9917176114589898
+        },
+        {
+            "x": 7.557063724755448,
+            "y": 4.2981314655224265,
+            "heading": 3.14,
+            "angularVelocity": 5.768827951780328e-16,
+            "velocityX": -1.0154883359055142,
+            "velocityY": 0.9961938923479718,
+            "timestamp": 2.0899921118088614
+        },
+        {
+            "x": 7.380593913469942,
+            "y": 4.42065077709202,
+            "heading": 3.14,
+            "angularVelocity": 6.878777879965837e-16,
+            "velocityX": -1.7956826100081775,
+            "velocityY": 1.246705006216337,
+            "timestamp": 2.1882666121587326
+        },
+        {
+            "x": 7.127418097854458,
+            "y": 4.5676865279586805,
+            "heading": 3.1400000000000006,
+            "angularVelocity": 5.966587967087216e-16,
+            "velocityX": -2.576210662116218,
+            "velocityY": 1.4961739855526293,
+            "timestamp": 2.286541112508604
+        },
+        {
+            "x": 6.7974355684839916,
+            "y": 4.73892129278293,
+            "heading": 3.1400000000000006,
+            "angularVelocity": 9.996236488627517e-17,
+            "velocityX": -3.357763490993921,
+            "velocityY": 1.7424129780831201,
+            "timestamp": 2.384815612858475
+        },
+        {
+            "x": 6.443766002914483,
+            "y": 4.853499817181314,
+            "heading": 3.1400000000000006,
+            "angularVelocity": -3.2402717865838093e-16,
+            "velocityX": -3.5987928130938722,
+            "velocityY": 1.1659028943466263,
+            "timestamp": 2.4830901132083465
+        },
+        {
+            "x": 6.090096257463813,
+            "y": 4.968077786339023,
+            "heading": 3.1400000000000006,
+            "angularVelocity": -3.4427620459908776e-16,
+            "velocityX": -3.5987946434889486,
+            "velocityY": 1.165897244450957,
+            "timestamp": 2.581364613558218
+        },
+        {
+            "x": 5.736426512012546,
+            "y": 5.082655755494858,
+            "heading": 3.1400000000000006,
+            "angularVelocity": -3.8345847210221107e-16,
+            "velocityX": -3.598794643495036,
+            "velocityY": 1.165897244431872,
+            "timestamp": 2.679639113908089
+        },
+        {
+            "x": 5.382756766561277,
+            "y": 5.197233724650691,
+            "heading": 3.1400000000000006,
+            "angularVelocity": -2.673892854874729e-16,
+            "velocityX": -3.5987946434950357,
+            "velocityY": 1.165897244431872,
+            "timestamp": 2.7779136142579604
+        },
+        {
+            "x": 5.02908702111004,
+            "y": 5.311811693806516,
+            "heading": 3.1400000000000006,
+            "angularVelocity": -2.8589912756820243e-16,
+            "velocityX": -3.598794643494731,
+            "velocityY": 1.1658972444317732,
+            "timestamp": 2.8761881146078316
+        },
+        {
+            "x": 4.722652230446586,
+            "y": 5.411087010523561,
+            "heading": 3.14,
+            "angularVelocity": 1.1044854210040432e-15,
+            "velocityX": -3.1181516016108115,
+            "velocityY": 1.0101838865993877,
+            "timestamp": 2.974462614957703
+        },
+        {
+            "x": 4.492826119821717,
+            "y": 5.485543503772035,
+            "heading": 3.14,
+            "angularVelocity": 7.043876441844827e-16,
+            "velocityX": -2.338613880575899,
+            "velocityY": 0.7576379730591117,
+            "timestamp": 3.072737115307574
+        },
+        {
+            "x": 4.33960870762911,
+            "y": 5.535181167592957,
+            "heading": 3.14,
+            "angularVelocity": 5.590882375321651e-16,
+            "velocityX": -1.5590759723746228,
+            "velocityY": 0.5050919988827703,
+            "timestamp": 3.1710116156574455
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 3.3318762312703263e-16,
+            "velocityX": -0.7795380017845207,
+            "velocityY": 0.2525460044943946,
+            "timestamp": 3.269286116007317
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -3.7380886634717777e-32,
+            "velocityY": 0,
+            "timestamp": 3.367560616357188
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S2.traj
new file mode 100644
index 00000000..9d250387
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S2.traj	
@@ -0,0 +1,328 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3211310801785023,
+            "y": 5.537078029621854,
+            "heading": 3.14,
+            "angularVelocity": 6.136916649287371e-17,
+            "velocityX": 0.7513410861003985,
+            "velocityY": -0.24211945152891107,
+            "timestamp": 0.09467215555545593
+        },
+        {
+            "x": 2.4633932375532455,
+            "y": 5.491234089826592,
+            "heading": 3.1399999999999997,
+            "angularVelocity": -6.918142497868558e-18,
+            "velocityX": 1.5026821406998672,
+            "velocityY": -0.48423889290665645,
+            "timestamp": 0.18934431111091185
+        },
+        {
+            "x": 2.676786466159711,
+            "y": 5.422468182536282,
+            "heading": 3.14,
+            "angularVelocity": -1.447784282504661e-16,
+            "velocityX": 2.254023132297508,
+            "velocityY": -0.7263583139820807,
+            "timestamp": 0.28401646666636776
+        },
+        {
+            "x": 2.9613107481043692,
+            "y": 5.330780313517108,
+            "heading": 3.14,
+            "angularVelocity": -2.19469189514941e-16,
+            "velocityX": 3.005363934889942,
+            "velocityY": -0.9684776741506306,
+            "timestamp": 0.3786886222218237
+        },
+        {
+            "x": 3.30218779492529,
+            "y": 5.220932782027199,
+            "heading": 3.14,
+            "angularVelocity": 3.7716439529503614e-16,
+            "velocityX": 3.6006051073934406,
+            "velocityY": -1.160293972873203,
+            "timestamp": 0.47336077777727964
+        },
+        {
+            "x": 3.643064841746281,
+            "y": 5.111085250537267,
+            "heading": 3.14,
+            "angularVelocity": 1.8926244658706998e-16,
+            "velocityX": 3.600605107394178,
+            "velocityY": -1.1602939728734405,
+            "timestamp": 0.5680329333327355
+        },
+        {
+            "x": 3.9839418885672715,
+            "y": 5.001237719047335,
+            "heading": 3.14,
+            "angularVelocity": 3.4257793215779713e-16,
+            "velocityX": 3.6006051073941783,
+            "velocityY": -1.1602939728734407,
+            "timestamp": 0.6627050888881915
+        },
+        {
+            "x": 4.3248189353882625,
+            "y": 4.891390187557403,
+            "heading": 3.14,
+            "angularVelocity": 4.383897524805657e-17,
+            "velocityX": 3.6006051073941783,
+            "velocityY": -1.1602939728734407,
+            "timestamp": 0.7573772444436474
+        },
+        {
+            "x": 4.665695982209253,
+            "y": 4.7815426560674705,
+            "heading": 3.14,
+            "angularVelocity": 1.5398680786578701e-19,
+            "velocityX": 3.600605107394178,
+            "velocityY": -1.160293972873441,
+            "timestamp": 0.8520493999991033
+        },
+        {
+            "x": 5.0065730290302435,
+            "y": 4.671695124577538,
+            "heading": 3.14,
+            "angularVelocity": 4.356845547140055e-17,
+            "velocityX": 3.600605107394178,
+            "velocityY": -1.1602939728734414,
+            "timestamp": 0.9467215555545593
+        },
+        {
+            "x": 5.347450075851235,
+            "y": 4.561847593087606,
+            "heading": 3.14,
+            "angularVelocity": 2.68380689151133e-16,
+            "velocityX": 3.600605107394178,
+            "velocityY": -1.160293972873442,
+            "timestamp": 1.041393711110015
+        },
+        {
+            "x": 5.688327122672225,
+            "y": 4.452000061597674,
+            "heading": 3.14,
+            "angularVelocity": 3.726757040891467e-16,
+            "velocityX": 3.600605107394178,
+            "velocityY": -1.1602939728734432,
+            "timestamp": 1.136065866665471
+        },
+        {
+            "x": 6.029204169493216,
+            "y": 4.342152530107741,
+            "heading": 3.14,
+            "angularVelocity": 1.8883338122910111e-16,
+            "velocityX": 3.6006051073941774,
+            "velocityY": -1.1602939728734463,
+            "timestamp": 1.230738022220927
+        },
+        {
+            "x": 6.370081216318914,
+            "y": 4.2323049986209496,
+            "heading": 3.14,
+            "angularVelocity": 2.0987063063623018e-16,
+            "velocityX": 3.600605107443935,
+            "velocityY": -1.1602939728401256,
+            "timestamp": 1.325410177776383
+        },
+        {
+            "x": 6.710958611265409,
+            "y": 4.122458547431971,
+            "heading": 3.14,
+            "angularVelocity": 1.3456713754312597e-16,
+            "velocityX": 3.600608784563045,
+            "velocityY": -1.1602825619053307,
+            "timestamp": 1.4200823333318389
+        },
+        {
+            "x": 7.047028916950751,
+            "y": 4.077522582297829,
+            "heading": 3.14,
+            "angularVelocity": 3.8640318477602567e-16,
+            "velocityX": 3.549832616713609,
+            "velocityY": -0.4746481673570916,
+            "timestamp": 1.5147544888872948
+        },
+        {
+            "x": 7.31196824975774,
+            "y": 4.055508828572456,
+            "heading": 3.14,
+            "angularVelocity": 5.274988476955795e-16,
+            "velocityX": 2.7984926639997774,
+            "velocityY": -0.23252616987768301,
+            "timestamp": 1.6094266444427507
+        },
+        {
+            "x": 7.5057765524653535,
+            "y": 4.056417169982893,
+            "heading": 3.1400000000000006,
+            "angularVelocity": 4.746542936809208e-16,
+            "velocityX": 2.0471521068735608,
+            "velocityY": 0.009594599437474344,
+            "timestamp": 1.7040987999982067
+        },
+        {
+            "x": 7.628453805780265,
+            "y": 4.080247567089719,
+            "heading": 3.14,
+            "angularVelocity": 5.453879442658063e-16,
+            "velocityX": 1.295811345956418,
+            "velocityY": 0.2517149521631642,
+            "timestamp": 1.7987709555536626
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 4.116008282611605e-16,
+            "velocityX": 0.5444704825542928,
+            "velocityY": 0.49383509476441706,
+            "timestamp": 1.8934431111091186
+        },
+        {
+            "x": 7.6568603335776855,
+            "y": 4.200231008500336,
+            "heading": 3.14,
+            "angularVelocity": 4.411427948098232e-16,
+            "velocityX": -0.23545951737158613,
+            "velocityY": 0.7451679554678371,
+            "timestamp": 1.9917176114589898
+        },
+        {
+            "x": 7.557063724755448,
+            "y": 4.2981314655224265,
+            "heading": 3.14,
+            "angularVelocity": 5.768827951780328e-16,
+            "velocityX": -1.0154883359055142,
+            "velocityY": 0.9961938923479718,
+            "timestamp": 2.0899921118088614
+        },
+        {
+            "x": 7.380593913469942,
+            "y": 4.42065077709202,
+            "heading": 3.14,
+            "angularVelocity": 6.878777879965837e-16,
+            "velocityX": -1.7956826100081775,
+            "velocityY": 1.246705006216337,
+            "timestamp": 2.1882666121587326
+        },
+        {
+            "x": 7.127418097854458,
+            "y": 4.5676865279586805,
+            "heading": 3.1400000000000006,
+            "angularVelocity": 5.966587967087216e-16,
+            "velocityX": -2.576210662116218,
+            "velocityY": 1.4961739855526293,
+            "timestamp": 2.286541112508604
+        },
+        {
+            "x": 6.7974355684839916,
+            "y": 4.73892129278293,
+            "heading": 3.1400000000000006,
+            "angularVelocity": 9.996236488627517e-17,
+            "velocityX": -3.357763490993921,
+            "velocityY": 1.7424129780831201,
+            "timestamp": 2.384815612858475
+        },
+        {
+            "x": 6.443766002914483,
+            "y": 4.853499817181314,
+            "heading": 3.1400000000000006,
+            "angularVelocity": -3.2402717865838093e-16,
+            "velocityX": -3.5987928130938722,
+            "velocityY": 1.1659028943466263,
+            "timestamp": 2.4830901132083465
+        },
+        {
+            "x": 6.090096257463813,
+            "y": 4.968077786339023,
+            "heading": 3.1400000000000006,
+            "angularVelocity": -3.4427620459908776e-16,
+            "velocityX": -3.5987946434889486,
+            "velocityY": 1.165897244450957,
+            "timestamp": 2.581364613558218
+        },
+        {
+            "x": 5.736426512012546,
+            "y": 5.082655755494858,
+            "heading": 3.1400000000000006,
+            "angularVelocity": -3.8345847210221107e-16,
+            "velocityX": -3.598794643495036,
+            "velocityY": 1.165897244431872,
+            "timestamp": 2.679639113908089
+        },
+        {
+            "x": 5.382756766561277,
+            "y": 5.197233724650691,
+            "heading": 3.1400000000000006,
+            "angularVelocity": -2.673892854874729e-16,
+            "velocityX": -3.5987946434950357,
+            "velocityY": 1.165897244431872,
+            "timestamp": 2.7779136142579604
+        },
+        {
+            "x": 5.02908702111004,
+            "y": 5.311811693806516,
+            "heading": 3.1400000000000006,
+            "angularVelocity": -2.8589912756820243e-16,
+            "velocityX": -3.598794643494731,
+            "velocityY": 1.1658972444317732,
+            "timestamp": 2.8761881146078316
+        },
+        {
+            "x": 4.722652230446586,
+            "y": 5.411087010523561,
+            "heading": 3.14,
+            "angularVelocity": 1.1044854210040432e-15,
+            "velocityX": -3.1181516016108115,
+            "velocityY": 1.0101838865993877,
+            "timestamp": 2.974462614957703
+        },
+        {
+            "x": 4.492826119821717,
+            "y": 5.485543503772035,
+            "heading": 3.14,
+            "angularVelocity": 7.043876441844827e-16,
+            "velocityX": -2.338613880575899,
+            "velocityY": 0.7576379730591117,
+            "timestamp": 3.072737115307574
+        },
+        {
+            "x": 4.33960870762911,
+            "y": 5.535181167592957,
+            "heading": 3.14,
+            "angularVelocity": 5.590882375321651e-16,
+            "velocityX": -1.5590759723746228,
+            "velocityY": 0.5050919988827703,
+            "timestamp": 3.1710116156574455
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 3.3318762312703263e-16,
+            "velocityX": -0.7795380017845207,
+            "velocityY": 0.2525460044943946,
+            "timestamp": 3.269286116007317
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -3.7380886634717777e-32,
+            "velocityY": 0,
+            "timestamp": 3.367560616357188
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S3.1.traj
new file mode 100644
index 00000000..f9f681f0
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S3.1.traj	
@@ -0,0 +1,346 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -2.1257809849356417e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 2.321508847678769,
+            "y": 5.553282811958262,
+            "heading": 3.1537553335344852,
+            "angularVelocity": 0.14810177732022164,
+            "velocityX": 0.7699258915674908,
+            "velocityY": -0.07232303637577411,
+            "timestamp": 0.09287757232476541
+        },
+        {
+            "x": 2.4645265668919087,
+            "y": 5.539848505148353,
+            "heading": 3.1812640229350246,
+            "angularVelocity": 0.2961822613574502,
+            "velocityX": 1.53985203998495,
+            "velocityY": -0.14464532689263399,
+            "timestamp": 0.18575514464953083
+        },
+        {
+            "x": 2.679053206199872,
+            "y": 5.519697173536265,
+            "heading": 3.2225218511092746,
+            "angularVelocity": 0.4442173405435694,
+            "velocityX": 2.309778711246101,
+            "velocityY": -0.21696660569059611,
+            "timestamp": 0.27863271697429626
+        },
+        {
+            "x": 2.965088840346323,
+            "y": 5.49282892094042,
+            "heading": 3.277521318880573,
+            "angularVelocity": 0.5921716771297765,
+            "velocityX": 3.079706187261968,
+            "velocityY": -0.2892867666899661,
+            "timestamp": 0.37151028929906166
+        },
+        {
+            "x": 3.3148991000673105,
+            "y": 5.459968004407915,
+            "heading": 3.277521325558456,
+            "angularVelocity": 7.189983956229139e-8,
+            "velocityX": 3.7663587770985667,
+            "velocityY": -0.35380895204280205,
+            "timestamp": 0.46438786162382706
+        },
+        {
+            "x": 3.664709359511986,
+            "y": 5.427107084931802,
+            "heading": 3.2775213322361236,
+            "angularVelocity": 7.18975223410498e-8,
+            "velocityX": 3.7663587741235602,
+            "velocityY": -0.3538089837362208,
+            "timestamp": 0.5572654339485925
+        },
+        {
+            "x": 4.014519618956661,
+            "y": 5.394246165455678,
+            "heading": 3.277521338913791,
+            "angularVelocity": 7.189752294324541e-8,
+            "velocityX": 3.7663587741235487,
+            "velocityY": -0.3538089837363427,
+            "timestamp": 0.650143006273358
+        },
+        {
+            "x": 4.364329878401335,
+            "y": 5.361385245979554,
+            "heading": 3.2775213455914582,
+            "angularVelocity": 7.189752233099723e-8,
+            "velocityX": 3.7663587741235482,
+            "velocityY": -0.3538089837363424,
+            "timestamp": 0.7430205785981234
+        },
+        {
+            "x": 4.714140137846011,
+            "y": 5.32852432650343,
+            "heading": 3.277521352269126,
+            "angularVelocity": 7.189752328963948e-8,
+            "velocityX": 3.7663587741235482,
+            "velocityY": -0.35380898373634245,
+            "timestamp": 0.8358981509228889
+        },
+        {
+            "x": 5.063950397290685,
+            "y": 5.295663407027306,
+            "heading": 3.2775213589467933,
+            "angularVelocity": 7.189752245277794e-8,
+            "velocityX": 3.7663587741235482,
+            "velocityY": -0.3538089837363426,
+            "timestamp": 0.9287757232476543
+        },
+        {
+            "x": 5.41376065673536,
+            "y": 5.262802487551181,
+            "heading": 3.2775213656244606,
+            "angularVelocity": 7.189752262506364e-8,
+            "velocityX": 3.7663587741235482,
+            "velocityY": -0.3538089837363426,
+            "timestamp": 1.0216532955724198
+        },
+        {
+            "x": 5.7635709161800355,
+            "y": 5.229941568075057,
+            "heading": 3.277521372302128,
+            "angularVelocity": 7.189752282304632e-8,
+            "velocityX": 3.7663587741235482,
+            "velocityY": -0.3538089837363426,
+            "timestamp": 1.1145308678971853
+        },
+        {
+            "x": 6.113381175624392,
+            "y": 5.19708064859555,
+            "heading": 3.2775213789797957,
+            "angularVelocity": 7.189752355462406e-8,
+            "velocityX": 3.766358774120127,
+            "velocityY": -0.3538089837727691,
+            "timestamp": 1.2074084402219507
+        },
+        {
+            "x": 6.463191352381069,
+            "y": 5.164218848902877,
+            "heading": 3.277521385657466,
+            "angularVelocity": 7.189755308023614e-8,
+            "velocityX": 3.766357883833297,
+            "velocityY": -0.35381846090640534,
+            "timestamp": 1.3002860125467162
+        },
+        {
+            "x": 6.802417095902647,
+            "y": 5.072715014503353,
+            "heading": 3.277521392775139,
+            "angularVelocity": 7.663500778773157e-8,
+            "velocityX": 3.6523967523118,
+            "velocityY": -0.9852091533956361,
+            "timestamp": 1.3931635848714816
+        },
+        {
+            "x": 7.115900861455289,
+            "y": 4.914048826005074,
+            "heading": 3.277521402122426,
+            "angularVelocity": 1.006409437934784e-7,
+            "velocityX": 3.375236429053956,
+            "velocityY": -1.7083369485958357,
+            "timestamp": 1.486041157196247
+        },
+        {
+            "x": 7.364244703869027,
+            "y": 4.725111728374696,
+            "heading": 3.264201738164946,
+            "angularVelocity": -0.1434109831263146,
+            "velocityX": 2.673883868813388,
+            "velocityY": -2.034259648494277,
+            "timestamp": 1.5789187295210125
+        },
+        {
+            "x": 7.541281052388914,
+            "y": 4.527619244510334,
+            "heading": 3.236812571923802,
+            "angularVelocity": -0.29489537199973426,
+            "velocityX": 1.9061259256523482,
+            "velocityY": -2.1263743110531466,
+            "timestamp": 1.671796301845778
+        },
+        {
+            "x": 7.646550613403749,
+            "y": 4.327374608371355,
+            "heading": 3.1953955958101785,
+            "angularVelocity": -0.44593086443733326,
+            "velocityX": 1.1334228315824013,
+            "velocityY": -2.156006354674992,
+            "timestamp": 1.7646738741705434
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": -0.5964367330411746,
+            "velocityX": 0.3601449279842051,
+            "velocityY": -2.1574057477590434,
+            "timestamp": 1.857551446495309
+        },
+        {
+            "x": 7.652622542952261,
+            "y": 3.949180878788953,
+            "heading": 3.079427144579805,
+            "angularVelocity": -0.7304577329243706,
+            "velocityX": -0.33014912487777537,
+            "velocityY": -2.1443491684415465,
+            "timestamp": 1.940475964492665
+        },
+        {
+            "x": 7.56799601367521,
+            "y": 3.773464984873132,
+            "heading": 3.0103677863230933,
+            "angularVelocity": -0.832797825353816,
+            "velocityX": -1.0205248257186026,
+            "velocityY": -2.1189860147444324,
+            "timestamp": 2.0234004824900214
+        },
+        {
+            "x": 7.426167818552031,
+            "y": 3.601500026216296,
+            "heading": 2.937043425915028,
+            "angularVelocity": -0.8842301671310652,
+            "velocityX": -1.7103288454169892,
+            "velocityY": -2.0737528876841793,
+            "timestamp": 2.1063250004873777
+        },
+        {
+            "x": 7.22742147527628,
+            "y": 3.436376202929032,
+            "heading": 2.8673776999748726,
+            "angularVelocity": -0.8401101100446183,
+            "velocityX": -2.3967138799900867,
+            "velocityY": -1.9912545441931753,
+            "timestamp": 2.189249518484734
+        },
+        {
+            "x": 6.9736090926507135,
+            "y": 3.285744578395889,
+            "heading": 2.8218659539774595,
+            "angularVelocity": -0.5488334101485464,
+            "velocityX": -3.0607640388534922,
+            "velocityY": -1.8164908059875007,
+            "timestamp": 2.2721740364820904
+        },
+        {
+            "x": 6.6802459764564,
+            "y": 3.1746372799457343,
+            "heading": 2.8218659065870306,
+            "angularVelocity": -5.714887481984483e-7,
+            "velocityX": -3.5377126485518553,
+            "velocityY": -1.3398606483753976,
+            "timestamp": 2.3550985544794467
+        },
+        {
+            "x": 6.371560257590401,
+            "y": 3.1187813398722177,
+            "heading": 2.821865892471116,
+            "angularVelocity": -1.702260676188045e-7,
+            "velocityX": -3.722490360159113,
+            "velocityY": -0.6735756977845532,
+            "timestamp": 2.438023072476803
+        },
+        {
+            "x": 6.0583631867424375,
+            "y": 3.1010512989609436,
+            "heading": 2.821865878845265,
+            "angularVelocity": -1.6431631700415249e-7,
+            "velocityX": -3.7768934738692015,
+            "velocityY": -0.21380939364446908,
+            "timestamp": 2.5209475904741594
+        },
+        {
+            "x": 5.745166091358163,
+            "y": 3.0833216914831887,
+            "heading": 2.821865865219415,
+            "angularVelocity": -1.6431629349932233e-7,
+            "velocityX": -3.7768937697564957,
+            "velocityY": -0.2138041668004641,
+            "timestamp": 2.6038721084715157
+        },
+        {
+            "x": 5.431968995973753,
+            "y": 3.065592084007832,
+            "heading": 2.8218658515935657,
+            "angularVelocity": -1.6431629385226094e-7,
+            "velocityX": -3.7768937697581326,
+            "velocityY": -0.213804166771547,
+            "timestamp": 2.686796626468872
+        },
+        {
+            "x": 5.118771900355572,
+            "y": 3.047862480662691,
+            "heading": 2.821865837967647,
+            "angularVelocity": -1.6431712462133498e-7,
+            "velocityX": -3.7768937725772025,
+            "velocityY": -0.21380411696461904,
+            "timestamp": 2.7697211444662284
+        },
+        {
+            "x": 4.833327806211659,
+            "y": 3.0319657734270002,
+            "heading": 2.7501020518876387,
+            "angularVelocity": -0.8654109521902384,
+            "velocityX": -3.4422158975107173,
+            "velocityY": -0.1917009301904812,
+            "timestamp": 2.8526456624635848
+        },
+        {
+            "x": 4.6051204329349895,
+            "y": 3.019195659985863,
+            "heading": 2.6830003866678815,
+            "angularVelocity": -0.809189692509236,
+            "velocityX": -2.7519891437167656,
+            "velocityY": -0.15399683651516197,
+            "timestamp": 2.935570180460941
+        },
+        {
+            "x": 4.434032594221504,
+            "y": 3.009603052392577,
+            "heading": 2.6296182647389936,
+            "angularVelocity": -0.6437435298760438,
+            "velocityX": -2.063175558270236,
+            "velocityY": -0.11567878626188524,
+            "timestamp": 3.0184946984582974
+        },
+        {
+            "x": 4.320003715449931,
+            "y": 3.0032022577019606,
+            "heading": 2.5928181043840297,
+            "angularVelocity": -0.44377900823176664,
+            "velocityX": -1.3750924518513103,
+            "velocityY": -0.07718820495068371,
+            "timestamp": 3.1014192164556538
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -0.22693052475300574,
+            "velocityX": -0.6874169042712922,
+            "velocityY": -0.03861653681318734,
+            "timestamp": 3.18434373445301
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 3.2672682524503665
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S3.traj
new file mode 100644
index 00000000..f9f681f0
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S3.traj	
@@ -0,0 +1,346 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -2.1257809849356417e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 2.321508847678769,
+            "y": 5.553282811958262,
+            "heading": 3.1537553335344852,
+            "angularVelocity": 0.14810177732022164,
+            "velocityX": 0.7699258915674908,
+            "velocityY": -0.07232303637577411,
+            "timestamp": 0.09287757232476541
+        },
+        {
+            "x": 2.4645265668919087,
+            "y": 5.539848505148353,
+            "heading": 3.1812640229350246,
+            "angularVelocity": 0.2961822613574502,
+            "velocityX": 1.53985203998495,
+            "velocityY": -0.14464532689263399,
+            "timestamp": 0.18575514464953083
+        },
+        {
+            "x": 2.679053206199872,
+            "y": 5.519697173536265,
+            "heading": 3.2225218511092746,
+            "angularVelocity": 0.4442173405435694,
+            "velocityX": 2.309778711246101,
+            "velocityY": -0.21696660569059611,
+            "timestamp": 0.27863271697429626
+        },
+        {
+            "x": 2.965088840346323,
+            "y": 5.49282892094042,
+            "heading": 3.277521318880573,
+            "angularVelocity": 0.5921716771297765,
+            "velocityX": 3.079706187261968,
+            "velocityY": -0.2892867666899661,
+            "timestamp": 0.37151028929906166
+        },
+        {
+            "x": 3.3148991000673105,
+            "y": 5.459968004407915,
+            "heading": 3.277521325558456,
+            "angularVelocity": 7.189983956229139e-8,
+            "velocityX": 3.7663587770985667,
+            "velocityY": -0.35380895204280205,
+            "timestamp": 0.46438786162382706
+        },
+        {
+            "x": 3.664709359511986,
+            "y": 5.427107084931802,
+            "heading": 3.2775213322361236,
+            "angularVelocity": 7.18975223410498e-8,
+            "velocityX": 3.7663587741235602,
+            "velocityY": -0.3538089837362208,
+            "timestamp": 0.5572654339485925
+        },
+        {
+            "x": 4.014519618956661,
+            "y": 5.394246165455678,
+            "heading": 3.277521338913791,
+            "angularVelocity": 7.189752294324541e-8,
+            "velocityX": 3.7663587741235487,
+            "velocityY": -0.3538089837363427,
+            "timestamp": 0.650143006273358
+        },
+        {
+            "x": 4.364329878401335,
+            "y": 5.361385245979554,
+            "heading": 3.2775213455914582,
+            "angularVelocity": 7.189752233099723e-8,
+            "velocityX": 3.7663587741235482,
+            "velocityY": -0.3538089837363424,
+            "timestamp": 0.7430205785981234
+        },
+        {
+            "x": 4.714140137846011,
+            "y": 5.32852432650343,
+            "heading": 3.277521352269126,
+            "angularVelocity": 7.189752328963948e-8,
+            "velocityX": 3.7663587741235482,
+            "velocityY": -0.35380898373634245,
+            "timestamp": 0.8358981509228889
+        },
+        {
+            "x": 5.063950397290685,
+            "y": 5.295663407027306,
+            "heading": 3.2775213589467933,
+            "angularVelocity": 7.189752245277794e-8,
+            "velocityX": 3.7663587741235482,
+            "velocityY": -0.3538089837363426,
+            "timestamp": 0.9287757232476543
+        },
+        {
+            "x": 5.41376065673536,
+            "y": 5.262802487551181,
+            "heading": 3.2775213656244606,
+            "angularVelocity": 7.189752262506364e-8,
+            "velocityX": 3.7663587741235482,
+            "velocityY": -0.3538089837363426,
+            "timestamp": 1.0216532955724198
+        },
+        {
+            "x": 5.7635709161800355,
+            "y": 5.229941568075057,
+            "heading": 3.277521372302128,
+            "angularVelocity": 7.189752282304632e-8,
+            "velocityX": 3.7663587741235482,
+            "velocityY": -0.3538089837363426,
+            "timestamp": 1.1145308678971853
+        },
+        {
+            "x": 6.113381175624392,
+            "y": 5.19708064859555,
+            "heading": 3.2775213789797957,
+            "angularVelocity": 7.189752355462406e-8,
+            "velocityX": 3.766358774120127,
+            "velocityY": -0.3538089837727691,
+            "timestamp": 1.2074084402219507
+        },
+        {
+            "x": 6.463191352381069,
+            "y": 5.164218848902877,
+            "heading": 3.277521385657466,
+            "angularVelocity": 7.189755308023614e-8,
+            "velocityX": 3.766357883833297,
+            "velocityY": -0.35381846090640534,
+            "timestamp": 1.3002860125467162
+        },
+        {
+            "x": 6.802417095902647,
+            "y": 5.072715014503353,
+            "heading": 3.277521392775139,
+            "angularVelocity": 7.663500778773157e-8,
+            "velocityX": 3.6523967523118,
+            "velocityY": -0.9852091533956361,
+            "timestamp": 1.3931635848714816
+        },
+        {
+            "x": 7.115900861455289,
+            "y": 4.914048826005074,
+            "heading": 3.277521402122426,
+            "angularVelocity": 1.006409437934784e-7,
+            "velocityX": 3.375236429053956,
+            "velocityY": -1.7083369485958357,
+            "timestamp": 1.486041157196247
+        },
+        {
+            "x": 7.364244703869027,
+            "y": 4.725111728374696,
+            "heading": 3.264201738164946,
+            "angularVelocity": -0.1434109831263146,
+            "velocityX": 2.673883868813388,
+            "velocityY": -2.034259648494277,
+            "timestamp": 1.5789187295210125
+        },
+        {
+            "x": 7.541281052388914,
+            "y": 4.527619244510334,
+            "heading": 3.236812571923802,
+            "angularVelocity": -0.29489537199973426,
+            "velocityX": 1.9061259256523482,
+            "velocityY": -2.1263743110531466,
+            "timestamp": 1.671796301845778
+        },
+        {
+            "x": 7.646550613403749,
+            "y": 4.327374608371355,
+            "heading": 3.1953955958101785,
+            "angularVelocity": -0.44593086443733326,
+            "velocityX": 1.1334228315824013,
+            "velocityY": -2.156006354674992,
+            "timestamp": 1.7646738741705434
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": -0.5964367330411746,
+            "velocityX": 0.3601449279842051,
+            "velocityY": -2.1574057477590434,
+            "timestamp": 1.857551446495309
+        },
+        {
+            "x": 7.652622542952261,
+            "y": 3.949180878788953,
+            "heading": 3.079427144579805,
+            "angularVelocity": -0.7304577329243706,
+            "velocityX": -0.33014912487777537,
+            "velocityY": -2.1443491684415465,
+            "timestamp": 1.940475964492665
+        },
+        {
+            "x": 7.56799601367521,
+            "y": 3.773464984873132,
+            "heading": 3.0103677863230933,
+            "angularVelocity": -0.832797825353816,
+            "velocityX": -1.0205248257186026,
+            "velocityY": -2.1189860147444324,
+            "timestamp": 2.0234004824900214
+        },
+        {
+            "x": 7.426167818552031,
+            "y": 3.601500026216296,
+            "heading": 2.937043425915028,
+            "angularVelocity": -0.8842301671310652,
+            "velocityX": -1.7103288454169892,
+            "velocityY": -2.0737528876841793,
+            "timestamp": 2.1063250004873777
+        },
+        {
+            "x": 7.22742147527628,
+            "y": 3.436376202929032,
+            "heading": 2.8673776999748726,
+            "angularVelocity": -0.8401101100446183,
+            "velocityX": -2.3967138799900867,
+            "velocityY": -1.9912545441931753,
+            "timestamp": 2.189249518484734
+        },
+        {
+            "x": 6.9736090926507135,
+            "y": 3.285744578395889,
+            "heading": 2.8218659539774595,
+            "angularVelocity": -0.5488334101485464,
+            "velocityX": -3.0607640388534922,
+            "velocityY": -1.8164908059875007,
+            "timestamp": 2.2721740364820904
+        },
+        {
+            "x": 6.6802459764564,
+            "y": 3.1746372799457343,
+            "heading": 2.8218659065870306,
+            "angularVelocity": -5.714887481984483e-7,
+            "velocityX": -3.5377126485518553,
+            "velocityY": -1.3398606483753976,
+            "timestamp": 2.3550985544794467
+        },
+        {
+            "x": 6.371560257590401,
+            "y": 3.1187813398722177,
+            "heading": 2.821865892471116,
+            "angularVelocity": -1.702260676188045e-7,
+            "velocityX": -3.722490360159113,
+            "velocityY": -0.6735756977845532,
+            "timestamp": 2.438023072476803
+        },
+        {
+            "x": 6.0583631867424375,
+            "y": 3.1010512989609436,
+            "heading": 2.821865878845265,
+            "angularVelocity": -1.6431631700415249e-7,
+            "velocityX": -3.7768934738692015,
+            "velocityY": -0.21380939364446908,
+            "timestamp": 2.5209475904741594
+        },
+        {
+            "x": 5.745166091358163,
+            "y": 3.0833216914831887,
+            "heading": 2.821865865219415,
+            "angularVelocity": -1.6431629349932233e-7,
+            "velocityX": -3.7768937697564957,
+            "velocityY": -0.2138041668004641,
+            "timestamp": 2.6038721084715157
+        },
+        {
+            "x": 5.431968995973753,
+            "y": 3.065592084007832,
+            "heading": 2.8218658515935657,
+            "angularVelocity": -1.6431629385226094e-7,
+            "velocityX": -3.7768937697581326,
+            "velocityY": -0.213804166771547,
+            "timestamp": 2.686796626468872
+        },
+        {
+            "x": 5.118771900355572,
+            "y": 3.047862480662691,
+            "heading": 2.821865837967647,
+            "angularVelocity": -1.6431712462133498e-7,
+            "velocityX": -3.7768937725772025,
+            "velocityY": -0.21380411696461904,
+            "timestamp": 2.7697211444662284
+        },
+        {
+            "x": 4.833327806211659,
+            "y": 3.0319657734270002,
+            "heading": 2.7501020518876387,
+            "angularVelocity": -0.8654109521902384,
+            "velocityX": -3.4422158975107173,
+            "velocityY": -0.1917009301904812,
+            "timestamp": 2.8526456624635848
+        },
+        {
+            "x": 4.6051204329349895,
+            "y": 3.019195659985863,
+            "heading": 2.6830003866678815,
+            "angularVelocity": -0.809189692509236,
+            "velocityX": -2.7519891437167656,
+            "velocityY": -0.15399683651516197,
+            "timestamp": 2.935570180460941
+        },
+        {
+            "x": 4.434032594221504,
+            "y": 3.009603052392577,
+            "heading": 2.6296182647389936,
+            "angularVelocity": -0.6437435298760438,
+            "velocityX": -2.063175558270236,
+            "velocityY": -0.11567878626188524,
+            "timestamp": 3.0184946984582974
+        },
+        {
+            "x": 4.320003715449931,
+            "y": 3.0032022577019606,
+            "heading": 2.5928181043840297,
+            "angularVelocity": -0.44377900823176664,
+            "velocityX": -1.3750924518513103,
+            "velocityY": -0.07718820495068371,
+            "timestamp": 3.1014192164556538
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -0.22693052475300574,
+            "velocityX": -0.6874169042712922,
+            "velocityY": -0.03861653681318734,
+            "timestamp": 3.18434373445301
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 3.2672682524503665
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S4.1.traj
new file mode 100644
index 00000000..f6bf2dd2
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S4.1.traj	
@@ -0,0 +1,355 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -4.116881309267156e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3172206852139876,
+            "y": 5.557554237561194,
+            "heading": 3.1526693404609274,
+            "angularVelocity": 0.14095873188728006,
+            "velocityX": 0.747895486240753,
+            "velocityY": -0.02721148531256728,
+            "timestamp": 0.08987978460983724
+        },
+        {
+            "x": 2.4516878784162954,
+            "y": 5.552665336111155,
+            "heading": 3.1761213228598115,
+            "angularVelocity": 0.2609261081415128,
+            "velocityX": 1.4960782759551832,
+            "velocityY": -0.054393782442455275,
+            "timestamp": 0.17975956921967448
+        },
+        {
+            "x": 2.6534358807362874,
+            "y": 5.545338085116866,
+            "heading": 3.2071661677578067,
+            "angularVelocity": 0.34540408649958604,
+            "velocityX": 2.2446426990871093,
+            "velocityY": -0.08152279209504047,
+            "timestamp": 0.2696393538295117
+        },
+        {
+            "x": 2.9224981140685604,
+            "y": 5.535583877021325,
+            "heading": 3.239227837145004,
+            "angularVelocity": 0.3567172476694272,
+            "velocityX": 2.9935789732947984,
+            "velocityY": -0.10852504974159909,
+            "timestamp": 0.35951913843934896
+        },
+        {
+            "x": 3.258623258854089,
+            "y": 5.523455945202495,
+            "heading": 3.2503847245896496,
+            "angularVelocity": 0.1241312214205911,
+            "velocityX": 3.739719073033272,
+            "velocityY": -0.13493503429591652,
+            "timestamp": 0.4493989230491862
+        },
+        {
+            "x": 3.598405730984745,
+            "y": 5.5110225450854795,
+            "heading": 3.2503847355595754,
+            "angularVelocity": 1.2205109450182653e-7,
+            "velocityX": 3.7804103960154256,
+            "velocityY": -0.13833366614078543,
+            "timestamp": 0.5392787076590234
+        },
+        {
+            "x": 3.938188203087489,
+            "y": 5.498589144205611,
+            "heading": 3.2503847465294977,
+            "angularVelocity": 1.22051053234482e-7,
+            "velocityX": 3.7804103957048714,
+            "velocityY": -0.13833367462827542,
+            "timestamp": 0.6291584922688607
+        },
+        {
+            "x": 4.277970675190232,
+            "y": 5.486155743325738,
+            "heading": 3.25038475749942,
+            "angularVelocity": 1.22051052798414e-7,
+            "velocityX": 3.7804103957048705,
+            "velocityY": -0.13833367462831278,
+            "timestamp": 0.7190382768786979
+        },
+        {
+            "x": 4.617753147292976,
+            "y": 5.473722342445865,
+            "heading": 3.2503847684693423,
+            "angularVelocity": 1.2205105381924992e-7,
+            "velocityX": 3.7804103957048705,
+            "velocityY": -0.1383336746283128,
+            "timestamp": 0.8089180614885352
+        },
+        {
+            "x": 4.957535619395719,
+            "y": 5.461288941565993,
+            "heading": 3.2503847794392646,
+            "angularVelocity": 1.2205105317325576e-7,
+            "velocityX": 3.78041039570487,
+            "velocityY": -0.13833367462831272,
+            "timestamp": 0.8987978460983724
+        },
+        {
+            "x": 5.297318091498462,
+            "y": 5.44885554068612,
+            "heading": 3.2503847904091874,
+            "angularVelocity": 1.2205105386911006e-7,
+            "velocityX": 3.78041039570487,
+            "velocityY": -0.13833367462831295,
+            "timestamp": 0.9886776307082097
+        },
+        {
+            "x": 5.637100563601198,
+            "y": 5.436422139806048,
+            "heading": 3.2503848013791092,
+            "angularVelocity": 1.2205105333670814e-7,
+            "velocityX": 3.7804103957047883,
+            "velocityY": -0.13833367463053942,
+            "timestamp": 1.0785574153180468
+        },
+        {
+            "x": 5.976883034047419,
+            "y": 5.423988693656475,
+            "heading": 3.2503848123490315,
+            "angularVelocity": 1.2205105297878886e-7,
+            "velocityX": 3.7804103772744444,
+            "velocityY": -0.1383341782977477,
+            "timestamp": 1.168437199927884
+        },
+        {
+            "x": 6.3161557778942825,
+            "y": 5.401611852989575,
+            "heading": 3.2503848233905317,
+            "angularVelocity": 1.2284742555590528e-7,
+            "velocityX": 3.774739173214837,
+            "velocityY": -0.24896411094026674,
+            "timestamp": 1.2583169845377211
+        },
+        {
+            "x": 6.6443599765864345,
+            "y": 5.312793385156624,
+            "heading": 3.2503848356259346,
+            "angularVelocity": 1.3613075412636898e-7,
+            "velocityX": 3.6515908456709,
+            "velocityY": -0.9881918188666926,
+            "timestamp": 1.3481967691475583
+        },
+        {
+            "x": 6.948614681552646,
+            "y": 5.161017938085189,
+            "heading": 3.2503848519237777,
+            "angularVelocity": 1.8132935219655535e-7,
+            "velocityX": 3.3851294402514114,
+            "velocityY": -1.6886494302392412,
+            "timestamp": 1.4380765537573954
+        },
+        {
+            "x": 7.216978880977283,
+            "y": 4.952242107786858,
+            "heading": 3.2503848494896554,
+            "angularVelocity": -2.7081974161956603e-8,
+            "velocityX": 2.985812667326646,
+            "velocityY": -2.322834118980229,
+            "timestamp": 1.5279563383672325
+        },
+        {
+            "x": 7.434432646263598,
+            "y": 4.699605663740004,
+            "heading": 3.233827697906493,
+            "angularVelocity": -0.18421441100386476,
+            "velocityX": 2.4193845838667345,
+            "velocityY": -2.8108260955843263,
+            "timestamp": 1.6178361229770697
+        },
+        {
+            "x": 7.589713042543029,
+            "y": 4.421668707909288,
+            "heading": 3.1971997410199715,
+            "angularVelocity": -0.40752163621075216,
+            "velocityX": 1.7276453982782098,
+            "velocityY": -3.0923188905848704,
+            "timestamp": 1.7077159075869068
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": -0.6364027380377996,
+            "velocityX": 1.0045301938460758,
+            "velocityY": -3.2784759018773304,
+            "timestamp": 1.797595692196744
+        },
+        {
+            "x": 7.709767191117991,
+            "y": 3.879341169574254,
+            "heading": 3.0798110703023642,
+            "angularVelocity": -0.8233834909878134,
+            "velocityX": 0.40721464665943,
+            "velocityY": -3.3879684087134527,
+            "timestamp": 1.8706952021534504
+        },
+        {
+            "x": 7.695293406413974,
+            "y": 3.6265437637575832,
+            "heading": 3.0152640841936,
+            "angularVelocity": -0.8830016254143865,
+            "velocityX": -0.19800111810029022,
+            "velocityY": -3.4582640289435425,
+            "timestamp": 1.9437947121101569
+        },
+        {
+            "x": 7.636370047387183,
+            "y": 3.3734118596165046,
+            "heading": 2.9616083332383942,
+            "angularVelocity": -0.7340097216379668,
+            "velocityX": -0.8060705066515889,
+            "velocityY": -3.46283996009006,
+            "timestamp": 2.0168942220668633
+        },
+        {
+            "x": 7.533938934527094,
+            "y": 3.12599493210629,
+            "heading": 2.935877856016127,
+            "angularVelocity": -0.3519924721450196,
+            "velocityX": -1.4012558076073192,
+            "velocityY": -3.3846591811184132,
+            "timestamp": 2.08999373202357
+        },
+        {
+            "x": 7.389145536880011,
+            "y": 2.8904033965878195,
+            "heading": 2.9358731972146472,
+            "angularVelocity": -0.00006373232119512069,
+            "velocityX": -1.9807711123211746,
+            "velocityY": -3.2228880283604715,
+            "timestamp": 2.1630932419802766
+        },
+        {
+            "x": 7.2083948751557925,
+            "y": 2.6811221610888363,
+            "heading": 2.935873140353821,
+            "angularVelocity": -7.778550956603191e-7,
+            "velocityX": -2.4726658472985523,
+            "velocityY": -2.8629635906306268,
+            "timestamp": 2.2361927519369833
+        },
+        {
+            "x": 6.996380296591032,
+            "y": 2.5035855179171795,
+            "heading": 2.9358730970284697,
+            "angularVelocity": -5.92690040120521e-7,
+            "velocityX": -2.9003556753023303,
+            "velocityY": -2.428698130491067,
+            "timestamp": 2.30929226189369
+        },
+        {
+            "x": 6.758605859634312,
+            "y": 2.362402091220058,
+            "heading": 2.935873058757806,
+            "angularVelocity": -5.235419952154551e-7,
+            "velocityX": -3.252750081327982,
+            "velocityY": -1.931386773738272,
+            "timestamp": 2.3823917718503966
+        },
+        {
+            "x": 6.50822939770154,
+            "y": 2.2450092870168694,
+            "heading": 2.935873021282274,
+            "angularVelocity": -5.126646128379785e-7,
+            "velocityX": -3.4251455595402076,
+            "velocityY": -1.6059314798786082,
+            "timestamp": 2.4554912818071033
+        },
+        {
+            "x": 6.2578527752872475,
+            "y": 2.12761682509076,
+            "heading": 2.935872983806744,
+            "angularVelocity": -5.126645909112232e-7,
+            "velocityX": -3.4251477549245686,
+            "velocityY": -1.6059267975342413,
+            "timestamp": 2.52859079176381
+        },
+        {
+            "x": 6.007476152294899,
+            "y": 2.010224364397617,
+            "heading": 2.9358729463311364,
+            "angularVelocity": -5.126656467056276e-7,
+            "velocityX": -3.425147762832367,
+            "velocityY": -1.6059267806672737,
+            "timestamp": 2.6016903017205166
+        },
+        {
+            "x": 5.771581826202875,
+            "y": 1.8997576718980689,
+            "heading": 2.8963919451713847,
+            "angularVelocity": -0.540099395784409,
+            "velocityX": -3.227030198037426,
+            "velocityY": -1.5111823945872924,
+            "timestamp": 2.6747898116772233
+        },
+        {
+            "x": 5.572260449403434,
+            "y": 1.8062657885588822,
+            "heading": 2.7909821338342944,
+            "angularVelocity": -1.4420043499543171,
+            "velocityX": -2.7267129002300594,
+            "velocityY": -1.2789673062725992,
+            "timestamp": 2.74788932163393
+        },
+        {
+            "x": 5.413306384281217,
+            "y": 1.7316311280227,
+            "heading": 2.684821280424371,
+            "angularVelocity": -1.4522785921931365,
+            "velocityX": -2.1744887922827503,
+            "velocityY": -1.0210008327056677,
+            "timestamp": 2.8209888315906366
+        },
+        {
+            "x": 5.294461986790537,
+            "y": 1.6757665506891541,
+            "heading": 2.596152824094934,
+            "angularVelocity": -1.2129829103089709,
+            "velocityX": -1.6257892503119276,
+            "velocityY": -0.7642264273266058,
+            "timestamp": 2.8940883415473433
+        },
+        {
+            "x": 5.2154339068234155,
+            "y": 1.6385760788874908,
+            "heading": 2.5330095835299122,
+            "angularVelocity": -0.8637984112673935,
+            "velocityX": -1.0811027326165235,
+            "velocityY": -0.5087649947815693,
+            "timestamp": 2.96718785150405
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.45157051736003884,
+            "velocityX": -0.5394551460983961,
+            "velocityY": -0.2541204297879725,
+            "timestamp": 3.0402873614607566
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": 1.8516151332127108e-31,
+            "velocityY": 3.1624508891409975e-32,
+            "timestamp": 3.1133868714174633
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S4.traj
new file mode 100644
index 00000000..f6bf2dd2
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S4.traj	
@@ -0,0 +1,355 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -4.116881309267156e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3172206852139876,
+            "y": 5.557554237561194,
+            "heading": 3.1526693404609274,
+            "angularVelocity": 0.14095873188728006,
+            "velocityX": 0.747895486240753,
+            "velocityY": -0.02721148531256728,
+            "timestamp": 0.08987978460983724
+        },
+        {
+            "x": 2.4516878784162954,
+            "y": 5.552665336111155,
+            "heading": 3.1761213228598115,
+            "angularVelocity": 0.2609261081415128,
+            "velocityX": 1.4960782759551832,
+            "velocityY": -0.054393782442455275,
+            "timestamp": 0.17975956921967448
+        },
+        {
+            "x": 2.6534358807362874,
+            "y": 5.545338085116866,
+            "heading": 3.2071661677578067,
+            "angularVelocity": 0.34540408649958604,
+            "velocityX": 2.2446426990871093,
+            "velocityY": -0.08152279209504047,
+            "timestamp": 0.2696393538295117
+        },
+        {
+            "x": 2.9224981140685604,
+            "y": 5.535583877021325,
+            "heading": 3.239227837145004,
+            "angularVelocity": 0.3567172476694272,
+            "velocityX": 2.9935789732947984,
+            "velocityY": -0.10852504974159909,
+            "timestamp": 0.35951913843934896
+        },
+        {
+            "x": 3.258623258854089,
+            "y": 5.523455945202495,
+            "heading": 3.2503847245896496,
+            "angularVelocity": 0.1241312214205911,
+            "velocityX": 3.739719073033272,
+            "velocityY": -0.13493503429591652,
+            "timestamp": 0.4493989230491862
+        },
+        {
+            "x": 3.598405730984745,
+            "y": 5.5110225450854795,
+            "heading": 3.2503847355595754,
+            "angularVelocity": 1.2205109450182653e-7,
+            "velocityX": 3.7804103960154256,
+            "velocityY": -0.13833366614078543,
+            "timestamp": 0.5392787076590234
+        },
+        {
+            "x": 3.938188203087489,
+            "y": 5.498589144205611,
+            "heading": 3.2503847465294977,
+            "angularVelocity": 1.22051053234482e-7,
+            "velocityX": 3.7804103957048714,
+            "velocityY": -0.13833367462827542,
+            "timestamp": 0.6291584922688607
+        },
+        {
+            "x": 4.277970675190232,
+            "y": 5.486155743325738,
+            "heading": 3.25038475749942,
+            "angularVelocity": 1.22051052798414e-7,
+            "velocityX": 3.7804103957048705,
+            "velocityY": -0.13833367462831278,
+            "timestamp": 0.7190382768786979
+        },
+        {
+            "x": 4.617753147292976,
+            "y": 5.473722342445865,
+            "heading": 3.2503847684693423,
+            "angularVelocity": 1.2205105381924992e-7,
+            "velocityX": 3.7804103957048705,
+            "velocityY": -0.1383336746283128,
+            "timestamp": 0.8089180614885352
+        },
+        {
+            "x": 4.957535619395719,
+            "y": 5.461288941565993,
+            "heading": 3.2503847794392646,
+            "angularVelocity": 1.2205105317325576e-7,
+            "velocityX": 3.78041039570487,
+            "velocityY": -0.13833367462831272,
+            "timestamp": 0.8987978460983724
+        },
+        {
+            "x": 5.297318091498462,
+            "y": 5.44885554068612,
+            "heading": 3.2503847904091874,
+            "angularVelocity": 1.2205105386911006e-7,
+            "velocityX": 3.78041039570487,
+            "velocityY": -0.13833367462831295,
+            "timestamp": 0.9886776307082097
+        },
+        {
+            "x": 5.637100563601198,
+            "y": 5.436422139806048,
+            "heading": 3.2503848013791092,
+            "angularVelocity": 1.2205105333670814e-7,
+            "velocityX": 3.7804103957047883,
+            "velocityY": -0.13833367463053942,
+            "timestamp": 1.0785574153180468
+        },
+        {
+            "x": 5.976883034047419,
+            "y": 5.423988693656475,
+            "heading": 3.2503848123490315,
+            "angularVelocity": 1.2205105297878886e-7,
+            "velocityX": 3.7804103772744444,
+            "velocityY": -0.1383341782977477,
+            "timestamp": 1.168437199927884
+        },
+        {
+            "x": 6.3161557778942825,
+            "y": 5.401611852989575,
+            "heading": 3.2503848233905317,
+            "angularVelocity": 1.2284742555590528e-7,
+            "velocityX": 3.774739173214837,
+            "velocityY": -0.24896411094026674,
+            "timestamp": 1.2583169845377211
+        },
+        {
+            "x": 6.6443599765864345,
+            "y": 5.312793385156624,
+            "heading": 3.2503848356259346,
+            "angularVelocity": 1.3613075412636898e-7,
+            "velocityX": 3.6515908456709,
+            "velocityY": -0.9881918188666926,
+            "timestamp": 1.3481967691475583
+        },
+        {
+            "x": 6.948614681552646,
+            "y": 5.161017938085189,
+            "heading": 3.2503848519237777,
+            "angularVelocity": 1.8132935219655535e-7,
+            "velocityX": 3.3851294402514114,
+            "velocityY": -1.6886494302392412,
+            "timestamp": 1.4380765537573954
+        },
+        {
+            "x": 7.216978880977283,
+            "y": 4.952242107786858,
+            "heading": 3.2503848494896554,
+            "angularVelocity": -2.7081974161956603e-8,
+            "velocityX": 2.985812667326646,
+            "velocityY": -2.322834118980229,
+            "timestamp": 1.5279563383672325
+        },
+        {
+            "x": 7.434432646263598,
+            "y": 4.699605663740004,
+            "heading": 3.233827697906493,
+            "angularVelocity": -0.18421441100386476,
+            "velocityX": 2.4193845838667345,
+            "velocityY": -2.8108260955843263,
+            "timestamp": 1.6178361229770697
+        },
+        {
+            "x": 7.589713042543029,
+            "y": 4.421668707909288,
+            "heading": 3.1971997410199715,
+            "angularVelocity": -0.40752163621075216,
+            "velocityX": 1.7276453982782098,
+            "velocityY": -3.0923188905848704,
+            "timestamp": 1.7077159075869068
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": -0.6364027380377996,
+            "velocityX": 1.0045301938460758,
+            "velocityY": -3.2784759018773304,
+            "timestamp": 1.797595692196744
+        },
+        {
+            "x": 7.709767191117991,
+            "y": 3.879341169574254,
+            "heading": 3.0798110703023642,
+            "angularVelocity": -0.8233834909878134,
+            "velocityX": 0.40721464665943,
+            "velocityY": -3.3879684087134527,
+            "timestamp": 1.8706952021534504
+        },
+        {
+            "x": 7.695293406413974,
+            "y": 3.6265437637575832,
+            "heading": 3.0152640841936,
+            "angularVelocity": -0.8830016254143865,
+            "velocityX": -0.19800111810029022,
+            "velocityY": -3.4582640289435425,
+            "timestamp": 1.9437947121101569
+        },
+        {
+            "x": 7.636370047387183,
+            "y": 3.3734118596165046,
+            "heading": 2.9616083332383942,
+            "angularVelocity": -0.7340097216379668,
+            "velocityX": -0.8060705066515889,
+            "velocityY": -3.46283996009006,
+            "timestamp": 2.0168942220668633
+        },
+        {
+            "x": 7.533938934527094,
+            "y": 3.12599493210629,
+            "heading": 2.935877856016127,
+            "angularVelocity": -0.3519924721450196,
+            "velocityX": -1.4012558076073192,
+            "velocityY": -3.3846591811184132,
+            "timestamp": 2.08999373202357
+        },
+        {
+            "x": 7.389145536880011,
+            "y": 2.8904033965878195,
+            "heading": 2.9358731972146472,
+            "angularVelocity": -0.00006373232119512069,
+            "velocityX": -1.9807711123211746,
+            "velocityY": -3.2228880283604715,
+            "timestamp": 2.1630932419802766
+        },
+        {
+            "x": 7.2083948751557925,
+            "y": 2.6811221610888363,
+            "heading": 2.935873140353821,
+            "angularVelocity": -7.778550956603191e-7,
+            "velocityX": -2.4726658472985523,
+            "velocityY": -2.8629635906306268,
+            "timestamp": 2.2361927519369833
+        },
+        {
+            "x": 6.996380296591032,
+            "y": 2.5035855179171795,
+            "heading": 2.9358730970284697,
+            "angularVelocity": -5.92690040120521e-7,
+            "velocityX": -2.9003556753023303,
+            "velocityY": -2.428698130491067,
+            "timestamp": 2.30929226189369
+        },
+        {
+            "x": 6.758605859634312,
+            "y": 2.362402091220058,
+            "heading": 2.935873058757806,
+            "angularVelocity": -5.235419952154551e-7,
+            "velocityX": -3.252750081327982,
+            "velocityY": -1.931386773738272,
+            "timestamp": 2.3823917718503966
+        },
+        {
+            "x": 6.50822939770154,
+            "y": 2.2450092870168694,
+            "heading": 2.935873021282274,
+            "angularVelocity": -5.126646128379785e-7,
+            "velocityX": -3.4251455595402076,
+            "velocityY": -1.6059314798786082,
+            "timestamp": 2.4554912818071033
+        },
+        {
+            "x": 6.2578527752872475,
+            "y": 2.12761682509076,
+            "heading": 2.935872983806744,
+            "angularVelocity": -5.126645909112232e-7,
+            "velocityX": -3.4251477549245686,
+            "velocityY": -1.6059267975342413,
+            "timestamp": 2.52859079176381
+        },
+        {
+            "x": 6.007476152294899,
+            "y": 2.010224364397617,
+            "heading": 2.9358729463311364,
+            "angularVelocity": -5.126656467056276e-7,
+            "velocityX": -3.425147762832367,
+            "velocityY": -1.6059267806672737,
+            "timestamp": 2.6016903017205166
+        },
+        {
+            "x": 5.771581826202875,
+            "y": 1.8997576718980689,
+            "heading": 2.8963919451713847,
+            "angularVelocity": -0.540099395784409,
+            "velocityX": -3.227030198037426,
+            "velocityY": -1.5111823945872924,
+            "timestamp": 2.6747898116772233
+        },
+        {
+            "x": 5.572260449403434,
+            "y": 1.8062657885588822,
+            "heading": 2.7909821338342944,
+            "angularVelocity": -1.4420043499543171,
+            "velocityX": -2.7267129002300594,
+            "velocityY": -1.2789673062725992,
+            "timestamp": 2.74788932163393
+        },
+        {
+            "x": 5.413306384281217,
+            "y": 1.7316311280227,
+            "heading": 2.684821280424371,
+            "angularVelocity": -1.4522785921931365,
+            "velocityX": -2.1744887922827503,
+            "velocityY": -1.0210008327056677,
+            "timestamp": 2.8209888315906366
+        },
+        {
+            "x": 5.294461986790537,
+            "y": 1.6757665506891541,
+            "heading": 2.596152824094934,
+            "angularVelocity": -1.2129829103089709,
+            "velocityX": -1.6257892503119276,
+            "velocityY": -0.7642264273266058,
+            "timestamp": 2.8940883415473433
+        },
+        {
+            "x": 5.2154339068234155,
+            "y": 1.6385760788874908,
+            "heading": 2.5330095835299122,
+            "angularVelocity": -0.8637984112673935,
+            "velocityX": -1.0811027326165235,
+            "velocityY": -0.5087649947815693,
+            "timestamp": 2.96718785150405
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.45157051736003884,
+            "velocityX": -0.5394551460983961,
+            "velocityY": -0.2541204297879725,
+            "timestamp": 3.0402873614607566
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": 1.8516151332127108e-31,
+            "velocityY": 3.1624508891409975e-32,
+            "timestamp": 3.1133868714174633
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S3.1.traj
new file mode 100644
index 00000000..55f7626f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S3.1.traj	
@@ -0,0 +1,364 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.1305523573234936e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3154171108625845,
+            "y": 5.531249621704393,
+            "heading": 3.152034306361113,
+            "angularVelocity": 0.12992542777148963,
+            "velocityX": 0.7062597425523925,
+            "velocityY": -0.3103963856794681,
+            "timestamp": 0.09262471994534183
+        },
+        {
+            "x": 2.4462513478934307,
+            "y": 5.473748850345723,
+            "heading": 3.1761003812465023,
+            "angularVelocity": 0.2598234564119746,
+            "velocityX": 1.4125196503487585,
+            "velocityY": -0.6207929307921308,
+            "timestamp": 0.18524943989068365
+        },
+        {
+            "x": 2.642502734215126,
+            "y": 5.387497655476605,
+            "heading": 3.2121940698357436,
+            "angularVelocity": 0.38967662855597096,
+            "velocityX": 2.1187798077824542,
+            "velocityY": -0.931189804622509,
+            "timestamp": 0.2778741598360255
+        },
+        {
+            "x": 2.9041712871426104,
+            "y": 5.2724959877699655,
+            "heading": 3.2603098134687913,
+            "angularVelocity": 0.5194697879944143,
+            "velocityX": 2.8250401521526483,
+            "velocityY": -1.2415872110005046,
+            "timestamp": 0.3704988797813673
+        },
+        {
+            "x": 3.22495104061126,
+            "y": 5.131513113466323,
+            "heading": 3.260309819428016,
+            "angularVelocity": 6.433730075600531e-8,
+            "velocityX": 3.4632196853922204,
+            "velocityY": -1.5220869157481582,
+            "timestamp": 0.46312359972670913
+        },
+        {
+            "x": 3.545730793052991,
+            "y": 4.99053023682554,
+            "heading": 3.2603098253870204,
+            "angularVelocity": 6.433492130788198e-8,
+            "velocityX": 3.4632196743053516,
+            "velocityY": -1.5220869409805253,
+            "timestamp": 0.555748319672051
+        },
+        {
+            "x": 3.866510545494718,
+            "y": 4.849547360184748,
+            "heading": 3.2603098313460244,
+            "angularVelocity": 6.433492218112887e-8,
+            "velocityX": 3.463219674305308,
+            "velocityY": -1.5220869409806246,
+            "timestamp": 0.6483730396173928
+        },
+        {
+            "x": 4.187290297936445,
+            "y": 4.708564483543956,
+            "heading": 3.2603098373050288,
+            "angularVelocity": 6.433492230011589e-8,
+            "velocityX": 3.463219674305308,
+            "velocityY": -1.5220869409806246,
+            "timestamp": 0.7409977595627346
+        },
+        {
+            "x": 4.508070050378173,
+            "y": 4.567581606903164,
+            "heading": 3.260309843264033,
+            "angularVelocity": 6.433492243162183e-8,
+            "velocityX": 3.463219674305308,
+            "velocityY": -1.5220869409806246,
+            "timestamp": 0.8336224795080764
+        },
+        {
+            "x": 4.828849802819899,
+            "y": 4.426598730262372,
+            "heading": 3.260309849223037,
+            "angularVelocity": 6.433492147964013e-8,
+            "velocityX": 3.463219674305308,
+            "velocityY": -1.5220869409806246,
+            "timestamp": 0.9262471994534183
+        },
+        {
+            "x": 5.149629555261627,
+            "y": 4.28561585362158,
+            "heading": 3.2603098551820415,
+            "angularVelocity": 6.433492220525221e-8,
+            "velocityX": 3.4632196743053076,
+            "velocityY": -1.5220869409806248,
+            "timestamp": 1.01887191939876
+        },
+        {
+            "x": 5.470409307703354,
+            "y": 4.1446329769807875,
+            "heading": 3.260309861141046,
+            "angularVelocity": 6.433492177225282e-8,
+            "velocityX": 3.463219674305308,
+            "velocityY": -1.5220869409806244,
+            "timestamp": 1.111496639344102
+        },
+        {
+            "x": 5.791189060145081,
+            "y": 4.003650100339995,
+            "heading": 3.2603098671000503,
+            "angularVelocity": 6.433492264273842e-8,
+            "velocityX": 3.463219674305308,
+            "velocityY": -1.5220869409806244,
+            "timestamp": 1.2041213592894437
+        },
+        {
+            "x": 6.111968812586809,
+            "y": 3.8626672236992023,
+            "heading": 3.2603098730590543,
+            "angularVelocity": 6.433492200729343e-8,
+            "velocityX": 3.463219674305308,
+            "velocityY": -1.5220869409806248,
+            "timestamp": 1.2967460792347856
+        },
+        {
+            "x": 6.432748565028133,
+            "y": 3.7216843470574936,
+            "heading": 3.2603098790180582,
+            "angularVelocity": 6.43349214222261e-8,
+            "velocityX": 3.4632196743009604,
+            "velocityY": -1.5220869409905164,
+            "timestamp": 1.3893707991801274
+        },
+        {
+            "x": 6.753528214821051,
+            "y": 3.5807012368590723,
+            "heading": 3.260309884977063,
+            "angularVelocity": 6.433492684215086e-8,
+            "velocityX": 3.4632185660826984,
+            "velocityY": -1.5220894625281032,
+            "timestamp": 1.4819955191254692
+        },
+        {
+            "x": 7.055963078223925,
+            "y": 3.403757816572982,
+            "heading": 3.260309891160044,
+            "angularVelocity": 6.675302824117912e-8,
+            "velocityX": 3.2651635932755534,
+            "velocityY": -1.9103261029075773,
+            "timestamp": 1.574620239070811
+        },
+        {
+            "x": 7.316159274039154,
+            "y": 3.1690804957678615,
+            "heading": 3.2603099328635268,
+            "angularVelocity": 4.502413954005259e-7,
+            "velocityX": 2.809144210841027,
+            "velocityY": -2.533635955321695,
+            "timestamp": 1.6672449590161529
+        },
+        {
+            "x": 7.507506683295118,
+            "y": 2.9535313290497767,
+            "heading": 3.2483076831663804,
+            "angularVelocity": -0.12957933588602658,
+            "velocityX": 2.0658352259405257,
+            "velocityY": -2.3271235459095725,
+            "timestamp": 1.7598696789614947
+        },
+        {
+            "x": 7.631583047935508,
+            "y": 2.7620770453961936,
+            "heading": 3.2242582782979583,
+            "angularVelocity": -0.25964348267518605,
+            "velocityX": 1.3395599437559367,
+            "velocityY": -2.066989069079623,
+            "timestamp": 1.8524943989068365
+        },
+        {
+            "x": 7.68897880573514,
+            "y": 2.5963063443763943,
+            "heading": 3.1881570664078476,
+            "angularVelocity": -0.3897578520228113,
+            "velocityX": 0.6196591777389554,
+            "velocityY": -1.7897025882250568,
+            "timestamp": 1.9451191188521784
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -0.5199159191656997,
+            "velocityX": -0.09693746702217505,
+            "velocityY": -1.5039866728730655,
+            "timestamp": 2.03774383879752
+        },
+        {
+            "x": 7.6191836827180115,
+            "y": 2.3540612532810705,
+            "heading": 3.087368523551757,
+            "angularVelocity": -0.6362125445554702,
+            "velocityX": -0.73515140709603,
+            "velocityY": -1.244329940996524,
+            "timestamp": 2.1204700863364088
+        },
+        {
+            "x": 7.505717742673261,
+            "y": 2.2730171165833535,
+            "heading": 3.0268817588529657,
+            "angularVelocity": -0.7311677550750295,
+            "velocityX": -1.3715833054244395,
+            "velocityY": -0.979666540049693,
+            "timestamp": 2.2031963338752973
+        },
+        {
+            "x": 7.339845145495756,
+            "y": 2.2145049491373583,
+            "heading": 2.9612826526647904,
+            "angularVelocity": -0.7929660553905423,
+            "velocityX": -2.005078220180729,
+            "velocityY": -0.7072987012796481,
+            "timestamp": 2.285922581414186
+        },
+        {
+            "x": 7.122035574070675,
+            "y": 2.1796305402319347,
+            "heading": 2.895398132602207,
+            "angularVelocity": -0.7964161559680392,
+            "velocityX": -2.6328955791532933,
+            "velocityY": -0.4215640131510867,
+            "timestamp": 2.3686488289530745
+        },
+        {
+            "x": 6.853520994844272,
+            "y": 2.1707672940791287,
+            "heading": 2.839999953502189,
+            "angularVelocity": -0.6696566174354214,
+            "velocityX": -3.245820851479772,
+            "velocityY": -0.1071394680223975,
+            "timestamp": 2.451375076491963
+        },
+        {
+            "x": 6.54155924782903,
+            "y": 2.195597775343439,
+            "heading": 2.83999969260268,
+            "angularVelocity": -0.0000031537694148635814,
+            "velocityX": -3.7710129045632463,
+            "velocityY": 0.30015239422817563,
+            "timestamp": 2.5341013240308516
+        },
+        {
+            "x": 6.239292092495854,
+            "y": 2.276661405774626,
+            "heading": 2.8399996811990342,
+            "angularVelocity": -1.3784796655157707e-7,
+            "velocityX": -3.6538240803329365,
+            "velocityY": 0.9799021815062957,
+            "timestamp": 2.61682757156974
+        },
+        {
+            "x": 5.945407443635962,
+            "y": 2.3842186409280646,
+            "heading": 2.8399996700710783,
+            "angularVelocity": -1.345154198919157e-7,
+            "velocityX": -3.5524958233085573,
+            "velocityY": 1.3001585150212127,
+            "timestamp": 2.699553819108629
+        },
+        {
+            "x": 5.651522869698407,
+            "y": 2.491776080795775,
+            "heading": 2.8399996589431233,
+            "angularVelocity": -1.3451541092672365e-7,
+            "velocityX": -3.5524949176427194,
+            "velocityY": 1.3001609896200028,
+            "timestamp": 2.7822800666475174
+        },
+        {
+            "x": 5.35763829576127,
+            "y": 2.5993335206646324,
+            "heading": 2.839999647815168,
+            "angularVelocity": -1.3451540998350566e-7,
+            "velocityX": -3.5524949176376506,
+            "velocityY": 1.3001609896338533,
+            "timestamp": 2.865006314186406
+        },
+        {
+            "x": 5.063753722106806,
+            "y": 2.7068909613057275,
+            "heading": 2.8399996366871263,
+            "angularVelocity": -1.3451645746164066e-7,
+            "velocityX": -3.5524949142206896,
+            "velocityY": 1.3001609989687195,
+            "timestamp": 2.9477325617252945
+        },
+        {
+            "x": 4.796835976566279,
+            "y": 2.804592771493776,
+            "heading": 2.7513017358564444,
+            "angularVelocity": -1.0721857145640414,
+            "velocityX": -3.226518227060295,
+            "velocityY": 1.1810255281084718,
+            "timestamp": 3.030458809264183
+        },
+        {
+            "x": 4.583301664019943,
+            "y": 2.882755208599232,
+            "heading": 2.680367864718666,
+            "angularVelocity": -0.8574530242585245,
+            "velocityX": -2.581215985240433,
+            "velocityY": 0.9448323770362348,
+            "timestamp": 3.1131850568030717
+        },
+        {
+            "x": 4.42315083623892,
+            "y": 2.9413775026442943,
+            "heading": 2.627181361737275,
+            "angularVelocity": -0.6429217396375844,
+            "velocityX": -1.9359131176080184,
+            "velocityY": 0.7086299184246748,
+            "timestamp": 3.1959113043419602
+        },
+        {
+            "x": 4.316383597066322,
+            "y": 2.980459172951351,
+            "heading": 2.5917278056853306,
+            "angularVelocity": -0.4285647797003965,
+            "velocityX": -1.2906089947136565,
+            "velocityY": 0.47242164935239944,
+            "timestamp": 3.278637551880849
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -0.21429481226012745,
+            "velocityX": -0.645304225133951,
+            "velocityY": 0.23621072670391596,
+            "timestamp": 3.3613637994197374
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 2.7848791215143805e-32,
+            "velocityY": -1.0794258279039968e-32,
+            "timestamp": 3.444090046958626
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S3.traj
new file mode 100644
index 00000000..55f7626f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S3.traj	
@@ -0,0 +1,364 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.1305523573234936e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3154171108625845,
+            "y": 5.531249621704393,
+            "heading": 3.152034306361113,
+            "angularVelocity": 0.12992542777148963,
+            "velocityX": 0.7062597425523925,
+            "velocityY": -0.3103963856794681,
+            "timestamp": 0.09262471994534183
+        },
+        {
+            "x": 2.4462513478934307,
+            "y": 5.473748850345723,
+            "heading": 3.1761003812465023,
+            "angularVelocity": 0.2598234564119746,
+            "velocityX": 1.4125196503487585,
+            "velocityY": -0.6207929307921308,
+            "timestamp": 0.18524943989068365
+        },
+        {
+            "x": 2.642502734215126,
+            "y": 5.387497655476605,
+            "heading": 3.2121940698357436,
+            "angularVelocity": 0.38967662855597096,
+            "velocityX": 2.1187798077824542,
+            "velocityY": -0.931189804622509,
+            "timestamp": 0.2778741598360255
+        },
+        {
+            "x": 2.9041712871426104,
+            "y": 5.2724959877699655,
+            "heading": 3.2603098134687913,
+            "angularVelocity": 0.5194697879944143,
+            "velocityX": 2.8250401521526483,
+            "velocityY": -1.2415872110005046,
+            "timestamp": 0.3704988797813673
+        },
+        {
+            "x": 3.22495104061126,
+            "y": 5.131513113466323,
+            "heading": 3.260309819428016,
+            "angularVelocity": 6.433730075600531e-8,
+            "velocityX": 3.4632196853922204,
+            "velocityY": -1.5220869157481582,
+            "timestamp": 0.46312359972670913
+        },
+        {
+            "x": 3.545730793052991,
+            "y": 4.99053023682554,
+            "heading": 3.2603098253870204,
+            "angularVelocity": 6.433492130788198e-8,
+            "velocityX": 3.4632196743053516,
+            "velocityY": -1.5220869409805253,
+            "timestamp": 0.555748319672051
+        },
+        {
+            "x": 3.866510545494718,
+            "y": 4.849547360184748,
+            "heading": 3.2603098313460244,
+            "angularVelocity": 6.433492218112887e-8,
+            "velocityX": 3.463219674305308,
+            "velocityY": -1.5220869409806246,
+            "timestamp": 0.6483730396173928
+        },
+        {
+            "x": 4.187290297936445,
+            "y": 4.708564483543956,
+            "heading": 3.2603098373050288,
+            "angularVelocity": 6.433492230011589e-8,
+            "velocityX": 3.463219674305308,
+            "velocityY": -1.5220869409806246,
+            "timestamp": 0.7409977595627346
+        },
+        {
+            "x": 4.508070050378173,
+            "y": 4.567581606903164,
+            "heading": 3.260309843264033,
+            "angularVelocity": 6.433492243162183e-8,
+            "velocityX": 3.463219674305308,
+            "velocityY": -1.5220869409806246,
+            "timestamp": 0.8336224795080764
+        },
+        {
+            "x": 4.828849802819899,
+            "y": 4.426598730262372,
+            "heading": 3.260309849223037,
+            "angularVelocity": 6.433492147964013e-8,
+            "velocityX": 3.463219674305308,
+            "velocityY": -1.5220869409806246,
+            "timestamp": 0.9262471994534183
+        },
+        {
+            "x": 5.149629555261627,
+            "y": 4.28561585362158,
+            "heading": 3.2603098551820415,
+            "angularVelocity": 6.433492220525221e-8,
+            "velocityX": 3.4632196743053076,
+            "velocityY": -1.5220869409806248,
+            "timestamp": 1.01887191939876
+        },
+        {
+            "x": 5.470409307703354,
+            "y": 4.1446329769807875,
+            "heading": 3.260309861141046,
+            "angularVelocity": 6.433492177225282e-8,
+            "velocityX": 3.463219674305308,
+            "velocityY": -1.5220869409806244,
+            "timestamp": 1.111496639344102
+        },
+        {
+            "x": 5.791189060145081,
+            "y": 4.003650100339995,
+            "heading": 3.2603098671000503,
+            "angularVelocity": 6.433492264273842e-8,
+            "velocityX": 3.463219674305308,
+            "velocityY": -1.5220869409806244,
+            "timestamp": 1.2041213592894437
+        },
+        {
+            "x": 6.111968812586809,
+            "y": 3.8626672236992023,
+            "heading": 3.2603098730590543,
+            "angularVelocity": 6.433492200729343e-8,
+            "velocityX": 3.463219674305308,
+            "velocityY": -1.5220869409806248,
+            "timestamp": 1.2967460792347856
+        },
+        {
+            "x": 6.432748565028133,
+            "y": 3.7216843470574936,
+            "heading": 3.2603098790180582,
+            "angularVelocity": 6.43349214222261e-8,
+            "velocityX": 3.4632196743009604,
+            "velocityY": -1.5220869409905164,
+            "timestamp": 1.3893707991801274
+        },
+        {
+            "x": 6.753528214821051,
+            "y": 3.5807012368590723,
+            "heading": 3.260309884977063,
+            "angularVelocity": 6.433492684215086e-8,
+            "velocityX": 3.4632185660826984,
+            "velocityY": -1.5220894625281032,
+            "timestamp": 1.4819955191254692
+        },
+        {
+            "x": 7.055963078223925,
+            "y": 3.403757816572982,
+            "heading": 3.260309891160044,
+            "angularVelocity": 6.675302824117912e-8,
+            "velocityX": 3.2651635932755534,
+            "velocityY": -1.9103261029075773,
+            "timestamp": 1.574620239070811
+        },
+        {
+            "x": 7.316159274039154,
+            "y": 3.1690804957678615,
+            "heading": 3.2603099328635268,
+            "angularVelocity": 4.502413954005259e-7,
+            "velocityX": 2.809144210841027,
+            "velocityY": -2.533635955321695,
+            "timestamp": 1.6672449590161529
+        },
+        {
+            "x": 7.507506683295118,
+            "y": 2.9535313290497767,
+            "heading": 3.2483076831663804,
+            "angularVelocity": -0.12957933588602658,
+            "velocityX": 2.0658352259405257,
+            "velocityY": -2.3271235459095725,
+            "timestamp": 1.7598696789614947
+        },
+        {
+            "x": 7.631583047935508,
+            "y": 2.7620770453961936,
+            "heading": 3.2242582782979583,
+            "angularVelocity": -0.25964348267518605,
+            "velocityX": 1.3395599437559367,
+            "velocityY": -2.066989069079623,
+            "timestamp": 1.8524943989068365
+        },
+        {
+            "x": 7.68897880573514,
+            "y": 2.5963063443763943,
+            "heading": 3.1881570664078476,
+            "angularVelocity": -0.3897578520228113,
+            "velocityX": 0.6196591777389554,
+            "velocityY": -1.7897025882250568,
+            "timestamp": 1.9451191188521784
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -0.5199159191656997,
+            "velocityX": -0.09693746702217505,
+            "velocityY": -1.5039866728730655,
+            "timestamp": 2.03774383879752
+        },
+        {
+            "x": 7.6191836827180115,
+            "y": 2.3540612532810705,
+            "heading": 3.087368523551757,
+            "angularVelocity": -0.6362125445554702,
+            "velocityX": -0.73515140709603,
+            "velocityY": -1.244329940996524,
+            "timestamp": 2.1204700863364088
+        },
+        {
+            "x": 7.505717742673261,
+            "y": 2.2730171165833535,
+            "heading": 3.0268817588529657,
+            "angularVelocity": -0.7311677550750295,
+            "velocityX": -1.3715833054244395,
+            "velocityY": -0.979666540049693,
+            "timestamp": 2.2031963338752973
+        },
+        {
+            "x": 7.339845145495756,
+            "y": 2.2145049491373583,
+            "heading": 2.9612826526647904,
+            "angularVelocity": -0.7929660553905423,
+            "velocityX": -2.005078220180729,
+            "velocityY": -0.7072987012796481,
+            "timestamp": 2.285922581414186
+        },
+        {
+            "x": 7.122035574070675,
+            "y": 2.1796305402319347,
+            "heading": 2.895398132602207,
+            "angularVelocity": -0.7964161559680392,
+            "velocityX": -2.6328955791532933,
+            "velocityY": -0.4215640131510867,
+            "timestamp": 2.3686488289530745
+        },
+        {
+            "x": 6.853520994844272,
+            "y": 2.1707672940791287,
+            "heading": 2.839999953502189,
+            "angularVelocity": -0.6696566174354214,
+            "velocityX": -3.245820851479772,
+            "velocityY": -0.1071394680223975,
+            "timestamp": 2.451375076491963
+        },
+        {
+            "x": 6.54155924782903,
+            "y": 2.195597775343439,
+            "heading": 2.83999969260268,
+            "angularVelocity": -0.0000031537694148635814,
+            "velocityX": -3.7710129045632463,
+            "velocityY": 0.30015239422817563,
+            "timestamp": 2.5341013240308516
+        },
+        {
+            "x": 6.239292092495854,
+            "y": 2.276661405774626,
+            "heading": 2.8399996811990342,
+            "angularVelocity": -1.3784796655157707e-7,
+            "velocityX": -3.6538240803329365,
+            "velocityY": 0.9799021815062957,
+            "timestamp": 2.61682757156974
+        },
+        {
+            "x": 5.945407443635962,
+            "y": 2.3842186409280646,
+            "heading": 2.8399996700710783,
+            "angularVelocity": -1.345154198919157e-7,
+            "velocityX": -3.5524958233085573,
+            "velocityY": 1.3001585150212127,
+            "timestamp": 2.699553819108629
+        },
+        {
+            "x": 5.651522869698407,
+            "y": 2.491776080795775,
+            "heading": 2.8399996589431233,
+            "angularVelocity": -1.3451541092672365e-7,
+            "velocityX": -3.5524949176427194,
+            "velocityY": 1.3001609896200028,
+            "timestamp": 2.7822800666475174
+        },
+        {
+            "x": 5.35763829576127,
+            "y": 2.5993335206646324,
+            "heading": 2.839999647815168,
+            "angularVelocity": -1.3451540998350566e-7,
+            "velocityX": -3.5524949176376506,
+            "velocityY": 1.3001609896338533,
+            "timestamp": 2.865006314186406
+        },
+        {
+            "x": 5.063753722106806,
+            "y": 2.7068909613057275,
+            "heading": 2.8399996366871263,
+            "angularVelocity": -1.3451645746164066e-7,
+            "velocityX": -3.5524949142206896,
+            "velocityY": 1.3001609989687195,
+            "timestamp": 2.9477325617252945
+        },
+        {
+            "x": 4.796835976566279,
+            "y": 2.804592771493776,
+            "heading": 2.7513017358564444,
+            "angularVelocity": -1.0721857145640414,
+            "velocityX": -3.226518227060295,
+            "velocityY": 1.1810255281084718,
+            "timestamp": 3.030458809264183
+        },
+        {
+            "x": 4.583301664019943,
+            "y": 2.882755208599232,
+            "heading": 2.680367864718666,
+            "angularVelocity": -0.8574530242585245,
+            "velocityX": -2.581215985240433,
+            "velocityY": 0.9448323770362348,
+            "timestamp": 3.1131850568030717
+        },
+        {
+            "x": 4.42315083623892,
+            "y": 2.9413775026442943,
+            "heading": 2.627181361737275,
+            "angularVelocity": -0.6429217396375844,
+            "velocityX": -1.9359131176080184,
+            "velocityY": 0.7086299184246748,
+            "timestamp": 3.1959113043419602
+        },
+        {
+            "x": 4.316383597066322,
+            "y": 2.980459172951351,
+            "heading": 2.5917278056853306,
+            "angularVelocity": -0.4285647797003965,
+            "velocityX": -1.2906089947136565,
+            "velocityY": 0.47242164935239944,
+            "timestamp": 3.278637551880849
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -0.21429481226012745,
+            "velocityX": -0.645304225133951,
+            "velocityY": 0.23621072670391596,
+            "timestamp": 3.3613637994197374
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 2.7848791215143805e-32,
+            "velocityY": -1.0794258279039968e-32,
+            "timestamp": 3.444090046958626
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S4.1.traj
new file mode 100644
index 00000000..b408904c
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S4.1.traj	
@@ -0,0 +1,346 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3154271184850326,
+            "y": 5.536626406530092,
+            "heading": 3.157504964629553,
+            "angularVelocity": 0.19152994653168934,
+            "velocityX": 0.7158684847619835,
+            "velocityY": -0.25574133980197417,
+            "timestamp": 0.09139544466297708
+        },
+        {
+            "x": 2.4462968906270097,
+            "y": 5.489871788853177,
+            "heading": 3.1916196887834967,
+            "angularVelocity": 0.3732650383145345,
+            "velocityX": 1.4319069470536998,
+            "velocityY": -0.5115639827490647,
+            "timestamp": 0.18279088932595416
+        },
+        {
+            "x": 2.6426368598266095,
+            "y": 5.419722773289121,
+            "heading": 3.240625955812914,
+            "angularVelocity": 0.536200323878483,
+            "velocityX": 2.148246774482153,
+            "velocityY": -0.7675329533406525,
+            "timestamp": 0.2741863339889312
+        },
+        {
+            "x": 2.9045066521861433,
+            "y": 5.326149233940284,
+            "heading": 3.2998389732795825,
+            "angularVelocity": 0.6478771199673835,
+            "velocityX": 2.8652389987837097,
+            "velocityY": -1.023831545367414,
+            "timestamp": 0.36558177865190833
+        },
+        {
+            "x": 3.230136445009879,
+            "y": 5.209943279570635,
+            "heading": 3.2998389932105203,
+            "angularVelocity": 2.1807364619110316e-7,
+            "velocityX": 3.5628667711449338,
+            "velocityY": -1.2714633075877884,
+            "timestamp": 0.45697722331488544
+        },
+        {
+            "x": 3.555730781054687,
+            "y": 5.093637996752385,
+            "heading": 3.29983900348091,
+            "angularVelocity": 1.1237311095674653e-7,
+            "velocityX": 3.5624788220621486,
+            "velocityY": -1.2725501062676312,
+            "timestamp": 0.5483726679778625
+        },
+        {
+            "x": 3.881325116955394,
+            "y": 4.977332713530725,
+            "heading": 3.2998390137513005,
+            "angularVelocity": 1.123731101511981e-7,
+            "velocityX": 3.562478820485468,
+            "velocityY": -1.2725501106815165,
+            "timestamp": 0.6397681126408397
+        },
+        {
+            "x": 4.2069194528561,
+            "y": 4.861027430309063,
+            "heading": 3.2998390240216913,
+            "angularVelocity": 1.1237311074592376e-7,
+            "velocityX": 3.562478820485462,
+            "velocityY": -1.2725501106815353,
+            "timestamp": 0.7311635573038168
+        },
+        {
+            "x": 4.532513788756806,
+            "y": 4.744722147087402,
+            "heading": 3.2998390342920816,
+            "angularVelocity": 1.1237311063100021e-7,
+            "velocityX": 3.562478820485462,
+            "velocityY": -1.272550110681535,
+            "timestamp": 0.8225590019667939
+        },
+        {
+            "x": 4.858108124657512,
+            "y": 4.62841686386574,
+            "heading": 3.299839044562472,
+            "angularVelocity": 1.1237311003038478e-7,
+            "velocityX": 3.5624788204854614,
+            "velocityY": -1.272550110681535,
+            "timestamp": 0.913954446629771
+        },
+        {
+            "x": 5.183702460558219,
+            "y": 4.5121115806440795,
+            "heading": 3.2998390548328627,
+            "angularVelocity": 1.1237311094761516e-7,
+            "velocityX": 3.5624788204854614,
+            "velocityY": -1.272550110681535,
+            "timestamp": 1.005349891292748
+        },
+        {
+            "x": 5.509296796458925,
+            "y": 4.395806297422418,
+            "heading": 3.299839065103253,
+            "angularVelocity": 1.1237310968464263e-7,
+            "velocityX": 3.562478820485462,
+            "velocityY": -1.2725501106815353,
+            "timestamp": 1.096745335955725
+        },
+        {
+            "x": 5.834891132359631,
+            "y": 4.279501014200756,
+            "heading": 3.2998390753736433,
+            "angularVelocity": 1.123731107718446e-7,
+            "velocityX": 3.562478820485462,
+            "velocityY": -1.2725501106815351,
+            "timestamp": 1.188140780618702
+        },
+        {
+            "x": 6.160485468260297,
+            "y": 4.16319573097898,
+            "heading": 3.299839085644034,
+            "angularVelocity": 1.1237310986388625e-7,
+            "velocityX": 3.562478820485015,
+            "velocityY": -1.2725501106827837,
+            "timestamp": 1.279536225281679
+        },
+        {
+            "x": 6.4860797940952795,
+            "y": 4.046890419578527,
+            "heading": 3.2998390959144244,
+            "angularVelocity": 1.1237311101180894e-7,
+            "velocityX": 3.5624787103517215,
+            "velocityY": -1.2725504189987695,
+            "timestamp": 1.370931669944656
+        },
+        {
+            "x": 6.809159340398316,
+            "y": 3.9237717890604613,
+            "heading": 3.299839106230886,
+            "angularVelocity": 1.1287719938305865e-7,
+            "velocityX": 3.5349633397413522,
+            "velocityY": -1.3470981072657797,
+            "timestamp": 1.462327114607633
+        },
+        {
+            "x": 7.101008389413158,
+            "y": 3.738399942122118,
+            "heading": 3.299839117623808,
+            "angularVelocity": 1.246552472151185e-7,
+            "velocityX": 3.1932559668706793,
+            "velocityY": -2.0282394557178747,
+            "timestamp": 1.55372255927061
+        },
+        {
+            "x": 7.349782731781937,
+            "y": 3.498296000909773,
+            "heading": 3.299839132467128,
+            "angularVelocity": 1.6240765120244838e-7,
+            "velocityX": 2.7219555994956917,
+            "velocityY": -2.6270887142977735,
+            "timestamp": 1.645118003933587
+        },
+        {
+            "x": 7.5357710820320545,
+            "y": 3.2282223692532166,
+            "heading": 3.2881158704604148,
+            "angularVelocity": -0.12826965337097124,
+            "velocityX": 2.0349849047286135,
+            "velocityY": -2.955001014026776,
+            "timestamp": 1.736513448596564
+        },
+        {
+            "x": 7.652314357234611,
+            "y": 2.959723163992482,
+            "heading": 3.2581736625054765,
+            "angularVelocity": -0.32761160105108156,
+            "velocityX": 1.2751540914573078,
+            "velocityY": -2.9377744837371598,
+            "timestamp": 1.827908893259541
+        },
+        {
+            "x": 7.700165626970879,
+            "y": 2.701389747494563,
+            "heading": 3.208935276055406,
+            "angularVelocity": -0.5387400502456196,
+            "velocityX": 0.5235629621664141,
+            "velocityY": -2.826545868347203,
+            "timestamp": 1.919304337922518
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -0.7542528657692632,
+            "velocityX": -0.2206414887004817,
+            "velocityY": -2.6739817109674653,
+            "timestamp": 2.0106997825854953
+        },
+        {
+            "x": 7.619243737534899,
+            "y": 2.269397277536314,
+            "heading": 3.071018168568731,
+            "angularVelocity": -0.9308752015312701,
+            "velocityX": -0.8198752757491441,
+            "velocityY": -2.531604604539154,
+            "timestamp": 2.08480405705527
+        },
+        {
+            "x": 7.514288335022884,
+            "y": 2.093479496062301,
+            "heading": 2.9929449451014944,
+            "angularVelocity": -1.0535589751865693,
+            "velocityX": -1.4163204924814625,
+            "velocityY": -2.373922189141621,
+            "timestamp": 2.1589083315250446
+        },
+        {
+            "x": 7.365556927410938,
+            "y": 1.9309603366701282,
+            "heading": 2.911854592478602,
+            "angularVelocity": -1.0942736192096787,
+            "velocityX": -2.0070557154244444,
+            "velocityY": -2.193114507294498,
+            "timestamp": 2.233012605994819
+        },
+        {
+            "x": 7.174068652738112,
+            "y": 1.784693946040825,
+            "heading": 2.8381473535529165,
+            "angularVelocity": -0.994642204556963,
+            "velocityX": -2.5840381819125056,
+            "velocityY": -1.973791548137795,
+            "timestamp": 2.307116880464594
+        },
+        {
+            "x": 6.9430736895852,
+            "y": 1.6600434204320091,
+            "heading": 2.7943942143969487,
+            "angularVelocity": -0.5904266585027068,
+            "velocityX": -3.117161119324147,
+            "velocityY": -1.6820962960737031,
+            "timestamp": 2.3812211549343685
+        },
+        {
+            "x": 6.679335807473864,
+            "y": 1.5650263715806954,
+            "heading": 2.794393527099141,
+            "angularVelocity": -0.000009274738771364558,
+            "velocityX": -3.5590103809586133,
+            "velocityY": -1.2822073966879628,
+            "timestamp": 2.455325429404143
+        },
+        {
+            "x": 6.403647884167786,
+            "y": 1.5142107470678074,
+            "heading": 2.794393497300607,
+            "angularVelocity": -4.021162732159612e-7,
+            "velocityX": -3.720270190601743,
+            "velocityY": -0.6857313545879138,
+            "timestamp": 2.5294297038739177
+        },
+        {
+            "x": 6.123365156631299,
+            "y": 1.5089521104338168,
+            "heading": 2.7943934713550327,
+            "angularVelocity": -3.501225017023653e-7,
+            "velocityX": -3.782274768114868,
+            "velocityY": -0.07096266270252378,
+            "timestamp": 2.6035339783436924
+        },
+        {
+            "x": 5.853614932127584,
+            "y": 1.5409906174836003,
+            "heading": 2.7698594509818024,
+            "angularVelocity": -0.33107429427995533,
+            "velocityX": -3.6401439246765253,
+            "velocityY": 0.43234357638746057,
+            "timestamp": 2.677638252813467
+        },
+        {
+            "x": 5.6273847181633965,
+            "y": 1.5674202114884777,
+            "heading": 2.697593854639993,
+            "angularVelocity": -0.9751879612723551,
+            "velocityX": -3.0528632198735024,
+            "velocityY": 0.3566541092809186,
+            "timestamp": 2.7517425272832416
+        },
+        {
+            "x": 5.446628200819817,
+            "y": 1.588490932967887,
+            "heading": 2.6254372460611823,
+            "angularVelocity": -0.9737172260984721,
+            "velocityX": -2.4392185017249606,
+            "velocityY": 0.284338813519285,
+            "timestamp": 2.8258468017530163
+        },
+        {
+            "x": 5.311228363745466,
+            "y": 1.6042594522545108,
+            "heading": 2.5652453697807402,
+            "angularVelocity": -0.8122591673836874,
+            "velocityX": -1.8271528605219525,
+            "velocityY": 0.21278825546128166,
+            "timestamp": 2.899951076222791
+        },
+        {
+            "x": 5.22105179084387,
+            "y": 1.614756634238219,
+            "heading": 2.5224127669101155,
+            "angularVelocity": -0.578004483237462,
+            "velocityX": -1.2168876025954114,
+            "velocityY": 0.14165420360473746,
+            "timestamp": 2.9740553506925655
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.302449043195724,
+            "velocityX": -0.6079513113949444,
+            "velocityY": 0.07075658994444238,
+            "timestamp": 3.04815962516234
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": 8.84173108800433e-33,
+            "velocityY": 0,
+            "timestamp": 3.122263899632115
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S4.traj
new file mode 100644
index 00000000..b408904c
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S4.traj	
@@ -0,0 +1,346 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3154271184850326,
+            "y": 5.536626406530092,
+            "heading": 3.157504964629553,
+            "angularVelocity": 0.19152994653168934,
+            "velocityX": 0.7158684847619835,
+            "velocityY": -0.25574133980197417,
+            "timestamp": 0.09139544466297708
+        },
+        {
+            "x": 2.4462968906270097,
+            "y": 5.489871788853177,
+            "heading": 3.1916196887834967,
+            "angularVelocity": 0.3732650383145345,
+            "velocityX": 1.4319069470536998,
+            "velocityY": -0.5115639827490647,
+            "timestamp": 0.18279088932595416
+        },
+        {
+            "x": 2.6426368598266095,
+            "y": 5.419722773289121,
+            "heading": 3.240625955812914,
+            "angularVelocity": 0.536200323878483,
+            "velocityX": 2.148246774482153,
+            "velocityY": -0.7675329533406525,
+            "timestamp": 0.2741863339889312
+        },
+        {
+            "x": 2.9045066521861433,
+            "y": 5.326149233940284,
+            "heading": 3.2998389732795825,
+            "angularVelocity": 0.6478771199673835,
+            "velocityX": 2.8652389987837097,
+            "velocityY": -1.023831545367414,
+            "timestamp": 0.36558177865190833
+        },
+        {
+            "x": 3.230136445009879,
+            "y": 5.209943279570635,
+            "heading": 3.2998389932105203,
+            "angularVelocity": 2.1807364619110316e-7,
+            "velocityX": 3.5628667711449338,
+            "velocityY": -1.2714633075877884,
+            "timestamp": 0.45697722331488544
+        },
+        {
+            "x": 3.555730781054687,
+            "y": 5.093637996752385,
+            "heading": 3.29983900348091,
+            "angularVelocity": 1.1237311095674653e-7,
+            "velocityX": 3.5624788220621486,
+            "velocityY": -1.2725501062676312,
+            "timestamp": 0.5483726679778625
+        },
+        {
+            "x": 3.881325116955394,
+            "y": 4.977332713530725,
+            "heading": 3.2998390137513005,
+            "angularVelocity": 1.123731101511981e-7,
+            "velocityX": 3.562478820485468,
+            "velocityY": -1.2725501106815165,
+            "timestamp": 0.6397681126408397
+        },
+        {
+            "x": 4.2069194528561,
+            "y": 4.861027430309063,
+            "heading": 3.2998390240216913,
+            "angularVelocity": 1.1237311074592376e-7,
+            "velocityX": 3.562478820485462,
+            "velocityY": -1.2725501106815353,
+            "timestamp": 0.7311635573038168
+        },
+        {
+            "x": 4.532513788756806,
+            "y": 4.744722147087402,
+            "heading": 3.2998390342920816,
+            "angularVelocity": 1.1237311063100021e-7,
+            "velocityX": 3.562478820485462,
+            "velocityY": -1.272550110681535,
+            "timestamp": 0.8225590019667939
+        },
+        {
+            "x": 4.858108124657512,
+            "y": 4.62841686386574,
+            "heading": 3.299839044562472,
+            "angularVelocity": 1.1237311003038478e-7,
+            "velocityX": 3.5624788204854614,
+            "velocityY": -1.272550110681535,
+            "timestamp": 0.913954446629771
+        },
+        {
+            "x": 5.183702460558219,
+            "y": 4.5121115806440795,
+            "heading": 3.2998390548328627,
+            "angularVelocity": 1.1237311094761516e-7,
+            "velocityX": 3.5624788204854614,
+            "velocityY": -1.272550110681535,
+            "timestamp": 1.005349891292748
+        },
+        {
+            "x": 5.509296796458925,
+            "y": 4.395806297422418,
+            "heading": 3.299839065103253,
+            "angularVelocity": 1.1237310968464263e-7,
+            "velocityX": 3.562478820485462,
+            "velocityY": -1.2725501106815353,
+            "timestamp": 1.096745335955725
+        },
+        {
+            "x": 5.834891132359631,
+            "y": 4.279501014200756,
+            "heading": 3.2998390753736433,
+            "angularVelocity": 1.123731107718446e-7,
+            "velocityX": 3.562478820485462,
+            "velocityY": -1.2725501106815351,
+            "timestamp": 1.188140780618702
+        },
+        {
+            "x": 6.160485468260297,
+            "y": 4.16319573097898,
+            "heading": 3.299839085644034,
+            "angularVelocity": 1.1237310986388625e-7,
+            "velocityX": 3.562478820485015,
+            "velocityY": -1.2725501106827837,
+            "timestamp": 1.279536225281679
+        },
+        {
+            "x": 6.4860797940952795,
+            "y": 4.046890419578527,
+            "heading": 3.2998390959144244,
+            "angularVelocity": 1.1237311101180894e-7,
+            "velocityX": 3.5624787103517215,
+            "velocityY": -1.2725504189987695,
+            "timestamp": 1.370931669944656
+        },
+        {
+            "x": 6.809159340398316,
+            "y": 3.9237717890604613,
+            "heading": 3.299839106230886,
+            "angularVelocity": 1.1287719938305865e-7,
+            "velocityX": 3.5349633397413522,
+            "velocityY": -1.3470981072657797,
+            "timestamp": 1.462327114607633
+        },
+        {
+            "x": 7.101008389413158,
+            "y": 3.738399942122118,
+            "heading": 3.299839117623808,
+            "angularVelocity": 1.246552472151185e-7,
+            "velocityX": 3.1932559668706793,
+            "velocityY": -2.0282394557178747,
+            "timestamp": 1.55372255927061
+        },
+        {
+            "x": 7.349782731781937,
+            "y": 3.498296000909773,
+            "heading": 3.299839132467128,
+            "angularVelocity": 1.6240765120244838e-7,
+            "velocityX": 2.7219555994956917,
+            "velocityY": -2.6270887142977735,
+            "timestamp": 1.645118003933587
+        },
+        {
+            "x": 7.5357710820320545,
+            "y": 3.2282223692532166,
+            "heading": 3.2881158704604148,
+            "angularVelocity": -0.12826965337097124,
+            "velocityX": 2.0349849047286135,
+            "velocityY": -2.955001014026776,
+            "timestamp": 1.736513448596564
+        },
+        {
+            "x": 7.652314357234611,
+            "y": 2.959723163992482,
+            "heading": 3.2581736625054765,
+            "angularVelocity": -0.32761160105108156,
+            "velocityX": 1.2751540914573078,
+            "velocityY": -2.9377744837371598,
+            "timestamp": 1.827908893259541
+        },
+        {
+            "x": 7.700165626970879,
+            "y": 2.701389747494563,
+            "heading": 3.208935276055406,
+            "angularVelocity": -0.5387400502456196,
+            "velocityX": 0.5235629621664141,
+            "velocityY": -2.826545868347203,
+            "timestamp": 1.919304337922518
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -0.7542528657692632,
+            "velocityX": -0.2206414887004817,
+            "velocityY": -2.6739817109674653,
+            "timestamp": 2.0106997825854953
+        },
+        {
+            "x": 7.619243737534899,
+            "y": 2.269397277536314,
+            "heading": 3.071018168568731,
+            "angularVelocity": -0.9308752015312701,
+            "velocityX": -0.8198752757491441,
+            "velocityY": -2.531604604539154,
+            "timestamp": 2.08480405705527
+        },
+        {
+            "x": 7.514288335022884,
+            "y": 2.093479496062301,
+            "heading": 2.9929449451014944,
+            "angularVelocity": -1.0535589751865693,
+            "velocityX": -1.4163204924814625,
+            "velocityY": -2.373922189141621,
+            "timestamp": 2.1589083315250446
+        },
+        {
+            "x": 7.365556927410938,
+            "y": 1.9309603366701282,
+            "heading": 2.911854592478602,
+            "angularVelocity": -1.0942736192096787,
+            "velocityX": -2.0070557154244444,
+            "velocityY": -2.193114507294498,
+            "timestamp": 2.233012605994819
+        },
+        {
+            "x": 7.174068652738112,
+            "y": 1.784693946040825,
+            "heading": 2.8381473535529165,
+            "angularVelocity": -0.994642204556963,
+            "velocityX": -2.5840381819125056,
+            "velocityY": -1.973791548137795,
+            "timestamp": 2.307116880464594
+        },
+        {
+            "x": 6.9430736895852,
+            "y": 1.6600434204320091,
+            "heading": 2.7943942143969487,
+            "angularVelocity": -0.5904266585027068,
+            "velocityX": -3.117161119324147,
+            "velocityY": -1.6820962960737031,
+            "timestamp": 2.3812211549343685
+        },
+        {
+            "x": 6.679335807473864,
+            "y": 1.5650263715806954,
+            "heading": 2.794393527099141,
+            "angularVelocity": -0.000009274738771364558,
+            "velocityX": -3.5590103809586133,
+            "velocityY": -1.2822073966879628,
+            "timestamp": 2.455325429404143
+        },
+        {
+            "x": 6.403647884167786,
+            "y": 1.5142107470678074,
+            "heading": 2.794393497300607,
+            "angularVelocity": -4.021162732159612e-7,
+            "velocityX": -3.720270190601743,
+            "velocityY": -0.6857313545879138,
+            "timestamp": 2.5294297038739177
+        },
+        {
+            "x": 6.123365156631299,
+            "y": 1.5089521104338168,
+            "heading": 2.7943934713550327,
+            "angularVelocity": -3.501225017023653e-7,
+            "velocityX": -3.782274768114868,
+            "velocityY": -0.07096266270252378,
+            "timestamp": 2.6035339783436924
+        },
+        {
+            "x": 5.853614932127584,
+            "y": 1.5409906174836003,
+            "heading": 2.7698594509818024,
+            "angularVelocity": -0.33107429427995533,
+            "velocityX": -3.6401439246765253,
+            "velocityY": 0.43234357638746057,
+            "timestamp": 2.677638252813467
+        },
+        {
+            "x": 5.6273847181633965,
+            "y": 1.5674202114884777,
+            "heading": 2.697593854639993,
+            "angularVelocity": -0.9751879612723551,
+            "velocityX": -3.0528632198735024,
+            "velocityY": 0.3566541092809186,
+            "timestamp": 2.7517425272832416
+        },
+        {
+            "x": 5.446628200819817,
+            "y": 1.588490932967887,
+            "heading": 2.6254372460611823,
+            "angularVelocity": -0.9737172260984721,
+            "velocityX": -2.4392185017249606,
+            "velocityY": 0.284338813519285,
+            "timestamp": 2.8258468017530163
+        },
+        {
+            "x": 5.311228363745466,
+            "y": 1.6042594522545108,
+            "heading": 2.5652453697807402,
+            "angularVelocity": -0.8122591673836874,
+            "velocityX": -1.8271528605219525,
+            "velocityY": 0.21278825546128166,
+            "timestamp": 2.899951076222791
+        },
+        {
+            "x": 5.22105179084387,
+            "y": 1.614756634238219,
+            "heading": 2.5224127669101155,
+            "angularVelocity": -0.578004483237462,
+            "velocityX": -1.2168876025954114,
+            "velocityY": 0.14165420360473746,
+            "timestamp": 2.9740553506925655
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.302449043195724,
+            "velocityX": -0.6079513113949444,
+            "velocityY": 0.07075658994444238,
+            "timestamp": 3.04815962516234
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": 8.84173108800433e-33,
+            "velocityY": 0,
+            "timestamp": 3.122263899632115
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj
new file mode 100644
index 00000000..7e708e4b
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -6.980040471932584e-34,
+            "velocityX": -4.489498521627733e-35,
+            "velocityY": 1.1490136335521518e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 2.252002036509344,
+            "y": 5.607878031980728,
+            "heading": 3.1558282188080855,
+            "angularVelocity": 0.20834807697002747,
+            "velocityX": 0.026352962503434416,
+            "velocityY": 0.6302222639984295,
+            "timestamp": 0.07597007391799691
+        },
+        {
+            "x": 2.2560056075028405,
+            "y": 5.703634203308701,
+            "heading": 3.187479727503087,
+            "angularVelocity": 0.41663127416680434,
+            "velocityX": 0.05269931681016954,
+            "velocityY": 1.260445941270677,
+            "timestamp": 0.15194014783599383
+        },
+        {
+            "x": 2.262010006101527,
+            "y": 5.847268776725278,
+            "heading": 3.2349419015403043,
+            "angularVelocity": 0.6247482934984211,
+            "velocityX": 0.07903636641406601,
+            "velocityY": 1.8906730770279163,
+            "timestamp": 0.22791022175399073
+        },
+        {
+            "x": 2.2700144000703353,
+            "y": 6.038782280998214,
+            "heading": 3.298188503077313,
+            "angularVelocity": 0.8325199420666319,
+            "velocityX": 0.105362461242945,
+            "velocityY": 2.520907172996284,
+            "timestamp": 0.30388029567198765
+        },
+        {
+            "x": 2.280009617993533,
+            "y": 6.278187235561798,
+            "heading": 3.376569762958043,
+            "angularVelocity": 1.0317386286254728,
+            "velocityX": 0.13156783201220376,
+            "velocityY": 3.151306063253281,
+            "timestamp": 0.3798503695899846
+        },
+        {
+            "x": 2.28800573619677,
+            "y": 6.469712012711673,
+            "heading": 3.4392336798072036,
+            "angularVelocity": 0.8248500181374163,
+            "velocityX": 0.10525352669615307,
+            "velocityY": 2.5210555587534333,
+            "timestamp": 0.4558204435079815
+        },
+        {
+            "x": 2.2940028560112626,
+            "y": 6.613355954230437,
+            "heading": 3.486212722152842,
+            "angularVelocity": 0.6183887934128925,
+            "velocityX": 0.07894055521080842,
+            "velocityY": 1.890796390086646,
+            "timestamp": 0.5317905174259784
+        },
+        {
+            "x": 2.2980009548636073,
+            "y": 6.709118670121978,
+            "heading": 3.517527021603037,
+            "angularVelocity": 0.41219256261348647,
+            "velocityX": 0.052627286589986035,
+            "velocityY": 1.2605320878706525,
+            "timestamp": 0.6077605913439753
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 0.20611123260786593,
+            "velocityX": 0.02631358682828647,
+            "velocityY": 0.6302656744773738,
+            "timestamp": 0.6837306652619722
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 2.252106897096389e-33,
+            "velocityX": 0,
+            "velocityY": 2.1844888905645107e-35,
+            "timestamp": 0.7597007391799692
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.traj
new file mode 100644
index 00000000..7e708e4b
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -6.980040471932584e-34,
+            "velocityX": -4.489498521627733e-35,
+            "velocityY": 1.1490136335521518e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 2.252002036509344,
+            "y": 5.607878031980728,
+            "heading": 3.1558282188080855,
+            "angularVelocity": 0.20834807697002747,
+            "velocityX": 0.026352962503434416,
+            "velocityY": 0.6302222639984295,
+            "timestamp": 0.07597007391799691
+        },
+        {
+            "x": 2.2560056075028405,
+            "y": 5.703634203308701,
+            "heading": 3.187479727503087,
+            "angularVelocity": 0.41663127416680434,
+            "velocityX": 0.05269931681016954,
+            "velocityY": 1.260445941270677,
+            "timestamp": 0.15194014783599383
+        },
+        {
+            "x": 2.262010006101527,
+            "y": 5.847268776725278,
+            "heading": 3.2349419015403043,
+            "angularVelocity": 0.6247482934984211,
+            "velocityX": 0.07903636641406601,
+            "velocityY": 1.8906730770279163,
+            "timestamp": 0.22791022175399073
+        },
+        {
+            "x": 2.2700144000703353,
+            "y": 6.038782280998214,
+            "heading": 3.298188503077313,
+            "angularVelocity": 0.8325199420666319,
+            "velocityX": 0.105362461242945,
+            "velocityY": 2.520907172996284,
+            "timestamp": 0.30388029567198765
+        },
+        {
+            "x": 2.280009617993533,
+            "y": 6.278187235561798,
+            "heading": 3.376569762958043,
+            "angularVelocity": 1.0317386286254728,
+            "velocityX": 0.13156783201220376,
+            "velocityY": 3.151306063253281,
+            "timestamp": 0.3798503695899846
+        },
+        {
+            "x": 2.28800573619677,
+            "y": 6.469712012711673,
+            "heading": 3.4392336798072036,
+            "angularVelocity": 0.8248500181374163,
+            "velocityX": 0.10525352669615307,
+            "velocityY": 2.5210555587534333,
+            "timestamp": 0.4558204435079815
+        },
+        {
+            "x": 2.2940028560112626,
+            "y": 6.613355954230437,
+            "heading": 3.486212722152842,
+            "angularVelocity": 0.6183887934128925,
+            "velocityX": 0.07894055521080842,
+            "velocityY": 1.890796390086646,
+            "timestamp": 0.5317905174259784
+        },
+        {
+            "x": 2.2980009548636073,
+            "y": 6.709118670121978,
+            "heading": 3.517527021603037,
+            "angularVelocity": 0.41219256261348647,
+            "velocityX": 0.052627286589986035,
+            "velocityY": 1.2605320878706525,
+            "timestamp": 0.6077605913439753
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 0.20611123260786593,
+            "velocityX": 0.02631358682828647,
+            "velocityY": 0.6302656744773738,
+            "timestamp": 0.6837306652619722
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 2.252106897096389e-33,
+            "velocityX": 0,
+            "velocityY": 2.1844888905645107e-35,
+            "timestamp": 0.7597007391799692
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj
new file mode 100644
index 00000000..7db396b8
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -7.990908752759601e-36,
+            "velocityX": 0,
+            "velocityY": 3.147004019802851e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 2.252001928264259,
+            "y": 5.512001885302468,
+            "heading": 3.1243023137364228,
+            "angularVelocity": -0.2063808903009179,
+            "velocityX": 0.026319785639686007,
+            "velocityY": -0.6310416374562365,
+            "timestamp": 0.07606172374142242
+        },
+        {
+            "x": 2.2560053075353443,
+            "y": 5.416005550394565,
+            "heading": 3.0929118769402844,
+            "angularVelocity": -0.41269688947430483,
+            "velocityX": 0.052633296672250224,
+            "velocityY": -1.2620846621127049,
+            "timestamp": 0.15212344748284484
+        },
+        {
+            "x": 2.262009466656741,
+            "y": 5.272010740000972,
+            "heading": 3.0458411253223887,
+            "angularVelocity": -0.6188493936571329,
+            "velocityX": 0.0789379838643735,
+            "velocityY": -1.8931310429292179,
+            "timestamp": 0.22818517122426726
+        },
+        {
+            "x": 2.2700136159756155,
+            "y": 5.080016943812723,
+            "heading": 2.983115684252823,
+            "angularVelocity": -0.8246649955344898,
+            "velocityX": 0.10523228931919847,
+            "velocityY": -2.524184132888531,
+            "timestamp": 0.3042468949656897
+        },
+        {
+            "x": 2.280009094575526,
+            "y": 4.840012208292782,
+            "heading": 2.9053630904797005,
+            "angularVelocity": -1.0222302355051571,
+            "velocityX": 0.1314127278247149,
+            "velocityY": -3.155394378595143,
+            "timestamp": 0.3803086187071121
+        },
+        {
+            "x": 2.2880054239679986,
+            "y": 4.64800763986969,
+            "heading": 2.8432007503387635,
+            "angularVelocity": -0.8172617853397972,
+            "velocityX": 0.10512947904857922,
+            "velocityY": -2.524325757799354,
+            "timestamp": 0.45637034244853447
+        },
+        {
+            "x": 2.294002700573331,
+            "y": 4.504003870137798,
+            "heading": 2.7965972482938968,
+            "angularVelocity": -0.6127063620501001,
+            "velocityX": 0.07884749793103833,
+            "velocityY": -1.8932488332954869,
+            "timestamp": 0.5324320661899569
+        },
+        {
+            "x": 2.2980009029345383,
+            "y": 4.408001272313515,
+            "heading": 2.7655331423967997,
+            "angularVelocity": -0.40840654627682993,
+            "velocityX": 0.05256523471383641,
+            "velocityY": -1.2621670020344438,
+            "timestamp": 0.6084937899313793
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": -0.20421759635116843,
+            "velocityX": 0.026282563254256014,
+            "velocityY": -0.631083151319306,
+            "timestamp": 0.6845555136728017
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": -4.352426400818003e-34,
+            "velocityX": -2.6471819427760516e-34,
+            "velocityY": 2.5922629666231517e-36,
+            "timestamp": 0.760617237414224
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.traj
new file mode 100644
index 00000000..7db396b8
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -7.990908752759601e-36,
+            "velocityX": 0,
+            "velocityY": 3.147004019802851e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 2.252001928264259,
+            "y": 5.512001885302468,
+            "heading": 3.1243023137364228,
+            "angularVelocity": -0.2063808903009179,
+            "velocityX": 0.026319785639686007,
+            "velocityY": -0.6310416374562365,
+            "timestamp": 0.07606172374142242
+        },
+        {
+            "x": 2.2560053075353443,
+            "y": 5.416005550394565,
+            "heading": 3.0929118769402844,
+            "angularVelocity": -0.41269688947430483,
+            "velocityX": 0.052633296672250224,
+            "velocityY": -1.2620846621127049,
+            "timestamp": 0.15212344748284484
+        },
+        {
+            "x": 2.262009466656741,
+            "y": 5.272010740000972,
+            "heading": 3.0458411253223887,
+            "angularVelocity": -0.6188493936571329,
+            "velocityX": 0.0789379838643735,
+            "velocityY": -1.8931310429292179,
+            "timestamp": 0.22818517122426726
+        },
+        {
+            "x": 2.2700136159756155,
+            "y": 5.080016943812723,
+            "heading": 2.983115684252823,
+            "angularVelocity": -0.8246649955344898,
+            "velocityX": 0.10523228931919847,
+            "velocityY": -2.524184132888531,
+            "timestamp": 0.3042468949656897
+        },
+        {
+            "x": 2.280009094575526,
+            "y": 4.840012208292782,
+            "heading": 2.9053630904797005,
+            "angularVelocity": -1.0222302355051571,
+            "velocityX": 0.1314127278247149,
+            "velocityY": -3.155394378595143,
+            "timestamp": 0.3803086187071121
+        },
+        {
+            "x": 2.2880054239679986,
+            "y": 4.64800763986969,
+            "heading": 2.8432007503387635,
+            "angularVelocity": -0.8172617853397972,
+            "velocityX": 0.10512947904857922,
+            "velocityY": -2.524325757799354,
+            "timestamp": 0.45637034244853447
+        },
+        {
+            "x": 2.294002700573331,
+            "y": 4.504003870137798,
+            "heading": 2.7965972482938968,
+            "angularVelocity": -0.6127063620501001,
+            "velocityX": 0.07884749793103833,
+            "velocityY": -1.8932488332954869,
+            "timestamp": 0.5324320661899569
+        },
+        {
+            "x": 2.2980009029345383,
+            "y": 4.408001272313515,
+            "heading": 2.7655331423967997,
+            "angularVelocity": -0.40840654627682993,
+            "velocityX": 0.05256523471383641,
+            "velocityY": -1.2621670020344438,
+            "timestamp": 0.6084937899313793
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": -0.20421759635116843,
+            "velocityX": 0.026282563254256014,
+            "velocityY": -0.631083151319306,
+            "timestamp": 0.6845555136728017
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": -4.352426400818003e-34,
+            "velocityX": -2.6471819427760516e-34,
+            "velocityY": 2.5922629666231517e-36,
+            "timestamp": 0.760617237414224
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S1.1.traj
new file mode 100644
index 00000000..99f476a8
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S1.1.traj	
@@ -0,0 +1,337 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": -8.850231580467738e-33,
+            "velocityY": -9.949674184637553e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3596219168979733,
+            "y": 4.390117696915229,
+            "heading": 2.7532876341018757,
+            "angularVelocity": 0.0367297993475745,
+            "velocityX": 0.6661024239682877,
+            "velocityY": 0.3364781268925937,
+            "timestamp": 0.08950863223523108
+        },
+        {
+            "x": 2.478866409519412,
+            "y": 4.45035344252589,
+            "heading": 2.759663263066401,
+            "angularVelocity": 0.07122920779048274,
+            "velocityX": 1.3322122084053396,
+            "velocityY": 0.6729601839112106,
+            "timestamp": 0.17901726447046215
+        },
+        {
+            "x": 2.6577344843676225,
+            "y": 4.540707777094465,
+            "heading": 2.7687936152184336,
+            "angularVelocity": 0.10200526948103018,
+            "velocityX": 1.998333237604842,
+            "velocityY": 1.009448276799944,
+            "timestamp": 0.2685258967056932
+        },
+        {
+            "x": 2.896227808747147,
+            "y": 4.661181607047397,
+            "heading": 2.7800101234260337,
+            "angularVelocity": 0.12531202776199954,
+            "velocityX": 2.664472894108777,
+            "velocityY": 1.3459464963817573,
+            "timestamp": 0.3580345289409243
+        },
+        {
+            "x": 3.194348569009026,
+            "y": 4.811776231628672,
+            "heading": 2.7912909680084645,
+            "angularVelocity": 0.126030800613561,
+            "velocityX": 3.3306369767600654,
+            "velocityY": 1.6824592312561388,
+            "timestamp": 0.4475431611761554
+        },
+        {
+            "x": 3.4965824721893735,
+            "y": 4.964447700014571,
+            "heading": 2.791290970219591,
+            "angularVelocity": 2.4702941902196968e-8,
+            "velocityX": 3.3765894487815307,
+            "velocityY": 1.7056619520749001,
+            "timestamp": 0.5370517934113864
+        },
+        {
+            "x": 3.798816375371316,
+            "y": 5.117119168397318,
+            "heading": 2.7912909724307147,
+            "angularVelocity": 2.470290930957206e-8,
+            "velocityX": 3.3765894487993435,
+            "velocityY": 1.7056619520396954,
+            "timestamp": 0.6265604256466175
+        },
+        {
+            "x": 4.101050278553258,
+            "y": 5.2697906367800655,
+            "heading": 2.7912909746418384,
+            "angularVelocity": 2.4702908438395345e-8,
+            "velocityX": 3.376589448799344,
+            "velocityY": 1.705661952039695,
+            "timestamp": 0.7160690578818486
+        },
+        {
+            "x": 4.4032841817351995,
+            "y": 5.422462105162813,
+            "heading": 2.791290976852962,
+            "angularVelocity": 2.470290834028317e-8,
+            "velocityX": 3.376589448799344,
+            "velocityY": 1.7056619520396945,
+            "timestamp": 0.8055776901170797
+        },
+        {
+            "x": 4.705518084917142,
+            "y": 5.5751335735455605,
+            "heading": 2.791290979064086,
+            "angularVelocity": 2.470290893539666e-8,
+            "velocityX": 3.376589448799343,
+            "velocityY": 1.7056619520396956,
+            "timestamp": 0.8950863223523108
+        },
+        {
+            "x": 5.007751988099084,
+            "y": 5.727805041928308,
+            "heading": 2.791290981275209,
+            "angularVelocity": 2.470290833665101e-8,
+            "velocityX": 3.376589448799344,
+            "velocityY": 1.705661952039695,
+            "timestamp": 0.9845949545875419
+        },
+        {
+            "x": 5.309985891281026,
+            "y": 5.8804765103110554,
+            "heading": 2.791290983486333,
+            "angularVelocity": 2.4702909020592533e-8,
+            "velocityX": 3.376589448799343,
+            "velocityY": 1.705661952039695,
+            "timestamp": 1.0741035868227728
+        },
+        {
+            "x": 5.612219794462969,
+            "y": 6.033147978693803,
+            "heading": 2.7912909856974566,
+            "angularVelocity": 2.470290845419301e-8,
+            "velocityX": 3.3765894487993435,
+            "velocityY": 1.7056619520396947,
+            "timestamp": 1.1636122190580038
+        },
+        {
+            "x": 5.914453697644911,
+            "y": 6.18581944707655,
+            "heading": 2.7912909879085803,
+            "angularVelocity": 2.4702909294524738e-8,
+            "velocityX": 3.3765894487993435,
+            "velocityY": 1.705661952039695,
+            "timestamp": 1.2531208512932348
+        },
+        {
+            "x": 6.216687600826853,
+            "y": 6.338490915459298,
+            "heading": 2.791290990119704,
+            "angularVelocity": 2.4702908192380297e-8,
+            "velocityX": 3.3765894487993426,
+            "velocityY": 1.705661952039695,
+            "timestamp": 1.3426294835284658
+        },
+        {
+            "x": 6.518921504008634,
+            "y": 6.491162383842364,
+            "heading": 2.791290992330828,
+            "angularVelocity": 2.470290883873948e-8,
+            "velocityX": 3.3765894487975445,
+            "velocityY": 1.705661952043257,
+            "timestamp": 1.4321381157636968
+        },
+        {
+            "x": 6.821155370898669,
+            "y": 6.643833924069847,
+            "heading": 2.7912909945419515,
+            "angularVelocity": 2.4702908942452264e-8,
+            "velocityX": 3.3765890433422854,
+            "velocityY": 1.705662754696757,
+            "timestamp": 1.5216467479989277
+        },
+        {
+            "x": 7.115501994166732,
+            "y": 6.811207678318402,
+            "heading": 2.7912909995251494,
+            "angularVelocity": 5.5672818987002594e-8,
+            "velocityX": 3.2884719151390045,
+            "velocityY": 1.8699174601248647,
+            "timestamp": 1.6111553802341587
+        },
+        {
+            "x": 7.349346251698842,
+            "y": 6.9991016785605416,
+            "heading": 2.863630358302155,
+            "angularVelocity": 0.8081830430264627,
+            "velocityX": 2.6125330227095924,
+            "velocityY": 2.099171840190225,
+            "timestamp": 1.7006640124693897
+        },
+        {
+            "x": 7.522269801890552,
+            "y": 7.159584774540021,
+            "heading": 2.9361878922250697,
+            "angularVelocity": 0.8106205190604594,
+            "velocityX": 1.9319203731910752,
+            "velocityY": 1.792934289932232,
+            "timestamp": 1.7901726447046207
+        },
+        {
+            "x": 7.634890963268431,
+            "y": 7.291326986197087,
+            "heading": 3.0068580812297814,
+            "angularVelocity": 0.789534900041686,
+            "velocityX": 1.258215644295718,
+            "velocityY": 1.4718380603877852,
+            "timestamp": 1.8796812769398517
+        },
+        {
+            "x": 7.687430049075347,
+            "y": 7.393874919842459,
+            "heading": 3.074917441639189,
+            "angularVelocity": 0.7603664441050307,
+            "velocityX": 0.5869722784819492,
+            "velocityY": 1.1456764681184401,
+            "timestamp": 1.9691899091750826
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 0.7271092936575372,
+            "velocityX": -0.08300930189416394,
+            "velocityY": 0.8169612062149058,
+            "timestamp": 2.0586985414103136
+        },
+        {
+            "x": 7.610902849690699,
+            "y": 7.510752049010022,
+            "heading": 3.20269374092293,
+            "angularVelocity": 0.6908898775103415,
+            "velocityX": -0.7614559445765343,
+            "velocityY": 0.4821509665298148,
+            "timestamp": 2.149442005692712
+        },
+        {
+            "x": 7.480264531664604,
+            "y": 7.524075152445223,
+            "heading": 3.2620627183539166,
+            "angularVelocity": 0.6542507264900841,
+            "velocityX": -1.4396443761452817,
+            "velocityY": 0.1468216310734938,
+            "timestamp": 2.2401854699751107
+        },
+        {
+            "x": 7.2881241468833045,
+            "y": 7.506890911811968,
+            "heading": 3.31804363381791,
+            "angularVelocity": 0.6169140213754432,
+            "velocityX": -2.117401912090868,
+            "velocityY": -0.18937166185078083,
+            "timestamp": 2.3309289342575092
+        },
+        {
+            "x": 7.034560081128849,
+            "y": 7.459042940332114,
+            "heading": 3.3705102766509376,
+            "angularVelocity": 0.5781864649750256,
+            "velocityX": -2.7942956306511584,
+            "velocityY": -0.5272883491746447,
+            "timestamp": 2.421672398539908
+        },
+        {
+            "x": 6.71980893194678,
+            "y": 7.380065366153176,
+            "heading": 3.4190860492179427,
+            "angularVelocity": 0.5353087734873627,
+            "velocityX": -3.468582025946757,
+            "velocityY": -0.8703389803716955,
+            "timestamp": 2.5124158628223063
+        },
+        {
+            "x": 6.409212684326435,
+            "y": 7.233883432324963,
+            "heading": 3.4190860479548357,
+            "angularVelocity": -1.3919538885721925e-8,
+            "velocityX": -3.4227946891442476,
+            "velocityY": -1.6109362253714221,
+            "timestamp": 2.603159327104705
+        },
+        {
+            "x": 6.100914553727412,
+            "y": 7.0829150593165995,
+            "heading": 3.4190860462953716,
+            "angularVelocity": -1.8287422949592592e-8,
+            "velocityX": -3.397469261693411,
+            "velocityY": -1.663683155610435,
+            "timestamp": 2.6939027913871034
+        },
+        {
+            "x": 5.79261648458282,
+            "y": 6.9319467005164075,
+            "heading": 3.4190860242893293,
+            "angularVelocity": -2.4250829002858187e-7,
+            "velocityX": -3.3974685844608317,
+            "velocityY": -1.6636829990352793,
+            "timestamp": 2.784646255669502
+        },
+        {
+            "x": 5.545968445126592,
+            "y": 6.811167311993174,
+            "heading": 3.4140434484907587,
+            "angularVelocity": -0.05556957559916094,
+            "velocityX": -2.7180804855393776,
+            "velocityY": -1.330998209935673,
+            "timestamp": 2.8753897199519005
+        },
+        {
+            "x": 5.3609835283362095,
+            "y": 6.720583315840053,
+            "heading": 3.409029867698435,
+            "angularVelocity": -0.05525004838608659,
+            "velocityX": -2.0385481009927036,
+            "velocityY": -0.9982426488724173,
+            "timestamp": 2.966133184234299
+        },
+        {
+            "x": 5.237660981460967,
+            "y": 6.6601943432298505,
+            "heading": 3.4052336568932624,
+            "angularVelocity": -0.041834536902387906,
+            "velocityX": -1.3590240118170522,
+            "velocityY": -0.6654911523133865,
+            "timestamp": 3.0568766485166976
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": -0.022572972388422655,
+            "velocityX": -0.6795087883031954,
+            "velocityY": -0.3327439994563607,
+            "timestamp": 3.147620112799096
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0,
+            "velocityX": -1.5756247890906886e-35,
+            "velocityY": 0,
+            "timestamp": 3.2383635770814947
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S1.traj
new file mode 100644
index 00000000..99f476a8
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S1.traj	
@@ -0,0 +1,337 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": -8.850231580467738e-33,
+            "velocityY": -9.949674184637553e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3596219168979733,
+            "y": 4.390117696915229,
+            "heading": 2.7532876341018757,
+            "angularVelocity": 0.0367297993475745,
+            "velocityX": 0.6661024239682877,
+            "velocityY": 0.3364781268925937,
+            "timestamp": 0.08950863223523108
+        },
+        {
+            "x": 2.478866409519412,
+            "y": 4.45035344252589,
+            "heading": 2.759663263066401,
+            "angularVelocity": 0.07122920779048274,
+            "velocityX": 1.3322122084053396,
+            "velocityY": 0.6729601839112106,
+            "timestamp": 0.17901726447046215
+        },
+        {
+            "x": 2.6577344843676225,
+            "y": 4.540707777094465,
+            "heading": 2.7687936152184336,
+            "angularVelocity": 0.10200526948103018,
+            "velocityX": 1.998333237604842,
+            "velocityY": 1.009448276799944,
+            "timestamp": 0.2685258967056932
+        },
+        {
+            "x": 2.896227808747147,
+            "y": 4.661181607047397,
+            "heading": 2.7800101234260337,
+            "angularVelocity": 0.12531202776199954,
+            "velocityX": 2.664472894108777,
+            "velocityY": 1.3459464963817573,
+            "timestamp": 0.3580345289409243
+        },
+        {
+            "x": 3.194348569009026,
+            "y": 4.811776231628672,
+            "heading": 2.7912909680084645,
+            "angularVelocity": 0.126030800613561,
+            "velocityX": 3.3306369767600654,
+            "velocityY": 1.6824592312561388,
+            "timestamp": 0.4475431611761554
+        },
+        {
+            "x": 3.4965824721893735,
+            "y": 4.964447700014571,
+            "heading": 2.791290970219591,
+            "angularVelocity": 2.4702941902196968e-8,
+            "velocityX": 3.3765894487815307,
+            "velocityY": 1.7056619520749001,
+            "timestamp": 0.5370517934113864
+        },
+        {
+            "x": 3.798816375371316,
+            "y": 5.117119168397318,
+            "heading": 2.7912909724307147,
+            "angularVelocity": 2.470290930957206e-8,
+            "velocityX": 3.3765894487993435,
+            "velocityY": 1.7056619520396954,
+            "timestamp": 0.6265604256466175
+        },
+        {
+            "x": 4.101050278553258,
+            "y": 5.2697906367800655,
+            "heading": 2.7912909746418384,
+            "angularVelocity": 2.4702908438395345e-8,
+            "velocityX": 3.376589448799344,
+            "velocityY": 1.705661952039695,
+            "timestamp": 0.7160690578818486
+        },
+        {
+            "x": 4.4032841817351995,
+            "y": 5.422462105162813,
+            "heading": 2.791290976852962,
+            "angularVelocity": 2.470290834028317e-8,
+            "velocityX": 3.376589448799344,
+            "velocityY": 1.7056619520396945,
+            "timestamp": 0.8055776901170797
+        },
+        {
+            "x": 4.705518084917142,
+            "y": 5.5751335735455605,
+            "heading": 2.791290979064086,
+            "angularVelocity": 2.470290893539666e-8,
+            "velocityX": 3.376589448799343,
+            "velocityY": 1.7056619520396956,
+            "timestamp": 0.8950863223523108
+        },
+        {
+            "x": 5.007751988099084,
+            "y": 5.727805041928308,
+            "heading": 2.791290981275209,
+            "angularVelocity": 2.470290833665101e-8,
+            "velocityX": 3.376589448799344,
+            "velocityY": 1.705661952039695,
+            "timestamp": 0.9845949545875419
+        },
+        {
+            "x": 5.309985891281026,
+            "y": 5.8804765103110554,
+            "heading": 2.791290983486333,
+            "angularVelocity": 2.4702909020592533e-8,
+            "velocityX": 3.376589448799343,
+            "velocityY": 1.705661952039695,
+            "timestamp": 1.0741035868227728
+        },
+        {
+            "x": 5.612219794462969,
+            "y": 6.033147978693803,
+            "heading": 2.7912909856974566,
+            "angularVelocity": 2.470290845419301e-8,
+            "velocityX": 3.3765894487993435,
+            "velocityY": 1.7056619520396947,
+            "timestamp": 1.1636122190580038
+        },
+        {
+            "x": 5.914453697644911,
+            "y": 6.18581944707655,
+            "heading": 2.7912909879085803,
+            "angularVelocity": 2.4702909294524738e-8,
+            "velocityX": 3.3765894487993435,
+            "velocityY": 1.705661952039695,
+            "timestamp": 1.2531208512932348
+        },
+        {
+            "x": 6.216687600826853,
+            "y": 6.338490915459298,
+            "heading": 2.791290990119704,
+            "angularVelocity": 2.4702908192380297e-8,
+            "velocityX": 3.3765894487993426,
+            "velocityY": 1.705661952039695,
+            "timestamp": 1.3426294835284658
+        },
+        {
+            "x": 6.518921504008634,
+            "y": 6.491162383842364,
+            "heading": 2.791290992330828,
+            "angularVelocity": 2.470290883873948e-8,
+            "velocityX": 3.3765894487975445,
+            "velocityY": 1.705661952043257,
+            "timestamp": 1.4321381157636968
+        },
+        {
+            "x": 6.821155370898669,
+            "y": 6.643833924069847,
+            "heading": 2.7912909945419515,
+            "angularVelocity": 2.4702908942452264e-8,
+            "velocityX": 3.3765890433422854,
+            "velocityY": 1.705662754696757,
+            "timestamp": 1.5216467479989277
+        },
+        {
+            "x": 7.115501994166732,
+            "y": 6.811207678318402,
+            "heading": 2.7912909995251494,
+            "angularVelocity": 5.5672818987002594e-8,
+            "velocityX": 3.2884719151390045,
+            "velocityY": 1.8699174601248647,
+            "timestamp": 1.6111553802341587
+        },
+        {
+            "x": 7.349346251698842,
+            "y": 6.9991016785605416,
+            "heading": 2.863630358302155,
+            "angularVelocity": 0.8081830430264627,
+            "velocityX": 2.6125330227095924,
+            "velocityY": 2.099171840190225,
+            "timestamp": 1.7006640124693897
+        },
+        {
+            "x": 7.522269801890552,
+            "y": 7.159584774540021,
+            "heading": 2.9361878922250697,
+            "angularVelocity": 0.8106205190604594,
+            "velocityX": 1.9319203731910752,
+            "velocityY": 1.792934289932232,
+            "timestamp": 1.7901726447046207
+        },
+        {
+            "x": 7.634890963268431,
+            "y": 7.291326986197087,
+            "heading": 3.0068580812297814,
+            "angularVelocity": 0.789534900041686,
+            "velocityX": 1.258215644295718,
+            "velocityY": 1.4718380603877852,
+            "timestamp": 1.8796812769398517
+        },
+        {
+            "x": 7.687430049075347,
+            "y": 7.393874919842459,
+            "heading": 3.074917441639189,
+            "angularVelocity": 0.7603664441050307,
+            "velocityX": 0.5869722784819492,
+            "velocityY": 1.1456764681184401,
+            "timestamp": 1.9691899091750826
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 0.7271092936575372,
+            "velocityX": -0.08300930189416394,
+            "velocityY": 0.8169612062149058,
+            "timestamp": 2.0586985414103136
+        },
+        {
+            "x": 7.610902849690699,
+            "y": 7.510752049010022,
+            "heading": 3.20269374092293,
+            "angularVelocity": 0.6908898775103415,
+            "velocityX": -0.7614559445765343,
+            "velocityY": 0.4821509665298148,
+            "timestamp": 2.149442005692712
+        },
+        {
+            "x": 7.480264531664604,
+            "y": 7.524075152445223,
+            "heading": 3.2620627183539166,
+            "angularVelocity": 0.6542507264900841,
+            "velocityX": -1.4396443761452817,
+            "velocityY": 0.1468216310734938,
+            "timestamp": 2.2401854699751107
+        },
+        {
+            "x": 7.2881241468833045,
+            "y": 7.506890911811968,
+            "heading": 3.31804363381791,
+            "angularVelocity": 0.6169140213754432,
+            "velocityX": -2.117401912090868,
+            "velocityY": -0.18937166185078083,
+            "timestamp": 2.3309289342575092
+        },
+        {
+            "x": 7.034560081128849,
+            "y": 7.459042940332114,
+            "heading": 3.3705102766509376,
+            "angularVelocity": 0.5781864649750256,
+            "velocityX": -2.7942956306511584,
+            "velocityY": -0.5272883491746447,
+            "timestamp": 2.421672398539908
+        },
+        {
+            "x": 6.71980893194678,
+            "y": 7.380065366153176,
+            "heading": 3.4190860492179427,
+            "angularVelocity": 0.5353087734873627,
+            "velocityX": -3.468582025946757,
+            "velocityY": -0.8703389803716955,
+            "timestamp": 2.5124158628223063
+        },
+        {
+            "x": 6.409212684326435,
+            "y": 7.233883432324963,
+            "heading": 3.4190860479548357,
+            "angularVelocity": -1.3919538885721925e-8,
+            "velocityX": -3.4227946891442476,
+            "velocityY": -1.6109362253714221,
+            "timestamp": 2.603159327104705
+        },
+        {
+            "x": 6.100914553727412,
+            "y": 7.0829150593165995,
+            "heading": 3.4190860462953716,
+            "angularVelocity": -1.8287422949592592e-8,
+            "velocityX": -3.397469261693411,
+            "velocityY": -1.663683155610435,
+            "timestamp": 2.6939027913871034
+        },
+        {
+            "x": 5.79261648458282,
+            "y": 6.9319467005164075,
+            "heading": 3.4190860242893293,
+            "angularVelocity": -2.4250829002858187e-7,
+            "velocityX": -3.3974685844608317,
+            "velocityY": -1.6636829990352793,
+            "timestamp": 2.784646255669502
+        },
+        {
+            "x": 5.545968445126592,
+            "y": 6.811167311993174,
+            "heading": 3.4140434484907587,
+            "angularVelocity": -0.05556957559916094,
+            "velocityX": -2.7180804855393776,
+            "velocityY": -1.330998209935673,
+            "timestamp": 2.8753897199519005
+        },
+        {
+            "x": 5.3609835283362095,
+            "y": 6.720583315840053,
+            "heading": 3.409029867698435,
+            "angularVelocity": -0.05525004838608659,
+            "velocityX": -2.0385481009927036,
+            "velocityY": -0.9982426488724173,
+            "timestamp": 2.966133184234299
+        },
+        {
+            "x": 5.237660981460967,
+            "y": 6.6601943432298505,
+            "heading": 3.4052336568932624,
+            "angularVelocity": -0.041834536902387906,
+            "velocityX": -1.3590240118170522,
+            "velocityY": -0.6654911523133865,
+            "timestamp": 3.0568766485166976
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": -0.022572972388422655,
+            "velocityX": -0.6795087883031954,
+            "velocityY": -0.3327439994563607,
+            "timestamp": 3.147620112799096
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0,
+            "velocityX": -1.5756247890906886e-35,
+            "velocityY": 0,
+            "timestamp": 3.2383635770814947
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S2.1.traj
new file mode 100644
index 00000000..30e62dac
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S2.1.traj	
@@ -0,0 +1,355 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3566378052654167,
+            "y": 4.392308940927803,
+            "heading": 2.7577639052389085,
+            "angularVelocity": 0.08777137611433758,
+            "velocityX": 0.6402935063310357,
+            "velocityY": 0.3652543557357081,
+            "timestamp": 0.08845600448138136
+        },
+        {
+            "x": 2.469918670668774,
+            "y": 4.456929940384967,
+            "heading": 2.772608563363178,
+            "angularVelocity": 0.16781967726556357,
+            "velocityX": 1.2806464192852178,
+            "velocityY": 0.7305439561286646,
+            "timestamp": 0.1769120089627627
+        },
+        {
+            "x": 2.639850302646433,
+            "y": 4.553867610725804,
+            "heading": 2.793388618859665,
+            "angularVelocity": 0.2349196712910238,
+            "velocityX": 1.92108645392669,
+            "velocityY": 1.0958856994410031,
+            "timestamp": 0.26536801344414407
+        },
+        {
+            "x": 2.8664440757683463,
+            "y": 4.683128905196596,
+            "heading": 2.8177875607011336,
+            "angularVelocity": 0.27583138063398066,
+            "velocityX": 2.561655078707626,
+            "velocityY": 1.4613060495853287,
+            "timestamp": 0.3538240179255254
+        },
+        {
+            "x": 3.1496996536335864,
+            "y": 4.844715100104656,
+            "heading": 2.8386513493911516,
+            "angularVelocity": 0.23586627965327242,
+            "velocityX": 3.2022199004575587,
+            "velocityY": 1.8267408284540865,
+            "timestamp": 0.4422800224069068
+        },
+        {
+            "x": 3.44035847389222,
+            "y": 5.010517813781029,
+            "heading": 2.83865135522876,
+            "angularVelocity": 6.599448670952606e-8,
+            "velocityX": 3.28591396324954,
+            "velocityY": 1.8744088052413612,
+            "timestamp": 0.5307360268882881
+        },
+        {
+            "x": 3.731017294885288,
+            "y": 5.17632052617049,
+            "heading": 2.8386513610659576,
+            "angularVelocity": 6.598983540705142e-8,
+            "velocityX": 3.285913971552369,
+            "velocityY": 1.8744087906927347,
+            "timestamp": 0.6191920313696695
+        },
+        {
+            "x": 4.021676115881456,
+            "y": 5.3421232385545165,
+            "heading": 2.8386513669031546,
+            "angularVelocity": 6.598983544244374e-8,
+            "velocityX": 3.285913971587413,
+            "velocityY": 1.8744087906313014,
+            "timestamp": 0.7076480358510508
+        },
+        {
+            "x": 4.31233493687768,
+            "y": 5.507925950938444,
+            "heading": 2.838651372740352,
+            "angularVelocity": 6.598983514854984e-8,
+            "velocityX": 3.2859139715880556,
+            "velocityY": 1.8744087906301747,
+            "timestamp": 0.7961040403324322
+        },
+        {
+            "x": 4.602993757873904,
+            "y": 5.673728663322372,
+            "heading": 2.8386513785775485,
+            "angularVelocity": 6.598983509017798e-8,
+            "velocityX": 3.2859139715880414,
+            "velocityY": 1.8744087906301996,
+            "timestamp": 0.8845600448138136
+        },
+        {
+            "x": 4.893652578870126,
+            "y": 5.839531375706303,
+            "heading": 2.8386513844147463,
+            "angularVelocity": 6.598983636221009e-8,
+            "velocityX": 3.2859139715880246,
+            "velocityY": 1.8744087906302287,
+            "timestamp": 0.9730160492951949
+        },
+        {
+            "x": 5.184311399866346,
+            "y": 6.005334088090239,
+            "heading": 2.8386513902519432,
+            "angularVelocity": 6.598983502831849e-8,
+            "velocityX": 3.285913971588007,
+            "velocityY": 1.8744087906302613,
+            "timestamp": 1.0614720537765763
+        },
+        {
+            "x": 5.474970220862562,
+            "y": 6.171136800474182,
+            "heading": 2.83865139608914,
+            "angularVelocity": 6.598983586934159e-8,
+            "velocityX": 3.2859139715879397,
+            "velocityY": 1.8744087906303781,
+            "timestamp": 1.1499280582579576
+        },
+        {
+            "x": 5.765629041858637,
+            "y": 6.336939512858371,
+            "heading": 2.8386514019263376,
+            "angularVelocity": 6.598983523621195e-8,
+            "velocityX": 3.2859139715863668,
+            "velocityY": 1.8744087906331355,
+            "timestamp": 1.238384062739339
+        },
+        {
+            "x": 6.0562878628501755,
+            "y": 6.502742225250514,
+            "heading": 2.8386514077635345,
+            "angularVelocity": 6.598983511287735e-8,
+            "velocityX": 3.285913971535071,
+            "velocityY": 1.87440879072306,
+            "timestamp": 1.3268400672207203
+        },
+        {
+            "x": 6.3469466835037505,
+            "y": 6.6685449382351205,
+            "heading": 2.8386514136007315,
+            "angularVelocity": 6.598983592798126e-8,
+            "velocityX": 3.2859139677143814,
+            "velocityY": 1.8744087974208865,
+            "timestamp": 1.4152960717021017
+        },
+        {
+            "x": 6.6376054603552355,
+            "y": 6.834347728006541,
+            "heading": 2.838651419437929,
+            "angularVelocity": 6.598983535546771e-8,
+            "velocityX": 3.2859134725293426,
+            "velocityY": 1.8744096655000533,
+            "timestamp": 1.503752076183483
+        },
+        {
+            "x": 6.928282728448998,
+            "y": 7.000118097747795,
+            "heading": 2.8386514252461743,
+            "angularVelocity": 6.56625327771565e-8,
+            "velocityX": 3.286122517041433,
+            "velocityY": 1.8740431552739683,
+            "timestamp": 1.5922080806648644
+        },
+        {
+            "x": 7.191968889959837,
+            "y": 7.158086456014882,
+            "heading": 2.9053215778244037,
+            "angularVelocity": 0.7537097449644049,
+            "velocityX": 2.980986571311004,
+            "velocityY": 1.7858409860730329,
+            "timestamp": 1.6806640851462458
+        },
+        {
+            "x": 7.39896469669124,
+            "y": 7.2837659035610995,
+            "heading": 2.9729435938200757,
+            "angularVelocity": 0.7644706132968896,
+            "velocityX": 2.3400989898312643,
+            "velocityY": 1.4208130729292534,
+            "timestamp": 1.7691200896276271
+        },
+        {
+            "x": 7.549292841201835,
+            "y": 7.377144263475716,
+            "heading": 3.03570050164413,
+            "angularVelocity": 0.7094702976016349,
+            "velocityX": 1.6994679489759839,
+            "velocityY": 1.0556474991393678,
+            "timestamp": 1.8575760941090085
+        },
+        {
+            "x": 7.642968334049115,
+            "y": 7.4382219539742405,
+            "heading": 3.0916990951391803,
+            "angularVelocity": 0.6330671820796432,
+            "velocityX": 1.0590066032994798,
+            "velocityY": 0.6904866532988578,
+            "timestamp": 1.9460320985903898
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 0.546044388326283,
+            "velocityX": 0.41864502209875154,
+            "velocityY": 0.32533739449908106,
+            "timestamp": 2.034488103071771
+        },
+        {
+            "x": 7.645871150286324,
+            "y": 7.456493783242831,
+            "heading": 3.185181806591133,
+            "angularVelocity": 0.43686385232963987,
+            "velocityX": -0.329992576357443,
+            "velocityY": -0.10158483407911743,
+            "timestamp": 2.1379111863737847
+        },
+        {
+            "x": 7.534332014927772,
+            "y": 7.401805741681953,
+            "heading": 3.219069089269022,
+            "angularVelocity": 0.32765685953225704,
+            "velocityX": -1.078474280570675,
+            "velocityY": -0.5287798411617203,
+            "timestamp": 2.2413342696757983
+        },
+        {
+            "x": 7.345414230675068,
+            "y": 7.30288053546568,
+            "heading": 3.2416598317701943,
+            "angularVelocity": 0.21843037144032681,
+            "velocityX": -1.826650088365854,
+            "velocityY": -0.9565099304515025,
+            "timestamp": 2.344757352977812
+        },
+        {
+            "x": 7.079208201344681,
+            "y": 7.159560464323527,
+            "heading": 3.2529526054228244,
+            "angularVelocity": 0.1091900694900192,
+            "velocityX": -2.573951779729956,
+            "velocityY": -1.3857648270226828,
+            "timestamp": 2.4481804362798254
+        },
+        {
+            "x": 6.738167350146867,
+            "y": 6.967824923059757,
+            "heading": 3.25295260062049,
+            "angularVelocity": -4.6433873545046094e-8,
+            "velocityX": -3.29753126970623,
+            "velocityY": -1.853895041050502,
+            "timestamp": 2.551603519581839
+        },
+        {
+            "x": 6.398085530846708,
+            "y": 6.774393439892416,
+            "heading": 3.252952596202531,
+            "angularVelocity": -4.2717337144488564e-8,
+            "velocityX": -3.2882583698173717,
+            "velocityY": -1.8702931395158464,
+            "timestamp": 2.6550266028838525
+        },
+        {
+            "x": 6.058003908621006,
+            "y": 6.580961610238813,
+            "heading": 3.252952591784573,
+            "angularVelocity": -4.2717336082734796e-8,
+            "velocityX": -3.288256464300223,
+            "velocityY": -1.8702964896989283,
+            "timestamp": 2.758449686185866
+        },
+        {
+            "x": 5.717922287179122,
+            "y": 6.387529779207143,
+            "heading": 3.252952587366614,
+            "angularVelocity": -4.271733673837129e-8,
+            "velocityX": -3.28825645672147,
+            "velocityY": -1.8702965030234862,
+            "timestamp": 2.8618727694878796
+        },
+        {
+            "x": 5.377840665722721,
+            "y": 6.194097948200997,
+            "heading": 3.2529525829486556,
+            "angularVelocity": -4.271733567219934e-8,
+            "velocityX": -3.2882564568618378,
+            "velocityY": -1.8702965027766985,
+            "timestamp": 2.965295852789893
+        },
+        {
+            "x": 5.037759049752838,
+            "y": 6.000666107536036,
+            "heading": 3.252952578536604,
+            "angularVelocity": -4.266021621617093e-8,
+            "velocityX": -3.288256403812571,
+            "velocityY": -1.8702965961679845,
+            "timestamp": 3.0687189360919067
+        },
+        {
+            "x": 4.7278555042556665,
+            "y": 5.824399716323421,
+            "heading": 3.2077639144902763,
+            "angularVelocity": -0.4369301572101497,
+            "velocityX": -2.996463996264777,
+            "velocityY": -1.7043234990188791,
+            "timestamp": 3.1721420193939203
+        },
+        {
+            "x": 4.495427770658729,
+            "y": 5.69219987124691,
+            "heading": 3.1738800653247865,
+            "angularVelocity": -0.32762366082757177,
+            "velocityX": -2.2473487172898445,
+            "velocityY": -1.278243123833972,
+            "timestamp": 3.275565102695934
+        },
+        {
+            "x": 4.340475927186926,
+            "y": 5.604066625906228,
+            "heading": 3.1512930339800644,
+            "angularVelocity": -0.2183944881894502,
+            "velocityX": -1.4982326819566802,
+            "velocityY": -0.8521622303921732,
+            "timestamp": 3.3789881859979474
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -0.10919258660163363,
+            "velocityX": -0.7491163936844148,
+            "velocityY": -0.4260811464839441,
+            "timestamp": 3.482411269299961
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 8.84233842596168e-24,
+            "velocityY": 0,
+            "timestamp": 3.5858343526019745
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S2.traj
new file mode 100644
index 00000000..30e62dac
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S2.traj	
@@ -0,0 +1,355 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3566378052654167,
+            "y": 4.392308940927803,
+            "heading": 2.7577639052389085,
+            "angularVelocity": 0.08777137611433758,
+            "velocityX": 0.6402935063310357,
+            "velocityY": 0.3652543557357081,
+            "timestamp": 0.08845600448138136
+        },
+        {
+            "x": 2.469918670668774,
+            "y": 4.456929940384967,
+            "heading": 2.772608563363178,
+            "angularVelocity": 0.16781967726556357,
+            "velocityX": 1.2806464192852178,
+            "velocityY": 0.7305439561286646,
+            "timestamp": 0.1769120089627627
+        },
+        {
+            "x": 2.639850302646433,
+            "y": 4.553867610725804,
+            "heading": 2.793388618859665,
+            "angularVelocity": 0.2349196712910238,
+            "velocityX": 1.92108645392669,
+            "velocityY": 1.0958856994410031,
+            "timestamp": 0.26536801344414407
+        },
+        {
+            "x": 2.8664440757683463,
+            "y": 4.683128905196596,
+            "heading": 2.8177875607011336,
+            "angularVelocity": 0.27583138063398066,
+            "velocityX": 2.561655078707626,
+            "velocityY": 1.4613060495853287,
+            "timestamp": 0.3538240179255254
+        },
+        {
+            "x": 3.1496996536335864,
+            "y": 4.844715100104656,
+            "heading": 2.8386513493911516,
+            "angularVelocity": 0.23586627965327242,
+            "velocityX": 3.2022199004575587,
+            "velocityY": 1.8267408284540865,
+            "timestamp": 0.4422800224069068
+        },
+        {
+            "x": 3.44035847389222,
+            "y": 5.010517813781029,
+            "heading": 2.83865135522876,
+            "angularVelocity": 6.599448670952606e-8,
+            "velocityX": 3.28591396324954,
+            "velocityY": 1.8744088052413612,
+            "timestamp": 0.5307360268882881
+        },
+        {
+            "x": 3.731017294885288,
+            "y": 5.17632052617049,
+            "heading": 2.8386513610659576,
+            "angularVelocity": 6.598983540705142e-8,
+            "velocityX": 3.285913971552369,
+            "velocityY": 1.8744087906927347,
+            "timestamp": 0.6191920313696695
+        },
+        {
+            "x": 4.021676115881456,
+            "y": 5.3421232385545165,
+            "heading": 2.8386513669031546,
+            "angularVelocity": 6.598983544244374e-8,
+            "velocityX": 3.285913971587413,
+            "velocityY": 1.8744087906313014,
+            "timestamp": 0.7076480358510508
+        },
+        {
+            "x": 4.31233493687768,
+            "y": 5.507925950938444,
+            "heading": 2.838651372740352,
+            "angularVelocity": 6.598983514854984e-8,
+            "velocityX": 3.2859139715880556,
+            "velocityY": 1.8744087906301747,
+            "timestamp": 0.7961040403324322
+        },
+        {
+            "x": 4.602993757873904,
+            "y": 5.673728663322372,
+            "heading": 2.8386513785775485,
+            "angularVelocity": 6.598983509017798e-8,
+            "velocityX": 3.2859139715880414,
+            "velocityY": 1.8744087906301996,
+            "timestamp": 0.8845600448138136
+        },
+        {
+            "x": 4.893652578870126,
+            "y": 5.839531375706303,
+            "heading": 2.8386513844147463,
+            "angularVelocity": 6.598983636221009e-8,
+            "velocityX": 3.2859139715880246,
+            "velocityY": 1.8744087906302287,
+            "timestamp": 0.9730160492951949
+        },
+        {
+            "x": 5.184311399866346,
+            "y": 6.005334088090239,
+            "heading": 2.8386513902519432,
+            "angularVelocity": 6.598983502831849e-8,
+            "velocityX": 3.285913971588007,
+            "velocityY": 1.8744087906302613,
+            "timestamp": 1.0614720537765763
+        },
+        {
+            "x": 5.474970220862562,
+            "y": 6.171136800474182,
+            "heading": 2.83865139608914,
+            "angularVelocity": 6.598983586934159e-8,
+            "velocityX": 3.2859139715879397,
+            "velocityY": 1.8744087906303781,
+            "timestamp": 1.1499280582579576
+        },
+        {
+            "x": 5.765629041858637,
+            "y": 6.336939512858371,
+            "heading": 2.8386514019263376,
+            "angularVelocity": 6.598983523621195e-8,
+            "velocityX": 3.2859139715863668,
+            "velocityY": 1.8744087906331355,
+            "timestamp": 1.238384062739339
+        },
+        {
+            "x": 6.0562878628501755,
+            "y": 6.502742225250514,
+            "heading": 2.8386514077635345,
+            "angularVelocity": 6.598983511287735e-8,
+            "velocityX": 3.285913971535071,
+            "velocityY": 1.87440879072306,
+            "timestamp": 1.3268400672207203
+        },
+        {
+            "x": 6.3469466835037505,
+            "y": 6.6685449382351205,
+            "heading": 2.8386514136007315,
+            "angularVelocity": 6.598983592798126e-8,
+            "velocityX": 3.2859139677143814,
+            "velocityY": 1.8744087974208865,
+            "timestamp": 1.4152960717021017
+        },
+        {
+            "x": 6.6376054603552355,
+            "y": 6.834347728006541,
+            "heading": 2.838651419437929,
+            "angularVelocity": 6.598983535546771e-8,
+            "velocityX": 3.2859134725293426,
+            "velocityY": 1.8744096655000533,
+            "timestamp": 1.503752076183483
+        },
+        {
+            "x": 6.928282728448998,
+            "y": 7.000118097747795,
+            "heading": 2.8386514252461743,
+            "angularVelocity": 6.56625327771565e-8,
+            "velocityX": 3.286122517041433,
+            "velocityY": 1.8740431552739683,
+            "timestamp": 1.5922080806648644
+        },
+        {
+            "x": 7.191968889959837,
+            "y": 7.158086456014882,
+            "heading": 2.9053215778244037,
+            "angularVelocity": 0.7537097449644049,
+            "velocityX": 2.980986571311004,
+            "velocityY": 1.7858409860730329,
+            "timestamp": 1.6806640851462458
+        },
+        {
+            "x": 7.39896469669124,
+            "y": 7.2837659035610995,
+            "heading": 2.9729435938200757,
+            "angularVelocity": 0.7644706132968896,
+            "velocityX": 2.3400989898312643,
+            "velocityY": 1.4208130729292534,
+            "timestamp": 1.7691200896276271
+        },
+        {
+            "x": 7.549292841201835,
+            "y": 7.377144263475716,
+            "heading": 3.03570050164413,
+            "angularVelocity": 0.7094702976016349,
+            "velocityX": 1.6994679489759839,
+            "velocityY": 1.0556474991393678,
+            "timestamp": 1.8575760941090085
+        },
+        {
+            "x": 7.642968334049115,
+            "y": 7.4382219539742405,
+            "heading": 3.0916990951391803,
+            "angularVelocity": 0.6330671820796432,
+            "velocityX": 1.0590066032994798,
+            "velocityY": 0.6904866532988578,
+            "timestamp": 1.9460320985903898
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 0.546044388326283,
+            "velocityX": 0.41864502209875154,
+            "velocityY": 0.32533739449908106,
+            "timestamp": 2.034488103071771
+        },
+        {
+            "x": 7.645871150286324,
+            "y": 7.456493783242831,
+            "heading": 3.185181806591133,
+            "angularVelocity": 0.43686385232963987,
+            "velocityX": -0.329992576357443,
+            "velocityY": -0.10158483407911743,
+            "timestamp": 2.1379111863737847
+        },
+        {
+            "x": 7.534332014927772,
+            "y": 7.401805741681953,
+            "heading": 3.219069089269022,
+            "angularVelocity": 0.32765685953225704,
+            "velocityX": -1.078474280570675,
+            "velocityY": -0.5287798411617203,
+            "timestamp": 2.2413342696757983
+        },
+        {
+            "x": 7.345414230675068,
+            "y": 7.30288053546568,
+            "heading": 3.2416598317701943,
+            "angularVelocity": 0.21843037144032681,
+            "velocityX": -1.826650088365854,
+            "velocityY": -0.9565099304515025,
+            "timestamp": 2.344757352977812
+        },
+        {
+            "x": 7.079208201344681,
+            "y": 7.159560464323527,
+            "heading": 3.2529526054228244,
+            "angularVelocity": 0.1091900694900192,
+            "velocityX": -2.573951779729956,
+            "velocityY": -1.3857648270226828,
+            "timestamp": 2.4481804362798254
+        },
+        {
+            "x": 6.738167350146867,
+            "y": 6.967824923059757,
+            "heading": 3.25295260062049,
+            "angularVelocity": -4.6433873545046094e-8,
+            "velocityX": -3.29753126970623,
+            "velocityY": -1.853895041050502,
+            "timestamp": 2.551603519581839
+        },
+        {
+            "x": 6.398085530846708,
+            "y": 6.774393439892416,
+            "heading": 3.252952596202531,
+            "angularVelocity": -4.2717337144488564e-8,
+            "velocityX": -3.2882583698173717,
+            "velocityY": -1.8702931395158464,
+            "timestamp": 2.6550266028838525
+        },
+        {
+            "x": 6.058003908621006,
+            "y": 6.580961610238813,
+            "heading": 3.252952591784573,
+            "angularVelocity": -4.2717336082734796e-8,
+            "velocityX": -3.288256464300223,
+            "velocityY": -1.8702964896989283,
+            "timestamp": 2.758449686185866
+        },
+        {
+            "x": 5.717922287179122,
+            "y": 6.387529779207143,
+            "heading": 3.252952587366614,
+            "angularVelocity": -4.271733673837129e-8,
+            "velocityX": -3.28825645672147,
+            "velocityY": -1.8702965030234862,
+            "timestamp": 2.8618727694878796
+        },
+        {
+            "x": 5.377840665722721,
+            "y": 6.194097948200997,
+            "heading": 3.2529525829486556,
+            "angularVelocity": -4.271733567219934e-8,
+            "velocityX": -3.2882564568618378,
+            "velocityY": -1.8702965027766985,
+            "timestamp": 2.965295852789893
+        },
+        {
+            "x": 5.037759049752838,
+            "y": 6.000666107536036,
+            "heading": 3.252952578536604,
+            "angularVelocity": -4.266021621617093e-8,
+            "velocityX": -3.288256403812571,
+            "velocityY": -1.8702965961679845,
+            "timestamp": 3.0687189360919067
+        },
+        {
+            "x": 4.7278555042556665,
+            "y": 5.824399716323421,
+            "heading": 3.2077639144902763,
+            "angularVelocity": -0.4369301572101497,
+            "velocityX": -2.996463996264777,
+            "velocityY": -1.7043234990188791,
+            "timestamp": 3.1721420193939203
+        },
+        {
+            "x": 4.495427770658729,
+            "y": 5.69219987124691,
+            "heading": 3.1738800653247865,
+            "angularVelocity": -0.32762366082757177,
+            "velocityX": -2.2473487172898445,
+            "velocityY": -1.278243123833972,
+            "timestamp": 3.275565102695934
+        },
+        {
+            "x": 4.340475927186926,
+            "y": 5.604066625906228,
+            "heading": 3.1512930339800644,
+            "angularVelocity": -0.2183944881894502,
+            "velocityX": -1.4982326819566802,
+            "velocityY": -0.8521622303921732,
+            "timestamp": 3.3789881859979474
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -0.10919258660163363,
+            "velocityX": -0.7491163936844148,
+            "velocityY": -0.4260811464839441,
+            "timestamp": 3.482411269299961
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 8.84233842596168e-24,
+            "velocityY": 0,
+            "timestamp": 3.5858343526019745
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S1.1.traj
new file mode 100644
index 00000000..a687772f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S1.1.traj	
@@ -0,0 +1,328 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3590330484085675,
+            "y": 4.366649962138697,
+            "heading": 2.7576232354660704,
+            "angularVelocity": 0.09028523567993728,
+            "velocityX": 0.6991536221325528,
+            "velocityY": 0.07875834370159086,
+            "timestamp": 0.08443501762674911
+        },
+        {
+            "x": 2.4770991403474683,
+            "y": 4.379949883012471,
+            "heading": 2.7728702101731937,
+            "angularVelocity": 0.18057643778228707,
+            "velocityX": 1.3983071864900902,
+            "velocityY": 0.15751664709264293,
+            "timestamp": 0.16887003525349822
+        },
+        {
+            "x": 2.6541982667626414,
+            "y": 4.399899758130858,
+            "heading": 2.7957418198907438,
+            "angularVelocity": 0.27087824886418327,
+            "velocityX": 2.097460643616519,
+            "velocityY": 0.23627489730122597,
+            "timestamp": 0.25330505288024735
+        },
+        {
+            "x": 2.8903304124163527,
+            "y": 4.426499582699937,
+            "heading": 2.8262393577196585,
+            "angularVelocity": 0.361195374693129,
+            "velocityX": 2.796613920275944,
+            "velocityY": 0.31503309073333907,
+            "timestamp": 0.33774007050699645
+        },
+        {
+            "x": 3.1854955469851514,
+            "y": 4.459749351941674,
+            "heading": 2.864364443644879,
+            "angularVelocity": 0.4515316866960985,
+            "velocityX": 3.4957668378017823,
+            "velocityY": 0.3937912275771662,
+            "timestamp": 0.42217508813374555
+        },
+        {
+            "x": 3.5029006029698784,
+            "y": 4.495505051079763,
+            "heading": 2.8643644482492165,
+            "angularVelocity": 5.453113642276571e-8,
+            "velocityX": 3.7591637321358573,
+            "velocityY": 0.4234700263361048,
+            "timestamp": 0.5066101057604947
+        },
+        {
+            "x": 3.820305658948058,
+            "y": 4.531260750276139,
+            "heading": 2.864364452853533,
+            "angularVelocity": 5.453089037976801e-8,
+            "velocityX": 3.759163732058311,
+            "velocityY": 0.4234700270264227,
+            "timestamp": 0.5910451233872438
+        },
+        {
+            "x": 4.1377107149262375,
+            "y": 4.567016449472515,
+            "heading": 2.8643644574578495,
+            "angularVelocity": 5.4530890155242205e-8,
+            "velocityX": 3.75916373205831,
+            "velocityY": 0.4234700270264262,
+            "timestamp": 0.6754801410139929
+        },
+        {
+            "x": 4.455115770904417,
+            "y": 4.602772148668891,
+            "heading": 2.864364462062166,
+            "angularVelocity": 5.453089038095968e-8,
+            "velocityX": 3.7591637320583104,
+            "velocityY": 0.4234700270264258,
+            "timestamp": 0.759915158640742
+        },
+        {
+            "x": 4.772520826882595,
+            "y": 4.638527847865267,
+            "heading": 2.864364466666483,
+            "angularVelocity": 5.4530891020586436e-8,
+            "velocityX": 3.7591637320583104,
+            "velocityY": 0.4234700270264263,
+            "timestamp": 0.8443501762674911
+        },
+        {
+            "x": 5.089925882860775,
+            "y": 4.674283547061643,
+            "heading": 2.8643644712708,
+            "angularVelocity": 5.4530890812107064e-8,
+            "velocityX": 3.75916373205831,
+            "velocityY": 0.4234700270264263,
+            "timestamp": 0.9287851938942402
+        },
+        {
+            "x": 5.407330938838954,
+            "y": 4.7100392462580185,
+            "heading": 2.8643644758751163,
+            "angularVelocity": 5.453088997566471e-8,
+            "velocityX": 3.7591637320583104,
+            "velocityY": 0.4234700270264265,
+            "timestamp": 1.0132202115209894
+        },
+        {
+            "x": 5.7247359948171335,
+            "y": 4.745794945454395,
+            "heading": 2.864364480479433,
+            "angularVelocity": 5.453089022113266e-8,
+            "velocityX": 3.7591637320583104,
+            "velocityY": 0.42347002702642605,
+            "timestamp": 1.0976552291477386
+        },
+        {
+            "x": 6.04214105079524,
+            "y": 4.7815506446514116,
+            "heading": 2.8643644850837493,
+            "angularVelocity": 5.453089025367266e-8,
+            "velocityX": 3.759163732057455,
+            "velocityY": 0.42347002703401615,
+            "timestamp": 1.1820902467744878
+        },
+        {
+            "x": 6.359546093014576,
+            "y": 4.817306465985485,
+            "heading": 2.8643644896880662,
+            "angularVelocity": 5.4530891813953255e-8,
+            "velocityX": 3.7591635691064473,
+            "velocityY": 0.42347147355537545,
+            "timestamp": 1.266525264401237
+        },
+        {
+            "x": 6.6740817237038605,
+            "y": 4.872910210638354,
+            "heading": 2.8643644943647257,
+            "angularVelocity": 5.53876754719752e-8,
+            "velocityX": 3.7251799020130547,
+            "velocityY": 0.6585389121213792,
+            "timestamp": 1.3509602820279862
+        },
+        {
+            "x": 6.972867149310035,
+            "y": 4.985833654432599,
+            "heading": 2.8643645123115293,
+            "angularVelocity": 2.1255166363796299e-7,
+            "velocityX": 3.5386434918149345,
+            "velocityY": 1.337400606623087,
+            "timestamp": 1.4353952996547354
+        },
+        {
+            "x": 7.232618211035156,
+            "y": 5.1421799444134875,
+            "heading": 2.9041055246483745,
+            "angularVelocity": 0.4706697938114134,
+            "velocityX": 3.0763428376762785,
+            "velocityY": 1.8516759322776348,
+            "timestamp": 1.5198303172814847
+        },
+        {
+            "x": 7.433588885971066,
+            "y": 5.306272472318755,
+            "heading": 2.9592505290938482,
+            "angularVelocity": 0.6531058557865896,
+            "velocityX": 2.3801815950855185,
+            "velocityY": 1.9434179386406838,
+            "timestamp": 1.6042653349082339
+        },
+        {
+            "x": 7.575142359826725,
+            "y": 5.471558749180071,
+            "heading": 3.0192595634287165,
+            "angularVelocity": 0.7107126405793248,
+            "velocityX": 1.676478288681205,
+            "velocityY": 1.9575560177173887,
+            "timestamp": 1.688700352534983
+        },
+        {
+            "x": 7.657265359417412,
+            "y": 5.635564900043681,
+            "heading": 3.0801944156150345,
+            "angularVelocity": 0.7216774970745534,
+            "velocityX": 0.9726177822774612,
+            "velocityY": 1.9423949384201076,
+            "timestamp": 1.7731353701617323
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 0.7083030958711996,
+            "velocityX": 0.26925606486028053,
+            "velocityY": 1.9119448836968793,
+            "timestamp": 1.8575703877884815
+        },
+        {
+            "x": 7.638446013209911,
+            "y": 5.962794429881767,
+            "heading": 3.200158410841702,
+            "angularVelocity": 0.6785760380077615,
+            "velocityX": -0.46872148590566404,
+            "velocityY": 1.870131304647713,
+            "timestamp": 1.9462242857633076
+        },
+        {
+            "x": 7.531523237847712,
+            "y": 6.124072395344993,
+            "heading": 3.2564305280159678,
+            "angularVelocity": 0.6347393454740654,
+            "velocityX": -1.206069646171223,
+            "velocityY": 1.8191863995537148,
+            "timestamp": 2.034878183738134
+        },
+        {
+            "x": 7.359349891233686,
+            "y": 6.279498639853745,
+            "heading": 3.30674894531401,
+            "angularVelocity": 0.5675826833054816,
+            "velocityX": -1.9420843363584046,
+            "velocityY": 1.7531800412530831,
+            "timestamp": 2.1235320817129604
+        },
+        {
+            "x": 7.122252404274228,
+            "y": 6.426460665486359,
+            "heading": 3.3470467672011193,
+            "angularVelocity": 0.45455217207203097,
+            "velocityX": -2.6744169447211816,
+            "velocityY": 1.6577051769832414,
+            "timestamp": 2.2121859796877867
+        },
+        {
+            "x": 6.821828497038046,
+            "y": 6.5576759176562405,
+            "heading": 3.365711055371459,
+            "angularVelocity": 0.2105298085780703,
+            "velocityX": -3.388727558504971,
+            "velocityY": 1.4800844087773837,
+            "timestamp": 2.300839877662613
+        },
+        {
+            "x": 6.494234201783015,
+            "y": 6.629485644322343,
+            "heading": 3.365711060189302,
+            "angularVelocity": 5.434439690558349e-8,
+            "velocityX": -3.6952046411772423,
+            "velocityY": 0.8100007817647606,
+            "timestamp": 2.3894937756374395
+        },
+        {
+            "x": 6.158929367283791,
+            "y": 6.636218351220065,
+            "heading": 3.3657110616036774,
+            "angularVelocity": 1.5953902696988204e-8,
+            "velocityX": -3.782178134958452,
+            "velocityY": 0.07594372104916906,
+            "timestamp": 2.478147673612266
+        },
+        {
+            "x": 5.8312862675507695,
+            "y": 6.634145572711449,
+            "heading": 3.3782020006814046,
+            "angularVelocity": 0.14089554281384958,
+            "velocityX": -3.6957551468978513,
+            "velocityY": -0.023380568209240477,
+            "timestamp": 2.566801571587092
+        },
+        {
+            "x": 5.569171767766223,
+            "y": 6.6324873452973785,
+            "heading": 3.388195171082853,
+            "angularVelocity": 0.11272116206650896,
+            "velocityX": -2.9566043430935838,
+            "velocityY": -0.018704506535536753,
+            "timestamp": 2.6554554695619186
+        },
+        {
+            "x": 5.372585886333595,
+            "y": 6.631243673213895,
+            "heading": 3.395690186633039,
+            "angularVelocity": 0.08454242533492021,
+            "velocityX": -2.2174533317017797,
+            "velocityY": -0.014028397080035612,
+            "timestamp": 2.744109367536745
+        },
+        {
+            "x": 5.241528629389324,
+            "y": 6.630414557878557,
+            "heading": 3.4006869200557808,
+            "angularVelocity": 0.05636225295091492,
+            "velocityX": -1.4783022510920478,
+            "velocityY": -0.00935227163472713,
+            "timestamp": 2.8327632655115713
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.02818135672404778,
+            "velocityX": -0.7391511358917447,
+            "velocityY": -0.00467613819613763,
+            "timestamp": 2.9214171634863977
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0,
+            "velocityX": -2.6920195969080552e-36,
+            "velocityY": 0,
+            "timestamp": 3.010071061461224
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S1.traj
new file mode 100644
index 00000000..a687772f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S1.traj	
@@ -0,0 +1,328 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3590330484085675,
+            "y": 4.366649962138697,
+            "heading": 2.7576232354660704,
+            "angularVelocity": 0.09028523567993728,
+            "velocityX": 0.6991536221325528,
+            "velocityY": 0.07875834370159086,
+            "timestamp": 0.08443501762674911
+        },
+        {
+            "x": 2.4770991403474683,
+            "y": 4.379949883012471,
+            "heading": 2.7728702101731937,
+            "angularVelocity": 0.18057643778228707,
+            "velocityX": 1.3983071864900902,
+            "velocityY": 0.15751664709264293,
+            "timestamp": 0.16887003525349822
+        },
+        {
+            "x": 2.6541982667626414,
+            "y": 4.399899758130858,
+            "heading": 2.7957418198907438,
+            "angularVelocity": 0.27087824886418327,
+            "velocityX": 2.097460643616519,
+            "velocityY": 0.23627489730122597,
+            "timestamp": 0.25330505288024735
+        },
+        {
+            "x": 2.8903304124163527,
+            "y": 4.426499582699937,
+            "heading": 2.8262393577196585,
+            "angularVelocity": 0.361195374693129,
+            "velocityX": 2.796613920275944,
+            "velocityY": 0.31503309073333907,
+            "timestamp": 0.33774007050699645
+        },
+        {
+            "x": 3.1854955469851514,
+            "y": 4.459749351941674,
+            "heading": 2.864364443644879,
+            "angularVelocity": 0.4515316866960985,
+            "velocityX": 3.4957668378017823,
+            "velocityY": 0.3937912275771662,
+            "timestamp": 0.42217508813374555
+        },
+        {
+            "x": 3.5029006029698784,
+            "y": 4.495505051079763,
+            "heading": 2.8643644482492165,
+            "angularVelocity": 5.453113642276571e-8,
+            "velocityX": 3.7591637321358573,
+            "velocityY": 0.4234700263361048,
+            "timestamp": 0.5066101057604947
+        },
+        {
+            "x": 3.820305658948058,
+            "y": 4.531260750276139,
+            "heading": 2.864364452853533,
+            "angularVelocity": 5.453089037976801e-8,
+            "velocityX": 3.759163732058311,
+            "velocityY": 0.4234700270264227,
+            "timestamp": 0.5910451233872438
+        },
+        {
+            "x": 4.1377107149262375,
+            "y": 4.567016449472515,
+            "heading": 2.8643644574578495,
+            "angularVelocity": 5.4530890155242205e-8,
+            "velocityX": 3.75916373205831,
+            "velocityY": 0.4234700270264262,
+            "timestamp": 0.6754801410139929
+        },
+        {
+            "x": 4.455115770904417,
+            "y": 4.602772148668891,
+            "heading": 2.864364462062166,
+            "angularVelocity": 5.453089038095968e-8,
+            "velocityX": 3.7591637320583104,
+            "velocityY": 0.4234700270264258,
+            "timestamp": 0.759915158640742
+        },
+        {
+            "x": 4.772520826882595,
+            "y": 4.638527847865267,
+            "heading": 2.864364466666483,
+            "angularVelocity": 5.4530891020586436e-8,
+            "velocityX": 3.7591637320583104,
+            "velocityY": 0.4234700270264263,
+            "timestamp": 0.8443501762674911
+        },
+        {
+            "x": 5.089925882860775,
+            "y": 4.674283547061643,
+            "heading": 2.8643644712708,
+            "angularVelocity": 5.4530890812107064e-8,
+            "velocityX": 3.75916373205831,
+            "velocityY": 0.4234700270264263,
+            "timestamp": 0.9287851938942402
+        },
+        {
+            "x": 5.407330938838954,
+            "y": 4.7100392462580185,
+            "heading": 2.8643644758751163,
+            "angularVelocity": 5.453088997566471e-8,
+            "velocityX": 3.7591637320583104,
+            "velocityY": 0.4234700270264265,
+            "timestamp": 1.0132202115209894
+        },
+        {
+            "x": 5.7247359948171335,
+            "y": 4.745794945454395,
+            "heading": 2.864364480479433,
+            "angularVelocity": 5.453089022113266e-8,
+            "velocityX": 3.7591637320583104,
+            "velocityY": 0.42347002702642605,
+            "timestamp": 1.0976552291477386
+        },
+        {
+            "x": 6.04214105079524,
+            "y": 4.7815506446514116,
+            "heading": 2.8643644850837493,
+            "angularVelocity": 5.453089025367266e-8,
+            "velocityX": 3.759163732057455,
+            "velocityY": 0.42347002703401615,
+            "timestamp": 1.1820902467744878
+        },
+        {
+            "x": 6.359546093014576,
+            "y": 4.817306465985485,
+            "heading": 2.8643644896880662,
+            "angularVelocity": 5.4530891813953255e-8,
+            "velocityX": 3.7591635691064473,
+            "velocityY": 0.42347147355537545,
+            "timestamp": 1.266525264401237
+        },
+        {
+            "x": 6.6740817237038605,
+            "y": 4.872910210638354,
+            "heading": 2.8643644943647257,
+            "angularVelocity": 5.53876754719752e-8,
+            "velocityX": 3.7251799020130547,
+            "velocityY": 0.6585389121213792,
+            "timestamp": 1.3509602820279862
+        },
+        {
+            "x": 6.972867149310035,
+            "y": 4.985833654432599,
+            "heading": 2.8643645123115293,
+            "angularVelocity": 2.1255166363796299e-7,
+            "velocityX": 3.5386434918149345,
+            "velocityY": 1.337400606623087,
+            "timestamp": 1.4353952996547354
+        },
+        {
+            "x": 7.232618211035156,
+            "y": 5.1421799444134875,
+            "heading": 2.9041055246483745,
+            "angularVelocity": 0.4706697938114134,
+            "velocityX": 3.0763428376762785,
+            "velocityY": 1.8516759322776348,
+            "timestamp": 1.5198303172814847
+        },
+        {
+            "x": 7.433588885971066,
+            "y": 5.306272472318755,
+            "heading": 2.9592505290938482,
+            "angularVelocity": 0.6531058557865896,
+            "velocityX": 2.3801815950855185,
+            "velocityY": 1.9434179386406838,
+            "timestamp": 1.6042653349082339
+        },
+        {
+            "x": 7.575142359826725,
+            "y": 5.471558749180071,
+            "heading": 3.0192595634287165,
+            "angularVelocity": 0.7107126405793248,
+            "velocityX": 1.676478288681205,
+            "velocityY": 1.9575560177173887,
+            "timestamp": 1.688700352534983
+        },
+        {
+            "x": 7.657265359417412,
+            "y": 5.635564900043681,
+            "heading": 3.0801944156150345,
+            "angularVelocity": 0.7216774970745534,
+            "velocityX": 0.9726177822774612,
+            "velocityY": 1.9423949384201076,
+            "timestamp": 1.7731353701617323
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 0.7083030958711996,
+            "velocityX": 0.26925606486028053,
+            "velocityY": 1.9119448836968793,
+            "timestamp": 1.8575703877884815
+        },
+        {
+            "x": 7.638446013209911,
+            "y": 5.962794429881767,
+            "heading": 3.200158410841702,
+            "angularVelocity": 0.6785760380077615,
+            "velocityX": -0.46872148590566404,
+            "velocityY": 1.870131304647713,
+            "timestamp": 1.9462242857633076
+        },
+        {
+            "x": 7.531523237847712,
+            "y": 6.124072395344993,
+            "heading": 3.2564305280159678,
+            "angularVelocity": 0.6347393454740654,
+            "velocityX": -1.206069646171223,
+            "velocityY": 1.8191863995537148,
+            "timestamp": 2.034878183738134
+        },
+        {
+            "x": 7.359349891233686,
+            "y": 6.279498639853745,
+            "heading": 3.30674894531401,
+            "angularVelocity": 0.5675826833054816,
+            "velocityX": -1.9420843363584046,
+            "velocityY": 1.7531800412530831,
+            "timestamp": 2.1235320817129604
+        },
+        {
+            "x": 7.122252404274228,
+            "y": 6.426460665486359,
+            "heading": 3.3470467672011193,
+            "angularVelocity": 0.45455217207203097,
+            "velocityX": -2.6744169447211816,
+            "velocityY": 1.6577051769832414,
+            "timestamp": 2.2121859796877867
+        },
+        {
+            "x": 6.821828497038046,
+            "y": 6.5576759176562405,
+            "heading": 3.365711055371459,
+            "angularVelocity": 0.2105298085780703,
+            "velocityX": -3.388727558504971,
+            "velocityY": 1.4800844087773837,
+            "timestamp": 2.300839877662613
+        },
+        {
+            "x": 6.494234201783015,
+            "y": 6.629485644322343,
+            "heading": 3.365711060189302,
+            "angularVelocity": 5.434439690558349e-8,
+            "velocityX": -3.6952046411772423,
+            "velocityY": 0.8100007817647606,
+            "timestamp": 2.3894937756374395
+        },
+        {
+            "x": 6.158929367283791,
+            "y": 6.636218351220065,
+            "heading": 3.3657110616036774,
+            "angularVelocity": 1.5953902696988204e-8,
+            "velocityX": -3.782178134958452,
+            "velocityY": 0.07594372104916906,
+            "timestamp": 2.478147673612266
+        },
+        {
+            "x": 5.8312862675507695,
+            "y": 6.634145572711449,
+            "heading": 3.3782020006814046,
+            "angularVelocity": 0.14089554281384958,
+            "velocityX": -3.6957551468978513,
+            "velocityY": -0.023380568209240477,
+            "timestamp": 2.566801571587092
+        },
+        {
+            "x": 5.569171767766223,
+            "y": 6.6324873452973785,
+            "heading": 3.388195171082853,
+            "angularVelocity": 0.11272116206650896,
+            "velocityX": -2.9566043430935838,
+            "velocityY": -0.018704506535536753,
+            "timestamp": 2.6554554695619186
+        },
+        {
+            "x": 5.372585886333595,
+            "y": 6.631243673213895,
+            "heading": 3.395690186633039,
+            "angularVelocity": 0.08454242533492021,
+            "velocityX": -2.2174533317017797,
+            "velocityY": -0.014028397080035612,
+            "timestamp": 2.744109367536745
+        },
+        {
+            "x": 5.241528629389324,
+            "y": 6.630414557878557,
+            "heading": 3.4006869200557808,
+            "angularVelocity": 0.05636225295091492,
+            "velocityX": -1.4783022510920478,
+            "velocityY": -0.00935227163472713,
+            "timestamp": 2.8327632655115713
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.02818135672404778,
+            "velocityX": -0.7391511358917447,
+            "velocityY": -0.00467613819613763,
+            "timestamp": 2.9214171634863977
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0,
+            "velocityX": -2.6920195969080552e-36,
+            "velocityY": 0,
+            "timestamp": 3.010071061461224
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S2.1.traj
new file mode 100644
index 00000000..af09df59
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S2.1.traj	
@@ -0,0 +1,337 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3595382644510017,
+            "y": 4.372038786355543,
+            "heading": 2.7583958335298844,
+            "angularVelocity": 0.09833011550569279,
+            "velocityX": 0.6972987732113981,
+            "velocityY": 0.140995560318735,
+            "timestamp": 0.08538415201391997
+        },
+        {
+            "x": 2.478614789358208,
+            "y": 4.396116350384182,
+            "heading": 2.7751880425894457,
+            "angularVelocity": 0.19666657879115437,
+            "velocityX": 1.3945974996366264,
+            "velocityY": 0.28199101895061424,
+            "timestamp": 0.17076830402783993
+        },
+        {
+            "x": 2.657229566273028,
+            "y": 4.432232679689,
+            "heading": 2.800377642298772,
+            "angularVelocity": 0.2950149309349538,
+            "velocityX": 2.0918961271138583,
+            "velocityY": 0.4229863323925762,
+            "timestamp": 0.2561524560417599
+        },
+        {
+            "x": 2.8953825799844894,
+            "y": 4.480387759130421,
+            "heading": 2.8339661569504204,
+            "angularVelocity": 0.39338113525063506,
+            "velocityX": 2.7891945764435975,
+            "velocityY": 0.5639814685232187,
+            "timestamp": 0.34153660805567987
+        },
+        {
+            "x": 3.1930738006682495,
+            "y": 4.5405815710050295,
+            "heading": 2.8759554693739764,
+            "angularVelocity": 0.49176939084328164,
+            "velocityX": 3.486492676477345,
+            "velocityY": 0.7049763973154618,
+            "timestamp": 0.42692076006959984
+        },
+        {
+            "x": 3.5096695268696414,
+            "y": 4.604598695680213,
+            "heading": 2.8759554743130047,
+            "angularVelocity": 5.784479190315687e-8,
+            "velocityX": 3.707897996688871,
+            "velocityY": 0.7497541776224135,
+            "timestamp": 0.5123049120835198
+        },
+        {
+            "x": 3.8262652530617234,
+            "y": 4.6686158204015085,
+            "heading": 2.875955479252014,
+            "angularVelocity": 5.784456729975194e-8,
+            "velocityX": 3.7078979965798418,
+            "velocityY": 0.7497541781624566,
+            "timestamp": 0.5976890640974397
+        },
+        {
+            "x": 4.142860979253806,
+            "y": 4.732632945122804,
+            "heading": 2.875955484191023,
+            "angularVelocity": 5.784456771643219e-8,
+            "velocityX": 3.707897996579841,
+            "velocityY": 0.7497541781624594,
+            "timestamp": 0.6830732161113597
+        },
+        {
+            "x": 4.459456705445889,
+            "y": 4.796650069844099,
+            "heading": 2.8759554891300323,
+            "angularVelocity": 5.7844567227510466e-8,
+            "velocityX": 3.707897996579841,
+            "velocityY": 0.7497541781624593,
+            "timestamp": 0.7684573681252798
+        },
+        {
+            "x": 4.7760524316379716,
+            "y": 4.860667194565394,
+            "heading": 2.8759554940690415,
+            "angularVelocity": 5.784456749923784e-8,
+            "velocityX": 3.707897996579841,
+            "velocityY": 0.7497541781624593,
+            "timestamp": 0.8538415201391998
+        },
+        {
+            "x": 5.092648157830054,
+            "y": 4.92468431928669,
+            "heading": 2.8759554990080507,
+            "angularVelocity": 5.784456784777939e-8,
+            "velocityX": 3.707897996579841,
+            "velocityY": 0.749754178162459,
+            "timestamp": 0.9392256721531198
+        },
+        {
+            "x": 5.409243884022137,
+            "y": 4.988701444007985,
+            "heading": 2.87595550394706,
+            "angularVelocity": 5.784456724140334e-8,
+            "velocityX": 3.707897996579841,
+            "velocityY": 0.749754178162459,
+            "timestamp": 1.0246098241670398
+        },
+        {
+            "x": 5.72583961021422,
+            "y": 5.05271856872928,
+            "heading": 2.875955508886069,
+            "angularVelocity": 5.7844566753037437e-8,
+            "velocityX": 3.7078979965798413,
+            "velocityY": 0.7497541781624589,
+            "timestamp": 1.1099939761809599
+        },
+        {
+            "x": 6.042435336406303,
+            "y": 5.116735693450575,
+            "heading": 2.8759555138250783,
+            "angularVelocity": 5.784456768545098e-8,
+            "velocityX": 3.7078979965798413,
+            "velocityY": 0.749754178162459,
+            "timestamp": 1.1953781281948799
+        },
+        {
+            "x": 6.35903106259824,
+            "y": 5.180752818172588,
+            "heading": 2.8759555187640875,
+            "angularVelocity": 5.784456775109183e-8,
+            "velocityX": 3.707897996578142,
+            "velocityY": 0.7497541781708625,
+            "timestamp": 1.2807622802088
+        },
+        {
+            "x": 6.67562676006639,
+            "y": 5.244770084947493,
+            "heading": 2.875955523703097,
+            "angularVelocity": 5.7844569052705035e-8,
+            "velocityX": 3.7078976601715947,
+            "velocityY": 0.7497558418624185,
+            "timestamp": 1.36614643222272
+        },
+        {
+            "x": 6.986762276730635,
+            "y": 5.331521073675251,
+            "heading": 2.875955533952509,
+            "angularVelocity": 1.2003881364523766e-7,
+            "velocityX": 3.643949249663132,
+            "velocityY": 1.0160080844231525,
+            "timestamp": 1.45153058423664
+        },
+        {
+            "x": 7.245394684994352,
+            "y": 5.443936340264843,
+            "heading": 2.937208027811149,
+            "angularVelocity": 0.717375442794747,
+            "velocityX": 3.0290446431036937,
+            "velocityY": 1.3165823392058056,
+            "timestamp": 1.53691473625056
+        },
+        {
+            "x": 7.443822925582165,
+            "y": 5.547959848416938,
+            "heading": 2.996476692643064,
+            "angularVelocity": 0.694141283059797,
+            "velocityX": 2.3239469609707415,
+            "velocityY": 1.2182999502663732,
+            "timestamp": 1.62229888826448
+        },
+        {
+            "x": 7.58233184253966,
+            "y": 5.641838110863567,
+            "heading": 3.05066970535842,
+            "angularVelocity": 0.6346963861223477,
+            "velocityX": 1.6221853082866564,
+            "velocityY": 1.0994811125058057,
+            "timestamp": 1.7076830402784
+        },
+        {
+            "x": 7.661036876366553,
+            "y": 5.724955999091069,
+            "heading": 3.0986966592522744,
+            "angularVelocity": 0.5624808909037848,
+            "velocityX": 0.9217756687922859,
+            "velocityY": 0.9734580278311056,
+            "timestamp": 1.79306719229232
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 0.4837354447344324,
+            "velocityX": 0.22209184241070226,
+            "velocityY": 0.8437631481915462,
+            "timestamp": 1.87845134430624
+        },
+        {
+            "x": 7.620399591023171,
+            "y": 5.865894759292055,
+            "heading": 3.178669357704941,
+            "angularVelocity": 0.3869200928483079,
+            "velocityX": -0.5963532146324005,
+            "velocityY": 0.6893511618538622,
+            "timestamp": 1.978392801004126
+        },
+        {
+            "x": 7.479095963903043,
+            "y": 5.918870767960299,
+            "heading": 3.2076554876039776,
+            "angularVelocity": 0.29003109276922934,
+            "velocityX": -1.413863993870693,
+            "velocityY": 0.5300704074024576,
+            "timestamp": 2.078334257702012
+        },
+        {
+            "x": 7.256273297326838,
+            "y": 5.955010806465711,
+            "heading": 3.2269465299619338,
+            "angularVelocity": 0.19302342586691915,
+            "velocityX": -2.229531907362301,
+            "velocityY": 0.36161208470935113,
+            "timestamp": 2.178275714399898
+        },
+        {
+            "x": 6.95245768893108,
+            "y": 5.971943859159761,
+            "heading": 3.236518587853394,
+            "angularVelocity": 0.09577664972800967,
+            "velocityX": -3.0399357627352264,
+            "velocityY": 0.16942971669141613,
+            "timestamp": 2.278217171097784
+        },
+        {
+            "x": 6.575020543317634,
+            "y": 5.950033200191844,
+            "heading": 3.236518582605277,
+            "angularVelocity": -5.2511914101363804e-8,
+            "velocityX": -3.7765823921738897,
+            "velocityY": -0.21923493705070526,
+            "timestamp": 2.37815862779567
+        },
+        {
+            "x": 6.202215559947926,
+            "y": 5.88714179095661,
+            "heading": 3.2365185785582873,
+            "angularVelocity": -4.049360572502445e-8,
+            "velocityX": -3.730233635643953,
+            "velocityY": -0.6292824951045971,
+            "timestamp": 2.478100084493556
+        },
+        {
+            "x": 5.829410606689244,
+            "y": 5.824250203230788,
+            "heading": 3.2365185745112974,
+            "angularVelocity": -4.049360338896351e-8,
+            "velocityX": -3.7302333343573055,
+            "velocityY": -0.6292842810560431,
+            "timestamp": 2.578041541191442
+        },
+        {
+            "x": 5.456605653430655,
+            "y": 5.761358615504415,
+            "heading": 3.2365185704643076,
+            "angularVelocity": -4.049360329558589e-8,
+            "velocityX": -3.730233334356378,
+            "velocityY": -0.6292842810615451,
+            "timestamp": 2.677982997889328
+        },
+        {
+            "x": 5.0838007001693555,
+            "y": 5.698467027794241,
+            "heading": 3.2365185664173017,
+            "angularVelocity": -4.0493766096106506e-8,
+            "velocityX": -3.7302333343834975,
+            "velocityY": -0.6292842808994649,
+            "timestamp": 2.777924454587214
+        },
+        {
+            "x": 4.755480441979736,
+            "y": 5.643080213031107,
+            "heading": 3.197910664693085,
+            "angularVelocity": -0.38630517304670825,
+            "velocityX": -3.28512580301988,
+            "velocityY": -0.5541925902737567,
+            "timestamp": 2.8778659112851
+        },
+        {
+            "x": 4.50924022643339,
+            "y": 5.601540105644793,
+            "heading": 3.1689552309170463,
+            "angularVelocity": -0.2897239517287447,
+            "velocityX": -2.4638445714345307,
+            "velocityY": -0.41564440582335266,
+            "timestamp": 2.977807367982986
+        },
+        {
+            "x": 4.345080076326452,
+            "y": 5.573846702024279,
+            "heading": 3.149651762381054,
+            "angularVelocity": -0.19314776043684534,
+            "velocityX": -1.6425631117543122,
+            "velocityY": -0.2770962575043274,
+            "timestamp": 3.077748824680872
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -0.09657416151367515,
+            "velocityX": -0.8212815686144453,
+            "velocityY": -0.1385481308936372,
+            "timestamp": 3.177690281378758
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -1.245204860871434e-36,
+            "velocityY": 0,
+            "timestamp": 3.277631738076644
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S2.traj
new file mode 100644
index 00000000..af09df59
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S2.traj	
@@ -0,0 +1,337 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3595382644510017,
+            "y": 4.372038786355543,
+            "heading": 2.7583958335298844,
+            "angularVelocity": 0.09833011550569279,
+            "velocityX": 0.6972987732113981,
+            "velocityY": 0.140995560318735,
+            "timestamp": 0.08538415201391997
+        },
+        {
+            "x": 2.478614789358208,
+            "y": 4.396116350384182,
+            "heading": 2.7751880425894457,
+            "angularVelocity": 0.19666657879115437,
+            "velocityX": 1.3945974996366264,
+            "velocityY": 0.28199101895061424,
+            "timestamp": 0.17076830402783993
+        },
+        {
+            "x": 2.657229566273028,
+            "y": 4.432232679689,
+            "heading": 2.800377642298772,
+            "angularVelocity": 0.2950149309349538,
+            "velocityX": 2.0918961271138583,
+            "velocityY": 0.4229863323925762,
+            "timestamp": 0.2561524560417599
+        },
+        {
+            "x": 2.8953825799844894,
+            "y": 4.480387759130421,
+            "heading": 2.8339661569504204,
+            "angularVelocity": 0.39338113525063506,
+            "velocityX": 2.7891945764435975,
+            "velocityY": 0.5639814685232187,
+            "timestamp": 0.34153660805567987
+        },
+        {
+            "x": 3.1930738006682495,
+            "y": 4.5405815710050295,
+            "heading": 2.8759554693739764,
+            "angularVelocity": 0.49176939084328164,
+            "velocityX": 3.486492676477345,
+            "velocityY": 0.7049763973154618,
+            "timestamp": 0.42692076006959984
+        },
+        {
+            "x": 3.5096695268696414,
+            "y": 4.604598695680213,
+            "heading": 2.8759554743130047,
+            "angularVelocity": 5.784479190315687e-8,
+            "velocityX": 3.707897996688871,
+            "velocityY": 0.7497541776224135,
+            "timestamp": 0.5123049120835198
+        },
+        {
+            "x": 3.8262652530617234,
+            "y": 4.6686158204015085,
+            "heading": 2.875955479252014,
+            "angularVelocity": 5.784456729975194e-8,
+            "velocityX": 3.7078979965798418,
+            "velocityY": 0.7497541781624566,
+            "timestamp": 0.5976890640974397
+        },
+        {
+            "x": 4.142860979253806,
+            "y": 4.732632945122804,
+            "heading": 2.875955484191023,
+            "angularVelocity": 5.784456771643219e-8,
+            "velocityX": 3.707897996579841,
+            "velocityY": 0.7497541781624594,
+            "timestamp": 0.6830732161113597
+        },
+        {
+            "x": 4.459456705445889,
+            "y": 4.796650069844099,
+            "heading": 2.8759554891300323,
+            "angularVelocity": 5.7844567227510466e-8,
+            "velocityX": 3.707897996579841,
+            "velocityY": 0.7497541781624593,
+            "timestamp": 0.7684573681252798
+        },
+        {
+            "x": 4.7760524316379716,
+            "y": 4.860667194565394,
+            "heading": 2.8759554940690415,
+            "angularVelocity": 5.784456749923784e-8,
+            "velocityX": 3.707897996579841,
+            "velocityY": 0.7497541781624593,
+            "timestamp": 0.8538415201391998
+        },
+        {
+            "x": 5.092648157830054,
+            "y": 4.92468431928669,
+            "heading": 2.8759554990080507,
+            "angularVelocity": 5.784456784777939e-8,
+            "velocityX": 3.707897996579841,
+            "velocityY": 0.749754178162459,
+            "timestamp": 0.9392256721531198
+        },
+        {
+            "x": 5.409243884022137,
+            "y": 4.988701444007985,
+            "heading": 2.87595550394706,
+            "angularVelocity": 5.784456724140334e-8,
+            "velocityX": 3.707897996579841,
+            "velocityY": 0.749754178162459,
+            "timestamp": 1.0246098241670398
+        },
+        {
+            "x": 5.72583961021422,
+            "y": 5.05271856872928,
+            "heading": 2.875955508886069,
+            "angularVelocity": 5.7844566753037437e-8,
+            "velocityX": 3.7078979965798413,
+            "velocityY": 0.7497541781624589,
+            "timestamp": 1.1099939761809599
+        },
+        {
+            "x": 6.042435336406303,
+            "y": 5.116735693450575,
+            "heading": 2.8759555138250783,
+            "angularVelocity": 5.784456768545098e-8,
+            "velocityX": 3.7078979965798413,
+            "velocityY": 0.749754178162459,
+            "timestamp": 1.1953781281948799
+        },
+        {
+            "x": 6.35903106259824,
+            "y": 5.180752818172588,
+            "heading": 2.8759555187640875,
+            "angularVelocity": 5.784456775109183e-8,
+            "velocityX": 3.707897996578142,
+            "velocityY": 0.7497541781708625,
+            "timestamp": 1.2807622802088
+        },
+        {
+            "x": 6.67562676006639,
+            "y": 5.244770084947493,
+            "heading": 2.875955523703097,
+            "angularVelocity": 5.7844569052705035e-8,
+            "velocityX": 3.7078976601715947,
+            "velocityY": 0.7497558418624185,
+            "timestamp": 1.36614643222272
+        },
+        {
+            "x": 6.986762276730635,
+            "y": 5.331521073675251,
+            "heading": 2.875955533952509,
+            "angularVelocity": 1.2003881364523766e-7,
+            "velocityX": 3.643949249663132,
+            "velocityY": 1.0160080844231525,
+            "timestamp": 1.45153058423664
+        },
+        {
+            "x": 7.245394684994352,
+            "y": 5.443936340264843,
+            "heading": 2.937208027811149,
+            "angularVelocity": 0.717375442794747,
+            "velocityX": 3.0290446431036937,
+            "velocityY": 1.3165823392058056,
+            "timestamp": 1.53691473625056
+        },
+        {
+            "x": 7.443822925582165,
+            "y": 5.547959848416938,
+            "heading": 2.996476692643064,
+            "angularVelocity": 0.694141283059797,
+            "velocityX": 2.3239469609707415,
+            "velocityY": 1.2182999502663732,
+            "timestamp": 1.62229888826448
+        },
+        {
+            "x": 7.58233184253966,
+            "y": 5.641838110863567,
+            "heading": 3.05066970535842,
+            "angularVelocity": 0.6346963861223477,
+            "velocityX": 1.6221853082866564,
+            "velocityY": 1.0994811125058057,
+            "timestamp": 1.7076830402784
+        },
+        {
+            "x": 7.661036876366553,
+            "y": 5.724955999091069,
+            "heading": 3.0986966592522744,
+            "angularVelocity": 0.5624808909037848,
+            "velocityX": 0.9217756687922859,
+            "velocityY": 0.9734580278311056,
+            "timestamp": 1.79306719229232
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 0.4837354447344324,
+            "velocityX": 0.22209184241070226,
+            "velocityY": 0.8437631481915462,
+            "timestamp": 1.87845134430624
+        },
+        {
+            "x": 7.620399591023171,
+            "y": 5.865894759292055,
+            "heading": 3.178669357704941,
+            "angularVelocity": 0.3869200928483079,
+            "velocityX": -0.5963532146324005,
+            "velocityY": 0.6893511618538622,
+            "timestamp": 1.978392801004126
+        },
+        {
+            "x": 7.479095963903043,
+            "y": 5.918870767960299,
+            "heading": 3.2076554876039776,
+            "angularVelocity": 0.29003109276922934,
+            "velocityX": -1.413863993870693,
+            "velocityY": 0.5300704074024576,
+            "timestamp": 2.078334257702012
+        },
+        {
+            "x": 7.256273297326838,
+            "y": 5.955010806465711,
+            "heading": 3.2269465299619338,
+            "angularVelocity": 0.19302342586691915,
+            "velocityX": -2.229531907362301,
+            "velocityY": 0.36161208470935113,
+            "timestamp": 2.178275714399898
+        },
+        {
+            "x": 6.95245768893108,
+            "y": 5.971943859159761,
+            "heading": 3.236518587853394,
+            "angularVelocity": 0.09577664972800967,
+            "velocityX": -3.0399357627352264,
+            "velocityY": 0.16942971669141613,
+            "timestamp": 2.278217171097784
+        },
+        {
+            "x": 6.575020543317634,
+            "y": 5.950033200191844,
+            "heading": 3.236518582605277,
+            "angularVelocity": -5.2511914101363804e-8,
+            "velocityX": -3.7765823921738897,
+            "velocityY": -0.21923493705070526,
+            "timestamp": 2.37815862779567
+        },
+        {
+            "x": 6.202215559947926,
+            "y": 5.88714179095661,
+            "heading": 3.2365185785582873,
+            "angularVelocity": -4.049360572502445e-8,
+            "velocityX": -3.730233635643953,
+            "velocityY": -0.6292824951045971,
+            "timestamp": 2.478100084493556
+        },
+        {
+            "x": 5.829410606689244,
+            "y": 5.824250203230788,
+            "heading": 3.2365185745112974,
+            "angularVelocity": -4.049360338896351e-8,
+            "velocityX": -3.7302333343573055,
+            "velocityY": -0.6292842810560431,
+            "timestamp": 2.578041541191442
+        },
+        {
+            "x": 5.456605653430655,
+            "y": 5.761358615504415,
+            "heading": 3.2365185704643076,
+            "angularVelocity": -4.049360329558589e-8,
+            "velocityX": -3.730233334356378,
+            "velocityY": -0.6292842810615451,
+            "timestamp": 2.677982997889328
+        },
+        {
+            "x": 5.0838007001693555,
+            "y": 5.698467027794241,
+            "heading": 3.2365185664173017,
+            "angularVelocity": -4.0493766096106506e-8,
+            "velocityX": -3.7302333343834975,
+            "velocityY": -0.6292842808994649,
+            "timestamp": 2.777924454587214
+        },
+        {
+            "x": 4.755480441979736,
+            "y": 5.643080213031107,
+            "heading": 3.197910664693085,
+            "angularVelocity": -0.38630517304670825,
+            "velocityX": -3.28512580301988,
+            "velocityY": -0.5541925902737567,
+            "timestamp": 2.8778659112851
+        },
+        {
+            "x": 4.50924022643339,
+            "y": 5.601540105644793,
+            "heading": 3.1689552309170463,
+            "angularVelocity": -0.2897239517287447,
+            "velocityX": -2.4638445714345307,
+            "velocityY": -0.41564440582335266,
+            "timestamp": 2.977807367982986
+        },
+        {
+            "x": 4.345080076326452,
+            "y": 5.573846702024279,
+            "heading": 3.149651762381054,
+            "angularVelocity": -0.19314776043684534,
+            "velocityX": -1.6425631117543122,
+            "velocityY": -0.2770962575043274,
+            "timestamp": 3.077748824680872
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -0.09657416151367515,
+            "velocityX": -0.8212815686144453,
+            "velocityY": -0.1385481308936372,
+            "timestamp": 3.177690281378758
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -1.245204860871434e-36,
+            "velocityY": 0,
+            "timestamp": 3.277631738076644
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S1.1.traj
new file mode 100644
index 00000000..fb79e562
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S1.1.traj	
@@ -0,0 +1,346 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.358164523227949,
+            "y": 4.34513375016413,
+            "heading": 2.762567979975215,
+            "angularVelocity": 0.1479885663322048,
+            "velocityX": 0.6848900476349593,
+            "velocityY": -0.17505080405008622,
+            "timestamp": 0.08492534448237501
+        },
+        {
+            "x": 2.474493569181997,
+            "y": 4.315401331546784,
+            "heading": 2.787705678078292,
+            "angularVelocity": 0.2959975994950906,
+            "velocityX": 1.3697800893606054,
+            "velocityY": -0.35010065368078286,
+            "timestamp": 0.16985068896475003
+        },
+        {
+            "x": 2.6489871423142275,
+            "y": 4.270802870950086,
+            "heading": 2.825415221133538,
+            "angularVelocity": 0.4440316761160991,
+            "velocityX": 2.054670183509755,
+            "velocityY": -0.525149010210423,
+            "timestamp": 0.25477603344712507
+        },
+        {
+            "x": 2.881645266961739,
+            "y": 4.211338540895288,
+            "heading": 2.8756978223326772,
+            "angularVelocity": 0.5920800381277782,
+            "velocityX": 2.739560564229402,
+            "velocityY": -0.7001953352940375,
+            "timestamp": 0.33970137792950006
+        },
+        {
+            "x": 3.172468005146889,
+            "y": 4.137008553769027,
+            "heading": 2.9385516414717108,
+            "angularVelocity": 0.7401067316492073,
+            "velocityX": 3.4244516752652747,
+            "velocityY": -0.8752391595148364,
+            "timestamp": 0.42462672241187505
+        },
+        {
+            "x": 3.48373017288665,
+            "y": 4.0574555172899185,
+            "heading": 2.9385516489955585,
+            "angularVelocity": 8.859366728426556e-8,
+            "velocityX": 3.6651269375110873,
+            "velocityY": -0.936740815877624,
+            "timestamp": 0.50955206689425
+        },
+        {
+            "x": 3.7949923406136996,
+            "y": 3.977902480761,
+            "heading": 2.9385516565193726,
+            "angularVelocity": 8.859327100912678e-8,
+            "velocityX": 3.665126937361409,
+            "velocityY": -0.9367408164641361,
+            "timestamp": 0.594477411376625
+        },
+        {
+            "x": 4.10625450834075,
+            "y": 3.898349444232082,
+            "heading": 2.938551664043187,
+            "angularVelocity": 8.859327019599772e-8,
+            "velocityX": 3.665126937361408,
+            "velocityY": -0.9367408164641391,
+            "timestamp": 0.679402755859
+        },
+        {
+            "x": 4.417516676067799,
+            "y": 3.818796407703164,
+            "heading": 2.938551671567001,
+            "angularVelocity": 8.859327006115765e-8,
+            "velocityX": 3.6651269373614084,
+            "velocityY": -0.9367408164641391,
+            "timestamp": 0.764328100341375
+        },
+        {
+            "x": 4.728778843794849,
+            "y": 3.7392433711742457,
+            "heading": 2.938551679090815,
+            "angularVelocity": 8.859327000476347e-8,
+            "velocityX": 3.665126937361408,
+            "velocityY": -0.9367408164641391,
+            "timestamp": 0.84925344482375
+        },
+        {
+            "x": 5.0400410115218985,
+            "y": 3.6596903346453273,
+            "heading": 2.9385516866146295,
+            "angularVelocity": 8.85932714022833e-8,
+            "velocityX": 3.665126937361408,
+            "velocityY": -0.9367408164641392,
+            "timestamp": 0.934178789306125
+        },
+        {
+            "x": 5.351303179248949,
+            "y": 3.580137298116409,
+            "heading": 2.9385516941384435,
+            "angularVelocity": 8.859327127858633e-8,
+            "velocityX": 3.665126937361408,
+            "velocityY": -0.936740816464139,
+            "timestamp": 1.0191041337885
+        },
+        {
+            "x": 5.662565346976546,
+            "y": 3.5005842615896343,
+            "heading": 2.938551701662258,
+            "angularVelocity": 8.859327107695318e-8,
+            "velocityX": 3.6651269373678588,
+            "velocityY": -0.9367408164388998,
+            "timestamp": 1.104029478270875
+        },
+        {
+            "x": 5.973827620415735,
+            "y": 3.4210316386751205,
+            "heading": 2.9385517091860733,
+            "angularVelocity": 8.859328561882092e-8,
+            "velocityX": 3.6651281821269137,
+            "velocityY": -0.9367359461347136,
+            "timestamp": 1.18895482275325
+        },
+        {
+            "x": 6.292581444856613,
+            "y": 3.380921520397279,
+            "heading": 2.9385517170132656,
+            "angularVelocity": 9.216556583241828e-8,
+            "velocityX": 3.753341554087312,
+            "velocityY": -0.4722985643722118,
+            "timestamp": 1.273880167235625
+        },
+        {
+            "x": 6.61322600810305,
+            "y": 3.400918618969079,
+            "heading": 2.938551726316265,
+            "angularVelocity": 1.0954326272813381e-7,
+            "velocityX": 3.7756050941068793,
+            "velocityY": 0.23546679373140608,
+            "timestamp": 1.358805511718
+        },
+        {
+            "x": 6.924526035610863,
+            "y": 3.4803224362243568,
+            "heading": 2.9385522977985694,
+            "angularVelocity": 0.000006729231518347491,
+            "velocityX": 3.665572738093738,
+            "velocityY": 0.9349837523680175,
+            "timestamp": 1.443730856200375
+        },
+        {
+            "x": 7.193617949641073,
+            "y": 3.601275977700439,
+            "heading": 2.9756713722126755,
+            "angularVelocity": 0.43707887957768976,
+            "velocityX": 3.1685701797306898,
+            "velocityY": 1.4242337456892449,
+            "timestamp": 1.52865620068275
+        },
+        {
+            "x": 7.4107131876465635,
+            "y": 3.752187582369628,
+            "heading": 3.026776803352479,
+            "angularVelocity": 0.6017689001003675,
+            "velocityX": 2.5563068284114614,
+            "velocityY": 1.7769913750601078,
+            "timestamp": 1.613581545165125
+        },
+        {
+            "x": 7.573210012728862,
+            "y": 3.928266853326757,
+            "heading": 3.0830399478590267,
+            "angularVelocity": 0.6625012220965879,
+            "velocityX": 1.9134079004651277,
+            "velocityY": 2.0733418513677266,
+            "timestamp": 1.6985068896475
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.6707073428803676,
+            "velocityX": 1.257457216359011,
+            "velocityY": 2.340092323257951,
+            "timestamp": 1.783432234129875
+        },
+        {
+            "x": 7.730461589944827,
+            "y": 4.347803549557207,
+            "heading": 3.1951904179502595,
+            "angularVelocity": 0.647312922727555,
+            "velocityX": 0.5918498262162951,
+            "velocityY": 2.5897428633591493,
+            "timestamp": 1.868693034605951
+        },
+        {
+            "x": 7.723612364676548,
+            "y": 4.588311365928096,
+            "heading": 3.24650445053595,
+            "angularVelocity": 0.6018478867095445,
+            "velocityX": -0.0803326409092394,
+            "velocityY": 2.820848678735727,
+            "timestamp": 1.953953835082027
+        },
+        {
+            "x": 7.6586227331171335,
+            "y": 4.845842471404587,
+            "heading": 3.2908004330134384,
+            "angularVelocity": 0.5195351466342113,
+            "velocityX": -0.762245148960926,
+            "velocityY": 3.020510059001304,
+            "timestamp": 2.039214635558103
+        },
+        {
+            "x": 7.534247896600953,
+            "y": 5.114930519867729,
+            "heading": 3.3218318995468055,
+            "angularVelocity": 0.3639593618649453,
+            "velocityX": -1.4587575512040862,
+            "velocityY": 3.1560581997895576,
+            "timestamp": 2.124475436034179
+        },
+        {
+            "x": 7.350032329869969,
+            "y": 5.3796839143357555,
+            "heading": 3.321832164657738,
+            "angularVelocity": 0.0000031094117170733377,
+            "velocityX": -2.160612681353773,
+            "velocityY": 3.1052182596188063,
+            "timestamp": 2.209736236510255
+        },
+        {
+            "x": 7.119535512814417,
+            "y": 5.605296483491154,
+            "heading": 3.3218321686163597,
+            "angularVelocity": 4.6429567622162366e-8,
+            "velocityX": -2.70343247739305,
+            "velocityY": 2.6461465045558032,
+            "timestamp": 2.294997036986331
+        },
+        {
+            "x": 6.850897968160264,
+            "y": 5.7838005054369255,
+            "heading": 3.321832171960481,
+            "angularVelocity": 3.9222265908183896e-8,
+            "velocityX": -3.1507743670496304,
+            "velocityY": 2.0936235755358563,
+            "timestamp": 2.380257837462407
+        },
+        {
+            "x": 6.563016789640927,
+            "y": 5.929245490779759,
+            "heading": 3.321832175184079,
+            "angularVelocity": 3.780866961118305e-8,
+            "velocityX": -3.376477547851724,
+            "velocityY": 1.7058834133705343,
+            "timestamp": 2.465518637938483
+        },
+        {
+            "x": 6.275135446510561,
+            "y": 6.074690150305356,
+            "heading": 3.321832178407676,
+            "angularVelocity": 3.780866514458699e-8,
+            "velocityX": -3.3764794785282892,
+            "velocityY": 1.7058795919516283,
+            "timestamp": 2.550779438414559
+        },
+        {
+            "x": 5.987254103377649,
+            "y": 6.2201348098258835,
+            "heading": 3.3218321816312852,
+            "angularVelocity": 3.780881195826611e-8,
+            "velocityX": -3.376479478558147,
+            "velocityY": 1.705879591892162,
+            "timestamp": 2.6360402388906348
+        },
+        {
+            "x": 5.716836088038681,
+            "y": 6.356756526684127,
+            "heading": 3.3489498411718635,
+            "angularVelocity": 0.3180554180720732,
+            "velocityX": -3.171657008015607,
+            "velocityY": 1.6023977736002932,
+            "timestamp": 2.7213010393667108
+        },
+        {
+            "x": 5.500501658783529,
+            "y": 6.4660539115745435,
+            "heading": 3.370644010379382,
+            "angularVelocity": 0.2544448220798278,
+            "velocityX": -2.537325805612817,
+            "velocityY": 1.2819183526324525,
+            "timestamp": 2.8065618398427867
+        },
+        {
+            "x": 5.33825083142265,
+            "y": 6.5480269544022365,
+            "heading": 3.3869146454118173,
+            "angularVelocity": 0.19083371187678314,
+            "velocityX": -1.9029944177735691,
+            "velocityY": 0.9614388132644309,
+            "timestamp": 2.8918226403188627
+        },
+        {
+            "x": 5.2300836110004285,
+            "y": 6.602675651200572,
+            "heading": 3.3977617464965464,
+            "angularVelocity": 0.12722260434057955,
+            "velocityX": -1.2686629707701718,
+            "velocityY": 0.6409592273728405,
+            "timestamp": 2.9770834407949387
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.0636114211074268,
+            "velocityX": -0.6343314946427733,
+            "velocityY": 0.3204796183809554,
+            "timestamp": 3.0623442412710147
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0,
+            "velocityX": -1.2693090039370116e-34,
+            "velocityY": 0,
+            "timestamp": 3.1476050417470907
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S1.traj
new file mode 100644
index 00000000..fb79e562
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S1.traj	
@@ -0,0 +1,346 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.358164523227949,
+            "y": 4.34513375016413,
+            "heading": 2.762567979975215,
+            "angularVelocity": 0.1479885663322048,
+            "velocityX": 0.6848900476349593,
+            "velocityY": -0.17505080405008622,
+            "timestamp": 0.08492534448237501
+        },
+        {
+            "x": 2.474493569181997,
+            "y": 4.315401331546784,
+            "heading": 2.787705678078292,
+            "angularVelocity": 0.2959975994950906,
+            "velocityX": 1.3697800893606054,
+            "velocityY": -0.35010065368078286,
+            "timestamp": 0.16985068896475003
+        },
+        {
+            "x": 2.6489871423142275,
+            "y": 4.270802870950086,
+            "heading": 2.825415221133538,
+            "angularVelocity": 0.4440316761160991,
+            "velocityX": 2.054670183509755,
+            "velocityY": -0.525149010210423,
+            "timestamp": 0.25477603344712507
+        },
+        {
+            "x": 2.881645266961739,
+            "y": 4.211338540895288,
+            "heading": 2.8756978223326772,
+            "angularVelocity": 0.5920800381277782,
+            "velocityX": 2.739560564229402,
+            "velocityY": -0.7001953352940375,
+            "timestamp": 0.33970137792950006
+        },
+        {
+            "x": 3.172468005146889,
+            "y": 4.137008553769027,
+            "heading": 2.9385516414717108,
+            "angularVelocity": 0.7401067316492073,
+            "velocityX": 3.4244516752652747,
+            "velocityY": -0.8752391595148364,
+            "timestamp": 0.42462672241187505
+        },
+        {
+            "x": 3.48373017288665,
+            "y": 4.0574555172899185,
+            "heading": 2.9385516489955585,
+            "angularVelocity": 8.859366728426556e-8,
+            "velocityX": 3.6651269375110873,
+            "velocityY": -0.936740815877624,
+            "timestamp": 0.50955206689425
+        },
+        {
+            "x": 3.7949923406136996,
+            "y": 3.977902480761,
+            "heading": 2.9385516565193726,
+            "angularVelocity": 8.859327100912678e-8,
+            "velocityX": 3.665126937361409,
+            "velocityY": -0.9367408164641361,
+            "timestamp": 0.594477411376625
+        },
+        {
+            "x": 4.10625450834075,
+            "y": 3.898349444232082,
+            "heading": 2.938551664043187,
+            "angularVelocity": 8.859327019599772e-8,
+            "velocityX": 3.665126937361408,
+            "velocityY": -0.9367408164641391,
+            "timestamp": 0.679402755859
+        },
+        {
+            "x": 4.417516676067799,
+            "y": 3.818796407703164,
+            "heading": 2.938551671567001,
+            "angularVelocity": 8.859327006115765e-8,
+            "velocityX": 3.6651269373614084,
+            "velocityY": -0.9367408164641391,
+            "timestamp": 0.764328100341375
+        },
+        {
+            "x": 4.728778843794849,
+            "y": 3.7392433711742457,
+            "heading": 2.938551679090815,
+            "angularVelocity": 8.859327000476347e-8,
+            "velocityX": 3.665126937361408,
+            "velocityY": -0.9367408164641391,
+            "timestamp": 0.84925344482375
+        },
+        {
+            "x": 5.0400410115218985,
+            "y": 3.6596903346453273,
+            "heading": 2.9385516866146295,
+            "angularVelocity": 8.85932714022833e-8,
+            "velocityX": 3.665126937361408,
+            "velocityY": -0.9367408164641392,
+            "timestamp": 0.934178789306125
+        },
+        {
+            "x": 5.351303179248949,
+            "y": 3.580137298116409,
+            "heading": 2.9385516941384435,
+            "angularVelocity": 8.859327127858633e-8,
+            "velocityX": 3.665126937361408,
+            "velocityY": -0.936740816464139,
+            "timestamp": 1.0191041337885
+        },
+        {
+            "x": 5.662565346976546,
+            "y": 3.5005842615896343,
+            "heading": 2.938551701662258,
+            "angularVelocity": 8.859327107695318e-8,
+            "velocityX": 3.6651269373678588,
+            "velocityY": -0.9367408164388998,
+            "timestamp": 1.104029478270875
+        },
+        {
+            "x": 5.973827620415735,
+            "y": 3.4210316386751205,
+            "heading": 2.9385517091860733,
+            "angularVelocity": 8.859328561882092e-8,
+            "velocityX": 3.6651281821269137,
+            "velocityY": -0.9367359461347136,
+            "timestamp": 1.18895482275325
+        },
+        {
+            "x": 6.292581444856613,
+            "y": 3.380921520397279,
+            "heading": 2.9385517170132656,
+            "angularVelocity": 9.216556583241828e-8,
+            "velocityX": 3.753341554087312,
+            "velocityY": -0.4722985643722118,
+            "timestamp": 1.273880167235625
+        },
+        {
+            "x": 6.61322600810305,
+            "y": 3.400918618969079,
+            "heading": 2.938551726316265,
+            "angularVelocity": 1.0954326272813381e-7,
+            "velocityX": 3.7756050941068793,
+            "velocityY": 0.23546679373140608,
+            "timestamp": 1.358805511718
+        },
+        {
+            "x": 6.924526035610863,
+            "y": 3.4803224362243568,
+            "heading": 2.9385522977985694,
+            "angularVelocity": 0.000006729231518347491,
+            "velocityX": 3.665572738093738,
+            "velocityY": 0.9349837523680175,
+            "timestamp": 1.443730856200375
+        },
+        {
+            "x": 7.193617949641073,
+            "y": 3.601275977700439,
+            "heading": 2.9756713722126755,
+            "angularVelocity": 0.43707887957768976,
+            "velocityX": 3.1685701797306898,
+            "velocityY": 1.4242337456892449,
+            "timestamp": 1.52865620068275
+        },
+        {
+            "x": 7.4107131876465635,
+            "y": 3.752187582369628,
+            "heading": 3.026776803352479,
+            "angularVelocity": 0.6017689001003675,
+            "velocityX": 2.5563068284114614,
+            "velocityY": 1.7769913750601078,
+            "timestamp": 1.613581545165125
+        },
+        {
+            "x": 7.573210012728862,
+            "y": 3.928266853326757,
+            "heading": 3.0830399478590267,
+            "angularVelocity": 0.6625012220965879,
+            "velocityX": 1.9134079004651277,
+            "velocityY": 2.0733418513677266,
+            "timestamp": 1.6985068896475
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.6707073428803676,
+            "velocityX": 1.257457216359011,
+            "velocityY": 2.340092323257951,
+            "timestamp": 1.783432234129875
+        },
+        {
+            "x": 7.730461589944827,
+            "y": 4.347803549557207,
+            "heading": 3.1951904179502595,
+            "angularVelocity": 0.647312922727555,
+            "velocityX": 0.5918498262162951,
+            "velocityY": 2.5897428633591493,
+            "timestamp": 1.868693034605951
+        },
+        {
+            "x": 7.723612364676548,
+            "y": 4.588311365928096,
+            "heading": 3.24650445053595,
+            "angularVelocity": 0.6018478867095445,
+            "velocityX": -0.0803326409092394,
+            "velocityY": 2.820848678735727,
+            "timestamp": 1.953953835082027
+        },
+        {
+            "x": 7.6586227331171335,
+            "y": 4.845842471404587,
+            "heading": 3.2908004330134384,
+            "angularVelocity": 0.5195351466342113,
+            "velocityX": -0.762245148960926,
+            "velocityY": 3.020510059001304,
+            "timestamp": 2.039214635558103
+        },
+        {
+            "x": 7.534247896600953,
+            "y": 5.114930519867729,
+            "heading": 3.3218318995468055,
+            "angularVelocity": 0.3639593618649453,
+            "velocityX": -1.4587575512040862,
+            "velocityY": 3.1560581997895576,
+            "timestamp": 2.124475436034179
+        },
+        {
+            "x": 7.350032329869969,
+            "y": 5.3796839143357555,
+            "heading": 3.321832164657738,
+            "angularVelocity": 0.0000031094117170733377,
+            "velocityX": -2.160612681353773,
+            "velocityY": 3.1052182596188063,
+            "timestamp": 2.209736236510255
+        },
+        {
+            "x": 7.119535512814417,
+            "y": 5.605296483491154,
+            "heading": 3.3218321686163597,
+            "angularVelocity": 4.6429567622162366e-8,
+            "velocityX": -2.70343247739305,
+            "velocityY": 2.6461465045558032,
+            "timestamp": 2.294997036986331
+        },
+        {
+            "x": 6.850897968160264,
+            "y": 5.7838005054369255,
+            "heading": 3.321832171960481,
+            "angularVelocity": 3.9222265908183896e-8,
+            "velocityX": -3.1507743670496304,
+            "velocityY": 2.0936235755358563,
+            "timestamp": 2.380257837462407
+        },
+        {
+            "x": 6.563016789640927,
+            "y": 5.929245490779759,
+            "heading": 3.321832175184079,
+            "angularVelocity": 3.780866961118305e-8,
+            "velocityX": -3.376477547851724,
+            "velocityY": 1.7058834133705343,
+            "timestamp": 2.465518637938483
+        },
+        {
+            "x": 6.275135446510561,
+            "y": 6.074690150305356,
+            "heading": 3.321832178407676,
+            "angularVelocity": 3.780866514458699e-8,
+            "velocityX": -3.3764794785282892,
+            "velocityY": 1.7058795919516283,
+            "timestamp": 2.550779438414559
+        },
+        {
+            "x": 5.987254103377649,
+            "y": 6.2201348098258835,
+            "heading": 3.3218321816312852,
+            "angularVelocity": 3.780881195826611e-8,
+            "velocityX": -3.376479478558147,
+            "velocityY": 1.705879591892162,
+            "timestamp": 2.6360402388906348
+        },
+        {
+            "x": 5.716836088038681,
+            "y": 6.356756526684127,
+            "heading": 3.3489498411718635,
+            "angularVelocity": 0.3180554180720732,
+            "velocityX": -3.171657008015607,
+            "velocityY": 1.6023977736002932,
+            "timestamp": 2.7213010393667108
+        },
+        {
+            "x": 5.500501658783529,
+            "y": 6.4660539115745435,
+            "heading": 3.370644010379382,
+            "angularVelocity": 0.2544448220798278,
+            "velocityX": -2.537325805612817,
+            "velocityY": 1.2819183526324525,
+            "timestamp": 2.8065618398427867
+        },
+        {
+            "x": 5.33825083142265,
+            "y": 6.5480269544022365,
+            "heading": 3.3869146454118173,
+            "angularVelocity": 0.19083371187678314,
+            "velocityX": -1.9029944177735691,
+            "velocityY": 0.9614388132644309,
+            "timestamp": 2.8918226403188627
+        },
+        {
+            "x": 5.2300836110004285,
+            "y": 6.602675651200572,
+            "heading": 3.3977617464965464,
+            "angularVelocity": 0.12722260434057955,
+            "velocityX": -1.2686629707701718,
+            "velocityY": 0.6409592273728405,
+            "timestamp": 2.9770834407949387
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.0636114211074268,
+            "velocityX": -0.6343314946427733,
+            "velocityY": 0.3204796183809554,
+            "timestamp": 3.0623442412710147
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0,
+            "velocityX": -1.2693090039370116e-34,
+            "velocityY": 0,
+            "timestamp": 3.1476050417470907
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S2.1.traj
new file mode 100644
index 00000000..28b5f4d2
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S2.1.traj	
@@ -0,0 +1,337 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.361838792645819,
+            "y": 4.349925567383164,
+            "heading": 2.7594883701133286,
+            "angularVelocity": 0.10940981634945876,
+            "velocityX": 0.7130593416825138,
+            "velocityY": -0.11616766728823658,
+            "timestamp": 0.08672320665473099
+        },
+        {
+            "x": 2.485516776956774,
+            "y": 4.329776279587658,
+            "heading": 2.7784262463080607,
+            "angularVelocity": 0.21837149392012115,
+            "velocityX": 1.4261232844324094,
+            "velocityY": -0.2323402071111933,
+            "timestamp": 0.17344641330946198
+        },
+        {
+            "x": 2.671034614420037,
+            "y": 4.299551431366552,
+            "heading": 2.806748673224486,
+            "angularVelocity": 0.3265841752046417,
+            "velocityX": 2.1391948547504818,
+            "velocityY": -0.34852087909340246,
+            "timestamp": 0.26016961996419297
+        },
+        {
+            "x": 2.9183936206497236,
+            "y": 4.259249598753385,
+            "heading": 2.8443244197137543,
+            "angularVelocity": 0.4332836381257234,
+            "velocityX": 2.852281595334577,
+            "velocityY": -0.46471797074593596,
+            "timestamp": 0.34689282661892396
+        },
+        {
+            "x": 3.2275976397156323,
+            "y": 4.208866459419414,
+            "heading": 2.8907555208713065,
+            "angularVelocity": 0.5353941920351031,
+            "velocityX": 3.5654126616527764,
+            "velocityY": -0.5809649029072455,
+            "timestamp": 0.43361603327365494
+        },
+        {
+            "x": 3.5513989095488143,
+            "y": 4.156123400332019,
+            "heading": 2.8907555264025566,
+            "angularVelocity": 6.378050766718983e-8,
+            "velocityX": 3.7337326688382726,
+            "velocityY": -0.6081769934704949,
+            "timestamp": 0.5203392399283859
+        },
+        {
+            "x": 3.8752001793990973,
+            "y": 4.103380341349552,
+            "heading": 2.8907555319337894,
+            "angularVelocity": 6.378030392692821e-8,
+            "velocityX": 3.7337326690354664,
+            "velocityY": -0.6081769922605936,
+            "timestamp": 0.6070624465831169
+        },
+        {
+            "x": 4.19900144924938,
+            "y": 4.050637282367084,
+            "heading": 2.890755537465022,
+            "angularVelocity": 6.378030384357619e-8,
+            "velocityX": 3.7337326690354677,
+            "velocityY": -0.6081769922605877,
+            "timestamp": 0.6937856532378479
+        },
+        {
+            "x": 4.522802719099664,
+            "y": 3.9978942233846166,
+            "heading": 2.8907555429962546,
+            "angularVelocity": 6.378030394841341e-8,
+            "velocityX": 3.7337326690354673,
+            "velocityY": -0.6081769922605876,
+            "timestamp": 0.7805088598925789
+        },
+        {
+            "x": 4.846603988949947,
+            "y": 3.9451511644021493,
+            "heading": 2.8907555485274874,
+            "angularVelocity": 6.37803046219163e-8,
+            "velocityX": 3.7337326690354673,
+            "velocityY": -0.6081769922605877,
+            "timestamp": 0.8672320665473099
+        },
+        {
+            "x": 5.170405258800231,
+            "y": 3.8924081054196815,
+            "heading": 2.8907555540587198,
+            "angularVelocity": 6.37803035811712e-8,
+            "velocityX": 3.7337326690354673,
+            "velocityY": -0.6081769922605876,
+            "timestamp": 0.9539552732020409
+        },
+        {
+            "x": 5.494206528650514,
+            "y": 3.839665046437214,
+            "heading": 2.8907555595899526,
+            "angularVelocity": 6.378030493154673e-8,
+            "velocityX": 3.7337326690354673,
+            "velocityY": -0.6081769922605876,
+            "timestamp": 1.0406784798567719
+        },
+        {
+            "x": 5.818007798500798,
+            "y": 3.7869219874547464,
+            "heading": 2.8907555651211854,
+            "angularVelocity": 6.378030454431603e-8,
+            "velocityX": 3.7337326690354677,
+            "velocityY": -0.6081769922605872,
+            "timestamp": 1.1274016865115029
+        },
+        {
+            "x": 6.141809068352602,
+            "y": 3.734178928481622,
+            "heading": 2.8907555706524177,
+            "angularVelocity": 6.378030476763932e-8,
+            "velocityX": 3.7337326690530155,
+            "velocityY": -0.608176992152858,
+            "timestamp": 1.2141248931662338
+        },
+        {
+            "x": 6.4656106506759485,
+            "y": 3.6814377878761273,
+            "heading": 2.890755576183656,
+            "angularVelocity": 6.378036992489158e-8,
+            "velocityX": 3.733736272143269,
+            "velocityY": -0.6081548715728616,
+            "timestamp": 1.3008480998209648
+        },
+        {
+            "x": 6.793655691240644,
+            "y": 3.6853799559796676,
+            "heading": 2.8907555914201803,
+            "angularVelocity": 1.756914361676573e-7,
+            "velocityX": 3.7826673299885396,
+            "velocityY": 0.045456899665096306,
+            "timestamp": 1.3875713064756958
+        },
+        {
+            "x": 7.092289943250509,
+            "y": 3.7425375783401047,
+            "heading": 2.945166528243714,
+            "angularVelocity": 0.6274091897932109,
+            "velocityX": 3.4435333231946736,
+            "velocityY": 0.6590810529873269,
+            "timestamp": 1.4742945131304268
+        },
+        {
+            "x": 7.330977190762065,
+            "y": 3.8181008616818835,
+            "heading": 3.0010867643989263,
+            "angularVelocity": 0.6448128282182443,
+            "velocityX": 2.7522880751150582,
+            "velocityY": 0.8713156057826263,
+            "timestamp": 1.5610177197851578
+        },
+        {
+            "x": 7.508670206250799,
+            "y": 3.908197537676412,
+            "heading": 3.053304413172006,
+            "angularVelocity": 0.6021185192211955,
+            "velocityX": 2.0489673104014514,
+            "velocityY": 1.0388992689491814,
+            "timestamp": 1.6477409264398888
+        },
+        {
+            "x": 7.6250594852295395,
+            "y": 4.011408797557566,
+            "heading": 3.099932944450418,
+            "angularVelocity": 0.5376707467014636,
+            "velocityX": 1.342077668346812,
+            "velocityY": 1.1901227348761163,
+            "timestamp": 1.7344641330946198
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.4620107707627286,
+            "velocityX": 0.6335157207596422,
+            "velocityY": 1.332875096542895,
+            "timestamp": 1.8211873397493508
+        },
+        {
+            "x": 7.66415878383622,
+            "y": 4.271751344750917,
+            "heading": 3.1759557228740287,
+            "angularVelocity": 0.3694329307480838,
+            "velocityX": -0.16276315552044252,
+            "velocityY": 1.487271267174521,
+            "timestamp": 1.918514134091254
+        },
+        {
+            "x": 7.570652845333858,
+            "y": 4.430649041463004,
+            "heading": 3.2028903885273214,
+            "angularVelocity": 0.2767446090814936,
+            "velocityX": -0.9607419943770155,
+            "velocityY": 1.6326202644041503,
+            "timestamp": 2.015840928433157
+        },
+        {
+            "x": 7.399206149109808,
+            "y": 4.602082311613017,
+            "heading": 3.2207860785976568,
+            "angularVelocity": 0.1838721822839176,
+            "velocityX": -1.761557003734937,
+            "velocityY": 1.7614190553505542,
+            "timestamp": 2.11316772277506
+        },
+        {
+            "x": 7.149293090301517,
+            "y": 4.782169514035782,
+            "heading": 3.229614246279924,
+            "angularVelocity": 0.09070644668783194,
+            "velocityX": -2.5677724258579846,
+            "velocityY": 1.8503352919456928,
+            "timestamp": 2.210494517116963
+        },
+        {
+            "x": 6.821439370412565,
+            "y": 4.949709126664123,
+            "heading": 3.229614240242725,
+            "angularVelocity": -6.203018521599257e-8,
+            "velocityX": -3.3685864422619463,
+            "velocityY": 1.7214130369873843,
+            "timestamp": 2.3078213114588664
+        },
+        {
+            "x": 6.465396251393308,
+            "y": 5.04346891931761,
+            "heading": 3.2296142362544686,
+            "angularVelocity": -4.0977992187262516e-8,
+            "velocityX": -3.658223014810281,
+            "velocityY": 0.9633502601977828,
+            "timestamp": 2.4051481058007695
+        },
+        {
+            "x": 6.106941254304619,
+            "y": 5.12753803654929,
+            "heading": 3.2296142322922576,
+            "angularVelocity": -4.071038102398141e-8,
+            "velocityX": -3.6830042488552306,
+            "velocityY": 0.8637818372641555,
+            "timestamp": 2.5024749001426727
+        },
+        {
+            "x": 5.7484862494466205,
+            "y": 5.211607120654073,
+            "heading": 3.2296142283300466,
+            "angularVelocity": -4.071038072165154e-8,
+            "velocityX": -3.6830043286822827,
+            "velocityY": 0.8637814968964618,
+            "timestamp": 2.599801694484576
+        },
+        {
+            "x": 5.390031244588595,
+            "y": 5.295676204758746,
+            "heading": 3.229614224367836,
+            "angularVelocity": -4.0710380903709755e-8,
+            "velocityX": -3.683004328682549,
+            "velocityY": 0.8637814968953244,
+            "timestamp": 2.697128488826479
+        },
+        {
+            "x": 5.031576239716237,
+            "y": 5.3797452888021455,
+            "heading": 3.2296142204055998,
+            "angularVelocity": -4.071064154144793e-8,
+            "velocityX": -3.683004328829807,
+            "velocityY": 0.8637814962657624,
+            "timestamp": 2.794455283168382
+        },
+        {
+            "x": 4.7241457561516045,
+            "y": 5.451847166184253,
+            "heading": 3.193769184032689,
+            "angularVelocity": -0.3682956642649294,
+            "velocityX": -3.1587445743322107,
+            "velocityY": 0.740822482335317,
+            "timestamp": 2.8917820775102854
+        },
+        {
+            "x": 4.493572881554858,
+            "y": 5.505923581174423,
+            "heading": 3.1668847241449476,
+            "angularVelocity": -0.27622876176636874,
+            "velocityX": -2.3690585532567625,
+            "velocityY": 0.5556169331972818,
+            "timestamp": 2.9891088718521885
+        },
+        {
+            "x": 4.3398576283261585,
+            "y": 5.541974526790158,
+            "heading": 3.1489615588617923,
+            "angularVelocity": -0.1841544808328986,
+            "velocityX": -1.5793724047738293,
+            "velocityY": 0.37041131231641944,
+            "timestamp": 3.0864356661940917
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -0.09207699608717179,
+            "velocityX": -0.7896862199751749,
+            "velocityY": 0.18520566028835728,
+            "timestamp": 3.183762460535995
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -4.104369359528437e-34,
+            "velocityY": 0,
+            "timestamp": 3.281089254877898
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S2.traj
new file mode 100644
index 00000000..28b5f4d2
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S2.traj	
@@ -0,0 +1,337 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.361838792645819,
+            "y": 4.349925567383164,
+            "heading": 2.7594883701133286,
+            "angularVelocity": 0.10940981634945876,
+            "velocityX": 0.7130593416825138,
+            "velocityY": -0.11616766728823658,
+            "timestamp": 0.08672320665473099
+        },
+        {
+            "x": 2.485516776956774,
+            "y": 4.329776279587658,
+            "heading": 2.7784262463080607,
+            "angularVelocity": 0.21837149392012115,
+            "velocityX": 1.4261232844324094,
+            "velocityY": -0.2323402071111933,
+            "timestamp": 0.17344641330946198
+        },
+        {
+            "x": 2.671034614420037,
+            "y": 4.299551431366552,
+            "heading": 2.806748673224486,
+            "angularVelocity": 0.3265841752046417,
+            "velocityX": 2.1391948547504818,
+            "velocityY": -0.34852087909340246,
+            "timestamp": 0.26016961996419297
+        },
+        {
+            "x": 2.9183936206497236,
+            "y": 4.259249598753385,
+            "heading": 2.8443244197137543,
+            "angularVelocity": 0.4332836381257234,
+            "velocityX": 2.852281595334577,
+            "velocityY": -0.46471797074593596,
+            "timestamp": 0.34689282661892396
+        },
+        {
+            "x": 3.2275976397156323,
+            "y": 4.208866459419414,
+            "heading": 2.8907555208713065,
+            "angularVelocity": 0.5353941920351031,
+            "velocityX": 3.5654126616527764,
+            "velocityY": -0.5809649029072455,
+            "timestamp": 0.43361603327365494
+        },
+        {
+            "x": 3.5513989095488143,
+            "y": 4.156123400332019,
+            "heading": 2.8907555264025566,
+            "angularVelocity": 6.378050766718983e-8,
+            "velocityX": 3.7337326688382726,
+            "velocityY": -0.6081769934704949,
+            "timestamp": 0.5203392399283859
+        },
+        {
+            "x": 3.8752001793990973,
+            "y": 4.103380341349552,
+            "heading": 2.8907555319337894,
+            "angularVelocity": 6.378030392692821e-8,
+            "velocityX": 3.7337326690354664,
+            "velocityY": -0.6081769922605936,
+            "timestamp": 0.6070624465831169
+        },
+        {
+            "x": 4.19900144924938,
+            "y": 4.050637282367084,
+            "heading": 2.890755537465022,
+            "angularVelocity": 6.378030384357619e-8,
+            "velocityX": 3.7337326690354677,
+            "velocityY": -0.6081769922605877,
+            "timestamp": 0.6937856532378479
+        },
+        {
+            "x": 4.522802719099664,
+            "y": 3.9978942233846166,
+            "heading": 2.8907555429962546,
+            "angularVelocity": 6.378030394841341e-8,
+            "velocityX": 3.7337326690354673,
+            "velocityY": -0.6081769922605876,
+            "timestamp": 0.7805088598925789
+        },
+        {
+            "x": 4.846603988949947,
+            "y": 3.9451511644021493,
+            "heading": 2.8907555485274874,
+            "angularVelocity": 6.37803046219163e-8,
+            "velocityX": 3.7337326690354673,
+            "velocityY": -0.6081769922605877,
+            "timestamp": 0.8672320665473099
+        },
+        {
+            "x": 5.170405258800231,
+            "y": 3.8924081054196815,
+            "heading": 2.8907555540587198,
+            "angularVelocity": 6.37803035811712e-8,
+            "velocityX": 3.7337326690354673,
+            "velocityY": -0.6081769922605876,
+            "timestamp": 0.9539552732020409
+        },
+        {
+            "x": 5.494206528650514,
+            "y": 3.839665046437214,
+            "heading": 2.8907555595899526,
+            "angularVelocity": 6.378030493154673e-8,
+            "velocityX": 3.7337326690354673,
+            "velocityY": -0.6081769922605876,
+            "timestamp": 1.0406784798567719
+        },
+        {
+            "x": 5.818007798500798,
+            "y": 3.7869219874547464,
+            "heading": 2.8907555651211854,
+            "angularVelocity": 6.378030454431603e-8,
+            "velocityX": 3.7337326690354677,
+            "velocityY": -0.6081769922605872,
+            "timestamp": 1.1274016865115029
+        },
+        {
+            "x": 6.141809068352602,
+            "y": 3.734178928481622,
+            "heading": 2.8907555706524177,
+            "angularVelocity": 6.378030476763932e-8,
+            "velocityX": 3.7337326690530155,
+            "velocityY": -0.608176992152858,
+            "timestamp": 1.2141248931662338
+        },
+        {
+            "x": 6.4656106506759485,
+            "y": 3.6814377878761273,
+            "heading": 2.890755576183656,
+            "angularVelocity": 6.378036992489158e-8,
+            "velocityX": 3.733736272143269,
+            "velocityY": -0.6081548715728616,
+            "timestamp": 1.3008480998209648
+        },
+        {
+            "x": 6.793655691240644,
+            "y": 3.6853799559796676,
+            "heading": 2.8907555914201803,
+            "angularVelocity": 1.756914361676573e-7,
+            "velocityX": 3.7826673299885396,
+            "velocityY": 0.045456899665096306,
+            "timestamp": 1.3875713064756958
+        },
+        {
+            "x": 7.092289943250509,
+            "y": 3.7425375783401047,
+            "heading": 2.945166528243714,
+            "angularVelocity": 0.6274091897932109,
+            "velocityX": 3.4435333231946736,
+            "velocityY": 0.6590810529873269,
+            "timestamp": 1.4742945131304268
+        },
+        {
+            "x": 7.330977190762065,
+            "y": 3.8181008616818835,
+            "heading": 3.0010867643989263,
+            "angularVelocity": 0.6448128282182443,
+            "velocityX": 2.7522880751150582,
+            "velocityY": 0.8713156057826263,
+            "timestamp": 1.5610177197851578
+        },
+        {
+            "x": 7.508670206250799,
+            "y": 3.908197537676412,
+            "heading": 3.053304413172006,
+            "angularVelocity": 0.6021185192211955,
+            "velocityX": 2.0489673104014514,
+            "velocityY": 1.0388992689491814,
+            "timestamp": 1.6477409264398888
+        },
+        {
+            "x": 7.6250594852295395,
+            "y": 4.011408797557566,
+            "heading": 3.099932944450418,
+            "angularVelocity": 0.5376707467014636,
+            "velocityX": 1.342077668346812,
+            "velocityY": 1.1901227348761163,
+            "timestamp": 1.7344641330946198
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.4620107707627286,
+            "velocityX": 0.6335157207596422,
+            "velocityY": 1.332875096542895,
+            "timestamp": 1.8211873397493508
+        },
+        {
+            "x": 7.66415878383622,
+            "y": 4.271751344750917,
+            "heading": 3.1759557228740287,
+            "angularVelocity": 0.3694329307480838,
+            "velocityX": -0.16276315552044252,
+            "velocityY": 1.487271267174521,
+            "timestamp": 1.918514134091254
+        },
+        {
+            "x": 7.570652845333858,
+            "y": 4.430649041463004,
+            "heading": 3.2028903885273214,
+            "angularVelocity": 0.2767446090814936,
+            "velocityX": -0.9607419943770155,
+            "velocityY": 1.6326202644041503,
+            "timestamp": 2.015840928433157
+        },
+        {
+            "x": 7.399206149109808,
+            "y": 4.602082311613017,
+            "heading": 3.2207860785976568,
+            "angularVelocity": 0.1838721822839176,
+            "velocityX": -1.761557003734937,
+            "velocityY": 1.7614190553505542,
+            "timestamp": 2.11316772277506
+        },
+        {
+            "x": 7.149293090301517,
+            "y": 4.782169514035782,
+            "heading": 3.229614246279924,
+            "angularVelocity": 0.09070644668783194,
+            "velocityX": -2.5677724258579846,
+            "velocityY": 1.8503352919456928,
+            "timestamp": 2.210494517116963
+        },
+        {
+            "x": 6.821439370412565,
+            "y": 4.949709126664123,
+            "heading": 3.229614240242725,
+            "angularVelocity": -6.203018521599257e-8,
+            "velocityX": -3.3685864422619463,
+            "velocityY": 1.7214130369873843,
+            "timestamp": 2.3078213114588664
+        },
+        {
+            "x": 6.465396251393308,
+            "y": 5.04346891931761,
+            "heading": 3.2296142362544686,
+            "angularVelocity": -4.0977992187262516e-8,
+            "velocityX": -3.658223014810281,
+            "velocityY": 0.9633502601977828,
+            "timestamp": 2.4051481058007695
+        },
+        {
+            "x": 6.106941254304619,
+            "y": 5.12753803654929,
+            "heading": 3.2296142322922576,
+            "angularVelocity": -4.071038102398141e-8,
+            "velocityX": -3.6830042488552306,
+            "velocityY": 0.8637818372641555,
+            "timestamp": 2.5024749001426727
+        },
+        {
+            "x": 5.7484862494466205,
+            "y": 5.211607120654073,
+            "heading": 3.2296142283300466,
+            "angularVelocity": -4.071038072165154e-8,
+            "velocityX": -3.6830043286822827,
+            "velocityY": 0.8637814968964618,
+            "timestamp": 2.599801694484576
+        },
+        {
+            "x": 5.390031244588595,
+            "y": 5.295676204758746,
+            "heading": 3.229614224367836,
+            "angularVelocity": -4.0710380903709755e-8,
+            "velocityX": -3.683004328682549,
+            "velocityY": 0.8637814968953244,
+            "timestamp": 2.697128488826479
+        },
+        {
+            "x": 5.031576239716237,
+            "y": 5.3797452888021455,
+            "heading": 3.2296142204055998,
+            "angularVelocity": -4.071064154144793e-8,
+            "velocityX": -3.683004328829807,
+            "velocityY": 0.8637814962657624,
+            "timestamp": 2.794455283168382
+        },
+        {
+            "x": 4.7241457561516045,
+            "y": 5.451847166184253,
+            "heading": 3.193769184032689,
+            "angularVelocity": -0.3682956642649294,
+            "velocityX": -3.1587445743322107,
+            "velocityY": 0.740822482335317,
+            "timestamp": 2.8917820775102854
+        },
+        {
+            "x": 4.493572881554858,
+            "y": 5.505923581174423,
+            "heading": 3.1668847241449476,
+            "angularVelocity": -0.27622876176636874,
+            "velocityX": -2.3690585532567625,
+            "velocityY": 0.5556169331972818,
+            "timestamp": 2.9891088718521885
+        },
+        {
+            "x": 4.3398576283261585,
+            "y": 5.541974526790158,
+            "heading": 3.1489615588617923,
+            "angularVelocity": -0.1841544808328986,
+            "velocityX": -1.5793724047738293,
+            "velocityY": 0.37041131231641944,
+            "timestamp": 3.0864356661940917
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -0.09207699608717179,
+            "velocityX": -0.7896862199751749,
+            "velocityY": 0.18520566028835728,
+            "timestamp": 3.183762460535995
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -4.104369359528437e-34,
+            "velocityY": 0,
+            "timestamp": 3.281089254877898
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S3.1.traj
new file mode 100644
index 00000000..dadc5c59
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S3.1.traj	
@@ -0,0 +1,355 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.362347004549993,
+            "y": 4.364661246321825,
+            "heading": 2.7702286217513103,
+            "angularVelocity": 0.23313109188524597,
+            "velocityX": 0.7185375961450525,
+            "velocityY": 0.05371999426914206,
+            "timestamp": 0.0867693004297674
+        },
+        {
+            "x": 2.4870912612086284,
+            "y": 4.373976445805142,
+            "heading": 2.8086286090699177,
+            "angularVelocity": 0.4425526900460507,
+            "velocityX": 1.4376542860294879,
+            "velocityY": 0.10735593622605764,
+            "timestamp": 0.1735386008595348
+        },
+        {
+            "x": 2.674306466073838,
+            "y": 4.3879313037176075,
+            "heading": 2.8616727700685,
+            "angularVelocity": 0.6113240597291352,
+            "velocityX": 2.1576203096940354,
+            "velocityY": 0.16082713405947677,
+            "timestamp": 0.2603079012893022
+        },
+        {
+            "x": 2.9240965513714046,
+            "y": 4.40648950041988,
+            "heading": 2.921990092412666,
+            "angularVelocity": 0.6951458873750875,
+            "velocityX": 2.878784132871411,
+            "velocityY": 0.2138797548251989,
+            "timestamp": 0.3470772017190696
+        },
+        {
+            "x": 3.236315453938978,
+            "y": 4.429486199889588,
+            "heading": 2.9648285300542776,
+            "angularVelocity": 0.493705001993024,
+            "velocityX": 3.598264605351849,
+            "velocityY": 0.26503267118445795,
+            "timestamp": 0.43384650214883697
+        },
+        {
+            "x": 3.563623741097209,
+            "y": 4.454241375249833,
+            "heading": 2.9648285457681323,
+            "angularVelocity": 1.810992388928579e-7,
+            "velocityX": 3.7721669477231816,
+            "velocityY": 0.28529877776624385,
+            "timestamp": 0.5206158025786044
+        },
+        {
+            "x": 3.8909320279988893,
+            "y": 4.478996554002297,
+            "heading": 2.9648285614819643,
+            "angularVelocity": 1.8109898446503704e-7,
+            "velocityX": 3.7721669447664756,
+            "velocityY": 0.28529881686094194,
+            "timestamp": 0.6073851030083718
+        },
+        {
+            "x": 4.218240314900568,
+            "y": 4.503751732754777,
+            "heading": 2.9648285771957963,
+            "angularVelocity": 1.810989844949235e-7,
+            "velocityX": 3.772166944766461,
+            "velocityY": 0.2852988168611311,
+            "timestamp": 0.6941544034381392
+        },
+        {
+            "x": 4.545548601802246,
+            "y": 4.528506911507257,
+            "heading": 2.9648285929096283,
+            "angularVelocity": 1.810989846493643e-7,
+            "velocityX": 3.7721669447664614,
+            "velocityY": 0.2852988168611311,
+            "timestamp": 0.7809237038679065
+        },
+        {
+            "x": 4.872856888703925,
+            "y": 4.5532620902597385,
+            "heading": 2.9648286086234608,
+            "angularVelocity": 1.8109898537942274e-7,
+            "velocityX": 3.7721669447664614,
+            "velocityY": 0.28529881686113107,
+            "timestamp": 0.8676930042976739
+        },
+        {
+            "x": 5.200165175605603,
+            "y": 4.578017269012219,
+            "heading": 2.964828624337293,
+            "angularVelocity": 1.8109898550822278e-7,
+            "velocityX": 3.7721669447664614,
+            "velocityY": 0.28529881686113095,
+            "timestamp": 0.9544623047274413
+        },
+        {
+            "x": 5.527473462507282,
+            "y": 4.602772447764699,
+            "heading": 2.9648286400511257,
+            "angularVelocity": 1.8109898581520642e-7,
+            "velocityX": 3.772166944766462,
+            "velocityY": 0.2852988168611311,
+            "timestamp": 1.0412316051572088
+        },
+        {
+            "x": 5.854781749408961,
+            "y": 4.627527626517179,
+            "heading": 2.964828655764958,
+            "angularVelocity": 1.8109898550546543e-7,
+            "velocityX": 3.772166944766461,
+            "velocityY": 0.28529881686113034,
+            "timestamp": 1.1280009055869762
+        },
+        {
+            "x": 6.182090036311611,
+            "y": 4.652282805256812,
+            "heading": 2.9648286714787906,
+            "angularVelocity": 1.810989852504463e-7,
+            "velocityX": 3.772166944777661,
+            "velocityY": 0.2852988167130518,
+            "timestamp": 1.2147702060167436
+        },
+        {
+            "x": 6.509398523986194,
+            "y": 4.6770353292870315,
+            "heading": 2.964828687192649,
+            "angularVelocity": 1.8109928558732558e-7,
+            "velocityX": 3.7721692586367297,
+            "velocityY": 0.2852682216823196,
+            "timestamp": 1.301539506446511
+        },
+        {
+            "x": 6.835912518328979,
+            "y": 4.643388028658291,
+            "heading": 2.964828718989663,
+            "angularVelocity": 3.6645465598978293e-7,
+            "velocityX": 3.763012871206354,
+            "velocityY": -0.38777886259409694,
+            "timestamp": 1.3883088068762783
+        },
+        {
+            "x": 7.128079325417914,
+            "y": 4.560807160262635,
+            "heading": 3.033554953244841,
+            "angularVelocity": 0.7920570284049364,
+            "velocityX": 3.3671679458268646,
+            "velocityY": -0.9517291021897464,
+            "timestamp": 1.4750781073060457
+        },
+        {
+            "x": 7.358963687834594,
+            "y": 4.4649781472961925,
+            "heading": 3.091042362636633,
+            "angularVelocity": 0.66253166854011,
+            "velocityX": 2.660899203671262,
+            "velocityY": -1.1044114968289644,
+            "timestamp": 1.561847407735813
+        },
+        {
+            "x": 7.527984669479397,
+            "y": 4.3598798773022285,
+            "heading": 3.1296058219303373,
+            "angularVelocity": 0.44443667406213694,
+            "velocityX": 1.947935281345401,
+            "velocityY": -1.211237954822889,
+            "timestamp": 1.6486167081655805
+        },
+        {
+            "x": 7.635013269633329,
+            "y": 4.246969115663087,
+            "heading": 3.1464129858106427,
+            "angularVelocity": 0.19369942822011468,
+            "velocityX": 1.2334846497991885,
+            "velocityY": -1.3012754635555983,
+            "timestamp": 1.7353860085953479
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": -0.07390846507784325,
+            "velocityX": 0.5184636748694783,
+            "velocityY": -1.3826216768935726,
+            "timestamp": 1.8221553090251152
+        },
+        {
+            "x": 7.665105053681803,
+            "y": 4.004188436329708,
+            "heading": 3.1110105191844553,
+            "angularVelocity": -0.3438049921625736,
+            "velocityX": -0.17664879667123173,
+            "velocityY": -1.4565017205308959,
+            "timestamp": 1.9064748549200063
+        },
+        {
+            "x": 7.591499034942275,
+            "y": 3.875650129704607,
+            "heading": 3.0609451613914906,
+            "angularVelocity": -0.5937574409541295,
+            "velocityX": -0.8729413561036249,
+            "velocityY": -1.524418867071828,
+            "timestamp": 1.9907944008148974
+        },
+        {
+            "x": 7.459038327956553,
+            "y": 3.7422011819159797,
+            "heading": 2.9924948430687532,
+            "angularVelocity": -0.8117965721502389,
+            "velocityX": -1.5709371484381591,
+            "velocityY": -1.582657334931321,
+            "timestamp": 2.0751139467097883
+        },
+        {
+            "x": 7.267508984071126,
+            "y": 3.6053961445739597,
+            "heading": 2.9106651455038604,
+            "angularVelocity": -0.9704712791847574,
+            "velocityX": -2.271470296153853,
+            "velocityY": -1.6224593703641987,
+            "timestamp": 2.159433492604679
+        },
+        {
+            "x": 7.016701982610521,
+            "y": 3.4693216018432858,
+            "heading": 2.828330372603407,
+            "angularVelocity": -0.9764612940763276,
+            "velocityX": -2.9744823551736075,
+            "velocityY": -1.6137959625671907,
+            "timestamp": 2.24375303849957
+        },
+        {
+            "x": 6.717326936237698,
+            "y": 3.359230054214691,
+            "heading": 2.828330320326688,
+            "angularVelocity": -6.199834052808441e-7,
+            "velocityX": -3.5504821947927914,
+            "velocityY": -1.3056468279114077,
+            "timestamp": 2.328072584394461
+        },
+        {
+            "x": 6.402750279383998,
+            "y": 3.3064370272518535,
+            "heading": 2.8283303027221653,
+            "angularVelocity": -2.0878341406433196e-7,
+            "velocityX": -3.730767920000857,
+            "velocityY": -0.6261066328398724,
+            "timestamp": 2.412392130289352
+        },
+        {
+            "x": 6.0870128630404565,
+            "y": 3.261099838136822,
+            "heading": 2.8283302851918997,
+            "angularVelocity": -2.0790275245783378e-7,
+            "velocityX": -3.7445341171206636,
+            "velocityY": -0.5376830322538348,
+            "timestamp": 2.4967116761842427
+        },
+        {
+            "x": 5.771275440975781,
+            "y": 3.215762688864957,
+            "heading": 2.8283302676616344,
+            "angularVelocity": -2.079027527224642e-7,
+            "velocityX": -3.7445341849713016,
+            "velocityY": -0.5376825597279692,
+            "timestamp": 2.5810312220791336
+        },
+        {
+            "x": 5.455538018911073,
+            "y": 3.1704255395933156,
+            "heading": 2.8283302501313687,
+            "angularVelocity": -2.079027528417884e-7,
+            "velocityX": -3.7445341849716827,
+            "velocityY": -0.5376825597253148,
+            "timestamp": 2.6653507679740245
+        },
+        {
+            "x": 5.13980059641411,
+            "y": 3.1250883933321525,
+            "heading": 2.8283302326010555,
+            "angularVelocity": -2.0790331306647263e-7,
+            "velocityX": -3.7445341900980704,
+            "velocityY": -0.5376825240221068,
+            "timestamp": 2.7496703138689154
+        },
+        {
+            "x": 4.847289842857696,
+            "y": 3.0834618283006696,
+            "heading": 2.7687586646684967,
+            "angularVelocity": -0.706497731935343,
+            "velocityX": -3.469074109116353,
+            "velocityY": -0.4936763426521838,
+            "timestamp": 2.8339898597638062
+        },
+        {
+            "x": 4.613405710202319,
+            "y": 3.0500832751446354,
+            "heading": 2.6983183223854565,
+            "angularVelocity": -0.8353975526724018,
+            "velocityX": -2.773783114853639,
+            "velocityY": -0.39585783820090814,
+            "timestamp": 2.918309405658697
+        },
+        {
+            "x": 4.43813741968374,
+            "y": 3.025042089065369,
+            "heading": 2.6386185119236694,
+            "angularVelocity": -0.7080186429870714,
+            "velocityX": -2.0786199529236167,
+            "velocityY": -0.29697961265685946,
+            "timestamp": 3.002628951553588
+        },
+        {
+            "x": 4.321361759336474,
+            "y": 3.008347212158301,
+            "heading": 2.596145764785128,
+            "angularVelocity": -0.5037117632427286,
+            "velocityX": -1.3849180413380506,
+            "velocityY": -0.19799533702279348,
+            "timestamp": 3.086948497448479
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -0.26264093989232323,
+            "velocityX": -0.6921498297586374,
+            "velocityY": -0.0989949847299469,
+            "timestamp": 3.1712680433433698
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": -1.507480653475374e-32,
+            "velocityY": 0,
+            "timestamp": 3.2555875892382606
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S3.traj
new file mode 100644
index 00000000..dadc5c59
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S3.traj	
@@ -0,0 +1,355 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.362347004549993,
+            "y": 4.364661246321825,
+            "heading": 2.7702286217513103,
+            "angularVelocity": 0.23313109188524597,
+            "velocityX": 0.7185375961450525,
+            "velocityY": 0.05371999426914206,
+            "timestamp": 0.0867693004297674
+        },
+        {
+            "x": 2.4870912612086284,
+            "y": 4.373976445805142,
+            "heading": 2.8086286090699177,
+            "angularVelocity": 0.4425526900460507,
+            "velocityX": 1.4376542860294879,
+            "velocityY": 0.10735593622605764,
+            "timestamp": 0.1735386008595348
+        },
+        {
+            "x": 2.674306466073838,
+            "y": 4.3879313037176075,
+            "heading": 2.8616727700685,
+            "angularVelocity": 0.6113240597291352,
+            "velocityX": 2.1576203096940354,
+            "velocityY": 0.16082713405947677,
+            "timestamp": 0.2603079012893022
+        },
+        {
+            "x": 2.9240965513714046,
+            "y": 4.40648950041988,
+            "heading": 2.921990092412666,
+            "angularVelocity": 0.6951458873750875,
+            "velocityX": 2.878784132871411,
+            "velocityY": 0.2138797548251989,
+            "timestamp": 0.3470772017190696
+        },
+        {
+            "x": 3.236315453938978,
+            "y": 4.429486199889588,
+            "heading": 2.9648285300542776,
+            "angularVelocity": 0.493705001993024,
+            "velocityX": 3.598264605351849,
+            "velocityY": 0.26503267118445795,
+            "timestamp": 0.43384650214883697
+        },
+        {
+            "x": 3.563623741097209,
+            "y": 4.454241375249833,
+            "heading": 2.9648285457681323,
+            "angularVelocity": 1.810992388928579e-7,
+            "velocityX": 3.7721669477231816,
+            "velocityY": 0.28529877776624385,
+            "timestamp": 0.5206158025786044
+        },
+        {
+            "x": 3.8909320279988893,
+            "y": 4.478996554002297,
+            "heading": 2.9648285614819643,
+            "angularVelocity": 1.8109898446503704e-7,
+            "velocityX": 3.7721669447664756,
+            "velocityY": 0.28529881686094194,
+            "timestamp": 0.6073851030083718
+        },
+        {
+            "x": 4.218240314900568,
+            "y": 4.503751732754777,
+            "heading": 2.9648285771957963,
+            "angularVelocity": 1.810989844949235e-7,
+            "velocityX": 3.772166944766461,
+            "velocityY": 0.2852988168611311,
+            "timestamp": 0.6941544034381392
+        },
+        {
+            "x": 4.545548601802246,
+            "y": 4.528506911507257,
+            "heading": 2.9648285929096283,
+            "angularVelocity": 1.810989846493643e-7,
+            "velocityX": 3.7721669447664614,
+            "velocityY": 0.2852988168611311,
+            "timestamp": 0.7809237038679065
+        },
+        {
+            "x": 4.872856888703925,
+            "y": 4.5532620902597385,
+            "heading": 2.9648286086234608,
+            "angularVelocity": 1.8109898537942274e-7,
+            "velocityX": 3.7721669447664614,
+            "velocityY": 0.28529881686113107,
+            "timestamp": 0.8676930042976739
+        },
+        {
+            "x": 5.200165175605603,
+            "y": 4.578017269012219,
+            "heading": 2.964828624337293,
+            "angularVelocity": 1.8109898550822278e-7,
+            "velocityX": 3.7721669447664614,
+            "velocityY": 0.28529881686113095,
+            "timestamp": 0.9544623047274413
+        },
+        {
+            "x": 5.527473462507282,
+            "y": 4.602772447764699,
+            "heading": 2.9648286400511257,
+            "angularVelocity": 1.8109898581520642e-7,
+            "velocityX": 3.772166944766462,
+            "velocityY": 0.2852988168611311,
+            "timestamp": 1.0412316051572088
+        },
+        {
+            "x": 5.854781749408961,
+            "y": 4.627527626517179,
+            "heading": 2.964828655764958,
+            "angularVelocity": 1.8109898550546543e-7,
+            "velocityX": 3.772166944766461,
+            "velocityY": 0.28529881686113034,
+            "timestamp": 1.1280009055869762
+        },
+        {
+            "x": 6.182090036311611,
+            "y": 4.652282805256812,
+            "heading": 2.9648286714787906,
+            "angularVelocity": 1.810989852504463e-7,
+            "velocityX": 3.772166944777661,
+            "velocityY": 0.2852988167130518,
+            "timestamp": 1.2147702060167436
+        },
+        {
+            "x": 6.509398523986194,
+            "y": 4.6770353292870315,
+            "heading": 2.964828687192649,
+            "angularVelocity": 1.8109928558732558e-7,
+            "velocityX": 3.7721692586367297,
+            "velocityY": 0.2852682216823196,
+            "timestamp": 1.301539506446511
+        },
+        {
+            "x": 6.835912518328979,
+            "y": 4.643388028658291,
+            "heading": 2.964828718989663,
+            "angularVelocity": 3.6645465598978293e-7,
+            "velocityX": 3.763012871206354,
+            "velocityY": -0.38777886259409694,
+            "timestamp": 1.3883088068762783
+        },
+        {
+            "x": 7.128079325417914,
+            "y": 4.560807160262635,
+            "heading": 3.033554953244841,
+            "angularVelocity": 0.7920570284049364,
+            "velocityX": 3.3671679458268646,
+            "velocityY": -0.9517291021897464,
+            "timestamp": 1.4750781073060457
+        },
+        {
+            "x": 7.358963687834594,
+            "y": 4.4649781472961925,
+            "heading": 3.091042362636633,
+            "angularVelocity": 0.66253166854011,
+            "velocityX": 2.660899203671262,
+            "velocityY": -1.1044114968289644,
+            "timestamp": 1.561847407735813
+        },
+        {
+            "x": 7.527984669479397,
+            "y": 4.3598798773022285,
+            "heading": 3.1296058219303373,
+            "angularVelocity": 0.44443667406213694,
+            "velocityX": 1.947935281345401,
+            "velocityY": -1.211237954822889,
+            "timestamp": 1.6486167081655805
+        },
+        {
+            "x": 7.635013269633329,
+            "y": 4.246969115663087,
+            "heading": 3.1464129858106427,
+            "angularVelocity": 0.19369942822011468,
+            "velocityX": 1.2334846497991885,
+            "velocityY": -1.3012754635555983,
+            "timestamp": 1.7353860085953479
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": -0.07390846507784325,
+            "velocityX": 0.5184636748694783,
+            "velocityY": -1.3826216768935726,
+            "timestamp": 1.8221553090251152
+        },
+        {
+            "x": 7.665105053681803,
+            "y": 4.004188436329708,
+            "heading": 3.1110105191844553,
+            "angularVelocity": -0.3438049921625736,
+            "velocityX": -0.17664879667123173,
+            "velocityY": -1.4565017205308959,
+            "timestamp": 1.9064748549200063
+        },
+        {
+            "x": 7.591499034942275,
+            "y": 3.875650129704607,
+            "heading": 3.0609451613914906,
+            "angularVelocity": -0.5937574409541295,
+            "velocityX": -0.8729413561036249,
+            "velocityY": -1.524418867071828,
+            "timestamp": 1.9907944008148974
+        },
+        {
+            "x": 7.459038327956553,
+            "y": 3.7422011819159797,
+            "heading": 2.9924948430687532,
+            "angularVelocity": -0.8117965721502389,
+            "velocityX": -1.5709371484381591,
+            "velocityY": -1.582657334931321,
+            "timestamp": 2.0751139467097883
+        },
+        {
+            "x": 7.267508984071126,
+            "y": 3.6053961445739597,
+            "heading": 2.9106651455038604,
+            "angularVelocity": -0.9704712791847574,
+            "velocityX": -2.271470296153853,
+            "velocityY": -1.6224593703641987,
+            "timestamp": 2.159433492604679
+        },
+        {
+            "x": 7.016701982610521,
+            "y": 3.4693216018432858,
+            "heading": 2.828330372603407,
+            "angularVelocity": -0.9764612940763276,
+            "velocityX": -2.9744823551736075,
+            "velocityY": -1.6137959625671907,
+            "timestamp": 2.24375303849957
+        },
+        {
+            "x": 6.717326936237698,
+            "y": 3.359230054214691,
+            "heading": 2.828330320326688,
+            "angularVelocity": -6.199834052808441e-7,
+            "velocityX": -3.5504821947927914,
+            "velocityY": -1.3056468279114077,
+            "timestamp": 2.328072584394461
+        },
+        {
+            "x": 6.402750279383998,
+            "y": 3.3064370272518535,
+            "heading": 2.8283303027221653,
+            "angularVelocity": -2.0878341406433196e-7,
+            "velocityX": -3.730767920000857,
+            "velocityY": -0.6261066328398724,
+            "timestamp": 2.412392130289352
+        },
+        {
+            "x": 6.0870128630404565,
+            "y": 3.261099838136822,
+            "heading": 2.8283302851918997,
+            "angularVelocity": -2.0790275245783378e-7,
+            "velocityX": -3.7445341171206636,
+            "velocityY": -0.5376830322538348,
+            "timestamp": 2.4967116761842427
+        },
+        {
+            "x": 5.771275440975781,
+            "y": 3.215762688864957,
+            "heading": 2.8283302676616344,
+            "angularVelocity": -2.079027527224642e-7,
+            "velocityX": -3.7445341849713016,
+            "velocityY": -0.5376825597279692,
+            "timestamp": 2.5810312220791336
+        },
+        {
+            "x": 5.455538018911073,
+            "y": 3.1704255395933156,
+            "heading": 2.8283302501313687,
+            "angularVelocity": -2.079027528417884e-7,
+            "velocityX": -3.7445341849716827,
+            "velocityY": -0.5376825597253148,
+            "timestamp": 2.6653507679740245
+        },
+        {
+            "x": 5.13980059641411,
+            "y": 3.1250883933321525,
+            "heading": 2.8283302326010555,
+            "angularVelocity": -2.0790331306647263e-7,
+            "velocityX": -3.7445341900980704,
+            "velocityY": -0.5376825240221068,
+            "timestamp": 2.7496703138689154
+        },
+        {
+            "x": 4.847289842857696,
+            "y": 3.0834618283006696,
+            "heading": 2.7687586646684967,
+            "angularVelocity": -0.706497731935343,
+            "velocityX": -3.469074109116353,
+            "velocityY": -0.4936763426521838,
+            "timestamp": 2.8339898597638062
+        },
+        {
+            "x": 4.613405710202319,
+            "y": 3.0500832751446354,
+            "heading": 2.6983183223854565,
+            "angularVelocity": -0.8353975526724018,
+            "velocityX": -2.773783114853639,
+            "velocityY": -0.39585783820090814,
+            "timestamp": 2.918309405658697
+        },
+        {
+            "x": 4.43813741968374,
+            "y": 3.025042089065369,
+            "heading": 2.6386185119236694,
+            "angularVelocity": -0.7080186429870714,
+            "velocityX": -2.0786199529236167,
+            "velocityY": -0.29697961265685946,
+            "timestamp": 3.002628951553588
+        },
+        {
+            "x": 4.321361759336474,
+            "y": 3.008347212158301,
+            "heading": 2.596145764785128,
+            "angularVelocity": -0.5037117632427286,
+            "velocityX": -1.3849180413380506,
+            "velocityY": -0.19799533702279348,
+            "timestamp": 3.086948497448479
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -0.26264093989232323,
+            "velocityX": -0.6921498297586374,
+            "velocityY": -0.0989949847299469,
+            "timestamp": 3.1712680433433698
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": -1.507480653475374e-32,
+            "velocityY": 0,
+            "timestamp": 3.2555875892382606
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S4.1.traj
new file mode 100644
index 00000000..47cb5e7c
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S4.1.traj	
@@ -0,0 +1,364 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.35775083125341,
+            "y": 4.369912807689986,
+            "heading": 2.783102260210542,
+            "angularVelocity": 0.39244175739179554,
+            "velocityX": 0.6846613362282545,
+            "velocityY": 0.11752066613584591,
+            "timestamp": 0.08434948520907419
+        },
+        {
+            "x": 2.473379945358936,
+            "y": 4.389732628016986,
+            "heading": 2.846401171320743,
+            "angularVelocity": 0.7504362469231927,
+            "velocityX": 1.3708336668436103,
+            "velocityY": 0.2349726293868104,
+            "timestamp": 0.16869897041814838
+        },
+        {
+            "x": 2.647085500332804,
+            "y": 4.419444254302336,
+            "heading": 2.9346614354688145,
+            "angularVelocity": 1.0463639929669244,
+            "velocityX": 2.0593552473178725,
+            "velocityY": 0.35224431081829954,
+            "timestamp": 0.25304845562722256
+        },
+        {
+            "x": 2.8791780514976093,
+            "y": 4.45897416371866,
+            "heading": 3.036264025920344,
+            "angularVelocity": 1.2045431006447933,
+            "velocityX": 2.7515585968251655,
+            "velocityY": 0.4686443470086779,
+            "timestamp": 0.33739794083629676
+        },
+        {
+            "x": 3.1692885380051594,
+            "y": 4.507734938420023,
+            "heading": 3.1081945167682674,
+            "angularVelocity": 0.8527673959080092,
+            "velocityX": 3.439386568732019,
+            "velocityY": 0.5780802879887197,
+            "timestamp": 0.42174742604537097
+        },
+        {
+            "x": 3.483614233052985,
+            "y": 4.562663983733042,
+            "heading": 3.108194542936824,
+            "angularVelocity": 3.1023966932602597e-7,
+            "velocityX": 3.726468445761316,
+            "velocityY": 0.651207831048041,
+            "timestamp": 0.5060969112544451
+        },
+        {
+            "x": 3.7979399253430755,
+            "y": 4.61759304482709,
+            "heading": 3.108194569105307,
+            "angularVelocity": 3.102387994950786e-7,
+            "velocityX": 3.726468413067156,
+            "velocityY": 0.6512080181390185,
+            "timestamp": 0.5904463964635193
+        },
+        {
+            "x": 4.112265617633151,
+            "y": 4.67252210592122,
+            "heading": 3.1081945952737904,
+            "angularVelocity": 3.1023880025122276e-7,
+            "velocityX": 3.7264684130669847,
+            "velocityY": 0.6512080181399977,
+            "timestamp": 0.6747958816725934
+        },
+        {
+            "x": 4.426591309923227,
+            "y": 4.72745116701535,
+            "heading": 3.1081946214422733,
+            "angularVelocity": 3.102388010308895e-7,
+            "velocityX": 3.7264684130669847,
+            "velocityY": 0.6512080181399976,
+            "timestamp": 0.7591453668816676
+        },
+        {
+            "x": 4.740917002213303,
+            "y": 4.7823802281094805,
+            "heading": 3.1081946476107567,
+            "angularVelocity": 3.1023880026526175e-7,
+            "velocityX": 3.726468413066985,
+            "velocityY": 0.6512080181399978,
+            "timestamp": 0.8434948520907417
+        },
+        {
+            "x": 5.055242694503379,
+            "y": 4.837309289203611,
+            "heading": 3.1081946737792396,
+            "angularVelocity": 3.1023880190764424e-7,
+            "velocityX": 3.7264684130669847,
+            "velocityY": 0.6512080181399977,
+            "timestamp": 0.9278443372998159
+        },
+        {
+            "x": 5.369568386793454,
+            "y": 4.892238350297741,
+            "heading": 3.108194699947723,
+            "angularVelocity": 3.102388017253904e-7,
+            "velocityX": 3.726468413066985,
+            "velocityY": 0.6512080181399975,
+            "timestamp": 1.01219382250889
+        },
+        {
+            "x": 5.683894079083919,
+            "y": 4.947167411389647,
+            "heading": 3.108194726116206,
+            "angularVelocity": 3.102388024033471e-7,
+            "velocityX": 3.7264684130715926,
+            "velocityY": 0.6512080181136309,
+            "timestamp": 1.0965433077179643
+        },
+        {
+            "x": 5.9982198456255675,
+            "y": 5.002096047585362,
+            "heading": 3.108194752284696,
+            "angularVelocity": 3.102388798208338e-7,
+            "velocityX": 3.7264692933518155,
+            "velocityY": 0.6512029807836425,
+            "timestamp": 1.1808927929270385
+        },
+        {
+            "x": 6.316938354394085,
+            "y": 5.017469863405169,
+            "heading": 3.1081947796852947,
+            "angularVelocity": 3.2484607308932857e-7,
+            "velocityX": 3.7785471716694046,
+            "velocityY": 0.18226330346533962,
+            "timestamp": 1.2652422781361128
+        },
+        {
+            "x": 6.6329944526576226,
+            "y": 4.973579291611239,
+            "heading": 3.1081948124098697,
+            "angularVelocity": 3.8796413400864556e-7,
+            "velocityX": 3.7469831319081606,
+            "velocityY": -0.5203419046973292,
+            "timestamp": 1.349591763345187
+        },
+        {
+            "x": 6.935463453156901,
+            "y": 4.87194123143387,
+            "heading": 3.1081950470159,
+            "angularVelocity": 0.0000027813569896549863,
+            "velocityX": 3.5859021516202274,
+            "velocityY": -1.2049636097415746,
+            "timestamp": 1.4339412485542613
+        },
+        {
+            "x": 7.201083494587787,
+            "y": 4.724560137748792,
+            "heading": 3.137800042082307,
+            "angularVelocity": 0.3509801511298589,
+            "velocityX": 3.1490416423111984,
+            "velocityY": -1.7472672574084946,
+            "timestamp": 1.5182907337633356
+        },
+        {
+            "x": 7.415442295317108,
+            "y": 4.547383804871535,
+            "heading": 3.160342409540337,
+            "angularVelocity": 0.26724961512396933,
+            "velocityX": 2.541317237419968,
+            "velocityY": -2.1005028357683178,
+            "timestamp": 1.6026402189724098
+        },
+        {
+            "x": 7.575455349602933,
+            "y": 4.347068945452764,
+            "heading": 3.163134577298841,
+            "angularVelocity": 0.033102368693571485,
+            "velocityX": 1.8970246693172619,
+            "velocityY": -2.374820177292801,
+            "timestamp": 1.686989704181484
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": -0.27427052152716735,
+            "velocityX": 1.2394225067045201,
+            "velocityY": -2.6090134979162847,
+            "timestamp": 1.7713391893905583
+        },
+        {
+            "x": 7.728694415127363,
+            "y": 3.916223066041241,
+            "heading": 3.095787658836112,
+            "angularVelocity": -0.5866618963302697,
+            "velocityX": 0.6461353813727732,
+            "velocityY": -2.796838903430975,
+            "timestamp": 1.84670174734847
+        },
+        {
+            "x": 7.732075801393433,
+            "y": 3.692700126888283,
+            "heading": 3.0330227964139005,
+            "angularVelocity": -0.8328388011625617,
+            "velocityX": 0.04486825232179016,
+            "velocityY": -2.9659680511082303,
+            "timestamp": 1.9220643053063815
+        },
+        {
+            "x": 7.689365986032336,
+            "y": 3.4587309949886484,
+            "heading": 2.9594753921163566,
+            "angularVelocity": -0.9759143836202951,
+            "velocityX": -0.5667245979754308,
+            "velocityY": -3.10458055351971,
+            "timestamp": 1.997426863264293
+        },
+        {
+            "x": 7.599710660115244,
+            "y": 3.218597864956733,
+            "heading": 2.8890529754773646,
+            "angularVelocity": -0.9344483327957235,
+            "velocityX": -1.1896534346294716,
+            "velocityY": -3.1863718076823337,
+            "timestamp": 2.072789421222205
+        },
+        {
+            "x": 7.463852101408951,
+            "y": 2.9820453977091343,
+            "heading": 2.853986095526745,
+            "angularVelocity": -0.4653090460411922,
+            "velocityX": -1.8027328475523154,
+            "velocityY": -3.138859317642961,
+            "timestamp": 2.1481519791801165
+        },
+        {
+            "x": 7.284189485763313,
+            "y": 2.760688689378167,
+            "heading": 2.8539858842251613,
+            "angularVelocity": -0.000002803800583333773,
+            "velocityX": -2.3839771434772157,
+            "velocityY": -2.9372239256341386,
+            "timestamp": 2.223514537138028
+        },
+        {
+            "x": 7.070363087296056,
+            "y": 2.5721263513481594,
+            "heading": 2.853985845147589,
+            "angularVelocity": -5.185276832708053e-7,
+            "velocityX": -2.8373028233287183,
+            "velocityY": -2.502069238882734,
+            "timestamp": 2.2988770950959396
+        },
+        {
+            "x": 6.82827305330887,
+            "y": 2.421561078917411,
+            "heading": 2.8539858112509897,
+            "angularVelocity": -4.4978037103687734e-7,
+            "velocityX": -3.212338335468772,
+            "velocityY": -1.9978790066392913,
+            "timestamp": 2.374239653053851
+        },
+        {
+            "x": 6.571710658920333,
+            "y": 2.2972500819001187,
+            "heading": 2.8539857783310936,
+            "angularVelocity": -4.368203133600748e-7,
+            "velocityX": -3.4043748160966305,
+            "velocityY": -1.6495060728527418,
+            "timestamp": 2.449602211011763
+        },
+        {
+            "x": 6.3151480830563145,
+            "y": 2.172939459426277,
+            "heading": 2.8539857454112014,
+            "angularVelocity": -4.3682025848078035e-7,
+            "velocityX": -3.4043772241290333,
+            "velocityY": -1.6495011029650315,
+            "timestamp": 2.5249647689696744
+        },
+        {
+            "x": 6.058585509566717,
+            "y": 2.048628832051934,
+            "heading": 2.853985712491288,
+            "angularVelocity": -4.3682054129494706e-7,
+            "velocityX": -3.404377192622409,
+            "velocityY": -1.6495011679907159,
+            "timestamp": 2.600327326927586
+        },
+        {
+            "x": 5.808645132247433,
+            "y": 1.9267432640802056,
+            "heading": 2.8368087867037386,
+            "angularVelocity": -0.22792386900060174,
+            "velocityX": -3.31650602224609,
+            "velocityY": -1.6173225972478102,
+            "timestamp": 2.6756898848854975
+        },
+        {
+            "x": 5.597200041398206,
+            "y": 1.824292282290654,
+            "heading": 2.751245197802903,
+            "angularVelocity": -1.1353594041834565,
+            "velocityX": -2.805704803270014,
+            "velocityY": -1.3594414065240215,
+            "timestamp": 2.751052442843409
+        },
+        {
+            "x": 5.4283718675083765,
+            "y": 1.7424505925946856,
+            "heading": 2.6610334034240704,
+            "angularVelocity": -1.1970373196357535,
+            "velocityX": -2.2402128917136284,
+            "velocityY": -1.0859728214330957,
+            "timestamp": 2.8264150008013207
+        },
+        {
+            "x": 5.302034408244249,
+            "y": 1.6811743111050435,
+            "heading": 2.5842866606936457,
+            "angularVelocity": -1.0183670088969912,
+            "velocityX": -1.6763955827333314,
+            "velocityY": -0.8130865399216356,
+            "timestamp": 2.9017775587592323
+        },
+        {
+            "x": 5.217968301319177,
+            "y": 1.6403775914720826,
+            "heading": 2.5290716090988563,
+            "angularVelocity": -0.7326589368904639,
+            "velocityX": -1.1154890333210488,
+            "velocityY": -0.5413393698200221,
+            "timestamp": 2.977140116717144
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.38575666599708336,
+            "velocityX": -0.5568853082536673,
+            "velocityY": -0.27039410582988466,
+            "timestamp": 3.0525026746750554
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": 5.6652938345374496e-33,
+            "velocityY": 3.5857679285094535e-32,
+            "timestamp": 3.127865232632967
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S4.traj
new file mode 100644
index 00000000..47cb5e7c
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S4.traj	
@@ -0,0 +1,364 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.35775083125341,
+            "y": 4.369912807689986,
+            "heading": 2.783102260210542,
+            "angularVelocity": 0.39244175739179554,
+            "velocityX": 0.6846613362282545,
+            "velocityY": 0.11752066613584591,
+            "timestamp": 0.08434948520907419
+        },
+        {
+            "x": 2.473379945358936,
+            "y": 4.389732628016986,
+            "heading": 2.846401171320743,
+            "angularVelocity": 0.7504362469231927,
+            "velocityX": 1.3708336668436103,
+            "velocityY": 0.2349726293868104,
+            "timestamp": 0.16869897041814838
+        },
+        {
+            "x": 2.647085500332804,
+            "y": 4.419444254302336,
+            "heading": 2.9346614354688145,
+            "angularVelocity": 1.0463639929669244,
+            "velocityX": 2.0593552473178725,
+            "velocityY": 0.35224431081829954,
+            "timestamp": 0.25304845562722256
+        },
+        {
+            "x": 2.8791780514976093,
+            "y": 4.45897416371866,
+            "heading": 3.036264025920344,
+            "angularVelocity": 1.2045431006447933,
+            "velocityX": 2.7515585968251655,
+            "velocityY": 0.4686443470086779,
+            "timestamp": 0.33739794083629676
+        },
+        {
+            "x": 3.1692885380051594,
+            "y": 4.507734938420023,
+            "heading": 3.1081945167682674,
+            "angularVelocity": 0.8527673959080092,
+            "velocityX": 3.439386568732019,
+            "velocityY": 0.5780802879887197,
+            "timestamp": 0.42174742604537097
+        },
+        {
+            "x": 3.483614233052985,
+            "y": 4.562663983733042,
+            "heading": 3.108194542936824,
+            "angularVelocity": 3.1023966932602597e-7,
+            "velocityX": 3.726468445761316,
+            "velocityY": 0.651207831048041,
+            "timestamp": 0.5060969112544451
+        },
+        {
+            "x": 3.7979399253430755,
+            "y": 4.61759304482709,
+            "heading": 3.108194569105307,
+            "angularVelocity": 3.102387994950786e-7,
+            "velocityX": 3.726468413067156,
+            "velocityY": 0.6512080181390185,
+            "timestamp": 0.5904463964635193
+        },
+        {
+            "x": 4.112265617633151,
+            "y": 4.67252210592122,
+            "heading": 3.1081945952737904,
+            "angularVelocity": 3.1023880025122276e-7,
+            "velocityX": 3.7264684130669847,
+            "velocityY": 0.6512080181399977,
+            "timestamp": 0.6747958816725934
+        },
+        {
+            "x": 4.426591309923227,
+            "y": 4.72745116701535,
+            "heading": 3.1081946214422733,
+            "angularVelocity": 3.102388010308895e-7,
+            "velocityX": 3.7264684130669847,
+            "velocityY": 0.6512080181399976,
+            "timestamp": 0.7591453668816676
+        },
+        {
+            "x": 4.740917002213303,
+            "y": 4.7823802281094805,
+            "heading": 3.1081946476107567,
+            "angularVelocity": 3.1023880026526175e-7,
+            "velocityX": 3.726468413066985,
+            "velocityY": 0.6512080181399978,
+            "timestamp": 0.8434948520907417
+        },
+        {
+            "x": 5.055242694503379,
+            "y": 4.837309289203611,
+            "heading": 3.1081946737792396,
+            "angularVelocity": 3.1023880190764424e-7,
+            "velocityX": 3.7264684130669847,
+            "velocityY": 0.6512080181399977,
+            "timestamp": 0.9278443372998159
+        },
+        {
+            "x": 5.369568386793454,
+            "y": 4.892238350297741,
+            "heading": 3.108194699947723,
+            "angularVelocity": 3.102388017253904e-7,
+            "velocityX": 3.726468413066985,
+            "velocityY": 0.6512080181399975,
+            "timestamp": 1.01219382250889
+        },
+        {
+            "x": 5.683894079083919,
+            "y": 4.947167411389647,
+            "heading": 3.108194726116206,
+            "angularVelocity": 3.102388024033471e-7,
+            "velocityX": 3.7264684130715926,
+            "velocityY": 0.6512080181136309,
+            "timestamp": 1.0965433077179643
+        },
+        {
+            "x": 5.9982198456255675,
+            "y": 5.002096047585362,
+            "heading": 3.108194752284696,
+            "angularVelocity": 3.102388798208338e-7,
+            "velocityX": 3.7264692933518155,
+            "velocityY": 0.6512029807836425,
+            "timestamp": 1.1808927929270385
+        },
+        {
+            "x": 6.316938354394085,
+            "y": 5.017469863405169,
+            "heading": 3.1081947796852947,
+            "angularVelocity": 3.2484607308932857e-7,
+            "velocityX": 3.7785471716694046,
+            "velocityY": 0.18226330346533962,
+            "timestamp": 1.2652422781361128
+        },
+        {
+            "x": 6.6329944526576226,
+            "y": 4.973579291611239,
+            "heading": 3.1081948124098697,
+            "angularVelocity": 3.8796413400864556e-7,
+            "velocityX": 3.7469831319081606,
+            "velocityY": -0.5203419046973292,
+            "timestamp": 1.349591763345187
+        },
+        {
+            "x": 6.935463453156901,
+            "y": 4.87194123143387,
+            "heading": 3.1081950470159,
+            "angularVelocity": 0.0000027813569896549863,
+            "velocityX": 3.5859021516202274,
+            "velocityY": -1.2049636097415746,
+            "timestamp": 1.4339412485542613
+        },
+        {
+            "x": 7.201083494587787,
+            "y": 4.724560137748792,
+            "heading": 3.137800042082307,
+            "angularVelocity": 0.3509801511298589,
+            "velocityX": 3.1490416423111984,
+            "velocityY": -1.7472672574084946,
+            "timestamp": 1.5182907337633356
+        },
+        {
+            "x": 7.415442295317108,
+            "y": 4.547383804871535,
+            "heading": 3.160342409540337,
+            "angularVelocity": 0.26724961512396933,
+            "velocityX": 2.541317237419968,
+            "velocityY": -2.1005028357683178,
+            "timestamp": 1.6026402189724098
+        },
+        {
+            "x": 7.575455349602933,
+            "y": 4.347068945452764,
+            "heading": 3.163134577298841,
+            "angularVelocity": 0.033102368693571485,
+            "velocityX": 1.8970246693172619,
+            "velocityY": -2.374820177292801,
+            "timestamp": 1.686989704181484
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": -0.27427052152716735,
+            "velocityX": 1.2394225067045201,
+            "velocityY": -2.6090134979162847,
+            "timestamp": 1.7713391893905583
+        },
+        {
+            "x": 7.728694415127363,
+            "y": 3.916223066041241,
+            "heading": 3.095787658836112,
+            "angularVelocity": -0.5866618963302697,
+            "velocityX": 0.6461353813727732,
+            "velocityY": -2.796838903430975,
+            "timestamp": 1.84670174734847
+        },
+        {
+            "x": 7.732075801393433,
+            "y": 3.692700126888283,
+            "heading": 3.0330227964139005,
+            "angularVelocity": -0.8328388011625617,
+            "velocityX": 0.04486825232179016,
+            "velocityY": -2.9659680511082303,
+            "timestamp": 1.9220643053063815
+        },
+        {
+            "x": 7.689365986032336,
+            "y": 3.4587309949886484,
+            "heading": 2.9594753921163566,
+            "angularVelocity": -0.9759143836202951,
+            "velocityX": -0.5667245979754308,
+            "velocityY": -3.10458055351971,
+            "timestamp": 1.997426863264293
+        },
+        {
+            "x": 7.599710660115244,
+            "y": 3.218597864956733,
+            "heading": 2.8890529754773646,
+            "angularVelocity": -0.9344483327957235,
+            "velocityX": -1.1896534346294716,
+            "velocityY": -3.1863718076823337,
+            "timestamp": 2.072789421222205
+        },
+        {
+            "x": 7.463852101408951,
+            "y": 2.9820453977091343,
+            "heading": 2.853986095526745,
+            "angularVelocity": -0.4653090460411922,
+            "velocityX": -1.8027328475523154,
+            "velocityY": -3.138859317642961,
+            "timestamp": 2.1481519791801165
+        },
+        {
+            "x": 7.284189485763313,
+            "y": 2.760688689378167,
+            "heading": 2.8539858842251613,
+            "angularVelocity": -0.000002803800583333773,
+            "velocityX": -2.3839771434772157,
+            "velocityY": -2.9372239256341386,
+            "timestamp": 2.223514537138028
+        },
+        {
+            "x": 7.070363087296056,
+            "y": 2.5721263513481594,
+            "heading": 2.853985845147589,
+            "angularVelocity": -5.185276832708053e-7,
+            "velocityX": -2.8373028233287183,
+            "velocityY": -2.502069238882734,
+            "timestamp": 2.2988770950959396
+        },
+        {
+            "x": 6.82827305330887,
+            "y": 2.421561078917411,
+            "heading": 2.8539858112509897,
+            "angularVelocity": -4.4978037103687734e-7,
+            "velocityX": -3.212338335468772,
+            "velocityY": -1.9978790066392913,
+            "timestamp": 2.374239653053851
+        },
+        {
+            "x": 6.571710658920333,
+            "y": 2.2972500819001187,
+            "heading": 2.8539857783310936,
+            "angularVelocity": -4.368203133600748e-7,
+            "velocityX": -3.4043748160966305,
+            "velocityY": -1.6495060728527418,
+            "timestamp": 2.449602211011763
+        },
+        {
+            "x": 6.3151480830563145,
+            "y": 2.172939459426277,
+            "heading": 2.8539857454112014,
+            "angularVelocity": -4.3682025848078035e-7,
+            "velocityX": -3.4043772241290333,
+            "velocityY": -1.6495011029650315,
+            "timestamp": 2.5249647689696744
+        },
+        {
+            "x": 6.058585509566717,
+            "y": 2.048628832051934,
+            "heading": 2.853985712491288,
+            "angularVelocity": -4.3682054129494706e-7,
+            "velocityX": -3.404377192622409,
+            "velocityY": -1.6495011679907159,
+            "timestamp": 2.600327326927586
+        },
+        {
+            "x": 5.808645132247433,
+            "y": 1.9267432640802056,
+            "heading": 2.8368087867037386,
+            "angularVelocity": -0.22792386900060174,
+            "velocityX": -3.31650602224609,
+            "velocityY": -1.6173225972478102,
+            "timestamp": 2.6756898848854975
+        },
+        {
+            "x": 5.597200041398206,
+            "y": 1.824292282290654,
+            "heading": 2.751245197802903,
+            "angularVelocity": -1.1353594041834565,
+            "velocityX": -2.805704803270014,
+            "velocityY": -1.3594414065240215,
+            "timestamp": 2.751052442843409
+        },
+        {
+            "x": 5.4283718675083765,
+            "y": 1.7424505925946856,
+            "heading": 2.6610334034240704,
+            "angularVelocity": -1.1970373196357535,
+            "velocityX": -2.2402128917136284,
+            "velocityY": -1.0859728214330957,
+            "timestamp": 2.8264150008013207
+        },
+        {
+            "x": 5.302034408244249,
+            "y": 1.6811743111050435,
+            "heading": 2.5842866606936457,
+            "angularVelocity": -1.0183670088969912,
+            "velocityX": -1.6763955827333314,
+            "velocityY": -0.8130865399216356,
+            "timestamp": 2.9017775587592323
+        },
+        {
+            "x": 5.217968301319177,
+            "y": 1.6403775914720826,
+            "heading": 2.5290716090988563,
+            "angularVelocity": -0.7326589368904639,
+            "velocityX": -1.1154890333210488,
+            "velocityY": -0.5413393698200221,
+            "timestamp": 2.977140116717144
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.38575666599708336,
+            "velocityX": -0.5568853082536673,
+            "velocityY": -0.27039410582988466,
+            "timestamp": 3.0525026746750554
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": 5.6652938345374496e-33,
+            "velocityY": 3.5857679285094535e-32,
+            "timestamp": 3.127865232632967
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S3.1.traj
new file mode 100644
index 00000000..41dd6517
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S3.1.traj	
@@ -0,0 +1,364 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -1.5074616592069685e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3610343967602465,
+            "y": 4.342403579285033,
+            "heading": 2.7671031550368004,
+            "angularVelocity": 0.1956775196968839,
+            "velocityX": 0.6982956857107938,
+            "velocityY": -0.20132098163403012,
+            "timestamp": 0.08740480287819585
+        },
+        {
+            "x": 2.4831385894662814,
+            "y": 4.307214947818553,
+            "heading": 2.799662518084232,
+            "angularVelocity": 0.3725122873717296,
+            "velocityX": 1.3969963741717348,
+            "velocityY": -0.4025937969966943,
+            "timestamp": 0.1748096057563917
+        },
+        {
+            "x": 2.6663651072482595,
+            "y": 4.254442931030586,
+            "heading": 2.8448933899423676,
+            "angularVelocity": 0.5174872589229184,
+            "velocityX": 2.0962980494025705,
+            "velocityY": -0.6037656404477811,
+            "timestamp": 0.26221440863458756
+        },
+        {
+            "x": 2.9107932372110517,
+            "y": 4.184113644093446,
+            "heading": 2.897077048059956,
+            "angularVelocity": 0.597034217791321,
+            "velocityX": 2.796506849897233,
+            "velocityY": -0.8046386997193782,
+            "timestamp": 0.3496192115127834
+        },
+        {
+            "x": 3.2164117287156215,
+            "y": 4.096391426129405,
+            "heading": 2.937460118957845,
+            "angularVelocity": 0.46202347660649606,
+            "velocityX": 3.4965869316182627,
+            "velocityY": -1.0036315519901937,
+            "timestamp": 0.43702401439097927
+        },
+        {
+            "x": 3.5340454438791253,
+            "y": 4.004541690732033,
+            "heading": 2.9374601327514616,
+            "angularVelocity": 1.5781302417851277e-7,
+            "velocityX": 3.6340533323568787,
+            "velocityY": -1.0508545568756669,
+            "timestamp": 0.5244288172691751
+        },
+        {
+            "x": 3.8516791581377654,
+            "y": 3.9126919522054426,
+            "heading": 2.93746014654506,
+            "angularVelocity": 1.5781282325664268e-7,
+            "velocityX": 3.634053322004319,
+            "velocityY": -1.050854592677122,
+            "timestamp": 0.611833620147371
+        },
+        {
+            "x": 4.169312872396402,
+            "y": 3.820842213678837,
+            "heading": 2.9374601603386585,
+            "angularVelocity": 1.5781282237025435e-7,
+            "velocityX": 3.63405332200427,
+            "velocityY": -1.0508545926772908,
+            "timestamp": 0.6992384230255668
+        },
+        {
+            "x": 4.486946586655037,
+            "y": 3.7289924751522316,
+            "heading": 2.9374601741322572,
+            "angularVelocity": 1.5781282238426364e-7,
+            "velocityX": 3.63405332200427,
+            "velocityY": -1.0508545926772912,
+            "timestamp": 0.7866432259037627
+        },
+        {
+            "x": 4.804580300913673,
+            "y": 3.637142736625626,
+            "heading": 2.937460187925856,
+            "angularVelocity": 1.578128226198401e-7,
+            "velocityX": 3.63405332200427,
+            "velocityY": -1.0508545926772912,
+            "timestamp": 0.8740480287819585
+        },
+        {
+            "x": 5.122214015172308,
+            "y": 3.545292998099021,
+            "heading": 2.9374602017194547,
+            "angularVelocity": 1.5781282274310823e-7,
+            "velocityX": 3.63405332200427,
+            "velocityY": -1.050854592677291,
+            "timestamp": 0.9614528316601544
+        },
+        {
+            "x": 5.439847729430944,
+            "y": 3.4534432595724156,
+            "heading": 2.937460215513054,
+            "angularVelocity": 1.5781282350457181e-7,
+            "velocityX": 3.63405332200427,
+            "velocityY": -1.050854592677291,
+            "timestamp": 1.0488576345383502
+        },
+        {
+            "x": 5.757481443689581,
+            "y": 3.3615935210458106,
+            "heading": 2.9374602293066525,
+            "angularVelocity": 1.5781282308668676e-7,
+            "velocityX": 3.6340533220042697,
+            "velocityY": -1.0508545926772905,
+            "timestamp": 1.136262437416546
+        },
+        {
+            "x": 6.075115157948217,
+            "y": 3.269743782519205,
+            "heading": 2.9374602431002512,
+            "angularVelocity": 1.578128234236369e-7,
+            "velocityX": 3.63405332200427,
+            "velocityY": -1.050854592677291,
+            "timestamp": 1.2236672402947417
+        },
+        {
+            "x": 6.3927488722066945,
+            "y": 3.1778940439920533,
+            "heading": 2.93746025689385,
+            "angularVelocity": 1.5781282293850814e-7,
+            "velocityX": 3.6340533220024622,
+            "velocityY": -1.0508545926835444,
+            "timestamp": 1.3110720431729375
+        },
+        {
+            "x": 6.710382552956458,
+            "y": 3.0860441895855293,
+            "heading": 2.9374602706874486,
+            "angularVelocity": 1.5781282618344319e-7,
+            "velocityX": 3.634052938628626,
+            "velocityY": -1.0508559184616288,
+            "timestamp": 1.3984768460511332
+        },
+        {
+            "x": 7.02127514506585,
+            "y": 2.9734682388937714,
+            "heading": 2.9374602922500688,
+            "angularVelocity": 2.466983432002081e-7,
+            "velocityX": 3.5569280162171513,
+            "velocityY": -1.2879835773857788,
+            "timestamp": 1.485881648929329
+        },
+        {
+            "x": 7.2761933316047935,
+            "y": 2.8394289044438907,
+            "heading": 3.0149898604295666,
+            "angularVelocity": 0.8870172533600954,
+            "velocityX": 2.9165237852453965,
+            "velocityY": -1.533546556207826,
+            "timestamp": 1.5732864518075247
+        },
+        {
+            "x": 7.469158921958533,
+            "y": 2.7195716127256344,
+            "heading": 3.0771718399112147,
+            "angularVelocity": 0.711425201293604,
+            "velocityX": 2.207722962577334,
+            "velocityY": -1.3712895375473233,
+            "timestamp": 1.6606912546857204
+        },
+        {
+            "x": 7.6006701652701585,
+            "y": 2.6155462997255583,
+            "heading": 3.1196274586402084,
+            "angularVelocity": 0.48573553547347,
+            "velocityX": 1.5046226177627207,
+            "velocityY": -1.1901555701125741,
+            "timestamp": 1.7480960575639162
+        },
+        {
+            "x": 7.670917138983036,
+            "y": 2.527925545784097,
+            "heading": 3.140832038047466,
+            "angularVelocity": 0.24260199335736132,
+            "velocityX": 0.8036969525664591,
+            "velocityY": -1.0024707001921451,
+            "timestamp": 1.835500860442112
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -0.009519363010585313,
+            "velocityX": 0.10391718438655147,
+            "velocityY": -0.8114605084452422,
+            "timestamp": 1.9229056633203077
+        },
+        {
+            "x": 7.63404119577111,
+            "y": 2.4050502975891916,
+            "heading": 3.1190998231163127,
+            "angularVelocity": -0.25309519096022687,
+            "velocityX": -0.5565480329352274,
+            "velocityY": -0.6290961040742543,
+            "timestamp": 2.005483988117367
+        },
+        {
+            "x": 7.533566509755774,
+            "y": 2.3683786594075307,
+            "heading": 3.079403507657828,
+            "angularVelocity": -0.4807110771021385,
+            "velocityX": -1.216719838556405,
+            "velocityY": -0.4440830965242228,
+            "timestamp": 2.0880623129144262
+        },
+        {
+            "x": 7.378621026771135,
+            "y": 2.3473334555907552,
+            "heading": 3.0230319998393305,
+            "angularVelocity": -0.6826429084997041,
+            "velocityX": -1.876345679879389,
+            "velocityY": -0.2548514258250533,
+            "timestamp": 2.1706406377114855
+        },
+        {
+            "x": 7.169309740069539,
+            "y": 2.342556806639995,
+            "heading": 2.953940328458666,
+            "angularVelocity": -0.8366804673073884,
+            "velocityX": -2.5347000828121677,
+            "velocityY": -0.05784385869415158,
+            "timestamp": 2.253218962508545
+        },
+        {
+            "x": 6.906019202766067,
+            "y": 2.3556284163077987,
+            "heading": 2.881970584756294,
+            "angularVelocity": -0.8715331036230265,
+            "velocityX": -3.188373437588175,
+            "velocityY": 0.15829347107643843,
+            "timestamp": 2.335797287305604
+        },
+        {
+            "x": 6.596097301196444,
+            "y": 2.394810276985141,
+            "heading": 2.8819705158229665,
+            "angularVelocity": -8.347629724920641e-7,
+            "velocityX": -3.753065981070365,
+            "velocityY": 0.47448117618799124,
+            "timestamp": 2.4183756121026634
+        },
+        {
+            "x": 6.2936576837370914,
+            "y": 2.4730221394670577,
+            "heading": 2.881970499784504,
+            "angularVelocity": -1.942212104248679e-7,
+            "velocityX": -3.6624576509951607,
+            "velocityY": 0.9471233846669401,
+            "timestamp": 2.5009539368997227
+        },
+        {
+            "x": 5.991218190130788,
+            "y": 2.5512344808782137,
+            "heading": 2.8819704837460454,
+            "angularVelocity": -1.9422117139312037e-7,
+            "velocityX": -3.662456151169988,
+            "velocityY": 0.9471291843636568,
+            "timestamp": 2.583532261696782
+        },
+        {
+            "x": 5.688778696525171,
+            "y": 2.6294468222920195,
+            "heading": 2.8819704677075864,
+            "angularVelocity": -1.9422117079756682e-7,
+            "velocityX": -3.662456151161689,
+            "velocityY": 0.9471291843957483,
+            "timestamp": 2.6661105864938413
+        },
+        {
+            "x": 5.38633920291957,
+            "y": 2.7076591637058867,
+            "heading": 2.881970451669127,
+            "angularVelocity": -1.9422117106487922e-7,
+            "velocityX": -3.6624561511614977,
+            "velocityY": 0.947129184396488,
+            "timestamp": 2.7486889112909005
+        },
+        {
+            "x": 5.0838997121673595,
+            "y": 2.7858715161533545,
+            "heading": 2.881970435630565,
+            "angularVelocity": -1.9422242240409168e-7,
+            "velocityX": -3.662456116607741,
+            "velocityY": 0.9471293180102566,
+            "timestamp": 2.83126723608796
+        },
+        {
+            "x": 4.810061938834441,
+            "y": 2.8575158595499386,
+            "heading": 2.7916066716165844,
+            "angularVelocity": -1.094279452096593,
+            "velocityX": -3.3160974626924187,
+            "velocityY": 0.8675925985741851,
+            "timestamp": 2.913845560885019
+        },
+        {
+            "x": 4.591150287853899,
+            "y": 2.914595495450747,
+            "heading": 2.7083456685941125,
+            "angularVelocity": -1.0082670389244417,
+            "velocityX": -2.650957760629438,
+            "velocityY": 0.6912181379446108,
+            "timestamp": 2.9964238856820784
+        },
+        {
+            "x": 4.427043317855732,
+            "y": 2.957326813953426,
+            "heading": 2.64244137634817,
+            "angularVelocity": -0.7980822135579249,
+            "velocityX": -1.98728867897803,
+            "velocityY": 0.5174640997827658,
+            "timestamp": 3.0790022104791377
+        },
+        {
+            "x": 4.317672771154393,
+            "y": 2.985782903772343,
+            "heading": 2.597132660112387,
+            "angularVelocity": -0.5486756524442865,
+            "velocityX": -1.3244461784629789,
+            "velocityY": 0.34459514514068673,
+            "timestamp": 3.161580535276197
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -0.28012992718412955,
+            "velocityX": -0.6620716911944379,
+            "velocityY": 0.1721649871512405,
+            "timestamp": 3.2441588600732563
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": -8.552238423258885e-34,
+            "velocityY": -9.381987056731701e-33,
+            "timestamp": 3.3267371848703156
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S3.traj
new file mode 100644
index 00000000..41dd6517
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S3.traj	
@@ -0,0 +1,364 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -1.5074616592069685e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3610343967602465,
+            "y": 4.342403579285033,
+            "heading": 2.7671031550368004,
+            "angularVelocity": 0.1956775196968839,
+            "velocityX": 0.6982956857107938,
+            "velocityY": -0.20132098163403012,
+            "timestamp": 0.08740480287819585
+        },
+        {
+            "x": 2.4831385894662814,
+            "y": 4.307214947818553,
+            "heading": 2.799662518084232,
+            "angularVelocity": 0.3725122873717296,
+            "velocityX": 1.3969963741717348,
+            "velocityY": -0.4025937969966943,
+            "timestamp": 0.1748096057563917
+        },
+        {
+            "x": 2.6663651072482595,
+            "y": 4.254442931030586,
+            "heading": 2.8448933899423676,
+            "angularVelocity": 0.5174872589229184,
+            "velocityX": 2.0962980494025705,
+            "velocityY": -0.6037656404477811,
+            "timestamp": 0.26221440863458756
+        },
+        {
+            "x": 2.9107932372110517,
+            "y": 4.184113644093446,
+            "heading": 2.897077048059956,
+            "angularVelocity": 0.597034217791321,
+            "velocityX": 2.796506849897233,
+            "velocityY": -0.8046386997193782,
+            "timestamp": 0.3496192115127834
+        },
+        {
+            "x": 3.2164117287156215,
+            "y": 4.096391426129405,
+            "heading": 2.937460118957845,
+            "angularVelocity": 0.46202347660649606,
+            "velocityX": 3.4965869316182627,
+            "velocityY": -1.0036315519901937,
+            "timestamp": 0.43702401439097927
+        },
+        {
+            "x": 3.5340454438791253,
+            "y": 4.004541690732033,
+            "heading": 2.9374601327514616,
+            "angularVelocity": 1.5781302417851277e-7,
+            "velocityX": 3.6340533323568787,
+            "velocityY": -1.0508545568756669,
+            "timestamp": 0.5244288172691751
+        },
+        {
+            "x": 3.8516791581377654,
+            "y": 3.9126919522054426,
+            "heading": 2.93746014654506,
+            "angularVelocity": 1.5781282325664268e-7,
+            "velocityX": 3.634053322004319,
+            "velocityY": -1.050854592677122,
+            "timestamp": 0.611833620147371
+        },
+        {
+            "x": 4.169312872396402,
+            "y": 3.820842213678837,
+            "heading": 2.9374601603386585,
+            "angularVelocity": 1.5781282237025435e-7,
+            "velocityX": 3.63405332200427,
+            "velocityY": -1.0508545926772908,
+            "timestamp": 0.6992384230255668
+        },
+        {
+            "x": 4.486946586655037,
+            "y": 3.7289924751522316,
+            "heading": 2.9374601741322572,
+            "angularVelocity": 1.5781282238426364e-7,
+            "velocityX": 3.63405332200427,
+            "velocityY": -1.0508545926772912,
+            "timestamp": 0.7866432259037627
+        },
+        {
+            "x": 4.804580300913673,
+            "y": 3.637142736625626,
+            "heading": 2.937460187925856,
+            "angularVelocity": 1.578128226198401e-7,
+            "velocityX": 3.63405332200427,
+            "velocityY": -1.0508545926772912,
+            "timestamp": 0.8740480287819585
+        },
+        {
+            "x": 5.122214015172308,
+            "y": 3.545292998099021,
+            "heading": 2.9374602017194547,
+            "angularVelocity": 1.5781282274310823e-7,
+            "velocityX": 3.63405332200427,
+            "velocityY": -1.050854592677291,
+            "timestamp": 0.9614528316601544
+        },
+        {
+            "x": 5.439847729430944,
+            "y": 3.4534432595724156,
+            "heading": 2.937460215513054,
+            "angularVelocity": 1.5781282350457181e-7,
+            "velocityX": 3.63405332200427,
+            "velocityY": -1.050854592677291,
+            "timestamp": 1.0488576345383502
+        },
+        {
+            "x": 5.757481443689581,
+            "y": 3.3615935210458106,
+            "heading": 2.9374602293066525,
+            "angularVelocity": 1.5781282308668676e-7,
+            "velocityX": 3.6340533220042697,
+            "velocityY": -1.0508545926772905,
+            "timestamp": 1.136262437416546
+        },
+        {
+            "x": 6.075115157948217,
+            "y": 3.269743782519205,
+            "heading": 2.9374602431002512,
+            "angularVelocity": 1.578128234236369e-7,
+            "velocityX": 3.63405332200427,
+            "velocityY": -1.050854592677291,
+            "timestamp": 1.2236672402947417
+        },
+        {
+            "x": 6.3927488722066945,
+            "y": 3.1778940439920533,
+            "heading": 2.93746025689385,
+            "angularVelocity": 1.5781282293850814e-7,
+            "velocityX": 3.6340533220024622,
+            "velocityY": -1.0508545926835444,
+            "timestamp": 1.3110720431729375
+        },
+        {
+            "x": 6.710382552956458,
+            "y": 3.0860441895855293,
+            "heading": 2.9374602706874486,
+            "angularVelocity": 1.5781282618344319e-7,
+            "velocityX": 3.634052938628626,
+            "velocityY": -1.0508559184616288,
+            "timestamp": 1.3984768460511332
+        },
+        {
+            "x": 7.02127514506585,
+            "y": 2.9734682388937714,
+            "heading": 2.9374602922500688,
+            "angularVelocity": 2.466983432002081e-7,
+            "velocityX": 3.5569280162171513,
+            "velocityY": -1.2879835773857788,
+            "timestamp": 1.485881648929329
+        },
+        {
+            "x": 7.2761933316047935,
+            "y": 2.8394289044438907,
+            "heading": 3.0149898604295666,
+            "angularVelocity": 0.8870172533600954,
+            "velocityX": 2.9165237852453965,
+            "velocityY": -1.533546556207826,
+            "timestamp": 1.5732864518075247
+        },
+        {
+            "x": 7.469158921958533,
+            "y": 2.7195716127256344,
+            "heading": 3.0771718399112147,
+            "angularVelocity": 0.711425201293604,
+            "velocityX": 2.207722962577334,
+            "velocityY": -1.3712895375473233,
+            "timestamp": 1.6606912546857204
+        },
+        {
+            "x": 7.6006701652701585,
+            "y": 2.6155462997255583,
+            "heading": 3.1196274586402084,
+            "angularVelocity": 0.48573553547347,
+            "velocityX": 1.5046226177627207,
+            "velocityY": -1.1901555701125741,
+            "timestamp": 1.7480960575639162
+        },
+        {
+            "x": 7.670917138983036,
+            "y": 2.527925545784097,
+            "heading": 3.140832038047466,
+            "angularVelocity": 0.24260199335736132,
+            "velocityX": 0.8036969525664591,
+            "velocityY": -1.0024707001921451,
+            "timestamp": 1.835500860442112
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -0.009519363010585313,
+            "velocityX": 0.10391718438655147,
+            "velocityY": -0.8114605084452422,
+            "timestamp": 1.9229056633203077
+        },
+        {
+            "x": 7.63404119577111,
+            "y": 2.4050502975891916,
+            "heading": 3.1190998231163127,
+            "angularVelocity": -0.25309519096022687,
+            "velocityX": -0.5565480329352274,
+            "velocityY": -0.6290961040742543,
+            "timestamp": 2.005483988117367
+        },
+        {
+            "x": 7.533566509755774,
+            "y": 2.3683786594075307,
+            "heading": 3.079403507657828,
+            "angularVelocity": -0.4807110771021385,
+            "velocityX": -1.216719838556405,
+            "velocityY": -0.4440830965242228,
+            "timestamp": 2.0880623129144262
+        },
+        {
+            "x": 7.378621026771135,
+            "y": 2.3473334555907552,
+            "heading": 3.0230319998393305,
+            "angularVelocity": -0.6826429084997041,
+            "velocityX": -1.876345679879389,
+            "velocityY": -0.2548514258250533,
+            "timestamp": 2.1706406377114855
+        },
+        {
+            "x": 7.169309740069539,
+            "y": 2.342556806639995,
+            "heading": 2.953940328458666,
+            "angularVelocity": -0.8366804673073884,
+            "velocityX": -2.5347000828121677,
+            "velocityY": -0.05784385869415158,
+            "timestamp": 2.253218962508545
+        },
+        {
+            "x": 6.906019202766067,
+            "y": 2.3556284163077987,
+            "heading": 2.881970584756294,
+            "angularVelocity": -0.8715331036230265,
+            "velocityX": -3.188373437588175,
+            "velocityY": 0.15829347107643843,
+            "timestamp": 2.335797287305604
+        },
+        {
+            "x": 6.596097301196444,
+            "y": 2.394810276985141,
+            "heading": 2.8819705158229665,
+            "angularVelocity": -8.347629724920641e-7,
+            "velocityX": -3.753065981070365,
+            "velocityY": 0.47448117618799124,
+            "timestamp": 2.4183756121026634
+        },
+        {
+            "x": 6.2936576837370914,
+            "y": 2.4730221394670577,
+            "heading": 2.881970499784504,
+            "angularVelocity": -1.942212104248679e-7,
+            "velocityX": -3.6624576509951607,
+            "velocityY": 0.9471233846669401,
+            "timestamp": 2.5009539368997227
+        },
+        {
+            "x": 5.991218190130788,
+            "y": 2.5512344808782137,
+            "heading": 2.8819704837460454,
+            "angularVelocity": -1.9422117139312037e-7,
+            "velocityX": -3.662456151169988,
+            "velocityY": 0.9471291843636568,
+            "timestamp": 2.583532261696782
+        },
+        {
+            "x": 5.688778696525171,
+            "y": 2.6294468222920195,
+            "heading": 2.8819704677075864,
+            "angularVelocity": -1.9422117079756682e-7,
+            "velocityX": -3.662456151161689,
+            "velocityY": 0.9471291843957483,
+            "timestamp": 2.6661105864938413
+        },
+        {
+            "x": 5.38633920291957,
+            "y": 2.7076591637058867,
+            "heading": 2.881970451669127,
+            "angularVelocity": -1.9422117106487922e-7,
+            "velocityX": -3.6624561511614977,
+            "velocityY": 0.947129184396488,
+            "timestamp": 2.7486889112909005
+        },
+        {
+            "x": 5.0838997121673595,
+            "y": 2.7858715161533545,
+            "heading": 2.881970435630565,
+            "angularVelocity": -1.9422242240409168e-7,
+            "velocityX": -3.662456116607741,
+            "velocityY": 0.9471293180102566,
+            "timestamp": 2.83126723608796
+        },
+        {
+            "x": 4.810061938834441,
+            "y": 2.8575158595499386,
+            "heading": 2.7916066716165844,
+            "angularVelocity": -1.094279452096593,
+            "velocityX": -3.3160974626924187,
+            "velocityY": 0.8675925985741851,
+            "timestamp": 2.913845560885019
+        },
+        {
+            "x": 4.591150287853899,
+            "y": 2.914595495450747,
+            "heading": 2.7083456685941125,
+            "angularVelocity": -1.0082670389244417,
+            "velocityX": -2.650957760629438,
+            "velocityY": 0.6912181379446108,
+            "timestamp": 2.9964238856820784
+        },
+        {
+            "x": 4.427043317855732,
+            "y": 2.957326813953426,
+            "heading": 2.64244137634817,
+            "angularVelocity": -0.7980822135579249,
+            "velocityX": -1.98728867897803,
+            "velocityY": 0.5174640997827658,
+            "timestamp": 3.0790022104791377
+        },
+        {
+            "x": 4.317672771154393,
+            "y": 2.985782903772343,
+            "heading": 2.597132660112387,
+            "angularVelocity": -0.5486756524442865,
+            "velocityX": -1.3244461784629789,
+            "velocityY": 0.34459514514068673,
+            "timestamp": 3.161580535276197
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -0.28012992718412955,
+            "velocityX": -0.6620716911944379,
+            "velocityY": 0.1721649871512405,
+            "timestamp": 3.2441588600732563
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": -8.552238423258885e-34,
+            "velocityY": -9.381987056731701e-33,
+            "timestamp": 3.3267371848703156
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S4.1.traj
new file mode 100644
index 00000000..43401240
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S4.1.traj	
@@ -0,0 +1,346 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.360128743568809,
+            "y": 4.349177185639382,
+            "heading": 2.7782206485451093,
+            "angularVelocity": 0.3283351874090942,
+            "velocityX": 0.6995722389859838,
+            "velocityY": -0.1259188206007136,
+            "timestamp": 0.0859507284851176
+        },
+        {
+            "x": 2.4804864804971807,
+            "y": 4.327553542642656,
+            "heading": 2.831856423124361,
+            "angularVelocity": 0.6240293191760308,
+            "velocityX": 1.4003108414515997,
+            "velocityY": -0.2515818466910346,
+            "timestamp": 0.1719014569702352
+        },
+        {
+            "x": 2.6612218913219765,
+            "y": 4.295165149158174,
+            "heading": 2.9060715845563863,
+            "angularVelocity": 0.863461691832829,
+            "velocityX": 2.102779278433835,
+            "velocityY": -0.3768251189411322,
+            "timestamp": 0.2578521854553528
+        },
+        {
+            "x": 2.902552042800783,
+            "y": 4.252097385240196,
+            "heading": 2.990739532355289,
+            "angularVelocity": 0.9850753948357858,
+            "velocityX": 2.8077731943900375,
+            "velocityY": -0.5010750307419894,
+            "timestamp": 0.3438029139404704
+        },
+        {
+            "x": 3.2042136582065344,
+            "y": 4.198837394002331,
+            "heading": 3.04987623940461,
+            "angularVelocity": 0.6880303179693488,
+            "velocityX": 3.5097039981224545,
+            "velocityY": -0.6196572405676055,
+            "timestamp": 0.429753642425588
+        },
+        {
+            "x": 3.524112842751923,
+            "y": 4.1406587673803905,
+            "heading": 3.0498762636795202,
+            "angularVelocity": 2.824282081012085e-7,
+            "velocityX": 3.7218903223232087,
+            "velocityY": -0.6768834615754469,
+            "timestamp": 0.5157043709107056
+        },
+        {
+            "x": 3.844012025527027,
+            "y": 4.082480131024319,
+            "heading": 3.0498762879543886,
+            "angularVelocity": 2.824277225814376e-7,
+            "velocityX": 3.7218903017266984,
+            "velocityY": -0.6768835748279012,
+            "timestamp": 0.6016550993958233
+        },
+        {
+            "x": 4.163911208302122,
+            "y": 4.0243014946682,
+            "heading": 3.049876312229257,
+            "angularVelocity": 2.824277213255298e-7,
+            "velocityX": 3.721890301726596,
+            "velocityY": -0.6768835748284626,
+            "timestamp": 0.6876058278809409
+        },
+        {
+            "x": 4.483810391077216,
+            "y": 3.9661228583120804,
+            "heading": 3.0498763365041253,
+            "angularVelocity": 2.8242772165714766e-7,
+            "velocityX": 3.7218903017265963,
+            "velocityY": -0.6768835748284627,
+            "timestamp": 0.7735565563660586
+        },
+        {
+            "x": 4.80370957385231,
+            "y": 3.9079442219559613,
+            "heading": 3.0498763607789936,
+            "angularVelocity": 2.8242772140625194e-7,
+            "velocityX": 3.7218903017265963,
+            "velocityY": -0.6768835748284627,
+            "timestamp": 0.8595072848511762
+        },
+        {
+            "x": 5.123608756627404,
+            "y": 3.8497655855998416,
+            "heading": 3.049876385053862,
+            "angularVelocity": 2.824277226048516e-7,
+            "velocityX": 3.7218903017265963,
+            "velocityY": -0.6768835748284628,
+            "timestamp": 0.9454580133362939
+        },
+        {
+            "x": 5.443507939402499,
+            "y": 3.7915869492437224,
+            "heading": 3.0498764093287303,
+            "angularVelocity": 2.8242772163119933e-7,
+            "velocityX": 3.7218903017265963,
+            "velocityY": -0.6768835748284626,
+            "timestamp": 1.0314087418214115
+        },
+        {
+            "x": 5.763407122177593,
+            "y": 3.733408312887603,
+            "heading": 3.0498764336035986,
+            "angularVelocity": 2.824277219712686e-7,
+            "velocityX": 3.7218903017265963,
+            "velocityY": -0.6768835748284628,
+            "timestamp": 1.117359470306529
+        },
+        {
+            "x": 6.083306304952576,
+            "y": 3.675229676530871,
+            "heading": 3.049876457878467,
+            "angularVelocity": 2.8242772161077964e-7,
+            "velocityX": 3.7218903017253,
+            "velocityY": -0.6768835748355908,
+            "timestamp": 1.2033101987916466
+        },
+        {
+            "x": 6.403205465262658,
+            "y": 3.6170509166494913,
+            "heading": 3.0498764821533357,
+            "angularVelocity": 2.8242772554820356e-7,
+            "velocityX": 3.721890040355771,
+            "velocityY": -0.6768850119920853,
+            "timestamp": 1.2892609272767641
+        },
+        {
+            "x": 6.718573965768454,
+            "y": 3.5379121175247272,
+            "heading": 3.0498765068050417,
+            "angularVelocity": 2.868120651983924e-7,
+            "velocityX": 3.6691777494404794,
+            "velocityY": -0.9207461125646453,
+            "timestamp": 1.3752116557618816
+        },
+        {
+            "x": 7.013358117396929,
+            "y": 3.4007168540694233,
+            "heading": 3.0498765454511387,
+            "angularVelocity": 4.49630829762963e-7,
+            "velocityX": 3.4296876457482686,
+            "velocityY": -1.5962082680784608,
+            "timestamp": 1.4611623842469992
+        },
+        {
+            "x": 7.268807462845475,
+            "y": 3.2166766255285517,
+            "heading": 3.0757384072562206,
+            "angularVelocity": 0.30089171157615124,
+            "velocityX": 2.9720439832312224,
+            "velocityY": -2.1412294204433464,
+            "timestamp": 1.5471131127321167
+        },
+        {
+            "x": 7.46355872560419,
+            "y": 3.0231641785099956,
+            "heading": 3.117732513124605,
+            "angularVelocity": 0.48858347809886377,
+            "velocityX": 2.2658477268453225,
+            "velocityY": -2.2514346350428145,
+            "timestamp": 1.6330638412172342
+        },
+        {
+            "x": 7.596796563191941,
+            "y": 2.830012972060279,
+            "heading": 3.1482585374873016,
+            "angularVelocity": 0.3551572499816586,
+            "velocityX": 1.5501653090794745,
+            "velocityY": -2.2472317553790035,
+            "timestamp": 1.7190145697023518
+        },
+        {
+            "x": 7.66884686477411,
+            "y": 2.640707793733853,
+            "heading": 3.1573927530357144,
+            "angularVelocity": 0.10627269494275392,
+            "velocityX": 0.8382744725035164,
+            "velocityY": -2.202484861535554,
+            "timestamp": 1.8049652981874693
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -0.2023572498136116,
+            "velocityX": 0.12976196272503107,
+            "velocityY": -2.1373616835098828,
+            "timestamp": 1.8909160266725868
+        },
+        {
+            "x": 7.6422705837740335,
+            "y": 2.299493905398916,
+            "heading": 3.1013004562411446,
+            "angularVelocity": -0.5083194372878264,
+            "velocityX": -0.49557678882949796,
+            "velocityY": -2.068846337723125,
+            "timestamp": 1.9670483578801472
+        },
+        {
+            "x": 7.556907286255345,
+            "y": 2.147926635640613,
+            "heading": 3.0424682290459026,
+            "angularVelocity": -0.7727627180470553,
+            "velocityX": -1.121248964332173,
+            "velocityY": -1.9908397306931678,
+            "timestamp": 2.043180689087708
+        },
+        {
+            "x": 7.423923039902345,
+            "y": 2.003393138237165,
+            "heading": 2.9682494555138628,
+            "angularVelocity": -0.974865373945302,
+            "velocityX": -1.7467512716828084,
+            "velocityY": -1.8984509617786702,
+            "timestamp": 2.119313020295268
+        },
+        {
+            "x": 7.2434803504733996,
+            "y": 1.8677477086842909,
+            "heading": 2.8865548866835007,
+            "angularVelocity": -1.07306012484416,
+            "velocityX": -2.3701190619920354,
+            "velocityY": -1.7817059769660042,
+            "timestamp": 2.1954453515028285
+        },
+        {
+            "x": 7.01645692191088,
+            "y": 1.7447501516568382,
+            "heading": 2.813573416858664,
+            "angularVelocity": -0.9586133600147891,
+            "velocityX": -2.9819581899254453,
+            "velocityY": -1.6155758673946832,
+            "timestamp": 2.271577682710389
+        },
+        {
+            "x": 6.750046151807222,
+            "y": 1.6436496548008426,
+            "heading": 2.8060686181878967,
+            "angularVelocity": -0.09857571089344144,
+            "velocityX": -3.499311867613085,
+            "velocityY": -1.327957455819293,
+            "timestamp": 2.347710013917949
+        },
+        {
+            "x": 6.467461314020777,
+            "y": 1.5880425214231262,
+            "heading": 2.8060685831707377,
+            "angularVelocity": -4.5995122509551036e-7,
+            "velocityX": -3.711758635316633,
+            "velocityY": -0.7304010332495747,
+            "timestamp": 2.4238423451255096
+        },
+        {
+            "x": 6.17955680980973,
+            "y": 1.580470049320341,
+            "heading": 2.806068554276105,
+            "angularVelocity": -3.7953169924465946e-7,
+            "velocityX": -3.7816325816443177,
+            "velocityY": -0.09946460304917487,
+            "timestamp": 2.49997467633307
+        },
+        {
+            "x": 5.8949085342429335,
+            "y": 1.5924545384965154,
+            "heading": 2.7975026231592013,
+            "angularVelocity": -0.11251371107452125,
+            "velocityX": -3.738861940149357,
+            "velocityY": 0.1574165533364508,
+            "timestamp": 2.5761070075406303
+        },
+        {
+            "x": 5.65482422655778,
+            "y": 1.6016450496652885,
+            "heading": 2.72071509082511,
+            "angularVelocity": -1.008606082543986,
+            "velocityX": -3.153513151076552,
+            "velocityY": 0.12071758506519875,
+            "timestamp": 2.6522393387481906
+        },
+        {
+            "x": 5.463026554407694,
+            "y": 1.608988353775347,
+            "heading": 2.6410025465355518,
+            "angularVelocity": -1.0470261848719398,
+            "velocityX": -2.5192670329138944,
+            "velocityY": 0.09645447595750602,
+            "timestamp": 2.728371669955751
+        },
+        {
+            "x": 5.319399038502622,
+            "y": 1.614493352033255,
+            "heading": 2.573619865722977,
+            "angularVelocity": -0.8850731317940332,
+            "velocityX": -1.8865508730252805,
+            "velocityY": 0.07230828441193854,
+            "timestamp": 2.8045040011633113
+        },
+        {
+            "x": 5.223767247762021,
+            "y": 1.6181637826708264,
+            "heading": 2.525354405173392,
+            "angularVelocity": -0.6339679842201956,
+            "velocityX": -1.2561258695714954,
+            "velocityY": 0.04821119463109347,
+            "timestamp": 2.8806363323708717
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.33303072126212585,
+            "velocityX": -0.6274239472819125,
+            "velocityY": 0.02411875874607297,
+            "timestamp": 2.956768663578432
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": -1.0690007247060788e-32,
+            "velocityY": 0,
+            "timestamp": 3.0329009947859924
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S4.traj
new file mode 100644
index 00000000..43401240
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S4.traj	
@@ -0,0 +1,346 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.360128743568809,
+            "y": 4.349177185639382,
+            "heading": 2.7782206485451093,
+            "angularVelocity": 0.3283351874090942,
+            "velocityX": 0.6995722389859838,
+            "velocityY": -0.1259188206007136,
+            "timestamp": 0.0859507284851176
+        },
+        {
+            "x": 2.4804864804971807,
+            "y": 4.327553542642656,
+            "heading": 2.831856423124361,
+            "angularVelocity": 0.6240293191760308,
+            "velocityX": 1.4003108414515997,
+            "velocityY": -0.2515818466910346,
+            "timestamp": 0.1719014569702352
+        },
+        {
+            "x": 2.6612218913219765,
+            "y": 4.295165149158174,
+            "heading": 2.9060715845563863,
+            "angularVelocity": 0.863461691832829,
+            "velocityX": 2.102779278433835,
+            "velocityY": -0.3768251189411322,
+            "timestamp": 0.2578521854553528
+        },
+        {
+            "x": 2.902552042800783,
+            "y": 4.252097385240196,
+            "heading": 2.990739532355289,
+            "angularVelocity": 0.9850753948357858,
+            "velocityX": 2.8077731943900375,
+            "velocityY": -0.5010750307419894,
+            "timestamp": 0.3438029139404704
+        },
+        {
+            "x": 3.2042136582065344,
+            "y": 4.198837394002331,
+            "heading": 3.04987623940461,
+            "angularVelocity": 0.6880303179693488,
+            "velocityX": 3.5097039981224545,
+            "velocityY": -0.6196572405676055,
+            "timestamp": 0.429753642425588
+        },
+        {
+            "x": 3.524112842751923,
+            "y": 4.1406587673803905,
+            "heading": 3.0498762636795202,
+            "angularVelocity": 2.824282081012085e-7,
+            "velocityX": 3.7218903223232087,
+            "velocityY": -0.6768834615754469,
+            "timestamp": 0.5157043709107056
+        },
+        {
+            "x": 3.844012025527027,
+            "y": 4.082480131024319,
+            "heading": 3.0498762879543886,
+            "angularVelocity": 2.824277225814376e-7,
+            "velocityX": 3.7218903017266984,
+            "velocityY": -0.6768835748279012,
+            "timestamp": 0.6016550993958233
+        },
+        {
+            "x": 4.163911208302122,
+            "y": 4.0243014946682,
+            "heading": 3.049876312229257,
+            "angularVelocity": 2.824277213255298e-7,
+            "velocityX": 3.721890301726596,
+            "velocityY": -0.6768835748284626,
+            "timestamp": 0.6876058278809409
+        },
+        {
+            "x": 4.483810391077216,
+            "y": 3.9661228583120804,
+            "heading": 3.0498763365041253,
+            "angularVelocity": 2.8242772165714766e-7,
+            "velocityX": 3.7218903017265963,
+            "velocityY": -0.6768835748284627,
+            "timestamp": 0.7735565563660586
+        },
+        {
+            "x": 4.80370957385231,
+            "y": 3.9079442219559613,
+            "heading": 3.0498763607789936,
+            "angularVelocity": 2.8242772140625194e-7,
+            "velocityX": 3.7218903017265963,
+            "velocityY": -0.6768835748284627,
+            "timestamp": 0.8595072848511762
+        },
+        {
+            "x": 5.123608756627404,
+            "y": 3.8497655855998416,
+            "heading": 3.049876385053862,
+            "angularVelocity": 2.824277226048516e-7,
+            "velocityX": 3.7218903017265963,
+            "velocityY": -0.6768835748284628,
+            "timestamp": 0.9454580133362939
+        },
+        {
+            "x": 5.443507939402499,
+            "y": 3.7915869492437224,
+            "heading": 3.0498764093287303,
+            "angularVelocity": 2.8242772163119933e-7,
+            "velocityX": 3.7218903017265963,
+            "velocityY": -0.6768835748284626,
+            "timestamp": 1.0314087418214115
+        },
+        {
+            "x": 5.763407122177593,
+            "y": 3.733408312887603,
+            "heading": 3.0498764336035986,
+            "angularVelocity": 2.824277219712686e-7,
+            "velocityX": 3.7218903017265963,
+            "velocityY": -0.6768835748284628,
+            "timestamp": 1.117359470306529
+        },
+        {
+            "x": 6.083306304952576,
+            "y": 3.675229676530871,
+            "heading": 3.049876457878467,
+            "angularVelocity": 2.8242772161077964e-7,
+            "velocityX": 3.7218903017253,
+            "velocityY": -0.6768835748355908,
+            "timestamp": 1.2033101987916466
+        },
+        {
+            "x": 6.403205465262658,
+            "y": 3.6170509166494913,
+            "heading": 3.0498764821533357,
+            "angularVelocity": 2.8242772554820356e-7,
+            "velocityX": 3.721890040355771,
+            "velocityY": -0.6768850119920853,
+            "timestamp": 1.2892609272767641
+        },
+        {
+            "x": 6.718573965768454,
+            "y": 3.5379121175247272,
+            "heading": 3.0498765068050417,
+            "angularVelocity": 2.868120651983924e-7,
+            "velocityX": 3.6691777494404794,
+            "velocityY": -0.9207461125646453,
+            "timestamp": 1.3752116557618816
+        },
+        {
+            "x": 7.013358117396929,
+            "y": 3.4007168540694233,
+            "heading": 3.0498765454511387,
+            "angularVelocity": 4.49630829762963e-7,
+            "velocityX": 3.4296876457482686,
+            "velocityY": -1.5962082680784608,
+            "timestamp": 1.4611623842469992
+        },
+        {
+            "x": 7.268807462845475,
+            "y": 3.2166766255285517,
+            "heading": 3.0757384072562206,
+            "angularVelocity": 0.30089171157615124,
+            "velocityX": 2.9720439832312224,
+            "velocityY": -2.1412294204433464,
+            "timestamp": 1.5471131127321167
+        },
+        {
+            "x": 7.46355872560419,
+            "y": 3.0231641785099956,
+            "heading": 3.117732513124605,
+            "angularVelocity": 0.48858347809886377,
+            "velocityX": 2.2658477268453225,
+            "velocityY": -2.2514346350428145,
+            "timestamp": 1.6330638412172342
+        },
+        {
+            "x": 7.596796563191941,
+            "y": 2.830012972060279,
+            "heading": 3.1482585374873016,
+            "angularVelocity": 0.3551572499816586,
+            "velocityX": 1.5501653090794745,
+            "velocityY": -2.2472317553790035,
+            "timestamp": 1.7190145697023518
+        },
+        {
+            "x": 7.66884686477411,
+            "y": 2.640707793733853,
+            "heading": 3.1573927530357144,
+            "angularVelocity": 0.10627269494275392,
+            "velocityX": 0.8382744725035164,
+            "velocityY": -2.202484861535554,
+            "timestamp": 1.8049652981874693
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -0.2023572498136116,
+            "velocityX": 0.12976196272503107,
+            "velocityY": -2.1373616835098828,
+            "timestamp": 1.8909160266725868
+        },
+        {
+            "x": 7.6422705837740335,
+            "y": 2.299493905398916,
+            "heading": 3.1013004562411446,
+            "angularVelocity": -0.5083194372878264,
+            "velocityX": -0.49557678882949796,
+            "velocityY": -2.068846337723125,
+            "timestamp": 1.9670483578801472
+        },
+        {
+            "x": 7.556907286255345,
+            "y": 2.147926635640613,
+            "heading": 3.0424682290459026,
+            "angularVelocity": -0.7727627180470553,
+            "velocityX": -1.121248964332173,
+            "velocityY": -1.9908397306931678,
+            "timestamp": 2.043180689087708
+        },
+        {
+            "x": 7.423923039902345,
+            "y": 2.003393138237165,
+            "heading": 2.9682494555138628,
+            "angularVelocity": -0.974865373945302,
+            "velocityX": -1.7467512716828084,
+            "velocityY": -1.8984509617786702,
+            "timestamp": 2.119313020295268
+        },
+        {
+            "x": 7.2434803504733996,
+            "y": 1.8677477086842909,
+            "heading": 2.8865548866835007,
+            "angularVelocity": -1.07306012484416,
+            "velocityX": -2.3701190619920354,
+            "velocityY": -1.7817059769660042,
+            "timestamp": 2.1954453515028285
+        },
+        {
+            "x": 7.01645692191088,
+            "y": 1.7447501516568382,
+            "heading": 2.813573416858664,
+            "angularVelocity": -0.9586133600147891,
+            "velocityX": -2.9819581899254453,
+            "velocityY": -1.6155758673946832,
+            "timestamp": 2.271577682710389
+        },
+        {
+            "x": 6.750046151807222,
+            "y": 1.6436496548008426,
+            "heading": 2.8060686181878967,
+            "angularVelocity": -0.09857571089344144,
+            "velocityX": -3.499311867613085,
+            "velocityY": -1.327957455819293,
+            "timestamp": 2.347710013917949
+        },
+        {
+            "x": 6.467461314020777,
+            "y": 1.5880425214231262,
+            "heading": 2.8060685831707377,
+            "angularVelocity": -4.5995122509551036e-7,
+            "velocityX": -3.711758635316633,
+            "velocityY": -0.7304010332495747,
+            "timestamp": 2.4238423451255096
+        },
+        {
+            "x": 6.17955680980973,
+            "y": 1.580470049320341,
+            "heading": 2.806068554276105,
+            "angularVelocity": -3.7953169924465946e-7,
+            "velocityX": -3.7816325816443177,
+            "velocityY": -0.09946460304917487,
+            "timestamp": 2.49997467633307
+        },
+        {
+            "x": 5.8949085342429335,
+            "y": 1.5924545384965154,
+            "heading": 2.7975026231592013,
+            "angularVelocity": -0.11251371107452125,
+            "velocityX": -3.738861940149357,
+            "velocityY": 0.1574165533364508,
+            "timestamp": 2.5761070075406303
+        },
+        {
+            "x": 5.65482422655778,
+            "y": 1.6016450496652885,
+            "heading": 2.72071509082511,
+            "angularVelocity": -1.008606082543986,
+            "velocityX": -3.153513151076552,
+            "velocityY": 0.12071758506519875,
+            "timestamp": 2.6522393387481906
+        },
+        {
+            "x": 5.463026554407694,
+            "y": 1.608988353775347,
+            "heading": 2.6410025465355518,
+            "angularVelocity": -1.0470261848719398,
+            "velocityX": -2.5192670329138944,
+            "velocityY": 0.09645447595750602,
+            "timestamp": 2.728371669955751
+        },
+        {
+            "x": 5.319399038502622,
+            "y": 1.614493352033255,
+            "heading": 2.573619865722977,
+            "angularVelocity": -0.8850731317940332,
+            "velocityX": -1.8865508730252805,
+            "velocityY": 0.07230828441193854,
+            "timestamp": 2.8045040011633113
+        },
+        {
+            "x": 5.223767247762021,
+            "y": 1.6181637826708264,
+            "heading": 2.525354405173392,
+            "angularVelocity": -0.6339679842201956,
+            "velocityX": -1.2561258695714954,
+            "velocityY": 0.04821119463109347,
+            "timestamp": 2.8806363323708717
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.33303072126212585,
+            "velocityX": -0.6274239472819125,
+            "velocityY": 0.02411875874607297,
+            "timestamp": 2.956768663578432
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": -1.0690007247060788e-32,
+            "velocityY": 0,
+            "timestamp": 3.0329009947859924
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S3.1.traj
new file mode 100644
index 00000000..fda2de80
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S3.1.traj	
@@ -0,0 +1,400 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.35465792702043,
+            "y": 4.32401597789768,
+            "heading": 2.7661167341638526,
+            "angularVelocity": 0.1817092285961366,
+            "velocityX": 0.6162445601306747,
+            "velocityY": -0.4057043338633825,
+            "timestamp": 0.08869518784691614
+        },
+        {
+            "x": 2.464008141320373,
+            "y": 4.252030892508781,
+            "heading": 2.796228115996352,
+            "angularVelocity": 0.3394928469453156,
+            "velocityX": 1.2328765173672827,
+            "velocityY": -0.8116007997315744,
+            "timestamp": 0.1773903756938323
+        },
+        {
+            "x": 2.6280987448252096,
+            "y": 4.144022899680214,
+            "heading": 2.836753181719006,
+            "angularVelocity": 0.4569026427070467,
+            "velocityX": 1.8500508030723093,
+            "velocityY": -1.2177435490072333,
+            "timestamp": 0.2660855635407484
+        },
+        {
+            "x": 2.8469891593833587,
+            "y": 3.9999743075055525,
+            "heading": 2.8803143946879453,
+            "angularVelocity": 0.49113389380407046,
+            "velocityX": 2.467895044497164,
+            "velocityY": -1.624085766899582,
+            "timestamp": 0.3547807513876646
+        },
+        {
+            "x": 3.12048957146799,
+            "y": 3.8200884895179508,
+            "heading": 2.9023929043889884,
+            "angularVelocity": 0.248925677221065,
+            "velocityX": 3.083599220249474,
+            "velocityY": -2.028135035894805,
+            "timestamp": 0.44347593923458073
+        },
+        {
+            "x": 3.4006775749010125,
+            "y": 3.6354966761760505,
+            "heading": 2.9023929178552135,
+            "angularVelocity": 1.5182588146629277e-7,
+            "velocityX": 3.1589989291934804,
+            "velocityY": -2.081193104416185,
+            "timestamp": 0.5321711270814968
+        },
+        {
+            "x": 3.680865577691327,
+            "y": 3.450904861858589,
+            "heading": 2.9023929313214296,
+            "angularVelocity": 1.518257825170986e-7,
+            "velocityX": 3.1589989219472225,
+            "velocityY": -2.081193115415224,
+            "timestamp": 0.6208663149284129
+        },
+        {
+            "x": 3.961053580481639,
+            "y": 3.2663130475411224,
+            "heading": 2.902392944787646,
+            "angularVelocity": 1.5182578291151429e-7,
+            "velocityX": 3.1589989219471897,
+            "velocityY": -2.081193115415273,
+            "timestamp": 0.709561502775329
+        },
+        {
+            "x": 4.24124158327195,
+            "y": 3.0817212332236563,
+            "heading": 2.9023929582538623,
+            "angularVelocity": 1.518257833819606e-7,
+            "velocityX": 3.1589989219471897,
+            "velocityY": -2.0811931154152736,
+            "timestamp": 0.7982566906222451
+        },
+        {
+            "x": 4.521429586062262,
+            "y": 2.89712941890619,
+            "heading": 2.9023929717200785,
+            "angularVelocity": 1.5182578356012397e-7,
+            "velocityX": 3.1589989219471892,
+            "velocityY": -2.081193115415273,
+            "timestamp": 0.8869518784691612
+        },
+        {
+            "x": 4.801617588852573,
+            "y": 2.7125376045887237,
+            "heading": 2.902392985186295,
+            "angularVelocity": 1.518257838228889e-7,
+            "velocityX": 3.15899892194719,
+            "velocityY": -2.0811931154152736,
+            "timestamp": 0.9756470663160773
+        },
+        {
+            "x": 5.081805591642885,
+            "y": 2.5279457902712577,
+            "heading": 2.902392998652511,
+            "angularVelocity": 1.5182578286793036e-7,
+            "velocityX": 3.1589989219471897,
+            "velocityY": -2.081193115415273,
+            "timestamp": 1.0643422541629934
+        },
+        {
+            "x": 5.361993594433196,
+            "y": 2.343353975953791,
+            "heading": 2.9023930121187274,
+            "angularVelocity": 1.5182578369194972e-7,
+            "velocityX": 3.1589989219471897,
+            "velocityY": -2.0811931154152736,
+            "timestamp": 1.1530374420099097
+        },
+        {
+            "x": 5.642181597223507,
+            "y": 2.158762161636325,
+            "heading": 2.902393025584944,
+            "angularVelocity": 1.5182578337777967e-7,
+            "velocityX": 3.1589989219471897,
+            "velocityY": -2.081193115415273,
+            "timestamp": 1.2417326298568259
+        },
+        {
+            "x": 5.922369600013819,
+            "y": 1.9741703473188588,
+            "heading": 2.90239303905116,
+            "angularVelocity": 1.5182578259222556e-7,
+            "velocityX": 3.15899892194719,
+            "velocityY": -2.081193115415273,
+            "timestamp": 1.330427817703742
+        },
+        {
+            "x": 6.202557602804131,
+            "y": 1.7895785330013922,
+            "heading": 2.9023930525173767,
+            "angularVelocity": 1.5182578252273246e-7,
+            "velocityX": 3.1589989219471892,
+            "velocityY": -2.0811931154152736,
+            "timestamp": 1.4191230055506583
+        },
+        {
+            "x": 6.482745605594443,
+            "y": 1.604986718683926,
+            "heading": 2.902393065983593,
+            "angularVelocity": 1.5182578277552693e-7,
+            "velocityX": 3.1589989219471897,
+            "velocityY": -2.081193115415273,
+            "timestamp": 1.5078181933975745
+        },
+        {
+            "x": 6.762933608384558,
+            "y": 1.4203949043661621,
+            "heading": 2.902393079449809,
+            "angularVelocity": 1.5182578233984683e-7,
+            "velocityX": 3.1589989219449803,
+            "velocityY": -2.081193115418628,
+            "timestamp": 1.5965133812444907
+        },
+        {
+            "x": 7.043121567747365,
+            "y": 1.2358030241312514,
+            "heading": 2.902393092916261,
+            "angularVelocity": 1.5182844252968712e-7,
+            "velocityX": 3.158998432320794,
+            "velocityY": -2.08119385860604,
+            "timestamp": 1.685208569091407
+        },
+        {
+            "x": 7.279459556862412,
+            "y": 1.0742756485961233,
+            "heading": 2.998760727595811,
+            "angularVelocity": 1.0865035298857004,
+            "velocityX": 2.6646089247023874,
+            "velocityY": -1.8211515129086453,
+            "timestamp": 1.7739037569383231
+        },
+        {
+            "x": 7.461316655805292,
+            "y": 0.9486305467963247,
+            "heading": 3.0706677283735453,
+            "angularVelocity": 0.8107204294086697,
+            "velocityX": 2.0503603787023565,
+            "velocityY": -1.416594348011941,
+            "timestamp": 1.8625989447852394
+        },
+        {
+            "x": 7.588692434846267,
+            "y": 0.8588692195479983,
+            "heading": 3.1181671121508234,
+            "angularVelocity": 0.5355350716350028,
+            "velocityX": 1.4361069876848378,
+            "velocityY": -1.0120202620603307,
+            "timestamp": 1.9512941326321556
+        },
+        {
+            "x": 7.661586847053616,
+            "y": 0.8049924095541465,
+            "heading": 3.141279756163227,
+            "angularVelocity": 0.26058509569081495,
+            "velocityX": 0.8218530675323843,
+            "velocityY": -0.6074378024526055,
+            "timestamp": 2.0399893204790716
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": -0.01442869894402112,
+            "velocityX": 0.20760036021529943,
+            "velocityY": -0.20285665988103574,
+            "timestamp": 2.1286845083259878
+        },
+        {
+            "x": 7.650032668413058,
+            "y": 0.8012856501840878,
+            "heading": 3.117704293378248,
+            "angularVelocity": -0.2703970639421689,
+            "velocityX": -0.3634367195783461,
+            "velocityY": 0.17325299134110608,
+            "timestamp": 2.211139939966771
+        },
+        {
+            "x": 7.572958138860234,
+            "y": 0.846621328160437,
+            "heading": 3.075672962840878,
+            "angularVelocity": -0.509746049483795,
+            "velocityX": -0.9347416903788446,
+            "velocityY": 0.5498203947782849,
+            "timestamp": 2.293595371607554
+        },
+        {
+            "x": 7.448745482136049,
+            "y": 0.9230647936190245,
+            "heading": 3.016123238078663,
+            "angularVelocity": -0.7222049970175785,
+            "velocityX": -1.5064217632783534,
+            "velocityY": 0.927088294093387,
+            "timestamp": 2.376050803248337
+        },
+        {
+            "x": 7.277351379823213,
+            "y": 1.0307130669975626,
+            "heading": 2.9432415686566307,
+            "angularVelocity": -0.8838916730136303,
+            "velocityX": -2.0786271917114503,
+            "velocityY": 1.305532834362055,
+            "timestamp": 2.45850623488912
+        },
+        {
+            "x": 7.058753992164297,
+            "y": 1.1697461046151478,
+            "heading": 2.8675628117721894,
+            "angularVelocity": -0.9178140891207157,
+            "velocityX": -2.6510974875643654,
+            "velocityY": 1.6861598423652984,
+            "timestamp": 2.540961666529903
+        },
+        {
+            "x": 6.796121093379507,
+            "y": 1.3380331209180303,
+            "heading": 2.867562732679127,
+            "angularVelocity": -9.592219802669244e-7,
+            "velocityX": -3.1851497658632115,
+            "velocityY": 2.0409451864375003,
+            "timestamp": 2.623417098170686
+        },
+        {
+            "x": 6.5354030679592325,
+            "y": 1.509271805879804,
+            "heading": 2.8675627155127748,
+            "angularVelocity": -2.0818946417191608e-7,
+            "velocityX": -3.161926633967453,
+            "velocityY": 2.076742326785391,
+            "timestamp": 2.705872529811469
+        },
+        {
+            "x": 6.274685053353285,
+            "y": 1.680510507306839,
+            "heading": 2.867562698346423,
+            "angularVelocity": -2.0818946436511562e-7,
+            "velocityX": -3.161926502813852,
+            "velocityY": 2.0767425264721937,
+            "timestamp": 2.7883279614522523
+        },
+        {
+            "x": 6.013967038747397,
+            "y": 1.851749208733966,
+            "heading": 2.8675626811800705,
+            "angularVelocity": -2.0818946408004615e-7,
+            "velocityX": -3.1619265028131216,
+            "velocityY": 2.076742526473307,
+            "timestamp": 2.8707833930930353
+        },
+        {
+            "x": 5.75324902414151,
+            "y": 2.022987910161093,
+            "heading": 2.8675626640137186,
+            "angularVelocity": -2.081894642016249e-7,
+            "velocityX": -3.1619265028131216,
+            "velocityY": 2.076742526473306,
+            "timestamp": 2.9532388247338184
+        },
+        {
+            "x": 5.492531009535623,
+            "y": 2.194226611588219,
+            "heading": 2.8675626468473663,
+            "angularVelocity": -2.0818946425512402e-7,
+            "velocityX": -3.1619265028131216,
+            "velocityY": 2.076742526473306,
+            "timestamp": 3.0356942563746014
+        },
+        {
+            "x": 5.231812994929702,
+            "y": 2.3654653130152945,
+            "heading": 2.8675626296810144,
+            "angularVelocity": -2.0818946468474427e-7,
+            "velocityX": -3.1619265028135324,
+            "velocityY": 2.0767425264726813,
+            "timestamp": 3.1181496880153845
+        },
+        {
+            "x": 4.971094974250145,
+            "y": 2.536704005194925,
+            "heading": 2.8675626125145612,
+            "angularVelocity": -2.0819068604958381e-7,
+            "velocityX": -3.16192657647314,
+            "velocityY": 2.076742414321854,
+            "timestamp": 3.2006051196561676
+        },
+        {
+            "x": 4.734733232154558,
+            "y": 2.6911196483021613,
+            "heading": 2.78339324120731,
+            "angularVelocity": -1.020786255463861,
+            "velocityX": -2.8665393824544814,
+            "velocityY": 1.8727164485651855,
+            "timestamp": 3.2830605512969506
+        },
+        {
+            "x": 4.545913318554367,
+            "y": 2.8146834617063132,
+            "heading": 2.703884624892798,
+            "angularVelocity": -0.9642617197238292,
+            "velocityX": -2.289963315246283,
+            "velocityY": 1.4985527447416418,
+            "timestamp": 3.3655159829377337
+        },
+        {
+            "x": 4.404411416841804,
+            "y": 2.9073475116185867,
+            "heading": 2.6403705813576916,
+            "angularVelocity": -0.7702833187728019,
+            "velocityX": -1.7161016429944431,
+            "velocityY": 1.1238077112489557,
+            "timestamp": 3.4479714145785167
+        },
+        {
+            "x": 4.3101255761487,
+            "y": 2.9691175811007473,
+            "heading": 2.5964788856355536,
+            "angularVelocity": -0.5323081190497229,
+            "velocityX": -1.1434764067922174,
+            "velocityY": 0.7491328133634895,
+            "timestamp": 3.5304268462193
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -0.27261861575697227,
+            "velocityX": -0.5715278570610401,
+            "velocityY": 0.37453468236989096,
+            "timestamp": 3.612882277860083
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": -3.9375732354803004e-35,
+            "velocityY": 0,
+            "timestamp": 3.695337709500866
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S3.traj
new file mode 100644
index 00000000..fda2de80
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S3.traj	
@@ -0,0 +1,400 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.35465792702043,
+            "y": 4.32401597789768,
+            "heading": 2.7661167341638526,
+            "angularVelocity": 0.1817092285961366,
+            "velocityX": 0.6162445601306747,
+            "velocityY": -0.4057043338633825,
+            "timestamp": 0.08869518784691614
+        },
+        {
+            "x": 2.464008141320373,
+            "y": 4.252030892508781,
+            "heading": 2.796228115996352,
+            "angularVelocity": 0.3394928469453156,
+            "velocityX": 1.2328765173672827,
+            "velocityY": -0.8116007997315744,
+            "timestamp": 0.1773903756938323
+        },
+        {
+            "x": 2.6280987448252096,
+            "y": 4.144022899680214,
+            "heading": 2.836753181719006,
+            "angularVelocity": 0.4569026427070467,
+            "velocityX": 1.8500508030723093,
+            "velocityY": -1.2177435490072333,
+            "timestamp": 0.2660855635407484
+        },
+        {
+            "x": 2.8469891593833587,
+            "y": 3.9999743075055525,
+            "heading": 2.8803143946879453,
+            "angularVelocity": 0.49113389380407046,
+            "velocityX": 2.467895044497164,
+            "velocityY": -1.624085766899582,
+            "timestamp": 0.3547807513876646
+        },
+        {
+            "x": 3.12048957146799,
+            "y": 3.8200884895179508,
+            "heading": 2.9023929043889884,
+            "angularVelocity": 0.248925677221065,
+            "velocityX": 3.083599220249474,
+            "velocityY": -2.028135035894805,
+            "timestamp": 0.44347593923458073
+        },
+        {
+            "x": 3.4006775749010125,
+            "y": 3.6354966761760505,
+            "heading": 2.9023929178552135,
+            "angularVelocity": 1.5182588146629277e-7,
+            "velocityX": 3.1589989291934804,
+            "velocityY": -2.081193104416185,
+            "timestamp": 0.5321711270814968
+        },
+        {
+            "x": 3.680865577691327,
+            "y": 3.450904861858589,
+            "heading": 2.9023929313214296,
+            "angularVelocity": 1.518257825170986e-7,
+            "velocityX": 3.1589989219472225,
+            "velocityY": -2.081193115415224,
+            "timestamp": 0.6208663149284129
+        },
+        {
+            "x": 3.961053580481639,
+            "y": 3.2663130475411224,
+            "heading": 2.902392944787646,
+            "angularVelocity": 1.5182578291151429e-7,
+            "velocityX": 3.1589989219471897,
+            "velocityY": -2.081193115415273,
+            "timestamp": 0.709561502775329
+        },
+        {
+            "x": 4.24124158327195,
+            "y": 3.0817212332236563,
+            "heading": 2.9023929582538623,
+            "angularVelocity": 1.518257833819606e-7,
+            "velocityX": 3.1589989219471897,
+            "velocityY": -2.0811931154152736,
+            "timestamp": 0.7982566906222451
+        },
+        {
+            "x": 4.521429586062262,
+            "y": 2.89712941890619,
+            "heading": 2.9023929717200785,
+            "angularVelocity": 1.5182578356012397e-7,
+            "velocityX": 3.1589989219471892,
+            "velocityY": -2.081193115415273,
+            "timestamp": 0.8869518784691612
+        },
+        {
+            "x": 4.801617588852573,
+            "y": 2.7125376045887237,
+            "heading": 2.902392985186295,
+            "angularVelocity": 1.518257838228889e-7,
+            "velocityX": 3.15899892194719,
+            "velocityY": -2.0811931154152736,
+            "timestamp": 0.9756470663160773
+        },
+        {
+            "x": 5.081805591642885,
+            "y": 2.5279457902712577,
+            "heading": 2.902392998652511,
+            "angularVelocity": 1.5182578286793036e-7,
+            "velocityX": 3.1589989219471897,
+            "velocityY": -2.081193115415273,
+            "timestamp": 1.0643422541629934
+        },
+        {
+            "x": 5.361993594433196,
+            "y": 2.343353975953791,
+            "heading": 2.9023930121187274,
+            "angularVelocity": 1.5182578369194972e-7,
+            "velocityX": 3.1589989219471897,
+            "velocityY": -2.0811931154152736,
+            "timestamp": 1.1530374420099097
+        },
+        {
+            "x": 5.642181597223507,
+            "y": 2.158762161636325,
+            "heading": 2.902393025584944,
+            "angularVelocity": 1.5182578337777967e-7,
+            "velocityX": 3.1589989219471897,
+            "velocityY": -2.081193115415273,
+            "timestamp": 1.2417326298568259
+        },
+        {
+            "x": 5.922369600013819,
+            "y": 1.9741703473188588,
+            "heading": 2.90239303905116,
+            "angularVelocity": 1.5182578259222556e-7,
+            "velocityX": 3.15899892194719,
+            "velocityY": -2.081193115415273,
+            "timestamp": 1.330427817703742
+        },
+        {
+            "x": 6.202557602804131,
+            "y": 1.7895785330013922,
+            "heading": 2.9023930525173767,
+            "angularVelocity": 1.5182578252273246e-7,
+            "velocityX": 3.1589989219471892,
+            "velocityY": -2.0811931154152736,
+            "timestamp": 1.4191230055506583
+        },
+        {
+            "x": 6.482745605594443,
+            "y": 1.604986718683926,
+            "heading": 2.902393065983593,
+            "angularVelocity": 1.5182578277552693e-7,
+            "velocityX": 3.1589989219471897,
+            "velocityY": -2.081193115415273,
+            "timestamp": 1.5078181933975745
+        },
+        {
+            "x": 6.762933608384558,
+            "y": 1.4203949043661621,
+            "heading": 2.902393079449809,
+            "angularVelocity": 1.5182578233984683e-7,
+            "velocityX": 3.1589989219449803,
+            "velocityY": -2.081193115418628,
+            "timestamp": 1.5965133812444907
+        },
+        {
+            "x": 7.043121567747365,
+            "y": 1.2358030241312514,
+            "heading": 2.902393092916261,
+            "angularVelocity": 1.5182844252968712e-7,
+            "velocityX": 3.158998432320794,
+            "velocityY": -2.08119385860604,
+            "timestamp": 1.685208569091407
+        },
+        {
+            "x": 7.279459556862412,
+            "y": 1.0742756485961233,
+            "heading": 2.998760727595811,
+            "angularVelocity": 1.0865035298857004,
+            "velocityX": 2.6646089247023874,
+            "velocityY": -1.8211515129086453,
+            "timestamp": 1.7739037569383231
+        },
+        {
+            "x": 7.461316655805292,
+            "y": 0.9486305467963247,
+            "heading": 3.0706677283735453,
+            "angularVelocity": 0.8107204294086697,
+            "velocityX": 2.0503603787023565,
+            "velocityY": -1.416594348011941,
+            "timestamp": 1.8625989447852394
+        },
+        {
+            "x": 7.588692434846267,
+            "y": 0.8588692195479983,
+            "heading": 3.1181671121508234,
+            "angularVelocity": 0.5355350716350028,
+            "velocityX": 1.4361069876848378,
+            "velocityY": -1.0120202620603307,
+            "timestamp": 1.9512941326321556
+        },
+        {
+            "x": 7.661586847053616,
+            "y": 0.8049924095541465,
+            "heading": 3.141279756163227,
+            "angularVelocity": 0.26058509569081495,
+            "velocityX": 0.8218530675323843,
+            "velocityY": -0.6074378024526055,
+            "timestamp": 2.0399893204790716
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": -0.01442869894402112,
+            "velocityX": 0.20760036021529943,
+            "velocityY": -0.20285665988103574,
+            "timestamp": 2.1286845083259878
+        },
+        {
+            "x": 7.650032668413058,
+            "y": 0.8012856501840878,
+            "heading": 3.117704293378248,
+            "angularVelocity": -0.2703970639421689,
+            "velocityX": -0.3634367195783461,
+            "velocityY": 0.17325299134110608,
+            "timestamp": 2.211139939966771
+        },
+        {
+            "x": 7.572958138860234,
+            "y": 0.846621328160437,
+            "heading": 3.075672962840878,
+            "angularVelocity": -0.509746049483795,
+            "velocityX": -0.9347416903788446,
+            "velocityY": 0.5498203947782849,
+            "timestamp": 2.293595371607554
+        },
+        {
+            "x": 7.448745482136049,
+            "y": 0.9230647936190245,
+            "heading": 3.016123238078663,
+            "angularVelocity": -0.7222049970175785,
+            "velocityX": -1.5064217632783534,
+            "velocityY": 0.927088294093387,
+            "timestamp": 2.376050803248337
+        },
+        {
+            "x": 7.277351379823213,
+            "y": 1.0307130669975626,
+            "heading": 2.9432415686566307,
+            "angularVelocity": -0.8838916730136303,
+            "velocityX": -2.0786271917114503,
+            "velocityY": 1.305532834362055,
+            "timestamp": 2.45850623488912
+        },
+        {
+            "x": 7.058753992164297,
+            "y": 1.1697461046151478,
+            "heading": 2.8675628117721894,
+            "angularVelocity": -0.9178140891207157,
+            "velocityX": -2.6510974875643654,
+            "velocityY": 1.6861598423652984,
+            "timestamp": 2.540961666529903
+        },
+        {
+            "x": 6.796121093379507,
+            "y": 1.3380331209180303,
+            "heading": 2.867562732679127,
+            "angularVelocity": -9.592219802669244e-7,
+            "velocityX": -3.1851497658632115,
+            "velocityY": 2.0409451864375003,
+            "timestamp": 2.623417098170686
+        },
+        {
+            "x": 6.5354030679592325,
+            "y": 1.509271805879804,
+            "heading": 2.8675627155127748,
+            "angularVelocity": -2.0818946417191608e-7,
+            "velocityX": -3.161926633967453,
+            "velocityY": 2.076742326785391,
+            "timestamp": 2.705872529811469
+        },
+        {
+            "x": 6.274685053353285,
+            "y": 1.680510507306839,
+            "heading": 2.867562698346423,
+            "angularVelocity": -2.0818946436511562e-7,
+            "velocityX": -3.161926502813852,
+            "velocityY": 2.0767425264721937,
+            "timestamp": 2.7883279614522523
+        },
+        {
+            "x": 6.013967038747397,
+            "y": 1.851749208733966,
+            "heading": 2.8675626811800705,
+            "angularVelocity": -2.0818946408004615e-7,
+            "velocityX": -3.1619265028131216,
+            "velocityY": 2.076742526473307,
+            "timestamp": 2.8707833930930353
+        },
+        {
+            "x": 5.75324902414151,
+            "y": 2.022987910161093,
+            "heading": 2.8675626640137186,
+            "angularVelocity": -2.081894642016249e-7,
+            "velocityX": -3.1619265028131216,
+            "velocityY": 2.076742526473306,
+            "timestamp": 2.9532388247338184
+        },
+        {
+            "x": 5.492531009535623,
+            "y": 2.194226611588219,
+            "heading": 2.8675626468473663,
+            "angularVelocity": -2.0818946425512402e-7,
+            "velocityX": -3.1619265028131216,
+            "velocityY": 2.076742526473306,
+            "timestamp": 3.0356942563746014
+        },
+        {
+            "x": 5.231812994929702,
+            "y": 2.3654653130152945,
+            "heading": 2.8675626296810144,
+            "angularVelocity": -2.0818946468474427e-7,
+            "velocityX": -3.1619265028135324,
+            "velocityY": 2.0767425264726813,
+            "timestamp": 3.1181496880153845
+        },
+        {
+            "x": 4.971094974250145,
+            "y": 2.536704005194925,
+            "heading": 2.8675626125145612,
+            "angularVelocity": -2.0819068604958381e-7,
+            "velocityX": -3.16192657647314,
+            "velocityY": 2.076742414321854,
+            "timestamp": 3.2006051196561676
+        },
+        {
+            "x": 4.734733232154558,
+            "y": 2.6911196483021613,
+            "heading": 2.78339324120731,
+            "angularVelocity": -1.020786255463861,
+            "velocityX": -2.8665393824544814,
+            "velocityY": 1.8727164485651855,
+            "timestamp": 3.2830605512969506
+        },
+        {
+            "x": 4.545913318554367,
+            "y": 2.8146834617063132,
+            "heading": 2.703884624892798,
+            "angularVelocity": -0.9642617197238292,
+            "velocityX": -2.289963315246283,
+            "velocityY": 1.4985527447416418,
+            "timestamp": 3.3655159829377337
+        },
+        {
+            "x": 4.404411416841804,
+            "y": 2.9073475116185867,
+            "heading": 2.6403705813576916,
+            "angularVelocity": -0.7702833187728019,
+            "velocityX": -1.7161016429944431,
+            "velocityY": 1.1238077112489557,
+            "timestamp": 3.4479714145785167
+        },
+        {
+            "x": 4.3101255761487,
+            "y": 2.9691175811007473,
+            "heading": 2.5964788856355536,
+            "angularVelocity": -0.5323081190497229,
+            "velocityX": -1.1434764067922174,
+            "velocityY": 0.7491328133634895,
+            "timestamp": 3.5304268462193
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -0.27261861575697227,
+            "velocityX": -0.5715278570610401,
+            "velocityY": 0.37453468236989096,
+            "timestamp": 3.612882277860083
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": -3.9375732354803004e-35,
+            "velocityY": 0,
+            "timestamp": 3.695337709500866
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S4.1.traj
new file mode 100644
index 00000000..111b63c2
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S4.1.traj	
@@ -0,0 +1,364 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3565686386495837,
+            "y": 4.327692083019332,
+            "heading": 2.7686204392526563,
+            "angularVelocity": 0.21032675786146612,
+            "velocityX": 0.6389698009998809,
+            "velocityY": -0.36493335842385927,
+            "timestamp": 0.08853100500377892
+        },
+        {
+            "x": 2.4697534077972008,
+            "y": 4.26305693150059,
+            "heading": 2.803412029343583,
+            "angularVelocity": 0.39298763285756855,
+            "velocityX": 1.2784760451187218,
+            "velocityY": -0.7300849178881736,
+            "timestamp": 0.17706201000755784
+        },
+        {
+            "x": 2.639620733022083,
+            "y": 4.166070131606458,
+            "heading": 2.850239105466856,
+            "angularVelocity": 0.5289341979263338,
+            "velocityX": 1.9187325978919179,
+            "velocityY": -1.095512243309488,
+            "timestamp": 0.26559301501133675
+        },
+        {
+            "x": 2.8662525015467275,
+            "y": 4.036714523350174,
+            "heading": 2.9005551954880358,
+            "angularVelocity": 0.5683442768897115,
+            "velocityX": 2.559914105967419,
+            "velocityY": -1.4611333989799566,
+            "timestamp": 0.35412402001511567
+        },
+        {
+            "x": 3.1493645211146295,
+            "y": 3.8752655198055206,
+            "heading": 2.9253298956108997,
+            "angularVelocity": 0.2798420747827705,
+            "velocityX": 3.197885526723865,
+            "velocityY": -1.8236436323948029,
+            "timestamp": 0.4426550250188946
+        },
+        {
+            "x": 3.4401071732873256,
+            "y": 3.709037480706115,
+            "heading": 2.9253299112862043,
+            "angularVelocity": 1.770600526020634e-7,
+            "velocityX": 3.28407716777061,
+            "velocityY": -1.877625122320825,
+            "timestamp": 0.5311860300226735
+        },
+        {
+            "x": 3.730849824609712,
+            "y": 3.5428094401194565,
+            "heading": 2.925329926961498,
+            "angularVelocity": 1.7705993300653618e-7,
+            "velocityX": 3.2840771581659602,
+            "velocityY": -1.87762513912005,
+            "timestamp": 0.6197170350264525
+        },
+        {
+            "x": 4.021592475932095,
+            "y": 3.376581399532791,
+            "heading": 2.9253299426367922,
+            "angularVelocity": 1.7705993370243028e-7,
+            "velocityX": 3.2840771581659167,
+            "velocityY": -1.8776251391201264,
+            "timestamp": 0.7082480400302313
+        },
+        {
+            "x": 4.312335127254477,
+            "y": 3.2103533589461257,
+            "heading": 2.925329958312086,
+            "angularVelocity": 1.7705993363835285e-7,
+            "velocityX": 3.2840771581659163,
+            "velocityY": -1.8776251391201266,
+            "timestamp": 0.7967790450340102
+        },
+        {
+            "x": 4.60307777857686,
+            "y": 3.0441253183594608,
+            "heading": 2.9253299739873797,
+            "angularVelocity": 1.7705993334445259e-7,
+            "velocityX": 3.2840771581659163,
+            "velocityY": -1.8776251391201269,
+            "timestamp": 0.8853100500377891
+        },
+        {
+            "x": 4.893820429899242,
+            "y": 2.877897277772796,
+            "heading": 2.9253299896626737,
+            "angularVelocity": 1.7705993259163212e-7,
+            "velocityX": 3.2840771581659163,
+            "velocityY": -1.877625139120127,
+            "timestamp": 0.9738410550415679
+        },
+        {
+            "x": 5.184563081221626,
+            "y": 2.7116692371861304,
+            "heading": 2.925330005337967,
+            "angularVelocity": 1.7705993390699708e-7,
+            "velocityX": 3.2840771581659163,
+            "velocityY": -1.8776251391201266,
+            "timestamp": 1.0623720600453468
+        },
+        {
+            "x": 5.475305732544008,
+            "y": 2.5454411965994654,
+            "heading": 2.925330021013261,
+            "angularVelocity": 1.770599334284566e-7,
+            "velocityX": 3.2840771581659163,
+            "velocityY": -1.8776251391201269,
+            "timestamp": 1.1509030650491257
+        },
+        {
+            "x": 5.766048383866392,
+            "y": 2.3792131560128005,
+            "heading": 2.925330036688555,
+            "angularVelocity": 1.7705993332802181e-7,
+            "velocityX": 3.2840771581659163,
+            "velocityY": -1.8776251391201264,
+            "timestamp": 1.2394340700529045
+        },
+        {
+            "x": 6.056791035188774,
+            "y": 2.212985115426135,
+            "heading": 2.9253300523638495,
+            "angularVelocity": 1.7705993348595005e-7,
+            "velocityX": 3.2840771581659163,
+            "velocityY": -1.8776251391201266,
+            "timestamp": 1.3279650750566834
+        },
+        {
+            "x": 6.347533686511157,
+            "y": 2.04675707483947,
+            "heading": 2.925330068039143,
+            "angularVelocity": 1.7705993292383098e-7,
+            "velocityX": 3.2840771581659163,
+            "velocityY": -1.8776251391201269,
+            "timestamp": 1.4164960800604622
+        },
+        {
+            "x": 6.638276337833025,
+            "y": 1.8805290342519057,
+            "heading": 2.925330083714437,
+            "angularVelocity": 1.7705993325721192e-7,
+            "velocityX": 3.284077158160109,
+            "velocityY": -1.877625139130284,
+            "timestamp": 1.505027085064241
+        },
+        {
+            "x": 6.929018876242265,
+            "y": 1.7143007961736503,
+            "heading": 2.9253300993897313,
+            "angularVelocity": 1.770599420488751e-7,
+            "velocityX": 3.2840758827580245,
+            "velocityY": -1.8776273698819952,
+            "timestamp": 1.59355809006802
+        },
+        {
+            "x": 7.201522264704821,
+            "y": 1.5196088586386673,
+            "heading": 2.925330126495543,
+            "angularVelocity": 3.061730945539565e-7,
+            "velocityX": 3.078055969780553,
+            "velocityY": -2.1991384546767203,
+            "timestamp": 1.6820890950717988
+        },
+        {
+            "x": 7.413524241952794,
+            "y": 1.3146281632272387,
+            "heading": 3.008225358482996,
+            "angularVelocity": 0.9363412511121879,
+            "velocityX": 2.394663623652733,
+            "velocityY": -2.3153548906699357,
+            "timestamp": 1.7706201000755777
+        },
+        {
+            "x": 7.566471934487395,
+            "y": 1.1372904255598042,
+            "heading": 3.0754099127352705,
+            "angularVelocity": 0.758881639821055,
+            "velocityX": 1.727617262766506,
+            "velocityY": -2.003114475655854,
+            "timestamp": 1.8591511050793565
+        },
+        {
+            "x": 7.66159309798317,
+            "y": 0.9897915834005926,
+            "heading": 3.120981260630991,
+            "angularVelocity": 0.5147501476321013,
+            "velocityX": 1.0744389888232937,
+            "velocityY": -1.6660698944162209,
+            "timestamp": 1.9476821100831354
+        },
+        {
+            "x": 7.6993540657193265,
+            "y": 0.8728983057701601,
+            "heading": 3.1428690494072535,
+            "angularVelocity": 0.2472330318097442,
+            "velocityX": 0.4265281720741691,
+            "velocityY": -1.3203654202890747,
+            "timestamp": 2.0362131150869143
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": -0.03240728383410146,
+            "velocityX": -0.21861341931571387,
+            "velocityY": -0.9702624043010712,
+            "timestamp": 2.124744120090693
+        },
+        {
+            "x": 7.618086573966619,
+            "y": 0.7353968639365192,
+            "heading": 3.1174668267835224,
+            "angularVelocity": -0.28713837638984974,
+            "velocityX": -0.7889577050333905,
+            "velocityY": -0.6575745263915584,
+            "timestamp": 2.203219082231727
+        },
+        {
+            "x": 7.5114563039412285,
+            "y": 0.7085217505748593,
+            "heading": 3.0768465482235205,
+            "angularVelocity": -0.5176208748841893,
+            "velocityX": -1.3587807769024405,
+            "velocityY": -0.34246736320004995,
+            "timestamp": 2.281694044372761
+        },
+        {
+            "x": 7.360182308394819,
+            "y": 0.7066609452418267,
+            "heading": 3.0210994010584384,
+            "angularVelocity": -0.7103813196472814,
+            "velocityX": -1.9276721060984434,
+            "velocityY": -0.02371208959223001,
+            "timestamp": 2.360169006513795
+        },
+        {
+            "x": 7.164421971622539,
+            "y": 0.7302939539488027,
+            "heading": 2.9554195577269637,
+            "angularVelocity": -0.8369528514504796,
+            "velocityX": -2.494557900142165,
+            "velocityY": 0.3011534897526061,
+            "timestamp": 2.4386439686548287
+        },
+        {
+            "x": 6.9246535759772865,
+            "y": 0.7803818201714893,
+            "heading": 2.891069702848338,
+            "angularVelocity": -0.8200049177840428,
+            "velocityX": -3.055348981428448,
+            "velocityY": 0.638265567209492,
+            "timestamp": 2.5171189307958626
+        },
+        {
+            "x": 6.644255242545633,
+            "y": 0.8592678201395815,
+            "heading": 2.8720123676443365,
+            "angularVelocity": -0.24284605795368294,
+            "velocityX": -3.573092943042356,
+            "velocityY": 1.0052378212854063,
+            "timestamp": 2.5955938929368965
+        },
+        {
+            "x": 6.376569654247088,
+            "y": 0.9876185603343286,
+            "heading": 2.872012347067197,
+            "angularVelocity": -2.622127987829542e-7,
+            "velocityX": -3.411095475490091,
+            "velocityY": 1.635562945084242,
+            "timestamp": 2.6740688550779304
+        },
+        {
+            "x": 6.113906618540549,
+            "y": 1.125957335049975,
+            "heading": 2.872012327384056,
+            "angularVelocity": -2.5082064696072804e-7,
+            "velocityX": -3.347093500147104,
+            "velocityY": 1.762839648995542,
+            "timestamp": 2.7525438172189642
+        },
+        {
+            "x": 5.851058985303004,
+            "y": 1.2639450359763313,
+            "heading": 2.8720122952205234,
+            "angularVelocity": -4.098572586075927e-7,
+            "velocityX": -3.3494458113298133,
+            "velocityY": 1.758365944520708,
+            "timestamp": 2.831018779359998
+        },
+        {
+            "x": 5.625853496223335,
+            "y": 1.3827528651043908,
+            "heading": 2.754606631455948,
+            "angularVelocity": -1.4960907346931063,
+            "velocityX": -2.8697750586351334,
+            "velocityY": 1.5139584128060528,
+            "timestamp": 2.909493741501032
+        },
+        {
+            "x": 5.445842564985353,
+            "y": 1.4776937741923677,
+            "heading": 2.6549514330576893,
+            "angularVelocity": -1.2698980117917973,
+            "velocityX": -2.2938645183985233,
+            "velocityY": 1.2098242101391037,
+            "timestamp": 2.987968703642066
+        },
+        {
+            "x": 5.310896371700566,
+            "y": 1.5488618130458742,
+            "heading": 2.5782320494228896,
+            "angularVelocity": -0.9776288072239107,
+            "velocityX": -1.7196082623431492,
+            "velocityY": 0.9068884764238665,
+            "timestamp": 3.0664436657831
+        },
+        {
+            "x": 5.220959019114767,
+            "y": 1.5962913343665528,
+            "heading": 2.5262728715572207,
+            "angularVelocity": -0.6621115378476069,
+            "velocityX": -1.146064300409159,
+            "velocityY": 0.604390496365404,
+            "timestamp": 3.1449186279241337
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.33479304532875703,
+            "velocityX": -0.5729090895764638,
+            "velocityY": 0.30211757975540204,
+            "timestamp": 3.2233935900651676
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": 1.304014234910508e-32,
+            "velocityY": 0,
+            "timestamp": 3.3018685522062015
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S4.traj
new file mode 100644
index 00000000..111b63c2
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S4.traj	
@@ -0,0 +1,364 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3565686386495837,
+            "y": 4.327692083019332,
+            "heading": 2.7686204392526563,
+            "angularVelocity": 0.21032675786146612,
+            "velocityX": 0.6389698009998809,
+            "velocityY": -0.36493335842385927,
+            "timestamp": 0.08853100500377892
+        },
+        {
+            "x": 2.4697534077972008,
+            "y": 4.26305693150059,
+            "heading": 2.803412029343583,
+            "angularVelocity": 0.39298763285756855,
+            "velocityX": 1.2784760451187218,
+            "velocityY": -0.7300849178881736,
+            "timestamp": 0.17706201000755784
+        },
+        {
+            "x": 2.639620733022083,
+            "y": 4.166070131606458,
+            "heading": 2.850239105466856,
+            "angularVelocity": 0.5289341979263338,
+            "velocityX": 1.9187325978919179,
+            "velocityY": -1.095512243309488,
+            "timestamp": 0.26559301501133675
+        },
+        {
+            "x": 2.8662525015467275,
+            "y": 4.036714523350174,
+            "heading": 2.9005551954880358,
+            "angularVelocity": 0.5683442768897115,
+            "velocityX": 2.559914105967419,
+            "velocityY": -1.4611333989799566,
+            "timestamp": 0.35412402001511567
+        },
+        {
+            "x": 3.1493645211146295,
+            "y": 3.8752655198055206,
+            "heading": 2.9253298956108997,
+            "angularVelocity": 0.2798420747827705,
+            "velocityX": 3.197885526723865,
+            "velocityY": -1.8236436323948029,
+            "timestamp": 0.4426550250188946
+        },
+        {
+            "x": 3.4401071732873256,
+            "y": 3.709037480706115,
+            "heading": 2.9253299112862043,
+            "angularVelocity": 1.770600526020634e-7,
+            "velocityX": 3.28407716777061,
+            "velocityY": -1.877625122320825,
+            "timestamp": 0.5311860300226735
+        },
+        {
+            "x": 3.730849824609712,
+            "y": 3.5428094401194565,
+            "heading": 2.925329926961498,
+            "angularVelocity": 1.7705993300653618e-7,
+            "velocityX": 3.2840771581659602,
+            "velocityY": -1.87762513912005,
+            "timestamp": 0.6197170350264525
+        },
+        {
+            "x": 4.021592475932095,
+            "y": 3.376581399532791,
+            "heading": 2.9253299426367922,
+            "angularVelocity": 1.7705993370243028e-7,
+            "velocityX": 3.2840771581659167,
+            "velocityY": -1.8776251391201264,
+            "timestamp": 0.7082480400302313
+        },
+        {
+            "x": 4.312335127254477,
+            "y": 3.2103533589461257,
+            "heading": 2.925329958312086,
+            "angularVelocity": 1.7705993363835285e-7,
+            "velocityX": 3.2840771581659163,
+            "velocityY": -1.8776251391201266,
+            "timestamp": 0.7967790450340102
+        },
+        {
+            "x": 4.60307777857686,
+            "y": 3.0441253183594608,
+            "heading": 2.9253299739873797,
+            "angularVelocity": 1.7705993334445259e-7,
+            "velocityX": 3.2840771581659163,
+            "velocityY": -1.8776251391201269,
+            "timestamp": 0.8853100500377891
+        },
+        {
+            "x": 4.893820429899242,
+            "y": 2.877897277772796,
+            "heading": 2.9253299896626737,
+            "angularVelocity": 1.7705993259163212e-7,
+            "velocityX": 3.2840771581659163,
+            "velocityY": -1.877625139120127,
+            "timestamp": 0.9738410550415679
+        },
+        {
+            "x": 5.184563081221626,
+            "y": 2.7116692371861304,
+            "heading": 2.925330005337967,
+            "angularVelocity": 1.7705993390699708e-7,
+            "velocityX": 3.2840771581659163,
+            "velocityY": -1.8776251391201266,
+            "timestamp": 1.0623720600453468
+        },
+        {
+            "x": 5.475305732544008,
+            "y": 2.5454411965994654,
+            "heading": 2.925330021013261,
+            "angularVelocity": 1.770599334284566e-7,
+            "velocityX": 3.2840771581659163,
+            "velocityY": -1.8776251391201269,
+            "timestamp": 1.1509030650491257
+        },
+        {
+            "x": 5.766048383866392,
+            "y": 2.3792131560128005,
+            "heading": 2.925330036688555,
+            "angularVelocity": 1.7705993332802181e-7,
+            "velocityX": 3.2840771581659163,
+            "velocityY": -1.8776251391201264,
+            "timestamp": 1.2394340700529045
+        },
+        {
+            "x": 6.056791035188774,
+            "y": 2.212985115426135,
+            "heading": 2.9253300523638495,
+            "angularVelocity": 1.7705993348595005e-7,
+            "velocityX": 3.2840771581659163,
+            "velocityY": -1.8776251391201266,
+            "timestamp": 1.3279650750566834
+        },
+        {
+            "x": 6.347533686511157,
+            "y": 2.04675707483947,
+            "heading": 2.925330068039143,
+            "angularVelocity": 1.7705993292383098e-7,
+            "velocityX": 3.2840771581659163,
+            "velocityY": -1.8776251391201269,
+            "timestamp": 1.4164960800604622
+        },
+        {
+            "x": 6.638276337833025,
+            "y": 1.8805290342519057,
+            "heading": 2.925330083714437,
+            "angularVelocity": 1.7705993325721192e-7,
+            "velocityX": 3.284077158160109,
+            "velocityY": -1.877625139130284,
+            "timestamp": 1.505027085064241
+        },
+        {
+            "x": 6.929018876242265,
+            "y": 1.7143007961736503,
+            "heading": 2.9253300993897313,
+            "angularVelocity": 1.770599420488751e-7,
+            "velocityX": 3.2840758827580245,
+            "velocityY": -1.8776273698819952,
+            "timestamp": 1.59355809006802
+        },
+        {
+            "x": 7.201522264704821,
+            "y": 1.5196088586386673,
+            "heading": 2.925330126495543,
+            "angularVelocity": 3.061730945539565e-7,
+            "velocityX": 3.078055969780553,
+            "velocityY": -2.1991384546767203,
+            "timestamp": 1.6820890950717988
+        },
+        {
+            "x": 7.413524241952794,
+            "y": 1.3146281632272387,
+            "heading": 3.008225358482996,
+            "angularVelocity": 0.9363412511121879,
+            "velocityX": 2.394663623652733,
+            "velocityY": -2.3153548906699357,
+            "timestamp": 1.7706201000755777
+        },
+        {
+            "x": 7.566471934487395,
+            "y": 1.1372904255598042,
+            "heading": 3.0754099127352705,
+            "angularVelocity": 0.758881639821055,
+            "velocityX": 1.727617262766506,
+            "velocityY": -2.003114475655854,
+            "timestamp": 1.8591511050793565
+        },
+        {
+            "x": 7.66159309798317,
+            "y": 0.9897915834005926,
+            "heading": 3.120981260630991,
+            "angularVelocity": 0.5147501476321013,
+            "velocityX": 1.0744389888232937,
+            "velocityY": -1.6660698944162209,
+            "timestamp": 1.9476821100831354
+        },
+        {
+            "x": 7.6993540657193265,
+            "y": 0.8728983057701601,
+            "heading": 3.1428690494072535,
+            "angularVelocity": 0.2472330318097442,
+            "velocityX": 0.4265281720741691,
+            "velocityY": -1.3203654202890747,
+            "timestamp": 2.0362131150869143
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": -0.03240728383410146,
+            "velocityX": -0.21861341931571387,
+            "velocityY": -0.9702624043010712,
+            "timestamp": 2.124744120090693
+        },
+        {
+            "x": 7.618086573966619,
+            "y": 0.7353968639365192,
+            "heading": 3.1174668267835224,
+            "angularVelocity": -0.28713837638984974,
+            "velocityX": -0.7889577050333905,
+            "velocityY": -0.6575745263915584,
+            "timestamp": 2.203219082231727
+        },
+        {
+            "x": 7.5114563039412285,
+            "y": 0.7085217505748593,
+            "heading": 3.0768465482235205,
+            "angularVelocity": -0.5176208748841893,
+            "velocityX": -1.3587807769024405,
+            "velocityY": -0.34246736320004995,
+            "timestamp": 2.281694044372761
+        },
+        {
+            "x": 7.360182308394819,
+            "y": 0.7066609452418267,
+            "heading": 3.0210994010584384,
+            "angularVelocity": -0.7103813196472814,
+            "velocityX": -1.9276721060984434,
+            "velocityY": -0.02371208959223001,
+            "timestamp": 2.360169006513795
+        },
+        {
+            "x": 7.164421971622539,
+            "y": 0.7302939539488027,
+            "heading": 2.9554195577269637,
+            "angularVelocity": -0.8369528514504796,
+            "velocityX": -2.494557900142165,
+            "velocityY": 0.3011534897526061,
+            "timestamp": 2.4386439686548287
+        },
+        {
+            "x": 6.9246535759772865,
+            "y": 0.7803818201714893,
+            "heading": 2.891069702848338,
+            "angularVelocity": -0.8200049177840428,
+            "velocityX": -3.055348981428448,
+            "velocityY": 0.638265567209492,
+            "timestamp": 2.5171189307958626
+        },
+        {
+            "x": 6.644255242545633,
+            "y": 0.8592678201395815,
+            "heading": 2.8720123676443365,
+            "angularVelocity": -0.24284605795368294,
+            "velocityX": -3.573092943042356,
+            "velocityY": 1.0052378212854063,
+            "timestamp": 2.5955938929368965
+        },
+        {
+            "x": 6.376569654247088,
+            "y": 0.9876185603343286,
+            "heading": 2.872012347067197,
+            "angularVelocity": -2.622127987829542e-7,
+            "velocityX": -3.411095475490091,
+            "velocityY": 1.635562945084242,
+            "timestamp": 2.6740688550779304
+        },
+        {
+            "x": 6.113906618540549,
+            "y": 1.125957335049975,
+            "heading": 2.872012327384056,
+            "angularVelocity": -2.5082064696072804e-7,
+            "velocityX": -3.347093500147104,
+            "velocityY": 1.762839648995542,
+            "timestamp": 2.7525438172189642
+        },
+        {
+            "x": 5.851058985303004,
+            "y": 1.2639450359763313,
+            "heading": 2.8720122952205234,
+            "angularVelocity": -4.098572586075927e-7,
+            "velocityX": -3.3494458113298133,
+            "velocityY": 1.758365944520708,
+            "timestamp": 2.831018779359998
+        },
+        {
+            "x": 5.625853496223335,
+            "y": 1.3827528651043908,
+            "heading": 2.754606631455948,
+            "angularVelocity": -1.4960907346931063,
+            "velocityX": -2.8697750586351334,
+            "velocityY": 1.5139584128060528,
+            "timestamp": 2.909493741501032
+        },
+        {
+            "x": 5.445842564985353,
+            "y": 1.4776937741923677,
+            "heading": 2.6549514330576893,
+            "angularVelocity": -1.2698980117917973,
+            "velocityX": -2.2938645183985233,
+            "velocityY": 1.2098242101391037,
+            "timestamp": 2.987968703642066
+        },
+        {
+            "x": 5.310896371700566,
+            "y": 1.5488618130458742,
+            "heading": 2.5782320494228896,
+            "angularVelocity": -0.9776288072239107,
+            "velocityX": -1.7196082623431492,
+            "velocityY": 0.9068884764238665,
+            "timestamp": 3.0664436657831
+        },
+        {
+            "x": 5.220959019114767,
+            "y": 1.5962913343665528,
+            "heading": 2.5262728715572207,
+            "angularVelocity": -0.6621115378476069,
+            "velocityX": -1.146064300409159,
+            "velocityY": 0.604390496365404,
+            "timestamp": 3.1449186279241337
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.33479304532875703,
+            "velocityX": -0.5729090895764638,
+            "velocityY": 0.30211757975540204,
+            "timestamp": 3.2233935900651676
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": 1.304014234910508e-32,
+            "velocityY": 0,
+            "timestamp": 3.3018685522062015
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj
new file mode 100644
index 00000000..34dfa28f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 8.35160049146123e-34,
+            "velocityX": 7.119134799605741e-35,
+            "velocityY": 1.2756128975400497e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 2.298001258966314,
+            "y": 4.408001448921345,
+            "heading": 2.7655229314430416,
+            "angularVelocity": 0.204083682327543,
+            "velocityX": -0.026277925124547522,
+            "velocityY": 0.6310864986340488,
+            "timestamp": 0.07606160015345173
+        },
+        {
+            "x": 2.294003795304181,
+            "y": 4.504004095385984,
+            "heading": 2.796583558711994,
+            "angularVelocity": 0.408361475518381,
+            "velocityX": -0.0525556082710341,
+            "velocityY": 1.262169692340901,
+            "timestamp": 0.15212320030690346
+        },
+        {
+            "x": 2.2880075283432446,
+            "y": 4.6480074792263695,
+            "heading": 2.8432070800984706,
+            "angularVelocity": 0.6129705566595356,
+            "velocityX": -0.07883435200994282,
+            "velocityY": 1.8932468361152464,
+            "timestamp": 0.2281848004603552
+        },
+        {
+            "x": 2.2800121406869254,
+            "y": 4.840010950636843,
+            "heading": 2.905427634623498,
+            "angularVelocity": 0.8180284716532374,
+            "velocityX": -0.10511726863738108,
+            "velocityY": 2.5243154367396032,
+            "timestamp": 0.3042464006138069
+        },
+        {
+            "x": 2.2700168989019116,
+            "y": 5.080013713319674,
+            "heading": 2.983284780095673,
+            "angularVelocity": 1.0236064625921717,
+            "velocityX": -0.13140982788751254,
+            "velocityY": 3.1553735682477444,
+            "timestamp": 0.3803080007672587
+        },
+        {
+            "x": 2.262010665765912,
+            "y": 5.27200837395213,
+            "heading": 3.04596409402067,
+            "angularVelocity": 0.8240598909113609,
+            "velocityX": -0.10525985674568247,
+            "velocityY": 2.5241995993394015,
+            "timestamp": 0.45636960092071044
+        },
+        {
+            "x": 2.2560054673554846,
+            "y": 5.416004212856887,
+            "heading": 3.092980839154268,
+            "angularVelocity": 0.6181403630576198,
+            "velocityX": -0.07895177590678841,
+            "velocityY": 1.8931476410468593,
+            "timestamp": 0.5324312010741622
+        },
+        {
+            "x": 2.2520018258057144,
+            "y": 5.512001403135216,
+            "heading": 3.1243269625502275,
+            "angularVelocity": 0.4121149611988094,
+            "velocityX": -0.05263683043340117,
+            "velocityY": 1.2620979585580252,
+            "timestamp": 0.608492801227614
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0.20605716180244424,
+            "velocityX": -0.026318481358210057,
+            "velocityY": 0.6310490019661509,
+            "timestamp": 0.6845544013810657
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -5.838549689746647e-34,
+            "velocityX": 5.793975279736999e-33,
+            "velocityY": -3.8925630164914687e-35,
+            "timestamp": 0.7606160015345175
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.traj
new file mode 100644
index 00000000..34dfa28f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 8.35160049146123e-34,
+            "velocityX": 7.119134799605741e-35,
+            "velocityY": 1.2756128975400497e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 2.298001258966314,
+            "y": 4.408001448921345,
+            "heading": 2.7655229314430416,
+            "angularVelocity": 0.204083682327543,
+            "velocityX": -0.026277925124547522,
+            "velocityY": 0.6310864986340488,
+            "timestamp": 0.07606160015345173
+        },
+        {
+            "x": 2.294003795304181,
+            "y": 4.504004095385984,
+            "heading": 2.796583558711994,
+            "angularVelocity": 0.408361475518381,
+            "velocityX": -0.0525556082710341,
+            "velocityY": 1.262169692340901,
+            "timestamp": 0.15212320030690346
+        },
+        {
+            "x": 2.2880075283432446,
+            "y": 4.6480074792263695,
+            "heading": 2.8432070800984706,
+            "angularVelocity": 0.6129705566595356,
+            "velocityX": -0.07883435200994282,
+            "velocityY": 1.8932468361152464,
+            "timestamp": 0.2281848004603552
+        },
+        {
+            "x": 2.2800121406869254,
+            "y": 4.840010950636843,
+            "heading": 2.905427634623498,
+            "angularVelocity": 0.8180284716532374,
+            "velocityX": -0.10511726863738108,
+            "velocityY": 2.5243154367396032,
+            "timestamp": 0.3042464006138069
+        },
+        {
+            "x": 2.2700168989019116,
+            "y": 5.080013713319674,
+            "heading": 2.983284780095673,
+            "angularVelocity": 1.0236064625921717,
+            "velocityX": -0.13140982788751254,
+            "velocityY": 3.1553735682477444,
+            "timestamp": 0.3803080007672587
+        },
+        {
+            "x": 2.262010665765912,
+            "y": 5.27200837395213,
+            "heading": 3.04596409402067,
+            "angularVelocity": 0.8240598909113609,
+            "velocityX": -0.10525985674568247,
+            "velocityY": 2.5241995993394015,
+            "timestamp": 0.45636960092071044
+        },
+        {
+            "x": 2.2560054673554846,
+            "y": 5.416004212856887,
+            "heading": 3.092980839154268,
+            "angularVelocity": 0.6181403630576198,
+            "velocityX": -0.07895177590678841,
+            "velocityY": 1.8931476410468593,
+            "timestamp": 0.5324312010741622
+        },
+        {
+            "x": 2.2520018258057144,
+            "y": 5.512001403135216,
+            "heading": 3.1243269625502275,
+            "angularVelocity": 0.4121149611988094,
+            "velocityX": -0.05263683043340117,
+            "velocityY": 1.2620979585580252,
+            "timestamp": 0.608492801227614
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0.20605716180244424,
+            "velocityX": -0.026318481358210057,
+            "velocityY": 0.6310490019661509,
+            "timestamp": 0.6845544013810657
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -5.838549689746647e-34,
+            "velocityX": 5.793975279736999e-33,
+            "velocityY": -3.8925630164914687e-35,
+            "timestamp": 0.7606160015345175
+        }
+    ]
+}
\ No newline at end of file

From 794162fb3fc5dabb0f7442b965ef7891b952a98c Mon Sep 17 00:00:00 2001
From: Lewis-Seiden <lewy@seiden.us>
Date: Sun, 18 Feb 2024 15:51:56 -0800
Subject: [PATCH 28/62] added shooter sysid gains

---
 src/main/java/frc/robot/Robot.java            |  6 ++---
 .../subsystems/shooter/ShooterIOReal.java     | 26 +++++++++----------
 .../subsystems/shooter/ShooterSubystem.java   | 21 ++++++++++++---
 3 files changed, 34 insertions(+), 19 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 22d554e6..df89cf79 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -72,7 +72,7 @@ public static enum Target {
   private final CommandXboxControllerSubsystem operator = new CommandXboxControllerSubsystem(1);
 
   private Target currentTarget = Target.SPEAKER;
-  private double flywheelIdleSpeed = 1.0;
+  private double flywheelIdleSpeed = -0.1;
 
   private final SwerveSubsystem swerve =
       new SwerveSubsystem(
@@ -233,7 +233,7 @@ public void robotInit() {
         .and(() -> true)
         .whileTrue(
             Commands.parallel(
-                shooter.runStateCmd(Rotation2d.fromDegrees(75.0), 80.0, 60.0),
+                shooter.runStateCmd(Rotation2d.fromDegrees(60.0), 80.0, 60.0),
                 Commands.waitSeconds(1.0)
                     .andThen(feeder.runVoltageCmd(FeederSubsystem.INDEXING_VOLTAGE))));
     controller
@@ -291,7 +291,7 @@ public void robotInit() {
                 elevator.setExtensionCmd(() -> ElevatorSubsystem.CLIMB_EXTENSION_METERS)));
     operator.leftTrigger().onTrue(Commands.runOnce(() -> currentTarget = Target.SPEAKER));
     operator.leftBumper().onTrue(Commands.runOnce(() -> currentTarget = Target.AMP));
-    operator.a().onTrue(Commands.runOnce(() -> flywheelIdleSpeed = 1.0));
+    operator.a().onTrue(Commands.runOnce(() -> flywheelIdleSpeed = -0.1));
     operator.b().onTrue(Commands.runOnce(() -> flywheelIdleSpeed = 20.0));
     operator.x().onTrue(Commands.runOnce(() -> flywheelIdleSpeed = 20.0));
     operator.y().onTrue(Commands.runOnce(() -> flywheelIdleSpeed = 80.0));
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java
index 9c76c464..60cf31b2 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java
@@ -55,25 +55,26 @@ public ShooterIOReal() {
     pivotConfig.CurrentLimits.StatorCurrentLimit = 40.0;
 
     pivotConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine;
-    pivotConfig.Slot0.kG = 0.0; // TODO: Find using sysid or hand tuning
-    pivotConfig.Slot0.kA = 0.0;
+    pivotConfig.Slot0.kG = 0.47195; // TODO: Find using sysid or hand tuning
+    pivotConfig.Slot0.kV = 7.8333;
+    pivotConfig.Slot0.kA = 0.098325;
     pivotConfig.Slot0.kS = 0.0;
-    pivotConfig.Slot0.kP = 0.0;
-    pivotConfig.Slot0.kD = 0.0;
+    pivotConfig.Slot0.kP = 69.562;
+    pivotConfig.Slot0.kD = 12.212;
 
-    pivotConfig.MotionMagic.MotionMagicAcceleration = 1.0;
+    pivotConfig.MotionMagic.MotionMagicAcceleration = 10.0;
     pivotConfig.MotionMagic.MotionMagicCruiseVelocity = 1.0;
 
     pivotMotor.getConfigurator().apply(pivotConfig);
     pivotMotor.setPosition(
         ShooterSubystem.PIVOT_MIN_ANGLE.getRotations()); // Assume we boot at hard stop
     BaseStatusSignal.setUpdateFrequencyForAll(
-        50.0, pivotVelocity, pivotVoltage, pivotAmps, pivotTempC, pivotRotations);
+        250.0, pivotVelocity, pivotVoltage, pivotAmps, pivotTempC, pivotRotations);
     pivotMotor.optimizeBusUtilization();
 
     var flywheelConfig = new TalonFXConfiguration();
 
-    flywheelConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+    flywheelConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
 
     flywheelConfig.Feedback.SensorToMechanismRatio =
         ShooterSubystem.FLYWHEEL_RATIO; // TODO add in once cad is done
@@ -81,15 +82,14 @@ public ShooterIOReal() {
     flywheelConfig.CurrentLimits.StatorCurrentLimitEnable = true;
     flywheelConfig.CurrentLimits.StatorCurrentLimit = 40.0;
 
-    flywheelConfig.Slot0.GravityType = GravityTypeValue.Arm_Cosine;
-    flywheelConfig.Slot0.kG = 0.0; // TODO: Find using sysid or hand tuning
-    flywheelConfig.Slot0.kA = 0.0;
-    flywheelConfig.Slot0.kS = 0.0;
-    flywheelConfig.Slot0.kP = 0.0;
+    flywheelConfig.Slot0.kA = 0.0051316;
+    flywheelConfig.Slot0.kV = 0.096766;
+    flywheelConfig.Slot0.kS = 0.23777;
+    flywheelConfig.Slot0.kP = 0.057995;
     flywheelConfig.Slot0.kD = 0.0;
 
     flywheelLeftMotor.getConfigurator().apply(flywheelConfig);
-    flywheelConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+    flywheelConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
     flywheelRightMotor.getConfigurator().apply(flywheelConfig);
 
     BaseStatusSignal.setUpdateFrequencyForAll(
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java
index 07feb35e..493c88fb 100644
--- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubystem.java
@@ -4,6 +4,7 @@
 
 import com.ctre.phoenix6.SignalLogger;
 import com.google.common.base.Supplier;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
@@ -90,7 +91,21 @@ public Command runStateCmd(
         () -> {
           Logger.recordOutput("Shooter/Left Velocity Setpoint", left.getAsDouble());
           Logger.recordOutput("Shooter/Right Velocity Setpoint", right.getAsDouble());
+          Logger.recordOutput(
+              "Shooter/Left At Target",
+              MathUtil.isNear(
+                  left.getAsDouble(), inputs.flywheelLeftVelocityRotationsPerSecond, 1.0));
+          Logger.recordOutput(
+              "Shooter/Right At Target",
+              MathUtil.isNear(
+                  right.getAsDouble(), inputs.flywheelRightVelocityRotationsPerSecond, 1.0));
           Logger.recordOutput("Shooter/Rotation Setpoint", rotation.get());
+          Logger.recordOutput(
+              "Shooter/Pivot At Target",
+              MathUtil.isNear(
+                  right.getAsDouble(),
+                  inputs.flywheelRightVelocityRotationsPerSecond,
+                  Units.degreesToRotations(0.5)));
           io.setFlywheelVelocity(left.getAsDouble(), right.getAsDouble());
           io.setPivotSetpoint(rotation.get());
         });
@@ -139,19 +154,19 @@ public Command runPivotSysidCmd() {
             .quasistatic(Direction.kForward)
             .until(() -> inputs.pivotRotation.getDegrees() > 80.0),
         this.runOnce(() -> io.setFlywheelVoltage(0.0, 0.0)),
-        Commands.waitSeconds(1.0),
+        Commands.waitSeconds(0.25),
         // Stop when near horizontal so we avoid hard stop
         pivotRoutine
             .quasistatic(Direction.kReverse)
             .until(() -> inputs.pivotRotation.getDegrees() < 10.0),
         this.runOnce(() -> io.setFlywheelVoltage(0.0, 0.0)),
-        Commands.waitSeconds(1.0),
+        Commands.waitSeconds(0.25),
         // Stop when we get close to vertical so it falls back
         pivotRoutine
             .dynamic(Direction.kForward)
             .until(() -> inputs.pivotRotation.getDegrees() > 80.0),
         this.runOnce(() -> io.setFlywheelVoltage(0.0, 0.0)),
-        Commands.waitSeconds(1.0),
+        Commands.waitSeconds(0.25),
         // Stop when near horizontal so we avoid hard stop
         pivotRoutine
             .dynamic(Direction.kReverse)

From 1e0107338751cc1c0da8781f44d94b7a82126563 Mon Sep 17 00:00:00 2001
From: Stoopipoopy <anotherguywithabeanie@gmail.com>
Date: Sun, 18 Feb 2024 16:38:35 -0800
Subject: [PATCH 29/62] deleted all trajectories made before

---
 ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat |   Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat |   Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat |   Bin 2048 -> 2048 bytes
 .../robot/utils/dynamicauto/ChoreoBuilder.py  |    64 +-
 .../frc/robot/utils/dynamicauto/data.chor     | 24392 ++--------------
 .../deploy/choreo/Amp Side To W1.1.traj       |   121 -
 .../deploy/choreo/Amp Side To W1.traj         |   121 -
 .../dynamicauto/deploy/choreo/New Path.1.traj |   265 -
 .../dynamicauto/deploy/choreo/New Path.traj   |   265 -
 .../deploy/choreo/S1 To C1 To S1.1.traj       |   247 -
 .../deploy/choreo/S1 To C1 To S1.traj         |   247 -
 .../deploy/choreo/S1 To C1 To S2.1.traj       |   265 -
 .../deploy/choreo/S1 To C1 To S2.traj         |   265 -
 .../deploy/choreo/S1 To C2 To S1.1.traj       |   247 -
 .../deploy/choreo/S1 To C2 To S1.traj         |   247 -
 .../deploy/choreo/S1 To C2 To S2.1.traj       |   256 -
 .../deploy/choreo/S1 To C2 To S2.traj         |   256 -
 .../deploy/choreo/S1 To C3 To S1.1.traj       |   301 -
 .../deploy/choreo/S1 To C3 To S1.traj         |   301 -
 .../deploy/choreo/S1 To C3 To S2.1.traj       |   292 -
 .../deploy/choreo/S1 To C3 To S2.traj         |   292 -
 .../deploy/choreo/S1 To C3 To S3.1.traj       |   310 -
 .../deploy/choreo/S1 To C3 To S3.traj         |   310 -
 .../deploy/choreo/S1 To C3 To S4.1.traj       |   319 -
 .../deploy/choreo/S1 To C3 To S4.traj         |   319 -
 .../dynamicauto/deploy/choreo/S1 To W1.1.traj |   130 -
 .../dynamicauto/deploy/choreo/S1 To W1.traj   |   130 -
 .../dynamicauto/deploy/choreo/S1 To W2.1.traj |   148 -
 .../dynamicauto/deploy/choreo/S1 To W2.traj   |   148 -
 .../deploy/choreo/S2 To C1 To S1.1.traj       |   265 -
 .../deploy/choreo/S2 To C1 To S1.traj         |   265 -
 .../deploy/choreo/S2 To C1 To S2.1.traj       |   283 -
 .../deploy/choreo/S2 To C1 To S2.traj         |   283 -
 .../deploy/choreo/S2 To C2 To S1.1.traj       |   256 -
 .../deploy/choreo/S2 To C2 To S1.traj         |   256 -
 .../deploy/choreo/S2 To C2 To S2.1.traj       |   265 -
 .../deploy/choreo/S2 To C2 To S2.traj         |   265 -
 .../deploy/choreo/S2 To C3 To S1.1.traj       |   292 -
 .../deploy/choreo/S2 To C3 To S1.traj         |   292 -
 .../deploy/choreo/S2 To C3 To S2.1.traj       |   283 -
 .../deploy/choreo/S2 To C3 To S2.traj         |   283 -
 .../deploy/choreo/S2 To C3 To S3.1.traj       |   301 -
 .../deploy/choreo/S2 To C3 To S3.traj         |   301 -
 .../deploy/choreo/S2 To C3 To S4.1.traj       |   310 -
 .../deploy/choreo/S2 To C3 To S4.traj         |   310 -
 .../dynamicauto/deploy/choreo/S2 To W1.1.traj |   130 -
 .../dynamicauto/deploy/choreo/S2 To W1.traj   |   130 -
 .../dynamicauto/deploy/choreo/S2 To W2.1.traj |   103 -
 .../dynamicauto/deploy/choreo/S2 To W2.traj   |   103 -
 .../dynamicauto/deploy/choreo/S2 To W3.1.traj |   130 -
 .../dynamicauto/deploy/choreo/S2 To W3.traj   |   130 -
 .../deploy/choreo/S3 To C3 To S1.1.traj       |   310 -
 .../deploy/choreo/S3 To C3 To S1.traj         |   310 -
 .../deploy/choreo/S3 To C3 To S2.1.traj       |   301 -
 .../deploy/choreo/S3 To C3 To S2.traj         |   301 -
 .../deploy/choreo/S3 To C3 To S3.1.traj       |   319 -
 .../deploy/choreo/S3 To C3 To S3.traj         |   319 -
 .../deploy/choreo/S3 To C3 To S4.1.traj       |   328 -
 .../deploy/choreo/S3 To C3 To S4.traj         |   328 -
 .../deploy/choreo/S3 To C4 To S3.1.traj       |   319 -
 .../deploy/choreo/S3 To C4 To S3.traj         |   319 -
 .../deploy/choreo/S3 To C4 To S4.1.traj       |   301 -
 .../deploy/choreo/S3 To C4 To S4.traj         |   301 -
 .../deploy/choreo/S3 To C5 To S3.1.traj       |   355 -
 .../deploy/choreo/S3 To C5 To S3.traj         |   355 -
 .../deploy/choreo/S3 To C5 To S4.1.traj       |   319 -
 .../deploy/choreo/S3 To C5 To S4.traj         |   319 -
 .../dynamicauto/deploy/choreo/S3 To W2.1.traj |   157 -
 .../dynamicauto/deploy/choreo/S3 To W2.traj   |   157 -
 .../dynamicauto/deploy/choreo/S3 To W3.1.traj |   121 -
 .../dynamicauto/deploy/choreo/S3 To W3.traj   |   121 -
 .../deploy/choreo/S4 To C3 To S1.1.traj       |   319 -
 .../deploy/choreo/S4 To C3 To S1.traj         |   319 -
 .../deploy/choreo/S4 To C3 To S2.1.traj       |   310 -
 .../deploy/choreo/S4 To C3 To S2.traj         |   310 -
 .../deploy/choreo/S4 To C3 To S3.1.traj       |   328 -
 .../deploy/choreo/S4 To C3 To S3.traj         |   328 -
 .../deploy/choreo/S4 To C3 To S4.1.traj       |   337 -
 .../deploy/choreo/S4 To C3 To S4.traj         |   337 -
 .../deploy/choreo/S4 To C4 To S3.1.traj       |   301 -
 .../deploy/choreo/S4 To C4 To S3.traj         |   301 -
 .../deploy/choreo/S4 To C4 To S4.1.traj       |   283 -
 .../deploy/choreo/S4 To C4 To S4.traj         |   283 -
 .../deploy/choreo/S4 To C5 To S3.1.traj       |   319 -
 .../deploy/choreo/S4 To C5 To S3.traj         |   319 -
 .../deploy/choreo/S4 To C5 To S4.1.traj       |   283 -
 .../deploy/choreo/S4 To C5 To S4.traj         |   283 -
 .../dynamicauto/deploy/choreo/S4 To W3.1.traj |   166 -
 .../dynamicauto/deploy/choreo/S4 To W3.traj   |   166 -
 .../deploy/choreo/W1 To C1 To S1.1.traj       |   319 -
 .../deploy/choreo/W1 To C1 To S1.traj         |   319 -
 .../deploy/choreo/W1 To C1 To S2.1.traj       |   337 -
 .../deploy/choreo/W1 To C1 To S2.traj         |   337 -
 .../deploy/choreo/W1 To C2 To S1.1.traj       |   319 -
 .../deploy/choreo/W1 To C2 To S1.traj         |   319 -
 .../deploy/choreo/W1 To C2 To S2.1.traj       |   328 -
 .../deploy/choreo/W1 To C2 To S2.traj         |   328 -
 .../deploy/choreo/W1 To C3 To S1.1.traj       |   364 -
 .../deploy/choreo/W1 To C3 To S1.traj         |   364 -
 .../deploy/choreo/W1 To C3 To S2.1.traj       |   355 -
 .../deploy/choreo/W1 To C3 To S2.traj         |   355 -
 .../deploy/choreo/W1 To C3 To S3.1.traj       |   373 -
 .../deploy/choreo/W1 To C3 To S3.traj         |   373 -
 .../deploy/choreo/W1 To C3 To S4.1.traj       |   382 -
 .../deploy/choreo/W1 To C3 To S4.traj         |   382 -
 .../dynamicauto/deploy/choreo/W1 To W2.1.traj |   103 -
 .../dynamicauto/deploy/choreo/W1 To W2.traj   |   103 -
 .../deploy/choreo/W2 To C1 To S1.1.traj       |   310 -
 .../deploy/choreo/W2 To C1 To S1.traj         |   310 -
 .../deploy/choreo/W2 To C1 To S2.1.traj       |   328 -
 .../deploy/choreo/W2 To C1 To S2.traj         |   328 -
 .../deploy/choreo/W2 To C2 To S1.1.traj       |   301 -
 .../deploy/choreo/W2 To C2 To S1.traj         |   301 -
 .../deploy/choreo/W2 To C2 To S2.1.traj       |   310 -
 .../deploy/choreo/W2 To C2 To S2.traj         |   310 -
 .../deploy/choreo/W2 To C3 To S1.1.traj       |   337 -
 .../deploy/choreo/W2 To C3 To S1.traj         |   337 -
 .../deploy/choreo/W2 To C3 To S2.1.traj       |   328 -
 .../deploy/choreo/W2 To C3 To S2.traj         |   328 -
 .../deploy/choreo/W2 To C3 To S3.1.traj       |   346 -
 .../deploy/choreo/W2 To C3 To S3.traj         |   346 -
 .../deploy/choreo/W2 To C3 To S4.1.traj       |   355 -
 .../deploy/choreo/W2 To C3 To S4.traj         |   355 -
 .../deploy/choreo/W2 To C4 To S3.1.traj       |   364 -
 .../deploy/choreo/W2 To C4 To S3.traj         |   364 -
 .../deploy/choreo/W2 To C4 To S4.1.traj       |   346 -
 .../deploy/choreo/W2 To C4 To S4.traj         |   346 -
 .../dynamicauto/deploy/choreo/W2 To W1.1.traj |   103 -
 .../dynamicauto/deploy/choreo/W2 To W1.traj   |   103 -
 .../dynamicauto/deploy/choreo/W2 To W3.1.traj |   103 -
 .../dynamicauto/deploy/choreo/W2 To W3.traj   |   103 -
 .../deploy/choreo/W3 To C1 To S1.1.traj       |   337 -
 .../deploy/choreo/W3 To C1 To S1.traj         |   337 -
 .../deploy/choreo/W3 To C1 To S2.1.traj       |   355 -
 .../deploy/choreo/W3 To C1 To S2.traj         |   355 -
 .../deploy/choreo/W3 To C2 To S1.1.traj       |   328 -
 .../deploy/choreo/W3 To C2 To S1.traj         |   328 -
 .../deploy/choreo/W3 To C2 To S2.1.traj       |   337 -
 .../deploy/choreo/W3 To C2 To S2.traj         |   337 -
 .../deploy/choreo/W3 To C3 To S1.1.traj       |   346 -
 .../deploy/choreo/W3 To C3 To S1.traj         |   346 -
 .../deploy/choreo/W3 To C3 To S2.1.traj       |   337 -
 .../deploy/choreo/W3 To C3 To S2.traj         |   337 -
 .../deploy/choreo/W3 To C3 To S3.1.traj       |   355 -
 .../deploy/choreo/W3 To C3 To S3.traj         |   355 -
 .../deploy/choreo/W3 To C3 To S4.1.traj       |   364 -
 .../deploy/choreo/W3 To C3 To S4.traj         |   364 -
 .../deploy/choreo/W3 To C4 To S3.1.traj       |   364 -
 .../deploy/choreo/W3 To C4 To S3.traj         |   364 -
 .../deploy/choreo/W3 To C4 To S4.1.traj       |   346 -
 .../deploy/choreo/W3 To C4 To S4.traj         |   346 -
 .../deploy/choreo/W3 To C5 To S3.1.traj       |   400 -
 .../deploy/choreo/W3 To C5 To S3.traj         |   400 -
 .../deploy/choreo/W3 To C5 To S4.1.traj       |   364 -
 .../deploy/choreo/W3 To C5 To S4.traj         |   364 -
 .../dynamicauto/deploy/choreo/W3 To W2.1.traj |   103 -
 .../dynamicauto/deploy/choreo/W3 To W2.traj   |   103 -
 157 files changed, 1680 insertions(+), 66260 deletions(-)
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.traj

diff --git a/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat
index 58cf25bfb864839b250fb122dad2f781a946d8c6..86cf1817e71f054658a63fc7a3ae2da55b29376b 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx-;q>`D{fUBBY$q5_oG$>e7BR5_0IVtsyZ`_I

delta 30
jcmZn=Xb{-Yz{qx*LH@Fs&O|{gwo?pBmqkFVMNDh}kH!eT

diff --git a/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat
index d1edf7c053d5cc7de88b0525f853807a5df621ab..e92615e27dbb98fe2103f24951c289c5d4477e5b 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx-;q>`D{fUBBY$q5_oG$>e7BR5_0IVtsyZ`_I

delta 30
jcmZn=Xb{-Yz{qx*LH@Fs&O|{gwo?pBmqkFVMNDh}kH!eT

diff --git a/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat
index 141bbf23eafd1a07bb2e79e2d670a3b140dbea9b..56a574f5e08735170b67330b5accdb5e13d9d168 100644
GIT binary patch
delta 36
ncmZn=Xb{-Yz{q)=Vek1IeFg@BiGt>w#~2Qr&w_FmF|h#v*J291

delta 36
mcmZn=Xb{-Yz{q)$LHx3S4g-V0L_u@TQw$=Pg`k{8Ol$zM2ME3Z

diff --git a/src/main/java/frc/robot/utils/dynamicauto/ChoreoBuilder.py b/src/main/java/frc/robot/utils/dynamicauto/ChoreoBuilder.py
index ff2c8a03..baac91e8 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/ChoreoBuilder.py
+++ b/src/main/java/frc/robot/utils/dynamicauto/ChoreoBuilder.py
@@ -3,27 +3,30 @@
 # Assuming locations is a list of dictionaries where each dictionary represents a location
 # L,M, and R were change to S1, S2, and S4 with an S3 being added.
 locations = [
-    {"name": "Amp Side", "x": 0.71, "y": 6.72, "angle": -2.10, "allowed_destinations": ["W1", "W2", "C1", "C2", "C3"]},
+    {"name": "Amp Side", "x": 0.71, "y": 6.72, "angle": -2.10, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]},
     {"name": "Center", "x": 1.35, "y": 5.56, "angle": 3.14, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]},
-    {"name": "Stage Side", "x": 0.71, "y": 4.36, "angle": 2.10, "allowed_destinations": ["W2", "W3", "C3", "C4", "C5"]},
-    {"name": "W1", "x": 2.3, "y": 6.757, "angle": -2.75, "allowed_destinations": ["W2", "C1", "C2", "C3"]},
-    {"name": "W2", "x": 2.25, "y": 5.56, "angle": 3.14, "allowed_destinations": ["W1", "W3", "C1", "C2", "C3", "C4"]},
-    {"name": "W3", "x": 2.3, "y": 4.36, "angle": 2.75, "allowed_destinations": ["W2", "C1", "C2", "C3", "C4", "C5"]},
-    {"name": "S1", "x": 5.176, "y": 6.63, "angle": -2.88, "allowed_destinations": ["W1", "W2", "C1", "C2", "C3"]},
-    {"name": "S2", "x": 4.263, "y": 5.56, "angle": 3.14, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3"]},
-    {"name": "S3", "x": 4.263, "y": 3, "angle": 2.574, "allowed_destinations": ["W2", "W3", "C3", "C4", "C5"]},
-    {"name": "S4", "x": 5.176, "y": 1.62, "angle": 2.5, "allowed_destinations": ["W3", "C3", "C4", "C5"]},
-]
-
+    {"name": "Stage Side", "x": 0.71, "y": 4.36, "angle": 2.10, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]},
+    {"name": "W1", "x": 2.3, "y": 6.757, "angle": -2.75, "allowed_destinations": [ "W2", "W3", "S1", "S2", "S3", "S4", "C1", "C2", "C3", "C4", "C5"]},
+    {"name": "W2", "x": 2.25, "y": 5.56, "angle": 3.14, "allowed_destinations": ["W1", "W3", "S1", "S2", "S3", "S4", "C1", "C2", "C3", "C4", "C5"]},
+    {"name": "W3", "x": 2.3, "y": 4.36, "angle": 2.75, "allowed_destinations": ["W1", "W2", "S1", "S2", "S3", "S4", "C1", "C2", "C3", "C4", "C5"]},
+    {"name": "S1", "x": 5.176, "y": 6.63, "angle": -2.88, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]},
+    {"name": "S2", "x": 4.263, "y": 5.56, "angle": 3.14, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]},
+    {"name": "S3", "x": 4.263, "y": 3, "angle": 2.574, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]},
+    {"name": "S4", "x": 5.176, "y": 1.62, "angle": 2.5, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]},
 
-midPoints = [
-    {"name": "C1", "x": 7.68, "y": 7.467, "angle": 3.14, "allowed_destinations": ["S1", "S2"]},
-    {"name": "C2", "x": 7.68, "y": 5.797, "angle": 3.14, "allowed_destinations": ["S1", "S2"]},
-    {"name": "C3", "x": 7.68, "y": 4.127, "angle": 3.14, "allowed_destinations": ["S1", "S2", "S3", "S4"]},
-    {"name": "C4", "x": 7.68, "y": 2.457, "angle": 3.14, "allowed_destinations": ["S3", "S4"]},
-    {"name": "C5", "x": 7.68, "y": 0.787, "angle": 3.14, "allowed_destinations": ["S3", "S4"]},
+    {"name": "C1", "x": 7.68, "y": 7.467, "angle": 3.14, "allowed_destinations": ["W1", "W2", "W3", "S1", "S2", "S3", "S4", "C2", "C3", "C4", "C5"]},
+    {"name": "C2", "x": 7.68, "y": 5.797, "angle": 3.14, "allowed_destinations": ["W1", "W2", "W3", "S1", "S2", "S3", "S4", "C1",  "C3", "C4", "C5"]},
+    {"name": "C3", "x": 7.68, "y": 4.127, "angle": 3.14, "allowed_destinations": ["W1", "W2", "W3", "S1", "S2", "S3", "S4", "C1", "C2",  "C4", "C5"]},
+    {"name": "C4", "x": 7.68, "y": 2.457, "angle": 3.14, "allowed_destinations": ["W1", "W2", "W3", "S3", "S4", "S3", "S4", "C1", "C2", "C3",  "C5"]},
+    {"name": "C5", "x": 7.68, "y": 0.787, "angle": 3.14, "allowed_destinations": ["W1", "W2", "W3", "S3", "S4", "S3", "S4", "C1", "C2", "C3", "C4"]},
 ]
-
+'''
+obastacles = [
+    {"x": , "y": , "radius": },
+    {"x": , "y": , "radius": },
+    {"x": , "y": , "radius": }
+]
+'''
 def create_waypoint(point):
     return {
         "x": point["x"],
@@ -68,16 +71,16 @@ def find_path(start, end_name, newPath, path_name):
         path_name += f" To {end['name']}"
         paths[path_name] = create_path(newPath)
         return
-    else:
-        midPoint = next((mid for mid in midPoints if mid["name"] == end_name), None)
-        if midPoint:
-            newPath.append(midPoint)
-            path_name += f" To {midPoint['name']}"
-            for end_name in midPoint["allowed_destinations"]:
-                find_path(midPoint, end_name, newPath.copy(), path_name)
-        else:
-            print("No midpoint found")
-
+    #else:
+    #    midPoint = next((mid for mid in midPoints if mid["name"] == end_name), None)
+    #    if midPoint:
+    #        newPath.append(midPoint)
+    #        path_name += f" To {midPoint['name']}"
+   #         for end_name in midPoint["allowed_destinations"]:
+   #             find_path(midPoint, end_name, newPath.copy(), path_name)
+   #     else:
+   #         break
+#
 for start in locations:
     newPath = [start]
     for end_name in start["allowed_destinations"]:
@@ -105,12 +108,7 @@ def find_path(start, end_name, newPath, path_name):
 
 #Prints names of all paths
 #for i in paths:
-#print(i)
-print(paths["Amp Side To C3 To S4"])
-print(len(paths["Amp Side To C3 To S4"]['waypoints']))
 
-#Total Number of paths
-print(len(paths))
 
 # Write the data to a JSON file
 with open('C:\\Users\\Robotics\\Desktop\\Crescendo\\src\\main\\java\\frc\\robot\\utils\\dynamicauto\\data.chor', 'w') as f:
diff --git a/src/main/java/frc/robot/utils/dynamicauto/data.chor b/src/main/java/frc/robot/utils/dynamicauto/data.chor
index f162506e..d2e54a6f 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/data.chor
+++ b/src/main/java/frc/robot/utils/dynamicauto/data.chor
@@ -22,7 +22,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 12
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 2.3,
@@ -34,125 +34,7 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 0.71,
-                    "y": 6.72,
-                    "heading": -2.1,
-                    "angularVelocity": 1.1712645267224225e-32,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 0.754119446225283,
-                    "y": 6.720987363814453,
-                    "heading": -2.1237071001932817,
-                    "angularVelocity": -0.324046398407224,
-                    "velocityX": 0.6030576296752822,
-                    "velocityY": 0.013496027999351609,
-                    "timestamp": 0.07315958550933178
-                },
-                {
-                    "x": 0.8424134953068435,
-                    "y": 6.722973003308925,
-                    "heading": -2.1697540075127364,
-                    "angularVelocity": -0.6294036112814133,
-                    "velocityX": 1.2068691814868098,
-                    "velocityY": 0.027141207548542687,
-                    "timestamp": 0.14631917101866357
-                },
-                {
-                    "x": 0.9749625172883624,
-                    "y": 6.725970405382453,
-                    "heading": -2.236002046597228,
-                    "angularVelocity": -0.9055278077813215,
-                    "velocityX": 1.8117792912401405,
-                    "velocityY": 0.04097073613339593,
-                    "timestamp": 0.21947875652799537
-                },
-                {
-                    "x": 1.15189309679064,
-                    "y": 6.729994604425149,
-                    "heading": -2.318545180808417,
-                    "angularVelocity": -1.128261370488901,
-                    "velocityX": 2.4184196543829377,
-                    "velocityY": 0.05500576602073325,
-                    "timestamp": 0.29263834203732714
-                },
-                {
-                    "x": 1.3734035805681073,
-                    "y": 6.735067453562316,
-                    "heading": -2.4074941689331384,
-                    "angularVelocity": -1.2158213787772831,
-                    "velocityX": 3.0277711694964378,
-                    "velocityY": 0.06933950078936708,
-                    "timestamp": 0.3657979275466589
-                },
-                {
-                    "x": 1.6366126990046324,
-                    "y": 6.741789664093089,
-                    "heading": -2.440634607899988,
-                    "angularVelocity": -0.45298833688286777,
-                    "velocityX": 3.5977393338693906,
-                    "velocityY": 0.09188420743425116,
-                    "timestamp": 0.4389575130559907
-                },
-                {
-                    "x": 1.8581204107797031,
-                    "y": 6.746837438064149,
-                    "heading": -2.53012067379602,
-                    "angularVelocity": -1.2231625599444977,
-                    "velocityX": 3.0277332796918652,
-                    "velocityY": 0.06899675464152406,
-                    "timestamp": 0.5121170985653225
-                },
-                {
-                    "x": 2.035048200585451,
-                    "y": 6.750889717758592,
-                    "heading": -2.613093680081622,
-                    "angularVelocity": -1.1341371839104541,
-                    "velocityX": 2.4183815227216985,
-                    "velocityY": 0.05538959339687604,
-                    "timestamp": 0.5852766840746543
-                },
-                {
-                    "x": 2.1675926710077147,
-                    "y": 6.75393921231849,
-                    "heading": -2.679769909140043,
-                    "angularVelocity": -0.9113806289937514,
-                    "velocityX": 1.8117170771198088,
-                    "velocityY": 0.04168277524633505,
-                    "timestamp": 0.658436269583986
-                },
-                {
-                    "x": 2.255882530144482,
-                    "y": 6.755978072079856,
-                    "heading": -2.726139004333996,
-                    "angularVelocity": -0.6338075164194407,
-                    "velocityX": 1.206811910183738,
-                    "velocityY": 0.027868662010218256,
-                    "timestamp": 0.7315958550933178
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": -2.75,
-                    "angularVelocity": -0.32614995697255006,
-                    "velocityX": 0.6030306151733326,
-                    "velocityY": 0.013968476079118136,
-                    "timestamp": 0.8047554406026496
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": -2.75,
-                    "angularVelocity": 2.3711039345986376e-33,
-                    "velocityX": -1.8060489353873473e-34,
-                    "velocityY": 7.872914025869492e-32,
-                    "timestamp": 0.8779150261119814
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -213,7 +95,7 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Amp Side To C1 To S1": {
+        "Amp Side To W3": {
             "waypoints": [
                 {
                     "x": 0.71,
@@ -225,18 +107,9 @@
                     "controlIntervalCount": 40
                 },
                 {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -263,7 +136,7 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Amp Side To C1 To S2": {
+        "Amp Side To C1": {
             "waypoints": [
                 {
                     "x": 0.71,
@@ -282,15 +155,6 @@
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
                 }
             ],
             "trajectory": [],
@@ -313,7 +177,7 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Amp Side To C2 To S1": {
+        "Amp Side To C2": {
             "waypoints": [
                 {
                     "x": 0.71,
@@ -332,15 +196,6 @@
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
                 }
             ],
             "trajectory": [],
@@ -363,7 +218,7 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Amp Side To C2 To S2": {
+        "Amp Side To C3": {
             "waypoints": [
                 {
                     "x": 0.71,
@@ -376,16 +231,7 @@
                 },
                 {
                     "x": 7.68,
-                    "y": 5.797,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
+                    "y": 4.127,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
@@ -413,7 +259,7 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Amp Side To C3 To S1": {
+        "Amp Side To C4": {
             "waypoints": [
                 {
                     "x": 0.71,
@@ -426,21 +272,12 @@
                 },
                 {
                     "x": 7.68,
-                    "y": 4.127,
+                    "y": 2.457,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
                 }
             ],
             "trajectory": [],
@@ -463,7 +300,7 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Amp Side To C3 To S2": {
+        "Amp Side To C5": {
             "waypoints": [
                 {
                     "x": 0.71,
@@ -472,21 +309,12 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 34
                 },
                 {
                     "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
+                    "y": 0.787,
+                    "heading": -3.143185307179586,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -511,22 +339,19 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.378675937652588,
+                    "y": 4.222219944000244,
+                    "radius": 0.5
+                }
+            ]
         },
-        "Amp Side To C3 To S3": {
+        "Center To W1": {
             "waypoints": [
                 {
-                    "x": 0.71,
-                    "y": 6.72,
-                    "heading": -2.1,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
+                    "x": 1.35,
+                    "y": 5.56,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
@@ -534,9 +359,9 @@
                     "controlIntervalCount": 40
                 },
                 {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -563,20 +388,11 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Amp Side To C3 To S4": {
+        "Center To W2": {
             "waypoints": [
                 {
-                    "x": 0.71,
-                    "y": 6.72,
-                    "heading": -2.1,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
+                    "x": 1.35,
+                    "y": 5.56,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
@@ -584,9 +400,9 @@
                     "controlIntervalCount": 40
                 },
                 {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -613,7 +429,7 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Center To W1": {
+        "Center To W3": {
             "waypoints": [
                 {
                     "x": 1.35,
@@ -626,8 +442,8 @@
                 },
                 {
                     "x": 2.3,
-                    "y": 6.757,
-                    "heading": -2.75,
+                    "y": 4.36,
+                    "heading": 2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -654,7 +470,7 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Center To W2": {
+        "Center To C1": {
             "waypoints": [
                 {
                     "x": 1.35,
@@ -666,8 +482,8 @@
                     "controlIntervalCount": 40
                 },
                 {
-                    "x": 2.25,
-                    "y": 5.56,
+                    "x": 7.68,
+                    "y": 7.467,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
@@ -695,7 +511,7 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Center To W3": {
+        "Center To C2": {
             "waypoints": [
                 {
                     "x": 1.35,
@@ -707,9 +523,9 @@
                     "controlIntervalCount": 40
                 },
                 {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -736,7 +552,7 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Center To C1 To S1": {
+        "Center To C3": {
             "waypoints": [
                 {
                     "x": 1.35,
@@ -749,21 +565,12 @@
                 },
                 {
                     "x": 7.68,
-                    "y": 7.467,
+                    "y": 4.127,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
                 }
             ],
             "trajectory": [],
@@ -786,7 +593,7 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Center To C1 To S2": {
+        "Center To C4": {
             "waypoints": [
                 {
                     "x": 1.35,
@@ -799,16 +606,7 @@
                 },
                 {
                     "x": 7.68,
-                    "y": 7.467,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
+                    "y": 2.457,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
@@ -836,7 +634,7 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Center To C2 To S1": {
+        "Center To C5": {
             "waypoints": [
                 {
                     "x": 1.35,
@@ -849,21 +647,12 @@
                 },
                 {
                     "x": 7.68,
-                    "y": 5.797,
+                    "y": 0.787,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
                 }
             ],
             "trajectory": [],
@@ -886,30 +675,21 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Center To C2 To S2": {
+        "Stage Side To W1": {
             "waypoints": [
                 {
-                    "x": 1.35,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 3.14,
+                    "x": 0.71,
+                    "y": 4.36,
+                    "heading": 2.1,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 },
                 {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -936,34 +716,25 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Center To C3 To S1": {
+        "Stage Side To W2": {
             "waypoints": [
                 {
-                    "x": 1.35,
-                    "y": 5.56,
-                    "heading": 3.14,
+                    "x": 0.71,
+                    "y": 4.36,
+                    "heading": 2.1,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 },
                 {
-                    "x": 7.68,
-                    "y": 4.127,
+                    "x": 2.25,
+                    "y": 5.56,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
                 }
             ],
             "trajectory": [],
@@ -986,30 +757,21 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Center To C3 To S2": {
+        "Stage Side To W3": {
             "waypoints": [
                 {
-                    "x": 1.35,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
+                    "x": 0.71,
+                    "y": 4.36,
+                    "heading": 2.1,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 },
                 {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -1036,12 +798,12 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Center To C3 To S3": {
+        "Stage Side To C1": {
             "waypoints": [
                 {
-                    "x": 1.35,
-                    "y": 5.56,
-                    "heading": 3.14,
+                    "x": 0.71,
+                    "y": 4.36,
+                    "heading": 2.1,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -1049,21 +811,12 @@
                 },
                 {
                     "x": 7.68,
-                    "y": 4.127,
+                    "y": 7.467,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
                 }
             ],
             "trajectory": [],
@@ -1086,12 +839,12 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Center To C3 To S4": {
+        "Stage Side To C2": {
             "waypoints": [
                 {
-                    "x": 1.35,
-                    "y": 5.56,
-                    "heading": 3.14,
+                    "x": 0.71,
+                    "y": 4.36,
+                    "heading": 2.1,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -1099,21 +852,12 @@
                 },
                 {
                     "x": 7.68,
-                    "y": 4.127,
+                    "y": 5.797,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
                 }
             ],
             "trajectory": [],
@@ -1136,12 +880,12 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Center To C4 To S3": {
+        "Stage Side To C3": {
             "waypoints": [
                 {
-                    "x": 1.35,
-                    "y": 5.56,
-                    "heading": 3.14,
+                    "x": 0.71,
+                    "y": 4.36,
+                    "heading": 2.1,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -1149,21 +893,12 @@
                 },
                 {
                     "x": 7.68,
-                    "y": 2.457,
+                    "y": 4.127,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
                 }
             ],
             "trajectory": [],
@@ -1186,12 +921,12 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Center To C4 To S4": {
+        "Stage Side To C4": {
             "waypoints": [
                 {
-                    "x": 1.35,
-                    "y": 5.56,
-                    "heading": 3.14,
+                    "x": 0.71,
+                    "y": 4.36,
+                    "heading": 2.1,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -1205,15 +940,6 @@
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
                 }
             ],
             "trajectory": [],
@@ -1236,12 +962,12 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Center To C5 To S3": {
+        "Stage Side To C5": {
             "waypoints": [
                 {
-                    "x": 1.35,
-                    "y": 5.56,
-                    "heading": 3.14,
+                    "x": 0.71,
+                    "y": 4.36,
+                    "heading": 2.1,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -1255,15 +981,6 @@
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
                 }
             ],
             "trajectory": [],
@@ -1286,34 +1003,25 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Center To C5 To S4": {
+        "W1 To W2": {
             "waypoints": [
                 {
-                    "x": 1.35,
-                    "y": 5.56,
-                    "heading": 3.14,
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 },
                 {
-                    "x": 7.68,
-                    "y": 0.787,
+                    "x": 2.25,
+                    "y": 5.56,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
                 }
             ],
             "trajectory": [],
@@ -1336,21 +1044,21 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Stage Side To W2": {
+        "W1 To W3": {
             "waypoints": [
                 {
-                    "x": 0.71,
-                    "y": 4.36,
-                    "heading": 2.1,
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 },
                 {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 3.14,
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -1377,21 +1085,21 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Stage Side To W3": {
+        "W1 To S1": {
             "waypoints": [
                 {
-                    "x": 0.71,
-                    "y": 4.36,
-                    "heading": 2.1,
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 },
                 {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -1418,34 +1126,25 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Stage Side To C3 To S1": {
+        "W1 To S2": {
             "waypoints": [
                 {
-                    "x": 0.71,
-                    "y": 4.36,
-                    "heading": 2.1,
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 },
                 {
-                    "x": 7.68,
-                    "y": 4.127,
+                    "x": 4.263,
+                    "y": 5.56,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
                 }
             ],
             "trajectory": [],
@@ -1468,21 +1167,12 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Stage Side To C3 To S2": {
+        "W1 To S3": {
             "waypoints": [
                 {
-                    "x": 0.71,
-                    "y": 4.36,
-                    "heading": 2.1,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -1490,8 +1180,8 @@
                 },
                 {
                     "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
+                    "y": 3,
+                    "heading": 2.574,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -1518,30 +1208,21 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Stage Side To C3 To S3": {
+        "W1 To S4": {
             "waypoints": [
                 {
-                    "x": 0.71,
-                    "y": 4.36,
-                    "heading": 2.1,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 },
                 {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -1568,12 +1249,12 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Stage Side To C3 To S4": {
+        "W1 To C1": {
             "waypoints": [
                 {
-                    "x": 0.71,
-                    "y": 4.36,
-                    "heading": 2.1,
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -1581,21 +1262,12 @@
                 },
                 {
                     "x": 7.68,
-                    "y": 4.127,
+                    "y": 7.467,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
                 }
             ],
             "trajectory": [],
@@ -1618,12 +1290,12 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Stage Side To C4 To S3": {
+        "W1 To C2": {
             "waypoints": [
                 {
-                    "x": 0.71,
-                    "y": 4.36,
-                    "heading": 2.1,
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -1631,21 +1303,12 @@
                 },
                 {
                     "x": 7.68,
-                    "y": 2.457,
+                    "y": 5.797,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
                 }
             ],
             "trajectory": [],
@@ -1668,12 +1331,12 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Stage Side To C4 To S4": {
+        "W1 To C3": {
             "waypoints": [
                 {
-                    "x": 0.71,
-                    "y": 4.36,
-                    "heading": 2.1,
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -1681,21 +1344,12 @@
                 },
                 {
                     "x": 7.68,
-                    "y": 2.457,
+                    "y": 4.127,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
                 }
             ],
             "trajectory": [],
@@ -1718,12 +1372,12 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Stage Side To C5 To S3": {
+        "W1 To C4": {
             "waypoints": [
                 {
-                    "x": 0.71,
-                    "y": 4.36,
-                    "heading": 2.1,
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -1731,21 +1385,12 @@
                 },
                 {
                     "x": 7.68,
-                    "y": 0.787,
+                    "y": 2.457,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
                 }
             ],
             "trajectory": [],
@@ -1768,12 +1413,12 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "Stage Side To C5 To S4": {
+        "W1 To C5": {
             "waypoints": [
                 {
-                    "x": 0.71,
-                    "y": 4.36,
-                    "heading": 2.1,
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -1787,19 +1432,10 @@
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [],
-            "constraints": [
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
                 {
                     "scope": [
                         "first"
@@ -1818,128 +1454,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W1 To W2": {
+        "W2 To W1": {
             "waypoints": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": -2.75,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 10
-                },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": -3.143185307179586,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
+                },
                 {
                     "x": 2.3,
                     "y": 6.757,
                     "heading": -2.75,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.2980013304879616,
-                    "y": 6.709118488011682,
-                    "heading": -2.76564785007466,
-                    "angularVelocity": -0.2059742190979395,
-                    "velocityX": -0.026308687136742944,
-                    "velocityY": -0.6302691420205845,
-                    "timestamp": 0.07596994489499287
-                },
-                {
-                    "x": 2.294004010549845,
-                    "y": 6.6133557271698225,
-                    "heading": -2.796958855018623,
-                    "angularVelocity": -0.4121498967419566,
-                    "velocityX": -0.05261712304308625,
-                    "velocityY": -1.2605348203717124,
-                    "timestamp": 0.15193988978998574
-                },
-                {
-                    "x": 2.2880079544710785,
-                    "y": 6.469712199120552,
-                    "heading": -2.8439591282551087,
-                    "angularVelocity": -0.618669307993449,
-                    "velocityX": -0.07892668721893806,
-                    "velocityY": -1.8907941587665607,
-                    "timestamp": 0.2279098346849786
-                },
-                {
-                    "x": 2.2800128266474267,
-                    "y": 6.278188582998367,
-                    "heading": -2.906683998413123,
-                    "angularVelocity": -0.8256537535299532,
-                    "velocityX": -0.10524066898696208,
-                    "velocityY": -2.5210445576459737,
-                    "timestamp": 0.3038797795799715
-                },
-                {
-                    "x": 2.27001785393274,
-                    "y": 6.038785706839934,
-                    "heading": -2.985174219418862,
-                    "angularVelocity": -1.0331746470822023,
-                    "velocityX": -0.13156482775526773,
-                    "velocityY": -3.151284056995696,
-                    "timestamp": 0.37984972447496435
-                },
-                {
-                    "x": 2.2620112641506833,
-                    "y": 5.847271276792759,
-                    "heading": -3.048371938278793,
-                    "angularVelocity": -0.8318779083923785,
-                    "velocityX": -0.10539154389440716,
-                    "velocityY": -2.5209236404197743,
-                    "timestamp": 0.4558196693699572
-                },
-                {
-                    "x": 2.256005772930322,
-                    "y": 5.703635612537123,
-                    "heading": -3.0957774424626403,
-                    "angularVelocity": -0.6240034035745593,
-                    "velocityX": -0.07905088293353696,
-                    "velocityY": -1.8906906468626767,
-                    "timestamp": 0.5317896142649501
-                },
-                {
-                    "x": 2.25200192771716,
-                    "y": 5.607878538669833,
-                    "heading": -3.127382702013982,
-                    "angularVelocity": -0.4160231996354264,
-                    "velocityX": -0.05270301589261432,
-                    "velocityY": -1.2604599621553931,
-                    "timestamp": 0.607759559159943
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -0.20801127587293633,
-                    "velocityX": -0.026351575217374957,
-                    "velocityY": -0.6302300039313168,
-                    "timestamp": 0.6837295040549358
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": 2.0416042206069368e-34,
-                    "velocityX": 0,
-                    "velocityY": 6.898965298259951e-35,
-                    "timestamp": 0.7596994489499287
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 }
             ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -1959,353 +1495,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W1 To C1 To S1": {
+        "W2 To W3": {
             "waypoints": [
                 {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": -2.75,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 21
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": -3.143185307179586,
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 13
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": -2.75,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.363517540312363,
-                    "y": 6.769205991625322,
-                    "heading": -2.7639736107792703,
-                    "angularVelocity": -0.15851225932569585,
-                    "velocityX": 0.7205230617028869,
-                    "velocityY": 0.13846094187126756,
-                    "timestamp": 0.08815476379374498
-                },
-                {
-                    "x": 2.4905727294218147,
-                    "y": 6.793631120419202,
-                    "heading": -2.790535805480005,
-                    "angularVelocity": -0.30131320824454183,
-                    "velocityX": 1.4412742277515702,
-                    "velocityY": 0.2770710026632852,
-                    "timestamp": 0.17630952758748997
-                },
-                {
-                    "x": 2.6811943332190395,
-                    "y": 6.830296876066563,
-                    "heading": -2.8273459830802556,
-                    "angularVelocity": -0.4175631130539097,
-                    "velocityX": 2.162351705044798,
-                    "velocityY": 0.4159248357030905,
-                    "timestamp": 0.26446429138123495
-                },
-                {
-                    "x": 2.935420930881945,
-                    "y": 6.8792439916128805,
-                    "heading": -2.869603740492264,
-                    "angularVelocity": -0.47935875037764497,
-                    "velocityX": 2.8838668124358944,
-                    "velocityY": 0.5552407316391297,
-                    "timestamp": 0.35261905517497993
-                },
-                {
-                    "x": 3.253189180048851,
-                    "y": 6.9405688545954956,
-                    "heading": -2.9017584747192204,
-                    "angularVelocity": -0.364753223117765,
-                    "velocityX": 3.604663383936769,
-                    "velocityY": 0.695650017576943,
-                    "timestamp": 0.4407738189687249
-                },
-                {
-                    "x": 3.5807176480413454,
-                    "y": 7.003313040154224,
-                    "heading": -2.901758484999235,
-                    "angularVelocity": -1.1661326594690383e-7,
-                    "velocityX": 3.7153802460274443,
-                    "velocityY": 0.7117503678590205,
-                    "timestamp": 0.5289285827624699
-                },
-                {
-                    "x": 3.9082461164396207,
-                    "y": 7.066057223594786,
-                    "heading": -2.901758495279238,
-                    "angularVelocity": -1.1661313280225847e-7,
-                    "velocityX": 3.7153802506304894,
-                    "velocityY": 0.7117503438312037,
-                    "timestamp": 0.6170833465562149
-                },
-                {
-                    "x": 4.235774584837897,
-                    "y": 7.128801407035338,
-                    "heading": -2.901758505559241,
-                    "angularVelocity": -1.1661313256198823e-7,
-                    "velocityX": 3.7153802506305107,
-                    "velocityY": 0.711750343831093,
-                    "timestamp": 0.7052381103499599
-                },
-                {
-                    "x": 4.563303053236174,
-                    "y": 7.19154559047589,
-                    "heading": -2.9017585158392443,
-                    "angularVelocity": -1.166131334044927e-7,
-                    "velocityX": 3.7153802506305107,
-                    "velocityY": 0.7117503438310929,
-                    "timestamp": 0.7933928741437049
-                },
-                {
-                    "x": 4.890831521634451,
-                    "y": 7.254289773916442,
-                    "heading": -2.9017585261192473,
-                    "angularVelocity": -1.1661313263433965e-7,
-                    "velocityX": 3.7153802506305107,
-                    "velocityY": 0.711750343831093,
-                    "timestamp": 0.8815476379374498
-                },
-                {
-                    "x": 5.218359990032727,
-                    "y": 7.317033957356994,
-                    "heading": -2.9017585363992504,
-                    "angularVelocity": -1.1661313267352917e-7,
-                    "velocityX": 3.7153802506305107,
-                    "velocityY": 0.7117503438310931,
-                    "timestamp": 0.9697024017311948
-                },
-                {
-                    "x": 5.545888458431004,
-                    "y": 7.379778140797546,
-                    "heading": -2.9017585466792535,
-                    "angularVelocity": -1.1661313271580576e-7,
-                    "velocityX": 3.7153802506305107,
-                    "velocityY": 0.7117503438310933,
-                    "timestamp": 1.0578571655249398
-                },
-                {
-                    "x": 5.873416926829281,
-                    "y": 7.442522324238098,
-                    "heading": -2.901758556959257,
-                    "angularVelocity": -1.166131330429824e-7,
-                    "velocityX": 3.7153802506305103,
-                    "velocityY": 0.7117503438310929,
-                    "timestamp": 1.1460119293186848
-                },
-                {
-                    "x": 6.200945395227635,
-                    "y": 7.505266507678241,
-                    "heading": -2.90175856723926,
-                    "angularVelocity": -1.1661313350589332e-7,
-                    "velocityX": 3.7153802506314,
-                    "velocityY": 0.7117503438264522,
-                    "timestamp": 1.2341666931124298
-                },
-                {
-                    "x": 6.528473880635578,
-                    "y": 7.568010602327234,
-                    "heading": -2.9017585775192636,
-                    "angularVelocity": -1.1661313512046063e-7,
-                    "velocityX": 3.715380443582822,
-                    "velocityY": 0.7117493366074876,
-                    "timestamp": 1.3223214569061748
-                },
-                {
-                    "x": 6.858875530524355,
-                    "y": 7.613248618194527,
-                    "heading": -2.901758593513828,
-                    "angularVelocity": -1.8143731961725824e-7,
-                    "velocityX": 3.747972720587331,
-                    "velocityY": 0.51316586784965,
-                    "timestamp": 1.4104762206999197
-                },
-                {
-                    "x": 7.1493370822627105,
-                    "y": 7.612204494163831,
-                    "heading": -2.9774836288017847,
-                    "angularVelocity": -0.859001057108326,
-                    "velocityX": 3.294904770182843,
-                    "velocityY": -0.011844215624353746,
-                    "timestamp": 1.4986309844936647
-                },
-                {
-                    "x": 7.3768731997980925,
-                    "y": 7.596010582302943,
-                    "heading": -3.0421265478344597,
-                    "angularVelocity": -0.7332890050493631,
-                    "velocityX": 2.581098374533033,
-                    "velocityY": -0.18369865863138202,
-                    "timestamp": 1.5867857482874097
-                },
-                {
-                    "x": 7.541210779550857,
-                    "y": 7.566116365480278,
-                    "heading": -3.09203212161725,
-                    "angularVelocity": -0.566113181354048,
-                    "velocityX": 1.8641939775059564,
-                    "velocityY": -0.33911062245705376,
-                    "timestamp": 1.6749405120811547
-                },
-                {
-                    "x": 7.642266411565545,
-                    "y": 7.52302718020075,
-                    "heading": -3.125932818808563,
-                    "angularVelocity": -0.3845588795476813,
-                    "velocityX": 1.146343404096945,
-                    "velocityY": -0.4887902074166845,
-                    "timestamp": 1.7630952758748997
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -0.19570681865122413,
-                    "velocityX": 0.4280379960263239,
-                    "velocityY": -0.6355547651607396,
-                    "timestamp": 1.8512500396686447
-                },
-                {
-                    "x": 7.65149635610838,
-                    "y": 7.395583820775626,
-                    "heading": -3.1428442524727873,
-                    "angularVelocity": 0.003749626131397279,
-                    "velocityX": -0.3133749684294368,
-                    "velocityY": -0.7851642756589506,
-                    "timestamp": 1.9422070297386083
-                },
-                {
-                    "x": 7.55553660648891,
-                    "y": 7.310596065504526,
-                    "heading": -3.1249452939328513,
-                    "angularVelocity": 0.19678485981305774,
-                    "velocityX": -1.0550013753274867,
-                    "velocityY": -0.9343729954757892,
-                    "timestamp": 2.033164019808572
-                },
-                {
-                    "x": 7.392089155098869,
-                    "y": 7.212098072056303,
-                    "heading": -3.09046475805122,
-                    "angularVelocity": 0.37908615770051013,
-                    "velocityX": -1.796975155667623,
-                    "velocityY": -1.082907354041688,
-                    "timestamp": 2.1241210098785355
-                },
-                {
-                    "x": 7.1610935040737775,
-                    "y": 7.100214825167467,
-                    "heading": -3.041377794518818,
-                    "angularVelocity": 0.5396722505288368,
-                    "velocityX": -2.539614062068222,
-                    "velocityY": -1.2300676045109389,
-                    "timestamp": 2.215077999948499
-                },
-                {
-                    "x": 6.862392163588639,
-                    "y": 6.975338305116689,
-                    "heading": -2.9837366917531454,
-                    "angularVelocity": 0.6337182301358967,
-                    "velocityX": -3.2839844442451716,
-                    "velocityY": -1.3729183425572538,
-                    "timestamp": 2.3060349900184627
-                },
-                {
-                    "x": 6.525454088685491,
-                    "y": 6.905573407470138,
-                    "heading": -2.9837366810920996,
-                    "angularVelocity": 1.1720974856117963e-7,
-                    "velocityX": -3.704367027141189,
-                    "velocityY": -0.7670097437560687,
-                    "timestamp": 2.3969919800884263
-                },
-                {
-                    "x": 6.1883268460675325,
-                    "y": 6.836728439791378,
-                    "heading": -2.983736670548346,
-                    "angularVelocity": 1.1592021231646331e-7,
-                    "velocityX": -3.7064467762031894,
-                    "velocityY": -0.7568958430329531,
-                    "timestamp": 2.48794897015839
-                },
-                {
-                    "x": 5.851199548758337,
-                    "y": 6.767884093073121,
-                    "heading": -2.9837365285519053,
-                    "angularVelocity": 0.0000015611382957440025,
-                    "velocityX": -3.706447377489981,
-                    "velocityY": -0.7568890160645022,
-                    "timestamp": 2.5789059602283535
-                },
-                {
-                    "x": 5.5810538404288925,
-                    "y": 6.712717106073741,
-                    "heading": -2.9505527658877098,
-                    "angularVelocity": 0.3648291641873475,
-                    "velocityX": -2.970037906065856,
-                    "velocityY": -0.6065172886320637,
-                    "timestamp": 2.669862950298317
-                },
-                {
-                    "x": 5.3784959131966685,
-                    "y": 6.671352294274059,
-                    "heading": -2.9178931983006144,
-                    "angularVelocity": 0.3590660548682879,
-                    "velocityX": -2.226963832866719,
-                    "velocityY": -0.45477331393503795,
-                    "timestamp": 2.7608199403682807
-                },
-                {
-                    "x": 5.2434899940019335,
-                    "y": 6.6437823583535645,
-                    "heading": -2.8932620140402783,
-                    "angularVelocity": 0.2708003446618372,
-                    "velocityX": -1.4842830561003668,
-                    "velocityY": -0.30310958948050626,
-                    "timestamp": 2.8517769304382443
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "angularVelocity": 0.14580533096009823,
-                    "velocityX": -0.7419989816068217,
-                    "velocityY": -0.15152610418353574,
-                    "timestamp": 2.942733920508208
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "angularVelocity": 0,
-                    "velocityX": 1.1797533379852482e-32,
-                    "velocityY": 0,
-                    "timestamp": 3.0336909105781715
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -2325,371 +1536,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W1 To C1 To S2": {
+        "W2 To S1": {
             "waypoints": [
                 {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": -2.75,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 21
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": -3.143185307179586,
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 15
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": -3.143185307179586,
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": -2.75,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.360726976398386,
-                    "y": 6.773087618158843,
-                    "heading": -2.760701916982345,
-                    "angularVelocity": -0.1232236665180751,
-                    "velocityX": 0.699220588306308,
-                    "velocityY": 0.18523553288193312,
-                    "timestamp": 0.08684952562036656
-                },
-                {
-                    "x": 2.4821843215925723,
-                    "y": 6.805261248290758,
-                    "heading": -2.781888481642216,
-                    "angularVelocity": -0.243945657831127,
-                    "velocityX": 1.3984802372452498,
-                    "velocityY": 0.37045257187160163,
-                    "timestamp": 0.17369905124073312
-                },
-                {
-                    "x": 2.664377574952537,
-                    "y": 6.853518187805784,
-                    "heading": -2.813196351929266,
-                    "angularVelocity": -0.3604840678552547,
-                    "velocityX": 2.0978036674189,
-                    "velocityY": 0.5556384927872844,
-                    "timestamp": 0.26054857686109967
-                },
-                {
-                    "x": 2.9073173781912582,
-                    "y": 6.917852930781389,
-                    "heading": -2.8538946648532777,
-                    "angularVelocity": -0.4686071988668809,
-                    "velocityX": 2.797249628060051,
-                    "velocityY": 0.7407610175878006,
-                    "timestamp": 0.34739810248146624
-                },
-                {
-                    "x": 3.211032053030438,
-                    "y": 6.99824783750104,
-                    "heading": -2.901766372941813,
-                    "angularVelocity": -0.5512028735505646,
-                    "velocityX": 3.4970216897535775,
-                    "velocityY": 0.9256804357344971,
-                    "timestamp": 0.4342476281018328
-                },
-                {
-                    "x": 3.5286107194849063,
-                    "y": 7.082430089360801,
-                    "heading": -2.9017663796929978,
-                    "angularVelocity": -7.773427725096788e-8,
-                    "velocityX": 3.656654013778666,
-                    "velocityY": 0.9692885626994009,
-                    "timestamp": 0.5210971537221993
-                },
-                {
-                    "x": 3.846189385784725,
-                    "y": 7.166612341804015,
-                    "heading": -2.9017663864441645,
-                    "angularVelocity": -7.773406554275384e-8,
-                    "velocityX": 3.6566540119980107,
-                    "velocityY": 0.9692885694173843,
-                    "timestamp": 0.6079466793425659
-                },
-                {
-                    "x": 4.163768052084543,
-                    "y": 7.250794594247232,
-                    "heading": -2.9017663931953313,
-                    "angularVelocity": -7.77340646408651e-8,
-                    "velocityX": 3.656654011998002,
-                    "velocityY": 0.969288569417417,
-                    "timestamp": 0.6947962049629324
-                },
-                {
-                    "x": 4.481346718384361,
-                    "y": 7.334976846690449,
-                    "heading": -2.901766399946498,
-                    "angularVelocity": -7.773406451460054e-8,
-                    "velocityX": 3.656654011998002,
-                    "velocityY": 0.9692885694174173,
-                    "timestamp": 0.7816457305832989
-                },
-                {
-                    "x": 4.798925384684178,
-                    "y": 7.419159099133665,
-                    "heading": -2.9017664066976647,
-                    "angularVelocity": -7.773406529246814e-8,
-                    "velocityX": 3.656654011998002,
-                    "velocityY": 0.9692885694174173,
-                    "timestamp": 0.8684952562036654
-                },
-                {
-                    "x": 5.1165040509839965,
-                    "y": 7.503341351576882,
-                    "heading": -2.9017664134488315,
-                    "angularVelocity": -7.773406535280122e-8,
-                    "velocityX": 3.656654011998002,
-                    "velocityY": 0.969288569417417,
-                    "timestamp": 0.9553447818240319
-                },
-                {
-                    "x": 5.434082717283814,
-                    "y": 7.5875236040201,
-                    "heading": -2.901766420199998,
-                    "angularVelocity": -7.773406538713727e-8,
-                    "velocityX": 3.6566540119980018,
-                    "velocityY": 0.9692885694174171,
-                    "timestamp": 1.0421943074443984
-                },
-                {
-                    "x": 5.751661383583632,
-                    "y": 7.6717058564633165,
-                    "heading": -2.901766426951165,
-                    "angularVelocity": -7.773406530909276e-8,
-                    "velocityX": 3.6566540119980027,
-                    "velocityY": 0.969288569417416,
-                    "timestamp": 1.129043833064765
-                },
-                {
-                    "x": 6.0692400498888315,
-                    "y": 7.755888108886232,
-                    "heading": -2.9017664337023317,
-                    "angularVelocity": -7.773406498550699e-8,
-                    "velocityX": 3.6566540120599655,
-                    "velocityY": 0.9692885691836598,
-                    "timestamp": 1.2158933586851315
-                },
-                {
-                    "x": 6.386819824167639,
-                    "y": 7.840066181352823,
-                    "heading": -2.901766440453515,
-                    "angularVelocity": -7.773425363968975e-8,
-                    "velocityX": 3.6566667694538877,
-                    "velocityY": 0.9692404404655809,
-                    "timestamp": 1.302742884305498
-                },
-                {
-                    "x": 6.714420971530167,
-                    "y": 7.864972939151579,
-                    "heading": -2.901766454732528,
-                    "angularVelocity": -1.6441095007226412e-7,
-                    "velocityX": 3.7720545394171228,
-                    "velocityY": 0.28678058539594425,
-                    "timestamp": 1.3895924099258645
-                },
-                {
-                    "x": 7.024629196469154,
-                    "y": 7.83076867028296,
-                    "heading": -2.946892502448731,
-                    "angularVelocity": -0.5195888796655828,
-                    "velocityX": 3.57178951437027,
-                    "velocityY": -0.39383368676145797,
-                    "timestamp": 1.476441935546231
-                },
-                {
-                    "x": 7.277681557115376,
-                    "y": 7.7704292121587075,
-                    "heading": -3.000549099570889,
-                    "angularVelocity": -0.6178110558347718,
-                    "velocityX": 2.9136873096139366,
-                    "velocityY": -0.6947586379213355,
-                    "timestamp": 1.5632914611665976
-                },
-                {
-                    "x": 7.471595029086891,
-                    "y": 7.688687529495423,
-                    "heading": -3.0531246016667453,
-                    "angularVelocity": -0.6053631464343129,
-                    "velocityX": 2.232752229631644,
-                    "velocityY": -0.941187439761266,
-                    "timestamp": 1.650140986786964
-                },
-                {
-                    "x": 7.605791298108634,
-                    "y": 7.5872519075766585,
-                    "heading": -3.101243004619086,
-                    "angularVelocity": -0.5540433595763132,
-                    "velocityX": 1.5451583421207176,
-                    "velocityY": -1.1679467584193457,
-                    "timestamp": 1.7369905124073306
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -0.4829307041215213,
-                    "velocityX": 0.854451436102516,
-                    "velocityY": -1.3846006263992148,
-                    "timestamp": 1.8238400380276971
-                },
-                {
-                    "x": 7.684759443310988,
-                    "y": 7.301931264284962,
-                    "heading": -3.1822787877008336,
-                    "angularVelocity": -0.3860471334464062,
-                    "velocityX": 0.046999382572933714,
-                    "velocityY": -1.6300495989311972,
-                    "timestamp": 1.925106119638729
-                },
-                {
-                    "x": 7.607446461395855,
-                    "y": 7.1130338870077985,
-                    "heading": -3.2115348858230854,
-                    "angularVelocity": -0.2889032305471994,
-                    "velocityX": -0.7634637450650118,
-                    "velocityY": -1.8653568329339223,
-                    "timestamp": 2.0263722012497607
-                },
-                {
-                    "x": 7.447519490138273,
-                    "y": 6.902260922095987,
-                    "heading": -3.2309059414306347,
-                    "angularVelocity": -0.1912886852083134,
-                    "velocityX": -1.5792748046828227,
-                    "velocityY": -2.0813777086972793,
-                    "timestamp": 2.1276382828607927
-                },
-                {
-                    "x": 7.203787473069244,
-                    "y": 6.674749702005586,
-                    "heading": -3.240291639650134,
-                    "angularVelocity": -0.09268353302700813,
-                    "velocityX": -2.406847516870182,
-                    "velocityY": -2.246667556121621,
-                    "timestamp": 2.2289043644718247
-                },
-                {
-                    "x": 6.8784205377457015,
-                    "y": 6.472538498060418,
-                    "heading": -3.240291634989942,
-                    "angularVelocity": 4.6019273769549756e-8,
-                    "velocityX": -3.212990274205886,
-                    "velocityY": -1.996830535247213,
-                    "timestamp": 2.3301704460828567
-                },
-                {
-                    "x": 6.516724017217928,
-                    "y": 6.34632974848032,
-                    "heading": -3.2402916309896224,
-                    "angularVelocity": 3.950305172889797e-8,
-                    "velocityX": -3.5717440111670684,
-                    "velocityY": -1.2463082166308144,
-                    "timestamp": 2.4314365276938887
-                },
-                {
-                    "x": 6.155023833001519,
-                    "y": 6.220131499152302,
-                    "heading": -3.240291626989329,
-                    "angularVelocity": 3.950279970360729e-8,
-                    "velocityX": -3.571780189992463,
-                    "velocityY": -1.2462045269279236,
-                    "timestamp": 2.5327026093049207
-                },
-                {
-                    "x": 5.793323648774168,
-                    "y": 6.093933249855646,
-                    "heading": -3.240291622989035,
-                    "angularVelocity": 3.950280005859099e-8,
-                    "velocityX": -3.571780190100518,
-                    "velocityY": -1.2462045266182225,
-                    "timestamp": 2.6339686909159528
-                },
-                {
-                    "x": 5.431623464546816,
-                    "y": 5.96773500055899,
-                    "heading": -3.240291618988741,
-                    "angularVelocity": 3.9502799442138326e-8,
-                    "velocityX": -3.5717801901005184,
-                    "velocityY": -1.246204526618221,
-                    "timestamp": 2.7352347725269848
-                },
-                {
-                    "x": 5.0699232803112935,
-                    "y": 5.841536751285806,
-                    "heading": -3.240291614988434,
-                    "angularVelocity": 3.9502931091413385e-8,
-                    "velocityX": -3.571780190181203,
-                    "velocityY": -1.2462045263864454,
-                    "timestamp": 2.836500854138017
-                },
-                {
-                    "x": 4.747153979068025,
-                    "y": 5.72892205607683,
-                    "heading": -3.2014500073341168,
-                    "angularVelocity": 0.383559895242649,
-                    "velocityX": -3.1873387032305858,
-                    "velocityY": -1.11206727284526,
-                    "timestamp": 2.937766935749049
-                },
-                {
-                    "x": 4.505076992705577,
-                    "y": 5.644461029601604,
-                    "heading": -3.172317846735081,
-                    "angularVelocity": 0.2876793506335712,
-                    "velocityX": -2.3905041304188885,
-                    "velocityY": -0.8340505046852051,
-                    "timestamp": 3.039033017360081
-                },
-                {
-                    "x": 4.343692332074418,
-                    "y": 5.588153676944576,
-                    "heading": -3.1528961309698516,
-                    "angularVelocity": 0.19178895298675563,
-                    "velocityX": -1.5936694504591111,
-                    "velocityY": -0.5560336863163278,
-                    "timestamp": 3.140299098971113
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": 0.0958941398322912,
-                    "velocityX": -0.796834742598023,
-                    "velocityY": -0.2780168492417652,
-                    "timestamp": 3.241565180582145
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": 0,
-                    "velocityX": 6.013328716711935e-32,
-                    "velocityY": 0,
-                    "timestamp": 3.342831262193177
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -2709,353 +1577,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W1 To C2 To S1": {
+        "W2 To S2": {
             "waypoints": [
                 {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": -2.75,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 21
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": -3.143185307179586,
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 13
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": -2.75,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.3639970489597717,
-                    "y": 6.742603274185352,
-                    "heading": -2.7625259389239276,
-                    "angularVelocity": -0.14112178255834015,
-                    "velocityX": 0.7210140239806111,
-                    "velocityY": -0.16219874791866862,
-                    "timestamp": 0.08875978390330586
-                },
-                {
-                    "x": 2.4920132643077064,
-                    "y": 6.7138069605767345,
-                    "heading": -2.7859988261252213,
-                    "angularVelocity": -0.26445408234505485,
-                    "velocityX": 1.4422772309517358,
-                    "velocityY": -0.3244297399370449,
-                    "timestamp": 0.1775195678066117
-                },
-                {
-                    "x": 2.684079448734256,
-                    "y": 6.670607796290334,
-                    "heading": -2.8177443395453605,
-                    "angularVelocity": -0.3576564973921373,
-                    "velocityX": 2.163887472233876,
-                    "velocityY": -0.4866974927908953,
-                    "timestamp": 0.26627935170991757
-                },
-                {
-                    "x": 2.9402327242018096,
-                    "y": 6.613005243899921,
-                    "heading": -2.852261622202671,
-                    "angularVelocity": -0.3888842574798656,
-                    "velocityX": 2.8859159430424604,
-                    "velocityY": -0.6489713004840627,
-                    "timestamp": 0.3550391356132234
-                },
-                {
-                    "x": 3.260353111631952,
-                    "y": 6.541052974820016,
-                    "heading": -2.8718469111116423,
-                    "angularVelocity": -0.22065498638783787,
-                    "velocityX": 3.6065926859272044,
-                    "velocityY": -0.8106404265054099,
-                    "timestamp": 0.4437989195165293
-                },
-                {
-                    "x": 3.5879291031343383,
-                    "y": 6.467313799666466,
-                    "heading": -2.87184692127097,
-                    "angularVelocity": -1.1445867602426544e-7,
-                    "velocityX": 3.690590232387729,
-                    "velocityY": -0.83077236008012,
-                    "timestamp": 0.5325587034198351
-                },
-                {
-                    "x": 3.915505094528652,
-                    "y": 6.3935746240328,
-                    "heading": -2.87184693143029,
-                    "angularVelocity": -1.1445859480672728e-7,
-                    "velocityX": 3.6905902311701486,
-                    "velocityY": -0.8307723654892717,
-                    "timestamp": 0.621318487323141
-                },
-                {
-                    "x": 4.243081085922966,
-                    "y": 6.3198354483991315,
-                    "heading": -2.87184694158961,
-                    "angularVelocity": -1.1445859516237229e-7,
-                    "velocityX": 3.6905902311701433,
-                    "velocityY": -0.8307723654892962,
-                    "timestamp": 0.7100782712264468
-                },
-                {
-                    "x": 4.5706570773172785,
-                    "y": 6.246096272765464,
-                    "heading": -2.87184695174893,
-                    "angularVelocity": -1.1445859514834922e-7,
-                    "velocityX": 3.6905902311701433,
-                    "velocityY": -0.830772365489296,
-                    "timestamp": 0.7988380551297527
-                },
-                {
-                    "x": 4.898233068711592,
-                    "y": 6.172357097131795,
-                    "heading": -2.8718469619082505,
-                    "angularVelocity": -1.1445859490669181e-7,
-                    "velocityX": 3.6905902311701437,
-                    "velocityY": -0.830772365489296,
-                    "timestamp": 0.8875978390330586
-                },
-                {
-                    "x": 5.225809060105905,
-                    "y": 6.0986179214981275,
-                    "heading": -2.8718469720675706,
-                    "angularVelocity": -1.1445859563440553e-7,
-                    "velocityX": 3.6905902311701433,
-                    "velocityY": -0.830772365489296,
-                    "timestamp": 0.9763576229363644
-                },
-                {
-                    "x": 5.553385051500219,
-                    "y": 6.024878745864458,
-                    "heading": -2.871846982226891,
-                    "angularVelocity": -1.1445859523892014e-7,
-                    "velocityX": 3.6905902311701437,
-                    "velocityY": -0.830772365489296,
-                    "timestamp": 1.0651174068396703
-                },
-                {
-                    "x": 5.8809610428945325,
-                    "y": 5.951139570230791,
-                    "heading": -2.871846992386211,
-                    "angularVelocity": -1.1445859598937227e-7,
-                    "velocityX": 3.6905902311701433,
-                    "velocityY": -0.8307723654892962,
-                    "timestamp": 1.1538771907429761
-                },
-                {
-                    "x": 6.208537034288882,
-                    "y": 5.877400394597284,
-                    "heading": -2.871847002545531,
-                    "angularVelocity": -1.1445859482620514e-7,
-                    "velocityX": 3.6905902311705536,
-                    "velocityY": -0.830772365487475,
-                    "timestamp": 1.242636974646282
-                },
-                {
-                    "x": 6.536113033734776,
-                    "y": 5.80366125473165,
-                    "heading": -2.8718470127048517,
-                    "angularVelocity": -1.1445859560539411e-7,
-                    "velocityX": 3.6905903218821825,
-                    "velocityY": -0.8307719625136204,
-                    "timestamp": 1.3313967585495878
-                },
-                {
-                    "x": 6.865344430510141,
-                    "y": 5.737705340336343,
-                    "heading": -2.8718470255455273,
-                    "angularVelocity": -1.4466772317254716e-7,
-                    "velocityX": 3.7092406301262124,
-                    "velocityY": -0.7430833142536372,
-                    "timestamp": 1.4201565424528937
-                },
-                {
-                    "x": 7.155715453065551,
-                    "y": 5.718947060788103,
-                    "heading": -2.9583296823858576,
-                    "angularVelocity": -0.974345058506937,
-                    "velocityX": 3.2714255238807013,
-                    "velocityY": -0.21133759821538473,
-                    "timestamp": 1.5089163263561995
-                },
-                {
-                    "x": 7.382461638266762,
-                    "y": 5.716006595883618,
-                    "heading": -3.030136340606869,
-                    "angularVelocity": -0.8089999216225757,
-                    "velocityX": 2.5546049711908463,
-                    "velocityY": -0.0331283468162716,
-                    "timestamp": 1.5976761102595054
-                },
-                {
-                    "x": 7.54544918703291,
-                    "y": 5.728176149550433,
-                    "heading": -3.085282134377374,
-                    "angularVelocity": -0.6212925645535633,
-                    "velocityX": 1.836276989404401,
-                    "velocityY": 0.13710661666404886,
-                    "timestamp": 1.6864358941628113
-                },
-                {
-                    "x": 7.644635592677741,
-                    "y": 5.7552144852221,
-                    "heading": -3.1230806073530184,
-                    "angularVelocity": -0.4258513407020198,
-                    "velocityX": 1.117470111834477,
-                    "velocityY": 0.3046237212691066,
-                    "timestamp": 1.775195678066117
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -0.2265068586519956,
-                    "velocityX": 0.39842827198390307,
-                    "velocityY": 0.4707708034014574,
-                    "timestamp": 1.863955461969423
-                },
-                {
-                    "x": 7.649158607972273,
-                    "y": 5.85526478345475,
-                    "heading": -3.144984703286767,
-                    "angularVelocity": -0.019773386750234018,
-                    "velocityX": -0.3389130220112019,
-                    "velocityY": 0.640265971773451,
-                    "timestamp": 1.9549563678794555
-                },
-                {
-                    "x": 7.551198365445194,
-                    "y": 5.928933141126823,
-                    "heading": -3.12869095344792,
-                    "angularVelocity": 0.17905041357452225,
-                    "velocityX": -1.076475465243452,
-                    "velocityY": 0.8095343330417561,
-                    "timestamp": 2.045957273789488
-                },
-                {
-                    "x": 7.386086795598405,
-                    "y": 6.0179701455014545,
-                    "heading": -3.0955113223095805,
-                    "angularVelocity": 0.3646077015007144,
-                    "velocityX": -1.814394793058725,
-                    "velocityY": 0.9784188792874091,
-                    "timestamp": 2.1369581796995205
-                },
-                {
-                    "x": 7.153763389046842,
-                    "y": 6.12230392350914,
-                    "heading": -3.047867696260251,
-                    "angularVelocity": 0.5235511182320745,
-                    "velocityX": -2.5529790525519296,
-                    "velocityY": 1.1465136194450147,
-                    "timestamp": 2.227959085609553
-                },
-                {
-                    "x": 6.854082785517415,
-                    "y": 6.241701438550574,
-                    "heading": -2.993162545915138,
-                    "angularVelocity": 0.6011495138212849,
-                    "velocityX": -3.29316066178182,
-                    "velocityY": 1.3120475433450827,
-                    "timestamp": 2.3189599915195855
-                },
-                {
-                    "x": 6.518695058786276,
-                    "y": 6.319314800630408,
-                    "heading": -2.9931625344725843,
-                    "angularVelocity": 1.257410975035093e-7,
-                    "velocityX": -3.685542724846253,
-                    "velocityY": 0.8528855982660521,
-                    "timestamp": 2.409960897429618
-                },
-                {
-                    "x": 6.18330715393461,
-                    "y": 6.396927393000049,
-                    "heading": -2.993162523030137,
-                    "angularVelocity": 1.2573992931558898e-7,
-                    "velocityX": -3.6855446821952005,
-                    "velocityY": 0.8528771399965199,
-                    "timestamp": 2.5009618033396506
-                },
-                {
-                    "x": 5.8479121197525,
-                    "y": 6.474508873089667,
-                    "heading": -2.9931623885517284,
-                    "angularVelocity": 0.0000014777699956967743,
-                    "velocityX": -3.685623025705882,
-                    "velocityY": 0.852535250212982,
-                    "timestamp": 2.591962709249683
-                },
-                {
-                    "x": 5.579069881205996,
-                    "y": 6.536724652078007,
-                    "heading": -2.956924011318997,
-                    "angularVelocity": 0.3982199613326688,
-                    "velocityX": -2.954280903668077,
-                    "velocityY": 0.6836830728899436,
-                    "timestamp": 2.6829636151597156
-                },
-                {
-                    "x": 5.377498755107576,
-                    "y": 6.583371353425055,
-                    "heading": -2.9212816182082424,
-                    "angularVelocity": 0.391670750464755,
-                    "velocityX": -2.2150452688647118,
-                    "velocityY": 0.5125960107821789,
-                    "timestamp": 2.773964521069748
-                },
-                {
-                    "x": 5.243156221242747,
-                    "y": 6.614459642803244,
-                    "heading": -2.894437298256596,
-                    "angularVelocity": 0.29498959030348726,
-                    "velocityX": -1.4762768845141638,
-                    "velocityY": 0.341626152699224,
-                    "timestamp": 2.8649654269797806
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "angularVelocity": 0.15865004982334352,
-                    "velocityX": -0.7379731066539015,
-                    "velocityY": 0.17077145596902096,
-                    "timestamp": 2.955966332889813
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "angularVelocity": 0,
-                    "velocityX": 2.398158538123596e-33,
-                    "velocityY": 0,
-                    "timestamp": 3.0469672387998457
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -3075,362 +1618,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W1 To C2 To S2": {
+        "W2 To S3": {
             "waypoints": [
                 {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": -2.75,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 21
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": -3.143185307179586,
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 14
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 4.263,
-                    "y": 5.56,
-                    "heading": -3.143185307179586,
+                    "y": 3,
+                    "heading": 2.574,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": -2.75,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.36406618503168,
-                    "y": 6.750841975187211,
-                    "heading": -2.7578611838826945,
-                    "angularVelocity": -0.0894504040603958,
-                    "velocityX": 0.7289927602771729,
-                    "velocityY": -0.07007059190289512,
-                    "timestamp": 0.08788315676457649
-                },
-                {
-                    "x": 2.4922026900458016,
-                    "y": 6.738526737153379,
-                    "heading": -2.773150566031247,
-                    "angularVelocity": -0.17397397534899775,
-                    "velocityX": 1.458032571103211,
-                    "velocityY": -0.1401319489105608,
-                    "timestamp": 0.17576631352915298
-                },
-                {
-                    "x": 2.6844159261129263,
-                    "y": 6.720055698035347,
-                    "heading": -2.7951433359678384,
-                    "angularVelocity": -0.25025011328969643,
-                    "velocityX": 2.187145331864103,
-                    "velocityY": -0.21017723757365894,
-                    "timestamp": 0.2636494702937295
-                },
-                {
-                    "x": 2.940716876843772,
-                    "y": 6.6954319085352205,
-                    "heading": -2.8223782521748206,
-                    "angularVelocity": -0.3098991571267751,
-                    "velocityX": 2.9163830723267137,
-                    "velocityY": -0.2801878130765108,
-                    "timestamp": 0.35153262705830596
-                },
-                {
-                    "x": 3.261123048295386,
-                    "y": 6.664666336504878,
-                    "heading": -2.850384657388346,
-                    "angularVelocity": -0.318677733533739,
-                    "velocityX": 3.645820009742318,
-                    "velocityY": -0.35007358819345447,
-                    "timestamp": 0.43941578382288243
-                },
-                {
-                    "x": 3.5920521261120903,
-                    "y": 6.632831781454392,
-                    "heading": -2.8503846627195863,
-                    "angularVelocity": -6.066282061484616e-8,
-                    "velocityX": 3.7655574742633027,
-                    "velocityY": -0.3622372730165362,
-                    "timestamp": 0.527298940587459
-                },
-                {
-                    "x": 3.922981203901898,
-                    "y": 6.6009972261242424,
-                    "heading": -2.850384668050817,
-                    "angularVelocity": -6.066271266149535e-8,
-                    "velocityX": 3.7655574739572506,
-                    "velocityY": -0.3622372761987803,
-                    "timestamp": 0.6151820973520354
-                },
-                {
-                    "x": 4.253910281691706,
-                    "y": 6.56916267079409,
-                    "heading": -2.8503846733820475,
-                    "angularVelocity": -6.06627128107855e-8,
-                    "velocityX": 3.765557473957249,
-                    "velocityY": -0.36223727619879537,
-                    "timestamp": 0.7030652541166119
-                },
-                {
-                    "x": 4.5848393594815136,
-                    "y": 6.537328115463938,
-                    "heading": -2.850384678713278,
-                    "angularVelocity": -6.066271279819625e-8,
-                    "velocityX": 3.765557473957249,
-                    "velocityY": -0.3622372761987953,
-                    "timestamp": 0.7909484108811884
-                },
-                {
-                    "x": 4.915768437271321,
-                    "y": 6.505493560133786,
-                    "heading": -2.850384684044509,
-                    "angularVelocity": -6.066271362462515e-8,
-                    "velocityX": 3.7655574739572493,
-                    "velocityY": -0.3622372761987953,
-                    "timestamp": 0.8788315676457649
-                },
-                {
-                    "x": 5.24669751506113,
-                    "y": 6.473659004803634,
-                    "heading": -2.8503846893757396,
-                    "angularVelocity": -6.066271352975027e-8,
-                    "velocityX": 3.765557473957249,
-                    "velocityY": -0.36223727619879525,
-                    "timestamp": 0.9667147244103413
-                },
-                {
-                    "x": 5.577626592850938,
-                    "y": 6.441824449473482,
-                    "heading": -2.85038469470697,
-                    "angularVelocity": -6.066271274322426e-8,
-                    "velocityX": 3.765557473957249,
-                    "velocityY": -0.3622372761987953,
-                    "timestamp": 1.054597881174918
-                },
-                {
-                    "x": 5.908555670640745,
-                    "y": 6.40998989414333,
-                    "heading": -2.8503847000382008,
-                    "angularVelocity": -6.066271341664136e-8,
-                    "velocityX": 3.7655574739572493,
-                    "velocityY": -0.36223727619879537,
-                    "timestamp": 1.1424810379394945
-                },
-                {
-                    "x": 6.239484748430477,
-                    "y": 6.37815533881239,
-                    "heading": -2.8503847053694313,
-                    "angularVelocity": -6.066271364618301e-8,
-                    "velocityX": 3.7655574739563864,
-                    "velocityY": -0.36223727620776275,
-                    "timestamp": 1.230364194704071
-                },
-                {
-                    "x": 6.5704138100618055,
-                    "y": 6.346320615510802,
-                    "heading": -2.8503847107006623,
-                    "angularVelocity": -6.066271546575477e-8,
-                    "velocityX": 3.7655572900940437,
-                    "velocityY": -0.3622391875028854,
-                    "timestamp": 1.3182473514686477
-                },
-                {
-                    "x": 6.897540531836615,
-                    "y": 6.287028122680583,
-                    "heading": -2.8503847211917432,
-                    "angularVelocity": -1.1937532942487347e-7,
-                    "velocityX": 3.722291435788134,
-                    "velocityY": -0.6746741356714466,
-                    "timestamp": 1.4061305082332243
-                },
-                {
-                    "x": 7.182619204890035,
-                    "y": 6.1828768490539545,
-                    "heading": -2.9185193386552397,
-                    "angularVelocity": -0.7752864140510717,
-                    "velocityX": 3.243837426289712,
-                    "velocityY": -1.1851107477355587,
-                    "timestamp": 1.4940136649978009
-                },
-                {
-                    "x": 7.403317140168432,
-                    "y": 6.080132379818418,
-                    "heading": -2.984427246863809,
-                    "angularVelocity": -0.749949258025913,
-                    "velocityX": 2.511265450666594,
-                    "velocityY": -1.1691030797945798,
-                    "timestamp": 1.5818968217623774
-                },
-                {
-                    "x": 7.559743583293149,
-                    "y": 5.981103818181206,
-                    "heading": -3.044561457345504,
-                    "angularVelocity": -0.6842518258963323,
-                    "velocityX": 1.7799365530730349,
-                    "velocityY": -1.1268207160843378,
-                    "timestamp": 1.669779978526954
-                },
-                {
-                    "x": 7.6519593983078185,
-                    "y": 5.886591133773424,
-                    "heading": -3.097689544379845,
-                    "angularVelocity": -0.6045309361913614,
-                    "velocityX": 1.0493002118904227,
-                    "velocityY": -1.0754357022127217,
-                    "timestamp": 1.7576631352915306
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -0.5176846676276772,
-                    "velocityX": 0.3190668465323417,
-                    "velocityY": -1.0194346342544611,
-                    "timestamp": 1.8455462920561072
-                },
-                {
-                    "x": 7.628959612668781,
-                    "y": 5.701821981186599,
-                    "heading": -3.184554593580224,
-                    "angularVelocity": -0.41404680845855485,
-                    "velocityX": -0.5108405610944724,
-                    "velocityY": -0.9525945055820302,
-                    "timestamp": 1.9454608078922528
-                },
-                {
-                    "x": 7.495051570255288,
-                    "y": 5.613939825179433,
-                    "heading": -3.215560246622884,
-                    "angularVelocity": -0.3103218064280789,
-                    "velocityX": -1.3402261052147335,
-                    "velocityY": -0.8795734560860885,
-                    "timestamp": 2.0453753237283987
-                },
-                {
-                    "x": 7.278385319183743,
-                    "y": 5.534503531789237,
-                    "heading": -3.2361878862694873,
-                    "angularVelocity": -0.2064528809850976,
-                    "velocityX": -2.1685162486986918,
-                    "velocityY": -0.7950425693946834,
-                    "timestamp": 2.1452898395645446
-                },
-                {
-                    "x": 6.979307104157413,
-                    "y": 5.4664019474569185,
-                    "heading": -3.246410388494101,
-                    "angularVelocity": -0.10231248321693356,
-                    "velocityX": -2.9933409827737334,
-                    "velocityY": -0.6815985021035508,
-                    "timestamp": 2.2452043554006904
-                },
-                {
-                    "x": 6.603136214940544,
-                    "y": 5.429560638127869,
-                    "heading": -3.2464103824694632,
-                    "angularVelocity": 6.029792436770947e-8,
-                    "velocityX": -3.7649273087983395,
-                    "velocityY": -0.368728297592585,
-                    "timestamp": 2.3451188712368363
-                },
-                {
-                    "x": 6.225751326953569,
-                    "y": 5.450595811881877,
-                    "heading": -3.2464103781263827,
-                    "angularVelocity": 4.346796393735324e-8,
-                    "velocityX": -3.777077683145315,
-                    "velocityY": 0.2105317088110093,
-                    "timestamp": 2.445033387072982
-                },
-                {
-                    "x": 5.848366461127548,
-                    "y": 5.47163138321431,
-                    "heading": -3.2464103737833025,
-                    "angularVelocity": 4.346795775537738e-8,
-                    "velocityX": -3.777077461346165,
-                    "velocityY": 0.21053568799683506,
-                    "timestamp": 2.544947902909128
-                },
-                {
-                    "x": 5.470981595301597,
-                    "y": 5.492666954547974,
-                    "heading": -3.246410369440223,
-                    "angularVelocity": 4.346795748687552e-8,
-                    "velocityX": -3.777077461345479,
-                    "velocityY": 0.21053568800914468,
-                    "timestamp": 2.644862418745274
-                },
-                {
-                    "x": 5.09359672947572,
-                    "y": 5.513702525882575,
-                    "heading": -3.246410365097125,
-                    "angularVelocity": 4.346813479097949e-8,
-                    "velocityX": -3.777077461344731,
-                    "velocityY": 0.21053568801853617,
-                    "timestamp": 2.7447769345814197
-                },
-                {
-                    "x": 4.761358057173187,
-                    "y": 5.532221526743186,
-                    "heading": -3.20512020544725,
-                    "angularVelocity": 0.4132548639637959,
-                    "velocityX": -3.3252292674608595,
-                    "velocityY": 0.18534845218066273,
-                    "timestamp": 2.8446914504175655
-                },
-                {
-                    "x": 4.5121790336148475,
-                    "y": 5.546110766147867,
-                    "heading": -3.174152719009984,
-                    "angularVelocity": 0.3099398138309664,
-                    "velocityX": -2.4939221440754293,
-                    "velocityY": 0.13901122663156756,
-                    "timestamp": 2.9446059662537114
-                },
-                {
-                    "x": 4.346059678819821,
-                    "y": 5.555370255329891,
-                    "heading": -3.153507786492084,
-                    "angularVelocity": 0.20662595765119826,
-                    "velocityX": -1.6626148203275395,
-                    "velocityY": 0.09267411351128947,
-                    "timestamp": 3.0445204820898573
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": 0.10331310947276948,
-                    "velocityX": -0.8313074243990123,
-                    "velocityY": 0.04633705754727876,
-                    "timestamp": 3.144434997926003
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": 0,
-                    "velocityX": 1.3822008000428574e-34,
-                    "velocityY": 0,
-                    "timestamp": 3.244349513762149
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -3450,398 +1659,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W1 To C3 To S1": {
+        "W2 To S4": {
             "waypoints": [
                 {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": -2.75,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 23
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": -3.143185307179586,
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 16
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
+                    "y": 1.62,
+                    "heading": 2.5,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": -2.75,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.352643709641578,
-                    "y": 6.724306544750454,
-                    "heading": -2.7643107246230096,
-                    "angularVelocity": -0.16582604780361404,
-                    "velocityX": 0.6100109212961704,
-                    "velocityY": -0.37883661491424325,
-                    "timestamp": 0.08629961825889286
-                },
-                {
-                    "x": 2.457948690141224,
-                    "y": 6.658909154192408,
-                    "heading": -2.791757157392081,
-                    "angularVelocity": -0.31803654897277683,
-                    "velocityX": 1.2202253338341789,
-                    "velocityY": -0.7577946678969769,
-                    "timestamp": 0.17259923651778572
-                },
-                {
-                    "x": 2.6159410271495345,
-                    "y": 6.560792064019538,
-                    "heading": -2.830360502782766,
-                    "angularVelocity": -0.44731768423791834,
-                    "velocityX": 1.8307420148076843,
-                    "velocityY": -1.136935390358925,
-                    "timestamp": 0.25889885477667857
-                },
-                {
-                    "x": 2.826660648973102,
-                    "y": 6.4299308538855104,
-                    "heading": -2.87607731089876,
-                    "angularVelocity": -0.5297451951435223,
-                    "velocityX": 2.4417213665020787,
-                    "velocityY": -1.516359084482017,
-                    "timestamp": 0.34519847303557144
-                },
-                {
-                    "x": 3.0901174136120235,
-                    "y": 6.266320048368765,
-                    "heading": -2.9160689628763303,
-                    "angularVelocity": -0.46340473786276914,
-                    "velocityX": 3.052814948135086,
-                    "velocityY": -1.8958462252537605,
-                    "timestamp": 0.4314980912944643
-                },
-                {
-                    "x": 3.3674552739873977,
-                    "y": 6.094087733168023,
-                    "heading": -2.916068973852836,
-                    "angularVelocity": -1.2719066550138966e-7,
-                    "velocityX": 3.213662655416737,
-                    "velocityY": -1.995748285747182,
-                    "timestamp": 0.5177977095533571
-                },
-                {
-                    "x": 3.6447931343598987,
-                    "y": 5.921855417962633,
-                    "heading": -2.9160689848293226,
-                    "angularVelocity": -1.2719044142347595e-7,
-                    "velocityX": 3.213662655383448,
-                    "velocityY": -1.9957482858010414,
-                    "timestamp": 0.60409732781225
-                },
-                {
-                    "x": 3.9221309947324,
-                    "y": 5.749623102757243,
-                    "heading": -2.9160689958058095,
-                    "angularVelocity": -1.2719044074317096e-7,
-                    "velocityX": 3.2136626553834478,
-                    "velocityY": -1.9957482858010414,
-                    "timestamp": 0.6903969460711429
-                },
-                {
-                    "x": 4.199468855104901,
-                    "y": 5.577390787551853,
-                    "heading": -2.916069006782296,
-                    "angularVelocity": -1.2719044064772273e-7,
-                    "velocityX": 3.213662655383448,
-                    "velocityY": -1.995748285801041,
-                    "timestamp": 0.7766965643300358
-                },
-                {
-                    "x": 4.476806715477402,
-                    "y": 5.405158472346463,
-                    "heading": -2.9160690177587827,
-                    "angularVelocity": -1.271904417367868e-7,
-                    "velocityX": 3.213662655383448,
-                    "velocityY": -1.9957482858010416,
-                    "timestamp": 0.8629961825889286
-                },
-                {
-                    "x": 4.754144575849903,
-                    "y": 5.2329261571410735,
-                    "heading": -2.9160690287352695,
-                    "angularVelocity": -1.2719044128845816e-7,
-                    "velocityX": 3.2136626553834478,
-                    "velocityY": -1.9957482858010416,
-                    "timestamp": 0.9492958008478215
-                },
-                {
-                    "x": 5.031482436222404,
-                    "y": 5.0606938419356835,
-                    "heading": -2.9160690397117563,
-                    "angularVelocity": -1.271904407298397e-7,
-                    "velocityX": 3.2136626553834486,
-                    "velocityY": -1.995748285801041,
-                    "timestamp": 1.0355954191067143
-                },
-                {
-                    "x": 5.308820296594906,
-                    "y": 4.8884615267302935,
-                    "heading": -2.9160690506882427,
-                    "angularVelocity": -1.2719044158933254e-7,
-                    "velocityX": 3.2136626553834478,
-                    "velocityY": -1.995748285801042,
-                    "timestamp": 1.1218950373656071
-                },
-                {
-                    "x": 5.586158156967406,
-                    "y": 4.716229211524904,
-                    "heading": -2.9160690616647296,
-                    "angularVelocity": -1.271904410743158e-7,
-                    "velocityX": 3.2136626553834478,
-                    "velocityY": -1.995748285801042,
-                    "timestamp": 1.2081946556245
-                },
-                {
-                    "x": 5.863496017339908,
-                    "y": 4.543996896319514,
-                    "heading": -2.9160690726412164,
-                    "angularVelocity": -1.2719044055646472e-7,
-                    "velocityX": 3.213662655383448,
-                    "velocityY": -1.9957482858010414,
-                    "timestamp": 1.2944942738833929
-                },
-                {
-                    "x": 6.140833877716964,
-                    "y": 4.371764581121458,
-                    "heading": -2.916069083617703,
-                    "angularVelocity": -1.2719044131717392e-7,
-                    "velocityX": 3.213662655436225,
-                    "velocityY": -1.9957482857160567,
-                    "timestamp": 1.3807938921422858
-                },
-                {
-                    "x": 6.418172664892274,
-                    "y": 4.19953375831344,
-                    "heading": -2.9160690945941994,
-                    "angularVelocity": -1.2719055821939482e-7,
-                    "velocityX": 3.21367339474496,
-                    "velocityY": -1.9957309925918116,
-                    "timestamp": 1.4670935104011786
-                },
-                {
-                    "x": 6.7207652632777295,
-                    "y": 4.0769859125864,
-                    "heading": -2.916069118255731,
-                    "angularVelocity": -2.741788669167458e-7,
-                    "velocityX": 3.5063028607839835,
-                    "velocityY": -1.4200276687058784,
-                    "timestamp": 1.5533931286600715
-                },
-                {
-                    "x": 7.011920641126383,
-                    "y": 4.012803875379112,
-                    "heading": -2.9821933696709393,
-                    "angularVelocity": -0.7662171948025339,
-                    "velocityX": 3.3737736472290445,
-                    "velocityY": -0.7437117162469018,
-                    "timestamp": 1.6396927469189644
-                },
-                {
-                    "x": 7.255025953055537,
-                    "y": 3.987925238494502,
-                    "heading": -3.0423989255025554,
-                    "angularVelocity": -0.6976340920894686,
-                    "velocityX": 2.8169917414808885,
-                    "velocityY": -0.28828211973553797,
-                    "timestamp": 1.7259923651778573
-                },
-                {
-                    "x": 7.447850515704627,
-                    "y": 3.9993347769870677,
-                    "heading": -3.0905963113163932,
-                    "angularVelocity": -0.5584889804459283,
-                    "velocityX": 2.234361710265075,
-                    "velocityY": 0.13220844684348906,
-                    "timestamp": 1.8122919834367501
-                },
-                {
-                    "x": 7.5896354595479805,
-                    "y": 4.045892337030357,
-                    "heading": -3.124573318920613,
-                    "angularVelocity": -0.3937098250312271,
-                    "velocityX": 1.6429382505152235,
-                    "velocityY": 0.539487439032544,
-                    "timestamp": 1.898591601695643
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -0.21566709836621822,
-                    "velocityX": 1.047102435377472,
-                    "velocityY": 0.9398380271749195,
-                    "timestamp": 1.9848912199545359
-                },
-                {
-                    "x": 7.718344137255122,
-                    "y": 4.246200601691832,
-                    "heading": -3.145373267573727,
-                    "angularVelocity": -0.024710376357260012,
-                    "velocityX": 0.43305082947346774,
-                    "velocityY": 1.34622717133274,
-                    "timestamp": 2.073435415339313
-                },
-                {
-                    "x": 7.702075707426083,
-                    "y": 4.401035710087067,
-                    "heading": -3.1311779977271272,
-                    "angularVelocity": 0.16031846906115368,
-                    "velocityX": -0.183732313080062,
-                    "velocityY": 1.7486759885425491,
-                    "timestamp": 2.16197961072409
-                },
-                {
-                    "x": 7.630801467153219,
-                    "y": 4.590926342055076,
-                    "heading": -3.10146947436901,
-                    "angularVelocity": 0.33552197554576096,
-                    "velocityX": -0.804956665562769,
-                    "velocityY": 2.1445858889079887,
-                    "timestamp": 2.250523806108867
-                },
-                {
-                    "x": 7.503771169060967,
-                    "y": 4.814724539953898,
-                    "heading": -3.057954484569731,
-                    "angularVelocity": 0.49144937855435067,
-                    "velocityX": -1.4346541581807906,
-                    "velocityY": 2.527530990879987,
-                    "timestamp": 2.339068001493644
-                },
-                {
-                    "x": 7.31900648308238,
-                    "y": 5.069082680437073,
-                    "heading": -3.0055052448244375,
-                    "angularVelocity": 0.5923509667139643,
-                    "velocityX": -2.086694505218799,
-                    "velocityY": 2.872668720699063,
-                    "timestamp": 2.427612196878421
-                },
-                {
-                    "x": 7.075378945667608,
-                    "y": 5.298956768781039,
-                    "heading": -3.0055052272028773,
-                    "angularVelocity": 1.990142925919268e-7,
-                    "velocityX": -2.75147948837462,
-                    "velocityY": 2.5961508526209696,
-                    "timestamp": 2.516156392263198
-                },
-                {
-                    "x": 6.801072699384408,
-                    "y": 5.491187253021094,
-                    "heading": -3.005505216579771,
-                    "angularVelocity": 1.1997518482429124e-7,
-                    "velocityX": -3.097958540262374,
-                    "velocityY": 2.1710117010503502,
-                    "timestamp": 2.604700587647975
-                },
-                {
-                    "x": 6.5267661173867575,
-                    "y": 5.683417258207369,
-                    "heading": -3.005505205956667,
-                    "angularVelocity": 1.199751543060781e-7,
-                    "velocityX": -3.0979623317525853,
-                    "velocityY": 2.1710062907151193,
-                    "timestamp": 2.693244783032752
-                },
-                {
-                    "x": 6.252459535387558,
-                    "y": 5.875647263391433,
-                    "heading": -3.005505195333564,
-                    "angularVelocity": 1.199751539565171e-7,
-                    "velocityX": -3.0979623317700846,
-                    "velocityY": 2.1710062906901486,
-                    "timestamp": 2.781788978417529
-                },
-                {
-                    "x": 5.978152953372341,
-                    "y": 6.067877268552632,
-                    "heading": -3.0055051847104526,
-                    "angularVelocity": 1.1997524424086504e-7,
-                    "velocityX": -3.0979623319509813,
-                    "velocityY": 2.171006290431916,
-                    "timestamp": 2.870333173802306
-                },
-                {
-                    "x": 5.71076650313837,
-                    "y": 6.255250599668089,
-                    "heading": -2.984811425443856,
-                    "angularVelocity": 0.23371107700143318,
-                    "velocityX": -3.0198077815501345,
-                    "velocityY": 2.1161560088926668,
-                    "timestamp": 2.958877369187083
-                },
-                {
-                    "x": 5.496808427326641,
-                    "y": 6.405185542172583,
-                    "heading": -2.9493217674847187,
-                    "angularVelocity": 0.40081292520855316,
-                    "velocityX": -2.4163986682814493,
-                    "velocityY": 1.6933345190325848,
-                    "timestamp": 3.04742156457186
-                },
-                {
-                    "x": 5.33638152944714,
-                    "y": 6.517608311818503,
-                    "heading": -2.9168038375204985,
-                    "angularVelocity": 0.3672508381444,
-                    "velocityX": -1.811828513234718,
-                    "velocityY": 1.269679724997829,
-                    "timestamp": 3.135965759956637
-                },
-                {
-                    "x": 5.229454180923455,
-                    "y": 6.592540440066129,
-                    "heading": -2.8927984833645977,
-                    "angularVelocity": 0.27111155118061186,
-                    "velocityX": -1.2076155648493634,
-                    "velocityY": 0.8462681028637682,
-                    "timestamp": 3.224509955341414
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "angularVelocity": 0.144543448733038,
-                    "velocityX": -0.6037005666060847,
-                    "velocityY": 0.4230605944433498,
-                    "timestamp": 3.313054150726191
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "angularVelocity": 0,
-                    "velocityX": 1.048960687291352e-31,
-                    "velocityY": 0,
-                    "timestamp": 3.401598346110968
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -3861,389 +1700,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W1 To C3 To S2": {
+        "W2 To C1": {
             "waypoints": [
                 {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": -2.75,
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 23
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 7.68,
-                    "y": 4.127,
-                    "heading": -3.143185307179586,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": -3.143185307179586,
+                    "y": 7.467,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": -2.75,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.357227124880225,
-                    "y": 6.730410634555629,
-                    "heading": -2.7571910501011128,
-                    "angularVelocity": -0.08264039553460875,
-                    "velocityX": 0.6576608657862654,
-                    "velocityY": -0.30556812237992936,
-                    "timestamp": 0.08701616267193764
-                },
-                {
-                    "x": 2.4716821810223912,
-                    "y": 6.677231589623697,
-                    "heading": -2.771475854118176,
-                    "angularVelocity": -0.1641626518388192,
-                    "velocityX": 1.3153309986062809,
-                    "velocityY": -0.6111398537812316,
-                    "timestamp": 0.17403232534387528
-                },
-                {
-                    "x": 2.6433664887429877,
-                    "y": 6.597462350883563,
-                    "heading": -2.792692156200411,
-                    "angularVelocity": -0.2438202447770986,
-                    "velocityX": 1.973016304659045,
-                    "velocityY": -0.916717495815976,
-                    "timestamp": 0.26104848801581293
-                },
-                {
-                    "x": 2.8722826002398385,
-                    "y": 6.491101923690649,
-                    "heading": -2.820514979829771,
-                    "angularVelocity": -0.319743169257603,
-                    "velocityX": 2.630730940870083,
-                    "velocityY": -1.222306568423469,
-                    "timestamp": 0.34806465068775055
-                },
-                {
-                    "x": 3.1584374767810863,
-                    "y": 6.358147629132494,
-                    "heading": -2.8539648898748324,
-                    "angularVelocity": -0.38441030973950396,
-                    "velocityX": 3.288525576795303,
-                    "velocityY": -1.5279264274087752,
-                    "timestamp": 0.4350808133596882
-                },
-                {
-                    "x": 3.456964402226087,
-                    "y": 6.219442363897474,
-                    "heading": -2.853964894160219,
-                    "angularVelocity": -4.924816792193532e-8,
-                    "velocityX": 3.4307066213720128,
-                    "velocityY": -1.5940172604250262,
-                    "timestamp": 0.5220969760316259
-                },
-                {
-                    "x": 3.755491327665543,
-                    "y": 6.0807370986505,
-                    "heading": -2.853964898445594,
-                    "angularVelocity": -4.924803343036069e-8,
-                    "velocityX": 3.4307066213082953,
-                    "velocityY": -1.5940172605623917,
-                    "timestamp": 0.6091131387035635
-                },
-                {
-                    "x": 4.054018253104999,
-                    "y": 5.942031833403527,
-                    "heading": -2.853964902730969,
-                    "angularVelocity": -4.924803431145499e-8,
-                    "velocityX": 3.4307066213082953,
-                    "velocityY": -1.594017260562392,
-                    "timestamp": 0.6961293013755012
-                },
-                {
-                    "x": 4.352545178544456,
-                    "y": 5.803326568156554,
-                    "heading": -2.8539649070163438,
-                    "angularVelocity": -4.9248033877501264e-8,
-                    "velocityX": 3.4307066213082953,
-                    "velocityY": -1.5940172605623917,
-                    "timestamp": 0.7831454640474389
-                },
-                {
-                    "x": 4.651072103983911,
-                    "y": 5.66462130290958,
-                    "heading": -2.8539649113017185,
-                    "angularVelocity": -4.924803325887112e-8,
-                    "velocityX": 3.430706621308295,
-                    "velocityY": -1.5940172605623921,
-                    "timestamp": 0.8701616267193766
-                },
-                {
-                    "x": 4.949599029423368,
-                    "y": 5.525916037662607,
-                    "heading": -2.853964915587093,
-                    "angularVelocity": -4.924803367441165e-8,
-                    "velocityX": 3.430706621308295,
-                    "velocityY": -1.5940172605623926,
-                    "timestamp": 0.9571777893913143
-                },
-                {
-                    "x": 5.248125954862823,
-                    "y": 5.387210772415633,
-                    "heading": -2.853964919872468,
-                    "angularVelocity": -4.924803391655779e-8,
-                    "velocityX": 3.4307066213082953,
-                    "velocityY": -1.594017260562392,
-                    "timestamp": 1.044193952063252
-                },
-                {
-                    "x": 5.54665288030228,
-                    "y": 5.24850550716866,
-                    "heading": -2.8539649241578426,
-                    "angularVelocity": -4.9248034279126105e-8,
-                    "velocityX": 3.4307066213082953,
-                    "velocityY": -1.5940172605623923,
-                    "timestamp": 1.1312101147351896
-                },
-                {
-                    "x": 5.8451798057417355,
-                    "y": 5.109800241921686,
-                    "heading": -2.8539649284432174,
-                    "angularVelocity": -4.9248034369336404e-8,
-                    "velocityX": 3.4307066213082953,
-                    "velocityY": -1.594017260562393,
-                    "timestamp": 1.2182262774071273
-                },
-                {
-                    "x": 6.143706731181192,
-                    "y": 4.9710949766747135,
-                    "heading": -2.853964932728592,
-                    "angularVelocity": -4.9248033497376594e-8,
-                    "velocityX": 3.4307066213082953,
-                    "velocityY": -1.594017260562392,
-                    "timestamp": 1.305242440079065
-                },
-                {
-                    "x": 6.4422336566206475,
-                    "y": 4.83238971142774,
-                    "heading": -2.8539649370139673,
-                    "angularVelocity": -4.924803317735332e-8,
-                    "velocityX": 3.4307066213082953,
-                    "velocityY": -1.5940172605623926,
-                    "timestamp": 1.3922586027510027
-                },
-                {
-                    "x": 6.740760582057679,
-                    "y": 4.693684446175547,
-                    "heading": -2.853964941299342,
-                    "angularVelocity": -4.9248033793238546e-8,
-                    "velocityX": 3.4307066212804305,
-                    "velocityY": -1.5940172606223628,
-                    "timestamp": 1.4792747654229403
-                },
-                {
-                    "x": 7.039286998892109,
-                    "y": 4.554978086294829,
-                    "heading": -2.8539649455851244,
-                    "angularVelocity": -4.925271638681629e-8,
-                    "velocityX": 3.4307007763593735,
-                    "velocityY": -1.59402984022244,
-                    "timestamp": 1.566290928094878
-                },
-                {
-                    "x": 7.281877390378217,
-                    "y": 4.416206845653086,
-                    "heading": -2.926969050497618,
-                    "angularVelocity": -0.8389717803085356,
-                    "velocityX": 2.7878773786049877,
-                    "velocityY": -1.594775457577107,
-                    "timestamp": 1.6533070907668157
-                },
-                {
-                    "x": 7.4672438532140255,
-                    "y": 4.304023352017119,
-                    "heading": -2.9923953501231186,
-                    "angularVelocity": -0.7518867485821702,
-                    "velocityX": 2.130253244269864,
-                    "velocityY": -1.2892259344843033,
-                    "timestamp": 1.7403232534387534
-                },
-                {
-                    "x": 7.595386429544574,
-                    "y": 4.218427699418672,
-                    "heading": -3.050241491364391,
-                    "angularVelocity": -0.6647746747849547,
-                    "velocityX": 1.4726295942704664,
-                    "velocityY": -0.9836753307676307,
-                    "timestamp": 1.827339416110691
-                },
-                {
-                    "x": 7.666305140070344,
-                    "y": 4.159419912093254,
-                    "heading": -3.100505408798514,
-                    "angularVelocity": -0.5776388649040337,
-                    "velocityX": 0.8150061821634523,
-                    "velocityY": -0.6781244485335891,
-                    "timestamp": 1.9143555787826287
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -0.4904824238455614,
-                    "velocityX": 0.15738294483620113,
-                    "velocityY": -0.37257345184803875,
-                    "timestamp": 2.0013717414545664
-                },
-                {
-                    "x": 7.62294760973909,
-                    "y": 4.124186267487049,
-                    "heading": -3.1816074637236196,
-                    "angularVelocity": -0.39238397540812464,
-                    "velocityX": -0.5826441228371791,
-                    "velocityY": -0.02873507497949264,
-                    "timestamp": 2.099291531639857
-                },
-                {
-                    "x": 7.4934981410182715,
-                    "y": 4.155183222691002,
-                    "heading": -3.2104217744560906,
-                    "angularVelocity": -0.29426442476997366,
-                    "velocityX": -1.3219949560335675,
-                    "velocityY": 0.31655455087575607,
-                    "timestamp": 2.1972113218251477
-                },
-                {
-                    "x": 7.291774718335501,
-                    "y": 4.220253220991955,
-                    "heading": -3.2296266134452343,
-                    "angularVelocity": -0.19612826940094027,
-                    "velocityX": -2.06008838765949,
-                    "velocityY": 0.6645234653569294,
-                    "timestamp": 2.2951311120104383
-                },
-                {
-                    "x": 7.018085982963682,
-                    "y": 4.320043342217767,
-                    "heading": -3.2392208692548645,
-                    "angularVelocity": -0.09798076355633002,
-                    "velocityX": -2.795029838747879,
-                    "velocityY": 1.0191006438737447,
-                    "timestamp": 2.393050902195729
-                },
-                {
-                    "x": 6.674609525490048,
-                    "y": 4.458745905991077,
-                    "heading": -3.2392208627403147,
-                    "angularVelocity": 6.652945395302414e-8,
-                    "velocityX": -3.5077327762210677,
-                    "velocityY": 1.4164916357648132,
-                    "timestamp": 2.4909706923810195
-                },
-                {
-                    "x": 6.337654568768723,
-                    "y": 4.612615494980773,
-                    "heading": -3.239220858536144,
-                    "angularVelocity": 4.2934841578218656e-8,
-                    "velocityX": -3.4411323398846756,
-                    "velocityY": 1.5713839735413375,
-                    "timestamp": 2.58889048256631
-                },
-                {
-                    "x": 6.000699635853928,
-                    "y": 4.766485136103752,
-                    "heading": -3.239220854331973,
-                    "angularVelocity": 4.293484065869374e-8,
-                    "velocityX": -3.441132096761908,
-                    "velocityY": 1.5713845059493727,
-                    "timestamp": 2.686810272751601
-                },
-                {
-                    "x": 5.66374470293921,
-                    "y": 4.920354777226902,
-                    "heading": -3.239220850127803,
-                    "angularVelocity": 4.293484113596361e-8,
-                    "velocityX": -3.4411320967611103,
-                    "velocityY": 1.5713845059511196,
-                    "timestamp": 2.7847300629368914
-                },
-                {
-                    "x": 5.326789770024494,
-                    "y": 5.074224418350052,
-                    "heading": -3.239220845923632,
-                    "angularVelocity": 4.293484003264988e-8,
-                    "velocityX": -3.441132096761111,
-                    "velocityY": 1.571384505951117,
-                    "timestamp": 2.882649853122182
-                },
-                {
-                    "x": 4.989834837078673,
-                    "y": 5.228094059405012,
-                    "heading": -3.2392208417194372,
-                    "angularVelocity": 4.293508551285566e-8,
-                    "velocityX": -3.4411320970787416,
-                    "velocityY": 1.571384505254732,
-                    "timestamp": 2.9805696433074726
-                },
-                {
-                    "x": 4.699100923806666,
-                    "y": 5.360856424917345,
-                    "heading": -3.200805721128661,
-                    "angularVelocity": 0.39231212115635317,
-                    "velocityX": -2.9691026984622892,
-                    "velocityY": 1.3558277163493837,
-                    "timestamp": 3.0784894334927633
-                },
-                {
-                    "x": 4.481050467012654,
-                    "y": 5.460428209814717,
-                    "heading": -3.171995332755761,
-                    "angularVelocity": 0.2942243679074833,
-                    "velocityX": -2.2268272468864767,
-                    "velocityY": 1.016870897179783,
-                    "timestamp": 3.176409223678054
-                },
-                {
-                    "x": 4.335683489671324,
-                    "y": 5.5268094029690245,
-                    "heading": -3.152788680819891,
-                    "angularVelocity": 0.19614678401093785,
-                    "velocityX": -1.4845515606830426,
-                    "velocityY": 0.6779139643650731,
-                    "timestamp": 3.2743290138633445
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": 0.09807387885669308,
-                    "velocityX": -0.7422757905606995,
-                    "velocityY": 0.3389569868171628,
-                    "timestamp": 3.372248804048635
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": 0,
-                    "velocityX": 2.220393944714812e-34,
-                    "velocityY": 0,
-                    "timestamp": 3.4701685942339258
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -4263,407 +1741,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W1 To C3 To S3": {
+        "W2 To C2": {
             "waypoints": [
                 {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": -2.75,
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 23
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 7.68,
-                    "y": 4.127,
-                    "heading": -3.143185307179586,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 17
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -3.7091853071795864,
+                    "y": 5.797,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": -2.75,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.3574291714983375,
-                    "y": 6.741815654710436,
-                    "heading": -2.758007728773179,
-                    "angularVelocity": -0.09483878465079744,
-                    "velocityX": 0.6801570061474024,
-                    "velocityY": -0.17983436924136462,
-                    "timestamp": 0.08443516861442411
-                },
-                {
-                    "x": 2.47228751228047,
-                    "y": 6.711446972989247,
-                    "heading": -2.7740235521881464,
-                    "angularVelocity": -0.18968190243216043,
-                    "velocityX": 1.3603139860670759,
-                    "velocityY": -0.35966863357457735,
-                    "timestamp": 0.16887033722884823
-                },
-                {
-                    "x": 2.6445750170696067,
-                    "y": 6.665893967918211,
-                    "heading": -2.798048184372895,
-                    "angularVelocity": -0.2845334779214143,
-                    "velocityX": 2.0404709034915762,
-                    "velocityY": -0.5395027429749605,
-                    "timestamp": 0.2533055058432723
-                },
-                {
-                    "x": 2.8742916749054777,
-                    "y": 6.605156656477843,
-                    "heading": -2.830082761948688,
-                    "angularVelocity": -0.37939851487799736,
-                    "velocityX": 2.720627691109149,
-                    "velocityY": -0.7193366512682123,
-                    "timestamp": 0.33774067445769645
-                },
-                {
-                    "x": 3.161437459702618,
-                    "y": 6.529235060521681,
-                    "heading": -2.870128852530637,
-                    "angularVelocity": -0.4742821177371166,
-                    "velocityX": 3.400784169785955,
-                    "velocityY": -0.8991703007411442,
-                    "timestamp": 0.4221758430721206
-                },
-                {
-                    "x": 3.470239032500689,
-                    "y": 6.44758707798969,
-                    "heading": -2.8701288573799584,
-                    "angularVelocity": -5.743248864696825e-8,
-                    "velocityX": 3.6572624637989906,
-                    "velocityY": -0.9669902230531149,
-                    "timestamp": 0.5066110116865447
-                },
-                {
-                    "x": 3.77904060528322,
-                    "y": 6.3659390953988515,
-                    "heading": -2.8701288622292584,
-                    "angularVelocity": -5.743222732502049e-8,
-                    "velocityX": 3.6572624636149422,
-                    "velocityY": -0.9669902237500612,
-                    "timestamp": 0.5910461803009688
-                },
-                {
-                    "x": 4.087842178065752,
-                    "y": 6.284291112808013,
-                    "heading": -2.870128867078558,
-                    "angularVelocity": -5.743222733813293e-8,
-                    "velocityX": 3.6572624636149413,
-                    "velocityY": -0.9669902237500647,
-                    "timestamp": 0.6754813489153929
-                },
-                {
-                    "x": 4.396643750848283,
-                    "y": 6.202643130217175,
-                    "heading": -2.870128871927858,
-                    "angularVelocity": -5.7432227678581676e-8,
-                    "velocityX": 3.657262463614942,
-                    "velocityY": -0.9669902237500649,
-                    "timestamp": 0.759916517529817
-                },
-                {
-                    "x": 4.705445323630815,
-                    "y": 6.120995147626337,
-                    "heading": -2.870128876777158,
-                    "angularVelocity": -5.743222718130824e-8,
-                    "velocityX": 3.657262463614942,
-                    "velocityY": -0.9669902237500648,
-                    "timestamp": 0.8443516861442412
-                },
-                {
-                    "x": 5.014246896413346,
-                    "y": 6.039347165035499,
-                    "heading": -2.870128881626458,
-                    "angularVelocity": -5.743222751764232e-8,
-                    "velocityX": 3.657262463614942,
-                    "velocityY": -0.9669902237500648,
-                    "timestamp": 0.9287868547586653
-                },
-                {
-                    "x": 5.323048469195878,
-                    "y": 5.957699182444661,
-                    "heading": -2.8701288864757575,
-                    "angularVelocity": -5.74322277347238e-8,
-                    "velocityX": 3.657262463614942,
-                    "velocityY": -0.9669902237500648,
-                    "timestamp": 1.0132220233730893
-                },
-                {
-                    "x": 5.631850041978409,
-                    "y": 5.876051199853823,
-                    "heading": -2.8701288913250576,
-                    "angularVelocity": -5.743222678749957e-8,
-                    "velocityX": 3.657262463614942,
-                    "velocityY": -0.9669902237500648,
-                    "timestamp": 1.0976571919875133
-                },
-                {
-                    "x": 5.940651614760942,
-                    "y": 5.7944032172629845,
-                    "heading": -2.870128896174357,
-                    "angularVelocity": -5.7432226879414726e-8,
-                    "velocityX": 3.6572624636149413,
-                    "velocityY": -0.9669902237500653,
-                    "timestamp": 1.1820923606019373
-                },
-                {
-                    "x": 6.249453187542404,
-                    "y": 5.712755234668102,
-                    "heading": -2.8701289010236573,
-                    "angularVelocity": -5.743222723099526e-8,
-                    "velocityX": 3.6572624636022777,
-                    "velocityY": -0.9669902237979594,
-                    "timestamp": 1.2665275292163614
-                },
-                {
-                    "x": 6.5582545569659105,
-                    "y": 5.631106482955165,
-                    "heading": -2.8701289058729587,
-                    "angularVelocity": -5.743224882801054e-8,
-                    "velocityX": 3.6572600551514123,
-                    "velocityY": -0.9669993327755135,
-                    "timestamp": 1.3509626978307854
-                },
-                {
-                    "x": 6.8516839479397085,
-                    "y": 5.504916165148498,
-                    "heading": -2.8701289109689694,
-                    "angularVelocity": -6.035412197409506e-8,
-                    "velocityX": 3.4752034701766337,
-                    "velocityY": -1.4945231930893996,
-                    "timestamp": 1.4353978664452094
-                },
-                {
-                    "x": 7.116648885902659,
-                    "y": 5.326539084420994,
-                    "heading": -2.8701289171366793,
-                    "angularVelocity": -7.304669371510495e-8,
-                    "velocityX": 3.1380873907283835,
-                    "velocityY": -2.1125922249539495,
-                    "timestamp": 1.5198330350596334
-                },
-                {
-                    "x": 7.343971949437644,
-                    "y": 5.1021533761176485,
-                    "heading": -2.8701292296822887,
-                    "angularVelocity": -0.0000037016046138767138,
-                    "velocityX": 2.6922793815105543,
-                    "velocityY": -2.6574910903300615,
-                    "timestamp": 1.6042682036740574
-                },
-                {
-                    "x": 7.516721651510428,
-                    "y": 4.857947803192162,
-                    "heading": -2.9165540487463932,
-                    "angularVelocity": -0.5498279902312642,
-                    "velocityX": 2.0459448936692075,
-                    "velocityY": -2.8922257980043913,
-                    "timestamp": 1.6887033722884814
-                },
-                {
-                    "x": 7.630386965757564,
-                    "y": 4.610372175146774,
-                    "heading": -2.9830505381288734,
-                    "angularVelocity": -0.7875449350512241,
-                    "velocityX": 1.3461844881981369,
-                    "velocityY": -2.932138729727077,
-                    "timestamp": 1.7731385409029055
-                },
-                {
-                    "x": 7.684751488226614,
-                    "y": 4.365674345538311,
-                    "heading": -3.0601706707306313,
-                    "angularVelocity": -0.9133650570882078,
-                    "velocityX": 0.6438611228136838,
-                    "velocityY": -2.8980557938586533,
-                    "timestamp": 1.8575737095173295
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -0.9831760605353209,
-                    "velocityX": -0.05627380515240752,
-                    "velocityY": -2.8267172252385406,
-                    "timestamp": 1.9420088781317535
-                },
-                {
-                    "x": 7.621934708150364,
-                    "y": 3.906654914920449,
-                    "heading": -3.2251079869587973,
-                    "angularVelocity": -1.0177912145669754,
-                    "velocityX": -0.7213917327302901,
-                    "velocityY": -2.737523874602733,
-                    "timestamp": 2.022499531427526
-                },
-                {
-                    "x": 7.510577058905177,
-                    "y": 3.6951455922988243,
-                    "heading": -3.30599509113983,
-                    "angularVelocity": -1.0049254275993986,
-                    "velocityX": -1.3834854692504928,
-                    "velocityY": -2.627750104653823,
-                    "timestamp": 2.102990184723298
-                },
-                {
-                    "x": 7.346480542958668,
-                    "y": 3.4951541921560745,
-                    "heading": -3.379583459687032,
-                    "angularVelocity": -0.9142473757388557,
-                    "velocityX": -2.0387027465600407,
-                    "velocityY": -2.484653707652887,
-                    "timestamp": 2.1834808380190704
-                },
-                {
-                    "x": 7.131234653225788,
-                    "y": 3.3116699228744952,
-                    "heading": -3.433702808915206,
-                    "angularVelocity": -0.6723681199262506,
-                    "velocityX": -2.67417247741705,
-                    "velocityY": -2.279572369817109,
-                    "timestamp": 2.2639714913148428
-                },
-                {
-                    "x": 6.871582405242626,
-                    "y": 3.1556914599677013,
-                    "heading": -3.4376019485164777,
-                    "angularVelocity": -0.04844214131228369,
-                    "velocityX": -3.2258683132939807,
-                    "velocityY": -1.9378456568569487,
-                    "timestamp": 2.344462144610615
-                },
-                {
-                    "x": 6.586927593692301,
-                    "y": 3.0475868041755487,
-                    "heading": -3.4376019671352136,
-                    "angularVelocity": -2.3131550136324309e-7,
-                    "velocityX": -3.5364952313696496,
-                    "velocityY": -1.3430709202335072,
-                    "timestamp": 2.4249527979063874
-                },
-                {
-                    "x": 6.287649067705066,
-                    "y": 2.9914855953111847,
-                    "heading": -3.4376019800205944,
-                    "angularVelocity": -1.6008543147444885e-7,
-                    "velocityX": -3.718177375048765,
-                    "velocityY": -0.6969903531309354,
-                    "timestamp": 2.5054434512021597
-                },
-                {
-                    "x": 5.983166666187018,
-                    "y": 2.9891511130138855,
-                    "heading": -3.437601992016166,
-                    "angularVelocity": -1.4903061473211454e-7,
-                    "velocityX": -3.7828292981942986,
-                    "velocityY": -0.029003147591768797,
-                    "timestamp": 2.585934104497932
-                },
-                {
-                    "x": 5.678681298356483,
-                    "y": 2.9910598954241565,
-                    "heading": -3.437602003982072,
-                    "angularVelocity": -1.4866205580605315e-7,
-                    "velocityX": -3.782866151075542,
-                    "velocityY": 0.02371433616337754,
-                    "timestamp": 2.6664247577937044
-                },
-                {
-                    "x": 5.374195930686038,
-                    "y": 2.9929687033714,
-                    "heading": -3.437602015947978,
-                    "angularVelocity": -1.4866205622499977e-7,
-                    "velocityX": -3.7828661490866247,
-                    "velocityY": 0.02371465342968995,
-                    "timestamp": 2.7469154110894767
-                },
-                {
-                    "x": 5.06971056314094,
-                    "y": 2.9948775312983553,
-                    "heading": -3.4376020279140773,
-                    "angularVelocity": -1.4866445697347623e-7,
-                    "velocityX": -3.7828661475293295,
-                    "velocityY": 0.023714901653687247,
-                    "timestamp": 2.827406064385249
-                },
-                {
-                    "x": 4.800808155197524,
-                    "y": 2.9965858943831902,
-                    "heading": -3.5281868157365524,
-                    "angularVelocity": -1.1254075363217084,
-                    "velocityX": -3.340790476073066,
-                    "velocityY": 0.021224365996295316,
-                    "timestamp": 2.9078967176810213
-                },
-                {
-                    "x": 4.585685258881781,
-                    "y": 2.9979519483478,
-                    "heading": -3.6006057670766456,
-                    "angularVelocity": -0.8997187670227613,
-                    "velocityX": -2.6726444314627344,
-                    "velocityY": 0.016971585006104423,
-                    "timestamp": 2.9883873709767936
-                },
-                {
-                    "x": 4.424342674038891,
-                    "y": 2.9989760866788195,
-                    "heading": -3.6548985624477908,
-                    "angularVelocity": -0.6745229805958503,
-                    "velocityX": -2.0044884497336284,
-                    "velocityY": 0.012723692616254528,
-                    "timestamp": 3.068878024272566
-                },
-                {
-                    "x": 4.316780863688961,
-                    "y": 2.9996586949819566,
-                    "heading": -3.6910886560266527,
-                    "angularVelocity": -0.44961858423356516,
-                    "velocityX": -1.33632671553406,
-                    "velocityY": 0.008480590915651792,
-                    "timestamp": 3.1493686775683383
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -3.7091853071795864,
-                    "angularVelocity": -0.22482922441248102,
-                    "velocityX": -0.6681628423530913,
-                    "velocityY": 0.004240306222743686,
-                    "timestamp": 3.2298593308641106
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -3.7091853071795864,
-                    "angularVelocity": 0,
-                    "velocityX": -1.1582419131838594e-33,
-                    "velocityY": 0,
-                    "timestamp": 3.310349984159883
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -4683,416 +1782,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W1 To C3 To S4": {
+        "W2 To C3": {
             "waypoints": [
                 {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": -2.75,
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 23
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": -3.143185307179586,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 18
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -3.7831853071795862,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": -2.75,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.352388442136136,
-                    "y": 6.742171711440638,
-                    "heading": -2.7776238211060527,
-                    "angularVelocity": -0.340136208728432,
-                    "velocityX": 0.6450666626176952,
-                    "velocityY": -0.18258291758985748,
-                    "timestamp": 0.08121399720716439
-                },
-                {
-                    "x": 2.4572309209740357,
-                    "y": 6.712518353067644,
-                    "heading": -2.831291417681962,
-                    "angularVelocity": -0.6608170810552816,
-                    "velocityX": 1.290941000856364,
-                    "velocityY": -0.3651262022858861,
-                    "timestamp": 0.16242799441432879
-                },
-                {
-                    "x": 2.614630940945627,
-                    "y": 6.668043884281434,
-                    "heading": -2.9082589809802553,
-                    "angularVelocity": -0.9477130290857405,
-                    "velocityX": 1.9380898045256882,
-                    "velocityY": -0.5476207342969771,
-                    "timestamp": 0.2436419916214932
-                },
-                {
-                    "x": 2.824773268720236,
-                    "y": 6.608764345103305,
-                    "heading": -3.0025843751568475,
-                    "angularVelocity": -1.1614425766389054,
-                    "velocityX": 2.587513667614254,
-                    "velocityY": -0.7299177631454756,
-                    "timestamp": 0.32485598882865757
-                },
-                {
-                    "x": 3.087924526957624,
-                    "y": 6.534856635997473,
-                    "heading": -3.094100877088787,
-                    "angularVelocity": -1.1268562695832722,
-                    "velocityX": 3.2402204950799356,
-                    "velocityY": -0.9100365903255841,
-                    "timestamp": 0.40606998603582195
-                },
-                {
-                    "x": 3.383391154718911,
-                    "y": 6.450664344423451,
-                    "heading": -3.094100898976011,
-                    "angularVelocity": -2.695006379530874e-7,
-                    "velocityX": 3.638124435716056,
-                    "velocityY": -1.036672180514684,
-                    "timestamp": 0.48728398324298633
-                },
-                {
-                    "x": 3.6788577741864907,
-                    "y": 6.366472023742971,
-                    "heading": -3.0941009208630392,
-                    "angularVelocity": -2.694982209366716e-7,
-                    "velocityX": 3.638124333594396,
-                    "velocityY": -1.036672538906832,
-                    "timestamp": 0.5684979804501508
-                },
-                {
-                    "x": 3.9743243936540216,
-                    "y": 6.282279703062319,
-                    "heading": -3.094100942750067,
-                    "angularVelocity": -2.694982225892601e-7,
-                    "velocityX": 3.638124333593797,
-                    "velocityY": -1.036672538908934,
-                    "timestamp": 0.6497119776573151
-                },
-                {
-                    "x": 4.269791013121552,
-                    "y": 6.198087382381668,
-                    "heading": -3.0941009646370947,
-                    "angularVelocity": -2.694982224333276e-7,
-                    "velocityX": 3.6381243335937974,
-                    "velocityY": -1.0366725389089342,
-                    "timestamp": 0.7309259748644795
-                },
-                {
-                    "x": 4.565257632589082,
-                    "y": 6.113895061701016,
-                    "heading": -3.094100986524123,
-                    "angularVelocity": -2.694982222034585e-7,
-                    "velocityX": 3.6381243335937974,
-                    "velocityY": -1.036672538908934,
-                    "timestamp": 0.8121399720716439
-                },
-                {
-                    "x": 4.860724252056613,
-                    "y": 6.0297027410203645,
-                    "heading": -3.0941010084111507,
-                    "angularVelocity": -2.694982230736536e-7,
-                    "velocityX": 3.638124333593797,
-                    "velocityY": -1.0366725389089342,
-                    "timestamp": 0.8933539692788083
-                },
-                {
-                    "x": 5.156190871524144,
-                    "y": 5.945510420339713,
-                    "heading": -3.094101030298179,
-                    "angularVelocity": -2.6949822300117995e-7,
-                    "velocityX": 3.638124333593797,
-                    "velocityY": -1.0366725389089342,
-                    "timestamp": 0.9745679664859727
-                },
-                {
-                    "x": 5.451657490991675,
-                    "y": 5.861318099659061,
-                    "heading": -3.0941010521852066,
-                    "angularVelocity": -2.694982228196196e-7,
-                    "velocityX": 3.638124333593797,
-                    "velocityY": -1.0366725389089342,
-                    "timestamp": 1.0557819636931371
-                },
-                {
-                    "x": 5.747124110459206,
-                    "y": 5.77712577897841,
-                    "heading": -3.0941010740722343,
-                    "angularVelocity": -2.694982236260836e-7,
-                    "velocityX": 3.638124333593797,
-                    "velocityY": -1.0366725389089344,
-                    "timestamp": 1.1369959609003015
-                },
-                {
-                    "x": 6.042590729926668,
-                    "y": 5.69293345829752,
-                    "heading": -3.0941010959592625,
-                    "angularVelocity": -2.694982236351797e-7,
-                    "velocityX": 3.6381243335929603,
-                    "velocityY": -1.0366725389118696,
-                    "timestamp": 1.218209958107466
-                },
-                {
-                    "x": 6.338057337809956,
-                    "y": 5.6087410969628495,
-                    "heading": -3.0941011178462903,
-                    "angularVelocity": -2.6949822192516605e-7,
-                    "velocityX": 3.6381241909552937,
-                    "velocityY": -1.0366730394879038,
-                    "timestamp": 1.2994239553146303
-                },
-                {
-                    "x": 6.63151320525297,
-                    "y": 5.517787680923723,
-                    "heading": -3.0941011398106846,
-                    "angularVelocity": -2.704508467508134e-7,
-                    "velocityX": 3.6133656455032592,
-                    "velocityY": -1.1199229094611813,
-                    "timestamp": 1.3806379525217947
-                },
-                {
-                    "x": 6.904052184030547,
-                    "y": 5.375973104754023,
-                    "heading": -3.09410116345768,
-                    "angularVelocity": -2.911689606888355e-7,
-                    "velocityX": 3.355812891235069,
-                    "velocityY": -1.7461839220887416,
-                    "timestamp": 1.461851949728959
-                },
-                {
-                    "x": 7.14694076900972,
-                    "y": 5.1878401702780765,
-                    "heading": -3.094101192989132,
-                    "angularVelocity": -3.636251518218712e-7,
-                    "velocityX": 2.9907231921877617,
-                    "velocityY": -2.3165087416877084,
-                    "timestamp": 1.5430659469361234
-                },
-                {
-                    "x": 7.352395911467515,
-                    "y": 4.959417222897554,
-                    "heading": -3.0941012381435535,
-                    "angularVelocity": -5.559930935407798e-7,
-                    "velocityX": 2.529799659188236,
-                    "velocityY": -2.812605649731368,
-                    "timestamp": 1.6242799441432878
-                },
-                {
-                    "x": 7.513833646479666,
-                    "y": 4.698024306067166,
-                    "heading": -3.094103186189668,
-                    "angularVelocity": -0.00002398658095119272,
-                    "velocityX": 1.9878067889975632,
-                    "velocityY": -3.2185697764010213,
-                    "timestamp": 1.7054939413504522
-                },
-                {
-                    "x": 7.6238137890728055,
-                    "y": 4.417795537290539,
-                    "heading": -3.1108263182271356,
-                    "angularVelocity": -0.20591440655693208,
-                    "velocityX": 1.35420181708456,
-                    "velocityY": -3.4504984166977444,
-                    "timestamp": 1.7867079385576166
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -0.3984410331631951,
-                    "velocityX": 0.6918291533072239,
-                    "velocityY": -3.5806086055413933,
-                    "timestamp": 1.867921935764781
-                },
-                {
-                    "x": 7.6870591404547595,
-                    "y": 3.8674277894608684,
-                    "heading": -3.1773151030340356,
-                    "angularVelocity": -0.47768106939784993,
-                    "velocityX": 0.09879982223438938,
-                    "velocityY": -3.63297605573316,
-                    "timestamp": 1.9393708543807355
-                },
-                {
-                    "x": 7.651588574491949,
-                    "y": 3.608358714127091,
-                    "heading": -3.205456863771594,
-                    "angularVelocity": -0.39387245154381195,
-                    "velocityX": -0.4964465053735158,
-                    "velocityY": -3.625934168821804,
-                    "timestamp": 2.0108197729966903
-                },
-                {
-                    "x": 7.574047848742119,
-                    "y": 3.354318466237311,
-                    "heading": -3.2189736743217594,
-                    "angularVelocity": -0.1891814573314478,
-                    "velocityX": -1.0852610123268396,
-                    "velocityY": -3.5555506341084104,
-                    "timestamp": 2.082268691612645
-                },
-                {
-                    "x": 7.455586952035965,
-                    "y": 3.1113741995114603,
-                    "heading": -3.2189745116430255,
-                    "angularVelocity": -0.00001171915938609546,
-                    "velocityX": -1.6579802605551721,
-                    "velocityY": -3.4002511365232904,
-                    "timestamp": 2.1537176102285995
-                },
-                {
-                    "x": 7.300453960582251,
-                    "y": 2.8900400447662866,
-                    "heading": -3.2189745885172623,
-                    "angularVelocity": -0.0000010759328215916604,
-                    "velocityX": -2.171243378574256,
-                    "velocityY": -3.097795726435581,
-                    "timestamp": 2.225166528844554
-                },
-                {
-                    "x": 7.11249649568723,
-                    "y": 2.6958055218428347,
-                    "heading": -3.2189746440183837,
-                    "angularVelocity": -7.767944182730611e-7,
-                    "velocityX": -2.6306551384378265,
-                    "velocityY": -2.718508924810283,
-                    "timestamp": 2.2966154474605087
-                },
-                {
-                    "x": 6.896376183100963,
-                    "y": 2.533487712060768,
-                    "heading": -3.2189746912030057,
-                    "angularVelocity": -6.603965892997474e-7,
-                    "velocityX": -3.024822723297278,
-                    "velocityY": -2.2718021899927887,
-                    "timestamp": 2.3680643660764633
-                },
-                {
-                    "x": 6.6576888713401265,
-                    "y": 2.4066672511412732,
-                    "heading": -3.218974736751788,
-                    "angularVelocity": -6.375013452668414e-7,
-                    "velocityX": -3.340670739366957,
-                    "velocityY": -1.7749808302282493,
-                    "timestamp": 2.439513284692418
-                },
-                {
-                    "x": 6.418991689499422,
-                    "y": 2.2798653705517777,
-                    "heading": -3.2189747823005193,
-                    "angularVelocity": -6.375006445255001e-7,
-                    "velocityX": -3.3408088808494862,
-                    "velocityY": -1.7747207802229084,
-                    "timestamp": 2.5109622033083725
-                },
-                {
-                    "x": 6.180294507575015,
-                    "y": 2.1530634901198433,
-                    "heading": -3.2189748278492507,
-                    "angularVelocity": -6.375006422464704e-7,
-                    "velocityX": -3.340808882020978,
-                    "velocityY": -1.7747207780176886,
-                    "timestamp": 2.582411121924327
-                },
-                {
-                    "x": 5.941597302835271,
-                    "y": 2.026261652636718,
-                    "heading": -3.21897487339828,
-                    "angularVelocity": -6.375048107159433e-7,
-                    "velocityX": -3.3408092013447153,
-                    "velocityY": -1.774720176905592,
-                    "timestamp": 2.6538600405402817
-                },
-                {
-                    "x": 5.72404161684181,
-                    "y": 1.9132773125800981,
-                    "heading": -3.2812904960777876,
-                    "angularVelocity": -0.8721702705344045,
-                    "velocityX": -3.0449122283008005,
-                    "velocityY": -1.5813303020905036,
-                    "timestamp": 2.7253089591562363
-                },
-                {
-                    "x": 5.540083719687043,
-                    "y": 1.8151536354630486,
-                    "heading": -3.418088909630078,
-                    "angularVelocity": -1.9146323863713337,
-                    "velocityX": -2.574677136023417,
-                    "velocityY": -1.3733402691069942,
-                    "timestamp": 2.796757877772191
-                },
-                {
-                    "x": 5.393751164472601,
-                    "y": 1.7369217513952075,
-                    "heading": -3.5523873708884453,
-                    "angularVelocity": -1.8796430213546256,
-                    "velocityX": -2.048072357826006,
-                    "velocityY": -1.0949344732596382,
-                    "timestamp": 2.8682067963881455
-                },
-                {
-                    "x": 5.284579981216215,
-                    "y": 1.6784003858088632,
-                    "heading": -3.6634662967687484,
-                    "angularVelocity": -1.554662100328298,
-                    "velocityX": -1.5279613095723332,
-                    "velocityY": -0.8190657986344941,
-                    "timestamp": 2.9396557150041
-                },
-                {
-                    "x": 5.21211226324901,
-                    "y": 1.6394523869599669,
-                    "heading": -3.742196708881707,
-                    "angularVelocity": -1.1019118782914785,
-                    "velocityX": -1.0142591290490286,
-                    "velocityY": -0.5451167016141149,
-                    "timestamp": 3.0111046336200546
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -3.7831853071795862,
-                    "angularVelocity": -0.5736769582023385,
-                    "velocityX": -0.505427709031773,
-                    "velocityY": -0.2722558624712981,
-                    "timestamp": 3.0825535522360092
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -3.7831853071795862,
-                    "angularVelocity": 0,
-                    "velocityX": -1.863217485402065e-29,
-                    "velocityY": -1.5751272024067454e-29,
-                    "timestamp": 3.154002470851964
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -5112,7 +1823,7 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W2 To W1": {
+        "W2 To C4": {
             "waypoints": [
                 {
                     "x": 2.25,
@@ -5121,119 +1832,19 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 10
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 3.5331853071795862,
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": -6.980040471932584e-34,
-                    "velocityX": -4.489498521627733e-35,
-                    "velocityY": 1.1490136335521518e-35,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.252002036509344,
-                    "y": 5.607878031980728,
-                    "heading": 3.1558282188080855,
-                    "angularVelocity": 0.20834807697002747,
-                    "velocityX": 0.026352962503434416,
-                    "velocityY": 0.6302222639984295,
-                    "timestamp": 0.07597007391799691
-                },
-                {
-                    "x": 2.2560056075028405,
-                    "y": 5.703634203308701,
-                    "heading": 3.187479727503087,
-                    "angularVelocity": 0.41663127416680434,
-                    "velocityX": 0.05269931681016954,
-                    "velocityY": 1.260445941270677,
-                    "timestamp": 0.15194014783599383
-                },
-                {
-                    "x": 2.262010006101527,
-                    "y": 5.847268776725278,
-                    "heading": 3.2349419015403043,
-                    "angularVelocity": 0.6247482934984211,
-                    "velocityX": 0.07903636641406601,
-                    "velocityY": 1.8906730770279163,
-                    "timestamp": 0.22791022175399073
-                },
-                {
-                    "x": 2.2700144000703353,
-                    "y": 6.038782280998214,
-                    "heading": 3.298188503077313,
-                    "angularVelocity": 0.8325199420666319,
-                    "velocityX": 0.105362461242945,
-                    "velocityY": 2.520907172996284,
-                    "timestamp": 0.30388029567198765
-                },
-                {
-                    "x": 2.280009617993533,
-                    "y": 6.278187235561798,
-                    "heading": 3.376569762958043,
-                    "angularVelocity": 1.0317386286254728,
-                    "velocityX": 0.13156783201220376,
-                    "velocityY": 3.151306063253281,
-                    "timestamp": 0.3798503695899846
-                },
-                {
-                    "x": 2.28800573619677,
-                    "y": 6.469712012711673,
-                    "heading": 3.4392336798072036,
-                    "angularVelocity": 0.8248500181374163,
-                    "velocityX": 0.10525352669615307,
-                    "velocityY": 2.5210555587534333,
-                    "timestamp": 0.4558204435079815
-                },
-                {
-                    "x": 2.2940028560112626,
-                    "y": 6.613355954230437,
-                    "heading": 3.486212722152842,
-                    "angularVelocity": 0.6183887934128925,
-                    "velocityX": 0.07894055521080842,
-                    "velocityY": 1.890796390086646,
-                    "timestamp": 0.5317905174259784
-                },
-                {
-                    "x": 2.2980009548636073,
-                    "y": 6.709118670121978,
-                    "heading": 3.517527021603037,
-                    "angularVelocity": 0.41219256261348647,
-                    "velocityX": 0.052627286589986035,
-                    "velocityY": 1.2605320878706525,
-                    "timestamp": 0.6077605913439753
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 3.5331853071795862,
-                    "angularVelocity": 0.20611123260786593,
-                    "velocityX": 0.02631358682828647,
-                    "velocityY": 0.6302656744773738,
-                    "timestamp": 0.6837306652619722
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 3.5331853071795862,
-                    "angularVelocity": 2.252106897096389e-33,
-                    "velocityX": 0,
-                    "velocityY": 2.1844888905645107e-35,
-                    "timestamp": 0.7597007391799692
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -5253,7 +1864,7 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W2 To W3": {
+        "W2 To C5": {
             "waypoints": [
                 {
                     "x": 2.25,
@@ -5262,119 +1873,60 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 10
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
+            "trajectory": [],
+            "constraints": [
                 {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": -7.990908752759601e-36,
-                    "velocityX": 0,
-                    "velocityY": 3.147004019802851e-35,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.252001928264259,
-                    "y": 5.512001885302468,
-                    "heading": 3.1243023137364228,
-                    "angularVelocity": -0.2063808903009179,
-                    "velocityX": 0.026319785639686007,
-                    "velocityY": -0.6310416374562365,
-                    "timestamp": 0.07606172374142242
-                },
-                {
-                    "x": 2.2560053075353443,
-                    "y": 5.416005550394565,
-                    "heading": 3.0929118769402844,
-                    "angularVelocity": -0.41269688947430483,
-                    "velocityX": 0.052633296672250224,
-                    "velocityY": -1.2620846621127049,
-                    "timestamp": 0.15212344748284484
-                },
-                {
-                    "x": 2.262009466656741,
-                    "y": 5.272010740000972,
-                    "heading": 3.0458411253223887,
-                    "angularVelocity": -0.6188493936571329,
-                    "velocityX": 0.0789379838643735,
-                    "velocityY": -1.8931310429292179,
-                    "timestamp": 0.22818517122426726
-                },
-                {
-                    "x": 2.2700136159756155,
-                    "y": 5.080016943812723,
-                    "heading": 2.983115684252823,
-                    "angularVelocity": -0.8246649955344898,
-                    "velocityX": 0.10523228931919847,
-                    "velocityY": -2.524184132888531,
-                    "timestamp": 0.3042468949656897
-                },
-                {
-                    "x": 2.280009094575526,
-                    "y": 4.840012208292782,
-                    "heading": 2.9053630904797005,
-                    "angularVelocity": -1.0222302355051571,
-                    "velocityX": 0.1314127278247149,
-                    "velocityY": -3.155394378595143,
-                    "timestamp": 0.3803086187071121
-                },
-                {
-                    "x": 2.2880054239679986,
-                    "y": 4.64800763986969,
-                    "heading": 2.8432007503387635,
-                    "angularVelocity": -0.8172617853397972,
-                    "velocityX": 0.10512947904857922,
-                    "velocityY": -2.524325757799354,
-                    "timestamp": 0.45637034244853447
-                },
-                {
-                    "x": 2.294002700573331,
-                    "y": 4.504003870137798,
-                    "heading": 2.7965972482938968,
-                    "angularVelocity": -0.6127063620501001,
-                    "velocityX": 0.07884749793103833,
-                    "velocityY": -1.8932488332954869,
-                    "timestamp": 0.5324320661899569
-                },
-                {
-                    "x": 2.2980009029345383,
-                    "y": 4.408001272313515,
-                    "heading": 2.7655331423967997,
-                    "angularVelocity": -0.40840654627682993,
-                    "velocityX": 0.05256523471383641,
-                    "velocityY": -1.2621670020344438,
-                    "timestamp": 0.6084937899313793
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W3 To W1": {
+            "waypoints": [
                 {
                     "x": 2.3,
                     "y": 4.36,
                     "heading": 2.75,
-                    "angularVelocity": -0.20421759635116843,
-                    "velocityX": 0.026282563254256014,
-                    "velocityY": -0.631083151319306,
-                    "timestamp": 0.6845555136728017
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": -4.352426400818003e-34,
-                    "velocityX": -2.6471819427760516e-34,
-                    "velocityY": 2.5922629666231517e-36,
-                    "timestamp": 0.760617237414224
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 }
             ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -5394,344 +1946,69 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W2 To C1 To S1": {
+        "W3 To W2": {
             "waypoints": [
                 {
-                    "x": 2.329045295715332,
-                    "y": 5.559675216674805,
-                    "heading": 3.14,
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 20
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 7.68,
-                    "y": 7.467,
+                    "x": 2.25,
+                    "y": 5.56,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 13
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
                 {
-                    "x": 5.233179569244385,
-                    "y": 6.553707599639893,
-                    "heading": 3.4031853071795863,
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W3 To S1": {
+            "waypoints": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.329045295715332,
-                    "y": 5.559675216674805,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.4000279527631365,
-                    "y": 5.585359375509262,
-                    "heading": 3.1338545708828756,
-                    "angularVelocity": -0.06457961815077011,
-                    "velocityX": 0.745925597725292,
-                    "velocityY": 0.2699035557060694,
-                    "timestamp": 0.09516050563794304
-                },
-                {
-                    "x": 2.5419932646198635,
-                    "y": 5.636727692199652,
-                    "heading": 3.1215638736263545,
-                    "angularVelocity": -0.12915754465735657,
-                    "velocityX": 1.4918511719231908,
-                    "velocityY": 0.539807101129259,
-                    "timestamp": 0.19032101127588608
-                },
-                {
-                    "x": 2.7549412265998368,
-                    "y": 5.713780164921229,
-                    "heading": 3.103128182719956,
-                    "angularVelocity": -0.1937325866743065,
-                    "velocityX": 2.2377766968814634,
-                    "velocityY": 0.8097106273770546,
-                    "timestamp": 0.2854815169138291
-                },
-                {
-                    "x": 3.038871822724849,
-                    "y": 5.81651678795237,
-                    "heading": 3.078547903585146,
-                    "angularVelocity": -0.258303368295786,
-                    "velocityX": 2.9837020539320758,
-                    "velocityY": 1.0796140934988998,
-                    "timestamp": 0.38064202255177215
-                },
-                {
-                    "x": 3.3773799452774673,
-                    "y": 5.939001713582426,
-                    "heading": 3.0785479006839798,
-                    "angularVelocity": -3.048712788248211e-8,
-                    "velocityX": 3.5572333320775313,
-                    "velocityY": 1.2871403405116655,
-                    "timestamp": 0.4758025281897152
-                },
-                {
-                    "x": 3.715888067817609,
-                    "y": 6.0614866392475255,
-                    "heading": 3.0785478977827805,
-                    "angularVelocity": -3.0487407415354604e-8,
-                    "velocityX": 3.5572333319465472,
-                    "velocityY": 1.287140340879956,
-                    "timestamp": 0.5709630338276582
-                },
-                {
-                    "x": 4.054396190357754,
-                    "y": 6.183971564912614,
-                    "heading": 3.0785478948815816,
-                    "angularVelocity": -3.0487407316957414e-8,
-                    "velocityX": 3.5572333319465868,
-                    "velocityY": 1.28714034087985,
-                    "timestamp": 0.6661235394656013
-                },
-                {
-                    "x": 4.392904312897899,
-                    "y": 6.306456490577704,
-                    "heading": 3.0785478919803824,
-                    "angularVelocity": -3.048740785433711e-8,
-                    "velocityX": 3.557233331946584,
-                    "velocityY": 1.287140340879858,
-                    "timestamp": 0.7612840451035443
-                },
-                {
-                    "x": 4.7314124354380445,
-                    "y": 6.428941416242792,
-                    "heading": 3.0785478890791835,
-                    "angularVelocity": -3.0487408354838206e-8,
-                    "velocityX": 3.557233331946584,
-                    "velocityY": 1.2871403408798578,
-                    "timestamp": 0.8564445507414873
-                },
-                {
-                    "x": 5.06992055797819,
-                    "y": 6.551426341907881,
-                    "heading": 3.0785478861779842,
-                    "angularVelocity": -3.04874074079248e-8,
-                    "velocityX": 3.557233331946584,
-                    "velocityY": 1.2871403408798578,
-                    "timestamp": 0.9516050563794303
-                },
-                {
-                    "x": 5.408428680518335,
-                    "y": 6.67391126757297,
-                    "heading": 3.0785478832767854,
-                    "angularVelocity": -3.048740819605906e-8,
-                    "velocityX": 3.557233331946584,
-                    "velocityY": 1.2871403408798576,
-                    "timestamp": 1.0467655620173733
-                },
-                {
-                    "x": 5.746936803058481,
-                    "y": 6.796396193238057,
-                    "heading": 3.078547880375586,
-                    "angularVelocity": -3.0487408175601846e-8,
-                    "velocityX": 3.5572333319465934,
-                    "velocityY": 1.2871403408798332,
-                    "timestamp": 1.1419260676553162
-                },
-                {
-                    "x": 6.085444925598784,
-                    "y": 6.918881118902711,
-                    "heading": 3.0785478774743873,
-                    "angularVelocity": -3.0487407710602e-8,
-                    "velocityX": 3.55723333194824,
-                    "velocityY": 1.2871403408752817,
-                    "timestamp": 1.2370865732932592
-                },
-                {
-                    "x": 6.4239530481377205,
-                    "y": 7.041366044571132,
-                    "heading": 3.078547874573188,
-                    "angularVelocity": -3.0487407747049345e-8,
-                    "velocityX": 3.55723333193393,
-                    "velocityY": 1.287140340914726,
-                    "timestamp": 1.3322470789312022
-                },
-                {
-                    "x": 6.762461177860979,
-                    "y": 7.163850950384819,
-                    "heading": 3.078547871671963,
-                    "angularVelocity": -3.0487683312908684e-8,
-                    "velocityX": 3.5572334074304637,
-                    "velocityY": 1.2871401322709841,
-                    "timestamp": 1.4274075845691452
-                },
-                {
-                    "x": 7.087934228336467,
-                    "y": 7.275849077547301,
-                    "heading": 3.0785482003586213,
-                    "angularVelocity": 0.000003454027869296417,
-                    "velocityX": 3.420253479039086,
-                    "velocityY": 1.1769391767257837,
-                    "timestamp": 1.5225680902070882
-                },
-                {
-                    "x": 7.34242464824946,
-                    "y": 7.362163048068745,
-                    "heading": 3.0846933316902727,
-                    "angularVelocity": 0.06457648885540108,
-                    "velocityX": 2.6743281596359925,
-                    "velocityY": 0.9070356440694888,
-                    "timestamp": 1.6177285958450311
-                },
-                {
-                    "x": 7.525932419508426,
-                    "y": 7.422792858981067,
-                    "heading": 3.0969834578057935,
-                    "angularVelocity": 0.12915154278707255,
-                    "velocityX": 1.9284026501171228,
-                    "velocityY": 0.6371320802228948,
-                    "timestamp": 1.7128891014829741
-                },
-                {
-                    "x": 7.638457537165259,
-                    "y": 7.457738509668789,
-                    "heading": 3.1154188790536077,
-                    "angularVelocity": 0.1937297529488966,
-                    "velocityX": 1.1824770886008305,
-                    "velocityY": 0.3672285099084715,
-                    "timestamp": 1.8080496071209171
                 },
                 {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 3.14,
-                    "angularVelocity": 0.2583122145204678,
-                    "velocityX": 0.4365515142645689,
-                    "velocityY": 0.09732493820788266,
-                    "timestamp": 1.90321011275886
-                },
-                {
-                    "x": 7.653654383343035,
-                    "y": 7.451878922022455,
-                    "heading": 3.169571245622926,
-                    "angularVelocity": 0.3208641076607671,
-                    "velocityX": -0.28586427797403835,
-                    "velocityY": -0.16407192492517308,
-                    "timestamp": 1.9953713888811428
-                },
-                {
-                    "x": 7.560730009307251,
-                    "y": 7.412667176131766,
-                    "heading": 3.204907797708199,
-                    "angularVelocity": 0.3834208202531959,
-                    "velocityX": -1.0082800276276813,
-                    "velocityY": -0.42546878190023074,
-                    "timestamp": 2.0875326650034256
-                },
-                {
-                    "x": 7.4012268833190475,
-                    "y": 7.349364762932986,
-                    "heading": 3.2460101421492427,
-                    "angularVelocity": 0.4459828050518832,
-                    "velocityX": -1.7306957184018388,
-                    "velocityY": -0.6868656323100164,
-                    "timestamp": 2.1796939411257084
-                },
-                {
-                    "x": 7.175145013744201,
-                    "y": 7.261971683800118,
-                    "heading": 3.292878761166739,
-                    "angularVelocity": 0.5085500222039014,
-                    "velocityX": -2.4531113184025837,
-                    "velocityY": -0.9482624678108277,
-                    "timestamp": 2.271855217247991
-                },
-                {
-                    "x": 6.8824844194459525,
-                    "y": 7.150487945420426,
-                    "heading": 3.3455140401053294,
-                    "angularVelocity": 0.5711214205450821,
-                    "velocityX": -3.1755267137264047,
-                    "velocityY": -1.2096592307504261,
-                    "timestamp": 2.364016493370274
-                },
-                {
-                    "x": 6.55464527272117,
-                    "y": 7.031863403145239,
-                    "heading": 3.3455140429153563,
-                    "angularVelocity": 3.049000281588377e-8,
-                    "velocityX": -3.557233151692005,
-                    "velocityY": -1.2871408390477628,
-                    "timestamp": 2.4561777694925566
-                },
-                {
-                    "x": 6.226806115184277,
-                    "y": 6.913238890751769,
-                    "heading": 3.3455140457253494,
-                    "angularVelocity": 3.0489828937045393e-8,
-                    "velocityX": -3.557233269011331,
-                    "velocityY": -1.287140514808609,
-                    "timestamp": 2.5483390456148394
-                },
-                {
-                    "x": 5.898966957623017,
-                    "y": 6.79461437842809,
-                    "heading": 3.345514048537748,
-                    "angularVelocity": 3.0514995916000634e-8,
-                    "velocityX": -3.557233269277337,
-                    "velocityY": -1.287140514052891,
-                    "timestamp": 2.640500321737122
-                },
-                {
-                    "x": 5.632652017742471,
-                    "y": 6.698251673595034,
-                    "heading": 3.3685823941434916,
-                    "angularVelocity": 0.2503041035574726,
-                    "velocityX": -2.889662026024368,
-                    "velocityY": -1.045587787926248,
-                    "timestamp": 2.732661597859405
-                },
-                {
-                    "x": 5.432915797517009,
-                    "y": 6.625979638346594,
-                    "heading": 3.3858838135535563,
-                    "angularVelocity": 0.18772981600595268,
-                    "velocityX": -2.167246685696753,
-                    "velocityY": -0.7841909128155133,
-                    "timestamp": 2.8248228739816876
-                },
-                {
-                    "x": 5.299758312935604,
-                    "y": 6.577798279546128,
-                    "heading": 3.3974181385441695,
-                    "angularVelocity": 0.1251537031151884,
-                    "velocityX": -1.4448311718780615,
-                    "velocityY": -0.5227939632329148,
-                    "timestamp": 2.9169841501039704
-                },
-                {
-                    "x": 5.233179569244385,
-                    "y": 6.553707599639893,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": 0.06257691818239948,
-                    "velocityX": -0.7224156011355166,
-                    "velocityY": -0.2613969871068907,
-                    "timestamp": 3.009145426226253
-                },
-                {
-                    "x": 5.233179569244385,
-                    "y": 6.553707599639893,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": 0,
-                    "velocityX": 1.5847972099366162e-31,
-                    "velocityY": 0,
-                    "timestamp": 3.101306702348536
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 }
             ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -5751,25 +2028,16 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W2 To C1 To S2": {
+        "W3 To S2": {
             "waypoints": [
                 {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 20
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 3.14,
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 15
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 4.263,
@@ -5781,332 +2049,376 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
+            "trajectory": [],
+            "constraints": [
                 {
-                    "x": 2.3199317069050815,
-                    "y": 5.589221584324863,
-                    "heading": 3.14,
-                    "angularVelocity": -4.40794531804863e-17,
-                    "velocityX": 0.7334971919039711,
-                    "velocityY": 0.3064983109073157,
-                    "timestamp": 0.09534011537734256
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
                 {
-                    "x": 2.4597951178135684,
-                    "y": 5.647664751762101,
-                    "heading": 3.14,
-                    "angularVelocity": -2.0051811551543008e-16,
-                    "velocityX": 1.4669943533729506,
-                    "velocityY": 0.6129966090971003,
-                    "timestamp": 0.19068023075468513
-                },
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W3 To S3": {
+            "waypoints": [
                 {
-                    "x": 2.6695902269221126,
-                    "y": 5.73532949988673,
-                    "heading": 3.14,
-                    "angularVelocity": -1.6298734307861892e-16,
-                    "velocityX": 2.200491453971973,
-                    "velocityY": 0.9194948818518327,
-                    "timestamp": 0.2860203461320277
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 2.9493170168206886,
-                    "y": 5.852215821423819,
-                    "heading": 3.14,
-                    "angularVelocity": -2.718821323245932e-16,
-                    "velocityX": 2.933988371961343,
-                    "velocityY": 1.2259930783015016,
-                    "timestamp": 0.38136046150937025
-                },
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
                 {
-                    "x": 3.2820984406189933,
-                    "y": 5.991271492558365,
-                    "heading": 3.14,
-                    "angularVelocity": -1.791056538237388e-18,
-                    "velocityX": 3.4904659227776635,
-                    "velocityY": 1.45852216125588,
-                    "timestamp": 0.4767005768867128
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
                 {
-                    "x": 3.6148798644173543,
-                    "y": 6.130327163692935,
-                    "heading": 3.14,
-                    "angularVelocity": 2.7598941425139603e-16,
-                    "velocityX": 3.4904659227782475,
-                    "velocityY": 1.4585221612561248,
-                    "timestamp": 0.5720406922640554
-                },
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W3 To S4": {
+            "waypoints": [
                 {
-                    "x": 3.947661288215715,
-                    "y": 6.269382834827505,
-                    "heading": 3.14,
-                    "angularVelocity": -1.359472794095135e-16,
-                    "velocityX": 3.4904659227782475,
-                    "velocityY": 1.4585221612561248,
-                    "timestamp": 0.667380807641398
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 4.280442712014075,
-                    "y": 6.408438505962075,
-                    "heading": 3.14,
-                    "angularVelocity": -3.500332184235888e-16,
-                    "velocityX": 3.4904659227782475,
-                    "velocityY": 1.4585221612561245,
-                    "timestamp": 0.7627209230187405
-                },
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
                 {
-                    "x": 4.613224135812436,
-                    "y": 6.547494177096645,
-                    "heading": 3.14,
-                    "angularVelocity": -3.2849392470850763e-16,
-                    "velocityX": 3.4904659227782475,
-                    "velocityY": 1.4585221612561245,
-                    "timestamp": 0.858061038396083
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
                 {
-                    "x": 4.9460055596107955,
-                    "y": 6.686549848231215,
-                    "heading": 3.14,
-                    "angularVelocity": -3.264382961483678e-16,
-                    "velocityX": 3.4904659227782475,
-                    "velocityY": 1.4585221612561245,
-                    "timestamp": 0.9534011537734256
-                },
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W3 To C1": {
+            "waypoints": [
                 {
-                    "x": 5.278786983409157,
-                    "y": 6.825605519365785,
-                    "heading": 3.14,
-                    "angularVelocity": -2.8870332355952577e-16,
-                    "velocityX": 3.4904659227782475,
-                    "velocityY": 1.4585221612561245,
-                    "timestamp": 1.0487412691507683
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 5.611568407207518,
-                    "y": 6.9646611905003555,
+                    "x": 7.68,
+                    "y": 7.467,
                     "heading": 3.14,
-                    "angularVelocity": -2.714509339961268e-16,
-                    "velocityX": 3.4904659227782475,
-                    "velocityY": 1.4585221612561245,
-                    "timestamp": 1.1440813845281108
-                },
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
                 {
-                    "x": 5.944349831005879,
-                    "y": 7.1037168616349256,
-                    "heading": 3.14,
-                    "angularVelocity": 3.236789006032357e-16,
-                    "velocityX": 3.4904659227782475,
-                    "velocityY": 1.4585221612561248,
-                    "timestamp": 1.2394214999054534
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
                 {
-                    "x": 6.2771312548052975,
-                    "y": 7.242772532766961,
-                    "heading": 3.14,
-                    "angularVelocity": 1.392859578179056e-16,
-                    "velocityX": 3.4904659227893537,
-                    "velocityY": 1.458522161229545,
-                    "timestamp": 1.334761615282796
-                },
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W3 To C2": {
+            "waypoints": [
                 {
-                    "x": 6.609912972036511,
-                    "y": 7.381827501669921,
-                    "heading": 3.14,
-                    "angularVelocity": 7.280552315335093e-17,
-                    "velocityX": 3.4904690005262737,
-                    "velocityY": 1.4585147957142848,
-                    "timestamp": 1.4301017306601385
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 6.961412726724582,
-                    "y": 7.462622346034388,
+                    "x": 7.68,
+                    "y": 5.797,
                     "heading": 3.14,
-                    "angularVelocity": -1.0172483638053557e-16,
-                    "velocityX": 3.6867980838588856,
-                    "velocityY": 0.8474380804417183,
-                    "timestamp": 1.525441846037481
-                },
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
                 {
-                    "x": 7.244997116389008,
-                    "y": 7.50977433975784,
-                    "heading": 3.14,
-                    "angularVelocity": -1.1360407734774875e-16,
-                    "velocityX": 2.974449826728655,
-                    "velocityY": 0.4945661491684764,
-                    "timestamp": 1.6207819614148236
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
                 {
-                    "x": 7.459465375503974,
-                    "y": 7.525824834495841,
-                    "heading": 3.14,
-                    "angularVelocity": -6.302038440751775e-17,
-                    "velocityX": 2.249507023000035,
-                    "velocityY": 0.16834985645314604,
-                    "timestamp": 1.716122076792166
-                },
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W3 To C3": {
+            "waypoints": [
                 {
-                    "x": 7.6045126721499186,
-                    "y": 7.511460301300605,
-                    "heading": 3.14,
-                    "angularVelocity": 8.943638136140749e-17,
-                    "velocityX": 1.5213669091113222,
-                    "velocityY": -0.15066620318617746,
-                    "timestamp": 1.8114621921695087
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 7.68,
-                    "y": 7.467,
+                    "y": 4.127,
                     "heading": 3.14,
-                    "angularVelocity": 1.2339640787414218e-16,
-                    "velocityX": 0.7917687906220042,
-                    "velocityY": -0.4663336217356026,
-                    "timestamp": 1.9068023075468512
-                },
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
                 {
-                    "x": 7.680079036283902,
-                    "y": 7.383775130350334,
-                    "heading": 3.14,
-                    "angularVelocity": 3.1298666285147066e-16,
-                    "velocityX": 0.0007655130286299905,
-                    "velocityY": -0.8060819521991853,
-                    "timestamp": 2.010048471814154
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
                 {
-                    "x": 7.598307382340287,
-                    "y": 7.265900859151241,
-                    "heading": 3.14,
-                    "angularVelocity": 4.0403721611171334e-16,
-                    "velocityX": -0.792006700916374,
-                    "velocityY": -1.141681843927082,
-                    "timestamp": 2.113294636081456
-                },
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W3 To C4": {
+            "waypoints": [
                 {
-                    "x": 7.434354633787184,
-                    "y": 7.114168304333743,
-                    "heading": 3.14,
-                    "angularVelocity": 4.565397791635157e-16,
-                    "velocityX": -1.5879790761875838,
-                    "velocityY": -1.4696192918573159,
-                    "timestamp": 2.2165408003487586
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 7.187443357098086,
-                    "y": 6.930529696196354,
+                    "x": 7.68,
+                    "y": 2.457,
                     "heading": 3.14,
-                    "angularVelocity": 3.88515705379733e-16,
-                    "velocityX": -2.3914813537270896,
-                    "velocityY": -1.7786482378362456,
-                    "timestamp": 2.319786964616061
-                },
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
                 {
-                    "x": 6.853793099378443,
-                    "y": 6.7274887650449,
-                    "heading": 3.14,
-                    "angularVelocity": -3.674797460353125e-16,
-                    "velocityX": -3.2315995474255925,
-                    "velocityY": -1.9665711805601358,
-                    "timestamp": 2.4230331288833633
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
                 {
-                    "x": 6.49770423144866,
-                    "y": 6.567024283806904,
-                    "heading": 3.14,
-                    "angularVelocity": 2.249424709718662e-16,
-                    "velocityX": -3.448930722577495,
-                    "velocityY": -1.5541931497092332,
-                    "timestamp": 2.5262792931506657
-                },
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "W3 To C5": {
+            "waypoints": [
                 {
-                    "x": 6.1416152766915,
-                    "y": 6.406559995248834,
-                    "heading": 3.14,
-                    "angularVelocity": 2.526804739134875e-16,
-                    "velocityX": -3.4489315635518585,
-                    "velocityY": -1.5541912834905027,
-                    "timestamp": 2.629525457417968
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 5.785526321934093,
-                    "y": 6.246095706691313,
+                    "x": 7.68,
+                    "y": 0.787,
                     "heading": 3.14,
-                    "angularVelocity": 2.2036218085246878e-16,
-                    "velocityX": -3.4489315635542535,
-                    "velocityY": -1.5541912834851872,
-                    "timestamp": 2.7327716216852704
-                },
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
                 {
-                    "x": 5.429437367176686,
-                    "y": 6.085631418133792,
-                    "heading": 3.14,
-                    "angularVelocity": 7.127301298549232e-19,
-                    "velocityX": -3.4489315635542535,
-                    "velocityY": -1.5541912834851876,
-                    "timestamp": 2.836017785952573
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
                 {
-                    "x": 5.07334841241929,
-                    "y": 5.925167129576275,
-                    "heading": 3.14,
-                    "angularVelocity": 1.0539129254381463e-16,
-                    "velocityX": -3.4489315635541473,
-                    "velocityY": -1.5541912834851401,
-                    "timestamp": 2.939263950219875
-                },
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S1 To W1": {
+            "waypoints": [
                 {
-                    "x": 4.749209064120322,
-                    "y": 5.7791002852572,
-                    "heading": 3.14,
-                    "angularVelocity": 8.727825696429895e-16,
-                    "velocityX": -3.1394807797389603,
-                    "velocityY": -1.4147435437980136,
-                    "timestamp": 3.0425101144871776
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 4.506104536371044,
-                    "y": 5.669550144571212,
-                    "heading": 3.14,
-                    "angularVelocity": 4.949097028202336e-16,
-                    "velocityX": -2.3546107448590976,
-                    "velocityY": -1.061057729974005,
-                    "timestamp": 3.14575627875448
-                },
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
                 {
-                    "x": 4.3440348464149885,
-                    "y": 5.596516715288762,
-                    "heading": 3.14,
-                    "angularVelocity": 3.5626049667024716e-16,
-                    "velocityX": -1.5697405429654518,
-                    "velocityY": -0.7073718408886114,
-                    "timestamp": 3.2490024430217823
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
                 {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 2.245803154155974e-16,
-                    "velocityX": -0.7848702854005415,
-                    "velocityY": -0.35368592671608795,
-                    "timestamp": 3.3522486072890847
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S1 To W2": {
+            "waypoints": [
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 4.263,
+                    "x": 2.25,
                     "y": 5.56,
                     "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": -4.241491761729664e-33,
-                    "velocityY": 0,
-                    "timestamp": 3.455494771556387
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 }
             ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -6126,335 +2438,69 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W2 To C2 To S1": {
+        "S1 To W3": {
             "waypoints": [
                 {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 3.14,
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 19
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 3.14,
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 13
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S1 To C1": {
+            "waypoints": [
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": 3.4031853071795863,
+                    "heading": -2.88,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.327391358066629,
-                    "y": 5.555067935622319,
-                    "heading": 3.133430176934097,
-                    "angularVelocity": -0.0681146690773455,
-                    "velocityX": 0.8023787994403143,
-                    "velocityY": -0.05113470021702577,
-                    "timestamp": 0.09645239644992105
-                },
-                {
-                    "x": 2.4821740708193567,
-                    "y": 5.545203805497392,
-                    "heading": 3.12029049964152,
-                    "angularVelocity": -0.13622966122359603,
-                    "velocityX": 1.6047575638319433,
-                    "velocityY": -0.10226941463344474,
-                    "timestamp": 0.1929047928998421
-                },
-                {
-                    "x": 2.7143481316892246,
-                    "y": 5.5304076076409565,
-                    "heading": 3.1005809392040384,
-                    "angularVelocity": -0.20434495318853443,
-                    "velocityX": 2.4071362601178543,
-                    "velocityY": -0.15340414962232987,
-                    "timestamp": 0.2893571893497632
-                },
-                {
-                    "x": 3.023913521714313,
-                    "y": 5.510679340033408,
-                    "heading": 3.0743015138082646,
-                    "angularVelocity": -0.2724600565981605,
-                    "velocityX": 3.2095147598102214,
-                    "velocityY": -0.2045389055500683,
-                    "timestamp": 0.3858095857996842
-                },
-                {
-                    "x": 3.388048508086598,
-                    "y": 5.487473394252625,
-                    "heading": 3.0743015107885725,
-                    "angularVelocity": -3.130758650216746e-8,
-                    "velocityX": 3.7752818983750913,
-                    "velocityY": -0.24059480774884917,
-                    "timestamp": 0.48226198224960526
-                },
-                {
-                    "x": 3.752183494458947,
-                    "y": 5.464267448472115,
-                    "heading": 3.0743015077689044,
-                    "angularVelocity": -3.130734274282633e-8,
-                    "velocityX": 3.7752818983757597,
-                    "velocityY": -0.24059480774599656,
-                    "timestamp": 0.5787143786995264
-                },
-                {
-                    "x": 4.116318480831297,
-                    "y": 5.441061502691605,
-                    "heading": 3.0743015047492364,
-                    "angularVelocity": -3.1307342782181776e-8,
-                    "velocityX": 3.77528189837576,
-                    "velocityY": -0.24059480774599656,
-                    "timestamp": 0.6751667751494475
-                },
-                {
-                    "x": 4.480453467203646,
-                    "y": 5.417855556911095,
-                    "heading": 3.0743015017295683,
-                    "angularVelocity": -3.130734248880116e-8,
-                    "velocityX": 3.7752818983757597,
-                    "velocityY": -0.2405948077459966,
-                    "timestamp": 0.7716191715993685
-                },
-                {
-                    "x": 4.844588453575996,
-                    "y": 5.3946496111305855,
-                    "heading": 3.0743014987098998,
-                    "angularVelocity": -3.130734337287564e-8,
-                    "velocityX": 3.77528189837576,
-                    "velocityY": -0.24059480774599656,
-                    "timestamp": 0.8680715680492896
-                },
-                {
-                    "x": 5.208723439948346,
-                    "y": 5.371443665350077,
-                    "heading": 3.0743014956902317,
-                    "angularVelocity": -3.130734341157956e-8,
-                    "velocityX": 3.7752818983757606,
-                    "velocityY": -0.24059480774599654,
-                    "timestamp": 0.9645239644992107
-                },
-                {
-                    "x": 5.572858426320695,
-                    "y": 5.348237719569567,
-                    "heading": 3.0743014926705636,
-                    "angularVelocity": -3.130734268712419e-8,
-                    "velocityX": 3.7752818983757597,
-                    "velocityY": -0.24059480774599656,
-                    "timestamp": 1.0609763609491318
-                },
-                {
-                    "x": 5.936993412693067,
-                    "y": 5.325031773789395,
-                    "heading": 3.0743014896508956,
-                    "angularVelocity": -3.130734257132029e-8,
-                    "velocityX": 3.7752818983759835,
-                    "velocityY": -0.24059480774249117,
-                    "timestamp": 1.157428757399053
-                },
-                {
-                    "x": 6.301128405222732,
-                    "y": 5.301825924626314,
-                    "heading": 3.0743014866312275,
-                    "angularVelocity": -3.1307343059822334e-8,
-                    "velocityX": 3.7752819622136293,
-                    "velocityY": -0.24059380603497976,
-                    "timestamp": 1.253881153848974
-                },
-                {
-                    "x": 6.666000662308113,
-                    "y": 5.302845713467377,
-                    "heading": 3.0743014835545788,
-                    "angularVelocity": -3.1898104709200575e-8,
-                    "velocityX": 3.7829257801264253,
-                    "velocityY": 0.010572975670877716,
-                    "timestamp": 1.3503335502988951
-                },
-                {
-                    "x": 7.022411728114523,
-                    "y": 5.380972301482392,
-                    "heading": 3.0743014635013113,
-                    "angularVelocity": -2.0790844162753672e-7,
-                    "velocityX": 3.6952017671376614,
-                    "velocityY": 0.8100015229334367,
-                    "timestamp": 1.4467859467488162
-                },
-                {
-                    "x": 7.302470785429282,
-                    "y": 5.472668060315495,
-                    "heading": 3.0808623231023553,
-                    "angularVelocity": 0.06802173758793187,
-                    "velocityX": 2.903598745316483,
-                    "velocityY": 0.9506840908894665,
-                    "timestamp": 1.5432383431987373
-                },
-                {
-                    "x": 7.505508952457786,
-                    "y": 5.5733927696291925,
-                    "heading": 3.0939992329731627,
-                    "angularVelocity": 0.1362009691239587,
-                    "velocityX": 2.1050608849716217,
-                    "velocityY": 1.0442945226974727,
-                    "timestamp": 1.6396907396486584
-                },
-                {
-                    "x": 7.631373643549882,
-                    "y": 5.681732940772809,
-                    "heading": 3.113712106449711,
-                    "angularVelocity": 0.20437930214396433,
-                    "velocityX": 1.3049410457877573,
-                    "velocityY": 1.1232501745031058,
-                    "timestamp": 1.7361431360985795
                 },
                 {
                     "x": 7.68,
-                    "y": 5.797,
+                    "y": 7.467,
                     "heading": 3.14,
-                    "angularVelocity": 0.27254785280465055,
-                    "velocityX": 0.5041487639486981,
-                    "velocityY": 1.1950668253954515,
-                    "timestamp": 1.8325955325485006
-                },
-                {
-                    "x": 7.657241486683139,
-                    "y": 5.911359552858986,
-                    "heading": 3.1705946317800286,
-                    "angularVelocity": 0.3367496839490721,
-                    "velocityX": -0.25049891829736254,
-                    "velocityY": 1.2587353088184159,
-                    "timestamp": 1.923448273436763
-                },
-                {
-                    "x": 7.565890794694817,
-                    "y": 6.031156159947816,
-                    "heading": 3.2067228278300997,
-                    "angularVelocity": 0.39765664411275126,
-                    "velocityX": -1.0054808594126166,
-                    "velocityY": 1.318580000092289,
-                    "timestamp": 2.0143010143250253
-                },
-                {
-                    "x": 7.40590178880199,
-                    "y": 6.1558149457566955,
-                    "heading": 3.247889559674286,
-                    "angularVelocity": 0.453114913669111,
-                    "velocityX": -1.7609706028527334,
-                    "velocityY": 1.3720971386234067,
-                    "timestamp": 2.1051537552132875
-                },
-                {
-                    "x": 7.1771984711004135,
-                    "y": 6.284201834322083,
-                    "heading": 3.293119082899787,
-                    "angularVelocity": 0.49783333758889264,
-                    "velocityX": -2.517296841741443,
-                    "velocityY": 1.4131317042298905,
-                    "timestamp": 2.1960064961015497
-                },
-                {
-                    "x": 6.879672767630686,
-                    "y": 6.413043079334037,
-                    "heading": 3.3395968476460443,
-                    "angularVelocity": 0.5115725105467077,
-                    "velocityX": -3.274812631526949,
-                    "velocityY": 1.4181327250260163,
-                    "timestamp": 2.286859236989812
-                },
-                {
-                    "x": 6.543860581996508,
-                    "y": 6.486209643935175,
-                    "heading": 3.33959685479452,
-                    "angularVelocity": 7.868200264443885e-8,
-                    "velocityX": -3.6962251479807913,
-                    "velocityY": 0.805331395462515,
-                    "timestamp": 2.377711977878074
-                },
-                {
-                    "x": 6.202053460852504,
-                    "y": 6.52214101663038,
-                    "heading": 3.33959685834771,
-                    "angularVelocity": 3.910933561720104e-8,
-                    "velocityX": -3.7622103395249686,
-                    "velocityY": 0.39549024436583907,
-                    "timestamp": 2.4685647187663364
-                },
-                {
-                    "x": 5.860246134397577,
-                    "y": 6.558070415270777,
-                    "heading": 3.339596863101522,
-                    "angularVelocity": 5.232436753390925e-8,
-                    "velocityX": -3.762212599345888,
-                    "velocityY": 0.39546851629479235,
-                    "timestamp": 2.5594174596545987
-                },
-                {
-                    "x": 5.5865423033177715,
-                    "y": 6.586842981129828,
-                    "heading": 3.3642400407233666,
-                    "angularVelocity": 0.2712430839279776,
-                    "velocityX": -3.0126095085719746,
-                    "velocityY": 0.31669452762507067,
-                    "timestamp": 2.650270200542861
-                },
-                {
-                    "x": 5.381269635001315,
-                    "y": 6.608421695048074,
-                    "heading": 3.3835025789166386,
-                    "angularVelocity": 0.2120193403626775,
-                    "velocityX": -2.2593998409901115,
-                    "velocityY": 0.23751307563506308,
-                    "timestamp": 2.741122941431123
-                },
-                {
-                    "x": 5.24442286400993,
-                    "y": 6.622807278395225,
-                    "heading": 3.3965772666434986,
-                    "angularVelocity": 0.14391076811804343,
-                    "velocityX": -1.5062481291531917,
-                    "velocityY": 0.15833956363345458,
-                    "timestamp": 2.8319756823193853
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": 0.07273352979206758,
-                    "velocityX": -0.753118324675337,
-                    "velocityY": 0.07916901058187684,
-                    "timestamp": 2.9228284232076476
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": -1.0239181929178098e-32,
-                    "velocityX": 1.4788561432464288e-34,
-                    "velocityY": 0,
-                    "timestamp": 3.01368116409591
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 }
             ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -6474,16 +2520,16 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W2 To C2 To S2": {
+        "S1 To C2": {
             "waypoints": [
                 {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 3.14,
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 19
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 7.68,
@@ -6492,326 +2538,133 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 14
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.3282521020736953,
-                    "y": 5.564172492511594,
-                    "heading": 3.14,
-                    "angularVelocity": 1.232216948997824e-17,
-                    "velocityX": 0.8071870820603332,
-                    "velocityY": 0.043040148021588745,
-                    "timestamp": 0.09694419527026792
-                },
-                {
-                    "x": 2.484756303050172,
-                    "y": 5.5725174773657065,
-                    "heading": 3.14,
-                    "angularVelocity": 2.4529935765970837e-17,
-                    "velocityX": 1.6143741314120201,
-                    "velocityY": 0.08608029429911494,
-                    "timestamp": 0.19388839054053583
-                },
-                {
-                    "x": 2.7195125965876055,
-                    "y": 5.5850349542241835,
-                    "heading": 3.14,
-                    "angularVelocity": 3.650998333784998e-17,
-                    "velocityX": 2.421561115346432,
-                    "velocityY": 0.12912043708851692,
-                    "timestamp": 0.29083258581080373
-                },
-                {
-                    "x": 3.032520963660537,
-                    "y": 5.601724922072566,
-                    "heading": 3.14,
-                    "angularVelocity": 4.7804313451185203e-17,
-                    "velocityX": 3.2287479030291797,
-                    "velocityY": 0.17216056941355468,
-                    "timestamp": 0.38777678108107166
-                },
-                {
-                    "x": 3.398734861706694,
-                    "y": 5.621251870322576,
-                    "heading": 3.14,
-                    "angularVelocity": 4.271471690686359e-16,
-                    "velocityX": 3.777574273786295,
-                    "velocityY": 0.20142462579390313,
-                    "timestamp": 0.4847209763513396
-                },
-                {
-                    "x": 3.7649487597528926,
-                    "y": 5.6407788185725884,
-                    "heading": 3.14,
-                    "angularVelocity": 3.505027380693511e-16,
-                    "velocityX": 3.7775742737867146,
-                    "velocityY": 0.20142462579393414,
-                    "timestamp": 0.5816651716216075
-                },
-                {
-                    "x": 4.1311626577990905,
-                    "y": 5.660305766822602,
-                    "heading": 3.14,
-                    "angularVelocity": 3.7529805795054796e-16,
-                    "velocityX": 3.7775742737867146,
-                    "velocityY": 0.20142462579393414,
-                    "timestamp": 0.6786093668918753
-                },
-                {
-                    "x": 4.497376555845288,
-                    "y": 5.679832715072616,
-                    "heading": 3.14,
-                    "angularVelocity": 4.2292762480360005e-16,
-                    "velocityX": 3.7775742737867146,
-                    "velocityY": 0.20142462579393414,
-                    "timestamp": 0.7755535621621432
-                },
-                {
-                    "x": 4.8635904538914865,
-                    "y": 5.699359663322629,
-                    "heading": 3.14,
-                    "angularVelocity": -1.0631316332366903e-16,
-                    "velocityX": 3.7775742737867146,
-                    "velocityY": 0.20142462579393414,
-                    "timestamp": 0.8724977574324111
-                },
-                {
-                    "x": 5.229804351937685,
-                    "y": 5.718886611572643,
-                    "heading": 3.14,
-                    "angularVelocity": -2.15507046342417e-16,
-                    "velocityX": 3.7775742737867146,
-                    "velocityY": 0.2014246257939341,
-                    "timestamp": 0.969441952702679
-                },
-                {
-                    "x": 5.596018249983883,
-                    "y": 5.738413559822655,
-                    "heading": 3.14,
-                    "angularVelocity": 2.7855582274724927e-16,
-                    "velocityX": 3.7775742737867146,
-                    "velocityY": 0.20142462579393414,
-                    "timestamp": 1.0663861479729468
-                },
-                {
-                    "x": 5.962232148030081,
-                    "y": 5.757940508072669,
-                    "heading": 3.14,
-                    "angularVelocity": 3.1548114792697624e-17,
-                    "velocityX": 3.7775742737867146,
-                    "velocityY": 0.201424625793935,
-                    "timestamp": 1.1633303432432147
-                },
-                {
-                    "x": 6.3284460460762855,
-                    "y": 5.777467456322548,
-                    "heading": 3.14,
-                    "angularVelocity": 5.639488659636306e-17,
-                    "velocityX": 3.777574273786789,
-                    "velocityY": 0.20142462579255413,
-                    "timestamp": 1.2602745385134826
-                },
-                {
-                    "x": 6.694659946182267,
-                    "y": 5.796994365942685,
-                    "heading": 3.14,
-                    "angularVelocity": 1.8236235813910642e-16,
-                    "velocityX": 3.7775742950338076,
-                    "velocityY": 0.20142422731870196,
-                    "timestamp": 1.3572187337837505
-                },
-                {
-                    "x": 7.048232130231478,
-                    "y": 5.805340495974463,
-                    "heading": 3.14,
-                    "angularVelocity": 9.880815779139735e-16,
-                    "velocityX": 3.6471723000895206,
-                    "velocityY": 0.08609210670542117,
-                    "timestamp": 1.4541629290540183
-                },
+            "trajectory": [],
+            "constraints": [
                 {
-                    "x": 7.323552241539944,
-                    "y": 5.8095141201136,
-                    "heading": 3.14,
-                    "angularVelocity": 1.0046882743013462e-15,
-                    "velocityX": 2.839985520604805,
-                    "velocityY": 0.04305182065552602,
-                    "timestamp": 1.5511071243242862
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
                 {
-                    "x": 7.520620260713227,
-                    "y": 5.809515244263857,
-                    "heading": 3.14,
-                    "angularVelocity": 1.1309282599454547e-15,
-                    "velocityX": 2.032798541062352,
-                    "velocityY": 0.000011595504179921879,
-                    "timestamp": 1.648051319594554
-                },
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S1 To C3": {
+            "waypoints": [
                 {
-                    "x": 7.639436181279198,
-                    "y": 5.805343870530581,
-                    "heading": 3.14,
-                    "angularVelocity": 1.1596973638353238e-15,
-                    "velocityX": 1.2256114947584758,
-                    "velocityY": -0.043028607930100944,
-                    "timestamp": 1.744995514864822
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 7.68,
-                    "y": 5.797,
-                    "heading": 3.14,
-                    "angularVelocity": 1.1762980395606437e-15,
-                    "velocityX": 0.41842441505544703,
-                    "velocityY": -0.08606880015967894,
-                    "timestamp": 1.8419397101350898
-                },
-                {
-                    "x": 7.637810963282883,
-                    "y": 5.783845940195137,
-                    "heading": 3.14,
-                    "angularVelocity": 1.2081191600227442e-15,
-                    "velocityX": -0.41931671098523704,
-                    "velocityY": -0.1307381629639976,
-                    "timestamp": 1.94255348238971
-                },
-                {
-                    "x": 7.511333424065826,
-                    "y": 5.766201461544033,
-                    "heading": 3.14,
-                    "angularVelocity": 1.221716047001012e-15,
-                    "velocityX": -1.257059901260624,
-                    "velocityY": -0.1753684232696404,
-                    "timestamp": 2.04316725464433
-                },
-                {
-                    "x": 7.3005670350867495,
-                    "y": 5.744073149987429,
-                    "heading": 3.14,
-                    "angularVelocity": 9.674484857548948e-16,
-                    "velocityX": -2.0948065429741827,
-                    "velocityY": -0.2199332259036739,
-                    "timestamp": 2.14378102689895
-                },
-                {
-                    "x": 7.005511097627955,
-                    "y": 5.717474286699145,
-                    "heading": 3.14,
-                    "angularVelocity": 6.103023189064465e-16,
-                    "velocityX": -2.9325601292436843,
-                    "velocityY": -0.2643660269377714,
-                    "timestamp": 2.2443947991535698
-                },
-                {
-                    "x": 6.626163481456156,
-                    "y": 5.686445611767655,
-                    "heading": 3.14,
-                    "angularVelocity": -3.898195701943848e-16,
-                    "velocityX": -3.7703348879039833,
-                    "velocityY": -0.30839391219134293,
-                    "timestamp": 2.3450085714081896
-                },
-                {
-                    "x": 6.24609124574338,
-                    "y": 5.666109085648216,
+                    "y": 4.127,
                     "heading": 3.14,
-                    "angularVelocity": 5.28249015807614e-17,
-                    "velocityX": -3.7775368838161545,
-                    "velocityY": -0.2021246762112827,
-                    "timestamp": 2.4456223436628095
-                },
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
                 {
-                    "x": 5.866019008161075,
-                    "y": 5.6457725944687756,
-                    "heading": 3.14,
-                    "angularVelocity": 1.0714715731827337e-16,
-                    "velocityX": -3.777536902397429,
-                    "velocityY": -0.20212432894236246,
-                    "timestamp": 2.5462361159174294
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
                 {
-                    "x": 5.485946770578765,
-                    "y": 5.625436103289443,
-                    "heading": 3.14,
-                    "angularVelocity": 4.4105778021487463e-16,
-                    "velocityX": -3.7775369023974865,
-                    "velocityY": -0.2021243289413037,
-                    "timestamp": 2.6468498881720492
-                },
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S1 To C4": {
+            "waypoints": [
                 {
-                    "x": 5.105874532996473,
-                    "y": 5.60509961211012,
-                    "heading": 3.14,
-                    "angularVelocity": 4.563638430357427e-16,
-                    "velocityX": -3.7775369023972933,
-                    "velocityY": -0.20212432894121146,
-                    "timestamp": 2.747463660426669
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 4.768724738760508,
-                    "y": 5.587059768280702,
+                    "x": 7.68,
+                    "y": 2.457,
                     "heading": 3.14,
-                    "angularVelocity": 7.096592098456484e-16,
-                    "velocityX": -3.3509308606209816,
-                    "velocityY": -0.1792979574700793,
-                    "timestamp": 2.848077432681289
-                },
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
                 {
-                    "x": 4.5158623742843815,
-                    "y": 5.573529884402755,
-                    "heading": 3.14,
-                    "angularVelocity": 5.58698579821863e-16,
-                    "velocityX": -2.5131983323104805,
-                    "velocityY": -0.13447347810003318,
-                    "timestamp": 2.948691204935909
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
                 {
-                    "x": 4.3472874591846,
-                    "y": 5.564509961525897,
-                    "heading": 3.14,
-                    "angularVelocity": 2.9765197430594465e-16,
-                    "velocityX": -1.6754656090315643,
-                    "velocityY": -0.08964898829784126,
-                    "timestamp": 3.0493049771905287
-                },
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S1 To C5": {
+            "waypoints": [
                 {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 1.413897346526725e-16,
-                    "velocityX": -0.8377328207631561,
-                    "velocityY": -0.04482449501826641,
-                    "timestamp": 3.1499187494451486
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 4.263,
-                    "y": 5.56,
+                    "x": 7.68,
+                    "y": 0.787,
                     "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": -1.608253361776519e-31,
-                    "velocityY": 0,
-                    "timestamp": 3.2505325216997685
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 }
             ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -6831,371 +2684,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W2 To C3 To S1": {
+        "S2 To W1": {
             "waypoints": [
                 {
-                    "x": 2.25,
+                    "x": 4.263,
                     "y": 5.56,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 20
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 16
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 3.4031853071795863,
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.3161821396104223,
-                    "y": 5.531175019358262,
-                    "heading": 3.133818954660017,
-                    "angularVelocity": -0.0664208658906851,
-                    "velocityX": 0.711186341084945,
-                    "velocityY": -0.3097502231737145,
-                    "timestamp": 0.09305878893773292
-                },
-                {
-                    "x": 2.448546415336416,
-                    "y": 5.473525059620157,
-                    "heading": 3.121456701812491,
-                    "angularVelocity": -0.13284347441699146,
-                    "velocityX": 1.4223726446145823,
-                    "velocityY": -0.6195004297410188,
-                    "timestamp": 0.18611757787546584
-                },
-                {
-                    "x": 2.647092820298068,
-                    "y": 5.387050123667136,
-                    "heading": 3.1029129720677258,
-                    "angularVelocity": -0.19926897777675723,
-                    "velocityX": 2.1335588742134064,
-                    "velocityY": -0.9292506053445697,
-                    "timestamp": 0.2791763668131988
-                },
-                {
-                    "x": 2.911821335852912,
-                    "y": 5.271750219180522,
-                    "heading": 3.078187424791321,
-                    "angularVelocity": -0.26569814155811494,
-                    "velocityX": 2.844744903482224,
-                    "velocityY": -1.2390006984054311,
-                    "timestamp": 0.3722351557509317
-                },
-                {
-                    "x": 3.2345733580792335,
-                    "y": 5.131178588369686,
-                    "heading": 3.07818742179356,
-                    "angularVelocity": -3.221363014134022e-8,
-                    "velocityX": 3.468259429448196,
-                    "velocityY": -1.5105680228107692,
-                    "timestamp": 0.4652939446886646
-                },
-                {
-                    "x": 3.557325380260581,
-                    "y": 4.990606957455228,
-                    "heading": 3.078187418795868,
-                    "angularVelocity": -3.2212882318357466e-8,
-                    "velocityX": 3.4682594289649007,
-                    "velocityY": -1.510568023924279,
-                    "timestamp": 0.5583527336263975
-                },
-                {
-                    "x": 3.8800774024419273,
-                    "y": 4.85003532654077,
-                    "heading": 3.0781874157981757,
-                    "angularVelocity": -3.221288214692397e-8,
-                    "velocityX": 3.468259428964899,
-                    "velocityY": -1.5105680239242834,
-                    "timestamp": 0.6514115225641304
-                },
-                {
-                    "x": 4.202829424623274,
-                    "y": 4.709463695626312,
-                    "heading": 3.078187412800484,
-                    "angularVelocity": -3.221288189984943e-8,
-                    "velocityX": 3.468259428964899,
-                    "velocityY": -1.5105680239242838,
-                    "timestamp": 0.7444703115018633
-                },
-                {
-                    "x": 4.525581446804621,
-                    "y": 4.568892064711854,
-                    "heading": 3.078187409802792,
-                    "angularVelocity": -3.2212882353617496e-8,
-                    "velocityX": 3.468259428964899,
-                    "velocityY": -1.5105680239242834,
-                    "timestamp": 0.8375291004395962
-                },
-                {
-                    "x": 4.848333468985968,
-                    "y": 4.428320433797395,
-                    "heading": 3.0781874068051,
-                    "angularVelocity": -3.221288275372931e-8,
-                    "velocityX": 3.468259428964899,
-                    "velocityY": -1.5105680239242836,
-                    "timestamp": 0.9305878893773291
-                },
-                {
-                    "x": 5.1710854911673145,
-                    "y": 4.287748802882937,
-                    "heading": 3.0781874038074077,
-                    "angularVelocity": -3.221288266203546e-8,
-                    "velocityX": 3.4682594289648985,
-                    "velocityY": -1.5105680239242834,
-                    "timestamp": 1.023646678315062
-                },
-                {
-                    "x": 5.493837513348661,
-                    "y": 4.147177171968479,
-                    "heading": 3.078187400809716,
-                    "angularVelocity": -3.221288308232409e-8,
-                    "velocityX": 3.4682594289648994,
-                    "velocityY": -1.5105680239242831,
-                    "timestamp": 1.116705467252795
-                },
-                {
-                    "x": 5.816589535530807,
-                    "y": 4.006605541055855,
-                    "heading": 3.078187397812024,
-                    "angularVelocity": -3.221288143740925e-8,
-                    "velocityX": 3.4682594289734867,
-                    "velocityY": -1.510568023904566,
-                    "timestamp": 1.209764256190528
-                },
-                {
-                    "x": 6.139341766628845,
-                    "y": 3.866034389814445,
-                    "heading": 3.0781873948143317,
-                    "angularVelocity": -3.2212889577605074e-8,
-                    "velocityX": 3.4682616739617793,
-                    "velocityY": -1.5105628694079405,
-                    "timestamp": 1.302823045128261
-                },
-                {
-                    "x": 6.479618734001887,
-                    "y": 3.7758078084398288,
-                    "heading": 3.078187391647213,
-                    "angularVelocity": -3.403352419183157e-8,
-                    "velocityX": 3.656580654630348,
-                    "velocityY": -0.9695653941401335,
-                    "timestamp": 1.395881834065994
-                },
-                {
-                    "x": 6.831146782632028,
-                    "y": 3.7569067855053886,
-                    "heading": 3.078187386672884,
-                    "angularVelocity": -5.3453615233680875e-8,
-                    "velocityX": 3.777483595508151,
-                    "velocityY": -0.20310841297416227,
-                    "timestamp": 1.488940623003727
-                },
-                {
-                    "x": 7.135392516475817,
-                    "y": 3.7925535020762564,
-                    "heading": 3.084237894820064,
-                    "angularVelocity": 0.06501812688781647,
-                    "velocityX": 3.269392792628807,
-                    "velocityY": 0.3830558830366858,
-                    "timestamp": 1.58199941194146
-                },
-                {
-                    "x": 7.379778215429103,
-                    "y": 3.868544177421427,
-                    "heading": 3.09656532372019,
-                    "angularVelocity": 0.1324692599252942,
-                    "velocityX": 2.6261431267583744,
-                    "velocityY": 0.8165878388554683,
-                    "timestamp": 1.675058200879193
-                },
-                {
-                    "x": 7.561679450282434,
-                    "y": 3.980681271949268,
-                    "heading": 3.1151578247073277,
-                    "angularVelocity": 0.1997930684395467,
-                    "velocityX": 1.954691619456221,
-                    "velocityY": 1.2050134738253881,
-                    "timestamp": 1.768116989816926
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": 0.26695141400662953,
-                    "velocityX": 1.2714602357090166,
-                    "velocityY": 1.572325727864734,
-                    "timestamp": 1.861175778754659
-                },
-                {
-                    "x": 7.734511016491924,
-                    "y": 4.293948601996246,
-                    "heading": 3.168900193264706,
-                    "angularVelocity": 0.3300287650543787,
-                    "velocityX": 0.6224942265925568,
-                    "velocityY": 1.9064869372920104,
-                    "timestamp": 1.948744488894043
-                },
-                {
-                    "x": 7.731795753069985,
-                    "y": 4.489388700342548,
-                    "heading": 3.202314872595517,
-                    "angularVelocity": 0.38158240857521825,
-                    "velocityX": -0.031007233264223562,
-                    "velocityY": 2.2318485453904535,
-                    "timestamp": 2.0363131990334273
-                },
-                {
-                    "x": 7.671228890668297,
-                    "y": 4.712045759892295,
-                    "heading": 3.2385899184542097,
-                    "angularVelocity": 0.41424666186076586,
-                    "velocityX": -0.6916495892800459,
-                    "velocityY": 2.5426554667225383,
-                    "timestamp": 2.1238819091728116
-                },
-                {
-                    "x": 7.551691509705012,
-                    "y": 4.959414788228928,
-                    "heading": 3.2745248408990206,
-                    "angularVelocity": 0.4103625871342999,
-                    "velocityX": -1.3650695639231716,
-                    "velocityY": 2.824856366422358,
-                    "timestamp": 2.211450619312196
-                },
-                {
-                    "x": 7.370754702688456,
-                    "y": 5.224445119410505,
-                    "heading": 3.301432833728465,
-                    "angularVelocity": 0.30727862482631746,
-                    "velocityX": -2.066226700479633,
-                    "velocityY": 3.0265414525316805,
-                    "timestamp": 2.29901932945158
-                },
-                {
-                    "x": 7.135099709363178,
-                    "y": 5.457264131297548,
-                    "heading": 3.3014328467429856,
-                    "angularVelocity": 1.4862067021464798e-7,
-                    "velocityX": -2.6910867243583128,
-                    "velocityY": 2.658700939142107,
-                    "timestamp": 2.3865880395909644
-                },
-                {
-                    "x": 6.859107156435953,
-                    "y": 5.640474617463791,
-                    "heading": 3.3014328522641176,
-                    "angularVelocity": 6.304914289451061e-8,
-                    "velocityX": -3.1517256847557116,
-                    "velocityY": 2.0921912161846987,
-                    "timestamp": 2.4741567497303487
-                },
-                {
-                    "x": 6.573538412185887,
-                    "y": 5.808368822234562,
-                    "heading": 3.3014328577109833,
-                    "angularVelocity": 6.220104875802801e-8,
-                    "velocityX": -3.26108199830194,
-                    "velocityY": 1.9172853465984772,
-                    "timestamp": 2.561725459869733
-                },
-                {
-                    "x": 6.287969619272061,
-                    "y": 5.976262944233841,
-                    "heading": 3.301432863157849,
-                    "angularVelocity": 6.220104736843505e-8,
-                    "velocityX": -3.2610825540228303,
-                    "velocityY": 1.9172844013808124,
-                    "timestamp": 2.6492941700091173
-                },
-                {
-                    "x": 6.002400826335374,
-                    "y": 6.144157066194225,
-                    "heading": 3.3014328686047243,
-                    "angularVelocity": 6.2201164432783e-8,
-                    "velocityX": -3.2610825542838673,
-                    "velocityY": 1.9172844009366552,
-                    "timestamp": 2.7368628801485015
-                },
-                {
-                    "x": 5.726906238126056,
-                    "y": 6.306118202865447,
-                    "heading": 3.330104518338368,
-                    "angularVelocity": 0.3274188884135285,
-                    "velocityX": -3.14603912482906,
-                    "velocityY": 1.849532057894034,
-                    "timestamp": 2.824431590287886
-                },
-                {
-                    "x": 5.5065316534228295,
-                    "y": 6.435677156249956,
-                    "heading": 3.357685145070299,
-                    "angularVelocity": 0.31495983768665414,
-                    "velocityX": -2.5165905076419874,
-                    "velocityY": 1.47951195327976,
-                    "timestamp": 2.91200030042727
-                },
-                {
-                    "x": 5.341261355658222,
-                    "y": 6.532840911354814,
-                    "heading": 3.379877882623065,
-                    "angularVelocity": 0.2534322764083417,
-                    "velocityX": -1.8873213674330116,
-                    "velocityY": 1.1095716146806525,
-                    "timestamp": 2.9995690105666544
-                },
-                {
-                    "x": 5.231085952105995,
-                    "y": 6.597614249467722,
-                    "heading": 3.395277171030749,
-                    "angularVelocity": 0.17585377680193118,
-                    "velocityX": -1.2581594884389657,
-                    "velocityY": 0.7396858764940984,
-                    "timestamp": 3.0871377207060386
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": 0.09030778386766043,
-                    "velocityX": -0.6290597636794472,
-                    "velocityY": 0.3698324490646197,
-                    "timestamp": 3.174706430845423
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": 0,
-                    "velocityX": -1.4368373863628223e-33,
-                    "velocityY": 0,
-                    "timestamp": 3.262275140984807
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -7215,28 +2725,19 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W2 To C3 To S2": {
+        "S2 To W2": {
             "waypoints": [
                 {
-                    "x": 2.25,
+                    "x": 4.263,
                     "y": 5.56,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 20
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 4.263,
+                    "x": 2.25,
                     "y": 5.56,
                     "heading": 3.14,
                     "isInitialGuess": false,
@@ -7245,332 +2746,48 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.3211310801785023,
-                    "y": 5.537078029621854,
-                    "heading": 3.14,
-                    "angularVelocity": 6.136916649287371e-17,
-                    "velocityX": 0.7513410861003985,
-                    "velocityY": -0.24211945152891107,
-                    "timestamp": 0.09467215555545593
-                },
-                {
-                    "x": 2.4633932375532455,
-                    "y": 5.491234089826592,
-                    "heading": 3.1399999999999997,
-                    "angularVelocity": -6.918142497868558e-18,
-                    "velocityX": 1.5026821406998672,
-                    "velocityY": -0.48423889290665645,
-                    "timestamp": 0.18934431111091185
-                },
-                {
-                    "x": 2.676786466159711,
-                    "y": 5.422468182536282,
-                    "heading": 3.14,
-                    "angularVelocity": -1.447784282504661e-16,
-                    "velocityX": 2.254023132297508,
-                    "velocityY": -0.7263583139820807,
-                    "timestamp": 0.28401646666636776
-                },
-                {
-                    "x": 2.9613107481043692,
-                    "y": 5.330780313517108,
-                    "heading": 3.14,
-                    "angularVelocity": -2.19469189514941e-16,
-                    "velocityX": 3.005363934889942,
-                    "velocityY": -0.9684776741506306,
-                    "timestamp": 0.3786886222218237
-                },
-                {
-                    "x": 3.30218779492529,
-                    "y": 5.220932782027199,
-                    "heading": 3.14,
-                    "angularVelocity": 3.7716439529503614e-16,
-                    "velocityX": 3.6006051073934406,
-                    "velocityY": -1.160293972873203,
-                    "timestamp": 0.47336077777727964
-                },
-                {
-                    "x": 3.643064841746281,
-                    "y": 5.111085250537267,
-                    "heading": 3.14,
-                    "angularVelocity": 1.8926244658706998e-16,
-                    "velocityX": 3.600605107394178,
-                    "velocityY": -1.1602939728734405,
-                    "timestamp": 0.5680329333327355
-                },
-                {
-                    "x": 3.9839418885672715,
-                    "y": 5.001237719047335,
-                    "heading": 3.14,
-                    "angularVelocity": 3.4257793215779713e-16,
-                    "velocityX": 3.6006051073941783,
-                    "velocityY": -1.1602939728734407,
-                    "timestamp": 0.6627050888881915
-                },
-                {
-                    "x": 4.3248189353882625,
-                    "y": 4.891390187557403,
-                    "heading": 3.14,
-                    "angularVelocity": 4.383897524805657e-17,
-                    "velocityX": 3.6006051073941783,
-                    "velocityY": -1.1602939728734407,
-                    "timestamp": 0.7573772444436474
-                },
-                {
-                    "x": 4.665695982209253,
-                    "y": 4.7815426560674705,
-                    "heading": 3.14,
-                    "angularVelocity": 1.5398680786578701e-19,
-                    "velocityX": 3.600605107394178,
-                    "velocityY": -1.160293972873441,
-                    "timestamp": 0.8520493999991033
-                },
-                {
-                    "x": 5.0065730290302435,
-                    "y": 4.671695124577538,
-                    "heading": 3.14,
-                    "angularVelocity": 4.356845547140055e-17,
-                    "velocityX": 3.600605107394178,
-                    "velocityY": -1.1602939728734414,
-                    "timestamp": 0.9467215555545593
-                },
-                {
-                    "x": 5.347450075851235,
-                    "y": 4.561847593087606,
-                    "heading": 3.14,
-                    "angularVelocity": 2.68380689151133e-16,
-                    "velocityX": 3.600605107394178,
-                    "velocityY": -1.160293972873442,
-                    "timestamp": 1.041393711110015
-                },
-                {
-                    "x": 5.688327122672225,
-                    "y": 4.452000061597674,
-                    "heading": 3.14,
-                    "angularVelocity": 3.726757040891467e-16,
-                    "velocityX": 3.600605107394178,
-                    "velocityY": -1.1602939728734432,
-                    "timestamp": 1.136065866665471
-                },
-                {
-                    "x": 6.029204169493216,
-                    "y": 4.342152530107741,
-                    "heading": 3.14,
-                    "angularVelocity": 1.8883338122910111e-16,
-                    "velocityX": 3.6006051073941774,
-                    "velocityY": -1.1602939728734463,
-                    "timestamp": 1.230738022220927
-                },
-                {
-                    "x": 6.370081216318914,
-                    "y": 4.2323049986209496,
-                    "heading": 3.14,
-                    "angularVelocity": 2.0987063063623018e-16,
-                    "velocityX": 3.600605107443935,
-                    "velocityY": -1.1602939728401256,
-                    "timestamp": 1.325410177776383
-                },
-                {
-                    "x": 6.710958611265409,
-                    "y": 4.122458547431971,
-                    "heading": 3.14,
-                    "angularVelocity": 1.3456713754312597e-16,
-                    "velocityX": 3.600608784563045,
-                    "velocityY": -1.1602825619053307,
-                    "timestamp": 1.4200823333318389
-                },
-                {
-                    "x": 7.047028916950751,
-                    "y": 4.077522582297829,
-                    "heading": 3.14,
-                    "angularVelocity": 3.8640318477602567e-16,
-                    "velocityX": 3.549832616713609,
-                    "velocityY": -0.4746481673570916,
-                    "timestamp": 1.5147544888872948
-                },
-                {
-                    "x": 7.31196824975774,
-                    "y": 4.055508828572456,
-                    "heading": 3.14,
-                    "angularVelocity": 5.274988476955795e-16,
-                    "velocityX": 2.7984926639997774,
-                    "velocityY": -0.23252616987768301,
-                    "timestamp": 1.6094266444427507
-                },
-                {
-                    "x": 7.5057765524653535,
-                    "y": 4.056417169982893,
-                    "heading": 3.1400000000000006,
-                    "angularVelocity": 4.746542936809208e-16,
-                    "velocityX": 2.0471521068735608,
-                    "velocityY": 0.009594599437474344,
-                    "timestamp": 1.7040987999982067
-                },
-                {
-                    "x": 7.628453805780265,
-                    "y": 4.080247567089719,
-                    "heading": 3.14,
-                    "angularVelocity": 5.453879442658063e-16,
-                    "velocityX": 1.295811345956418,
-                    "velocityY": 0.2517149521631642,
-                    "timestamp": 1.7987709555536626
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": 4.116008282611605e-16,
-                    "velocityX": 0.5444704825542928,
-                    "velocityY": 0.49383509476441706,
-                    "timestamp": 1.8934431111091186
-                },
-                {
-                    "x": 7.6568603335776855,
-                    "y": 4.200231008500336,
-                    "heading": 3.14,
-                    "angularVelocity": 4.411427948098232e-16,
-                    "velocityX": -0.23545951737158613,
-                    "velocityY": 0.7451679554678371,
-                    "timestamp": 1.9917176114589898
-                },
-                {
-                    "x": 7.557063724755448,
-                    "y": 4.2981314655224265,
-                    "heading": 3.14,
-                    "angularVelocity": 5.768827951780328e-16,
-                    "velocityX": -1.0154883359055142,
-                    "velocityY": 0.9961938923479718,
-                    "timestamp": 2.0899921118088614
-                },
-                {
-                    "x": 7.380593913469942,
-                    "y": 4.42065077709202,
-                    "heading": 3.14,
-                    "angularVelocity": 6.878777879965837e-16,
-                    "velocityX": -1.7956826100081775,
-                    "velocityY": 1.246705006216337,
-                    "timestamp": 2.1882666121587326
-                },
-                {
-                    "x": 7.127418097854458,
-                    "y": 4.5676865279586805,
-                    "heading": 3.1400000000000006,
-                    "angularVelocity": 5.966587967087216e-16,
-                    "velocityX": -2.576210662116218,
-                    "velocityY": 1.4961739855526293,
-                    "timestamp": 2.286541112508604
-                },
-                {
-                    "x": 6.7974355684839916,
-                    "y": 4.73892129278293,
-                    "heading": 3.1400000000000006,
-                    "angularVelocity": 9.996236488627517e-17,
-                    "velocityX": -3.357763490993921,
-                    "velocityY": 1.7424129780831201,
-                    "timestamp": 2.384815612858475
-                },
-                {
-                    "x": 6.443766002914483,
-                    "y": 4.853499817181314,
-                    "heading": 3.1400000000000006,
-                    "angularVelocity": -3.2402717865838093e-16,
-                    "velocityX": -3.5987928130938722,
-                    "velocityY": 1.1659028943466263,
-                    "timestamp": 2.4830901132083465
-                },
-                {
-                    "x": 6.090096257463813,
-                    "y": 4.968077786339023,
-                    "heading": 3.1400000000000006,
-                    "angularVelocity": -3.4427620459908776e-16,
-                    "velocityX": -3.5987946434889486,
-                    "velocityY": 1.165897244450957,
-                    "timestamp": 2.581364613558218
-                },
-                {
-                    "x": 5.736426512012546,
-                    "y": 5.082655755494858,
-                    "heading": 3.1400000000000006,
-                    "angularVelocity": -3.8345847210221107e-16,
-                    "velocityX": -3.598794643495036,
-                    "velocityY": 1.165897244431872,
-                    "timestamp": 2.679639113908089
-                },
-                {
-                    "x": 5.382756766561277,
-                    "y": 5.197233724650691,
-                    "heading": 3.1400000000000006,
-                    "angularVelocity": -2.673892854874729e-16,
-                    "velocityX": -3.5987946434950357,
-                    "velocityY": 1.165897244431872,
-                    "timestamp": 2.7779136142579604
-                },
-                {
-                    "x": 5.02908702111004,
-                    "y": 5.311811693806516,
-                    "heading": 3.1400000000000006,
-                    "angularVelocity": -2.8589912756820243e-16,
-                    "velocityX": -3.598794643494731,
-                    "velocityY": 1.1658972444317732,
-                    "timestamp": 2.8761881146078316
-                },
-                {
-                    "x": 4.722652230446586,
-                    "y": 5.411087010523561,
-                    "heading": 3.14,
-                    "angularVelocity": 1.1044854210040432e-15,
-                    "velocityX": -3.1181516016108115,
-                    "velocityY": 1.0101838865993877,
-                    "timestamp": 2.974462614957703
-                },
+            "trajectory": [],
+            "constraints": [
                 {
-                    "x": 4.492826119821717,
-                    "y": 5.485543503772035,
-                    "heading": 3.14,
-                    "angularVelocity": 7.043876441844827e-16,
-                    "velocityX": -2.338613880575899,
-                    "velocityY": 0.7576379730591117,
-                    "timestamp": 3.072737115307574
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
                 {
-                    "x": 4.33960870762911,
-                    "y": 5.535181167592957,
-                    "heading": 3.14,
-                    "angularVelocity": 5.590882375321651e-16,
-                    "velocityX": -1.5590759723746228,
-                    "velocityY": 0.5050919988827703,
-                    "timestamp": 3.1710116156574455
-                },
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S2 To W3": {
+            "waypoints": [
                 {
                     "x": 4.263,
                     "y": 5.56,
                     "heading": 3.14,
-                    "angularVelocity": 3.3318762312703263e-16,
-                    "velocityX": -0.7795380017845207,
-                    "velocityY": 0.2525460044943946,
-                    "timestamp": 3.269286116007317
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": -3.7380886634717777e-32,
-                    "velocityY": 0,
-                    "timestamp": 3.367560616357188
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 }
             ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -7590,380 +2807,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W2 To C3 To S3": {
+        "S2 To C1": {
             "waypoints": [
                 {
-                    "x": 2.25,
+                    "x": 4.263,
                     "y": 5.56,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 20
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 7.68,
-                    "y": 4.127,
+                    "y": 7.467,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 17
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -2.1257809849356417e-31,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.321508847678769,
-                    "y": 5.553282811958262,
-                    "heading": 3.1537553335344852,
-                    "angularVelocity": 0.14810177732022164,
-                    "velocityX": 0.7699258915674908,
-                    "velocityY": -0.07232303637577411,
-                    "timestamp": 0.09287757232476541
-                },
-                {
-                    "x": 2.4645265668919087,
-                    "y": 5.539848505148353,
-                    "heading": 3.1812640229350246,
-                    "angularVelocity": 0.2961822613574502,
-                    "velocityX": 1.53985203998495,
-                    "velocityY": -0.14464532689263399,
-                    "timestamp": 0.18575514464953083
-                },
-                {
-                    "x": 2.679053206199872,
-                    "y": 5.519697173536265,
-                    "heading": 3.2225218511092746,
-                    "angularVelocity": 0.4442173405435694,
-                    "velocityX": 2.309778711246101,
-                    "velocityY": -0.21696660569059611,
-                    "timestamp": 0.27863271697429626
-                },
-                {
-                    "x": 2.965088840346323,
-                    "y": 5.49282892094042,
-                    "heading": 3.277521318880573,
-                    "angularVelocity": 0.5921716771297765,
-                    "velocityX": 3.079706187261968,
-                    "velocityY": -0.2892867666899661,
-                    "timestamp": 0.37151028929906166
-                },
-                {
-                    "x": 3.3148991000673105,
-                    "y": 5.459968004407915,
-                    "heading": 3.277521325558456,
-                    "angularVelocity": 7.189983956229139e-8,
-                    "velocityX": 3.7663587770985667,
-                    "velocityY": -0.35380895204280205,
-                    "timestamp": 0.46438786162382706
-                },
-                {
-                    "x": 3.664709359511986,
-                    "y": 5.427107084931802,
-                    "heading": 3.2775213322361236,
-                    "angularVelocity": 7.18975223410498e-8,
-                    "velocityX": 3.7663587741235602,
-                    "velocityY": -0.3538089837362208,
-                    "timestamp": 0.5572654339485925
-                },
-                {
-                    "x": 4.014519618956661,
-                    "y": 5.394246165455678,
-                    "heading": 3.277521338913791,
-                    "angularVelocity": 7.189752294324541e-8,
-                    "velocityX": 3.7663587741235487,
-                    "velocityY": -0.3538089837363427,
-                    "timestamp": 0.650143006273358
-                },
-                {
-                    "x": 4.364329878401335,
-                    "y": 5.361385245979554,
-                    "heading": 3.2775213455914582,
-                    "angularVelocity": 7.189752233099723e-8,
-                    "velocityX": 3.7663587741235482,
-                    "velocityY": -0.3538089837363424,
-                    "timestamp": 0.7430205785981234
-                },
-                {
-                    "x": 4.714140137846011,
-                    "y": 5.32852432650343,
-                    "heading": 3.277521352269126,
-                    "angularVelocity": 7.189752328963948e-8,
-                    "velocityX": 3.7663587741235482,
-                    "velocityY": -0.35380898373634245,
-                    "timestamp": 0.8358981509228889
-                },
-                {
-                    "x": 5.063950397290685,
-                    "y": 5.295663407027306,
-                    "heading": 3.2775213589467933,
-                    "angularVelocity": 7.189752245277794e-8,
-                    "velocityX": 3.7663587741235482,
-                    "velocityY": -0.3538089837363426,
-                    "timestamp": 0.9287757232476543
-                },
-                {
-                    "x": 5.41376065673536,
-                    "y": 5.262802487551181,
-                    "heading": 3.2775213656244606,
-                    "angularVelocity": 7.189752262506364e-8,
-                    "velocityX": 3.7663587741235482,
-                    "velocityY": -0.3538089837363426,
-                    "timestamp": 1.0216532955724198
-                },
-                {
-                    "x": 5.7635709161800355,
-                    "y": 5.229941568075057,
-                    "heading": 3.277521372302128,
-                    "angularVelocity": 7.189752282304632e-8,
-                    "velocityX": 3.7663587741235482,
-                    "velocityY": -0.3538089837363426,
-                    "timestamp": 1.1145308678971853
-                },
-                {
-                    "x": 6.113381175624392,
-                    "y": 5.19708064859555,
-                    "heading": 3.2775213789797957,
-                    "angularVelocity": 7.189752355462406e-8,
-                    "velocityX": 3.766358774120127,
-                    "velocityY": -0.3538089837727691,
-                    "timestamp": 1.2074084402219507
-                },
-                {
-                    "x": 6.463191352381069,
-                    "y": 5.164218848902877,
-                    "heading": 3.277521385657466,
-                    "angularVelocity": 7.189755308023614e-8,
-                    "velocityX": 3.766357883833297,
-                    "velocityY": -0.35381846090640534,
-                    "timestamp": 1.3002860125467162
-                },
-                {
-                    "x": 6.802417095902647,
-                    "y": 5.072715014503353,
-                    "heading": 3.277521392775139,
-                    "angularVelocity": 7.663500778773157e-8,
-                    "velocityX": 3.6523967523118,
-                    "velocityY": -0.9852091533956361,
-                    "timestamp": 1.3931635848714816
-                },
-                {
-                    "x": 7.115900861455289,
-                    "y": 4.914048826005074,
-                    "heading": 3.277521402122426,
-                    "angularVelocity": 1.006409437934784e-7,
-                    "velocityX": 3.375236429053956,
-                    "velocityY": -1.7083369485958357,
-                    "timestamp": 1.486041157196247
-                },
-                {
-                    "x": 7.364244703869027,
-                    "y": 4.725111728374696,
-                    "heading": 3.264201738164946,
-                    "angularVelocity": -0.1434109831263146,
-                    "velocityX": 2.673883868813388,
-                    "velocityY": -2.034259648494277,
-                    "timestamp": 1.5789187295210125
-                },
-                {
-                    "x": 7.541281052388914,
-                    "y": 4.527619244510334,
-                    "heading": 3.236812571923802,
-                    "angularVelocity": -0.29489537199973426,
-                    "velocityX": 1.9061259256523482,
-                    "velocityY": -2.1263743110531466,
-                    "timestamp": 1.671796301845778
-                },
-                {
-                    "x": 7.646550613403749,
-                    "y": 4.327374608371355,
-                    "heading": 3.1953955958101785,
-                    "angularVelocity": -0.44593086443733326,
-                    "velocityX": 1.1334228315824013,
-                    "velocityY": -2.156006354674992,
-                    "timestamp": 1.7646738741705434
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": -0.5964367330411746,
-                    "velocityX": 0.3601449279842051,
-                    "velocityY": -2.1574057477590434,
-                    "timestamp": 1.857551446495309
-                },
-                {
-                    "x": 7.652622542952261,
-                    "y": 3.949180878788953,
-                    "heading": 3.079427144579805,
-                    "angularVelocity": -0.7304577329243706,
-                    "velocityX": -0.33014912487777537,
-                    "velocityY": -2.1443491684415465,
-                    "timestamp": 1.940475964492665
-                },
-                {
-                    "x": 7.56799601367521,
-                    "y": 3.773464984873132,
-                    "heading": 3.0103677863230933,
-                    "angularVelocity": -0.832797825353816,
-                    "velocityX": -1.0205248257186026,
-                    "velocityY": -2.1189860147444324,
-                    "timestamp": 2.0234004824900214
-                },
-                {
-                    "x": 7.426167818552031,
-                    "y": 3.601500026216296,
-                    "heading": 2.937043425915028,
-                    "angularVelocity": -0.8842301671310652,
-                    "velocityX": -1.7103288454169892,
-                    "velocityY": -2.0737528876841793,
-                    "timestamp": 2.1063250004873777
-                },
-                {
-                    "x": 7.22742147527628,
-                    "y": 3.436376202929032,
-                    "heading": 2.8673776999748726,
-                    "angularVelocity": -0.8401101100446183,
-                    "velocityX": -2.3967138799900867,
-                    "velocityY": -1.9912545441931753,
-                    "timestamp": 2.189249518484734
-                },
-                {
-                    "x": 6.9736090926507135,
-                    "y": 3.285744578395889,
-                    "heading": 2.8218659539774595,
-                    "angularVelocity": -0.5488334101485464,
-                    "velocityX": -3.0607640388534922,
-                    "velocityY": -1.8164908059875007,
-                    "timestamp": 2.2721740364820904
-                },
-                {
-                    "x": 6.6802459764564,
-                    "y": 3.1746372799457343,
-                    "heading": 2.8218659065870306,
-                    "angularVelocity": -5.714887481984483e-7,
-                    "velocityX": -3.5377126485518553,
-                    "velocityY": -1.3398606483753976,
-                    "timestamp": 2.3550985544794467
-                },
-                {
-                    "x": 6.371560257590401,
-                    "y": 3.1187813398722177,
-                    "heading": 2.821865892471116,
-                    "angularVelocity": -1.702260676188045e-7,
-                    "velocityX": -3.722490360159113,
-                    "velocityY": -0.6735756977845532,
-                    "timestamp": 2.438023072476803
-                },
-                {
-                    "x": 6.0583631867424375,
-                    "y": 3.1010512989609436,
-                    "heading": 2.821865878845265,
-                    "angularVelocity": -1.6431631700415249e-7,
-                    "velocityX": -3.7768934738692015,
-                    "velocityY": -0.21380939364446908,
-                    "timestamp": 2.5209475904741594
-                },
-                {
-                    "x": 5.745166091358163,
-                    "y": 3.0833216914831887,
-                    "heading": 2.821865865219415,
-                    "angularVelocity": -1.6431629349932233e-7,
-                    "velocityX": -3.7768937697564957,
-                    "velocityY": -0.2138041668004641,
-                    "timestamp": 2.6038721084715157
-                },
-                {
-                    "x": 5.431968995973753,
-                    "y": 3.065592084007832,
-                    "heading": 2.8218658515935657,
-                    "angularVelocity": -1.6431629385226094e-7,
-                    "velocityX": -3.7768937697581326,
-                    "velocityY": -0.213804166771547,
-                    "timestamp": 2.686796626468872
-                },
-                {
-                    "x": 5.118771900355572,
-                    "y": 3.047862480662691,
-                    "heading": 2.821865837967647,
-                    "angularVelocity": -1.6431712462133498e-7,
-                    "velocityX": -3.7768937725772025,
-                    "velocityY": -0.21380411696461904,
-                    "timestamp": 2.7697211444662284
-                },
-                {
-                    "x": 4.833327806211659,
-                    "y": 3.0319657734270002,
-                    "heading": 2.7501020518876387,
-                    "angularVelocity": -0.8654109521902384,
-                    "velocityX": -3.4422158975107173,
-                    "velocityY": -0.1917009301904812,
-                    "timestamp": 2.8526456624635848
-                },
-                {
-                    "x": 4.6051204329349895,
-                    "y": 3.019195659985863,
-                    "heading": 2.6830003866678815,
-                    "angularVelocity": -0.809189692509236,
-                    "velocityX": -2.7519891437167656,
-                    "velocityY": -0.15399683651516197,
-                    "timestamp": 2.935570180460941
-                },
-                {
-                    "x": 4.434032594221504,
-                    "y": 3.009603052392577,
-                    "heading": 2.6296182647389936,
-                    "angularVelocity": -0.6437435298760438,
-                    "velocityX": -2.063175558270236,
-                    "velocityY": -0.11567878626188524,
-                    "timestamp": 3.0184946984582974
-                },
-                {
-                    "x": 4.320003715449931,
-                    "y": 3.0032022577019606,
-                    "heading": 2.5928181043840297,
-                    "angularVelocity": -0.44377900823176664,
-                    "velocityX": -1.3750924518513103,
-                    "velocityY": -0.07718820495068371,
-                    "timestamp": 3.1014192164556538
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": -0.22693052475300574,
-                    "velocityX": -0.6874169042712922,
-                    "velocityY": -0.03861653681318734,
-                    "timestamp": 3.18434373445301
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 3.2672682524503665
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -7983,389 +2848,69 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W2 To C3 To S4": {
+        "S2 To C2": {
             "waypoints": [
                 {
-                    "x": 2.25,
+                    "x": 4.263,
                     "y": 5.56,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 20
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 7.68,
-                    "y": 4.127,
+                    "y": 5.797,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 18
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
+            "trajectory": [],
+            "constraints": [
                 {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -4.116881309267156e-31,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.3172206852139876,
-                    "y": 5.557554237561194,
-                    "heading": 3.1526693404609274,
-                    "angularVelocity": 0.14095873188728006,
-                    "velocityX": 0.747895486240753,
-                    "velocityY": -0.02721148531256728,
-                    "timestamp": 0.08987978460983724
-                },
-                {
-                    "x": 2.4516878784162954,
-                    "y": 5.552665336111155,
-                    "heading": 3.1761213228598115,
-                    "angularVelocity": 0.2609261081415128,
-                    "velocityX": 1.4960782759551832,
-                    "velocityY": -0.054393782442455275,
-                    "timestamp": 0.17975956921967448
-                },
-                {
-                    "x": 2.6534358807362874,
-                    "y": 5.545338085116866,
-                    "heading": 3.2071661677578067,
-                    "angularVelocity": 0.34540408649958604,
-                    "velocityX": 2.2446426990871093,
-                    "velocityY": -0.08152279209504047,
-                    "timestamp": 0.2696393538295117
-                },
-                {
-                    "x": 2.9224981140685604,
-                    "y": 5.535583877021325,
-                    "heading": 3.239227837145004,
-                    "angularVelocity": 0.3567172476694272,
-                    "velocityX": 2.9935789732947984,
-                    "velocityY": -0.10852504974159909,
-                    "timestamp": 0.35951913843934896
-                },
-                {
-                    "x": 3.258623258854089,
-                    "y": 5.523455945202495,
-                    "heading": 3.2503847245896496,
-                    "angularVelocity": 0.1241312214205911,
-                    "velocityX": 3.739719073033272,
-                    "velocityY": -0.13493503429591652,
-                    "timestamp": 0.4493989230491862
-                },
-                {
-                    "x": 3.598405730984745,
-                    "y": 5.5110225450854795,
-                    "heading": 3.2503847355595754,
-                    "angularVelocity": 1.2205109450182653e-7,
-                    "velocityX": 3.7804103960154256,
-                    "velocityY": -0.13833366614078543,
-                    "timestamp": 0.5392787076590234
-                },
-                {
-                    "x": 3.938188203087489,
-                    "y": 5.498589144205611,
-                    "heading": 3.2503847465294977,
-                    "angularVelocity": 1.22051053234482e-7,
-                    "velocityX": 3.7804103957048714,
-                    "velocityY": -0.13833367462827542,
-                    "timestamp": 0.6291584922688607
-                },
-                {
-                    "x": 4.277970675190232,
-                    "y": 5.486155743325738,
-                    "heading": 3.25038475749942,
-                    "angularVelocity": 1.22051052798414e-7,
-                    "velocityX": 3.7804103957048705,
-                    "velocityY": -0.13833367462831278,
-                    "timestamp": 0.7190382768786979
-                },
-                {
-                    "x": 4.617753147292976,
-                    "y": 5.473722342445865,
-                    "heading": 3.2503847684693423,
-                    "angularVelocity": 1.2205105381924992e-7,
-                    "velocityX": 3.7804103957048705,
-                    "velocityY": -0.1383336746283128,
-                    "timestamp": 0.8089180614885352
-                },
-                {
-                    "x": 4.957535619395719,
-                    "y": 5.461288941565993,
-                    "heading": 3.2503847794392646,
-                    "angularVelocity": 1.2205105317325576e-7,
-                    "velocityX": 3.78041039570487,
-                    "velocityY": -0.13833367462831272,
-                    "timestamp": 0.8987978460983724
-                },
-                {
-                    "x": 5.297318091498462,
-                    "y": 5.44885554068612,
-                    "heading": 3.2503847904091874,
-                    "angularVelocity": 1.2205105386911006e-7,
-                    "velocityX": 3.78041039570487,
-                    "velocityY": -0.13833367462831295,
-                    "timestamp": 0.9886776307082097
-                },
-                {
-                    "x": 5.637100563601198,
-                    "y": 5.436422139806048,
-                    "heading": 3.2503848013791092,
-                    "angularVelocity": 1.2205105333670814e-7,
-                    "velocityX": 3.7804103957047883,
-                    "velocityY": -0.13833367463053942,
-                    "timestamp": 1.0785574153180468
-                },
-                {
-                    "x": 5.976883034047419,
-                    "y": 5.423988693656475,
-                    "heading": 3.2503848123490315,
-                    "angularVelocity": 1.2205105297878886e-7,
-                    "velocityX": 3.7804103772744444,
-                    "velocityY": -0.1383341782977477,
-                    "timestamp": 1.168437199927884
-                },
-                {
-                    "x": 6.3161557778942825,
-                    "y": 5.401611852989575,
-                    "heading": 3.2503848233905317,
-                    "angularVelocity": 1.2284742555590528e-7,
-                    "velocityX": 3.774739173214837,
-                    "velocityY": -0.24896411094026674,
-                    "timestamp": 1.2583169845377211
-                },
-                {
-                    "x": 6.6443599765864345,
-                    "y": 5.312793385156624,
-                    "heading": 3.2503848356259346,
-                    "angularVelocity": 1.3613075412636898e-7,
-                    "velocityX": 3.6515908456709,
-                    "velocityY": -0.9881918188666926,
-                    "timestamp": 1.3481967691475583
-                },
-                {
-                    "x": 6.948614681552646,
-                    "y": 5.161017938085189,
-                    "heading": 3.2503848519237777,
-                    "angularVelocity": 1.8132935219655535e-7,
-                    "velocityX": 3.3851294402514114,
-                    "velocityY": -1.6886494302392412,
-                    "timestamp": 1.4380765537573954
-                },
-                {
-                    "x": 7.216978880977283,
-                    "y": 4.952242107786858,
-                    "heading": 3.2503848494896554,
-                    "angularVelocity": -2.7081974161956603e-8,
-                    "velocityX": 2.985812667326646,
-                    "velocityY": -2.322834118980229,
-                    "timestamp": 1.5279563383672325
-                },
-                {
-                    "x": 7.434432646263598,
-                    "y": 4.699605663740004,
-                    "heading": 3.233827697906493,
-                    "angularVelocity": -0.18421441100386476,
-                    "velocityX": 2.4193845838667345,
-                    "velocityY": -2.8108260955843263,
-                    "timestamp": 1.6178361229770697
-                },
-                {
-                    "x": 7.589713042543029,
-                    "y": 4.421668707909288,
-                    "heading": 3.1971997410199715,
-                    "angularVelocity": -0.40752163621075216,
-                    "velocityX": 1.7276453982782098,
-                    "velocityY": -3.0923188905848704,
-                    "timestamp": 1.7077159075869068
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
                 {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": -0.6364027380377996,
-                    "velocityX": 1.0045301938460758,
-                    "velocityY": -3.2784759018773304,
-                    "timestamp": 1.797595692196744
-                },
-                {
-                    "x": 7.709767191117991,
-                    "y": 3.879341169574254,
-                    "heading": 3.0798110703023642,
-                    "angularVelocity": -0.8233834909878134,
-                    "velocityX": 0.40721464665943,
-                    "velocityY": -3.3879684087134527,
-                    "timestamp": 1.8706952021534504
-                },
-                {
-                    "x": 7.695293406413974,
-                    "y": 3.6265437637575832,
-                    "heading": 3.0152640841936,
-                    "angularVelocity": -0.8830016254143865,
-                    "velocityX": -0.19800111810029022,
-                    "velocityY": -3.4582640289435425,
-                    "timestamp": 1.9437947121101569
-                },
-                {
-                    "x": 7.636370047387183,
-                    "y": 3.3734118596165046,
-                    "heading": 2.9616083332383942,
-                    "angularVelocity": -0.7340097216379668,
-                    "velocityX": -0.8060705066515889,
-                    "velocityY": -3.46283996009006,
-                    "timestamp": 2.0168942220668633
-                },
-                {
-                    "x": 7.533938934527094,
-                    "y": 3.12599493210629,
-                    "heading": 2.935877856016127,
-                    "angularVelocity": -0.3519924721450196,
-                    "velocityX": -1.4012558076073192,
-                    "velocityY": -3.3846591811184132,
-                    "timestamp": 2.08999373202357
-                },
-                {
-                    "x": 7.389145536880011,
-                    "y": 2.8904033965878195,
-                    "heading": 2.9358731972146472,
-                    "angularVelocity": -0.00006373232119512069,
-                    "velocityX": -1.9807711123211746,
-                    "velocityY": -3.2228880283604715,
-                    "timestamp": 2.1630932419802766
-                },
-                {
-                    "x": 7.2083948751557925,
-                    "y": 2.6811221610888363,
-                    "heading": 2.935873140353821,
-                    "angularVelocity": -7.778550956603191e-7,
-                    "velocityX": -2.4726658472985523,
-                    "velocityY": -2.8629635906306268,
-                    "timestamp": 2.2361927519369833
-                },
-                {
-                    "x": 6.996380296591032,
-                    "y": 2.5035855179171795,
-                    "heading": 2.9358730970284697,
-                    "angularVelocity": -5.92690040120521e-7,
-                    "velocityX": -2.9003556753023303,
-                    "velocityY": -2.428698130491067,
-                    "timestamp": 2.30929226189369
-                },
-                {
-                    "x": 6.758605859634312,
-                    "y": 2.362402091220058,
-                    "heading": 2.935873058757806,
-                    "angularVelocity": -5.235419952154551e-7,
-                    "velocityX": -3.252750081327982,
-                    "velocityY": -1.931386773738272,
-                    "timestamp": 2.3823917718503966
-                },
-                {
-                    "x": 6.50822939770154,
-                    "y": 2.2450092870168694,
-                    "heading": 2.935873021282274,
-                    "angularVelocity": -5.126646128379785e-7,
-                    "velocityX": -3.4251455595402076,
-                    "velocityY": -1.6059314798786082,
-                    "timestamp": 2.4554912818071033
-                },
-                {
-                    "x": 6.2578527752872475,
-                    "y": 2.12761682509076,
-                    "heading": 2.935872983806744,
-                    "angularVelocity": -5.126645909112232e-7,
-                    "velocityX": -3.4251477549245686,
-                    "velocityY": -1.6059267975342413,
-                    "timestamp": 2.52859079176381
-                },
-                {
-                    "x": 6.007476152294899,
-                    "y": 2.010224364397617,
-                    "heading": 2.9358729463311364,
-                    "angularVelocity": -5.126656467056276e-7,
-                    "velocityX": -3.425147762832367,
-                    "velocityY": -1.6059267806672737,
-                    "timestamp": 2.6016903017205166
-                },
-                {
-                    "x": 5.771581826202875,
-                    "y": 1.8997576718980689,
-                    "heading": 2.8963919451713847,
-                    "angularVelocity": -0.540099395784409,
-                    "velocityX": -3.227030198037426,
-                    "velocityY": -1.5111823945872924,
-                    "timestamp": 2.6747898116772233
-                },
-                {
-                    "x": 5.572260449403434,
-                    "y": 1.8062657885588822,
-                    "heading": 2.7909821338342944,
-                    "angularVelocity": -1.4420043499543171,
-                    "velocityX": -2.7267129002300594,
-                    "velocityY": -1.2789673062725992,
-                    "timestamp": 2.74788932163393
-                },
-                {
-                    "x": 5.413306384281217,
-                    "y": 1.7316311280227,
-                    "heading": 2.684821280424371,
-                    "angularVelocity": -1.4522785921931365,
-                    "velocityX": -2.1744887922827503,
-                    "velocityY": -1.0210008327056677,
-                    "timestamp": 2.8209888315906366
-                },
-                {
-                    "x": 5.294461986790537,
-                    "y": 1.6757665506891541,
-                    "heading": 2.596152824094934,
-                    "angularVelocity": -1.2129829103089709,
-                    "velocityX": -1.6257892503119276,
-                    "velocityY": -0.7642264273266058,
-                    "timestamp": 2.8940883415473433
-                },
-                {
-                    "x": 5.2154339068234155,
-                    "y": 1.6385760788874908,
-                    "heading": 2.5330095835299122,
-                    "angularVelocity": -0.8637984112673935,
-                    "velocityX": -1.0811027326165235,
-                    "velocityY": -0.5087649947815693,
-                    "timestamp": 2.96718785150405
-                },
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S2 To C3": {
+            "waypoints": [
                 {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": -0.45157051736003884,
-                    "velocityX": -0.5394551460983961,
-                    "velocityY": -0.2541204297879725,
-                    "timestamp": 3.0402873614607566
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": 0,
-                    "velocityX": 1.8516151332127108e-31,
-                    "velocityY": 3.1624508891409975e-32,
-                    "timestamp": 3.1133868714174633
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 }
             ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -8385,16 +2930,16 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W2 To C4 To S3": {
+        "S2 To C4": {
             "waypoints": [
                 {
-                    "x": 2.25,
+                    "x": 4.263,
                     "y": 5.56,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 22
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 7.68,
@@ -8403,380 +2948,10 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 17
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 2.1305523573234936e-32,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.3154171108625845,
-                    "y": 5.531249621704393,
-                    "heading": 3.152034306361113,
-                    "angularVelocity": 0.12992542777148963,
-                    "velocityX": 0.7062597425523925,
-                    "velocityY": -0.3103963856794681,
-                    "timestamp": 0.09262471994534183
-                },
-                {
-                    "x": 2.4462513478934307,
-                    "y": 5.473748850345723,
-                    "heading": 3.1761003812465023,
-                    "angularVelocity": 0.2598234564119746,
-                    "velocityX": 1.4125196503487585,
-                    "velocityY": -0.6207929307921308,
-                    "timestamp": 0.18524943989068365
-                },
-                {
-                    "x": 2.642502734215126,
-                    "y": 5.387497655476605,
-                    "heading": 3.2121940698357436,
-                    "angularVelocity": 0.38967662855597096,
-                    "velocityX": 2.1187798077824542,
-                    "velocityY": -0.931189804622509,
-                    "timestamp": 0.2778741598360255
-                },
-                {
-                    "x": 2.9041712871426104,
-                    "y": 5.2724959877699655,
-                    "heading": 3.2603098134687913,
-                    "angularVelocity": 0.5194697879944143,
-                    "velocityX": 2.8250401521526483,
-                    "velocityY": -1.2415872110005046,
-                    "timestamp": 0.3704988797813673
-                },
-                {
-                    "x": 3.22495104061126,
-                    "y": 5.131513113466323,
-                    "heading": 3.260309819428016,
-                    "angularVelocity": 6.433730075600531e-8,
-                    "velocityX": 3.4632196853922204,
-                    "velocityY": -1.5220869157481582,
-                    "timestamp": 0.46312359972670913
-                },
-                {
-                    "x": 3.545730793052991,
-                    "y": 4.99053023682554,
-                    "heading": 3.2603098253870204,
-                    "angularVelocity": 6.433492130788198e-8,
-                    "velocityX": 3.4632196743053516,
-                    "velocityY": -1.5220869409805253,
-                    "timestamp": 0.555748319672051
-                },
-                {
-                    "x": 3.866510545494718,
-                    "y": 4.849547360184748,
-                    "heading": 3.2603098313460244,
-                    "angularVelocity": 6.433492218112887e-8,
-                    "velocityX": 3.463219674305308,
-                    "velocityY": -1.5220869409806246,
-                    "timestamp": 0.6483730396173928
-                },
-                {
-                    "x": 4.187290297936445,
-                    "y": 4.708564483543956,
-                    "heading": 3.2603098373050288,
-                    "angularVelocity": 6.433492230011589e-8,
-                    "velocityX": 3.463219674305308,
-                    "velocityY": -1.5220869409806246,
-                    "timestamp": 0.7409977595627346
-                },
-                {
-                    "x": 4.508070050378173,
-                    "y": 4.567581606903164,
-                    "heading": 3.260309843264033,
-                    "angularVelocity": 6.433492243162183e-8,
-                    "velocityX": 3.463219674305308,
-                    "velocityY": -1.5220869409806246,
-                    "timestamp": 0.8336224795080764
-                },
-                {
-                    "x": 4.828849802819899,
-                    "y": 4.426598730262372,
-                    "heading": 3.260309849223037,
-                    "angularVelocity": 6.433492147964013e-8,
-                    "velocityX": 3.463219674305308,
-                    "velocityY": -1.5220869409806246,
-                    "timestamp": 0.9262471994534183
-                },
-                {
-                    "x": 5.149629555261627,
-                    "y": 4.28561585362158,
-                    "heading": 3.2603098551820415,
-                    "angularVelocity": 6.433492220525221e-8,
-                    "velocityX": 3.4632196743053076,
-                    "velocityY": -1.5220869409806248,
-                    "timestamp": 1.01887191939876
-                },
-                {
-                    "x": 5.470409307703354,
-                    "y": 4.1446329769807875,
-                    "heading": 3.260309861141046,
-                    "angularVelocity": 6.433492177225282e-8,
-                    "velocityX": 3.463219674305308,
-                    "velocityY": -1.5220869409806244,
-                    "timestamp": 1.111496639344102
-                },
-                {
-                    "x": 5.791189060145081,
-                    "y": 4.003650100339995,
-                    "heading": 3.2603098671000503,
-                    "angularVelocity": 6.433492264273842e-8,
-                    "velocityX": 3.463219674305308,
-                    "velocityY": -1.5220869409806244,
-                    "timestamp": 1.2041213592894437
-                },
-                {
-                    "x": 6.111968812586809,
-                    "y": 3.8626672236992023,
-                    "heading": 3.2603098730590543,
-                    "angularVelocity": 6.433492200729343e-8,
-                    "velocityX": 3.463219674305308,
-                    "velocityY": -1.5220869409806248,
-                    "timestamp": 1.2967460792347856
-                },
-                {
-                    "x": 6.432748565028133,
-                    "y": 3.7216843470574936,
-                    "heading": 3.2603098790180582,
-                    "angularVelocity": 6.43349214222261e-8,
-                    "velocityX": 3.4632196743009604,
-                    "velocityY": -1.5220869409905164,
-                    "timestamp": 1.3893707991801274
-                },
-                {
-                    "x": 6.753528214821051,
-                    "y": 3.5807012368590723,
-                    "heading": 3.260309884977063,
-                    "angularVelocity": 6.433492684215086e-8,
-                    "velocityX": 3.4632185660826984,
-                    "velocityY": -1.5220894625281032,
-                    "timestamp": 1.4819955191254692
-                },
-                {
-                    "x": 7.055963078223925,
-                    "y": 3.403757816572982,
-                    "heading": 3.260309891160044,
-                    "angularVelocity": 6.675302824117912e-8,
-                    "velocityX": 3.2651635932755534,
-                    "velocityY": -1.9103261029075773,
-                    "timestamp": 1.574620239070811
-                },
-                {
-                    "x": 7.316159274039154,
-                    "y": 3.1690804957678615,
-                    "heading": 3.2603099328635268,
-                    "angularVelocity": 4.502413954005259e-7,
-                    "velocityX": 2.809144210841027,
-                    "velocityY": -2.533635955321695,
-                    "timestamp": 1.6672449590161529
-                },
-                {
-                    "x": 7.507506683295118,
-                    "y": 2.9535313290497767,
-                    "heading": 3.2483076831663804,
-                    "angularVelocity": -0.12957933588602658,
-                    "velocityX": 2.0658352259405257,
-                    "velocityY": -2.3271235459095725,
-                    "timestamp": 1.7598696789614947
-                },
-                {
-                    "x": 7.631583047935508,
-                    "y": 2.7620770453961936,
-                    "heading": 3.2242582782979583,
-                    "angularVelocity": -0.25964348267518605,
-                    "velocityX": 1.3395599437559367,
-                    "velocityY": -2.066989069079623,
-                    "timestamp": 1.8524943989068365
-                },
-                {
-                    "x": 7.68897880573514,
-                    "y": 2.5963063443763943,
-                    "heading": 3.1881570664078476,
-                    "angularVelocity": -0.3897578520228113,
-                    "velocityX": 0.6196591777389554,
-                    "velocityY": -1.7897025882250568,
-                    "timestamp": 1.9451191188521784
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": -0.5199159191656997,
-                    "velocityX": -0.09693746702217505,
-                    "velocityY": -1.5039866728730655,
-                    "timestamp": 2.03774383879752
-                },
-                {
-                    "x": 7.6191836827180115,
-                    "y": 2.3540612532810705,
-                    "heading": 3.087368523551757,
-                    "angularVelocity": -0.6362125445554702,
-                    "velocityX": -0.73515140709603,
-                    "velocityY": -1.244329940996524,
-                    "timestamp": 2.1204700863364088
-                },
-                {
-                    "x": 7.505717742673261,
-                    "y": 2.2730171165833535,
-                    "heading": 3.0268817588529657,
-                    "angularVelocity": -0.7311677550750295,
-                    "velocityX": -1.3715833054244395,
-                    "velocityY": -0.979666540049693,
-                    "timestamp": 2.2031963338752973
-                },
-                {
-                    "x": 7.339845145495756,
-                    "y": 2.2145049491373583,
-                    "heading": 2.9612826526647904,
-                    "angularVelocity": -0.7929660553905423,
-                    "velocityX": -2.005078220180729,
-                    "velocityY": -0.7072987012796481,
-                    "timestamp": 2.285922581414186
-                },
-                {
-                    "x": 7.122035574070675,
-                    "y": 2.1796305402319347,
-                    "heading": 2.895398132602207,
-                    "angularVelocity": -0.7964161559680392,
-                    "velocityX": -2.6328955791532933,
-                    "velocityY": -0.4215640131510867,
-                    "timestamp": 2.3686488289530745
-                },
-                {
-                    "x": 6.853520994844272,
-                    "y": 2.1707672940791287,
-                    "heading": 2.839999953502189,
-                    "angularVelocity": -0.6696566174354214,
-                    "velocityX": -3.245820851479772,
-                    "velocityY": -0.1071394680223975,
-                    "timestamp": 2.451375076491963
-                },
-                {
-                    "x": 6.54155924782903,
-                    "y": 2.195597775343439,
-                    "heading": 2.83999969260268,
-                    "angularVelocity": -0.0000031537694148635814,
-                    "velocityX": -3.7710129045632463,
-                    "velocityY": 0.30015239422817563,
-                    "timestamp": 2.5341013240308516
-                },
-                {
-                    "x": 6.239292092495854,
-                    "y": 2.276661405774626,
-                    "heading": 2.8399996811990342,
-                    "angularVelocity": -1.3784796655157707e-7,
-                    "velocityX": -3.6538240803329365,
-                    "velocityY": 0.9799021815062957,
-                    "timestamp": 2.61682757156974
-                },
-                {
-                    "x": 5.945407443635962,
-                    "y": 2.3842186409280646,
-                    "heading": 2.8399996700710783,
-                    "angularVelocity": -1.345154198919157e-7,
-                    "velocityX": -3.5524958233085573,
-                    "velocityY": 1.3001585150212127,
-                    "timestamp": 2.699553819108629
-                },
-                {
-                    "x": 5.651522869698407,
-                    "y": 2.491776080795775,
-                    "heading": 2.8399996589431233,
-                    "angularVelocity": -1.3451541092672365e-7,
-                    "velocityX": -3.5524949176427194,
-                    "velocityY": 1.3001609896200028,
-                    "timestamp": 2.7822800666475174
-                },
-                {
-                    "x": 5.35763829576127,
-                    "y": 2.5993335206646324,
-                    "heading": 2.839999647815168,
-                    "angularVelocity": -1.3451540998350566e-7,
-                    "velocityX": -3.5524949176376506,
-                    "velocityY": 1.3001609896338533,
-                    "timestamp": 2.865006314186406
-                },
-                {
-                    "x": 5.063753722106806,
-                    "y": 2.7068909613057275,
-                    "heading": 2.8399996366871263,
-                    "angularVelocity": -1.3451645746164066e-7,
-                    "velocityX": -3.5524949142206896,
-                    "velocityY": 1.3001609989687195,
-                    "timestamp": 2.9477325617252945
-                },
-                {
-                    "x": 4.796835976566279,
-                    "y": 2.804592771493776,
-                    "heading": 2.7513017358564444,
-                    "angularVelocity": -1.0721857145640414,
-                    "velocityX": -3.226518227060295,
-                    "velocityY": 1.1810255281084718,
-                    "timestamp": 3.030458809264183
-                },
-                {
-                    "x": 4.583301664019943,
-                    "y": 2.882755208599232,
-                    "heading": 2.680367864718666,
-                    "angularVelocity": -0.8574530242585245,
-                    "velocityX": -2.581215985240433,
-                    "velocityY": 0.9448323770362348,
-                    "timestamp": 3.1131850568030717
-                },
-                {
-                    "x": 4.42315083623892,
-                    "y": 2.9413775026442943,
-                    "heading": 2.627181361737275,
-                    "angularVelocity": -0.6429217396375844,
-                    "velocityX": -1.9359131176080184,
-                    "velocityY": 0.7086299184246748,
-                    "timestamp": 3.1959113043419602
-                },
-                {
-                    "x": 4.316383597066322,
-                    "y": 2.980459172951351,
-                    "heading": 2.5917278056853306,
-                    "angularVelocity": -0.4285647797003965,
-                    "velocityX": -1.2906089947136565,
-                    "velocityY": 0.47242164935239944,
-                    "timestamp": 3.278637551880849
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": -0.21429481226012745,
-                    "velocityX": -0.645304225133951,
-                    "velocityY": 0.23621072670391596,
-                    "timestamp": 3.3613637994197374
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": 0,
-                    "velocityX": 2.7848791215143805e-32,
-                    "velocityY": -1.0794258279039968e-32,
-                    "timestamp": 3.444090046958626
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -8796,380 +2971,69 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W2 To C4 To S4": {
+        "S2 To C5": {
             "waypoints": [
                 {
-                    "x": 2.25,
+                    "x": 4.263,
                     "y": 5.56,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 22
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 7.68,
-                    "y": 2.457,
+                    "y": 0.787,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
+            "trajectory": [],
+            "constraints": [
                 {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.3154271184850326,
-                    "y": 5.536626406530092,
-                    "heading": 3.157504964629553,
-                    "angularVelocity": 0.19152994653168934,
-                    "velocityX": 0.7158684847619835,
-                    "velocityY": -0.25574133980197417,
-                    "timestamp": 0.09139544466297708
-                },
-                {
-                    "x": 2.4462968906270097,
-                    "y": 5.489871788853177,
-                    "heading": 3.1916196887834967,
-                    "angularVelocity": 0.3732650383145345,
-                    "velocityX": 1.4319069470536998,
-                    "velocityY": -0.5115639827490647,
-                    "timestamp": 0.18279088932595416
-                },
-                {
-                    "x": 2.6426368598266095,
-                    "y": 5.419722773289121,
-                    "heading": 3.240625955812914,
-                    "angularVelocity": 0.536200323878483,
-                    "velocityX": 2.148246774482153,
-                    "velocityY": -0.7675329533406525,
-                    "timestamp": 0.2741863339889312
-                },
-                {
-                    "x": 2.9045066521861433,
-                    "y": 5.326149233940284,
-                    "heading": 3.2998389732795825,
-                    "angularVelocity": 0.6478771199673835,
-                    "velocityX": 2.8652389987837097,
-                    "velocityY": -1.023831545367414,
-                    "timestamp": 0.36558177865190833
-                },
-                {
-                    "x": 3.230136445009879,
-                    "y": 5.209943279570635,
-                    "heading": 3.2998389932105203,
-                    "angularVelocity": 2.1807364619110316e-7,
-                    "velocityX": 3.5628667711449338,
-                    "velocityY": -1.2714633075877884,
-                    "timestamp": 0.45697722331488544
-                },
-                {
-                    "x": 3.555730781054687,
-                    "y": 5.093637996752385,
-                    "heading": 3.29983900348091,
-                    "angularVelocity": 1.1237311095674653e-7,
-                    "velocityX": 3.5624788220621486,
-                    "velocityY": -1.2725501062676312,
-                    "timestamp": 0.5483726679778625
-                },
-                {
-                    "x": 3.881325116955394,
-                    "y": 4.977332713530725,
-                    "heading": 3.2998390137513005,
-                    "angularVelocity": 1.123731101511981e-7,
-                    "velocityX": 3.562478820485468,
-                    "velocityY": -1.2725501106815165,
-                    "timestamp": 0.6397681126408397
-                },
-                {
-                    "x": 4.2069194528561,
-                    "y": 4.861027430309063,
-                    "heading": 3.2998390240216913,
-                    "angularVelocity": 1.1237311074592376e-7,
-                    "velocityX": 3.562478820485462,
-                    "velocityY": -1.2725501106815353,
-                    "timestamp": 0.7311635573038168
-                },
-                {
-                    "x": 4.532513788756806,
-                    "y": 4.744722147087402,
-                    "heading": 3.2998390342920816,
-                    "angularVelocity": 1.1237311063100021e-7,
-                    "velocityX": 3.562478820485462,
-                    "velocityY": -1.272550110681535,
-                    "timestamp": 0.8225590019667939
-                },
-                {
-                    "x": 4.858108124657512,
-                    "y": 4.62841686386574,
-                    "heading": 3.299839044562472,
-                    "angularVelocity": 1.1237311003038478e-7,
-                    "velocityX": 3.5624788204854614,
-                    "velocityY": -1.272550110681535,
-                    "timestamp": 0.913954446629771
-                },
-                {
-                    "x": 5.183702460558219,
-                    "y": 4.5121115806440795,
-                    "heading": 3.2998390548328627,
-                    "angularVelocity": 1.1237311094761516e-7,
-                    "velocityX": 3.5624788204854614,
-                    "velocityY": -1.272550110681535,
-                    "timestamp": 1.005349891292748
-                },
-                {
-                    "x": 5.509296796458925,
-                    "y": 4.395806297422418,
-                    "heading": 3.299839065103253,
-                    "angularVelocity": 1.1237310968464263e-7,
-                    "velocityX": 3.562478820485462,
-                    "velocityY": -1.2725501106815353,
-                    "timestamp": 1.096745335955725
-                },
-                {
-                    "x": 5.834891132359631,
-                    "y": 4.279501014200756,
-                    "heading": 3.2998390753736433,
-                    "angularVelocity": 1.123731107718446e-7,
-                    "velocityX": 3.562478820485462,
-                    "velocityY": -1.2725501106815351,
-                    "timestamp": 1.188140780618702
-                },
-                {
-                    "x": 6.160485468260297,
-                    "y": 4.16319573097898,
-                    "heading": 3.299839085644034,
-                    "angularVelocity": 1.1237310986388625e-7,
-                    "velocityX": 3.562478820485015,
-                    "velocityY": -1.2725501106827837,
-                    "timestamp": 1.279536225281679
-                },
-                {
-                    "x": 6.4860797940952795,
-                    "y": 4.046890419578527,
-                    "heading": 3.2998390959144244,
-                    "angularVelocity": 1.1237311101180894e-7,
-                    "velocityX": 3.5624787103517215,
-                    "velocityY": -1.2725504189987695,
-                    "timestamp": 1.370931669944656
-                },
-                {
-                    "x": 6.809159340398316,
-                    "y": 3.9237717890604613,
-                    "heading": 3.299839106230886,
-                    "angularVelocity": 1.1287719938305865e-7,
-                    "velocityX": 3.5349633397413522,
-                    "velocityY": -1.3470981072657797,
-                    "timestamp": 1.462327114607633
-                },
-                {
-                    "x": 7.101008389413158,
-                    "y": 3.738399942122118,
-                    "heading": 3.299839117623808,
-                    "angularVelocity": 1.246552472151185e-7,
-                    "velocityX": 3.1932559668706793,
-                    "velocityY": -2.0282394557178747,
-                    "timestamp": 1.55372255927061
-                },
-                {
-                    "x": 7.349782731781937,
-                    "y": 3.498296000909773,
-                    "heading": 3.299839132467128,
-                    "angularVelocity": 1.6240765120244838e-7,
-                    "velocityX": 2.7219555994956917,
-                    "velocityY": -2.6270887142977735,
-                    "timestamp": 1.645118003933587
-                },
-                {
-                    "x": 7.5357710820320545,
-                    "y": 3.2282223692532166,
-                    "heading": 3.2881158704604148,
-                    "angularVelocity": -0.12826965337097124,
-                    "velocityX": 2.0349849047286135,
-                    "velocityY": -2.955001014026776,
-                    "timestamp": 1.736513448596564
-                },
-                {
-                    "x": 7.652314357234611,
-                    "y": 2.959723163992482,
-                    "heading": 3.2581736625054765,
-                    "angularVelocity": -0.32761160105108156,
-                    "velocityX": 1.2751540914573078,
-                    "velocityY": -2.9377744837371598,
-                    "timestamp": 1.827908893259541
-                },
-                {
-                    "x": 7.700165626970879,
-                    "y": 2.701389747494563,
-                    "heading": 3.208935276055406,
-                    "angularVelocity": -0.5387400502456196,
-                    "velocityX": 0.5235629621664141,
-                    "velocityY": -2.826545868347203,
-                    "timestamp": 1.919304337922518
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
                 {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": -0.7542528657692632,
-                    "velocityX": -0.2206414887004817,
-                    "velocityY": -2.6739817109674653,
-                    "timestamp": 2.0106997825854953
-                },
-                {
-                    "x": 7.619243737534899,
-                    "y": 2.269397277536314,
-                    "heading": 3.071018168568731,
-                    "angularVelocity": -0.9308752015312701,
-                    "velocityX": -0.8198752757491441,
-                    "velocityY": -2.531604604539154,
-                    "timestamp": 2.08480405705527
-                },
-                {
-                    "x": 7.514288335022884,
-                    "y": 2.093479496062301,
-                    "heading": 2.9929449451014944,
-                    "angularVelocity": -1.0535589751865693,
-                    "velocityX": -1.4163204924814625,
-                    "velocityY": -2.373922189141621,
-                    "timestamp": 2.1589083315250446
-                },
-                {
-                    "x": 7.365556927410938,
-                    "y": 1.9309603366701282,
-                    "heading": 2.911854592478602,
-                    "angularVelocity": -1.0942736192096787,
-                    "velocityX": -2.0070557154244444,
-                    "velocityY": -2.193114507294498,
-                    "timestamp": 2.233012605994819
-                },
-                {
-                    "x": 7.174068652738112,
-                    "y": 1.784693946040825,
-                    "heading": 2.8381473535529165,
-                    "angularVelocity": -0.994642204556963,
-                    "velocityX": -2.5840381819125056,
-                    "velocityY": -1.973791548137795,
-                    "timestamp": 2.307116880464594
-                },
-                {
-                    "x": 6.9430736895852,
-                    "y": 1.6600434204320091,
-                    "heading": 2.7943942143969487,
-                    "angularVelocity": -0.5904266585027068,
-                    "velocityX": -3.117161119324147,
-                    "velocityY": -1.6820962960737031,
-                    "timestamp": 2.3812211549343685
-                },
-                {
-                    "x": 6.679335807473864,
-                    "y": 1.5650263715806954,
-                    "heading": 2.794393527099141,
-                    "angularVelocity": -0.000009274738771364558,
-                    "velocityX": -3.5590103809586133,
-                    "velocityY": -1.2822073966879628,
-                    "timestamp": 2.455325429404143
-                },
-                {
-                    "x": 6.403647884167786,
-                    "y": 1.5142107470678074,
-                    "heading": 2.794393497300607,
-                    "angularVelocity": -4.021162732159612e-7,
-                    "velocityX": -3.720270190601743,
-                    "velocityY": -0.6857313545879138,
-                    "timestamp": 2.5294297038739177
-                },
-                {
-                    "x": 6.123365156631299,
-                    "y": 1.5089521104338168,
-                    "heading": 2.7943934713550327,
-                    "angularVelocity": -3.501225017023653e-7,
-                    "velocityX": -3.782274768114868,
-                    "velocityY": -0.07096266270252378,
-                    "timestamp": 2.6035339783436924
-                },
-                {
-                    "x": 5.853614932127584,
-                    "y": 1.5409906174836003,
-                    "heading": 2.7698594509818024,
-                    "angularVelocity": -0.33107429427995533,
-                    "velocityX": -3.6401439246765253,
-                    "velocityY": 0.43234357638746057,
-                    "timestamp": 2.677638252813467
-                },
-                {
-                    "x": 5.6273847181633965,
-                    "y": 1.5674202114884777,
-                    "heading": 2.697593854639993,
-                    "angularVelocity": -0.9751879612723551,
-                    "velocityX": -3.0528632198735024,
-                    "velocityY": 0.3566541092809186,
-                    "timestamp": 2.7517425272832416
-                },
-                {
-                    "x": 5.446628200819817,
-                    "y": 1.588490932967887,
-                    "heading": 2.6254372460611823,
-                    "angularVelocity": -0.9737172260984721,
-                    "velocityX": -2.4392185017249606,
-                    "velocityY": 0.284338813519285,
-                    "timestamp": 2.8258468017530163
-                },
-                {
-                    "x": 5.311228363745466,
-                    "y": 1.6042594522545108,
-                    "heading": 2.5652453697807402,
-                    "angularVelocity": -0.8122591673836874,
-                    "velocityX": -1.8271528605219525,
-                    "velocityY": 0.21278825546128166,
-                    "timestamp": 2.899951076222791
-                },
-                {
-                    "x": 5.22105179084387,
-                    "y": 1.614756634238219,
-                    "heading": 2.5224127669101155,
-                    "angularVelocity": -0.578004483237462,
-                    "velocityX": -1.2168876025954114,
-                    "velocityY": 0.14165420360473746,
-                    "timestamp": 2.9740553506925655
-                },
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S3 To W1": {
+            "waypoints": [
                 {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": -0.302449043195724,
-                    "velocityX": -0.6079513113949444,
-                    "velocityY": 0.07075658994444238,
-                    "timestamp": 3.04815962516234
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": 0,
-                    "velocityX": 8.84173108800433e-33,
-                    "velocityY": 0,
-                    "timestamp": 3.122263899632115
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 }
             ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -9189,16 +3053,16 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W3 To W2": {
+        "S3 To W2": {
             "waypoints": [
                 {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 10
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 2.25,
@@ -9210,107 +3074,7 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": 8.35160049146123e-34,
-                    "velocityX": 7.119134799605741e-35,
-                    "velocityY": 1.2756128975400497e-35,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.298001258966314,
-                    "y": 4.408001448921345,
-                    "heading": 2.7655229314430416,
-                    "angularVelocity": 0.204083682327543,
-                    "velocityX": -0.026277925124547522,
-                    "velocityY": 0.6310864986340488,
-                    "timestamp": 0.07606160015345173
-                },
-                {
-                    "x": 2.294003795304181,
-                    "y": 4.504004095385984,
-                    "heading": 2.796583558711994,
-                    "angularVelocity": 0.408361475518381,
-                    "velocityX": -0.0525556082710341,
-                    "velocityY": 1.262169692340901,
-                    "timestamp": 0.15212320030690346
-                },
-                {
-                    "x": 2.2880075283432446,
-                    "y": 4.6480074792263695,
-                    "heading": 2.8432070800984706,
-                    "angularVelocity": 0.6129705566595356,
-                    "velocityX": -0.07883435200994282,
-                    "velocityY": 1.8932468361152464,
-                    "timestamp": 0.2281848004603552
-                },
-                {
-                    "x": 2.2800121406869254,
-                    "y": 4.840010950636843,
-                    "heading": 2.905427634623498,
-                    "angularVelocity": 0.8180284716532374,
-                    "velocityX": -0.10511726863738108,
-                    "velocityY": 2.5243154367396032,
-                    "timestamp": 0.3042464006138069
-                },
-                {
-                    "x": 2.2700168989019116,
-                    "y": 5.080013713319674,
-                    "heading": 2.983284780095673,
-                    "angularVelocity": 1.0236064625921717,
-                    "velocityX": -0.13140982788751254,
-                    "velocityY": 3.1553735682477444,
-                    "timestamp": 0.3803080007672587
-                },
-                {
-                    "x": 2.262010665765912,
-                    "y": 5.27200837395213,
-                    "heading": 3.04596409402067,
-                    "angularVelocity": 0.8240598909113609,
-                    "velocityX": -0.10525985674568247,
-                    "velocityY": 2.5241995993394015,
-                    "timestamp": 0.45636960092071044
-                },
-                {
-                    "x": 2.2560054673554846,
-                    "y": 5.416004212856887,
-                    "heading": 3.092980839154268,
-                    "angularVelocity": 0.6181403630576198,
-                    "velocityX": -0.07895177590678841,
-                    "velocityY": 1.8931476410468593,
-                    "timestamp": 0.5324312010741622
-                },
-                {
-                    "x": 2.2520018258057144,
-                    "y": 5.512001403135216,
-                    "heading": 3.1243269625502275,
-                    "angularVelocity": 0.4121149611988094,
-                    "velocityX": -0.05263683043340117,
-                    "velocityY": 1.2620979585580252,
-                    "timestamp": 0.608492801227614
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0.20605716180244424,
-                    "velocityX": -0.026318481358210057,
-                    "velocityY": 0.6310490019661509,
-                    "timestamp": 0.6845544013810657
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": -5.838549689746647e-34,
-                    "velocityX": 5.793975279736999e-33,
-                    "velocityY": -3.8925630164914687e-35,
-                    "timestamp": 0.7606160015345175
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -9330,371 +3094,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W3 To C1 To S1": {
+        "S3 To W3": {
             "waypoints": [
                 {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 23
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 3.14,
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 13
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 3.4031853071795863,
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": 0,
-                    "velocityX": -8.850231580467738e-33,
-                    "velocityY": -9.949674184637553e-35,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.3596219168979733,
-                    "y": 4.390117696915229,
-                    "heading": 2.7532876341018757,
-                    "angularVelocity": 0.0367297993475745,
-                    "velocityX": 0.6661024239682877,
-                    "velocityY": 0.3364781268925937,
-                    "timestamp": 0.08950863223523108
-                },
-                {
-                    "x": 2.478866409519412,
-                    "y": 4.45035344252589,
-                    "heading": 2.759663263066401,
-                    "angularVelocity": 0.07122920779048274,
-                    "velocityX": 1.3322122084053396,
-                    "velocityY": 0.6729601839112106,
-                    "timestamp": 0.17901726447046215
-                },
-                {
-                    "x": 2.6577344843676225,
-                    "y": 4.540707777094465,
-                    "heading": 2.7687936152184336,
-                    "angularVelocity": 0.10200526948103018,
-                    "velocityX": 1.998333237604842,
-                    "velocityY": 1.009448276799944,
-                    "timestamp": 0.2685258967056932
-                },
-                {
-                    "x": 2.896227808747147,
-                    "y": 4.661181607047397,
-                    "heading": 2.7800101234260337,
-                    "angularVelocity": 0.12531202776199954,
-                    "velocityX": 2.664472894108777,
-                    "velocityY": 1.3459464963817573,
-                    "timestamp": 0.3580345289409243
-                },
-                {
-                    "x": 3.194348569009026,
-                    "y": 4.811776231628672,
-                    "heading": 2.7912909680084645,
-                    "angularVelocity": 0.126030800613561,
-                    "velocityX": 3.3306369767600654,
-                    "velocityY": 1.6824592312561388,
-                    "timestamp": 0.4475431611761554
-                },
-                {
-                    "x": 3.4965824721893735,
-                    "y": 4.964447700014571,
-                    "heading": 2.791290970219591,
-                    "angularVelocity": 2.4702941902196968e-8,
-                    "velocityX": 3.3765894487815307,
-                    "velocityY": 1.7056619520749001,
-                    "timestamp": 0.5370517934113864
-                },
-                {
-                    "x": 3.798816375371316,
-                    "y": 5.117119168397318,
-                    "heading": 2.7912909724307147,
-                    "angularVelocity": 2.470290930957206e-8,
-                    "velocityX": 3.3765894487993435,
-                    "velocityY": 1.7056619520396954,
-                    "timestamp": 0.6265604256466175
-                },
-                {
-                    "x": 4.101050278553258,
-                    "y": 5.2697906367800655,
-                    "heading": 2.7912909746418384,
-                    "angularVelocity": 2.4702908438395345e-8,
-                    "velocityX": 3.376589448799344,
-                    "velocityY": 1.705661952039695,
-                    "timestamp": 0.7160690578818486
-                },
-                {
-                    "x": 4.4032841817351995,
-                    "y": 5.422462105162813,
-                    "heading": 2.791290976852962,
-                    "angularVelocity": 2.470290834028317e-8,
-                    "velocityX": 3.376589448799344,
-                    "velocityY": 1.7056619520396945,
-                    "timestamp": 0.8055776901170797
-                },
-                {
-                    "x": 4.705518084917142,
-                    "y": 5.5751335735455605,
-                    "heading": 2.791290979064086,
-                    "angularVelocity": 2.470290893539666e-8,
-                    "velocityX": 3.376589448799343,
-                    "velocityY": 1.7056619520396956,
-                    "timestamp": 0.8950863223523108
-                },
-                {
-                    "x": 5.007751988099084,
-                    "y": 5.727805041928308,
-                    "heading": 2.791290981275209,
-                    "angularVelocity": 2.470290833665101e-8,
-                    "velocityX": 3.376589448799344,
-                    "velocityY": 1.705661952039695,
-                    "timestamp": 0.9845949545875419
-                },
-                {
-                    "x": 5.309985891281026,
-                    "y": 5.8804765103110554,
-                    "heading": 2.791290983486333,
-                    "angularVelocity": 2.4702909020592533e-8,
-                    "velocityX": 3.376589448799343,
-                    "velocityY": 1.705661952039695,
-                    "timestamp": 1.0741035868227728
-                },
-                {
-                    "x": 5.612219794462969,
-                    "y": 6.033147978693803,
-                    "heading": 2.7912909856974566,
-                    "angularVelocity": 2.470290845419301e-8,
-                    "velocityX": 3.3765894487993435,
-                    "velocityY": 1.7056619520396947,
-                    "timestamp": 1.1636122190580038
-                },
-                {
-                    "x": 5.914453697644911,
-                    "y": 6.18581944707655,
-                    "heading": 2.7912909879085803,
-                    "angularVelocity": 2.4702909294524738e-8,
-                    "velocityX": 3.3765894487993435,
-                    "velocityY": 1.705661952039695,
-                    "timestamp": 1.2531208512932348
-                },
-                {
-                    "x": 6.216687600826853,
-                    "y": 6.338490915459298,
-                    "heading": 2.791290990119704,
-                    "angularVelocity": 2.4702908192380297e-8,
-                    "velocityX": 3.3765894487993426,
-                    "velocityY": 1.705661952039695,
-                    "timestamp": 1.3426294835284658
-                },
-                {
-                    "x": 6.518921504008634,
-                    "y": 6.491162383842364,
-                    "heading": 2.791290992330828,
-                    "angularVelocity": 2.470290883873948e-8,
-                    "velocityX": 3.3765894487975445,
-                    "velocityY": 1.705661952043257,
-                    "timestamp": 1.4321381157636968
-                },
-                {
-                    "x": 6.821155370898669,
-                    "y": 6.643833924069847,
-                    "heading": 2.7912909945419515,
-                    "angularVelocity": 2.4702908942452264e-8,
-                    "velocityX": 3.3765890433422854,
-                    "velocityY": 1.705662754696757,
-                    "timestamp": 1.5216467479989277
-                },
-                {
-                    "x": 7.115501994166732,
-                    "y": 6.811207678318402,
-                    "heading": 2.7912909995251494,
-                    "angularVelocity": 5.5672818987002594e-8,
-                    "velocityX": 3.2884719151390045,
-                    "velocityY": 1.8699174601248647,
-                    "timestamp": 1.6111553802341587
-                },
-                {
-                    "x": 7.349346251698842,
-                    "y": 6.9991016785605416,
-                    "heading": 2.863630358302155,
-                    "angularVelocity": 0.8081830430264627,
-                    "velocityX": 2.6125330227095924,
-                    "velocityY": 2.099171840190225,
-                    "timestamp": 1.7006640124693897
-                },
-                {
-                    "x": 7.522269801890552,
-                    "y": 7.159584774540021,
-                    "heading": 2.9361878922250697,
-                    "angularVelocity": 0.8106205190604594,
-                    "velocityX": 1.9319203731910752,
-                    "velocityY": 1.792934289932232,
-                    "timestamp": 1.7901726447046207
-                },
-                {
-                    "x": 7.634890963268431,
-                    "y": 7.291326986197087,
-                    "heading": 3.0068580812297814,
-                    "angularVelocity": 0.789534900041686,
-                    "velocityX": 1.258215644295718,
-                    "velocityY": 1.4718380603877852,
-                    "timestamp": 1.8796812769398517
-                },
-                {
-                    "x": 7.687430049075347,
-                    "y": 7.393874919842459,
-                    "heading": 3.074917441639189,
-                    "angularVelocity": 0.7603664441050307,
-                    "velocityX": 0.5869722784819492,
-                    "velocityY": 1.1456764681184401,
-                    "timestamp": 1.9691899091750826
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 3.14,
-                    "angularVelocity": 0.7271092936575372,
-                    "velocityX": -0.08300930189416394,
-                    "velocityY": 0.8169612062149058,
-                    "timestamp": 2.0586985414103136
-                },
-                {
-                    "x": 7.610902849690699,
-                    "y": 7.510752049010022,
-                    "heading": 3.20269374092293,
-                    "angularVelocity": 0.6908898775103415,
-                    "velocityX": -0.7614559445765343,
-                    "velocityY": 0.4821509665298148,
-                    "timestamp": 2.149442005692712
-                },
-                {
-                    "x": 7.480264531664604,
-                    "y": 7.524075152445223,
-                    "heading": 3.2620627183539166,
-                    "angularVelocity": 0.6542507264900841,
-                    "velocityX": -1.4396443761452817,
-                    "velocityY": 0.1468216310734938,
-                    "timestamp": 2.2401854699751107
-                },
-                {
-                    "x": 7.2881241468833045,
-                    "y": 7.506890911811968,
-                    "heading": 3.31804363381791,
-                    "angularVelocity": 0.6169140213754432,
-                    "velocityX": -2.117401912090868,
-                    "velocityY": -0.18937166185078083,
-                    "timestamp": 2.3309289342575092
-                },
-                {
-                    "x": 7.034560081128849,
-                    "y": 7.459042940332114,
-                    "heading": 3.3705102766509376,
-                    "angularVelocity": 0.5781864649750256,
-                    "velocityX": -2.7942956306511584,
-                    "velocityY": -0.5272883491746447,
-                    "timestamp": 2.421672398539908
-                },
-                {
-                    "x": 6.71980893194678,
-                    "y": 7.380065366153176,
-                    "heading": 3.4190860492179427,
-                    "angularVelocity": 0.5353087734873627,
-                    "velocityX": -3.468582025946757,
-                    "velocityY": -0.8703389803716955,
-                    "timestamp": 2.5124158628223063
-                },
-                {
-                    "x": 6.409212684326435,
-                    "y": 7.233883432324963,
-                    "heading": 3.4190860479548357,
-                    "angularVelocity": -1.3919538885721925e-8,
-                    "velocityX": -3.4227946891442476,
-                    "velocityY": -1.6109362253714221,
-                    "timestamp": 2.603159327104705
-                },
-                {
-                    "x": 6.100914553727412,
-                    "y": 7.0829150593165995,
-                    "heading": 3.4190860462953716,
-                    "angularVelocity": -1.8287422949592592e-8,
-                    "velocityX": -3.397469261693411,
-                    "velocityY": -1.663683155610435,
-                    "timestamp": 2.6939027913871034
-                },
-                {
-                    "x": 5.79261648458282,
-                    "y": 6.9319467005164075,
-                    "heading": 3.4190860242893293,
-                    "angularVelocity": -2.4250829002858187e-7,
-                    "velocityX": -3.3974685844608317,
-                    "velocityY": -1.6636829990352793,
-                    "timestamp": 2.784646255669502
-                },
-                {
-                    "x": 5.545968445126592,
-                    "y": 6.811167311993174,
-                    "heading": 3.4140434484907587,
-                    "angularVelocity": -0.05556957559916094,
-                    "velocityX": -2.7180804855393776,
-                    "velocityY": -1.330998209935673,
-                    "timestamp": 2.8753897199519005
-                },
-                {
-                    "x": 5.3609835283362095,
-                    "y": 6.720583315840053,
-                    "heading": 3.409029867698435,
-                    "angularVelocity": -0.05525004838608659,
-                    "velocityX": -2.0385481009927036,
-                    "velocityY": -0.9982426488724173,
-                    "timestamp": 2.966133184234299
-                },
-                {
-                    "x": 5.237660981460967,
-                    "y": 6.6601943432298505,
-                    "heading": 3.4052336568932624,
-                    "angularVelocity": -0.041834536902387906,
-                    "velocityX": -1.3590240118170522,
-                    "velocityY": -0.6654911523133865,
-                    "timestamp": 3.0568766485166976
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": -0.022572972388422655,
-                    "velocityX": -0.6795087883031954,
-                    "velocityY": -0.3327439994563607,
-                    "timestamp": 3.147620112799096
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": 0,
-                    "velocityX": -1.5756247890906886e-35,
-                    "velocityY": 0,
-                    "timestamp": 3.2383635770814947
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -9714,16 +3135,16 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W3 To C1 To S2": {
+        "S3 To C1": {
             "waypoints": [
                 {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 23
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 7.68,
@@ -9732,371 +3153,10 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.3566378052654167,
-                    "y": 4.392308940927803,
-                    "heading": 2.7577639052389085,
-                    "angularVelocity": 0.08777137611433758,
-                    "velocityX": 0.6402935063310357,
-                    "velocityY": 0.3652543557357081,
-                    "timestamp": 0.08845600448138136
-                },
-                {
-                    "x": 2.469918670668774,
-                    "y": 4.456929940384967,
-                    "heading": 2.772608563363178,
-                    "angularVelocity": 0.16781967726556357,
-                    "velocityX": 1.2806464192852178,
-                    "velocityY": 0.7305439561286646,
-                    "timestamp": 0.1769120089627627
-                },
-                {
-                    "x": 2.639850302646433,
-                    "y": 4.553867610725804,
-                    "heading": 2.793388618859665,
-                    "angularVelocity": 0.2349196712910238,
-                    "velocityX": 1.92108645392669,
-                    "velocityY": 1.0958856994410031,
-                    "timestamp": 0.26536801344414407
-                },
-                {
-                    "x": 2.8664440757683463,
-                    "y": 4.683128905196596,
-                    "heading": 2.8177875607011336,
-                    "angularVelocity": 0.27583138063398066,
-                    "velocityX": 2.561655078707626,
-                    "velocityY": 1.4613060495853287,
-                    "timestamp": 0.3538240179255254
-                },
-                {
-                    "x": 3.1496996536335864,
-                    "y": 4.844715100104656,
-                    "heading": 2.8386513493911516,
-                    "angularVelocity": 0.23586627965327242,
-                    "velocityX": 3.2022199004575587,
-                    "velocityY": 1.8267408284540865,
-                    "timestamp": 0.4422800224069068
-                },
-                {
-                    "x": 3.44035847389222,
-                    "y": 5.010517813781029,
-                    "heading": 2.83865135522876,
-                    "angularVelocity": 6.599448670952606e-8,
-                    "velocityX": 3.28591396324954,
-                    "velocityY": 1.8744088052413612,
-                    "timestamp": 0.5307360268882881
-                },
-                {
-                    "x": 3.731017294885288,
-                    "y": 5.17632052617049,
-                    "heading": 2.8386513610659576,
-                    "angularVelocity": 6.598983540705142e-8,
-                    "velocityX": 3.285913971552369,
-                    "velocityY": 1.8744087906927347,
-                    "timestamp": 0.6191920313696695
-                },
-                {
-                    "x": 4.021676115881456,
-                    "y": 5.3421232385545165,
-                    "heading": 2.8386513669031546,
-                    "angularVelocity": 6.598983544244374e-8,
-                    "velocityX": 3.285913971587413,
-                    "velocityY": 1.8744087906313014,
-                    "timestamp": 0.7076480358510508
-                },
-                {
-                    "x": 4.31233493687768,
-                    "y": 5.507925950938444,
-                    "heading": 2.838651372740352,
-                    "angularVelocity": 6.598983514854984e-8,
-                    "velocityX": 3.2859139715880556,
-                    "velocityY": 1.8744087906301747,
-                    "timestamp": 0.7961040403324322
-                },
-                {
-                    "x": 4.602993757873904,
-                    "y": 5.673728663322372,
-                    "heading": 2.8386513785775485,
-                    "angularVelocity": 6.598983509017798e-8,
-                    "velocityX": 3.2859139715880414,
-                    "velocityY": 1.8744087906301996,
-                    "timestamp": 0.8845600448138136
-                },
-                {
-                    "x": 4.893652578870126,
-                    "y": 5.839531375706303,
-                    "heading": 2.8386513844147463,
-                    "angularVelocity": 6.598983636221009e-8,
-                    "velocityX": 3.2859139715880246,
-                    "velocityY": 1.8744087906302287,
-                    "timestamp": 0.9730160492951949
-                },
-                {
-                    "x": 5.184311399866346,
-                    "y": 6.005334088090239,
-                    "heading": 2.8386513902519432,
-                    "angularVelocity": 6.598983502831849e-8,
-                    "velocityX": 3.285913971588007,
-                    "velocityY": 1.8744087906302613,
-                    "timestamp": 1.0614720537765763
-                },
-                {
-                    "x": 5.474970220862562,
-                    "y": 6.171136800474182,
-                    "heading": 2.83865139608914,
-                    "angularVelocity": 6.598983586934159e-8,
-                    "velocityX": 3.2859139715879397,
-                    "velocityY": 1.8744087906303781,
-                    "timestamp": 1.1499280582579576
-                },
-                {
-                    "x": 5.765629041858637,
-                    "y": 6.336939512858371,
-                    "heading": 2.8386514019263376,
-                    "angularVelocity": 6.598983523621195e-8,
-                    "velocityX": 3.2859139715863668,
-                    "velocityY": 1.8744087906331355,
-                    "timestamp": 1.238384062739339
-                },
-                {
-                    "x": 6.0562878628501755,
-                    "y": 6.502742225250514,
-                    "heading": 2.8386514077635345,
-                    "angularVelocity": 6.598983511287735e-8,
-                    "velocityX": 3.285913971535071,
-                    "velocityY": 1.87440879072306,
-                    "timestamp": 1.3268400672207203
-                },
-                {
-                    "x": 6.3469466835037505,
-                    "y": 6.6685449382351205,
-                    "heading": 2.8386514136007315,
-                    "angularVelocity": 6.598983592798126e-8,
-                    "velocityX": 3.2859139677143814,
-                    "velocityY": 1.8744087974208865,
-                    "timestamp": 1.4152960717021017
-                },
-                {
-                    "x": 6.6376054603552355,
-                    "y": 6.834347728006541,
-                    "heading": 2.838651419437929,
-                    "angularVelocity": 6.598983535546771e-8,
-                    "velocityX": 3.2859134725293426,
-                    "velocityY": 1.8744096655000533,
-                    "timestamp": 1.503752076183483
-                },
-                {
-                    "x": 6.928282728448998,
-                    "y": 7.000118097747795,
-                    "heading": 2.8386514252461743,
-                    "angularVelocity": 6.56625327771565e-8,
-                    "velocityX": 3.286122517041433,
-                    "velocityY": 1.8740431552739683,
-                    "timestamp": 1.5922080806648644
-                },
-                {
-                    "x": 7.191968889959837,
-                    "y": 7.158086456014882,
-                    "heading": 2.9053215778244037,
-                    "angularVelocity": 0.7537097449644049,
-                    "velocityX": 2.980986571311004,
-                    "velocityY": 1.7858409860730329,
-                    "timestamp": 1.6806640851462458
-                },
-                {
-                    "x": 7.39896469669124,
-                    "y": 7.2837659035610995,
-                    "heading": 2.9729435938200757,
-                    "angularVelocity": 0.7644706132968896,
-                    "velocityX": 2.3400989898312643,
-                    "velocityY": 1.4208130729292534,
-                    "timestamp": 1.7691200896276271
-                },
-                {
-                    "x": 7.549292841201835,
-                    "y": 7.377144263475716,
-                    "heading": 3.03570050164413,
-                    "angularVelocity": 0.7094702976016349,
-                    "velocityX": 1.6994679489759839,
-                    "velocityY": 1.0556474991393678,
-                    "timestamp": 1.8575760941090085
-                },
-                {
-                    "x": 7.642968334049115,
-                    "y": 7.4382219539742405,
-                    "heading": 3.0916990951391803,
-                    "angularVelocity": 0.6330671820796432,
-                    "velocityX": 1.0590066032994798,
-                    "velocityY": 0.6904866532988578,
-                    "timestamp": 1.9460320985903898
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 3.14,
-                    "angularVelocity": 0.546044388326283,
-                    "velocityX": 0.41864502209875154,
-                    "velocityY": 0.32533739449908106,
-                    "timestamp": 2.034488103071771
-                },
-                {
-                    "x": 7.645871150286324,
-                    "y": 7.456493783242831,
-                    "heading": 3.185181806591133,
-                    "angularVelocity": 0.43686385232963987,
-                    "velocityX": -0.329992576357443,
-                    "velocityY": -0.10158483407911743,
-                    "timestamp": 2.1379111863737847
-                },
-                {
-                    "x": 7.534332014927772,
-                    "y": 7.401805741681953,
-                    "heading": 3.219069089269022,
-                    "angularVelocity": 0.32765685953225704,
-                    "velocityX": -1.078474280570675,
-                    "velocityY": -0.5287798411617203,
-                    "timestamp": 2.2413342696757983
-                },
-                {
-                    "x": 7.345414230675068,
-                    "y": 7.30288053546568,
-                    "heading": 3.2416598317701943,
-                    "angularVelocity": 0.21843037144032681,
-                    "velocityX": -1.826650088365854,
-                    "velocityY": -0.9565099304515025,
-                    "timestamp": 2.344757352977812
-                },
-                {
-                    "x": 7.079208201344681,
-                    "y": 7.159560464323527,
-                    "heading": 3.2529526054228244,
-                    "angularVelocity": 0.1091900694900192,
-                    "velocityX": -2.573951779729956,
-                    "velocityY": -1.3857648270226828,
-                    "timestamp": 2.4481804362798254
-                },
-                {
-                    "x": 6.738167350146867,
-                    "y": 6.967824923059757,
-                    "heading": 3.25295260062049,
-                    "angularVelocity": -4.6433873545046094e-8,
-                    "velocityX": -3.29753126970623,
-                    "velocityY": -1.853895041050502,
-                    "timestamp": 2.551603519581839
-                },
-                {
-                    "x": 6.398085530846708,
-                    "y": 6.774393439892416,
-                    "heading": 3.252952596202531,
-                    "angularVelocity": -4.2717337144488564e-8,
-                    "velocityX": -3.2882583698173717,
-                    "velocityY": -1.8702931395158464,
-                    "timestamp": 2.6550266028838525
-                },
-                {
-                    "x": 6.058003908621006,
-                    "y": 6.580961610238813,
-                    "heading": 3.252952591784573,
-                    "angularVelocity": -4.2717336082734796e-8,
-                    "velocityX": -3.288256464300223,
-                    "velocityY": -1.8702964896989283,
-                    "timestamp": 2.758449686185866
-                },
-                {
-                    "x": 5.717922287179122,
-                    "y": 6.387529779207143,
-                    "heading": 3.252952587366614,
-                    "angularVelocity": -4.271733673837129e-8,
-                    "velocityX": -3.28825645672147,
-                    "velocityY": -1.8702965030234862,
-                    "timestamp": 2.8618727694878796
-                },
-                {
-                    "x": 5.377840665722721,
-                    "y": 6.194097948200997,
-                    "heading": 3.2529525829486556,
-                    "angularVelocity": -4.271733567219934e-8,
-                    "velocityX": -3.2882564568618378,
-                    "velocityY": -1.8702965027766985,
-                    "timestamp": 2.965295852789893
-                },
-                {
-                    "x": 5.037759049752838,
-                    "y": 6.000666107536036,
-                    "heading": 3.252952578536604,
-                    "angularVelocity": -4.266021621617093e-8,
-                    "velocityX": -3.288256403812571,
-                    "velocityY": -1.8702965961679845,
-                    "timestamp": 3.0687189360919067
-                },
-                {
-                    "x": 4.7278555042556665,
-                    "y": 5.824399716323421,
-                    "heading": 3.2077639144902763,
-                    "angularVelocity": -0.4369301572101497,
-                    "velocityX": -2.996463996264777,
-                    "velocityY": -1.7043234990188791,
-                    "timestamp": 3.1721420193939203
-                },
-                {
-                    "x": 4.495427770658729,
-                    "y": 5.69219987124691,
-                    "heading": 3.1738800653247865,
-                    "angularVelocity": -0.32762366082757177,
-                    "velocityX": -2.2473487172898445,
-                    "velocityY": -1.278243123833972,
-                    "timestamp": 3.275565102695934
-                },
-                {
-                    "x": 4.340475927186926,
-                    "y": 5.604066625906228,
-                    "heading": 3.1512930339800644,
-                    "angularVelocity": -0.2183944881894502,
-                    "velocityX": -1.4982326819566802,
-                    "velocityY": -0.8521622303921732,
-                    "timestamp": 3.3789881859979474
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": -0.10919258660163363,
-                    "velocityX": -0.7491163936844148,
-                    "velocityY": -0.4260811464839441,
-                    "timestamp": 3.482411269299961
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": 8.84233842596168e-24,
-                    "velocityY": 0,
-                    "timestamp": 3.5858343526019745
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -10116,16 +3176,16 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W3 To C2 To S1": {
+        "S3 To C2": {
             "waypoints": [
                 {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 22
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 7.68,
@@ -10134,344 +3194,51 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 13
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
                 {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 3.4031853071795863,
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S3 To C3": {
+            "waypoints": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.3590330484085675,
-                    "y": 4.366649962138697,
-                    "heading": 2.7576232354660704,
-                    "angularVelocity": 0.09028523567993728,
-                    "velocityX": 0.6991536221325528,
-                    "velocityY": 0.07875834370159086,
-                    "timestamp": 0.08443501762674911
-                },
-                {
-                    "x": 2.4770991403474683,
-                    "y": 4.379949883012471,
-                    "heading": 2.7728702101731937,
-                    "angularVelocity": 0.18057643778228707,
-                    "velocityX": 1.3983071864900902,
-                    "velocityY": 0.15751664709264293,
-                    "timestamp": 0.16887003525349822
-                },
-                {
-                    "x": 2.6541982667626414,
-                    "y": 4.399899758130858,
-                    "heading": 2.7957418198907438,
-                    "angularVelocity": 0.27087824886418327,
-                    "velocityX": 2.097460643616519,
-                    "velocityY": 0.23627489730122597,
-                    "timestamp": 0.25330505288024735
-                },
-                {
-                    "x": 2.8903304124163527,
-                    "y": 4.426499582699937,
-                    "heading": 2.8262393577196585,
-                    "angularVelocity": 0.361195374693129,
-                    "velocityX": 2.796613920275944,
-                    "velocityY": 0.31503309073333907,
-                    "timestamp": 0.33774007050699645
-                },
-                {
-                    "x": 3.1854955469851514,
-                    "y": 4.459749351941674,
-                    "heading": 2.864364443644879,
-                    "angularVelocity": 0.4515316866960985,
-                    "velocityX": 3.4957668378017823,
-                    "velocityY": 0.3937912275771662,
-                    "timestamp": 0.42217508813374555
-                },
-                {
-                    "x": 3.5029006029698784,
-                    "y": 4.495505051079763,
-                    "heading": 2.8643644482492165,
-                    "angularVelocity": 5.453113642276571e-8,
-                    "velocityX": 3.7591637321358573,
-                    "velocityY": 0.4234700263361048,
-                    "timestamp": 0.5066101057604947
-                },
-                {
-                    "x": 3.820305658948058,
-                    "y": 4.531260750276139,
-                    "heading": 2.864364452853533,
-                    "angularVelocity": 5.453089037976801e-8,
-                    "velocityX": 3.759163732058311,
-                    "velocityY": 0.4234700270264227,
-                    "timestamp": 0.5910451233872438
-                },
-                {
-                    "x": 4.1377107149262375,
-                    "y": 4.567016449472515,
-                    "heading": 2.8643644574578495,
-                    "angularVelocity": 5.4530890155242205e-8,
-                    "velocityX": 3.75916373205831,
-                    "velocityY": 0.4234700270264262,
-                    "timestamp": 0.6754801410139929
-                },
-                {
-                    "x": 4.455115770904417,
-                    "y": 4.602772148668891,
-                    "heading": 2.864364462062166,
-                    "angularVelocity": 5.453089038095968e-8,
-                    "velocityX": 3.7591637320583104,
-                    "velocityY": 0.4234700270264258,
-                    "timestamp": 0.759915158640742
-                },
-                {
-                    "x": 4.772520826882595,
-                    "y": 4.638527847865267,
-                    "heading": 2.864364466666483,
-                    "angularVelocity": 5.4530891020586436e-8,
-                    "velocityX": 3.7591637320583104,
-                    "velocityY": 0.4234700270264263,
-                    "timestamp": 0.8443501762674911
-                },
-                {
-                    "x": 5.089925882860775,
-                    "y": 4.674283547061643,
-                    "heading": 2.8643644712708,
-                    "angularVelocity": 5.4530890812107064e-8,
-                    "velocityX": 3.75916373205831,
-                    "velocityY": 0.4234700270264263,
-                    "timestamp": 0.9287851938942402
-                },
-                {
-                    "x": 5.407330938838954,
-                    "y": 4.7100392462580185,
-                    "heading": 2.8643644758751163,
-                    "angularVelocity": 5.453088997566471e-8,
-                    "velocityX": 3.7591637320583104,
-                    "velocityY": 0.4234700270264265,
-                    "timestamp": 1.0132202115209894
-                },
-                {
-                    "x": 5.7247359948171335,
-                    "y": 4.745794945454395,
-                    "heading": 2.864364480479433,
-                    "angularVelocity": 5.453089022113266e-8,
-                    "velocityX": 3.7591637320583104,
-                    "velocityY": 0.42347002702642605,
-                    "timestamp": 1.0976552291477386
-                },
-                {
-                    "x": 6.04214105079524,
-                    "y": 4.7815506446514116,
-                    "heading": 2.8643644850837493,
-                    "angularVelocity": 5.453089025367266e-8,
-                    "velocityX": 3.759163732057455,
-                    "velocityY": 0.42347002703401615,
-                    "timestamp": 1.1820902467744878
-                },
-                {
-                    "x": 6.359546093014576,
-                    "y": 4.817306465985485,
-                    "heading": 2.8643644896880662,
-                    "angularVelocity": 5.4530891813953255e-8,
-                    "velocityX": 3.7591635691064473,
-                    "velocityY": 0.42347147355537545,
-                    "timestamp": 1.266525264401237
-                },
-                {
-                    "x": 6.6740817237038605,
-                    "y": 4.872910210638354,
-                    "heading": 2.8643644943647257,
-                    "angularVelocity": 5.53876754719752e-8,
-                    "velocityX": 3.7251799020130547,
-                    "velocityY": 0.6585389121213792,
-                    "timestamp": 1.3509602820279862
-                },
-                {
-                    "x": 6.972867149310035,
-                    "y": 4.985833654432599,
-                    "heading": 2.8643645123115293,
-                    "angularVelocity": 2.1255166363796299e-7,
-                    "velocityX": 3.5386434918149345,
-                    "velocityY": 1.337400606623087,
-                    "timestamp": 1.4353952996547354
-                },
-                {
-                    "x": 7.232618211035156,
-                    "y": 5.1421799444134875,
-                    "heading": 2.9041055246483745,
-                    "angularVelocity": 0.4706697938114134,
-                    "velocityX": 3.0763428376762785,
-                    "velocityY": 1.8516759322776348,
-                    "timestamp": 1.5198303172814847
-                },
-                {
-                    "x": 7.433588885971066,
-                    "y": 5.306272472318755,
-                    "heading": 2.9592505290938482,
-                    "angularVelocity": 0.6531058557865896,
-                    "velocityX": 2.3801815950855185,
-                    "velocityY": 1.9434179386406838,
-                    "timestamp": 1.6042653349082339
-                },
-                {
-                    "x": 7.575142359826725,
-                    "y": 5.471558749180071,
-                    "heading": 3.0192595634287165,
-                    "angularVelocity": 0.7107126405793248,
-                    "velocityX": 1.676478288681205,
-                    "velocityY": 1.9575560177173887,
-                    "timestamp": 1.688700352534983
-                },
-                {
-                    "x": 7.657265359417412,
-                    "y": 5.635564900043681,
-                    "heading": 3.0801944156150345,
-                    "angularVelocity": 0.7216774970745534,
-                    "velocityX": 0.9726177822774612,
-                    "velocityY": 1.9423949384201076,
-                    "timestamp": 1.7731353701617323
                 },
                 {
                     "x": 7.68,
-                    "y": 5.797,
+                    "y": 4.127,
                     "heading": 3.14,
-                    "angularVelocity": 0.7083030958711996,
-                    "velocityX": 0.26925606486028053,
-                    "velocityY": 1.9119448836968793,
-                    "timestamp": 1.8575703877884815
-                },
-                {
-                    "x": 7.638446013209911,
-                    "y": 5.962794429881767,
-                    "heading": 3.200158410841702,
-                    "angularVelocity": 0.6785760380077615,
-                    "velocityX": -0.46872148590566404,
-                    "velocityY": 1.870131304647713,
-                    "timestamp": 1.9462242857633076
-                },
-                {
-                    "x": 7.531523237847712,
-                    "y": 6.124072395344993,
-                    "heading": 3.2564305280159678,
-                    "angularVelocity": 0.6347393454740654,
-                    "velocityX": -1.206069646171223,
-                    "velocityY": 1.8191863995537148,
-                    "timestamp": 2.034878183738134
-                },
-                {
-                    "x": 7.359349891233686,
-                    "y": 6.279498639853745,
-                    "heading": 3.30674894531401,
-                    "angularVelocity": 0.5675826833054816,
-                    "velocityX": -1.9420843363584046,
-                    "velocityY": 1.7531800412530831,
-                    "timestamp": 2.1235320817129604
-                },
-                {
-                    "x": 7.122252404274228,
-                    "y": 6.426460665486359,
-                    "heading": 3.3470467672011193,
-                    "angularVelocity": 0.45455217207203097,
-                    "velocityX": -2.6744169447211816,
-                    "velocityY": 1.6577051769832414,
-                    "timestamp": 2.2121859796877867
-                },
-                {
-                    "x": 6.821828497038046,
-                    "y": 6.5576759176562405,
-                    "heading": 3.365711055371459,
-                    "angularVelocity": 0.2105298085780703,
-                    "velocityX": -3.388727558504971,
-                    "velocityY": 1.4800844087773837,
-                    "timestamp": 2.300839877662613
-                },
-                {
-                    "x": 6.494234201783015,
-                    "y": 6.629485644322343,
-                    "heading": 3.365711060189302,
-                    "angularVelocity": 5.434439690558349e-8,
-                    "velocityX": -3.6952046411772423,
-                    "velocityY": 0.8100007817647606,
-                    "timestamp": 2.3894937756374395
-                },
-                {
-                    "x": 6.158929367283791,
-                    "y": 6.636218351220065,
-                    "heading": 3.3657110616036774,
-                    "angularVelocity": 1.5953902696988204e-8,
-                    "velocityX": -3.782178134958452,
-                    "velocityY": 0.07594372104916906,
-                    "timestamp": 2.478147673612266
-                },
-                {
-                    "x": 5.8312862675507695,
-                    "y": 6.634145572711449,
-                    "heading": 3.3782020006814046,
-                    "angularVelocity": 0.14089554281384958,
-                    "velocityX": -3.6957551468978513,
-                    "velocityY": -0.023380568209240477,
-                    "timestamp": 2.566801571587092
-                },
-                {
-                    "x": 5.569171767766223,
-                    "y": 6.6324873452973785,
-                    "heading": 3.388195171082853,
-                    "angularVelocity": 0.11272116206650896,
-                    "velocityX": -2.9566043430935838,
-                    "velocityY": -0.018704506535536753,
-                    "timestamp": 2.6554554695619186
-                },
-                {
-                    "x": 5.372585886333595,
-                    "y": 6.631243673213895,
-                    "heading": 3.395690186633039,
-                    "angularVelocity": 0.08454242533492021,
-                    "velocityX": -2.2174533317017797,
-                    "velocityY": -0.014028397080035612,
-                    "timestamp": 2.744109367536745
-                },
-                {
-                    "x": 5.241528629389324,
-                    "y": 6.630414557878557,
-                    "heading": 3.4006869200557808,
-                    "angularVelocity": 0.05636225295091492,
-                    "velocityX": -1.4783022510920478,
-                    "velocityY": -0.00935227163472713,
-                    "timestamp": 2.8327632655115713
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": 0.02818135672404778,
-                    "velocityX": -0.7391511358917447,
-                    "velocityY": -0.00467613819613763,
-                    "timestamp": 2.9214171634863977
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": 0,
-                    "velocityX": -2.6920195969080552e-36,
-                    "velocityY": 0,
-                    "timestamp": 3.010071061461224
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 }
             ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -10491,29 +3258,20 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W3 To C2 To S2": {
+        "S3 To C4": {
             "waypoints": [
                 {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 22
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 7.68,
-                    "y": 5.797,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 14
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
+                    "y": 2.457,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
@@ -10521,341 +3279,7 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.3595382644510017,
-                    "y": 4.372038786355543,
-                    "heading": 2.7583958335298844,
-                    "angularVelocity": 0.09833011550569279,
-                    "velocityX": 0.6972987732113981,
-                    "velocityY": 0.140995560318735,
-                    "timestamp": 0.08538415201391997
-                },
-                {
-                    "x": 2.478614789358208,
-                    "y": 4.396116350384182,
-                    "heading": 2.7751880425894457,
-                    "angularVelocity": 0.19666657879115437,
-                    "velocityX": 1.3945974996366264,
-                    "velocityY": 0.28199101895061424,
-                    "timestamp": 0.17076830402783993
-                },
-                {
-                    "x": 2.657229566273028,
-                    "y": 4.432232679689,
-                    "heading": 2.800377642298772,
-                    "angularVelocity": 0.2950149309349538,
-                    "velocityX": 2.0918961271138583,
-                    "velocityY": 0.4229863323925762,
-                    "timestamp": 0.2561524560417599
-                },
-                {
-                    "x": 2.8953825799844894,
-                    "y": 4.480387759130421,
-                    "heading": 2.8339661569504204,
-                    "angularVelocity": 0.39338113525063506,
-                    "velocityX": 2.7891945764435975,
-                    "velocityY": 0.5639814685232187,
-                    "timestamp": 0.34153660805567987
-                },
-                {
-                    "x": 3.1930738006682495,
-                    "y": 4.5405815710050295,
-                    "heading": 2.8759554693739764,
-                    "angularVelocity": 0.49176939084328164,
-                    "velocityX": 3.486492676477345,
-                    "velocityY": 0.7049763973154618,
-                    "timestamp": 0.42692076006959984
-                },
-                {
-                    "x": 3.5096695268696414,
-                    "y": 4.604598695680213,
-                    "heading": 2.8759554743130047,
-                    "angularVelocity": 5.784479190315687e-8,
-                    "velocityX": 3.707897996688871,
-                    "velocityY": 0.7497541776224135,
-                    "timestamp": 0.5123049120835198
-                },
-                {
-                    "x": 3.8262652530617234,
-                    "y": 4.6686158204015085,
-                    "heading": 2.875955479252014,
-                    "angularVelocity": 5.784456729975194e-8,
-                    "velocityX": 3.7078979965798418,
-                    "velocityY": 0.7497541781624566,
-                    "timestamp": 0.5976890640974397
-                },
-                {
-                    "x": 4.142860979253806,
-                    "y": 4.732632945122804,
-                    "heading": 2.875955484191023,
-                    "angularVelocity": 5.784456771643219e-8,
-                    "velocityX": 3.707897996579841,
-                    "velocityY": 0.7497541781624594,
-                    "timestamp": 0.6830732161113597
-                },
-                {
-                    "x": 4.459456705445889,
-                    "y": 4.796650069844099,
-                    "heading": 2.8759554891300323,
-                    "angularVelocity": 5.7844567227510466e-8,
-                    "velocityX": 3.707897996579841,
-                    "velocityY": 0.7497541781624593,
-                    "timestamp": 0.7684573681252798
-                },
-                {
-                    "x": 4.7760524316379716,
-                    "y": 4.860667194565394,
-                    "heading": 2.8759554940690415,
-                    "angularVelocity": 5.784456749923784e-8,
-                    "velocityX": 3.707897996579841,
-                    "velocityY": 0.7497541781624593,
-                    "timestamp": 0.8538415201391998
-                },
-                {
-                    "x": 5.092648157830054,
-                    "y": 4.92468431928669,
-                    "heading": 2.8759554990080507,
-                    "angularVelocity": 5.784456784777939e-8,
-                    "velocityX": 3.707897996579841,
-                    "velocityY": 0.749754178162459,
-                    "timestamp": 0.9392256721531198
-                },
-                {
-                    "x": 5.409243884022137,
-                    "y": 4.988701444007985,
-                    "heading": 2.87595550394706,
-                    "angularVelocity": 5.784456724140334e-8,
-                    "velocityX": 3.707897996579841,
-                    "velocityY": 0.749754178162459,
-                    "timestamp": 1.0246098241670398
-                },
-                {
-                    "x": 5.72583961021422,
-                    "y": 5.05271856872928,
-                    "heading": 2.875955508886069,
-                    "angularVelocity": 5.7844566753037437e-8,
-                    "velocityX": 3.7078979965798413,
-                    "velocityY": 0.7497541781624589,
-                    "timestamp": 1.1099939761809599
-                },
-                {
-                    "x": 6.042435336406303,
-                    "y": 5.116735693450575,
-                    "heading": 2.8759555138250783,
-                    "angularVelocity": 5.784456768545098e-8,
-                    "velocityX": 3.7078979965798413,
-                    "velocityY": 0.749754178162459,
-                    "timestamp": 1.1953781281948799
-                },
-                {
-                    "x": 6.35903106259824,
-                    "y": 5.180752818172588,
-                    "heading": 2.8759555187640875,
-                    "angularVelocity": 5.784456775109183e-8,
-                    "velocityX": 3.707897996578142,
-                    "velocityY": 0.7497541781708625,
-                    "timestamp": 1.2807622802088
-                },
-                {
-                    "x": 6.67562676006639,
-                    "y": 5.244770084947493,
-                    "heading": 2.875955523703097,
-                    "angularVelocity": 5.7844569052705035e-8,
-                    "velocityX": 3.7078976601715947,
-                    "velocityY": 0.7497558418624185,
-                    "timestamp": 1.36614643222272
-                },
-                {
-                    "x": 6.986762276730635,
-                    "y": 5.331521073675251,
-                    "heading": 2.875955533952509,
-                    "angularVelocity": 1.2003881364523766e-7,
-                    "velocityX": 3.643949249663132,
-                    "velocityY": 1.0160080844231525,
-                    "timestamp": 1.45153058423664
-                },
-                {
-                    "x": 7.245394684994352,
-                    "y": 5.443936340264843,
-                    "heading": 2.937208027811149,
-                    "angularVelocity": 0.717375442794747,
-                    "velocityX": 3.0290446431036937,
-                    "velocityY": 1.3165823392058056,
-                    "timestamp": 1.53691473625056
-                },
-                {
-                    "x": 7.443822925582165,
-                    "y": 5.547959848416938,
-                    "heading": 2.996476692643064,
-                    "angularVelocity": 0.694141283059797,
-                    "velocityX": 2.3239469609707415,
-                    "velocityY": 1.2182999502663732,
-                    "timestamp": 1.62229888826448
-                },
-                {
-                    "x": 7.58233184253966,
-                    "y": 5.641838110863567,
-                    "heading": 3.05066970535842,
-                    "angularVelocity": 0.6346963861223477,
-                    "velocityX": 1.6221853082866564,
-                    "velocityY": 1.0994811125058057,
-                    "timestamp": 1.7076830402784
-                },
-                {
-                    "x": 7.661036876366553,
-                    "y": 5.724955999091069,
-                    "heading": 3.0986966592522744,
-                    "angularVelocity": 0.5624808909037848,
-                    "velocityX": 0.9217756687922859,
-                    "velocityY": 0.9734580278311056,
-                    "timestamp": 1.79306719229232
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 3.14,
-                    "angularVelocity": 0.4837354447344324,
-                    "velocityX": 0.22209184241070226,
-                    "velocityY": 0.8437631481915462,
-                    "timestamp": 1.87845134430624
-                },
-                {
-                    "x": 7.620399591023171,
-                    "y": 5.865894759292055,
-                    "heading": 3.178669357704941,
-                    "angularVelocity": 0.3869200928483079,
-                    "velocityX": -0.5963532146324005,
-                    "velocityY": 0.6893511618538622,
-                    "timestamp": 1.978392801004126
-                },
-                {
-                    "x": 7.479095963903043,
-                    "y": 5.918870767960299,
-                    "heading": 3.2076554876039776,
-                    "angularVelocity": 0.29003109276922934,
-                    "velocityX": -1.413863993870693,
-                    "velocityY": 0.5300704074024576,
-                    "timestamp": 2.078334257702012
-                },
-                {
-                    "x": 7.256273297326838,
-                    "y": 5.955010806465711,
-                    "heading": 3.2269465299619338,
-                    "angularVelocity": 0.19302342586691915,
-                    "velocityX": -2.229531907362301,
-                    "velocityY": 0.36161208470935113,
-                    "timestamp": 2.178275714399898
-                },
-                {
-                    "x": 6.95245768893108,
-                    "y": 5.971943859159761,
-                    "heading": 3.236518587853394,
-                    "angularVelocity": 0.09577664972800967,
-                    "velocityX": -3.0399357627352264,
-                    "velocityY": 0.16942971669141613,
-                    "timestamp": 2.278217171097784
-                },
-                {
-                    "x": 6.575020543317634,
-                    "y": 5.950033200191844,
-                    "heading": 3.236518582605277,
-                    "angularVelocity": -5.2511914101363804e-8,
-                    "velocityX": -3.7765823921738897,
-                    "velocityY": -0.21923493705070526,
-                    "timestamp": 2.37815862779567
-                },
-                {
-                    "x": 6.202215559947926,
-                    "y": 5.88714179095661,
-                    "heading": 3.2365185785582873,
-                    "angularVelocity": -4.049360572502445e-8,
-                    "velocityX": -3.730233635643953,
-                    "velocityY": -0.6292824951045971,
-                    "timestamp": 2.478100084493556
-                },
-                {
-                    "x": 5.829410606689244,
-                    "y": 5.824250203230788,
-                    "heading": 3.2365185745112974,
-                    "angularVelocity": -4.049360338896351e-8,
-                    "velocityX": -3.7302333343573055,
-                    "velocityY": -0.6292842810560431,
-                    "timestamp": 2.578041541191442
-                },
-                {
-                    "x": 5.456605653430655,
-                    "y": 5.761358615504415,
-                    "heading": 3.2365185704643076,
-                    "angularVelocity": -4.049360329558589e-8,
-                    "velocityX": -3.730233334356378,
-                    "velocityY": -0.6292842810615451,
-                    "timestamp": 2.677982997889328
-                },
-                {
-                    "x": 5.0838007001693555,
-                    "y": 5.698467027794241,
-                    "heading": 3.2365185664173017,
-                    "angularVelocity": -4.0493766096106506e-8,
-                    "velocityX": -3.7302333343834975,
-                    "velocityY": -0.6292842808994649,
-                    "timestamp": 2.777924454587214
-                },
-                {
-                    "x": 4.755480441979736,
-                    "y": 5.643080213031107,
-                    "heading": 3.197910664693085,
-                    "angularVelocity": -0.38630517304670825,
-                    "velocityX": -3.28512580301988,
-                    "velocityY": -0.5541925902737567,
-                    "timestamp": 2.8778659112851
-                },
-                {
-                    "x": 4.50924022643339,
-                    "y": 5.601540105644793,
-                    "heading": 3.1689552309170463,
-                    "angularVelocity": -0.2897239517287447,
-                    "velocityX": -2.4638445714345307,
-                    "velocityY": -0.41564440582335266,
-                    "timestamp": 2.977807367982986
-                },
-                {
-                    "x": 4.345080076326452,
-                    "y": 5.573846702024279,
-                    "heading": 3.149651762381054,
-                    "angularVelocity": -0.19314776043684534,
-                    "velocityX": -1.6425631117543122,
-                    "velocityY": -0.2770962575043274,
-                    "timestamp": 3.077748824680872
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": -0.09657416151367515,
-                    "velocityX": -0.8212815686144453,
-                    "velocityY": -0.1385481308936372,
-                    "timestamp": 3.177690281378758
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": -1.245204860871434e-36,
-                    "velocityY": 0,
-                    "timestamp": 3.277631738076644
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -10875,380 +3299,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W3 To C3 To S1": {
+        "S3 To C5": {
             "waypoints": [
                 {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 21
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 7.68,
-                    "y": 4.127,
+                    "y": 0.787,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 16
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.358164523227949,
-                    "y": 4.34513375016413,
-                    "heading": 2.762567979975215,
-                    "angularVelocity": 0.1479885663322048,
-                    "velocityX": 0.6848900476349593,
-                    "velocityY": -0.17505080405008622,
-                    "timestamp": 0.08492534448237501
-                },
-                {
-                    "x": 2.474493569181997,
-                    "y": 4.315401331546784,
-                    "heading": 2.787705678078292,
-                    "angularVelocity": 0.2959975994950906,
-                    "velocityX": 1.3697800893606054,
-                    "velocityY": -0.35010065368078286,
-                    "timestamp": 0.16985068896475003
-                },
-                {
-                    "x": 2.6489871423142275,
-                    "y": 4.270802870950086,
-                    "heading": 2.825415221133538,
-                    "angularVelocity": 0.4440316761160991,
-                    "velocityX": 2.054670183509755,
-                    "velocityY": -0.525149010210423,
-                    "timestamp": 0.25477603344712507
-                },
-                {
-                    "x": 2.881645266961739,
-                    "y": 4.211338540895288,
-                    "heading": 2.8756978223326772,
-                    "angularVelocity": 0.5920800381277782,
-                    "velocityX": 2.739560564229402,
-                    "velocityY": -0.7001953352940375,
-                    "timestamp": 0.33970137792950006
-                },
-                {
-                    "x": 3.172468005146889,
-                    "y": 4.137008553769027,
-                    "heading": 2.9385516414717108,
-                    "angularVelocity": 0.7401067316492073,
-                    "velocityX": 3.4244516752652747,
-                    "velocityY": -0.8752391595148364,
-                    "timestamp": 0.42462672241187505
-                },
-                {
-                    "x": 3.48373017288665,
-                    "y": 4.0574555172899185,
-                    "heading": 2.9385516489955585,
-                    "angularVelocity": 8.859366728426556e-8,
-                    "velocityX": 3.6651269375110873,
-                    "velocityY": -0.936740815877624,
-                    "timestamp": 0.50955206689425
-                },
-                {
-                    "x": 3.7949923406136996,
-                    "y": 3.977902480761,
-                    "heading": 2.9385516565193726,
-                    "angularVelocity": 8.859327100912678e-8,
-                    "velocityX": 3.665126937361409,
-                    "velocityY": -0.9367408164641361,
-                    "timestamp": 0.594477411376625
-                },
-                {
-                    "x": 4.10625450834075,
-                    "y": 3.898349444232082,
-                    "heading": 2.938551664043187,
-                    "angularVelocity": 8.859327019599772e-8,
-                    "velocityX": 3.665126937361408,
-                    "velocityY": -0.9367408164641391,
-                    "timestamp": 0.679402755859
-                },
-                {
-                    "x": 4.417516676067799,
-                    "y": 3.818796407703164,
-                    "heading": 2.938551671567001,
-                    "angularVelocity": 8.859327006115765e-8,
-                    "velocityX": 3.6651269373614084,
-                    "velocityY": -0.9367408164641391,
-                    "timestamp": 0.764328100341375
-                },
-                {
-                    "x": 4.728778843794849,
-                    "y": 3.7392433711742457,
-                    "heading": 2.938551679090815,
-                    "angularVelocity": 8.859327000476347e-8,
-                    "velocityX": 3.665126937361408,
-                    "velocityY": -0.9367408164641391,
-                    "timestamp": 0.84925344482375
-                },
-                {
-                    "x": 5.0400410115218985,
-                    "y": 3.6596903346453273,
-                    "heading": 2.9385516866146295,
-                    "angularVelocity": 8.85932714022833e-8,
-                    "velocityX": 3.665126937361408,
-                    "velocityY": -0.9367408164641392,
-                    "timestamp": 0.934178789306125
-                },
-                {
-                    "x": 5.351303179248949,
-                    "y": 3.580137298116409,
-                    "heading": 2.9385516941384435,
-                    "angularVelocity": 8.859327127858633e-8,
-                    "velocityX": 3.665126937361408,
-                    "velocityY": -0.936740816464139,
-                    "timestamp": 1.0191041337885
-                },
-                {
-                    "x": 5.662565346976546,
-                    "y": 3.5005842615896343,
-                    "heading": 2.938551701662258,
-                    "angularVelocity": 8.859327107695318e-8,
-                    "velocityX": 3.6651269373678588,
-                    "velocityY": -0.9367408164388998,
-                    "timestamp": 1.104029478270875
-                },
-                {
-                    "x": 5.973827620415735,
-                    "y": 3.4210316386751205,
-                    "heading": 2.9385517091860733,
-                    "angularVelocity": 8.859328561882092e-8,
-                    "velocityX": 3.6651281821269137,
-                    "velocityY": -0.9367359461347136,
-                    "timestamp": 1.18895482275325
-                },
-                {
-                    "x": 6.292581444856613,
-                    "y": 3.380921520397279,
-                    "heading": 2.9385517170132656,
-                    "angularVelocity": 9.216556583241828e-8,
-                    "velocityX": 3.753341554087312,
-                    "velocityY": -0.4722985643722118,
-                    "timestamp": 1.273880167235625
-                },
-                {
-                    "x": 6.61322600810305,
-                    "y": 3.400918618969079,
-                    "heading": 2.938551726316265,
-                    "angularVelocity": 1.0954326272813381e-7,
-                    "velocityX": 3.7756050941068793,
-                    "velocityY": 0.23546679373140608,
-                    "timestamp": 1.358805511718
-                },
-                {
-                    "x": 6.924526035610863,
-                    "y": 3.4803224362243568,
-                    "heading": 2.9385522977985694,
-                    "angularVelocity": 0.000006729231518347491,
-                    "velocityX": 3.665572738093738,
-                    "velocityY": 0.9349837523680175,
-                    "timestamp": 1.443730856200375
-                },
-                {
-                    "x": 7.193617949641073,
-                    "y": 3.601275977700439,
-                    "heading": 2.9756713722126755,
-                    "angularVelocity": 0.43707887957768976,
-                    "velocityX": 3.1685701797306898,
-                    "velocityY": 1.4242337456892449,
-                    "timestamp": 1.52865620068275
-                },
-                {
-                    "x": 7.4107131876465635,
-                    "y": 3.752187582369628,
-                    "heading": 3.026776803352479,
-                    "angularVelocity": 0.6017689001003675,
-                    "velocityX": 2.5563068284114614,
-                    "velocityY": 1.7769913750601078,
-                    "timestamp": 1.613581545165125
-                },
-                {
-                    "x": 7.573210012728862,
-                    "y": 3.928266853326757,
-                    "heading": 3.0830399478590267,
-                    "angularVelocity": 0.6625012220965879,
-                    "velocityX": 1.9134079004651277,
-                    "velocityY": 2.0733418513677266,
-                    "timestamp": 1.6985068896475
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": 0.6707073428803676,
-                    "velocityX": 1.257457216359011,
-                    "velocityY": 2.340092323257951,
-                    "timestamp": 1.783432234129875
-                },
-                {
-                    "x": 7.730461589944827,
-                    "y": 4.347803549557207,
-                    "heading": 3.1951904179502595,
-                    "angularVelocity": 0.647312922727555,
-                    "velocityX": 0.5918498262162951,
-                    "velocityY": 2.5897428633591493,
-                    "timestamp": 1.868693034605951
-                },
-                {
-                    "x": 7.723612364676548,
-                    "y": 4.588311365928096,
-                    "heading": 3.24650445053595,
-                    "angularVelocity": 0.6018478867095445,
-                    "velocityX": -0.0803326409092394,
-                    "velocityY": 2.820848678735727,
-                    "timestamp": 1.953953835082027
-                },
-                {
-                    "x": 7.6586227331171335,
-                    "y": 4.845842471404587,
-                    "heading": 3.2908004330134384,
-                    "angularVelocity": 0.5195351466342113,
-                    "velocityX": -0.762245148960926,
-                    "velocityY": 3.020510059001304,
-                    "timestamp": 2.039214635558103
-                },
-                {
-                    "x": 7.534247896600953,
-                    "y": 5.114930519867729,
-                    "heading": 3.3218318995468055,
-                    "angularVelocity": 0.3639593618649453,
-                    "velocityX": -1.4587575512040862,
-                    "velocityY": 3.1560581997895576,
-                    "timestamp": 2.124475436034179
-                },
-                {
-                    "x": 7.350032329869969,
-                    "y": 5.3796839143357555,
-                    "heading": 3.321832164657738,
-                    "angularVelocity": 0.0000031094117170733377,
-                    "velocityX": -2.160612681353773,
-                    "velocityY": 3.1052182596188063,
-                    "timestamp": 2.209736236510255
-                },
-                {
-                    "x": 7.119535512814417,
-                    "y": 5.605296483491154,
-                    "heading": 3.3218321686163597,
-                    "angularVelocity": 4.6429567622162366e-8,
-                    "velocityX": -2.70343247739305,
-                    "velocityY": 2.6461465045558032,
-                    "timestamp": 2.294997036986331
-                },
-                {
-                    "x": 6.850897968160264,
-                    "y": 5.7838005054369255,
-                    "heading": 3.321832171960481,
-                    "angularVelocity": 3.9222265908183896e-8,
-                    "velocityX": -3.1507743670496304,
-                    "velocityY": 2.0936235755358563,
-                    "timestamp": 2.380257837462407
-                },
-                {
-                    "x": 6.563016789640927,
-                    "y": 5.929245490779759,
-                    "heading": 3.321832175184079,
-                    "angularVelocity": 3.780866961118305e-8,
-                    "velocityX": -3.376477547851724,
-                    "velocityY": 1.7058834133705343,
-                    "timestamp": 2.465518637938483
-                },
-                {
-                    "x": 6.275135446510561,
-                    "y": 6.074690150305356,
-                    "heading": 3.321832178407676,
-                    "angularVelocity": 3.780866514458699e-8,
-                    "velocityX": -3.3764794785282892,
-                    "velocityY": 1.7058795919516283,
-                    "timestamp": 2.550779438414559
-                },
-                {
-                    "x": 5.987254103377649,
-                    "y": 6.2201348098258835,
-                    "heading": 3.3218321816312852,
-                    "angularVelocity": 3.780881195826611e-8,
-                    "velocityX": -3.376479478558147,
-                    "velocityY": 1.705879591892162,
-                    "timestamp": 2.6360402388906348
-                },
-                {
-                    "x": 5.716836088038681,
-                    "y": 6.356756526684127,
-                    "heading": 3.3489498411718635,
-                    "angularVelocity": 0.3180554180720732,
-                    "velocityX": -3.171657008015607,
-                    "velocityY": 1.6023977736002932,
-                    "timestamp": 2.7213010393667108
-                },
-                {
-                    "x": 5.500501658783529,
-                    "y": 6.4660539115745435,
-                    "heading": 3.370644010379382,
-                    "angularVelocity": 0.2544448220798278,
-                    "velocityX": -2.537325805612817,
-                    "velocityY": 1.2819183526324525,
-                    "timestamp": 2.8065618398427867
-                },
-                {
-                    "x": 5.33825083142265,
-                    "y": 6.5480269544022365,
-                    "heading": 3.3869146454118173,
-                    "angularVelocity": 0.19083371187678314,
-                    "velocityX": -1.9029944177735691,
-                    "velocityY": 0.9614388132644309,
-                    "timestamp": 2.8918226403188627
-                },
-                {
-                    "x": 5.2300836110004285,
-                    "y": 6.602675651200572,
-                    "heading": 3.3977617464965464,
-                    "angularVelocity": 0.12722260434057955,
-                    "velocityX": -1.2686629707701718,
-                    "velocityY": 0.6409592273728405,
-                    "timestamp": 2.9770834407949387
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": 0.0636114211074268,
-                    "velocityX": -0.6343314946427733,
-                    "velocityY": 0.3204796183809554,
-                    "timestamp": 3.0623442412710147
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": 0,
-                    "velocityX": -1.2693090039370116e-34,
-                    "velocityY": 0,
-                    "timestamp": 3.1476050417470907
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -11268,371 +3340,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W3 To C3 To S2": {
+        "S4 To W1": {
             "waypoints": [
                 {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 21
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 15
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.361838792645819,
-                    "y": 4.349925567383164,
-                    "heading": 2.7594883701133286,
-                    "angularVelocity": 0.10940981634945876,
-                    "velocityX": 0.7130593416825138,
-                    "velocityY": -0.11616766728823658,
-                    "timestamp": 0.08672320665473099
-                },
-                {
-                    "x": 2.485516776956774,
-                    "y": 4.329776279587658,
-                    "heading": 2.7784262463080607,
-                    "angularVelocity": 0.21837149392012115,
-                    "velocityX": 1.4261232844324094,
-                    "velocityY": -0.2323402071111933,
-                    "timestamp": 0.17344641330946198
-                },
-                {
-                    "x": 2.671034614420037,
-                    "y": 4.299551431366552,
-                    "heading": 2.806748673224486,
-                    "angularVelocity": 0.3265841752046417,
-                    "velocityX": 2.1391948547504818,
-                    "velocityY": -0.34852087909340246,
-                    "timestamp": 0.26016961996419297
-                },
-                {
-                    "x": 2.9183936206497236,
-                    "y": 4.259249598753385,
-                    "heading": 2.8443244197137543,
-                    "angularVelocity": 0.4332836381257234,
-                    "velocityX": 2.852281595334577,
-                    "velocityY": -0.46471797074593596,
-                    "timestamp": 0.34689282661892396
-                },
-                {
-                    "x": 3.2275976397156323,
-                    "y": 4.208866459419414,
-                    "heading": 2.8907555208713065,
-                    "angularVelocity": 0.5353941920351031,
-                    "velocityX": 3.5654126616527764,
-                    "velocityY": -0.5809649029072455,
-                    "timestamp": 0.43361603327365494
-                },
-                {
-                    "x": 3.5513989095488143,
-                    "y": 4.156123400332019,
-                    "heading": 2.8907555264025566,
-                    "angularVelocity": 6.378050766718983e-8,
-                    "velocityX": 3.7337326688382726,
-                    "velocityY": -0.6081769934704949,
-                    "timestamp": 0.5203392399283859
-                },
-                {
-                    "x": 3.8752001793990973,
-                    "y": 4.103380341349552,
-                    "heading": 2.8907555319337894,
-                    "angularVelocity": 6.378030392692821e-8,
-                    "velocityX": 3.7337326690354664,
-                    "velocityY": -0.6081769922605936,
-                    "timestamp": 0.6070624465831169
-                },
-                {
-                    "x": 4.19900144924938,
-                    "y": 4.050637282367084,
-                    "heading": 2.890755537465022,
-                    "angularVelocity": 6.378030384357619e-8,
-                    "velocityX": 3.7337326690354677,
-                    "velocityY": -0.6081769922605877,
-                    "timestamp": 0.6937856532378479
-                },
-                {
-                    "x": 4.522802719099664,
-                    "y": 3.9978942233846166,
-                    "heading": 2.8907555429962546,
-                    "angularVelocity": 6.378030394841341e-8,
-                    "velocityX": 3.7337326690354673,
-                    "velocityY": -0.6081769922605876,
-                    "timestamp": 0.7805088598925789
-                },
-                {
-                    "x": 4.846603988949947,
-                    "y": 3.9451511644021493,
-                    "heading": 2.8907555485274874,
-                    "angularVelocity": 6.37803046219163e-8,
-                    "velocityX": 3.7337326690354673,
-                    "velocityY": -0.6081769922605877,
-                    "timestamp": 0.8672320665473099
-                },
-                {
-                    "x": 5.170405258800231,
-                    "y": 3.8924081054196815,
-                    "heading": 2.8907555540587198,
-                    "angularVelocity": 6.37803035811712e-8,
-                    "velocityX": 3.7337326690354673,
-                    "velocityY": -0.6081769922605876,
-                    "timestamp": 0.9539552732020409
-                },
-                {
-                    "x": 5.494206528650514,
-                    "y": 3.839665046437214,
-                    "heading": 2.8907555595899526,
-                    "angularVelocity": 6.378030493154673e-8,
-                    "velocityX": 3.7337326690354673,
-                    "velocityY": -0.6081769922605876,
-                    "timestamp": 1.0406784798567719
-                },
-                {
-                    "x": 5.818007798500798,
-                    "y": 3.7869219874547464,
-                    "heading": 2.8907555651211854,
-                    "angularVelocity": 6.378030454431603e-8,
-                    "velocityX": 3.7337326690354677,
-                    "velocityY": -0.6081769922605872,
-                    "timestamp": 1.1274016865115029
-                },
-                {
-                    "x": 6.141809068352602,
-                    "y": 3.734178928481622,
-                    "heading": 2.8907555706524177,
-                    "angularVelocity": 6.378030476763932e-8,
-                    "velocityX": 3.7337326690530155,
-                    "velocityY": -0.608176992152858,
-                    "timestamp": 1.2141248931662338
-                },
-                {
-                    "x": 6.4656106506759485,
-                    "y": 3.6814377878761273,
-                    "heading": 2.890755576183656,
-                    "angularVelocity": 6.378036992489158e-8,
-                    "velocityX": 3.733736272143269,
-                    "velocityY": -0.6081548715728616,
-                    "timestamp": 1.3008480998209648
-                },
-                {
-                    "x": 6.793655691240644,
-                    "y": 3.6853799559796676,
-                    "heading": 2.8907555914201803,
-                    "angularVelocity": 1.756914361676573e-7,
-                    "velocityX": 3.7826673299885396,
-                    "velocityY": 0.045456899665096306,
-                    "timestamp": 1.3875713064756958
-                },
-                {
-                    "x": 7.092289943250509,
-                    "y": 3.7425375783401047,
-                    "heading": 2.945166528243714,
-                    "angularVelocity": 0.6274091897932109,
-                    "velocityX": 3.4435333231946736,
-                    "velocityY": 0.6590810529873269,
-                    "timestamp": 1.4742945131304268
-                },
-                {
-                    "x": 7.330977190762065,
-                    "y": 3.8181008616818835,
-                    "heading": 3.0010867643989263,
-                    "angularVelocity": 0.6448128282182443,
-                    "velocityX": 2.7522880751150582,
-                    "velocityY": 0.8713156057826263,
-                    "timestamp": 1.5610177197851578
-                },
-                {
-                    "x": 7.508670206250799,
-                    "y": 3.908197537676412,
-                    "heading": 3.053304413172006,
-                    "angularVelocity": 0.6021185192211955,
-                    "velocityX": 2.0489673104014514,
-                    "velocityY": 1.0388992689491814,
-                    "timestamp": 1.6477409264398888
-                },
-                {
-                    "x": 7.6250594852295395,
-                    "y": 4.011408797557566,
-                    "heading": 3.099932944450418,
-                    "angularVelocity": 0.5376707467014636,
-                    "velocityX": 1.342077668346812,
-                    "velocityY": 1.1901227348761163,
-                    "timestamp": 1.7344641330946198
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": 0.4620107707627286,
-                    "velocityX": 0.6335157207596422,
-                    "velocityY": 1.332875096542895,
-                    "timestamp": 1.8211873397493508
-                },
-                {
-                    "x": 7.66415878383622,
-                    "y": 4.271751344750917,
-                    "heading": 3.1759557228740287,
-                    "angularVelocity": 0.3694329307480838,
-                    "velocityX": -0.16276315552044252,
-                    "velocityY": 1.487271267174521,
-                    "timestamp": 1.918514134091254
-                },
-                {
-                    "x": 7.570652845333858,
-                    "y": 4.430649041463004,
-                    "heading": 3.2028903885273214,
-                    "angularVelocity": 0.2767446090814936,
-                    "velocityX": -0.9607419943770155,
-                    "velocityY": 1.6326202644041503,
-                    "timestamp": 2.015840928433157
-                },
-                {
-                    "x": 7.399206149109808,
-                    "y": 4.602082311613017,
-                    "heading": 3.2207860785976568,
-                    "angularVelocity": 0.1838721822839176,
-                    "velocityX": -1.761557003734937,
-                    "velocityY": 1.7614190553505542,
-                    "timestamp": 2.11316772277506
-                },
-                {
-                    "x": 7.149293090301517,
-                    "y": 4.782169514035782,
-                    "heading": 3.229614246279924,
-                    "angularVelocity": 0.09070644668783194,
-                    "velocityX": -2.5677724258579846,
-                    "velocityY": 1.8503352919456928,
-                    "timestamp": 2.210494517116963
-                },
-                {
-                    "x": 6.821439370412565,
-                    "y": 4.949709126664123,
-                    "heading": 3.229614240242725,
-                    "angularVelocity": -6.203018521599257e-8,
-                    "velocityX": -3.3685864422619463,
-                    "velocityY": 1.7214130369873843,
-                    "timestamp": 2.3078213114588664
-                },
-                {
-                    "x": 6.465396251393308,
-                    "y": 5.04346891931761,
-                    "heading": 3.2296142362544686,
-                    "angularVelocity": -4.0977992187262516e-8,
-                    "velocityX": -3.658223014810281,
-                    "velocityY": 0.9633502601977828,
-                    "timestamp": 2.4051481058007695
-                },
-                {
-                    "x": 6.106941254304619,
-                    "y": 5.12753803654929,
-                    "heading": 3.2296142322922576,
-                    "angularVelocity": -4.071038102398141e-8,
-                    "velocityX": -3.6830042488552306,
-                    "velocityY": 0.8637818372641555,
-                    "timestamp": 2.5024749001426727
-                },
-                {
-                    "x": 5.7484862494466205,
-                    "y": 5.211607120654073,
-                    "heading": 3.2296142283300466,
-                    "angularVelocity": -4.071038072165154e-8,
-                    "velocityX": -3.6830043286822827,
-                    "velocityY": 0.8637814968964618,
-                    "timestamp": 2.599801694484576
-                },
-                {
-                    "x": 5.390031244588595,
-                    "y": 5.295676204758746,
-                    "heading": 3.229614224367836,
-                    "angularVelocity": -4.0710380903709755e-8,
-                    "velocityX": -3.683004328682549,
-                    "velocityY": 0.8637814968953244,
-                    "timestamp": 2.697128488826479
-                },
-                {
-                    "x": 5.031576239716237,
-                    "y": 5.3797452888021455,
-                    "heading": 3.2296142204055998,
-                    "angularVelocity": -4.071064154144793e-8,
-                    "velocityX": -3.683004328829807,
-                    "velocityY": 0.8637814962657624,
-                    "timestamp": 2.794455283168382
-                },
-                {
-                    "x": 4.7241457561516045,
-                    "y": 5.451847166184253,
-                    "heading": 3.193769184032689,
-                    "angularVelocity": -0.3682956642649294,
-                    "velocityX": -3.1587445743322107,
-                    "velocityY": 0.740822482335317,
-                    "timestamp": 2.8917820775102854
-                },
-                {
-                    "x": 4.493572881554858,
-                    "y": 5.505923581174423,
-                    "heading": 3.1668847241449476,
-                    "angularVelocity": -0.27622876176636874,
-                    "velocityX": -2.3690585532567625,
-                    "velocityY": 0.5556169331972818,
-                    "timestamp": 2.9891088718521885
-                },
-                {
-                    "x": 4.3398576283261585,
-                    "y": 5.541974526790158,
-                    "heading": 3.1489615588617923,
-                    "angularVelocity": -0.1841544808328986,
-                    "velocityX": -1.5793724047738293,
-                    "velocityY": 0.37041131231641944,
-                    "timestamp": 3.0864356661940917
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": -0.09207699608717179,
-                    "velocityX": -0.7896862199751749,
-                    "velocityY": 0.18520566028835728,
-                    "timestamp": 3.183762460535995
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": -4.104369359528437e-34,
-                    "velocityY": 0,
-                    "timestamp": 3.281089254877898
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -11652,389 +3381,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W3 To C3 To S3": {
+        "S4 To W2": {
             "waypoints": [
                 {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 21
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 7.68,
-                    "y": 4.127,
+                    "x": 2.25,
+                    "y": 5.56,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 17
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.362347004549993,
-                    "y": 4.364661246321825,
-                    "heading": 2.7702286217513103,
-                    "angularVelocity": 0.23313109188524597,
-                    "velocityX": 0.7185375961450525,
-                    "velocityY": 0.05371999426914206,
-                    "timestamp": 0.0867693004297674
-                },
-                {
-                    "x": 2.4870912612086284,
-                    "y": 4.373976445805142,
-                    "heading": 2.8086286090699177,
-                    "angularVelocity": 0.4425526900460507,
-                    "velocityX": 1.4376542860294879,
-                    "velocityY": 0.10735593622605764,
-                    "timestamp": 0.1735386008595348
-                },
-                {
-                    "x": 2.674306466073838,
-                    "y": 4.3879313037176075,
-                    "heading": 2.8616727700685,
-                    "angularVelocity": 0.6113240597291352,
-                    "velocityX": 2.1576203096940354,
-                    "velocityY": 0.16082713405947677,
-                    "timestamp": 0.2603079012893022
-                },
-                {
-                    "x": 2.9240965513714046,
-                    "y": 4.40648950041988,
-                    "heading": 2.921990092412666,
-                    "angularVelocity": 0.6951458873750875,
-                    "velocityX": 2.878784132871411,
-                    "velocityY": 0.2138797548251989,
-                    "timestamp": 0.3470772017190696
-                },
-                {
-                    "x": 3.236315453938978,
-                    "y": 4.429486199889588,
-                    "heading": 2.9648285300542776,
-                    "angularVelocity": 0.493705001993024,
-                    "velocityX": 3.598264605351849,
-                    "velocityY": 0.26503267118445795,
-                    "timestamp": 0.43384650214883697
-                },
-                {
-                    "x": 3.563623741097209,
-                    "y": 4.454241375249833,
-                    "heading": 2.9648285457681323,
-                    "angularVelocity": 1.810992388928579e-7,
-                    "velocityX": 3.7721669477231816,
-                    "velocityY": 0.28529877776624385,
-                    "timestamp": 0.5206158025786044
-                },
-                {
-                    "x": 3.8909320279988893,
-                    "y": 4.478996554002297,
-                    "heading": 2.9648285614819643,
-                    "angularVelocity": 1.8109898446503704e-7,
-                    "velocityX": 3.7721669447664756,
-                    "velocityY": 0.28529881686094194,
-                    "timestamp": 0.6073851030083718
-                },
-                {
-                    "x": 4.218240314900568,
-                    "y": 4.503751732754777,
-                    "heading": 2.9648285771957963,
-                    "angularVelocity": 1.810989844949235e-7,
-                    "velocityX": 3.772166944766461,
-                    "velocityY": 0.2852988168611311,
-                    "timestamp": 0.6941544034381392
-                },
-                {
-                    "x": 4.545548601802246,
-                    "y": 4.528506911507257,
-                    "heading": 2.9648285929096283,
-                    "angularVelocity": 1.810989846493643e-7,
-                    "velocityX": 3.7721669447664614,
-                    "velocityY": 0.2852988168611311,
-                    "timestamp": 0.7809237038679065
-                },
-                {
-                    "x": 4.872856888703925,
-                    "y": 4.5532620902597385,
-                    "heading": 2.9648286086234608,
-                    "angularVelocity": 1.8109898537942274e-7,
-                    "velocityX": 3.7721669447664614,
-                    "velocityY": 0.28529881686113107,
-                    "timestamp": 0.8676930042976739
-                },
-                {
-                    "x": 5.200165175605603,
-                    "y": 4.578017269012219,
-                    "heading": 2.964828624337293,
-                    "angularVelocity": 1.8109898550822278e-7,
-                    "velocityX": 3.7721669447664614,
-                    "velocityY": 0.28529881686113095,
-                    "timestamp": 0.9544623047274413
-                },
-                {
-                    "x": 5.527473462507282,
-                    "y": 4.602772447764699,
-                    "heading": 2.9648286400511257,
-                    "angularVelocity": 1.8109898581520642e-7,
-                    "velocityX": 3.772166944766462,
-                    "velocityY": 0.2852988168611311,
-                    "timestamp": 1.0412316051572088
-                },
-                {
-                    "x": 5.854781749408961,
-                    "y": 4.627527626517179,
-                    "heading": 2.964828655764958,
-                    "angularVelocity": 1.8109898550546543e-7,
-                    "velocityX": 3.772166944766461,
-                    "velocityY": 0.28529881686113034,
-                    "timestamp": 1.1280009055869762
-                },
-                {
-                    "x": 6.182090036311611,
-                    "y": 4.652282805256812,
-                    "heading": 2.9648286714787906,
-                    "angularVelocity": 1.810989852504463e-7,
-                    "velocityX": 3.772166944777661,
-                    "velocityY": 0.2852988167130518,
-                    "timestamp": 1.2147702060167436
-                },
-                {
-                    "x": 6.509398523986194,
-                    "y": 4.6770353292870315,
-                    "heading": 2.964828687192649,
-                    "angularVelocity": 1.8109928558732558e-7,
-                    "velocityX": 3.7721692586367297,
-                    "velocityY": 0.2852682216823196,
-                    "timestamp": 1.301539506446511
-                },
-                {
-                    "x": 6.835912518328979,
-                    "y": 4.643388028658291,
-                    "heading": 2.964828718989663,
-                    "angularVelocity": 3.6645465598978293e-7,
-                    "velocityX": 3.763012871206354,
-                    "velocityY": -0.38777886259409694,
-                    "timestamp": 1.3883088068762783
-                },
-                {
-                    "x": 7.128079325417914,
-                    "y": 4.560807160262635,
-                    "heading": 3.033554953244841,
-                    "angularVelocity": 0.7920570284049364,
-                    "velocityX": 3.3671679458268646,
-                    "velocityY": -0.9517291021897464,
-                    "timestamp": 1.4750781073060457
-                },
-                {
-                    "x": 7.358963687834594,
-                    "y": 4.4649781472961925,
-                    "heading": 3.091042362636633,
-                    "angularVelocity": 0.66253166854011,
-                    "velocityX": 2.660899203671262,
-                    "velocityY": -1.1044114968289644,
-                    "timestamp": 1.561847407735813
-                },
-                {
-                    "x": 7.527984669479397,
-                    "y": 4.3598798773022285,
-                    "heading": 3.1296058219303373,
-                    "angularVelocity": 0.44443667406213694,
-                    "velocityX": 1.947935281345401,
-                    "velocityY": -1.211237954822889,
-                    "timestamp": 1.6486167081655805
-                },
-                {
-                    "x": 7.635013269633329,
-                    "y": 4.246969115663087,
-                    "heading": 3.1464129858106427,
-                    "angularVelocity": 0.19369942822011468,
-                    "velocityX": 1.2334846497991885,
-                    "velocityY": -1.3012754635555983,
-                    "timestamp": 1.7353860085953479
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": -0.07390846507784325,
-                    "velocityX": 0.5184636748694783,
-                    "velocityY": -1.3826216768935726,
-                    "timestamp": 1.8221553090251152
-                },
-                {
-                    "x": 7.665105053681803,
-                    "y": 4.004188436329708,
-                    "heading": 3.1110105191844553,
-                    "angularVelocity": -0.3438049921625736,
-                    "velocityX": -0.17664879667123173,
-                    "velocityY": -1.4565017205308959,
-                    "timestamp": 1.9064748549200063
-                },
-                {
-                    "x": 7.591499034942275,
-                    "y": 3.875650129704607,
-                    "heading": 3.0609451613914906,
-                    "angularVelocity": -0.5937574409541295,
-                    "velocityX": -0.8729413561036249,
-                    "velocityY": -1.524418867071828,
-                    "timestamp": 1.9907944008148974
-                },
-                {
-                    "x": 7.459038327956553,
-                    "y": 3.7422011819159797,
-                    "heading": 2.9924948430687532,
-                    "angularVelocity": -0.8117965721502389,
-                    "velocityX": -1.5709371484381591,
-                    "velocityY": -1.582657334931321,
-                    "timestamp": 2.0751139467097883
-                },
-                {
-                    "x": 7.267508984071126,
-                    "y": 3.6053961445739597,
-                    "heading": 2.9106651455038604,
-                    "angularVelocity": -0.9704712791847574,
-                    "velocityX": -2.271470296153853,
-                    "velocityY": -1.6224593703641987,
-                    "timestamp": 2.159433492604679
-                },
-                {
-                    "x": 7.016701982610521,
-                    "y": 3.4693216018432858,
-                    "heading": 2.828330372603407,
-                    "angularVelocity": -0.9764612940763276,
-                    "velocityX": -2.9744823551736075,
-                    "velocityY": -1.6137959625671907,
-                    "timestamp": 2.24375303849957
-                },
-                {
-                    "x": 6.717326936237698,
-                    "y": 3.359230054214691,
-                    "heading": 2.828330320326688,
-                    "angularVelocity": -6.199834052808441e-7,
-                    "velocityX": -3.5504821947927914,
-                    "velocityY": -1.3056468279114077,
-                    "timestamp": 2.328072584394461
-                },
-                {
-                    "x": 6.402750279383998,
-                    "y": 3.3064370272518535,
-                    "heading": 2.8283303027221653,
-                    "angularVelocity": -2.0878341406433196e-7,
-                    "velocityX": -3.730767920000857,
-                    "velocityY": -0.6261066328398724,
-                    "timestamp": 2.412392130289352
-                },
-                {
-                    "x": 6.0870128630404565,
-                    "y": 3.261099838136822,
-                    "heading": 2.8283302851918997,
-                    "angularVelocity": -2.0790275245783378e-7,
-                    "velocityX": -3.7445341171206636,
-                    "velocityY": -0.5376830322538348,
-                    "timestamp": 2.4967116761842427
-                },
-                {
-                    "x": 5.771275440975781,
-                    "y": 3.215762688864957,
-                    "heading": 2.8283302676616344,
-                    "angularVelocity": -2.079027527224642e-7,
-                    "velocityX": -3.7445341849713016,
-                    "velocityY": -0.5376825597279692,
-                    "timestamp": 2.5810312220791336
-                },
-                {
-                    "x": 5.455538018911073,
-                    "y": 3.1704255395933156,
-                    "heading": 2.8283302501313687,
-                    "angularVelocity": -2.079027528417884e-7,
-                    "velocityX": -3.7445341849716827,
-                    "velocityY": -0.5376825597253148,
-                    "timestamp": 2.6653507679740245
-                },
-                {
-                    "x": 5.13980059641411,
-                    "y": 3.1250883933321525,
-                    "heading": 2.8283302326010555,
-                    "angularVelocity": -2.0790331306647263e-7,
-                    "velocityX": -3.7445341900980704,
-                    "velocityY": -0.5376825240221068,
-                    "timestamp": 2.7496703138689154
-                },
-                {
-                    "x": 4.847289842857696,
-                    "y": 3.0834618283006696,
-                    "heading": 2.7687586646684967,
-                    "angularVelocity": -0.706497731935343,
-                    "velocityX": -3.469074109116353,
-                    "velocityY": -0.4936763426521838,
-                    "timestamp": 2.8339898597638062
-                },
-                {
-                    "x": 4.613405710202319,
-                    "y": 3.0500832751446354,
-                    "heading": 2.6983183223854565,
-                    "angularVelocity": -0.8353975526724018,
-                    "velocityX": -2.773783114853639,
-                    "velocityY": -0.39585783820090814,
-                    "timestamp": 2.918309405658697
-                },
-                {
-                    "x": 4.43813741968374,
-                    "y": 3.025042089065369,
-                    "heading": 2.6386185119236694,
-                    "angularVelocity": -0.7080186429870714,
-                    "velocityX": -2.0786199529236167,
-                    "velocityY": -0.29697961265685946,
-                    "timestamp": 3.002628951553588
-                },
-                {
-                    "x": 4.321361759336474,
-                    "y": 3.008347212158301,
-                    "heading": 2.596145764785128,
-                    "angularVelocity": -0.5037117632427286,
-                    "velocityX": -1.3849180413380506,
-                    "velocityY": -0.19799533702279348,
-                    "timestamp": 3.086948497448479
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": -0.26264093989232323,
-                    "velocityX": -0.6921498297586374,
-                    "velocityY": -0.0989949847299469,
-                    "timestamp": 3.1712680433433698
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": 0,
-                    "velocityX": -1.507480653475374e-32,
-                    "velocityY": 0,
-                    "timestamp": 3.2555875892382606
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -12054,26 +3422,8 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W3 To C3 To S4": {
+        "S4 To W3": {
             "waypoints": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 21
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 18
-                },
                 {
                     "x": 5.176,
                     "y": 1.62,
@@ -12082,370 +3432,18 @@
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
+                },
                 {
                     "x": 2.3,
                     "y": 4.36,
                     "heading": 2.75,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.35775083125341,
-                    "y": 4.369912807689986,
-                    "heading": 2.783102260210542,
-                    "angularVelocity": 0.39244175739179554,
-                    "velocityX": 0.6846613362282545,
-                    "velocityY": 0.11752066613584591,
-                    "timestamp": 0.08434948520907419
-                },
-                {
-                    "x": 2.473379945358936,
-                    "y": 4.389732628016986,
-                    "heading": 2.846401171320743,
-                    "angularVelocity": 0.7504362469231927,
-                    "velocityX": 1.3708336668436103,
-                    "velocityY": 0.2349726293868104,
-                    "timestamp": 0.16869897041814838
-                },
-                {
-                    "x": 2.647085500332804,
-                    "y": 4.419444254302336,
-                    "heading": 2.9346614354688145,
-                    "angularVelocity": 1.0463639929669244,
-                    "velocityX": 2.0593552473178725,
-                    "velocityY": 0.35224431081829954,
-                    "timestamp": 0.25304845562722256
-                },
-                {
-                    "x": 2.8791780514976093,
-                    "y": 4.45897416371866,
-                    "heading": 3.036264025920344,
-                    "angularVelocity": 1.2045431006447933,
-                    "velocityX": 2.7515585968251655,
-                    "velocityY": 0.4686443470086779,
-                    "timestamp": 0.33739794083629676
-                },
-                {
-                    "x": 3.1692885380051594,
-                    "y": 4.507734938420023,
-                    "heading": 3.1081945167682674,
-                    "angularVelocity": 0.8527673959080092,
-                    "velocityX": 3.439386568732019,
-                    "velocityY": 0.5780802879887197,
-                    "timestamp": 0.42174742604537097
-                },
-                {
-                    "x": 3.483614233052985,
-                    "y": 4.562663983733042,
-                    "heading": 3.108194542936824,
-                    "angularVelocity": 3.1023966932602597e-7,
-                    "velocityX": 3.726468445761316,
-                    "velocityY": 0.651207831048041,
-                    "timestamp": 0.5060969112544451
-                },
-                {
-                    "x": 3.7979399253430755,
-                    "y": 4.61759304482709,
-                    "heading": 3.108194569105307,
-                    "angularVelocity": 3.102387994950786e-7,
-                    "velocityX": 3.726468413067156,
-                    "velocityY": 0.6512080181390185,
-                    "timestamp": 0.5904463964635193
-                },
-                {
-                    "x": 4.112265617633151,
-                    "y": 4.67252210592122,
-                    "heading": 3.1081945952737904,
-                    "angularVelocity": 3.1023880025122276e-7,
-                    "velocityX": 3.7264684130669847,
-                    "velocityY": 0.6512080181399977,
-                    "timestamp": 0.6747958816725934
-                },
-                {
-                    "x": 4.426591309923227,
-                    "y": 4.72745116701535,
-                    "heading": 3.1081946214422733,
-                    "angularVelocity": 3.102388010308895e-7,
-                    "velocityX": 3.7264684130669847,
-                    "velocityY": 0.6512080181399976,
-                    "timestamp": 0.7591453668816676
-                },
-                {
-                    "x": 4.740917002213303,
-                    "y": 4.7823802281094805,
-                    "heading": 3.1081946476107567,
-                    "angularVelocity": 3.1023880026526175e-7,
-                    "velocityX": 3.726468413066985,
-                    "velocityY": 0.6512080181399978,
-                    "timestamp": 0.8434948520907417
-                },
-                {
-                    "x": 5.055242694503379,
-                    "y": 4.837309289203611,
-                    "heading": 3.1081946737792396,
-                    "angularVelocity": 3.1023880190764424e-7,
-                    "velocityX": 3.7264684130669847,
-                    "velocityY": 0.6512080181399977,
-                    "timestamp": 0.9278443372998159
-                },
-                {
-                    "x": 5.369568386793454,
-                    "y": 4.892238350297741,
-                    "heading": 3.108194699947723,
-                    "angularVelocity": 3.102388017253904e-7,
-                    "velocityX": 3.726468413066985,
-                    "velocityY": 0.6512080181399975,
-                    "timestamp": 1.01219382250889
-                },
-                {
-                    "x": 5.683894079083919,
-                    "y": 4.947167411389647,
-                    "heading": 3.108194726116206,
-                    "angularVelocity": 3.102388024033471e-7,
-                    "velocityX": 3.7264684130715926,
-                    "velocityY": 0.6512080181136309,
-                    "timestamp": 1.0965433077179643
-                },
-                {
-                    "x": 5.9982198456255675,
-                    "y": 5.002096047585362,
-                    "heading": 3.108194752284696,
-                    "angularVelocity": 3.102388798208338e-7,
-                    "velocityX": 3.7264692933518155,
-                    "velocityY": 0.6512029807836425,
-                    "timestamp": 1.1808927929270385
-                },
-                {
-                    "x": 6.316938354394085,
-                    "y": 5.017469863405169,
-                    "heading": 3.1081947796852947,
-                    "angularVelocity": 3.2484607308932857e-7,
-                    "velocityX": 3.7785471716694046,
-                    "velocityY": 0.18226330346533962,
-                    "timestamp": 1.2652422781361128
-                },
-                {
-                    "x": 6.6329944526576226,
-                    "y": 4.973579291611239,
-                    "heading": 3.1081948124098697,
-                    "angularVelocity": 3.8796413400864556e-7,
-                    "velocityX": 3.7469831319081606,
-                    "velocityY": -0.5203419046973292,
-                    "timestamp": 1.349591763345187
-                },
-                {
-                    "x": 6.935463453156901,
-                    "y": 4.87194123143387,
-                    "heading": 3.1081950470159,
-                    "angularVelocity": 0.0000027813569896549863,
-                    "velocityX": 3.5859021516202274,
-                    "velocityY": -1.2049636097415746,
-                    "timestamp": 1.4339412485542613
-                },
-                {
-                    "x": 7.201083494587787,
-                    "y": 4.724560137748792,
-                    "heading": 3.137800042082307,
-                    "angularVelocity": 0.3509801511298589,
-                    "velocityX": 3.1490416423111984,
-                    "velocityY": -1.7472672574084946,
-                    "timestamp": 1.5182907337633356
-                },
-                {
-                    "x": 7.415442295317108,
-                    "y": 4.547383804871535,
-                    "heading": 3.160342409540337,
-                    "angularVelocity": 0.26724961512396933,
-                    "velocityX": 2.541317237419968,
-                    "velocityY": -2.1005028357683178,
-                    "timestamp": 1.6026402189724098
-                },
-                {
-                    "x": 7.575455349602933,
-                    "y": 4.347068945452764,
-                    "heading": 3.163134577298841,
-                    "angularVelocity": 0.033102368693571485,
-                    "velocityX": 1.8970246693172619,
-                    "velocityY": -2.374820177292801,
-                    "timestamp": 1.686989704181484
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": -0.27427052152716735,
-                    "velocityX": 1.2394225067045201,
-                    "velocityY": -2.6090134979162847,
-                    "timestamp": 1.7713391893905583
-                },
-                {
-                    "x": 7.728694415127363,
-                    "y": 3.916223066041241,
-                    "heading": 3.095787658836112,
-                    "angularVelocity": -0.5866618963302697,
-                    "velocityX": 0.6461353813727732,
-                    "velocityY": -2.796838903430975,
-                    "timestamp": 1.84670174734847
-                },
-                {
-                    "x": 7.732075801393433,
-                    "y": 3.692700126888283,
-                    "heading": 3.0330227964139005,
-                    "angularVelocity": -0.8328388011625617,
-                    "velocityX": 0.04486825232179016,
-                    "velocityY": -2.9659680511082303,
-                    "timestamp": 1.9220643053063815
-                },
-                {
-                    "x": 7.689365986032336,
-                    "y": 3.4587309949886484,
-                    "heading": 2.9594753921163566,
-                    "angularVelocity": -0.9759143836202951,
-                    "velocityX": -0.5667245979754308,
-                    "velocityY": -3.10458055351971,
-                    "timestamp": 1.997426863264293
-                },
-                {
-                    "x": 7.599710660115244,
-                    "y": 3.218597864956733,
-                    "heading": 2.8890529754773646,
-                    "angularVelocity": -0.9344483327957235,
-                    "velocityX": -1.1896534346294716,
-                    "velocityY": -3.1863718076823337,
-                    "timestamp": 2.072789421222205
-                },
-                {
-                    "x": 7.463852101408951,
-                    "y": 2.9820453977091343,
-                    "heading": 2.853986095526745,
-                    "angularVelocity": -0.4653090460411922,
-                    "velocityX": -1.8027328475523154,
-                    "velocityY": -3.138859317642961,
-                    "timestamp": 2.1481519791801165
-                },
-                {
-                    "x": 7.284189485763313,
-                    "y": 2.760688689378167,
-                    "heading": 2.8539858842251613,
-                    "angularVelocity": -0.000002803800583333773,
-                    "velocityX": -2.3839771434772157,
-                    "velocityY": -2.9372239256341386,
-                    "timestamp": 2.223514537138028
-                },
-                {
-                    "x": 7.070363087296056,
-                    "y": 2.5721263513481594,
-                    "heading": 2.853985845147589,
-                    "angularVelocity": -5.185276832708053e-7,
-                    "velocityX": -2.8373028233287183,
-                    "velocityY": -2.502069238882734,
-                    "timestamp": 2.2988770950959396
-                },
-                {
-                    "x": 6.82827305330887,
-                    "y": 2.421561078917411,
-                    "heading": 2.8539858112509897,
-                    "angularVelocity": -4.4978037103687734e-7,
-                    "velocityX": -3.212338335468772,
-                    "velocityY": -1.9978790066392913,
-                    "timestamp": 2.374239653053851
-                },
-                {
-                    "x": 6.571710658920333,
-                    "y": 2.2972500819001187,
-                    "heading": 2.8539857783310936,
-                    "angularVelocity": -4.368203133600748e-7,
-                    "velocityX": -3.4043748160966305,
-                    "velocityY": -1.6495060728527418,
-                    "timestamp": 2.449602211011763
-                },
-                {
-                    "x": 6.3151480830563145,
-                    "y": 2.172939459426277,
-                    "heading": 2.8539857454112014,
-                    "angularVelocity": -4.3682025848078035e-7,
-                    "velocityX": -3.4043772241290333,
-                    "velocityY": -1.6495011029650315,
-                    "timestamp": 2.5249647689696744
-                },
-                {
-                    "x": 6.058585509566717,
-                    "y": 2.048628832051934,
-                    "heading": 2.853985712491288,
-                    "angularVelocity": -4.3682054129494706e-7,
-                    "velocityX": -3.404377192622409,
-                    "velocityY": -1.6495011679907159,
-                    "timestamp": 2.600327326927586
-                },
-                {
-                    "x": 5.808645132247433,
-                    "y": 1.9267432640802056,
-                    "heading": 2.8368087867037386,
-                    "angularVelocity": -0.22792386900060174,
-                    "velocityX": -3.31650602224609,
-                    "velocityY": -1.6173225972478102,
-                    "timestamp": 2.6756898848854975
-                },
-                {
-                    "x": 5.597200041398206,
-                    "y": 1.824292282290654,
-                    "heading": 2.751245197802903,
-                    "angularVelocity": -1.1353594041834565,
-                    "velocityX": -2.805704803270014,
-                    "velocityY": -1.3594414065240215,
-                    "timestamp": 2.751052442843409
-                },
-                {
-                    "x": 5.4283718675083765,
-                    "y": 1.7424505925946856,
-                    "heading": 2.6610334034240704,
-                    "angularVelocity": -1.1970373196357535,
-                    "velocityX": -2.2402128917136284,
-                    "velocityY": -1.0859728214330957,
-                    "timestamp": 2.8264150008013207
-                },
-                {
-                    "x": 5.302034408244249,
-                    "y": 1.6811743111050435,
-                    "heading": 2.5842866606936457,
-                    "angularVelocity": -1.0183670088969912,
-                    "velocityX": -1.6763955827333314,
-                    "velocityY": -0.8130865399216356,
-                    "timestamp": 2.9017775587592323
-                },
-                {
-                    "x": 5.217968301319177,
-                    "y": 1.6403775914720826,
-                    "heading": 2.5290716090988563,
-                    "angularVelocity": -0.7326589368904639,
-                    "velocityX": -1.1154890333210488,
-                    "velocityY": -0.5413393698200221,
-                    "timestamp": 2.977140116717144
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": -0.38575666599708336,
-                    "velocityX": -0.5568853082536673,
-                    "velocityY": -0.27039410582988466,
-                    "timestamp": 3.0525026746750554
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": 0,
-                    "velocityX": 5.6652938345374496e-33,
-                    "velocityY": 3.5857679285094535e-32,
-                    "timestamp": 3.127865232632967
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 }
             ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -12465,398 +3463,69 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W3 To C4 To S3": {
+        "S4 To C1": {
             "waypoints": [
                 {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 22
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 7.68,
-                    "y": 2.457,
+                    "y": 7.467,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 17
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
+            "trajectory": [],
+            "constraints": [
                 {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -1.5074616592069685e-33,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.3610343967602465,
-                    "y": 4.342403579285033,
-                    "heading": 2.7671031550368004,
-                    "angularVelocity": 0.1956775196968839,
-                    "velocityX": 0.6982956857107938,
-                    "velocityY": -0.20132098163403012,
-                    "timestamp": 0.08740480287819585
-                },
-                {
-                    "x": 2.4831385894662814,
-                    "y": 4.307214947818553,
-                    "heading": 2.799662518084232,
-                    "angularVelocity": 0.3725122873717296,
-                    "velocityX": 1.3969963741717348,
-                    "velocityY": -0.4025937969966943,
-                    "timestamp": 0.1748096057563917
-                },
-                {
-                    "x": 2.6663651072482595,
-                    "y": 4.254442931030586,
-                    "heading": 2.8448933899423676,
-                    "angularVelocity": 0.5174872589229184,
-                    "velocityX": 2.0962980494025705,
-                    "velocityY": -0.6037656404477811,
-                    "timestamp": 0.26221440863458756
-                },
-                {
-                    "x": 2.9107932372110517,
-                    "y": 4.184113644093446,
-                    "heading": 2.897077048059956,
-                    "angularVelocity": 0.597034217791321,
-                    "velocityX": 2.796506849897233,
-                    "velocityY": -0.8046386997193782,
-                    "timestamp": 0.3496192115127834
-                },
-                {
-                    "x": 3.2164117287156215,
-                    "y": 4.096391426129405,
-                    "heading": 2.937460118957845,
-                    "angularVelocity": 0.46202347660649606,
-                    "velocityX": 3.4965869316182627,
-                    "velocityY": -1.0036315519901937,
-                    "timestamp": 0.43702401439097927
-                },
-                {
-                    "x": 3.5340454438791253,
-                    "y": 4.004541690732033,
-                    "heading": 2.9374601327514616,
-                    "angularVelocity": 1.5781302417851277e-7,
-                    "velocityX": 3.6340533323568787,
-                    "velocityY": -1.0508545568756669,
-                    "timestamp": 0.5244288172691751
-                },
-                {
-                    "x": 3.8516791581377654,
-                    "y": 3.9126919522054426,
-                    "heading": 2.93746014654506,
-                    "angularVelocity": 1.5781282325664268e-7,
-                    "velocityX": 3.634053322004319,
-                    "velocityY": -1.050854592677122,
-                    "timestamp": 0.611833620147371
-                },
-                {
-                    "x": 4.169312872396402,
-                    "y": 3.820842213678837,
-                    "heading": 2.9374601603386585,
-                    "angularVelocity": 1.5781282237025435e-7,
-                    "velocityX": 3.63405332200427,
-                    "velocityY": -1.0508545926772908,
-                    "timestamp": 0.6992384230255668
-                },
-                {
-                    "x": 4.486946586655037,
-                    "y": 3.7289924751522316,
-                    "heading": 2.9374601741322572,
-                    "angularVelocity": 1.5781282238426364e-7,
-                    "velocityX": 3.63405332200427,
-                    "velocityY": -1.0508545926772912,
-                    "timestamp": 0.7866432259037627
-                },
-                {
-                    "x": 4.804580300913673,
-                    "y": 3.637142736625626,
-                    "heading": 2.937460187925856,
-                    "angularVelocity": 1.578128226198401e-7,
-                    "velocityX": 3.63405332200427,
-                    "velocityY": -1.0508545926772912,
-                    "timestamp": 0.8740480287819585
-                },
-                {
-                    "x": 5.122214015172308,
-                    "y": 3.545292998099021,
-                    "heading": 2.9374602017194547,
-                    "angularVelocity": 1.5781282274310823e-7,
-                    "velocityX": 3.63405332200427,
-                    "velocityY": -1.050854592677291,
-                    "timestamp": 0.9614528316601544
-                },
-                {
-                    "x": 5.439847729430944,
-                    "y": 3.4534432595724156,
-                    "heading": 2.937460215513054,
-                    "angularVelocity": 1.5781282350457181e-7,
-                    "velocityX": 3.63405332200427,
-                    "velocityY": -1.050854592677291,
-                    "timestamp": 1.0488576345383502
-                },
-                {
-                    "x": 5.757481443689581,
-                    "y": 3.3615935210458106,
-                    "heading": 2.9374602293066525,
-                    "angularVelocity": 1.5781282308668676e-7,
-                    "velocityX": 3.6340533220042697,
-                    "velocityY": -1.0508545926772905,
-                    "timestamp": 1.136262437416546
-                },
-                {
-                    "x": 6.075115157948217,
-                    "y": 3.269743782519205,
-                    "heading": 2.9374602431002512,
-                    "angularVelocity": 1.578128234236369e-7,
-                    "velocityX": 3.63405332200427,
-                    "velocityY": -1.050854592677291,
-                    "timestamp": 1.2236672402947417
-                },
-                {
-                    "x": 6.3927488722066945,
-                    "y": 3.1778940439920533,
-                    "heading": 2.93746025689385,
-                    "angularVelocity": 1.5781282293850814e-7,
-                    "velocityX": 3.6340533220024622,
-                    "velocityY": -1.0508545926835444,
-                    "timestamp": 1.3110720431729375
-                },
-                {
-                    "x": 6.710382552956458,
-                    "y": 3.0860441895855293,
-                    "heading": 2.9374602706874486,
-                    "angularVelocity": 1.5781282618344319e-7,
-                    "velocityX": 3.634052938628626,
-                    "velocityY": -1.0508559184616288,
-                    "timestamp": 1.3984768460511332
-                },
-                {
-                    "x": 7.02127514506585,
-                    "y": 2.9734682388937714,
-                    "heading": 2.9374602922500688,
-                    "angularVelocity": 2.466983432002081e-7,
-                    "velocityX": 3.5569280162171513,
-                    "velocityY": -1.2879835773857788,
-                    "timestamp": 1.485881648929329
-                },
-                {
-                    "x": 7.2761933316047935,
-                    "y": 2.8394289044438907,
-                    "heading": 3.0149898604295666,
-                    "angularVelocity": 0.8870172533600954,
-                    "velocityX": 2.9165237852453965,
-                    "velocityY": -1.533546556207826,
-                    "timestamp": 1.5732864518075247
-                },
-                {
-                    "x": 7.469158921958533,
-                    "y": 2.7195716127256344,
-                    "heading": 3.0771718399112147,
-                    "angularVelocity": 0.711425201293604,
-                    "velocityX": 2.207722962577334,
-                    "velocityY": -1.3712895375473233,
-                    "timestamp": 1.6606912546857204
-                },
-                {
-                    "x": 7.6006701652701585,
-                    "y": 2.6155462997255583,
-                    "heading": 3.1196274586402084,
-                    "angularVelocity": 0.48573553547347,
-                    "velocityX": 1.5046226177627207,
-                    "velocityY": -1.1901555701125741,
-                    "timestamp": 1.7480960575639162
-                },
-                {
-                    "x": 7.670917138983036,
-                    "y": 2.527925545784097,
-                    "heading": 3.140832038047466,
-                    "angularVelocity": 0.24260199335736132,
-                    "velocityX": 0.8036969525664591,
-                    "velocityY": -1.0024707001921451,
-                    "timestamp": 1.835500860442112
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
                 {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": -0.009519363010585313,
-                    "velocityX": 0.10391718438655147,
-                    "velocityY": -0.8114605084452422,
-                    "timestamp": 1.9229056633203077
-                },
-                {
-                    "x": 7.63404119577111,
-                    "y": 2.4050502975891916,
-                    "heading": 3.1190998231163127,
-                    "angularVelocity": -0.25309519096022687,
-                    "velocityX": -0.5565480329352274,
-                    "velocityY": -0.6290961040742543,
-                    "timestamp": 2.005483988117367
-                },
-                {
-                    "x": 7.533566509755774,
-                    "y": 2.3683786594075307,
-                    "heading": 3.079403507657828,
-                    "angularVelocity": -0.4807110771021385,
-                    "velocityX": -1.216719838556405,
-                    "velocityY": -0.4440830965242228,
-                    "timestamp": 2.0880623129144262
-                },
-                {
-                    "x": 7.378621026771135,
-                    "y": 2.3473334555907552,
-                    "heading": 3.0230319998393305,
-                    "angularVelocity": -0.6826429084997041,
-                    "velocityX": -1.876345679879389,
-                    "velocityY": -0.2548514258250533,
-                    "timestamp": 2.1706406377114855
-                },
-                {
-                    "x": 7.169309740069539,
-                    "y": 2.342556806639995,
-                    "heading": 2.953940328458666,
-                    "angularVelocity": -0.8366804673073884,
-                    "velocityX": -2.5347000828121677,
-                    "velocityY": -0.05784385869415158,
-                    "timestamp": 2.253218962508545
-                },
-                {
-                    "x": 6.906019202766067,
-                    "y": 2.3556284163077987,
-                    "heading": 2.881970584756294,
-                    "angularVelocity": -0.8715331036230265,
-                    "velocityX": -3.188373437588175,
-                    "velocityY": 0.15829347107643843,
-                    "timestamp": 2.335797287305604
-                },
-                {
-                    "x": 6.596097301196444,
-                    "y": 2.394810276985141,
-                    "heading": 2.8819705158229665,
-                    "angularVelocity": -8.347629724920641e-7,
-                    "velocityX": -3.753065981070365,
-                    "velocityY": 0.47448117618799124,
-                    "timestamp": 2.4183756121026634
-                },
-                {
-                    "x": 6.2936576837370914,
-                    "y": 2.4730221394670577,
-                    "heading": 2.881970499784504,
-                    "angularVelocity": -1.942212104248679e-7,
-                    "velocityX": -3.6624576509951607,
-                    "velocityY": 0.9471233846669401,
-                    "timestamp": 2.5009539368997227
-                },
-                {
-                    "x": 5.991218190130788,
-                    "y": 2.5512344808782137,
-                    "heading": 2.8819704837460454,
-                    "angularVelocity": -1.9422117139312037e-7,
-                    "velocityX": -3.662456151169988,
-                    "velocityY": 0.9471291843636568,
-                    "timestamp": 2.583532261696782
-                },
-                {
-                    "x": 5.688778696525171,
-                    "y": 2.6294468222920195,
-                    "heading": 2.8819704677075864,
-                    "angularVelocity": -1.9422117079756682e-7,
-                    "velocityX": -3.662456151161689,
-                    "velocityY": 0.9471291843957483,
-                    "timestamp": 2.6661105864938413
-                },
-                {
-                    "x": 5.38633920291957,
-                    "y": 2.7076591637058867,
-                    "heading": 2.881970451669127,
-                    "angularVelocity": -1.9422117106487922e-7,
-                    "velocityX": -3.6624561511614977,
-                    "velocityY": 0.947129184396488,
-                    "timestamp": 2.7486889112909005
-                },
-                {
-                    "x": 5.0838997121673595,
-                    "y": 2.7858715161533545,
-                    "heading": 2.881970435630565,
-                    "angularVelocity": -1.9422242240409168e-7,
-                    "velocityX": -3.662456116607741,
-                    "velocityY": 0.9471293180102566,
-                    "timestamp": 2.83126723608796
-                },
-                {
-                    "x": 4.810061938834441,
-                    "y": 2.8575158595499386,
-                    "heading": 2.7916066716165844,
-                    "angularVelocity": -1.094279452096593,
-                    "velocityX": -3.3160974626924187,
-                    "velocityY": 0.8675925985741851,
-                    "timestamp": 2.913845560885019
-                },
-                {
-                    "x": 4.591150287853899,
-                    "y": 2.914595495450747,
-                    "heading": 2.7083456685941125,
-                    "angularVelocity": -1.0082670389244417,
-                    "velocityX": -2.650957760629438,
-                    "velocityY": 0.6912181379446108,
-                    "timestamp": 2.9964238856820784
-                },
-                {
-                    "x": 4.427043317855732,
-                    "y": 2.957326813953426,
-                    "heading": 2.64244137634817,
-                    "angularVelocity": -0.7980822135579249,
-                    "velocityX": -1.98728867897803,
-                    "velocityY": 0.5174640997827658,
-                    "timestamp": 3.0790022104791377
-                },
-                {
-                    "x": 4.317672771154393,
-                    "y": 2.985782903772343,
-                    "heading": 2.597132660112387,
-                    "angularVelocity": -0.5486756524442865,
-                    "velocityX": -1.3244461784629789,
-                    "velocityY": 0.34459514514068673,
-                    "timestamp": 3.161580535276197
-                },
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "S4 To C2": {
+            "waypoints": [
                 {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": -0.28012992718412955,
-                    "velocityX": -0.6620716911944379,
-                    "velocityY": 0.1721649871512405,
-                    "timestamp": 3.2441588600732563
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": 0,
-                    "velocityX": -8.552238423258885e-34,
-                    "velocityY": -9.381987056731701e-33,
-                    "timestamp": 3.3267371848703156
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 }
             ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -12876,26 +3545,8 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W3 To C4 To S4": {
+        "S4 To C3": {
             "waypoints": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 22
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
                 {
                     "x": 5.176,
                     "y": 1.62,
@@ -12904,352 +3555,18 @@
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.360128743568809,
-                    "y": 4.349177185639382,
-                    "heading": 2.7782206485451093,
-                    "angularVelocity": 0.3283351874090942,
-                    "velocityX": 0.6995722389859838,
-                    "velocityY": -0.1259188206007136,
-                    "timestamp": 0.0859507284851176
-                },
-                {
-                    "x": 2.4804864804971807,
-                    "y": 4.327553542642656,
-                    "heading": 2.831856423124361,
-                    "angularVelocity": 0.6240293191760308,
-                    "velocityX": 1.4003108414515997,
-                    "velocityY": -0.2515818466910346,
-                    "timestamp": 0.1719014569702352
-                },
-                {
-                    "x": 2.6612218913219765,
-                    "y": 4.295165149158174,
-                    "heading": 2.9060715845563863,
-                    "angularVelocity": 0.863461691832829,
-                    "velocityX": 2.102779278433835,
-                    "velocityY": -0.3768251189411322,
-                    "timestamp": 0.2578521854553528
-                },
-                {
-                    "x": 2.902552042800783,
-                    "y": 4.252097385240196,
-                    "heading": 2.990739532355289,
-                    "angularVelocity": 0.9850753948357858,
-                    "velocityX": 2.8077731943900375,
-                    "velocityY": -0.5010750307419894,
-                    "timestamp": 0.3438029139404704
-                },
-                {
-                    "x": 3.2042136582065344,
-                    "y": 4.198837394002331,
-                    "heading": 3.04987623940461,
-                    "angularVelocity": 0.6880303179693488,
-                    "velocityX": 3.5097039981224545,
-                    "velocityY": -0.6196572405676055,
-                    "timestamp": 0.429753642425588
-                },
-                {
-                    "x": 3.524112842751923,
-                    "y": 4.1406587673803905,
-                    "heading": 3.0498762636795202,
-                    "angularVelocity": 2.824282081012085e-7,
-                    "velocityX": 3.7218903223232087,
-                    "velocityY": -0.6768834615754469,
-                    "timestamp": 0.5157043709107056
-                },
-                {
-                    "x": 3.844012025527027,
-                    "y": 4.082480131024319,
-                    "heading": 3.0498762879543886,
-                    "angularVelocity": 2.824277225814376e-7,
-                    "velocityX": 3.7218903017266984,
-                    "velocityY": -0.6768835748279012,
-                    "timestamp": 0.6016550993958233
-                },
-                {
-                    "x": 4.163911208302122,
-                    "y": 4.0243014946682,
-                    "heading": 3.049876312229257,
-                    "angularVelocity": 2.824277213255298e-7,
-                    "velocityX": 3.721890301726596,
-                    "velocityY": -0.6768835748284626,
-                    "timestamp": 0.6876058278809409
-                },
-                {
-                    "x": 4.483810391077216,
-                    "y": 3.9661228583120804,
-                    "heading": 3.0498763365041253,
-                    "angularVelocity": 2.8242772165714766e-7,
-                    "velocityX": 3.7218903017265963,
-                    "velocityY": -0.6768835748284627,
-                    "timestamp": 0.7735565563660586
-                },
-                {
-                    "x": 4.80370957385231,
-                    "y": 3.9079442219559613,
-                    "heading": 3.0498763607789936,
-                    "angularVelocity": 2.8242772140625194e-7,
-                    "velocityX": 3.7218903017265963,
-                    "velocityY": -0.6768835748284627,
-                    "timestamp": 0.8595072848511762
-                },
-                {
-                    "x": 5.123608756627404,
-                    "y": 3.8497655855998416,
-                    "heading": 3.049876385053862,
-                    "angularVelocity": 2.824277226048516e-7,
-                    "velocityX": 3.7218903017265963,
-                    "velocityY": -0.6768835748284628,
-                    "timestamp": 0.9454580133362939
-                },
-                {
-                    "x": 5.443507939402499,
-                    "y": 3.7915869492437224,
-                    "heading": 3.0498764093287303,
-                    "angularVelocity": 2.8242772163119933e-7,
-                    "velocityX": 3.7218903017265963,
-                    "velocityY": -0.6768835748284626,
-                    "timestamp": 1.0314087418214115
-                },
-                {
-                    "x": 5.763407122177593,
-                    "y": 3.733408312887603,
-                    "heading": 3.0498764336035986,
-                    "angularVelocity": 2.824277219712686e-7,
-                    "velocityX": 3.7218903017265963,
-                    "velocityY": -0.6768835748284628,
-                    "timestamp": 1.117359470306529
-                },
-                {
-                    "x": 6.083306304952576,
-                    "y": 3.675229676530871,
-                    "heading": 3.049876457878467,
-                    "angularVelocity": 2.8242772161077964e-7,
-                    "velocityX": 3.7218903017253,
-                    "velocityY": -0.6768835748355908,
-                    "timestamp": 1.2033101987916466
-                },
-                {
-                    "x": 6.403205465262658,
-                    "y": 3.6170509166494913,
-                    "heading": 3.0498764821533357,
-                    "angularVelocity": 2.8242772554820356e-7,
-                    "velocityX": 3.721890040355771,
-                    "velocityY": -0.6768850119920853,
-                    "timestamp": 1.2892609272767641
-                },
-                {
-                    "x": 6.718573965768454,
-                    "y": 3.5379121175247272,
-                    "heading": 3.0498765068050417,
-                    "angularVelocity": 2.868120651983924e-7,
-                    "velocityX": 3.6691777494404794,
-                    "velocityY": -0.9207461125646453,
-                    "timestamp": 1.3752116557618816
-                },
-                {
-                    "x": 7.013358117396929,
-                    "y": 3.4007168540694233,
-                    "heading": 3.0498765454511387,
-                    "angularVelocity": 4.49630829762963e-7,
-                    "velocityX": 3.4296876457482686,
-                    "velocityY": -1.5962082680784608,
-                    "timestamp": 1.4611623842469992
-                },
-                {
-                    "x": 7.268807462845475,
-                    "y": 3.2166766255285517,
-                    "heading": 3.0757384072562206,
-                    "angularVelocity": 0.30089171157615124,
-                    "velocityX": 2.9720439832312224,
-                    "velocityY": -2.1412294204433464,
-                    "timestamp": 1.5471131127321167
-                },
-                {
-                    "x": 7.46355872560419,
-                    "y": 3.0231641785099956,
-                    "heading": 3.117732513124605,
-                    "angularVelocity": 0.48858347809886377,
-                    "velocityX": 2.2658477268453225,
-                    "velocityY": -2.2514346350428145,
-                    "timestamp": 1.6330638412172342
-                },
-                {
-                    "x": 7.596796563191941,
-                    "y": 2.830012972060279,
-                    "heading": 3.1482585374873016,
-                    "angularVelocity": 0.3551572499816586,
-                    "velocityX": 1.5501653090794745,
-                    "velocityY": -2.2472317553790035,
-                    "timestamp": 1.7190145697023518
-                },
-                {
-                    "x": 7.66884686477411,
-                    "y": 2.640707793733853,
-                    "heading": 3.1573927530357144,
-                    "angularVelocity": 0.10627269494275392,
-                    "velocityX": 0.8382744725035164,
-                    "velocityY": -2.202484861535554,
-                    "timestamp": 1.8049652981874693
                 },
                 {
                     "x": 7.68,
-                    "y": 2.457,
+                    "y": 4.127,
                     "heading": 3.14,
-                    "angularVelocity": -0.2023572498136116,
-                    "velocityX": 0.12976196272503107,
-                    "velocityY": -2.1373616835098828,
-                    "timestamp": 1.8909160266725868
-                },
-                {
-                    "x": 7.6422705837740335,
-                    "y": 2.299493905398916,
-                    "heading": 3.1013004562411446,
-                    "angularVelocity": -0.5083194372878264,
-                    "velocityX": -0.49557678882949796,
-                    "velocityY": -2.068846337723125,
-                    "timestamp": 1.9670483578801472
-                },
-                {
-                    "x": 7.556907286255345,
-                    "y": 2.147926635640613,
-                    "heading": 3.0424682290459026,
-                    "angularVelocity": -0.7727627180470553,
-                    "velocityX": -1.121248964332173,
-                    "velocityY": -1.9908397306931678,
-                    "timestamp": 2.043180689087708
-                },
-                {
-                    "x": 7.423923039902345,
-                    "y": 2.003393138237165,
-                    "heading": 2.9682494555138628,
-                    "angularVelocity": -0.974865373945302,
-                    "velocityX": -1.7467512716828084,
-                    "velocityY": -1.8984509617786702,
-                    "timestamp": 2.119313020295268
-                },
-                {
-                    "x": 7.2434803504733996,
-                    "y": 1.8677477086842909,
-                    "heading": 2.8865548866835007,
-                    "angularVelocity": -1.07306012484416,
-                    "velocityX": -2.3701190619920354,
-                    "velocityY": -1.7817059769660042,
-                    "timestamp": 2.1954453515028285
-                },
-                {
-                    "x": 7.01645692191088,
-                    "y": 1.7447501516568382,
-                    "heading": 2.813573416858664,
-                    "angularVelocity": -0.9586133600147891,
-                    "velocityX": -2.9819581899254453,
-                    "velocityY": -1.6155758673946832,
-                    "timestamp": 2.271577682710389
-                },
-                {
-                    "x": 6.750046151807222,
-                    "y": 1.6436496548008426,
-                    "heading": 2.8060686181878967,
-                    "angularVelocity": -0.09857571089344144,
-                    "velocityX": -3.499311867613085,
-                    "velocityY": -1.327957455819293,
-                    "timestamp": 2.347710013917949
-                },
-                {
-                    "x": 6.467461314020777,
-                    "y": 1.5880425214231262,
-                    "heading": 2.8060685831707377,
-                    "angularVelocity": -4.5995122509551036e-7,
-                    "velocityX": -3.711758635316633,
-                    "velocityY": -0.7304010332495747,
-                    "timestamp": 2.4238423451255096
-                },
-                {
-                    "x": 6.17955680980973,
-                    "y": 1.580470049320341,
-                    "heading": 2.806068554276105,
-                    "angularVelocity": -3.7953169924465946e-7,
-                    "velocityX": -3.7816325816443177,
-                    "velocityY": -0.09946460304917487,
-                    "timestamp": 2.49997467633307
-                },
-                {
-                    "x": 5.8949085342429335,
-                    "y": 1.5924545384965154,
-                    "heading": 2.7975026231592013,
-                    "angularVelocity": -0.11251371107452125,
-                    "velocityX": -3.738861940149357,
-                    "velocityY": 0.1574165533364508,
-                    "timestamp": 2.5761070075406303
-                },
-                {
-                    "x": 5.65482422655778,
-                    "y": 1.6016450496652885,
-                    "heading": 2.72071509082511,
-                    "angularVelocity": -1.008606082543986,
-                    "velocityX": -3.153513151076552,
-                    "velocityY": 0.12071758506519875,
-                    "timestamp": 2.6522393387481906
-                },
-                {
-                    "x": 5.463026554407694,
-                    "y": 1.608988353775347,
-                    "heading": 2.6410025465355518,
-                    "angularVelocity": -1.0470261848719398,
-                    "velocityX": -2.5192670329138944,
-                    "velocityY": 0.09645447595750602,
-                    "timestamp": 2.728371669955751
-                },
-                {
-                    "x": 5.319399038502622,
-                    "y": 1.614493352033255,
-                    "heading": 2.573619865722977,
-                    "angularVelocity": -0.8850731317940332,
-                    "velocityX": -1.8865508730252805,
-                    "velocityY": 0.07230828441193854,
-                    "timestamp": 2.8045040011633113
-                },
-                {
-                    "x": 5.223767247762021,
-                    "y": 1.6181637826708264,
-                    "heading": 2.525354405173392,
-                    "angularVelocity": -0.6339679842201956,
-                    "velocityX": -1.2561258695714954,
-                    "velocityY": 0.04821119463109347,
-                    "timestamp": 2.8806363323708717
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": -0.33303072126212585,
-                    "velocityX": -0.6274239472819125,
-                    "velocityY": 0.02411875874607297,
-                    "timestamp": 2.956768663578432
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": 0,
-                    "velocityX": -1.0690007247060788e-32,
-                    "velocityY": 0,
-                    "timestamp": 3.0329009947859924
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 }
             ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -13269,434 +3586,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W3 To C5 To S3": {
+        "S4 To C4": {
             "waypoints": [
                 {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 24
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 7.68,
-                    "y": 0.787,
+                    "y": 2.457,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 19
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.35465792702043,
-                    "y": 4.32401597789768,
-                    "heading": 2.7661167341638526,
-                    "angularVelocity": 0.1817092285961366,
-                    "velocityX": 0.6162445601306747,
-                    "velocityY": -0.4057043338633825,
-                    "timestamp": 0.08869518784691614
-                },
-                {
-                    "x": 2.464008141320373,
-                    "y": 4.252030892508781,
-                    "heading": 2.796228115996352,
-                    "angularVelocity": 0.3394928469453156,
-                    "velocityX": 1.2328765173672827,
-                    "velocityY": -0.8116007997315744,
-                    "timestamp": 0.1773903756938323
-                },
-                {
-                    "x": 2.6280987448252096,
-                    "y": 4.144022899680214,
-                    "heading": 2.836753181719006,
-                    "angularVelocity": 0.4569026427070467,
-                    "velocityX": 1.8500508030723093,
-                    "velocityY": -1.2177435490072333,
-                    "timestamp": 0.2660855635407484
-                },
-                {
-                    "x": 2.8469891593833587,
-                    "y": 3.9999743075055525,
-                    "heading": 2.8803143946879453,
-                    "angularVelocity": 0.49113389380407046,
-                    "velocityX": 2.467895044497164,
-                    "velocityY": -1.624085766899582,
-                    "timestamp": 0.3547807513876646
-                },
-                {
-                    "x": 3.12048957146799,
-                    "y": 3.8200884895179508,
-                    "heading": 2.9023929043889884,
-                    "angularVelocity": 0.248925677221065,
-                    "velocityX": 3.083599220249474,
-                    "velocityY": -2.028135035894805,
-                    "timestamp": 0.44347593923458073
-                },
-                {
-                    "x": 3.4006775749010125,
-                    "y": 3.6354966761760505,
-                    "heading": 2.9023929178552135,
-                    "angularVelocity": 1.5182588146629277e-7,
-                    "velocityX": 3.1589989291934804,
-                    "velocityY": -2.081193104416185,
-                    "timestamp": 0.5321711270814968
-                },
-                {
-                    "x": 3.680865577691327,
-                    "y": 3.450904861858589,
-                    "heading": 2.9023929313214296,
-                    "angularVelocity": 1.518257825170986e-7,
-                    "velocityX": 3.1589989219472225,
-                    "velocityY": -2.081193115415224,
-                    "timestamp": 0.6208663149284129
-                },
-                {
-                    "x": 3.961053580481639,
-                    "y": 3.2663130475411224,
-                    "heading": 2.902392944787646,
-                    "angularVelocity": 1.5182578291151429e-7,
-                    "velocityX": 3.1589989219471897,
-                    "velocityY": -2.081193115415273,
-                    "timestamp": 0.709561502775329
-                },
-                {
-                    "x": 4.24124158327195,
-                    "y": 3.0817212332236563,
-                    "heading": 2.9023929582538623,
-                    "angularVelocity": 1.518257833819606e-7,
-                    "velocityX": 3.1589989219471897,
-                    "velocityY": -2.0811931154152736,
-                    "timestamp": 0.7982566906222451
-                },
-                {
-                    "x": 4.521429586062262,
-                    "y": 2.89712941890619,
-                    "heading": 2.9023929717200785,
-                    "angularVelocity": 1.5182578356012397e-7,
-                    "velocityX": 3.1589989219471892,
-                    "velocityY": -2.081193115415273,
-                    "timestamp": 0.8869518784691612
-                },
-                {
-                    "x": 4.801617588852573,
-                    "y": 2.7125376045887237,
-                    "heading": 2.902392985186295,
-                    "angularVelocity": 1.518257838228889e-7,
-                    "velocityX": 3.15899892194719,
-                    "velocityY": -2.0811931154152736,
-                    "timestamp": 0.9756470663160773
-                },
-                {
-                    "x": 5.081805591642885,
-                    "y": 2.5279457902712577,
-                    "heading": 2.902392998652511,
-                    "angularVelocity": 1.5182578286793036e-7,
-                    "velocityX": 3.1589989219471897,
-                    "velocityY": -2.081193115415273,
-                    "timestamp": 1.0643422541629934
-                },
-                {
-                    "x": 5.361993594433196,
-                    "y": 2.343353975953791,
-                    "heading": 2.9023930121187274,
-                    "angularVelocity": 1.5182578369194972e-7,
-                    "velocityX": 3.1589989219471897,
-                    "velocityY": -2.0811931154152736,
-                    "timestamp": 1.1530374420099097
-                },
-                {
-                    "x": 5.642181597223507,
-                    "y": 2.158762161636325,
-                    "heading": 2.902393025584944,
-                    "angularVelocity": 1.5182578337777967e-7,
-                    "velocityX": 3.1589989219471897,
-                    "velocityY": -2.081193115415273,
-                    "timestamp": 1.2417326298568259
-                },
-                {
-                    "x": 5.922369600013819,
-                    "y": 1.9741703473188588,
-                    "heading": 2.90239303905116,
-                    "angularVelocity": 1.5182578259222556e-7,
-                    "velocityX": 3.15899892194719,
-                    "velocityY": -2.081193115415273,
-                    "timestamp": 1.330427817703742
-                },
-                {
-                    "x": 6.202557602804131,
-                    "y": 1.7895785330013922,
-                    "heading": 2.9023930525173767,
-                    "angularVelocity": 1.5182578252273246e-7,
-                    "velocityX": 3.1589989219471892,
-                    "velocityY": -2.0811931154152736,
-                    "timestamp": 1.4191230055506583
-                },
-                {
-                    "x": 6.482745605594443,
-                    "y": 1.604986718683926,
-                    "heading": 2.902393065983593,
-                    "angularVelocity": 1.5182578277552693e-7,
-                    "velocityX": 3.1589989219471897,
-                    "velocityY": -2.081193115415273,
-                    "timestamp": 1.5078181933975745
-                },
-                {
-                    "x": 6.762933608384558,
-                    "y": 1.4203949043661621,
-                    "heading": 2.902393079449809,
-                    "angularVelocity": 1.5182578233984683e-7,
-                    "velocityX": 3.1589989219449803,
-                    "velocityY": -2.081193115418628,
-                    "timestamp": 1.5965133812444907
-                },
-                {
-                    "x": 7.043121567747365,
-                    "y": 1.2358030241312514,
-                    "heading": 2.902393092916261,
-                    "angularVelocity": 1.5182844252968712e-7,
-                    "velocityX": 3.158998432320794,
-                    "velocityY": -2.08119385860604,
-                    "timestamp": 1.685208569091407
-                },
-                {
-                    "x": 7.279459556862412,
-                    "y": 1.0742756485961233,
-                    "heading": 2.998760727595811,
-                    "angularVelocity": 1.0865035298857004,
-                    "velocityX": 2.6646089247023874,
-                    "velocityY": -1.8211515129086453,
-                    "timestamp": 1.7739037569383231
-                },
-                {
-                    "x": 7.461316655805292,
-                    "y": 0.9486305467963247,
-                    "heading": 3.0706677283735453,
-                    "angularVelocity": 0.8107204294086697,
-                    "velocityX": 2.0503603787023565,
-                    "velocityY": -1.416594348011941,
-                    "timestamp": 1.8625989447852394
-                },
-                {
-                    "x": 7.588692434846267,
-                    "y": 0.8588692195479983,
-                    "heading": 3.1181671121508234,
-                    "angularVelocity": 0.5355350716350028,
-                    "velocityX": 1.4361069876848378,
-                    "velocityY": -1.0120202620603307,
-                    "timestamp": 1.9512941326321556
-                },
-                {
-                    "x": 7.661586847053616,
-                    "y": 0.8049924095541465,
-                    "heading": 3.141279756163227,
-                    "angularVelocity": 0.26058509569081495,
-                    "velocityX": 0.8218530675323843,
-                    "velocityY": -0.6074378024526055,
-                    "timestamp": 2.0399893204790716
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 3.14,
-                    "angularVelocity": -0.01442869894402112,
-                    "velocityX": 0.20760036021529943,
-                    "velocityY": -0.20285665988103574,
-                    "timestamp": 2.1286845083259878
-                },
-                {
-                    "x": 7.650032668413058,
-                    "y": 0.8012856501840878,
-                    "heading": 3.117704293378248,
-                    "angularVelocity": -0.2703970639421689,
-                    "velocityX": -0.3634367195783461,
-                    "velocityY": 0.17325299134110608,
-                    "timestamp": 2.211139939966771
-                },
-                {
-                    "x": 7.572958138860234,
-                    "y": 0.846621328160437,
-                    "heading": 3.075672962840878,
-                    "angularVelocity": -0.509746049483795,
-                    "velocityX": -0.9347416903788446,
-                    "velocityY": 0.5498203947782849,
-                    "timestamp": 2.293595371607554
-                },
-                {
-                    "x": 7.448745482136049,
-                    "y": 0.9230647936190245,
-                    "heading": 3.016123238078663,
-                    "angularVelocity": -0.7222049970175785,
-                    "velocityX": -1.5064217632783534,
-                    "velocityY": 0.927088294093387,
-                    "timestamp": 2.376050803248337
-                },
-                {
-                    "x": 7.277351379823213,
-                    "y": 1.0307130669975626,
-                    "heading": 2.9432415686566307,
-                    "angularVelocity": -0.8838916730136303,
-                    "velocityX": -2.0786271917114503,
-                    "velocityY": 1.305532834362055,
-                    "timestamp": 2.45850623488912
-                },
-                {
-                    "x": 7.058753992164297,
-                    "y": 1.1697461046151478,
-                    "heading": 2.8675628117721894,
-                    "angularVelocity": -0.9178140891207157,
-                    "velocityX": -2.6510974875643654,
-                    "velocityY": 1.6861598423652984,
-                    "timestamp": 2.540961666529903
-                },
-                {
-                    "x": 6.796121093379507,
-                    "y": 1.3380331209180303,
-                    "heading": 2.867562732679127,
-                    "angularVelocity": -9.592219802669244e-7,
-                    "velocityX": -3.1851497658632115,
-                    "velocityY": 2.0409451864375003,
-                    "timestamp": 2.623417098170686
-                },
-                {
-                    "x": 6.5354030679592325,
-                    "y": 1.509271805879804,
-                    "heading": 2.8675627155127748,
-                    "angularVelocity": -2.0818946417191608e-7,
-                    "velocityX": -3.161926633967453,
-                    "velocityY": 2.076742326785391,
-                    "timestamp": 2.705872529811469
-                },
-                {
-                    "x": 6.274685053353285,
-                    "y": 1.680510507306839,
-                    "heading": 2.867562698346423,
-                    "angularVelocity": -2.0818946436511562e-7,
-                    "velocityX": -3.161926502813852,
-                    "velocityY": 2.0767425264721937,
-                    "timestamp": 2.7883279614522523
-                },
-                {
-                    "x": 6.013967038747397,
-                    "y": 1.851749208733966,
-                    "heading": 2.8675626811800705,
-                    "angularVelocity": -2.0818946408004615e-7,
-                    "velocityX": -3.1619265028131216,
-                    "velocityY": 2.076742526473307,
-                    "timestamp": 2.8707833930930353
-                },
-                {
-                    "x": 5.75324902414151,
-                    "y": 2.022987910161093,
-                    "heading": 2.8675626640137186,
-                    "angularVelocity": -2.081894642016249e-7,
-                    "velocityX": -3.1619265028131216,
-                    "velocityY": 2.076742526473306,
-                    "timestamp": 2.9532388247338184
-                },
-                {
-                    "x": 5.492531009535623,
-                    "y": 2.194226611588219,
-                    "heading": 2.8675626468473663,
-                    "angularVelocity": -2.0818946425512402e-7,
-                    "velocityX": -3.1619265028131216,
-                    "velocityY": 2.076742526473306,
-                    "timestamp": 3.0356942563746014
-                },
-                {
-                    "x": 5.231812994929702,
-                    "y": 2.3654653130152945,
-                    "heading": 2.8675626296810144,
-                    "angularVelocity": -2.0818946468474427e-7,
-                    "velocityX": -3.1619265028135324,
-                    "velocityY": 2.0767425264726813,
-                    "timestamp": 3.1181496880153845
-                },
-                {
-                    "x": 4.971094974250145,
-                    "y": 2.536704005194925,
-                    "heading": 2.8675626125145612,
-                    "angularVelocity": -2.0819068604958381e-7,
-                    "velocityX": -3.16192657647314,
-                    "velocityY": 2.076742414321854,
-                    "timestamp": 3.2006051196561676
-                },
-                {
-                    "x": 4.734733232154558,
-                    "y": 2.6911196483021613,
-                    "heading": 2.78339324120731,
-                    "angularVelocity": -1.020786255463861,
-                    "velocityX": -2.8665393824544814,
-                    "velocityY": 1.8727164485651855,
-                    "timestamp": 3.2830605512969506
-                },
-                {
-                    "x": 4.545913318554367,
-                    "y": 2.8146834617063132,
-                    "heading": 2.703884624892798,
-                    "angularVelocity": -0.9642617197238292,
-                    "velocityX": -2.289963315246283,
-                    "velocityY": 1.4985527447416418,
-                    "timestamp": 3.3655159829377337
-                },
-                {
-                    "x": 4.404411416841804,
-                    "y": 2.9073475116185867,
-                    "heading": 2.6403705813576916,
-                    "angularVelocity": -0.7702833187728019,
-                    "velocityX": -1.7161016429944431,
-                    "velocityY": 1.1238077112489557,
-                    "timestamp": 3.4479714145785167
-                },
-                {
-                    "x": 4.3101255761487,
-                    "y": 2.9691175811007473,
-                    "heading": 2.5964788856355536,
-                    "angularVelocity": -0.5323081190497229,
-                    "velocityX": -1.1434764067922174,
-                    "velocityY": 0.7491328133634895,
-                    "timestamp": 3.5304268462193
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": -0.27261861575697227,
-                    "velocityX": -0.5715278570610401,
-                    "velocityY": 0.37453468236989096,
-                    "timestamp": 3.612882277860083
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": 0,
-                    "velocityX": -3.9375732354803004e-35,
-                    "velocityY": 0,
-                    "timestamp": 3.695337709500866
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -13716,26 +3627,8 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "W3 To C5 To S4": {
+        "S4 To C5": {
             "waypoints": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 24
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
                 {
                     "x": 5.176,
                     "y": 1.62,
@@ -13744,370 +3637,18 @@
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.3565686386495837,
-                    "y": 4.327692083019332,
-                    "heading": 2.7686204392526563,
-                    "angularVelocity": 0.21032675786146612,
-                    "velocityX": 0.6389698009998809,
-                    "velocityY": -0.36493335842385927,
-                    "timestamp": 0.08853100500377892
-                },
-                {
-                    "x": 2.4697534077972008,
-                    "y": 4.26305693150059,
-                    "heading": 2.803412029343583,
-                    "angularVelocity": 0.39298763285756855,
-                    "velocityX": 1.2784760451187218,
-                    "velocityY": -0.7300849178881736,
-                    "timestamp": 0.17706201000755784
-                },
-                {
-                    "x": 2.639620733022083,
-                    "y": 4.166070131606458,
-                    "heading": 2.850239105466856,
-                    "angularVelocity": 0.5289341979263338,
-                    "velocityX": 1.9187325978919179,
-                    "velocityY": -1.095512243309488,
-                    "timestamp": 0.26559301501133675
-                },
-                {
-                    "x": 2.8662525015467275,
-                    "y": 4.036714523350174,
-                    "heading": 2.9005551954880358,
-                    "angularVelocity": 0.5683442768897115,
-                    "velocityX": 2.559914105967419,
-                    "velocityY": -1.4611333989799566,
-                    "timestamp": 0.35412402001511567
-                },
-                {
-                    "x": 3.1493645211146295,
-                    "y": 3.8752655198055206,
-                    "heading": 2.9253298956108997,
-                    "angularVelocity": 0.2798420747827705,
-                    "velocityX": 3.197885526723865,
-                    "velocityY": -1.8236436323948029,
-                    "timestamp": 0.4426550250188946
-                },
-                {
-                    "x": 3.4401071732873256,
-                    "y": 3.709037480706115,
-                    "heading": 2.9253299112862043,
-                    "angularVelocity": 1.770600526020634e-7,
-                    "velocityX": 3.28407716777061,
-                    "velocityY": -1.877625122320825,
-                    "timestamp": 0.5311860300226735
-                },
-                {
-                    "x": 3.730849824609712,
-                    "y": 3.5428094401194565,
-                    "heading": 2.925329926961498,
-                    "angularVelocity": 1.7705993300653618e-7,
-                    "velocityX": 3.2840771581659602,
-                    "velocityY": -1.87762513912005,
-                    "timestamp": 0.6197170350264525
-                },
-                {
-                    "x": 4.021592475932095,
-                    "y": 3.376581399532791,
-                    "heading": 2.9253299426367922,
-                    "angularVelocity": 1.7705993370243028e-7,
-                    "velocityX": 3.2840771581659167,
-                    "velocityY": -1.8776251391201264,
-                    "timestamp": 0.7082480400302313
-                },
-                {
-                    "x": 4.312335127254477,
-                    "y": 3.2103533589461257,
-                    "heading": 2.925329958312086,
-                    "angularVelocity": 1.7705993363835285e-7,
-                    "velocityX": 3.2840771581659163,
-                    "velocityY": -1.8776251391201266,
-                    "timestamp": 0.7967790450340102
-                },
-                {
-                    "x": 4.60307777857686,
-                    "y": 3.0441253183594608,
-                    "heading": 2.9253299739873797,
-                    "angularVelocity": 1.7705993334445259e-7,
-                    "velocityX": 3.2840771581659163,
-                    "velocityY": -1.8776251391201269,
-                    "timestamp": 0.8853100500377891
-                },
-                {
-                    "x": 4.893820429899242,
-                    "y": 2.877897277772796,
-                    "heading": 2.9253299896626737,
-                    "angularVelocity": 1.7705993259163212e-7,
-                    "velocityX": 3.2840771581659163,
-                    "velocityY": -1.877625139120127,
-                    "timestamp": 0.9738410550415679
-                },
-                {
-                    "x": 5.184563081221626,
-                    "y": 2.7116692371861304,
-                    "heading": 2.925330005337967,
-                    "angularVelocity": 1.7705993390699708e-7,
-                    "velocityX": 3.2840771581659163,
-                    "velocityY": -1.8776251391201266,
-                    "timestamp": 1.0623720600453468
-                },
-                {
-                    "x": 5.475305732544008,
-                    "y": 2.5454411965994654,
-                    "heading": 2.925330021013261,
-                    "angularVelocity": 1.770599334284566e-7,
-                    "velocityX": 3.2840771581659163,
-                    "velocityY": -1.8776251391201269,
-                    "timestamp": 1.1509030650491257
-                },
-                {
-                    "x": 5.766048383866392,
-                    "y": 2.3792131560128005,
-                    "heading": 2.925330036688555,
-                    "angularVelocity": 1.7705993332802181e-7,
-                    "velocityX": 3.2840771581659163,
-                    "velocityY": -1.8776251391201264,
-                    "timestamp": 1.2394340700529045
-                },
-                {
-                    "x": 6.056791035188774,
-                    "y": 2.212985115426135,
-                    "heading": 2.9253300523638495,
-                    "angularVelocity": 1.7705993348595005e-7,
-                    "velocityX": 3.2840771581659163,
-                    "velocityY": -1.8776251391201266,
-                    "timestamp": 1.3279650750566834
-                },
-                {
-                    "x": 6.347533686511157,
-                    "y": 2.04675707483947,
-                    "heading": 2.925330068039143,
-                    "angularVelocity": 1.7705993292383098e-7,
-                    "velocityX": 3.2840771581659163,
-                    "velocityY": -1.8776251391201269,
-                    "timestamp": 1.4164960800604622
-                },
-                {
-                    "x": 6.638276337833025,
-                    "y": 1.8805290342519057,
-                    "heading": 2.925330083714437,
-                    "angularVelocity": 1.7705993325721192e-7,
-                    "velocityX": 3.284077158160109,
-                    "velocityY": -1.877625139130284,
-                    "timestamp": 1.505027085064241
-                },
-                {
-                    "x": 6.929018876242265,
-                    "y": 1.7143007961736503,
-                    "heading": 2.9253300993897313,
-                    "angularVelocity": 1.770599420488751e-7,
-                    "velocityX": 3.2840758827580245,
-                    "velocityY": -1.8776273698819952,
-                    "timestamp": 1.59355809006802
-                },
-                {
-                    "x": 7.201522264704821,
-                    "y": 1.5196088586386673,
-                    "heading": 2.925330126495543,
-                    "angularVelocity": 3.061730945539565e-7,
-                    "velocityX": 3.078055969780553,
-                    "velocityY": -2.1991384546767203,
-                    "timestamp": 1.6820890950717988
-                },
-                {
-                    "x": 7.413524241952794,
-                    "y": 1.3146281632272387,
-                    "heading": 3.008225358482996,
-                    "angularVelocity": 0.9363412511121879,
-                    "velocityX": 2.394663623652733,
-                    "velocityY": -2.3153548906699357,
-                    "timestamp": 1.7706201000755777
-                },
-                {
-                    "x": 7.566471934487395,
-                    "y": 1.1372904255598042,
-                    "heading": 3.0754099127352705,
-                    "angularVelocity": 0.758881639821055,
-                    "velocityX": 1.727617262766506,
-                    "velocityY": -2.003114475655854,
-                    "timestamp": 1.8591511050793565
-                },
-                {
-                    "x": 7.66159309798317,
-                    "y": 0.9897915834005926,
-                    "heading": 3.120981260630991,
-                    "angularVelocity": 0.5147501476321013,
-                    "velocityX": 1.0744389888232937,
-                    "velocityY": -1.6660698944162209,
-                    "timestamp": 1.9476821100831354
-                },
-                {
-                    "x": 7.6993540657193265,
-                    "y": 0.8728983057701601,
-                    "heading": 3.1428690494072535,
-                    "angularVelocity": 0.2472330318097442,
-                    "velocityX": 0.4265281720741691,
-                    "velocityY": -1.3203654202890747,
-                    "timestamp": 2.0362131150869143
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
                     "heading": 3.14,
-                    "angularVelocity": -0.03240728383410146,
-                    "velocityX": -0.21861341931571387,
-                    "velocityY": -0.9702624043010712,
-                    "timestamp": 2.124744120090693
-                },
-                {
-                    "x": 7.618086573966619,
-                    "y": 0.7353968639365192,
-                    "heading": 3.1174668267835224,
-                    "angularVelocity": -0.28713837638984974,
-                    "velocityX": -0.7889577050333905,
-                    "velocityY": -0.6575745263915584,
-                    "timestamp": 2.203219082231727
-                },
-                {
-                    "x": 7.5114563039412285,
-                    "y": 0.7085217505748593,
-                    "heading": 3.0768465482235205,
-                    "angularVelocity": -0.5176208748841893,
-                    "velocityX": -1.3587807769024405,
-                    "velocityY": -0.34246736320004995,
-                    "timestamp": 2.281694044372761
-                },
-                {
-                    "x": 7.360182308394819,
-                    "y": 0.7066609452418267,
-                    "heading": 3.0210994010584384,
-                    "angularVelocity": -0.7103813196472814,
-                    "velocityX": -1.9276721060984434,
-                    "velocityY": -0.02371208959223001,
-                    "timestamp": 2.360169006513795
-                },
-                {
-                    "x": 7.164421971622539,
-                    "y": 0.7302939539488027,
-                    "heading": 2.9554195577269637,
-                    "angularVelocity": -0.8369528514504796,
-                    "velocityX": -2.494557900142165,
-                    "velocityY": 0.3011534897526061,
-                    "timestamp": 2.4386439686548287
-                },
-                {
-                    "x": 6.9246535759772865,
-                    "y": 0.7803818201714893,
-                    "heading": 2.891069702848338,
-                    "angularVelocity": -0.8200049177840428,
-                    "velocityX": -3.055348981428448,
-                    "velocityY": 0.638265567209492,
-                    "timestamp": 2.5171189307958626
-                },
-                {
-                    "x": 6.644255242545633,
-                    "y": 0.8592678201395815,
-                    "heading": 2.8720123676443365,
-                    "angularVelocity": -0.24284605795368294,
-                    "velocityX": -3.573092943042356,
-                    "velocityY": 1.0052378212854063,
-                    "timestamp": 2.5955938929368965
-                },
-                {
-                    "x": 6.376569654247088,
-                    "y": 0.9876185603343286,
-                    "heading": 2.872012347067197,
-                    "angularVelocity": -2.622127987829542e-7,
-                    "velocityX": -3.411095475490091,
-                    "velocityY": 1.635562945084242,
-                    "timestamp": 2.6740688550779304
-                },
-                {
-                    "x": 6.113906618540549,
-                    "y": 1.125957335049975,
-                    "heading": 2.872012327384056,
-                    "angularVelocity": -2.5082064696072804e-7,
-                    "velocityX": -3.347093500147104,
-                    "velocityY": 1.762839648995542,
-                    "timestamp": 2.7525438172189642
-                },
-                {
-                    "x": 5.851058985303004,
-                    "y": 1.2639450359763313,
-                    "heading": 2.8720122952205234,
-                    "angularVelocity": -4.098572586075927e-7,
-                    "velocityX": -3.3494458113298133,
-                    "velocityY": 1.758365944520708,
-                    "timestamp": 2.831018779359998
-                },
-                {
-                    "x": 5.625853496223335,
-                    "y": 1.3827528651043908,
-                    "heading": 2.754606631455948,
-                    "angularVelocity": -1.4960907346931063,
-                    "velocityX": -2.8697750586351334,
-                    "velocityY": 1.5139584128060528,
-                    "timestamp": 2.909493741501032
-                },
-                {
-                    "x": 5.445842564985353,
-                    "y": 1.4776937741923677,
-                    "heading": 2.6549514330576893,
-                    "angularVelocity": -1.2698980117917973,
-                    "velocityX": -2.2938645183985233,
-                    "velocityY": 1.2098242101391037,
-                    "timestamp": 2.987968703642066
-                },
-                {
-                    "x": 5.310896371700566,
-                    "y": 1.5488618130458742,
-                    "heading": 2.5782320494228896,
-                    "angularVelocity": -0.9776288072239107,
-                    "velocityX": -1.7196082623431492,
-                    "velocityY": 0.9068884764238665,
-                    "timestamp": 3.0664436657831
-                },
-                {
-                    "x": 5.220959019114767,
-                    "y": 1.5962913343665528,
-                    "heading": 2.5262728715572207,
-                    "angularVelocity": -0.6621115378476069,
-                    "velocityX": -1.146064300409159,
-                    "velocityY": 0.604390496365404,
-                    "timestamp": 3.1449186279241337
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": -0.33479304532875703,
-                    "velocityX": -0.5729090895764638,
-                    "velocityY": 0.30211757975540204,
-                    "timestamp": 3.2233935900651676
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": 0,
-                    "velocityX": 1.304014234910508e-32,
-                    "velocityY": 0,
-                    "timestamp": 3.3018685522062015
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 }
             ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -14127,16 +3668,16 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S1 To W1": {
+        "C1 To W1": {
             "waypoints": [
                 {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 13
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 2.3,
@@ -14148,134 +3689,7 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "angularVelocity": 1.76189972977325e-30,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.102979050763036,
-                    "y": 6.633224483540667,
-                    "heading": -2.873498740443943,
-                    "angularVelocity": 0.06942713881135329,
-                    "velocityX": -0.779792828619582,
-                    "velocityY": 0.034434352980739984,
-                    "timestamp": 0.09364147316695418
-                },
-                {
-                    "x": 4.956937154228187,
-                    "y": 6.639673450002842,
-                    "heading": -2.8604964293774233,
-                    "angularVelocity": 0.138852055897772,
-                    "velocityX": -1.559585636531679,
-                    "velocityY": 0.06886869934946069,
-                    "timestamp": 0.18728294633390835
-                },
-                {
-                    "x": 4.73787431448253,
-                    "y": 6.649346898579915,
-                    "heading": -2.840993426039055,
-                    "angularVelocity": 0.20827313666458666,
-                    "velocityX": -2.339378400797736,
-                    "velocityY": 0.10330303710435738,
-                    "timestamp": 0.28092441950086255
-                },
-                {
-                    "x": 4.4457905464926135,
-                    "y": 6.6622448281102296,
-                    "heading": -2.814990271784832,
-                    "angularVelocity": 0.27768843627487766,
-                    "velocityX": -3.1191710052355526,
-                    "velocityY": 0.13773736245389714,
-                    "timestamp": 0.3745658926678167
-                },
-                {
-                    "x": 4.091895298236837,
-                    "y": 6.6778724148973385,
-                    "heading": -2.8149902687925503,
-                    "angularVelocity": 3.195462281879478e-8,
-                    "velocityX": -3.779257590542345,
-                    "velocityY": 0.16688745123911564,
-                    "timestamp": 0.46820736583477085
-                },
-                {
-                    "x": 3.7380000499851023,
-                    "y": 6.693500001778431,
-                    "heading": -2.814990265800417,
-                    "angularVelocity": 3.1953066955491674e-8,
-                    "velocityX": -3.779257590499122,
-                    "velocityY": 0.16688745224276955,
-                    "timestamp": 0.561848839001725
-                },
-                {
-                    "x": 3.3841048017333675,
-                    "y": 6.709127588659523,
-                    "heading": -2.8149902628082843,
-                    "angularVelocity": 3.1953067022179835e-8,
-                    "velocityX": -3.779257590499122,
-                    "velocityY": 0.1668874522427702,
-                    "timestamp": 0.6554903121686791
-                },
-                {
-                    "x": 3.030209553477566,
-                    "y": 6.724755175446062,
-                    "heading": -2.8149902598160015,
-                    "angularVelocity": 3.195463264613959e-8,
-                    "velocityX": -3.7792575905426142,
-                    "velocityY": 0.16688745123301843,
-                    "timestamp": 0.7491317853356333
-                },
-                {
-                    "x": 2.7381257501719745,
-                    "y": 6.737653104232043,
-                    "heading": -2.7889939719160233,
-                    "angularVelocity": 0.27761511027933256,
-                    "velocityX": -3.1191713823726643,
-                    "velocityY": 0.13773735450514524,
-                    "timestamp": 0.8427732585025874
-                },
-                {
-                    "x": 2.5190628796730365,
-                    "y": 6.747326551843937,
-                    "heading": -2.769496951872836,
-                    "angularVelocity": 0.2082092408823168,
-                    "velocityX": -2.3393787292129176,
-                    "velocityY": 0.10330302679718467,
-                    "timestamp": 0.9364147316695416
-                },
-                {
-                    "x": 2.373020960810499,
-                    "y": 6.753775517241458,
-                    "heading": -2.7564989943923397,
-                    "angularVelocity": 0.13880556382672637,
-                    "velocityX": -1.5595858749697047,
-                    "velocityY": 0.06886868797998212,
-                    "timestamp": 1.0300562048364958
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": -2.75,
-                    "angularVelocity": 0.06940294906251296,
-                    "velocityX": -0.779792952213697,
-                    "velocityY": 0.03443434462839962,
-                    "timestamp": 1.12369767800345
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": -2.75,
-                    "angularVelocity": 1.276683149554956e-30,
-                    "velocityX": 2.9209786111644144e-32,
-                    "velocityY": 0,
-                    "timestamp": 1.2173391511704041
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -14295,173 +3709,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S1 To W2": {
+        "C1 To W2": {
             "waypoints": [
                 {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 15
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": -3.143185307179586,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -1.6791892318677926e-35,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.11892498821706,
-                    "y": 6.609128402762145,
-                    "heading": -2.8887795529976006,
-                    "angularVelocity": -0.10279668380897901,
-                    "velocityX": -0.6682711456093472,
-                    "velocityY": -0.24437815711512118,
-                    "timestamp": 0.08540696715387537
-                },
-                {
-                    "x": 5.004774958946586,
-                    "y": 6.5673852200358445,
-                    "heading": -2.90633824182768,
-                    "angularVelocity": -0.2055884831789443,
-                    "velocityX": -1.3365423580117657,
-                    "velocityY": -0.48875617666053534,
-                    "timestamp": 0.17081393430775074
-                },
-                {
-                    "x": 4.833549902172449,
-                    "y": 6.504770468021476,
-                    "heading": -2.93267521126463,
-                    "angularVelocity": -0.30837026901446724,
-                    "velocityX": -2.004813687689499,
-                    "velocityY": -0.7331340065214745,
-                    "timestamp": 0.2562209014616261
-                },
-                {
-                    "x": 4.605249804128149,
-                    "y": 6.4212841662523426,
-                    "heading": -2.9677889698411977,
-                    "angularVelocity": -0.4111345917869177,
-                    "velocityX": -2.6730851785543184,
-                    "velocityY": -0.9775116076738672,
-                    "timestamp": 0.3416278686155015
-                },
-                {
-                    "x": 4.319874653666952,
-                    "y": 6.316926338018247,
-                    "heading": -3.0116770321088655,
-                    "angularVelocity": -0.5138698133209222,
-                    "velocityX": -3.341356799932311,
-                    "velocityY": -1.2218889361341865,
-                    "timestamp": 0.42703483576937684
-                },
-                {
-                    "x": 4.016437784224315,
-                    "y": 6.205962942906548,
-                    "heading": -3.011677037359934,
-                    "angularVelocity": -6.148290543535782e-8,
-                    "velocityX": -3.5528350854087023,
-                    "velocityY": -1.2992311846383482,
-                    "timestamp": 0.5124418029232523
-                },
-                {
-                    "x": 3.713000914796963,
-                    "y": 6.094999547753008,
-                    "heading": -3.0116770426109816,
-                    "angularVelocity": -6.148266047339023e-8,
-                    "velocityX": -3.55283508522973,
-                    "velocityY": -1.299231185128267,
-                    "timestamp": 0.5978487700771277
-                },
-                {
-                    "x": 3.4095640453696117,
-                    "y": 5.984036152599468,
-                    "heading": -3.011677047862029,
-                    "angularVelocity": -6.148266099885158e-8,
-                    "velocityX": -3.5528350852297295,
-                    "velocityY": -1.2992311851282667,
-                    "timestamp": 0.6832557372310031
-                },
-                {
-                    "x": 3.1061271759243305,
-                    "y": 5.873072757495,
-                    "heading": -3.011677053113097,
-                    "angularVelocity": -6.1482904284003e-8,
-                    "velocityX": -3.5528350854396615,
-                    "velocityY": -1.2992311845536897,
-                    "timestamp": 0.7686627043848785
-                },
-                {
-                    "x": 2.820751491800317,
-                    "y": 5.768715178575412,
-                    "heading": -3.055515411809828,
-                    "angularVelocity": -0.513287851771491,
-                    "velocityX": -3.341363048401663,
-                    "velocityY": -1.2218860169986983,
-                    "timestamp": 0.8540696715387539
-                },
-                {
-                    "x": 2.592450909011727,
-                    "y": 5.685229109517301,
-                    "heading": -3.0905842313047507,
-                    "angularVelocity": -0.41060841595908354,
-                    "velocityX": -2.673090854253924,
-                    "velocityY": -0.977508882942736,
-                    "timestamp": 0.9394766386926293
-                },
-                {
-                    "x": 2.421225457633144,
-                    "y": 5.622614555370758,
-                    "heading": -3.116884896329009,
-                    "angularVelocity": -0.30794519347436133,
-                    "velocityX": -2.00481830797353,
-                    "velocityY": -0.7331316897570141,
-                    "timestamp": 1.0248836058465047
-                },
-                {
-                    "x": 2.3070751524559587,
-                    "y": 5.5808715184580695,
-                    "heading": -3.134418439279544,
-                    "angularVelocity": -0.2052940589606165,
-                    "velocityX": -1.336545588505958,
-                    "velocityY": -0.4887544693804859,
-                    "timestamp": 1.11029057300038
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -0.1026481584839197,
-                    "velocityX": -0.668272792700015,
-                    "velocityY": -0.24437723471044392,
-                    "timestamp": 1.1956975401542553
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -2.233003674732857e-32,
-                    "velocityX": 0,
-                    "velocityY": 6.615380928603251e-34,
-                    "timestamp": 1.2811045073081306
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -14481,281 +3750,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S1 To C1 To S1": {
+        "C1 To W3": {
             "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 13
-                },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": -3.143185307179586,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 13
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "angularVelocity": 2.29574628206475e-30,
-                    "velocityX": 4.692945968016159e-32,
-                    "velocityY": -1.566641075737276e-31,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.235048481309196,
-                    "y": 6.649731386951801,
-                    "heading": -2.8906451335254117,
-                    "angularVelocity": -0.12312313043682141,
-                    "velocityX": 0.6829631445217772,
-                    "velocityY": 0.2282160316336707,
-                    "timestamp": 0.08645925008229886
-                },
-                {
-                    "x": 5.3531499211746905,
-                    "y": 6.689193367474774,
-                    "heading": -2.911632072054118,
-                    "angularVelocity": -0.24273791998680166,
-                    "velocityX": 1.3659780735212093,
-                    "velocityY": 0.45642288691397265,
-                    "timestamp": 0.17291850016459773
-                },
-                {
-                    "x": 5.530311551906127,
-                    "y": 6.748384549002882,
-                    "heading": -2.9424540818551694,
-                    "angularVelocity": -0.3564917550356453,
-                    "velocityX": 2.0490766524439916,
-                    "velocityY": 0.6846136355771774,
-                    "timestamp": 0.2593777502468966
-                },
-                {
-                    "x": 5.7665469637988895,
-                    "y": 6.827301853965155,
-                    "heading": -2.9820943384526712,
-                    "angularVelocity": -0.4584848533802071,
-                    "velocityX": 2.732332418658699,
-                    "velocityY": 0.9127687886162316,
-                    "timestamp": 0.34583700032919545
-                },
-                {
-                    "x": 6.061889894006546,
-                    "y": 6.925933356393463,
-                    "heading": -3.027468756644365,
-                    "angularVelocity": -0.5248069830402681,
-                    "velocityX": 3.415978393596438,
-                    "velocityY": 1.1407860042093994,
-                    "timestamp": 0.43229625041149433
-                },
-                {
-                    "x": 6.3720848481132855,
-                    "y": 7.029634881790706,
-                    "heading": -3.0274687634280455,
-                    "angularVelocity": -7.846101114866984e-8,
-                    "velocityX": 3.587759017241877,
-                    "velocityY": 1.1994266119419068,
-                    "timestamp": 0.5187555004937932
-                },
-                {
-                    "x": 6.682279802039258,
-                    "y": 7.133336407728698,
-                    "heading": -3.027468770211708,
-                    "angularVelocity": -7.846080505503267e-8,
-                    "velocityX": 3.5877590151510974,
-                    "velocityY": 1.1994266181962872,
-                    "timestamp": 0.605214750576092
-                },
-                {
-                    "x": 6.992474757407683,
-                    "y": 7.237037929350706,
-                    "heading": -3.027468776995789,
-                    "angularVelocity": -7.846565119739109e-8,
-                    "velocityX": 3.5877590318346937,
-                    "velocityY": 1.1994265682769871,
-                    "timestamp": 0.6916740006583908
-                },
-                {
-                    "x": 7.248034956342824,
-                    "y": 7.322496159466712,
-                    "heading": -3.07460707499528,
-                    "angularVelocity": -0.5452082681093516,
-                    "velocityX": 2.9558456578310177,
-                    "velocityY": 0.9884220605040753,
-                    "timestamp": 0.7781332507406896
-                },
-                {
-                    "x": 7.444567623191234,
-                    "y": 7.388221479482509,
-                    "heading": -3.109745402875848,
-                    "angularVelocity": -0.406414904676237,
-                    "velocityX": 2.2731248149998473,
-                    "velocityY": 0.7601884119189827,
-                    "timestamp": 0.8645925008229884
-                },
-                {
-                    "x": 7.58207269769946,
-                    "y": 7.434213892330609,
-                    "heading": -3.1328871823753137,
-                    "angularVelocity": -0.2676611175500936,
-                    "velocityX": 1.5904032752686705,
-                    "velocityY": 0.5319547972517508,
-                    "timestamp": 0.9510517509052873
-                },
-                {
-                    "x": 7.660550155243088,
-                    "y": 7.460473398860823,
-                    "heading": -3.1440339918196103,
-                    "angularVelocity": -0.12892558556646783,
-                    "velocityX": 0.907681450727999,
-                    "velocityY": 0.3037211924135578,
-                    "timestamp": 1.037511000987586
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": 0.009816007412950826,
-                    "velocityX": 0.22495967450928966,
-                    "velocityY": 0.07548759829543861,
-                    "timestamp": 1.123970251069885
-                },
-                {
-                    "x": 7.635308945850804,
-                    "y": 7.452085548446669,
-                    "heading": -3.129057152990018,
-                    "angularVelocity": 0.1555765979823413,
-                    "velocityX": -0.49212955006536996,
-                    "velocityY": -0.1642351578439422,
-                    "timestamp": 1.2147818151045664
-                },
-                {
-                    "x": 7.525497368411343,
-                    "y": 7.41540168270092,
-                    "heading": -3.1017205952496476,
-                    "angularVelocity": 0.301025073527845,
-                    "velocityX": -1.2092246026888915,
-                    "velocityY": -0.4039558853066335,
-                    "timestamp": 1.3055933791392478
-                },
-                {
-                    "x": 7.350564387959683,
-                    "y": 7.356948703524257,
-                    "heading": -3.061223374709373,
-                    "angularVelocity": 0.44594783682873773,
-                    "velocityX": -1.9263293426462433,
-                    "velocityY": -0.6436733008397082,
-                    "timestamp": 1.3964049431739292
-                },
-                {
-                    "x": 7.110508199108376,
-                    "y": 7.276727219900285,
-                    "heading": -3.007665771604216,
-                    "angularVelocity": 0.5897663328854458,
-                    "velocityX": -2.6434539631934633,
-                    "velocityY": -0.8833840103589294,
-                    "timestamp": 1.4872165072086105
-                },
-                {
-                    "x": 6.80532324732103,
-                    "y": 7.174739191350757,
-                    "heading": -2.9413637265725296,
-                    "angularVelocity": 0.7301057495614385,
-                    "velocityX": -3.3606397492593936,
-                    "velocityY": -1.1230731419900883,
-                    "timestamp": 1.578028071243292
-                },
-                {
-                    "x": 6.479515641287707,
-                    "y": 7.065810200947661,
-                    "heading": -2.9413637193977467,
-                    "angularVelocity": 7.900737542269723e-8,
-                    "velocityX": -3.587732570148064,
-                    "velocityY": -1.1995057189140004,
-                    "timestamp": 1.6688396352779733
-                },
-                {
-                    "x": 6.153708035353397,
-                    "y": 6.956881210248373,
-                    "heading": -2.9413637122229894,
-                    "angularVelocity": 7.900708560619817e-8,
-                    "velocityX": -3.5877325690577564,
-                    "velocityY": -1.1995057221756227,
-                    "timestamp": 1.7596511993126547
-                },
-                {
-                    "x": 5.827900259692805,
-                    "y": 6.847953284193167,
-                    "heading": -2.9413633662901284,
-                    "angularVelocity": 0.0000038093481209652,
-                    "velocityX": -3.587734438051908,
-                    "velocityY": -1.1994939985136122,
-                    "timestamp": 1.850462763347336
-                },
-                {
-                    "x": 5.5671157361716075,
-                    "y": 6.7607637759552865,
-                    "heading": -2.922540068712598,
-                    "angularVelocity": 0.2072786409826639,
-                    "velocityX": -2.8717105171937094,
-                    "velocityY": -0.9601145973503112,
-                    "timestamp": 1.9412743273820174
-                },
-                {
-                    "x": 5.371545145894187,
-                    "y": 6.69537761850095,
-                    "heading": -2.9031356081339226,
-                    "angularVelocity": 0.2136782995204373,
-                    "velocityX": -2.1535868515901284,
-                    "velocityY": -0.7200201664777647,
-                    "timestamp": 2.0320858914166986
-                },
-                {
-                    "x": 5.241178014496252,
-                    "y": 6.651791296591565,
-                    "heading": -2.888169171521839,
-                    "angularVelocity": 0.1648076076208368,
-                    "velocityX": -1.4355785277318724,
-                    "velocityY": -0.47996444475676003,
-                    "timestamp": 2.1228974554513798
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "angularVelocity": 0.08995739264147623,
-                    "velocityX": -0.717728135057343,
-                    "velocityY": -0.23996169235946166,
-                    "timestamp": 2.213709019486061
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -2.956634078664977e-32,
-                    "timestamp": 2.304520583520742
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -14775,299 +3791,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S1 To C1 To S2": {
+        "C1 To S1": {
             "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 13
-                },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": -3.143185307179586,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": -3.143185307179586,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
+                },
                 {
                     "x": 5.176,
                     "y": 6.63,
                     "heading": -2.88,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.230255854586943,
-                    "y": 6.654457619561188,
-                    "heading": -2.8852614761787456,
-                    "angularVelocity": -0.06226832690913654,
-                    "velocityX": 0.6421052144646586,
-                    "velocityY": 0.2894501464070739,
-                    "timestamp": 0.08449682912507997
-                },
-                {
-                    "x": 5.338767562811348,
-                    "y": 6.703372857257978,
-                    "heading": -2.8957843391178932,
-                    "angularVelocity": -0.12453559557330285,
-                    "velocityX": 1.2842104176924247,
-                    "velocityY": 0.5789002759426788,
-                    "timestamp": 0.16899365825015994
-                },
-                {
-                    "x": 5.501535123103558,
-                    "y": 6.776745710953194,
-                    "heading": -2.911568425934469,
-                    "angularVelocity": -0.1868009365559807,
-                    "velocityX": 1.9263156023436965,
-                    "velocityY": 0.8683503801853032,
-                    "timestamp": 0.25349048737523994
-                },
-                {
-                    "x": 5.718558531629796,
-                    "y": 6.87457617714364,
-                    "heading": -2.9326134806355157,
-                    "angularVelocity": -0.24906324792252543,
-                    "velocityX": 2.568420741623094,
-                    "velocityY": 1.1578004429684545,
-                    "timestamp": 0.3379873165003199
-                },
-                {
-                    "x": 5.9898377739629405,
-                    "y": 6.996864247325266,
-                    "heading": -2.9589191023342685,
-                    "angularVelocity": -0.31132081488895513,
-                    "velocityX": 3.210525710160934,
-                    "velocityY": 1.4472504051081343,
-                    "timestamp": 0.4224841456253998
-                },
-                {
-                    "x": 6.281244778996208,
-                    "y": 7.128225687635927,
-                    "heading": -2.958919105469154,
-                    "angularVelocity": -3.710063015796767e-8,
-                    "velocityX": 3.448733024074779,
-                    "velocityY": 1.5546315958934869,
-                    "timestamp": 0.5069809747504798
-                },
-                {
-                    "x": 6.572651810300593,
-                    "y": 7.259587069667809,
-                    "heading": -2.9589191086040265,
-                    "angularVelocity": -3.7100470021746385e-8,
-                    "velocityX": 3.448733334987257,
-                    "velocityY": 1.5546309061778836,
-                    "timestamp": 0.5914778038755597
-                },
-                {
-                    "x": 6.868681501268645,
-                    "y": 7.380170200918755,
-                    "heading": -2.958919113683838,
-                    "angularVelocity": -6.01183729074805e-8,
-                    "velocityX": 3.5034414194388437,
-                    "velocityY": 1.4270728558635946,
-                    "timestamp": 0.6759746330006396
-                },
-                {
-                    "x": 7.138377967809157,
-                    "y": 7.448808221216772,
-                    "heading": -3.0028485661720943,
-                    "angularVelocity": -0.5198946865003425,
-                    "velocityX": 3.1917939327792095,
-                    "velocityY": 0.8123147461121022,
-                    "timestamp": 0.7604714621257196
-                },
-                {
-                    "x": 7.3546590217483025,
-                    "y": 7.491172600361784,
-                    "heading": -3.044173791439457,
-                    "angularVelocity": -0.4890742729077964,
-                    "velocityX": 2.5596351505567982,
-                    "velocityY": 0.5013724134227592,
-                    "timestamp": 0.8449682912507995
-                },
-                {
-                    "x": 7.517104833144657,
-                    "y": 7.508147678254589,
-                    "heading": -3.0815979795537904,
-                    "angularVelocity": -0.4429064202981498,
-                    "velocityX": 1.9225077802137127,
-                    "velocityY": 0.200896034426051,
-                    "timestamp": 0.9294651203758795
-                },
-                {
-                    "x": 7.625575199280341,
-                    "y": 7.500038616852461,
-                    "heading": -3.1146766365119762,
-                    "angularVelocity": -0.39147808622758395,
-                    "velocityX": 1.2837211438445335,
-                    "velocityY": -0.09596882493808385,
-                    "timestamp": 1.0139619495009595
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -0.3373933786960057,
-                    "velocityX": 0.6441046520111882,
-                    "velocityY": -0.39100422104069327,
-                    "timestamp": 1.0984587786260396
-                },
-                {
-                    "x": 7.666478172550486,
-                    "y": 7.390434189445638,
-                    "heading": -3.1708333738054395,
-                    "angularVelocity": -0.26987816293521383,
-                    "velocityX": -0.13198919117872188,
-                    "velocityY": -0.7473737883984474,
-                    "timestamp": 1.200905254195427
-                },
-                {
-                    "x": 7.573314043125627,
-                    "y": 7.277653381548422,
-                    "heading": -3.1915604539305784,
-                    "angularVelocity": -0.2023210658047483,
-                    "velocityX": -0.909393211499577,
-                    "velocityY": -1.1008754305152135,
-                    "timestamp": 1.3033517297648145
-                },
-                {
-                    "x": 7.400248960624901,
-                    "y": 7.129232012070322,
-                    "heading": -3.205357963605193,
-                    "angularVelocity": -0.13468017906843224,
-                    "velocityX": -1.6893219755862467,
-                    "velocityY": -1.4487698932851405,
-                    "timestamp": 1.405798205334202
-                },
-                {
-                    "x": 7.146578363694586,
-                    "y": 6.946793949790035,
-                    "heading": -3.2122031163802927,
-                    "angularVelocity": -0.06681686936573578,
-                    "velocityX": -2.476128100263464,
-                    "velocityY": -1.780813456649585,
-                    "timestamp": 1.5082446809035894
-                },
-                {
-                    "x": 6.805429935311203,
-                    "y": 6.762913771007078,
-                    "heading": -3.2122031133395477,
-                    "angularVelocity": 2.9681302827383053e-8,
-                    "velocityX": -3.330016249825229,
-                    "velocityY": -1.7948902366915849,
-                    "timestamp": 1.610691156472977
-                },
-                {
-                    "x": 6.4551130042311256,
-                    "y": 6.597166308184442,
-                    "heading": -3.2122031105786535,
-                    "angularVelocity": 2.694963002504427e-8,
-                    "velocityX": -3.4195117902597523,
-                    "velocityY": -1.6178932647651112,
-                    "timestamp": 1.7131376320423644
-                },
-                {
-                    "x": 6.104796046388704,
-                    "y": 6.431418901925591,
-                    "heading": -3.212203107817759,
-                    "angularVelocity": 2.6949630458066062e-8,
-                    "velocityX": -3.419512051492193,
-                    "velocityY": -1.617892712634998,
-                    "timestamp": 1.8155841076117518
-                },
-                {
-                    "x": 5.754479088546207,
-                    "y": 6.265671495666902,
-                    "heading": -3.2122031050568642,
-                    "angularVelocity": 2.694963045994546e-8,
-                    "velocityX": -3.4195120514929416,
-                    "velocityY": -1.617892712633416,
-                    "timestamp": 1.9180305831811393
-                },
-                {
-                    "x": 5.404162130703709,
-                    "y": 6.099924089408213,
-                    "heading": -3.212203102295969,
-                    "angularVelocity": 2.6949631214728657e-8,
-                    "velocityX": -3.419512051492942,
-                    "velocityY": -1.6178927126334155,
-                    "timestamp": 2.0204770587505267
-                },
-                {
-                    "x": 5.05384517285956,
-                    "y": 5.9341766831530425,
-                    "heading": -3.212203099535067,
-                    "angularVelocity": 2.6949706103007404e-8,
-                    "velocityX": -3.4195120515090505,
-                    "velocityY": -1.6178927125990576,
-                    "timestamp": 2.122923534319914
-                },
-                {
-                    "x": 4.737507119027181,
-                    "y": 5.784506016757487,
-                    "heading": -3.184596232441734,
-                    "angularVelocity": 0.2694760062744644,
-                    "velocityX": -3.087837351887456,
-                    "velocityY": -1.4609645238032911,
-                    "timestamp": 2.2253700098893017
-                },
-                {
-                    "x": 4.500253563523173,
-                    "y": 5.67225301019093,
-                    "heading": -3.163890824246715,
-                    "angularVelocity": 0.20210952187413175,
-                    "velocityX": -2.315878161599758,
-                    "velocityY": -1.0957234589346887,
-                    "timestamp": 2.327816485458689
-                },
-                {
-                    "x": 4.3420845221283875,
-                    "y": 5.597417670515286,
-                    "heading": -3.150087142577917,
-                    "angularVelocity": 0.1347404251056839,
-                    "velocityX": -1.5439188172721183,
-                    "velocityY": -0.7304823251333565,
-                    "timestamp": 2.4302629610280766
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": 0.06737015949032035,
-                    "velocityX": -0.771959422604275,
-                    "velocityY": -0.3652411691795466,
-                    "timestamp": 2.532709436597464
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -1.9737281778417947e-31,
-                    "velocityX": -1.0240473596982798e-33,
-                    "velocityY": -4.498532055712875e-34,
-                    "timestamp": 2.6351559121668515
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 }
             ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -15087,281 +3832,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S1 To C2 To S1": {
+        "C1 To S2": {
             "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 13
-                },
                 {
                     "x": 7.68,
-                    "y": 5.797,
-                    "heading": -3.143185307179586,
+                    "y": 7.467,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 13
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "angularVelocity": 1.4108683409597047e-30,
-                    "velocityX": 3.332067657020877e-32,
-                    "velocityY": 1.6252866310843352e-31,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.234960531442942,
-                    "y": 6.610392010976446,
-                    "heading": -2.8906900859192084,
-                    "angularVelocity": -0.12376379247862908,
-                    "velocityX": 0.6826118174441478,
-                    "velocityY": -0.22701025069196354,
-                    "timestamp": 0.08637490581940685
-                },
-                {
-                    "x": 5.352887916547719,
-                    "y": 6.571175678219841,
-                    "heading": -2.91160851046633,
-                    "angularVelocity": -0.24218173494536643,
-                    "velocityX": 1.3652968299769979,
-                    "velocityY": -0.45402460801062294,
-                    "timestamp": 0.1727498116388137
-                },
-                {
-                    "x": 5.529792169405666,
-                    "y": 6.5123505926970395,
-                    "heading": -2.941981384820343,
-                    "angularVelocity": -0.35164002861494675,
-                    "velocityX": 2.0480977800178555,
-                    "velocityY": -0.6810437008846502,
-                    "timestamp": 0.2591247174582205
-                },
-                {
-                    "x": 5.765691321896416,
-                    "y": 6.43391666324999,
-                    "heading": -2.9802445174117826,
-                    "angularVelocity": -0.4429889934853137,
-                    "velocityX": 2.7311074930027868,
-                    "velocityY": -0.9080638491352222,
-                    "timestamp": 0.3454996232776274
-                },
-                {
-                    "x": 6.060622443246375,
-                    "y": 6.335879897503487,
-                    "heading": -3.0215937804128985,
-                    "angularVelocity": -0.47871847278597873,
-                    "velocityX": 3.4145463726079974,
-                    "velocityY": -1.135014444490357,
-                    "timestamp": 0.4318745290970343
-                },
-                {
-                    "x": 6.370665856130874,
-                    "y": 6.2327325092534815,
-                    "heading": -3.021593787446907,
-                    "angularVelocity": -8.143578577450647e-8,
-                    "velocityX": 3.5895079704369173,
-                    "velocityY": -1.1941823527503022,
-                    "timestamp": 0.5182494349164412
-                },
-                {
-                    "x": 6.680709268872309,
-                    "y": 6.129585120573416,
-                    "heading": -3.021593794480898,
-                    "angularVelocity": -8.143559184955701e-8,
-                    "velocityX": 3.589507968780596,
-                    "velocityY": -1.1941823577293083,
-                    "timestamp": 0.604624340735848
-                },
-                {
-                    "x": 6.990752683844134,
-                    "y": 6.026437738598777,
-                    "heading": -3.0215938015153188,
-                    "angularVelocity": -8.144055599607105e-8,
-                    "velocityX": 3.5895079946028012,
-                    "velocityY": -1.1941822800976392,
-                    "timestamp": 0.6909992465552549
-                },
-                {
-                    "x": 7.246471127975024,
-                    "y": 5.941338278881442,
-                    "heading": -3.0707797962131482,
-                    "angularVelocity": -0.5694477375264861,
-                    "velocityX": 2.96056408634992,
-                    "velocityY": -0.9852336035564434,
-                    "timestamp": 0.7773741523746618
-                },
-                {
-                    "x": 7.443255050635665,
-                    "y": 5.875844773804915,
-                    "heading": -3.107535505632415,
-                    "angularVelocity": -0.4255368972118188,
-                    "velocityX": 2.2782533977168096,
-                    "velocityY": -0.7582469058018522,
-                    "timestamp": 0.8637490581940687
-                },
-                {
-                    "x": 7.581104494826472,
-                    "y": 5.829957223632905,
-                    "heading": -3.1318559851891097,
-                    "angularVelocity": -0.2815688112890415,
-                    "velocityX": 1.5959432069182882,
-                    "velocityY": -0.531260204994182,
-                    "timestamp": 0.9501239640134755
-                },
-                {
-                    "x": 7.660019481037545,
-                    "y": 5.803675631519179,
-                    "heading": -3.1437391060728066,
-                    "angularVelocity": -0.13757607919844067,
-                    "velocityX": 0.9136332533425802,
-                    "velocityY": -0.3042734676739749,
-                    "timestamp": 1.0364988698328823
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": 0.0064115716005977265,
-                    "velocityX": 0.2313231924569808,
-                    "velocityY": -0.07728670099033955,
-                    "timestamp": 1.122873775652289
-                },
-                {
-                    "x": 7.635819311328683,
-                    "y": 5.811667633489592,
-                    "heading": -3.128848165121335,
-                    "angularVelocity": 0.15781563504855214,
-                    "velocityX": -0.48631752487124336,
-                    "velocityY": 0.1614535089628058,
-                    "timestamp": 1.2137211879121816
-                },
-                {
-                    "x": 7.526434828772679,
-                    "y": 5.848024979672397,
-                    "heading": -3.1012666400693027,
-                    "angularVelocity": 0.30360275946215987,
-                    "velocityX": -1.2040462114976394,
-                    "velocityY": 0.4002023313416631,
-                    "timestamp": 1.3045686001720742
-                },
-                {
-                    "x": 7.351833825159198,
-                    "y": 5.906073128317211,
-                    "heading": -3.0612921137460094,
-                    "angularVelocity": 0.44001832665334883,
-                    "velocityX": -1.921914992073542,
-                    "velocityY": 0.6389631493158797,
-                    "timestamp": 1.3954160124319668
-                },
-                {
-                    "x": 7.111993081438883,
-                    "y": 5.98581338993347,
-                    "heading": -3.010630767411968,
-                    "angularVelocity": 0.5576531579045794,
-                    "velocityX": -2.640039355596495,
-                    "velocityY": 0.8777383926810057,
-                    "timestamp": 1.4862634246918593
-                },
-                {
-                    "x": 6.806861000464256,
-                    "y": 6.08724274158431,
-                    "heading": -2.954485863102405,
-                    "angularVelocity": 0.6180132478540051,
-                    "velocityX": -3.358731673078681,
-                    "velocityY": 1.116480361164798,
-                    "timestamp": 1.577110836951752
-                },
-                {
-                    "x": 6.480775270198975,
-                    "y": 6.195766521383185,
-                    "heading": -2.9544858545944934,
-                    "angularVelocity": 9.365056825417393e-8,
-                    "velocityX": -3.5893783009738223,
-                    "velocityY": 1.1945720532843969,
-                    "timestamp": 1.6679582492116445
-                },
-                {
-                    "x": 6.154689540144831,
-                    "y": 6.30429030181652,
-                    "heading": -2.954485846086605,
-                    "angularVelocity": 9.365030974191779e-8,
-                    "velocityX": -3.589378298649725,
-                    "velocityY": 1.1945720602682006,
-                    "timestamp": 1.758805661471537
-                },
-                {
-                    "x": 5.828602321664721,
-                    "y": 6.412809153986278,
-                    "heading": -2.954485580357569,
-                    "angularVelocity": 0.0000029250024503863607,
-                    "velocityX": -3.5893946824518035,
-                    "velocityY": 1.194517812563883,
-                    "timestamp": 1.8496530737314296
-                },
-                {
-                    "x": 5.567525835725181,
-                    "y": 6.4996975117043,
-                    "heading": -2.9315080017864195,
-                    "angularVelocity": 0.25292496505229944,
-                    "velocityX": -2.8737911124282145,
-                    "velocityY": 0.9564208330939352,
-                    "timestamp": 1.9405004859913222
-                },
-                {
-                    "x": 5.371744680904255,
-                    "y": 6.564854924968385,
-                    "heading": -2.907956180338786,
-                    "angularVelocity": 0.2592459252452843,
-                    "velocityX": -2.155054832611796,
-                    "velocityY": 0.7172181534207646,
-                    "timestamp": 2.0313478982512145
-                },
-                {
-                    "x": 5.241242968677622,
-                    "y": 6.608286759589549,
-                    "heading": -2.8898557615846676,
-                    "angularVelocity": 0.19923978354186256,
-                    "velocityX": -1.4364934452213165,
-                    "velocityY": 0.478074537741632,
-                    "timestamp": 2.122195310511107
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "angularVelocity": 0.10848698206630299,
-                    "velocityX": -0.7181599019131975,
-                    "velocityY": 0.2390078029777863,
-                    "timestamp": 2.2130427227709997
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -3.247565811207596e-32,
-                    "timestamp": 2.3038901350308922
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -15381,290 +3873,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S1 To C2 To S2": {
+        "C1 To S3": {
             "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 13
-                },
                 {
                     "x": 7.68,
-                    "y": 5.797,
-                    "heading": -3.143185307179586,
+                    "y": 7.467,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 14
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 4.263,
-                    "y": 5.56,
-                    "heading": -3.143185307179586,
+                    "y": 3,
+                    "heading": 2.574,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.2343405973213555,
-                    "y": 6.622412668725056,
-                    "heading": -2.8862658883257666,
-                    "angularVelocity": -0.07457855479400594,
-                    "velocityX": 0.6943879634997097,
-                    "velocityY": -0.0903067804291561,
-                    "timestamp": 0.08401729348434916
-                },
-                {
-                    "x": 5.351021788709109,
-                    "y": 6.607238006430742,
-                    "heading": -2.898797916255787,
-                    "angularVelocity": -0.14916010038283942,
-                    "velocityX": 1.3887758882579295,
-                    "velocityY": -0.18061355781642813,
-                    "timestamp": 0.16803458696869833
-                },
-                {
-                    "x": 5.52604356848206,
-                    "y": 6.584476013331715,
-                    "heading": -2.917596514461126,
-                    "angularVelocity": -0.22374677195285786,
-                    "velocityX": 2.083163745396705,
-                    "velocityY": -0.27092033264873905,
-                    "timestamp": 0.2520518804530475
-                },
-                {
-                    "x": 5.759405926747038,
-                    "y": 6.554126689512535,
-                    "heading": -2.9426622755752048,
-                    "angularVelocity": -0.2983404972304698,
-                    "velocityX": 2.7775514847838854,
-                    "velocityY": -0.3612271064746121,
-                    "timestamp": 0.33606917393739666
-                },
-                {
-                    "x": 6.051108840456095,
-                    "y": 6.516190035692408,
-                    "heading": -2.97399589297024,
-                    "angularVelocity": -0.37294247524019475,
-                    "velocityX": 3.4719389498472166,
-                    "velocityY": -0.45153387174027243,
-                    "timestamp": 0.4200864674217458
-                },
-                {
-                    "x": 6.366286997596586,
-                    "y": 6.47520006127383,
-                    "heading": -2.973995896754836,
-                    "angularVelocity": -4.504543806754251e-8,
-                    "velocityX": 3.751348610143015,
-                    "velocityY": -0.4878754446691907,
-                    "timestamp": 0.504103760906095
-                },
-                {
-                    "x": 6.681465049565767,
-                    "y": 6.434209278184785,
-                    "heading": -2.973995900539415,
-                    "angularVelocity": -4.5045241068948784e-8,
-                    "velocityX": 3.751347358361316,
-                    "velocityY": -0.48788506971698453,
-                    "timestamp": 0.5881210543904442
-                },
-                {
-                    "x": 6.987163031152994,
-                    "y": 6.347225437890881,
-                    "heading": -2.9739959119301913,
-                    "angularVelocity": -1.3557656915583158e-7,
-                    "velocityX": 3.6385126074570944,
-                    "velocityY": -1.0353087642618135,
-                    "timestamp": 0.6721383478747933
-                },
-                {
-                    "x": 7.243185278067537,
-                    "y": 6.230387598842041,
-                    "heading": -3.0107958850128846,
-                    "angularVelocity": -0.4380047435061505,
-                    "velocityX": 3.047256538467705,
-                    "velocityY": -1.3906403575188384,
-                    "timestamp": 0.7561556413591425
-                },
-                {
-                    "x": 7.4403635227243035,
-                    "y": 6.1148085109072055,
-                    "heading": -3.048556145231695,
-                    "angularVelocity": -0.44943438014751774,
-                    "velocityX": 2.3468768925946972,
-                    "velocityY": -1.3756583096355752,
-                    "timestamp": 0.8401729348434916
-                },
-                {
-                    "x": 7.578836794258849,
-                    "y": 6.003419207644454,
-                    "heading": -3.0839307394839546,
-                    "angularVelocity": -0.42103944063431625,
-                    "velocityX": 1.648152014803231,
-                    "velocityY": -1.3257901872726057,
-                    "timestamp": 0.9241902283278408
-                },
-                {
-                    "x": 7.658696859900876,
-                    "y": 5.8972967644784475,
-                    "heading": -3.115687244057696,
-                    "angularVelocity": -0.37797581017838017,
-                    "velocityX": 0.9505193791669008,
-                    "velocityY": -1.2631023776762744,
-                    "timestamp": 1.00820752181219
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -0.32729051343474413,
-                    "velocityX": 0.25355660978405975,
-                    "velocityY": -1.1937633351296981,
-                    "timestamp": 1.092224815296539
-                },
-                {
-                    "x": 7.6231583877847156,
-                    "y": 5.686873944343296,
-                    "heading": -3.169227404734073,
-                    "angularVelocity": -0.2617377186372048,
-                    "velocityX": -0.5712901533281343,
-                    "velocityY": -1.1068287610009868,
-                    "timestamp": 1.1917217379796003
-                },
-                {
-                    "x": 7.484327538641781,
-                    "y": 5.5861293490761845,
-                    "heading": -3.18873884204171,
-                    "angularVelocity": -0.1961009122843807,
-                    "velocityX": -1.3953280704486528,
-                    "velocityY": -1.0125398107841346,
-                    "timestamp": 1.2912186606626617
-                },
-                {
-                    "x": 7.263675847711948,
-                    "y": 5.496135627515027,
-                    "heading": -3.2017054758509116,
-                    "angularVelocity": -0.13032195830322837,
-                    "velocityX": -2.2176735217500094,
-                    "velocityY": -0.9044874869931846,
-                    "timestamp": 1.390715583345723
-                },
-                {
-                    "x": 6.9617360146088005,
-                    "y": 5.420370006770522,
-                    "heading": -3.2080997246142733,
-                    "angularVelocity": -0.0642657942671267,
-                    "velocityX": -3.034665042505381,
-                    "velocityY": -0.7614870761968194,
-                    "timestamp": 1.4902125060287843
-                },
-                {
-                    "x": 6.587133153221649,
-                    "y": 5.383725358359533,
-                    "heading": -3.208099721013608,
-                    "angularVelocity": 3.618871368220605e-8,
-                    "velocityX": -3.7649693205127126,
-                    "velocityY": -0.3682993144191623,
-                    "timestamp": 1.5897094287118456
-                },
-                {
-                    "x": 6.211820115708755,
-                    "y": 5.412190570741109,
-                    "heading": -3.2080997182657316,
-                    "angularVelocity": 2.761770262617581e-8,
-                    "velocityX": -3.7721069897651076,
-                    "velocityY": 0.2860913846777879,
-                    "timestamp": 1.689206351394907
-                },
-                {
-                    "x": 5.836507126679018,
-                    "y": 5.440656422364601,
-                    "heading": -3.208099715517856,
-                    "angularVelocity": 2.761769551512973e-8,
-                    "velocityX": -3.7721065024821283,
-                    "velocityY": 0.28609780941835966,
-                    "timestamp": 1.7887032740779683
-                },
-                {
-                    "x": 5.461194137649431,
-                    "y": 5.46912227399009,
-                    "heading": -3.20809971276998,
-                    "angularVelocity": 2.7617694565057146e-8,
-                    "velocityX": -3.772106502480606,
-                    "velocityY": 0.28609780943843166,
-                    "timestamp": 1.8882001967610296
-                },
-                {
-                    "x": 5.08588114861977,
-                    "y": 5.497588125614274,
-                    "heading": -3.2080997100220925,
-                    "angularVelocity": 2.7617811191359224e-8,
-                    "velocityX": -3.7721065024813654,
-                    "velocityY": 0.2860978094253166,
-                    "timestamp": 1.9876971194440909
-                },
-                {
-                    "x": 4.756728707190495,
-                    "y": 5.522552875223879,
-                    "heading": -3.182133930988408,
-                    "angularVelocity": 0.26097067460464174,
-                    "velocityX": -3.3081670523395013,
-                    "velocityY": 0.2509097662158744,
-                    "timestamp": 2.087194042127152
-                },
-                {
-                    "x": 4.509864358241751,
-                    "y": 5.541276437563115,
-                    "heading": -3.1626596163193277,
-                    "angularVelocity": 0.19572780890032443,
-                    "velocityX": -2.4811254689263977,
-                    "velocityY": 0.18818232598889303,
-                    "timestamp": 2.1866909648102135
-                },
-                {
-                    "x": 4.3452881204218,
-                    "y": 5.553758812444608,
-                    "heading": -3.1496767469198765,
-                    "angularVelocity": 0.13048513511123086,
-                    "velocityX": -1.654083698087773,
-                    "velocityY": 0.12545488387870263,
-                    "timestamp": 2.286187887493275
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": 0.06524261821612926,
-                    "velocityX": -0.8270418642385756,
-                    "velocityY": 0.06272744309160665,
-                    "timestamp": 2.385684810176336
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -3.6755949365770895e-31,
-                    "velocityX": -3.301266761312006e-32,
-                    "velocityY": -7.495504571617436e-31,
-                    "timestamp": 2.4851817328593975
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -15684,335 +3914,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S1 To C3 To S1": {
+        "C1 To S4": {
             "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 16
-                },
                 {
                     "x": 7.68,
-                    "y": 4.127,
-                    "heading": -3.143185307179586,
+                    "y": 7.467,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 16
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
+                    "y": 1.62,
+                    "heading": 2.5,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.218507748835192,
-                    "y": 6.587508953337151,
-                    "heading": -2.890727584630965,
-                    "angularVelocity": -0.12627463531080194,
-                    "velocityX": 0.5003596491390202,
-                    "velocityY": -0.5001630474999694,
-                    "timestamp": 0.08495439012388621
-                },
-                {
-                    "x": 5.303524107208959,
-                    "y": 6.502525872657889,
-                    "heading": -2.9120884337900024,
-                    "angularVelocity": -0.25143902661031875,
-                    "velocityX": 1.0007294296365374,
-                    "velocityY": -1.0003377171605015,
-                    "timestamp": 0.16990878024777242
-                },
-                {
-                    "x": 5.43105048877714,
-                    "y": 6.375049128446974,
-                    "heading": -2.9439256135386342,
-                    "angularVelocity": -0.37475614505901766,
-                    "velocityX": 1.5011158503081115,
-                    "velocityY": -1.5005315678808802,
-                    "timestamp": 0.2548631703716586
-                },
-                {
-                    "x": 5.601089619523365,
-                    "y": 6.205075520007896,
-                    "heading": -2.985927322319944,
-                    "angularVelocity": -0.49440304050481265,
-                    "velocityX": 2.001534358593136,
-                    "velocityY": -2.0007630940704417,
-                    "timestamp": 0.33981756049554485
-                },
-                {
-                    "x": 5.813649033766866,
-                    "y": 5.992595894100207,
-                    "heading": -3.037161099536011,
-                    "angularVelocity": -0.6030739216881468,
-                    "velocityX": 2.502041553515969,
-                    "velocityY": -2.501102363255205,
-                    "timestamp": 0.42477195061943107
-                },
-                {
-                    "x": 6.040945217563345,
-                    "y": 5.7653958045642115,
-                    "heading": -3.03716110602298,
-                    "angularVelocity": -7.635824927871784e-8,
-                    "velocityX": 2.6755083929744035,
-                    "velocityY": -2.6743772653146864,
-                    "timestamp": 0.5097263407433172
-                },
-                {
-                    "x": 6.268241401429681,
-                    "y": 5.53819571509808,
-                    "heading": -3.037161112509923,
-                    "angularVelocity": -7.635794871774561e-8,
-                    "velocityX": 2.675508393796696,
-                    "velocityY": -2.674377264492316,
-                    "timestamp": 0.5946807308672034
-                },
-                {
-                    "x": 6.495537585296019,
-                    "y": 5.310995625631949,
-                    "heading": -3.037161118996866,
-                    "angularVelocity": -7.635794841586156e-8,
-                    "velocityX": 2.675508393796701,
-                    "velocityY": -2.6743772644923114,
-                    "timestamp": 0.6796351209910896
-                },
-                {
-                    "x": 6.722833769162356,
-                    "y": 5.083795536165818,
-                    "heading": -3.037161125483809,
-                    "angularVelocity": -7.635794938585852e-8,
-                    "velocityX": 2.6755083937967,
-                    "velocityY": -2.6743772644923127,
-                    "timestamp": 0.7645895111149758
-                },
-                {
-                    "x": 6.950129953013132,
-                    "y": 4.856595446684119,
-                    "heading": -3.037161131970752,
-                    "angularVelocity": -7.635794903516443e-8,
-                    "velocityX": 2.67550839361353,
-                    "velocityY": -2.6743772646755604,
-                    "timestamp": 0.8495439012388619
-                },
-                {
-                    "x": 7.177423048635347,
-                    "y": 4.629392269264043,
-                    "heading": -3.0371611395742963,
-                    "angularVelocity": -8.950149400601411e-8,
-                    "velocityX": 2.6754720420070424,
-                    "velocityY": -2.674413612866287,
-                    "timestamp": 0.9344982913627481
-                },
-                {
-                    "x": 7.362966742660802,
-                    "y": 4.443922816476664,
-                    "heading": -3.078668671836781,
-                    "angularVelocity": -0.48858607779956104,
-                    "velocityX": 2.1840389149365182,
-                    "velocityY": -2.183165019685597,
-                    "timestamp": 1.0194526814866343
-                },
-                {
-                    "x": 7.505991293187966,
-                    "y": 4.3009534921360935,
-                    "heading": -3.1105416993817094,
-                    "angularVelocity": -0.37517811025716574,
-                    "velocityX": 1.6835451389668437,
-                    "velocityY": -1.6828950703063337,
-                    "timestamp": 1.1044070716105205
-                },
-                {
-                    "x": 7.6065035905836105,
-                    "y": 4.200477915848541,
-                    "heading": -3.1320612562280816,
-                    "angularVelocity": -0.25330717829595906,
-                    "velocityX": 1.1831324696564203,
-                    "velocityY": -1.1827002246831095,
-                    "timestamp": 1.1893614617344068
-                },
-                {
-                    "x": 7.664506074481624,
-                    "y": 4.142493806219386,
-                    "heading": -3.1429833930528956,
-                    "angularVelocity": -0.12856471347350074,
-                    "velocityX": 0.6827485173302748,
-                    "velocityY": -0.6825322334062814,
-                    "timestamp": 1.274315851858293
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -0.002376735636840867,
-                    "velocityX": 0.1823793390257534,
-                    "velocityY": -0.18237793475472894,
-                    "timestamp": 1.3592702419821794
-                },
-                {
-                    "x": 7.6496885572239846,
-                    "y": 4.157291685194411,
-                    "heading": -3.131583473376336,
-                    "angularVelocity": 0.13054330158990457,
-                    "velocityX": -0.3410629632388108,
-                    "velocityY": 0.3408406518372109,
-                    "timestamp": 1.44814369364031
-                },
-                {
-                    "x": 7.572857027601893,
-                    "y": 4.234083644862719,
-                    "heading": -3.108168408493633,
-                    "angularVelocity": 0.26346523563343455,
-                    "velocityX": -0.8645048458050324,
-                    "velocityY": 0.8640596065007748,
-                    "timestamp": 1.5370171452984407
-                },
-                {
-                    "x": 7.449505471220409,
-                    "y": 4.357375922418621,
-                    "heading": -3.0729392389653802,
-                    "angularVelocity": 0.39639699900261377,
-                    "velocityX": -1.387946052278649,
-                    "velocityY": 1.3872790496532752,
-                    "timestamp": 1.6258905969565713
-                },
-                {
-                    "x": 7.279633974763547,
-                    "y": 4.52716856280588,
-                    "heading": -3.0258936977546345,
-                    "angularVelocity": 0.5293542709674363,
-                    "velocityX": -1.911386283387714,
-                    "velocityY": 1.9104989985109575,
-                    "timestamp": 1.714764048614702
-                },
-                {
-                    "x": 7.063242657628,
-                    "y": 4.743461594393602,
-                    "heading": -2.9670273926808983,
-                    "angularVelocity": 0.6623609635456799,
-                    "velocityX": -2.434825171050669,
-                    "velocityY": 2.4337192665764835,
-                    "timestamp": 1.8036375002728326
-                },
-                {
-                    "x": 6.825460366769834,
-                    "y": 4.981142108011423,
-                    "heading": -2.967027386113265,
-                    "angularVelocity": 7.38987079974334e-8,
-                    "velocityX": -2.675515425830906,
-                    "velocityY": 2.6743702329927124,
-                    "timestamp": 1.8925109519309633
-                },
-                {
-                    "x": 6.587678075764525,
-                    "y": 5.218822621482074,
-                    "heading": -2.9670273795456663,
-                    "angularVelocity": 7.389832008297281e-8,
-                    "velocityX": -2.6755154274865576,
-                    "velocityY": 2.6743702313367446,
-                    "timestamp": 1.981384403589094
-                },
-                {
-                    "x": 6.349895784759215,
-                    "y": 5.456503134952723,
-                    "heading": -2.967027372978067,
-                    "angularVelocity": 7.389832049238788e-8,
-                    "velocityX": -2.675515427486565,
-                    "velocityY": 2.6743702313367375,
-                    "timestamp": 2.0702578552472244
-                },
-                {
-                    "x": 6.112113493753906,
-                    "y": 5.6941836484233725,
-                    "heading": -2.967027366410468,
-                    "angularVelocity": 7.389831996958896e-8,
-                    "velocityX": -2.67551542748656,
-                    "velocityY": 2.6743702313367437,
-                    "timestamp": 2.159131306905355
-                },
-                {
-                    "x": 5.874331202859884,
-                    "y": 5.931864162005351,
-                    "heading": -2.9670273598428634,
-                    "angularVelocity": 7.389838540319935e-8,
-                    "velocityX": -2.6755154262343623,
-                    "velocityY": 2.6743702325894136,
-                    "timestamp": 2.2480047585634853
-                },
-                {
-                    "x": 5.641547676468134,
-                    "y": 6.164595563213349,
-                    "heading": -2.949497080521501,
-                    "angularVelocity": 0.1972498985246826,
-                    "velocityX": -2.619269557429503,
-                    "velocityY": 2.6186830472537257,
-                    "timestamp": 2.3368782102216157
-                },
-                {
-                    "x": 5.455313702452088,
-                    "y": 6.350776206364247,
-                    "heading": -2.9252668204425896,
-                    "angularVelocity": 0.2726377745748803,
-                    "velocityX": -2.0954961300759565,
-                    "velocityY": 2.0948960536284607,
-                    "timestamp": 2.425751661879746
-                },
-                {
-                    "x": 5.3156503816730245,
-                    "y": 6.490395881310486,
-                    "heading": -2.9038323021536026,
-                    "angularVelocity": 0.2411802162410647,
-                    "velocityX": -1.571485276799544,
-                    "velocityY": 1.5709941758913435,
-                    "timestamp": 2.5146251135378765
-                },
-                {
-                    "x": 5.222548326123915,
-                    "y": 6.583467415924721,
-                    "heading": -2.888242371448522,
-                    "angularVelocity": 0.17541718493253458,
-                    "velocityX": -1.0475800569474265,
-                    "velocityY": 1.047236636788705,
-                    "timestamp": 2.603498565196007
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "angularVelocity": 0.09274278532860386,
-                    "velocityX": -0.5237596296246885,
-                    "velocityY": 0.5235825008157663,
-                    "timestamp": 2.6923720168541374
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "angularVelocity": 3.731495977364104e-28,
-                    "velocityX": 5.297862503022749e-31,
-                    "velocityY": -3.827373628676626e-30,
-                    "timestamp": 2.781245468512268
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -16032,326 +3955,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S1 To C3 To S2": {
+        "C1 To C2": {
             "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 16
-                },
                 {
                     "x": 7.68,
-                    "y": 4.127,
-                    "heading": -3.143185307179586,
+                    "y": 7.467,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 15
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": -3.143185307179586,
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.2230421692043185,
-                    "y": 6.596479510791831,
-                    "heading": -2.8866360691814505,
-                    "angularVelocity": -0.07970895698970118,
-                    "velocityX": 0.5650456828103535,
-                    "velocityY": -0.40263040657207605,
-                    "timestamp": 0.08325374502526237
-                },
-                {
-                    "x": 5.3171265089770285,
-                    "y": 6.529438536200997,
-                    "heading": -2.8999080848900367,
-                    "angularVelocity": -0.15941644072056302,
-                    "velocityX": 1.130091382005238,
-                    "velocityY": -0.8052607671941977,
-                    "timestamp": 0.16650749005052473
-                },
-                {
-                    "x": 5.458253021148553,
-                    "y": 6.428877082265555,
-                    "heading": -2.919815877976496,
-                    "angularVelocity": -0.23912189271985748,
-                    "velocityX": 1.6951371031862086,
-                    "velocityY": -1.207891055290405,
-                    "timestamp": 0.2497612350757871
-                },
-                {
-                    "x": 5.646421706294536,
-                    "y": 6.294795158564417,
-                    "heading": -2.946359278217123,
-                    "angularVelocity": -0.31882530008192794,
-                    "velocityX": 2.260182831281474,
-                    "velocityY": -1.6105212283297445,
-                    "timestamp": 0.33301498010104946
-                },
-                {
-                    "x": 5.881632555955815,
-                    "y": 6.127192783772945,
-                    "heading": -2.9795381480937526,
-                    "angularVelocity": -0.3985270556484735,
-                    "velocityX": 2.825228457769793,
-                    "velocityY": -2.01315117705052,
-                    "timestamp": 0.41626872512631186
-                },
-                {
-                    "x": 6.138121957560345,
-                    "y": 5.944428216164069,
-                    "heading": -2.9795381521700137,
-                    "angularVelocity": -4.8961898041700444e-8,
-                    "velocityX": 3.080815181667819,
-                    "velocityY": -2.195271426569656,
-                    "timestamp": 0.49952247015157425
-                },
-                {
-                    "x": 6.3946113591646245,
-                    "y": 5.761663648554803,
-                    "heading": -2.979538156246253,
-                    "angularVelocity": -4.896162950841139e-8,
-                    "velocityX": 3.080815181664829,
-                    "velocityY": -2.1952714265743465,
-                    "timestamp": 0.5827762151768366
-                },
-                {
-                    "x": 6.651100760768856,
-                    "y": 5.57889908094547,
-                    "heading": -2.979538160322492,
-                    "angularVelocity": -4.8961628925618056e-8,
-                    "velocityX": 3.080815181664257,
-                    "velocityY": -2.1952714265751494,
-                    "timestamp": 0.666029960202099
-                },
-                {
-                    "x": 6.907590153574081,
-                    "y": 5.396134500987725,
-                    "heading": -2.979538164398731,
-                    "angularVelocity": -4.8961630365108595e-8,
-                    "velocityX": 3.080815075975223,
-                    "velocityY": -2.195271574897749,
-                    "timestamp": 0.7492837052273614
-                },
-                {
-                    "x": 7.162448400773746,
-                    "y": 5.211102141322406,
-                    "heading": -2.979538168481962,
-                    "angularVelocity": -4.904561353756505e-8,
-                    "velocityX": 3.06122261674062,
-                    "velocityY": -2.2225109466143445,
-                    "timestamp": 0.8325374502526238
-                },
-                {
-                    "x": 7.379207979156498,
-                    "y": 4.982618051640636,
-                    "heading": -2.979538183621576,
-                    "angularVelocity": -1.8184904794867305e-7,
-                    "velocityX": 2.6036015354861264,
-                    "velocityY": -2.7444301708283056,
-                    "timestamp": 0.9157911952778862
-                },
-                {
-                    "x": 7.538979437885237,
-                    "y": 4.753039236440615,
-                    "heading": -3.0140329583631105,
-                    "angularVelocity": -0.4143330096570687,
-                    "velocityX": 1.919090350593248,
-                    "velocityY": -2.7575794353799603,
-                    "timestamp": 0.9990449403031486
-                },
-                {
-                    "x": 7.647684941731154,
-                    "y": 4.550516222432533,
-                    "heading": -3.0504775725311664,
-                    "angularVelocity": -0.43775345069461125,
-                    "velocityX": 1.3057130800893728,
-                    "velocityY": -2.432599445786314,
-                    "timestamp": 1.082298685328411
-                },
-                {
-                    "x": 7.7070137148586175,
-                    "y": 4.378021543776967,
-                    "heading": -3.0850144113956173,
-                    "angularVelocity": -0.4148382616778724,
-                    "velocityX": 0.7126258777842882,
-                    "velocityY": -2.071914946327394,
-                    "timestamp": 1.1655524303536733
-                },
-                {
-                    "x": 7.717659875456922,
-                    "y": 4.236658143123209,
-                    "heading": -3.1161746508326322,
-                    "angularVelocity": -0.3742803333058527,
-                    "velocityX": 0.12787605644728667,
-                    "velocityY": -1.6979824824801262,
-                    "timestamp": 1.2488061753789357
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -0.32443773356692346,
-                    "velocityX": -0.4523505272405549,
-                    "velocityY": -1.3171556797827937,
-                    "timestamp": 1.332059920404198
-                },
-                {
-                    "x": 7.570205120538345,
-                    "y": 4.042598994966427,
-                    "heading": -3.1684471523328126,
-                    "angularVelocity": -0.2594470408705627,
-                    "velocityX": -1.1276277091512135,
-                    "velocityY": -0.8668246863853641,
-                    "timestamp": 1.4294279422882057
-                },
-                {
-                    "x": 7.395116825349968,
-                    "y": 4.002723380018642,
-                    "heading": -3.1873733598366223,
-                    "angularVelocity": -0.19437806312174594,
-                    "velocityX": -1.7982114846386814,
-                    "velocityY": -0.4095350216243075,
-                    "timestamp": 1.5267959641722133
-                },
-                {
-                    "x": 7.155591929265255,
-                    "y": 4.008604211117546,
-                    "heading": -3.1999520195770272,
-                    "angularVelocity": -0.1291867647818698,
-                    "velocityX": -2.4599955041712693,
-                    "velocityY": 0.06039797240528109,
-                    "timestamp": 1.624163986056221
-                },
-                {
-                    "x": 6.8538087913529315,
-                    "y": 4.063163767369359,
-                    "heading": -3.206164046421751,
-                    "angularVelocity": -0.06379945617179969,
-                    "velocityX": -3.0994070956050406,
-                    "velocityY": 0.5603436857001141,
-                    "timestamp": 1.7215320079402285
-                },
-                {
-                    "x": 6.5048799215848865,
-                    "y": 4.181151469294334,
-                    "heading": -3.206164042303272,
-                    "angularVelocity": 4.229807303696183e-8,
-                    "velocityX": -3.5836084888703787,
-                    "velocityY": 1.2117705550753732,
-                    "timestamp": 1.8189000298242362
-                },
-                {
-                    "x": 6.189160518675962,
-                    "y": 4.3708716014574716,
-                    "heading": -3.2061640395158935,
-                    "angularVelocity": 2.862724812638766e-8,
-                    "velocityX": -3.2425368904488527,
-                    "velocityY": 1.9484850209768463,
-                    "timestamp": 1.9162680517082438
-                },
-                {
-                    "x": 5.875739114062726,
-                    "y": 4.564364477524865,
-                    "heading": -3.2061640367361566,
-                    "angularVelocity": 2.854876387634348e-8,
-                    "velocityX": -3.218935730116917,
-                    "velocityY": 1.987232279381165,
-                    "timestamp": 2.0136360735922514
-                },
-                {
-                    "x": 5.562317717200868,
-                    "y": 4.757857366148008,
-                    "heading": -3.20616403395642,
-                    "angularVelocity": 2.854876379789868e-8,
-                    "velocityX": -3.21893565050783,
-                    "velocityY": 1.9872324083326358,
-                    "timestamp": 2.111004095476259
-                },
-                {
-                    "x": 5.248896320339037,
-                    "y": 4.951350254771193,
-                    "heading": -3.2061640311766837,
-                    "angularVelocity": 2.8548763614821095e-8,
-                    "velocityX": -3.218935650507565,
-                    "velocityY": 1.9872324083330646,
-                    "timestamp": 2.208372117360266
-                },
-                {
-                    "x": 4.935474923469446,
-                    "y": 5.14484314338174,
-                    "heading": -3.2061640283969295,
-                    "angularVelocity": 2.8548942066572532e-8,
-                    "velocityX": -3.2189356505872584,
-                    "velocityY": 1.9872324082032717,
-                    "timestamp": 2.3057401392442736
-                },
-                {
-                    "x": 4.666484969375691,
-                    "y": 5.310905875767285,
-                    "heading": -3.180972420287378,
-                    "angularVelocity": 0.2587256844917891,
-                    "velocityX": -2.76261085404609,
-                    "velocityY": 1.7055161353012933,
-                    "timestamp": 2.403108161128281
-                },
-                {
-                    "x": 4.464742488596807,
-                    "y": 5.435452935258762,
-                    "heading": -3.1620788420473156,
-                    "angularVelocity": 0.19404295039052252,
-                    "velocityX": -2.071958296731311,
-                    "velocityY": 1.279137206256954,
-                    "timestamp": 2.5004761830122884
-                },
-                {
-                    "x": 4.330247497040812,
-                    "y": 5.518484311233343,
-                    "heading": -3.149483158535891,
-                    "angularVelocity": 0.12936160422802073,
-                    "velocityX": -1.3813055760362147,
-                    "velocityY": 0.8527581681128775,
-                    "timestamp": 2.5978442048962958
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": 0.0646809007150999,
-                    "velocityX": -0.6906528009876122,
-                    "velocityY": 0.4263790920607684,
-                    "timestamp": 2.695212226780303
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -3.400041679826015e-29,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 2.7925802486643105
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -16371,344 +3996,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S1 To C3 To S3": {
+        "C1 To C3": {
             "waypoints": [
                 {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 16
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": -3.143185307179586,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 17
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -3.7091853071795864,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "angularVelocity": 2.6904386896052926e-33,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.221431258578075,
-                    "y": 6.6088996918922325,
-                    "heading": -2.8866746127276346,
-                    "angularVelocity": -0.08608437747729661,
-                    "velocityX": 0.5859398548340703,
-                    "velocityY": -0.2721366710185261,
-                    "timestamp": 0.07753570303037556
-                },
-                {
-                    "x": 5.31229377552925,
-                    "y": 6.566699082350374,
-                    "heading": -2.90002400824425,
-                    "angularVelocity": -0.17217094828412036,
-                    "velocityX": 1.1718797070244922,
-                    "velocityY": -0.5442732559647441,
-                    "timestamp": 0.15507140606075112
-                },
-                {
-                    "x": 5.448587549407382,
-                    "y": 6.503398181602668,
-                    "heading": -2.920048533381972,
-                    "angularVelocity": -0.2582619922834353,
-                    "velocityX": 1.7578195405636137,
-                    "velocityY": -0.8164097089944291,
-                    "timestamp": 0.2326071090911267
-                },
-                {
-                    "x": 5.630312575309777,
-                    "y": 6.418997003981843,
-                    "heading": -2.946748763466125,
-                    "angularVelocity": -0.34436045641700236,
-                    "velocityX": 2.3437593108713033,
-                    "velocityY": -1.0885459771707946,
-                    "timestamp": 0.31014281212150224
-                },
-                {
-                    "x": 5.85746883610922,
-                    "y": 6.313495571165909,
-                    "heading": -2.9801255206908306,
-                    "angularVelocity": -0.43046952462183896,
-                    "velocityX": 2.929698860284435,
-                    "velocityY": -1.3606819657597289,
-                    "timestamp": 0.3876785151518778
-                },
-                {
-                    "x": 6.123490041046321,
-                    "y": 6.189943134833017,
-                    "heading": -2.9801255253705285,
-                    "angularVelocity": -6.035539216471427e-8,
-                    "velocityX": 3.4309510914331183,
-                    "velocityY": -1.5934908887649881,
-                    "timestamp": 0.46521421818225334
-                },
-                {
-                    "x": 6.3895112454442975,
-                    "y": 6.066390697338873,
-                    "heading": -2.980125530050055,
-                    "angularVelocity": -6.035318736040994e-8,
-                    "velocityX": 3.4309510844798714,
-                    "velocityY": -1.5934909037419929,
-                    "timestamp": 0.5427499212126289
-                },
-                {
-                    "x": 6.655532416724182,
-                    "y": 5.9428381885380706,
-                    "heading": -2.980125534729582,
-                    "angularVelocity": -6.035318830910377e-8,
-                    "velocityX": 3.4309506573464303,
-                    "velocityY": -1.5934918234042366,
-                    "timestamp": 0.6202856242430045
-                },
-                {
-                    "x": 6.916684100099049,
-                    "y": 5.8093006126159725,
-                    "heading": -2.9801255394461488,
-                    "angularVelocity": -6.083090321698073e-8,
-                    "velocityX": 3.368147487778084,
-                    "velocityY": -1.722272072128935,
-                    "timestamp": 0.6978213272733801
-                },
-                {
-                    "x": 7.151284630214051,
-                    "y": 5.6332464271795715,
-                    "heading": -2.980125544587418,
-                    "angularVelocity": -6.630841173330857e-8,
-                    "velocityX": 3.0257097175361216,
-                    "velocityY": -2.2706208695551444,
-                    "timestamp": 0.7753570303037557
-                },
-                {
-                    "x": 7.352481762263237,
-                    "y": 5.41981682210962,
-                    "heading": -2.9801255509932516,
-                    "angularVelocity": -8.261785689390399e-8,
-                    "velocityX": 2.594896598414324,
-                    "velocityY": -2.7526622798059472,
-                    "timestamp": 0.8528927333341313
-                },
-                {
-                    "x": 7.514399141163384,
-                    "y": 5.175245297922962,
-                    "heading": -2.9801256025732923,
-                    "angularVelocity": -6.652424424834864e-7,
-                    "velocityX": 2.0882944575444746,
-                    "velocityY": -3.1543084621396242,
-                    "timestamp": 0.9304284363645069
-                },
-                {
-                    "x": 7.630164379984533,
-                    "y": 4.911565821303128,
-                    "heading": -2.995124870089402,
-                    "angularVelocity": -0.19344981640565623,
-                    "velocityX": 1.4930571890964461,
-                    "velocityY": -3.4007491557345486,
-                    "timestamp": 1.0079641393948824
-                },
-                {
-                    "x": 7.696376897938806,
-                    "y": 4.643893289420577,
-                    "heading": -3.0332372938699343,
-                    "angularVelocity": -0.49154676221354465,
-                    "velocityX": 0.8539616636781268,
-                    "velocityY": -3.452248724406194,
-                    "timestamp": 1.085499842425258
-                },
-                {
-                    "x": 7.712814001135022,
-                    "y": 4.380911639847311,
-                    "heading": -3.084305372604304,
-                    "angularVelocity": -0.6586395265464133,
-                    "velocityX": 0.2119939918488342,
-                    "velocityY": -3.3917490819711635,
-                    "timestamp": 1.1630355454556336
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -0.7593912516949146,
-                    "velocityX": -0.4232114993806923,
-                    "velocityY": -3.274770588561481,
-                    "timestamp": 1.2405712484860092
-                },
-                {
-                    "x": 7.5963416463862465,
-                    "y": 3.881029306471945,
-                    "heading": -3.207914317977363,
-                    "angularVelocity": -0.8216221979368662,
-                    "velocityX": -1.061897277970947,
-                    "velocityY": -3.1221700958161,
-                    "timestamp": 1.3193532129464565
-                },
-                {
-                    "x": 7.463114472360868,
-                    "y": 3.64992032438806,
-                    "heading": -3.2707770828701133,
-                    "angularVelocity": -0.7979334524504146,
-                    "velocityX": -1.6910872296446224,
-                    "velocityY": -2.933526520526327,
-                    "timestamp": 1.3981351774069037
-                },
-                {
-                    "x": 7.282107143226341,
-                    "y": 3.438334424773196,
-                    "heading": -3.3202070228970326,
-                    "angularVelocity": -0.627427106768014,
-                    "velocityX": -2.297573186632101,
-                    "velocityY": -2.6857149483888696,
-                    "timestamp": 1.476917141867351
-                },
-                {
-                    "x": 7.057858991686637,
-                    "y": 3.2538746449727443,
-                    "heading": -3.3390490142356852,
-                    "angularVelocity": -0.23916630497468983,
-                    "velocityX": -2.846440210973517,
-                    "velocityY": -2.341396042403339,
-                    "timestamp": 1.5556991063277983
-                },
-                {
-                    "x": 6.798499862682402,
-                    "y": 3.1070645806670387,
-                    "heading": -3.339049130726268,
-                    "angularVelocity": -0.0000014786453189461519,
-                    "velocityX": -3.2921129954108745,
-                    "velocityY": -1.8634983947298314,
-                    "timestamp": 1.6344810707882456
-                },
-                {
-                    "x": 6.517654165139062,
-                    "y": 3.0073345934329407,
-                    "heading": -3.339049157320925,
-                    "angularVelocity": -3.3757290550660805e-7,
-                    "velocityX": -3.564847607783876,
-                    "velocityY": -1.26589871066451,
-                    "timestamp": 1.7132630352486928
-                },
-                {
-                    "x": 6.223790307061648,
-                    "y": 2.9576917507277325,
-                    "heading": -3.339049179064471,
-                    "angularVelocity": -2.7599649426865103e-7,
-                    "velocityX": -3.730090511070579,
-                    "velocityY": -0.6301295359311817,
-                    "timestamp": 1.79204499970914
-                },
-                {
-                    "x": 5.925769134173891,
-                    "y": 2.9596309032298342,
-                    "heading": -3.3390491994000007,
-                    "angularVelocity": -2.581241798261079e-7,
-                    "velocityX": -3.7828603910655114,
-                    "velocityY": 0.024614167917522748,
-                    "timestamp": 1.8708269641695874
-                },
-                {
-                    "x": 5.627822525147453,
-                    "y": 2.9665733818436117,
-                    "heading": -3.339049219680213,
-                    "angularVelocity": -2.574220136164825e-7,
-                    "velocityX": -3.781913932547634,
-                    "velocityY": 0.08812268977200981,
-                    "timestamp": 1.9496089286300347
-                },
-                {
-                    "x": 5.329875916862242,
-                    "y": 2.9735158922681233,
-                    "heading": -3.3390492399604246,
-                    "angularVelocity": -2.5742201457831597e-7,
-                    "velocityX": -3.7819139231390566,
-                    "velocityY": 0.08812309355394705,
-                    "timestamp": 2.028390893090482
-                },
-                {
-                    "x": 5.031929325692052,
-                    "y": 2.9804591371189906,
-                    "heading": -3.3390492602433595,
-                    "angularVelocity": -2.5745657158705766e-7,
-                    "velocityX": -3.781913705893629,
-                    "velocityY": 0.08813241581901742,
-                    "timestamp": 2.107172857550929
-                },
-                {
-                    "x": 4.775295960622385,
-                    "y": 2.98713582308431,
-                    "heading": -3.452004590816206,
-                    "angularVelocity": -1.4337714392683885,
-                    "velocityX": -3.2575141636447755,
-                    "velocityY": 0.08474891443804398,
-                    "timestamp": 2.1859548220113765
-                },
-                {
-                    "x": 4.570259992018404,
-                    "y": 2.9923299087295048,
-                    "heading": -3.5517317371257473,
-                    "angularVelocity": -1.2658626500689703,
-                    "velocityX": -2.6025749676110213,
-                    "velocityY": 0.06592988231212166,
-                    "timestamp": 2.2647367864718237
-                },
-                {
-                    "x": 4.416588409818771,
-                    "y": 2.996180871589336,
-                    "heading": -3.6294028651739794,
-                    "angularVelocity": -0.9858998640130027,
-                    "velocityX": -1.9505934289920475,
-                    "velocityY": 0.04888127487306719,
-                    "timestamp": 2.343518750932271
-                },
-                {
-                    "x": 4.314185287460311,
-                    "y": 2.998730700935922,
-                    "heading": -3.6823213519430347,
-                    "angularVelocity": -0.6717081394387356,
-                    "velocityX": -1.2998295112312361,
-                    "velocityY": 0.03236564820448933,
-                    "timestamp": 2.4223007153927183
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -3.7091853071795864,
-                    "angularVelocity": -0.3409911826968791,
-                    "velocityX": -0.6497081890615813,
-                    "velocityY": 0.01611154371144828,
-                    "timestamp": 2.5010826798531656
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -3.7091853071795864,
-                    "angularVelocity": 0,
-                    "velocityX": -2.823998043085552e-35,
-                    "velocityY": 0,
-                    "timestamp": 2.579864644313613
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -16728,353 +4037,69 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S1 To C3 To S4": {
+        "C1 To C4": {
             "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 16
-                },
                 {
                     "x": 7.68,
-                    "y": 4.127,
-                    "heading": -3.143185307179586,
+                    "y": 7.467,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 18
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -3.7831853071795862,
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
+            "trajectory": [],
+            "constraints": [
                 {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": -2.88,
-                    "angularVelocity": -4.965302317163387e-33,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.214448464194176,
-                    "y": 6.6065493619277555,
-                    "heading": -2.899983532805956,
-                    "angularVelocity": -0.2708663369562031,
-                    "velocityX": 0.5211488258354376,
-                    "velocityY": -0.31786113574578073,
-                    "timestamp": 0.0737763615461288
-                },
-                {
-                    "x": 5.29140014559419,
-                    "y": 6.559619691972876,
-                    "heading": -2.9381129725152273,
-                    "angularVelocity": -0.5168246157738562,
-                    "velocityX": 1.0430398001113212,
-                    "velocityY": -0.636107134743106,
-                    "timestamp": 0.1475527230922576
-                },
-                {
-                    "x": 5.4069285579459345,
-                    "y": 6.489171539636289,
-                    "heading": -2.991543589570423,
-                    "angularVelocity": -0.7242240730696432,
-                    "velocityX": 1.5659272147693208,
-                    "velocityY": -0.9548878646250154,
-                    "timestamp": 0.22132908463838638
-                },
-                {
-                    "x": 5.5611322195100845,
-                    "y": 6.395152264966145,
-                    "heading": -3.0552740925010653,
-                    "angularVelocity": -0.8638336398684346,
-                    "velocityX": 2.090149992931468,
-                    "velocityY": -1.274382101526624,
-                    "timestamp": 0.2951054461845152
-                },
-                {
-                    "x": 5.754097394020428,
-                    "y": 6.277527348774539,
-                    "heading": -3.118382886347054,
-                    "angularVelocity": -0.8554066983437507,
-                    "velocityX": 2.615542030894161,
-                    "velocityY": -1.5943442279687556,
-                    "timestamp": 0.36888180773064394
-                },
-                {
-                    "x": 5.984744637231227,
-                    "y": 6.137241174757932,
-                    "heading": -3.141361477565195,
-                    "angularVelocity": -0.3114627874915312,
-                    "velocityX": 3.126302766592624,
-                    "velocityY": -1.9015057272632323,
-                    "timestamp": 0.4426581692767727
-                },
-                {
-                    "x": 6.222972456839802,
-                    "y": 5.99184648556835,
-                    "heading": -3.141361501066067,
-                    "angularVelocity": -3.1854203464323085e-7,
-                    "velocityX": 3.2290535154627107,
-                    "velocityY": -1.9707489789757882,
-                    "timestamp": 0.5164345308229015
-                },
-                {
-                    "x": 6.461200274584459,
-                    "y": 5.84645179332473,
-                    "heading": -3.141361524566912,
-                    "angularVelocity": -3.1854166338718743e-7,
-                    "velocityX": 3.229053490198278,
-                    "velocityY": -1.9707490203716698,
-                    "timestamp": 0.5902108923690302
-                },
-                {
-                    "x": 6.699428092323813,
-                    "y": 5.701057101072421,
-                    "heading": -3.141361548067757,
-                    "angularVelocity": -3.1854166408242085e-7,
-                    "velocityX": 3.229053490126394,
-                    "velocityY": -1.9707490204894516,
-                    "timestamp": 0.663987253915159
-                },
-                {
-                    "x": 6.937655250211389,
-                    "y": 5.555661327664803,
-                    "heading": -3.1413615715686114,
-                    "angularVelocity": -3.1854179676885927e-7,
-                    "velocityX": 3.229044546180602,
-                    "velocityY": -1.9707636749842845,
-                    "timestamp": 0.7377636154612878
-                },
-                {
-                    "x": 7.1538386137488,
-                    "y": 5.379148261525069,
-                    "heading": -3.141361595947866,
-                    "angularVelocity": -3.304480485706385e-7,
-                    "velocityX": 2.9302524413899382,
-                    "velocityY": -2.3925423054289428,
-                    "timestamp": 0.8115399770074165
-                },
-                {
-                    "x": 7.338555522796062,
-                    "y": 5.1699312191681095,
-                    "heading": -3.1413616242387703,
-                    "angularVelocity": -3.834684085405442e-7,
-                    "velocityX": 2.5037411059064913,
-                    "velocityY": -2.835827600770833,
-                    "timestamp": 0.8853163385535453
-                },
-                {
-                    "x": 7.486921372318085,
-                    "y": 4.933542391468771,
-                    "heading": -3.14136166220073,
-                    "angularVelocity": -5.145545098256939e-7,
-                    "velocityX": 2.011021503537517,
-                    "velocityY": -3.2041269418191103,
-                    "timestamp": 0.9590927000996741
-                },
-                {
-                    "x": 7.595012870290821,
-                    "y": 4.676232612674377,
-                    "heading": -3.1413617253677457,
-                    "angularVelocity": -8.561958656088552e-7,
-                    "velocityX": 1.4651237294367088,
-                    "velocityY": -3.4876994934686665,
-                    "timestamp": 1.032869061645803
-                },
-                {
-                    "x": 7.659971709721358,
-                    "y": 4.404806001492882,
-                    "heading": -3.141361954512953,
-                    "angularVelocity": -0.000003105943453461285,
-                    "velocityX": 0.8804830987757537,
-                    "velocityY": -3.679045774191308,
-                    "timestamp": 1.1066454231919318
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
                 {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -0.02471459188852694,
-                    "velocityX": 0.271473001092883,
-                    "velocityY": -3.7655150738110477,
-                    "timestamp": 1.1804217847380607
-                },
-                {
-                    "x": 7.658280036401298,
-                    "y": 3.864604229609803,
-                    "heading": -3.146329645491869,
-                    "angularVelocity": -0.04501004755691485,
-                    "velocityX": -0.3109132979403334,
-                    "velocityY": -3.756099036118669,
-                    "timestamp": 1.2502803714970885
-                },
-                {
-                    "x": 7.5962523104298665,
-                    "y": 3.6077169181356963,
-                    "heading": -3.1463327455058754,
-                    "angularVelocity": -0.00004437556140498802,
-                    "velocityX": -0.8879041052661434,
-                    "velocityY": -3.677247470811584,
-                    "timestamp": 1.3201389582561163
-                },
-                {
-                    "x": 7.495521811257821,
-                    "y": 3.363396565109804,
-                    "heading": -3.1463328962340493,
-                    "angularVelocity": -0.0000021576184275855436,
-                    "velocityX": -1.4419200823444587,
-                    "velocityY": -3.4973560783394904,
-                    "timestamp": 1.389997545015144
-                },
-                {
-                    "x": 7.358477102372377,
-                    "y": 3.1374367675877326,
-                    "heading": -3.146332979999805,
-                    "angularVelocity": -0.000001199076011915671,
-                    "velocityX": -1.9617446507781449,
-                    "velocityY": -3.234531472866801,
-                    "timestamp": 1.4598561317741718
-                },
-                {
-                    "x": 7.188367445952169,
-                    "y": 2.9351948516310506,
-                    "heading": -3.1463330407624492,
-                    "angularVelocity": -8.697949218469567e-7,
-                    "velocityX": -2.4350572250622284,
-                    "velocityY": -2.895018713365922,
-                    "timestamp": 1.5297147185331996
-                },
-                {
-                    "x": 6.989226108875858,
-                    "y": 2.761465700266129,
-                    "heading": -3.146333092561237,
-                    "angularVelocity": -7.414806172090105e-7,
-                    "velocityX": -2.8506350659974578,
-                    "velocityY": -2.486868965216661,
-                    "timestamp": 1.5995733052922274
-                },
-                {
-                    "x": 6.765782539563676,
-                    "y": 2.620355593747368,
-                    "heading": -3.14633314264619,
-                    "angularVelocity": -7.169476958064807e-7,
-                    "velocityX": -3.19851259062737,
-                    "velocityY": -2.0199393240734156,
-                    "timestamp": 1.6694318920512552
-                },
-                {
-                    "x": 6.5419983548650125,
-                    "y": 2.479786288735506,
-                    "heading": -3.1463331927321803,
-                    "angularVelocity": -7.169625493020222e-7,
-                    "velocityX": -3.203388374726096,
-                    "velocityY": -2.012197949219134,
-                    "timestamp": 1.739290478810283
-                },
-                {
-                    "x": 6.318214167123251,
-                    "y": 2.339216988568208,
-                    "heading": -3.1463332428181707,
-                    "angularVelocity": -7.16962547152982e-7,
-                    "velocityX": -3.2033884182869192,
-                    "velocityY": -2.012197879870968,
-                    "timestamp": 1.8091490655693108
-                },
-                {
-                    "x": 6.094429979379188,
-                    "y": 2.198647688404573,
-                    "heading": -3.1463332929041607,
-                    "angularVelocity": -7.1696254162218e-7,
-                    "velocityX": -3.2033884183198493,
-                    "velocityY": -2.0121978798185434,
-                    "timestamp": 1.8790076523283386
-                },
-                {
-                    "x": 5.870645537599717,
-                    "y": 2.05807879266491,
-                    "heading": -3.1463333429939095,
-                    "angularVelocity": -7.170163458750797e-7,
-                    "velocityX": -3.203392054743378,
-                    "velocityY": -2.012192090637989,
-                    "timestamp": 1.9488662390873663
-                },
-                {
-                    "x": 5.673381673927696,
-                    "y": 1.935533968246103,
-                    "heading": -3.225487827303808,
-                    "angularVelocity": -1.1330673576740993,
-                    "velocityX": -2.8237597240904497,
-                    "velocityY": -1.754184132603728,
-                    "timestamp": 2.018724825846394
-                },
-                {
-                    "x": 5.505993389711308,
-                    "y": 1.8298743063416731,
-                    "heading": -3.3794217717080413,
-                    "angularVelocity": -2.2035078512999076,
-                    "velocityX": -2.396101781929577,
-                    "velocityY": -1.512479235643509,
-                    "timestamp": 2.088583412605422
-                },
-                {
-                    "x": 5.373136379336833,
-                    "y": 1.745695395482932,
-                    "heading": -3.5283087444702135,
-                    "angularVelocity": -2.1312623067475838,
-                    "velocityX": -1.9017992853585055,
-                    "velocityY": -1.2049901775011624,
-                    "timestamp": 2.15844199936445
-                },
-                {
-                    "x": 5.274205775679982,
-                    "y": 1.6827613854161347,
-                    "heading": -3.651053741383974,
-                    "angularVelocity": -1.7570495283158336,
-                    "velocityX": -1.4161552394139385,
-                    "velocityY": -0.9008772290782794,
-                    "timestamp": 2.228300586123478
-                },
-                {
-                    "x": 5.2086365166677675,
-                    "y": 1.6409004652612855,
-                    "heading": -3.73798062119163,
-                    "angularVelocity": -1.2443263432669185,
-                    "velocityX": -0.9385998494128075,
-                    "velocityY": -0.5992236902708281,
-                    "timestamp": 2.298159172882506
-                },
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "C1 To C5": {
+            "waypoints": [
                 {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -3.7831853071795862,
-                    "angularVelocity": -0.6470884695089293,
-                    "velocityX": -0.46717974384944866,
-                    "velocityY": -0.29918248036394063,
-                    "timestamp": 2.368017759641534
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -3.7831853071795862,
-                    "angularVelocity": 0,
-                    "velocityX": -3.585209340915008e-32,
-                    "velocityY": 0,
-                    "timestamp": 2.437876346400562
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 }
             ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -17094,155 +4119,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S2 To W1": {
+        "C2 To W1": {
             "waypoints": [
                 {
-                    "x": 4.263,
-                    "y": 5.56,
+                    "x": 7.68,
+                    "y": 5.797,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 13
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": 3.5331853071795862,
+                    "heading": -2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": -6.652842580111428e-35,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.215224473336601,
-                    "y": 5.589131466417677,
-                    "heading": 3.1531161748208096,
-                    "angularVelocity": 0.15993673271356004,
-                    "velocityX": -0.5825678402891398,
-                    "velocityY": 0.3552248747558748,
-                    "timestamp": 0.08200852048353235
-                },
-                {
-                    "x": 4.119673422721879,
-                    "y": 5.647394502413024,
-                    "heading": 3.179344235647322,
-                    "angularVelocity": 0.3198211682379922,
-                    "velocityX": -1.1651356475076162,
-                    "velocityY": 0.7104510074297316,
-                    "timestamp": 0.1640170409670647
-                },
-                {
-                    "x": 3.9763468590229047,
-                    "y": 5.73478928117174,
-                    "heading": 3.2186777846940484,
-                    "angularVelocity": 0.47962759009443284,
-                    "velocityX": -1.7477033222146137,
-                    "velocityY": 1.065679251904866,
-                    "timestamp": 0.24602556145059706
-                },
-                {
-                    "x": 3.785244828834372,
-                    "y": 5.85131604858519,
-                    "heading": 3.2711097941834586,
-                    "angularVelocity": 0.6393483162513433,
-                    "velocityX": -2.3302704287526703,
-                    "velocityY": 1.420910494743635,
-                    "timestamp": 0.3280340819341294
-                },
-                {
-                    "x": 3.546367452965707,
-                    "y": 5.996975111850807,
-                    "heading": 3.3366347863626045,
-                    "angularVelocity": 0.7990022474835781,
-                    "velocityX": -2.9128360621581053,
-                    "velocityY": 1.7761454835033377,
-                    "timestamp": 0.4100426024176618
-                },
-                {
-                    "x": 3.281494477712196,
-                    "y": 6.158489941563481,
-                    "heading": 3.336634794646938,
-                    "angularVelocity": 1.0101796436143703e-7,
-                    "velocityX": -3.2298226293047043,
-                    "velocityY": 1.9694883990146768,
-                    "timestamp": 0.4920511229011941
-                },
-                {
-                    "x": 3.016621502775785,
-                    "y": 6.320004771796177,
-                    "heading": 3.336634802931272,
-                    "angularVelocity": 1.0101796370003125e-7,
-                    "velocityX": -3.2298226254380333,
-                    "velocityY": 1.969488405355746,
-                    "timestamp": 0.5740596433847265
-                },
-                {
-                    "x": 2.7777474341519097,
-                    "y": 6.465669636134063,
-                    "heading": 3.402143395936758,
-                    "angularVelocity": 0.7988022783393678,
-                    "velocityX": -2.912795734094993,
-                    "velocityY": 1.7762162209368904,
-                    "timestamp": 0.6560681638682588
-                },
-                {
-                    "x": 2.586648366744574,
-                    "y": 6.582201701925324,
-                    "heading": 3.4545567132875594,
-                    "angularVelocity": 0.6391203870252214,
-                    "velocityX": -2.3302343010286215,
-                    "velocityY": 1.4209751023939097,
-                    "timestamp": 0.7380766843517912
-                },
-                {
-                    "x": 2.4433241632679117,
-                    "y": 6.669600830029101,
-                    "heading": 3.4938702766455694,
-                    "angularVelocity": 0.4793838875059847,
-                    "velocityX": -1.7476745420061834,
-                    "velocityY": 1.0657322871875106,
-                    "timestamp": 0.8200852048353235
-                },
-                {
-                    "x": 2.3477747237681617,
-                    "y": 6.727866941749082,
-                    "heading": 3.5200804322455714,
-                    "angularVelocity": 0.3196028345038231,
-                    "velocityX": -1.1651160018054092,
-                    "velocityY": 0.7104885123696424,
-                    "timestamp": 0.9020937253188559
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 3.5331853071795862,
-                    "angularVelocity": 0.15979894353351334,
-                    "velocityX": -0.5825580499011068,
-                    "velocityY": 0.35524428533945085,
-                    "timestamp": 0.9841022458023883
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 3.5331853071795862,
-                    "angularVelocity": 2.6358633635199364e-33,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 1.0661107662859206
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -17262,16 +4160,16 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S2 To W2": {
+        "C2 To W2": {
             "waypoints": [
                 {
-                    "x": 4.263,
-                    "y": 5.56,
+                    "x": 7.68,
+                    "y": 5.797,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 10
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 2.25,
@@ -17283,107 +4181,7 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": -4.0059408516052846e-40,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.181096344253034,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 1.5012541318382292e-16,
-                    "velocityX": -0.8263919813283321,
-                    "velocityY": -1.2317462001949155e-21,
-                    "timestamp": 0.09910993523353699
-                },
-                {
-                    "x": 4.017289035836238,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 3.002363652724146e-16,
-                    "velocityX": -1.6527839316089836,
-                    "velocityY": -2.463492548222639e-21,
-                    "timestamp": 0.19821987046707398
-                },
-                {
-                    "x": 3.771578080903875,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 4.503226484774511e-16,
-                    "velocityX": -2.4791758197942855,
-                    "velocityY": -3.695244043691227e-21,
-                    "timestamp": 0.29732980570061096
-                },
-                {
-                    "x": 3.4439634979187304,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 6.003547774343467e-16,
-                    "velocityX": -3.305567521693684,
-                    "velocityY": -4.926945384758097e-21,
-                    "timestamp": 0.39643974093414797
-                },
-                {
-                    "x": 3.0690365020812695,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 4.535737431947664e-16,
-                    "velocityX": -3.7829405796099507,
-                    "velocityY": 2.4634613990171587e-20,
-                    "timestamp": 0.49554967616768497
-                },
-                {
-                    "x": 2.741421919096125,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 5.990674751034296e-16,
-                    "velocityX": -3.305567521693684,
-                    "velocityY": -4.926861067570722e-21,
-                    "timestamp": 0.5946596114012219
-                },
-                {
-                    "x": 2.4957109641637625,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 4.492129826743599e-16,
-                    "velocityX": -2.479175819794286,
-                    "velocityY": -3.695159726389389e-21,
-                    "timestamp": 0.6937695466347589
-                },
-                {
-                    "x": 2.3319036557469652,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 2.9945516157260173e-16,
-                    "velocityX": -1.6527839316089836,
-                    "velocityY": -2.4634409378754245e-21,
-                    "timestamp": 0.7928794818682958
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 1.4972226131077696e-16,
-                    "velocityX": -0.8263919813283321,
-                    "velocityY": -1.2317240814696432e-21,
-                    "timestamp": 0.8919894171018328
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 2.546248641381176e-40,
-                    "timestamp": 0.9910993523353697
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -17403,16 +4201,16 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S2 To W3": {
+        "C2 To W3": {
             "waypoints": [
                 {
-                    "x": 4.263,
-                    "y": 5.56,
+                    "x": 7.68,
+                    "y": 5.797,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 13
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 2.3,
@@ -17424,134 +4222,7 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": -1.339537382865945e-32,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.215219135134553,
-                    "y": 5.5307922107130985,
-                    "heading": 3.1269908374623143,
-                    "angularVelocity": -0.1585720754034668,
-                    "velocityX": -0.582413424718004,
-                    "velocityY": -0.3560213620019152,
-                    "timestamp": 0.08203942910241563
-                },
-                {
-                    "x": 4.119657409619225,
-                    "y": 5.472376532354226,
-                    "heading": 3.100976599590504,
-                    "angularVelocity": -0.3170943308166476,
-                    "velocityX": -1.1648267980513523,
-                    "velocityY": -0.7120439403100625,
-                    "timestamp": 0.16407885820483126
-                },
-                {
-                    "x": 3.976314836365869,
-                    "y": 5.384752798145142,
-                    "heading": 3.0619633632091103,
-                    "angularVelocity": -0.4755425142304533,
-                    "velocityX": -1.7472400139987678,
-                    "velocityY": -1.0680685515217063,
-                    "timestamp": 0.24611828730724689
-                },
-                {
-                    "x": 3.785191463205027,
-                    "y": 5.267920772243767,
-                    "heading": 3.009957765442303,
-                    "angularVelocity": -0.6339097960065634,
-                    "velocityX": -2.3296526469272343,
-                    "velocityY": -1.4240960374739504,
-                    "timestamp": 0.3281577164096625
-                },
-                {
-                    "x": 3.546287409634579,
-                    "y": 5.121880161282017,
-                    "heading": 2.944964910348519,
-                    "angularVelocity": -0.7922148630830776,
-                    "velocityX": -2.9120638232648712,
-                    "velocityY": -1.780127099366288,
-                    "timestamp": 0.41019714551207814
-                },
-                {
-                    "x": 3.281494622858476,
-                    "y": 4.960009610093287,
-                    "heading": 2.9449649021355118,
-                    "angularVelocity": -1.0011048865301711e-7,
-                    "velocityX": -3.227628344969881,
-                    "velocityY": -1.9730823697792366,
-                    "timestamp": 0.49223657461449377
-                },
-                {
-                    "x": 3.0167018363816,
-                    "y": 4.798139058415072,
-                    "heading": 2.9449648939225046,
-                    "angularVelocity": -1.0011048873886247e-7,
-                    "velocityX": -3.2276283413225157,
-                    "velocityY": -1.9730823757457077,
-                    "timestamp": 0.5742760037169095
-                },
-                {
-                    "x": 2.777800999846863,
-                    "y": 4.652092907401912,
-                    "heading": 2.879984644909019,
-                    "angularVelocity": -0.7920612042822246,
-                    "velocityX": -2.912024609977479,
-                    "velocityY": -1.7801946284979562,
-                    "timestamp": 0.656315432819325
-                },
-                {
-                    "x": 2.58668051019359,
-                    "y": 4.5352558204412405,
-                    "heading": 2.827994144000888,
-                    "angularVelocity": -0.6337257764583869,
-                    "velocityX": -2.329617499101367,
-                    "velocityY": -1.424157728045816,
-                    "timestamp": 0.7383548619217406
-                },
-                {
-                    "x": 2.443340235924958,
-                    "y": 4.447627930217013,
-                    "heading": 2.788997784467408,
-                    "angularVelocity": -0.4753367979291705,
-                    "velocityX": -1.7472119910743198,
-                    "velocityY": -1.0681192102743058,
-                    "timestamp": 0.8203942910241562
-                },
-                {
-                    "x": 2.3477800812496348,
-                    "y": 4.389209311628481,
-                    "heading": 2.7629991294364147,
-                    "angularVelocity": -0.31690438750539845,
-                    "velocityX": -1.1648076506728071,
-                    "velocityY": -0.7120797795363016,
-                    "timestamp": 0.9024337201265717
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": -0.1584497793150997,
-                    "velocityX": -0.5824038730204719,
-                    "velocityY": -0.35603991822050945,
-                    "timestamp": 0.9844731492289873
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": -7.396671024736362e-34,
-                    "velocityX": -1.1837806823878881e-33,
-                    "velocityY": 0,
-                    "timestamp": 1.066512578331403
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -17571,299 +4242,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S2 To C1 To S1": {
+        "C2 To S1": {
             "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
                 {
                     "x": 7.68,
-                    "y": 7.467,
+                    "y": 5.797,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 13
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": 3.4031853071795863,
+                    "heading": -2.88,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": -8.153039521410402e-32,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.331992841648679,
-                    "y": 5.5918650858232235,
-                    "heading": 3.133856142863887,
-                    "angularVelocity": -0.06434641956265956,
-                    "velocityX": 0.7225822862075633,
-                    "velocityY": 0.3337323991032911,
-                    "timestamp": 0.09548094793574066
-                },
-                {
-                    "x": 4.469978522807239,
-                    "y": 5.655595256777472,
-                    "heading": 3.121568622421363,
-                    "angularVelocity": -0.12869080908995395,
-                    "velocityX": 1.445164550014868,
-                    "velocityY": 0.6674647909569773,
-                    "timestamp": 0.19096189587148132
-                },
-                {
-                    "x": 4.676957039051013,
-                    "y": 5.751190511387417,
-                    "heading": 3.1031377745555058,
-                    "angularVelocity": -0.19303168081514768,
-                    "velocityX": 2.1677467674813493,
-                    "velocityY": 1.0011971673591054,
-                    "timestamp": 0.28644284380722196
-                },
-                {
-                    "x": 4.9529283757083125,
-                    "y": 5.878650843878014,
-                    "heading": 3.078564198952369,
-                    "angularVelocity": -0.2573662718522563,
-                    "velocityX": 2.890328831286909,
-                    "velocityY": 1.334929483277519,
-                    "timestamp": 0.38192379174296265
-                },
-                {
-                    "x": 5.280841898732695,
-                    "y": 6.030101358943919,
-                    "heading": 3.0785641960647663,
-                    "angularVelocity": -3.024270852224024e-8,
-                    "velocityX": 3.4343345988250538,
-                    "velocityY": 1.5861857086708926,
-                    "timestamp": 0.47740473967870334
-                },
-                {
-                    "x": 5.608755421663104,
-                    "y": 6.181551874214712,
-                    "heading": 3.078564193177445,
-                    "angularVelocity": -3.023976799325844e-8,
-                    "velocityX": 3.434334597840841,
-                    "velocityY": 1.5861857108167463,
-                    "timestamp": 0.572885687614444
-                },
-                {
-                    "x": 5.936668944593511,
-                    "y": 6.333002389485506,
-                    "heading": 3.0785641902901233,
-                    "angularVelocity": -3.023976796721421e-8,
-                    "velocityX": 3.434334597840832,
-                    "velocityY": 1.5861857108167654,
-                    "timestamp": 0.6683666355501847
-                },
-                {
-                    "x": 6.26458246752392,
-                    "y": 6.484452904756295,
-                    "heading": 3.078564187402802,
-                    "angularVelocity": -3.023976775003668e-8,
-                    "velocityX": 3.434334597840855,
-                    "velocityY": 1.5861857108167163,
-                    "timestamp": 0.7638475834859254
-                },
-                {
-                    "x": 6.592495990360559,
-                    "y": 6.635903420230111,
-                    "heading": 3.0785641845154803,
-                    "angularVelocity": -3.02397677343441e-8,
-                    "velocityX": 3.434334596858777,
-                    "velocityY": 1.586185712943067,
-                    "timestamp": 0.8593285314216661
-                },
-                {
-                    "x": 6.920387522031495,
-                    "y": 6.787401540858459,
-                    "heading": 3.0785641816279576,
-                    "angularVelocity": -3.0241868882472936e-8,
-                    "velocityX": 3.4341042769240095,
-                    "velocityY": 1.5866842957027985,
-                    "timestamp": 0.9548094793574068
-                },
-                {
-                    "x": 7.210301641874205,
-                    "y": 6.987038475991185,
-                    "heading": 3.078564386301996,
-                    "angularVelocity": 0.000002143611293414107,
-                    "velocityX": 3.036355693052319,
-                    "velocityY": 2.0908562330876395,
-                    "timestamp": 1.0502904272931475
-                },
-                {
-                    "x": 7.431218296435779,
-                    "y": 7.154820398489436,
-                    "heading": 3.084707768304825,
-                    "angularVelocity": 0.06434144335228988,
-                    "velocityX": 2.3137249821846093,
-                    "velocityY": 1.757229333447492,
-                    "timestamp": 1.145771375228888
-                },
-                {
-                    "x": 7.58313979167575,
-                    "y": 7.2907422724167805,
-                    "heading": 3.096994605047932,
-                    "angularVelocity": 0.12868364850522696,
-                    "velocityX": 1.5911184223080599,
-                    "velocityY": 1.4235496909665903,
-                    "timestamp": 1.2412523231646286
-                },
-                {
-                    "x": 7.666066897511305,
-                    "y": 7.39480241917028,
-                    "heading": 3.11542521266587,
-                    "angularVelocity": 0.19302916462815903,
-                    "velocityX": 0.8685199260002638,
-                    "velocityY": 1.0898524679843054,
-                    "timestamp": 1.3367332711003692
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 3.14,
-                    "angularVelocity": 0.2573789626666942,
-                    "velocityX": 0.145925472986578,
-                    "velocityY": 0.756146460527717,
-                    "timestamp": 1.4322142190361098
-                },
-                {
-                    "x": 7.628765464766867,
-                    "y": 7.5070371939069025,
-                    "heading": 3.1695683378543595,
-                    "angularVelocity": 0.3197138219543324,
-                    "velocityX": -0.5539841013770347,
-                    "velocityY": 0.4329105121655262,
-                    "timestamp": 1.5246979836528902
-                },
-                {
-                    "x": 7.512802856979312,
-                    "y": 7.517175485453599,
-                    "heading": 3.204897367018404,
-                    "angularVelocity": 0.3820024986055476,
-                    "velocityX": -1.25386989022506,
-                    "velocityY": 0.10962239252574857,
-                    "timestamp": 1.6171817482696706
-                },
-                {
-                    "x": 7.3321158425333,
-                    "y": 7.497406833459743,
-                    "heading": 3.2459797295297097,
-                    "angularVelocity": 0.44421161575322776,
-                    "velocityX": -1.9537160408082612,
-                    "velocityY": -0.21375267405386408,
-                    "timestamp": 1.709665512886451
-                },
-                {
-                    "x": 7.086711752516261,
-                    "y": 7.447715159830384,
-                    "heading": 3.292800204439529,
-                    "angularVelocity": 0.5062561532175872,
-                    "velocityX": -2.6534829224793675,
-                    "velocityY": -0.5373015883889657,
-                    "timestamp": 1.8021492775032315
-                },
-                {
-                    "x": 6.7766125854356485,
-                    "y": 7.3680522721143955,
-                    "heading": 3.345311930276191,
-                    "angularVelocity": 0.5677939912444252,
-                    "velocityX": -3.3530119406998606,
-                    "velocityY": -0.8613715936546359,
-                    "timestamp": 1.894633042120012
-                },
-                {
-                    "x": 6.4587265246711105,
-                    "y": 7.22193286725247,
-                    "heading": 3.3453119331318786,
-                    "angularVelocity": 3.087771911901371e-8,
-                    "velocityX": -3.437209353240929,
-                    "velocityY": -1.5799465502853158,
-                    "timestamp": 1.9871168067367924
-                },
-                {
-                    "x": 6.1410584209016115,
-                    "y": 7.075340220973225,
-                    "heading": 3.3453119359389976,
-                    "angularVelocity": 3.035256115645747e-8,
-                    "velocityX": -3.434852647767093,
-                    "velocityY": -1.5850635718263661,
-                    "timestamp": 2.079600571353573
-                },
-                {
-                    "x": 5.823390305014834,
-                    "y": 6.928747600950052,
-                    "heading": 3.3453119387456907,
-                    "angularVelocity": 3.034795349843415e-8,
-                    "velocityX": -3.4348527787877012,
-                    "velocityY": -1.5850632879271098,
-                    "timestamp": 2.1720843359703537
-                },
-                {
-                    "x": 5.564434200353795,
-                    "y": 6.809248569525128,
-                    "heading": 3.3684615293972238,
-                    "angularVelocity": 0.2503097786643198,
-                    "velocityX": -2.8000169082002904,
-                    "velocityY": -1.292108208616342,
-                    "timestamp": 2.2645681005871343
-                },
-                {
-                    "x": 5.3702171046964775,
-                    "y": 6.719624287090389,
-                    "heading": 3.3858234872314386,
-                    "angularVelocity": 0.18772979134186565,
-                    "velocityX": -2.1000128667129405,
-                    "velocityY": -0.9690812523282153,
-                    "timestamp": 2.357051865203915
-                },
-                {
-                    "x": 5.240739035958368,
-                    "y": 6.659874762854457,
-                    "heading": 3.3973980568576057,
-                    "angularVelocity": 0.1251524489081645,
-                    "velocityX": -1.4000086315106373,
-                    "velocityY": -0.6460541964699718,
-                    "timestamp": 2.4495356298206956
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": 0.06257585151184072,
-                    "velocityX": -0.7000043329401022,
-                    "velocityY": -0.3230271061984462,
-                    "timestamp": 2.5420193944374763
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": 1.89050013704423e-29,
-                    "velocityX": -9.26196849914815e-31,
-                    "velocityY": -1.067270908871219e-31,
-                    "timestamp": 2.634503159054257
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -17883,317 +4283,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S2 To C1 To S2": {
+        "C2 To S2": {
             "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -6.721620698039908e-29,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.32871710747576,
-                    "y": 5.596676184944768,
-                    "heading": 3.14,
-                    "angularVelocity": -1.6840159888455038e-16,
-                    "velocityX": 0.6917264182881007,
-                    "velocityY": 0.3860469065482612,
-                    "timestamp": 0.09500447827150149
-                },
-                {
-                    "x": 4.460151319654045,
-                    "y": 5.670028553286586,
-                    "heading": 3.14,
-                    "angularVelocity": -1.2818077527297718e-16,
-                    "velocityX": 1.3834528073858645,
-                    "velocityY": 0.772093796805632,
-                    "timestamp": 0.19000895654300298
-                },
-                {
-                    "x": 4.657302630988413,
-                    "y": 5.780057101930027,
-                    "heading": 3.14,
-                    "angularVelocity": -1.8176327661301372e-16,
-                    "velocityX": 2.0751791381032545,
-                    "velocityY": 1.1581406544813895,
-                    "timestamp": 0.2850134348145045
-                },
-                {
-                    "x": 4.920171024839782,
-                    "y": 5.926761821588956,
-                    "heading": 3.14,
-                    "angularVelocity": -2.1864698969841676e-16,
-                    "velocityX": 2.766905293682386,
-                    "velocityY": 1.5441874144139043,
-                    "timestamp": 0.38001791308600597
-                },
-                {
-                    "x": 5.23400140491651,
-                    "y": 6.101908012635582,
-                    "heading": 3.14,
-                    "angularVelocity": 2.4726801201282114e-16,
-                    "velocityX": 3.3033219674134005,
-                    "velocityY": 1.8435572115473686,
-                    "timestamp": 0.47502239135750746
-                },
-                {
-                    "x": 5.547831784993484,
-                    "y": 6.277054203682345,
-                    "heading": 3.14,
-                    "angularVelocity": 3.7708427520389947e-17,
-                    "velocityX": 3.303321967416651,
-                    "velocityY": 1.843557211549182,
-                    "timestamp": 0.570026869629009
-                },
-                {
-                    "x": 5.861662165070458,
-                    "y": 6.452200394729108,
-                    "heading": 3.14,
-                    "angularVelocity": 2.1976269787785873e-16,
-                    "velocityX": 3.3033219674166516,
-                    "velocityY": 1.843557211549183,
-                    "timestamp": 0.6650313479005104
-                },
-                {
-                    "x": 6.175492545147431,
-                    "y": 6.627346585775871,
-                    "heading": 3.14,
-                    "angularVelocity": 1.9941780105068909e-16,
-                    "velocityX": 3.3033219674166516,
-                    "velocityY": 1.843557211549183,
-                    "timestamp": 0.7600358261720119
-                },
-                {
-                    "x": 6.489322925224406,
-                    "y": 6.802492776822635,
-                    "heading": 3.14,
-                    "angularVelocity": 1.915977022732522e-16,
-                    "velocityX": 3.3033219674166516,
-                    "velocityY": 1.8435572115491827,
-                    "timestamp": 0.8550403044435134
-                },
-                {
-                    "x": 6.803153305301396,
-                    "y": 6.977638967869407,
-                    "heading": 3.14,
-                    "angularVelocity": 1.0071749090943876e-16,
-                    "velocityX": 3.3033219674167866,
-                    "velocityY": 1.8435572115492582,
-                    "timestamp": 0.9500447827150149
-                },
-                {
-                    "x": 7.109956833123924,
-                    "y": 7.148863529636325,
-                    "heading": 3.14,
-                    "angularVelocity": 7.28120504001254e-16,
-                    "velocityX": 3.2293585881581666,
-                    "velocityY": 1.802278849171093,
-                    "timestamp": 1.0450492609865165
-                },
-                {
-                    "x": 7.351043278430091,
-                    "y": 7.283411920388113,
-                    "heading": 3.14,
-                    "angularVelocity": 8.519808203444012e-16,
-                    "velocityX": 2.5376324325879027,
-                    "velocityY": 1.4162320892435267,
-                    "timestamp": 1.140053739258018
-                },
-                {
-                    "x": 7.526412624580293,
-                    "y": 7.381284130838343,
-                    "heading": 3.14,
-                    "angularVelocity": 9.198517694714364e-16,
-                    "velocityX": 1.8459061018722946,
-                    "velocityY": 1.0301852315687638,
-                    "timestamp": 1.2350582175295193
-                },
-                {
-                    "x": 7.636064866028015,
-                    "y": 7.442480157891549,
-                    "heading": 3.14,
-                    "angularVelocity": 7.057693969312242e-16,
-                    "velocityX": 1.1541797127752547,
-                    "velocityY": 0.6441383413117968,
-                    "timestamp": 1.3300626958010207
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 3.14,
-                    "angularVelocity": 6.521841549416907e-16,
-                    "velocityX": 0.4624532944875416,
-                    "velocityY": 0.25809143476375196,
-                    "timestamp": 1.425067174072522
-                },
-                {
-                    "x": 7.649929424023421,
-                    "y": 7.450217855315383,
-                    "heading": 3.14,
-                    "angularVelocity": 4.173997569351088e-16,
-                    "velocityX": -0.2907017552877216,
-                    "velocityY": -0.16223829304468454,
-                    "timestamp": 1.5285085032844143
-                },
-                {
-                    "x": 7.5419514905723615,
-                    "y": 7.389956245982292,
-                    "heading": 3.14,
-                    "angularVelocity": 3.561755142449842e-16,
-                    "velocityX": -1.043856785955406,
-                    "velocityY": -0.5825680101893362,
-                    "timestamp": 1.6319498324963067
-                },
-                {
-                    "x": 7.356066203105843,
-                    "y": 7.286215173931179,
-                    "heading": 3.14,
-                    "angularVelocity": 4.0492399811570283e-16,
-                    "velocityX": -1.7970117831835097,
-                    "velocityY": -1.0028977086716289,
-                    "timestamp": 1.735391161708199
-                },
-                {
-                    "x": 7.092273569234474,
-                    "y": 7.138994643409464,
-                    "heading": 3.14,
-                    "angularVelocity": 3.5744526417986045e-16,
-                    "velocityX": -2.550166706837388,
-                    "velocityY": -1.4232273660927433,
-                    "timestamp": 1.8388324909200913
-                },
-                {
-                    "x": 6.7505736194163815,
-                    "y": 6.94829467141558,
-                    "heading": 3.14,
-                    "angularVelocity": 3.486084398579742e-18,
-                    "velocityX": -3.3033213360452667,
-                    "velocityY": -1.8435568591859304,
-                    "timestamp": 1.9422738201319836
-                },
-                {
-                    "x": 6.4088736018749985,
-                    "y": 6.757594661625877,
-                    "heading": 3.14,
-                    "angularVelocity": 3.295553774982102e-16,
-                    "velocityX": -3.303321990779068,
-                    "velocityY": -1.8435572245875573,
-                    "timestamp": 2.045715149343876
-                },
-                {
-                    "x": 6.0671735843335775,
-                    "y": 6.566894651836152,
-                    "heading": 3.14,
-                    "angularVelocity": -4.7007888222967745e-17,
-                    "velocityX": -3.303321990779443,
-                    "velocityY": -1.8435572245877663,
-                    "timestamp": 2.1491564785557684
-                },
-                {
-                    "x": 5.725473566792156,
-                    "y": 6.376194642046427,
-                    "heading": 3.14,
-                    "angularVelocity": -1.693931270443523e-16,
-                    "velocityX": -3.3033219907794424,
-                    "velocityY": -1.843557224587766,
-                    "timestamp": 2.252597807767661
-                },
-                {
-                    "x": 5.383773549250734,
-                    "y": 6.185494632256702,
-                    "heading": 3.14,
-                    "angularVelocity": -1.7179108161052583e-16,
-                    "velocityX": -3.3033219907794424,
-                    "velocityY": -1.8435572245877665,
-                    "timestamp": 2.3560391369795535
-                },
-                {
-                    "x": 5.042073531709293,
-                    "y": 5.994794622466965,
-                    "heading": 3.14,
-                    "angularVelocity": -2.8062613243046787e-16,
-                    "velocityX": -3.3033219907797844,
-                    "velocityY": -1.8435572245879575,
-                    "timestamp": 2.459480466191446
-                },
-                {
-                    "x": 4.730444135370463,
-                    "y": 5.820876782602128,
-                    "heading": 3.14,
-                    "angularVelocity": -4.299065406379285e-16,
-                    "velocityX": -3.0126197982407033,
-                    "velocityY": -1.6813186875168347,
-                    "timestamp": 2.5629217954033385
-                },
-                {
-                    "x": 4.496722071912365,
-                    "y": 5.690438393660192,
-                    "heading": 3.14,
-                    "angularVelocity": -4.936527887438678e-16,
-                    "velocityX": -2.2594650053308842,
-                    "velocityY": -1.2609891030629203,
-                    "timestamp": 2.666363124615231
-                },
-                {
-                    "x": 4.340907358243485,
-                    "y": 5.6034794650776485,
-                    "heading": 3.14,
-                    "angularVelocity": -3.0491715981392867e-16,
-                    "velocityX": -1.50631004895991,
-                    "velocityY": -0.8406594273826603,
-                    "timestamp": 2.7698044538271236
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
+                {
+                    "x": 7.68,
+                    "y": 5.797,
                     "heading": 3.14,
-                    "angularVelocity": -6.545212108880601e-17,
-                    "velocityX": -0.7531550381017611,
-                    "velocityY": -0.42032972129354973,
-                    "timestamp": 2.873245783039016
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 4.263,
                     "y": 5.56,
                     "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": -8.479397572029164e-30,
-                    "velocityY": 1.1992173369619903e-31,
-                    "timestamp": 2.9766871122509087
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 }
             ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -18213,17 +4324,8 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S2 To C2 To S1": {
+        "C2 To S3": {
             "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 14
-                },
                 {
                     "x": 7.68,
                     "y": 5.797,
@@ -18231,272 +4333,60 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 13
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 3.4031853071795863,
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
+            "trajectory": [],
+            "constraints": [
                 {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": -3.0990371742085845e-31,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.334488852780742,
-                    "y": 5.55372681466254,
-                    "heading": 3.133953491828579,
-                    "angularVelocity": -0.06516682898121992,
-                    "velocityX": 0.7704780529773917,
-                    "velocityY": -0.06760986415037558,
-                    "timestamp": 0.09278506052766491
-                },
-                {
-                    "x": 4.477466555007721,
-                    "y": 5.541180442989367,
-                    "heading": 3.121860441564243,
-                    "angularVelocity": -0.13033402355464482,
-                    "velocityX": 1.5409560700168046,
-                    "velocityY": -0.1352197390595365,
-                    "timestamp": 0.18557012105532983
-                },
-                {
-                    "x": 4.691933100191895,
-                    "y": 5.522360883581774,
-                    "heading": 3.1037208124155655,
-                    "angularVelocity": -0.19550161465122112,
-                    "velocityX": 2.311434017119952,
-                    "velocityY": -0.20282962904337912,
-                    "timestamp": 0.27835518158299477
-                },
-                {
-                    "x": 4.977888469750385,
-                    "y": 5.497268135402371,
-                    "heading": 3.079534623451408,
-                    "angularVelocity": -0.26066900023130835,
-                    "velocityX": 3.0819117639443303,
-                    "velocityY": -0.2704395302078152,
-                    "timestamp": 0.37114024211065966
-                },
-                {
-                    "x": 5.327545209579434,
-                    "y": 5.4665855253125155,
-                    "heading": 3.079534620495457,
-                    "angularVelocity": -3.185804655621534e-8,
-                    "velocityX": 3.768459467941971,
-                    "velocityY": -0.33068480976750836,
-                    "timestamp": 0.46392530263832454
-                },
-                {
-                    "x": 5.677201949407512,
-                    "y": 5.435902915209702,
-                    "heading": 3.0795346175395855,
-                    "angularVelocity": -3.185719286228054e-8,
-                    "velocityX": 3.768459467931527,
-                    "velocityY": -0.3306848099071595,
-                    "timestamp": 0.5567103631659894
-                },
-                {
-                    "x": 6.026858689235635,
-                    "y": 5.405220305107383,
-                    "heading": 3.0795346145837144,
-                    "angularVelocity": -3.185719241382707e-8,
-                    "velocityX": 3.7684594679319945,
-                    "velocityY": -0.3306848099018315,
-                    "timestamp": 0.6494954236936543
-                },
-                {
-                    "x": 6.376515440276988,
-                    "y": 5.37453782279048,
-                    "heading": 3.079534611627843,
-                    "angularVelocity": -3.1857194092858966e-8,
-                    "velocityX": 3.7684595887836787,
-                    "velocityY": -0.3306834326820888,
-                    "timestamp": 0.7422804842213192
-                },
-                {
-                    "x": 6.7274981570931205,
-                    "y": 5.371017863275757,
-                    "heading": 3.079534608531603,
-                    "angularVelocity": -3.337002331102787e-8,
-                    "velocityX": 3.782750313683133,
-                    "velocityY": -0.03793670548581862,
-                    "timestamp": 0.8350655447489841
-                },
-                {
-                    "x": 7.060187879890856,
-                    "y": 5.436910997452384,
-                    "heading": 3.079737215344131,
-                    "angularVelocity": 0.0021836145967468554,
-                    "velocityX": 3.585595794255326,
-                    "velocityY": 0.7101696523329738,
-                    "timestamp": 0.927850605276649
-                },
-                {
-                    "x": 7.322040445742604,
-                    "y": 5.514304234937123,
-                    "heading": 3.085826008048483,
-                    "angularVelocity": 0.065622554641053,
-                    "velocityX": 2.82214145642183,
-                    "velocityY": 0.834113132487145,
-                    "timestamp": 1.0206356658043139
-                },
-                {
-                    "x": 7.51268687185891,
-                    "y": 5.600628661192279,
-                    "heading": 3.097884063831068,
-                    "angularVelocity": 0.12995686712932528,
-                    "velocityX": 2.054710370743838,
-                    "velocityY": 0.9303698867493723,
-                    "timestamp": 1.1134207263319789
-                },
-                {
-                    "x": 7.632022870387607,
-                    "y": 5.695007866387598,
-                    "heading": 3.115937777513367,
-                    "angularVelocity": 0.19457565236926141,
-                    "velocityX": 1.2861553126119536,
-                    "velocityY": 1.0171810489597053,
-                    "timestamp": 1.2062057868596439
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "C2 To S4": {
+            "waypoints": [
                 {
                     "x": 7.68,
                     "y": 5.797,
                     "heading": 3.14,
-                    "angularVelocity": 0.25933293948166103,
-                    "velocityX": 0.517078173356003,
-                    "velocityY": 1.0992301242503861,
-                    "timestamp": 1.2989908473873089
-                },
-                {
-                    "x": 7.658324369192282,
-                    "y": 5.904176742987938,
-                    "heading": 3.16941072815558,
-                    "angularVelocity": 0.32296823834349714,
-                    "velocityX": -0.23802675880415525,
-                    "velocityY": 1.176940730303702,
-                    "timestamp": 1.3900546879260585
-                },
-                {
-                    "x": 7.56785439743373,
-                    "y": 6.01813279809926,
-                    "heading": 3.204362801369678,
-                    "angularVelocity": 0.3838194502596341,
-                    "velocityX": -0.9934785445386147,
-                    "velocityY": 1.2513864387569726,
-                    "timestamp": 1.481118528464808
-                },
-                {
-                    "x": 7.4085407741930185,
-                    "y": 6.138375580441298,
-                    "heading": 3.2444365070983814,
-                    "angularVelocity": 0.44006166983116235,
-                    "velocityX": -1.7494718243615388,
-                    "velocityY": 1.3204229212238012,
-                    "timestamp": 1.5721823690035577
-                },
-                {
-                    "x": 7.180297052881674,
-                    "y": 6.26393107037867,
-                    "heading": 3.2888027958474897,
-                    "angularVelocity": 0.48719984229357544,
-                    "velocityX": -2.5064144007215123,
-                    "velocityY": 1.3787633949387568,
-                    "timestamp": 1.6632462095423073
-                },
-                {
-                    "x": 6.882954936764855,
-                    "y": 6.39196860518662,
-                    "heading": 3.335055491791258,
-                    "angularVelocity": 0.5079150590414778,
-                    "velocityX": -3.2652050952131715,
-                    "velocityY": 1.406019491935018,
-                    "timestamp": 1.754310050081057
-                },
-                {
-                    "x": 6.546228290367871,
-                    "y": 6.464686559433002,
-                    "heading": 3.335055498171187,
-                    "angularVelocity": 7.005996108772343e-8,
-                    "velocityX": -3.697698717787989,
-                    "velocityY": 0.7985381883321813,
-                    "timestamp": 1.8453738906198065
-                },
-                {
-                    "x": 6.204219309221898,
-                    "y": 6.505949051941506,
-                    "heading": 3.3350555015810333,
-                    "angularVelocity": 3.7444564221531034e-8,
-                    "velocityX": -3.7557056579492913,
-                    "velocityY": 0.45311610255384965,
-                    "timestamp": 1.936437731158556
-                },
-                {
-                    "x": 5.862210306846827,
-                    "y": 6.547211368473936,
-                    "heading": 3.335055504991925,
-                    "angularVelocity": 3.7456047151170976e-8,
-                    "velocityX": -3.7557058910725254,
-                    "velocityY": 0.45311417010650673,
-                    "timestamp": 2.0275015716973055
-                },
-                {
-                    "x": 5.587726197097608,
-                    "y": 6.580326820073676,
-                    "heading": 3.3623069003974067,
-                    "angularVelocity": 0.29925594225114466,
-                    "velocityX": -3.0141943072610453,
-                    "velocityY": 0.3636509442587018,
-                    "timestamp": 2.118565412236055
-                },
-                {
-                    "x": 5.381863101775985,
-                    "y": 6.605163409773074,
-                    "heading": 3.382745950256302,
-                    "angularVelocity": 0.22444748363294062,
-                    "velocityX": -2.2606458733093975,
-                    "velocityY": 0.2727382191708659,
-                    "timestamp": 2.2096292527748043
-                },
-                {
-                    "x": 5.244621034490526,
-                    "y": 6.621721136542047,
-                    "heading": 3.3963721295427387,
-                    "angularVelocity": 0.1496332595442014,
-                    "velocityX": -1.5070972899178572,
-                    "velocityY": 0.18182548277135752,
-                    "timestamp": 2.3006930933135536
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": 0.07481759605718756,
-                    "velocityX": -0.7535486542688541,
-                    "velocityY": 0.09091274219244924,
-                    "timestamp": 2.391756933852303
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 5.176,
-                    "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": 0,
-                    "velocityX": -1.5663528740094776e-30,
-                    "velocityY": 4.877949363390491e-30,
-                    "timestamp": 2.4828207743910524
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 }
             ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -18516,17 +4406,8 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S2 To C2 To S2": {
+        "C2 To C1": {
             "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 14
-                },
                 {
                     "x": 7.68,
                     "y": 5.797,
@@ -18534,11 +4415,11 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 14
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 4.263,
-                    "y": 5.56,
+                    "x": 7.68,
+                    "y": 7.467,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
@@ -18546,269 +4427,171 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -2.9840248880147764e-34,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.335913751201177,
-                    "y": 5.5650572312071045,
-                    "heading": 3.14,
-                    "angularVelocity": -1.4550376605668876e-16,
-                    "velocityX": 0.7787859499921702,
-                    "velocityY": 0.0540158823962963,
-                    "timestamp": 0.09362489295282081
-                },
-                {
-                    "x": 4.481741250496669,
-                    "y": 5.575171693405826,
-                    "heading": 3.14,
-                    "angularVelocity": -2.6552600483679955e-16,
-                    "velocityX": 1.5575718668001757,
-                    "velocityY": 0.10803176249096925,
-                    "timestamp": 0.18724978590564162
-                },
-                {
-                    "x": 4.7004824916727515,
-                    "y": 5.590343386165186,
-                    "heading": 3.14,
-                    "angularVelocity": -3.921189080478138e-16,
-                    "velocityX": 2.3363577172399013,
-                    "velocityY": 0.1620476379823989,
-                    "timestamp": 0.2808746788584624
-                },
-                {
-                    "x": 4.992137456088294,
-                    "y": 5.6105723081922525,
-                    "heading": 3.14,
-                    "angularVelocity": -4.0854142114404067e-16,
-                    "velocityX": 3.1151433685752,
-                    "velocityY": 0.2160634996641273,
-                    "timestamp": 0.37449957181128324
-                },
-                {
-                    "x": 5.345466004271564,
-                    "y": 5.63507885367643,
-                    "heading": 3.14,
-                    "angularVelocity": 5.235558245992657e-16,
-                    "velocityX": 3.773873988420134,
-                    "velocityY": 0.2617524539817304,
-                    "timestamp": 0.4681244647641041
-                },
-                {
-                    "x": 5.69879455245494,
-                    "y": 5.6595853991606155,
-                    "heading": 3.14,
-                    "angularVelocity": -5.535576803898805e-16,
-                    "velocityX": 3.773873988421262,
-                    "velocityY": 0.2617524539818086,
-                    "timestamp": 0.5617493577169249
-                },
-                {
-                    "x": 6.052123100638315,
-                    "y": 5.6840919446448,
-                    "heading": 3.14,
-                    "angularVelocity": -2.4126594725597773e-16,
-                    "velocityX": 3.7738739884212626,
-                    "velocityY": 0.2617524539818086,
-                    "timestamp": 0.6553742506697458
-                },
-                {
-                    "x": 6.405451648821692,
-                    "y": 5.708598490128986,
-                    "heading": 3.14,
-                    "angularVelocity": 3.1966951315745e-16,
-                    "velocityX": 3.773873988421262,
-                    "velocityY": 0.26175245398180863,
-                    "timestamp": 0.7489991436225666
-                },
-                {
-                    "x": 6.7587801970050565,
-                    "y": 5.733105035613169,
-                    "heading": 3.14,
-                    "angularVelocity": -3.2948712369984396e-16,
-                    "velocityX": 3.773873988421149,
-                    "velocityY": 0.26175245398180075,
-                    "timestamp": 0.8426240365753874
-                },
-                {
-                    "x": 7.088851630801794,
-                    "y": 5.755998488879142,
-                    "heading": 3.14,
-                    "angularVelocity": -9.417571312555162e-16,
-                    "velocityX": 3.5254666081494603,
-                    "velocityY": 0.24452314490237434,
-                    "timestamp": 0.9362489295282083
-                },
-                {
-                    "x": 7.3460093413592045,
-                    "y": 5.773834712877417,
-                    "heading": 3.14,
-                    "angularVelocity": -8.090853196926974e-16,
-                    "velocityX": 2.746680956815585,
-                    "velocityY": 0.19050728322074467,
-                    "timestamp": 1.029873822481029
-                },
+            "trajectory": [],
+            "constraints": [
                 {
-                    "x": 7.530253310036057,
-                    "y": 5.786613706315055,
-                    "heading": 3.14,
-                    "angularVelocity": -6.531335051646392e-16,
-                    "velocityX": 1.9678951063762244,
-                    "velocityY": 0.13649140772934035,
-                    "timestamp": 1.1234987154338498
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
                 {
-                    "x": 7.641583530618614,
-                    "y": 5.79433546876108,
-                    "heading": 3.14,
-                    "angularVelocity": -6.271183503983396e-16,
-                    "velocityX": 1.1891091895683832,
-                    "velocityY": 0.08247552763467879,
-                    "timestamp": 1.2171236083866706
-                },
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "C2 To C3": {
+            "waypoints": [
                 {
                     "x": 7.68,
                     "y": 5.797,
                     "heading": 3.14,
-                    "angularVelocity": -6.05510209215834e-16,
-                    "velocityX": 0.41032323957630595,
-                    "velocityY": 0.028459645238388953,
-                    "timestamp": 1.3107485013394913
-                },
-                {
-                    "x": 7.6370908002198155,
-                    "y": 5.794023857082849,
-                    "heading": 3.14,
-                    "angularVelocity": -4.789725529265609e-16,
-                    "velocityX": -0.42651589559076464,
-                    "velocityY": -0.02958275307434032,
-                    "timestamp": 1.4113524887260582
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 7.509992248723152,
-                    "y": 5.785208417602395,
+                    "x": 7.68,
+                    "y": 4.127,
                     "heading": 3.14,
-                    "angularVelocity": -2.32893689231122e-16,
-                    "velocityX": -1.2633550100583235,
-                    "velocityY": -0.08762514995137034,
-                    "timestamp": 1.5119564761126252
-                },
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
                 {
-                    "x": 7.298704349055096,
-                    "y": 5.770553681804523,
-                    "heading": 3.14,
-                    "angularVelocity": -1.4093989000106693e-18,
-                    "velocityX": -2.1001940892878346,
-                    "velocityY": -0.14566754438432153,
-                    "timestamp": 1.6125604634991921
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
                 {
-                    "x": 7.003227108598371,
-                    "y": 5.75005965020129,
-                    "heading": 3.14,
-                    "angularVelocity": 2.7928007738396434e-16,
-                    "velocityX": -2.9370330951333345,
-                    "velocityY": -0.2037099337274247,
-                    "timestamp": 1.713164450885759
-                },
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "C2 To C4": {
+            "waypoints": [
                 {
-                    "x": 6.623560552157282,
-                    "y": 5.723726324513103,
+                    "x": 7.68,
+                    "y": 5.797,
                     "heading": 3.14,
-                    "angularVelocity": 4.2079673901381875e-16,
-                    "velocityX": -3.773871854424956,
-                    "velocityY": -0.2617523059697759,
-                    "timestamp": 1.813768438272326
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 6.243893779806084,
-                    "y": 5.6973929838495865,
+                    "x": 7.68,
+                    "y": 2.457,
                     "heading": 3.14,
-                    "angularVelocity": -4.939929195074519e-16,
-                    "velocityX": -3.773874000563654,
-                    "velocityY": -0.2617524548239938,
-                    "timestamp": 1.914372425658893
-                },
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
                 {
-                    "x": 5.864227007454884,
-                    "y": 5.67105964318607,
-                    "heading": 3.14,
-                    "angularVelocity": 2.0250011365009936e-16,
-                    "velocityX": -3.773874000563654,
-                    "velocityY": -0.26175245482399373,
-                    "timestamp": 2.01497641304546
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
                 {
-                    "x": 5.484560235103686,
-                    "y": 5.644726302522554,
-                    "heading": 3.14,
-                    "angularVelocity": -5.77312332256685e-16,
-                    "velocityX": -3.773874000563654,
-                    "velocityY": -0.2617524548239938,
-                    "timestamp": 2.115580400432027
-                },
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "C2 To C5": {
+            "waypoints": [
                 {
-                    "x": 5.104893462752507,
-                    "y": 5.61839296185904,
+                    "x": 7.68,
+                    "y": 5.797,
                     "heading": 3.14,
-                    "angularVelocity": -1.6679194482656825e-16,
-                    "velocityX": -3.773874000563452,
-                    "velocityY": -0.26175245482397974,
-                    "timestamp": 2.2161843878185943
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 4.768136096424167,
-                    "y": 5.595035778417478,
+                    "x": 7.68,
+                    "y": 0.787,
                     "heading": 3.14,
-                    "angularVelocity": 9.060703447293421e-16,
-                    "velocityX": -3.3473560549281536,
-                    "velocityY": -0.2321695595604253,
-                    "timestamp": 2.3167883752051615
-                },
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [],
+            "constraints": [
                 {
-                    "x": 4.515568053067083,
-                    "y": 5.577517889545478,
-                    "heading": 3.14,
-                    "angularVelocity": 6.700681499162007e-16,
-                    "velocityX": -2.51051722618707,
-                    "velocityY": -0.1741271825011227,
-                    "timestamp": 2.4173923625917286
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
                 {
-                    "x": 4.34718935210125,
-                    "y": 5.565839296589989,
-                    "heading": 3.14,
-                    "angularVelocity": 4.365333326959493e-16,
-                    "velocityX": -1.6736782044119611,
-                    "velocityY": -0.11608479205315635,
-                    "timestamp": 2.517996349978296
-                },
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "C3 To W1": {
+            "waypoints": [
                 {
-                    "x": 4.263,
-                    "y": 5.56,
+                    "x": 7.68,
+                    "y": 4.127,
                     "heading": 3.14,
-                    "angularVelocity": 2.0339545203820728e-16,
-                    "velocityX": -0.836839118292156,
-                    "velocityY": -0.05804239714230061,
-                    "timestamp": 2.618600337364863
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 2.71920432475143
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 }
             ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -18828,17 +4611,8 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S2 To C3 To S1": {
+        "C3 To W2": {
             "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
                 {
                     "x": 7.68,
                     "y": 4.127,
@@ -18846,308 +4620,19 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 16
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 3.4031853071795863,
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": -4.804532357029023e-32,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.321076149980653,
-                    "y": 5.523337393191978,
-                    "heading": 3.1351500057236232,
-                    "angularVelocity": -0.05343391557798715,
-                    "velocityX": 0.6398432489449108,
-                    "velocityY": -0.40392349462987753,
-                    "timestamp": 0.09076621512600157
-                },
-                {
-                    "x": 4.4372316676864765,
-                    "y": 5.45001014084527,
-                    "heading": 3.1261940931811654,
-                    "angularVelocity": -0.098670111230538,
-                    "velocityX": 1.2797219487953386,
-                    "velocityY": -0.807869450598055,
-                    "timestamp": 0.18153243025200314
-                },
-                {
-                    "x": 4.611470902956015,
-                    "y": 5.340015489147945,
-                    "heading": 3.1144364232978923,
-                    "angularVelocity": -0.1295379549202309,
-                    "velocityX": 1.9196485721880063,
-                    "velocityY": -1.2118457461802494,
-                    "timestamp": 0.2722986453780047
-                },
-                {
-                    "x": 4.843797638177826,
-                    "y": 5.193351042771614,
-                    "heading": 3.102754458759954,
-                    "angularVelocity": -0.1287038852696506,
-                    "velocityX": 2.5596168673475677,
-                    "velocityY": -1.6158484318502528,
-                    "timestamp": 0.36306486050400627
-                },
-                {
-                    "x": 5.1341460210960115,
-                    "y": 5.0100588028009865,
-                    "heading": 3.102754388459234,
-                    "angularVelocity": -7.745251732579077e-7,
-                    "velocityX": 3.1988596474483897,
-                    "velocityY": -2.0193883783319753,
-                    "timestamp": 0.45383107563000785
-                },
-                {
-                    "x": 5.424494292461022,
-                    "y": 4.826766239115272,
-                    "heading": 3.1027543844284144,
-                    "angularVelocity": -4.440880652615192e-8,
-                    "velocityX": 3.1988584184318825,
-                    "velocityY": -2.019391944803125,
-                    "timestamp": 0.5445972907560094
-                },
-                {
-                    "x": 5.714842563825266,
-                    "y": 4.643473675428344,
-                    "heading": 3.102754380397595,
-                    "angularVelocity": -4.440880657505379e-8,
-                    "velocityX": 3.1988584184234474,
-                    "velocityY": -2.019391944816487,
-                    "timestamp": 0.635363505882011
-                },
-                {
-                    "x": 6.005190835190169,
-                    "y": 4.460181111742461,
-                    "heading": 3.102754376366776,
-                    "angularVelocity": -4.4408806414514103e-8,
-                    "velocityX": 3.198858418430713,
-                    "velocityY": -2.019391944804976,
-                    "timestamp": 0.7261297210080125
-                },
-                {
-                    "x": 6.295539259817851,
-                    "y": 4.276888790835792,
-                    "heading": 3.1027543723359563,
-                    "angularVelocity": -4.440881138366432e-8,
-                    "velocityX": 3.1988601069750424,
-                    "velocityY": -2.019389270029866,
-                    "timestamp": 0.8168959361340141
-                },
-                {
-                    "x": 6.605220322395041,
-                    "y": 4.128578571418711,
-                    "heading": 3.1027543676059754,
-                    "angularVelocity": -5.211168523884132e-8,
-                    "velocityX": 3.4118538725812453,
-                    "velocityY": -1.633980432159657,
-                    "timestamp": 0.9076621512600157
-                },
-                {
-                    "x": 6.930843742926924,
-                    "y": 4.047062548737473,
-                    "heading": 3.096403794352487,
-                    "angularVelocity": -0.0699662671256465,
-                    "velocityX": 3.5874958549263174,
-                    "velocityY": -0.8980877143337577,
-                    "timestamp": 0.9984283663860173
-                },
-                {
-                    "x": 7.20268686077656,
-                    "y": 4.00824503573056,
-                    "heading": 3.096579244871637,
-                    "angularVelocity": 0.0019329936684692615,
-                    "velocityX": 2.9949813096454783,
-                    "velocityY": -0.4276647754125783,
-                    "timestamp": 1.0891945815120188
-                },
-                {
-                    "x": 7.418574834249914,
-                    "y": 4.009226418838573,
-                    "heading": 3.1038020925835856,
-                    "angularVelocity": 0.07957638976044962,
-                    "velocityX": 2.37850584794857,
-                    "velocityY": 0.010812207016131005,
-                    "timestamp": 1.1799607966380203
-                },
-                {
-                    "x": 7.577796617923462,
-                    "y": 4.04898509700118,
-                    "heading": 3.1182467242773515,
-                    "angularVelocity": 0.15914106007078685,
-                    "velocityX": 1.7541965747113617,
-                    "velocityY": 0.43803388857203507,
-                    "timestamp": 1.270727011764022
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": 0.23966269489643754,
-                    "velocityX": 1.1260068730932273,
-                    "velocityY": 0.8595147752996188,
-                    "timestamp": 1.3614932268900235
-                },
-                {
-                    "x": 7.725157318651882,
-                    "y": 4.240486437949111,
-                    "heading": 3.168528121789897,
-                    "angularVelocity": 0.31944830036475336,
-                    "velocityX": 0.505656446597267,
-                    "velocityY": 1.2707829132351232,
-                    "timestamp": 1.4507975736570093
-                },
-                {
-                    "x": 7.714677077614687,
-                    "y": 4.390346197703396,
-                    "heading": 3.203767814945465,
-                    "angularVelocity": 0.39460221625624564,
-                    "velocityX": -0.11735420969529425,
-                    "velocityY": 1.6780791213366064,
-                    "timestamp": 1.5401019204239952
-                },
-                {
-                    "x": 7.648174313953831,
-                    "y": 4.575993018943536,
-                    "heading": 3.2450393487188225,
-                    "angularVelocity": 0.4621447361464423,
-                    "velocityX": -0.7446755512850395,
-                    "velocityY": 2.0788105838177287,
-                    "timestamp": 1.629406267190981
-                },
-                {
-                    "x": 7.524918843988051,
-                    "y": 4.7962734130800495,
-                    "heading": 3.2910178393067144,
-                    "angularVelocity": 0.5148516533899635,
-                    "velocityX": -1.3801732438330052,
-                    "velocityY": 2.4666256695351096,
-                    "timestamp": 1.7187106139579669
-                },
-                {
-                    "x": 7.343009789801653,
-                    "y": 5.047887037555876,
-                    "heading": 3.3379953942154983,
-                    "angularVelocity": 0.5260388391996106,
-                    "velocityX": -2.0369563271207567,
-                    "velocityY": 2.817484630757546,
-                    "timestamp": 1.8080149607249527
-                },
-                {
-                    "x": 7.098537759361482,
-                    "y": 5.281049171644365,
-                    "heading": 3.337995405344767,
-                    "angularVelocity": 1.2462180234431456e-7,
-                    "velocityX": -2.7375154658266756,
-                    "velocityY": 2.6108710553234187,
-                    "timestamp": 1.8973193074919386
-                },
-                {
-                    "x": 6.821988530708413,
-                    "y": 5.475089583706822,
-                    "heading": 3.337995410181252,
-                    "angularVelocity": 5.415732950343717e-8,
-                    "velocityX": -3.096705128750813,
-                    "velocityY": 2.1727991871297463,
-                    "timestamp": 1.9866236542589244
-                },
-                {
-                    "x": 6.5454389358432925,
-                    "y": 5.669129473837467,
-                    "heading": 3.3379954150177356,
-                    "angularVelocity": 5.4157314847566374e-8,
-                    "velocityX": -3.0967092294700773,
-                    "velocityY": 2.172793342713053,
-                    "timestamp": 2.0759280010259102
-                },
-                {
-                    "x": 6.268889340976521,
-                    "y": 5.86316936396576,
-                    "heading": 3.337995419854219,
-                    "angularVelocity": 5.415731402245697e-8,
-                    "velocityX": -3.096709229488555,
-                    "velocityY": 2.172793342686719,
-                    "timestamp": 2.165232347792896
-                },
-                {
-                    "x": 5.9923397461188825,
-                    "y": 6.057209254107063,
-                    "heading": 3.337995424690707,
-                    "angularVelocity": 5.415735914657007e-8,
-                    "velocityX": -3.0967092293862986,
-                    "velocityY": 2.1727933428323993,
-                    "timestamp": 2.2545366945598815
-                },
-                {
-                    "x": 5.720218360303881,
-                    "y": 6.248146363272753,
-                    "heading": 3.35123648127501,
-                    "angularVelocity": 0.14826889242971145,
-                    "velocityX": -3.04712363581837,
-                    "velocityY": 2.1380494463935373,
-                    "timestamp": 2.343841041326867
-                },
-                {
-                    "x": 5.5025203930179725,
-                    "y": 6.400895625132601,
-                    "heading": 3.369358291202865,
-                    "angularVelocity": 0.20292192467559766,
-                    "velocityX": -2.437708523348032,
-                    "velocityY": 1.7104347928147705,
-                    "timestamp": 2.4331453880938527
-                },
-                {
-                    "x": 5.339255695066747,
-                    "y": 6.515451087573219,
-                    "heading": 3.385376867292448,
-                    "angularVelocity": 0.17937062046238467,
-                    "velocityX": -1.828183104874154,
-                    "velocityY": 1.28275348947479,
-                    "timestamp": 2.5224497348608383
-                },
-                {
-                    "x": 5.230417322474398,
-                    "y": 6.591817929626251,
-                    "heading": 3.3970262318270823,
-                    "angularVelocity": 0.1304456608929641,
-                    "velocityX": -1.218735442702819,
-                    "velocityY": 0.8551301791870324,
-                    "timestamp": 2.611754081627824
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": 0.06896725160058048,
-                    "velocityX": -0.6093468509028436,
-                    "velocityY": 0.42754996543868296,
-                    "timestamp": 2.7010584283948096
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": 2.6480923788288046e-29,
-                    "velocityX": -1.5869679624644047e-31,
-                    "velocityY": 0,
-                    "timestamp": 2.790362775161795
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -19167,17 +4652,8 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S2 To C3 To S2": {
+        "C3 To W3": {
             "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
                 {
                     "x": 7.68,
                     "y": 4.127,
@@ -19185,299 +4661,60 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 15
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -2.7122542067664996e-31,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.329648409276986,
-                    "y": 5.532049408693613,
-                    "heading": 3.14,
-                    "angularVelocity": 3.6127527846474837e-17,
-                    "velocityX": 0.715878164292546,
-                    "velocityY": -0.3002204885663498,
-                    "timestamp": 0.09310021257998485
-                },
-                {
-                    "x": 4.462945224981569,
-                    "y": 5.4761482272758,
-                    "heading": 3.14,
-                    "angularVelocity": 1.058102916611751e-16,
-                    "velocityX": 1.4317562979794687,
-                    "velocityY": -0.6004409642975056,
-                    "timestamp": 0.1862004251599697
-                },
-                {
-                    "x": 4.662890441407609,
-                    "y": 5.392296458139565,
-                    "heading": 3.14,
-                    "angularVelocity": 1.669202321487025e-16,
-                    "velocityX": 2.147634370376114,
-                    "velocityY": -0.9006614143251297,
-                    "timestamp": 0.27930063773995456
-                },
-                {
-                    "x": 4.929484041376741,
-                    "y": 5.280494108489063,
-                    "heading": 3.14,
-                    "angularVelocity": 1.646239395597301e-16,
-                    "velocityX": 2.8635122582579458,
-                    "velocityY": -1.2008817869720905,
-                    "timestamp": 0.3724008503199394
-                },
-                {
-                    "x": 5.2542718637391,
-                    "y": 5.1442866313321245,
-                    "heading": 3.14,
-                    "angularVelocity": -2.6454281962617153e-17,
-                    "velocityX": 3.4885830371581816,
-                    "velocityY": -1.4630200445559467,
-                    "timestamp": 0.46550106289992427
-                },
-                {
-                    "x": 5.579059685941281,
-                    "y": 5.008079154242361,
-                    "heading": 3.14,
-                    "angularVelocity": -9.771658175099026e-17,
-                    "velocityX": 3.4885830354377245,
-                    "velocityY": -1.463020043834432,
-                    "timestamp": 0.5586012754799091
-                },
-                {
-                    "x": 5.903847508143462,
-                    "y": 4.871871677152596,
-                    "heading": 3.14,
-                    "angularVelocity": -3.1326214825920436e-16,
-                    "velocityX": 3.488583035437725,
-                    "velocityY": -1.4630200438344323,
-                    "timestamp": 0.6517014880598939
-                },
-                {
-                    "x": 6.228635330345643,
-                    "y": 4.735664200062831,
-                    "heading": 3.14,
-                    "angularVelocity": -1.2137930063882497e-16,
-                    "velocityX": 3.488583035437725,
-                    "velocityY": -1.4630200438344325,
-                    "timestamp": 0.7448017006398788
-                },
-                {
-                    "x": 6.553423152547824,
-                    "y": 4.599456722973066,
-                    "heading": 3.14,
-                    "angularVelocity": 1.3394779600477518e-16,
-                    "velocityX": 3.488583035437725,
-                    "velocityY": -1.4630200438344325,
-                    "timestamp": 0.8379019132198637
-                },
-                {
-                    "x": 6.878210974643575,
-                    "y": 4.463249245927935,
-                    "heading": 3.14,
-                    "angularVelocity": 2.91910120342859e-17,
-                    "velocityX": 3.488583034294527,
-                    "velocityY": -1.4630200433550054,
-                    "timestamp": 0.9310021257998486
-                },
-                {
-                    "x": 7.17186557168986,
-                    "y": 4.3400982252760985,
-                    "heading": 3.14,
-                    "angularVelocity": -6.102921228742647e-16,
-                    "velocityX": 3.1541775137602737,
-                    "velocityY": -1.3227791563413718,
-                    "timestamp": 1.0241023383798336
-                },
-                {
-                    "x": 7.3988717849138865,
-                    "y": 4.2448977852556045,
-                    "heading": 3.14,
-                    "angularVelocity": -6.383129451815479e-16,
-                    "velocityX": 2.4382996228798555,
-                    "velocityY": -1.0225587824368827,
-                    "timestamp": 1.1172025509598185
-                },
+            "trajectory": [],
+            "constraints": [
                 {
-                    "x": 7.559229597346586,
-                    "y": 4.1776479329828335,
-                    "heading": 3.14,
-                    "angularVelocity": -7.263779621185202e-16,
-                    "velocityX": 1.7224215497327395,
-                    "velocityY": -0.7223383320945309,
-                    "timestamp": 1.2103027635398034
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
                 {
-                    "x": 7.65293900332055,
-                    "y": 4.138348670834548,
-                    "heading": 3.14,
-                    "angularVelocity": -7.150088563233795e-16,
-                    "velocityX": 1.0065434157113666,
-                    "velocityY": -0.42211785622311543,
-                    "timestamp": 1.3034029761197883
-                },
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "C3 To S1": {
+            "waypoints": [
                 {
                     "x": 7.68,
                     "y": 4.127,
                     "heading": 3.14,
-                    "angularVelocity": -7.092042046536786e-16,
-                    "velocityX": 0.2906652512305344,
-                    "velocityY": -0.12189736757780324,
-                    "timestamp": 1.3965031886997732
-                },
-                {
-                    "x": 7.634550444733744,
-                    "y": 4.146060349047862,
-                    "heading": 3.14,
-                    "angularVelocity": -8.252913531228688e-16,
-                    "velocityX": -0.4634351183891671,
-                    "velocityY": 0.19435250940932913,
-                    "timestamp": 1.4945742097824644
-                },
-                {
-                    "x": 7.515145498014414,
-                    "y": 4.196135645696618,
-                    "heading": 3.14,
-                    "angularVelocity": -8.640334878143468e-16,
-                    "velocityX": -1.217535469712484,
-                    "velocityY": 0.5106023787234387,
-                    "timestamp": 1.5926452308651555
-                },
-                {
-                    "x": 7.321785162832602,
-                    "y": 4.277225888692093,
-                    "heading": 3.14,
-                    "angularVelocity": -7.792870163094326e-16,
-                    "velocityX": -1.971635790541653,
-                    "velocityY": 0.826852235249104,
-                    "timestamp": 1.6907162519478467
-                },
-                {
-                    "x": 7.054469445169555,
-                    "y": 4.389331075525908,
-                    "heading": 3.14,
-                    "angularVelocity": -9.085136126677818e-16,
-                    "velocityX": -2.725736050381882,
-                    "velocityY": 1.1431020661976106,
-                    "timestamp": 1.7887872730305379
-                },
-                {
-                    "x": 6.713198362969528,
-                    "y": 4.532451198672714,
-                    "heading": 3.14,
-                    "angularVelocity": -7.684534164287992e-16,
-                    "velocityX": -3.4798361272500533,
-                    "velocityY": 1.4593518204124447,
-                    "timestamp": 1.886858294113229
-                },
-                {
-                    "x": 6.371069462531013,
-                    "y": 4.675931068244968,
-                    "heading": 3.14,
-                    "angularVelocity": -1.8367347430029744e-16,
-                    "velocityX": -3.4885830356557705,
-                    "velocityY": 1.4630200439258751,
-                    "timestamp": 1.9849293151959202
-                },
-                {
-                    "x": 6.028940562092558,
-                    "y": 4.819410937817198,
-                    "heading": 3.14,
-                    "angularVelocity": 1.6315422621813397e-16,
-                    "velocityX": -3.48858303565516,
-                    "velocityY": 1.463020043925619,
-                    "timestamp": 2.0830003362786114
-                },
-                {
-                    "x": 5.686811661654102,
-                    "y": 4.962890807389427,
-                    "heading": 3.14,
-                    "angularVelocity": -1.6415107007869627e-16,
-                    "velocityX": -3.4885830356551595,
-                    "velocityY": 1.4630200439256191,
-                    "timestamp": 2.1810713573613025
-                },
-                {
-                    "x": 5.344682761215647,
-                    "y": 5.106370676961656,
-                    "heading": 3.14,
-                    "angularVelocity": -2.6663466601394013e-16,
-                    "velocityX": -3.48858303565516,
-                    "velocityY": 1.4630200439256194,
-                    "timestamp": 2.2791423784439937
-                },
-                {
-                    "x": 5.002553860860093,
-                    "y": 5.249850546499118,
-                    "heading": 3.14,
-                    "angularVelocity": -2.7119156877069447e-16,
-                    "velocityX": -3.4885830348098277,
-                    "velocityY": 1.4630200435711092,
-                    "timestamp": 2.377213399526685
-                },
-                {
-                    "x": 4.706732333820954,
-                    "y": 5.373910320642252,
-                    "heading": 3.14,
-                    "angularVelocity": 2.3228605057035087e-16,
-                    "velocityX": -3.01640100993532,
-                    "velocityY": 1.264999311453705,
-                    "timestamp": 2.475284420609376
-                },
-                {
-                    "x": 4.4848661713885285,
-                    "y": 5.466955158443148,
-                    "heading": 3.14,
-                    "angularVelocity": 1.3635627120067326e-16,
-                    "velocityX": -2.262300932356051,
-                    "velocityY": 0.9487495569406552,
-                    "timestamp": 2.573355441692067
-                },
-                {
-                    "x": 4.336955391458343,
-                    "y": 5.528985052396896,
-                    "heading": 3.14,
-                    "angularVelocity": 8.074174127249248e-18,
-                    "velocityX": -1.5082006723016521,
-                    "velocityY": 0.6324997259023314,
-                    "timestamp": 2.6714264627747584
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 3.321022798774287e-17,
-                    "velocityX": -0.7541003513770345,
-                    "velocityY": 0.31624986933663735,
-                    "timestamp": 2.7694974838574495
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": -1.5033553036563547e-32,
-                    "velocityY": 0,
-                    "timestamp": 2.8675685049401407
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 }
             ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -19497,17 +4734,8 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S2 To C3 To S3": {
+        "C3 To S2": {
             "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
                 {
                     "x": 7.68,
                     "y": 4.127,
@@ -19515,317 +4743,19 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 17
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
+                    "y": 5.56,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": -2.3834052223810005e-28,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.3290756213238994,
-                    "y": 5.551919291811216,
-                    "heading": 3.150896767326327,
-                    "angularVelocity": 0.12189104355668522,
-                    "velocityX": 0.7391207131092994,
-                    "velocityY": -0.09039065663175272,
-                    "timestamp": 0.08939760468350924
-                },
-                {
-                    "x": 4.461244485010799,
-                    "y": 5.535758895626619,
-                    "heading": 3.171238121679265,
-                    "angularVelocity": 0.22753802436807322,
-                    "velocityX": 1.478438534845478,
-                    "velocityY": -0.18076990140321392,
-                    "timestamp": 0.17879520936701848
-                },
-                {
-                    "x": 4.65953073652644,
-                    "y": 5.511521259157241,
-                    "heading": 3.1985778553739457,
-                    "angularVelocity": 0.30582176996086496,
-                    "velocityX": 2.2180264473277496,
-                    "velocityY": -0.2711217661229951,
-                    "timestamp": 0.26819281405052775
-                },
-                {
-                    "x": 4.923962030908283,
-                    "y": 5.4792142049190105,
-                    "heading": 3.2279129750090267,
-                    "angularVelocity": 0.328142121241728,
-                    "velocityX": 2.9579237085624133,
-                    "velocityY": -0.3613861283199711,
-                    "timestamp": 0.35759041873403696
-                },
-                {
-                    "x": 5.25442830298904,
-                    "y": 5.438888092999425,
-                    "heading": 3.243107062504638,
-                    "angularVelocity": 0.1699607897635962,
-                    "velocityX": 3.6965897828133016,
-                    "velocityY": -0.4510871634824905,
-                    "timestamp": 0.4469880234175462
-                },
-                {
-                    "x": 5.590105329368368,
-                    "y": 5.397771389697725,
-                    "heading": 3.243107071754486,
-                    "angularVelocity": 1.0346835398617793e-7,
-                    "velocityX": 3.7548771867867403,
-                    "velocityY": -0.45993070448202833,
-                    "timestamp": 0.5363856281010554
-                },
-                {
-                    "x": 5.925782355662538,
-                    "y": 5.356654685708362,
-                    "heading": 3.2431070810044496,
-                    "angularVelocity": 1.0346963637170158e-7,
-                    "velocityX": 3.7548771858341166,
-                    "velocityY": -0.4599307121748044,
-                    "timestamp": 0.6257832327845646
-                },
-                {
-                    "x": 6.26145937114871,
-                    "y": 5.315537893473946,
-                    "heading": 3.243107090254437,
-                    "angularVelocity": 1.0346990400921205e-7,
-                    "velocityX": 3.754877064936017,
-                    "velocityY": -0.45993169928168404,
-                    "timestamp": 0.7151808374680738
-                },
-                {
-                    "x": 6.594482534239531,
-                    "y": 5.256671755131022,
-                    "heading": 3.24310709962475,
-                    "angularVelocity": 1.0481586200532189e-7,
-                    "velocityX": 3.725191119725111,
-                    "velocityY": -0.658475565944433,
-                    "timestamp": 0.804578442151583
-                },
-                {
-                    "x": 6.90949802384657,
-                    "y": 5.13364781945348,
-                    "heading": 3.243107110210723,
-                    "angularVelocity": 1.1841422044120688e-7,
-                    "velocityX": 3.523757607622681,
-                    "velocityY": -1.3761435344273285,
-                    "timestamp": 0.8939760468350922
-                },
-                {
-                    "x": 7.194274658905514,
-                    "y": 4.9512421336811645,
-                    "heading": 3.243107206435074,
-                    "angularVelocity": 0.0000010763582143626502,
-                    "velocityX": 3.185506323875142,
-                    "velocityY": -2.0403867242241884,
-                    "timestamp": 0.9833736515186015
-                },
-                {
-                    "x": 7.415334413339818,
-                    "y": 4.749404498288232,
-                    "heading": 3.236772739824744,
-                    "angularVelocity": -0.07085722970314587,
-                    "velocityX": 2.472770441853857,
-                    "velocityY": -2.2577521635961224,
-                    "timestamp": 1.0727712562021108
-                },
-                {
-                    "x": 7.5700702382409935,
-                    "y": 4.542039922027483,
-                    "heading": 3.21849203872718,
-                    "angularVelocity": -0.20448759406641112,
-                    "velocityX": 1.7308721575766997,
-                    "velocityY": -2.3195764248545423,
-                    "timestamp": 1.16216886088562
-                },
-                {
-                    "x": 7.658284258545904,
-                    "y": 4.333806888908036,
-                    "heading": 3.1865144497864955,
-                    "angularVelocity": -0.3577007354214622,
-                    "velocityX": 0.9867604463982368,
-                    "velocityY": -2.3292909676746256,
-                    "timestamp": 1.2515664655691292
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": -0.5203097996961199,
-                    "velocityX": 0.24291189378586123,
-                    "velocityY": -2.3133381441461327,
-                    "timestamp": 1.3409640702526384
-                },
-                {
-                    "x": 7.643813431477439,
-                    "y": 3.93924818499591,
-                    "heading": 3.084542602133154,
-                    "angularVelocity": -0.6748386526461468,
-                    "velocityX": -0.4403397217071775,
-                    "velocityY": -2.284675926642036,
-                    "timestamp": 1.4231428216475348
-                },
-                {
-                    "x": 7.551490590699835,
-                    "y": 3.7550289645599473,
-                    "heading": 3.0195743357345632,
-                    "angularVelocity": -0.7905725664570535,
-                    "velocityX": -1.1234393223010666,
-                    "velocityY": -2.2416892116036538,
-                    "timestamp": 1.5053215730424312
-                },
-                {
-                    "x": 7.403134940422789,
-                    "y": 3.5762585586759936,
-                    "heading": 2.9502561501390385,
-                    "angularVelocity": -0.8435049744515962,
-                    "velocityX": -1.8052799264521386,
-                    "velocityY": -2.175384790327918,
-                    "timestamp": 1.5875003244373276
-                },
-                {
-                    "x": 7.199227882598224,
-                    "y": 3.4065730339632285,
-                    "heading": 2.886416818113751,
-                    "angularVelocity": -0.7768350205485519,
-                    "velocityX": -2.4812625448848915,
-                    "velocityY": -2.064834544462633,
-                    "timestamp": 1.669679075832224
-                },
-                {
-                    "x": 6.942760679940155,
-                    "y": 3.255041432853574,
-                    "heading": 2.8546081791353424,
-                    "angularVelocity": -0.3870664673889389,
-                    "velocityX": -3.1208456967576708,
-                    "velocityY": -1.843926787962309,
-                    "timestamp": 1.7518578272271204
-                },
-                {
-                    "x": 6.651351957860763,
-                    "y": 3.1467563415044117,
-                    "heading": 2.854608138694005,
-                    "angularVelocity": -4.921174528263849e-7,
-                    "velocityX": -3.5460349194222314,
-                    "velocityY": -1.3176774957700823,
-                    "timestamp": 1.8340365786220167
-                },
-                {
-                    "x": 6.345190283970336,
-                    "y": 3.092814329077674,
-                    "heading": 2.85460812188226,
-                    "angularVelocity": -2.045787704737326e-7,
-                    "velocityX": -3.7255576252046376,
-                    "velocityY": -0.6563985399585123,
-                    "timestamp": 1.9162153300169131
-                },
-                {
-                    "x": 6.034627693227853,
-                    "y": 3.0788290052051352,
-                    "heading": 2.8546081056797767,
-                    "angularVelocity": -1.9716489468637527e-7,
-                    "velocityX": -3.7791106030377564,
-                    "velocityY": -0.17018175224969478,
-                    "timestamp": 1.9983940814118095
-                },
-                {
-                    "x": 5.724065078469444,
-                    "y": 3.064844225150663,
-                    "heading": 2.8546080894768826,
-                    "angularVelocity": -1.9716993391114644e-7,
-                    "velocityX": -3.7791108952905743,
-                    "velocityY": -0.17017513428705025,
-                    "timestamp": 2.080572832806706
-                },
-                {
-                    "x": 5.413502463745052,
-                    "y": 3.050859444277176,
-                    "heading": 2.8546080732739703,
-                    "angularVelocity": -1.9717015240502906e-7,
-                    "velocityX": -3.7791108948748344,
-                    "velocityY": -0.170175144292348,
-                    "timestamp": 2.1627515842016023
-                },
-                {
-                    "x": 5.102939848618815,
-                    "y": 3.0368746723382456,
-                    "heading": 2.854608057073056,
-                    "angularVelocity": -1.9714443134713148e-7,
-                    "velocityX": -3.779110899765686,
-                    "velocityY": -0.17017503556443397,
-                    "timestamp": 2.2449303355964987
-                },
-                {
-                    "x": 4.82269393470373,
-                    "y": 3.0246815634320945,
-                    "heading": 2.775318198682859,
-                    "angularVelocity": -0.9648462289850903,
-                    "velocityX": -3.4101992200919007,
-                    "velocityY": -0.1483730125178458,
-                    "timestamp": 2.327109086991395
-                },
-                {
-                    "x": 4.59870268568745,
-                    "y": 3.0148363143730834,
-                    "heading": 2.699114427610376,
-                    "angularVelocity": -0.927292880108085,
-                    "velocityX": -2.725658947290856,
-                    "velocityY": -0.11980285529234247,
-                    "timestamp": 2.4092878383862915
-                },
-                {
-                    "x": 4.43080987518015,
-                    "y": 3.007426917155393,
-                    "heading": 2.637997904363483,
-                    "angularVelocity": -0.7437022612594317,
-                    "velocityX": -2.043019730258355,
-                    "velocityY": -0.09016195906753964,
-                    "timestamp": 2.491466589781188
-                },
-                {
-                    "x": 4.318925850575796,
-                    "y": 3.002477699207252,
-                    "heading": 2.595691429442944,
-                    "angularVelocity": -0.5148103882976692,
-                    "velocityX": -1.361471459539352,
-                    "velocityY": -0.06022503220498214,
-                    "timestamp": 2.5736453411760842
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": -0.26395423478763,
-                    "velocityX": -0.6805390642559688,
-                    "velocityY": -0.03015011990058865,
-                    "timestamp": 2.6558240925709806
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": 0,
-                    "velocityX": 2.4341219671984428e-29,
-                    "velocityY": 0,
-                    "timestamp": 2.738002843965877
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -19845,17 +4775,8 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S2 To C3 To S4": {
+        "C3 To S3": {
             "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
                 {
                     "x": 7.68,
                     "y": 4.127,
@@ -19863,326 +4784,60 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 18
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
+            "trajectory": [],
+            "constraints": [
                 {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": -2.6674512845646027e-30,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.322765898095933,
-                    "y": 5.556151172121073,
-                    "heading": 3.1479530411377072,
-                    "angularVelocity": 0.09380834863319021,
-                    "velocityX": 0.7049555142252463,
-                    "velocityY": -0.04539800325896039,
-                    "timestamp": 0.08477967317075337
-                },
-                {
-                    "x": 4.442297694457063,
-                    "y": 5.548453524989103,
-                    "heading": 3.163858908065189,
-                    "angularVelocity": 0.18761415717719215,
-                    "velocityX": 1.4099110304470142,
-                    "velocityY": -0.09079590477320977,
-                    "timestamp": 0.16955934634150674
-                },
-                {
-                    "x": 4.621595390179887,
-                    "y": 5.536907071113832,
-                    "heading": 3.1877171391651413,
-                    "angularVelocity": 0.28141452081573415,
-                    "velocityX": 2.1148665596022633,
-                    "velocityY": -0.13619365873154282,
-                    "timestamp": 0.2543390195122601
-                },
-                {
-                    "x": 4.860658986379666,
-                    "y": 5.521511826336959,
-                    "heading": 3.2195268977958102,
-                    "angularVelocity": 0.3752050160303855,
-                    "velocityX": 2.819822101912331,
-                    "velocityY": -0.1815912258326068,
-                    "timestamp": 0.3391186926830135
-                },
-                {
-                    "x": 5.159488476433141,
-                    "y": 5.5022678093145005,
-                    "heading": 3.2592867547687217,
-                    "angularVelocity": 0.46897865356654944,
-                    "velocityX": 3.524777566099216,
-                    "velocityY": -0.22698857288071755,
-                    "timestamp": 0.42389836585376683
-                },
-                {
-                    "x": 5.47954195294372,
-                    "y": 5.481656666167451,
-                    "heading": 3.2592867595783463,
-                    "angularVelocity": 5.673087575653179e-8,
-                    "velocityX": 3.7751204332431567,
-                    "velocityY": -0.24311420858382377,
-                    "timestamp": 0.5086780390245202
-                },
-                {
-                    "x": 5.799595429451853,
-                    "y": 5.461045522982243,
-                    "heading": 3.2592867643879506,
-                    "angularVelocity": 5.6730628986964756e-8,
-                    "velocityX": 3.7751204332143167,
-                    "velocityY": -0.2431142090339428,
-                    "timestamp": 0.5934577121952737
-                },
-                {
-                    "x": 6.1196488941034595,
-                    "y": 5.440434195687293,
-                    "heading": 3.259286769197555,
-                    "angularVelocity": 5.673063176342314e-8,
-                    "velocityX": 3.7751202933632646,
-                    "velocityY": -0.24311638065988433,
-                    "timestamp": 0.6782373853660271
-                },
-                {
-                    "x": 6.4368892365574455,
-                    "y": 5.393342668998585,
-                    "heading": 3.2592867741163385,
-                    "angularVelocity": 5.801842614465255e-8,
-                    "velocityX": 3.741938728815606,
-                    "velocityY": -0.5554577521660955,
-                    "timestamp": 0.7630170585367805
-                },
-                {
-                    "x": 6.7398295350074555,
-                    "y": 5.288051546129373,
-                    "heading": 3.2592867797575464,
-                    "angularVelocity": 6.653963355876342e-8,
-                    "velocityX": 3.5732657029690613,
-                    "velocityY": -1.2419382964267203,
-                    "timestamp": 0.8477967317075339
-                },
-                {
-                    "x": 7.017891386263095,
-                    "y": 5.128237130868345,
-                    "heading": 3.259286787454074,
-                    "angularVelocity": 9.07826943070083e-8,
-                    "velocityX": 3.2798174474623285,
-                    "velocityY": -1.8850558074130808,
-                    "timestamp": 0.9325764048782873
-                },
-                {
-                    "x": 7.261365099007722,
-                    "y": 4.919479947434011,
-                    "heading": 3.259286723898943,
-                    "angularVelocity": -7.496505777325929e-7,
-                    "velocityX": 2.8718406622562314,
-                    "velocityY": -2.462349471599812,
-                    "timestamp": 1.0173560780490407
-                },
-                {
-                    "x": 7.457139881435657,
-                    "y": 4.674807078885688,
-                    "heading": 3.2408811636287,
-                    "angularVelocity": -0.21709874055771505,
-                    "velocityX": 2.309218414106716,
-                    "velocityY": -2.885985040961276,
-                    "timestamp": 1.102135751219794
-                },
-                {
-                    "x": 7.597538484149158,
-                    "y": 4.408115178432638,
-                    "heading": 3.1997348588691503,
-                    "angularVelocity": -0.48533219368821684,
-                    "velocityX": 1.6560408581747834,
-                    "velocityY": -3.1457056919299244,
-                    "timestamp": 1.1869154243905473
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
                 {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": -0.7045893978594314,
-                    "velocityX": 0.9726566848724733,
-                    "velocityY": -3.315832297049189,
-                    "timestamp": 1.2716950975613006
-                },
-                {
-                    "x": 7.707365632796218,
-                    "y": 3.8777713806946585,
-                    "heading": 3.0766920768105654,
-                    "angularVelocity": -0.8684660682637303,
-                    "velocityX": 0.37540519927184846,
-                    "velocityY": -3.418949607021354,
-                    "timestamp": 1.344591351660309
-                },
-                {
-                    "x": 7.690646428306815,
-                    "y": 3.624138174667908,
-                    "heading": 3.0114310872436763,
-                    "angularVelocity": -0.8952584789497043,
-                    "velocityX": -0.229356154113369,
-                    "velocityY": -3.4793722827239795,
-                    "timestamp": 1.4174876057593173
-                },
-                {
-                    "x": 7.629817161501398,
-                    "y": 3.371341042658395,
-                    "heading": 2.9608367604127053,
-                    "angularVelocity": -0.6940593512835794,
-                    "velocityX": -0.8344635476447348,
-                    "velocityY": -3.4679029139977815,
-                    "timestamp": 1.4903838598583257
-                },
-                {
-                    "x": 7.52574877175356,
-                    "y": 3.1246562301261127,
-                    "heading": 2.9373487413645845,
-                    "angularVelocity": -0.32221160522605474,
-                    "velocityX": -1.4276232850943003,
-                    "velocityY": -3.3840533451439727,
-                    "timestamp": 1.563280113957334
-                },
-                {
-                    "x": 7.379748340613123,
-                    "y": 2.890715038062695,
-                    "heading": 2.9373478462882416,
-                    "angularVelocity": -0.00001227877368205191,
-                    "velocityX": -2.002852313126858,
-                    "velocityY": -3.2092347536203247,
-                    "timestamp": 1.6361763680563424
-                },
-                {
-                    "x": 7.198165609339985,
-                    "y": 2.6831759130730863,
-                    "heading": 2.937347790914402,
-                    "angularVelocity": -7.596252972489614e-7,
-                    "velocityX": -2.4909747903727215,
-                    "velocityY": -2.847047870356197,
-                    "timestamp": 1.7090726221553507
-                },
-                {
-                    "x": 6.985688529633763,
-                    "y": 2.5073968727951343,
-                    "heading": 2.9373477481889543,
-                    "angularVelocity": -5.861130796388339e-7,
-                    "velocityX": -2.91478735543415,
-                    "velocityY": -2.4113590259261937,
-                    "timestamp": 1.781968876254359
-                },
-                {
-                    "x": 6.7478025524071645,
-                    "y": 2.367915534392956,
-                    "heading": 2.937347710107927,
-                    "angularVelocity": -5.224003305629458e-7,
-                    "velocityX": -3.263349813607291,
-                    "velocityY": -1.9134225774154996,
-                    "timestamp": 1.8548651303533674
-                },
-                {
-                    "x": 6.4987639591558635,
-                    "y": 2.249490193910499,
-                    "heading": 2.937347672689208,
-                    "angularVelocity": -5.133147047667194e-7,
-                    "velocityX": -3.4163428056670964,
-                    "velocityY": -1.624573744513203,
-                    "timestamp": 1.9277613844523758
-                },
-                {
-                    "x": 6.249725239429151,
-                    "y": 2.1310651193958834,
-                    "heading": 2.937347635270487,
-                    "angularVelocity": -5.133147287283584e-7,
-                    "velocityX": -3.4163445406727813,
-                    "velocityY": -1.6245700959327491,
-                    "timestamp": 2.000657638551384
-                },
-                {
-                    "x": 6.0006865195043355,
-                    "y": 2.012640045297908,
-                    "heading": 2.937347597851683,
-                    "angularVelocity": -5.133158656564982e-7,
-                    "velocityX": -3.4163445433903696,
-                    "velocityY": -1.6245700902172364,
-                    "timestamp": 2.0735538926503922
-                },
-                {
-                    "x": 5.766671175118509,
-                    "y": 1.901395758932124,
-                    "heading": 2.8964169231036223,
-                    "angularVelocity": -0.5614921542139502,
-                    "velocityX": -3.2102519845241893,
-                    "velocityY": -1.5260631391951232,
-                    "timestamp": 2.1464501467494004
-                },
-                {
-                    "x": 5.568987668741884,
-                    "y": 1.8073555304166649,
-                    "heading": 2.790644028809865,
-                    "angularVelocity": -1.4510058932587933,
-                    "velocityX": -2.7118472522341635,
-                    "velocityY": -1.2900557055764181,
-                    "timestamp": 2.2193464008484085
-                },
-                {
-                    "x": 5.41134573614175,
-                    "y": 1.7322845721056195,
-                    "heading": 2.684499308002556,
-                    "angularVelocity": -1.456106656228851,
-                    "velocityX": -2.162551897193064,
-                    "velocityY": -1.029832866434731,
-                    "timestamp": 2.2922426549474166
-                },
-                {
-                    "x": 5.293483125084031,
-                    "y": 1.6760930732996284,
-                    "heading": 2.5959515176489267,
-                    "angularVelocity": -1.2147097467272172,
-                    "velocityX": -1.6168541513476236,
-                    "velocityY": -0.7708420617846653,
-                    "timestamp": 2.3651389090464248
-                },
-                {
-                    "x": 5.2151080549458655,
-                    "y": 1.6386848703250345,
-                    "heading": 2.5329322459715486,
-                    "angularVelocity": -0.8645063104618199,
-                    "velocityX": -1.075159088856404,
-                    "velocityY": -0.5131704425282329,
-                    "timestamp": 2.438035163145433
-                },
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "C3 To S4": {
+            "waypoints": [
                 {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": -0.45176870030861394,
-                    "velocityX": -0.536489226081922,
-                    "velocityY": -0.25632140575671664,
-                    "timestamp": 2.510931417244441
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 5.176,
                     "y": 1.62,
                     "heading": 2.5,
-                    "angularVelocity": 0,
-                    "velocityX": 2.4074157612584747e-31,
-                    "velocityY": 0,
-                    "timestamp": 2.583827671343449
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 }
             ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -20202,20 +4857,20 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S3 To W2": {
+        "C3 To C1": {
             "waypoints": [
                 {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 16
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 2.25,
-                    "y": 5.56,
+                    "x": 7.68,
+                    "y": 7.467,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
@@ -20223,161 +4878,7 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.228111844192817,
-                    "y": 3.0443410125088586,
-                    "heading": 2.5974492034990555,
-                    "angularVelocity": 0.2841110410157656,
-                    "velocityX": -0.42270562690532876,
-                    "velocityY": 0.5372366368048371,
-                    "timestamp": 0.08253534750081891
-                },
-                {
-                    "x": 4.158301873890454,
-                    "y": 3.133062319081129,
-                    "heading": 2.642673247648486,
-                    "angularVelocity": 0.5479354666675527,
-                    "velocityX": -0.8458190631798037,
-                    "velocityY": 1.0749492097478566,
-                    "timestamp": 0.16507069500163782
-                },
-                {
-                    "x": 4.053518280244317,
-                    "y": 3.266222365985673,
-                    "heading": 2.7068891131091086,
-                    "angularVelocity": 0.7780407716825238,
-                    "velocityX": -1.2695602165496185,
-                    "velocityY": 1.6133699189092645,
-                    "timestamp": 0.2476060425024567
-                },
-                {
-                    "x": 3.9136724453244414,
-                    "y": 3.4439114508470956,
-                    "heading": 2.784478333107965,
-                    "angularVelocity": 0.9400726155310244,
-                    "velocityX": -1.694375066616006,
-                    "velocityY": 2.152884675982738,
-                    "timestamp": 0.33014139000327564
-                },
-                {
-                    "x": 3.738653008239037,
-                    "y": 3.6661781083911804,
-                    "heading": 2.8571480043531183,
-                    "angularVelocity": 0.8804672597329689,
-                    "velocityX": -2.1205391675811076,
-                    "velocityY": 2.6929874808109258,
-                    "timestamp": 0.41267673750409456
-                },
-                {
-                    "x": 3.545725477812246,
-                    "y": 3.911665858291861,
-                    "heading": 2.8571480229647763,
-                    "angularVelocity": 2.254992345614167e-7,
-                    "velocityX": -2.3375140018024174,
-                    "velocityY": 2.9743347224441523,
-                    "timestamp": 0.4952120850049135
-                },
-                {
-                    "x": 3.352797950657087,
-                    "y": 4.157153610763737,
-                    "heading": 2.8571480415763673,
-                    "angularVelocity": 2.2549842301439466e-7,
-                    "velocityX": -2.3375139621632335,
-                    "velocityY": 2.9743347535968137,
-                    "timestamp": 0.5777474325057324
-                },
-                {
-                    "x": 3.1598704235019452,
-                    "y": 4.402641363235625,
-                    "heading": 2.8571480601879577,
-                    "angularVelocity": 2.2549842390160243e-7,
-                    "velocityX": -2.3375139621630274,
-                    "velocityY": 2.974334753596974,
-                    "timestamp": 0.6602827800065513
-                },
-                {
-                    "x": 2.966942896346795,
-                    "y": 4.648129115707507,
-                    "heading": 2.8571480787995487,
-                    "angularVelocity": 2.2549842409910227e-7,
-                    "velocityX": -2.337513962163132,
-                    "velocityY": 2.974334753596892,
-                    "timestamp": 0.7428181275073702
-                },
-                {
-                    "x": 2.7740153675253203,
-                    "y": 4.8936168668697935,
-                    "heading": 2.857148097411205,
-                    "angularVelocity": 2.2549920897733148e-7,
-                    "velocityX": -2.3375139823523616,
-                    "velocityY": 2.9743347377297993,
-                    "timestamp": 0.8253534750081891
-                },
-                {
-                    "x": 2.599116566256145,
-                    "y": 5.1159598004085165,
-                    "heading": 2.929233412420125,
-                    "angularVelocity": 0.873387187328517,
-                    "velocityX": -2.1190775415035294,
-                    "velocityY": 2.693911642360469,
-                    "timestamp": 0.907888822509008
-                },
-                {
-                    "x": 2.459351765780957,
-                    "y": 5.293700413500499,
-                    "heading": 3.0069352217627348,
-                    "angularVelocity": 0.9414367503794493,
-                    "velocityX": -1.6933932515859442,
-                    "velocityY": 2.1535089931040625,
-                    "timestamp": 0.990424170009827
-                },
-                {
-                    "x": 2.354631169439892,
-                    "y": 5.426897486256648,
-                    "heading": 3.071284016075413,
-                    "angularVelocity": 0.7796513404397946,
-                    "velocityX": -1.268796939881139,
-                    "velocityY": 1.6138185249032515,
-                    "timestamp": 1.0729595175106459
-                },
-                {
-                    "x": 2.284865378797646,
-                    "y": 5.515644940856661,
-                    "heading": 3.116557551514127,
-                    "angularVelocity": 0.5485351041656966,
-                    "velocityX": -0.8452837814919688,
-                    "velocityY": 1.0752660197999702,
-                    "timestamp": 1.1554948650114647
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0.2840291971344776,
-                    "velocityX": -0.4224296601804454,
-                    "velocityY": 0.5374068261225751,
-                    "timestamp": 1.2380302125122835
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 2.192310159207654e-31,
-                    "velocityX": 0,
-                    "velocityY": 7.442690740363628e-33,
-                    "timestamp": 1.3205655600131023
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -20397,146 +4898,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S3 To W3": {
+        "C3 To C2": {
             "waypoints": [
                 {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 12
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 2.097905783412131e-34,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.206954796561596,
-                    "y": 3.0388324489204006,
-                    "heading": 2.584970820102585,
-                    "angularVelocity": 0.12125870375097321,
-                    "velocityX": -0.619458587129609,
-                    "velocityY": 0.42920878982002253,
-                    "timestamp": 0.0904744959596105
-                },
-                {
-                    "x": 4.094846273424844,
-                    "y": 3.1165082311139285,
-                    "heading": 2.6050859488479765,
-                    "angularVelocity": 0.2223292711613592,
-                    "velocityX": -1.2391174103561666,
-                    "velocityY": 0.858537882633839,
-                    "timestamp": 0.180948991919221
-                },
-                {
-                    "x": 3.9266509261170293,
-                    "y": 3.2330407963022716,
-                    "heading": 2.631256329120159,
-                    "angularVelocity": 0.2892569888851994,
-                    "velocityX": -1.8590360247256863,
-                    "velocityY": 1.2880156330505033,
-                    "timestamp": 0.2714234878788315
-                },
-                {
-                    "x": 3.702348687884298,
-                    "y": 3.38843810676698,
-                    "heading": 2.65711164443334,
-                    "angularVelocity": 0.2857746267492099,
-                    "velocityX": -2.479176433686503,
-                    "velocityY": 1.7175813892799228,
-                    "timestamp": 0.361897983838442
-                },
-                {
-                    "x": 3.4221471625561564,
-                    "y": 3.582533883045857,
-                    "heading": 2.6614389879992997,
-                    "angularVelocity": 0.04782942994113533,
-                    "velocityX": -3.0970222310299356,
-                    "velocityY": 2.1453092854534943,
-                    "timestamp": 0.4523724797980525
-                },
-                {
-                    "x": 3.1408345803954827,
-                    "y": 3.7774823101067563,
-                    "heading": 2.6614389980641375,
-                    "angularVelocity": 1.1124502748712501e-7,
-                    "velocityX": -3.1093025628598916,
-                    "velocityY": 2.154733496917496,
-                    "timestamp": 0.542846975757663
-                },
-                {
-                    "x": 2.8606418991642766,
-                    "y": 3.9715702772190298,
-                    "heading": 2.6657880248697206,
-                    "angularVelocity": 0.04806909128871381,
-                    "velocityX": -3.096924478654065,
-                    "velocityY": 2.1452229719955302,
-                    "timestamp": 0.6333214717172736
-                },
-                {
-                    "x": 2.6363424534806392,
-                    "y": 4.126965038945093,
-                    "heading": 2.691984108325433,
-                    "angularVelocity": 0.28954108202389867,
-                    "velocityX": -2.479145568092125,
-                    "velocityY": 1.7175532184830777,
-                    "timestamp": 0.7237959676768841
-                },
-                {
-                    "x": 2.46814994135131,
-                    "y": 4.24349512219199,
-                    "heading": 2.7185033740588933,
-                    "angularVelocity": 0.2931131635737346,
-                    "velocityX": -1.8590046879555269,
-                    "velocityY": 1.2879882005523255,
-                    "timestamp": 0.8142704636364947
-                },
-                {
-                    "x": 2.3560438127968273,
-                    "y": 4.3211688040728875,
-                    "heading": 2.738885032189894,
-                    "angularVelocity": 0.22527517743895228,
-                    "velocityX": -1.2390909434248611,
-                    "velocityY": 0.8585146682173574,
-                    "timestamp": 0.9047449595961052
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": 0.12285194509475847,
-                    "velocityX": -0.6194432165927374,
-                    "velocityY": 0.42919494068745534,
-                    "timestamp": 0.9952194555557158
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": 6.588066214768088e-31,
-                    "velocityX": -4.3339190939808694e-33,
-                    "velocityY": -1.4815259513582048e-34,
-                    "timestamp": 1.0856939515153263
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -20556,17 +4939,8 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S3 To C3 To S1": {
+        "C3 To C4": {
             "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 17
-                },
                 {
                     "x": 7.68,
                     "y": 4.127,
@@ -20574,326 +4948,19 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 16
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 3.4031853071795863,
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.308593242288023,
-                    "y": 2.998690970742917,
-                    "heading": 2.600069907206834,
-                    "angularVelocity": 0.35025874949099595,
-                    "velocityX": 0.6125619052781636,
-                    "velocityY": -0.017587287403649503,
-                    "timestamp": 0.07443042392151207
-                },
-                {
-                    "x": 4.399870755267574,
-                    "y": 2.9960680712021444,
-                    "heading": 2.65011196683443,
-                    "angularVelocity": 0.6723333952842253,
-                    "velocityX": 1.2263468104898216,
-                    "velocityY": -0.03523961577241709,
-                    "timestamp": 0.14886084784302414
-                },
-                {
-                    "x": 4.536960438371983,
-                    "y": 2.992122797800661,
-                    "heading": 2.720779852914046,
-                    "angularVelocity": 0.9494489263440683,
-                    "velocityX": 1.8418500914219216,
-                    "velocityY": -0.05300619281227453,
-                    "timestamp": 0.22329127176453623
-                },
-                {
-                    "x": 4.72004322969291,
-                    "y": 2.9868327872754663,
-                    "heading": 2.805997286445448,
-                    "angularVelocity": 1.1449274240499019,
-                    "velocityX": 2.4597843418706042,
-                    "velocityY": -0.07107322847942685,
-                    "timestamp": 0.2977216956860483
-                },
-                {
-                    "x": 4.949295920013874,
-                    "y": 2.980134342239005,
-                    "heading": 2.892264553695649,
-                    "angularVelocity": 1.1590323244855822,
-                    "velocityX": 3.0800938412323915,
-                    "velocityY": -0.08999606187282569,
-                    "timestamp": 0.37215211960756034
-                },
-                {
-                    "x": 5.2218046444028055,
-                    "y": 2.971482120946463,
-                    "heading": 2.9184186664707665,
-                    "angularVelocity": 0.35139008213490475,
-                    "velocityX": 3.661254498245178,
-                    "velocityY": -0.11624576129869893,
-                    "timestamp": 0.4465825435290724
-                },
-                {
-                    "x": 5.503268186944086,
-                    "y": 2.9638921982075033,
-                    "heading": 2.918418695115671,
-                    "angularVelocity": 3.8485478250287795e-7,
-                    "velocityX": 3.7815657591590193,
-                    "velocityY": -0.10197339124338625,
-                    "timestamp": 0.5210129674505845
-                },
-                {
-                    "x": 5.784731729781038,
-                    "y": 2.9563022864327206,
-                    "heading": 2.9184187237605435,
-                    "angularVelocity": 3.8485435121714565e-7,
-                    "velocityX": 3.781565763131487,
-                    "velocityY": -0.10197324393565109,
-                    "timestamp": 0.5954433913720966
-                },
-                {
-                    "x": 6.066195282639446,
-                    "y": 2.9487127463017835,
-                    "heading": 2.9184187524054175,
-                    "angularVelocity": 3.848543706133971e-7,
-                    "velocityX": 3.7815658977733997,
-                    "velocityY": -0.10196825076449405,
-                    "timestamp": 0.6698738152936087
-                },
-                {
-                    "x": 6.347042051479281,
-                    "y": 2.968823062647617,
-                    "heading": 2.918418781616621,
-                    "angularVelocity": 3.924632205757564e-7,
-                    "velocityX": 3.773279178632548,
-                    "velocityY": 0.2701894640159414,
-                    "timestamp": 0.7443042392151208
-                },
-                {
-                    "x": 6.6208214562671275,
-                    "y": 3.0345814714160126,
-                    "heading": 2.918418814269471,
-                    "angularVelocity": 4.3870299176986663e-7,
-                    "velocityX": 3.678326554696914,
-                    "velocityY": 0.8834883009364279,
-                    "timestamp": 0.8187346631366329
-                },
-                {
-                    "x": 6.88016498053821,
-                    "y": 3.1442184338201953,
-                    "heading": 2.9184188559848527,
-                    "angularVelocity": 5.604614269784904e-7,
-                    "velocityX": 3.4843752138851785,
-                    "velocityY": 1.4730127362944399,
-                    "timestamp": 0.893165087058145
-                },
-                {
-                    "x": 7.118091210487748,
-                    "y": 3.294782489019261,
-                    "heading": 2.918422820305874,
-                    "angularVelocity": 0.00005326210462723065,
-                    "velocityX": 3.196626022181901,
-                    "velocityY": 2.022883214501594,
-                    "timestamp": 0.9675955109796571
-                },
-                {
-                    "x": 7.320693053454344,
-                    "y": 3.474127369690306,
-                    "heading": 2.9455538073395324,
-                    "angularVelocity": 0.36451474550610485,
-                    "velocityX": 2.722029948133077,
-                    "velocityY": 2.4095641435572857,
-                    "timestamp": 1.0420259349011691
-                },
-                {
-                    "x": 7.483696807015107,
-                    "y": 3.6758684658318805,
-                    "heading": 3.001437207311255,
-                    "angularVelocity": 0.7508139417646484,
-                    "velocityX": 2.1900151171065767,
-                    "velocityY": 2.710465499354305,
-                    "timestamp": 1.1164563588226812
-                },
-                {
-                    "x": 7.603947516595235,
-                    "y": 3.8947228146234494,
-                    "heading": 3.0702288107606313,
-                    "angularVelocity": 0.9242403821576988,
-                    "velocityX": 1.6156123160998233,
-                    "velocityY": 2.940388315164694,
-                    "timestamp": 1.1908867827441934
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": 0.9374014759467769,
-                    "velocityX": 1.0217929631162956,
-                    "velocityY": 3.12072903980085,
-                    "timestamp": 1.2653172066657055
-                },
-                {
-                    "x": 7.709300887372812,
-                    "y": 4.397378653394831,
-                    "heading": 3.2093959242930556,
-                    "angularVelocity": 0.8431459366282483,
-                    "velocityX": 0.355999640896236,
-                    "velocityY": 3.2850439746024853,
-                    "timestamp": 1.3476231530960114
-                },
-                {
-                    "x": 7.683172834427827,
-                    "y": 4.6781135002756455,
-                    "heading": 3.266996168893978,
-                    "angularVelocity": 0.6998309004282659,
-                    "velocityX": -0.31745036753947053,
-                    "velocityY": 3.4108695550756014,
-                    "timestamp": 1.4299290995263174
-                },
-                {
-                    "x": 7.601002938928111,
-                    "y": 4.963519420188029,
-                    "heading": 3.305651937027416,
-                    "angularVelocity": 0.4696594816046569,
-                    "velocityX": -0.9983470097059609,
-                    "velocityY": 3.467622113477001,
-                    "timestamp": 1.5122350459566234
-                },
-                {
-                    "x": 7.463684437134381,
-                    "y": 5.241326726377331,
-                    "heading": 3.3097255928301426,
-                    "angularVelocity": 0.049494064273674344,
-                    "velocityX": -1.6683910185032116,
-                    "velocityY": 3.3753005492080685,
-                    "timestamp": 1.5945409923869294
-                },
-                {
-                    "x": 7.277476070304,
-                    "y": 5.490867404030893,
-                    "heading": 3.3097256042381358,
-                    "angularVelocity": 1.3860472723422523e-7,
-                    "velocityX": -2.2623926326885195,
-                    "velocityY": 3.031866936429266,
-                    "timestamp": 1.6768469388172353
-                },
-                {
-                    "x": 7.049248371933673,
-                    "y": 5.702660296381284,
-                    "heading": 3.3097256090357283,
-                    "angularVelocity": 5.8289739150802425e-8,
-                    "velocityX": -2.7729187047691926,
-                    "velocityY": 2.573239255923398,
-                    "timestamp": 1.7591528852475413
-                },
-                {
-                    "x": 6.786512642610724,
-                    "y": 5.869735280017231,
-                    "heading": 3.309725613101849,
-                    "angularVelocity": 4.940251326866522e-8,
-                    "velocityX": -3.192184048881866,
-                    "velocityY": 2.029926036722274,
-                    "timestamp": 1.8414588316778473
-                },
-                {
-                    "x": 6.5049501154842,
-                    "y": 6.00265125873641,
-                    "heading": 3.309725617013439,
-                    "angularVelocity": 4.7524998408670815e-8,
-                    "velocityX": -3.4209256966013024,
-                    "velocityY": 1.6149012857986877,
-                    "timestamp": 1.9237647781081533
-                },
-                {
-                    "x": 6.223387384136782,
-                    "y": 6.135566804843462,
-                    "heading": 3.309725620925028,
-                    "angularVelocity": 4.75249886637903e-8,
-                    "velocityX": -3.4209281778423657,
-                    "velocityY": 1.6148960296520045,
-                    "timestamp": 2.0060707245384592
-                },
-                {
-                    "x": 5.941824652769856,
-                    "y": 6.268482350909119,
-                    "heading": 3.3097256248366427,
-                    "angularVelocity": 4.752530281025494e-8,
-                    "velocityX": -3.42092817807938,
-                    "velocityY": 1.6148960291490546,
-                    "timestamp": 2.0883766709687652
-                },
-                {
-                    "x": 5.686549787544516,
-                    "y": 6.388988216738383,
-                    "heading": 3.340879096998464,
-                    "angularVelocity": 0.3785081578303866,
-                    "velocityX": -3.1015361136937893,
-                    "velocityY": 1.464121015014444,
-                    "timestamp": 2.170682617399071
-                },
-                {
-                    "x": 5.482329878299888,
-                    "y": 6.485392923986267,
-                    "heading": 3.365801660864199,
-                    "angularVelocity": 0.30280392786490506,
-                    "velocityX": -2.4812290982833782,
-                    "velocityY": 1.1712969892098404,
-                    "timestamp": 2.252988563829377
-                },
-                {
-                    "x": 5.32916494108436,
-                    "y": 6.557696460239907,
-                    "heading": 3.3844934905089157,
-                    "angularVelocity": 0.22710181287501843,
-                    "velocityX": -1.8609218878884095,
-                    "velocityY": 0.8784728125915443,
-                    "timestamp": 2.335294510259683
-                },
-                {
-                    "x": 5.227054980864034,
-                    "y": 6.6058988198426265,
-                    "heading": 3.396954693375837,
-                    "angularVelocity": 0.1514010032977761,
-                    "velocityX": -1.2406146171563652,
-                    "velocityY": 0.5856485672458114,
-                    "timestamp": 2.417600456689989
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": 0.07570065194530232,
-                    "velocityX": -0.6203073177375487,
-                    "velocityY": 0.29282428794839654,
-                    "timestamp": 2.499906403120295
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": 0,
-                    "velocityX": 1.5419247738054587e-33,
-                    "velocityY": 0,
-                    "timestamp": 2.582212349550601
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -20913,17 +4980,8 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S3 To C3 To S2": {
+        "C3 To C5": {
             "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 17
-                },
                 {
                     "x": 7.68,
                     "y": 4.127,
@@ -20931,11 +4989,11 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 15
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 4.263,
-                    "y": 5.56,
+                    "x": 7.68,
+                    "y": 0.787,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
@@ -20943,305 +5001,7 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.312496748515021,
-                    "y": 3.002119555669804,
-                    "heading": 2.5925232531186833,
-                    "angularVelocity": 0.239650083468562,
-                    "velocityX": 0.6403788706575965,
-                    "velocityY": 0.027422380395612005,
-                    "timestamp": 0.07729291327834321
-                },
-                {
-                    "x": 4.411499664032465,
-                    "y": 3.0063632003056355,
-                    "heading": 2.629246894113083,
-                    "angularVelocity": 0.4751230020552096,
-                    "velocityX": 1.2808795957906263,
-                    "velocityY": 0.05490341165625346,
-                    "timestamp": 0.15458582655668643
-                },
-                {
-                    "x": 4.560023871242694,
-                    "y": 3.012738860842861,
-                    "heading": 2.6836373496883423,
-                    "angularVelocity": 0.7036926578170436,
-                    "velocityX": 1.9215760010930336,
-                    "velocityY": 0.08248699999527603,
-                    "timestamp": 0.23187873983502966
-                },
-                {
-                    "x": 4.758098277209683,
-                    "y": 3.021263020053228,
-                    "heading": 2.7546133652741296,
-                    "angularVelocity": 0.9182732617438291,
-                    "velocityX": 2.5626464001129405,
-                    "velocityY": 0.11028383908458261,
-                    "timestamp": 0.30917165311337286
-                },
-                {
-                    "x": 5.005797064956168,
-                    "y": 3.0319826680841246,
-                    "heading": 2.8389548559272906,
-                    "angularVelocity": 1.0911930612504541,
-                    "velocityX": 3.2046765640012143,
-                    "velocityY": 0.13868862714869012,
-                    "timestamp": 0.38646456639171606
-                },
-                {
-                    "x": 5.297951167759167,
-                    "y": 3.043836589819136,
-                    "heading": 2.838954877461829,
-                    "angularVelocity": 2.786094794708315e-7,
-                    "velocityX": 3.7798303933881865,
-                    "velocityY": 0.15336362975896514,
-                    "timestamp": 0.46375747967005926
-                },
-                {
-                    "x": 5.590081141172345,
-                    "y": 3.0562710547940686,
-                    "heading": 2.838954891655661,
-                    "angularVelocity": 1.83636908600502e-7,
-                    "velocityX": 3.7795182122476696,
-                    "velocityY": 0.1608745801850421,
-                    "timestamp": 0.5410503929484025
-                },
-                {
-                    "x": 5.882211114419822,
-                    "y": 3.0687055236618757,
-                    "heading": 2.8389549058494925,
-                    "angularVelocity": 1.8363690785211144e-7,
-                    "velocityX": 3.779518210103882,
-                    "velocityY": 0.16087463055026802,
-                    "timestamp": 0.6183433062267457
-                },
-                {
-                    "x": 6.17434108478517,
-                    "y": 3.0811400602410006,
-                    "heading": 2.838954920043324,
-                    "angularVelocity": 1.8363690859499724e-7,
-                    "velocityX": 3.779518172815467,
-                    "velocityY": 0.1608755065855243,
-                    "timestamp": 0.6956362195050889
-                },
-                {
-                    "x": 6.465903802710874,
-                    "y": 3.1031790875652407,
-                    "heading": 2.8389549343224108,
-                    "angularVelocity": 1.8473992278083087e-7,
-                    "velocityX": 3.772179176061626,
-                    "velocityY": 0.28513645545840094,
-                    "timestamp": 0.7729291327834321
-                },
-                {
-                    "x": 6.7494946065782635,
-                    "y": 3.174388582838956,
-                    "heading": 2.838954949771126,
-                    "angularVelocity": 1.998723373980939e-7,
-                    "velocityX": 3.669040172494696,
-                    "velocityY": 0.9212939744848142,
-                    "timestamp": 0.8502220460617753
-                },
-                {
-                    "x": 7.016882485558567,
-                    "y": 3.2927027326639142,
-                    "heading": 2.838955049089198,
-                    "angularVelocity": 0.000001284957026149309,
-                    "velocityX": 3.4594100240134633,
-                    "velocityY": 1.5307244197005683,
-                    "timestamp": 0.9275149593401185
-                },
-                {
-                    "x": 7.246855043486079,
-                    "y": 3.4413903121844696,
-                    "heading": 2.884377696826009,
-                    "angularVelocity": 0.5876689829666233,
-                    "velocityX": 2.9753382059665197,
-                    "velocityY": 1.9236896788339406,
-                    "timestamp": 1.0048078726184617
-                },
-                {
-                    "x": 7.429257242099238,
-                    "y": 3.603847510115455,
-                    "heading": 2.9490702967114695,
-                    "angularVelocity": 0.8369797066969458,
-                    "velocityX": 2.359882567193478,
-                    "velocityY": 2.101838202759324,
-                    "timestamp": 1.082100785896805
-                },
-                {
-                    "x": 7.562439804570718,
-                    "y": 3.7739114806600167,
-                    "heading": 3.01726271663618,
-                    "angularVelocity": 0.8822596669262517,
-                    "velocityX": 1.7230889200911585,
-                    "velocityY": 2.2002530805396896,
-                    "timestamp": 1.1593936991751483
-                },
-                {
-                    "x": 7.646052714432655,
-                    "y": 3.9488127153581356,
-                    "heading": 3.0821685087673805,
-                    "angularVelocity": 0.8397379446349181,
-                    "velocityX": 1.0817668310784703,
-                    "velocityY": 2.2628366208461252,
-                    "timestamp": 1.2366866124534917
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": 0.7482120776630553,
-                    "velocityX": 0.43920307991361807,
-                    "velocityY": 2.305350867034668,
-                    "timestamp": 1.313979525731835
-                },
-                {
-                    "x": 7.646672814687191,
-                    "y": 4.349453153850708,
-                    "heading": 3.196736252005302,
-                    "angularVelocity": 0.5972944316963834,
-                    "velocityX": -0.35085402203857,
-                    "velocityY": 2.34188945184307,
-                    "timestamp": 1.4089682766635396
-                },
-                {
-                    "x": 7.538236721836807,
-                    "y": 4.57348977092197,
-                    "heading": 3.2390603114191148,
-                    "angularVelocity": 0.44556917528311457,
-                    "velocityX": -1.1415677307763321,
-                    "velocityY": 2.3585594596600363,
-                    "timestamp": 1.5039570275952443
-                },
-                {
-                    "x": 7.354699862509888,
-                    "y": 4.7956671896432965,
-                    "heading": 3.2668511132107456,
-                    "angularVelocity": 0.2925693992082484,
-                    "velocityX": -1.932195733986223,
-                    "velocityY": 2.3389866330705615,
-                    "timestamp": 1.598945778526949
-                },
-                {
-                    "x": 7.096712135037422,
-                    "y": 5.0078309608342675,
-                    "heading": 3.2799432590323385,
-                    "angularVelocity": 0.13782838171022743,
-                    "velocityX": -2.7159818919816576,
-                    "velocityY": 2.2335673341310986,
-                    "timestamp": 1.6939345294586536
-                },
-                {
-                    "x": 6.778591882911988,
-                    "y": 5.174931015852947,
-                    "heading": 3.2799432523824414,
-                    "angularVelocity": -7.000720422097915e-8,
-                    "velocityX": -3.349030795806095,
-                    "velocityY": 1.7591562514473036,
-                    "timestamp": 1.7889232803903583
-                },
-                {
-                    "x": 6.432650840523452,
-                    "y": 5.272130447349719,
-                    "heading": 3.2799432455857884,
-                    "angularVelocity": -7.155218790875671e-8,
-                    "velocityX": -3.6419159005181747,
-                    "velocityY": 1.0232730775316359,
-                    "timestamp": 1.883912031322063
-                },
-                {
-                    "x": 6.076435890184285,
-                    "y": 5.319394007657195,
-                    "heading": 3.2799432391201027,
-                    "angularVelocity": -6.806791276827721e-8,
-                    "velocityX": -3.7500751072648586,
-                    "velocityY": 0.4975700790239653,
-                    "timestamp": 1.9789007822537676
-                },
-                {
-                    "x": 5.72022088924427,
-                    "y": 5.36665718659578,
-                    "heading": 3.279943232654418,
-                    "angularVelocity": -6.806790136756466e-8,
-                    "velocityX": -3.7500756399684434,
-                    "velocityY": 0.49756606413917676,
-                    "timestamp": 2.073889533185472
-                },
-                {
-                    "x": 5.364005888304073,
-                    "y": 5.413920365532998,
-                    "heading": 3.279943226188733,
-                    "angularVelocity": -6.806790152018608e-8,
-                    "velocityX": -3.750075639970353,
-                    "velocityY": 0.4975660641247859,
-                    "timestamp": 2.1688782841171768
-                },
-                {
-                    "x": 5.007790887255383,
-                    "y": 5.4611835436519325,
-                    "heading": 3.2799432197229685,
-                    "angularVelocity": -6.806874126926854e-8,
-                    "velocityX": -3.750075641112522,
-                    "velocityY": 0.4975660555102637,
-                    "timestamp": 2.2638670350488814
-                },
-                {
-                    "x": 4.7098745121627585,
-                    "y": 5.500710065242911,
-                    "heading": 3.22396872446115,
-                    "angularVelocity": -0.5892749900676477,
-                    "velocityX": -3.1363332202022756,
-                    "velocityY": 0.416117921367316,
-                    "timestamp": 2.358855785980586
-                },
-                {
-                    "x": 4.4864372535551835,
-                    "y": 5.530355016492048,
-                    "heading": 3.181984877578949,
-                    "angularVelocity": -0.44198756663709243,
-                    "velocityX": -2.352249676050825,
-                    "velocityY": 0.3120890732677509,
-                    "timestamp": 2.4538445369122908
-                },
-                {
-                    "x": 4.337479086660562,
-                    "y": 5.550118338543265,
-                    "heading": 3.1539948813381518,
-                    "angularVelocity": -0.2946664311958542,
-                    "velocityX": -1.5681663926891658,
-                    "velocityY": 0.20805960555714925,
-                    "timestamp": 2.5488332878439954
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": -0.147331986165536,
-                    "velocityX": -0.7840832301722918,
-                    "velocityY": 0.10402980731729136,
-                    "timestamp": 2.6438220387757
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": -1.2414335719227057e-31,
-                    "velocityX": 3.5860623226783847e-34,
-                    "velocityY": 0,
-                    "timestamp": 2.7388107897074048
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -21261,353 +5021,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S3 To C3 To S3": {
+        "C4 To W1": {
             "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 17
-                },
                 {
                     "x": 7.68,
-                    "y": 4.127,
+                    "y": 2.457,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 17
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.312741719551674,
-                    "y": 3.016403262503484,
-                    "heading": 2.6023238516402656,
-                    "angularVelocity": 0.35533614833193405,
-                    "velocityX": 0.624033456373989,
-                    "velocityY": 0.2057867055686534,
-                    "timestamp": 0.07971002042214732
-                },
-                {
-                    "x": 4.412293799902437,
-                    "y": 3.049211318978086,
-                    "heading": 2.6573890862312104,
-                    "angularVelocity": 0.6908194766394113,
-                    "velocityX": 1.248928049742448,
-                    "velocityY": 0.41159262412491066,
-                    "timestamp": 0.15942004084429465
-                },
-                {
-                    "x": 4.5617613818375675,
-                    "y": 3.0984309719087952,
-                    "heading": 2.7365422108568223,
-                    "angularVelocity": 0.9930134781852262,
-                    "velocityX": 1.875141684113798,
-                    "velocityY": 0.6174838830806988,
-                    "timestamp": 0.23913006126644198
-                },
-                {
-                    "x": 4.761327239793144,
-                    "y": 3.1640795845539236,
-                    "heading": 2.8341908945522007,
-                    "angularVelocity": 1.225049036221883,
-                    "velocityX": 2.503648310446647,
-                    "velocityY": 0.8235929723446413,
-                    "timestamp": 0.3188400816885893
-                },
-                {
-                    "x": 5.011274516078503,
-                    "y": 3.2461089339964913,
-                    "heading": 2.9311433561003417,
-                    "angularVelocity": 1.2163145992771967,
-                    "velocityX": 3.1357070912995426,
-                    "velocityY": 1.0290970822506025,
-                    "timestamp": 0.3985501021107366
-                },
-                {
-                    "x": 5.297580410210689,
-                    "y": 3.3407358757733725,
-                    "heading": 2.9311433795557584,
-                    "angularVelocity": 2.942593217144604e-7,
-                    "velocityX": 3.5918431913064333,
-                    "velocityY": 1.1871398511220184,
-                    "timestamp": 0.4782601225328839
-                },
-                {
-                    "x": 5.583886278072779,
-                    "y": 3.4353628970344365,
-                    "heading": 2.931143403010656,
-                    "angularVelocity": 2.942528124810342e-7,
-                    "velocityX": 3.5918428617356097,
-                    "velocityY": 1.187140848288782,
-                    "timestamp": 0.5579701429550312
-                },
-                {
-                    "x": 5.8701921459347055,
-                    "y": 3.529989918295993,
-                    "heading": 2.9311434264655536,
-                    "angularVelocity": 2.942528130233853e-7,
-                    "velocityX": 3.591842861733567,
-                    "velocityY": 1.1871408482949637,
-                    "timestamp": 0.6376801633771786
-                },
-                {
-                    "x": 6.156498013796632,
-                    "y": 3.62461693955755,
-                    "heading": 2.931143449920451,
-                    "angularVelocity": 2.9425281250003654e-7,
-                    "velocityX": 3.5918428617335665,
-                    "velocityY": 1.1871408482949637,
-                    "timestamp": 0.717390183799326
-                },
-                {
-                    "x": 6.442803881658564,
-                    "y": 3.7192439608190906,
-                    "heading": 2.9311434733753488,
-                    "angularVelocity": 2.9425281349790425e-7,
-                    "velocityX": 3.5918428617336366,
-                    "velocityY": 1.1871408482947532,
-                    "timestamp": 0.7971002042214733
-                },
-                {
-                    "x": 6.729109750413145,
-                    "y": 3.813870979379799,
-                    "heading": 2.9311434968302543,
-                    "angularVelocity": 2.942529087425497e-7,
-                    "velocityX": 3.591842872932335,
-                    "velocityY": 1.1871408144115423,
-                    "timestamp": 0.8768102246436207
-                },
-                {
-                    "x": 7.011880136025942,
-                    "y": 3.9068513947536463,
-                    "heading": 2.940966575440456,
-                    "angularVelocity": 0.12323517869119305,
-                    "velocityX": 3.547488560600464,
-                    "velocityY": 1.1664833967099661,
-                    "timestamp": 0.956520245065768
-                },
-                {
-                    "x": 7.2458966997348915,
-                    "y": 3.984032577425885,
-                    "heading": 2.999027591028601,
-                    "angularVelocity": 0.7284029696724628,
-                    "velocityX": 2.935848748621424,
-                    "velocityY": 0.9682745314012468,
-                    "timestamp": 1.0362302654879154
-                },
-                {
-                    "x": 7.429606919081267,
-                    "y": 4.044603850649925,
-                    "heading": 3.057792091345064,
-                    "angularVelocity": 0.7372285191403096,
-                    "velocityX": 2.304731806282817,
-                    "velocityY": 0.7598953419313301,
-                    "timestamp": 1.1159402859100627
-                },
-                {
-                    "x": 7.563114805498745,
-                    "y": 4.088594555269305,
-                    "heading": 3.1044636591164667,
-                    "angularVelocity": 0.5855169466050564,
-                    "velocityX": 1.674919736695776,
-                    "velocityY": 0.55188424725527,
-                    "timestamp": 1.19565030633221
-                },
-                {
-                    "x": 7.646547133770953,
-                    "y": 4.116048088709135,
-                    "heading": 3.133033734418083,
-                    "angularVelocity": 0.35842514090835725,
-                    "velocityX": 1.046698116878499,
-                    "velocityY": 0.34441759385375514,
-                    "timestamp": 1.2753603267543572
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": 0.0873951047186249,
-                    "velocityX": 0.4196820682252798,
-                    "velocityY": 0.13739691989618263,
-                    "timestamp": 1.3550703471765044
-                },
-                {
-                    "x": 7.658055452006423,
-                    "y": 4.119659499210501,
-                    "heading": 3.1197811319835678,
-                    "angularVelocity": -0.235277452949756,
-                    "velocityX": -0.2553583788106411,
-                    "velocityY": -0.08541795355335377,
-                    "timestamp": 1.4410066226799219
-                },
-                {
-                    "x": 7.578036609646341,
-                    "y": 4.0931397899338755,
-                    "heading": 3.0739363035782867,
-                    "angularVelocity": -0.5334746954847726,
-                    "velocityX": -0.9311416150086622,
-                    "velocityY": -0.30859737778105817,
-                    "timestamp": 1.5269428981833393
-                },
-                {
-                    "x": 7.439853098092205,
-                    "y": 4.047395776011725,
-                    "heading": 3.0057619833378117,
-                    "angularVelocity": -0.7933124846417565,
-                    "velocityX": -1.6079765005482662,
-                    "velocityY": -0.5323015647836847,
-                    "timestamp": 1.6128791736867567
-                },
-                {
-                    "x": 7.243372469557272,
-                    "y": 3.982360857227892,
-                    "heading": 2.9212012505051064,
-                    "angularVelocity": -0.9839934572140336,
-                    "velocityX": -2.2863526186577556,
-                    "velocityY": -0.7567807471624278,
-                    "timestamp": 1.6988154491901741
-                },
-                {
-                    "x": 6.9884466912904,
-                    "y": 3.897942797791821,
-                    "heading": 2.8348834977989155,
-                    "angularVelocity": -1.0044390707013913,
-                    "velocityX": -2.9664513242342614,
-                    "velocityY": -0.9823332340335614,
-                    "timestamp": 1.7847517246935916
-                },
-                {
-                    "x": 6.679412229111129,
-                    "y": 3.7970350761603098,
-                    "heading": 2.8348833945193865,
-                    "angularVelocity": -0.000001201815274119231,
-                    "velocityX": -3.596088617629014,
-                    "velocityY": -1.1742156736534226,
-                    "timestamp": 1.870688000197009
-                },
-                {
-                    "x": 6.370694817722953,
-                    "y": 3.695161383669821,
-                    "heading": 2.8348833730448595,
-                    "angularVelocity": -2.4988896913294006e-7,
-                    "velocityX": -3.5923992467639496,
-                    "velocityY": -1.1854562219936775,
-                    "timestamp": 1.9566242757004264
-                },
-                {
-                    "x": 6.061977407921374,
-                    "y": 3.59328768637132,
-                    "heading": 2.834883351570332,
-                    "angularVelocity": -2.498889692815502e-7,
-                    "velocityX": -3.592399228301472,
-                    "velocityY": -1.185456277942254,
-                    "timestamp": 2.042560551203844
-                },
-                {
-                    "x": 5.753259998119802,
-                    "y": 3.4914139890727944,
-                    "heading": 2.834883330095805,
-                    "angularVelocity": -2.4988896886019487e-7,
-                    "velocityX": -3.59239922830138,
-                    "velocityY": -1.1854562779425326,
-                    "timestamp": 2.128496826707261
-                },
-                {
-                    "x": 5.444542588318229,
-                    "y": 3.3895402917742743,
-                    "heading": 2.8348833086212775,
-                    "angularVelocity": -2.4988896799949907e-7,
-                    "velocityX": -3.5923992283013995,
-                    "velocityY": -1.1854562779424735,
-                    "timestamp": 2.2144331022106782
-                },
-                {
-                    "x": 5.135825178181493,
-                    "y": 3.28766659549148,
-                    "heading": 2.8348832871467184,
-                    "angularVelocity": -2.498893376295858e-7,
-                    "velocityX": -3.5923992322015317,
-                    "velocityY": -1.1854562661229522,
-                    "timestamp": 2.3003693777140954
-                },
-                {
-                    "x": 4.844823310807952,
-                    "y": 3.1918227901947045,
-                    "heading": 2.7888021821480646,
-                    "angularVelocity": -0.5362241350199245,
-                    "velocityX": -3.386251797263078,
-                    "velocityY": -1.1152892621342902,
-                    "timestamp": 2.3863056532175126
-                },
-                {
-                    "x": 4.611844119098646,
-                    "y": 3.115032101312665,
-                    "heading": 2.714656158607103,
-                    "angularVelocity": -0.8628023859145838,
-                    "velocityX": -2.7110692236137233,
-                    "velocityY": -0.8935771120193101,
-                    "timestamp": 2.47224192872093
-                },
-                {
-                    "x": 4.43731744566658,
-                    "y": 3.0574895143251735,
-                    "heading": 2.6481597119281033,
-                    "angularVelocity": -0.7737878595443164,
-                    "velocityX": -2.030884773742906,
-                    "velocityY": -0.669596007627809,
-                    "timestamp": 2.558178204224347
-                },
-                {
-                    "x": 4.321077385755849,
-                    "y": 3.019155902534149,
-                    "heading": 2.5996556036782517,
-                    "angularVelocity": -0.5644194836896647,
-                    "velocityX": -1.3526308794486799,
-                    "velocityY": -0.44607020221047544,
-                    "timestamp": 2.6441144797277643
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": -0.2985421875449052,
-                    "velocityX": -0.675819209241155,
-                    "velocityY": -0.2229082238197189,
-                    "timestamp": 2.7300507552311815
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": 0,
-                    "velocityX": -1.2326632641143652e-35,
-                    "velocityY": 0,
-                    "timestamp": 2.8159870307345987
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -21627,362 +5062,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S3 To C3 To S4": {
+        "C4 To W2": {
             "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 17
-                },
                 {
                     "x": 7.68,
-                    "y": 4.127,
+                    "y": 2.457,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 18
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -2.99079667903027e-32,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.307911912995487,
-                    "y": 3.0252386980683,
-                    "heading": 2.6037905893528444,
-                    "angularVelocity": 0.3765521075910298,
-                    "velocityX": 0.5676851603736718,
-                    "velocityY": 0.31901634566234616,
-                    "timestamp": 0.07911412192970564
-                },
-                {
-                    "x": 4.397813948925716,
-                    "y": 3.0757408190390847,
-                    "heading": 2.6615026890848092,
-                    "angularVelocity": 0.7294791160450911,
-                    "velocityX": 1.1363588919069176,
-                    "velocityY": 0.6383452124471162,
-                    "timestamp": 0.15822824385941128
-                },
-                {
-                    "x": 4.532822684228442,
-                    "y": 3.151547007950623,
-                    "heading": 2.744086644451416,
-                    "angularVelocity": 1.0438585849437199,
-                    "velocityX": 1.7065061459278321,
-                    "velocityY": 0.9581878312306107,
-                    "timestamp": 0.23734236578911694
-                },
-                {
-                    "x": 4.713127943842242,
-                    "y": 3.2527364142046724,
-                    "heading": 2.845383620284061,
-                    "angularVelocity": 1.2803905720226456,
-                    "velocityX": 2.2790527811710266,
-                    "velocityY": 1.2790308959500052,
-                    "timestamp": 0.31645648771882257
-                },
-                {
-                    "x": 4.938980637634121,
-                    "y": 3.3793948659075563,
-                    "heading": 2.944365706531666,
-                    "angularVelocity": 1.251130440852921,
-                    "velocityX": 2.854770909200696,
-                    "velocityY": 1.6009588252198856,
-                    "timestamp": 0.3955706096485282
-                },
-                {
-                    "x": 5.199953974250007,
-                    "y": 3.5259002853585115,
-                    "heading": 2.944365731476778,
-                    "angularVelocity": 3.1530542976951733e-7,
-                    "velocityX": 3.2986947241576456,
-                    "velocityY": 1.8518238700939902,
-                    "timestamp": 0.4746847315782338
-                },
-                {
-                    "x": 5.460927260915445,
-                    "y": 3.6724057937882604,
-                    "heading": 2.944365756420666,
-                    "angularVelocity": 3.152899521547411e-7,
-                    "velocityX": 3.2986940927855755,
-                    "velocityY": 1.8518249947831291,
-                    "timestamp": 0.5537988535079394
-                },
-                {
-                    "x": 5.721900547580569,
-                    "y": 3.818911302218567,
-                    "heading": 2.9443657813645534,
-                    "angularVelocity": 3.1528995198652365e-7,
-                    "velocityX": 3.2986940927816133,
-                    "velocityY": 1.851824994790188,
-                    "timestamp": 0.6329129754376451
-                },
-                {
-                    "x": 5.982873834247296,
-                    "y": 3.965416810646019,
-                    "heading": 2.9443658063084412,
-                    "angularVelocity": 3.1528995220649985e-7,
-                    "velocityX": 3.2986940928018726,
-                    "velocityY": 1.851824994754099,
-                    "timestamp": 0.7120270973673508
-                },
-                {
-                    "x": 6.243847376304821,
-                    "y": 4.1119218641396325,
-                    "heading": 2.944365831252335,
-                    "angularVelocity": 3.152900317708399e-7,
-                    "velocityX": 3.298697320933481,
-                    "velocityY": 1.8518192444047645,
-                    "timestamp": 0.7911412192970565
-                },
-                {
-                    "x": 6.520761156875476,
-                    "y": 4.225454581965132,
-                    "heading": 2.9443658572464595,
-                    "angularVelocity": 3.2856491270029264e-7,
-                    "velocityX": 3.5001814317891196,
-                    "velocityY": 1.4350499639795518,
-                    "timestamp": 0.8702553412267622
-                },
-                {
-                    "x": 6.81318632649156,
-                    "y": 4.28915727346349,
-                    "heading": 2.9443676715667886,
-                    "angularVelocity": 0.000022932951594388774,
-                    "velocityX": 3.696244898931049,
-                    "velocityY": 0.8052000065798415,
-                    "timestamp": 0.9493694631564679
-                },
-                {
-                    "x": 7.072106261164272,
-                    "y": 4.3159902610395875,
-                    "heading": 2.999666770720341,
-                    "angularVelocity": 0.6989788650209022,
-                    "velocityX": 3.2727397885142184,
-                    "velocityY": 0.3391681146374708,
-                    "timestamp": 1.0284835850861735
-                },
-                {
-                    "x": 7.2892635874980165,
-                    "y": 4.311538961400137,
-                    "heading": 3.058239945124217,
-                    "angularVelocity": 0.7403630726751829,
-                    "velocityX": 2.7448617394337207,
-                    "velocityY": -0.05626428671490304,
-                    "timestamp": 1.1075977070158791
-                },
-                {
-                    "x": 7.463265555506274,
-                    "y": 4.277921305765472,
-                    "heading": 3.1051985992516418,
-                    "angularVelocity": 0.593555903573676,
-                    "velocityX": 2.1993793745554173,
-                    "velocityY": -0.4249261044006207,
-                    "timestamp": 1.1867118289455847
-                },
-                {
-                    "x": 7.593597601326297,
-                    "y": 4.21619741378488,
-                    "heading": 3.1337465954664303,
-                    "angularVelocity": 0.3608457696105646,
-                    "velocityX": 1.6473929387198967,
-                    "velocityY": -0.7801880432350068,
-                    "timestamp": 1.2658259508752903
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": 0.07904283560305705,
-                    "velocityX": 1.0921235876253772,
-                    "velocityY": -1.1274524902663325,
-                    "timestamp": 1.3449400728049958
-                },
-                {
-                    "x": 7.722328891064707,
-                    "y": 4.011878570496831,
-                    "heading": 3.1218227549328446,
-                    "angularVelocity": -0.23159012596074638,
-                    "velocityX": 0.539298071695534,
-                    "velocityY": -1.4667231618939216,
-                    "timestamp": 1.423428933207607
-                },
-                {
-                    "x": 7.720975293339473,
-                    "y": 3.870431716581803,
-                    "heading": 3.0818439654671956,
-                    "angularVelocity": -0.5093562227885172,
-                    "velocityX": -0.01724573039142055,
-                    "velocityY": -1.8021264825284895,
-                    "timestamp": 1.5019177936102184
-                },
-                {
-                    "x": 7.675510236414314,
-                    "y": 3.703145056798568,
-                    "heading": 3.02401855987363,
-                    "angularVelocity": -0.7367339173603616,
-                    "velocityX": -0.5792548992550559,
-                    "velocityY": -2.131342701692084,
-                    "timestamp": 1.5804066540128296
-                },
-                {
-                    "x": 7.585250814368908,
-                    "y": 3.51090702633694,
-                    "heading": 2.955217915344013,
-                    "angularVelocity": -0.8765657212590741,
-                    "velocityX": -1.149964741269228,
-                    "velocityY": -2.4492396688591995,
-                    "timestamp": 1.658895514415441
-                },
-                {
-                    "x": 7.449012873441734,
-                    "y": 3.2957924623075137,
-                    "heading": 2.8903514658300926,
-                    "angularVelocity": -0.8264414743848527,
-                    "velocityX": -1.7357614854940644,
-                    "velocityY": -2.7407018387830946,
-                    "timestamp": 1.7373843748180522
-                },
-                {
-                    "x": 7.265754194056636,
-                    "y": 3.066909845502957,
-                    "heading": 2.8806156888219427,
-                    "angularVelocity": -0.12404023906336885,
-                    "velocityX": -2.334836796522032,
-                    "velocityY": -2.916115938370047,
-                    "timestamp": 1.8158732352206635
-                },
-                {
-                    "x": 7.0429291564371885,
-                    "y": 2.8706709413875,
-                    "heading": 2.8806156606364275,
-                    "angularVelocity": -3.5910210875012317e-7,
-                    "velocityX": -2.838938372610575,
-                    "velocityY": -2.5002134456895257,
-                    "timestamp": 1.8943620956232747
-                },
-                {
-                    "x": 6.796142716598087,
-                    "y": 2.7055735191439014,
-                    "heading": 2.8806156351575583,
-                    "angularVelocity": -3.246176492669905e-7,
-                    "velocityX": -3.1442224867732946,
-                    "velocityY": -2.103450367309771,
-                    "timestamp": 1.972850956025886
-                },
-                {
-                    "x": 6.549355877169833,
-                    "y": 2.5404766942048504,
-                    "heading": 2.880615609678698,
-                    "angularVelocity": -3.246175317472835e-7,
-                    "velocityX": -3.1442275778034396,
-                    "velocityY": -2.10344275725474,
-                    "timestamp": 2.0513398164284973
-                },
-                {
-                    "x": 6.302569037739046,
-                    "y": 2.375379869269585,
-                    "heading": 2.8806155841998384,
-                    "angularVelocity": -3.24617530454395e-7,
-                    "velocityX": -3.144227577835705,
-                    "velocityY": -2.103442757206509,
-                    "timestamp": 2.1298286768311088
-                },
-                {
-                    "x": 6.055782203816465,
-                    "y": 2.2102830361006496,
-                    "heading": 2.8806155587209785,
-                    "angularVelocity": -3.246175312544412e-7,
-                    "velocityX": -3.1442275076575137,
-                    "velocityY": -2.103442862108911,
-                    "timestamp": 2.2083175372337203
-                },
-                {
-                    "x": 5.809854810532114,
-                    "y": 2.043908693921413,
-                    "heading": 2.8806155277577266,
-                    "angularVelocity": -3.944923106855428e-7,
-                    "velocityX": -3.1332776654274115,
-                    "velocityY": -2.1197191719412425,
-                    "timestamp": 2.2868063976363318
-                },
-                {
-                    "x": 5.598037560183058,
-                    "y": 1.9023806628591073,
-                    "heading": 2.776131644051983,
-                    "angularVelocity": -1.3311937919570889,
-                    "velocityX": -2.6986918814024308,
-                    "velocityY": -1.8031607330815247,
-                    "timestamp": 2.3652952580389432
-                },
-                {
-                    "x": 5.428977486755044,
-                    "y": 1.7893186281455802,
-                    "heading": 2.67268813100655,
-                    "angularVelocity": -1.3179387815648507,
-                    "velocityX": -2.1539371645965275,
-                    "velocityY": -1.4404851100343643,
-                    "timestamp": 2.4437841184415547
-                },
-                {
-                    "x": 5.302396782822421,
-                    "y": 1.7046186401543708,
-                    "heading": 2.5887102200267456,
-                    "angularVelocity": -1.0699341352267786,
-                    "velocityX": -1.6127218981562843,
-                    "velocityY": -1.0791338739884617,
-                    "timestamp": 2.5222729788441662
-                },
-                {
-                    "x": 5.218108357184172,
-                    "y": 1.6481959100495345,
-                    "heading": 2.5301489625141826,
-                    "angularVelocity": -0.7461091575565081,
-                    "velocityX": -1.0738902973732658,
-                    "velocityY": -0.7188629037982504,
-                    "timestamp": 2.6007618392467777
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": -0.3841177252355578,
-                    "velocityX": -0.5364883241797085,
-                    "velocityY": -0.35923454493927354,
-                    "timestamp": 2.6792506996493892
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": 0,
-                    "velocityX": -9.31070457943821e-33,
-                    "velocityY": 1.510534612718486e-32,
-                    "timestamp": 2.7577395600520007
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -22002,17 +5103,8 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S3 To C4 To S3": {
+        "C4 To W3": {
             "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 17
-                },
                 {
                     "x": 7.68,
                     "y": 2.457,
@@ -22020,335 +5112,19 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 17
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -2.4689012164756437e-31,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.313159042990802,
-                    "y": 2.9920825634147805,
-                    "heading": 2.597885779471472,
-                    "angularVelocity": 0.30476728851208423,
-                    "velocityX": 0.6399973484188943,
-                    "velocityY": -0.10102143339823635,
-                    "timestamp": 0.07837382938338545
-                },
-                {
-                    "x": 4.4134829053058295,
-                    "y": 2.976248991975609,
-                    "heading": 2.6455288467924127,
-                    "angularVelocity": 0.6078951060041105,
-                    "velocityX": 1.2800683991625412,
-                    "velocityY": -0.20202625753700879,
-                    "timestamp": 0.1567476587667709
-                },
-                {
-                    "x": 4.563981380382106,
-                    "y": 2.952502286743102,
-                    "heading": 2.7167039498074406,
-                    "angularVelocity": 0.9081488498779698,
-                    "velocityX": 1.9202644079068125,
-                    "velocityY": -0.3029927900593087,
-                    "timestamp": 0.23512148815015638
-                },
-                {
-                    "x": 4.764676150331059,
-                    "y": 2.9208479584714695,
-                    "heading": 2.8108575648883316,
-                    "angularVelocity": 1.2013399858304654,
-                    "velocityX": 2.560737066543005,
-                    "velocityY": -0.40388900887800433,
-                    "timestamp": 0.3134953175335418
-                },
-                {
-                    "x": 5.015633164055166,
-                    "y": 2.881293970992892,
-                    "heading": 2.92608193331875,
-                    "angularVelocity": 1.47018933918326,
-                    "velocityX": 3.2020511910485845,
-                    "velocityY": -0.5046836142851278,
-                    "timestamp": 0.39186914691692726
-                },
-                {
-                    "x": 5.308481077006102,
-                    "y": 2.835005929387577,
-                    "heading": 2.926081954236969,
-                    "angularVelocity": 2.6690310524294676e-7,
-                    "velocityX": 3.7365523064899215,
-                    "velocityY": -0.5906058434235878,
-                    "timestamp": 0.4702429763003127
-                },
-                {
-                    "x": 5.601350966367862,
-                    "y": 2.7888571291196405,
-                    "heading": 2.92608197144068,
-                    "angularVelocity": 2.1950836119943446e-7,
-                    "velocityX": 3.7368327114541455,
-                    "velocityY": -0.5888292129019572,
-                    "timestamp": 0.5486168056836982
-                },
-                {
-                    "x": 5.8942208558716445,
-                    "y": 2.7427083297530035,
-                    "heading": 2.9260819886443907,
-                    "angularVelocity": 2.1950836114981294e-7,
-                    "velocityX": 3.736832713266251,
-                    "velocityY": -0.5888292014019535,
-                    "timestamp": 0.6269906350670836
-                },
-                {
-                    "x": 6.187090745375428,
-                    "y": 2.6965595303863723,
-                    "heading": 2.9260820058481016,
-                    "angularVelocity": 2.195083613443253e-7,
-                    "velocityX": 3.736832713266263,
-                    "velocityY": -0.5888292014018791,
-                    "timestamp": 0.7053644644504691
-                },
-                {
-                    "x": 6.479960634879199,
-                    "y": 2.6504107310196727,
-                    "heading": 2.9260820230518125,
-                    "angularVelocity": 2.1950836077924463e-7,
-                    "velocityX": 3.736832713266126,
-                    "velocityY": -0.5888292014027512,
-                    "timestamp": 0.7837382938338545
-                },
-                {
-                    "x": 6.772830522718861,
-                    "y": 2.604261921092308,
-                    "heading": 2.926082040255567,
-                    "angularVelocity": 2.1950891675310117e-7,
-                    "velocityX": 3.736832692033143,
-                    "velocityY": -0.5888293361500981,
-                    "timestamp": 0.86211212321724
-                },
-                {
-                    "x": 7.050151670300153,
-                    "y": 2.559252271073386,
-                    "heading": 2.978396963955354,
-                    "angularVelocity": 0.6675050091514937,
-                    "velocityX": 3.5384406984211854,
-                    "velocityY": -0.5742943833809976,
-                    "timestamp": 0.9404859526006254
-                },
-                {
-                    "x": 7.276980321576378,
-                    "y": 2.5226261995289687,
-                    "heading": 3.0372586376427035,
-                    "angularVelocity": 0.7510373571182644,
-                    "velocityX": 2.894188698712594,
-                    "velocityY": -0.4673252772333,
-                    "timestamp": 1.0188597819840108
-                },
-                {
-                    "x": 7.453315564542908,
-                    "y": 2.49415241035015,
-                    "heading": 3.0866031396716647,
-                    "angularVelocity": 0.6296043260507225,
-                    "velocityX": 2.2499250624075064,
-                    "velocityY": -0.3633073616900632,
-                    "timestamp": 1.0972336113673964
-                },
-                {
-                    "x": 7.579233655925443,
-                    "y": 2.473748793081471,
-                    "heading": 3.1215813110163633,
-                    "angularVelocity": 0.446299123315424,
-                    "velocityX": 1.6066344132117514,
-                    "velocityY": -0.2603371231087117,
-                    "timestamp": 1.175607440750782
-                },
-                {
-                    "x": 7.654784082070509,
-                    "y": 2.4613731780152506,
-                    "heading": 3.1398442445430734,
-                    "angularVelocity": 0.2330233659676447,
-                    "velocityX": 0.9639751781872319,
-                    "velocityY": -0.1579049430605922,
-                    "timestamp": 1.2539812701341675
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": 0.0019873401380948097,
-                    "velocityX": 0.32173900558233287,
-                    "velocityY": -0.05579895801519462,
-                    "timestamp": 1.332355099517553
-                },
-                {
-                    "x": 7.650333485517468,
-                    "y": 2.4613165476638152,
-                    "heading": 3.1188382258181035,
-                    "angularVelocity": -0.255122622507773,
-                    "velocityX": -0.3576542737105397,
-                    "velocityY": 0.05203953840108144,
-                    "timestamp": 1.4153025620901392
-                },
-                {
-                    "x": 7.564253765684918,
-                    "y": 2.4745901696035957,
-                    "heading": 3.07855569322522,
-                    "angularVelocity": -0.4856391183470325,
-                    "velocityX": -1.0377619418703934,
-                    "velocityY": 0.16002444834484145,
-                    "timestamp": 1.4982500246627253
-                },
-                {
-                    "x": 7.42168176124644,
-                    "y": 2.4968358287790196,
-                    "heading": 3.0225573774767818,
-                    "angularVelocity": -0.6751058321938196,
-                    "velocityX": -1.7188229756119993,
-                    "velocityY": 0.2681897490952097,
-                    "timestamp": 1.5811974872353114
-                },
-                {
-                    "x": 7.222516189064582,
-                    "y": 2.528071510677467,
-                    "heading": 2.9567671784015253,
-                    "angularVelocity": -0.7931550530274372,
-                    "velocityX": -2.401105061020643,
-                    "velocityY": 0.37657187971380685,
-                    "timestamp": 1.6641449498078975
-                },
-                {
-                    "x": 6.966699685219415,
-                    "y": 2.5683095941313905,
-                    "heading": 2.8941605042172336,
-                    "angularVelocity": -0.7547750376269705,
-                    "velocityX": -3.0840787157449774,
-                    "velocityY": 0.48510324735627736,
-                    "timestamp": 1.7470924123804836
-                },
-                {
-                    "x": 6.656762628445554,
-                    "y": 2.6172997543511465,
-                    "heading": 2.894159452148734,
-                    "angularVelocity": -0.000012683552539851008,
-                    "velocityX": -3.736546570097186,
-                    "velocityY": 0.5906167434222167,
-                    "timestamp": 1.8300398749530697
-                },
-                {
-                    "x": 6.3468583527209415,
-                    "y": 2.666498972584932,
-                    "heading": 2.8941594322307855,
-                    "angularVelocity": -2.401272821235818e-7,
-                    "velocityX": -3.736151367540867,
-                    "velocityY": 0.5931371100198551,
-                    "timestamp": 1.9129873375256559
-                },
-                {
-                    "x": 6.036954077181096,
-                    "y": 2.715698191982551,
-                    "heading": 2.894159412312837,
-                    "angularVelocity": -2.4012728242851975e-7,
-                    "velocityX": -3.736151365313363,
-                    "velocityY": 0.5931371240508304,
-                    "timestamp": 1.995934800098242
-                },
-                {
-                    "x": 5.72704980164125,
-                    "y": 2.7648974113801765,
-                    "heading": 2.8941593923948883,
-                    "angularVelocity": -2.401272814815578e-7,
-                    "velocityX": -3.7361513653133502,
-                    "velocityY": 0.5931371240509082,
-                    "timestamp": 2.078882262670828
-                },
-                {
-                    "x": 5.417145526101417,
-                    "y": 2.8140966307778834,
-                    "heading": 2.8941593724769397,
-                    "angularVelocity": -2.4012728169619825e-7,
-                    "velocityX": -3.7361513653131957,
-                    "velocityY": 0.5931371240518842,
-                    "timestamp": 2.1618297252434138
-                },
-                {
-                    "x": 5.10724125286934,
-                    "y": 2.8632958647117834,
-                    "heading": 2.894159352558893,
-                    "angularVelocity": -2.401284654616401e-7,
-                    "velocityX": -3.7361513374913162,
-                    "velocityY": 0.59313729929767,
-                    "timestamp": 2.2447771878159997
-                },
-                {
-                    "x": 4.825498631830447,
-                    "y": 2.909240953390356,
-                    "heading": 2.811052225227297,
-                    "angularVelocity": -1.0019248902143632,
-                    "velocityX": -3.3966394185035327,
-                    "velocityY": 0.5539058972221842,
-                    "timestamp": 2.3277246503885856
-                },
-                {
-                    "x": 4.60031373082884,
-                    "y": 2.9456643222027448,
-                    "heading": 2.723257007302943,
-                    "angularVelocity": -1.0584436847304166,
-                    "velocityX": -2.7147895067260346,
-                    "velocityY": 0.43911371948857386,
-                    "timestamp": 2.4106721129611715
-                },
-                {
-                    "x": 4.431586205140753,
-                    "y": 2.9728717926897983,
-                    "heading": 2.6509541339197864,
-                    "angularVelocity": -0.871670707466155,
-                    "velocityX": -2.0341493332654417,
-                    "velocityY": 0.3280084723899037,
-                    "timestamp": 2.4936195755337573
-                },
-                {
-                    "x": 4.3191766366235065,
-                    "y": 2.9909670138264888,
-                    "heading": 2.600230923186435,
-                    "angularVelocity": -0.6115100951879456,
-                    "velocityX": -1.3551899603785846,
-                    "velocityY": 0.21815279907876073,
-                    "timestamp": 2.5765670381063432
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": -0.31623539012403434,
-                    "velocityX": -0.6772556372577037,
-                    "velocityY": 0.10890009041091805,
-                    "timestamp": 2.659514500678929
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": 0,
-                    "velocityX": -4.6317137057442136e-32,
-                    "velocityY": 0,
-                    "timestamp": 2.742461963251515
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -22368,17 +5144,8 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S3 To C4 To S4": {
+        "C4 To S3": {
             "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 17
-                },
                 {
                     "x": 7.68,
                     "y": 2.457,
@@ -22386,317 +5153,19 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 15
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.312461905031287,
-                    "y": 3.0009727246573483,
-                    "heading": 2.5993071037453466,
-                    "angularVelocity": 0.3268807464808282,
-                    "velocityX": 0.6388777080808309,
-                    "velocityY": 0.012564257266019683,
-                    "timestamp": 0.07741998884241831
-                },
-                {
-                    "x": 4.411454270150545,
-                    "y": 3.0029274539385553,
-                    "heading": 2.648169844004936,
-                    "angularVelocity": 0.6311385598239377,
-                    "velocityX": 1.2786409117256237,
-                    "velocityY": 0.025248379784522566,
-                    "timestamp": 0.15483997768483662
-                },
-                {
-                    "x": 4.560077070415931,
-                    "y": 3.0058795423873246,
-                    "heading": 2.717741930312934,
-                    "angularVelocity": 0.8986320890539764,
-                    "velocityX": 1.9196954492966272,
-                    "velocityY": 0.03813083020172101,
-                    "timestamp": 0.23225996652725495
-                },
-                {
-                    "x": 4.758482787325499,
-                    "y": 3.0098532742147075,
-                    "heading": 2.8026323249857676,
-                    "angularVelocity": 1.0964919517828993,
-                    "velocityX": 2.562719523421868,
-                    "velocityY": 0.05132694910962975,
-                    "timestamp": 0.30967995536967324
-                },
-                {
-                    "x": 5.006851286997014,
-                    "y": 3.0148884552327933,
-                    "heading": 2.889629605764704,
-                    "angularVelocity": 1.1237056744610465,
-                    "velocityX": 3.2080668492092808,
-                    "velocityY": 0.06503722221317501,
-                    "timestamp": 0.38709994421209154
-                },
-                {
-                    "x": 5.299689191212239,
-                    "y": 3.01955932024386,
-                    "heading": 2.889629778554125,
-                    "angularVelocity": 0.000002231845078236991,
-                    "velocityX": 3.7824586207480784,
-                    "velocityY": 0.060331512325246216,
-                    "timestamp": 0.46451993305450984
-                },
-                {
-                    "x": 5.592507772821644,
-                    "y": 3.025318229635612,
-                    "heading": 2.8896298031470753,
-                    "angularVelocity": 3.176563362313136e-7,
-                    "velocityX": 3.782209039133448,
-                    "velocityY": 0.07438530381958017,
-                    "timestamp": 0.5419399218969282
-                },
-                {
-                    "x": 5.8853263542850165,
-                    "y": 3.0310771464526267,
-                    "heading": 2.889629827740025,
-                    "angularVelocity": 3.176563361839336e-7,
-                    "velocityX": 3.782209037247192,
-                    "velocityY": 0.07438539972844682,
-                    "timestamp": 0.6193599107393465
-                },
-                {
-                    "x": 6.178144935748446,
-                    "y": 3.0368360632666453,
-                    "heading": 2.8896298523329755,
-                    "angularVelocity": 3.1765633664876355e-7,
-                    "velocityX": 3.782209037247953,
-                    "velocityY": 0.07438539968973952,
-                    "timestamp": 0.6967798995817648
-                },
-                {
-                    "x": 6.4709635259990606,
-                    "y": 3.0425945332689603,
-                    "heading": 2.889629876925932,
-                    "angularVelocity": 3.176564212988765e-7,
-                    "velocityX": 3.7822091507481463,
-                    "velocityY": 0.07437962841916478,
-                    "timestamp": 0.7741998884241831
-                },
-                {
-                    "x": 6.762585697804425,
-                    "y": 3.0155317439126783,
-                    "heading": 2.889629906398516,
-                    "angularVelocity": 3.80684423008717e-7,
-                    "velocityX": 3.766755539049949,
-                    "velocityY": -0.3495581665784765,
-                    "timestamp": 0.8516198772666014
-                },
-                {
-                    "x": 7.037113234733603,
-                    "y": 2.9421863256805914,
-                    "heading": 2.9183632792459075,
-                    "angularVelocity": 0.37113635996351935,
-                    "velocityX": 3.5459516467763192,
-                    "velocityY": -0.9473705606103254,
-                    "timestamp": 0.9290398661090197
-                },
-                {
-                    "x": 7.2647684874925345,
-                    "y": 2.8553520162741264,
-                    "heading": 2.9862391901434,
-                    "angularVelocity": 0.8767233360837648,
-                    "velocityX": 2.9405229342296155,
-                    "velocityY": -1.1216006448051647,
-                    "timestamp": 1.006459854951438
-                },
-                {
-                    "x": 7.442903065756583,
-                    "y": 2.761816317140275,
-                    "heading": 3.0514695730897063,
-                    "angularVelocity": 0.8425522132155971,
-                    "velocityX": 2.3008861267937584,
-                    "velocityY": -1.208159553267764,
-                    "timestamp": 1.0838798437938564
-                },
-                {
-                    "x": 7.571438256526523,
-                    "y": 2.6636606141768233,
-                    "heading": 3.1022478511869496,
-                    "angularVelocity": 0.6558807209414358,
-                    "velocityX": 1.660232618110589,
-                    "velocityY": -1.2678341140456517,
-                    "timestamp": 1.1612998326362747
-                },
-                {
-                    "x": 7.650445589096316,
-                    "y": 2.561865725059965,
-                    "heading": 3.1328810502986992,
-                    "angularVelocity": 0.39567558158786426,
-                    "velocityX": 1.0205030219082787,
-                    "velocityY": -1.3148398835868218,
-                    "timestamp": 1.238719821478693
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": 0.09195234729096653,
-                    "velocityX": 0.38174134801077375,
-                    "velocityY": -1.354504523029707,
-                    "timestamp": 1.3161398103211113
-                },
-                {
-                    "x": 7.659642368469709,
-                    "y": 2.34860980319894,
-                    "heading": 3.1210552256706268,
-                    "angularVelocity": -0.24288242827145334,
-                    "velocityX": -0.26099603478867267,
-                    "velocityY": -1.3896219475702174,
-                    "timestamp": 1.3941395835620152
-                },
-                {
-                    "x": 7.589031675721614,
-                    "y": 2.237822483491401,
-                    "heading": 3.078729120037355,
-                    "angularVelocity": -0.5426439574708317,
-                    "velocityX": -0.9052679234080746,
-                    "velocityY": -1.4203543818694149,
-                    "timestamp": 1.472139356802919
-                },
-                {
-                    "x": 7.468012328059872,
-                    "y": 2.1251640911848773,
-                    "heading": 3.017134343801584,
-                    "angularVelocity": -0.7896789141365163,
-                    "velocityX": -1.5515346087990811,
-                    "velocityY": -1.4443425618504895,
-                    "timestamp": 1.550139130043823
-                },
-                {
-                    "x": 7.29639145154735,
-                    "y": 2.0115501518980783,
-                    "heading": 2.9431614871389193,
-                    "angularVelocity": -0.948372714292366,
-                    "velocityX": -2.2002740441625033,
-                    "velocityY": -1.4565931997763661,
-                    "timestamp": 1.6281389032847269
-                },
-                {
-                    "x": 7.0740549155451475,
-                    "y": 1.8989556561262921,
-                    "heading": 2.8711554567434443,
-                    "angularVelocity": -0.9231569196115829,
-                    "velocityX": -2.8504766970989883,
-                    "velocityY": -1.44352337312617,
-                    "timestamp": 1.7061386765256308
-                },
-                {
-                    "x": 6.804126889371604,
-                    "y": 1.794205881873725,
-                    "heading": 2.8575112203208874,
-                    "angularVelocity": -0.17492661652254402,
-                    "velocityX": -3.460625780793808,
-                    "velocityY": -1.3429497279312002,
-                    "timestamp": 1.7841384497665347
-                },
-                {
-                    "x": 6.514819933259115,
-                    "y": 1.7361808711845228,
-                    "heading": 2.8575111902020094,
-                    "angularVelocity": -3.8614058154759053e-7,
-                    "velocityX": -3.7090743228055154,
-                    "velocityY": -0.7439125561300094,
-                    "timestamp": 1.8621382230074386
-                },
-                {
-                    "x": 6.220884578970619,
-                    "y": 1.7103463006817032,
-                    "heading": 2.8575111626758107,
-                    "angularVelocity": -3.52901004822605e-7,
-                    "velocityX": -3.768412933466744,
-                    "velocityY": -0.3312134052368198,
-                    "timestamp": 1.9401379962483425
-                },
-                {
-                    "x": 5.926733464556045,
-                    "y": 1.687097421621547,
-                    "heading": 2.857511000804422,
-                    "angularVelocity": -0.000002075280251147508,
-                    "velocityX": -3.771179096970985,
-                    "velocityY": -0.2980634185736858,
-                    "timestamp": 2.0181377694892464
-                },
-                {
-                    "x": 5.675971267912864,
-                    "y": 1.6645564964621176,
-                    "heading": 2.7589225755051734,
-                    "angularVelocity": -1.2639578450408617,
-                    "velocityX": -3.214909303244982,
-                    "velocityY": -0.28898706012659137,
-                    "timestamp": 2.0961375427301503
-                },
-                {
-                    "x": 5.475720463425886,
-                    "y": 1.6466626572874457,
-                    "heading": 2.662658583499747,
-                    "angularVelocity": -1.2341573315618837,
-                    "velocityX": -2.5673254698895036,
-                    "velocityY": -0.22940886147715536,
-                    "timestamp": 2.174137315971054
-                },
-                {
-                    "x": 5.325756154484835,
-                    "y": 1.6333048659006117,
-                    "heading": 2.583901126299631,
-                    "angularVelocity": -1.009713925152988,
-                    "velocityX": -1.9226249347915934,
-                    "velocityY": -0.17125423359344924,
-                    "timestamp": 2.252137089211958
-                },
-                {
-                    "x": 5.225890415283439,
-                    "y": 1.6244276229064765,
-                    "heading": 2.5286254494455984,
-                    "angularVelocity": -0.7086645839765799,
-                    "velocityX": -1.2803337119065468,
-                    "velocityY": -0.11381113848520645,
-                    "timestamp": 2.330136862452862
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": -0.3669940085234364,
-                    "velocityX": -0.6396225682522365,
-                    "velocityY": -0.05676456126098677,
-                    "timestamp": 2.408136635693766
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": -3.495963808080905e-32,
-                    "velocityX": 2.431919697102105e-35,
-                    "velocityY": 0,
-                    "timestamp": 2.48613640893467
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -22716,389 +5185,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S3 To C5 To S3": {
+        "C4 To S4": {
             "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 19
-                },
                 {
                     "x": 7.68,
-                    "y": 0.787,
+                    "y": 2.457,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 19
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -4.85982063591045e-33,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.306024668622343,
-                    "y": 2.9721182741266654,
-                    "heading": 2.598597610192154,
-                    "angularVelocity": 0.31231028150986523,
-                    "velocityX": 0.5462744658665957,
-                    "velocityY": -0.3540079539621319,
-                    "timestamp": 0.0787601678472939
-                },
-                {
-                    "x": 4.392095194160992,
-                    "y": 2.9163526825964556,
-                    "heading": 2.647262656420547,
-                    "angularVelocity": 0.6178890619271524,
-                    "velocityX": 1.0928179546992538,
-                    "velocityY": -0.7080430762709009,
-                    "timestamp": 0.1575203356945878
-                },
-                {
-                    "x": 4.521245672207201,
-                    "y": 2.83269821600786,
-                    "heading": 2.7190885011917048,
-                    "angularVelocity": 0.911956471581102,
-                    "velocityX": 1.6397943475262466,
-                    "velocityY": -1.0621418018152553,
-                    "timestamp": 0.23628050354188168
-                },
-                {
-                    "x": 4.693538421890133,
-                    "y": 2.721142508781873,
-                    "heading": 2.812276483563172,
-                    "angularVelocity": 1.1831866909189597,
-                    "velocityX": 2.1875619922114358,
-                    "velocityY": -1.4163975303135559,
-                    "timestamp": 0.3150406713891756
-                },
-                {
-                    "x": 4.909132383686271,
-                    "y": 2.581660366752094,
-                    "heading": 2.9215937215973726,
-                    "angularVelocity": 1.387976194339083,
-                    "velocityX": 2.7373476681023576,
-                    "velocityY": -1.7709731434323392,
-                    "timestamp": 0.39380083923646947
-                },
-                {
-                    "x": 5.159074984583097,
-                    "y": 2.4194878202605934,
-                    "heading": 2.9215937398089027,
-                    "angularVelocity": 2.3122767087752481e-7,
-                    "velocityX": 3.1734645535727406,
-                    "velocityY": -2.059068066055073,
-                    "timestamp": 0.47256100708376336
-                },
-                {
-                    "x": 5.409017280295355,
-                    "y": 2.257314803413765,
-                    "heading": 2.921593758018432,
-                    "angularVelocity": 2.3120226821213244e-7,
-                    "velocityX": 3.173460678713437,
-                    "velocityY": -2.059074038050069,
-                    "timestamp": 0.5513211749310573
-                },
-                {
-                    "x": 5.658959576005671,
-                    "y": 2.0951417865639432,
-                    "heading": 2.9215937762279616,
-                    "angularVelocity": 2.312022681051112e-7,
-                    "velocityX": 3.1734606786887793,
-                    "velocityY": -2.0590740380880725,
-                    "timestamp": 0.6300813427783512
-                },
-                {
-                    "x": 5.908901871715987,
-                    "y": 1.9329687697141213,
-                    "heading": 2.9215937944374906,
-                    "angularVelocity": 2.3120226848439114e-7,
-                    "velocityX": 3.173460678688779,
-                    "velocityY": -2.0590740380880725,
-                    "timestamp": 0.708841510625645
-                },
-                {
-                    "x": 6.158844167426302,
-                    "y": 1.7707957528642995,
-                    "heading": 2.92159381264702,
-                    "angularVelocity": 2.3120226852070691e-7,
-                    "velocityX": 3.1734606786887785,
-                    "velocityY": -2.059074038088073,
-                    "timestamp": 0.7876016784729389
-                },
-                {
-                    "x": 6.408786463136619,
-                    "y": 1.6086227360144778,
-                    "heading": 2.921593830856549,
-                    "angularVelocity": 2.3120226809334475e-7,
-                    "velocityX": 3.173460678688779,
-                    "velocityY": -2.059074038088073,
-                    "timestamp": 0.8663618463202328
-                },
-                {
-                    "x": 6.658728758846954,
-                    "y": 1.4464497191646866,
-                    "heading": 2.9215938490660784,
-                    "angularVelocity": 2.31202268371391e-7,
-                    "velocityX": 3.173460678689032,
-                    "velocityY": -2.0590740380876835,
-                    "timestamp": 0.9451220141675267
-                },
-                {
-                    "x": 6.908671057688978,
-                    "y": 1.2842767071415118,
-                    "heading": 2.9215938672756456,
-                    "angularVelocity": 2.3120274459298332e-7,
-                    "velocityX": 3.173460718451369,
-                    "velocityY": -2.05907397680523,
-                    "timestamp": 1.0238821820148205
-                },
-                {
-                    "x": 7.1455069915896265,
-                    "y": 1.1314725250685531,
-                    "heading": 2.963985587877118,
-                    "angularVelocity": 0.5382380683048028,
-                    "velocityX": 3.007052173375748,
-                    "velocityY": -1.9401200663922304,
-                    "timestamp": 1.1026423498621143
-                },
-                {
-                    "x": 7.339036455456064,
-                    "y": 1.0065280886493264,
-                    "heading": 3.0237371798128563,
-                    "angularVelocity": 0.7586524199845985,
-                    "velocityX": 2.457199738853539,
-                    "velocityY": -1.5863911902965262,
-                    "timestamp": 1.181402517709408
-                },
-                {
-                    "x": 7.489157100180206,
-                    "y": 0.9096422022009641,
-                    "heading": 3.07668127081849,
-                    "angularVelocity": 0.6722191236093652,
-                    "velocityX": 1.9060478009036246,
-                    "velocityY": -1.2301381408456074,
-                    "timestamp": 1.2601626855567019
-                },
-                {
-                    "x": 7.5959791241645505,
-                    "y": 0.8407792473093463,
-                    "heading": 3.115718535259042,
-                    "angularVelocity": 0.4956472987238782,
-                    "velocityX": 1.3562950270935392,
-                    "velocityY": -0.8743373303257803,
-                    "timestamp": 1.3389228534039956
-                },
-                {
-                    "x": 7.659576823404001,
-                    "y": 0.7999065653267177,
-                    "heading": 3.1374811645909735,
-                    "angularVelocity": 0.2763151720819586,
-                    "velocityX": 0.8074855726914214,
-                    "velocityY": -0.5189511792543529,
-                    "timestamp": 1.4176830212512894
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 3.14,
-                    "angularVelocity": 0.03198108228905446,
-                    "velocityX": 0.2593084442835197,
-                    "velocityY": -0.16387173465319213,
-                    "timestamp": 1.4964431890985832
-                },
-                {
-                    "x": 7.653968415649204,
-                    "y": 0.8042046719225767,
-                    "heading": 3.120030327667123,
-                    "angularVelocity": -0.24175586185815795,
-                    "velocityX": -0.3151422820242312,
-                    "velocityY": 0.20828235032084352,
-                    "timestamp": 1.5790458287176843
-                },
-                {
-                    "x": 7.58042792526076,
-                    "y": 0.8521912576149779,
-                    "heading": 3.0798198381250614,
-                    "angularVelocity": -0.4867942444391582,
-                    "velocityX": -0.8902922561258279,
-                    "velocityY": 0.5809328359683593,
-                    "timestamp": 1.6616484683367854
-                },
-                {
-                    "x": 7.459301398894465,
-                    "y": 0.9310142655562479,
-                    "heading": 3.0230678620569518,
-                    "angularVelocity": -0.6870479724353368,
-                    "velocityX": -1.466375976903756,
-                    "velocityY": 0.9542432094753368,
-                    "timestamp": 1.7442511079558864
-                },
-                {
-                    "x": 7.290490307507352,
-                    "y": 1.0407418073187003,
-                    "heading": 2.956300453743236,
-                    "angularVelocity": -0.808296304108442,
-                    "velocityX": -2.0436525051200767,
-                    "velocityY": 1.3283781519383162,
-                    "timestamp": 1.8268537475749875
-                },
-                {
-                    "x": 7.073948364025268,
-                    "y": 1.1814068761754821,
-                    "heading": 2.8937785154835103,
-                    "angularVelocity": -0.756900004019734,
-                    "velocityX": -2.621489391629628,
-                    "velocityY": 1.7029125159367249,
-                    "timestamp": 1.9094563871940886
-                },
-                {
-                    "x": 6.811621234855711,
-                    "y": 1.3511947989186324,
-                    "heading": 2.893774866555063,
-                    "angularVelocity": -0.000044174477718030994,
-                    "velocityX": -3.1757717474792755,
-                    "velocityY": 2.0554781726840226,
-                    "timestamp": 1.9920590268131897
-                },
-                {
-                    "x": 6.5493872326198215,
-                    "y": 1.5211289667970835,
-                    "heading": 2.893774844652908,
-                    "angularVelocity": -2.6515078170743926e-7,
-                    "velocityX": -3.174644338789979,
-                    "velocityY": 2.057248638325058,
-                    "timestamp": 2.074661666432291
-                },
-                {
-                    "x": 6.287153230916077,
-                    "y": 1.691063135496712,
-                    "heading": 2.893774822750754,
-                    "angularVelocity": -2.651507828628349e-7,
-                    "velocityX": -3.1746443323477713,
-                    "velocityY": 2.057248648266354,
-                    "timestamp": 2.1572643060513923
-                },
-                {
-                    "x": 6.024919229212335,
-                    "y": 1.860997304196345,
-                    "heading": 2.893774800848599,
-                    "angularVelocity": -2.651507825022106e-7,
-                    "velocityX": -3.1746443323477354,
-                    "velocityY": 2.0572486482664103,
-                    "timestamp": 2.2398669456704936
-                },
-                {
-                    "x": 5.762685227508594,
-                    "y": 2.030931472895978,
-                    "heading": 2.8937747789464447,
-                    "angularVelocity": -2.651507826588173e-7,
-                    "velocityX": -3.174644332347735,
-                    "velocityY": 2.0572486482664103,
-                    "timestamp": 2.322469585289595
-                },
-                {
-                    "x": 5.500451225804852,
-                    "y": 2.2008656415956116,
-                    "heading": 2.89377475704429,
-                    "angularVelocity": -2.651507831859411e-7,
-                    "velocityX": -3.1746443323477345,
-                    "velocityY": 2.05724864826641,
-                    "timestamp": 2.405072224908696
-                },
-                {
-                    "x": 5.238217224101064,
-                    "y": 2.3707998102951717,
-                    "heading": 2.8937747351421352,
-                    "angularVelocity": -2.6515078363410014e-7,
-                    "velocityX": -3.1746443323483056,
-                    "velocityY": 2.057248648265529,
-                    "timestamp": 2.4876748645277975
-                },
-                {
-                    "x": 4.975983214041022,
-                    "y": 2.5407339660996877,
-                    "heading": 2.89377471323987,
-                    "angularVelocity": -2.651521251593368e-7,
-                    "velocityX": -3.1746444335103496,
-                    "velocityY": 2.0572484921561784,
-                    "timestamp": 2.570277504146899
-                },
-                {
-                    "x": 4.737824648225833,
-                    "y": 2.6937649564980313,
-                    "heading": 2.811064183589652,
-                    "angularVelocity": -1.0013061329711461,
-                    "velocityX": -2.883183478317317,
-                    "velocityY": 1.8526162251473437,
-                    "timestamp": 2.652880143766
-                },
-                {
-                    "x": 4.54767021844827,
-                    "y": 2.816291569523401,
-                    "heading": 2.7234816518817766,
-                    "angularVelocity": -1.0602873238885226,
-                    "velocityX": -2.302038175225493,
-                    "velocityY": 1.48332563693325,
-                    "timestamp": 2.7354827833851014
-                },
-                {
-                    "x": 4.405252215667174,
-                    "y": 2.908161976795727,
-                    "heading": 2.651165155434109,
-                    "angularVelocity": -0.875474401073941,
-                    "velocityX": -1.724133798118536,
-                    "velocityY": 1.1121969914758385,
-                    "timestamp": 2.8180854230042027
-                },
-                {
-                    "x": 4.3103959059570425,
-                    "y": 2.9693921451036718,
-                    "heading": 2.600323851470343,
-                    "angularVelocity": -0.6154924854483996,
-                    "velocityX": -1.1483447762388197,
-                    "velocityY": 0.7412616423674994,
-                    "timestamp": 2.900688062623304
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": -0.3186805118059785,
-                    "velocityX": -0.5737819781013825,
-                    "velocityY": 0.3705433027015611,
-                    "timestamp": 2.9832907022424053
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": 0,
-                    "velocityX": 5.015233372101362e-32,
-                    "velocityY": -2.828100691709421e-32,
-                    "timestamp": 3.0658933418615066
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -23118,353 +5226,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S3 To C5 To S4": {
+        "C4 To C1": {
             "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 19
-                },
                 {
                     "x": 7.68,
-                    "y": 0.787,
+                    "y": 2.457,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 15
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.307906047793953,
-                    "y": 2.9763559763722562,
-                    "heading": 2.599832328454837,
-                    "angularVelocity": 0.3294814998044202,
-                    "velocityX": 0.5727595173353921,
-                    "velocityY": -0.3015705061160393,
-                    "timestamp": 0.07840297094122421
-                },
-                {
-                    "x": 4.397737480320486,
-                    "y": 2.929052986687658,
-                    "heading": 2.6508685642334093,
-                    "angularVelocity": 0.6509477276930885,
-                    "velocityX": 1.1457656699499514,
-                    "velocityY": -0.6033315972179728,
-                    "timestamp": 0.15680594188244842
-                },
-                {
-                    "x": 4.532524605763972,
-                    "y": 2.858064592924414,
-                    "heading": 2.7260427369404674,
-                    "angularVelocity": 0.95881790963547,
-                    "velocityX": 1.7191583918999656,
-                    "velocityY": -0.9054298952072739,
-                    "timestamp": 0.23520891282367262
-                },
-                {
-                    "x": 4.712319110504002,
-                    "y": 2.7633347501885597,
-                    "heading": 2.823264198017844,
-                    "angularVelocity": 1.2400226663643632,
-                    "velocityX": 2.293210353914077,
-                    "velocityY": -1.20824302444969,
-                    "timestamp": 0.31361188376489685
-                },
-                {
-                    "x": 4.937226049144337,
-                    "y": 2.644712434074336,
-                    "heading": 2.936900931840023,
-                    "angularVelocity": 1.4493932112280175,
-                    "velocityX": 2.8686022473427424,
-                    "velocityY": -1.5129824124033164,
-                    "timestamp": 0.3920148547061211
-                },
-                {
-                    "x": 5.200333937862815,
-                    "y": 2.5078106422289963,
-                    "heading": 2.936900964883958,
-                    "angularVelocity": 4.214627997065615e-7,
-                    "velocityX": 3.355840799907034,
-                    "velocityY": -1.7461301555518065,
-                    "timestamp": 0.4704178256473453
-                },
-                {
-                    "x": 5.462885368981394,
-                    "y": 2.3698446646000946,
-                    "heading": 2.936900984463062,
-                    "angularVelocity": 2.4972400074747634e-7,
-                    "velocityX": 3.3487433953925927,
-                    "velocityY": -1.7597034394566977,
-                    "timestamp": 0.5488207965885695
-                },
-                {
-                    "x": 5.725436796470205,
-                    "y": 2.2318786800636885,
-                    "heading": 2.9369010040421655,
-                    "angularVelocity": 2.4972400141004584e-7,
-                    "velocityX": 3.3487433490962957,
-                    "velocityY": -1.759703527559279,
-                    "timestamp": 0.6272237675297937
-                },
-                {
-                    "x": 5.987988223958993,
-                    "y": 2.0939126955272376,
-                    "heading": 2.936901023621269,
-                    "angularVelocity": 2.497240000885312e-7,
-                    "velocityX": 3.3487433490959946,
-                    "velocityY": -1.7597035275598514,
-                    "timestamp": 0.7056267384710179
-                },
-                {
-                    "x": 6.250539651447782,
-                    "y": 1.9559467109907867,
-                    "heading": 2.9369010432003724,
-                    "angularVelocity": 2.497240001698141e-7,
-                    "velocityX": 3.348743349095995,
-                    "velocityY": -1.759703527559851,
-                    "timestamp": 0.784029709412242
-                },
-                {
-                    "x": 6.513091078936429,
-                    "y": 1.8179807264540686,
-                    "heading": 2.936901062779476,
-                    "angularVelocity": 2.497239999907855e-7,
-                    "velocityX": 3.3487433490942036,
-                    "velocityY": -1.7597035275632584,
-                    "timestamp": 0.8624326803534662
-                },
-                {
-                    "x": 6.775642484801246,
-                    "y": 1.6800147007668695,
-                    "heading": 2.9369010823585797,
-                    "angularVelocity": 2.4972399921138707e-7,
-                    "velocityX": 3.348743073290482,
-                    "velocityY": -1.759704052421956,
-                    "timestamp": 0.9408356512946904
-                },
-                {
-                    "x": 7.03487156699359,
-                    "y": 1.5359031200183482,
-                    "heading": 2.936901102721687,
-                    "angularVelocity": 2.5972367283965845e-7,
-                    "velocityX": 3.3063681016204165,
-                    "velocityY": -1.8380882639839122,
-                    "timestamp": 1.0192386222359147
-                },
-                {
-                    "x": 7.257601032453945,
-                    "y": 1.356628823714497,
-                    "heading": 2.964092637940216,
-                    "angularVelocity": 0.34681766382323054,
-                    "velocityX": 2.8408294071826354,
-                    "velocityY": -2.286575293661087,
-                    "timestamp": 1.0976415931771388
-                },
-                {
-                    "x": 7.433803163736967,
-                    "y": 1.1974021851496213,
-                    "heading": 3.0191989559887835,
-                    "angularVelocity": 0.7028600751601236,
-                    "velocityX": 2.2473910002092454,
-                    "velocityY": -2.030875063193899,
-                    "timestamp": 1.176044564118363
-                },
-                {
-                    "x": 7.563846036619734,
-                    "y": 1.0604378478062424,
-                    "heading": 3.0711979216149987,
-                    "angularVelocity": 0.6632269798184063,
-                    "velocityX": 1.6586472594285269,
-                    "velocityY": -1.74692789953109,
-                    "timestamp": 1.2544475350595872
-                },
-                {
-                    "x": 7.648096830602628,
-                    "y": 0.946251036791659,
-                    "heading": 3.1111882105248365,
-                    "angularVelocity": 0.5100608871025,
-                    "velocityX": 1.0745867531729072,
-                    "velocityY": -1.4564092360754384,
-                    "timestamp": 1.3328505060008113
-                },
-                {
-                    "x": 7.6867694062848315,
-                    "y": 0.855064254512633,
-                    "heading": 3.1349445229229786,
-                    "angularVelocity": 0.3030027065666845,
-                    "velocityX": 0.49325395731671434,
-                    "velocityY": -1.1630526392597296,
-                    "timestamp": 1.4112534769420355
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 3.14,
-                    "angularVelocity": 0.06448068250805035,
-                    "velocityX": -0.08634119604942615,
-                    "velocityY": -0.8681336140135004,
-                    "timestamp": 1.4896564478832597
-                },
-                {
-                    "x": 7.627073807612577,
-                    "y": 0.7419574324370846,
-                    "heading": 3.1244199952297187,
-                    "angularVelocity": -0.1970875494622002,
-                    "velocityX": -0.6695179952660093,
-                    "velocityY": -0.5697898937387053,
-                    "timestamp": 1.568707635097908
-                },
-                {
-                    "x": 7.5280560417619835,
-                    "y": 0.7206319042387348,
-                    "heading": 3.0899060052252425,
-                    "angularVelocity": -0.4366030570890143,
-                    "velocityX": -1.2525778465750235,
-                    "velocityY": -0.26976860120365537,
-                    "timestamp": 1.6477588223125563
-                },
-                {
-                    "x": 7.382968853200293,
-                    "y": 0.7232223421332327,
-                    "heading": 3.03915644307554,
-                    "angularVelocity": -0.6419835544263846,
-                    "velocityX": -1.8353574901749392,
-                    "velocityY": 0.0327691206892746,
-                    "timestamp": 1.7268100095272048
-                },
-                {
-                    "x": 7.191873936786201,
-                    "y": 0.7500593523651382,
-                    "heading": 2.9768884847472514,
-                    "angularVelocity": -0.7876916276946772,
-                    "velocityX": -2.417356691875479,
-                    "velocityY": 0.3394890219549461,
-                    "timestamp": 1.8058611967418532
-                },
-                {
-                    "x": 6.955009615423507,
-                    "y": 0.8017966281088876,
-                    "heading": 2.913255262792046,
-                    "angularVelocity": -0.8049622554368411,
-                    "velocityX": -2.9963411013610584,
-                    "velocityY": 0.6544781623992936,
-                    "timestamp": 1.8849123839565016
-                },
-                {
-                    "x": 6.67449530614552,
-                    "y": 0.8800439513487449,
-                    "heading": 2.8869556643049132,
-                    "angularVelocity": -0.33269074651445785,
-                    "velocityX": -3.5485148188383207,
-                    "velocityY": 0.9898310954821422,
-                    "timestamp": 1.96396357117115
-                },
-                {
-                    "x": 6.4045779974291,
-                    "y": 1.0087805726115764,
-                    "heading": 2.886955644734896,
-                    "angularVelocity": -2.475613351225265e-7,
-                    "velocityX": -3.4144624290526653,
-                    "velocityY": 1.6285223005349794,
-                    "timestamp": 2.0430147583857985
-                },
-                {
-                    "x": 6.136902414581452,
-                    "y": 1.1421156905122876,
-                    "heading": 2.8869556256683206,
-                    "angularVelocity": -2.4119277641286314e-7,
-                    "velocityX": -3.386104526435805,
-                    "velocityY": 1.6866934273696874,
-                    "timestamp": 2.122065945600447
-                },
-                {
-                    "x": 5.869226659738861,
-                    "y": 1.2754504631222496,
-                    "heading": 2.8869556065980015,
-                    "angularVelocity": -2.4124013985780433e-7,
-                    "velocityX": -3.3861067021774174,
-                    "velocityY": 1.6866890594304795,
-                    "timestamp": 2.2011171328150954
-                },
-                {
-                    "x": 5.637978571315548,
-                    "y": 1.3902481295023497,
-                    "heading": 2.7615946389842474,
-                    "angularVelocity": -1.5858201758975898,
-                    "velocityX": -2.9252955783626895,
-                    "velocityY": 1.4521940836692195,
-                    "timestamp": 2.280168320029744
-                },
-                {
-                    "x": 5.453130511537557,
-                    "y": 1.4821316832792537,
-                    "heading": 2.658075983788866,
-                    "angularVelocity": -1.309514238087925,
-                    "velocityX": -2.3383337593158178,
-                    "velocityY": 1.162329839872467,
-                    "timestamp": 2.3592195072443922
-                },
-                {
-                    "x": 5.3145458591491295,
-                    "y": 1.5510603964907557,
-                    "heading": 2.5794202908528,
-                    "angularVelocity": -0.9949969849595353,
-                    "velocityX": -1.753100203443375,
-                    "velocityY": 0.8719503860752608,
-                    "timestamp": 2.4382706944590407
-                },
-                {
-                    "x": 5.222177020265271,
-                    "y": 1.597019066974222,
-                    "heading": 2.5265746527994444,
-                    "angularVelocity": -0.668498980411977,
-                    "velocityX": -1.1684687117102668,
-                    "velocityY": 0.5813786244434977,
-                    "timestamp": 2.517321881673689
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": -0.336170192198179,
-                    "velocityX": -0.5841407560381138,
-                    "velocityY": 0.29070952424007707,
-                    "timestamp": 2.5963730688883375
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": 0,
-                    "velocityX": 3.1840213941352295e-31,
-                    "velocityY": 0,
-                    "timestamp": 2.675424256102986
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -23484,191 +5267,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S4 To W3": {
+        "C4 To C2": {
             "waypoints": [
                 {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 17
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": 0,
-                    "velocityX": -2.991539680273006e-28,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.128677687485332,
-                    "y": 1.6650804220486217,
-                    "heading": 2.5120089352888275,
-                    "angularVelocity": 0.13555115758601896,
-                    "velocityX": -0.5341517867092611,
-                    "velocityY": 0.5088463919723994,
-                    "timestamp": 0.0885933805561246
-                },
-                {
-                    "x": 5.034016646343104,
-                    "y": 1.7552508281834698,
-                    "heading": 2.534662979379905,
-                    "angularVelocity": 0.2557080895761949,
-                    "velocityX": -1.0684888707035958,
-                    "velocityY": 1.017800715683549,
-                    "timestamp": 0.1771867611122492
-                },
-                {
-                    "x": 4.891992835259765,
-                    "y": 1.890523822602506,
-                    "heading": 2.5656675717312063,
-                    "angularVelocity": 0.3499651120288192,
-                    "velocityX": -1.6030973216262263,
-                    "velocityY": 1.5268973095946068,
-                    "timestamp": 0.2657801416683738
-                },
-                {
-                    "x": 4.702571182201446,
-                    "y": 2.070911500413953,
-                    "heading": 2.6003407126574074,
-                    "angularVelocity": 0.3913739458702845,
-                    "velocityX": -2.1381016490087936,
-                    "velocityY": 2.036130427342408,
-                    "timestamp": 0.3543735222244984
-                },
-                {
-                    "x": 4.465767136473608,
-                    "y": 2.2963300542080516,
-                    "heading": 2.6236809711316846,
-                    "angularVelocity": 0.2634537515983315,
-                    "velocityX": -2.672931591969363,
-                    "velocityY": 2.5444175668553926,
-                    "timestamp": 0.442966902780623
-                },
-                {
-                    "x": 4.223169514205327,
-                    "y": 2.5275605696083283,
-                    "heading": 2.6236809810890134,
-                    "angularVelocity": 1.1239359640247893e-7,
-                    "velocityX": -2.7383267321489955,
-                    "velocityY": 2.610020228924234,
-                    "timestamp": 0.5315602833367477
-                },
-                {
-                    "x": 3.980571892636886,
-                    "y": 2.7587910857428417,
-                    "heading": 2.623680991046373,
-                    "angularVelocity": 1.1239395834581428e-7,
-                    "velocityX": -2.7383267242494984,
-                    "velocityY": 2.6100202372119665,
-                    "timestamp": 0.6201536638928723
-                },
-                {
-                    "x": 3.737974271068317,
-                    "y": 2.9900216018772205,
-                    "heading": 2.623681001003734,
-                    "angularVelocity": 1.1239395847018201e-7,
-                    "velocityX": -2.7383267242509515,
-                    "velocityY": 2.610020237210452,
-                    "timestamp": 0.7087470444489969
-                },
-                {
-                    "x": 3.4953766494997485,
-                    "y": 3.2212521180116,
-                    "heading": 2.623681010961094,
-                    "angularVelocity": 1.1239395711385692e-7,
-                    "velocityX": -2.73832672425095,
-                    "velocityY": 2.610020237210453,
-                    "timestamp": 0.7973404250051216
-                },
-                {
-                    "x": 3.252779027931301,
-                    "y": 3.4524826341461052,
-                    "heading": 2.6236810209184545,
-                    "angularVelocity": 1.1239395822212682e-7,
-                    "velocityX": -2.7383267242495783,
-                    "velocityY": 2.6100202372118835,
-                    "timestamp": 0.8859338055612462
-                },
-                {
-                    "x": 3.0101814056711773,
-                    "y": 3.68371314955494,
-                    "heading": 2.6236810308757845,
-                    "angularVelocity": 1.1239361401031205e-7,
-                    "velocityX": -2.7383267320569105,
-                    "velocityY": 2.6100202290208365,
-                    "timestamp": 0.9745271861173709
-                },
-                {
-                    "x": 2.773398473292677,
-                    "y": 3.9091148718156945,
-                    "heading": 2.6467919550399244,
-                    "angularVelocity": 0.2608651348337584,
-                    "velocityX": -2.6726932745104093,
-                    "velocityY": 2.5442275805015666,
-                    "timestamp": 1.0631205666734955
-                },
-                {
-                    "x": 2.58398624574228,
-                    "y": 4.089495303792956,
-                    "heading": 2.682294638537745,
-                    "angularVelocity": 0.40073742840518267,
-                    "velocityX": -2.1379952583522854,
-                    "velocityY": 2.036048639807706,
-                    "timestamp": 1.15171394722962
-                },
-                {
-                    "x": 2.441971645222967,
-                    "y": 4.224760268869052,
-                    "heading": 2.7142172786134493,
-                    "angularVelocity": 0.36032759869046743,
-                    "velocityX": -1.6029933571543074,
-                    "velocityY": 1.5268066781852525,
-                    "timestamp": 1.2403073277857446
-                },
-                {
-                    "x": 2.3473180655703776,
-                    "y": 4.314923716116089,
-                    "heading": 2.737593550901004,
-                    "angularVelocity": 0.263860258416364,
-                    "velocityX": -1.0684046489525902,
-                    "velocityY": 1.0177221670632457,
-                    "timestamp": 1.3289007083418691
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": 0.1400381046653499,
-                    "velocityX": -0.5341038492193103,
-                    "velocityY": 0.5087996823346718,
-                    "timestamp": 1.4174940888979937
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": 0,
-                    "velocityX": -3.3897612225154066e-29,
-                    "velocityY": 0,
-                    "timestamp": 1.5060874694541182
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -23688,16 +5308,16 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S4 To C3 To S1": {
+        "C4 To C3": {
             "waypoints": [
                 {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 18
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 7.68,
@@ -23706,335 +5326,10 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 16
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.205167733325116,
-                    "y": 1.6385570868106298,
-                    "heading": 2.543052954613419,
-                    "angularVelocity": 0.6503304289456182,
-                    "velocityX": 0.44058914643605,
-                    "velocityY": 0.2803114985693042,
-                    "timestamp": 0.0662016610283172
-                },
-                {
-                    "x": 5.263930522170129,
-                    "y": 1.6757412702349752,
-                    "heading": 2.6242045140781594,
-                    "angularVelocity": 1.225823615363673,
-                    "velocityX": 0.8876331489609754,
-                    "velocityY": 0.5616805204997857,
-                    "timestamp": 0.1324033220566344
-                },
-                {
-                    "x": 5.3527880828329195,
-                    "y": 1.7316217896984443,
-                    "heading": 2.7372039865474704,
-                    "angularVelocity": 1.706897843863404,
-                    "velocityX": 1.3422255466488056,
-                    "velocityY": 0.8440954289583611,
-                    "timestamp": 0.19860498308495161
-                },
-                {
-                    "x": 5.472262183244267,
-                    "y": 1.806361940098468,
-                    "heading": 2.873328504655769,
-                    "angularVelocity": 2.056210010353016,
-                    "velocityX": 1.804699437378857,
-                    "velocityY": 1.1289769658201285,
-                    "timestamp": 0.2648066441132688
-                },
-                {
-                    "x": 5.622851983789364,
-                    "y": 1.9002058058145468,
-                    "heading": 3.0148229359693395,
-                    "angularVelocity": 2.137324488779513,
-                    "velocityX": 2.2747133260099135,
-                    "velocityY": 1.4175454853906315,
-                    "timestamp": 0.331008305141586
-                },
-                {
-                    "x": 5.803032153079139,
-                    "y": 2.011379887463594,
-                    "heading": 3.1112408203645807,
-                    "angularVelocity": 1.456426967203298,
-                    "velocityX": 2.721686533102414,
-                    "velocityY": 1.6793246562416593,
-                    "timestamp": 0.39720996616990323
-                },
-                {
-                    "x": 6.010803138038067,
-                    "y": 2.135461093514753,
-                    "heading": 3.1324618293668918,
-                    "angularVelocity": 0.32055100540867,
-                    "velocityX": 3.138455768807129,
-                    "velocityY": 1.8742914320237858,
-                    "timestamp": 0.46341162719822043
-                },
-                {
-                    "x": 6.223708618139815,
-                    "y": 2.267331907058253,
-                    "heading": 3.132461891539073,
-                    "angularVelocity": 9.391332530924462e-7,
-                    "velocityX": 3.2160141723739923,
-                    "velocityY": 1.9919562665816168,
-                    "timestamp": 0.5296132882265376
-                },
-                {
-                    "x": 6.436614072470765,
-                    "y": 2.3992027622087693,
-                    "heading": 3.1324619537111715,
-                    "angularVelocity": 9.391320033179244e-7,
-                    "velocityX": 3.2160137830967814,
-                    "velocityY": 1.991956895070635,
-                    "timestamp": 0.5958149492548548
-                },
-                {
-                    "x": 6.649519526799896,
-                    "y": 2.5310736173622237,
-                    "heading": 3.1324620158832706,
-                    "angularVelocity": 9.391320117961031e-7,
-                    "velocityX": 3.2160137830692905,
-                    "velocityY": 1.991956895115017,
-                    "timestamp": 0.6620166102831719
-                },
-                {
-                    "x": 6.862424842072437,
-                    "y": 2.6629446970222306,
-                    "heading": 3.132462078055361,
-                    "angularVelocity": 9.391318749443678e-7,
-                    "velocityX": 3.216011682569308,
-                    "velocityY": 1.991960286367572,
-                    "timestamp": 0.7282182713114891
-                },
-                {
-                    "x": 7.065700834244428,
-                    "y": 2.8092236283210066,
-                    "heading": 3.1324621404036685,
-                    "angularVelocity": 9.417936977148102e-7,
-                    "velocityX": 3.070557279295834,
-                    "velocityY": 2.209596089080417,
-                    "timestamp": 0.7944199323398062
-                },
-                {
-                    "x": 7.245525316742551,
-                    "y": 2.9835274116090518,
-                    "heading": 3.132462207800352,
-                    "angularVelocity": 0.000001018051251212501,
-                    "velocityX": 2.7163137556501122,
-                    "velocityY": 2.632921600162078,
-                    "timestamp": 0.8606215933681234
-                },
-                {
-                    "x": 7.3980692436855175,
-                    "y": 3.182145135413928,
-                    "heading": 3.132462290722476,
-                    "angularVelocity": 0.0000012525686368484786,
-                    "velocityX": 2.304231111024479,
-                    "velocityY": 3.0001924531762993,
-                    "timestamp": 0.9268232543964405
-                },
-                {
-                    "x": 7.520084601366705,
-                    "y": 3.4008478887342375,
-                    "heading": 3.132462412903275,
-                    "angularVelocity": 0.0000018455850946514453,
-                    "velocityX": 1.843086046269626,
-                    "velocityY": 3.3035840781513723,
-                    "timestamp": 0.9930249154247577
-                },
-                {
-                    "x": 7.608973390349284,
-                    "y": 3.6349790028149913,
-                    "heading": 3.1324627892694235,
-                    "angularVelocity": 0.000005685146605966413,
-                    "velocityX": 1.3426972616976043,
-                    "velocityY": 3.536635039725298,
-                    "timestamp": 1.0592265764530748
-                },
-                {
-                    "x": 7.662614259939079,
-                    "y": 3.8787278318222707,
-                    "heading": 3.1349569914257884,
-                    "angularVelocity": 0.03767582440419081,
-                    "velocityX": 0.8102647087184026,
-                    "velocityY": 3.6819140973379696,
-                    "timestamp": 1.125428237481392
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": 0.07617646589996442,
-                    "velocityX": 0.26261788285761967,
-                    "velocityY": 3.7502407692104156,
-                    "timestamp": 1.1916298985097091
-                },
-                {
-                    "x": 7.6496343984109,
-                    "y": 4.420930599340373,
-                    "heading": 3.140398780477199,
-                    "angularVelocity": 0.005103020206742044,
-                    "velocityX": -0.38857538706933203,
-                    "velocityY": 3.7613019480289434,
-                    "timestamp": 1.2697758712078666
-                },
-                {
-                    "x": 7.569234672947985,
-                    "y": 4.705409013581856,
-                    "heading": 3.140398835230542,
-                    "angularVelocity": 7.00654698073033e-7,
-                    "velocityX": -1.0288402931970753,
-                    "velocityY": 3.640346449333809,
-                    "timestamp": 1.347921843906024
-                },
-                {
-                    "x": 7.441209757564435,
-                    "y": 4.9718704928955155,
-                    "heading": 3.1403988601468993,
-                    "angularVelocity": 3.1884377743325344e-7,
-                    "velocityX": -1.6382791200012203,
-                    "velocityY": 3.409791574837347,
-                    "timestamp": 1.4260678166041816
-                },
-                {
-                    "x": 7.269357950164967,
-                    "y": 5.212409598206138,
-                    "heading": 3.1403988766618367,
-                    "angularVelocity": 2.1133446439744448e-7,
-                    "velocityX": -2.199112781707609,
-                    "velocityY": 3.07807423729561,
-                    "timestamp": 1.504213789302339
-                },
-                {
-                    "x": 7.058777847289789,
-                    "y": 5.4198900260822835,
-                    "heading": 3.140398889837091,
-                    "angularVelocity": 1.685979934733241e-7,
-                    "velocityX": -2.6947019226259408,
-                    "velocityY": 2.6550367307797926,
-                    "timestamp": 1.5823597620004966
-                },
-                {
-                    "x": 6.8157174042290976,
-                    "y": 5.588156866874436,
-                    "heading": 3.1403989019071075,
-                    "angularVelocity": 1.5445475097502255e-7,
-                    "velocityX": -3.110338699084864,
-                    "velocityY": 2.1532375243707835,
-                    "timestamp": 1.660505734698654
-                },
-                {
-                    "x": 6.566201090125382,
-                    "y": 5.746693038711312,
-                    "heading": 3.1403989138819113,
-                    "angularVelocity": 1.5323635026193936e-7,
-                    "velocityX": -3.1929516709384456,
-                    "velocityY": 2.0287183889726994,
-                    "timestamp": 1.7386517073968115
-                },
-                {
-                    "x": 6.316684733354014,
-                    "y": 5.905229143394562,
-                    "heading": 3.140398925856715,
-                    "angularVelocity": 1.532363481603047e-7,
-                    "velocityX": -3.1929522169378273,
-                    "velocityY": 2.0287175296369964,
-                    "timestamp": 1.816797680094969
-                },
-                {
-                    "x": 6.067168376581658,
-                    "y": 6.063765248076258,
-                    "heading": 3.1403989378315185,
-                    "angularVelocity": 1.5323634796892713e-7,
-                    "velocityX": -3.1929522169504736,
-                    "velocityY": 2.028717529617091,
-                    "timestamp": 1.8949436527931265
-                },
-                {
-                    "x": 5.817651906877993,
-                    "y": 6.222301175016583,
-                    "heading": 3.1403989498077705,
-                    "angularVelocity": 1.5325488550289546e-7,
-                    "velocityX": -3.1929536620832875,
-                    "velocityY": 2.0287152551381347,
-                    "timestamp": 1.973089625491284
-                },
-                {
-                    "x": 5.6037668941620185,
-                    "y": 6.358199133155801,
-                    "heading": 3.2279940884560396,
-                    "angularVelocity": 1.1209168639620608,
-                    "velocityX": -2.736993415413761,
-                    "velocityY": 1.7390270214456762,
-                    "timestamp": 2.0512355981894417
-                },
-                {
-                    "x": 5.432659669200931,
-                    "y": 6.4669188463641385,
-                    "heading": 3.29806779388705,
-                    "angularVelocity": 0.8967027091917401,
-                    "velocityX": -2.189584684318749,
-                    "velocityY": 1.3912388502514341,
-                    "timestamp": 2.1293815708875994
-                },
-                {
-                    "x": 5.304329710503707,
-                    "y": 6.548459271235515,
-                    "heading": 3.3506253655164633,
-                    "angularVelocity": 0.6725563687371077,
-                    "velocityX": -1.6421826265173798,
-                    "velocityY": 1.0434373270437258,
-                    "timestamp": 2.207527543585757
-                },
-                {
-                    "x": 5.218776570774893,
-                    "y": 6.602819757390003,
-                    "heading": 3.3856653169356727,
-                    "angularVelocity": 0.4483910073584796,
-                    "velocityX": -1.0947862925612357,
-                    "velocityY": 0.6956274812071462,
-                    "timestamp": 2.285673516283915
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": 0.2241956896684635,
-                    "velocityX": -0.5473931579317236,
-                    "velocityY": 0.34781373462440857,
-                    "timestamp": 2.3638194889820725
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": 0,
-                    "velocityX": -1.468222292761223e-31,
-                    "velocityY": 0,
-                    "timestamp": 2.44196546168023
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -24054,29 +5349,20 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S4 To C3 To S2": {
+        "C4 To C5": {
             "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 18
-                },
                 {
                     "x": 7.68,
-                    "y": 4.127,
+                    "y": 2.457,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 15
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 4.263,
-                    "y": 5.56,
+                    "x": 7.68,
+                    "y": 0.787,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
@@ -24084,314 +5370,7 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.210251836832851,
-                    "y": 1.636837042886923,
-                    "heading": 2.532387023792424,
-                    "angularVelocity": 0.47220005236446794,
-                    "velocityX": 0.4993888678908201,
-                    "velocityY": 0.24548265329423538,
-                    "timestamp": 0.06858750571977071
-                },
-                {
-                    "x": 5.278911164181792,
-                    "y": 1.6705273462343695,
-                    "heading": 2.5947497489204117,
-                    "angularVelocity": 0.909243228391831,
-                    "velocityX": 1.0010471532448244,
-                    "velocityY": 0.4912017574322581,
-                    "timestamp": 0.13717501143954142
-                },
-                {
-                    "x": 5.382181986675021,
-                    "y": 1.7210991538184346,
-                    "heading": 2.6836393944616295,
-                    "angularVelocity": 1.2960034718917393,
-                    "velocityX": 1.5056798087273409,
-                    "velocityY": 0.7373326534236055,
-                    "timestamp": 0.20576251715931212
-                },
-                {
-                    "x": 5.520335944035756,
-                    "y": 1.7886220906689136,
-                    "heading": 2.793394080133565,
-                    "angularVelocity": 1.6002139824177626,
-                    "velocityX": 2.0142729482712705,
-                    "velocityY": 0.9844786764277269,
-                    "timestamp": 0.27435002287908283
-                },
-                {
-                    "x": 5.693737584114165,
-                    "y": 1.8732111914756295,
-                    "heading": 2.9110576970550115,
-                    "angularVelocity": 1.7155255273779335,
-                    "velocityX": 2.5281811644657237,
-                    "velocityY": 1.2333018954257229,
-                    "timestamp": 0.3429375285988535
-                },
-                {
-                    "x": 5.900852745156476,
-                    "y": 1.9734126829741925,
-                    "heading": 2.983977883284084,
-                    "angularVelocity": 1.0631701133294391,
-                    "velocityX": 3.0197214327712314,
-                    "velocityY": 1.4609292238729052,
-                    "timestamp": 0.4115250343186242
-                },
-                {
-                    "x": 6.133968719369523,
-                    "y": 2.087332400713016,
-                    "heading": 2.9839779219365523,
-                    "angularVelocity": 5.635497037494653e-7,
-                    "velocityX": 3.3988110774212195,
-                    "velocityY": 1.6609397957153824,
-                    "timestamp": 0.4801125400383949
-                },
-                {
-                    "x": 6.367084623966495,
-                    "y": 2.2012522609094827,
-                    "heading": 2.9839779605877594,
-                    "angularVelocity": 5.635313124243683e-7,
-                    "velocityX": 3.3988100624247526,
-                    "velocityY": 1.660941872735705,
-                    "timestamp": 0.5487000457581656
-                },
-                {
-                    "x": 6.600200528555652,
-                    "y": 2.315172121121942,
-                    "heading": 2.9839779992389666,
-                    "angularVelocity": 5.635313147177628e-7,
-                    "velocityX": 3.3988100623108077,
-                    "velocityY": 1.6609418729688719,
-                    "timestamp": 0.6172875514779362
-                },
-                {
-                    "x": 6.833315692249175,
-                    "y": 2.4290934974276186,
-                    "heading": 2.983978037890185,
-                    "angularVelocity": 5.635314723100833e-7,
-                    "velocityX": 3.398799260115481,
-                    "velocityY": 1.6609639774790368,
-                    "timestamp": 0.6858750571977069
-                },
-                {
-                    "x": 7.049439158690477,
-                    "y": 2.572660228674406,
-                    "heading": 2.9839780777366456,
-                    "angularVelocity": 5.809580085952453e-7,
-                    "velocityX": 3.151061759328619,
-                    "velocityY": 2.093190731171372,
-                    "timestamp": 0.7544625629174776
-                },
-                {
-                    "x": 7.241451301040205,
-                    "y": 2.747165513732945,
-                    "heading": 2.983978123402154,
-                    "angularVelocity": 6.657992258942737e-7,
-                    "velocityX": 2.7995207047510204,
-                    "velocityY": 2.5442722144105754,
-                    "timestamp": 0.8230500686372483
-                },
-                {
-                    "x": 7.4049637163298945,
-                    "y": 2.9486213241330868,
-                    "heading": 2.98397818333491,
-                    "angularVelocity": 8.738144839700177e-7,
-                    "velocityX": 2.383997108128638,
-                    "velocityY": 2.9372085817383917,
-                    "timestamp": 0.891637574357019
-                },
-                {
-                    "x": 7.536239247186927,
-                    "y": 3.1724233234500354,
-                    "heading": 2.983979044678588,
-                    "angularVelocity": 0.000012558317568342283,
-                    "velocityX": 1.9139860748601447,
-                    "velocityY": 3.263014115593316,
-                    "timestamp": 0.9602250800767896
-                },
-                {
-                    "x": 7.630290050662344,
-                    "y": 3.4075679677071538,
-                    "heading": 3.001663168154554,
-                    "angularVelocity": 0.25783301623794963,
-                    "velocityX": 1.371252715613873,
-                    "velocityY": 3.428388914124583,
-                    "timestamp": 1.0288125857965604
-                },
-                {
-                    "x": 7.68591505099028,
-                    "y": 3.648060804187146,
-                    "heading": 3.0401905596367595,
-                    "angularVelocity": 0.5617260910408102,
-                    "velocityX": 0.8110077738532104,
-                    "velocityY": 3.5063651018682354,
-                    "timestamp": 1.097400091516331
-                },
-                {
-                    "x": 7.702477054476245,
-                    "y": 3.8894159117324456,
-                    "heading": 3.0910773123993565,
-                    "angularVelocity": 0.7419245273403817,
-                    "velocityX": 0.24147260222047945,
-                    "velocityY": 3.5189369406639295,
-                    "timestamp": 1.1659875972361018
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": 0.7132886243236253,
-                    "velocityX": -0.3277135425813607,
-                    "velocityY": 3.463955800321066,
-                    "timestamp": 1.2345751029558725
-                },
-                {
-                    "x": 7.5839717767270045,
-                    "y": 4.427707701571831,
-                    "heading": 3.18571906595995,
-                    "angularVelocity": 0.5075163501765089,
-                    "velocityX": -1.0659861999835865,
-                    "velocityY": 3.338083838051171,
-                    "timestamp": 1.3246590301910224
-                },
-                {
-                    "x": 7.4226682910577315,
-                    "y": 4.711490758148744,
-                    "heading": 3.2114421388245056,
-                    "angularVelocity": 0.28554564231429624,
-                    "velocityX": -1.7905911811350714,
-                    "velocityY": 3.1502074264162947,
-                    "timestamp": 1.4147429574261723
-                },
-                {
-                    "x": 7.199929791587311,
-                    "y": 4.9676132378837865,
-                    "heading": 3.2147906679837175,
-                    "angularVelocity": 0.03717121646428904,
-                    "velocityX": -2.472566486682988,
-                    "velocityY": 2.8431540186572435,
-                    "timestamp": 1.5048268846613222
-                },
-                {
-                    "x": 6.929834673893736,
-                    "y": 5.175413812947385,
-                    "heading": 3.214790655255866,
-                    "angularVelocity": -1.4128881851850989e-7,
-                    "velocityX": -2.998260910501095,
-                    "velocityY": 2.306744182246487,
-                    "timestamp": 1.594910811896472
-                },
-                {
-                    "x": 6.624007279414812,
-                    "y": 5.325753649029949,
-                    "heading": 3.2147906451916457,
-                    "angularVelocity": -1.1172048505786767e-7,
-                    "velocityX": -3.394916317098503,
-                    "velocityY": 1.6688863451759368,
-                    "timestamp": 1.684994739131622
-                },
-                {
-                    "x": 6.294504922277233,
-                    "y": 5.412705714996136,
-                    "heading": 3.2147906373147057,
-                    "angularVelocity": -8.744001355884246e-8,
-                    "velocityX": -3.6577263808388842,
-                    "velocityY": 0.9652339616501584,
-                    "timestamp": 1.775078666366772
-                },
-                {
-                    "x": 5.954617378458506,
-                    "y": 5.437382093793352,
-                    "heading": 3.214790629982673,
-                    "angularVelocity": -8.139113095546486e-8,
-                    "velocityX": -3.773009839274702,
-                    "velocityY": 0.273926543331081,
-                    "timestamp": 1.8651625936019218
-                },
-                {
-                    "x": 5.614729638884886,
-                    "y": 5.46205577615271,
-                    "heading": 3.214790622650651,
-                    "angularVelocity": -8.139101337806881e-8,
-                    "velocityX": -3.7730120123026722,
-                    "velocityY": 0.273896610823274,
-                    "timestamp": 1.9552465208370717
-                },
-                {
-                    "x": 5.27484189928346,
-                    "y": 5.486729458129014,
-                    "heading": 3.214790615318626,
-                    "angularVelocity": -8.139103977990715e-8,
-                    "velocityX": -3.7730120126113382,
-                    "velocityY": 0.27389660657108617,
-                    "timestamp": 2.0453304480722214
-                },
-                {
-                    "x": 4.937591000410069,
-                    "y": 5.511126660727889,
-                    "heading": 3.206674021661731,
-                    "angularVelocity": -0.09010035314854972,
-                    "velocityX": -3.7437410781731515,
-                    "velocityY": 0.27082747552945735,
-                    "timestamp": 2.1354143753073713
-                },
-                {
-                    "x": 4.66772599134931,
-                    "y": 5.5306707771302355,
-                    "heading": 3.1851297651856467,
-                    "angularVelocity": -0.23915760710394238,
-                    "velocityX": -2.9957065299375607,
-                    "velocityY": 0.21695453342447346,
-                    "timestamp": 2.2254983025425212
-                },
-                {
-                    "x": 4.465349331515348,
-                    "y": 5.545333951302465,
-                    "heading": 3.164279125670826,
-                    "angularVelocity": -0.23145793211693713,
-                    "velocityX": -2.246534604399402,
-                    "velocityY": 0.16277236819342672,
-                    "timestamp": 2.315582229777671
-                },
-                {
-                    "x": 4.330445855865838,
-                    "y": 5.555111001202187,
-                    "heading": 3.1485187249188784,
-                    "angularVelocity": -0.17495241643725573,
-                    "velocityX": -1.4975310223472684,
-                    "velocityY": 0.10853267835671068,
-                    "timestamp": 2.405666157012821
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": -0.09456431552591144,
-                    "velocityX": -0.7487002169630299,
-                    "velocityY": 0.054271599250453016,
-                    "timestamp": 2.495750084247971
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": -3.2898689544721607e-31,
-                    "velocityX": 6.680323432143241e-33,
-                    "velocityY": 0,
-                    "timestamp": 2.585834011483121
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -24411,362 +5390,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S4 To C3 To S3": {
+        "C5 To W1": {
             "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 18
-                },
                 {
                     "x": 7.68,
-                    "y": 4.127,
+                    "y": 0.787,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 17
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -5.457496407482876e-33,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.213718688496592,
-                    "y": 1.6454507753697194,
-                    "heading": 2.523944374487045,
-                    "angularVelocity": 0.3224093526452332,
-                    "velocityX": 0.5078795417016481,
-                    "velocityY": 0.3426929367359288,
-                    "timestamp": 0.07426699719034811
-                },
-                {
-                    "x": 5.289237393773012,
-                    "y": 1.696398991795747,
-                    "heading": 2.5693854471889703,
-                    "angularVelocity": 0.6118609129363125,
-                    "velocityX": 1.0168541631333754,
-                    "velocityY": 0.6860142237264,
-                    "timestamp": 0.14853399438069623
-                },
-                {
-                    "x": 5.402661733248807,
-                    "y": 1.772907618565081,
-                    "heading": 2.632642220499669,
-                    "angularVelocity": 0.8517480940904348,
-                    "velocityX": 1.5272509158420084,
-                    "velocityY": 1.0301833878275757,
-                    "timestamp": 0.22280099157104433
-                },
-                {
-                    "x": 5.554123710239057,
-                    "y": 1.8750622586328765,
-                    "heading": 2.707444867040942,
-                    "angularVelocity": 1.007212481602717,
-                    "velocityX": 2.039425084092856,
-                    "velocityY": 1.3755051898216737,
-                    "timestamp": 0.29706798876139245
-                },
-                {
-                    "x": 5.743710865909146,
-                    "y": 2.002930120234068,
-                    "heading": 2.7801295579879124,
-                    "angularVelocity": 0.9786943554574798,
-                    "velocityX": 2.552777988103829,
-                    "velocityY": 1.7217319460683576,
-                    "timestamp": 0.3713349859517406
-                },
-                {
-                    "x": 5.969240710205324,
-                    "y": 2.1555656332559185,
-                    "heading": 2.8014617866621823,
-                    "angularVelocity": 0.2872369892591052,
-                    "velocityX": 3.036743813919657,
-                    "velocityY": 2.055226665899016,
-                    "timestamp": 0.4456019831420887
-                },
-                {
-                    "x": 6.202237157849221,
-                    "y": 2.312550420967927,
-                    "heading": 2.8014618141625047,
-                    "angularVelocity": 3.702899476409415e-7,
-                    "velocityX": 3.137281113530435,
-                    "velocityY": 2.1137893499268903,
-                    "timestamp": 0.5198689803324368
-                },
-                {
-                    "x": 6.435233608100515,
-                    "y": 2.4695352048100654,
-                    "heading": 2.8014618416628014,
-                    "angularVelocity": 3.7028959815407443e-7,
-                    "velocityX": 3.137281148638853,
-                    "velocityY": 2.1137892978193555,
-                    "timestamp": 0.5941359775227849
-                },
-                {
-                    "x": 6.6682300583517184,
-                    "y": 2.6265199886523383,
-                    "heading": 2.801461869163098,
-                    "angularVelocity": 3.702895988133056e-7,
-                    "velocityX": 3.137281148637628,
-                    "velocityY": 2.1137892978211736,
-                    "timestamp": 0.668402974713133
-                },
-                {
-                    "x": 6.901226494182671,
-                    "y": 2.7835047938971096,
-                    "heading": 2.801461896663395,
-                    "angularVelocity": 3.702895996177554e-7,
-                    "velocityX": 3.137280954469964,
-                    "velocityY": 2.1137895860043394,
-                    "timestamp": 0.7426699719034812
-                },
-                {
-                    "x": 7.132347068567706,
-                    "y": 2.9432384730568093,
-                    "heading": 2.801461924236189,
-                    "angularVelocity": 3.712657746616385e-7,
-                    "velocityX": 3.1120226093518935,
-                    "velocityY": 2.1508029838650415,
-                    "timestamp": 0.8169369690938293
-                },
-                {
-                    "x": 7.334313444212558,
-                    "y": 3.1385360610663486,
-                    "heading": 2.8014619668447454,
-                    "angularVelocity": 5.737212724741241e-7,
-                    "velocityX": 2.7194633321070794,
-                    "velocityY": 2.6296685660925183,
-                    "timestamp": 0.8912039662841774
-                },
-                {
-                    "x": 7.4948116486140535,
-                    "y": 3.3502854712542645,
-                    "heading": 2.8423857101264045,
-                    "angularVelocity": 0.5510353835468871,
-                    "velocityX": 2.161097263568294,
-                    "velocityY": 2.8511912181557233,
-                    "timestamp": 0.9654709634745255
-                },
-                {
-                    "x": 7.612584040336913,
-                    "y": 3.548974342070663,
-                    "heading": 2.9223254670447334,
-                    "angularVelocity": 1.0763833188709784,
-                    "velocityX": 1.5857971397578559,
-                    "velocityY": 2.675331955419639,
-                    "timestamp": 1.0397379606648736
-                },
-                {
-                    "x": 7.689064457271449,
-                    "y": 3.727414986995987,
-                    "heading": 3.0016093061540334,
-                    "angularVelocity": 1.0675514307666671,
-                    "velocityX": 1.0298035443457485,
-                    "velocityY": 2.402690988945958,
-                    "timestamp": 1.1140049578552216
-                },
-                {
-                    "x": 7.725394752128812,
-                    "y": 3.8837274225386396,
-                    "heading": 3.06785364542737,
-                    "angularVelocity": 0.8919754639271454,
-                    "velocityX": 0.4891849170129639,
-                    "velocityY": 2.1047361742931416,
-                    "timestamp": 1.1882719550455696
-                },
-                {
-                    "x": 7.722234181622283,
-                    "y": 4.017077206893078,
-                    "heading": 3.1151383358166123,
-                    "angularVelocity": 0.6366850980665145,
-                    "velocityX": -0.04255686409979938,
-                    "velocityY": 1.7955456582236569,
-                    "timestamp": 1.2625389522359176
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": 0.33476059520309653,
-                    "velocityX": -0.5686803455111581,
-                    "velocityY": 1.4801028352497836,
-                    "timestamp": 1.3368059494262656
-                },
-                {
-                    "x": 7.583164521123183,
-                    "y": 4.220739525524416,
-                    "heading": 3.1367175382385155,
-                    "angularVelocity": -0.03922526988843187,
-                    "velocityX": -1.1571796016903517,
-                    "velocityY": 1.1201830988719972,
-                    "timestamp": 1.4204882738773397
-                },
-                {
-                    "x": 7.437331010349068,
-                    "y": 4.2837766667768715,
-                    "heading": 3.1044919064382244,
-                    "angularVelocity": -0.38509484543694944,
-                    "velocityX": -1.742703871226464,
-                    "velocityY": 0.753290992643391,
-                    "timestamp": 1.5041705983284137
-                },
-                {
-                    "x": 7.242933099071244,
-                    "y": 4.315173048252524,
-                    "heading": 3.047144336604056,
-                    "angularVelocity": -0.6853008710064864,
-                    "velocityX": -2.3230462651820925,
-                    "velocityY": 0.3751853414876064,
-                    "timestamp": 1.5878529227794878
-                },
-                {
-                    "x": 7.00089933731331,
-                    "y": 4.3131727597182845,
-                    "heading": 2.9720298467353206,
-                    "angularVelocity": -0.8976147634695787,
-                    "velocityX": -2.8922925282679244,
-                    "velocityY": -0.023903357696631062,
-                    "timestamp": 1.6715352472305618
-                },
-                {
-                    "x": 6.7143889772576815,
-                    "y": 4.273350582816976,
-                    "heading": 2.8998851691901093,
-                    "angularVelocity": -0.8621256402526404,
-                    "velocityX": -3.4237858703738695,
-                    "velocityY": -0.47587321650692566,
-                    "timestamp": 1.7552175716816358
-                },
-                {
-                    "x": 6.411635278119781,
-                    "y": 4.1808641012010535,
-                    "heading": 2.8998851130559236,
-                    "angularVelocity": -6.708009886588717e-7,
-                    "velocityX": -3.6178930392272837,
-                    "velocityY": -1.1052092807244585,
-                    "timestamp": 1.8388998961327099
-                },
-                {
-                    "x": 6.131017053711007,
-                    "y": 4.034347987511598,
-                    "heading": 2.8998850873919646,
-                    "angularVelocity": -3.066831498282175e-7,
-                    "velocityX": -3.3533751153487676,
-                    "velocityY": -1.7508609452539547,
-                    "timestamp": 1.922582220583784
-                },
-                {
-                    "x": 5.854044563819553,
-                    "y": 3.8810515447858025,
-                    "heading": 2.8998850618676295,
-                    "angularVelocity": -3.050146543457835e-7,
-                    "velocityX": -3.3098087524252473,
-                    "velocityY": -1.8318855711927797,
-                    "timestamp": 2.006264545034858
-                },
-                {
-                    "x": 5.577072094171527,
-                    "y": 3.7277550654846525,
-                    "heading": 2.8998850363432944,
-                    "angularVelocity": -3.0501465285690934e-7,
-                    "velocityX": -3.3098085105171973,
-                    "velocityY": -1.8318860082666197,
-                    "timestamp": 2.089946869485932
-                },
-                {
-                    "x": 5.300099624523611,
-                    "y": 3.5744585861833027,
-                    "heading": 2.8998850108189593,
-                    "angularVelocity": -3.050146519273835e-7,
-                    "velocityX": -3.3098085105158774,
-                    "velocityY": -1.8318860082690038,
-                    "timestamp": 2.173629193937006
-                },
-                {
-                    "x": 5.0231271554381145,
-                    "y": 3.421162105865856,
-                    "heading": 2.899884985294552,
-                    "angularVelocity": -3.050155161997765e-7,
-                    "velocityX": -3.3098085037949927,
-                    "velocityY": -1.831886020411314,
-                    "timestamp": 2.25731151838808
-                },
-                {
-                    "x": 4.7695419048400876,
-                    "y": 3.2806617941163765,
-                    "heading": 2.834618611781297,
-                    "angularVelocity": -0.7799302175385181,
-                    "velocityX": -3.030332298504562,
-                    "velocityY": -1.678972383608021,
-                    "timestamp": 2.340993842839154
-                },
-                {
-                    "x": 4.566605904742026,
-                    "y": 3.1682378317487854,
-                    "heading": 2.7427589462823403,
-                    "angularVelocity": -1.0977188564194784,
-                    "velocityX": -2.4250760411980474,
-                    "velocityY": -1.3434612757837647,
-                    "timestamp": 2.424676167290228
-                },
-                {
-                    "x": 4.414672096834118,
-                    "y": 3.08405727220976,
-                    "heading": 2.6624278185804466,
-                    "angularVelocity": -0.959953350111109,
-                    "velocityX": -1.8156021466246228,
-                    "velocityY": -1.0059538868120508,
-                    "timestamp": 2.508358491741302
-                },
-                {
-                    "x": 4.313522085490217,
-                    "y": 3.0280030162190834,
-                    "heading": 2.6044623353701315,
-                    "angularVelocity": -0.6926849079605295,
-                    "velocityX": -1.2087380699258492,
-                    "velocityY": -0.6698458289533948,
-                    "timestamp": 2.5920408161923763
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": -0.3640235326869004,
-                    "velocityX": -0.6037366411798936,
-                    "velocityY": -0.3346347798387892,
-                    "timestamp": 2.6757231406434503
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": 0,
-                    "velocityX": 3.2974830734555563e-32,
-                    "velocityY": 2.8302357269459625e-32,
-                    "timestamp": 2.7594054650945243
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -24786,371 +5431,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S4 To C3 To S4": {
+        "C5 To W2": {
             "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 18
-                },
                 {
                     "x": 7.68,
-                    "y": 4.127,
+                    "y": 0.787,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 18
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 6.749906626357912e-35,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.210246643603351,
-                    "y": 1.6542137133955805,
-                    "heading": 2.5206016461454785,
-                    "angularVelocity": 0.26943757558327686,
-                    "velocityX": 0.4478929770559344,
-                    "velocityY": 0.44746230101760404,
-                    "timestamp": 0.07646166686617559
-                },
-                {
-                    "x": 5.278793169653249,
-                    "y": 1.7226907202744617,
-                    "heading": 2.5594414345510907,
-                    "angularVelocity": 0.5079641864673213,
-                    "velocityX": 0.8964822356001046,
-                    "velocityY": 0.8955730326770261,
-                    "timestamp": 0.15292333373235117
-                },
-                {
-                    "x": 5.381708599884549,
-                    "y": 1.8254946394710425,
-                    "heading": 2.61294205757613,
-                    "angularVelocity": 0.6997051623093229,
-                    "velocityX": 1.3459741913738752,
-                    "velocityY": 1.344515800008778,
-                    "timestamp": 0.22938500059852676
-                },
-                {
-                    "x": 5.519076425443274,
-                    "y": 1.9627026546285364,
-                    "heading": 2.6750057017308966,
-                    "angularVelocity": 0.8116961962572822,
-                    "velocityX": 1.796558081831356,
-                    "velocityY": 1.7944680096712713,
-                    "timestamp": 0.30584666746470235
-                },
-                {
-                    "x": 5.690930173156719,
-                    "y": 2.1343411411965687,
-                    "heading": 2.7325200708925137,
-                    "angularVelocity": 0.7521987359009454,
-                    "velocityX": 2.2475804511851214,
-                    "velocityY": 2.2447651693028954,
-                    "timestamp": 0.38230833433087796
-                },
-                {
-                    "x": 5.895075735242969,
-                    "y": 2.338989079985295,
-                    "heading": 2.7330245204544825,
-                    "angularVelocity": 0.006597417799580435,
-                    "velocityX": 2.6699072941157542,
-                    "velocityY": 2.6764776021284593,
-                    "timestamp": 0.4587700011970536
-                },
-                {
-                    "x": 6.099717519184054,
-                    "y": 2.543408411889471,
-                    "heading": 2.7330245436350364,
-                    "angularVelocity": 3.031656877658216e-7,
-                    "velocityX": 2.676397106268319,
-                    "velocityY": 2.673487778679394,
-                    "timestamp": 0.5352316680632292
-                },
-                {
-                    "x": 6.3043593056762255,
-                    "y": 2.7478277412397842,
-                    "heading": 2.7330245668155904,
-                    "angularVelocity": 3.03165682232601e-7,
-                    "velocityX": 2.676397139632574,
-                    "velocityY": 2.673487745278835,
-                    "timestamp": 0.6116933349294048
-                },
-                {
-                    "x": 6.509001092168415,
-                    "y": 2.9522470705900794,
-                    "heading": 2.733024589996144,
-                    "angularVelocity": 3.031656817444859e-7,
-                    "velocityX": 2.676397139632809,
-                    "velocityY": 2.6734877452786003,
-                    "timestamp": 0.6881550017955804
-                },
-                {
-                    "x": 6.713642878660604,
-                    "y": 3.1566663999403746,
-                    "heading": 2.733024613176697,
-                    "angularVelocity": 3.0316568050462653e-7,
-                    "velocityX": 2.676397139632809,
-                    "velocityY": 2.6734877452786003,
-                    "timestamp": 0.764616668661756
-                },
-                {
-                    "x": 6.918284665152036,
-                    "y": 3.3610857292914273,
-                    "heading": 2.7330246363572503,
-                    "angularVelocity": 3.0316568069769535e-7,
-                    "velocityX": 2.676397139622915,
-                    "velocityY": 2.6734877452885044,
-                    "timestamp": 0.8410783355279317
-                },
-                {
-                    "x": 7.122926344234402,
-                    "y": 3.565505166167973,
-                    "heading": 2.733024659538502,
-                    "angularVelocity": 3.031748161580926e-7,
-                    "velocityX": 2.6763957348789207,
-                    "velocityY": 2.67348915155517,
-                    "timestamp": 0.9175400023941073
-                },
-                {
-                    "x": 7.301295269977114,
-                    "y": 3.7451643880510668,
-                    "heading": 2.8254238510952887,
-                    "angularVelocity": 1.2084381016504042,
-                    "velocityX": 2.332788873866637,
-                    "velocityY": 2.349663945955243,
-                    "timestamp": 0.9940016692602829
-                },
-                {
-                    "x": 7.445441288033757,
-                    "y": 3.8901748417746975,
-                    "heading": 2.925487102833734,
-                    "angularVelocity": 1.308672120810262,
-                    "velocityX": 1.8852063257910538,
-                    "velocityY": 1.8965118034561068,
-                    "timestamp": 1.0704633361264584
-                },
-                {
-                    "x": 7.555314221872798,
-                    "y": 4.0007360886631185,
-                    "heading": 3.011754015083649,
-                    "angularVelocity": 1.1282374003289979,
-                    "velocityX": 1.436967546513772,
-                    "velocityY": 1.445969613531011,
-                    "timestamp": 1.146925002992634
-                },
-                {
-                    "x": 7.6309987143881095,
-                    "y": 4.077005222028419,
-                    "heading": 3.0780078899884065,
-                    "angularVelocity": 0.8664979148403406,
-                    "velocityX": 0.98983576499549,
-                    "velocityY": 0.997481960454622,
-                    "timestamp": 1.2233866698588094
-                },
-                {
-                    "x": 7.672548767265181,
-                    "y": 4.1190736771641765,
-                    "heading": 3.121324471245367,
-                    "angularVelocity": 0.5665136928387217,
-                    "velocityX": 0.5434102417593452,
-                    "velocityY": 0.5501901391894305,
-                    "timestamp": 1.299848336724985
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": 0.24424694778521738,
-                    "velocityX": 0.09745056627996526,
-                    "velocityY": 0.10366400787072177,
-                    "timestamp": 1.3763100035911604
-                },
-                {
-                    "x": 7.651764263007655,
-                    "y": 4.099219122472668,
-                    "heading": 3.13204050140682,
-                    "angularVelocity": -0.10143830296235676,
-                    "velocityX": -0.35984493368073844,
-                    "velocityY": -0.35404806448388143,
-                    "timestamp": 1.4547764049477947
-                },
-                {
-                    "x": 7.587580185818596,
-                    "y": 4.035451706139359,
-                    "heading": 3.0993558868019444,
-                    "angularVelocity": -0.41654280099226804,
-                    "velocityX": -0.8179816594027142,
-                    "velocityY": -0.8126716050540244,
-                    "timestamp": 1.533242806304429
-                },
-                {
-                    "x": 7.487355398269796,
-                    "y": 3.9356017103716967,
-                    "heading": 3.045691872479544,
-                    "angularVelocity": -0.683910736246133,
-                    "velocityX": -1.2772955789481495,
-                    "velocityY": -1.272519116989166,
-                    "timestamp": 1.6117092076610633
-                },
-                {
-                    "x": 7.35096170591853,
-                    "y": 3.7995397339434045,
-                    "heading": 2.9777353222524336,
-                    "angularVelocity": -0.8660592183684257,
-                    "velocityX": -1.7382432479775494,
-                    "velocityY": -1.7340157580297684,
-                    "timestamp": 1.6901756090176976
-                },
-                {
-                    "x": 7.178293181957013,
-                    "y": 3.6271506268794376,
-                    "heading": 2.9104315316891,
-                    "angularVelocity": -0.8577402480513148,
-                    "velocityX": -2.2005408808889824,
-                    "velocityY": -2.1969799058382784,
-                    "timestamp": 1.7686420103743319
-                },
-                {
-                    "x": 6.971558922924333,
-                    "y": 3.4198249259322475,
-                    "heading": 2.9002273959835922,
-                    "angularVelocity": -0.13004465005511517,
-                    "velocityX": -2.6346851067256227,
-                    "velocityY": -2.6422226247497056,
-                    "timestamp": 1.8471084117309662
-                },
-                {
-                    "x": 6.761730033100129,
-                    "y": 3.2098675579352265,
-                    "heading": 2.900227368387608,
-                    "angularVelocity": -3.5169172941648496e-7,
-                    "velocityX": -2.6741240352099296,
-                    "velocityY": -2.6757614006376103,
-                    "timestamp": 1.9255748130876005
-                },
-                {
-                    "x": 6.551901141744289,
-                    "y": 2.9999101914688975,
-                    "heading": 2.900227340791633,
-                    "angularVelocity": -3.5169161244263704e-7,
-                    "velocityX": -2.674124054729561,
-                    "velocityY": -2.6757613811300027,
-                    "timestamp": 2.0040412144442348
-                },
-                {
-                    "x": 6.342072250388439,
-                    "y": 2.7899528250025782,
-                    "heading": 2.9002273131956575,
-                    "angularVelocity": -3.5169161245965383e-7,
-                    "velocityX": -2.674124054729688,
-                    "velocityY": -2.6757613811298757,
-                    "timestamp": 2.082507615800869
-                },
-                {
-                    "x": 6.132243359032647,
-                    "y": 2.579995458536201,
-                    "heading": 2.9002272855996822,
-                    "angularVelocity": -3.5169161351505577e-7,
-                    "velocityX": -2.674124054728939,
-                    "velocityY": -2.675761381130624,
-                    "timestamp": 2.1609740171575034
-                },
-                {
-                    "x": 5.922414476696474,
-                    "y": 2.3700380830557237,
-                    "heading": 2.900227258003707,
-                    "angularVelocity": -3.51691614830637e-7,
-                    "velocityX": -2.6741239397801397,
-                    "velocityY": -2.67576149600908,
-                    "timestamp": 2.2394404185141377
-                },
-                {
-                    "x": 5.71397323769833,
-                    "y": 2.1587030259315876,
-                    "heading": 2.9002272179386366,
-                    "angularVelocity": -5.106016050290645e-7,
-                    "velocityX": -2.6564393854481323,
-                    "velocityY": -2.6933190954381305,
-                    "timestamp": 2.317906819870772
-                },
-                {
-                    "x": 5.5341856675792425,
-                    "y": 1.9787047842990844,
-                    "heading": 2.7906748594245325,
-                    "angularVelocity": -1.39616901782181,
-                    "velocityX": -2.2912681990084183,
-                    "velocityY": -2.2939530616983537,
-                    "timestamp": 2.3963732212274063
-                },
-                {
-                    "x": 5.390687970791945,
-                    "y": 1.8350178958205943,
-                    "heading": 2.6821705547459764,
-                    "angularVelocity": -1.3828122967612335,
-                    "velocityX": -1.8287788697622565,
-                    "velocityY": -1.8311899869783075,
-                    "timestamp": 2.4748396225840406
-                },
-                {
-                    "x": 5.2832586949423685,
-                    "y": 1.7274312477093827,
-                    "heading": 2.5937450000537616,
-                    "angularVelocity": -1.1269225192361696,
-                    "velocityX": -1.369111798071441,
-                    "velocityY": -1.371117398671364,
-                    "timestamp": 2.553306023940675
-                },
-                {
-                    "x": 5.211730147578831,
-                    "y": 1.6557903820034874,
-                    "heading": 2.5319094528254147,
-                    "angularVelocity": -0.7880512698333239,
-                    "velocityX": -0.9115818506628853,
-                    "velocityY": -0.913013270231208,
-                    "timestamp": 2.631772425297309
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": -0.4066639003920201,
-                    "velocityX": -0.4553560117589102,
-                    "velocityY": -0.45612365782926495,
-                    "timestamp": 2.7102388266539434
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": 0,
-                    "velocityX": -6.900806950408652e-34,
-                    "velocityY": 1.697639013664865e-32,
-                    "timestamp": 2.7887052280105777
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -25170,335 +5472,69 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S4 To C4 To S3": {
+        "C5 To W3": {
             "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
                 {
                     "x": 7.68,
-                    "y": 2.457,
+                    "y": 0.787,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 17
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
+            "trajectory": [],
+            "constraints": [
                 {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": 2.0168042378406012e-32,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.220318940234659,
-                    "y": 1.6240042188799209,
-                    "heading": 2.522514839920101,
-                    "angularVelocity": 0.3066580902466342,
-                    "velocityX": 0.6036357185902667,
-                    "velocityY": 0.05453852299210067,
-                    "timestamp": 0.07342000956828917
-                },
-                {
-                    "x": 5.309030400883647,
-                    "y": 1.632013165892952,
-                    "heading": 2.5656393100423864,
-                    "angularVelocity": 0.5873667189075205,
-                    "velocityX": 1.208273618739824,
-                    "velocityY": 0.10908398214768973,
-                    "timestamp": 0.14684001913657835
-                },
-                {
-                    "x": 5.442235678671135,
-                    "y": 1.6440289719213574,
-                    "heading": 2.6263948924042286,
-                    "angularVelocity": 0.8275071430674761,
-                    "velocityX": 1.8142912071346369,
-                    "velocityY": 0.1636584644847863,
-                    "timestamp": 0.22026002870486752
-                },
-                {
-                    "x": 5.6200752852220655,
-                    "y": 1.6600527803178184,
-                    "heading": 2.6994681964366594,
-                    "angularVelocity": 0.9952777786614565,
-                    "velocityX": 2.4222226011223906,
-                    "velocityY": 0.21824851958859787,
-                    "timestamp": 0.2936800382731567
-                },
-                {
-                    "x": 5.842683997650443,
-                    "y": 1.680064789350092,
-                    "heading": 2.773109972156216,
-                    "angularVelocity": 1.0030205137886674,
-                    "velocityX": 3.031989695143347,
-                    "velocityY": 0.2725688698481895,
-                    "timestamp": 0.3671000478414459
-                },
-                {
-                    "x": 6.108354667125484,
-                    "y": 1.7034775424781843,
-                    "heading": 2.8018843070789763,
-                    "angularVelocity": 0.39191407208947715,
-                    "velocityX": 3.6185049693835545,
-                    "velocityY": 0.31888790625005003,
-                    "timestamp": 0.4405200574097351
-                },
-                {
-                    "x": 6.384952370737624,
-                    "y": 1.7286802043771023,
-                    "heading": 2.8018843326053964,
-                    "angularVelocity": 3.476766132233899e-7,
-                    "velocityX": 3.7673340719858177,
-                    "velocityY": 0.34326693836064737,
-                    "timestamp": 0.5139400669780243
-                },
-                {
-                    "x": 6.661294346947145,
-                    "y": 1.7565473171945905,
-                    "heading": 2.8018843581575315,
-                    "angularVelocity": 3.48026857889212e-7,
-                    "velocityX": 3.7638509969478653,
-                    "velocityY": 0.37955746643565225,
-                    "timestamp": 0.5873600765463135
-                },
-                {
-                    "x": 6.9295239726753675,
-                    "y": 1.8286189976607798,
-                    "heading": 2.8018844037308708,
-                    "angularVelocity": 6.207209630445694e-7,
-                    "velocityX": 3.6533586321414817,
-                    "velocityY": 0.9816354000767032,
-                    "timestamp": 0.6607800861146027
-                },
-                {
-                    "x": 7.165852331775521,
-                    "y": 1.9289604111011978,
-                    "heading": 2.8534870271763118,
-                    "angularVelocity": 0.7028414154241911,
-                    "velocityX": 3.218854921019001,
-                    "velocityY": 1.3666766598157905,
-                    "timestamp": 0.7342000956828919
-                },
-                {
-                    "x": 7.3582678693157275,
-                    "y": 2.0352947515499316,
-                    "heading": 2.9321246763669695,
-                    "angularVelocity": 1.0710656352818668,
-                    "velocityX": 2.620750646473749,
-                    "velocityY": 1.4483019148865994,
-                    "timestamp": 0.8076201052511811
-                },
-                {
-                    "x": 7.505754872565929,
-                    "y": 2.142580812119965,
-                    "heading": 3.007723504913372,
-                    "angularVelocity": 1.0296760922659598,
-                    "velocityX": 2.0088120951961708,
-                    "velocityY": 1.4612645953179093,
-                    "timestamp": 0.8810401148194703
-                },
-                {
-                    "x": 7.608443195664495,
-                    "y": 2.2491262386362876,
-                    "heading": 3.070552698416666,
-                    "angularVelocity": 0.855750276698862,
-                    "velocityX": 1.3986421917181704,
-                    "velocityY": 1.4511769631033729,
-                    "timestamp": 0.9544601243877595
-                },
-                {
-                    "x": 7.666484933948513,
-                    "y": 2.354097419514004,
-                    "heading": 3.115652624171886,
-                    "angularVelocity": 0.6142729484838055,
-                    "velocityX": 0.7905438670643103,
-                    "velocityY": 1.4297353200436178,
-                    "timestamp": 1.0278801339560486
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "C5 To S3": {
+            "waypoints": [
                 {
                     "x": 7.68,
-                    "y": 2.457,
+                    "y": 0.787,
                     "heading": 3.14,
-                    "angularVelocity": 0.331617715269703,
-                    "velocityX": 0.18407878357617766,
-                    "velocityY": 1.4015604341523722,
-                    "timestamp": 1.1013001435243377
-                },
-                {
-                    "x": 7.640202623194362,
-                    "y": 2.56833046361043,
-                    "heading": 3.1389376214813485,
-                    "angularVelocity": -0.013028904924243205,
-                    "velocityX": -0.48807108721701464,
-                    "velocityY": 1.3653457784439293,
-                    "timestamp": 1.1828402671824687
-                },
-                {
-                    "x": 7.5455521042399925,
-                    "y": 2.676247019412303,
-                    "heading": 3.111715108217083,
-                    "angularVelocity": -0.3338542062850063,
-                    "velocityX": -1.1607845893293163,
-                    "velocityY": 1.3234779512271038,
-                    "timestamp": 1.2643803908406
-                },
-                {
-                    "x": 7.395994092884954,
-                    "y": 2.780007432926005,
-                    "heading": 3.061424395012115,
-                    "angularVelocity": -0.616760325454245,
-                    "velocityX": -1.8341646375480058,
-                    "velocityY": 1.2725074338705782,
-                    "timestamp": 1.3459205144987312
-                },
-                {
-                    "x": 7.191491634258091,
-                    "y": 2.8782237282133547,
-                    "heading": 2.9936811867966338,
-                    "angularVelocity": -0.8307959955948208,
-                    "velocityX": -2.5079978966461463,
-                    "velocityY": 1.2045149170871063,
-                    "timestamp": 1.4274606381568624
-                },
-                {
-                    "x": 6.932297950713545,
-                    "y": 2.9674313510808874,
-                    "heading": 2.9221270442336884,
-                    "angularVelocity": -0.8775329169593383,
-                    "velocityX": -3.178725661874757,
-                    "velocityY": 1.0940334508388325,
-                    "timestamp": 1.5090007618149937
-                },
-                {
-                    "x": 6.629938780171593,
-                    "y": 3.0284835582667458,
-                    "heading": 2.9221269432503374,
-                    "angularVelocity": -0.0000012384498122465325,
-                    "velocityX": -3.7081029188726404,
-                    "velocityY": 0.7487382217106987,
-                    "timestamp": 1.5905408854731249
-                },
-                {
-                    "x": 6.3215346451204795,
-                    "y": 3.034428801743739,
-                    "heading": 2.922126920201386,
-                    "angularVelocity": -2.8267005270994496e-7,
-                    "velocityX": -3.7822377648596976,
-                    "velocityY": 0.07291187712592015,
-                    "timestamp": 1.6720810091312561
-                },
-                {
-                    "x": 6.01312270862256,
-                    "y": 3.0289030469499765,
-                    "heading": 2.9221268973831047,
-                    "angularVelocity": -2.7984114359464816e-7,
-                    "velocityX": -3.7823334410305938,
-                    "velocityY": -0.06776730946506612,
-                    "timestamp": 1.7536211327893874
-                },
-                {
-                    "x": 5.704710773369897,
-                    "y": 3.023377222654451,
-                    "heading": 2.9221268745648237,
-                    "angularVelocity": -2.7984113775105834e-7,
-                    "velocityX": -3.782333425758887,
-                    "velocityY": -0.06776816182780301,
-                    "timestamp": 1.8351612564475186
-                },
-                {
-                    "x": 5.396298838117239,
-                    "y": 3.0178513983586757,
-                    "heading": 2.922126851746543,
-                    "angularVelocity": -2.7984113682317286e-7,
-                    "velocityX": -3.7823334257588326,
-                    "velocityY": -0.06776816183086862,
-                    "timestamp": 1.9167013801056498
-                },
-                {
-                    "x": 5.087886902401733,
-                    "y": 3.012325599899855,
-                    "heading": 2.922126828928091,
-                    "angularVelocity": -2.798432358955337e-7,
-                    "velocityX": -3.7823334314351498,
-                    "velocityY": -0.06776784496901485,
-                    "timestamp": 1.998241503763781
-                },
-                {
-                    "x": 4.812446473600025,
-                    "y": 3.0084984766412206,
-                    "heading": 2.8330493298750046,
-                    "angularVelocity": -1.0924376252672112,
-                    "velocityX": -3.3779741364697116,
-                    "velocityY": -0.04693546056758059,
-                    "timestamp": 2.0797816274219123
-                },
-                {
-                    "x": 4.592410263299027,
-                    "y": 3.0051727297391726,
-                    "heading": 2.737251875069825,
-                    "angularVelocity": -1.1748504969997702,
-                    "velocityX": -2.6985022885608188,
-                    "velocityY": -0.040786630591711566,
-                    "timestamp": 2.1613217510800435
-                },
-                {
-                    "x": 4.427608751414137,
-                    "y": 3.0026089111743004,
-                    "heading": 2.658186226973151,
-                    "angularVelocity": -0.9696532768109137,
-                    "velocityX": -2.0211094181785265,
-                    "velocityY": -0.03144241693353635,
-                    "timestamp": 2.2428618747381748
-                },
-                {
-                    "x": 4.3178442485460256,
-                    "y": 3.0008747293604148,
-                    "heading": 2.602696579769804,
-                    "angularVelocity": -0.6805195370563186,
-                    "velocityX": -1.346140991008498,
-                    "velocityY": -0.021267833994898407,
-                    "timestamp": 2.324401998396306
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": -0.3519320119027408,
-                    "velocityX": -0.6726044318496349,
-                    "velocityY": -0.010727594234238386,
-                    "timestamp": 2.4059421220544372
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 4.263,
                     "y": 3,
                     "heading": 2.574,
-                    "angularVelocity": 0,
-                    "velocityX": -1.5467932880075213e-32,
-                    "velocityY": 0,
-                    "timestamp": 2.4874822457125685
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 }
             ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -25518,25 +5554,16 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S4 To C4 To S4": {
+        "C5 To S4": {
             "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
                 {
                     "x": 7.68,
-                    "y": 2.457,
+                    "y": 0.787,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 15
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 5.176,
@@ -25548,287 +5575,7 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.2208311166528825,
-                    "y": 1.6350333822394376,
-                    "heading": 2.5207239283142147,
-                    "angularVelocity": 0.274168379737783,
-                    "velocityX": 0.5930957889931558,
-                    "velocityY": 0.1988849791445399,
-                    "timestamp": 0.07558832398555491
-                },
-                {
-                    "x": 5.31056444260237,
-                    "y": 1.6651195189308639,
-                    "heading": 2.5599126898589697,
-                    "angularVelocity": 0.5184499335141133,
-                    "velocityX": 1.1871321021304377,
-                    "velocityY": 0.39802624406880477,
-                    "timestamp": 0.15117664797110983
-                },
-                {
-                    "x": 5.445292613790804,
-                    "y": 1.7102861706342203,
-                    "heading": 2.6140890806736206,
-                    "angularVelocity": 0.716729621164803,
-                    "velocityX": 1.7823939476973087,
-                    "velocityY": 0.5975347688882264,
-                    "timestamp": 0.22676497195666473
-                },
-                {
-                    "x": 5.625130316677607,
-                    "y": 1.7705705454307188,
-                    "heading": 2.677161642931129,
-                    "angularVelocity": 0.834422023559608,
-                    "velocityX": 2.379173044254461,
-                    "velocityY": 0.79753554011886,
-                    "timestamp": 0.30235329594221966
-                },
-                {
-                    "x": 5.850123959727638,
-                    "y": 1.8459876978345418,
-                    "heading": 2.735754513087534,
-                    "angularVelocity": 0.7751576839777234,
-                    "velocityX": 2.97656610421776,
-                    "velocityY": 0.9977354758948757,
-                    "timestamp": 0.3779416199277746
-                },
-                {
-                    "x": 6.118241955907835,
-                    "y": 1.9356767665379448,
-                    "heading": 2.743662980850177,
-                    "angularVelocity": 0.10462552079005556,
-                    "velocityX": 3.547082168820654,
-                    "velocityY": 1.1865465983945254,
-                    "timestamp": 0.4535299439133295
-                },
-                {
-                    "x": 6.3893659801779465,
-                    "y": 2.026544533897615,
-                    "heading": 2.7436630046141004,
-                    "angularVelocity": 3.143861609924561e-7,
-                    "velocityX": 3.5868505871611136,
-                    "velocityY": 1.2021402588187382,
-                    "timestamp": 0.5291182678988844
-                },
-                {
-                    "x": 6.660490004070523,
-                    "y": 2.1174123023837605,
-                    "heading": 2.7436630283780157,
-                    "angularVelocity": 3.1438606173533106e-7,
-                    "velocityX": 3.5868505821664702,
-                    "velocityY": 1.2021402737215172,
-                    "timestamp": 0.6047065918844393
-                },
-                {
-                    "x": 6.931614067804288,
-                    "y": 2.2082799519937164,
-                    "heading": 2.743663052142803,
-                    "angularVelocity": 3.1439759977679495e-7,
-                    "velocityX": 3.5868511092477546,
-                    "velocityY": 1.2021387010422189,
-                    "timestamp": 0.6802949158699942
-                },
-                {
-                    "x": 7.168848548798668,
-                    "y": 2.287192936383632,
-                    "heading": 2.8325335746925835,
-                    "angularVelocity": 1.1757175958389672,
-                    "velocityX": 3.1385069609389844,
-                    "velocityY": 1.0439837825349283,
-                    "timestamp": 0.7558832398555492
-                },
-                {
-                    "x": 7.360921839643199,
-                    "y": 2.3511354933788766,
-                    "heading": 2.9301346596996316,
-                    "angularVelocity": 1.2912190648082638,
-                    "velocityX": 2.541044445981228,
-                    "velocityY": 0.8459316680637452,
-                    "timestamp": 0.8314715638411041
-                },
-                {
-                    "x": 7.507932695990372,
-                    "y": 2.4000526423462776,
-                    "heading": 3.014739800319359,
-                    "angularVelocity": 1.119288484765134,
-                    "velocityX": 1.944888424504045,
-                    "velocityY": 0.6471521842017212,
-                    "timestamp": 0.907059887826659
-                },
-                {
-                    "x": 7.6100514108149255,
-                    "y": 2.4339833202352414,
-                    "heading": 3.0798385166063986,
-                    "angularVelocity": 0.8612271426930879,
-                    "velocityX": 1.3509853035511394,
-                    "velocityY": 0.4488878189103197,
-                    "timestamp": 0.982648211812214
-                },
-                {
-                    "x": 7.667383998050378,
-                    "y": 2.452958398052505,
-                    "heading": 3.1222248732691185,
-                    "angularVelocity": 0.5607526986683374,
-                    "velocityX": 0.7584846999174467,
-                    "velocityY": 0.25103186334555994,
-                    "timestamp": 1.0582365357977688
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": 0.23515704269716123,
-                    "velocityX": 0.16690411011130135,
-                    "velocityY": 0.05346860115942382,
-                    "timestamp": 1.1338248597833236
-                },
-                {
-                    "x": 7.644913557520251,
-                    "y": 2.4451050214750283,
-                    "heading": 3.130617959515128,
-                    "angularVelocity": -0.11950075188579921,
-                    "velocityX": -0.44690238377135916,
-                    "velocityY": -0.1515084996373644,
-                    "timestamp": 1.212335164838424
-                },
-                {
-                    "x": 7.561539712456839,
-                    "y": 2.4170767124271264,
-                    "heading": 3.096030936228231,
-                    "angularVelocity": -0.44054119089994853,
-                    "velocityX": -1.0619477915019166,
-                    "velocityY": -0.3570016576579214,
-                    "timestamp": 1.2908454698935243
-                },
-                {
-                    "x": 7.429747941085825,
-                    "y": 2.3728628641773573,
-                    "heading": 3.040236463304404,
-                    "angularVelocity": -0.7106643246980755,
-                    "velocityX": -1.6786557035858298,
-                    "velocityY": -0.5631598070945942,
-                    "timestamp": 1.3693557749486247
-                },
-                {
-                    "x": 7.249364854543017,
-                    "y": 2.3124007511137603,
-                    "heading": 2.9700149395216235,
-                    "angularVelocity": -0.8944242890598059,
-                    "velocityX": -2.2975721010917782,
-                    "velocityY": -0.7701169040314122,
-                    "timestamp": 1.447866080003725
-                },
-                {
-                    "x": 7.020237112848181,
-                    "y": 2.2356470641616397,
-                    "heading": 2.9000211501626434,
-                    "angularVelocity": -0.8915235943849231,
-                    "velocityX": -2.9184416177471677,
-                    "velocityY": -0.977625636510412,
-                    "timestamp": 1.5263763850588254
-                },
-                {
-                    "x": 6.744411982227483,
-                    "y": 2.1437703180815415,
-                    "heading": 2.8843371221032816,
-                    "angularVelocity": -0.19977031102303894,
-                    "velocityX": -3.5132347330342664,
-                    "velocityY": -1.1702507844749341,
-                    "timestamp": 1.6048866901139258
-                },
-                {
-                    "x": 6.462777076833342,
-                    "y": 2.049479989397194,
-                    "heading": 2.8843370939243,
-                    "angularVelocity": -3.589208009765681e-7,
-                    "velocityX": -3.5872348884198493,
-                    "velocityY": -1.2009930240135072,
-                    "timestamp": 1.6833969951690262
-                },
-                {
-                    "x": 6.181142164450445,
-                    "y": 1.955189681587478,
-                    "heading": 2.884337065745333,
-                    "angularVelocity": -3.589206129086503e-7,
-                    "velocityX": -3.5872349774369066,
-                    "velocityY": -1.200992758129539,
-                    "timestamp": 1.7619073002241266
-                },
-                {
-                    "x": 5.898328744228412,
-                    "y": 1.8644954419041788,
-                    "heading": 2.8843369910470673,
-                    "angularVelocity": -9.514453601861041e-7,
-                    "velocityX": -3.6022458456064737,
-                    "velocityY": -1.1551889859509814,
-                    "timestamp": 1.840417605279227
-                },
-                {
-                    "x": 5.657030939387785,
-                    "y": 1.782595874715138,
-                    "heading": 2.7806821628594123,
-                    "angularVelocity": -1.3202703532347235,
-                    "velocityX": -3.0734539201099995,
-                    "velocityY": -1.043169646730562,
-                    "timestamp": 1.9189279103343273
-                },
-                {
-                    "x": 5.464337892157779,
-                    "y": 1.7174002123662173,
-                    "heading": 2.6765644069162446,
-                    "angularVelocity": -1.3261667480478392,
-                    "velocityX": -2.4543663038217995,
-                    "velocityY": -0.8304089801098533,
-                    "timestamp": 1.9974382153894277
-                },
-                {
-                    "x": 5.320057899798496,
-                    "y": 1.6686446362166114,
-                    "heading": 2.5910880570386063,
-                    "angularVelocity": -1.0887277767886236,
-                    "velocityX": -1.8377204401132547,
-                    "velocityY": -0.6210086193829906,
-                    "timestamp": 2.075948520444528
-                },
-                {
-                    "x": 5.223989702910862,
-                    "y": 1.6362005448482675,
-                    "heading": 2.5310608538348376,
-                    "angularVelocity": -0.7645773782389272,
-                    "velocityX": -1.2236380538862024,
-                    "velocityY": -0.41324627825065113,
-                    "timestamp": 2.154458825499628
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": -0.3956277308187441,
-                    "velocityX": -0.6112535529849511,
-                    "velocityY": -0.2063492790774072,
-                    "timestamp": 2.2329691305547286
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": 3.766832812159788e-31,
-                    "velocityX": 0,
-                    "velocityY": 1.2430862122144045e-32,
-                    "timestamp": 2.311479435609829
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -25848,17 +5595,8 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S4 To C5 To S3": {
+        "C5 To C1": {
             "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
                 {
                     "x": 7.68,
                     "y": 0.787,
@@ -25866,335 +5604,19 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 19
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.21689679817953,
-                    "y": 1.6000159633135866,
-                    "heading": 2.520043259980684,
-                    "angularVelocity": 0.2702645151942541,
-                    "velocityX": 0.5514548702977145,
-                    "velocityY": -0.2694659447557082,
-                    "timestamp": 0.0741616411102716
-                },
-                {
-                    "x": 5.298748395600195,
-                    "y": 1.5600344658293337,
-                    "heading": 2.558338172376498,
-                    "angularVelocity": 0.5163708869233576,
-                    "velocityX": 1.1036918303757857,
-                    "velocityY": -0.5391129010319948,
-                    "timestamp": 0.1483232822205432
-                },
-                {
-                    "x": 5.421633479629326,
-                    "y": 1.5000375028373136,
-                    "heading": 2.612131147144223,
-                    "angularVelocity": 0.7253476859785596,
-                    "velocityX": 1.656989815616613,
-                    "velocityY": -0.8090026338927551,
-                    "timestamp": 0.2224849233308148
-                },
-                {
-                    "x": 5.585659490393147,
-                    "y": 1.420004250322953,
-                    "heading": 2.6766780389547544,
-                    "angularVelocity": 0.8703541459463568,
-                    "velocityX": 2.211736529939155,
-                    "velocityY": -1.0791731590102869,
-                    "timestamp": 0.2966465644410864
-                },
-                {
-                    "x": 5.790940733567372,
-                    "y": 1.3199448136082716,
-                    "heading": 2.7418726481321705,
-                    "angularVelocity": 0.8790880056234819,
-                    "velocityX": 2.7680245488229662,
-                    "velocityY": -1.3492074233619007,
-                    "timestamp": 0.37080820555135796
-                },
-                {
-                    "x": 6.036365145395861,
-                    "y": 1.2006925884246085,
-                    "heading": 2.766396446394458,
-                    "angularVelocity": 0.3306803611021658,
-                    "velocityX": 3.3093174335714934,
-                    "velocityY": -1.6080041298754981,
-                    "timestamp": 0.44496984666162953
-                },
-                {
-                    "x": 6.288321832391407,
-                    "y": 1.0773004957458903,
-                    "heading": 2.7663964696788734,
-                    "angularVelocity": 3.1396844397951453e-7,
-                    "velocityX": 3.3973990222372876,
-                    "velocityY": -1.6638263505420645,
-                    "timestamp": 0.5191314877719011
-                },
-                {
-                    "x": 6.540278624834678,
-                    "y": 0.9539086183831178,
-                    "heading": 2.766396492963265,
-                    "angularVelocity": 3.1396812929995246e-7,
-                    "velocityX": 3.397400444100689,
-                    "velocityY": -1.663823447208988,
-                    "timestamp": 0.5932931288821727
-                },
-                {
-                    "x": 6.801316019800969,
-                    "y": 0.8511114616545457,
-                    "heading": 2.7663965374148853,
-                    "angularVelocity": 5.993883039125867e-7,
-                    "velocityX": 3.519843831100697,
-                    "velocityY": -1.3861230036120238,
-                    "timestamp": 0.6674547699924442
-                },
-                {
-                    "x": 7.047191253658378,
-                    "y": 0.7842163440856953,
-                    "heading": 2.8499152738316673,
-                    "angularVelocity": 1.1261716322139188,
-                    "velocityX": 3.3153963447464685,
-                    "velocityY": -0.9020177623819695,
-                    "timestamp": 0.7416164111027158
-                },
-                {
-                    "x": 7.2541679092956945,
-                    "y": 0.7415676707832506,
-                    "heading": 2.93858588461799,
-                    "angularVelocity": 1.1956398140448308,
-                    "velocityX": 2.790885591778509,
-                    "velocityY": -0.575077259132303,
-                    "timestamp": 0.8157780522129874
-                },
-                {
-                    "x": 7.421132571302565,
-                    "y": 0.7211096612789919,
-                    "heading": 3.016140310820358,
-                    "angularVelocity": 1.045748516905977,
-                    "velocityX": 2.251361478889076,
-                    "velocityY": -0.2758570225521344,
-                    "timestamp": 0.889939693323259
-                },
-                {
-                    "x": 7.547824539966462,
-                    "y": 0.722094092716002,
-                    "heading": 3.0771627100726597,
-                    "angularVelocity": 0.8228296776978581,
-                    "velocityX": 1.7083220754986685,
-                    "velocityY": 0.013274132317907437,
-                    "timestamp": 0.9641013344335305
-                },
-                {
-                    "x": 7.634133595568631,
-                    "y": 0.7441357582804435,
-                    "heading": 3.1189807676901022,
-                    "angularVelocity": 0.5638771876052062,
-                    "velocityX": 1.1637964628348345,
-                    "velocityY": 0.29721113549347455,
-                    "timestamp": 1.0382629755438022
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 3.14,
-                    "angularVelocity": 0.28342458439730217,
-                    "velocityX": 0.6184653379387841,
-                    "velocityY": 0.5779839965491583,
-                    "timestamp": 1.1124246166540739
-                },
-                {
-                    "x": 7.681059166008756,
-                    "y": 0.8599952012380814,
-                    "heading": 3.136373480041968,
-                    "angularVelocity": -0.044073969444744315,
-                    "velocityX": 0.012872299297217518,
-                    "velocityY": 0.8871282403539436,
-                    "timestamp": 1.1947071975906878
-                },
-                {
-                    "x": 7.6321422839644315,
-                    "y": 0.9582569372062664,
-                    "heading": 3.107236664084856,
-                    "angularVelocity": -0.3541067334720519,
-                    "velocityX": -0.5944986349176513,
-                    "velocityY": 1.1941985150402432,
-                    "timestamp": 1.2769897785273017
-                },
-                {
-                    "x": 7.533021561059945,
-                    "y": 1.0815072200967695,
-                    "heading": 3.0548999267391683,
-                    "angularVelocity": -0.6360609590747484,
-                    "velocityX": -1.204637989914761,
-                    "velocityY": 1.4978903370258465,
-                    "timestamp": 1.3592723594639156
-                },
-                {
-                    "x": 7.383292536414222,
-                    "y": 1.229213843710233,
-                    "heading": 2.9837724589347805,
-                    "angularVelocity": -0.8644292266327552,
-                    "velocityX": -1.819692855296544,
-                    "velocityY": 1.7951141290432664,
-                    "timestamp": 1.4415549404005294
-                },
-                {
-                    "x": 7.182069766586899,
-                    "y": 1.399958151778736,
-                    "heading": 2.9050185961423023,
-                    "angularVelocity": -0.9571146395266165,
-                    "velocityX": -2.445508727811161,
-                    "velocityY": 2.0750966501650514,
-                    "timestamp": 1.5238375213371433
-                },
-                {
-                    "x": 6.9296160896416765,
-                    "y": 1.5820466019910795,
-                    "heading": 2.9050185123503516,
-                    "angularVelocity": -0.0000010183437364658426,
-                    "velocityX": -3.068130266109342,
-                    "velocityY": 2.2129647385832873,
-                    "timestamp": 1.6061201022737572
-                },
-                {
-                    "x": 6.6549097990315635,
-                    "y": 1.728421234696809,
-                    "heading": 2.9050184910214054,
-                    "angularVelocity": -2.59215813112527e-7,
-                    "velocityX": -3.338571633062085,
-                    "velocityY": 1.7789261231181734,
-                    "timestamp": 1.6884026832103711
-                },
-                {
-                    "x": 6.380203216821482,
-                    "y": 1.8747953201455823,
-                    "heading": 2.905018469692463,
-                    "angularVelocity": -2.59215765603584e-7,
-                    "velocityX": -3.338575176946638,
-                    "velocityY": 1.7789194721727335,
-                    "timestamp": 1.770685264146985
-                },
-                {
-                    "x": 6.105496634609771,
-                    "y": 2.0211694055912957,
-                    "heading": 2.9050184483635206,
-                    "angularVelocity": -2.592157647120513e-7,
-                    "velocityX": -3.3385751769664513,
-                    "velocityY": 1.7789194721355484,
-                    "timestamp": 1.852967845083599
-                },
-                {
-                    "x": 5.83079005239806,
-                    "y": 2.167543491037009,
-                    "heading": 2.905018427034578,
-                    "angularVelocity": -2.592157660778243e-7,
-                    "velocityX": -3.3385751769664513,
-                    "velocityY": 1.778919472135548,
-                    "timestamp": 1.9352504260202128
-                },
-                {
-                    "x": 5.556083470186349,
-                    "y": 2.3139175764827224,
-                    "heading": 2.905018405705636,
-                    "angularVelocity": -2.592157651075657e-7,
-                    "velocityX": -3.338575176966452,
-                    "velocityY": 1.7789194721355481,
-                    "timestamp": 2.0175330069568265
-                },
-                {
-                    "x": 5.281376887974587,
-                    "y": 2.460291661928342,
-                    "heading": 2.9050183843766937,
-                    "angularVelocity": -2.5921576594087425e-7,
-                    "velocityX": -3.3385751769670593,
-                    "velocityY": 1.7789194721344066,
-                    "timestamp": 2.09981558789344
-                },
-                {
-                    "x": 5.006670296810042,
-                    "y": 2.6066657305717675,
-                    "heading": 2.905018363047624,
-                    "angularVelocity": -2.5921731220751964e-7,
-                    "velocityX": -3.338575285772395,
-                    "velocityY": 1.778919267933304,
-                    "timestamp": 2.182098168830054
-                },
-                {
-                    "x": 4.758260828306463,
-                    "y": 2.7376390899942815,
-                    "heading": 2.815277658009563,
-                    "angularVelocity": -1.0906403763293109,
-                    "velocityX": -3.0189800280443744,
-                    "velocityY": 1.591750743980768,
-                    "timestamp": 2.2643807497666675
-                },
-                {
-                    "x": 4.559939575601031,
-                    "y": 2.842572976973571,
-                    "heading": 2.72503756976798,
-                    "angularVelocity": -1.0967095005333054,
-                    "velocityX": -2.4102458922406798,
-                    "velocityY": 1.2752867713292104,
-                    "timestamp": 2.346663330703281
-                },
-                {
-                    "x": 4.4113908880653625,
-                    "y": 2.9212861863102546,
-                    "heading": 2.651616705628037,
-                    "angularVelocity": -0.8923014239977028,
-                    "velocityX": -1.8053479344565475,
-                    "velocityY": 0.9566205682988939,
-                    "timestamp": 2.428945911639895
-                },
-                {
-                    "x": 4.31244325419048,
-                    "y": 2.973762547349605,
-                    "heading": 2.6003949192739833,
-                    "angularVelocity": -0.6225106914611183,
-                    "velocityX": -1.202534397299805,
-                    "velocityY": 0.6377578394116608,
-                    "timestamp": 2.5112284925765085
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": -0.3207838034920002,
-                    "velocityX": -0.6008957622339189,
-                    "velocityY": 0.31887007373537113,
-                    "timestamp": 2.593511073513122
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": 0,
-                    "velocityX": -9.140912570286281e-33,
-                    "velocityY": 0,
-                    "timestamp": 2.675793654449736
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -26214,17 +5636,8 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "S4 To C5 To S4": {
+        "C5 To C2": {
             "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
                 {
                     "x": 7.68,
                     "y": 0.787,
@@ -26232,299 +5645,60 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 15
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
+            "trajectory": [],
+            "constraints": [
                 {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.220564813130112,
-                    "y": 1.6051202900758712,
-                    "heading": 2.517168506154745,
-                    "angularVelocity": 0.22812300403374605,
-                    "velocityX": 0.5921458136084756,
-                    "velocityY": -0.19771109358311004,
-                    "timestamp": 0.07525986354364067
-                },
-                {
-                    "x": 5.309738195223409,
-                    "y": 1.5753445036804834,
-                    "heading": 2.5497776530467964,
-                    "angularVelocity": 0.433287350742303,
-                    "velocityX": 1.1848730238739802,
-                    "velocityY": -0.3956396543042072,
-                    "timestamp": 0.15051972708728134
-                },
-                {
-                    "x": 5.443577884042542,
-                    "y": 1.5306492621981627,
-                    "heading": 2.5951663634005544,
-                    "angularVelocity": 0.6030931789749864,
-                    "velocityX": 1.7783674128179001,
-                    "velocityY": -0.593878853585797,
-                    "timestamp": 0.225779590630922
-                },
-                {
-                    "x": 5.622156845349364,
-                    "y": 1.4710006417461063,
-                    "heading": 2.6487285344970175,
-                    "angularVelocity": 0.7116963621041407,
-                    "velocityX": 2.372831319356166,
-                    "velocityY": -0.7925688095018658,
-                    "timestamp": 0.3010394541745627
-                },
-                {
-                    "x": 5.845518423751245,
-                    "y": 1.3963607866160492,
-                    "heading": 2.700604108231769,
-                    "angularVelocity": 0.6892860456047826,
-                    "velocityX": 2.967871158474283,
-                    "velocityY": -0.9917617653767852,
-                    "timestamp": 0.37629931771820335
-                },
-                {
-                    "x": 6.112314775127604,
-                    "y": 1.3070273852513161,
-                    "heading": 2.7110861192214117,
-                    "angularVelocity": 0.13927757101984,
-                    "velocityX": 3.5450017953016038,
-                    "velocityY": -1.186999247121034,
-                    "timestamp": 0.451559181261844
-                },
-                {
-                    "x": 6.382409273163311,
-                    "y": 1.216999105128909,
-                    "heading": 2.7110861382363836,
-                    "angularVelocity": 2.526575375097703e-7,
-                    "velocityX": 3.588825242542295,
-                    "velocityY": -1.196232306084407,
-                    "timestamp": 0.5268190448054847
-                },
-                {
-                    "x": 6.652503772105593,
-                    "y": 1.1269708277263142,
-                    "heading": 2.711086157251347,
-                    "angularVelocity": 2.5265742250097044e-7,
-                    "velocityX": 3.5888252545882353,
-                    "velocityY": -1.19623226994546,
-                    "timestamp": 0.6020789083491254
-                },
-                {
-                    "x": 6.9225982993416295,
-                    "y": 1.0369426352093585,
-                    "heading": 2.7110861762673086,
-                    "angularVelocity": 2.5267068695978854e-7,
-                    "velocityX": 3.5888256305356965,
-                    "velocityY": -1.19623114204494,
-                    "timestamp": 0.677338771892766
-                },
-                {
-                    "x": 7.1604160781282875,
-                    "y": 0.9586172665469176,
-                    "heading": 2.823441998219159,
-                    "angularVelocity": 1.4929049384563269,
-                    "velocityX": 3.1599549559208913,
-                    "velocityY": -1.040732270488674,
-                    "timestamp": 0.7525986354364067
-                },
-                {
-                    "x": 7.353386220004786,
-                    "y": 0.8948816690648573,
-                    "heading": 2.9268528477366496,
-                    "angularVelocity": 1.3740504519719978,
-                    "velocityX": 2.5640511793461145,
-                    "velocityY": -0.8468736784926819,
-                    "timestamp": 0.8278584989800474
-                },
-                {
-                    "x": 7.501750789212216,
-                    "y": 0.8458382793290063,
-                    "heading": 3.012370231908866,
-                    "angularVelocity": 1.136294701393235,
-                    "velocityX": 1.9713637817240808,
-                    "velocityY": -0.6516539816393957,
-                    "timestamp": 0.903118362523688
-                },
-                {
-                    "x": 7.605609860307225,
-                    "y": 0.8115063107648604,
-                    "heading": 3.0771995592704795,
-                    "angularVelocity": 0.8614063899281612,
-                    "velocityX": 1.3800061042468479,
-                    "velocityY": -0.45617899033577186,
-                    "timestamp": 0.9783782260673287
-                },
-                {
-                    "x": 7.665015516622623,
-                    "y": 0.7918924721730815,
-                    "heading": 3.1200072835634023,
-                    "angularVelocity": 0.5687988560874772,
-                    "velocityX": 0.7893404733713316,
-                    "velocityY": -0.2606148572194197,
-                    "timestamp": 1.0536380896109694
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
+        },
+        "C5 To C3": {
+            "waypoints": [
                 {
                     "x": 7.68,
                     "y": 0.787,
                     "heading": 3.14,
-                    "angularVelocity": 0.26564911886937803,
-                    "velocityX": 0.19910324935265278,
-                    "velocityY": -0.06500772048629082,
-                    "timestamp": 1.1288979531546102
-                },
-                {
-                    "x": 7.647079450231064,
-                    "y": 0.7979971975497204,
-                    "heading": 3.1353264592199808,
-                    "angularVelocity": -0.059354936296623806,
-                    "velocityX": -0.418097803433937,
-                    "velocityY": 0.13966668757779366,
-                    "timestamp": 1.2076368259698977
-                },
-                {
-                    "x": 7.565488841826375,
-                    "y": 0.8251307377838253,
-                    "heading": 3.107083972632397,
-                    "angularVelocity": -0.3586854317033129,
-                    "velocityX": -1.0362176328849728,
-                    "velocityY": 0.3446015832326818,
-                    "timestamp": 1.2863756987851853
-                },
-                {
-                    "x": 7.435127415733675,
-                    "y": 0.8684254377651182,
-                    "heading": 3.05838611435167,
-                    "angularVelocity": -0.6184728907024979,
-                    "velocityX": -1.6556171231776886,
-                    "velocityY": 0.5498516607274432,
-                    "timestamp": 1.365114571600473
-                },
-                {
-                    "x": 7.25585035840985,
-                    "y": 0.927906970601061,
-                    "heading": 2.99467735542164,
-                    "angularVelocity": -0.809114439312372,
-                    "velocityX": -2.2768557754742957,
-                    "velocityY": 0.75542779200662,
-                    "timestamp": 1.4438534444157605
-                },
-                {
-                    "x": 7.027485658601358,
-                    "y": 1.0035719757661283,
-                    "heading": 2.9276702363354157,
-                    "angularVelocity": -0.8510042967393713,
-                    "velocityX": -2.900279006332858,
-                    "velocityY": 0.9609612439152991,
-                    "timestamp": 1.522592317231048
-                },
-                {
-                    "x": 6.751324119312769,
-                    "y": 1.0947095363284396,
-                    "heading": 2.9052334045847372,
-                    "angularVelocity": -0.28495241230227925,
-                    "velocityX": -3.507308771569951,
-                    "velocityY": 1.1574659034821269,
-                    "timestamp": 1.6013311900463356
-                },
-                {
-                    "x": 6.468725179881112,
-                    "y": 1.1888428468535348,
-                    "heading": 2.9052333806250203,
-                    "angularVelocity": -3.0429336907534885e-7,
-                    "velocityX": -3.5890650872613086,
-                    "velocityY": 1.195512548749856,
-                    "timestamp": 1.6800700628616232
-                },
-                {
-                    "x": 6.186126242787232,
-                    "y": 1.2829761643969295,
-                    "heading": 2.9052333566653217,
-                    "angularVelocity": -3.0429313853702755e-7,
-                    "velocityX": -3.5890650575710485,
-                    "velocityY": 1.1955126378837118,
-                    "timestamp": 1.7588089356769108
-                },
-                {
-                    "x": 5.9036205043534045,
-                    "y": 1.377388777995483,
-                    "heading": 2.905233301444114,
-                    "angularVelocity": -7.013207784930713e-7,
-                    "velocityX": -3.5878814152769594,
-                    "velocityY": 1.1990597556563287,
-                    "timestamp": 1.8375478084921983
-                },
-                {
-                    "x": 5.660687795689049,
-                    "y": 1.4583340101651068,
-                    "heading": 2.781705854564737,
-                    "angularVelocity": -1.5688241711201327,
-                    "velocityX": -3.0852957373957746,
-                    "velocityY": 1.0280212209731776,
-                    "timestamp": 1.916286681307486
-                },
-                {
-                    "x": 5.4666551877353555,
-                    "y": 1.5230274173690972,
-                    "heading": 2.6729354985546605,
-                    "angularVelocity": -1.3814060593074367,
-                    "velocityX": -2.464254325927075,
-                    "velocityY": 0.8216196764176925,
-                    "timestamp": 1.9950255541227735
-                },
-                {
-                    "x": 5.321269342371624,
-                    "y": 1.571523398115741,
-                    "heading": 2.5878352744293056,
-                    "angularVelocity": -1.080790479754395,
-                    "velocityX": -1.8464303610846597,
-                    "velocityY": 0.6159090041891974,
-                    "timestamp": 2.073764426938061
-                },
-                {
-                    "x": 5.2244075328588835,
-                    "y": 1.6038438553412442,
-                    "heading": 2.5296373366742775,
-                    "angularVelocity": -0.7391258685091143,
-                    "velocityX": -1.2301650512570566,
-                    "velocityY": 0.4104765037889631,
-                    "timestamp": 2.1525032997533486
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": -0.3764003168270292,
-                    "velocityX": -0.6147856976876184,
-                    "velocityY": 0.20518638483251173,
-                    "timestamp": 2.231242172568636
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": 3.3983560967726525e-33,
-                    "velocityX": 0,
-                    "velocityY": -1.4165312907158555e-34,
-                    "timestamp": 2.309981045383924
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
                 }
             ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -26544,290 +5718,28 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
         },
-        "New Path": {
+        "C5 To C4": {
             "waypoints": [
                 {
-                    "x": 2.002868175506592,
-                    "y": 7.052158832550049,
-                    "heading": 0,
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 21
+                    "controlIntervalCount": 40
                 },
                 {
-                    "x": 7.3945817947387695,
-                    "y": 4.159608364105225,
-                    "heading": 0,
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.002868175506592,
-                    "y": 7.052158832550049,
-                    "heading": 0,
-                    "angularVelocity": 8.788933310419519e-34,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.0607038716342068,
-                    "y": 7.050473549314507,
-                    "heading": 1.1142678013967149e-17,
-                    "angularVelocity": 1.3376235580902032e-16,
-                    "velocityX": 0.6942889931584871,
-                    "velocityY": -0.020230993644643338,
-                    "timestamp": 0.0833020495752158
-                },
-                {
-                    "x": 2.1763752619443593,
-                    "y": 7.0471029829001015,
-                    "heading": 3.3885337626879046e-17,
-                    "angularVelocity": 2.730144097029411e-16,
-                    "velocityX": 1.3885779629672816,
-                    "velocityY": -0.040461986608896404,
-                    "timestamp": 0.1666040991504316
-                },
-                {
-                    "x": 2.349882343195254,
-                    "y": 7.0420471334012955,
-                    "heading": 6.768693556041358e-17,
-                    "angularVelocity": 4.057715055994475e-16,
-                    "velocityX": 2.0828668938599213,
-                    "velocityY": -0.06069297843916565,
-                    "timestamp": 0.24990614872564737
-                },
-                {
-                    "x": 2.5812251089033005,
-                    "y": 7.035306001007014,
-                    "heading": 1.1310438234374613e-16,
-                    "angularVelocity": 5.452140379341678e-16,
-                    "velocityX": 2.7771557469202537,
-                    "velocityY": -0.08092396800146733,
-                    "timestamp": 0.3332081983008632
-                },
-                {
-                    "x": 2.8704035396177314,
-                    "y": 7.026879586284038,
-                    "heading": 1.7011242610247892e-16,
-                    "angularVelocity": 6.843534341468776e-16,
-                    "velocityX": 3.4714443664837273,
-                    "velocityY": -0.10115495075986815,
-                    "timestamp": 0.416510247876079
-                },
-                {
-                    "x": 3.1853965350688225,
-                    "y": 7.01770095699296,
-                    "heading": 1.5081927746826734e-16,
-                    "angularVelocity": -2.3160472920646563e-16,
-                    "velocityX": 3.7813354780265604,
-                    "velocityY": -0.11018491547189069,
-                    "timestamp": 0.4998122974512948
-                },
-                {
-                    "x": 3.5003895305199366,
-                    "y": 7.00852232770188,
-                    "heading": 1.0198438674694206e-16,
-                    "angularVelocity": -5.862387656768365e-16,
-                    "velocityX": 3.7813354780268433,
-                    "velocityY": -0.11018491547189893,
-                    "timestamp": 0.5831143470265105
-                },
-                {
-                    "x": 3.815382525971051,
-                    "y": 6.999343698410801,
-                    "heading": 1.4640214038258707e-16,
-                    "angularVelocity": 5.332132145865036e-16,
-                    "velocityX": 3.7813354780268433,
-                    "velocityY": -0.11018491547189894,
-                    "timestamp": 0.6664163966017264
-                },
-                {
-                    "x": 4.130375521422166,
-                    "y": 6.990165069119721,
-                    "heading": 9.202484523489465e-17,
-                    "angularVelocity": -6.527725959322717e-16,
-                    "velocityX": 3.7813354780268433,
-                    "velocityY": -0.11018491547189893,
-                    "timestamp": 0.7497184461769422
-                },
-                {
-                    "x": 4.44536851687328,
-                    "y": 6.980986439828642,
-                    "heading": 8.683300340325528e-17,
-                    "angularVelocity": -6.232549928980416e-17,
-                    "velocityX": 3.7813354780268433,
-                    "velocityY": -0.11018491547189896,
-                    "timestamp": 0.833020495752158
-                },
-                {
-                    "x": 4.760361512324395,
-                    "y": 6.971807810537562,
-                    "heading": 4.9895892426490756e-17,
-                    "angularVelocity": -4.4341179076768094e-16,
-                    "velocityX": 3.7813354780268433,
-                    "velocityY": -0.1101849154718995,
-                    "timestamp": 0.9163225453273738
-                },
-                {
-                    "x": 5.075354507775272,
-                    "y": 6.962629181238361,
-                    "heading": 7.658568949420125e-17,
-                    "angularVelocity": 3.203978437962892e-16,
-                    "velocityX": 3.781335478024002,
-                    "velocityY": -0.11018491556939063,
-                    "timestamp": 0.9996245949025896
-                },
-                {
-                    "x": 5.390347460810252,
-                    "y": 6.953449096380609,
-                    "heading": 6.207074204602565e-17,
-                    "angularVelocity": -1.7424478175737502e-16,
-                    "velocityX": 3.7813349688420876,
-                    "velocityY": -0.11020238883121884,
-                    "timestamp": 1.0829266444778054
-                },
-                {
-                    "x": 5.699832756918085,
-                    "y": 6.89408845944864,
-                    "heading": 1.1352912563557224e-17,
-                    "angularVelocity": -6.088425140747107e-16,
-                    "velocityX": 3.715218265168718,
-                    "velocityY": -0.7125951550372694,
-                    "timestamp": 1.166228694053021
-                },
-                {
-                    "x": 5.993248235249296,
-                    "y": 6.779144176535828,
-                    "heading": -2.0188379939239748e-17,
-                    "angularVelocity": -3.786376525383924e-16,
-                    "velocityX": 3.5223080323645073,
-                    "velocityY": -1.3798493974512511,
-                    "timestamp": 1.2495307436282368
-                },
-                {
-                    "x": 6.260702133178711,
-                    "y": 6.612491130828857,
-                    "heading": -1.9922096402182107e-17,
-                    "angularVelocity": 3.1966024655343145e-18,
-                    "velocityX": 3.210652070306157,
-                    "velocityY": -2.000587579258764,
-                    "timestamp": 1.3328327932034525
-                },
-                {
-                    "x": 6.492446929695756,
-                    "y": 6.400713070229776,
-                    "heading": -6.399527557587997e-17,
-                    "angularVelocity": -5.310840097207522e-16,
-                    "velocityX": 2.792536369594054,
-                    "velocityY": -2.5519362047963385,
-                    "timestamp": 1.4158200011912707
-                },
-                {
-                    "x": 6.6817397771824,
-                    "y": 6.15026582896043,
-                    "heading": -8.43162270708746e-17,
-                    "angularVelocity": -2.4486848018766247e-16,
-                    "velocityX": 2.2809882640519046,
-                    "velocityY": -3.0179017627163183,
-                    "timestamp": 1.498807209179089
-                },
-                {
-                    "x": 6.822247473161292,
-                    "y": 5.869528955070651,
-                    "heading": -6.616259576550212e-17,
-                    "angularVelocity": 2.1875216368331533e-16,
-                    "velocityX": 1.6931247524259796,
-                    "velocityY": -3.382893348225265,
-                    "timestamp": 1.5817944171669072
-                },
-                {
-                    "x": 6.921893026221615,
-                    "y": 5.5718271423734045,
-                    "heading": -4.4342219960097724e-17,
-                    "angularVelocity": 2.629366179978876e-16,
-                    "velocityX": 1.2007338899140572,
-                    "velocityY": -3.5873217079546538,
-                    "timestamp": 1.6647816251547254
-                },
-                {
-                    "x": 7.021537931073347,
-                    "y": 5.2741251127111655,
-                    "heading": -6.875165326695044e-17,
-                    "angularVelocity": -2.94134890168089e-16,
-                    "velocityX": 1.2007260789681786,
-                    "velocityY": -3.5873243223936337,
-                    "timestamp": 1.7477688331425436
-                },
-                {
-                    "x": 7.1211828359216325,
-                    "y": 4.976423083047797,
-                    "heading": -9.319435110608268e-17,
-                    "angularVelocity": -2.945357294430311e-16,
-                    "velocityX": 1.2007260789266394,
-                    "velocityY": -3.587324322407249,
-                    "timestamp": 1.8307560411303618
-                },
-                {
-                    "x": 7.212315815029836,
-                    "y": 4.704151531470839,
-                    "heading": -5.872326423531157e-17,
-                    "angularVelocity": 4.1537832644440137e-16,
-                    "velocityX": 1.098156948738158,
-                    "velocityY": -3.2808857916623166,
-                    "timestamp": 1.91374324911818
-                },
-                {
-                    "x": 7.285222204628618,
-                    "y": 4.486334271350703,
-                    "heading": -3.7707698468715826e-17,
-                    "angularVelocity": 2.5323862035767555e-16,
-                    "velocityX": 0.8785256350530809,
-                    "velocityY": -2.624708860576564,
-                    "timestamp": 1.9967304571059983
-                },
-                {
-                    "x": 7.339901998909188,
-                    "y": 4.3229713200418995,
-                    "heading": -2.239413365263319e-17,
-                    "angularVelocity": 1.8452922091102423e-16,
-                    "velocityX": 0.6588942513717942,
-                    "velocityY": -1.96853172036809,
-                    "timestamp": 2.0797176650938165
-                },
-                {
-                    "x": 7.376355195935282,
-                    "y": 4.214062683329266,
-                    "heading": -7.106347444201965e-18,
-                    "angularVelocity": 1.8421858583516529e-16,
-                    "velocityX": 0.43926284435843027,
-                    "velocityY": -1.31235451045202,
-                    "timestamp": 2.1627048730816347
-                },
-                {
-                    "x": 7.3945817947387695,
-                    "y": 4.159608364105225,
-                    "heading": 0,
-                    "angularVelocity": 8.563184520521882e-17,
-                    "velocityX": 0.21963142567902694,
-                    "velocityY": -0.6561772656821501,
-                    "timestamp": 2.245692081069453
-                },
-                {
-                    "x": 7.3945817947387695,
-                    "y": 4.159608364105225,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": -1.513321847281751e-32,
-                    "velocityY": -2.5829360962249507e-33,
-                    "timestamp": 2.328679289057271
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -26845,13 +5757,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 4.849137783050537,
-                    "y": 4.090187072753906,
-                    "radius": 0.8600171478139426
-                }
-            ]
+            "circleObstacles": []
         }
     },
     "splitTrajectoriesAtStopPoints": false,
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj
deleted file mode 100644
index 2328dc93..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj	
+++ /dev/null
@@ -1,121 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": -2.1,
-            "angularVelocity": 1.1712645267224225e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.754119446225283,
-            "y": 6.720987363814453,
-            "heading": -2.1237071001932817,
-            "angularVelocity": -0.324046398407224,
-            "velocityX": 0.6030576296752822,
-            "velocityY": 0.013496027999351609,
-            "timestamp": 0.07315958550933178
-        },
-        {
-            "x": 0.8424134953068435,
-            "y": 6.722973003308925,
-            "heading": -2.1697540075127364,
-            "angularVelocity": -0.6294036112814133,
-            "velocityX": 1.2068691814868098,
-            "velocityY": 0.027141207548542687,
-            "timestamp": 0.14631917101866357
-        },
-        {
-            "x": 0.9749625172883624,
-            "y": 6.725970405382453,
-            "heading": -2.236002046597228,
-            "angularVelocity": -0.9055278077813215,
-            "velocityX": 1.8117792912401405,
-            "velocityY": 0.04097073613339593,
-            "timestamp": 0.21947875652799537
-        },
-        {
-            "x": 1.15189309679064,
-            "y": 6.729994604425149,
-            "heading": -2.318545180808417,
-            "angularVelocity": -1.128261370488901,
-            "velocityX": 2.4184196543829377,
-            "velocityY": 0.05500576602073325,
-            "timestamp": 0.29263834203732714
-        },
-        {
-            "x": 1.3734035805681073,
-            "y": 6.735067453562316,
-            "heading": -2.4074941689331384,
-            "angularVelocity": -1.2158213787772831,
-            "velocityX": 3.0277711694964378,
-            "velocityY": 0.06933950078936708,
-            "timestamp": 0.3657979275466589
-        },
-        {
-            "x": 1.6366126990046324,
-            "y": 6.741789664093089,
-            "heading": -2.440634607899988,
-            "angularVelocity": -0.45298833688286777,
-            "velocityX": 3.5977393338693906,
-            "velocityY": 0.09188420743425116,
-            "timestamp": 0.4389575130559907
-        },
-        {
-            "x": 1.8581204107797031,
-            "y": 6.746837438064149,
-            "heading": -2.53012067379602,
-            "angularVelocity": -1.2231625599444977,
-            "velocityX": 3.0277332796918652,
-            "velocityY": 0.06899675464152406,
-            "timestamp": 0.5121170985653225
-        },
-        {
-            "x": 2.035048200585451,
-            "y": 6.750889717758592,
-            "heading": -2.613093680081622,
-            "angularVelocity": -1.1341371839104541,
-            "velocityX": 2.4183815227216985,
-            "velocityY": 0.05538959339687604,
-            "timestamp": 0.5852766840746543
-        },
-        {
-            "x": 2.1675926710077147,
-            "y": 6.75393921231849,
-            "heading": -2.679769909140043,
-            "angularVelocity": -0.9113806289937514,
-            "velocityX": 1.8117170771198088,
-            "velocityY": 0.04168277524633505,
-            "timestamp": 0.658436269583986
-        },
-        {
-            "x": 2.255882530144482,
-            "y": 6.755978072079856,
-            "heading": -2.726139004333996,
-            "angularVelocity": -0.6338075164194407,
-            "velocityX": 1.206811910183738,
-            "velocityY": 0.027868662010218256,
-            "timestamp": 0.7315958550933178
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": -0.32614995697255006,
-            "velocityX": 0.6030306151733326,
-            "velocityY": 0.013968476079118136,
-            "timestamp": 0.8047554406026496
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 2.3711039345986376e-33,
-            "velocityX": -1.8060489353873473e-34,
-            "velocityY": 7.872914025869492e-32,
-            "timestamp": 0.8779150261119814
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.traj
deleted file mode 100644
index 2328dc93..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.traj	
+++ /dev/null
@@ -1,121 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": -2.1,
-            "angularVelocity": 1.1712645267224225e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.754119446225283,
-            "y": 6.720987363814453,
-            "heading": -2.1237071001932817,
-            "angularVelocity": -0.324046398407224,
-            "velocityX": 0.6030576296752822,
-            "velocityY": 0.013496027999351609,
-            "timestamp": 0.07315958550933178
-        },
-        {
-            "x": 0.8424134953068435,
-            "y": 6.722973003308925,
-            "heading": -2.1697540075127364,
-            "angularVelocity": -0.6294036112814133,
-            "velocityX": 1.2068691814868098,
-            "velocityY": 0.027141207548542687,
-            "timestamp": 0.14631917101866357
-        },
-        {
-            "x": 0.9749625172883624,
-            "y": 6.725970405382453,
-            "heading": -2.236002046597228,
-            "angularVelocity": -0.9055278077813215,
-            "velocityX": 1.8117792912401405,
-            "velocityY": 0.04097073613339593,
-            "timestamp": 0.21947875652799537
-        },
-        {
-            "x": 1.15189309679064,
-            "y": 6.729994604425149,
-            "heading": -2.318545180808417,
-            "angularVelocity": -1.128261370488901,
-            "velocityX": 2.4184196543829377,
-            "velocityY": 0.05500576602073325,
-            "timestamp": 0.29263834203732714
-        },
-        {
-            "x": 1.3734035805681073,
-            "y": 6.735067453562316,
-            "heading": -2.4074941689331384,
-            "angularVelocity": -1.2158213787772831,
-            "velocityX": 3.0277711694964378,
-            "velocityY": 0.06933950078936708,
-            "timestamp": 0.3657979275466589
-        },
-        {
-            "x": 1.6366126990046324,
-            "y": 6.741789664093089,
-            "heading": -2.440634607899988,
-            "angularVelocity": -0.45298833688286777,
-            "velocityX": 3.5977393338693906,
-            "velocityY": 0.09188420743425116,
-            "timestamp": 0.4389575130559907
-        },
-        {
-            "x": 1.8581204107797031,
-            "y": 6.746837438064149,
-            "heading": -2.53012067379602,
-            "angularVelocity": -1.2231625599444977,
-            "velocityX": 3.0277332796918652,
-            "velocityY": 0.06899675464152406,
-            "timestamp": 0.5121170985653225
-        },
-        {
-            "x": 2.035048200585451,
-            "y": 6.750889717758592,
-            "heading": -2.613093680081622,
-            "angularVelocity": -1.1341371839104541,
-            "velocityX": 2.4183815227216985,
-            "velocityY": 0.05538959339687604,
-            "timestamp": 0.5852766840746543
-        },
-        {
-            "x": 2.1675926710077147,
-            "y": 6.75393921231849,
-            "heading": -2.679769909140043,
-            "angularVelocity": -0.9113806289937514,
-            "velocityX": 1.8117170771198088,
-            "velocityY": 0.04168277524633505,
-            "timestamp": 0.658436269583986
-        },
-        {
-            "x": 2.255882530144482,
-            "y": 6.755978072079856,
-            "heading": -2.726139004333996,
-            "angularVelocity": -0.6338075164194407,
-            "velocityX": 1.206811910183738,
-            "velocityY": 0.027868662010218256,
-            "timestamp": 0.7315958550933178
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": -0.32614995697255006,
-            "velocityX": 0.6030306151733326,
-            "velocityY": 0.013968476079118136,
-            "timestamp": 0.8047554406026496
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 2.3711039345986376e-33,
-            "velocityX": -1.8060489353873473e-34,
-            "velocityY": 7.872914025869492e-32,
-            "timestamp": 0.8779150261119814
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.1.traj
deleted file mode 100644
index a21af086..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.1.traj	
+++ /dev/null
@@ -1,265 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.002868175506592,
-            "y": 7.052158832550049,
-            "heading": 0,
-            "angularVelocity": 8.788933310419519e-34,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.0607038716342068,
-            "y": 7.050473549314507,
-            "heading": 1.1142678013967149e-17,
-            "angularVelocity": 1.3376235580902032e-16,
-            "velocityX": 0.6942889931584871,
-            "velocityY": -0.020230993644643338,
-            "timestamp": 0.0833020495752158
-        },
-        {
-            "x": 2.1763752619443593,
-            "y": 7.0471029829001015,
-            "heading": 3.3885337626879046e-17,
-            "angularVelocity": 2.730144097029411e-16,
-            "velocityX": 1.3885779629672816,
-            "velocityY": -0.040461986608896404,
-            "timestamp": 0.1666040991504316
-        },
-        {
-            "x": 2.349882343195254,
-            "y": 7.0420471334012955,
-            "heading": 6.768693556041358e-17,
-            "angularVelocity": 4.057715055994475e-16,
-            "velocityX": 2.0828668938599213,
-            "velocityY": -0.06069297843916565,
-            "timestamp": 0.24990614872564737
-        },
-        {
-            "x": 2.5812251089033005,
-            "y": 7.035306001007014,
-            "heading": 1.1310438234374613e-16,
-            "angularVelocity": 5.452140379341678e-16,
-            "velocityX": 2.7771557469202537,
-            "velocityY": -0.08092396800146733,
-            "timestamp": 0.3332081983008632
-        },
-        {
-            "x": 2.8704035396177314,
-            "y": 7.026879586284038,
-            "heading": 1.7011242610247892e-16,
-            "angularVelocity": 6.843534341468776e-16,
-            "velocityX": 3.4714443664837273,
-            "velocityY": -0.10115495075986815,
-            "timestamp": 0.416510247876079
-        },
-        {
-            "x": 3.1853965350688225,
-            "y": 7.01770095699296,
-            "heading": 1.5081927746826734e-16,
-            "angularVelocity": -2.3160472920646563e-16,
-            "velocityX": 3.7813354780265604,
-            "velocityY": -0.11018491547189069,
-            "timestamp": 0.4998122974512948
-        },
-        {
-            "x": 3.5003895305199366,
-            "y": 7.00852232770188,
-            "heading": 1.0198438674694206e-16,
-            "angularVelocity": -5.862387656768365e-16,
-            "velocityX": 3.7813354780268433,
-            "velocityY": -0.11018491547189893,
-            "timestamp": 0.5831143470265105
-        },
-        {
-            "x": 3.815382525971051,
-            "y": 6.999343698410801,
-            "heading": 1.4640214038258707e-16,
-            "angularVelocity": 5.332132145865036e-16,
-            "velocityX": 3.7813354780268433,
-            "velocityY": -0.11018491547189894,
-            "timestamp": 0.6664163966017264
-        },
-        {
-            "x": 4.130375521422166,
-            "y": 6.990165069119721,
-            "heading": 9.202484523489465e-17,
-            "angularVelocity": -6.527725959322717e-16,
-            "velocityX": 3.7813354780268433,
-            "velocityY": -0.11018491547189893,
-            "timestamp": 0.7497184461769422
-        },
-        {
-            "x": 4.44536851687328,
-            "y": 6.980986439828642,
-            "heading": 8.683300340325528e-17,
-            "angularVelocity": -6.232549928980416e-17,
-            "velocityX": 3.7813354780268433,
-            "velocityY": -0.11018491547189896,
-            "timestamp": 0.833020495752158
-        },
-        {
-            "x": 4.760361512324395,
-            "y": 6.971807810537562,
-            "heading": 4.9895892426490756e-17,
-            "angularVelocity": -4.4341179076768094e-16,
-            "velocityX": 3.7813354780268433,
-            "velocityY": -0.1101849154718995,
-            "timestamp": 0.9163225453273738
-        },
-        {
-            "x": 5.075354507775272,
-            "y": 6.962629181238361,
-            "heading": 7.658568949420125e-17,
-            "angularVelocity": 3.203978437962892e-16,
-            "velocityX": 3.781335478024002,
-            "velocityY": -0.11018491556939063,
-            "timestamp": 0.9996245949025896
-        },
-        {
-            "x": 5.390347460810252,
-            "y": 6.953449096380609,
-            "heading": 6.207074204602565e-17,
-            "angularVelocity": -1.7424478175737502e-16,
-            "velocityX": 3.7813349688420876,
-            "velocityY": -0.11020238883121884,
-            "timestamp": 1.0829266444778054
-        },
-        {
-            "x": 5.699832756918085,
-            "y": 6.89408845944864,
-            "heading": 1.1352912563557224e-17,
-            "angularVelocity": -6.088425140747107e-16,
-            "velocityX": 3.715218265168718,
-            "velocityY": -0.7125951550372694,
-            "timestamp": 1.166228694053021
-        },
-        {
-            "x": 5.993248235249296,
-            "y": 6.779144176535828,
-            "heading": -2.0188379939239748e-17,
-            "angularVelocity": -3.786376525383924e-16,
-            "velocityX": 3.5223080323645073,
-            "velocityY": -1.3798493974512511,
-            "timestamp": 1.2495307436282368
-        },
-        {
-            "x": 6.260702133178711,
-            "y": 6.612491130828857,
-            "heading": -1.9922096402182107e-17,
-            "angularVelocity": 3.1966024655343145e-18,
-            "velocityX": 3.210652070306157,
-            "velocityY": -2.000587579258764,
-            "timestamp": 1.3328327932034525
-        },
-        {
-            "x": 6.492446929695756,
-            "y": 6.400713070229776,
-            "heading": -6.399527557587997e-17,
-            "angularVelocity": -5.310840097207522e-16,
-            "velocityX": 2.792536369594054,
-            "velocityY": -2.5519362047963385,
-            "timestamp": 1.4158200011912707
-        },
-        {
-            "x": 6.6817397771824,
-            "y": 6.15026582896043,
-            "heading": -8.43162270708746e-17,
-            "angularVelocity": -2.4486848018766247e-16,
-            "velocityX": 2.2809882640519046,
-            "velocityY": -3.0179017627163183,
-            "timestamp": 1.498807209179089
-        },
-        {
-            "x": 6.822247473161292,
-            "y": 5.869528955070651,
-            "heading": -6.616259576550212e-17,
-            "angularVelocity": 2.1875216368331533e-16,
-            "velocityX": 1.6931247524259796,
-            "velocityY": -3.382893348225265,
-            "timestamp": 1.5817944171669072
-        },
-        {
-            "x": 6.921893026221615,
-            "y": 5.5718271423734045,
-            "heading": -4.4342219960097724e-17,
-            "angularVelocity": 2.629366179978876e-16,
-            "velocityX": 1.2007338899140572,
-            "velocityY": -3.5873217079546538,
-            "timestamp": 1.6647816251547254
-        },
-        {
-            "x": 7.021537931073347,
-            "y": 5.2741251127111655,
-            "heading": -6.875165326695044e-17,
-            "angularVelocity": -2.94134890168089e-16,
-            "velocityX": 1.2007260789681786,
-            "velocityY": -3.5873243223936337,
-            "timestamp": 1.7477688331425436
-        },
-        {
-            "x": 7.1211828359216325,
-            "y": 4.976423083047797,
-            "heading": -9.319435110608268e-17,
-            "angularVelocity": -2.945357294430311e-16,
-            "velocityX": 1.2007260789266394,
-            "velocityY": -3.587324322407249,
-            "timestamp": 1.8307560411303618
-        },
-        {
-            "x": 7.212315815029836,
-            "y": 4.704151531470839,
-            "heading": -5.872326423531157e-17,
-            "angularVelocity": 4.1537832644440137e-16,
-            "velocityX": 1.098156948738158,
-            "velocityY": -3.2808857916623166,
-            "timestamp": 1.91374324911818
-        },
-        {
-            "x": 7.285222204628618,
-            "y": 4.486334271350703,
-            "heading": -3.7707698468715826e-17,
-            "angularVelocity": 2.5323862035767555e-16,
-            "velocityX": 0.8785256350530809,
-            "velocityY": -2.624708860576564,
-            "timestamp": 1.9967304571059983
-        },
-        {
-            "x": 7.339901998909188,
-            "y": 4.3229713200418995,
-            "heading": -2.239413365263319e-17,
-            "angularVelocity": 1.8452922091102423e-16,
-            "velocityX": 0.6588942513717942,
-            "velocityY": -1.96853172036809,
-            "timestamp": 2.0797176650938165
-        },
-        {
-            "x": 7.376355195935282,
-            "y": 4.214062683329266,
-            "heading": -7.106347444201965e-18,
-            "angularVelocity": 1.8421858583516529e-16,
-            "velocityX": 0.43926284435843027,
-            "velocityY": -1.31235451045202,
-            "timestamp": 2.1627048730816347
-        },
-        {
-            "x": 7.3945817947387695,
-            "y": 4.159608364105225,
-            "heading": 0,
-            "angularVelocity": 8.563184520521882e-17,
-            "velocityX": 0.21963142567902694,
-            "velocityY": -0.6561772656821501,
-            "timestamp": 2.245692081069453
-        },
-        {
-            "x": 7.3945817947387695,
-            "y": 4.159608364105225,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -1.513321847281751e-32,
-            "velocityY": -2.5829360962249507e-33,
-            "timestamp": 2.328679289057271
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.traj
deleted file mode 100644
index a21af086..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.traj	
+++ /dev/null
@@ -1,265 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.002868175506592,
-            "y": 7.052158832550049,
-            "heading": 0,
-            "angularVelocity": 8.788933310419519e-34,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.0607038716342068,
-            "y": 7.050473549314507,
-            "heading": 1.1142678013967149e-17,
-            "angularVelocity": 1.3376235580902032e-16,
-            "velocityX": 0.6942889931584871,
-            "velocityY": -0.020230993644643338,
-            "timestamp": 0.0833020495752158
-        },
-        {
-            "x": 2.1763752619443593,
-            "y": 7.0471029829001015,
-            "heading": 3.3885337626879046e-17,
-            "angularVelocity": 2.730144097029411e-16,
-            "velocityX": 1.3885779629672816,
-            "velocityY": -0.040461986608896404,
-            "timestamp": 0.1666040991504316
-        },
-        {
-            "x": 2.349882343195254,
-            "y": 7.0420471334012955,
-            "heading": 6.768693556041358e-17,
-            "angularVelocity": 4.057715055994475e-16,
-            "velocityX": 2.0828668938599213,
-            "velocityY": -0.06069297843916565,
-            "timestamp": 0.24990614872564737
-        },
-        {
-            "x": 2.5812251089033005,
-            "y": 7.035306001007014,
-            "heading": 1.1310438234374613e-16,
-            "angularVelocity": 5.452140379341678e-16,
-            "velocityX": 2.7771557469202537,
-            "velocityY": -0.08092396800146733,
-            "timestamp": 0.3332081983008632
-        },
-        {
-            "x": 2.8704035396177314,
-            "y": 7.026879586284038,
-            "heading": 1.7011242610247892e-16,
-            "angularVelocity": 6.843534341468776e-16,
-            "velocityX": 3.4714443664837273,
-            "velocityY": -0.10115495075986815,
-            "timestamp": 0.416510247876079
-        },
-        {
-            "x": 3.1853965350688225,
-            "y": 7.01770095699296,
-            "heading": 1.5081927746826734e-16,
-            "angularVelocity": -2.3160472920646563e-16,
-            "velocityX": 3.7813354780265604,
-            "velocityY": -0.11018491547189069,
-            "timestamp": 0.4998122974512948
-        },
-        {
-            "x": 3.5003895305199366,
-            "y": 7.00852232770188,
-            "heading": 1.0198438674694206e-16,
-            "angularVelocity": -5.862387656768365e-16,
-            "velocityX": 3.7813354780268433,
-            "velocityY": -0.11018491547189893,
-            "timestamp": 0.5831143470265105
-        },
-        {
-            "x": 3.815382525971051,
-            "y": 6.999343698410801,
-            "heading": 1.4640214038258707e-16,
-            "angularVelocity": 5.332132145865036e-16,
-            "velocityX": 3.7813354780268433,
-            "velocityY": -0.11018491547189894,
-            "timestamp": 0.6664163966017264
-        },
-        {
-            "x": 4.130375521422166,
-            "y": 6.990165069119721,
-            "heading": 9.202484523489465e-17,
-            "angularVelocity": -6.527725959322717e-16,
-            "velocityX": 3.7813354780268433,
-            "velocityY": -0.11018491547189893,
-            "timestamp": 0.7497184461769422
-        },
-        {
-            "x": 4.44536851687328,
-            "y": 6.980986439828642,
-            "heading": 8.683300340325528e-17,
-            "angularVelocity": -6.232549928980416e-17,
-            "velocityX": 3.7813354780268433,
-            "velocityY": -0.11018491547189896,
-            "timestamp": 0.833020495752158
-        },
-        {
-            "x": 4.760361512324395,
-            "y": 6.971807810537562,
-            "heading": 4.9895892426490756e-17,
-            "angularVelocity": -4.4341179076768094e-16,
-            "velocityX": 3.7813354780268433,
-            "velocityY": -0.1101849154718995,
-            "timestamp": 0.9163225453273738
-        },
-        {
-            "x": 5.075354507775272,
-            "y": 6.962629181238361,
-            "heading": 7.658568949420125e-17,
-            "angularVelocity": 3.203978437962892e-16,
-            "velocityX": 3.781335478024002,
-            "velocityY": -0.11018491556939063,
-            "timestamp": 0.9996245949025896
-        },
-        {
-            "x": 5.390347460810252,
-            "y": 6.953449096380609,
-            "heading": 6.207074204602565e-17,
-            "angularVelocity": -1.7424478175737502e-16,
-            "velocityX": 3.7813349688420876,
-            "velocityY": -0.11020238883121884,
-            "timestamp": 1.0829266444778054
-        },
-        {
-            "x": 5.699832756918085,
-            "y": 6.89408845944864,
-            "heading": 1.1352912563557224e-17,
-            "angularVelocity": -6.088425140747107e-16,
-            "velocityX": 3.715218265168718,
-            "velocityY": -0.7125951550372694,
-            "timestamp": 1.166228694053021
-        },
-        {
-            "x": 5.993248235249296,
-            "y": 6.779144176535828,
-            "heading": -2.0188379939239748e-17,
-            "angularVelocity": -3.786376525383924e-16,
-            "velocityX": 3.5223080323645073,
-            "velocityY": -1.3798493974512511,
-            "timestamp": 1.2495307436282368
-        },
-        {
-            "x": 6.260702133178711,
-            "y": 6.612491130828857,
-            "heading": -1.9922096402182107e-17,
-            "angularVelocity": 3.1966024655343145e-18,
-            "velocityX": 3.210652070306157,
-            "velocityY": -2.000587579258764,
-            "timestamp": 1.3328327932034525
-        },
-        {
-            "x": 6.492446929695756,
-            "y": 6.400713070229776,
-            "heading": -6.399527557587997e-17,
-            "angularVelocity": -5.310840097207522e-16,
-            "velocityX": 2.792536369594054,
-            "velocityY": -2.5519362047963385,
-            "timestamp": 1.4158200011912707
-        },
-        {
-            "x": 6.6817397771824,
-            "y": 6.15026582896043,
-            "heading": -8.43162270708746e-17,
-            "angularVelocity": -2.4486848018766247e-16,
-            "velocityX": 2.2809882640519046,
-            "velocityY": -3.0179017627163183,
-            "timestamp": 1.498807209179089
-        },
-        {
-            "x": 6.822247473161292,
-            "y": 5.869528955070651,
-            "heading": -6.616259576550212e-17,
-            "angularVelocity": 2.1875216368331533e-16,
-            "velocityX": 1.6931247524259796,
-            "velocityY": -3.382893348225265,
-            "timestamp": 1.5817944171669072
-        },
-        {
-            "x": 6.921893026221615,
-            "y": 5.5718271423734045,
-            "heading": -4.4342219960097724e-17,
-            "angularVelocity": 2.629366179978876e-16,
-            "velocityX": 1.2007338899140572,
-            "velocityY": -3.5873217079546538,
-            "timestamp": 1.6647816251547254
-        },
-        {
-            "x": 7.021537931073347,
-            "y": 5.2741251127111655,
-            "heading": -6.875165326695044e-17,
-            "angularVelocity": -2.94134890168089e-16,
-            "velocityX": 1.2007260789681786,
-            "velocityY": -3.5873243223936337,
-            "timestamp": 1.7477688331425436
-        },
-        {
-            "x": 7.1211828359216325,
-            "y": 4.976423083047797,
-            "heading": -9.319435110608268e-17,
-            "angularVelocity": -2.945357294430311e-16,
-            "velocityX": 1.2007260789266394,
-            "velocityY": -3.587324322407249,
-            "timestamp": 1.8307560411303618
-        },
-        {
-            "x": 7.212315815029836,
-            "y": 4.704151531470839,
-            "heading": -5.872326423531157e-17,
-            "angularVelocity": 4.1537832644440137e-16,
-            "velocityX": 1.098156948738158,
-            "velocityY": -3.2808857916623166,
-            "timestamp": 1.91374324911818
-        },
-        {
-            "x": 7.285222204628618,
-            "y": 4.486334271350703,
-            "heading": -3.7707698468715826e-17,
-            "angularVelocity": 2.5323862035767555e-16,
-            "velocityX": 0.8785256350530809,
-            "velocityY": -2.624708860576564,
-            "timestamp": 1.9967304571059983
-        },
-        {
-            "x": 7.339901998909188,
-            "y": 4.3229713200418995,
-            "heading": -2.239413365263319e-17,
-            "angularVelocity": 1.8452922091102423e-16,
-            "velocityX": 0.6588942513717942,
-            "velocityY": -1.96853172036809,
-            "timestamp": 2.0797176650938165
-        },
-        {
-            "x": 7.376355195935282,
-            "y": 4.214062683329266,
-            "heading": -7.106347444201965e-18,
-            "angularVelocity": 1.8421858583516529e-16,
-            "velocityX": 0.43926284435843027,
-            "velocityY": -1.31235451045202,
-            "timestamp": 2.1627048730816347
-        },
-        {
-            "x": 7.3945817947387695,
-            "y": 4.159608364105225,
-            "heading": 0,
-            "angularVelocity": 8.563184520521882e-17,
-            "velocityX": 0.21963142567902694,
-            "velocityY": -0.6561772656821501,
-            "timestamp": 2.245692081069453
-        },
-        {
-            "x": 7.3945817947387695,
-            "y": 4.159608364105225,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -1.513321847281751e-32,
-            "velocityY": -2.5829360962249507e-33,
-            "timestamp": 2.328679289057271
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S1.1.traj
deleted file mode 100644
index 7ba7fd1a..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S1.1.traj	
+++ /dev/null
@@ -1,247 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 2.29574628206475e-30,
-            "velocityX": 4.692945968016159e-32,
-            "velocityY": -1.566641075737276e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 5.235048481309196,
-            "y": 6.649731386951801,
-            "heading": -2.8906451335254117,
-            "angularVelocity": -0.12312313043682141,
-            "velocityX": 0.6829631445217772,
-            "velocityY": 0.2282160316336707,
-            "timestamp": 0.08645925008229886
-        },
-        {
-            "x": 5.3531499211746905,
-            "y": 6.689193367474774,
-            "heading": -2.911632072054118,
-            "angularVelocity": -0.24273791998680166,
-            "velocityX": 1.3659780735212093,
-            "velocityY": 0.45642288691397265,
-            "timestamp": 0.17291850016459773
-        },
-        {
-            "x": 5.530311551906127,
-            "y": 6.748384549002882,
-            "heading": -2.9424540818551694,
-            "angularVelocity": -0.3564917550356453,
-            "velocityX": 2.0490766524439916,
-            "velocityY": 0.6846136355771774,
-            "timestamp": 0.2593777502468966
-        },
-        {
-            "x": 5.7665469637988895,
-            "y": 6.827301853965155,
-            "heading": -2.9820943384526712,
-            "angularVelocity": -0.4584848533802071,
-            "velocityX": 2.732332418658699,
-            "velocityY": 0.9127687886162316,
-            "timestamp": 0.34583700032919545
-        },
-        {
-            "x": 6.061889894006546,
-            "y": 6.925933356393463,
-            "heading": -3.027468756644365,
-            "angularVelocity": -0.5248069830402681,
-            "velocityX": 3.415978393596438,
-            "velocityY": 1.1407860042093994,
-            "timestamp": 0.43229625041149433
-        },
-        {
-            "x": 6.3720848481132855,
-            "y": 7.029634881790706,
-            "heading": -3.0274687634280455,
-            "angularVelocity": -7.846101114866984e-8,
-            "velocityX": 3.587759017241877,
-            "velocityY": 1.1994266119419068,
-            "timestamp": 0.5187555004937932
-        },
-        {
-            "x": 6.682279802039258,
-            "y": 7.133336407728698,
-            "heading": -3.027468770211708,
-            "angularVelocity": -7.846080505503267e-8,
-            "velocityX": 3.5877590151510974,
-            "velocityY": 1.1994266181962872,
-            "timestamp": 0.605214750576092
-        },
-        {
-            "x": 6.992474757407683,
-            "y": 7.237037929350706,
-            "heading": -3.027468776995789,
-            "angularVelocity": -7.846565119739109e-8,
-            "velocityX": 3.5877590318346937,
-            "velocityY": 1.1994265682769871,
-            "timestamp": 0.6916740006583908
-        },
-        {
-            "x": 7.248034956342824,
-            "y": 7.322496159466712,
-            "heading": -3.07460707499528,
-            "angularVelocity": -0.5452082681093516,
-            "velocityX": 2.9558456578310177,
-            "velocityY": 0.9884220605040753,
-            "timestamp": 0.7781332507406896
-        },
-        {
-            "x": 7.444567623191234,
-            "y": 7.388221479482509,
-            "heading": -3.109745402875848,
-            "angularVelocity": -0.406414904676237,
-            "velocityX": 2.2731248149998473,
-            "velocityY": 0.7601884119189827,
-            "timestamp": 0.8645925008229884
-        },
-        {
-            "x": 7.58207269769946,
-            "y": 7.434213892330609,
-            "heading": -3.1328871823753137,
-            "angularVelocity": -0.2676611175500936,
-            "velocityX": 1.5904032752686705,
-            "velocityY": 0.5319547972517508,
-            "timestamp": 0.9510517509052873
-        },
-        {
-            "x": 7.660550155243088,
-            "y": 7.460473398860823,
-            "heading": -3.1440339918196103,
-            "angularVelocity": -0.12892558556646783,
-            "velocityX": 0.907681450727999,
-            "velocityY": 0.3037211924135578,
-            "timestamp": 1.037511000987586
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0.009816007412950826,
-            "velocityX": 0.22495967450928966,
-            "velocityY": 0.07548759829543861,
-            "timestamp": 1.123970251069885
-        },
-        {
-            "x": 7.635308945850804,
-            "y": 7.452085548446669,
-            "heading": -3.129057152990018,
-            "angularVelocity": 0.1555765979823413,
-            "velocityX": -0.49212955006536996,
-            "velocityY": -0.1642351578439422,
-            "timestamp": 1.2147818151045664
-        },
-        {
-            "x": 7.525497368411343,
-            "y": 7.41540168270092,
-            "heading": -3.1017205952496476,
-            "angularVelocity": 0.301025073527845,
-            "velocityX": -1.2092246026888915,
-            "velocityY": -0.4039558853066335,
-            "timestamp": 1.3055933791392478
-        },
-        {
-            "x": 7.350564387959683,
-            "y": 7.356948703524257,
-            "heading": -3.061223374709373,
-            "angularVelocity": 0.44594783682873773,
-            "velocityX": -1.9263293426462433,
-            "velocityY": -0.6436733008397082,
-            "timestamp": 1.3964049431739292
-        },
-        {
-            "x": 7.110508199108376,
-            "y": 7.276727219900285,
-            "heading": -3.007665771604216,
-            "angularVelocity": 0.5897663328854458,
-            "velocityX": -2.6434539631934633,
-            "velocityY": -0.8833840103589294,
-            "timestamp": 1.4872165072086105
-        },
-        {
-            "x": 6.80532324732103,
-            "y": 7.174739191350757,
-            "heading": -2.9413637265725296,
-            "angularVelocity": 0.7301057495614385,
-            "velocityX": -3.3606397492593936,
-            "velocityY": -1.1230731419900883,
-            "timestamp": 1.578028071243292
-        },
-        {
-            "x": 6.479515641287707,
-            "y": 7.065810200947661,
-            "heading": -2.9413637193977467,
-            "angularVelocity": 7.900737542269723e-8,
-            "velocityX": -3.587732570148064,
-            "velocityY": -1.1995057189140004,
-            "timestamp": 1.6688396352779733
-        },
-        {
-            "x": 6.153708035353397,
-            "y": 6.956881210248373,
-            "heading": -2.9413637122229894,
-            "angularVelocity": 7.900708560619817e-8,
-            "velocityX": -3.5877325690577564,
-            "velocityY": -1.1995057221756227,
-            "timestamp": 1.7596511993126547
-        },
-        {
-            "x": 5.827900259692805,
-            "y": 6.847953284193167,
-            "heading": -2.9413633662901284,
-            "angularVelocity": 0.0000038093481209652,
-            "velocityX": -3.587734438051908,
-            "velocityY": -1.1994939985136122,
-            "timestamp": 1.850462763347336
-        },
-        {
-            "x": 5.5671157361716075,
-            "y": 6.7607637759552865,
-            "heading": -2.922540068712598,
-            "angularVelocity": 0.2072786409826639,
-            "velocityX": -2.8717105171937094,
-            "velocityY": -0.9601145973503112,
-            "timestamp": 1.9412743273820174
-        },
-        {
-            "x": 5.371545145894187,
-            "y": 6.69537761850095,
-            "heading": -2.9031356081339226,
-            "angularVelocity": 0.2136782995204373,
-            "velocityX": -2.1535868515901284,
-            "velocityY": -0.7200201664777647,
-            "timestamp": 2.0320858914166986
-        },
-        {
-            "x": 5.241178014496252,
-            "y": 6.651791296591565,
-            "heading": -2.888169171521839,
-            "angularVelocity": 0.1648076076208368,
-            "velocityX": -1.4355785277318724,
-            "velocityY": -0.47996444475676003,
-            "timestamp": 2.1228974554513798
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0.08995739264147623,
-            "velocityX": -0.717728135057343,
-            "velocityY": -0.23996169235946166,
-            "timestamp": 2.213709019486061
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -2.956634078664977e-32,
-            "timestamp": 2.304520583520742
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S1.traj
deleted file mode 100644
index 7ba7fd1a..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S1.traj	
+++ /dev/null
@@ -1,247 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 2.29574628206475e-30,
-            "velocityX": 4.692945968016159e-32,
-            "velocityY": -1.566641075737276e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 5.235048481309196,
-            "y": 6.649731386951801,
-            "heading": -2.8906451335254117,
-            "angularVelocity": -0.12312313043682141,
-            "velocityX": 0.6829631445217772,
-            "velocityY": 0.2282160316336707,
-            "timestamp": 0.08645925008229886
-        },
-        {
-            "x": 5.3531499211746905,
-            "y": 6.689193367474774,
-            "heading": -2.911632072054118,
-            "angularVelocity": -0.24273791998680166,
-            "velocityX": 1.3659780735212093,
-            "velocityY": 0.45642288691397265,
-            "timestamp": 0.17291850016459773
-        },
-        {
-            "x": 5.530311551906127,
-            "y": 6.748384549002882,
-            "heading": -2.9424540818551694,
-            "angularVelocity": -0.3564917550356453,
-            "velocityX": 2.0490766524439916,
-            "velocityY": 0.6846136355771774,
-            "timestamp": 0.2593777502468966
-        },
-        {
-            "x": 5.7665469637988895,
-            "y": 6.827301853965155,
-            "heading": -2.9820943384526712,
-            "angularVelocity": -0.4584848533802071,
-            "velocityX": 2.732332418658699,
-            "velocityY": 0.9127687886162316,
-            "timestamp": 0.34583700032919545
-        },
-        {
-            "x": 6.061889894006546,
-            "y": 6.925933356393463,
-            "heading": -3.027468756644365,
-            "angularVelocity": -0.5248069830402681,
-            "velocityX": 3.415978393596438,
-            "velocityY": 1.1407860042093994,
-            "timestamp": 0.43229625041149433
-        },
-        {
-            "x": 6.3720848481132855,
-            "y": 7.029634881790706,
-            "heading": -3.0274687634280455,
-            "angularVelocity": -7.846101114866984e-8,
-            "velocityX": 3.587759017241877,
-            "velocityY": 1.1994266119419068,
-            "timestamp": 0.5187555004937932
-        },
-        {
-            "x": 6.682279802039258,
-            "y": 7.133336407728698,
-            "heading": -3.027468770211708,
-            "angularVelocity": -7.846080505503267e-8,
-            "velocityX": 3.5877590151510974,
-            "velocityY": 1.1994266181962872,
-            "timestamp": 0.605214750576092
-        },
-        {
-            "x": 6.992474757407683,
-            "y": 7.237037929350706,
-            "heading": -3.027468776995789,
-            "angularVelocity": -7.846565119739109e-8,
-            "velocityX": 3.5877590318346937,
-            "velocityY": 1.1994265682769871,
-            "timestamp": 0.6916740006583908
-        },
-        {
-            "x": 7.248034956342824,
-            "y": 7.322496159466712,
-            "heading": -3.07460707499528,
-            "angularVelocity": -0.5452082681093516,
-            "velocityX": 2.9558456578310177,
-            "velocityY": 0.9884220605040753,
-            "timestamp": 0.7781332507406896
-        },
-        {
-            "x": 7.444567623191234,
-            "y": 7.388221479482509,
-            "heading": -3.109745402875848,
-            "angularVelocity": -0.406414904676237,
-            "velocityX": 2.2731248149998473,
-            "velocityY": 0.7601884119189827,
-            "timestamp": 0.8645925008229884
-        },
-        {
-            "x": 7.58207269769946,
-            "y": 7.434213892330609,
-            "heading": -3.1328871823753137,
-            "angularVelocity": -0.2676611175500936,
-            "velocityX": 1.5904032752686705,
-            "velocityY": 0.5319547972517508,
-            "timestamp": 0.9510517509052873
-        },
-        {
-            "x": 7.660550155243088,
-            "y": 7.460473398860823,
-            "heading": -3.1440339918196103,
-            "angularVelocity": -0.12892558556646783,
-            "velocityX": 0.907681450727999,
-            "velocityY": 0.3037211924135578,
-            "timestamp": 1.037511000987586
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0.009816007412950826,
-            "velocityX": 0.22495967450928966,
-            "velocityY": 0.07548759829543861,
-            "timestamp": 1.123970251069885
-        },
-        {
-            "x": 7.635308945850804,
-            "y": 7.452085548446669,
-            "heading": -3.129057152990018,
-            "angularVelocity": 0.1555765979823413,
-            "velocityX": -0.49212955006536996,
-            "velocityY": -0.1642351578439422,
-            "timestamp": 1.2147818151045664
-        },
-        {
-            "x": 7.525497368411343,
-            "y": 7.41540168270092,
-            "heading": -3.1017205952496476,
-            "angularVelocity": 0.301025073527845,
-            "velocityX": -1.2092246026888915,
-            "velocityY": -0.4039558853066335,
-            "timestamp": 1.3055933791392478
-        },
-        {
-            "x": 7.350564387959683,
-            "y": 7.356948703524257,
-            "heading": -3.061223374709373,
-            "angularVelocity": 0.44594783682873773,
-            "velocityX": -1.9263293426462433,
-            "velocityY": -0.6436733008397082,
-            "timestamp": 1.3964049431739292
-        },
-        {
-            "x": 7.110508199108376,
-            "y": 7.276727219900285,
-            "heading": -3.007665771604216,
-            "angularVelocity": 0.5897663328854458,
-            "velocityX": -2.6434539631934633,
-            "velocityY": -0.8833840103589294,
-            "timestamp": 1.4872165072086105
-        },
-        {
-            "x": 6.80532324732103,
-            "y": 7.174739191350757,
-            "heading": -2.9413637265725296,
-            "angularVelocity": 0.7301057495614385,
-            "velocityX": -3.3606397492593936,
-            "velocityY": -1.1230731419900883,
-            "timestamp": 1.578028071243292
-        },
-        {
-            "x": 6.479515641287707,
-            "y": 7.065810200947661,
-            "heading": -2.9413637193977467,
-            "angularVelocity": 7.900737542269723e-8,
-            "velocityX": -3.587732570148064,
-            "velocityY": -1.1995057189140004,
-            "timestamp": 1.6688396352779733
-        },
-        {
-            "x": 6.153708035353397,
-            "y": 6.956881210248373,
-            "heading": -2.9413637122229894,
-            "angularVelocity": 7.900708560619817e-8,
-            "velocityX": -3.5877325690577564,
-            "velocityY": -1.1995057221756227,
-            "timestamp": 1.7596511993126547
-        },
-        {
-            "x": 5.827900259692805,
-            "y": 6.847953284193167,
-            "heading": -2.9413633662901284,
-            "angularVelocity": 0.0000038093481209652,
-            "velocityX": -3.587734438051908,
-            "velocityY": -1.1994939985136122,
-            "timestamp": 1.850462763347336
-        },
-        {
-            "x": 5.5671157361716075,
-            "y": 6.7607637759552865,
-            "heading": -2.922540068712598,
-            "angularVelocity": 0.2072786409826639,
-            "velocityX": -2.8717105171937094,
-            "velocityY": -0.9601145973503112,
-            "timestamp": 1.9412743273820174
-        },
-        {
-            "x": 5.371545145894187,
-            "y": 6.69537761850095,
-            "heading": -2.9031356081339226,
-            "angularVelocity": 0.2136782995204373,
-            "velocityX": -2.1535868515901284,
-            "velocityY": -0.7200201664777647,
-            "timestamp": 2.0320858914166986
-        },
-        {
-            "x": 5.241178014496252,
-            "y": 6.651791296591565,
-            "heading": -2.888169171521839,
-            "angularVelocity": 0.1648076076208368,
-            "velocityX": -1.4355785277318724,
-            "velocityY": -0.47996444475676003,
-            "timestamp": 2.1228974554513798
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0.08995739264147623,
-            "velocityX": -0.717728135057343,
-            "velocityY": -0.23996169235946166,
-            "timestamp": 2.213709019486061
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -2.956634078664977e-32,
-            "timestamp": 2.304520583520742
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S2.1.traj
deleted file mode 100644
index 98b1af2d..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S2.1.traj	
+++ /dev/null
@@ -1,265 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.230255854586943,
-            "y": 6.654457619561188,
-            "heading": -2.8852614761787456,
-            "angularVelocity": -0.06226832690913654,
-            "velocityX": 0.6421052144646586,
-            "velocityY": 0.2894501464070739,
-            "timestamp": 0.08449682912507997
-        },
-        {
-            "x": 5.338767562811348,
-            "y": 6.703372857257978,
-            "heading": -2.8957843391178932,
-            "angularVelocity": -0.12453559557330285,
-            "velocityX": 1.2842104176924247,
-            "velocityY": 0.5789002759426788,
-            "timestamp": 0.16899365825015994
-        },
-        {
-            "x": 5.501535123103558,
-            "y": 6.776745710953194,
-            "heading": -2.911568425934469,
-            "angularVelocity": -0.1868009365559807,
-            "velocityX": 1.9263156023436965,
-            "velocityY": 0.8683503801853032,
-            "timestamp": 0.25349048737523994
-        },
-        {
-            "x": 5.718558531629796,
-            "y": 6.87457617714364,
-            "heading": -2.9326134806355157,
-            "angularVelocity": -0.24906324792252543,
-            "velocityX": 2.568420741623094,
-            "velocityY": 1.1578004429684545,
-            "timestamp": 0.3379873165003199
-        },
-        {
-            "x": 5.9898377739629405,
-            "y": 6.996864247325266,
-            "heading": -2.9589191023342685,
-            "angularVelocity": -0.31132081488895513,
-            "velocityX": 3.210525710160934,
-            "velocityY": 1.4472504051081343,
-            "timestamp": 0.4224841456253998
-        },
-        {
-            "x": 6.281244778996208,
-            "y": 7.128225687635927,
-            "heading": -2.958919105469154,
-            "angularVelocity": -3.710063015796767e-8,
-            "velocityX": 3.448733024074779,
-            "velocityY": 1.5546315958934869,
-            "timestamp": 0.5069809747504798
-        },
-        {
-            "x": 6.572651810300593,
-            "y": 7.259587069667809,
-            "heading": -2.9589191086040265,
-            "angularVelocity": -3.7100470021746385e-8,
-            "velocityX": 3.448733334987257,
-            "velocityY": 1.5546309061778836,
-            "timestamp": 0.5914778038755597
-        },
-        {
-            "x": 6.868681501268645,
-            "y": 7.380170200918755,
-            "heading": -2.958919113683838,
-            "angularVelocity": -6.01183729074805e-8,
-            "velocityX": 3.5034414194388437,
-            "velocityY": 1.4270728558635946,
-            "timestamp": 0.6759746330006396
-        },
-        {
-            "x": 7.138377967809157,
-            "y": 7.448808221216772,
-            "heading": -3.0028485661720943,
-            "angularVelocity": -0.5198946865003425,
-            "velocityX": 3.1917939327792095,
-            "velocityY": 0.8123147461121022,
-            "timestamp": 0.7604714621257196
-        },
-        {
-            "x": 7.3546590217483025,
-            "y": 7.491172600361784,
-            "heading": -3.044173791439457,
-            "angularVelocity": -0.4890742729077964,
-            "velocityX": 2.5596351505567982,
-            "velocityY": 0.5013724134227592,
-            "timestamp": 0.8449682912507995
-        },
-        {
-            "x": 7.517104833144657,
-            "y": 7.508147678254589,
-            "heading": -3.0815979795537904,
-            "angularVelocity": -0.4429064202981498,
-            "velocityX": 1.9225077802137127,
-            "velocityY": 0.200896034426051,
-            "timestamp": 0.9294651203758795
-        },
-        {
-            "x": 7.625575199280341,
-            "y": 7.500038616852461,
-            "heading": -3.1146766365119762,
-            "angularVelocity": -0.39147808622758395,
-            "velocityX": 1.2837211438445335,
-            "velocityY": -0.09596882493808385,
-            "timestamp": 1.0139619495009595
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.3373933786960057,
-            "velocityX": 0.6441046520111882,
-            "velocityY": -0.39100422104069327,
-            "timestamp": 1.0984587786260396
-        },
-        {
-            "x": 7.666478172550486,
-            "y": 7.390434189445638,
-            "heading": -3.1708333738054395,
-            "angularVelocity": -0.26987816293521383,
-            "velocityX": -0.13198919117872188,
-            "velocityY": -0.7473737883984474,
-            "timestamp": 1.200905254195427
-        },
-        {
-            "x": 7.573314043125627,
-            "y": 7.277653381548422,
-            "heading": -3.1915604539305784,
-            "angularVelocity": -0.2023210658047483,
-            "velocityX": -0.909393211499577,
-            "velocityY": -1.1008754305152135,
-            "timestamp": 1.3033517297648145
-        },
-        {
-            "x": 7.400248960624901,
-            "y": 7.129232012070322,
-            "heading": -3.205357963605193,
-            "angularVelocity": -0.13468017906843224,
-            "velocityX": -1.6893219755862467,
-            "velocityY": -1.4487698932851405,
-            "timestamp": 1.405798205334202
-        },
-        {
-            "x": 7.146578363694586,
-            "y": 6.946793949790035,
-            "heading": -3.2122031163802927,
-            "angularVelocity": -0.06681686936573578,
-            "velocityX": -2.476128100263464,
-            "velocityY": -1.780813456649585,
-            "timestamp": 1.5082446809035894
-        },
-        {
-            "x": 6.805429935311203,
-            "y": 6.762913771007078,
-            "heading": -3.2122031133395477,
-            "angularVelocity": 2.9681302827383053e-8,
-            "velocityX": -3.330016249825229,
-            "velocityY": -1.7948902366915849,
-            "timestamp": 1.610691156472977
-        },
-        {
-            "x": 6.4551130042311256,
-            "y": 6.597166308184442,
-            "heading": -3.2122031105786535,
-            "angularVelocity": 2.694963002504427e-8,
-            "velocityX": -3.4195117902597523,
-            "velocityY": -1.6178932647651112,
-            "timestamp": 1.7131376320423644
-        },
-        {
-            "x": 6.104796046388704,
-            "y": 6.431418901925591,
-            "heading": -3.212203107817759,
-            "angularVelocity": 2.6949630458066062e-8,
-            "velocityX": -3.419512051492193,
-            "velocityY": -1.617892712634998,
-            "timestamp": 1.8155841076117518
-        },
-        {
-            "x": 5.754479088546207,
-            "y": 6.265671495666902,
-            "heading": -3.2122031050568642,
-            "angularVelocity": 2.694963045994546e-8,
-            "velocityX": -3.4195120514929416,
-            "velocityY": -1.617892712633416,
-            "timestamp": 1.9180305831811393
-        },
-        {
-            "x": 5.404162130703709,
-            "y": 6.099924089408213,
-            "heading": -3.212203102295969,
-            "angularVelocity": 2.6949631214728657e-8,
-            "velocityX": -3.419512051492942,
-            "velocityY": -1.6178927126334155,
-            "timestamp": 2.0204770587505267
-        },
-        {
-            "x": 5.05384517285956,
-            "y": 5.9341766831530425,
-            "heading": -3.212203099535067,
-            "angularVelocity": 2.6949706103007404e-8,
-            "velocityX": -3.4195120515090505,
-            "velocityY": -1.6178927125990576,
-            "timestamp": 2.122923534319914
-        },
-        {
-            "x": 4.737507119027181,
-            "y": 5.784506016757487,
-            "heading": -3.184596232441734,
-            "angularVelocity": 0.2694760062744644,
-            "velocityX": -3.087837351887456,
-            "velocityY": -1.4609645238032911,
-            "timestamp": 2.2253700098893017
-        },
-        {
-            "x": 4.500253563523173,
-            "y": 5.67225301019093,
-            "heading": -3.163890824246715,
-            "angularVelocity": 0.20210952187413175,
-            "velocityX": -2.315878161599758,
-            "velocityY": -1.0957234589346887,
-            "timestamp": 2.327816485458689
-        },
-        {
-            "x": 4.3420845221283875,
-            "y": 5.597417670515286,
-            "heading": -3.150087142577917,
-            "angularVelocity": 0.1347404251056839,
-            "velocityX": -1.5439188172721183,
-            "velocityY": -0.7304823251333565,
-            "timestamp": 2.4302629610280766
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0.06737015949032035,
-            "velocityX": -0.771959422604275,
-            "velocityY": -0.3652411691795466,
-            "timestamp": 2.532709436597464
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": -1.9737281778417947e-31,
-            "velocityX": -1.0240473596982798e-33,
-            "velocityY": -4.498532055712875e-34,
-            "timestamp": 2.6351559121668515
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S2.traj
deleted file mode 100644
index 98b1af2d..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1 To S2.traj	
+++ /dev/null
@@ -1,265 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.230255854586943,
-            "y": 6.654457619561188,
-            "heading": -2.8852614761787456,
-            "angularVelocity": -0.06226832690913654,
-            "velocityX": 0.6421052144646586,
-            "velocityY": 0.2894501464070739,
-            "timestamp": 0.08449682912507997
-        },
-        {
-            "x": 5.338767562811348,
-            "y": 6.703372857257978,
-            "heading": -2.8957843391178932,
-            "angularVelocity": -0.12453559557330285,
-            "velocityX": 1.2842104176924247,
-            "velocityY": 0.5789002759426788,
-            "timestamp": 0.16899365825015994
-        },
-        {
-            "x": 5.501535123103558,
-            "y": 6.776745710953194,
-            "heading": -2.911568425934469,
-            "angularVelocity": -0.1868009365559807,
-            "velocityX": 1.9263156023436965,
-            "velocityY": 0.8683503801853032,
-            "timestamp": 0.25349048737523994
-        },
-        {
-            "x": 5.718558531629796,
-            "y": 6.87457617714364,
-            "heading": -2.9326134806355157,
-            "angularVelocity": -0.24906324792252543,
-            "velocityX": 2.568420741623094,
-            "velocityY": 1.1578004429684545,
-            "timestamp": 0.3379873165003199
-        },
-        {
-            "x": 5.9898377739629405,
-            "y": 6.996864247325266,
-            "heading": -2.9589191023342685,
-            "angularVelocity": -0.31132081488895513,
-            "velocityX": 3.210525710160934,
-            "velocityY": 1.4472504051081343,
-            "timestamp": 0.4224841456253998
-        },
-        {
-            "x": 6.281244778996208,
-            "y": 7.128225687635927,
-            "heading": -2.958919105469154,
-            "angularVelocity": -3.710063015796767e-8,
-            "velocityX": 3.448733024074779,
-            "velocityY": 1.5546315958934869,
-            "timestamp": 0.5069809747504798
-        },
-        {
-            "x": 6.572651810300593,
-            "y": 7.259587069667809,
-            "heading": -2.9589191086040265,
-            "angularVelocity": -3.7100470021746385e-8,
-            "velocityX": 3.448733334987257,
-            "velocityY": 1.5546309061778836,
-            "timestamp": 0.5914778038755597
-        },
-        {
-            "x": 6.868681501268645,
-            "y": 7.380170200918755,
-            "heading": -2.958919113683838,
-            "angularVelocity": -6.01183729074805e-8,
-            "velocityX": 3.5034414194388437,
-            "velocityY": 1.4270728558635946,
-            "timestamp": 0.6759746330006396
-        },
-        {
-            "x": 7.138377967809157,
-            "y": 7.448808221216772,
-            "heading": -3.0028485661720943,
-            "angularVelocity": -0.5198946865003425,
-            "velocityX": 3.1917939327792095,
-            "velocityY": 0.8123147461121022,
-            "timestamp": 0.7604714621257196
-        },
-        {
-            "x": 7.3546590217483025,
-            "y": 7.491172600361784,
-            "heading": -3.044173791439457,
-            "angularVelocity": -0.4890742729077964,
-            "velocityX": 2.5596351505567982,
-            "velocityY": 0.5013724134227592,
-            "timestamp": 0.8449682912507995
-        },
-        {
-            "x": 7.517104833144657,
-            "y": 7.508147678254589,
-            "heading": -3.0815979795537904,
-            "angularVelocity": -0.4429064202981498,
-            "velocityX": 1.9225077802137127,
-            "velocityY": 0.200896034426051,
-            "timestamp": 0.9294651203758795
-        },
-        {
-            "x": 7.625575199280341,
-            "y": 7.500038616852461,
-            "heading": -3.1146766365119762,
-            "angularVelocity": -0.39147808622758395,
-            "velocityX": 1.2837211438445335,
-            "velocityY": -0.09596882493808385,
-            "timestamp": 1.0139619495009595
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.3373933786960057,
-            "velocityX": 0.6441046520111882,
-            "velocityY": -0.39100422104069327,
-            "timestamp": 1.0984587786260396
-        },
-        {
-            "x": 7.666478172550486,
-            "y": 7.390434189445638,
-            "heading": -3.1708333738054395,
-            "angularVelocity": -0.26987816293521383,
-            "velocityX": -0.13198919117872188,
-            "velocityY": -0.7473737883984474,
-            "timestamp": 1.200905254195427
-        },
-        {
-            "x": 7.573314043125627,
-            "y": 7.277653381548422,
-            "heading": -3.1915604539305784,
-            "angularVelocity": -0.2023210658047483,
-            "velocityX": -0.909393211499577,
-            "velocityY": -1.1008754305152135,
-            "timestamp": 1.3033517297648145
-        },
-        {
-            "x": 7.400248960624901,
-            "y": 7.129232012070322,
-            "heading": -3.205357963605193,
-            "angularVelocity": -0.13468017906843224,
-            "velocityX": -1.6893219755862467,
-            "velocityY": -1.4487698932851405,
-            "timestamp": 1.405798205334202
-        },
-        {
-            "x": 7.146578363694586,
-            "y": 6.946793949790035,
-            "heading": -3.2122031163802927,
-            "angularVelocity": -0.06681686936573578,
-            "velocityX": -2.476128100263464,
-            "velocityY": -1.780813456649585,
-            "timestamp": 1.5082446809035894
-        },
-        {
-            "x": 6.805429935311203,
-            "y": 6.762913771007078,
-            "heading": -3.2122031133395477,
-            "angularVelocity": 2.9681302827383053e-8,
-            "velocityX": -3.330016249825229,
-            "velocityY": -1.7948902366915849,
-            "timestamp": 1.610691156472977
-        },
-        {
-            "x": 6.4551130042311256,
-            "y": 6.597166308184442,
-            "heading": -3.2122031105786535,
-            "angularVelocity": 2.694963002504427e-8,
-            "velocityX": -3.4195117902597523,
-            "velocityY": -1.6178932647651112,
-            "timestamp": 1.7131376320423644
-        },
-        {
-            "x": 6.104796046388704,
-            "y": 6.431418901925591,
-            "heading": -3.212203107817759,
-            "angularVelocity": 2.6949630458066062e-8,
-            "velocityX": -3.419512051492193,
-            "velocityY": -1.617892712634998,
-            "timestamp": 1.8155841076117518
-        },
-        {
-            "x": 5.754479088546207,
-            "y": 6.265671495666902,
-            "heading": -3.2122031050568642,
-            "angularVelocity": 2.694963045994546e-8,
-            "velocityX": -3.4195120514929416,
-            "velocityY": -1.617892712633416,
-            "timestamp": 1.9180305831811393
-        },
-        {
-            "x": 5.404162130703709,
-            "y": 6.099924089408213,
-            "heading": -3.212203102295969,
-            "angularVelocity": 2.6949631214728657e-8,
-            "velocityX": -3.419512051492942,
-            "velocityY": -1.6178927126334155,
-            "timestamp": 2.0204770587505267
-        },
-        {
-            "x": 5.05384517285956,
-            "y": 5.9341766831530425,
-            "heading": -3.212203099535067,
-            "angularVelocity": 2.6949706103007404e-8,
-            "velocityX": -3.4195120515090505,
-            "velocityY": -1.6178927125990576,
-            "timestamp": 2.122923534319914
-        },
-        {
-            "x": 4.737507119027181,
-            "y": 5.784506016757487,
-            "heading": -3.184596232441734,
-            "angularVelocity": 0.2694760062744644,
-            "velocityX": -3.087837351887456,
-            "velocityY": -1.4609645238032911,
-            "timestamp": 2.2253700098893017
-        },
-        {
-            "x": 4.500253563523173,
-            "y": 5.67225301019093,
-            "heading": -3.163890824246715,
-            "angularVelocity": 0.20210952187413175,
-            "velocityX": -2.315878161599758,
-            "velocityY": -1.0957234589346887,
-            "timestamp": 2.327816485458689
-        },
-        {
-            "x": 4.3420845221283875,
-            "y": 5.597417670515286,
-            "heading": -3.150087142577917,
-            "angularVelocity": 0.1347404251056839,
-            "velocityX": -1.5439188172721183,
-            "velocityY": -0.7304823251333565,
-            "timestamp": 2.4302629610280766
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0.06737015949032035,
-            "velocityX": -0.771959422604275,
-            "velocityY": -0.3652411691795466,
-            "timestamp": 2.532709436597464
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": -1.9737281778417947e-31,
-            "velocityX": -1.0240473596982798e-33,
-            "velocityY": -4.498532055712875e-34,
-            "timestamp": 2.6351559121668515
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S1.1.traj
deleted file mode 100644
index 50078b64..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S1.1.traj	
+++ /dev/null
@@ -1,247 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 1.4108683409597047e-30,
-            "velocityX": 3.332067657020877e-32,
-            "velocityY": 1.6252866310843352e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 5.234960531442942,
-            "y": 6.610392010976446,
-            "heading": -2.8906900859192084,
-            "angularVelocity": -0.12376379247862908,
-            "velocityX": 0.6826118174441478,
-            "velocityY": -0.22701025069196354,
-            "timestamp": 0.08637490581940685
-        },
-        {
-            "x": 5.352887916547719,
-            "y": 6.571175678219841,
-            "heading": -2.91160851046633,
-            "angularVelocity": -0.24218173494536643,
-            "velocityX": 1.3652968299769979,
-            "velocityY": -0.45402460801062294,
-            "timestamp": 0.1727498116388137
-        },
-        {
-            "x": 5.529792169405666,
-            "y": 6.5123505926970395,
-            "heading": -2.941981384820343,
-            "angularVelocity": -0.35164002861494675,
-            "velocityX": 2.0480977800178555,
-            "velocityY": -0.6810437008846502,
-            "timestamp": 0.2591247174582205
-        },
-        {
-            "x": 5.765691321896416,
-            "y": 6.43391666324999,
-            "heading": -2.9802445174117826,
-            "angularVelocity": -0.4429889934853137,
-            "velocityX": 2.7311074930027868,
-            "velocityY": -0.9080638491352222,
-            "timestamp": 0.3454996232776274
-        },
-        {
-            "x": 6.060622443246375,
-            "y": 6.335879897503487,
-            "heading": -3.0215937804128985,
-            "angularVelocity": -0.47871847278597873,
-            "velocityX": 3.4145463726079974,
-            "velocityY": -1.135014444490357,
-            "timestamp": 0.4318745290970343
-        },
-        {
-            "x": 6.370665856130874,
-            "y": 6.2327325092534815,
-            "heading": -3.021593787446907,
-            "angularVelocity": -8.143578577450647e-8,
-            "velocityX": 3.5895079704369173,
-            "velocityY": -1.1941823527503022,
-            "timestamp": 0.5182494349164412
-        },
-        {
-            "x": 6.680709268872309,
-            "y": 6.129585120573416,
-            "heading": -3.021593794480898,
-            "angularVelocity": -8.143559184955701e-8,
-            "velocityX": 3.589507968780596,
-            "velocityY": -1.1941823577293083,
-            "timestamp": 0.604624340735848
-        },
-        {
-            "x": 6.990752683844134,
-            "y": 6.026437738598777,
-            "heading": -3.0215938015153188,
-            "angularVelocity": -8.144055599607105e-8,
-            "velocityX": 3.5895079946028012,
-            "velocityY": -1.1941822800976392,
-            "timestamp": 0.6909992465552549
-        },
-        {
-            "x": 7.246471127975024,
-            "y": 5.941338278881442,
-            "heading": -3.0707797962131482,
-            "angularVelocity": -0.5694477375264861,
-            "velocityX": 2.96056408634992,
-            "velocityY": -0.9852336035564434,
-            "timestamp": 0.7773741523746618
-        },
-        {
-            "x": 7.443255050635665,
-            "y": 5.875844773804915,
-            "heading": -3.107535505632415,
-            "angularVelocity": -0.4255368972118188,
-            "velocityX": 2.2782533977168096,
-            "velocityY": -0.7582469058018522,
-            "timestamp": 0.8637490581940687
-        },
-        {
-            "x": 7.581104494826472,
-            "y": 5.829957223632905,
-            "heading": -3.1318559851891097,
-            "angularVelocity": -0.2815688112890415,
-            "velocityX": 1.5959432069182882,
-            "velocityY": -0.531260204994182,
-            "timestamp": 0.9501239640134755
-        },
-        {
-            "x": 7.660019481037545,
-            "y": 5.803675631519179,
-            "heading": -3.1437391060728066,
-            "angularVelocity": -0.13757607919844067,
-            "velocityX": 0.9136332533425802,
-            "velocityY": -0.3042734676739749,
-            "timestamp": 1.0364988698328823
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0.0064115716005977265,
-            "velocityX": 0.2313231924569808,
-            "velocityY": -0.07728670099033955,
-            "timestamp": 1.122873775652289
-        },
-        {
-            "x": 7.635819311328683,
-            "y": 5.811667633489592,
-            "heading": -3.128848165121335,
-            "angularVelocity": 0.15781563504855214,
-            "velocityX": -0.48631752487124336,
-            "velocityY": 0.1614535089628058,
-            "timestamp": 1.2137211879121816
-        },
-        {
-            "x": 7.526434828772679,
-            "y": 5.848024979672397,
-            "heading": -3.1012666400693027,
-            "angularVelocity": 0.30360275946215987,
-            "velocityX": -1.2040462114976394,
-            "velocityY": 0.4002023313416631,
-            "timestamp": 1.3045686001720742
-        },
-        {
-            "x": 7.351833825159198,
-            "y": 5.906073128317211,
-            "heading": -3.0612921137460094,
-            "angularVelocity": 0.44001832665334883,
-            "velocityX": -1.921914992073542,
-            "velocityY": 0.6389631493158797,
-            "timestamp": 1.3954160124319668
-        },
-        {
-            "x": 7.111993081438883,
-            "y": 5.98581338993347,
-            "heading": -3.010630767411968,
-            "angularVelocity": 0.5576531579045794,
-            "velocityX": -2.640039355596495,
-            "velocityY": 0.8777383926810057,
-            "timestamp": 1.4862634246918593
-        },
-        {
-            "x": 6.806861000464256,
-            "y": 6.08724274158431,
-            "heading": -2.954485863102405,
-            "angularVelocity": 0.6180132478540051,
-            "velocityX": -3.358731673078681,
-            "velocityY": 1.116480361164798,
-            "timestamp": 1.577110836951752
-        },
-        {
-            "x": 6.480775270198975,
-            "y": 6.195766521383185,
-            "heading": -2.9544858545944934,
-            "angularVelocity": 9.365056825417393e-8,
-            "velocityX": -3.5893783009738223,
-            "velocityY": 1.1945720532843969,
-            "timestamp": 1.6679582492116445
-        },
-        {
-            "x": 6.154689540144831,
-            "y": 6.30429030181652,
-            "heading": -2.954485846086605,
-            "angularVelocity": 9.365030974191779e-8,
-            "velocityX": -3.589378298649725,
-            "velocityY": 1.1945720602682006,
-            "timestamp": 1.758805661471537
-        },
-        {
-            "x": 5.828602321664721,
-            "y": 6.412809153986278,
-            "heading": -2.954485580357569,
-            "angularVelocity": 0.0000029250024503863607,
-            "velocityX": -3.5893946824518035,
-            "velocityY": 1.194517812563883,
-            "timestamp": 1.8496530737314296
-        },
-        {
-            "x": 5.567525835725181,
-            "y": 6.4996975117043,
-            "heading": -2.9315080017864195,
-            "angularVelocity": 0.25292496505229944,
-            "velocityX": -2.8737911124282145,
-            "velocityY": 0.9564208330939352,
-            "timestamp": 1.9405004859913222
-        },
-        {
-            "x": 5.371744680904255,
-            "y": 6.564854924968385,
-            "heading": -2.907956180338786,
-            "angularVelocity": 0.2592459252452843,
-            "velocityX": -2.155054832611796,
-            "velocityY": 0.7172181534207646,
-            "timestamp": 2.0313478982512145
-        },
-        {
-            "x": 5.241242968677622,
-            "y": 6.608286759589549,
-            "heading": -2.8898557615846676,
-            "angularVelocity": 0.19923978354186256,
-            "velocityX": -1.4364934452213165,
-            "velocityY": 0.478074537741632,
-            "timestamp": 2.122195310511107
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0.10848698206630299,
-            "velocityX": -0.7181599019131975,
-            "velocityY": 0.2390078029777863,
-            "timestamp": 2.2130427227709997
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -3.247565811207596e-32,
-            "timestamp": 2.3038901350308922
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S1.traj
deleted file mode 100644
index 50078b64..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S1.traj	
+++ /dev/null
@@ -1,247 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 1.4108683409597047e-30,
-            "velocityX": 3.332067657020877e-32,
-            "velocityY": 1.6252866310843352e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 5.234960531442942,
-            "y": 6.610392010976446,
-            "heading": -2.8906900859192084,
-            "angularVelocity": -0.12376379247862908,
-            "velocityX": 0.6826118174441478,
-            "velocityY": -0.22701025069196354,
-            "timestamp": 0.08637490581940685
-        },
-        {
-            "x": 5.352887916547719,
-            "y": 6.571175678219841,
-            "heading": -2.91160851046633,
-            "angularVelocity": -0.24218173494536643,
-            "velocityX": 1.3652968299769979,
-            "velocityY": -0.45402460801062294,
-            "timestamp": 0.1727498116388137
-        },
-        {
-            "x": 5.529792169405666,
-            "y": 6.5123505926970395,
-            "heading": -2.941981384820343,
-            "angularVelocity": -0.35164002861494675,
-            "velocityX": 2.0480977800178555,
-            "velocityY": -0.6810437008846502,
-            "timestamp": 0.2591247174582205
-        },
-        {
-            "x": 5.765691321896416,
-            "y": 6.43391666324999,
-            "heading": -2.9802445174117826,
-            "angularVelocity": -0.4429889934853137,
-            "velocityX": 2.7311074930027868,
-            "velocityY": -0.9080638491352222,
-            "timestamp": 0.3454996232776274
-        },
-        {
-            "x": 6.060622443246375,
-            "y": 6.335879897503487,
-            "heading": -3.0215937804128985,
-            "angularVelocity": -0.47871847278597873,
-            "velocityX": 3.4145463726079974,
-            "velocityY": -1.135014444490357,
-            "timestamp": 0.4318745290970343
-        },
-        {
-            "x": 6.370665856130874,
-            "y": 6.2327325092534815,
-            "heading": -3.021593787446907,
-            "angularVelocity": -8.143578577450647e-8,
-            "velocityX": 3.5895079704369173,
-            "velocityY": -1.1941823527503022,
-            "timestamp": 0.5182494349164412
-        },
-        {
-            "x": 6.680709268872309,
-            "y": 6.129585120573416,
-            "heading": -3.021593794480898,
-            "angularVelocity": -8.143559184955701e-8,
-            "velocityX": 3.589507968780596,
-            "velocityY": -1.1941823577293083,
-            "timestamp": 0.604624340735848
-        },
-        {
-            "x": 6.990752683844134,
-            "y": 6.026437738598777,
-            "heading": -3.0215938015153188,
-            "angularVelocity": -8.144055599607105e-8,
-            "velocityX": 3.5895079946028012,
-            "velocityY": -1.1941822800976392,
-            "timestamp": 0.6909992465552549
-        },
-        {
-            "x": 7.246471127975024,
-            "y": 5.941338278881442,
-            "heading": -3.0707797962131482,
-            "angularVelocity": -0.5694477375264861,
-            "velocityX": 2.96056408634992,
-            "velocityY": -0.9852336035564434,
-            "timestamp": 0.7773741523746618
-        },
-        {
-            "x": 7.443255050635665,
-            "y": 5.875844773804915,
-            "heading": -3.107535505632415,
-            "angularVelocity": -0.4255368972118188,
-            "velocityX": 2.2782533977168096,
-            "velocityY": -0.7582469058018522,
-            "timestamp": 0.8637490581940687
-        },
-        {
-            "x": 7.581104494826472,
-            "y": 5.829957223632905,
-            "heading": -3.1318559851891097,
-            "angularVelocity": -0.2815688112890415,
-            "velocityX": 1.5959432069182882,
-            "velocityY": -0.531260204994182,
-            "timestamp": 0.9501239640134755
-        },
-        {
-            "x": 7.660019481037545,
-            "y": 5.803675631519179,
-            "heading": -3.1437391060728066,
-            "angularVelocity": -0.13757607919844067,
-            "velocityX": 0.9136332533425802,
-            "velocityY": -0.3042734676739749,
-            "timestamp": 1.0364988698328823
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0.0064115716005977265,
-            "velocityX": 0.2313231924569808,
-            "velocityY": -0.07728670099033955,
-            "timestamp": 1.122873775652289
-        },
-        {
-            "x": 7.635819311328683,
-            "y": 5.811667633489592,
-            "heading": -3.128848165121335,
-            "angularVelocity": 0.15781563504855214,
-            "velocityX": -0.48631752487124336,
-            "velocityY": 0.1614535089628058,
-            "timestamp": 1.2137211879121816
-        },
-        {
-            "x": 7.526434828772679,
-            "y": 5.848024979672397,
-            "heading": -3.1012666400693027,
-            "angularVelocity": 0.30360275946215987,
-            "velocityX": -1.2040462114976394,
-            "velocityY": 0.4002023313416631,
-            "timestamp": 1.3045686001720742
-        },
-        {
-            "x": 7.351833825159198,
-            "y": 5.906073128317211,
-            "heading": -3.0612921137460094,
-            "angularVelocity": 0.44001832665334883,
-            "velocityX": -1.921914992073542,
-            "velocityY": 0.6389631493158797,
-            "timestamp": 1.3954160124319668
-        },
-        {
-            "x": 7.111993081438883,
-            "y": 5.98581338993347,
-            "heading": -3.010630767411968,
-            "angularVelocity": 0.5576531579045794,
-            "velocityX": -2.640039355596495,
-            "velocityY": 0.8777383926810057,
-            "timestamp": 1.4862634246918593
-        },
-        {
-            "x": 6.806861000464256,
-            "y": 6.08724274158431,
-            "heading": -2.954485863102405,
-            "angularVelocity": 0.6180132478540051,
-            "velocityX": -3.358731673078681,
-            "velocityY": 1.116480361164798,
-            "timestamp": 1.577110836951752
-        },
-        {
-            "x": 6.480775270198975,
-            "y": 6.195766521383185,
-            "heading": -2.9544858545944934,
-            "angularVelocity": 9.365056825417393e-8,
-            "velocityX": -3.5893783009738223,
-            "velocityY": 1.1945720532843969,
-            "timestamp": 1.6679582492116445
-        },
-        {
-            "x": 6.154689540144831,
-            "y": 6.30429030181652,
-            "heading": -2.954485846086605,
-            "angularVelocity": 9.365030974191779e-8,
-            "velocityX": -3.589378298649725,
-            "velocityY": 1.1945720602682006,
-            "timestamp": 1.758805661471537
-        },
-        {
-            "x": 5.828602321664721,
-            "y": 6.412809153986278,
-            "heading": -2.954485580357569,
-            "angularVelocity": 0.0000029250024503863607,
-            "velocityX": -3.5893946824518035,
-            "velocityY": 1.194517812563883,
-            "timestamp": 1.8496530737314296
-        },
-        {
-            "x": 5.567525835725181,
-            "y": 6.4996975117043,
-            "heading": -2.9315080017864195,
-            "angularVelocity": 0.25292496505229944,
-            "velocityX": -2.8737911124282145,
-            "velocityY": 0.9564208330939352,
-            "timestamp": 1.9405004859913222
-        },
-        {
-            "x": 5.371744680904255,
-            "y": 6.564854924968385,
-            "heading": -2.907956180338786,
-            "angularVelocity": 0.2592459252452843,
-            "velocityX": -2.155054832611796,
-            "velocityY": 0.7172181534207646,
-            "timestamp": 2.0313478982512145
-        },
-        {
-            "x": 5.241242968677622,
-            "y": 6.608286759589549,
-            "heading": -2.8898557615846676,
-            "angularVelocity": 0.19923978354186256,
-            "velocityX": -1.4364934452213165,
-            "velocityY": 0.478074537741632,
-            "timestamp": 2.122195310511107
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0.10848698206630299,
-            "velocityX": -0.7181599019131975,
-            "velocityY": 0.2390078029777863,
-            "timestamp": 2.2130427227709997
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -3.247565811207596e-32,
-            "timestamp": 2.3038901350308922
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S2.1.traj
deleted file mode 100644
index 526be462..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S2.1.traj	
+++ /dev/null
@@ -1,256 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.2343405973213555,
-            "y": 6.622412668725056,
-            "heading": -2.8862658883257666,
-            "angularVelocity": -0.07457855479400594,
-            "velocityX": 0.6943879634997097,
-            "velocityY": -0.0903067804291561,
-            "timestamp": 0.08401729348434916
-        },
-        {
-            "x": 5.351021788709109,
-            "y": 6.607238006430742,
-            "heading": -2.898797916255787,
-            "angularVelocity": -0.14916010038283942,
-            "velocityX": 1.3887758882579295,
-            "velocityY": -0.18061355781642813,
-            "timestamp": 0.16803458696869833
-        },
-        {
-            "x": 5.52604356848206,
-            "y": 6.584476013331715,
-            "heading": -2.917596514461126,
-            "angularVelocity": -0.22374677195285786,
-            "velocityX": 2.083163745396705,
-            "velocityY": -0.27092033264873905,
-            "timestamp": 0.2520518804530475
-        },
-        {
-            "x": 5.759405926747038,
-            "y": 6.554126689512535,
-            "heading": -2.9426622755752048,
-            "angularVelocity": -0.2983404972304698,
-            "velocityX": 2.7775514847838854,
-            "velocityY": -0.3612271064746121,
-            "timestamp": 0.33606917393739666
-        },
-        {
-            "x": 6.051108840456095,
-            "y": 6.516190035692408,
-            "heading": -2.97399589297024,
-            "angularVelocity": -0.37294247524019475,
-            "velocityX": 3.4719389498472166,
-            "velocityY": -0.45153387174027243,
-            "timestamp": 0.4200864674217458
-        },
-        {
-            "x": 6.366286997596586,
-            "y": 6.47520006127383,
-            "heading": -2.973995896754836,
-            "angularVelocity": -4.504543806754251e-8,
-            "velocityX": 3.751348610143015,
-            "velocityY": -0.4878754446691907,
-            "timestamp": 0.504103760906095
-        },
-        {
-            "x": 6.681465049565767,
-            "y": 6.434209278184785,
-            "heading": -2.973995900539415,
-            "angularVelocity": -4.5045241068948784e-8,
-            "velocityX": 3.751347358361316,
-            "velocityY": -0.48788506971698453,
-            "timestamp": 0.5881210543904442
-        },
-        {
-            "x": 6.987163031152994,
-            "y": 6.347225437890881,
-            "heading": -2.9739959119301913,
-            "angularVelocity": -1.3557656915583158e-7,
-            "velocityX": 3.6385126074570944,
-            "velocityY": -1.0353087642618135,
-            "timestamp": 0.6721383478747933
-        },
-        {
-            "x": 7.243185278067537,
-            "y": 6.230387598842041,
-            "heading": -3.0107958850128846,
-            "angularVelocity": -0.4380047435061505,
-            "velocityX": 3.047256538467705,
-            "velocityY": -1.3906403575188384,
-            "timestamp": 0.7561556413591425
-        },
-        {
-            "x": 7.4403635227243035,
-            "y": 6.1148085109072055,
-            "heading": -3.048556145231695,
-            "angularVelocity": -0.44943438014751774,
-            "velocityX": 2.3468768925946972,
-            "velocityY": -1.3756583096355752,
-            "timestamp": 0.8401729348434916
-        },
-        {
-            "x": 7.578836794258849,
-            "y": 6.003419207644454,
-            "heading": -3.0839307394839546,
-            "angularVelocity": -0.42103944063431625,
-            "velocityX": 1.648152014803231,
-            "velocityY": -1.3257901872726057,
-            "timestamp": 0.9241902283278408
-        },
-        {
-            "x": 7.658696859900876,
-            "y": 5.8972967644784475,
-            "heading": -3.115687244057696,
-            "angularVelocity": -0.37797581017838017,
-            "velocityX": 0.9505193791669008,
-            "velocityY": -1.2631023776762744,
-            "timestamp": 1.00820752181219
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.32729051343474413,
-            "velocityX": 0.25355660978405975,
-            "velocityY": -1.1937633351296981,
-            "timestamp": 1.092224815296539
-        },
-        {
-            "x": 7.6231583877847156,
-            "y": 5.686873944343296,
-            "heading": -3.169227404734073,
-            "angularVelocity": -0.2617377186372048,
-            "velocityX": -0.5712901533281343,
-            "velocityY": -1.1068287610009868,
-            "timestamp": 1.1917217379796003
-        },
-        {
-            "x": 7.484327538641781,
-            "y": 5.5861293490761845,
-            "heading": -3.18873884204171,
-            "angularVelocity": -0.1961009122843807,
-            "velocityX": -1.3953280704486528,
-            "velocityY": -1.0125398107841346,
-            "timestamp": 1.2912186606626617
-        },
-        {
-            "x": 7.263675847711948,
-            "y": 5.496135627515027,
-            "heading": -3.2017054758509116,
-            "angularVelocity": -0.13032195830322837,
-            "velocityX": -2.2176735217500094,
-            "velocityY": -0.9044874869931846,
-            "timestamp": 1.390715583345723
-        },
-        {
-            "x": 6.9617360146088005,
-            "y": 5.420370006770522,
-            "heading": -3.2080997246142733,
-            "angularVelocity": -0.0642657942671267,
-            "velocityX": -3.034665042505381,
-            "velocityY": -0.7614870761968194,
-            "timestamp": 1.4902125060287843
-        },
-        {
-            "x": 6.587133153221649,
-            "y": 5.383725358359533,
-            "heading": -3.208099721013608,
-            "angularVelocity": 3.618871368220605e-8,
-            "velocityX": -3.7649693205127126,
-            "velocityY": -0.3682993144191623,
-            "timestamp": 1.5897094287118456
-        },
-        {
-            "x": 6.211820115708755,
-            "y": 5.412190570741109,
-            "heading": -3.2080997182657316,
-            "angularVelocity": 2.761770262617581e-8,
-            "velocityX": -3.7721069897651076,
-            "velocityY": 0.2860913846777879,
-            "timestamp": 1.689206351394907
-        },
-        {
-            "x": 5.836507126679018,
-            "y": 5.440656422364601,
-            "heading": -3.208099715517856,
-            "angularVelocity": 2.761769551512973e-8,
-            "velocityX": -3.7721065024821283,
-            "velocityY": 0.28609780941835966,
-            "timestamp": 1.7887032740779683
-        },
-        {
-            "x": 5.461194137649431,
-            "y": 5.46912227399009,
-            "heading": -3.20809971276998,
-            "angularVelocity": 2.7617694565057146e-8,
-            "velocityX": -3.772106502480606,
-            "velocityY": 0.28609780943843166,
-            "timestamp": 1.8882001967610296
-        },
-        {
-            "x": 5.08588114861977,
-            "y": 5.497588125614274,
-            "heading": -3.2080997100220925,
-            "angularVelocity": 2.7617811191359224e-8,
-            "velocityX": -3.7721065024813654,
-            "velocityY": 0.2860978094253166,
-            "timestamp": 1.9876971194440909
-        },
-        {
-            "x": 4.756728707190495,
-            "y": 5.522552875223879,
-            "heading": -3.182133930988408,
-            "angularVelocity": 0.26097067460464174,
-            "velocityX": -3.3081670523395013,
-            "velocityY": 0.2509097662158744,
-            "timestamp": 2.087194042127152
-        },
-        {
-            "x": 4.509864358241751,
-            "y": 5.541276437563115,
-            "heading": -3.1626596163193277,
-            "angularVelocity": 0.19572780890032443,
-            "velocityX": -2.4811254689263977,
-            "velocityY": 0.18818232598889303,
-            "timestamp": 2.1866909648102135
-        },
-        {
-            "x": 4.3452881204218,
-            "y": 5.553758812444608,
-            "heading": -3.1496767469198765,
-            "angularVelocity": 0.13048513511123086,
-            "velocityX": -1.654083698087773,
-            "velocityY": 0.12545488387870263,
-            "timestamp": 2.286187887493275
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0.06524261821612926,
-            "velocityX": -0.8270418642385756,
-            "velocityY": 0.06272744309160665,
-            "timestamp": 2.385684810176336
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": -3.6755949365770895e-31,
-            "velocityX": -3.301266761312006e-32,
-            "velocityY": -7.495504571617436e-31,
-            "timestamp": 2.4851817328593975
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S2.traj
deleted file mode 100644
index 526be462..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2 To S2.traj	
+++ /dev/null
@@ -1,256 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.2343405973213555,
-            "y": 6.622412668725056,
-            "heading": -2.8862658883257666,
-            "angularVelocity": -0.07457855479400594,
-            "velocityX": 0.6943879634997097,
-            "velocityY": -0.0903067804291561,
-            "timestamp": 0.08401729348434916
-        },
-        {
-            "x": 5.351021788709109,
-            "y": 6.607238006430742,
-            "heading": -2.898797916255787,
-            "angularVelocity": -0.14916010038283942,
-            "velocityX": 1.3887758882579295,
-            "velocityY": -0.18061355781642813,
-            "timestamp": 0.16803458696869833
-        },
-        {
-            "x": 5.52604356848206,
-            "y": 6.584476013331715,
-            "heading": -2.917596514461126,
-            "angularVelocity": -0.22374677195285786,
-            "velocityX": 2.083163745396705,
-            "velocityY": -0.27092033264873905,
-            "timestamp": 0.2520518804530475
-        },
-        {
-            "x": 5.759405926747038,
-            "y": 6.554126689512535,
-            "heading": -2.9426622755752048,
-            "angularVelocity": -0.2983404972304698,
-            "velocityX": 2.7775514847838854,
-            "velocityY": -0.3612271064746121,
-            "timestamp": 0.33606917393739666
-        },
-        {
-            "x": 6.051108840456095,
-            "y": 6.516190035692408,
-            "heading": -2.97399589297024,
-            "angularVelocity": -0.37294247524019475,
-            "velocityX": 3.4719389498472166,
-            "velocityY": -0.45153387174027243,
-            "timestamp": 0.4200864674217458
-        },
-        {
-            "x": 6.366286997596586,
-            "y": 6.47520006127383,
-            "heading": -2.973995896754836,
-            "angularVelocity": -4.504543806754251e-8,
-            "velocityX": 3.751348610143015,
-            "velocityY": -0.4878754446691907,
-            "timestamp": 0.504103760906095
-        },
-        {
-            "x": 6.681465049565767,
-            "y": 6.434209278184785,
-            "heading": -2.973995900539415,
-            "angularVelocity": -4.5045241068948784e-8,
-            "velocityX": 3.751347358361316,
-            "velocityY": -0.48788506971698453,
-            "timestamp": 0.5881210543904442
-        },
-        {
-            "x": 6.987163031152994,
-            "y": 6.347225437890881,
-            "heading": -2.9739959119301913,
-            "angularVelocity": -1.3557656915583158e-7,
-            "velocityX": 3.6385126074570944,
-            "velocityY": -1.0353087642618135,
-            "timestamp": 0.6721383478747933
-        },
-        {
-            "x": 7.243185278067537,
-            "y": 6.230387598842041,
-            "heading": -3.0107958850128846,
-            "angularVelocity": -0.4380047435061505,
-            "velocityX": 3.047256538467705,
-            "velocityY": -1.3906403575188384,
-            "timestamp": 0.7561556413591425
-        },
-        {
-            "x": 7.4403635227243035,
-            "y": 6.1148085109072055,
-            "heading": -3.048556145231695,
-            "angularVelocity": -0.44943438014751774,
-            "velocityX": 2.3468768925946972,
-            "velocityY": -1.3756583096355752,
-            "timestamp": 0.8401729348434916
-        },
-        {
-            "x": 7.578836794258849,
-            "y": 6.003419207644454,
-            "heading": -3.0839307394839546,
-            "angularVelocity": -0.42103944063431625,
-            "velocityX": 1.648152014803231,
-            "velocityY": -1.3257901872726057,
-            "timestamp": 0.9241902283278408
-        },
-        {
-            "x": 7.658696859900876,
-            "y": 5.8972967644784475,
-            "heading": -3.115687244057696,
-            "angularVelocity": -0.37797581017838017,
-            "velocityX": 0.9505193791669008,
-            "velocityY": -1.2631023776762744,
-            "timestamp": 1.00820752181219
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.32729051343474413,
-            "velocityX": 0.25355660978405975,
-            "velocityY": -1.1937633351296981,
-            "timestamp": 1.092224815296539
-        },
-        {
-            "x": 7.6231583877847156,
-            "y": 5.686873944343296,
-            "heading": -3.169227404734073,
-            "angularVelocity": -0.2617377186372048,
-            "velocityX": -0.5712901533281343,
-            "velocityY": -1.1068287610009868,
-            "timestamp": 1.1917217379796003
-        },
-        {
-            "x": 7.484327538641781,
-            "y": 5.5861293490761845,
-            "heading": -3.18873884204171,
-            "angularVelocity": -0.1961009122843807,
-            "velocityX": -1.3953280704486528,
-            "velocityY": -1.0125398107841346,
-            "timestamp": 1.2912186606626617
-        },
-        {
-            "x": 7.263675847711948,
-            "y": 5.496135627515027,
-            "heading": -3.2017054758509116,
-            "angularVelocity": -0.13032195830322837,
-            "velocityX": -2.2176735217500094,
-            "velocityY": -0.9044874869931846,
-            "timestamp": 1.390715583345723
-        },
-        {
-            "x": 6.9617360146088005,
-            "y": 5.420370006770522,
-            "heading": -3.2080997246142733,
-            "angularVelocity": -0.0642657942671267,
-            "velocityX": -3.034665042505381,
-            "velocityY": -0.7614870761968194,
-            "timestamp": 1.4902125060287843
-        },
-        {
-            "x": 6.587133153221649,
-            "y": 5.383725358359533,
-            "heading": -3.208099721013608,
-            "angularVelocity": 3.618871368220605e-8,
-            "velocityX": -3.7649693205127126,
-            "velocityY": -0.3682993144191623,
-            "timestamp": 1.5897094287118456
-        },
-        {
-            "x": 6.211820115708755,
-            "y": 5.412190570741109,
-            "heading": -3.2080997182657316,
-            "angularVelocity": 2.761770262617581e-8,
-            "velocityX": -3.7721069897651076,
-            "velocityY": 0.2860913846777879,
-            "timestamp": 1.689206351394907
-        },
-        {
-            "x": 5.836507126679018,
-            "y": 5.440656422364601,
-            "heading": -3.208099715517856,
-            "angularVelocity": 2.761769551512973e-8,
-            "velocityX": -3.7721065024821283,
-            "velocityY": 0.28609780941835966,
-            "timestamp": 1.7887032740779683
-        },
-        {
-            "x": 5.461194137649431,
-            "y": 5.46912227399009,
-            "heading": -3.20809971276998,
-            "angularVelocity": 2.7617694565057146e-8,
-            "velocityX": -3.772106502480606,
-            "velocityY": 0.28609780943843166,
-            "timestamp": 1.8882001967610296
-        },
-        {
-            "x": 5.08588114861977,
-            "y": 5.497588125614274,
-            "heading": -3.2080997100220925,
-            "angularVelocity": 2.7617811191359224e-8,
-            "velocityX": -3.7721065024813654,
-            "velocityY": 0.2860978094253166,
-            "timestamp": 1.9876971194440909
-        },
-        {
-            "x": 4.756728707190495,
-            "y": 5.522552875223879,
-            "heading": -3.182133930988408,
-            "angularVelocity": 0.26097067460464174,
-            "velocityX": -3.3081670523395013,
-            "velocityY": 0.2509097662158744,
-            "timestamp": 2.087194042127152
-        },
-        {
-            "x": 4.509864358241751,
-            "y": 5.541276437563115,
-            "heading": -3.1626596163193277,
-            "angularVelocity": 0.19572780890032443,
-            "velocityX": -2.4811254689263977,
-            "velocityY": 0.18818232598889303,
-            "timestamp": 2.1866909648102135
-        },
-        {
-            "x": 4.3452881204218,
-            "y": 5.553758812444608,
-            "heading": -3.1496767469198765,
-            "angularVelocity": 0.13048513511123086,
-            "velocityX": -1.654083698087773,
-            "velocityY": 0.12545488387870263,
-            "timestamp": 2.286187887493275
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0.06524261821612926,
-            "velocityX": -0.8270418642385756,
-            "velocityY": 0.06272744309160665,
-            "timestamp": 2.385684810176336
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": -3.6755949365770895e-31,
-            "velocityX": -3.301266761312006e-32,
-            "velocityY": -7.495504571617436e-31,
-            "timestamp": 2.4851817328593975
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S1.1.traj
deleted file mode 100644
index 41e8c0c3..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S1.1.traj	
+++ /dev/null
@@ -1,301 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.218507748835192,
-            "y": 6.587508953337151,
-            "heading": -2.890727584630965,
-            "angularVelocity": -0.12627463531080194,
-            "velocityX": 0.5003596491390202,
-            "velocityY": -0.5001630474999694,
-            "timestamp": 0.08495439012388621
-        },
-        {
-            "x": 5.303524107208959,
-            "y": 6.502525872657889,
-            "heading": -2.9120884337900024,
-            "angularVelocity": -0.25143902661031875,
-            "velocityX": 1.0007294296365374,
-            "velocityY": -1.0003377171605015,
-            "timestamp": 0.16990878024777242
-        },
-        {
-            "x": 5.43105048877714,
-            "y": 6.375049128446974,
-            "heading": -2.9439256135386342,
-            "angularVelocity": -0.37475614505901766,
-            "velocityX": 1.5011158503081115,
-            "velocityY": -1.5005315678808802,
-            "timestamp": 0.2548631703716586
-        },
-        {
-            "x": 5.601089619523365,
-            "y": 6.205075520007896,
-            "heading": -2.985927322319944,
-            "angularVelocity": -0.49440304050481265,
-            "velocityX": 2.001534358593136,
-            "velocityY": -2.0007630940704417,
-            "timestamp": 0.33981756049554485
-        },
-        {
-            "x": 5.813649033766866,
-            "y": 5.992595894100207,
-            "heading": -3.037161099536011,
-            "angularVelocity": -0.6030739216881468,
-            "velocityX": 2.502041553515969,
-            "velocityY": -2.501102363255205,
-            "timestamp": 0.42477195061943107
-        },
-        {
-            "x": 6.040945217563345,
-            "y": 5.7653958045642115,
-            "heading": -3.03716110602298,
-            "angularVelocity": -7.635824927871784e-8,
-            "velocityX": 2.6755083929744035,
-            "velocityY": -2.6743772653146864,
-            "timestamp": 0.5097263407433172
-        },
-        {
-            "x": 6.268241401429681,
-            "y": 5.53819571509808,
-            "heading": -3.037161112509923,
-            "angularVelocity": -7.635794871774561e-8,
-            "velocityX": 2.675508393796696,
-            "velocityY": -2.674377264492316,
-            "timestamp": 0.5946807308672034
-        },
-        {
-            "x": 6.495537585296019,
-            "y": 5.310995625631949,
-            "heading": -3.037161118996866,
-            "angularVelocity": -7.635794841586156e-8,
-            "velocityX": 2.675508393796701,
-            "velocityY": -2.6743772644923114,
-            "timestamp": 0.6796351209910896
-        },
-        {
-            "x": 6.722833769162356,
-            "y": 5.083795536165818,
-            "heading": -3.037161125483809,
-            "angularVelocity": -7.635794938585852e-8,
-            "velocityX": 2.6755083937967,
-            "velocityY": -2.6743772644923127,
-            "timestamp": 0.7645895111149758
-        },
-        {
-            "x": 6.950129953013132,
-            "y": 4.856595446684119,
-            "heading": -3.037161131970752,
-            "angularVelocity": -7.635794903516443e-8,
-            "velocityX": 2.67550839361353,
-            "velocityY": -2.6743772646755604,
-            "timestamp": 0.8495439012388619
-        },
-        {
-            "x": 7.177423048635347,
-            "y": 4.629392269264043,
-            "heading": -3.0371611395742963,
-            "angularVelocity": -8.950149400601411e-8,
-            "velocityX": 2.6754720420070424,
-            "velocityY": -2.674413612866287,
-            "timestamp": 0.9344982913627481
-        },
-        {
-            "x": 7.362966742660802,
-            "y": 4.443922816476664,
-            "heading": -3.078668671836781,
-            "angularVelocity": -0.48858607779956104,
-            "velocityX": 2.1840389149365182,
-            "velocityY": -2.183165019685597,
-            "timestamp": 1.0194526814866343
-        },
-        {
-            "x": 7.505991293187966,
-            "y": 4.3009534921360935,
-            "heading": -3.1105416993817094,
-            "angularVelocity": -0.37517811025716574,
-            "velocityX": 1.6835451389668437,
-            "velocityY": -1.6828950703063337,
-            "timestamp": 1.1044070716105205
-        },
-        {
-            "x": 7.6065035905836105,
-            "y": 4.200477915848541,
-            "heading": -3.1320612562280816,
-            "angularVelocity": -0.25330717829595906,
-            "velocityX": 1.1831324696564203,
-            "velocityY": -1.1827002246831095,
-            "timestamp": 1.1893614617344068
-        },
-        {
-            "x": 7.664506074481624,
-            "y": 4.142493806219386,
-            "heading": -3.1429833930528956,
-            "angularVelocity": -0.12856471347350074,
-            "velocityX": 0.6827485173302748,
-            "velocityY": -0.6825322334062814,
-            "timestamp": 1.274315851858293
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.002376735636840867,
-            "velocityX": 0.1823793390257534,
-            "velocityY": -0.18237793475472894,
-            "timestamp": 1.3592702419821794
-        },
-        {
-            "x": 7.6496885572239846,
-            "y": 4.157291685194411,
-            "heading": -3.131583473376336,
-            "angularVelocity": 0.13054330158990457,
-            "velocityX": -0.3410629632388108,
-            "velocityY": 0.3408406518372109,
-            "timestamp": 1.44814369364031
-        },
-        {
-            "x": 7.572857027601893,
-            "y": 4.234083644862719,
-            "heading": -3.108168408493633,
-            "angularVelocity": 0.26346523563343455,
-            "velocityX": -0.8645048458050324,
-            "velocityY": 0.8640596065007748,
-            "timestamp": 1.5370171452984407
-        },
-        {
-            "x": 7.449505471220409,
-            "y": 4.357375922418621,
-            "heading": -3.0729392389653802,
-            "angularVelocity": 0.39639699900261377,
-            "velocityX": -1.387946052278649,
-            "velocityY": 1.3872790496532752,
-            "timestamp": 1.6258905969565713
-        },
-        {
-            "x": 7.279633974763547,
-            "y": 4.52716856280588,
-            "heading": -3.0258936977546345,
-            "angularVelocity": 0.5293542709674363,
-            "velocityX": -1.911386283387714,
-            "velocityY": 1.9104989985109575,
-            "timestamp": 1.714764048614702
-        },
-        {
-            "x": 7.063242657628,
-            "y": 4.743461594393602,
-            "heading": -2.9670273926808983,
-            "angularVelocity": 0.6623609635456799,
-            "velocityX": -2.434825171050669,
-            "velocityY": 2.4337192665764835,
-            "timestamp": 1.8036375002728326
-        },
-        {
-            "x": 6.825460366769834,
-            "y": 4.981142108011423,
-            "heading": -2.967027386113265,
-            "angularVelocity": 7.38987079974334e-8,
-            "velocityX": -2.675515425830906,
-            "velocityY": 2.6743702329927124,
-            "timestamp": 1.8925109519309633
-        },
-        {
-            "x": 6.587678075764525,
-            "y": 5.218822621482074,
-            "heading": -2.9670273795456663,
-            "angularVelocity": 7.389832008297281e-8,
-            "velocityX": -2.6755154274865576,
-            "velocityY": 2.6743702313367446,
-            "timestamp": 1.981384403589094
-        },
-        {
-            "x": 6.349895784759215,
-            "y": 5.456503134952723,
-            "heading": -2.967027372978067,
-            "angularVelocity": 7.389832049238788e-8,
-            "velocityX": -2.675515427486565,
-            "velocityY": 2.6743702313367375,
-            "timestamp": 2.0702578552472244
-        },
-        {
-            "x": 6.112113493753906,
-            "y": 5.6941836484233725,
-            "heading": -2.967027366410468,
-            "angularVelocity": 7.389831996958896e-8,
-            "velocityX": -2.67551542748656,
-            "velocityY": 2.6743702313367437,
-            "timestamp": 2.159131306905355
-        },
-        {
-            "x": 5.874331202859884,
-            "y": 5.931864162005351,
-            "heading": -2.9670273598428634,
-            "angularVelocity": 7.389838540319935e-8,
-            "velocityX": -2.6755154262343623,
-            "velocityY": 2.6743702325894136,
-            "timestamp": 2.2480047585634853
-        },
-        {
-            "x": 5.641547676468134,
-            "y": 6.164595563213349,
-            "heading": -2.949497080521501,
-            "angularVelocity": 0.1972498985246826,
-            "velocityX": -2.619269557429503,
-            "velocityY": 2.6186830472537257,
-            "timestamp": 2.3368782102216157
-        },
-        {
-            "x": 5.455313702452088,
-            "y": 6.350776206364247,
-            "heading": -2.9252668204425896,
-            "angularVelocity": 0.2726377745748803,
-            "velocityX": -2.0954961300759565,
-            "velocityY": 2.0948960536284607,
-            "timestamp": 2.425751661879746
-        },
-        {
-            "x": 5.3156503816730245,
-            "y": 6.490395881310486,
-            "heading": -2.9038323021536026,
-            "angularVelocity": 0.2411802162410647,
-            "velocityX": -1.571485276799544,
-            "velocityY": 1.5709941758913435,
-            "timestamp": 2.5146251135378765
-        },
-        {
-            "x": 5.222548326123915,
-            "y": 6.583467415924721,
-            "heading": -2.888242371448522,
-            "angularVelocity": 0.17541718493253458,
-            "velocityX": -1.0475800569474265,
-            "velocityY": 1.047236636788705,
-            "timestamp": 2.603498565196007
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0.09274278532860386,
-            "velocityX": -0.5237596296246885,
-            "velocityY": 0.5235825008157663,
-            "timestamp": 2.6923720168541374
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 3.731495977364104e-28,
-            "velocityX": 5.297862503022749e-31,
-            "velocityY": -3.827373628676626e-30,
-            "timestamp": 2.781245468512268
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S1.traj
deleted file mode 100644
index 41e8c0c3..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S1.traj	
+++ /dev/null
@@ -1,301 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.218507748835192,
-            "y": 6.587508953337151,
-            "heading": -2.890727584630965,
-            "angularVelocity": -0.12627463531080194,
-            "velocityX": 0.5003596491390202,
-            "velocityY": -0.5001630474999694,
-            "timestamp": 0.08495439012388621
-        },
-        {
-            "x": 5.303524107208959,
-            "y": 6.502525872657889,
-            "heading": -2.9120884337900024,
-            "angularVelocity": -0.25143902661031875,
-            "velocityX": 1.0007294296365374,
-            "velocityY": -1.0003377171605015,
-            "timestamp": 0.16990878024777242
-        },
-        {
-            "x": 5.43105048877714,
-            "y": 6.375049128446974,
-            "heading": -2.9439256135386342,
-            "angularVelocity": -0.37475614505901766,
-            "velocityX": 1.5011158503081115,
-            "velocityY": -1.5005315678808802,
-            "timestamp": 0.2548631703716586
-        },
-        {
-            "x": 5.601089619523365,
-            "y": 6.205075520007896,
-            "heading": -2.985927322319944,
-            "angularVelocity": -0.49440304050481265,
-            "velocityX": 2.001534358593136,
-            "velocityY": -2.0007630940704417,
-            "timestamp": 0.33981756049554485
-        },
-        {
-            "x": 5.813649033766866,
-            "y": 5.992595894100207,
-            "heading": -3.037161099536011,
-            "angularVelocity": -0.6030739216881468,
-            "velocityX": 2.502041553515969,
-            "velocityY": -2.501102363255205,
-            "timestamp": 0.42477195061943107
-        },
-        {
-            "x": 6.040945217563345,
-            "y": 5.7653958045642115,
-            "heading": -3.03716110602298,
-            "angularVelocity": -7.635824927871784e-8,
-            "velocityX": 2.6755083929744035,
-            "velocityY": -2.6743772653146864,
-            "timestamp": 0.5097263407433172
-        },
-        {
-            "x": 6.268241401429681,
-            "y": 5.53819571509808,
-            "heading": -3.037161112509923,
-            "angularVelocity": -7.635794871774561e-8,
-            "velocityX": 2.675508393796696,
-            "velocityY": -2.674377264492316,
-            "timestamp": 0.5946807308672034
-        },
-        {
-            "x": 6.495537585296019,
-            "y": 5.310995625631949,
-            "heading": -3.037161118996866,
-            "angularVelocity": -7.635794841586156e-8,
-            "velocityX": 2.675508393796701,
-            "velocityY": -2.6743772644923114,
-            "timestamp": 0.6796351209910896
-        },
-        {
-            "x": 6.722833769162356,
-            "y": 5.083795536165818,
-            "heading": -3.037161125483809,
-            "angularVelocity": -7.635794938585852e-8,
-            "velocityX": 2.6755083937967,
-            "velocityY": -2.6743772644923127,
-            "timestamp": 0.7645895111149758
-        },
-        {
-            "x": 6.950129953013132,
-            "y": 4.856595446684119,
-            "heading": -3.037161131970752,
-            "angularVelocity": -7.635794903516443e-8,
-            "velocityX": 2.67550839361353,
-            "velocityY": -2.6743772646755604,
-            "timestamp": 0.8495439012388619
-        },
-        {
-            "x": 7.177423048635347,
-            "y": 4.629392269264043,
-            "heading": -3.0371611395742963,
-            "angularVelocity": -8.950149400601411e-8,
-            "velocityX": 2.6754720420070424,
-            "velocityY": -2.674413612866287,
-            "timestamp": 0.9344982913627481
-        },
-        {
-            "x": 7.362966742660802,
-            "y": 4.443922816476664,
-            "heading": -3.078668671836781,
-            "angularVelocity": -0.48858607779956104,
-            "velocityX": 2.1840389149365182,
-            "velocityY": -2.183165019685597,
-            "timestamp": 1.0194526814866343
-        },
-        {
-            "x": 7.505991293187966,
-            "y": 4.3009534921360935,
-            "heading": -3.1105416993817094,
-            "angularVelocity": -0.37517811025716574,
-            "velocityX": 1.6835451389668437,
-            "velocityY": -1.6828950703063337,
-            "timestamp": 1.1044070716105205
-        },
-        {
-            "x": 7.6065035905836105,
-            "y": 4.200477915848541,
-            "heading": -3.1320612562280816,
-            "angularVelocity": -0.25330717829595906,
-            "velocityX": 1.1831324696564203,
-            "velocityY": -1.1827002246831095,
-            "timestamp": 1.1893614617344068
-        },
-        {
-            "x": 7.664506074481624,
-            "y": 4.142493806219386,
-            "heading": -3.1429833930528956,
-            "angularVelocity": -0.12856471347350074,
-            "velocityX": 0.6827485173302748,
-            "velocityY": -0.6825322334062814,
-            "timestamp": 1.274315851858293
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.002376735636840867,
-            "velocityX": 0.1823793390257534,
-            "velocityY": -0.18237793475472894,
-            "timestamp": 1.3592702419821794
-        },
-        {
-            "x": 7.6496885572239846,
-            "y": 4.157291685194411,
-            "heading": -3.131583473376336,
-            "angularVelocity": 0.13054330158990457,
-            "velocityX": -0.3410629632388108,
-            "velocityY": 0.3408406518372109,
-            "timestamp": 1.44814369364031
-        },
-        {
-            "x": 7.572857027601893,
-            "y": 4.234083644862719,
-            "heading": -3.108168408493633,
-            "angularVelocity": 0.26346523563343455,
-            "velocityX": -0.8645048458050324,
-            "velocityY": 0.8640596065007748,
-            "timestamp": 1.5370171452984407
-        },
-        {
-            "x": 7.449505471220409,
-            "y": 4.357375922418621,
-            "heading": -3.0729392389653802,
-            "angularVelocity": 0.39639699900261377,
-            "velocityX": -1.387946052278649,
-            "velocityY": 1.3872790496532752,
-            "timestamp": 1.6258905969565713
-        },
-        {
-            "x": 7.279633974763547,
-            "y": 4.52716856280588,
-            "heading": -3.0258936977546345,
-            "angularVelocity": 0.5293542709674363,
-            "velocityX": -1.911386283387714,
-            "velocityY": 1.9104989985109575,
-            "timestamp": 1.714764048614702
-        },
-        {
-            "x": 7.063242657628,
-            "y": 4.743461594393602,
-            "heading": -2.9670273926808983,
-            "angularVelocity": 0.6623609635456799,
-            "velocityX": -2.434825171050669,
-            "velocityY": 2.4337192665764835,
-            "timestamp": 1.8036375002728326
-        },
-        {
-            "x": 6.825460366769834,
-            "y": 4.981142108011423,
-            "heading": -2.967027386113265,
-            "angularVelocity": 7.38987079974334e-8,
-            "velocityX": -2.675515425830906,
-            "velocityY": 2.6743702329927124,
-            "timestamp": 1.8925109519309633
-        },
-        {
-            "x": 6.587678075764525,
-            "y": 5.218822621482074,
-            "heading": -2.9670273795456663,
-            "angularVelocity": 7.389832008297281e-8,
-            "velocityX": -2.6755154274865576,
-            "velocityY": 2.6743702313367446,
-            "timestamp": 1.981384403589094
-        },
-        {
-            "x": 6.349895784759215,
-            "y": 5.456503134952723,
-            "heading": -2.967027372978067,
-            "angularVelocity": 7.389832049238788e-8,
-            "velocityX": -2.675515427486565,
-            "velocityY": 2.6743702313367375,
-            "timestamp": 2.0702578552472244
-        },
-        {
-            "x": 6.112113493753906,
-            "y": 5.6941836484233725,
-            "heading": -2.967027366410468,
-            "angularVelocity": 7.389831996958896e-8,
-            "velocityX": -2.67551542748656,
-            "velocityY": 2.6743702313367437,
-            "timestamp": 2.159131306905355
-        },
-        {
-            "x": 5.874331202859884,
-            "y": 5.931864162005351,
-            "heading": -2.9670273598428634,
-            "angularVelocity": 7.389838540319935e-8,
-            "velocityX": -2.6755154262343623,
-            "velocityY": 2.6743702325894136,
-            "timestamp": 2.2480047585634853
-        },
-        {
-            "x": 5.641547676468134,
-            "y": 6.164595563213349,
-            "heading": -2.949497080521501,
-            "angularVelocity": 0.1972498985246826,
-            "velocityX": -2.619269557429503,
-            "velocityY": 2.6186830472537257,
-            "timestamp": 2.3368782102216157
-        },
-        {
-            "x": 5.455313702452088,
-            "y": 6.350776206364247,
-            "heading": -2.9252668204425896,
-            "angularVelocity": 0.2726377745748803,
-            "velocityX": -2.0954961300759565,
-            "velocityY": 2.0948960536284607,
-            "timestamp": 2.425751661879746
-        },
-        {
-            "x": 5.3156503816730245,
-            "y": 6.490395881310486,
-            "heading": -2.9038323021536026,
-            "angularVelocity": 0.2411802162410647,
-            "velocityX": -1.571485276799544,
-            "velocityY": 1.5709941758913435,
-            "timestamp": 2.5146251135378765
-        },
-        {
-            "x": 5.222548326123915,
-            "y": 6.583467415924721,
-            "heading": -2.888242371448522,
-            "angularVelocity": 0.17541718493253458,
-            "velocityX": -1.0475800569474265,
-            "velocityY": 1.047236636788705,
-            "timestamp": 2.603498565196007
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0.09274278532860386,
-            "velocityX": -0.5237596296246885,
-            "velocityY": 0.5235825008157663,
-            "timestamp": 2.6923720168541374
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 3.731495977364104e-28,
-            "velocityX": 5.297862503022749e-31,
-            "velocityY": -3.827373628676626e-30,
-            "timestamp": 2.781245468512268
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S2.1.traj
deleted file mode 100644
index 3bf6de58..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S2.1.traj	
+++ /dev/null
@@ -1,292 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.2230421692043185,
-            "y": 6.596479510791831,
-            "heading": -2.8866360691814505,
-            "angularVelocity": -0.07970895698970118,
-            "velocityX": 0.5650456828103535,
-            "velocityY": -0.40263040657207605,
-            "timestamp": 0.08325374502526237
-        },
-        {
-            "x": 5.3171265089770285,
-            "y": 6.529438536200997,
-            "heading": -2.8999080848900367,
-            "angularVelocity": -0.15941644072056302,
-            "velocityX": 1.130091382005238,
-            "velocityY": -0.8052607671941977,
-            "timestamp": 0.16650749005052473
-        },
-        {
-            "x": 5.458253021148553,
-            "y": 6.428877082265555,
-            "heading": -2.919815877976496,
-            "angularVelocity": -0.23912189271985748,
-            "velocityX": 1.6951371031862086,
-            "velocityY": -1.207891055290405,
-            "timestamp": 0.2497612350757871
-        },
-        {
-            "x": 5.646421706294536,
-            "y": 6.294795158564417,
-            "heading": -2.946359278217123,
-            "angularVelocity": -0.31882530008192794,
-            "velocityX": 2.260182831281474,
-            "velocityY": -1.6105212283297445,
-            "timestamp": 0.33301498010104946
-        },
-        {
-            "x": 5.881632555955815,
-            "y": 6.127192783772945,
-            "heading": -2.9795381480937526,
-            "angularVelocity": -0.3985270556484735,
-            "velocityX": 2.825228457769793,
-            "velocityY": -2.01315117705052,
-            "timestamp": 0.41626872512631186
-        },
-        {
-            "x": 6.138121957560345,
-            "y": 5.944428216164069,
-            "heading": -2.9795381521700137,
-            "angularVelocity": -4.8961898041700444e-8,
-            "velocityX": 3.080815181667819,
-            "velocityY": -2.195271426569656,
-            "timestamp": 0.49952247015157425
-        },
-        {
-            "x": 6.3946113591646245,
-            "y": 5.761663648554803,
-            "heading": -2.979538156246253,
-            "angularVelocity": -4.896162950841139e-8,
-            "velocityX": 3.080815181664829,
-            "velocityY": -2.1952714265743465,
-            "timestamp": 0.5827762151768366
-        },
-        {
-            "x": 6.651100760768856,
-            "y": 5.57889908094547,
-            "heading": -2.979538160322492,
-            "angularVelocity": -4.8961628925618056e-8,
-            "velocityX": 3.080815181664257,
-            "velocityY": -2.1952714265751494,
-            "timestamp": 0.666029960202099
-        },
-        {
-            "x": 6.907590153574081,
-            "y": 5.396134500987725,
-            "heading": -2.979538164398731,
-            "angularVelocity": -4.8961630365108595e-8,
-            "velocityX": 3.080815075975223,
-            "velocityY": -2.195271574897749,
-            "timestamp": 0.7492837052273614
-        },
-        {
-            "x": 7.162448400773746,
-            "y": 5.211102141322406,
-            "heading": -2.979538168481962,
-            "angularVelocity": -4.904561353756505e-8,
-            "velocityX": 3.06122261674062,
-            "velocityY": -2.2225109466143445,
-            "timestamp": 0.8325374502526238
-        },
-        {
-            "x": 7.379207979156498,
-            "y": 4.982618051640636,
-            "heading": -2.979538183621576,
-            "angularVelocity": -1.8184904794867305e-7,
-            "velocityX": 2.6036015354861264,
-            "velocityY": -2.7444301708283056,
-            "timestamp": 0.9157911952778862
-        },
-        {
-            "x": 7.538979437885237,
-            "y": 4.753039236440615,
-            "heading": -3.0140329583631105,
-            "angularVelocity": -0.4143330096570687,
-            "velocityX": 1.919090350593248,
-            "velocityY": -2.7575794353799603,
-            "timestamp": 0.9990449403031486
-        },
-        {
-            "x": 7.647684941731154,
-            "y": 4.550516222432533,
-            "heading": -3.0504775725311664,
-            "angularVelocity": -0.43775345069461125,
-            "velocityX": 1.3057130800893728,
-            "velocityY": -2.432599445786314,
-            "timestamp": 1.082298685328411
-        },
-        {
-            "x": 7.7070137148586175,
-            "y": 4.378021543776967,
-            "heading": -3.0850144113956173,
-            "angularVelocity": -0.4148382616778724,
-            "velocityX": 0.7126258777842882,
-            "velocityY": -2.071914946327394,
-            "timestamp": 1.1655524303536733
-        },
-        {
-            "x": 7.717659875456922,
-            "y": 4.236658143123209,
-            "heading": -3.1161746508326322,
-            "angularVelocity": -0.3742803333058527,
-            "velocityX": 0.12787605644728667,
-            "velocityY": -1.6979824824801262,
-            "timestamp": 1.2488061753789357
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.32443773356692346,
-            "velocityX": -0.4523505272405549,
-            "velocityY": -1.3171556797827937,
-            "timestamp": 1.332059920404198
-        },
-        {
-            "x": 7.570205120538345,
-            "y": 4.042598994966427,
-            "heading": -3.1684471523328126,
-            "angularVelocity": -0.2594470408705627,
-            "velocityX": -1.1276277091512135,
-            "velocityY": -0.8668246863853641,
-            "timestamp": 1.4294279422882057
-        },
-        {
-            "x": 7.395116825349968,
-            "y": 4.002723380018642,
-            "heading": -3.1873733598366223,
-            "angularVelocity": -0.19437806312174594,
-            "velocityX": -1.7982114846386814,
-            "velocityY": -0.4095350216243075,
-            "timestamp": 1.5267959641722133
-        },
-        {
-            "x": 7.155591929265255,
-            "y": 4.008604211117546,
-            "heading": -3.1999520195770272,
-            "angularVelocity": -0.1291867647818698,
-            "velocityX": -2.4599955041712693,
-            "velocityY": 0.06039797240528109,
-            "timestamp": 1.624163986056221
-        },
-        {
-            "x": 6.8538087913529315,
-            "y": 4.063163767369359,
-            "heading": -3.206164046421751,
-            "angularVelocity": -0.06379945617179969,
-            "velocityX": -3.0994070956050406,
-            "velocityY": 0.5603436857001141,
-            "timestamp": 1.7215320079402285
-        },
-        {
-            "x": 6.5048799215848865,
-            "y": 4.181151469294334,
-            "heading": -3.206164042303272,
-            "angularVelocity": 4.229807303696183e-8,
-            "velocityX": -3.5836084888703787,
-            "velocityY": 1.2117705550753732,
-            "timestamp": 1.8189000298242362
-        },
-        {
-            "x": 6.189160518675962,
-            "y": 4.3708716014574716,
-            "heading": -3.2061640395158935,
-            "angularVelocity": 2.862724812638766e-8,
-            "velocityX": -3.2425368904488527,
-            "velocityY": 1.9484850209768463,
-            "timestamp": 1.9162680517082438
-        },
-        {
-            "x": 5.875739114062726,
-            "y": 4.564364477524865,
-            "heading": -3.2061640367361566,
-            "angularVelocity": 2.854876387634348e-8,
-            "velocityX": -3.218935730116917,
-            "velocityY": 1.987232279381165,
-            "timestamp": 2.0136360735922514
-        },
-        {
-            "x": 5.562317717200868,
-            "y": 4.757857366148008,
-            "heading": -3.20616403395642,
-            "angularVelocity": 2.854876379789868e-8,
-            "velocityX": -3.21893565050783,
-            "velocityY": 1.9872324083326358,
-            "timestamp": 2.111004095476259
-        },
-        {
-            "x": 5.248896320339037,
-            "y": 4.951350254771193,
-            "heading": -3.2061640311766837,
-            "angularVelocity": 2.8548763614821095e-8,
-            "velocityX": -3.218935650507565,
-            "velocityY": 1.9872324083330646,
-            "timestamp": 2.208372117360266
-        },
-        {
-            "x": 4.935474923469446,
-            "y": 5.14484314338174,
-            "heading": -3.2061640283969295,
-            "angularVelocity": 2.8548942066572532e-8,
-            "velocityX": -3.2189356505872584,
-            "velocityY": 1.9872324082032717,
-            "timestamp": 2.3057401392442736
-        },
-        {
-            "x": 4.666484969375691,
-            "y": 5.310905875767285,
-            "heading": -3.180972420287378,
-            "angularVelocity": 0.2587256844917891,
-            "velocityX": -2.76261085404609,
-            "velocityY": 1.7055161353012933,
-            "timestamp": 2.403108161128281
-        },
-        {
-            "x": 4.464742488596807,
-            "y": 5.435452935258762,
-            "heading": -3.1620788420473156,
-            "angularVelocity": 0.19404295039052252,
-            "velocityX": -2.071958296731311,
-            "velocityY": 1.279137206256954,
-            "timestamp": 2.5004761830122884
-        },
-        {
-            "x": 4.330247497040812,
-            "y": 5.518484311233343,
-            "heading": -3.149483158535891,
-            "angularVelocity": 0.12936160422802073,
-            "velocityX": -1.3813055760362147,
-            "velocityY": 0.8527581681128775,
-            "timestamp": 2.5978442048962958
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0.0646809007150999,
-            "velocityX": -0.6906528009876122,
-            "velocityY": 0.4263790920607684,
-            "timestamp": 2.695212226780303
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": -3.400041679826015e-29,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 2.7925802486643105
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S2.traj
deleted file mode 100644
index 3bf6de58..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S2.traj	
+++ /dev/null
@@ -1,292 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.2230421692043185,
-            "y": 6.596479510791831,
-            "heading": -2.8866360691814505,
-            "angularVelocity": -0.07970895698970118,
-            "velocityX": 0.5650456828103535,
-            "velocityY": -0.40263040657207605,
-            "timestamp": 0.08325374502526237
-        },
-        {
-            "x": 5.3171265089770285,
-            "y": 6.529438536200997,
-            "heading": -2.8999080848900367,
-            "angularVelocity": -0.15941644072056302,
-            "velocityX": 1.130091382005238,
-            "velocityY": -0.8052607671941977,
-            "timestamp": 0.16650749005052473
-        },
-        {
-            "x": 5.458253021148553,
-            "y": 6.428877082265555,
-            "heading": -2.919815877976496,
-            "angularVelocity": -0.23912189271985748,
-            "velocityX": 1.6951371031862086,
-            "velocityY": -1.207891055290405,
-            "timestamp": 0.2497612350757871
-        },
-        {
-            "x": 5.646421706294536,
-            "y": 6.294795158564417,
-            "heading": -2.946359278217123,
-            "angularVelocity": -0.31882530008192794,
-            "velocityX": 2.260182831281474,
-            "velocityY": -1.6105212283297445,
-            "timestamp": 0.33301498010104946
-        },
-        {
-            "x": 5.881632555955815,
-            "y": 6.127192783772945,
-            "heading": -2.9795381480937526,
-            "angularVelocity": -0.3985270556484735,
-            "velocityX": 2.825228457769793,
-            "velocityY": -2.01315117705052,
-            "timestamp": 0.41626872512631186
-        },
-        {
-            "x": 6.138121957560345,
-            "y": 5.944428216164069,
-            "heading": -2.9795381521700137,
-            "angularVelocity": -4.8961898041700444e-8,
-            "velocityX": 3.080815181667819,
-            "velocityY": -2.195271426569656,
-            "timestamp": 0.49952247015157425
-        },
-        {
-            "x": 6.3946113591646245,
-            "y": 5.761663648554803,
-            "heading": -2.979538156246253,
-            "angularVelocity": -4.896162950841139e-8,
-            "velocityX": 3.080815181664829,
-            "velocityY": -2.1952714265743465,
-            "timestamp": 0.5827762151768366
-        },
-        {
-            "x": 6.651100760768856,
-            "y": 5.57889908094547,
-            "heading": -2.979538160322492,
-            "angularVelocity": -4.8961628925618056e-8,
-            "velocityX": 3.080815181664257,
-            "velocityY": -2.1952714265751494,
-            "timestamp": 0.666029960202099
-        },
-        {
-            "x": 6.907590153574081,
-            "y": 5.396134500987725,
-            "heading": -2.979538164398731,
-            "angularVelocity": -4.8961630365108595e-8,
-            "velocityX": 3.080815075975223,
-            "velocityY": -2.195271574897749,
-            "timestamp": 0.7492837052273614
-        },
-        {
-            "x": 7.162448400773746,
-            "y": 5.211102141322406,
-            "heading": -2.979538168481962,
-            "angularVelocity": -4.904561353756505e-8,
-            "velocityX": 3.06122261674062,
-            "velocityY": -2.2225109466143445,
-            "timestamp": 0.8325374502526238
-        },
-        {
-            "x": 7.379207979156498,
-            "y": 4.982618051640636,
-            "heading": -2.979538183621576,
-            "angularVelocity": -1.8184904794867305e-7,
-            "velocityX": 2.6036015354861264,
-            "velocityY": -2.7444301708283056,
-            "timestamp": 0.9157911952778862
-        },
-        {
-            "x": 7.538979437885237,
-            "y": 4.753039236440615,
-            "heading": -3.0140329583631105,
-            "angularVelocity": -0.4143330096570687,
-            "velocityX": 1.919090350593248,
-            "velocityY": -2.7575794353799603,
-            "timestamp": 0.9990449403031486
-        },
-        {
-            "x": 7.647684941731154,
-            "y": 4.550516222432533,
-            "heading": -3.0504775725311664,
-            "angularVelocity": -0.43775345069461125,
-            "velocityX": 1.3057130800893728,
-            "velocityY": -2.432599445786314,
-            "timestamp": 1.082298685328411
-        },
-        {
-            "x": 7.7070137148586175,
-            "y": 4.378021543776967,
-            "heading": -3.0850144113956173,
-            "angularVelocity": -0.4148382616778724,
-            "velocityX": 0.7126258777842882,
-            "velocityY": -2.071914946327394,
-            "timestamp": 1.1655524303536733
-        },
-        {
-            "x": 7.717659875456922,
-            "y": 4.236658143123209,
-            "heading": -3.1161746508326322,
-            "angularVelocity": -0.3742803333058527,
-            "velocityX": 0.12787605644728667,
-            "velocityY": -1.6979824824801262,
-            "timestamp": 1.2488061753789357
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.32443773356692346,
-            "velocityX": -0.4523505272405549,
-            "velocityY": -1.3171556797827937,
-            "timestamp": 1.332059920404198
-        },
-        {
-            "x": 7.570205120538345,
-            "y": 4.042598994966427,
-            "heading": -3.1684471523328126,
-            "angularVelocity": -0.2594470408705627,
-            "velocityX": -1.1276277091512135,
-            "velocityY": -0.8668246863853641,
-            "timestamp": 1.4294279422882057
-        },
-        {
-            "x": 7.395116825349968,
-            "y": 4.002723380018642,
-            "heading": -3.1873733598366223,
-            "angularVelocity": -0.19437806312174594,
-            "velocityX": -1.7982114846386814,
-            "velocityY": -0.4095350216243075,
-            "timestamp": 1.5267959641722133
-        },
-        {
-            "x": 7.155591929265255,
-            "y": 4.008604211117546,
-            "heading": -3.1999520195770272,
-            "angularVelocity": -0.1291867647818698,
-            "velocityX": -2.4599955041712693,
-            "velocityY": 0.06039797240528109,
-            "timestamp": 1.624163986056221
-        },
-        {
-            "x": 6.8538087913529315,
-            "y": 4.063163767369359,
-            "heading": -3.206164046421751,
-            "angularVelocity": -0.06379945617179969,
-            "velocityX": -3.0994070956050406,
-            "velocityY": 0.5603436857001141,
-            "timestamp": 1.7215320079402285
-        },
-        {
-            "x": 6.5048799215848865,
-            "y": 4.181151469294334,
-            "heading": -3.206164042303272,
-            "angularVelocity": 4.229807303696183e-8,
-            "velocityX": -3.5836084888703787,
-            "velocityY": 1.2117705550753732,
-            "timestamp": 1.8189000298242362
-        },
-        {
-            "x": 6.189160518675962,
-            "y": 4.3708716014574716,
-            "heading": -3.2061640395158935,
-            "angularVelocity": 2.862724812638766e-8,
-            "velocityX": -3.2425368904488527,
-            "velocityY": 1.9484850209768463,
-            "timestamp": 1.9162680517082438
-        },
-        {
-            "x": 5.875739114062726,
-            "y": 4.564364477524865,
-            "heading": -3.2061640367361566,
-            "angularVelocity": 2.854876387634348e-8,
-            "velocityX": -3.218935730116917,
-            "velocityY": 1.987232279381165,
-            "timestamp": 2.0136360735922514
-        },
-        {
-            "x": 5.562317717200868,
-            "y": 4.757857366148008,
-            "heading": -3.20616403395642,
-            "angularVelocity": 2.854876379789868e-8,
-            "velocityX": -3.21893565050783,
-            "velocityY": 1.9872324083326358,
-            "timestamp": 2.111004095476259
-        },
-        {
-            "x": 5.248896320339037,
-            "y": 4.951350254771193,
-            "heading": -3.2061640311766837,
-            "angularVelocity": 2.8548763614821095e-8,
-            "velocityX": -3.218935650507565,
-            "velocityY": 1.9872324083330646,
-            "timestamp": 2.208372117360266
-        },
-        {
-            "x": 4.935474923469446,
-            "y": 5.14484314338174,
-            "heading": -3.2061640283969295,
-            "angularVelocity": 2.8548942066572532e-8,
-            "velocityX": -3.2189356505872584,
-            "velocityY": 1.9872324082032717,
-            "timestamp": 2.3057401392442736
-        },
-        {
-            "x": 4.666484969375691,
-            "y": 5.310905875767285,
-            "heading": -3.180972420287378,
-            "angularVelocity": 0.2587256844917891,
-            "velocityX": -2.76261085404609,
-            "velocityY": 1.7055161353012933,
-            "timestamp": 2.403108161128281
-        },
-        {
-            "x": 4.464742488596807,
-            "y": 5.435452935258762,
-            "heading": -3.1620788420473156,
-            "angularVelocity": 0.19404295039052252,
-            "velocityX": -2.071958296731311,
-            "velocityY": 1.279137206256954,
-            "timestamp": 2.5004761830122884
-        },
-        {
-            "x": 4.330247497040812,
-            "y": 5.518484311233343,
-            "heading": -3.149483158535891,
-            "angularVelocity": 0.12936160422802073,
-            "velocityX": -1.3813055760362147,
-            "velocityY": 0.8527581681128775,
-            "timestamp": 2.5978442048962958
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0.0646809007150999,
-            "velocityX": -0.6906528009876122,
-            "velocityY": 0.4263790920607684,
-            "timestamp": 2.695212226780303
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": -3.400041679826015e-29,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 2.7925802486643105
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S3.1.traj
deleted file mode 100644
index 77d8e571..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S3.1.traj	
+++ /dev/null
@@ -1,310 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 2.6904386896052926e-33,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.221431258578075,
-            "y": 6.6088996918922325,
-            "heading": -2.8866746127276346,
-            "angularVelocity": -0.08608437747729661,
-            "velocityX": 0.5859398548340703,
-            "velocityY": -0.2721366710185261,
-            "timestamp": 0.07753570303037556
-        },
-        {
-            "x": 5.31229377552925,
-            "y": 6.566699082350374,
-            "heading": -2.90002400824425,
-            "angularVelocity": -0.17217094828412036,
-            "velocityX": 1.1718797070244922,
-            "velocityY": -0.5442732559647441,
-            "timestamp": 0.15507140606075112
-        },
-        {
-            "x": 5.448587549407382,
-            "y": 6.503398181602668,
-            "heading": -2.920048533381972,
-            "angularVelocity": -0.2582619922834353,
-            "velocityX": 1.7578195405636137,
-            "velocityY": -0.8164097089944291,
-            "timestamp": 0.2326071090911267
-        },
-        {
-            "x": 5.630312575309777,
-            "y": 6.418997003981843,
-            "heading": -2.946748763466125,
-            "angularVelocity": -0.34436045641700236,
-            "velocityX": 2.3437593108713033,
-            "velocityY": -1.0885459771707946,
-            "timestamp": 0.31014281212150224
-        },
-        {
-            "x": 5.85746883610922,
-            "y": 6.313495571165909,
-            "heading": -2.9801255206908306,
-            "angularVelocity": -0.43046952462183896,
-            "velocityX": 2.929698860284435,
-            "velocityY": -1.3606819657597289,
-            "timestamp": 0.3876785151518778
-        },
-        {
-            "x": 6.123490041046321,
-            "y": 6.189943134833017,
-            "heading": -2.9801255253705285,
-            "angularVelocity": -6.035539216471427e-8,
-            "velocityX": 3.4309510914331183,
-            "velocityY": -1.5934908887649881,
-            "timestamp": 0.46521421818225334
-        },
-        {
-            "x": 6.3895112454442975,
-            "y": 6.066390697338873,
-            "heading": -2.980125530050055,
-            "angularVelocity": -6.035318736040994e-8,
-            "velocityX": 3.4309510844798714,
-            "velocityY": -1.5934909037419929,
-            "timestamp": 0.5427499212126289
-        },
-        {
-            "x": 6.655532416724182,
-            "y": 5.9428381885380706,
-            "heading": -2.980125534729582,
-            "angularVelocity": -6.035318830910377e-8,
-            "velocityX": 3.4309506573464303,
-            "velocityY": -1.5934918234042366,
-            "timestamp": 0.6202856242430045
-        },
-        {
-            "x": 6.916684100099049,
-            "y": 5.8093006126159725,
-            "heading": -2.9801255394461488,
-            "angularVelocity": -6.083090321698073e-8,
-            "velocityX": 3.368147487778084,
-            "velocityY": -1.722272072128935,
-            "timestamp": 0.6978213272733801
-        },
-        {
-            "x": 7.151284630214051,
-            "y": 5.6332464271795715,
-            "heading": -2.980125544587418,
-            "angularVelocity": -6.630841173330857e-8,
-            "velocityX": 3.0257097175361216,
-            "velocityY": -2.2706208695551444,
-            "timestamp": 0.7753570303037557
-        },
-        {
-            "x": 7.352481762263237,
-            "y": 5.41981682210962,
-            "heading": -2.9801255509932516,
-            "angularVelocity": -8.261785689390399e-8,
-            "velocityX": 2.594896598414324,
-            "velocityY": -2.7526622798059472,
-            "timestamp": 0.8528927333341313
-        },
-        {
-            "x": 7.514399141163384,
-            "y": 5.175245297922962,
-            "heading": -2.9801256025732923,
-            "angularVelocity": -6.652424424834864e-7,
-            "velocityX": 2.0882944575444746,
-            "velocityY": -3.1543084621396242,
-            "timestamp": 0.9304284363645069
-        },
-        {
-            "x": 7.630164379984533,
-            "y": 4.911565821303128,
-            "heading": -2.995124870089402,
-            "angularVelocity": -0.19344981640565623,
-            "velocityX": 1.4930571890964461,
-            "velocityY": -3.4007491557345486,
-            "timestamp": 1.0079641393948824
-        },
-        {
-            "x": 7.696376897938806,
-            "y": 4.643893289420577,
-            "heading": -3.0332372938699343,
-            "angularVelocity": -0.49154676221354465,
-            "velocityX": 0.8539616636781268,
-            "velocityY": -3.452248724406194,
-            "timestamp": 1.085499842425258
-        },
-        {
-            "x": 7.712814001135022,
-            "y": 4.380911639847311,
-            "heading": -3.084305372604304,
-            "angularVelocity": -0.6586395265464133,
-            "velocityX": 0.2119939918488342,
-            "velocityY": -3.3917490819711635,
-            "timestamp": 1.1630355454556336
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.7593912516949146,
-            "velocityX": -0.4232114993806923,
-            "velocityY": -3.274770588561481,
-            "timestamp": 1.2405712484860092
-        },
-        {
-            "x": 7.5963416463862465,
-            "y": 3.881029306471945,
-            "heading": -3.207914317977363,
-            "angularVelocity": -0.8216221979368662,
-            "velocityX": -1.061897277970947,
-            "velocityY": -3.1221700958161,
-            "timestamp": 1.3193532129464565
-        },
-        {
-            "x": 7.463114472360868,
-            "y": 3.64992032438806,
-            "heading": -3.2707770828701133,
-            "angularVelocity": -0.7979334524504146,
-            "velocityX": -1.6910872296446224,
-            "velocityY": -2.933526520526327,
-            "timestamp": 1.3981351774069037
-        },
-        {
-            "x": 7.282107143226341,
-            "y": 3.438334424773196,
-            "heading": -3.3202070228970326,
-            "angularVelocity": -0.627427106768014,
-            "velocityX": -2.297573186632101,
-            "velocityY": -2.6857149483888696,
-            "timestamp": 1.476917141867351
-        },
-        {
-            "x": 7.057858991686637,
-            "y": 3.2538746449727443,
-            "heading": -3.3390490142356852,
-            "angularVelocity": -0.23916630497468983,
-            "velocityX": -2.846440210973517,
-            "velocityY": -2.341396042403339,
-            "timestamp": 1.5556991063277983
-        },
-        {
-            "x": 6.798499862682402,
-            "y": 3.1070645806670387,
-            "heading": -3.339049130726268,
-            "angularVelocity": -0.0000014786453189461519,
-            "velocityX": -3.2921129954108745,
-            "velocityY": -1.8634983947298314,
-            "timestamp": 1.6344810707882456
-        },
-        {
-            "x": 6.517654165139062,
-            "y": 3.0073345934329407,
-            "heading": -3.339049157320925,
-            "angularVelocity": -3.3757290550660805e-7,
-            "velocityX": -3.564847607783876,
-            "velocityY": -1.26589871066451,
-            "timestamp": 1.7132630352486928
-        },
-        {
-            "x": 6.223790307061648,
-            "y": 2.9576917507277325,
-            "heading": -3.339049179064471,
-            "angularVelocity": -2.7599649426865103e-7,
-            "velocityX": -3.730090511070579,
-            "velocityY": -0.6301295359311817,
-            "timestamp": 1.79204499970914
-        },
-        {
-            "x": 5.925769134173891,
-            "y": 2.9596309032298342,
-            "heading": -3.3390491994000007,
-            "angularVelocity": -2.581241798261079e-7,
-            "velocityX": -3.7828603910655114,
-            "velocityY": 0.024614167917522748,
-            "timestamp": 1.8708269641695874
-        },
-        {
-            "x": 5.627822525147453,
-            "y": 2.9665733818436117,
-            "heading": -3.339049219680213,
-            "angularVelocity": -2.574220136164825e-7,
-            "velocityX": -3.781913932547634,
-            "velocityY": 0.08812268977200981,
-            "timestamp": 1.9496089286300347
-        },
-        {
-            "x": 5.329875916862242,
-            "y": 2.9735158922681233,
-            "heading": -3.3390492399604246,
-            "angularVelocity": -2.5742201457831597e-7,
-            "velocityX": -3.7819139231390566,
-            "velocityY": 0.08812309355394705,
-            "timestamp": 2.028390893090482
-        },
-        {
-            "x": 5.031929325692052,
-            "y": 2.9804591371189906,
-            "heading": -3.3390492602433595,
-            "angularVelocity": -2.5745657158705766e-7,
-            "velocityX": -3.781913705893629,
-            "velocityY": 0.08813241581901742,
-            "timestamp": 2.107172857550929
-        },
-        {
-            "x": 4.775295960622385,
-            "y": 2.98713582308431,
-            "heading": -3.452004590816206,
-            "angularVelocity": -1.4337714392683885,
-            "velocityX": -3.2575141636447755,
-            "velocityY": 0.08474891443804398,
-            "timestamp": 2.1859548220113765
-        },
-        {
-            "x": 4.570259992018404,
-            "y": 2.9923299087295048,
-            "heading": -3.5517317371257473,
-            "angularVelocity": -1.2658626500689703,
-            "velocityX": -2.6025749676110213,
-            "velocityY": 0.06592988231212166,
-            "timestamp": 2.2647367864718237
-        },
-        {
-            "x": 4.416588409818771,
-            "y": 2.996180871589336,
-            "heading": -3.6294028651739794,
-            "angularVelocity": -0.9858998640130027,
-            "velocityX": -1.9505934289920475,
-            "velocityY": 0.04888127487306719,
-            "timestamp": 2.343518750932271
-        },
-        {
-            "x": 4.314185287460311,
-            "y": 2.998730700935922,
-            "heading": -3.6823213519430347,
-            "angularVelocity": -0.6717081394387356,
-            "velocityX": -1.2998295112312361,
-            "velocityY": 0.03236564820448933,
-            "timestamp": 2.4223007153927183
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -3.7091853071795864,
-            "angularVelocity": -0.3409911826968791,
-            "velocityX": -0.6497081890615813,
-            "velocityY": 0.01611154371144828,
-            "timestamp": 2.5010826798531656
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -3.7091853071795864,
-            "angularVelocity": 0,
-            "velocityX": -2.823998043085552e-35,
-            "velocityY": 0,
-            "timestamp": 2.579864644313613
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S3.traj
deleted file mode 100644
index 77d8e571..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S3.traj	
+++ /dev/null
@@ -1,310 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 2.6904386896052926e-33,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.221431258578075,
-            "y": 6.6088996918922325,
-            "heading": -2.8866746127276346,
-            "angularVelocity": -0.08608437747729661,
-            "velocityX": 0.5859398548340703,
-            "velocityY": -0.2721366710185261,
-            "timestamp": 0.07753570303037556
-        },
-        {
-            "x": 5.31229377552925,
-            "y": 6.566699082350374,
-            "heading": -2.90002400824425,
-            "angularVelocity": -0.17217094828412036,
-            "velocityX": 1.1718797070244922,
-            "velocityY": -0.5442732559647441,
-            "timestamp": 0.15507140606075112
-        },
-        {
-            "x": 5.448587549407382,
-            "y": 6.503398181602668,
-            "heading": -2.920048533381972,
-            "angularVelocity": -0.2582619922834353,
-            "velocityX": 1.7578195405636137,
-            "velocityY": -0.8164097089944291,
-            "timestamp": 0.2326071090911267
-        },
-        {
-            "x": 5.630312575309777,
-            "y": 6.418997003981843,
-            "heading": -2.946748763466125,
-            "angularVelocity": -0.34436045641700236,
-            "velocityX": 2.3437593108713033,
-            "velocityY": -1.0885459771707946,
-            "timestamp": 0.31014281212150224
-        },
-        {
-            "x": 5.85746883610922,
-            "y": 6.313495571165909,
-            "heading": -2.9801255206908306,
-            "angularVelocity": -0.43046952462183896,
-            "velocityX": 2.929698860284435,
-            "velocityY": -1.3606819657597289,
-            "timestamp": 0.3876785151518778
-        },
-        {
-            "x": 6.123490041046321,
-            "y": 6.189943134833017,
-            "heading": -2.9801255253705285,
-            "angularVelocity": -6.035539216471427e-8,
-            "velocityX": 3.4309510914331183,
-            "velocityY": -1.5934908887649881,
-            "timestamp": 0.46521421818225334
-        },
-        {
-            "x": 6.3895112454442975,
-            "y": 6.066390697338873,
-            "heading": -2.980125530050055,
-            "angularVelocity": -6.035318736040994e-8,
-            "velocityX": 3.4309510844798714,
-            "velocityY": -1.5934909037419929,
-            "timestamp": 0.5427499212126289
-        },
-        {
-            "x": 6.655532416724182,
-            "y": 5.9428381885380706,
-            "heading": -2.980125534729582,
-            "angularVelocity": -6.035318830910377e-8,
-            "velocityX": 3.4309506573464303,
-            "velocityY": -1.5934918234042366,
-            "timestamp": 0.6202856242430045
-        },
-        {
-            "x": 6.916684100099049,
-            "y": 5.8093006126159725,
-            "heading": -2.9801255394461488,
-            "angularVelocity": -6.083090321698073e-8,
-            "velocityX": 3.368147487778084,
-            "velocityY": -1.722272072128935,
-            "timestamp": 0.6978213272733801
-        },
-        {
-            "x": 7.151284630214051,
-            "y": 5.6332464271795715,
-            "heading": -2.980125544587418,
-            "angularVelocity": -6.630841173330857e-8,
-            "velocityX": 3.0257097175361216,
-            "velocityY": -2.2706208695551444,
-            "timestamp": 0.7753570303037557
-        },
-        {
-            "x": 7.352481762263237,
-            "y": 5.41981682210962,
-            "heading": -2.9801255509932516,
-            "angularVelocity": -8.261785689390399e-8,
-            "velocityX": 2.594896598414324,
-            "velocityY": -2.7526622798059472,
-            "timestamp": 0.8528927333341313
-        },
-        {
-            "x": 7.514399141163384,
-            "y": 5.175245297922962,
-            "heading": -2.9801256025732923,
-            "angularVelocity": -6.652424424834864e-7,
-            "velocityX": 2.0882944575444746,
-            "velocityY": -3.1543084621396242,
-            "timestamp": 0.9304284363645069
-        },
-        {
-            "x": 7.630164379984533,
-            "y": 4.911565821303128,
-            "heading": -2.995124870089402,
-            "angularVelocity": -0.19344981640565623,
-            "velocityX": 1.4930571890964461,
-            "velocityY": -3.4007491557345486,
-            "timestamp": 1.0079641393948824
-        },
-        {
-            "x": 7.696376897938806,
-            "y": 4.643893289420577,
-            "heading": -3.0332372938699343,
-            "angularVelocity": -0.49154676221354465,
-            "velocityX": 0.8539616636781268,
-            "velocityY": -3.452248724406194,
-            "timestamp": 1.085499842425258
-        },
-        {
-            "x": 7.712814001135022,
-            "y": 4.380911639847311,
-            "heading": -3.084305372604304,
-            "angularVelocity": -0.6586395265464133,
-            "velocityX": 0.2119939918488342,
-            "velocityY": -3.3917490819711635,
-            "timestamp": 1.1630355454556336
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.7593912516949146,
-            "velocityX": -0.4232114993806923,
-            "velocityY": -3.274770588561481,
-            "timestamp": 1.2405712484860092
-        },
-        {
-            "x": 7.5963416463862465,
-            "y": 3.881029306471945,
-            "heading": -3.207914317977363,
-            "angularVelocity": -0.8216221979368662,
-            "velocityX": -1.061897277970947,
-            "velocityY": -3.1221700958161,
-            "timestamp": 1.3193532129464565
-        },
-        {
-            "x": 7.463114472360868,
-            "y": 3.64992032438806,
-            "heading": -3.2707770828701133,
-            "angularVelocity": -0.7979334524504146,
-            "velocityX": -1.6910872296446224,
-            "velocityY": -2.933526520526327,
-            "timestamp": 1.3981351774069037
-        },
-        {
-            "x": 7.282107143226341,
-            "y": 3.438334424773196,
-            "heading": -3.3202070228970326,
-            "angularVelocity": -0.627427106768014,
-            "velocityX": -2.297573186632101,
-            "velocityY": -2.6857149483888696,
-            "timestamp": 1.476917141867351
-        },
-        {
-            "x": 7.057858991686637,
-            "y": 3.2538746449727443,
-            "heading": -3.3390490142356852,
-            "angularVelocity": -0.23916630497468983,
-            "velocityX": -2.846440210973517,
-            "velocityY": -2.341396042403339,
-            "timestamp": 1.5556991063277983
-        },
-        {
-            "x": 6.798499862682402,
-            "y": 3.1070645806670387,
-            "heading": -3.339049130726268,
-            "angularVelocity": -0.0000014786453189461519,
-            "velocityX": -3.2921129954108745,
-            "velocityY": -1.8634983947298314,
-            "timestamp": 1.6344810707882456
-        },
-        {
-            "x": 6.517654165139062,
-            "y": 3.0073345934329407,
-            "heading": -3.339049157320925,
-            "angularVelocity": -3.3757290550660805e-7,
-            "velocityX": -3.564847607783876,
-            "velocityY": -1.26589871066451,
-            "timestamp": 1.7132630352486928
-        },
-        {
-            "x": 6.223790307061648,
-            "y": 2.9576917507277325,
-            "heading": -3.339049179064471,
-            "angularVelocity": -2.7599649426865103e-7,
-            "velocityX": -3.730090511070579,
-            "velocityY": -0.6301295359311817,
-            "timestamp": 1.79204499970914
-        },
-        {
-            "x": 5.925769134173891,
-            "y": 2.9596309032298342,
-            "heading": -3.3390491994000007,
-            "angularVelocity": -2.581241798261079e-7,
-            "velocityX": -3.7828603910655114,
-            "velocityY": 0.024614167917522748,
-            "timestamp": 1.8708269641695874
-        },
-        {
-            "x": 5.627822525147453,
-            "y": 2.9665733818436117,
-            "heading": -3.339049219680213,
-            "angularVelocity": -2.574220136164825e-7,
-            "velocityX": -3.781913932547634,
-            "velocityY": 0.08812268977200981,
-            "timestamp": 1.9496089286300347
-        },
-        {
-            "x": 5.329875916862242,
-            "y": 2.9735158922681233,
-            "heading": -3.3390492399604246,
-            "angularVelocity": -2.5742201457831597e-7,
-            "velocityX": -3.7819139231390566,
-            "velocityY": 0.08812309355394705,
-            "timestamp": 2.028390893090482
-        },
-        {
-            "x": 5.031929325692052,
-            "y": 2.9804591371189906,
-            "heading": -3.3390492602433595,
-            "angularVelocity": -2.5745657158705766e-7,
-            "velocityX": -3.781913705893629,
-            "velocityY": 0.08813241581901742,
-            "timestamp": 2.107172857550929
-        },
-        {
-            "x": 4.775295960622385,
-            "y": 2.98713582308431,
-            "heading": -3.452004590816206,
-            "angularVelocity": -1.4337714392683885,
-            "velocityX": -3.2575141636447755,
-            "velocityY": 0.08474891443804398,
-            "timestamp": 2.1859548220113765
-        },
-        {
-            "x": 4.570259992018404,
-            "y": 2.9923299087295048,
-            "heading": -3.5517317371257473,
-            "angularVelocity": -1.2658626500689703,
-            "velocityX": -2.6025749676110213,
-            "velocityY": 0.06592988231212166,
-            "timestamp": 2.2647367864718237
-        },
-        {
-            "x": 4.416588409818771,
-            "y": 2.996180871589336,
-            "heading": -3.6294028651739794,
-            "angularVelocity": -0.9858998640130027,
-            "velocityX": -1.9505934289920475,
-            "velocityY": 0.04888127487306719,
-            "timestamp": 2.343518750932271
-        },
-        {
-            "x": 4.314185287460311,
-            "y": 2.998730700935922,
-            "heading": -3.6823213519430347,
-            "angularVelocity": -0.6717081394387356,
-            "velocityX": -1.2998295112312361,
-            "velocityY": 0.03236564820448933,
-            "timestamp": 2.4223007153927183
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -3.7091853071795864,
-            "angularVelocity": -0.3409911826968791,
-            "velocityX": -0.6497081890615813,
-            "velocityY": 0.01611154371144828,
-            "timestamp": 2.5010826798531656
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -3.7091853071795864,
-            "angularVelocity": 0,
-            "velocityX": -2.823998043085552e-35,
-            "velocityY": 0,
-            "timestamp": 2.579864644313613
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S4.1.traj
deleted file mode 100644
index 4d0afc34..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S4.1.traj	
+++ /dev/null
@@ -1,319 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": -4.965302317163387e-33,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.214448464194176,
-            "y": 6.6065493619277555,
-            "heading": -2.899983532805956,
-            "angularVelocity": -0.2708663369562031,
-            "velocityX": 0.5211488258354376,
-            "velocityY": -0.31786113574578073,
-            "timestamp": 0.0737763615461288
-        },
-        {
-            "x": 5.29140014559419,
-            "y": 6.559619691972876,
-            "heading": -2.9381129725152273,
-            "angularVelocity": -0.5168246157738562,
-            "velocityX": 1.0430398001113212,
-            "velocityY": -0.636107134743106,
-            "timestamp": 0.1475527230922576
-        },
-        {
-            "x": 5.4069285579459345,
-            "y": 6.489171539636289,
-            "heading": -2.991543589570423,
-            "angularVelocity": -0.7242240730696432,
-            "velocityX": 1.5659272147693208,
-            "velocityY": -0.9548878646250154,
-            "timestamp": 0.22132908463838638
-        },
-        {
-            "x": 5.5611322195100845,
-            "y": 6.395152264966145,
-            "heading": -3.0552740925010653,
-            "angularVelocity": -0.8638336398684346,
-            "velocityX": 2.090149992931468,
-            "velocityY": -1.274382101526624,
-            "timestamp": 0.2951054461845152
-        },
-        {
-            "x": 5.754097394020428,
-            "y": 6.277527348774539,
-            "heading": -3.118382886347054,
-            "angularVelocity": -0.8554066983437507,
-            "velocityX": 2.615542030894161,
-            "velocityY": -1.5943442279687556,
-            "timestamp": 0.36888180773064394
-        },
-        {
-            "x": 5.984744637231227,
-            "y": 6.137241174757932,
-            "heading": -3.141361477565195,
-            "angularVelocity": -0.3114627874915312,
-            "velocityX": 3.126302766592624,
-            "velocityY": -1.9015057272632323,
-            "timestamp": 0.4426581692767727
-        },
-        {
-            "x": 6.222972456839802,
-            "y": 5.99184648556835,
-            "heading": -3.141361501066067,
-            "angularVelocity": -3.1854203464323085e-7,
-            "velocityX": 3.2290535154627107,
-            "velocityY": -1.9707489789757882,
-            "timestamp": 0.5164345308229015
-        },
-        {
-            "x": 6.461200274584459,
-            "y": 5.84645179332473,
-            "heading": -3.141361524566912,
-            "angularVelocity": -3.1854166338718743e-7,
-            "velocityX": 3.229053490198278,
-            "velocityY": -1.9707490203716698,
-            "timestamp": 0.5902108923690302
-        },
-        {
-            "x": 6.699428092323813,
-            "y": 5.701057101072421,
-            "heading": -3.141361548067757,
-            "angularVelocity": -3.1854166408242085e-7,
-            "velocityX": 3.229053490126394,
-            "velocityY": -1.9707490204894516,
-            "timestamp": 0.663987253915159
-        },
-        {
-            "x": 6.937655250211389,
-            "y": 5.555661327664803,
-            "heading": -3.1413615715686114,
-            "angularVelocity": -3.1854179676885927e-7,
-            "velocityX": 3.229044546180602,
-            "velocityY": -1.9707636749842845,
-            "timestamp": 0.7377636154612878
-        },
-        {
-            "x": 7.1538386137488,
-            "y": 5.379148261525069,
-            "heading": -3.141361595947866,
-            "angularVelocity": -3.304480485706385e-7,
-            "velocityX": 2.9302524413899382,
-            "velocityY": -2.3925423054289428,
-            "timestamp": 0.8115399770074165
-        },
-        {
-            "x": 7.338555522796062,
-            "y": 5.1699312191681095,
-            "heading": -3.1413616242387703,
-            "angularVelocity": -3.834684085405442e-7,
-            "velocityX": 2.5037411059064913,
-            "velocityY": -2.835827600770833,
-            "timestamp": 0.8853163385535453
-        },
-        {
-            "x": 7.486921372318085,
-            "y": 4.933542391468771,
-            "heading": -3.14136166220073,
-            "angularVelocity": -5.145545098256939e-7,
-            "velocityX": 2.011021503537517,
-            "velocityY": -3.2041269418191103,
-            "timestamp": 0.9590927000996741
-        },
-        {
-            "x": 7.595012870290821,
-            "y": 4.676232612674377,
-            "heading": -3.1413617253677457,
-            "angularVelocity": -8.561958656088552e-7,
-            "velocityX": 1.4651237294367088,
-            "velocityY": -3.4876994934686665,
-            "timestamp": 1.032869061645803
-        },
-        {
-            "x": 7.659971709721358,
-            "y": 4.404806001492882,
-            "heading": -3.141361954512953,
-            "angularVelocity": -0.000003105943453461285,
-            "velocityX": 0.8804830987757537,
-            "velocityY": -3.679045774191308,
-            "timestamp": 1.1066454231919318
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.02471459188852694,
-            "velocityX": 0.271473001092883,
-            "velocityY": -3.7655150738110477,
-            "timestamp": 1.1804217847380607
-        },
-        {
-            "x": 7.658280036401298,
-            "y": 3.864604229609803,
-            "heading": -3.146329645491869,
-            "angularVelocity": -0.04501004755691485,
-            "velocityX": -0.3109132979403334,
-            "velocityY": -3.756099036118669,
-            "timestamp": 1.2502803714970885
-        },
-        {
-            "x": 7.5962523104298665,
-            "y": 3.6077169181356963,
-            "heading": -3.1463327455058754,
-            "angularVelocity": -0.00004437556140498802,
-            "velocityX": -0.8879041052661434,
-            "velocityY": -3.677247470811584,
-            "timestamp": 1.3201389582561163
-        },
-        {
-            "x": 7.495521811257821,
-            "y": 3.363396565109804,
-            "heading": -3.1463328962340493,
-            "angularVelocity": -0.0000021576184275855436,
-            "velocityX": -1.4419200823444587,
-            "velocityY": -3.4973560783394904,
-            "timestamp": 1.389997545015144
-        },
-        {
-            "x": 7.358477102372377,
-            "y": 3.1374367675877326,
-            "heading": -3.146332979999805,
-            "angularVelocity": -0.000001199076011915671,
-            "velocityX": -1.9617446507781449,
-            "velocityY": -3.234531472866801,
-            "timestamp": 1.4598561317741718
-        },
-        {
-            "x": 7.188367445952169,
-            "y": 2.9351948516310506,
-            "heading": -3.1463330407624492,
-            "angularVelocity": -8.697949218469567e-7,
-            "velocityX": -2.4350572250622284,
-            "velocityY": -2.895018713365922,
-            "timestamp": 1.5297147185331996
-        },
-        {
-            "x": 6.989226108875858,
-            "y": 2.761465700266129,
-            "heading": -3.146333092561237,
-            "angularVelocity": -7.414806172090105e-7,
-            "velocityX": -2.8506350659974578,
-            "velocityY": -2.486868965216661,
-            "timestamp": 1.5995733052922274
-        },
-        {
-            "x": 6.765782539563676,
-            "y": 2.620355593747368,
-            "heading": -3.14633314264619,
-            "angularVelocity": -7.169476958064807e-7,
-            "velocityX": -3.19851259062737,
-            "velocityY": -2.0199393240734156,
-            "timestamp": 1.6694318920512552
-        },
-        {
-            "x": 6.5419983548650125,
-            "y": 2.479786288735506,
-            "heading": -3.1463331927321803,
-            "angularVelocity": -7.169625493020222e-7,
-            "velocityX": -3.203388374726096,
-            "velocityY": -2.012197949219134,
-            "timestamp": 1.739290478810283
-        },
-        {
-            "x": 6.318214167123251,
-            "y": 2.339216988568208,
-            "heading": -3.1463332428181707,
-            "angularVelocity": -7.16962547152982e-7,
-            "velocityX": -3.2033884182869192,
-            "velocityY": -2.012197879870968,
-            "timestamp": 1.8091490655693108
-        },
-        {
-            "x": 6.094429979379188,
-            "y": 2.198647688404573,
-            "heading": -3.1463332929041607,
-            "angularVelocity": -7.1696254162218e-7,
-            "velocityX": -3.2033884183198493,
-            "velocityY": -2.0121978798185434,
-            "timestamp": 1.8790076523283386
-        },
-        {
-            "x": 5.870645537599717,
-            "y": 2.05807879266491,
-            "heading": -3.1463333429939095,
-            "angularVelocity": -7.170163458750797e-7,
-            "velocityX": -3.203392054743378,
-            "velocityY": -2.012192090637989,
-            "timestamp": 1.9488662390873663
-        },
-        {
-            "x": 5.673381673927696,
-            "y": 1.935533968246103,
-            "heading": -3.225487827303808,
-            "angularVelocity": -1.1330673576740993,
-            "velocityX": -2.8237597240904497,
-            "velocityY": -1.754184132603728,
-            "timestamp": 2.018724825846394
-        },
-        {
-            "x": 5.505993389711308,
-            "y": 1.8298743063416731,
-            "heading": -3.3794217717080413,
-            "angularVelocity": -2.2035078512999076,
-            "velocityX": -2.396101781929577,
-            "velocityY": -1.512479235643509,
-            "timestamp": 2.088583412605422
-        },
-        {
-            "x": 5.373136379336833,
-            "y": 1.745695395482932,
-            "heading": -3.5283087444702135,
-            "angularVelocity": -2.1312623067475838,
-            "velocityX": -1.9017992853585055,
-            "velocityY": -1.2049901775011624,
-            "timestamp": 2.15844199936445
-        },
-        {
-            "x": 5.274205775679982,
-            "y": 1.6827613854161347,
-            "heading": -3.651053741383974,
-            "angularVelocity": -1.7570495283158336,
-            "velocityX": -1.4161552394139385,
-            "velocityY": -0.9008772290782794,
-            "timestamp": 2.228300586123478
-        },
-        {
-            "x": 5.2086365166677675,
-            "y": 1.6409004652612855,
-            "heading": -3.73798062119163,
-            "angularVelocity": -1.2443263432669185,
-            "velocityX": -0.9385998494128075,
-            "velocityY": -0.5992236902708281,
-            "timestamp": 2.298159172882506
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -3.7831853071795862,
-            "angularVelocity": -0.6470884695089293,
-            "velocityX": -0.46717974384944866,
-            "velocityY": -0.29918248036394063,
-            "timestamp": 2.368017759641534
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -3.7831853071795862,
-            "angularVelocity": 0,
-            "velocityX": -3.585209340915008e-32,
-            "velocityY": 0,
-            "timestamp": 2.437876346400562
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S4.traj
deleted file mode 100644
index 4d0afc34..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3 To S4.traj	
+++ /dev/null
@@ -1,319 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": -4.965302317163387e-33,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.214448464194176,
-            "y": 6.6065493619277555,
-            "heading": -2.899983532805956,
-            "angularVelocity": -0.2708663369562031,
-            "velocityX": 0.5211488258354376,
-            "velocityY": -0.31786113574578073,
-            "timestamp": 0.0737763615461288
-        },
-        {
-            "x": 5.29140014559419,
-            "y": 6.559619691972876,
-            "heading": -2.9381129725152273,
-            "angularVelocity": -0.5168246157738562,
-            "velocityX": 1.0430398001113212,
-            "velocityY": -0.636107134743106,
-            "timestamp": 0.1475527230922576
-        },
-        {
-            "x": 5.4069285579459345,
-            "y": 6.489171539636289,
-            "heading": -2.991543589570423,
-            "angularVelocity": -0.7242240730696432,
-            "velocityX": 1.5659272147693208,
-            "velocityY": -0.9548878646250154,
-            "timestamp": 0.22132908463838638
-        },
-        {
-            "x": 5.5611322195100845,
-            "y": 6.395152264966145,
-            "heading": -3.0552740925010653,
-            "angularVelocity": -0.8638336398684346,
-            "velocityX": 2.090149992931468,
-            "velocityY": -1.274382101526624,
-            "timestamp": 0.2951054461845152
-        },
-        {
-            "x": 5.754097394020428,
-            "y": 6.277527348774539,
-            "heading": -3.118382886347054,
-            "angularVelocity": -0.8554066983437507,
-            "velocityX": 2.615542030894161,
-            "velocityY": -1.5943442279687556,
-            "timestamp": 0.36888180773064394
-        },
-        {
-            "x": 5.984744637231227,
-            "y": 6.137241174757932,
-            "heading": -3.141361477565195,
-            "angularVelocity": -0.3114627874915312,
-            "velocityX": 3.126302766592624,
-            "velocityY": -1.9015057272632323,
-            "timestamp": 0.4426581692767727
-        },
-        {
-            "x": 6.222972456839802,
-            "y": 5.99184648556835,
-            "heading": -3.141361501066067,
-            "angularVelocity": -3.1854203464323085e-7,
-            "velocityX": 3.2290535154627107,
-            "velocityY": -1.9707489789757882,
-            "timestamp": 0.5164345308229015
-        },
-        {
-            "x": 6.461200274584459,
-            "y": 5.84645179332473,
-            "heading": -3.141361524566912,
-            "angularVelocity": -3.1854166338718743e-7,
-            "velocityX": 3.229053490198278,
-            "velocityY": -1.9707490203716698,
-            "timestamp": 0.5902108923690302
-        },
-        {
-            "x": 6.699428092323813,
-            "y": 5.701057101072421,
-            "heading": -3.141361548067757,
-            "angularVelocity": -3.1854166408242085e-7,
-            "velocityX": 3.229053490126394,
-            "velocityY": -1.9707490204894516,
-            "timestamp": 0.663987253915159
-        },
-        {
-            "x": 6.937655250211389,
-            "y": 5.555661327664803,
-            "heading": -3.1413615715686114,
-            "angularVelocity": -3.1854179676885927e-7,
-            "velocityX": 3.229044546180602,
-            "velocityY": -1.9707636749842845,
-            "timestamp": 0.7377636154612878
-        },
-        {
-            "x": 7.1538386137488,
-            "y": 5.379148261525069,
-            "heading": -3.141361595947866,
-            "angularVelocity": -3.304480485706385e-7,
-            "velocityX": 2.9302524413899382,
-            "velocityY": -2.3925423054289428,
-            "timestamp": 0.8115399770074165
-        },
-        {
-            "x": 7.338555522796062,
-            "y": 5.1699312191681095,
-            "heading": -3.1413616242387703,
-            "angularVelocity": -3.834684085405442e-7,
-            "velocityX": 2.5037411059064913,
-            "velocityY": -2.835827600770833,
-            "timestamp": 0.8853163385535453
-        },
-        {
-            "x": 7.486921372318085,
-            "y": 4.933542391468771,
-            "heading": -3.14136166220073,
-            "angularVelocity": -5.145545098256939e-7,
-            "velocityX": 2.011021503537517,
-            "velocityY": -3.2041269418191103,
-            "timestamp": 0.9590927000996741
-        },
-        {
-            "x": 7.595012870290821,
-            "y": 4.676232612674377,
-            "heading": -3.1413617253677457,
-            "angularVelocity": -8.561958656088552e-7,
-            "velocityX": 1.4651237294367088,
-            "velocityY": -3.4876994934686665,
-            "timestamp": 1.032869061645803
-        },
-        {
-            "x": 7.659971709721358,
-            "y": 4.404806001492882,
-            "heading": -3.141361954512953,
-            "angularVelocity": -0.000003105943453461285,
-            "velocityX": 0.8804830987757537,
-            "velocityY": -3.679045774191308,
-            "timestamp": 1.1066454231919318
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.02471459188852694,
-            "velocityX": 0.271473001092883,
-            "velocityY": -3.7655150738110477,
-            "timestamp": 1.1804217847380607
-        },
-        {
-            "x": 7.658280036401298,
-            "y": 3.864604229609803,
-            "heading": -3.146329645491869,
-            "angularVelocity": -0.04501004755691485,
-            "velocityX": -0.3109132979403334,
-            "velocityY": -3.756099036118669,
-            "timestamp": 1.2502803714970885
-        },
-        {
-            "x": 7.5962523104298665,
-            "y": 3.6077169181356963,
-            "heading": -3.1463327455058754,
-            "angularVelocity": -0.00004437556140498802,
-            "velocityX": -0.8879041052661434,
-            "velocityY": -3.677247470811584,
-            "timestamp": 1.3201389582561163
-        },
-        {
-            "x": 7.495521811257821,
-            "y": 3.363396565109804,
-            "heading": -3.1463328962340493,
-            "angularVelocity": -0.0000021576184275855436,
-            "velocityX": -1.4419200823444587,
-            "velocityY": -3.4973560783394904,
-            "timestamp": 1.389997545015144
-        },
-        {
-            "x": 7.358477102372377,
-            "y": 3.1374367675877326,
-            "heading": -3.146332979999805,
-            "angularVelocity": -0.000001199076011915671,
-            "velocityX": -1.9617446507781449,
-            "velocityY": -3.234531472866801,
-            "timestamp": 1.4598561317741718
-        },
-        {
-            "x": 7.188367445952169,
-            "y": 2.9351948516310506,
-            "heading": -3.1463330407624492,
-            "angularVelocity": -8.697949218469567e-7,
-            "velocityX": -2.4350572250622284,
-            "velocityY": -2.895018713365922,
-            "timestamp": 1.5297147185331996
-        },
-        {
-            "x": 6.989226108875858,
-            "y": 2.761465700266129,
-            "heading": -3.146333092561237,
-            "angularVelocity": -7.414806172090105e-7,
-            "velocityX": -2.8506350659974578,
-            "velocityY": -2.486868965216661,
-            "timestamp": 1.5995733052922274
-        },
-        {
-            "x": 6.765782539563676,
-            "y": 2.620355593747368,
-            "heading": -3.14633314264619,
-            "angularVelocity": -7.169476958064807e-7,
-            "velocityX": -3.19851259062737,
-            "velocityY": -2.0199393240734156,
-            "timestamp": 1.6694318920512552
-        },
-        {
-            "x": 6.5419983548650125,
-            "y": 2.479786288735506,
-            "heading": -3.1463331927321803,
-            "angularVelocity": -7.169625493020222e-7,
-            "velocityX": -3.203388374726096,
-            "velocityY": -2.012197949219134,
-            "timestamp": 1.739290478810283
-        },
-        {
-            "x": 6.318214167123251,
-            "y": 2.339216988568208,
-            "heading": -3.1463332428181707,
-            "angularVelocity": -7.16962547152982e-7,
-            "velocityX": -3.2033884182869192,
-            "velocityY": -2.012197879870968,
-            "timestamp": 1.8091490655693108
-        },
-        {
-            "x": 6.094429979379188,
-            "y": 2.198647688404573,
-            "heading": -3.1463332929041607,
-            "angularVelocity": -7.1696254162218e-7,
-            "velocityX": -3.2033884183198493,
-            "velocityY": -2.0121978798185434,
-            "timestamp": 1.8790076523283386
-        },
-        {
-            "x": 5.870645537599717,
-            "y": 2.05807879266491,
-            "heading": -3.1463333429939095,
-            "angularVelocity": -7.170163458750797e-7,
-            "velocityX": -3.203392054743378,
-            "velocityY": -2.012192090637989,
-            "timestamp": 1.9488662390873663
-        },
-        {
-            "x": 5.673381673927696,
-            "y": 1.935533968246103,
-            "heading": -3.225487827303808,
-            "angularVelocity": -1.1330673576740993,
-            "velocityX": -2.8237597240904497,
-            "velocityY": -1.754184132603728,
-            "timestamp": 2.018724825846394
-        },
-        {
-            "x": 5.505993389711308,
-            "y": 1.8298743063416731,
-            "heading": -3.3794217717080413,
-            "angularVelocity": -2.2035078512999076,
-            "velocityX": -2.396101781929577,
-            "velocityY": -1.512479235643509,
-            "timestamp": 2.088583412605422
-        },
-        {
-            "x": 5.373136379336833,
-            "y": 1.745695395482932,
-            "heading": -3.5283087444702135,
-            "angularVelocity": -2.1312623067475838,
-            "velocityX": -1.9017992853585055,
-            "velocityY": -1.2049901775011624,
-            "timestamp": 2.15844199936445
-        },
-        {
-            "x": 5.274205775679982,
-            "y": 1.6827613854161347,
-            "heading": -3.651053741383974,
-            "angularVelocity": -1.7570495283158336,
-            "velocityX": -1.4161552394139385,
-            "velocityY": -0.9008772290782794,
-            "timestamp": 2.228300586123478
-        },
-        {
-            "x": 5.2086365166677675,
-            "y": 1.6409004652612855,
-            "heading": -3.73798062119163,
-            "angularVelocity": -1.2443263432669185,
-            "velocityX": -0.9385998494128075,
-            "velocityY": -0.5992236902708281,
-            "timestamp": 2.298159172882506
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -3.7831853071795862,
-            "angularVelocity": -0.6470884695089293,
-            "velocityX": -0.46717974384944866,
-            "velocityY": -0.29918248036394063,
-            "timestamp": 2.368017759641534
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -3.7831853071795862,
-            "angularVelocity": 0,
-            "velocityX": -3.585209340915008e-32,
-            "velocityY": 0,
-            "timestamp": 2.437876346400562
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj
deleted file mode 100644
index 7043ffc8..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 1.76189972977325e-30,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.102979050763036,
-            "y": 6.633224483540667,
-            "heading": -2.873498740443943,
-            "angularVelocity": 0.06942713881135329,
-            "velocityX": -0.779792828619582,
-            "velocityY": 0.034434352980739984,
-            "timestamp": 0.09364147316695418
-        },
-        {
-            "x": 4.956937154228187,
-            "y": 6.639673450002842,
-            "heading": -2.8604964293774233,
-            "angularVelocity": 0.138852055897772,
-            "velocityX": -1.559585636531679,
-            "velocityY": 0.06886869934946069,
-            "timestamp": 0.18728294633390835
-        },
-        {
-            "x": 4.73787431448253,
-            "y": 6.649346898579915,
-            "heading": -2.840993426039055,
-            "angularVelocity": 0.20827313666458666,
-            "velocityX": -2.339378400797736,
-            "velocityY": 0.10330303710435738,
-            "timestamp": 0.28092441950086255
-        },
-        {
-            "x": 4.4457905464926135,
-            "y": 6.6622448281102296,
-            "heading": -2.814990271784832,
-            "angularVelocity": 0.27768843627487766,
-            "velocityX": -3.1191710052355526,
-            "velocityY": 0.13773736245389714,
-            "timestamp": 0.3745658926678167
-        },
-        {
-            "x": 4.091895298236837,
-            "y": 6.6778724148973385,
-            "heading": -2.8149902687925503,
-            "angularVelocity": 3.195462281879478e-8,
-            "velocityX": -3.779257590542345,
-            "velocityY": 0.16688745123911564,
-            "timestamp": 0.46820736583477085
-        },
-        {
-            "x": 3.7380000499851023,
-            "y": 6.693500001778431,
-            "heading": -2.814990265800417,
-            "angularVelocity": 3.1953066955491674e-8,
-            "velocityX": -3.779257590499122,
-            "velocityY": 0.16688745224276955,
-            "timestamp": 0.561848839001725
-        },
-        {
-            "x": 3.3841048017333675,
-            "y": 6.709127588659523,
-            "heading": -2.8149902628082843,
-            "angularVelocity": 3.1953067022179835e-8,
-            "velocityX": -3.779257590499122,
-            "velocityY": 0.1668874522427702,
-            "timestamp": 0.6554903121686791
-        },
-        {
-            "x": 3.030209553477566,
-            "y": 6.724755175446062,
-            "heading": -2.8149902598160015,
-            "angularVelocity": 3.195463264613959e-8,
-            "velocityX": -3.7792575905426142,
-            "velocityY": 0.16688745123301843,
-            "timestamp": 0.7491317853356333
-        },
-        {
-            "x": 2.7381257501719745,
-            "y": 6.737653104232043,
-            "heading": -2.7889939719160233,
-            "angularVelocity": 0.27761511027933256,
-            "velocityX": -3.1191713823726643,
-            "velocityY": 0.13773735450514524,
-            "timestamp": 0.8427732585025874
-        },
-        {
-            "x": 2.5190628796730365,
-            "y": 6.747326551843937,
-            "heading": -2.769496951872836,
-            "angularVelocity": 0.2082092408823168,
-            "velocityX": -2.3393787292129176,
-            "velocityY": 0.10330302679718467,
-            "timestamp": 0.9364147316695416
-        },
-        {
-            "x": 2.373020960810499,
-            "y": 6.753775517241458,
-            "heading": -2.7564989943923397,
-            "angularVelocity": 0.13880556382672637,
-            "velocityX": -1.5595858749697047,
-            "velocityY": 0.06886868797998212,
-            "timestamp": 1.0300562048364958
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0.06940294906251296,
-            "velocityX": -0.779792952213697,
-            "velocityY": 0.03443434462839962,
-            "timestamp": 1.12369767800345
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 1.276683149554956e-30,
-            "velocityX": 2.9209786111644144e-32,
-            "velocityY": 0,
-            "timestamp": 1.2173391511704041
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.traj
deleted file mode 100644
index 7043ffc8..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 1.76189972977325e-30,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.102979050763036,
-            "y": 6.633224483540667,
-            "heading": -2.873498740443943,
-            "angularVelocity": 0.06942713881135329,
-            "velocityX": -0.779792828619582,
-            "velocityY": 0.034434352980739984,
-            "timestamp": 0.09364147316695418
-        },
-        {
-            "x": 4.956937154228187,
-            "y": 6.639673450002842,
-            "heading": -2.8604964293774233,
-            "angularVelocity": 0.138852055897772,
-            "velocityX": -1.559585636531679,
-            "velocityY": 0.06886869934946069,
-            "timestamp": 0.18728294633390835
-        },
-        {
-            "x": 4.73787431448253,
-            "y": 6.649346898579915,
-            "heading": -2.840993426039055,
-            "angularVelocity": 0.20827313666458666,
-            "velocityX": -2.339378400797736,
-            "velocityY": 0.10330303710435738,
-            "timestamp": 0.28092441950086255
-        },
-        {
-            "x": 4.4457905464926135,
-            "y": 6.6622448281102296,
-            "heading": -2.814990271784832,
-            "angularVelocity": 0.27768843627487766,
-            "velocityX": -3.1191710052355526,
-            "velocityY": 0.13773736245389714,
-            "timestamp": 0.3745658926678167
-        },
-        {
-            "x": 4.091895298236837,
-            "y": 6.6778724148973385,
-            "heading": -2.8149902687925503,
-            "angularVelocity": 3.195462281879478e-8,
-            "velocityX": -3.779257590542345,
-            "velocityY": 0.16688745123911564,
-            "timestamp": 0.46820736583477085
-        },
-        {
-            "x": 3.7380000499851023,
-            "y": 6.693500001778431,
-            "heading": -2.814990265800417,
-            "angularVelocity": 3.1953066955491674e-8,
-            "velocityX": -3.779257590499122,
-            "velocityY": 0.16688745224276955,
-            "timestamp": 0.561848839001725
-        },
-        {
-            "x": 3.3841048017333675,
-            "y": 6.709127588659523,
-            "heading": -2.8149902628082843,
-            "angularVelocity": 3.1953067022179835e-8,
-            "velocityX": -3.779257590499122,
-            "velocityY": 0.1668874522427702,
-            "timestamp": 0.6554903121686791
-        },
-        {
-            "x": 3.030209553477566,
-            "y": 6.724755175446062,
-            "heading": -2.8149902598160015,
-            "angularVelocity": 3.195463264613959e-8,
-            "velocityX": -3.7792575905426142,
-            "velocityY": 0.16688745123301843,
-            "timestamp": 0.7491317853356333
-        },
-        {
-            "x": 2.7381257501719745,
-            "y": 6.737653104232043,
-            "heading": -2.7889939719160233,
-            "angularVelocity": 0.27761511027933256,
-            "velocityX": -3.1191713823726643,
-            "velocityY": 0.13773735450514524,
-            "timestamp": 0.8427732585025874
-        },
-        {
-            "x": 2.5190628796730365,
-            "y": 6.747326551843937,
-            "heading": -2.769496951872836,
-            "angularVelocity": 0.2082092408823168,
-            "velocityX": -2.3393787292129176,
-            "velocityY": 0.10330302679718467,
-            "timestamp": 0.9364147316695416
-        },
-        {
-            "x": 2.373020960810499,
-            "y": 6.753775517241458,
-            "heading": -2.7564989943923397,
-            "angularVelocity": 0.13880556382672637,
-            "velocityX": -1.5595858749697047,
-            "velocityY": 0.06886868797998212,
-            "timestamp": 1.0300562048364958
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0.06940294906251296,
-            "velocityX": -0.779792952213697,
-            "velocityY": 0.03443434462839962,
-            "timestamp": 1.12369767800345
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 1.276683149554956e-30,
-            "velocityX": 2.9209786111644144e-32,
-            "velocityY": 0,
-            "timestamp": 1.2173391511704041
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj
deleted file mode 100644
index 051fce47..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -1.6791892318677926e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 5.11892498821706,
-            "y": 6.609128402762145,
-            "heading": -2.8887795529976006,
-            "angularVelocity": -0.10279668380897901,
-            "velocityX": -0.6682711456093472,
-            "velocityY": -0.24437815711512118,
-            "timestamp": 0.08540696715387537
-        },
-        {
-            "x": 5.004774958946586,
-            "y": 6.5673852200358445,
-            "heading": -2.90633824182768,
-            "angularVelocity": -0.2055884831789443,
-            "velocityX": -1.3365423580117657,
-            "velocityY": -0.48875617666053534,
-            "timestamp": 0.17081393430775074
-        },
-        {
-            "x": 4.833549902172449,
-            "y": 6.504770468021476,
-            "heading": -2.93267521126463,
-            "angularVelocity": -0.30837026901446724,
-            "velocityX": -2.004813687689499,
-            "velocityY": -0.7331340065214745,
-            "timestamp": 0.2562209014616261
-        },
-        {
-            "x": 4.605249804128149,
-            "y": 6.4212841662523426,
-            "heading": -2.9677889698411977,
-            "angularVelocity": -0.4111345917869177,
-            "velocityX": -2.6730851785543184,
-            "velocityY": -0.9775116076738672,
-            "timestamp": 0.3416278686155015
-        },
-        {
-            "x": 4.319874653666952,
-            "y": 6.316926338018247,
-            "heading": -3.0116770321088655,
-            "angularVelocity": -0.5138698133209222,
-            "velocityX": -3.341356799932311,
-            "velocityY": -1.2218889361341865,
-            "timestamp": 0.42703483576937684
-        },
-        {
-            "x": 4.016437784224315,
-            "y": 6.205962942906548,
-            "heading": -3.011677037359934,
-            "angularVelocity": -6.148290543535782e-8,
-            "velocityX": -3.5528350854087023,
-            "velocityY": -1.2992311846383482,
-            "timestamp": 0.5124418029232523
-        },
-        {
-            "x": 3.713000914796963,
-            "y": 6.094999547753008,
-            "heading": -3.0116770426109816,
-            "angularVelocity": -6.148266047339023e-8,
-            "velocityX": -3.55283508522973,
-            "velocityY": -1.299231185128267,
-            "timestamp": 0.5978487700771277
-        },
-        {
-            "x": 3.4095640453696117,
-            "y": 5.984036152599468,
-            "heading": -3.011677047862029,
-            "angularVelocity": -6.148266099885158e-8,
-            "velocityX": -3.5528350852297295,
-            "velocityY": -1.2992311851282667,
-            "timestamp": 0.6832557372310031
-        },
-        {
-            "x": 3.1061271759243305,
-            "y": 5.873072757495,
-            "heading": -3.011677053113097,
-            "angularVelocity": -6.1482904284003e-8,
-            "velocityX": -3.5528350854396615,
-            "velocityY": -1.2992311845536897,
-            "timestamp": 0.7686627043848785
-        },
-        {
-            "x": 2.820751491800317,
-            "y": 5.768715178575412,
-            "heading": -3.055515411809828,
-            "angularVelocity": -0.513287851771491,
-            "velocityX": -3.341363048401663,
-            "velocityY": -1.2218860169986983,
-            "timestamp": 0.8540696715387539
-        },
-        {
-            "x": 2.592450909011727,
-            "y": 5.685229109517301,
-            "heading": -3.0905842313047507,
-            "angularVelocity": -0.41060841595908354,
-            "velocityX": -2.673090854253924,
-            "velocityY": -0.977508882942736,
-            "timestamp": 0.9394766386926293
-        },
-        {
-            "x": 2.421225457633144,
-            "y": 5.622614555370758,
-            "heading": -3.116884896329009,
-            "angularVelocity": -0.30794519347436133,
-            "velocityX": -2.00481830797353,
-            "velocityY": -0.7331316897570141,
-            "timestamp": 1.0248836058465047
-        },
-        {
-            "x": 2.3070751524559587,
-            "y": 5.5808715184580695,
-            "heading": -3.134418439279544,
-            "angularVelocity": -0.2052940589606165,
-            "velocityX": -1.336545588505958,
-            "velocityY": -0.4887544693804859,
-            "timestamp": 1.11029057300038
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.1026481584839197,
-            "velocityX": -0.668272792700015,
-            "velocityY": -0.24437723471044392,
-            "timestamp": 1.1956975401542553
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": -2.233003674732857e-32,
-            "velocityX": 0,
-            "velocityY": 6.615380928603251e-34,
-            "timestamp": 1.2811045073081306
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.traj
deleted file mode 100644
index 051fce47..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -1.6791892318677926e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 5.11892498821706,
-            "y": 6.609128402762145,
-            "heading": -2.8887795529976006,
-            "angularVelocity": -0.10279668380897901,
-            "velocityX": -0.6682711456093472,
-            "velocityY": -0.24437815711512118,
-            "timestamp": 0.08540696715387537
-        },
-        {
-            "x": 5.004774958946586,
-            "y": 6.5673852200358445,
-            "heading": -2.90633824182768,
-            "angularVelocity": -0.2055884831789443,
-            "velocityX": -1.3365423580117657,
-            "velocityY": -0.48875617666053534,
-            "timestamp": 0.17081393430775074
-        },
-        {
-            "x": 4.833549902172449,
-            "y": 6.504770468021476,
-            "heading": -2.93267521126463,
-            "angularVelocity": -0.30837026901446724,
-            "velocityX": -2.004813687689499,
-            "velocityY": -0.7331340065214745,
-            "timestamp": 0.2562209014616261
-        },
-        {
-            "x": 4.605249804128149,
-            "y": 6.4212841662523426,
-            "heading": -2.9677889698411977,
-            "angularVelocity": -0.4111345917869177,
-            "velocityX": -2.6730851785543184,
-            "velocityY": -0.9775116076738672,
-            "timestamp": 0.3416278686155015
-        },
-        {
-            "x": 4.319874653666952,
-            "y": 6.316926338018247,
-            "heading": -3.0116770321088655,
-            "angularVelocity": -0.5138698133209222,
-            "velocityX": -3.341356799932311,
-            "velocityY": -1.2218889361341865,
-            "timestamp": 0.42703483576937684
-        },
-        {
-            "x": 4.016437784224315,
-            "y": 6.205962942906548,
-            "heading": -3.011677037359934,
-            "angularVelocity": -6.148290543535782e-8,
-            "velocityX": -3.5528350854087023,
-            "velocityY": -1.2992311846383482,
-            "timestamp": 0.5124418029232523
-        },
-        {
-            "x": 3.713000914796963,
-            "y": 6.094999547753008,
-            "heading": -3.0116770426109816,
-            "angularVelocity": -6.148266047339023e-8,
-            "velocityX": -3.55283508522973,
-            "velocityY": -1.299231185128267,
-            "timestamp": 0.5978487700771277
-        },
-        {
-            "x": 3.4095640453696117,
-            "y": 5.984036152599468,
-            "heading": -3.011677047862029,
-            "angularVelocity": -6.148266099885158e-8,
-            "velocityX": -3.5528350852297295,
-            "velocityY": -1.2992311851282667,
-            "timestamp": 0.6832557372310031
-        },
-        {
-            "x": 3.1061271759243305,
-            "y": 5.873072757495,
-            "heading": -3.011677053113097,
-            "angularVelocity": -6.1482904284003e-8,
-            "velocityX": -3.5528350854396615,
-            "velocityY": -1.2992311845536897,
-            "timestamp": 0.7686627043848785
-        },
-        {
-            "x": 2.820751491800317,
-            "y": 5.768715178575412,
-            "heading": -3.055515411809828,
-            "angularVelocity": -0.513287851771491,
-            "velocityX": -3.341363048401663,
-            "velocityY": -1.2218860169986983,
-            "timestamp": 0.8540696715387539
-        },
-        {
-            "x": 2.592450909011727,
-            "y": 5.685229109517301,
-            "heading": -3.0905842313047507,
-            "angularVelocity": -0.41060841595908354,
-            "velocityX": -2.673090854253924,
-            "velocityY": -0.977508882942736,
-            "timestamp": 0.9394766386926293
-        },
-        {
-            "x": 2.421225457633144,
-            "y": 5.622614555370758,
-            "heading": -3.116884896329009,
-            "angularVelocity": -0.30794519347436133,
-            "velocityX": -2.00481830797353,
-            "velocityY": -0.7331316897570141,
-            "timestamp": 1.0248836058465047
-        },
-        {
-            "x": 2.3070751524559587,
-            "y": 5.5808715184580695,
-            "heading": -3.134418439279544,
-            "angularVelocity": -0.2052940589606165,
-            "velocityX": -1.336545588505958,
-            "velocityY": -0.4887544693804859,
-            "timestamp": 1.11029057300038
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.1026481584839197,
-            "velocityX": -0.668272792700015,
-            "velocityY": -0.24437723471044392,
-            "timestamp": 1.1956975401542553
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": -2.233003674732857e-32,
-            "velocityX": 0,
-            "velocityY": 6.615380928603251e-34,
-            "timestamp": 1.2811045073081306
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S1.1.traj
deleted file mode 100644
index eb6a515e..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S1.1.traj	
+++ /dev/null
@@ -1,265 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": -8.153039521410402e-32,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.331992841648679,
-            "y": 5.5918650858232235,
-            "heading": 3.133856142863887,
-            "angularVelocity": -0.06434641956265956,
-            "velocityX": 0.7225822862075633,
-            "velocityY": 0.3337323991032911,
-            "timestamp": 0.09548094793574066
-        },
-        {
-            "x": 4.469978522807239,
-            "y": 5.655595256777472,
-            "heading": 3.121568622421363,
-            "angularVelocity": -0.12869080908995395,
-            "velocityX": 1.445164550014868,
-            "velocityY": 0.6674647909569773,
-            "timestamp": 0.19096189587148132
-        },
-        {
-            "x": 4.676957039051013,
-            "y": 5.751190511387417,
-            "heading": 3.1031377745555058,
-            "angularVelocity": -0.19303168081514768,
-            "velocityX": 2.1677467674813493,
-            "velocityY": 1.0011971673591054,
-            "timestamp": 0.28644284380722196
-        },
-        {
-            "x": 4.9529283757083125,
-            "y": 5.878650843878014,
-            "heading": 3.078564198952369,
-            "angularVelocity": -0.2573662718522563,
-            "velocityX": 2.890328831286909,
-            "velocityY": 1.334929483277519,
-            "timestamp": 0.38192379174296265
-        },
-        {
-            "x": 5.280841898732695,
-            "y": 6.030101358943919,
-            "heading": 3.0785641960647663,
-            "angularVelocity": -3.024270852224024e-8,
-            "velocityX": 3.4343345988250538,
-            "velocityY": 1.5861857086708926,
-            "timestamp": 0.47740473967870334
-        },
-        {
-            "x": 5.608755421663104,
-            "y": 6.181551874214712,
-            "heading": 3.078564193177445,
-            "angularVelocity": -3.023976799325844e-8,
-            "velocityX": 3.434334597840841,
-            "velocityY": 1.5861857108167463,
-            "timestamp": 0.572885687614444
-        },
-        {
-            "x": 5.936668944593511,
-            "y": 6.333002389485506,
-            "heading": 3.0785641902901233,
-            "angularVelocity": -3.023976796721421e-8,
-            "velocityX": 3.434334597840832,
-            "velocityY": 1.5861857108167654,
-            "timestamp": 0.6683666355501847
-        },
-        {
-            "x": 6.26458246752392,
-            "y": 6.484452904756295,
-            "heading": 3.078564187402802,
-            "angularVelocity": -3.023976775003668e-8,
-            "velocityX": 3.434334597840855,
-            "velocityY": 1.5861857108167163,
-            "timestamp": 0.7638475834859254
-        },
-        {
-            "x": 6.592495990360559,
-            "y": 6.635903420230111,
-            "heading": 3.0785641845154803,
-            "angularVelocity": -3.02397677343441e-8,
-            "velocityX": 3.434334596858777,
-            "velocityY": 1.586185712943067,
-            "timestamp": 0.8593285314216661
-        },
-        {
-            "x": 6.920387522031495,
-            "y": 6.787401540858459,
-            "heading": 3.0785641816279576,
-            "angularVelocity": -3.0241868882472936e-8,
-            "velocityX": 3.4341042769240095,
-            "velocityY": 1.5866842957027985,
-            "timestamp": 0.9548094793574068
-        },
-        {
-            "x": 7.210301641874205,
-            "y": 6.987038475991185,
-            "heading": 3.078564386301996,
-            "angularVelocity": 0.000002143611293414107,
-            "velocityX": 3.036355693052319,
-            "velocityY": 2.0908562330876395,
-            "timestamp": 1.0502904272931475
-        },
-        {
-            "x": 7.431218296435779,
-            "y": 7.154820398489436,
-            "heading": 3.084707768304825,
-            "angularVelocity": 0.06434144335228988,
-            "velocityX": 2.3137249821846093,
-            "velocityY": 1.757229333447492,
-            "timestamp": 1.145771375228888
-        },
-        {
-            "x": 7.58313979167575,
-            "y": 7.2907422724167805,
-            "heading": 3.096994605047932,
-            "angularVelocity": 0.12868364850522696,
-            "velocityX": 1.5911184223080599,
-            "velocityY": 1.4235496909665903,
-            "timestamp": 1.2412523231646286
-        },
-        {
-            "x": 7.666066897511305,
-            "y": 7.39480241917028,
-            "heading": 3.11542521266587,
-            "angularVelocity": 0.19302916462815903,
-            "velocityX": 0.8685199260002638,
-            "velocityY": 1.0898524679843054,
-            "timestamp": 1.3367332711003692
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 0.2573789626666942,
-            "velocityX": 0.145925472986578,
-            "velocityY": 0.756146460527717,
-            "timestamp": 1.4322142190361098
-        },
-        {
-            "x": 7.628765464766867,
-            "y": 7.5070371939069025,
-            "heading": 3.1695683378543595,
-            "angularVelocity": 0.3197138219543324,
-            "velocityX": -0.5539841013770347,
-            "velocityY": 0.4329105121655262,
-            "timestamp": 1.5246979836528902
-        },
-        {
-            "x": 7.512802856979312,
-            "y": 7.517175485453599,
-            "heading": 3.204897367018404,
-            "angularVelocity": 0.3820024986055476,
-            "velocityX": -1.25386989022506,
-            "velocityY": 0.10962239252574857,
-            "timestamp": 1.6171817482696706
-        },
-        {
-            "x": 7.3321158425333,
-            "y": 7.497406833459743,
-            "heading": 3.2459797295297097,
-            "angularVelocity": 0.44421161575322776,
-            "velocityX": -1.9537160408082612,
-            "velocityY": -0.21375267405386408,
-            "timestamp": 1.709665512886451
-        },
-        {
-            "x": 7.086711752516261,
-            "y": 7.447715159830384,
-            "heading": 3.292800204439529,
-            "angularVelocity": 0.5062561532175872,
-            "velocityX": -2.6534829224793675,
-            "velocityY": -0.5373015883889657,
-            "timestamp": 1.8021492775032315
-        },
-        {
-            "x": 6.7766125854356485,
-            "y": 7.3680522721143955,
-            "heading": 3.345311930276191,
-            "angularVelocity": 0.5677939912444252,
-            "velocityX": -3.3530119406998606,
-            "velocityY": -0.8613715936546359,
-            "timestamp": 1.894633042120012
-        },
-        {
-            "x": 6.4587265246711105,
-            "y": 7.22193286725247,
-            "heading": 3.3453119331318786,
-            "angularVelocity": 3.087771911901371e-8,
-            "velocityX": -3.437209353240929,
-            "velocityY": -1.5799465502853158,
-            "timestamp": 1.9871168067367924
-        },
-        {
-            "x": 6.1410584209016115,
-            "y": 7.075340220973225,
-            "heading": 3.3453119359389976,
-            "angularVelocity": 3.035256115645747e-8,
-            "velocityX": -3.434852647767093,
-            "velocityY": -1.5850635718263661,
-            "timestamp": 2.079600571353573
-        },
-        {
-            "x": 5.823390305014834,
-            "y": 6.928747600950052,
-            "heading": 3.3453119387456907,
-            "angularVelocity": 3.034795349843415e-8,
-            "velocityX": -3.4348527787877012,
-            "velocityY": -1.5850632879271098,
-            "timestamp": 2.1720843359703537
-        },
-        {
-            "x": 5.564434200353795,
-            "y": 6.809248569525128,
-            "heading": 3.3684615293972238,
-            "angularVelocity": 0.2503097786643198,
-            "velocityX": -2.8000169082002904,
-            "velocityY": -1.292108208616342,
-            "timestamp": 2.2645681005871343
-        },
-        {
-            "x": 5.3702171046964775,
-            "y": 6.719624287090389,
-            "heading": 3.3858234872314386,
-            "angularVelocity": 0.18772979134186565,
-            "velocityX": -2.1000128667129405,
-            "velocityY": -0.9690812523282153,
-            "timestamp": 2.357051865203915
-        },
-        {
-            "x": 5.240739035958368,
-            "y": 6.659874762854457,
-            "heading": 3.3973980568576057,
-            "angularVelocity": 0.1251524489081645,
-            "velocityX": -1.4000086315106373,
-            "velocityY": -0.6460541964699718,
-            "timestamp": 2.4495356298206956
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.06257585151184072,
-            "velocityX": -0.7000043329401022,
-            "velocityY": -0.3230271061984462,
-            "timestamp": 2.5420193944374763
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 1.89050013704423e-29,
-            "velocityX": -9.26196849914815e-31,
-            "velocityY": -1.067270908871219e-31,
-            "timestamp": 2.634503159054257
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S1.traj
deleted file mode 100644
index eb6a515e..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S1.traj	
+++ /dev/null
@@ -1,265 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": -8.153039521410402e-32,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.331992841648679,
-            "y": 5.5918650858232235,
-            "heading": 3.133856142863887,
-            "angularVelocity": -0.06434641956265956,
-            "velocityX": 0.7225822862075633,
-            "velocityY": 0.3337323991032911,
-            "timestamp": 0.09548094793574066
-        },
-        {
-            "x": 4.469978522807239,
-            "y": 5.655595256777472,
-            "heading": 3.121568622421363,
-            "angularVelocity": -0.12869080908995395,
-            "velocityX": 1.445164550014868,
-            "velocityY": 0.6674647909569773,
-            "timestamp": 0.19096189587148132
-        },
-        {
-            "x": 4.676957039051013,
-            "y": 5.751190511387417,
-            "heading": 3.1031377745555058,
-            "angularVelocity": -0.19303168081514768,
-            "velocityX": 2.1677467674813493,
-            "velocityY": 1.0011971673591054,
-            "timestamp": 0.28644284380722196
-        },
-        {
-            "x": 4.9529283757083125,
-            "y": 5.878650843878014,
-            "heading": 3.078564198952369,
-            "angularVelocity": -0.2573662718522563,
-            "velocityX": 2.890328831286909,
-            "velocityY": 1.334929483277519,
-            "timestamp": 0.38192379174296265
-        },
-        {
-            "x": 5.280841898732695,
-            "y": 6.030101358943919,
-            "heading": 3.0785641960647663,
-            "angularVelocity": -3.024270852224024e-8,
-            "velocityX": 3.4343345988250538,
-            "velocityY": 1.5861857086708926,
-            "timestamp": 0.47740473967870334
-        },
-        {
-            "x": 5.608755421663104,
-            "y": 6.181551874214712,
-            "heading": 3.078564193177445,
-            "angularVelocity": -3.023976799325844e-8,
-            "velocityX": 3.434334597840841,
-            "velocityY": 1.5861857108167463,
-            "timestamp": 0.572885687614444
-        },
-        {
-            "x": 5.936668944593511,
-            "y": 6.333002389485506,
-            "heading": 3.0785641902901233,
-            "angularVelocity": -3.023976796721421e-8,
-            "velocityX": 3.434334597840832,
-            "velocityY": 1.5861857108167654,
-            "timestamp": 0.6683666355501847
-        },
-        {
-            "x": 6.26458246752392,
-            "y": 6.484452904756295,
-            "heading": 3.078564187402802,
-            "angularVelocity": -3.023976775003668e-8,
-            "velocityX": 3.434334597840855,
-            "velocityY": 1.5861857108167163,
-            "timestamp": 0.7638475834859254
-        },
-        {
-            "x": 6.592495990360559,
-            "y": 6.635903420230111,
-            "heading": 3.0785641845154803,
-            "angularVelocity": -3.02397677343441e-8,
-            "velocityX": 3.434334596858777,
-            "velocityY": 1.586185712943067,
-            "timestamp": 0.8593285314216661
-        },
-        {
-            "x": 6.920387522031495,
-            "y": 6.787401540858459,
-            "heading": 3.0785641816279576,
-            "angularVelocity": -3.0241868882472936e-8,
-            "velocityX": 3.4341042769240095,
-            "velocityY": 1.5866842957027985,
-            "timestamp": 0.9548094793574068
-        },
-        {
-            "x": 7.210301641874205,
-            "y": 6.987038475991185,
-            "heading": 3.078564386301996,
-            "angularVelocity": 0.000002143611293414107,
-            "velocityX": 3.036355693052319,
-            "velocityY": 2.0908562330876395,
-            "timestamp": 1.0502904272931475
-        },
-        {
-            "x": 7.431218296435779,
-            "y": 7.154820398489436,
-            "heading": 3.084707768304825,
-            "angularVelocity": 0.06434144335228988,
-            "velocityX": 2.3137249821846093,
-            "velocityY": 1.757229333447492,
-            "timestamp": 1.145771375228888
-        },
-        {
-            "x": 7.58313979167575,
-            "y": 7.2907422724167805,
-            "heading": 3.096994605047932,
-            "angularVelocity": 0.12868364850522696,
-            "velocityX": 1.5911184223080599,
-            "velocityY": 1.4235496909665903,
-            "timestamp": 1.2412523231646286
-        },
-        {
-            "x": 7.666066897511305,
-            "y": 7.39480241917028,
-            "heading": 3.11542521266587,
-            "angularVelocity": 0.19302916462815903,
-            "velocityX": 0.8685199260002638,
-            "velocityY": 1.0898524679843054,
-            "timestamp": 1.3367332711003692
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 0.2573789626666942,
-            "velocityX": 0.145925472986578,
-            "velocityY": 0.756146460527717,
-            "timestamp": 1.4322142190361098
-        },
-        {
-            "x": 7.628765464766867,
-            "y": 7.5070371939069025,
-            "heading": 3.1695683378543595,
-            "angularVelocity": 0.3197138219543324,
-            "velocityX": -0.5539841013770347,
-            "velocityY": 0.4329105121655262,
-            "timestamp": 1.5246979836528902
-        },
-        {
-            "x": 7.512802856979312,
-            "y": 7.517175485453599,
-            "heading": 3.204897367018404,
-            "angularVelocity": 0.3820024986055476,
-            "velocityX": -1.25386989022506,
-            "velocityY": 0.10962239252574857,
-            "timestamp": 1.6171817482696706
-        },
-        {
-            "x": 7.3321158425333,
-            "y": 7.497406833459743,
-            "heading": 3.2459797295297097,
-            "angularVelocity": 0.44421161575322776,
-            "velocityX": -1.9537160408082612,
-            "velocityY": -0.21375267405386408,
-            "timestamp": 1.709665512886451
-        },
-        {
-            "x": 7.086711752516261,
-            "y": 7.447715159830384,
-            "heading": 3.292800204439529,
-            "angularVelocity": 0.5062561532175872,
-            "velocityX": -2.6534829224793675,
-            "velocityY": -0.5373015883889657,
-            "timestamp": 1.8021492775032315
-        },
-        {
-            "x": 6.7766125854356485,
-            "y": 7.3680522721143955,
-            "heading": 3.345311930276191,
-            "angularVelocity": 0.5677939912444252,
-            "velocityX": -3.3530119406998606,
-            "velocityY": -0.8613715936546359,
-            "timestamp": 1.894633042120012
-        },
-        {
-            "x": 6.4587265246711105,
-            "y": 7.22193286725247,
-            "heading": 3.3453119331318786,
-            "angularVelocity": 3.087771911901371e-8,
-            "velocityX": -3.437209353240929,
-            "velocityY": -1.5799465502853158,
-            "timestamp": 1.9871168067367924
-        },
-        {
-            "x": 6.1410584209016115,
-            "y": 7.075340220973225,
-            "heading": 3.3453119359389976,
-            "angularVelocity": 3.035256115645747e-8,
-            "velocityX": -3.434852647767093,
-            "velocityY": -1.5850635718263661,
-            "timestamp": 2.079600571353573
-        },
-        {
-            "x": 5.823390305014834,
-            "y": 6.928747600950052,
-            "heading": 3.3453119387456907,
-            "angularVelocity": 3.034795349843415e-8,
-            "velocityX": -3.4348527787877012,
-            "velocityY": -1.5850632879271098,
-            "timestamp": 2.1720843359703537
-        },
-        {
-            "x": 5.564434200353795,
-            "y": 6.809248569525128,
-            "heading": 3.3684615293972238,
-            "angularVelocity": 0.2503097786643198,
-            "velocityX": -2.8000169082002904,
-            "velocityY": -1.292108208616342,
-            "timestamp": 2.2645681005871343
-        },
-        {
-            "x": 5.3702171046964775,
-            "y": 6.719624287090389,
-            "heading": 3.3858234872314386,
-            "angularVelocity": 0.18772979134186565,
-            "velocityX": -2.1000128667129405,
-            "velocityY": -0.9690812523282153,
-            "timestamp": 2.357051865203915
-        },
-        {
-            "x": 5.240739035958368,
-            "y": 6.659874762854457,
-            "heading": 3.3973980568576057,
-            "angularVelocity": 0.1251524489081645,
-            "velocityX": -1.4000086315106373,
-            "velocityY": -0.6460541964699718,
-            "timestamp": 2.4495356298206956
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.06257585151184072,
-            "velocityX": -0.7000043329401022,
-            "velocityY": -0.3230271061984462,
-            "timestamp": 2.5420193944374763
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 1.89050013704423e-29,
-            "velocityX": -9.26196849914815e-31,
-            "velocityY": -1.067270908871219e-31,
-            "timestamp": 2.634503159054257
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S2.1.traj
deleted file mode 100644
index 1b1b6343..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S2.1.traj	
+++ /dev/null
@@ -1,283 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -6.721620698039908e-29,
-            "timestamp": 0
-        },
-        {
-            "x": 4.32871710747576,
-            "y": 5.596676184944768,
-            "heading": 3.14,
-            "angularVelocity": -1.6840159888455038e-16,
-            "velocityX": 0.6917264182881007,
-            "velocityY": 0.3860469065482612,
-            "timestamp": 0.09500447827150149
-        },
-        {
-            "x": 4.460151319654045,
-            "y": 5.670028553286586,
-            "heading": 3.14,
-            "angularVelocity": -1.2818077527297718e-16,
-            "velocityX": 1.3834528073858645,
-            "velocityY": 0.772093796805632,
-            "timestamp": 0.19000895654300298
-        },
-        {
-            "x": 4.657302630988413,
-            "y": 5.780057101930027,
-            "heading": 3.14,
-            "angularVelocity": -1.8176327661301372e-16,
-            "velocityX": 2.0751791381032545,
-            "velocityY": 1.1581406544813895,
-            "timestamp": 0.2850134348145045
-        },
-        {
-            "x": 4.920171024839782,
-            "y": 5.926761821588956,
-            "heading": 3.14,
-            "angularVelocity": -2.1864698969841676e-16,
-            "velocityX": 2.766905293682386,
-            "velocityY": 1.5441874144139043,
-            "timestamp": 0.38001791308600597
-        },
-        {
-            "x": 5.23400140491651,
-            "y": 6.101908012635582,
-            "heading": 3.14,
-            "angularVelocity": 2.4726801201282114e-16,
-            "velocityX": 3.3033219674134005,
-            "velocityY": 1.8435572115473686,
-            "timestamp": 0.47502239135750746
-        },
-        {
-            "x": 5.547831784993484,
-            "y": 6.277054203682345,
-            "heading": 3.14,
-            "angularVelocity": 3.7708427520389947e-17,
-            "velocityX": 3.303321967416651,
-            "velocityY": 1.843557211549182,
-            "timestamp": 0.570026869629009
-        },
-        {
-            "x": 5.861662165070458,
-            "y": 6.452200394729108,
-            "heading": 3.14,
-            "angularVelocity": 2.1976269787785873e-16,
-            "velocityX": 3.3033219674166516,
-            "velocityY": 1.843557211549183,
-            "timestamp": 0.6650313479005104
-        },
-        {
-            "x": 6.175492545147431,
-            "y": 6.627346585775871,
-            "heading": 3.14,
-            "angularVelocity": 1.9941780105068909e-16,
-            "velocityX": 3.3033219674166516,
-            "velocityY": 1.843557211549183,
-            "timestamp": 0.7600358261720119
-        },
-        {
-            "x": 6.489322925224406,
-            "y": 6.802492776822635,
-            "heading": 3.14,
-            "angularVelocity": 1.915977022732522e-16,
-            "velocityX": 3.3033219674166516,
-            "velocityY": 1.8435572115491827,
-            "timestamp": 0.8550403044435134
-        },
-        {
-            "x": 6.803153305301396,
-            "y": 6.977638967869407,
-            "heading": 3.14,
-            "angularVelocity": 1.0071749090943876e-16,
-            "velocityX": 3.3033219674167866,
-            "velocityY": 1.8435572115492582,
-            "timestamp": 0.9500447827150149
-        },
-        {
-            "x": 7.109956833123924,
-            "y": 7.148863529636325,
-            "heading": 3.14,
-            "angularVelocity": 7.28120504001254e-16,
-            "velocityX": 3.2293585881581666,
-            "velocityY": 1.802278849171093,
-            "timestamp": 1.0450492609865165
-        },
-        {
-            "x": 7.351043278430091,
-            "y": 7.283411920388113,
-            "heading": 3.14,
-            "angularVelocity": 8.519808203444012e-16,
-            "velocityX": 2.5376324325879027,
-            "velocityY": 1.4162320892435267,
-            "timestamp": 1.140053739258018
-        },
-        {
-            "x": 7.526412624580293,
-            "y": 7.381284130838343,
-            "heading": 3.14,
-            "angularVelocity": 9.198517694714364e-16,
-            "velocityX": 1.8459061018722946,
-            "velocityY": 1.0301852315687638,
-            "timestamp": 1.2350582175295193
-        },
-        {
-            "x": 7.636064866028015,
-            "y": 7.442480157891549,
-            "heading": 3.14,
-            "angularVelocity": 7.057693969312242e-16,
-            "velocityX": 1.1541797127752547,
-            "velocityY": 0.6441383413117968,
-            "timestamp": 1.3300626958010207
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 6.521841549416907e-16,
-            "velocityX": 0.4624532944875416,
-            "velocityY": 0.25809143476375196,
-            "timestamp": 1.425067174072522
-        },
-        {
-            "x": 7.649929424023421,
-            "y": 7.450217855315383,
-            "heading": 3.14,
-            "angularVelocity": 4.173997569351088e-16,
-            "velocityX": -0.2907017552877216,
-            "velocityY": -0.16223829304468454,
-            "timestamp": 1.5285085032844143
-        },
-        {
-            "x": 7.5419514905723615,
-            "y": 7.389956245982292,
-            "heading": 3.14,
-            "angularVelocity": 3.561755142449842e-16,
-            "velocityX": -1.043856785955406,
-            "velocityY": -0.5825680101893362,
-            "timestamp": 1.6319498324963067
-        },
-        {
-            "x": 7.356066203105843,
-            "y": 7.286215173931179,
-            "heading": 3.14,
-            "angularVelocity": 4.0492399811570283e-16,
-            "velocityX": -1.7970117831835097,
-            "velocityY": -1.0028977086716289,
-            "timestamp": 1.735391161708199
-        },
-        {
-            "x": 7.092273569234474,
-            "y": 7.138994643409464,
-            "heading": 3.14,
-            "angularVelocity": 3.5744526417986045e-16,
-            "velocityX": -2.550166706837388,
-            "velocityY": -1.4232273660927433,
-            "timestamp": 1.8388324909200913
-        },
-        {
-            "x": 6.7505736194163815,
-            "y": 6.94829467141558,
-            "heading": 3.14,
-            "angularVelocity": 3.486084398579742e-18,
-            "velocityX": -3.3033213360452667,
-            "velocityY": -1.8435568591859304,
-            "timestamp": 1.9422738201319836
-        },
-        {
-            "x": 6.4088736018749985,
-            "y": 6.757594661625877,
-            "heading": 3.14,
-            "angularVelocity": 3.295553774982102e-16,
-            "velocityX": -3.303321990779068,
-            "velocityY": -1.8435572245875573,
-            "timestamp": 2.045715149343876
-        },
-        {
-            "x": 6.0671735843335775,
-            "y": 6.566894651836152,
-            "heading": 3.14,
-            "angularVelocity": -4.7007888222967745e-17,
-            "velocityX": -3.303321990779443,
-            "velocityY": -1.8435572245877663,
-            "timestamp": 2.1491564785557684
-        },
-        {
-            "x": 5.725473566792156,
-            "y": 6.376194642046427,
-            "heading": 3.14,
-            "angularVelocity": -1.693931270443523e-16,
-            "velocityX": -3.3033219907794424,
-            "velocityY": -1.843557224587766,
-            "timestamp": 2.252597807767661
-        },
-        {
-            "x": 5.383773549250734,
-            "y": 6.185494632256702,
-            "heading": 3.14,
-            "angularVelocity": -1.7179108161052583e-16,
-            "velocityX": -3.3033219907794424,
-            "velocityY": -1.8435572245877665,
-            "timestamp": 2.3560391369795535
-        },
-        {
-            "x": 5.042073531709293,
-            "y": 5.994794622466965,
-            "heading": 3.14,
-            "angularVelocity": -2.8062613243046787e-16,
-            "velocityX": -3.3033219907797844,
-            "velocityY": -1.8435572245879575,
-            "timestamp": 2.459480466191446
-        },
-        {
-            "x": 4.730444135370463,
-            "y": 5.820876782602128,
-            "heading": 3.14,
-            "angularVelocity": -4.299065406379285e-16,
-            "velocityX": -3.0126197982407033,
-            "velocityY": -1.6813186875168347,
-            "timestamp": 2.5629217954033385
-        },
-        {
-            "x": 4.496722071912365,
-            "y": 5.690438393660192,
-            "heading": 3.14,
-            "angularVelocity": -4.936527887438678e-16,
-            "velocityX": -2.2594650053308842,
-            "velocityY": -1.2609891030629203,
-            "timestamp": 2.666363124615231
-        },
-        {
-            "x": 4.340907358243485,
-            "y": 5.6034794650776485,
-            "heading": 3.14,
-            "angularVelocity": -3.0491715981392867e-16,
-            "velocityX": -1.50631004895991,
-            "velocityY": -0.8406594273826603,
-            "timestamp": 2.7698044538271236
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -6.545212108880601e-17,
-            "velocityX": -0.7531550381017611,
-            "velocityY": -0.42032972129354973,
-            "timestamp": 2.873245783039016
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": -8.479397572029164e-30,
-            "velocityY": 1.1992173369619903e-31,
-            "timestamp": 2.9766871122509087
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S2.traj
deleted file mode 100644
index 1b1b6343..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1 To S2.traj	
+++ /dev/null
@@ -1,283 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -6.721620698039908e-29,
-            "timestamp": 0
-        },
-        {
-            "x": 4.32871710747576,
-            "y": 5.596676184944768,
-            "heading": 3.14,
-            "angularVelocity": -1.6840159888455038e-16,
-            "velocityX": 0.6917264182881007,
-            "velocityY": 0.3860469065482612,
-            "timestamp": 0.09500447827150149
-        },
-        {
-            "x": 4.460151319654045,
-            "y": 5.670028553286586,
-            "heading": 3.14,
-            "angularVelocity": -1.2818077527297718e-16,
-            "velocityX": 1.3834528073858645,
-            "velocityY": 0.772093796805632,
-            "timestamp": 0.19000895654300298
-        },
-        {
-            "x": 4.657302630988413,
-            "y": 5.780057101930027,
-            "heading": 3.14,
-            "angularVelocity": -1.8176327661301372e-16,
-            "velocityX": 2.0751791381032545,
-            "velocityY": 1.1581406544813895,
-            "timestamp": 0.2850134348145045
-        },
-        {
-            "x": 4.920171024839782,
-            "y": 5.926761821588956,
-            "heading": 3.14,
-            "angularVelocity": -2.1864698969841676e-16,
-            "velocityX": 2.766905293682386,
-            "velocityY": 1.5441874144139043,
-            "timestamp": 0.38001791308600597
-        },
-        {
-            "x": 5.23400140491651,
-            "y": 6.101908012635582,
-            "heading": 3.14,
-            "angularVelocity": 2.4726801201282114e-16,
-            "velocityX": 3.3033219674134005,
-            "velocityY": 1.8435572115473686,
-            "timestamp": 0.47502239135750746
-        },
-        {
-            "x": 5.547831784993484,
-            "y": 6.277054203682345,
-            "heading": 3.14,
-            "angularVelocity": 3.7708427520389947e-17,
-            "velocityX": 3.303321967416651,
-            "velocityY": 1.843557211549182,
-            "timestamp": 0.570026869629009
-        },
-        {
-            "x": 5.861662165070458,
-            "y": 6.452200394729108,
-            "heading": 3.14,
-            "angularVelocity": 2.1976269787785873e-16,
-            "velocityX": 3.3033219674166516,
-            "velocityY": 1.843557211549183,
-            "timestamp": 0.6650313479005104
-        },
-        {
-            "x": 6.175492545147431,
-            "y": 6.627346585775871,
-            "heading": 3.14,
-            "angularVelocity": 1.9941780105068909e-16,
-            "velocityX": 3.3033219674166516,
-            "velocityY": 1.843557211549183,
-            "timestamp": 0.7600358261720119
-        },
-        {
-            "x": 6.489322925224406,
-            "y": 6.802492776822635,
-            "heading": 3.14,
-            "angularVelocity": 1.915977022732522e-16,
-            "velocityX": 3.3033219674166516,
-            "velocityY": 1.8435572115491827,
-            "timestamp": 0.8550403044435134
-        },
-        {
-            "x": 6.803153305301396,
-            "y": 6.977638967869407,
-            "heading": 3.14,
-            "angularVelocity": 1.0071749090943876e-16,
-            "velocityX": 3.3033219674167866,
-            "velocityY": 1.8435572115492582,
-            "timestamp": 0.9500447827150149
-        },
-        {
-            "x": 7.109956833123924,
-            "y": 7.148863529636325,
-            "heading": 3.14,
-            "angularVelocity": 7.28120504001254e-16,
-            "velocityX": 3.2293585881581666,
-            "velocityY": 1.802278849171093,
-            "timestamp": 1.0450492609865165
-        },
-        {
-            "x": 7.351043278430091,
-            "y": 7.283411920388113,
-            "heading": 3.14,
-            "angularVelocity": 8.519808203444012e-16,
-            "velocityX": 2.5376324325879027,
-            "velocityY": 1.4162320892435267,
-            "timestamp": 1.140053739258018
-        },
-        {
-            "x": 7.526412624580293,
-            "y": 7.381284130838343,
-            "heading": 3.14,
-            "angularVelocity": 9.198517694714364e-16,
-            "velocityX": 1.8459061018722946,
-            "velocityY": 1.0301852315687638,
-            "timestamp": 1.2350582175295193
-        },
-        {
-            "x": 7.636064866028015,
-            "y": 7.442480157891549,
-            "heading": 3.14,
-            "angularVelocity": 7.057693969312242e-16,
-            "velocityX": 1.1541797127752547,
-            "velocityY": 0.6441383413117968,
-            "timestamp": 1.3300626958010207
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 6.521841549416907e-16,
-            "velocityX": 0.4624532944875416,
-            "velocityY": 0.25809143476375196,
-            "timestamp": 1.425067174072522
-        },
-        {
-            "x": 7.649929424023421,
-            "y": 7.450217855315383,
-            "heading": 3.14,
-            "angularVelocity": 4.173997569351088e-16,
-            "velocityX": -0.2907017552877216,
-            "velocityY": -0.16223829304468454,
-            "timestamp": 1.5285085032844143
-        },
-        {
-            "x": 7.5419514905723615,
-            "y": 7.389956245982292,
-            "heading": 3.14,
-            "angularVelocity": 3.561755142449842e-16,
-            "velocityX": -1.043856785955406,
-            "velocityY": -0.5825680101893362,
-            "timestamp": 1.6319498324963067
-        },
-        {
-            "x": 7.356066203105843,
-            "y": 7.286215173931179,
-            "heading": 3.14,
-            "angularVelocity": 4.0492399811570283e-16,
-            "velocityX": -1.7970117831835097,
-            "velocityY": -1.0028977086716289,
-            "timestamp": 1.735391161708199
-        },
-        {
-            "x": 7.092273569234474,
-            "y": 7.138994643409464,
-            "heading": 3.14,
-            "angularVelocity": 3.5744526417986045e-16,
-            "velocityX": -2.550166706837388,
-            "velocityY": -1.4232273660927433,
-            "timestamp": 1.8388324909200913
-        },
-        {
-            "x": 6.7505736194163815,
-            "y": 6.94829467141558,
-            "heading": 3.14,
-            "angularVelocity": 3.486084398579742e-18,
-            "velocityX": -3.3033213360452667,
-            "velocityY": -1.8435568591859304,
-            "timestamp": 1.9422738201319836
-        },
-        {
-            "x": 6.4088736018749985,
-            "y": 6.757594661625877,
-            "heading": 3.14,
-            "angularVelocity": 3.295553774982102e-16,
-            "velocityX": -3.303321990779068,
-            "velocityY": -1.8435572245875573,
-            "timestamp": 2.045715149343876
-        },
-        {
-            "x": 6.0671735843335775,
-            "y": 6.566894651836152,
-            "heading": 3.14,
-            "angularVelocity": -4.7007888222967745e-17,
-            "velocityX": -3.303321990779443,
-            "velocityY": -1.8435572245877663,
-            "timestamp": 2.1491564785557684
-        },
-        {
-            "x": 5.725473566792156,
-            "y": 6.376194642046427,
-            "heading": 3.14,
-            "angularVelocity": -1.693931270443523e-16,
-            "velocityX": -3.3033219907794424,
-            "velocityY": -1.843557224587766,
-            "timestamp": 2.252597807767661
-        },
-        {
-            "x": 5.383773549250734,
-            "y": 6.185494632256702,
-            "heading": 3.14,
-            "angularVelocity": -1.7179108161052583e-16,
-            "velocityX": -3.3033219907794424,
-            "velocityY": -1.8435572245877665,
-            "timestamp": 2.3560391369795535
-        },
-        {
-            "x": 5.042073531709293,
-            "y": 5.994794622466965,
-            "heading": 3.14,
-            "angularVelocity": -2.8062613243046787e-16,
-            "velocityX": -3.3033219907797844,
-            "velocityY": -1.8435572245879575,
-            "timestamp": 2.459480466191446
-        },
-        {
-            "x": 4.730444135370463,
-            "y": 5.820876782602128,
-            "heading": 3.14,
-            "angularVelocity": -4.299065406379285e-16,
-            "velocityX": -3.0126197982407033,
-            "velocityY": -1.6813186875168347,
-            "timestamp": 2.5629217954033385
-        },
-        {
-            "x": 4.496722071912365,
-            "y": 5.690438393660192,
-            "heading": 3.14,
-            "angularVelocity": -4.936527887438678e-16,
-            "velocityX": -2.2594650053308842,
-            "velocityY": -1.2609891030629203,
-            "timestamp": 2.666363124615231
-        },
-        {
-            "x": 4.340907358243485,
-            "y": 5.6034794650776485,
-            "heading": 3.14,
-            "angularVelocity": -3.0491715981392867e-16,
-            "velocityX": -1.50631004895991,
-            "velocityY": -0.8406594273826603,
-            "timestamp": 2.7698044538271236
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -6.545212108880601e-17,
-            "velocityX": -0.7531550381017611,
-            "velocityY": -0.42032972129354973,
-            "timestamp": 2.873245783039016
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": -8.479397572029164e-30,
-            "velocityY": 1.1992173369619903e-31,
-            "timestamp": 2.9766871122509087
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S1.1.traj
deleted file mode 100644
index 505b3c8d..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S1.1.traj	
+++ /dev/null
@@ -1,256 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -3.0990371742085845e-31,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.334488852780742,
-            "y": 5.55372681466254,
-            "heading": 3.133953491828579,
-            "angularVelocity": -0.06516682898121992,
-            "velocityX": 0.7704780529773917,
-            "velocityY": -0.06760986415037558,
-            "timestamp": 0.09278506052766491
-        },
-        {
-            "x": 4.477466555007721,
-            "y": 5.541180442989367,
-            "heading": 3.121860441564243,
-            "angularVelocity": -0.13033402355464482,
-            "velocityX": 1.5409560700168046,
-            "velocityY": -0.1352197390595365,
-            "timestamp": 0.18557012105532983
-        },
-        {
-            "x": 4.691933100191895,
-            "y": 5.522360883581774,
-            "heading": 3.1037208124155655,
-            "angularVelocity": -0.19550161465122112,
-            "velocityX": 2.311434017119952,
-            "velocityY": -0.20282962904337912,
-            "timestamp": 0.27835518158299477
-        },
-        {
-            "x": 4.977888469750385,
-            "y": 5.497268135402371,
-            "heading": 3.079534623451408,
-            "angularVelocity": -0.26066900023130835,
-            "velocityX": 3.0819117639443303,
-            "velocityY": -0.2704395302078152,
-            "timestamp": 0.37114024211065966
-        },
-        {
-            "x": 5.327545209579434,
-            "y": 5.4665855253125155,
-            "heading": 3.079534620495457,
-            "angularVelocity": -3.185804655621534e-8,
-            "velocityX": 3.768459467941971,
-            "velocityY": -0.33068480976750836,
-            "timestamp": 0.46392530263832454
-        },
-        {
-            "x": 5.677201949407512,
-            "y": 5.435902915209702,
-            "heading": 3.0795346175395855,
-            "angularVelocity": -3.185719286228054e-8,
-            "velocityX": 3.768459467931527,
-            "velocityY": -0.3306848099071595,
-            "timestamp": 0.5567103631659894
-        },
-        {
-            "x": 6.026858689235635,
-            "y": 5.405220305107383,
-            "heading": 3.0795346145837144,
-            "angularVelocity": -3.185719241382707e-8,
-            "velocityX": 3.7684594679319945,
-            "velocityY": -0.3306848099018315,
-            "timestamp": 0.6494954236936543
-        },
-        {
-            "x": 6.376515440276988,
-            "y": 5.37453782279048,
-            "heading": 3.079534611627843,
-            "angularVelocity": -3.1857194092858966e-8,
-            "velocityX": 3.7684595887836787,
-            "velocityY": -0.3306834326820888,
-            "timestamp": 0.7422804842213192
-        },
-        {
-            "x": 6.7274981570931205,
-            "y": 5.371017863275757,
-            "heading": 3.079534608531603,
-            "angularVelocity": -3.337002331102787e-8,
-            "velocityX": 3.782750313683133,
-            "velocityY": -0.03793670548581862,
-            "timestamp": 0.8350655447489841
-        },
-        {
-            "x": 7.060187879890856,
-            "y": 5.436910997452384,
-            "heading": 3.079737215344131,
-            "angularVelocity": 0.0021836145967468554,
-            "velocityX": 3.585595794255326,
-            "velocityY": 0.7101696523329738,
-            "timestamp": 0.927850605276649
-        },
-        {
-            "x": 7.322040445742604,
-            "y": 5.514304234937123,
-            "heading": 3.085826008048483,
-            "angularVelocity": 0.065622554641053,
-            "velocityX": 2.82214145642183,
-            "velocityY": 0.834113132487145,
-            "timestamp": 1.0206356658043139
-        },
-        {
-            "x": 7.51268687185891,
-            "y": 5.600628661192279,
-            "heading": 3.097884063831068,
-            "angularVelocity": 0.12995686712932528,
-            "velocityX": 2.054710370743838,
-            "velocityY": 0.9303698867493723,
-            "timestamp": 1.1134207263319789
-        },
-        {
-            "x": 7.632022870387607,
-            "y": 5.695007866387598,
-            "heading": 3.115937777513367,
-            "angularVelocity": 0.19457565236926141,
-            "velocityX": 1.2861553126119536,
-            "velocityY": 1.0171810489597053,
-            "timestamp": 1.2062057868596439
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 0.25933293948166103,
-            "velocityX": 0.517078173356003,
-            "velocityY": 1.0992301242503861,
-            "timestamp": 1.2989908473873089
-        },
-        {
-            "x": 7.658324369192282,
-            "y": 5.904176742987938,
-            "heading": 3.16941072815558,
-            "angularVelocity": 0.32296823834349714,
-            "velocityX": -0.23802675880415525,
-            "velocityY": 1.176940730303702,
-            "timestamp": 1.3900546879260585
-        },
-        {
-            "x": 7.56785439743373,
-            "y": 6.01813279809926,
-            "heading": 3.204362801369678,
-            "angularVelocity": 0.3838194502596341,
-            "velocityX": -0.9934785445386147,
-            "velocityY": 1.2513864387569726,
-            "timestamp": 1.481118528464808
-        },
-        {
-            "x": 7.4085407741930185,
-            "y": 6.138375580441298,
-            "heading": 3.2444365070983814,
-            "angularVelocity": 0.44006166983116235,
-            "velocityX": -1.7494718243615388,
-            "velocityY": 1.3204229212238012,
-            "timestamp": 1.5721823690035577
-        },
-        {
-            "x": 7.180297052881674,
-            "y": 6.26393107037867,
-            "heading": 3.2888027958474897,
-            "angularVelocity": 0.48719984229357544,
-            "velocityX": -2.5064144007215123,
-            "velocityY": 1.3787633949387568,
-            "timestamp": 1.6632462095423073
-        },
-        {
-            "x": 6.882954936764855,
-            "y": 6.39196860518662,
-            "heading": 3.335055491791258,
-            "angularVelocity": 0.5079150590414778,
-            "velocityX": -3.2652050952131715,
-            "velocityY": 1.406019491935018,
-            "timestamp": 1.754310050081057
-        },
-        {
-            "x": 6.546228290367871,
-            "y": 6.464686559433002,
-            "heading": 3.335055498171187,
-            "angularVelocity": 7.005996108772343e-8,
-            "velocityX": -3.697698717787989,
-            "velocityY": 0.7985381883321813,
-            "timestamp": 1.8453738906198065
-        },
-        {
-            "x": 6.204219309221898,
-            "y": 6.505949051941506,
-            "heading": 3.3350555015810333,
-            "angularVelocity": 3.7444564221531034e-8,
-            "velocityX": -3.7557056579492913,
-            "velocityY": 0.45311610255384965,
-            "timestamp": 1.936437731158556
-        },
-        {
-            "x": 5.862210306846827,
-            "y": 6.547211368473936,
-            "heading": 3.335055504991925,
-            "angularVelocity": 3.7456047151170976e-8,
-            "velocityX": -3.7557058910725254,
-            "velocityY": 0.45311417010650673,
-            "timestamp": 2.0275015716973055
-        },
-        {
-            "x": 5.587726197097608,
-            "y": 6.580326820073676,
-            "heading": 3.3623069003974067,
-            "angularVelocity": 0.29925594225114466,
-            "velocityX": -3.0141943072610453,
-            "velocityY": 0.3636509442587018,
-            "timestamp": 2.118565412236055
-        },
-        {
-            "x": 5.381863101775985,
-            "y": 6.605163409773074,
-            "heading": 3.382745950256302,
-            "angularVelocity": 0.22444748363294062,
-            "velocityX": -2.2606458733093975,
-            "velocityY": 0.2727382191708659,
-            "timestamp": 2.2096292527748043
-        },
-        {
-            "x": 5.244621034490526,
-            "y": 6.621721136542047,
-            "heading": 3.3963721295427387,
-            "angularVelocity": 0.1496332595442014,
-            "velocityX": -1.5070972899178572,
-            "velocityY": 0.18182548277135752,
-            "timestamp": 2.3006930933135536
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.07481759605718756,
-            "velocityX": -0.7535486542688541,
-            "velocityY": 0.09091274219244924,
-            "timestamp": 2.391756933852303
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0,
-            "velocityX": -1.5663528740094776e-30,
-            "velocityY": 4.877949363390491e-30,
-            "timestamp": 2.4828207743910524
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S1.traj
deleted file mode 100644
index 505b3c8d..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S1.traj	
+++ /dev/null
@@ -1,256 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -3.0990371742085845e-31,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.334488852780742,
-            "y": 5.55372681466254,
-            "heading": 3.133953491828579,
-            "angularVelocity": -0.06516682898121992,
-            "velocityX": 0.7704780529773917,
-            "velocityY": -0.06760986415037558,
-            "timestamp": 0.09278506052766491
-        },
-        {
-            "x": 4.477466555007721,
-            "y": 5.541180442989367,
-            "heading": 3.121860441564243,
-            "angularVelocity": -0.13033402355464482,
-            "velocityX": 1.5409560700168046,
-            "velocityY": -0.1352197390595365,
-            "timestamp": 0.18557012105532983
-        },
-        {
-            "x": 4.691933100191895,
-            "y": 5.522360883581774,
-            "heading": 3.1037208124155655,
-            "angularVelocity": -0.19550161465122112,
-            "velocityX": 2.311434017119952,
-            "velocityY": -0.20282962904337912,
-            "timestamp": 0.27835518158299477
-        },
-        {
-            "x": 4.977888469750385,
-            "y": 5.497268135402371,
-            "heading": 3.079534623451408,
-            "angularVelocity": -0.26066900023130835,
-            "velocityX": 3.0819117639443303,
-            "velocityY": -0.2704395302078152,
-            "timestamp": 0.37114024211065966
-        },
-        {
-            "x": 5.327545209579434,
-            "y": 5.4665855253125155,
-            "heading": 3.079534620495457,
-            "angularVelocity": -3.185804655621534e-8,
-            "velocityX": 3.768459467941971,
-            "velocityY": -0.33068480976750836,
-            "timestamp": 0.46392530263832454
-        },
-        {
-            "x": 5.677201949407512,
-            "y": 5.435902915209702,
-            "heading": 3.0795346175395855,
-            "angularVelocity": -3.185719286228054e-8,
-            "velocityX": 3.768459467931527,
-            "velocityY": -0.3306848099071595,
-            "timestamp": 0.5567103631659894
-        },
-        {
-            "x": 6.026858689235635,
-            "y": 5.405220305107383,
-            "heading": 3.0795346145837144,
-            "angularVelocity": -3.185719241382707e-8,
-            "velocityX": 3.7684594679319945,
-            "velocityY": -0.3306848099018315,
-            "timestamp": 0.6494954236936543
-        },
-        {
-            "x": 6.376515440276988,
-            "y": 5.37453782279048,
-            "heading": 3.079534611627843,
-            "angularVelocity": -3.1857194092858966e-8,
-            "velocityX": 3.7684595887836787,
-            "velocityY": -0.3306834326820888,
-            "timestamp": 0.7422804842213192
-        },
-        {
-            "x": 6.7274981570931205,
-            "y": 5.371017863275757,
-            "heading": 3.079534608531603,
-            "angularVelocity": -3.337002331102787e-8,
-            "velocityX": 3.782750313683133,
-            "velocityY": -0.03793670548581862,
-            "timestamp": 0.8350655447489841
-        },
-        {
-            "x": 7.060187879890856,
-            "y": 5.436910997452384,
-            "heading": 3.079737215344131,
-            "angularVelocity": 0.0021836145967468554,
-            "velocityX": 3.585595794255326,
-            "velocityY": 0.7101696523329738,
-            "timestamp": 0.927850605276649
-        },
-        {
-            "x": 7.322040445742604,
-            "y": 5.514304234937123,
-            "heading": 3.085826008048483,
-            "angularVelocity": 0.065622554641053,
-            "velocityX": 2.82214145642183,
-            "velocityY": 0.834113132487145,
-            "timestamp": 1.0206356658043139
-        },
-        {
-            "x": 7.51268687185891,
-            "y": 5.600628661192279,
-            "heading": 3.097884063831068,
-            "angularVelocity": 0.12995686712932528,
-            "velocityX": 2.054710370743838,
-            "velocityY": 0.9303698867493723,
-            "timestamp": 1.1134207263319789
-        },
-        {
-            "x": 7.632022870387607,
-            "y": 5.695007866387598,
-            "heading": 3.115937777513367,
-            "angularVelocity": 0.19457565236926141,
-            "velocityX": 1.2861553126119536,
-            "velocityY": 1.0171810489597053,
-            "timestamp": 1.2062057868596439
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 0.25933293948166103,
-            "velocityX": 0.517078173356003,
-            "velocityY": 1.0992301242503861,
-            "timestamp": 1.2989908473873089
-        },
-        {
-            "x": 7.658324369192282,
-            "y": 5.904176742987938,
-            "heading": 3.16941072815558,
-            "angularVelocity": 0.32296823834349714,
-            "velocityX": -0.23802675880415525,
-            "velocityY": 1.176940730303702,
-            "timestamp": 1.3900546879260585
-        },
-        {
-            "x": 7.56785439743373,
-            "y": 6.01813279809926,
-            "heading": 3.204362801369678,
-            "angularVelocity": 0.3838194502596341,
-            "velocityX": -0.9934785445386147,
-            "velocityY": 1.2513864387569726,
-            "timestamp": 1.481118528464808
-        },
-        {
-            "x": 7.4085407741930185,
-            "y": 6.138375580441298,
-            "heading": 3.2444365070983814,
-            "angularVelocity": 0.44006166983116235,
-            "velocityX": -1.7494718243615388,
-            "velocityY": 1.3204229212238012,
-            "timestamp": 1.5721823690035577
-        },
-        {
-            "x": 7.180297052881674,
-            "y": 6.26393107037867,
-            "heading": 3.2888027958474897,
-            "angularVelocity": 0.48719984229357544,
-            "velocityX": -2.5064144007215123,
-            "velocityY": 1.3787633949387568,
-            "timestamp": 1.6632462095423073
-        },
-        {
-            "x": 6.882954936764855,
-            "y": 6.39196860518662,
-            "heading": 3.335055491791258,
-            "angularVelocity": 0.5079150590414778,
-            "velocityX": -3.2652050952131715,
-            "velocityY": 1.406019491935018,
-            "timestamp": 1.754310050081057
-        },
-        {
-            "x": 6.546228290367871,
-            "y": 6.464686559433002,
-            "heading": 3.335055498171187,
-            "angularVelocity": 7.005996108772343e-8,
-            "velocityX": -3.697698717787989,
-            "velocityY": 0.7985381883321813,
-            "timestamp": 1.8453738906198065
-        },
-        {
-            "x": 6.204219309221898,
-            "y": 6.505949051941506,
-            "heading": 3.3350555015810333,
-            "angularVelocity": 3.7444564221531034e-8,
-            "velocityX": -3.7557056579492913,
-            "velocityY": 0.45311610255384965,
-            "timestamp": 1.936437731158556
-        },
-        {
-            "x": 5.862210306846827,
-            "y": 6.547211368473936,
-            "heading": 3.335055504991925,
-            "angularVelocity": 3.7456047151170976e-8,
-            "velocityX": -3.7557058910725254,
-            "velocityY": 0.45311417010650673,
-            "timestamp": 2.0275015716973055
-        },
-        {
-            "x": 5.587726197097608,
-            "y": 6.580326820073676,
-            "heading": 3.3623069003974067,
-            "angularVelocity": 0.29925594225114466,
-            "velocityX": -3.0141943072610453,
-            "velocityY": 0.3636509442587018,
-            "timestamp": 2.118565412236055
-        },
-        {
-            "x": 5.381863101775985,
-            "y": 6.605163409773074,
-            "heading": 3.382745950256302,
-            "angularVelocity": 0.22444748363294062,
-            "velocityX": -2.2606458733093975,
-            "velocityY": 0.2727382191708659,
-            "timestamp": 2.2096292527748043
-        },
-        {
-            "x": 5.244621034490526,
-            "y": 6.621721136542047,
-            "heading": 3.3963721295427387,
-            "angularVelocity": 0.1496332595442014,
-            "velocityX": -1.5070972899178572,
-            "velocityY": 0.18182548277135752,
-            "timestamp": 2.3006930933135536
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.07481759605718756,
-            "velocityX": -0.7535486542688541,
-            "velocityY": 0.09091274219244924,
-            "timestamp": 2.391756933852303
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0,
-            "velocityX": -1.5663528740094776e-30,
-            "velocityY": 4.877949363390491e-30,
-            "timestamp": 2.4828207743910524
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S2.1.traj
deleted file mode 100644
index e9fe1d34..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S2.1.traj	
+++ /dev/null
@@ -1,265 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -2.9840248880147764e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 4.335913751201177,
-            "y": 5.5650572312071045,
-            "heading": 3.14,
-            "angularVelocity": -1.4550376605668876e-16,
-            "velocityX": 0.7787859499921702,
-            "velocityY": 0.0540158823962963,
-            "timestamp": 0.09362489295282081
-        },
-        {
-            "x": 4.481741250496669,
-            "y": 5.575171693405826,
-            "heading": 3.14,
-            "angularVelocity": -2.6552600483679955e-16,
-            "velocityX": 1.5575718668001757,
-            "velocityY": 0.10803176249096925,
-            "timestamp": 0.18724978590564162
-        },
-        {
-            "x": 4.7004824916727515,
-            "y": 5.590343386165186,
-            "heading": 3.14,
-            "angularVelocity": -3.921189080478138e-16,
-            "velocityX": 2.3363577172399013,
-            "velocityY": 0.1620476379823989,
-            "timestamp": 0.2808746788584624
-        },
-        {
-            "x": 4.992137456088294,
-            "y": 5.6105723081922525,
-            "heading": 3.14,
-            "angularVelocity": -4.0854142114404067e-16,
-            "velocityX": 3.1151433685752,
-            "velocityY": 0.2160634996641273,
-            "timestamp": 0.37449957181128324
-        },
-        {
-            "x": 5.345466004271564,
-            "y": 5.63507885367643,
-            "heading": 3.14,
-            "angularVelocity": 5.235558245992657e-16,
-            "velocityX": 3.773873988420134,
-            "velocityY": 0.2617524539817304,
-            "timestamp": 0.4681244647641041
-        },
-        {
-            "x": 5.69879455245494,
-            "y": 5.6595853991606155,
-            "heading": 3.14,
-            "angularVelocity": -5.535576803898805e-16,
-            "velocityX": 3.773873988421262,
-            "velocityY": 0.2617524539818086,
-            "timestamp": 0.5617493577169249
-        },
-        {
-            "x": 6.052123100638315,
-            "y": 5.6840919446448,
-            "heading": 3.14,
-            "angularVelocity": -2.4126594725597773e-16,
-            "velocityX": 3.7738739884212626,
-            "velocityY": 0.2617524539818086,
-            "timestamp": 0.6553742506697458
-        },
-        {
-            "x": 6.405451648821692,
-            "y": 5.708598490128986,
-            "heading": 3.14,
-            "angularVelocity": 3.1966951315745e-16,
-            "velocityX": 3.773873988421262,
-            "velocityY": 0.26175245398180863,
-            "timestamp": 0.7489991436225666
-        },
-        {
-            "x": 6.7587801970050565,
-            "y": 5.733105035613169,
-            "heading": 3.14,
-            "angularVelocity": -3.2948712369984396e-16,
-            "velocityX": 3.773873988421149,
-            "velocityY": 0.26175245398180075,
-            "timestamp": 0.8426240365753874
-        },
-        {
-            "x": 7.088851630801794,
-            "y": 5.755998488879142,
-            "heading": 3.14,
-            "angularVelocity": -9.417571312555162e-16,
-            "velocityX": 3.5254666081494603,
-            "velocityY": 0.24452314490237434,
-            "timestamp": 0.9362489295282083
-        },
-        {
-            "x": 7.3460093413592045,
-            "y": 5.773834712877417,
-            "heading": 3.14,
-            "angularVelocity": -8.090853196926974e-16,
-            "velocityX": 2.746680956815585,
-            "velocityY": 0.19050728322074467,
-            "timestamp": 1.029873822481029
-        },
-        {
-            "x": 7.530253310036057,
-            "y": 5.786613706315055,
-            "heading": 3.14,
-            "angularVelocity": -6.531335051646392e-16,
-            "velocityX": 1.9678951063762244,
-            "velocityY": 0.13649140772934035,
-            "timestamp": 1.1234987154338498
-        },
-        {
-            "x": 7.641583530618614,
-            "y": 5.79433546876108,
-            "heading": 3.14,
-            "angularVelocity": -6.271183503983396e-16,
-            "velocityX": 1.1891091895683832,
-            "velocityY": 0.08247552763467879,
-            "timestamp": 1.2171236083866706
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": -6.05510209215834e-16,
-            "velocityX": 0.41032323957630595,
-            "velocityY": 0.028459645238388953,
-            "timestamp": 1.3107485013394913
-        },
-        {
-            "x": 7.6370908002198155,
-            "y": 5.794023857082849,
-            "heading": 3.14,
-            "angularVelocity": -4.789725529265609e-16,
-            "velocityX": -0.42651589559076464,
-            "velocityY": -0.02958275307434032,
-            "timestamp": 1.4113524887260582
-        },
-        {
-            "x": 7.509992248723152,
-            "y": 5.785208417602395,
-            "heading": 3.14,
-            "angularVelocity": -2.32893689231122e-16,
-            "velocityX": -1.2633550100583235,
-            "velocityY": -0.08762514995137034,
-            "timestamp": 1.5119564761126252
-        },
-        {
-            "x": 7.298704349055096,
-            "y": 5.770553681804523,
-            "heading": 3.14,
-            "angularVelocity": -1.4093989000106693e-18,
-            "velocityX": -2.1001940892878346,
-            "velocityY": -0.14566754438432153,
-            "timestamp": 1.6125604634991921
-        },
-        {
-            "x": 7.003227108598371,
-            "y": 5.75005965020129,
-            "heading": 3.14,
-            "angularVelocity": 2.7928007738396434e-16,
-            "velocityX": -2.9370330951333345,
-            "velocityY": -0.2037099337274247,
-            "timestamp": 1.713164450885759
-        },
-        {
-            "x": 6.623560552157282,
-            "y": 5.723726324513103,
-            "heading": 3.14,
-            "angularVelocity": 4.2079673901381875e-16,
-            "velocityX": -3.773871854424956,
-            "velocityY": -0.2617523059697759,
-            "timestamp": 1.813768438272326
-        },
-        {
-            "x": 6.243893779806084,
-            "y": 5.6973929838495865,
-            "heading": 3.14,
-            "angularVelocity": -4.939929195074519e-16,
-            "velocityX": -3.773874000563654,
-            "velocityY": -0.2617524548239938,
-            "timestamp": 1.914372425658893
-        },
-        {
-            "x": 5.864227007454884,
-            "y": 5.67105964318607,
-            "heading": 3.14,
-            "angularVelocity": 2.0250011365009936e-16,
-            "velocityX": -3.773874000563654,
-            "velocityY": -0.26175245482399373,
-            "timestamp": 2.01497641304546
-        },
-        {
-            "x": 5.484560235103686,
-            "y": 5.644726302522554,
-            "heading": 3.14,
-            "angularVelocity": -5.77312332256685e-16,
-            "velocityX": -3.773874000563654,
-            "velocityY": -0.2617524548239938,
-            "timestamp": 2.115580400432027
-        },
-        {
-            "x": 5.104893462752507,
-            "y": 5.61839296185904,
-            "heading": 3.14,
-            "angularVelocity": -1.6679194482656825e-16,
-            "velocityX": -3.773874000563452,
-            "velocityY": -0.26175245482397974,
-            "timestamp": 2.2161843878185943
-        },
-        {
-            "x": 4.768136096424167,
-            "y": 5.595035778417478,
-            "heading": 3.14,
-            "angularVelocity": 9.060703447293421e-16,
-            "velocityX": -3.3473560549281536,
-            "velocityY": -0.2321695595604253,
-            "timestamp": 2.3167883752051615
-        },
-        {
-            "x": 4.515568053067083,
-            "y": 5.577517889545478,
-            "heading": 3.14,
-            "angularVelocity": 6.700681499162007e-16,
-            "velocityX": -2.51051722618707,
-            "velocityY": -0.1741271825011227,
-            "timestamp": 2.4173923625917286
-        },
-        {
-            "x": 4.34718935210125,
-            "y": 5.565839296589989,
-            "heading": 3.14,
-            "angularVelocity": 4.365333326959493e-16,
-            "velocityX": -1.6736782044119611,
-            "velocityY": -0.11608479205315635,
-            "timestamp": 2.517996349978296
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 2.0339545203820728e-16,
-            "velocityX": -0.836839118292156,
-            "velocityY": -0.05804239714230061,
-            "timestamp": 2.618600337364863
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 2.71920432475143
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S2.traj
deleted file mode 100644
index e9fe1d34..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2 To S2.traj	
+++ /dev/null
@@ -1,265 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -2.9840248880147764e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 4.335913751201177,
-            "y": 5.5650572312071045,
-            "heading": 3.14,
-            "angularVelocity": -1.4550376605668876e-16,
-            "velocityX": 0.7787859499921702,
-            "velocityY": 0.0540158823962963,
-            "timestamp": 0.09362489295282081
-        },
-        {
-            "x": 4.481741250496669,
-            "y": 5.575171693405826,
-            "heading": 3.14,
-            "angularVelocity": -2.6552600483679955e-16,
-            "velocityX": 1.5575718668001757,
-            "velocityY": 0.10803176249096925,
-            "timestamp": 0.18724978590564162
-        },
-        {
-            "x": 4.7004824916727515,
-            "y": 5.590343386165186,
-            "heading": 3.14,
-            "angularVelocity": -3.921189080478138e-16,
-            "velocityX": 2.3363577172399013,
-            "velocityY": 0.1620476379823989,
-            "timestamp": 0.2808746788584624
-        },
-        {
-            "x": 4.992137456088294,
-            "y": 5.6105723081922525,
-            "heading": 3.14,
-            "angularVelocity": -4.0854142114404067e-16,
-            "velocityX": 3.1151433685752,
-            "velocityY": 0.2160634996641273,
-            "timestamp": 0.37449957181128324
-        },
-        {
-            "x": 5.345466004271564,
-            "y": 5.63507885367643,
-            "heading": 3.14,
-            "angularVelocity": 5.235558245992657e-16,
-            "velocityX": 3.773873988420134,
-            "velocityY": 0.2617524539817304,
-            "timestamp": 0.4681244647641041
-        },
-        {
-            "x": 5.69879455245494,
-            "y": 5.6595853991606155,
-            "heading": 3.14,
-            "angularVelocity": -5.535576803898805e-16,
-            "velocityX": 3.773873988421262,
-            "velocityY": 0.2617524539818086,
-            "timestamp": 0.5617493577169249
-        },
-        {
-            "x": 6.052123100638315,
-            "y": 5.6840919446448,
-            "heading": 3.14,
-            "angularVelocity": -2.4126594725597773e-16,
-            "velocityX": 3.7738739884212626,
-            "velocityY": 0.2617524539818086,
-            "timestamp": 0.6553742506697458
-        },
-        {
-            "x": 6.405451648821692,
-            "y": 5.708598490128986,
-            "heading": 3.14,
-            "angularVelocity": 3.1966951315745e-16,
-            "velocityX": 3.773873988421262,
-            "velocityY": 0.26175245398180863,
-            "timestamp": 0.7489991436225666
-        },
-        {
-            "x": 6.7587801970050565,
-            "y": 5.733105035613169,
-            "heading": 3.14,
-            "angularVelocity": -3.2948712369984396e-16,
-            "velocityX": 3.773873988421149,
-            "velocityY": 0.26175245398180075,
-            "timestamp": 0.8426240365753874
-        },
-        {
-            "x": 7.088851630801794,
-            "y": 5.755998488879142,
-            "heading": 3.14,
-            "angularVelocity": -9.417571312555162e-16,
-            "velocityX": 3.5254666081494603,
-            "velocityY": 0.24452314490237434,
-            "timestamp": 0.9362489295282083
-        },
-        {
-            "x": 7.3460093413592045,
-            "y": 5.773834712877417,
-            "heading": 3.14,
-            "angularVelocity": -8.090853196926974e-16,
-            "velocityX": 2.746680956815585,
-            "velocityY": 0.19050728322074467,
-            "timestamp": 1.029873822481029
-        },
-        {
-            "x": 7.530253310036057,
-            "y": 5.786613706315055,
-            "heading": 3.14,
-            "angularVelocity": -6.531335051646392e-16,
-            "velocityX": 1.9678951063762244,
-            "velocityY": 0.13649140772934035,
-            "timestamp": 1.1234987154338498
-        },
-        {
-            "x": 7.641583530618614,
-            "y": 5.79433546876108,
-            "heading": 3.14,
-            "angularVelocity": -6.271183503983396e-16,
-            "velocityX": 1.1891091895683832,
-            "velocityY": 0.08247552763467879,
-            "timestamp": 1.2171236083866706
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": -6.05510209215834e-16,
-            "velocityX": 0.41032323957630595,
-            "velocityY": 0.028459645238388953,
-            "timestamp": 1.3107485013394913
-        },
-        {
-            "x": 7.6370908002198155,
-            "y": 5.794023857082849,
-            "heading": 3.14,
-            "angularVelocity": -4.789725529265609e-16,
-            "velocityX": -0.42651589559076464,
-            "velocityY": -0.02958275307434032,
-            "timestamp": 1.4113524887260582
-        },
-        {
-            "x": 7.509992248723152,
-            "y": 5.785208417602395,
-            "heading": 3.14,
-            "angularVelocity": -2.32893689231122e-16,
-            "velocityX": -1.2633550100583235,
-            "velocityY": -0.08762514995137034,
-            "timestamp": 1.5119564761126252
-        },
-        {
-            "x": 7.298704349055096,
-            "y": 5.770553681804523,
-            "heading": 3.14,
-            "angularVelocity": -1.4093989000106693e-18,
-            "velocityX": -2.1001940892878346,
-            "velocityY": -0.14566754438432153,
-            "timestamp": 1.6125604634991921
-        },
-        {
-            "x": 7.003227108598371,
-            "y": 5.75005965020129,
-            "heading": 3.14,
-            "angularVelocity": 2.7928007738396434e-16,
-            "velocityX": -2.9370330951333345,
-            "velocityY": -0.2037099337274247,
-            "timestamp": 1.713164450885759
-        },
-        {
-            "x": 6.623560552157282,
-            "y": 5.723726324513103,
-            "heading": 3.14,
-            "angularVelocity": 4.2079673901381875e-16,
-            "velocityX": -3.773871854424956,
-            "velocityY": -0.2617523059697759,
-            "timestamp": 1.813768438272326
-        },
-        {
-            "x": 6.243893779806084,
-            "y": 5.6973929838495865,
-            "heading": 3.14,
-            "angularVelocity": -4.939929195074519e-16,
-            "velocityX": -3.773874000563654,
-            "velocityY": -0.2617524548239938,
-            "timestamp": 1.914372425658893
-        },
-        {
-            "x": 5.864227007454884,
-            "y": 5.67105964318607,
-            "heading": 3.14,
-            "angularVelocity": 2.0250011365009936e-16,
-            "velocityX": -3.773874000563654,
-            "velocityY": -0.26175245482399373,
-            "timestamp": 2.01497641304546
-        },
-        {
-            "x": 5.484560235103686,
-            "y": 5.644726302522554,
-            "heading": 3.14,
-            "angularVelocity": -5.77312332256685e-16,
-            "velocityX": -3.773874000563654,
-            "velocityY": -0.2617524548239938,
-            "timestamp": 2.115580400432027
-        },
-        {
-            "x": 5.104893462752507,
-            "y": 5.61839296185904,
-            "heading": 3.14,
-            "angularVelocity": -1.6679194482656825e-16,
-            "velocityX": -3.773874000563452,
-            "velocityY": -0.26175245482397974,
-            "timestamp": 2.2161843878185943
-        },
-        {
-            "x": 4.768136096424167,
-            "y": 5.595035778417478,
-            "heading": 3.14,
-            "angularVelocity": 9.060703447293421e-16,
-            "velocityX": -3.3473560549281536,
-            "velocityY": -0.2321695595604253,
-            "timestamp": 2.3167883752051615
-        },
-        {
-            "x": 4.515568053067083,
-            "y": 5.577517889545478,
-            "heading": 3.14,
-            "angularVelocity": 6.700681499162007e-16,
-            "velocityX": -2.51051722618707,
-            "velocityY": -0.1741271825011227,
-            "timestamp": 2.4173923625917286
-        },
-        {
-            "x": 4.34718935210125,
-            "y": 5.565839296589989,
-            "heading": 3.14,
-            "angularVelocity": 4.365333326959493e-16,
-            "velocityX": -1.6736782044119611,
-            "velocityY": -0.11608479205315635,
-            "timestamp": 2.517996349978296
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 2.0339545203820728e-16,
-            "velocityX": -0.836839118292156,
-            "velocityY": -0.05804239714230061,
-            "timestamp": 2.618600337364863
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 2.71920432475143
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S1.1.traj
deleted file mode 100644
index 28f6b2dd..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S1.1.traj	
+++ /dev/null
@@ -1,292 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -4.804532357029023e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.321076149980653,
-            "y": 5.523337393191978,
-            "heading": 3.1351500057236232,
-            "angularVelocity": -0.05343391557798715,
-            "velocityX": 0.6398432489449108,
-            "velocityY": -0.40392349462987753,
-            "timestamp": 0.09076621512600157
-        },
-        {
-            "x": 4.4372316676864765,
-            "y": 5.45001014084527,
-            "heading": 3.1261940931811654,
-            "angularVelocity": -0.098670111230538,
-            "velocityX": 1.2797219487953386,
-            "velocityY": -0.807869450598055,
-            "timestamp": 0.18153243025200314
-        },
-        {
-            "x": 4.611470902956015,
-            "y": 5.340015489147945,
-            "heading": 3.1144364232978923,
-            "angularVelocity": -0.1295379549202309,
-            "velocityX": 1.9196485721880063,
-            "velocityY": -1.2118457461802494,
-            "timestamp": 0.2722986453780047
-        },
-        {
-            "x": 4.843797638177826,
-            "y": 5.193351042771614,
-            "heading": 3.102754458759954,
-            "angularVelocity": -0.1287038852696506,
-            "velocityX": 2.5596168673475677,
-            "velocityY": -1.6158484318502528,
-            "timestamp": 0.36306486050400627
-        },
-        {
-            "x": 5.1341460210960115,
-            "y": 5.0100588028009865,
-            "heading": 3.102754388459234,
-            "angularVelocity": -7.745251732579077e-7,
-            "velocityX": 3.1988596474483897,
-            "velocityY": -2.0193883783319753,
-            "timestamp": 0.45383107563000785
-        },
-        {
-            "x": 5.424494292461022,
-            "y": 4.826766239115272,
-            "heading": 3.1027543844284144,
-            "angularVelocity": -4.440880652615192e-8,
-            "velocityX": 3.1988584184318825,
-            "velocityY": -2.019391944803125,
-            "timestamp": 0.5445972907560094
-        },
-        {
-            "x": 5.714842563825266,
-            "y": 4.643473675428344,
-            "heading": 3.102754380397595,
-            "angularVelocity": -4.440880657505379e-8,
-            "velocityX": 3.1988584184234474,
-            "velocityY": -2.019391944816487,
-            "timestamp": 0.635363505882011
-        },
-        {
-            "x": 6.005190835190169,
-            "y": 4.460181111742461,
-            "heading": 3.102754376366776,
-            "angularVelocity": -4.4408806414514103e-8,
-            "velocityX": 3.198858418430713,
-            "velocityY": -2.019391944804976,
-            "timestamp": 0.7261297210080125
-        },
-        {
-            "x": 6.295539259817851,
-            "y": 4.276888790835792,
-            "heading": 3.1027543723359563,
-            "angularVelocity": -4.440881138366432e-8,
-            "velocityX": 3.1988601069750424,
-            "velocityY": -2.019389270029866,
-            "timestamp": 0.8168959361340141
-        },
-        {
-            "x": 6.605220322395041,
-            "y": 4.128578571418711,
-            "heading": 3.1027543676059754,
-            "angularVelocity": -5.211168523884132e-8,
-            "velocityX": 3.4118538725812453,
-            "velocityY": -1.633980432159657,
-            "timestamp": 0.9076621512600157
-        },
-        {
-            "x": 6.930843742926924,
-            "y": 4.047062548737473,
-            "heading": 3.096403794352487,
-            "angularVelocity": -0.0699662671256465,
-            "velocityX": 3.5874958549263174,
-            "velocityY": -0.8980877143337577,
-            "timestamp": 0.9984283663860173
-        },
-        {
-            "x": 7.20268686077656,
-            "y": 4.00824503573056,
-            "heading": 3.096579244871637,
-            "angularVelocity": 0.0019329936684692615,
-            "velocityX": 2.9949813096454783,
-            "velocityY": -0.4276647754125783,
-            "timestamp": 1.0891945815120188
-        },
-        {
-            "x": 7.418574834249914,
-            "y": 4.009226418838573,
-            "heading": 3.1038020925835856,
-            "angularVelocity": 0.07957638976044962,
-            "velocityX": 2.37850584794857,
-            "velocityY": 0.010812207016131005,
-            "timestamp": 1.1799607966380203
-        },
-        {
-            "x": 7.577796617923462,
-            "y": 4.04898509700118,
-            "heading": 3.1182467242773515,
-            "angularVelocity": 0.15914106007078685,
-            "velocityX": 1.7541965747113617,
-            "velocityY": 0.43803388857203507,
-            "timestamp": 1.270727011764022
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.23966269489643754,
-            "velocityX": 1.1260068730932273,
-            "velocityY": 0.8595147752996188,
-            "timestamp": 1.3614932268900235
-        },
-        {
-            "x": 7.725157318651882,
-            "y": 4.240486437949111,
-            "heading": 3.168528121789897,
-            "angularVelocity": 0.31944830036475336,
-            "velocityX": 0.505656446597267,
-            "velocityY": 1.2707829132351232,
-            "timestamp": 1.4507975736570093
-        },
-        {
-            "x": 7.714677077614687,
-            "y": 4.390346197703396,
-            "heading": 3.203767814945465,
-            "angularVelocity": 0.39460221625624564,
-            "velocityX": -0.11735420969529425,
-            "velocityY": 1.6780791213366064,
-            "timestamp": 1.5401019204239952
-        },
-        {
-            "x": 7.648174313953831,
-            "y": 4.575993018943536,
-            "heading": 3.2450393487188225,
-            "angularVelocity": 0.4621447361464423,
-            "velocityX": -0.7446755512850395,
-            "velocityY": 2.0788105838177287,
-            "timestamp": 1.629406267190981
-        },
-        {
-            "x": 7.524918843988051,
-            "y": 4.7962734130800495,
-            "heading": 3.2910178393067144,
-            "angularVelocity": 0.5148516533899635,
-            "velocityX": -1.3801732438330052,
-            "velocityY": 2.4666256695351096,
-            "timestamp": 1.7187106139579669
-        },
-        {
-            "x": 7.343009789801653,
-            "y": 5.047887037555876,
-            "heading": 3.3379953942154983,
-            "angularVelocity": 0.5260388391996106,
-            "velocityX": -2.0369563271207567,
-            "velocityY": 2.817484630757546,
-            "timestamp": 1.8080149607249527
-        },
-        {
-            "x": 7.098537759361482,
-            "y": 5.281049171644365,
-            "heading": 3.337995405344767,
-            "angularVelocity": 1.2462180234431456e-7,
-            "velocityX": -2.7375154658266756,
-            "velocityY": 2.6108710553234187,
-            "timestamp": 1.8973193074919386
-        },
-        {
-            "x": 6.821988530708413,
-            "y": 5.475089583706822,
-            "heading": 3.337995410181252,
-            "angularVelocity": 5.415732950343717e-8,
-            "velocityX": -3.096705128750813,
-            "velocityY": 2.1727991871297463,
-            "timestamp": 1.9866236542589244
-        },
-        {
-            "x": 6.5454389358432925,
-            "y": 5.669129473837467,
-            "heading": 3.3379954150177356,
-            "angularVelocity": 5.4157314847566374e-8,
-            "velocityX": -3.0967092294700773,
-            "velocityY": 2.172793342713053,
-            "timestamp": 2.0759280010259102
-        },
-        {
-            "x": 6.268889340976521,
-            "y": 5.86316936396576,
-            "heading": 3.337995419854219,
-            "angularVelocity": 5.415731402245697e-8,
-            "velocityX": -3.096709229488555,
-            "velocityY": 2.172793342686719,
-            "timestamp": 2.165232347792896
-        },
-        {
-            "x": 5.9923397461188825,
-            "y": 6.057209254107063,
-            "heading": 3.337995424690707,
-            "angularVelocity": 5.415735914657007e-8,
-            "velocityX": -3.0967092293862986,
-            "velocityY": 2.1727933428323993,
-            "timestamp": 2.2545366945598815
-        },
-        {
-            "x": 5.720218360303881,
-            "y": 6.248146363272753,
-            "heading": 3.35123648127501,
-            "angularVelocity": 0.14826889242971145,
-            "velocityX": -3.04712363581837,
-            "velocityY": 2.1380494463935373,
-            "timestamp": 2.343841041326867
-        },
-        {
-            "x": 5.5025203930179725,
-            "y": 6.400895625132601,
-            "heading": 3.369358291202865,
-            "angularVelocity": 0.20292192467559766,
-            "velocityX": -2.437708523348032,
-            "velocityY": 1.7104347928147705,
-            "timestamp": 2.4331453880938527
-        },
-        {
-            "x": 5.339255695066747,
-            "y": 6.515451087573219,
-            "heading": 3.385376867292448,
-            "angularVelocity": 0.17937062046238467,
-            "velocityX": -1.828183104874154,
-            "velocityY": 1.28275348947479,
-            "timestamp": 2.5224497348608383
-        },
-        {
-            "x": 5.230417322474398,
-            "y": 6.591817929626251,
-            "heading": 3.3970262318270823,
-            "angularVelocity": 0.1304456608929641,
-            "velocityX": -1.218735442702819,
-            "velocityY": 0.8551301791870324,
-            "timestamp": 2.611754081627824
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.06896725160058048,
-            "velocityX": -0.6093468509028436,
-            "velocityY": 0.42754996543868296,
-            "timestamp": 2.7010584283948096
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 2.6480923788288046e-29,
-            "velocityX": -1.5869679624644047e-31,
-            "velocityY": 0,
-            "timestamp": 2.790362775161795
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S1.traj
deleted file mode 100644
index 28f6b2dd..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S1.traj	
+++ /dev/null
@@ -1,292 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -4.804532357029023e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.321076149980653,
-            "y": 5.523337393191978,
-            "heading": 3.1351500057236232,
-            "angularVelocity": -0.05343391557798715,
-            "velocityX": 0.6398432489449108,
-            "velocityY": -0.40392349462987753,
-            "timestamp": 0.09076621512600157
-        },
-        {
-            "x": 4.4372316676864765,
-            "y": 5.45001014084527,
-            "heading": 3.1261940931811654,
-            "angularVelocity": -0.098670111230538,
-            "velocityX": 1.2797219487953386,
-            "velocityY": -0.807869450598055,
-            "timestamp": 0.18153243025200314
-        },
-        {
-            "x": 4.611470902956015,
-            "y": 5.340015489147945,
-            "heading": 3.1144364232978923,
-            "angularVelocity": -0.1295379549202309,
-            "velocityX": 1.9196485721880063,
-            "velocityY": -1.2118457461802494,
-            "timestamp": 0.2722986453780047
-        },
-        {
-            "x": 4.843797638177826,
-            "y": 5.193351042771614,
-            "heading": 3.102754458759954,
-            "angularVelocity": -0.1287038852696506,
-            "velocityX": 2.5596168673475677,
-            "velocityY": -1.6158484318502528,
-            "timestamp": 0.36306486050400627
-        },
-        {
-            "x": 5.1341460210960115,
-            "y": 5.0100588028009865,
-            "heading": 3.102754388459234,
-            "angularVelocity": -7.745251732579077e-7,
-            "velocityX": 3.1988596474483897,
-            "velocityY": -2.0193883783319753,
-            "timestamp": 0.45383107563000785
-        },
-        {
-            "x": 5.424494292461022,
-            "y": 4.826766239115272,
-            "heading": 3.1027543844284144,
-            "angularVelocity": -4.440880652615192e-8,
-            "velocityX": 3.1988584184318825,
-            "velocityY": -2.019391944803125,
-            "timestamp": 0.5445972907560094
-        },
-        {
-            "x": 5.714842563825266,
-            "y": 4.643473675428344,
-            "heading": 3.102754380397595,
-            "angularVelocity": -4.440880657505379e-8,
-            "velocityX": 3.1988584184234474,
-            "velocityY": -2.019391944816487,
-            "timestamp": 0.635363505882011
-        },
-        {
-            "x": 6.005190835190169,
-            "y": 4.460181111742461,
-            "heading": 3.102754376366776,
-            "angularVelocity": -4.4408806414514103e-8,
-            "velocityX": 3.198858418430713,
-            "velocityY": -2.019391944804976,
-            "timestamp": 0.7261297210080125
-        },
-        {
-            "x": 6.295539259817851,
-            "y": 4.276888790835792,
-            "heading": 3.1027543723359563,
-            "angularVelocity": -4.440881138366432e-8,
-            "velocityX": 3.1988601069750424,
-            "velocityY": -2.019389270029866,
-            "timestamp": 0.8168959361340141
-        },
-        {
-            "x": 6.605220322395041,
-            "y": 4.128578571418711,
-            "heading": 3.1027543676059754,
-            "angularVelocity": -5.211168523884132e-8,
-            "velocityX": 3.4118538725812453,
-            "velocityY": -1.633980432159657,
-            "timestamp": 0.9076621512600157
-        },
-        {
-            "x": 6.930843742926924,
-            "y": 4.047062548737473,
-            "heading": 3.096403794352487,
-            "angularVelocity": -0.0699662671256465,
-            "velocityX": 3.5874958549263174,
-            "velocityY": -0.8980877143337577,
-            "timestamp": 0.9984283663860173
-        },
-        {
-            "x": 7.20268686077656,
-            "y": 4.00824503573056,
-            "heading": 3.096579244871637,
-            "angularVelocity": 0.0019329936684692615,
-            "velocityX": 2.9949813096454783,
-            "velocityY": -0.4276647754125783,
-            "timestamp": 1.0891945815120188
-        },
-        {
-            "x": 7.418574834249914,
-            "y": 4.009226418838573,
-            "heading": 3.1038020925835856,
-            "angularVelocity": 0.07957638976044962,
-            "velocityX": 2.37850584794857,
-            "velocityY": 0.010812207016131005,
-            "timestamp": 1.1799607966380203
-        },
-        {
-            "x": 7.577796617923462,
-            "y": 4.04898509700118,
-            "heading": 3.1182467242773515,
-            "angularVelocity": 0.15914106007078685,
-            "velocityX": 1.7541965747113617,
-            "velocityY": 0.43803388857203507,
-            "timestamp": 1.270727011764022
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.23966269489643754,
-            "velocityX": 1.1260068730932273,
-            "velocityY": 0.8595147752996188,
-            "timestamp": 1.3614932268900235
-        },
-        {
-            "x": 7.725157318651882,
-            "y": 4.240486437949111,
-            "heading": 3.168528121789897,
-            "angularVelocity": 0.31944830036475336,
-            "velocityX": 0.505656446597267,
-            "velocityY": 1.2707829132351232,
-            "timestamp": 1.4507975736570093
-        },
-        {
-            "x": 7.714677077614687,
-            "y": 4.390346197703396,
-            "heading": 3.203767814945465,
-            "angularVelocity": 0.39460221625624564,
-            "velocityX": -0.11735420969529425,
-            "velocityY": 1.6780791213366064,
-            "timestamp": 1.5401019204239952
-        },
-        {
-            "x": 7.648174313953831,
-            "y": 4.575993018943536,
-            "heading": 3.2450393487188225,
-            "angularVelocity": 0.4621447361464423,
-            "velocityX": -0.7446755512850395,
-            "velocityY": 2.0788105838177287,
-            "timestamp": 1.629406267190981
-        },
-        {
-            "x": 7.524918843988051,
-            "y": 4.7962734130800495,
-            "heading": 3.2910178393067144,
-            "angularVelocity": 0.5148516533899635,
-            "velocityX": -1.3801732438330052,
-            "velocityY": 2.4666256695351096,
-            "timestamp": 1.7187106139579669
-        },
-        {
-            "x": 7.343009789801653,
-            "y": 5.047887037555876,
-            "heading": 3.3379953942154983,
-            "angularVelocity": 0.5260388391996106,
-            "velocityX": -2.0369563271207567,
-            "velocityY": 2.817484630757546,
-            "timestamp": 1.8080149607249527
-        },
-        {
-            "x": 7.098537759361482,
-            "y": 5.281049171644365,
-            "heading": 3.337995405344767,
-            "angularVelocity": 1.2462180234431456e-7,
-            "velocityX": -2.7375154658266756,
-            "velocityY": 2.6108710553234187,
-            "timestamp": 1.8973193074919386
-        },
-        {
-            "x": 6.821988530708413,
-            "y": 5.475089583706822,
-            "heading": 3.337995410181252,
-            "angularVelocity": 5.415732950343717e-8,
-            "velocityX": -3.096705128750813,
-            "velocityY": 2.1727991871297463,
-            "timestamp": 1.9866236542589244
-        },
-        {
-            "x": 6.5454389358432925,
-            "y": 5.669129473837467,
-            "heading": 3.3379954150177356,
-            "angularVelocity": 5.4157314847566374e-8,
-            "velocityX": -3.0967092294700773,
-            "velocityY": 2.172793342713053,
-            "timestamp": 2.0759280010259102
-        },
-        {
-            "x": 6.268889340976521,
-            "y": 5.86316936396576,
-            "heading": 3.337995419854219,
-            "angularVelocity": 5.415731402245697e-8,
-            "velocityX": -3.096709229488555,
-            "velocityY": 2.172793342686719,
-            "timestamp": 2.165232347792896
-        },
-        {
-            "x": 5.9923397461188825,
-            "y": 6.057209254107063,
-            "heading": 3.337995424690707,
-            "angularVelocity": 5.415735914657007e-8,
-            "velocityX": -3.0967092293862986,
-            "velocityY": 2.1727933428323993,
-            "timestamp": 2.2545366945598815
-        },
-        {
-            "x": 5.720218360303881,
-            "y": 6.248146363272753,
-            "heading": 3.35123648127501,
-            "angularVelocity": 0.14826889242971145,
-            "velocityX": -3.04712363581837,
-            "velocityY": 2.1380494463935373,
-            "timestamp": 2.343841041326867
-        },
-        {
-            "x": 5.5025203930179725,
-            "y": 6.400895625132601,
-            "heading": 3.369358291202865,
-            "angularVelocity": 0.20292192467559766,
-            "velocityX": -2.437708523348032,
-            "velocityY": 1.7104347928147705,
-            "timestamp": 2.4331453880938527
-        },
-        {
-            "x": 5.339255695066747,
-            "y": 6.515451087573219,
-            "heading": 3.385376867292448,
-            "angularVelocity": 0.17937062046238467,
-            "velocityX": -1.828183104874154,
-            "velocityY": 1.28275348947479,
-            "timestamp": 2.5224497348608383
-        },
-        {
-            "x": 5.230417322474398,
-            "y": 6.591817929626251,
-            "heading": 3.3970262318270823,
-            "angularVelocity": 0.1304456608929641,
-            "velocityX": -1.218735442702819,
-            "velocityY": 0.8551301791870324,
-            "timestamp": 2.611754081627824
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.06896725160058048,
-            "velocityX": -0.6093468509028436,
-            "velocityY": 0.42754996543868296,
-            "timestamp": 2.7010584283948096
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 2.6480923788288046e-29,
-            "velocityX": -1.5869679624644047e-31,
-            "velocityY": 0,
-            "timestamp": 2.790362775161795
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S2.1.traj
deleted file mode 100644
index 698e9db0..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S2.1.traj	
+++ /dev/null
@@ -1,283 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -2.7122542067664996e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 4.329648409276986,
-            "y": 5.532049408693613,
-            "heading": 3.14,
-            "angularVelocity": 3.6127527846474837e-17,
-            "velocityX": 0.715878164292546,
-            "velocityY": -0.3002204885663498,
-            "timestamp": 0.09310021257998485
-        },
-        {
-            "x": 4.462945224981569,
-            "y": 5.4761482272758,
-            "heading": 3.14,
-            "angularVelocity": 1.058102916611751e-16,
-            "velocityX": 1.4317562979794687,
-            "velocityY": -0.6004409642975056,
-            "timestamp": 0.1862004251599697
-        },
-        {
-            "x": 4.662890441407609,
-            "y": 5.392296458139565,
-            "heading": 3.14,
-            "angularVelocity": 1.669202321487025e-16,
-            "velocityX": 2.147634370376114,
-            "velocityY": -0.9006614143251297,
-            "timestamp": 0.27930063773995456
-        },
-        {
-            "x": 4.929484041376741,
-            "y": 5.280494108489063,
-            "heading": 3.14,
-            "angularVelocity": 1.646239395597301e-16,
-            "velocityX": 2.8635122582579458,
-            "velocityY": -1.2008817869720905,
-            "timestamp": 0.3724008503199394
-        },
-        {
-            "x": 5.2542718637391,
-            "y": 5.1442866313321245,
-            "heading": 3.14,
-            "angularVelocity": -2.6454281962617153e-17,
-            "velocityX": 3.4885830371581816,
-            "velocityY": -1.4630200445559467,
-            "timestamp": 0.46550106289992427
-        },
-        {
-            "x": 5.579059685941281,
-            "y": 5.008079154242361,
-            "heading": 3.14,
-            "angularVelocity": -9.771658175099026e-17,
-            "velocityX": 3.4885830354377245,
-            "velocityY": -1.463020043834432,
-            "timestamp": 0.5586012754799091
-        },
-        {
-            "x": 5.903847508143462,
-            "y": 4.871871677152596,
-            "heading": 3.14,
-            "angularVelocity": -3.1326214825920436e-16,
-            "velocityX": 3.488583035437725,
-            "velocityY": -1.4630200438344323,
-            "timestamp": 0.6517014880598939
-        },
-        {
-            "x": 6.228635330345643,
-            "y": 4.735664200062831,
-            "heading": 3.14,
-            "angularVelocity": -1.2137930063882497e-16,
-            "velocityX": 3.488583035437725,
-            "velocityY": -1.4630200438344325,
-            "timestamp": 0.7448017006398788
-        },
-        {
-            "x": 6.553423152547824,
-            "y": 4.599456722973066,
-            "heading": 3.14,
-            "angularVelocity": 1.3394779600477518e-16,
-            "velocityX": 3.488583035437725,
-            "velocityY": -1.4630200438344325,
-            "timestamp": 0.8379019132198637
-        },
-        {
-            "x": 6.878210974643575,
-            "y": 4.463249245927935,
-            "heading": 3.14,
-            "angularVelocity": 2.91910120342859e-17,
-            "velocityX": 3.488583034294527,
-            "velocityY": -1.4630200433550054,
-            "timestamp": 0.9310021257998486
-        },
-        {
-            "x": 7.17186557168986,
-            "y": 4.3400982252760985,
-            "heading": 3.14,
-            "angularVelocity": -6.102921228742647e-16,
-            "velocityX": 3.1541775137602737,
-            "velocityY": -1.3227791563413718,
-            "timestamp": 1.0241023383798336
-        },
-        {
-            "x": 7.3988717849138865,
-            "y": 4.2448977852556045,
-            "heading": 3.14,
-            "angularVelocity": -6.383129451815479e-16,
-            "velocityX": 2.4382996228798555,
-            "velocityY": -1.0225587824368827,
-            "timestamp": 1.1172025509598185
-        },
-        {
-            "x": 7.559229597346586,
-            "y": 4.1776479329828335,
-            "heading": 3.14,
-            "angularVelocity": -7.263779621185202e-16,
-            "velocityX": 1.7224215497327395,
-            "velocityY": -0.7223383320945309,
-            "timestamp": 1.2103027635398034
-        },
-        {
-            "x": 7.65293900332055,
-            "y": 4.138348670834548,
-            "heading": 3.14,
-            "angularVelocity": -7.150088563233795e-16,
-            "velocityX": 1.0065434157113666,
-            "velocityY": -0.42211785622311543,
-            "timestamp": 1.3034029761197883
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": -7.092042046536786e-16,
-            "velocityX": 0.2906652512305344,
-            "velocityY": -0.12189736757780324,
-            "timestamp": 1.3965031886997732
-        },
-        {
-            "x": 7.634550444733744,
-            "y": 4.146060349047862,
-            "heading": 3.14,
-            "angularVelocity": -8.252913531228688e-16,
-            "velocityX": -0.4634351183891671,
-            "velocityY": 0.19435250940932913,
-            "timestamp": 1.4945742097824644
-        },
-        {
-            "x": 7.515145498014414,
-            "y": 4.196135645696618,
-            "heading": 3.14,
-            "angularVelocity": -8.640334878143468e-16,
-            "velocityX": -1.217535469712484,
-            "velocityY": 0.5106023787234387,
-            "timestamp": 1.5926452308651555
-        },
-        {
-            "x": 7.321785162832602,
-            "y": 4.277225888692093,
-            "heading": 3.14,
-            "angularVelocity": -7.792870163094326e-16,
-            "velocityX": -1.971635790541653,
-            "velocityY": 0.826852235249104,
-            "timestamp": 1.6907162519478467
-        },
-        {
-            "x": 7.054469445169555,
-            "y": 4.389331075525908,
-            "heading": 3.14,
-            "angularVelocity": -9.085136126677818e-16,
-            "velocityX": -2.725736050381882,
-            "velocityY": 1.1431020661976106,
-            "timestamp": 1.7887872730305379
-        },
-        {
-            "x": 6.713198362969528,
-            "y": 4.532451198672714,
-            "heading": 3.14,
-            "angularVelocity": -7.684534164287992e-16,
-            "velocityX": -3.4798361272500533,
-            "velocityY": 1.4593518204124447,
-            "timestamp": 1.886858294113229
-        },
-        {
-            "x": 6.371069462531013,
-            "y": 4.675931068244968,
-            "heading": 3.14,
-            "angularVelocity": -1.8367347430029744e-16,
-            "velocityX": -3.4885830356557705,
-            "velocityY": 1.4630200439258751,
-            "timestamp": 1.9849293151959202
-        },
-        {
-            "x": 6.028940562092558,
-            "y": 4.819410937817198,
-            "heading": 3.14,
-            "angularVelocity": 1.6315422621813397e-16,
-            "velocityX": -3.48858303565516,
-            "velocityY": 1.463020043925619,
-            "timestamp": 2.0830003362786114
-        },
-        {
-            "x": 5.686811661654102,
-            "y": 4.962890807389427,
-            "heading": 3.14,
-            "angularVelocity": -1.6415107007869627e-16,
-            "velocityX": -3.4885830356551595,
-            "velocityY": 1.4630200439256191,
-            "timestamp": 2.1810713573613025
-        },
-        {
-            "x": 5.344682761215647,
-            "y": 5.106370676961656,
-            "heading": 3.14,
-            "angularVelocity": -2.6663466601394013e-16,
-            "velocityX": -3.48858303565516,
-            "velocityY": 1.4630200439256194,
-            "timestamp": 2.2791423784439937
-        },
-        {
-            "x": 5.002553860860093,
-            "y": 5.249850546499118,
-            "heading": 3.14,
-            "angularVelocity": -2.7119156877069447e-16,
-            "velocityX": -3.4885830348098277,
-            "velocityY": 1.4630200435711092,
-            "timestamp": 2.377213399526685
-        },
-        {
-            "x": 4.706732333820954,
-            "y": 5.373910320642252,
-            "heading": 3.14,
-            "angularVelocity": 2.3228605057035087e-16,
-            "velocityX": -3.01640100993532,
-            "velocityY": 1.264999311453705,
-            "timestamp": 2.475284420609376
-        },
-        {
-            "x": 4.4848661713885285,
-            "y": 5.466955158443148,
-            "heading": 3.14,
-            "angularVelocity": 1.3635627120067326e-16,
-            "velocityX": -2.262300932356051,
-            "velocityY": 0.9487495569406552,
-            "timestamp": 2.573355441692067
-        },
-        {
-            "x": 4.336955391458343,
-            "y": 5.528985052396896,
-            "heading": 3.14,
-            "angularVelocity": 8.074174127249248e-18,
-            "velocityX": -1.5082006723016521,
-            "velocityY": 0.6324997259023314,
-            "timestamp": 2.6714264627747584
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 3.321022798774287e-17,
-            "velocityX": -0.7541003513770345,
-            "velocityY": 0.31624986933663735,
-            "timestamp": 2.7694974838574495
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": -1.5033553036563547e-32,
-            "velocityY": 0,
-            "timestamp": 2.8675685049401407
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S2.traj
deleted file mode 100644
index 698e9db0..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S2.traj	
+++ /dev/null
@@ -1,283 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -2.7122542067664996e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 4.329648409276986,
-            "y": 5.532049408693613,
-            "heading": 3.14,
-            "angularVelocity": 3.6127527846474837e-17,
-            "velocityX": 0.715878164292546,
-            "velocityY": -0.3002204885663498,
-            "timestamp": 0.09310021257998485
-        },
-        {
-            "x": 4.462945224981569,
-            "y": 5.4761482272758,
-            "heading": 3.14,
-            "angularVelocity": 1.058102916611751e-16,
-            "velocityX": 1.4317562979794687,
-            "velocityY": -0.6004409642975056,
-            "timestamp": 0.1862004251599697
-        },
-        {
-            "x": 4.662890441407609,
-            "y": 5.392296458139565,
-            "heading": 3.14,
-            "angularVelocity": 1.669202321487025e-16,
-            "velocityX": 2.147634370376114,
-            "velocityY": -0.9006614143251297,
-            "timestamp": 0.27930063773995456
-        },
-        {
-            "x": 4.929484041376741,
-            "y": 5.280494108489063,
-            "heading": 3.14,
-            "angularVelocity": 1.646239395597301e-16,
-            "velocityX": 2.8635122582579458,
-            "velocityY": -1.2008817869720905,
-            "timestamp": 0.3724008503199394
-        },
-        {
-            "x": 5.2542718637391,
-            "y": 5.1442866313321245,
-            "heading": 3.14,
-            "angularVelocity": -2.6454281962617153e-17,
-            "velocityX": 3.4885830371581816,
-            "velocityY": -1.4630200445559467,
-            "timestamp": 0.46550106289992427
-        },
-        {
-            "x": 5.579059685941281,
-            "y": 5.008079154242361,
-            "heading": 3.14,
-            "angularVelocity": -9.771658175099026e-17,
-            "velocityX": 3.4885830354377245,
-            "velocityY": -1.463020043834432,
-            "timestamp": 0.5586012754799091
-        },
-        {
-            "x": 5.903847508143462,
-            "y": 4.871871677152596,
-            "heading": 3.14,
-            "angularVelocity": -3.1326214825920436e-16,
-            "velocityX": 3.488583035437725,
-            "velocityY": -1.4630200438344323,
-            "timestamp": 0.6517014880598939
-        },
-        {
-            "x": 6.228635330345643,
-            "y": 4.735664200062831,
-            "heading": 3.14,
-            "angularVelocity": -1.2137930063882497e-16,
-            "velocityX": 3.488583035437725,
-            "velocityY": -1.4630200438344325,
-            "timestamp": 0.7448017006398788
-        },
-        {
-            "x": 6.553423152547824,
-            "y": 4.599456722973066,
-            "heading": 3.14,
-            "angularVelocity": 1.3394779600477518e-16,
-            "velocityX": 3.488583035437725,
-            "velocityY": -1.4630200438344325,
-            "timestamp": 0.8379019132198637
-        },
-        {
-            "x": 6.878210974643575,
-            "y": 4.463249245927935,
-            "heading": 3.14,
-            "angularVelocity": 2.91910120342859e-17,
-            "velocityX": 3.488583034294527,
-            "velocityY": -1.4630200433550054,
-            "timestamp": 0.9310021257998486
-        },
-        {
-            "x": 7.17186557168986,
-            "y": 4.3400982252760985,
-            "heading": 3.14,
-            "angularVelocity": -6.102921228742647e-16,
-            "velocityX": 3.1541775137602737,
-            "velocityY": -1.3227791563413718,
-            "timestamp": 1.0241023383798336
-        },
-        {
-            "x": 7.3988717849138865,
-            "y": 4.2448977852556045,
-            "heading": 3.14,
-            "angularVelocity": -6.383129451815479e-16,
-            "velocityX": 2.4382996228798555,
-            "velocityY": -1.0225587824368827,
-            "timestamp": 1.1172025509598185
-        },
-        {
-            "x": 7.559229597346586,
-            "y": 4.1776479329828335,
-            "heading": 3.14,
-            "angularVelocity": -7.263779621185202e-16,
-            "velocityX": 1.7224215497327395,
-            "velocityY": -0.7223383320945309,
-            "timestamp": 1.2103027635398034
-        },
-        {
-            "x": 7.65293900332055,
-            "y": 4.138348670834548,
-            "heading": 3.14,
-            "angularVelocity": -7.150088563233795e-16,
-            "velocityX": 1.0065434157113666,
-            "velocityY": -0.42211785622311543,
-            "timestamp": 1.3034029761197883
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": -7.092042046536786e-16,
-            "velocityX": 0.2906652512305344,
-            "velocityY": -0.12189736757780324,
-            "timestamp": 1.3965031886997732
-        },
-        {
-            "x": 7.634550444733744,
-            "y": 4.146060349047862,
-            "heading": 3.14,
-            "angularVelocity": -8.252913531228688e-16,
-            "velocityX": -0.4634351183891671,
-            "velocityY": 0.19435250940932913,
-            "timestamp": 1.4945742097824644
-        },
-        {
-            "x": 7.515145498014414,
-            "y": 4.196135645696618,
-            "heading": 3.14,
-            "angularVelocity": -8.640334878143468e-16,
-            "velocityX": -1.217535469712484,
-            "velocityY": 0.5106023787234387,
-            "timestamp": 1.5926452308651555
-        },
-        {
-            "x": 7.321785162832602,
-            "y": 4.277225888692093,
-            "heading": 3.14,
-            "angularVelocity": -7.792870163094326e-16,
-            "velocityX": -1.971635790541653,
-            "velocityY": 0.826852235249104,
-            "timestamp": 1.6907162519478467
-        },
-        {
-            "x": 7.054469445169555,
-            "y": 4.389331075525908,
-            "heading": 3.14,
-            "angularVelocity": -9.085136126677818e-16,
-            "velocityX": -2.725736050381882,
-            "velocityY": 1.1431020661976106,
-            "timestamp": 1.7887872730305379
-        },
-        {
-            "x": 6.713198362969528,
-            "y": 4.532451198672714,
-            "heading": 3.14,
-            "angularVelocity": -7.684534164287992e-16,
-            "velocityX": -3.4798361272500533,
-            "velocityY": 1.4593518204124447,
-            "timestamp": 1.886858294113229
-        },
-        {
-            "x": 6.371069462531013,
-            "y": 4.675931068244968,
-            "heading": 3.14,
-            "angularVelocity": -1.8367347430029744e-16,
-            "velocityX": -3.4885830356557705,
-            "velocityY": 1.4630200439258751,
-            "timestamp": 1.9849293151959202
-        },
-        {
-            "x": 6.028940562092558,
-            "y": 4.819410937817198,
-            "heading": 3.14,
-            "angularVelocity": 1.6315422621813397e-16,
-            "velocityX": -3.48858303565516,
-            "velocityY": 1.463020043925619,
-            "timestamp": 2.0830003362786114
-        },
-        {
-            "x": 5.686811661654102,
-            "y": 4.962890807389427,
-            "heading": 3.14,
-            "angularVelocity": -1.6415107007869627e-16,
-            "velocityX": -3.4885830356551595,
-            "velocityY": 1.4630200439256191,
-            "timestamp": 2.1810713573613025
-        },
-        {
-            "x": 5.344682761215647,
-            "y": 5.106370676961656,
-            "heading": 3.14,
-            "angularVelocity": -2.6663466601394013e-16,
-            "velocityX": -3.48858303565516,
-            "velocityY": 1.4630200439256194,
-            "timestamp": 2.2791423784439937
-        },
-        {
-            "x": 5.002553860860093,
-            "y": 5.249850546499118,
-            "heading": 3.14,
-            "angularVelocity": -2.7119156877069447e-16,
-            "velocityX": -3.4885830348098277,
-            "velocityY": 1.4630200435711092,
-            "timestamp": 2.377213399526685
-        },
-        {
-            "x": 4.706732333820954,
-            "y": 5.373910320642252,
-            "heading": 3.14,
-            "angularVelocity": 2.3228605057035087e-16,
-            "velocityX": -3.01640100993532,
-            "velocityY": 1.264999311453705,
-            "timestamp": 2.475284420609376
-        },
-        {
-            "x": 4.4848661713885285,
-            "y": 5.466955158443148,
-            "heading": 3.14,
-            "angularVelocity": 1.3635627120067326e-16,
-            "velocityX": -2.262300932356051,
-            "velocityY": 0.9487495569406552,
-            "timestamp": 2.573355441692067
-        },
-        {
-            "x": 4.336955391458343,
-            "y": 5.528985052396896,
-            "heading": 3.14,
-            "angularVelocity": 8.074174127249248e-18,
-            "velocityX": -1.5082006723016521,
-            "velocityY": 0.6324997259023314,
-            "timestamp": 2.6714264627747584
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 3.321022798774287e-17,
-            "velocityX": -0.7541003513770345,
-            "velocityY": 0.31624986933663735,
-            "timestamp": 2.7694974838574495
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": -1.5033553036563547e-32,
-            "velocityY": 0,
-            "timestamp": 2.8675685049401407
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S3.1.traj
deleted file mode 100644
index 13a23ce8..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S3.1.traj	
+++ /dev/null
@@ -1,301 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -2.3834052223810005e-28,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.3290756213238994,
-            "y": 5.551919291811216,
-            "heading": 3.150896767326327,
-            "angularVelocity": 0.12189104355668522,
-            "velocityX": 0.7391207131092994,
-            "velocityY": -0.09039065663175272,
-            "timestamp": 0.08939760468350924
-        },
-        {
-            "x": 4.461244485010799,
-            "y": 5.535758895626619,
-            "heading": 3.171238121679265,
-            "angularVelocity": 0.22753802436807322,
-            "velocityX": 1.478438534845478,
-            "velocityY": -0.18076990140321392,
-            "timestamp": 0.17879520936701848
-        },
-        {
-            "x": 4.65953073652644,
-            "y": 5.511521259157241,
-            "heading": 3.1985778553739457,
-            "angularVelocity": 0.30582176996086496,
-            "velocityX": 2.2180264473277496,
-            "velocityY": -0.2711217661229951,
-            "timestamp": 0.26819281405052775
-        },
-        {
-            "x": 4.923962030908283,
-            "y": 5.4792142049190105,
-            "heading": 3.2279129750090267,
-            "angularVelocity": 0.328142121241728,
-            "velocityX": 2.9579237085624133,
-            "velocityY": -0.3613861283199711,
-            "timestamp": 0.35759041873403696
-        },
-        {
-            "x": 5.25442830298904,
-            "y": 5.438888092999425,
-            "heading": 3.243107062504638,
-            "angularVelocity": 0.1699607897635962,
-            "velocityX": 3.6965897828133016,
-            "velocityY": -0.4510871634824905,
-            "timestamp": 0.4469880234175462
-        },
-        {
-            "x": 5.590105329368368,
-            "y": 5.397771389697725,
-            "heading": 3.243107071754486,
-            "angularVelocity": 1.0346835398617793e-7,
-            "velocityX": 3.7548771867867403,
-            "velocityY": -0.45993070448202833,
-            "timestamp": 0.5363856281010554
-        },
-        {
-            "x": 5.925782355662538,
-            "y": 5.356654685708362,
-            "heading": 3.2431070810044496,
-            "angularVelocity": 1.0346963637170158e-7,
-            "velocityX": 3.7548771858341166,
-            "velocityY": -0.4599307121748044,
-            "timestamp": 0.6257832327845646
-        },
-        {
-            "x": 6.26145937114871,
-            "y": 5.315537893473946,
-            "heading": 3.243107090254437,
-            "angularVelocity": 1.0346990400921205e-7,
-            "velocityX": 3.754877064936017,
-            "velocityY": -0.45993169928168404,
-            "timestamp": 0.7151808374680738
-        },
-        {
-            "x": 6.594482534239531,
-            "y": 5.256671755131022,
-            "heading": 3.24310709962475,
-            "angularVelocity": 1.0481586200532189e-7,
-            "velocityX": 3.725191119725111,
-            "velocityY": -0.658475565944433,
-            "timestamp": 0.804578442151583
-        },
-        {
-            "x": 6.90949802384657,
-            "y": 5.13364781945348,
-            "heading": 3.243107110210723,
-            "angularVelocity": 1.1841422044120688e-7,
-            "velocityX": 3.523757607622681,
-            "velocityY": -1.3761435344273285,
-            "timestamp": 0.8939760468350922
-        },
-        {
-            "x": 7.194274658905514,
-            "y": 4.9512421336811645,
-            "heading": 3.243107206435074,
-            "angularVelocity": 0.0000010763582143626502,
-            "velocityX": 3.185506323875142,
-            "velocityY": -2.0403867242241884,
-            "timestamp": 0.9833736515186015
-        },
-        {
-            "x": 7.415334413339818,
-            "y": 4.749404498288232,
-            "heading": 3.236772739824744,
-            "angularVelocity": -0.07085722970314587,
-            "velocityX": 2.472770441853857,
-            "velocityY": -2.2577521635961224,
-            "timestamp": 1.0727712562021108
-        },
-        {
-            "x": 7.5700702382409935,
-            "y": 4.542039922027483,
-            "heading": 3.21849203872718,
-            "angularVelocity": -0.20448759406641112,
-            "velocityX": 1.7308721575766997,
-            "velocityY": -2.3195764248545423,
-            "timestamp": 1.16216886088562
-        },
-        {
-            "x": 7.658284258545904,
-            "y": 4.333806888908036,
-            "heading": 3.1865144497864955,
-            "angularVelocity": -0.3577007354214622,
-            "velocityX": 0.9867604463982368,
-            "velocityY": -2.3292909676746256,
-            "timestamp": 1.2515664655691292
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": -0.5203097996961199,
-            "velocityX": 0.24291189378586123,
-            "velocityY": -2.3133381441461327,
-            "timestamp": 1.3409640702526384
-        },
-        {
-            "x": 7.643813431477439,
-            "y": 3.93924818499591,
-            "heading": 3.084542602133154,
-            "angularVelocity": -0.6748386526461468,
-            "velocityX": -0.4403397217071775,
-            "velocityY": -2.284675926642036,
-            "timestamp": 1.4231428216475348
-        },
-        {
-            "x": 7.551490590699835,
-            "y": 3.7550289645599473,
-            "heading": 3.0195743357345632,
-            "angularVelocity": -0.7905725664570535,
-            "velocityX": -1.1234393223010666,
-            "velocityY": -2.2416892116036538,
-            "timestamp": 1.5053215730424312
-        },
-        {
-            "x": 7.403134940422789,
-            "y": 3.5762585586759936,
-            "heading": 2.9502561501390385,
-            "angularVelocity": -0.8435049744515962,
-            "velocityX": -1.8052799264521386,
-            "velocityY": -2.175384790327918,
-            "timestamp": 1.5875003244373276
-        },
-        {
-            "x": 7.199227882598224,
-            "y": 3.4065730339632285,
-            "heading": 2.886416818113751,
-            "angularVelocity": -0.7768350205485519,
-            "velocityX": -2.4812625448848915,
-            "velocityY": -2.064834544462633,
-            "timestamp": 1.669679075832224
-        },
-        {
-            "x": 6.942760679940155,
-            "y": 3.255041432853574,
-            "heading": 2.8546081791353424,
-            "angularVelocity": -0.3870664673889389,
-            "velocityX": -3.1208456967576708,
-            "velocityY": -1.843926787962309,
-            "timestamp": 1.7518578272271204
-        },
-        {
-            "x": 6.651351957860763,
-            "y": 3.1467563415044117,
-            "heading": 2.854608138694005,
-            "angularVelocity": -4.921174528263849e-7,
-            "velocityX": -3.5460349194222314,
-            "velocityY": -1.3176774957700823,
-            "timestamp": 1.8340365786220167
-        },
-        {
-            "x": 6.345190283970336,
-            "y": 3.092814329077674,
-            "heading": 2.85460812188226,
-            "angularVelocity": -2.045787704737326e-7,
-            "velocityX": -3.7255576252046376,
-            "velocityY": -0.6563985399585123,
-            "timestamp": 1.9162153300169131
-        },
-        {
-            "x": 6.034627693227853,
-            "y": 3.0788290052051352,
-            "heading": 2.8546081056797767,
-            "angularVelocity": -1.9716489468637527e-7,
-            "velocityX": -3.7791106030377564,
-            "velocityY": -0.17018175224969478,
-            "timestamp": 1.9983940814118095
-        },
-        {
-            "x": 5.724065078469444,
-            "y": 3.064844225150663,
-            "heading": 2.8546080894768826,
-            "angularVelocity": -1.9716993391114644e-7,
-            "velocityX": -3.7791108952905743,
-            "velocityY": -0.17017513428705025,
-            "timestamp": 2.080572832806706
-        },
-        {
-            "x": 5.413502463745052,
-            "y": 3.050859444277176,
-            "heading": 2.8546080732739703,
-            "angularVelocity": -1.9717015240502906e-7,
-            "velocityX": -3.7791108948748344,
-            "velocityY": -0.170175144292348,
-            "timestamp": 2.1627515842016023
-        },
-        {
-            "x": 5.102939848618815,
-            "y": 3.0368746723382456,
-            "heading": 2.854608057073056,
-            "angularVelocity": -1.9714443134713148e-7,
-            "velocityX": -3.779110899765686,
-            "velocityY": -0.17017503556443397,
-            "timestamp": 2.2449303355964987
-        },
-        {
-            "x": 4.82269393470373,
-            "y": 3.0246815634320945,
-            "heading": 2.775318198682859,
-            "angularVelocity": -0.9648462289850903,
-            "velocityX": -3.4101992200919007,
-            "velocityY": -0.1483730125178458,
-            "timestamp": 2.327109086991395
-        },
-        {
-            "x": 4.59870268568745,
-            "y": 3.0148363143730834,
-            "heading": 2.699114427610376,
-            "angularVelocity": -0.927292880108085,
-            "velocityX": -2.725658947290856,
-            "velocityY": -0.11980285529234247,
-            "timestamp": 2.4092878383862915
-        },
-        {
-            "x": 4.43080987518015,
-            "y": 3.007426917155393,
-            "heading": 2.637997904363483,
-            "angularVelocity": -0.7437022612594317,
-            "velocityX": -2.043019730258355,
-            "velocityY": -0.09016195906753964,
-            "timestamp": 2.491466589781188
-        },
-        {
-            "x": 4.318925850575796,
-            "y": 3.002477699207252,
-            "heading": 2.595691429442944,
-            "angularVelocity": -0.5148103882976692,
-            "velocityX": -1.361471459539352,
-            "velocityY": -0.06022503220498214,
-            "timestamp": 2.5736453411760842
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -0.26395423478763,
-            "velocityX": -0.6805390642559688,
-            "velocityY": -0.03015011990058865,
-            "timestamp": 2.6558240925709806
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 2.4341219671984428e-29,
-            "velocityY": 0,
-            "timestamp": 2.738002843965877
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S3.traj
deleted file mode 100644
index 13a23ce8..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S3.traj	
+++ /dev/null
@@ -1,301 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -2.3834052223810005e-28,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.3290756213238994,
-            "y": 5.551919291811216,
-            "heading": 3.150896767326327,
-            "angularVelocity": 0.12189104355668522,
-            "velocityX": 0.7391207131092994,
-            "velocityY": -0.09039065663175272,
-            "timestamp": 0.08939760468350924
-        },
-        {
-            "x": 4.461244485010799,
-            "y": 5.535758895626619,
-            "heading": 3.171238121679265,
-            "angularVelocity": 0.22753802436807322,
-            "velocityX": 1.478438534845478,
-            "velocityY": -0.18076990140321392,
-            "timestamp": 0.17879520936701848
-        },
-        {
-            "x": 4.65953073652644,
-            "y": 5.511521259157241,
-            "heading": 3.1985778553739457,
-            "angularVelocity": 0.30582176996086496,
-            "velocityX": 2.2180264473277496,
-            "velocityY": -0.2711217661229951,
-            "timestamp": 0.26819281405052775
-        },
-        {
-            "x": 4.923962030908283,
-            "y": 5.4792142049190105,
-            "heading": 3.2279129750090267,
-            "angularVelocity": 0.328142121241728,
-            "velocityX": 2.9579237085624133,
-            "velocityY": -0.3613861283199711,
-            "timestamp": 0.35759041873403696
-        },
-        {
-            "x": 5.25442830298904,
-            "y": 5.438888092999425,
-            "heading": 3.243107062504638,
-            "angularVelocity": 0.1699607897635962,
-            "velocityX": 3.6965897828133016,
-            "velocityY": -0.4510871634824905,
-            "timestamp": 0.4469880234175462
-        },
-        {
-            "x": 5.590105329368368,
-            "y": 5.397771389697725,
-            "heading": 3.243107071754486,
-            "angularVelocity": 1.0346835398617793e-7,
-            "velocityX": 3.7548771867867403,
-            "velocityY": -0.45993070448202833,
-            "timestamp": 0.5363856281010554
-        },
-        {
-            "x": 5.925782355662538,
-            "y": 5.356654685708362,
-            "heading": 3.2431070810044496,
-            "angularVelocity": 1.0346963637170158e-7,
-            "velocityX": 3.7548771858341166,
-            "velocityY": -0.4599307121748044,
-            "timestamp": 0.6257832327845646
-        },
-        {
-            "x": 6.26145937114871,
-            "y": 5.315537893473946,
-            "heading": 3.243107090254437,
-            "angularVelocity": 1.0346990400921205e-7,
-            "velocityX": 3.754877064936017,
-            "velocityY": -0.45993169928168404,
-            "timestamp": 0.7151808374680738
-        },
-        {
-            "x": 6.594482534239531,
-            "y": 5.256671755131022,
-            "heading": 3.24310709962475,
-            "angularVelocity": 1.0481586200532189e-7,
-            "velocityX": 3.725191119725111,
-            "velocityY": -0.658475565944433,
-            "timestamp": 0.804578442151583
-        },
-        {
-            "x": 6.90949802384657,
-            "y": 5.13364781945348,
-            "heading": 3.243107110210723,
-            "angularVelocity": 1.1841422044120688e-7,
-            "velocityX": 3.523757607622681,
-            "velocityY": -1.3761435344273285,
-            "timestamp": 0.8939760468350922
-        },
-        {
-            "x": 7.194274658905514,
-            "y": 4.9512421336811645,
-            "heading": 3.243107206435074,
-            "angularVelocity": 0.0000010763582143626502,
-            "velocityX": 3.185506323875142,
-            "velocityY": -2.0403867242241884,
-            "timestamp": 0.9833736515186015
-        },
-        {
-            "x": 7.415334413339818,
-            "y": 4.749404498288232,
-            "heading": 3.236772739824744,
-            "angularVelocity": -0.07085722970314587,
-            "velocityX": 2.472770441853857,
-            "velocityY": -2.2577521635961224,
-            "timestamp": 1.0727712562021108
-        },
-        {
-            "x": 7.5700702382409935,
-            "y": 4.542039922027483,
-            "heading": 3.21849203872718,
-            "angularVelocity": -0.20448759406641112,
-            "velocityX": 1.7308721575766997,
-            "velocityY": -2.3195764248545423,
-            "timestamp": 1.16216886088562
-        },
-        {
-            "x": 7.658284258545904,
-            "y": 4.333806888908036,
-            "heading": 3.1865144497864955,
-            "angularVelocity": -0.3577007354214622,
-            "velocityX": 0.9867604463982368,
-            "velocityY": -2.3292909676746256,
-            "timestamp": 1.2515664655691292
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": -0.5203097996961199,
-            "velocityX": 0.24291189378586123,
-            "velocityY": -2.3133381441461327,
-            "timestamp": 1.3409640702526384
-        },
-        {
-            "x": 7.643813431477439,
-            "y": 3.93924818499591,
-            "heading": 3.084542602133154,
-            "angularVelocity": -0.6748386526461468,
-            "velocityX": -0.4403397217071775,
-            "velocityY": -2.284675926642036,
-            "timestamp": 1.4231428216475348
-        },
-        {
-            "x": 7.551490590699835,
-            "y": 3.7550289645599473,
-            "heading": 3.0195743357345632,
-            "angularVelocity": -0.7905725664570535,
-            "velocityX": -1.1234393223010666,
-            "velocityY": -2.2416892116036538,
-            "timestamp": 1.5053215730424312
-        },
-        {
-            "x": 7.403134940422789,
-            "y": 3.5762585586759936,
-            "heading": 2.9502561501390385,
-            "angularVelocity": -0.8435049744515962,
-            "velocityX": -1.8052799264521386,
-            "velocityY": -2.175384790327918,
-            "timestamp": 1.5875003244373276
-        },
-        {
-            "x": 7.199227882598224,
-            "y": 3.4065730339632285,
-            "heading": 2.886416818113751,
-            "angularVelocity": -0.7768350205485519,
-            "velocityX": -2.4812625448848915,
-            "velocityY": -2.064834544462633,
-            "timestamp": 1.669679075832224
-        },
-        {
-            "x": 6.942760679940155,
-            "y": 3.255041432853574,
-            "heading": 2.8546081791353424,
-            "angularVelocity": -0.3870664673889389,
-            "velocityX": -3.1208456967576708,
-            "velocityY": -1.843926787962309,
-            "timestamp": 1.7518578272271204
-        },
-        {
-            "x": 6.651351957860763,
-            "y": 3.1467563415044117,
-            "heading": 2.854608138694005,
-            "angularVelocity": -4.921174528263849e-7,
-            "velocityX": -3.5460349194222314,
-            "velocityY": -1.3176774957700823,
-            "timestamp": 1.8340365786220167
-        },
-        {
-            "x": 6.345190283970336,
-            "y": 3.092814329077674,
-            "heading": 2.85460812188226,
-            "angularVelocity": -2.045787704737326e-7,
-            "velocityX": -3.7255576252046376,
-            "velocityY": -0.6563985399585123,
-            "timestamp": 1.9162153300169131
-        },
-        {
-            "x": 6.034627693227853,
-            "y": 3.0788290052051352,
-            "heading": 2.8546081056797767,
-            "angularVelocity": -1.9716489468637527e-7,
-            "velocityX": -3.7791106030377564,
-            "velocityY": -0.17018175224969478,
-            "timestamp": 1.9983940814118095
-        },
-        {
-            "x": 5.724065078469444,
-            "y": 3.064844225150663,
-            "heading": 2.8546080894768826,
-            "angularVelocity": -1.9716993391114644e-7,
-            "velocityX": -3.7791108952905743,
-            "velocityY": -0.17017513428705025,
-            "timestamp": 2.080572832806706
-        },
-        {
-            "x": 5.413502463745052,
-            "y": 3.050859444277176,
-            "heading": 2.8546080732739703,
-            "angularVelocity": -1.9717015240502906e-7,
-            "velocityX": -3.7791108948748344,
-            "velocityY": -0.170175144292348,
-            "timestamp": 2.1627515842016023
-        },
-        {
-            "x": 5.102939848618815,
-            "y": 3.0368746723382456,
-            "heading": 2.854608057073056,
-            "angularVelocity": -1.9714443134713148e-7,
-            "velocityX": -3.779110899765686,
-            "velocityY": -0.17017503556443397,
-            "timestamp": 2.2449303355964987
-        },
-        {
-            "x": 4.82269393470373,
-            "y": 3.0246815634320945,
-            "heading": 2.775318198682859,
-            "angularVelocity": -0.9648462289850903,
-            "velocityX": -3.4101992200919007,
-            "velocityY": -0.1483730125178458,
-            "timestamp": 2.327109086991395
-        },
-        {
-            "x": 4.59870268568745,
-            "y": 3.0148363143730834,
-            "heading": 2.699114427610376,
-            "angularVelocity": -0.927292880108085,
-            "velocityX": -2.725658947290856,
-            "velocityY": -0.11980285529234247,
-            "timestamp": 2.4092878383862915
-        },
-        {
-            "x": 4.43080987518015,
-            "y": 3.007426917155393,
-            "heading": 2.637997904363483,
-            "angularVelocity": -0.7437022612594317,
-            "velocityX": -2.043019730258355,
-            "velocityY": -0.09016195906753964,
-            "timestamp": 2.491466589781188
-        },
-        {
-            "x": 4.318925850575796,
-            "y": 3.002477699207252,
-            "heading": 2.595691429442944,
-            "angularVelocity": -0.5148103882976692,
-            "velocityX": -1.361471459539352,
-            "velocityY": -0.06022503220498214,
-            "timestamp": 2.5736453411760842
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -0.26395423478763,
-            "velocityX": -0.6805390642559688,
-            "velocityY": -0.03015011990058865,
-            "timestamp": 2.6558240925709806
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 2.4341219671984428e-29,
-            "velocityY": 0,
-            "timestamp": 2.738002843965877
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S4.1.traj
deleted file mode 100644
index 20dd60b7..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S4.1.traj	
+++ /dev/null
@@ -1,310 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -2.6674512845646027e-30,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.322765898095933,
-            "y": 5.556151172121073,
-            "heading": 3.1479530411377072,
-            "angularVelocity": 0.09380834863319021,
-            "velocityX": 0.7049555142252463,
-            "velocityY": -0.04539800325896039,
-            "timestamp": 0.08477967317075337
-        },
-        {
-            "x": 4.442297694457063,
-            "y": 5.548453524989103,
-            "heading": 3.163858908065189,
-            "angularVelocity": 0.18761415717719215,
-            "velocityX": 1.4099110304470142,
-            "velocityY": -0.09079590477320977,
-            "timestamp": 0.16955934634150674
-        },
-        {
-            "x": 4.621595390179887,
-            "y": 5.536907071113832,
-            "heading": 3.1877171391651413,
-            "angularVelocity": 0.28141452081573415,
-            "velocityX": 2.1148665596022633,
-            "velocityY": -0.13619365873154282,
-            "timestamp": 0.2543390195122601
-        },
-        {
-            "x": 4.860658986379666,
-            "y": 5.521511826336959,
-            "heading": 3.2195268977958102,
-            "angularVelocity": 0.3752050160303855,
-            "velocityX": 2.819822101912331,
-            "velocityY": -0.1815912258326068,
-            "timestamp": 0.3391186926830135
-        },
-        {
-            "x": 5.159488476433141,
-            "y": 5.5022678093145005,
-            "heading": 3.2592867547687217,
-            "angularVelocity": 0.46897865356654944,
-            "velocityX": 3.524777566099216,
-            "velocityY": -0.22698857288071755,
-            "timestamp": 0.42389836585376683
-        },
-        {
-            "x": 5.47954195294372,
-            "y": 5.481656666167451,
-            "heading": 3.2592867595783463,
-            "angularVelocity": 5.673087575653179e-8,
-            "velocityX": 3.7751204332431567,
-            "velocityY": -0.24311420858382377,
-            "timestamp": 0.5086780390245202
-        },
-        {
-            "x": 5.799595429451853,
-            "y": 5.461045522982243,
-            "heading": 3.2592867643879506,
-            "angularVelocity": 5.6730628986964756e-8,
-            "velocityX": 3.7751204332143167,
-            "velocityY": -0.2431142090339428,
-            "timestamp": 0.5934577121952737
-        },
-        {
-            "x": 6.1196488941034595,
-            "y": 5.440434195687293,
-            "heading": 3.259286769197555,
-            "angularVelocity": 5.673063176342314e-8,
-            "velocityX": 3.7751202933632646,
-            "velocityY": -0.24311638065988433,
-            "timestamp": 0.6782373853660271
-        },
-        {
-            "x": 6.4368892365574455,
-            "y": 5.393342668998585,
-            "heading": 3.2592867741163385,
-            "angularVelocity": 5.801842614465255e-8,
-            "velocityX": 3.741938728815606,
-            "velocityY": -0.5554577521660955,
-            "timestamp": 0.7630170585367805
-        },
-        {
-            "x": 6.7398295350074555,
-            "y": 5.288051546129373,
-            "heading": 3.2592867797575464,
-            "angularVelocity": 6.653963355876342e-8,
-            "velocityX": 3.5732657029690613,
-            "velocityY": -1.2419382964267203,
-            "timestamp": 0.8477967317075339
-        },
-        {
-            "x": 7.017891386263095,
-            "y": 5.128237130868345,
-            "heading": 3.259286787454074,
-            "angularVelocity": 9.07826943070083e-8,
-            "velocityX": 3.2798174474623285,
-            "velocityY": -1.8850558074130808,
-            "timestamp": 0.9325764048782873
-        },
-        {
-            "x": 7.261365099007722,
-            "y": 4.919479947434011,
-            "heading": 3.259286723898943,
-            "angularVelocity": -7.496505777325929e-7,
-            "velocityX": 2.8718406622562314,
-            "velocityY": -2.462349471599812,
-            "timestamp": 1.0173560780490407
-        },
-        {
-            "x": 7.457139881435657,
-            "y": 4.674807078885688,
-            "heading": 3.2408811636287,
-            "angularVelocity": -0.21709874055771505,
-            "velocityX": 2.309218414106716,
-            "velocityY": -2.885985040961276,
-            "timestamp": 1.102135751219794
-        },
-        {
-            "x": 7.597538484149158,
-            "y": 4.408115178432638,
-            "heading": 3.1997348588691503,
-            "angularVelocity": -0.48533219368821684,
-            "velocityX": 1.6560408581747834,
-            "velocityY": -3.1457056919299244,
-            "timestamp": 1.1869154243905473
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": -0.7045893978594314,
-            "velocityX": 0.9726566848724733,
-            "velocityY": -3.315832297049189,
-            "timestamp": 1.2716950975613006
-        },
-        {
-            "x": 7.707365632796218,
-            "y": 3.8777713806946585,
-            "heading": 3.0766920768105654,
-            "angularVelocity": -0.8684660682637303,
-            "velocityX": 0.37540519927184846,
-            "velocityY": -3.418949607021354,
-            "timestamp": 1.344591351660309
-        },
-        {
-            "x": 7.690646428306815,
-            "y": 3.624138174667908,
-            "heading": 3.0114310872436763,
-            "angularVelocity": -0.8952584789497043,
-            "velocityX": -0.229356154113369,
-            "velocityY": -3.4793722827239795,
-            "timestamp": 1.4174876057593173
-        },
-        {
-            "x": 7.629817161501398,
-            "y": 3.371341042658395,
-            "heading": 2.9608367604127053,
-            "angularVelocity": -0.6940593512835794,
-            "velocityX": -0.8344635476447348,
-            "velocityY": -3.4679029139977815,
-            "timestamp": 1.4903838598583257
-        },
-        {
-            "x": 7.52574877175356,
-            "y": 3.1246562301261127,
-            "heading": 2.9373487413645845,
-            "angularVelocity": -0.32221160522605474,
-            "velocityX": -1.4276232850943003,
-            "velocityY": -3.3840533451439727,
-            "timestamp": 1.563280113957334
-        },
-        {
-            "x": 7.379748340613123,
-            "y": 2.890715038062695,
-            "heading": 2.9373478462882416,
-            "angularVelocity": -0.00001227877368205191,
-            "velocityX": -2.002852313126858,
-            "velocityY": -3.2092347536203247,
-            "timestamp": 1.6361763680563424
-        },
-        {
-            "x": 7.198165609339985,
-            "y": 2.6831759130730863,
-            "heading": 2.937347790914402,
-            "angularVelocity": -7.596252972489614e-7,
-            "velocityX": -2.4909747903727215,
-            "velocityY": -2.847047870356197,
-            "timestamp": 1.7090726221553507
-        },
-        {
-            "x": 6.985688529633763,
-            "y": 2.5073968727951343,
-            "heading": 2.9373477481889543,
-            "angularVelocity": -5.861130796388339e-7,
-            "velocityX": -2.91478735543415,
-            "velocityY": -2.4113590259261937,
-            "timestamp": 1.781968876254359
-        },
-        {
-            "x": 6.7478025524071645,
-            "y": 2.367915534392956,
-            "heading": 2.937347710107927,
-            "angularVelocity": -5.224003305629458e-7,
-            "velocityX": -3.263349813607291,
-            "velocityY": -1.9134225774154996,
-            "timestamp": 1.8548651303533674
-        },
-        {
-            "x": 6.4987639591558635,
-            "y": 2.249490193910499,
-            "heading": 2.937347672689208,
-            "angularVelocity": -5.133147047667194e-7,
-            "velocityX": -3.4163428056670964,
-            "velocityY": -1.624573744513203,
-            "timestamp": 1.9277613844523758
-        },
-        {
-            "x": 6.249725239429151,
-            "y": 2.1310651193958834,
-            "heading": 2.937347635270487,
-            "angularVelocity": -5.133147287283584e-7,
-            "velocityX": -3.4163445406727813,
-            "velocityY": -1.6245700959327491,
-            "timestamp": 2.000657638551384
-        },
-        {
-            "x": 6.0006865195043355,
-            "y": 2.012640045297908,
-            "heading": 2.937347597851683,
-            "angularVelocity": -5.133158656564982e-7,
-            "velocityX": -3.4163445433903696,
-            "velocityY": -1.6245700902172364,
-            "timestamp": 2.0735538926503922
-        },
-        {
-            "x": 5.766671175118509,
-            "y": 1.901395758932124,
-            "heading": 2.8964169231036223,
-            "angularVelocity": -0.5614921542139502,
-            "velocityX": -3.2102519845241893,
-            "velocityY": -1.5260631391951232,
-            "timestamp": 2.1464501467494004
-        },
-        {
-            "x": 5.568987668741884,
-            "y": 1.8073555304166649,
-            "heading": 2.790644028809865,
-            "angularVelocity": -1.4510058932587933,
-            "velocityX": -2.7118472522341635,
-            "velocityY": -1.2900557055764181,
-            "timestamp": 2.2193464008484085
-        },
-        {
-            "x": 5.41134573614175,
-            "y": 1.7322845721056195,
-            "heading": 2.684499308002556,
-            "angularVelocity": -1.456106656228851,
-            "velocityX": -2.162551897193064,
-            "velocityY": -1.029832866434731,
-            "timestamp": 2.2922426549474166
-        },
-        {
-            "x": 5.293483125084031,
-            "y": 1.6760930732996284,
-            "heading": 2.5959515176489267,
-            "angularVelocity": -1.2147097467272172,
-            "velocityX": -1.6168541513476236,
-            "velocityY": -0.7708420617846653,
-            "timestamp": 2.3651389090464248
-        },
-        {
-            "x": 5.2151080549458655,
-            "y": 1.6386848703250345,
-            "heading": 2.5329322459715486,
-            "angularVelocity": -0.8645063104618199,
-            "velocityX": -1.075159088856404,
-            "velocityY": -0.5131704425282329,
-            "timestamp": 2.438035163145433
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.45176870030861394,
-            "velocityX": -0.536489226081922,
-            "velocityY": -0.25632140575671664,
-            "timestamp": 2.510931417244441
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": 2.4074157612584747e-31,
-            "velocityY": 0,
-            "timestamp": 2.583827671343449
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S4.traj
deleted file mode 100644
index 20dd60b7..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3 To S4.traj	
+++ /dev/null
@@ -1,310 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -2.6674512845646027e-30,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.322765898095933,
-            "y": 5.556151172121073,
-            "heading": 3.1479530411377072,
-            "angularVelocity": 0.09380834863319021,
-            "velocityX": 0.7049555142252463,
-            "velocityY": -0.04539800325896039,
-            "timestamp": 0.08477967317075337
-        },
-        {
-            "x": 4.442297694457063,
-            "y": 5.548453524989103,
-            "heading": 3.163858908065189,
-            "angularVelocity": 0.18761415717719215,
-            "velocityX": 1.4099110304470142,
-            "velocityY": -0.09079590477320977,
-            "timestamp": 0.16955934634150674
-        },
-        {
-            "x": 4.621595390179887,
-            "y": 5.536907071113832,
-            "heading": 3.1877171391651413,
-            "angularVelocity": 0.28141452081573415,
-            "velocityX": 2.1148665596022633,
-            "velocityY": -0.13619365873154282,
-            "timestamp": 0.2543390195122601
-        },
-        {
-            "x": 4.860658986379666,
-            "y": 5.521511826336959,
-            "heading": 3.2195268977958102,
-            "angularVelocity": 0.3752050160303855,
-            "velocityX": 2.819822101912331,
-            "velocityY": -0.1815912258326068,
-            "timestamp": 0.3391186926830135
-        },
-        {
-            "x": 5.159488476433141,
-            "y": 5.5022678093145005,
-            "heading": 3.2592867547687217,
-            "angularVelocity": 0.46897865356654944,
-            "velocityX": 3.524777566099216,
-            "velocityY": -0.22698857288071755,
-            "timestamp": 0.42389836585376683
-        },
-        {
-            "x": 5.47954195294372,
-            "y": 5.481656666167451,
-            "heading": 3.2592867595783463,
-            "angularVelocity": 5.673087575653179e-8,
-            "velocityX": 3.7751204332431567,
-            "velocityY": -0.24311420858382377,
-            "timestamp": 0.5086780390245202
-        },
-        {
-            "x": 5.799595429451853,
-            "y": 5.461045522982243,
-            "heading": 3.2592867643879506,
-            "angularVelocity": 5.6730628986964756e-8,
-            "velocityX": 3.7751204332143167,
-            "velocityY": -0.2431142090339428,
-            "timestamp": 0.5934577121952737
-        },
-        {
-            "x": 6.1196488941034595,
-            "y": 5.440434195687293,
-            "heading": 3.259286769197555,
-            "angularVelocity": 5.673063176342314e-8,
-            "velocityX": 3.7751202933632646,
-            "velocityY": -0.24311638065988433,
-            "timestamp": 0.6782373853660271
-        },
-        {
-            "x": 6.4368892365574455,
-            "y": 5.393342668998585,
-            "heading": 3.2592867741163385,
-            "angularVelocity": 5.801842614465255e-8,
-            "velocityX": 3.741938728815606,
-            "velocityY": -0.5554577521660955,
-            "timestamp": 0.7630170585367805
-        },
-        {
-            "x": 6.7398295350074555,
-            "y": 5.288051546129373,
-            "heading": 3.2592867797575464,
-            "angularVelocity": 6.653963355876342e-8,
-            "velocityX": 3.5732657029690613,
-            "velocityY": -1.2419382964267203,
-            "timestamp": 0.8477967317075339
-        },
-        {
-            "x": 7.017891386263095,
-            "y": 5.128237130868345,
-            "heading": 3.259286787454074,
-            "angularVelocity": 9.07826943070083e-8,
-            "velocityX": 3.2798174474623285,
-            "velocityY": -1.8850558074130808,
-            "timestamp": 0.9325764048782873
-        },
-        {
-            "x": 7.261365099007722,
-            "y": 4.919479947434011,
-            "heading": 3.259286723898943,
-            "angularVelocity": -7.496505777325929e-7,
-            "velocityX": 2.8718406622562314,
-            "velocityY": -2.462349471599812,
-            "timestamp": 1.0173560780490407
-        },
-        {
-            "x": 7.457139881435657,
-            "y": 4.674807078885688,
-            "heading": 3.2408811636287,
-            "angularVelocity": -0.21709874055771505,
-            "velocityX": 2.309218414106716,
-            "velocityY": -2.885985040961276,
-            "timestamp": 1.102135751219794
-        },
-        {
-            "x": 7.597538484149158,
-            "y": 4.408115178432638,
-            "heading": 3.1997348588691503,
-            "angularVelocity": -0.48533219368821684,
-            "velocityX": 1.6560408581747834,
-            "velocityY": -3.1457056919299244,
-            "timestamp": 1.1869154243905473
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": -0.7045893978594314,
-            "velocityX": 0.9726566848724733,
-            "velocityY": -3.315832297049189,
-            "timestamp": 1.2716950975613006
-        },
-        {
-            "x": 7.707365632796218,
-            "y": 3.8777713806946585,
-            "heading": 3.0766920768105654,
-            "angularVelocity": -0.8684660682637303,
-            "velocityX": 0.37540519927184846,
-            "velocityY": -3.418949607021354,
-            "timestamp": 1.344591351660309
-        },
-        {
-            "x": 7.690646428306815,
-            "y": 3.624138174667908,
-            "heading": 3.0114310872436763,
-            "angularVelocity": -0.8952584789497043,
-            "velocityX": -0.229356154113369,
-            "velocityY": -3.4793722827239795,
-            "timestamp": 1.4174876057593173
-        },
-        {
-            "x": 7.629817161501398,
-            "y": 3.371341042658395,
-            "heading": 2.9608367604127053,
-            "angularVelocity": -0.6940593512835794,
-            "velocityX": -0.8344635476447348,
-            "velocityY": -3.4679029139977815,
-            "timestamp": 1.4903838598583257
-        },
-        {
-            "x": 7.52574877175356,
-            "y": 3.1246562301261127,
-            "heading": 2.9373487413645845,
-            "angularVelocity": -0.32221160522605474,
-            "velocityX": -1.4276232850943003,
-            "velocityY": -3.3840533451439727,
-            "timestamp": 1.563280113957334
-        },
-        {
-            "x": 7.379748340613123,
-            "y": 2.890715038062695,
-            "heading": 2.9373478462882416,
-            "angularVelocity": -0.00001227877368205191,
-            "velocityX": -2.002852313126858,
-            "velocityY": -3.2092347536203247,
-            "timestamp": 1.6361763680563424
-        },
-        {
-            "x": 7.198165609339985,
-            "y": 2.6831759130730863,
-            "heading": 2.937347790914402,
-            "angularVelocity": -7.596252972489614e-7,
-            "velocityX": -2.4909747903727215,
-            "velocityY": -2.847047870356197,
-            "timestamp": 1.7090726221553507
-        },
-        {
-            "x": 6.985688529633763,
-            "y": 2.5073968727951343,
-            "heading": 2.9373477481889543,
-            "angularVelocity": -5.861130796388339e-7,
-            "velocityX": -2.91478735543415,
-            "velocityY": -2.4113590259261937,
-            "timestamp": 1.781968876254359
-        },
-        {
-            "x": 6.7478025524071645,
-            "y": 2.367915534392956,
-            "heading": 2.937347710107927,
-            "angularVelocity": -5.224003305629458e-7,
-            "velocityX": -3.263349813607291,
-            "velocityY": -1.9134225774154996,
-            "timestamp": 1.8548651303533674
-        },
-        {
-            "x": 6.4987639591558635,
-            "y": 2.249490193910499,
-            "heading": 2.937347672689208,
-            "angularVelocity": -5.133147047667194e-7,
-            "velocityX": -3.4163428056670964,
-            "velocityY": -1.624573744513203,
-            "timestamp": 1.9277613844523758
-        },
-        {
-            "x": 6.249725239429151,
-            "y": 2.1310651193958834,
-            "heading": 2.937347635270487,
-            "angularVelocity": -5.133147287283584e-7,
-            "velocityX": -3.4163445406727813,
-            "velocityY": -1.6245700959327491,
-            "timestamp": 2.000657638551384
-        },
-        {
-            "x": 6.0006865195043355,
-            "y": 2.012640045297908,
-            "heading": 2.937347597851683,
-            "angularVelocity": -5.133158656564982e-7,
-            "velocityX": -3.4163445433903696,
-            "velocityY": -1.6245700902172364,
-            "timestamp": 2.0735538926503922
-        },
-        {
-            "x": 5.766671175118509,
-            "y": 1.901395758932124,
-            "heading": 2.8964169231036223,
-            "angularVelocity": -0.5614921542139502,
-            "velocityX": -3.2102519845241893,
-            "velocityY": -1.5260631391951232,
-            "timestamp": 2.1464501467494004
-        },
-        {
-            "x": 5.568987668741884,
-            "y": 1.8073555304166649,
-            "heading": 2.790644028809865,
-            "angularVelocity": -1.4510058932587933,
-            "velocityX": -2.7118472522341635,
-            "velocityY": -1.2900557055764181,
-            "timestamp": 2.2193464008484085
-        },
-        {
-            "x": 5.41134573614175,
-            "y": 1.7322845721056195,
-            "heading": 2.684499308002556,
-            "angularVelocity": -1.456106656228851,
-            "velocityX": -2.162551897193064,
-            "velocityY": -1.029832866434731,
-            "timestamp": 2.2922426549474166
-        },
-        {
-            "x": 5.293483125084031,
-            "y": 1.6760930732996284,
-            "heading": 2.5959515176489267,
-            "angularVelocity": -1.2147097467272172,
-            "velocityX": -1.6168541513476236,
-            "velocityY": -0.7708420617846653,
-            "timestamp": 2.3651389090464248
-        },
-        {
-            "x": 5.2151080549458655,
-            "y": 1.6386848703250345,
-            "heading": 2.5329322459715486,
-            "angularVelocity": -0.8645063104618199,
-            "velocityX": -1.075159088856404,
-            "velocityY": -0.5131704425282329,
-            "timestamp": 2.438035163145433
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.45176870030861394,
-            "velocityX": -0.536489226081922,
-            "velocityY": -0.25632140575671664,
-            "timestamp": 2.510931417244441
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": 2.4074157612584747e-31,
-            "velocityY": 0,
-            "timestamp": 2.583827671343449
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj
deleted file mode 100644
index f00ff892..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": -6.652842580111428e-35,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.215224473336601,
-            "y": 5.589131466417677,
-            "heading": 3.1531161748208096,
-            "angularVelocity": 0.15993673271356004,
-            "velocityX": -0.5825678402891398,
-            "velocityY": 0.3552248747558748,
-            "timestamp": 0.08200852048353235
-        },
-        {
-            "x": 4.119673422721879,
-            "y": 5.647394502413024,
-            "heading": 3.179344235647322,
-            "angularVelocity": 0.3198211682379922,
-            "velocityX": -1.1651356475076162,
-            "velocityY": 0.7104510074297316,
-            "timestamp": 0.1640170409670647
-        },
-        {
-            "x": 3.9763468590229047,
-            "y": 5.73478928117174,
-            "heading": 3.2186777846940484,
-            "angularVelocity": 0.47962759009443284,
-            "velocityX": -1.7477033222146137,
-            "velocityY": 1.065679251904866,
-            "timestamp": 0.24602556145059706
-        },
-        {
-            "x": 3.785244828834372,
-            "y": 5.85131604858519,
-            "heading": 3.2711097941834586,
-            "angularVelocity": 0.6393483162513433,
-            "velocityX": -2.3302704287526703,
-            "velocityY": 1.420910494743635,
-            "timestamp": 0.3280340819341294
-        },
-        {
-            "x": 3.546367452965707,
-            "y": 5.996975111850807,
-            "heading": 3.3366347863626045,
-            "angularVelocity": 0.7990022474835781,
-            "velocityX": -2.9128360621581053,
-            "velocityY": 1.7761454835033377,
-            "timestamp": 0.4100426024176618
-        },
-        {
-            "x": 3.281494477712196,
-            "y": 6.158489941563481,
-            "heading": 3.336634794646938,
-            "angularVelocity": 1.0101796436143703e-7,
-            "velocityX": -3.2298226293047043,
-            "velocityY": 1.9694883990146768,
-            "timestamp": 0.4920511229011941
-        },
-        {
-            "x": 3.016621502775785,
-            "y": 6.320004771796177,
-            "heading": 3.336634802931272,
-            "angularVelocity": 1.0101796370003125e-7,
-            "velocityX": -3.2298226254380333,
-            "velocityY": 1.969488405355746,
-            "timestamp": 0.5740596433847265
-        },
-        {
-            "x": 2.7777474341519097,
-            "y": 6.465669636134063,
-            "heading": 3.402143395936758,
-            "angularVelocity": 0.7988022783393678,
-            "velocityX": -2.912795734094993,
-            "velocityY": 1.7762162209368904,
-            "timestamp": 0.6560681638682588
-        },
-        {
-            "x": 2.586648366744574,
-            "y": 6.582201701925324,
-            "heading": 3.4545567132875594,
-            "angularVelocity": 0.6391203870252214,
-            "velocityX": -2.3302343010286215,
-            "velocityY": 1.4209751023939097,
-            "timestamp": 0.7380766843517912
-        },
-        {
-            "x": 2.4433241632679117,
-            "y": 6.669600830029101,
-            "heading": 3.4938702766455694,
-            "angularVelocity": 0.4793838875059847,
-            "velocityX": -1.7476745420061834,
-            "velocityY": 1.0657322871875106,
-            "timestamp": 0.8200852048353235
-        },
-        {
-            "x": 2.3477747237681617,
-            "y": 6.727866941749082,
-            "heading": 3.5200804322455714,
-            "angularVelocity": 0.3196028345038231,
-            "velocityX": -1.1651160018054092,
-            "velocityY": 0.7104885123696424,
-            "timestamp": 0.9020937253188559
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 0.15979894353351334,
-            "velocityX": -0.5825580499011068,
-            "velocityY": 0.35524428533945085,
-            "timestamp": 0.9841022458023883
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 2.6358633635199364e-33,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 1.0661107662859206
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.traj
deleted file mode 100644
index f00ff892..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": -6.652842580111428e-35,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.215224473336601,
-            "y": 5.589131466417677,
-            "heading": 3.1531161748208096,
-            "angularVelocity": 0.15993673271356004,
-            "velocityX": -0.5825678402891398,
-            "velocityY": 0.3552248747558748,
-            "timestamp": 0.08200852048353235
-        },
-        {
-            "x": 4.119673422721879,
-            "y": 5.647394502413024,
-            "heading": 3.179344235647322,
-            "angularVelocity": 0.3198211682379922,
-            "velocityX": -1.1651356475076162,
-            "velocityY": 0.7104510074297316,
-            "timestamp": 0.1640170409670647
-        },
-        {
-            "x": 3.9763468590229047,
-            "y": 5.73478928117174,
-            "heading": 3.2186777846940484,
-            "angularVelocity": 0.47962759009443284,
-            "velocityX": -1.7477033222146137,
-            "velocityY": 1.065679251904866,
-            "timestamp": 0.24602556145059706
-        },
-        {
-            "x": 3.785244828834372,
-            "y": 5.85131604858519,
-            "heading": 3.2711097941834586,
-            "angularVelocity": 0.6393483162513433,
-            "velocityX": -2.3302704287526703,
-            "velocityY": 1.420910494743635,
-            "timestamp": 0.3280340819341294
-        },
-        {
-            "x": 3.546367452965707,
-            "y": 5.996975111850807,
-            "heading": 3.3366347863626045,
-            "angularVelocity": 0.7990022474835781,
-            "velocityX": -2.9128360621581053,
-            "velocityY": 1.7761454835033377,
-            "timestamp": 0.4100426024176618
-        },
-        {
-            "x": 3.281494477712196,
-            "y": 6.158489941563481,
-            "heading": 3.336634794646938,
-            "angularVelocity": 1.0101796436143703e-7,
-            "velocityX": -3.2298226293047043,
-            "velocityY": 1.9694883990146768,
-            "timestamp": 0.4920511229011941
-        },
-        {
-            "x": 3.016621502775785,
-            "y": 6.320004771796177,
-            "heading": 3.336634802931272,
-            "angularVelocity": 1.0101796370003125e-7,
-            "velocityX": -3.2298226254380333,
-            "velocityY": 1.969488405355746,
-            "timestamp": 0.5740596433847265
-        },
-        {
-            "x": 2.7777474341519097,
-            "y": 6.465669636134063,
-            "heading": 3.402143395936758,
-            "angularVelocity": 0.7988022783393678,
-            "velocityX": -2.912795734094993,
-            "velocityY": 1.7762162209368904,
-            "timestamp": 0.6560681638682588
-        },
-        {
-            "x": 2.586648366744574,
-            "y": 6.582201701925324,
-            "heading": 3.4545567132875594,
-            "angularVelocity": 0.6391203870252214,
-            "velocityX": -2.3302343010286215,
-            "velocityY": 1.4209751023939097,
-            "timestamp": 0.7380766843517912
-        },
-        {
-            "x": 2.4433241632679117,
-            "y": 6.669600830029101,
-            "heading": 3.4938702766455694,
-            "angularVelocity": 0.4793838875059847,
-            "velocityX": -1.7476745420061834,
-            "velocityY": 1.0657322871875106,
-            "timestamp": 0.8200852048353235
-        },
-        {
-            "x": 2.3477747237681617,
-            "y": 6.727866941749082,
-            "heading": 3.5200804322455714,
-            "angularVelocity": 0.3196028345038231,
-            "velocityX": -1.1651160018054092,
-            "velocityY": 0.7104885123696424,
-            "timestamp": 0.9020937253188559
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 0.15979894353351334,
-            "velocityX": -0.5825580499011068,
-            "velocityY": 0.35524428533945085,
-            "timestamp": 0.9841022458023883
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 2.6358633635199364e-33,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 1.0661107662859206
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj
deleted file mode 100644
index d7ef3c12..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -4.0059408516052846e-40,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.181096344253034,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 1.5012541318382292e-16,
-            "velocityX": -0.8263919813283321,
-            "velocityY": -1.2317462001949155e-21,
-            "timestamp": 0.09910993523353699
-        },
-        {
-            "x": 4.017289035836238,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 3.002363652724146e-16,
-            "velocityX": -1.6527839316089836,
-            "velocityY": -2.463492548222639e-21,
-            "timestamp": 0.19821987046707398
-        },
-        {
-            "x": 3.771578080903875,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 4.503226484774511e-16,
-            "velocityX": -2.4791758197942855,
-            "velocityY": -3.695244043691227e-21,
-            "timestamp": 0.29732980570061096
-        },
-        {
-            "x": 3.4439634979187304,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 6.003547774343467e-16,
-            "velocityX": -3.305567521693684,
-            "velocityY": -4.926945384758097e-21,
-            "timestamp": 0.39643974093414797
-        },
-        {
-            "x": 3.0690365020812695,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 4.535737431947664e-16,
-            "velocityX": -3.7829405796099507,
-            "velocityY": 2.4634613990171587e-20,
-            "timestamp": 0.49554967616768497
-        },
-        {
-            "x": 2.741421919096125,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 5.990674751034296e-16,
-            "velocityX": -3.305567521693684,
-            "velocityY": -4.926861067570722e-21,
-            "timestamp": 0.5946596114012219
-        },
-        {
-            "x": 2.4957109641637625,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 4.492129826743599e-16,
-            "velocityX": -2.479175819794286,
-            "velocityY": -3.695159726389389e-21,
-            "timestamp": 0.6937695466347589
-        },
-        {
-            "x": 2.3319036557469652,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 2.9945516157260173e-16,
-            "velocityX": -1.6527839316089836,
-            "velocityY": -2.4634409378754245e-21,
-            "timestamp": 0.7928794818682958
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 1.4972226131077696e-16,
-            "velocityX": -0.8263919813283321,
-            "velocityY": -1.2317240814696432e-21,
-            "timestamp": 0.8919894171018328
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.546248641381176e-40,
-            "timestamp": 0.9910993523353697
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.traj
deleted file mode 100644
index d7ef3c12..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -4.0059408516052846e-40,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.181096344253034,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 1.5012541318382292e-16,
-            "velocityX": -0.8263919813283321,
-            "velocityY": -1.2317462001949155e-21,
-            "timestamp": 0.09910993523353699
-        },
-        {
-            "x": 4.017289035836238,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 3.002363652724146e-16,
-            "velocityX": -1.6527839316089836,
-            "velocityY": -2.463492548222639e-21,
-            "timestamp": 0.19821987046707398
-        },
-        {
-            "x": 3.771578080903875,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 4.503226484774511e-16,
-            "velocityX": -2.4791758197942855,
-            "velocityY": -3.695244043691227e-21,
-            "timestamp": 0.29732980570061096
-        },
-        {
-            "x": 3.4439634979187304,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 6.003547774343467e-16,
-            "velocityX": -3.305567521693684,
-            "velocityY": -4.926945384758097e-21,
-            "timestamp": 0.39643974093414797
-        },
-        {
-            "x": 3.0690365020812695,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 4.535737431947664e-16,
-            "velocityX": -3.7829405796099507,
-            "velocityY": 2.4634613990171587e-20,
-            "timestamp": 0.49554967616768497
-        },
-        {
-            "x": 2.741421919096125,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 5.990674751034296e-16,
-            "velocityX": -3.305567521693684,
-            "velocityY": -4.926861067570722e-21,
-            "timestamp": 0.5946596114012219
-        },
-        {
-            "x": 2.4957109641637625,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 4.492129826743599e-16,
-            "velocityX": -2.479175819794286,
-            "velocityY": -3.695159726389389e-21,
-            "timestamp": 0.6937695466347589
-        },
-        {
-            "x": 2.3319036557469652,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 2.9945516157260173e-16,
-            "velocityX": -1.6527839316089836,
-            "velocityY": -2.4634409378754245e-21,
-            "timestamp": 0.7928794818682958
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 1.4972226131077696e-16,
-            "velocityX": -0.8263919813283321,
-            "velocityY": -1.2317240814696432e-21,
-            "timestamp": 0.8919894171018328
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.546248641381176e-40,
-            "timestamp": 0.9910993523353697
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj
deleted file mode 100644
index e124574f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -1.339537382865945e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.215219135134553,
-            "y": 5.5307922107130985,
-            "heading": 3.1269908374623143,
-            "angularVelocity": -0.1585720754034668,
-            "velocityX": -0.582413424718004,
-            "velocityY": -0.3560213620019152,
-            "timestamp": 0.08203942910241563
-        },
-        {
-            "x": 4.119657409619225,
-            "y": 5.472376532354226,
-            "heading": 3.100976599590504,
-            "angularVelocity": -0.3170943308166476,
-            "velocityX": -1.1648267980513523,
-            "velocityY": -0.7120439403100625,
-            "timestamp": 0.16407885820483126
-        },
-        {
-            "x": 3.976314836365869,
-            "y": 5.384752798145142,
-            "heading": 3.0619633632091103,
-            "angularVelocity": -0.4755425142304533,
-            "velocityX": -1.7472400139987678,
-            "velocityY": -1.0680685515217063,
-            "timestamp": 0.24611828730724689
-        },
-        {
-            "x": 3.785191463205027,
-            "y": 5.267920772243767,
-            "heading": 3.009957765442303,
-            "angularVelocity": -0.6339097960065634,
-            "velocityX": -2.3296526469272343,
-            "velocityY": -1.4240960374739504,
-            "timestamp": 0.3281577164096625
-        },
-        {
-            "x": 3.546287409634579,
-            "y": 5.121880161282017,
-            "heading": 2.944964910348519,
-            "angularVelocity": -0.7922148630830776,
-            "velocityX": -2.9120638232648712,
-            "velocityY": -1.780127099366288,
-            "timestamp": 0.41019714551207814
-        },
-        {
-            "x": 3.281494622858476,
-            "y": 4.960009610093287,
-            "heading": 2.9449649021355118,
-            "angularVelocity": -1.0011048865301711e-7,
-            "velocityX": -3.227628344969881,
-            "velocityY": -1.9730823697792366,
-            "timestamp": 0.49223657461449377
-        },
-        {
-            "x": 3.0167018363816,
-            "y": 4.798139058415072,
-            "heading": 2.9449648939225046,
-            "angularVelocity": -1.0011048873886247e-7,
-            "velocityX": -3.2276283413225157,
-            "velocityY": -1.9730823757457077,
-            "timestamp": 0.5742760037169095
-        },
-        {
-            "x": 2.777800999846863,
-            "y": 4.652092907401912,
-            "heading": 2.879984644909019,
-            "angularVelocity": -0.7920612042822246,
-            "velocityX": -2.912024609977479,
-            "velocityY": -1.7801946284979562,
-            "timestamp": 0.656315432819325
-        },
-        {
-            "x": 2.58668051019359,
-            "y": 4.5352558204412405,
-            "heading": 2.827994144000888,
-            "angularVelocity": -0.6337257764583869,
-            "velocityX": -2.329617499101367,
-            "velocityY": -1.424157728045816,
-            "timestamp": 0.7383548619217406
-        },
-        {
-            "x": 2.443340235924958,
-            "y": 4.447627930217013,
-            "heading": 2.788997784467408,
-            "angularVelocity": -0.4753367979291705,
-            "velocityX": -1.7472119910743198,
-            "velocityY": -1.0681192102743058,
-            "timestamp": 0.8203942910241562
-        },
-        {
-            "x": 2.3477800812496348,
-            "y": 4.389209311628481,
-            "heading": 2.7629991294364147,
-            "angularVelocity": -0.31690438750539845,
-            "velocityX": -1.1648076506728071,
-            "velocityY": -0.7120797795363016,
-            "timestamp": 0.9024337201265717
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": -0.1584497793150997,
-            "velocityX": -0.5824038730204719,
-            "velocityY": -0.35603991822050945,
-            "timestamp": 0.9844731492289873
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": -7.396671024736362e-34,
-            "velocityX": -1.1837806823878881e-33,
-            "velocityY": 0,
-            "timestamp": 1.066512578331403
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.traj
deleted file mode 100644
index e124574f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -1.339537382865945e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.215219135134553,
-            "y": 5.5307922107130985,
-            "heading": 3.1269908374623143,
-            "angularVelocity": -0.1585720754034668,
-            "velocityX": -0.582413424718004,
-            "velocityY": -0.3560213620019152,
-            "timestamp": 0.08203942910241563
-        },
-        {
-            "x": 4.119657409619225,
-            "y": 5.472376532354226,
-            "heading": 3.100976599590504,
-            "angularVelocity": -0.3170943308166476,
-            "velocityX": -1.1648267980513523,
-            "velocityY": -0.7120439403100625,
-            "timestamp": 0.16407885820483126
-        },
-        {
-            "x": 3.976314836365869,
-            "y": 5.384752798145142,
-            "heading": 3.0619633632091103,
-            "angularVelocity": -0.4755425142304533,
-            "velocityX": -1.7472400139987678,
-            "velocityY": -1.0680685515217063,
-            "timestamp": 0.24611828730724689
-        },
-        {
-            "x": 3.785191463205027,
-            "y": 5.267920772243767,
-            "heading": 3.009957765442303,
-            "angularVelocity": -0.6339097960065634,
-            "velocityX": -2.3296526469272343,
-            "velocityY": -1.4240960374739504,
-            "timestamp": 0.3281577164096625
-        },
-        {
-            "x": 3.546287409634579,
-            "y": 5.121880161282017,
-            "heading": 2.944964910348519,
-            "angularVelocity": -0.7922148630830776,
-            "velocityX": -2.9120638232648712,
-            "velocityY": -1.780127099366288,
-            "timestamp": 0.41019714551207814
-        },
-        {
-            "x": 3.281494622858476,
-            "y": 4.960009610093287,
-            "heading": 2.9449649021355118,
-            "angularVelocity": -1.0011048865301711e-7,
-            "velocityX": -3.227628344969881,
-            "velocityY": -1.9730823697792366,
-            "timestamp": 0.49223657461449377
-        },
-        {
-            "x": 3.0167018363816,
-            "y": 4.798139058415072,
-            "heading": 2.9449648939225046,
-            "angularVelocity": -1.0011048873886247e-7,
-            "velocityX": -3.2276283413225157,
-            "velocityY": -1.9730823757457077,
-            "timestamp": 0.5742760037169095
-        },
-        {
-            "x": 2.777800999846863,
-            "y": 4.652092907401912,
-            "heading": 2.879984644909019,
-            "angularVelocity": -0.7920612042822246,
-            "velocityX": -2.912024609977479,
-            "velocityY": -1.7801946284979562,
-            "timestamp": 0.656315432819325
-        },
-        {
-            "x": 2.58668051019359,
-            "y": 4.5352558204412405,
-            "heading": 2.827994144000888,
-            "angularVelocity": -0.6337257764583869,
-            "velocityX": -2.329617499101367,
-            "velocityY": -1.424157728045816,
-            "timestamp": 0.7383548619217406
-        },
-        {
-            "x": 2.443340235924958,
-            "y": 4.447627930217013,
-            "heading": 2.788997784467408,
-            "angularVelocity": -0.4753367979291705,
-            "velocityX": -1.7472119910743198,
-            "velocityY": -1.0681192102743058,
-            "timestamp": 0.8203942910241562
-        },
-        {
-            "x": 2.3477800812496348,
-            "y": 4.389209311628481,
-            "heading": 2.7629991294364147,
-            "angularVelocity": -0.31690438750539845,
-            "velocityX": -1.1648076506728071,
-            "velocityY": -0.7120797795363016,
-            "timestamp": 0.9024337201265717
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": -0.1584497793150997,
-            "velocityX": -0.5824038730204719,
-            "velocityY": -0.35603991822050945,
-            "timestamp": 0.9844731492289873
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": -7.396671024736362e-34,
-            "velocityX": -1.1837806823878881e-33,
-            "velocityY": 0,
-            "timestamp": 1.066512578331403
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S1.1.traj
deleted file mode 100644
index c69cb9ab..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S1.1.traj	
+++ /dev/null
@@ -1,310 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.308593242288023,
-            "y": 2.998690970742917,
-            "heading": 2.600069907206834,
-            "angularVelocity": 0.35025874949099595,
-            "velocityX": 0.6125619052781636,
-            "velocityY": -0.017587287403649503,
-            "timestamp": 0.07443042392151207
-        },
-        {
-            "x": 4.399870755267574,
-            "y": 2.9960680712021444,
-            "heading": 2.65011196683443,
-            "angularVelocity": 0.6723333952842253,
-            "velocityX": 1.2263468104898216,
-            "velocityY": -0.03523961577241709,
-            "timestamp": 0.14886084784302414
-        },
-        {
-            "x": 4.536960438371983,
-            "y": 2.992122797800661,
-            "heading": 2.720779852914046,
-            "angularVelocity": 0.9494489263440683,
-            "velocityX": 1.8418500914219216,
-            "velocityY": -0.05300619281227453,
-            "timestamp": 0.22329127176453623
-        },
-        {
-            "x": 4.72004322969291,
-            "y": 2.9868327872754663,
-            "heading": 2.805997286445448,
-            "angularVelocity": 1.1449274240499019,
-            "velocityX": 2.4597843418706042,
-            "velocityY": -0.07107322847942685,
-            "timestamp": 0.2977216956860483
-        },
-        {
-            "x": 4.949295920013874,
-            "y": 2.980134342239005,
-            "heading": 2.892264553695649,
-            "angularVelocity": 1.1590323244855822,
-            "velocityX": 3.0800938412323915,
-            "velocityY": -0.08999606187282569,
-            "timestamp": 0.37215211960756034
-        },
-        {
-            "x": 5.2218046444028055,
-            "y": 2.971482120946463,
-            "heading": 2.9184186664707665,
-            "angularVelocity": 0.35139008213490475,
-            "velocityX": 3.661254498245178,
-            "velocityY": -0.11624576129869893,
-            "timestamp": 0.4465825435290724
-        },
-        {
-            "x": 5.503268186944086,
-            "y": 2.9638921982075033,
-            "heading": 2.918418695115671,
-            "angularVelocity": 3.8485478250287795e-7,
-            "velocityX": 3.7815657591590193,
-            "velocityY": -0.10197339124338625,
-            "timestamp": 0.5210129674505845
-        },
-        {
-            "x": 5.784731729781038,
-            "y": 2.9563022864327206,
-            "heading": 2.9184187237605435,
-            "angularVelocity": 3.8485435121714565e-7,
-            "velocityX": 3.781565763131487,
-            "velocityY": -0.10197324393565109,
-            "timestamp": 0.5954433913720966
-        },
-        {
-            "x": 6.066195282639446,
-            "y": 2.9487127463017835,
-            "heading": 2.9184187524054175,
-            "angularVelocity": 3.848543706133971e-7,
-            "velocityX": 3.7815658977733997,
-            "velocityY": -0.10196825076449405,
-            "timestamp": 0.6698738152936087
-        },
-        {
-            "x": 6.347042051479281,
-            "y": 2.968823062647617,
-            "heading": 2.918418781616621,
-            "angularVelocity": 3.924632205757564e-7,
-            "velocityX": 3.773279178632548,
-            "velocityY": 0.2701894640159414,
-            "timestamp": 0.7443042392151208
-        },
-        {
-            "x": 6.6208214562671275,
-            "y": 3.0345814714160126,
-            "heading": 2.918418814269471,
-            "angularVelocity": 4.3870299176986663e-7,
-            "velocityX": 3.678326554696914,
-            "velocityY": 0.8834883009364279,
-            "timestamp": 0.8187346631366329
-        },
-        {
-            "x": 6.88016498053821,
-            "y": 3.1442184338201953,
-            "heading": 2.9184188559848527,
-            "angularVelocity": 5.604614269784904e-7,
-            "velocityX": 3.4843752138851785,
-            "velocityY": 1.4730127362944399,
-            "timestamp": 0.893165087058145
-        },
-        {
-            "x": 7.118091210487748,
-            "y": 3.294782489019261,
-            "heading": 2.918422820305874,
-            "angularVelocity": 0.00005326210462723065,
-            "velocityX": 3.196626022181901,
-            "velocityY": 2.022883214501594,
-            "timestamp": 0.9675955109796571
-        },
-        {
-            "x": 7.320693053454344,
-            "y": 3.474127369690306,
-            "heading": 2.9455538073395324,
-            "angularVelocity": 0.36451474550610485,
-            "velocityX": 2.722029948133077,
-            "velocityY": 2.4095641435572857,
-            "timestamp": 1.0420259349011691
-        },
-        {
-            "x": 7.483696807015107,
-            "y": 3.6758684658318805,
-            "heading": 3.001437207311255,
-            "angularVelocity": 0.7508139417646484,
-            "velocityX": 2.1900151171065767,
-            "velocityY": 2.710465499354305,
-            "timestamp": 1.1164563588226812
-        },
-        {
-            "x": 7.603947516595235,
-            "y": 3.8947228146234494,
-            "heading": 3.0702288107606313,
-            "angularVelocity": 0.9242403821576988,
-            "velocityX": 1.6156123160998233,
-            "velocityY": 2.940388315164694,
-            "timestamp": 1.1908867827441934
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.9374014759467769,
-            "velocityX": 1.0217929631162956,
-            "velocityY": 3.12072903980085,
-            "timestamp": 1.2653172066657055
-        },
-        {
-            "x": 7.709300887372812,
-            "y": 4.397378653394831,
-            "heading": 3.2093959242930556,
-            "angularVelocity": 0.8431459366282483,
-            "velocityX": 0.355999640896236,
-            "velocityY": 3.2850439746024853,
-            "timestamp": 1.3476231530960114
-        },
-        {
-            "x": 7.683172834427827,
-            "y": 4.6781135002756455,
-            "heading": 3.266996168893978,
-            "angularVelocity": 0.6998309004282659,
-            "velocityX": -0.31745036753947053,
-            "velocityY": 3.4108695550756014,
-            "timestamp": 1.4299290995263174
-        },
-        {
-            "x": 7.601002938928111,
-            "y": 4.963519420188029,
-            "heading": 3.305651937027416,
-            "angularVelocity": 0.4696594816046569,
-            "velocityX": -0.9983470097059609,
-            "velocityY": 3.467622113477001,
-            "timestamp": 1.5122350459566234
-        },
-        {
-            "x": 7.463684437134381,
-            "y": 5.241326726377331,
-            "heading": 3.3097255928301426,
-            "angularVelocity": 0.049494064273674344,
-            "velocityX": -1.6683910185032116,
-            "velocityY": 3.3753005492080685,
-            "timestamp": 1.5945409923869294
-        },
-        {
-            "x": 7.277476070304,
-            "y": 5.490867404030893,
-            "heading": 3.3097256042381358,
-            "angularVelocity": 1.3860472723422523e-7,
-            "velocityX": -2.2623926326885195,
-            "velocityY": 3.031866936429266,
-            "timestamp": 1.6768469388172353
-        },
-        {
-            "x": 7.049248371933673,
-            "y": 5.702660296381284,
-            "heading": 3.3097256090357283,
-            "angularVelocity": 5.8289739150802425e-8,
-            "velocityX": -2.7729187047691926,
-            "velocityY": 2.573239255923398,
-            "timestamp": 1.7591528852475413
-        },
-        {
-            "x": 6.786512642610724,
-            "y": 5.869735280017231,
-            "heading": 3.309725613101849,
-            "angularVelocity": 4.940251326866522e-8,
-            "velocityX": -3.192184048881866,
-            "velocityY": 2.029926036722274,
-            "timestamp": 1.8414588316778473
-        },
-        {
-            "x": 6.5049501154842,
-            "y": 6.00265125873641,
-            "heading": 3.309725617013439,
-            "angularVelocity": 4.7524998408670815e-8,
-            "velocityX": -3.4209256966013024,
-            "velocityY": 1.6149012857986877,
-            "timestamp": 1.9237647781081533
-        },
-        {
-            "x": 6.223387384136782,
-            "y": 6.135566804843462,
-            "heading": 3.309725620925028,
-            "angularVelocity": 4.75249886637903e-8,
-            "velocityX": -3.4209281778423657,
-            "velocityY": 1.6148960296520045,
-            "timestamp": 2.0060707245384592
-        },
-        {
-            "x": 5.941824652769856,
-            "y": 6.268482350909119,
-            "heading": 3.3097256248366427,
-            "angularVelocity": 4.752530281025494e-8,
-            "velocityX": -3.42092817807938,
-            "velocityY": 1.6148960291490546,
-            "timestamp": 2.0883766709687652
-        },
-        {
-            "x": 5.686549787544516,
-            "y": 6.388988216738383,
-            "heading": 3.340879096998464,
-            "angularVelocity": 0.3785081578303866,
-            "velocityX": -3.1015361136937893,
-            "velocityY": 1.464121015014444,
-            "timestamp": 2.170682617399071
-        },
-        {
-            "x": 5.482329878299888,
-            "y": 6.485392923986267,
-            "heading": 3.365801660864199,
-            "angularVelocity": 0.30280392786490506,
-            "velocityX": -2.4812290982833782,
-            "velocityY": 1.1712969892098404,
-            "timestamp": 2.252988563829377
-        },
-        {
-            "x": 5.32916494108436,
-            "y": 6.557696460239907,
-            "heading": 3.3844934905089157,
-            "angularVelocity": 0.22710181287501843,
-            "velocityX": -1.8609218878884095,
-            "velocityY": 0.8784728125915443,
-            "timestamp": 2.335294510259683
-        },
-        {
-            "x": 5.227054980864034,
-            "y": 6.6058988198426265,
-            "heading": 3.396954693375837,
-            "angularVelocity": 0.1514010032977761,
-            "velocityX": -1.2406146171563652,
-            "velocityY": 0.5856485672458114,
-            "timestamp": 2.417600456689989
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.07570065194530232,
-            "velocityX": -0.6203073177375487,
-            "velocityY": 0.29282428794839654,
-            "timestamp": 2.499906403120295
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0,
-            "velocityX": 1.5419247738054587e-33,
-            "velocityY": 0,
-            "timestamp": 2.582212349550601
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S1.traj
deleted file mode 100644
index c69cb9ab..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S1.traj	
+++ /dev/null
@@ -1,310 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.308593242288023,
-            "y": 2.998690970742917,
-            "heading": 2.600069907206834,
-            "angularVelocity": 0.35025874949099595,
-            "velocityX": 0.6125619052781636,
-            "velocityY": -0.017587287403649503,
-            "timestamp": 0.07443042392151207
-        },
-        {
-            "x": 4.399870755267574,
-            "y": 2.9960680712021444,
-            "heading": 2.65011196683443,
-            "angularVelocity": 0.6723333952842253,
-            "velocityX": 1.2263468104898216,
-            "velocityY": -0.03523961577241709,
-            "timestamp": 0.14886084784302414
-        },
-        {
-            "x": 4.536960438371983,
-            "y": 2.992122797800661,
-            "heading": 2.720779852914046,
-            "angularVelocity": 0.9494489263440683,
-            "velocityX": 1.8418500914219216,
-            "velocityY": -0.05300619281227453,
-            "timestamp": 0.22329127176453623
-        },
-        {
-            "x": 4.72004322969291,
-            "y": 2.9868327872754663,
-            "heading": 2.805997286445448,
-            "angularVelocity": 1.1449274240499019,
-            "velocityX": 2.4597843418706042,
-            "velocityY": -0.07107322847942685,
-            "timestamp": 0.2977216956860483
-        },
-        {
-            "x": 4.949295920013874,
-            "y": 2.980134342239005,
-            "heading": 2.892264553695649,
-            "angularVelocity": 1.1590323244855822,
-            "velocityX": 3.0800938412323915,
-            "velocityY": -0.08999606187282569,
-            "timestamp": 0.37215211960756034
-        },
-        {
-            "x": 5.2218046444028055,
-            "y": 2.971482120946463,
-            "heading": 2.9184186664707665,
-            "angularVelocity": 0.35139008213490475,
-            "velocityX": 3.661254498245178,
-            "velocityY": -0.11624576129869893,
-            "timestamp": 0.4465825435290724
-        },
-        {
-            "x": 5.503268186944086,
-            "y": 2.9638921982075033,
-            "heading": 2.918418695115671,
-            "angularVelocity": 3.8485478250287795e-7,
-            "velocityX": 3.7815657591590193,
-            "velocityY": -0.10197339124338625,
-            "timestamp": 0.5210129674505845
-        },
-        {
-            "x": 5.784731729781038,
-            "y": 2.9563022864327206,
-            "heading": 2.9184187237605435,
-            "angularVelocity": 3.8485435121714565e-7,
-            "velocityX": 3.781565763131487,
-            "velocityY": -0.10197324393565109,
-            "timestamp": 0.5954433913720966
-        },
-        {
-            "x": 6.066195282639446,
-            "y": 2.9487127463017835,
-            "heading": 2.9184187524054175,
-            "angularVelocity": 3.848543706133971e-7,
-            "velocityX": 3.7815658977733997,
-            "velocityY": -0.10196825076449405,
-            "timestamp": 0.6698738152936087
-        },
-        {
-            "x": 6.347042051479281,
-            "y": 2.968823062647617,
-            "heading": 2.918418781616621,
-            "angularVelocity": 3.924632205757564e-7,
-            "velocityX": 3.773279178632548,
-            "velocityY": 0.2701894640159414,
-            "timestamp": 0.7443042392151208
-        },
-        {
-            "x": 6.6208214562671275,
-            "y": 3.0345814714160126,
-            "heading": 2.918418814269471,
-            "angularVelocity": 4.3870299176986663e-7,
-            "velocityX": 3.678326554696914,
-            "velocityY": 0.8834883009364279,
-            "timestamp": 0.8187346631366329
-        },
-        {
-            "x": 6.88016498053821,
-            "y": 3.1442184338201953,
-            "heading": 2.9184188559848527,
-            "angularVelocity": 5.604614269784904e-7,
-            "velocityX": 3.4843752138851785,
-            "velocityY": 1.4730127362944399,
-            "timestamp": 0.893165087058145
-        },
-        {
-            "x": 7.118091210487748,
-            "y": 3.294782489019261,
-            "heading": 2.918422820305874,
-            "angularVelocity": 0.00005326210462723065,
-            "velocityX": 3.196626022181901,
-            "velocityY": 2.022883214501594,
-            "timestamp": 0.9675955109796571
-        },
-        {
-            "x": 7.320693053454344,
-            "y": 3.474127369690306,
-            "heading": 2.9455538073395324,
-            "angularVelocity": 0.36451474550610485,
-            "velocityX": 2.722029948133077,
-            "velocityY": 2.4095641435572857,
-            "timestamp": 1.0420259349011691
-        },
-        {
-            "x": 7.483696807015107,
-            "y": 3.6758684658318805,
-            "heading": 3.001437207311255,
-            "angularVelocity": 0.7508139417646484,
-            "velocityX": 2.1900151171065767,
-            "velocityY": 2.710465499354305,
-            "timestamp": 1.1164563588226812
-        },
-        {
-            "x": 7.603947516595235,
-            "y": 3.8947228146234494,
-            "heading": 3.0702288107606313,
-            "angularVelocity": 0.9242403821576988,
-            "velocityX": 1.6156123160998233,
-            "velocityY": 2.940388315164694,
-            "timestamp": 1.1908867827441934
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.9374014759467769,
-            "velocityX": 1.0217929631162956,
-            "velocityY": 3.12072903980085,
-            "timestamp": 1.2653172066657055
-        },
-        {
-            "x": 7.709300887372812,
-            "y": 4.397378653394831,
-            "heading": 3.2093959242930556,
-            "angularVelocity": 0.8431459366282483,
-            "velocityX": 0.355999640896236,
-            "velocityY": 3.2850439746024853,
-            "timestamp": 1.3476231530960114
-        },
-        {
-            "x": 7.683172834427827,
-            "y": 4.6781135002756455,
-            "heading": 3.266996168893978,
-            "angularVelocity": 0.6998309004282659,
-            "velocityX": -0.31745036753947053,
-            "velocityY": 3.4108695550756014,
-            "timestamp": 1.4299290995263174
-        },
-        {
-            "x": 7.601002938928111,
-            "y": 4.963519420188029,
-            "heading": 3.305651937027416,
-            "angularVelocity": 0.4696594816046569,
-            "velocityX": -0.9983470097059609,
-            "velocityY": 3.467622113477001,
-            "timestamp": 1.5122350459566234
-        },
-        {
-            "x": 7.463684437134381,
-            "y": 5.241326726377331,
-            "heading": 3.3097255928301426,
-            "angularVelocity": 0.049494064273674344,
-            "velocityX": -1.6683910185032116,
-            "velocityY": 3.3753005492080685,
-            "timestamp": 1.5945409923869294
-        },
-        {
-            "x": 7.277476070304,
-            "y": 5.490867404030893,
-            "heading": 3.3097256042381358,
-            "angularVelocity": 1.3860472723422523e-7,
-            "velocityX": -2.2623926326885195,
-            "velocityY": 3.031866936429266,
-            "timestamp": 1.6768469388172353
-        },
-        {
-            "x": 7.049248371933673,
-            "y": 5.702660296381284,
-            "heading": 3.3097256090357283,
-            "angularVelocity": 5.8289739150802425e-8,
-            "velocityX": -2.7729187047691926,
-            "velocityY": 2.573239255923398,
-            "timestamp": 1.7591528852475413
-        },
-        {
-            "x": 6.786512642610724,
-            "y": 5.869735280017231,
-            "heading": 3.309725613101849,
-            "angularVelocity": 4.940251326866522e-8,
-            "velocityX": -3.192184048881866,
-            "velocityY": 2.029926036722274,
-            "timestamp": 1.8414588316778473
-        },
-        {
-            "x": 6.5049501154842,
-            "y": 6.00265125873641,
-            "heading": 3.309725617013439,
-            "angularVelocity": 4.7524998408670815e-8,
-            "velocityX": -3.4209256966013024,
-            "velocityY": 1.6149012857986877,
-            "timestamp": 1.9237647781081533
-        },
-        {
-            "x": 6.223387384136782,
-            "y": 6.135566804843462,
-            "heading": 3.309725620925028,
-            "angularVelocity": 4.75249886637903e-8,
-            "velocityX": -3.4209281778423657,
-            "velocityY": 1.6148960296520045,
-            "timestamp": 2.0060707245384592
-        },
-        {
-            "x": 5.941824652769856,
-            "y": 6.268482350909119,
-            "heading": 3.3097256248366427,
-            "angularVelocity": 4.752530281025494e-8,
-            "velocityX": -3.42092817807938,
-            "velocityY": 1.6148960291490546,
-            "timestamp": 2.0883766709687652
-        },
-        {
-            "x": 5.686549787544516,
-            "y": 6.388988216738383,
-            "heading": 3.340879096998464,
-            "angularVelocity": 0.3785081578303866,
-            "velocityX": -3.1015361136937893,
-            "velocityY": 1.464121015014444,
-            "timestamp": 2.170682617399071
-        },
-        {
-            "x": 5.482329878299888,
-            "y": 6.485392923986267,
-            "heading": 3.365801660864199,
-            "angularVelocity": 0.30280392786490506,
-            "velocityX": -2.4812290982833782,
-            "velocityY": 1.1712969892098404,
-            "timestamp": 2.252988563829377
-        },
-        {
-            "x": 5.32916494108436,
-            "y": 6.557696460239907,
-            "heading": 3.3844934905089157,
-            "angularVelocity": 0.22710181287501843,
-            "velocityX": -1.8609218878884095,
-            "velocityY": 0.8784728125915443,
-            "timestamp": 2.335294510259683
-        },
-        {
-            "x": 5.227054980864034,
-            "y": 6.6058988198426265,
-            "heading": 3.396954693375837,
-            "angularVelocity": 0.1514010032977761,
-            "velocityX": -1.2406146171563652,
-            "velocityY": 0.5856485672458114,
-            "timestamp": 2.417600456689989
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.07570065194530232,
-            "velocityX": -0.6203073177375487,
-            "velocityY": 0.29282428794839654,
-            "timestamp": 2.499906403120295
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0,
-            "velocityX": 1.5419247738054587e-33,
-            "velocityY": 0,
-            "timestamp": 2.582212349550601
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S2.1.traj
deleted file mode 100644
index 9ab4bfc6..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S2.1.traj	
+++ /dev/null
@@ -1,301 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.312496748515021,
-            "y": 3.002119555669804,
-            "heading": 2.5925232531186833,
-            "angularVelocity": 0.239650083468562,
-            "velocityX": 0.6403788706575965,
-            "velocityY": 0.027422380395612005,
-            "timestamp": 0.07729291327834321
-        },
-        {
-            "x": 4.411499664032465,
-            "y": 3.0063632003056355,
-            "heading": 2.629246894113083,
-            "angularVelocity": 0.4751230020552096,
-            "velocityX": 1.2808795957906263,
-            "velocityY": 0.05490341165625346,
-            "timestamp": 0.15458582655668643
-        },
-        {
-            "x": 4.560023871242694,
-            "y": 3.012738860842861,
-            "heading": 2.6836373496883423,
-            "angularVelocity": 0.7036926578170436,
-            "velocityX": 1.9215760010930336,
-            "velocityY": 0.08248699999527603,
-            "timestamp": 0.23187873983502966
-        },
-        {
-            "x": 4.758098277209683,
-            "y": 3.021263020053228,
-            "heading": 2.7546133652741296,
-            "angularVelocity": 0.9182732617438291,
-            "velocityX": 2.5626464001129405,
-            "velocityY": 0.11028383908458261,
-            "timestamp": 0.30917165311337286
-        },
-        {
-            "x": 5.005797064956168,
-            "y": 3.0319826680841246,
-            "heading": 2.8389548559272906,
-            "angularVelocity": 1.0911930612504541,
-            "velocityX": 3.2046765640012143,
-            "velocityY": 0.13868862714869012,
-            "timestamp": 0.38646456639171606
-        },
-        {
-            "x": 5.297951167759167,
-            "y": 3.043836589819136,
-            "heading": 2.838954877461829,
-            "angularVelocity": 2.786094794708315e-7,
-            "velocityX": 3.7798303933881865,
-            "velocityY": 0.15336362975896514,
-            "timestamp": 0.46375747967005926
-        },
-        {
-            "x": 5.590081141172345,
-            "y": 3.0562710547940686,
-            "heading": 2.838954891655661,
-            "angularVelocity": 1.83636908600502e-7,
-            "velocityX": 3.7795182122476696,
-            "velocityY": 0.1608745801850421,
-            "timestamp": 0.5410503929484025
-        },
-        {
-            "x": 5.882211114419822,
-            "y": 3.0687055236618757,
-            "heading": 2.8389549058494925,
-            "angularVelocity": 1.8363690785211144e-7,
-            "velocityX": 3.779518210103882,
-            "velocityY": 0.16087463055026802,
-            "timestamp": 0.6183433062267457
-        },
-        {
-            "x": 6.17434108478517,
-            "y": 3.0811400602410006,
-            "heading": 2.838954920043324,
-            "angularVelocity": 1.8363690859499724e-7,
-            "velocityX": 3.779518172815467,
-            "velocityY": 0.1608755065855243,
-            "timestamp": 0.6956362195050889
-        },
-        {
-            "x": 6.465903802710874,
-            "y": 3.1031790875652407,
-            "heading": 2.8389549343224108,
-            "angularVelocity": 1.8473992278083087e-7,
-            "velocityX": 3.772179176061626,
-            "velocityY": 0.28513645545840094,
-            "timestamp": 0.7729291327834321
-        },
-        {
-            "x": 6.7494946065782635,
-            "y": 3.174388582838956,
-            "heading": 2.838954949771126,
-            "angularVelocity": 1.998723373980939e-7,
-            "velocityX": 3.669040172494696,
-            "velocityY": 0.9212939744848142,
-            "timestamp": 0.8502220460617753
-        },
-        {
-            "x": 7.016882485558567,
-            "y": 3.2927027326639142,
-            "heading": 2.838955049089198,
-            "angularVelocity": 0.000001284957026149309,
-            "velocityX": 3.4594100240134633,
-            "velocityY": 1.5307244197005683,
-            "timestamp": 0.9275149593401185
-        },
-        {
-            "x": 7.246855043486079,
-            "y": 3.4413903121844696,
-            "heading": 2.884377696826009,
-            "angularVelocity": 0.5876689829666233,
-            "velocityX": 2.9753382059665197,
-            "velocityY": 1.9236896788339406,
-            "timestamp": 1.0048078726184617
-        },
-        {
-            "x": 7.429257242099238,
-            "y": 3.603847510115455,
-            "heading": 2.9490702967114695,
-            "angularVelocity": 0.8369797066969458,
-            "velocityX": 2.359882567193478,
-            "velocityY": 2.101838202759324,
-            "timestamp": 1.082100785896805
-        },
-        {
-            "x": 7.562439804570718,
-            "y": 3.7739114806600167,
-            "heading": 3.01726271663618,
-            "angularVelocity": 0.8822596669262517,
-            "velocityX": 1.7230889200911585,
-            "velocityY": 2.2002530805396896,
-            "timestamp": 1.1593936991751483
-        },
-        {
-            "x": 7.646052714432655,
-            "y": 3.9488127153581356,
-            "heading": 3.0821685087673805,
-            "angularVelocity": 0.8397379446349181,
-            "velocityX": 1.0817668310784703,
-            "velocityY": 2.2628366208461252,
-            "timestamp": 1.2366866124534917
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.7482120776630553,
-            "velocityX": 0.43920307991361807,
-            "velocityY": 2.305350867034668,
-            "timestamp": 1.313979525731835
-        },
-        {
-            "x": 7.646672814687191,
-            "y": 4.349453153850708,
-            "heading": 3.196736252005302,
-            "angularVelocity": 0.5972944316963834,
-            "velocityX": -0.35085402203857,
-            "velocityY": 2.34188945184307,
-            "timestamp": 1.4089682766635396
-        },
-        {
-            "x": 7.538236721836807,
-            "y": 4.57348977092197,
-            "heading": 3.2390603114191148,
-            "angularVelocity": 0.44556917528311457,
-            "velocityX": -1.1415677307763321,
-            "velocityY": 2.3585594596600363,
-            "timestamp": 1.5039570275952443
-        },
-        {
-            "x": 7.354699862509888,
-            "y": 4.7956671896432965,
-            "heading": 3.2668511132107456,
-            "angularVelocity": 0.2925693992082484,
-            "velocityX": -1.932195733986223,
-            "velocityY": 2.3389866330705615,
-            "timestamp": 1.598945778526949
-        },
-        {
-            "x": 7.096712135037422,
-            "y": 5.0078309608342675,
-            "heading": 3.2799432590323385,
-            "angularVelocity": 0.13782838171022743,
-            "velocityX": -2.7159818919816576,
-            "velocityY": 2.2335673341310986,
-            "timestamp": 1.6939345294586536
-        },
-        {
-            "x": 6.778591882911988,
-            "y": 5.174931015852947,
-            "heading": 3.2799432523824414,
-            "angularVelocity": -7.000720422097915e-8,
-            "velocityX": -3.349030795806095,
-            "velocityY": 1.7591562514473036,
-            "timestamp": 1.7889232803903583
-        },
-        {
-            "x": 6.432650840523452,
-            "y": 5.272130447349719,
-            "heading": 3.2799432455857884,
-            "angularVelocity": -7.155218790875671e-8,
-            "velocityX": -3.6419159005181747,
-            "velocityY": 1.0232730775316359,
-            "timestamp": 1.883912031322063
-        },
-        {
-            "x": 6.076435890184285,
-            "y": 5.319394007657195,
-            "heading": 3.2799432391201027,
-            "angularVelocity": -6.806791276827721e-8,
-            "velocityX": -3.7500751072648586,
-            "velocityY": 0.4975700790239653,
-            "timestamp": 1.9789007822537676
-        },
-        {
-            "x": 5.72022088924427,
-            "y": 5.36665718659578,
-            "heading": 3.279943232654418,
-            "angularVelocity": -6.806790136756466e-8,
-            "velocityX": -3.7500756399684434,
-            "velocityY": 0.49756606413917676,
-            "timestamp": 2.073889533185472
-        },
-        {
-            "x": 5.364005888304073,
-            "y": 5.413920365532998,
-            "heading": 3.279943226188733,
-            "angularVelocity": -6.806790152018608e-8,
-            "velocityX": -3.750075639970353,
-            "velocityY": 0.4975660641247859,
-            "timestamp": 2.1688782841171768
-        },
-        {
-            "x": 5.007790887255383,
-            "y": 5.4611835436519325,
-            "heading": 3.2799432197229685,
-            "angularVelocity": -6.806874126926854e-8,
-            "velocityX": -3.750075641112522,
-            "velocityY": 0.4975660555102637,
-            "timestamp": 2.2638670350488814
-        },
-        {
-            "x": 4.7098745121627585,
-            "y": 5.500710065242911,
-            "heading": 3.22396872446115,
-            "angularVelocity": -0.5892749900676477,
-            "velocityX": -3.1363332202022756,
-            "velocityY": 0.416117921367316,
-            "timestamp": 2.358855785980586
-        },
-        {
-            "x": 4.4864372535551835,
-            "y": 5.530355016492048,
-            "heading": 3.181984877578949,
-            "angularVelocity": -0.44198756663709243,
-            "velocityX": -2.352249676050825,
-            "velocityY": 0.3120890732677509,
-            "timestamp": 2.4538445369122908
-        },
-        {
-            "x": 4.337479086660562,
-            "y": 5.550118338543265,
-            "heading": 3.1539948813381518,
-            "angularVelocity": -0.2946664311958542,
-            "velocityX": -1.5681663926891658,
-            "velocityY": 0.20805960555714925,
-            "timestamp": 2.5488332878439954
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -0.147331986165536,
-            "velocityX": -0.7840832301722918,
-            "velocityY": 0.10402980731729136,
-            "timestamp": 2.6438220387757
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -1.2414335719227057e-31,
-            "velocityX": 3.5860623226783847e-34,
-            "velocityY": 0,
-            "timestamp": 2.7388107897074048
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S2.traj
deleted file mode 100644
index 9ab4bfc6..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S2.traj	
+++ /dev/null
@@ -1,301 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.312496748515021,
-            "y": 3.002119555669804,
-            "heading": 2.5925232531186833,
-            "angularVelocity": 0.239650083468562,
-            "velocityX": 0.6403788706575965,
-            "velocityY": 0.027422380395612005,
-            "timestamp": 0.07729291327834321
-        },
-        {
-            "x": 4.411499664032465,
-            "y": 3.0063632003056355,
-            "heading": 2.629246894113083,
-            "angularVelocity": 0.4751230020552096,
-            "velocityX": 1.2808795957906263,
-            "velocityY": 0.05490341165625346,
-            "timestamp": 0.15458582655668643
-        },
-        {
-            "x": 4.560023871242694,
-            "y": 3.012738860842861,
-            "heading": 2.6836373496883423,
-            "angularVelocity": 0.7036926578170436,
-            "velocityX": 1.9215760010930336,
-            "velocityY": 0.08248699999527603,
-            "timestamp": 0.23187873983502966
-        },
-        {
-            "x": 4.758098277209683,
-            "y": 3.021263020053228,
-            "heading": 2.7546133652741296,
-            "angularVelocity": 0.9182732617438291,
-            "velocityX": 2.5626464001129405,
-            "velocityY": 0.11028383908458261,
-            "timestamp": 0.30917165311337286
-        },
-        {
-            "x": 5.005797064956168,
-            "y": 3.0319826680841246,
-            "heading": 2.8389548559272906,
-            "angularVelocity": 1.0911930612504541,
-            "velocityX": 3.2046765640012143,
-            "velocityY": 0.13868862714869012,
-            "timestamp": 0.38646456639171606
-        },
-        {
-            "x": 5.297951167759167,
-            "y": 3.043836589819136,
-            "heading": 2.838954877461829,
-            "angularVelocity": 2.786094794708315e-7,
-            "velocityX": 3.7798303933881865,
-            "velocityY": 0.15336362975896514,
-            "timestamp": 0.46375747967005926
-        },
-        {
-            "x": 5.590081141172345,
-            "y": 3.0562710547940686,
-            "heading": 2.838954891655661,
-            "angularVelocity": 1.83636908600502e-7,
-            "velocityX": 3.7795182122476696,
-            "velocityY": 0.1608745801850421,
-            "timestamp": 0.5410503929484025
-        },
-        {
-            "x": 5.882211114419822,
-            "y": 3.0687055236618757,
-            "heading": 2.8389549058494925,
-            "angularVelocity": 1.8363690785211144e-7,
-            "velocityX": 3.779518210103882,
-            "velocityY": 0.16087463055026802,
-            "timestamp": 0.6183433062267457
-        },
-        {
-            "x": 6.17434108478517,
-            "y": 3.0811400602410006,
-            "heading": 2.838954920043324,
-            "angularVelocity": 1.8363690859499724e-7,
-            "velocityX": 3.779518172815467,
-            "velocityY": 0.1608755065855243,
-            "timestamp": 0.6956362195050889
-        },
-        {
-            "x": 6.465903802710874,
-            "y": 3.1031790875652407,
-            "heading": 2.8389549343224108,
-            "angularVelocity": 1.8473992278083087e-7,
-            "velocityX": 3.772179176061626,
-            "velocityY": 0.28513645545840094,
-            "timestamp": 0.7729291327834321
-        },
-        {
-            "x": 6.7494946065782635,
-            "y": 3.174388582838956,
-            "heading": 2.838954949771126,
-            "angularVelocity": 1.998723373980939e-7,
-            "velocityX": 3.669040172494696,
-            "velocityY": 0.9212939744848142,
-            "timestamp": 0.8502220460617753
-        },
-        {
-            "x": 7.016882485558567,
-            "y": 3.2927027326639142,
-            "heading": 2.838955049089198,
-            "angularVelocity": 0.000001284957026149309,
-            "velocityX": 3.4594100240134633,
-            "velocityY": 1.5307244197005683,
-            "timestamp": 0.9275149593401185
-        },
-        {
-            "x": 7.246855043486079,
-            "y": 3.4413903121844696,
-            "heading": 2.884377696826009,
-            "angularVelocity": 0.5876689829666233,
-            "velocityX": 2.9753382059665197,
-            "velocityY": 1.9236896788339406,
-            "timestamp": 1.0048078726184617
-        },
-        {
-            "x": 7.429257242099238,
-            "y": 3.603847510115455,
-            "heading": 2.9490702967114695,
-            "angularVelocity": 0.8369797066969458,
-            "velocityX": 2.359882567193478,
-            "velocityY": 2.101838202759324,
-            "timestamp": 1.082100785896805
-        },
-        {
-            "x": 7.562439804570718,
-            "y": 3.7739114806600167,
-            "heading": 3.01726271663618,
-            "angularVelocity": 0.8822596669262517,
-            "velocityX": 1.7230889200911585,
-            "velocityY": 2.2002530805396896,
-            "timestamp": 1.1593936991751483
-        },
-        {
-            "x": 7.646052714432655,
-            "y": 3.9488127153581356,
-            "heading": 3.0821685087673805,
-            "angularVelocity": 0.8397379446349181,
-            "velocityX": 1.0817668310784703,
-            "velocityY": 2.2628366208461252,
-            "timestamp": 1.2366866124534917
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.7482120776630553,
-            "velocityX": 0.43920307991361807,
-            "velocityY": 2.305350867034668,
-            "timestamp": 1.313979525731835
-        },
-        {
-            "x": 7.646672814687191,
-            "y": 4.349453153850708,
-            "heading": 3.196736252005302,
-            "angularVelocity": 0.5972944316963834,
-            "velocityX": -0.35085402203857,
-            "velocityY": 2.34188945184307,
-            "timestamp": 1.4089682766635396
-        },
-        {
-            "x": 7.538236721836807,
-            "y": 4.57348977092197,
-            "heading": 3.2390603114191148,
-            "angularVelocity": 0.44556917528311457,
-            "velocityX": -1.1415677307763321,
-            "velocityY": 2.3585594596600363,
-            "timestamp": 1.5039570275952443
-        },
-        {
-            "x": 7.354699862509888,
-            "y": 4.7956671896432965,
-            "heading": 3.2668511132107456,
-            "angularVelocity": 0.2925693992082484,
-            "velocityX": -1.932195733986223,
-            "velocityY": 2.3389866330705615,
-            "timestamp": 1.598945778526949
-        },
-        {
-            "x": 7.096712135037422,
-            "y": 5.0078309608342675,
-            "heading": 3.2799432590323385,
-            "angularVelocity": 0.13782838171022743,
-            "velocityX": -2.7159818919816576,
-            "velocityY": 2.2335673341310986,
-            "timestamp": 1.6939345294586536
-        },
-        {
-            "x": 6.778591882911988,
-            "y": 5.174931015852947,
-            "heading": 3.2799432523824414,
-            "angularVelocity": -7.000720422097915e-8,
-            "velocityX": -3.349030795806095,
-            "velocityY": 1.7591562514473036,
-            "timestamp": 1.7889232803903583
-        },
-        {
-            "x": 6.432650840523452,
-            "y": 5.272130447349719,
-            "heading": 3.2799432455857884,
-            "angularVelocity": -7.155218790875671e-8,
-            "velocityX": -3.6419159005181747,
-            "velocityY": 1.0232730775316359,
-            "timestamp": 1.883912031322063
-        },
-        {
-            "x": 6.076435890184285,
-            "y": 5.319394007657195,
-            "heading": 3.2799432391201027,
-            "angularVelocity": -6.806791276827721e-8,
-            "velocityX": -3.7500751072648586,
-            "velocityY": 0.4975700790239653,
-            "timestamp": 1.9789007822537676
-        },
-        {
-            "x": 5.72022088924427,
-            "y": 5.36665718659578,
-            "heading": 3.279943232654418,
-            "angularVelocity": -6.806790136756466e-8,
-            "velocityX": -3.7500756399684434,
-            "velocityY": 0.49756606413917676,
-            "timestamp": 2.073889533185472
-        },
-        {
-            "x": 5.364005888304073,
-            "y": 5.413920365532998,
-            "heading": 3.279943226188733,
-            "angularVelocity": -6.806790152018608e-8,
-            "velocityX": -3.750075639970353,
-            "velocityY": 0.4975660641247859,
-            "timestamp": 2.1688782841171768
-        },
-        {
-            "x": 5.007790887255383,
-            "y": 5.4611835436519325,
-            "heading": 3.2799432197229685,
-            "angularVelocity": -6.806874126926854e-8,
-            "velocityX": -3.750075641112522,
-            "velocityY": 0.4975660555102637,
-            "timestamp": 2.2638670350488814
-        },
-        {
-            "x": 4.7098745121627585,
-            "y": 5.500710065242911,
-            "heading": 3.22396872446115,
-            "angularVelocity": -0.5892749900676477,
-            "velocityX": -3.1363332202022756,
-            "velocityY": 0.416117921367316,
-            "timestamp": 2.358855785980586
-        },
-        {
-            "x": 4.4864372535551835,
-            "y": 5.530355016492048,
-            "heading": 3.181984877578949,
-            "angularVelocity": -0.44198756663709243,
-            "velocityX": -2.352249676050825,
-            "velocityY": 0.3120890732677509,
-            "timestamp": 2.4538445369122908
-        },
-        {
-            "x": 4.337479086660562,
-            "y": 5.550118338543265,
-            "heading": 3.1539948813381518,
-            "angularVelocity": -0.2946664311958542,
-            "velocityX": -1.5681663926891658,
-            "velocityY": 0.20805960555714925,
-            "timestamp": 2.5488332878439954
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -0.147331986165536,
-            "velocityX": -0.7840832301722918,
-            "velocityY": 0.10402980731729136,
-            "timestamp": 2.6438220387757
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -1.2414335719227057e-31,
-            "velocityX": 3.5860623226783847e-34,
-            "velocityY": 0,
-            "timestamp": 2.7388107897074048
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S3.1.traj
deleted file mode 100644
index 49c85196..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S3.1.traj	
+++ /dev/null
@@ -1,319 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.312741719551674,
-            "y": 3.016403262503484,
-            "heading": 2.6023238516402656,
-            "angularVelocity": 0.35533614833193405,
-            "velocityX": 0.624033456373989,
-            "velocityY": 0.2057867055686534,
-            "timestamp": 0.07971002042214732
-        },
-        {
-            "x": 4.412293799902437,
-            "y": 3.049211318978086,
-            "heading": 2.6573890862312104,
-            "angularVelocity": 0.6908194766394113,
-            "velocityX": 1.248928049742448,
-            "velocityY": 0.41159262412491066,
-            "timestamp": 0.15942004084429465
-        },
-        {
-            "x": 4.5617613818375675,
-            "y": 3.0984309719087952,
-            "heading": 2.7365422108568223,
-            "angularVelocity": 0.9930134781852262,
-            "velocityX": 1.875141684113798,
-            "velocityY": 0.6174838830806988,
-            "timestamp": 0.23913006126644198
-        },
-        {
-            "x": 4.761327239793144,
-            "y": 3.1640795845539236,
-            "heading": 2.8341908945522007,
-            "angularVelocity": 1.225049036221883,
-            "velocityX": 2.503648310446647,
-            "velocityY": 0.8235929723446413,
-            "timestamp": 0.3188400816885893
-        },
-        {
-            "x": 5.011274516078503,
-            "y": 3.2461089339964913,
-            "heading": 2.9311433561003417,
-            "angularVelocity": 1.2163145992771967,
-            "velocityX": 3.1357070912995426,
-            "velocityY": 1.0290970822506025,
-            "timestamp": 0.3985501021107366
-        },
-        {
-            "x": 5.297580410210689,
-            "y": 3.3407358757733725,
-            "heading": 2.9311433795557584,
-            "angularVelocity": 2.942593217144604e-7,
-            "velocityX": 3.5918431913064333,
-            "velocityY": 1.1871398511220184,
-            "timestamp": 0.4782601225328839
-        },
-        {
-            "x": 5.583886278072779,
-            "y": 3.4353628970344365,
-            "heading": 2.931143403010656,
-            "angularVelocity": 2.942528124810342e-7,
-            "velocityX": 3.5918428617356097,
-            "velocityY": 1.187140848288782,
-            "timestamp": 0.5579701429550312
-        },
-        {
-            "x": 5.8701921459347055,
-            "y": 3.529989918295993,
-            "heading": 2.9311434264655536,
-            "angularVelocity": 2.942528130233853e-7,
-            "velocityX": 3.591842861733567,
-            "velocityY": 1.1871408482949637,
-            "timestamp": 0.6376801633771786
-        },
-        {
-            "x": 6.156498013796632,
-            "y": 3.62461693955755,
-            "heading": 2.931143449920451,
-            "angularVelocity": 2.9425281250003654e-7,
-            "velocityX": 3.5918428617335665,
-            "velocityY": 1.1871408482949637,
-            "timestamp": 0.717390183799326
-        },
-        {
-            "x": 6.442803881658564,
-            "y": 3.7192439608190906,
-            "heading": 2.9311434733753488,
-            "angularVelocity": 2.9425281349790425e-7,
-            "velocityX": 3.5918428617336366,
-            "velocityY": 1.1871408482947532,
-            "timestamp": 0.7971002042214733
-        },
-        {
-            "x": 6.729109750413145,
-            "y": 3.813870979379799,
-            "heading": 2.9311434968302543,
-            "angularVelocity": 2.942529087425497e-7,
-            "velocityX": 3.591842872932335,
-            "velocityY": 1.1871408144115423,
-            "timestamp": 0.8768102246436207
-        },
-        {
-            "x": 7.011880136025942,
-            "y": 3.9068513947536463,
-            "heading": 2.940966575440456,
-            "angularVelocity": 0.12323517869119305,
-            "velocityX": 3.547488560600464,
-            "velocityY": 1.1664833967099661,
-            "timestamp": 0.956520245065768
-        },
-        {
-            "x": 7.2458966997348915,
-            "y": 3.984032577425885,
-            "heading": 2.999027591028601,
-            "angularVelocity": 0.7284029696724628,
-            "velocityX": 2.935848748621424,
-            "velocityY": 0.9682745314012468,
-            "timestamp": 1.0362302654879154
-        },
-        {
-            "x": 7.429606919081267,
-            "y": 4.044603850649925,
-            "heading": 3.057792091345064,
-            "angularVelocity": 0.7372285191403096,
-            "velocityX": 2.304731806282817,
-            "velocityY": 0.7598953419313301,
-            "timestamp": 1.1159402859100627
-        },
-        {
-            "x": 7.563114805498745,
-            "y": 4.088594555269305,
-            "heading": 3.1044636591164667,
-            "angularVelocity": 0.5855169466050564,
-            "velocityX": 1.674919736695776,
-            "velocityY": 0.55188424725527,
-            "timestamp": 1.19565030633221
-        },
-        {
-            "x": 7.646547133770953,
-            "y": 4.116048088709135,
-            "heading": 3.133033734418083,
-            "angularVelocity": 0.35842514090835725,
-            "velocityX": 1.046698116878499,
-            "velocityY": 0.34441759385375514,
-            "timestamp": 1.2753603267543572
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.0873951047186249,
-            "velocityX": 0.4196820682252798,
-            "velocityY": 0.13739691989618263,
-            "timestamp": 1.3550703471765044
-        },
-        {
-            "x": 7.658055452006423,
-            "y": 4.119659499210501,
-            "heading": 3.1197811319835678,
-            "angularVelocity": -0.235277452949756,
-            "velocityX": -0.2553583788106411,
-            "velocityY": -0.08541795355335377,
-            "timestamp": 1.4410066226799219
-        },
-        {
-            "x": 7.578036609646341,
-            "y": 4.0931397899338755,
-            "heading": 3.0739363035782867,
-            "angularVelocity": -0.5334746954847726,
-            "velocityX": -0.9311416150086622,
-            "velocityY": -0.30859737778105817,
-            "timestamp": 1.5269428981833393
-        },
-        {
-            "x": 7.439853098092205,
-            "y": 4.047395776011725,
-            "heading": 3.0057619833378117,
-            "angularVelocity": -0.7933124846417565,
-            "velocityX": -1.6079765005482662,
-            "velocityY": -0.5323015647836847,
-            "timestamp": 1.6128791736867567
-        },
-        {
-            "x": 7.243372469557272,
-            "y": 3.982360857227892,
-            "heading": 2.9212012505051064,
-            "angularVelocity": -0.9839934572140336,
-            "velocityX": -2.2863526186577556,
-            "velocityY": -0.7567807471624278,
-            "timestamp": 1.6988154491901741
-        },
-        {
-            "x": 6.9884466912904,
-            "y": 3.897942797791821,
-            "heading": 2.8348834977989155,
-            "angularVelocity": -1.0044390707013913,
-            "velocityX": -2.9664513242342614,
-            "velocityY": -0.9823332340335614,
-            "timestamp": 1.7847517246935916
-        },
-        {
-            "x": 6.679412229111129,
-            "y": 3.7970350761603098,
-            "heading": 2.8348833945193865,
-            "angularVelocity": -0.000001201815274119231,
-            "velocityX": -3.596088617629014,
-            "velocityY": -1.1742156736534226,
-            "timestamp": 1.870688000197009
-        },
-        {
-            "x": 6.370694817722953,
-            "y": 3.695161383669821,
-            "heading": 2.8348833730448595,
-            "angularVelocity": -2.4988896913294006e-7,
-            "velocityX": -3.5923992467639496,
-            "velocityY": -1.1854562219936775,
-            "timestamp": 1.9566242757004264
-        },
-        {
-            "x": 6.061977407921374,
-            "y": 3.59328768637132,
-            "heading": 2.834883351570332,
-            "angularVelocity": -2.498889692815502e-7,
-            "velocityX": -3.592399228301472,
-            "velocityY": -1.185456277942254,
-            "timestamp": 2.042560551203844
-        },
-        {
-            "x": 5.753259998119802,
-            "y": 3.4914139890727944,
-            "heading": 2.834883330095805,
-            "angularVelocity": -2.4988896886019487e-7,
-            "velocityX": -3.59239922830138,
-            "velocityY": -1.1854562779425326,
-            "timestamp": 2.128496826707261
-        },
-        {
-            "x": 5.444542588318229,
-            "y": 3.3895402917742743,
-            "heading": 2.8348833086212775,
-            "angularVelocity": -2.4988896799949907e-7,
-            "velocityX": -3.5923992283013995,
-            "velocityY": -1.1854562779424735,
-            "timestamp": 2.2144331022106782
-        },
-        {
-            "x": 5.135825178181493,
-            "y": 3.28766659549148,
-            "heading": 2.8348832871467184,
-            "angularVelocity": -2.498893376295858e-7,
-            "velocityX": -3.5923992322015317,
-            "velocityY": -1.1854562661229522,
-            "timestamp": 2.3003693777140954
-        },
-        {
-            "x": 4.844823310807952,
-            "y": 3.1918227901947045,
-            "heading": 2.7888021821480646,
-            "angularVelocity": -0.5362241350199245,
-            "velocityX": -3.386251797263078,
-            "velocityY": -1.1152892621342902,
-            "timestamp": 2.3863056532175126
-        },
-        {
-            "x": 4.611844119098646,
-            "y": 3.115032101312665,
-            "heading": 2.714656158607103,
-            "angularVelocity": -0.8628023859145838,
-            "velocityX": -2.7110692236137233,
-            "velocityY": -0.8935771120193101,
-            "timestamp": 2.47224192872093
-        },
-        {
-            "x": 4.43731744566658,
-            "y": 3.0574895143251735,
-            "heading": 2.6481597119281033,
-            "angularVelocity": -0.7737878595443164,
-            "velocityX": -2.030884773742906,
-            "velocityY": -0.669596007627809,
-            "timestamp": 2.558178204224347
-        },
-        {
-            "x": 4.321077385755849,
-            "y": 3.019155902534149,
-            "heading": 2.5996556036782517,
-            "angularVelocity": -0.5644194836896647,
-            "velocityX": -1.3526308794486799,
-            "velocityY": -0.44607020221047544,
-            "timestamp": 2.6441144797277643
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -0.2985421875449052,
-            "velocityX": -0.675819209241155,
-            "velocityY": -0.2229082238197189,
-            "timestamp": 2.7300507552311815
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": -1.2326632641143652e-35,
-            "velocityY": 0,
-            "timestamp": 2.8159870307345987
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S3.traj
deleted file mode 100644
index 49c85196..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S3.traj	
+++ /dev/null
@@ -1,319 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.312741719551674,
-            "y": 3.016403262503484,
-            "heading": 2.6023238516402656,
-            "angularVelocity": 0.35533614833193405,
-            "velocityX": 0.624033456373989,
-            "velocityY": 0.2057867055686534,
-            "timestamp": 0.07971002042214732
-        },
-        {
-            "x": 4.412293799902437,
-            "y": 3.049211318978086,
-            "heading": 2.6573890862312104,
-            "angularVelocity": 0.6908194766394113,
-            "velocityX": 1.248928049742448,
-            "velocityY": 0.41159262412491066,
-            "timestamp": 0.15942004084429465
-        },
-        {
-            "x": 4.5617613818375675,
-            "y": 3.0984309719087952,
-            "heading": 2.7365422108568223,
-            "angularVelocity": 0.9930134781852262,
-            "velocityX": 1.875141684113798,
-            "velocityY": 0.6174838830806988,
-            "timestamp": 0.23913006126644198
-        },
-        {
-            "x": 4.761327239793144,
-            "y": 3.1640795845539236,
-            "heading": 2.8341908945522007,
-            "angularVelocity": 1.225049036221883,
-            "velocityX": 2.503648310446647,
-            "velocityY": 0.8235929723446413,
-            "timestamp": 0.3188400816885893
-        },
-        {
-            "x": 5.011274516078503,
-            "y": 3.2461089339964913,
-            "heading": 2.9311433561003417,
-            "angularVelocity": 1.2163145992771967,
-            "velocityX": 3.1357070912995426,
-            "velocityY": 1.0290970822506025,
-            "timestamp": 0.3985501021107366
-        },
-        {
-            "x": 5.297580410210689,
-            "y": 3.3407358757733725,
-            "heading": 2.9311433795557584,
-            "angularVelocity": 2.942593217144604e-7,
-            "velocityX": 3.5918431913064333,
-            "velocityY": 1.1871398511220184,
-            "timestamp": 0.4782601225328839
-        },
-        {
-            "x": 5.583886278072779,
-            "y": 3.4353628970344365,
-            "heading": 2.931143403010656,
-            "angularVelocity": 2.942528124810342e-7,
-            "velocityX": 3.5918428617356097,
-            "velocityY": 1.187140848288782,
-            "timestamp": 0.5579701429550312
-        },
-        {
-            "x": 5.8701921459347055,
-            "y": 3.529989918295993,
-            "heading": 2.9311434264655536,
-            "angularVelocity": 2.942528130233853e-7,
-            "velocityX": 3.591842861733567,
-            "velocityY": 1.1871408482949637,
-            "timestamp": 0.6376801633771786
-        },
-        {
-            "x": 6.156498013796632,
-            "y": 3.62461693955755,
-            "heading": 2.931143449920451,
-            "angularVelocity": 2.9425281250003654e-7,
-            "velocityX": 3.5918428617335665,
-            "velocityY": 1.1871408482949637,
-            "timestamp": 0.717390183799326
-        },
-        {
-            "x": 6.442803881658564,
-            "y": 3.7192439608190906,
-            "heading": 2.9311434733753488,
-            "angularVelocity": 2.9425281349790425e-7,
-            "velocityX": 3.5918428617336366,
-            "velocityY": 1.1871408482947532,
-            "timestamp": 0.7971002042214733
-        },
-        {
-            "x": 6.729109750413145,
-            "y": 3.813870979379799,
-            "heading": 2.9311434968302543,
-            "angularVelocity": 2.942529087425497e-7,
-            "velocityX": 3.591842872932335,
-            "velocityY": 1.1871408144115423,
-            "timestamp": 0.8768102246436207
-        },
-        {
-            "x": 7.011880136025942,
-            "y": 3.9068513947536463,
-            "heading": 2.940966575440456,
-            "angularVelocity": 0.12323517869119305,
-            "velocityX": 3.547488560600464,
-            "velocityY": 1.1664833967099661,
-            "timestamp": 0.956520245065768
-        },
-        {
-            "x": 7.2458966997348915,
-            "y": 3.984032577425885,
-            "heading": 2.999027591028601,
-            "angularVelocity": 0.7284029696724628,
-            "velocityX": 2.935848748621424,
-            "velocityY": 0.9682745314012468,
-            "timestamp": 1.0362302654879154
-        },
-        {
-            "x": 7.429606919081267,
-            "y": 4.044603850649925,
-            "heading": 3.057792091345064,
-            "angularVelocity": 0.7372285191403096,
-            "velocityX": 2.304731806282817,
-            "velocityY": 0.7598953419313301,
-            "timestamp": 1.1159402859100627
-        },
-        {
-            "x": 7.563114805498745,
-            "y": 4.088594555269305,
-            "heading": 3.1044636591164667,
-            "angularVelocity": 0.5855169466050564,
-            "velocityX": 1.674919736695776,
-            "velocityY": 0.55188424725527,
-            "timestamp": 1.19565030633221
-        },
-        {
-            "x": 7.646547133770953,
-            "y": 4.116048088709135,
-            "heading": 3.133033734418083,
-            "angularVelocity": 0.35842514090835725,
-            "velocityX": 1.046698116878499,
-            "velocityY": 0.34441759385375514,
-            "timestamp": 1.2753603267543572
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.0873951047186249,
-            "velocityX": 0.4196820682252798,
-            "velocityY": 0.13739691989618263,
-            "timestamp": 1.3550703471765044
-        },
-        {
-            "x": 7.658055452006423,
-            "y": 4.119659499210501,
-            "heading": 3.1197811319835678,
-            "angularVelocity": -0.235277452949756,
-            "velocityX": -0.2553583788106411,
-            "velocityY": -0.08541795355335377,
-            "timestamp": 1.4410066226799219
-        },
-        {
-            "x": 7.578036609646341,
-            "y": 4.0931397899338755,
-            "heading": 3.0739363035782867,
-            "angularVelocity": -0.5334746954847726,
-            "velocityX": -0.9311416150086622,
-            "velocityY": -0.30859737778105817,
-            "timestamp": 1.5269428981833393
-        },
-        {
-            "x": 7.439853098092205,
-            "y": 4.047395776011725,
-            "heading": 3.0057619833378117,
-            "angularVelocity": -0.7933124846417565,
-            "velocityX": -1.6079765005482662,
-            "velocityY": -0.5323015647836847,
-            "timestamp": 1.6128791736867567
-        },
-        {
-            "x": 7.243372469557272,
-            "y": 3.982360857227892,
-            "heading": 2.9212012505051064,
-            "angularVelocity": -0.9839934572140336,
-            "velocityX": -2.2863526186577556,
-            "velocityY": -0.7567807471624278,
-            "timestamp": 1.6988154491901741
-        },
-        {
-            "x": 6.9884466912904,
-            "y": 3.897942797791821,
-            "heading": 2.8348834977989155,
-            "angularVelocity": -1.0044390707013913,
-            "velocityX": -2.9664513242342614,
-            "velocityY": -0.9823332340335614,
-            "timestamp": 1.7847517246935916
-        },
-        {
-            "x": 6.679412229111129,
-            "y": 3.7970350761603098,
-            "heading": 2.8348833945193865,
-            "angularVelocity": -0.000001201815274119231,
-            "velocityX": -3.596088617629014,
-            "velocityY": -1.1742156736534226,
-            "timestamp": 1.870688000197009
-        },
-        {
-            "x": 6.370694817722953,
-            "y": 3.695161383669821,
-            "heading": 2.8348833730448595,
-            "angularVelocity": -2.4988896913294006e-7,
-            "velocityX": -3.5923992467639496,
-            "velocityY": -1.1854562219936775,
-            "timestamp": 1.9566242757004264
-        },
-        {
-            "x": 6.061977407921374,
-            "y": 3.59328768637132,
-            "heading": 2.834883351570332,
-            "angularVelocity": -2.498889692815502e-7,
-            "velocityX": -3.592399228301472,
-            "velocityY": -1.185456277942254,
-            "timestamp": 2.042560551203844
-        },
-        {
-            "x": 5.753259998119802,
-            "y": 3.4914139890727944,
-            "heading": 2.834883330095805,
-            "angularVelocity": -2.4988896886019487e-7,
-            "velocityX": -3.59239922830138,
-            "velocityY": -1.1854562779425326,
-            "timestamp": 2.128496826707261
-        },
-        {
-            "x": 5.444542588318229,
-            "y": 3.3895402917742743,
-            "heading": 2.8348833086212775,
-            "angularVelocity": -2.4988896799949907e-7,
-            "velocityX": -3.5923992283013995,
-            "velocityY": -1.1854562779424735,
-            "timestamp": 2.2144331022106782
-        },
-        {
-            "x": 5.135825178181493,
-            "y": 3.28766659549148,
-            "heading": 2.8348832871467184,
-            "angularVelocity": -2.498893376295858e-7,
-            "velocityX": -3.5923992322015317,
-            "velocityY": -1.1854562661229522,
-            "timestamp": 2.3003693777140954
-        },
-        {
-            "x": 4.844823310807952,
-            "y": 3.1918227901947045,
-            "heading": 2.7888021821480646,
-            "angularVelocity": -0.5362241350199245,
-            "velocityX": -3.386251797263078,
-            "velocityY": -1.1152892621342902,
-            "timestamp": 2.3863056532175126
-        },
-        {
-            "x": 4.611844119098646,
-            "y": 3.115032101312665,
-            "heading": 2.714656158607103,
-            "angularVelocity": -0.8628023859145838,
-            "velocityX": -2.7110692236137233,
-            "velocityY": -0.8935771120193101,
-            "timestamp": 2.47224192872093
-        },
-        {
-            "x": 4.43731744566658,
-            "y": 3.0574895143251735,
-            "heading": 2.6481597119281033,
-            "angularVelocity": -0.7737878595443164,
-            "velocityX": -2.030884773742906,
-            "velocityY": -0.669596007627809,
-            "timestamp": 2.558178204224347
-        },
-        {
-            "x": 4.321077385755849,
-            "y": 3.019155902534149,
-            "heading": 2.5996556036782517,
-            "angularVelocity": -0.5644194836896647,
-            "velocityX": -1.3526308794486799,
-            "velocityY": -0.44607020221047544,
-            "timestamp": 2.6441144797277643
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -0.2985421875449052,
-            "velocityX": -0.675819209241155,
-            "velocityY": -0.2229082238197189,
-            "timestamp": 2.7300507552311815
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": -1.2326632641143652e-35,
-            "velocityY": 0,
-            "timestamp": 2.8159870307345987
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S4.1.traj
deleted file mode 100644
index 13cbda64..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S4.1.traj	
+++ /dev/null
@@ -1,328 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -2.99079667903027e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 4.307911912995487,
-            "y": 3.0252386980683,
-            "heading": 2.6037905893528444,
-            "angularVelocity": 0.3765521075910298,
-            "velocityX": 0.5676851603736718,
-            "velocityY": 0.31901634566234616,
-            "timestamp": 0.07911412192970564
-        },
-        {
-            "x": 4.397813948925716,
-            "y": 3.0757408190390847,
-            "heading": 2.6615026890848092,
-            "angularVelocity": 0.7294791160450911,
-            "velocityX": 1.1363588919069176,
-            "velocityY": 0.6383452124471162,
-            "timestamp": 0.15822824385941128
-        },
-        {
-            "x": 4.532822684228442,
-            "y": 3.151547007950623,
-            "heading": 2.744086644451416,
-            "angularVelocity": 1.0438585849437199,
-            "velocityX": 1.7065061459278321,
-            "velocityY": 0.9581878312306107,
-            "timestamp": 0.23734236578911694
-        },
-        {
-            "x": 4.713127943842242,
-            "y": 3.2527364142046724,
-            "heading": 2.845383620284061,
-            "angularVelocity": 1.2803905720226456,
-            "velocityX": 2.2790527811710266,
-            "velocityY": 1.2790308959500052,
-            "timestamp": 0.31645648771882257
-        },
-        {
-            "x": 4.938980637634121,
-            "y": 3.3793948659075563,
-            "heading": 2.944365706531666,
-            "angularVelocity": 1.251130440852921,
-            "velocityX": 2.854770909200696,
-            "velocityY": 1.6009588252198856,
-            "timestamp": 0.3955706096485282
-        },
-        {
-            "x": 5.199953974250007,
-            "y": 3.5259002853585115,
-            "heading": 2.944365731476778,
-            "angularVelocity": 3.1530542976951733e-7,
-            "velocityX": 3.2986947241576456,
-            "velocityY": 1.8518238700939902,
-            "timestamp": 0.4746847315782338
-        },
-        {
-            "x": 5.460927260915445,
-            "y": 3.6724057937882604,
-            "heading": 2.944365756420666,
-            "angularVelocity": 3.152899521547411e-7,
-            "velocityX": 3.2986940927855755,
-            "velocityY": 1.8518249947831291,
-            "timestamp": 0.5537988535079394
-        },
-        {
-            "x": 5.721900547580569,
-            "y": 3.818911302218567,
-            "heading": 2.9443657813645534,
-            "angularVelocity": 3.1528995198652365e-7,
-            "velocityX": 3.2986940927816133,
-            "velocityY": 1.851824994790188,
-            "timestamp": 0.6329129754376451
-        },
-        {
-            "x": 5.982873834247296,
-            "y": 3.965416810646019,
-            "heading": 2.9443658063084412,
-            "angularVelocity": 3.1528995220649985e-7,
-            "velocityX": 3.2986940928018726,
-            "velocityY": 1.851824994754099,
-            "timestamp": 0.7120270973673508
-        },
-        {
-            "x": 6.243847376304821,
-            "y": 4.1119218641396325,
-            "heading": 2.944365831252335,
-            "angularVelocity": 3.152900317708399e-7,
-            "velocityX": 3.298697320933481,
-            "velocityY": 1.8518192444047645,
-            "timestamp": 0.7911412192970565
-        },
-        {
-            "x": 6.520761156875476,
-            "y": 4.225454581965132,
-            "heading": 2.9443658572464595,
-            "angularVelocity": 3.2856491270029264e-7,
-            "velocityX": 3.5001814317891196,
-            "velocityY": 1.4350499639795518,
-            "timestamp": 0.8702553412267622
-        },
-        {
-            "x": 6.81318632649156,
-            "y": 4.28915727346349,
-            "heading": 2.9443676715667886,
-            "angularVelocity": 0.000022932951594388774,
-            "velocityX": 3.696244898931049,
-            "velocityY": 0.8052000065798415,
-            "timestamp": 0.9493694631564679
-        },
-        {
-            "x": 7.072106261164272,
-            "y": 4.3159902610395875,
-            "heading": 2.999666770720341,
-            "angularVelocity": 0.6989788650209022,
-            "velocityX": 3.2727397885142184,
-            "velocityY": 0.3391681146374708,
-            "timestamp": 1.0284835850861735
-        },
-        {
-            "x": 7.2892635874980165,
-            "y": 4.311538961400137,
-            "heading": 3.058239945124217,
-            "angularVelocity": 0.7403630726751829,
-            "velocityX": 2.7448617394337207,
-            "velocityY": -0.05626428671490304,
-            "timestamp": 1.1075977070158791
-        },
-        {
-            "x": 7.463265555506274,
-            "y": 4.277921305765472,
-            "heading": 3.1051985992516418,
-            "angularVelocity": 0.593555903573676,
-            "velocityX": 2.1993793745554173,
-            "velocityY": -0.4249261044006207,
-            "timestamp": 1.1867118289455847
-        },
-        {
-            "x": 7.593597601326297,
-            "y": 4.21619741378488,
-            "heading": 3.1337465954664303,
-            "angularVelocity": 0.3608457696105646,
-            "velocityX": 1.6473929387198967,
-            "velocityY": -0.7801880432350068,
-            "timestamp": 1.2658259508752903
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.07904283560305705,
-            "velocityX": 1.0921235876253772,
-            "velocityY": -1.1274524902663325,
-            "timestamp": 1.3449400728049958
-        },
-        {
-            "x": 7.722328891064707,
-            "y": 4.011878570496831,
-            "heading": 3.1218227549328446,
-            "angularVelocity": -0.23159012596074638,
-            "velocityX": 0.539298071695534,
-            "velocityY": -1.4667231618939216,
-            "timestamp": 1.423428933207607
-        },
-        {
-            "x": 7.720975293339473,
-            "y": 3.870431716581803,
-            "heading": 3.0818439654671956,
-            "angularVelocity": -0.5093562227885172,
-            "velocityX": -0.01724573039142055,
-            "velocityY": -1.8021264825284895,
-            "timestamp": 1.5019177936102184
-        },
-        {
-            "x": 7.675510236414314,
-            "y": 3.703145056798568,
-            "heading": 3.02401855987363,
-            "angularVelocity": -0.7367339173603616,
-            "velocityX": -0.5792548992550559,
-            "velocityY": -2.131342701692084,
-            "timestamp": 1.5804066540128296
-        },
-        {
-            "x": 7.585250814368908,
-            "y": 3.51090702633694,
-            "heading": 2.955217915344013,
-            "angularVelocity": -0.8765657212590741,
-            "velocityX": -1.149964741269228,
-            "velocityY": -2.4492396688591995,
-            "timestamp": 1.658895514415441
-        },
-        {
-            "x": 7.449012873441734,
-            "y": 3.2957924623075137,
-            "heading": 2.8903514658300926,
-            "angularVelocity": -0.8264414743848527,
-            "velocityX": -1.7357614854940644,
-            "velocityY": -2.7407018387830946,
-            "timestamp": 1.7373843748180522
-        },
-        {
-            "x": 7.265754194056636,
-            "y": 3.066909845502957,
-            "heading": 2.8806156888219427,
-            "angularVelocity": -0.12404023906336885,
-            "velocityX": -2.334836796522032,
-            "velocityY": -2.916115938370047,
-            "timestamp": 1.8158732352206635
-        },
-        {
-            "x": 7.0429291564371885,
-            "y": 2.8706709413875,
-            "heading": 2.8806156606364275,
-            "angularVelocity": -3.5910210875012317e-7,
-            "velocityX": -2.838938372610575,
-            "velocityY": -2.5002134456895257,
-            "timestamp": 1.8943620956232747
-        },
-        {
-            "x": 6.796142716598087,
-            "y": 2.7055735191439014,
-            "heading": 2.8806156351575583,
-            "angularVelocity": -3.246176492669905e-7,
-            "velocityX": -3.1442224867732946,
-            "velocityY": -2.103450367309771,
-            "timestamp": 1.972850956025886
-        },
-        {
-            "x": 6.549355877169833,
-            "y": 2.5404766942048504,
-            "heading": 2.880615609678698,
-            "angularVelocity": -3.246175317472835e-7,
-            "velocityX": -3.1442275778034396,
-            "velocityY": -2.10344275725474,
-            "timestamp": 2.0513398164284973
-        },
-        {
-            "x": 6.302569037739046,
-            "y": 2.375379869269585,
-            "heading": 2.8806155841998384,
-            "angularVelocity": -3.24617530454395e-7,
-            "velocityX": -3.144227577835705,
-            "velocityY": -2.103442757206509,
-            "timestamp": 2.1298286768311088
-        },
-        {
-            "x": 6.055782203816465,
-            "y": 2.2102830361006496,
-            "heading": 2.8806155587209785,
-            "angularVelocity": -3.246175312544412e-7,
-            "velocityX": -3.1442275076575137,
-            "velocityY": -2.103442862108911,
-            "timestamp": 2.2083175372337203
-        },
-        {
-            "x": 5.809854810532114,
-            "y": 2.043908693921413,
-            "heading": 2.8806155277577266,
-            "angularVelocity": -3.944923106855428e-7,
-            "velocityX": -3.1332776654274115,
-            "velocityY": -2.1197191719412425,
-            "timestamp": 2.2868063976363318
-        },
-        {
-            "x": 5.598037560183058,
-            "y": 1.9023806628591073,
-            "heading": 2.776131644051983,
-            "angularVelocity": -1.3311937919570889,
-            "velocityX": -2.6986918814024308,
-            "velocityY": -1.8031607330815247,
-            "timestamp": 2.3652952580389432
-        },
-        {
-            "x": 5.428977486755044,
-            "y": 1.7893186281455802,
-            "heading": 2.67268813100655,
-            "angularVelocity": -1.3179387815648507,
-            "velocityX": -2.1539371645965275,
-            "velocityY": -1.4404851100343643,
-            "timestamp": 2.4437841184415547
-        },
-        {
-            "x": 5.302396782822421,
-            "y": 1.7046186401543708,
-            "heading": 2.5887102200267456,
-            "angularVelocity": -1.0699341352267786,
-            "velocityX": -1.6127218981562843,
-            "velocityY": -1.0791338739884617,
-            "timestamp": 2.5222729788441662
-        },
-        {
-            "x": 5.218108357184172,
-            "y": 1.6481959100495345,
-            "heading": 2.5301489625141826,
-            "angularVelocity": -0.7461091575565081,
-            "velocityX": -1.0738902973732658,
-            "velocityY": -0.7188629037982504,
-            "timestamp": 2.6007618392467777
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.3841177252355578,
-            "velocityX": -0.5364883241797085,
-            "velocityY": -0.35923454493927354,
-            "timestamp": 2.6792506996493892
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": -9.31070457943821e-33,
-            "velocityY": 1.510534612718486e-32,
-            "timestamp": 2.7577395600520007
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S4.traj
deleted file mode 100644
index 13cbda64..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3 To S4.traj	
+++ /dev/null
@@ -1,328 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -2.99079667903027e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 4.307911912995487,
-            "y": 3.0252386980683,
-            "heading": 2.6037905893528444,
-            "angularVelocity": 0.3765521075910298,
-            "velocityX": 0.5676851603736718,
-            "velocityY": 0.31901634566234616,
-            "timestamp": 0.07911412192970564
-        },
-        {
-            "x": 4.397813948925716,
-            "y": 3.0757408190390847,
-            "heading": 2.6615026890848092,
-            "angularVelocity": 0.7294791160450911,
-            "velocityX": 1.1363588919069176,
-            "velocityY": 0.6383452124471162,
-            "timestamp": 0.15822824385941128
-        },
-        {
-            "x": 4.532822684228442,
-            "y": 3.151547007950623,
-            "heading": 2.744086644451416,
-            "angularVelocity": 1.0438585849437199,
-            "velocityX": 1.7065061459278321,
-            "velocityY": 0.9581878312306107,
-            "timestamp": 0.23734236578911694
-        },
-        {
-            "x": 4.713127943842242,
-            "y": 3.2527364142046724,
-            "heading": 2.845383620284061,
-            "angularVelocity": 1.2803905720226456,
-            "velocityX": 2.2790527811710266,
-            "velocityY": 1.2790308959500052,
-            "timestamp": 0.31645648771882257
-        },
-        {
-            "x": 4.938980637634121,
-            "y": 3.3793948659075563,
-            "heading": 2.944365706531666,
-            "angularVelocity": 1.251130440852921,
-            "velocityX": 2.854770909200696,
-            "velocityY": 1.6009588252198856,
-            "timestamp": 0.3955706096485282
-        },
-        {
-            "x": 5.199953974250007,
-            "y": 3.5259002853585115,
-            "heading": 2.944365731476778,
-            "angularVelocity": 3.1530542976951733e-7,
-            "velocityX": 3.2986947241576456,
-            "velocityY": 1.8518238700939902,
-            "timestamp": 0.4746847315782338
-        },
-        {
-            "x": 5.460927260915445,
-            "y": 3.6724057937882604,
-            "heading": 2.944365756420666,
-            "angularVelocity": 3.152899521547411e-7,
-            "velocityX": 3.2986940927855755,
-            "velocityY": 1.8518249947831291,
-            "timestamp": 0.5537988535079394
-        },
-        {
-            "x": 5.721900547580569,
-            "y": 3.818911302218567,
-            "heading": 2.9443657813645534,
-            "angularVelocity": 3.1528995198652365e-7,
-            "velocityX": 3.2986940927816133,
-            "velocityY": 1.851824994790188,
-            "timestamp": 0.6329129754376451
-        },
-        {
-            "x": 5.982873834247296,
-            "y": 3.965416810646019,
-            "heading": 2.9443658063084412,
-            "angularVelocity": 3.1528995220649985e-7,
-            "velocityX": 3.2986940928018726,
-            "velocityY": 1.851824994754099,
-            "timestamp": 0.7120270973673508
-        },
-        {
-            "x": 6.243847376304821,
-            "y": 4.1119218641396325,
-            "heading": 2.944365831252335,
-            "angularVelocity": 3.152900317708399e-7,
-            "velocityX": 3.298697320933481,
-            "velocityY": 1.8518192444047645,
-            "timestamp": 0.7911412192970565
-        },
-        {
-            "x": 6.520761156875476,
-            "y": 4.225454581965132,
-            "heading": 2.9443658572464595,
-            "angularVelocity": 3.2856491270029264e-7,
-            "velocityX": 3.5001814317891196,
-            "velocityY": 1.4350499639795518,
-            "timestamp": 0.8702553412267622
-        },
-        {
-            "x": 6.81318632649156,
-            "y": 4.28915727346349,
-            "heading": 2.9443676715667886,
-            "angularVelocity": 0.000022932951594388774,
-            "velocityX": 3.696244898931049,
-            "velocityY": 0.8052000065798415,
-            "timestamp": 0.9493694631564679
-        },
-        {
-            "x": 7.072106261164272,
-            "y": 4.3159902610395875,
-            "heading": 2.999666770720341,
-            "angularVelocity": 0.6989788650209022,
-            "velocityX": 3.2727397885142184,
-            "velocityY": 0.3391681146374708,
-            "timestamp": 1.0284835850861735
-        },
-        {
-            "x": 7.2892635874980165,
-            "y": 4.311538961400137,
-            "heading": 3.058239945124217,
-            "angularVelocity": 0.7403630726751829,
-            "velocityX": 2.7448617394337207,
-            "velocityY": -0.05626428671490304,
-            "timestamp": 1.1075977070158791
-        },
-        {
-            "x": 7.463265555506274,
-            "y": 4.277921305765472,
-            "heading": 3.1051985992516418,
-            "angularVelocity": 0.593555903573676,
-            "velocityX": 2.1993793745554173,
-            "velocityY": -0.4249261044006207,
-            "timestamp": 1.1867118289455847
-        },
-        {
-            "x": 7.593597601326297,
-            "y": 4.21619741378488,
-            "heading": 3.1337465954664303,
-            "angularVelocity": 0.3608457696105646,
-            "velocityX": 1.6473929387198967,
-            "velocityY": -0.7801880432350068,
-            "timestamp": 1.2658259508752903
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.07904283560305705,
-            "velocityX": 1.0921235876253772,
-            "velocityY": -1.1274524902663325,
-            "timestamp": 1.3449400728049958
-        },
-        {
-            "x": 7.722328891064707,
-            "y": 4.011878570496831,
-            "heading": 3.1218227549328446,
-            "angularVelocity": -0.23159012596074638,
-            "velocityX": 0.539298071695534,
-            "velocityY": -1.4667231618939216,
-            "timestamp": 1.423428933207607
-        },
-        {
-            "x": 7.720975293339473,
-            "y": 3.870431716581803,
-            "heading": 3.0818439654671956,
-            "angularVelocity": -0.5093562227885172,
-            "velocityX": -0.01724573039142055,
-            "velocityY": -1.8021264825284895,
-            "timestamp": 1.5019177936102184
-        },
-        {
-            "x": 7.675510236414314,
-            "y": 3.703145056798568,
-            "heading": 3.02401855987363,
-            "angularVelocity": -0.7367339173603616,
-            "velocityX": -0.5792548992550559,
-            "velocityY": -2.131342701692084,
-            "timestamp": 1.5804066540128296
-        },
-        {
-            "x": 7.585250814368908,
-            "y": 3.51090702633694,
-            "heading": 2.955217915344013,
-            "angularVelocity": -0.8765657212590741,
-            "velocityX": -1.149964741269228,
-            "velocityY": -2.4492396688591995,
-            "timestamp": 1.658895514415441
-        },
-        {
-            "x": 7.449012873441734,
-            "y": 3.2957924623075137,
-            "heading": 2.8903514658300926,
-            "angularVelocity": -0.8264414743848527,
-            "velocityX": -1.7357614854940644,
-            "velocityY": -2.7407018387830946,
-            "timestamp": 1.7373843748180522
-        },
-        {
-            "x": 7.265754194056636,
-            "y": 3.066909845502957,
-            "heading": 2.8806156888219427,
-            "angularVelocity": -0.12404023906336885,
-            "velocityX": -2.334836796522032,
-            "velocityY": -2.916115938370047,
-            "timestamp": 1.8158732352206635
-        },
-        {
-            "x": 7.0429291564371885,
-            "y": 2.8706709413875,
-            "heading": 2.8806156606364275,
-            "angularVelocity": -3.5910210875012317e-7,
-            "velocityX": -2.838938372610575,
-            "velocityY": -2.5002134456895257,
-            "timestamp": 1.8943620956232747
-        },
-        {
-            "x": 6.796142716598087,
-            "y": 2.7055735191439014,
-            "heading": 2.8806156351575583,
-            "angularVelocity": -3.246176492669905e-7,
-            "velocityX": -3.1442224867732946,
-            "velocityY": -2.103450367309771,
-            "timestamp": 1.972850956025886
-        },
-        {
-            "x": 6.549355877169833,
-            "y": 2.5404766942048504,
-            "heading": 2.880615609678698,
-            "angularVelocity": -3.246175317472835e-7,
-            "velocityX": -3.1442275778034396,
-            "velocityY": -2.10344275725474,
-            "timestamp": 2.0513398164284973
-        },
-        {
-            "x": 6.302569037739046,
-            "y": 2.375379869269585,
-            "heading": 2.8806155841998384,
-            "angularVelocity": -3.24617530454395e-7,
-            "velocityX": -3.144227577835705,
-            "velocityY": -2.103442757206509,
-            "timestamp": 2.1298286768311088
-        },
-        {
-            "x": 6.055782203816465,
-            "y": 2.2102830361006496,
-            "heading": 2.8806155587209785,
-            "angularVelocity": -3.246175312544412e-7,
-            "velocityX": -3.1442275076575137,
-            "velocityY": -2.103442862108911,
-            "timestamp": 2.2083175372337203
-        },
-        {
-            "x": 5.809854810532114,
-            "y": 2.043908693921413,
-            "heading": 2.8806155277577266,
-            "angularVelocity": -3.944923106855428e-7,
-            "velocityX": -3.1332776654274115,
-            "velocityY": -2.1197191719412425,
-            "timestamp": 2.2868063976363318
-        },
-        {
-            "x": 5.598037560183058,
-            "y": 1.9023806628591073,
-            "heading": 2.776131644051983,
-            "angularVelocity": -1.3311937919570889,
-            "velocityX": -2.6986918814024308,
-            "velocityY": -1.8031607330815247,
-            "timestamp": 2.3652952580389432
-        },
-        {
-            "x": 5.428977486755044,
-            "y": 1.7893186281455802,
-            "heading": 2.67268813100655,
-            "angularVelocity": -1.3179387815648507,
-            "velocityX": -2.1539371645965275,
-            "velocityY": -1.4404851100343643,
-            "timestamp": 2.4437841184415547
-        },
-        {
-            "x": 5.302396782822421,
-            "y": 1.7046186401543708,
-            "heading": 2.5887102200267456,
-            "angularVelocity": -1.0699341352267786,
-            "velocityX": -1.6127218981562843,
-            "velocityY": -1.0791338739884617,
-            "timestamp": 2.5222729788441662
-        },
-        {
-            "x": 5.218108357184172,
-            "y": 1.6481959100495345,
-            "heading": 2.5301489625141826,
-            "angularVelocity": -0.7461091575565081,
-            "velocityX": -1.0738902973732658,
-            "velocityY": -0.7188629037982504,
-            "timestamp": 2.6007618392467777
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.3841177252355578,
-            "velocityX": -0.5364883241797085,
-            "velocityY": -0.35923454493927354,
-            "timestamp": 2.6792506996493892
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": -9.31070457943821e-33,
-            "velocityY": 1.510534612718486e-32,
-            "timestamp": 2.7577395600520007
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S3.1.traj
deleted file mode 100644
index 219e0c08..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S3.1.traj	
+++ /dev/null
@@ -1,319 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -2.4689012164756437e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 4.313159042990802,
-            "y": 2.9920825634147805,
-            "heading": 2.597885779471472,
-            "angularVelocity": 0.30476728851208423,
-            "velocityX": 0.6399973484188943,
-            "velocityY": -0.10102143339823635,
-            "timestamp": 0.07837382938338545
-        },
-        {
-            "x": 4.4134829053058295,
-            "y": 2.976248991975609,
-            "heading": 2.6455288467924127,
-            "angularVelocity": 0.6078951060041105,
-            "velocityX": 1.2800683991625412,
-            "velocityY": -0.20202625753700879,
-            "timestamp": 0.1567476587667709
-        },
-        {
-            "x": 4.563981380382106,
-            "y": 2.952502286743102,
-            "heading": 2.7167039498074406,
-            "angularVelocity": 0.9081488498779698,
-            "velocityX": 1.9202644079068125,
-            "velocityY": -0.3029927900593087,
-            "timestamp": 0.23512148815015638
-        },
-        {
-            "x": 4.764676150331059,
-            "y": 2.9208479584714695,
-            "heading": 2.8108575648883316,
-            "angularVelocity": 1.2013399858304654,
-            "velocityX": 2.560737066543005,
-            "velocityY": -0.40388900887800433,
-            "timestamp": 0.3134953175335418
-        },
-        {
-            "x": 5.015633164055166,
-            "y": 2.881293970992892,
-            "heading": 2.92608193331875,
-            "angularVelocity": 1.47018933918326,
-            "velocityX": 3.2020511910485845,
-            "velocityY": -0.5046836142851278,
-            "timestamp": 0.39186914691692726
-        },
-        {
-            "x": 5.308481077006102,
-            "y": 2.835005929387577,
-            "heading": 2.926081954236969,
-            "angularVelocity": 2.6690310524294676e-7,
-            "velocityX": 3.7365523064899215,
-            "velocityY": -0.5906058434235878,
-            "timestamp": 0.4702429763003127
-        },
-        {
-            "x": 5.601350966367862,
-            "y": 2.7888571291196405,
-            "heading": 2.92608197144068,
-            "angularVelocity": 2.1950836119943446e-7,
-            "velocityX": 3.7368327114541455,
-            "velocityY": -0.5888292129019572,
-            "timestamp": 0.5486168056836982
-        },
-        {
-            "x": 5.8942208558716445,
-            "y": 2.7427083297530035,
-            "heading": 2.9260819886443907,
-            "angularVelocity": 2.1950836114981294e-7,
-            "velocityX": 3.736832713266251,
-            "velocityY": -0.5888292014019535,
-            "timestamp": 0.6269906350670836
-        },
-        {
-            "x": 6.187090745375428,
-            "y": 2.6965595303863723,
-            "heading": 2.9260820058481016,
-            "angularVelocity": 2.195083613443253e-7,
-            "velocityX": 3.736832713266263,
-            "velocityY": -0.5888292014018791,
-            "timestamp": 0.7053644644504691
-        },
-        {
-            "x": 6.479960634879199,
-            "y": 2.6504107310196727,
-            "heading": 2.9260820230518125,
-            "angularVelocity": 2.1950836077924463e-7,
-            "velocityX": 3.736832713266126,
-            "velocityY": -0.5888292014027512,
-            "timestamp": 0.7837382938338545
-        },
-        {
-            "x": 6.772830522718861,
-            "y": 2.604261921092308,
-            "heading": 2.926082040255567,
-            "angularVelocity": 2.1950891675310117e-7,
-            "velocityX": 3.736832692033143,
-            "velocityY": -0.5888293361500981,
-            "timestamp": 0.86211212321724
-        },
-        {
-            "x": 7.050151670300153,
-            "y": 2.559252271073386,
-            "heading": 2.978396963955354,
-            "angularVelocity": 0.6675050091514937,
-            "velocityX": 3.5384406984211854,
-            "velocityY": -0.5742943833809976,
-            "timestamp": 0.9404859526006254
-        },
-        {
-            "x": 7.276980321576378,
-            "y": 2.5226261995289687,
-            "heading": 3.0372586376427035,
-            "angularVelocity": 0.7510373571182644,
-            "velocityX": 2.894188698712594,
-            "velocityY": -0.4673252772333,
-            "timestamp": 1.0188597819840108
-        },
-        {
-            "x": 7.453315564542908,
-            "y": 2.49415241035015,
-            "heading": 3.0866031396716647,
-            "angularVelocity": 0.6296043260507225,
-            "velocityX": 2.2499250624075064,
-            "velocityY": -0.3633073616900632,
-            "timestamp": 1.0972336113673964
-        },
-        {
-            "x": 7.579233655925443,
-            "y": 2.473748793081471,
-            "heading": 3.1215813110163633,
-            "angularVelocity": 0.446299123315424,
-            "velocityX": 1.6066344132117514,
-            "velocityY": -0.2603371231087117,
-            "timestamp": 1.175607440750782
-        },
-        {
-            "x": 7.654784082070509,
-            "y": 2.4613731780152506,
-            "heading": 3.1398442445430734,
-            "angularVelocity": 0.2330233659676447,
-            "velocityX": 0.9639751781872319,
-            "velocityY": -0.1579049430605922,
-            "timestamp": 1.2539812701341675
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 0.0019873401380948097,
-            "velocityX": 0.32173900558233287,
-            "velocityY": -0.05579895801519462,
-            "timestamp": 1.332355099517553
-        },
-        {
-            "x": 7.650333485517468,
-            "y": 2.4613165476638152,
-            "heading": 3.1188382258181035,
-            "angularVelocity": -0.255122622507773,
-            "velocityX": -0.3576542737105397,
-            "velocityY": 0.05203953840108144,
-            "timestamp": 1.4153025620901392
-        },
-        {
-            "x": 7.564253765684918,
-            "y": 2.4745901696035957,
-            "heading": 3.07855569322522,
-            "angularVelocity": -0.4856391183470325,
-            "velocityX": -1.0377619418703934,
-            "velocityY": 0.16002444834484145,
-            "timestamp": 1.4982500246627253
-        },
-        {
-            "x": 7.42168176124644,
-            "y": 2.4968358287790196,
-            "heading": 3.0225573774767818,
-            "angularVelocity": -0.6751058321938196,
-            "velocityX": -1.7188229756119993,
-            "velocityY": 0.2681897490952097,
-            "timestamp": 1.5811974872353114
-        },
-        {
-            "x": 7.222516189064582,
-            "y": 2.528071510677467,
-            "heading": 2.9567671784015253,
-            "angularVelocity": -0.7931550530274372,
-            "velocityX": -2.401105061020643,
-            "velocityY": 0.37657187971380685,
-            "timestamp": 1.6641449498078975
-        },
-        {
-            "x": 6.966699685219415,
-            "y": 2.5683095941313905,
-            "heading": 2.8941605042172336,
-            "angularVelocity": -0.7547750376269705,
-            "velocityX": -3.0840787157449774,
-            "velocityY": 0.48510324735627736,
-            "timestamp": 1.7470924123804836
-        },
-        {
-            "x": 6.656762628445554,
-            "y": 2.6172997543511465,
-            "heading": 2.894159452148734,
-            "angularVelocity": -0.000012683552539851008,
-            "velocityX": -3.736546570097186,
-            "velocityY": 0.5906167434222167,
-            "timestamp": 1.8300398749530697
-        },
-        {
-            "x": 6.3468583527209415,
-            "y": 2.666498972584932,
-            "heading": 2.8941594322307855,
-            "angularVelocity": -2.401272821235818e-7,
-            "velocityX": -3.736151367540867,
-            "velocityY": 0.5931371100198551,
-            "timestamp": 1.9129873375256559
-        },
-        {
-            "x": 6.036954077181096,
-            "y": 2.715698191982551,
-            "heading": 2.894159412312837,
-            "angularVelocity": -2.4012728242851975e-7,
-            "velocityX": -3.736151365313363,
-            "velocityY": 0.5931371240508304,
-            "timestamp": 1.995934800098242
-        },
-        {
-            "x": 5.72704980164125,
-            "y": 2.7648974113801765,
-            "heading": 2.8941593923948883,
-            "angularVelocity": -2.401272814815578e-7,
-            "velocityX": -3.7361513653133502,
-            "velocityY": 0.5931371240509082,
-            "timestamp": 2.078882262670828
-        },
-        {
-            "x": 5.417145526101417,
-            "y": 2.8140966307778834,
-            "heading": 2.8941593724769397,
-            "angularVelocity": -2.4012728169619825e-7,
-            "velocityX": -3.7361513653131957,
-            "velocityY": 0.5931371240518842,
-            "timestamp": 2.1618297252434138
-        },
-        {
-            "x": 5.10724125286934,
-            "y": 2.8632958647117834,
-            "heading": 2.894159352558893,
-            "angularVelocity": -2.401284654616401e-7,
-            "velocityX": -3.7361513374913162,
-            "velocityY": 0.59313729929767,
-            "timestamp": 2.2447771878159997
-        },
-        {
-            "x": 4.825498631830447,
-            "y": 2.909240953390356,
-            "heading": 2.811052225227297,
-            "angularVelocity": -1.0019248902143632,
-            "velocityX": -3.3966394185035327,
-            "velocityY": 0.5539058972221842,
-            "timestamp": 2.3277246503885856
-        },
-        {
-            "x": 4.60031373082884,
-            "y": 2.9456643222027448,
-            "heading": 2.723257007302943,
-            "angularVelocity": -1.0584436847304166,
-            "velocityX": -2.7147895067260346,
-            "velocityY": 0.43911371948857386,
-            "timestamp": 2.4106721129611715
-        },
-        {
-            "x": 4.431586205140753,
-            "y": 2.9728717926897983,
-            "heading": 2.6509541339197864,
-            "angularVelocity": -0.871670707466155,
-            "velocityX": -2.0341493332654417,
-            "velocityY": 0.3280084723899037,
-            "timestamp": 2.4936195755337573
-        },
-        {
-            "x": 4.3191766366235065,
-            "y": 2.9909670138264888,
-            "heading": 2.600230923186435,
-            "angularVelocity": -0.6115100951879456,
-            "velocityX": -1.3551899603785846,
-            "velocityY": 0.21815279907876073,
-            "timestamp": 2.5765670381063432
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -0.31623539012403434,
-            "velocityX": -0.6772556372577037,
-            "velocityY": 0.10890009041091805,
-            "timestamp": 2.659514500678929
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": -4.6317137057442136e-32,
-            "velocityY": 0,
-            "timestamp": 2.742461963251515
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S3.traj
deleted file mode 100644
index 219e0c08..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S3.traj	
+++ /dev/null
@@ -1,319 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -2.4689012164756437e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 4.313159042990802,
-            "y": 2.9920825634147805,
-            "heading": 2.597885779471472,
-            "angularVelocity": 0.30476728851208423,
-            "velocityX": 0.6399973484188943,
-            "velocityY": -0.10102143339823635,
-            "timestamp": 0.07837382938338545
-        },
-        {
-            "x": 4.4134829053058295,
-            "y": 2.976248991975609,
-            "heading": 2.6455288467924127,
-            "angularVelocity": 0.6078951060041105,
-            "velocityX": 1.2800683991625412,
-            "velocityY": -0.20202625753700879,
-            "timestamp": 0.1567476587667709
-        },
-        {
-            "x": 4.563981380382106,
-            "y": 2.952502286743102,
-            "heading": 2.7167039498074406,
-            "angularVelocity": 0.9081488498779698,
-            "velocityX": 1.9202644079068125,
-            "velocityY": -0.3029927900593087,
-            "timestamp": 0.23512148815015638
-        },
-        {
-            "x": 4.764676150331059,
-            "y": 2.9208479584714695,
-            "heading": 2.8108575648883316,
-            "angularVelocity": 1.2013399858304654,
-            "velocityX": 2.560737066543005,
-            "velocityY": -0.40388900887800433,
-            "timestamp": 0.3134953175335418
-        },
-        {
-            "x": 5.015633164055166,
-            "y": 2.881293970992892,
-            "heading": 2.92608193331875,
-            "angularVelocity": 1.47018933918326,
-            "velocityX": 3.2020511910485845,
-            "velocityY": -0.5046836142851278,
-            "timestamp": 0.39186914691692726
-        },
-        {
-            "x": 5.308481077006102,
-            "y": 2.835005929387577,
-            "heading": 2.926081954236969,
-            "angularVelocity": 2.6690310524294676e-7,
-            "velocityX": 3.7365523064899215,
-            "velocityY": -0.5906058434235878,
-            "timestamp": 0.4702429763003127
-        },
-        {
-            "x": 5.601350966367862,
-            "y": 2.7888571291196405,
-            "heading": 2.92608197144068,
-            "angularVelocity": 2.1950836119943446e-7,
-            "velocityX": 3.7368327114541455,
-            "velocityY": -0.5888292129019572,
-            "timestamp": 0.5486168056836982
-        },
-        {
-            "x": 5.8942208558716445,
-            "y": 2.7427083297530035,
-            "heading": 2.9260819886443907,
-            "angularVelocity": 2.1950836114981294e-7,
-            "velocityX": 3.736832713266251,
-            "velocityY": -0.5888292014019535,
-            "timestamp": 0.6269906350670836
-        },
-        {
-            "x": 6.187090745375428,
-            "y": 2.6965595303863723,
-            "heading": 2.9260820058481016,
-            "angularVelocity": 2.195083613443253e-7,
-            "velocityX": 3.736832713266263,
-            "velocityY": -0.5888292014018791,
-            "timestamp": 0.7053644644504691
-        },
-        {
-            "x": 6.479960634879199,
-            "y": 2.6504107310196727,
-            "heading": 2.9260820230518125,
-            "angularVelocity": 2.1950836077924463e-7,
-            "velocityX": 3.736832713266126,
-            "velocityY": -0.5888292014027512,
-            "timestamp": 0.7837382938338545
-        },
-        {
-            "x": 6.772830522718861,
-            "y": 2.604261921092308,
-            "heading": 2.926082040255567,
-            "angularVelocity": 2.1950891675310117e-7,
-            "velocityX": 3.736832692033143,
-            "velocityY": -0.5888293361500981,
-            "timestamp": 0.86211212321724
-        },
-        {
-            "x": 7.050151670300153,
-            "y": 2.559252271073386,
-            "heading": 2.978396963955354,
-            "angularVelocity": 0.6675050091514937,
-            "velocityX": 3.5384406984211854,
-            "velocityY": -0.5742943833809976,
-            "timestamp": 0.9404859526006254
-        },
-        {
-            "x": 7.276980321576378,
-            "y": 2.5226261995289687,
-            "heading": 3.0372586376427035,
-            "angularVelocity": 0.7510373571182644,
-            "velocityX": 2.894188698712594,
-            "velocityY": -0.4673252772333,
-            "timestamp": 1.0188597819840108
-        },
-        {
-            "x": 7.453315564542908,
-            "y": 2.49415241035015,
-            "heading": 3.0866031396716647,
-            "angularVelocity": 0.6296043260507225,
-            "velocityX": 2.2499250624075064,
-            "velocityY": -0.3633073616900632,
-            "timestamp": 1.0972336113673964
-        },
-        {
-            "x": 7.579233655925443,
-            "y": 2.473748793081471,
-            "heading": 3.1215813110163633,
-            "angularVelocity": 0.446299123315424,
-            "velocityX": 1.6066344132117514,
-            "velocityY": -0.2603371231087117,
-            "timestamp": 1.175607440750782
-        },
-        {
-            "x": 7.654784082070509,
-            "y": 2.4613731780152506,
-            "heading": 3.1398442445430734,
-            "angularVelocity": 0.2330233659676447,
-            "velocityX": 0.9639751781872319,
-            "velocityY": -0.1579049430605922,
-            "timestamp": 1.2539812701341675
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 0.0019873401380948097,
-            "velocityX": 0.32173900558233287,
-            "velocityY": -0.05579895801519462,
-            "timestamp": 1.332355099517553
-        },
-        {
-            "x": 7.650333485517468,
-            "y": 2.4613165476638152,
-            "heading": 3.1188382258181035,
-            "angularVelocity": -0.255122622507773,
-            "velocityX": -0.3576542737105397,
-            "velocityY": 0.05203953840108144,
-            "timestamp": 1.4153025620901392
-        },
-        {
-            "x": 7.564253765684918,
-            "y": 2.4745901696035957,
-            "heading": 3.07855569322522,
-            "angularVelocity": -0.4856391183470325,
-            "velocityX": -1.0377619418703934,
-            "velocityY": 0.16002444834484145,
-            "timestamp": 1.4982500246627253
-        },
-        {
-            "x": 7.42168176124644,
-            "y": 2.4968358287790196,
-            "heading": 3.0225573774767818,
-            "angularVelocity": -0.6751058321938196,
-            "velocityX": -1.7188229756119993,
-            "velocityY": 0.2681897490952097,
-            "timestamp": 1.5811974872353114
-        },
-        {
-            "x": 7.222516189064582,
-            "y": 2.528071510677467,
-            "heading": 2.9567671784015253,
-            "angularVelocity": -0.7931550530274372,
-            "velocityX": -2.401105061020643,
-            "velocityY": 0.37657187971380685,
-            "timestamp": 1.6641449498078975
-        },
-        {
-            "x": 6.966699685219415,
-            "y": 2.5683095941313905,
-            "heading": 2.8941605042172336,
-            "angularVelocity": -0.7547750376269705,
-            "velocityX": -3.0840787157449774,
-            "velocityY": 0.48510324735627736,
-            "timestamp": 1.7470924123804836
-        },
-        {
-            "x": 6.656762628445554,
-            "y": 2.6172997543511465,
-            "heading": 2.894159452148734,
-            "angularVelocity": -0.000012683552539851008,
-            "velocityX": -3.736546570097186,
-            "velocityY": 0.5906167434222167,
-            "timestamp": 1.8300398749530697
-        },
-        {
-            "x": 6.3468583527209415,
-            "y": 2.666498972584932,
-            "heading": 2.8941594322307855,
-            "angularVelocity": -2.401272821235818e-7,
-            "velocityX": -3.736151367540867,
-            "velocityY": 0.5931371100198551,
-            "timestamp": 1.9129873375256559
-        },
-        {
-            "x": 6.036954077181096,
-            "y": 2.715698191982551,
-            "heading": 2.894159412312837,
-            "angularVelocity": -2.4012728242851975e-7,
-            "velocityX": -3.736151365313363,
-            "velocityY": 0.5931371240508304,
-            "timestamp": 1.995934800098242
-        },
-        {
-            "x": 5.72704980164125,
-            "y": 2.7648974113801765,
-            "heading": 2.8941593923948883,
-            "angularVelocity": -2.401272814815578e-7,
-            "velocityX": -3.7361513653133502,
-            "velocityY": 0.5931371240509082,
-            "timestamp": 2.078882262670828
-        },
-        {
-            "x": 5.417145526101417,
-            "y": 2.8140966307778834,
-            "heading": 2.8941593724769397,
-            "angularVelocity": -2.4012728169619825e-7,
-            "velocityX": -3.7361513653131957,
-            "velocityY": 0.5931371240518842,
-            "timestamp": 2.1618297252434138
-        },
-        {
-            "x": 5.10724125286934,
-            "y": 2.8632958647117834,
-            "heading": 2.894159352558893,
-            "angularVelocity": -2.401284654616401e-7,
-            "velocityX": -3.7361513374913162,
-            "velocityY": 0.59313729929767,
-            "timestamp": 2.2447771878159997
-        },
-        {
-            "x": 4.825498631830447,
-            "y": 2.909240953390356,
-            "heading": 2.811052225227297,
-            "angularVelocity": -1.0019248902143632,
-            "velocityX": -3.3966394185035327,
-            "velocityY": 0.5539058972221842,
-            "timestamp": 2.3277246503885856
-        },
-        {
-            "x": 4.60031373082884,
-            "y": 2.9456643222027448,
-            "heading": 2.723257007302943,
-            "angularVelocity": -1.0584436847304166,
-            "velocityX": -2.7147895067260346,
-            "velocityY": 0.43911371948857386,
-            "timestamp": 2.4106721129611715
-        },
-        {
-            "x": 4.431586205140753,
-            "y": 2.9728717926897983,
-            "heading": 2.6509541339197864,
-            "angularVelocity": -0.871670707466155,
-            "velocityX": -2.0341493332654417,
-            "velocityY": 0.3280084723899037,
-            "timestamp": 2.4936195755337573
-        },
-        {
-            "x": 4.3191766366235065,
-            "y": 2.9909670138264888,
-            "heading": 2.600230923186435,
-            "angularVelocity": -0.6115100951879456,
-            "velocityX": -1.3551899603785846,
-            "velocityY": 0.21815279907876073,
-            "timestamp": 2.5765670381063432
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -0.31623539012403434,
-            "velocityX": -0.6772556372577037,
-            "velocityY": 0.10890009041091805,
-            "timestamp": 2.659514500678929
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": -4.6317137057442136e-32,
-            "velocityY": 0,
-            "timestamp": 2.742461963251515
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S4.1.traj
deleted file mode 100644
index 1cf75a8d..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S4.1.traj	
+++ /dev/null
@@ -1,301 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.312461905031287,
-            "y": 3.0009727246573483,
-            "heading": 2.5993071037453466,
-            "angularVelocity": 0.3268807464808282,
-            "velocityX": 0.6388777080808309,
-            "velocityY": 0.012564257266019683,
-            "timestamp": 0.07741998884241831
-        },
-        {
-            "x": 4.411454270150545,
-            "y": 3.0029274539385553,
-            "heading": 2.648169844004936,
-            "angularVelocity": 0.6311385598239377,
-            "velocityX": 1.2786409117256237,
-            "velocityY": 0.025248379784522566,
-            "timestamp": 0.15483997768483662
-        },
-        {
-            "x": 4.560077070415931,
-            "y": 3.0058795423873246,
-            "heading": 2.717741930312934,
-            "angularVelocity": 0.8986320890539764,
-            "velocityX": 1.9196954492966272,
-            "velocityY": 0.03813083020172101,
-            "timestamp": 0.23225996652725495
-        },
-        {
-            "x": 4.758482787325499,
-            "y": 3.0098532742147075,
-            "heading": 2.8026323249857676,
-            "angularVelocity": 1.0964919517828993,
-            "velocityX": 2.562719523421868,
-            "velocityY": 0.05132694910962975,
-            "timestamp": 0.30967995536967324
-        },
-        {
-            "x": 5.006851286997014,
-            "y": 3.0148884552327933,
-            "heading": 2.889629605764704,
-            "angularVelocity": 1.1237056744610465,
-            "velocityX": 3.2080668492092808,
-            "velocityY": 0.06503722221317501,
-            "timestamp": 0.38709994421209154
-        },
-        {
-            "x": 5.299689191212239,
-            "y": 3.01955932024386,
-            "heading": 2.889629778554125,
-            "angularVelocity": 0.000002231845078236991,
-            "velocityX": 3.7824586207480784,
-            "velocityY": 0.060331512325246216,
-            "timestamp": 0.46451993305450984
-        },
-        {
-            "x": 5.592507772821644,
-            "y": 3.025318229635612,
-            "heading": 2.8896298031470753,
-            "angularVelocity": 3.176563362313136e-7,
-            "velocityX": 3.782209039133448,
-            "velocityY": 0.07438530381958017,
-            "timestamp": 0.5419399218969282
-        },
-        {
-            "x": 5.8853263542850165,
-            "y": 3.0310771464526267,
-            "heading": 2.889629827740025,
-            "angularVelocity": 3.176563361839336e-7,
-            "velocityX": 3.782209037247192,
-            "velocityY": 0.07438539972844682,
-            "timestamp": 0.6193599107393465
-        },
-        {
-            "x": 6.178144935748446,
-            "y": 3.0368360632666453,
-            "heading": 2.8896298523329755,
-            "angularVelocity": 3.1765633664876355e-7,
-            "velocityX": 3.782209037247953,
-            "velocityY": 0.07438539968973952,
-            "timestamp": 0.6967798995817648
-        },
-        {
-            "x": 6.4709635259990606,
-            "y": 3.0425945332689603,
-            "heading": 2.889629876925932,
-            "angularVelocity": 3.176564212988765e-7,
-            "velocityX": 3.7822091507481463,
-            "velocityY": 0.07437962841916478,
-            "timestamp": 0.7741998884241831
-        },
-        {
-            "x": 6.762585697804425,
-            "y": 3.0155317439126783,
-            "heading": 2.889629906398516,
-            "angularVelocity": 3.80684423008717e-7,
-            "velocityX": 3.766755539049949,
-            "velocityY": -0.3495581665784765,
-            "timestamp": 0.8516198772666014
-        },
-        {
-            "x": 7.037113234733603,
-            "y": 2.9421863256805914,
-            "heading": 2.9183632792459075,
-            "angularVelocity": 0.37113635996351935,
-            "velocityX": 3.5459516467763192,
-            "velocityY": -0.9473705606103254,
-            "timestamp": 0.9290398661090197
-        },
-        {
-            "x": 7.2647684874925345,
-            "y": 2.8553520162741264,
-            "heading": 2.9862391901434,
-            "angularVelocity": 0.8767233360837648,
-            "velocityX": 2.9405229342296155,
-            "velocityY": -1.1216006448051647,
-            "timestamp": 1.006459854951438
-        },
-        {
-            "x": 7.442903065756583,
-            "y": 2.761816317140275,
-            "heading": 3.0514695730897063,
-            "angularVelocity": 0.8425522132155971,
-            "velocityX": 2.3008861267937584,
-            "velocityY": -1.208159553267764,
-            "timestamp": 1.0838798437938564
-        },
-        {
-            "x": 7.571438256526523,
-            "y": 2.6636606141768233,
-            "heading": 3.1022478511869496,
-            "angularVelocity": 0.6558807209414358,
-            "velocityX": 1.660232618110589,
-            "velocityY": -1.2678341140456517,
-            "timestamp": 1.1612998326362747
-        },
-        {
-            "x": 7.650445589096316,
-            "y": 2.561865725059965,
-            "heading": 3.1328810502986992,
-            "angularVelocity": 0.39567558158786426,
-            "velocityX": 1.0205030219082787,
-            "velocityY": -1.3148398835868218,
-            "timestamp": 1.238719821478693
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 0.09195234729096653,
-            "velocityX": 0.38174134801077375,
-            "velocityY": -1.354504523029707,
-            "timestamp": 1.3161398103211113
-        },
-        {
-            "x": 7.659642368469709,
-            "y": 2.34860980319894,
-            "heading": 3.1210552256706268,
-            "angularVelocity": -0.24288242827145334,
-            "velocityX": -0.26099603478867267,
-            "velocityY": -1.3896219475702174,
-            "timestamp": 1.3941395835620152
-        },
-        {
-            "x": 7.589031675721614,
-            "y": 2.237822483491401,
-            "heading": 3.078729120037355,
-            "angularVelocity": -0.5426439574708317,
-            "velocityX": -0.9052679234080746,
-            "velocityY": -1.4203543818694149,
-            "timestamp": 1.472139356802919
-        },
-        {
-            "x": 7.468012328059872,
-            "y": 2.1251640911848773,
-            "heading": 3.017134343801584,
-            "angularVelocity": -0.7896789141365163,
-            "velocityX": -1.5515346087990811,
-            "velocityY": -1.4443425618504895,
-            "timestamp": 1.550139130043823
-        },
-        {
-            "x": 7.29639145154735,
-            "y": 2.0115501518980783,
-            "heading": 2.9431614871389193,
-            "angularVelocity": -0.948372714292366,
-            "velocityX": -2.2002740441625033,
-            "velocityY": -1.4565931997763661,
-            "timestamp": 1.6281389032847269
-        },
-        {
-            "x": 7.0740549155451475,
-            "y": 1.8989556561262921,
-            "heading": 2.8711554567434443,
-            "angularVelocity": -0.9231569196115829,
-            "velocityX": -2.8504766970989883,
-            "velocityY": -1.44352337312617,
-            "timestamp": 1.7061386765256308
-        },
-        {
-            "x": 6.804126889371604,
-            "y": 1.794205881873725,
-            "heading": 2.8575112203208874,
-            "angularVelocity": -0.17492661652254402,
-            "velocityX": -3.460625780793808,
-            "velocityY": -1.3429497279312002,
-            "timestamp": 1.7841384497665347
-        },
-        {
-            "x": 6.514819933259115,
-            "y": 1.7361808711845228,
-            "heading": 2.8575111902020094,
-            "angularVelocity": -3.8614058154759053e-7,
-            "velocityX": -3.7090743228055154,
-            "velocityY": -0.7439125561300094,
-            "timestamp": 1.8621382230074386
-        },
-        {
-            "x": 6.220884578970619,
-            "y": 1.7103463006817032,
-            "heading": 2.8575111626758107,
-            "angularVelocity": -3.52901004822605e-7,
-            "velocityX": -3.768412933466744,
-            "velocityY": -0.3312134052368198,
-            "timestamp": 1.9401379962483425
-        },
-        {
-            "x": 5.926733464556045,
-            "y": 1.687097421621547,
-            "heading": 2.857511000804422,
-            "angularVelocity": -0.000002075280251147508,
-            "velocityX": -3.771179096970985,
-            "velocityY": -0.2980634185736858,
-            "timestamp": 2.0181377694892464
-        },
-        {
-            "x": 5.675971267912864,
-            "y": 1.6645564964621176,
-            "heading": 2.7589225755051734,
-            "angularVelocity": -1.2639578450408617,
-            "velocityX": -3.214909303244982,
-            "velocityY": -0.28898706012659137,
-            "timestamp": 2.0961375427301503
-        },
-        {
-            "x": 5.475720463425886,
-            "y": 1.6466626572874457,
-            "heading": 2.662658583499747,
-            "angularVelocity": -1.2341573315618837,
-            "velocityX": -2.5673254698895036,
-            "velocityY": -0.22940886147715536,
-            "timestamp": 2.174137315971054
-        },
-        {
-            "x": 5.325756154484835,
-            "y": 1.6333048659006117,
-            "heading": 2.583901126299631,
-            "angularVelocity": -1.009713925152988,
-            "velocityX": -1.9226249347915934,
-            "velocityY": -0.17125423359344924,
-            "timestamp": 2.252137089211958
-        },
-        {
-            "x": 5.225890415283439,
-            "y": 1.6244276229064765,
-            "heading": 2.5286254494455984,
-            "angularVelocity": -0.7086645839765799,
-            "velocityX": -1.2803337119065468,
-            "velocityY": -0.11381113848520645,
-            "timestamp": 2.330136862452862
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.3669940085234364,
-            "velocityX": -0.6396225682522365,
-            "velocityY": -0.05676456126098677,
-            "timestamp": 2.408136635693766
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -3.495963808080905e-32,
-            "velocityX": 2.431919697102105e-35,
-            "velocityY": 0,
-            "timestamp": 2.48613640893467
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S4.traj
deleted file mode 100644
index 1cf75a8d..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4 To S4.traj	
+++ /dev/null
@@ -1,301 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.312461905031287,
-            "y": 3.0009727246573483,
-            "heading": 2.5993071037453466,
-            "angularVelocity": 0.3268807464808282,
-            "velocityX": 0.6388777080808309,
-            "velocityY": 0.012564257266019683,
-            "timestamp": 0.07741998884241831
-        },
-        {
-            "x": 4.411454270150545,
-            "y": 3.0029274539385553,
-            "heading": 2.648169844004936,
-            "angularVelocity": 0.6311385598239377,
-            "velocityX": 1.2786409117256237,
-            "velocityY": 0.025248379784522566,
-            "timestamp": 0.15483997768483662
-        },
-        {
-            "x": 4.560077070415931,
-            "y": 3.0058795423873246,
-            "heading": 2.717741930312934,
-            "angularVelocity": 0.8986320890539764,
-            "velocityX": 1.9196954492966272,
-            "velocityY": 0.03813083020172101,
-            "timestamp": 0.23225996652725495
-        },
-        {
-            "x": 4.758482787325499,
-            "y": 3.0098532742147075,
-            "heading": 2.8026323249857676,
-            "angularVelocity": 1.0964919517828993,
-            "velocityX": 2.562719523421868,
-            "velocityY": 0.05132694910962975,
-            "timestamp": 0.30967995536967324
-        },
-        {
-            "x": 5.006851286997014,
-            "y": 3.0148884552327933,
-            "heading": 2.889629605764704,
-            "angularVelocity": 1.1237056744610465,
-            "velocityX": 3.2080668492092808,
-            "velocityY": 0.06503722221317501,
-            "timestamp": 0.38709994421209154
-        },
-        {
-            "x": 5.299689191212239,
-            "y": 3.01955932024386,
-            "heading": 2.889629778554125,
-            "angularVelocity": 0.000002231845078236991,
-            "velocityX": 3.7824586207480784,
-            "velocityY": 0.060331512325246216,
-            "timestamp": 0.46451993305450984
-        },
-        {
-            "x": 5.592507772821644,
-            "y": 3.025318229635612,
-            "heading": 2.8896298031470753,
-            "angularVelocity": 3.176563362313136e-7,
-            "velocityX": 3.782209039133448,
-            "velocityY": 0.07438530381958017,
-            "timestamp": 0.5419399218969282
-        },
-        {
-            "x": 5.8853263542850165,
-            "y": 3.0310771464526267,
-            "heading": 2.889629827740025,
-            "angularVelocity": 3.176563361839336e-7,
-            "velocityX": 3.782209037247192,
-            "velocityY": 0.07438539972844682,
-            "timestamp": 0.6193599107393465
-        },
-        {
-            "x": 6.178144935748446,
-            "y": 3.0368360632666453,
-            "heading": 2.8896298523329755,
-            "angularVelocity": 3.1765633664876355e-7,
-            "velocityX": 3.782209037247953,
-            "velocityY": 0.07438539968973952,
-            "timestamp": 0.6967798995817648
-        },
-        {
-            "x": 6.4709635259990606,
-            "y": 3.0425945332689603,
-            "heading": 2.889629876925932,
-            "angularVelocity": 3.176564212988765e-7,
-            "velocityX": 3.7822091507481463,
-            "velocityY": 0.07437962841916478,
-            "timestamp": 0.7741998884241831
-        },
-        {
-            "x": 6.762585697804425,
-            "y": 3.0155317439126783,
-            "heading": 2.889629906398516,
-            "angularVelocity": 3.80684423008717e-7,
-            "velocityX": 3.766755539049949,
-            "velocityY": -0.3495581665784765,
-            "timestamp": 0.8516198772666014
-        },
-        {
-            "x": 7.037113234733603,
-            "y": 2.9421863256805914,
-            "heading": 2.9183632792459075,
-            "angularVelocity": 0.37113635996351935,
-            "velocityX": 3.5459516467763192,
-            "velocityY": -0.9473705606103254,
-            "timestamp": 0.9290398661090197
-        },
-        {
-            "x": 7.2647684874925345,
-            "y": 2.8553520162741264,
-            "heading": 2.9862391901434,
-            "angularVelocity": 0.8767233360837648,
-            "velocityX": 2.9405229342296155,
-            "velocityY": -1.1216006448051647,
-            "timestamp": 1.006459854951438
-        },
-        {
-            "x": 7.442903065756583,
-            "y": 2.761816317140275,
-            "heading": 3.0514695730897063,
-            "angularVelocity": 0.8425522132155971,
-            "velocityX": 2.3008861267937584,
-            "velocityY": -1.208159553267764,
-            "timestamp": 1.0838798437938564
-        },
-        {
-            "x": 7.571438256526523,
-            "y": 2.6636606141768233,
-            "heading": 3.1022478511869496,
-            "angularVelocity": 0.6558807209414358,
-            "velocityX": 1.660232618110589,
-            "velocityY": -1.2678341140456517,
-            "timestamp": 1.1612998326362747
-        },
-        {
-            "x": 7.650445589096316,
-            "y": 2.561865725059965,
-            "heading": 3.1328810502986992,
-            "angularVelocity": 0.39567558158786426,
-            "velocityX": 1.0205030219082787,
-            "velocityY": -1.3148398835868218,
-            "timestamp": 1.238719821478693
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 0.09195234729096653,
-            "velocityX": 0.38174134801077375,
-            "velocityY": -1.354504523029707,
-            "timestamp": 1.3161398103211113
-        },
-        {
-            "x": 7.659642368469709,
-            "y": 2.34860980319894,
-            "heading": 3.1210552256706268,
-            "angularVelocity": -0.24288242827145334,
-            "velocityX": -0.26099603478867267,
-            "velocityY": -1.3896219475702174,
-            "timestamp": 1.3941395835620152
-        },
-        {
-            "x": 7.589031675721614,
-            "y": 2.237822483491401,
-            "heading": 3.078729120037355,
-            "angularVelocity": -0.5426439574708317,
-            "velocityX": -0.9052679234080746,
-            "velocityY": -1.4203543818694149,
-            "timestamp": 1.472139356802919
-        },
-        {
-            "x": 7.468012328059872,
-            "y": 2.1251640911848773,
-            "heading": 3.017134343801584,
-            "angularVelocity": -0.7896789141365163,
-            "velocityX": -1.5515346087990811,
-            "velocityY": -1.4443425618504895,
-            "timestamp": 1.550139130043823
-        },
-        {
-            "x": 7.29639145154735,
-            "y": 2.0115501518980783,
-            "heading": 2.9431614871389193,
-            "angularVelocity": -0.948372714292366,
-            "velocityX": -2.2002740441625033,
-            "velocityY": -1.4565931997763661,
-            "timestamp": 1.6281389032847269
-        },
-        {
-            "x": 7.0740549155451475,
-            "y": 1.8989556561262921,
-            "heading": 2.8711554567434443,
-            "angularVelocity": -0.9231569196115829,
-            "velocityX": -2.8504766970989883,
-            "velocityY": -1.44352337312617,
-            "timestamp": 1.7061386765256308
-        },
-        {
-            "x": 6.804126889371604,
-            "y": 1.794205881873725,
-            "heading": 2.8575112203208874,
-            "angularVelocity": -0.17492661652254402,
-            "velocityX": -3.460625780793808,
-            "velocityY": -1.3429497279312002,
-            "timestamp": 1.7841384497665347
-        },
-        {
-            "x": 6.514819933259115,
-            "y": 1.7361808711845228,
-            "heading": 2.8575111902020094,
-            "angularVelocity": -3.8614058154759053e-7,
-            "velocityX": -3.7090743228055154,
-            "velocityY": -0.7439125561300094,
-            "timestamp": 1.8621382230074386
-        },
-        {
-            "x": 6.220884578970619,
-            "y": 1.7103463006817032,
-            "heading": 2.8575111626758107,
-            "angularVelocity": -3.52901004822605e-7,
-            "velocityX": -3.768412933466744,
-            "velocityY": -0.3312134052368198,
-            "timestamp": 1.9401379962483425
-        },
-        {
-            "x": 5.926733464556045,
-            "y": 1.687097421621547,
-            "heading": 2.857511000804422,
-            "angularVelocity": -0.000002075280251147508,
-            "velocityX": -3.771179096970985,
-            "velocityY": -0.2980634185736858,
-            "timestamp": 2.0181377694892464
-        },
-        {
-            "x": 5.675971267912864,
-            "y": 1.6645564964621176,
-            "heading": 2.7589225755051734,
-            "angularVelocity": -1.2639578450408617,
-            "velocityX": -3.214909303244982,
-            "velocityY": -0.28898706012659137,
-            "timestamp": 2.0961375427301503
-        },
-        {
-            "x": 5.475720463425886,
-            "y": 1.6466626572874457,
-            "heading": 2.662658583499747,
-            "angularVelocity": -1.2341573315618837,
-            "velocityX": -2.5673254698895036,
-            "velocityY": -0.22940886147715536,
-            "timestamp": 2.174137315971054
-        },
-        {
-            "x": 5.325756154484835,
-            "y": 1.6333048659006117,
-            "heading": 2.583901126299631,
-            "angularVelocity": -1.009713925152988,
-            "velocityX": -1.9226249347915934,
-            "velocityY": -0.17125423359344924,
-            "timestamp": 2.252137089211958
-        },
-        {
-            "x": 5.225890415283439,
-            "y": 1.6244276229064765,
-            "heading": 2.5286254494455984,
-            "angularVelocity": -0.7086645839765799,
-            "velocityX": -1.2803337119065468,
-            "velocityY": -0.11381113848520645,
-            "timestamp": 2.330136862452862
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.3669940085234364,
-            "velocityX": -0.6396225682522365,
-            "velocityY": -0.05676456126098677,
-            "timestamp": 2.408136635693766
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -3.495963808080905e-32,
-            "velocityX": 2.431919697102105e-35,
-            "velocityY": 0,
-            "timestamp": 2.48613640893467
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S3.1.traj
deleted file mode 100644
index ba2da1d0..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S3.1.traj	
+++ /dev/null
@@ -1,355 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -4.85982063591045e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 4.306024668622343,
-            "y": 2.9721182741266654,
-            "heading": 2.598597610192154,
-            "angularVelocity": 0.31231028150986523,
-            "velocityX": 0.5462744658665957,
-            "velocityY": -0.3540079539621319,
-            "timestamp": 0.0787601678472939
-        },
-        {
-            "x": 4.392095194160992,
-            "y": 2.9163526825964556,
-            "heading": 2.647262656420547,
-            "angularVelocity": 0.6178890619271524,
-            "velocityX": 1.0928179546992538,
-            "velocityY": -0.7080430762709009,
-            "timestamp": 0.1575203356945878
-        },
-        {
-            "x": 4.521245672207201,
-            "y": 2.83269821600786,
-            "heading": 2.7190885011917048,
-            "angularVelocity": 0.911956471581102,
-            "velocityX": 1.6397943475262466,
-            "velocityY": -1.0621418018152553,
-            "timestamp": 0.23628050354188168
-        },
-        {
-            "x": 4.693538421890133,
-            "y": 2.721142508781873,
-            "heading": 2.812276483563172,
-            "angularVelocity": 1.1831866909189597,
-            "velocityX": 2.1875619922114358,
-            "velocityY": -1.4163975303135559,
-            "timestamp": 0.3150406713891756
-        },
-        {
-            "x": 4.909132383686271,
-            "y": 2.581660366752094,
-            "heading": 2.9215937215973726,
-            "angularVelocity": 1.387976194339083,
-            "velocityX": 2.7373476681023576,
-            "velocityY": -1.7709731434323392,
-            "timestamp": 0.39380083923646947
-        },
-        {
-            "x": 5.159074984583097,
-            "y": 2.4194878202605934,
-            "heading": 2.9215937398089027,
-            "angularVelocity": 2.3122767087752481e-7,
-            "velocityX": 3.1734645535727406,
-            "velocityY": -2.059068066055073,
-            "timestamp": 0.47256100708376336
-        },
-        {
-            "x": 5.409017280295355,
-            "y": 2.257314803413765,
-            "heading": 2.921593758018432,
-            "angularVelocity": 2.3120226821213244e-7,
-            "velocityX": 3.173460678713437,
-            "velocityY": -2.059074038050069,
-            "timestamp": 0.5513211749310573
-        },
-        {
-            "x": 5.658959576005671,
-            "y": 2.0951417865639432,
-            "heading": 2.9215937762279616,
-            "angularVelocity": 2.312022681051112e-7,
-            "velocityX": 3.1734606786887793,
-            "velocityY": -2.0590740380880725,
-            "timestamp": 0.6300813427783512
-        },
-        {
-            "x": 5.908901871715987,
-            "y": 1.9329687697141213,
-            "heading": 2.9215937944374906,
-            "angularVelocity": 2.3120226848439114e-7,
-            "velocityX": 3.173460678688779,
-            "velocityY": -2.0590740380880725,
-            "timestamp": 0.708841510625645
-        },
-        {
-            "x": 6.158844167426302,
-            "y": 1.7707957528642995,
-            "heading": 2.92159381264702,
-            "angularVelocity": 2.3120226852070691e-7,
-            "velocityX": 3.1734606786887785,
-            "velocityY": -2.059074038088073,
-            "timestamp": 0.7876016784729389
-        },
-        {
-            "x": 6.408786463136619,
-            "y": 1.6086227360144778,
-            "heading": 2.921593830856549,
-            "angularVelocity": 2.3120226809334475e-7,
-            "velocityX": 3.173460678688779,
-            "velocityY": -2.059074038088073,
-            "timestamp": 0.8663618463202328
-        },
-        {
-            "x": 6.658728758846954,
-            "y": 1.4464497191646866,
-            "heading": 2.9215938490660784,
-            "angularVelocity": 2.31202268371391e-7,
-            "velocityX": 3.173460678689032,
-            "velocityY": -2.0590740380876835,
-            "timestamp": 0.9451220141675267
-        },
-        {
-            "x": 6.908671057688978,
-            "y": 1.2842767071415118,
-            "heading": 2.9215938672756456,
-            "angularVelocity": 2.3120274459298332e-7,
-            "velocityX": 3.173460718451369,
-            "velocityY": -2.05907397680523,
-            "timestamp": 1.0238821820148205
-        },
-        {
-            "x": 7.1455069915896265,
-            "y": 1.1314725250685531,
-            "heading": 2.963985587877118,
-            "angularVelocity": 0.5382380683048028,
-            "velocityX": 3.007052173375748,
-            "velocityY": -1.9401200663922304,
-            "timestamp": 1.1026423498621143
-        },
-        {
-            "x": 7.339036455456064,
-            "y": 1.0065280886493264,
-            "heading": 3.0237371798128563,
-            "angularVelocity": 0.7586524199845985,
-            "velocityX": 2.457199738853539,
-            "velocityY": -1.5863911902965262,
-            "timestamp": 1.181402517709408
-        },
-        {
-            "x": 7.489157100180206,
-            "y": 0.9096422022009641,
-            "heading": 3.07668127081849,
-            "angularVelocity": 0.6722191236093652,
-            "velocityX": 1.9060478009036246,
-            "velocityY": -1.2301381408456074,
-            "timestamp": 1.2601626855567019
-        },
-        {
-            "x": 7.5959791241645505,
-            "y": 0.8407792473093463,
-            "heading": 3.115718535259042,
-            "angularVelocity": 0.4956472987238782,
-            "velocityX": 1.3562950270935392,
-            "velocityY": -0.8743373303257803,
-            "timestamp": 1.3389228534039956
-        },
-        {
-            "x": 7.659576823404001,
-            "y": 0.7999065653267177,
-            "heading": 3.1374811645909735,
-            "angularVelocity": 0.2763151720819586,
-            "velocityX": 0.8074855726914214,
-            "velocityY": -0.5189511792543529,
-            "timestamp": 1.4176830212512894
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 0.03198108228905446,
-            "velocityX": 0.2593084442835197,
-            "velocityY": -0.16387173465319213,
-            "timestamp": 1.4964431890985832
-        },
-        {
-            "x": 7.653968415649204,
-            "y": 0.8042046719225767,
-            "heading": 3.120030327667123,
-            "angularVelocity": -0.24175586185815795,
-            "velocityX": -0.3151422820242312,
-            "velocityY": 0.20828235032084352,
-            "timestamp": 1.5790458287176843
-        },
-        {
-            "x": 7.58042792526076,
-            "y": 0.8521912576149779,
-            "heading": 3.0798198381250614,
-            "angularVelocity": -0.4867942444391582,
-            "velocityX": -0.8902922561258279,
-            "velocityY": 0.5809328359683593,
-            "timestamp": 1.6616484683367854
-        },
-        {
-            "x": 7.459301398894465,
-            "y": 0.9310142655562479,
-            "heading": 3.0230678620569518,
-            "angularVelocity": -0.6870479724353368,
-            "velocityX": -1.466375976903756,
-            "velocityY": 0.9542432094753368,
-            "timestamp": 1.7442511079558864
-        },
-        {
-            "x": 7.290490307507352,
-            "y": 1.0407418073187003,
-            "heading": 2.956300453743236,
-            "angularVelocity": -0.808296304108442,
-            "velocityX": -2.0436525051200767,
-            "velocityY": 1.3283781519383162,
-            "timestamp": 1.8268537475749875
-        },
-        {
-            "x": 7.073948364025268,
-            "y": 1.1814068761754821,
-            "heading": 2.8937785154835103,
-            "angularVelocity": -0.756900004019734,
-            "velocityX": -2.621489391629628,
-            "velocityY": 1.7029125159367249,
-            "timestamp": 1.9094563871940886
-        },
-        {
-            "x": 6.811621234855711,
-            "y": 1.3511947989186324,
-            "heading": 2.893774866555063,
-            "angularVelocity": -0.000044174477718030994,
-            "velocityX": -3.1757717474792755,
-            "velocityY": 2.0554781726840226,
-            "timestamp": 1.9920590268131897
-        },
-        {
-            "x": 6.5493872326198215,
-            "y": 1.5211289667970835,
-            "heading": 2.893774844652908,
-            "angularVelocity": -2.6515078170743926e-7,
-            "velocityX": -3.174644338789979,
-            "velocityY": 2.057248638325058,
-            "timestamp": 2.074661666432291
-        },
-        {
-            "x": 6.287153230916077,
-            "y": 1.691063135496712,
-            "heading": 2.893774822750754,
-            "angularVelocity": -2.651507828628349e-7,
-            "velocityX": -3.1746443323477713,
-            "velocityY": 2.057248648266354,
-            "timestamp": 2.1572643060513923
-        },
-        {
-            "x": 6.024919229212335,
-            "y": 1.860997304196345,
-            "heading": 2.893774800848599,
-            "angularVelocity": -2.651507825022106e-7,
-            "velocityX": -3.1746443323477354,
-            "velocityY": 2.0572486482664103,
-            "timestamp": 2.2398669456704936
-        },
-        {
-            "x": 5.762685227508594,
-            "y": 2.030931472895978,
-            "heading": 2.8937747789464447,
-            "angularVelocity": -2.651507826588173e-7,
-            "velocityX": -3.174644332347735,
-            "velocityY": 2.0572486482664103,
-            "timestamp": 2.322469585289595
-        },
-        {
-            "x": 5.500451225804852,
-            "y": 2.2008656415956116,
-            "heading": 2.89377475704429,
-            "angularVelocity": -2.651507831859411e-7,
-            "velocityX": -3.1746443323477345,
-            "velocityY": 2.05724864826641,
-            "timestamp": 2.405072224908696
-        },
-        {
-            "x": 5.238217224101064,
-            "y": 2.3707998102951717,
-            "heading": 2.8937747351421352,
-            "angularVelocity": -2.6515078363410014e-7,
-            "velocityX": -3.1746443323483056,
-            "velocityY": 2.057248648265529,
-            "timestamp": 2.4876748645277975
-        },
-        {
-            "x": 4.975983214041022,
-            "y": 2.5407339660996877,
-            "heading": 2.89377471323987,
-            "angularVelocity": -2.651521251593368e-7,
-            "velocityX": -3.1746444335103496,
-            "velocityY": 2.0572484921561784,
-            "timestamp": 2.570277504146899
-        },
-        {
-            "x": 4.737824648225833,
-            "y": 2.6937649564980313,
-            "heading": 2.811064183589652,
-            "angularVelocity": -1.0013061329711461,
-            "velocityX": -2.883183478317317,
-            "velocityY": 1.8526162251473437,
-            "timestamp": 2.652880143766
-        },
-        {
-            "x": 4.54767021844827,
-            "y": 2.816291569523401,
-            "heading": 2.7234816518817766,
-            "angularVelocity": -1.0602873238885226,
-            "velocityX": -2.302038175225493,
-            "velocityY": 1.48332563693325,
-            "timestamp": 2.7354827833851014
-        },
-        {
-            "x": 4.405252215667174,
-            "y": 2.908161976795727,
-            "heading": 2.651165155434109,
-            "angularVelocity": -0.875474401073941,
-            "velocityX": -1.724133798118536,
-            "velocityY": 1.1121969914758385,
-            "timestamp": 2.8180854230042027
-        },
-        {
-            "x": 4.3103959059570425,
-            "y": 2.9693921451036718,
-            "heading": 2.600323851470343,
-            "angularVelocity": -0.6154924854483996,
-            "velocityX": -1.1483447762388197,
-            "velocityY": 0.7412616423674994,
-            "timestamp": 2.900688062623304
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -0.3186805118059785,
-            "velocityX": -0.5737819781013825,
-            "velocityY": 0.3705433027015611,
-            "timestamp": 2.9832907022424053
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 5.015233372101362e-32,
-            "velocityY": -2.828100691709421e-32,
-            "timestamp": 3.0658933418615066
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S3.traj
deleted file mode 100644
index ba2da1d0..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S3.traj	
+++ /dev/null
@@ -1,355 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -4.85982063591045e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 4.306024668622343,
-            "y": 2.9721182741266654,
-            "heading": 2.598597610192154,
-            "angularVelocity": 0.31231028150986523,
-            "velocityX": 0.5462744658665957,
-            "velocityY": -0.3540079539621319,
-            "timestamp": 0.0787601678472939
-        },
-        {
-            "x": 4.392095194160992,
-            "y": 2.9163526825964556,
-            "heading": 2.647262656420547,
-            "angularVelocity": 0.6178890619271524,
-            "velocityX": 1.0928179546992538,
-            "velocityY": -0.7080430762709009,
-            "timestamp": 0.1575203356945878
-        },
-        {
-            "x": 4.521245672207201,
-            "y": 2.83269821600786,
-            "heading": 2.7190885011917048,
-            "angularVelocity": 0.911956471581102,
-            "velocityX": 1.6397943475262466,
-            "velocityY": -1.0621418018152553,
-            "timestamp": 0.23628050354188168
-        },
-        {
-            "x": 4.693538421890133,
-            "y": 2.721142508781873,
-            "heading": 2.812276483563172,
-            "angularVelocity": 1.1831866909189597,
-            "velocityX": 2.1875619922114358,
-            "velocityY": -1.4163975303135559,
-            "timestamp": 0.3150406713891756
-        },
-        {
-            "x": 4.909132383686271,
-            "y": 2.581660366752094,
-            "heading": 2.9215937215973726,
-            "angularVelocity": 1.387976194339083,
-            "velocityX": 2.7373476681023576,
-            "velocityY": -1.7709731434323392,
-            "timestamp": 0.39380083923646947
-        },
-        {
-            "x": 5.159074984583097,
-            "y": 2.4194878202605934,
-            "heading": 2.9215937398089027,
-            "angularVelocity": 2.3122767087752481e-7,
-            "velocityX": 3.1734645535727406,
-            "velocityY": -2.059068066055073,
-            "timestamp": 0.47256100708376336
-        },
-        {
-            "x": 5.409017280295355,
-            "y": 2.257314803413765,
-            "heading": 2.921593758018432,
-            "angularVelocity": 2.3120226821213244e-7,
-            "velocityX": 3.173460678713437,
-            "velocityY": -2.059074038050069,
-            "timestamp": 0.5513211749310573
-        },
-        {
-            "x": 5.658959576005671,
-            "y": 2.0951417865639432,
-            "heading": 2.9215937762279616,
-            "angularVelocity": 2.312022681051112e-7,
-            "velocityX": 3.1734606786887793,
-            "velocityY": -2.0590740380880725,
-            "timestamp": 0.6300813427783512
-        },
-        {
-            "x": 5.908901871715987,
-            "y": 1.9329687697141213,
-            "heading": 2.9215937944374906,
-            "angularVelocity": 2.3120226848439114e-7,
-            "velocityX": 3.173460678688779,
-            "velocityY": -2.0590740380880725,
-            "timestamp": 0.708841510625645
-        },
-        {
-            "x": 6.158844167426302,
-            "y": 1.7707957528642995,
-            "heading": 2.92159381264702,
-            "angularVelocity": 2.3120226852070691e-7,
-            "velocityX": 3.1734606786887785,
-            "velocityY": -2.059074038088073,
-            "timestamp": 0.7876016784729389
-        },
-        {
-            "x": 6.408786463136619,
-            "y": 1.6086227360144778,
-            "heading": 2.921593830856549,
-            "angularVelocity": 2.3120226809334475e-7,
-            "velocityX": 3.173460678688779,
-            "velocityY": -2.059074038088073,
-            "timestamp": 0.8663618463202328
-        },
-        {
-            "x": 6.658728758846954,
-            "y": 1.4464497191646866,
-            "heading": 2.9215938490660784,
-            "angularVelocity": 2.31202268371391e-7,
-            "velocityX": 3.173460678689032,
-            "velocityY": -2.0590740380876835,
-            "timestamp": 0.9451220141675267
-        },
-        {
-            "x": 6.908671057688978,
-            "y": 1.2842767071415118,
-            "heading": 2.9215938672756456,
-            "angularVelocity": 2.3120274459298332e-7,
-            "velocityX": 3.173460718451369,
-            "velocityY": -2.05907397680523,
-            "timestamp": 1.0238821820148205
-        },
-        {
-            "x": 7.1455069915896265,
-            "y": 1.1314725250685531,
-            "heading": 2.963985587877118,
-            "angularVelocity": 0.5382380683048028,
-            "velocityX": 3.007052173375748,
-            "velocityY": -1.9401200663922304,
-            "timestamp": 1.1026423498621143
-        },
-        {
-            "x": 7.339036455456064,
-            "y": 1.0065280886493264,
-            "heading": 3.0237371798128563,
-            "angularVelocity": 0.7586524199845985,
-            "velocityX": 2.457199738853539,
-            "velocityY": -1.5863911902965262,
-            "timestamp": 1.181402517709408
-        },
-        {
-            "x": 7.489157100180206,
-            "y": 0.9096422022009641,
-            "heading": 3.07668127081849,
-            "angularVelocity": 0.6722191236093652,
-            "velocityX": 1.9060478009036246,
-            "velocityY": -1.2301381408456074,
-            "timestamp": 1.2601626855567019
-        },
-        {
-            "x": 7.5959791241645505,
-            "y": 0.8407792473093463,
-            "heading": 3.115718535259042,
-            "angularVelocity": 0.4956472987238782,
-            "velocityX": 1.3562950270935392,
-            "velocityY": -0.8743373303257803,
-            "timestamp": 1.3389228534039956
-        },
-        {
-            "x": 7.659576823404001,
-            "y": 0.7999065653267177,
-            "heading": 3.1374811645909735,
-            "angularVelocity": 0.2763151720819586,
-            "velocityX": 0.8074855726914214,
-            "velocityY": -0.5189511792543529,
-            "timestamp": 1.4176830212512894
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 0.03198108228905446,
-            "velocityX": 0.2593084442835197,
-            "velocityY": -0.16387173465319213,
-            "timestamp": 1.4964431890985832
-        },
-        {
-            "x": 7.653968415649204,
-            "y": 0.8042046719225767,
-            "heading": 3.120030327667123,
-            "angularVelocity": -0.24175586185815795,
-            "velocityX": -0.3151422820242312,
-            "velocityY": 0.20828235032084352,
-            "timestamp": 1.5790458287176843
-        },
-        {
-            "x": 7.58042792526076,
-            "y": 0.8521912576149779,
-            "heading": 3.0798198381250614,
-            "angularVelocity": -0.4867942444391582,
-            "velocityX": -0.8902922561258279,
-            "velocityY": 0.5809328359683593,
-            "timestamp": 1.6616484683367854
-        },
-        {
-            "x": 7.459301398894465,
-            "y": 0.9310142655562479,
-            "heading": 3.0230678620569518,
-            "angularVelocity": -0.6870479724353368,
-            "velocityX": -1.466375976903756,
-            "velocityY": 0.9542432094753368,
-            "timestamp": 1.7442511079558864
-        },
-        {
-            "x": 7.290490307507352,
-            "y": 1.0407418073187003,
-            "heading": 2.956300453743236,
-            "angularVelocity": -0.808296304108442,
-            "velocityX": -2.0436525051200767,
-            "velocityY": 1.3283781519383162,
-            "timestamp": 1.8268537475749875
-        },
-        {
-            "x": 7.073948364025268,
-            "y": 1.1814068761754821,
-            "heading": 2.8937785154835103,
-            "angularVelocity": -0.756900004019734,
-            "velocityX": -2.621489391629628,
-            "velocityY": 1.7029125159367249,
-            "timestamp": 1.9094563871940886
-        },
-        {
-            "x": 6.811621234855711,
-            "y": 1.3511947989186324,
-            "heading": 2.893774866555063,
-            "angularVelocity": -0.000044174477718030994,
-            "velocityX": -3.1757717474792755,
-            "velocityY": 2.0554781726840226,
-            "timestamp": 1.9920590268131897
-        },
-        {
-            "x": 6.5493872326198215,
-            "y": 1.5211289667970835,
-            "heading": 2.893774844652908,
-            "angularVelocity": -2.6515078170743926e-7,
-            "velocityX": -3.174644338789979,
-            "velocityY": 2.057248638325058,
-            "timestamp": 2.074661666432291
-        },
-        {
-            "x": 6.287153230916077,
-            "y": 1.691063135496712,
-            "heading": 2.893774822750754,
-            "angularVelocity": -2.651507828628349e-7,
-            "velocityX": -3.1746443323477713,
-            "velocityY": 2.057248648266354,
-            "timestamp": 2.1572643060513923
-        },
-        {
-            "x": 6.024919229212335,
-            "y": 1.860997304196345,
-            "heading": 2.893774800848599,
-            "angularVelocity": -2.651507825022106e-7,
-            "velocityX": -3.1746443323477354,
-            "velocityY": 2.0572486482664103,
-            "timestamp": 2.2398669456704936
-        },
-        {
-            "x": 5.762685227508594,
-            "y": 2.030931472895978,
-            "heading": 2.8937747789464447,
-            "angularVelocity": -2.651507826588173e-7,
-            "velocityX": -3.174644332347735,
-            "velocityY": 2.0572486482664103,
-            "timestamp": 2.322469585289595
-        },
-        {
-            "x": 5.500451225804852,
-            "y": 2.2008656415956116,
-            "heading": 2.89377475704429,
-            "angularVelocity": -2.651507831859411e-7,
-            "velocityX": -3.1746443323477345,
-            "velocityY": 2.05724864826641,
-            "timestamp": 2.405072224908696
-        },
-        {
-            "x": 5.238217224101064,
-            "y": 2.3707998102951717,
-            "heading": 2.8937747351421352,
-            "angularVelocity": -2.6515078363410014e-7,
-            "velocityX": -3.1746443323483056,
-            "velocityY": 2.057248648265529,
-            "timestamp": 2.4876748645277975
-        },
-        {
-            "x": 4.975983214041022,
-            "y": 2.5407339660996877,
-            "heading": 2.89377471323987,
-            "angularVelocity": -2.651521251593368e-7,
-            "velocityX": -3.1746444335103496,
-            "velocityY": 2.0572484921561784,
-            "timestamp": 2.570277504146899
-        },
-        {
-            "x": 4.737824648225833,
-            "y": 2.6937649564980313,
-            "heading": 2.811064183589652,
-            "angularVelocity": -1.0013061329711461,
-            "velocityX": -2.883183478317317,
-            "velocityY": 1.8526162251473437,
-            "timestamp": 2.652880143766
-        },
-        {
-            "x": 4.54767021844827,
-            "y": 2.816291569523401,
-            "heading": 2.7234816518817766,
-            "angularVelocity": -1.0602873238885226,
-            "velocityX": -2.302038175225493,
-            "velocityY": 1.48332563693325,
-            "timestamp": 2.7354827833851014
-        },
-        {
-            "x": 4.405252215667174,
-            "y": 2.908161976795727,
-            "heading": 2.651165155434109,
-            "angularVelocity": -0.875474401073941,
-            "velocityX": -1.724133798118536,
-            "velocityY": 1.1121969914758385,
-            "timestamp": 2.8180854230042027
-        },
-        {
-            "x": 4.3103959059570425,
-            "y": 2.9693921451036718,
-            "heading": 2.600323851470343,
-            "angularVelocity": -0.6154924854483996,
-            "velocityX": -1.1483447762388197,
-            "velocityY": 0.7412616423674994,
-            "timestamp": 2.900688062623304
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -0.3186805118059785,
-            "velocityX": -0.5737819781013825,
-            "velocityY": 0.3705433027015611,
-            "timestamp": 2.9832907022424053
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 5.015233372101362e-32,
-            "velocityY": -2.828100691709421e-32,
-            "timestamp": 3.0658933418615066
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S4.1.traj
deleted file mode 100644
index 15d6e495..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S4.1.traj	
+++ /dev/null
@@ -1,319 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.307906047793953,
-            "y": 2.9763559763722562,
-            "heading": 2.599832328454837,
-            "angularVelocity": 0.3294814998044202,
-            "velocityX": 0.5727595173353921,
-            "velocityY": -0.3015705061160393,
-            "timestamp": 0.07840297094122421
-        },
-        {
-            "x": 4.397737480320486,
-            "y": 2.929052986687658,
-            "heading": 2.6508685642334093,
-            "angularVelocity": 0.6509477276930885,
-            "velocityX": 1.1457656699499514,
-            "velocityY": -0.6033315972179728,
-            "timestamp": 0.15680594188244842
-        },
-        {
-            "x": 4.532524605763972,
-            "y": 2.858064592924414,
-            "heading": 2.7260427369404674,
-            "angularVelocity": 0.95881790963547,
-            "velocityX": 1.7191583918999656,
-            "velocityY": -0.9054298952072739,
-            "timestamp": 0.23520891282367262
-        },
-        {
-            "x": 4.712319110504002,
-            "y": 2.7633347501885597,
-            "heading": 2.823264198017844,
-            "angularVelocity": 1.2400226663643632,
-            "velocityX": 2.293210353914077,
-            "velocityY": -1.20824302444969,
-            "timestamp": 0.31361188376489685
-        },
-        {
-            "x": 4.937226049144337,
-            "y": 2.644712434074336,
-            "heading": 2.936900931840023,
-            "angularVelocity": 1.4493932112280175,
-            "velocityX": 2.8686022473427424,
-            "velocityY": -1.5129824124033164,
-            "timestamp": 0.3920148547061211
-        },
-        {
-            "x": 5.200333937862815,
-            "y": 2.5078106422289963,
-            "heading": 2.936900964883958,
-            "angularVelocity": 4.214627997065615e-7,
-            "velocityX": 3.355840799907034,
-            "velocityY": -1.7461301555518065,
-            "timestamp": 0.4704178256473453
-        },
-        {
-            "x": 5.462885368981394,
-            "y": 2.3698446646000946,
-            "heading": 2.936900984463062,
-            "angularVelocity": 2.4972400074747634e-7,
-            "velocityX": 3.3487433953925927,
-            "velocityY": -1.7597034394566977,
-            "timestamp": 0.5488207965885695
-        },
-        {
-            "x": 5.725436796470205,
-            "y": 2.2318786800636885,
-            "heading": 2.9369010040421655,
-            "angularVelocity": 2.4972400141004584e-7,
-            "velocityX": 3.3487433490962957,
-            "velocityY": -1.759703527559279,
-            "timestamp": 0.6272237675297937
-        },
-        {
-            "x": 5.987988223958993,
-            "y": 2.0939126955272376,
-            "heading": 2.936901023621269,
-            "angularVelocity": 2.497240000885312e-7,
-            "velocityX": 3.3487433490959946,
-            "velocityY": -1.7597035275598514,
-            "timestamp": 0.7056267384710179
-        },
-        {
-            "x": 6.250539651447782,
-            "y": 1.9559467109907867,
-            "heading": 2.9369010432003724,
-            "angularVelocity": 2.497240001698141e-7,
-            "velocityX": 3.348743349095995,
-            "velocityY": -1.759703527559851,
-            "timestamp": 0.784029709412242
-        },
-        {
-            "x": 6.513091078936429,
-            "y": 1.8179807264540686,
-            "heading": 2.936901062779476,
-            "angularVelocity": 2.497239999907855e-7,
-            "velocityX": 3.3487433490942036,
-            "velocityY": -1.7597035275632584,
-            "timestamp": 0.8624326803534662
-        },
-        {
-            "x": 6.775642484801246,
-            "y": 1.6800147007668695,
-            "heading": 2.9369010823585797,
-            "angularVelocity": 2.4972399921138707e-7,
-            "velocityX": 3.348743073290482,
-            "velocityY": -1.759704052421956,
-            "timestamp": 0.9408356512946904
-        },
-        {
-            "x": 7.03487156699359,
-            "y": 1.5359031200183482,
-            "heading": 2.936901102721687,
-            "angularVelocity": 2.5972367283965845e-7,
-            "velocityX": 3.3063681016204165,
-            "velocityY": -1.8380882639839122,
-            "timestamp": 1.0192386222359147
-        },
-        {
-            "x": 7.257601032453945,
-            "y": 1.356628823714497,
-            "heading": 2.964092637940216,
-            "angularVelocity": 0.34681766382323054,
-            "velocityX": 2.8408294071826354,
-            "velocityY": -2.286575293661087,
-            "timestamp": 1.0976415931771388
-        },
-        {
-            "x": 7.433803163736967,
-            "y": 1.1974021851496213,
-            "heading": 3.0191989559887835,
-            "angularVelocity": 0.7028600751601236,
-            "velocityX": 2.2473910002092454,
-            "velocityY": -2.030875063193899,
-            "timestamp": 1.176044564118363
-        },
-        {
-            "x": 7.563846036619734,
-            "y": 1.0604378478062424,
-            "heading": 3.0711979216149987,
-            "angularVelocity": 0.6632269798184063,
-            "velocityX": 1.6586472594285269,
-            "velocityY": -1.74692789953109,
-            "timestamp": 1.2544475350595872
-        },
-        {
-            "x": 7.648096830602628,
-            "y": 0.946251036791659,
-            "heading": 3.1111882105248365,
-            "angularVelocity": 0.5100608871025,
-            "velocityX": 1.0745867531729072,
-            "velocityY": -1.4564092360754384,
-            "timestamp": 1.3328505060008113
-        },
-        {
-            "x": 7.6867694062848315,
-            "y": 0.855064254512633,
-            "heading": 3.1349445229229786,
-            "angularVelocity": 0.3030027065666845,
-            "velocityX": 0.49325395731671434,
-            "velocityY": -1.1630526392597296,
-            "timestamp": 1.4112534769420355
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 0.06448068250805035,
-            "velocityX": -0.08634119604942615,
-            "velocityY": -0.8681336140135004,
-            "timestamp": 1.4896564478832597
-        },
-        {
-            "x": 7.627073807612577,
-            "y": 0.7419574324370846,
-            "heading": 3.1244199952297187,
-            "angularVelocity": -0.1970875494622002,
-            "velocityX": -0.6695179952660093,
-            "velocityY": -0.5697898937387053,
-            "timestamp": 1.568707635097908
-        },
-        {
-            "x": 7.5280560417619835,
-            "y": 0.7206319042387348,
-            "heading": 3.0899060052252425,
-            "angularVelocity": -0.4366030570890143,
-            "velocityX": -1.2525778465750235,
-            "velocityY": -0.26976860120365537,
-            "timestamp": 1.6477588223125563
-        },
-        {
-            "x": 7.382968853200293,
-            "y": 0.7232223421332327,
-            "heading": 3.03915644307554,
-            "angularVelocity": -0.6419835544263846,
-            "velocityX": -1.8353574901749392,
-            "velocityY": 0.0327691206892746,
-            "timestamp": 1.7268100095272048
-        },
-        {
-            "x": 7.191873936786201,
-            "y": 0.7500593523651382,
-            "heading": 2.9768884847472514,
-            "angularVelocity": -0.7876916276946772,
-            "velocityX": -2.417356691875479,
-            "velocityY": 0.3394890219549461,
-            "timestamp": 1.8058611967418532
-        },
-        {
-            "x": 6.955009615423507,
-            "y": 0.8017966281088876,
-            "heading": 2.913255262792046,
-            "angularVelocity": -0.8049622554368411,
-            "velocityX": -2.9963411013610584,
-            "velocityY": 0.6544781623992936,
-            "timestamp": 1.8849123839565016
-        },
-        {
-            "x": 6.67449530614552,
-            "y": 0.8800439513487449,
-            "heading": 2.8869556643049132,
-            "angularVelocity": -0.33269074651445785,
-            "velocityX": -3.5485148188383207,
-            "velocityY": 0.9898310954821422,
-            "timestamp": 1.96396357117115
-        },
-        {
-            "x": 6.4045779974291,
-            "y": 1.0087805726115764,
-            "heading": 2.886955644734896,
-            "angularVelocity": -2.475613351225265e-7,
-            "velocityX": -3.4144624290526653,
-            "velocityY": 1.6285223005349794,
-            "timestamp": 2.0430147583857985
-        },
-        {
-            "x": 6.136902414581452,
-            "y": 1.1421156905122876,
-            "heading": 2.8869556256683206,
-            "angularVelocity": -2.4119277641286314e-7,
-            "velocityX": -3.386104526435805,
-            "velocityY": 1.6866934273696874,
-            "timestamp": 2.122065945600447
-        },
-        {
-            "x": 5.869226659738861,
-            "y": 1.2754504631222496,
-            "heading": 2.8869556065980015,
-            "angularVelocity": -2.4124013985780433e-7,
-            "velocityX": -3.3861067021774174,
-            "velocityY": 1.6866890594304795,
-            "timestamp": 2.2011171328150954
-        },
-        {
-            "x": 5.637978571315548,
-            "y": 1.3902481295023497,
-            "heading": 2.7615946389842474,
-            "angularVelocity": -1.5858201758975898,
-            "velocityX": -2.9252955783626895,
-            "velocityY": 1.4521940836692195,
-            "timestamp": 2.280168320029744
-        },
-        {
-            "x": 5.453130511537557,
-            "y": 1.4821316832792537,
-            "heading": 2.658075983788866,
-            "angularVelocity": -1.309514238087925,
-            "velocityX": -2.3383337593158178,
-            "velocityY": 1.162329839872467,
-            "timestamp": 2.3592195072443922
-        },
-        {
-            "x": 5.3145458591491295,
-            "y": 1.5510603964907557,
-            "heading": 2.5794202908528,
-            "angularVelocity": -0.9949969849595353,
-            "velocityX": -1.753100203443375,
-            "velocityY": 0.8719503860752608,
-            "timestamp": 2.4382706944590407
-        },
-        {
-            "x": 5.222177020265271,
-            "y": 1.597019066974222,
-            "heading": 2.5265746527994444,
-            "angularVelocity": -0.668498980411977,
-            "velocityX": -1.1684687117102668,
-            "velocityY": 0.5813786244434977,
-            "timestamp": 2.517321881673689
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.336170192198179,
-            "velocityX": -0.5841407560381138,
-            "velocityY": 0.29070952424007707,
-            "timestamp": 2.5963730688883375
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": 3.1840213941352295e-31,
-            "velocityY": 0,
-            "timestamp": 2.675424256102986
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S4.traj
deleted file mode 100644
index 15d6e495..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5 To S4.traj	
+++ /dev/null
@@ -1,319 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.307906047793953,
-            "y": 2.9763559763722562,
-            "heading": 2.599832328454837,
-            "angularVelocity": 0.3294814998044202,
-            "velocityX": 0.5727595173353921,
-            "velocityY": -0.3015705061160393,
-            "timestamp": 0.07840297094122421
-        },
-        {
-            "x": 4.397737480320486,
-            "y": 2.929052986687658,
-            "heading": 2.6508685642334093,
-            "angularVelocity": 0.6509477276930885,
-            "velocityX": 1.1457656699499514,
-            "velocityY": -0.6033315972179728,
-            "timestamp": 0.15680594188244842
-        },
-        {
-            "x": 4.532524605763972,
-            "y": 2.858064592924414,
-            "heading": 2.7260427369404674,
-            "angularVelocity": 0.95881790963547,
-            "velocityX": 1.7191583918999656,
-            "velocityY": -0.9054298952072739,
-            "timestamp": 0.23520891282367262
-        },
-        {
-            "x": 4.712319110504002,
-            "y": 2.7633347501885597,
-            "heading": 2.823264198017844,
-            "angularVelocity": 1.2400226663643632,
-            "velocityX": 2.293210353914077,
-            "velocityY": -1.20824302444969,
-            "timestamp": 0.31361188376489685
-        },
-        {
-            "x": 4.937226049144337,
-            "y": 2.644712434074336,
-            "heading": 2.936900931840023,
-            "angularVelocity": 1.4493932112280175,
-            "velocityX": 2.8686022473427424,
-            "velocityY": -1.5129824124033164,
-            "timestamp": 0.3920148547061211
-        },
-        {
-            "x": 5.200333937862815,
-            "y": 2.5078106422289963,
-            "heading": 2.936900964883958,
-            "angularVelocity": 4.214627997065615e-7,
-            "velocityX": 3.355840799907034,
-            "velocityY": -1.7461301555518065,
-            "timestamp": 0.4704178256473453
-        },
-        {
-            "x": 5.462885368981394,
-            "y": 2.3698446646000946,
-            "heading": 2.936900984463062,
-            "angularVelocity": 2.4972400074747634e-7,
-            "velocityX": 3.3487433953925927,
-            "velocityY": -1.7597034394566977,
-            "timestamp": 0.5488207965885695
-        },
-        {
-            "x": 5.725436796470205,
-            "y": 2.2318786800636885,
-            "heading": 2.9369010040421655,
-            "angularVelocity": 2.4972400141004584e-7,
-            "velocityX": 3.3487433490962957,
-            "velocityY": -1.759703527559279,
-            "timestamp": 0.6272237675297937
-        },
-        {
-            "x": 5.987988223958993,
-            "y": 2.0939126955272376,
-            "heading": 2.936901023621269,
-            "angularVelocity": 2.497240000885312e-7,
-            "velocityX": 3.3487433490959946,
-            "velocityY": -1.7597035275598514,
-            "timestamp": 0.7056267384710179
-        },
-        {
-            "x": 6.250539651447782,
-            "y": 1.9559467109907867,
-            "heading": 2.9369010432003724,
-            "angularVelocity": 2.497240001698141e-7,
-            "velocityX": 3.348743349095995,
-            "velocityY": -1.759703527559851,
-            "timestamp": 0.784029709412242
-        },
-        {
-            "x": 6.513091078936429,
-            "y": 1.8179807264540686,
-            "heading": 2.936901062779476,
-            "angularVelocity": 2.497239999907855e-7,
-            "velocityX": 3.3487433490942036,
-            "velocityY": -1.7597035275632584,
-            "timestamp": 0.8624326803534662
-        },
-        {
-            "x": 6.775642484801246,
-            "y": 1.6800147007668695,
-            "heading": 2.9369010823585797,
-            "angularVelocity": 2.4972399921138707e-7,
-            "velocityX": 3.348743073290482,
-            "velocityY": -1.759704052421956,
-            "timestamp": 0.9408356512946904
-        },
-        {
-            "x": 7.03487156699359,
-            "y": 1.5359031200183482,
-            "heading": 2.936901102721687,
-            "angularVelocity": 2.5972367283965845e-7,
-            "velocityX": 3.3063681016204165,
-            "velocityY": -1.8380882639839122,
-            "timestamp": 1.0192386222359147
-        },
-        {
-            "x": 7.257601032453945,
-            "y": 1.356628823714497,
-            "heading": 2.964092637940216,
-            "angularVelocity": 0.34681766382323054,
-            "velocityX": 2.8408294071826354,
-            "velocityY": -2.286575293661087,
-            "timestamp": 1.0976415931771388
-        },
-        {
-            "x": 7.433803163736967,
-            "y": 1.1974021851496213,
-            "heading": 3.0191989559887835,
-            "angularVelocity": 0.7028600751601236,
-            "velocityX": 2.2473910002092454,
-            "velocityY": -2.030875063193899,
-            "timestamp": 1.176044564118363
-        },
-        {
-            "x": 7.563846036619734,
-            "y": 1.0604378478062424,
-            "heading": 3.0711979216149987,
-            "angularVelocity": 0.6632269798184063,
-            "velocityX": 1.6586472594285269,
-            "velocityY": -1.74692789953109,
-            "timestamp": 1.2544475350595872
-        },
-        {
-            "x": 7.648096830602628,
-            "y": 0.946251036791659,
-            "heading": 3.1111882105248365,
-            "angularVelocity": 0.5100608871025,
-            "velocityX": 1.0745867531729072,
-            "velocityY": -1.4564092360754384,
-            "timestamp": 1.3328505060008113
-        },
-        {
-            "x": 7.6867694062848315,
-            "y": 0.855064254512633,
-            "heading": 3.1349445229229786,
-            "angularVelocity": 0.3030027065666845,
-            "velocityX": 0.49325395731671434,
-            "velocityY": -1.1630526392597296,
-            "timestamp": 1.4112534769420355
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 0.06448068250805035,
-            "velocityX": -0.08634119604942615,
-            "velocityY": -0.8681336140135004,
-            "timestamp": 1.4896564478832597
-        },
-        {
-            "x": 7.627073807612577,
-            "y": 0.7419574324370846,
-            "heading": 3.1244199952297187,
-            "angularVelocity": -0.1970875494622002,
-            "velocityX": -0.6695179952660093,
-            "velocityY": -0.5697898937387053,
-            "timestamp": 1.568707635097908
-        },
-        {
-            "x": 7.5280560417619835,
-            "y": 0.7206319042387348,
-            "heading": 3.0899060052252425,
-            "angularVelocity": -0.4366030570890143,
-            "velocityX": -1.2525778465750235,
-            "velocityY": -0.26976860120365537,
-            "timestamp": 1.6477588223125563
-        },
-        {
-            "x": 7.382968853200293,
-            "y": 0.7232223421332327,
-            "heading": 3.03915644307554,
-            "angularVelocity": -0.6419835544263846,
-            "velocityX": -1.8353574901749392,
-            "velocityY": 0.0327691206892746,
-            "timestamp": 1.7268100095272048
-        },
-        {
-            "x": 7.191873936786201,
-            "y": 0.7500593523651382,
-            "heading": 2.9768884847472514,
-            "angularVelocity": -0.7876916276946772,
-            "velocityX": -2.417356691875479,
-            "velocityY": 0.3394890219549461,
-            "timestamp": 1.8058611967418532
-        },
-        {
-            "x": 6.955009615423507,
-            "y": 0.8017966281088876,
-            "heading": 2.913255262792046,
-            "angularVelocity": -0.8049622554368411,
-            "velocityX": -2.9963411013610584,
-            "velocityY": 0.6544781623992936,
-            "timestamp": 1.8849123839565016
-        },
-        {
-            "x": 6.67449530614552,
-            "y": 0.8800439513487449,
-            "heading": 2.8869556643049132,
-            "angularVelocity": -0.33269074651445785,
-            "velocityX": -3.5485148188383207,
-            "velocityY": 0.9898310954821422,
-            "timestamp": 1.96396357117115
-        },
-        {
-            "x": 6.4045779974291,
-            "y": 1.0087805726115764,
-            "heading": 2.886955644734896,
-            "angularVelocity": -2.475613351225265e-7,
-            "velocityX": -3.4144624290526653,
-            "velocityY": 1.6285223005349794,
-            "timestamp": 2.0430147583857985
-        },
-        {
-            "x": 6.136902414581452,
-            "y": 1.1421156905122876,
-            "heading": 2.8869556256683206,
-            "angularVelocity": -2.4119277641286314e-7,
-            "velocityX": -3.386104526435805,
-            "velocityY": 1.6866934273696874,
-            "timestamp": 2.122065945600447
-        },
-        {
-            "x": 5.869226659738861,
-            "y": 1.2754504631222496,
-            "heading": 2.8869556065980015,
-            "angularVelocity": -2.4124013985780433e-7,
-            "velocityX": -3.3861067021774174,
-            "velocityY": 1.6866890594304795,
-            "timestamp": 2.2011171328150954
-        },
-        {
-            "x": 5.637978571315548,
-            "y": 1.3902481295023497,
-            "heading": 2.7615946389842474,
-            "angularVelocity": -1.5858201758975898,
-            "velocityX": -2.9252955783626895,
-            "velocityY": 1.4521940836692195,
-            "timestamp": 2.280168320029744
-        },
-        {
-            "x": 5.453130511537557,
-            "y": 1.4821316832792537,
-            "heading": 2.658075983788866,
-            "angularVelocity": -1.309514238087925,
-            "velocityX": -2.3383337593158178,
-            "velocityY": 1.162329839872467,
-            "timestamp": 2.3592195072443922
-        },
-        {
-            "x": 5.3145458591491295,
-            "y": 1.5510603964907557,
-            "heading": 2.5794202908528,
-            "angularVelocity": -0.9949969849595353,
-            "velocityX": -1.753100203443375,
-            "velocityY": 0.8719503860752608,
-            "timestamp": 2.4382706944590407
-        },
-        {
-            "x": 5.222177020265271,
-            "y": 1.597019066974222,
-            "heading": 2.5265746527994444,
-            "angularVelocity": -0.668498980411977,
-            "velocityX": -1.1684687117102668,
-            "velocityY": 0.5813786244434977,
-            "timestamp": 2.517321881673689
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.336170192198179,
-            "velocityX": -0.5841407560381138,
-            "velocityY": 0.29070952424007707,
-            "timestamp": 2.5963730688883375
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": 3.1840213941352295e-31,
-            "velocityY": 0,
-            "timestamp": 2.675424256102986
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.1.traj
deleted file mode 100644
index 0702b196..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.1.traj	
+++ /dev/null
@@ -1,157 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.228111844192817,
-            "y": 3.0443410125088586,
-            "heading": 2.5974492034990555,
-            "angularVelocity": 0.2841110410157656,
-            "velocityX": -0.42270562690532876,
-            "velocityY": 0.5372366368048371,
-            "timestamp": 0.08253534750081891
-        },
-        {
-            "x": 4.158301873890454,
-            "y": 3.133062319081129,
-            "heading": 2.642673247648486,
-            "angularVelocity": 0.5479354666675527,
-            "velocityX": -0.8458190631798037,
-            "velocityY": 1.0749492097478566,
-            "timestamp": 0.16507069500163782
-        },
-        {
-            "x": 4.053518280244317,
-            "y": 3.266222365985673,
-            "heading": 2.7068891131091086,
-            "angularVelocity": 0.7780407716825238,
-            "velocityX": -1.2695602165496185,
-            "velocityY": 1.6133699189092645,
-            "timestamp": 0.2476060425024567
-        },
-        {
-            "x": 3.9136724453244414,
-            "y": 3.4439114508470956,
-            "heading": 2.784478333107965,
-            "angularVelocity": 0.9400726155310244,
-            "velocityX": -1.694375066616006,
-            "velocityY": 2.152884675982738,
-            "timestamp": 0.33014139000327564
-        },
-        {
-            "x": 3.738653008239037,
-            "y": 3.6661781083911804,
-            "heading": 2.8571480043531183,
-            "angularVelocity": 0.8804672597329689,
-            "velocityX": -2.1205391675811076,
-            "velocityY": 2.6929874808109258,
-            "timestamp": 0.41267673750409456
-        },
-        {
-            "x": 3.545725477812246,
-            "y": 3.911665858291861,
-            "heading": 2.8571480229647763,
-            "angularVelocity": 2.254992345614167e-7,
-            "velocityX": -2.3375140018024174,
-            "velocityY": 2.9743347224441523,
-            "timestamp": 0.4952120850049135
-        },
-        {
-            "x": 3.352797950657087,
-            "y": 4.157153610763737,
-            "heading": 2.8571480415763673,
-            "angularVelocity": 2.2549842301439466e-7,
-            "velocityX": -2.3375139621632335,
-            "velocityY": 2.9743347535968137,
-            "timestamp": 0.5777474325057324
-        },
-        {
-            "x": 3.1598704235019452,
-            "y": 4.402641363235625,
-            "heading": 2.8571480601879577,
-            "angularVelocity": 2.2549842390160243e-7,
-            "velocityX": -2.3375139621630274,
-            "velocityY": 2.974334753596974,
-            "timestamp": 0.6602827800065513
-        },
-        {
-            "x": 2.966942896346795,
-            "y": 4.648129115707507,
-            "heading": 2.8571480787995487,
-            "angularVelocity": 2.2549842409910227e-7,
-            "velocityX": -2.337513962163132,
-            "velocityY": 2.974334753596892,
-            "timestamp": 0.7428181275073702
-        },
-        {
-            "x": 2.7740153675253203,
-            "y": 4.8936168668697935,
-            "heading": 2.857148097411205,
-            "angularVelocity": 2.2549920897733148e-7,
-            "velocityX": -2.3375139823523616,
-            "velocityY": 2.9743347377297993,
-            "timestamp": 0.8253534750081891
-        },
-        {
-            "x": 2.599116566256145,
-            "y": 5.1159598004085165,
-            "heading": 2.929233412420125,
-            "angularVelocity": 0.873387187328517,
-            "velocityX": -2.1190775415035294,
-            "velocityY": 2.693911642360469,
-            "timestamp": 0.907888822509008
-        },
-        {
-            "x": 2.459351765780957,
-            "y": 5.293700413500499,
-            "heading": 3.0069352217627348,
-            "angularVelocity": 0.9414367503794493,
-            "velocityX": -1.6933932515859442,
-            "velocityY": 2.1535089931040625,
-            "timestamp": 0.990424170009827
-        },
-        {
-            "x": 2.354631169439892,
-            "y": 5.426897486256648,
-            "heading": 3.071284016075413,
-            "angularVelocity": 0.7796513404397946,
-            "velocityX": -1.268796939881139,
-            "velocityY": 1.6138185249032515,
-            "timestamp": 1.0729595175106459
-        },
-        {
-            "x": 2.284865378797646,
-            "y": 5.515644940856661,
-            "heading": 3.116557551514127,
-            "angularVelocity": 0.5485351041656966,
-            "velocityX": -0.8452837814919688,
-            "velocityY": 1.0752660197999702,
-            "timestamp": 1.1554948650114647
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0.2840291971344776,
-            "velocityX": -0.4224296601804454,
-            "velocityY": 0.5374068261225751,
-            "timestamp": 1.2380302125122835
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 2.192310159207654e-31,
-            "velocityX": 0,
-            "velocityY": 7.442690740363628e-33,
-            "timestamp": 1.3205655600131023
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.traj
deleted file mode 100644
index 0702b196..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.traj	
+++ /dev/null
@@ -1,157 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.228111844192817,
-            "y": 3.0443410125088586,
-            "heading": 2.5974492034990555,
-            "angularVelocity": 0.2841110410157656,
-            "velocityX": -0.42270562690532876,
-            "velocityY": 0.5372366368048371,
-            "timestamp": 0.08253534750081891
-        },
-        {
-            "x": 4.158301873890454,
-            "y": 3.133062319081129,
-            "heading": 2.642673247648486,
-            "angularVelocity": 0.5479354666675527,
-            "velocityX": -0.8458190631798037,
-            "velocityY": 1.0749492097478566,
-            "timestamp": 0.16507069500163782
-        },
-        {
-            "x": 4.053518280244317,
-            "y": 3.266222365985673,
-            "heading": 2.7068891131091086,
-            "angularVelocity": 0.7780407716825238,
-            "velocityX": -1.2695602165496185,
-            "velocityY": 1.6133699189092645,
-            "timestamp": 0.2476060425024567
-        },
-        {
-            "x": 3.9136724453244414,
-            "y": 3.4439114508470956,
-            "heading": 2.784478333107965,
-            "angularVelocity": 0.9400726155310244,
-            "velocityX": -1.694375066616006,
-            "velocityY": 2.152884675982738,
-            "timestamp": 0.33014139000327564
-        },
-        {
-            "x": 3.738653008239037,
-            "y": 3.6661781083911804,
-            "heading": 2.8571480043531183,
-            "angularVelocity": 0.8804672597329689,
-            "velocityX": -2.1205391675811076,
-            "velocityY": 2.6929874808109258,
-            "timestamp": 0.41267673750409456
-        },
-        {
-            "x": 3.545725477812246,
-            "y": 3.911665858291861,
-            "heading": 2.8571480229647763,
-            "angularVelocity": 2.254992345614167e-7,
-            "velocityX": -2.3375140018024174,
-            "velocityY": 2.9743347224441523,
-            "timestamp": 0.4952120850049135
-        },
-        {
-            "x": 3.352797950657087,
-            "y": 4.157153610763737,
-            "heading": 2.8571480415763673,
-            "angularVelocity": 2.2549842301439466e-7,
-            "velocityX": -2.3375139621632335,
-            "velocityY": 2.9743347535968137,
-            "timestamp": 0.5777474325057324
-        },
-        {
-            "x": 3.1598704235019452,
-            "y": 4.402641363235625,
-            "heading": 2.8571480601879577,
-            "angularVelocity": 2.2549842390160243e-7,
-            "velocityX": -2.3375139621630274,
-            "velocityY": 2.974334753596974,
-            "timestamp": 0.6602827800065513
-        },
-        {
-            "x": 2.966942896346795,
-            "y": 4.648129115707507,
-            "heading": 2.8571480787995487,
-            "angularVelocity": 2.2549842409910227e-7,
-            "velocityX": -2.337513962163132,
-            "velocityY": 2.974334753596892,
-            "timestamp": 0.7428181275073702
-        },
-        {
-            "x": 2.7740153675253203,
-            "y": 4.8936168668697935,
-            "heading": 2.857148097411205,
-            "angularVelocity": 2.2549920897733148e-7,
-            "velocityX": -2.3375139823523616,
-            "velocityY": 2.9743347377297993,
-            "timestamp": 0.8253534750081891
-        },
-        {
-            "x": 2.599116566256145,
-            "y": 5.1159598004085165,
-            "heading": 2.929233412420125,
-            "angularVelocity": 0.873387187328517,
-            "velocityX": -2.1190775415035294,
-            "velocityY": 2.693911642360469,
-            "timestamp": 0.907888822509008
-        },
-        {
-            "x": 2.459351765780957,
-            "y": 5.293700413500499,
-            "heading": 3.0069352217627348,
-            "angularVelocity": 0.9414367503794493,
-            "velocityX": -1.6933932515859442,
-            "velocityY": 2.1535089931040625,
-            "timestamp": 0.990424170009827
-        },
-        {
-            "x": 2.354631169439892,
-            "y": 5.426897486256648,
-            "heading": 3.071284016075413,
-            "angularVelocity": 0.7796513404397946,
-            "velocityX": -1.268796939881139,
-            "velocityY": 1.6138185249032515,
-            "timestamp": 1.0729595175106459
-        },
-        {
-            "x": 2.284865378797646,
-            "y": 5.515644940856661,
-            "heading": 3.116557551514127,
-            "angularVelocity": 0.5485351041656966,
-            "velocityX": -0.8452837814919688,
-            "velocityY": 1.0752660197999702,
-            "timestamp": 1.1554948650114647
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0.2840291971344776,
-            "velocityX": -0.4224296601804454,
-            "velocityY": 0.5374068261225751,
-            "timestamp": 1.2380302125122835
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 2.192310159207654e-31,
-            "velocityX": 0,
-            "velocityY": 7.442690740363628e-33,
-            "timestamp": 1.3205655600131023
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.1.traj
deleted file mode 100644
index a2adec3d..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.1.traj	
+++ /dev/null
@@ -1,121 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.097905783412131e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 4.206954796561596,
-            "y": 3.0388324489204006,
-            "heading": 2.584970820102585,
-            "angularVelocity": 0.12125870375097321,
-            "velocityX": -0.619458587129609,
-            "velocityY": 0.42920878982002253,
-            "timestamp": 0.0904744959596105
-        },
-        {
-            "x": 4.094846273424844,
-            "y": 3.1165082311139285,
-            "heading": 2.6050859488479765,
-            "angularVelocity": 0.2223292711613592,
-            "velocityX": -1.2391174103561666,
-            "velocityY": 0.858537882633839,
-            "timestamp": 0.180948991919221
-        },
-        {
-            "x": 3.9266509261170293,
-            "y": 3.2330407963022716,
-            "heading": 2.631256329120159,
-            "angularVelocity": 0.2892569888851994,
-            "velocityX": -1.8590360247256863,
-            "velocityY": 1.2880156330505033,
-            "timestamp": 0.2714234878788315
-        },
-        {
-            "x": 3.702348687884298,
-            "y": 3.38843810676698,
-            "heading": 2.65711164443334,
-            "angularVelocity": 0.2857746267492099,
-            "velocityX": -2.479176433686503,
-            "velocityY": 1.7175813892799228,
-            "timestamp": 0.361897983838442
-        },
-        {
-            "x": 3.4221471625561564,
-            "y": 3.582533883045857,
-            "heading": 2.6614389879992997,
-            "angularVelocity": 0.04782942994113533,
-            "velocityX": -3.0970222310299356,
-            "velocityY": 2.1453092854534943,
-            "timestamp": 0.4523724797980525
-        },
-        {
-            "x": 3.1408345803954827,
-            "y": 3.7774823101067563,
-            "heading": 2.6614389980641375,
-            "angularVelocity": 1.1124502748712501e-7,
-            "velocityX": -3.1093025628598916,
-            "velocityY": 2.154733496917496,
-            "timestamp": 0.542846975757663
-        },
-        {
-            "x": 2.8606418991642766,
-            "y": 3.9715702772190298,
-            "heading": 2.6657880248697206,
-            "angularVelocity": 0.04806909128871381,
-            "velocityX": -3.096924478654065,
-            "velocityY": 2.1452229719955302,
-            "timestamp": 0.6333214717172736
-        },
-        {
-            "x": 2.6363424534806392,
-            "y": 4.126965038945093,
-            "heading": 2.691984108325433,
-            "angularVelocity": 0.28954108202389867,
-            "velocityX": -2.479145568092125,
-            "velocityY": 1.7175532184830777,
-            "timestamp": 0.7237959676768841
-        },
-        {
-            "x": 2.46814994135131,
-            "y": 4.24349512219199,
-            "heading": 2.7185033740588933,
-            "angularVelocity": 0.2931131635737346,
-            "velocityX": -1.8590046879555269,
-            "velocityY": 1.2879882005523255,
-            "timestamp": 0.8142704636364947
-        },
-        {
-            "x": 2.3560438127968273,
-            "y": 4.3211688040728875,
-            "heading": 2.738885032189894,
-            "angularVelocity": 0.22527517743895228,
-            "velocityX": -1.2390909434248611,
-            "velocityY": 0.8585146682173574,
-            "timestamp": 0.9047449595961052
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0.12285194509475847,
-            "velocityX": -0.6194432165927374,
-            "velocityY": 0.42919494068745534,
-            "timestamp": 0.9952194555557158
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 6.588066214768088e-31,
-            "velocityX": -4.3339190939808694e-33,
-            "velocityY": -1.4815259513582048e-34,
-            "timestamp": 1.0856939515153263
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.traj
deleted file mode 100644
index a2adec3d..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.traj	
+++ /dev/null
@@ -1,121 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.097905783412131e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 4.206954796561596,
-            "y": 3.0388324489204006,
-            "heading": 2.584970820102585,
-            "angularVelocity": 0.12125870375097321,
-            "velocityX": -0.619458587129609,
-            "velocityY": 0.42920878982002253,
-            "timestamp": 0.0904744959596105
-        },
-        {
-            "x": 4.094846273424844,
-            "y": 3.1165082311139285,
-            "heading": 2.6050859488479765,
-            "angularVelocity": 0.2223292711613592,
-            "velocityX": -1.2391174103561666,
-            "velocityY": 0.858537882633839,
-            "timestamp": 0.180948991919221
-        },
-        {
-            "x": 3.9266509261170293,
-            "y": 3.2330407963022716,
-            "heading": 2.631256329120159,
-            "angularVelocity": 0.2892569888851994,
-            "velocityX": -1.8590360247256863,
-            "velocityY": 1.2880156330505033,
-            "timestamp": 0.2714234878788315
-        },
-        {
-            "x": 3.702348687884298,
-            "y": 3.38843810676698,
-            "heading": 2.65711164443334,
-            "angularVelocity": 0.2857746267492099,
-            "velocityX": -2.479176433686503,
-            "velocityY": 1.7175813892799228,
-            "timestamp": 0.361897983838442
-        },
-        {
-            "x": 3.4221471625561564,
-            "y": 3.582533883045857,
-            "heading": 2.6614389879992997,
-            "angularVelocity": 0.04782942994113533,
-            "velocityX": -3.0970222310299356,
-            "velocityY": 2.1453092854534943,
-            "timestamp": 0.4523724797980525
-        },
-        {
-            "x": 3.1408345803954827,
-            "y": 3.7774823101067563,
-            "heading": 2.6614389980641375,
-            "angularVelocity": 1.1124502748712501e-7,
-            "velocityX": -3.1093025628598916,
-            "velocityY": 2.154733496917496,
-            "timestamp": 0.542846975757663
-        },
-        {
-            "x": 2.8606418991642766,
-            "y": 3.9715702772190298,
-            "heading": 2.6657880248697206,
-            "angularVelocity": 0.04806909128871381,
-            "velocityX": -3.096924478654065,
-            "velocityY": 2.1452229719955302,
-            "timestamp": 0.6333214717172736
-        },
-        {
-            "x": 2.6363424534806392,
-            "y": 4.126965038945093,
-            "heading": 2.691984108325433,
-            "angularVelocity": 0.28954108202389867,
-            "velocityX": -2.479145568092125,
-            "velocityY": 1.7175532184830777,
-            "timestamp": 0.7237959676768841
-        },
-        {
-            "x": 2.46814994135131,
-            "y": 4.24349512219199,
-            "heading": 2.7185033740588933,
-            "angularVelocity": 0.2931131635737346,
-            "velocityX": -1.8590046879555269,
-            "velocityY": 1.2879882005523255,
-            "timestamp": 0.8142704636364947
-        },
-        {
-            "x": 2.3560438127968273,
-            "y": 4.3211688040728875,
-            "heading": 2.738885032189894,
-            "angularVelocity": 0.22527517743895228,
-            "velocityX": -1.2390909434248611,
-            "velocityY": 0.8585146682173574,
-            "timestamp": 0.9047449595961052
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0.12285194509475847,
-            "velocityX": -0.6194432165927374,
-            "velocityY": 0.42919494068745534,
-            "timestamp": 0.9952194555557158
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 6.588066214768088e-31,
-            "velocityX": -4.3339190939808694e-33,
-            "velocityY": -1.4815259513582048e-34,
-            "timestamp": 1.0856939515153263
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S1.1.traj
deleted file mode 100644
index a99f9c86..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S1.1.traj	
+++ /dev/null
@@ -1,319 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.205167733325116,
-            "y": 1.6385570868106298,
-            "heading": 2.543052954613419,
-            "angularVelocity": 0.6503304289456182,
-            "velocityX": 0.44058914643605,
-            "velocityY": 0.2803114985693042,
-            "timestamp": 0.0662016610283172
-        },
-        {
-            "x": 5.263930522170129,
-            "y": 1.6757412702349752,
-            "heading": 2.6242045140781594,
-            "angularVelocity": 1.225823615363673,
-            "velocityX": 0.8876331489609754,
-            "velocityY": 0.5616805204997857,
-            "timestamp": 0.1324033220566344
-        },
-        {
-            "x": 5.3527880828329195,
-            "y": 1.7316217896984443,
-            "heading": 2.7372039865474704,
-            "angularVelocity": 1.706897843863404,
-            "velocityX": 1.3422255466488056,
-            "velocityY": 0.8440954289583611,
-            "timestamp": 0.19860498308495161
-        },
-        {
-            "x": 5.472262183244267,
-            "y": 1.806361940098468,
-            "heading": 2.873328504655769,
-            "angularVelocity": 2.056210010353016,
-            "velocityX": 1.804699437378857,
-            "velocityY": 1.1289769658201285,
-            "timestamp": 0.2648066441132688
-        },
-        {
-            "x": 5.622851983789364,
-            "y": 1.9002058058145468,
-            "heading": 3.0148229359693395,
-            "angularVelocity": 2.137324488779513,
-            "velocityX": 2.2747133260099135,
-            "velocityY": 1.4175454853906315,
-            "timestamp": 0.331008305141586
-        },
-        {
-            "x": 5.803032153079139,
-            "y": 2.011379887463594,
-            "heading": 3.1112408203645807,
-            "angularVelocity": 1.456426967203298,
-            "velocityX": 2.721686533102414,
-            "velocityY": 1.6793246562416593,
-            "timestamp": 0.39720996616990323
-        },
-        {
-            "x": 6.010803138038067,
-            "y": 2.135461093514753,
-            "heading": 3.1324618293668918,
-            "angularVelocity": 0.32055100540867,
-            "velocityX": 3.138455768807129,
-            "velocityY": 1.8742914320237858,
-            "timestamp": 0.46341162719822043
-        },
-        {
-            "x": 6.223708618139815,
-            "y": 2.267331907058253,
-            "heading": 3.132461891539073,
-            "angularVelocity": 9.391332530924462e-7,
-            "velocityX": 3.2160141723739923,
-            "velocityY": 1.9919562665816168,
-            "timestamp": 0.5296132882265376
-        },
-        {
-            "x": 6.436614072470765,
-            "y": 2.3992027622087693,
-            "heading": 3.1324619537111715,
-            "angularVelocity": 9.391320033179244e-7,
-            "velocityX": 3.2160137830967814,
-            "velocityY": 1.991956895070635,
-            "timestamp": 0.5958149492548548
-        },
-        {
-            "x": 6.649519526799896,
-            "y": 2.5310736173622237,
-            "heading": 3.1324620158832706,
-            "angularVelocity": 9.391320117961031e-7,
-            "velocityX": 3.2160137830692905,
-            "velocityY": 1.991956895115017,
-            "timestamp": 0.6620166102831719
-        },
-        {
-            "x": 6.862424842072437,
-            "y": 2.6629446970222306,
-            "heading": 3.132462078055361,
-            "angularVelocity": 9.391318749443678e-7,
-            "velocityX": 3.216011682569308,
-            "velocityY": 1.991960286367572,
-            "timestamp": 0.7282182713114891
-        },
-        {
-            "x": 7.065700834244428,
-            "y": 2.8092236283210066,
-            "heading": 3.1324621404036685,
-            "angularVelocity": 9.417936977148102e-7,
-            "velocityX": 3.070557279295834,
-            "velocityY": 2.209596089080417,
-            "timestamp": 0.7944199323398062
-        },
-        {
-            "x": 7.245525316742551,
-            "y": 2.9835274116090518,
-            "heading": 3.132462207800352,
-            "angularVelocity": 0.000001018051251212501,
-            "velocityX": 2.7163137556501122,
-            "velocityY": 2.632921600162078,
-            "timestamp": 0.8606215933681234
-        },
-        {
-            "x": 7.3980692436855175,
-            "y": 3.182145135413928,
-            "heading": 3.132462290722476,
-            "angularVelocity": 0.0000012525686368484786,
-            "velocityX": 2.304231111024479,
-            "velocityY": 3.0001924531762993,
-            "timestamp": 0.9268232543964405
-        },
-        {
-            "x": 7.520084601366705,
-            "y": 3.4008478887342375,
-            "heading": 3.132462412903275,
-            "angularVelocity": 0.0000018455850946514453,
-            "velocityX": 1.843086046269626,
-            "velocityY": 3.3035840781513723,
-            "timestamp": 0.9930249154247577
-        },
-        {
-            "x": 7.608973390349284,
-            "y": 3.6349790028149913,
-            "heading": 3.1324627892694235,
-            "angularVelocity": 0.000005685146605966413,
-            "velocityX": 1.3426972616976043,
-            "velocityY": 3.536635039725298,
-            "timestamp": 1.0592265764530748
-        },
-        {
-            "x": 7.662614259939079,
-            "y": 3.8787278318222707,
-            "heading": 3.1349569914257884,
-            "angularVelocity": 0.03767582440419081,
-            "velocityX": 0.8102647087184026,
-            "velocityY": 3.6819140973379696,
-            "timestamp": 1.125428237481392
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.07617646589996442,
-            "velocityX": 0.26261788285761967,
-            "velocityY": 3.7502407692104156,
-            "timestamp": 1.1916298985097091
-        },
-        {
-            "x": 7.6496343984109,
-            "y": 4.420930599340373,
-            "heading": 3.140398780477199,
-            "angularVelocity": 0.005103020206742044,
-            "velocityX": -0.38857538706933203,
-            "velocityY": 3.7613019480289434,
-            "timestamp": 1.2697758712078666
-        },
-        {
-            "x": 7.569234672947985,
-            "y": 4.705409013581856,
-            "heading": 3.140398835230542,
-            "angularVelocity": 7.00654698073033e-7,
-            "velocityX": -1.0288402931970753,
-            "velocityY": 3.640346449333809,
-            "timestamp": 1.347921843906024
-        },
-        {
-            "x": 7.441209757564435,
-            "y": 4.9718704928955155,
-            "heading": 3.1403988601468993,
-            "angularVelocity": 3.1884377743325344e-7,
-            "velocityX": -1.6382791200012203,
-            "velocityY": 3.409791574837347,
-            "timestamp": 1.4260678166041816
-        },
-        {
-            "x": 7.269357950164967,
-            "y": 5.212409598206138,
-            "heading": 3.1403988766618367,
-            "angularVelocity": 2.1133446439744448e-7,
-            "velocityX": -2.199112781707609,
-            "velocityY": 3.07807423729561,
-            "timestamp": 1.504213789302339
-        },
-        {
-            "x": 7.058777847289789,
-            "y": 5.4198900260822835,
-            "heading": 3.140398889837091,
-            "angularVelocity": 1.685979934733241e-7,
-            "velocityX": -2.6947019226259408,
-            "velocityY": 2.6550367307797926,
-            "timestamp": 1.5823597620004966
-        },
-        {
-            "x": 6.8157174042290976,
-            "y": 5.588156866874436,
-            "heading": 3.1403989019071075,
-            "angularVelocity": 1.5445475097502255e-7,
-            "velocityX": -3.110338699084864,
-            "velocityY": 2.1532375243707835,
-            "timestamp": 1.660505734698654
-        },
-        {
-            "x": 6.566201090125382,
-            "y": 5.746693038711312,
-            "heading": 3.1403989138819113,
-            "angularVelocity": 1.5323635026193936e-7,
-            "velocityX": -3.1929516709384456,
-            "velocityY": 2.0287183889726994,
-            "timestamp": 1.7386517073968115
-        },
-        {
-            "x": 6.316684733354014,
-            "y": 5.905229143394562,
-            "heading": 3.140398925856715,
-            "angularVelocity": 1.532363481603047e-7,
-            "velocityX": -3.1929522169378273,
-            "velocityY": 2.0287175296369964,
-            "timestamp": 1.816797680094969
-        },
-        {
-            "x": 6.067168376581658,
-            "y": 6.063765248076258,
-            "heading": 3.1403989378315185,
-            "angularVelocity": 1.5323634796892713e-7,
-            "velocityX": -3.1929522169504736,
-            "velocityY": 2.028717529617091,
-            "timestamp": 1.8949436527931265
-        },
-        {
-            "x": 5.817651906877993,
-            "y": 6.222301175016583,
-            "heading": 3.1403989498077705,
-            "angularVelocity": 1.5325488550289546e-7,
-            "velocityX": -3.1929536620832875,
-            "velocityY": 2.0287152551381347,
-            "timestamp": 1.973089625491284
-        },
-        {
-            "x": 5.6037668941620185,
-            "y": 6.358199133155801,
-            "heading": 3.2279940884560396,
-            "angularVelocity": 1.1209168639620608,
-            "velocityX": -2.736993415413761,
-            "velocityY": 1.7390270214456762,
-            "timestamp": 2.0512355981894417
-        },
-        {
-            "x": 5.432659669200931,
-            "y": 6.4669188463641385,
-            "heading": 3.29806779388705,
-            "angularVelocity": 0.8967027091917401,
-            "velocityX": -2.189584684318749,
-            "velocityY": 1.3912388502514341,
-            "timestamp": 2.1293815708875994
-        },
-        {
-            "x": 5.304329710503707,
-            "y": 6.548459271235515,
-            "heading": 3.3506253655164633,
-            "angularVelocity": 0.6725563687371077,
-            "velocityX": -1.6421826265173798,
-            "velocityY": 1.0434373270437258,
-            "timestamp": 2.207527543585757
-        },
-        {
-            "x": 5.218776570774893,
-            "y": 6.602819757390003,
-            "heading": 3.3856653169356727,
-            "angularVelocity": 0.4483910073584796,
-            "velocityX": -1.0947862925612357,
-            "velocityY": 0.6956274812071462,
-            "timestamp": 2.285673516283915
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.2241956896684635,
-            "velocityX": -0.5473931579317236,
-            "velocityY": 0.34781373462440857,
-            "timestamp": 2.3638194889820725
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0,
-            "velocityX": -1.468222292761223e-31,
-            "velocityY": 0,
-            "timestamp": 2.44196546168023
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S1.traj
deleted file mode 100644
index a99f9c86..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S1.traj	
+++ /dev/null
@@ -1,319 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.205167733325116,
-            "y": 1.6385570868106298,
-            "heading": 2.543052954613419,
-            "angularVelocity": 0.6503304289456182,
-            "velocityX": 0.44058914643605,
-            "velocityY": 0.2803114985693042,
-            "timestamp": 0.0662016610283172
-        },
-        {
-            "x": 5.263930522170129,
-            "y": 1.6757412702349752,
-            "heading": 2.6242045140781594,
-            "angularVelocity": 1.225823615363673,
-            "velocityX": 0.8876331489609754,
-            "velocityY": 0.5616805204997857,
-            "timestamp": 0.1324033220566344
-        },
-        {
-            "x": 5.3527880828329195,
-            "y": 1.7316217896984443,
-            "heading": 2.7372039865474704,
-            "angularVelocity": 1.706897843863404,
-            "velocityX": 1.3422255466488056,
-            "velocityY": 0.8440954289583611,
-            "timestamp": 0.19860498308495161
-        },
-        {
-            "x": 5.472262183244267,
-            "y": 1.806361940098468,
-            "heading": 2.873328504655769,
-            "angularVelocity": 2.056210010353016,
-            "velocityX": 1.804699437378857,
-            "velocityY": 1.1289769658201285,
-            "timestamp": 0.2648066441132688
-        },
-        {
-            "x": 5.622851983789364,
-            "y": 1.9002058058145468,
-            "heading": 3.0148229359693395,
-            "angularVelocity": 2.137324488779513,
-            "velocityX": 2.2747133260099135,
-            "velocityY": 1.4175454853906315,
-            "timestamp": 0.331008305141586
-        },
-        {
-            "x": 5.803032153079139,
-            "y": 2.011379887463594,
-            "heading": 3.1112408203645807,
-            "angularVelocity": 1.456426967203298,
-            "velocityX": 2.721686533102414,
-            "velocityY": 1.6793246562416593,
-            "timestamp": 0.39720996616990323
-        },
-        {
-            "x": 6.010803138038067,
-            "y": 2.135461093514753,
-            "heading": 3.1324618293668918,
-            "angularVelocity": 0.32055100540867,
-            "velocityX": 3.138455768807129,
-            "velocityY": 1.8742914320237858,
-            "timestamp": 0.46341162719822043
-        },
-        {
-            "x": 6.223708618139815,
-            "y": 2.267331907058253,
-            "heading": 3.132461891539073,
-            "angularVelocity": 9.391332530924462e-7,
-            "velocityX": 3.2160141723739923,
-            "velocityY": 1.9919562665816168,
-            "timestamp": 0.5296132882265376
-        },
-        {
-            "x": 6.436614072470765,
-            "y": 2.3992027622087693,
-            "heading": 3.1324619537111715,
-            "angularVelocity": 9.391320033179244e-7,
-            "velocityX": 3.2160137830967814,
-            "velocityY": 1.991956895070635,
-            "timestamp": 0.5958149492548548
-        },
-        {
-            "x": 6.649519526799896,
-            "y": 2.5310736173622237,
-            "heading": 3.1324620158832706,
-            "angularVelocity": 9.391320117961031e-7,
-            "velocityX": 3.2160137830692905,
-            "velocityY": 1.991956895115017,
-            "timestamp": 0.6620166102831719
-        },
-        {
-            "x": 6.862424842072437,
-            "y": 2.6629446970222306,
-            "heading": 3.132462078055361,
-            "angularVelocity": 9.391318749443678e-7,
-            "velocityX": 3.216011682569308,
-            "velocityY": 1.991960286367572,
-            "timestamp": 0.7282182713114891
-        },
-        {
-            "x": 7.065700834244428,
-            "y": 2.8092236283210066,
-            "heading": 3.1324621404036685,
-            "angularVelocity": 9.417936977148102e-7,
-            "velocityX": 3.070557279295834,
-            "velocityY": 2.209596089080417,
-            "timestamp": 0.7944199323398062
-        },
-        {
-            "x": 7.245525316742551,
-            "y": 2.9835274116090518,
-            "heading": 3.132462207800352,
-            "angularVelocity": 0.000001018051251212501,
-            "velocityX": 2.7163137556501122,
-            "velocityY": 2.632921600162078,
-            "timestamp": 0.8606215933681234
-        },
-        {
-            "x": 7.3980692436855175,
-            "y": 3.182145135413928,
-            "heading": 3.132462290722476,
-            "angularVelocity": 0.0000012525686368484786,
-            "velocityX": 2.304231111024479,
-            "velocityY": 3.0001924531762993,
-            "timestamp": 0.9268232543964405
-        },
-        {
-            "x": 7.520084601366705,
-            "y": 3.4008478887342375,
-            "heading": 3.132462412903275,
-            "angularVelocity": 0.0000018455850946514453,
-            "velocityX": 1.843086046269626,
-            "velocityY": 3.3035840781513723,
-            "timestamp": 0.9930249154247577
-        },
-        {
-            "x": 7.608973390349284,
-            "y": 3.6349790028149913,
-            "heading": 3.1324627892694235,
-            "angularVelocity": 0.000005685146605966413,
-            "velocityX": 1.3426972616976043,
-            "velocityY": 3.536635039725298,
-            "timestamp": 1.0592265764530748
-        },
-        {
-            "x": 7.662614259939079,
-            "y": 3.8787278318222707,
-            "heading": 3.1349569914257884,
-            "angularVelocity": 0.03767582440419081,
-            "velocityX": 0.8102647087184026,
-            "velocityY": 3.6819140973379696,
-            "timestamp": 1.125428237481392
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.07617646589996442,
-            "velocityX": 0.26261788285761967,
-            "velocityY": 3.7502407692104156,
-            "timestamp": 1.1916298985097091
-        },
-        {
-            "x": 7.6496343984109,
-            "y": 4.420930599340373,
-            "heading": 3.140398780477199,
-            "angularVelocity": 0.005103020206742044,
-            "velocityX": -0.38857538706933203,
-            "velocityY": 3.7613019480289434,
-            "timestamp": 1.2697758712078666
-        },
-        {
-            "x": 7.569234672947985,
-            "y": 4.705409013581856,
-            "heading": 3.140398835230542,
-            "angularVelocity": 7.00654698073033e-7,
-            "velocityX": -1.0288402931970753,
-            "velocityY": 3.640346449333809,
-            "timestamp": 1.347921843906024
-        },
-        {
-            "x": 7.441209757564435,
-            "y": 4.9718704928955155,
-            "heading": 3.1403988601468993,
-            "angularVelocity": 3.1884377743325344e-7,
-            "velocityX": -1.6382791200012203,
-            "velocityY": 3.409791574837347,
-            "timestamp": 1.4260678166041816
-        },
-        {
-            "x": 7.269357950164967,
-            "y": 5.212409598206138,
-            "heading": 3.1403988766618367,
-            "angularVelocity": 2.1133446439744448e-7,
-            "velocityX": -2.199112781707609,
-            "velocityY": 3.07807423729561,
-            "timestamp": 1.504213789302339
-        },
-        {
-            "x": 7.058777847289789,
-            "y": 5.4198900260822835,
-            "heading": 3.140398889837091,
-            "angularVelocity": 1.685979934733241e-7,
-            "velocityX": -2.6947019226259408,
-            "velocityY": 2.6550367307797926,
-            "timestamp": 1.5823597620004966
-        },
-        {
-            "x": 6.8157174042290976,
-            "y": 5.588156866874436,
-            "heading": 3.1403989019071075,
-            "angularVelocity": 1.5445475097502255e-7,
-            "velocityX": -3.110338699084864,
-            "velocityY": 2.1532375243707835,
-            "timestamp": 1.660505734698654
-        },
-        {
-            "x": 6.566201090125382,
-            "y": 5.746693038711312,
-            "heading": 3.1403989138819113,
-            "angularVelocity": 1.5323635026193936e-7,
-            "velocityX": -3.1929516709384456,
-            "velocityY": 2.0287183889726994,
-            "timestamp": 1.7386517073968115
-        },
-        {
-            "x": 6.316684733354014,
-            "y": 5.905229143394562,
-            "heading": 3.140398925856715,
-            "angularVelocity": 1.532363481603047e-7,
-            "velocityX": -3.1929522169378273,
-            "velocityY": 2.0287175296369964,
-            "timestamp": 1.816797680094969
-        },
-        {
-            "x": 6.067168376581658,
-            "y": 6.063765248076258,
-            "heading": 3.1403989378315185,
-            "angularVelocity": 1.5323634796892713e-7,
-            "velocityX": -3.1929522169504736,
-            "velocityY": 2.028717529617091,
-            "timestamp": 1.8949436527931265
-        },
-        {
-            "x": 5.817651906877993,
-            "y": 6.222301175016583,
-            "heading": 3.1403989498077705,
-            "angularVelocity": 1.5325488550289546e-7,
-            "velocityX": -3.1929536620832875,
-            "velocityY": 2.0287152551381347,
-            "timestamp": 1.973089625491284
-        },
-        {
-            "x": 5.6037668941620185,
-            "y": 6.358199133155801,
-            "heading": 3.2279940884560396,
-            "angularVelocity": 1.1209168639620608,
-            "velocityX": -2.736993415413761,
-            "velocityY": 1.7390270214456762,
-            "timestamp": 2.0512355981894417
-        },
-        {
-            "x": 5.432659669200931,
-            "y": 6.4669188463641385,
-            "heading": 3.29806779388705,
-            "angularVelocity": 0.8967027091917401,
-            "velocityX": -2.189584684318749,
-            "velocityY": 1.3912388502514341,
-            "timestamp": 2.1293815708875994
-        },
-        {
-            "x": 5.304329710503707,
-            "y": 6.548459271235515,
-            "heading": 3.3506253655164633,
-            "angularVelocity": 0.6725563687371077,
-            "velocityX": -1.6421826265173798,
-            "velocityY": 1.0434373270437258,
-            "timestamp": 2.207527543585757
-        },
-        {
-            "x": 5.218776570774893,
-            "y": 6.602819757390003,
-            "heading": 3.3856653169356727,
-            "angularVelocity": 0.4483910073584796,
-            "velocityX": -1.0947862925612357,
-            "velocityY": 0.6956274812071462,
-            "timestamp": 2.285673516283915
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.2241956896684635,
-            "velocityX": -0.5473931579317236,
-            "velocityY": 0.34781373462440857,
-            "timestamp": 2.3638194889820725
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0,
-            "velocityX": -1.468222292761223e-31,
-            "velocityY": 0,
-            "timestamp": 2.44196546168023
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S2.1.traj
deleted file mode 100644
index 5b390158..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S2.1.traj	
+++ /dev/null
@@ -1,310 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.210251836832851,
-            "y": 1.636837042886923,
-            "heading": 2.532387023792424,
-            "angularVelocity": 0.47220005236446794,
-            "velocityX": 0.4993888678908201,
-            "velocityY": 0.24548265329423538,
-            "timestamp": 0.06858750571977071
-        },
-        {
-            "x": 5.278911164181792,
-            "y": 1.6705273462343695,
-            "heading": 2.5947497489204117,
-            "angularVelocity": 0.909243228391831,
-            "velocityX": 1.0010471532448244,
-            "velocityY": 0.4912017574322581,
-            "timestamp": 0.13717501143954142
-        },
-        {
-            "x": 5.382181986675021,
-            "y": 1.7210991538184346,
-            "heading": 2.6836393944616295,
-            "angularVelocity": 1.2960034718917393,
-            "velocityX": 1.5056798087273409,
-            "velocityY": 0.7373326534236055,
-            "timestamp": 0.20576251715931212
-        },
-        {
-            "x": 5.520335944035756,
-            "y": 1.7886220906689136,
-            "heading": 2.793394080133565,
-            "angularVelocity": 1.6002139824177626,
-            "velocityX": 2.0142729482712705,
-            "velocityY": 0.9844786764277269,
-            "timestamp": 0.27435002287908283
-        },
-        {
-            "x": 5.693737584114165,
-            "y": 1.8732111914756295,
-            "heading": 2.9110576970550115,
-            "angularVelocity": 1.7155255273779335,
-            "velocityX": 2.5281811644657237,
-            "velocityY": 1.2333018954257229,
-            "timestamp": 0.3429375285988535
-        },
-        {
-            "x": 5.900852745156476,
-            "y": 1.9734126829741925,
-            "heading": 2.983977883284084,
-            "angularVelocity": 1.0631701133294391,
-            "velocityX": 3.0197214327712314,
-            "velocityY": 1.4609292238729052,
-            "timestamp": 0.4115250343186242
-        },
-        {
-            "x": 6.133968719369523,
-            "y": 2.087332400713016,
-            "heading": 2.9839779219365523,
-            "angularVelocity": 5.635497037494653e-7,
-            "velocityX": 3.3988110774212195,
-            "velocityY": 1.6609397957153824,
-            "timestamp": 0.4801125400383949
-        },
-        {
-            "x": 6.367084623966495,
-            "y": 2.2012522609094827,
-            "heading": 2.9839779605877594,
-            "angularVelocity": 5.635313124243683e-7,
-            "velocityX": 3.3988100624247526,
-            "velocityY": 1.660941872735705,
-            "timestamp": 0.5487000457581656
-        },
-        {
-            "x": 6.600200528555652,
-            "y": 2.315172121121942,
-            "heading": 2.9839779992389666,
-            "angularVelocity": 5.635313147177628e-7,
-            "velocityX": 3.3988100623108077,
-            "velocityY": 1.6609418729688719,
-            "timestamp": 0.6172875514779362
-        },
-        {
-            "x": 6.833315692249175,
-            "y": 2.4290934974276186,
-            "heading": 2.983978037890185,
-            "angularVelocity": 5.635314723100833e-7,
-            "velocityX": 3.398799260115481,
-            "velocityY": 1.6609639774790368,
-            "timestamp": 0.6858750571977069
-        },
-        {
-            "x": 7.049439158690477,
-            "y": 2.572660228674406,
-            "heading": 2.9839780777366456,
-            "angularVelocity": 5.809580085952453e-7,
-            "velocityX": 3.151061759328619,
-            "velocityY": 2.093190731171372,
-            "timestamp": 0.7544625629174776
-        },
-        {
-            "x": 7.241451301040205,
-            "y": 2.747165513732945,
-            "heading": 2.983978123402154,
-            "angularVelocity": 6.657992258942737e-7,
-            "velocityX": 2.7995207047510204,
-            "velocityY": 2.5442722144105754,
-            "timestamp": 0.8230500686372483
-        },
-        {
-            "x": 7.4049637163298945,
-            "y": 2.9486213241330868,
-            "heading": 2.98397818333491,
-            "angularVelocity": 8.738144839700177e-7,
-            "velocityX": 2.383997108128638,
-            "velocityY": 2.9372085817383917,
-            "timestamp": 0.891637574357019
-        },
-        {
-            "x": 7.536239247186927,
-            "y": 3.1724233234500354,
-            "heading": 2.983979044678588,
-            "angularVelocity": 0.000012558317568342283,
-            "velocityX": 1.9139860748601447,
-            "velocityY": 3.263014115593316,
-            "timestamp": 0.9602250800767896
-        },
-        {
-            "x": 7.630290050662344,
-            "y": 3.4075679677071538,
-            "heading": 3.001663168154554,
-            "angularVelocity": 0.25783301623794963,
-            "velocityX": 1.371252715613873,
-            "velocityY": 3.428388914124583,
-            "timestamp": 1.0288125857965604
-        },
-        {
-            "x": 7.68591505099028,
-            "y": 3.648060804187146,
-            "heading": 3.0401905596367595,
-            "angularVelocity": 0.5617260910408102,
-            "velocityX": 0.8110077738532104,
-            "velocityY": 3.5063651018682354,
-            "timestamp": 1.097400091516331
-        },
-        {
-            "x": 7.702477054476245,
-            "y": 3.8894159117324456,
-            "heading": 3.0910773123993565,
-            "angularVelocity": 0.7419245273403817,
-            "velocityX": 0.24147260222047945,
-            "velocityY": 3.5189369406639295,
-            "timestamp": 1.1659875972361018
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.7132886243236253,
-            "velocityX": -0.3277135425813607,
-            "velocityY": 3.463955800321066,
-            "timestamp": 1.2345751029558725
-        },
-        {
-            "x": 7.5839717767270045,
-            "y": 4.427707701571831,
-            "heading": 3.18571906595995,
-            "angularVelocity": 0.5075163501765089,
-            "velocityX": -1.0659861999835865,
-            "velocityY": 3.338083838051171,
-            "timestamp": 1.3246590301910224
-        },
-        {
-            "x": 7.4226682910577315,
-            "y": 4.711490758148744,
-            "heading": 3.2114421388245056,
-            "angularVelocity": 0.28554564231429624,
-            "velocityX": -1.7905911811350714,
-            "velocityY": 3.1502074264162947,
-            "timestamp": 1.4147429574261723
-        },
-        {
-            "x": 7.199929791587311,
-            "y": 4.9676132378837865,
-            "heading": 3.2147906679837175,
-            "angularVelocity": 0.03717121646428904,
-            "velocityX": -2.472566486682988,
-            "velocityY": 2.8431540186572435,
-            "timestamp": 1.5048268846613222
-        },
-        {
-            "x": 6.929834673893736,
-            "y": 5.175413812947385,
-            "heading": 3.214790655255866,
-            "angularVelocity": -1.4128881851850989e-7,
-            "velocityX": -2.998260910501095,
-            "velocityY": 2.306744182246487,
-            "timestamp": 1.594910811896472
-        },
-        {
-            "x": 6.624007279414812,
-            "y": 5.325753649029949,
-            "heading": 3.2147906451916457,
-            "angularVelocity": -1.1172048505786767e-7,
-            "velocityX": -3.394916317098503,
-            "velocityY": 1.6688863451759368,
-            "timestamp": 1.684994739131622
-        },
-        {
-            "x": 6.294504922277233,
-            "y": 5.412705714996136,
-            "heading": 3.2147906373147057,
-            "angularVelocity": -8.744001355884246e-8,
-            "velocityX": -3.6577263808388842,
-            "velocityY": 0.9652339616501584,
-            "timestamp": 1.775078666366772
-        },
-        {
-            "x": 5.954617378458506,
-            "y": 5.437382093793352,
-            "heading": 3.214790629982673,
-            "angularVelocity": -8.139113095546486e-8,
-            "velocityX": -3.773009839274702,
-            "velocityY": 0.273926543331081,
-            "timestamp": 1.8651625936019218
-        },
-        {
-            "x": 5.614729638884886,
-            "y": 5.46205577615271,
-            "heading": 3.214790622650651,
-            "angularVelocity": -8.139101337806881e-8,
-            "velocityX": -3.7730120123026722,
-            "velocityY": 0.273896610823274,
-            "timestamp": 1.9552465208370717
-        },
-        {
-            "x": 5.27484189928346,
-            "y": 5.486729458129014,
-            "heading": 3.214790615318626,
-            "angularVelocity": -8.139103977990715e-8,
-            "velocityX": -3.7730120126113382,
-            "velocityY": 0.27389660657108617,
-            "timestamp": 2.0453304480722214
-        },
-        {
-            "x": 4.937591000410069,
-            "y": 5.511126660727889,
-            "heading": 3.206674021661731,
-            "angularVelocity": -0.09010035314854972,
-            "velocityX": -3.7437410781731515,
-            "velocityY": 0.27082747552945735,
-            "timestamp": 2.1354143753073713
-        },
-        {
-            "x": 4.66772599134931,
-            "y": 5.5306707771302355,
-            "heading": 3.1851297651856467,
-            "angularVelocity": -0.23915760710394238,
-            "velocityX": -2.9957065299375607,
-            "velocityY": 0.21695453342447346,
-            "timestamp": 2.2254983025425212
-        },
-        {
-            "x": 4.465349331515348,
-            "y": 5.545333951302465,
-            "heading": 3.164279125670826,
-            "angularVelocity": -0.23145793211693713,
-            "velocityX": -2.246534604399402,
-            "velocityY": 0.16277236819342672,
-            "timestamp": 2.315582229777671
-        },
-        {
-            "x": 4.330445855865838,
-            "y": 5.555111001202187,
-            "heading": 3.1485187249188784,
-            "angularVelocity": -0.17495241643725573,
-            "velocityX": -1.4975310223472684,
-            "velocityY": 0.10853267835671068,
-            "timestamp": 2.405666157012821
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -0.09456431552591144,
-            "velocityX": -0.7487002169630299,
-            "velocityY": 0.054271599250453016,
-            "timestamp": 2.495750084247971
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -3.2898689544721607e-31,
-            "velocityX": 6.680323432143241e-33,
-            "velocityY": 0,
-            "timestamp": 2.585834011483121
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S2.traj
deleted file mode 100644
index 5b390158..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S2.traj	
+++ /dev/null
@@ -1,310 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.210251836832851,
-            "y": 1.636837042886923,
-            "heading": 2.532387023792424,
-            "angularVelocity": 0.47220005236446794,
-            "velocityX": 0.4993888678908201,
-            "velocityY": 0.24548265329423538,
-            "timestamp": 0.06858750571977071
-        },
-        {
-            "x": 5.278911164181792,
-            "y": 1.6705273462343695,
-            "heading": 2.5947497489204117,
-            "angularVelocity": 0.909243228391831,
-            "velocityX": 1.0010471532448244,
-            "velocityY": 0.4912017574322581,
-            "timestamp": 0.13717501143954142
-        },
-        {
-            "x": 5.382181986675021,
-            "y": 1.7210991538184346,
-            "heading": 2.6836393944616295,
-            "angularVelocity": 1.2960034718917393,
-            "velocityX": 1.5056798087273409,
-            "velocityY": 0.7373326534236055,
-            "timestamp": 0.20576251715931212
-        },
-        {
-            "x": 5.520335944035756,
-            "y": 1.7886220906689136,
-            "heading": 2.793394080133565,
-            "angularVelocity": 1.6002139824177626,
-            "velocityX": 2.0142729482712705,
-            "velocityY": 0.9844786764277269,
-            "timestamp": 0.27435002287908283
-        },
-        {
-            "x": 5.693737584114165,
-            "y": 1.8732111914756295,
-            "heading": 2.9110576970550115,
-            "angularVelocity": 1.7155255273779335,
-            "velocityX": 2.5281811644657237,
-            "velocityY": 1.2333018954257229,
-            "timestamp": 0.3429375285988535
-        },
-        {
-            "x": 5.900852745156476,
-            "y": 1.9734126829741925,
-            "heading": 2.983977883284084,
-            "angularVelocity": 1.0631701133294391,
-            "velocityX": 3.0197214327712314,
-            "velocityY": 1.4609292238729052,
-            "timestamp": 0.4115250343186242
-        },
-        {
-            "x": 6.133968719369523,
-            "y": 2.087332400713016,
-            "heading": 2.9839779219365523,
-            "angularVelocity": 5.635497037494653e-7,
-            "velocityX": 3.3988110774212195,
-            "velocityY": 1.6609397957153824,
-            "timestamp": 0.4801125400383949
-        },
-        {
-            "x": 6.367084623966495,
-            "y": 2.2012522609094827,
-            "heading": 2.9839779605877594,
-            "angularVelocity": 5.635313124243683e-7,
-            "velocityX": 3.3988100624247526,
-            "velocityY": 1.660941872735705,
-            "timestamp": 0.5487000457581656
-        },
-        {
-            "x": 6.600200528555652,
-            "y": 2.315172121121942,
-            "heading": 2.9839779992389666,
-            "angularVelocity": 5.635313147177628e-7,
-            "velocityX": 3.3988100623108077,
-            "velocityY": 1.6609418729688719,
-            "timestamp": 0.6172875514779362
-        },
-        {
-            "x": 6.833315692249175,
-            "y": 2.4290934974276186,
-            "heading": 2.983978037890185,
-            "angularVelocity": 5.635314723100833e-7,
-            "velocityX": 3.398799260115481,
-            "velocityY": 1.6609639774790368,
-            "timestamp": 0.6858750571977069
-        },
-        {
-            "x": 7.049439158690477,
-            "y": 2.572660228674406,
-            "heading": 2.9839780777366456,
-            "angularVelocity": 5.809580085952453e-7,
-            "velocityX": 3.151061759328619,
-            "velocityY": 2.093190731171372,
-            "timestamp": 0.7544625629174776
-        },
-        {
-            "x": 7.241451301040205,
-            "y": 2.747165513732945,
-            "heading": 2.983978123402154,
-            "angularVelocity": 6.657992258942737e-7,
-            "velocityX": 2.7995207047510204,
-            "velocityY": 2.5442722144105754,
-            "timestamp": 0.8230500686372483
-        },
-        {
-            "x": 7.4049637163298945,
-            "y": 2.9486213241330868,
-            "heading": 2.98397818333491,
-            "angularVelocity": 8.738144839700177e-7,
-            "velocityX": 2.383997108128638,
-            "velocityY": 2.9372085817383917,
-            "timestamp": 0.891637574357019
-        },
-        {
-            "x": 7.536239247186927,
-            "y": 3.1724233234500354,
-            "heading": 2.983979044678588,
-            "angularVelocity": 0.000012558317568342283,
-            "velocityX": 1.9139860748601447,
-            "velocityY": 3.263014115593316,
-            "timestamp": 0.9602250800767896
-        },
-        {
-            "x": 7.630290050662344,
-            "y": 3.4075679677071538,
-            "heading": 3.001663168154554,
-            "angularVelocity": 0.25783301623794963,
-            "velocityX": 1.371252715613873,
-            "velocityY": 3.428388914124583,
-            "timestamp": 1.0288125857965604
-        },
-        {
-            "x": 7.68591505099028,
-            "y": 3.648060804187146,
-            "heading": 3.0401905596367595,
-            "angularVelocity": 0.5617260910408102,
-            "velocityX": 0.8110077738532104,
-            "velocityY": 3.5063651018682354,
-            "timestamp": 1.097400091516331
-        },
-        {
-            "x": 7.702477054476245,
-            "y": 3.8894159117324456,
-            "heading": 3.0910773123993565,
-            "angularVelocity": 0.7419245273403817,
-            "velocityX": 0.24147260222047945,
-            "velocityY": 3.5189369406639295,
-            "timestamp": 1.1659875972361018
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.7132886243236253,
-            "velocityX": -0.3277135425813607,
-            "velocityY": 3.463955800321066,
-            "timestamp": 1.2345751029558725
-        },
-        {
-            "x": 7.5839717767270045,
-            "y": 4.427707701571831,
-            "heading": 3.18571906595995,
-            "angularVelocity": 0.5075163501765089,
-            "velocityX": -1.0659861999835865,
-            "velocityY": 3.338083838051171,
-            "timestamp": 1.3246590301910224
-        },
-        {
-            "x": 7.4226682910577315,
-            "y": 4.711490758148744,
-            "heading": 3.2114421388245056,
-            "angularVelocity": 0.28554564231429624,
-            "velocityX": -1.7905911811350714,
-            "velocityY": 3.1502074264162947,
-            "timestamp": 1.4147429574261723
-        },
-        {
-            "x": 7.199929791587311,
-            "y": 4.9676132378837865,
-            "heading": 3.2147906679837175,
-            "angularVelocity": 0.03717121646428904,
-            "velocityX": -2.472566486682988,
-            "velocityY": 2.8431540186572435,
-            "timestamp": 1.5048268846613222
-        },
-        {
-            "x": 6.929834673893736,
-            "y": 5.175413812947385,
-            "heading": 3.214790655255866,
-            "angularVelocity": -1.4128881851850989e-7,
-            "velocityX": -2.998260910501095,
-            "velocityY": 2.306744182246487,
-            "timestamp": 1.594910811896472
-        },
-        {
-            "x": 6.624007279414812,
-            "y": 5.325753649029949,
-            "heading": 3.2147906451916457,
-            "angularVelocity": -1.1172048505786767e-7,
-            "velocityX": -3.394916317098503,
-            "velocityY": 1.6688863451759368,
-            "timestamp": 1.684994739131622
-        },
-        {
-            "x": 6.294504922277233,
-            "y": 5.412705714996136,
-            "heading": 3.2147906373147057,
-            "angularVelocity": -8.744001355884246e-8,
-            "velocityX": -3.6577263808388842,
-            "velocityY": 0.9652339616501584,
-            "timestamp": 1.775078666366772
-        },
-        {
-            "x": 5.954617378458506,
-            "y": 5.437382093793352,
-            "heading": 3.214790629982673,
-            "angularVelocity": -8.139113095546486e-8,
-            "velocityX": -3.773009839274702,
-            "velocityY": 0.273926543331081,
-            "timestamp": 1.8651625936019218
-        },
-        {
-            "x": 5.614729638884886,
-            "y": 5.46205577615271,
-            "heading": 3.214790622650651,
-            "angularVelocity": -8.139101337806881e-8,
-            "velocityX": -3.7730120123026722,
-            "velocityY": 0.273896610823274,
-            "timestamp": 1.9552465208370717
-        },
-        {
-            "x": 5.27484189928346,
-            "y": 5.486729458129014,
-            "heading": 3.214790615318626,
-            "angularVelocity": -8.139103977990715e-8,
-            "velocityX": -3.7730120126113382,
-            "velocityY": 0.27389660657108617,
-            "timestamp": 2.0453304480722214
-        },
-        {
-            "x": 4.937591000410069,
-            "y": 5.511126660727889,
-            "heading": 3.206674021661731,
-            "angularVelocity": -0.09010035314854972,
-            "velocityX": -3.7437410781731515,
-            "velocityY": 0.27082747552945735,
-            "timestamp": 2.1354143753073713
-        },
-        {
-            "x": 4.66772599134931,
-            "y": 5.5306707771302355,
-            "heading": 3.1851297651856467,
-            "angularVelocity": -0.23915760710394238,
-            "velocityX": -2.9957065299375607,
-            "velocityY": 0.21695453342447346,
-            "timestamp": 2.2254983025425212
-        },
-        {
-            "x": 4.465349331515348,
-            "y": 5.545333951302465,
-            "heading": 3.164279125670826,
-            "angularVelocity": -0.23145793211693713,
-            "velocityX": -2.246534604399402,
-            "velocityY": 0.16277236819342672,
-            "timestamp": 2.315582229777671
-        },
-        {
-            "x": 4.330445855865838,
-            "y": 5.555111001202187,
-            "heading": 3.1485187249188784,
-            "angularVelocity": -0.17495241643725573,
-            "velocityX": -1.4975310223472684,
-            "velocityY": 0.10853267835671068,
-            "timestamp": 2.405666157012821
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -0.09456431552591144,
-            "velocityX": -0.7487002169630299,
-            "velocityY": 0.054271599250453016,
-            "timestamp": 2.495750084247971
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -3.2898689544721607e-31,
-            "velocityX": 6.680323432143241e-33,
-            "velocityY": 0,
-            "timestamp": 2.585834011483121
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S3.1.traj
deleted file mode 100644
index 06ec0a62..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S3.1.traj	
+++ /dev/null
@@ -1,328 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -5.457496407482876e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 5.213718688496592,
-            "y": 1.6454507753697194,
-            "heading": 2.523944374487045,
-            "angularVelocity": 0.3224093526452332,
-            "velocityX": 0.5078795417016481,
-            "velocityY": 0.3426929367359288,
-            "timestamp": 0.07426699719034811
-        },
-        {
-            "x": 5.289237393773012,
-            "y": 1.696398991795747,
-            "heading": 2.5693854471889703,
-            "angularVelocity": 0.6118609129363125,
-            "velocityX": 1.0168541631333754,
-            "velocityY": 0.6860142237264,
-            "timestamp": 0.14853399438069623
-        },
-        {
-            "x": 5.402661733248807,
-            "y": 1.772907618565081,
-            "heading": 2.632642220499669,
-            "angularVelocity": 0.8517480940904348,
-            "velocityX": 1.5272509158420084,
-            "velocityY": 1.0301833878275757,
-            "timestamp": 0.22280099157104433
-        },
-        {
-            "x": 5.554123710239057,
-            "y": 1.8750622586328765,
-            "heading": 2.707444867040942,
-            "angularVelocity": 1.007212481602717,
-            "velocityX": 2.039425084092856,
-            "velocityY": 1.3755051898216737,
-            "timestamp": 0.29706798876139245
-        },
-        {
-            "x": 5.743710865909146,
-            "y": 2.002930120234068,
-            "heading": 2.7801295579879124,
-            "angularVelocity": 0.9786943554574798,
-            "velocityX": 2.552777988103829,
-            "velocityY": 1.7217319460683576,
-            "timestamp": 0.3713349859517406
-        },
-        {
-            "x": 5.969240710205324,
-            "y": 2.1555656332559185,
-            "heading": 2.8014617866621823,
-            "angularVelocity": 0.2872369892591052,
-            "velocityX": 3.036743813919657,
-            "velocityY": 2.055226665899016,
-            "timestamp": 0.4456019831420887
-        },
-        {
-            "x": 6.202237157849221,
-            "y": 2.312550420967927,
-            "heading": 2.8014618141625047,
-            "angularVelocity": 3.702899476409415e-7,
-            "velocityX": 3.137281113530435,
-            "velocityY": 2.1137893499268903,
-            "timestamp": 0.5198689803324368
-        },
-        {
-            "x": 6.435233608100515,
-            "y": 2.4695352048100654,
-            "heading": 2.8014618416628014,
-            "angularVelocity": 3.7028959815407443e-7,
-            "velocityX": 3.137281148638853,
-            "velocityY": 2.1137892978193555,
-            "timestamp": 0.5941359775227849
-        },
-        {
-            "x": 6.6682300583517184,
-            "y": 2.6265199886523383,
-            "heading": 2.801461869163098,
-            "angularVelocity": 3.702895988133056e-7,
-            "velocityX": 3.137281148637628,
-            "velocityY": 2.1137892978211736,
-            "timestamp": 0.668402974713133
-        },
-        {
-            "x": 6.901226494182671,
-            "y": 2.7835047938971096,
-            "heading": 2.801461896663395,
-            "angularVelocity": 3.702895996177554e-7,
-            "velocityX": 3.137280954469964,
-            "velocityY": 2.1137895860043394,
-            "timestamp": 0.7426699719034812
-        },
-        {
-            "x": 7.132347068567706,
-            "y": 2.9432384730568093,
-            "heading": 2.801461924236189,
-            "angularVelocity": 3.712657746616385e-7,
-            "velocityX": 3.1120226093518935,
-            "velocityY": 2.1508029838650415,
-            "timestamp": 0.8169369690938293
-        },
-        {
-            "x": 7.334313444212558,
-            "y": 3.1385360610663486,
-            "heading": 2.8014619668447454,
-            "angularVelocity": 5.737212724741241e-7,
-            "velocityX": 2.7194633321070794,
-            "velocityY": 2.6296685660925183,
-            "timestamp": 0.8912039662841774
-        },
-        {
-            "x": 7.4948116486140535,
-            "y": 3.3502854712542645,
-            "heading": 2.8423857101264045,
-            "angularVelocity": 0.5510353835468871,
-            "velocityX": 2.161097263568294,
-            "velocityY": 2.8511912181557233,
-            "timestamp": 0.9654709634745255
-        },
-        {
-            "x": 7.612584040336913,
-            "y": 3.548974342070663,
-            "heading": 2.9223254670447334,
-            "angularVelocity": 1.0763833188709784,
-            "velocityX": 1.5857971397578559,
-            "velocityY": 2.675331955419639,
-            "timestamp": 1.0397379606648736
-        },
-        {
-            "x": 7.689064457271449,
-            "y": 3.727414986995987,
-            "heading": 3.0016093061540334,
-            "angularVelocity": 1.0675514307666671,
-            "velocityX": 1.0298035443457485,
-            "velocityY": 2.402690988945958,
-            "timestamp": 1.1140049578552216
-        },
-        {
-            "x": 7.725394752128812,
-            "y": 3.8837274225386396,
-            "heading": 3.06785364542737,
-            "angularVelocity": 0.8919754639271454,
-            "velocityX": 0.4891849170129639,
-            "velocityY": 2.1047361742931416,
-            "timestamp": 1.1882719550455696
-        },
-        {
-            "x": 7.722234181622283,
-            "y": 4.017077206893078,
-            "heading": 3.1151383358166123,
-            "angularVelocity": 0.6366850980665145,
-            "velocityX": -0.04255686409979938,
-            "velocityY": 1.7955456582236569,
-            "timestamp": 1.2625389522359176
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.33476059520309653,
-            "velocityX": -0.5686803455111581,
-            "velocityY": 1.4801028352497836,
-            "timestamp": 1.3368059494262656
-        },
-        {
-            "x": 7.583164521123183,
-            "y": 4.220739525524416,
-            "heading": 3.1367175382385155,
-            "angularVelocity": -0.03922526988843187,
-            "velocityX": -1.1571796016903517,
-            "velocityY": 1.1201830988719972,
-            "timestamp": 1.4204882738773397
-        },
-        {
-            "x": 7.437331010349068,
-            "y": 4.2837766667768715,
-            "heading": 3.1044919064382244,
-            "angularVelocity": -0.38509484543694944,
-            "velocityX": -1.742703871226464,
-            "velocityY": 0.753290992643391,
-            "timestamp": 1.5041705983284137
-        },
-        {
-            "x": 7.242933099071244,
-            "y": 4.315173048252524,
-            "heading": 3.047144336604056,
-            "angularVelocity": -0.6853008710064864,
-            "velocityX": -2.3230462651820925,
-            "velocityY": 0.3751853414876064,
-            "timestamp": 1.5878529227794878
-        },
-        {
-            "x": 7.00089933731331,
-            "y": 4.3131727597182845,
-            "heading": 2.9720298467353206,
-            "angularVelocity": -0.8976147634695787,
-            "velocityX": -2.8922925282679244,
-            "velocityY": -0.023903357696631062,
-            "timestamp": 1.6715352472305618
-        },
-        {
-            "x": 6.7143889772576815,
-            "y": 4.273350582816976,
-            "heading": 2.8998851691901093,
-            "angularVelocity": -0.8621256402526404,
-            "velocityX": -3.4237858703738695,
-            "velocityY": -0.47587321650692566,
-            "timestamp": 1.7552175716816358
-        },
-        {
-            "x": 6.411635278119781,
-            "y": 4.1808641012010535,
-            "heading": 2.8998851130559236,
-            "angularVelocity": -6.708009886588717e-7,
-            "velocityX": -3.6178930392272837,
-            "velocityY": -1.1052092807244585,
-            "timestamp": 1.8388998961327099
-        },
-        {
-            "x": 6.131017053711007,
-            "y": 4.034347987511598,
-            "heading": 2.8998850873919646,
-            "angularVelocity": -3.066831498282175e-7,
-            "velocityX": -3.3533751153487676,
-            "velocityY": -1.7508609452539547,
-            "timestamp": 1.922582220583784
-        },
-        {
-            "x": 5.854044563819553,
-            "y": 3.8810515447858025,
-            "heading": 2.8998850618676295,
-            "angularVelocity": -3.050146543457835e-7,
-            "velocityX": -3.3098087524252473,
-            "velocityY": -1.8318855711927797,
-            "timestamp": 2.006264545034858
-        },
-        {
-            "x": 5.577072094171527,
-            "y": 3.7277550654846525,
-            "heading": 2.8998850363432944,
-            "angularVelocity": -3.0501465285690934e-7,
-            "velocityX": -3.3098085105171973,
-            "velocityY": -1.8318860082666197,
-            "timestamp": 2.089946869485932
-        },
-        {
-            "x": 5.300099624523611,
-            "y": 3.5744585861833027,
-            "heading": 2.8998850108189593,
-            "angularVelocity": -3.050146519273835e-7,
-            "velocityX": -3.3098085105158774,
-            "velocityY": -1.8318860082690038,
-            "timestamp": 2.173629193937006
-        },
-        {
-            "x": 5.0231271554381145,
-            "y": 3.421162105865856,
-            "heading": 2.899884985294552,
-            "angularVelocity": -3.050155161997765e-7,
-            "velocityX": -3.3098085037949927,
-            "velocityY": -1.831886020411314,
-            "timestamp": 2.25731151838808
-        },
-        {
-            "x": 4.7695419048400876,
-            "y": 3.2806617941163765,
-            "heading": 2.834618611781297,
-            "angularVelocity": -0.7799302175385181,
-            "velocityX": -3.030332298504562,
-            "velocityY": -1.678972383608021,
-            "timestamp": 2.340993842839154
-        },
-        {
-            "x": 4.566605904742026,
-            "y": 3.1682378317487854,
-            "heading": 2.7427589462823403,
-            "angularVelocity": -1.0977188564194784,
-            "velocityX": -2.4250760411980474,
-            "velocityY": -1.3434612757837647,
-            "timestamp": 2.424676167290228
-        },
-        {
-            "x": 4.414672096834118,
-            "y": 3.08405727220976,
-            "heading": 2.6624278185804466,
-            "angularVelocity": -0.959953350111109,
-            "velocityX": -1.8156021466246228,
-            "velocityY": -1.0059538868120508,
-            "timestamp": 2.508358491741302
-        },
-        {
-            "x": 4.313522085490217,
-            "y": 3.0280030162190834,
-            "heading": 2.6044623353701315,
-            "angularVelocity": -0.6926849079605295,
-            "velocityX": -1.2087380699258492,
-            "velocityY": -0.6698458289533948,
-            "timestamp": 2.5920408161923763
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -0.3640235326869004,
-            "velocityX": -0.6037366411798936,
-            "velocityY": -0.3346347798387892,
-            "timestamp": 2.6757231406434503
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 3.2974830734555563e-32,
-            "velocityY": 2.8302357269459625e-32,
-            "timestamp": 2.7594054650945243
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S3.traj
deleted file mode 100644
index 06ec0a62..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S3.traj	
+++ /dev/null
@@ -1,328 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -5.457496407482876e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 5.213718688496592,
-            "y": 1.6454507753697194,
-            "heading": 2.523944374487045,
-            "angularVelocity": 0.3224093526452332,
-            "velocityX": 0.5078795417016481,
-            "velocityY": 0.3426929367359288,
-            "timestamp": 0.07426699719034811
-        },
-        {
-            "x": 5.289237393773012,
-            "y": 1.696398991795747,
-            "heading": 2.5693854471889703,
-            "angularVelocity": 0.6118609129363125,
-            "velocityX": 1.0168541631333754,
-            "velocityY": 0.6860142237264,
-            "timestamp": 0.14853399438069623
-        },
-        {
-            "x": 5.402661733248807,
-            "y": 1.772907618565081,
-            "heading": 2.632642220499669,
-            "angularVelocity": 0.8517480940904348,
-            "velocityX": 1.5272509158420084,
-            "velocityY": 1.0301833878275757,
-            "timestamp": 0.22280099157104433
-        },
-        {
-            "x": 5.554123710239057,
-            "y": 1.8750622586328765,
-            "heading": 2.707444867040942,
-            "angularVelocity": 1.007212481602717,
-            "velocityX": 2.039425084092856,
-            "velocityY": 1.3755051898216737,
-            "timestamp": 0.29706798876139245
-        },
-        {
-            "x": 5.743710865909146,
-            "y": 2.002930120234068,
-            "heading": 2.7801295579879124,
-            "angularVelocity": 0.9786943554574798,
-            "velocityX": 2.552777988103829,
-            "velocityY": 1.7217319460683576,
-            "timestamp": 0.3713349859517406
-        },
-        {
-            "x": 5.969240710205324,
-            "y": 2.1555656332559185,
-            "heading": 2.8014617866621823,
-            "angularVelocity": 0.2872369892591052,
-            "velocityX": 3.036743813919657,
-            "velocityY": 2.055226665899016,
-            "timestamp": 0.4456019831420887
-        },
-        {
-            "x": 6.202237157849221,
-            "y": 2.312550420967927,
-            "heading": 2.8014618141625047,
-            "angularVelocity": 3.702899476409415e-7,
-            "velocityX": 3.137281113530435,
-            "velocityY": 2.1137893499268903,
-            "timestamp": 0.5198689803324368
-        },
-        {
-            "x": 6.435233608100515,
-            "y": 2.4695352048100654,
-            "heading": 2.8014618416628014,
-            "angularVelocity": 3.7028959815407443e-7,
-            "velocityX": 3.137281148638853,
-            "velocityY": 2.1137892978193555,
-            "timestamp": 0.5941359775227849
-        },
-        {
-            "x": 6.6682300583517184,
-            "y": 2.6265199886523383,
-            "heading": 2.801461869163098,
-            "angularVelocity": 3.702895988133056e-7,
-            "velocityX": 3.137281148637628,
-            "velocityY": 2.1137892978211736,
-            "timestamp": 0.668402974713133
-        },
-        {
-            "x": 6.901226494182671,
-            "y": 2.7835047938971096,
-            "heading": 2.801461896663395,
-            "angularVelocity": 3.702895996177554e-7,
-            "velocityX": 3.137280954469964,
-            "velocityY": 2.1137895860043394,
-            "timestamp": 0.7426699719034812
-        },
-        {
-            "x": 7.132347068567706,
-            "y": 2.9432384730568093,
-            "heading": 2.801461924236189,
-            "angularVelocity": 3.712657746616385e-7,
-            "velocityX": 3.1120226093518935,
-            "velocityY": 2.1508029838650415,
-            "timestamp": 0.8169369690938293
-        },
-        {
-            "x": 7.334313444212558,
-            "y": 3.1385360610663486,
-            "heading": 2.8014619668447454,
-            "angularVelocity": 5.737212724741241e-7,
-            "velocityX": 2.7194633321070794,
-            "velocityY": 2.6296685660925183,
-            "timestamp": 0.8912039662841774
-        },
-        {
-            "x": 7.4948116486140535,
-            "y": 3.3502854712542645,
-            "heading": 2.8423857101264045,
-            "angularVelocity": 0.5510353835468871,
-            "velocityX": 2.161097263568294,
-            "velocityY": 2.8511912181557233,
-            "timestamp": 0.9654709634745255
-        },
-        {
-            "x": 7.612584040336913,
-            "y": 3.548974342070663,
-            "heading": 2.9223254670447334,
-            "angularVelocity": 1.0763833188709784,
-            "velocityX": 1.5857971397578559,
-            "velocityY": 2.675331955419639,
-            "timestamp": 1.0397379606648736
-        },
-        {
-            "x": 7.689064457271449,
-            "y": 3.727414986995987,
-            "heading": 3.0016093061540334,
-            "angularVelocity": 1.0675514307666671,
-            "velocityX": 1.0298035443457485,
-            "velocityY": 2.402690988945958,
-            "timestamp": 1.1140049578552216
-        },
-        {
-            "x": 7.725394752128812,
-            "y": 3.8837274225386396,
-            "heading": 3.06785364542737,
-            "angularVelocity": 0.8919754639271454,
-            "velocityX": 0.4891849170129639,
-            "velocityY": 2.1047361742931416,
-            "timestamp": 1.1882719550455696
-        },
-        {
-            "x": 7.722234181622283,
-            "y": 4.017077206893078,
-            "heading": 3.1151383358166123,
-            "angularVelocity": 0.6366850980665145,
-            "velocityX": -0.04255686409979938,
-            "velocityY": 1.7955456582236569,
-            "timestamp": 1.2625389522359176
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.33476059520309653,
-            "velocityX": -0.5686803455111581,
-            "velocityY": 1.4801028352497836,
-            "timestamp": 1.3368059494262656
-        },
-        {
-            "x": 7.583164521123183,
-            "y": 4.220739525524416,
-            "heading": 3.1367175382385155,
-            "angularVelocity": -0.03922526988843187,
-            "velocityX": -1.1571796016903517,
-            "velocityY": 1.1201830988719972,
-            "timestamp": 1.4204882738773397
-        },
-        {
-            "x": 7.437331010349068,
-            "y": 4.2837766667768715,
-            "heading": 3.1044919064382244,
-            "angularVelocity": -0.38509484543694944,
-            "velocityX": -1.742703871226464,
-            "velocityY": 0.753290992643391,
-            "timestamp": 1.5041705983284137
-        },
-        {
-            "x": 7.242933099071244,
-            "y": 4.315173048252524,
-            "heading": 3.047144336604056,
-            "angularVelocity": -0.6853008710064864,
-            "velocityX": -2.3230462651820925,
-            "velocityY": 0.3751853414876064,
-            "timestamp": 1.5878529227794878
-        },
-        {
-            "x": 7.00089933731331,
-            "y": 4.3131727597182845,
-            "heading": 2.9720298467353206,
-            "angularVelocity": -0.8976147634695787,
-            "velocityX": -2.8922925282679244,
-            "velocityY": -0.023903357696631062,
-            "timestamp": 1.6715352472305618
-        },
-        {
-            "x": 6.7143889772576815,
-            "y": 4.273350582816976,
-            "heading": 2.8998851691901093,
-            "angularVelocity": -0.8621256402526404,
-            "velocityX": -3.4237858703738695,
-            "velocityY": -0.47587321650692566,
-            "timestamp": 1.7552175716816358
-        },
-        {
-            "x": 6.411635278119781,
-            "y": 4.1808641012010535,
-            "heading": 2.8998851130559236,
-            "angularVelocity": -6.708009886588717e-7,
-            "velocityX": -3.6178930392272837,
-            "velocityY": -1.1052092807244585,
-            "timestamp": 1.8388998961327099
-        },
-        {
-            "x": 6.131017053711007,
-            "y": 4.034347987511598,
-            "heading": 2.8998850873919646,
-            "angularVelocity": -3.066831498282175e-7,
-            "velocityX": -3.3533751153487676,
-            "velocityY": -1.7508609452539547,
-            "timestamp": 1.922582220583784
-        },
-        {
-            "x": 5.854044563819553,
-            "y": 3.8810515447858025,
-            "heading": 2.8998850618676295,
-            "angularVelocity": -3.050146543457835e-7,
-            "velocityX": -3.3098087524252473,
-            "velocityY": -1.8318855711927797,
-            "timestamp": 2.006264545034858
-        },
-        {
-            "x": 5.577072094171527,
-            "y": 3.7277550654846525,
-            "heading": 2.8998850363432944,
-            "angularVelocity": -3.0501465285690934e-7,
-            "velocityX": -3.3098085105171973,
-            "velocityY": -1.8318860082666197,
-            "timestamp": 2.089946869485932
-        },
-        {
-            "x": 5.300099624523611,
-            "y": 3.5744585861833027,
-            "heading": 2.8998850108189593,
-            "angularVelocity": -3.050146519273835e-7,
-            "velocityX": -3.3098085105158774,
-            "velocityY": -1.8318860082690038,
-            "timestamp": 2.173629193937006
-        },
-        {
-            "x": 5.0231271554381145,
-            "y": 3.421162105865856,
-            "heading": 2.899884985294552,
-            "angularVelocity": -3.050155161997765e-7,
-            "velocityX": -3.3098085037949927,
-            "velocityY": -1.831886020411314,
-            "timestamp": 2.25731151838808
-        },
-        {
-            "x": 4.7695419048400876,
-            "y": 3.2806617941163765,
-            "heading": 2.834618611781297,
-            "angularVelocity": -0.7799302175385181,
-            "velocityX": -3.030332298504562,
-            "velocityY": -1.678972383608021,
-            "timestamp": 2.340993842839154
-        },
-        {
-            "x": 4.566605904742026,
-            "y": 3.1682378317487854,
-            "heading": 2.7427589462823403,
-            "angularVelocity": -1.0977188564194784,
-            "velocityX": -2.4250760411980474,
-            "velocityY": -1.3434612757837647,
-            "timestamp": 2.424676167290228
-        },
-        {
-            "x": 4.414672096834118,
-            "y": 3.08405727220976,
-            "heading": 2.6624278185804466,
-            "angularVelocity": -0.959953350111109,
-            "velocityX": -1.8156021466246228,
-            "velocityY": -1.0059538868120508,
-            "timestamp": 2.508358491741302
-        },
-        {
-            "x": 4.313522085490217,
-            "y": 3.0280030162190834,
-            "heading": 2.6044623353701315,
-            "angularVelocity": -0.6926849079605295,
-            "velocityX": -1.2087380699258492,
-            "velocityY": -0.6698458289533948,
-            "timestamp": 2.5920408161923763
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -0.3640235326869004,
-            "velocityX": -0.6037366411798936,
-            "velocityY": -0.3346347798387892,
-            "timestamp": 2.6757231406434503
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 3.2974830734555563e-32,
-            "velocityY": 2.8302357269459625e-32,
-            "timestamp": 2.7594054650945243
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S4.1.traj
deleted file mode 100644
index 752a3083..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S4.1.traj	
+++ /dev/null
@@ -1,337 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 6.749906626357912e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 5.210246643603351,
-            "y": 1.6542137133955805,
-            "heading": 2.5206016461454785,
-            "angularVelocity": 0.26943757558327686,
-            "velocityX": 0.4478929770559344,
-            "velocityY": 0.44746230101760404,
-            "timestamp": 0.07646166686617559
-        },
-        {
-            "x": 5.278793169653249,
-            "y": 1.7226907202744617,
-            "heading": 2.5594414345510907,
-            "angularVelocity": 0.5079641864673213,
-            "velocityX": 0.8964822356001046,
-            "velocityY": 0.8955730326770261,
-            "timestamp": 0.15292333373235117
-        },
-        {
-            "x": 5.381708599884549,
-            "y": 1.8254946394710425,
-            "heading": 2.61294205757613,
-            "angularVelocity": 0.6997051623093229,
-            "velocityX": 1.3459741913738752,
-            "velocityY": 1.344515800008778,
-            "timestamp": 0.22938500059852676
-        },
-        {
-            "x": 5.519076425443274,
-            "y": 1.9627026546285364,
-            "heading": 2.6750057017308966,
-            "angularVelocity": 0.8116961962572822,
-            "velocityX": 1.796558081831356,
-            "velocityY": 1.7944680096712713,
-            "timestamp": 0.30584666746470235
-        },
-        {
-            "x": 5.690930173156719,
-            "y": 2.1343411411965687,
-            "heading": 2.7325200708925137,
-            "angularVelocity": 0.7521987359009454,
-            "velocityX": 2.2475804511851214,
-            "velocityY": 2.2447651693028954,
-            "timestamp": 0.38230833433087796
-        },
-        {
-            "x": 5.895075735242969,
-            "y": 2.338989079985295,
-            "heading": 2.7330245204544825,
-            "angularVelocity": 0.006597417799580435,
-            "velocityX": 2.6699072941157542,
-            "velocityY": 2.6764776021284593,
-            "timestamp": 0.4587700011970536
-        },
-        {
-            "x": 6.099717519184054,
-            "y": 2.543408411889471,
-            "heading": 2.7330245436350364,
-            "angularVelocity": 3.031656877658216e-7,
-            "velocityX": 2.676397106268319,
-            "velocityY": 2.673487778679394,
-            "timestamp": 0.5352316680632292
-        },
-        {
-            "x": 6.3043593056762255,
-            "y": 2.7478277412397842,
-            "heading": 2.7330245668155904,
-            "angularVelocity": 3.03165682232601e-7,
-            "velocityX": 2.676397139632574,
-            "velocityY": 2.673487745278835,
-            "timestamp": 0.6116933349294048
-        },
-        {
-            "x": 6.509001092168415,
-            "y": 2.9522470705900794,
-            "heading": 2.733024589996144,
-            "angularVelocity": 3.031656817444859e-7,
-            "velocityX": 2.676397139632809,
-            "velocityY": 2.6734877452786003,
-            "timestamp": 0.6881550017955804
-        },
-        {
-            "x": 6.713642878660604,
-            "y": 3.1566663999403746,
-            "heading": 2.733024613176697,
-            "angularVelocity": 3.0316568050462653e-7,
-            "velocityX": 2.676397139632809,
-            "velocityY": 2.6734877452786003,
-            "timestamp": 0.764616668661756
-        },
-        {
-            "x": 6.918284665152036,
-            "y": 3.3610857292914273,
-            "heading": 2.7330246363572503,
-            "angularVelocity": 3.0316568069769535e-7,
-            "velocityX": 2.676397139622915,
-            "velocityY": 2.6734877452885044,
-            "timestamp": 0.8410783355279317
-        },
-        {
-            "x": 7.122926344234402,
-            "y": 3.565505166167973,
-            "heading": 2.733024659538502,
-            "angularVelocity": 3.031748161580926e-7,
-            "velocityX": 2.6763957348789207,
-            "velocityY": 2.67348915155517,
-            "timestamp": 0.9175400023941073
-        },
-        {
-            "x": 7.301295269977114,
-            "y": 3.7451643880510668,
-            "heading": 2.8254238510952887,
-            "angularVelocity": 1.2084381016504042,
-            "velocityX": 2.332788873866637,
-            "velocityY": 2.349663945955243,
-            "timestamp": 0.9940016692602829
-        },
-        {
-            "x": 7.445441288033757,
-            "y": 3.8901748417746975,
-            "heading": 2.925487102833734,
-            "angularVelocity": 1.308672120810262,
-            "velocityX": 1.8852063257910538,
-            "velocityY": 1.8965118034561068,
-            "timestamp": 1.0704633361264584
-        },
-        {
-            "x": 7.555314221872798,
-            "y": 4.0007360886631185,
-            "heading": 3.011754015083649,
-            "angularVelocity": 1.1282374003289979,
-            "velocityX": 1.436967546513772,
-            "velocityY": 1.445969613531011,
-            "timestamp": 1.146925002992634
-        },
-        {
-            "x": 7.6309987143881095,
-            "y": 4.077005222028419,
-            "heading": 3.0780078899884065,
-            "angularVelocity": 0.8664979148403406,
-            "velocityX": 0.98983576499549,
-            "velocityY": 0.997481960454622,
-            "timestamp": 1.2233866698588094
-        },
-        {
-            "x": 7.672548767265181,
-            "y": 4.1190736771641765,
-            "heading": 3.121324471245367,
-            "angularVelocity": 0.5665136928387217,
-            "velocityX": 0.5434102417593452,
-            "velocityY": 0.5501901391894305,
-            "timestamp": 1.299848336724985
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.24424694778521738,
-            "velocityX": 0.09745056627996526,
-            "velocityY": 0.10366400787072177,
-            "timestamp": 1.3763100035911604
-        },
-        {
-            "x": 7.651764263007655,
-            "y": 4.099219122472668,
-            "heading": 3.13204050140682,
-            "angularVelocity": -0.10143830296235676,
-            "velocityX": -0.35984493368073844,
-            "velocityY": -0.35404806448388143,
-            "timestamp": 1.4547764049477947
-        },
-        {
-            "x": 7.587580185818596,
-            "y": 4.035451706139359,
-            "heading": 3.0993558868019444,
-            "angularVelocity": -0.41654280099226804,
-            "velocityX": -0.8179816594027142,
-            "velocityY": -0.8126716050540244,
-            "timestamp": 1.533242806304429
-        },
-        {
-            "x": 7.487355398269796,
-            "y": 3.9356017103716967,
-            "heading": 3.045691872479544,
-            "angularVelocity": -0.683910736246133,
-            "velocityX": -1.2772955789481495,
-            "velocityY": -1.272519116989166,
-            "timestamp": 1.6117092076610633
-        },
-        {
-            "x": 7.35096170591853,
-            "y": 3.7995397339434045,
-            "heading": 2.9777353222524336,
-            "angularVelocity": -0.8660592183684257,
-            "velocityX": -1.7382432479775494,
-            "velocityY": -1.7340157580297684,
-            "timestamp": 1.6901756090176976
-        },
-        {
-            "x": 7.178293181957013,
-            "y": 3.6271506268794376,
-            "heading": 2.9104315316891,
-            "angularVelocity": -0.8577402480513148,
-            "velocityX": -2.2005408808889824,
-            "velocityY": -2.1969799058382784,
-            "timestamp": 1.7686420103743319
-        },
-        {
-            "x": 6.971558922924333,
-            "y": 3.4198249259322475,
-            "heading": 2.9002273959835922,
-            "angularVelocity": -0.13004465005511517,
-            "velocityX": -2.6346851067256227,
-            "velocityY": -2.6422226247497056,
-            "timestamp": 1.8471084117309662
-        },
-        {
-            "x": 6.761730033100129,
-            "y": 3.2098675579352265,
-            "heading": 2.900227368387608,
-            "angularVelocity": -3.5169172941648496e-7,
-            "velocityX": -2.6741240352099296,
-            "velocityY": -2.6757614006376103,
-            "timestamp": 1.9255748130876005
-        },
-        {
-            "x": 6.551901141744289,
-            "y": 2.9999101914688975,
-            "heading": 2.900227340791633,
-            "angularVelocity": -3.5169161244263704e-7,
-            "velocityX": -2.674124054729561,
-            "velocityY": -2.6757613811300027,
-            "timestamp": 2.0040412144442348
-        },
-        {
-            "x": 6.342072250388439,
-            "y": 2.7899528250025782,
-            "heading": 2.9002273131956575,
-            "angularVelocity": -3.5169161245965383e-7,
-            "velocityX": -2.674124054729688,
-            "velocityY": -2.6757613811298757,
-            "timestamp": 2.082507615800869
-        },
-        {
-            "x": 6.132243359032647,
-            "y": 2.579995458536201,
-            "heading": 2.9002272855996822,
-            "angularVelocity": -3.5169161351505577e-7,
-            "velocityX": -2.674124054728939,
-            "velocityY": -2.675761381130624,
-            "timestamp": 2.1609740171575034
-        },
-        {
-            "x": 5.922414476696474,
-            "y": 2.3700380830557237,
-            "heading": 2.900227258003707,
-            "angularVelocity": -3.51691614830637e-7,
-            "velocityX": -2.6741239397801397,
-            "velocityY": -2.67576149600908,
-            "timestamp": 2.2394404185141377
-        },
-        {
-            "x": 5.71397323769833,
-            "y": 2.1587030259315876,
-            "heading": 2.9002272179386366,
-            "angularVelocity": -5.106016050290645e-7,
-            "velocityX": -2.6564393854481323,
-            "velocityY": -2.6933190954381305,
-            "timestamp": 2.317906819870772
-        },
-        {
-            "x": 5.5341856675792425,
-            "y": 1.9787047842990844,
-            "heading": 2.7906748594245325,
-            "angularVelocity": -1.39616901782181,
-            "velocityX": -2.2912681990084183,
-            "velocityY": -2.2939530616983537,
-            "timestamp": 2.3963732212274063
-        },
-        {
-            "x": 5.390687970791945,
-            "y": 1.8350178958205943,
-            "heading": 2.6821705547459764,
-            "angularVelocity": -1.3828122967612335,
-            "velocityX": -1.8287788697622565,
-            "velocityY": -1.8311899869783075,
-            "timestamp": 2.4748396225840406
-        },
-        {
-            "x": 5.2832586949423685,
-            "y": 1.7274312477093827,
-            "heading": 2.5937450000537616,
-            "angularVelocity": -1.1269225192361696,
-            "velocityX": -1.369111798071441,
-            "velocityY": -1.371117398671364,
-            "timestamp": 2.553306023940675
-        },
-        {
-            "x": 5.211730147578831,
-            "y": 1.6557903820034874,
-            "heading": 2.5319094528254147,
-            "angularVelocity": -0.7880512698333239,
-            "velocityX": -0.9115818506628853,
-            "velocityY": -0.913013270231208,
-            "timestamp": 2.631772425297309
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.4066639003920201,
-            "velocityX": -0.4553560117589102,
-            "velocityY": -0.45612365782926495,
-            "timestamp": 2.7102388266539434
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": -6.900806950408652e-34,
-            "velocityY": 1.697639013664865e-32,
-            "timestamp": 2.7887052280105777
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S4.traj
deleted file mode 100644
index 752a3083..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3 To S4.traj	
+++ /dev/null
@@ -1,337 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 6.749906626357912e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 5.210246643603351,
-            "y": 1.6542137133955805,
-            "heading": 2.5206016461454785,
-            "angularVelocity": 0.26943757558327686,
-            "velocityX": 0.4478929770559344,
-            "velocityY": 0.44746230101760404,
-            "timestamp": 0.07646166686617559
-        },
-        {
-            "x": 5.278793169653249,
-            "y": 1.7226907202744617,
-            "heading": 2.5594414345510907,
-            "angularVelocity": 0.5079641864673213,
-            "velocityX": 0.8964822356001046,
-            "velocityY": 0.8955730326770261,
-            "timestamp": 0.15292333373235117
-        },
-        {
-            "x": 5.381708599884549,
-            "y": 1.8254946394710425,
-            "heading": 2.61294205757613,
-            "angularVelocity": 0.6997051623093229,
-            "velocityX": 1.3459741913738752,
-            "velocityY": 1.344515800008778,
-            "timestamp": 0.22938500059852676
-        },
-        {
-            "x": 5.519076425443274,
-            "y": 1.9627026546285364,
-            "heading": 2.6750057017308966,
-            "angularVelocity": 0.8116961962572822,
-            "velocityX": 1.796558081831356,
-            "velocityY": 1.7944680096712713,
-            "timestamp": 0.30584666746470235
-        },
-        {
-            "x": 5.690930173156719,
-            "y": 2.1343411411965687,
-            "heading": 2.7325200708925137,
-            "angularVelocity": 0.7521987359009454,
-            "velocityX": 2.2475804511851214,
-            "velocityY": 2.2447651693028954,
-            "timestamp": 0.38230833433087796
-        },
-        {
-            "x": 5.895075735242969,
-            "y": 2.338989079985295,
-            "heading": 2.7330245204544825,
-            "angularVelocity": 0.006597417799580435,
-            "velocityX": 2.6699072941157542,
-            "velocityY": 2.6764776021284593,
-            "timestamp": 0.4587700011970536
-        },
-        {
-            "x": 6.099717519184054,
-            "y": 2.543408411889471,
-            "heading": 2.7330245436350364,
-            "angularVelocity": 3.031656877658216e-7,
-            "velocityX": 2.676397106268319,
-            "velocityY": 2.673487778679394,
-            "timestamp": 0.5352316680632292
-        },
-        {
-            "x": 6.3043593056762255,
-            "y": 2.7478277412397842,
-            "heading": 2.7330245668155904,
-            "angularVelocity": 3.03165682232601e-7,
-            "velocityX": 2.676397139632574,
-            "velocityY": 2.673487745278835,
-            "timestamp": 0.6116933349294048
-        },
-        {
-            "x": 6.509001092168415,
-            "y": 2.9522470705900794,
-            "heading": 2.733024589996144,
-            "angularVelocity": 3.031656817444859e-7,
-            "velocityX": 2.676397139632809,
-            "velocityY": 2.6734877452786003,
-            "timestamp": 0.6881550017955804
-        },
-        {
-            "x": 6.713642878660604,
-            "y": 3.1566663999403746,
-            "heading": 2.733024613176697,
-            "angularVelocity": 3.0316568050462653e-7,
-            "velocityX": 2.676397139632809,
-            "velocityY": 2.6734877452786003,
-            "timestamp": 0.764616668661756
-        },
-        {
-            "x": 6.918284665152036,
-            "y": 3.3610857292914273,
-            "heading": 2.7330246363572503,
-            "angularVelocity": 3.0316568069769535e-7,
-            "velocityX": 2.676397139622915,
-            "velocityY": 2.6734877452885044,
-            "timestamp": 0.8410783355279317
-        },
-        {
-            "x": 7.122926344234402,
-            "y": 3.565505166167973,
-            "heading": 2.733024659538502,
-            "angularVelocity": 3.031748161580926e-7,
-            "velocityX": 2.6763957348789207,
-            "velocityY": 2.67348915155517,
-            "timestamp": 0.9175400023941073
-        },
-        {
-            "x": 7.301295269977114,
-            "y": 3.7451643880510668,
-            "heading": 2.8254238510952887,
-            "angularVelocity": 1.2084381016504042,
-            "velocityX": 2.332788873866637,
-            "velocityY": 2.349663945955243,
-            "timestamp": 0.9940016692602829
-        },
-        {
-            "x": 7.445441288033757,
-            "y": 3.8901748417746975,
-            "heading": 2.925487102833734,
-            "angularVelocity": 1.308672120810262,
-            "velocityX": 1.8852063257910538,
-            "velocityY": 1.8965118034561068,
-            "timestamp": 1.0704633361264584
-        },
-        {
-            "x": 7.555314221872798,
-            "y": 4.0007360886631185,
-            "heading": 3.011754015083649,
-            "angularVelocity": 1.1282374003289979,
-            "velocityX": 1.436967546513772,
-            "velocityY": 1.445969613531011,
-            "timestamp": 1.146925002992634
-        },
-        {
-            "x": 7.6309987143881095,
-            "y": 4.077005222028419,
-            "heading": 3.0780078899884065,
-            "angularVelocity": 0.8664979148403406,
-            "velocityX": 0.98983576499549,
-            "velocityY": 0.997481960454622,
-            "timestamp": 1.2233866698588094
-        },
-        {
-            "x": 7.672548767265181,
-            "y": 4.1190736771641765,
-            "heading": 3.121324471245367,
-            "angularVelocity": 0.5665136928387217,
-            "velocityX": 0.5434102417593452,
-            "velocityY": 0.5501901391894305,
-            "timestamp": 1.299848336724985
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.24424694778521738,
-            "velocityX": 0.09745056627996526,
-            "velocityY": 0.10366400787072177,
-            "timestamp": 1.3763100035911604
-        },
-        {
-            "x": 7.651764263007655,
-            "y": 4.099219122472668,
-            "heading": 3.13204050140682,
-            "angularVelocity": -0.10143830296235676,
-            "velocityX": -0.35984493368073844,
-            "velocityY": -0.35404806448388143,
-            "timestamp": 1.4547764049477947
-        },
-        {
-            "x": 7.587580185818596,
-            "y": 4.035451706139359,
-            "heading": 3.0993558868019444,
-            "angularVelocity": -0.41654280099226804,
-            "velocityX": -0.8179816594027142,
-            "velocityY": -0.8126716050540244,
-            "timestamp": 1.533242806304429
-        },
-        {
-            "x": 7.487355398269796,
-            "y": 3.9356017103716967,
-            "heading": 3.045691872479544,
-            "angularVelocity": -0.683910736246133,
-            "velocityX": -1.2772955789481495,
-            "velocityY": -1.272519116989166,
-            "timestamp": 1.6117092076610633
-        },
-        {
-            "x": 7.35096170591853,
-            "y": 3.7995397339434045,
-            "heading": 2.9777353222524336,
-            "angularVelocity": -0.8660592183684257,
-            "velocityX": -1.7382432479775494,
-            "velocityY": -1.7340157580297684,
-            "timestamp": 1.6901756090176976
-        },
-        {
-            "x": 7.178293181957013,
-            "y": 3.6271506268794376,
-            "heading": 2.9104315316891,
-            "angularVelocity": -0.8577402480513148,
-            "velocityX": -2.2005408808889824,
-            "velocityY": -2.1969799058382784,
-            "timestamp": 1.7686420103743319
-        },
-        {
-            "x": 6.971558922924333,
-            "y": 3.4198249259322475,
-            "heading": 2.9002273959835922,
-            "angularVelocity": -0.13004465005511517,
-            "velocityX": -2.6346851067256227,
-            "velocityY": -2.6422226247497056,
-            "timestamp": 1.8471084117309662
-        },
-        {
-            "x": 6.761730033100129,
-            "y": 3.2098675579352265,
-            "heading": 2.900227368387608,
-            "angularVelocity": -3.5169172941648496e-7,
-            "velocityX": -2.6741240352099296,
-            "velocityY": -2.6757614006376103,
-            "timestamp": 1.9255748130876005
-        },
-        {
-            "x": 6.551901141744289,
-            "y": 2.9999101914688975,
-            "heading": 2.900227340791633,
-            "angularVelocity": -3.5169161244263704e-7,
-            "velocityX": -2.674124054729561,
-            "velocityY": -2.6757613811300027,
-            "timestamp": 2.0040412144442348
-        },
-        {
-            "x": 6.342072250388439,
-            "y": 2.7899528250025782,
-            "heading": 2.9002273131956575,
-            "angularVelocity": -3.5169161245965383e-7,
-            "velocityX": -2.674124054729688,
-            "velocityY": -2.6757613811298757,
-            "timestamp": 2.082507615800869
-        },
-        {
-            "x": 6.132243359032647,
-            "y": 2.579995458536201,
-            "heading": 2.9002272855996822,
-            "angularVelocity": -3.5169161351505577e-7,
-            "velocityX": -2.674124054728939,
-            "velocityY": -2.675761381130624,
-            "timestamp": 2.1609740171575034
-        },
-        {
-            "x": 5.922414476696474,
-            "y": 2.3700380830557237,
-            "heading": 2.900227258003707,
-            "angularVelocity": -3.51691614830637e-7,
-            "velocityX": -2.6741239397801397,
-            "velocityY": -2.67576149600908,
-            "timestamp": 2.2394404185141377
-        },
-        {
-            "x": 5.71397323769833,
-            "y": 2.1587030259315876,
-            "heading": 2.9002272179386366,
-            "angularVelocity": -5.106016050290645e-7,
-            "velocityX": -2.6564393854481323,
-            "velocityY": -2.6933190954381305,
-            "timestamp": 2.317906819870772
-        },
-        {
-            "x": 5.5341856675792425,
-            "y": 1.9787047842990844,
-            "heading": 2.7906748594245325,
-            "angularVelocity": -1.39616901782181,
-            "velocityX": -2.2912681990084183,
-            "velocityY": -2.2939530616983537,
-            "timestamp": 2.3963732212274063
-        },
-        {
-            "x": 5.390687970791945,
-            "y": 1.8350178958205943,
-            "heading": 2.6821705547459764,
-            "angularVelocity": -1.3828122967612335,
-            "velocityX": -1.8287788697622565,
-            "velocityY": -1.8311899869783075,
-            "timestamp": 2.4748396225840406
-        },
-        {
-            "x": 5.2832586949423685,
-            "y": 1.7274312477093827,
-            "heading": 2.5937450000537616,
-            "angularVelocity": -1.1269225192361696,
-            "velocityX": -1.369111798071441,
-            "velocityY": -1.371117398671364,
-            "timestamp": 2.553306023940675
-        },
-        {
-            "x": 5.211730147578831,
-            "y": 1.6557903820034874,
-            "heading": 2.5319094528254147,
-            "angularVelocity": -0.7880512698333239,
-            "velocityX": -0.9115818506628853,
-            "velocityY": -0.913013270231208,
-            "timestamp": 2.631772425297309
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.4066639003920201,
-            "velocityX": -0.4553560117589102,
-            "velocityY": -0.45612365782926495,
-            "timestamp": 2.7102388266539434
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": -6.900806950408652e-34,
-            "velocityY": 1.697639013664865e-32,
-            "timestamp": 2.7887052280105777
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S3.1.traj
deleted file mode 100644
index 0baf5ca9..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S3.1.traj	
+++ /dev/null
@@ -1,301 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 2.0168042378406012e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.220318940234659,
-            "y": 1.6240042188799209,
-            "heading": 2.522514839920101,
-            "angularVelocity": 0.3066580902466342,
-            "velocityX": 0.6036357185902667,
-            "velocityY": 0.05453852299210067,
-            "timestamp": 0.07342000956828917
-        },
-        {
-            "x": 5.309030400883647,
-            "y": 1.632013165892952,
-            "heading": 2.5656393100423864,
-            "angularVelocity": 0.5873667189075205,
-            "velocityX": 1.208273618739824,
-            "velocityY": 0.10908398214768973,
-            "timestamp": 0.14684001913657835
-        },
-        {
-            "x": 5.442235678671135,
-            "y": 1.6440289719213574,
-            "heading": 2.6263948924042286,
-            "angularVelocity": 0.8275071430674761,
-            "velocityX": 1.8142912071346369,
-            "velocityY": 0.1636584644847863,
-            "timestamp": 0.22026002870486752
-        },
-        {
-            "x": 5.6200752852220655,
-            "y": 1.6600527803178184,
-            "heading": 2.6994681964366594,
-            "angularVelocity": 0.9952777786614565,
-            "velocityX": 2.4222226011223906,
-            "velocityY": 0.21824851958859787,
-            "timestamp": 0.2936800382731567
-        },
-        {
-            "x": 5.842683997650443,
-            "y": 1.680064789350092,
-            "heading": 2.773109972156216,
-            "angularVelocity": 1.0030205137886674,
-            "velocityX": 3.031989695143347,
-            "velocityY": 0.2725688698481895,
-            "timestamp": 0.3671000478414459
-        },
-        {
-            "x": 6.108354667125484,
-            "y": 1.7034775424781843,
-            "heading": 2.8018843070789763,
-            "angularVelocity": 0.39191407208947715,
-            "velocityX": 3.6185049693835545,
-            "velocityY": 0.31888790625005003,
-            "timestamp": 0.4405200574097351
-        },
-        {
-            "x": 6.384952370737624,
-            "y": 1.7286802043771023,
-            "heading": 2.8018843326053964,
-            "angularVelocity": 3.476766132233899e-7,
-            "velocityX": 3.7673340719858177,
-            "velocityY": 0.34326693836064737,
-            "timestamp": 0.5139400669780243
-        },
-        {
-            "x": 6.661294346947145,
-            "y": 1.7565473171945905,
-            "heading": 2.8018843581575315,
-            "angularVelocity": 3.48026857889212e-7,
-            "velocityX": 3.7638509969478653,
-            "velocityY": 0.37955746643565225,
-            "timestamp": 0.5873600765463135
-        },
-        {
-            "x": 6.9295239726753675,
-            "y": 1.8286189976607798,
-            "heading": 2.8018844037308708,
-            "angularVelocity": 6.207209630445694e-7,
-            "velocityX": 3.6533586321414817,
-            "velocityY": 0.9816354000767032,
-            "timestamp": 0.6607800861146027
-        },
-        {
-            "x": 7.165852331775521,
-            "y": 1.9289604111011978,
-            "heading": 2.8534870271763118,
-            "angularVelocity": 0.7028414154241911,
-            "velocityX": 3.218854921019001,
-            "velocityY": 1.3666766598157905,
-            "timestamp": 0.7342000956828919
-        },
-        {
-            "x": 7.3582678693157275,
-            "y": 2.0352947515499316,
-            "heading": 2.9321246763669695,
-            "angularVelocity": 1.0710656352818668,
-            "velocityX": 2.620750646473749,
-            "velocityY": 1.4483019148865994,
-            "timestamp": 0.8076201052511811
-        },
-        {
-            "x": 7.505754872565929,
-            "y": 2.142580812119965,
-            "heading": 3.007723504913372,
-            "angularVelocity": 1.0296760922659598,
-            "velocityX": 2.0088120951961708,
-            "velocityY": 1.4612645953179093,
-            "timestamp": 0.8810401148194703
-        },
-        {
-            "x": 7.608443195664495,
-            "y": 2.2491262386362876,
-            "heading": 3.070552698416666,
-            "angularVelocity": 0.855750276698862,
-            "velocityX": 1.3986421917181704,
-            "velocityY": 1.4511769631033729,
-            "timestamp": 0.9544601243877595
-        },
-        {
-            "x": 7.666484933948513,
-            "y": 2.354097419514004,
-            "heading": 3.115652624171886,
-            "angularVelocity": 0.6142729484838055,
-            "velocityX": 0.7905438670643103,
-            "velocityY": 1.4297353200436178,
-            "timestamp": 1.0278801339560486
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 0.331617715269703,
-            "velocityX": 0.18407878357617766,
-            "velocityY": 1.4015604341523722,
-            "timestamp": 1.1013001435243377
-        },
-        {
-            "x": 7.640202623194362,
-            "y": 2.56833046361043,
-            "heading": 3.1389376214813485,
-            "angularVelocity": -0.013028904924243205,
-            "velocityX": -0.48807108721701464,
-            "velocityY": 1.3653457784439293,
-            "timestamp": 1.1828402671824687
-        },
-        {
-            "x": 7.5455521042399925,
-            "y": 2.676247019412303,
-            "heading": 3.111715108217083,
-            "angularVelocity": -0.3338542062850063,
-            "velocityX": -1.1607845893293163,
-            "velocityY": 1.3234779512271038,
-            "timestamp": 1.2643803908406
-        },
-        {
-            "x": 7.395994092884954,
-            "y": 2.780007432926005,
-            "heading": 3.061424395012115,
-            "angularVelocity": -0.616760325454245,
-            "velocityX": -1.8341646375480058,
-            "velocityY": 1.2725074338705782,
-            "timestamp": 1.3459205144987312
-        },
-        {
-            "x": 7.191491634258091,
-            "y": 2.8782237282133547,
-            "heading": 2.9936811867966338,
-            "angularVelocity": -0.8307959955948208,
-            "velocityX": -2.5079978966461463,
-            "velocityY": 1.2045149170871063,
-            "timestamp": 1.4274606381568624
-        },
-        {
-            "x": 6.932297950713545,
-            "y": 2.9674313510808874,
-            "heading": 2.9221270442336884,
-            "angularVelocity": -0.8775329169593383,
-            "velocityX": -3.178725661874757,
-            "velocityY": 1.0940334508388325,
-            "timestamp": 1.5090007618149937
-        },
-        {
-            "x": 6.629938780171593,
-            "y": 3.0284835582667458,
-            "heading": 2.9221269432503374,
-            "angularVelocity": -0.0000012384498122465325,
-            "velocityX": -3.7081029188726404,
-            "velocityY": 0.7487382217106987,
-            "timestamp": 1.5905408854731249
-        },
-        {
-            "x": 6.3215346451204795,
-            "y": 3.034428801743739,
-            "heading": 2.922126920201386,
-            "angularVelocity": -2.8267005270994496e-7,
-            "velocityX": -3.7822377648596976,
-            "velocityY": 0.07291187712592015,
-            "timestamp": 1.6720810091312561
-        },
-        {
-            "x": 6.01312270862256,
-            "y": 3.0289030469499765,
-            "heading": 2.9221268973831047,
-            "angularVelocity": -2.7984114359464816e-7,
-            "velocityX": -3.7823334410305938,
-            "velocityY": -0.06776730946506612,
-            "timestamp": 1.7536211327893874
-        },
-        {
-            "x": 5.704710773369897,
-            "y": 3.023377222654451,
-            "heading": 2.9221268745648237,
-            "angularVelocity": -2.7984113775105834e-7,
-            "velocityX": -3.782333425758887,
-            "velocityY": -0.06776816182780301,
-            "timestamp": 1.8351612564475186
-        },
-        {
-            "x": 5.396298838117239,
-            "y": 3.0178513983586757,
-            "heading": 2.922126851746543,
-            "angularVelocity": -2.7984113682317286e-7,
-            "velocityX": -3.7823334257588326,
-            "velocityY": -0.06776816183086862,
-            "timestamp": 1.9167013801056498
-        },
-        {
-            "x": 5.087886902401733,
-            "y": 3.012325599899855,
-            "heading": 2.922126828928091,
-            "angularVelocity": -2.798432358955337e-7,
-            "velocityX": -3.7823334314351498,
-            "velocityY": -0.06776784496901485,
-            "timestamp": 1.998241503763781
-        },
-        {
-            "x": 4.812446473600025,
-            "y": 3.0084984766412206,
-            "heading": 2.8330493298750046,
-            "angularVelocity": -1.0924376252672112,
-            "velocityX": -3.3779741364697116,
-            "velocityY": -0.04693546056758059,
-            "timestamp": 2.0797816274219123
-        },
-        {
-            "x": 4.592410263299027,
-            "y": 3.0051727297391726,
-            "heading": 2.737251875069825,
-            "angularVelocity": -1.1748504969997702,
-            "velocityX": -2.6985022885608188,
-            "velocityY": -0.040786630591711566,
-            "timestamp": 2.1613217510800435
-        },
-        {
-            "x": 4.427608751414137,
-            "y": 3.0026089111743004,
-            "heading": 2.658186226973151,
-            "angularVelocity": -0.9696532768109137,
-            "velocityX": -2.0211094181785265,
-            "velocityY": -0.03144241693353635,
-            "timestamp": 2.2428618747381748
-        },
-        {
-            "x": 4.3178442485460256,
-            "y": 3.0008747293604148,
-            "heading": 2.602696579769804,
-            "angularVelocity": -0.6805195370563186,
-            "velocityX": -1.346140991008498,
-            "velocityY": -0.021267833994898407,
-            "timestamp": 2.324401998396306
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -0.3519320119027408,
-            "velocityX": -0.6726044318496349,
-            "velocityY": -0.010727594234238386,
-            "timestamp": 2.4059421220544372
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": -1.5467932880075213e-32,
-            "velocityY": 0,
-            "timestamp": 2.4874822457125685
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S3.traj
deleted file mode 100644
index 0baf5ca9..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S3.traj	
+++ /dev/null
@@ -1,301 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 2.0168042378406012e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.220318940234659,
-            "y": 1.6240042188799209,
-            "heading": 2.522514839920101,
-            "angularVelocity": 0.3066580902466342,
-            "velocityX": 0.6036357185902667,
-            "velocityY": 0.05453852299210067,
-            "timestamp": 0.07342000956828917
-        },
-        {
-            "x": 5.309030400883647,
-            "y": 1.632013165892952,
-            "heading": 2.5656393100423864,
-            "angularVelocity": 0.5873667189075205,
-            "velocityX": 1.208273618739824,
-            "velocityY": 0.10908398214768973,
-            "timestamp": 0.14684001913657835
-        },
-        {
-            "x": 5.442235678671135,
-            "y": 1.6440289719213574,
-            "heading": 2.6263948924042286,
-            "angularVelocity": 0.8275071430674761,
-            "velocityX": 1.8142912071346369,
-            "velocityY": 0.1636584644847863,
-            "timestamp": 0.22026002870486752
-        },
-        {
-            "x": 5.6200752852220655,
-            "y": 1.6600527803178184,
-            "heading": 2.6994681964366594,
-            "angularVelocity": 0.9952777786614565,
-            "velocityX": 2.4222226011223906,
-            "velocityY": 0.21824851958859787,
-            "timestamp": 0.2936800382731567
-        },
-        {
-            "x": 5.842683997650443,
-            "y": 1.680064789350092,
-            "heading": 2.773109972156216,
-            "angularVelocity": 1.0030205137886674,
-            "velocityX": 3.031989695143347,
-            "velocityY": 0.2725688698481895,
-            "timestamp": 0.3671000478414459
-        },
-        {
-            "x": 6.108354667125484,
-            "y": 1.7034775424781843,
-            "heading": 2.8018843070789763,
-            "angularVelocity": 0.39191407208947715,
-            "velocityX": 3.6185049693835545,
-            "velocityY": 0.31888790625005003,
-            "timestamp": 0.4405200574097351
-        },
-        {
-            "x": 6.384952370737624,
-            "y": 1.7286802043771023,
-            "heading": 2.8018843326053964,
-            "angularVelocity": 3.476766132233899e-7,
-            "velocityX": 3.7673340719858177,
-            "velocityY": 0.34326693836064737,
-            "timestamp": 0.5139400669780243
-        },
-        {
-            "x": 6.661294346947145,
-            "y": 1.7565473171945905,
-            "heading": 2.8018843581575315,
-            "angularVelocity": 3.48026857889212e-7,
-            "velocityX": 3.7638509969478653,
-            "velocityY": 0.37955746643565225,
-            "timestamp": 0.5873600765463135
-        },
-        {
-            "x": 6.9295239726753675,
-            "y": 1.8286189976607798,
-            "heading": 2.8018844037308708,
-            "angularVelocity": 6.207209630445694e-7,
-            "velocityX": 3.6533586321414817,
-            "velocityY": 0.9816354000767032,
-            "timestamp": 0.6607800861146027
-        },
-        {
-            "x": 7.165852331775521,
-            "y": 1.9289604111011978,
-            "heading": 2.8534870271763118,
-            "angularVelocity": 0.7028414154241911,
-            "velocityX": 3.218854921019001,
-            "velocityY": 1.3666766598157905,
-            "timestamp": 0.7342000956828919
-        },
-        {
-            "x": 7.3582678693157275,
-            "y": 2.0352947515499316,
-            "heading": 2.9321246763669695,
-            "angularVelocity": 1.0710656352818668,
-            "velocityX": 2.620750646473749,
-            "velocityY": 1.4483019148865994,
-            "timestamp": 0.8076201052511811
-        },
-        {
-            "x": 7.505754872565929,
-            "y": 2.142580812119965,
-            "heading": 3.007723504913372,
-            "angularVelocity": 1.0296760922659598,
-            "velocityX": 2.0088120951961708,
-            "velocityY": 1.4612645953179093,
-            "timestamp": 0.8810401148194703
-        },
-        {
-            "x": 7.608443195664495,
-            "y": 2.2491262386362876,
-            "heading": 3.070552698416666,
-            "angularVelocity": 0.855750276698862,
-            "velocityX": 1.3986421917181704,
-            "velocityY": 1.4511769631033729,
-            "timestamp": 0.9544601243877595
-        },
-        {
-            "x": 7.666484933948513,
-            "y": 2.354097419514004,
-            "heading": 3.115652624171886,
-            "angularVelocity": 0.6142729484838055,
-            "velocityX": 0.7905438670643103,
-            "velocityY": 1.4297353200436178,
-            "timestamp": 1.0278801339560486
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 0.331617715269703,
-            "velocityX": 0.18407878357617766,
-            "velocityY": 1.4015604341523722,
-            "timestamp": 1.1013001435243377
-        },
-        {
-            "x": 7.640202623194362,
-            "y": 2.56833046361043,
-            "heading": 3.1389376214813485,
-            "angularVelocity": -0.013028904924243205,
-            "velocityX": -0.48807108721701464,
-            "velocityY": 1.3653457784439293,
-            "timestamp": 1.1828402671824687
-        },
-        {
-            "x": 7.5455521042399925,
-            "y": 2.676247019412303,
-            "heading": 3.111715108217083,
-            "angularVelocity": -0.3338542062850063,
-            "velocityX": -1.1607845893293163,
-            "velocityY": 1.3234779512271038,
-            "timestamp": 1.2643803908406
-        },
-        {
-            "x": 7.395994092884954,
-            "y": 2.780007432926005,
-            "heading": 3.061424395012115,
-            "angularVelocity": -0.616760325454245,
-            "velocityX": -1.8341646375480058,
-            "velocityY": 1.2725074338705782,
-            "timestamp": 1.3459205144987312
-        },
-        {
-            "x": 7.191491634258091,
-            "y": 2.8782237282133547,
-            "heading": 2.9936811867966338,
-            "angularVelocity": -0.8307959955948208,
-            "velocityX": -2.5079978966461463,
-            "velocityY": 1.2045149170871063,
-            "timestamp": 1.4274606381568624
-        },
-        {
-            "x": 6.932297950713545,
-            "y": 2.9674313510808874,
-            "heading": 2.9221270442336884,
-            "angularVelocity": -0.8775329169593383,
-            "velocityX": -3.178725661874757,
-            "velocityY": 1.0940334508388325,
-            "timestamp": 1.5090007618149937
-        },
-        {
-            "x": 6.629938780171593,
-            "y": 3.0284835582667458,
-            "heading": 2.9221269432503374,
-            "angularVelocity": -0.0000012384498122465325,
-            "velocityX": -3.7081029188726404,
-            "velocityY": 0.7487382217106987,
-            "timestamp": 1.5905408854731249
-        },
-        {
-            "x": 6.3215346451204795,
-            "y": 3.034428801743739,
-            "heading": 2.922126920201386,
-            "angularVelocity": -2.8267005270994496e-7,
-            "velocityX": -3.7822377648596976,
-            "velocityY": 0.07291187712592015,
-            "timestamp": 1.6720810091312561
-        },
-        {
-            "x": 6.01312270862256,
-            "y": 3.0289030469499765,
-            "heading": 2.9221268973831047,
-            "angularVelocity": -2.7984114359464816e-7,
-            "velocityX": -3.7823334410305938,
-            "velocityY": -0.06776730946506612,
-            "timestamp": 1.7536211327893874
-        },
-        {
-            "x": 5.704710773369897,
-            "y": 3.023377222654451,
-            "heading": 2.9221268745648237,
-            "angularVelocity": -2.7984113775105834e-7,
-            "velocityX": -3.782333425758887,
-            "velocityY": -0.06776816182780301,
-            "timestamp": 1.8351612564475186
-        },
-        {
-            "x": 5.396298838117239,
-            "y": 3.0178513983586757,
-            "heading": 2.922126851746543,
-            "angularVelocity": -2.7984113682317286e-7,
-            "velocityX": -3.7823334257588326,
-            "velocityY": -0.06776816183086862,
-            "timestamp": 1.9167013801056498
-        },
-        {
-            "x": 5.087886902401733,
-            "y": 3.012325599899855,
-            "heading": 2.922126828928091,
-            "angularVelocity": -2.798432358955337e-7,
-            "velocityX": -3.7823334314351498,
-            "velocityY": -0.06776784496901485,
-            "timestamp": 1.998241503763781
-        },
-        {
-            "x": 4.812446473600025,
-            "y": 3.0084984766412206,
-            "heading": 2.8330493298750046,
-            "angularVelocity": -1.0924376252672112,
-            "velocityX": -3.3779741364697116,
-            "velocityY": -0.04693546056758059,
-            "timestamp": 2.0797816274219123
-        },
-        {
-            "x": 4.592410263299027,
-            "y": 3.0051727297391726,
-            "heading": 2.737251875069825,
-            "angularVelocity": -1.1748504969997702,
-            "velocityX": -2.6985022885608188,
-            "velocityY": -0.040786630591711566,
-            "timestamp": 2.1613217510800435
-        },
-        {
-            "x": 4.427608751414137,
-            "y": 3.0026089111743004,
-            "heading": 2.658186226973151,
-            "angularVelocity": -0.9696532768109137,
-            "velocityX": -2.0211094181785265,
-            "velocityY": -0.03144241693353635,
-            "timestamp": 2.2428618747381748
-        },
-        {
-            "x": 4.3178442485460256,
-            "y": 3.0008747293604148,
-            "heading": 2.602696579769804,
-            "angularVelocity": -0.6805195370563186,
-            "velocityX": -1.346140991008498,
-            "velocityY": -0.021267833994898407,
-            "timestamp": 2.324401998396306
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -0.3519320119027408,
-            "velocityX": -0.6726044318496349,
-            "velocityY": -0.010727594234238386,
-            "timestamp": 2.4059421220544372
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": -1.5467932880075213e-32,
-            "velocityY": 0,
-            "timestamp": 2.4874822457125685
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S4.1.traj
deleted file mode 100644
index 7a429afb..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S4.1.traj	
+++ /dev/null
@@ -1,283 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.2208311166528825,
-            "y": 1.6350333822394376,
-            "heading": 2.5207239283142147,
-            "angularVelocity": 0.274168379737783,
-            "velocityX": 0.5930957889931558,
-            "velocityY": 0.1988849791445399,
-            "timestamp": 0.07558832398555491
-        },
-        {
-            "x": 5.31056444260237,
-            "y": 1.6651195189308639,
-            "heading": 2.5599126898589697,
-            "angularVelocity": 0.5184499335141133,
-            "velocityX": 1.1871321021304377,
-            "velocityY": 0.39802624406880477,
-            "timestamp": 0.15117664797110983
-        },
-        {
-            "x": 5.445292613790804,
-            "y": 1.7102861706342203,
-            "heading": 2.6140890806736206,
-            "angularVelocity": 0.716729621164803,
-            "velocityX": 1.7823939476973087,
-            "velocityY": 0.5975347688882264,
-            "timestamp": 0.22676497195666473
-        },
-        {
-            "x": 5.625130316677607,
-            "y": 1.7705705454307188,
-            "heading": 2.677161642931129,
-            "angularVelocity": 0.834422023559608,
-            "velocityX": 2.379173044254461,
-            "velocityY": 0.79753554011886,
-            "timestamp": 0.30235329594221966
-        },
-        {
-            "x": 5.850123959727638,
-            "y": 1.8459876978345418,
-            "heading": 2.735754513087534,
-            "angularVelocity": 0.7751576839777234,
-            "velocityX": 2.97656610421776,
-            "velocityY": 0.9977354758948757,
-            "timestamp": 0.3779416199277746
-        },
-        {
-            "x": 6.118241955907835,
-            "y": 1.9356767665379448,
-            "heading": 2.743662980850177,
-            "angularVelocity": 0.10462552079005556,
-            "velocityX": 3.547082168820654,
-            "velocityY": 1.1865465983945254,
-            "timestamp": 0.4535299439133295
-        },
-        {
-            "x": 6.3893659801779465,
-            "y": 2.026544533897615,
-            "heading": 2.7436630046141004,
-            "angularVelocity": 3.143861609924561e-7,
-            "velocityX": 3.5868505871611136,
-            "velocityY": 1.2021402588187382,
-            "timestamp": 0.5291182678988844
-        },
-        {
-            "x": 6.660490004070523,
-            "y": 2.1174123023837605,
-            "heading": 2.7436630283780157,
-            "angularVelocity": 3.1438606173533106e-7,
-            "velocityX": 3.5868505821664702,
-            "velocityY": 1.2021402737215172,
-            "timestamp": 0.6047065918844393
-        },
-        {
-            "x": 6.931614067804288,
-            "y": 2.2082799519937164,
-            "heading": 2.743663052142803,
-            "angularVelocity": 3.1439759977679495e-7,
-            "velocityX": 3.5868511092477546,
-            "velocityY": 1.2021387010422189,
-            "timestamp": 0.6802949158699942
-        },
-        {
-            "x": 7.168848548798668,
-            "y": 2.287192936383632,
-            "heading": 2.8325335746925835,
-            "angularVelocity": 1.1757175958389672,
-            "velocityX": 3.1385069609389844,
-            "velocityY": 1.0439837825349283,
-            "timestamp": 0.7558832398555492
-        },
-        {
-            "x": 7.360921839643199,
-            "y": 2.3511354933788766,
-            "heading": 2.9301346596996316,
-            "angularVelocity": 1.2912190648082638,
-            "velocityX": 2.541044445981228,
-            "velocityY": 0.8459316680637452,
-            "timestamp": 0.8314715638411041
-        },
-        {
-            "x": 7.507932695990372,
-            "y": 2.4000526423462776,
-            "heading": 3.014739800319359,
-            "angularVelocity": 1.119288484765134,
-            "velocityX": 1.944888424504045,
-            "velocityY": 0.6471521842017212,
-            "timestamp": 0.907059887826659
-        },
-        {
-            "x": 7.6100514108149255,
-            "y": 2.4339833202352414,
-            "heading": 3.0798385166063986,
-            "angularVelocity": 0.8612271426930879,
-            "velocityX": 1.3509853035511394,
-            "velocityY": 0.4488878189103197,
-            "timestamp": 0.982648211812214
-        },
-        {
-            "x": 7.667383998050378,
-            "y": 2.452958398052505,
-            "heading": 3.1222248732691185,
-            "angularVelocity": 0.5607526986683374,
-            "velocityX": 0.7584846999174467,
-            "velocityY": 0.25103186334555994,
-            "timestamp": 1.0582365357977688
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 0.23515704269716123,
-            "velocityX": 0.16690411011130135,
-            "velocityY": 0.05346860115942382,
-            "timestamp": 1.1338248597833236
-        },
-        {
-            "x": 7.644913557520251,
-            "y": 2.4451050214750283,
-            "heading": 3.130617959515128,
-            "angularVelocity": -0.11950075188579921,
-            "velocityX": -0.44690238377135916,
-            "velocityY": -0.1515084996373644,
-            "timestamp": 1.212335164838424
-        },
-        {
-            "x": 7.561539712456839,
-            "y": 2.4170767124271264,
-            "heading": 3.096030936228231,
-            "angularVelocity": -0.44054119089994853,
-            "velocityX": -1.0619477915019166,
-            "velocityY": -0.3570016576579214,
-            "timestamp": 1.2908454698935243
-        },
-        {
-            "x": 7.429747941085825,
-            "y": 2.3728628641773573,
-            "heading": 3.040236463304404,
-            "angularVelocity": -0.7106643246980755,
-            "velocityX": -1.6786557035858298,
-            "velocityY": -0.5631598070945942,
-            "timestamp": 1.3693557749486247
-        },
-        {
-            "x": 7.249364854543017,
-            "y": 2.3124007511137603,
-            "heading": 2.9700149395216235,
-            "angularVelocity": -0.8944242890598059,
-            "velocityX": -2.2975721010917782,
-            "velocityY": -0.7701169040314122,
-            "timestamp": 1.447866080003725
-        },
-        {
-            "x": 7.020237112848181,
-            "y": 2.2356470641616397,
-            "heading": 2.9000211501626434,
-            "angularVelocity": -0.8915235943849231,
-            "velocityX": -2.9184416177471677,
-            "velocityY": -0.977625636510412,
-            "timestamp": 1.5263763850588254
-        },
-        {
-            "x": 6.744411982227483,
-            "y": 2.1437703180815415,
-            "heading": 2.8843371221032816,
-            "angularVelocity": -0.19977031102303894,
-            "velocityX": -3.5132347330342664,
-            "velocityY": -1.1702507844749341,
-            "timestamp": 1.6048866901139258
-        },
-        {
-            "x": 6.462777076833342,
-            "y": 2.049479989397194,
-            "heading": 2.8843370939243,
-            "angularVelocity": -3.589208009765681e-7,
-            "velocityX": -3.5872348884198493,
-            "velocityY": -1.2009930240135072,
-            "timestamp": 1.6833969951690262
-        },
-        {
-            "x": 6.181142164450445,
-            "y": 1.955189681587478,
-            "heading": 2.884337065745333,
-            "angularVelocity": -3.589206129086503e-7,
-            "velocityX": -3.5872349774369066,
-            "velocityY": -1.200992758129539,
-            "timestamp": 1.7619073002241266
-        },
-        {
-            "x": 5.898328744228412,
-            "y": 1.8644954419041788,
-            "heading": 2.8843369910470673,
-            "angularVelocity": -9.514453601861041e-7,
-            "velocityX": -3.6022458456064737,
-            "velocityY": -1.1551889859509814,
-            "timestamp": 1.840417605279227
-        },
-        {
-            "x": 5.657030939387785,
-            "y": 1.782595874715138,
-            "heading": 2.7806821628594123,
-            "angularVelocity": -1.3202703532347235,
-            "velocityX": -3.0734539201099995,
-            "velocityY": -1.043169646730562,
-            "timestamp": 1.9189279103343273
-        },
-        {
-            "x": 5.464337892157779,
-            "y": 1.7174002123662173,
-            "heading": 2.6765644069162446,
-            "angularVelocity": -1.3261667480478392,
-            "velocityX": -2.4543663038217995,
-            "velocityY": -0.8304089801098533,
-            "timestamp": 1.9974382153894277
-        },
-        {
-            "x": 5.320057899798496,
-            "y": 1.6686446362166114,
-            "heading": 2.5910880570386063,
-            "angularVelocity": -1.0887277767886236,
-            "velocityX": -1.8377204401132547,
-            "velocityY": -0.6210086193829906,
-            "timestamp": 2.075948520444528
-        },
-        {
-            "x": 5.223989702910862,
-            "y": 1.6362005448482675,
-            "heading": 2.5310608538348376,
-            "angularVelocity": -0.7645773782389272,
-            "velocityX": -1.2236380538862024,
-            "velocityY": -0.41324627825065113,
-            "timestamp": 2.154458825499628
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.3956277308187441,
-            "velocityX": -0.6112535529849511,
-            "velocityY": -0.2063492790774072,
-            "timestamp": 2.2329691305547286
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 3.766832812159788e-31,
-            "velocityX": 0,
-            "velocityY": 1.2430862122144045e-32,
-            "timestamp": 2.311479435609829
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S4.traj
deleted file mode 100644
index 7a429afb..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4 To S4.traj	
+++ /dev/null
@@ -1,283 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.2208311166528825,
-            "y": 1.6350333822394376,
-            "heading": 2.5207239283142147,
-            "angularVelocity": 0.274168379737783,
-            "velocityX": 0.5930957889931558,
-            "velocityY": 0.1988849791445399,
-            "timestamp": 0.07558832398555491
-        },
-        {
-            "x": 5.31056444260237,
-            "y": 1.6651195189308639,
-            "heading": 2.5599126898589697,
-            "angularVelocity": 0.5184499335141133,
-            "velocityX": 1.1871321021304377,
-            "velocityY": 0.39802624406880477,
-            "timestamp": 0.15117664797110983
-        },
-        {
-            "x": 5.445292613790804,
-            "y": 1.7102861706342203,
-            "heading": 2.6140890806736206,
-            "angularVelocity": 0.716729621164803,
-            "velocityX": 1.7823939476973087,
-            "velocityY": 0.5975347688882264,
-            "timestamp": 0.22676497195666473
-        },
-        {
-            "x": 5.625130316677607,
-            "y": 1.7705705454307188,
-            "heading": 2.677161642931129,
-            "angularVelocity": 0.834422023559608,
-            "velocityX": 2.379173044254461,
-            "velocityY": 0.79753554011886,
-            "timestamp": 0.30235329594221966
-        },
-        {
-            "x": 5.850123959727638,
-            "y": 1.8459876978345418,
-            "heading": 2.735754513087534,
-            "angularVelocity": 0.7751576839777234,
-            "velocityX": 2.97656610421776,
-            "velocityY": 0.9977354758948757,
-            "timestamp": 0.3779416199277746
-        },
-        {
-            "x": 6.118241955907835,
-            "y": 1.9356767665379448,
-            "heading": 2.743662980850177,
-            "angularVelocity": 0.10462552079005556,
-            "velocityX": 3.547082168820654,
-            "velocityY": 1.1865465983945254,
-            "timestamp": 0.4535299439133295
-        },
-        {
-            "x": 6.3893659801779465,
-            "y": 2.026544533897615,
-            "heading": 2.7436630046141004,
-            "angularVelocity": 3.143861609924561e-7,
-            "velocityX": 3.5868505871611136,
-            "velocityY": 1.2021402588187382,
-            "timestamp": 0.5291182678988844
-        },
-        {
-            "x": 6.660490004070523,
-            "y": 2.1174123023837605,
-            "heading": 2.7436630283780157,
-            "angularVelocity": 3.1438606173533106e-7,
-            "velocityX": 3.5868505821664702,
-            "velocityY": 1.2021402737215172,
-            "timestamp": 0.6047065918844393
-        },
-        {
-            "x": 6.931614067804288,
-            "y": 2.2082799519937164,
-            "heading": 2.743663052142803,
-            "angularVelocity": 3.1439759977679495e-7,
-            "velocityX": 3.5868511092477546,
-            "velocityY": 1.2021387010422189,
-            "timestamp": 0.6802949158699942
-        },
-        {
-            "x": 7.168848548798668,
-            "y": 2.287192936383632,
-            "heading": 2.8325335746925835,
-            "angularVelocity": 1.1757175958389672,
-            "velocityX": 3.1385069609389844,
-            "velocityY": 1.0439837825349283,
-            "timestamp": 0.7558832398555492
-        },
-        {
-            "x": 7.360921839643199,
-            "y": 2.3511354933788766,
-            "heading": 2.9301346596996316,
-            "angularVelocity": 1.2912190648082638,
-            "velocityX": 2.541044445981228,
-            "velocityY": 0.8459316680637452,
-            "timestamp": 0.8314715638411041
-        },
-        {
-            "x": 7.507932695990372,
-            "y": 2.4000526423462776,
-            "heading": 3.014739800319359,
-            "angularVelocity": 1.119288484765134,
-            "velocityX": 1.944888424504045,
-            "velocityY": 0.6471521842017212,
-            "timestamp": 0.907059887826659
-        },
-        {
-            "x": 7.6100514108149255,
-            "y": 2.4339833202352414,
-            "heading": 3.0798385166063986,
-            "angularVelocity": 0.8612271426930879,
-            "velocityX": 1.3509853035511394,
-            "velocityY": 0.4488878189103197,
-            "timestamp": 0.982648211812214
-        },
-        {
-            "x": 7.667383998050378,
-            "y": 2.452958398052505,
-            "heading": 3.1222248732691185,
-            "angularVelocity": 0.5607526986683374,
-            "velocityX": 0.7584846999174467,
-            "velocityY": 0.25103186334555994,
-            "timestamp": 1.0582365357977688
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 0.23515704269716123,
-            "velocityX": 0.16690411011130135,
-            "velocityY": 0.05346860115942382,
-            "timestamp": 1.1338248597833236
-        },
-        {
-            "x": 7.644913557520251,
-            "y": 2.4451050214750283,
-            "heading": 3.130617959515128,
-            "angularVelocity": -0.11950075188579921,
-            "velocityX": -0.44690238377135916,
-            "velocityY": -0.1515084996373644,
-            "timestamp": 1.212335164838424
-        },
-        {
-            "x": 7.561539712456839,
-            "y": 2.4170767124271264,
-            "heading": 3.096030936228231,
-            "angularVelocity": -0.44054119089994853,
-            "velocityX": -1.0619477915019166,
-            "velocityY": -0.3570016576579214,
-            "timestamp": 1.2908454698935243
-        },
-        {
-            "x": 7.429747941085825,
-            "y": 2.3728628641773573,
-            "heading": 3.040236463304404,
-            "angularVelocity": -0.7106643246980755,
-            "velocityX": -1.6786557035858298,
-            "velocityY": -0.5631598070945942,
-            "timestamp": 1.3693557749486247
-        },
-        {
-            "x": 7.249364854543017,
-            "y": 2.3124007511137603,
-            "heading": 2.9700149395216235,
-            "angularVelocity": -0.8944242890598059,
-            "velocityX": -2.2975721010917782,
-            "velocityY": -0.7701169040314122,
-            "timestamp": 1.447866080003725
-        },
-        {
-            "x": 7.020237112848181,
-            "y": 2.2356470641616397,
-            "heading": 2.9000211501626434,
-            "angularVelocity": -0.8915235943849231,
-            "velocityX": -2.9184416177471677,
-            "velocityY": -0.977625636510412,
-            "timestamp": 1.5263763850588254
-        },
-        {
-            "x": 6.744411982227483,
-            "y": 2.1437703180815415,
-            "heading": 2.8843371221032816,
-            "angularVelocity": -0.19977031102303894,
-            "velocityX": -3.5132347330342664,
-            "velocityY": -1.1702507844749341,
-            "timestamp": 1.6048866901139258
-        },
-        {
-            "x": 6.462777076833342,
-            "y": 2.049479989397194,
-            "heading": 2.8843370939243,
-            "angularVelocity": -3.589208009765681e-7,
-            "velocityX": -3.5872348884198493,
-            "velocityY": -1.2009930240135072,
-            "timestamp": 1.6833969951690262
-        },
-        {
-            "x": 6.181142164450445,
-            "y": 1.955189681587478,
-            "heading": 2.884337065745333,
-            "angularVelocity": -3.589206129086503e-7,
-            "velocityX": -3.5872349774369066,
-            "velocityY": -1.200992758129539,
-            "timestamp": 1.7619073002241266
-        },
-        {
-            "x": 5.898328744228412,
-            "y": 1.8644954419041788,
-            "heading": 2.8843369910470673,
-            "angularVelocity": -9.514453601861041e-7,
-            "velocityX": -3.6022458456064737,
-            "velocityY": -1.1551889859509814,
-            "timestamp": 1.840417605279227
-        },
-        {
-            "x": 5.657030939387785,
-            "y": 1.782595874715138,
-            "heading": 2.7806821628594123,
-            "angularVelocity": -1.3202703532347235,
-            "velocityX": -3.0734539201099995,
-            "velocityY": -1.043169646730562,
-            "timestamp": 1.9189279103343273
-        },
-        {
-            "x": 5.464337892157779,
-            "y": 1.7174002123662173,
-            "heading": 2.6765644069162446,
-            "angularVelocity": -1.3261667480478392,
-            "velocityX": -2.4543663038217995,
-            "velocityY": -0.8304089801098533,
-            "timestamp": 1.9974382153894277
-        },
-        {
-            "x": 5.320057899798496,
-            "y": 1.6686446362166114,
-            "heading": 2.5910880570386063,
-            "angularVelocity": -1.0887277767886236,
-            "velocityX": -1.8377204401132547,
-            "velocityY": -0.6210086193829906,
-            "timestamp": 2.075948520444528
-        },
-        {
-            "x": 5.223989702910862,
-            "y": 1.6362005448482675,
-            "heading": 2.5310608538348376,
-            "angularVelocity": -0.7645773782389272,
-            "velocityX": -1.2236380538862024,
-            "velocityY": -0.41324627825065113,
-            "timestamp": 2.154458825499628
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.3956277308187441,
-            "velocityX": -0.6112535529849511,
-            "velocityY": -0.2063492790774072,
-            "timestamp": 2.2329691305547286
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 3.766832812159788e-31,
-            "velocityX": 0,
-            "velocityY": 1.2430862122144045e-32,
-            "timestamp": 2.311479435609829
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S3.1.traj
deleted file mode 100644
index 19b7b304..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S3.1.traj	
+++ /dev/null
@@ -1,319 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.21689679817953,
-            "y": 1.6000159633135866,
-            "heading": 2.520043259980684,
-            "angularVelocity": 0.2702645151942541,
-            "velocityX": 0.5514548702977145,
-            "velocityY": -0.2694659447557082,
-            "timestamp": 0.0741616411102716
-        },
-        {
-            "x": 5.298748395600195,
-            "y": 1.5600344658293337,
-            "heading": 2.558338172376498,
-            "angularVelocity": 0.5163708869233576,
-            "velocityX": 1.1036918303757857,
-            "velocityY": -0.5391129010319948,
-            "timestamp": 0.1483232822205432
-        },
-        {
-            "x": 5.421633479629326,
-            "y": 1.5000375028373136,
-            "heading": 2.612131147144223,
-            "angularVelocity": 0.7253476859785596,
-            "velocityX": 1.656989815616613,
-            "velocityY": -0.8090026338927551,
-            "timestamp": 0.2224849233308148
-        },
-        {
-            "x": 5.585659490393147,
-            "y": 1.420004250322953,
-            "heading": 2.6766780389547544,
-            "angularVelocity": 0.8703541459463568,
-            "velocityX": 2.211736529939155,
-            "velocityY": -1.0791731590102869,
-            "timestamp": 0.2966465644410864
-        },
-        {
-            "x": 5.790940733567372,
-            "y": 1.3199448136082716,
-            "heading": 2.7418726481321705,
-            "angularVelocity": 0.8790880056234819,
-            "velocityX": 2.7680245488229662,
-            "velocityY": -1.3492074233619007,
-            "timestamp": 0.37080820555135796
-        },
-        {
-            "x": 6.036365145395861,
-            "y": 1.2006925884246085,
-            "heading": 2.766396446394458,
-            "angularVelocity": 0.3306803611021658,
-            "velocityX": 3.3093174335714934,
-            "velocityY": -1.6080041298754981,
-            "timestamp": 0.44496984666162953
-        },
-        {
-            "x": 6.288321832391407,
-            "y": 1.0773004957458903,
-            "heading": 2.7663964696788734,
-            "angularVelocity": 3.1396844397951453e-7,
-            "velocityX": 3.3973990222372876,
-            "velocityY": -1.6638263505420645,
-            "timestamp": 0.5191314877719011
-        },
-        {
-            "x": 6.540278624834678,
-            "y": 0.9539086183831178,
-            "heading": 2.766396492963265,
-            "angularVelocity": 3.1396812929995246e-7,
-            "velocityX": 3.397400444100689,
-            "velocityY": -1.663823447208988,
-            "timestamp": 0.5932931288821727
-        },
-        {
-            "x": 6.801316019800969,
-            "y": 0.8511114616545457,
-            "heading": 2.7663965374148853,
-            "angularVelocity": 5.993883039125867e-7,
-            "velocityX": 3.519843831100697,
-            "velocityY": -1.3861230036120238,
-            "timestamp": 0.6674547699924442
-        },
-        {
-            "x": 7.047191253658378,
-            "y": 0.7842163440856953,
-            "heading": 2.8499152738316673,
-            "angularVelocity": 1.1261716322139188,
-            "velocityX": 3.3153963447464685,
-            "velocityY": -0.9020177623819695,
-            "timestamp": 0.7416164111027158
-        },
-        {
-            "x": 7.2541679092956945,
-            "y": 0.7415676707832506,
-            "heading": 2.93858588461799,
-            "angularVelocity": 1.1956398140448308,
-            "velocityX": 2.790885591778509,
-            "velocityY": -0.575077259132303,
-            "timestamp": 0.8157780522129874
-        },
-        {
-            "x": 7.421132571302565,
-            "y": 0.7211096612789919,
-            "heading": 3.016140310820358,
-            "angularVelocity": 1.045748516905977,
-            "velocityX": 2.251361478889076,
-            "velocityY": -0.2758570225521344,
-            "timestamp": 0.889939693323259
-        },
-        {
-            "x": 7.547824539966462,
-            "y": 0.722094092716002,
-            "heading": 3.0771627100726597,
-            "angularVelocity": 0.8228296776978581,
-            "velocityX": 1.7083220754986685,
-            "velocityY": 0.013274132317907437,
-            "timestamp": 0.9641013344335305
-        },
-        {
-            "x": 7.634133595568631,
-            "y": 0.7441357582804435,
-            "heading": 3.1189807676901022,
-            "angularVelocity": 0.5638771876052062,
-            "velocityX": 1.1637964628348345,
-            "velocityY": 0.29721113549347455,
-            "timestamp": 1.0382629755438022
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 0.28342458439730217,
-            "velocityX": 0.6184653379387841,
-            "velocityY": 0.5779839965491583,
-            "timestamp": 1.1124246166540739
-        },
-        {
-            "x": 7.681059166008756,
-            "y": 0.8599952012380814,
-            "heading": 3.136373480041968,
-            "angularVelocity": -0.044073969444744315,
-            "velocityX": 0.012872299297217518,
-            "velocityY": 0.8871282403539436,
-            "timestamp": 1.1947071975906878
-        },
-        {
-            "x": 7.6321422839644315,
-            "y": 0.9582569372062664,
-            "heading": 3.107236664084856,
-            "angularVelocity": -0.3541067334720519,
-            "velocityX": -0.5944986349176513,
-            "velocityY": 1.1941985150402432,
-            "timestamp": 1.2769897785273017
-        },
-        {
-            "x": 7.533021561059945,
-            "y": 1.0815072200967695,
-            "heading": 3.0548999267391683,
-            "angularVelocity": -0.6360609590747484,
-            "velocityX": -1.204637989914761,
-            "velocityY": 1.4978903370258465,
-            "timestamp": 1.3592723594639156
-        },
-        {
-            "x": 7.383292536414222,
-            "y": 1.229213843710233,
-            "heading": 2.9837724589347805,
-            "angularVelocity": -0.8644292266327552,
-            "velocityX": -1.819692855296544,
-            "velocityY": 1.7951141290432664,
-            "timestamp": 1.4415549404005294
-        },
-        {
-            "x": 7.182069766586899,
-            "y": 1.399958151778736,
-            "heading": 2.9050185961423023,
-            "angularVelocity": -0.9571146395266165,
-            "velocityX": -2.445508727811161,
-            "velocityY": 2.0750966501650514,
-            "timestamp": 1.5238375213371433
-        },
-        {
-            "x": 6.9296160896416765,
-            "y": 1.5820466019910795,
-            "heading": 2.9050185123503516,
-            "angularVelocity": -0.0000010183437364658426,
-            "velocityX": -3.068130266109342,
-            "velocityY": 2.2129647385832873,
-            "timestamp": 1.6061201022737572
-        },
-        {
-            "x": 6.6549097990315635,
-            "y": 1.728421234696809,
-            "heading": 2.9050184910214054,
-            "angularVelocity": -2.59215813112527e-7,
-            "velocityX": -3.338571633062085,
-            "velocityY": 1.7789261231181734,
-            "timestamp": 1.6884026832103711
-        },
-        {
-            "x": 6.380203216821482,
-            "y": 1.8747953201455823,
-            "heading": 2.905018469692463,
-            "angularVelocity": -2.59215765603584e-7,
-            "velocityX": -3.338575176946638,
-            "velocityY": 1.7789194721727335,
-            "timestamp": 1.770685264146985
-        },
-        {
-            "x": 6.105496634609771,
-            "y": 2.0211694055912957,
-            "heading": 2.9050184483635206,
-            "angularVelocity": -2.592157647120513e-7,
-            "velocityX": -3.3385751769664513,
-            "velocityY": 1.7789194721355484,
-            "timestamp": 1.852967845083599
-        },
-        {
-            "x": 5.83079005239806,
-            "y": 2.167543491037009,
-            "heading": 2.905018427034578,
-            "angularVelocity": -2.592157660778243e-7,
-            "velocityX": -3.3385751769664513,
-            "velocityY": 1.778919472135548,
-            "timestamp": 1.9352504260202128
-        },
-        {
-            "x": 5.556083470186349,
-            "y": 2.3139175764827224,
-            "heading": 2.905018405705636,
-            "angularVelocity": -2.592157651075657e-7,
-            "velocityX": -3.338575176966452,
-            "velocityY": 1.7789194721355481,
-            "timestamp": 2.0175330069568265
-        },
-        {
-            "x": 5.281376887974587,
-            "y": 2.460291661928342,
-            "heading": 2.9050183843766937,
-            "angularVelocity": -2.5921576594087425e-7,
-            "velocityX": -3.3385751769670593,
-            "velocityY": 1.7789194721344066,
-            "timestamp": 2.09981558789344
-        },
-        {
-            "x": 5.006670296810042,
-            "y": 2.6066657305717675,
-            "heading": 2.905018363047624,
-            "angularVelocity": -2.5921731220751964e-7,
-            "velocityX": -3.338575285772395,
-            "velocityY": 1.778919267933304,
-            "timestamp": 2.182098168830054
-        },
-        {
-            "x": 4.758260828306463,
-            "y": 2.7376390899942815,
-            "heading": 2.815277658009563,
-            "angularVelocity": -1.0906403763293109,
-            "velocityX": -3.0189800280443744,
-            "velocityY": 1.591750743980768,
-            "timestamp": 2.2643807497666675
-        },
-        {
-            "x": 4.559939575601031,
-            "y": 2.842572976973571,
-            "heading": 2.72503756976798,
-            "angularVelocity": -1.0967095005333054,
-            "velocityX": -2.4102458922406798,
-            "velocityY": 1.2752867713292104,
-            "timestamp": 2.346663330703281
-        },
-        {
-            "x": 4.4113908880653625,
-            "y": 2.9212861863102546,
-            "heading": 2.651616705628037,
-            "angularVelocity": -0.8923014239977028,
-            "velocityX": -1.8053479344565475,
-            "velocityY": 0.9566205682988939,
-            "timestamp": 2.428945911639895
-        },
-        {
-            "x": 4.31244325419048,
-            "y": 2.973762547349605,
-            "heading": 2.6003949192739833,
-            "angularVelocity": -0.6225106914611183,
-            "velocityX": -1.202534397299805,
-            "velocityY": 0.6377578394116608,
-            "timestamp": 2.5112284925765085
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -0.3207838034920002,
-            "velocityX": -0.6008957622339189,
-            "velocityY": 0.31887007373537113,
-            "timestamp": 2.593511073513122
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": -9.140912570286281e-33,
-            "velocityY": 0,
-            "timestamp": 2.675793654449736
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S3.traj
deleted file mode 100644
index 19b7b304..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S3.traj	
+++ /dev/null
@@ -1,319 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.21689679817953,
-            "y": 1.6000159633135866,
-            "heading": 2.520043259980684,
-            "angularVelocity": 0.2702645151942541,
-            "velocityX": 0.5514548702977145,
-            "velocityY": -0.2694659447557082,
-            "timestamp": 0.0741616411102716
-        },
-        {
-            "x": 5.298748395600195,
-            "y": 1.5600344658293337,
-            "heading": 2.558338172376498,
-            "angularVelocity": 0.5163708869233576,
-            "velocityX": 1.1036918303757857,
-            "velocityY": -0.5391129010319948,
-            "timestamp": 0.1483232822205432
-        },
-        {
-            "x": 5.421633479629326,
-            "y": 1.5000375028373136,
-            "heading": 2.612131147144223,
-            "angularVelocity": 0.7253476859785596,
-            "velocityX": 1.656989815616613,
-            "velocityY": -0.8090026338927551,
-            "timestamp": 0.2224849233308148
-        },
-        {
-            "x": 5.585659490393147,
-            "y": 1.420004250322953,
-            "heading": 2.6766780389547544,
-            "angularVelocity": 0.8703541459463568,
-            "velocityX": 2.211736529939155,
-            "velocityY": -1.0791731590102869,
-            "timestamp": 0.2966465644410864
-        },
-        {
-            "x": 5.790940733567372,
-            "y": 1.3199448136082716,
-            "heading": 2.7418726481321705,
-            "angularVelocity": 0.8790880056234819,
-            "velocityX": 2.7680245488229662,
-            "velocityY": -1.3492074233619007,
-            "timestamp": 0.37080820555135796
-        },
-        {
-            "x": 6.036365145395861,
-            "y": 1.2006925884246085,
-            "heading": 2.766396446394458,
-            "angularVelocity": 0.3306803611021658,
-            "velocityX": 3.3093174335714934,
-            "velocityY": -1.6080041298754981,
-            "timestamp": 0.44496984666162953
-        },
-        {
-            "x": 6.288321832391407,
-            "y": 1.0773004957458903,
-            "heading": 2.7663964696788734,
-            "angularVelocity": 3.1396844397951453e-7,
-            "velocityX": 3.3973990222372876,
-            "velocityY": -1.6638263505420645,
-            "timestamp": 0.5191314877719011
-        },
-        {
-            "x": 6.540278624834678,
-            "y": 0.9539086183831178,
-            "heading": 2.766396492963265,
-            "angularVelocity": 3.1396812929995246e-7,
-            "velocityX": 3.397400444100689,
-            "velocityY": -1.663823447208988,
-            "timestamp": 0.5932931288821727
-        },
-        {
-            "x": 6.801316019800969,
-            "y": 0.8511114616545457,
-            "heading": 2.7663965374148853,
-            "angularVelocity": 5.993883039125867e-7,
-            "velocityX": 3.519843831100697,
-            "velocityY": -1.3861230036120238,
-            "timestamp": 0.6674547699924442
-        },
-        {
-            "x": 7.047191253658378,
-            "y": 0.7842163440856953,
-            "heading": 2.8499152738316673,
-            "angularVelocity": 1.1261716322139188,
-            "velocityX": 3.3153963447464685,
-            "velocityY": -0.9020177623819695,
-            "timestamp": 0.7416164111027158
-        },
-        {
-            "x": 7.2541679092956945,
-            "y": 0.7415676707832506,
-            "heading": 2.93858588461799,
-            "angularVelocity": 1.1956398140448308,
-            "velocityX": 2.790885591778509,
-            "velocityY": -0.575077259132303,
-            "timestamp": 0.8157780522129874
-        },
-        {
-            "x": 7.421132571302565,
-            "y": 0.7211096612789919,
-            "heading": 3.016140310820358,
-            "angularVelocity": 1.045748516905977,
-            "velocityX": 2.251361478889076,
-            "velocityY": -0.2758570225521344,
-            "timestamp": 0.889939693323259
-        },
-        {
-            "x": 7.547824539966462,
-            "y": 0.722094092716002,
-            "heading": 3.0771627100726597,
-            "angularVelocity": 0.8228296776978581,
-            "velocityX": 1.7083220754986685,
-            "velocityY": 0.013274132317907437,
-            "timestamp": 0.9641013344335305
-        },
-        {
-            "x": 7.634133595568631,
-            "y": 0.7441357582804435,
-            "heading": 3.1189807676901022,
-            "angularVelocity": 0.5638771876052062,
-            "velocityX": 1.1637964628348345,
-            "velocityY": 0.29721113549347455,
-            "timestamp": 1.0382629755438022
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 0.28342458439730217,
-            "velocityX": 0.6184653379387841,
-            "velocityY": 0.5779839965491583,
-            "timestamp": 1.1124246166540739
-        },
-        {
-            "x": 7.681059166008756,
-            "y": 0.8599952012380814,
-            "heading": 3.136373480041968,
-            "angularVelocity": -0.044073969444744315,
-            "velocityX": 0.012872299297217518,
-            "velocityY": 0.8871282403539436,
-            "timestamp": 1.1947071975906878
-        },
-        {
-            "x": 7.6321422839644315,
-            "y": 0.9582569372062664,
-            "heading": 3.107236664084856,
-            "angularVelocity": -0.3541067334720519,
-            "velocityX": -0.5944986349176513,
-            "velocityY": 1.1941985150402432,
-            "timestamp": 1.2769897785273017
-        },
-        {
-            "x": 7.533021561059945,
-            "y": 1.0815072200967695,
-            "heading": 3.0548999267391683,
-            "angularVelocity": -0.6360609590747484,
-            "velocityX": -1.204637989914761,
-            "velocityY": 1.4978903370258465,
-            "timestamp": 1.3592723594639156
-        },
-        {
-            "x": 7.383292536414222,
-            "y": 1.229213843710233,
-            "heading": 2.9837724589347805,
-            "angularVelocity": -0.8644292266327552,
-            "velocityX": -1.819692855296544,
-            "velocityY": 1.7951141290432664,
-            "timestamp": 1.4415549404005294
-        },
-        {
-            "x": 7.182069766586899,
-            "y": 1.399958151778736,
-            "heading": 2.9050185961423023,
-            "angularVelocity": -0.9571146395266165,
-            "velocityX": -2.445508727811161,
-            "velocityY": 2.0750966501650514,
-            "timestamp": 1.5238375213371433
-        },
-        {
-            "x": 6.9296160896416765,
-            "y": 1.5820466019910795,
-            "heading": 2.9050185123503516,
-            "angularVelocity": -0.0000010183437364658426,
-            "velocityX": -3.068130266109342,
-            "velocityY": 2.2129647385832873,
-            "timestamp": 1.6061201022737572
-        },
-        {
-            "x": 6.6549097990315635,
-            "y": 1.728421234696809,
-            "heading": 2.9050184910214054,
-            "angularVelocity": -2.59215813112527e-7,
-            "velocityX": -3.338571633062085,
-            "velocityY": 1.7789261231181734,
-            "timestamp": 1.6884026832103711
-        },
-        {
-            "x": 6.380203216821482,
-            "y": 1.8747953201455823,
-            "heading": 2.905018469692463,
-            "angularVelocity": -2.59215765603584e-7,
-            "velocityX": -3.338575176946638,
-            "velocityY": 1.7789194721727335,
-            "timestamp": 1.770685264146985
-        },
-        {
-            "x": 6.105496634609771,
-            "y": 2.0211694055912957,
-            "heading": 2.9050184483635206,
-            "angularVelocity": -2.592157647120513e-7,
-            "velocityX": -3.3385751769664513,
-            "velocityY": 1.7789194721355484,
-            "timestamp": 1.852967845083599
-        },
-        {
-            "x": 5.83079005239806,
-            "y": 2.167543491037009,
-            "heading": 2.905018427034578,
-            "angularVelocity": -2.592157660778243e-7,
-            "velocityX": -3.3385751769664513,
-            "velocityY": 1.778919472135548,
-            "timestamp": 1.9352504260202128
-        },
-        {
-            "x": 5.556083470186349,
-            "y": 2.3139175764827224,
-            "heading": 2.905018405705636,
-            "angularVelocity": -2.592157651075657e-7,
-            "velocityX": -3.338575176966452,
-            "velocityY": 1.7789194721355481,
-            "timestamp": 2.0175330069568265
-        },
-        {
-            "x": 5.281376887974587,
-            "y": 2.460291661928342,
-            "heading": 2.9050183843766937,
-            "angularVelocity": -2.5921576594087425e-7,
-            "velocityX": -3.3385751769670593,
-            "velocityY": 1.7789194721344066,
-            "timestamp": 2.09981558789344
-        },
-        {
-            "x": 5.006670296810042,
-            "y": 2.6066657305717675,
-            "heading": 2.905018363047624,
-            "angularVelocity": -2.5921731220751964e-7,
-            "velocityX": -3.338575285772395,
-            "velocityY": 1.778919267933304,
-            "timestamp": 2.182098168830054
-        },
-        {
-            "x": 4.758260828306463,
-            "y": 2.7376390899942815,
-            "heading": 2.815277658009563,
-            "angularVelocity": -1.0906403763293109,
-            "velocityX": -3.0189800280443744,
-            "velocityY": 1.591750743980768,
-            "timestamp": 2.2643807497666675
-        },
-        {
-            "x": 4.559939575601031,
-            "y": 2.842572976973571,
-            "heading": 2.72503756976798,
-            "angularVelocity": -1.0967095005333054,
-            "velocityX": -2.4102458922406798,
-            "velocityY": 1.2752867713292104,
-            "timestamp": 2.346663330703281
-        },
-        {
-            "x": 4.4113908880653625,
-            "y": 2.9212861863102546,
-            "heading": 2.651616705628037,
-            "angularVelocity": -0.8923014239977028,
-            "velocityX": -1.8053479344565475,
-            "velocityY": 0.9566205682988939,
-            "timestamp": 2.428945911639895
-        },
-        {
-            "x": 4.31244325419048,
-            "y": 2.973762547349605,
-            "heading": 2.6003949192739833,
-            "angularVelocity": -0.6225106914611183,
-            "velocityX": -1.202534397299805,
-            "velocityY": 0.6377578394116608,
-            "timestamp": 2.5112284925765085
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -0.3207838034920002,
-            "velocityX": -0.6008957622339189,
-            "velocityY": 0.31887007373537113,
-            "timestamp": 2.593511073513122
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": -9.140912570286281e-33,
-            "velocityY": 0,
-            "timestamp": 2.675793654449736
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S4.1.traj
deleted file mode 100644
index 1dfddc31..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S4.1.traj	
+++ /dev/null
@@ -1,283 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.220564813130112,
-            "y": 1.6051202900758712,
-            "heading": 2.517168506154745,
-            "angularVelocity": 0.22812300403374605,
-            "velocityX": 0.5921458136084756,
-            "velocityY": -0.19771109358311004,
-            "timestamp": 0.07525986354364067
-        },
-        {
-            "x": 5.309738195223409,
-            "y": 1.5753445036804834,
-            "heading": 2.5497776530467964,
-            "angularVelocity": 0.433287350742303,
-            "velocityX": 1.1848730238739802,
-            "velocityY": -0.3956396543042072,
-            "timestamp": 0.15051972708728134
-        },
-        {
-            "x": 5.443577884042542,
-            "y": 1.5306492621981627,
-            "heading": 2.5951663634005544,
-            "angularVelocity": 0.6030931789749864,
-            "velocityX": 1.7783674128179001,
-            "velocityY": -0.593878853585797,
-            "timestamp": 0.225779590630922
-        },
-        {
-            "x": 5.622156845349364,
-            "y": 1.4710006417461063,
-            "heading": 2.6487285344970175,
-            "angularVelocity": 0.7116963621041407,
-            "velocityX": 2.372831319356166,
-            "velocityY": -0.7925688095018658,
-            "timestamp": 0.3010394541745627
-        },
-        {
-            "x": 5.845518423751245,
-            "y": 1.3963607866160492,
-            "heading": 2.700604108231769,
-            "angularVelocity": 0.6892860456047826,
-            "velocityX": 2.967871158474283,
-            "velocityY": -0.9917617653767852,
-            "timestamp": 0.37629931771820335
-        },
-        {
-            "x": 6.112314775127604,
-            "y": 1.3070273852513161,
-            "heading": 2.7110861192214117,
-            "angularVelocity": 0.13927757101984,
-            "velocityX": 3.5450017953016038,
-            "velocityY": -1.186999247121034,
-            "timestamp": 0.451559181261844
-        },
-        {
-            "x": 6.382409273163311,
-            "y": 1.216999105128909,
-            "heading": 2.7110861382363836,
-            "angularVelocity": 2.526575375097703e-7,
-            "velocityX": 3.588825242542295,
-            "velocityY": -1.196232306084407,
-            "timestamp": 0.5268190448054847
-        },
-        {
-            "x": 6.652503772105593,
-            "y": 1.1269708277263142,
-            "heading": 2.711086157251347,
-            "angularVelocity": 2.5265742250097044e-7,
-            "velocityX": 3.5888252545882353,
-            "velocityY": -1.19623226994546,
-            "timestamp": 0.6020789083491254
-        },
-        {
-            "x": 6.9225982993416295,
-            "y": 1.0369426352093585,
-            "heading": 2.7110861762673086,
-            "angularVelocity": 2.5267068695978854e-7,
-            "velocityX": 3.5888256305356965,
-            "velocityY": -1.19623114204494,
-            "timestamp": 0.677338771892766
-        },
-        {
-            "x": 7.1604160781282875,
-            "y": 0.9586172665469176,
-            "heading": 2.823441998219159,
-            "angularVelocity": 1.4929049384563269,
-            "velocityX": 3.1599549559208913,
-            "velocityY": -1.040732270488674,
-            "timestamp": 0.7525986354364067
-        },
-        {
-            "x": 7.353386220004786,
-            "y": 0.8948816690648573,
-            "heading": 2.9268528477366496,
-            "angularVelocity": 1.3740504519719978,
-            "velocityX": 2.5640511793461145,
-            "velocityY": -0.8468736784926819,
-            "timestamp": 0.8278584989800474
-        },
-        {
-            "x": 7.501750789212216,
-            "y": 0.8458382793290063,
-            "heading": 3.012370231908866,
-            "angularVelocity": 1.136294701393235,
-            "velocityX": 1.9713637817240808,
-            "velocityY": -0.6516539816393957,
-            "timestamp": 0.903118362523688
-        },
-        {
-            "x": 7.605609860307225,
-            "y": 0.8115063107648604,
-            "heading": 3.0771995592704795,
-            "angularVelocity": 0.8614063899281612,
-            "velocityX": 1.3800061042468479,
-            "velocityY": -0.45617899033577186,
-            "timestamp": 0.9783782260673287
-        },
-        {
-            "x": 7.665015516622623,
-            "y": 0.7918924721730815,
-            "heading": 3.1200072835634023,
-            "angularVelocity": 0.5687988560874772,
-            "velocityX": 0.7893404733713316,
-            "velocityY": -0.2606148572194197,
-            "timestamp": 1.0536380896109694
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 0.26564911886937803,
-            "velocityX": 0.19910324935265278,
-            "velocityY": -0.06500772048629082,
-            "timestamp": 1.1288979531546102
-        },
-        {
-            "x": 7.647079450231064,
-            "y": 0.7979971975497204,
-            "heading": 3.1353264592199808,
-            "angularVelocity": -0.059354936296623806,
-            "velocityX": -0.418097803433937,
-            "velocityY": 0.13966668757779366,
-            "timestamp": 1.2076368259698977
-        },
-        {
-            "x": 7.565488841826375,
-            "y": 0.8251307377838253,
-            "heading": 3.107083972632397,
-            "angularVelocity": -0.3586854317033129,
-            "velocityX": -1.0362176328849728,
-            "velocityY": 0.3446015832326818,
-            "timestamp": 1.2863756987851853
-        },
-        {
-            "x": 7.435127415733675,
-            "y": 0.8684254377651182,
-            "heading": 3.05838611435167,
-            "angularVelocity": -0.6184728907024979,
-            "velocityX": -1.6556171231776886,
-            "velocityY": 0.5498516607274432,
-            "timestamp": 1.365114571600473
-        },
-        {
-            "x": 7.25585035840985,
-            "y": 0.927906970601061,
-            "heading": 2.99467735542164,
-            "angularVelocity": -0.809114439312372,
-            "velocityX": -2.2768557754742957,
-            "velocityY": 0.75542779200662,
-            "timestamp": 1.4438534444157605
-        },
-        {
-            "x": 7.027485658601358,
-            "y": 1.0035719757661283,
-            "heading": 2.9276702363354157,
-            "angularVelocity": -0.8510042967393713,
-            "velocityX": -2.900279006332858,
-            "velocityY": 0.9609612439152991,
-            "timestamp": 1.522592317231048
-        },
-        {
-            "x": 6.751324119312769,
-            "y": 1.0947095363284396,
-            "heading": 2.9052334045847372,
-            "angularVelocity": -0.28495241230227925,
-            "velocityX": -3.507308771569951,
-            "velocityY": 1.1574659034821269,
-            "timestamp": 1.6013311900463356
-        },
-        {
-            "x": 6.468725179881112,
-            "y": 1.1888428468535348,
-            "heading": 2.9052333806250203,
-            "angularVelocity": -3.0429336907534885e-7,
-            "velocityX": -3.5890650872613086,
-            "velocityY": 1.195512548749856,
-            "timestamp": 1.6800700628616232
-        },
-        {
-            "x": 6.186126242787232,
-            "y": 1.2829761643969295,
-            "heading": 2.9052333566653217,
-            "angularVelocity": -3.0429313853702755e-7,
-            "velocityX": -3.5890650575710485,
-            "velocityY": 1.1955126378837118,
-            "timestamp": 1.7588089356769108
-        },
-        {
-            "x": 5.9036205043534045,
-            "y": 1.377388777995483,
-            "heading": 2.905233301444114,
-            "angularVelocity": -7.013207784930713e-7,
-            "velocityX": -3.5878814152769594,
-            "velocityY": 1.1990597556563287,
-            "timestamp": 1.8375478084921983
-        },
-        {
-            "x": 5.660687795689049,
-            "y": 1.4583340101651068,
-            "heading": 2.781705854564737,
-            "angularVelocity": -1.5688241711201327,
-            "velocityX": -3.0852957373957746,
-            "velocityY": 1.0280212209731776,
-            "timestamp": 1.916286681307486
-        },
-        {
-            "x": 5.4666551877353555,
-            "y": 1.5230274173690972,
-            "heading": 2.6729354985546605,
-            "angularVelocity": -1.3814060593074367,
-            "velocityX": -2.464254325927075,
-            "velocityY": 0.8216196764176925,
-            "timestamp": 1.9950255541227735
-        },
-        {
-            "x": 5.321269342371624,
-            "y": 1.571523398115741,
-            "heading": 2.5878352744293056,
-            "angularVelocity": -1.080790479754395,
-            "velocityX": -1.8464303610846597,
-            "velocityY": 0.6159090041891974,
-            "timestamp": 2.073764426938061
-        },
-        {
-            "x": 5.2244075328588835,
-            "y": 1.6038438553412442,
-            "heading": 2.5296373366742775,
-            "angularVelocity": -0.7391258685091143,
-            "velocityX": -1.2301650512570566,
-            "velocityY": 0.4104765037889631,
-            "timestamp": 2.1525032997533486
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.3764003168270292,
-            "velocityX": -0.6147856976876184,
-            "velocityY": 0.20518638483251173,
-            "timestamp": 2.231242172568636
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 3.3983560967726525e-33,
-            "velocityX": 0,
-            "velocityY": -1.4165312907158555e-34,
-            "timestamp": 2.309981045383924
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S4.traj
deleted file mode 100644
index 1dfddc31..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5 To S4.traj	
+++ /dev/null
@@ -1,283 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.220564813130112,
-            "y": 1.6051202900758712,
-            "heading": 2.517168506154745,
-            "angularVelocity": 0.22812300403374605,
-            "velocityX": 0.5921458136084756,
-            "velocityY": -0.19771109358311004,
-            "timestamp": 0.07525986354364067
-        },
-        {
-            "x": 5.309738195223409,
-            "y": 1.5753445036804834,
-            "heading": 2.5497776530467964,
-            "angularVelocity": 0.433287350742303,
-            "velocityX": 1.1848730238739802,
-            "velocityY": -0.3956396543042072,
-            "timestamp": 0.15051972708728134
-        },
-        {
-            "x": 5.443577884042542,
-            "y": 1.5306492621981627,
-            "heading": 2.5951663634005544,
-            "angularVelocity": 0.6030931789749864,
-            "velocityX": 1.7783674128179001,
-            "velocityY": -0.593878853585797,
-            "timestamp": 0.225779590630922
-        },
-        {
-            "x": 5.622156845349364,
-            "y": 1.4710006417461063,
-            "heading": 2.6487285344970175,
-            "angularVelocity": 0.7116963621041407,
-            "velocityX": 2.372831319356166,
-            "velocityY": -0.7925688095018658,
-            "timestamp": 0.3010394541745627
-        },
-        {
-            "x": 5.845518423751245,
-            "y": 1.3963607866160492,
-            "heading": 2.700604108231769,
-            "angularVelocity": 0.6892860456047826,
-            "velocityX": 2.967871158474283,
-            "velocityY": -0.9917617653767852,
-            "timestamp": 0.37629931771820335
-        },
-        {
-            "x": 6.112314775127604,
-            "y": 1.3070273852513161,
-            "heading": 2.7110861192214117,
-            "angularVelocity": 0.13927757101984,
-            "velocityX": 3.5450017953016038,
-            "velocityY": -1.186999247121034,
-            "timestamp": 0.451559181261844
-        },
-        {
-            "x": 6.382409273163311,
-            "y": 1.216999105128909,
-            "heading": 2.7110861382363836,
-            "angularVelocity": 2.526575375097703e-7,
-            "velocityX": 3.588825242542295,
-            "velocityY": -1.196232306084407,
-            "timestamp": 0.5268190448054847
-        },
-        {
-            "x": 6.652503772105593,
-            "y": 1.1269708277263142,
-            "heading": 2.711086157251347,
-            "angularVelocity": 2.5265742250097044e-7,
-            "velocityX": 3.5888252545882353,
-            "velocityY": -1.19623226994546,
-            "timestamp": 0.6020789083491254
-        },
-        {
-            "x": 6.9225982993416295,
-            "y": 1.0369426352093585,
-            "heading": 2.7110861762673086,
-            "angularVelocity": 2.5267068695978854e-7,
-            "velocityX": 3.5888256305356965,
-            "velocityY": -1.19623114204494,
-            "timestamp": 0.677338771892766
-        },
-        {
-            "x": 7.1604160781282875,
-            "y": 0.9586172665469176,
-            "heading": 2.823441998219159,
-            "angularVelocity": 1.4929049384563269,
-            "velocityX": 3.1599549559208913,
-            "velocityY": -1.040732270488674,
-            "timestamp": 0.7525986354364067
-        },
-        {
-            "x": 7.353386220004786,
-            "y": 0.8948816690648573,
-            "heading": 2.9268528477366496,
-            "angularVelocity": 1.3740504519719978,
-            "velocityX": 2.5640511793461145,
-            "velocityY": -0.8468736784926819,
-            "timestamp": 0.8278584989800474
-        },
-        {
-            "x": 7.501750789212216,
-            "y": 0.8458382793290063,
-            "heading": 3.012370231908866,
-            "angularVelocity": 1.136294701393235,
-            "velocityX": 1.9713637817240808,
-            "velocityY": -0.6516539816393957,
-            "timestamp": 0.903118362523688
-        },
-        {
-            "x": 7.605609860307225,
-            "y": 0.8115063107648604,
-            "heading": 3.0771995592704795,
-            "angularVelocity": 0.8614063899281612,
-            "velocityX": 1.3800061042468479,
-            "velocityY": -0.45617899033577186,
-            "timestamp": 0.9783782260673287
-        },
-        {
-            "x": 7.665015516622623,
-            "y": 0.7918924721730815,
-            "heading": 3.1200072835634023,
-            "angularVelocity": 0.5687988560874772,
-            "velocityX": 0.7893404733713316,
-            "velocityY": -0.2606148572194197,
-            "timestamp": 1.0536380896109694
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 0.26564911886937803,
-            "velocityX": 0.19910324935265278,
-            "velocityY": -0.06500772048629082,
-            "timestamp": 1.1288979531546102
-        },
-        {
-            "x": 7.647079450231064,
-            "y": 0.7979971975497204,
-            "heading": 3.1353264592199808,
-            "angularVelocity": -0.059354936296623806,
-            "velocityX": -0.418097803433937,
-            "velocityY": 0.13966668757779366,
-            "timestamp": 1.2076368259698977
-        },
-        {
-            "x": 7.565488841826375,
-            "y": 0.8251307377838253,
-            "heading": 3.107083972632397,
-            "angularVelocity": -0.3586854317033129,
-            "velocityX": -1.0362176328849728,
-            "velocityY": 0.3446015832326818,
-            "timestamp": 1.2863756987851853
-        },
-        {
-            "x": 7.435127415733675,
-            "y": 0.8684254377651182,
-            "heading": 3.05838611435167,
-            "angularVelocity": -0.6184728907024979,
-            "velocityX": -1.6556171231776886,
-            "velocityY": 0.5498516607274432,
-            "timestamp": 1.365114571600473
-        },
-        {
-            "x": 7.25585035840985,
-            "y": 0.927906970601061,
-            "heading": 2.99467735542164,
-            "angularVelocity": -0.809114439312372,
-            "velocityX": -2.2768557754742957,
-            "velocityY": 0.75542779200662,
-            "timestamp": 1.4438534444157605
-        },
-        {
-            "x": 7.027485658601358,
-            "y": 1.0035719757661283,
-            "heading": 2.9276702363354157,
-            "angularVelocity": -0.8510042967393713,
-            "velocityX": -2.900279006332858,
-            "velocityY": 0.9609612439152991,
-            "timestamp": 1.522592317231048
-        },
-        {
-            "x": 6.751324119312769,
-            "y": 1.0947095363284396,
-            "heading": 2.9052334045847372,
-            "angularVelocity": -0.28495241230227925,
-            "velocityX": -3.507308771569951,
-            "velocityY": 1.1574659034821269,
-            "timestamp": 1.6013311900463356
-        },
-        {
-            "x": 6.468725179881112,
-            "y": 1.1888428468535348,
-            "heading": 2.9052333806250203,
-            "angularVelocity": -3.0429336907534885e-7,
-            "velocityX": -3.5890650872613086,
-            "velocityY": 1.195512548749856,
-            "timestamp": 1.6800700628616232
-        },
-        {
-            "x": 6.186126242787232,
-            "y": 1.2829761643969295,
-            "heading": 2.9052333566653217,
-            "angularVelocity": -3.0429313853702755e-7,
-            "velocityX": -3.5890650575710485,
-            "velocityY": 1.1955126378837118,
-            "timestamp": 1.7588089356769108
-        },
-        {
-            "x": 5.9036205043534045,
-            "y": 1.377388777995483,
-            "heading": 2.905233301444114,
-            "angularVelocity": -7.013207784930713e-7,
-            "velocityX": -3.5878814152769594,
-            "velocityY": 1.1990597556563287,
-            "timestamp": 1.8375478084921983
-        },
-        {
-            "x": 5.660687795689049,
-            "y": 1.4583340101651068,
-            "heading": 2.781705854564737,
-            "angularVelocity": -1.5688241711201327,
-            "velocityX": -3.0852957373957746,
-            "velocityY": 1.0280212209731776,
-            "timestamp": 1.916286681307486
-        },
-        {
-            "x": 5.4666551877353555,
-            "y": 1.5230274173690972,
-            "heading": 2.6729354985546605,
-            "angularVelocity": -1.3814060593074367,
-            "velocityX": -2.464254325927075,
-            "velocityY": 0.8216196764176925,
-            "timestamp": 1.9950255541227735
-        },
-        {
-            "x": 5.321269342371624,
-            "y": 1.571523398115741,
-            "heading": 2.5878352744293056,
-            "angularVelocity": -1.080790479754395,
-            "velocityX": -1.8464303610846597,
-            "velocityY": 0.6159090041891974,
-            "timestamp": 2.073764426938061
-        },
-        {
-            "x": 5.2244075328588835,
-            "y": 1.6038438553412442,
-            "heading": 2.5296373366742775,
-            "angularVelocity": -0.7391258685091143,
-            "velocityX": -1.2301650512570566,
-            "velocityY": 0.4104765037889631,
-            "timestamp": 2.1525032997533486
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.3764003168270292,
-            "velocityX": -0.6147856976876184,
-            "velocityY": 0.20518638483251173,
-            "timestamp": 2.231242172568636
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 3.3983560967726525e-33,
-            "velocityX": 0,
-            "velocityY": -1.4165312907158555e-34,
-            "timestamp": 2.309981045383924
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.1.traj
deleted file mode 100644
index 8867d64b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.1.traj	
+++ /dev/null
@@ -1,166 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": -2.991539680273006e-28,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.128677687485332,
-            "y": 1.6650804220486217,
-            "heading": 2.5120089352888275,
-            "angularVelocity": 0.13555115758601896,
-            "velocityX": -0.5341517867092611,
-            "velocityY": 0.5088463919723994,
-            "timestamp": 0.0885933805561246
-        },
-        {
-            "x": 5.034016646343104,
-            "y": 1.7552508281834698,
-            "heading": 2.534662979379905,
-            "angularVelocity": 0.2557080895761949,
-            "velocityX": -1.0684888707035958,
-            "velocityY": 1.017800715683549,
-            "timestamp": 0.1771867611122492
-        },
-        {
-            "x": 4.891992835259765,
-            "y": 1.890523822602506,
-            "heading": 2.5656675717312063,
-            "angularVelocity": 0.3499651120288192,
-            "velocityX": -1.6030973216262263,
-            "velocityY": 1.5268973095946068,
-            "timestamp": 0.2657801416683738
-        },
-        {
-            "x": 4.702571182201446,
-            "y": 2.070911500413953,
-            "heading": 2.6003407126574074,
-            "angularVelocity": 0.3913739458702845,
-            "velocityX": -2.1381016490087936,
-            "velocityY": 2.036130427342408,
-            "timestamp": 0.3543735222244984
-        },
-        {
-            "x": 4.465767136473608,
-            "y": 2.2963300542080516,
-            "heading": 2.6236809711316846,
-            "angularVelocity": 0.2634537515983315,
-            "velocityX": -2.672931591969363,
-            "velocityY": 2.5444175668553926,
-            "timestamp": 0.442966902780623
-        },
-        {
-            "x": 4.223169514205327,
-            "y": 2.5275605696083283,
-            "heading": 2.6236809810890134,
-            "angularVelocity": 1.1239359640247893e-7,
-            "velocityX": -2.7383267321489955,
-            "velocityY": 2.610020228924234,
-            "timestamp": 0.5315602833367477
-        },
-        {
-            "x": 3.980571892636886,
-            "y": 2.7587910857428417,
-            "heading": 2.623680991046373,
-            "angularVelocity": 1.1239395834581428e-7,
-            "velocityX": -2.7383267242494984,
-            "velocityY": 2.6100202372119665,
-            "timestamp": 0.6201536638928723
-        },
-        {
-            "x": 3.737974271068317,
-            "y": 2.9900216018772205,
-            "heading": 2.623681001003734,
-            "angularVelocity": 1.1239395847018201e-7,
-            "velocityX": -2.7383267242509515,
-            "velocityY": 2.610020237210452,
-            "timestamp": 0.7087470444489969
-        },
-        {
-            "x": 3.4953766494997485,
-            "y": 3.2212521180116,
-            "heading": 2.623681010961094,
-            "angularVelocity": 1.1239395711385692e-7,
-            "velocityX": -2.73832672425095,
-            "velocityY": 2.610020237210453,
-            "timestamp": 0.7973404250051216
-        },
-        {
-            "x": 3.252779027931301,
-            "y": 3.4524826341461052,
-            "heading": 2.6236810209184545,
-            "angularVelocity": 1.1239395822212682e-7,
-            "velocityX": -2.7383267242495783,
-            "velocityY": 2.6100202372118835,
-            "timestamp": 0.8859338055612462
-        },
-        {
-            "x": 3.0101814056711773,
-            "y": 3.68371314955494,
-            "heading": 2.6236810308757845,
-            "angularVelocity": 1.1239361401031205e-7,
-            "velocityX": -2.7383267320569105,
-            "velocityY": 2.6100202290208365,
-            "timestamp": 0.9745271861173709
-        },
-        {
-            "x": 2.773398473292677,
-            "y": 3.9091148718156945,
-            "heading": 2.6467919550399244,
-            "angularVelocity": 0.2608651348337584,
-            "velocityX": -2.6726932745104093,
-            "velocityY": 2.5442275805015666,
-            "timestamp": 1.0631205666734955
-        },
-        {
-            "x": 2.58398624574228,
-            "y": 4.089495303792956,
-            "heading": 2.682294638537745,
-            "angularVelocity": 0.40073742840518267,
-            "velocityX": -2.1379952583522854,
-            "velocityY": 2.036048639807706,
-            "timestamp": 1.15171394722962
-        },
-        {
-            "x": 2.441971645222967,
-            "y": 4.224760268869052,
-            "heading": 2.7142172786134493,
-            "angularVelocity": 0.36032759869046743,
-            "velocityX": -1.6029933571543074,
-            "velocityY": 1.5268066781852525,
-            "timestamp": 1.2403073277857446
-        },
-        {
-            "x": 2.3473180655703776,
-            "y": 4.314923716116089,
-            "heading": 2.737593550901004,
-            "angularVelocity": 0.263860258416364,
-            "velocityX": -1.0684046489525902,
-            "velocityY": 1.0177221670632457,
-            "timestamp": 1.3289007083418691
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0.1400381046653499,
-            "velocityX": -0.5341038492193103,
-            "velocityY": 0.5087996823346718,
-            "timestamp": 1.4174940888979937
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0,
-            "velocityX": -3.3897612225154066e-29,
-            "velocityY": 0,
-            "timestamp": 1.5060874694541182
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.traj
deleted file mode 100644
index 8867d64b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.traj	
+++ /dev/null
@@ -1,166 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": -2.991539680273006e-28,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.128677687485332,
-            "y": 1.6650804220486217,
-            "heading": 2.5120089352888275,
-            "angularVelocity": 0.13555115758601896,
-            "velocityX": -0.5341517867092611,
-            "velocityY": 0.5088463919723994,
-            "timestamp": 0.0885933805561246
-        },
-        {
-            "x": 5.034016646343104,
-            "y": 1.7552508281834698,
-            "heading": 2.534662979379905,
-            "angularVelocity": 0.2557080895761949,
-            "velocityX": -1.0684888707035958,
-            "velocityY": 1.017800715683549,
-            "timestamp": 0.1771867611122492
-        },
-        {
-            "x": 4.891992835259765,
-            "y": 1.890523822602506,
-            "heading": 2.5656675717312063,
-            "angularVelocity": 0.3499651120288192,
-            "velocityX": -1.6030973216262263,
-            "velocityY": 1.5268973095946068,
-            "timestamp": 0.2657801416683738
-        },
-        {
-            "x": 4.702571182201446,
-            "y": 2.070911500413953,
-            "heading": 2.6003407126574074,
-            "angularVelocity": 0.3913739458702845,
-            "velocityX": -2.1381016490087936,
-            "velocityY": 2.036130427342408,
-            "timestamp": 0.3543735222244984
-        },
-        {
-            "x": 4.465767136473608,
-            "y": 2.2963300542080516,
-            "heading": 2.6236809711316846,
-            "angularVelocity": 0.2634537515983315,
-            "velocityX": -2.672931591969363,
-            "velocityY": 2.5444175668553926,
-            "timestamp": 0.442966902780623
-        },
-        {
-            "x": 4.223169514205327,
-            "y": 2.5275605696083283,
-            "heading": 2.6236809810890134,
-            "angularVelocity": 1.1239359640247893e-7,
-            "velocityX": -2.7383267321489955,
-            "velocityY": 2.610020228924234,
-            "timestamp": 0.5315602833367477
-        },
-        {
-            "x": 3.980571892636886,
-            "y": 2.7587910857428417,
-            "heading": 2.623680991046373,
-            "angularVelocity": 1.1239395834581428e-7,
-            "velocityX": -2.7383267242494984,
-            "velocityY": 2.6100202372119665,
-            "timestamp": 0.6201536638928723
-        },
-        {
-            "x": 3.737974271068317,
-            "y": 2.9900216018772205,
-            "heading": 2.623681001003734,
-            "angularVelocity": 1.1239395847018201e-7,
-            "velocityX": -2.7383267242509515,
-            "velocityY": 2.610020237210452,
-            "timestamp": 0.7087470444489969
-        },
-        {
-            "x": 3.4953766494997485,
-            "y": 3.2212521180116,
-            "heading": 2.623681010961094,
-            "angularVelocity": 1.1239395711385692e-7,
-            "velocityX": -2.73832672425095,
-            "velocityY": 2.610020237210453,
-            "timestamp": 0.7973404250051216
-        },
-        {
-            "x": 3.252779027931301,
-            "y": 3.4524826341461052,
-            "heading": 2.6236810209184545,
-            "angularVelocity": 1.1239395822212682e-7,
-            "velocityX": -2.7383267242495783,
-            "velocityY": 2.6100202372118835,
-            "timestamp": 0.8859338055612462
-        },
-        {
-            "x": 3.0101814056711773,
-            "y": 3.68371314955494,
-            "heading": 2.6236810308757845,
-            "angularVelocity": 1.1239361401031205e-7,
-            "velocityX": -2.7383267320569105,
-            "velocityY": 2.6100202290208365,
-            "timestamp": 0.9745271861173709
-        },
-        {
-            "x": 2.773398473292677,
-            "y": 3.9091148718156945,
-            "heading": 2.6467919550399244,
-            "angularVelocity": 0.2608651348337584,
-            "velocityX": -2.6726932745104093,
-            "velocityY": 2.5442275805015666,
-            "timestamp": 1.0631205666734955
-        },
-        {
-            "x": 2.58398624574228,
-            "y": 4.089495303792956,
-            "heading": 2.682294638537745,
-            "angularVelocity": 0.40073742840518267,
-            "velocityX": -2.1379952583522854,
-            "velocityY": 2.036048639807706,
-            "timestamp": 1.15171394722962
-        },
-        {
-            "x": 2.441971645222967,
-            "y": 4.224760268869052,
-            "heading": 2.7142172786134493,
-            "angularVelocity": 0.36032759869046743,
-            "velocityX": -1.6029933571543074,
-            "velocityY": 1.5268066781852525,
-            "timestamp": 1.2403073277857446
-        },
-        {
-            "x": 2.3473180655703776,
-            "y": 4.314923716116089,
-            "heading": 2.737593550901004,
-            "angularVelocity": 0.263860258416364,
-            "velocityX": -1.0684046489525902,
-            "velocityY": 1.0177221670632457,
-            "timestamp": 1.3289007083418691
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0.1400381046653499,
-            "velocityX": -0.5341038492193103,
-            "velocityY": 0.5087996823346718,
-            "timestamp": 1.4174940888979937
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0,
-            "velocityX": -3.3897612225154066e-29,
-            "velocityY": 0,
-            "timestamp": 1.5060874694541182
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S1.1.traj
deleted file mode 100644
index f17937c5..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S1.1.traj	
+++ /dev/null
@@ -1,319 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.363517540312363,
-            "y": 6.769205991625322,
-            "heading": -2.7639736107792703,
-            "angularVelocity": -0.15851225932569585,
-            "velocityX": 0.7205230617028869,
-            "velocityY": 0.13846094187126756,
-            "timestamp": 0.08815476379374498
-        },
-        {
-            "x": 2.4905727294218147,
-            "y": 6.793631120419202,
-            "heading": -2.790535805480005,
-            "angularVelocity": -0.30131320824454183,
-            "velocityX": 1.4412742277515702,
-            "velocityY": 0.2770710026632852,
-            "timestamp": 0.17630952758748997
-        },
-        {
-            "x": 2.6811943332190395,
-            "y": 6.830296876066563,
-            "heading": -2.8273459830802556,
-            "angularVelocity": -0.4175631130539097,
-            "velocityX": 2.162351705044798,
-            "velocityY": 0.4159248357030905,
-            "timestamp": 0.26446429138123495
-        },
-        {
-            "x": 2.935420930881945,
-            "y": 6.8792439916128805,
-            "heading": -2.869603740492264,
-            "angularVelocity": -0.47935875037764497,
-            "velocityX": 2.8838668124358944,
-            "velocityY": 0.5552407316391297,
-            "timestamp": 0.35261905517497993
-        },
-        {
-            "x": 3.253189180048851,
-            "y": 6.9405688545954956,
-            "heading": -2.9017584747192204,
-            "angularVelocity": -0.364753223117765,
-            "velocityX": 3.604663383936769,
-            "velocityY": 0.695650017576943,
-            "timestamp": 0.4407738189687249
-        },
-        {
-            "x": 3.5807176480413454,
-            "y": 7.003313040154224,
-            "heading": -2.901758484999235,
-            "angularVelocity": -1.1661326594690383e-7,
-            "velocityX": 3.7153802460274443,
-            "velocityY": 0.7117503678590205,
-            "timestamp": 0.5289285827624699
-        },
-        {
-            "x": 3.9082461164396207,
-            "y": 7.066057223594786,
-            "heading": -2.901758495279238,
-            "angularVelocity": -1.1661313280225847e-7,
-            "velocityX": 3.7153802506304894,
-            "velocityY": 0.7117503438312037,
-            "timestamp": 0.6170833465562149
-        },
-        {
-            "x": 4.235774584837897,
-            "y": 7.128801407035338,
-            "heading": -2.901758505559241,
-            "angularVelocity": -1.1661313256198823e-7,
-            "velocityX": 3.7153802506305107,
-            "velocityY": 0.711750343831093,
-            "timestamp": 0.7052381103499599
-        },
-        {
-            "x": 4.563303053236174,
-            "y": 7.19154559047589,
-            "heading": -2.9017585158392443,
-            "angularVelocity": -1.166131334044927e-7,
-            "velocityX": 3.7153802506305107,
-            "velocityY": 0.7117503438310929,
-            "timestamp": 0.7933928741437049
-        },
-        {
-            "x": 4.890831521634451,
-            "y": 7.254289773916442,
-            "heading": -2.9017585261192473,
-            "angularVelocity": -1.1661313263433965e-7,
-            "velocityX": 3.7153802506305107,
-            "velocityY": 0.711750343831093,
-            "timestamp": 0.8815476379374498
-        },
-        {
-            "x": 5.218359990032727,
-            "y": 7.317033957356994,
-            "heading": -2.9017585363992504,
-            "angularVelocity": -1.1661313267352917e-7,
-            "velocityX": 3.7153802506305107,
-            "velocityY": 0.7117503438310931,
-            "timestamp": 0.9697024017311948
-        },
-        {
-            "x": 5.545888458431004,
-            "y": 7.379778140797546,
-            "heading": -2.9017585466792535,
-            "angularVelocity": -1.1661313271580576e-7,
-            "velocityX": 3.7153802506305107,
-            "velocityY": 0.7117503438310933,
-            "timestamp": 1.0578571655249398
-        },
-        {
-            "x": 5.873416926829281,
-            "y": 7.442522324238098,
-            "heading": -2.901758556959257,
-            "angularVelocity": -1.166131330429824e-7,
-            "velocityX": 3.7153802506305103,
-            "velocityY": 0.7117503438310929,
-            "timestamp": 1.1460119293186848
-        },
-        {
-            "x": 6.200945395227635,
-            "y": 7.505266507678241,
-            "heading": -2.90175856723926,
-            "angularVelocity": -1.1661313350589332e-7,
-            "velocityX": 3.7153802506314,
-            "velocityY": 0.7117503438264522,
-            "timestamp": 1.2341666931124298
-        },
-        {
-            "x": 6.528473880635578,
-            "y": 7.568010602327234,
-            "heading": -2.9017585775192636,
-            "angularVelocity": -1.1661313512046063e-7,
-            "velocityX": 3.715380443582822,
-            "velocityY": 0.7117493366074876,
-            "timestamp": 1.3223214569061748
-        },
-        {
-            "x": 6.858875530524355,
-            "y": 7.613248618194527,
-            "heading": -2.901758593513828,
-            "angularVelocity": -1.8143731961725824e-7,
-            "velocityX": 3.747972720587331,
-            "velocityY": 0.51316586784965,
-            "timestamp": 1.4104762206999197
-        },
-        {
-            "x": 7.1493370822627105,
-            "y": 7.612204494163831,
-            "heading": -2.9774836288017847,
-            "angularVelocity": -0.859001057108326,
-            "velocityX": 3.294904770182843,
-            "velocityY": -0.011844215624353746,
-            "timestamp": 1.4986309844936647
-        },
-        {
-            "x": 7.3768731997980925,
-            "y": 7.596010582302943,
-            "heading": -3.0421265478344597,
-            "angularVelocity": -0.7332890050493631,
-            "velocityX": 2.581098374533033,
-            "velocityY": -0.18369865863138202,
-            "timestamp": 1.5867857482874097
-        },
-        {
-            "x": 7.541210779550857,
-            "y": 7.566116365480278,
-            "heading": -3.09203212161725,
-            "angularVelocity": -0.566113181354048,
-            "velocityX": 1.8641939775059564,
-            "velocityY": -0.33911062245705376,
-            "timestamp": 1.6749405120811547
-        },
-        {
-            "x": 7.642266411565545,
-            "y": 7.52302718020075,
-            "heading": -3.125932818808563,
-            "angularVelocity": -0.3845588795476813,
-            "velocityX": 1.146343404096945,
-            "velocityY": -0.4887902074166845,
-            "timestamp": 1.7630952758748997
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.19570681865122413,
-            "velocityX": 0.4280379960263239,
-            "velocityY": -0.6355547651607396,
-            "timestamp": 1.8512500396686447
-        },
-        {
-            "x": 7.65149635610838,
-            "y": 7.395583820775626,
-            "heading": -3.1428442524727873,
-            "angularVelocity": 0.003749626131397279,
-            "velocityX": -0.3133749684294368,
-            "velocityY": -0.7851642756589506,
-            "timestamp": 1.9422070297386083
-        },
-        {
-            "x": 7.55553660648891,
-            "y": 7.310596065504526,
-            "heading": -3.1249452939328513,
-            "angularVelocity": 0.19678485981305774,
-            "velocityX": -1.0550013753274867,
-            "velocityY": -0.9343729954757892,
-            "timestamp": 2.033164019808572
-        },
-        {
-            "x": 7.392089155098869,
-            "y": 7.212098072056303,
-            "heading": -3.09046475805122,
-            "angularVelocity": 0.37908615770051013,
-            "velocityX": -1.796975155667623,
-            "velocityY": -1.082907354041688,
-            "timestamp": 2.1241210098785355
-        },
-        {
-            "x": 7.1610935040737775,
-            "y": 7.100214825167467,
-            "heading": -3.041377794518818,
-            "angularVelocity": 0.5396722505288368,
-            "velocityX": -2.539614062068222,
-            "velocityY": -1.2300676045109389,
-            "timestamp": 2.215077999948499
-        },
-        {
-            "x": 6.862392163588639,
-            "y": 6.975338305116689,
-            "heading": -2.9837366917531454,
-            "angularVelocity": 0.6337182301358967,
-            "velocityX": -3.2839844442451716,
-            "velocityY": -1.3729183425572538,
-            "timestamp": 2.3060349900184627
-        },
-        {
-            "x": 6.525454088685491,
-            "y": 6.905573407470138,
-            "heading": -2.9837366810920996,
-            "angularVelocity": 1.1720974856117963e-7,
-            "velocityX": -3.704367027141189,
-            "velocityY": -0.7670097437560687,
-            "timestamp": 2.3969919800884263
-        },
-        {
-            "x": 6.1883268460675325,
-            "y": 6.836728439791378,
-            "heading": -2.983736670548346,
-            "angularVelocity": 1.1592021231646331e-7,
-            "velocityX": -3.7064467762031894,
-            "velocityY": -0.7568958430329531,
-            "timestamp": 2.48794897015839
-        },
-        {
-            "x": 5.851199548758337,
-            "y": 6.767884093073121,
-            "heading": -2.9837365285519053,
-            "angularVelocity": 0.0000015611382957440025,
-            "velocityX": -3.706447377489981,
-            "velocityY": -0.7568890160645022,
-            "timestamp": 2.5789059602283535
-        },
-        {
-            "x": 5.5810538404288925,
-            "y": 6.712717106073741,
-            "heading": -2.9505527658877098,
-            "angularVelocity": 0.3648291641873475,
-            "velocityX": -2.970037906065856,
-            "velocityY": -0.6065172886320637,
-            "timestamp": 2.669862950298317
-        },
-        {
-            "x": 5.3784959131966685,
-            "y": 6.671352294274059,
-            "heading": -2.9178931983006144,
-            "angularVelocity": 0.3590660548682879,
-            "velocityX": -2.226963832866719,
-            "velocityY": -0.45477331393503795,
-            "timestamp": 2.7608199403682807
-        },
-        {
-            "x": 5.2434899940019335,
-            "y": 6.6437823583535645,
-            "heading": -2.8932620140402783,
-            "angularVelocity": 0.2708003446618372,
-            "velocityX": -1.4842830561003668,
-            "velocityY": -0.30310958948050626,
-            "timestamp": 2.8517769304382443
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0.14580533096009823,
-            "velocityX": -0.7419989816068217,
-            "velocityY": -0.15152610418353574,
-            "timestamp": 2.942733920508208
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0,
-            "velocityX": 1.1797533379852482e-32,
-            "velocityY": 0,
-            "timestamp": 3.0336909105781715
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S1.traj
deleted file mode 100644
index f17937c5..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S1.traj	
+++ /dev/null
@@ -1,319 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.363517540312363,
-            "y": 6.769205991625322,
-            "heading": -2.7639736107792703,
-            "angularVelocity": -0.15851225932569585,
-            "velocityX": 0.7205230617028869,
-            "velocityY": 0.13846094187126756,
-            "timestamp": 0.08815476379374498
-        },
-        {
-            "x": 2.4905727294218147,
-            "y": 6.793631120419202,
-            "heading": -2.790535805480005,
-            "angularVelocity": -0.30131320824454183,
-            "velocityX": 1.4412742277515702,
-            "velocityY": 0.2770710026632852,
-            "timestamp": 0.17630952758748997
-        },
-        {
-            "x": 2.6811943332190395,
-            "y": 6.830296876066563,
-            "heading": -2.8273459830802556,
-            "angularVelocity": -0.4175631130539097,
-            "velocityX": 2.162351705044798,
-            "velocityY": 0.4159248357030905,
-            "timestamp": 0.26446429138123495
-        },
-        {
-            "x": 2.935420930881945,
-            "y": 6.8792439916128805,
-            "heading": -2.869603740492264,
-            "angularVelocity": -0.47935875037764497,
-            "velocityX": 2.8838668124358944,
-            "velocityY": 0.5552407316391297,
-            "timestamp": 0.35261905517497993
-        },
-        {
-            "x": 3.253189180048851,
-            "y": 6.9405688545954956,
-            "heading": -2.9017584747192204,
-            "angularVelocity": -0.364753223117765,
-            "velocityX": 3.604663383936769,
-            "velocityY": 0.695650017576943,
-            "timestamp": 0.4407738189687249
-        },
-        {
-            "x": 3.5807176480413454,
-            "y": 7.003313040154224,
-            "heading": -2.901758484999235,
-            "angularVelocity": -1.1661326594690383e-7,
-            "velocityX": 3.7153802460274443,
-            "velocityY": 0.7117503678590205,
-            "timestamp": 0.5289285827624699
-        },
-        {
-            "x": 3.9082461164396207,
-            "y": 7.066057223594786,
-            "heading": -2.901758495279238,
-            "angularVelocity": -1.1661313280225847e-7,
-            "velocityX": 3.7153802506304894,
-            "velocityY": 0.7117503438312037,
-            "timestamp": 0.6170833465562149
-        },
-        {
-            "x": 4.235774584837897,
-            "y": 7.128801407035338,
-            "heading": -2.901758505559241,
-            "angularVelocity": -1.1661313256198823e-7,
-            "velocityX": 3.7153802506305107,
-            "velocityY": 0.711750343831093,
-            "timestamp": 0.7052381103499599
-        },
-        {
-            "x": 4.563303053236174,
-            "y": 7.19154559047589,
-            "heading": -2.9017585158392443,
-            "angularVelocity": -1.166131334044927e-7,
-            "velocityX": 3.7153802506305107,
-            "velocityY": 0.7117503438310929,
-            "timestamp": 0.7933928741437049
-        },
-        {
-            "x": 4.890831521634451,
-            "y": 7.254289773916442,
-            "heading": -2.9017585261192473,
-            "angularVelocity": -1.1661313263433965e-7,
-            "velocityX": 3.7153802506305107,
-            "velocityY": 0.711750343831093,
-            "timestamp": 0.8815476379374498
-        },
-        {
-            "x": 5.218359990032727,
-            "y": 7.317033957356994,
-            "heading": -2.9017585363992504,
-            "angularVelocity": -1.1661313267352917e-7,
-            "velocityX": 3.7153802506305107,
-            "velocityY": 0.7117503438310931,
-            "timestamp": 0.9697024017311948
-        },
-        {
-            "x": 5.545888458431004,
-            "y": 7.379778140797546,
-            "heading": -2.9017585466792535,
-            "angularVelocity": -1.1661313271580576e-7,
-            "velocityX": 3.7153802506305107,
-            "velocityY": 0.7117503438310933,
-            "timestamp": 1.0578571655249398
-        },
-        {
-            "x": 5.873416926829281,
-            "y": 7.442522324238098,
-            "heading": -2.901758556959257,
-            "angularVelocity": -1.166131330429824e-7,
-            "velocityX": 3.7153802506305103,
-            "velocityY": 0.7117503438310929,
-            "timestamp": 1.1460119293186848
-        },
-        {
-            "x": 6.200945395227635,
-            "y": 7.505266507678241,
-            "heading": -2.90175856723926,
-            "angularVelocity": -1.1661313350589332e-7,
-            "velocityX": 3.7153802506314,
-            "velocityY": 0.7117503438264522,
-            "timestamp": 1.2341666931124298
-        },
-        {
-            "x": 6.528473880635578,
-            "y": 7.568010602327234,
-            "heading": -2.9017585775192636,
-            "angularVelocity": -1.1661313512046063e-7,
-            "velocityX": 3.715380443582822,
-            "velocityY": 0.7117493366074876,
-            "timestamp": 1.3223214569061748
-        },
-        {
-            "x": 6.858875530524355,
-            "y": 7.613248618194527,
-            "heading": -2.901758593513828,
-            "angularVelocity": -1.8143731961725824e-7,
-            "velocityX": 3.747972720587331,
-            "velocityY": 0.51316586784965,
-            "timestamp": 1.4104762206999197
-        },
-        {
-            "x": 7.1493370822627105,
-            "y": 7.612204494163831,
-            "heading": -2.9774836288017847,
-            "angularVelocity": -0.859001057108326,
-            "velocityX": 3.294904770182843,
-            "velocityY": -0.011844215624353746,
-            "timestamp": 1.4986309844936647
-        },
-        {
-            "x": 7.3768731997980925,
-            "y": 7.596010582302943,
-            "heading": -3.0421265478344597,
-            "angularVelocity": -0.7332890050493631,
-            "velocityX": 2.581098374533033,
-            "velocityY": -0.18369865863138202,
-            "timestamp": 1.5867857482874097
-        },
-        {
-            "x": 7.541210779550857,
-            "y": 7.566116365480278,
-            "heading": -3.09203212161725,
-            "angularVelocity": -0.566113181354048,
-            "velocityX": 1.8641939775059564,
-            "velocityY": -0.33911062245705376,
-            "timestamp": 1.6749405120811547
-        },
-        {
-            "x": 7.642266411565545,
-            "y": 7.52302718020075,
-            "heading": -3.125932818808563,
-            "angularVelocity": -0.3845588795476813,
-            "velocityX": 1.146343404096945,
-            "velocityY": -0.4887902074166845,
-            "timestamp": 1.7630952758748997
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.19570681865122413,
-            "velocityX": 0.4280379960263239,
-            "velocityY": -0.6355547651607396,
-            "timestamp": 1.8512500396686447
-        },
-        {
-            "x": 7.65149635610838,
-            "y": 7.395583820775626,
-            "heading": -3.1428442524727873,
-            "angularVelocity": 0.003749626131397279,
-            "velocityX": -0.3133749684294368,
-            "velocityY": -0.7851642756589506,
-            "timestamp": 1.9422070297386083
-        },
-        {
-            "x": 7.55553660648891,
-            "y": 7.310596065504526,
-            "heading": -3.1249452939328513,
-            "angularVelocity": 0.19678485981305774,
-            "velocityX": -1.0550013753274867,
-            "velocityY": -0.9343729954757892,
-            "timestamp": 2.033164019808572
-        },
-        {
-            "x": 7.392089155098869,
-            "y": 7.212098072056303,
-            "heading": -3.09046475805122,
-            "angularVelocity": 0.37908615770051013,
-            "velocityX": -1.796975155667623,
-            "velocityY": -1.082907354041688,
-            "timestamp": 2.1241210098785355
-        },
-        {
-            "x": 7.1610935040737775,
-            "y": 7.100214825167467,
-            "heading": -3.041377794518818,
-            "angularVelocity": 0.5396722505288368,
-            "velocityX": -2.539614062068222,
-            "velocityY": -1.2300676045109389,
-            "timestamp": 2.215077999948499
-        },
-        {
-            "x": 6.862392163588639,
-            "y": 6.975338305116689,
-            "heading": -2.9837366917531454,
-            "angularVelocity": 0.6337182301358967,
-            "velocityX": -3.2839844442451716,
-            "velocityY": -1.3729183425572538,
-            "timestamp": 2.3060349900184627
-        },
-        {
-            "x": 6.525454088685491,
-            "y": 6.905573407470138,
-            "heading": -2.9837366810920996,
-            "angularVelocity": 1.1720974856117963e-7,
-            "velocityX": -3.704367027141189,
-            "velocityY": -0.7670097437560687,
-            "timestamp": 2.3969919800884263
-        },
-        {
-            "x": 6.1883268460675325,
-            "y": 6.836728439791378,
-            "heading": -2.983736670548346,
-            "angularVelocity": 1.1592021231646331e-7,
-            "velocityX": -3.7064467762031894,
-            "velocityY": -0.7568958430329531,
-            "timestamp": 2.48794897015839
-        },
-        {
-            "x": 5.851199548758337,
-            "y": 6.767884093073121,
-            "heading": -2.9837365285519053,
-            "angularVelocity": 0.0000015611382957440025,
-            "velocityX": -3.706447377489981,
-            "velocityY": -0.7568890160645022,
-            "timestamp": 2.5789059602283535
-        },
-        {
-            "x": 5.5810538404288925,
-            "y": 6.712717106073741,
-            "heading": -2.9505527658877098,
-            "angularVelocity": 0.3648291641873475,
-            "velocityX": -2.970037906065856,
-            "velocityY": -0.6065172886320637,
-            "timestamp": 2.669862950298317
-        },
-        {
-            "x": 5.3784959131966685,
-            "y": 6.671352294274059,
-            "heading": -2.9178931983006144,
-            "angularVelocity": 0.3590660548682879,
-            "velocityX": -2.226963832866719,
-            "velocityY": -0.45477331393503795,
-            "timestamp": 2.7608199403682807
-        },
-        {
-            "x": 5.2434899940019335,
-            "y": 6.6437823583535645,
-            "heading": -2.8932620140402783,
-            "angularVelocity": 0.2708003446618372,
-            "velocityX": -1.4842830561003668,
-            "velocityY": -0.30310958948050626,
-            "timestamp": 2.8517769304382443
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0.14580533096009823,
-            "velocityX": -0.7419989816068217,
-            "velocityY": -0.15152610418353574,
-            "timestamp": 2.942733920508208
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0,
-            "velocityX": 1.1797533379852482e-32,
-            "velocityY": 0,
-            "timestamp": 3.0336909105781715
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S2.1.traj
deleted file mode 100644
index 7e79decb..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S2.1.traj	
+++ /dev/null
@@ -1,337 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.360726976398386,
-            "y": 6.773087618158843,
-            "heading": -2.760701916982345,
-            "angularVelocity": -0.1232236665180751,
-            "velocityX": 0.699220588306308,
-            "velocityY": 0.18523553288193312,
-            "timestamp": 0.08684952562036656
-        },
-        {
-            "x": 2.4821843215925723,
-            "y": 6.805261248290758,
-            "heading": -2.781888481642216,
-            "angularVelocity": -0.243945657831127,
-            "velocityX": 1.3984802372452498,
-            "velocityY": 0.37045257187160163,
-            "timestamp": 0.17369905124073312
-        },
-        {
-            "x": 2.664377574952537,
-            "y": 6.853518187805784,
-            "heading": -2.813196351929266,
-            "angularVelocity": -0.3604840678552547,
-            "velocityX": 2.0978036674189,
-            "velocityY": 0.5556384927872844,
-            "timestamp": 0.26054857686109967
-        },
-        {
-            "x": 2.9073173781912582,
-            "y": 6.917852930781389,
-            "heading": -2.8538946648532777,
-            "angularVelocity": -0.4686071988668809,
-            "velocityX": 2.797249628060051,
-            "velocityY": 0.7407610175878006,
-            "timestamp": 0.34739810248146624
-        },
-        {
-            "x": 3.211032053030438,
-            "y": 6.99824783750104,
-            "heading": -2.901766372941813,
-            "angularVelocity": -0.5512028735505646,
-            "velocityX": 3.4970216897535775,
-            "velocityY": 0.9256804357344971,
-            "timestamp": 0.4342476281018328
-        },
-        {
-            "x": 3.5286107194849063,
-            "y": 7.082430089360801,
-            "heading": -2.9017663796929978,
-            "angularVelocity": -7.773427725096788e-8,
-            "velocityX": 3.656654013778666,
-            "velocityY": 0.9692885626994009,
-            "timestamp": 0.5210971537221993
-        },
-        {
-            "x": 3.846189385784725,
-            "y": 7.166612341804015,
-            "heading": -2.9017663864441645,
-            "angularVelocity": -7.773406554275384e-8,
-            "velocityX": 3.6566540119980107,
-            "velocityY": 0.9692885694173843,
-            "timestamp": 0.6079466793425659
-        },
-        {
-            "x": 4.163768052084543,
-            "y": 7.250794594247232,
-            "heading": -2.9017663931953313,
-            "angularVelocity": -7.77340646408651e-8,
-            "velocityX": 3.656654011998002,
-            "velocityY": 0.969288569417417,
-            "timestamp": 0.6947962049629324
-        },
-        {
-            "x": 4.481346718384361,
-            "y": 7.334976846690449,
-            "heading": -2.901766399946498,
-            "angularVelocity": -7.773406451460054e-8,
-            "velocityX": 3.656654011998002,
-            "velocityY": 0.9692885694174173,
-            "timestamp": 0.7816457305832989
-        },
-        {
-            "x": 4.798925384684178,
-            "y": 7.419159099133665,
-            "heading": -2.9017664066976647,
-            "angularVelocity": -7.773406529246814e-8,
-            "velocityX": 3.656654011998002,
-            "velocityY": 0.9692885694174173,
-            "timestamp": 0.8684952562036654
-        },
-        {
-            "x": 5.1165040509839965,
-            "y": 7.503341351576882,
-            "heading": -2.9017664134488315,
-            "angularVelocity": -7.773406535280122e-8,
-            "velocityX": 3.656654011998002,
-            "velocityY": 0.969288569417417,
-            "timestamp": 0.9553447818240319
-        },
-        {
-            "x": 5.434082717283814,
-            "y": 7.5875236040201,
-            "heading": -2.901766420199998,
-            "angularVelocity": -7.773406538713727e-8,
-            "velocityX": 3.6566540119980018,
-            "velocityY": 0.9692885694174171,
-            "timestamp": 1.0421943074443984
-        },
-        {
-            "x": 5.751661383583632,
-            "y": 7.6717058564633165,
-            "heading": -2.901766426951165,
-            "angularVelocity": -7.773406530909276e-8,
-            "velocityX": 3.6566540119980027,
-            "velocityY": 0.969288569417416,
-            "timestamp": 1.129043833064765
-        },
-        {
-            "x": 6.0692400498888315,
-            "y": 7.755888108886232,
-            "heading": -2.9017664337023317,
-            "angularVelocity": -7.773406498550699e-8,
-            "velocityX": 3.6566540120599655,
-            "velocityY": 0.9692885691836598,
-            "timestamp": 1.2158933586851315
-        },
-        {
-            "x": 6.386819824167639,
-            "y": 7.840066181352823,
-            "heading": -2.901766440453515,
-            "angularVelocity": -7.773425363968975e-8,
-            "velocityX": 3.6566667694538877,
-            "velocityY": 0.9692404404655809,
-            "timestamp": 1.302742884305498
-        },
-        {
-            "x": 6.714420971530167,
-            "y": 7.864972939151579,
-            "heading": -2.901766454732528,
-            "angularVelocity": -1.6441095007226412e-7,
-            "velocityX": 3.7720545394171228,
-            "velocityY": 0.28678058539594425,
-            "timestamp": 1.3895924099258645
-        },
-        {
-            "x": 7.024629196469154,
-            "y": 7.83076867028296,
-            "heading": -2.946892502448731,
-            "angularVelocity": -0.5195888796655828,
-            "velocityX": 3.57178951437027,
-            "velocityY": -0.39383368676145797,
-            "timestamp": 1.476441935546231
-        },
-        {
-            "x": 7.277681557115376,
-            "y": 7.7704292121587075,
-            "heading": -3.000549099570889,
-            "angularVelocity": -0.6178110558347718,
-            "velocityX": 2.9136873096139366,
-            "velocityY": -0.6947586379213355,
-            "timestamp": 1.5632914611665976
-        },
-        {
-            "x": 7.471595029086891,
-            "y": 7.688687529495423,
-            "heading": -3.0531246016667453,
-            "angularVelocity": -0.6053631464343129,
-            "velocityX": 2.232752229631644,
-            "velocityY": -0.941187439761266,
-            "timestamp": 1.650140986786964
-        },
-        {
-            "x": 7.605791298108634,
-            "y": 7.5872519075766585,
-            "heading": -3.101243004619086,
-            "angularVelocity": -0.5540433595763132,
-            "velocityX": 1.5451583421207176,
-            "velocityY": -1.1679467584193457,
-            "timestamp": 1.7369905124073306
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.4829307041215213,
-            "velocityX": 0.854451436102516,
-            "velocityY": -1.3846006263992148,
-            "timestamp": 1.8238400380276971
-        },
-        {
-            "x": 7.684759443310988,
-            "y": 7.301931264284962,
-            "heading": -3.1822787877008336,
-            "angularVelocity": -0.3860471334464062,
-            "velocityX": 0.046999382572933714,
-            "velocityY": -1.6300495989311972,
-            "timestamp": 1.925106119638729
-        },
-        {
-            "x": 7.607446461395855,
-            "y": 7.1130338870077985,
-            "heading": -3.2115348858230854,
-            "angularVelocity": -0.2889032305471994,
-            "velocityX": -0.7634637450650118,
-            "velocityY": -1.8653568329339223,
-            "timestamp": 2.0263722012497607
-        },
-        {
-            "x": 7.447519490138273,
-            "y": 6.902260922095987,
-            "heading": -3.2309059414306347,
-            "angularVelocity": -0.1912886852083134,
-            "velocityX": -1.5792748046828227,
-            "velocityY": -2.0813777086972793,
-            "timestamp": 2.1276382828607927
-        },
-        {
-            "x": 7.203787473069244,
-            "y": 6.674749702005586,
-            "heading": -3.240291639650134,
-            "angularVelocity": -0.09268353302700813,
-            "velocityX": -2.406847516870182,
-            "velocityY": -2.246667556121621,
-            "timestamp": 2.2289043644718247
-        },
-        {
-            "x": 6.8784205377457015,
-            "y": 6.472538498060418,
-            "heading": -3.240291634989942,
-            "angularVelocity": 4.6019273769549756e-8,
-            "velocityX": -3.212990274205886,
-            "velocityY": -1.996830535247213,
-            "timestamp": 2.3301704460828567
-        },
-        {
-            "x": 6.516724017217928,
-            "y": 6.34632974848032,
-            "heading": -3.2402916309896224,
-            "angularVelocity": 3.950305172889797e-8,
-            "velocityX": -3.5717440111670684,
-            "velocityY": -1.2463082166308144,
-            "timestamp": 2.4314365276938887
-        },
-        {
-            "x": 6.155023833001519,
-            "y": 6.220131499152302,
-            "heading": -3.240291626989329,
-            "angularVelocity": 3.950279970360729e-8,
-            "velocityX": -3.571780189992463,
-            "velocityY": -1.2462045269279236,
-            "timestamp": 2.5327026093049207
-        },
-        {
-            "x": 5.793323648774168,
-            "y": 6.093933249855646,
-            "heading": -3.240291622989035,
-            "angularVelocity": 3.950280005859099e-8,
-            "velocityX": -3.571780190100518,
-            "velocityY": -1.2462045266182225,
-            "timestamp": 2.6339686909159528
-        },
-        {
-            "x": 5.431623464546816,
-            "y": 5.96773500055899,
-            "heading": -3.240291618988741,
-            "angularVelocity": 3.9502799442138326e-8,
-            "velocityX": -3.5717801901005184,
-            "velocityY": -1.246204526618221,
-            "timestamp": 2.7352347725269848
-        },
-        {
-            "x": 5.0699232803112935,
-            "y": 5.841536751285806,
-            "heading": -3.240291614988434,
-            "angularVelocity": 3.9502931091413385e-8,
-            "velocityX": -3.571780190181203,
-            "velocityY": -1.2462045263864454,
-            "timestamp": 2.836500854138017
-        },
-        {
-            "x": 4.747153979068025,
-            "y": 5.72892205607683,
-            "heading": -3.2014500073341168,
-            "angularVelocity": 0.383559895242649,
-            "velocityX": -3.1873387032305858,
-            "velocityY": -1.11206727284526,
-            "timestamp": 2.937766935749049
-        },
-        {
-            "x": 4.505076992705577,
-            "y": 5.644461029601604,
-            "heading": -3.172317846735081,
-            "angularVelocity": 0.2876793506335712,
-            "velocityX": -2.3905041304188885,
-            "velocityY": -0.8340505046852051,
-            "timestamp": 3.039033017360081
-        },
-        {
-            "x": 4.343692332074418,
-            "y": 5.588153676944576,
-            "heading": -3.1528961309698516,
-            "angularVelocity": 0.19178895298675563,
-            "velocityX": -1.5936694504591111,
-            "velocityY": -0.5560336863163278,
-            "timestamp": 3.140299098971113
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0.0958941398322912,
-            "velocityX": -0.796834742598023,
-            "velocityY": -0.2780168492417652,
-            "timestamp": 3.241565180582145
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0,
-            "velocityX": 6.013328716711935e-32,
-            "velocityY": 0,
-            "timestamp": 3.342831262193177
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S2.traj
deleted file mode 100644
index 7e79decb..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1 To S2.traj	
+++ /dev/null
@@ -1,337 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.360726976398386,
-            "y": 6.773087618158843,
-            "heading": -2.760701916982345,
-            "angularVelocity": -0.1232236665180751,
-            "velocityX": 0.699220588306308,
-            "velocityY": 0.18523553288193312,
-            "timestamp": 0.08684952562036656
-        },
-        {
-            "x": 2.4821843215925723,
-            "y": 6.805261248290758,
-            "heading": -2.781888481642216,
-            "angularVelocity": -0.243945657831127,
-            "velocityX": 1.3984802372452498,
-            "velocityY": 0.37045257187160163,
-            "timestamp": 0.17369905124073312
-        },
-        {
-            "x": 2.664377574952537,
-            "y": 6.853518187805784,
-            "heading": -2.813196351929266,
-            "angularVelocity": -0.3604840678552547,
-            "velocityX": 2.0978036674189,
-            "velocityY": 0.5556384927872844,
-            "timestamp": 0.26054857686109967
-        },
-        {
-            "x": 2.9073173781912582,
-            "y": 6.917852930781389,
-            "heading": -2.8538946648532777,
-            "angularVelocity": -0.4686071988668809,
-            "velocityX": 2.797249628060051,
-            "velocityY": 0.7407610175878006,
-            "timestamp": 0.34739810248146624
-        },
-        {
-            "x": 3.211032053030438,
-            "y": 6.99824783750104,
-            "heading": -2.901766372941813,
-            "angularVelocity": -0.5512028735505646,
-            "velocityX": 3.4970216897535775,
-            "velocityY": 0.9256804357344971,
-            "timestamp": 0.4342476281018328
-        },
-        {
-            "x": 3.5286107194849063,
-            "y": 7.082430089360801,
-            "heading": -2.9017663796929978,
-            "angularVelocity": -7.773427725096788e-8,
-            "velocityX": 3.656654013778666,
-            "velocityY": 0.9692885626994009,
-            "timestamp": 0.5210971537221993
-        },
-        {
-            "x": 3.846189385784725,
-            "y": 7.166612341804015,
-            "heading": -2.9017663864441645,
-            "angularVelocity": -7.773406554275384e-8,
-            "velocityX": 3.6566540119980107,
-            "velocityY": 0.9692885694173843,
-            "timestamp": 0.6079466793425659
-        },
-        {
-            "x": 4.163768052084543,
-            "y": 7.250794594247232,
-            "heading": -2.9017663931953313,
-            "angularVelocity": -7.77340646408651e-8,
-            "velocityX": 3.656654011998002,
-            "velocityY": 0.969288569417417,
-            "timestamp": 0.6947962049629324
-        },
-        {
-            "x": 4.481346718384361,
-            "y": 7.334976846690449,
-            "heading": -2.901766399946498,
-            "angularVelocity": -7.773406451460054e-8,
-            "velocityX": 3.656654011998002,
-            "velocityY": 0.9692885694174173,
-            "timestamp": 0.7816457305832989
-        },
-        {
-            "x": 4.798925384684178,
-            "y": 7.419159099133665,
-            "heading": -2.9017664066976647,
-            "angularVelocity": -7.773406529246814e-8,
-            "velocityX": 3.656654011998002,
-            "velocityY": 0.9692885694174173,
-            "timestamp": 0.8684952562036654
-        },
-        {
-            "x": 5.1165040509839965,
-            "y": 7.503341351576882,
-            "heading": -2.9017664134488315,
-            "angularVelocity": -7.773406535280122e-8,
-            "velocityX": 3.656654011998002,
-            "velocityY": 0.969288569417417,
-            "timestamp": 0.9553447818240319
-        },
-        {
-            "x": 5.434082717283814,
-            "y": 7.5875236040201,
-            "heading": -2.901766420199998,
-            "angularVelocity": -7.773406538713727e-8,
-            "velocityX": 3.6566540119980018,
-            "velocityY": 0.9692885694174171,
-            "timestamp": 1.0421943074443984
-        },
-        {
-            "x": 5.751661383583632,
-            "y": 7.6717058564633165,
-            "heading": -2.901766426951165,
-            "angularVelocity": -7.773406530909276e-8,
-            "velocityX": 3.6566540119980027,
-            "velocityY": 0.969288569417416,
-            "timestamp": 1.129043833064765
-        },
-        {
-            "x": 6.0692400498888315,
-            "y": 7.755888108886232,
-            "heading": -2.9017664337023317,
-            "angularVelocity": -7.773406498550699e-8,
-            "velocityX": 3.6566540120599655,
-            "velocityY": 0.9692885691836598,
-            "timestamp": 1.2158933586851315
-        },
-        {
-            "x": 6.386819824167639,
-            "y": 7.840066181352823,
-            "heading": -2.901766440453515,
-            "angularVelocity": -7.773425363968975e-8,
-            "velocityX": 3.6566667694538877,
-            "velocityY": 0.9692404404655809,
-            "timestamp": 1.302742884305498
-        },
-        {
-            "x": 6.714420971530167,
-            "y": 7.864972939151579,
-            "heading": -2.901766454732528,
-            "angularVelocity": -1.6441095007226412e-7,
-            "velocityX": 3.7720545394171228,
-            "velocityY": 0.28678058539594425,
-            "timestamp": 1.3895924099258645
-        },
-        {
-            "x": 7.024629196469154,
-            "y": 7.83076867028296,
-            "heading": -2.946892502448731,
-            "angularVelocity": -0.5195888796655828,
-            "velocityX": 3.57178951437027,
-            "velocityY": -0.39383368676145797,
-            "timestamp": 1.476441935546231
-        },
-        {
-            "x": 7.277681557115376,
-            "y": 7.7704292121587075,
-            "heading": -3.000549099570889,
-            "angularVelocity": -0.6178110558347718,
-            "velocityX": 2.9136873096139366,
-            "velocityY": -0.6947586379213355,
-            "timestamp": 1.5632914611665976
-        },
-        {
-            "x": 7.471595029086891,
-            "y": 7.688687529495423,
-            "heading": -3.0531246016667453,
-            "angularVelocity": -0.6053631464343129,
-            "velocityX": 2.232752229631644,
-            "velocityY": -0.941187439761266,
-            "timestamp": 1.650140986786964
-        },
-        {
-            "x": 7.605791298108634,
-            "y": 7.5872519075766585,
-            "heading": -3.101243004619086,
-            "angularVelocity": -0.5540433595763132,
-            "velocityX": 1.5451583421207176,
-            "velocityY": -1.1679467584193457,
-            "timestamp": 1.7369905124073306
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.4829307041215213,
-            "velocityX": 0.854451436102516,
-            "velocityY": -1.3846006263992148,
-            "timestamp": 1.8238400380276971
-        },
-        {
-            "x": 7.684759443310988,
-            "y": 7.301931264284962,
-            "heading": -3.1822787877008336,
-            "angularVelocity": -0.3860471334464062,
-            "velocityX": 0.046999382572933714,
-            "velocityY": -1.6300495989311972,
-            "timestamp": 1.925106119638729
-        },
-        {
-            "x": 7.607446461395855,
-            "y": 7.1130338870077985,
-            "heading": -3.2115348858230854,
-            "angularVelocity": -0.2889032305471994,
-            "velocityX": -0.7634637450650118,
-            "velocityY": -1.8653568329339223,
-            "timestamp": 2.0263722012497607
-        },
-        {
-            "x": 7.447519490138273,
-            "y": 6.902260922095987,
-            "heading": -3.2309059414306347,
-            "angularVelocity": -0.1912886852083134,
-            "velocityX": -1.5792748046828227,
-            "velocityY": -2.0813777086972793,
-            "timestamp": 2.1276382828607927
-        },
-        {
-            "x": 7.203787473069244,
-            "y": 6.674749702005586,
-            "heading": -3.240291639650134,
-            "angularVelocity": -0.09268353302700813,
-            "velocityX": -2.406847516870182,
-            "velocityY": -2.246667556121621,
-            "timestamp": 2.2289043644718247
-        },
-        {
-            "x": 6.8784205377457015,
-            "y": 6.472538498060418,
-            "heading": -3.240291634989942,
-            "angularVelocity": 4.6019273769549756e-8,
-            "velocityX": -3.212990274205886,
-            "velocityY": -1.996830535247213,
-            "timestamp": 2.3301704460828567
-        },
-        {
-            "x": 6.516724017217928,
-            "y": 6.34632974848032,
-            "heading": -3.2402916309896224,
-            "angularVelocity": 3.950305172889797e-8,
-            "velocityX": -3.5717440111670684,
-            "velocityY": -1.2463082166308144,
-            "timestamp": 2.4314365276938887
-        },
-        {
-            "x": 6.155023833001519,
-            "y": 6.220131499152302,
-            "heading": -3.240291626989329,
-            "angularVelocity": 3.950279970360729e-8,
-            "velocityX": -3.571780189992463,
-            "velocityY": -1.2462045269279236,
-            "timestamp": 2.5327026093049207
-        },
-        {
-            "x": 5.793323648774168,
-            "y": 6.093933249855646,
-            "heading": -3.240291622989035,
-            "angularVelocity": 3.950280005859099e-8,
-            "velocityX": -3.571780190100518,
-            "velocityY": -1.2462045266182225,
-            "timestamp": 2.6339686909159528
-        },
-        {
-            "x": 5.431623464546816,
-            "y": 5.96773500055899,
-            "heading": -3.240291618988741,
-            "angularVelocity": 3.9502799442138326e-8,
-            "velocityX": -3.5717801901005184,
-            "velocityY": -1.246204526618221,
-            "timestamp": 2.7352347725269848
-        },
-        {
-            "x": 5.0699232803112935,
-            "y": 5.841536751285806,
-            "heading": -3.240291614988434,
-            "angularVelocity": 3.9502931091413385e-8,
-            "velocityX": -3.571780190181203,
-            "velocityY": -1.2462045263864454,
-            "timestamp": 2.836500854138017
-        },
-        {
-            "x": 4.747153979068025,
-            "y": 5.72892205607683,
-            "heading": -3.2014500073341168,
-            "angularVelocity": 0.383559895242649,
-            "velocityX": -3.1873387032305858,
-            "velocityY": -1.11206727284526,
-            "timestamp": 2.937766935749049
-        },
-        {
-            "x": 4.505076992705577,
-            "y": 5.644461029601604,
-            "heading": -3.172317846735081,
-            "angularVelocity": 0.2876793506335712,
-            "velocityX": -2.3905041304188885,
-            "velocityY": -0.8340505046852051,
-            "timestamp": 3.039033017360081
-        },
-        {
-            "x": 4.343692332074418,
-            "y": 5.588153676944576,
-            "heading": -3.1528961309698516,
-            "angularVelocity": 0.19178895298675563,
-            "velocityX": -1.5936694504591111,
-            "velocityY": -0.5560336863163278,
-            "timestamp": 3.140299098971113
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0.0958941398322912,
-            "velocityX": -0.796834742598023,
-            "velocityY": -0.2780168492417652,
-            "timestamp": 3.241565180582145
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0,
-            "velocityX": 6.013328716711935e-32,
-            "velocityY": 0,
-            "timestamp": 3.342831262193177
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S1.1.traj
deleted file mode 100644
index a6012f8c..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S1.1.traj	
+++ /dev/null
@@ -1,319 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3639970489597717,
-            "y": 6.742603274185352,
-            "heading": -2.7625259389239276,
-            "angularVelocity": -0.14112178255834015,
-            "velocityX": 0.7210140239806111,
-            "velocityY": -0.16219874791866862,
-            "timestamp": 0.08875978390330586
-        },
-        {
-            "x": 2.4920132643077064,
-            "y": 6.7138069605767345,
-            "heading": -2.7859988261252213,
-            "angularVelocity": -0.26445408234505485,
-            "velocityX": 1.4422772309517358,
-            "velocityY": -0.3244297399370449,
-            "timestamp": 0.1775195678066117
-        },
-        {
-            "x": 2.684079448734256,
-            "y": 6.670607796290334,
-            "heading": -2.8177443395453605,
-            "angularVelocity": -0.3576564973921373,
-            "velocityX": 2.163887472233876,
-            "velocityY": -0.4866974927908953,
-            "timestamp": 0.26627935170991757
-        },
-        {
-            "x": 2.9402327242018096,
-            "y": 6.613005243899921,
-            "heading": -2.852261622202671,
-            "angularVelocity": -0.3888842574798656,
-            "velocityX": 2.8859159430424604,
-            "velocityY": -0.6489713004840627,
-            "timestamp": 0.3550391356132234
-        },
-        {
-            "x": 3.260353111631952,
-            "y": 6.541052974820016,
-            "heading": -2.8718469111116423,
-            "angularVelocity": -0.22065498638783787,
-            "velocityX": 3.6065926859272044,
-            "velocityY": -0.8106404265054099,
-            "timestamp": 0.4437989195165293
-        },
-        {
-            "x": 3.5879291031343383,
-            "y": 6.467313799666466,
-            "heading": -2.87184692127097,
-            "angularVelocity": -1.1445867602426544e-7,
-            "velocityX": 3.690590232387729,
-            "velocityY": -0.83077236008012,
-            "timestamp": 0.5325587034198351
-        },
-        {
-            "x": 3.915505094528652,
-            "y": 6.3935746240328,
-            "heading": -2.87184693143029,
-            "angularVelocity": -1.1445859480672728e-7,
-            "velocityX": 3.6905902311701486,
-            "velocityY": -0.8307723654892717,
-            "timestamp": 0.621318487323141
-        },
-        {
-            "x": 4.243081085922966,
-            "y": 6.3198354483991315,
-            "heading": -2.87184694158961,
-            "angularVelocity": -1.1445859516237229e-7,
-            "velocityX": 3.6905902311701433,
-            "velocityY": -0.8307723654892962,
-            "timestamp": 0.7100782712264468
-        },
-        {
-            "x": 4.5706570773172785,
-            "y": 6.246096272765464,
-            "heading": -2.87184695174893,
-            "angularVelocity": -1.1445859514834922e-7,
-            "velocityX": 3.6905902311701433,
-            "velocityY": -0.830772365489296,
-            "timestamp": 0.7988380551297527
-        },
-        {
-            "x": 4.898233068711592,
-            "y": 6.172357097131795,
-            "heading": -2.8718469619082505,
-            "angularVelocity": -1.1445859490669181e-7,
-            "velocityX": 3.6905902311701437,
-            "velocityY": -0.830772365489296,
-            "timestamp": 0.8875978390330586
-        },
-        {
-            "x": 5.225809060105905,
-            "y": 6.0986179214981275,
-            "heading": -2.8718469720675706,
-            "angularVelocity": -1.1445859563440553e-7,
-            "velocityX": 3.6905902311701433,
-            "velocityY": -0.830772365489296,
-            "timestamp": 0.9763576229363644
-        },
-        {
-            "x": 5.553385051500219,
-            "y": 6.024878745864458,
-            "heading": -2.871846982226891,
-            "angularVelocity": -1.1445859523892014e-7,
-            "velocityX": 3.6905902311701437,
-            "velocityY": -0.830772365489296,
-            "timestamp": 1.0651174068396703
-        },
-        {
-            "x": 5.8809610428945325,
-            "y": 5.951139570230791,
-            "heading": -2.871846992386211,
-            "angularVelocity": -1.1445859598937227e-7,
-            "velocityX": 3.6905902311701433,
-            "velocityY": -0.8307723654892962,
-            "timestamp": 1.1538771907429761
-        },
-        {
-            "x": 6.208537034288882,
-            "y": 5.877400394597284,
-            "heading": -2.871847002545531,
-            "angularVelocity": -1.1445859482620514e-7,
-            "velocityX": 3.6905902311705536,
-            "velocityY": -0.830772365487475,
-            "timestamp": 1.242636974646282
-        },
-        {
-            "x": 6.536113033734776,
-            "y": 5.80366125473165,
-            "heading": -2.8718470127048517,
-            "angularVelocity": -1.1445859560539411e-7,
-            "velocityX": 3.6905903218821825,
-            "velocityY": -0.8307719625136204,
-            "timestamp": 1.3313967585495878
-        },
-        {
-            "x": 6.865344430510141,
-            "y": 5.737705340336343,
-            "heading": -2.8718470255455273,
-            "angularVelocity": -1.4466772317254716e-7,
-            "velocityX": 3.7092406301262124,
-            "velocityY": -0.7430833142536372,
-            "timestamp": 1.4201565424528937
-        },
-        {
-            "x": 7.155715453065551,
-            "y": 5.718947060788103,
-            "heading": -2.9583296823858576,
-            "angularVelocity": -0.974345058506937,
-            "velocityX": 3.2714255238807013,
-            "velocityY": -0.21133759821538473,
-            "timestamp": 1.5089163263561995
-        },
-        {
-            "x": 7.382461638266762,
-            "y": 5.716006595883618,
-            "heading": -3.030136340606869,
-            "angularVelocity": -0.8089999216225757,
-            "velocityX": 2.5546049711908463,
-            "velocityY": -0.0331283468162716,
-            "timestamp": 1.5976761102595054
-        },
-        {
-            "x": 7.54544918703291,
-            "y": 5.728176149550433,
-            "heading": -3.085282134377374,
-            "angularVelocity": -0.6212925645535633,
-            "velocityX": 1.836276989404401,
-            "velocityY": 0.13710661666404886,
-            "timestamp": 1.6864358941628113
-        },
-        {
-            "x": 7.644635592677741,
-            "y": 5.7552144852221,
-            "heading": -3.1230806073530184,
-            "angularVelocity": -0.4258513407020198,
-            "velocityX": 1.117470111834477,
-            "velocityY": 0.3046237212691066,
-            "timestamp": 1.775195678066117
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.2265068586519956,
-            "velocityX": 0.39842827198390307,
-            "velocityY": 0.4707708034014574,
-            "timestamp": 1.863955461969423
-        },
-        {
-            "x": 7.649158607972273,
-            "y": 5.85526478345475,
-            "heading": -3.144984703286767,
-            "angularVelocity": -0.019773386750234018,
-            "velocityX": -0.3389130220112019,
-            "velocityY": 0.640265971773451,
-            "timestamp": 1.9549563678794555
-        },
-        {
-            "x": 7.551198365445194,
-            "y": 5.928933141126823,
-            "heading": -3.12869095344792,
-            "angularVelocity": 0.17905041357452225,
-            "velocityX": -1.076475465243452,
-            "velocityY": 0.8095343330417561,
-            "timestamp": 2.045957273789488
-        },
-        {
-            "x": 7.386086795598405,
-            "y": 6.0179701455014545,
-            "heading": -3.0955113223095805,
-            "angularVelocity": 0.3646077015007144,
-            "velocityX": -1.814394793058725,
-            "velocityY": 0.9784188792874091,
-            "timestamp": 2.1369581796995205
-        },
-        {
-            "x": 7.153763389046842,
-            "y": 6.12230392350914,
-            "heading": -3.047867696260251,
-            "angularVelocity": 0.5235511182320745,
-            "velocityX": -2.5529790525519296,
-            "velocityY": 1.1465136194450147,
-            "timestamp": 2.227959085609553
-        },
-        {
-            "x": 6.854082785517415,
-            "y": 6.241701438550574,
-            "heading": -2.993162545915138,
-            "angularVelocity": 0.6011495138212849,
-            "velocityX": -3.29316066178182,
-            "velocityY": 1.3120475433450827,
-            "timestamp": 2.3189599915195855
-        },
-        {
-            "x": 6.518695058786276,
-            "y": 6.319314800630408,
-            "heading": -2.9931625344725843,
-            "angularVelocity": 1.257410975035093e-7,
-            "velocityX": -3.685542724846253,
-            "velocityY": 0.8528855982660521,
-            "timestamp": 2.409960897429618
-        },
-        {
-            "x": 6.18330715393461,
-            "y": 6.396927393000049,
-            "heading": -2.993162523030137,
-            "angularVelocity": 1.2573992931558898e-7,
-            "velocityX": -3.6855446821952005,
-            "velocityY": 0.8528771399965199,
-            "timestamp": 2.5009618033396506
-        },
-        {
-            "x": 5.8479121197525,
-            "y": 6.474508873089667,
-            "heading": -2.9931623885517284,
-            "angularVelocity": 0.0000014777699956967743,
-            "velocityX": -3.685623025705882,
-            "velocityY": 0.852535250212982,
-            "timestamp": 2.591962709249683
-        },
-        {
-            "x": 5.579069881205996,
-            "y": 6.536724652078007,
-            "heading": -2.956924011318997,
-            "angularVelocity": 0.3982199613326688,
-            "velocityX": -2.954280903668077,
-            "velocityY": 0.6836830728899436,
-            "timestamp": 2.6829636151597156
-        },
-        {
-            "x": 5.377498755107576,
-            "y": 6.583371353425055,
-            "heading": -2.9212816182082424,
-            "angularVelocity": 0.391670750464755,
-            "velocityX": -2.2150452688647118,
-            "velocityY": 0.5125960107821789,
-            "timestamp": 2.773964521069748
-        },
-        {
-            "x": 5.243156221242747,
-            "y": 6.614459642803244,
-            "heading": -2.894437298256596,
-            "angularVelocity": 0.29498959030348726,
-            "velocityX": -1.4762768845141638,
-            "velocityY": 0.341626152699224,
-            "timestamp": 2.8649654269797806
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0.15865004982334352,
-            "velocityX": -0.7379731066539015,
-            "velocityY": 0.17077145596902096,
-            "timestamp": 2.955966332889813
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0,
-            "velocityX": 2.398158538123596e-33,
-            "velocityY": 0,
-            "timestamp": 3.0469672387998457
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S1.traj
deleted file mode 100644
index a6012f8c..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S1.traj	
+++ /dev/null
@@ -1,319 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3639970489597717,
-            "y": 6.742603274185352,
-            "heading": -2.7625259389239276,
-            "angularVelocity": -0.14112178255834015,
-            "velocityX": 0.7210140239806111,
-            "velocityY": -0.16219874791866862,
-            "timestamp": 0.08875978390330586
-        },
-        {
-            "x": 2.4920132643077064,
-            "y": 6.7138069605767345,
-            "heading": -2.7859988261252213,
-            "angularVelocity": -0.26445408234505485,
-            "velocityX": 1.4422772309517358,
-            "velocityY": -0.3244297399370449,
-            "timestamp": 0.1775195678066117
-        },
-        {
-            "x": 2.684079448734256,
-            "y": 6.670607796290334,
-            "heading": -2.8177443395453605,
-            "angularVelocity": -0.3576564973921373,
-            "velocityX": 2.163887472233876,
-            "velocityY": -0.4866974927908953,
-            "timestamp": 0.26627935170991757
-        },
-        {
-            "x": 2.9402327242018096,
-            "y": 6.613005243899921,
-            "heading": -2.852261622202671,
-            "angularVelocity": -0.3888842574798656,
-            "velocityX": 2.8859159430424604,
-            "velocityY": -0.6489713004840627,
-            "timestamp": 0.3550391356132234
-        },
-        {
-            "x": 3.260353111631952,
-            "y": 6.541052974820016,
-            "heading": -2.8718469111116423,
-            "angularVelocity": -0.22065498638783787,
-            "velocityX": 3.6065926859272044,
-            "velocityY": -0.8106404265054099,
-            "timestamp": 0.4437989195165293
-        },
-        {
-            "x": 3.5879291031343383,
-            "y": 6.467313799666466,
-            "heading": -2.87184692127097,
-            "angularVelocity": -1.1445867602426544e-7,
-            "velocityX": 3.690590232387729,
-            "velocityY": -0.83077236008012,
-            "timestamp": 0.5325587034198351
-        },
-        {
-            "x": 3.915505094528652,
-            "y": 6.3935746240328,
-            "heading": -2.87184693143029,
-            "angularVelocity": -1.1445859480672728e-7,
-            "velocityX": 3.6905902311701486,
-            "velocityY": -0.8307723654892717,
-            "timestamp": 0.621318487323141
-        },
-        {
-            "x": 4.243081085922966,
-            "y": 6.3198354483991315,
-            "heading": -2.87184694158961,
-            "angularVelocity": -1.1445859516237229e-7,
-            "velocityX": 3.6905902311701433,
-            "velocityY": -0.8307723654892962,
-            "timestamp": 0.7100782712264468
-        },
-        {
-            "x": 4.5706570773172785,
-            "y": 6.246096272765464,
-            "heading": -2.87184695174893,
-            "angularVelocity": -1.1445859514834922e-7,
-            "velocityX": 3.6905902311701433,
-            "velocityY": -0.830772365489296,
-            "timestamp": 0.7988380551297527
-        },
-        {
-            "x": 4.898233068711592,
-            "y": 6.172357097131795,
-            "heading": -2.8718469619082505,
-            "angularVelocity": -1.1445859490669181e-7,
-            "velocityX": 3.6905902311701437,
-            "velocityY": -0.830772365489296,
-            "timestamp": 0.8875978390330586
-        },
-        {
-            "x": 5.225809060105905,
-            "y": 6.0986179214981275,
-            "heading": -2.8718469720675706,
-            "angularVelocity": -1.1445859563440553e-7,
-            "velocityX": 3.6905902311701433,
-            "velocityY": -0.830772365489296,
-            "timestamp": 0.9763576229363644
-        },
-        {
-            "x": 5.553385051500219,
-            "y": 6.024878745864458,
-            "heading": -2.871846982226891,
-            "angularVelocity": -1.1445859523892014e-7,
-            "velocityX": 3.6905902311701437,
-            "velocityY": -0.830772365489296,
-            "timestamp": 1.0651174068396703
-        },
-        {
-            "x": 5.8809610428945325,
-            "y": 5.951139570230791,
-            "heading": -2.871846992386211,
-            "angularVelocity": -1.1445859598937227e-7,
-            "velocityX": 3.6905902311701433,
-            "velocityY": -0.8307723654892962,
-            "timestamp": 1.1538771907429761
-        },
-        {
-            "x": 6.208537034288882,
-            "y": 5.877400394597284,
-            "heading": -2.871847002545531,
-            "angularVelocity": -1.1445859482620514e-7,
-            "velocityX": 3.6905902311705536,
-            "velocityY": -0.830772365487475,
-            "timestamp": 1.242636974646282
-        },
-        {
-            "x": 6.536113033734776,
-            "y": 5.80366125473165,
-            "heading": -2.8718470127048517,
-            "angularVelocity": -1.1445859560539411e-7,
-            "velocityX": 3.6905903218821825,
-            "velocityY": -0.8307719625136204,
-            "timestamp": 1.3313967585495878
-        },
-        {
-            "x": 6.865344430510141,
-            "y": 5.737705340336343,
-            "heading": -2.8718470255455273,
-            "angularVelocity": -1.4466772317254716e-7,
-            "velocityX": 3.7092406301262124,
-            "velocityY": -0.7430833142536372,
-            "timestamp": 1.4201565424528937
-        },
-        {
-            "x": 7.155715453065551,
-            "y": 5.718947060788103,
-            "heading": -2.9583296823858576,
-            "angularVelocity": -0.974345058506937,
-            "velocityX": 3.2714255238807013,
-            "velocityY": -0.21133759821538473,
-            "timestamp": 1.5089163263561995
-        },
-        {
-            "x": 7.382461638266762,
-            "y": 5.716006595883618,
-            "heading": -3.030136340606869,
-            "angularVelocity": -0.8089999216225757,
-            "velocityX": 2.5546049711908463,
-            "velocityY": -0.0331283468162716,
-            "timestamp": 1.5976761102595054
-        },
-        {
-            "x": 7.54544918703291,
-            "y": 5.728176149550433,
-            "heading": -3.085282134377374,
-            "angularVelocity": -0.6212925645535633,
-            "velocityX": 1.836276989404401,
-            "velocityY": 0.13710661666404886,
-            "timestamp": 1.6864358941628113
-        },
-        {
-            "x": 7.644635592677741,
-            "y": 5.7552144852221,
-            "heading": -3.1230806073530184,
-            "angularVelocity": -0.4258513407020198,
-            "velocityX": 1.117470111834477,
-            "velocityY": 0.3046237212691066,
-            "timestamp": 1.775195678066117
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.2265068586519956,
-            "velocityX": 0.39842827198390307,
-            "velocityY": 0.4707708034014574,
-            "timestamp": 1.863955461969423
-        },
-        {
-            "x": 7.649158607972273,
-            "y": 5.85526478345475,
-            "heading": -3.144984703286767,
-            "angularVelocity": -0.019773386750234018,
-            "velocityX": -0.3389130220112019,
-            "velocityY": 0.640265971773451,
-            "timestamp": 1.9549563678794555
-        },
-        {
-            "x": 7.551198365445194,
-            "y": 5.928933141126823,
-            "heading": -3.12869095344792,
-            "angularVelocity": 0.17905041357452225,
-            "velocityX": -1.076475465243452,
-            "velocityY": 0.8095343330417561,
-            "timestamp": 2.045957273789488
-        },
-        {
-            "x": 7.386086795598405,
-            "y": 6.0179701455014545,
-            "heading": -3.0955113223095805,
-            "angularVelocity": 0.3646077015007144,
-            "velocityX": -1.814394793058725,
-            "velocityY": 0.9784188792874091,
-            "timestamp": 2.1369581796995205
-        },
-        {
-            "x": 7.153763389046842,
-            "y": 6.12230392350914,
-            "heading": -3.047867696260251,
-            "angularVelocity": 0.5235511182320745,
-            "velocityX": -2.5529790525519296,
-            "velocityY": 1.1465136194450147,
-            "timestamp": 2.227959085609553
-        },
-        {
-            "x": 6.854082785517415,
-            "y": 6.241701438550574,
-            "heading": -2.993162545915138,
-            "angularVelocity": 0.6011495138212849,
-            "velocityX": -3.29316066178182,
-            "velocityY": 1.3120475433450827,
-            "timestamp": 2.3189599915195855
-        },
-        {
-            "x": 6.518695058786276,
-            "y": 6.319314800630408,
-            "heading": -2.9931625344725843,
-            "angularVelocity": 1.257410975035093e-7,
-            "velocityX": -3.685542724846253,
-            "velocityY": 0.8528855982660521,
-            "timestamp": 2.409960897429618
-        },
-        {
-            "x": 6.18330715393461,
-            "y": 6.396927393000049,
-            "heading": -2.993162523030137,
-            "angularVelocity": 1.2573992931558898e-7,
-            "velocityX": -3.6855446821952005,
-            "velocityY": 0.8528771399965199,
-            "timestamp": 2.5009618033396506
-        },
-        {
-            "x": 5.8479121197525,
-            "y": 6.474508873089667,
-            "heading": -2.9931623885517284,
-            "angularVelocity": 0.0000014777699956967743,
-            "velocityX": -3.685623025705882,
-            "velocityY": 0.852535250212982,
-            "timestamp": 2.591962709249683
-        },
-        {
-            "x": 5.579069881205996,
-            "y": 6.536724652078007,
-            "heading": -2.956924011318997,
-            "angularVelocity": 0.3982199613326688,
-            "velocityX": -2.954280903668077,
-            "velocityY": 0.6836830728899436,
-            "timestamp": 2.6829636151597156
-        },
-        {
-            "x": 5.377498755107576,
-            "y": 6.583371353425055,
-            "heading": -2.9212816182082424,
-            "angularVelocity": 0.391670750464755,
-            "velocityX": -2.2150452688647118,
-            "velocityY": 0.5125960107821789,
-            "timestamp": 2.773964521069748
-        },
-        {
-            "x": 5.243156221242747,
-            "y": 6.614459642803244,
-            "heading": -2.894437298256596,
-            "angularVelocity": 0.29498959030348726,
-            "velocityX": -1.4762768845141638,
-            "velocityY": 0.341626152699224,
-            "timestamp": 2.8649654269797806
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0.15865004982334352,
-            "velocityX": -0.7379731066539015,
-            "velocityY": 0.17077145596902096,
-            "timestamp": 2.955966332889813
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0,
-            "velocityX": 2.398158538123596e-33,
-            "velocityY": 0,
-            "timestamp": 3.0469672387998457
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S2.1.traj
deleted file mode 100644
index 84327156..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S2.1.traj	
+++ /dev/null
@@ -1,328 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.36406618503168,
-            "y": 6.750841975187211,
-            "heading": -2.7578611838826945,
-            "angularVelocity": -0.0894504040603958,
-            "velocityX": 0.7289927602771729,
-            "velocityY": -0.07007059190289512,
-            "timestamp": 0.08788315676457649
-        },
-        {
-            "x": 2.4922026900458016,
-            "y": 6.738526737153379,
-            "heading": -2.773150566031247,
-            "angularVelocity": -0.17397397534899775,
-            "velocityX": 1.458032571103211,
-            "velocityY": -0.1401319489105608,
-            "timestamp": 0.17576631352915298
-        },
-        {
-            "x": 2.6844159261129263,
-            "y": 6.720055698035347,
-            "heading": -2.7951433359678384,
-            "angularVelocity": -0.25025011328969643,
-            "velocityX": 2.187145331864103,
-            "velocityY": -0.21017723757365894,
-            "timestamp": 0.2636494702937295
-        },
-        {
-            "x": 2.940716876843772,
-            "y": 6.6954319085352205,
-            "heading": -2.8223782521748206,
-            "angularVelocity": -0.3098991571267751,
-            "velocityX": 2.9163830723267137,
-            "velocityY": -0.2801878130765108,
-            "timestamp": 0.35153262705830596
-        },
-        {
-            "x": 3.261123048295386,
-            "y": 6.664666336504878,
-            "heading": -2.850384657388346,
-            "angularVelocity": -0.318677733533739,
-            "velocityX": 3.645820009742318,
-            "velocityY": -0.35007358819345447,
-            "timestamp": 0.43941578382288243
-        },
-        {
-            "x": 3.5920521261120903,
-            "y": 6.632831781454392,
-            "heading": -2.8503846627195863,
-            "angularVelocity": -6.066282061484616e-8,
-            "velocityX": 3.7655574742633027,
-            "velocityY": -0.3622372730165362,
-            "timestamp": 0.527298940587459
-        },
-        {
-            "x": 3.922981203901898,
-            "y": 6.6009972261242424,
-            "heading": -2.850384668050817,
-            "angularVelocity": -6.066271266149535e-8,
-            "velocityX": 3.7655574739572506,
-            "velocityY": -0.3622372761987803,
-            "timestamp": 0.6151820973520354
-        },
-        {
-            "x": 4.253910281691706,
-            "y": 6.56916267079409,
-            "heading": -2.8503846733820475,
-            "angularVelocity": -6.06627128107855e-8,
-            "velocityX": 3.765557473957249,
-            "velocityY": -0.36223727619879537,
-            "timestamp": 0.7030652541166119
-        },
-        {
-            "x": 4.5848393594815136,
-            "y": 6.537328115463938,
-            "heading": -2.850384678713278,
-            "angularVelocity": -6.066271279819625e-8,
-            "velocityX": 3.765557473957249,
-            "velocityY": -0.3622372761987953,
-            "timestamp": 0.7909484108811884
-        },
-        {
-            "x": 4.915768437271321,
-            "y": 6.505493560133786,
-            "heading": -2.850384684044509,
-            "angularVelocity": -6.066271362462515e-8,
-            "velocityX": 3.7655574739572493,
-            "velocityY": -0.3622372761987953,
-            "timestamp": 0.8788315676457649
-        },
-        {
-            "x": 5.24669751506113,
-            "y": 6.473659004803634,
-            "heading": -2.8503846893757396,
-            "angularVelocity": -6.066271352975027e-8,
-            "velocityX": 3.765557473957249,
-            "velocityY": -0.36223727619879525,
-            "timestamp": 0.9667147244103413
-        },
-        {
-            "x": 5.577626592850938,
-            "y": 6.441824449473482,
-            "heading": -2.85038469470697,
-            "angularVelocity": -6.066271274322426e-8,
-            "velocityX": 3.765557473957249,
-            "velocityY": -0.3622372761987953,
-            "timestamp": 1.054597881174918
-        },
-        {
-            "x": 5.908555670640745,
-            "y": 6.40998989414333,
-            "heading": -2.8503847000382008,
-            "angularVelocity": -6.066271341664136e-8,
-            "velocityX": 3.7655574739572493,
-            "velocityY": -0.36223727619879537,
-            "timestamp": 1.1424810379394945
-        },
-        {
-            "x": 6.239484748430477,
-            "y": 6.37815533881239,
-            "heading": -2.8503847053694313,
-            "angularVelocity": -6.066271364618301e-8,
-            "velocityX": 3.7655574739563864,
-            "velocityY": -0.36223727620776275,
-            "timestamp": 1.230364194704071
-        },
-        {
-            "x": 6.5704138100618055,
-            "y": 6.346320615510802,
-            "heading": -2.8503847107006623,
-            "angularVelocity": -6.066271546575477e-8,
-            "velocityX": 3.7655572900940437,
-            "velocityY": -0.3622391875028854,
-            "timestamp": 1.3182473514686477
-        },
-        {
-            "x": 6.897540531836615,
-            "y": 6.287028122680583,
-            "heading": -2.8503847211917432,
-            "angularVelocity": -1.1937532942487347e-7,
-            "velocityX": 3.722291435788134,
-            "velocityY": -0.6746741356714466,
-            "timestamp": 1.4061305082332243
-        },
-        {
-            "x": 7.182619204890035,
-            "y": 6.1828768490539545,
-            "heading": -2.9185193386552397,
-            "angularVelocity": -0.7752864140510717,
-            "velocityX": 3.243837426289712,
-            "velocityY": -1.1851107477355587,
-            "timestamp": 1.4940136649978009
-        },
-        {
-            "x": 7.403317140168432,
-            "y": 6.080132379818418,
-            "heading": -2.984427246863809,
-            "angularVelocity": -0.749949258025913,
-            "velocityX": 2.511265450666594,
-            "velocityY": -1.1691030797945798,
-            "timestamp": 1.5818968217623774
-        },
-        {
-            "x": 7.559743583293149,
-            "y": 5.981103818181206,
-            "heading": -3.044561457345504,
-            "angularVelocity": -0.6842518258963323,
-            "velocityX": 1.7799365530730349,
-            "velocityY": -1.1268207160843378,
-            "timestamp": 1.669779978526954
-        },
-        {
-            "x": 7.6519593983078185,
-            "y": 5.886591133773424,
-            "heading": -3.097689544379845,
-            "angularVelocity": -0.6045309361913614,
-            "velocityX": 1.0493002118904227,
-            "velocityY": -1.0754357022127217,
-            "timestamp": 1.7576631352915306
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.5176846676276772,
-            "velocityX": 0.3190668465323417,
-            "velocityY": -1.0194346342544611,
-            "timestamp": 1.8455462920561072
-        },
-        {
-            "x": 7.628959612668781,
-            "y": 5.701821981186599,
-            "heading": -3.184554593580224,
-            "angularVelocity": -0.41404680845855485,
-            "velocityX": -0.5108405610944724,
-            "velocityY": -0.9525945055820302,
-            "timestamp": 1.9454608078922528
-        },
-        {
-            "x": 7.495051570255288,
-            "y": 5.613939825179433,
-            "heading": -3.215560246622884,
-            "angularVelocity": -0.3103218064280789,
-            "velocityX": -1.3402261052147335,
-            "velocityY": -0.8795734560860885,
-            "timestamp": 2.0453753237283987
-        },
-        {
-            "x": 7.278385319183743,
-            "y": 5.534503531789237,
-            "heading": -3.2361878862694873,
-            "angularVelocity": -0.2064528809850976,
-            "velocityX": -2.1685162486986918,
-            "velocityY": -0.7950425693946834,
-            "timestamp": 2.1452898395645446
-        },
-        {
-            "x": 6.979307104157413,
-            "y": 5.4664019474569185,
-            "heading": -3.246410388494101,
-            "angularVelocity": -0.10231248321693356,
-            "velocityX": -2.9933409827737334,
-            "velocityY": -0.6815985021035508,
-            "timestamp": 2.2452043554006904
-        },
-        {
-            "x": 6.603136214940544,
-            "y": 5.429560638127869,
-            "heading": -3.2464103824694632,
-            "angularVelocity": 6.029792436770947e-8,
-            "velocityX": -3.7649273087983395,
-            "velocityY": -0.368728297592585,
-            "timestamp": 2.3451188712368363
-        },
-        {
-            "x": 6.225751326953569,
-            "y": 5.450595811881877,
-            "heading": -3.2464103781263827,
-            "angularVelocity": 4.346796393735324e-8,
-            "velocityX": -3.777077683145315,
-            "velocityY": 0.2105317088110093,
-            "timestamp": 2.445033387072982
-        },
-        {
-            "x": 5.848366461127548,
-            "y": 5.47163138321431,
-            "heading": -3.2464103737833025,
-            "angularVelocity": 4.346795775537738e-8,
-            "velocityX": -3.777077461346165,
-            "velocityY": 0.21053568799683506,
-            "timestamp": 2.544947902909128
-        },
-        {
-            "x": 5.470981595301597,
-            "y": 5.492666954547974,
-            "heading": -3.246410369440223,
-            "angularVelocity": 4.346795748687552e-8,
-            "velocityX": -3.777077461345479,
-            "velocityY": 0.21053568800914468,
-            "timestamp": 2.644862418745274
-        },
-        {
-            "x": 5.09359672947572,
-            "y": 5.513702525882575,
-            "heading": -3.246410365097125,
-            "angularVelocity": 4.346813479097949e-8,
-            "velocityX": -3.777077461344731,
-            "velocityY": 0.21053568801853617,
-            "timestamp": 2.7447769345814197
-        },
-        {
-            "x": 4.761358057173187,
-            "y": 5.532221526743186,
-            "heading": -3.20512020544725,
-            "angularVelocity": 0.4132548639637959,
-            "velocityX": -3.3252292674608595,
-            "velocityY": 0.18534845218066273,
-            "timestamp": 2.8446914504175655
-        },
-        {
-            "x": 4.5121790336148475,
-            "y": 5.546110766147867,
-            "heading": -3.174152719009984,
-            "angularVelocity": 0.3099398138309664,
-            "velocityX": -2.4939221440754293,
-            "velocityY": 0.13901122663156756,
-            "timestamp": 2.9446059662537114
-        },
-        {
-            "x": 4.346059678819821,
-            "y": 5.555370255329891,
-            "heading": -3.153507786492084,
-            "angularVelocity": 0.20662595765119826,
-            "velocityX": -1.6626148203275395,
-            "velocityY": 0.09267411351128947,
-            "timestamp": 3.0445204820898573
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0.10331310947276948,
-            "velocityX": -0.8313074243990123,
-            "velocityY": 0.04633705754727876,
-            "timestamp": 3.144434997926003
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0,
-            "velocityX": 1.3822008000428574e-34,
-            "velocityY": 0,
-            "timestamp": 3.244349513762149
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S2.traj
deleted file mode 100644
index 84327156..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2 To S2.traj	
+++ /dev/null
@@ -1,328 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.36406618503168,
-            "y": 6.750841975187211,
-            "heading": -2.7578611838826945,
-            "angularVelocity": -0.0894504040603958,
-            "velocityX": 0.7289927602771729,
-            "velocityY": -0.07007059190289512,
-            "timestamp": 0.08788315676457649
-        },
-        {
-            "x": 2.4922026900458016,
-            "y": 6.738526737153379,
-            "heading": -2.773150566031247,
-            "angularVelocity": -0.17397397534899775,
-            "velocityX": 1.458032571103211,
-            "velocityY": -0.1401319489105608,
-            "timestamp": 0.17576631352915298
-        },
-        {
-            "x": 2.6844159261129263,
-            "y": 6.720055698035347,
-            "heading": -2.7951433359678384,
-            "angularVelocity": -0.25025011328969643,
-            "velocityX": 2.187145331864103,
-            "velocityY": -0.21017723757365894,
-            "timestamp": 0.2636494702937295
-        },
-        {
-            "x": 2.940716876843772,
-            "y": 6.6954319085352205,
-            "heading": -2.8223782521748206,
-            "angularVelocity": -0.3098991571267751,
-            "velocityX": 2.9163830723267137,
-            "velocityY": -0.2801878130765108,
-            "timestamp": 0.35153262705830596
-        },
-        {
-            "x": 3.261123048295386,
-            "y": 6.664666336504878,
-            "heading": -2.850384657388346,
-            "angularVelocity": -0.318677733533739,
-            "velocityX": 3.645820009742318,
-            "velocityY": -0.35007358819345447,
-            "timestamp": 0.43941578382288243
-        },
-        {
-            "x": 3.5920521261120903,
-            "y": 6.632831781454392,
-            "heading": -2.8503846627195863,
-            "angularVelocity": -6.066282061484616e-8,
-            "velocityX": 3.7655574742633027,
-            "velocityY": -0.3622372730165362,
-            "timestamp": 0.527298940587459
-        },
-        {
-            "x": 3.922981203901898,
-            "y": 6.6009972261242424,
-            "heading": -2.850384668050817,
-            "angularVelocity": -6.066271266149535e-8,
-            "velocityX": 3.7655574739572506,
-            "velocityY": -0.3622372761987803,
-            "timestamp": 0.6151820973520354
-        },
-        {
-            "x": 4.253910281691706,
-            "y": 6.56916267079409,
-            "heading": -2.8503846733820475,
-            "angularVelocity": -6.06627128107855e-8,
-            "velocityX": 3.765557473957249,
-            "velocityY": -0.36223727619879537,
-            "timestamp": 0.7030652541166119
-        },
-        {
-            "x": 4.5848393594815136,
-            "y": 6.537328115463938,
-            "heading": -2.850384678713278,
-            "angularVelocity": -6.066271279819625e-8,
-            "velocityX": 3.765557473957249,
-            "velocityY": -0.3622372761987953,
-            "timestamp": 0.7909484108811884
-        },
-        {
-            "x": 4.915768437271321,
-            "y": 6.505493560133786,
-            "heading": -2.850384684044509,
-            "angularVelocity": -6.066271362462515e-8,
-            "velocityX": 3.7655574739572493,
-            "velocityY": -0.3622372761987953,
-            "timestamp": 0.8788315676457649
-        },
-        {
-            "x": 5.24669751506113,
-            "y": 6.473659004803634,
-            "heading": -2.8503846893757396,
-            "angularVelocity": -6.066271352975027e-8,
-            "velocityX": 3.765557473957249,
-            "velocityY": -0.36223727619879525,
-            "timestamp": 0.9667147244103413
-        },
-        {
-            "x": 5.577626592850938,
-            "y": 6.441824449473482,
-            "heading": -2.85038469470697,
-            "angularVelocity": -6.066271274322426e-8,
-            "velocityX": 3.765557473957249,
-            "velocityY": -0.3622372761987953,
-            "timestamp": 1.054597881174918
-        },
-        {
-            "x": 5.908555670640745,
-            "y": 6.40998989414333,
-            "heading": -2.8503847000382008,
-            "angularVelocity": -6.066271341664136e-8,
-            "velocityX": 3.7655574739572493,
-            "velocityY": -0.36223727619879537,
-            "timestamp": 1.1424810379394945
-        },
-        {
-            "x": 6.239484748430477,
-            "y": 6.37815533881239,
-            "heading": -2.8503847053694313,
-            "angularVelocity": -6.066271364618301e-8,
-            "velocityX": 3.7655574739563864,
-            "velocityY": -0.36223727620776275,
-            "timestamp": 1.230364194704071
-        },
-        {
-            "x": 6.5704138100618055,
-            "y": 6.346320615510802,
-            "heading": -2.8503847107006623,
-            "angularVelocity": -6.066271546575477e-8,
-            "velocityX": 3.7655572900940437,
-            "velocityY": -0.3622391875028854,
-            "timestamp": 1.3182473514686477
-        },
-        {
-            "x": 6.897540531836615,
-            "y": 6.287028122680583,
-            "heading": -2.8503847211917432,
-            "angularVelocity": -1.1937532942487347e-7,
-            "velocityX": 3.722291435788134,
-            "velocityY": -0.6746741356714466,
-            "timestamp": 1.4061305082332243
-        },
-        {
-            "x": 7.182619204890035,
-            "y": 6.1828768490539545,
-            "heading": -2.9185193386552397,
-            "angularVelocity": -0.7752864140510717,
-            "velocityX": 3.243837426289712,
-            "velocityY": -1.1851107477355587,
-            "timestamp": 1.4940136649978009
-        },
-        {
-            "x": 7.403317140168432,
-            "y": 6.080132379818418,
-            "heading": -2.984427246863809,
-            "angularVelocity": -0.749949258025913,
-            "velocityX": 2.511265450666594,
-            "velocityY": -1.1691030797945798,
-            "timestamp": 1.5818968217623774
-        },
-        {
-            "x": 7.559743583293149,
-            "y": 5.981103818181206,
-            "heading": -3.044561457345504,
-            "angularVelocity": -0.6842518258963323,
-            "velocityX": 1.7799365530730349,
-            "velocityY": -1.1268207160843378,
-            "timestamp": 1.669779978526954
-        },
-        {
-            "x": 7.6519593983078185,
-            "y": 5.886591133773424,
-            "heading": -3.097689544379845,
-            "angularVelocity": -0.6045309361913614,
-            "velocityX": 1.0493002118904227,
-            "velocityY": -1.0754357022127217,
-            "timestamp": 1.7576631352915306
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.5176846676276772,
-            "velocityX": 0.3190668465323417,
-            "velocityY": -1.0194346342544611,
-            "timestamp": 1.8455462920561072
-        },
-        {
-            "x": 7.628959612668781,
-            "y": 5.701821981186599,
-            "heading": -3.184554593580224,
-            "angularVelocity": -0.41404680845855485,
-            "velocityX": -0.5108405610944724,
-            "velocityY": -0.9525945055820302,
-            "timestamp": 1.9454608078922528
-        },
-        {
-            "x": 7.495051570255288,
-            "y": 5.613939825179433,
-            "heading": -3.215560246622884,
-            "angularVelocity": -0.3103218064280789,
-            "velocityX": -1.3402261052147335,
-            "velocityY": -0.8795734560860885,
-            "timestamp": 2.0453753237283987
-        },
-        {
-            "x": 7.278385319183743,
-            "y": 5.534503531789237,
-            "heading": -3.2361878862694873,
-            "angularVelocity": -0.2064528809850976,
-            "velocityX": -2.1685162486986918,
-            "velocityY": -0.7950425693946834,
-            "timestamp": 2.1452898395645446
-        },
-        {
-            "x": 6.979307104157413,
-            "y": 5.4664019474569185,
-            "heading": -3.246410388494101,
-            "angularVelocity": -0.10231248321693356,
-            "velocityX": -2.9933409827737334,
-            "velocityY": -0.6815985021035508,
-            "timestamp": 2.2452043554006904
-        },
-        {
-            "x": 6.603136214940544,
-            "y": 5.429560638127869,
-            "heading": -3.2464103824694632,
-            "angularVelocity": 6.029792436770947e-8,
-            "velocityX": -3.7649273087983395,
-            "velocityY": -0.368728297592585,
-            "timestamp": 2.3451188712368363
-        },
-        {
-            "x": 6.225751326953569,
-            "y": 5.450595811881877,
-            "heading": -3.2464103781263827,
-            "angularVelocity": 4.346796393735324e-8,
-            "velocityX": -3.777077683145315,
-            "velocityY": 0.2105317088110093,
-            "timestamp": 2.445033387072982
-        },
-        {
-            "x": 5.848366461127548,
-            "y": 5.47163138321431,
-            "heading": -3.2464103737833025,
-            "angularVelocity": 4.346795775537738e-8,
-            "velocityX": -3.777077461346165,
-            "velocityY": 0.21053568799683506,
-            "timestamp": 2.544947902909128
-        },
-        {
-            "x": 5.470981595301597,
-            "y": 5.492666954547974,
-            "heading": -3.246410369440223,
-            "angularVelocity": 4.346795748687552e-8,
-            "velocityX": -3.777077461345479,
-            "velocityY": 0.21053568800914468,
-            "timestamp": 2.644862418745274
-        },
-        {
-            "x": 5.09359672947572,
-            "y": 5.513702525882575,
-            "heading": -3.246410365097125,
-            "angularVelocity": 4.346813479097949e-8,
-            "velocityX": -3.777077461344731,
-            "velocityY": 0.21053568801853617,
-            "timestamp": 2.7447769345814197
-        },
-        {
-            "x": 4.761358057173187,
-            "y": 5.532221526743186,
-            "heading": -3.20512020544725,
-            "angularVelocity": 0.4132548639637959,
-            "velocityX": -3.3252292674608595,
-            "velocityY": 0.18534845218066273,
-            "timestamp": 2.8446914504175655
-        },
-        {
-            "x": 4.5121790336148475,
-            "y": 5.546110766147867,
-            "heading": -3.174152719009984,
-            "angularVelocity": 0.3099398138309664,
-            "velocityX": -2.4939221440754293,
-            "velocityY": 0.13901122663156756,
-            "timestamp": 2.9446059662537114
-        },
-        {
-            "x": 4.346059678819821,
-            "y": 5.555370255329891,
-            "heading": -3.153507786492084,
-            "angularVelocity": 0.20662595765119826,
-            "velocityX": -1.6626148203275395,
-            "velocityY": 0.09267411351128947,
-            "timestamp": 3.0445204820898573
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0.10331310947276948,
-            "velocityX": -0.8313074243990123,
-            "velocityY": 0.04633705754727876,
-            "timestamp": 3.144434997926003
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0,
-            "velocityX": 1.3822008000428574e-34,
-            "velocityY": 0,
-            "timestamp": 3.244349513762149
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S1.1.traj
deleted file mode 100644
index 6ed764e0..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S1.1.traj	
+++ /dev/null
@@ -1,364 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.352643709641578,
-            "y": 6.724306544750454,
-            "heading": -2.7643107246230096,
-            "angularVelocity": -0.16582604780361404,
-            "velocityX": 0.6100109212961704,
-            "velocityY": -0.37883661491424325,
-            "timestamp": 0.08629961825889286
-        },
-        {
-            "x": 2.457948690141224,
-            "y": 6.658909154192408,
-            "heading": -2.791757157392081,
-            "angularVelocity": -0.31803654897277683,
-            "velocityX": 1.2202253338341789,
-            "velocityY": -0.7577946678969769,
-            "timestamp": 0.17259923651778572
-        },
-        {
-            "x": 2.6159410271495345,
-            "y": 6.560792064019538,
-            "heading": -2.830360502782766,
-            "angularVelocity": -0.44731768423791834,
-            "velocityX": 1.8307420148076843,
-            "velocityY": -1.136935390358925,
-            "timestamp": 0.25889885477667857
-        },
-        {
-            "x": 2.826660648973102,
-            "y": 6.4299308538855104,
-            "heading": -2.87607731089876,
-            "angularVelocity": -0.5297451951435223,
-            "velocityX": 2.4417213665020787,
-            "velocityY": -1.516359084482017,
-            "timestamp": 0.34519847303557144
-        },
-        {
-            "x": 3.0901174136120235,
-            "y": 6.266320048368765,
-            "heading": -2.9160689628763303,
-            "angularVelocity": -0.46340473786276914,
-            "velocityX": 3.052814948135086,
-            "velocityY": -1.8958462252537605,
-            "timestamp": 0.4314980912944643
-        },
-        {
-            "x": 3.3674552739873977,
-            "y": 6.094087733168023,
-            "heading": -2.916068973852836,
-            "angularVelocity": -1.2719066550138966e-7,
-            "velocityX": 3.213662655416737,
-            "velocityY": -1.995748285747182,
-            "timestamp": 0.5177977095533571
-        },
-        {
-            "x": 3.6447931343598987,
-            "y": 5.921855417962633,
-            "heading": -2.9160689848293226,
-            "angularVelocity": -1.2719044142347595e-7,
-            "velocityX": 3.213662655383448,
-            "velocityY": -1.9957482858010414,
-            "timestamp": 0.60409732781225
-        },
-        {
-            "x": 3.9221309947324,
-            "y": 5.749623102757243,
-            "heading": -2.9160689958058095,
-            "angularVelocity": -1.2719044074317096e-7,
-            "velocityX": 3.2136626553834478,
-            "velocityY": -1.9957482858010414,
-            "timestamp": 0.6903969460711429
-        },
-        {
-            "x": 4.199468855104901,
-            "y": 5.577390787551853,
-            "heading": -2.916069006782296,
-            "angularVelocity": -1.2719044064772273e-7,
-            "velocityX": 3.213662655383448,
-            "velocityY": -1.995748285801041,
-            "timestamp": 0.7766965643300358
-        },
-        {
-            "x": 4.476806715477402,
-            "y": 5.405158472346463,
-            "heading": -2.9160690177587827,
-            "angularVelocity": -1.271904417367868e-7,
-            "velocityX": 3.213662655383448,
-            "velocityY": -1.9957482858010416,
-            "timestamp": 0.8629961825889286
-        },
-        {
-            "x": 4.754144575849903,
-            "y": 5.2329261571410735,
-            "heading": -2.9160690287352695,
-            "angularVelocity": -1.2719044128845816e-7,
-            "velocityX": 3.2136626553834478,
-            "velocityY": -1.9957482858010416,
-            "timestamp": 0.9492958008478215
-        },
-        {
-            "x": 5.031482436222404,
-            "y": 5.0606938419356835,
-            "heading": -2.9160690397117563,
-            "angularVelocity": -1.271904407298397e-7,
-            "velocityX": 3.2136626553834486,
-            "velocityY": -1.995748285801041,
-            "timestamp": 1.0355954191067143
-        },
-        {
-            "x": 5.308820296594906,
-            "y": 4.8884615267302935,
-            "heading": -2.9160690506882427,
-            "angularVelocity": -1.2719044158933254e-7,
-            "velocityX": 3.2136626553834478,
-            "velocityY": -1.995748285801042,
-            "timestamp": 1.1218950373656071
-        },
-        {
-            "x": 5.586158156967406,
-            "y": 4.716229211524904,
-            "heading": -2.9160690616647296,
-            "angularVelocity": -1.271904410743158e-7,
-            "velocityX": 3.2136626553834478,
-            "velocityY": -1.995748285801042,
-            "timestamp": 1.2081946556245
-        },
-        {
-            "x": 5.863496017339908,
-            "y": 4.543996896319514,
-            "heading": -2.9160690726412164,
-            "angularVelocity": -1.2719044055646472e-7,
-            "velocityX": 3.213662655383448,
-            "velocityY": -1.9957482858010414,
-            "timestamp": 1.2944942738833929
-        },
-        {
-            "x": 6.140833877716964,
-            "y": 4.371764581121458,
-            "heading": -2.916069083617703,
-            "angularVelocity": -1.2719044131717392e-7,
-            "velocityX": 3.213662655436225,
-            "velocityY": -1.9957482857160567,
-            "timestamp": 1.3807938921422858
-        },
-        {
-            "x": 6.418172664892274,
-            "y": 4.19953375831344,
-            "heading": -2.9160690945941994,
-            "angularVelocity": -1.2719055821939482e-7,
-            "velocityX": 3.21367339474496,
-            "velocityY": -1.9957309925918116,
-            "timestamp": 1.4670935104011786
-        },
-        {
-            "x": 6.7207652632777295,
-            "y": 4.0769859125864,
-            "heading": -2.916069118255731,
-            "angularVelocity": -2.741788669167458e-7,
-            "velocityX": 3.5063028607839835,
-            "velocityY": -1.4200276687058784,
-            "timestamp": 1.5533931286600715
-        },
-        {
-            "x": 7.011920641126383,
-            "y": 4.012803875379112,
-            "heading": -2.9821933696709393,
-            "angularVelocity": -0.7662171948025339,
-            "velocityX": 3.3737736472290445,
-            "velocityY": -0.7437117162469018,
-            "timestamp": 1.6396927469189644
-        },
-        {
-            "x": 7.255025953055537,
-            "y": 3.987925238494502,
-            "heading": -3.0423989255025554,
-            "angularVelocity": -0.6976340920894686,
-            "velocityX": 2.8169917414808885,
-            "velocityY": -0.28828211973553797,
-            "timestamp": 1.7259923651778573
-        },
-        {
-            "x": 7.447850515704627,
-            "y": 3.9993347769870677,
-            "heading": -3.0905963113163932,
-            "angularVelocity": -0.5584889804459283,
-            "velocityX": 2.234361710265075,
-            "velocityY": 0.13220844684348906,
-            "timestamp": 1.8122919834367501
-        },
-        {
-            "x": 7.5896354595479805,
-            "y": 4.045892337030357,
-            "heading": -3.124573318920613,
-            "angularVelocity": -0.3937098250312271,
-            "velocityX": 1.6429382505152235,
-            "velocityY": 0.539487439032544,
-            "timestamp": 1.898591601695643
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.21566709836621822,
-            "velocityX": 1.047102435377472,
-            "velocityY": 0.9398380271749195,
-            "timestamp": 1.9848912199545359
-        },
-        {
-            "x": 7.718344137255122,
-            "y": 4.246200601691832,
-            "heading": -3.145373267573727,
-            "angularVelocity": -0.024710376357260012,
-            "velocityX": 0.43305082947346774,
-            "velocityY": 1.34622717133274,
-            "timestamp": 2.073435415339313
-        },
-        {
-            "x": 7.702075707426083,
-            "y": 4.401035710087067,
-            "heading": -3.1311779977271272,
-            "angularVelocity": 0.16031846906115368,
-            "velocityX": -0.183732313080062,
-            "velocityY": 1.7486759885425491,
-            "timestamp": 2.16197961072409
-        },
-        {
-            "x": 7.630801467153219,
-            "y": 4.590926342055076,
-            "heading": -3.10146947436901,
-            "angularVelocity": 0.33552197554576096,
-            "velocityX": -0.804956665562769,
-            "velocityY": 2.1445858889079887,
-            "timestamp": 2.250523806108867
-        },
-        {
-            "x": 7.503771169060967,
-            "y": 4.814724539953898,
-            "heading": -3.057954484569731,
-            "angularVelocity": 0.49144937855435067,
-            "velocityX": -1.4346541581807906,
-            "velocityY": 2.527530990879987,
-            "timestamp": 2.339068001493644
-        },
-        {
-            "x": 7.31900648308238,
-            "y": 5.069082680437073,
-            "heading": -3.0055052448244375,
-            "angularVelocity": 0.5923509667139643,
-            "velocityX": -2.086694505218799,
-            "velocityY": 2.872668720699063,
-            "timestamp": 2.427612196878421
-        },
-        {
-            "x": 7.075378945667608,
-            "y": 5.298956768781039,
-            "heading": -3.0055052272028773,
-            "angularVelocity": 1.990142925919268e-7,
-            "velocityX": -2.75147948837462,
-            "velocityY": 2.5961508526209696,
-            "timestamp": 2.516156392263198
-        },
-        {
-            "x": 6.801072699384408,
-            "y": 5.491187253021094,
-            "heading": -3.005505216579771,
-            "angularVelocity": 1.1997518482429124e-7,
-            "velocityX": -3.097958540262374,
-            "velocityY": 2.1710117010503502,
-            "timestamp": 2.604700587647975
-        },
-        {
-            "x": 6.5267661173867575,
-            "y": 5.683417258207369,
-            "heading": -3.005505205956667,
-            "angularVelocity": 1.199751543060781e-7,
-            "velocityX": -3.0979623317525853,
-            "velocityY": 2.1710062907151193,
-            "timestamp": 2.693244783032752
-        },
-        {
-            "x": 6.252459535387558,
-            "y": 5.875647263391433,
-            "heading": -3.005505195333564,
-            "angularVelocity": 1.199751539565171e-7,
-            "velocityX": -3.0979623317700846,
-            "velocityY": 2.1710062906901486,
-            "timestamp": 2.781788978417529
-        },
-        {
-            "x": 5.978152953372341,
-            "y": 6.067877268552632,
-            "heading": -3.0055051847104526,
-            "angularVelocity": 1.1997524424086504e-7,
-            "velocityX": -3.0979623319509813,
-            "velocityY": 2.171006290431916,
-            "timestamp": 2.870333173802306
-        },
-        {
-            "x": 5.71076650313837,
-            "y": 6.255250599668089,
-            "heading": -2.984811425443856,
-            "angularVelocity": 0.23371107700143318,
-            "velocityX": -3.0198077815501345,
-            "velocityY": 2.1161560088926668,
-            "timestamp": 2.958877369187083
-        },
-        {
-            "x": 5.496808427326641,
-            "y": 6.405185542172583,
-            "heading": -2.9493217674847187,
-            "angularVelocity": 0.40081292520855316,
-            "velocityX": -2.4163986682814493,
-            "velocityY": 1.6933345190325848,
-            "timestamp": 3.04742156457186
-        },
-        {
-            "x": 5.33638152944714,
-            "y": 6.517608311818503,
-            "heading": -2.9168038375204985,
-            "angularVelocity": 0.3672508381444,
-            "velocityX": -1.811828513234718,
-            "velocityY": 1.269679724997829,
-            "timestamp": 3.135965759956637
-        },
-        {
-            "x": 5.229454180923455,
-            "y": 6.592540440066129,
-            "heading": -2.8927984833645977,
-            "angularVelocity": 0.27111155118061186,
-            "velocityX": -1.2076155648493634,
-            "velocityY": 0.8462681028637682,
-            "timestamp": 3.224509955341414
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0.144543448733038,
-            "velocityX": -0.6037005666060847,
-            "velocityY": 0.4230605944433498,
-            "timestamp": 3.313054150726191
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0,
-            "velocityX": 1.048960687291352e-31,
-            "velocityY": 0,
-            "timestamp": 3.401598346110968
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S1.traj
deleted file mode 100644
index 6ed764e0..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S1.traj	
+++ /dev/null
@@ -1,364 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.352643709641578,
-            "y": 6.724306544750454,
-            "heading": -2.7643107246230096,
-            "angularVelocity": -0.16582604780361404,
-            "velocityX": 0.6100109212961704,
-            "velocityY": -0.37883661491424325,
-            "timestamp": 0.08629961825889286
-        },
-        {
-            "x": 2.457948690141224,
-            "y": 6.658909154192408,
-            "heading": -2.791757157392081,
-            "angularVelocity": -0.31803654897277683,
-            "velocityX": 1.2202253338341789,
-            "velocityY": -0.7577946678969769,
-            "timestamp": 0.17259923651778572
-        },
-        {
-            "x": 2.6159410271495345,
-            "y": 6.560792064019538,
-            "heading": -2.830360502782766,
-            "angularVelocity": -0.44731768423791834,
-            "velocityX": 1.8307420148076843,
-            "velocityY": -1.136935390358925,
-            "timestamp": 0.25889885477667857
-        },
-        {
-            "x": 2.826660648973102,
-            "y": 6.4299308538855104,
-            "heading": -2.87607731089876,
-            "angularVelocity": -0.5297451951435223,
-            "velocityX": 2.4417213665020787,
-            "velocityY": -1.516359084482017,
-            "timestamp": 0.34519847303557144
-        },
-        {
-            "x": 3.0901174136120235,
-            "y": 6.266320048368765,
-            "heading": -2.9160689628763303,
-            "angularVelocity": -0.46340473786276914,
-            "velocityX": 3.052814948135086,
-            "velocityY": -1.8958462252537605,
-            "timestamp": 0.4314980912944643
-        },
-        {
-            "x": 3.3674552739873977,
-            "y": 6.094087733168023,
-            "heading": -2.916068973852836,
-            "angularVelocity": -1.2719066550138966e-7,
-            "velocityX": 3.213662655416737,
-            "velocityY": -1.995748285747182,
-            "timestamp": 0.5177977095533571
-        },
-        {
-            "x": 3.6447931343598987,
-            "y": 5.921855417962633,
-            "heading": -2.9160689848293226,
-            "angularVelocity": -1.2719044142347595e-7,
-            "velocityX": 3.213662655383448,
-            "velocityY": -1.9957482858010414,
-            "timestamp": 0.60409732781225
-        },
-        {
-            "x": 3.9221309947324,
-            "y": 5.749623102757243,
-            "heading": -2.9160689958058095,
-            "angularVelocity": -1.2719044074317096e-7,
-            "velocityX": 3.2136626553834478,
-            "velocityY": -1.9957482858010414,
-            "timestamp": 0.6903969460711429
-        },
-        {
-            "x": 4.199468855104901,
-            "y": 5.577390787551853,
-            "heading": -2.916069006782296,
-            "angularVelocity": -1.2719044064772273e-7,
-            "velocityX": 3.213662655383448,
-            "velocityY": -1.995748285801041,
-            "timestamp": 0.7766965643300358
-        },
-        {
-            "x": 4.476806715477402,
-            "y": 5.405158472346463,
-            "heading": -2.9160690177587827,
-            "angularVelocity": -1.271904417367868e-7,
-            "velocityX": 3.213662655383448,
-            "velocityY": -1.9957482858010416,
-            "timestamp": 0.8629961825889286
-        },
-        {
-            "x": 4.754144575849903,
-            "y": 5.2329261571410735,
-            "heading": -2.9160690287352695,
-            "angularVelocity": -1.2719044128845816e-7,
-            "velocityX": 3.2136626553834478,
-            "velocityY": -1.9957482858010416,
-            "timestamp": 0.9492958008478215
-        },
-        {
-            "x": 5.031482436222404,
-            "y": 5.0606938419356835,
-            "heading": -2.9160690397117563,
-            "angularVelocity": -1.271904407298397e-7,
-            "velocityX": 3.2136626553834486,
-            "velocityY": -1.995748285801041,
-            "timestamp": 1.0355954191067143
-        },
-        {
-            "x": 5.308820296594906,
-            "y": 4.8884615267302935,
-            "heading": -2.9160690506882427,
-            "angularVelocity": -1.2719044158933254e-7,
-            "velocityX": 3.2136626553834478,
-            "velocityY": -1.995748285801042,
-            "timestamp": 1.1218950373656071
-        },
-        {
-            "x": 5.586158156967406,
-            "y": 4.716229211524904,
-            "heading": -2.9160690616647296,
-            "angularVelocity": -1.271904410743158e-7,
-            "velocityX": 3.2136626553834478,
-            "velocityY": -1.995748285801042,
-            "timestamp": 1.2081946556245
-        },
-        {
-            "x": 5.863496017339908,
-            "y": 4.543996896319514,
-            "heading": -2.9160690726412164,
-            "angularVelocity": -1.2719044055646472e-7,
-            "velocityX": 3.213662655383448,
-            "velocityY": -1.9957482858010414,
-            "timestamp": 1.2944942738833929
-        },
-        {
-            "x": 6.140833877716964,
-            "y": 4.371764581121458,
-            "heading": -2.916069083617703,
-            "angularVelocity": -1.2719044131717392e-7,
-            "velocityX": 3.213662655436225,
-            "velocityY": -1.9957482857160567,
-            "timestamp": 1.3807938921422858
-        },
-        {
-            "x": 6.418172664892274,
-            "y": 4.19953375831344,
-            "heading": -2.9160690945941994,
-            "angularVelocity": -1.2719055821939482e-7,
-            "velocityX": 3.21367339474496,
-            "velocityY": -1.9957309925918116,
-            "timestamp": 1.4670935104011786
-        },
-        {
-            "x": 6.7207652632777295,
-            "y": 4.0769859125864,
-            "heading": -2.916069118255731,
-            "angularVelocity": -2.741788669167458e-7,
-            "velocityX": 3.5063028607839835,
-            "velocityY": -1.4200276687058784,
-            "timestamp": 1.5533931286600715
-        },
-        {
-            "x": 7.011920641126383,
-            "y": 4.012803875379112,
-            "heading": -2.9821933696709393,
-            "angularVelocity": -0.7662171948025339,
-            "velocityX": 3.3737736472290445,
-            "velocityY": -0.7437117162469018,
-            "timestamp": 1.6396927469189644
-        },
-        {
-            "x": 7.255025953055537,
-            "y": 3.987925238494502,
-            "heading": -3.0423989255025554,
-            "angularVelocity": -0.6976340920894686,
-            "velocityX": 2.8169917414808885,
-            "velocityY": -0.28828211973553797,
-            "timestamp": 1.7259923651778573
-        },
-        {
-            "x": 7.447850515704627,
-            "y": 3.9993347769870677,
-            "heading": -3.0905963113163932,
-            "angularVelocity": -0.5584889804459283,
-            "velocityX": 2.234361710265075,
-            "velocityY": 0.13220844684348906,
-            "timestamp": 1.8122919834367501
-        },
-        {
-            "x": 7.5896354595479805,
-            "y": 4.045892337030357,
-            "heading": -3.124573318920613,
-            "angularVelocity": -0.3937098250312271,
-            "velocityX": 1.6429382505152235,
-            "velocityY": 0.539487439032544,
-            "timestamp": 1.898591601695643
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.21566709836621822,
-            "velocityX": 1.047102435377472,
-            "velocityY": 0.9398380271749195,
-            "timestamp": 1.9848912199545359
-        },
-        {
-            "x": 7.718344137255122,
-            "y": 4.246200601691832,
-            "heading": -3.145373267573727,
-            "angularVelocity": -0.024710376357260012,
-            "velocityX": 0.43305082947346774,
-            "velocityY": 1.34622717133274,
-            "timestamp": 2.073435415339313
-        },
-        {
-            "x": 7.702075707426083,
-            "y": 4.401035710087067,
-            "heading": -3.1311779977271272,
-            "angularVelocity": 0.16031846906115368,
-            "velocityX": -0.183732313080062,
-            "velocityY": 1.7486759885425491,
-            "timestamp": 2.16197961072409
-        },
-        {
-            "x": 7.630801467153219,
-            "y": 4.590926342055076,
-            "heading": -3.10146947436901,
-            "angularVelocity": 0.33552197554576096,
-            "velocityX": -0.804956665562769,
-            "velocityY": 2.1445858889079887,
-            "timestamp": 2.250523806108867
-        },
-        {
-            "x": 7.503771169060967,
-            "y": 4.814724539953898,
-            "heading": -3.057954484569731,
-            "angularVelocity": 0.49144937855435067,
-            "velocityX": -1.4346541581807906,
-            "velocityY": 2.527530990879987,
-            "timestamp": 2.339068001493644
-        },
-        {
-            "x": 7.31900648308238,
-            "y": 5.069082680437073,
-            "heading": -3.0055052448244375,
-            "angularVelocity": 0.5923509667139643,
-            "velocityX": -2.086694505218799,
-            "velocityY": 2.872668720699063,
-            "timestamp": 2.427612196878421
-        },
-        {
-            "x": 7.075378945667608,
-            "y": 5.298956768781039,
-            "heading": -3.0055052272028773,
-            "angularVelocity": 1.990142925919268e-7,
-            "velocityX": -2.75147948837462,
-            "velocityY": 2.5961508526209696,
-            "timestamp": 2.516156392263198
-        },
-        {
-            "x": 6.801072699384408,
-            "y": 5.491187253021094,
-            "heading": -3.005505216579771,
-            "angularVelocity": 1.1997518482429124e-7,
-            "velocityX": -3.097958540262374,
-            "velocityY": 2.1710117010503502,
-            "timestamp": 2.604700587647975
-        },
-        {
-            "x": 6.5267661173867575,
-            "y": 5.683417258207369,
-            "heading": -3.005505205956667,
-            "angularVelocity": 1.199751543060781e-7,
-            "velocityX": -3.0979623317525853,
-            "velocityY": 2.1710062907151193,
-            "timestamp": 2.693244783032752
-        },
-        {
-            "x": 6.252459535387558,
-            "y": 5.875647263391433,
-            "heading": -3.005505195333564,
-            "angularVelocity": 1.199751539565171e-7,
-            "velocityX": -3.0979623317700846,
-            "velocityY": 2.1710062906901486,
-            "timestamp": 2.781788978417529
-        },
-        {
-            "x": 5.978152953372341,
-            "y": 6.067877268552632,
-            "heading": -3.0055051847104526,
-            "angularVelocity": 1.1997524424086504e-7,
-            "velocityX": -3.0979623319509813,
-            "velocityY": 2.171006290431916,
-            "timestamp": 2.870333173802306
-        },
-        {
-            "x": 5.71076650313837,
-            "y": 6.255250599668089,
-            "heading": -2.984811425443856,
-            "angularVelocity": 0.23371107700143318,
-            "velocityX": -3.0198077815501345,
-            "velocityY": 2.1161560088926668,
-            "timestamp": 2.958877369187083
-        },
-        {
-            "x": 5.496808427326641,
-            "y": 6.405185542172583,
-            "heading": -2.9493217674847187,
-            "angularVelocity": 0.40081292520855316,
-            "velocityX": -2.4163986682814493,
-            "velocityY": 1.6933345190325848,
-            "timestamp": 3.04742156457186
-        },
-        {
-            "x": 5.33638152944714,
-            "y": 6.517608311818503,
-            "heading": -2.9168038375204985,
-            "angularVelocity": 0.3672508381444,
-            "velocityX": -1.811828513234718,
-            "velocityY": 1.269679724997829,
-            "timestamp": 3.135965759956637
-        },
-        {
-            "x": 5.229454180923455,
-            "y": 6.592540440066129,
-            "heading": -2.8927984833645977,
-            "angularVelocity": 0.27111155118061186,
-            "velocityX": -1.2076155648493634,
-            "velocityY": 0.8462681028637682,
-            "timestamp": 3.224509955341414
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0.144543448733038,
-            "velocityX": -0.6037005666060847,
-            "velocityY": 0.4230605944433498,
-            "timestamp": 3.313054150726191
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 0,
-            "velocityX": 1.048960687291352e-31,
-            "velocityY": 0,
-            "timestamp": 3.401598346110968
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S2.1.traj
deleted file mode 100644
index 9420bca5..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S2.1.traj	
+++ /dev/null
@@ -1,355 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.357227124880225,
-            "y": 6.730410634555629,
-            "heading": -2.7571910501011128,
-            "angularVelocity": -0.08264039553460875,
-            "velocityX": 0.6576608657862654,
-            "velocityY": -0.30556812237992936,
-            "timestamp": 0.08701616267193764
-        },
-        {
-            "x": 2.4716821810223912,
-            "y": 6.677231589623697,
-            "heading": -2.771475854118176,
-            "angularVelocity": -0.1641626518388192,
-            "velocityX": 1.3153309986062809,
-            "velocityY": -0.6111398537812316,
-            "timestamp": 0.17403232534387528
-        },
-        {
-            "x": 2.6433664887429877,
-            "y": 6.597462350883563,
-            "heading": -2.792692156200411,
-            "angularVelocity": -0.2438202447770986,
-            "velocityX": 1.973016304659045,
-            "velocityY": -0.916717495815976,
-            "timestamp": 0.26104848801581293
-        },
-        {
-            "x": 2.8722826002398385,
-            "y": 6.491101923690649,
-            "heading": -2.820514979829771,
-            "angularVelocity": -0.319743169257603,
-            "velocityX": 2.630730940870083,
-            "velocityY": -1.222306568423469,
-            "timestamp": 0.34806465068775055
-        },
-        {
-            "x": 3.1584374767810863,
-            "y": 6.358147629132494,
-            "heading": -2.8539648898748324,
-            "angularVelocity": -0.38441030973950396,
-            "velocityX": 3.288525576795303,
-            "velocityY": -1.5279264274087752,
-            "timestamp": 0.4350808133596882
-        },
-        {
-            "x": 3.456964402226087,
-            "y": 6.219442363897474,
-            "heading": -2.853964894160219,
-            "angularVelocity": -4.924816792193532e-8,
-            "velocityX": 3.4307066213720128,
-            "velocityY": -1.5940172604250262,
-            "timestamp": 0.5220969760316259
-        },
-        {
-            "x": 3.755491327665543,
-            "y": 6.0807370986505,
-            "heading": -2.853964898445594,
-            "angularVelocity": -4.924803343036069e-8,
-            "velocityX": 3.4307066213082953,
-            "velocityY": -1.5940172605623917,
-            "timestamp": 0.6091131387035635
-        },
-        {
-            "x": 4.054018253104999,
-            "y": 5.942031833403527,
-            "heading": -2.853964902730969,
-            "angularVelocity": -4.924803431145499e-8,
-            "velocityX": 3.4307066213082953,
-            "velocityY": -1.594017260562392,
-            "timestamp": 0.6961293013755012
-        },
-        {
-            "x": 4.352545178544456,
-            "y": 5.803326568156554,
-            "heading": -2.8539649070163438,
-            "angularVelocity": -4.9248033877501264e-8,
-            "velocityX": 3.4307066213082953,
-            "velocityY": -1.5940172605623917,
-            "timestamp": 0.7831454640474389
-        },
-        {
-            "x": 4.651072103983911,
-            "y": 5.66462130290958,
-            "heading": -2.8539649113017185,
-            "angularVelocity": -4.924803325887112e-8,
-            "velocityX": 3.430706621308295,
-            "velocityY": -1.5940172605623921,
-            "timestamp": 0.8701616267193766
-        },
-        {
-            "x": 4.949599029423368,
-            "y": 5.525916037662607,
-            "heading": -2.853964915587093,
-            "angularVelocity": -4.924803367441165e-8,
-            "velocityX": 3.430706621308295,
-            "velocityY": -1.5940172605623926,
-            "timestamp": 0.9571777893913143
-        },
-        {
-            "x": 5.248125954862823,
-            "y": 5.387210772415633,
-            "heading": -2.853964919872468,
-            "angularVelocity": -4.924803391655779e-8,
-            "velocityX": 3.4307066213082953,
-            "velocityY": -1.594017260562392,
-            "timestamp": 1.044193952063252
-        },
-        {
-            "x": 5.54665288030228,
-            "y": 5.24850550716866,
-            "heading": -2.8539649241578426,
-            "angularVelocity": -4.9248034279126105e-8,
-            "velocityX": 3.4307066213082953,
-            "velocityY": -1.5940172605623923,
-            "timestamp": 1.1312101147351896
-        },
-        {
-            "x": 5.8451798057417355,
-            "y": 5.109800241921686,
-            "heading": -2.8539649284432174,
-            "angularVelocity": -4.9248034369336404e-8,
-            "velocityX": 3.4307066213082953,
-            "velocityY": -1.594017260562393,
-            "timestamp": 1.2182262774071273
-        },
-        {
-            "x": 6.143706731181192,
-            "y": 4.9710949766747135,
-            "heading": -2.853964932728592,
-            "angularVelocity": -4.9248033497376594e-8,
-            "velocityX": 3.4307066213082953,
-            "velocityY": -1.594017260562392,
-            "timestamp": 1.305242440079065
-        },
-        {
-            "x": 6.4422336566206475,
-            "y": 4.83238971142774,
-            "heading": -2.8539649370139673,
-            "angularVelocity": -4.924803317735332e-8,
-            "velocityX": 3.4307066213082953,
-            "velocityY": -1.5940172605623926,
-            "timestamp": 1.3922586027510027
-        },
-        {
-            "x": 6.740760582057679,
-            "y": 4.693684446175547,
-            "heading": -2.853964941299342,
-            "angularVelocity": -4.9248033793238546e-8,
-            "velocityX": 3.4307066212804305,
-            "velocityY": -1.5940172606223628,
-            "timestamp": 1.4792747654229403
-        },
-        {
-            "x": 7.039286998892109,
-            "y": 4.554978086294829,
-            "heading": -2.8539649455851244,
-            "angularVelocity": -4.925271638681629e-8,
-            "velocityX": 3.4307007763593735,
-            "velocityY": -1.59402984022244,
-            "timestamp": 1.566290928094878
-        },
-        {
-            "x": 7.281877390378217,
-            "y": 4.416206845653086,
-            "heading": -2.926969050497618,
-            "angularVelocity": -0.8389717803085356,
-            "velocityX": 2.7878773786049877,
-            "velocityY": -1.594775457577107,
-            "timestamp": 1.6533070907668157
-        },
-        {
-            "x": 7.4672438532140255,
-            "y": 4.304023352017119,
-            "heading": -2.9923953501231186,
-            "angularVelocity": -0.7518867485821702,
-            "velocityX": 2.130253244269864,
-            "velocityY": -1.2892259344843033,
-            "timestamp": 1.7403232534387534
-        },
-        {
-            "x": 7.595386429544574,
-            "y": 4.218427699418672,
-            "heading": -3.050241491364391,
-            "angularVelocity": -0.6647746747849547,
-            "velocityX": 1.4726295942704664,
-            "velocityY": -0.9836753307676307,
-            "timestamp": 1.827339416110691
-        },
-        {
-            "x": 7.666305140070344,
-            "y": 4.159419912093254,
-            "heading": -3.100505408798514,
-            "angularVelocity": -0.5776388649040337,
-            "velocityX": 0.8150061821634523,
-            "velocityY": -0.6781244485335891,
-            "timestamp": 1.9143555787826287
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.4904824238455614,
-            "velocityX": 0.15738294483620113,
-            "velocityY": -0.37257345184803875,
-            "timestamp": 2.0013717414545664
-        },
-        {
-            "x": 7.62294760973909,
-            "y": 4.124186267487049,
-            "heading": -3.1816074637236196,
-            "angularVelocity": -0.39238397540812464,
-            "velocityX": -0.5826441228371791,
-            "velocityY": -0.02873507497949264,
-            "timestamp": 2.099291531639857
-        },
-        {
-            "x": 7.4934981410182715,
-            "y": 4.155183222691002,
-            "heading": -3.2104217744560906,
-            "angularVelocity": -0.29426442476997366,
-            "velocityX": -1.3219949560335675,
-            "velocityY": 0.31655455087575607,
-            "timestamp": 2.1972113218251477
-        },
-        {
-            "x": 7.291774718335501,
-            "y": 4.220253220991955,
-            "heading": -3.2296266134452343,
-            "angularVelocity": -0.19612826940094027,
-            "velocityX": -2.06008838765949,
-            "velocityY": 0.6645234653569294,
-            "timestamp": 2.2951311120104383
-        },
-        {
-            "x": 7.018085982963682,
-            "y": 4.320043342217767,
-            "heading": -3.2392208692548645,
-            "angularVelocity": -0.09798076355633002,
-            "velocityX": -2.795029838747879,
-            "velocityY": 1.0191006438737447,
-            "timestamp": 2.393050902195729
-        },
-        {
-            "x": 6.674609525490048,
-            "y": 4.458745905991077,
-            "heading": -3.2392208627403147,
-            "angularVelocity": 6.652945395302414e-8,
-            "velocityX": -3.5077327762210677,
-            "velocityY": 1.4164916357648132,
-            "timestamp": 2.4909706923810195
-        },
-        {
-            "x": 6.337654568768723,
-            "y": 4.612615494980773,
-            "heading": -3.239220858536144,
-            "angularVelocity": 4.2934841578218656e-8,
-            "velocityX": -3.4411323398846756,
-            "velocityY": 1.5713839735413375,
-            "timestamp": 2.58889048256631
-        },
-        {
-            "x": 6.000699635853928,
-            "y": 4.766485136103752,
-            "heading": -3.239220854331973,
-            "angularVelocity": 4.293484065869374e-8,
-            "velocityX": -3.441132096761908,
-            "velocityY": 1.5713845059493727,
-            "timestamp": 2.686810272751601
-        },
-        {
-            "x": 5.66374470293921,
-            "y": 4.920354777226902,
-            "heading": -3.239220850127803,
-            "angularVelocity": 4.293484113596361e-8,
-            "velocityX": -3.4411320967611103,
-            "velocityY": 1.5713845059511196,
-            "timestamp": 2.7847300629368914
-        },
-        {
-            "x": 5.326789770024494,
-            "y": 5.074224418350052,
-            "heading": -3.239220845923632,
-            "angularVelocity": 4.293484003264988e-8,
-            "velocityX": -3.441132096761111,
-            "velocityY": 1.571384505951117,
-            "timestamp": 2.882649853122182
-        },
-        {
-            "x": 4.989834837078673,
-            "y": 5.228094059405012,
-            "heading": -3.2392208417194372,
-            "angularVelocity": 4.293508551285566e-8,
-            "velocityX": -3.4411320970787416,
-            "velocityY": 1.571384505254732,
-            "timestamp": 2.9805696433074726
-        },
-        {
-            "x": 4.699100923806666,
-            "y": 5.360856424917345,
-            "heading": -3.200805721128661,
-            "angularVelocity": 0.39231212115635317,
-            "velocityX": -2.9691026984622892,
-            "velocityY": 1.3558277163493837,
-            "timestamp": 3.0784894334927633
-        },
-        {
-            "x": 4.481050467012654,
-            "y": 5.460428209814717,
-            "heading": -3.171995332755761,
-            "angularVelocity": 0.2942243679074833,
-            "velocityX": -2.2268272468864767,
-            "velocityY": 1.016870897179783,
-            "timestamp": 3.176409223678054
-        },
-        {
-            "x": 4.335683489671324,
-            "y": 5.5268094029690245,
-            "heading": -3.152788680819891,
-            "angularVelocity": 0.19614678401093785,
-            "velocityX": -1.4845515606830426,
-            "velocityY": 0.6779139643650731,
-            "timestamp": 3.2743290138633445
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0.09807387885669308,
-            "velocityX": -0.7422757905606995,
-            "velocityY": 0.3389569868171628,
-            "timestamp": 3.372248804048635
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0,
-            "velocityX": 2.220393944714812e-34,
-            "velocityY": 0,
-            "timestamp": 3.4701685942339258
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S2.traj
deleted file mode 100644
index 9420bca5..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S2.traj	
+++ /dev/null
@@ -1,355 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.357227124880225,
-            "y": 6.730410634555629,
-            "heading": -2.7571910501011128,
-            "angularVelocity": -0.08264039553460875,
-            "velocityX": 0.6576608657862654,
-            "velocityY": -0.30556812237992936,
-            "timestamp": 0.08701616267193764
-        },
-        {
-            "x": 2.4716821810223912,
-            "y": 6.677231589623697,
-            "heading": -2.771475854118176,
-            "angularVelocity": -0.1641626518388192,
-            "velocityX": 1.3153309986062809,
-            "velocityY": -0.6111398537812316,
-            "timestamp": 0.17403232534387528
-        },
-        {
-            "x": 2.6433664887429877,
-            "y": 6.597462350883563,
-            "heading": -2.792692156200411,
-            "angularVelocity": -0.2438202447770986,
-            "velocityX": 1.973016304659045,
-            "velocityY": -0.916717495815976,
-            "timestamp": 0.26104848801581293
-        },
-        {
-            "x": 2.8722826002398385,
-            "y": 6.491101923690649,
-            "heading": -2.820514979829771,
-            "angularVelocity": -0.319743169257603,
-            "velocityX": 2.630730940870083,
-            "velocityY": -1.222306568423469,
-            "timestamp": 0.34806465068775055
-        },
-        {
-            "x": 3.1584374767810863,
-            "y": 6.358147629132494,
-            "heading": -2.8539648898748324,
-            "angularVelocity": -0.38441030973950396,
-            "velocityX": 3.288525576795303,
-            "velocityY": -1.5279264274087752,
-            "timestamp": 0.4350808133596882
-        },
-        {
-            "x": 3.456964402226087,
-            "y": 6.219442363897474,
-            "heading": -2.853964894160219,
-            "angularVelocity": -4.924816792193532e-8,
-            "velocityX": 3.4307066213720128,
-            "velocityY": -1.5940172604250262,
-            "timestamp": 0.5220969760316259
-        },
-        {
-            "x": 3.755491327665543,
-            "y": 6.0807370986505,
-            "heading": -2.853964898445594,
-            "angularVelocity": -4.924803343036069e-8,
-            "velocityX": 3.4307066213082953,
-            "velocityY": -1.5940172605623917,
-            "timestamp": 0.6091131387035635
-        },
-        {
-            "x": 4.054018253104999,
-            "y": 5.942031833403527,
-            "heading": -2.853964902730969,
-            "angularVelocity": -4.924803431145499e-8,
-            "velocityX": 3.4307066213082953,
-            "velocityY": -1.594017260562392,
-            "timestamp": 0.6961293013755012
-        },
-        {
-            "x": 4.352545178544456,
-            "y": 5.803326568156554,
-            "heading": -2.8539649070163438,
-            "angularVelocity": -4.9248033877501264e-8,
-            "velocityX": 3.4307066213082953,
-            "velocityY": -1.5940172605623917,
-            "timestamp": 0.7831454640474389
-        },
-        {
-            "x": 4.651072103983911,
-            "y": 5.66462130290958,
-            "heading": -2.8539649113017185,
-            "angularVelocity": -4.924803325887112e-8,
-            "velocityX": 3.430706621308295,
-            "velocityY": -1.5940172605623921,
-            "timestamp": 0.8701616267193766
-        },
-        {
-            "x": 4.949599029423368,
-            "y": 5.525916037662607,
-            "heading": -2.853964915587093,
-            "angularVelocity": -4.924803367441165e-8,
-            "velocityX": 3.430706621308295,
-            "velocityY": -1.5940172605623926,
-            "timestamp": 0.9571777893913143
-        },
-        {
-            "x": 5.248125954862823,
-            "y": 5.387210772415633,
-            "heading": -2.853964919872468,
-            "angularVelocity": -4.924803391655779e-8,
-            "velocityX": 3.4307066213082953,
-            "velocityY": -1.594017260562392,
-            "timestamp": 1.044193952063252
-        },
-        {
-            "x": 5.54665288030228,
-            "y": 5.24850550716866,
-            "heading": -2.8539649241578426,
-            "angularVelocity": -4.9248034279126105e-8,
-            "velocityX": 3.4307066213082953,
-            "velocityY": -1.5940172605623923,
-            "timestamp": 1.1312101147351896
-        },
-        {
-            "x": 5.8451798057417355,
-            "y": 5.109800241921686,
-            "heading": -2.8539649284432174,
-            "angularVelocity": -4.9248034369336404e-8,
-            "velocityX": 3.4307066213082953,
-            "velocityY": -1.594017260562393,
-            "timestamp": 1.2182262774071273
-        },
-        {
-            "x": 6.143706731181192,
-            "y": 4.9710949766747135,
-            "heading": -2.853964932728592,
-            "angularVelocity": -4.9248033497376594e-8,
-            "velocityX": 3.4307066213082953,
-            "velocityY": -1.594017260562392,
-            "timestamp": 1.305242440079065
-        },
-        {
-            "x": 6.4422336566206475,
-            "y": 4.83238971142774,
-            "heading": -2.8539649370139673,
-            "angularVelocity": -4.924803317735332e-8,
-            "velocityX": 3.4307066213082953,
-            "velocityY": -1.5940172605623926,
-            "timestamp": 1.3922586027510027
-        },
-        {
-            "x": 6.740760582057679,
-            "y": 4.693684446175547,
-            "heading": -2.853964941299342,
-            "angularVelocity": -4.9248033793238546e-8,
-            "velocityX": 3.4307066212804305,
-            "velocityY": -1.5940172606223628,
-            "timestamp": 1.4792747654229403
-        },
-        {
-            "x": 7.039286998892109,
-            "y": 4.554978086294829,
-            "heading": -2.8539649455851244,
-            "angularVelocity": -4.925271638681629e-8,
-            "velocityX": 3.4307007763593735,
-            "velocityY": -1.59402984022244,
-            "timestamp": 1.566290928094878
-        },
-        {
-            "x": 7.281877390378217,
-            "y": 4.416206845653086,
-            "heading": -2.926969050497618,
-            "angularVelocity": -0.8389717803085356,
-            "velocityX": 2.7878773786049877,
-            "velocityY": -1.594775457577107,
-            "timestamp": 1.6533070907668157
-        },
-        {
-            "x": 7.4672438532140255,
-            "y": 4.304023352017119,
-            "heading": -2.9923953501231186,
-            "angularVelocity": -0.7518867485821702,
-            "velocityX": 2.130253244269864,
-            "velocityY": -1.2892259344843033,
-            "timestamp": 1.7403232534387534
-        },
-        {
-            "x": 7.595386429544574,
-            "y": 4.218427699418672,
-            "heading": -3.050241491364391,
-            "angularVelocity": -0.6647746747849547,
-            "velocityX": 1.4726295942704664,
-            "velocityY": -0.9836753307676307,
-            "timestamp": 1.827339416110691
-        },
-        {
-            "x": 7.666305140070344,
-            "y": 4.159419912093254,
-            "heading": -3.100505408798514,
-            "angularVelocity": -0.5776388649040337,
-            "velocityX": 0.8150061821634523,
-            "velocityY": -0.6781244485335891,
-            "timestamp": 1.9143555787826287
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.4904824238455614,
-            "velocityX": 0.15738294483620113,
-            "velocityY": -0.37257345184803875,
-            "timestamp": 2.0013717414545664
-        },
-        {
-            "x": 7.62294760973909,
-            "y": 4.124186267487049,
-            "heading": -3.1816074637236196,
-            "angularVelocity": -0.39238397540812464,
-            "velocityX": -0.5826441228371791,
-            "velocityY": -0.02873507497949264,
-            "timestamp": 2.099291531639857
-        },
-        {
-            "x": 7.4934981410182715,
-            "y": 4.155183222691002,
-            "heading": -3.2104217744560906,
-            "angularVelocity": -0.29426442476997366,
-            "velocityX": -1.3219949560335675,
-            "velocityY": 0.31655455087575607,
-            "timestamp": 2.1972113218251477
-        },
-        {
-            "x": 7.291774718335501,
-            "y": 4.220253220991955,
-            "heading": -3.2296266134452343,
-            "angularVelocity": -0.19612826940094027,
-            "velocityX": -2.06008838765949,
-            "velocityY": 0.6645234653569294,
-            "timestamp": 2.2951311120104383
-        },
-        {
-            "x": 7.018085982963682,
-            "y": 4.320043342217767,
-            "heading": -3.2392208692548645,
-            "angularVelocity": -0.09798076355633002,
-            "velocityX": -2.795029838747879,
-            "velocityY": 1.0191006438737447,
-            "timestamp": 2.393050902195729
-        },
-        {
-            "x": 6.674609525490048,
-            "y": 4.458745905991077,
-            "heading": -3.2392208627403147,
-            "angularVelocity": 6.652945395302414e-8,
-            "velocityX": -3.5077327762210677,
-            "velocityY": 1.4164916357648132,
-            "timestamp": 2.4909706923810195
-        },
-        {
-            "x": 6.337654568768723,
-            "y": 4.612615494980773,
-            "heading": -3.239220858536144,
-            "angularVelocity": 4.2934841578218656e-8,
-            "velocityX": -3.4411323398846756,
-            "velocityY": 1.5713839735413375,
-            "timestamp": 2.58889048256631
-        },
-        {
-            "x": 6.000699635853928,
-            "y": 4.766485136103752,
-            "heading": -3.239220854331973,
-            "angularVelocity": 4.293484065869374e-8,
-            "velocityX": -3.441132096761908,
-            "velocityY": 1.5713845059493727,
-            "timestamp": 2.686810272751601
-        },
-        {
-            "x": 5.66374470293921,
-            "y": 4.920354777226902,
-            "heading": -3.239220850127803,
-            "angularVelocity": 4.293484113596361e-8,
-            "velocityX": -3.4411320967611103,
-            "velocityY": 1.5713845059511196,
-            "timestamp": 2.7847300629368914
-        },
-        {
-            "x": 5.326789770024494,
-            "y": 5.074224418350052,
-            "heading": -3.239220845923632,
-            "angularVelocity": 4.293484003264988e-8,
-            "velocityX": -3.441132096761111,
-            "velocityY": 1.571384505951117,
-            "timestamp": 2.882649853122182
-        },
-        {
-            "x": 4.989834837078673,
-            "y": 5.228094059405012,
-            "heading": -3.2392208417194372,
-            "angularVelocity": 4.293508551285566e-8,
-            "velocityX": -3.4411320970787416,
-            "velocityY": 1.571384505254732,
-            "timestamp": 2.9805696433074726
-        },
-        {
-            "x": 4.699100923806666,
-            "y": 5.360856424917345,
-            "heading": -3.200805721128661,
-            "angularVelocity": 0.39231212115635317,
-            "velocityX": -2.9691026984622892,
-            "velocityY": 1.3558277163493837,
-            "timestamp": 3.0784894334927633
-        },
-        {
-            "x": 4.481050467012654,
-            "y": 5.460428209814717,
-            "heading": -3.171995332755761,
-            "angularVelocity": 0.2942243679074833,
-            "velocityX": -2.2268272468864767,
-            "velocityY": 1.016870897179783,
-            "timestamp": 3.176409223678054
-        },
-        {
-            "x": 4.335683489671324,
-            "y": 5.5268094029690245,
-            "heading": -3.152788680819891,
-            "angularVelocity": 0.19614678401093785,
-            "velocityX": -1.4845515606830426,
-            "velocityY": 0.6779139643650731,
-            "timestamp": 3.2743290138633445
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0.09807387885669308,
-            "velocityX": -0.7422757905606995,
-            "velocityY": 0.3389569868171628,
-            "timestamp": 3.372248804048635
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0,
-            "velocityX": 2.220393944714812e-34,
-            "velocityY": 0,
-            "timestamp": 3.4701685942339258
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S3.1.traj
deleted file mode 100644
index c8a9e073..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S3.1.traj	
+++ /dev/null
@@ -1,373 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3574291714983375,
-            "y": 6.741815654710436,
-            "heading": -2.758007728773179,
-            "angularVelocity": -0.09483878465079744,
-            "velocityX": 0.6801570061474024,
-            "velocityY": -0.17983436924136462,
-            "timestamp": 0.08443516861442411
-        },
-        {
-            "x": 2.47228751228047,
-            "y": 6.711446972989247,
-            "heading": -2.7740235521881464,
-            "angularVelocity": -0.18968190243216043,
-            "velocityX": 1.3603139860670759,
-            "velocityY": -0.35966863357457735,
-            "timestamp": 0.16887033722884823
-        },
-        {
-            "x": 2.6445750170696067,
-            "y": 6.665893967918211,
-            "heading": -2.798048184372895,
-            "angularVelocity": -0.2845334779214143,
-            "velocityX": 2.0404709034915762,
-            "velocityY": -0.5395027429749605,
-            "timestamp": 0.2533055058432723
-        },
-        {
-            "x": 2.8742916749054777,
-            "y": 6.605156656477843,
-            "heading": -2.830082761948688,
-            "angularVelocity": -0.37939851487799736,
-            "velocityX": 2.720627691109149,
-            "velocityY": -0.7193366512682123,
-            "timestamp": 0.33774067445769645
-        },
-        {
-            "x": 3.161437459702618,
-            "y": 6.529235060521681,
-            "heading": -2.870128852530637,
-            "angularVelocity": -0.4742821177371166,
-            "velocityX": 3.400784169785955,
-            "velocityY": -0.8991703007411442,
-            "timestamp": 0.4221758430721206
-        },
-        {
-            "x": 3.470239032500689,
-            "y": 6.44758707798969,
-            "heading": -2.8701288573799584,
-            "angularVelocity": -5.743248864696825e-8,
-            "velocityX": 3.6572624637989906,
-            "velocityY": -0.9669902230531149,
-            "timestamp": 0.5066110116865447
-        },
-        {
-            "x": 3.77904060528322,
-            "y": 6.3659390953988515,
-            "heading": -2.8701288622292584,
-            "angularVelocity": -5.743222732502049e-8,
-            "velocityX": 3.6572624636149422,
-            "velocityY": -0.9669902237500612,
-            "timestamp": 0.5910461803009688
-        },
-        {
-            "x": 4.087842178065752,
-            "y": 6.284291112808013,
-            "heading": -2.870128867078558,
-            "angularVelocity": -5.743222733813293e-8,
-            "velocityX": 3.6572624636149413,
-            "velocityY": -0.9669902237500647,
-            "timestamp": 0.6754813489153929
-        },
-        {
-            "x": 4.396643750848283,
-            "y": 6.202643130217175,
-            "heading": -2.870128871927858,
-            "angularVelocity": -5.7432227678581676e-8,
-            "velocityX": 3.657262463614942,
-            "velocityY": -0.9669902237500649,
-            "timestamp": 0.759916517529817
-        },
-        {
-            "x": 4.705445323630815,
-            "y": 6.120995147626337,
-            "heading": -2.870128876777158,
-            "angularVelocity": -5.743222718130824e-8,
-            "velocityX": 3.657262463614942,
-            "velocityY": -0.9669902237500648,
-            "timestamp": 0.8443516861442412
-        },
-        {
-            "x": 5.014246896413346,
-            "y": 6.039347165035499,
-            "heading": -2.870128881626458,
-            "angularVelocity": -5.743222751764232e-8,
-            "velocityX": 3.657262463614942,
-            "velocityY": -0.9669902237500648,
-            "timestamp": 0.9287868547586653
-        },
-        {
-            "x": 5.323048469195878,
-            "y": 5.957699182444661,
-            "heading": -2.8701288864757575,
-            "angularVelocity": -5.74322277347238e-8,
-            "velocityX": 3.657262463614942,
-            "velocityY": -0.9669902237500648,
-            "timestamp": 1.0132220233730893
-        },
-        {
-            "x": 5.631850041978409,
-            "y": 5.876051199853823,
-            "heading": -2.8701288913250576,
-            "angularVelocity": -5.743222678749957e-8,
-            "velocityX": 3.657262463614942,
-            "velocityY": -0.9669902237500648,
-            "timestamp": 1.0976571919875133
-        },
-        {
-            "x": 5.940651614760942,
-            "y": 5.7944032172629845,
-            "heading": -2.870128896174357,
-            "angularVelocity": -5.7432226879414726e-8,
-            "velocityX": 3.6572624636149413,
-            "velocityY": -0.9669902237500653,
-            "timestamp": 1.1820923606019373
-        },
-        {
-            "x": 6.249453187542404,
-            "y": 5.712755234668102,
-            "heading": -2.8701289010236573,
-            "angularVelocity": -5.743222723099526e-8,
-            "velocityX": 3.6572624636022777,
-            "velocityY": -0.9669902237979594,
-            "timestamp": 1.2665275292163614
-        },
-        {
-            "x": 6.5582545569659105,
-            "y": 5.631106482955165,
-            "heading": -2.8701289058729587,
-            "angularVelocity": -5.743224882801054e-8,
-            "velocityX": 3.6572600551514123,
-            "velocityY": -0.9669993327755135,
-            "timestamp": 1.3509626978307854
-        },
-        {
-            "x": 6.8516839479397085,
-            "y": 5.504916165148498,
-            "heading": -2.8701289109689694,
-            "angularVelocity": -6.035412197409506e-8,
-            "velocityX": 3.4752034701766337,
-            "velocityY": -1.4945231930893996,
-            "timestamp": 1.4353978664452094
-        },
-        {
-            "x": 7.116648885902659,
-            "y": 5.326539084420994,
-            "heading": -2.8701289171366793,
-            "angularVelocity": -7.304669371510495e-8,
-            "velocityX": 3.1380873907283835,
-            "velocityY": -2.1125922249539495,
-            "timestamp": 1.5198330350596334
-        },
-        {
-            "x": 7.343971949437644,
-            "y": 5.1021533761176485,
-            "heading": -2.8701292296822887,
-            "angularVelocity": -0.0000037016046138767138,
-            "velocityX": 2.6922793815105543,
-            "velocityY": -2.6574910903300615,
-            "timestamp": 1.6042682036740574
-        },
-        {
-            "x": 7.516721651510428,
-            "y": 4.857947803192162,
-            "heading": -2.9165540487463932,
-            "angularVelocity": -0.5498279902312642,
-            "velocityX": 2.0459448936692075,
-            "velocityY": -2.8922257980043913,
-            "timestamp": 1.6887033722884814
-        },
-        {
-            "x": 7.630386965757564,
-            "y": 4.610372175146774,
-            "heading": -2.9830505381288734,
-            "angularVelocity": -0.7875449350512241,
-            "velocityX": 1.3461844881981369,
-            "velocityY": -2.932138729727077,
-            "timestamp": 1.7731385409029055
-        },
-        {
-            "x": 7.684751488226614,
-            "y": 4.365674345538311,
-            "heading": -3.0601706707306313,
-            "angularVelocity": -0.9133650570882078,
-            "velocityX": 0.6438611228136838,
-            "velocityY": -2.8980557938586533,
-            "timestamp": 1.8575737095173295
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.9831760605353209,
-            "velocityX": -0.05627380515240752,
-            "velocityY": -2.8267172252385406,
-            "timestamp": 1.9420088781317535
-        },
-        {
-            "x": 7.621934708150364,
-            "y": 3.906654914920449,
-            "heading": -3.2251079869587973,
-            "angularVelocity": -1.0177912145669754,
-            "velocityX": -0.7213917327302901,
-            "velocityY": -2.737523874602733,
-            "timestamp": 2.022499531427526
-        },
-        {
-            "x": 7.510577058905177,
-            "y": 3.6951455922988243,
-            "heading": -3.30599509113983,
-            "angularVelocity": -1.0049254275993986,
-            "velocityX": -1.3834854692504928,
-            "velocityY": -2.627750104653823,
-            "timestamp": 2.102990184723298
-        },
-        {
-            "x": 7.346480542958668,
-            "y": 3.4951541921560745,
-            "heading": -3.379583459687032,
-            "angularVelocity": -0.9142473757388557,
-            "velocityX": -2.0387027465600407,
-            "velocityY": -2.484653707652887,
-            "timestamp": 2.1834808380190704
-        },
-        {
-            "x": 7.131234653225788,
-            "y": 3.3116699228744952,
-            "heading": -3.433702808915206,
-            "angularVelocity": -0.6723681199262506,
-            "velocityX": -2.67417247741705,
-            "velocityY": -2.279572369817109,
-            "timestamp": 2.2639714913148428
-        },
-        {
-            "x": 6.871582405242626,
-            "y": 3.1556914599677013,
-            "heading": -3.4376019485164777,
-            "angularVelocity": -0.04844214131228369,
-            "velocityX": -3.2258683132939807,
-            "velocityY": -1.9378456568569487,
-            "timestamp": 2.344462144610615
-        },
-        {
-            "x": 6.586927593692301,
-            "y": 3.0475868041755487,
-            "heading": -3.4376019671352136,
-            "angularVelocity": -2.3131550136324309e-7,
-            "velocityX": -3.5364952313696496,
-            "velocityY": -1.3430709202335072,
-            "timestamp": 2.4249527979063874
-        },
-        {
-            "x": 6.287649067705066,
-            "y": 2.9914855953111847,
-            "heading": -3.4376019800205944,
-            "angularVelocity": -1.6008543147444885e-7,
-            "velocityX": -3.718177375048765,
-            "velocityY": -0.6969903531309354,
-            "timestamp": 2.5054434512021597
-        },
-        {
-            "x": 5.983166666187018,
-            "y": 2.9891511130138855,
-            "heading": -3.437601992016166,
-            "angularVelocity": -1.4903061473211454e-7,
-            "velocityX": -3.7828292981942986,
-            "velocityY": -0.029003147591768797,
-            "timestamp": 2.585934104497932
-        },
-        {
-            "x": 5.678681298356483,
-            "y": 2.9910598954241565,
-            "heading": -3.437602003982072,
-            "angularVelocity": -1.4866205580605315e-7,
-            "velocityX": -3.782866151075542,
-            "velocityY": 0.02371433616337754,
-            "timestamp": 2.6664247577937044
-        },
-        {
-            "x": 5.374195930686038,
-            "y": 2.9929687033714,
-            "heading": -3.437602015947978,
-            "angularVelocity": -1.4866205622499977e-7,
-            "velocityX": -3.7828661490866247,
-            "velocityY": 0.02371465342968995,
-            "timestamp": 2.7469154110894767
-        },
-        {
-            "x": 5.06971056314094,
-            "y": 2.9948775312983553,
-            "heading": -3.4376020279140773,
-            "angularVelocity": -1.4866445697347623e-7,
-            "velocityX": -3.7828661475293295,
-            "velocityY": 0.023714901653687247,
-            "timestamp": 2.827406064385249
-        },
-        {
-            "x": 4.800808155197524,
-            "y": 2.9965858943831902,
-            "heading": -3.5281868157365524,
-            "angularVelocity": -1.1254075363217084,
-            "velocityX": -3.340790476073066,
-            "velocityY": 0.021224365996295316,
-            "timestamp": 2.9078967176810213
-        },
-        {
-            "x": 4.585685258881781,
-            "y": 2.9979519483478,
-            "heading": -3.6006057670766456,
-            "angularVelocity": -0.8997187670227613,
-            "velocityX": -2.6726444314627344,
-            "velocityY": 0.016971585006104423,
-            "timestamp": 2.9883873709767936
-        },
-        {
-            "x": 4.424342674038891,
-            "y": 2.9989760866788195,
-            "heading": -3.6548985624477908,
-            "angularVelocity": -0.6745229805958503,
-            "velocityX": -2.0044884497336284,
-            "velocityY": 0.012723692616254528,
-            "timestamp": 3.068878024272566
-        },
-        {
-            "x": 4.316780863688961,
-            "y": 2.9996586949819566,
-            "heading": -3.6910886560266527,
-            "angularVelocity": -0.44961858423356516,
-            "velocityX": -1.33632671553406,
-            "velocityY": 0.008480590915651792,
-            "timestamp": 3.1493686775683383
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -3.7091853071795864,
-            "angularVelocity": -0.22482922441248102,
-            "velocityX": -0.6681628423530913,
-            "velocityY": 0.004240306222743686,
-            "timestamp": 3.2298593308641106
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -3.7091853071795864,
-            "angularVelocity": 0,
-            "velocityX": -1.1582419131838594e-33,
-            "velocityY": 0,
-            "timestamp": 3.310349984159883
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S3.traj
deleted file mode 100644
index c8a9e073..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S3.traj	
+++ /dev/null
@@ -1,373 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3574291714983375,
-            "y": 6.741815654710436,
-            "heading": -2.758007728773179,
-            "angularVelocity": -0.09483878465079744,
-            "velocityX": 0.6801570061474024,
-            "velocityY": -0.17983436924136462,
-            "timestamp": 0.08443516861442411
-        },
-        {
-            "x": 2.47228751228047,
-            "y": 6.711446972989247,
-            "heading": -2.7740235521881464,
-            "angularVelocity": -0.18968190243216043,
-            "velocityX": 1.3603139860670759,
-            "velocityY": -0.35966863357457735,
-            "timestamp": 0.16887033722884823
-        },
-        {
-            "x": 2.6445750170696067,
-            "y": 6.665893967918211,
-            "heading": -2.798048184372895,
-            "angularVelocity": -0.2845334779214143,
-            "velocityX": 2.0404709034915762,
-            "velocityY": -0.5395027429749605,
-            "timestamp": 0.2533055058432723
-        },
-        {
-            "x": 2.8742916749054777,
-            "y": 6.605156656477843,
-            "heading": -2.830082761948688,
-            "angularVelocity": -0.37939851487799736,
-            "velocityX": 2.720627691109149,
-            "velocityY": -0.7193366512682123,
-            "timestamp": 0.33774067445769645
-        },
-        {
-            "x": 3.161437459702618,
-            "y": 6.529235060521681,
-            "heading": -2.870128852530637,
-            "angularVelocity": -0.4742821177371166,
-            "velocityX": 3.400784169785955,
-            "velocityY": -0.8991703007411442,
-            "timestamp": 0.4221758430721206
-        },
-        {
-            "x": 3.470239032500689,
-            "y": 6.44758707798969,
-            "heading": -2.8701288573799584,
-            "angularVelocity": -5.743248864696825e-8,
-            "velocityX": 3.6572624637989906,
-            "velocityY": -0.9669902230531149,
-            "timestamp": 0.5066110116865447
-        },
-        {
-            "x": 3.77904060528322,
-            "y": 6.3659390953988515,
-            "heading": -2.8701288622292584,
-            "angularVelocity": -5.743222732502049e-8,
-            "velocityX": 3.6572624636149422,
-            "velocityY": -0.9669902237500612,
-            "timestamp": 0.5910461803009688
-        },
-        {
-            "x": 4.087842178065752,
-            "y": 6.284291112808013,
-            "heading": -2.870128867078558,
-            "angularVelocity": -5.743222733813293e-8,
-            "velocityX": 3.6572624636149413,
-            "velocityY": -0.9669902237500647,
-            "timestamp": 0.6754813489153929
-        },
-        {
-            "x": 4.396643750848283,
-            "y": 6.202643130217175,
-            "heading": -2.870128871927858,
-            "angularVelocity": -5.7432227678581676e-8,
-            "velocityX": 3.657262463614942,
-            "velocityY": -0.9669902237500649,
-            "timestamp": 0.759916517529817
-        },
-        {
-            "x": 4.705445323630815,
-            "y": 6.120995147626337,
-            "heading": -2.870128876777158,
-            "angularVelocity": -5.743222718130824e-8,
-            "velocityX": 3.657262463614942,
-            "velocityY": -0.9669902237500648,
-            "timestamp": 0.8443516861442412
-        },
-        {
-            "x": 5.014246896413346,
-            "y": 6.039347165035499,
-            "heading": -2.870128881626458,
-            "angularVelocity": -5.743222751764232e-8,
-            "velocityX": 3.657262463614942,
-            "velocityY": -0.9669902237500648,
-            "timestamp": 0.9287868547586653
-        },
-        {
-            "x": 5.323048469195878,
-            "y": 5.957699182444661,
-            "heading": -2.8701288864757575,
-            "angularVelocity": -5.74322277347238e-8,
-            "velocityX": 3.657262463614942,
-            "velocityY": -0.9669902237500648,
-            "timestamp": 1.0132220233730893
-        },
-        {
-            "x": 5.631850041978409,
-            "y": 5.876051199853823,
-            "heading": -2.8701288913250576,
-            "angularVelocity": -5.743222678749957e-8,
-            "velocityX": 3.657262463614942,
-            "velocityY": -0.9669902237500648,
-            "timestamp": 1.0976571919875133
-        },
-        {
-            "x": 5.940651614760942,
-            "y": 5.7944032172629845,
-            "heading": -2.870128896174357,
-            "angularVelocity": -5.7432226879414726e-8,
-            "velocityX": 3.6572624636149413,
-            "velocityY": -0.9669902237500653,
-            "timestamp": 1.1820923606019373
-        },
-        {
-            "x": 6.249453187542404,
-            "y": 5.712755234668102,
-            "heading": -2.8701289010236573,
-            "angularVelocity": -5.743222723099526e-8,
-            "velocityX": 3.6572624636022777,
-            "velocityY": -0.9669902237979594,
-            "timestamp": 1.2665275292163614
-        },
-        {
-            "x": 6.5582545569659105,
-            "y": 5.631106482955165,
-            "heading": -2.8701289058729587,
-            "angularVelocity": -5.743224882801054e-8,
-            "velocityX": 3.6572600551514123,
-            "velocityY": -0.9669993327755135,
-            "timestamp": 1.3509626978307854
-        },
-        {
-            "x": 6.8516839479397085,
-            "y": 5.504916165148498,
-            "heading": -2.8701289109689694,
-            "angularVelocity": -6.035412197409506e-8,
-            "velocityX": 3.4752034701766337,
-            "velocityY": -1.4945231930893996,
-            "timestamp": 1.4353978664452094
-        },
-        {
-            "x": 7.116648885902659,
-            "y": 5.326539084420994,
-            "heading": -2.8701289171366793,
-            "angularVelocity": -7.304669371510495e-8,
-            "velocityX": 3.1380873907283835,
-            "velocityY": -2.1125922249539495,
-            "timestamp": 1.5198330350596334
-        },
-        {
-            "x": 7.343971949437644,
-            "y": 5.1021533761176485,
-            "heading": -2.8701292296822887,
-            "angularVelocity": -0.0000037016046138767138,
-            "velocityX": 2.6922793815105543,
-            "velocityY": -2.6574910903300615,
-            "timestamp": 1.6042682036740574
-        },
-        {
-            "x": 7.516721651510428,
-            "y": 4.857947803192162,
-            "heading": -2.9165540487463932,
-            "angularVelocity": -0.5498279902312642,
-            "velocityX": 2.0459448936692075,
-            "velocityY": -2.8922257980043913,
-            "timestamp": 1.6887033722884814
-        },
-        {
-            "x": 7.630386965757564,
-            "y": 4.610372175146774,
-            "heading": -2.9830505381288734,
-            "angularVelocity": -0.7875449350512241,
-            "velocityX": 1.3461844881981369,
-            "velocityY": -2.932138729727077,
-            "timestamp": 1.7731385409029055
-        },
-        {
-            "x": 7.684751488226614,
-            "y": 4.365674345538311,
-            "heading": -3.0601706707306313,
-            "angularVelocity": -0.9133650570882078,
-            "velocityX": 0.6438611228136838,
-            "velocityY": -2.8980557938586533,
-            "timestamp": 1.8575737095173295
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.9831760605353209,
-            "velocityX": -0.05627380515240752,
-            "velocityY": -2.8267172252385406,
-            "timestamp": 1.9420088781317535
-        },
-        {
-            "x": 7.621934708150364,
-            "y": 3.906654914920449,
-            "heading": -3.2251079869587973,
-            "angularVelocity": -1.0177912145669754,
-            "velocityX": -0.7213917327302901,
-            "velocityY": -2.737523874602733,
-            "timestamp": 2.022499531427526
-        },
-        {
-            "x": 7.510577058905177,
-            "y": 3.6951455922988243,
-            "heading": -3.30599509113983,
-            "angularVelocity": -1.0049254275993986,
-            "velocityX": -1.3834854692504928,
-            "velocityY": -2.627750104653823,
-            "timestamp": 2.102990184723298
-        },
-        {
-            "x": 7.346480542958668,
-            "y": 3.4951541921560745,
-            "heading": -3.379583459687032,
-            "angularVelocity": -0.9142473757388557,
-            "velocityX": -2.0387027465600407,
-            "velocityY": -2.484653707652887,
-            "timestamp": 2.1834808380190704
-        },
-        {
-            "x": 7.131234653225788,
-            "y": 3.3116699228744952,
-            "heading": -3.433702808915206,
-            "angularVelocity": -0.6723681199262506,
-            "velocityX": -2.67417247741705,
-            "velocityY": -2.279572369817109,
-            "timestamp": 2.2639714913148428
-        },
-        {
-            "x": 6.871582405242626,
-            "y": 3.1556914599677013,
-            "heading": -3.4376019485164777,
-            "angularVelocity": -0.04844214131228369,
-            "velocityX": -3.2258683132939807,
-            "velocityY": -1.9378456568569487,
-            "timestamp": 2.344462144610615
-        },
-        {
-            "x": 6.586927593692301,
-            "y": 3.0475868041755487,
-            "heading": -3.4376019671352136,
-            "angularVelocity": -2.3131550136324309e-7,
-            "velocityX": -3.5364952313696496,
-            "velocityY": -1.3430709202335072,
-            "timestamp": 2.4249527979063874
-        },
-        {
-            "x": 6.287649067705066,
-            "y": 2.9914855953111847,
-            "heading": -3.4376019800205944,
-            "angularVelocity": -1.6008543147444885e-7,
-            "velocityX": -3.718177375048765,
-            "velocityY": -0.6969903531309354,
-            "timestamp": 2.5054434512021597
-        },
-        {
-            "x": 5.983166666187018,
-            "y": 2.9891511130138855,
-            "heading": -3.437601992016166,
-            "angularVelocity": -1.4903061473211454e-7,
-            "velocityX": -3.7828292981942986,
-            "velocityY": -0.029003147591768797,
-            "timestamp": 2.585934104497932
-        },
-        {
-            "x": 5.678681298356483,
-            "y": 2.9910598954241565,
-            "heading": -3.437602003982072,
-            "angularVelocity": -1.4866205580605315e-7,
-            "velocityX": -3.782866151075542,
-            "velocityY": 0.02371433616337754,
-            "timestamp": 2.6664247577937044
-        },
-        {
-            "x": 5.374195930686038,
-            "y": 2.9929687033714,
-            "heading": -3.437602015947978,
-            "angularVelocity": -1.4866205622499977e-7,
-            "velocityX": -3.7828661490866247,
-            "velocityY": 0.02371465342968995,
-            "timestamp": 2.7469154110894767
-        },
-        {
-            "x": 5.06971056314094,
-            "y": 2.9948775312983553,
-            "heading": -3.4376020279140773,
-            "angularVelocity": -1.4866445697347623e-7,
-            "velocityX": -3.7828661475293295,
-            "velocityY": 0.023714901653687247,
-            "timestamp": 2.827406064385249
-        },
-        {
-            "x": 4.800808155197524,
-            "y": 2.9965858943831902,
-            "heading": -3.5281868157365524,
-            "angularVelocity": -1.1254075363217084,
-            "velocityX": -3.340790476073066,
-            "velocityY": 0.021224365996295316,
-            "timestamp": 2.9078967176810213
-        },
-        {
-            "x": 4.585685258881781,
-            "y": 2.9979519483478,
-            "heading": -3.6006057670766456,
-            "angularVelocity": -0.8997187670227613,
-            "velocityX": -2.6726444314627344,
-            "velocityY": 0.016971585006104423,
-            "timestamp": 2.9883873709767936
-        },
-        {
-            "x": 4.424342674038891,
-            "y": 2.9989760866788195,
-            "heading": -3.6548985624477908,
-            "angularVelocity": -0.6745229805958503,
-            "velocityX": -2.0044884497336284,
-            "velocityY": 0.012723692616254528,
-            "timestamp": 3.068878024272566
-        },
-        {
-            "x": 4.316780863688961,
-            "y": 2.9996586949819566,
-            "heading": -3.6910886560266527,
-            "angularVelocity": -0.44961858423356516,
-            "velocityX": -1.33632671553406,
-            "velocityY": 0.008480590915651792,
-            "timestamp": 3.1493686775683383
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -3.7091853071795864,
-            "angularVelocity": -0.22482922441248102,
-            "velocityX": -0.6681628423530913,
-            "velocityY": 0.004240306222743686,
-            "timestamp": 3.2298593308641106
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -3.7091853071795864,
-            "angularVelocity": 0,
-            "velocityX": -1.1582419131838594e-33,
-            "velocityY": 0,
-            "timestamp": 3.310349984159883
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S4.1.traj
deleted file mode 100644
index f8080ad6..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S4.1.traj	
+++ /dev/null
@@ -1,382 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.352388442136136,
-            "y": 6.742171711440638,
-            "heading": -2.7776238211060527,
-            "angularVelocity": -0.340136208728432,
-            "velocityX": 0.6450666626176952,
-            "velocityY": -0.18258291758985748,
-            "timestamp": 0.08121399720716439
-        },
-        {
-            "x": 2.4572309209740357,
-            "y": 6.712518353067644,
-            "heading": -2.831291417681962,
-            "angularVelocity": -0.6608170810552816,
-            "velocityX": 1.290941000856364,
-            "velocityY": -0.3651262022858861,
-            "timestamp": 0.16242799441432879
-        },
-        {
-            "x": 2.614630940945627,
-            "y": 6.668043884281434,
-            "heading": -2.9082589809802553,
-            "angularVelocity": -0.9477130290857405,
-            "velocityX": 1.9380898045256882,
-            "velocityY": -0.5476207342969771,
-            "timestamp": 0.2436419916214932
-        },
-        {
-            "x": 2.824773268720236,
-            "y": 6.608764345103305,
-            "heading": -3.0025843751568475,
-            "angularVelocity": -1.1614425766389054,
-            "velocityX": 2.587513667614254,
-            "velocityY": -0.7299177631454756,
-            "timestamp": 0.32485598882865757
-        },
-        {
-            "x": 3.087924526957624,
-            "y": 6.534856635997473,
-            "heading": -3.094100877088787,
-            "angularVelocity": -1.1268562695832722,
-            "velocityX": 3.2402204950799356,
-            "velocityY": -0.9100365903255841,
-            "timestamp": 0.40606998603582195
-        },
-        {
-            "x": 3.383391154718911,
-            "y": 6.450664344423451,
-            "heading": -3.094100898976011,
-            "angularVelocity": -2.695006379530874e-7,
-            "velocityX": 3.638124435716056,
-            "velocityY": -1.036672180514684,
-            "timestamp": 0.48728398324298633
-        },
-        {
-            "x": 3.6788577741864907,
-            "y": 6.366472023742971,
-            "heading": -3.0941009208630392,
-            "angularVelocity": -2.694982209366716e-7,
-            "velocityX": 3.638124333594396,
-            "velocityY": -1.036672538906832,
-            "timestamp": 0.5684979804501508
-        },
-        {
-            "x": 3.9743243936540216,
-            "y": 6.282279703062319,
-            "heading": -3.094100942750067,
-            "angularVelocity": -2.694982225892601e-7,
-            "velocityX": 3.638124333593797,
-            "velocityY": -1.036672538908934,
-            "timestamp": 0.6497119776573151
-        },
-        {
-            "x": 4.269791013121552,
-            "y": 6.198087382381668,
-            "heading": -3.0941009646370947,
-            "angularVelocity": -2.694982224333276e-7,
-            "velocityX": 3.6381243335937974,
-            "velocityY": -1.0366725389089342,
-            "timestamp": 0.7309259748644795
-        },
-        {
-            "x": 4.565257632589082,
-            "y": 6.113895061701016,
-            "heading": -3.094100986524123,
-            "angularVelocity": -2.694982222034585e-7,
-            "velocityX": 3.6381243335937974,
-            "velocityY": -1.036672538908934,
-            "timestamp": 0.8121399720716439
-        },
-        {
-            "x": 4.860724252056613,
-            "y": 6.0297027410203645,
-            "heading": -3.0941010084111507,
-            "angularVelocity": -2.694982230736536e-7,
-            "velocityX": 3.638124333593797,
-            "velocityY": -1.0366725389089342,
-            "timestamp": 0.8933539692788083
-        },
-        {
-            "x": 5.156190871524144,
-            "y": 5.945510420339713,
-            "heading": -3.094101030298179,
-            "angularVelocity": -2.6949822300117995e-7,
-            "velocityX": 3.638124333593797,
-            "velocityY": -1.0366725389089342,
-            "timestamp": 0.9745679664859727
-        },
-        {
-            "x": 5.451657490991675,
-            "y": 5.861318099659061,
-            "heading": -3.0941010521852066,
-            "angularVelocity": -2.694982228196196e-7,
-            "velocityX": 3.638124333593797,
-            "velocityY": -1.0366725389089342,
-            "timestamp": 1.0557819636931371
-        },
-        {
-            "x": 5.747124110459206,
-            "y": 5.77712577897841,
-            "heading": -3.0941010740722343,
-            "angularVelocity": -2.694982236260836e-7,
-            "velocityX": 3.638124333593797,
-            "velocityY": -1.0366725389089344,
-            "timestamp": 1.1369959609003015
-        },
-        {
-            "x": 6.042590729926668,
-            "y": 5.69293345829752,
-            "heading": -3.0941010959592625,
-            "angularVelocity": -2.694982236351797e-7,
-            "velocityX": 3.6381243335929603,
-            "velocityY": -1.0366725389118696,
-            "timestamp": 1.218209958107466
-        },
-        {
-            "x": 6.338057337809956,
-            "y": 5.6087410969628495,
-            "heading": -3.0941011178462903,
-            "angularVelocity": -2.6949822192516605e-7,
-            "velocityX": 3.6381241909552937,
-            "velocityY": -1.0366730394879038,
-            "timestamp": 1.2994239553146303
-        },
-        {
-            "x": 6.63151320525297,
-            "y": 5.517787680923723,
-            "heading": -3.0941011398106846,
-            "angularVelocity": -2.704508467508134e-7,
-            "velocityX": 3.6133656455032592,
-            "velocityY": -1.1199229094611813,
-            "timestamp": 1.3806379525217947
-        },
-        {
-            "x": 6.904052184030547,
-            "y": 5.375973104754023,
-            "heading": -3.09410116345768,
-            "angularVelocity": -2.911689606888355e-7,
-            "velocityX": 3.355812891235069,
-            "velocityY": -1.7461839220887416,
-            "timestamp": 1.461851949728959
-        },
-        {
-            "x": 7.14694076900972,
-            "y": 5.1878401702780765,
-            "heading": -3.094101192989132,
-            "angularVelocity": -3.636251518218712e-7,
-            "velocityX": 2.9907231921877617,
-            "velocityY": -2.3165087416877084,
-            "timestamp": 1.5430659469361234
-        },
-        {
-            "x": 7.352395911467515,
-            "y": 4.959417222897554,
-            "heading": -3.0941012381435535,
-            "angularVelocity": -5.559930935407798e-7,
-            "velocityX": 2.529799659188236,
-            "velocityY": -2.812605649731368,
-            "timestamp": 1.6242799441432878
-        },
-        {
-            "x": 7.513833646479666,
-            "y": 4.698024306067166,
-            "heading": -3.094103186189668,
-            "angularVelocity": -0.00002398658095119272,
-            "velocityX": 1.9878067889975632,
-            "velocityY": -3.2185697764010213,
-            "timestamp": 1.7054939413504522
-        },
-        {
-            "x": 7.6238137890728055,
-            "y": 4.417795537290539,
-            "heading": -3.1108263182271356,
-            "angularVelocity": -0.20591440655693208,
-            "velocityX": 1.35420181708456,
-            "velocityY": -3.4504984166977444,
-            "timestamp": 1.7867079385576166
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.3984410331631951,
-            "velocityX": 0.6918291533072239,
-            "velocityY": -3.5806086055413933,
-            "timestamp": 1.867921935764781
-        },
-        {
-            "x": 7.6870591404547595,
-            "y": 3.8674277894608684,
-            "heading": -3.1773151030340356,
-            "angularVelocity": -0.47768106939784993,
-            "velocityX": 0.09879982223438938,
-            "velocityY": -3.63297605573316,
-            "timestamp": 1.9393708543807355
-        },
-        {
-            "x": 7.651588574491949,
-            "y": 3.608358714127091,
-            "heading": -3.205456863771594,
-            "angularVelocity": -0.39387245154381195,
-            "velocityX": -0.4964465053735158,
-            "velocityY": -3.625934168821804,
-            "timestamp": 2.0108197729966903
-        },
-        {
-            "x": 7.574047848742119,
-            "y": 3.354318466237311,
-            "heading": -3.2189736743217594,
-            "angularVelocity": -0.1891814573314478,
-            "velocityX": -1.0852610123268396,
-            "velocityY": -3.5555506341084104,
-            "timestamp": 2.082268691612645
-        },
-        {
-            "x": 7.455586952035965,
-            "y": 3.1113741995114603,
-            "heading": -3.2189745116430255,
-            "angularVelocity": -0.00001171915938609546,
-            "velocityX": -1.6579802605551721,
-            "velocityY": -3.4002511365232904,
-            "timestamp": 2.1537176102285995
-        },
-        {
-            "x": 7.300453960582251,
-            "y": 2.8900400447662866,
-            "heading": -3.2189745885172623,
-            "angularVelocity": -0.0000010759328215916604,
-            "velocityX": -2.171243378574256,
-            "velocityY": -3.097795726435581,
-            "timestamp": 2.225166528844554
-        },
-        {
-            "x": 7.11249649568723,
-            "y": 2.6958055218428347,
-            "heading": -3.2189746440183837,
-            "angularVelocity": -7.767944182730611e-7,
-            "velocityX": -2.6306551384378265,
-            "velocityY": -2.718508924810283,
-            "timestamp": 2.2966154474605087
-        },
-        {
-            "x": 6.896376183100963,
-            "y": 2.533487712060768,
-            "heading": -3.2189746912030057,
-            "angularVelocity": -6.603965892997474e-7,
-            "velocityX": -3.024822723297278,
-            "velocityY": -2.2718021899927887,
-            "timestamp": 2.3680643660764633
-        },
-        {
-            "x": 6.6576888713401265,
-            "y": 2.4066672511412732,
-            "heading": -3.218974736751788,
-            "angularVelocity": -6.375013452668414e-7,
-            "velocityX": -3.340670739366957,
-            "velocityY": -1.7749808302282493,
-            "timestamp": 2.439513284692418
-        },
-        {
-            "x": 6.418991689499422,
-            "y": 2.2798653705517777,
-            "heading": -3.2189747823005193,
-            "angularVelocity": -6.375006445255001e-7,
-            "velocityX": -3.3408088808494862,
-            "velocityY": -1.7747207802229084,
-            "timestamp": 2.5109622033083725
-        },
-        {
-            "x": 6.180294507575015,
-            "y": 2.1530634901198433,
-            "heading": -3.2189748278492507,
-            "angularVelocity": -6.375006422464704e-7,
-            "velocityX": -3.340808882020978,
-            "velocityY": -1.7747207780176886,
-            "timestamp": 2.582411121924327
-        },
-        {
-            "x": 5.941597302835271,
-            "y": 2.026261652636718,
-            "heading": -3.21897487339828,
-            "angularVelocity": -6.375048107159433e-7,
-            "velocityX": -3.3408092013447153,
-            "velocityY": -1.774720176905592,
-            "timestamp": 2.6538600405402817
-        },
-        {
-            "x": 5.72404161684181,
-            "y": 1.9132773125800981,
-            "heading": -3.2812904960777876,
-            "angularVelocity": -0.8721702705344045,
-            "velocityX": -3.0449122283008005,
-            "velocityY": -1.5813303020905036,
-            "timestamp": 2.7253089591562363
-        },
-        {
-            "x": 5.540083719687043,
-            "y": 1.8151536354630486,
-            "heading": -3.418088909630078,
-            "angularVelocity": -1.9146323863713337,
-            "velocityX": -2.574677136023417,
-            "velocityY": -1.3733402691069942,
-            "timestamp": 2.796757877772191
-        },
-        {
-            "x": 5.393751164472601,
-            "y": 1.7369217513952075,
-            "heading": -3.5523873708884453,
-            "angularVelocity": -1.8796430213546256,
-            "velocityX": -2.048072357826006,
-            "velocityY": -1.0949344732596382,
-            "timestamp": 2.8682067963881455
-        },
-        {
-            "x": 5.284579981216215,
-            "y": 1.6784003858088632,
-            "heading": -3.6634662967687484,
-            "angularVelocity": -1.554662100328298,
-            "velocityX": -1.5279613095723332,
-            "velocityY": -0.8190657986344941,
-            "timestamp": 2.9396557150041
-        },
-        {
-            "x": 5.21211226324901,
-            "y": 1.6394523869599669,
-            "heading": -3.742196708881707,
-            "angularVelocity": -1.1019118782914785,
-            "velocityX": -1.0142591290490286,
-            "velocityY": -0.5451167016141149,
-            "timestamp": 3.0111046336200546
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -3.7831853071795862,
-            "angularVelocity": -0.5736769582023385,
-            "velocityX": -0.505427709031773,
-            "velocityY": -0.2722558624712981,
-            "timestamp": 3.0825535522360092
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -3.7831853071795862,
-            "angularVelocity": 0,
-            "velocityX": -1.863217485402065e-29,
-            "velocityY": -1.5751272024067454e-29,
-            "timestamp": 3.154002470851964
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S4.traj
deleted file mode 100644
index f8080ad6..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3 To S4.traj	
+++ /dev/null
@@ -1,382 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.352388442136136,
-            "y": 6.742171711440638,
-            "heading": -2.7776238211060527,
-            "angularVelocity": -0.340136208728432,
-            "velocityX": 0.6450666626176952,
-            "velocityY": -0.18258291758985748,
-            "timestamp": 0.08121399720716439
-        },
-        {
-            "x": 2.4572309209740357,
-            "y": 6.712518353067644,
-            "heading": -2.831291417681962,
-            "angularVelocity": -0.6608170810552816,
-            "velocityX": 1.290941000856364,
-            "velocityY": -0.3651262022858861,
-            "timestamp": 0.16242799441432879
-        },
-        {
-            "x": 2.614630940945627,
-            "y": 6.668043884281434,
-            "heading": -2.9082589809802553,
-            "angularVelocity": -0.9477130290857405,
-            "velocityX": 1.9380898045256882,
-            "velocityY": -0.5476207342969771,
-            "timestamp": 0.2436419916214932
-        },
-        {
-            "x": 2.824773268720236,
-            "y": 6.608764345103305,
-            "heading": -3.0025843751568475,
-            "angularVelocity": -1.1614425766389054,
-            "velocityX": 2.587513667614254,
-            "velocityY": -0.7299177631454756,
-            "timestamp": 0.32485598882865757
-        },
-        {
-            "x": 3.087924526957624,
-            "y": 6.534856635997473,
-            "heading": -3.094100877088787,
-            "angularVelocity": -1.1268562695832722,
-            "velocityX": 3.2402204950799356,
-            "velocityY": -0.9100365903255841,
-            "timestamp": 0.40606998603582195
-        },
-        {
-            "x": 3.383391154718911,
-            "y": 6.450664344423451,
-            "heading": -3.094100898976011,
-            "angularVelocity": -2.695006379530874e-7,
-            "velocityX": 3.638124435716056,
-            "velocityY": -1.036672180514684,
-            "timestamp": 0.48728398324298633
-        },
-        {
-            "x": 3.6788577741864907,
-            "y": 6.366472023742971,
-            "heading": -3.0941009208630392,
-            "angularVelocity": -2.694982209366716e-7,
-            "velocityX": 3.638124333594396,
-            "velocityY": -1.036672538906832,
-            "timestamp": 0.5684979804501508
-        },
-        {
-            "x": 3.9743243936540216,
-            "y": 6.282279703062319,
-            "heading": -3.094100942750067,
-            "angularVelocity": -2.694982225892601e-7,
-            "velocityX": 3.638124333593797,
-            "velocityY": -1.036672538908934,
-            "timestamp": 0.6497119776573151
-        },
-        {
-            "x": 4.269791013121552,
-            "y": 6.198087382381668,
-            "heading": -3.0941009646370947,
-            "angularVelocity": -2.694982224333276e-7,
-            "velocityX": 3.6381243335937974,
-            "velocityY": -1.0366725389089342,
-            "timestamp": 0.7309259748644795
-        },
-        {
-            "x": 4.565257632589082,
-            "y": 6.113895061701016,
-            "heading": -3.094100986524123,
-            "angularVelocity": -2.694982222034585e-7,
-            "velocityX": 3.6381243335937974,
-            "velocityY": -1.036672538908934,
-            "timestamp": 0.8121399720716439
-        },
-        {
-            "x": 4.860724252056613,
-            "y": 6.0297027410203645,
-            "heading": -3.0941010084111507,
-            "angularVelocity": -2.694982230736536e-7,
-            "velocityX": 3.638124333593797,
-            "velocityY": -1.0366725389089342,
-            "timestamp": 0.8933539692788083
-        },
-        {
-            "x": 5.156190871524144,
-            "y": 5.945510420339713,
-            "heading": -3.094101030298179,
-            "angularVelocity": -2.6949822300117995e-7,
-            "velocityX": 3.638124333593797,
-            "velocityY": -1.0366725389089342,
-            "timestamp": 0.9745679664859727
-        },
-        {
-            "x": 5.451657490991675,
-            "y": 5.861318099659061,
-            "heading": -3.0941010521852066,
-            "angularVelocity": -2.694982228196196e-7,
-            "velocityX": 3.638124333593797,
-            "velocityY": -1.0366725389089342,
-            "timestamp": 1.0557819636931371
-        },
-        {
-            "x": 5.747124110459206,
-            "y": 5.77712577897841,
-            "heading": -3.0941010740722343,
-            "angularVelocity": -2.694982236260836e-7,
-            "velocityX": 3.638124333593797,
-            "velocityY": -1.0366725389089344,
-            "timestamp": 1.1369959609003015
-        },
-        {
-            "x": 6.042590729926668,
-            "y": 5.69293345829752,
-            "heading": -3.0941010959592625,
-            "angularVelocity": -2.694982236351797e-7,
-            "velocityX": 3.6381243335929603,
-            "velocityY": -1.0366725389118696,
-            "timestamp": 1.218209958107466
-        },
-        {
-            "x": 6.338057337809956,
-            "y": 5.6087410969628495,
-            "heading": -3.0941011178462903,
-            "angularVelocity": -2.6949822192516605e-7,
-            "velocityX": 3.6381241909552937,
-            "velocityY": -1.0366730394879038,
-            "timestamp": 1.2994239553146303
-        },
-        {
-            "x": 6.63151320525297,
-            "y": 5.517787680923723,
-            "heading": -3.0941011398106846,
-            "angularVelocity": -2.704508467508134e-7,
-            "velocityX": 3.6133656455032592,
-            "velocityY": -1.1199229094611813,
-            "timestamp": 1.3806379525217947
-        },
-        {
-            "x": 6.904052184030547,
-            "y": 5.375973104754023,
-            "heading": -3.09410116345768,
-            "angularVelocity": -2.911689606888355e-7,
-            "velocityX": 3.355812891235069,
-            "velocityY": -1.7461839220887416,
-            "timestamp": 1.461851949728959
-        },
-        {
-            "x": 7.14694076900972,
-            "y": 5.1878401702780765,
-            "heading": -3.094101192989132,
-            "angularVelocity": -3.636251518218712e-7,
-            "velocityX": 2.9907231921877617,
-            "velocityY": -2.3165087416877084,
-            "timestamp": 1.5430659469361234
-        },
-        {
-            "x": 7.352395911467515,
-            "y": 4.959417222897554,
-            "heading": -3.0941012381435535,
-            "angularVelocity": -5.559930935407798e-7,
-            "velocityX": 2.529799659188236,
-            "velocityY": -2.812605649731368,
-            "timestamp": 1.6242799441432878
-        },
-        {
-            "x": 7.513833646479666,
-            "y": 4.698024306067166,
-            "heading": -3.094103186189668,
-            "angularVelocity": -0.00002398658095119272,
-            "velocityX": 1.9878067889975632,
-            "velocityY": -3.2185697764010213,
-            "timestamp": 1.7054939413504522
-        },
-        {
-            "x": 7.6238137890728055,
-            "y": 4.417795537290539,
-            "heading": -3.1108263182271356,
-            "angularVelocity": -0.20591440655693208,
-            "velocityX": 1.35420181708456,
-            "velocityY": -3.4504984166977444,
-            "timestamp": 1.7867079385576166
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.3984410331631951,
-            "velocityX": 0.6918291533072239,
-            "velocityY": -3.5806086055413933,
-            "timestamp": 1.867921935764781
-        },
-        {
-            "x": 7.6870591404547595,
-            "y": 3.8674277894608684,
-            "heading": -3.1773151030340356,
-            "angularVelocity": -0.47768106939784993,
-            "velocityX": 0.09879982223438938,
-            "velocityY": -3.63297605573316,
-            "timestamp": 1.9393708543807355
-        },
-        {
-            "x": 7.651588574491949,
-            "y": 3.608358714127091,
-            "heading": -3.205456863771594,
-            "angularVelocity": -0.39387245154381195,
-            "velocityX": -0.4964465053735158,
-            "velocityY": -3.625934168821804,
-            "timestamp": 2.0108197729966903
-        },
-        {
-            "x": 7.574047848742119,
-            "y": 3.354318466237311,
-            "heading": -3.2189736743217594,
-            "angularVelocity": -0.1891814573314478,
-            "velocityX": -1.0852610123268396,
-            "velocityY": -3.5555506341084104,
-            "timestamp": 2.082268691612645
-        },
-        {
-            "x": 7.455586952035965,
-            "y": 3.1113741995114603,
-            "heading": -3.2189745116430255,
-            "angularVelocity": -0.00001171915938609546,
-            "velocityX": -1.6579802605551721,
-            "velocityY": -3.4002511365232904,
-            "timestamp": 2.1537176102285995
-        },
-        {
-            "x": 7.300453960582251,
-            "y": 2.8900400447662866,
-            "heading": -3.2189745885172623,
-            "angularVelocity": -0.0000010759328215916604,
-            "velocityX": -2.171243378574256,
-            "velocityY": -3.097795726435581,
-            "timestamp": 2.225166528844554
-        },
-        {
-            "x": 7.11249649568723,
-            "y": 2.6958055218428347,
-            "heading": -3.2189746440183837,
-            "angularVelocity": -7.767944182730611e-7,
-            "velocityX": -2.6306551384378265,
-            "velocityY": -2.718508924810283,
-            "timestamp": 2.2966154474605087
-        },
-        {
-            "x": 6.896376183100963,
-            "y": 2.533487712060768,
-            "heading": -3.2189746912030057,
-            "angularVelocity": -6.603965892997474e-7,
-            "velocityX": -3.024822723297278,
-            "velocityY": -2.2718021899927887,
-            "timestamp": 2.3680643660764633
-        },
-        {
-            "x": 6.6576888713401265,
-            "y": 2.4066672511412732,
-            "heading": -3.218974736751788,
-            "angularVelocity": -6.375013452668414e-7,
-            "velocityX": -3.340670739366957,
-            "velocityY": -1.7749808302282493,
-            "timestamp": 2.439513284692418
-        },
-        {
-            "x": 6.418991689499422,
-            "y": 2.2798653705517777,
-            "heading": -3.2189747823005193,
-            "angularVelocity": -6.375006445255001e-7,
-            "velocityX": -3.3408088808494862,
-            "velocityY": -1.7747207802229084,
-            "timestamp": 2.5109622033083725
-        },
-        {
-            "x": 6.180294507575015,
-            "y": 2.1530634901198433,
-            "heading": -3.2189748278492507,
-            "angularVelocity": -6.375006422464704e-7,
-            "velocityX": -3.340808882020978,
-            "velocityY": -1.7747207780176886,
-            "timestamp": 2.582411121924327
-        },
-        {
-            "x": 5.941597302835271,
-            "y": 2.026261652636718,
-            "heading": -3.21897487339828,
-            "angularVelocity": -6.375048107159433e-7,
-            "velocityX": -3.3408092013447153,
-            "velocityY": -1.774720176905592,
-            "timestamp": 2.6538600405402817
-        },
-        {
-            "x": 5.72404161684181,
-            "y": 1.9132773125800981,
-            "heading": -3.2812904960777876,
-            "angularVelocity": -0.8721702705344045,
-            "velocityX": -3.0449122283008005,
-            "velocityY": -1.5813303020905036,
-            "timestamp": 2.7253089591562363
-        },
-        {
-            "x": 5.540083719687043,
-            "y": 1.8151536354630486,
-            "heading": -3.418088909630078,
-            "angularVelocity": -1.9146323863713337,
-            "velocityX": -2.574677136023417,
-            "velocityY": -1.3733402691069942,
-            "timestamp": 2.796757877772191
-        },
-        {
-            "x": 5.393751164472601,
-            "y": 1.7369217513952075,
-            "heading": -3.5523873708884453,
-            "angularVelocity": -1.8796430213546256,
-            "velocityX": -2.048072357826006,
-            "velocityY": -1.0949344732596382,
-            "timestamp": 2.8682067963881455
-        },
-        {
-            "x": 5.284579981216215,
-            "y": 1.6784003858088632,
-            "heading": -3.6634662967687484,
-            "angularVelocity": -1.554662100328298,
-            "velocityX": -1.5279613095723332,
-            "velocityY": -0.8190657986344941,
-            "timestamp": 2.9396557150041
-        },
-        {
-            "x": 5.21211226324901,
-            "y": 1.6394523869599669,
-            "heading": -3.742196708881707,
-            "angularVelocity": -1.1019118782914785,
-            "velocityX": -1.0142591290490286,
-            "velocityY": -0.5451167016141149,
-            "timestamp": 3.0111046336200546
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -3.7831853071795862,
-            "angularVelocity": -0.5736769582023385,
-            "velocityX": -0.505427709031773,
-            "velocityY": -0.2722558624712981,
-            "timestamp": 3.0825535522360092
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -3.7831853071795862,
-            "angularVelocity": 0,
-            "velocityX": -1.863217485402065e-29,
-            "velocityY": -1.5751272024067454e-29,
-            "timestamp": 3.154002470851964
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.1.traj
deleted file mode 100644
index 8e3875a1..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.1.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2980013304879616,
-            "y": 6.709118488011682,
-            "heading": -2.76564785007466,
-            "angularVelocity": -0.2059742190979395,
-            "velocityX": -0.026308687136742944,
-            "velocityY": -0.6302691420205845,
-            "timestamp": 0.07596994489499287
-        },
-        {
-            "x": 2.294004010549845,
-            "y": 6.6133557271698225,
-            "heading": -2.796958855018623,
-            "angularVelocity": -0.4121498967419566,
-            "velocityX": -0.05261712304308625,
-            "velocityY": -1.2605348203717124,
-            "timestamp": 0.15193988978998574
-        },
-        {
-            "x": 2.2880079544710785,
-            "y": 6.469712199120552,
-            "heading": -2.8439591282551087,
-            "angularVelocity": -0.618669307993449,
-            "velocityX": -0.07892668721893806,
-            "velocityY": -1.8907941587665607,
-            "timestamp": 0.2279098346849786
-        },
-        {
-            "x": 2.2800128266474267,
-            "y": 6.278188582998367,
-            "heading": -2.906683998413123,
-            "angularVelocity": -0.8256537535299532,
-            "velocityX": -0.10524066898696208,
-            "velocityY": -2.5210445576459737,
-            "timestamp": 0.3038797795799715
-        },
-        {
-            "x": 2.27001785393274,
-            "y": 6.038785706839934,
-            "heading": -2.985174219418862,
-            "angularVelocity": -1.0331746470822023,
-            "velocityX": -0.13156482775526773,
-            "velocityY": -3.151284056995696,
-            "timestamp": 0.37984972447496435
-        },
-        {
-            "x": 2.2620112641506833,
-            "y": 5.847271276792759,
-            "heading": -3.048371938278793,
-            "angularVelocity": -0.8318779083923785,
-            "velocityX": -0.10539154389440716,
-            "velocityY": -2.5209236404197743,
-            "timestamp": 0.4558196693699572
-        },
-        {
-            "x": 2.256005772930322,
-            "y": 5.703635612537123,
-            "heading": -3.0957774424626403,
-            "angularVelocity": -0.6240034035745593,
-            "velocityX": -0.07905088293353696,
-            "velocityY": -1.8906906468626767,
-            "timestamp": 0.5317896142649501
-        },
-        {
-            "x": 2.25200192771716,
-            "y": 5.607878538669833,
-            "heading": -3.127382702013982,
-            "angularVelocity": -0.4160231996354264,
-            "velocityX": -0.05270301589261432,
-            "velocityY": -1.2604599621553931,
-            "timestamp": 0.607759559159943
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.20801127587293633,
-            "velocityX": -0.026351575217374957,
-            "velocityY": -0.6302300039313168,
-            "timestamp": 0.6837295040549358
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": 2.0416042206069368e-34,
-            "velocityX": 0,
-            "velocityY": 6.898965298259951e-35,
-            "timestamp": 0.7596994489499287
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.traj
deleted file mode 100644
index 8e3875a1..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2980013304879616,
-            "y": 6.709118488011682,
-            "heading": -2.76564785007466,
-            "angularVelocity": -0.2059742190979395,
-            "velocityX": -0.026308687136742944,
-            "velocityY": -0.6302691420205845,
-            "timestamp": 0.07596994489499287
-        },
-        {
-            "x": 2.294004010549845,
-            "y": 6.6133557271698225,
-            "heading": -2.796958855018623,
-            "angularVelocity": -0.4121498967419566,
-            "velocityX": -0.05261712304308625,
-            "velocityY": -1.2605348203717124,
-            "timestamp": 0.15193988978998574
-        },
-        {
-            "x": 2.2880079544710785,
-            "y": 6.469712199120552,
-            "heading": -2.8439591282551087,
-            "angularVelocity": -0.618669307993449,
-            "velocityX": -0.07892668721893806,
-            "velocityY": -1.8907941587665607,
-            "timestamp": 0.2279098346849786
-        },
-        {
-            "x": 2.2800128266474267,
-            "y": 6.278188582998367,
-            "heading": -2.906683998413123,
-            "angularVelocity": -0.8256537535299532,
-            "velocityX": -0.10524066898696208,
-            "velocityY": -2.5210445576459737,
-            "timestamp": 0.3038797795799715
-        },
-        {
-            "x": 2.27001785393274,
-            "y": 6.038785706839934,
-            "heading": -2.985174219418862,
-            "angularVelocity": -1.0331746470822023,
-            "velocityX": -0.13156482775526773,
-            "velocityY": -3.151284056995696,
-            "timestamp": 0.37984972447496435
-        },
-        {
-            "x": 2.2620112641506833,
-            "y": 5.847271276792759,
-            "heading": -3.048371938278793,
-            "angularVelocity": -0.8318779083923785,
-            "velocityX": -0.10539154389440716,
-            "velocityY": -2.5209236404197743,
-            "timestamp": 0.4558196693699572
-        },
-        {
-            "x": 2.256005772930322,
-            "y": 5.703635612537123,
-            "heading": -3.0957774424626403,
-            "angularVelocity": -0.6240034035745593,
-            "velocityX": -0.07905088293353696,
-            "velocityY": -1.8906906468626767,
-            "timestamp": 0.5317896142649501
-        },
-        {
-            "x": 2.25200192771716,
-            "y": 5.607878538669833,
-            "heading": -3.127382702013982,
-            "angularVelocity": -0.4160231996354264,
-            "velocityX": -0.05270301589261432,
-            "velocityY": -1.2604599621553931,
-            "timestamp": 0.607759559159943
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.20801127587293633,
-            "velocityX": -0.026351575217374957,
-            "velocityY": -0.6302300039313168,
-            "timestamp": 0.6837295040549358
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": 2.0416042206069368e-34,
-            "velocityX": 0,
-            "velocityY": 6.898965298259951e-35,
-            "timestamp": 0.7596994489499287
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S1.1.traj
deleted file mode 100644
index ce6e1914..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S1.1.traj	
+++ /dev/null
@@ -1,310 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.329045295715332,
-            "y": 5.559675216674805,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.4000279527631365,
-            "y": 5.585359375509262,
-            "heading": 3.1338545708828756,
-            "angularVelocity": -0.06457961815077011,
-            "velocityX": 0.745925597725292,
-            "velocityY": 0.2699035557060694,
-            "timestamp": 0.09516050563794304
-        },
-        {
-            "x": 2.5419932646198635,
-            "y": 5.636727692199652,
-            "heading": 3.1215638736263545,
-            "angularVelocity": -0.12915754465735657,
-            "velocityX": 1.4918511719231908,
-            "velocityY": 0.539807101129259,
-            "timestamp": 0.19032101127588608
-        },
-        {
-            "x": 2.7549412265998368,
-            "y": 5.713780164921229,
-            "heading": 3.103128182719956,
-            "angularVelocity": -0.1937325866743065,
-            "velocityX": 2.2377766968814634,
-            "velocityY": 0.8097106273770546,
-            "timestamp": 0.2854815169138291
-        },
-        {
-            "x": 3.038871822724849,
-            "y": 5.81651678795237,
-            "heading": 3.078547903585146,
-            "angularVelocity": -0.258303368295786,
-            "velocityX": 2.9837020539320758,
-            "velocityY": 1.0796140934988998,
-            "timestamp": 0.38064202255177215
-        },
-        {
-            "x": 3.3773799452774673,
-            "y": 5.939001713582426,
-            "heading": 3.0785479006839798,
-            "angularVelocity": -3.048712788248211e-8,
-            "velocityX": 3.5572333320775313,
-            "velocityY": 1.2871403405116655,
-            "timestamp": 0.4758025281897152
-        },
-        {
-            "x": 3.715888067817609,
-            "y": 6.0614866392475255,
-            "heading": 3.0785478977827805,
-            "angularVelocity": -3.0487407415354604e-8,
-            "velocityX": 3.5572333319465472,
-            "velocityY": 1.287140340879956,
-            "timestamp": 0.5709630338276582
-        },
-        {
-            "x": 4.054396190357754,
-            "y": 6.183971564912614,
-            "heading": 3.0785478948815816,
-            "angularVelocity": -3.0487407316957414e-8,
-            "velocityX": 3.5572333319465868,
-            "velocityY": 1.28714034087985,
-            "timestamp": 0.6661235394656013
-        },
-        {
-            "x": 4.392904312897899,
-            "y": 6.306456490577704,
-            "heading": 3.0785478919803824,
-            "angularVelocity": -3.048740785433711e-8,
-            "velocityX": 3.557233331946584,
-            "velocityY": 1.287140340879858,
-            "timestamp": 0.7612840451035443
-        },
-        {
-            "x": 4.7314124354380445,
-            "y": 6.428941416242792,
-            "heading": 3.0785478890791835,
-            "angularVelocity": -3.0487408354838206e-8,
-            "velocityX": 3.557233331946584,
-            "velocityY": 1.2871403408798578,
-            "timestamp": 0.8564445507414873
-        },
-        {
-            "x": 5.06992055797819,
-            "y": 6.551426341907881,
-            "heading": 3.0785478861779842,
-            "angularVelocity": -3.04874074079248e-8,
-            "velocityX": 3.557233331946584,
-            "velocityY": 1.2871403408798578,
-            "timestamp": 0.9516050563794303
-        },
-        {
-            "x": 5.408428680518335,
-            "y": 6.67391126757297,
-            "heading": 3.0785478832767854,
-            "angularVelocity": -3.048740819605906e-8,
-            "velocityX": 3.557233331946584,
-            "velocityY": 1.2871403408798576,
-            "timestamp": 1.0467655620173733
-        },
-        {
-            "x": 5.746936803058481,
-            "y": 6.796396193238057,
-            "heading": 3.078547880375586,
-            "angularVelocity": -3.0487408175601846e-8,
-            "velocityX": 3.5572333319465934,
-            "velocityY": 1.2871403408798332,
-            "timestamp": 1.1419260676553162
-        },
-        {
-            "x": 6.085444925598784,
-            "y": 6.918881118902711,
-            "heading": 3.0785478774743873,
-            "angularVelocity": -3.0487407710602e-8,
-            "velocityX": 3.55723333194824,
-            "velocityY": 1.2871403408752817,
-            "timestamp": 1.2370865732932592
-        },
-        {
-            "x": 6.4239530481377205,
-            "y": 7.041366044571132,
-            "heading": 3.078547874573188,
-            "angularVelocity": -3.0487407747049345e-8,
-            "velocityX": 3.55723333193393,
-            "velocityY": 1.287140340914726,
-            "timestamp": 1.3322470789312022
-        },
-        {
-            "x": 6.762461177860979,
-            "y": 7.163850950384819,
-            "heading": 3.078547871671963,
-            "angularVelocity": -3.0487683312908684e-8,
-            "velocityX": 3.5572334074304637,
-            "velocityY": 1.2871401322709841,
-            "timestamp": 1.4274075845691452
-        },
-        {
-            "x": 7.087934228336467,
-            "y": 7.275849077547301,
-            "heading": 3.0785482003586213,
-            "angularVelocity": 0.000003454027869296417,
-            "velocityX": 3.420253479039086,
-            "velocityY": 1.1769391767257837,
-            "timestamp": 1.5225680902070882
-        },
-        {
-            "x": 7.34242464824946,
-            "y": 7.362163048068745,
-            "heading": 3.0846933316902727,
-            "angularVelocity": 0.06457648885540108,
-            "velocityX": 2.6743281596359925,
-            "velocityY": 0.9070356440694888,
-            "timestamp": 1.6177285958450311
-        },
-        {
-            "x": 7.525932419508426,
-            "y": 7.422792858981067,
-            "heading": 3.0969834578057935,
-            "angularVelocity": 0.12915154278707255,
-            "velocityX": 1.9284026501171228,
-            "velocityY": 0.6371320802228948,
-            "timestamp": 1.7128891014829741
-        },
-        {
-            "x": 7.638457537165259,
-            "y": 7.457738509668789,
-            "heading": 3.1154188790536077,
-            "angularVelocity": 0.1937297529488966,
-            "velocityX": 1.1824770886008305,
-            "velocityY": 0.3672285099084715,
-            "timestamp": 1.8080496071209171
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 0.2583122145204678,
-            "velocityX": 0.4365515142645689,
-            "velocityY": 0.09732493820788266,
-            "timestamp": 1.90321011275886
-        },
-        {
-            "x": 7.653654383343035,
-            "y": 7.451878922022455,
-            "heading": 3.169571245622926,
-            "angularVelocity": 0.3208641076607671,
-            "velocityX": -0.28586427797403835,
-            "velocityY": -0.16407192492517308,
-            "timestamp": 1.9953713888811428
-        },
-        {
-            "x": 7.560730009307251,
-            "y": 7.412667176131766,
-            "heading": 3.204907797708199,
-            "angularVelocity": 0.3834208202531959,
-            "velocityX": -1.0082800276276813,
-            "velocityY": -0.42546878190023074,
-            "timestamp": 2.0875326650034256
-        },
-        {
-            "x": 7.4012268833190475,
-            "y": 7.349364762932986,
-            "heading": 3.2460101421492427,
-            "angularVelocity": 0.4459828050518832,
-            "velocityX": -1.7306957184018388,
-            "velocityY": -0.6868656323100164,
-            "timestamp": 2.1796939411257084
-        },
-        {
-            "x": 7.175145013744201,
-            "y": 7.261971683800118,
-            "heading": 3.292878761166739,
-            "angularVelocity": 0.5085500222039014,
-            "velocityX": -2.4531113184025837,
-            "velocityY": -0.9482624678108277,
-            "timestamp": 2.271855217247991
-        },
-        {
-            "x": 6.8824844194459525,
-            "y": 7.150487945420426,
-            "heading": 3.3455140401053294,
-            "angularVelocity": 0.5711214205450821,
-            "velocityX": -3.1755267137264047,
-            "velocityY": -1.2096592307504261,
-            "timestamp": 2.364016493370274
-        },
-        {
-            "x": 6.55464527272117,
-            "y": 7.031863403145239,
-            "heading": 3.3455140429153563,
-            "angularVelocity": 3.049000281588377e-8,
-            "velocityX": -3.557233151692005,
-            "velocityY": -1.2871408390477628,
-            "timestamp": 2.4561777694925566
-        },
-        {
-            "x": 6.226806115184277,
-            "y": 6.913238890751769,
-            "heading": 3.3455140457253494,
-            "angularVelocity": 3.0489828937045393e-8,
-            "velocityX": -3.557233269011331,
-            "velocityY": -1.287140514808609,
-            "timestamp": 2.5483390456148394
-        },
-        {
-            "x": 5.898966957623017,
-            "y": 6.79461437842809,
-            "heading": 3.345514048537748,
-            "angularVelocity": 3.0514995916000634e-8,
-            "velocityX": -3.557233269277337,
-            "velocityY": -1.287140514052891,
-            "timestamp": 2.640500321737122
-        },
-        {
-            "x": 5.632652017742471,
-            "y": 6.698251673595034,
-            "heading": 3.3685823941434916,
-            "angularVelocity": 0.2503041035574726,
-            "velocityX": -2.889662026024368,
-            "velocityY": -1.045587787926248,
-            "timestamp": 2.732661597859405
-        },
-        {
-            "x": 5.432915797517009,
-            "y": 6.625979638346594,
-            "heading": 3.3858838135535563,
-            "angularVelocity": 0.18772981600595268,
-            "velocityX": -2.167246685696753,
-            "velocityY": -0.7841909128155133,
-            "timestamp": 2.8248228739816876
-        },
-        {
-            "x": 5.299758312935604,
-            "y": 6.577798279546128,
-            "heading": 3.3974181385441695,
-            "angularVelocity": 0.1251537031151884,
-            "velocityX": -1.4448311718780615,
-            "velocityY": -0.5227939632329148,
-            "timestamp": 2.9169841501039704
-        },
-        {
-            "x": 5.233179569244385,
-            "y": 6.553707599639893,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.06257691818239948,
-            "velocityX": -0.7224156011355166,
-            "velocityY": -0.2613969871068907,
-            "timestamp": 3.009145426226253
-        },
-        {
-            "x": 5.233179569244385,
-            "y": 6.553707599639893,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0,
-            "velocityX": 1.5847972099366162e-31,
-            "velocityY": 0,
-            "timestamp": 3.101306702348536
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S1.traj
deleted file mode 100644
index ce6e1914..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S1.traj	
+++ /dev/null
@@ -1,310 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.329045295715332,
-            "y": 5.559675216674805,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.4000279527631365,
-            "y": 5.585359375509262,
-            "heading": 3.1338545708828756,
-            "angularVelocity": -0.06457961815077011,
-            "velocityX": 0.745925597725292,
-            "velocityY": 0.2699035557060694,
-            "timestamp": 0.09516050563794304
-        },
-        {
-            "x": 2.5419932646198635,
-            "y": 5.636727692199652,
-            "heading": 3.1215638736263545,
-            "angularVelocity": -0.12915754465735657,
-            "velocityX": 1.4918511719231908,
-            "velocityY": 0.539807101129259,
-            "timestamp": 0.19032101127588608
-        },
-        {
-            "x": 2.7549412265998368,
-            "y": 5.713780164921229,
-            "heading": 3.103128182719956,
-            "angularVelocity": -0.1937325866743065,
-            "velocityX": 2.2377766968814634,
-            "velocityY": 0.8097106273770546,
-            "timestamp": 0.2854815169138291
-        },
-        {
-            "x": 3.038871822724849,
-            "y": 5.81651678795237,
-            "heading": 3.078547903585146,
-            "angularVelocity": -0.258303368295786,
-            "velocityX": 2.9837020539320758,
-            "velocityY": 1.0796140934988998,
-            "timestamp": 0.38064202255177215
-        },
-        {
-            "x": 3.3773799452774673,
-            "y": 5.939001713582426,
-            "heading": 3.0785479006839798,
-            "angularVelocity": -3.048712788248211e-8,
-            "velocityX": 3.5572333320775313,
-            "velocityY": 1.2871403405116655,
-            "timestamp": 0.4758025281897152
-        },
-        {
-            "x": 3.715888067817609,
-            "y": 6.0614866392475255,
-            "heading": 3.0785478977827805,
-            "angularVelocity": -3.0487407415354604e-8,
-            "velocityX": 3.5572333319465472,
-            "velocityY": 1.287140340879956,
-            "timestamp": 0.5709630338276582
-        },
-        {
-            "x": 4.054396190357754,
-            "y": 6.183971564912614,
-            "heading": 3.0785478948815816,
-            "angularVelocity": -3.0487407316957414e-8,
-            "velocityX": 3.5572333319465868,
-            "velocityY": 1.28714034087985,
-            "timestamp": 0.6661235394656013
-        },
-        {
-            "x": 4.392904312897899,
-            "y": 6.306456490577704,
-            "heading": 3.0785478919803824,
-            "angularVelocity": -3.048740785433711e-8,
-            "velocityX": 3.557233331946584,
-            "velocityY": 1.287140340879858,
-            "timestamp": 0.7612840451035443
-        },
-        {
-            "x": 4.7314124354380445,
-            "y": 6.428941416242792,
-            "heading": 3.0785478890791835,
-            "angularVelocity": -3.0487408354838206e-8,
-            "velocityX": 3.557233331946584,
-            "velocityY": 1.2871403408798578,
-            "timestamp": 0.8564445507414873
-        },
-        {
-            "x": 5.06992055797819,
-            "y": 6.551426341907881,
-            "heading": 3.0785478861779842,
-            "angularVelocity": -3.04874074079248e-8,
-            "velocityX": 3.557233331946584,
-            "velocityY": 1.2871403408798578,
-            "timestamp": 0.9516050563794303
-        },
-        {
-            "x": 5.408428680518335,
-            "y": 6.67391126757297,
-            "heading": 3.0785478832767854,
-            "angularVelocity": -3.048740819605906e-8,
-            "velocityX": 3.557233331946584,
-            "velocityY": 1.2871403408798576,
-            "timestamp": 1.0467655620173733
-        },
-        {
-            "x": 5.746936803058481,
-            "y": 6.796396193238057,
-            "heading": 3.078547880375586,
-            "angularVelocity": -3.0487408175601846e-8,
-            "velocityX": 3.5572333319465934,
-            "velocityY": 1.2871403408798332,
-            "timestamp": 1.1419260676553162
-        },
-        {
-            "x": 6.085444925598784,
-            "y": 6.918881118902711,
-            "heading": 3.0785478774743873,
-            "angularVelocity": -3.0487407710602e-8,
-            "velocityX": 3.55723333194824,
-            "velocityY": 1.2871403408752817,
-            "timestamp": 1.2370865732932592
-        },
-        {
-            "x": 6.4239530481377205,
-            "y": 7.041366044571132,
-            "heading": 3.078547874573188,
-            "angularVelocity": -3.0487407747049345e-8,
-            "velocityX": 3.55723333193393,
-            "velocityY": 1.287140340914726,
-            "timestamp": 1.3322470789312022
-        },
-        {
-            "x": 6.762461177860979,
-            "y": 7.163850950384819,
-            "heading": 3.078547871671963,
-            "angularVelocity": -3.0487683312908684e-8,
-            "velocityX": 3.5572334074304637,
-            "velocityY": 1.2871401322709841,
-            "timestamp": 1.4274075845691452
-        },
-        {
-            "x": 7.087934228336467,
-            "y": 7.275849077547301,
-            "heading": 3.0785482003586213,
-            "angularVelocity": 0.000003454027869296417,
-            "velocityX": 3.420253479039086,
-            "velocityY": 1.1769391767257837,
-            "timestamp": 1.5225680902070882
-        },
-        {
-            "x": 7.34242464824946,
-            "y": 7.362163048068745,
-            "heading": 3.0846933316902727,
-            "angularVelocity": 0.06457648885540108,
-            "velocityX": 2.6743281596359925,
-            "velocityY": 0.9070356440694888,
-            "timestamp": 1.6177285958450311
-        },
-        {
-            "x": 7.525932419508426,
-            "y": 7.422792858981067,
-            "heading": 3.0969834578057935,
-            "angularVelocity": 0.12915154278707255,
-            "velocityX": 1.9284026501171228,
-            "velocityY": 0.6371320802228948,
-            "timestamp": 1.7128891014829741
-        },
-        {
-            "x": 7.638457537165259,
-            "y": 7.457738509668789,
-            "heading": 3.1154188790536077,
-            "angularVelocity": 0.1937297529488966,
-            "velocityX": 1.1824770886008305,
-            "velocityY": 0.3672285099084715,
-            "timestamp": 1.8080496071209171
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 0.2583122145204678,
-            "velocityX": 0.4365515142645689,
-            "velocityY": 0.09732493820788266,
-            "timestamp": 1.90321011275886
-        },
-        {
-            "x": 7.653654383343035,
-            "y": 7.451878922022455,
-            "heading": 3.169571245622926,
-            "angularVelocity": 0.3208641076607671,
-            "velocityX": -0.28586427797403835,
-            "velocityY": -0.16407192492517308,
-            "timestamp": 1.9953713888811428
-        },
-        {
-            "x": 7.560730009307251,
-            "y": 7.412667176131766,
-            "heading": 3.204907797708199,
-            "angularVelocity": 0.3834208202531959,
-            "velocityX": -1.0082800276276813,
-            "velocityY": -0.42546878190023074,
-            "timestamp": 2.0875326650034256
-        },
-        {
-            "x": 7.4012268833190475,
-            "y": 7.349364762932986,
-            "heading": 3.2460101421492427,
-            "angularVelocity": 0.4459828050518832,
-            "velocityX": -1.7306957184018388,
-            "velocityY": -0.6868656323100164,
-            "timestamp": 2.1796939411257084
-        },
-        {
-            "x": 7.175145013744201,
-            "y": 7.261971683800118,
-            "heading": 3.292878761166739,
-            "angularVelocity": 0.5085500222039014,
-            "velocityX": -2.4531113184025837,
-            "velocityY": -0.9482624678108277,
-            "timestamp": 2.271855217247991
-        },
-        {
-            "x": 6.8824844194459525,
-            "y": 7.150487945420426,
-            "heading": 3.3455140401053294,
-            "angularVelocity": 0.5711214205450821,
-            "velocityX": -3.1755267137264047,
-            "velocityY": -1.2096592307504261,
-            "timestamp": 2.364016493370274
-        },
-        {
-            "x": 6.55464527272117,
-            "y": 7.031863403145239,
-            "heading": 3.3455140429153563,
-            "angularVelocity": 3.049000281588377e-8,
-            "velocityX": -3.557233151692005,
-            "velocityY": -1.2871408390477628,
-            "timestamp": 2.4561777694925566
-        },
-        {
-            "x": 6.226806115184277,
-            "y": 6.913238890751769,
-            "heading": 3.3455140457253494,
-            "angularVelocity": 3.0489828937045393e-8,
-            "velocityX": -3.557233269011331,
-            "velocityY": -1.287140514808609,
-            "timestamp": 2.5483390456148394
-        },
-        {
-            "x": 5.898966957623017,
-            "y": 6.79461437842809,
-            "heading": 3.345514048537748,
-            "angularVelocity": 3.0514995916000634e-8,
-            "velocityX": -3.557233269277337,
-            "velocityY": -1.287140514052891,
-            "timestamp": 2.640500321737122
-        },
-        {
-            "x": 5.632652017742471,
-            "y": 6.698251673595034,
-            "heading": 3.3685823941434916,
-            "angularVelocity": 0.2503041035574726,
-            "velocityX": -2.889662026024368,
-            "velocityY": -1.045587787926248,
-            "timestamp": 2.732661597859405
-        },
-        {
-            "x": 5.432915797517009,
-            "y": 6.625979638346594,
-            "heading": 3.3858838135535563,
-            "angularVelocity": 0.18772981600595268,
-            "velocityX": -2.167246685696753,
-            "velocityY": -0.7841909128155133,
-            "timestamp": 2.8248228739816876
-        },
-        {
-            "x": 5.299758312935604,
-            "y": 6.577798279546128,
-            "heading": 3.3974181385441695,
-            "angularVelocity": 0.1251537031151884,
-            "velocityX": -1.4448311718780615,
-            "velocityY": -0.5227939632329148,
-            "timestamp": 2.9169841501039704
-        },
-        {
-            "x": 5.233179569244385,
-            "y": 6.553707599639893,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.06257691818239948,
-            "velocityX": -0.7224156011355166,
-            "velocityY": -0.2613969871068907,
-            "timestamp": 3.009145426226253
-        },
-        {
-            "x": 5.233179569244385,
-            "y": 6.553707599639893,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0,
-            "velocityX": 1.5847972099366162e-31,
-            "velocityY": 0,
-            "timestamp": 3.101306702348536
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S2.1.traj
deleted file mode 100644
index 42ce41bc..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S2.1.traj	
+++ /dev/null
@@ -1,328 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3199317069050815,
-            "y": 5.589221584324863,
-            "heading": 3.14,
-            "angularVelocity": -4.40794531804863e-17,
-            "velocityX": 0.7334971919039711,
-            "velocityY": 0.3064983109073157,
-            "timestamp": 0.09534011537734256
-        },
-        {
-            "x": 2.4597951178135684,
-            "y": 5.647664751762101,
-            "heading": 3.14,
-            "angularVelocity": -2.0051811551543008e-16,
-            "velocityX": 1.4669943533729506,
-            "velocityY": 0.6129966090971003,
-            "timestamp": 0.19068023075468513
-        },
-        {
-            "x": 2.6695902269221126,
-            "y": 5.73532949988673,
-            "heading": 3.14,
-            "angularVelocity": -1.6298734307861892e-16,
-            "velocityX": 2.200491453971973,
-            "velocityY": 0.9194948818518327,
-            "timestamp": 0.2860203461320277
-        },
-        {
-            "x": 2.9493170168206886,
-            "y": 5.852215821423819,
-            "heading": 3.14,
-            "angularVelocity": -2.718821323245932e-16,
-            "velocityX": 2.933988371961343,
-            "velocityY": 1.2259930783015016,
-            "timestamp": 0.38136046150937025
-        },
-        {
-            "x": 3.2820984406189933,
-            "y": 5.991271492558365,
-            "heading": 3.14,
-            "angularVelocity": -1.791056538237388e-18,
-            "velocityX": 3.4904659227776635,
-            "velocityY": 1.45852216125588,
-            "timestamp": 0.4767005768867128
-        },
-        {
-            "x": 3.6148798644173543,
-            "y": 6.130327163692935,
-            "heading": 3.14,
-            "angularVelocity": 2.7598941425139603e-16,
-            "velocityX": 3.4904659227782475,
-            "velocityY": 1.4585221612561248,
-            "timestamp": 0.5720406922640554
-        },
-        {
-            "x": 3.947661288215715,
-            "y": 6.269382834827505,
-            "heading": 3.14,
-            "angularVelocity": -1.359472794095135e-16,
-            "velocityX": 3.4904659227782475,
-            "velocityY": 1.4585221612561248,
-            "timestamp": 0.667380807641398
-        },
-        {
-            "x": 4.280442712014075,
-            "y": 6.408438505962075,
-            "heading": 3.14,
-            "angularVelocity": -3.500332184235888e-16,
-            "velocityX": 3.4904659227782475,
-            "velocityY": 1.4585221612561245,
-            "timestamp": 0.7627209230187405
-        },
-        {
-            "x": 4.613224135812436,
-            "y": 6.547494177096645,
-            "heading": 3.14,
-            "angularVelocity": -3.2849392470850763e-16,
-            "velocityX": 3.4904659227782475,
-            "velocityY": 1.4585221612561245,
-            "timestamp": 0.858061038396083
-        },
-        {
-            "x": 4.9460055596107955,
-            "y": 6.686549848231215,
-            "heading": 3.14,
-            "angularVelocity": -3.264382961483678e-16,
-            "velocityX": 3.4904659227782475,
-            "velocityY": 1.4585221612561245,
-            "timestamp": 0.9534011537734256
-        },
-        {
-            "x": 5.278786983409157,
-            "y": 6.825605519365785,
-            "heading": 3.14,
-            "angularVelocity": -2.8870332355952577e-16,
-            "velocityX": 3.4904659227782475,
-            "velocityY": 1.4585221612561245,
-            "timestamp": 1.0487412691507683
-        },
-        {
-            "x": 5.611568407207518,
-            "y": 6.9646611905003555,
-            "heading": 3.14,
-            "angularVelocity": -2.714509339961268e-16,
-            "velocityX": 3.4904659227782475,
-            "velocityY": 1.4585221612561245,
-            "timestamp": 1.1440813845281108
-        },
-        {
-            "x": 5.944349831005879,
-            "y": 7.1037168616349256,
-            "heading": 3.14,
-            "angularVelocity": 3.236789006032357e-16,
-            "velocityX": 3.4904659227782475,
-            "velocityY": 1.4585221612561248,
-            "timestamp": 1.2394214999054534
-        },
-        {
-            "x": 6.2771312548052975,
-            "y": 7.242772532766961,
-            "heading": 3.14,
-            "angularVelocity": 1.392859578179056e-16,
-            "velocityX": 3.4904659227893537,
-            "velocityY": 1.458522161229545,
-            "timestamp": 1.334761615282796
-        },
-        {
-            "x": 6.609912972036511,
-            "y": 7.381827501669921,
-            "heading": 3.14,
-            "angularVelocity": 7.280552315335093e-17,
-            "velocityX": 3.4904690005262737,
-            "velocityY": 1.4585147957142848,
-            "timestamp": 1.4301017306601385
-        },
-        {
-            "x": 6.961412726724582,
-            "y": 7.462622346034388,
-            "heading": 3.14,
-            "angularVelocity": -1.0172483638053557e-16,
-            "velocityX": 3.6867980838588856,
-            "velocityY": 0.8474380804417183,
-            "timestamp": 1.525441846037481
-        },
-        {
-            "x": 7.244997116389008,
-            "y": 7.50977433975784,
-            "heading": 3.14,
-            "angularVelocity": -1.1360407734774875e-16,
-            "velocityX": 2.974449826728655,
-            "velocityY": 0.4945661491684764,
-            "timestamp": 1.6207819614148236
-        },
-        {
-            "x": 7.459465375503974,
-            "y": 7.525824834495841,
-            "heading": 3.14,
-            "angularVelocity": -6.302038440751775e-17,
-            "velocityX": 2.249507023000035,
-            "velocityY": 0.16834985645314604,
-            "timestamp": 1.716122076792166
-        },
-        {
-            "x": 7.6045126721499186,
-            "y": 7.511460301300605,
-            "heading": 3.14,
-            "angularVelocity": 8.943638136140749e-17,
-            "velocityX": 1.5213669091113222,
-            "velocityY": -0.15066620318617746,
-            "timestamp": 1.8114621921695087
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 1.2339640787414218e-16,
-            "velocityX": 0.7917687906220042,
-            "velocityY": -0.4663336217356026,
-            "timestamp": 1.9068023075468512
-        },
-        {
-            "x": 7.680079036283902,
-            "y": 7.383775130350334,
-            "heading": 3.14,
-            "angularVelocity": 3.1298666285147066e-16,
-            "velocityX": 0.0007655130286299905,
-            "velocityY": -0.8060819521991853,
-            "timestamp": 2.010048471814154
-        },
-        {
-            "x": 7.598307382340287,
-            "y": 7.265900859151241,
-            "heading": 3.14,
-            "angularVelocity": 4.0403721611171334e-16,
-            "velocityX": -0.792006700916374,
-            "velocityY": -1.141681843927082,
-            "timestamp": 2.113294636081456
-        },
-        {
-            "x": 7.434354633787184,
-            "y": 7.114168304333743,
-            "heading": 3.14,
-            "angularVelocity": 4.565397791635157e-16,
-            "velocityX": -1.5879790761875838,
-            "velocityY": -1.4696192918573159,
-            "timestamp": 2.2165408003487586
-        },
-        {
-            "x": 7.187443357098086,
-            "y": 6.930529696196354,
-            "heading": 3.14,
-            "angularVelocity": 3.88515705379733e-16,
-            "velocityX": -2.3914813537270896,
-            "velocityY": -1.7786482378362456,
-            "timestamp": 2.319786964616061
-        },
-        {
-            "x": 6.853793099378443,
-            "y": 6.7274887650449,
-            "heading": 3.14,
-            "angularVelocity": -3.674797460353125e-16,
-            "velocityX": -3.2315995474255925,
-            "velocityY": -1.9665711805601358,
-            "timestamp": 2.4230331288833633
-        },
-        {
-            "x": 6.49770423144866,
-            "y": 6.567024283806904,
-            "heading": 3.14,
-            "angularVelocity": 2.249424709718662e-16,
-            "velocityX": -3.448930722577495,
-            "velocityY": -1.5541931497092332,
-            "timestamp": 2.5262792931506657
-        },
-        {
-            "x": 6.1416152766915,
-            "y": 6.406559995248834,
-            "heading": 3.14,
-            "angularVelocity": 2.526804739134875e-16,
-            "velocityX": -3.4489315635518585,
-            "velocityY": -1.5541912834905027,
-            "timestamp": 2.629525457417968
-        },
-        {
-            "x": 5.785526321934093,
-            "y": 6.246095706691313,
-            "heading": 3.14,
-            "angularVelocity": 2.2036218085246878e-16,
-            "velocityX": -3.4489315635542535,
-            "velocityY": -1.5541912834851872,
-            "timestamp": 2.7327716216852704
-        },
-        {
-            "x": 5.429437367176686,
-            "y": 6.085631418133792,
-            "heading": 3.14,
-            "angularVelocity": 7.127301298549232e-19,
-            "velocityX": -3.4489315635542535,
-            "velocityY": -1.5541912834851876,
-            "timestamp": 2.836017785952573
-        },
-        {
-            "x": 5.07334841241929,
-            "y": 5.925167129576275,
-            "heading": 3.14,
-            "angularVelocity": 1.0539129254381463e-16,
-            "velocityX": -3.4489315635541473,
-            "velocityY": -1.5541912834851401,
-            "timestamp": 2.939263950219875
-        },
-        {
-            "x": 4.749209064120322,
-            "y": 5.7791002852572,
-            "heading": 3.14,
-            "angularVelocity": 8.727825696429895e-16,
-            "velocityX": -3.1394807797389603,
-            "velocityY": -1.4147435437980136,
-            "timestamp": 3.0425101144871776
-        },
-        {
-            "x": 4.506104536371044,
-            "y": 5.669550144571212,
-            "heading": 3.14,
-            "angularVelocity": 4.949097028202336e-16,
-            "velocityX": -2.3546107448590976,
-            "velocityY": -1.061057729974005,
-            "timestamp": 3.14575627875448
-        },
-        {
-            "x": 4.3440348464149885,
-            "y": 5.596516715288762,
-            "heading": 3.14,
-            "angularVelocity": 3.5626049667024716e-16,
-            "velocityX": -1.5697405429654518,
-            "velocityY": -0.7073718408886114,
-            "timestamp": 3.2490024430217823
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 2.245803154155974e-16,
-            "velocityX": -0.7848702854005415,
-            "velocityY": -0.35368592671608795,
-            "timestamp": 3.3522486072890847
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": -4.241491761729664e-33,
-            "velocityY": 0,
-            "timestamp": 3.455494771556387
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S2.traj
deleted file mode 100644
index 42ce41bc..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1 To S2.traj	
+++ /dev/null
@@ -1,328 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3199317069050815,
-            "y": 5.589221584324863,
-            "heading": 3.14,
-            "angularVelocity": -4.40794531804863e-17,
-            "velocityX": 0.7334971919039711,
-            "velocityY": 0.3064983109073157,
-            "timestamp": 0.09534011537734256
-        },
-        {
-            "x": 2.4597951178135684,
-            "y": 5.647664751762101,
-            "heading": 3.14,
-            "angularVelocity": -2.0051811551543008e-16,
-            "velocityX": 1.4669943533729506,
-            "velocityY": 0.6129966090971003,
-            "timestamp": 0.19068023075468513
-        },
-        {
-            "x": 2.6695902269221126,
-            "y": 5.73532949988673,
-            "heading": 3.14,
-            "angularVelocity": -1.6298734307861892e-16,
-            "velocityX": 2.200491453971973,
-            "velocityY": 0.9194948818518327,
-            "timestamp": 0.2860203461320277
-        },
-        {
-            "x": 2.9493170168206886,
-            "y": 5.852215821423819,
-            "heading": 3.14,
-            "angularVelocity": -2.718821323245932e-16,
-            "velocityX": 2.933988371961343,
-            "velocityY": 1.2259930783015016,
-            "timestamp": 0.38136046150937025
-        },
-        {
-            "x": 3.2820984406189933,
-            "y": 5.991271492558365,
-            "heading": 3.14,
-            "angularVelocity": -1.791056538237388e-18,
-            "velocityX": 3.4904659227776635,
-            "velocityY": 1.45852216125588,
-            "timestamp": 0.4767005768867128
-        },
-        {
-            "x": 3.6148798644173543,
-            "y": 6.130327163692935,
-            "heading": 3.14,
-            "angularVelocity": 2.7598941425139603e-16,
-            "velocityX": 3.4904659227782475,
-            "velocityY": 1.4585221612561248,
-            "timestamp": 0.5720406922640554
-        },
-        {
-            "x": 3.947661288215715,
-            "y": 6.269382834827505,
-            "heading": 3.14,
-            "angularVelocity": -1.359472794095135e-16,
-            "velocityX": 3.4904659227782475,
-            "velocityY": 1.4585221612561248,
-            "timestamp": 0.667380807641398
-        },
-        {
-            "x": 4.280442712014075,
-            "y": 6.408438505962075,
-            "heading": 3.14,
-            "angularVelocity": -3.500332184235888e-16,
-            "velocityX": 3.4904659227782475,
-            "velocityY": 1.4585221612561245,
-            "timestamp": 0.7627209230187405
-        },
-        {
-            "x": 4.613224135812436,
-            "y": 6.547494177096645,
-            "heading": 3.14,
-            "angularVelocity": -3.2849392470850763e-16,
-            "velocityX": 3.4904659227782475,
-            "velocityY": 1.4585221612561245,
-            "timestamp": 0.858061038396083
-        },
-        {
-            "x": 4.9460055596107955,
-            "y": 6.686549848231215,
-            "heading": 3.14,
-            "angularVelocity": -3.264382961483678e-16,
-            "velocityX": 3.4904659227782475,
-            "velocityY": 1.4585221612561245,
-            "timestamp": 0.9534011537734256
-        },
-        {
-            "x": 5.278786983409157,
-            "y": 6.825605519365785,
-            "heading": 3.14,
-            "angularVelocity": -2.8870332355952577e-16,
-            "velocityX": 3.4904659227782475,
-            "velocityY": 1.4585221612561245,
-            "timestamp": 1.0487412691507683
-        },
-        {
-            "x": 5.611568407207518,
-            "y": 6.9646611905003555,
-            "heading": 3.14,
-            "angularVelocity": -2.714509339961268e-16,
-            "velocityX": 3.4904659227782475,
-            "velocityY": 1.4585221612561245,
-            "timestamp": 1.1440813845281108
-        },
-        {
-            "x": 5.944349831005879,
-            "y": 7.1037168616349256,
-            "heading": 3.14,
-            "angularVelocity": 3.236789006032357e-16,
-            "velocityX": 3.4904659227782475,
-            "velocityY": 1.4585221612561248,
-            "timestamp": 1.2394214999054534
-        },
-        {
-            "x": 6.2771312548052975,
-            "y": 7.242772532766961,
-            "heading": 3.14,
-            "angularVelocity": 1.392859578179056e-16,
-            "velocityX": 3.4904659227893537,
-            "velocityY": 1.458522161229545,
-            "timestamp": 1.334761615282796
-        },
-        {
-            "x": 6.609912972036511,
-            "y": 7.381827501669921,
-            "heading": 3.14,
-            "angularVelocity": 7.280552315335093e-17,
-            "velocityX": 3.4904690005262737,
-            "velocityY": 1.4585147957142848,
-            "timestamp": 1.4301017306601385
-        },
-        {
-            "x": 6.961412726724582,
-            "y": 7.462622346034388,
-            "heading": 3.14,
-            "angularVelocity": -1.0172483638053557e-16,
-            "velocityX": 3.6867980838588856,
-            "velocityY": 0.8474380804417183,
-            "timestamp": 1.525441846037481
-        },
-        {
-            "x": 7.244997116389008,
-            "y": 7.50977433975784,
-            "heading": 3.14,
-            "angularVelocity": -1.1360407734774875e-16,
-            "velocityX": 2.974449826728655,
-            "velocityY": 0.4945661491684764,
-            "timestamp": 1.6207819614148236
-        },
-        {
-            "x": 7.459465375503974,
-            "y": 7.525824834495841,
-            "heading": 3.14,
-            "angularVelocity": -6.302038440751775e-17,
-            "velocityX": 2.249507023000035,
-            "velocityY": 0.16834985645314604,
-            "timestamp": 1.716122076792166
-        },
-        {
-            "x": 7.6045126721499186,
-            "y": 7.511460301300605,
-            "heading": 3.14,
-            "angularVelocity": 8.943638136140749e-17,
-            "velocityX": 1.5213669091113222,
-            "velocityY": -0.15066620318617746,
-            "timestamp": 1.8114621921695087
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 1.2339640787414218e-16,
-            "velocityX": 0.7917687906220042,
-            "velocityY": -0.4663336217356026,
-            "timestamp": 1.9068023075468512
-        },
-        {
-            "x": 7.680079036283902,
-            "y": 7.383775130350334,
-            "heading": 3.14,
-            "angularVelocity": 3.1298666285147066e-16,
-            "velocityX": 0.0007655130286299905,
-            "velocityY": -0.8060819521991853,
-            "timestamp": 2.010048471814154
-        },
-        {
-            "x": 7.598307382340287,
-            "y": 7.265900859151241,
-            "heading": 3.14,
-            "angularVelocity": 4.0403721611171334e-16,
-            "velocityX": -0.792006700916374,
-            "velocityY": -1.141681843927082,
-            "timestamp": 2.113294636081456
-        },
-        {
-            "x": 7.434354633787184,
-            "y": 7.114168304333743,
-            "heading": 3.14,
-            "angularVelocity": 4.565397791635157e-16,
-            "velocityX": -1.5879790761875838,
-            "velocityY": -1.4696192918573159,
-            "timestamp": 2.2165408003487586
-        },
-        {
-            "x": 7.187443357098086,
-            "y": 6.930529696196354,
-            "heading": 3.14,
-            "angularVelocity": 3.88515705379733e-16,
-            "velocityX": -2.3914813537270896,
-            "velocityY": -1.7786482378362456,
-            "timestamp": 2.319786964616061
-        },
-        {
-            "x": 6.853793099378443,
-            "y": 6.7274887650449,
-            "heading": 3.14,
-            "angularVelocity": -3.674797460353125e-16,
-            "velocityX": -3.2315995474255925,
-            "velocityY": -1.9665711805601358,
-            "timestamp": 2.4230331288833633
-        },
-        {
-            "x": 6.49770423144866,
-            "y": 6.567024283806904,
-            "heading": 3.14,
-            "angularVelocity": 2.249424709718662e-16,
-            "velocityX": -3.448930722577495,
-            "velocityY": -1.5541931497092332,
-            "timestamp": 2.5262792931506657
-        },
-        {
-            "x": 6.1416152766915,
-            "y": 6.406559995248834,
-            "heading": 3.14,
-            "angularVelocity": 2.526804739134875e-16,
-            "velocityX": -3.4489315635518585,
-            "velocityY": -1.5541912834905027,
-            "timestamp": 2.629525457417968
-        },
-        {
-            "x": 5.785526321934093,
-            "y": 6.246095706691313,
-            "heading": 3.14,
-            "angularVelocity": 2.2036218085246878e-16,
-            "velocityX": -3.4489315635542535,
-            "velocityY": -1.5541912834851872,
-            "timestamp": 2.7327716216852704
-        },
-        {
-            "x": 5.429437367176686,
-            "y": 6.085631418133792,
-            "heading": 3.14,
-            "angularVelocity": 7.127301298549232e-19,
-            "velocityX": -3.4489315635542535,
-            "velocityY": -1.5541912834851876,
-            "timestamp": 2.836017785952573
-        },
-        {
-            "x": 5.07334841241929,
-            "y": 5.925167129576275,
-            "heading": 3.14,
-            "angularVelocity": 1.0539129254381463e-16,
-            "velocityX": -3.4489315635541473,
-            "velocityY": -1.5541912834851401,
-            "timestamp": 2.939263950219875
-        },
-        {
-            "x": 4.749209064120322,
-            "y": 5.7791002852572,
-            "heading": 3.14,
-            "angularVelocity": 8.727825696429895e-16,
-            "velocityX": -3.1394807797389603,
-            "velocityY": -1.4147435437980136,
-            "timestamp": 3.0425101144871776
-        },
-        {
-            "x": 4.506104536371044,
-            "y": 5.669550144571212,
-            "heading": 3.14,
-            "angularVelocity": 4.949097028202336e-16,
-            "velocityX": -2.3546107448590976,
-            "velocityY": -1.061057729974005,
-            "timestamp": 3.14575627875448
-        },
-        {
-            "x": 4.3440348464149885,
-            "y": 5.596516715288762,
-            "heading": 3.14,
-            "angularVelocity": 3.5626049667024716e-16,
-            "velocityX": -1.5697405429654518,
-            "velocityY": -0.7073718408886114,
-            "timestamp": 3.2490024430217823
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 2.245803154155974e-16,
-            "velocityX": -0.7848702854005415,
-            "velocityY": -0.35368592671608795,
-            "timestamp": 3.3522486072890847
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": -4.241491761729664e-33,
-            "velocityY": 0,
-            "timestamp": 3.455494771556387
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S1.1.traj
deleted file mode 100644
index e9a814a4..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S1.1.traj	
+++ /dev/null
@@ -1,301 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.327391358066629,
-            "y": 5.555067935622319,
-            "heading": 3.133430176934097,
-            "angularVelocity": -0.0681146690773455,
-            "velocityX": 0.8023787994403143,
-            "velocityY": -0.05113470021702577,
-            "timestamp": 0.09645239644992105
-        },
-        {
-            "x": 2.4821740708193567,
-            "y": 5.545203805497392,
-            "heading": 3.12029049964152,
-            "angularVelocity": -0.13622966122359603,
-            "velocityX": 1.6047575638319433,
-            "velocityY": -0.10226941463344474,
-            "timestamp": 0.1929047928998421
-        },
-        {
-            "x": 2.7143481316892246,
-            "y": 5.5304076076409565,
-            "heading": 3.1005809392040384,
-            "angularVelocity": -0.20434495318853443,
-            "velocityX": 2.4071362601178543,
-            "velocityY": -0.15340414962232987,
-            "timestamp": 0.2893571893497632
-        },
-        {
-            "x": 3.023913521714313,
-            "y": 5.510679340033408,
-            "heading": 3.0743015138082646,
-            "angularVelocity": -0.2724600565981605,
-            "velocityX": 3.2095147598102214,
-            "velocityY": -0.2045389055500683,
-            "timestamp": 0.3858095857996842
-        },
-        {
-            "x": 3.388048508086598,
-            "y": 5.487473394252625,
-            "heading": 3.0743015107885725,
-            "angularVelocity": -3.130758650216746e-8,
-            "velocityX": 3.7752818983750913,
-            "velocityY": -0.24059480774884917,
-            "timestamp": 0.48226198224960526
-        },
-        {
-            "x": 3.752183494458947,
-            "y": 5.464267448472115,
-            "heading": 3.0743015077689044,
-            "angularVelocity": -3.130734274282633e-8,
-            "velocityX": 3.7752818983757597,
-            "velocityY": -0.24059480774599656,
-            "timestamp": 0.5787143786995264
-        },
-        {
-            "x": 4.116318480831297,
-            "y": 5.441061502691605,
-            "heading": 3.0743015047492364,
-            "angularVelocity": -3.1307342782181776e-8,
-            "velocityX": 3.77528189837576,
-            "velocityY": -0.24059480774599656,
-            "timestamp": 0.6751667751494475
-        },
-        {
-            "x": 4.480453467203646,
-            "y": 5.417855556911095,
-            "heading": 3.0743015017295683,
-            "angularVelocity": -3.130734248880116e-8,
-            "velocityX": 3.7752818983757597,
-            "velocityY": -0.2405948077459966,
-            "timestamp": 0.7716191715993685
-        },
-        {
-            "x": 4.844588453575996,
-            "y": 5.3946496111305855,
-            "heading": 3.0743014987098998,
-            "angularVelocity": -3.130734337287564e-8,
-            "velocityX": 3.77528189837576,
-            "velocityY": -0.24059480774599656,
-            "timestamp": 0.8680715680492896
-        },
-        {
-            "x": 5.208723439948346,
-            "y": 5.371443665350077,
-            "heading": 3.0743014956902317,
-            "angularVelocity": -3.130734341157956e-8,
-            "velocityX": 3.7752818983757606,
-            "velocityY": -0.24059480774599654,
-            "timestamp": 0.9645239644992107
-        },
-        {
-            "x": 5.572858426320695,
-            "y": 5.348237719569567,
-            "heading": 3.0743014926705636,
-            "angularVelocity": -3.130734268712419e-8,
-            "velocityX": 3.7752818983757597,
-            "velocityY": -0.24059480774599656,
-            "timestamp": 1.0609763609491318
-        },
-        {
-            "x": 5.936993412693067,
-            "y": 5.325031773789395,
-            "heading": 3.0743014896508956,
-            "angularVelocity": -3.130734257132029e-8,
-            "velocityX": 3.7752818983759835,
-            "velocityY": -0.24059480774249117,
-            "timestamp": 1.157428757399053
-        },
-        {
-            "x": 6.301128405222732,
-            "y": 5.301825924626314,
-            "heading": 3.0743014866312275,
-            "angularVelocity": -3.1307343059822334e-8,
-            "velocityX": 3.7752819622136293,
-            "velocityY": -0.24059380603497976,
-            "timestamp": 1.253881153848974
-        },
-        {
-            "x": 6.666000662308113,
-            "y": 5.302845713467377,
-            "heading": 3.0743014835545788,
-            "angularVelocity": -3.1898104709200575e-8,
-            "velocityX": 3.7829257801264253,
-            "velocityY": 0.010572975670877716,
-            "timestamp": 1.3503335502988951
-        },
-        {
-            "x": 7.022411728114523,
-            "y": 5.380972301482392,
-            "heading": 3.0743014635013113,
-            "angularVelocity": -2.0790844162753672e-7,
-            "velocityX": 3.6952017671376614,
-            "velocityY": 0.8100015229334367,
-            "timestamp": 1.4467859467488162
-        },
-        {
-            "x": 7.302470785429282,
-            "y": 5.472668060315495,
-            "heading": 3.0808623231023553,
-            "angularVelocity": 0.06802173758793187,
-            "velocityX": 2.903598745316483,
-            "velocityY": 0.9506840908894665,
-            "timestamp": 1.5432383431987373
-        },
-        {
-            "x": 7.505508952457786,
-            "y": 5.5733927696291925,
-            "heading": 3.0939992329731627,
-            "angularVelocity": 0.1362009691239587,
-            "velocityX": 2.1050608849716217,
-            "velocityY": 1.0442945226974727,
-            "timestamp": 1.6396907396486584
-        },
-        {
-            "x": 7.631373643549882,
-            "y": 5.681732940772809,
-            "heading": 3.113712106449711,
-            "angularVelocity": 0.20437930214396433,
-            "velocityX": 1.3049410457877573,
-            "velocityY": 1.1232501745031058,
-            "timestamp": 1.7361431360985795
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 0.27254785280465055,
-            "velocityX": 0.5041487639486981,
-            "velocityY": 1.1950668253954515,
-            "timestamp": 1.8325955325485006
-        },
-        {
-            "x": 7.657241486683139,
-            "y": 5.911359552858986,
-            "heading": 3.1705946317800286,
-            "angularVelocity": 0.3367496839490721,
-            "velocityX": -0.25049891829736254,
-            "velocityY": 1.2587353088184159,
-            "timestamp": 1.923448273436763
-        },
-        {
-            "x": 7.565890794694817,
-            "y": 6.031156159947816,
-            "heading": 3.2067228278300997,
-            "angularVelocity": 0.39765664411275126,
-            "velocityX": -1.0054808594126166,
-            "velocityY": 1.318580000092289,
-            "timestamp": 2.0143010143250253
-        },
-        {
-            "x": 7.40590178880199,
-            "y": 6.1558149457566955,
-            "heading": 3.247889559674286,
-            "angularVelocity": 0.453114913669111,
-            "velocityX": -1.7609706028527334,
-            "velocityY": 1.3720971386234067,
-            "timestamp": 2.1051537552132875
-        },
-        {
-            "x": 7.1771984711004135,
-            "y": 6.284201834322083,
-            "heading": 3.293119082899787,
-            "angularVelocity": 0.49783333758889264,
-            "velocityX": -2.517296841741443,
-            "velocityY": 1.4131317042298905,
-            "timestamp": 2.1960064961015497
-        },
-        {
-            "x": 6.879672767630686,
-            "y": 6.413043079334037,
-            "heading": 3.3395968476460443,
-            "angularVelocity": 0.5115725105467077,
-            "velocityX": -3.274812631526949,
-            "velocityY": 1.4181327250260163,
-            "timestamp": 2.286859236989812
-        },
-        {
-            "x": 6.543860581996508,
-            "y": 6.486209643935175,
-            "heading": 3.33959685479452,
-            "angularVelocity": 7.868200264443885e-8,
-            "velocityX": -3.6962251479807913,
-            "velocityY": 0.805331395462515,
-            "timestamp": 2.377711977878074
-        },
-        {
-            "x": 6.202053460852504,
-            "y": 6.52214101663038,
-            "heading": 3.33959685834771,
-            "angularVelocity": 3.910933561720104e-8,
-            "velocityX": -3.7622103395249686,
-            "velocityY": 0.39549024436583907,
-            "timestamp": 2.4685647187663364
-        },
-        {
-            "x": 5.860246134397577,
-            "y": 6.558070415270777,
-            "heading": 3.339596863101522,
-            "angularVelocity": 5.232436753390925e-8,
-            "velocityX": -3.762212599345888,
-            "velocityY": 0.39546851629479235,
-            "timestamp": 2.5594174596545987
-        },
-        {
-            "x": 5.5865423033177715,
-            "y": 6.586842981129828,
-            "heading": 3.3642400407233666,
-            "angularVelocity": 0.2712430839279776,
-            "velocityX": -3.0126095085719746,
-            "velocityY": 0.31669452762507067,
-            "timestamp": 2.650270200542861
-        },
-        {
-            "x": 5.381269635001315,
-            "y": 6.608421695048074,
-            "heading": 3.3835025789166386,
-            "angularVelocity": 0.2120193403626775,
-            "velocityX": -2.2593998409901115,
-            "velocityY": 0.23751307563506308,
-            "timestamp": 2.741122941431123
-        },
-        {
-            "x": 5.24442286400993,
-            "y": 6.622807278395225,
-            "heading": 3.3965772666434986,
-            "angularVelocity": 0.14391076811804343,
-            "velocityX": -1.5062481291531917,
-            "velocityY": 0.15833956363345458,
-            "timestamp": 2.8319756823193853
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.07273352979206758,
-            "velocityX": -0.753118324675337,
-            "velocityY": 0.07916901058187684,
-            "timestamp": 2.9228284232076476
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": -1.0239181929178098e-32,
-            "velocityX": 1.4788561432464288e-34,
-            "velocityY": 0,
-            "timestamp": 3.01368116409591
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S1.traj
deleted file mode 100644
index e9a814a4..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S1.traj	
+++ /dev/null
@@ -1,301 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.327391358066629,
-            "y": 5.555067935622319,
-            "heading": 3.133430176934097,
-            "angularVelocity": -0.0681146690773455,
-            "velocityX": 0.8023787994403143,
-            "velocityY": -0.05113470021702577,
-            "timestamp": 0.09645239644992105
-        },
-        {
-            "x": 2.4821740708193567,
-            "y": 5.545203805497392,
-            "heading": 3.12029049964152,
-            "angularVelocity": -0.13622966122359603,
-            "velocityX": 1.6047575638319433,
-            "velocityY": -0.10226941463344474,
-            "timestamp": 0.1929047928998421
-        },
-        {
-            "x": 2.7143481316892246,
-            "y": 5.5304076076409565,
-            "heading": 3.1005809392040384,
-            "angularVelocity": -0.20434495318853443,
-            "velocityX": 2.4071362601178543,
-            "velocityY": -0.15340414962232987,
-            "timestamp": 0.2893571893497632
-        },
-        {
-            "x": 3.023913521714313,
-            "y": 5.510679340033408,
-            "heading": 3.0743015138082646,
-            "angularVelocity": -0.2724600565981605,
-            "velocityX": 3.2095147598102214,
-            "velocityY": -0.2045389055500683,
-            "timestamp": 0.3858095857996842
-        },
-        {
-            "x": 3.388048508086598,
-            "y": 5.487473394252625,
-            "heading": 3.0743015107885725,
-            "angularVelocity": -3.130758650216746e-8,
-            "velocityX": 3.7752818983750913,
-            "velocityY": -0.24059480774884917,
-            "timestamp": 0.48226198224960526
-        },
-        {
-            "x": 3.752183494458947,
-            "y": 5.464267448472115,
-            "heading": 3.0743015077689044,
-            "angularVelocity": -3.130734274282633e-8,
-            "velocityX": 3.7752818983757597,
-            "velocityY": -0.24059480774599656,
-            "timestamp": 0.5787143786995264
-        },
-        {
-            "x": 4.116318480831297,
-            "y": 5.441061502691605,
-            "heading": 3.0743015047492364,
-            "angularVelocity": -3.1307342782181776e-8,
-            "velocityX": 3.77528189837576,
-            "velocityY": -0.24059480774599656,
-            "timestamp": 0.6751667751494475
-        },
-        {
-            "x": 4.480453467203646,
-            "y": 5.417855556911095,
-            "heading": 3.0743015017295683,
-            "angularVelocity": -3.130734248880116e-8,
-            "velocityX": 3.7752818983757597,
-            "velocityY": -0.2405948077459966,
-            "timestamp": 0.7716191715993685
-        },
-        {
-            "x": 4.844588453575996,
-            "y": 5.3946496111305855,
-            "heading": 3.0743014987098998,
-            "angularVelocity": -3.130734337287564e-8,
-            "velocityX": 3.77528189837576,
-            "velocityY": -0.24059480774599656,
-            "timestamp": 0.8680715680492896
-        },
-        {
-            "x": 5.208723439948346,
-            "y": 5.371443665350077,
-            "heading": 3.0743014956902317,
-            "angularVelocity": -3.130734341157956e-8,
-            "velocityX": 3.7752818983757606,
-            "velocityY": -0.24059480774599654,
-            "timestamp": 0.9645239644992107
-        },
-        {
-            "x": 5.572858426320695,
-            "y": 5.348237719569567,
-            "heading": 3.0743014926705636,
-            "angularVelocity": -3.130734268712419e-8,
-            "velocityX": 3.7752818983757597,
-            "velocityY": -0.24059480774599656,
-            "timestamp": 1.0609763609491318
-        },
-        {
-            "x": 5.936993412693067,
-            "y": 5.325031773789395,
-            "heading": 3.0743014896508956,
-            "angularVelocity": -3.130734257132029e-8,
-            "velocityX": 3.7752818983759835,
-            "velocityY": -0.24059480774249117,
-            "timestamp": 1.157428757399053
-        },
-        {
-            "x": 6.301128405222732,
-            "y": 5.301825924626314,
-            "heading": 3.0743014866312275,
-            "angularVelocity": -3.1307343059822334e-8,
-            "velocityX": 3.7752819622136293,
-            "velocityY": -0.24059380603497976,
-            "timestamp": 1.253881153848974
-        },
-        {
-            "x": 6.666000662308113,
-            "y": 5.302845713467377,
-            "heading": 3.0743014835545788,
-            "angularVelocity": -3.1898104709200575e-8,
-            "velocityX": 3.7829257801264253,
-            "velocityY": 0.010572975670877716,
-            "timestamp": 1.3503335502988951
-        },
-        {
-            "x": 7.022411728114523,
-            "y": 5.380972301482392,
-            "heading": 3.0743014635013113,
-            "angularVelocity": -2.0790844162753672e-7,
-            "velocityX": 3.6952017671376614,
-            "velocityY": 0.8100015229334367,
-            "timestamp": 1.4467859467488162
-        },
-        {
-            "x": 7.302470785429282,
-            "y": 5.472668060315495,
-            "heading": 3.0808623231023553,
-            "angularVelocity": 0.06802173758793187,
-            "velocityX": 2.903598745316483,
-            "velocityY": 0.9506840908894665,
-            "timestamp": 1.5432383431987373
-        },
-        {
-            "x": 7.505508952457786,
-            "y": 5.5733927696291925,
-            "heading": 3.0939992329731627,
-            "angularVelocity": 0.1362009691239587,
-            "velocityX": 2.1050608849716217,
-            "velocityY": 1.0442945226974727,
-            "timestamp": 1.6396907396486584
-        },
-        {
-            "x": 7.631373643549882,
-            "y": 5.681732940772809,
-            "heading": 3.113712106449711,
-            "angularVelocity": 0.20437930214396433,
-            "velocityX": 1.3049410457877573,
-            "velocityY": 1.1232501745031058,
-            "timestamp": 1.7361431360985795
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 0.27254785280465055,
-            "velocityX": 0.5041487639486981,
-            "velocityY": 1.1950668253954515,
-            "timestamp": 1.8325955325485006
-        },
-        {
-            "x": 7.657241486683139,
-            "y": 5.911359552858986,
-            "heading": 3.1705946317800286,
-            "angularVelocity": 0.3367496839490721,
-            "velocityX": -0.25049891829736254,
-            "velocityY": 1.2587353088184159,
-            "timestamp": 1.923448273436763
-        },
-        {
-            "x": 7.565890794694817,
-            "y": 6.031156159947816,
-            "heading": 3.2067228278300997,
-            "angularVelocity": 0.39765664411275126,
-            "velocityX": -1.0054808594126166,
-            "velocityY": 1.318580000092289,
-            "timestamp": 2.0143010143250253
-        },
-        {
-            "x": 7.40590178880199,
-            "y": 6.1558149457566955,
-            "heading": 3.247889559674286,
-            "angularVelocity": 0.453114913669111,
-            "velocityX": -1.7609706028527334,
-            "velocityY": 1.3720971386234067,
-            "timestamp": 2.1051537552132875
-        },
-        {
-            "x": 7.1771984711004135,
-            "y": 6.284201834322083,
-            "heading": 3.293119082899787,
-            "angularVelocity": 0.49783333758889264,
-            "velocityX": -2.517296841741443,
-            "velocityY": 1.4131317042298905,
-            "timestamp": 2.1960064961015497
-        },
-        {
-            "x": 6.879672767630686,
-            "y": 6.413043079334037,
-            "heading": 3.3395968476460443,
-            "angularVelocity": 0.5115725105467077,
-            "velocityX": -3.274812631526949,
-            "velocityY": 1.4181327250260163,
-            "timestamp": 2.286859236989812
-        },
-        {
-            "x": 6.543860581996508,
-            "y": 6.486209643935175,
-            "heading": 3.33959685479452,
-            "angularVelocity": 7.868200264443885e-8,
-            "velocityX": -3.6962251479807913,
-            "velocityY": 0.805331395462515,
-            "timestamp": 2.377711977878074
-        },
-        {
-            "x": 6.202053460852504,
-            "y": 6.52214101663038,
-            "heading": 3.33959685834771,
-            "angularVelocity": 3.910933561720104e-8,
-            "velocityX": -3.7622103395249686,
-            "velocityY": 0.39549024436583907,
-            "timestamp": 2.4685647187663364
-        },
-        {
-            "x": 5.860246134397577,
-            "y": 6.558070415270777,
-            "heading": 3.339596863101522,
-            "angularVelocity": 5.232436753390925e-8,
-            "velocityX": -3.762212599345888,
-            "velocityY": 0.39546851629479235,
-            "timestamp": 2.5594174596545987
-        },
-        {
-            "x": 5.5865423033177715,
-            "y": 6.586842981129828,
-            "heading": 3.3642400407233666,
-            "angularVelocity": 0.2712430839279776,
-            "velocityX": -3.0126095085719746,
-            "velocityY": 0.31669452762507067,
-            "timestamp": 2.650270200542861
-        },
-        {
-            "x": 5.381269635001315,
-            "y": 6.608421695048074,
-            "heading": 3.3835025789166386,
-            "angularVelocity": 0.2120193403626775,
-            "velocityX": -2.2593998409901115,
-            "velocityY": 0.23751307563506308,
-            "timestamp": 2.741122941431123
-        },
-        {
-            "x": 5.24442286400993,
-            "y": 6.622807278395225,
-            "heading": 3.3965772666434986,
-            "angularVelocity": 0.14391076811804343,
-            "velocityX": -1.5062481291531917,
-            "velocityY": 0.15833956363345458,
-            "timestamp": 2.8319756823193853
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.07273352979206758,
-            "velocityX": -0.753118324675337,
-            "velocityY": 0.07916901058187684,
-            "timestamp": 2.9228284232076476
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": -1.0239181929178098e-32,
-            "velocityX": 1.4788561432464288e-34,
-            "velocityY": 0,
-            "timestamp": 3.01368116409591
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S2.1.traj
deleted file mode 100644
index 9ae1f0d7..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S2.1.traj	
+++ /dev/null
@@ -1,310 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3282521020736953,
-            "y": 5.564172492511594,
-            "heading": 3.14,
-            "angularVelocity": 1.232216948997824e-17,
-            "velocityX": 0.8071870820603332,
-            "velocityY": 0.043040148021588745,
-            "timestamp": 0.09694419527026792
-        },
-        {
-            "x": 2.484756303050172,
-            "y": 5.5725174773657065,
-            "heading": 3.14,
-            "angularVelocity": 2.4529935765970837e-17,
-            "velocityX": 1.6143741314120201,
-            "velocityY": 0.08608029429911494,
-            "timestamp": 0.19388839054053583
-        },
-        {
-            "x": 2.7195125965876055,
-            "y": 5.5850349542241835,
-            "heading": 3.14,
-            "angularVelocity": 3.650998333784998e-17,
-            "velocityX": 2.421561115346432,
-            "velocityY": 0.12912043708851692,
-            "timestamp": 0.29083258581080373
-        },
-        {
-            "x": 3.032520963660537,
-            "y": 5.601724922072566,
-            "heading": 3.14,
-            "angularVelocity": 4.7804313451185203e-17,
-            "velocityX": 3.2287479030291797,
-            "velocityY": 0.17216056941355468,
-            "timestamp": 0.38777678108107166
-        },
-        {
-            "x": 3.398734861706694,
-            "y": 5.621251870322576,
-            "heading": 3.14,
-            "angularVelocity": 4.271471690686359e-16,
-            "velocityX": 3.777574273786295,
-            "velocityY": 0.20142462579390313,
-            "timestamp": 0.4847209763513396
-        },
-        {
-            "x": 3.7649487597528926,
-            "y": 5.6407788185725884,
-            "heading": 3.14,
-            "angularVelocity": 3.505027380693511e-16,
-            "velocityX": 3.7775742737867146,
-            "velocityY": 0.20142462579393414,
-            "timestamp": 0.5816651716216075
-        },
-        {
-            "x": 4.1311626577990905,
-            "y": 5.660305766822602,
-            "heading": 3.14,
-            "angularVelocity": 3.7529805795054796e-16,
-            "velocityX": 3.7775742737867146,
-            "velocityY": 0.20142462579393414,
-            "timestamp": 0.6786093668918753
-        },
-        {
-            "x": 4.497376555845288,
-            "y": 5.679832715072616,
-            "heading": 3.14,
-            "angularVelocity": 4.2292762480360005e-16,
-            "velocityX": 3.7775742737867146,
-            "velocityY": 0.20142462579393414,
-            "timestamp": 0.7755535621621432
-        },
-        {
-            "x": 4.8635904538914865,
-            "y": 5.699359663322629,
-            "heading": 3.14,
-            "angularVelocity": -1.0631316332366903e-16,
-            "velocityX": 3.7775742737867146,
-            "velocityY": 0.20142462579393414,
-            "timestamp": 0.8724977574324111
-        },
-        {
-            "x": 5.229804351937685,
-            "y": 5.718886611572643,
-            "heading": 3.14,
-            "angularVelocity": -2.15507046342417e-16,
-            "velocityX": 3.7775742737867146,
-            "velocityY": 0.2014246257939341,
-            "timestamp": 0.969441952702679
-        },
-        {
-            "x": 5.596018249983883,
-            "y": 5.738413559822655,
-            "heading": 3.14,
-            "angularVelocity": 2.7855582274724927e-16,
-            "velocityX": 3.7775742737867146,
-            "velocityY": 0.20142462579393414,
-            "timestamp": 1.0663861479729468
-        },
-        {
-            "x": 5.962232148030081,
-            "y": 5.757940508072669,
-            "heading": 3.14,
-            "angularVelocity": 3.1548114792697624e-17,
-            "velocityX": 3.7775742737867146,
-            "velocityY": 0.201424625793935,
-            "timestamp": 1.1633303432432147
-        },
-        {
-            "x": 6.3284460460762855,
-            "y": 5.777467456322548,
-            "heading": 3.14,
-            "angularVelocity": 5.639488659636306e-17,
-            "velocityX": 3.777574273786789,
-            "velocityY": 0.20142462579255413,
-            "timestamp": 1.2602745385134826
-        },
-        {
-            "x": 6.694659946182267,
-            "y": 5.796994365942685,
-            "heading": 3.14,
-            "angularVelocity": 1.8236235813910642e-16,
-            "velocityX": 3.7775742950338076,
-            "velocityY": 0.20142422731870196,
-            "timestamp": 1.3572187337837505
-        },
-        {
-            "x": 7.048232130231478,
-            "y": 5.805340495974463,
-            "heading": 3.14,
-            "angularVelocity": 9.880815779139735e-16,
-            "velocityX": 3.6471723000895206,
-            "velocityY": 0.08609210670542117,
-            "timestamp": 1.4541629290540183
-        },
-        {
-            "x": 7.323552241539944,
-            "y": 5.8095141201136,
-            "heading": 3.14,
-            "angularVelocity": 1.0046882743013462e-15,
-            "velocityX": 2.839985520604805,
-            "velocityY": 0.04305182065552602,
-            "timestamp": 1.5511071243242862
-        },
-        {
-            "x": 7.520620260713227,
-            "y": 5.809515244263857,
-            "heading": 3.14,
-            "angularVelocity": 1.1309282599454547e-15,
-            "velocityX": 2.032798541062352,
-            "velocityY": 0.000011595504179921879,
-            "timestamp": 1.648051319594554
-        },
-        {
-            "x": 7.639436181279198,
-            "y": 5.805343870530581,
-            "heading": 3.14,
-            "angularVelocity": 1.1596973638353238e-15,
-            "velocityX": 1.2256114947584758,
-            "velocityY": -0.043028607930100944,
-            "timestamp": 1.744995514864822
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 1.1762980395606437e-15,
-            "velocityX": 0.41842441505544703,
-            "velocityY": -0.08606880015967894,
-            "timestamp": 1.8419397101350898
-        },
-        {
-            "x": 7.637810963282883,
-            "y": 5.783845940195137,
-            "heading": 3.14,
-            "angularVelocity": 1.2081191600227442e-15,
-            "velocityX": -0.41931671098523704,
-            "velocityY": -0.1307381629639976,
-            "timestamp": 1.94255348238971
-        },
-        {
-            "x": 7.511333424065826,
-            "y": 5.766201461544033,
-            "heading": 3.14,
-            "angularVelocity": 1.221716047001012e-15,
-            "velocityX": -1.257059901260624,
-            "velocityY": -0.1753684232696404,
-            "timestamp": 2.04316725464433
-        },
-        {
-            "x": 7.3005670350867495,
-            "y": 5.744073149987429,
-            "heading": 3.14,
-            "angularVelocity": 9.674484857548948e-16,
-            "velocityX": -2.0948065429741827,
-            "velocityY": -0.2199332259036739,
-            "timestamp": 2.14378102689895
-        },
-        {
-            "x": 7.005511097627955,
-            "y": 5.717474286699145,
-            "heading": 3.14,
-            "angularVelocity": 6.103023189064465e-16,
-            "velocityX": -2.9325601292436843,
-            "velocityY": -0.2643660269377714,
-            "timestamp": 2.2443947991535698
-        },
-        {
-            "x": 6.626163481456156,
-            "y": 5.686445611767655,
-            "heading": 3.14,
-            "angularVelocity": -3.898195701943848e-16,
-            "velocityX": -3.7703348879039833,
-            "velocityY": -0.30839391219134293,
-            "timestamp": 2.3450085714081896
-        },
-        {
-            "x": 6.24609124574338,
-            "y": 5.666109085648216,
-            "heading": 3.14,
-            "angularVelocity": 5.28249015807614e-17,
-            "velocityX": -3.7775368838161545,
-            "velocityY": -0.2021246762112827,
-            "timestamp": 2.4456223436628095
-        },
-        {
-            "x": 5.866019008161075,
-            "y": 5.6457725944687756,
-            "heading": 3.14,
-            "angularVelocity": 1.0714715731827337e-16,
-            "velocityX": -3.777536902397429,
-            "velocityY": -0.20212432894236246,
-            "timestamp": 2.5462361159174294
-        },
-        {
-            "x": 5.485946770578765,
-            "y": 5.625436103289443,
-            "heading": 3.14,
-            "angularVelocity": 4.4105778021487463e-16,
-            "velocityX": -3.7775369023974865,
-            "velocityY": -0.2021243289413037,
-            "timestamp": 2.6468498881720492
-        },
-        {
-            "x": 5.105874532996473,
-            "y": 5.60509961211012,
-            "heading": 3.14,
-            "angularVelocity": 4.563638430357427e-16,
-            "velocityX": -3.7775369023972933,
-            "velocityY": -0.20212432894121146,
-            "timestamp": 2.747463660426669
-        },
-        {
-            "x": 4.768724738760508,
-            "y": 5.587059768280702,
-            "heading": 3.14,
-            "angularVelocity": 7.096592098456484e-16,
-            "velocityX": -3.3509308606209816,
-            "velocityY": -0.1792979574700793,
-            "timestamp": 2.848077432681289
-        },
-        {
-            "x": 4.5158623742843815,
-            "y": 5.573529884402755,
-            "heading": 3.14,
-            "angularVelocity": 5.58698579821863e-16,
-            "velocityX": -2.5131983323104805,
-            "velocityY": -0.13447347810003318,
-            "timestamp": 2.948691204935909
-        },
-        {
-            "x": 4.3472874591846,
-            "y": 5.564509961525897,
-            "heading": 3.14,
-            "angularVelocity": 2.9765197430594465e-16,
-            "velocityX": -1.6754656090315643,
-            "velocityY": -0.08964898829784126,
-            "timestamp": 3.0493049771905287
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 1.413897346526725e-16,
-            "velocityX": -0.8377328207631561,
-            "velocityY": -0.04482449501826641,
-            "timestamp": 3.1499187494451486
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": -1.608253361776519e-31,
-            "velocityY": 0,
-            "timestamp": 3.2505325216997685
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S2.traj
deleted file mode 100644
index 9ae1f0d7..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2 To S2.traj	
+++ /dev/null
@@ -1,310 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3282521020736953,
-            "y": 5.564172492511594,
-            "heading": 3.14,
-            "angularVelocity": 1.232216948997824e-17,
-            "velocityX": 0.8071870820603332,
-            "velocityY": 0.043040148021588745,
-            "timestamp": 0.09694419527026792
-        },
-        {
-            "x": 2.484756303050172,
-            "y": 5.5725174773657065,
-            "heading": 3.14,
-            "angularVelocity": 2.4529935765970837e-17,
-            "velocityX": 1.6143741314120201,
-            "velocityY": 0.08608029429911494,
-            "timestamp": 0.19388839054053583
-        },
-        {
-            "x": 2.7195125965876055,
-            "y": 5.5850349542241835,
-            "heading": 3.14,
-            "angularVelocity": 3.650998333784998e-17,
-            "velocityX": 2.421561115346432,
-            "velocityY": 0.12912043708851692,
-            "timestamp": 0.29083258581080373
-        },
-        {
-            "x": 3.032520963660537,
-            "y": 5.601724922072566,
-            "heading": 3.14,
-            "angularVelocity": 4.7804313451185203e-17,
-            "velocityX": 3.2287479030291797,
-            "velocityY": 0.17216056941355468,
-            "timestamp": 0.38777678108107166
-        },
-        {
-            "x": 3.398734861706694,
-            "y": 5.621251870322576,
-            "heading": 3.14,
-            "angularVelocity": 4.271471690686359e-16,
-            "velocityX": 3.777574273786295,
-            "velocityY": 0.20142462579390313,
-            "timestamp": 0.4847209763513396
-        },
-        {
-            "x": 3.7649487597528926,
-            "y": 5.6407788185725884,
-            "heading": 3.14,
-            "angularVelocity": 3.505027380693511e-16,
-            "velocityX": 3.7775742737867146,
-            "velocityY": 0.20142462579393414,
-            "timestamp": 0.5816651716216075
-        },
-        {
-            "x": 4.1311626577990905,
-            "y": 5.660305766822602,
-            "heading": 3.14,
-            "angularVelocity": 3.7529805795054796e-16,
-            "velocityX": 3.7775742737867146,
-            "velocityY": 0.20142462579393414,
-            "timestamp": 0.6786093668918753
-        },
-        {
-            "x": 4.497376555845288,
-            "y": 5.679832715072616,
-            "heading": 3.14,
-            "angularVelocity": 4.2292762480360005e-16,
-            "velocityX": 3.7775742737867146,
-            "velocityY": 0.20142462579393414,
-            "timestamp": 0.7755535621621432
-        },
-        {
-            "x": 4.8635904538914865,
-            "y": 5.699359663322629,
-            "heading": 3.14,
-            "angularVelocity": -1.0631316332366903e-16,
-            "velocityX": 3.7775742737867146,
-            "velocityY": 0.20142462579393414,
-            "timestamp": 0.8724977574324111
-        },
-        {
-            "x": 5.229804351937685,
-            "y": 5.718886611572643,
-            "heading": 3.14,
-            "angularVelocity": -2.15507046342417e-16,
-            "velocityX": 3.7775742737867146,
-            "velocityY": 0.2014246257939341,
-            "timestamp": 0.969441952702679
-        },
-        {
-            "x": 5.596018249983883,
-            "y": 5.738413559822655,
-            "heading": 3.14,
-            "angularVelocity": 2.7855582274724927e-16,
-            "velocityX": 3.7775742737867146,
-            "velocityY": 0.20142462579393414,
-            "timestamp": 1.0663861479729468
-        },
-        {
-            "x": 5.962232148030081,
-            "y": 5.757940508072669,
-            "heading": 3.14,
-            "angularVelocity": 3.1548114792697624e-17,
-            "velocityX": 3.7775742737867146,
-            "velocityY": 0.201424625793935,
-            "timestamp": 1.1633303432432147
-        },
-        {
-            "x": 6.3284460460762855,
-            "y": 5.777467456322548,
-            "heading": 3.14,
-            "angularVelocity": 5.639488659636306e-17,
-            "velocityX": 3.777574273786789,
-            "velocityY": 0.20142462579255413,
-            "timestamp": 1.2602745385134826
-        },
-        {
-            "x": 6.694659946182267,
-            "y": 5.796994365942685,
-            "heading": 3.14,
-            "angularVelocity": 1.8236235813910642e-16,
-            "velocityX": 3.7775742950338076,
-            "velocityY": 0.20142422731870196,
-            "timestamp": 1.3572187337837505
-        },
-        {
-            "x": 7.048232130231478,
-            "y": 5.805340495974463,
-            "heading": 3.14,
-            "angularVelocity": 9.880815779139735e-16,
-            "velocityX": 3.6471723000895206,
-            "velocityY": 0.08609210670542117,
-            "timestamp": 1.4541629290540183
-        },
-        {
-            "x": 7.323552241539944,
-            "y": 5.8095141201136,
-            "heading": 3.14,
-            "angularVelocity": 1.0046882743013462e-15,
-            "velocityX": 2.839985520604805,
-            "velocityY": 0.04305182065552602,
-            "timestamp": 1.5511071243242862
-        },
-        {
-            "x": 7.520620260713227,
-            "y": 5.809515244263857,
-            "heading": 3.14,
-            "angularVelocity": 1.1309282599454547e-15,
-            "velocityX": 2.032798541062352,
-            "velocityY": 0.000011595504179921879,
-            "timestamp": 1.648051319594554
-        },
-        {
-            "x": 7.639436181279198,
-            "y": 5.805343870530581,
-            "heading": 3.14,
-            "angularVelocity": 1.1596973638353238e-15,
-            "velocityX": 1.2256114947584758,
-            "velocityY": -0.043028607930100944,
-            "timestamp": 1.744995514864822
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 1.1762980395606437e-15,
-            "velocityX": 0.41842441505544703,
-            "velocityY": -0.08606880015967894,
-            "timestamp": 1.8419397101350898
-        },
-        {
-            "x": 7.637810963282883,
-            "y": 5.783845940195137,
-            "heading": 3.14,
-            "angularVelocity": 1.2081191600227442e-15,
-            "velocityX": -0.41931671098523704,
-            "velocityY": -0.1307381629639976,
-            "timestamp": 1.94255348238971
-        },
-        {
-            "x": 7.511333424065826,
-            "y": 5.766201461544033,
-            "heading": 3.14,
-            "angularVelocity": 1.221716047001012e-15,
-            "velocityX": -1.257059901260624,
-            "velocityY": -0.1753684232696404,
-            "timestamp": 2.04316725464433
-        },
-        {
-            "x": 7.3005670350867495,
-            "y": 5.744073149987429,
-            "heading": 3.14,
-            "angularVelocity": 9.674484857548948e-16,
-            "velocityX": -2.0948065429741827,
-            "velocityY": -0.2199332259036739,
-            "timestamp": 2.14378102689895
-        },
-        {
-            "x": 7.005511097627955,
-            "y": 5.717474286699145,
-            "heading": 3.14,
-            "angularVelocity": 6.103023189064465e-16,
-            "velocityX": -2.9325601292436843,
-            "velocityY": -0.2643660269377714,
-            "timestamp": 2.2443947991535698
-        },
-        {
-            "x": 6.626163481456156,
-            "y": 5.686445611767655,
-            "heading": 3.14,
-            "angularVelocity": -3.898195701943848e-16,
-            "velocityX": -3.7703348879039833,
-            "velocityY": -0.30839391219134293,
-            "timestamp": 2.3450085714081896
-        },
-        {
-            "x": 6.24609124574338,
-            "y": 5.666109085648216,
-            "heading": 3.14,
-            "angularVelocity": 5.28249015807614e-17,
-            "velocityX": -3.7775368838161545,
-            "velocityY": -0.2021246762112827,
-            "timestamp": 2.4456223436628095
-        },
-        {
-            "x": 5.866019008161075,
-            "y": 5.6457725944687756,
-            "heading": 3.14,
-            "angularVelocity": 1.0714715731827337e-16,
-            "velocityX": -3.777536902397429,
-            "velocityY": -0.20212432894236246,
-            "timestamp": 2.5462361159174294
-        },
-        {
-            "x": 5.485946770578765,
-            "y": 5.625436103289443,
-            "heading": 3.14,
-            "angularVelocity": 4.4105778021487463e-16,
-            "velocityX": -3.7775369023974865,
-            "velocityY": -0.2021243289413037,
-            "timestamp": 2.6468498881720492
-        },
-        {
-            "x": 5.105874532996473,
-            "y": 5.60509961211012,
-            "heading": 3.14,
-            "angularVelocity": 4.563638430357427e-16,
-            "velocityX": -3.7775369023972933,
-            "velocityY": -0.20212432894121146,
-            "timestamp": 2.747463660426669
-        },
-        {
-            "x": 4.768724738760508,
-            "y": 5.587059768280702,
-            "heading": 3.14,
-            "angularVelocity": 7.096592098456484e-16,
-            "velocityX": -3.3509308606209816,
-            "velocityY": -0.1792979574700793,
-            "timestamp": 2.848077432681289
-        },
-        {
-            "x": 4.5158623742843815,
-            "y": 5.573529884402755,
-            "heading": 3.14,
-            "angularVelocity": 5.58698579821863e-16,
-            "velocityX": -2.5131983323104805,
-            "velocityY": -0.13447347810003318,
-            "timestamp": 2.948691204935909
-        },
-        {
-            "x": 4.3472874591846,
-            "y": 5.564509961525897,
-            "heading": 3.14,
-            "angularVelocity": 2.9765197430594465e-16,
-            "velocityX": -1.6754656090315643,
-            "velocityY": -0.08964898829784126,
-            "timestamp": 3.0493049771905287
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 1.413897346526725e-16,
-            "velocityX": -0.8377328207631561,
-            "velocityY": -0.04482449501826641,
-            "timestamp": 3.1499187494451486
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": -1.608253361776519e-31,
-            "velocityY": 0,
-            "timestamp": 3.2505325216997685
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S1.1.traj
deleted file mode 100644
index b581bfce..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S1.1.traj	
+++ /dev/null
@@ -1,337 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3161821396104223,
-            "y": 5.531175019358262,
-            "heading": 3.133818954660017,
-            "angularVelocity": -0.0664208658906851,
-            "velocityX": 0.711186341084945,
-            "velocityY": -0.3097502231737145,
-            "timestamp": 0.09305878893773292
-        },
-        {
-            "x": 2.448546415336416,
-            "y": 5.473525059620157,
-            "heading": 3.121456701812491,
-            "angularVelocity": -0.13284347441699146,
-            "velocityX": 1.4223726446145823,
-            "velocityY": -0.6195004297410188,
-            "timestamp": 0.18611757787546584
-        },
-        {
-            "x": 2.647092820298068,
-            "y": 5.387050123667136,
-            "heading": 3.1029129720677258,
-            "angularVelocity": -0.19926897777675723,
-            "velocityX": 2.1335588742134064,
-            "velocityY": -0.9292506053445697,
-            "timestamp": 0.2791763668131988
-        },
-        {
-            "x": 2.911821335852912,
-            "y": 5.271750219180522,
-            "heading": 3.078187424791321,
-            "angularVelocity": -0.26569814155811494,
-            "velocityX": 2.844744903482224,
-            "velocityY": -1.2390006984054311,
-            "timestamp": 0.3722351557509317
-        },
-        {
-            "x": 3.2345733580792335,
-            "y": 5.131178588369686,
-            "heading": 3.07818742179356,
-            "angularVelocity": -3.221363014134022e-8,
-            "velocityX": 3.468259429448196,
-            "velocityY": -1.5105680228107692,
-            "timestamp": 0.4652939446886646
-        },
-        {
-            "x": 3.557325380260581,
-            "y": 4.990606957455228,
-            "heading": 3.078187418795868,
-            "angularVelocity": -3.2212882318357466e-8,
-            "velocityX": 3.4682594289649007,
-            "velocityY": -1.510568023924279,
-            "timestamp": 0.5583527336263975
-        },
-        {
-            "x": 3.8800774024419273,
-            "y": 4.85003532654077,
-            "heading": 3.0781874157981757,
-            "angularVelocity": -3.221288214692397e-8,
-            "velocityX": 3.468259428964899,
-            "velocityY": -1.5105680239242834,
-            "timestamp": 0.6514115225641304
-        },
-        {
-            "x": 4.202829424623274,
-            "y": 4.709463695626312,
-            "heading": 3.078187412800484,
-            "angularVelocity": -3.221288189984943e-8,
-            "velocityX": 3.468259428964899,
-            "velocityY": -1.5105680239242838,
-            "timestamp": 0.7444703115018633
-        },
-        {
-            "x": 4.525581446804621,
-            "y": 4.568892064711854,
-            "heading": 3.078187409802792,
-            "angularVelocity": -3.2212882353617496e-8,
-            "velocityX": 3.468259428964899,
-            "velocityY": -1.5105680239242834,
-            "timestamp": 0.8375291004395962
-        },
-        {
-            "x": 4.848333468985968,
-            "y": 4.428320433797395,
-            "heading": 3.0781874068051,
-            "angularVelocity": -3.221288275372931e-8,
-            "velocityX": 3.468259428964899,
-            "velocityY": -1.5105680239242836,
-            "timestamp": 0.9305878893773291
-        },
-        {
-            "x": 5.1710854911673145,
-            "y": 4.287748802882937,
-            "heading": 3.0781874038074077,
-            "angularVelocity": -3.221288266203546e-8,
-            "velocityX": 3.4682594289648985,
-            "velocityY": -1.5105680239242834,
-            "timestamp": 1.023646678315062
-        },
-        {
-            "x": 5.493837513348661,
-            "y": 4.147177171968479,
-            "heading": 3.078187400809716,
-            "angularVelocity": -3.221288308232409e-8,
-            "velocityX": 3.4682594289648994,
-            "velocityY": -1.5105680239242831,
-            "timestamp": 1.116705467252795
-        },
-        {
-            "x": 5.816589535530807,
-            "y": 4.006605541055855,
-            "heading": 3.078187397812024,
-            "angularVelocity": -3.221288143740925e-8,
-            "velocityX": 3.4682594289734867,
-            "velocityY": -1.510568023904566,
-            "timestamp": 1.209764256190528
-        },
-        {
-            "x": 6.139341766628845,
-            "y": 3.866034389814445,
-            "heading": 3.0781873948143317,
-            "angularVelocity": -3.2212889577605074e-8,
-            "velocityX": 3.4682616739617793,
-            "velocityY": -1.5105628694079405,
-            "timestamp": 1.302823045128261
-        },
-        {
-            "x": 6.479618734001887,
-            "y": 3.7758078084398288,
-            "heading": 3.078187391647213,
-            "angularVelocity": -3.403352419183157e-8,
-            "velocityX": 3.656580654630348,
-            "velocityY": -0.9695653941401335,
-            "timestamp": 1.395881834065994
-        },
-        {
-            "x": 6.831146782632028,
-            "y": 3.7569067855053886,
-            "heading": 3.078187386672884,
-            "angularVelocity": -5.3453615233680875e-8,
-            "velocityX": 3.777483595508151,
-            "velocityY": -0.20310841297416227,
-            "timestamp": 1.488940623003727
-        },
-        {
-            "x": 7.135392516475817,
-            "y": 3.7925535020762564,
-            "heading": 3.084237894820064,
-            "angularVelocity": 0.06501812688781647,
-            "velocityX": 3.269392792628807,
-            "velocityY": 0.3830558830366858,
-            "timestamp": 1.58199941194146
-        },
-        {
-            "x": 7.379778215429103,
-            "y": 3.868544177421427,
-            "heading": 3.09656532372019,
-            "angularVelocity": 0.1324692599252942,
-            "velocityX": 2.6261431267583744,
-            "velocityY": 0.8165878388554683,
-            "timestamp": 1.675058200879193
-        },
-        {
-            "x": 7.561679450282434,
-            "y": 3.980681271949268,
-            "heading": 3.1151578247073277,
-            "angularVelocity": 0.1997930684395467,
-            "velocityX": 1.954691619456221,
-            "velocityY": 1.2050134738253881,
-            "timestamp": 1.768116989816926
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.26695141400662953,
-            "velocityX": 1.2714602357090166,
-            "velocityY": 1.572325727864734,
-            "timestamp": 1.861175778754659
-        },
-        {
-            "x": 7.734511016491924,
-            "y": 4.293948601996246,
-            "heading": 3.168900193264706,
-            "angularVelocity": 0.3300287650543787,
-            "velocityX": 0.6224942265925568,
-            "velocityY": 1.9064869372920104,
-            "timestamp": 1.948744488894043
-        },
-        {
-            "x": 7.731795753069985,
-            "y": 4.489388700342548,
-            "heading": 3.202314872595517,
-            "angularVelocity": 0.38158240857521825,
-            "velocityX": -0.031007233264223562,
-            "velocityY": 2.2318485453904535,
-            "timestamp": 2.0363131990334273
-        },
-        {
-            "x": 7.671228890668297,
-            "y": 4.712045759892295,
-            "heading": 3.2385899184542097,
-            "angularVelocity": 0.41424666186076586,
-            "velocityX": -0.6916495892800459,
-            "velocityY": 2.5426554667225383,
-            "timestamp": 2.1238819091728116
-        },
-        {
-            "x": 7.551691509705012,
-            "y": 4.959414788228928,
-            "heading": 3.2745248408990206,
-            "angularVelocity": 0.4103625871342999,
-            "velocityX": -1.3650695639231716,
-            "velocityY": 2.824856366422358,
-            "timestamp": 2.211450619312196
-        },
-        {
-            "x": 7.370754702688456,
-            "y": 5.224445119410505,
-            "heading": 3.301432833728465,
-            "angularVelocity": 0.30727862482631746,
-            "velocityX": -2.066226700479633,
-            "velocityY": 3.0265414525316805,
-            "timestamp": 2.29901932945158
-        },
-        {
-            "x": 7.135099709363178,
-            "y": 5.457264131297548,
-            "heading": 3.3014328467429856,
-            "angularVelocity": 1.4862067021464798e-7,
-            "velocityX": -2.6910867243583128,
-            "velocityY": 2.658700939142107,
-            "timestamp": 2.3865880395909644
-        },
-        {
-            "x": 6.859107156435953,
-            "y": 5.640474617463791,
-            "heading": 3.3014328522641176,
-            "angularVelocity": 6.304914289451061e-8,
-            "velocityX": -3.1517256847557116,
-            "velocityY": 2.0921912161846987,
-            "timestamp": 2.4741567497303487
-        },
-        {
-            "x": 6.573538412185887,
-            "y": 5.808368822234562,
-            "heading": 3.3014328577109833,
-            "angularVelocity": 6.220104875802801e-8,
-            "velocityX": -3.26108199830194,
-            "velocityY": 1.9172853465984772,
-            "timestamp": 2.561725459869733
-        },
-        {
-            "x": 6.287969619272061,
-            "y": 5.976262944233841,
-            "heading": 3.301432863157849,
-            "angularVelocity": 6.220104736843505e-8,
-            "velocityX": -3.2610825540228303,
-            "velocityY": 1.9172844013808124,
-            "timestamp": 2.6492941700091173
-        },
-        {
-            "x": 6.002400826335374,
-            "y": 6.144157066194225,
-            "heading": 3.3014328686047243,
-            "angularVelocity": 6.2201164432783e-8,
-            "velocityX": -3.2610825542838673,
-            "velocityY": 1.9172844009366552,
-            "timestamp": 2.7368628801485015
-        },
-        {
-            "x": 5.726906238126056,
-            "y": 6.306118202865447,
-            "heading": 3.330104518338368,
-            "angularVelocity": 0.3274188884135285,
-            "velocityX": -3.14603912482906,
-            "velocityY": 1.849532057894034,
-            "timestamp": 2.824431590287886
-        },
-        {
-            "x": 5.5065316534228295,
-            "y": 6.435677156249956,
-            "heading": 3.357685145070299,
-            "angularVelocity": 0.31495983768665414,
-            "velocityX": -2.5165905076419874,
-            "velocityY": 1.47951195327976,
-            "timestamp": 2.91200030042727
-        },
-        {
-            "x": 5.341261355658222,
-            "y": 6.532840911354814,
-            "heading": 3.379877882623065,
-            "angularVelocity": 0.2534322764083417,
-            "velocityX": -1.8873213674330116,
-            "velocityY": 1.1095716146806525,
-            "timestamp": 2.9995690105666544
-        },
-        {
-            "x": 5.231085952105995,
-            "y": 6.597614249467722,
-            "heading": 3.395277171030749,
-            "angularVelocity": 0.17585377680193118,
-            "velocityX": -1.2581594884389657,
-            "velocityY": 0.7396858764940984,
-            "timestamp": 3.0871377207060386
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.09030778386766043,
-            "velocityX": -0.6290597636794472,
-            "velocityY": 0.3698324490646197,
-            "timestamp": 3.174706430845423
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0,
-            "velocityX": -1.4368373863628223e-33,
-            "velocityY": 0,
-            "timestamp": 3.262275140984807
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S1.traj
deleted file mode 100644
index b581bfce..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S1.traj	
+++ /dev/null
@@ -1,337 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3161821396104223,
-            "y": 5.531175019358262,
-            "heading": 3.133818954660017,
-            "angularVelocity": -0.0664208658906851,
-            "velocityX": 0.711186341084945,
-            "velocityY": -0.3097502231737145,
-            "timestamp": 0.09305878893773292
-        },
-        {
-            "x": 2.448546415336416,
-            "y": 5.473525059620157,
-            "heading": 3.121456701812491,
-            "angularVelocity": -0.13284347441699146,
-            "velocityX": 1.4223726446145823,
-            "velocityY": -0.6195004297410188,
-            "timestamp": 0.18611757787546584
-        },
-        {
-            "x": 2.647092820298068,
-            "y": 5.387050123667136,
-            "heading": 3.1029129720677258,
-            "angularVelocity": -0.19926897777675723,
-            "velocityX": 2.1335588742134064,
-            "velocityY": -0.9292506053445697,
-            "timestamp": 0.2791763668131988
-        },
-        {
-            "x": 2.911821335852912,
-            "y": 5.271750219180522,
-            "heading": 3.078187424791321,
-            "angularVelocity": -0.26569814155811494,
-            "velocityX": 2.844744903482224,
-            "velocityY": -1.2390006984054311,
-            "timestamp": 0.3722351557509317
-        },
-        {
-            "x": 3.2345733580792335,
-            "y": 5.131178588369686,
-            "heading": 3.07818742179356,
-            "angularVelocity": -3.221363014134022e-8,
-            "velocityX": 3.468259429448196,
-            "velocityY": -1.5105680228107692,
-            "timestamp": 0.4652939446886646
-        },
-        {
-            "x": 3.557325380260581,
-            "y": 4.990606957455228,
-            "heading": 3.078187418795868,
-            "angularVelocity": -3.2212882318357466e-8,
-            "velocityX": 3.4682594289649007,
-            "velocityY": -1.510568023924279,
-            "timestamp": 0.5583527336263975
-        },
-        {
-            "x": 3.8800774024419273,
-            "y": 4.85003532654077,
-            "heading": 3.0781874157981757,
-            "angularVelocity": -3.221288214692397e-8,
-            "velocityX": 3.468259428964899,
-            "velocityY": -1.5105680239242834,
-            "timestamp": 0.6514115225641304
-        },
-        {
-            "x": 4.202829424623274,
-            "y": 4.709463695626312,
-            "heading": 3.078187412800484,
-            "angularVelocity": -3.221288189984943e-8,
-            "velocityX": 3.468259428964899,
-            "velocityY": -1.5105680239242838,
-            "timestamp": 0.7444703115018633
-        },
-        {
-            "x": 4.525581446804621,
-            "y": 4.568892064711854,
-            "heading": 3.078187409802792,
-            "angularVelocity": -3.2212882353617496e-8,
-            "velocityX": 3.468259428964899,
-            "velocityY": -1.5105680239242834,
-            "timestamp": 0.8375291004395962
-        },
-        {
-            "x": 4.848333468985968,
-            "y": 4.428320433797395,
-            "heading": 3.0781874068051,
-            "angularVelocity": -3.221288275372931e-8,
-            "velocityX": 3.468259428964899,
-            "velocityY": -1.5105680239242836,
-            "timestamp": 0.9305878893773291
-        },
-        {
-            "x": 5.1710854911673145,
-            "y": 4.287748802882937,
-            "heading": 3.0781874038074077,
-            "angularVelocity": -3.221288266203546e-8,
-            "velocityX": 3.4682594289648985,
-            "velocityY": -1.5105680239242834,
-            "timestamp": 1.023646678315062
-        },
-        {
-            "x": 5.493837513348661,
-            "y": 4.147177171968479,
-            "heading": 3.078187400809716,
-            "angularVelocity": -3.221288308232409e-8,
-            "velocityX": 3.4682594289648994,
-            "velocityY": -1.5105680239242831,
-            "timestamp": 1.116705467252795
-        },
-        {
-            "x": 5.816589535530807,
-            "y": 4.006605541055855,
-            "heading": 3.078187397812024,
-            "angularVelocity": -3.221288143740925e-8,
-            "velocityX": 3.4682594289734867,
-            "velocityY": -1.510568023904566,
-            "timestamp": 1.209764256190528
-        },
-        {
-            "x": 6.139341766628845,
-            "y": 3.866034389814445,
-            "heading": 3.0781873948143317,
-            "angularVelocity": -3.2212889577605074e-8,
-            "velocityX": 3.4682616739617793,
-            "velocityY": -1.5105628694079405,
-            "timestamp": 1.302823045128261
-        },
-        {
-            "x": 6.479618734001887,
-            "y": 3.7758078084398288,
-            "heading": 3.078187391647213,
-            "angularVelocity": -3.403352419183157e-8,
-            "velocityX": 3.656580654630348,
-            "velocityY": -0.9695653941401335,
-            "timestamp": 1.395881834065994
-        },
-        {
-            "x": 6.831146782632028,
-            "y": 3.7569067855053886,
-            "heading": 3.078187386672884,
-            "angularVelocity": -5.3453615233680875e-8,
-            "velocityX": 3.777483595508151,
-            "velocityY": -0.20310841297416227,
-            "timestamp": 1.488940623003727
-        },
-        {
-            "x": 7.135392516475817,
-            "y": 3.7925535020762564,
-            "heading": 3.084237894820064,
-            "angularVelocity": 0.06501812688781647,
-            "velocityX": 3.269392792628807,
-            "velocityY": 0.3830558830366858,
-            "timestamp": 1.58199941194146
-        },
-        {
-            "x": 7.379778215429103,
-            "y": 3.868544177421427,
-            "heading": 3.09656532372019,
-            "angularVelocity": 0.1324692599252942,
-            "velocityX": 2.6261431267583744,
-            "velocityY": 0.8165878388554683,
-            "timestamp": 1.675058200879193
-        },
-        {
-            "x": 7.561679450282434,
-            "y": 3.980681271949268,
-            "heading": 3.1151578247073277,
-            "angularVelocity": 0.1997930684395467,
-            "velocityX": 1.954691619456221,
-            "velocityY": 1.2050134738253881,
-            "timestamp": 1.768116989816926
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.26695141400662953,
-            "velocityX": 1.2714602357090166,
-            "velocityY": 1.572325727864734,
-            "timestamp": 1.861175778754659
-        },
-        {
-            "x": 7.734511016491924,
-            "y": 4.293948601996246,
-            "heading": 3.168900193264706,
-            "angularVelocity": 0.3300287650543787,
-            "velocityX": 0.6224942265925568,
-            "velocityY": 1.9064869372920104,
-            "timestamp": 1.948744488894043
-        },
-        {
-            "x": 7.731795753069985,
-            "y": 4.489388700342548,
-            "heading": 3.202314872595517,
-            "angularVelocity": 0.38158240857521825,
-            "velocityX": -0.031007233264223562,
-            "velocityY": 2.2318485453904535,
-            "timestamp": 2.0363131990334273
-        },
-        {
-            "x": 7.671228890668297,
-            "y": 4.712045759892295,
-            "heading": 3.2385899184542097,
-            "angularVelocity": 0.41424666186076586,
-            "velocityX": -0.6916495892800459,
-            "velocityY": 2.5426554667225383,
-            "timestamp": 2.1238819091728116
-        },
-        {
-            "x": 7.551691509705012,
-            "y": 4.959414788228928,
-            "heading": 3.2745248408990206,
-            "angularVelocity": 0.4103625871342999,
-            "velocityX": -1.3650695639231716,
-            "velocityY": 2.824856366422358,
-            "timestamp": 2.211450619312196
-        },
-        {
-            "x": 7.370754702688456,
-            "y": 5.224445119410505,
-            "heading": 3.301432833728465,
-            "angularVelocity": 0.30727862482631746,
-            "velocityX": -2.066226700479633,
-            "velocityY": 3.0265414525316805,
-            "timestamp": 2.29901932945158
-        },
-        {
-            "x": 7.135099709363178,
-            "y": 5.457264131297548,
-            "heading": 3.3014328467429856,
-            "angularVelocity": 1.4862067021464798e-7,
-            "velocityX": -2.6910867243583128,
-            "velocityY": 2.658700939142107,
-            "timestamp": 2.3865880395909644
-        },
-        {
-            "x": 6.859107156435953,
-            "y": 5.640474617463791,
-            "heading": 3.3014328522641176,
-            "angularVelocity": 6.304914289451061e-8,
-            "velocityX": -3.1517256847557116,
-            "velocityY": 2.0921912161846987,
-            "timestamp": 2.4741567497303487
-        },
-        {
-            "x": 6.573538412185887,
-            "y": 5.808368822234562,
-            "heading": 3.3014328577109833,
-            "angularVelocity": 6.220104875802801e-8,
-            "velocityX": -3.26108199830194,
-            "velocityY": 1.9172853465984772,
-            "timestamp": 2.561725459869733
-        },
-        {
-            "x": 6.287969619272061,
-            "y": 5.976262944233841,
-            "heading": 3.301432863157849,
-            "angularVelocity": 6.220104736843505e-8,
-            "velocityX": -3.2610825540228303,
-            "velocityY": 1.9172844013808124,
-            "timestamp": 2.6492941700091173
-        },
-        {
-            "x": 6.002400826335374,
-            "y": 6.144157066194225,
-            "heading": 3.3014328686047243,
-            "angularVelocity": 6.2201164432783e-8,
-            "velocityX": -3.2610825542838673,
-            "velocityY": 1.9172844009366552,
-            "timestamp": 2.7368628801485015
-        },
-        {
-            "x": 5.726906238126056,
-            "y": 6.306118202865447,
-            "heading": 3.330104518338368,
-            "angularVelocity": 0.3274188884135285,
-            "velocityX": -3.14603912482906,
-            "velocityY": 1.849532057894034,
-            "timestamp": 2.824431590287886
-        },
-        {
-            "x": 5.5065316534228295,
-            "y": 6.435677156249956,
-            "heading": 3.357685145070299,
-            "angularVelocity": 0.31495983768665414,
-            "velocityX": -2.5165905076419874,
-            "velocityY": 1.47951195327976,
-            "timestamp": 2.91200030042727
-        },
-        {
-            "x": 5.341261355658222,
-            "y": 6.532840911354814,
-            "heading": 3.379877882623065,
-            "angularVelocity": 0.2534322764083417,
-            "velocityX": -1.8873213674330116,
-            "velocityY": 1.1095716146806525,
-            "timestamp": 2.9995690105666544
-        },
-        {
-            "x": 5.231085952105995,
-            "y": 6.597614249467722,
-            "heading": 3.395277171030749,
-            "angularVelocity": 0.17585377680193118,
-            "velocityX": -1.2581594884389657,
-            "velocityY": 0.7396858764940984,
-            "timestamp": 3.0871377207060386
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.09030778386766043,
-            "velocityX": -0.6290597636794472,
-            "velocityY": 0.3698324490646197,
-            "timestamp": 3.174706430845423
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0,
-            "velocityX": -1.4368373863628223e-33,
-            "velocityY": 0,
-            "timestamp": 3.262275140984807
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S2.1.traj
deleted file mode 100644
index 9d250387..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S2.1.traj	
+++ /dev/null
@@ -1,328 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3211310801785023,
-            "y": 5.537078029621854,
-            "heading": 3.14,
-            "angularVelocity": 6.136916649287371e-17,
-            "velocityX": 0.7513410861003985,
-            "velocityY": -0.24211945152891107,
-            "timestamp": 0.09467215555545593
-        },
-        {
-            "x": 2.4633932375532455,
-            "y": 5.491234089826592,
-            "heading": 3.1399999999999997,
-            "angularVelocity": -6.918142497868558e-18,
-            "velocityX": 1.5026821406998672,
-            "velocityY": -0.48423889290665645,
-            "timestamp": 0.18934431111091185
-        },
-        {
-            "x": 2.676786466159711,
-            "y": 5.422468182536282,
-            "heading": 3.14,
-            "angularVelocity": -1.447784282504661e-16,
-            "velocityX": 2.254023132297508,
-            "velocityY": -0.7263583139820807,
-            "timestamp": 0.28401646666636776
-        },
-        {
-            "x": 2.9613107481043692,
-            "y": 5.330780313517108,
-            "heading": 3.14,
-            "angularVelocity": -2.19469189514941e-16,
-            "velocityX": 3.005363934889942,
-            "velocityY": -0.9684776741506306,
-            "timestamp": 0.3786886222218237
-        },
-        {
-            "x": 3.30218779492529,
-            "y": 5.220932782027199,
-            "heading": 3.14,
-            "angularVelocity": 3.7716439529503614e-16,
-            "velocityX": 3.6006051073934406,
-            "velocityY": -1.160293972873203,
-            "timestamp": 0.47336077777727964
-        },
-        {
-            "x": 3.643064841746281,
-            "y": 5.111085250537267,
-            "heading": 3.14,
-            "angularVelocity": 1.8926244658706998e-16,
-            "velocityX": 3.600605107394178,
-            "velocityY": -1.1602939728734405,
-            "timestamp": 0.5680329333327355
-        },
-        {
-            "x": 3.9839418885672715,
-            "y": 5.001237719047335,
-            "heading": 3.14,
-            "angularVelocity": 3.4257793215779713e-16,
-            "velocityX": 3.6006051073941783,
-            "velocityY": -1.1602939728734407,
-            "timestamp": 0.6627050888881915
-        },
-        {
-            "x": 4.3248189353882625,
-            "y": 4.891390187557403,
-            "heading": 3.14,
-            "angularVelocity": 4.383897524805657e-17,
-            "velocityX": 3.6006051073941783,
-            "velocityY": -1.1602939728734407,
-            "timestamp": 0.7573772444436474
-        },
-        {
-            "x": 4.665695982209253,
-            "y": 4.7815426560674705,
-            "heading": 3.14,
-            "angularVelocity": 1.5398680786578701e-19,
-            "velocityX": 3.600605107394178,
-            "velocityY": -1.160293972873441,
-            "timestamp": 0.8520493999991033
-        },
-        {
-            "x": 5.0065730290302435,
-            "y": 4.671695124577538,
-            "heading": 3.14,
-            "angularVelocity": 4.356845547140055e-17,
-            "velocityX": 3.600605107394178,
-            "velocityY": -1.1602939728734414,
-            "timestamp": 0.9467215555545593
-        },
-        {
-            "x": 5.347450075851235,
-            "y": 4.561847593087606,
-            "heading": 3.14,
-            "angularVelocity": 2.68380689151133e-16,
-            "velocityX": 3.600605107394178,
-            "velocityY": -1.160293972873442,
-            "timestamp": 1.041393711110015
-        },
-        {
-            "x": 5.688327122672225,
-            "y": 4.452000061597674,
-            "heading": 3.14,
-            "angularVelocity": 3.726757040891467e-16,
-            "velocityX": 3.600605107394178,
-            "velocityY": -1.1602939728734432,
-            "timestamp": 1.136065866665471
-        },
-        {
-            "x": 6.029204169493216,
-            "y": 4.342152530107741,
-            "heading": 3.14,
-            "angularVelocity": 1.8883338122910111e-16,
-            "velocityX": 3.6006051073941774,
-            "velocityY": -1.1602939728734463,
-            "timestamp": 1.230738022220927
-        },
-        {
-            "x": 6.370081216318914,
-            "y": 4.2323049986209496,
-            "heading": 3.14,
-            "angularVelocity": 2.0987063063623018e-16,
-            "velocityX": 3.600605107443935,
-            "velocityY": -1.1602939728401256,
-            "timestamp": 1.325410177776383
-        },
-        {
-            "x": 6.710958611265409,
-            "y": 4.122458547431971,
-            "heading": 3.14,
-            "angularVelocity": 1.3456713754312597e-16,
-            "velocityX": 3.600608784563045,
-            "velocityY": -1.1602825619053307,
-            "timestamp": 1.4200823333318389
-        },
-        {
-            "x": 7.047028916950751,
-            "y": 4.077522582297829,
-            "heading": 3.14,
-            "angularVelocity": 3.8640318477602567e-16,
-            "velocityX": 3.549832616713609,
-            "velocityY": -0.4746481673570916,
-            "timestamp": 1.5147544888872948
-        },
-        {
-            "x": 7.31196824975774,
-            "y": 4.055508828572456,
-            "heading": 3.14,
-            "angularVelocity": 5.274988476955795e-16,
-            "velocityX": 2.7984926639997774,
-            "velocityY": -0.23252616987768301,
-            "timestamp": 1.6094266444427507
-        },
-        {
-            "x": 7.5057765524653535,
-            "y": 4.056417169982893,
-            "heading": 3.1400000000000006,
-            "angularVelocity": 4.746542936809208e-16,
-            "velocityX": 2.0471521068735608,
-            "velocityY": 0.009594599437474344,
-            "timestamp": 1.7040987999982067
-        },
-        {
-            "x": 7.628453805780265,
-            "y": 4.080247567089719,
-            "heading": 3.14,
-            "angularVelocity": 5.453879442658063e-16,
-            "velocityX": 1.295811345956418,
-            "velocityY": 0.2517149521631642,
-            "timestamp": 1.7987709555536626
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 4.116008282611605e-16,
-            "velocityX": 0.5444704825542928,
-            "velocityY": 0.49383509476441706,
-            "timestamp": 1.8934431111091186
-        },
-        {
-            "x": 7.6568603335776855,
-            "y": 4.200231008500336,
-            "heading": 3.14,
-            "angularVelocity": 4.411427948098232e-16,
-            "velocityX": -0.23545951737158613,
-            "velocityY": 0.7451679554678371,
-            "timestamp": 1.9917176114589898
-        },
-        {
-            "x": 7.557063724755448,
-            "y": 4.2981314655224265,
-            "heading": 3.14,
-            "angularVelocity": 5.768827951780328e-16,
-            "velocityX": -1.0154883359055142,
-            "velocityY": 0.9961938923479718,
-            "timestamp": 2.0899921118088614
-        },
-        {
-            "x": 7.380593913469942,
-            "y": 4.42065077709202,
-            "heading": 3.14,
-            "angularVelocity": 6.878777879965837e-16,
-            "velocityX": -1.7956826100081775,
-            "velocityY": 1.246705006216337,
-            "timestamp": 2.1882666121587326
-        },
-        {
-            "x": 7.127418097854458,
-            "y": 4.5676865279586805,
-            "heading": 3.1400000000000006,
-            "angularVelocity": 5.966587967087216e-16,
-            "velocityX": -2.576210662116218,
-            "velocityY": 1.4961739855526293,
-            "timestamp": 2.286541112508604
-        },
-        {
-            "x": 6.7974355684839916,
-            "y": 4.73892129278293,
-            "heading": 3.1400000000000006,
-            "angularVelocity": 9.996236488627517e-17,
-            "velocityX": -3.357763490993921,
-            "velocityY": 1.7424129780831201,
-            "timestamp": 2.384815612858475
-        },
-        {
-            "x": 6.443766002914483,
-            "y": 4.853499817181314,
-            "heading": 3.1400000000000006,
-            "angularVelocity": -3.2402717865838093e-16,
-            "velocityX": -3.5987928130938722,
-            "velocityY": 1.1659028943466263,
-            "timestamp": 2.4830901132083465
-        },
-        {
-            "x": 6.090096257463813,
-            "y": 4.968077786339023,
-            "heading": 3.1400000000000006,
-            "angularVelocity": -3.4427620459908776e-16,
-            "velocityX": -3.5987946434889486,
-            "velocityY": 1.165897244450957,
-            "timestamp": 2.581364613558218
-        },
-        {
-            "x": 5.736426512012546,
-            "y": 5.082655755494858,
-            "heading": 3.1400000000000006,
-            "angularVelocity": -3.8345847210221107e-16,
-            "velocityX": -3.598794643495036,
-            "velocityY": 1.165897244431872,
-            "timestamp": 2.679639113908089
-        },
-        {
-            "x": 5.382756766561277,
-            "y": 5.197233724650691,
-            "heading": 3.1400000000000006,
-            "angularVelocity": -2.673892854874729e-16,
-            "velocityX": -3.5987946434950357,
-            "velocityY": 1.165897244431872,
-            "timestamp": 2.7779136142579604
-        },
-        {
-            "x": 5.02908702111004,
-            "y": 5.311811693806516,
-            "heading": 3.1400000000000006,
-            "angularVelocity": -2.8589912756820243e-16,
-            "velocityX": -3.598794643494731,
-            "velocityY": 1.1658972444317732,
-            "timestamp": 2.8761881146078316
-        },
-        {
-            "x": 4.722652230446586,
-            "y": 5.411087010523561,
-            "heading": 3.14,
-            "angularVelocity": 1.1044854210040432e-15,
-            "velocityX": -3.1181516016108115,
-            "velocityY": 1.0101838865993877,
-            "timestamp": 2.974462614957703
-        },
-        {
-            "x": 4.492826119821717,
-            "y": 5.485543503772035,
-            "heading": 3.14,
-            "angularVelocity": 7.043876441844827e-16,
-            "velocityX": -2.338613880575899,
-            "velocityY": 0.7576379730591117,
-            "timestamp": 3.072737115307574
-        },
-        {
-            "x": 4.33960870762911,
-            "y": 5.535181167592957,
-            "heading": 3.14,
-            "angularVelocity": 5.590882375321651e-16,
-            "velocityX": -1.5590759723746228,
-            "velocityY": 0.5050919988827703,
-            "timestamp": 3.1710116156574455
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 3.3318762312703263e-16,
-            "velocityX": -0.7795380017845207,
-            "velocityY": 0.2525460044943946,
-            "timestamp": 3.269286116007317
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": -3.7380886634717777e-32,
-            "velocityY": 0,
-            "timestamp": 3.367560616357188
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S2.traj
deleted file mode 100644
index 9d250387..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S2.traj	
+++ /dev/null
@@ -1,328 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3211310801785023,
-            "y": 5.537078029621854,
-            "heading": 3.14,
-            "angularVelocity": 6.136916649287371e-17,
-            "velocityX": 0.7513410861003985,
-            "velocityY": -0.24211945152891107,
-            "timestamp": 0.09467215555545593
-        },
-        {
-            "x": 2.4633932375532455,
-            "y": 5.491234089826592,
-            "heading": 3.1399999999999997,
-            "angularVelocity": -6.918142497868558e-18,
-            "velocityX": 1.5026821406998672,
-            "velocityY": -0.48423889290665645,
-            "timestamp": 0.18934431111091185
-        },
-        {
-            "x": 2.676786466159711,
-            "y": 5.422468182536282,
-            "heading": 3.14,
-            "angularVelocity": -1.447784282504661e-16,
-            "velocityX": 2.254023132297508,
-            "velocityY": -0.7263583139820807,
-            "timestamp": 0.28401646666636776
-        },
-        {
-            "x": 2.9613107481043692,
-            "y": 5.330780313517108,
-            "heading": 3.14,
-            "angularVelocity": -2.19469189514941e-16,
-            "velocityX": 3.005363934889942,
-            "velocityY": -0.9684776741506306,
-            "timestamp": 0.3786886222218237
-        },
-        {
-            "x": 3.30218779492529,
-            "y": 5.220932782027199,
-            "heading": 3.14,
-            "angularVelocity": 3.7716439529503614e-16,
-            "velocityX": 3.6006051073934406,
-            "velocityY": -1.160293972873203,
-            "timestamp": 0.47336077777727964
-        },
-        {
-            "x": 3.643064841746281,
-            "y": 5.111085250537267,
-            "heading": 3.14,
-            "angularVelocity": 1.8926244658706998e-16,
-            "velocityX": 3.600605107394178,
-            "velocityY": -1.1602939728734405,
-            "timestamp": 0.5680329333327355
-        },
-        {
-            "x": 3.9839418885672715,
-            "y": 5.001237719047335,
-            "heading": 3.14,
-            "angularVelocity": 3.4257793215779713e-16,
-            "velocityX": 3.6006051073941783,
-            "velocityY": -1.1602939728734407,
-            "timestamp": 0.6627050888881915
-        },
-        {
-            "x": 4.3248189353882625,
-            "y": 4.891390187557403,
-            "heading": 3.14,
-            "angularVelocity": 4.383897524805657e-17,
-            "velocityX": 3.6006051073941783,
-            "velocityY": -1.1602939728734407,
-            "timestamp": 0.7573772444436474
-        },
-        {
-            "x": 4.665695982209253,
-            "y": 4.7815426560674705,
-            "heading": 3.14,
-            "angularVelocity": 1.5398680786578701e-19,
-            "velocityX": 3.600605107394178,
-            "velocityY": -1.160293972873441,
-            "timestamp": 0.8520493999991033
-        },
-        {
-            "x": 5.0065730290302435,
-            "y": 4.671695124577538,
-            "heading": 3.14,
-            "angularVelocity": 4.356845547140055e-17,
-            "velocityX": 3.600605107394178,
-            "velocityY": -1.1602939728734414,
-            "timestamp": 0.9467215555545593
-        },
-        {
-            "x": 5.347450075851235,
-            "y": 4.561847593087606,
-            "heading": 3.14,
-            "angularVelocity": 2.68380689151133e-16,
-            "velocityX": 3.600605107394178,
-            "velocityY": -1.160293972873442,
-            "timestamp": 1.041393711110015
-        },
-        {
-            "x": 5.688327122672225,
-            "y": 4.452000061597674,
-            "heading": 3.14,
-            "angularVelocity": 3.726757040891467e-16,
-            "velocityX": 3.600605107394178,
-            "velocityY": -1.1602939728734432,
-            "timestamp": 1.136065866665471
-        },
-        {
-            "x": 6.029204169493216,
-            "y": 4.342152530107741,
-            "heading": 3.14,
-            "angularVelocity": 1.8883338122910111e-16,
-            "velocityX": 3.6006051073941774,
-            "velocityY": -1.1602939728734463,
-            "timestamp": 1.230738022220927
-        },
-        {
-            "x": 6.370081216318914,
-            "y": 4.2323049986209496,
-            "heading": 3.14,
-            "angularVelocity": 2.0987063063623018e-16,
-            "velocityX": 3.600605107443935,
-            "velocityY": -1.1602939728401256,
-            "timestamp": 1.325410177776383
-        },
-        {
-            "x": 6.710958611265409,
-            "y": 4.122458547431971,
-            "heading": 3.14,
-            "angularVelocity": 1.3456713754312597e-16,
-            "velocityX": 3.600608784563045,
-            "velocityY": -1.1602825619053307,
-            "timestamp": 1.4200823333318389
-        },
-        {
-            "x": 7.047028916950751,
-            "y": 4.077522582297829,
-            "heading": 3.14,
-            "angularVelocity": 3.8640318477602567e-16,
-            "velocityX": 3.549832616713609,
-            "velocityY": -0.4746481673570916,
-            "timestamp": 1.5147544888872948
-        },
-        {
-            "x": 7.31196824975774,
-            "y": 4.055508828572456,
-            "heading": 3.14,
-            "angularVelocity": 5.274988476955795e-16,
-            "velocityX": 2.7984926639997774,
-            "velocityY": -0.23252616987768301,
-            "timestamp": 1.6094266444427507
-        },
-        {
-            "x": 7.5057765524653535,
-            "y": 4.056417169982893,
-            "heading": 3.1400000000000006,
-            "angularVelocity": 4.746542936809208e-16,
-            "velocityX": 2.0471521068735608,
-            "velocityY": 0.009594599437474344,
-            "timestamp": 1.7040987999982067
-        },
-        {
-            "x": 7.628453805780265,
-            "y": 4.080247567089719,
-            "heading": 3.14,
-            "angularVelocity": 5.453879442658063e-16,
-            "velocityX": 1.295811345956418,
-            "velocityY": 0.2517149521631642,
-            "timestamp": 1.7987709555536626
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 4.116008282611605e-16,
-            "velocityX": 0.5444704825542928,
-            "velocityY": 0.49383509476441706,
-            "timestamp": 1.8934431111091186
-        },
-        {
-            "x": 7.6568603335776855,
-            "y": 4.200231008500336,
-            "heading": 3.14,
-            "angularVelocity": 4.411427948098232e-16,
-            "velocityX": -0.23545951737158613,
-            "velocityY": 0.7451679554678371,
-            "timestamp": 1.9917176114589898
-        },
-        {
-            "x": 7.557063724755448,
-            "y": 4.2981314655224265,
-            "heading": 3.14,
-            "angularVelocity": 5.768827951780328e-16,
-            "velocityX": -1.0154883359055142,
-            "velocityY": 0.9961938923479718,
-            "timestamp": 2.0899921118088614
-        },
-        {
-            "x": 7.380593913469942,
-            "y": 4.42065077709202,
-            "heading": 3.14,
-            "angularVelocity": 6.878777879965837e-16,
-            "velocityX": -1.7956826100081775,
-            "velocityY": 1.246705006216337,
-            "timestamp": 2.1882666121587326
-        },
-        {
-            "x": 7.127418097854458,
-            "y": 4.5676865279586805,
-            "heading": 3.1400000000000006,
-            "angularVelocity": 5.966587967087216e-16,
-            "velocityX": -2.576210662116218,
-            "velocityY": 1.4961739855526293,
-            "timestamp": 2.286541112508604
-        },
-        {
-            "x": 6.7974355684839916,
-            "y": 4.73892129278293,
-            "heading": 3.1400000000000006,
-            "angularVelocity": 9.996236488627517e-17,
-            "velocityX": -3.357763490993921,
-            "velocityY": 1.7424129780831201,
-            "timestamp": 2.384815612858475
-        },
-        {
-            "x": 6.443766002914483,
-            "y": 4.853499817181314,
-            "heading": 3.1400000000000006,
-            "angularVelocity": -3.2402717865838093e-16,
-            "velocityX": -3.5987928130938722,
-            "velocityY": 1.1659028943466263,
-            "timestamp": 2.4830901132083465
-        },
-        {
-            "x": 6.090096257463813,
-            "y": 4.968077786339023,
-            "heading": 3.1400000000000006,
-            "angularVelocity": -3.4427620459908776e-16,
-            "velocityX": -3.5987946434889486,
-            "velocityY": 1.165897244450957,
-            "timestamp": 2.581364613558218
-        },
-        {
-            "x": 5.736426512012546,
-            "y": 5.082655755494858,
-            "heading": 3.1400000000000006,
-            "angularVelocity": -3.8345847210221107e-16,
-            "velocityX": -3.598794643495036,
-            "velocityY": 1.165897244431872,
-            "timestamp": 2.679639113908089
-        },
-        {
-            "x": 5.382756766561277,
-            "y": 5.197233724650691,
-            "heading": 3.1400000000000006,
-            "angularVelocity": -2.673892854874729e-16,
-            "velocityX": -3.5987946434950357,
-            "velocityY": 1.165897244431872,
-            "timestamp": 2.7779136142579604
-        },
-        {
-            "x": 5.02908702111004,
-            "y": 5.311811693806516,
-            "heading": 3.1400000000000006,
-            "angularVelocity": -2.8589912756820243e-16,
-            "velocityX": -3.598794643494731,
-            "velocityY": 1.1658972444317732,
-            "timestamp": 2.8761881146078316
-        },
-        {
-            "x": 4.722652230446586,
-            "y": 5.411087010523561,
-            "heading": 3.14,
-            "angularVelocity": 1.1044854210040432e-15,
-            "velocityX": -3.1181516016108115,
-            "velocityY": 1.0101838865993877,
-            "timestamp": 2.974462614957703
-        },
-        {
-            "x": 4.492826119821717,
-            "y": 5.485543503772035,
-            "heading": 3.14,
-            "angularVelocity": 7.043876441844827e-16,
-            "velocityX": -2.338613880575899,
-            "velocityY": 0.7576379730591117,
-            "timestamp": 3.072737115307574
-        },
-        {
-            "x": 4.33960870762911,
-            "y": 5.535181167592957,
-            "heading": 3.14,
-            "angularVelocity": 5.590882375321651e-16,
-            "velocityX": -1.5590759723746228,
-            "velocityY": 0.5050919988827703,
-            "timestamp": 3.1710116156574455
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 3.3318762312703263e-16,
-            "velocityX": -0.7795380017845207,
-            "velocityY": 0.2525460044943946,
-            "timestamp": 3.269286116007317
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": -3.7380886634717777e-32,
-            "velocityY": 0,
-            "timestamp": 3.367560616357188
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S3.1.traj
deleted file mode 100644
index f9f681f0..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S3.1.traj	
+++ /dev/null
@@ -1,346 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -2.1257809849356417e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 2.321508847678769,
-            "y": 5.553282811958262,
-            "heading": 3.1537553335344852,
-            "angularVelocity": 0.14810177732022164,
-            "velocityX": 0.7699258915674908,
-            "velocityY": -0.07232303637577411,
-            "timestamp": 0.09287757232476541
-        },
-        {
-            "x": 2.4645265668919087,
-            "y": 5.539848505148353,
-            "heading": 3.1812640229350246,
-            "angularVelocity": 0.2961822613574502,
-            "velocityX": 1.53985203998495,
-            "velocityY": -0.14464532689263399,
-            "timestamp": 0.18575514464953083
-        },
-        {
-            "x": 2.679053206199872,
-            "y": 5.519697173536265,
-            "heading": 3.2225218511092746,
-            "angularVelocity": 0.4442173405435694,
-            "velocityX": 2.309778711246101,
-            "velocityY": -0.21696660569059611,
-            "timestamp": 0.27863271697429626
-        },
-        {
-            "x": 2.965088840346323,
-            "y": 5.49282892094042,
-            "heading": 3.277521318880573,
-            "angularVelocity": 0.5921716771297765,
-            "velocityX": 3.079706187261968,
-            "velocityY": -0.2892867666899661,
-            "timestamp": 0.37151028929906166
-        },
-        {
-            "x": 3.3148991000673105,
-            "y": 5.459968004407915,
-            "heading": 3.277521325558456,
-            "angularVelocity": 7.189983956229139e-8,
-            "velocityX": 3.7663587770985667,
-            "velocityY": -0.35380895204280205,
-            "timestamp": 0.46438786162382706
-        },
-        {
-            "x": 3.664709359511986,
-            "y": 5.427107084931802,
-            "heading": 3.2775213322361236,
-            "angularVelocity": 7.18975223410498e-8,
-            "velocityX": 3.7663587741235602,
-            "velocityY": -0.3538089837362208,
-            "timestamp": 0.5572654339485925
-        },
-        {
-            "x": 4.014519618956661,
-            "y": 5.394246165455678,
-            "heading": 3.277521338913791,
-            "angularVelocity": 7.189752294324541e-8,
-            "velocityX": 3.7663587741235487,
-            "velocityY": -0.3538089837363427,
-            "timestamp": 0.650143006273358
-        },
-        {
-            "x": 4.364329878401335,
-            "y": 5.361385245979554,
-            "heading": 3.2775213455914582,
-            "angularVelocity": 7.189752233099723e-8,
-            "velocityX": 3.7663587741235482,
-            "velocityY": -0.3538089837363424,
-            "timestamp": 0.7430205785981234
-        },
-        {
-            "x": 4.714140137846011,
-            "y": 5.32852432650343,
-            "heading": 3.277521352269126,
-            "angularVelocity": 7.189752328963948e-8,
-            "velocityX": 3.7663587741235482,
-            "velocityY": -0.35380898373634245,
-            "timestamp": 0.8358981509228889
-        },
-        {
-            "x": 5.063950397290685,
-            "y": 5.295663407027306,
-            "heading": 3.2775213589467933,
-            "angularVelocity": 7.189752245277794e-8,
-            "velocityX": 3.7663587741235482,
-            "velocityY": -0.3538089837363426,
-            "timestamp": 0.9287757232476543
-        },
-        {
-            "x": 5.41376065673536,
-            "y": 5.262802487551181,
-            "heading": 3.2775213656244606,
-            "angularVelocity": 7.189752262506364e-8,
-            "velocityX": 3.7663587741235482,
-            "velocityY": -0.3538089837363426,
-            "timestamp": 1.0216532955724198
-        },
-        {
-            "x": 5.7635709161800355,
-            "y": 5.229941568075057,
-            "heading": 3.277521372302128,
-            "angularVelocity": 7.189752282304632e-8,
-            "velocityX": 3.7663587741235482,
-            "velocityY": -0.3538089837363426,
-            "timestamp": 1.1145308678971853
-        },
-        {
-            "x": 6.113381175624392,
-            "y": 5.19708064859555,
-            "heading": 3.2775213789797957,
-            "angularVelocity": 7.189752355462406e-8,
-            "velocityX": 3.766358774120127,
-            "velocityY": -0.3538089837727691,
-            "timestamp": 1.2074084402219507
-        },
-        {
-            "x": 6.463191352381069,
-            "y": 5.164218848902877,
-            "heading": 3.277521385657466,
-            "angularVelocity": 7.189755308023614e-8,
-            "velocityX": 3.766357883833297,
-            "velocityY": -0.35381846090640534,
-            "timestamp": 1.3002860125467162
-        },
-        {
-            "x": 6.802417095902647,
-            "y": 5.072715014503353,
-            "heading": 3.277521392775139,
-            "angularVelocity": 7.663500778773157e-8,
-            "velocityX": 3.6523967523118,
-            "velocityY": -0.9852091533956361,
-            "timestamp": 1.3931635848714816
-        },
-        {
-            "x": 7.115900861455289,
-            "y": 4.914048826005074,
-            "heading": 3.277521402122426,
-            "angularVelocity": 1.006409437934784e-7,
-            "velocityX": 3.375236429053956,
-            "velocityY": -1.7083369485958357,
-            "timestamp": 1.486041157196247
-        },
-        {
-            "x": 7.364244703869027,
-            "y": 4.725111728374696,
-            "heading": 3.264201738164946,
-            "angularVelocity": -0.1434109831263146,
-            "velocityX": 2.673883868813388,
-            "velocityY": -2.034259648494277,
-            "timestamp": 1.5789187295210125
-        },
-        {
-            "x": 7.541281052388914,
-            "y": 4.527619244510334,
-            "heading": 3.236812571923802,
-            "angularVelocity": -0.29489537199973426,
-            "velocityX": 1.9061259256523482,
-            "velocityY": -2.1263743110531466,
-            "timestamp": 1.671796301845778
-        },
-        {
-            "x": 7.646550613403749,
-            "y": 4.327374608371355,
-            "heading": 3.1953955958101785,
-            "angularVelocity": -0.44593086443733326,
-            "velocityX": 1.1334228315824013,
-            "velocityY": -2.156006354674992,
-            "timestamp": 1.7646738741705434
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": -0.5964367330411746,
-            "velocityX": 0.3601449279842051,
-            "velocityY": -2.1574057477590434,
-            "timestamp": 1.857551446495309
-        },
-        {
-            "x": 7.652622542952261,
-            "y": 3.949180878788953,
-            "heading": 3.079427144579805,
-            "angularVelocity": -0.7304577329243706,
-            "velocityX": -0.33014912487777537,
-            "velocityY": -2.1443491684415465,
-            "timestamp": 1.940475964492665
-        },
-        {
-            "x": 7.56799601367521,
-            "y": 3.773464984873132,
-            "heading": 3.0103677863230933,
-            "angularVelocity": -0.832797825353816,
-            "velocityX": -1.0205248257186026,
-            "velocityY": -2.1189860147444324,
-            "timestamp": 2.0234004824900214
-        },
-        {
-            "x": 7.426167818552031,
-            "y": 3.601500026216296,
-            "heading": 2.937043425915028,
-            "angularVelocity": -0.8842301671310652,
-            "velocityX": -1.7103288454169892,
-            "velocityY": -2.0737528876841793,
-            "timestamp": 2.1063250004873777
-        },
-        {
-            "x": 7.22742147527628,
-            "y": 3.436376202929032,
-            "heading": 2.8673776999748726,
-            "angularVelocity": -0.8401101100446183,
-            "velocityX": -2.3967138799900867,
-            "velocityY": -1.9912545441931753,
-            "timestamp": 2.189249518484734
-        },
-        {
-            "x": 6.9736090926507135,
-            "y": 3.285744578395889,
-            "heading": 2.8218659539774595,
-            "angularVelocity": -0.5488334101485464,
-            "velocityX": -3.0607640388534922,
-            "velocityY": -1.8164908059875007,
-            "timestamp": 2.2721740364820904
-        },
-        {
-            "x": 6.6802459764564,
-            "y": 3.1746372799457343,
-            "heading": 2.8218659065870306,
-            "angularVelocity": -5.714887481984483e-7,
-            "velocityX": -3.5377126485518553,
-            "velocityY": -1.3398606483753976,
-            "timestamp": 2.3550985544794467
-        },
-        {
-            "x": 6.371560257590401,
-            "y": 3.1187813398722177,
-            "heading": 2.821865892471116,
-            "angularVelocity": -1.702260676188045e-7,
-            "velocityX": -3.722490360159113,
-            "velocityY": -0.6735756977845532,
-            "timestamp": 2.438023072476803
-        },
-        {
-            "x": 6.0583631867424375,
-            "y": 3.1010512989609436,
-            "heading": 2.821865878845265,
-            "angularVelocity": -1.6431631700415249e-7,
-            "velocityX": -3.7768934738692015,
-            "velocityY": -0.21380939364446908,
-            "timestamp": 2.5209475904741594
-        },
-        {
-            "x": 5.745166091358163,
-            "y": 3.0833216914831887,
-            "heading": 2.821865865219415,
-            "angularVelocity": -1.6431629349932233e-7,
-            "velocityX": -3.7768937697564957,
-            "velocityY": -0.2138041668004641,
-            "timestamp": 2.6038721084715157
-        },
-        {
-            "x": 5.431968995973753,
-            "y": 3.065592084007832,
-            "heading": 2.8218658515935657,
-            "angularVelocity": -1.6431629385226094e-7,
-            "velocityX": -3.7768937697581326,
-            "velocityY": -0.213804166771547,
-            "timestamp": 2.686796626468872
-        },
-        {
-            "x": 5.118771900355572,
-            "y": 3.047862480662691,
-            "heading": 2.821865837967647,
-            "angularVelocity": -1.6431712462133498e-7,
-            "velocityX": -3.7768937725772025,
-            "velocityY": -0.21380411696461904,
-            "timestamp": 2.7697211444662284
-        },
-        {
-            "x": 4.833327806211659,
-            "y": 3.0319657734270002,
-            "heading": 2.7501020518876387,
-            "angularVelocity": -0.8654109521902384,
-            "velocityX": -3.4422158975107173,
-            "velocityY": -0.1917009301904812,
-            "timestamp": 2.8526456624635848
-        },
-        {
-            "x": 4.6051204329349895,
-            "y": 3.019195659985863,
-            "heading": 2.6830003866678815,
-            "angularVelocity": -0.809189692509236,
-            "velocityX": -2.7519891437167656,
-            "velocityY": -0.15399683651516197,
-            "timestamp": 2.935570180460941
-        },
-        {
-            "x": 4.434032594221504,
-            "y": 3.009603052392577,
-            "heading": 2.6296182647389936,
-            "angularVelocity": -0.6437435298760438,
-            "velocityX": -2.063175558270236,
-            "velocityY": -0.11567878626188524,
-            "timestamp": 3.0184946984582974
-        },
-        {
-            "x": 4.320003715449931,
-            "y": 3.0032022577019606,
-            "heading": 2.5928181043840297,
-            "angularVelocity": -0.44377900823176664,
-            "velocityX": -1.3750924518513103,
-            "velocityY": -0.07718820495068371,
-            "timestamp": 3.1014192164556538
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -0.22693052475300574,
-            "velocityX": -0.6874169042712922,
-            "velocityY": -0.03861653681318734,
-            "timestamp": 3.18434373445301
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 3.2672682524503665
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S3.traj
deleted file mode 100644
index f9f681f0..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S3.traj	
+++ /dev/null
@@ -1,346 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -2.1257809849356417e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 2.321508847678769,
-            "y": 5.553282811958262,
-            "heading": 3.1537553335344852,
-            "angularVelocity": 0.14810177732022164,
-            "velocityX": 0.7699258915674908,
-            "velocityY": -0.07232303637577411,
-            "timestamp": 0.09287757232476541
-        },
-        {
-            "x": 2.4645265668919087,
-            "y": 5.539848505148353,
-            "heading": 3.1812640229350246,
-            "angularVelocity": 0.2961822613574502,
-            "velocityX": 1.53985203998495,
-            "velocityY": -0.14464532689263399,
-            "timestamp": 0.18575514464953083
-        },
-        {
-            "x": 2.679053206199872,
-            "y": 5.519697173536265,
-            "heading": 3.2225218511092746,
-            "angularVelocity": 0.4442173405435694,
-            "velocityX": 2.309778711246101,
-            "velocityY": -0.21696660569059611,
-            "timestamp": 0.27863271697429626
-        },
-        {
-            "x": 2.965088840346323,
-            "y": 5.49282892094042,
-            "heading": 3.277521318880573,
-            "angularVelocity": 0.5921716771297765,
-            "velocityX": 3.079706187261968,
-            "velocityY": -0.2892867666899661,
-            "timestamp": 0.37151028929906166
-        },
-        {
-            "x": 3.3148991000673105,
-            "y": 5.459968004407915,
-            "heading": 3.277521325558456,
-            "angularVelocity": 7.189983956229139e-8,
-            "velocityX": 3.7663587770985667,
-            "velocityY": -0.35380895204280205,
-            "timestamp": 0.46438786162382706
-        },
-        {
-            "x": 3.664709359511986,
-            "y": 5.427107084931802,
-            "heading": 3.2775213322361236,
-            "angularVelocity": 7.18975223410498e-8,
-            "velocityX": 3.7663587741235602,
-            "velocityY": -0.3538089837362208,
-            "timestamp": 0.5572654339485925
-        },
-        {
-            "x": 4.014519618956661,
-            "y": 5.394246165455678,
-            "heading": 3.277521338913791,
-            "angularVelocity": 7.189752294324541e-8,
-            "velocityX": 3.7663587741235487,
-            "velocityY": -0.3538089837363427,
-            "timestamp": 0.650143006273358
-        },
-        {
-            "x": 4.364329878401335,
-            "y": 5.361385245979554,
-            "heading": 3.2775213455914582,
-            "angularVelocity": 7.189752233099723e-8,
-            "velocityX": 3.7663587741235482,
-            "velocityY": -0.3538089837363424,
-            "timestamp": 0.7430205785981234
-        },
-        {
-            "x": 4.714140137846011,
-            "y": 5.32852432650343,
-            "heading": 3.277521352269126,
-            "angularVelocity": 7.189752328963948e-8,
-            "velocityX": 3.7663587741235482,
-            "velocityY": -0.35380898373634245,
-            "timestamp": 0.8358981509228889
-        },
-        {
-            "x": 5.063950397290685,
-            "y": 5.295663407027306,
-            "heading": 3.2775213589467933,
-            "angularVelocity": 7.189752245277794e-8,
-            "velocityX": 3.7663587741235482,
-            "velocityY": -0.3538089837363426,
-            "timestamp": 0.9287757232476543
-        },
-        {
-            "x": 5.41376065673536,
-            "y": 5.262802487551181,
-            "heading": 3.2775213656244606,
-            "angularVelocity": 7.189752262506364e-8,
-            "velocityX": 3.7663587741235482,
-            "velocityY": -0.3538089837363426,
-            "timestamp": 1.0216532955724198
-        },
-        {
-            "x": 5.7635709161800355,
-            "y": 5.229941568075057,
-            "heading": 3.277521372302128,
-            "angularVelocity": 7.189752282304632e-8,
-            "velocityX": 3.7663587741235482,
-            "velocityY": -0.3538089837363426,
-            "timestamp": 1.1145308678971853
-        },
-        {
-            "x": 6.113381175624392,
-            "y": 5.19708064859555,
-            "heading": 3.2775213789797957,
-            "angularVelocity": 7.189752355462406e-8,
-            "velocityX": 3.766358774120127,
-            "velocityY": -0.3538089837727691,
-            "timestamp": 1.2074084402219507
-        },
-        {
-            "x": 6.463191352381069,
-            "y": 5.164218848902877,
-            "heading": 3.277521385657466,
-            "angularVelocity": 7.189755308023614e-8,
-            "velocityX": 3.766357883833297,
-            "velocityY": -0.35381846090640534,
-            "timestamp": 1.3002860125467162
-        },
-        {
-            "x": 6.802417095902647,
-            "y": 5.072715014503353,
-            "heading": 3.277521392775139,
-            "angularVelocity": 7.663500778773157e-8,
-            "velocityX": 3.6523967523118,
-            "velocityY": -0.9852091533956361,
-            "timestamp": 1.3931635848714816
-        },
-        {
-            "x": 7.115900861455289,
-            "y": 4.914048826005074,
-            "heading": 3.277521402122426,
-            "angularVelocity": 1.006409437934784e-7,
-            "velocityX": 3.375236429053956,
-            "velocityY": -1.7083369485958357,
-            "timestamp": 1.486041157196247
-        },
-        {
-            "x": 7.364244703869027,
-            "y": 4.725111728374696,
-            "heading": 3.264201738164946,
-            "angularVelocity": -0.1434109831263146,
-            "velocityX": 2.673883868813388,
-            "velocityY": -2.034259648494277,
-            "timestamp": 1.5789187295210125
-        },
-        {
-            "x": 7.541281052388914,
-            "y": 4.527619244510334,
-            "heading": 3.236812571923802,
-            "angularVelocity": -0.29489537199973426,
-            "velocityX": 1.9061259256523482,
-            "velocityY": -2.1263743110531466,
-            "timestamp": 1.671796301845778
-        },
-        {
-            "x": 7.646550613403749,
-            "y": 4.327374608371355,
-            "heading": 3.1953955958101785,
-            "angularVelocity": -0.44593086443733326,
-            "velocityX": 1.1334228315824013,
-            "velocityY": -2.156006354674992,
-            "timestamp": 1.7646738741705434
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": -0.5964367330411746,
-            "velocityX": 0.3601449279842051,
-            "velocityY": -2.1574057477590434,
-            "timestamp": 1.857551446495309
-        },
-        {
-            "x": 7.652622542952261,
-            "y": 3.949180878788953,
-            "heading": 3.079427144579805,
-            "angularVelocity": -0.7304577329243706,
-            "velocityX": -0.33014912487777537,
-            "velocityY": -2.1443491684415465,
-            "timestamp": 1.940475964492665
-        },
-        {
-            "x": 7.56799601367521,
-            "y": 3.773464984873132,
-            "heading": 3.0103677863230933,
-            "angularVelocity": -0.832797825353816,
-            "velocityX": -1.0205248257186026,
-            "velocityY": -2.1189860147444324,
-            "timestamp": 2.0234004824900214
-        },
-        {
-            "x": 7.426167818552031,
-            "y": 3.601500026216296,
-            "heading": 2.937043425915028,
-            "angularVelocity": -0.8842301671310652,
-            "velocityX": -1.7103288454169892,
-            "velocityY": -2.0737528876841793,
-            "timestamp": 2.1063250004873777
-        },
-        {
-            "x": 7.22742147527628,
-            "y": 3.436376202929032,
-            "heading": 2.8673776999748726,
-            "angularVelocity": -0.8401101100446183,
-            "velocityX": -2.3967138799900867,
-            "velocityY": -1.9912545441931753,
-            "timestamp": 2.189249518484734
-        },
-        {
-            "x": 6.9736090926507135,
-            "y": 3.285744578395889,
-            "heading": 2.8218659539774595,
-            "angularVelocity": -0.5488334101485464,
-            "velocityX": -3.0607640388534922,
-            "velocityY": -1.8164908059875007,
-            "timestamp": 2.2721740364820904
-        },
-        {
-            "x": 6.6802459764564,
-            "y": 3.1746372799457343,
-            "heading": 2.8218659065870306,
-            "angularVelocity": -5.714887481984483e-7,
-            "velocityX": -3.5377126485518553,
-            "velocityY": -1.3398606483753976,
-            "timestamp": 2.3550985544794467
-        },
-        {
-            "x": 6.371560257590401,
-            "y": 3.1187813398722177,
-            "heading": 2.821865892471116,
-            "angularVelocity": -1.702260676188045e-7,
-            "velocityX": -3.722490360159113,
-            "velocityY": -0.6735756977845532,
-            "timestamp": 2.438023072476803
-        },
-        {
-            "x": 6.0583631867424375,
-            "y": 3.1010512989609436,
-            "heading": 2.821865878845265,
-            "angularVelocity": -1.6431631700415249e-7,
-            "velocityX": -3.7768934738692015,
-            "velocityY": -0.21380939364446908,
-            "timestamp": 2.5209475904741594
-        },
-        {
-            "x": 5.745166091358163,
-            "y": 3.0833216914831887,
-            "heading": 2.821865865219415,
-            "angularVelocity": -1.6431629349932233e-7,
-            "velocityX": -3.7768937697564957,
-            "velocityY": -0.2138041668004641,
-            "timestamp": 2.6038721084715157
-        },
-        {
-            "x": 5.431968995973753,
-            "y": 3.065592084007832,
-            "heading": 2.8218658515935657,
-            "angularVelocity": -1.6431629385226094e-7,
-            "velocityX": -3.7768937697581326,
-            "velocityY": -0.213804166771547,
-            "timestamp": 2.686796626468872
-        },
-        {
-            "x": 5.118771900355572,
-            "y": 3.047862480662691,
-            "heading": 2.821865837967647,
-            "angularVelocity": -1.6431712462133498e-7,
-            "velocityX": -3.7768937725772025,
-            "velocityY": -0.21380411696461904,
-            "timestamp": 2.7697211444662284
-        },
-        {
-            "x": 4.833327806211659,
-            "y": 3.0319657734270002,
-            "heading": 2.7501020518876387,
-            "angularVelocity": -0.8654109521902384,
-            "velocityX": -3.4422158975107173,
-            "velocityY": -0.1917009301904812,
-            "timestamp": 2.8526456624635848
-        },
-        {
-            "x": 4.6051204329349895,
-            "y": 3.019195659985863,
-            "heading": 2.6830003866678815,
-            "angularVelocity": -0.809189692509236,
-            "velocityX": -2.7519891437167656,
-            "velocityY": -0.15399683651516197,
-            "timestamp": 2.935570180460941
-        },
-        {
-            "x": 4.434032594221504,
-            "y": 3.009603052392577,
-            "heading": 2.6296182647389936,
-            "angularVelocity": -0.6437435298760438,
-            "velocityX": -2.063175558270236,
-            "velocityY": -0.11567878626188524,
-            "timestamp": 3.0184946984582974
-        },
-        {
-            "x": 4.320003715449931,
-            "y": 3.0032022577019606,
-            "heading": 2.5928181043840297,
-            "angularVelocity": -0.44377900823176664,
-            "velocityX": -1.3750924518513103,
-            "velocityY": -0.07718820495068371,
-            "timestamp": 3.1014192164556538
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -0.22693052475300574,
-            "velocityX": -0.6874169042712922,
-            "velocityY": -0.03861653681318734,
-            "timestamp": 3.18434373445301
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 3.2672682524503665
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S4.1.traj
deleted file mode 100644
index f6bf2dd2..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S4.1.traj	
+++ /dev/null
@@ -1,355 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -4.116881309267156e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3172206852139876,
-            "y": 5.557554237561194,
-            "heading": 3.1526693404609274,
-            "angularVelocity": 0.14095873188728006,
-            "velocityX": 0.747895486240753,
-            "velocityY": -0.02721148531256728,
-            "timestamp": 0.08987978460983724
-        },
-        {
-            "x": 2.4516878784162954,
-            "y": 5.552665336111155,
-            "heading": 3.1761213228598115,
-            "angularVelocity": 0.2609261081415128,
-            "velocityX": 1.4960782759551832,
-            "velocityY": -0.054393782442455275,
-            "timestamp": 0.17975956921967448
-        },
-        {
-            "x": 2.6534358807362874,
-            "y": 5.545338085116866,
-            "heading": 3.2071661677578067,
-            "angularVelocity": 0.34540408649958604,
-            "velocityX": 2.2446426990871093,
-            "velocityY": -0.08152279209504047,
-            "timestamp": 0.2696393538295117
-        },
-        {
-            "x": 2.9224981140685604,
-            "y": 5.535583877021325,
-            "heading": 3.239227837145004,
-            "angularVelocity": 0.3567172476694272,
-            "velocityX": 2.9935789732947984,
-            "velocityY": -0.10852504974159909,
-            "timestamp": 0.35951913843934896
-        },
-        {
-            "x": 3.258623258854089,
-            "y": 5.523455945202495,
-            "heading": 3.2503847245896496,
-            "angularVelocity": 0.1241312214205911,
-            "velocityX": 3.739719073033272,
-            "velocityY": -0.13493503429591652,
-            "timestamp": 0.4493989230491862
-        },
-        {
-            "x": 3.598405730984745,
-            "y": 5.5110225450854795,
-            "heading": 3.2503847355595754,
-            "angularVelocity": 1.2205109450182653e-7,
-            "velocityX": 3.7804103960154256,
-            "velocityY": -0.13833366614078543,
-            "timestamp": 0.5392787076590234
-        },
-        {
-            "x": 3.938188203087489,
-            "y": 5.498589144205611,
-            "heading": 3.2503847465294977,
-            "angularVelocity": 1.22051053234482e-7,
-            "velocityX": 3.7804103957048714,
-            "velocityY": -0.13833367462827542,
-            "timestamp": 0.6291584922688607
-        },
-        {
-            "x": 4.277970675190232,
-            "y": 5.486155743325738,
-            "heading": 3.25038475749942,
-            "angularVelocity": 1.22051052798414e-7,
-            "velocityX": 3.7804103957048705,
-            "velocityY": -0.13833367462831278,
-            "timestamp": 0.7190382768786979
-        },
-        {
-            "x": 4.617753147292976,
-            "y": 5.473722342445865,
-            "heading": 3.2503847684693423,
-            "angularVelocity": 1.2205105381924992e-7,
-            "velocityX": 3.7804103957048705,
-            "velocityY": -0.1383336746283128,
-            "timestamp": 0.8089180614885352
-        },
-        {
-            "x": 4.957535619395719,
-            "y": 5.461288941565993,
-            "heading": 3.2503847794392646,
-            "angularVelocity": 1.2205105317325576e-7,
-            "velocityX": 3.78041039570487,
-            "velocityY": -0.13833367462831272,
-            "timestamp": 0.8987978460983724
-        },
-        {
-            "x": 5.297318091498462,
-            "y": 5.44885554068612,
-            "heading": 3.2503847904091874,
-            "angularVelocity": 1.2205105386911006e-7,
-            "velocityX": 3.78041039570487,
-            "velocityY": -0.13833367462831295,
-            "timestamp": 0.9886776307082097
-        },
-        {
-            "x": 5.637100563601198,
-            "y": 5.436422139806048,
-            "heading": 3.2503848013791092,
-            "angularVelocity": 1.2205105333670814e-7,
-            "velocityX": 3.7804103957047883,
-            "velocityY": -0.13833367463053942,
-            "timestamp": 1.0785574153180468
-        },
-        {
-            "x": 5.976883034047419,
-            "y": 5.423988693656475,
-            "heading": 3.2503848123490315,
-            "angularVelocity": 1.2205105297878886e-7,
-            "velocityX": 3.7804103772744444,
-            "velocityY": -0.1383341782977477,
-            "timestamp": 1.168437199927884
-        },
-        {
-            "x": 6.3161557778942825,
-            "y": 5.401611852989575,
-            "heading": 3.2503848233905317,
-            "angularVelocity": 1.2284742555590528e-7,
-            "velocityX": 3.774739173214837,
-            "velocityY": -0.24896411094026674,
-            "timestamp": 1.2583169845377211
-        },
-        {
-            "x": 6.6443599765864345,
-            "y": 5.312793385156624,
-            "heading": 3.2503848356259346,
-            "angularVelocity": 1.3613075412636898e-7,
-            "velocityX": 3.6515908456709,
-            "velocityY": -0.9881918188666926,
-            "timestamp": 1.3481967691475583
-        },
-        {
-            "x": 6.948614681552646,
-            "y": 5.161017938085189,
-            "heading": 3.2503848519237777,
-            "angularVelocity": 1.8132935219655535e-7,
-            "velocityX": 3.3851294402514114,
-            "velocityY": -1.6886494302392412,
-            "timestamp": 1.4380765537573954
-        },
-        {
-            "x": 7.216978880977283,
-            "y": 4.952242107786858,
-            "heading": 3.2503848494896554,
-            "angularVelocity": -2.7081974161956603e-8,
-            "velocityX": 2.985812667326646,
-            "velocityY": -2.322834118980229,
-            "timestamp": 1.5279563383672325
-        },
-        {
-            "x": 7.434432646263598,
-            "y": 4.699605663740004,
-            "heading": 3.233827697906493,
-            "angularVelocity": -0.18421441100386476,
-            "velocityX": 2.4193845838667345,
-            "velocityY": -2.8108260955843263,
-            "timestamp": 1.6178361229770697
-        },
-        {
-            "x": 7.589713042543029,
-            "y": 4.421668707909288,
-            "heading": 3.1971997410199715,
-            "angularVelocity": -0.40752163621075216,
-            "velocityX": 1.7276453982782098,
-            "velocityY": -3.0923188905848704,
-            "timestamp": 1.7077159075869068
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": -0.6364027380377996,
-            "velocityX": 1.0045301938460758,
-            "velocityY": -3.2784759018773304,
-            "timestamp": 1.797595692196744
-        },
-        {
-            "x": 7.709767191117991,
-            "y": 3.879341169574254,
-            "heading": 3.0798110703023642,
-            "angularVelocity": -0.8233834909878134,
-            "velocityX": 0.40721464665943,
-            "velocityY": -3.3879684087134527,
-            "timestamp": 1.8706952021534504
-        },
-        {
-            "x": 7.695293406413974,
-            "y": 3.6265437637575832,
-            "heading": 3.0152640841936,
-            "angularVelocity": -0.8830016254143865,
-            "velocityX": -0.19800111810029022,
-            "velocityY": -3.4582640289435425,
-            "timestamp": 1.9437947121101569
-        },
-        {
-            "x": 7.636370047387183,
-            "y": 3.3734118596165046,
-            "heading": 2.9616083332383942,
-            "angularVelocity": -0.7340097216379668,
-            "velocityX": -0.8060705066515889,
-            "velocityY": -3.46283996009006,
-            "timestamp": 2.0168942220668633
-        },
-        {
-            "x": 7.533938934527094,
-            "y": 3.12599493210629,
-            "heading": 2.935877856016127,
-            "angularVelocity": -0.3519924721450196,
-            "velocityX": -1.4012558076073192,
-            "velocityY": -3.3846591811184132,
-            "timestamp": 2.08999373202357
-        },
-        {
-            "x": 7.389145536880011,
-            "y": 2.8904033965878195,
-            "heading": 2.9358731972146472,
-            "angularVelocity": -0.00006373232119512069,
-            "velocityX": -1.9807711123211746,
-            "velocityY": -3.2228880283604715,
-            "timestamp": 2.1630932419802766
-        },
-        {
-            "x": 7.2083948751557925,
-            "y": 2.6811221610888363,
-            "heading": 2.935873140353821,
-            "angularVelocity": -7.778550956603191e-7,
-            "velocityX": -2.4726658472985523,
-            "velocityY": -2.8629635906306268,
-            "timestamp": 2.2361927519369833
-        },
-        {
-            "x": 6.996380296591032,
-            "y": 2.5035855179171795,
-            "heading": 2.9358730970284697,
-            "angularVelocity": -5.92690040120521e-7,
-            "velocityX": -2.9003556753023303,
-            "velocityY": -2.428698130491067,
-            "timestamp": 2.30929226189369
-        },
-        {
-            "x": 6.758605859634312,
-            "y": 2.362402091220058,
-            "heading": 2.935873058757806,
-            "angularVelocity": -5.235419952154551e-7,
-            "velocityX": -3.252750081327982,
-            "velocityY": -1.931386773738272,
-            "timestamp": 2.3823917718503966
-        },
-        {
-            "x": 6.50822939770154,
-            "y": 2.2450092870168694,
-            "heading": 2.935873021282274,
-            "angularVelocity": -5.126646128379785e-7,
-            "velocityX": -3.4251455595402076,
-            "velocityY": -1.6059314798786082,
-            "timestamp": 2.4554912818071033
-        },
-        {
-            "x": 6.2578527752872475,
-            "y": 2.12761682509076,
-            "heading": 2.935872983806744,
-            "angularVelocity": -5.126645909112232e-7,
-            "velocityX": -3.4251477549245686,
-            "velocityY": -1.6059267975342413,
-            "timestamp": 2.52859079176381
-        },
-        {
-            "x": 6.007476152294899,
-            "y": 2.010224364397617,
-            "heading": 2.9358729463311364,
-            "angularVelocity": -5.126656467056276e-7,
-            "velocityX": -3.425147762832367,
-            "velocityY": -1.6059267806672737,
-            "timestamp": 2.6016903017205166
-        },
-        {
-            "x": 5.771581826202875,
-            "y": 1.8997576718980689,
-            "heading": 2.8963919451713847,
-            "angularVelocity": -0.540099395784409,
-            "velocityX": -3.227030198037426,
-            "velocityY": -1.5111823945872924,
-            "timestamp": 2.6747898116772233
-        },
-        {
-            "x": 5.572260449403434,
-            "y": 1.8062657885588822,
-            "heading": 2.7909821338342944,
-            "angularVelocity": -1.4420043499543171,
-            "velocityX": -2.7267129002300594,
-            "velocityY": -1.2789673062725992,
-            "timestamp": 2.74788932163393
-        },
-        {
-            "x": 5.413306384281217,
-            "y": 1.7316311280227,
-            "heading": 2.684821280424371,
-            "angularVelocity": -1.4522785921931365,
-            "velocityX": -2.1744887922827503,
-            "velocityY": -1.0210008327056677,
-            "timestamp": 2.8209888315906366
-        },
-        {
-            "x": 5.294461986790537,
-            "y": 1.6757665506891541,
-            "heading": 2.596152824094934,
-            "angularVelocity": -1.2129829103089709,
-            "velocityX": -1.6257892503119276,
-            "velocityY": -0.7642264273266058,
-            "timestamp": 2.8940883415473433
-        },
-        {
-            "x": 5.2154339068234155,
-            "y": 1.6385760788874908,
-            "heading": 2.5330095835299122,
-            "angularVelocity": -0.8637984112673935,
-            "velocityX": -1.0811027326165235,
-            "velocityY": -0.5087649947815693,
-            "timestamp": 2.96718785150405
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.45157051736003884,
-            "velocityX": -0.5394551460983961,
-            "velocityY": -0.2541204297879725,
-            "timestamp": 3.0402873614607566
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": 1.8516151332127108e-31,
-            "velocityY": 3.1624508891409975e-32,
-            "timestamp": 3.1133868714174633
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S4.traj
deleted file mode 100644
index f6bf2dd2..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3 To S4.traj	
+++ /dev/null
@@ -1,355 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -4.116881309267156e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3172206852139876,
-            "y": 5.557554237561194,
-            "heading": 3.1526693404609274,
-            "angularVelocity": 0.14095873188728006,
-            "velocityX": 0.747895486240753,
-            "velocityY": -0.02721148531256728,
-            "timestamp": 0.08987978460983724
-        },
-        {
-            "x": 2.4516878784162954,
-            "y": 5.552665336111155,
-            "heading": 3.1761213228598115,
-            "angularVelocity": 0.2609261081415128,
-            "velocityX": 1.4960782759551832,
-            "velocityY": -0.054393782442455275,
-            "timestamp": 0.17975956921967448
-        },
-        {
-            "x": 2.6534358807362874,
-            "y": 5.545338085116866,
-            "heading": 3.2071661677578067,
-            "angularVelocity": 0.34540408649958604,
-            "velocityX": 2.2446426990871093,
-            "velocityY": -0.08152279209504047,
-            "timestamp": 0.2696393538295117
-        },
-        {
-            "x": 2.9224981140685604,
-            "y": 5.535583877021325,
-            "heading": 3.239227837145004,
-            "angularVelocity": 0.3567172476694272,
-            "velocityX": 2.9935789732947984,
-            "velocityY": -0.10852504974159909,
-            "timestamp": 0.35951913843934896
-        },
-        {
-            "x": 3.258623258854089,
-            "y": 5.523455945202495,
-            "heading": 3.2503847245896496,
-            "angularVelocity": 0.1241312214205911,
-            "velocityX": 3.739719073033272,
-            "velocityY": -0.13493503429591652,
-            "timestamp": 0.4493989230491862
-        },
-        {
-            "x": 3.598405730984745,
-            "y": 5.5110225450854795,
-            "heading": 3.2503847355595754,
-            "angularVelocity": 1.2205109450182653e-7,
-            "velocityX": 3.7804103960154256,
-            "velocityY": -0.13833366614078543,
-            "timestamp": 0.5392787076590234
-        },
-        {
-            "x": 3.938188203087489,
-            "y": 5.498589144205611,
-            "heading": 3.2503847465294977,
-            "angularVelocity": 1.22051053234482e-7,
-            "velocityX": 3.7804103957048714,
-            "velocityY": -0.13833367462827542,
-            "timestamp": 0.6291584922688607
-        },
-        {
-            "x": 4.277970675190232,
-            "y": 5.486155743325738,
-            "heading": 3.25038475749942,
-            "angularVelocity": 1.22051052798414e-7,
-            "velocityX": 3.7804103957048705,
-            "velocityY": -0.13833367462831278,
-            "timestamp": 0.7190382768786979
-        },
-        {
-            "x": 4.617753147292976,
-            "y": 5.473722342445865,
-            "heading": 3.2503847684693423,
-            "angularVelocity": 1.2205105381924992e-7,
-            "velocityX": 3.7804103957048705,
-            "velocityY": -0.1383336746283128,
-            "timestamp": 0.8089180614885352
-        },
-        {
-            "x": 4.957535619395719,
-            "y": 5.461288941565993,
-            "heading": 3.2503847794392646,
-            "angularVelocity": 1.2205105317325576e-7,
-            "velocityX": 3.78041039570487,
-            "velocityY": -0.13833367462831272,
-            "timestamp": 0.8987978460983724
-        },
-        {
-            "x": 5.297318091498462,
-            "y": 5.44885554068612,
-            "heading": 3.2503847904091874,
-            "angularVelocity": 1.2205105386911006e-7,
-            "velocityX": 3.78041039570487,
-            "velocityY": -0.13833367462831295,
-            "timestamp": 0.9886776307082097
-        },
-        {
-            "x": 5.637100563601198,
-            "y": 5.436422139806048,
-            "heading": 3.2503848013791092,
-            "angularVelocity": 1.2205105333670814e-7,
-            "velocityX": 3.7804103957047883,
-            "velocityY": -0.13833367463053942,
-            "timestamp": 1.0785574153180468
-        },
-        {
-            "x": 5.976883034047419,
-            "y": 5.423988693656475,
-            "heading": 3.2503848123490315,
-            "angularVelocity": 1.2205105297878886e-7,
-            "velocityX": 3.7804103772744444,
-            "velocityY": -0.1383341782977477,
-            "timestamp": 1.168437199927884
-        },
-        {
-            "x": 6.3161557778942825,
-            "y": 5.401611852989575,
-            "heading": 3.2503848233905317,
-            "angularVelocity": 1.2284742555590528e-7,
-            "velocityX": 3.774739173214837,
-            "velocityY": -0.24896411094026674,
-            "timestamp": 1.2583169845377211
-        },
-        {
-            "x": 6.6443599765864345,
-            "y": 5.312793385156624,
-            "heading": 3.2503848356259346,
-            "angularVelocity": 1.3613075412636898e-7,
-            "velocityX": 3.6515908456709,
-            "velocityY": -0.9881918188666926,
-            "timestamp": 1.3481967691475583
-        },
-        {
-            "x": 6.948614681552646,
-            "y": 5.161017938085189,
-            "heading": 3.2503848519237777,
-            "angularVelocity": 1.8132935219655535e-7,
-            "velocityX": 3.3851294402514114,
-            "velocityY": -1.6886494302392412,
-            "timestamp": 1.4380765537573954
-        },
-        {
-            "x": 7.216978880977283,
-            "y": 4.952242107786858,
-            "heading": 3.2503848494896554,
-            "angularVelocity": -2.7081974161956603e-8,
-            "velocityX": 2.985812667326646,
-            "velocityY": -2.322834118980229,
-            "timestamp": 1.5279563383672325
-        },
-        {
-            "x": 7.434432646263598,
-            "y": 4.699605663740004,
-            "heading": 3.233827697906493,
-            "angularVelocity": -0.18421441100386476,
-            "velocityX": 2.4193845838667345,
-            "velocityY": -2.8108260955843263,
-            "timestamp": 1.6178361229770697
-        },
-        {
-            "x": 7.589713042543029,
-            "y": 4.421668707909288,
-            "heading": 3.1971997410199715,
-            "angularVelocity": -0.40752163621075216,
-            "velocityX": 1.7276453982782098,
-            "velocityY": -3.0923188905848704,
-            "timestamp": 1.7077159075869068
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": -0.6364027380377996,
-            "velocityX": 1.0045301938460758,
-            "velocityY": -3.2784759018773304,
-            "timestamp": 1.797595692196744
-        },
-        {
-            "x": 7.709767191117991,
-            "y": 3.879341169574254,
-            "heading": 3.0798110703023642,
-            "angularVelocity": -0.8233834909878134,
-            "velocityX": 0.40721464665943,
-            "velocityY": -3.3879684087134527,
-            "timestamp": 1.8706952021534504
-        },
-        {
-            "x": 7.695293406413974,
-            "y": 3.6265437637575832,
-            "heading": 3.0152640841936,
-            "angularVelocity": -0.8830016254143865,
-            "velocityX": -0.19800111810029022,
-            "velocityY": -3.4582640289435425,
-            "timestamp": 1.9437947121101569
-        },
-        {
-            "x": 7.636370047387183,
-            "y": 3.3734118596165046,
-            "heading": 2.9616083332383942,
-            "angularVelocity": -0.7340097216379668,
-            "velocityX": -0.8060705066515889,
-            "velocityY": -3.46283996009006,
-            "timestamp": 2.0168942220668633
-        },
-        {
-            "x": 7.533938934527094,
-            "y": 3.12599493210629,
-            "heading": 2.935877856016127,
-            "angularVelocity": -0.3519924721450196,
-            "velocityX": -1.4012558076073192,
-            "velocityY": -3.3846591811184132,
-            "timestamp": 2.08999373202357
-        },
-        {
-            "x": 7.389145536880011,
-            "y": 2.8904033965878195,
-            "heading": 2.9358731972146472,
-            "angularVelocity": -0.00006373232119512069,
-            "velocityX": -1.9807711123211746,
-            "velocityY": -3.2228880283604715,
-            "timestamp": 2.1630932419802766
-        },
-        {
-            "x": 7.2083948751557925,
-            "y": 2.6811221610888363,
-            "heading": 2.935873140353821,
-            "angularVelocity": -7.778550956603191e-7,
-            "velocityX": -2.4726658472985523,
-            "velocityY": -2.8629635906306268,
-            "timestamp": 2.2361927519369833
-        },
-        {
-            "x": 6.996380296591032,
-            "y": 2.5035855179171795,
-            "heading": 2.9358730970284697,
-            "angularVelocity": -5.92690040120521e-7,
-            "velocityX": -2.9003556753023303,
-            "velocityY": -2.428698130491067,
-            "timestamp": 2.30929226189369
-        },
-        {
-            "x": 6.758605859634312,
-            "y": 2.362402091220058,
-            "heading": 2.935873058757806,
-            "angularVelocity": -5.235419952154551e-7,
-            "velocityX": -3.252750081327982,
-            "velocityY": -1.931386773738272,
-            "timestamp": 2.3823917718503966
-        },
-        {
-            "x": 6.50822939770154,
-            "y": 2.2450092870168694,
-            "heading": 2.935873021282274,
-            "angularVelocity": -5.126646128379785e-7,
-            "velocityX": -3.4251455595402076,
-            "velocityY": -1.6059314798786082,
-            "timestamp": 2.4554912818071033
-        },
-        {
-            "x": 6.2578527752872475,
-            "y": 2.12761682509076,
-            "heading": 2.935872983806744,
-            "angularVelocity": -5.126645909112232e-7,
-            "velocityX": -3.4251477549245686,
-            "velocityY": -1.6059267975342413,
-            "timestamp": 2.52859079176381
-        },
-        {
-            "x": 6.007476152294899,
-            "y": 2.010224364397617,
-            "heading": 2.9358729463311364,
-            "angularVelocity": -5.126656467056276e-7,
-            "velocityX": -3.425147762832367,
-            "velocityY": -1.6059267806672737,
-            "timestamp": 2.6016903017205166
-        },
-        {
-            "x": 5.771581826202875,
-            "y": 1.8997576718980689,
-            "heading": 2.8963919451713847,
-            "angularVelocity": -0.540099395784409,
-            "velocityX": -3.227030198037426,
-            "velocityY": -1.5111823945872924,
-            "timestamp": 2.6747898116772233
-        },
-        {
-            "x": 5.572260449403434,
-            "y": 1.8062657885588822,
-            "heading": 2.7909821338342944,
-            "angularVelocity": -1.4420043499543171,
-            "velocityX": -2.7267129002300594,
-            "velocityY": -1.2789673062725992,
-            "timestamp": 2.74788932163393
-        },
-        {
-            "x": 5.413306384281217,
-            "y": 1.7316311280227,
-            "heading": 2.684821280424371,
-            "angularVelocity": -1.4522785921931365,
-            "velocityX": -2.1744887922827503,
-            "velocityY": -1.0210008327056677,
-            "timestamp": 2.8209888315906366
-        },
-        {
-            "x": 5.294461986790537,
-            "y": 1.6757665506891541,
-            "heading": 2.596152824094934,
-            "angularVelocity": -1.2129829103089709,
-            "velocityX": -1.6257892503119276,
-            "velocityY": -0.7642264273266058,
-            "timestamp": 2.8940883415473433
-        },
-        {
-            "x": 5.2154339068234155,
-            "y": 1.6385760788874908,
-            "heading": 2.5330095835299122,
-            "angularVelocity": -0.8637984112673935,
-            "velocityX": -1.0811027326165235,
-            "velocityY": -0.5087649947815693,
-            "timestamp": 2.96718785150405
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.45157051736003884,
-            "velocityX": -0.5394551460983961,
-            "velocityY": -0.2541204297879725,
-            "timestamp": 3.0402873614607566
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": 1.8516151332127108e-31,
-            "velocityY": 3.1624508891409975e-32,
-            "timestamp": 3.1133868714174633
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S3.1.traj
deleted file mode 100644
index 55f7626f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S3.1.traj	
+++ /dev/null
@@ -1,364 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.1305523573234936e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3154171108625845,
-            "y": 5.531249621704393,
-            "heading": 3.152034306361113,
-            "angularVelocity": 0.12992542777148963,
-            "velocityX": 0.7062597425523925,
-            "velocityY": -0.3103963856794681,
-            "timestamp": 0.09262471994534183
-        },
-        {
-            "x": 2.4462513478934307,
-            "y": 5.473748850345723,
-            "heading": 3.1761003812465023,
-            "angularVelocity": 0.2598234564119746,
-            "velocityX": 1.4125196503487585,
-            "velocityY": -0.6207929307921308,
-            "timestamp": 0.18524943989068365
-        },
-        {
-            "x": 2.642502734215126,
-            "y": 5.387497655476605,
-            "heading": 3.2121940698357436,
-            "angularVelocity": 0.38967662855597096,
-            "velocityX": 2.1187798077824542,
-            "velocityY": -0.931189804622509,
-            "timestamp": 0.2778741598360255
-        },
-        {
-            "x": 2.9041712871426104,
-            "y": 5.2724959877699655,
-            "heading": 3.2603098134687913,
-            "angularVelocity": 0.5194697879944143,
-            "velocityX": 2.8250401521526483,
-            "velocityY": -1.2415872110005046,
-            "timestamp": 0.3704988797813673
-        },
-        {
-            "x": 3.22495104061126,
-            "y": 5.131513113466323,
-            "heading": 3.260309819428016,
-            "angularVelocity": 6.433730075600531e-8,
-            "velocityX": 3.4632196853922204,
-            "velocityY": -1.5220869157481582,
-            "timestamp": 0.46312359972670913
-        },
-        {
-            "x": 3.545730793052991,
-            "y": 4.99053023682554,
-            "heading": 3.2603098253870204,
-            "angularVelocity": 6.433492130788198e-8,
-            "velocityX": 3.4632196743053516,
-            "velocityY": -1.5220869409805253,
-            "timestamp": 0.555748319672051
-        },
-        {
-            "x": 3.866510545494718,
-            "y": 4.849547360184748,
-            "heading": 3.2603098313460244,
-            "angularVelocity": 6.433492218112887e-8,
-            "velocityX": 3.463219674305308,
-            "velocityY": -1.5220869409806246,
-            "timestamp": 0.6483730396173928
-        },
-        {
-            "x": 4.187290297936445,
-            "y": 4.708564483543956,
-            "heading": 3.2603098373050288,
-            "angularVelocity": 6.433492230011589e-8,
-            "velocityX": 3.463219674305308,
-            "velocityY": -1.5220869409806246,
-            "timestamp": 0.7409977595627346
-        },
-        {
-            "x": 4.508070050378173,
-            "y": 4.567581606903164,
-            "heading": 3.260309843264033,
-            "angularVelocity": 6.433492243162183e-8,
-            "velocityX": 3.463219674305308,
-            "velocityY": -1.5220869409806246,
-            "timestamp": 0.8336224795080764
-        },
-        {
-            "x": 4.828849802819899,
-            "y": 4.426598730262372,
-            "heading": 3.260309849223037,
-            "angularVelocity": 6.433492147964013e-8,
-            "velocityX": 3.463219674305308,
-            "velocityY": -1.5220869409806246,
-            "timestamp": 0.9262471994534183
-        },
-        {
-            "x": 5.149629555261627,
-            "y": 4.28561585362158,
-            "heading": 3.2603098551820415,
-            "angularVelocity": 6.433492220525221e-8,
-            "velocityX": 3.4632196743053076,
-            "velocityY": -1.5220869409806248,
-            "timestamp": 1.01887191939876
-        },
-        {
-            "x": 5.470409307703354,
-            "y": 4.1446329769807875,
-            "heading": 3.260309861141046,
-            "angularVelocity": 6.433492177225282e-8,
-            "velocityX": 3.463219674305308,
-            "velocityY": -1.5220869409806244,
-            "timestamp": 1.111496639344102
-        },
-        {
-            "x": 5.791189060145081,
-            "y": 4.003650100339995,
-            "heading": 3.2603098671000503,
-            "angularVelocity": 6.433492264273842e-8,
-            "velocityX": 3.463219674305308,
-            "velocityY": -1.5220869409806244,
-            "timestamp": 1.2041213592894437
-        },
-        {
-            "x": 6.111968812586809,
-            "y": 3.8626672236992023,
-            "heading": 3.2603098730590543,
-            "angularVelocity": 6.433492200729343e-8,
-            "velocityX": 3.463219674305308,
-            "velocityY": -1.5220869409806248,
-            "timestamp": 1.2967460792347856
-        },
-        {
-            "x": 6.432748565028133,
-            "y": 3.7216843470574936,
-            "heading": 3.2603098790180582,
-            "angularVelocity": 6.43349214222261e-8,
-            "velocityX": 3.4632196743009604,
-            "velocityY": -1.5220869409905164,
-            "timestamp": 1.3893707991801274
-        },
-        {
-            "x": 6.753528214821051,
-            "y": 3.5807012368590723,
-            "heading": 3.260309884977063,
-            "angularVelocity": 6.433492684215086e-8,
-            "velocityX": 3.4632185660826984,
-            "velocityY": -1.5220894625281032,
-            "timestamp": 1.4819955191254692
-        },
-        {
-            "x": 7.055963078223925,
-            "y": 3.403757816572982,
-            "heading": 3.260309891160044,
-            "angularVelocity": 6.675302824117912e-8,
-            "velocityX": 3.2651635932755534,
-            "velocityY": -1.9103261029075773,
-            "timestamp": 1.574620239070811
-        },
-        {
-            "x": 7.316159274039154,
-            "y": 3.1690804957678615,
-            "heading": 3.2603099328635268,
-            "angularVelocity": 4.502413954005259e-7,
-            "velocityX": 2.809144210841027,
-            "velocityY": -2.533635955321695,
-            "timestamp": 1.6672449590161529
-        },
-        {
-            "x": 7.507506683295118,
-            "y": 2.9535313290497767,
-            "heading": 3.2483076831663804,
-            "angularVelocity": -0.12957933588602658,
-            "velocityX": 2.0658352259405257,
-            "velocityY": -2.3271235459095725,
-            "timestamp": 1.7598696789614947
-        },
-        {
-            "x": 7.631583047935508,
-            "y": 2.7620770453961936,
-            "heading": 3.2242582782979583,
-            "angularVelocity": -0.25964348267518605,
-            "velocityX": 1.3395599437559367,
-            "velocityY": -2.066989069079623,
-            "timestamp": 1.8524943989068365
-        },
-        {
-            "x": 7.68897880573514,
-            "y": 2.5963063443763943,
-            "heading": 3.1881570664078476,
-            "angularVelocity": -0.3897578520228113,
-            "velocityX": 0.6196591777389554,
-            "velocityY": -1.7897025882250568,
-            "timestamp": 1.9451191188521784
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -0.5199159191656997,
-            "velocityX": -0.09693746702217505,
-            "velocityY": -1.5039866728730655,
-            "timestamp": 2.03774383879752
-        },
-        {
-            "x": 7.6191836827180115,
-            "y": 2.3540612532810705,
-            "heading": 3.087368523551757,
-            "angularVelocity": -0.6362125445554702,
-            "velocityX": -0.73515140709603,
-            "velocityY": -1.244329940996524,
-            "timestamp": 2.1204700863364088
-        },
-        {
-            "x": 7.505717742673261,
-            "y": 2.2730171165833535,
-            "heading": 3.0268817588529657,
-            "angularVelocity": -0.7311677550750295,
-            "velocityX": -1.3715833054244395,
-            "velocityY": -0.979666540049693,
-            "timestamp": 2.2031963338752973
-        },
-        {
-            "x": 7.339845145495756,
-            "y": 2.2145049491373583,
-            "heading": 2.9612826526647904,
-            "angularVelocity": -0.7929660553905423,
-            "velocityX": -2.005078220180729,
-            "velocityY": -0.7072987012796481,
-            "timestamp": 2.285922581414186
-        },
-        {
-            "x": 7.122035574070675,
-            "y": 2.1796305402319347,
-            "heading": 2.895398132602207,
-            "angularVelocity": -0.7964161559680392,
-            "velocityX": -2.6328955791532933,
-            "velocityY": -0.4215640131510867,
-            "timestamp": 2.3686488289530745
-        },
-        {
-            "x": 6.853520994844272,
-            "y": 2.1707672940791287,
-            "heading": 2.839999953502189,
-            "angularVelocity": -0.6696566174354214,
-            "velocityX": -3.245820851479772,
-            "velocityY": -0.1071394680223975,
-            "timestamp": 2.451375076491963
-        },
-        {
-            "x": 6.54155924782903,
-            "y": 2.195597775343439,
-            "heading": 2.83999969260268,
-            "angularVelocity": -0.0000031537694148635814,
-            "velocityX": -3.7710129045632463,
-            "velocityY": 0.30015239422817563,
-            "timestamp": 2.5341013240308516
-        },
-        {
-            "x": 6.239292092495854,
-            "y": 2.276661405774626,
-            "heading": 2.8399996811990342,
-            "angularVelocity": -1.3784796655157707e-7,
-            "velocityX": -3.6538240803329365,
-            "velocityY": 0.9799021815062957,
-            "timestamp": 2.61682757156974
-        },
-        {
-            "x": 5.945407443635962,
-            "y": 2.3842186409280646,
-            "heading": 2.8399996700710783,
-            "angularVelocity": -1.345154198919157e-7,
-            "velocityX": -3.5524958233085573,
-            "velocityY": 1.3001585150212127,
-            "timestamp": 2.699553819108629
-        },
-        {
-            "x": 5.651522869698407,
-            "y": 2.491776080795775,
-            "heading": 2.8399996589431233,
-            "angularVelocity": -1.3451541092672365e-7,
-            "velocityX": -3.5524949176427194,
-            "velocityY": 1.3001609896200028,
-            "timestamp": 2.7822800666475174
-        },
-        {
-            "x": 5.35763829576127,
-            "y": 2.5993335206646324,
-            "heading": 2.839999647815168,
-            "angularVelocity": -1.3451540998350566e-7,
-            "velocityX": -3.5524949176376506,
-            "velocityY": 1.3001609896338533,
-            "timestamp": 2.865006314186406
-        },
-        {
-            "x": 5.063753722106806,
-            "y": 2.7068909613057275,
-            "heading": 2.8399996366871263,
-            "angularVelocity": -1.3451645746164066e-7,
-            "velocityX": -3.5524949142206896,
-            "velocityY": 1.3001609989687195,
-            "timestamp": 2.9477325617252945
-        },
-        {
-            "x": 4.796835976566279,
-            "y": 2.804592771493776,
-            "heading": 2.7513017358564444,
-            "angularVelocity": -1.0721857145640414,
-            "velocityX": -3.226518227060295,
-            "velocityY": 1.1810255281084718,
-            "timestamp": 3.030458809264183
-        },
-        {
-            "x": 4.583301664019943,
-            "y": 2.882755208599232,
-            "heading": 2.680367864718666,
-            "angularVelocity": -0.8574530242585245,
-            "velocityX": -2.581215985240433,
-            "velocityY": 0.9448323770362348,
-            "timestamp": 3.1131850568030717
-        },
-        {
-            "x": 4.42315083623892,
-            "y": 2.9413775026442943,
-            "heading": 2.627181361737275,
-            "angularVelocity": -0.6429217396375844,
-            "velocityX": -1.9359131176080184,
-            "velocityY": 0.7086299184246748,
-            "timestamp": 3.1959113043419602
-        },
-        {
-            "x": 4.316383597066322,
-            "y": 2.980459172951351,
-            "heading": 2.5917278056853306,
-            "angularVelocity": -0.4285647797003965,
-            "velocityX": -1.2906089947136565,
-            "velocityY": 0.47242164935239944,
-            "timestamp": 3.278637551880849
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -0.21429481226012745,
-            "velocityX": -0.645304225133951,
-            "velocityY": 0.23621072670391596,
-            "timestamp": 3.3613637994197374
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 2.7848791215143805e-32,
-            "velocityY": -1.0794258279039968e-32,
-            "timestamp": 3.444090046958626
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S3.traj
deleted file mode 100644
index 55f7626f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S3.traj	
+++ /dev/null
@@ -1,364 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.1305523573234936e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3154171108625845,
-            "y": 5.531249621704393,
-            "heading": 3.152034306361113,
-            "angularVelocity": 0.12992542777148963,
-            "velocityX": 0.7062597425523925,
-            "velocityY": -0.3103963856794681,
-            "timestamp": 0.09262471994534183
-        },
-        {
-            "x": 2.4462513478934307,
-            "y": 5.473748850345723,
-            "heading": 3.1761003812465023,
-            "angularVelocity": 0.2598234564119746,
-            "velocityX": 1.4125196503487585,
-            "velocityY": -0.6207929307921308,
-            "timestamp": 0.18524943989068365
-        },
-        {
-            "x": 2.642502734215126,
-            "y": 5.387497655476605,
-            "heading": 3.2121940698357436,
-            "angularVelocity": 0.38967662855597096,
-            "velocityX": 2.1187798077824542,
-            "velocityY": -0.931189804622509,
-            "timestamp": 0.2778741598360255
-        },
-        {
-            "x": 2.9041712871426104,
-            "y": 5.2724959877699655,
-            "heading": 3.2603098134687913,
-            "angularVelocity": 0.5194697879944143,
-            "velocityX": 2.8250401521526483,
-            "velocityY": -1.2415872110005046,
-            "timestamp": 0.3704988797813673
-        },
-        {
-            "x": 3.22495104061126,
-            "y": 5.131513113466323,
-            "heading": 3.260309819428016,
-            "angularVelocity": 6.433730075600531e-8,
-            "velocityX": 3.4632196853922204,
-            "velocityY": -1.5220869157481582,
-            "timestamp": 0.46312359972670913
-        },
-        {
-            "x": 3.545730793052991,
-            "y": 4.99053023682554,
-            "heading": 3.2603098253870204,
-            "angularVelocity": 6.433492130788198e-8,
-            "velocityX": 3.4632196743053516,
-            "velocityY": -1.5220869409805253,
-            "timestamp": 0.555748319672051
-        },
-        {
-            "x": 3.866510545494718,
-            "y": 4.849547360184748,
-            "heading": 3.2603098313460244,
-            "angularVelocity": 6.433492218112887e-8,
-            "velocityX": 3.463219674305308,
-            "velocityY": -1.5220869409806246,
-            "timestamp": 0.6483730396173928
-        },
-        {
-            "x": 4.187290297936445,
-            "y": 4.708564483543956,
-            "heading": 3.2603098373050288,
-            "angularVelocity": 6.433492230011589e-8,
-            "velocityX": 3.463219674305308,
-            "velocityY": -1.5220869409806246,
-            "timestamp": 0.7409977595627346
-        },
-        {
-            "x": 4.508070050378173,
-            "y": 4.567581606903164,
-            "heading": 3.260309843264033,
-            "angularVelocity": 6.433492243162183e-8,
-            "velocityX": 3.463219674305308,
-            "velocityY": -1.5220869409806246,
-            "timestamp": 0.8336224795080764
-        },
-        {
-            "x": 4.828849802819899,
-            "y": 4.426598730262372,
-            "heading": 3.260309849223037,
-            "angularVelocity": 6.433492147964013e-8,
-            "velocityX": 3.463219674305308,
-            "velocityY": -1.5220869409806246,
-            "timestamp": 0.9262471994534183
-        },
-        {
-            "x": 5.149629555261627,
-            "y": 4.28561585362158,
-            "heading": 3.2603098551820415,
-            "angularVelocity": 6.433492220525221e-8,
-            "velocityX": 3.4632196743053076,
-            "velocityY": -1.5220869409806248,
-            "timestamp": 1.01887191939876
-        },
-        {
-            "x": 5.470409307703354,
-            "y": 4.1446329769807875,
-            "heading": 3.260309861141046,
-            "angularVelocity": 6.433492177225282e-8,
-            "velocityX": 3.463219674305308,
-            "velocityY": -1.5220869409806244,
-            "timestamp": 1.111496639344102
-        },
-        {
-            "x": 5.791189060145081,
-            "y": 4.003650100339995,
-            "heading": 3.2603098671000503,
-            "angularVelocity": 6.433492264273842e-8,
-            "velocityX": 3.463219674305308,
-            "velocityY": -1.5220869409806244,
-            "timestamp": 1.2041213592894437
-        },
-        {
-            "x": 6.111968812586809,
-            "y": 3.8626672236992023,
-            "heading": 3.2603098730590543,
-            "angularVelocity": 6.433492200729343e-8,
-            "velocityX": 3.463219674305308,
-            "velocityY": -1.5220869409806248,
-            "timestamp": 1.2967460792347856
-        },
-        {
-            "x": 6.432748565028133,
-            "y": 3.7216843470574936,
-            "heading": 3.2603098790180582,
-            "angularVelocity": 6.43349214222261e-8,
-            "velocityX": 3.4632196743009604,
-            "velocityY": -1.5220869409905164,
-            "timestamp": 1.3893707991801274
-        },
-        {
-            "x": 6.753528214821051,
-            "y": 3.5807012368590723,
-            "heading": 3.260309884977063,
-            "angularVelocity": 6.433492684215086e-8,
-            "velocityX": 3.4632185660826984,
-            "velocityY": -1.5220894625281032,
-            "timestamp": 1.4819955191254692
-        },
-        {
-            "x": 7.055963078223925,
-            "y": 3.403757816572982,
-            "heading": 3.260309891160044,
-            "angularVelocity": 6.675302824117912e-8,
-            "velocityX": 3.2651635932755534,
-            "velocityY": -1.9103261029075773,
-            "timestamp": 1.574620239070811
-        },
-        {
-            "x": 7.316159274039154,
-            "y": 3.1690804957678615,
-            "heading": 3.2603099328635268,
-            "angularVelocity": 4.502413954005259e-7,
-            "velocityX": 2.809144210841027,
-            "velocityY": -2.533635955321695,
-            "timestamp": 1.6672449590161529
-        },
-        {
-            "x": 7.507506683295118,
-            "y": 2.9535313290497767,
-            "heading": 3.2483076831663804,
-            "angularVelocity": -0.12957933588602658,
-            "velocityX": 2.0658352259405257,
-            "velocityY": -2.3271235459095725,
-            "timestamp": 1.7598696789614947
-        },
-        {
-            "x": 7.631583047935508,
-            "y": 2.7620770453961936,
-            "heading": 3.2242582782979583,
-            "angularVelocity": -0.25964348267518605,
-            "velocityX": 1.3395599437559367,
-            "velocityY": -2.066989069079623,
-            "timestamp": 1.8524943989068365
-        },
-        {
-            "x": 7.68897880573514,
-            "y": 2.5963063443763943,
-            "heading": 3.1881570664078476,
-            "angularVelocity": -0.3897578520228113,
-            "velocityX": 0.6196591777389554,
-            "velocityY": -1.7897025882250568,
-            "timestamp": 1.9451191188521784
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -0.5199159191656997,
-            "velocityX": -0.09693746702217505,
-            "velocityY": -1.5039866728730655,
-            "timestamp": 2.03774383879752
-        },
-        {
-            "x": 7.6191836827180115,
-            "y": 2.3540612532810705,
-            "heading": 3.087368523551757,
-            "angularVelocity": -0.6362125445554702,
-            "velocityX": -0.73515140709603,
-            "velocityY": -1.244329940996524,
-            "timestamp": 2.1204700863364088
-        },
-        {
-            "x": 7.505717742673261,
-            "y": 2.2730171165833535,
-            "heading": 3.0268817588529657,
-            "angularVelocity": -0.7311677550750295,
-            "velocityX": -1.3715833054244395,
-            "velocityY": -0.979666540049693,
-            "timestamp": 2.2031963338752973
-        },
-        {
-            "x": 7.339845145495756,
-            "y": 2.2145049491373583,
-            "heading": 2.9612826526647904,
-            "angularVelocity": -0.7929660553905423,
-            "velocityX": -2.005078220180729,
-            "velocityY": -0.7072987012796481,
-            "timestamp": 2.285922581414186
-        },
-        {
-            "x": 7.122035574070675,
-            "y": 2.1796305402319347,
-            "heading": 2.895398132602207,
-            "angularVelocity": -0.7964161559680392,
-            "velocityX": -2.6328955791532933,
-            "velocityY": -0.4215640131510867,
-            "timestamp": 2.3686488289530745
-        },
-        {
-            "x": 6.853520994844272,
-            "y": 2.1707672940791287,
-            "heading": 2.839999953502189,
-            "angularVelocity": -0.6696566174354214,
-            "velocityX": -3.245820851479772,
-            "velocityY": -0.1071394680223975,
-            "timestamp": 2.451375076491963
-        },
-        {
-            "x": 6.54155924782903,
-            "y": 2.195597775343439,
-            "heading": 2.83999969260268,
-            "angularVelocity": -0.0000031537694148635814,
-            "velocityX": -3.7710129045632463,
-            "velocityY": 0.30015239422817563,
-            "timestamp": 2.5341013240308516
-        },
-        {
-            "x": 6.239292092495854,
-            "y": 2.276661405774626,
-            "heading": 2.8399996811990342,
-            "angularVelocity": -1.3784796655157707e-7,
-            "velocityX": -3.6538240803329365,
-            "velocityY": 0.9799021815062957,
-            "timestamp": 2.61682757156974
-        },
-        {
-            "x": 5.945407443635962,
-            "y": 2.3842186409280646,
-            "heading": 2.8399996700710783,
-            "angularVelocity": -1.345154198919157e-7,
-            "velocityX": -3.5524958233085573,
-            "velocityY": 1.3001585150212127,
-            "timestamp": 2.699553819108629
-        },
-        {
-            "x": 5.651522869698407,
-            "y": 2.491776080795775,
-            "heading": 2.8399996589431233,
-            "angularVelocity": -1.3451541092672365e-7,
-            "velocityX": -3.5524949176427194,
-            "velocityY": 1.3001609896200028,
-            "timestamp": 2.7822800666475174
-        },
-        {
-            "x": 5.35763829576127,
-            "y": 2.5993335206646324,
-            "heading": 2.839999647815168,
-            "angularVelocity": -1.3451540998350566e-7,
-            "velocityX": -3.5524949176376506,
-            "velocityY": 1.3001609896338533,
-            "timestamp": 2.865006314186406
-        },
-        {
-            "x": 5.063753722106806,
-            "y": 2.7068909613057275,
-            "heading": 2.8399996366871263,
-            "angularVelocity": -1.3451645746164066e-7,
-            "velocityX": -3.5524949142206896,
-            "velocityY": 1.3001609989687195,
-            "timestamp": 2.9477325617252945
-        },
-        {
-            "x": 4.796835976566279,
-            "y": 2.804592771493776,
-            "heading": 2.7513017358564444,
-            "angularVelocity": -1.0721857145640414,
-            "velocityX": -3.226518227060295,
-            "velocityY": 1.1810255281084718,
-            "timestamp": 3.030458809264183
-        },
-        {
-            "x": 4.583301664019943,
-            "y": 2.882755208599232,
-            "heading": 2.680367864718666,
-            "angularVelocity": -0.8574530242585245,
-            "velocityX": -2.581215985240433,
-            "velocityY": 0.9448323770362348,
-            "timestamp": 3.1131850568030717
-        },
-        {
-            "x": 4.42315083623892,
-            "y": 2.9413775026442943,
-            "heading": 2.627181361737275,
-            "angularVelocity": -0.6429217396375844,
-            "velocityX": -1.9359131176080184,
-            "velocityY": 0.7086299184246748,
-            "timestamp": 3.1959113043419602
-        },
-        {
-            "x": 4.316383597066322,
-            "y": 2.980459172951351,
-            "heading": 2.5917278056853306,
-            "angularVelocity": -0.4285647797003965,
-            "velocityX": -1.2906089947136565,
-            "velocityY": 0.47242164935239944,
-            "timestamp": 3.278637551880849
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -0.21429481226012745,
-            "velocityX": -0.645304225133951,
-            "velocityY": 0.23621072670391596,
-            "timestamp": 3.3613637994197374
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": 2.7848791215143805e-32,
-            "velocityY": -1.0794258279039968e-32,
-            "timestamp": 3.444090046958626
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S4.1.traj
deleted file mode 100644
index b408904c..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S4.1.traj	
+++ /dev/null
@@ -1,346 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3154271184850326,
-            "y": 5.536626406530092,
-            "heading": 3.157504964629553,
-            "angularVelocity": 0.19152994653168934,
-            "velocityX": 0.7158684847619835,
-            "velocityY": -0.25574133980197417,
-            "timestamp": 0.09139544466297708
-        },
-        {
-            "x": 2.4462968906270097,
-            "y": 5.489871788853177,
-            "heading": 3.1916196887834967,
-            "angularVelocity": 0.3732650383145345,
-            "velocityX": 1.4319069470536998,
-            "velocityY": -0.5115639827490647,
-            "timestamp": 0.18279088932595416
-        },
-        {
-            "x": 2.6426368598266095,
-            "y": 5.419722773289121,
-            "heading": 3.240625955812914,
-            "angularVelocity": 0.536200323878483,
-            "velocityX": 2.148246774482153,
-            "velocityY": -0.7675329533406525,
-            "timestamp": 0.2741863339889312
-        },
-        {
-            "x": 2.9045066521861433,
-            "y": 5.326149233940284,
-            "heading": 3.2998389732795825,
-            "angularVelocity": 0.6478771199673835,
-            "velocityX": 2.8652389987837097,
-            "velocityY": -1.023831545367414,
-            "timestamp": 0.36558177865190833
-        },
-        {
-            "x": 3.230136445009879,
-            "y": 5.209943279570635,
-            "heading": 3.2998389932105203,
-            "angularVelocity": 2.1807364619110316e-7,
-            "velocityX": 3.5628667711449338,
-            "velocityY": -1.2714633075877884,
-            "timestamp": 0.45697722331488544
-        },
-        {
-            "x": 3.555730781054687,
-            "y": 5.093637996752385,
-            "heading": 3.29983900348091,
-            "angularVelocity": 1.1237311095674653e-7,
-            "velocityX": 3.5624788220621486,
-            "velocityY": -1.2725501062676312,
-            "timestamp": 0.5483726679778625
-        },
-        {
-            "x": 3.881325116955394,
-            "y": 4.977332713530725,
-            "heading": 3.2998390137513005,
-            "angularVelocity": 1.123731101511981e-7,
-            "velocityX": 3.562478820485468,
-            "velocityY": -1.2725501106815165,
-            "timestamp": 0.6397681126408397
-        },
-        {
-            "x": 4.2069194528561,
-            "y": 4.861027430309063,
-            "heading": 3.2998390240216913,
-            "angularVelocity": 1.1237311074592376e-7,
-            "velocityX": 3.562478820485462,
-            "velocityY": -1.2725501106815353,
-            "timestamp": 0.7311635573038168
-        },
-        {
-            "x": 4.532513788756806,
-            "y": 4.744722147087402,
-            "heading": 3.2998390342920816,
-            "angularVelocity": 1.1237311063100021e-7,
-            "velocityX": 3.562478820485462,
-            "velocityY": -1.272550110681535,
-            "timestamp": 0.8225590019667939
-        },
-        {
-            "x": 4.858108124657512,
-            "y": 4.62841686386574,
-            "heading": 3.299839044562472,
-            "angularVelocity": 1.1237311003038478e-7,
-            "velocityX": 3.5624788204854614,
-            "velocityY": -1.272550110681535,
-            "timestamp": 0.913954446629771
-        },
-        {
-            "x": 5.183702460558219,
-            "y": 4.5121115806440795,
-            "heading": 3.2998390548328627,
-            "angularVelocity": 1.1237311094761516e-7,
-            "velocityX": 3.5624788204854614,
-            "velocityY": -1.272550110681535,
-            "timestamp": 1.005349891292748
-        },
-        {
-            "x": 5.509296796458925,
-            "y": 4.395806297422418,
-            "heading": 3.299839065103253,
-            "angularVelocity": 1.1237310968464263e-7,
-            "velocityX": 3.562478820485462,
-            "velocityY": -1.2725501106815353,
-            "timestamp": 1.096745335955725
-        },
-        {
-            "x": 5.834891132359631,
-            "y": 4.279501014200756,
-            "heading": 3.2998390753736433,
-            "angularVelocity": 1.123731107718446e-7,
-            "velocityX": 3.562478820485462,
-            "velocityY": -1.2725501106815351,
-            "timestamp": 1.188140780618702
-        },
-        {
-            "x": 6.160485468260297,
-            "y": 4.16319573097898,
-            "heading": 3.299839085644034,
-            "angularVelocity": 1.1237310986388625e-7,
-            "velocityX": 3.562478820485015,
-            "velocityY": -1.2725501106827837,
-            "timestamp": 1.279536225281679
-        },
-        {
-            "x": 6.4860797940952795,
-            "y": 4.046890419578527,
-            "heading": 3.2998390959144244,
-            "angularVelocity": 1.1237311101180894e-7,
-            "velocityX": 3.5624787103517215,
-            "velocityY": -1.2725504189987695,
-            "timestamp": 1.370931669944656
-        },
-        {
-            "x": 6.809159340398316,
-            "y": 3.9237717890604613,
-            "heading": 3.299839106230886,
-            "angularVelocity": 1.1287719938305865e-7,
-            "velocityX": 3.5349633397413522,
-            "velocityY": -1.3470981072657797,
-            "timestamp": 1.462327114607633
-        },
-        {
-            "x": 7.101008389413158,
-            "y": 3.738399942122118,
-            "heading": 3.299839117623808,
-            "angularVelocity": 1.246552472151185e-7,
-            "velocityX": 3.1932559668706793,
-            "velocityY": -2.0282394557178747,
-            "timestamp": 1.55372255927061
-        },
-        {
-            "x": 7.349782731781937,
-            "y": 3.498296000909773,
-            "heading": 3.299839132467128,
-            "angularVelocity": 1.6240765120244838e-7,
-            "velocityX": 2.7219555994956917,
-            "velocityY": -2.6270887142977735,
-            "timestamp": 1.645118003933587
-        },
-        {
-            "x": 7.5357710820320545,
-            "y": 3.2282223692532166,
-            "heading": 3.2881158704604148,
-            "angularVelocity": -0.12826965337097124,
-            "velocityX": 2.0349849047286135,
-            "velocityY": -2.955001014026776,
-            "timestamp": 1.736513448596564
-        },
-        {
-            "x": 7.652314357234611,
-            "y": 2.959723163992482,
-            "heading": 3.2581736625054765,
-            "angularVelocity": -0.32761160105108156,
-            "velocityX": 1.2751540914573078,
-            "velocityY": -2.9377744837371598,
-            "timestamp": 1.827908893259541
-        },
-        {
-            "x": 7.700165626970879,
-            "y": 2.701389747494563,
-            "heading": 3.208935276055406,
-            "angularVelocity": -0.5387400502456196,
-            "velocityX": 0.5235629621664141,
-            "velocityY": -2.826545868347203,
-            "timestamp": 1.919304337922518
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -0.7542528657692632,
-            "velocityX": -0.2206414887004817,
-            "velocityY": -2.6739817109674653,
-            "timestamp": 2.0106997825854953
-        },
-        {
-            "x": 7.619243737534899,
-            "y": 2.269397277536314,
-            "heading": 3.071018168568731,
-            "angularVelocity": -0.9308752015312701,
-            "velocityX": -0.8198752757491441,
-            "velocityY": -2.531604604539154,
-            "timestamp": 2.08480405705527
-        },
-        {
-            "x": 7.514288335022884,
-            "y": 2.093479496062301,
-            "heading": 2.9929449451014944,
-            "angularVelocity": -1.0535589751865693,
-            "velocityX": -1.4163204924814625,
-            "velocityY": -2.373922189141621,
-            "timestamp": 2.1589083315250446
-        },
-        {
-            "x": 7.365556927410938,
-            "y": 1.9309603366701282,
-            "heading": 2.911854592478602,
-            "angularVelocity": -1.0942736192096787,
-            "velocityX": -2.0070557154244444,
-            "velocityY": -2.193114507294498,
-            "timestamp": 2.233012605994819
-        },
-        {
-            "x": 7.174068652738112,
-            "y": 1.784693946040825,
-            "heading": 2.8381473535529165,
-            "angularVelocity": -0.994642204556963,
-            "velocityX": -2.5840381819125056,
-            "velocityY": -1.973791548137795,
-            "timestamp": 2.307116880464594
-        },
-        {
-            "x": 6.9430736895852,
-            "y": 1.6600434204320091,
-            "heading": 2.7943942143969487,
-            "angularVelocity": -0.5904266585027068,
-            "velocityX": -3.117161119324147,
-            "velocityY": -1.6820962960737031,
-            "timestamp": 2.3812211549343685
-        },
-        {
-            "x": 6.679335807473864,
-            "y": 1.5650263715806954,
-            "heading": 2.794393527099141,
-            "angularVelocity": -0.000009274738771364558,
-            "velocityX": -3.5590103809586133,
-            "velocityY": -1.2822073966879628,
-            "timestamp": 2.455325429404143
-        },
-        {
-            "x": 6.403647884167786,
-            "y": 1.5142107470678074,
-            "heading": 2.794393497300607,
-            "angularVelocity": -4.021162732159612e-7,
-            "velocityX": -3.720270190601743,
-            "velocityY": -0.6857313545879138,
-            "timestamp": 2.5294297038739177
-        },
-        {
-            "x": 6.123365156631299,
-            "y": 1.5089521104338168,
-            "heading": 2.7943934713550327,
-            "angularVelocity": -3.501225017023653e-7,
-            "velocityX": -3.782274768114868,
-            "velocityY": -0.07096266270252378,
-            "timestamp": 2.6035339783436924
-        },
-        {
-            "x": 5.853614932127584,
-            "y": 1.5409906174836003,
-            "heading": 2.7698594509818024,
-            "angularVelocity": -0.33107429427995533,
-            "velocityX": -3.6401439246765253,
-            "velocityY": 0.43234357638746057,
-            "timestamp": 2.677638252813467
-        },
-        {
-            "x": 5.6273847181633965,
-            "y": 1.5674202114884777,
-            "heading": 2.697593854639993,
-            "angularVelocity": -0.9751879612723551,
-            "velocityX": -3.0528632198735024,
-            "velocityY": 0.3566541092809186,
-            "timestamp": 2.7517425272832416
-        },
-        {
-            "x": 5.446628200819817,
-            "y": 1.588490932967887,
-            "heading": 2.6254372460611823,
-            "angularVelocity": -0.9737172260984721,
-            "velocityX": -2.4392185017249606,
-            "velocityY": 0.284338813519285,
-            "timestamp": 2.8258468017530163
-        },
-        {
-            "x": 5.311228363745466,
-            "y": 1.6042594522545108,
-            "heading": 2.5652453697807402,
-            "angularVelocity": -0.8122591673836874,
-            "velocityX": -1.8271528605219525,
-            "velocityY": 0.21278825546128166,
-            "timestamp": 2.899951076222791
-        },
-        {
-            "x": 5.22105179084387,
-            "y": 1.614756634238219,
-            "heading": 2.5224127669101155,
-            "angularVelocity": -0.578004483237462,
-            "velocityX": -1.2168876025954114,
-            "velocityY": 0.14165420360473746,
-            "timestamp": 2.9740553506925655
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.302449043195724,
-            "velocityX": -0.6079513113949444,
-            "velocityY": 0.07075658994444238,
-            "timestamp": 3.04815962516234
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": 8.84173108800433e-33,
-            "velocityY": 0,
-            "timestamp": 3.122263899632115
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S4.traj
deleted file mode 100644
index b408904c..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4 To S4.traj	
+++ /dev/null
@@ -1,346 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3154271184850326,
-            "y": 5.536626406530092,
-            "heading": 3.157504964629553,
-            "angularVelocity": 0.19152994653168934,
-            "velocityX": 0.7158684847619835,
-            "velocityY": -0.25574133980197417,
-            "timestamp": 0.09139544466297708
-        },
-        {
-            "x": 2.4462968906270097,
-            "y": 5.489871788853177,
-            "heading": 3.1916196887834967,
-            "angularVelocity": 0.3732650383145345,
-            "velocityX": 1.4319069470536998,
-            "velocityY": -0.5115639827490647,
-            "timestamp": 0.18279088932595416
-        },
-        {
-            "x": 2.6426368598266095,
-            "y": 5.419722773289121,
-            "heading": 3.240625955812914,
-            "angularVelocity": 0.536200323878483,
-            "velocityX": 2.148246774482153,
-            "velocityY": -0.7675329533406525,
-            "timestamp": 0.2741863339889312
-        },
-        {
-            "x": 2.9045066521861433,
-            "y": 5.326149233940284,
-            "heading": 3.2998389732795825,
-            "angularVelocity": 0.6478771199673835,
-            "velocityX": 2.8652389987837097,
-            "velocityY": -1.023831545367414,
-            "timestamp": 0.36558177865190833
-        },
-        {
-            "x": 3.230136445009879,
-            "y": 5.209943279570635,
-            "heading": 3.2998389932105203,
-            "angularVelocity": 2.1807364619110316e-7,
-            "velocityX": 3.5628667711449338,
-            "velocityY": -1.2714633075877884,
-            "timestamp": 0.45697722331488544
-        },
-        {
-            "x": 3.555730781054687,
-            "y": 5.093637996752385,
-            "heading": 3.29983900348091,
-            "angularVelocity": 1.1237311095674653e-7,
-            "velocityX": 3.5624788220621486,
-            "velocityY": -1.2725501062676312,
-            "timestamp": 0.5483726679778625
-        },
-        {
-            "x": 3.881325116955394,
-            "y": 4.977332713530725,
-            "heading": 3.2998390137513005,
-            "angularVelocity": 1.123731101511981e-7,
-            "velocityX": 3.562478820485468,
-            "velocityY": -1.2725501106815165,
-            "timestamp": 0.6397681126408397
-        },
-        {
-            "x": 4.2069194528561,
-            "y": 4.861027430309063,
-            "heading": 3.2998390240216913,
-            "angularVelocity": 1.1237311074592376e-7,
-            "velocityX": 3.562478820485462,
-            "velocityY": -1.2725501106815353,
-            "timestamp": 0.7311635573038168
-        },
-        {
-            "x": 4.532513788756806,
-            "y": 4.744722147087402,
-            "heading": 3.2998390342920816,
-            "angularVelocity": 1.1237311063100021e-7,
-            "velocityX": 3.562478820485462,
-            "velocityY": -1.272550110681535,
-            "timestamp": 0.8225590019667939
-        },
-        {
-            "x": 4.858108124657512,
-            "y": 4.62841686386574,
-            "heading": 3.299839044562472,
-            "angularVelocity": 1.1237311003038478e-7,
-            "velocityX": 3.5624788204854614,
-            "velocityY": -1.272550110681535,
-            "timestamp": 0.913954446629771
-        },
-        {
-            "x": 5.183702460558219,
-            "y": 4.5121115806440795,
-            "heading": 3.2998390548328627,
-            "angularVelocity": 1.1237311094761516e-7,
-            "velocityX": 3.5624788204854614,
-            "velocityY": -1.272550110681535,
-            "timestamp": 1.005349891292748
-        },
-        {
-            "x": 5.509296796458925,
-            "y": 4.395806297422418,
-            "heading": 3.299839065103253,
-            "angularVelocity": 1.1237310968464263e-7,
-            "velocityX": 3.562478820485462,
-            "velocityY": -1.2725501106815353,
-            "timestamp": 1.096745335955725
-        },
-        {
-            "x": 5.834891132359631,
-            "y": 4.279501014200756,
-            "heading": 3.2998390753736433,
-            "angularVelocity": 1.123731107718446e-7,
-            "velocityX": 3.562478820485462,
-            "velocityY": -1.2725501106815351,
-            "timestamp": 1.188140780618702
-        },
-        {
-            "x": 6.160485468260297,
-            "y": 4.16319573097898,
-            "heading": 3.299839085644034,
-            "angularVelocity": 1.1237310986388625e-7,
-            "velocityX": 3.562478820485015,
-            "velocityY": -1.2725501106827837,
-            "timestamp": 1.279536225281679
-        },
-        {
-            "x": 6.4860797940952795,
-            "y": 4.046890419578527,
-            "heading": 3.2998390959144244,
-            "angularVelocity": 1.1237311101180894e-7,
-            "velocityX": 3.5624787103517215,
-            "velocityY": -1.2725504189987695,
-            "timestamp": 1.370931669944656
-        },
-        {
-            "x": 6.809159340398316,
-            "y": 3.9237717890604613,
-            "heading": 3.299839106230886,
-            "angularVelocity": 1.1287719938305865e-7,
-            "velocityX": 3.5349633397413522,
-            "velocityY": -1.3470981072657797,
-            "timestamp": 1.462327114607633
-        },
-        {
-            "x": 7.101008389413158,
-            "y": 3.738399942122118,
-            "heading": 3.299839117623808,
-            "angularVelocity": 1.246552472151185e-7,
-            "velocityX": 3.1932559668706793,
-            "velocityY": -2.0282394557178747,
-            "timestamp": 1.55372255927061
-        },
-        {
-            "x": 7.349782731781937,
-            "y": 3.498296000909773,
-            "heading": 3.299839132467128,
-            "angularVelocity": 1.6240765120244838e-7,
-            "velocityX": 2.7219555994956917,
-            "velocityY": -2.6270887142977735,
-            "timestamp": 1.645118003933587
-        },
-        {
-            "x": 7.5357710820320545,
-            "y": 3.2282223692532166,
-            "heading": 3.2881158704604148,
-            "angularVelocity": -0.12826965337097124,
-            "velocityX": 2.0349849047286135,
-            "velocityY": -2.955001014026776,
-            "timestamp": 1.736513448596564
-        },
-        {
-            "x": 7.652314357234611,
-            "y": 2.959723163992482,
-            "heading": 3.2581736625054765,
-            "angularVelocity": -0.32761160105108156,
-            "velocityX": 1.2751540914573078,
-            "velocityY": -2.9377744837371598,
-            "timestamp": 1.827908893259541
-        },
-        {
-            "x": 7.700165626970879,
-            "y": 2.701389747494563,
-            "heading": 3.208935276055406,
-            "angularVelocity": -0.5387400502456196,
-            "velocityX": 0.5235629621664141,
-            "velocityY": -2.826545868347203,
-            "timestamp": 1.919304337922518
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -0.7542528657692632,
-            "velocityX": -0.2206414887004817,
-            "velocityY": -2.6739817109674653,
-            "timestamp": 2.0106997825854953
-        },
-        {
-            "x": 7.619243737534899,
-            "y": 2.269397277536314,
-            "heading": 3.071018168568731,
-            "angularVelocity": -0.9308752015312701,
-            "velocityX": -0.8198752757491441,
-            "velocityY": -2.531604604539154,
-            "timestamp": 2.08480405705527
-        },
-        {
-            "x": 7.514288335022884,
-            "y": 2.093479496062301,
-            "heading": 2.9929449451014944,
-            "angularVelocity": -1.0535589751865693,
-            "velocityX": -1.4163204924814625,
-            "velocityY": -2.373922189141621,
-            "timestamp": 2.1589083315250446
-        },
-        {
-            "x": 7.365556927410938,
-            "y": 1.9309603366701282,
-            "heading": 2.911854592478602,
-            "angularVelocity": -1.0942736192096787,
-            "velocityX": -2.0070557154244444,
-            "velocityY": -2.193114507294498,
-            "timestamp": 2.233012605994819
-        },
-        {
-            "x": 7.174068652738112,
-            "y": 1.784693946040825,
-            "heading": 2.8381473535529165,
-            "angularVelocity": -0.994642204556963,
-            "velocityX": -2.5840381819125056,
-            "velocityY": -1.973791548137795,
-            "timestamp": 2.307116880464594
-        },
-        {
-            "x": 6.9430736895852,
-            "y": 1.6600434204320091,
-            "heading": 2.7943942143969487,
-            "angularVelocity": -0.5904266585027068,
-            "velocityX": -3.117161119324147,
-            "velocityY": -1.6820962960737031,
-            "timestamp": 2.3812211549343685
-        },
-        {
-            "x": 6.679335807473864,
-            "y": 1.5650263715806954,
-            "heading": 2.794393527099141,
-            "angularVelocity": -0.000009274738771364558,
-            "velocityX": -3.5590103809586133,
-            "velocityY": -1.2822073966879628,
-            "timestamp": 2.455325429404143
-        },
-        {
-            "x": 6.403647884167786,
-            "y": 1.5142107470678074,
-            "heading": 2.794393497300607,
-            "angularVelocity": -4.021162732159612e-7,
-            "velocityX": -3.720270190601743,
-            "velocityY": -0.6857313545879138,
-            "timestamp": 2.5294297038739177
-        },
-        {
-            "x": 6.123365156631299,
-            "y": 1.5089521104338168,
-            "heading": 2.7943934713550327,
-            "angularVelocity": -3.501225017023653e-7,
-            "velocityX": -3.782274768114868,
-            "velocityY": -0.07096266270252378,
-            "timestamp": 2.6035339783436924
-        },
-        {
-            "x": 5.853614932127584,
-            "y": 1.5409906174836003,
-            "heading": 2.7698594509818024,
-            "angularVelocity": -0.33107429427995533,
-            "velocityX": -3.6401439246765253,
-            "velocityY": 0.43234357638746057,
-            "timestamp": 2.677638252813467
-        },
-        {
-            "x": 5.6273847181633965,
-            "y": 1.5674202114884777,
-            "heading": 2.697593854639993,
-            "angularVelocity": -0.9751879612723551,
-            "velocityX": -3.0528632198735024,
-            "velocityY": 0.3566541092809186,
-            "timestamp": 2.7517425272832416
-        },
-        {
-            "x": 5.446628200819817,
-            "y": 1.588490932967887,
-            "heading": 2.6254372460611823,
-            "angularVelocity": -0.9737172260984721,
-            "velocityX": -2.4392185017249606,
-            "velocityY": 0.284338813519285,
-            "timestamp": 2.8258468017530163
-        },
-        {
-            "x": 5.311228363745466,
-            "y": 1.6042594522545108,
-            "heading": 2.5652453697807402,
-            "angularVelocity": -0.8122591673836874,
-            "velocityX": -1.8271528605219525,
-            "velocityY": 0.21278825546128166,
-            "timestamp": 2.899951076222791
-        },
-        {
-            "x": 5.22105179084387,
-            "y": 1.614756634238219,
-            "heading": 2.5224127669101155,
-            "angularVelocity": -0.578004483237462,
-            "velocityX": -1.2168876025954114,
-            "velocityY": 0.14165420360473746,
-            "timestamp": 2.9740553506925655
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.302449043195724,
-            "velocityX": -0.6079513113949444,
-            "velocityY": 0.07075658994444238,
-            "timestamp": 3.04815962516234
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": 8.84173108800433e-33,
-            "velocityY": 0,
-            "timestamp": 3.122263899632115
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj
deleted file mode 100644
index 7e708e4b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -6.980040471932584e-34,
-            "velocityX": -4.489498521627733e-35,
-            "velocityY": 1.1490136335521518e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.252002036509344,
-            "y": 5.607878031980728,
-            "heading": 3.1558282188080855,
-            "angularVelocity": 0.20834807697002747,
-            "velocityX": 0.026352962503434416,
-            "velocityY": 0.6302222639984295,
-            "timestamp": 0.07597007391799691
-        },
-        {
-            "x": 2.2560056075028405,
-            "y": 5.703634203308701,
-            "heading": 3.187479727503087,
-            "angularVelocity": 0.41663127416680434,
-            "velocityX": 0.05269931681016954,
-            "velocityY": 1.260445941270677,
-            "timestamp": 0.15194014783599383
-        },
-        {
-            "x": 2.262010006101527,
-            "y": 5.847268776725278,
-            "heading": 3.2349419015403043,
-            "angularVelocity": 0.6247482934984211,
-            "velocityX": 0.07903636641406601,
-            "velocityY": 1.8906730770279163,
-            "timestamp": 0.22791022175399073
-        },
-        {
-            "x": 2.2700144000703353,
-            "y": 6.038782280998214,
-            "heading": 3.298188503077313,
-            "angularVelocity": 0.8325199420666319,
-            "velocityX": 0.105362461242945,
-            "velocityY": 2.520907172996284,
-            "timestamp": 0.30388029567198765
-        },
-        {
-            "x": 2.280009617993533,
-            "y": 6.278187235561798,
-            "heading": 3.376569762958043,
-            "angularVelocity": 1.0317386286254728,
-            "velocityX": 0.13156783201220376,
-            "velocityY": 3.151306063253281,
-            "timestamp": 0.3798503695899846
-        },
-        {
-            "x": 2.28800573619677,
-            "y": 6.469712012711673,
-            "heading": 3.4392336798072036,
-            "angularVelocity": 0.8248500181374163,
-            "velocityX": 0.10525352669615307,
-            "velocityY": 2.5210555587534333,
-            "timestamp": 0.4558204435079815
-        },
-        {
-            "x": 2.2940028560112626,
-            "y": 6.613355954230437,
-            "heading": 3.486212722152842,
-            "angularVelocity": 0.6183887934128925,
-            "velocityX": 0.07894055521080842,
-            "velocityY": 1.890796390086646,
-            "timestamp": 0.5317905174259784
-        },
-        {
-            "x": 2.2980009548636073,
-            "y": 6.709118670121978,
-            "heading": 3.517527021603037,
-            "angularVelocity": 0.41219256261348647,
-            "velocityX": 0.052627286589986035,
-            "velocityY": 1.2605320878706525,
-            "timestamp": 0.6077605913439753
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 0.20611123260786593,
-            "velocityX": 0.02631358682828647,
-            "velocityY": 0.6302656744773738,
-            "timestamp": 0.6837306652619722
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 2.252106897096389e-33,
-            "velocityX": 0,
-            "velocityY": 2.1844888905645107e-35,
-            "timestamp": 0.7597007391799692
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.traj
deleted file mode 100644
index 7e708e4b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -6.980040471932584e-34,
-            "velocityX": -4.489498521627733e-35,
-            "velocityY": 1.1490136335521518e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.252002036509344,
-            "y": 5.607878031980728,
-            "heading": 3.1558282188080855,
-            "angularVelocity": 0.20834807697002747,
-            "velocityX": 0.026352962503434416,
-            "velocityY": 0.6302222639984295,
-            "timestamp": 0.07597007391799691
-        },
-        {
-            "x": 2.2560056075028405,
-            "y": 5.703634203308701,
-            "heading": 3.187479727503087,
-            "angularVelocity": 0.41663127416680434,
-            "velocityX": 0.05269931681016954,
-            "velocityY": 1.260445941270677,
-            "timestamp": 0.15194014783599383
-        },
-        {
-            "x": 2.262010006101527,
-            "y": 5.847268776725278,
-            "heading": 3.2349419015403043,
-            "angularVelocity": 0.6247482934984211,
-            "velocityX": 0.07903636641406601,
-            "velocityY": 1.8906730770279163,
-            "timestamp": 0.22791022175399073
-        },
-        {
-            "x": 2.2700144000703353,
-            "y": 6.038782280998214,
-            "heading": 3.298188503077313,
-            "angularVelocity": 0.8325199420666319,
-            "velocityX": 0.105362461242945,
-            "velocityY": 2.520907172996284,
-            "timestamp": 0.30388029567198765
-        },
-        {
-            "x": 2.280009617993533,
-            "y": 6.278187235561798,
-            "heading": 3.376569762958043,
-            "angularVelocity": 1.0317386286254728,
-            "velocityX": 0.13156783201220376,
-            "velocityY": 3.151306063253281,
-            "timestamp": 0.3798503695899846
-        },
-        {
-            "x": 2.28800573619677,
-            "y": 6.469712012711673,
-            "heading": 3.4392336798072036,
-            "angularVelocity": 0.8248500181374163,
-            "velocityX": 0.10525352669615307,
-            "velocityY": 2.5210555587534333,
-            "timestamp": 0.4558204435079815
-        },
-        {
-            "x": 2.2940028560112626,
-            "y": 6.613355954230437,
-            "heading": 3.486212722152842,
-            "angularVelocity": 0.6183887934128925,
-            "velocityX": 0.07894055521080842,
-            "velocityY": 1.890796390086646,
-            "timestamp": 0.5317905174259784
-        },
-        {
-            "x": 2.2980009548636073,
-            "y": 6.709118670121978,
-            "heading": 3.517527021603037,
-            "angularVelocity": 0.41219256261348647,
-            "velocityX": 0.052627286589986035,
-            "velocityY": 1.2605320878706525,
-            "timestamp": 0.6077605913439753
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 0.20611123260786593,
-            "velocityX": 0.02631358682828647,
-            "velocityY": 0.6302656744773738,
-            "timestamp": 0.6837306652619722
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 2.252106897096389e-33,
-            "velocityX": 0,
-            "velocityY": 2.1844888905645107e-35,
-            "timestamp": 0.7597007391799692
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj
deleted file mode 100644
index 7db396b8..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -7.990908752759601e-36,
-            "velocityX": 0,
-            "velocityY": 3.147004019802851e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.252001928264259,
-            "y": 5.512001885302468,
-            "heading": 3.1243023137364228,
-            "angularVelocity": -0.2063808903009179,
-            "velocityX": 0.026319785639686007,
-            "velocityY": -0.6310416374562365,
-            "timestamp": 0.07606172374142242
-        },
-        {
-            "x": 2.2560053075353443,
-            "y": 5.416005550394565,
-            "heading": 3.0929118769402844,
-            "angularVelocity": -0.41269688947430483,
-            "velocityX": 0.052633296672250224,
-            "velocityY": -1.2620846621127049,
-            "timestamp": 0.15212344748284484
-        },
-        {
-            "x": 2.262009466656741,
-            "y": 5.272010740000972,
-            "heading": 3.0458411253223887,
-            "angularVelocity": -0.6188493936571329,
-            "velocityX": 0.0789379838643735,
-            "velocityY": -1.8931310429292179,
-            "timestamp": 0.22818517122426726
-        },
-        {
-            "x": 2.2700136159756155,
-            "y": 5.080016943812723,
-            "heading": 2.983115684252823,
-            "angularVelocity": -0.8246649955344898,
-            "velocityX": 0.10523228931919847,
-            "velocityY": -2.524184132888531,
-            "timestamp": 0.3042468949656897
-        },
-        {
-            "x": 2.280009094575526,
-            "y": 4.840012208292782,
-            "heading": 2.9053630904797005,
-            "angularVelocity": -1.0222302355051571,
-            "velocityX": 0.1314127278247149,
-            "velocityY": -3.155394378595143,
-            "timestamp": 0.3803086187071121
-        },
-        {
-            "x": 2.2880054239679986,
-            "y": 4.64800763986969,
-            "heading": 2.8432007503387635,
-            "angularVelocity": -0.8172617853397972,
-            "velocityX": 0.10512947904857922,
-            "velocityY": -2.524325757799354,
-            "timestamp": 0.45637034244853447
-        },
-        {
-            "x": 2.294002700573331,
-            "y": 4.504003870137798,
-            "heading": 2.7965972482938968,
-            "angularVelocity": -0.6127063620501001,
-            "velocityX": 0.07884749793103833,
-            "velocityY": -1.8932488332954869,
-            "timestamp": 0.5324320661899569
-        },
-        {
-            "x": 2.2980009029345383,
-            "y": 4.408001272313515,
-            "heading": 2.7655331423967997,
-            "angularVelocity": -0.40840654627682993,
-            "velocityX": 0.05256523471383641,
-            "velocityY": -1.2621670020344438,
-            "timestamp": 0.6084937899313793
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": -0.20421759635116843,
-            "velocityX": 0.026282563254256014,
-            "velocityY": -0.631083151319306,
-            "timestamp": 0.6845555136728017
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": -4.352426400818003e-34,
-            "velocityX": -2.6471819427760516e-34,
-            "velocityY": 2.5922629666231517e-36,
-            "timestamp": 0.760617237414224
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.traj
deleted file mode 100644
index 7db396b8..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -7.990908752759601e-36,
-            "velocityX": 0,
-            "velocityY": 3.147004019802851e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.252001928264259,
-            "y": 5.512001885302468,
-            "heading": 3.1243023137364228,
-            "angularVelocity": -0.2063808903009179,
-            "velocityX": 0.026319785639686007,
-            "velocityY": -0.6310416374562365,
-            "timestamp": 0.07606172374142242
-        },
-        {
-            "x": 2.2560053075353443,
-            "y": 5.416005550394565,
-            "heading": 3.0929118769402844,
-            "angularVelocity": -0.41269688947430483,
-            "velocityX": 0.052633296672250224,
-            "velocityY": -1.2620846621127049,
-            "timestamp": 0.15212344748284484
-        },
-        {
-            "x": 2.262009466656741,
-            "y": 5.272010740000972,
-            "heading": 3.0458411253223887,
-            "angularVelocity": -0.6188493936571329,
-            "velocityX": 0.0789379838643735,
-            "velocityY": -1.8931310429292179,
-            "timestamp": 0.22818517122426726
-        },
-        {
-            "x": 2.2700136159756155,
-            "y": 5.080016943812723,
-            "heading": 2.983115684252823,
-            "angularVelocity": -0.8246649955344898,
-            "velocityX": 0.10523228931919847,
-            "velocityY": -2.524184132888531,
-            "timestamp": 0.3042468949656897
-        },
-        {
-            "x": 2.280009094575526,
-            "y": 4.840012208292782,
-            "heading": 2.9053630904797005,
-            "angularVelocity": -1.0222302355051571,
-            "velocityX": 0.1314127278247149,
-            "velocityY": -3.155394378595143,
-            "timestamp": 0.3803086187071121
-        },
-        {
-            "x": 2.2880054239679986,
-            "y": 4.64800763986969,
-            "heading": 2.8432007503387635,
-            "angularVelocity": -0.8172617853397972,
-            "velocityX": 0.10512947904857922,
-            "velocityY": -2.524325757799354,
-            "timestamp": 0.45637034244853447
-        },
-        {
-            "x": 2.294002700573331,
-            "y": 4.504003870137798,
-            "heading": 2.7965972482938968,
-            "angularVelocity": -0.6127063620501001,
-            "velocityX": 0.07884749793103833,
-            "velocityY": -1.8932488332954869,
-            "timestamp": 0.5324320661899569
-        },
-        {
-            "x": 2.2980009029345383,
-            "y": 4.408001272313515,
-            "heading": 2.7655331423967997,
-            "angularVelocity": -0.40840654627682993,
-            "velocityX": 0.05256523471383641,
-            "velocityY": -1.2621670020344438,
-            "timestamp": 0.6084937899313793
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": -0.20421759635116843,
-            "velocityX": 0.026282563254256014,
-            "velocityY": -0.631083151319306,
-            "timestamp": 0.6845555136728017
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": -4.352426400818003e-34,
-            "velocityX": -2.6471819427760516e-34,
-            "velocityY": 2.5922629666231517e-36,
-            "timestamp": 0.760617237414224
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S1.1.traj
deleted file mode 100644
index 99f476a8..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S1.1.traj	
+++ /dev/null
@@ -1,337 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0,
-            "velocityX": -8.850231580467738e-33,
-            "velocityY": -9.949674184637553e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3596219168979733,
-            "y": 4.390117696915229,
-            "heading": 2.7532876341018757,
-            "angularVelocity": 0.0367297993475745,
-            "velocityX": 0.6661024239682877,
-            "velocityY": 0.3364781268925937,
-            "timestamp": 0.08950863223523108
-        },
-        {
-            "x": 2.478866409519412,
-            "y": 4.45035344252589,
-            "heading": 2.759663263066401,
-            "angularVelocity": 0.07122920779048274,
-            "velocityX": 1.3322122084053396,
-            "velocityY": 0.6729601839112106,
-            "timestamp": 0.17901726447046215
-        },
-        {
-            "x": 2.6577344843676225,
-            "y": 4.540707777094465,
-            "heading": 2.7687936152184336,
-            "angularVelocity": 0.10200526948103018,
-            "velocityX": 1.998333237604842,
-            "velocityY": 1.009448276799944,
-            "timestamp": 0.2685258967056932
-        },
-        {
-            "x": 2.896227808747147,
-            "y": 4.661181607047397,
-            "heading": 2.7800101234260337,
-            "angularVelocity": 0.12531202776199954,
-            "velocityX": 2.664472894108777,
-            "velocityY": 1.3459464963817573,
-            "timestamp": 0.3580345289409243
-        },
-        {
-            "x": 3.194348569009026,
-            "y": 4.811776231628672,
-            "heading": 2.7912909680084645,
-            "angularVelocity": 0.126030800613561,
-            "velocityX": 3.3306369767600654,
-            "velocityY": 1.6824592312561388,
-            "timestamp": 0.4475431611761554
-        },
-        {
-            "x": 3.4965824721893735,
-            "y": 4.964447700014571,
-            "heading": 2.791290970219591,
-            "angularVelocity": 2.4702941902196968e-8,
-            "velocityX": 3.3765894487815307,
-            "velocityY": 1.7056619520749001,
-            "timestamp": 0.5370517934113864
-        },
-        {
-            "x": 3.798816375371316,
-            "y": 5.117119168397318,
-            "heading": 2.7912909724307147,
-            "angularVelocity": 2.470290930957206e-8,
-            "velocityX": 3.3765894487993435,
-            "velocityY": 1.7056619520396954,
-            "timestamp": 0.6265604256466175
-        },
-        {
-            "x": 4.101050278553258,
-            "y": 5.2697906367800655,
-            "heading": 2.7912909746418384,
-            "angularVelocity": 2.4702908438395345e-8,
-            "velocityX": 3.376589448799344,
-            "velocityY": 1.705661952039695,
-            "timestamp": 0.7160690578818486
-        },
-        {
-            "x": 4.4032841817351995,
-            "y": 5.422462105162813,
-            "heading": 2.791290976852962,
-            "angularVelocity": 2.470290834028317e-8,
-            "velocityX": 3.376589448799344,
-            "velocityY": 1.7056619520396945,
-            "timestamp": 0.8055776901170797
-        },
-        {
-            "x": 4.705518084917142,
-            "y": 5.5751335735455605,
-            "heading": 2.791290979064086,
-            "angularVelocity": 2.470290893539666e-8,
-            "velocityX": 3.376589448799343,
-            "velocityY": 1.7056619520396956,
-            "timestamp": 0.8950863223523108
-        },
-        {
-            "x": 5.007751988099084,
-            "y": 5.727805041928308,
-            "heading": 2.791290981275209,
-            "angularVelocity": 2.470290833665101e-8,
-            "velocityX": 3.376589448799344,
-            "velocityY": 1.705661952039695,
-            "timestamp": 0.9845949545875419
-        },
-        {
-            "x": 5.309985891281026,
-            "y": 5.8804765103110554,
-            "heading": 2.791290983486333,
-            "angularVelocity": 2.4702909020592533e-8,
-            "velocityX": 3.376589448799343,
-            "velocityY": 1.705661952039695,
-            "timestamp": 1.0741035868227728
-        },
-        {
-            "x": 5.612219794462969,
-            "y": 6.033147978693803,
-            "heading": 2.7912909856974566,
-            "angularVelocity": 2.470290845419301e-8,
-            "velocityX": 3.3765894487993435,
-            "velocityY": 1.7056619520396947,
-            "timestamp": 1.1636122190580038
-        },
-        {
-            "x": 5.914453697644911,
-            "y": 6.18581944707655,
-            "heading": 2.7912909879085803,
-            "angularVelocity": 2.4702909294524738e-8,
-            "velocityX": 3.3765894487993435,
-            "velocityY": 1.705661952039695,
-            "timestamp": 1.2531208512932348
-        },
-        {
-            "x": 6.216687600826853,
-            "y": 6.338490915459298,
-            "heading": 2.791290990119704,
-            "angularVelocity": 2.4702908192380297e-8,
-            "velocityX": 3.3765894487993426,
-            "velocityY": 1.705661952039695,
-            "timestamp": 1.3426294835284658
-        },
-        {
-            "x": 6.518921504008634,
-            "y": 6.491162383842364,
-            "heading": 2.791290992330828,
-            "angularVelocity": 2.470290883873948e-8,
-            "velocityX": 3.3765894487975445,
-            "velocityY": 1.705661952043257,
-            "timestamp": 1.4321381157636968
-        },
-        {
-            "x": 6.821155370898669,
-            "y": 6.643833924069847,
-            "heading": 2.7912909945419515,
-            "angularVelocity": 2.4702908942452264e-8,
-            "velocityX": 3.3765890433422854,
-            "velocityY": 1.705662754696757,
-            "timestamp": 1.5216467479989277
-        },
-        {
-            "x": 7.115501994166732,
-            "y": 6.811207678318402,
-            "heading": 2.7912909995251494,
-            "angularVelocity": 5.5672818987002594e-8,
-            "velocityX": 3.2884719151390045,
-            "velocityY": 1.8699174601248647,
-            "timestamp": 1.6111553802341587
-        },
-        {
-            "x": 7.349346251698842,
-            "y": 6.9991016785605416,
-            "heading": 2.863630358302155,
-            "angularVelocity": 0.8081830430264627,
-            "velocityX": 2.6125330227095924,
-            "velocityY": 2.099171840190225,
-            "timestamp": 1.7006640124693897
-        },
-        {
-            "x": 7.522269801890552,
-            "y": 7.159584774540021,
-            "heading": 2.9361878922250697,
-            "angularVelocity": 0.8106205190604594,
-            "velocityX": 1.9319203731910752,
-            "velocityY": 1.792934289932232,
-            "timestamp": 1.7901726447046207
-        },
-        {
-            "x": 7.634890963268431,
-            "y": 7.291326986197087,
-            "heading": 3.0068580812297814,
-            "angularVelocity": 0.789534900041686,
-            "velocityX": 1.258215644295718,
-            "velocityY": 1.4718380603877852,
-            "timestamp": 1.8796812769398517
-        },
-        {
-            "x": 7.687430049075347,
-            "y": 7.393874919842459,
-            "heading": 3.074917441639189,
-            "angularVelocity": 0.7603664441050307,
-            "velocityX": 0.5869722784819492,
-            "velocityY": 1.1456764681184401,
-            "timestamp": 1.9691899091750826
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 0.7271092936575372,
-            "velocityX": -0.08300930189416394,
-            "velocityY": 0.8169612062149058,
-            "timestamp": 2.0586985414103136
-        },
-        {
-            "x": 7.610902849690699,
-            "y": 7.510752049010022,
-            "heading": 3.20269374092293,
-            "angularVelocity": 0.6908898775103415,
-            "velocityX": -0.7614559445765343,
-            "velocityY": 0.4821509665298148,
-            "timestamp": 2.149442005692712
-        },
-        {
-            "x": 7.480264531664604,
-            "y": 7.524075152445223,
-            "heading": 3.2620627183539166,
-            "angularVelocity": 0.6542507264900841,
-            "velocityX": -1.4396443761452817,
-            "velocityY": 0.1468216310734938,
-            "timestamp": 2.2401854699751107
-        },
-        {
-            "x": 7.2881241468833045,
-            "y": 7.506890911811968,
-            "heading": 3.31804363381791,
-            "angularVelocity": 0.6169140213754432,
-            "velocityX": -2.117401912090868,
-            "velocityY": -0.18937166185078083,
-            "timestamp": 2.3309289342575092
-        },
-        {
-            "x": 7.034560081128849,
-            "y": 7.459042940332114,
-            "heading": 3.3705102766509376,
-            "angularVelocity": 0.5781864649750256,
-            "velocityX": -2.7942956306511584,
-            "velocityY": -0.5272883491746447,
-            "timestamp": 2.421672398539908
-        },
-        {
-            "x": 6.71980893194678,
-            "y": 7.380065366153176,
-            "heading": 3.4190860492179427,
-            "angularVelocity": 0.5353087734873627,
-            "velocityX": -3.468582025946757,
-            "velocityY": -0.8703389803716955,
-            "timestamp": 2.5124158628223063
-        },
-        {
-            "x": 6.409212684326435,
-            "y": 7.233883432324963,
-            "heading": 3.4190860479548357,
-            "angularVelocity": -1.3919538885721925e-8,
-            "velocityX": -3.4227946891442476,
-            "velocityY": -1.6109362253714221,
-            "timestamp": 2.603159327104705
-        },
-        {
-            "x": 6.100914553727412,
-            "y": 7.0829150593165995,
-            "heading": 3.4190860462953716,
-            "angularVelocity": -1.8287422949592592e-8,
-            "velocityX": -3.397469261693411,
-            "velocityY": -1.663683155610435,
-            "timestamp": 2.6939027913871034
-        },
-        {
-            "x": 5.79261648458282,
-            "y": 6.9319467005164075,
-            "heading": 3.4190860242893293,
-            "angularVelocity": -2.4250829002858187e-7,
-            "velocityX": -3.3974685844608317,
-            "velocityY": -1.6636829990352793,
-            "timestamp": 2.784646255669502
-        },
-        {
-            "x": 5.545968445126592,
-            "y": 6.811167311993174,
-            "heading": 3.4140434484907587,
-            "angularVelocity": -0.05556957559916094,
-            "velocityX": -2.7180804855393776,
-            "velocityY": -1.330998209935673,
-            "timestamp": 2.8753897199519005
-        },
-        {
-            "x": 5.3609835283362095,
-            "y": 6.720583315840053,
-            "heading": 3.409029867698435,
-            "angularVelocity": -0.05525004838608659,
-            "velocityX": -2.0385481009927036,
-            "velocityY": -0.9982426488724173,
-            "timestamp": 2.966133184234299
-        },
-        {
-            "x": 5.237660981460967,
-            "y": 6.6601943432298505,
-            "heading": 3.4052336568932624,
-            "angularVelocity": -0.041834536902387906,
-            "velocityX": -1.3590240118170522,
-            "velocityY": -0.6654911523133865,
-            "timestamp": 3.0568766485166976
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": -0.022572972388422655,
-            "velocityX": -0.6795087883031954,
-            "velocityY": -0.3327439994563607,
-            "timestamp": 3.147620112799096
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0,
-            "velocityX": -1.5756247890906886e-35,
-            "velocityY": 0,
-            "timestamp": 3.2383635770814947
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S1.traj
deleted file mode 100644
index 99f476a8..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S1.traj	
+++ /dev/null
@@ -1,337 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0,
-            "velocityX": -8.850231580467738e-33,
-            "velocityY": -9.949674184637553e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3596219168979733,
-            "y": 4.390117696915229,
-            "heading": 2.7532876341018757,
-            "angularVelocity": 0.0367297993475745,
-            "velocityX": 0.6661024239682877,
-            "velocityY": 0.3364781268925937,
-            "timestamp": 0.08950863223523108
-        },
-        {
-            "x": 2.478866409519412,
-            "y": 4.45035344252589,
-            "heading": 2.759663263066401,
-            "angularVelocity": 0.07122920779048274,
-            "velocityX": 1.3322122084053396,
-            "velocityY": 0.6729601839112106,
-            "timestamp": 0.17901726447046215
-        },
-        {
-            "x": 2.6577344843676225,
-            "y": 4.540707777094465,
-            "heading": 2.7687936152184336,
-            "angularVelocity": 0.10200526948103018,
-            "velocityX": 1.998333237604842,
-            "velocityY": 1.009448276799944,
-            "timestamp": 0.2685258967056932
-        },
-        {
-            "x": 2.896227808747147,
-            "y": 4.661181607047397,
-            "heading": 2.7800101234260337,
-            "angularVelocity": 0.12531202776199954,
-            "velocityX": 2.664472894108777,
-            "velocityY": 1.3459464963817573,
-            "timestamp": 0.3580345289409243
-        },
-        {
-            "x": 3.194348569009026,
-            "y": 4.811776231628672,
-            "heading": 2.7912909680084645,
-            "angularVelocity": 0.126030800613561,
-            "velocityX": 3.3306369767600654,
-            "velocityY": 1.6824592312561388,
-            "timestamp": 0.4475431611761554
-        },
-        {
-            "x": 3.4965824721893735,
-            "y": 4.964447700014571,
-            "heading": 2.791290970219591,
-            "angularVelocity": 2.4702941902196968e-8,
-            "velocityX": 3.3765894487815307,
-            "velocityY": 1.7056619520749001,
-            "timestamp": 0.5370517934113864
-        },
-        {
-            "x": 3.798816375371316,
-            "y": 5.117119168397318,
-            "heading": 2.7912909724307147,
-            "angularVelocity": 2.470290930957206e-8,
-            "velocityX": 3.3765894487993435,
-            "velocityY": 1.7056619520396954,
-            "timestamp": 0.6265604256466175
-        },
-        {
-            "x": 4.101050278553258,
-            "y": 5.2697906367800655,
-            "heading": 2.7912909746418384,
-            "angularVelocity": 2.4702908438395345e-8,
-            "velocityX": 3.376589448799344,
-            "velocityY": 1.705661952039695,
-            "timestamp": 0.7160690578818486
-        },
-        {
-            "x": 4.4032841817351995,
-            "y": 5.422462105162813,
-            "heading": 2.791290976852962,
-            "angularVelocity": 2.470290834028317e-8,
-            "velocityX": 3.376589448799344,
-            "velocityY": 1.7056619520396945,
-            "timestamp": 0.8055776901170797
-        },
-        {
-            "x": 4.705518084917142,
-            "y": 5.5751335735455605,
-            "heading": 2.791290979064086,
-            "angularVelocity": 2.470290893539666e-8,
-            "velocityX": 3.376589448799343,
-            "velocityY": 1.7056619520396956,
-            "timestamp": 0.8950863223523108
-        },
-        {
-            "x": 5.007751988099084,
-            "y": 5.727805041928308,
-            "heading": 2.791290981275209,
-            "angularVelocity": 2.470290833665101e-8,
-            "velocityX": 3.376589448799344,
-            "velocityY": 1.705661952039695,
-            "timestamp": 0.9845949545875419
-        },
-        {
-            "x": 5.309985891281026,
-            "y": 5.8804765103110554,
-            "heading": 2.791290983486333,
-            "angularVelocity": 2.4702909020592533e-8,
-            "velocityX": 3.376589448799343,
-            "velocityY": 1.705661952039695,
-            "timestamp": 1.0741035868227728
-        },
-        {
-            "x": 5.612219794462969,
-            "y": 6.033147978693803,
-            "heading": 2.7912909856974566,
-            "angularVelocity": 2.470290845419301e-8,
-            "velocityX": 3.3765894487993435,
-            "velocityY": 1.7056619520396947,
-            "timestamp": 1.1636122190580038
-        },
-        {
-            "x": 5.914453697644911,
-            "y": 6.18581944707655,
-            "heading": 2.7912909879085803,
-            "angularVelocity": 2.4702909294524738e-8,
-            "velocityX": 3.3765894487993435,
-            "velocityY": 1.705661952039695,
-            "timestamp": 1.2531208512932348
-        },
-        {
-            "x": 6.216687600826853,
-            "y": 6.338490915459298,
-            "heading": 2.791290990119704,
-            "angularVelocity": 2.4702908192380297e-8,
-            "velocityX": 3.3765894487993426,
-            "velocityY": 1.705661952039695,
-            "timestamp": 1.3426294835284658
-        },
-        {
-            "x": 6.518921504008634,
-            "y": 6.491162383842364,
-            "heading": 2.791290992330828,
-            "angularVelocity": 2.470290883873948e-8,
-            "velocityX": 3.3765894487975445,
-            "velocityY": 1.705661952043257,
-            "timestamp": 1.4321381157636968
-        },
-        {
-            "x": 6.821155370898669,
-            "y": 6.643833924069847,
-            "heading": 2.7912909945419515,
-            "angularVelocity": 2.4702908942452264e-8,
-            "velocityX": 3.3765890433422854,
-            "velocityY": 1.705662754696757,
-            "timestamp": 1.5216467479989277
-        },
-        {
-            "x": 7.115501994166732,
-            "y": 6.811207678318402,
-            "heading": 2.7912909995251494,
-            "angularVelocity": 5.5672818987002594e-8,
-            "velocityX": 3.2884719151390045,
-            "velocityY": 1.8699174601248647,
-            "timestamp": 1.6111553802341587
-        },
-        {
-            "x": 7.349346251698842,
-            "y": 6.9991016785605416,
-            "heading": 2.863630358302155,
-            "angularVelocity": 0.8081830430264627,
-            "velocityX": 2.6125330227095924,
-            "velocityY": 2.099171840190225,
-            "timestamp": 1.7006640124693897
-        },
-        {
-            "x": 7.522269801890552,
-            "y": 7.159584774540021,
-            "heading": 2.9361878922250697,
-            "angularVelocity": 0.8106205190604594,
-            "velocityX": 1.9319203731910752,
-            "velocityY": 1.792934289932232,
-            "timestamp": 1.7901726447046207
-        },
-        {
-            "x": 7.634890963268431,
-            "y": 7.291326986197087,
-            "heading": 3.0068580812297814,
-            "angularVelocity": 0.789534900041686,
-            "velocityX": 1.258215644295718,
-            "velocityY": 1.4718380603877852,
-            "timestamp": 1.8796812769398517
-        },
-        {
-            "x": 7.687430049075347,
-            "y": 7.393874919842459,
-            "heading": 3.074917441639189,
-            "angularVelocity": 0.7603664441050307,
-            "velocityX": 0.5869722784819492,
-            "velocityY": 1.1456764681184401,
-            "timestamp": 1.9691899091750826
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 0.7271092936575372,
-            "velocityX": -0.08300930189416394,
-            "velocityY": 0.8169612062149058,
-            "timestamp": 2.0586985414103136
-        },
-        {
-            "x": 7.610902849690699,
-            "y": 7.510752049010022,
-            "heading": 3.20269374092293,
-            "angularVelocity": 0.6908898775103415,
-            "velocityX": -0.7614559445765343,
-            "velocityY": 0.4821509665298148,
-            "timestamp": 2.149442005692712
-        },
-        {
-            "x": 7.480264531664604,
-            "y": 7.524075152445223,
-            "heading": 3.2620627183539166,
-            "angularVelocity": 0.6542507264900841,
-            "velocityX": -1.4396443761452817,
-            "velocityY": 0.1468216310734938,
-            "timestamp": 2.2401854699751107
-        },
-        {
-            "x": 7.2881241468833045,
-            "y": 7.506890911811968,
-            "heading": 3.31804363381791,
-            "angularVelocity": 0.6169140213754432,
-            "velocityX": -2.117401912090868,
-            "velocityY": -0.18937166185078083,
-            "timestamp": 2.3309289342575092
-        },
-        {
-            "x": 7.034560081128849,
-            "y": 7.459042940332114,
-            "heading": 3.3705102766509376,
-            "angularVelocity": 0.5781864649750256,
-            "velocityX": -2.7942956306511584,
-            "velocityY": -0.5272883491746447,
-            "timestamp": 2.421672398539908
-        },
-        {
-            "x": 6.71980893194678,
-            "y": 7.380065366153176,
-            "heading": 3.4190860492179427,
-            "angularVelocity": 0.5353087734873627,
-            "velocityX": -3.468582025946757,
-            "velocityY": -0.8703389803716955,
-            "timestamp": 2.5124158628223063
-        },
-        {
-            "x": 6.409212684326435,
-            "y": 7.233883432324963,
-            "heading": 3.4190860479548357,
-            "angularVelocity": -1.3919538885721925e-8,
-            "velocityX": -3.4227946891442476,
-            "velocityY": -1.6109362253714221,
-            "timestamp": 2.603159327104705
-        },
-        {
-            "x": 6.100914553727412,
-            "y": 7.0829150593165995,
-            "heading": 3.4190860462953716,
-            "angularVelocity": -1.8287422949592592e-8,
-            "velocityX": -3.397469261693411,
-            "velocityY": -1.663683155610435,
-            "timestamp": 2.6939027913871034
-        },
-        {
-            "x": 5.79261648458282,
-            "y": 6.9319467005164075,
-            "heading": 3.4190860242893293,
-            "angularVelocity": -2.4250829002858187e-7,
-            "velocityX": -3.3974685844608317,
-            "velocityY": -1.6636829990352793,
-            "timestamp": 2.784646255669502
-        },
-        {
-            "x": 5.545968445126592,
-            "y": 6.811167311993174,
-            "heading": 3.4140434484907587,
-            "angularVelocity": -0.05556957559916094,
-            "velocityX": -2.7180804855393776,
-            "velocityY": -1.330998209935673,
-            "timestamp": 2.8753897199519005
-        },
-        {
-            "x": 5.3609835283362095,
-            "y": 6.720583315840053,
-            "heading": 3.409029867698435,
-            "angularVelocity": -0.05525004838608659,
-            "velocityX": -2.0385481009927036,
-            "velocityY": -0.9982426488724173,
-            "timestamp": 2.966133184234299
-        },
-        {
-            "x": 5.237660981460967,
-            "y": 6.6601943432298505,
-            "heading": 3.4052336568932624,
-            "angularVelocity": -0.041834536902387906,
-            "velocityX": -1.3590240118170522,
-            "velocityY": -0.6654911523133865,
-            "timestamp": 3.0568766485166976
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": -0.022572972388422655,
-            "velocityX": -0.6795087883031954,
-            "velocityY": -0.3327439994563607,
-            "timestamp": 3.147620112799096
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0,
-            "velocityX": -1.5756247890906886e-35,
-            "velocityY": 0,
-            "timestamp": 3.2383635770814947
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S2.1.traj
deleted file mode 100644
index 30e62dac..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S2.1.traj	
+++ /dev/null
@@ -1,355 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3566378052654167,
-            "y": 4.392308940927803,
-            "heading": 2.7577639052389085,
-            "angularVelocity": 0.08777137611433758,
-            "velocityX": 0.6402935063310357,
-            "velocityY": 0.3652543557357081,
-            "timestamp": 0.08845600448138136
-        },
-        {
-            "x": 2.469918670668774,
-            "y": 4.456929940384967,
-            "heading": 2.772608563363178,
-            "angularVelocity": 0.16781967726556357,
-            "velocityX": 1.2806464192852178,
-            "velocityY": 0.7305439561286646,
-            "timestamp": 0.1769120089627627
-        },
-        {
-            "x": 2.639850302646433,
-            "y": 4.553867610725804,
-            "heading": 2.793388618859665,
-            "angularVelocity": 0.2349196712910238,
-            "velocityX": 1.92108645392669,
-            "velocityY": 1.0958856994410031,
-            "timestamp": 0.26536801344414407
-        },
-        {
-            "x": 2.8664440757683463,
-            "y": 4.683128905196596,
-            "heading": 2.8177875607011336,
-            "angularVelocity": 0.27583138063398066,
-            "velocityX": 2.561655078707626,
-            "velocityY": 1.4613060495853287,
-            "timestamp": 0.3538240179255254
-        },
-        {
-            "x": 3.1496996536335864,
-            "y": 4.844715100104656,
-            "heading": 2.8386513493911516,
-            "angularVelocity": 0.23586627965327242,
-            "velocityX": 3.2022199004575587,
-            "velocityY": 1.8267408284540865,
-            "timestamp": 0.4422800224069068
-        },
-        {
-            "x": 3.44035847389222,
-            "y": 5.010517813781029,
-            "heading": 2.83865135522876,
-            "angularVelocity": 6.599448670952606e-8,
-            "velocityX": 3.28591396324954,
-            "velocityY": 1.8744088052413612,
-            "timestamp": 0.5307360268882881
-        },
-        {
-            "x": 3.731017294885288,
-            "y": 5.17632052617049,
-            "heading": 2.8386513610659576,
-            "angularVelocity": 6.598983540705142e-8,
-            "velocityX": 3.285913971552369,
-            "velocityY": 1.8744087906927347,
-            "timestamp": 0.6191920313696695
-        },
-        {
-            "x": 4.021676115881456,
-            "y": 5.3421232385545165,
-            "heading": 2.8386513669031546,
-            "angularVelocity": 6.598983544244374e-8,
-            "velocityX": 3.285913971587413,
-            "velocityY": 1.8744087906313014,
-            "timestamp": 0.7076480358510508
-        },
-        {
-            "x": 4.31233493687768,
-            "y": 5.507925950938444,
-            "heading": 2.838651372740352,
-            "angularVelocity": 6.598983514854984e-8,
-            "velocityX": 3.2859139715880556,
-            "velocityY": 1.8744087906301747,
-            "timestamp": 0.7961040403324322
-        },
-        {
-            "x": 4.602993757873904,
-            "y": 5.673728663322372,
-            "heading": 2.8386513785775485,
-            "angularVelocity": 6.598983509017798e-8,
-            "velocityX": 3.2859139715880414,
-            "velocityY": 1.8744087906301996,
-            "timestamp": 0.8845600448138136
-        },
-        {
-            "x": 4.893652578870126,
-            "y": 5.839531375706303,
-            "heading": 2.8386513844147463,
-            "angularVelocity": 6.598983636221009e-8,
-            "velocityX": 3.2859139715880246,
-            "velocityY": 1.8744087906302287,
-            "timestamp": 0.9730160492951949
-        },
-        {
-            "x": 5.184311399866346,
-            "y": 6.005334088090239,
-            "heading": 2.8386513902519432,
-            "angularVelocity": 6.598983502831849e-8,
-            "velocityX": 3.285913971588007,
-            "velocityY": 1.8744087906302613,
-            "timestamp": 1.0614720537765763
-        },
-        {
-            "x": 5.474970220862562,
-            "y": 6.171136800474182,
-            "heading": 2.83865139608914,
-            "angularVelocity": 6.598983586934159e-8,
-            "velocityX": 3.2859139715879397,
-            "velocityY": 1.8744087906303781,
-            "timestamp": 1.1499280582579576
-        },
-        {
-            "x": 5.765629041858637,
-            "y": 6.336939512858371,
-            "heading": 2.8386514019263376,
-            "angularVelocity": 6.598983523621195e-8,
-            "velocityX": 3.2859139715863668,
-            "velocityY": 1.8744087906331355,
-            "timestamp": 1.238384062739339
-        },
-        {
-            "x": 6.0562878628501755,
-            "y": 6.502742225250514,
-            "heading": 2.8386514077635345,
-            "angularVelocity": 6.598983511287735e-8,
-            "velocityX": 3.285913971535071,
-            "velocityY": 1.87440879072306,
-            "timestamp": 1.3268400672207203
-        },
-        {
-            "x": 6.3469466835037505,
-            "y": 6.6685449382351205,
-            "heading": 2.8386514136007315,
-            "angularVelocity": 6.598983592798126e-8,
-            "velocityX": 3.2859139677143814,
-            "velocityY": 1.8744087974208865,
-            "timestamp": 1.4152960717021017
-        },
-        {
-            "x": 6.6376054603552355,
-            "y": 6.834347728006541,
-            "heading": 2.838651419437929,
-            "angularVelocity": 6.598983535546771e-8,
-            "velocityX": 3.2859134725293426,
-            "velocityY": 1.8744096655000533,
-            "timestamp": 1.503752076183483
-        },
-        {
-            "x": 6.928282728448998,
-            "y": 7.000118097747795,
-            "heading": 2.8386514252461743,
-            "angularVelocity": 6.56625327771565e-8,
-            "velocityX": 3.286122517041433,
-            "velocityY": 1.8740431552739683,
-            "timestamp": 1.5922080806648644
-        },
-        {
-            "x": 7.191968889959837,
-            "y": 7.158086456014882,
-            "heading": 2.9053215778244037,
-            "angularVelocity": 0.7537097449644049,
-            "velocityX": 2.980986571311004,
-            "velocityY": 1.7858409860730329,
-            "timestamp": 1.6806640851462458
-        },
-        {
-            "x": 7.39896469669124,
-            "y": 7.2837659035610995,
-            "heading": 2.9729435938200757,
-            "angularVelocity": 0.7644706132968896,
-            "velocityX": 2.3400989898312643,
-            "velocityY": 1.4208130729292534,
-            "timestamp": 1.7691200896276271
-        },
-        {
-            "x": 7.549292841201835,
-            "y": 7.377144263475716,
-            "heading": 3.03570050164413,
-            "angularVelocity": 0.7094702976016349,
-            "velocityX": 1.6994679489759839,
-            "velocityY": 1.0556474991393678,
-            "timestamp": 1.8575760941090085
-        },
-        {
-            "x": 7.642968334049115,
-            "y": 7.4382219539742405,
-            "heading": 3.0916990951391803,
-            "angularVelocity": 0.6330671820796432,
-            "velocityX": 1.0590066032994798,
-            "velocityY": 0.6904866532988578,
-            "timestamp": 1.9460320985903898
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 0.546044388326283,
-            "velocityX": 0.41864502209875154,
-            "velocityY": 0.32533739449908106,
-            "timestamp": 2.034488103071771
-        },
-        {
-            "x": 7.645871150286324,
-            "y": 7.456493783242831,
-            "heading": 3.185181806591133,
-            "angularVelocity": 0.43686385232963987,
-            "velocityX": -0.329992576357443,
-            "velocityY": -0.10158483407911743,
-            "timestamp": 2.1379111863737847
-        },
-        {
-            "x": 7.534332014927772,
-            "y": 7.401805741681953,
-            "heading": 3.219069089269022,
-            "angularVelocity": 0.32765685953225704,
-            "velocityX": -1.078474280570675,
-            "velocityY": -0.5287798411617203,
-            "timestamp": 2.2413342696757983
-        },
-        {
-            "x": 7.345414230675068,
-            "y": 7.30288053546568,
-            "heading": 3.2416598317701943,
-            "angularVelocity": 0.21843037144032681,
-            "velocityX": -1.826650088365854,
-            "velocityY": -0.9565099304515025,
-            "timestamp": 2.344757352977812
-        },
-        {
-            "x": 7.079208201344681,
-            "y": 7.159560464323527,
-            "heading": 3.2529526054228244,
-            "angularVelocity": 0.1091900694900192,
-            "velocityX": -2.573951779729956,
-            "velocityY": -1.3857648270226828,
-            "timestamp": 2.4481804362798254
-        },
-        {
-            "x": 6.738167350146867,
-            "y": 6.967824923059757,
-            "heading": 3.25295260062049,
-            "angularVelocity": -4.6433873545046094e-8,
-            "velocityX": -3.29753126970623,
-            "velocityY": -1.853895041050502,
-            "timestamp": 2.551603519581839
-        },
-        {
-            "x": 6.398085530846708,
-            "y": 6.774393439892416,
-            "heading": 3.252952596202531,
-            "angularVelocity": -4.2717337144488564e-8,
-            "velocityX": -3.2882583698173717,
-            "velocityY": -1.8702931395158464,
-            "timestamp": 2.6550266028838525
-        },
-        {
-            "x": 6.058003908621006,
-            "y": 6.580961610238813,
-            "heading": 3.252952591784573,
-            "angularVelocity": -4.2717336082734796e-8,
-            "velocityX": -3.288256464300223,
-            "velocityY": -1.8702964896989283,
-            "timestamp": 2.758449686185866
-        },
-        {
-            "x": 5.717922287179122,
-            "y": 6.387529779207143,
-            "heading": 3.252952587366614,
-            "angularVelocity": -4.271733673837129e-8,
-            "velocityX": -3.28825645672147,
-            "velocityY": -1.8702965030234862,
-            "timestamp": 2.8618727694878796
-        },
-        {
-            "x": 5.377840665722721,
-            "y": 6.194097948200997,
-            "heading": 3.2529525829486556,
-            "angularVelocity": -4.271733567219934e-8,
-            "velocityX": -3.2882564568618378,
-            "velocityY": -1.8702965027766985,
-            "timestamp": 2.965295852789893
-        },
-        {
-            "x": 5.037759049752838,
-            "y": 6.000666107536036,
-            "heading": 3.252952578536604,
-            "angularVelocity": -4.266021621617093e-8,
-            "velocityX": -3.288256403812571,
-            "velocityY": -1.8702965961679845,
-            "timestamp": 3.0687189360919067
-        },
-        {
-            "x": 4.7278555042556665,
-            "y": 5.824399716323421,
-            "heading": 3.2077639144902763,
-            "angularVelocity": -0.4369301572101497,
-            "velocityX": -2.996463996264777,
-            "velocityY": -1.7043234990188791,
-            "timestamp": 3.1721420193939203
-        },
-        {
-            "x": 4.495427770658729,
-            "y": 5.69219987124691,
-            "heading": 3.1738800653247865,
-            "angularVelocity": -0.32762366082757177,
-            "velocityX": -2.2473487172898445,
-            "velocityY": -1.278243123833972,
-            "timestamp": 3.275565102695934
-        },
-        {
-            "x": 4.340475927186926,
-            "y": 5.604066625906228,
-            "heading": 3.1512930339800644,
-            "angularVelocity": -0.2183944881894502,
-            "velocityX": -1.4982326819566802,
-            "velocityY": -0.8521622303921732,
-            "timestamp": 3.3789881859979474
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -0.10919258660163363,
-            "velocityX": -0.7491163936844148,
-            "velocityY": -0.4260811464839441,
-            "timestamp": 3.482411269299961
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 8.84233842596168e-24,
-            "velocityY": 0,
-            "timestamp": 3.5858343526019745
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S2.traj
deleted file mode 100644
index 30e62dac..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1 To S2.traj	
+++ /dev/null
@@ -1,355 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3566378052654167,
-            "y": 4.392308940927803,
-            "heading": 2.7577639052389085,
-            "angularVelocity": 0.08777137611433758,
-            "velocityX": 0.6402935063310357,
-            "velocityY": 0.3652543557357081,
-            "timestamp": 0.08845600448138136
-        },
-        {
-            "x": 2.469918670668774,
-            "y": 4.456929940384967,
-            "heading": 2.772608563363178,
-            "angularVelocity": 0.16781967726556357,
-            "velocityX": 1.2806464192852178,
-            "velocityY": 0.7305439561286646,
-            "timestamp": 0.1769120089627627
-        },
-        {
-            "x": 2.639850302646433,
-            "y": 4.553867610725804,
-            "heading": 2.793388618859665,
-            "angularVelocity": 0.2349196712910238,
-            "velocityX": 1.92108645392669,
-            "velocityY": 1.0958856994410031,
-            "timestamp": 0.26536801344414407
-        },
-        {
-            "x": 2.8664440757683463,
-            "y": 4.683128905196596,
-            "heading": 2.8177875607011336,
-            "angularVelocity": 0.27583138063398066,
-            "velocityX": 2.561655078707626,
-            "velocityY": 1.4613060495853287,
-            "timestamp": 0.3538240179255254
-        },
-        {
-            "x": 3.1496996536335864,
-            "y": 4.844715100104656,
-            "heading": 2.8386513493911516,
-            "angularVelocity": 0.23586627965327242,
-            "velocityX": 3.2022199004575587,
-            "velocityY": 1.8267408284540865,
-            "timestamp": 0.4422800224069068
-        },
-        {
-            "x": 3.44035847389222,
-            "y": 5.010517813781029,
-            "heading": 2.83865135522876,
-            "angularVelocity": 6.599448670952606e-8,
-            "velocityX": 3.28591396324954,
-            "velocityY": 1.8744088052413612,
-            "timestamp": 0.5307360268882881
-        },
-        {
-            "x": 3.731017294885288,
-            "y": 5.17632052617049,
-            "heading": 2.8386513610659576,
-            "angularVelocity": 6.598983540705142e-8,
-            "velocityX": 3.285913971552369,
-            "velocityY": 1.8744087906927347,
-            "timestamp": 0.6191920313696695
-        },
-        {
-            "x": 4.021676115881456,
-            "y": 5.3421232385545165,
-            "heading": 2.8386513669031546,
-            "angularVelocity": 6.598983544244374e-8,
-            "velocityX": 3.285913971587413,
-            "velocityY": 1.8744087906313014,
-            "timestamp": 0.7076480358510508
-        },
-        {
-            "x": 4.31233493687768,
-            "y": 5.507925950938444,
-            "heading": 2.838651372740352,
-            "angularVelocity": 6.598983514854984e-8,
-            "velocityX": 3.2859139715880556,
-            "velocityY": 1.8744087906301747,
-            "timestamp": 0.7961040403324322
-        },
-        {
-            "x": 4.602993757873904,
-            "y": 5.673728663322372,
-            "heading": 2.8386513785775485,
-            "angularVelocity": 6.598983509017798e-8,
-            "velocityX": 3.2859139715880414,
-            "velocityY": 1.8744087906301996,
-            "timestamp": 0.8845600448138136
-        },
-        {
-            "x": 4.893652578870126,
-            "y": 5.839531375706303,
-            "heading": 2.8386513844147463,
-            "angularVelocity": 6.598983636221009e-8,
-            "velocityX": 3.2859139715880246,
-            "velocityY": 1.8744087906302287,
-            "timestamp": 0.9730160492951949
-        },
-        {
-            "x": 5.184311399866346,
-            "y": 6.005334088090239,
-            "heading": 2.8386513902519432,
-            "angularVelocity": 6.598983502831849e-8,
-            "velocityX": 3.285913971588007,
-            "velocityY": 1.8744087906302613,
-            "timestamp": 1.0614720537765763
-        },
-        {
-            "x": 5.474970220862562,
-            "y": 6.171136800474182,
-            "heading": 2.83865139608914,
-            "angularVelocity": 6.598983586934159e-8,
-            "velocityX": 3.2859139715879397,
-            "velocityY": 1.8744087906303781,
-            "timestamp": 1.1499280582579576
-        },
-        {
-            "x": 5.765629041858637,
-            "y": 6.336939512858371,
-            "heading": 2.8386514019263376,
-            "angularVelocity": 6.598983523621195e-8,
-            "velocityX": 3.2859139715863668,
-            "velocityY": 1.8744087906331355,
-            "timestamp": 1.238384062739339
-        },
-        {
-            "x": 6.0562878628501755,
-            "y": 6.502742225250514,
-            "heading": 2.8386514077635345,
-            "angularVelocity": 6.598983511287735e-8,
-            "velocityX": 3.285913971535071,
-            "velocityY": 1.87440879072306,
-            "timestamp": 1.3268400672207203
-        },
-        {
-            "x": 6.3469466835037505,
-            "y": 6.6685449382351205,
-            "heading": 2.8386514136007315,
-            "angularVelocity": 6.598983592798126e-8,
-            "velocityX": 3.2859139677143814,
-            "velocityY": 1.8744087974208865,
-            "timestamp": 1.4152960717021017
-        },
-        {
-            "x": 6.6376054603552355,
-            "y": 6.834347728006541,
-            "heading": 2.838651419437929,
-            "angularVelocity": 6.598983535546771e-8,
-            "velocityX": 3.2859134725293426,
-            "velocityY": 1.8744096655000533,
-            "timestamp": 1.503752076183483
-        },
-        {
-            "x": 6.928282728448998,
-            "y": 7.000118097747795,
-            "heading": 2.8386514252461743,
-            "angularVelocity": 6.56625327771565e-8,
-            "velocityX": 3.286122517041433,
-            "velocityY": 1.8740431552739683,
-            "timestamp": 1.5922080806648644
-        },
-        {
-            "x": 7.191968889959837,
-            "y": 7.158086456014882,
-            "heading": 2.9053215778244037,
-            "angularVelocity": 0.7537097449644049,
-            "velocityX": 2.980986571311004,
-            "velocityY": 1.7858409860730329,
-            "timestamp": 1.6806640851462458
-        },
-        {
-            "x": 7.39896469669124,
-            "y": 7.2837659035610995,
-            "heading": 2.9729435938200757,
-            "angularVelocity": 0.7644706132968896,
-            "velocityX": 2.3400989898312643,
-            "velocityY": 1.4208130729292534,
-            "timestamp": 1.7691200896276271
-        },
-        {
-            "x": 7.549292841201835,
-            "y": 7.377144263475716,
-            "heading": 3.03570050164413,
-            "angularVelocity": 0.7094702976016349,
-            "velocityX": 1.6994679489759839,
-            "velocityY": 1.0556474991393678,
-            "timestamp": 1.8575760941090085
-        },
-        {
-            "x": 7.642968334049115,
-            "y": 7.4382219539742405,
-            "heading": 3.0916990951391803,
-            "angularVelocity": 0.6330671820796432,
-            "velocityX": 1.0590066032994798,
-            "velocityY": 0.6904866532988578,
-            "timestamp": 1.9460320985903898
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 0.546044388326283,
-            "velocityX": 0.41864502209875154,
-            "velocityY": 0.32533739449908106,
-            "timestamp": 2.034488103071771
-        },
-        {
-            "x": 7.645871150286324,
-            "y": 7.456493783242831,
-            "heading": 3.185181806591133,
-            "angularVelocity": 0.43686385232963987,
-            "velocityX": -0.329992576357443,
-            "velocityY": -0.10158483407911743,
-            "timestamp": 2.1379111863737847
-        },
-        {
-            "x": 7.534332014927772,
-            "y": 7.401805741681953,
-            "heading": 3.219069089269022,
-            "angularVelocity": 0.32765685953225704,
-            "velocityX": -1.078474280570675,
-            "velocityY": -0.5287798411617203,
-            "timestamp": 2.2413342696757983
-        },
-        {
-            "x": 7.345414230675068,
-            "y": 7.30288053546568,
-            "heading": 3.2416598317701943,
-            "angularVelocity": 0.21843037144032681,
-            "velocityX": -1.826650088365854,
-            "velocityY": -0.9565099304515025,
-            "timestamp": 2.344757352977812
-        },
-        {
-            "x": 7.079208201344681,
-            "y": 7.159560464323527,
-            "heading": 3.2529526054228244,
-            "angularVelocity": 0.1091900694900192,
-            "velocityX": -2.573951779729956,
-            "velocityY": -1.3857648270226828,
-            "timestamp": 2.4481804362798254
-        },
-        {
-            "x": 6.738167350146867,
-            "y": 6.967824923059757,
-            "heading": 3.25295260062049,
-            "angularVelocity": -4.6433873545046094e-8,
-            "velocityX": -3.29753126970623,
-            "velocityY": -1.853895041050502,
-            "timestamp": 2.551603519581839
-        },
-        {
-            "x": 6.398085530846708,
-            "y": 6.774393439892416,
-            "heading": 3.252952596202531,
-            "angularVelocity": -4.2717337144488564e-8,
-            "velocityX": -3.2882583698173717,
-            "velocityY": -1.8702931395158464,
-            "timestamp": 2.6550266028838525
-        },
-        {
-            "x": 6.058003908621006,
-            "y": 6.580961610238813,
-            "heading": 3.252952591784573,
-            "angularVelocity": -4.2717336082734796e-8,
-            "velocityX": -3.288256464300223,
-            "velocityY": -1.8702964896989283,
-            "timestamp": 2.758449686185866
-        },
-        {
-            "x": 5.717922287179122,
-            "y": 6.387529779207143,
-            "heading": 3.252952587366614,
-            "angularVelocity": -4.271733673837129e-8,
-            "velocityX": -3.28825645672147,
-            "velocityY": -1.8702965030234862,
-            "timestamp": 2.8618727694878796
-        },
-        {
-            "x": 5.377840665722721,
-            "y": 6.194097948200997,
-            "heading": 3.2529525829486556,
-            "angularVelocity": -4.271733567219934e-8,
-            "velocityX": -3.2882564568618378,
-            "velocityY": -1.8702965027766985,
-            "timestamp": 2.965295852789893
-        },
-        {
-            "x": 5.037759049752838,
-            "y": 6.000666107536036,
-            "heading": 3.252952578536604,
-            "angularVelocity": -4.266021621617093e-8,
-            "velocityX": -3.288256403812571,
-            "velocityY": -1.8702965961679845,
-            "timestamp": 3.0687189360919067
-        },
-        {
-            "x": 4.7278555042556665,
-            "y": 5.824399716323421,
-            "heading": 3.2077639144902763,
-            "angularVelocity": -0.4369301572101497,
-            "velocityX": -2.996463996264777,
-            "velocityY": -1.7043234990188791,
-            "timestamp": 3.1721420193939203
-        },
-        {
-            "x": 4.495427770658729,
-            "y": 5.69219987124691,
-            "heading": 3.1738800653247865,
-            "angularVelocity": -0.32762366082757177,
-            "velocityX": -2.2473487172898445,
-            "velocityY": -1.278243123833972,
-            "timestamp": 3.275565102695934
-        },
-        {
-            "x": 4.340475927186926,
-            "y": 5.604066625906228,
-            "heading": 3.1512930339800644,
-            "angularVelocity": -0.2183944881894502,
-            "velocityX": -1.4982326819566802,
-            "velocityY": -0.8521622303921732,
-            "timestamp": 3.3789881859979474
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -0.10919258660163363,
-            "velocityX": -0.7491163936844148,
-            "velocityY": -0.4260811464839441,
-            "timestamp": 3.482411269299961
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 8.84233842596168e-24,
-            "velocityY": 0,
-            "timestamp": 3.5858343526019745
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S1.1.traj
deleted file mode 100644
index a687772f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S1.1.traj	
+++ /dev/null
@@ -1,328 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3590330484085675,
-            "y": 4.366649962138697,
-            "heading": 2.7576232354660704,
-            "angularVelocity": 0.09028523567993728,
-            "velocityX": 0.6991536221325528,
-            "velocityY": 0.07875834370159086,
-            "timestamp": 0.08443501762674911
-        },
-        {
-            "x": 2.4770991403474683,
-            "y": 4.379949883012471,
-            "heading": 2.7728702101731937,
-            "angularVelocity": 0.18057643778228707,
-            "velocityX": 1.3983071864900902,
-            "velocityY": 0.15751664709264293,
-            "timestamp": 0.16887003525349822
-        },
-        {
-            "x": 2.6541982667626414,
-            "y": 4.399899758130858,
-            "heading": 2.7957418198907438,
-            "angularVelocity": 0.27087824886418327,
-            "velocityX": 2.097460643616519,
-            "velocityY": 0.23627489730122597,
-            "timestamp": 0.25330505288024735
-        },
-        {
-            "x": 2.8903304124163527,
-            "y": 4.426499582699937,
-            "heading": 2.8262393577196585,
-            "angularVelocity": 0.361195374693129,
-            "velocityX": 2.796613920275944,
-            "velocityY": 0.31503309073333907,
-            "timestamp": 0.33774007050699645
-        },
-        {
-            "x": 3.1854955469851514,
-            "y": 4.459749351941674,
-            "heading": 2.864364443644879,
-            "angularVelocity": 0.4515316866960985,
-            "velocityX": 3.4957668378017823,
-            "velocityY": 0.3937912275771662,
-            "timestamp": 0.42217508813374555
-        },
-        {
-            "x": 3.5029006029698784,
-            "y": 4.495505051079763,
-            "heading": 2.8643644482492165,
-            "angularVelocity": 5.453113642276571e-8,
-            "velocityX": 3.7591637321358573,
-            "velocityY": 0.4234700263361048,
-            "timestamp": 0.5066101057604947
-        },
-        {
-            "x": 3.820305658948058,
-            "y": 4.531260750276139,
-            "heading": 2.864364452853533,
-            "angularVelocity": 5.453089037976801e-8,
-            "velocityX": 3.759163732058311,
-            "velocityY": 0.4234700270264227,
-            "timestamp": 0.5910451233872438
-        },
-        {
-            "x": 4.1377107149262375,
-            "y": 4.567016449472515,
-            "heading": 2.8643644574578495,
-            "angularVelocity": 5.4530890155242205e-8,
-            "velocityX": 3.75916373205831,
-            "velocityY": 0.4234700270264262,
-            "timestamp": 0.6754801410139929
-        },
-        {
-            "x": 4.455115770904417,
-            "y": 4.602772148668891,
-            "heading": 2.864364462062166,
-            "angularVelocity": 5.453089038095968e-8,
-            "velocityX": 3.7591637320583104,
-            "velocityY": 0.4234700270264258,
-            "timestamp": 0.759915158640742
-        },
-        {
-            "x": 4.772520826882595,
-            "y": 4.638527847865267,
-            "heading": 2.864364466666483,
-            "angularVelocity": 5.4530891020586436e-8,
-            "velocityX": 3.7591637320583104,
-            "velocityY": 0.4234700270264263,
-            "timestamp": 0.8443501762674911
-        },
-        {
-            "x": 5.089925882860775,
-            "y": 4.674283547061643,
-            "heading": 2.8643644712708,
-            "angularVelocity": 5.4530890812107064e-8,
-            "velocityX": 3.75916373205831,
-            "velocityY": 0.4234700270264263,
-            "timestamp": 0.9287851938942402
-        },
-        {
-            "x": 5.407330938838954,
-            "y": 4.7100392462580185,
-            "heading": 2.8643644758751163,
-            "angularVelocity": 5.453088997566471e-8,
-            "velocityX": 3.7591637320583104,
-            "velocityY": 0.4234700270264265,
-            "timestamp": 1.0132202115209894
-        },
-        {
-            "x": 5.7247359948171335,
-            "y": 4.745794945454395,
-            "heading": 2.864364480479433,
-            "angularVelocity": 5.453089022113266e-8,
-            "velocityX": 3.7591637320583104,
-            "velocityY": 0.42347002702642605,
-            "timestamp": 1.0976552291477386
-        },
-        {
-            "x": 6.04214105079524,
-            "y": 4.7815506446514116,
-            "heading": 2.8643644850837493,
-            "angularVelocity": 5.453089025367266e-8,
-            "velocityX": 3.759163732057455,
-            "velocityY": 0.42347002703401615,
-            "timestamp": 1.1820902467744878
-        },
-        {
-            "x": 6.359546093014576,
-            "y": 4.817306465985485,
-            "heading": 2.8643644896880662,
-            "angularVelocity": 5.4530891813953255e-8,
-            "velocityX": 3.7591635691064473,
-            "velocityY": 0.42347147355537545,
-            "timestamp": 1.266525264401237
-        },
-        {
-            "x": 6.6740817237038605,
-            "y": 4.872910210638354,
-            "heading": 2.8643644943647257,
-            "angularVelocity": 5.53876754719752e-8,
-            "velocityX": 3.7251799020130547,
-            "velocityY": 0.6585389121213792,
-            "timestamp": 1.3509602820279862
-        },
-        {
-            "x": 6.972867149310035,
-            "y": 4.985833654432599,
-            "heading": 2.8643645123115293,
-            "angularVelocity": 2.1255166363796299e-7,
-            "velocityX": 3.5386434918149345,
-            "velocityY": 1.337400606623087,
-            "timestamp": 1.4353952996547354
-        },
-        {
-            "x": 7.232618211035156,
-            "y": 5.1421799444134875,
-            "heading": 2.9041055246483745,
-            "angularVelocity": 0.4706697938114134,
-            "velocityX": 3.0763428376762785,
-            "velocityY": 1.8516759322776348,
-            "timestamp": 1.5198303172814847
-        },
-        {
-            "x": 7.433588885971066,
-            "y": 5.306272472318755,
-            "heading": 2.9592505290938482,
-            "angularVelocity": 0.6531058557865896,
-            "velocityX": 2.3801815950855185,
-            "velocityY": 1.9434179386406838,
-            "timestamp": 1.6042653349082339
-        },
-        {
-            "x": 7.575142359826725,
-            "y": 5.471558749180071,
-            "heading": 3.0192595634287165,
-            "angularVelocity": 0.7107126405793248,
-            "velocityX": 1.676478288681205,
-            "velocityY": 1.9575560177173887,
-            "timestamp": 1.688700352534983
-        },
-        {
-            "x": 7.657265359417412,
-            "y": 5.635564900043681,
-            "heading": 3.0801944156150345,
-            "angularVelocity": 0.7216774970745534,
-            "velocityX": 0.9726177822774612,
-            "velocityY": 1.9423949384201076,
-            "timestamp": 1.7731353701617323
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 0.7083030958711996,
-            "velocityX": 0.26925606486028053,
-            "velocityY": 1.9119448836968793,
-            "timestamp": 1.8575703877884815
-        },
-        {
-            "x": 7.638446013209911,
-            "y": 5.962794429881767,
-            "heading": 3.200158410841702,
-            "angularVelocity": 0.6785760380077615,
-            "velocityX": -0.46872148590566404,
-            "velocityY": 1.870131304647713,
-            "timestamp": 1.9462242857633076
-        },
-        {
-            "x": 7.531523237847712,
-            "y": 6.124072395344993,
-            "heading": 3.2564305280159678,
-            "angularVelocity": 0.6347393454740654,
-            "velocityX": -1.206069646171223,
-            "velocityY": 1.8191863995537148,
-            "timestamp": 2.034878183738134
-        },
-        {
-            "x": 7.359349891233686,
-            "y": 6.279498639853745,
-            "heading": 3.30674894531401,
-            "angularVelocity": 0.5675826833054816,
-            "velocityX": -1.9420843363584046,
-            "velocityY": 1.7531800412530831,
-            "timestamp": 2.1235320817129604
-        },
-        {
-            "x": 7.122252404274228,
-            "y": 6.426460665486359,
-            "heading": 3.3470467672011193,
-            "angularVelocity": 0.45455217207203097,
-            "velocityX": -2.6744169447211816,
-            "velocityY": 1.6577051769832414,
-            "timestamp": 2.2121859796877867
-        },
-        {
-            "x": 6.821828497038046,
-            "y": 6.5576759176562405,
-            "heading": 3.365711055371459,
-            "angularVelocity": 0.2105298085780703,
-            "velocityX": -3.388727558504971,
-            "velocityY": 1.4800844087773837,
-            "timestamp": 2.300839877662613
-        },
-        {
-            "x": 6.494234201783015,
-            "y": 6.629485644322343,
-            "heading": 3.365711060189302,
-            "angularVelocity": 5.434439690558349e-8,
-            "velocityX": -3.6952046411772423,
-            "velocityY": 0.8100007817647606,
-            "timestamp": 2.3894937756374395
-        },
-        {
-            "x": 6.158929367283791,
-            "y": 6.636218351220065,
-            "heading": 3.3657110616036774,
-            "angularVelocity": 1.5953902696988204e-8,
-            "velocityX": -3.782178134958452,
-            "velocityY": 0.07594372104916906,
-            "timestamp": 2.478147673612266
-        },
-        {
-            "x": 5.8312862675507695,
-            "y": 6.634145572711449,
-            "heading": 3.3782020006814046,
-            "angularVelocity": 0.14089554281384958,
-            "velocityX": -3.6957551468978513,
-            "velocityY": -0.023380568209240477,
-            "timestamp": 2.566801571587092
-        },
-        {
-            "x": 5.569171767766223,
-            "y": 6.6324873452973785,
-            "heading": 3.388195171082853,
-            "angularVelocity": 0.11272116206650896,
-            "velocityX": -2.9566043430935838,
-            "velocityY": -0.018704506535536753,
-            "timestamp": 2.6554554695619186
-        },
-        {
-            "x": 5.372585886333595,
-            "y": 6.631243673213895,
-            "heading": 3.395690186633039,
-            "angularVelocity": 0.08454242533492021,
-            "velocityX": -2.2174533317017797,
-            "velocityY": -0.014028397080035612,
-            "timestamp": 2.744109367536745
-        },
-        {
-            "x": 5.241528629389324,
-            "y": 6.630414557878557,
-            "heading": 3.4006869200557808,
-            "angularVelocity": 0.05636225295091492,
-            "velocityX": -1.4783022510920478,
-            "velocityY": -0.00935227163472713,
-            "timestamp": 2.8327632655115713
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.02818135672404778,
-            "velocityX": -0.7391511358917447,
-            "velocityY": -0.00467613819613763,
-            "timestamp": 2.9214171634863977
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0,
-            "velocityX": -2.6920195969080552e-36,
-            "velocityY": 0,
-            "timestamp": 3.010071061461224
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S1.traj
deleted file mode 100644
index a687772f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S1.traj	
+++ /dev/null
@@ -1,328 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3590330484085675,
-            "y": 4.366649962138697,
-            "heading": 2.7576232354660704,
-            "angularVelocity": 0.09028523567993728,
-            "velocityX": 0.6991536221325528,
-            "velocityY": 0.07875834370159086,
-            "timestamp": 0.08443501762674911
-        },
-        {
-            "x": 2.4770991403474683,
-            "y": 4.379949883012471,
-            "heading": 2.7728702101731937,
-            "angularVelocity": 0.18057643778228707,
-            "velocityX": 1.3983071864900902,
-            "velocityY": 0.15751664709264293,
-            "timestamp": 0.16887003525349822
-        },
-        {
-            "x": 2.6541982667626414,
-            "y": 4.399899758130858,
-            "heading": 2.7957418198907438,
-            "angularVelocity": 0.27087824886418327,
-            "velocityX": 2.097460643616519,
-            "velocityY": 0.23627489730122597,
-            "timestamp": 0.25330505288024735
-        },
-        {
-            "x": 2.8903304124163527,
-            "y": 4.426499582699937,
-            "heading": 2.8262393577196585,
-            "angularVelocity": 0.361195374693129,
-            "velocityX": 2.796613920275944,
-            "velocityY": 0.31503309073333907,
-            "timestamp": 0.33774007050699645
-        },
-        {
-            "x": 3.1854955469851514,
-            "y": 4.459749351941674,
-            "heading": 2.864364443644879,
-            "angularVelocity": 0.4515316866960985,
-            "velocityX": 3.4957668378017823,
-            "velocityY": 0.3937912275771662,
-            "timestamp": 0.42217508813374555
-        },
-        {
-            "x": 3.5029006029698784,
-            "y": 4.495505051079763,
-            "heading": 2.8643644482492165,
-            "angularVelocity": 5.453113642276571e-8,
-            "velocityX": 3.7591637321358573,
-            "velocityY": 0.4234700263361048,
-            "timestamp": 0.5066101057604947
-        },
-        {
-            "x": 3.820305658948058,
-            "y": 4.531260750276139,
-            "heading": 2.864364452853533,
-            "angularVelocity": 5.453089037976801e-8,
-            "velocityX": 3.759163732058311,
-            "velocityY": 0.4234700270264227,
-            "timestamp": 0.5910451233872438
-        },
-        {
-            "x": 4.1377107149262375,
-            "y": 4.567016449472515,
-            "heading": 2.8643644574578495,
-            "angularVelocity": 5.4530890155242205e-8,
-            "velocityX": 3.75916373205831,
-            "velocityY": 0.4234700270264262,
-            "timestamp": 0.6754801410139929
-        },
-        {
-            "x": 4.455115770904417,
-            "y": 4.602772148668891,
-            "heading": 2.864364462062166,
-            "angularVelocity": 5.453089038095968e-8,
-            "velocityX": 3.7591637320583104,
-            "velocityY": 0.4234700270264258,
-            "timestamp": 0.759915158640742
-        },
-        {
-            "x": 4.772520826882595,
-            "y": 4.638527847865267,
-            "heading": 2.864364466666483,
-            "angularVelocity": 5.4530891020586436e-8,
-            "velocityX": 3.7591637320583104,
-            "velocityY": 0.4234700270264263,
-            "timestamp": 0.8443501762674911
-        },
-        {
-            "x": 5.089925882860775,
-            "y": 4.674283547061643,
-            "heading": 2.8643644712708,
-            "angularVelocity": 5.4530890812107064e-8,
-            "velocityX": 3.75916373205831,
-            "velocityY": 0.4234700270264263,
-            "timestamp": 0.9287851938942402
-        },
-        {
-            "x": 5.407330938838954,
-            "y": 4.7100392462580185,
-            "heading": 2.8643644758751163,
-            "angularVelocity": 5.453088997566471e-8,
-            "velocityX": 3.7591637320583104,
-            "velocityY": 0.4234700270264265,
-            "timestamp": 1.0132202115209894
-        },
-        {
-            "x": 5.7247359948171335,
-            "y": 4.745794945454395,
-            "heading": 2.864364480479433,
-            "angularVelocity": 5.453089022113266e-8,
-            "velocityX": 3.7591637320583104,
-            "velocityY": 0.42347002702642605,
-            "timestamp": 1.0976552291477386
-        },
-        {
-            "x": 6.04214105079524,
-            "y": 4.7815506446514116,
-            "heading": 2.8643644850837493,
-            "angularVelocity": 5.453089025367266e-8,
-            "velocityX": 3.759163732057455,
-            "velocityY": 0.42347002703401615,
-            "timestamp": 1.1820902467744878
-        },
-        {
-            "x": 6.359546093014576,
-            "y": 4.817306465985485,
-            "heading": 2.8643644896880662,
-            "angularVelocity": 5.4530891813953255e-8,
-            "velocityX": 3.7591635691064473,
-            "velocityY": 0.42347147355537545,
-            "timestamp": 1.266525264401237
-        },
-        {
-            "x": 6.6740817237038605,
-            "y": 4.872910210638354,
-            "heading": 2.8643644943647257,
-            "angularVelocity": 5.53876754719752e-8,
-            "velocityX": 3.7251799020130547,
-            "velocityY": 0.6585389121213792,
-            "timestamp": 1.3509602820279862
-        },
-        {
-            "x": 6.972867149310035,
-            "y": 4.985833654432599,
-            "heading": 2.8643645123115293,
-            "angularVelocity": 2.1255166363796299e-7,
-            "velocityX": 3.5386434918149345,
-            "velocityY": 1.337400606623087,
-            "timestamp": 1.4353952996547354
-        },
-        {
-            "x": 7.232618211035156,
-            "y": 5.1421799444134875,
-            "heading": 2.9041055246483745,
-            "angularVelocity": 0.4706697938114134,
-            "velocityX": 3.0763428376762785,
-            "velocityY": 1.8516759322776348,
-            "timestamp": 1.5198303172814847
-        },
-        {
-            "x": 7.433588885971066,
-            "y": 5.306272472318755,
-            "heading": 2.9592505290938482,
-            "angularVelocity": 0.6531058557865896,
-            "velocityX": 2.3801815950855185,
-            "velocityY": 1.9434179386406838,
-            "timestamp": 1.6042653349082339
-        },
-        {
-            "x": 7.575142359826725,
-            "y": 5.471558749180071,
-            "heading": 3.0192595634287165,
-            "angularVelocity": 0.7107126405793248,
-            "velocityX": 1.676478288681205,
-            "velocityY": 1.9575560177173887,
-            "timestamp": 1.688700352534983
-        },
-        {
-            "x": 7.657265359417412,
-            "y": 5.635564900043681,
-            "heading": 3.0801944156150345,
-            "angularVelocity": 0.7216774970745534,
-            "velocityX": 0.9726177822774612,
-            "velocityY": 1.9423949384201076,
-            "timestamp": 1.7731353701617323
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 0.7083030958711996,
-            "velocityX": 0.26925606486028053,
-            "velocityY": 1.9119448836968793,
-            "timestamp": 1.8575703877884815
-        },
-        {
-            "x": 7.638446013209911,
-            "y": 5.962794429881767,
-            "heading": 3.200158410841702,
-            "angularVelocity": 0.6785760380077615,
-            "velocityX": -0.46872148590566404,
-            "velocityY": 1.870131304647713,
-            "timestamp": 1.9462242857633076
-        },
-        {
-            "x": 7.531523237847712,
-            "y": 6.124072395344993,
-            "heading": 3.2564305280159678,
-            "angularVelocity": 0.6347393454740654,
-            "velocityX": -1.206069646171223,
-            "velocityY": 1.8191863995537148,
-            "timestamp": 2.034878183738134
-        },
-        {
-            "x": 7.359349891233686,
-            "y": 6.279498639853745,
-            "heading": 3.30674894531401,
-            "angularVelocity": 0.5675826833054816,
-            "velocityX": -1.9420843363584046,
-            "velocityY": 1.7531800412530831,
-            "timestamp": 2.1235320817129604
-        },
-        {
-            "x": 7.122252404274228,
-            "y": 6.426460665486359,
-            "heading": 3.3470467672011193,
-            "angularVelocity": 0.45455217207203097,
-            "velocityX": -2.6744169447211816,
-            "velocityY": 1.6577051769832414,
-            "timestamp": 2.2121859796877867
-        },
-        {
-            "x": 6.821828497038046,
-            "y": 6.5576759176562405,
-            "heading": 3.365711055371459,
-            "angularVelocity": 0.2105298085780703,
-            "velocityX": -3.388727558504971,
-            "velocityY": 1.4800844087773837,
-            "timestamp": 2.300839877662613
-        },
-        {
-            "x": 6.494234201783015,
-            "y": 6.629485644322343,
-            "heading": 3.365711060189302,
-            "angularVelocity": 5.434439690558349e-8,
-            "velocityX": -3.6952046411772423,
-            "velocityY": 0.8100007817647606,
-            "timestamp": 2.3894937756374395
-        },
-        {
-            "x": 6.158929367283791,
-            "y": 6.636218351220065,
-            "heading": 3.3657110616036774,
-            "angularVelocity": 1.5953902696988204e-8,
-            "velocityX": -3.782178134958452,
-            "velocityY": 0.07594372104916906,
-            "timestamp": 2.478147673612266
-        },
-        {
-            "x": 5.8312862675507695,
-            "y": 6.634145572711449,
-            "heading": 3.3782020006814046,
-            "angularVelocity": 0.14089554281384958,
-            "velocityX": -3.6957551468978513,
-            "velocityY": -0.023380568209240477,
-            "timestamp": 2.566801571587092
-        },
-        {
-            "x": 5.569171767766223,
-            "y": 6.6324873452973785,
-            "heading": 3.388195171082853,
-            "angularVelocity": 0.11272116206650896,
-            "velocityX": -2.9566043430935838,
-            "velocityY": -0.018704506535536753,
-            "timestamp": 2.6554554695619186
-        },
-        {
-            "x": 5.372585886333595,
-            "y": 6.631243673213895,
-            "heading": 3.395690186633039,
-            "angularVelocity": 0.08454242533492021,
-            "velocityX": -2.2174533317017797,
-            "velocityY": -0.014028397080035612,
-            "timestamp": 2.744109367536745
-        },
-        {
-            "x": 5.241528629389324,
-            "y": 6.630414557878557,
-            "heading": 3.4006869200557808,
-            "angularVelocity": 0.05636225295091492,
-            "velocityX": -1.4783022510920478,
-            "velocityY": -0.00935227163472713,
-            "timestamp": 2.8327632655115713
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.02818135672404778,
-            "velocityX": -0.7391511358917447,
-            "velocityY": -0.00467613819613763,
-            "timestamp": 2.9214171634863977
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0,
-            "velocityX": -2.6920195969080552e-36,
-            "velocityY": 0,
-            "timestamp": 3.010071061461224
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S2.1.traj
deleted file mode 100644
index af09df59..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S2.1.traj	
+++ /dev/null
@@ -1,337 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3595382644510017,
-            "y": 4.372038786355543,
-            "heading": 2.7583958335298844,
-            "angularVelocity": 0.09833011550569279,
-            "velocityX": 0.6972987732113981,
-            "velocityY": 0.140995560318735,
-            "timestamp": 0.08538415201391997
-        },
-        {
-            "x": 2.478614789358208,
-            "y": 4.396116350384182,
-            "heading": 2.7751880425894457,
-            "angularVelocity": 0.19666657879115437,
-            "velocityX": 1.3945974996366264,
-            "velocityY": 0.28199101895061424,
-            "timestamp": 0.17076830402783993
-        },
-        {
-            "x": 2.657229566273028,
-            "y": 4.432232679689,
-            "heading": 2.800377642298772,
-            "angularVelocity": 0.2950149309349538,
-            "velocityX": 2.0918961271138583,
-            "velocityY": 0.4229863323925762,
-            "timestamp": 0.2561524560417599
-        },
-        {
-            "x": 2.8953825799844894,
-            "y": 4.480387759130421,
-            "heading": 2.8339661569504204,
-            "angularVelocity": 0.39338113525063506,
-            "velocityX": 2.7891945764435975,
-            "velocityY": 0.5639814685232187,
-            "timestamp": 0.34153660805567987
-        },
-        {
-            "x": 3.1930738006682495,
-            "y": 4.5405815710050295,
-            "heading": 2.8759554693739764,
-            "angularVelocity": 0.49176939084328164,
-            "velocityX": 3.486492676477345,
-            "velocityY": 0.7049763973154618,
-            "timestamp": 0.42692076006959984
-        },
-        {
-            "x": 3.5096695268696414,
-            "y": 4.604598695680213,
-            "heading": 2.8759554743130047,
-            "angularVelocity": 5.784479190315687e-8,
-            "velocityX": 3.707897996688871,
-            "velocityY": 0.7497541776224135,
-            "timestamp": 0.5123049120835198
-        },
-        {
-            "x": 3.8262652530617234,
-            "y": 4.6686158204015085,
-            "heading": 2.875955479252014,
-            "angularVelocity": 5.784456729975194e-8,
-            "velocityX": 3.7078979965798418,
-            "velocityY": 0.7497541781624566,
-            "timestamp": 0.5976890640974397
-        },
-        {
-            "x": 4.142860979253806,
-            "y": 4.732632945122804,
-            "heading": 2.875955484191023,
-            "angularVelocity": 5.784456771643219e-8,
-            "velocityX": 3.707897996579841,
-            "velocityY": 0.7497541781624594,
-            "timestamp": 0.6830732161113597
-        },
-        {
-            "x": 4.459456705445889,
-            "y": 4.796650069844099,
-            "heading": 2.8759554891300323,
-            "angularVelocity": 5.7844567227510466e-8,
-            "velocityX": 3.707897996579841,
-            "velocityY": 0.7497541781624593,
-            "timestamp": 0.7684573681252798
-        },
-        {
-            "x": 4.7760524316379716,
-            "y": 4.860667194565394,
-            "heading": 2.8759554940690415,
-            "angularVelocity": 5.784456749923784e-8,
-            "velocityX": 3.707897996579841,
-            "velocityY": 0.7497541781624593,
-            "timestamp": 0.8538415201391998
-        },
-        {
-            "x": 5.092648157830054,
-            "y": 4.92468431928669,
-            "heading": 2.8759554990080507,
-            "angularVelocity": 5.784456784777939e-8,
-            "velocityX": 3.707897996579841,
-            "velocityY": 0.749754178162459,
-            "timestamp": 0.9392256721531198
-        },
-        {
-            "x": 5.409243884022137,
-            "y": 4.988701444007985,
-            "heading": 2.87595550394706,
-            "angularVelocity": 5.784456724140334e-8,
-            "velocityX": 3.707897996579841,
-            "velocityY": 0.749754178162459,
-            "timestamp": 1.0246098241670398
-        },
-        {
-            "x": 5.72583961021422,
-            "y": 5.05271856872928,
-            "heading": 2.875955508886069,
-            "angularVelocity": 5.7844566753037437e-8,
-            "velocityX": 3.7078979965798413,
-            "velocityY": 0.7497541781624589,
-            "timestamp": 1.1099939761809599
-        },
-        {
-            "x": 6.042435336406303,
-            "y": 5.116735693450575,
-            "heading": 2.8759555138250783,
-            "angularVelocity": 5.784456768545098e-8,
-            "velocityX": 3.7078979965798413,
-            "velocityY": 0.749754178162459,
-            "timestamp": 1.1953781281948799
-        },
-        {
-            "x": 6.35903106259824,
-            "y": 5.180752818172588,
-            "heading": 2.8759555187640875,
-            "angularVelocity": 5.784456775109183e-8,
-            "velocityX": 3.707897996578142,
-            "velocityY": 0.7497541781708625,
-            "timestamp": 1.2807622802088
-        },
-        {
-            "x": 6.67562676006639,
-            "y": 5.244770084947493,
-            "heading": 2.875955523703097,
-            "angularVelocity": 5.7844569052705035e-8,
-            "velocityX": 3.7078976601715947,
-            "velocityY": 0.7497558418624185,
-            "timestamp": 1.36614643222272
-        },
-        {
-            "x": 6.986762276730635,
-            "y": 5.331521073675251,
-            "heading": 2.875955533952509,
-            "angularVelocity": 1.2003881364523766e-7,
-            "velocityX": 3.643949249663132,
-            "velocityY": 1.0160080844231525,
-            "timestamp": 1.45153058423664
-        },
-        {
-            "x": 7.245394684994352,
-            "y": 5.443936340264843,
-            "heading": 2.937208027811149,
-            "angularVelocity": 0.717375442794747,
-            "velocityX": 3.0290446431036937,
-            "velocityY": 1.3165823392058056,
-            "timestamp": 1.53691473625056
-        },
-        {
-            "x": 7.443822925582165,
-            "y": 5.547959848416938,
-            "heading": 2.996476692643064,
-            "angularVelocity": 0.694141283059797,
-            "velocityX": 2.3239469609707415,
-            "velocityY": 1.2182999502663732,
-            "timestamp": 1.62229888826448
-        },
-        {
-            "x": 7.58233184253966,
-            "y": 5.641838110863567,
-            "heading": 3.05066970535842,
-            "angularVelocity": 0.6346963861223477,
-            "velocityX": 1.6221853082866564,
-            "velocityY": 1.0994811125058057,
-            "timestamp": 1.7076830402784
-        },
-        {
-            "x": 7.661036876366553,
-            "y": 5.724955999091069,
-            "heading": 3.0986966592522744,
-            "angularVelocity": 0.5624808909037848,
-            "velocityX": 0.9217756687922859,
-            "velocityY": 0.9734580278311056,
-            "timestamp": 1.79306719229232
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 0.4837354447344324,
-            "velocityX": 0.22209184241070226,
-            "velocityY": 0.8437631481915462,
-            "timestamp": 1.87845134430624
-        },
-        {
-            "x": 7.620399591023171,
-            "y": 5.865894759292055,
-            "heading": 3.178669357704941,
-            "angularVelocity": 0.3869200928483079,
-            "velocityX": -0.5963532146324005,
-            "velocityY": 0.6893511618538622,
-            "timestamp": 1.978392801004126
-        },
-        {
-            "x": 7.479095963903043,
-            "y": 5.918870767960299,
-            "heading": 3.2076554876039776,
-            "angularVelocity": 0.29003109276922934,
-            "velocityX": -1.413863993870693,
-            "velocityY": 0.5300704074024576,
-            "timestamp": 2.078334257702012
-        },
-        {
-            "x": 7.256273297326838,
-            "y": 5.955010806465711,
-            "heading": 3.2269465299619338,
-            "angularVelocity": 0.19302342586691915,
-            "velocityX": -2.229531907362301,
-            "velocityY": 0.36161208470935113,
-            "timestamp": 2.178275714399898
-        },
-        {
-            "x": 6.95245768893108,
-            "y": 5.971943859159761,
-            "heading": 3.236518587853394,
-            "angularVelocity": 0.09577664972800967,
-            "velocityX": -3.0399357627352264,
-            "velocityY": 0.16942971669141613,
-            "timestamp": 2.278217171097784
-        },
-        {
-            "x": 6.575020543317634,
-            "y": 5.950033200191844,
-            "heading": 3.236518582605277,
-            "angularVelocity": -5.2511914101363804e-8,
-            "velocityX": -3.7765823921738897,
-            "velocityY": -0.21923493705070526,
-            "timestamp": 2.37815862779567
-        },
-        {
-            "x": 6.202215559947926,
-            "y": 5.88714179095661,
-            "heading": 3.2365185785582873,
-            "angularVelocity": -4.049360572502445e-8,
-            "velocityX": -3.730233635643953,
-            "velocityY": -0.6292824951045971,
-            "timestamp": 2.478100084493556
-        },
-        {
-            "x": 5.829410606689244,
-            "y": 5.824250203230788,
-            "heading": 3.2365185745112974,
-            "angularVelocity": -4.049360338896351e-8,
-            "velocityX": -3.7302333343573055,
-            "velocityY": -0.6292842810560431,
-            "timestamp": 2.578041541191442
-        },
-        {
-            "x": 5.456605653430655,
-            "y": 5.761358615504415,
-            "heading": 3.2365185704643076,
-            "angularVelocity": -4.049360329558589e-8,
-            "velocityX": -3.730233334356378,
-            "velocityY": -0.6292842810615451,
-            "timestamp": 2.677982997889328
-        },
-        {
-            "x": 5.0838007001693555,
-            "y": 5.698467027794241,
-            "heading": 3.2365185664173017,
-            "angularVelocity": -4.0493766096106506e-8,
-            "velocityX": -3.7302333343834975,
-            "velocityY": -0.6292842808994649,
-            "timestamp": 2.777924454587214
-        },
-        {
-            "x": 4.755480441979736,
-            "y": 5.643080213031107,
-            "heading": 3.197910664693085,
-            "angularVelocity": -0.38630517304670825,
-            "velocityX": -3.28512580301988,
-            "velocityY": -0.5541925902737567,
-            "timestamp": 2.8778659112851
-        },
-        {
-            "x": 4.50924022643339,
-            "y": 5.601540105644793,
-            "heading": 3.1689552309170463,
-            "angularVelocity": -0.2897239517287447,
-            "velocityX": -2.4638445714345307,
-            "velocityY": -0.41564440582335266,
-            "timestamp": 2.977807367982986
-        },
-        {
-            "x": 4.345080076326452,
-            "y": 5.573846702024279,
-            "heading": 3.149651762381054,
-            "angularVelocity": -0.19314776043684534,
-            "velocityX": -1.6425631117543122,
-            "velocityY": -0.2770962575043274,
-            "timestamp": 3.077748824680872
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -0.09657416151367515,
-            "velocityX": -0.8212815686144453,
-            "velocityY": -0.1385481308936372,
-            "timestamp": 3.177690281378758
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": -1.245204860871434e-36,
-            "velocityY": 0,
-            "timestamp": 3.277631738076644
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S2.traj
deleted file mode 100644
index af09df59..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2 To S2.traj	
+++ /dev/null
@@ -1,337 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3595382644510017,
-            "y": 4.372038786355543,
-            "heading": 2.7583958335298844,
-            "angularVelocity": 0.09833011550569279,
-            "velocityX": 0.6972987732113981,
-            "velocityY": 0.140995560318735,
-            "timestamp": 0.08538415201391997
-        },
-        {
-            "x": 2.478614789358208,
-            "y": 4.396116350384182,
-            "heading": 2.7751880425894457,
-            "angularVelocity": 0.19666657879115437,
-            "velocityX": 1.3945974996366264,
-            "velocityY": 0.28199101895061424,
-            "timestamp": 0.17076830402783993
-        },
-        {
-            "x": 2.657229566273028,
-            "y": 4.432232679689,
-            "heading": 2.800377642298772,
-            "angularVelocity": 0.2950149309349538,
-            "velocityX": 2.0918961271138583,
-            "velocityY": 0.4229863323925762,
-            "timestamp": 0.2561524560417599
-        },
-        {
-            "x": 2.8953825799844894,
-            "y": 4.480387759130421,
-            "heading": 2.8339661569504204,
-            "angularVelocity": 0.39338113525063506,
-            "velocityX": 2.7891945764435975,
-            "velocityY": 0.5639814685232187,
-            "timestamp": 0.34153660805567987
-        },
-        {
-            "x": 3.1930738006682495,
-            "y": 4.5405815710050295,
-            "heading": 2.8759554693739764,
-            "angularVelocity": 0.49176939084328164,
-            "velocityX": 3.486492676477345,
-            "velocityY": 0.7049763973154618,
-            "timestamp": 0.42692076006959984
-        },
-        {
-            "x": 3.5096695268696414,
-            "y": 4.604598695680213,
-            "heading": 2.8759554743130047,
-            "angularVelocity": 5.784479190315687e-8,
-            "velocityX": 3.707897996688871,
-            "velocityY": 0.7497541776224135,
-            "timestamp": 0.5123049120835198
-        },
-        {
-            "x": 3.8262652530617234,
-            "y": 4.6686158204015085,
-            "heading": 2.875955479252014,
-            "angularVelocity": 5.784456729975194e-8,
-            "velocityX": 3.7078979965798418,
-            "velocityY": 0.7497541781624566,
-            "timestamp": 0.5976890640974397
-        },
-        {
-            "x": 4.142860979253806,
-            "y": 4.732632945122804,
-            "heading": 2.875955484191023,
-            "angularVelocity": 5.784456771643219e-8,
-            "velocityX": 3.707897996579841,
-            "velocityY": 0.7497541781624594,
-            "timestamp": 0.6830732161113597
-        },
-        {
-            "x": 4.459456705445889,
-            "y": 4.796650069844099,
-            "heading": 2.8759554891300323,
-            "angularVelocity": 5.7844567227510466e-8,
-            "velocityX": 3.707897996579841,
-            "velocityY": 0.7497541781624593,
-            "timestamp": 0.7684573681252798
-        },
-        {
-            "x": 4.7760524316379716,
-            "y": 4.860667194565394,
-            "heading": 2.8759554940690415,
-            "angularVelocity": 5.784456749923784e-8,
-            "velocityX": 3.707897996579841,
-            "velocityY": 0.7497541781624593,
-            "timestamp": 0.8538415201391998
-        },
-        {
-            "x": 5.092648157830054,
-            "y": 4.92468431928669,
-            "heading": 2.8759554990080507,
-            "angularVelocity": 5.784456784777939e-8,
-            "velocityX": 3.707897996579841,
-            "velocityY": 0.749754178162459,
-            "timestamp": 0.9392256721531198
-        },
-        {
-            "x": 5.409243884022137,
-            "y": 4.988701444007985,
-            "heading": 2.87595550394706,
-            "angularVelocity": 5.784456724140334e-8,
-            "velocityX": 3.707897996579841,
-            "velocityY": 0.749754178162459,
-            "timestamp": 1.0246098241670398
-        },
-        {
-            "x": 5.72583961021422,
-            "y": 5.05271856872928,
-            "heading": 2.875955508886069,
-            "angularVelocity": 5.7844566753037437e-8,
-            "velocityX": 3.7078979965798413,
-            "velocityY": 0.7497541781624589,
-            "timestamp": 1.1099939761809599
-        },
-        {
-            "x": 6.042435336406303,
-            "y": 5.116735693450575,
-            "heading": 2.8759555138250783,
-            "angularVelocity": 5.784456768545098e-8,
-            "velocityX": 3.7078979965798413,
-            "velocityY": 0.749754178162459,
-            "timestamp": 1.1953781281948799
-        },
-        {
-            "x": 6.35903106259824,
-            "y": 5.180752818172588,
-            "heading": 2.8759555187640875,
-            "angularVelocity": 5.784456775109183e-8,
-            "velocityX": 3.707897996578142,
-            "velocityY": 0.7497541781708625,
-            "timestamp": 1.2807622802088
-        },
-        {
-            "x": 6.67562676006639,
-            "y": 5.244770084947493,
-            "heading": 2.875955523703097,
-            "angularVelocity": 5.7844569052705035e-8,
-            "velocityX": 3.7078976601715947,
-            "velocityY": 0.7497558418624185,
-            "timestamp": 1.36614643222272
-        },
-        {
-            "x": 6.986762276730635,
-            "y": 5.331521073675251,
-            "heading": 2.875955533952509,
-            "angularVelocity": 1.2003881364523766e-7,
-            "velocityX": 3.643949249663132,
-            "velocityY": 1.0160080844231525,
-            "timestamp": 1.45153058423664
-        },
-        {
-            "x": 7.245394684994352,
-            "y": 5.443936340264843,
-            "heading": 2.937208027811149,
-            "angularVelocity": 0.717375442794747,
-            "velocityX": 3.0290446431036937,
-            "velocityY": 1.3165823392058056,
-            "timestamp": 1.53691473625056
-        },
-        {
-            "x": 7.443822925582165,
-            "y": 5.547959848416938,
-            "heading": 2.996476692643064,
-            "angularVelocity": 0.694141283059797,
-            "velocityX": 2.3239469609707415,
-            "velocityY": 1.2182999502663732,
-            "timestamp": 1.62229888826448
-        },
-        {
-            "x": 7.58233184253966,
-            "y": 5.641838110863567,
-            "heading": 3.05066970535842,
-            "angularVelocity": 0.6346963861223477,
-            "velocityX": 1.6221853082866564,
-            "velocityY": 1.0994811125058057,
-            "timestamp": 1.7076830402784
-        },
-        {
-            "x": 7.661036876366553,
-            "y": 5.724955999091069,
-            "heading": 3.0986966592522744,
-            "angularVelocity": 0.5624808909037848,
-            "velocityX": 0.9217756687922859,
-            "velocityY": 0.9734580278311056,
-            "timestamp": 1.79306719229232
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 0.4837354447344324,
-            "velocityX": 0.22209184241070226,
-            "velocityY": 0.8437631481915462,
-            "timestamp": 1.87845134430624
-        },
-        {
-            "x": 7.620399591023171,
-            "y": 5.865894759292055,
-            "heading": 3.178669357704941,
-            "angularVelocity": 0.3869200928483079,
-            "velocityX": -0.5963532146324005,
-            "velocityY": 0.6893511618538622,
-            "timestamp": 1.978392801004126
-        },
-        {
-            "x": 7.479095963903043,
-            "y": 5.918870767960299,
-            "heading": 3.2076554876039776,
-            "angularVelocity": 0.29003109276922934,
-            "velocityX": -1.413863993870693,
-            "velocityY": 0.5300704074024576,
-            "timestamp": 2.078334257702012
-        },
-        {
-            "x": 7.256273297326838,
-            "y": 5.955010806465711,
-            "heading": 3.2269465299619338,
-            "angularVelocity": 0.19302342586691915,
-            "velocityX": -2.229531907362301,
-            "velocityY": 0.36161208470935113,
-            "timestamp": 2.178275714399898
-        },
-        {
-            "x": 6.95245768893108,
-            "y": 5.971943859159761,
-            "heading": 3.236518587853394,
-            "angularVelocity": 0.09577664972800967,
-            "velocityX": -3.0399357627352264,
-            "velocityY": 0.16942971669141613,
-            "timestamp": 2.278217171097784
-        },
-        {
-            "x": 6.575020543317634,
-            "y": 5.950033200191844,
-            "heading": 3.236518582605277,
-            "angularVelocity": -5.2511914101363804e-8,
-            "velocityX": -3.7765823921738897,
-            "velocityY": -0.21923493705070526,
-            "timestamp": 2.37815862779567
-        },
-        {
-            "x": 6.202215559947926,
-            "y": 5.88714179095661,
-            "heading": 3.2365185785582873,
-            "angularVelocity": -4.049360572502445e-8,
-            "velocityX": -3.730233635643953,
-            "velocityY": -0.6292824951045971,
-            "timestamp": 2.478100084493556
-        },
-        {
-            "x": 5.829410606689244,
-            "y": 5.824250203230788,
-            "heading": 3.2365185745112974,
-            "angularVelocity": -4.049360338896351e-8,
-            "velocityX": -3.7302333343573055,
-            "velocityY": -0.6292842810560431,
-            "timestamp": 2.578041541191442
-        },
-        {
-            "x": 5.456605653430655,
-            "y": 5.761358615504415,
-            "heading": 3.2365185704643076,
-            "angularVelocity": -4.049360329558589e-8,
-            "velocityX": -3.730233334356378,
-            "velocityY": -0.6292842810615451,
-            "timestamp": 2.677982997889328
-        },
-        {
-            "x": 5.0838007001693555,
-            "y": 5.698467027794241,
-            "heading": 3.2365185664173017,
-            "angularVelocity": -4.0493766096106506e-8,
-            "velocityX": -3.7302333343834975,
-            "velocityY": -0.6292842808994649,
-            "timestamp": 2.777924454587214
-        },
-        {
-            "x": 4.755480441979736,
-            "y": 5.643080213031107,
-            "heading": 3.197910664693085,
-            "angularVelocity": -0.38630517304670825,
-            "velocityX": -3.28512580301988,
-            "velocityY": -0.5541925902737567,
-            "timestamp": 2.8778659112851
-        },
-        {
-            "x": 4.50924022643339,
-            "y": 5.601540105644793,
-            "heading": 3.1689552309170463,
-            "angularVelocity": -0.2897239517287447,
-            "velocityX": -2.4638445714345307,
-            "velocityY": -0.41564440582335266,
-            "timestamp": 2.977807367982986
-        },
-        {
-            "x": 4.345080076326452,
-            "y": 5.573846702024279,
-            "heading": 3.149651762381054,
-            "angularVelocity": -0.19314776043684534,
-            "velocityX": -1.6425631117543122,
-            "velocityY": -0.2770962575043274,
-            "timestamp": 3.077748824680872
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -0.09657416151367515,
-            "velocityX": -0.8212815686144453,
-            "velocityY": -0.1385481308936372,
-            "timestamp": 3.177690281378758
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": -1.245204860871434e-36,
-            "velocityY": 0,
-            "timestamp": 3.277631738076644
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S1.1.traj
deleted file mode 100644
index fb79e562..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S1.1.traj	
+++ /dev/null
@@ -1,346 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.358164523227949,
-            "y": 4.34513375016413,
-            "heading": 2.762567979975215,
-            "angularVelocity": 0.1479885663322048,
-            "velocityX": 0.6848900476349593,
-            "velocityY": -0.17505080405008622,
-            "timestamp": 0.08492534448237501
-        },
-        {
-            "x": 2.474493569181997,
-            "y": 4.315401331546784,
-            "heading": 2.787705678078292,
-            "angularVelocity": 0.2959975994950906,
-            "velocityX": 1.3697800893606054,
-            "velocityY": -0.35010065368078286,
-            "timestamp": 0.16985068896475003
-        },
-        {
-            "x": 2.6489871423142275,
-            "y": 4.270802870950086,
-            "heading": 2.825415221133538,
-            "angularVelocity": 0.4440316761160991,
-            "velocityX": 2.054670183509755,
-            "velocityY": -0.525149010210423,
-            "timestamp": 0.25477603344712507
-        },
-        {
-            "x": 2.881645266961739,
-            "y": 4.211338540895288,
-            "heading": 2.8756978223326772,
-            "angularVelocity": 0.5920800381277782,
-            "velocityX": 2.739560564229402,
-            "velocityY": -0.7001953352940375,
-            "timestamp": 0.33970137792950006
-        },
-        {
-            "x": 3.172468005146889,
-            "y": 4.137008553769027,
-            "heading": 2.9385516414717108,
-            "angularVelocity": 0.7401067316492073,
-            "velocityX": 3.4244516752652747,
-            "velocityY": -0.8752391595148364,
-            "timestamp": 0.42462672241187505
-        },
-        {
-            "x": 3.48373017288665,
-            "y": 4.0574555172899185,
-            "heading": 2.9385516489955585,
-            "angularVelocity": 8.859366728426556e-8,
-            "velocityX": 3.6651269375110873,
-            "velocityY": -0.936740815877624,
-            "timestamp": 0.50955206689425
-        },
-        {
-            "x": 3.7949923406136996,
-            "y": 3.977902480761,
-            "heading": 2.9385516565193726,
-            "angularVelocity": 8.859327100912678e-8,
-            "velocityX": 3.665126937361409,
-            "velocityY": -0.9367408164641361,
-            "timestamp": 0.594477411376625
-        },
-        {
-            "x": 4.10625450834075,
-            "y": 3.898349444232082,
-            "heading": 2.938551664043187,
-            "angularVelocity": 8.859327019599772e-8,
-            "velocityX": 3.665126937361408,
-            "velocityY": -0.9367408164641391,
-            "timestamp": 0.679402755859
-        },
-        {
-            "x": 4.417516676067799,
-            "y": 3.818796407703164,
-            "heading": 2.938551671567001,
-            "angularVelocity": 8.859327006115765e-8,
-            "velocityX": 3.6651269373614084,
-            "velocityY": -0.9367408164641391,
-            "timestamp": 0.764328100341375
-        },
-        {
-            "x": 4.728778843794849,
-            "y": 3.7392433711742457,
-            "heading": 2.938551679090815,
-            "angularVelocity": 8.859327000476347e-8,
-            "velocityX": 3.665126937361408,
-            "velocityY": -0.9367408164641391,
-            "timestamp": 0.84925344482375
-        },
-        {
-            "x": 5.0400410115218985,
-            "y": 3.6596903346453273,
-            "heading": 2.9385516866146295,
-            "angularVelocity": 8.85932714022833e-8,
-            "velocityX": 3.665126937361408,
-            "velocityY": -0.9367408164641392,
-            "timestamp": 0.934178789306125
-        },
-        {
-            "x": 5.351303179248949,
-            "y": 3.580137298116409,
-            "heading": 2.9385516941384435,
-            "angularVelocity": 8.859327127858633e-8,
-            "velocityX": 3.665126937361408,
-            "velocityY": -0.936740816464139,
-            "timestamp": 1.0191041337885
-        },
-        {
-            "x": 5.662565346976546,
-            "y": 3.5005842615896343,
-            "heading": 2.938551701662258,
-            "angularVelocity": 8.859327107695318e-8,
-            "velocityX": 3.6651269373678588,
-            "velocityY": -0.9367408164388998,
-            "timestamp": 1.104029478270875
-        },
-        {
-            "x": 5.973827620415735,
-            "y": 3.4210316386751205,
-            "heading": 2.9385517091860733,
-            "angularVelocity": 8.859328561882092e-8,
-            "velocityX": 3.6651281821269137,
-            "velocityY": -0.9367359461347136,
-            "timestamp": 1.18895482275325
-        },
-        {
-            "x": 6.292581444856613,
-            "y": 3.380921520397279,
-            "heading": 2.9385517170132656,
-            "angularVelocity": 9.216556583241828e-8,
-            "velocityX": 3.753341554087312,
-            "velocityY": -0.4722985643722118,
-            "timestamp": 1.273880167235625
-        },
-        {
-            "x": 6.61322600810305,
-            "y": 3.400918618969079,
-            "heading": 2.938551726316265,
-            "angularVelocity": 1.0954326272813381e-7,
-            "velocityX": 3.7756050941068793,
-            "velocityY": 0.23546679373140608,
-            "timestamp": 1.358805511718
-        },
-        {
-            "x": 6.924526035610863,
-            "y": 3.4803224362243568,
-            "heading": 2.9385522977985694,
-            "angularVelocity": 0.000006729231518347491,
-            "velocityX": 3.665572738093738,
-            "velocityY": 0.9349837523680175,
-            "timestamp": 1.443730856200375
-        },
-        {
-            "x": 7.193617949641073,
-            "y": 3.601275977700439,
-            "heading": 2.9756713722126755,
-            "angularVelocity": 0.43707887957768976,
-            "velocityX": 3.1685701797306898,
-            "velocityY": 1.4242337456892449,
-            "timestamp": 1.52865620068275
-        },
-        {
-            "x": 7.4107131876465635,
-            "y": 3.752187582369628,
-            "heading": 3.026776803352479,
-            "angularVelocity": 0.6017689001003675,
-            "velocityX": 2.5563068284114614,
-            "velocityY": 1.7769913750601078,
-            "timestamp": 1.613581545165125
-        },
-        {
-            "x": 7.573210012728862,
-            "y": 3.928266853326757,
-            "heading": 3.0830399478590267,
-            "angularVelocity": 0.6625012220965879,
-            "velocityX": 1.9134079004651277,
-            "velocityY": 2.0733418513677266,
-            "timestamp": 1.6985068896475
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.6707073428803676,
-            "velocityX": 1.257457216359011,
-            "velocityY": 2.340092323257951,
-            "timestamp": 1.783432234129875
-        },
-        {
-            "x": 7.730461589944827,
-            "y": 4.347803549557207,
-            "heading": 3.1951904179502595,
-            "angularVelocity": 0.647312922727555,
-            "velocityX": 0.5918498262162951,
-            "velocityY": 2.5897428633591493,
-            "timestamp": 1.868693034605951
-        },
-        {
-            "x": 7.723612364676548,
-            "y": 4.588311365928096,
-            "heading": 3.24650445053595,
-            "angularVelocity": 0.6018478867095445,
-            "velocityX": -0.0803326409092394,
-            "velocityY": 2.820848678735727,
-            "timestamp": 1.953953835082027
-        },
-        {
-            "x": 7.6586227331171335,
-            "y": 4.845842471404587,
-            "heading": 3.2908004330134384,
-            "angularVelocity": 0.5195351466342113,
-            "velocityX": -0.762245148960926,
-            "velocityY": 3.020510059001304,
-            "timestamp": 2.039214635558103
-        },
-        {
-            "x": 7.534247896600953,
-            "y": 5.114930519867729,
-            "heading": 3.3218318995468055,
-            "angularVelocity": 0.3639593618649453,
-            "velocityX": -1.4587575512040862,
-            "velocityY": 3.1560581997895576,
-            "timestamp": 2.124475436034179
-        },
-        {
-            "x": 7.350032329869969,
-            "y": 5.3796839143357555,
-            "heading": 3.321832164657738,
-            "angularVelocity": 0.0000031094117170733377,
-            "velocityX": -2.160612681353773,
-            "velocityY": 3.1052182596188063,
-            "timestamp": 2.209736236510255
-        },
-        {
-            "x": 7.119535512814417,
-            "y": 5.605296483491154,
-            "heading": 3.3218321686163597,
-            "angularVelocity": 4.6429567622162366e-8,
-            "velocityX": -2.70343247739305,
-            "velocityY": 2.6461465045558032,
-            "timestamp": 2.294997036986331
-        },
-        {
-            "x": 6.850897968160264,
-            "y": 5.7838005054369255,
-            "heading": 3.321832171960481,
-            "angularVelocity": 3.9222265908183896e-8,
-            "velocityX": -3.1507743670496304,
-            "velocityY": 2.0936235755358563,
-            "timestamp": 2.380257837462407
-        },
-        {
-            "x": 6.563016789640927,
-            "y": 5.929245490779759,
-            "heading": 3.321832175184079,
-            "angularVelocity": 3.780866961118305e-8,
-            "velocityX": -3.376477547851724,
-            "velocityY": 1.7058834133705343,
-            "timestamp": 2.465518637938483
-        },
-        {
-            "x": 6.275135446510561,
-            "y": 6.074690150305356,
-            "heading": 3.321832178407676,
-            "angularVelocity": 3.780866514458699e-8,
-            "velocityX": -3.3764794785282892,
-            "velocityY": 1.7058795919516283,
-            "timestamp": 2.550779438414559
-        },
-        {
-            "x": 5.987254103377649,
-            "y": 6.2201348098258835,
-            "heading": 3.3218321816312852,
-            "angularVelocity": 3.780881195826611e-8,
-            "velocityX": -3.376479478558147,
-            "velocityY": 1.705879591892162,
-            "timestamp": 2.6360402388906348
-        },
-        {
-            "x": 5.716836088038681,
-            "y": 6.356756526684127,
-            "heading": 3.3489498411718635,
-            "angularVelocity": 0.3180554180720732,
-            "velocityX": -3.171657008015607,
-            "velocityY": 1.6023977736002932,
-            "timestamp": 2.7213010393667108
-        },
-        {
-            "x": 5.500501658783529,
-            "y": 6.4660539115745435,
-            "heading": 3.370644010379382,
-            "angularVelocity": 0.2544448220798278,
-            "velocityX": -2.537325805612817,
-            "velocityY": 1.2819183526324525,
-            "timestamp": 2.8065618398427867
-        },
-        {
-            "x": 5.33825083142265,
-            "y": 6.5480269544022365,
-            "heading": 3.3869146454118173,
-            "angularVelocity": 0.19083371187678314,
-            "velocityX": -1.9029944177735691,
-            "velocityY": 0.9614388132644309,
-            "timestamp": 2.8918226403188627
-        },
-        {
-            "x": 5.2300836110004285,
-            "y": 6.602675651200572,
-            "heading": 3.3977617464965464,
-            "angularVelocity": 0.12722260434057955,
-            "velocityX": -1.2686629707701718,
-            "velocityY": 0.6409592273728405,
-            "timestamp": 2.9770834407949387
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.0636114211074268,
-            "velocityX": -0.6343314946427733,
-            "velocityY": 0.3204796183809554,
-            "timestamp": 3.0623442412710147
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0,
-            "velocityX": -1.2693090039370116e-34,
-            "velocityY": 0,
-            "timestamp": 3.1476050417470907
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S1.traj
deleted file mode 100644
index fb79e562..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S1.traj	
+++ /dev/null
@@ -1,346 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.358164523227949,
-            "y": 4.34513375016413,
-            "heading": 2.762567979975215,
-            "angularVelocity": 0.1479885663322048,
-            "velocityX": 0.6848900476349593,
-            "velocityY": -0.17505080405008622,
-            "timestamp": 0.08492534448237501
-        },
-        {
-            "x": 2.474493569181997,
-            "y": 4.315401331546784,
-            "heading": 2.787705678078292,
-            "angularVelocity": 0.2959975994950906,
-            "velocityX": 1.3697800893606054,
-            "velocityY": -0.35010065368078286,
-            "timestamp": 0.16985068896475003
-        },
-        {
-            "x": 2.6489871423142275,
-            "y": 4.270802870950086,
-            "heading": 2.825415221133538,
-            "angularVelocity": 0.4440316761160991,
-            "velocityX": 2.054670183509755,
-            "velocityY": -0.525149010210423,
-            "timestamp": 0.25477603344712507
-        },
-        {
-            "x": 2.881645266961739,
-            "y": 4.211338540895288,
-            "heading": 2.8756978223326772,
-            "angularVelocity": 0.5920800381277782,
-            "velocityX": 2.739560564229402,
-            "velocityY": -0.7001953352940375,
-            "timestamp": 0.33970137792950006
-        },
-        {
-            "x": 3.172468005146889,
-            "y": 4.137008553769027,
-            "heading": 2.9385516414717108,
-            "angularVelocity": 0.7401067316492073,
-            "velocityX": 3.4244516752652747,
-            "velocityY": -0.8752391595148364,
-            "timestamp": 0.42462672241187505
-        },
-        {
-            "x": 3.48373017288665,
-            "y": 4.0574555172899185,
-            "heading": 2.9385516489955585,
-            "angularVelocity": 8.859366728426556e-8,
-            "velocityX": 3.6651269375110873,
-            "velocityY": -0.936740815877624,
-            "timestamp": 0.50955206689425
-        },
-        {
-            "x": 3.7949923406136996,
-            "y": 3.977902480761,
-            "heading": 2.9385516565193726,
-            "angularVelocity": 8.859327100912678e-8,
-            "velocityX": 3.665126937361409,
-            "velocityY": -0.9367408164641361,
-            "timestamp": 0.594477411376625
-        },
-        {
-            "x": 4.10625450834075,
-            "y": 3.898349444232082,
-            "heading": 2.938551664043187,
-            "angularVelocity": 8.859327019599772e-8,
-            "velocityX": 3.665126937361408,
-            "velocityY": -0.9367408164641391,
-            "timestamp": 0.679402755859
-        },
-        {
-            "x": 4.417516676067799,
-            "y": 3.818796407703164,
-            "heading": 2.938551671567001,
-            "angularVelocity": 8.859327006115765e-8,
-            "velocityX": 3.6651269373614084,
-            "velocityY": -0.9367408164641391,
-            "timestamp": 0.764328100341375
-        },
-        {
-            "x": 4.728778843794849,
-            "y": 3.7392433711742457,
-            "heading": 2.938551679090815,
-            "angularVelocity": 8.859327000476347e-8,
-            "velocityX": 3.665126937361408,
-            "velocityY": -0.9367408164641391,
-            "timestamp": 0.84925344482375
-        },
-        {
-            "x": 5.0400410115218985,
-            "y": 3.6596903346453273,
-            "heading": 2.9385516866146295,
-            "angularVelocity": 8.85932714022833e-8,
-            "velocityX": 3.665126937361408,
-            "velocityY": -0.9367408164641392,
-            "timestamp": 0.934178789306125
-        },
-        {
-            "x": 5.351303179248949,
-            "y": 3.580137298116409,
-            "heading": 2.9385516941384435,
-            "angularVelocity": 8.859327127858633e-8,
-            "velocityX": 3.665126937361408,
-            "velocityY": -0.936740816464139,
-            "timestamp": 1.0191041337885
-        },
-        {
-            "x": 5.662565346976546,
-            "y": 3.5005842615896343,
-            "heading": 2.938551701662258,
-            "angularVelocity": 8.859327107695318e-8,
-            "velocityX": 3.6651269373678588,
-            "velocityY": -0.9367408164388998,
-            "timestamp": 1.104029478270875
-        },
-        {
-            "x": 5.973827620415735,
-            "y": 3.4210316386751205,
-            "heading": 2.9385517091860733,
-            "angularVelocity": 8.859328561882092e-8,
-            "velocityX": 3.6651281821269137,
-            "velocityY": -0.9367359461347136,
-            "timestamp": 1.18895482275325
-        },
-        {
-            "x": 6.292581444856613,
-            "y": 3.380921520397279,
-            "heading": 2.9385517170132656,
-            "angularVelocity": 9.216556583241828e-8,
-            "velocityX": 3.753341554087312,
-            "velocityY": -0.4722985643722118,
-            "timestamp": 1.273880167235625
-        },
-        {
-            "x": 6.61322600810305,
-            "y": 3.400918618969079,
-            "heading": 2.938551726316265,
-            "angularVelocity": 1.0954326272813381e-7,
-            "velocityX": 3.7756050941068793,
-            "velocityY": 0.23546679373140608,
-            "timestamp": 1.358805511718
-        },
-        {
-            "x": 6.924526035610863,
-            "y": 3.4803224362243568,
-            "heading": 2.9385522977985694,
-            "angularVelocity": 0.000006729231518347491,
-            "velocityX": 3.665572738093738,
-            "velocityY": 0.9349837523680175,
-            "timestamp": 1.443730856200375
-        },
-        {
-            "x": 7.193617949641073,
-            "y": 3.601275977700439,
-            "heading": 2.9756713722126755,
-            "angularVelocity": 0.43707887957768976,
-            "velocityX": 3.1685701797306898,
-            "velocityY": 1.4242337456892449,
-            "timestamp": 1.52865620068275
-        },
-        {
-            "x": 7.4107131876465635,
-            "y": 3.752187582369628,
-            "heading": 3.026776803352479,
-            "angularVelocity": 0.6017689001003675,
-            "velocityX": 2.5563068284114614,
-            "velocityY": 1.7769913750601078,
-            "timestamp": 1.613581545165125
-        },
-        {
-            "x": 7.573210012728862,
-            "y": 3.928266853326757,
-            "heading": 3.0830399478590267,
-            "angularVelocity": 0.6625012220965879,
-            "velocityX": 1.9134079004651277,
-            "velocityY": 2.0733418513677266,
-            "timestamp": 1.6985068896475
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.6707073428803676,
-            "velocityX": 1.257457216359011,
-            "velocityY": 2.340092323257951,
-            "timestamp": 1.783432234129875
-        },
-        {
-            "x": 7.730461589944827,
-            "y": 4.347803549557207,
-            "heading": 3.1951904179502595,
-            "angularVelocity": 0.647312922727555,
-            "velocityX": 0.5918498262162951,
-            "velocityY": 2.5897428633591493,
-            "timestamp": 1.868693034605951
-        },
-        {
-            "x": 7.723612364676548,
-            "y": 4.588311365928096,
-            "heading": 3.24650445053595,
-            "angularVelocity": 0.6018478867095445,
-            "velocityX": -0.0803326409092394,
-            "velocityY": 2.820848678735727,
-            "timestamp": 1.953953835082027
-        },
-        {
-            "x": 7.6586227331171335,
-            "y": 4.845842471404587,
-            "heading": 3.2908004330134384,
-            "angularVelocity": 0.5195351466342113,
-            "velocityX": -0.762245148960926,
-            "velocityY": 3.020510059001304,
-            "timestamp": 2.039214635558103
-        },
-        {
-            "x": 7.534247896600953,
-            "y": 5.114930519867729,
-            "heading": 3.3218318995468055,
-            "angularVelocity": 0.3639593618649453,
-            "velocityX": -1.4587575512040862,
-            "velocityY": 3.1560581997895576,
-            "timestamp": 2.124475436034179
-        },
-        {
-            "x": 7.350032329869969,
-            "y": 5.3796839143357555,
-            "heading": 3.321832164657738,
-            "angularVelocity": 0.0000031094117170733377,
-            "velocityX": -2.160612681353773,
-            "velocityY": 3.1052182596188063,
-            "timestamp": 2.209736236510255
-        },
-        {
-            "x": 7.119535512814417,
-            "y": 5.605296483491154,
-            "heading": 3.3218321686163597,
-            "angularVelocity": 4.6429567622162366e-8,
-            "velocityX": -2.70343247739305,
-            "velocityY": 2.6461465045558032,
-            "timestamp": 2.294997036986331
-        },
-        {
-            "x": 6.850897968160264,
-            "y": 5.7838005054369255,
-            "heading": 3.321832171960481,
-            "angularVelocity": 3.9222265908183896e-8,
-            "velocityX": -3.1507743670496304,
-            "velocityY": 2.0936235755358563,
-            "timestamp": 2.380257837462407
-        },
-        {
-            "x": 6.563016789640927,
-            "y": 5.929245490779759,
-            "heading": 3.321832175184079,
-            "angularVelocity": 3.780866961118305e-8,
-            "velocityX": -3.376477547851724,
-            "velocityY": 1.7058834133705343,
-            "timestamp": 2.465518637938483
-        },
-        {
-            "x": 6.275135446510561,
-            "y": 6.074690150305356,
-            "heading": 3.321832178407676,
-            "angularVelocity": 3.780866514458699e-8,
-            "velocityX": -3.3764794785282892,
-            "velocityY": 1.7058795919516283,
-            "timestamp": 2.550779438414559
-        },
-        {
-            "x": 5.987254103377649,
-            "y": 6.2201348098258835,
-            "heading": 3.3218321816312852,
-            "angularVelocity": 3.780881195826611e-8,
-            "velocityX": -3.376479478558147,
-            "velocityY": 1.705879591892162,
-            "timestamp": 2.6360402388906348
-        },
-        {
-            "x": 5.716836088038681,
-            "y": 6.356756526684127,
-            "heading": 3.3489498411718635,
-            "angularVelocity": 0.3180554180720732,
-            "velocityX": -3.171657008015607,
-            "velocityY": 1.6023977736002932,
-            "timestamp": 2.7213010393667108
-        },
-        {
-            "x": 5.500501658783529,
-            "y": 6.4660539115745435,
-            "heading": 3.370644010379382,
-            "angularVelocity": 0.2544448220798278,
-            "velocityX": -2.537325805612817,
-            "velocityY": 1.2819183526324525,
-            "timestamp": 2.8065618398427867
-        },
-        {
-            "x": 5.33825083142265,
-            "y": 6.5480269544022365,
-            "heading": 3.3869146454118173,
-            "angularVelocity": 0.19083371187678314,
-            "velocityX": -1.9029944177735691,
-            "velocityY": 0.9614388132644309,
-            "timestamp": 2.8918226403188627
-        },
-        {
-            "x": 5.2300836110004285,
-            "y": 6.602675651200572,
-            "heading": 3.3977617464965464,
-            "angularVelocity": 0.12722260434057955,
-            "velocityX": -1.2686629707701718,
-            "velocityY": 0.6409592273728405,
-            "timestamp": 2.9770834407949387
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.0636114211074268,
-            "velocityX": -0.6343314946427733,
-            "velocityY": 0.3204796183809554,
-            "timestamp": 3.0623442412710147
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0,
-            "velocityX": -1.2693090039370116e-34,
-            "velocityY": 0,
-            "timestamp": 3.1476050417470907
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S2.1.traj
deleted file mode 100644
index 28b5f4d2..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S2.1.traj	
+++ /dev/null
@@ -1,337 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.361838792645819,
-            "y": 4.349925567383164,
-            "heading": 2.7594883701133286,
-            "angularVelocity": 0.10940981634945876,
-            "velocityX": 0.7130593416825138,
-            "velocityY": -0.11616766728823658,
-            "timestamp": 0.08672320665473099
-        },
-        {
-            "x": 2.485516776956774,
-            "y": 4.329776279587658,
-            "heading": 2.7784262463080607,
-            "angularVelocity": 0.21837149392012115,
-            "velocityX": 1.4261232844324094,
-            "velocityY": -0.2323402071111933,
-            "timestamp": 0.17344641330946198
-        },
-        {
-            "x": 2.671034614420037,
-            "y": 4.299551431366552,
-            "heading": 2.806748673224486,
-            "angularVelocity": 0.3265841752046417,
-            "velocityX": 2.1391948547504818,
-            "velocityY": -0.34852087909340246,
-            "timestamp": 0.26016961996419297
-        },
-        {
-            "x": 2.9183936206497236,
-            "y": 4.259249598753385,
-            "heading": 2.8443244197137543,
-            "angularVelocity": 0.4332836381257234,
-            "velocityX": 2.852281595334577,
-            "velocityY": -0.46471797074593596,
-            "timestamp": 0.34689282661892396
-        },
-        {
-            "x": 3.2275976397156323,
-            "y": 4.208866459419414,
-            "heading": 2.8907555208713065,
-            "angularVelocity": 0.5353941920351031,
-            "velocityX": 3.5654126616527764,
-            "velocityY": -0.5809649029072455,
-            "timestamp": 0.43361603327365494
-        },
-        {
-            "x": 3.5513989095488143,
-            "y": 4.156123400332019,
-            "heading": 2.8907555264025566,
-            "angularVelocity": 6.378050766718983e-8,
-            "velocityX": 3.7337326688382726,
-            "velocityY": -0.6081769934704949,
-            "timestamp": 0.5203392399283859
-        },
-        {
-            "x": 3.8752001793990973,
-            "y": 4.103380341349552,
-            "heading": 2.8907555319337894,
-            "angularVelocity": 6.378030392692821e-8,
-            "velocityX": 3.7337326690354664,
-            "velocityY": -0.6081769922605936,
-            "timestamp": 0.6070624465831169
-        },
-        {
-            "x": 4.19900144924938,
-            "y": 4.050637282367084,
-            "heading": 2.890755537465022,
-            "angularVelocity": 6.378030384357619e-8,
-            "velocityX": 3.7337326690354677,
-            "velocityY": -0.6081769922605877,
-            "timestamp": 0.6937856532378479
-        },
-        {
-            "x": 4.522802719099664,
-            "y": 3.9978942233846166,
-            "heading": 2.8907555429962546,
-            "angularVelocity": 6.378030394841341e-8,
-            "velocityX": 3.7337326690354673,
-            "velocityY": -0.6081769922605876,
-            "timestamp": 0.7805088598925789
-        },
-        {
-            "x": 4.846603988949947,
-            "y": 3.9451511644021493,
-            "heading": 2.8907555485274874,
-            "angularVelocity": 6.37803046219163e-8,
-            "velocityX": 3.7337326690354673,
-            "velocityY": -0.6081769922605877,
-            "timestamp": 0.8672320665473099
-        },
-        {
-            "x": 5.170405258800231,
-            "y": 3.8924081054196815,
-            "heading": 2.8907555540587198,
-            "angularVelocity": 6.37803035811712e-8,
-            "velocityX": 3.7337326690354673,
-            "velocityY": -0.6081769922605876,
-            "timestamp": 0.9539552732020409
-        },
-        {
-            "x": 5.494206528650514,
-            "y": 3.839665046437214,
-            "heading": 2.8907555595899526,
-            "angularVelocity": 6.378030493154673e-8,
-            "velocityX": 3.7337326690354673,
-            "velocityY": -0.6081769922605876,
-            "timestamp": 1.0406784798567719
-        },
-        {
-            "x": 5.818007798500798,
-            "y": 3.7869219874547464,
-            "heading": 2.8907555651211854,
-            "angularVelocity": 6.378030454431603e-8,
-            "velocityX": 3.7337326690354677,
-            "velocityY": -0.6081769922605872,
-            "timestamp": 1.1274016865115029
-        },
-        {
-            "x": 6.141809068352602,
-            "y": 3.734178928481622,
-            "heading": 2.8907555706524177,
-            "angularVelocity": 6.378030476763932e-8,
-            "velocityX": 3.7337326690530155,
-            "velocityY": -0.608176992152858,
-            "timestamp": 1.2141248931662338
-        },
-        {
-            "x": 6.4656106506759485,
-            "y": 3.6814377878761273,
-            "heading": 2.890755576183656,
-            "angularVelocity": 6.378036992489158e-8,
-            "velocityX": 3.733736272143269,
-            "velocityY": -0.6081548715728616,
-            "timestamp": 1.3008480998209648
-        },
-        {
-            "x": 6.793655691240644,
-            "y": 3.6853799559796676,
-            "heading": 2.8907555914201803,
-            "angularVelocity": 1.756914361676573e-7,
-            "velocityX": 3.7826673299885396,
-            "velocityY": 0.045456899665096306,
-            "timestamp": 1.3875713064756958
-        },
-        {
-            "x": 7.092289943250509,
-            "y": 3.7425375783401047,
-            "heading": 2.945166528243714,
-            "angularVelocity": 0.6274091897932109,
-            "velocityX": 3.4435333231946736,
-            "velocityY": 0.6590810529873269,
-            "timestamp": 1.4742945131304268
-        },
-        {
-            "x": 7.330977190762065,
-            "y": 3.8181008616818835,
-            "heading": 3.0010867643989263,
-            "angularVelocity": 0.6448128282182443,
-            "velocityX": 2.7522880751150582,
-            "velocityY": 0.8713156057826263,
-            "timestamp": 1.5610177197851578
-        },
-        {
-            "x": 7.508670206250799,
-            "y": 3.908197537676412,
-            "heading": 3.053304413172006,
-            "angularVelocity": 0.6021185192211955,
-            "velocityX": 2.0489673104014514,
-            "velocityY": 1.0388992689491814,
-            "timestamp": 1.6477409264398888
-        },
-        {
-            "x": 7.6250594852295395,
-            "y": 4.011408797557566,
-            "heading": 3.099932944450418,
-            "angularVelocity": 0.5376707467014636,
-            "velocityX": 1.342077668346812,
-            "velocityY": 1.1901227348761163,
-            "timestamp": 1.7344641330946198
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.4620107707627286,
-            "velocityX": 0.6335157207596422,
-            "velocityY": 1.332875096542895,
-            "timestamp": 1.8211873397493508
-        },
-        {
-            "x": 7.66415878383622,
-            "y": 4.271751344750917,
-            "heading": 3.1759557228740287,
-            "angularVelocity": 0.3694329307480838,
-            "velocityX": -0.16276315552044252,
-            "velocityY": 1.487271267174521,
-            "timestamp": 1.918514134091254
-        },
-        {
-            "x": 7.570652845333858,
-            "y": 4.430649041463004,
-            "heading": 3.2028903885273214,
-            "angularVelocity": 0.2767446090814936,
-            "velocityX": -0.9607419943770155,
-            "velocityY": 1.6326202644041503,
-            "timestamp": 2.015840928433157
-        },
-        {
-            "x": 7.399206149109808,
-            "y": 4.602082311613017,
-            "heading": 3.2207860785976568,
-            "angularVelocity": 0.1838721822839176,
-            "velocityX": -1.761557003734937,
-            "velocityY": 1.7614190553505542,
-            "timestamp": 2.11316772277506
-        },
-        {
-            "x": 7.149293090301517,
-            "y": 4.782169514035782,
-            "heading": 3.229614246279924,
-            "angularVelocity": 0.09070644668783194,
-            "velocityX": -2.5677724258579846,
-            "velocityY": 1.8503352919456928,
-            "timestamp": 2.210494517116963
-        },
-        {
-            "x": 6.821439370412565,
-            "y": 4.949709126664123,
-            "heading": 3.229614240242725,
-            "angularVelocity": -6.203018521599257e-8,
-            "velocityX": -3.3685864422619463,
-            "velocityY": 1.7214130369873843,
-            "timestamp": 2.3078213114588664
-        },
-        {
-            "x": 6.465396251393308,
-            "y": 5.04346891931761,
-            "heading": 3.2296142362544686,
-            "angularVelocity": -4.0977992187262516e-8,
-            "velocityX": -3.658223014810281,
-            "velocityY": 0.9633502601977828,
-            "timestamp": 2.4051481058007695
-        },
-        {
-            "x": 6.106941254304619,
-            "y": 5.12753803654929,
-            "heading": 3.2296142322922576,
-            "angularVelocity": -4.071038102398141e-8,
-            "velocityX": -3.6830042488552306,
-            "velocityY": 0.8637818372641555,
-            "timestamp": 2.5024749001426727
-        },
-        {
-            "x": 5.7484862494466205,
-            "y": 5.211607120654073,
-            "heading": 3.2296142283300466,
-            "angularVelocity": -4.071038072165154e-8,
-            "velocityX": -3.6830043286822827,
-            "velocityY": 0.8637814968964618,
-            "timestamp": 2.599801694484576
-        },
-        {
-            "x": 5.390031244588595,
-            "y": 5.295676204758746,
-            "heading": 3.229614224367836,
-            "angularVelocity": -4.0710380903709755e-8,
-            "velocityX": -3.683004328682549,
-            "velocityY": 0.8637814968953244,
-            "timestamp": 2.697128488826479
-        },
-        {
-            "x": 5.031576239716237,
-            "y": 5.3797452888021455,
-            "heading": 3.2296142204055998,
-            "angularVelocity": -4.071064154144793e-8,
-            "velocityX": -3.683004328829807,
-            "velocityY": 0.8637814962657624,
-            "timestamp": 2.794455283168382
-        },
-        {
-            "x": 4.7241457561516045,
-            "y": 5.451847166184253,
-            "heading": 3.193769184032689,
-            "angularVelocity": -0.3682956642649294,
-            "velocityX": -3.1587445743322107,
-            "velocityY": 0.740822482335317,
-            "timestamp": 2.8917820775102854
-        },
-        {
-            "x": 4.493572881554858,
-            "y": 5.505923581174423,
-            "heading": 3.1668847241449476,
-            "angularVelocity": -0.27622876176636874,
-            "velocityX": -2.3690585532567625,
-            "velocityY": 0.5556169331972818,
-            "timestamp": 2.9891088718521885
-        },
-        {
-            "x": 4.3398576283261585,
-            "y": 5.541974526790158,
-            "heading": 3.1489615588617923,
-            "angularVelocity": -0.1841544808328986,
-            "velocityX": -1.5793724047738293,
-            "velocityY": 0.37041131231641944,
-            "timestamp": 3.0864356661940917
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -0.09207699608717179,
-            "velocityX": -0.7896862199751749,
-            "velocityY": 0.18520566028835728,
-            "timestamp": 3.183762460535995
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": -4.104369359528437e-34,
-            "velocityY": 0,
-            "timestamp": 3.281089254877898
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S2.traj
deleted file mode 100644
index 28b5f4d2..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S2.traj	
+++ /dev/null
@@ -1,337 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.361838792645819,
-            "y": 4.349925567383164,
-            "heading": 2.7594883701133286,
-            "angularVelocity": 0.10940981634945876,
-            "velocityX": 0.7130593416825138,
-            "velocityY": -0.11616766728823658,
-            "timestamp": 0.08672320665473099
-        },
-        {
-            "x": 2.485516776956774,
-            "y": 4.329776279587658,
-            "heading": 2.7784262463080607,
-            "angularVelocity": 0.21837149392012115,
-            "velocityX": 1.4261232844324094,
-            "velocityY": -0.2323402071111933,
-            "timestamp": 0.17344641330946198
-        },
-        {
-            "x": 2.671034614420037,
-            "y": 4.299551431366552,
-            "heading": 2.806748673224486,
-            "angularVelocity": 0.3265841752046417,
-            "velocityX": 2.1391948547504818,
-            "velocityY": -0.34852087909340246,
-            "timestamp": 0.26016961996419297
-        },
-        {
-            "x": 2.9183936206497236,
-            "y": 4.259249598753385,
-            "heading": 2.8443244197137543,
-            "angularVelocity": 0.4332836381257234,
-            "velocityX": 2.852281595334577,
-            "velocityY": -0.46471797074593596,
-            "timestamp": 0.34689282661892396
-        },
-        {
-            "x": 3.2275976397156323,
-            "y": 4.208866459419414,
-            "heading": 2.8907555208713065,
-            "angularVelocity": 0.5353941920351031,
-            "velocityX": 3.5654126616527764,
-            "velocityY": -0.5809649029072455,
-            "timestamp": 0.43361603327365494
-        },
-        {
-            "x": 3.5513989095488143,
-            "y": 4.156123400332019,
-            "heading": 2.8907555264025566,
-            "angularVelocity": 6.378050766718983e-8,
-            "velocityX": 3.7337326688382726,
-            "velocityY": -0.6081769934704949,
-            "timestamp": 0.5203392399283859
-        },
-        {
-            "x": 3.8752001793990973,
-            "y": 4.103380341349552,
-            "heading": 2.8907555319337894,
-            "angularVelocity": 6.378030392692821e-8,
-            "velocityX": 3.7337326690354664,
-            "velocityY": -0.6081769922605936,
-            "timestamp": 0.6070624465831169
-        },
-        {
-            "x": 4.19900144924938,
-            "y": 4.050637282367084,
-            "heading": 2.890755537465022,
-            "angularVelocity": 6.378030384357619e-8,
-            "velocityX": 3.7337326690354677,
-            "velocityY": -0.6081769922605877,
-            "timestamp": 0.6937856532378479
-        },
-        {
-            "x": 4.522802719099664,
-            "y": 3.9978942233846166,
-            "heading": 2.8907555429962546,
-            "angularVelocity": 6.378030394841341e-8,
-            "velocityX": 3.7337326690354673,
-            "velocityY": -0.6081769922605876,
-            "timestamp": 0.7805088598925789
-        },
-        {
-            "x": 4.846603988949947,
-            "y": 3.9451511644021493,
-            "heading": 2.8907555485274874,
-            "angularVelocity": 6.37803046219163e-8,
-            "velocityX": 3.7337326690354673,
-            "velocityY": -0.6081769922605877,
-            "timestamp": 0.8672320665473099
-        },
-        {
-            "x": 5.170405258800231,
-            "y": 3.8924081054196815,
-            "heading": 2.8907555540587198,
-            "angularVelocity": 6.37803035811712e-8,
-            "velocityX": 3.7337326690354673,
-            "velocityY": -0.6081769922605876,
-            "timestamp": 0.9539552732020409
-        },
-        {
-            "x": 5.494206528650514,
-            "y": 3.839665046437214,
-            "heading": 2.8907555595899526,
-            "angularVelocity": 6.378030493154673e-8,
-            "velocityX": 3.7337326690354673,
-            "velocityY": -0.6081769922605876,
-            "timestamp": 1.0406784798567719
-        },
-        {
-            "x": 5.818007798500798,
-            "y": 3.7869219874547464,
-            "heading": 2.8907555651211854,
-            "angularVelocity": 6.378030454431603e-8,
-            "velocityX": 3.7337326690354677,
-            "velocityY": -0.6081769922605872,
-            "timestamp": 1.1274016865115029
-        },
-        {
-            "x": 6.141809068352602,
-            "y": 3.734178928481622,
-            "heading": 2.8907555706524177,
-            "angularVelocity": 6.378030476763932e-8,
-            "velocityX": 3.7337326690530155,
-            "velocityY": -0.608176992152858,
-            "timestamp": 1.2141248931662338
-        },
-        {
-            "x": 6.4656106506759485,
-            "y": 3.6814377878761273,
-            "heading": 2.890755576183656,
-            "angularVelocity": 6.378036992489158e-8,
-            "velocityX": 3.733736272143269,
-            "velocityY": -0.6081548715728616,
-            "timestamp": 1.3008480998209648
-        },
-        {
-            "x": 6.793655691240644,
-            "y": 3.6853799559796676,
-            "heading": 2.8907555914201803,
-            "angularVelocity": 1.756914361676573e-7,
-            "velocityX": 3.7826673299885396,
-            "velocityY": 0.045456899665096306,
-            "timestamp": 1.3875713064756958
-        },
-        {
-            "x": 7.092289943250509,
-            "y": 3.7425375783401047,
-            "heading": 2.945166528243714,
-            "angularVelocity": 0.6274091897932109,
-            "velocityX": 3.4435333231946736,
-            "velocityY": 0.6590810529873269,
-            "timestamp": 1.4742945131304268
-        },
-        {
-            "x": 7.330977190762065,
-            "y": 3.8181008616818835,
-            "heading": 3.0010867643989263,
-            "angularVelocity": 0.6448128282182443,
-            "velocityX": 2.7522880751150582,
-            "velocityY": 0.8713156057826263,
-            "timestamp": 1.5610177197851578
-        },
-        {
-            "x": 7.508670206250799,
-            "y": 3.908197537676412,
-            "heading": 3.053304413172006,
-            "angularVelocity": 0.6021185192211955,
-            "velocityX": 2.0489673104014514,
-            "velocityY": 1.0388992689491814,
-            "timestamp": 1.6477409264398888
-        },
-        {
-            "x": 7.6250594852295395,
-            "y": 4.011408797557566,
-            "heading": 3.099932944450418,
-            "angularVelocity": 0.5376707467014636,
-            "velocityX": 1.342077668346812,
-            "velocityY": 1.1901227348761163,
-            "timestamp": 1.7344641330946198
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.4620107707627286,
-            "velocityX": 0.6335157207596422,
-            "velocityY": 1.332875096542895,
-            "timestamp": 1.8211873397493508
-        },
-        {
-            "x": 7.66415878383622,
-            "y": 4.271751344750917,
-            "heading": 3.1759557228740287,
-            "angularVelocity": 0.3694329307480838,
-            "velocityX": -0.16276315552044252,
-            "velocityY": 1.487271267174521,
-            "timestamp": 1.918514134091254
-        },
-        {
-            "x": 7.570652845333858,
-            "y": 4.430649041463004,
-            "heading": 3.2028903885273214,
-            "angularVelocity": 0.2767446090814936,
-            "velocityX": -0.9607419943770155,
-            "velocityY": 1.6326202644041503,
-            "timestamp": 2.015840928433157
-        },
-        {
-            "x": 7.399206149109808,
-            "y": 4.602082311613017,
-            "heading": 3.2207860785976568,
-            "angularVelocity": 0.1838721822839176,
-            "velocityX": -1.761557003734937,
-            "velocityY": 1.7614190553505542,
-            "timestamp": 2.11316772277506
-        },
-        {
-            "x": 7.149293090301517,
-            "y": 4.782169514035782,
-            "heading": 3.229614246279924,
-            "angularVelocity": 0.09070644668783194,
-            "velocityX": -2.5677724258579846,
-            "velocityY": 1.8503352919456928,
-            "timestamp": 2.210494517116963
-        },
-        {
-            "x": 6.821439370412565,
-            "y": 4.949709126664123,
-            "heading": 3.229614240242725,
-            "angularVelocity": -6.203018521599257e-8,
-            "velocityX": -3.3685864422619463,
-            "velocityY": 1.7214130369873843,
-            "timestamp": 2.3078213114588664
-        },
-        {
-            "x": 6.465396251393308,
-            "y": 5.04346891931761,
-            "heading": 3.2296142362544686,
-            "angularVelocity": -4.0977992187262516e-8,
-            "velocityX": -3.658223014810281,
-            "velocityY": 0.9633502601977828,
-            "timestamp": 2.4051481058007695
-        },
-        {
-            "x": 6.106941254304619,
-            "y": 5.12753803654929,
-            "heading": 3.2296142322922576,
-            "angularVelocity": -4.071038102398141e-8,
-            "velocityX": -3.6830042488552306,
-            "velocityY": 0.8637818372641555,
-            "timestamp": 2.5024749001426727
-        },
-        {
-            "x": 5.7484862494466205,
-            "y": 5.211607120654073,
-            "heading": 3.2296142283300466,
-            "angularVelocity": -4.071038072165154e-8,
-            "velocityX": -3.6830043286822827,
-            "velocityY": 0.8637814968964618,
-            "timestamp": 2.599801694484576
-        },
-        {
-            "x": 5.390031244588595,
-            "y": 5.295676204758746,
-            "heading": 3.229614224367836,
-            "angularVelocity": -4.0710380903709755e-8,
-            "velocityX": -3.683004328682549,
-            "velocityY": 0.8637814968953244,
-            "timestamp": 2.697128488826479
-        },
-        {
-            "x": 5.031576239716237,
-            "y": 5.3797452888021455,
-            "heading": 3.2296142204055998,
-            "angularVelocity": -4.071064154144793e-8,
-            "velocityX": -3.683004328829807,
-            "velocityY": 0.8637814962657624,
-            "timestamp": 2.794455283168382
-        },
-        {
-            "x": 4.7241457561516045,
-            "y": 5.451847166184253,
-            "heading": 3.193769184032689,
-            "angularVelocity": -0.3682956642649294,
-            "velocityX": -3.1587445743322107,
-            "velocityY": 0.740822482335317,
-            "timestamp": 2.8917820775102854
-        },
-        {
-            "x": 4.493572881554858,
-            "y": 5.505923581174423,
-            "heading": 3.1668847241449476,
-            "angularVelocity": -0.27622876176636874,
-            "velocityX": -2.3690585532567625,
-            "velocityY": 0.5556169331972818,
-            "timestamp": 2.9891088718521885
-        },
-        {
-            "x": 4.3398576283261585,
-            "y": 5.541974526790158,
-            "heading": 3.1489615588617923,
-            "angularVelocity": -0.1841544808328986,
-            "velocityX": -1.5793724047738293,
-            "velocityY": 0.37041131231641944,
-            "timestamp": 3.0864356661940917
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -0.09207699608717179,
-            "velocityX": -0.7896862199751749,
-            "velocityY": 0.18520566028835728,
-            "timestamp": 3.183762460535995
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": -4.104369359528437e-34,
-            "velocityY": 0,
-            "timestamp": 3.281089254877898
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S3.1.traj
deleted file mode 100644
index dadc5c59..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S3.1.traj	
+++ /dev/null
@@ -1,355 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.362347004549993,
-            "y": 4.364661246321825,
-            "heading": 2.7702286217513103,
-            "angularVelocity": 0.23313109188524597,
-            "velocityX": 0.7185375961450525,
-            "velocityY": 0.05371999426914206,
-            "timestamp": 0.0867693004297674
-        },
-        {
-            "x": 2.4870912612086284,
-            "y": 4.373976445805142,
-            "heading": 2.8086286090699177,
-            "angularVelocity": 0.4425526900460507,
-            "velocityX": 1.4376542860294879,
-            "velocityY": 0.10735593622605764,
-            "timestamp": 0.1735386008595348
-        },
-        {
-            "x": 2.674306466073838,
-            "y": 4.3879313037176075,
-            "heading": 2.8616727700685,
-            "angularVelocity": 0.6113240597291352,
-            "velocityX": 2.1576203096940354,
-            "velocityY": 0.16082713405947677,
-            "timestamp": 0.2603079012893022
-        },
-        {
-            "x": 2.9240965513714046,
-            "y": 4.40648950041988,
-            "heading": 2.921990092412666,
-            "angularVelocity": 0.6951458873750875,
-            "velocityX": 2.878784132871411,
-            "velocityY": 0.2138797548251989,
-            "timestamp": 0.3470772017190696
-        },
-        {
-            "x": 3.236315453938978,
-            "y": 4.429486199889588,
-            "heading": 2.9648285300542776,
-            "angularVelocity": 0.493705001993024,
-            "velocityX": 3.598264605351849,
-            "velocityY": 0.26503267118445795,
-            "timestamp": 0.43384650214883697
-        },
-        {
-            "x": 3.563623741097209,
-            "y": 4.454241375249833,
-            "heading": 2.9648285457681323,
-            "angularVelocity": 1.810992388928579e-7,
-            "velocityX": 3.7721669477231816,
-            "velocityY": 0.28529877776624385,
-            "timestamp": 0.5206158025786044
-        },
-        {
-            "x": 3.8909320279988893,
-            "y": 4.478996554002297,
-            "heading": 2.9648285614819643,
-            "angularVelocity": 1.8109898446503704e-7,
-            "velocityX": 3.7721669447664756,
-            "velocityY": 0.28529881686094194,
-            "timestamp": 0.6073851030083718
-        },
-        {
-            "x": 4.218240314900568,
-            "y": 4.503751732754777,
-            "heading": 2.9648285771957963,
-            "angularVelocity": 1.810989844949235e-7,
-            "velocityX": 3.772166944766461,
-            "velocityY": 0.2852988168611311,
-            "timestamp": 0.6941544034381392
-        },
-        {
-            "x": 4.545548601802246,
-            "y": 4.528506911507257,
-            "heading": 2.9648285929096283,
-            "angularVelocity": 1.810989846493643e-7,
-            "velocityX": 3.7721669447664614,
-            "velocityY": 0.2852988168611311,
-            "timestamp": 0.7809237038679065
-        },
-        {
-            "x": 4.872856888703925,
-            "y": 4.5532620902597385,
-            "heading": 2.9648286086234608,
-            "angularVelocity": 1.8109898537942274e-7,
-            "velocityX": 3.7721669447664614,
-            "velocityY": 0.28529881686113107,
-            "timestamp": 0.8676930042976739
-        },
-        {
-            "x": 5.200165175605603,
-            "y": 4.578017269012219,
-            "heading": 2.964828624337293,
-            "angularVelocity": 1.8109898550822278e-7,
-            "velocityX": 3.7721669447664614,
-            "velocityY": 0.28529881686113095,
-            "timestamp": 0.9544623047274413
-        },
-        {
-            "x": 5.527473462507282,
-            "y": 4.602772447764699,
-            "heading": 2.9648286400511257,
-            "angularVelocity": 1.8109898581520642e-7,
-            "velocityX": 3.772166944766462,
-            "velocityY": 0.2852988168611311,
-            "timestamp": 1.0412316051572088
-        },
-        {
-            "x": 5.854781749408961,
-            "y": 4.627527626517179,
-            "heading": 2.964828655764958,
-            "angularVelocity": 1.8109898550546543e-7,
-            "velocityX": 3.772166944766461,
-            "velocityY": 0.28529881686113034,
-            "timestamp": 1.1280009055869762
-        },
-        {
-            "x": 6.182090036311611,
-            "y": 4.652282805256812,
-            "heading": 2.9648286714787906,
-            "angularVelocity": 1.810989852504463e-7,
-            "velocityX": 3.772166944777661,
-            "velocityY": 0.2852988167130518,
-            "timestamp": 1.2147702060167436
-        },
-        {
-            "x": 6.509398523986194,
-            "y": 4.6770353292870315,
-            "heading": 2.964828687192649,
-            "angularVelocity": 1.8109928558732558e-7,
-            "velocityX": 3.7721692586367297,
-            "velocityY": 0.2852682216823196,
-            "timestamp": 1.301539506446511
-        },
-        {
-            "x": 6.835912518328979,
-            "y": 4.643388028658291,
-            "heading": 2.964828718989663,
-            "angularVelocity": 3.6645465598978293e-7,
-            "velocityX": 3.763012871206354,
-            "velocityY": -0.38777886259409694,
-            "timestamp": 1.3883088068762783
-        },
-        {
-            "x": 7.128079325417914,
-            "y": 4.560807160262635,
-            "heading": 3.033554953244841,
-            "angularVelocity": 0.7920570284049364,
-            "velocityX": 3.3671679458268646,
-            "velocityY": -0.9517291021897464,
-            "timestamp": 1.4750781073060457
-        },
-        {
-            "x": 7.358963687834594,
-            "y": 4.4649781472961925,
-            "heading": 3.091042362636633,
-            "angularVelocity": 0.66253166854011,
-            "velocityX": 2.660899203671262,
-            "velocityY": -1.1044114968289644,
-            "timestamp": 1.561847407735813
-        },
-        {
-            "x": 7.527984669479397,
-            "y": 4.3598798773022285,
-            "heading": 3.1296058219303373,
-            "angularVelocity": 0.44443667406213694,
-            "velocityX": 1.947935281345401,
-            "velocityY": -1.211237954822889,
-            "timestamp": 1.6486167081655805
-        },
-        {
-            "x": 7.635013269633329,
-            "y": 4.246969115663087,
-            "heading": 3.1464129858106427,
-            "angularVelocity": 0.19369942822011468,
-            "velocityX": 1.2334846497991885,
-            "velocityY": -1.3012754635555983,
-            "timestamp": 1.7353860085953479
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": -0.07390846507784325,
-            "velocityX": 0.5184636748694783,
-            "velocityY": -1.3826216768935726,
-            "timestamp": 1.8221553090251152
-        },
-        {
-            "x": 7.665105053681803,
-            "y": 4.004188436329708,
-            "heading": 3.1110105191844553,
-            "angularVelocity": -0.3438049921625736,
-            "velocityX": -0.17664879667123173,
-            "velocityY": -1.4565017205308959,
-            "timestamp": 1.9064748549200063
-        },
-        {
-            "x": 7.591499034942275,
-            "y": 3.875650129704607,
-            "heading": 3.0609451613914906,
-            "angularVelocity": -0.5937574409541295,
-            "velocityX": -0.8729413561036249,
-            "velocityY": -1.524418867071828,
-            "timestamp": 1.9907944008148974
-        },
-        {
-            "x": 7.459038327956553,
-            "y": 3.7422011819159797,
-            "heading": 2.9924948430687532,
-            "angularVelocity": -0.8117965721502389,
-            "velocityX": -1.5709371484381591,
-            "velocityY": -1.582657334931321,
-            "timestamp": 2.0751139467097883
-        },
-        {
-            "x": 7.267508984071126,
-            "y": 3.6053961445739597,
-            "heading": 2.9106651455038604,
-            "angularVelocity": -0.9704712791847574,
-            "velocityX": -2.271470296153853,
-            "velocityY": -1.6224593703641987,
-            "timestamp": 2.159433492604679
-        },
-        {
-            "x": 7.016701982610521,
-            "y": 3.4693216018432858,
-            "heading": 2.828330372603407,
-            "angularVelocity": -0.9764612940763276,
-            "velocityX": -2.9744823551736075,
-            "velocityY": -1.6137959625671907,
-            "timestamp": 2.24375303849957
-        },
-        {
-            "x": 6.717326936237698,
-            "y": 3.359230054214691,
-            "heading": 2.828330320326688,
-            "angularVelocity": -6.199834052808441e-7,
-            "velocityX": -3.5504821947927914,
-            "velocityY": -1.3056468279114077,
-            "timestamp": 2.328072584394461
-        },
-        {
-            "x": 6.402750279383998,
-            "y": 3.3064370272518535,
-            "heading": 2.8283303027221653,
-            "angularVelocity": -2.0878341406433196e-7,
-            "velocityX": -3.730767920000857,
-            "velocityY": -0.6261066328398724,
-            "timestamp": 2.412392130289352
-        },
-        {
-            "x": 6.0870128630404565,
-            "y": 3.261099838136822,
-            "heading": 2.8283302851918997,
-            "angularVelocity": -2.0790275245783378e-7,
-            "velocityX": -3.7445341171206636,
-            "velocityY": -0.5376830322538348,
-            "timestamp": 2.4967116761842427
-        },
-        {
-            "x": 5.771275440975781,
-            "y": 3.215762688864957,
-            "heading": 2.8283302676616344,
-            "angularVelocity": -2.079027527224642e-7,
-            "velocityX": -3.7445341849713016,
-            "velocityY": -0.5376825597279692,
-            "timestamp": 2.5810312220791336
-        },
-        {
-            "x": 5.455538018911073,
-            "y": 3.1704255395933156,
-            "heading": 2.8283302501313687,
-            "angularVelocity": -2.079027528417884e-7,
-            "velocityX": -3.7445341849716827,
-            "velocityY": -0.5376825597253148,
-            "timestamp": 2.6653507679740245
-        },
-        {
-            "x": 5.13980059641411,
-            "y": 3.1250883933321525,
-            "heading": 2.8283302326010555,
-            "angularVelocity": -2.0790331306647263e-7,
-            "velocityX": -3.7445341900980704,
-            "velocityY": -0.5376825240221068,
-            "timestamp": 2.7496703138689154
-        },
-        {
-            "x": 4.847289842857696,
-            "y": 3.0834618283006696,
-            "heading": 2.7687586646684967,
-            "angularVelocity": -0.706497731935343,
-            "velocityX": -3.469074109116353,
-            "velocityY": -0.4936763426521838,
-            "timestamp": 2.8339898597638062
-        },
-        {
-            "x": 4.613405710202319,
-            "y": 3.0500832751446354,
-            "heading": 2.6983183223854565,
-            "angularVelocity": -0.8353975526724018,
-            "velocityX": -2.773783114853639,
-            "velocityY": -0.39585783820090814,
-            "timestamp": 2.918309405658697
-        },
-        {
-            "x": 4.43813741968374,
-            "y": 3.025042089065369,
-            "heading": 2.6386185119236694,
-            "angularVelocity": -0.7080186429870714,
-            "velocityX": -2.0786199529236167,
-            "velocityY": -0.29697961265685946,
-            "timestamp": 3.002628951553588
-        },
-        {
-            "x": 4.321361759336474,
-            "y": 3.008347212158301,
-            "heading": 2.596145764785128,
-            "angularVelocity": -0.5037117632427286,
-            "velocityX": -1.3849180413380506,
-            "velocityY": -0.19799533702279348,
-            "timestamp": 3.086948497448479
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -0.26264093989232323,
-            "velocityX": -0.6921498297586374,
-            "velocityY": -0.0989949847299469,
-            "timestamp": 3.1712680433433698
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": -1.507480653475374e-32,
-            "velocityY": 0,
-            "timestamp": 3.2555875892382606
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S3.traj
deleted file mode 100644
index dadc5c59..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S3.traj	
+++ /dev/null
@@ -1,355 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.362347004549993,
-            "y": 4.364661246321825,
-            "heading": 2.7702286217513103,
-            "angularVelocity": 0.23313109188524597,
-            "velocityX": 0.7185375961450525,
-            "velocityY": 0.05371999426914206,
-            "timestamp": 0.0867693004297674
-        },
-        {
-            "x": 2.4870912612086284,
-            "y": 4.373976445805142,
-            "heading": 2.8086286090699177,
-            "angularVelocity": 0.4425526900460507,
-            "velocityX": 1.4376542860294879,
-            "velocityY": 0.10735593622605764,
-            "timestamp": 0.1735386008595348
-        },
-        {
-            "x": 2.674306466073838,
-            "y": 4.3879313037176075,
-            "heading": 2.8616727700685,
-            "angularVelocity": 0.6113240597291352,
-            "velocityX": 2.1576203096940354,
-            "velocityY": 0.16082713405947677,
-            "timestamp": 0.2603079012893022
-        },
-        {
-            "x": 2.9240965513714046,
-            "y": 4.40648950041988,
-            "heading": 2.921990092412666,
-            "angularVelocity": 0.6951458873750875,
-            "velocityX": 2.878784132871411,
-            "velocityY": 0.2138797548251989,
-            "timestamp": 0.3470772017190696
-        },
-        {
-            "x": 3.236315453938978,
-            "y": 4.429486199889588,
-            "heading": 2.9648285300542776,
-            "angularVelocity": 0.493705001993024,
-            "velocityX": 3.598264605351849,
-            "velocityY": 0.26503267118445795,
-            "timestamp": 0.43384650214883697
-        },
-        {
-            "x": 3.563623741097209,
-            "y": 4.454241375249833,
-            "heading": 2.9648285457681323,
-            "angularVelocity": 1.810992388928579e-7,
-            "velocityX": 3.7721669477231816,
-            "velocityY": 0.28529877776624385,
-            "timestamp": 0.5206158025786044
-        },
-        {
-            "x": 3.8909320279988893,
-            "y": 4.478996554002297,
-            "heading": 2.9648285614819643,
-            "angularVelocity": 1.8109898446503704e-7,
-            "velocityX": 3.7721669447664756,
-            "velocityY": 0.28529881686094194,
-            "timestamp": 0.6073851030083718
-        },
-        {
-            "x": 4.218240314900568,
-            "y": 4.503751732754777,
-            "heading": 2.9648285771957963,
-            "angularVelocity": 1.810989844949235e-7,
-            "velocityX": 3.772166944766461,
-            "velocityY": 0.2852988168611311,
-            "timestamp": 0.6941544034381392
-        },
-        {
-            "x": 4.545548601802246,
-            "y": 4.528506911507257,
-            "heading": 2.9648285929096283,
-            "angularVelocity": 1.810989846493643e-7,
-            "velocityX": 3.7721669447664614,
-            "velocityY": 0.2852988168611311,
-            "timestamp": 0.7809237038679065
-        },
-        {
-            "x": 4.872856888703925,
-            "y": 4.5532620902597385,
-            "heading": 2.9648286086234608,
-            "angularVelocity": 1.8109898537942274e-7,
-            "velocityX": 3.7721669447664614,
-            "velocityY": 0.28529881686113107,
-            "timestamp": 0.8676930042976739
-        },
-        {
-            "x": 5.200165175605603,
-            "y": 4.578017269012219,
-            "heading": 2.964828624337293,
-            "angularVelocity": 1.8109898550822278e-7,
-            "velocityX": 3.7721669447664614,
-            "velocityY": 0.28529881686113095,
-            "timestamp": 0.9544623047274413
-        },
-        {
-            "x": 5.527473462507282,
-            "y": 4.602772447764699,
-            "heading": 2.9648286400511257,
-            "angularVelocity": 1.8109898581520642e-7,
-            "velocityX": 3.772166944766462,
-            "velocityY": 0.2852988168611311,
-            "timestamp": 1.0412316051572088
-        },
-        {
-            "x": 5.854781749408961,
-            "y": 4.627527626517179,
-            "heading": 2.964828655764958,
-            "angularVelocity": 1.8109898550546543e-7,
-            "velocityX": 3.772166944766461,
-            "velocityY": 0.28529881686113034,
-            "timestamp": 1.1280009055869762
-        },
-        {
-            "x": 6.182090036311611,
-            "y": 4.652282805256812,
-            "heading": 2.9648286714787906,
-            "angularVelocity": 1.810989852504463e-7,
-            "velocityX": 3.772166944777661,
-            "velocityY": 0.2852988167130518,
-            "timestamp": 1.2147702060167436
-        },
-        {
-            "x": 6.509398523986194,
-            "y": 4.6770353292870315,
-            "heading": 2.964828687192649,
-            "angularVelocity": 1.8109928558732558e-7,
-            "velocityX": 3.7721692586367297,
-            "velocityY": 0.2852682216823196,
-            "timestamp": 1.301539506446511
-        },
-        {
-            "x": 6.835912518328979,
-            "y": 4.643388028658291,
-            "heading": 2.964828718989663,
-            "angularVelocity": 3.6645465598978293e-7,
-            "velocityX": 3.763012871206354,
-            "velocityY": -0.38777886259409694,
-            "timestamp": 1.3883088068762783
-        },
-        {
-            "x": 7.128079325417914,
-            "y": 4.560807160262635,
-            "heading": 3.033554953244841,
-            "angularVelocity": 0.7920570284049364,
-            "velocityX": 3.3671679458268646,
-            "velocityY": -0.9517291021897464,
-            "timestamp": 1.4750781073060457
-        },
-        {
-            "x": 7.358963687834594,
-            "y": 4.4649781472961925,
-            "heading": 3.091042362636633,
-            "angularVelocity": 0.66253166854011,
-            "velocityX": 2.660899203671262,
-            "velocityY": -1.1044114968289644,
-            "timestamp": 1.561847407735813
-        },
-        {
-            "x": 7.527984669479397,
-            "y": 4.3598798773022285,
-            "heading": 3.1296058219303373,
-            "angularVelocity": 0.44443667406213694,
-            "velocityX": 1.947935281345401,
-            "velocityY": -1.211237954822889,
-            "timestamp": 1.6486167081655805
-        },
-        {
-            "x": 7.635013269633329,
-            "y": 4.246969115663087,
-            "heading": 3.1464129858106427,
-            "angularVelocity": 0.19369942822011468,
-            "velocityX": 1.2334846497991885,
-            "velocityY": -1.3012754635555983,
-            "timestamp": 1.7353860085953479
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": -0.07390846507784325,
-            "velocityX": 0.5184636748694783,
-            "velocityY": -1.3826216768935726,
-            "timestamp": 1.8221553090251152
-        },
-        {
-            "x": 7.665105053681803,
-            "y": 4.004188436329708,
-            "heading": 3.1110105191844553,
-            "angularVelocity": -0.3438049921625736,
-            "velocityX": -0.17664879667123173,
-            "velocityY": -1.4565017205308959,
-            "timestamp": 1.9064748549200063
-        },
-        {
-            "x": 7.591499034942275,
-            "y": 3.875650129704607,
-            "heading": 3.0609451613914906,
-            "angularVelocity": -0.5937574409541295,
-            "velocityX": -0.8729413561036249,
-            "velocityY": -1.524418867071828,
-            "timestamp": 1.9907944008148974
-        },
-        {
-            "x": 7.459038327956553,
-            "y": 3.7422011819159797,
-            "heading": 2.9924948430687532,
-            "angularVelocity": -0.8117965721502389,
-            "velocityX": -1.5709371484381591,
-            "velocityY": -1.582657334931321,
-            "timestamp": 2.0751139467097883
-        },
-        {
-            "x": 7.267508984071126,
-            "y": 3.6053961445739597,
-            "heading": 2.9106651455038604,
-            "angularVelocity": -0.9704712791847574,
-            "velocityX": -2.271470296153853,
-            "velocityY": -1.6224593703641987,
-            "timestamp": 2.159433492604679
-        },
-        {
-            "x": 7.016701982610521,
-            "y": 3.4693216018432858,
-            "heading": 2.828330372603407,
-            "angularVelocity": -0.9764612940763276,
-            "velocityX": -2.9744823551736075,
-            "velocityY": -1.6137959625671907,
-            "timestamp": 2.24375303849957
-        },
-        {
-            "x": 6.717326936237698,
-            "y": 3.359230054214691,
-            "heading": 2.828330320326688,
-            "angularVelocity": -6.199834052808441e-7,
-            "velocityX": -3.5504821947927914,
-            "velocityY": -1.3056468279114077,
-            "timestamp": 2.328072584394461
-        },
-        {
-            "x": 6.402750279383998,
-            "y": 3.3064370272518535,
-            "heading": 2.8283303027221653,
-            "angularVelocity": -2.0878341406433196e-7,
-            "velocityX": -3.730767920000857,
-            "velocityY": -0.6261066328398724,
-            "timestamp": 2.412392130289352
-        },
-        {
-            "x": 6.0870128630404565,
-            "y": 3.261099838136822,
-            "heading": 2.8283302851918997,
-            "angularVelocity": -2.0790275245783378e-7,
-            "velocityX": -3.7445341171206636,
-            "velocityY": -0.5376830322538348,
-            "timestamp": 2.4967116761842427
-        },
-        {
-            "x": 5.771275440975781,
-            "y": 3.215762688864957,
-            "heading": 2.8283302676616344,
-            "angularVelocity": -2.079027527224642e-7,
-            "velocityX": -3.7445341849713016,
-            "velocityY": -0.5376825597279692,
-            "timestamp": 2.5810312220791336
-        },
-        {
-            "x": 5.455538018911073,
-            "y": 3.1704255395933156,
-            "heading": 2.8283302501313687,
-            "angularVelocity": -2.079027528417884e-7,
-            "velocityX": -3.7445341849716827,
-            "velocityY": -0.5376825597253148,
-            "timestamp": 2.6653507679740245
-        },
-        {
-            "x": 5.13980059641411,
-            "y": 3.1250883933321525,
-            "heading": 2.8283302326010555,
-            "angularVelocity": -2.0790331306647263e-7,
-            "velocityX": -3.7445341900980704,
-            "velocityY": -0.5376825240221068,
-            "timestamp": 2.7496703138689154
-        },
-        {
-            "x": 4.847289842857696,
-            "y": 3.0834618283006696,
-            "heading": 2.7687586646684967,
-            "angularVelocity": -0.706497731935343,
-            "velocityX": -3.469074109116353,
-            "velocityY": -0.4936763426521838,
-            "timestamp": 2.8339898597638062
-        },
-        {
-            "x": 4.613405710202319,
-            "y": 3.0500832751446354,
-            "heading": 2.6983183223854565,
-            "angularVelocity": -0.8353975526724018,
-            "velocityX": -2.773783114853639,
-            "velocityY": -0.39585783820090814,
-            "timestamp": 2.918309405658697
-        },
-        {
-            "x": 4.43813741968374,
-            "y": 3.025042089065369,
-            "heading": 2.6386185119236694,
-            "angularVelocity": -0.7080186429870714,
-            "velocityX": -2.0786199529236167,
-            "velocityY": -0.29697961265685946,
-            "timestamp": 3.002628951553588
-        },
-        {
-            "x": 4.321361759336474,
-            "y": 3.008347212158301,
-            "heading": 2.596145764785128,
-            "angularVelocity": -0.5037117632427286,
-            "velocityX": -1.3849180413380506,
-            "velocityY": -0.19799533702279348,
-            "timestamp": 3.086948497448479
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -0.26264093989232323,
-            "velocityX": -0.6921498297586374,
-            "velocityY": -0.0989949847299469,
-            "timestamp": 3.1712680433433698
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": -1.507480653475374e-32,
-            "velocityY": 0,
-            "timestamp": 3.2555875892382606
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S4.1.traj
deleted file mode 100644
index 47cb5e7c..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S4.1.traj	
+++ /dev/null
@@ -1,364 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.35775083125341,
-            "y": 4.369912807689986,
-            "heading": 2.783102260210542,
-            "angularVelocity": 0.39244175739179554,
-            "velocityX": 0.6846613362282545,
-            "velocityY": 0.11752066613584591,
-            "timestamp": 0.08434948520907419
-        },
-        {
-            "x": 2.473379945358936,
-            "y": 4.389732628016986,
-            "heading": 2.846401171320743,
-            "angularVelocity": 0.7504362469231927,
-            "velocityX": 1.3708336668436103,
-            "velocityY": 0.2349726293868104,
-            "timestamp": 0.16869897041814838
-        },
-        {
-            "x": 2.647085500332804,
-            "y": 4.419444254302336,
-            "heading": 2.9346614354688145,
-            "angularVelocity": 1.0463639929669244,
-            "velocityX": 2.0593552473178725,
-            "velocityY": 0.35224431081829954,
-            "timestamp": 0.25304845562722256
-        },
-        {
-            "x": 2.8791780514976093,
-            "y": 4.45897416371866,
-            "heading": 3.036264025920344,
-            "angularVelocity": 1.2045431006447933,
-            "velocityX": 2.7515585968251655,
-            "velocityY": 0.4686443470086779,
-            "timestamp": 0.33739794083629676
-        },
-        {
-            "x": 3.1692885380051594,
-            "y": 4.507734938420023,
-            "heading": 3.1081945167682674,
-            "angularVelocity": 0.8527673959080092,
-            "velocityX": 3.439386568732019,
-            "velocityY": 0.5780802879887197,
-            "timestamp": 0.42174742604537097
-        },
-        {
-            "x": 3.483614233052985,
-            "y": 4.562663983733042,
-            "heading": 3.108194542936824,
-            "angularVelocity": 3.1023966932602597e-7,
-            "velocityX": 3.726468445761316,
-            "velocityY": 0.651207831048041,
-            "timestamp": 0.5060969112544451
-        },
-        {
-            "x": 3.7979399253430755,
-            "y": 4.61759304482709,
-            "heading": 3.108194569105307,
-            "angularVelocity": 3.102387994950786e-7,
-            "velocityX": 3.726468413067156,
-            "velocityY": 0.6512080181390185,
-            "timestamp": 0.5904463964635193
-        },
-        {
-            "x": 4.112265617633151,
-            "y": 4.67252210592122,
-            "heading": 3.1081945952737904,
-            "angularVelocity": 3.1023880025122276e-7,
-            "velocityX": 3.7264684130669847,
-            "velocityY": 0.6512080181399977,
-            "timestamp": 0.6747958816725934
-        },
-        {
-            "x": 4.426591309923227,
-            "y": 4.72745116701535,
-            "heading": 3.1081946214422733,
-            "angularVelocity": 3.102388010308895e-7,
-            "velocityX": 3.7264684130669847,
-            "velocityY": 0.6512080181399976,
-            "timestamp": 0.7591453668816676
-        },
-        {
-            "x": 4.740917002213303,
-            "y": 4.7823802281094805,
-            "heading": 3.1081946476107567,
-            "angularVelocity": 3.1023880026526175e-7,
-            "velocityX": 3.726468413066985,
-            "velocityY": 0.6512080181399978,
-            "timestamp": 0.8434948520907417
-        },
-        {
-            "x": 5.055242694503379,
-            "y": 4.837309289203611,
-            "heading": 3.1081946737792396,
-            "angularVelocity": 3.1023880190764424e-7,
-            "velocityX": 3.7264684130669847,
-            "velocityY": 0.6512080181399977,
-            "timestamp": 0.9278443372998159
-        },
-        {
-            "x": 5.369568386793454,
-            "y": 4.892238350297741,
-            "heading": 3.108194699947723,
-            "angularVelocity": 3.102388017253904e-7,
-            "velocityX": 3.726468413066985,
-            "velocityY": 0.6512080181399975,
-            "timestamp": 1.01219382250889
-        },
-        {
-            "x": 5.683894079083919,
-            "y": 4.947167411389647,
-            "heading": 3.108194726116206,
-            "angularVelocity": 3.102388024033471e-7,
-            "velocityX": 3.7264684130715926,
-            "velocityY": 0.6512080181136309,
-            "timestamp": 1.0965433077179643
-        },
-        {
-            "x": 5.9982198456255675,
-            "y": 5.002096047585362,
-            "heading": 3.108194752284696,
-            "angularVelocity": 3.102388798208338e-7,
-            "velocityX": 3.7264692933518155,
-            "velocityY": 0.6512029807836425,
-            "timestamp": 1.1808927929270385
-        },
-        {
-            "x": 6.316938354394085,
-            "y": 5.017469863405169,
-            "heading": 3.1081947796852947,
-            "angularVelocity": 3.2484607308932857e-7,
-            "velocityX": 3.7785471716694046,
-            "velocityY": 0.18226330346533962,
-            "timestamp": 1.2652422781361128
-        },
-        {
-            "x": 6.6329944526576226,
-            "y": 4.973579291611239,
-            "heading": 3.1081948124098697,
-            "angularVelocity": 3.8796413400864556e-7,
-            "velocityX": 3.7469831319081606,
-            "velocityY": -0.5203419046973292,
-            "timestamp": 1.349591763345187
-        },
-        {
-            "x": 6.935463453156901,
-            "y": 4.87194123143387,
-            "heading": 3.1081950470159,
-            "angularVelocity": 0.0000027813569896549863,
-            "velocityX": 3.5859021516202274,
-            "velocityY": -1.2049636097415746,
-            "timestamp": 1.4339412485542613
-        },
-        {
-            "x": 7.201083494587787,
-            "y": 4.724560137748792,
-            "heading": 3.137800042082307,
-            "angularVelocity": 0.3509801511298589,
-            "velocityX": 3.1490416423111984,
-            "velocityY": -1.7472672574084946,
-            "timestamp": 1.5182907337633356
-        },
-        {
-            "x": 7.415442295317108,
-            "y": 4.547383804871535,
-            "heading": 3.160342409540337,
-            "angularVelocity": 0.26724961512396933,
-            "velocityX": 2.541317237419968,
-            "velocityY": -2.1005028357683178,
-            "timestamp": 1.6026402189724098
-        },
-        {
-            "x": 7.575455349602933,
-            "y": 4.347068945452764,
-            "heading": 3.163134577298841,
-            "angularVelocity": 0.033102368693571485,
-            "velocityX": 1.8970246693172619,
-            "velocityY": -2.374820177292801,
-            "timestamp": 1.686989704181484
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": -0.27427052152716735,
-            "velocityX": 1.2394225067045201,
-            "velocityY": -2.6090134979162847,
-            "timestamp": 1.7713391893905583
-        },
-        {
-            "x": 7.728694415127363,
-            "y": 3.916223066041241,
-            "heading": 3.095787658836112,
-            "angularVelocity": -0.5866618963302697,
-            "velocityX": 0.6461353813727732,
-            "velocityY": -2.796838903430975,
-            "timestamp": 1.84670174734847
-        },
-        {
-            "x": 7.732075801393433,
-            "y": 3.692700126888283,
-            "heading": 3.0330227964139005,
-            "angularVelocity": -0.8328388011625617,
-            "velocityX": 0.04486825232179016,
-            "velocityY": -2.9659680511082303,
-            "timestamp": 1.9220643053063815
-        },
-        {
-            "x": 7.689365986032336,
-            "y": 3.4587309949886484,
-            "heading": 2.9594753921163566,
-            "angularVelocity": -0.9759143836202951,
-            "velocityX": -0.5667245979754308,
-            "velocityY": -3.10458055351971,
-            "timestamp": 1.997426863264293
-        },
-        {
-            "x": 7.599710660115244,
-            "y": 3.218597864956733,
-            "heading": 2.8890529754773646,
-            "angularVelocity": -0.9344483327957235,
-            "velocityX": -1.1896534346294716,
-            "velocityY": -3.1863718076823337,
-            "timestamp": 2.072789421222205
-        },
-        {
-            "x": 7.463852101408951,
-            "y": 2.9820453977091343,
-            "heading": 2.853986095526745,
-            "angularVelocity": -0.4653090460411922,
-            "velocityX": -1.8027328475523154,
-            "velocityY": -3.138859317642961,
-            "timestamp": 2.1481519791801165
-        },
-        {
-            "x": 7.284189485763313,
-            "y": 2.760688689378167,
-            "heading": 2.8539858842251613,
-            "angularVelocity": -0.000002803800583333773,
-            "velocityX": -2.3839771434772157,
-            "velocityY": -2.9372239256341386,
-            "timestamp": 2.223514537138028
-        },
-        {
-            "x": 7.070363087296056,
-            "y": 2.5721263513481594,
-            "heading": 2.853985845147589,
-            "angularVelocity": -5.185276832708053e-7,
-            "velocityX": -2.8373028233287183,
-            "velocityY": -2.502069238882734,
-            "timestamp": 2.2988770950959396
-        },
-        {
-            "x": 6.82827305330887,
-            "y": 2.421561078917411,
-            "heading": 2.8539858112509897,
-            "angularVelocity": -4.4978037103687734e-7,
-            "velocityX": -3.212338335468772,
-            "velocityY": -1.9978790066392913,
-            "timestamp": 2.374239653053851
-        },
-        {
-            "x": 6.571710658920333,
-            "y": 2.2972500819001187,
-            "heading": 2.8539857783310936,
-            "angularVelocity": -4.368203133600748e-7,
-            "velocityX": -3.4043748160966305,
-            "velocityY": -1.6495060728527418,
-            "timestamp": 2.449602211011763
-        },
-        {
-            "x": 6.3151480830563145,
-            "y": 2.172939459426277,
-            "heading": 2.8539857454112014,
-            "angularVelocity": -4.3682025848078035e-7,
-            "velocityX": -3.4043772241290333,
-            "velocityY": -1.6495011029650315,
-            "timestamp": 2.5249647689696744
-        },
-        {
-            "x": 6.058585509566717,
-            "y": 2.048628832051934,
-            "heading": 2.853985712491288,
-            "angularVelocity": -4.3682054129494706e-7,
-            "velocityX": -3.404377192622409,
-            "velocityY": -1.6495011679907159,
-            "timestamp": 2.600327326927586
-        },
-        {
-            "x": 5.808645132247433,
-            "y": 1.9267432640802056,
-            "heading": 2.8368087867037386,
-            "angularVelocity": -0.22792386900060174,
-            "velocityX": -3.31650602224609,
-            "velocityY": -1.6173225972478102,
-            "timestamp": 2.6756898848854975
-        },
-        {
-            "x": 5.597200041398206,
-            "y": 1.824292282290654,
-            "heading": 2.751245197802903,
-            "angularVelocity": -1.1353594041834565,
-            "velocityX": -2.805704803270014,
-            "velocityY": -1.3594414065240215,
-            "timestamp": 2.751052442843409
-        },
-        {
-            "x": 5.4283718675083765,
-            "y": 1.7424505925946856,
-            "heading": 2.6610334034240704,
-            "angularVelocity": -1.1970373196357535,
-            "velocityX": -2.2402128917136284,
-            "velocityY": -1.0859728214330957,
-            "timestamp": 2.8264150008013207
-        },
-        {
-            "x": 5.302034408244249,
-            "y": 1.6811743111050435,
-            "heading": 2.5842866606936457,
-            "angularVelocity": -1.0183670088969912,
-            "velocityX": -1.6763955827333314,
-            "velocityY": -0.8130865399216356,
-            "timestamp": 2.9017775587592323
-        },
-        {
-            "x": 5.217968301319177,
-            "y": 1.6403775914720826,
-            "heading": 2.5290716090988563,
-            "angularVelocity": -0.7326589368904639,
-            "velocityX": -1.1154890333210488,
-            "velocityY": -0.5413393698200221,
-            "timestamp": 2.977140116717144
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.38575666599708336,
-            "velocityX": -0.5568853082536673,
-            "velocityY": -0.27039410582988466,
-            "timestamp": 3.0525026746750554
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": 5.6652938345374496e-33,
-            "velocityY": 3.5857679285094535e-32,
-            "timestamp": 3.127865232632967
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S4.traj
deleted file mode 100644
index 47cb5e7c..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3 To S4.traj	
+++ /dev/null
@@ -1,364 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.35775083125341,
-            "y": 4.369912807689986,
-            "heading": 2.783102260210542,
-            "angularVelocity": 0.39244175739179554,
-            "velocityX": 0.6846613362282545,
-            "velocityY": 0.11752066613584591,
-            "timestamp": 0.08434948520907419
-        },
-        {
-            "x": 2.473379945358936,
-            "y": 4.389732628016986,
-            "heading": 2.846401171320743,
-            "angularVelocity": 0.7504362469231927,
-            "velocityX": 1.3708336668436103,
-            "velocityY": 0.2349726293868104,
-            "timestamp": 0.16869897041814838
-        },
-        {
-            "x": 2.647085500332804,
-            "y": 4.419444254302336,
-            "heading": 2.9346614354688145,
-            "angularVelocity": 1.0463639929669244,
-            "velocityX": 2.0593552473178725,
-            "velocityY": 0.35224431081829954,
-            "timestamp": 0.25304845562722256
-        },
-        {
-            "x": 2.8791780514976093,
-            "y": 4.45897416371866,
-            "heading": 3.036264025920344,
-            "angularVelocity": 1.2045431006447933,
-            "velocityX": 2.7515585968251655,
-            "velocityY": 0.4686443470086779,
-            "timestamp": 0.33739794083629676
-        },
-        {
-            "x": 3.1692885380051594,
-            "y": 4.507734938420023,
-            "heading": 3.1081945167682674,
-            "angularVelocity": 0.8527673959080092,
-            "velocityX": 3.439386568732019,
-            "velocityY": 0.5780802879887197,
-            "timestamp": 0.42174742604537097
-        },
-        {
-            "x": 3.483614233052985,
-            "y": 4.562663983733042,
-            "heading": 3.108194542936824,
-            "angularVelocity": 3.1023966932602597e-7,
-            "velocityX": 3.726468445761316,
-            "velocityY": 0.651207831048041,
-            "timestamp": 0.5060969112544451
-        },
-        {
-            "x": 3.7979399253430755,
-            "y": 4.61759304482709,
-            "heading": 3.108194569105307,
-            "angularVelocity": 3.102387994950786e-7,
-            "velocityX": 3.726468413067156,
-            "velocityY": 0.6512080181390185,
-            "timestamp": 0.5904463964635193
-        },
-        {
-            "x": 4.112265617633151,
-            "y": 4.67252210592122,
-            "heading": 3.1081945952737904,
-            "angularVelocity": 3.1023880025122276e-7,
-            "velocityX": 3.7264684130669847,
-            "velocityY": 0.6512080181399977,
-            "timestamp": 0.6747958816725934
-        },
-        {
-            "x": 4.426591309923227,
-            "y": 4.72745116701535,
-            "heading": 3.1081946214422733,
-            "angularVelocity": 3.102388010308895e-7,
-            "velocityX": 3.7264684130669847,
-            "velocityY": 0.6512080181399976,
-            "timestamp": 0.7591453668816676
-        },
-        {
-            "x": 4.740917002213303,
-            "y": 4.7823802281094805,
-            "heading": 3.1081946476107567,
-            "angularVelocity": 3.1023880026526175e-7,
-            "velocityX": 3.726468413066985,
-            "velocityY": 0.6512080181399978,
-            "timestamp": 0.8434948520907417
-        },
-        {
-            "x": 5.055242694503379,
-            "y": 4.837309289203611,
-            "heading": 3.1081946737792396,
-            "angularVelocity": 3.1023880190764424e-7,
-            "velocityX": 3.7264684130669847,
-            "velocityY": 0.6512080181399977,
-            "timestamp": 0.9278443372998159
-        },
-        {
-            "x": 5.369568386793454,
-            "y": 4.892238350297741,
-            "heading": 3.108194699947723,
-            "angularVelocity": 3.102388017253904e-7,
-            "velocityX": 3.726468413066985,
-            "velocityY": 0.6512080181399975,
-            "timestamp": 1.01219382250889
-        },
-        {
-            "x": 5.683894079083919,
-            "y": 4.947167411389647,
-            "heading": 3.108194726116206,
-            "angularVelocity": 3.102388024033471e-7,
-            "velocityX": 3.7264684130715926,
-            "velocityY": 0.6512080181136309,
-            "timestamp": 1.0965433077179643
-        },
-        {
-            "x": 5.9982198456255675,
-            "y": 5.002096047585362,
-            "heading": 3.108194752284696,
-            "angularVelocity": 3.102388798208338e-7,
-            "velocityX": 3.7264692933518155,
-            "velocityY": 0.6512029807836425,
-            "timestamp": 1.1808927929270385
-        },
-        {
-            "x": 6.316938354394085,
-            "y": 5.017469863405169,
-            "heading": 3.1081947796852947,
-            "angularVelocity": 3.2484607308932857e-7,
-            "velocityX": 3.7785471716694046,
-            "velocityY": 0.18226330346533962,
-            "timestamp": 1.2652422781361128
-        },
-        {
-            "x": 6.6329944526576226,
-            "y": 4.973579291611239,
-            "heading": 3.1081948124098697,
-            "angularVelocity": 3.8796413400864556e-7,
-            "velocityX": 3.7469831319081606,
-            "velocityY": -0.5203419046973292,
-            "timestamp": 1.349591763345187
-        },
-        {
-            "x": 6.935463453156901,
-            "y": 4.87194123143387,
-            "heading": 3.1081950470159,
-            "angularVelocity": 0.0000027813569896549863,
-            "velocityX": 3.5859021516202274,
-            "velocityY": -1.2049636097415746,
-            "timestamp": 1.4339412485542613
-        },
-        {
-            "x": 7.201083494587787,
-            "y": 4.724560137748792,
-            "heading": 3.137800042082307,
-            "angularVelocity": 0.3509801511298589,
-            "velocityX": 3.1490416423111984,
-            "velocityY": -1.7472672574084946,
-            "timestamp": 1.5182907337633356
-        },
-        {
-            "x": 7.415442295317108,
-            "y": 4.547383804871535,
-            "heading": 3.160342409540337,
-            "angularVelocity": 0.26724961512396933,
-            "velocityX": 2.541317237419968,
-            "velocityY": -2.1005028357683178,
-            "timestamp": 1.6026402189724098
-        },
-        {
-            "x": 7.575455349602933,
-            "y": 4.347068945452764,
-            "heading": 3.163134577298841,
-            "angularVelocity": 0.033102368693571485,
-            "velocityX": 1.8970246693172619,
-            "velocityY": -2.374820177292801,
-            "timestamp": 1.686989704181484
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": -0.27427052152716735,
-            "velocityX": 1.2394225067045201,
-            "velocityY": -2.6090134979162847,
-            "timestamp": 1.7713391893905583
-        },
-        {
-            "x": 7.728694415127363,
-            "y": 3.916223066041241,
-            "heading": 3.095787658836112,
-            "angularVelocity": -0.5866618963302697,
-            "velocityX": 0.6461353813727732,
-            "velocityY": -2.796838903430975,
-            "timestamp": 1.84670174734847
-        },
-        {
-            "x": 7.732075801393433,
-            "y": 3.692700126888283,
-            "heading": 3.0330227964139005,
-            "angularVelocity": -0.8328388011625617,
-            "velocityX": 0.04486825232179016,
-            "velocityY": -2.9659680511082303,
-            "timestamp": 1.9220643053063815
-        },
-        {
-            "x": 7.689365986032336,
-            "y": 3.4587309949886484,
-            "heading": 2.9594753921163566,
-            "angularVelocity": -0.9759143836202951,
-            "velocityX": -0.5667245979754308,
-            "velocityY": -3.10458055351971,
-            "timestamp": 1.997426863264293
-        },
-        {
-            "x": 7.599710660115244,
-            "y": 3.218597864956733,
-            "heading": 2.8890529754773646,
-            "angularVelocity": -0.9344483327957235,
-            "velocityX": -1.1896534346294716,
-            "velocityY": -3.1863718076823337,
-            "timestamp": 2.072789421222205
-        },
-        {
-            "x": 7.463852101408951,
-            "y": 2.9820453977091343,
-            "heading": 2.853986095526745,
-            "angularVelocity": -0.4653090460411922,
-            "velocityX": -1.8027328475523154,
-            "velocityY": -3.138859317642961,
-            "timestamp": 2.1481519791801165
-        },
-        {
-            "x": 7.284189485763313,
-            "y": 2.760688689378167,
-            "heading": 2.8539858842251613,
-            "angularVelocity": -0.000002803800583333773,
-            "velocityX": -2.3839771434772157,
-            "velocityY": -2.9372239256341386,
-            "timestamp": 2.223514537138028
-        },
-        {
-            "x": 7.070363087296056,
-            "y": 2.5721263513481594,
-            "heading": 2.853985845147589,
-            "angularVelocity": -5.185276832708053e-7,
-            "velocityX": -2.8373028233287183,
-            "velocityY": -2.502069238882734,
-            "timestamp": 2.2988770950959396
-        },
-        {
-            "x": 6.82827305330887,
-            "y": 2.421561078917411,
-            "heading": 2.8539858112509897,
-            "angularVelocity": -4.4978037103687734e-7,
-            "velocityX": -3.212338335468772,
-            "velocityY": -1.9978790066392913,
-            "timestamp": 2.374239653053851
-        },
-        {
-            "x": 6.571710658920333,
-            "y": 2.2972500819001187,
-            "heading": 2.8539857783310936,
-            "angularVelocity": -4.368203133600748e-7,
-            "velocityX": -3.4043748160966305,
-            "velocityY": -1.6495060728527418,
-            "timestamp": 2.449602211011763
-        },
-        {
-            "x": 6.3151480830563145,
-            "y": 2.172939459426277,
-            "heading": 2.8539857454112014,
-            "angularVelocity": -4.3682025848078035e-7,
-            "velocityX": -3.4043772241290333,
-            "velocityY": -1.6495011029650315,
-            "timestamp": 2.5249647689696744
-        },
-        {
-            "x": 6.058585509566717,
-            "y": 2.048628832051934,
-            "heading": 2.853985712491288,
-            "angularVelocity": -4.3682054129494706e-7,
-            "velocityX": -3.404377192622409,
-            "velocityY": -1.6495011679907159,
-            "timestamp": 2.600327326927586
-        },
-        {
-            "x": 5.808645132247433,
-            "y": 1.9267432640802056,
-            "heading": 2.8368087867037386,
-            "angularVelocity": -0.22792386900060174,
-            "velocityX": -3.31650602224609,
-            "velocityY": -1.6173225972478102,
-            "timestamp": 2.6756898848854975
-        },
-        {
-            "x": 5.597200041398206,
-            "y": 1.824292282290654,
-            "heading": 2.751245197802903,
-            "angularVelocity": -1.1353594041834565,
-            "velocityX": -2.805704803270014,
-            "velocityY": -1.3594414065240215,
-            "timestamp": 2.751052442843409
-        },
-        {
-            "x": 5.4283718675083765,
-            "y": 1.7424505925946856,
-            "heading": 2.6610334034240704,
-            "angularVelocity": -1.1970373196357535,
-            "velocityX": -2.2402128917136284,
-            "velocityY": -1.0859728214330957,
-            "timestamp": 2.8264150008013207
-        },
-        {
-            "x": 5.302034408244249,
-            "y": 1.6811743111050435,
-            "heading": 2.5842866606936457,
-            "angularVelocity": -1.0183670088969912,
-            "velocityX": -1.6763955827333314,
-            "velocityY": -0.8130865399216356,
-            "timestamp": 2.9017775587592323
-        },
-        {
-            "x": 5.217968301319177,
-            "y": 1.6403775914720826,
-            "heading": 2.5290716090988563,
-            "angularVelocity": -0.7326589368904639,
-            "velocityX": -1.1154890333210488,
-            "velocityY": -0.5413393698200221,
-            "timestamp": 2.977140116717144
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.38575666599708336,
-            "velocityX": -0.5568853082536673,
-            "velocityY": -0.27039410582988466,
-            "timestamp": 3.0525026746750554
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": 5.6652938345374496e-33,
-            "velocityY": 3.5857679285094535e-32,
-            "timestamp": 3.127865232632967
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S3.1.traj
deleted file mode 100644
index 41dd6517..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S3.1.traj	
+++ /dev/null
@@ -1,364 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -1.5074616592069685e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3610343967602465,
-            "y": 4.342403579285033,
-            "heading": 2.7671031550368004,
-            "angularVelocity": 0.1956775196968839,
-            "velocityX": 0.6982956857107938,
-            "velocityY": -0.20132098163403012,
-            "timestamp": 0.08740480287819585
-        },
-        {
-            "x": 2.4831385894662814,
-            "y": 4.307214947818553,
-            "heading": 2.799662518084232,
-            "angularVelocity": 0.3725122873717296,
-            "velocityX": 1.3969963741717348,
-            "velocityY": -0.4025937969966943,
-            "timestamp": 0.1748096057563917
-        },
-        {
-            "x": 2.6663651072482595,
-            "y": 4.254442931030586,
-            "heading": 2.8448933899423676,
-            "angularVelocity": 0.5174872589229184,
-            "velocityX": 2.0962980494025705,
-            "velocityY": -0.6037656404477811,
-            "timestamp": 0.26221440863458756
-        },
-        {
-            "x": 2.9107932372110517,
-            "y": 4.184113644093446,
-            "heading": 2.897077048059956,
-            "angularVelocity": 0.597034217791321,
-            "velocityX": 2.796506849897233,
-            "velocityY": -0.8046386997193782,
-            "timestamp": 0.3496192115127834
-        },
-        {
-            "x": 3.2164117287156215,
-            "y": 4.096391426129405,
-            "heading": 2.937460118957845,
-            "angularVelocity": 0.46202347660649606,
-            "velocityX": 3.4965869316182627,
-            "velocityY": -1.0036315519901937,
-            "timestamp": 0.43702401439097927
-        },
-        {
-            "x": 3.5340454438791253,
-            "y": 4.004541690732033,
-            "heading": 2.9374601327514616,
-            "angularVelocity": 1.5781302417851277e-7,
-            "velocityX": 3.6340533323568787,
-            "velocityY": -1.0508545568756669,
-            "timestamp": 0.5244288172691751
-        },
-        {
-            "x": 3.8516791581377654,
-            "y": 3.9126919522054426,
-            "heading": 2.93746014654506,
-            "angularVelocity": 1.5781282325664268e-7,
-            "velocityX": 3.634053322004319,
-            "velocityY": -1.050854592677122,
-            "timestamp": 0.611833620147371
-        },
-        {
-            "x": 4.169312872396402,
-            "y": 3.820842213678837,
-            "heading": 2.9374601603386585,
-            "angularVelocity": 1.5781282237025435e-7,
-            "velocityX": 3.63405332200427,
-            "velocityY": -1.0508545926772908,
-            "timestamp": 0.6992384230255668
-        },
-        {
-            "x": 4.486946586655037,
-            "y": 3.7289924751522316,
-            "heading": 2.9374601741322572,
-            "angularVelocity": 1.5781282238426364e-7,
-            "velocityX": 3.63405332200427,
-            "velocityY": -1.0508545926772912,
-            "timestamp": 0.7866432259037627
-        },
-        {
-            "x": 4.804580300913673,
-            "y": 3.637142736625626,
-            "heading": 2.937460187925856,
-            "angularVelocity": 1.578128226198401e-7,
-            "velocityX": 3.63405332200427,
-            "velocityY": -1.0508545926772912,
-            "timestamp": 0.8740480287819585
-        },
-        {
-            "x": 5.122214015172308,
-            "y": 3.545292998099021,
-            "heading": 2.9374602017194547,
-            "angularVelocity": 1.5781282274310823e-7,
-            "velocityX": 3.63405332200427,
-            "velocityY": -1.050854592677291,
-            "timestamp": 0.9614528316601544
-        },
-        {
-            "x": 5.439847729430944,
-            "y": 3.4534432595724156,
-            "heading": 2.937460215513054,
-            "angularVelocity": 1.5781282350457181e-7,
-            "velocityX": 3.63405332200427,
-            "velocityY": -1.050854592677291,
-            "timestamp": 1.0488576345383502
-        },
-        {
-            "x": 5.757481443689581,
-            "y": 3.3615935210458106,
-            "heading": 2.9374602293066525,
-            "angularVelocity": 1.5781282308668676e-7,
-            "velocityX": 3.6340533220042697,
-            "velocityY": -1.0508545926772905,
-            "timestamp": 1.136262437416546
-        },
-        {
-            "x": 6.075115157948217,
-            "y": 3.269743782519205,
-            "heading": 2.9374602431002512,
-            "angularVelocity": 1.578128234236369e-7,
-            "velocityX": 3.63405332200427,
-            "velocityY": -1.050854592677291,
-            "timestamp": 1.2236672402947417
-        },
-        {
-            "x": 6.3927488722066945,
-            "y": 3.1778940439920533,
-            "heading": 2.93746025689385,
-            "angularVelocity": 1.5781282293850814e-7,
-            "velocityX": 3.6340533220024622,
-            "velocityY": -1.0508545926835444,
-            "timestamp": 1.3110720431729375
-        },
-        {
-            "x": 6.710382552956458,
-            "y": 3.0860441895855293,
-            "heading": 2.9374602706874486,
-            "angularVelocity": 1.5781282618344319e-7,
-            "velocityX": 3.634052938628626,
-            "velocityY": -1.0508559184616288,
-            "timestamp": 1.3984768460511332
-        },
-        {
-            "x": 7.02127514506585,
-            "y": 2.9734682388937714,
-            "heading": 2.9374602922500688,
-            "angularVelocity": 2.466983432002081e-7,
-            "velocityX": 3.5569280162171513,
-            "velocityY": -1.2879835773857788,
-            "timestamp": 1.485881648929329
-        },
-        {
-            "x": 7.2761933316047935,
-            "y": 2.8394289044438907,
-            "heading": 3.0149898604295666,
-            "angularVelocity": 0.8870172533600954,
-            "velocityX": 2.9165237852453965,
-            "velocityY": -1.533546556207826,
-            "timestamp": 1.5732864518075247
-        },
-        {
-            "x": 7.469158921958533,
-            "y": 2.7195716127256344,
-            "heading": 3.0771718399112147,
-            "angularVelocity": 0.711425201293604,
-            "velocityX": 2.207722962577334,
-            "velocityY": -1.3712895375473233,
-            "timestamp": 1.6606912546857204
-        },
-        {
-            "x": 7.6006701652701585,
-            "y": 2.6155462997255583,
-            "heading": 3.1196274586402084,
-            "angularVelocity": 0.48573553547347,
-            "velocityX": 1.5046226177627207,
-            "velocityY": -1.1901555701125741,
-            "timestamp": 1.7480960575639162
-        },
-        {
-            "x": 7.670917138983036,
-            "y": 2.527925545784097,
-            "heading": 3.140832038047466,
-            "angularVelocity": 0.24260199335736132,
-            "velocityX": 0.8036969525664591,
-            "velocityY": -1.0024707001921451,
-            "timestamp": 1.835500860442112
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -0.009519363010585313,
-            "velocityX": 0.10391718438655147,
-            "velocityY": -0.8114605084452422,
-            "timestamp": 1.9229056633203077
-        },
-        {
-            "x": 7.63404119577111,
-            "y": 2.4050502975891916,
-            "heading": 3.1190998231163127,
-            "angularVelocity": -0.25309519096022687,
-            "velocityX": -0.5565480329352274,
-            "velocityY": -0.6290961040742543,
-            "timestamp": 2.005483988117367
-        },
-        {
-            "x": 7.533566509755774,
-            "y": 2.3683786594075307,
-            "heading": 3.079403507657828,
-            "angularVelocity": -0.4807110771021385,
-            "velocityX": -1.216719838556405,
-            "velocityY": -0.4440830965242228,
-            "timestamp": 2.0880623129144262
-        },
-        {
-            "x": 7.378621026771135,
-            "y": 2.3473334555907552,
-            "heading": 3.0230319998393305,
-            "angularVelocity": -0.6826429084997041,
-            "velocityX": -1.876345679879389,
-            "velocityY": -0.2548514258250533,
-            "timestamp": 2.1706406377114855
-        },
-        {
-            "x": 7.169309740069539,
-            "y": 2.342556806639995,
-            "heading": 2.953940328458666,
-            "angularVelocity": -0.8366804673073884,
-            "velocityX": -2.5347000828121677,
-            "velocityY": -0.05784385869415158,
-            "timestamp": 2.253218962508545
-        },
-        {
-            "x": 6.906019202766067,
-            "y": 2.3556284163077987,
-            "heading": 2.881970584756294,
-            "angularVelocity": -0.8715331036230265,
-            "velocityX": -3.188373437588175,
-            "velocityY": 0.15829347107643843,
-            "timestamp": 2.335797287305604
-        },
-        {
-            "x": 6.596097301196444,
-            "y": 2.394810276985141,
-            "heading": 2.8819705158229665,
-            "angularVelocity": -8.347629724920641e-7,
-            "velocityX": -3.753065981070365,
-            "velocityY": 0.47448117618799124,
-            "timestamp": 2.4183756121026634
-        },
-        {
-            "x": 6.2936576837370914,
-            "y": 2.4730221394670577,
-            "heading": 2.881970499784504,
-            "angularVelocity": -1.942212104248679e-7,
-            "velocityX": -3.6624576509951607,
-            "velocityY": 0.9471233846669401,
-            "timestamp": 2.5009539368997227
-        },
-        {
-            "x": 5.991218190130788,
-            "y": 2.5512344808782137,
-            "heading": 2.8819704837460454,
-            "angularVelocity": -1.9422117139312037e-7,
-            "velocityX": -3.662456151169988,
-            "velocityY": 0.9471291843636568,
-            "timestamp": 2.583532261696782
-        },
-        {
-            "x": 5.688778696525171,
-            "y": 2.6294468222920195,
-            "heading": 2.8819704677075864,
-            "angularVelocity": -1.9422117079756682e-7,
-            "velocityX": -3.662456151161689,
-            "velocityY": 0.9471291843957483,
-            "timestamp": 2.6661105864938413
-        },
-        {
-            "x": 5.38633920291957,
-            "y": 2.7076591637058867,
-            "heading": 2.881970451669127,
-            "angularVelocity": -1.9422117106487922e-7,
-            "velocityX": -3.6624561511614977,
-            "velocityY": 0.947129184396488,
-            "timestamp": 2.7486889112909005
-        },
-        {
-            "x": 5.0838997121673595,
-            "y": 2.7858715161533545,
-            "heading": 2.881970435630565,
-            "angularVelocity": -1.9422242240409168e-7,
-            "velocityX": -3.662456116607741,
-            "velocityY": 0.9471293180102566,
-            "timestamp": 2.83126723608796
-        },
-        {
-            "x": 4.810061938834441,
-            "y": 2.8575158595499386,
-            "heading": 2.7916066716165844,
-            "angularVelocity": -1.094279452096593,
-            "velocityX": -3.3160974626924187,
-            "velocityY": 0.8675925985741851,
-            "timestamp": 2.913845560885019
-        },
-        {
-            "x": 4.591150287853899,
-            "y": 2.914595495450747,
-            "heading": 2.7083456685941125,
-            "angularVelocity": -1.0082670389244417,
-            "velocityX": -2.650957760629438,
-            "velocityY": 0.6912181379446108,
-            "timestamp": 2.9964238856820784
-        },
-        {
-            "x": 4.427043317855732,
-            "y": 2.957326813953426,
-            "heading": 2.64244137634817,
-            "angularVelocity": -0.7980822135579249,
-            "velocityX": -1.98728867897803,
-            "velocityY": 0.5174640997827658,
-            "timestamp": 3.0790022104791377
-        },
-        {
-            "x": 4.317672771154393,
-            "y": 2.985782903772343,
-            "heading": 2.597132660112387,
-            "angularVelocity": -0.5486756524442865,
-            "velocityX": -1.3244461784629789,
-            "velocityY": 0.34459514514068673,
-            "timestamp": 3.161580535276197
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -0.28012992718412955,
-            "velocityX": -0.6620716911944379,
-            "velocityY": 0.1721649871512405,
-            "timestamp": 3.2441588600732563
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": -8.552238423258885e-34,
-            "velocityY": -9.381987056731701e-33,
-            "timestamp": 3.3267371848703156
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S3.traj
deleted file mode 100644
index 41dd6517..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S3.traj	
+++ /dev/null
@@ -1,364 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -1.5074616592069685e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3610343967602465,
-            "y": 4.342403579285033,
-            "heading": 2.7671031550368004,
-            "angularVelocity": 0.1956775196968839,
-            "velocityX": 0.6982956857107938,
-            "velocityY": -0.20132098163403012,
-            "timestamp": 0.08740480287819585
-        },
-        {
-            "x": 2.4831385894662814,
-            "y": 4.307214947818553,
-            "heading": 2.799662518084232,
-            "angularVelocity": 0.3725122873717296,
-            "velocityX": 1.3969963741717348,
-            "velocityY": -0.4025937969966943,
-            "timestamp": 0.1748096057563917
-        },
-        {
-            "x": 2.6663651072482595,
-            "y": 4.254442931030586,
-            "heading": 2.8448933899423676,
-            "angularVelocity": 0.5174872589229184,
-            "velocityX": 2.0962980494025705,
-            "velocityY": -0.6037656404477811,
-            "timestamp": 0.26221440863458756
-        },
-        {
-            "x": 2.9107932372110517,
-            "y": 4.184113644093446,
-            "heading": 2.897077048059956,
-            "angularVelocity": 0.597034217791321,
-            "velocityX": 2.796506849897233,
-            "velocityY": -0.8046386997193782,
-            "timestamp": 0.3496192115127834
-        },
-        {
-            "x": 3.2164117287156215,
-            "y": 4.096391426129405,
-            "heading": 2.937460118957845,
-            "angularVelocity": 0.46202347660649606,
-            "velocityX": 3.4965869316182627,
-            "velocityY": -1.0036315519901937,
-            "timestamp": 0.43702401439097927
-        },
-        {
-            "x": 3.5340454438791253,
-            "y": 4.004541690732033,
-            "heading": 2.9374601327514616,
-            "angularVelocity": 1.5781302417851277e-7,
-            "velocityX": 3.6340533323568787,
-            "velocityY": -1.0508545568756669,
-            "timestamp": 0.5244288172691751
-        },
-        {
-            "x": 3.8516791581377654,
-            "y": 3.9126919522054426,
-            "heading": 2.93746014654506,
-            "angularVelocity": 1.5781282325664268e-7,
-            "velocityX": 3.634053322004319,
-            "velocityY": -1.050854592677122,
-            "timestamp": 0.611833620147371
-        },
-        {
-            "x": 4.169312872396402,
-            "y": 3.820842213678837,
-            "heading": 2.9374601603386585,
-            "angularVelocity": 1.5781282237025435e-7,
-            "velocityX": 3.63405332200427,
-            "velocityY": -1.0508545926772908,
-            "timestamp": 0.6992384230255668
-        },
-        {
-            "x": 4.486946586655037,
-            "y": 3.7289924751522316,
-            "heading": 2.9374601741322572,
-            "angularVelocity": 1.5781282238426364e-7,
-            "velocityX": 3.63405332200427,
-            "velocityY": -1.0508545926772912,
-            "timestamp": 0.7866432259037627
-        },
-        {
-            "x": 4.804580300913673,
-            "y": 3.637142736625626,
-            "heading": 2.937460187925856,
-            "angularVelocity": 1.578128226198401e-7,
-            "velocityX": 3.63405332200427,
-            "velocityY": -1.0508545926772912,
-            "timestamp": 0.8740480287819585
-        },
-        {
-            "x": 5.122214015172308,
-            "y": 3.545292998099021,
-            "heading": 2.9374602017194547,
-            "angularVelocity": 1.5781282274310823e-7,
-            "velocityX": 3.63405332200427,
-            "velocityY": -1.050854592677291,
-            "timestamp": 0.9614528316601544
-        },
-        {
-            "x": 5.439847729430944,
-            "y": 3.4534432595724156,
-            "heading": 2.937460215513054,
-            "angularVelocity": 1.5781282350457181e-7,
-            "velocityX": 3.63405332200427,
-            "velocityY": -1.050854592677291,
-            "timestamp": 1.0488576345383502
-        },
-        {
-            "x": 5.757481443689581,
-            "y": 3.3615935210458106,
-            "heading": 2.9374602293066525,
-            "angularVelocity": 1.5781282308668676e-7,
-            "velocityX": 3.6340533220042697,
-            "velocityY": -1.0508545926772905,
-            "timestamp": 1.136262437416546
-        },
-        {
-            "x": 6.075115157948217,
-            "y": 3.269743782519205,
-            "heading": 2.9374602431002512,
-            "angularVelocity": 1.578128234236369e-7,
-            "velocityX": 3.63405332200427,
-            "velocityY": -1.050854592677291,
-            "timestamp": 1.2236672402947417
-        },
-        {
-            "x": 6.3927488722066945,
-            "y": 3.1778940439920533,
-            "heading": 2.93746025689385,
-            "angularVelocity": 1.5781282293850814e-7,
-            "velocityX": 3.6340533220024622,
-            "velocityY": -1.0508545926835444,
-            "timestamp": 1.3110720431729375
-        },
-        {
-            "x": 6.710382552956458,
-            "y": 3.0860441895855293,
-            "heading": 2.9374602706874486,
-            "angularVelocity": 1.5781282618344319e-7,
-            "velocityX": 3.634052938628626,
-            "velocityY": -1.0508559184616288,
-            "timestamp": 1.3984768460511332
-        },
-        {
-            "x": 7.02127514506585,
-            "y": 2.9734682388937714,
-            "heading": 2.9374602922500688,
-            "angularVelocity": 2.466983432002081e-7,
-            "velocityX": 3.5569280162171513,
-            "velocityY": -1.2879835773857788,
-            "timestamp": 1.485881648929329
-        },
-        {
-            "x": 7.2761933316047935,
-            "y": 2.8394289044438907,
-            "heading": 3.0149898604295666,
-            "angularVelocity": 0.8870172533600954,
-            "velocityX": 2.9165237852453965,
-            "velocityY": -1.533546556207826,
-            "timestamp": 1.5732864518075247
-        },
-        {
-            "x": 7.469158921958533,
-            "y": 2.7195716127256344,
-            "heading": 3.0771718399112147,
-            "angularVelocity": 0.711425201293604,
-            "velocityX": 2.207722962577334,
-            "velocityY": -1.3712895375473233,
-            "timestamp": 1.6606912546857204
-        },
-        {
-            "x": 7.6006701652701585,
-            "y": 2.6155462997255583,
-            "heading": 3.1196274586402084,
-            "angularVelocity": 0.48573553547347,
-            "velocityX": 1.5046226177627207,
-            "velocityY": -1.1901555701125741,
-            "timestamp": 1.7480960575639162
-        },
-        {
-            "x": 7.670917138983036,
-            "y": 2.527925545784097,
-            "heading": 3.140832038047466,
-            "angularVelocity": 0.24260199335736132,
-            "velocityX": 0.8036969525664591,
-            "velocityY": -1.0024707001921451,
-            "timestamp": 1.835500860442112
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -0.009519363010585313,
-            "velocityX": 0.10391718438655147,
-            "velocityY": -0.8114605084452422,
-            "timestamp": 1.9229056633203077
-        },
-        {
-            "x": 7.63404119577111,
-            "y": 2.4050502975891916,
-            "heading": 3.1190998231163127,
-            "angularVelocity": -0.25309519096022687,
-            "velocityX": -0.5565480329352274,
-            "velocityY": -0.6290961040742543,
-            "timestamp": 2.005483988117367
-        },
-        {
-            "x": 7.533566509755774,
-            "y": 2.3683786594075307,
-            "heading": 3.079403507657828,
-            "angularVelocity": -0.4807110771021385,
-            "velocityX": -1.216719838556405,
-            "velocityY": -0.4440830965242228,
-            "timestamp": 2.0880623129144262
-        },
-        {
-            "x": 7.378621026771135,
-            "y": 2.3473334555907552,
-            "heading": 3.0230319998393305,
-            "angularVelocity": -0.6826429084997041,
-            "velocityX": -1.876345679879389,
-            "velocityY": -0.2548514258250533,
-            "timestamp": 2.1706406377114855
-        },
-        {
-            "x": 7.169309740069539,
-            "y": 2.342556806639995,
-            "heading": 2.953940328458666,
-            "angularVelocity": -0.8366804673073884,
-            "velocityX": -2.5347000828121677,
-            "velocityY": -0.05784385869415158,
-            "timestamp": 2.253218962508545
-        },
-        {
-            "x": 6.906019202766067,
-            "y": 2.3556284163077987,
-            "heading": 2.881970584756294,
-            "angularVelocity": -0.8715331036230265,
-            "velocityX": -3.188373437588175,
-            "velocityY": 0.15829347107643843,
-            "timestamp": 2.335797287305604
-        },
-        {
-            "x": 6.596097301196444,
-            "y": 2.394810276985141,
-            "heading": 2.8819705158229665,
-            "angularVelocity": -8.347629724920641e-7,
-            "velocityX": -3.753065981070365,
-            "velocityY": 0.47448117618799124,
-            "timestamp": 2.4183756121026634
-        },
-        {
-            "x": 6.2936576837370914,
-            "y": 2.4730221394670577,
-            "heading": 2.881970499784504,
-            "angularVelocity": -1.942212104248679e-7,
-            "velocityX": -3.6624576509951607,
-            "velocityY": 0.9471233846669401,
-            "timestamp": 2.5009539368997227
-        },
-        {
-            "x": 5.991218190130788,
-            "y": 2.5512344808782137,
-            "heading": 2.8819704837460454,
-            "angularVelocity": -1.9422117139312037e-7,
-            "velocityX": -3.662456151169988,
-            "velocityY": 0.9471291843636568,
-            "timestamp": 2.583532261696782
-        },
-        {
-            "x": 5.688778696525171,
-            "y": 2.6294468222920195,
-            "heading": 2.8819704677075864,
-            "angularVelocity": -1.9422117079756682e-7,
-            "velocityX": -3.662456151161689,
-            "velocityY": 0.9471291843957483,
-            "timestamp": 2.6661105864938413
-        },
-        {
-            "x": 5.38633920291957,
-            "y": 2.7076591637058867,
-            "heading": 2.881970451669127,
-            "angularVelocity": -1.9422117106487922e-7,
-            "velocityX": -3.6624561511614977,
-            "velocityY": 0.947129184396488,
-            "timestamp": 2.7486889112909005
-        },
-        {
-            "x": 5.0838997121673595,
-            "y": 2.7858715161533545,
-            "heading": 2.881970435630565,
-            "angularVelocity": -1.9422242240409168e-7,
-            "velocityX": -3.662456116607741,
-            "velocityY": 0.9471293180102566,
-            "timestamp": 2.83126723608796
-        },
-        {
-            "x": 4.810061938834441,
-            "y": 2.8575158595499386,
-            "heading": 2.7916066716165844,
-            "angularVelocity": -1.094279452096593,
-            "velocityX": -3.3160974626924187,
-            "velocityY": 0.8675925985741851,
-            "timestamp": 2.913845560885019
-        },
-        {
-            "x": 4.591150287853899,
-            "y": 2.914595495450747,
-            "heading": 2.7083456685941125,
-            "angularVelocity": -1.0082670389244417,
-            "velocityX": -2.650957760629438,
-            "velocityY": 0.6912181379446108,
-            "timestamp": 2.9964238856820784
-        },
-        {
-            "x": 4.427043317855732,
-            "y": 2.957326813953426,
-            "heading": 2.64244137634817,
-            "angularVelocity": -0.7980822135579249,
-            "velocityX": -1.98728867897803,
-            "velocityY": 0.5174640997827658,
-            "timestamp": 3.0790022104791377
-        },
-        {
-            "x": 4.317672771154393,
-            "y": 2.985782903772343,
-            "heading": 2.597132660112387,
-            "angularVelocity": -0.5486756524442865,
-            "velocityX": -1.3244461784629789,
-            "velocityY": 0.34459514514068673,
-            "timestamp": 3.161580535276197
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -0.28012992718412955,
-            "velocityX": -0.6620716911944379,
-            "velocityY": 0.1721649871512405,
-            "timestamp": 3.2441588600732563
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": -8.552238423258885e-34,
-            "velocityY": -9.381987056731701e-33,
-            "timestamp": 3.3267371848703156
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S4.1.traj
deleted file mode 100644
index 43401240..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S4.1.traj	
+++ /dev/null
@@ -1,346 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.360128743568809,
-            "y": 4.349177185639382,
-            "heading": 2.7782206485451093,
-            "angularVelocity": 0.3283351874090942,
-            "velocityX": 0.6995722389859838,
-            "velocityY": -0.1259188206007136,
-            "timestamp": 0.0859507284851176
-        },
-        {
-            "x": 2.4804864804971807,
-            "y": 4.327553542642656,
-            "heading": 2.831856423124361,
-            "angularVelocity": 0.6240293191760308,
-            "velocityX": 1.4003108414515997,
-            "velocityY": -0.2515818466910346,
-            "timestamp": 0.1719014569702352
-        },
-        {
-            "x": 2.6612218913219765,
-            "y": 4.295165149158174,
-            "heading": 2.9060715845563863,
-            "angularVelocity": 0.863461691832829,
-            "velocityX": 2.102779278433835,
-            "velocityY": -0.3768251189411322,
-            "timestamp": 0.2578521854553528
-        },
-        {
-            "x": 2.902552042800783,
-            "y": 4.252097385240196,
-            "heading": 2.990739532355289,
-            "angularVelocity": 0.9850753948357858,
-            "velocityX": 2.8077731943900375,
-            "velocityY": -0.5010750307419894,
-            "timestamp": 0.3438029139404704
-        },
-        {
-            "x": 3.2042136582065344,
-            "y": 4.198837394002331,
-            "heading": 3.04987623940461,
-            "angularVelocity": 0.6880303179693488,
-            "velocityX": 3.5097039981224545,
-            "velocityY": -0.6196572405676055,
-            "timestamp": 0.429753642425588
-        },
-        {
-            "x": 3.524112842751923,
-            "y": 4.1406587673803905,
-            "heading": 3.0498762636795202,
-            "angularVelocity": 2.824282081012085e-7,
-            "velocityX": 3.7218903223232087,
-            "velocityY": -0.6768834615754469,
-            "timestamp": 0.5157043709107056
-        },
-        {
-            "x": 3.844012025527027,
-            "y": 4.082480131024319,
-            "heading": 3.0498762879543886,
-            "angularVelocity": 2.824277225814376e-7,
-            "velocityX": 3.7218903017266984,
-            "velocityY": -0.6768835748279012,
-            "timestamp": 0.6016550993958233
-        },
-        {
-            "x": 4.163911208302122,
-            "y": 4.0243014946682,
-            "heading": 3.049876312229257,
-            "angularVelocity": 2.824277213255298e-7,
-            "velocityX": 3.721890301726596,
-            "velocityY": -0.6768835748284626,
-            "timestamp": 0.6876058278809409
-        },
-        {
-            "x": 4.483810391077216,
-            "y": 3.9661228583120804,
-            "heading": 3.0498763365041253,
-            "angularVelocity": 2.8242772165714766e-7,
-            "velocityX": 3.7218903017265963,
-            "velocityY": -0.6768835748284627,
-            "timestamp": 0.7735565563660586
-        },
-        {
-            "x": 4.80370957385231,
-            "y": 3.9079442219559613,
-            "heading": 3.0498763607789936,
-            "angularVelocity": 2.8242772140625194e-7,
-            "velocityX": 3.7218903017265963,
-            "velocityY": -0.6768835748284627,
-            "timestamp": 0.8595072848511762
-        },
-        {
-            "x": 5.123608756627404,
-            "y": 3.8497655855998416,
-            "heading": 3.049876385053862,
-            "angularVelocity": 2.824277226048516e-7,
-            "velocityX": 3.7218903017265963,
-            "velocityY": -0.6768835748284628,
-            "timestamp": 0.9454580133362939
-        },
-        {
-            "x": 5.443507939402499,
-            "y": 3.7915869492437224,
-            "heading": 3.0498764093287303,
-            "angularVelocity": 2.8242772163119933e-7,
-            "velocityX": 3.7218903017265963,
-            "velocityY": -0.6768835748284626,
-            "timestamp": 1.0314087418214115
-        },
-        {
-            "x": 5.763407122177593,
-            "y": 3.733408312887603,
-            "heading": 3.0498764336035986,
-            "angularVelocity": 2.824277219712686e-7,
-            "velocityX": 3.7218903017265963,
-            "velocityY": -0.6768835748284628,
-            "timestamp": 1.117359470306529
-        },
-        {
-            "x": 6.083306304952576,
-            "y": 3.675229676530871,
-            "heading": 3.049876457878467,
-            "angularVelocity": 2.8242772161077964e-7,
-            "velocityX": 3.7218903017253,
-            "velocityY": -0.6768835748355908,
-            "timestamp": 1.2033101987916466
-        },
-        {
-            "x": 6.403205465262658,
-            "y": 3.6170509166494913,
-            "heading": 3.0498764821533357,
-            "angularVelocity": 2.8242772554820356e-7,
-            "velocityX": 3.721890040355771,
-            "velocityY": -0.6768850119920853,
-            "timestamp": 1.2892609272767641
-        },
-        {
-            "x": 6.718573965768454,
-            "y": 3.5379121175247272,
-            "heading": 3.0498765068050417,
-            "angularVelocity": 2.868120651983924e-7,
-            "velocityX": 3.6691777494404794,
-            "velocityY": -0.9207461125646453,
-            "timestamp": 1.3752116557618816
-        },
-        {
-            "x": 7.013358117396929,
-            "y": 3.4007168540694233,
-            "heading": 3.0498765454511387,
-            "angularVelocity": 4.49630829762963e-7,
-            "velocityX": 3.4296876457482686,
-            "velocityY": -1.5962082680784608,
-            "timestamp": 1.4611623842469992
-        },
-        {
-            "x": 7.268807462845475,
-            "y": 3.2166766255285517,
-            "heading": 3.0757384072562206,
-            "angularVelocity": 0.30089171157615124,
-            "velocityX": 2.9720439832312224,
-            "velocityY": -2.1412294204433464,
-            "timestamp": 1.5471131127321167
-        },
-        {
-            "x": 7.46355872560419,
-            "y": 3.0231641785099956,
-            "heading": 3.117732513124605,
-            "angularVelocity": 0.48858347809886377,
-            "velocityX": 2.2658477268453225,
-            "velocityY": -2.2514346350428145,
-            "timestamp": 1.6330638412172342
-        },
-        {
-            "x": 7.596796563191941,
-            "y": 2.830012972060279,
-            "heading": 3.1482585374873016,
-            "angularVelocity": 0.3551572499816586,
-            "velocityX": 1.5501653090794745,
-            "velocityY": -2.2472317553790035,
-            "timestamp": 1.7190145697023518
-        },
-        {
-            "x": 7.66884686477411,
-            "y": 2.640707793733853,
-            "heading": 3.1573927530357144,
-            "angularVelocity": 0.10627269494275392,
-            "velocityX": 0.8382744725035164,
-            "velocityY": -2.202484861535554,
-            "timestamp": 1.8049652981874693
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -0.2023572498136116,
-            "velocityX": 0.12976196272503107,
-            "velocityY": -2.1373616835098828,
-            "timestamp": 1.8909160266725868
-        },
-        {
-            "x": 7.6422705837740335,
-            "y": 2.299493905398916,
-            "heading": 3.1013004562411446,
-            "angularVelocity": -0.5083194372878264,
-            "velocityX": -0.49557678882949796,
-            "velocityY": -2.068846337723125,
-            "timestamp": 1.9670483578801472
-        },
-        {
-            "x": 7.556907286255345,
-            "y": 2.147926635640613,
-            "heading": 3.0424682290459026,
-            "angularVelocity": -0.7727627180470553,
-            "velocityX": -1.121248964332173,
-            "velocityY": -1.9908397306931678,
-            "timestamp": 2.043180689087708
-        },
-        {
-            "x": 7.423923039902345,
-            "y": 2.003393138237165,
-            "heading": 2.9682494555138628,
-            "angularVelocity": -0.974865373945302,
-            "velocityX": -1.7467512716828084,
-            "velocityY": -1.8984509617786702,
-            "timestamp": 2.119313020295268
-        },
-        {
-            "x": 7.2434803504733996,
-            "y": 1.8677477086842909,
-            "heading": 2.8865548866835007,
-            "angularVelocity": -1.07306012484416,
-            "velocityX": -2.3701190619920354,
-            "velocityY": -1.7817059769660042,
-            "timestamp": 2.1954453515028285
-        },
-        {
-            "x": 7.01645692191088,
-            "y": 1.7447501516568382,
-            "heading": 2.813573416858664,
-            "angularVelocity": -0.9586133600147891,
-            "velocityX": -2.9819581899254453,
-            "velocityY": -1.6155758673946832,
-            "timestamp": 2.271577682710389
-        },
-        {
-            "x": 6.750046151807222,
-            "y": 1.6436496548008426,
-            "heading": 2.8060686181878967,
-            "angularVelocity": -0.09857571089344144,
-            "velocityX": -3.499311867613085,
-            "velocityY": -1.327957455819293,
-            "timestamp": 2.347710013917949
-        },
-        {
-            "x": 6.467461314020777,
-            "y": 1.5880425214231262,
-            "heading": 2.8060685831707377,
-            "angularVelocity": -4.5995122509551036e-7,
-            "velocityX": -3.711758635316633,
-            "velocityY": -0.7304010332495747,
-            "timestamp": 2.4238423451255096
-        },
-        {
-            "x": 6.17955680980973,
-            "y": 1.580470049320341,
-            "heading": 2.806068554276105,
-            "angularVelocity": -3.7953169924465946e-7,
-            "velocityX": -3.7816325816443177,
-            "velocityY": -0.09946460304917487,
-            "timestamp": 2.49997467633307
-        },
-        {
-            "x": 5.8949085342429335,
-            "y": 1.5924545384965154,
-            "heading": 2.7975026231592013,
-            "angularVelocity": -0.11251371107452125,
-            "velocityX": -3.738861940149357,
-            "velocityY": 0.1574165533364508,
-            "timestamp": 2.5761070075406303
-        },
-        {
-            "x": 5.65482422655778,
-            "y": 1.6016450496652885,
-            "heading": 2.72071509082511,
-            "angularVelocity": -1.008606082543986,
-            "velocityX": -3.153513151076552,
-            "velocityY": 0.12071758506519875,
-            "timestamp": 2.6522393387481906
-        },
-        {
-            "x": 5.463026554407694,
-            "y": 1.608988353775347,
-            "heading": 2.6410025465355518,
-            "angularVelocity": -1.0470261848719398,
-            "velocityX": -2.5192670329138944,
-            "velocityY": 0.09645447595750602,
-            "timestamp": 2.728371669955751
-        },
-        {
-            "x": 5.319399038502622,
-            "y": 1.614493352033255,
-            "heading": 2.573619865722977,
-            "angularVelocity": -0.8850731317940332,
-            "velocityX": -1.8865508730252805,
-            "velocityY": 0.07230828441193854,
-            "timestamp": 2.8045040011633113
-        },
-        {
-            "x": 5.223767247762021,
-            "y": 1.6181637826708264,
-            "heading": 2.525354405173392,
-            "angularVelocity": -0.6339679842201956,
-            "velocityX": -1.2561258695714954,
-            "velocityY": 0.04821119463109347,
-            "timestamp": 2.8806363323708717
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.33303072126212585,
-            "velocityX": -0.6274239472819125,
-            "velocityY": 0.02411875874607297,
-            "timestamp": 2.956768663578432
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": -1.0690007247060788e-32,
-            "velocityY": 0,
-            "timestamp": 3.0329009947859924
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S4.traj
deleted file mode 100644
index 43401240..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4 To S4.traj	
+++ /dev/null
@@ -1,346 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.360128743568809,
-            "y": 4.349177185639382,
-            "heading": 2.7782206485451093,
-            "angularVelocity": 0.3283351874090942,
-            "velocityX": 0.6995722389859838,
-            "velocityY": -0.1259188206007136,
-            "timestamp": 0.0859507284851176
-        },
-        {
-            "x": 2.4804864804971807,
-            "y": 4.327553542642656,
-            "heading": 2.831856423124361,
-            "angularVelocity": 0.6240293191760308,
-            "velocityX": 1.4003108414515997,
-            "velocityY": -0.2515818466910346,
-            "timestamp": 0.1719014569702352
-        },
-        {
-            "x": 2.6612218913219765,
-            "y": 4.295165149158174,
-            "heading": 2.9060715845563863,
-            "angularVelocity": 0.863461691832829,
-            "velocityX": 2.102779278433835,
-            "velocityY": -0.3768251189411322,
-            "timestamp": 0.2578521854553528
-        },
-        {
-            "x": 2.902552042800783,
-            "y": 4.252097385240196,
-            "heading": 2.990739532355289,
-            "angularVelocity": 0.9850753948357858,
-            "velocityX": 2.8077731943900375,
-            "velocityY": -0.5010750307419894,
-            "timestamp": 0.3438029139404704
-        },
-        {
-            "x": 3.2042136582065344,
-            "y": 4.198837394002331,
-            "heading": 3.04987623940461,
-            "angularVelocity": 0.6880303179693488,
-            "velocityX": 3.5097039981224545,
-            "velocityY": -0.6196572405676055,
-            "timestamp": 0.429753642425588
-        },
-        {
-            "x": 3.524112842751923,
-            "y": 4.1406587673803905,
-            "heading": 3.0498762636795202,
-            "angularVelocity": 2.824282081012085e-7,
-            "velocityX": 3.7218903223232087,
-            "velocityY": -0.6768834615754469,
-            "timestamp": 0.5157043709107056
-        },
-        {
-            "x": 3.844012025527027,
-            "y": 4.082480131024319,
-            "heading": 3.0498762879543886,
-            "angularVelocity": 2.824277225814376e-7,
-            "velocityX": 3.7218903017266984,
-            "velocityY": -0.6768835748279012,
-            "timestamp": 0.6016550993958233
-        },
-        {
-            "x": 4.163911208302122,
-            "y": 4.0243014946682,
-            "heading": 3.049876312229257,
-            "angularVelocity": 2.824277213255298e-7,
-            "velocityX": 3.721890301726596,
-            "velocityY": -0.6768835748284626,
-            "timestamp": 0.6876058278809409
-        },
-        {
-            "x": 4.483810391077216,
-            "y": 3.9661228583120804,
-            "heading": 3.0498763365041253,
-            "angularVelocity": 2.8242772165714766e-7,
-            "velocityX": 3.7218903017265963,
-            "velocityY": -0.6768835748284627,
-            "timestamp": 0.7735565563660586
-        },
-        {
-            "x": 4.80370957385231,
-            "y": 3.9079442219559613,
-            "heading": 3.0498763607789936,
-            "angularVelocity": 2.8242772140625194e-7,
-            "velocityX": 3.7218903017265963,
-            "velocityY": -0.6768835748284627,
-            "timestamp": 0.8595072848511762
-        },
-        {
-            "x": 5.123608756627404,
-            "y": 3.8497655855998416,
-            "heading": 3.049876385053862,
-            "angularVelocity": 2.824277226048516e-7,
-            "velocityX": 3.7218903017265963,
-            "velocityY": -0.6768835748284628,
-            "timestamp": 0.9454580133362939
-        },
-        {
-            "x": 5.443507939402499,
-            "y": 3.7915869492437224,
-            "heading": 3.0498764093287303,
-            "angularVelocity": 2.8242772163119933e-7,
-            "velocityX": 3.7218903017265963,
-            "velocityY": -0.6768835748284626,
-            "timestamp": 1.0314087418214115
-        },
-        {
-            "x": 5.763407122177593,
-            "y": 3.733408312887603,
-            "heading": 3.0498764336035986,
-            "angularVelocity": 2.824277219712686e-7,
-            "velocityX": 3.7218903017265963,
-            "velocityY": -0.6768835748284628,
-            "timestamp": 1.117359470306529
-        },
-        {
-            "x": 6.083306304952576,
-            "y": 3.675229676530871,
-            "heading": 3.049876457878467,
-            "angularVelocity": 2.8242772161077964e-7,
-            "velocityX": 3.7218903017253,
-            "velocityY": -0.6768835748355908,
-            "timestamp": 1.2033101987916466
-        },
-        {
-            "x": 6.403205465262658,
-            "y": 3.6170509166494913,
-            "heading": 3.0498764821533357,
-            "angularVelocity": 2.8242772554820356e-7,
-            "velocityX": 3.721890040355771,
-            "velocityY": -0.6768850119920853,
-            "timestamp": 1.2892609272767641
-        },
-        {
-            "x": 6.718573965768454,
-            "y": 3.5379121175247272,
-            "heading": 3.0498765068050417,
-            "angularVelocity": 2.868120651983924e-7,
-            "velocityX": 3.6691777494404794,
-            "velocityY": -0.9207461125646453,
-            "timestamp": 1.3752116557618816
-        },
-        {
-            "x": 7.013358117396929,
-            "y": 3.4007168540694233,
-            "heading": 3.0498765454511387,
-            "angularVelocity": 4.49630829762963e-7,
-            "velocityX": 3.4296876457482686,
-            "velocityY": -1.5962082680784608,
-            "timestamp": 1.4611623842469992
-        },
-        {
-            "x": 7.268807462845475,
-            "y": 3.2166766255285517,
-            "heading": 3.0757384072562206,
-            "angularVelocity": 0.30089171157615124,
-            "velocityX": 2.9720439832312224,
-            "velocityY": -2.1412294204433464,
-            "timestamp": 1.5471131127321167
-        },
-        {
-            "x": 7.46355872560419,
-            "y": 3.0231641785099956,
-            "heading": 3.117732513124605,
-            "angularVelocity": 0.48858347809886377,
-            "velocityX": 2.2658477268453225,
-            "velocityY": -2.2514346350428145,
-            "timestamp": 1.6330638412172342
-        },
-        {
-            "x": 7.596796563191941,
-            "y": 2.830012972060279,
-            "heading": 3.1482585374873016,
-            "angularVelocity": 0.3551572499816586,
-            "velocityX": 1.5501653090794745,
-            "velocityY": -2.2472317553790035,
-            "timestamp": 1.7190145697023518
-        },
-        {
-            "x": 7.66884686477411,
-            "y": 2.640707793733853,
-            "heading": 3.1573927530357144,
-            "angularVelocity": 0.10627269494275392,
-            "velocityX": 0.8382744725035164,
-            "velocityY": -2.202484861535554,
-            "timestamp": 1.8049652981874693
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -0.2023572498136116,
-            "velocityX": 0.12976196272503107,
-            "velocityY": -2.1373616835098828,
-            "timestamp": 1.8909160266725868
-        },
-        {
-            "x": 7.6422705837740335,
-            "y": 2.299493905398916,
-            "heading": 3.1013004562411446,
-            "angularVelocity": -0.5083194372878264,
-            "velocityX": -0.49557678882949796,
-            "velocityY": -2.068846337723125,
-            "timestamp": 1.9670483578801472
-        },
-        {
-            "x": 7.556907286255345,
-            "y": 2.147926635640613,
-            "heading": 3.0424682290459026,
-            "angularVelocity": -0.7727627180470553,
-            "velocityX": -1.121248964332173,
-            "velocityY": -1.9908397306931678,
-            "timestamp": 2.043180689087708
-        },
-        {
-            "x": 7.423923039902345,
-            "y": 2.003393138237165,
-            "heading": 2.9682494555138628,
-            "angularVelocity": -0.974865373945302,
-            "velocityX": -1.7467512716828084,
-            "velocityY": -1.8984509617786702,
-            "timestamp": 2.119313020295268
-        },
-        {
-            "x": 7.2434803504733996,
-            "y": 1.8677477086842909,
-            "heading": 2.8865548866835007,
-            "angularVelocity": -1.07306012484416,
-            "velocityX": -2.3701190619920354,
-            "velocityY": -1.7817059769660042,
-            "timestamp": 2.1954453515028285
-        },
-        {
-            "x": 7.01645692191088,
-            "y": 1.7447501516568382,
-            "heading": 2.813573416858664,
-            "angularVelocity": -0.9586133600147891,
-            "velocityX": -2.9819581899254453,
-            "velocityY": -1.6155758673946832,
-            "timestamp": 2.271577682710389
-        },
-        {
-            "x": 6.750046151807222,
-            "y": 1.6436496548008426,
-            "heading": 2.8060686181878967,
-            "angularVelocity": -0.09857571089344144,
-            "velocityX": -3.499311867613085,
-            "velocityY": -1.327957455819293,
-            "timestamp": 2.347710013917949
-        },
-        {
-            "x": 6.467461314020777,
-            "y": 1.5880425214231262,
-            "heading": 2.8060685831707377,
-            "angularVelocity": -4.5995122509551036e-7,
-            "velocityX": -3.711758635316633,
-            "velocityY": -0.7304010332495747,
-            "timestamp": 2.4238423451255096
-        },
-        {
-            "x": 6.17955680980973,
-            "y": 1.580470049320341,
-            "heading": 2.806068554276105,
-            "angularVelocity": -3.7953169924465946e-7,
-            "velocityX": -3.7816325816443177,
-            "velocityY": -0.09946460304917487,
-            "timestamp": 2.49997467633307
-        },
-        {
-            "x": 5.8949085342429335,
-            "y": 1.5924545384965154,
-            "heading": 2.7975026231592013,
-            "angularVelocity": -0.11251371107452125,
-            "velocityX": -3.738861940149357,
-            "velocityY": 0.1574165533364508,
-            "timestamp": 2.5761070075406303
-        },
-        {
-            "x": 5.65482422655778,
-            "y": 1.6016450496652885,
-            "heading": 2.72071509082511,
-            "angularVelocity": -1.008606082543986,
-            "velocityX": -3.153513151076552,
-            "velocityY": 0.12071758506519875,
-            "timestamp": 2.6522393387481906
-        },
-        {
-            "x": 5.463026554407694,
-            "y": 1.608988353775347,
-            "heading": 2.6410025465355518,
-            "angularVelocity": -1.0470261848719398,
-            "velocityX": -2.5192670329138944,
-            "velocityY": 0.09645447595750602,
-            "timestamp": 2.728371669955751
-        },
-        {
-            "x": 5.319399038502622,
-            "y": 1.614493352033255,
-            "heading": 2.573619865722977,
-            "angularVelocity": -0.8850731317940332,
-            "velocityX": -1.8865508730252805,
-            "velocityY": 0.07230828441193854,
-            "timestamp": 2.8045040011633113
-        },
-        {
-            "x": 5.223767247762021,
-            "y": 1.6181637826708264,
-            "heading": 2.525354405173392,
-            "angularVelocity": -0.6339679842201956,
-            "velocityX": -1.2561258695714954,
-            "velocityY": 0.04821119463109347,
-            "timestamp": 2.8806363323708717
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.33303072126212585,
-            "velocityX": -0.6274239472819125,
-            "velocityY": 0.02411875874607297,
-            "timestamp": 2.956768663578432
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": -1.0690007247060788e-32,
-            "velocityY": 0,
-            "timestamp": 3.0329009947859924
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S3.1.traj
deleted file mode 100644
index fda2de80..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S3.1.traj	
+++ /dev/null
@@ -1,400 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.35465792702043,
-            "y": 4.32401597789768,
-            "heading": 2.7661167341638526,
-            "angularVelocity": 0.1817092285961366,
-            "velocityX": 0.6162445601306747,
-            "velocityY": -0.4057043338633825,
-            "timestamp": 0.08869518784691614
-        },
-        {
-            "x": 2.464008141320373,
-            "y": 4.252030892508781,
-            "heading": 2.796228115996352,
-            "angularVelocity": 0.3394928469453156,
-            "velocityX": 1.2328765173672827,
-            "velocityY": -0.8116007997315744,
-            "timestamp": 0.1773903756938323
-        },
-        {
-            "x": 2.6280987448252096,
-            "y": 4.144022899680214,
-            "heading": 2.836753181719006,
-            "angularVelocity": 0.4569026427070467,
-            "velocityX": 1.8500508030723093,
-            "velocityY": -1.2177435490072333,
-            "timestamp": 0.2660855635407484
-        },
-        {
-            "x": 2.8469891593833587,
-            "y": 3.9999743075055525,
-            "heading": 2.8803143946879453,
-            "angularVelocity": 0.49113389380407046,
-            "velocityX": 2.467895044497164,
-            "velocityY": -1.624085766899582,
-            "timestamp": 0.3547807513876646
-        },
-        {
-            "x": 3.12048957146799,
-            "y": 3.8200884895179508,
-            "heading": 2.9023929043889884,
-            "angularVelocity": 0.248925677221065,
-            "velocityX": 3.083599220249474,
-            "velocityY": -2.028135035894805,
-            "timestamp": 0.44347593923458073
-        },
-        {
-            "x": 3.4006775749010125,
-            "y": 3.6354966761760505,
-            "heading": 2.9023929178552135,
-            "angularVelocity": 1.5182588146629277e-7,
-            "velocityX": 3.1589989291934804,
-            "velocityY": -2.081193104416185,
-            "timestamp": 0.5321711270814968
-        },
-        {
-            "x": 3.680865577691327,
-            "y": 3.450904861858589,
-            "heading": 2.9023929313214296,
-            "angularVelocity": 1.518257825170986e-7,
-            "velocityX": 3.1589989219472225,
-            "velocityY": -2.081193115415224,
-            "timestamp": 0.6208663149284129
-        },
-        {
-            "x": 3.961053580481639,
-            "y": 3.2663130475411224,
-            "heading": 2.902392944787646,
-            "angularVelocity": 1.5182578291151429e-7,
-            "velocityX": 3.1589989219471897,
-            "velocityY": -2.081193115415273,
-            "timestamp": 0.709561502775329
-        },
-        {
-            "x": 4.24124158327195,
-            "y": 3.0817212332236563,
-            "heading": 2.9023929582538623,
-            "angularVelocity": 1.518257833819606e-7,
-            "velocityX": 3.1589989219471897,
-            "velocityY": -2.0811931154152736,
-            "timestamp": 0.7982566906222451
-        },
-        {
-            "x": 4.521429586062262,
-            "y": 2.89712941890619,
-            "heading": 2.9023929717200785,
-            "angularVelocity": 1.5182578356012397e-7,
-            "velocityX": 3.1589989219471892,
-            "velocityY": -2.081193115415273,
-            "timestamp": 0.8869518784691612
-        },
-        {
-            "x": 4.801617588852573,
-            "y": 2.7125376045887237,
-            "heading": 2.902392985186295,
-            "angularVelocity": 1.518257838228889e-7,
-            "velocityX": 3.15899892194719,
-            "velocityY": -2.0811931154152736,
-            "timestamp": 0.9756470663160773
-        },
-        {
-            "x": 5.081805591642885,
-            "y": 2.5279457902712577,
-            "heading": 2.902392998652511,
-            "angularVelocity": 1.5182578286793036e-7,
-            "velocityX": 3.1589989219471897,
-            "velocityY": -2.081193115415273,
-            "timestamp": 1.0643422541629934
-        },
-        {
-            "x": 5.361993594433196,
-            "y": 2.343353975953791,
-            "heading": 2.9023930121187274,
-            "angularVelocity": 1.5182578369194972e-7,
-            "velocityX": 3.1589989219471897,
-            "velocityY": -2.0811931154152736,
-            "timestamp": 1.1530374420099097
-        },
-        {
-            "x": 5.642181597223507,
-            "y": 2.158762161636325,
-            "heading": 2.902393025584944,
-            "angularVelocity": 1.5182578337777967e-7,
-            "velocityX": 3.1589989219471897,
-            "velocityY": -2.081193115415273,
-            "timestamp": 1.2417326298568259
-        },
-        {
-            "x": 5.922369600013819,
-            "y": 1.9741703473188588,
-            "heading": 2.90239303905116,
-            "angularVelocity": 1.5182578259222556e-7,
-            "velocityX": 3.15899892194719,
-            "velocityY": -2.081193115415273,
-            "timestamp": 1.330427817703742
-        },
-        {
-            "x": 6.202557602804131,
-            "y": 1.7895785330013922,
-            "heading": 2.9023930525173767,
-            "angularVelocity": 1.5182578252273246e-7,
-            "velocityX": 3.1589989219471892,
-            "velocityY": -2.0811931154152736,
-            "timestamp": 1.4191230055506583
-        },
-        {
-            "x": 6.482745605594443,
-            "y": 1.604986718683926,
-            "heading": 2.902393065983593,
-            "angularVelocity": 1.5182578277552693e-7,
-            "velocityX": 3.1589989219471897,
-            "velocityY": -2.081193115415273,
-            "timestamp": 1.5078181933975745
-        },
-        {
-            "x": 6.762933608384558,
-            "y": 1.4203949043661621,
-            "heading": 2.902393079449809,
-            "angularVelocity": 1.5182578233984683e-7,
-            "velocityX": 3.1589989219449803,
-            "velocityY": -2.081193115418628,
-            "timestamp": 1.5965133812444907
-        },
-        {
-            "x": 7.043121567747365,
-            "y": 1.2358030241312514,
-            "heading": 2.902393092916261,
-            "angularVelocity": 1.5182844252968712e-7,
-            "velocityX": 3.158998432320794,
-            "velocityY": -2.08119385860604,
-            "timestamp": 1.685208569091407
-        },
-        {
-            "x": 7.279459556862412,
-            "y": 1.0742756485961233,
-            "heading": 2.998760727595811,
-            "angularVelocity": 1.0865035298857004,
-            "velocityX": 2.6646089247023874,
-            "velocityY": -1.8211515129086453,
-            "timestamp": 1.7739037569383231
-        },
-        {
-            "x": 7.461316655805292,
-            "y": 0.9486305467963247,
-            "heading": 3.0706677283735453,
-            "angularVelocity": 0.8107204294086697,
-            "velocityX": 2.0503603787023565,
-            "velocityY": -1.416594348011941,
-            "timestamp": 1.8625989447852394
-        },
-        {
-            "x": 7.588692434846267,
-            "y": 0.8588692195479983,
-            "heading": 3.1181671121508234,
-            "angularVelocity": 0.5355350716350028,
-            "velocityX": 1.4361069876848378,
-            "velocityY": -1.0120202620603307,
-            "timestamp": 1.9512941326321556
-        },
-        {
-            "x": 7.661586847053616,
-            "y": 0.8049924095541465,
-            "heading": 3.141279756163227,
-            "angularVelocity": 0.26058509569081495,
-            "velocityX": 0.8218530675323843,
-            "velocityY": -0.6074378024526055,
-            "timestamp": 2.0399893204790716
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": -0.01442869894402112,
-            "velocityX": 0.20760036021529943,
-            "velocityY": -0.20285665988103574,
-            "timestamp": 2.1286845083259878
-        },
-        {
-            "x": 7.650032668413058,
-            "y": 0.8012856501840878,
-            "heading": 3.117704293378248,
-            "angularVelocity": -0.2703970639421689,
-            "velocityX": -0.3634367195783461,
-            "velocityY": 0.17325299134110608,
-            "timestamp": 2.211139939966771
-        },
-        {
-            "x": 7.572958138860234,
-            "y": 0.846621328160437,
-            "heading": 3.075672962840878,
-            "angularVelocity": -0.509746049483795,
-            "velocityX": -0.9347416903788446,
-            "velocityY": 0.5498203947782849,
-            "timestamp": 2.293595371607554
-        },
-        {
-            "x": 7.448745482136049,
-            "y": 0.9230647936190245,
-            "heading": 3.016123238078663,
-            "angularVelocity": -0.7222049970175785,
-            "velocityX": -1.5064217632783534,
-            "velocityY": 0.927088294093387,
-            "timestamp": 2.376050803248337
-        },
-        {
-            "x": 7.277351379823213,
-            "y": 1.0307130669975626,
-            "heading": 2.9432415686566307,
-            "angularVelocity": -0.8838916730136303,
-            "velocityX": -2.0786271917114503,
-            "velocityY": 1.305532834362055,
-            "timestamp": 2.45850623488912
-        },
-        {
-            "x": 7.058753992164297,
-            "y": 1.1697461046151478,
-            "heading": 2.8675628117721894,
-            "angularVelocity": -0.9178140891207157,
-            "velocityX": -2.6510974875643654,
-            "velocityY": 1.6861598423652984,
-            "timestamp": 2.540961666529903
-        },
-        {
-            "x": 6.796121093379507,
-            "y": 1.3380331209180303,
-            "heading": 2.867562732679127,
-            "angularVelocity": -9.592219802669244e-7,
-            "velocityX": -3.1851497658632115,
-            "velocityY": 2.0409451864375003,
-            "timestamp": 2.623417098170686
-        },
-        {
-            "x": 6.5354030679592325,
-            "y": 1.509271805879804,
-            "heading": 2.8675627155127748,
-            "angularVelocity": -2.0818946417191608e-7,
-            "velocityX": -3.161926633967453,
-            "velocityY": 2.076742326785391,
-            "timestamp": 2.705872529811469
-        },
-        {
-            "x": 6.274685053353285,
-            "y": 1.680510507306839,
-            "heading": 2.867562698346423,
-            "angularVelocity": -2.0818946436511562e-7,
-            "velocityX": -3.161926502813852,
-            "velocityY": 2.0767425264721937,
-            "timestamp": 2.7883279614522523
-        },
-        {
-            "x": 6.013967038747397,
-            "y": 1.851749208733966,
-            "heading": 2.8675626811800705,
-            "angularVelocity": -2.0818946408004615e-7,
-            "velocityX": -3.1619265028131216,
-            "velocityY": 2.076742526473307,
-            "timestamp": 2.8707833930930353
-        },
-        {
-            "x": 5.75324902414151,
-            "y": 2.022987910161093,
-            "heading": 2.8675626640137186,
-            "angularVelocity": -2.081894642016249e-7,
-            "velocityX": -3.1619265028131216,
-            "velocityY": 2.076742526473306,
-            "timestamp": 2.9532388247338184
-        },
-        {
-            "x": 5.492531009535623,
-            "y": 2.194226611588219,
-            "heading": 2.8675626468473663,
-            "angularVelocity": -2.0818946425512402e-7,
-            "velocityX": -3.1619265028131216,
-            "velocityY": 2.076742526473306,
-            "timestamp": 3.0356942563746014
-        },
-        {
-            "x": 5.231812994929702,
-            "y": 2.3654653130152945,
-            "heading": 2.8675626296810144,
-            "angularVelocity": -2.0818946468474427e-7,
-            "velocityX": -3.1619265028135324,
-            "velocityY": 2.0767425264726813,
-            "timestamp": 3.1181496880153845
-        },
-        {
-            "x": 4.971094974250145,
-            "y": 2.536704005194925,
-            "heading": 2.8675626125145612,
-            "angularVelocity": -2.0819068604958381e-7,
-            "velocityX": -3.16192657647314,
-            "velocityY": 2.076742414321854,
-            "timestamp": 3.2006051196561676
-        },
-        {
-            "x": 4.734733232154558,
-            "y": 2.6911196483021613,
-            "heading": 2.78339324120731,
-            "angularVelocity": -1.020786255463861,
-            "velocityX": -2.8665393824544814,
-            "velocityY": 1.8727164485651855,
-            "timestamp": 3.2830605512969506
-        },
-        {
-            "x": 4.545913318554367,
-            "y": 2.8146834617063132,
-            "heading": 2.703884624892798,
-            "angularVelocity": -0.9642617197238292,
-            "velocityX": -2.289963315246283,
-            "velocityY": 1.4985527447416418,
-            "timestamp": 3.3655159829377337
-        },
-        {
-            "x": 4.404411416841804,
-            "y": 2.9073475116185867,
-            "heading": 2.6403705813576916,
-            "angularVelocity": -0.7702833187728019,
-            "velocityX": -1.7161016429944431,
-            "velocityY": 1.1238077112489557,
-            "timestamp": 3.4479714145785167
-        },
-        {
-            "x": 4.3101255761487,
-            "y": 2.9691175811007473,
-            "heading": 2.5964788856355536,
-            "angularVelocity": -0.5323081190497229,
-            "velocityX": -1.1434764067922174,
-            "velocityY": 0.7491328133634895,
-            "timestamp": 3.5304268462193
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -0.27261861575697227,
-            "velocityX": -0.5715278570610401,
-            "velocityY": 0.37453468236989096,
-            "timestamp": 3.612882277860083
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": -3.9375732354803004e-35,
-            "velocityY": 0,
-            "timestamp": 3.695337709500866
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S3.traj
deleted file mode 100644
index fda2de80..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S3.traj	
+++ /dev/null
@@ -1,400 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.35465792702043,
-            "y": 4.32401597789768,
-            "heading": 2.7661167341638526,
-            "angularVelocity": 0.1817092285961366,
-            "velocityX": 0.6162445601306747,
-            "velocityY": -0.4057043338633825,
-            "timestamp": 0.08869518784691614
-        },
-        {
-            "x": 2.464008141320373,
-            "y": 4.252030892508781,
-            "heading": 2.796228115996352,
-            "angularVelocity": 0.3394928469453156,
-            "velocityX": 1.2328765173672827,
-            "velocityY": -0.8116007997315744,
-            "timestamp": 0.1773903756938323
-        },
-        {
-            "x": 2.6280987448252096,
-            "y": 4.144022899680214,
-            "heading": 2.836753181719006,
-            "angularVelocity": 0.4569026427070467,
-            "velocityX": 1.8500508030723093,
-            "velocityY": -1.2177435490072333,
-            "timestamp": 0.2660855635407484
-        },
-        {
-            "x": 2.8469891593833587,
-            "y": 3.9999743075055525,
-            "heading": 2.8803143946879453,
-            "angularVelocity": 0.49113389380407046,
-            "velocityX": 2.467895044497164,
-            "velocityY": -1.624085766899582,
-            "timestamp": 0.3547807513876646
-        },
-        {
-            "x": 3.12048957146799,
-            "y": 3.8200884895179508,
-            "heading": 2.9023929043889884,
-            "angularVelocity": 0.248925677221065,
-            "velocityX": 3.083599220249474,
-            "velocityY": -2.028135035894805,
-            "timestamp": 0.44347593923458073
-        },
-        {
-            "x": 3.4006775749010125,
-            "y": 3.6354966761760505,
-            "heading": 2.9023929178552135,
-            "angularVelocity": 1.5182588146629277e-7,
-            "velocityX": 3.1589989291934804,
-            "velocityY": -2.081193104416185,
-            "timestamp": 0.5321711270814968
-        },
-        {
-            "x": 3.680865577691327,
-            "y": 3.450904861858589,
-            "heading": 2.9023929313214296,
-            "angularVelocity": 1.518257825170986e-7,
-            "velocityX": 3.1589989219472225,
-            "velocityY": -2.081193115415224,
-            "timestamp": 0.6208663149284129
-        },
-        {
-            "x": 3.961053580481639,
-            "y": 3.2663130475411224,
-            "heading": 2.902392944787646,
-            "angularVelocity": 1.5182578291151429e-7,
-            "velocityX": 3.1589989219471897,
-            "velocityY": -2.081193115415273,
-            "timestamp": 0.709561502775329
-        },
-        {
-            "x": 4.24124158327195,
-            "y": 3.0817212332236563,
-            "heading": 2.9023929582538623,
-            "angularVelocity": 1.518257833819606e-7,
-            "velocityX": 3.1589989219471897,
-            "velocityY": -2.0811931154152736,
-            "timestamp": 0.7982566906222451
-        },
-        {
-            "x": 4.521429586062262,
-            "y": 2.89712941890619,
-            "heading": 2.9023929717200785,
-            "angularVelocity": 1.5182578356012397e-7,
-            "velocityX": 3.1589989219471892,
-            "velocityY": -2.081193115415273,
-            "timestamp": 0.8869518784691612
-        },
-        {
-            "x": 4.801617588852573,
-            "y": 2.7125376045887237,
-            "heading": 2.902392985186295,
-            "angularVelocity": 1.518257838228889e-7,
-            "velocityX": 3.15899892194719,
-            "velocityY": -2.0811931154152736,
-            "timestamp": 0.9756470663160773
-        },
-        {
-            "x": 5.081805591642885,
-            "y": 2.5279457902712577,
-            "heading": 2.902392998652511,
-            "angularVelocity": 1.5182578286793036e-7,
-            "velocityX": 3.1589989219471897,
-            "velocityY": -2.081193115415273,
-            "timestamp": 1.0643422541629934
-        },
-        {
-            "x": 5.361993594433196,
-            "y": 2.343353975953791,
-            "heading": 2.9023930121187274,
-            "angularVelocity": 1.5182578369194972e-7,
-            "velocityX": 3.1589989219471897,
-            "velocityY": -2.0811931154152736,
-            "timestamp": 1.1530374420099097
-        },
-        {
-            "x": 5.642181597223507,
-            "y": 2.158762161636325,
-            "heading": 2.902393025584944,
-            "angularVelocity": 1.5182578337777967e-7,
-            "velocityX": 3.1589989219471897,
-            "velocityY": -2.081193115415273,
-            "timestamp": 1.2417326298568259
-        },
-        {
-            "x": 5.922369600013819,
-            "y": 1.9741703473188588,
-            "heading": 2.90239303905116,
-            "angularVelocity": 1.5182578259222556e-7,
-            "velocityX": 3.15899892194719,
-            "velocityY": -2.081193115415273,
-            "timestamp": 1.330427817703742
-        },
-        {
-            "x": 6.202557602804131,
-            "y": 1.7895785330013922,
-            "heading": 2.9023930525173767,
-            "angularVelocity": 1.5182578252273246e-7,
-            "velocityX": 3.1589989219471892,
-            "velocityY": -2.0811931154152736,
-            "timestamp": 1.4191230055506583
-        },
-        {
-            "x": 6.482745605594443,
-            "y": 1.604986718683926,
-            "heading": 2.902393065983593,
-            "angularVelocity": 1.5182578277552693e-7,
-            "velocityX": 3.1589989219471897,
-            "velocityY": -2.081193115415273,
-            "timestamp": 1.5078181933975745
-        },
-        {
-            "x": 6.762933608384558,
-            "y": 1.4203949043661621,
-            "heading": 2.902393079449809,
-            "angularVelocity": 1.5182578233984683e-7,
-            "velocityX": 3.1589989219449803,
-            "velocityY": -2.081193115418628,
-            "timestamp": 1.5965133812444907
-        },
-        {
-            "x": 7.043121567747365,
-            "y": 1.2358030241312514,
-            "heading": 2.902393092916261,
-            "angularVelocity": 1.5182844252968712e-7,
-            "velocityX": 3.158998432320794,
-            "velocityY": -2.08119385860604,
-            "timestamp": 1.685208569091407
-        },
-        {
-            "x": 7.279459556862412,
-            "y": 1.0742756485961233,
-            "heading": 2.998760727595811,
-            "angularVelocity": 1.0865035298857004,
-            "velocityX": 2.6646089247023874,
-            "velocityY": -1.8211515129086453,
-            "timestamp": 1.7739037569383231
-        },
-        {
-            "x": 7.461316655805292,
-            "y": 0.9486305467963247,
-            "heading": 3.0706677283735453,
-            "angularVelocity": 0.8107204294086697,
-            "velocityX": 2.0503603787023565,
-            "velocityY": -1.416594348011941,
-            "timestamp": 1.8625989447852394
-        },
-        {
-            "x": 7.588692434846267,
-            "y": 0.8588692195479983,
-            "heading": 3.1181671121508234,
-            "angularVelocity": 0.5355350716350028,
-            "velocityX": 1.4361069876848378,
-            "velocityY": -1.0120202620603307,
-            "timestamp": 1.9512941326321556
-        },
-        {
-            "x": 7.661586847053616,
-            "y": 0.8049924095541465,
-            "heading": 3.141279756163227,
-            "angularVelocity": 0.26058509569081495,
-            "velocityX": 0.8218530675323843,
-            "velocityY": -0.6074378024526055,
-            "timestamp": 2.0399893204790716
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": -0.01442869894402112,
-            "velocityX": 0.20760036021529943,
-            "velocityY": -0.20285665988103574,
-            "timestamp": 2.1286845083259878
-        },
-        {
-            "x": 7.650032668413058,
-            "y": 0.8012856501840878,
-            "heading": 3.117704293378248,
-            "angularVelocity": -0.2703970639421689,
-            "velocityX": -0.3634367195783461,
-            "velocityY": 0.17325299134110608,
-            "timestamp": 2.211139939966771
-        },
-        {
-            "x": 7.572958138860234,
-            "y": 0.846621328160437,
-            "heading": 3.075672962840878,
-            "angularVelocity": -0.509746049483795,
-            "velocityX": -0.9347416903788446,
-            "velocityY": 0.5498203947782849,
-            "timestamp": 2.293595371607554
-        },
-        {
-            "x": 7.448745482136049,
-            "y": 0.9230647936190245,
-            "heading": 3.016123238078663,
-            "angularVelocity": -0.7222049970175785,
-            "velocityX": -1.5064217632783534,
-            "velocityY": 0.927088294093387,
-            "timestamp": 2.376050803248337
-        },
-        {
-            "x": 7.277351379823213,
-            "y": 1.0307130669975626,
-            "heading": 2.9432415686566307,
-            "angularVelocity": -0.8838916730136303,
-            "velocityX": -2.0786271917114503,
-            "velocityY": 1.305532834362055,
-            "timestamp": 2.45850623488912
-        },
-        {
-            "x": 7.058753992164297,
-            "y": 1.1697461046151478,
-            "heading": 2.8675628117721894,
-            "angularVelocity": -0.9178140891207157,
-            "velocityX": -2.6510974875643654,
-            "velocityY": 1.6861598423652984,
-            "timestamp": 2.540961666529903
-        },
-        {
-            "x": 6.796121093379507,
-            "y": 1.3380331209180303,
-            "heading": 2.867562732679127,
-            "angularVelocity": -9.592219802669244e-7,
-            "velocityX": -3.1851497658632115,
-            "velocityY": 2.0409451864375003,
-            "timestamp": 2.623417098170686
-        },
-        {
-            "x": 6.5354030679592325,
-            "y": 1.509271805879804,
-            "heading": 2.8675627155127748,
-            "angularVelocity": -2.0818946417191608e-7,
-            "velocityX": -3.161926633967453,
-            "velocityY": 2.076742326785391,
-            "timestamp": 2.705872529811469
-        },
-        {
-            "x": 6.274685053353285,
-            "y": 1.680510507306839,
-            "heading": 2.867562698346423,
-            "angularVelocity": -2.0818946436511562e-7,
-            "velocityX": -3.161926502813852,
-            "velocityY": 2.0767425264721937,
-            "timestamp": 2.7883279614522523
-        },
-        {
-            "x": 6.013967038747397,
-            "y": 1.851749208733966,
-            "heading": 2.8675626811800705,
-            "angularVelocity": -2.0818946408004615e-7,
-            "velocityX": -3.1619265028131216,
-            "velocityY": 2.076742526473307,
-            "timestamp": 2.8707833930930353
-        },
-        {
-            "x": 5.75324902414151,
-            "y": 2.022987910161093,
-            "heading": 2.8675626640137186,
-            "angularVelocity": -2.081894642016249e-7,
-            "velocityX": -3.1619265028131216,
-            "velocityY": 2.076742526473306,
-            "timestamp": 2.9532388247338184
-        },
-        {
-            "x": 5.492531009535623,
-            "y": 2.194226611588219,
-            "heading": 2.8675626468473663,
-            "angularVelocity": -2.0818946425512402e-7,
-            "velocityX": -3.1619265028131216,
-            "velocityY": 2.076742526473306,
-            "timestamp": 3.0356942563746014
-        },
-        {
-            "x": 5.231812994929702,
-            "y": 2.3654653130152945,
-            "heading": 2.8675626296810144,
-            "angularVelocity": -2.0818946468474427e-7,
-            "velocityX": -3.1619265028135324,
-            "velocityY": 2.0767425264726813,
-            "timestamp": 3.1181496880153845
-        },
-        {
-            "x": 4.971094974250145,
-            "y": 2.536704005194925,
-            "heading": 2.8675626125145612,
-            "angularVelocity": -2.0819068604958381e-7,
-            "velocityX": -3.16192657647314,
-            "velocityY": 2.076742414321854,
-            "timestamp": 3.2006051196561676
-        },
-        {
-            "x": 4.734733232154558,
-            "y": 2.6911196483021613,
-            "heading": 2.78339324120731,
-            "angularVelocity": -1.020786255463861,
-            "velocityX": -2.8665393824544814,
-            "velocityY": 1.8727164485651855,
-            "timestamp": 3.2830605512969506
-        },
-        {
-            "x": 4.545913318554367,
-            "y": 2.8146834617063132,
-            "heading": 2.703884624892798,
-            "angularVelocity": -0.9642617197238292,
-            "velocityX": -2.289963315246283,
-            "velocityY": 1.4985527447416418,
-            "timestamp": 3.3655159829377337
-        },
-        {
-            "x": 4.404411416841804,
-            "y": 2.9073475116185867,
-            "heading": 2.6403705813576916,
-            "angularVelocity": -0.7702833187728019,
-            "velocityX": -1.7161016429944431,
-            "velocityY": 1.1238077112489557,
-            "timestamp": 3.4479714145785167
-        },
-        {
-            "x": 4.3101255761487,
-            "y": 2.9691175811007473,
-            "heading": 2.5964788856355536,
-            "angularVelocity": -0.5323081190497229,
-            "velocityX": -1.1434764067922174,
-            "velocityY": 0.7491328133634895,
-            "timestamp": 3.5304268462193
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -0.27261861575697227,
-            "velocityX": -0.5715278570610401,
-            "velocityY": 0.37453468236989096,
-            "timestamp": 3.612882277860083
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0,
-            "velocityX": -3.9375732354803004e-35,
-            "velocityY": 0,
-            "timestamp": 3.695337709500866
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S4.1.traj
deleted file mode 100644
index 111b63c2..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S4.1.traj	
+++ /dev/null
@@ -1,364 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3565686386495837,
-            "y": 4.327692083019332,
-            "heading": 2.7686204392526563,
-            "angularVelocity": 0.21032675786146612,
-            "velocityX": 0.6389698009998809,
-            "velocityY": -0.36493335842385927,
-            "timestamp": 0.08853100500377892
-        },
-        {
-            "x": 2.4697534077972008,
-            "y": 4.26305693150059,
-            "heading": 2.803412029343583,
-            "angularVelocity": 0.39298763285756855,
-            "velocityX": 1.2784760451187218,
-            "velocityY": -0.7300849178881736,
-            "timestamp": 0.17706201000755784
-        },
-        {
-            "x": 2.639620733022083,
-            "y": 4.166070131606458,
-            "heading": 2.850239105466856,
-            "angularVelocity": 0.5289341979263338,
-            "velocityX": 1.9187325978919179,
-            "velocityY": -1.095512243309488,
-            "timestamp": 0.26559301501133675
-        },
-        {
-            "x": 2.8662525015467275,
-            "y": 4.036714523350174,
-            "heading": 2.9005551954880358,
-            "angularVelocity": 0.5683442768897115,
-            "velocityX": 2.559914105967419,
-            "velocityY": -1.4611333989799566,
-            "timestamp": 0.35412402001511567
-        },
-        {
-            "x": 3.1493645211146295,
-            "y": 3.8752655198055206,
-            "heading": 2.9253298956108997,
-            "angularVelocity": 0.2798420747827705,
-            "velocityX": 3.197885526723865,
-            "velocityY": -1.8236436323948029,
-            "timestamp": 0.4426550250188946
-        },
-        {
-            "x": 3.4401071732873256,
-            "y": 3.709037480706115,
-            "heading": 2.9253299112862043,
-            "angularVelocity": 1.770600526020634e-7,
-            "velocityX": 3.28407716777061,
-            "velocityY": -1.877625122320825,
-            "timestamp": 0.5311860300226735
-        },
-        {
-            "x": 3.730849824609712,
-            "y": 3.5428094401194565,
-            "heading": 2.925329926961498,
-            "angularVelocity": 1.7705993300653618e-7,
-            "velocityX": 3.2840771581659602,
-            "velocityY": -1.87762513912005,
-            "timestamp": 0.6197170350264525
-        },
-        {
-            "x": 4.021592475932095,
-            "y": 3.376581399532791,
-            "heading": 2.9253299426367922,
-            "angularVelocity": 1.7705993370243028e-7,
-            "velocityX": 3.2840771581659167,
-            "velocityY": -1.8776251391201264,
-            "timestamp": 0.7082480400302313
-        },
-        {
-            "x": 4.312335127254477,
-            "y": 3.2103533589461257,
-            "heading": 2.925329958312086,
-            "angularVelocity": 1.7705993363835285e-7,
-            "velocityX": 3.2840771581659163,
-            "velocityY": -1.8776251391201266,
-            "timestamp": 0.7967790450340102
-        },
-        {
-            "x": 4.60307777857686,
-            "y": 3.0441253183594608,
-            "heading": 2.9253299739873797,
-            "angularVelocity": 1.7705993334445259e-7,
-            "velocityX": 3.2840771581659163,
-            "velocityY": -1.8776251391201269,
-            "timestamp": 0.8853100500377891
-        },
-        {
-            "x": 4.893820429899242,
-            "y": 2.877897277772796,
-            "heading": 2.9253299896626737,
-            "angularVelocity": 1.7705993259163212e-7,
-            "velocityX": 3.2840771581659163,
-            "velocityY": -1.877625139120127,
-            "timestamp": 0.9738410550415679
-        },
-        {
-            "x": 5.184563081221626,
-            "y": 2.7116692371861304,
-            "heading": 2.925330005337967,
-            "angularVelocity": 1.7705993390699708e-7,
-            "velocityX": 3.2840771581659163,
-            "velocityY": -1.8776251391201266,
-            "timestamp": 1.0623720600453468
-        },
-        {
-            "x": 5.475305732544008,
-            "y": 2.5454411965994654,
-            "heading": 2.925330021013261,
-            "angularVelocity": 1.770599334284566e-7,
-            "velocityX": 3.2840771581659163,
-            "velocityY": -1.8776251391201269,
-            "timestamp": 1.1509030650491257
-        },
-        {
-            "x": 5.766048383866392,
-            "y": 2.3792131560128005,
-            "heading": 2.925330036688555,
-            "angularVelocity": 1.7705993332802181e-7,
-            "velocityX": 3.2840771581659163,
-            "velocityY": -1.8776251391201264,
-            "timestamp": 1.2394340700529045
-        },
-        {
-            "x": 6.056791035188774,
-            "y": 2.212985115426135,
-            "heading": 2.9253300523638495,
-            "angularVelocity": 1.7705993348595005e-7,
-            "velocityX": 3.2840771581659163,
-            "velocityY": -1.8776251391201266,
-            "timestamp": 1.3279650750566834
-        },
-        {
-            "x": 6.347533686511157,
-            "y": 2.04675707483947,
-            "heading": 2.925330068039143,
-            "angularVelocity": 1.7705993292383098e-7,
-            "velocityX": 3.2840771581659163,
-            "velocityY": -1.8776251391201269,
-            "timestamp": 1.4164960800604622
-        },
-        {
-            "x": 6.638276337833025,
-            "y": 1.8805290342519057,
-            "heading": 2.925330083714437,
-            "angularVelocity": 1.7705993325721192e-7,
-            "velocityX": 3.284077158160109,
-            "velocityY": -1.877625139130284,
-            "timestamp": 1.505027085064241
-        },
-        {
-            "x": 6.929018876242265,
-            "y": 1.7143007961736503,
-            "heading": 2.9253300993897313,
-            "angularVelocity": 1.770599420488751e-7,
-            "velocityX": 3.2840758827580245,
-            "velocityY": -1.8776273698819952,
-            "timestamp": 1.59355809006802
-        },
-        {
-            "x": 7.201522264704821,
-            "y": 1.5196088586386673,
-            "heading": 2.925330126495543,
-            "angularVelocity": 3.061730945539565e-7,
-            "velocityX": 3.078055969780553,
-            "velocityY": -2.1991384546767203,
-            "timestamp": 1.6820890950717988
-        },
-        {
-            "x": 7.413524241952794,
-            "y": 1.3146281632272387,
-            "heading": 3.008225358482996,
-            "angularVelocity": 0.9363412511121879,
-            "velocityX": 2.394663623652733,
-            "velocityY": -2.3153548906699357,
-            "timestamp": 1.7706201000755777
-        },
-        {
-            "x": 7.566471934487395,
-            "y": 1.1372904255598042,
-            "heading": 3.0754099127352705,
-            "angularVelocity": 0.758881639821055,
-            "velocityX": 1.727617262766506,
-            "velocityY": -2.003114475655854,
-            "timestamp": 1.8591511050793565
-        },
-        {
-            "x": 7.66159309798317,
-            "y": 0.9897915834005926,
-            "heading": 3.120981260630991,
-            "angularVelocity": 0.5147501476321013,
-            "velocityX": 1.0744389888232937,
-            "velocityY": -1.6660698944162209,
-            "timestamp": 1.9476821100831354
-        },
-        {
-            "x": 7.6993540657193265,
-            "y": 0.8728983057701601,
-            "heading": 3.1428690494072535,
-            "angularVelocity": 0.2472330318097442,
-            "velocityX": 0.4265281720741691,
-            "velocityY": -1.3203654202890747,
-            "timestamp": 2.0362131150869143
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": -0.03240728383410146,
-            "velocityX": -0.21861341931571387,
-            "velocityY": -0.9702624043010712,
-            "timestamp": 2.124744120090693
-        },
-        {
-            "x": 7.618086573966619,
-            "y": 0.7353968639365192,
-            "heading": 3.1174668267835224,
-            "angularVelocity": -0.28713837638984974,
-            "velocityX": -0.7889577050333905,
-            "velocityY": -0.6575745263915584,
-            "timestamp": 2.203219082231727
-        },
-        {
-            "x": 7.5114563039412285,
-            "y": 0.7085217505748593,
-            "heading": 3.0768465482235205,
-            "angularVelocity": -0.5176208748841893,
-            "velocityX": -1.3587807769024405,
-            "velocityY": -0.34246736320004995,
-            "timestamp": 2.281694044372761
-        },
-        {
-            "x": 7.360182308394819,
-            "y": 0.7066609452418267,
-            "heading": 3.0210994010584384,
-            "angularVelocity": -0.7103813196472814,
-            "velocityX": -1.9276721060984434,
-            "velocityY": -0.02371208959223001,
-            "timestamp": 2.360169006513795
-        },
-        {
-            "x": 7.164421971622539,
-            "y": 0.7302939539488027,
-            "heading": 2.9554195577269637,
-            "angularVelocity": -0.8369528514504796,
-            "velocityX": -2.494557900142165,
-            "velocityY": 0.3011534897526061,
-            "timestamp": 2.4386439686548287
-        },
-        {
-            "x": 6.9246535759772865,
-            "y": 0.7803818201714893,
-            "heading": 2.891069702848338,
-            "angularVelocity": -0.8200049177840428,
-            "velocityX": -3.055348981428448,
-            "velocityY": 0.638265567209492,
-            "timestamp": 2.5171189307958626
-        },
-        {
-            "x": 6.644255242545633,
-            "y": 0.8592678201395815,
-            "heading": 2.8720123676443365,
-            "angularVelocity": -0.24284605795368294,
-            "velocityX": -3.573092943042356,
-            "velocityY": 1.0052378212854063,
-            "timestamp": 2.5955938929368965
-        },
-        {
-            "x": 6.376569654247088,
-            "y": 0.9876185603343286,
-            "heading": 2.872012347067197,
-            "angularVelocity": -2.622127987829542e-7,
-            "velocityX": -3.411095475490091,
-            "velocityY": 1.635562945084242,
-            "timestamp": 2.6740688550779304
-        },
-        {
-            "x": 6.113906618540549,
-            "y": 1.125957335049975,
-            "heading": 2.872012327384056,
-            "angularVelocity": -2.5082064696072804e-7,
-            "velocityX": -3.347093500147104,
-            "velocityY": 1.762839648995542,
-            "timestamp": 2.7525438172189642
-        },
-        {
-            "x": 5.851058985303004,
-            "y": 1.2639450359763313,
-            "heading": 2.8720122952205234,
-            "angularVelocity": -4.098572586075927e-7,
-            "velocityX": -3.3494458113298133,
-            "velocityY": 1.758365944520708,
-            "timestamp": 2.831018779359998
-        },
-        {
-            "x": 5.625853496223335,
-            "y": 1.3827528651043908,
-            "heading": 2.754606631455948,
-            "angularVelocity": -1.4960907346931063,
-            "velocityX": -2.8697750586351334,
-            "velocityY": 1.5139584128060528,
-            "timestamp": 2.909493741501032
-        },
-        {
-            "x": 5.445842564985353,
-            "y": 1.4776937741923677,
-            "heading": 2.6549514330576893,
-            "angularVelocity": -1.2698980117917973,
-            "velocityX": -2.2938645183985233,
-            "velocityY": 1.2098242101391037,
-            "timestamp": 2.987968703642066
-        },
-        {
-            "x": 5.310896371700566,
-            "y": 1.5488618130458742,
-            "heading": 2.5782320494228896,
-            "angularVelocity": -0.9776288072239107,
-            "velocityX": -1.7196082623431492,
-            "velocityY": 0.9068884764238665,
-            "timestamp": 3.0664436657831
-        },
-        {
-            "x": 5.220959019114767,
-            "y": 1.5962913343665528,
-            "heading": 2.5262728715572207,
-            "angularVelocity": -0.6621115378476069,
-            "velocityX": -1.146064300409159,
-            "velocityY": 0.604390496365404,
-            "timestamp": 3.1449186279241337
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.33479304532875703,
-            "velocityX": -0.5729090895764638,
-            "velocityY": 0.30211757975540204,
-            "timestamp": 3.2233935900651676
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": 1.304014234910508e-32,
-            "velocityY": 0,
-            "timestamp": 3.3018685522062015
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S4.traj
deleted file mode 100644
index 111b63c2..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5 To S4.traj	
+++ /dev/null
@@ -1,364 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3565686386495837,
-            "y": 4.327692083019332,
-            "heading": 2.7686204392526563,
-            "angularVelocity": 0.21032675786146612,
-            "velocityX": 0.6389698009998809,
-            "velocityY": -0.36493335842385927,
-            "timestamp": 0.08853100500377892
-        },
-        {
-            "x": 2.4697534077972008,
-            "y": 4.26305693150059,
-            "heading": 2.803412029343583,
-            "angularVelocity": 0.39298763285756855,
-            "velocityX": 1.2784760451187218,
-            "velocityY": -0.7300849178881736,
-            "timestamp": 0.17706201000755784
-        },
-        {
-            "x": 2.639620733022083,
-            "y": 4.166070131606458,
-            "heading": 2.850239105466856,
-            "angularVelocity": 0.5289341979263338,
-            "velocityX": 1.9187325978919179,
-            "velocityY": -1.095512243309488,
-            "timestamp": 0.26559301501133675
-        },
-        {
-            "x": 2.8662525015467275,
-            "y": 4.036714523350174,
-            "heading": 2.9005551954880358,
-            "angularVelocity": 0.5683442768897115,
-            "velocityX": 2.559914105967419,
-            "velocityY": -1.4611333989799566,
-            "timestamp": 0.35412402001511567
-        },
-        {
-            "x": 3.1493645211146295,
-            "y": 3.8752655198055206,
-            "heading": 2.9253298956108997,
-            "angularVelocity": 0.2798420747827705,
-            "velocityX": 3.197885526723865,
-            "velocityY": -1.8236436323948029,
-            "timestamp": 0.4426550250188946
-        },
-        {
-            "x": 3.4401071732873256,
-            "y": 3.709037480706115,
-            "heading": 2.9253299112862043,
-            "angularVelocity": 1.770600526020634e-7,
-            "velocityX": 3.28407716777061,
-            "velocityY": -1.877625122320825,
-            "timestamp": 0.5311860300226735
-        },
-        {
-            "x": 3.730849824609712,
-            "y": 3.5428094401194565,
-            "heading": 2.925329926961498,
-            "angularVelocity": 1.7705993300653618e-7,
-            "velocityX": 3.2840771581659602,
-            "velocityY": -1.87762513912005,
-            "timestamp": 0.6197170350264525
-        },
-        {
-            "x": 4.021592475932095,
-            "y": 3.376581399532791,
-            "heading": 2.9253299426367922,
-            "angularVelocity": 1.7705993370243028e-7,
-            "velocityX": 3.2840771581659167,
-            "velocityY": -1.8776251391201264,
-            "timestamp": 0.7082480400302313
-        },
-        {
-            "x": 4.312335127254477,
-            "y": 3.2103533589461257,
-            "heading": 2.925329958312086,
-            "angularVelocity": 1.7705993363835285e-7,
-            "velocityX": 3.2840771581659163,
-            "velocityY": -1.8776251391201266,
-            "timestamp": 0.7967790450340102
-        },
-        {
-            "x": 4.60307777857686,
-            "y": 3.0441253183594608,
-            "heading": 2.9253299739873797,
-            "angularVelocity": 1.7705993334445259e-7,
-            "velocityX": 3.2840771581659163,
-            "velocityY": -1.8776251391201269,
-            "timestamp": 0.8853100500377891
-        },
-        {
-            "x": 4.893820429899242,
-            "y": 2.877897277772796,
-            "heading": 2.9253299896626737,
-            "angularVelocity": 1.7705993259163212e-7,
-            "velocityX": 3.2840771581659163,
-            "velocityY": -1.877625139120127,
-            "timestamp": 0.9738410550415679
-        },
-        {
-            "x": 5.184563081221626,
-            "y": 2.7116692371861304,
-            "heading": 2.925330005337967,
-            "angularVelocity": 1.7705993390699708e-7,
-            "velocityX": 3.2840771581659163,
-            "velocityY": -1.8776251391201266,
-            "timestamp": 1.0623720600453468
-        },
-        {
-            "x": 5.475305732544008,
-            "y": 2.5454411965994654,
-            "heading": 2.925330021013261,
-            "angularVelocity": 1.770599334284566e-7,
-            "velocityX": 3.2840771581659163,
-            "velocityY": -1.8776251391201269,
-            "timestamp": 1.1509030650491257
-        },
-        {
-            "x": 5.766048383866392,
-            "y": 2.3792131560128005,
-            "heading": 2.925330036688555,
-            "angularVelocity": 1.7705993332802181e-7,
-            "velocityX": 3.2840771581659163,
-            "velocityY": -1.8776251391201264,
-            "timestamp": 1.2394340700529045
-        },
-        {
-            "x": 6.056791035188774,
-            "y": 2.212985115426135,
-            "heading": 2.9253300523638495,
-            "angularVelocity": 1.7705993348595005e-7,
-            "velocityX": 3.2840771581659163,
-            "velocityY": -1.8776251391201266,
-            "timestamp": 1.3279650750566834
-        },
-        {
-            "x": 6.347533686511157,
-            "y": 2.04675707483947,
-            "heading": 2.925330068039143,
-            "angularVelocity": 1.7705993292383098e-7,
-            "velocityX": 3.2840771581659163,
-            "velocityY": -1.8776251391201269,
-            "timestamp": 1.4164960800604622
-        },
-        {
-            "x": 6.638276337833025,
-            "y": 1.8805290342519057,
-            "heading": 2.925330083714437,
-            "angularVelocity": 1.7705993325721192e-7,
-            "velocityX": 3.284077158160109,
-            "velocityY": -1.877625139130284,
-            "timestamp": 1.505027085064241
-        },
-        {
-            "x": 6.929018876242265,
-            "y": 1.7143007961736503,
-            "heading": 2.9253300993897313,
-            "angularVelocity": 1.770599420488751e-7,
-            "velocityX": 3.2840758827580245,
-            "velocityY": -1.8776273698819952,
-            "timestamp": 1.59355809006802
-        },
-        {
-            "x": 7.201522264704821,
-            "y": 1.5196088586386673,
-            "heading": 2.925330126495543,
-            "angularVelocity": 3.061730945539565e-7,
-            "velocityX": 3.078055969780553,
-            "velocityY": -2.1991384546767203,
-            "timestamp": 1.6820890950717988
-        },
-        {
-            "x": 7.413524241952794,
-            "y": 1.3146281632272387,
-            "heading": 3.008225358482996,
-            "angularVelocity": 0.9363412511121879,
-            "velocityX": 2.394663623652733,
-            "velocityY": -2.3153548906699357,
-            "timestamp": 1.7706201000755777
-        },
-        {
-            "x": 7.566471934487395,
-            "y": 1.1372904255598042,
-            "heading": 3.0754099127352705,
-            "angularVelocity": 0.758881639821055,
-            "velocityX": 1.727617262766506,
-            "velocityY": -2.003114475655854,
-            "timestamp": 1.8591511050793565
-        },
-        {
-            "x": 7.66159309798317,
-            "y": 0.9897915834005926,
-            "heading": 3.120981260630991,
-            "angularVelocity": 0.5147501476321013,
-            "velocityX": 1.0744389888232937,
-            "velocityY": -1.6660698944162209,
-            "timestamp": 1.9476821100831354
-        },
-        {
-            "x": 7.6993540657193265,
-            "y": 0.8728983057701601,
-            "heading": 3.1428690494072535,
-            "angularVelocity": 0.2472330318097442,
-            "velocityX": 0.4265281720741691,
-            "velocityY": -1.3203654202890747,
-            "timestamp": 2.0362131150869143
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": -0.03240728383410146,
-            "velocityX": -0.21861341931571387,
-            "velocityY": -0.9702624043010712,
-            "timestamp": 2.124744120090693
-        },
-        {
-            "x": 7.618086573966619,
-            "y": 0.7353968639365192,
-            "heading": 3.1174668267835224,
-            "angularVelocity": -0.28713837638984974,
-            "velocityX": -0.7889577050333905,
-            "velocityY": -0.6575745263915584,
-            "timestamp": 2.203219082231727
-        },
-        {
-            "x": 7.5114563039412285,
-            "y": 0.7085217505748593,
-            "heading": 3.0768465482235205,
-            "angularVelocity": -0.5176208748841893,
-            "velocityX": -1.3587807769024405,
-            "velocityY": -0.34246736320004995,
-            "timestamp": 2.281694044372761
-        },
-        {
-            "x": 7.360182308394819,
-            "y": 0.7066609452418267,
-            "heading": 3.0210994010584384,
-            "angularVelocity": -0.7103813196472814,
-            "velocityX": -1.9276721060984434,
-            "velocityY": -0.02371208959223001,
-            "timestamp": 2.360169006513795
-        },
-        {
-            "x": 7.164421971622539,
-            "y": 0.7302939539488027,
-            "heading": 2.9554195577269637,
-            "angularVelocity": -0.8369528514504796,
-            "velocityX": -2.494557900142165,
-            "velocityY": 0.3011534897526061,
-            "timestamp": 2.4386439686548287
-        },
-        {
-            "x": 6.9246535759772865,
-            "y": 0.7803818201714893,
-            "heading": 2.891069702848338,
-            "angularVelocity": -0.8200049177840428,
-            "velocityX": -3.055348981428448,
-            "velocityY": 0.638265567209492,
-            "timestamp": 2.5171189307958626
-        },
-        {
-            "x": 6.644255242545633,
-            "y": 0.8592678201395815,
-            "heading": 2.8720123676443365,
-            "angularVelocity": -0.24284605795368294,
-            "velocityX": -3.573092943042356,
-            "velocityY": 1.0052378212854063,
-            "timestamp": 2.5955938929368965
-        },
-        {
-            "x": 6.376569654247088,
-            "y": 0.9876185603343286,
-            "heading": 2.872012347067197,
-            "angularVelocity": -2.622127987829542e-7,
-            "velocityX": -3.411095475490091,
-            "velocityY": 1.635562945084242,
-            "timestamp": 2.6740688550779304
-        },
-        {
-            "x": 6.113906618540549,
-            "y": 1.125957335049975,
-            "heading": 2.872012327384056,
-            "angularVelocity": -2.5082064696072804e-7,
-            "velocityX": -3.347093500147104,
-            "velocityY": 1.762839648995542,
-            "timestamp": 2.7525438172189642
-        },
-        {
-            "x": 5.851058985303004,
-            "y": 1.2639450359763313,
-            "heading": 2.8720122952205234,
-            "angularVelocity": -4.098572586075927e-7,
-            "velocityX": -3.3494458113298133,
-            "velocityY": 1.758365944520708,
-            "timestamp": 2.831018779359998
-        },
-        {
-            "x": 5.625853496223335,
-            "y": 1.3827528651043908,
-            "heading": 2.754606631455948,
-            "angularVelocity": -1.4960907346931063,
-            "velocityX": -2.8697750586351334,
-            "velocityY": 1.5139584128060528,
-            "timestamp": 2.909493741501032
-        },
-        {
-            "x": 5.445842564985353,
-            "y": 1.4776937741923677,
-            "heading": 2.6549514330576893,
-            "angularVelocity": -1.2698980117917973,
-            "velocityX": -2.2938645183985233,
-            "velocityY": 1.2098242101391037,
-            "timestamp": 2.987968703642066
-        },
-        {
-            "x": 5.310896371700566,
-            "y": 1.5488618130458742,
-            "heading": 2.5782320494228896,
-            "angularVelocity": -0.9776288072239107,
-            "velocityX": -1.7196082623431492,
-            "velocityY": 0.9068884764238665,
-            "timestamp": 3.0664436657831
-        },
-        {
-            "x": 5.220959019114767,
-            "y": 1.5962913343665528,
-            "heading": 2.5262728715572207,
-            "angularVelocity": -0.6621115378476069,
-            "velocityX": -1.146064300409159,
-            "velocityY": 0.604390496365404,
-            "timestamp": 3.1449186279241337
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.33479304532875703,
-            "velocityX": -0.5729090895764638,
-            "velocityY": 0.30211757975540204,
-            "timestamp": 3.2233935900651676
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": 1.304014234910508e-32,
-            "velocityY": 0,
-            "timestamp": 3.3018685522062015
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj
deleted file mode 100644
index 34dfa28f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 8.35160049146123e-34,
-            "velocityX": 7.119134799605741e-35,
-            "velocityY": 1.2756128975400497e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.298001258966314,
-            "y": 4.408001448921345,
-            "heading": 2.7655229314430416,
-            "angularVelocity": 0.204083682327543,
-            "velocityX": -0.026277925124547522,
-            "velocityY": 0.6310864986340488,
-            "timestamp": 0.07606160015345173
-        },
-        {
-            "x": 2.294003795304181,
-            "y": 4.504004095385984,
-            "heading": 2.796583558711994,
-            "angularVelocity": 0.408361475518381,
-            "velocityX": -0.0525556082710341,
-            "velocityY": 1.262169692340901,
-            "timestamp": 0.15212320030690346
-        },
-        {
-            "x": 2.2880075283432446,
-            "y": 4.6480074792263695,
-            "heading": 2.8432070800984706,
-            "angularVelocity": 0.6129705566595356,
-            "velocityX": -0.07883435200994282,
-            "velocityY": 1.8932468361152464,
-            "timestamp": 0.2281848004603552
-        },
-        {
-            "x": 2.2800121406869254,
-            "y": 4.840010950636843,
-            "heading": 2.905427634623498,
-            "angularVelocity": 0.8180284716532374,
-            "velocityX": -0.10511726863738108,
-            "velocityY": 2.5243154367396032,
-            "timestamp": 0.3042464006138069
-        },
-        {
-            "x": 2.2700168989019116,
-            "y": 5.080013713319674,
-            "heading": 2.983284780095673,
-            "angularVelocity": 1.0236064625921717,
-            "velocityX": -0.13140982788751254,
-            "velocityY": 3.1553735682477444,
-            "timestamp": 0.3803080007672587
-        },
-        {
-            "x": 2.262010665765912,
-            "y": 5.27200837395213,
-            "heading": 3.04596409402067,
-            "angularVelocity": 0.8240598909113609,
-            "velocityX": -0.10525985674568247,
-            "velocityY": 2.5241995993394015,
-            "timestamp": 0.45636960092071044
-        },
-        {
-            "x": 2.2560054673554846,
-            "y": 5.416004212856887,
-            "heading": 3.092980839154268,
-            "angularVelocity": 0.6181403630576198,
-            "velocityX": -0.07895177590678841,
-            "velocityY": 1.8931476410468593,
-            "timestamp": 0.5324312010741622
-        },
-        {
-            "x": 2.2520018258057144,
-            "y": 5.512001403135216,
-            "heading": 3.1243269625502275,
-            "angularVelocity": 0.4121149611988094,
-            "velocityX": -0.05263683043340117,
-            "velocityY": 1.2620979585580252,
-            "timestamp": 0.608492801227614
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0.20605716180244424,
-            "velocityX": -0.026318481358210057,
-            "velocityY": 0.6310490019661509,
-            "timestamp": 0.6845544013810657
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -5.838549689746647e-34,
-            "velocityX": 5.793975279736999e-33,
-            "velocityY": -3.8925630164914687e-35,
-            "timestamp": 0.7606160015345175
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.traj
deleted file mode 100644
index 34dfa28f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 8.35160049146123e-34,
-            "velocityX": 7.119134799605741e-35,
-            "velocityY": 1.2756128975400497e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.298001258966314,
-            "y": 4.408001448921345,
-            "heading": 2.7655229314430416,
-            "angularVelocity": 0.204083682327543,
-            "velocityX": -0.026277925124547522,
-            "velocityY": 0.6310864986340488,
-            "timestamp": 0.07606160015345173
-        },
-        {
-            "x": 2.294003795304181,
-            "y": 4.504004095385984,
-            "heading": 2.796583558711994,
-            "angularVelocity": 0.408361475518381,
-            "velocityX": -0.0525556082710341,
-            "velocityY": 1.262169692340901,
-            "timestamp": 0.15212320030690346
-        },
-        {
-            "x": 2.2880075283432446,
-            "y": 4.6480074792263695,
-            "heading": 2.8432070800984706,
-            "angularVelocity": 0.6129705566595356,
-            "velocityX": -0.07883435200994282,
-            "velocityY": 1.8932468361152464,
-            "timestamp": 0.2281848004603552
-        },
-        {
-            "x": 2.2800121406869254,
-            "y": 4.840010950636843,
-            "heading": 2.905427634623498,
-            "angularVelocity": 0.8180284716532374,
-            "velocityX": -0.10511726863738108,
-            "velocityY": 2.5243154367396032,
-            "timestamp": 0.3042464006138069
-        },
-        {
-            "x": 2.2700168989019116,
-            "y": 5.080013713319674,
-            "heading": 2.983284780095673,
-            "angularVelocity": 1.0236064625921717,
-            "velocityX": -0.13140982788751254,
-            "velocityY": 3.1553735682477444,
-            "timestamp": 0.3803080007672587
-        },
-        {
-            "x": 2.262010665765912,
-            "y": 5.27200837395213,
-            "heading": 3.04596409402067,
-            "angularVelocity": 0.8240598909113609,
-            "velocityX": -0.10525985674568247,
-            "velocityY": 2.5241995993394015,
-            "timestamp": 0.45636960092071044
-        },
-        {
-            "x": 2.2560054673554846,
-            "y": 5.416004212856887,
-            "heading": 3.092980839154268,
-            "angularVelocity": 0.6181403630576198,
-            "velocityX": -0.07895177590678841,
-            "velocityY": 1.8931476410468593,
-            "timestamp": 0.5324312010741622
-        },
-        {
-            "x": 2.2520018258057144,
-            "y": 5.512001403135216,
-            "heading": 3.1243269625502275,
-            "angularVelocity": 0.4121149611988094,
-            "velocityX": -0.05263683043340117,
-            "velocityY": 1.2620979585580252,
-            "timestamp": 0.608492801227614
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0.20605716180244424,
-            "velocityX": -0.026318481358210057,
-            "velocityY": 0.6310490019661509,
-            "timestamp": 0.6845544013810657
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -5.838549689746647e-34,
-            "velocityX": 5.793975279736999e-33,
-            "velocityY": -3.8925630164914687e-35,
-            "timestamp": 0.7606160015345175
-        }
-    ]
-}
\ No newline at end of file

From dc815e14652904bb84b7bbaa40aaba58d380ec25 Mon Sep 17 00:00:00 2001
From: Lewis-Seiden <lewy@seiden.us>
Date: Sun, 18 Feb 2024 16:49:55 -0800
Subject: [PATCH 30/62] tuned intake pid

---
 src/main/java/frc/robot/Robot.java            |  7 +++++-
 .../frc/robot/subsystems/intake/IntakeIO.java |  4 ++++
 .../robot/subsystems/intake/IntakeIOReal.java | 23 +++++++++++++++++++
 .../subsystems/intake/IntakeSubsystem.java    |  8 +++++++
 .../frc/robot/subsystems/swerve/Module.java   | 10 +++++---
 .../robot/subsystems/swerve/ModuleIOReal.java | 14 ++++++-----
 6 files changed, 56 insertions(+), 10 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index df89cf79..099cc73a 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -198,7 +198,7 @@ public void robotInit() {
                 () -> feeder.getFirstBeambreak()));
 
     // ---- Controller bindings here ----
-    controller.leftTrigger().whileTrue(intake.runVoltageCmd(8.0));
+    controller.leftTrigger().whileTrue(intake.runVelocityCmd(80.0));
     controller
         .rightTrigger()
         .and(() -> currentTarget == Target.SPEAKER)
@@ -296,6 +296,11 @@ public void robotInit() {
     operator.x().onTrue(Commands.runOnce(() -> flywheelIdleSpeed = 20.0));
     operator.y().onTrue(Commands.runOnce(() -> flywheelIdleSpeed = 80.0));
 
+    operator
+        .start()
+        .whileTrue(shooter.runPivotCurrentZeroing())
+        .whileTrue(elevator.runCurrentZeroing());
+
     NamedCommands.registerCommand("stop", swerve.stopWithXCmd().asProxy());
     NamedCommands.registerCommand(
         "auto aim amp 4 local sgmt 1", autonomousAutoAim("amp 4 local sgmt 1"));
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
index a709888c..ea4e01ad 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
@@ -26,6 +26,10 @@ public static class IntakeIOInputs {
   /** Run the intake at a specified voltage */
   public void setIntakeVoltage(final double volts);
 
+  public void setIntakeSpeed(final double rps);
+
   /** Run the centering wheels at a specified voltage */
   public void setCenteringVoltage(final double volts);
+
+  public void setCenteringSpeed(final double rps);
 }
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java
index f9b98b5e..3d4c9eca 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java
@@ -7,6 +7,7 @@
 import com.ctre.phoenix6.BaseStatusSignal;
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.InvertedValue;
@@ -17,7 +18,11 @@ public class IntakeIOReal implements IntakeIO {
   private final TalonFX centeringMotor = new TalonFX(15, "canivore");
 
   private final VoltageOut intakeVoltageOut = new VoltageOut(0.0).withEnableFOC(true);
+  private final VelocityVoltage intakeVelocityVoltage =
+      new VelocityVoltage(0.0).withEnableFOC(true);
   private final VoltageOut centeringVoltageOut = new VoltageOut(0.0).withEnableFOC(true);
+  private final VelocityVoltage centeringVelocityVoltage =
+      new VelocityVoltage(0.0).withEnableFOC(true);
 
   private final StatusSignal<Double> intakeVelocity = intakeMotor.getVelocity();
   private final StatusSignal<Double> intakeVoltage = intakeMotor.getMotorVoltage();
@@ -34,12 +39,20 @@ public IntakeIOReal() {
     intakeConfig.CurrentLimits.SupplyCurrentLimit = 20.0;
     intakeConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
     intakeConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+
+    intakeConfig.Slot0.kV = (12.0 * 60.0) / 5800;
+    intakeConfig.Slot0.kP = 1.0;
+
     intakeMotor.getConfigurator().apply(intakeConfig);
 
     var centeringConfig = new TalonFXConfiguration();
     centeringConfig.CurrentLimits.SupplyCurrentLimit = 20.0;
     centeringConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
     centeringConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+
+    centeringConfig.Slot0.kV = (12.0 * 60.0) / 5800;
+    centeringConfig.Slot0.kP = 1.0;
+
     centeringMotor.getConfigurator().apply(centeringConfig);
 
     BaseStatusSignal.setUpdateFrequencyForAll(
@@ -82,4 +95,14 @@ public void setIntakeVoltage(final double volts) {
   public void setCenteringVoltage(final double volts) {
     centeringMotor.setControl(centeringVoltageOut.withOutput(volts));
   }
+
+  @Override
+  public void setIntakeSpeed(final double rps) {
+    intakeMotor.setControl(intakeVelocityVoltage.withVelocity(rps));
+  }
+
+  @Override
+  public void setCenteringSpeed(final double rps) {
+    centeringMotor.setControl(centeringVelocityVoltage.withVelocity(rps));
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java
index 1e3ce8c1..fa2491c7 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java
@@ -32,4 +32,12 @@ public Command runVoltageCmd(double voltage) {
           io.setCenteringVoltage(voltage);
         });
   }
+
+  public Command runVelocityCmd(double velocity) {
+    return this.run(
+        () -> {
+          io.setIntakeSpeed(velocity);
+          io.setCenteringSpeed(velocity);
+        });
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/swerve/Module.java b/src/main/java/frc/robot/subsystems/swerve/Module.java
index 186fc8a4..e8505977 100644
--- a/src/main/java/frc/robot/subsystems/swerve/Module.java
+++ b/src/main/java/frc/robot/subsystems/swerve/Module.java
@@ -34,7 +34,9 @@ public record ModuleConstants(
   public static final double DRIVE_GEAR_RATIO = (50.0 / 16.0) * (16.0 / 28.0) * (45.0 / 15.0);
   public static final double TURN_GEAR_RATIO = 150.0 / 7.0;
 
-  public static final double DRIVE_STATOR_CURRENT_LIMIT = 80.0; // TODO bump as needed
+  public static final double DRIVE_STATOR_CURRENT_LIMIT = 60.0;
+  public static final double DRIVE_SUPPLY_TIME_CURRENT_LIMIT = 40.0;
+  public static final double DRIVE_SUPPLY_TIME_CUTOFF = 0.5;
   public static final double TURN_STATOR_CURRENT_LIMIT = 20.0;
 
   private final ModuleIO io;
@@ -73,10 +75,12 @@ public void periodic() {
   /** Runs the module closed loop with the specified setpoint state. Returns the optimized state. */
   public SwerveModuleState runSetpoint(SwerveModuleState state) {
     // Optimize state based on current angle
-    final var optimizedState = state; // SwerveModuleState.optimize(state, getAngle());
+    final var optimizedState = SwerveModuleState.optimize(state, getAngle());
 
     io.setTurnSetpoint(optimizedState.angle);
-    io.setDriveSetpoint(optimizedState.speedMetersPerSecond);
+    io.setDriveSetpoint(
+        optimizedState.speedMetersPerSecond
+            * Math.cos(optimizedState.angle.minus(inputs.turnPosition).getRadians()));
 
     return optimizedState;
   }
diff --git a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java
index 038b1293..1dfdde9c 100644
--- a/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java
+++ b/src/main/java/frc/robot/subsystems/swerve/ModuleIOReal.java
@@ -17,8 +17,8 @@
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.configs.CANcoderConfiguration;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
-import com.ctre.phoenix6.controls.MotionMagicVelocityVoltage;
 import com.ctre.phoenix6.controls.MotionMagicVoltage;
+import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.controls.VoltageOut;
 import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.hardware.TalonFX;
@@ -71,8 +71,7 @@ public class ModuleIOReal implements ModuleIO {
   // Control modes
   private final VoltageOut driveVoltage = new VoltageOut(0.0).withEnableFOC(true);
   private final VoltageOut turnVoltage = new VoltageOut(0.0).withEnableFOC(true);
-  private final MotionMagicVelocityVoltage drivePIDF =
-      new MotionMagicVelocityVoltage(0.0).withEnableFOC(true);
+  private final VelocityVoltage drivePIDF = new VelocityVoltage(0.0).withEnableFOC(true);
   private final MotionMagicVoltage turnPID = new MotionMagicVoltage(0.0).withEnableFOC(true);
 
   public ModuleIOReal(ModuleConstants constants) {
@@ -87,6 +86,9 @@ public ModuleIOReal(ModuleConstants constants) {
     // TODO: Do we want to limit supply current?
     driveConfig.CurrentLimits.StatorCurrentLimit = Module.DRIVE_STATOR_CURRENT_LIMIT;
     driveConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+    driveConfig.CurrentLimits.SupplyTimeThreshold = Module.DRIVE_SUPPLY_TIME_CUTOFF;
+    driveConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
+    driveConfig.CurrentLimits.SupplyCurrentThreshold = Module.DRIVE_SUPPLY_TIME_CURRENT_LIMIT;
     // Inverts
     driveConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
     driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
@@ -96,8 +98,8 @@ public ModuleIOReal(ModuleConstants constants) {
         (Module.DRIVE_GEAR_RATIO) * (1.0 / (Module.WHEEL_RADIUS * 2 * Math.PI));
     // Controls Gains
     driveConfig.Slot0.kV = 2.0733;
-    driveConfig.Slot0.kA = 0.6;
-    driveConfig.Slot0.kS = 0.052218;
+    driveConfig.Slot0.kA = 0.4;
+    driveConfig.Slot0.kS = 0.04;
     driveConfig.Slot0.kP = 1.9855;
     driveConfig.Slot0.kD = 0.0;
 
@@ -126,7 +128,7 @@ public ModuleIOReal(ModuleConstants constants) {
     // Controls Gains
     turnConfig.Slot0.kV = 2.7935;
     turnConfig.Slot0.kA = 0.031543;
-    turnConfig.Slot0.kS = 0.34822;
+    turnConfig.Slot0.kS = 0.28;
     turnConfig.Slot0.kP = 28.579;
     turnConfig.Slot0.kD = 0.68275;
     turnConfig.MotionMagic.MotionMagicCruiseVelocity = 5500 / Module.TURN_GEAR_RATIO;

From aeeff02f185a975e5b85e8a82026c8df33b1b173 Mon Sep 17 00:00:00 2001
From: Stoopipoopy <anotherguywithabeanie@gmail.com>
Date: Sun, 18 Feb 2024 17:11:38 -0800
Subject: [PATCH 31/62] generated paths for other distribtued generating

---
 .../robot/utils/dynamicauto/ChoreoBuilder.py  |    12 +-
 .../frc/robot/utils/dynamicauto/data.chor     | 14517 +++++++++++++++-
 .../deploy/choreo/Amp Side To C1.1.traj       |   274 +
 .../deploy/choreo/Amp Side To C1.traj         |   274 +
 .../deploy/choreo/Amp Side To C2.1.traj       |   274 +
 .../deploy/choreo/Amp Side To C2.traj         |   274 +
 .../deploy/choreo/Amp Side To C5.1.traj       |   679 +
 .../deploy/choreo/Amp Side To C5.traj         |   679 +
 .../deploy/choreo/Amp Side To W1.1.traj       |   121 +
 .../deploy/choreo/Amp Side To W1.traj         |   121 +
 .../deploy/choreo/Amp Side To W3.1.traj       |   193 +
 .../deploy/choreo/Amp Side To W3.traj         |   193 +
 .../dynamicauto/deploy/choreo/C1 To C4.1.traj |   175 +
 .../dynamicauto/deploy/choreo/C1 To C4.traj   |   175 +
 .../dynamicauto/deploy/choreo/C1 To C5.1.traj |   220 +
 .../dynamicauto/deploy/choreo/C1 To C5.traj   |   220 +
 .../dynamicauto/deploy/choreo/C1 To S1.1.traj |   130 +
 .../dynamicauto/deploy/choreo/C1 To S1.traj   |   130 +
 .../dynamicauto/deploy/choreo/C1 To S2.1.traj |   148 +
 .../dynamicauto/deploy/choreo/C1 To S2.traj   |   148 +
 .../dynamicauto/deploy/choreo/C1 To W1.1.traj |   202 +
 .../dynamicauto/deploy/choreo/C1 To W1.traj   |   202 +
 .../dynamicauto/deploy/choreo/C2 To C1.1.traj |    94 +
 .../dynamicauto/deploy/choreo/C2 To C1.traj   |    94 +
 .../dynamicauto/deploy/choreo/C2 To C4.1.traj |   139 +
 .../dynamicauto/deploy/choreo/C2 To C4.traj   |   139 +
 .../dynamicauto/deploy/choreo/C2 To C5.1.traj |   175 +
 .../dynamicauto/deploy/choreo/C2 To C5.traj   |   175 +
 .../dynamicauto/deploy/choreo/C2 To S1.1.traj |   130 +
 .../dynamicauto/deploy/choreo/C2 To S1.traj   |   130 +
 .../dynamicauto/deploy/choreo/C2 To S2.1.traj |   139 +
 .../dynamicauto/deploy/choreo/C2 To S2.traj   |   139 +
 .../dynamicauto/deploy/choreo/C2 To S3.1.traj |   193 +
 .../dynamicauto/deploy/choreo/C2 To S3.traj   |   193 +
 .../dynamicauto/deploy/choreo/C2 To W1.1.traj |   202 +
 .../dynamicauto/deploy/choreo/C2 To W1.traj   |   202 +
 .../dynamicauto/deploy/choreo/C2 To W2.1.traj |   184 +
 .../dynamicauto/deploy/choreo/C2 To W2.traj   |   184 +
 .../dynamicauto/deploy/choreo/C2 To W3.1.traj |   211 +
 .../dynamicauto/deploy/choreo/C2 To W3.traj   |   211 +
 .../dynamicauto/deploy/choreo/C3 To C4.1.traj |    94 +
 .../dynamicauto/deploy/choreo/C3 To C4.traj   |    94 +
 .../dynamicauto/deploy/choreo/C3 To C5.1.traj |   139 +
 .../dynamicauto/deploy/choreo/C3 To C5.traj   |   139 +
 .../dynamicauto/deploy/choreo/C3 To S1.1.traj |   157 +
 .../dynamicauto/deploy/choreo/C3 To S1.traj   |   157 +
 .../dynamicauto/deploy/choreo/C3 To W2.1.traj |   193 +
 .../dynamicauto/deploy/choreo/C3 To W2.traj   |   193 +
 .../dynamicauto/deploy/choreo/C4 To C1.1.traj |   175 +
 .../dynamicauto/deploy/choreo/C4 To C1.traj   |   175 +
 .../dynamicauto/deploy/choreo/C4 To C2.1.traj |   139 +
 .../dynamicauto/deploy/choreo/C4 To C2.traj   |   139 +
 .../dynamicauto/deploy/choreo/C4 To C3.1.traj |    94 +
 .../dynamicauto/deploy/choreo/C4 To C3.traj   |    94 +
 .../dynamicauto/deploy/choreo/C4 To S4.1.traj |   148 +
 .../dynamicauto/deploy/choreo/C4 To S4.traj   |   148 +
 .../dynamicauto/deploy/choreo/C4 To W1.1.traj |   238 +
 .../dynamicauto/deploy/choreo/C4 To W1.traj   |   238 +
 .../dynamicauto/deploy/choreo/C4 To W2.1.traj |   211 +
 .../dynamicauto/deploy/choreo/C4 To W2.traj   |   211 +
 .../dynamicauto/deploy/choreo/C5 To C1.1.traj |   220 +
 .../dynamicauto/deploy/choreo/C5 To C1.traj   |   220 +
 .../dynamicauto/deploy/choreo/C5 To C2.1.traj |   175 +
 .../dynamicauto/deploy/choreo/C5 To C2.traj   |   175 +
 .../dynamicauto/deploy/choreo/C5 To C4.1.traj |    94 +
 .../dynamicauto/deploy/choreo/C5 To C4.traj   |    94 +
 .../dynamicauto/deploy/choreo/C5 To S3.1.traj |   184 +
 .../dynamicauto/deploy/choreo/C5 To S3.traj   |   184 +
 .../dynamicauto/deploy/choreo/C5 To S4.1.traj |   148 +
 .../dynamicauto/deploy/choreo/C5 To S4.traj   |   148 +
 .../dynamicauto/deploy/choreo/C5 To W1.1.traj |   265 +
 .../dynamicauto/deploy/choreo/C5 To W1.traj   |   265 +
 .../deploy/choreo/Center To C1.1.traj         |   220 +
 .../deploy/choreo/Center To C1.traj           |   220 +
 .../deploy/choreo/Center To C3.1.traj         |   211 +
 .../deploy/choreo/Center To C3.traj           |   211 +
 .../deploy/choreo/Center To W2.1.traj         |    76 +
 .../deploy/choreo/Center To W2.traj           |    76 +
 .../deploy/choreo/Center To W3.1.traj         |   112 +
 .../deploy/choreo/Center To W3.traj           |   112 +
 .../dynamicauto/deploy/choreo/S1 To C1.1.traj |   130 +
 .../dynamicauto/deploy/choreo/S1 To C1.traj   |   130 +
 .../dynamicauto/deploy/choreo/S1 To C2.1.traj |   130 +
 .../dynamicauto/deploy/choreo/S1 To C2.traj   |   130 +
 .../dynamicauto/deploy/choreo/S1 To W1.1.traj |   130 +
 .../dynamicauto/deploy/choreo/S1 To W1.traj   |   130 +
 .../dynamicauto/deploy/choreo/S1 To W2.1.traj |   148 +
 .../dynamicauto/deploy/choreo/S1 To W2.traj   |   148 +
 .../dynamicauto/deploy/choreo/S2 To C1.1.traj |   148 +
 .../dynamicauto/deploy/choreo/S2 To C1.traj   |   148 +
 .../dynamicauto/deploy/choreo/S2 To C3.1.traj |   148 +
 .../dynamicauto/deploy/choreo/S2 To C3.traj   |   148 +
 .../dynamicauto/deploy/choreo/S2 To W1.1.traj |   130 +
 .../dynamicauto/deploy/choreo/S2 To W1.traj   |   130 +
 .../dynamicauto/deploy/choreo/S2 To W2.1.traj |   103 +
 .../dynamicauto/deploy/choreo/S2 To W2.traj   |   103 +
 .../dynamicauto/deploy/choreo/S2 To W3.1.traj |   130 +
 .../dynamicauto/deploy/choreo/S2 To W3.traj   |   130 +
 .../dynamicauto/deploy/choreo/S3 To C2.1.traj |   193 +
 .../dynamicauto/deploy/choreo/S3 To C2.traj   |   193 +
 .../dynamicauto/deploy/choreo/S3 To C3.1.traj |   166 +
 .../dynamicauto/deploy/choreo/S3 To C3.traj   |   166 +
 .../dynamicauto/deploy/choreo/S3 To C5.1.traj |   184 +
 .../dynamicauto/deploy/choreo/S3 To C5.traj   |   184 +
 .../dynamicauto/deploy/choreo/S4 To C4.1.traj |   148 +
 .../dynamicauto/deploy/choreo/S4 To C4.traj   |   148 +
 .../dynamicauto/deploy/choreo/S4 To C5.1.traj |   148 +
 .../dynamicauto/deploy/choreo/S4 To C5.traj   |   148 +
 .../deploy/choreo/Stage Side To C3.1.traj     |   274 +
 .../deploy/choreo/Stage Side To C3.traj       |   274 +
 .../deploy/choreo/Stage Side To C5.1.traj     |   292 +
 .../deploy/choreo/Stage Side To C5.traj       |   292 +
 .../deploy/choreo/Stage Side To W1.1.traj     |   193 +
 .../deploy/choreo/Stage Side To W1.traj       |   193 +
 .../deploy/choreo/Stage Side To W2.1.traj     |   148 +
 .../deploy/choreo/Stage Side To W2.traj       |   148 +
 .../deploy/choreo/Stage Side To W3.1.traj     |   121 +
 .../deploy/choreo/Stage Side To W3.traj       |   121 +
 .../dynamicauto/deploy/choreo/W1 To C1.1.traj |   202 +
 .../dynamicauto/deploy/choreo/W1 To C1.traj   |   202 +
 .../dynamicauto/deploy/choreo/W1 To C2.1.traj |   202 +
 .../dynamicauto/deploy/choreo/W1 To C2.traj   |   202 +
 .../dynamicauto/deploy/choreo/W1 To S1.1.traj |   130 +
 .../dynamicauto/deploy/choreo/W1 To S1.traj   |   130 +
 .../dynamicauto/deploy/choreo/W1 To S2.1.traj |   130 +
 .../dynamicauto/deploy/choreo/W1 To S2.traj   |   130 +
 .../dynamicauto/deploy/choreo/W1 To W3.1.traj |   148 +
 .../dynamicauto/deploy/choreo/W1 To W3.traj   |   148 +
 .../dynamicauto/deploy/choreo/W2 To C1.1.traj |   193 +
 .../dynamicauto/deploy/choreo/W2 To C1.traj   |   193 +
 .../dynamicauto/deploy/choreo/W2 To S2.1.traj |   103 +
 .../dynamicauto/deploy/choreo/W2 To S2.traj   |   103 +
 .../dynamicauto/deploy/choreo/W2 To W1.1.traj |   103 +
 .../dynamicauto/deploy/choreo/W2 To W1.traj   |   103 +
 .../dynamicauto/deploy/choreo/W2 To W3.1.traj |   103 +
 .../dynamicauto/deploy/choreo/W2 To W3.traj   |   103 +
 .../dynamicauto/deploy/choreo/W3 To S1.1.traj |   175 +
 .../dynamicauto/deploy/choreo/W3 To S1.traj   |   175 +
 .../dynamicauto/deploy/choreo/W3 To S2.1.traj |   130 +
 .../dynamicauto/deploy/choreo/W3 To S2.traj   |   130 +
 .../dynamicauto/deploy/choreo/W3 To S3.1.traj |   121 +
 .../dynamicauto/deploy/choreo/W3 To S3.traj   |   121 +
 .../dynamicauto/deploy/choreo/W3 To S4.1.traj |   166 +
 .../dynamicauto/deploy/choreo/W3 To S4.traj   |   166 +
 .../dynamicauto/deploy/choreo/W3 To W1.1.traj |   148 +
 .../dynamicauto/deploy/choreo/W3 To W1.traj   |   148 +
 .../dynamicauto/deploy/choreo/W3 To W2.1.traj |   103 +
 .../dynamicauto/deploy/choreo/W3 To W2.traj   |   103 +
 148 files changed, 38930 insertions(+), 393 deletions(-)
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.traj

diff --git a/src/main/java/frc/robot/utils/dynamicauto/ChoreoBuilder.py b/src/main/java/frc/robot/utils/dynamicauto/ChoreoBuilder.py
index baac91e8..7078fb9f 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/ChoreoBuilder.py
+++ b/src/main/java/frc/robot/utils/dynamicauto/ChoreoBuilder.py
@@ -20,13 +20,13 @@
     {"name": "C4", "x": 7.68, "y": 2.457, "angle": 3.14, "allowed_destinations": ["W1", "W2", "W3", "S3", "S4", "S3", "S4", "C1", "C2", "C3",  "C5"]},
     {"name": "C5", "x": 7.68, "y": 0.787, "angle": 3.14, "allowed_destinations": ["W1", "W2", "W3", "S3", "S4", "S3", "S4", "C1", "C2", "C3", "C4"]},
 ]
-'''
+
 obastacles = [
-    {"x": , "y": , "radius": },
-    {"x": , "y": , "radius": },
-    {"x": , "y": , "radius": }
+    {"x": 3.42, "y": 4.04, "radius": 0.50},
+    {"x": 5.61, "y": 2.81, "radius": 0.50},
+    {"x": 5.61, "y": 5.44, "radius": 0.50}
 ]
-'''
+
 def create_waypoint(point):
     return {
         "x": point["x"],
@@ -59,7 +59,7 @@ def create_path(allPoints):
         "usesControlIntervalGuessing": True,
         "defaultControlIntervalCount": 40,
         "usesDefaultFieldObstacles": True,
-        "circleObstacles": []
+        "circleObstacles": obastacles
     }
 
 
diff --git a/src/main/java/frc/robot/utils/dynamicauto/data.chor b/src/main/java/frc/robot/utils/dynamicauto/data.chor
index d2e54a6f..480e8e58 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/data.chor
+++ b/src/main/java/frc/robot/utils/dynamicauto/data.chor
@@ -22,7 +22,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 12
                 },
                 {
                     "x": 2.3,
@@ -34,7 +34,125 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 0.71,
+                    "y": 6.72,
+                    "heading": -2.1,
+                    "angularVelocity": -8.4685465984255e-32,
+                    "velocityX": -1.4242674774019307e-33,
+                    "velocityY": -4.454347432487034e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 0.7541194456754691,
+                    "y": 6.720987374332373,
+                    "heading": -2.1237071075059206,
+                    "angularVelocity": -0.324046498361879,
+                    "velocityX": 0.6030576221600736,
+                    "velocityY": 0.013496171766179704,
+                    "timestamp": 0.07315958550932472
+                },
+                {
+                    "x": 0.8424134938705199,
+                    "y": 6.722973032705717,
+                    "heading": -2.1697540257675185,
+                    "angularVelocity": -0.6294037608468737,
+                    "velocityX": 1.2068691693694522,
+                    "velocityY": 0.027141465599130124,
+                    "timestamp": 0.14631917101864944
+                },
+                {
+                    "x": 0.9749625148381232,
+                    "y": 6.725970459573204,
+                    "heading": -2.2360020755513283,
+                    "angularVelocity": -0.9055279540276729,
+                    "velocityX": 1.811779277381365,
+                    "velocityY": 0.040971075035846904,
+                    "timestamp": 0.21947875652797416
+                },
+                {
+                    "x": 1.1518930933890432,
+                    "y": 6.729994686272588,
+                    "heading": -2.318545216019314,
+                    "angularVelocity": -1.1282614560115307,
+                    "velocityX": 2.418419641379311,
+                    "velocityY": 0.055006144053120966,
+                    "timestamp": 0.2926383420372989
+                },
+                {
+                    "x": 1.3734035763726509,
+                    "y": 6.735067561069991,
+                    "heading": -2.4074942006161044,
+                    "angularVelocity": -1.215821330554877,
+                    "velocityX": 3.0277711586456673,
+                    "velocityY": 0.0693398515327658,
+                    "timestamp": 0.3657979275466236
+                },
+                {
+                    "x": 1.6366126950009532,
+                    "y": 6.741789773246868,
+                    "heading": -2.44063463913809,
+                    "angularVelocity": -0.45298833080218254,
+                    "velocityX": 3.59773933649109,
+                    "velocityY": 0.09188422993442831,
+                    "timestamp": 0.4389575130559483
+                },
+                {
+                    "x": 1.858120407547834,
+                    "y": 6.746837523733308,
+                    "heading": -2.530120705195818,
+                    "angularVelocity": -1.2231625621549276,
+                    "velocityX": 3.027733290241823,
+                    "velocityY": 0.06899643363609939,
+                    "timestamp": 0.512117098565273
+                },
+                {
+                    "x": 2.0350481983019497,
+                    "y": 6.750889776022306,
+                    "heading": -2.613093703885261,
+                    "angularVelocity": -1.134137080080631,
+                    "velocityX": 2.418381535684925,
+                    "velocityY": 0.05538921879868258,
+                    "timestamp": 0.5852766840745978
+                },
+                {
+                    "x": 2.1675926697007295,
+                    "y": 6.753939244710426,
+                    "heading": -2.6797699231631307,
+                    "angularVelocity": -0.9113804953059623,
+                    "velocityX": 1.8117170904677296,
+                    "velocityY": 0.041682421611463606,
+                    "timestamp": 0.6584362695839225
+                },
+                {
+                    "x": 2.255882529656213,
+                    "y": 6.7559780839356005,
+                    "heading": -2.7261390096327216,
+                    "angularVelocity": -0.6338073971683945,
+                    "velocityX": 1.2068119213746804,
+                    "velocityY": 0.027868381306137554,
+                    "timestamp": 0.7315958550932473
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "angularVelocity": -0.32614988454563837,
+                    "velocityX": 0.603030621847418,
+                    "velocityY": 0.013968314025900756,
+                    "timestamp": 0.8047554406025721
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "angularVelocity": -2.091519469848374e-32,
+                    "velocityX": -8.675585045811285e-34,
+                    "velocityY": 3.70068025566541e-32,
+                    "timestamp": 0.8779150261118969
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -52,7 +170,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "Amp Side To W2": {
             "waypoints": [
@@ -63,12 +197,12 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 15
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": -3.143185307179586,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -93,7 +227,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "Amp Side To W3": {
             "waypoints": [
@@ -104,19 +254,209 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 20
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -3.5331853071795862,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 0.71,
+                    "y": 6.72,
+                    "heading": -2.1,
+                    "angularVelocity": 2.001911519996272e-31,
+                    "velocityX": 6.781624829737293e-33,
+                    "velocityY": -4.0943813533671796e-32,
+                    "timestamp": 0
+                },
+                {
+                    "x": 0.7269029739475109,
+                    "y": 6.6955056066592755,
+                    "heading": -2.1380447151842032,
+                    "angularVelocity": -0.6185663850269976,
+                    "velocityX": 0.27482428085724375,
+                    "velocityY": -0.3982526421565261,
+                    "timestamp": 0.061504659976863815
+                },
+                {
+                    "x": 0.7606767634590716,
+                    "y": 6.646279647618601,
+                    "heading": -2.2124384537514556,
+                    "angularVelocity": -1.2095626346887691,
+                    "velocityX": 0.5491257007886129,
+                    "velocityY": -0.8003614532490962,
+                    "timestamp": 0.12300931995372763
+                },
+                {
+                    "x": 0.8112924785066278,
+                    "y": 6.571981628339174,
+                    "heading": -2.320229893429466,
+                    "angularVelocity": -1.752573540257899,
+                    "velocityX": 0.8229573997579402,
+                    "velocityY": -1.208006341428056,
+                    "timestamp": 0.18451397993059143
+                },
+                {
+                    "x": 0.8787853408643597,
+                    "y": 6.472118688114863,
+                    "heading": -2.4555147279599576,
+                    "angularVelocity": -2.199586739954034,
+                    "velocityX": 1.0973617671103395,
+                    "velocityY": -1.6236646176383343,
+                    "timestamp": 0.24601863990745526
+                },
+                {
+                    "x": 0.9632675519056847,
+                    "y": 6.346052032301826,
+                    "heading": -2.6067469489099335,
+                    "angularVelocity": -2.4588741894819854,
+                    "velocityX": 1.37359040880975,
+                    "velocityY": -2.049709011649839,
+                    "timestamp": 0.3075232998843191
+                },
+                {
+                    "x": 1.0643460613698084,
+                    "y": 6.193349880124978,
+                    "heading": -2.74665413649709,
+                    "angularVelocity": -2.274741257650809,
+                    "velocityX": 1.643428473584707,
+                    "velocityY": -2.482773699330916,
+                    "timestamp": 0.3690279598611829
+                },
+                {
+                    "x": 1.1777092396516182,
+                    "y": 6.018032409257481,
+                    "heading": -2.813593357981312,
+                    "angularVelocity": -1.0883601585538758,
+                    "velocityX": 1.8431640517068764,
+                    "velocityY": -2.850474597102823,
+                    "timestamp": 0.43053261983804675
+                },
+                {
+                    "x": 1.3085884905177934,
+                    "y": 5.825664975179578,
+                    "heading": -2.813593427712779,
+                    "angularVelocity": -0.0000011337590858279882,
+                    "velocityX": 2.1279566607702214,
+                    "velocityY": -3.1276887661888684,
+                    "timestamp": 0.4920372798149106
+                },
+                {
+                    "x": 1.4394691966966826,
+                    "y": 5.633298531244646,
+                    "heading": -2.8135934974374672,
+                    "angularVelocity": -0.000001133648866416165,
+                    "velocityX": 2.127980322598685,
+                    "velocityY": -3.127672667522972,
+                    "timestamp": 0.5535419397917744
+                },
+                {
+                    "x": 1.5703499198247641,
+                    "y": 5.440932098841455,
+                    "heading": -2.813593567162125,
+                    "angularVelocity": -0.0000011336483749077044,
+                    "velocityX": 2.1279805981744278,
+                    "velocityY": -3.1276724800292155,
+                    "timestamp": 0.6150465997686382
+                },
+                {
+                    "x": 1.7012306619349875,
+                    "y": 5.248565679353167,
+                    "heading": -2.81359363688675,
+                    "angularVelocity": -0.0000011336478448323905,
+                    "velocityX": 2.1279809068037574,
+                    "velocityY": -3.1276722700467015,
+                    "timestamp": 0.676551259745502
+                },
+                {
+                    "x": 1.8321095906543323,
+                    "y": 5.056198026100399,
+                    "heading": -2.8135937066202716,
+                    "angularVelocity": -0.0000011337924880961462,
+                    "velocityX": 2.1279514230072554,
+                    "velocityY": -3.127692329737781,
+                    "timestamp": 0.7380559197223658
+                },
+                {
+                    "x": 1.945432077761735,
+                    "y": 4.881276454017442,
+                    "heading": -2.879360901618447,
+                    "angularVelocity": -1.0693042612204542,
+                    "velocityX": 1.8425024567249244,
+                    "velocityY": -2.8440377062283995,
+                    "timestamp": 0.7995605796992297
+                },
+                {
+                    "x": 2.046464590922967,
+                    "y": 4.730069111144926,
+                    "heading": -3.0091287979589167,
+                    "angularVelocity": -2.109887224631182,
+                    "velocityX": 1.6426806228867414,
+                    "velocityY": -2.458469698546365,
+                    "timestamp": 0.8610652396760935
+                },
+                {
+                    "x": 2.131137367212944,
+                    "y": 4.6049715874663555,
+                    "heading": -3.1629399624477488,
+                    "angularVelocity": -2.500805053579533,
+                    "velocityX": 1.3766887959681082,
+                    "velocityY": -2.0339519595039928,
+                    "timestamp": 0.9225698996529573
+                },
+                {
+                    "x": 2.1987047903626262,
+                    "y": 4.5060589736155245,
+                    "heading": -3.302229643809037,
+                    "angularVelocity": -2.2647012667606727,
+                    "velocityX": 1.0985740458544027,
+                    "velocityY": -1.6082133270558499,
+                    "timestamp": 0.9840745596298212
+                },
+                {
+                    "x": 2.2493460059898798,
+                    "y": 4.432630316522509,
+                    "heading": -3.4143130823982606,
+                    "angularVelocity": -1.8223568528203613,
+                    "velocityX": 0.8233720119142732,
+                    "velocityY": -1.1938714419466272,
+                    "timestamp": 1.0455792196066849
+                },
+                {
+                    "x": 2.2831132091553847,
+                    "y": 4.384103436110321,
+                    "heading": -3.4927231006079147,
+                    "angularVelocity": -1.2748630467862023,
+                    "velocityX": 0.5490186138449845,
+                    "velocityY": -0.7889951823234699,
+                    "timestamp": 1.1070838795835487
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": -3.5331853071795862,
+                    "angularVelocity": -0.6578722097950352,
+                    "velocityX": 0.2745611609098736,
+                    "velocityY": -0.39189609566798916,
+                    "timestamp": 1.1685885395604125
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": -3.5331853071795862,
+                    "angularVelocity": 8.150679561734844e-31,
+                    "velocityX": 0,
+                    "velocityY": -5.044788235947052e-32,
+                    "timestamp": 1.2300931995372764
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -134,7 +474,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "Amp Side To C1": {
             "waypoints": [
@@ -145,19 +501,290 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 29
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
+                    "heading": -3.143185307179586,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 0.71,
+                    "y": 6.72,
+                    "heading": -2.1,
+                    "angularVelocity": 0,
+                    "velocityX": -2.7669422223227046e-34,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 0.7613814086171108,
+                    "y": 6.725487327512705,
+                    "heading": -2.1469081960592686,
+                    "angularVelocity": -0.5864829148967596,
+                    "velocityX": 0.642410513062366,
+                    "velocityY": 0.06860685562450702,
+                    "timestamp": 0.07998220385929865
+                },
+                {
+                    "x": 0.8643773558424356,
+                    "y": 6.73664563014898,
+                    "heading": -2.237067397875382,
+                    "angularVelocity": -1.1272407793953514,
+                    "velocityX": 1.2877357994099667,
+                    "velocityY": 0.13950981715763813,
+                    "timestamp": 0.1599644077185973
+                },
+                {
+                    "x": 1.019344149471572,
+                    "y": 6.7536581703311604,
+                    "heading": -2.3645059003180053,
+                    "angularVelocity": -1.5933357208661998,
+                    "velocityX": 1.9375159241891806,
+                    "velocityY": 0.21270406867141206,
+                    "timestamp": 0.23994661157789593
+                },
+                {
+                    "x": 1.2269206708549203,
+                    "y": 6.776747585260753,
+                    "heading": -2.5146569204728233,
+                    "angularVelocity": -1.8773053618147075,
+                    "velocityX": 2.59528384274717,
+                    "velocityY": 0.28868190441726393,
+                    "timestamp": 0.3199288154371946
+                },
+                {
+                    "x": 1.4861241127588112,
+                    "y": 6.80668700452381,
+                    "heading": -2.6232938505110392,
+                    "angularVelocity": -1.3582637736430165,
+                    "velocityX": 3.240763937436266,
+                    "velocityY": 0.37432601026755163,
+                    "timestamp": 0.39991101929649325
+                },
+                {
+                    "x": 1.7870157378738414,
+                    "y": 6.83849222949431,
+                    "heading": -2.623293892215126,
+                    "angularVelocity": -5.214170797715324e-7,
+                    "velocityX": 3.7619821734888195,
+                    "velocityY": 0.39765377091197274,
+                    "timestamp": 0.4798932231557919
+                },
+                {
+                    "x": 2.0879074285185357,
+                    "y": 6.870296834526242,
+                    "heading": -2.623293933917092,
+                    "angularVelocity": -5.213905536622117e-7,
+                    "velocityX": 3.7619829927918795,
+                    "velocityY": 0.3976460199556515,
+                    "timestamp": 0.5598754270150905
+                },
+                {
+                    "x": 2.3887991234818986,
+                    "y": 6.9021013987008635,
+                    "heading": -2.623293975619048,
+                    "angularVelocity": -5.213904366102477e-7,
+                    "velocityX": 3.7619830467872393,
+                    "velocityY": 0.3976455091256453,
+                    "timestamp": 0.6398576308743892
+                },
+                {
+                    "x": 2.689690823126622,
+                    "y": 6.933905918586809,
+                    "heading": -2.623294017320994,
+                    "angularVelocity": -5.213903027395266e-7,
+                    "velocityX": 3.761983105317272,
+                    "velocityY": 0.39764495539400585,
+                    "timestamp": 0.7198398347336878
+                },
+                {
+                    "x": 2.9905825278407376,
+                    "y": 6.965710390512973,
+                    "heading": -2.62329405902293,
+                    "angularVelocity": -5.213901895916786e-7,
+                    "velocityX": 3.7619831686987752,
+                    "velocityY": 0.39764435576335205,
+                    "timestamp": 0.7998220385929865
+                },
+                {
+                    "x": 3.2914742379996778,
+                    "y": 6.997514810927433,
+                    "heading": -2.623294100724858,
+                    "angularVelocity": -5.21390082509775e-7,
+                    "velocityX": 3.761983236774224,
+                    "velocityY": 0.39764371172379265,
+                    "timestamp": 0.8798042424522852
+                },
+                {
+                    "x": 3.5923659539723536,
+                    "y": 7.029319176339941,
+                    "heading": -2.6232941424267775,
+                    "angularVelocity": -5.213899786160023e-7,
+                    "velocityX": 3.7619833094620927,
+                    "velocityY": 0.3976430240464025,
+                    "timestamp": 0.9597864463115838
+                },
+                {
+                    "x": 3.8932576761280617,
+                    "y": 7.0611234832565914,
+                    "heading": -2.6232941841286896,
+                    "angularVelocity": -5.213898862684442e-7,
+                    "velocityX": 3.761983386767183,
+                    "velocityY": 0.39764229268549517,
+                    "timestamp": 1.0397686501708825
+                },
+                {
+                    "x": 4.194149404850362,
+                    "y": 7.092927728048495,
+                    "heading": -2.623294225830597,
+                    "angularVelocity": -5.213898264324783e-7,
+                    "velocityX": 3.7619834688678555,
+                    "velocityY": 0.3976415159533747,
+                    "timestamp": 1.119750854030181
+                },
+                {
+                    "x": 4.49504114052058,
+                    "y": 7.124731907107895,
+                    "heading": -2.623294267532503,
+                    "angularVelocity": -5.213898089255833e-7,
+                    "velocityX": 3.761983555736148,
+                    "velocityY": 0.3976406941142751,
+                    "timestamp": 1.1997330578894796
+                },
+                {
+                    "x": 4.795932883453666,
+                    "y": 7.15653601745498,
+                    "heading": -2.6232943092344048,
+                    "angularVelocity": -5.213897595490136e-7,
+                    "velocityX": 3.761983646542196,
+                    "velocityY": 0.39763983501921824,
+                    "timestamp": 1.2797152617487781
+                },
+                {
+                    "x": 5.0968246339465235,
+                    "y": 7.188340056280654,
+                    "heading": -2.6232943509362996,
+                    "angularVelocity": -5.21389667495749e-7,
+                    "velocityX": 3.7619837410603636,
+                    "velocityY": 0.3976389408026617,
+                    "timestamp": 1.3596974656080767
+                },
+                {
+                    "x": 5.3977163922506195,
+                    "y": 7.220144021205662,
+                    "heading": -2.6232943926381824,
+                    "angularVelocity": -5.213895176915956e-7,
+                    "velocityX": 3.7619838387225806,
+                    "velocityY": 0.39763801683879874,
+                    "timestamp": 1.4396796694673752
+                },
+                {
+                    "x": 5.698608158576608,
+                    "y": 7.251947910236858,
+                    "heading": -2.623294434340057,
+                    "angularVelocity": -5.213894134163355e-7,
+                    "velocityX": 3.7619839390185463,
+                    "velocityY": 0.3976370679550778,
+                    "timestamp": 1.5196618733266738
+                },
+                {
+                    "x": 5.999499932773201,
+                    "y": 7.283751724805354,
+                    "heading": -2.62329447604193,
+                    "angularVelocity": -5.213893967687742e-7,
+                    "velocityX": 3.761984037422991,
+                    "velocityY": 0.3976361369642237,
+                    "timestamp": 1.5996440771859723
+                },
+                {
+                    "x": 6.300391714248171,
+                    "y": 7.315555470513951,
+                    "heading": -2.6232945177438096,
+                    "angularVelocity": -5.213894781911953e-7,
+                    "velocityX": 3.7619841284229465,
+                    "velocityY": 0.39763527602395277,
+                    "timestamp": 1.679626281045271
+                },
+                {
+                    "x": 6.601283502174795,
+                    "y": 7.347359155184038,
+                    "heading": -2.6232945594457013,
+                    "angularVelocity": -5.213896319500141e-7,
+                    "velocityX": 3.76198420908656,
+                    "velocityY": 0.39763451287282225,
+                    "timestamp": 1.7596084849045694
+                },
+                {
+                    "x": 6.902175248741792,
+                    "y": 7.379163231145718,
+                    "heading": -2.6232946011491864,
+                    "angularVelocity": -5.214095570695983e-7,
+                    "velocityX": 3.7619836919762037,
+                    "velocityY": 0.39763940510601253,
+                    "timestamp": 1.839590688763868
+                },
+                {
+                    "x": 7.162101881336522,
+                    "y": 7.409614310010791,
+                    "heading": -2.734801003168492,
+                    "angularVelocity": -1.3941401541705845,
+                    "velocityX": 3.249805832457199,
+                    "velocityY": 0.3807231783540282,
+                    "timestamp": 1.9195728926231665
+                },
+                {
+                    "x": 7.370045087720824,
+                    "y": 7.432975674976622,
+                    "heading": -2.8811468250983516,
+                    "angularVelocity": -1.8297298007354832,
+                    "velocityX": 2.5998684251074975,
+                    "velocityY": 0.29208203623556095,
+                    "timestamp": 1.999555096482465
+                },
+                {
+                    "x": 7.525332087472029,
+                    "y": 7.4501357007114875,
+                    "heading": -3.00655027342712,
+                    "angularVelocity": -1.5678918844168468,
+                    "velocityX": 1.9415193912933517,
+                    "velocityY": 0.21454804827649476,
+                    "timestamp": 2.0795373003417637
+                },
+                {
+                    "x": 7.62852758064907,
+                    "y": 7.461415013154985,
+                    "heading": -3.096236840109313,
+                    "angularVelocity": -1.1213315256974696,
+                    "velocityX": 1.2902306787967257,
+                    "velocityY": 0.14102277630833499,
+                    "timestamp": 2.159519504201062
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -0.5869864145386997,
+                    "velocityX": 0.6435484003601272,
+                    "velocityY": 0.06982786889492548,
+                    "timestamp": 2.2395017080603608
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -1.1375235030887645e-31,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 2.3194839119196593
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -175,7 +802,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "Amp Side To C2": {
             "waypoints": [
@@ -186,19 +829,290 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 29
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
+                    "heading": -3.143185307179586,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 0.71,
+                    "y": 6.72,
+                    "heading": -2.1,
+                    "angularVelocity": 0,
+                    "velocityX": -7.879763479482e-33,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 0.7634290430022197,
+                    "y": 6.720160003423765,
+                    "heading": -2.158070133372329,
+                    "angularVelocity": -0.7094408010373536,
+                    "velocityX": 0.6527407612984071,
+                    "velocityY": 0.0019547562668324166,
+                    "timestamp": 0.08185338831290496
+                },
+                {
+                    "x": 0.8707119576691547,
+                    "y": 6.720819673735276,
+                    "heading": -2.2686879013575907,
+                    "angularVelocity": -1.351413426679394,
+                    "velocityX": 1.3106716396005418,
+                    "velocityY": 0.008059169267236224,
+                    "timestamp": 0.16370677662580993
+                },
+                {
+                    "x": 1.0324112516405533,
+                    "y": 6.722281472922437,
+                    "heading": -2.4240599050623355,
+                    "angularVelocity": -1.898174368919177,
+                    "velocityX": 1.9754746541861241,
+                    "velocityY": 0.017858749861073935,
+                    "timestamp": 0.2455601649387149
+                },
+                {
+                    "x": 1.249511925601742,
+                    "y": 6.724737672202976,
+                    "heading": -2.6048330548635152,
+                    "angularVelocity": -2.2084992878991065,
+                    "velocityX": 2.652311387908187,
+                    "velocityY": 0.030007301239015387,
+                    "timestamp": 0.32741355325161986
+                },
+                {
+                    "x": 1.518512933103673,
+                    "y": 6.730171304043105,
+                    "heading": -2.7105117995327306,
+                    "angularVelocity": -1.2910735504953308,
+                    "velocityX": 3.2863759588498382,
+                    "velocityY": 0.0663824913314231,
+                    "timestamp": 0.4092669415645248
+                },
+                {
+                    "x": 1.8281517964730614,
+                    "y": 6.7323448579922625,
+                    "heading": -2.71051185012585,
+                    "angularVelocity": -6.180943842017169e-7,
+                    "velocityX": 3.782847231512476,
+                    "velocityY": 0.026554233049558205,
+                    "timestamp": 0.49112032987742976
+                },
+                {
+                    "x": 2.1377906615261124,
+                    "y": 6.734518172124384,
+                    "heading": -2.710511900718092,
+                    "angularVelocity": -6.180836584503726e-7,
+                    "velocityX": 3.7828472520817247,
+                    "velocityY": 0.026551303213165345,
+                    "timestamp": 0.5729737181903347
+                },
+                {
+                    "x": 2.4474295268603647,
+                    "y": 6.736691446193322,
+                    "heading": -2.7105119513103224,
+                    "angularVelocity": -6.180835185678023e-7,
+                    "velocityX": 3.7828472555171566,
+                    "velocityY": 0.026550813762651566,
+                    "timestamp": 0.6548271065032397
+                },
+                {
+                    "x": 2.7570683925033053,
+                    "y": 6.738864676282315,
+                    "heading": -2.7105120019025377,
+                    "angularVelocity": -6.180833387834062e-7,
+                    "velocityX": 3.7828472592883804,
+                    "velocityY": 0.026550276461191975,
+                    "timestamp": 0.7366804948161447
+                },
+                {
+                    "x": 3.0667072584849335,
+                    "y": 6.741037858115924,
+                    "heading": -2.7105120524947366,
+                    "angularVelocity": -6.180831373401399e-7,
+                    "velocityX": 3.7828472634261248,
+                    "velocityY": 0.026549686926837145,
+                    "timestamp": 0.8185338831290497
+                },
+                {
+                    "x": 3.376346124838015,
+                    "y": 6.7432109870244785,
+                    "heading": -2.7105121030869213,
+                    "angularVelocity": -6.18082958211415e-7,
+                    "velocityX": 3.7828472679641574,
+                    "velocityY": 0.02654904034329684,
+                    "timestamp": 0.9003872714419547
+                },
+                {
+                    "x": 3.6859849915958844,
+                    "y": 6.745384058256853,
+                    "heading": -2.710512153679093,
+                    "angularVelocity": -6.180828069034381e-7,
+                    "velocityX": 3.782847272909432,
+                    "velocityY": 0.026548335715409193,
+                    "timestamp": 0.9822406597548597
+                },
+                {
+                    "x": 3.99562385879327,
+                    "y": 6.747557066862843,
+                    "heading": -2.7105122042712537,
+                    "angularVelocity": -6.180826661377271e-7,
+                    "velocityX": 3.7828472782789873,
+                    "velocityY": 0.026547570611047785,
+                    "timestamp": 1.0640940480677648
+                },
+                {
+                    "x": 4.305262726470487,
+                    "y": 6.749730007095753,
+                    "heading": -2.710512254863404,
+                    "angularVelocity": -6.180825475741077e-7,
+                    "velocityX": 3.782847284141071,
+                    "velocityY": 0.026546735299496697,
+                    "timestamp": 1.1459474363806696
+                },
+                {
+                    "x": 4.614901594678237,
+                    "y": 6.751902871728125,
+                    "heading": -2.710512305455549,
+                    "angularVelocity": -6.180824717562797e-7,
+                    "velocityX": 3.78284729062258,
+                    "velocityY": 0.0265458116903545,
+                    "timestamp": 1.2278008246935745
+                },
+                {
+                    "x": 4.924540463476948,
+                    "y": 6.754075652145792,
+                    "heading": -2.7105123560476914,
+                    "angularVelocity": -6.180824480187367e-7,
+                    "velocityX": 3.782847297842324,
+                    "velocityY": 0.02654478284224149,
+                    "timestamp": 1.3096542130064794
+                },
+                {
+                    "x": 5.234179332930726,
+                    "y": 6.756248339209459,
+                    "heading": -2.7105124066398263,
+                    "angularVelocity": -6.180823518950674e-7,
+                    "velocityX": 3.782847305845261,
+                    "velocityY": 0.026543642339650734,
+                    "timestamp": 1.3915076013193843
+                },
+                {
+                    "x": 5.543818204521741,
+                    "y": 6.758420721665928,
+                    "heading": -2.710512457231931,
+                    "angularVelocity": -6.180819838461299e-7,
+                    "velocityX": 3.782847331955805,
+                    "velocityY": 0.026539920964112583,
+                    "timestamp": 1.4733609896322892
+                },
+                {
+                    "x": 5.852559225022806,
+                    "y": 6.734757839446224,
+                    "heading": -2.710512508060742,
+                    "angularVelocity": -6.209738209329952e-7,
+                    "velocityX": 3.771878316396938,
+                    "velocityY": -0.2890886096156145,
+                    "timestamp": 1.5552143779451941
+                },
+                {
+                    "x": 6.152023822280739,
+                    "y": 6.6560056480568655,
+                    "heading": -2.710512532930908,
+                    "angularVelocity": -3.03837945152059e-7,
+                    "velocityX": 3.6585485760608294,
+                    "velocityY": -0.9621127849748639,
+                    "timestamp": 1.637067766258099
+                },
+                {
+                    "x": 6.432464531176267,
+                    "y": 6.5247274697787905,
+                    "heading": -2.710512568697855,
+                    "angularVelocity": -4.369635492236228e-7,
+                    "velocityX": 3.4261343931600465,
+                    "velocityY": -1.603820941122575,
+                    "timestamp": 1.718921154571004
+                },
+                {
+                    "x": 6.699924709540006,
+                    "y": 6.368695406956045,
+                    "heading": -2.710512603773811,
+                    "angularVelocity": -4.2852173297406095e-7,
+                    "velocityX": 3.267551702823897,
+                    "velocityY": -1.9062382882218016,
+                    "timestamp": 1.8007745428839088
+                },
+                {
+                    "x": 6.967384720466867,
+                    "y": 6.212663057124264,
+                    "heading": -2.710512638850014,
+                    "angularVelocity": -4.2852475398494205e-7,
+                    "velocityX": 3.2675496572533906,
+                    "velocityY": -1.9062417946012,
+                    "timestamp": 1.8826279311968137
+                },
+                {
+                    "x": 7.2050885186595135,
+                    "y": 6.073759568822601,
+                    "heading": -2.7948440651741784,
+                    "angularVelocity": -1.0302741042531607,
+                    "velocityX": 2.9040190405309123,
+                    "velocityY": -1.6969790886440652,
+                    "timestamp": 1.9644813195097186
+                },
+                {
+                    "x": 7.39556651196216,
+                    "y": 5.9626310397123286,
+                    "heading": -2.9179406119000064,
+                    "angularVelocity": -1.5038662328217944,
+                    "velocityX": 2.3270630236404783,
+                    "velocityY": -1.3576533775908837,
+                    "timestamp": 2.0463347078226235
+                },
+                {
+                    "x": 7.537983690499959,
+                    "y": 5.879640169020095,
+                    "heading": -3.0253464428320265,
+                    "angularVelocity": -1.3121733033388265,
+                    "velocityX": 1.7399057201318795,
+                    "velocityY": -1.0138965826922115,
+                    "timestamp": 2.1281880961355286
+                },
+                {
+                    "x": 7.632713372468284,
+                    "y": 5.824499681694904,
+                    "heading": -3.1026583555829736,
+                    "angularVelocity": -0.9445169509098753,
+                    "velocityX": 1.1573092320405494,
+                    "velocityY": -0.6736494171066164,
+                    "timestamp": 2.2100414844484337
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -0.49511635904048823,
+                    "velocityX": 0.5776990849902374,
+                    "velocityY": -0.33596265544659853,
+                    "timestamp": 2.291894872761339
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -7.614661899058019e-31,
+                    "velocityX": 0,
+                    "velocityY": -2.3591575038755638e-32,
+                    "timestamp": 2.373748261074244
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -216,7 +1130,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "Amp Side To C3": {
             "waypoints": [
@@ -227,12 +1157,12 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 30
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
+                    "heading": -3.143185307179586,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -257,7 +1187,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "Amp Side To C4": {
             "waypoints": [
@@ -298,7 +1244,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "Amp Side To C5": {
             "waypoints": [
@@ -309,12 +1271,12 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 34
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": -3.143185307179586,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -341,8 +1303,18 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": [
                 {
-                    "x": 3.378675937652588,
-                    "y": 4.222219944000244,
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
                     "radius": 0.5
                 }
             ]
@@ -356,12 +1328,12 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 11
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 3.5331853071795862,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -386,7 +1358,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "Center To W2": {
             "waypoints": [
@@ -397,7 +1385,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 7
                 },
                 {
                     "x": 2.25,
@@ -409,7 +1397,80 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 1.35,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 1.425000004529508,
+                    "y": 5.5600000086998405,
+                    "heading": 3.139999994805454,
+                    "angularVelocity": -5.477109235911092e-8,
+                    "velocityX": 0.7907971475606559,
+                    "velocityY": 9.173079093903646e-8,
+                    "timestamp": 0.09484101550044495
+                },
+                {
+                    "x": 1.5750000090590133,
+                    "y": 5.560000023679525,
+                    "heading": 3.1399999868961004,
+                    "angularVelocity": -8.339591750663206e-8,
+                    "velocityX": 1.5815942473623312,
+                    "velocityY": 1.5794521377308047e-7,
+                    "timestamp": 0.1896820310008899
+                },
+                {
+                    "x": 1.7999999999999887,
+                    "y": 5.5600000423644245,
+                    "heading": 3.1399999785762156,
+                    "angularVelocity": -8.772454789703223e-8,
+                    "velocityX": 2.372391203887096,
+                    "velocityY": 1.9701287329918457e-7,
+                    "timestamp": 0.28452304650133486
+                },
+                {
+                    "x": 2.0249999909409757,
+                    "y": 5.560000024808709,
+                    "heading": 3.1399999889274675,
+                    "angularVelocity": 1.0914320422483106e-7,
+                    "velocityX": 2.372391203887217,
+                    "velocityY": -1.851067857722626e-7,
+                    "timestamp": 0.3793640620017798
+                },
+                {
+                    "x": 2.174999995470489,
+                    "y": 5.560000009522542,
+                    "heading": 3.1399999962990623,
+                    "angularVelocity": 7.772581212467168e-8,
+                    "velocityX": 1.5815942473624114,
+                    "velocityY": -1.6117675426775267e-7,
+                    "timestamp": 0.47420507750222474
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 3.902254149386954e-8,
+                    "velocityX": 0.7907971475606953,
+                    "velocityY": -1.0040532392087884e-7,
+                    "timestamp": 0.5690460930026697
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 1.1924381690495433e-31,
+                    "velocityX": 0,
+                    "velocityY": -3.802893138506909e-33,
+                    "timestamp": 0.6638871085031147
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -427,7 +1488,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "Center To W3": {
             "waypoints": [
@@ -438,7 +1515,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 11
                 },
                 {
                     "x": 2.3,
@@ -450,7 +1527,116 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 1.35,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 3.0431878595519687e-30,
+                    "velocityX": -9.811045254051387e-32,
+                    "velocityY": 1.5487452756550695e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 1.3816658692579147,
+                    "y": 5.519999048484489,
+                    "heading": 3.127016326666322,
+                    "angularVelocity": -0.16577538674563452,
+                    "velocityX": 0.4043094421708381,
+                    "velocityY": -0.5107316733298447,
+                    "timestamp": 0.07832087494146221
+                },
+                {
+                    "x": 1.4449977551622273,
+                    "y": 5.439997223821588,
+                    "heading": 3.1010513131375346,
+                    "angularVelocity": -0.33152098400576074,
+                    "velocityX": 0.8086207661960841,
+                    "velocityY": -1.0214623460564694,
+                    "timestamp": 0.15664174988292442
+                },
+                {
+                    "x": 1.5399958776695735,
+                    "y": 5.319994658705983,
+                    "heading": 3.0621074441034875,
+                    "angularVelocity": -0.49723485677543305,
+                    "velocityX": 1.2129348986250355,
+                    "velocityY": -1.5321913245389087,
+                    "timestamp": 0.23496262482438662
+                },
+                {
+                    "x": 1.6666605105444467,
+                    "y": 5.159991566646187,
+                    "heading": 3.010185320857369,
+                    "angularVelocity": -0.6629410522408352,
+                    "velocityX": 1.6172525264757869,
+                    "velocityY": -2.042917576947189,
+                    "timestamp": 0.31328349976584885
+                },
+                {
+                    "x": 1.8249919396490273,
+                    "y": 4.959988272167279,
+                    "heading": 2.9452809657766084,
+                    "angularVelocity": -0.8286980339439337,
+                    "velocityX": 2.021573804211457,
+                    "velocityY": -2.5536396858231227,
+                    "timestamp": 0.3916043747073111
+                },
+                {
+                    "x": 1.9833278878841576,
+                    "y": 4.759991849616898,
+                    "heading": 2.880203589095056,
+                    "angularVelocity": -0.8309071716856798,
+                    "velocityX": 2.021631504416561,
+                    "velocityY": -2.553551945121407,
+                    "timestamp": 0.4699252496487733
+                },
+                {
+                    "x": 2.109996705593866,
+                    "y": 4.599994984440838,
+                    "heading": 2.828128420495832,
+                    "angularVelocity": -0.6648951334896099,
+                    "velocityX": 1.6173059583971898,
+                    "velocityY": -2.0428380721696886,
+                    "timestamp": 0.5482461245902355
+                },
+                {
+                    "x": 2.2049983397447894,
+                    "y": 4.479997467084651,
+                    "heading": 2.789065384524658,
+                    "angularVelocity": -0.4987563788117872,
+                    "velocityX": 1.212979735248467,
+                    "velocityY": -1.5321268748066883,
+                    "timestamp": 0.6265669995316977
+                },
+                {
+                    "x": 2.268332774342349,
+                    "y": 4.3999991596507995,
+                    "heading": 2.7630214971574416,
+                    "angularVelocity": -0.3325280442370214,
+                    "velocityX": 0.8086533078811499,
+                    "velocityY": -1.0214174381177892,
+                    "timestamp": 0.7048878744731599
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": -0.1662583208777211,
+                    "velocityX": 0.4043267606665432,
+                    "velocityY": -0.510708794822514,
+                    "timestamp": 0.7832087494146222
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 6.867412911994789e-31,
+                    "velocityX": 0,
+                    "velocityY": 9.317443122449736e-32,
+                    "timestamp": 0.8615296243560844
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -468,7 +1654,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "Center To C1": {
             "waypoints": [
@@ -479,7 +1681,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 23
                 },
                 {
                     "x": 7.68,
@@ -491,14 +1693,231 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
-            "constraints": [
+            "trajectory": [
                 {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
+                    "x": 1.35,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": -5.5460688640051806e-30,
+                    "velocityX": 1.8487232872526845e-30,
+                    "velocityY": -2.9628257998243394e-30,
+                    "timestamp": 0
+                },
+                {
+                    "x": 1.423394920733784,
+                    "y": 5.582111402692338,
+                    "heading": 3.1399999953305415,
+                    "angularVelocity": -4.870437841651369e-8,
+                    "velocityX": 0.7654820737524797,
+                    "velocityY": 0.23061381110796328,
+                    "timestamp": 0.09588065253313424
+                },
+                {
+                    "x": 1.5701847607820971,
+                    "y": 5.626334202094221,
+                    "heading": 3.1399999893931145,
+                    "angularVelocity": -6.193245197617188e-8,
+                    "velocityX": 1.530964132702707,
+                    "velocityY": 0.46122755981751334,
+                    "timestamp": 0.19176130506626848
+                },
+                {
+                    "x": 1.7903695158505508,
+                    "y": 5.692668391072556,
+                    "heading": 3.1399999853590668,
+                    "angularVelocity": -4.208451353086861e-8,
+                    "velocityX": 2.2964461468641186,
+                    "velocityY": 0.6918412341313931,
+                    "timestamp": 0.2876419575994027
+                },
+                {
+                    "x": 2.0839491699386645,
+                    "y": 5.781113958575535,
+                    "heading": 3.139999985983334,
+                    "angularVelocity": 6.496408603804424e-9,
+                    "velocityX": 3.0619279941465973,
+                    "velocityY": 0.922454793178729,
+                    "timestamp": 0.38352261013253697
+                },
+                {
+                    "x": 2.431241902978436,
+                    "y": 5.885741341465996,
+                    "heading": 3.1399999859835575,
+                    "angularVelocity": 2.3334348335212476e-12,
+                    "velocityX": 3.6221356849840607,
+                    "velocityY": 1.0912251859537307,
+                    "timestamp": 0.47940326266567124
+                },
+                {
+                    "x": 2.7785346564747577,
+                    "y": 5.990368656458575,
+                    "heading": 3.139999985983492,
+                    "angularVelocity": -6.768139199697334e-13,
+                    "velocityX": 3.62213589833913,
+                    "velocityY": 1.0912244778026317,
+                    "timestamp": 0.5752839151988055
+                },
+                {
+                    "x": 3.125827431847794,
+                    "y": 6.094995898834488,
+                    "heading": 3.139999985983408,
+                    "angularVelocity": -8.748780481443456e-13,
+                    "velocityX": 3.6221361265054535,
+                    "velocityY": 1.091223720437653,
+                    "timestamp": 0.6711645677319398
+                },
+                {
+                    "x": 3.473120229952072,
+                    "y": 6.199623065757117,
+                    "heading": 3.1399999859833065,
+                    "angularVelocity": -1.0615094861925232e-12,
+                    "velocityX": 3.6221363635841364,
+                    "velocityY": 1.0912229334878236,
+                    "timestamp": 0.767045220265074
+                },
+                {
+                    "x": 3.820413051729017,
+                    "y": 6.304250154101482,
+                    "heading": 3.139999985983188,
+                    "angularVelocity": -1.2288425429953676e-12,
+                    "velocityX": 3.6221366104815065,
+                    "velocityY": 1.0912221139455949,
+                    "timestamp": 0.8629258727982083
+                },
+                {
+                    "x": 4.167705898643341,
+                    "y": 6.408877159005614,
+                    "heading": 3.139999985983049,
+                    "angularVelocity": -1.4178096575705385e-12,
+                    "velocityX": 3.6221368726552323,
+                    "velocityY": 1.0912212436948225,
+                    "timestamp": 0.9588065253313426
+                },
+                {
+                    "x": 4.5149987734968,
+                    "y": 6.5135040711691525,
+                    "heading": 3.139999985982883,
+                    "angularVelocity": -1.676018359549876e-12,
+                    "velocityX": 3.622137164050193,
+                    "velocityY": 1.0912202764447858,
+                    "timestamp": 1.0546871778644769
+                },
+                {
+                    "x": 4.862291683371807,
+                    "y": 6.6181308670796435,
+                    "heading": 3.1399999859826795,
+                    "angularVelocity": -2.066469238283253e-12,
+                    "velocityX": 3.6221375293116886,
+                    "velocityY": 1.0912190639696435,
+                    "timestamp": 1.150567830397611
+                },
+                {
+                    "x": 5.209584643601777,
+                    "y": 6.722757495842404,
+                    "heading": 3.139999985982697,
+                    "angularVelocity": 1.7634954198261724e-13,
+                    "velocityX": 3.6221380544925466,
+                    "velocityY": 1.0912173206895408,
+                    "timestamp": 1.2464484829307452
+                },
+                {
+                    "x": 5.55687764566899,
+                    "y": 6.827383985745101,
+                    "heading": 3.139999985982843,
+                    "angularVelocity": 1.4429880633934061e-12,
+                    "velocityX": 3.6221384908382315,
+                    "velocityY": 1.091215872433919,
+                    "timestamp": 1.3423291354638793
+                },
+                {
+                    "x": 5.904170680203765,
+                    "y": 6.932010367880933,
+                    "heading": 3.1399999859829473,
+                    "angularVelocity": 9.965559645934633e-13,
+                    "velocityX": 3.6221388294623984,
+                    "velocityY": 1.0912147484660204,
+                    "timestamp": 1.4382097879970135
+                },
+                {
+                    "x": 6.251463742058712,
+                    "y": 7.036636659333844,
+                    "heading": 3.1399999859830086,
+                    "angularVelocity": 5.792335690532441e-13,
+                    "velocityX": 3.6221391144013104,
+                    "velocityY": 1.0912138026768703,
+                    "timestamp": 1.5340904405301476
+                },
+                {
+                    "x": 6.598756826003656,
+                    "y": 7.141262877462233,
+                    "heading": 3.139999985983042,
+                    "angularVelocity": 3.176331549030912e-13,
+                    "velocityX": 3.6221393447915826,
+                    "velocityY": 1.0912130379289529,
+                    "timestamp": 1.6299710930632818
+                },
+                {
+                    "x": 6.9460499272931315,
+                    "y": 7.245889038012359,
+                    "heading": 3.139999985982778,
+                    "angularVelocity": -2.760419154137832e-12,
+                    "velocityX": 3.622139525688641,
+                    "velocityY": 1.0912124374075387,
+                    "timestamp": 1.725851745596416
+                },
+                {
+                    "x": 7.239629936933939,
+                    "y": 7.334333425317839,
+                    "heading": 3.139999987228266,
+                    "angularVelocity": 1.3004483336236485e-8,
+                    "velocityX": 3.061931702425989,
+                    "velocityY": 0.9224424841673509,
+                    "timestamp": 1.8217323981295501
+                },
+                {
+                    "x": 7.459814962920636,
+                    "y": 7.4006667150284455,
+                    "heading": 3.13999999169634,
+                    "angularVelocity": 4.661129161255315e-8,
+                    "velocityX": 2.2964489724386126,
+                    "velocityY": 0.6918318551100047,
+                    "timestamp": 1.9176130506626843
+                },
+                {
+                    "x": 7.606604986293984,
+                    "y": 7.444888905913496,
+                    "heading": 3.1399999966584766,
+                    "angularVelocity": 5.176056299229228e-8,
+                    "velocityX": 1.530966044713388,
+                    "velocityY": 0.46122121321737414,
+                    "timestamp": 2.0134937031958184
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "angularVelocity": 3.485452160899378e-8,
+                    "velocityX": 0.7654830434176825,
+                    "velocityY": 0.23061059246584142,
+                    "timestamp": 2.109374355728953
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "angularVelocity": -1.7922203019823127e-29,
+                    "velocityX": 0,
+                    "velocityY": -5.07505109843192e-30,
+                    "timestamp": 2.205255008262087
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
                 {
                     "scope": [
                         "last"
@@ -509,7 +1928,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "Center To C2": {
             "waypoints": [
@@ -520,7 +1955,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 22
                 },
                 {
                     "x": 7.68,
@@ -550,7 +1985,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "Center To C3": {
             "waypoints": [
@@ -561,7 +2012,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 22
                 },
                 {
                     "x": 7.68,
@@ -573,7 +2024,215 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 1.35,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 4.242495082651476e-31,
+                    "velocityX": 1.1828951034939025e-32,
+                    "velocityY": -1.8669507935571478e-37,
+                    "timestamp": 0
+                },
+                {
+                    "x": 1.458096319535533,
+                    "y": 5.590985596762502,
+                    "heading": 3.1146996746822455,
+                    "angularVelocity": -0.21764643682588541,
+                    "velocityX": 0.9299002477407399,
+                    "velocityY": 0.2665540716802577,
+                    "timestamp": 0.11624507015474053
+                },
+                {
+                    "x": 1.674289226072187,
+                    "y": 5.652956746779397,
+                    "heading": 3.0641193580719652,
+                    "angularVelocity": -0.4351179498876838,
+                    "velocityX": 1.8598027963583075,
+                    "velocityY": 0.5331077690813153,
+                    "timestamp": 0.23249014030948106
+                },
+                {
+                    "x": 1.9985791294926345,
+                    "y": 5.745913563807996,
+                    "heading": 2.9882935824900567,
+                    "angularVelocity": -0.652292398128992,
+                    "velocityX": 2.789708870997854,
+                    "velocityY": 0.7996624450813951,
+                    "timestamp": 0.3487352104642216
+                },
+                {
+                    "x": 2.421300960273809,
+                    "y": 5.867094149195328,
+                    "heading": 2.9882935750965625,
+                    "angularVelocity": -6.360264782367115e-8,
+                    "velocityX": 3.6364710367369995,
+                    "velocityY": 1.0424578455329059,
+                    "timestamp": 0.4649802806189621
+                },
+                {
+                    "x": 2.844022813063846,
+                    "y": 5.988274657808269,
+                    "heading": 2.988293567703214,
+                    "angularVelocity": -6.360139701488917e-8,
+                    "velocityX": 3.636471226068578,
+                    "velocityY": 1.0424571850800302,
+                    "timestamp": 0.5812253507737026
+                },
+                {
+                    "x": 3.266744690508405,
+                    "y": 6.109455080417214,
+                    "heading": 2.988293560309863,
+                    "angularVelocity": -6.360141400691824e-8,
+                    "velocityX": 3.636471438159484,
+                    "velocityY": 1.0424564452293308,
+                    "timestamp": 0.6974704209284431
+                },
+                {
+                    "x": 3.6894665932724995,
+                    "y": 6.2306354147022835,
+                    "heading": 2.9882935529165104,
+                    "angularVelocity": -6.360143032479277e-8,
+                    "velocityX": 3.6364716559711723,
+                    "velocityY": 1.0424556854218385,
+                    "timestamp": 0.8137154910831836
+                },
+                {
+                    "x": 4.112188522356607,
+                    "y": 6.35181565717337,
+                    "heading": 2.988293545523163,
+                    "angularVelocity": -6.360138139725806e-8,
+                    "velocityX": 3.6364718823894826,
+                    "velocityY": 1.042454895590637,
+                    "timestamp": 0.9299605612379241
+                },
+                {
+                    "x": 4.534910480631917,
+                    "y": 6.472995797814601,
+                    "heading": 2.988293538129828,
+                    "angularVelocity": -6.360128257953913e-8,
+                    "velocityX": 3.636472133507264,
+                    "velocityY": 1.0424540195977474,
+                    "timestamp": 1.0462056313926646
+                },
+                {
+                    "x": 4.95763256393494,
+                    "y": 6.594175502311251,
+                    "heading": 2.9882935307365046,
+                    "angularVelocity": -6.360117492960731e-8,
+                    "velocityX": 3.6364732090600738,
+                    "velocityY": 1.0424502676572964,
+                    "timestamp": 1.1624507015474053
+                },
+                {
+                    "x": 5.395072294009931,
+                    "y": 6.63917509969073,
+                    "heading": 2.988293514020312,
+                    "angularVelocity": -1.4380130211846583e-7,
+                    "velocityX": 3.763081991285213,
+                    "velocityY": 0.3871097270583396,
+                    "timestamp": 1.2786957717021459
+                },
+                {
+                    "x": 5.834408437796079,
+                    "y": 6.6201427711176,
+                    "heading": 2.9882934665190843,
+                    "angularVelocity": -4.0863004174605414e-7,
+                    "velocityX": 3.7793959193394016,
+                    "velocityY": -0.16372589863635184,
+                    "timestamp": 1.3949408418568865
+                },
+                {
+                    "x": 6.258700069994256,
+                    "y": 6.504578567848497,
+                    "heading": 2.988293412737735,
+                    "angularVelocity": -4.6265488113342876e-7,
+                    "velocityX": 3.649975277518237,
+                    "velocityY": -0.9941428321671584,
+                    "timestamp": 1.511185912011627
+                },
+                {
+                    "x": 6.6396986873456,
+                    "y": 6.284991393688008,
+                    "heading": 2.988293385419437,
+                    "angularVelocity": -2.350060774071174e-7,
+                    "velocityX": 3.277546452887644,
+                    "velocityY": -1.8890020356836066,
+                    "timestamp": 1.6274309821663677
+                },
+                {
+                    "x": 6.952392117761244,
+                    "y": 5.9757968201953435,
+                    "heading": 2.988293399279684,
+                    "angularVelocity": 1.1923298633596667e-7,
+                    "velocityX": 2.689950034004883,
+                    "velocityY": -2.6598510636285697,
+                    "timestamp": 1.7436760523211083
+                },
+                {
+                    "x": 7.176252511109016,
+                    "y": 5.597293189983828,
+                    "heading": 2.988293408698133,
+                    "angularVelocity": 8.102235350708823e-8,
+                    "velocityX": 1.9257624693226023,
+                    "velocityY": -3.2560832877271055,
+                    "timestamp": 1.859921122475849
+                },
+                {
+                    "x": 7.318788154943764,
+                    "y": 5.181285899414456,
+                    "heading": 2.98829341726252,
+                    "angularVelocity": 7.367527341392295e-8,
+                    "velocityX": 1.226165063559341,
+                    "velocityY": -3.5787091015180317,
+                    "timestamp": 1.9761661926305896
+                },
+                {
+                    "x": 7.461323091390308,
+                    "y": 4.765278366475121,
+                    "heading": 2.9882934258271017,
+                    "angularVelocity": 7.36769447232769e-8,
+                    "velocityX": 1.2261589782414601,
+                    "velocityY": -3.578711186509359,
+                    "timestamp": 2.09241126278533
+                },
+                {
+                    "x": 7.570661558741158,
+                    "y": 4.44613912293617,
+                    "heading": 3.0641409244839792,
+                    "angularVelocity": 0.6524792712147914,
+                    "velocityX": 0.9405858433850398,
+                    "velocityY": -2.745400240320956,
+                    "timestamp": 2.2086563329400706
+                },
+                {
+                    "x": 7.643553854632525,
+                    "y": 4.2333797165247065,
+                    "heading": 3.1147139591088293,
+                    "angularVelocity": 0.43505530649626106,
+                    "velocityX": 0.6270570940714855,
+                    "velocityY": -1.8302660588379978,
+                    "timestamp": 2.324901403094811
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": 0.21752355482697977,
+                    "velocityX": 0.31352852485666055,
+                    "velocityY": -0.9151331439956911,
+                    "timestamp": 2.4411464732495514
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": -2.3792206080273036e-31,
+                    "velocityX": 0,
+                    "velocityY": -4.1394011967092386e-32,
+                    "timestamp": 2.5573915434042918
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -591,7 +2250,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "Center To C4": {
             "waypoints": [
@@ -632,7 +2307,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "Center To C5": {
             "waypoints": [
@@ -643,7 +2334,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 26
                 },
                 {
                     "x": 7.68,
@@ -673,7 +2364,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "Stage Side To W1": {
             "waypoints": [
@@ -684,19 +2391,209 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 20
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 3.5331853071795862,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 0.71,
+                    "y": 4.36,
+                    "heading": 2.1,
+                    "angularVelocity": 1.7952667234276065e-30,
+                    "velocityX": 8.378323076428447e-32,
+                    "velocityY": 1.4697485073236794e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 0.7269607278962015,
+                    "y": 4.384959045157939,
+                    "heading": 2.138406592301522,
+                    "angularVelocity": 0.6202480809957288,
+                    "velocityX": 0.2739076366713438,
+                    "velocityY": 0.4030766317709414,
+                    "timestamp": 0.06192133999006512
+                },
+                {
+                    "x": 0.7608491761808356,
+                    "y": 4.435120384688249,
+                    "heading": 2.213462658415359,
+                    "angularVelocity": 1.2121195394976814,
+                    "velocityX": 0.5472822178924311,
+                    "velocityY": 0.8100816219151135,
+                    "timestamp": 0.12384267998013024
+                },
+                {
+                    "x": 0.8116352150421299,
+                    "y": 4.5108343869767795,
+                    "heading": 2.3220976741276163,
+                    "angularVelocity": 1.7544035017602526,
+                    "velocityX": 0.8201702170760925,
+                    "velocityY": 1.2227448937745646,
+                    "timestamp": 0.18576401997019537
+                },
+                {
+                    "x": 0.879352990275961,
+                    "y": 4.612608489350272,
+                    "heading": 2.458161516132951,
+                    "angularVelocity": 2.197365916615584,
+                    "velocityX": 1.093609654518071,
+                    "velocityY": 1.6436030355580518,
+                    "timestamp": 0.24768535996026048
+                },
+                {
+                    "x": 0.9641073070497683,
+                    "y": 4.741095934499406,
+                    "heading": 2.609605752410294,
+                    "angularVelocity": 2.445751921738789,
+                    "velocityX": 1.3687416452454926,
+                    "velocityY": 2.075010734098273,
+                    "timestamp": 0.3096066999503256
+                },
+                {
+                    "x": 1.065422526066935,
+                    "y": 4.896666065794743,
+                    "heading": 2.7474397378967583,
+                    "angularVelocity": 2.2259528864940465,
+                    "velocityX": 1.6361922890141296,
+                    "velocityY": 2.5123831512738084,
+                    "timestamp": 0.3715280399403907
+                },
+                {
+                    "x": 1.1791068919270769,
+                    "y": 5.075184614148285,
+                    "heading": 2.811143860874752,
+                    "angularVelocity": 1.0287910918629104,
+                    "velocityX": 1.8359480895985416,
+                    "velocityY": 2.882989101692323,
+                    "timestamp": 0.4334493799304558
+                },
+                {
+                    "x": 1.309522757298702,
+                    "y": 5.269766971770384,
+                    "heading": 2.8111439317105513,
+                    "angularVelocity": 0.0000011439642533999806,
+                    "velocityX": 2.1061537975849616,
+                    "velocityY": 3.142411931869009,
+                    "timestamp": 0.4953707199205209
+                },
+                {
+                    "x": 1.4399391293197474,
+                    "y": 5.464348989818422,
+                    "heading": 2.811144002543952,
+                    "angularVelocity": 0.0000011439255168645237,
+                    "velocityX": 2.1061619797305715,
+                    "velocityY": 3.142406447910498,
+                    "timestamp": 0.5572920599105861
+                },
+                {
+                    "x": 1.570355539071906,
+                    "y": 5.658930982577611,
+                    "heading": 2.811144073377348,
+                    "angularVelocity": 0.0000011439254352819296,
+                    "velocityX": 2.1061625890699887,
+                    "velocityY": 3.1424060395076645,
+                    "timestamp": 0.6192133999006512
+                },
+                {
+                    "x": 1.7007719870139304,
+                    "y": 5.853512949740463,
+                    "heading": 2.811144144210739,
+                    "angularVelocity": 0.0000011439253582531217,
+                    "velocityX": 2.1061632058180395,
+                    "velocityY": 3.1424056261390936,
+                    "timestamp": 0.6811347398907163
+                },
+                {
+                    "x": 1.8311879242943474,
+                    "y": 6.048095259166571,
+                    "heading": 2.811144215047033,
+                    "angularVelocity": 0.0000011439722415453888,
+                    "velocityX": 2.1061549588775286,
+                    "velocityY": 3.1424111535268193,
+                    "timestamp": 0.7430560798807814
+                },
+                {
+                    "x": 1.9446720292676574,
+                    "y": 6.226098534814738,
+                    "heading": 2.87431577200777,
+                    "angularVelocity": 1.0201904056157727,
+                    "velocityX": 1.8327139721381605,
+                    "velocityY": 2.8746676941540104,
+                    "timestamp": 0.8049774198708465
+                },
+                {
+                    "x": 2.045803800007245,
+                    "y": 6.3800371688336455,
+                    "heading": 3.00267788576032,
+                    "angularVelocity": 2.072986692037753,
+                    "velocityX": 1.6332296871452403,
+                    "velocityY": 2.486035251233359,
+                    "timestamp": 0.8668987598609116
+                },
+                {
+                    "x": 2.1306920593857335,
+                    "y": 6.5074855802636336,
+                    "heading": 3.157852124619465,
+                    "angularVelocity": 2.5059896779372313,
+                    "velocityX": 1.370904754194726,
+                    "velocityY": 2.0582308369042988,
+                    "timestamp": 0.9288200998509767
+                },
+                {
+                    "x": 2.1984321789198162,
+                    "y": 6.6082437599743225,
+                    "heading": 3.2989222381105456,
+                    "angularVelocity": 2.278214804681454,
+                    "velocityX": 1.0939705042712429,
+                    "velocityY": 1.6271963708610766,
+                    "timestamp": 0.9907414398410418
+                },
+                {
+                    "x": 2.249205760326253,
+                    "y": 6.6830338505565665,
+                    "heading": 3.412569338792277,
+                    "angularVelocity": 1.8353462748055114,
+                    "velocityX": 0.8199690351433494,
+                    "velocityY": 1.2078241619810486,
+                    "timestamp": 1.052662779831107
+                },
+                {
+                    "x": 2.2830654942840813,
+                    "y": 6.732454760660193,
+                    "heading": 3.492119269429147,
+                    "angularVelocity": 1.2846933003974643,
+                    "velocityX": 0.5468184952596451,
+                    "velocityY": 0.7981240411069244,
+                    "timestamp": 1.1145841198211721
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 3.5331853071795862,
+                    "angularVelocity": 0.66319685195811,
+                    "velocityX": 0.2734841610119513,
+                    "velocityY": 0.3963938658908936,
+                    "timestamp": 1.1765054598112372
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 3.5331853071795862,
+                    "angularVelocity": -1.1911850535673714e-30,
+                    "velocityX": 0,
+                    "velocityY": -7.498214836328326e-32,
+                    "timestamp": 1.2384267998013023
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -714,7 +2611,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "Stage Side To W2": {
             "waypoints": [
@@ -725,7 +2638,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 15
                 },
                 {
                     "x": 2.25,
@@ -737,7 +2650,152 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 0.71,
+                    "y": 4.36,
+                    "heading": 2.1,
+                    "angularVelocity": 8.845615831720266e-31,
+                    "velocityX": 5.520878611448458e-31,
+                    "velocityY": 4.542986326160959e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 0.7374422286617038,
+                    "y": 4.3811878915004145,
+                    "heading": 2.1328463142733396,
+                    "angularVelocity": 0.5012247413747498,
+                    "velocityX": 0.41876004258027855,
+                    "velocityY": 0.3233207643693322,
+                    "timestamp": 0.06553210877669326
+                },
+                {
+                    "x": 0.7924456920447251,
+                    "y": 4.423718037322063,
+                    "heading": 2.19563146481745,
+                    "angularVelocity": 0.9580822548845105,
+                    "velocityX": 0.8393360813467762,
+                    "velocityY": 0.6489970583210544,
+                    "timestamp": 0.13106421755338651
+                },
+                {
+                    "x": 0.8751785876629536,
+                    "y": 4.487773457465306,
+                    "heading": 2.284091563401004,
+                    "angularVelocity": 1.3498741339912115,
+                    "velocityX": 1.262478762893885,
+                    "velocityY": 0.9774661816777632,
+                    "timestamp": 0.19659632633007978
+                },
+                {
+                    "x": 0.985868892669873,
+                    "y": 4.573562834319481,
+                    "heading": 2.3916808862774843,
+                    "angularVelocity": 1.641780264436542,
+                    "velocityX": 1.6891003062957237,
+                    "velocityY": 1.309119734671004,
+                    "timestamp": 0.26212843510677303
+                },
+                {
+                    "x": 1.1247521909380378,
+                    "y": 4.681315175779475,
+                    "heading": 2.507240555710109,
+                    "angularVelocity": 1.7634053228228717,
+                    "velocityX": 2.1193167877661256,
+                    "velocityY": 1.6442678783186102,
+                    "timestamp": 0.32766054388346627
+                },
+                {
+                    "x": 1.291492862531941,
+                    "y": 4.810942835643569,
+                    "heading": 2.6039577624805075,
+                    "angularVelocity": 1.4758750874318616,
+                    "velocityX": 2.5444118113470626,
+                    "velocityY": 1.9780785676502115,
+                    "timestamp": 0.3931926526601595
+                },
+                {
+                    "x": 1.4800025601939901,
+                    "y": 4.960730002903915,
+                    "heading": 2.6228917229305413,
+                    "angularVelocity": 0.2889264637363189,
+                    "velocityX": 2.8766005120392,
+                    "velocityY": 2.28570650413168,
+                    "timestamp": 0.45872476143685276
+                },
+                {
+                    "x": 1.6682727616271493,
+                    "y": 5.110599963848934,
+                    "heading": 2.6420440088534156,
+                    "angularVelocity": 0.2922580438871624,
+                    "velocityX": 2.8729458726059685,
+                    "velocityY": 2.2869699105167616,
+                    "timestamp": 0.5242568702135461
+                },
+                {
+                    "x": 1.8350043100801503,
+                    "y": 5.239747952271642,
+                    "heading": 2.7356929069056295,
+                    "angularVelocity": 1.429053631881899,
+                    "velocityX": 2.544272594998539,
+                    "velocityY": 1.9707589277004427,
+                    "timestamp": 0.5897889789902393
+                },
+                {
+                    "x": 1.9739563679504606,
+                    "y": 5.347167856813287,
+                    "heading": 2.8514085217331835,
+                    "angularVelocity": 1.765785002003577,
+                    "velocityX": 2.1203660383309844,
+                    "velocityY": 1.6391949923004236,
+                    "timestamp": 0.6553210877669325
+                },
+                {
+                    "x": 2.0846918002256074,
+                    "y": 5.4326634298654595,
+                    "heading": 2.9588833399365435,
+                    "angularVelocity": 1.640032958035698,
+                    "velocityX": 1.6897889346501833,
+                    "velocityY": 1.304636378229562,
+                    "timestamp": 0.7208531965436258
+                },
+                {
+                    "x": 2.1674771737861436,
+                    "y": 5.496490038501947,
+                    "heading": 3.046413104094783,
+                    "angularVelocity": 1.3356775143083182,
+                    "velocityX": 1.2632795602936622,
+                    "velocityY": 0.9739745878464279,
+                    "timestamp": 0.786385305320319
+                },
+                {
+                    "x": 2.2225294464291743,
+                    "y": 5.538874362150552,
+                    "heading": 3.1080101238304096,
+                    "angularVelocity": 0.9399517409934369,
+                    "velocityX": 0.8400808957732031,
+                    "velocityY": 0.6467718564197722,
+                    "timestamp": 0.8519174140970123
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0.4881557570289305,
+                    "velocityX": 0.41919227205757975,
+                    "velocityY": 0.3223707926359362,
+                    "timestamp": 0.9174495228737055
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 7.973690679262731e-27,
+                    "velocityX": -1.618419533015103e-29,
+                    "velocityY": -2.1923403816587234e-30,
+                    "timestamp": 0.9829816316503988
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -755,7 +2813,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "Stage Side To W3": {
             "waypoints": [
@@ -766,7 +2840,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 12
                 },
                 {
                     "x": 2.3,
@@ -778,7 +2852,125 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 0.71,
+                    "y": 4.36,
+                    "heading": 2.1,
+                    "angularVelocity": -5.8020277899403155e-31,
+                    "velocityX": 1.3777732804976719e-32,
+                    "velocityY": 5.338313673616136e-29,
+                    "timestamp": 0
+                },
+                {
+                    "x": 0.7541175647990941,
+                    "y": 4.360036481014467,
+                    "heading": 2.123748591233672,
+                    "angularVelocity": 0.32465550054626485,
+                    "velocityX": 0.603109883101075,
+                    "velocityY": 0.0004987142982711039,
+                    "timestamp": 0.07315012742330113
+                },
+                {
+                    "x": 0.8424087590063745,
+                    "y": 4.360099698646948,
+                    "heading": 2.1698601870465493,
+                    "angularVelocity": 0.6303693163234944,
+                    "velocityX": 1.2069861983474726,
+                    "velocityY": 0.0008642176672118523,
+                    "timestamp": 0.14630025484660225
+                },
+                {
+                    "x": 0.9749550737228709,
+                    "y": 4.360177642554779,
+                    "heading": 2.23617090700284,
+                    "angularVelocity": 0.906501769606733,
+                    "velocityX": 1.8119765390084532,
+                    "velocityY": 0.0010655334525686096,
+                    "timestamp": 0.21945038226990338
+                },
+                {
+                    "x": 1.1518842000389728,
+                    "y": 4.360257435104989,
+                    "heading": 2.318743489919177,
+                    "angularVelocity": 1.1288098302053016,
+                    "velocityX": 2.4187124827857356,
+                    "velocityY": 0.0010908053480699021,
+                    "timestamp": 0.2926005096932045
+                },
+                {
+                    "x": 1.3733949578522506,
+                    "y": 4.360320997027151,
+                    "heading": 2.4076375641139776,
+                    "angularVelocity": 1.2152278789650626,
+                    "velocityX": 3.028166397188238,
+                    "velocityY": 0.0008689242848102628,
+                    "timestamp": 0.36575063711650563
+                },
+                {
+                    "x": 1.6366157123115315,
+                    "y": 4.3597910751743765,
+                    "heading": 2.4407985098803153,
+                    "angularVelocity": 0.45332724541279523,
+                    "velocityX": 3.5983635809144485,
+                    "velocityY": -0.007244305258449855,
+                    "timestamp": 0.43890076453980675
+                },
+                {
+                    "x": 1.8581231700320338,
+                    "y": 4.359897336997892,
+                    "heading": 2.530170275911139,
+                    "angularVelocity": 1.2217581729403981,
+                    "velocityX": 3.0281212832164655,
+                    "velocityY": 0.0014526539769375909,
+                    "timestamp": 0.5120508919631079
+                },
+                {
+                    "x": 2.0350502325675994,
+                    "y": 4.359957426055228,
+                    "heading": 2.6131109520937392,
+                    "angularVelocity": 1.1338418551586247,
+                    "velocityX": 2.4186842698404125,
+                    "velocityY": 0.000821448430104839,
+                    "timestamp": 0.585201019386409
+                },
+                {
+                    "x": 2.1675938018574246,
+                    "y": 4.359986783385126,
+                    "heading": 2.6797755559230114,
+                    "angularVelocity": 0.9113395448181003,
+                    "velocityX": 1.8119390076086361,
+                    "velocityY": 0.00040132985313166364,
+                    "timestamp": 0.6583511468097101
+                },
+                {
+                    "x": 2.255882928551814,
+                    "y": 4.359997887713372,
+                    "heading": 2.726140403287784,
+                    "angularVelocity": 0.6338313957609201,
+                    "velocityX": 1.2069579343789958,
+                    "velocityY": 0.00015180189885440488,
+                    "timestamp": 0.7315012742330111
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 0.3261730027365948,
+                    "velocityX": 0.6031031387394808,
+                    "velocityY": 0.000028876048490911067,
+                    "timestamp": 0.8046514016563122
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 1.0612763050961144e-31,
+                    "velocityX": 9.233550959236252e-32,
+                    "velocityY": 1.483165202160369e-28,
+                    "timestamp": 0.8778015290796133
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -796,7 +2988,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "Stage Side To C1": {
             "waypoints": [
@@ -807,7 +3015,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 30
                 },
                 {
                     "x": 7.68,
@@ -832,12 +3040,35 @@
                         "last"
                     ],
                     "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        0,
+                        1
+                    ],
+                    "type": "StraightLine"
                 }
             ],
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "Stage Side To C2": {
             "waypoints": [
@@ -848,7 +3079,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 29
                 },
                 {
                     "x": 7.68,
@@ -878,7 +3109,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "Stage Side To C3": {
             "waypoints": [
@@ -889,7 +3136,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 29
                 },
                 {
                     "x": 7.68,
@@ -901,7 +3148,278 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 0.71,
+                    "y": 4.36,
+                    "heading": 2.1,
+                    "angularVelocity": 0,
+                    "velocityX": 1.276910697826558e-30,
+                    "velocityY": -1.1746574795316565e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 0.761216454009274,
+                    "y": 4.38433482667126,
+                    "heading": 2.165113129644946,
+                    "angularVelocity": 0.7716161812421117,
+                    "velocityX": 0.6069351123943075,
+                    "velocityY": 0.28837726169296907,
+                    "timestamp": 0.08438538644967974
+                },
+                {
+                    "x": 0.8642342955307559,
+                    "y": 4.433284699389541,
+                    "heading": 2.2862365234671036,
+                    "angularVelocity": 1.435359828498015,
+                    "velocityX": 1.2208019167265607,
+                    "velocityY": 0.58007523313856,
+                    "timestamp": 0.1687707728993595
+                },
+                {
+                    "x": 1.0198907802831574,
+                    "y": 4.507085180578877,
+                    "heading": 2.44934046785083,
+                    "angularVelocity": 1.9328458545483174,
+                    "velocityX": 1.844590530437629,
+                    "velocityY": 0.8745647119048293,
+                    "timestamp": 0.25315615934903923
+                },
+                {
+                    "x": 1.22914875569361,
+                    "y": 4.606070154859929,
+                    "heading": 2.62757889489394,
+                    "angularVelocity": 2.112195423189735,
+                    "velocityX": 2.4797892646404183,
+                    "velocityY": 1.1730108546707563,
+                    "timestamp": 0.337541545798719
+                },
+                {
+                    "x": 1.4854756632487267,
+                    "y": 4.729647702991699,
+                    "heading": 2.7127404908574917,
+                    "angularVelocity": 1.0091983878551618,
+                    "velocityX": 3.0375746126133176,
+                    "velocityY": 1.4644425217566792,
+                    "timestamp": 0.4219269322483987
+                },
+                {
+                    "x": 1.7744042397686677,
+                    "y": 4.865385720511678,
+                    "heading": 2.7127405410221264,
+                    "angularVelocity": 5.944706378546851e-7,
+                    "velocityX": 3.4239172050510245,
+                    "velocityY": 1.6085488640966181,
+                    "timestamp": 0.5063123186980785
+                },
+                {
+                    "x": 2.0633325652631687,
+                    "y": 5.001124272354809,
+                    "heading": 2.712740591190551,
+                    "angularVelocity": 5.945155495227871e-7,
+                    "velocityX": 3.423914230300861,
+                    "velocityY": 1.608555196036035,
+                    "timestamp": 0.5906977051477582
+                },
+                {
+                    "x": 2.352241240419818,
+                    "y": 5.1369046433555585,
+                    "heading": 2.712740641359129,
+                    "angularVelocity": 5.945173720540621e-7,
+                    "velocityX": 3.4236813660732097,
+                    "velocityY": 1.6090507694920106,
+                    "timestamp": 0.675083091597438
+                },
+                {
+                    "x": 2.647177658207551,
+                    "y": 5.259039982285096,
+                    "heading": 2.712740691973867,
+                    "angularVelocity": 5.998045355791158e-7,
+                    "velocityX": 3.4951124856626223,
+                    "velocityY": 1.4473517758000545,
+                    "timestamp": 0.7594684780471177
+                },
+                {
+                    "x": 2.9596305957770053,
+                    "y": 5.324444044648143,
+                    "heading": 2.712740749087516,
+                    "angularVelocity": 6.768191931683843e-7,
+                    "velocityX": 3.7026901305450077,
+                    "velocityY": 0.7750638483126594,
+                    "timestamp": 0.8438538644967974
+                },
+                {
+                    "x": 3.2787911260313356,
+                    "y": 5.330853358998101,
+                    "heading": 2.712740828995151,
+                    "angularVelocity": 9.469368813287509e-7,
+                    "velocityX": 3.7821777405102064,
+                    "velocityY": 0.0759528944478236,
+                    "timestamp": 0.9282392509464772
+                },
+                {
+                    "x": 3.5936179112160946,
+                    "y": 5.278046121689486,
+                    "heading": 2.7127406682187845,
+                    "angularVelocity": -0.000001905263138772926,
+                    "velocityX": 3.730821157908589,
+                    "velocityY": -0.6257865198035278,
+                    "timestamp": 1.012624637396157
+                },
+                {
+                    "x": 3.893219599027341,
+                    "y": 5.167849261276542,
+                    "heading": 2.7127405436852854,
+                    "angularVelocity": -0.0000014757709178373628,
+                    "velocityX": 3.550397769285573,
+                    "velocityY": -1.3058761125501486,
+                    "timestamp": 1.0970100238458367
+                },
+                {
+                    "x": 4.167231798281599,
+                    "y": 5.0040755200168485,
+                    "heading": 2.7127404397173622,
+                    "angularVelocity": -0.0000012320607563537726,
+                    "velocityX": 3.247152271058871,
+                    "velocityY": -1.940783210815242,
+                    "timestamp": 1.1813954102955164
+                },
+                {
+                    "x": 4.431867133754474,
+                    "y": 4.825546352606267,
+                    "heading": 2.712740340652136,
+                    "angularVelocity": -0.0000011739618702897745,
+                    "velocityX": 3.136032749351509,
+                    "velocityY": -2.115640810829663,
+                    "timestamp": 1.2657807967451962
+                },
+                {
+                    "x": 4.666216273140183,
+                    "y": 4.608786081760519,
+                    "heading": 2.712740321249029,
+                    "angularVelocity": -2.2993444839053286e-7,
+                    "velocityX": 2.7771294206901915,
+                    "velocityY": -2.5686944145832347,
+                    "timestamp": 1.350166183194876
+                },
+                {
+                    "x": 4.936653737211477,
+                    "y": 4.439174492795635,
+                    "heading": 2.7127404800965853,
+                    "angularVelocity": 0.0000018824059850759979,
+                    "velocityX": 3.2047902539685085,
+                    "velocityY": -2.009964000887954,
+                    "timestamp": 1.4345515696445557
+                },
+                {
+                    "x": 5.233678140767672,
+                    "y": 4.322208601201109,
+                    "heading": 2.7127406283303626,
+                    "angularVelocity": 0.0000017566285300639338,
+                    "velocityX": 3.5198559377731047,
+                    "velocityY": -1.3860917928520893,
+                    "timestamp": 1.5189369560942354
+                },
+                {
+                    "x": 5.5320155723758075,
+                    "y": 4.208633545762192,
+                    "heading": 2.7127407751281813,
+                    "angularVelocity": 0.000001739611855911675,
+                    "velocityX": 3.5354158363194896,
+                    "velocityY": -1.3459090515232621,
+                    "timestamp": 1.6033223425439151
+                },
+                {
+                    "x": 5.844921751101867,
+                    "y": 4.1454334406216455,
+                    "heading": 2.712740834966484,
+                    "angularVelocity": 7.091074065603108e-7,
+                    "velocityX": 3.708061216412759,
+                    "velocityY": -0.748946088885059,
+                    "timestamp": 1.687707728993595
+                },
+                {
+                    "x": 6.164108889380454,
+                    "y": 4.140523996019331,
+                    "heading": 2.7127408721494533,
+                    "angularVelocity": 4.406328022097576e-7,
+                    "velocityX": 3.7824930560568397,
+                    "velocityY": -0.05817884836341004,
+                    "timestamp": 1.7720931154432746
+                },
+                {
+                    "x": 6.483326965734431,
+                    "y": 4.138437864876922,
+                    "heading": 2.712740909220737,
+                    "angularVelocity": 4.393092878808321e-7,
+                    "velocityX": 3.7828596844114597,
+                    "velocityY": -0.02472147406577555,
+                    "timestamp": 1.8564785018929544
+                },
+                {
+                    "x": 6.802545327798344,
+                    "y": 4.1363959875067975,
+                    "heading": 2.712740946289213,
+                    "angularVelocity": 4.392760075001551e-7,
+                    "velocityX": 3.782863070186549,
+                    "velocityY": -0.024197049465056585,
+                    "timestamp": 1.9408638883426341
+                },
+                {
+                    "x": 7.094929199701602,
+                    "y": 4.132589002725807,
+                    "heading": 2.784661628236826,
+                    "angularVelocity": 0.8522883519706954,
+                    "velocityX": 3.464863813565768,
+                    "velocityY": -0.04511426611993158,
+                    "timestamp": 2.025249274792314
+                },
+                {
+                    "x": 7.329645788590582,
+                    "y": 4.130189422490391,
+                    "heading": 2.9078456854173678,
+                    "angularVelocity": 1.4597794993094293,
+                    "velocityX": 2.7814838417425016,
+                    "velocityY": -0.02843596902640405,
+                    "timestamp": 2.1096346612419934
+                },
+                {
+                    "x": 7.5051104174827215,
+                    "y": 4.128539949714214,
+                    "heading": 3.0176429032166108,
+                    "angularVelocity": 1.3011401904845583,
+                    "velocityX": 2.0793248247641927,
+                    "velocityY": -0.01954690077910012,
+                    "timestamp": 2.194020047691673
+                },
+                {
+                    "x": 7.621781937801462,
+                    "y": 4.1275005025427935,
+                    "heading": 3.0976594405390174,
+                    "angularVelocity": 0.9482274205158485,
+                    "velocityX": 1.3826033775209736,
+                    "velocityY": -0.01231785757139544,
+                    "timestamp": 2.2784054341413524
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": 0.5017522730222338,
+                    "velocityX": 0.6899069216594048,
+                    "velocityY": -0.00593115187164541,
+                    "timestamp": 2.362790820591032
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": 3.889752365812458e-29,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 2.4471762070407115
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -919,7 +3437,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "Stage Side To C4": {
             "waypoints": [
@@ -930,7 +3464,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 29
                 },
                 {
                     "x": 7.68,
@@ -960,7 +3494,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "Stage Side To C5": {
             "waypoints": [
@@ -971,7 +3521,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 31
                 },
                 {
                     "x": 7.68,
@@ -983,10 +3533,299 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
-            "constraints": [
+            "trajectory": [
                 {
-                    "scope": [
+                    "x": 0.71,
+                    "y": 4.36,
+                    "heading": 2.1,
+                    "angularVelocity": 0,
+                    "velocityX": 8.526478185532948e-32,
+                    "velocityY": -7.785685234118549e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 0.755735101962148,
+                    "y": 4.33059726025757,
+                    "heading": 2.1511666979053023,
+                    "angularVelocity": 0.6232717514717016,
+                    "velocityX": 0.5571083980530026,
+                    "velocityY": -0.35816063665566134,
+                    "timestamp": 0.08209372201529226
+                },
+                {
+                    "x": 0.8475193720501324,
+                    "y": 4.271815657764732,
+                    "heading": 2.250112545454498,
+                    "angularVelocity": 1.2052791019849762,
+                    "velocityX": 1.1180424986808006,
+                    "velocityY": -0.7160304229096619,
+                    "timestamp": 0.16418744403058452
+                },
+                {
+                    "x": 0.9858871774715989,
+                    "y": 4.183669855686051,
+                    "heading": 2.389658659666248,
+                    "angularVelocity": 1.6998390472021188,
+                    "velocityX": 1.6854858328349602,
+                    "velocityY": -1.0737215942317955,
+                    "timestamp": 0.2462811660458768
+                },
+                {
+                    "x": 1.17173213163872,
+                    "y": 4.066157035949835,
+                    "heading": 2.5522635706365153,
+                    "angularVelocity": 1.9807228491842224,
+                    "velocityX": 2.2638144501780824,
+                    "velocityY": -1.4314470930472174,
+                    "timestamp": 0.32837488806116905
+                },
+                {
+                    "x": 1.4047988716632624,
+                    "y": 3.9214804758601747,
+                    "heading": 2.664319566325619,
+                    "angularVelocity": 1.3649764310629042,
+                    "velocityX": 2.839032441252049,
+                    "velocityY": -1.76233403161705,
+                    "timestamp": 0.4104686100764613
+                },
+                {
+                    "x": 1.6657539489096052,
+                    "y": 3.7531140130982656,
+                    "heading": 2.6643196178388515,
+                    "angularVelocity": 6.274929563351661e-7,
+                    "velocityX": 3.178745838783293,
+                    "velocityY": -2.050905460597173,
+                    "timestamp": 0.49256233209175354
+                },
+                {
+                    "x": 1.9267089834441824,
+                    "y": 3.584747484136064,
+                    "heading": 2.664319669351516,
+                    "angularVelocity": 6.274860319306578e-7,
+                    "velocityX": 3.1787453185027585,
+                    "velocityY": -2.05090626699614,
+                    "timestamp": 0.5746560541070458
+                },
+                {
+                    "x": 2.1876640582968827,
+                    "y": 3.416381017663811,
+                    "heading": 2.6643197208641496,
+                    "angularVelocity": 6.274856686006067e-7,
+                    "velocityX": 3.1787458096258594,
+                    "velocityY": -2.0509055057936005,
+                    "timestamp": 0.6567497761223381
+                },
+                {
+                    "x": 2.4486192865071836,
+                    "y": 3.248014788884269,
+                    "heading": 2.6643197723767935,
+                    "angularVelocity": 6.274857907572903e-7,
+                    "velocityX": 3.178747677705374,
+                    "velocityY": -2.050902610411298,
+                    "timestamp": 0.7388434981376304
+                },
+                {
+                    "x": 2.7195077051073344,
+                    "y": 3.0961455009278414,
+                    "heading": 2.664319841917229,
+                    "angularVelocity": 8.470859127420906e-7,
+                    "velocityX": 3.2997458508421667,
+                    "velocityY": -1.849950084223712,
+                    "timestamp": 0.8209372201529227
+                },
+                {
+                    "x": 2.9903964357661867,
+                    "y": 2.9442767695902643,
+                    "heading": 2.664319911457928,
+                    "angularVelocity": 8.470891223849179e-7,
+                    "velocityX": 3.2997496520914464,
+                    "velocityY": -1.8499433039384836,
+                    "timestamp": 0.903030942168215
+                },
+                {
+                    "x": 3.2612853045995296,
+                    "y": 2.79240828471517,
+                    "heading": 2.6643199809985263,
+                    "angularVelocity": 8.470878995035866e-7,
+                    "velocityX": 3.299751335222467,
+                    "velocityY": -1.8499403017298282,
+                    "timestamp": 0.9851246641835073
+                },
+                {
+                    "x": 3.5321743153682585,
+                    "y": 2.6405400530113767,
+                    "heading": 2.664320050538881,
+                    "angularVelocity": 8.47084928778718e-7,
+                    "velocityX": 3.299753064165714,
+                    "velocityY": -1.8499372177997218,
+                    "timestamp": 1.0672183861987996
+                },
+                {
+                    "x": 3.8030633934021476,
+                    "y": 2.4886719412892284,
+                    "heading": 2.6643201200792013,
+                    "angularVelocity": 8.470845077302885e-7,
+                    "velocityX": 3.299753883536049,
+                    "velocityY": -1.8499357562793664,
+                    "timestamp": 1.1493121082140918
+                },
+                {
+                    "x": 4.073952519237506,
+                    "y": 2.3368039148311976,
+                    "heading": 2.664320189619518,
+                    "angularVelocity": 8.470844698893479e-7,
+                    "velocityX": 3.2997544658152695,
+                    "velocityY": -1.8499347176601568,
+                    "timestamp": 1.231405830229384
+                },
+                {
+                    "x": 4.3448416924892905,
+                    "y": 2.1849359729505413,
+                    "heading": 2.6643202591598345,
+                    "angularVelocity": 8.470844641828709e-7,
+                    "velocityX": 3.2997550434042244,
+                    "velocityY": -1.8499336874062895,
+                    "timestamp": 1.3134995522446762
+                },
+                {
+                    "x": 4.615730948551723,
+                    "y": 2.0330681787806033,
+                    "heading": 2.664320328700174,
+                    "angularVelocity": 8.470847405524923e-7,
+                    "velocityX": 3.29975605213723,
+                    "velocityY": -1.8499318881125704,
+                    "timestamp": 1.3955932742599684
+                },
+                {
+                    "x": 4.889033051365783,
+                    "y": 1.8855867361226037,
+                    "heading": 2.6643204031763625,
+                    "angularVelocity": 9.072093065949494e-7,
+                    "velocityX": 3.3291474196181556,
+                    "velocityY": -1.7965008655659056,
+                    "timestamp": 1.4776869962752606
+                },
+                {
+                    "x": 5.16486669963939,
+                    "y": 1.742896114789829,
+                    "heading": 2.6643204810266874,
+                    "angularVelocity": 9.483103351737687e-7,
+                    "velocityX": 3.359984679732577,
+                    "velocityY": -1.738142915559301,
+                    "timestamp": 1.5597807182905528
+                },
+                {
+                    "x": 5.440736180585003,
+                    "y": 1.6002747825074959,
+                    "heading": 2.6643205588558216,
+                    "angularVelocity": 9.480522059758943e-7,
+                    "velocityX": 3.3604211646564863,
+                    "velocityY": -1.7372988918174013,
+                    "timestamp": 1.641874440305845
+                },
+                {
+                    "x": 5.732995703254166,
+                    "y": 1.4952548268359402,
+                    "heading": 2.664320597611399,
+                    "angularVelocity": 4.720894158515159e-7,
+                    "velocityX": 3.5600715315931244,
+                    "velocityY": -1.2792690243961995,
+                    "timestamp": 1.7239681623211371
+                },
+                {
+                    "x": 6.025255587661767,
+                    "y": 1.3902358778499353,
+                    "heading": 2.6643206363668037,
+                    "angularVelocity": 4.720873108891009e-7,
+                    "velocityX": 3.560075938001194,
+                    "velocityY": -1.2792567617587365,
+                    "timestamp": 1.8060618843364293
+                },
+                {
+                    "x": 6.317515503776411,
+                    "y": 1.2852170171023363,
+                    "heading": 2.664320675122227,
+                    "angularVelocity": 4.7208754093652347e-7,
+                    "velocityX": 3.5600763242310083,
+                    "velocityY": -1.2792556869091232,
+                    "timestamp": 1.8881556063517215
+                },
+                {
+                    "x": 6.60977544021274,
+                    "y": 1.1801982129085715,
+                    "heading": 2.6643207138776748,
+                    "angularVelocity": 4.720878420422611e-7,
+                    "velocityX": 3.5600765717735112,
+                    "velocityY": -1.2792549980155836,
+                    "timestamp": 1.9702493283670137
+                },
+                {
+                    "x": 6.902035364394445,
+                    "y": 1.0751793746113998,
+                    "heading": 2.664320752633444,
+                    "angularVelocity": 4.720917543940102e-7,
+                    "velocityX": 3.5600764224974903,
+                    "velocityY": -1.2792554134359897,
+                    "timestamp": 2.052343050382306
+                },
+                {
+                    "x": 7.1614900474496475,
+                    "y": 0.9773985940984262,
+                    "heading": 2.7732250681220454,
+                    "angularVelocity": 1.3265851859940632,
+                    "velocityX": 3.1604692379140946,
+                    "velocityY": -1.1910871880648686,
+                    "timestamp": 2.1344367723975983
+                },
+                {
+                    "x": 7.369273961287434,
+                    "y": 0.9006112993544645,
+                    "heading": 2.9067869208138672,
+                    "angularVelocity": 1.626943564173427,
+                    "velocityX": 2.5310572932614863,
+                    "velocityY": -0.935361351135448,
+                    "timestamp": 2.2165304944128907
+                },
+                {
+                    "x": 7.524785907770447,
+                    "y": 0.8435839044250819,
+                    "heading": 3.0191766997290386,
+                    "angularVelocity": 1.3690423111067538,
+                    "velocityX": 1.8943220341994518,
+                    "velocityY": -0.6946620707337358,
+                    "timestamp": 2.298624216428183
+                },
+                {
+                    "x": 7.62830248590124,
+                    "y": 0.8058044628378582,
+                    "heading": 3.098670404043058,
+                    "angularVelocity": 0.9683286658535433,
+                    "velocityX": 1.2609560827502735,
+                    "velocityY": -0.460198912411186,
+                    "timestamp": 2.3807179384434756
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": 0.5034440508038269,
+                    "velocityX": 0.6297377293860565,
+                    "velocityY": -0.22906091204337628,
+                    "timestamp": 2.462811660458768
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": 1.633672716236069e-30,
+                    "velocityX": -5.394468608823283e-31,
+                    "velocityY": 5.5679704387753575e-31,
+                    "timestamp": 2.5449053824740604
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
                         "first"
                     ],
                     "type": "StopPoint"
@@ -1001,7 +3840,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W1 To W2": {
             "waypoints": [
@@ -1012,12 +3867,12 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 10
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": -3.143185307179586,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -1042,7 +3897,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W1 To W3": {
             "waypoints": [
@@ -1053,19 +3924,164 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 15
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -3.5331853071795862,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "angularVelocity": -3.577903969873187e-35,
+                    "velocityX": -4.511566930271669e-37,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.3001399949944257,
+                    "y": 6.713175615322878,
+                    "heading": -2.7771678602975163,
+                    "angularVelocity": -0.3719998319011172,
+                    "velocityX": 0.001916901582349506,
+                    "velocityY": -0.6000716859748281,
+                    "timestamp": 0.07303191552177325
+                },
+                {
+                    "x": 2.3004163850099326,
+                    "y": 6.6254379467440225,
+                    "heading": -2.829600510901429,
+                    "angularVelocity": -0.7179416044247195,
+                    "velocityX": 0.0037845100122603365,
+                    "velocityY": -1.2013606373599366,
+                    "timestamp": 0.1460638310435465
+                },
+                {
+                    "x": 2.3008288453246886,
+                    "y": 6.493659773055691,
+                    "heading": -2.9042529721112227,
+                    "angularVelocity": -1.022189554750722,
+                    "velocityX": 0.005647672141821651,
+                    "velocityY": -1.8043915834173219,
+                    "timestamp": 0.21909574656531977
+                },
+                {
+                    "x": 2.301391538953172,
+                    "y": 6.317653855065593,
+                    "heading": -2.995610403714753,
+                    "angularVelocity": -1.2509247628359643,
+                    "velocityX": 0.007704763382735362,
+                    "velocityY": -2.4099863290265735,
+                    "timestamp": 0.292127662087093
+                },
+                {
+                    "x": 2.3021600106867623,
+                    "y": 6.097194240566218,
+                    "heading": -3.0915567903699093,
+                    "angularVelocity": -1.3137596894408665,
+                    "velocityX": 0.010522409662952134,
+                    "velocityY": -3.0186749577128253,
+                    "timestamp": 0.36515957760886625
+                },
+                {
+                    "x": 2.303349915614448,
+                    "y": 5.834382556129677,
+                    "heading": -3.1370632737304405,
+                    "angularVelocity": -0.6231040639617954,
+                    "velocityX": 0.016292944244779627,
+                    "velocityY": -3.598586762498201,
+                    "timestamp": 0.4381914931306395
+                },
+                {
+                    "x": 2.3036783877036533,
+                    "y": 5.558107366997832,
+                    "heading": -3.137063302917444,
+                    "angularVelocity": -3.99647241237663e-7,
+                    "velocityX": 0.004497651292023743,
+                    "velocityY": -3.7829377356188716,
+                    "timestamp": 0.5112234086524128
+                },
+                {
+                    "x": 2.3040068828134284,
+                    "y": 5.281832177893351,
+                    "heading": -3.1370633321044417,
+                    "angularVelocity": -3.996471578074284e-7,
+                    "velocityX": 0.0044979665044766115,
+                    "velocityY": -3.782937735244173,
+                    "timestamp": 0.584255324174186
+                },
+                {
+                    "x": 2.302092596714716,
+                    "y": 5.019360768744023,
+                    "heading": -3.182327854705923,
+                    "angularVelocity": -0.6197909814920075,
+                    "velocityX": -0.026211637542794358,
+                    "velocityY": -3.5939274942210355,
+                    "timestamp": 0.6572872396959593
+                },
+                {
+                    "x": 2.3012286144088434,
+                    "y": 4.799012511457164,
+                    "heading": -3.280568867162049,
+                    "angularVelocity": -1.3451791830222102,
+                    "velocityX": -0.01183020190145165,
+                    "velocityY": -3.017150183075313,
+                    "timestamp": 0.7303191552177325
+                },
+                {
+                    "x": 2.3006858203530896,
+                    "y": 4.623118549812072,
+                    "heading": -3.3743826060457494,
+                    "angularVelocity": -1.284558103309387,
+                    "velocityX": -0.007432285623012945,
+                    "velocityY": -2.408453350681352,
+                    "timestamp": 0.8033510707395057
+                },
+                {
+                    "x": 2.3003278174317203,
+                    "y": 4.491440647891757,
+                    "heading": -3.4511285642395753,
+                    "angularVelocity": -1.0508550630983442,
+                    "velocityX": -0.00490200645582827,
+                    "velocityY": -1.8030185978218984,
+                    "timestamp": 0.876382986261279
+                },
+                {
+                    "x": 2.3001060473289097,
+                    "y": 4.403780156277999,
+                    "heading": -3.505127434733723,
+                    "angularVelocity": -0.7393872953811381,
+                    "velocityX": -0.0030366190072672575,
+                    "velocityY": -1.2003038806728776,
+                    "timestamp": 0.9494149017830522
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": -3.5331853071795862,
+                    "angularVelocity": -0.38418645116186434,
+                    "velocityX": -0.001452068293050597,
+                    "velocityY": -0.5994660822629829,
+                    "timestamp": 1.0224468173048256
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": -3.5331853071795862,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 8.704854669398703e-34,
+                    "timestamp": 1.095478732826599
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -1083,7 +4099,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W1 To S1": {
             "waypoints": [
@@ -1094,7 +4126,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 13
                 },
                 {
                     "x": 5.176,
@@ -1106,7 +4138,134 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "angularVelocity": 0,
+                    "velocityX": -1.6054364331739777e-34,
+                    "velocityY": -2.5654655421376103e-34,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.373020964607517,
+                    "y": 6.753775569864816,
+                    "heading": -2.7564987511657213,
+                    "angularVelocity": -0.06940035185307017,
+                    "velocityX": 0.7797929951747273,
+                    "velocityY": -0.03443378276852441,
+                    "timestamp": 0.09364147287724231
+                },
+                {
+                    "x": 2.5190628897265572,
+                    "y": 6.7473267051761905,
+                    "heading": -2.769496449901771,
+                    "angularVelocity": -0.13880280111661264,
+                    "velocityX": 1.5595859466082067,
+                    "velocityY": -0.06886761271975776,
+                    "timestamp": 0.18728294575448462
+                },
+                {
+                    "x": 2.738125767578475,
+                    "y": 6.73765340144405,
+                    "heading": -2.7889934253789708,
+                    "angularVelocity": -0.20820876560484325,
+                    "velocityX": 2.339378814973306,
+                    "velocityY": -0.10330149062058414,
+                    "timestamp": 0.2809244186317269
+                },
+                {
+                    "x": 3.030209577950943,
+                    "y": 6.724755654214012,
+                    "heading": -2.814990111841214,
+                    "angularVelocity": -0.2776193673960349,
+                    "velocityX": 3.1191714674903457,
+                    "velocityY": -0.13773541608988796,
+                    "timestamp": 0.37456589150896924
+                },
+                {
+                    "x": 3.3841048307253856,
+                    "y": 6.709128194593673,
+                    "heading": -2.814990114833307,
+                    "angularVelocity": -3.1952637479967674e-8,
+                    "velocityX": 3.779257650489721,
+                    "velocityY": -0.16688609373799812,
+                    "timestamp": 0.4682073643862116
+                },
+                {
+                    "x": 3.7380000807913594,
+                    "y": 6.693500673639671,
+                    "heading": -2.814990117825208,
+                    "angularVelocity": -3.1950597051067127e-8,
+                    "velocityX": 3.779257621565883,
+                    "velocityY": -0.16688674872173656,
+                    "timestamp": 0.5618488372634539
+                },
+                {
+                    "x": 4.091895327848636,
+                    "y": 6.677873084552109,
+                    "heading": -2.8149901208171126,
+                    "angularVelocity": -3.1950635675887034e-8,
+                    "velocityX": 3.779257589435919,
+                    "velocityY": -0.16688747632201045,
+                    "timestamp": 0.6554903101406963
+                },
+                {
+                    "x": 4.445790571527361,
+                    "y": 6.662245418924005,
+                    "heading": -2.814990123809099,
+                    "angularVelocity": -3.195149337938281e-8,
+                    "velocityX": 3.779257553356315,
+                    "velocityY": -0.16688829370019806,
+                    "timestamp": 0.7491317830179386
+                },
+                {
+                    "x": 4.737874328479073,
+                    "y": 6.649347276687054,
+                    "heading": -2.8409937626507493,
+                    "angularVelocity": -0.27769361205743737,
+                    "velocityX": 3.119170897008512,
+                    "velocityY": -0.13773963438078918,
+                    "timestamp": 0.8427732558951809
+                },
+                {
+                    "x": 4.956937160751323,
+                    "y": 6.639673651230963,
+                    "heading": -2.8604968062706018,
+                    "angularVelocity": -0.20827356747619122,
+                    "velocityX": 2.3393783282266916,
+                    "velocityY": -0.10330492632038867,
+                    "timestamp": 0.9364147287724233
+                },
+                {
+                    "x": 5.102979052789527,
+                    "y": 6.633224554793233,
+                    "heading": -2.8734989345094433,
+                    "angularVelocity": -0.1388501039053446,
+                    "velocityX": 1.5595855933369822,
+                    "velocityY": -0.06887008757539573,
+                    "timestamp": 1.0300562016496655
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "angularVelocity": -0.06942506659508625,
+                    "velocityX": 0.7797928093911877,
+                    "velocityY": -0.0344351139955655,
+                    "timestamp": 1.1236976745269078
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "angularVelocity": 1.756012262603739e-32,
+                    "velocityX": 4.952933956711668e-34,
+                    "velocityY": 0,
+                    "timestamp": 1.2173391474041502
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -1124,7 +4283,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W1 To S2": {
             "waypoints": [
@@ -1135,19 +4310,146 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 13
                 },
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": -3.143185307179586,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.347774752133698,
+                    "y": 6.727866832421063,
+                    "heading": -2.7630995428589378,
+                    "angularVelocity": -0.15973393577530476,
+                    "velocityX": 0.5825584351440611,
+                    "velocityY": -0.3552456424698836,
+                    "timestamp": 0.08200851494302656
+                },
+                {
+                    "x": 2.4433243460965994,
+                    "y": 6.669600579952885,
+                    "heading": -2.78929580141764,
+                    "angularVelocity": -0.31943339757952466,
+                    "velocityX": 1.1651179640221794,
+                    "velocityY": -0.7104902766335507,
+                    "timestamp": 0.16401702988605313
+                },
+                {
+                    "x": 2.586648917905952,
+                    "y": 6.582201380168291,
+                    "heading": -2.8285851612262256,
+                    "angularVelocity": -0.4790887853033408,
+                    "velocityX": 1.74767915147499,
+                    "velocityY": -1.065733233254043,
+                    "timestamp": 0.24602554482907968
+                },
+                {
+                    "x": 2.7777486316597817,
+                    "y": 6.465669444023111,
+                    "heading": -2.8809649184638944,
+                    "angularVelocity": -0.6387112030264004,
+                    "velocityX": 2.3302423399154564,
+                    "velocityY": -1.4209736175095709,
+                    "timestamp": 0.32803405977210626
+                },
+                {
+                    "x": 3.016623643228877,
+                    "y": 6.320005072616726,
+                    "heading": -2.9464355230657095,
+                    "angularVelocity": -0.7983391071927027,
+                    "velocityX": 2.9128074290218198,
+                    "velocityY": -1.776210330202685,
+                    "timestamp": 0.41004257471513283
+                },
+                {
+                    "x": 3.2814961744061293,
+                    "y": 6.158489554910389,
+                    "heading": -2.9464355313484747,
+                    "angularVelocity": -1.0099884469874073e-7,
+                    "velocityX": 3.229817432510106,
+                    "velocityY": -1.9694969213690323,
+                    "timestamp": 0.4920510896581594
+                },
+                {
+                    "x": 3.546368691279801,
+                    "y": 5.996974013747309,
+                    "heading": -2.9464355396312305,
+                    "angularVelocity": -1.009987284542081e-7,
+                    "velocityX": 3.229817258094308,
+                    "velocityY": -1.9694972073971664,
+                    "timestamp": 0.574059604601186
+                },
+                {
+                    "x": 3.785245806593771,
+                    "y": 5.851315740974219,
+                    "heading": -3.0120060162505475,
+                    "angularVelocity": -0.7995569321657724,
+                    "velocityX": 2.912833081783328,
+                    "velocityY": -1.7761359643481167,
+                    "timestamp": 0.6560681195442125
+                },
+                {
+                    "x": 3.976347491680443,
+                    "y": 5.7347893459358685,
+                    "heading": -3.0644727513052605,
+                    "angularVelocity": -0.6397717979793073,
+                    "velocityX": 2.3302663780637296,
+                    "velocityY": -1.4209060500523003,
+                    "timestamp": 0.738076634487239
+                },
+                {
+                    "x": 4.119673744072056,
+                    "y": 5.64739465388186,
+                    "heading": -3.1038280635353432,
+                    "angularVelocity": -0.4798929996162412,
+                    "velocityX": 1.7476996442526245,
+                    "velocityY": -1.0656782666376068,
+                    "timestamp": 0.8200851494302656
+                },
+                {
+                    "x": 4.2152245778751,
+                    "y": 5.589131554648123,
+                    "heading": -3.130066448240266,
+                    "angularVelocity": -0.3199470777290782,
+                    "velocityX": 1.1651330824539987,
+                    "velocityY": -0.7104518265476871,
+                    "timestamp": 0.9020936643732921
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -0.15996947327279604,
+                    "velocityX": 0.5825666049201219,
+                    "velocityY": -0.3552259746242433,
+                    "timestamp": 0.9841021793163186
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": 0,
+                    "velocityX": -4.515906316961801e-34,
+                    "velocityY": 2.878377535752905e-33,
+                    "timestamp": 1.0661106942593452
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -1165,7 +4467,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W1 To S3": {
             "waypoints": [
@@ -1206,7 +4524,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W1 To S4": {
             "waypoints": [
@@ -1247,7 +4581,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W1 To C1": {
             "waypoints": [
@@ -1258,19 +4608,218 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 21
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
+                    "heading": -3.143185307179586,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "angularVelocity": -1.8545515236340358e-32,
+                    "velocityX": 2.346645959584709e-34,
+                    "velocityY": 1.5264694726605652e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.3666998117674303,
+                    "y": 6.765820939449683,
+                    "heading": -2.7737296132590643,
+                    "angularVelocity": -0.2635310200357621,
+                    "velocityX": 0.7407398190339184,
+                    "velocityY": 0.09796161216242502,
+                    "timestamp": 0.09004485792922705
+                },
+                {
+                    "x": 2.500184310330562,
+                    "y": 6.783480740643308,
+                    "heading": -2.817788411123353,
+                    "angularVelocity": -0.4892983217200214,
+                    "velocityX": 1.4824222241324077,
+                    "velocityY": 0.19612226172321479,
+                    "timestamp": 0.1800897158584541
+                },
+                {
+                    "x": 2.700568444033807,
+                    "y": 6.810009643084086,
+                    "heading": -2.8763096511057067,
+                    "angularVelocity": -0.6499120696969705,
+                    "velocityX": 2.2253811967890735,
+                    "velocityY": 0.294618738380691,
+                    "timestamp": 0.27013457378768113
+                },
+                {
+                    "x": 2.9679652961316036,
+                    "y": 6.845459467588075,
+                    "heading": -2.9368283914330156,
+                    "angularVelocity": -0.6720954612963594,
+                    "velocityX": 2.969596024105716,
+                    "velocityY": 0.3936907150417343,
+                    "timestamp": 0.3601794317169082
+                },
+                {
+                    "x": 3.3011724137612752,
+                    "y": 6.889825844077578,
+                    "heading": -2.9514563142993295,
+                    "angularVelocity": -0.1624515069790128,
+                    "velocityX": 3.7004569199449846,
+                    "velocityY": 0.49271415947342084,
+                    "timestamp": 0.45022428964613526
+                },
+                {
+                    "x": 3.638895175262513,
+                    "y": 6.93426783648319,
+                    "heading": -2.9514563336020103,
+                    "angularVelocity": -2.143673866142303e-7,
+                    "velocityX": 3.750605745490536,
+                    "velocityY": 0.49355391776554697,
+                    "timestamp": 0.5402691475753623
+                },
+                {
+                    "x": 3.976617946407746,
+                    "y": 6.9787097556023205,
+                    "heading": -2.9514563529046813,
+                    "angularVelocity": -2.1436727225160337e-7,
+                    "velocityX": 3.75060585259265,
+                    "velocityY": 0.4935531038769741,
+                    "timestamp": 0.6303140055045893
+                },
+                {
+                    "x": 4.314340727620448,
+                    "y": 7.023151598216748,
+                    "heading": -2.9514563722073515,
+                    "angularVelocity": -2.1436726735785024e-7,
+                    "velocityX": 3.7506059643976966,
+                    "velocityY": 0.49355225424817656,
+                    "timestamp": 0.7203588634338163
+                },
+                {
+                    "x": 4.65206351967383,
+                    "y": 7.06759335845053,
+                    "heading": -2.951456391510026,
+                    "angularVelocity": -2.1436731957684306e-7,
+                    "velocityX": 3.7506060847896903,
+                    "velocityY": 0.49355133936368123,
+                    "timestamp": 0.8104037213630433
+                },
+                {
+                    "x": 4.989786323250196,
+                    "y": 7.11203503111852,
+                    "heading": -2.9514564108127086,
+                    "angularVelocity": -2.143674043301644e-7,
+                    "velocityX": 3.7506062127590543,
+                    "velocityY": 0.4935503668951324,
+                    "timestamp": 0.9004485792922703
+                },
+                {
+                    "x": 5.327509138668733,
+                    "y": 7.15647661379494,
+                    "heading": -2.951456430115396,
+                    "angularVelocity": -2.1436746122645403e-7,
+                    "velocityX": 3.7506063442731943,
+                    "velocityY": 0.4935493674869138,
+                    "timestamp": 0.9904934372214973
+                },
+                {
+                    "x": 5.66523196589638,
+                    "y": 7.200918106730859,
+                    "heading": -2.95145644941808,
+                    "angularVelocity": -2.143674220103917e-7,
+                    "velocityX": 3.7506064754201436,
+                    "velocityY": 0.49354837086698544,
+                    "timestamp": 1.0805382951507243
+                },
+                {
+                    "x": 6.002954804577347,
+                    "y": 7.24535951262982,
+                    "heading": -2.9514564687207536,
+                    "angularVelocity": -2.1436730521441728e-7,
+                    "velocityX": 3.7506066026158704,
+                    "velocityY": 0.4935474042714428,
+                    "timestamp": 1.1705831530799513
+                },
+                {
+                    "x": 6.340677653913631,
+                    "y": 7.289800837555904,
+                    "heading": -2.95145648802342,
+                    "angularVelocity": -2.1436722572774826e-7,
+                    "velocityX": 3.750606720949308,
+                    "velocityY": 0.49354650502102115,
+                    "timestamp": 1.2606280110091783
+                },
+                {
+                    "x": 6.678400512367262,
+                    "y": 7.334242093196452,
+                    "heading": -2.9514565073260908,
+                    "angularVelocity": -2.1436727271528376e-7,
+                    "velocityX": 3.750606822202691,
+                    "velocityY": 0.4935457355652348,
+                    "timestamp": 1.3506728689384053
+                },
+                {
+                    "x": 7.0118090549698895,
+                    "y": 7.378580230758345,
+                    "heading": -2.965622818302132,
+                    "angularVelocity": -0.15732504111646,
+                    "velocityX": 3.7026938602610375,
+                    "velocityY": 0.4924005499208098,
+                    "timestamp": 1.4407177268676323
+                },
+                {
+                    "x": 7.2792751287326976,
+                    "y": 7.414016497420869,
+                    "heading": -3.0230864740546695,
+                    "angularVelocity": -0.6381669878107006,
+                    "velocityX": 2.9703647705572496,
+                    "velocityY": 0.39354014740493215,
+                    "timestamp": 1.5307625847968593
+                },
+                {
+                    "x": 7.479727754094387,
+                    "y": 7.440534879064414,
+                    "heading": -3.078696937244319,
+                    "angularVelocity": -0.617586217231393,
+                    "velocityX": 2.2261418360973004,
+                    "velocityY": 0.2945018988689779,
+                    "timestamp": 1.6208074427260863
+                },
+                {
+                    "x": 7.613268208977269,
+                    "y": 7.458185411749662,
+                    "heading": -3.120596791778004,
+                    "angularVelocity": -0.4653220127974119,
+                    "velocityX": 1.48304365128591,
+                    "velocityY": 0.1960193296003781,
+                    "timestamp": 1.7108523006553134
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -0.2508584712225979,
+                    "velocityX": 0.7410949670794122,
+                    "velocityY": 0.09789107843632346,
+                    "timestamp": 1.8008971585845404
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": 4.6252031231996675e-32,
+                    "velocityX": 0,
+                    "velocityY": -8.558401294468068e-35,
+                    "timestamp": 1.8909420165137674
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -1288,7 +4837,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W1 To C2": {
             "waypoints": [
@@ -1299,19 +4864,218 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 21
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
+                    "heading": -3.143185307179586,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "angularVelocity": 1.5124244158252309e-30,
+                    "velocityX": -1.0784945790349375e-32,
+                    "velocityY": 7.7332582255010576e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.368718302927459,
+                    "y": 6.754046521321464,
+                    "heading": -2.805815501863529,
+                    "angularVelocity": -0.6069900647837034,
+                    "velocityX": 0.7473072131063252,
+                    "velocityY": -0.0321188944749646,
+                    "timestamp": 0.09195455593398905
+                },
+                {
+                    "x": 2.506490387717156,
+                    "y": 6.748005444139933,
+                    "heading": -2.9118057011244183,
+                    "angularVelocity": -1.1526367365307737,
+                    "velocityX": 1.498262738483551,
+                    "velocityY": -0.06569633358752333,
+                    "timestamp": 0.1839091118679781
+                },
+                {
+                    "x": 2.7138594941009426,
+                    "y": 6.738708328412372,
+                    "heading": -3.056707583490325,
+                    "angularVelocity": -1.5757988377424839,
+                    "velocityX": 2.255125961704928,
+                    "velocityY": -0.10110554755149347,
+                    "timestamp": 0.27586366780196714
+                },
+                {
+                    "x": 2.99155716708356,
+                    "y": 6.725962728573371,
+                    "heading": -3.2144148322825616,
+                    "angularVelocity": -1.7150563905223961,
+                    "velocityX": 3.019944690744492,
+                    "velocityY": -0.13860759490970812,
+                    "timestamp": 0.3678182237359562
+                },
+                {
+                    "x": 3.331057359197656,
+                    "y": 6.709255401861718,
+                    "heading": -3.240411366891808,
+                    "angularVelocity": -0.28271067534607036,
+                    "velocityX": 3.6920431909628384,
+                    "velocityY": -0.1816911249470588,
+                    "timestamp": 0.45977277966994523
+                },
+                {
+                    "x": 3.6785771644429963,
+                    "y": 6.69390611896786,
+                    "heading": -3.2404114102003327,
+                    "angularVelocity": -4.7097747760964064e-7,
+                    "velocityX": 3.7792559782988104,
+                    "velocityY": -0.1669224840243556,
+                    "timestamp": 0.5517273356039343
+                },
+                {
+                    "x": 4.026096967074563,
+                    "y": 6.678556776895953,
+                    "heading": -3.2404114535088273,
+                    "angularVelocity": -4.70977144522105e-7,
+                    "velocityX": 3.779255949874186,
+                    "velocityY": -0.1669231275818996,
+                    "timestamp": 0.6436818915379233
+                },
+                {
+                    "x": 4.373616766444988,
+                    "y": 6.663207360989526,
+                    "heading": -3.240411496817302,
+                    "angularVelocity": -4.7097693251045375e-7,
+                    "velocityX": 3.779255914409469,
+                    "velocityY": -0.16692393052765972,
+                    "timestamp": 0.7356364474719124
+                },
+                {
+                    "x": 4.721136562002901,
+                    "y": 6.647857858765673,
+                    "heading": -3.240411540125758,
+                    "angularVelocity": -4.709767302327839e-7,
+                    "velocityX": 3.779255872948645,
+                    "velocityY": -0.1669248692242324,
+                    "timestamp": 0.8275910034059014
+                },
+                {
+                    "x": 5.068656352361164,
+                    "y": 6.632508238819668,
+                    "heading": -3.2404115834342044,
+                    "angularVelocity": -4.709766303037568e-7,
+                    "velocityX": 3.7792558164027836,
+                    "velocityY": -0.166926149445212,
+                    "timestamp": 0.9195455593398905
+                },
+                {
+                    "x": 5.4161761153722905,
+                    "y": 6.617157999739619,
+                    "heading": -3.2404116267428487,
+                    "angularVelocity": -4.709787784210011e-7,
+                    "velocityX": 3.7792555190043995,
+                    "velocityY": -0.16693288248891244,
+                    "timestamp": 1.0115001152738796
+                },
+                {
+                    "x": 5.762341737151603,
+                    "y": 6.582880068913748,
+                    "heading": -3.240411680913547,
+                    "angularVelocity": -5.891029283022056e-7,
+                    "velocityX": 3.7645293184582718,
+                    "velocityY": -0.37277033723568115,
+                    "timestamp": 1.1034546712078686
+                },
+                {
+                    "x": 6.108499177819837,
+                    "y": 6.548519619057327,
+                    "heading": -3.2404117350781716,
+                    "angularVelocity": -5.89036882514826e-7,
+                    "velocityX": 3.7644403493909295,
+                    "velocityY": -0.37366772649185126,
+                    "timestamp": 1.1954092271418575
+                },
+                {
+                    "x": 6.443025175700078,
+                    "y": 6.453136314569308,
+                    "heading": -3.2404117521954174,
+                    "angularVelocity": -1.8614896803626685e-7,
+                    "velocityX": 3.637949142186984,
+                    "velocityY": -1.0372874244152857,
+                    "timestamp": 1.2873637830758464
+                },
+                {
+                    "x": 6.751449154461275,
+                    "y": 6.292262864409884,
+                    "heading": -3.240411747251824,
+                    "angularVelocity": 5.3761265862029046e-8,
+                    "velocityX": 3.3540913294454215,
+                    "velocityY": -1.7494886308287982,
+                    "timestamp": 1.3793183390098354
+                },
+                {
+                    "x": 7.05837809298162,
+                    "y": 6.128555062623798,
+                    "heading": -3.240411742317762,
+                    "angularVelocity": 5.3657614685044897e-8,
+                    "velocityX": 3.337832861056693,
+                    "velocityY": -1.780312026123053,
+                    "timestamp": 1.4712728949438243
+                },
+                {
+                    "x": 7.307026842216431,
+                    "y": 5.995933066242413,
+                    "heading": -3.201520178002944,
+                    "angularVelocity": 0.422943310636366,
+                    "velocityX": 2.7040394759048985,
+                    "velocityY": -1.44225585164687,
+                    "timestamp": 1.5632274508778132
+                },
+                {
+                    "x": 7.493513419749655,
+                    "y": 5.8964665430996925,
+                    "heading": -3.172352554188821,
+                    "angularVelocity": 0.3171960705792624,
+                    "velocityX": 2.0280297766550737,
+                    "velocityY": -1.0816921699249264,
+                    "timestamp": 1.6551820068118022
+                },
+                {
+                    "x": 7.617837807165489,
+                    "y": 5.830155516567485,
+                    "heading": -3.1529077720147476,
+                    "angularVelocity": 0.21146078056243686,
+                    "velocityX": 1.3520198771345535,
+                    "velocityY": -0.721128234035621,
+                    "timestamp": 1.747136562745791
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": 0.10573119228742708,
+                    "velocityX": 0.6760099290690327,
+                    "velocityY": -0.3605641529201271,
+                    "timestamp": 1.83909111867978
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": 3.5257869003902484e-31,
+                    "velocityX": 0,
+                    "velocityY": -2.4831219594454356e-32,
+                    "timestamp": 1.931045674613769
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -1329,7 +5093,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W1 To C3": {
             "waypoints": [
@@ -1370,7 +5150,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W1 To C4": {
             "waypoints": [
@@ -1411,7 +5207,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W1 To C5": {
             "waypoints": [
@@ -1452,7 +5264,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W2 To W1": {
             "waypoints": [
@@ -1463,25 +5291,125 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 10
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 3.5331853071795862,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
-            "constraints": [
+            "trajectory": [
                 {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 7.733955963828693e-33,
+                    "velocityY": 1.1033116642945717e-34,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.2520020198561768,
+                    "y": 5.607878032622388,
+                    "heading": 3.1558282209608954,
+                    "angularVelocity": 0.2083481053076329,
+                    "velocityX": 0.026352743296497748,
+                    "velocityY": 0.6302222724446542,
+                    "timestamp": 0.07597007391799751
+                },
+                {
+                    "x": 2.2560055614843093,
+                    "y": 5.703634205053828,
+                    "heading": 3.187479734547443,
+                    "angularVelocity": 0.41663133855460127,
+                    "velocityX": 0.052698930271593884,
+                    "velocityY": 1.2604459557957022,
+                    "timestamp": 0.15194014783599502
+                },
+                {
+                    "x": 2.2620099218556464,
+                    "y": 5.847268779856728,
+                    "heading": 3.234941916913622,
+                    "angularVelocity": 0.6247484031331855,
+                    "velocityX": 0.07903586322448838,
+                    "velocityY": 1.8906730952761694,
+                    "timestamp": 0.22791022175399253
+                },
+                {
+                    "x": 2.270014272624616,
+                    "y": 6.038782285632903,
+                    "heading": 3.2981885303415055,
+                    "angularVelocity": 0.8325200985871338,
+                    "velocityX": 0.10536189260009911,
+                    "velocityY": 2.520907192783506,
+                    "timestamp": 0.30388029567199004
+                },
+                {
+                    "x": 2.2800095231252646,
+                    "y": 6.278187238938093,
+                    "heading": 3.376569786131263,
+                    "angularVelocity": 1.0317385747756829,
+                    "velocityX": 0.13156826083172354,
+                    "velocityY": 3.151306046688924,
+                    "timestamp": 0.3798503695899875
+                },
+                {
+                    "x": 2.2880056735413383,
+                    "y": 6.469712014906391,
+                    "heading": 3.4392336964848362,
+                    "angularVelocity": 0.8248499326354866,
+                    "velocityX": 0.10525395071624018,
+                    "velocityY": 2.5210555432002106,
+                    "timestamp": 0.455820443507985
+                },
+                {
+                    "x": 2.2940028218748654,
+                    "y": 6.613355955414726,
+                    "heading": 3.486212731686583,
+                    "angularVelocity": 0.6183886993772851,
+                    "velocityX": 0.07894093060907224,
+                    "velocityY": 1.8907963767862817,
+                    "timestamp": 0.5317905174259825
+                },
+                {
+                    "x": 2.29800094256259,
+                    "y": 6.709118670546653,
+                    "heading": 3.517527025119772,
+                    "angularVelocity": 0.412192483411159,
+                    "velocityX": 0.052627574010790594,
+                    "velocityY": 1.2605320778717877,
+                    "timestamp": 0.60776059134398
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 3.5331853071795862,
+                    "angularVelocity": 0.20611118631681047,
+                    "velocityX": 0.026313748747538647,
+                    "velocityY": 0.6302656688873288,
+                    "timestamp": 0.6837306652619775
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 3.5331853071795862,
+                    "angularVelocity": -1.7034359069625615e-32,
+                    "velocityX": 1.3795062534340913e-33,
+                    "velocityY": 0,
+                    "timestamp": 0.7597007391799749
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
                 {
                     "scope": [
@@ -1493,7 +5421,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W2 To W3": {
             "waypoints": [
@@ -1504,7 +5448,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 10
                 },
                 {
                     "x": 2.3,
@@ -1516,7 +5460,107 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": -1.4697340738263578e-34,
+                    "velocityY": 1.0017538611600725e-35,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.2520018980434946,
+                    "y": 5.512001884856013,
+                    "heading": 3.124302281529173,
+                    "angularVelocity": -0.20638131373671467,
+                    "velocityX": 0.026319388320742283,
+                    "velocityY": -0.6310416433261579,
+                    "timestamp": 0.07606172374138878
+                },
+                {
+                    "x": 2.2560052212232096,
+                    "y": 5.416005549224728,
+                    "heading": 3.092911780798619,
+                    "angularVelocity": -0.4126977300341266,
+                    "velocityX": 0.05263255922684882,
+                    "velocityY": -1.2620846716237228,
+                    "timestamp": 0.15212344748277756
+                },
+                {
+                    "x": 2.262009303064208,
+                    "y": 5.272010738006526,
+                    "heading": 3.045840934307484,
+                    "angularVelocity": -0.618850640976494,
+                    "velocityX": 0.07893696784222709,
+                    "velocityY": -1.8931310537713693,
+                    "timestamp": 0.22818517122416634
+                },
+                {
+                    "x": 2.2700133593824035,
+                    "y": 5.080016941067703,
+                    "heading": 2.9831153695364567,
+                    "angularVelocity": -0.8246666218648364,
+                    "velocityX": 0.10523106661912582,
+                    "velocityY": -2.524184142757597,
+                    "timestamp": 0.3042468949655551
+                },
+                {
+                    "x": 2.2800088926734445,
+                    "y": 4.840012206333479,
+                    "heading": 2.90536283495199,
+                    "angularVelocity": -1.0222294573395003,
+                    "velocityX": 0.1314134468609495,
+                    "velocityY": -3.1553943682665504,
+                    "timestamp": 0.3803086187069439
+                },
+                {
+                    "x": 2.288005283531118,
+                    "y": 4.648007638478312,
+                    "heading": 2.8432005737453654,
+                    "angularVelocity": -0.8172607475736078,
+                    "velocityX": 0.10513028714497022,
+                    "velocityY": -2.524325750333843,
+                    "timestamp": 0.45637034244833263
+                },
+                {
+                    "x": 2.2940026200964168,
+                    "y": 4.504003869225784,
+                    "heading": 2.7965971516354546,
+                    "angularVelocity": -0.6127053111281423,
+                    "velocityX": 0.07884828623776054,
+                    "velocityY": -1.8932488269940233,
+                    "timestamp": 0.5324320661897214
+                },
+                {
+                    "x": 2.2980008723577208,
+                    "y": 4.408001271885206,
+                    "heading": 2.7655331089053763,
+                    "angularVelocity": -0.40840571580650337,
+                    "velocityX": 0.052565890761270766,
+                    "velocityY": -1.262166995675615,
+                    "timestamp": 0.6084937899311101
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": -0.20421715603224325,
+                    "velocityX": 0.026282965254326718,
+                    "velocityY": -0.6310831456885047,
+                    "timestamp": 0.6845555136724989
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 2.533367966722586e-34,
+                    "velocityY": 7.359145603780692e-32,
+                    "timestamp": 0.7606172374138876
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -1534,7 +5578,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W2 To S1": {
             "waypoints": [
@@ -1545,12 +5605,12 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 15
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": -2.88,
+                    "heading": 3.4031853071795863,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -1575,7 +5635,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W2 To S2": {
             "waypoints": [
@@ -1586,7 +5662,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 10
                 },
                 {
                     "x": 4.263,
@@ -1598,7 +5674,107 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 1.3281175985971763e-33,
+                    "velocityX": 1.9314152785727527e-39,
+                    "velocityY": 8.278387842337126e-35,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.3319036557469475,
+                    "y": 5.560000044140862,
+                    "heading": 3.140000009517521,
+                    "angularVelocity": 9.602993340973177e-8,
+                    "velocityX": 0.8263919813281884,
+                    "velocityY": 4.4537273180015373e-7,
+                    "timestamp": 0.09910993523353247
+                },
+                {
+                    "x": 2.495710964163716,
+                    "y": 5.5600001253696005,
+                    "heading": 3.140000023792121,
+                    "angularVelocity": 1.44027928091657e-7,
+                    "velocityX": 1.6527839316087705,
+                    "velocityY": 8.195821841849307e-7,
+                    "timestamp": 0.19821987046706493
+                },
+                {
+                    "x": 2.741421919096052,
+                    "y": 5.5600002360460925,
+                    "heading": 3.140000035820423,
+                    "angularVelocity": 1.213632154532524e-7,
+                    "velocityX": 2.479175819794127,
+                    "velocityY": 0.0000011167042923727,
+                    "timestamp": 0.2973298057005974
+                },
+                {
+                    "x": 3.0690365020812136,
+                    "y": 5.5600003677528065,
+                    "heading": 3.140000035237491,
+                    "angularVelocity": -5.88169262513392e-9,
+                    "velocityX": 3.305567521694012,
+                    "velocityY": 0.0000013288951255827482,
+                    "timestamp": 0.39643974093412987
+                },
+                {
+                    "x": 3.443963497918667,
+                    "y": 5.560000377607592,
+                    "heading": 3.140000035237483,
+                    "angularVelocity": -8.264095134625168e-14,
+                    "velocityX": 3.782940579610041,
+                    "velocityY": 9.943285507786426e-8,
+                    "timestamp": 0.4955496761676623
+                },
+                {
+                    "x": 3.771578080903882,
+                    "y": 5.560000246492901,
+                    "heading": 3.1399999920753214,
+                    "angularVelocity": -4.354978004755772e-7,
+                    "velocityX": 3.305567521694553,
+                    "velocityY": -0.0000013229217178930416,
+                    "timestamp": 0.5946596114011948
+                },
+                {
+                    "x": 4.017289035836251,
+                    "y": 5.560000133124366,
+                    "heading": 3.1399999814016817,
+                    "angularVelocity": -1.0769493378117993e-7,
+                    "velocityX": 2.4791758197944644,
+                    "velocityY": -0.000001143866475595614,
+                    "timestamp": 0.6937695466347273
+                },
+                {
+                    "x": 4.181096344253041,
+                    "y": 5.560000047628769,
+                    "heading": 3.1399999890121935,
+                    "angularVelocity": 7.678859424954975e-8,
+                    "velocityX": 1.6527839316089916,
+                    "velocityY": -8.626339566508034e-7,
+                    "timestamp": 0.7928794818682598
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 1.1086483608499845e-7,
+                    "velocityX": 0.8263919813282989,
+                    "velocityY": -4.805650223133441e-7,
+                    "timestamp": 0.8919894171017924
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 2.938735877055719e-39,
+                    "velocityY": -3.0539806085263666e-33,
+                    "timestamp": 0.9910993523353249
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -1616,7 +5792,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W2 To S3": {
             "waypoints": [
@@ -1657,7 +5849,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W2 To S4": {
             "waypoints": [
@@ -1698,7 +5906,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W2 To C1": {
             "waypoints": [
@@ -1709,7 +5933,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 20
                 },
                 {
                     "x": 7.68,
@@ -1721,7 +5945,197 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 3.909956769253128e-30,
+                    "velocityX": -2.302876892990051e-31,
+                    "velocityY": 1.6352823102584027e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.326889522840126,
+                    "y": 5.587767009829938,
+                    "heading": 3.1412161105492817,
+                    "angularVelocity": 0.012281818163829663,
+                    "velocityX": 0.7765273796780119,
+                    "velocityY": 0.2804262868111325,
+                    "timestamp": 0.0990171433131028
+                },
+                {
+                    "x": 2.4806685686648633,
+                    "y": 5.6433010199081535,
+                    "heading": 3.143648327345736,
+                    "angularVelocity": 0.024563592879525555,
+                    "velocityX": 1.5530547608152194,
+                    "velocityY": 0.560852476854545,
+                    "timestamp": 0.1980342866262056
+                },
+                {
+                    "x": 2.711337134772754,
+                    "y": 5.72660201934395,
+                    "heading": 3.147296644122033,
+                    "angularVelocity": 0.03684530429983614,
+                    "velocityX": 2.3295821146697007,
+                    "velocityY": 0.8412785569099963,
+                    "timestamp": 0.2970514299393084
+                },
+                {
+                    "x": 3.0188952067968695,
+                    "y": 5.837669992778753,
+                    "heading": 3.1521610500975985,
+                    "angularVelocity": 0.04912690684429555,
+                    "velocityX": 3.106109323428804,
+                    "velocityY": 1.1217044818551853,
+                    "timestamp": 0.3960685732524112
+                },
+                {
+                    "x": 3.3712021943007997,
+                    "y": 5.964897910508787,
+                    "heading": 3.1521610506223987,
+                    "angularVelocity": 5.300094570798509e-9,
+                    "velocityX": 3.558040312169958,
+                    "velocityY": 1.2849079813151831,
+                    "timestamp": 0.495085716565514
+                },
+                {
+                    "x": 3.7235092123405478,
+                    "y": 6.092125743682096,
+                    "heading": 3.1521610511472007,
+                    "angularVelocity": 5.300112966968611e-9,
+                    "velocityX": 3.558040620559158,
+                    "velocityY": 1.2849071273547275,
+                    "timestamp": 0.5941028598786168
+                },
+                {
+                    "x": 4.075816261929694,
+                    "y": 6.219353489491835,
+                    "heading": 3.1521610516720138,
+                    "angularVelocity": 5.300218907643243e-9,
+                    "velocityX": 3.5580409391847763,
+                    "velocityY": 1.284906245047206,
+                    "timestamp": 0.6931200031917195
+                },
+                {
+                    "x": 4.428123345781717,
+                    "y": 6.346581140424019,
+                    "heading": 3.152161052196838,
+                    "angularVelocity": 5.300337345645622e-9,
+                    "velocityX": 3.558041285214527,
+                    "velocityY": 1.284905286853995,
+                    "timestamp": 0.7921371465048224
+                },
+                {
+                    "x": 4.780430481129625,
+                    "y": 6.473808648758581,
+                    "heading": 3.152161052721672,
+                    "angularVelocity": 5.300431772031768e-9,
+                    "velocityX": 3.558041805284913,
+                    "velocityY": 1.2849038467233598,
+                    "timestamp": 0.8911542898179252
+                },
+                {
+                    "x": 5.134794229593029,
+                    "y": 6.595190238737374,
+                    "heading": 3.1521610521993333,
+                    "angularVelocity": -5.2752356524836585e-9,
+                    "velocityX": 3.5788120784587116,
+                    "velocityY": 1.225864389916352,
+                    "timestamp": 0.9901714331310281
+                },
+                {
+                    "x": 5.489158282910728,
+                    "y": 6.716570938689376,
+                    "heading": 3.152161051676551,
+                    "angularVelocity": -5.279713305479767e-9,
+                    "velocityX": 3.578815157261821,
+                    "velocityY": 1.225855401303425,
+                    "timestamp": 1.089188576444131
+                },
+                {
+                    "x": 5.843522381964551,
+                    "y": 6.837951505117218,
+                    "heading": 3.1521610511537586,
+                    "angularVelocity": -5.279818233241214e-9,
+                    "velocityX": 3.5788156191628895,
+                    "velocityY": 1.2258540528080475,
+                    "timestamp": 1.1882057197572338
+                },
+                {
+                    "x": 6.19788651637458,
+                    "y": 6.959331968324431,
+                    "heading": 3.1521610506309807,
+                    "angularVelocity": -5.279669747527449e-9,
+                    "velocityX": 3.5788159762344476,
+                    "velocityY": 1.2258530103559488,
+                    "timestamp": 1.2872228630703366
+                },
+                {
+                    "x": 6.5522506780354695,
+                    "y": 7.080712351974079,
+                    "heading": 3.1521610501082176,
+                    "angularVelocity": -5.2795212284316414e-9,
+                    "velocityX": 3.5788162514480155,
+                    "velocityY": 1.2258522068833249,
+                    "timestamp": 1.3862400063834395
+                },
+                {
+                    "x": 6.9066148601448845,
+                    "y": 7.202092675925132,
+                    "heading": 3.1521610495854597,
+                    "angularVelocity": -5.279467664817609e-9,
+                    "velocityX": 3.5788164579630073,
+                    "velocityY": 1.2258516039716054,
+                    "timestamp": 1.4852571496965423
+                },
+                {
+                    "x": 7.215968896352917,
+                    "y": 7.308055607481538,
+                    "heading": 3.147296630745593,
+                    "angularVelocity": -0.049127036764239344,
+                    "velocityX": 3.1242472349441757,
+                    "velocityY": 1.0701473301581856,
+                    "timestamp": 1.5842742930096452
+                },
+                {
+                    "x": 7.447984442479348,
+                    "y": 7.3875278059692215,
+                    "heading": 3.1436483149588383,
+                    "angularVelocity": -0.0368452943061847,
+                    "velocityX": 2.3431856177952266,
+                    "velocityY": 0.8026104958045841,
+                    "timestamp": 1.683291436322748
+                },
+                {
+                    "x": 7.6026614794307905,
+                    "y": 7.440509269529631,
+                    "heading": 3.141216104773647,
+                    "angularVelocity": -0.024563526110654314,
+                    "velocityX": 1.5621238078171693,
+                    "velocityY": 0.5350736426810189,
+                    "timestamp": 1.7823085796358509
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "angularVelocity": -0.012281759834182485,
+                    "velocityX": 0.7810619250512634,
+                    "velocityY": 0.2675368081121253,
+                    "timestamp": 1.8813257229489537
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "angularVelocity": 1.8685058153122784e-28,
+                    "velocityX": 0,
+                    "velocityY": -1.820396114239373e-31,
+                    "timestamp": 1.9803428662620566
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -1739,7 +6153,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W2 To C2": {
             "waypoints": [
@@ -1750,7 +6180,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 19
                 },
                 {
                     "x": 7.68,
@@ -1780,7 +6210,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W2 To C3": {
             "waypoints": [
@@ -1821,7 +6267,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W2 To C4": {
             "waypoints": [
@@ -1862,7 +6324,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W2 To C5": {
             "waypoints": [
@@ -1903,7 +6381,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W3 To W1": {
             "waypoints": [
@@ -1914,19 +6408,164 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 15
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 3.5331853071795862,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 1.6975195374214185e-34,
+                    "velocityY": -4.870070626129674e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.300139974954616,
+                    "y": 4.403824386953961,
+                    "heading": 2.777167813185812,
+                    "angularVelocity": 0.371999186817317,
+                    "velocityX": 0.0019166271843757909,
+                    "velocityY": 0.6000717171507387,
+                    "timestamp": 0.07303191552177725
+                },
+                {
+                    "x": 2.3004163368424435,
+                    "y": 4.4915620593167,
+                    "heading": 2.8296003827219796,
+                    "angularVelocity": 0.7179404943929278,
+                    "velocityX": 0.003784124869971189,
+                    "velocityY": 1.2013606891712643,
+                    "timestamp": 0.1460638310435545
+                },
+                {
+                    "x": 2.3008287718098006,
+                    "y": 4.623340237169809,
+                    "heading": 2.9042527526949042,
+                    "angularVelocity": 1.022188305476718,
+                    "velocityX": 0.005647325068915905,
+                    "velocityY": 1.8043916404440152,
+                    "timestamp": 0.21909574656533176
+                },
+                {
+                    "x": 2.30139145489797,
+                    "y": 4.799346158732557,
+                    "heading": 2.995610111682823,
+                    "angularVelocity": 1.2509237685362016,
+                    "velocityX": 0.007704619057970502,
+                    "velocityY": 2.409986377945481,
+                    "timestamp": 0.292127662087109
+                },
+                {
+                    "x": 2.3021599471353418,
+                    "y": 5.019805776345521,
+                    "heading": 3.0915564646710374,
+                    "angularVelocity": 1.3137592284513517,
+                    "velocityX": 0.01052269041392973,
+                    "velocityY": 3.0186750003459215,
+                    "timestamp": 0.36515957760888623
+                },
+                {
+                    "x": 2.303349941373319,
+                    "y": 5.282617464741668,
+                    "heading": 3.1370629336033304,
+                    "angularVelocity": 0.6231038664010262,
+                    "velocityX": 0.0162941671387857,
+                    "velocityY": 3.598586816715496,
+                    "timestamp": 0.43819149313066347
+                },
+                {
+                    "x": 2.30367850771832,
+                    "y": 5.558892653761466,
+                    "heading": 3.1370629627902624,
+                    "angularVelocity": 3.996462650884406e-7,
+                    "velocityX": 0.004498941903047958,
+                    "velocityY": 3.7829377340844195,
+                    "timestamp": 0.5112234086524408
+                },
+                {
+                    "x": 2.304007097232836,
+                    "y": 5.835167842753699,
+                    "heading": 3.137062991977204,
+                    "angularVelocity": 3.996463893760175e-7,
+                    "velocityX": 0.004499259154964363,
+                    "velocityY": 3.7829377337070014,
+                    "timestamp": 0.584255324174218
+                },
+                {
+                    "x": 2.3020927945039733,
+                    "y": 6.097639248519623,
+                    "heading": 3.182327539673564,
+                    "angularVelocity": 0.6197913251072079,
+                    "velocityX": -0.026211865253511206,
+                    "velocityY": 3.5939274478930847,
+                    "timestamp": 0.6572872396959952
+                },
+                {
+                    "x": 2.3012287765145363,
+                    "y": 6.317987502538012,
+                    "heading": 3.2805685838685283,
+                    "angularVelocity": 1.3451796176107471,
+                    "velocityX": -0.011830690503794171,
+                    "velocityY": 3.0171501383211403,
+                    "timestamp": 0.7303191552177725
+                },
+                {
+                    "x": 2.3006859363348666,
+                    "y": 6.493881460544365,
+                    "heading": 3.3743823922179033,
+                    "angularVelocity": 1.284559054478048,
+                    "velocityX": -0.007432917181360783,
+                    "velocityY": 2.40845330085727,
+                    "timestamp": 0.8033510707395497
+                },
+                {
+                    "x": 2.3003278852317357,
+                    "y": 6.625559358254098,
+                    "heading": 3.451128438683188,
+                    "angularVelocity": 1.050856271767924,
+                    "velocityX": -0.004902666191527756,
+                    "velocityY": 1.80301854016781,
+                    "timestamp": 0.8763829862613269
+                },
+                {
+                    "x": 2.300106073487947,
+                    "y": 6.713219846040187,
+                    "heading": 3.5051273882405054,
+                    "angularVelocity": 0.739388377964907,
+                    "velocityX": -0.0030371891823418365,
+                    "velocityY": 1.2003038282618885,
+                    "timestamp": 0.9494149017831042
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 3.5331853071795862,
+                    "angularVelocity": 0.3841870877769111,
+                    "velocityX": -0.0014524264794266268,
+                    "velocityY": 0.5994660505208633,
+                    "timestamp": 1.0224468173048815
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 3.5331853071795862,
+                    "angularVelocity": -9.351204897899337e-33,
+                    "velocityX": 1.378221818744695e-30,
+                    "velocityY": 0,
+                    "timestamp": 1.0954787328266589
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -1944,7 +6583,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W3 To W2": {
             "waypoints": [
@@ -1955,7 +6610,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 10
                 },
                 {
                     "x": 2.25,
@@ -1967,7 +6622,107 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 0,
+                    "velocityX": -7.080178799155261e-34,
+                    "velocityY": 4.018324582530293e-34,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.298001228420339,
+                    "y": 4.408001448490065,
+                    "heading": 2.765522898073599,
+                    "angularVelocity": 0.20408324361211075,
+                    "velocityX": -0.026278326719846846,
+                    "velocityY": 0.6310864929657463,
+                    "timestamp": 0.07606160015322963
+                },
+                {
+                    "x": 2.294003714931991,
+                    "y": 4.50400409446738,
+                    "heading": 2.7965834623359793,
+                    "angularVelocity": 0.40836064715713705,
+                    "velocityX": -0.052556263348324604,
+                    "velocityY": 1.2621696859376152,
+                    "timestamp": 0.15212320030645926
+                },
+                {
+                    "x": 2.288007388147497,
+                    "y": 4.648007477823444,
+                    "heading": 2.8432069040547985,
+                    "angularVelocity": 0.6129695092516301,
+                    "velocityX": -0.07883513852474495,
+                    "velocityY": 1.8932468297532885,
+                    "timestamp": 0.2281848004596889
+                },
+                {
+                    "x": 2.2800119392615206,
+                    "y": 4.840010948654735,
+                    "heading": 2.9054273803096926,
+                    "angularVelocity": 0.8180274426195046,
+                    "velocityX": -0.10511807363859851,
+                    "velocityY": 2.5243154291323253,
+                    "timestamp": 0.3042464006129185
+                },
+                {
+                    "x": 2.270016643226651,
+                    "y": 5.0800137105269405,
+                    "heading": 2.98328446819424,
+                    "angularVelocity": 1.0236057054768744,
+                    "velocityX": -0.13141054112368558,
+                    "velocityY": 3.155373557599477,
+                    "timestamp": 0.3803080007661481
+                },
+                {
+                    "x": 2.262010502624602,
+                    "y": 5.272008371939251,
+                    "heading": 3.045963904195738,
+                    "angularVelocity": 0.8240614958826565,
+                    "velocityX": -0.10525864018007076,
+                    "velocityY": 2.524199609599701,
+                    "timestamp": 0.4563696009193777
+                },
+                {
+                    "x": 2.2560053812396474,
+                    "y": 5.416004211679494,
+                    "heading": 3.092980743508332,
+                    "angularVelocity": 0.6181416012531585,
+                    "velocityX": -0.07895076323476027,
+                    "velocityY": 1.8931476520367212,
+                    "timestamp": 0.5324312010726073
+                },
+                {
+                    "x": 2.2520017956431593,
+                    "y": 5.512001402686665,
+                    "heading": 3.124326930482601,
+                    "angularVelocity": 0.412115797079227,
+                    "velocityX": -0.05263609480240691,
+                    "velocityY": 1.2620979681439777,
+                    "timestamp": 0.6084928012258369
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0.20605758340377522,
+                    "velocityX": -0.026318084803984385,
+                    "velocityY": 0.6310490078651869,
+                    "timestamp": 0.6845544013790665
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": -1.7962813810508635e-31,
+                    "velocityX": -5.242761228396242e-32,
+                    "velocityY": 0,
+                    "timestamp": 0.7606160015322961
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -1985,7 +6740,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W3 To S1": {
             "waypoints": [
@@ -1996,19 +6767,191 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 18
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": -2.88,
+                    "heading": 3.4031853071795863,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": -1.3064230645361168e-30,
+                    "velocityX": 1.1529790698064783e-31,
+                    "velocityY": -2.8761439718242387e-32,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.322180658668465,
+                    "y": 4.411221384877813,
+                    "heading": 2.7352853365602194,
+                    "angularVelocity": -0.1796018847298344,
+                    "velocityX": 0.2707291347647005,
+                    "velocityY": 0.6251897843383167,
+                    "timestamp": 0.08192933755631272
+                },
+                {
+                    "x": 2.368773081487876,
+                    "y": 4.512701566380937,
+                    "heading": 2.7089075651304313,
+                    "angularVelocity": -0.32195758218669895,
+                    "velocityX": 0.5686903398600793,
+                    "velocityY": 1.238630562994284,
+                    "timestamp": 0.16385867511262545
+                },
+                {
+                    "x": 2.4433012157913003,
+                    "y": 4.662644128727173,
+                    "heading": 2.675827445796147,
+                    "angularVelocity": -0.4037640278922902,
+                    "velocityX": 0.9096635774968738,
+                    "velocityY": 1.8301449373147467,
+                    "timestamp": 0.24578801266893818
+                },
+                {
+                    "x": 2.5519618748459716,
+                    "y": 4.856937153180571,
+                    "heading": 2.6453653517833016,
+                    "angularVelocity": -0.3718093532967731,
+                    "velocityX": 1.3262728870471487,
+                    "velocityY": 2.371470711817401,
+                    "timestamp": 0.3277173502252509
+                },
+                {
+                    "x": 2.7016003249644123,
+                    "y": 5.089209181124471,
+                    "heading": 2.6267184772011043,
+                    "angularVelocity": -0.22759703835491757,
+                    "velocityX": 1.8264330529413755,
+                    "velocityY": 2.835028756143084,
+                    "timestamp": 0.4096466877815636
+                },
+                {
+                    "x": 2.9069631472288275,
+                    "y": 5.3213406432232695,
+                    "heading": 2.6267188101864813,
+                    "angularVelocity": 0.00000406429963372636,
+                    "velocityX": 2.5065846788186557,
+                    "velocityY": 2.833313060040874,
+                    "timestamp": 0.4915760253378763
+                },
+                {
+                    "x": 3.1507251830977756,
+                    "y": 5.512753871430069,
+                    "heading": 2.626718909792111,
+                    "angularVelocity": 0.0000012157504557439165,
+                    "velocityX": 2.975271656522333,
+                    "velocityY": 2.3363209555458044,
+                    "timestamp": 0.5735053628941891
+                },
+                {
+                    "x": 3.4078762515345984,
+                    "y": 5.685763359739643,
+                    "heading": 2.6267190032534695,
+                    "angularVelocity": 0.0000011407556990966525,
+                    "velocityX": 3.1386933680511517,
+                    "velocityY": 2.1116915316282108,
+                    "timestamp": 0.6554347004505018
+                },
+                {
+                    "x": 3.665027465532842,
+                    "y": 5.858772631695159,
+                    "heading": 2.626719096714767,
+                    "angularVelocity": 0.0000011407549466335633,
+                    "velocityX": 3.1386951447214493,
+                    "velocityY": 2.111688890888465,
+                    "timestamp": 0.7373640380068145
+                },
+                {
+                    "x": 3.922178727707041,
+                    "y": 6.031781832044647,
+                    "heading": 2.6267191901760296,
+                    "angularVelocity": 0.0000011407545266438845,
+                    "velocityX": 3.138695732739822,
+                    "velocityY": 2.1116880168910543,
+                    "timestamp": 0.8192933755631272
+                },
+                {
+                    "x": 4.179330140012749,
+                    "y": 6.204790809246775,
+                    "heading": 2.6267192836372875,
+                    "angularVelocity": 0.0000011407544682834688,
+                    "velocityX": 3.1386975651909763,
+                    "velocityY": 2.111685293234714,
+                    "timestamp": 0.9012227131194399
+                },
+                {
+                    "x": 4.443624087689148,
+                    "y": 6.354175617474429,
+                    "heading": 2.642647782574834,
+                    "angularVelocity": 0.1944175238399595,
+                    "velocityX": 3.225876780643333,
+                    "velocityY": 1.823337191332418,
+                    "timestamp": 0.9831520506757526
+                },
+                {
+                    "x": 4.674751705763445,
+                    "y": 6.469415592948305,
+                    "heading": 2.769232553851288,
+                    "angularVelocity": 1.5450481482222196,
+                    "velocityX": 2.821060501257377,
+                    "velocityY": 1.4065776547341065,
+                    "timestamp": 1.0650813882320653
+                },
+                {
+                    "x": 4.870834347570915,
+                    "y": 6.549065174445882,
+                    "heading": 2.9849459088839683,
+                    "angularVelocity": 2.6329195556404166,
+                    "velocityX": 2.39331413698659,
+                    "velocityY": 0.9721741182495338,
+                    "timestamp": 1.1470107257883781
+                },
+                {
+                    "x": 5.022947063703052,
+                    "y": 6.5945122449585005,
+                    "heading": 3.181337324435562,
+                    "angularVelocity": 2.397082918150139,
+                    "velocityX": 1.856633053179344,
+                    "velocityY": 0.5547105819253249,
+                    "timestamp": 1.228940063344691
+                },
+                {
+                    "x": 5.124965104320653,
+                    "y": 6.619330120195266,
+                    "heading": 3.326379958974982,
+                    "angularVelocity": 1.7703381824578692,
+                    "velocityX": 1.2451954777185934,
+                    "velocityY": 0.3029180508106365,
+                    "timestamp": 1.3108694009010038
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": 0.93745843058738,
+                    "velocityX": 0.6229135643171693,
+                    "velocityY": 0.13023271178533075,
+                    "timestamp": 1.3927987384573166
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": -5.432893499766539e-33,
+                    "velocityX": 0,
+                    "velocityY": 6.79272724985994e-33,
+                    "timestamp": 1.4747280760136294
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -2026,7 +6969,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W3 To S2": {
             "waypoints": [
@@ -2037,7 +6996,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 13
                 },
                 {
                     "x": 4.263,
@@ -2049,7 +7008,134 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 0,
+                    "velocityX": -1.3306255005182603e-31,
+                    "velocityY": -4.9176166375307755e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.322374290257025,
+                    "y": 4.418269020876138,
+                    "heading": 2.7110352663465282,
+                    "angularVelocity": -0.44706114894751375,
+                    "velocityX": 0.25671100431863964,
+                    "velocityY": 0.6685485303865802,
+                    "timestamp": 0.08715750349857801
+                },
+                {
+                    "x": 2.3748782855305732,
+                    "y": 4.5313296521256,
+                    "heading": 2.6355865457570187,
+                    "angularVelocity": -0.8656594964395732,
+                    "velocityX": 0.6024036160513037,
+                    "velocityY": 1.297199055859919,
+                    "timestamp": 0.17431500699715602
+                },
+                {
+                    "x": 2.469236718208719,
+                    "y": 4.691158857574848,
+                    "heading": 2.529430325019821,
+                    "angularVelocity": -1.2179814298941005,
+                    "velocityX": 1.0826197273959968,
+                    "velocityY": 1.8337974245884079,
+                    "timestamp": 0.26147251049573406
+                },
+                {
+                    "x": 2.6203114191501853,
+                    "y": 4.878748173786945,
+                    "heading": 2.4055559201698435,
+                    "angularVelocity": -1.421270686717166,
+                    "velocityX": 1.7333527794761936,
+                    "velocityY": 2.1523025405971743,
+                    "timestamp": 0.34863001399431204
+                },
+                {
+                    "x": 2.832356831160347,
+                    "y": 5.08316808870822,
+                    "heading": 2.2936588957805637,
+                    "angularVelocity": -1.2838484341294274,
+                    "velocityX": 2.4328991021825375,
+                    "velocityY": 2.345408102752848,
+                    "timestamp": 0.43578751749289
+                },
+                {
+                    "x": 3.098575041354179,
+                    "y": 5.2672072069550175,
+                    "heading": 2.274563036259226,
+                    "angularVelocity": -0.2190959900732964,
+                    "velocityX": 3.054449697474129,
+                    "velocityY": 2.1115694100829816,
+                    "timestamp": 0.522945020991468
+                },
+                {
+                    "x": 3.397459622153223,
+                    "y": 5.3975878258937575,
+                    "heading": 2.2845714271399826,
+                    "angularVelocity": 0.11483108715843302,
+                    "velocityX": 3.429246694794569,
+                    "velocityY": 1.495919613402737,
+                    "timestamp": 0.610102524490046
+                },
+                {
+                    "x": 3.6781811959965687,
+                    "y": 5.472346031669266,
+                    "heading": 2.382872565425927,
+                    "angularVelocity": 1.1278562870671076,
+                    "velocityX": 3.220853771332781,
+                    "velocityY": 0.8577368875271776,
+                    "timestamp": 0.697260027988624
+                },
+                {
+                    "x": 3.9150228109281926,
+                    "y": 5.514569108720165,
+                    "heading": 2.601398031866265,
+                    "angularVelocity": 2.5072478865104606,
+                    "velocityX": 2.717397876540693,
+                    "velocityY": 0.484445691489864,
+                    "timestamp": 0.784417531487202
+                },
+                {
+                    "x": 4.091172675855641,
+                    "y": 5.53970946369802,
+                    "heading": 2.8415186627639684,
+                    "angularVelocity": 2.7550196054160887,
+                    "velocityX": 2.0210522084346136,
+                    "velocityY": 0.28844739659465746,
+                    "timestamp": 0.8715750349857799
+                },
+                {
+                    "x": 4.206403236830247,
+                    "y": 5.5538325492768035,
+                    "heading": 3.0331865406048246,
+                    "angularVelocity": 2.1990978418052483,
+                    "velocityX": 1.322095704318632,
+                    "velocityY": 0.16204096046663827,
+                    "timestamp": 0.9587325384843579
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 1.2255222454474992,
+                    "velocityX": 0.649361912605446,
+                    "velocityY": 0.0707621314933191,
+                    "timestamp": 1.045890041982936
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 2.336425691005573e-30,
+                    "velocityX": -1.4618072975561068e-30,
+                    "velocityY": 3.4366383722351896e-30,
+                    "timestamp": 1.133047545481514
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -2067,7 +7153,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W3 To S3": {
             "waypoints": [
@@ -2078,7 +7180,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 12
                 },
                 {
                     "x": 4.263,
@@ -2090,7 +7192,125 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": -2.857623496877536e-30,
+                    "velocityX": -2.5600502945380354e-30,
+                    "velocityY": 7.096305433334873e-32,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.2589562715242293,
+                    "y": 4.250539211803029,
+                    "heading": 2.836221335933998,
+                    "angularVelocity": 0.7208664352087386,
+                    "velocityX": -0.3431522593974162,
+                    "velocityY": -0.9151633679526462,
+                    "timestamp": 0.1196079214161022
+                },
+                {
+                    "x": 2.2003066234183892,
+                    "y": 4.024396154786088,
+                    "heading": 2.9853349315252267,
+                    "angularVelocity": 1.2466866226394515,
+                    "velocityX": -0.49034919603525046,
+                    "velocityY": -1.8907030097966573,
+                    "timestamp": 0.2392158428322044
+                },
+                {
+                    "x": 2.1964987876971427,
+                    "y": 3.692388997223788,
+                    "heading": 3.120971884356915,
+                    "angularVelocity": 1.1340131257679336,
+                    "velocityX": -0.03183598273568803,
+                    "velocityY": -2.7757957301782654,
+                    "timestamp": 0.3588237642483066
+                },
+                {
+                    "x": 2.306992116857372,
+                    "y": 3.3594977499513337,
+                    "heading": 3.1303317506759742,
+                    "angularVelocity": 0.07825456883065958,
+                    "velocityX": 0.9237960818316977,
+                    "velocityY": -2.7831872950485823,
+                    "timestamp": 0.4784316856644088
+                },
+                {
+                    "x": 2.522086295400282,
+                    "y": 3.073270003946497,
+                    "heading": 3.020615571964542,
+                    "angularVelocity": -0.9172985987240034,
+                    "velocityX": 1.7983272010439872,
+                    "velocityY": -2.39305008076411,
+                    "timestamp": 0.598039607080511
+                },
+                {
+                    "x": 2.8292729817758957,
+                    "y": 2.8628580598282483,
+                    "heading": 2.911598621970614,
+                    "angularVelocity": -0.9114525919611488,
+                    "velocityX": 2.568280451149536,
+                    "velocityY": -1.7591806765559896,
+                    "timestamp": 0.7176475284966132
+                },
+                {
+                    "x": 3.212139121131616,
+                    "y": 2.7446490517974147,
+                    "heading": 2.8046662811978735,
+                    "angularVelocity": -0.8940239033224252,
+                    "velocityX": 3.2010098898363166,
+                    "velocityY": -0.9883041744332437,
+                    "timestamp": 0.8372554499127154
+                },
+                {
+                    "x": 3.61962025055875,
+                    "y": 2.7431499033487023,
+                    "heading": 2.702792215888405,
+                    "angularVelocity": -0.8517334312273422,
+                    "velocityX": 3.406807213124051,
+                    "velocityY": -0.012533855876376997,
+                    "timestamp": 0.9568633713288176
+                },
+                {
+                    "x": 3.9493476486905155,
+                    "y": 2.8320906325242436,
+                    "heading": 2.608128750394254,
+                    "angularVelocity": -0.7914481279620891,
+                    "velocityX": 2.7567354588888784,
+                    "velocityY": 0.7436023310372396,
+                    "timestamp": 1.0764712927449198
+                },
+                {
+                    "x": 4.162172137163363,
+                    "y": 2.936272219295665,
+                    "heading": 2.5785368758755864,
+                    "angularVelocity": -0.2474073135651704,
+                    "velocityX": 1.7793511161560736,
+                    "velocityY": 0.8710258111499355,
+                    "timestamp": 1.196079214161022
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": -0.037931232495828804,
+                    "velocityX": 0.842986498242598,
+                    "velocityY": 0.5328056866955708,
+                    "timestamp": 1.315687135577124
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": 0,
+                    "velocityX": 1.3483931579260095e-31,
+                    "velocityY": -3.3355137434500586e-29,
+                    "timestamp": 1.4352950569932261
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -2108,7 +7328,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W3 To S4": {
             "waypoints": [
@@ -2119,7 +7355,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 17
                 },
                 {
                     "x": 5.176,
@@ -2131,7 +7367,170 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": -2.979591758162217e-32,
+                    "velocityX": -5.0524169564327655e-33,
+                    "velocityY": 3.4190943066430706e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.2859321803383907,
+                    "y": 4.28632531036449,
+                    "heading": 2.8392348343623115,
+                    "angularVelocity": 0.9181215337942725,
+                    "velocityX": -0.14474132503475426,
+                    "velocityY": -0.7580259383384993,
+                    "timestamp": 0.09719283458425766
+                },
+                {
+                    "x": 2.269559639424535,
+                    "y": 4.136180116377401,
+                    "heading": 2.9969853519762393,
+                    "angularVelocity": 1.623067361793753,
+                    "velocityX": -0.1684541971009417,
+                    "velocityY": -1.5448175231161345,
+                    "timestamp": 0.19438566916851532
+                },
+                {
+                    "x": 2.2724010855339243,
+                    "y": 3.9101733835281753,
+                    "heading": 3.1867879122348075,
+                    "angularVelocity": 1.952845197595569,
+                    "velocityX": 0.029235139828396976,
+                    "velocityY": -2.325343568957208,
+                    "timestamp": 0.291578503752773
+                },
+                {
+                    "x": 2.3226715061490353,
+                    "y": 3.6216059722814986,
+                    "heading": 3.3538116791494788,
+                    "angularVelocity": 1.7184781947055583,
+                    "velocityX": 0.5172235260978133,
+                    "velocityY": -2.969019398199714,
+                    "timestamp": 0.38877133833703065
+                },
+                {
+                    "x": 2.437426080371791,
+                    "y": 3.296473518840458,
+                    "heading": 3.427293312254084,
+                    "angularVelocity": 0.7560396136085858,
+                    "velocityX": 1.1806896538579041,
+                    "velocityY": -3.3452306935155747,
+                    "timestamp": 0.4859641729212883
+                },
+                {
+                    "x": 2.6276283995685685,
+                    "y": 2.9818185735083116,
+                    "heading": 3.4272931426736677,
+                    "angularVelocity": -0.000001744783111288276,
+                    "velocityX": 1.9569582470803324,
+                    "velocityY": -3.2374294532933714,
+                    "timestamp": 0.583157007505546
+                },
+                {
+                    "x": 2.8804858808256992,
+                    "y": 2.7148958271590775,
+                    "heading": 3.4272929522201836,
+                    "angularVelocity": -0.00000195954243555236,
+                    "velocityX": 2.6016062021313475,
+                    "velocityY": -2.7463212436492483,
+                    "timestamp": 0.6803498420898036
+                },
+                {
+                    "x": 3.1843940400703103,
+                    "y": 2.5079549973523347,
+                    "heading": 3.4272928289705424,
+                    "angularVelocity": -0.0000012680939050558275,
+                    "velocityX": 3.126857659256241,
+                    "velocityY": -2.1291778420902356,
+                    "timestamp": 0.7775426766740613
+                },
+                {
+                    "x": 3.5145691798617946,
+                    "y": 2.3461864714628624,
+                    "heading": 3.4272927145634307,
+                    "angularVelocity": -0.0000011771146716870812,
+                    "velocityX": 3.397114007465763,
+                    "velocityY": -1.66440794304886,
+                    "timestamp": 0.8747355112583189
+                },
+                {
+                    "x": 3.844744525773579,
+                    "y": 2.1844183662726744,
+                    "heading": 3.427292600156351,
+                    "angularVelocity": -0.0000011771143448425205,
+                    "velocityX": 3.397116128201317,
+                    "velocityY": -1.6644036145478303,
+                    "timestamp": 0.9719283458425766
+                },
+                {
+                    "x": 4.174919952487512,
+                    "y": 2.022650426003138,
+                    "heading": 3.42729248574922,
+                    "angularVelocity": -0.0000011771148700868944,
+                    "velocityX": 3.3971169595604263,
+                    "velocityY": -1.6644019177082188,
+                    "timestamp": 1.0691211804268343
+                },
+                {
+                    "x": 4.503263696345804,
+                    "y": 1.8851757396047895,
+                    "heading": 3.3983831121261034,
+                    "angularVelocity": -0.29744346634992885,
+                    "velocityX": 3.3782710964525586,
+                    "velocityY": -1.4144528965164656,
+                    "timestamp": 1.166314015011092
+                },
+                {
+                    "x": 4.772302673813676,
+                    "y": 1.7735532891774939,
+                    "heading": 3.2030632550848326,
+                    "angularVelocity": -2.009611694902728,
+                    "velocityX": 2.7680947738450667,
+                    "velocityY": -1.1484637823843746,
+                    "timestamp": 1.2635068495953496
+                },
+                {
+                    "x": 4.977265937404966,
+                    "y": 1.6951772631841453,
+                    "heading": 2.8890004427677543,
+                    "angularVelocity": -3.2313371007284832,
+                    "velocityX": 2.1088310107223527,
+                    "velocityY": -0.8063971621838382,
+                    "timestamp": 1.3606996841796073
+                },
+                {
+                    "x": 5.110692645903806,
+                    "y": 1.6448171116124384,
+                    "heading": 2.6382240127177106,
+                    "angularVelocity": -2.5801946318649924,
+                    "velocityX": 1.3728039630655127,
+                    "velocityY": -0.5181467521461082,
+                    "timestamp": 1.457892518763865
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": -1.4221625833731872,
+                    "velocityX": 0.6719358929652187,
+                    "velocityY": -0.2553389014590782,
+                    "timestamp": 1.5550853533481226
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": 5.001833480552254e-32,
+                    "velocityX": 0,
+                    "velocityY": -3.22512482003026e-32,
+                    "timestamp": 1.6522781879323802
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -2149,7 +7548,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W3 To C1": {
             "waypoints": [
@@ -2160,7 +7575,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 23
                 },
                 {
                     "x": 7.68,
@@ -2190,7 +7605,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W3 To C2": {
             "waypoints": [
@@ -2231,7 +7662,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W3 To C3": {
             "waypoints": [
@@ -2272,7 +7719,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W3 To C4": {
             "waypoints": [
@@ -2313,7 +7776,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "W3 To C5": {
             "waypoints": [
@@ -2354,7 +7833,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S1 To W1": {
             "waypoints": [
@@ -2365,7 +7860,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 13
                 },
                 {
                     "x": 2.3,
@@ -2377,7 +7872,134 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "angularVelocity": -6.0746648103992426e-33,
+                    "velocityX": -3.2850296304780664e-35,
+                    "velocityY": -2.0665663458795258e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.102979054151253,
+                    "y": 6.633224554548809,
+                    "heading": -2.8734987043090925,
+                    "angularVelocity": 0.06942752469656709,
+                    "velocityX": -0.7797927924375683,
+                    "velocityY": 0.03443511127885569,
+                    "timestamp": 0.09364147316685267
+                },
+                {
+                    "x": 4.9569371637209425,
+                    "y": 6.639673650012151,
+                    "heading": -2.8604963348705175,
+                    "angularVelocity": 0.1388526792548496,
+                    "velocityX": -1.5595855713428277,
+                    "velocityY": 0.0688700769568818,
+                    "timestamp": 0.18728294633370535
+                },
+                {
+                    "x": 4.737874332155748,
+                    "y": 6.649347273486247,
+                    "heading": -2.8409932661051047,
+                    "angularVelocity": 0.2082738353622199,
+                    "velocityX": -2.3393783134408976,
+                    "velocityY": 0.10330490483508797,
+                    "timestamp": 0.280924419500558
+                },
+                {
+                    "x": 4.445790573832511,
+                    "y": 6.662245412808618,
+                    "heading": -2.8149900541710062,
+                    "angularVelocity": 0.27768905224050994,
+                    "velocityX": -3.119170902008226,
+                    "velocityY": 0.13773960282991304,
+                    "timestamp": 0.3745658926674107
+                },
+                {
+                    "x": 4.091895329085733,
+                    "y": 6.677873079084158,
+                    "heading": -2.814990051178798,
+                    "angularVelocity": 3.195386469208516e-8,
+                    "velocityX": -3.7792575530737516,
+                    "velocityY": 0.16688830009804687,
+                    "timestamp": 0.46820736583426337
+                },
+                {
+                    "x": 3.738000080960507,
+                    "y": 6.693500668821502,
+                    "heading": -2.8149900481866714,
+                    "angularVelocity": 3.195300400483476e-8,
+                    "velocityX": -3.7792575891522513,
+                    "velocityY": 0.16688748274491266,
+                    "timestamp": 0.561848839001116
+                },
+                {
+                    "x": 3.384104829826614,
+                    "y": 6.709128190425974,
+                    "heading": -2.8149900451945484,
+                    "angularVelocity": 3.1952965397811645e-8,
+                    "velocityX": -3.7792576212818934,
+                    "velocityY": 0.1668867551519713,
+                    "timestamp": 0.6554903121679687
+                },
+                {
+                    "x": 3.0302095759842613,
+                    "y": 6.724755650696981,
+                    "heading": -2.8149900422022327,
+                    "angularVelocity": 3.195501290053148e-8,
+                    "velocityX": -3.779257650205636,
+                    "velocityY": 0.16688610017037783,
+                    "timestamp": 0.7491317853348214
+                },
+                {
+                    "x": 2.738125764107778,
+                    "y": 6.7376533993595,
+                    "heading": -2.7889938425825695,
+                    "angularVelocity": 0.2776141675314341,
+                    "velocityX": -3.1191714739049314,
+                    "velocityY": 0.1377354309613996,
+                    "timestamp": 0.8427732585016741
+                },
+                {
+                    "x": 2.519062886855627,
+                    "y": 6.747326704296012,
+                    "heading": -2.7694968873701145,
+                    "angularVelocity": 0.20820854855310947,
+                    "velocityX": -2.339378801333246,
+                    "velocityY": 0.10330150316261463,
+                    "timestamp": 0.9364147316685267
+                },
+                {
+                    "x": 2.3730209632759087,
+                    "y": 6.753775569660814,
+                    "heading": -2.756498972823295,
+                    "angularVelocity": 0.13880510533677407,
+                    "velocityX": -1.559585925346323,
+                    "velocityY": 0.0688676197276826,
+                    "timestamp": 1.0300562048353794
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "angularVelocity": 0.06940271872607656,
+                    "velocityX": -0.779792978542731,
+                    "velocityY": 0.034433784840577714,
+                    "timestamp": 1.123697678002232
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "angularVelocity": 0,
+                    "velocityX": -2.0910816279231796e-36,
+                    "velocityY": 1.251790530664498e-31,
+                    "timestamp": 1.2173391511690848
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -2395,7 +8017,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S1 To W2": {
             "waypoints": [
@@ -2406,19 +8044,164 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 15
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": -3.143185307179586,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "angularVelocity": 0,
+                    "velocityX": -8.07520618943781e-35,
+                    "velocityY": -1.6093142036511268e-34,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.118924952955758,
+                    "y": 6.609128496946765,
+                    "heading": -2.888779485010293,
+                    "angularVelocity": -0.10279588776958423,
+                    "velocityX": -0.6682715584712982,
+                    "velocityY": -0.24437705434061835,
+                    "timestamp": 0.08540696715389907
+                },
+                {
+                    "x": 5.004774858765426,
+                    "y": 6.567385488245384,
+                    "heading": -2.906338067516999,
+                    "angularVelocity": -0.20558723827608577,
+                    "velocityX": -1.3365431181350638,
+                    "velocityY": -0.48875413906409965,
+                    "timestamp": 0.17081393430779815
+                },
+                {
+                    "x": 4.833549713525512,
+                    "y": 6.504770974662745,
+                    "heading": -2.9326749312069125,
+                    "angularVelocity": -0.3083690308596943,
+                    "velocityX": -2.004814723503466,
+                    "velocityY": -0.7331312148084055,
+                    "timestamp": 0.25622090146169724
+                },
+                {
+                    "x": 4.6052495100619275,
+                    "y": 6.421284959128349,
+                    "heading": -2.96778862782457,
+                    "angularVelocity": -0.4111338663318194,
+                    "velocityX": -2.673086412870736,
+                    "velocityY": -0.9775082562521819,
+                    "timestamp": 0.3416278686155963
+                },
+                {
+                    "x": 4.319874242645404,
+                    "y": 6.316927450773692,
+                    "heading": -3.011676691839701,
+                    "angularVelocity": -0.513869833781204,
+                    "velocityX": -3.3413581693199683,
+                    "velocityY": -1.2218851907785282,
+                    "timestamp": 0.42703483576949536
+                },
+                {
+                    "x": 4.016437344871394,
+                    "y": 6.205964133135793,
+                    "heading": -3.0116766970908038,
+                    "angularVelocity": -6.148330724628331e-8,
+                    "velocityX": -3.5528354171297467,
+                    "velocityY": -1.2992302775246547,
+                    "timestamp": 0.5124418029233945
+                },
+                {
+                    "x": 3.7130004778936962,
+                    "y": 6.095000731283241,
+                    "heading": -3.011676702341878,
+                    "angularVelocity": -6.148297185895772e-8,
+                    "velocityX": -3.552835056546619,
+                    "velocityY": -1.299231263564264,
+                    "timestamp": 0.5978487700772935
+                },
+                {
+                    "x": 3.4095636397411098,
+                    "y": 5.984037250606526,
+                    "heading": -3.011676707592942,
+                    "angularVelocity": -6.148285239694146e-8,
+                    "velocityX": -3.5528347190435716,
+                    "velocityY": -1.2992321864885386,
+                    "timestamp": 0.6832557372311926
+                },
+                {
+                    "x": 3.106126829490984,
+                    "y": 5.873073693628802,
+                    "heading": -3.0116767128440194,
+                    "angularVelocity": -6.14830072681285e-8,
+                    "velocityX": -3.5528343923435184,
+                    "velocityY": -1.2992330798700915,
+                    "timestamp": 0.7686627043850917
+                },
+                {
+                    "x": 2.8207512525311835,
+                    "y": 5.768715824967853,
+                    "heading": -3.055515171895212,
+                    "angularVelocity": -0.5132890267862834,
+                    "velocityX": -3.3413617936528297,
+                    "velocityY": -1.221889409477577,
+                    "timestamp": 0.8540696715389907
+                },
+                {
+                    "x": 2.5924507605120093,
+                    "y": 5.68522951062146,
+                    "heading": -3.090584080219249,
+                    "angularVelocity": -0.4106094560276795,
+                    "velocityX": -2.6730897914661687,
+                    "velocityY": -0.9775117549362862,
+                    "timestamp": 0.9394766386928898
+                },
+                {
+                    "x": 2.421225380941837,
+                    "y": 5.6226147624914855,
+                    "heading": -3.116884817520286,
+                    "angularVelocity": -0.3079460397375414,
+                    "velocityX": -2.004817467193667,
+                    "velocityY": -0.7331339610402823,
+                    "timestamp": 1.024883605846789
+                },
+                {
+                    "x": 2.307075126087855,
+                    "y": 5.580871589665102,
+                    "heading": -3.1344184120174683,
+                    "angularVelocity": -0.20529466250203854,
+                    "velocityX": -1.3365449992889762,
+                    "velocityY": -0.48875606074577516,
+                    "timestamp": 1.110290573000688
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -0.10264847768590626,
+                    "velocityX": -0.6682724839650215,
+                    "velocityY": -0.24437806844836182,
+                    "timestamp": 1.195697540154587
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": 0,
+                    "velocityX": -3.560921302709756e-34,
+                    "velocityY": 7.627797313302701e-34,
+                    "timestamp": 1.2811045073084861
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -2436,7 +8219,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S1 To W3": {
             "waypoints": [
@@ -2447,12 +8246,12 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 18
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -3.5331853071795862,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -2477,7 +8276,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S1 To C1": {
             "waypoints": [
@@ -2488,29 +8303,156 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 13
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
+                    "heading": -3.143185307179586,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
-            "constraints": [
+            "trajectory": [
                 {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "angularVelocity": 0,
+                    "velocityX": 1.8796456462447601e-29,
+                    "velocityY": -4.3119341309057906e-30,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.238209828475023,
+                    "y": 6.650800952122015,
+                    "heading": -2.892998872920493,
+                    "angularVelocity": -0.1464435777830739,
+                    "velocityX": 0.7008476743058382,
+                    "velocityY": 0.23434076697564932,
+                    "timestamp": 0.08876369396038813
+                },
+                {
+                    "x": 5.362652804152687,
+                    "y": 6.692404857453675,
+                    "heading": -2.917433036424788,
+                    "angularVelocity": -0.27527204439886777,
+                    "velocityX": 1.401958054303432,
+                    "velocityY": 0.46870407793850294,
+                    "timestamp": 0.17752738792077627
+                },
+                {
+                    "x": 5.5493620218671325,
+                    "y": 6.7548129941209,
+                    "heading": -2.950667481161702,
+                    "angularVelocity": -0.37441484524641305,
+                    "velocityX": 2.1034412763117816,
+                    "velocityY": 0.70308178809801,
+                    "timestamp": 0.2662910818811644
+                },
+                {
+                    "x": 5.798380867010647,
+                    "y": 6.838019600580292,
+                    "heading": -2.987306905639368,
+                    "angularVelocity": -0.4127748952720924,
+                    "velocityX": 2.8054132723935825,
+                    "velocityY": 0.9373945894737359,
+                    "timestamp": 0.35505477584155254
+                },
+                {
+                    "x": 6.109622963136531,
+                    "y": 6.941929172833481,
+                    "heading": -3.009821564076527,
+                    "angularVelocity": -0.25364715496791174,
+                    "velocityX": 3.5064121628854164,
+                    "velocityY": 1.1706314554748125,
+                    "timestamp": 0.4438184698019407
+                },
+                {
+                    "x": 6.428045089115211,
+                    "y": 7.048516124767363,
+                    "heading": -3.0098215751251205,
+                    "angularVelocity": -1.2447253578662016e-7,
+                    "velocityX": 3.5873014266485415,
+                    "velocityY": 1.2007944597359943,
+                    "timestamp": 0.5325821637623288
+                },
+                {
+                    "x": 6.746467236003722,
+                    "y": 7.1551030142348395,
+                    "heading": -3.0098215861735884,
+                    "angularVelocity": -1.2447113137335796e-7,
+                    "velocityX": 3.5873016622160976,
+                    "velocityY": 1.2007937559974857,
+                    "timestamp": 0.6213458577227169
+                },
+                {
+                    "x": 7.057671617899985,
+                    "y": 7.2589999980659305,
+                    "heading": -3.0321320878151,
+                    "angularVelocity": -0.2513471515963816,
+                    "velocityX": 3.5059872793849554,
+                    "velocityY": 1.1704896359626165,
+                    "timestamp": 0.7101095516831051
+                },
+                {
+                    "x": 7.306674739642628,
+                    "y": 7.342201198643625,
+                    "heading": -3.06987813605104,
+                    "angularVelocity": -0.42524197169930134,
+                    "velocityX": 2.8052361346509134,
+                    "velocityY": 0.9373336875121538,
+                    "timestamp": 0.7988732456434933
+                },
+                {
+                    "x": 7.493368024895104,
+                    "y": 7.4046034302940065,
+                    "heading": -3.1043079780358913,
+                    "angularVelocity": -0.3878820320283419,
+                    "velocityX": 2.1032617833134357,
+                    "velocityY": 0.7030152629492495,
+                    "timestamp": 0.8876369396038815
+                },
+                {
+                    "x": 7.617797798391618,
+                    "y": 7.446202168700309,
+                    "heading": -3.129675012430027,
+                    "angularVelocity": -0.28578164406378687,
+                    "velocityX": 1.4018093202853217,
+                    "velocityY": 0.4686458680340631,
+                    "timestamp": 0.9764006335642696
                 },
                 {
-                    "scope": [
-                        "last"
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -0.15220518825005472,
+                    "velocityX": 0.7007617510398297,
+                    "velocityY": 0.23430560820061222,
+                    "timestamp": 1.0651643275246578
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": 0,
+                    "velocityX": 1.9101182509980996e-31,
+                    "velocityY": -4.716397884209056e-30,
+                    "timestamp": 1.153928021485046
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
                     ],
                     "type": "StopPoint"
                 }
@@ -2518,7 +8460,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S1 To C2": {
             "waypoints": [
@@ -2529,19 +8487,146 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 13
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
+                    "heading": -3.143185307179586,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": -2.88,
+                    "angularVelocity": 0,
+                    "velocityX": -2.1431055300877995e-28,
+                    "velocityY": 1.1784110457685481e-29,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.242060097443899,
+                    "y": 6.625920772567306,
+                    "heading": -2.9233556616628746,
+                    "angularVelocity": -0.48269065593589344,
+                    "velocityX": 0.7354654627191669,
+                    "velocityY": -0.04541517508160964,
+                    "timestamp": 0.08982080164534181
+                },
+                {
+                    "x": 5.3743817426165945,
+                    "y": 6.616908903759451,
+                    "heading": -3.004544486756046,
+                    "angularVelocity": -0.9038978010210977,
+                    "velocityX": 1.4731737275644603,
+                    "velocityY": -0.1003316452622891,
+                    "timestamp": 0.17964160329068363
+                },
+                {
+                    "x": 5.573213116332803,
+                    "y": 6.601503149315071,
+                    "heading": -3.1137348179578126,
+                    "angularVelocity": -1.215646367006541,
+                    "velocityX": 2.2136450585388348,
+                    "velocityY": -0.17151655476435285,
+                    "timestamp": 0.26946240493602547
+                },
+                {
+                    "x": 5.83860703608144,
+                    "y": 6.576562673459042,
+                    "heading": -3.229878025812627,
+                    "angularVelocity": -1.2930546791757207,
+                    "velocityX": 2.954704421327105,
+                    "velocityY": -0.2776692636802449,
+                    "timestamp": 0.35928320658136725
+                },
+                {
+                    "x": 6.164587325578701,
+                    "y": 6.531858569137778,
+                    "heading": -3.2661345564867124,
+                    "angularVelocity": -0.40365405351480704,
+                    "velocityX": 3.629229349170066,
+                    "velocityY": -0.49770324359594226,
+                    "timestamp": 0.44910400822670904
+                },
+                {
+                    "x": 6.48571912004567,
+                    "y": 6.420820709470225,
+                    "heading": -3.2661345579017715,
+                    "angularVelocity": -1.5754250259377596e-8,
+                    "velocityX": 3.5752497036818096,
+                    "velocityY": -1.2362154159565475,
+                    "timestamp": 0.5389248098720508
+                },
+                {
+                    "x": 6.786864453228811,
+                    "y": 6.263446876676794,
+                    "heading": -3.2661345453374624,
+                    "angularVelocity": 1.3988195638330222e-7,
+                    "velocityX": 3.3527348639373855,
+                    "velocityY": -1.7520867094330927,
+                    "timestamp": 0.6287456115173926
+                },
+                {
+                    "x": 7.084475527096153,
+                    "y": 6.108061043338325,
+                    "heading": -3.255293907795362,
+                    "angularVelocity": 0.12069183689660036,
+                    "velocityX": 3.313386970675918,
+                    "velocityY": -1.7299537578389734,
+                    "timestamp": 0.7185664131627344
+                },
+                {
+                    "x": 7.322762050188167,
+                    "y": 5.983606942670793,
+                    "heading": -3.219406300389853,
+                    "angularVelocity": 0.3995467280198762,
+                    "velocityX": 2.652910224881898,
+                    "velocityY": -1.385582163460734,
+                    "timestamp": 0.8083872148080762
+                },
+                {
+                    "x": 7.5014175228899544,
+                    "y": 5.890287602383411,
+                    "heading": -3.184260886361704,
+                    "angularVelocity": 0.39128368244731204,
+                    "velocityX": 1.9890211335143717,
+                    "velocityY": -1.0389502050521982,
+                    "timestamp": 0.898208016453418
+                },
+                {
+                    "x": 7.620482912490659,
+                    "y": 5.8280911603292145,
+                    "heading": -3.1576094170182762,
+                    "angularVelocity": 0.296718230690803,
+                    "velocityX": 1.325588142386374,
+                    "velocityY": -0.6924503112294588,
+                    "timestamp": 0.9880288180987598
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": 0.16058763197915146,
+                    "velocityX": 0.6626203108757032,
+                    "velocityY": -0.3461465468987679,
+                    "timestamp": 1.0778496197441017
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": 2.6110530197350993e-29,
+                    "velocityX": -1.9135813261242845e-30,
+                    "velocityY": -4.114109248291899e-30,
+                    "timestamp": 1.1676704213894435
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -2559,7 +8644,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S1 To C3": {
             "waypoints": [
@@ -2570,12 +8671,12 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 16
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
+                    "heading": -3.143185307179586,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -2600,7 +8701,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S1 To C4": {
             "waypoints": [
@@ -2611,12 +8728,12 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 19
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
+                    "heading": -3.143185307179586,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -2641,7 +8758,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S1 To C5": {
             "waypoints": [
@@ -2682,7 +8815,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S2 To W1": {
             "waypoints": [
@@ -2693,19 +8842,146 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 13
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 3.5331853071795862,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": -7.11323283877368e-33,
+                    "velocityX": -2.26183607094562e-34,
+                    "velocityY": 8.143994372388985e-34,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.215224482174133,
+                    "y": 5.589131482897013,
+                    "heading": 3.153116118672446,
+                    "angularVelocity": 0.15993604804856262,
+                    "velocityX": -0.5825677325255438,
+                    "velocityY": 0.35522507570249656,
+                    "timestamp": 0.0820085204835332
+                },
+                {
+                    "x": 4.119673447940142,
+                    "y": 5.647394549238196,
+                    "heading": 3.179344081068732,
+                    "angularVelocity": 0.3198199679940867,
+                    "velocityX": -1.1651354477633493,
+                    "velocityY": 0.7104513774624363,
+                    "timestamp": 0.1640170409670664
+                },
+                {
+                    "x": 3.976346906814042,
+                    "y": 5.734789369483009,
+                    "heading": 3.218677503841872,
+                    "angularVelocity": 0.47962605033262895,
+                    "velocityX": -1.7477030469642283,
+                    "velocityY": 1.0656797577803132,
+                    "timestamp": 0.24602556145059962
+                },
+                {
+                    "x": 3.785244904014577,
+                    "y": 5.851316186750812,
+                    "heading": 3.271109373786434,
+                    "angularVelocity": 0.6393466146617031,
+                    "velocityX": -2.3302700947743262,
+                    "velocityY": 1.4209111026603802,
+                    "timestamp": 0.3280340819341328
+                },
+                {
+                    "x": 3.5463675589434582,
+                    "y": 5.996975305481239,
+                    "heading": 3.3366342258406214,
+                    "angularVelocity": 0.7990005388201682,
+                    "velocityX": -2.9128356866172687,
+                    "velocityY": 1.7761461598331643,
+                    "timestamp": 0.410042602417666
+                },
+                {
+                    "x": 3.2814945861155147,
+                    "y": 6.158490139171684,
+                    "heading": 3.3366342341249666,
+                    "angularVelocity": 1.010181030485091e-7,
+                    "velocityX": -3.2298225997276515,
+                    "velocityY": 1.9694884475190098,
+                    "timestamp": 0.4920511229011992
+                },
+                {
+                    "x": 3.0166215989993184,
+                    "y": 6.32000494943038,
+                    "heading": 3.336634242409321,
+                    "angularVelocity": 1.0101821933668142e-7,
+                    "velocityX": -3.2298227739565295,
+                    "velocityY": 1.969488161795665,
+                    "timestamp": 0.5740596433847324
+                },
+                {
+                    "x": 2.7777475013239767,
+                    "y": 6.46566975957478,
+                    "heading": 3.402143020545178,
+                    "angularVelocity": 0.7988045357922341,
+                    "velocityX": -2.9127960883443285,
+                    "velocityY": 1.776215560109378,
+                    "timestamp": 0.6560681638682656
+                },
+                {
+                    "x": 2.5866484088099964,
+                    "y": 6.582201778902395,
+                    "heading": 3.4545564874050534,
+                    "angularVelocity": 0.6391222101171739,
+                    "velocityX": -2.3302346071753846,
+                    "velocityY": 1.4209745358229475,
+                    "timestamp": 0.7380766843517989
+                },
+                {
+                    "x": 2.443324185159995,
+                    "y": 6.669600869933244,
+                    "heading": 3.493870163524806,
+                    "angularVelocity": 0.47938526250630853,
+                    "velocityX": -1.7476747879969323,
+                    "velocityY": 1.0657318351255574,
+                    "timestamp": 0.8200852048353321
+                },
+                {
+                    "x": 2.347774731346008,
+                    "y": 6.727866955511231,
+                    "heading": 3.5200803945158796,
+                    "angularVelocity": 0.3196037538116112,
+                    "velocityX": -1.1651161763511244,
+                    "velocityY": 0.710488193597954,
+                    "timestamp": 0.9020937253188653
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 3.5331853071795862,
+                    "angularVelocity": 0.1597994036038963,
+                    "velocityX": -0.5825581423042643,
+                    "velocityY": 0.35524411752579316,
+                    "timestamp": 0.9841022458023986
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 3.5331853071795862,
+                    "angularVelocity": 0,
+                    "velocityX": -5.266786892384168e-34,
+                    "velocityY": -2.2308589105860002e-32,
+                    "timestamp": 1.0661107662859317
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -2723,7 +8999,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S2 To W2": {
             "waypoints": [
@@ -2734,7 +9026,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 10
                 },
                 {
                     "x": 2.25,
@@ -2746,7 +9038,107 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 9.713847301484863e-36,
+                    "velocityY": -1.0645735433948216e-32,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.181096344253042,
+                    "y": 5.560000047628424,
+                    "heading": 3.139999989014019,
+                    "angularVelocity": -1.10846415392014e-7,
+                    "velocityX": -0.826391981328299,
+                    "velocityY": 4.805615505681296e-7,
+                    "timestamp": 0.09910993523353247
+                },
+                {
+                    "x": 4.017289035836252,
+                    "y": 5.560000133123364,
+                    "heading": 3.139999981407119,
+                    "angularVelocity": -7.675215101799687e-8,
+                    "velocityX": -1.6527839316089914,
+                    "velocityY": 8.626273272073532e-7,
+                    "timestamp": 0.19821987046706493
+                },
+                {
+                    "x": 3.7715780809038826,
+                    "y": 5.560000246490977,
+                    "heading": 3.1399999920859667,
+                    "angularVelocity": 1.0774748441307597e-7,
+                    "velocityX": -2.4791758197944644,
+                    "velocityY": 0.0000011438571635435649,
+                    "timestamp": 0.2973298057005974
+                },
+                {
+                    "x": 3.4439634979186673,
+                    "y": 5.56000037760455,
+                    "heading": 3.1400000352544852,
+                    "angularVelocity": 4.355619377078705e-7,
+                    "velocityX": -3.305567521694553,
+                    "velocityY": 0.0000013229104502120412,
+                    "timestamp": 0.39643974093412987
+                },
+                {
+                    "x": 3.069036502081214,
+                    "y": 5.560000367749523,
+                    "heading": 3.140000035254493,
+                    "angularVelocity": 8.147365661806906e-14,
+                    "velocityX": -3.782940579610041,
+                    "velocityY": -9.943529898465916e-8,
+                    "timestamp": 0.4955496761676623
+                },
+                {
+                    "x": 2.741421919096052,
+                    "y": 5.560000236043854,
+                    "heading": 3.1400000358317235,
+                    "angularVelocity": 5.824165114605996e-9,
+                    "velocityX": -3.305567521694012,
+                    "velocityY": -0.0000013288845794272055,
+                    "timestamp": 0.5946596114011948
+                },
+                {
+                    "x": 2.4957109641637163,
+                    "y": 5.560000125368319,
+                    "heading": 3.1400000237983328,
+                    "angularVelocity": -1.214145560764502e-7,
+                    "velocityX": -2.479175819794127,
+                    "velocityY": -0.0000011166946380844406,
+                    "timestamp": 0.6937695466347273
+                },
+                {
+                    "x": 2.3319036557469475,
+                    "y": 5.560000044140379,
+                    "heading": 3.140000009519778,
+                    "angularVelocity": -1.4406783354118398e-7,
+                    "velocityX": -1.6527839316087705,
+                    "velocityY": -8.195741267361974e-7,
+                    "timestamp": 0.7928794818682598
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": -9.605270403416775e-8,
+                    "velocityX": -0.8263919813281883,
+                    "velocityY": -4.4536784825580755e-7,
+                    "timestamp": 0.8919894171017924
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 4.934081445690025e-38,
+                    "velocityY": -9.331512134237052e-34,
+                    "timestamp": 0.9910993523353249
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -2764,7 +9156,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S2 To W3": {
             "waypoints": [
@@ -2775,7 +9183,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 13
                 },
                 {
                     "x": 2.3,
@@ -2787,7 +9195,134 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 1.4062915610922596e-31,
+                    "velocityX": -1.6151999437241794e-32,
+                    "velocityY": -1.528021229010722e-32,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.206513702340257,
+                    "y": 5.554388398990967,
+                    "heading": 3.0326218488137218,
+                    "angularVelocity": -1.231959178646129,
+                    "velocityX": -0.6480723694798501,
+                    "velocityY": -0.0643824026918128,
+                    "timestamp": 0.0871604782426985
+                },
+                {
+                    "x": 4.0911807616230185,
+                    "y": 5.5411300283093805,
+                    "heading": 2.844661289825114,
+                    "angularVelocity": -2.156488385311873,
+                    "velocityX": -1.323225193832629,
+                    "velocityY": -0.15211447836104172,
+                    "timestamp": 0.174320956485397
+                },
+                {
+                    "x": 3.915187082588679,
+                    "y": 5.516242186917599,
+                    "heading": 2.6068500185118766,
+                    "angularVelocity": -2.728430087901214,
+                    "velocityX": -2.0191912961317713,
+                    "velocityY": -0.2855404409608899,
+                    "timestamp": 0.2614814347280955
+                },
+                {
+                    "x": 3.678761658477574,
+                    "y": 5.47299330729491,
+                    "heading": 2.386294579571215,
+                    "angularVelocity": -2.530452372307143,
+                    "velocityX": -2.7125301384049063,
+                    "velocityY": -0.4961982826925549,
+                    "timestamp": 0.348641912970794
+                },
+                {
+                    "x": 3.3968919941686,
+                    "y": 5.398640264151657,
+                    "heading": 2.288226309782543,
+                    "angularVelocity": -1.1251460726913511,
+                    "velocityX": -3.233915990273797,
+                    "velocityY": -0.8530591460984852,
+                    "timestamp": 0.4358023912134925
+                },
+                {
+                    "x": 3.0985878200503763,
+                    "y": 5.268158980231371,
+                    "heading": 2.276780497851342,
+                    "angularVelocity": -0.1313188289230101,
+                    "velocityX": -3.4224705982864747,
+                    "velocityY": -1.4970234967844136,
+                    "timestamp": 0.522962869456191
+                },
+                {
+                    "x": 2.8320780769343714,
+                    "y": 5.083465560010488,
+                    "heading": 2.2937556690853027,
+                    "angularVelocity": 0.19475766512768708,
+                    "velocityX": -3.0576902340290975,
+                    "velocityY": -2.11900420861165,
+                    "timestamp": 0.6101233476988894
+                },
+                {
+                    "x": 2.620143221340131,
+                    "y": 4.878958491159276,
+                    "heading": 2.4039835676400654,
+                    "angularVelocity": 1.264654586311841,
+                    "velocityX": -2.4315476448409066,
+                    "velocityY": -2.34632798000215,
+                    "timestamp": 0.6972838259415879
+                },
+                {
+                    "x": 2.469192165605932,
+                    "y": 4.691124569407488,
+                    "heading": 2.528247411458178,
+                    "angularVelocity": 1.42569024773017,
+                    "velocityX": -1.7318750284260176,
+                    "velocityY": -2.155035464912952,
+                    "timestamp": 0.7844443041842863
+                },
+                {
+                    "x": 2.3748255822885,
+                    "y": 4.531333386407196,
+                    "heading": 2.6349251655798476,
+                    "angularVelocity": 1.2239234601791131,
+                    "velocityX": -1.0826762911358505,
+                    "velocityY": -1.833298603013082,
+                    "timestamp": 0.8716047824269848
+                },
+                {
+                    "x": 2.3223557063303257,
+                    "y": 4.418269595148307,
+                    "heading": 2.710801114054306,
+                    "angularVelocity": 0.8705315758270817,
+                    "velocityX": -0.6019916023415092,
+                    "velocityY": -1.2971910381682688,
+                    "timestamp": 0.9587652606696833
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 0.44973234126302364,
+                    "velocityX": -0.2564890278375538,
+                    "velocityY": -0.6685323018312901,
+                    "timestamp": 1.0459257389123817
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 6.1103463204495e-32,
+                    "velocityY": 1.8635567714760387e-30,
+                    "timestamp": 1.1330862171550802
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -2805,7 +9340,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S2 To C1": {
             "waypoints": [
@@ -2816,7 +9367,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 15
                 },
                 {
                     "x": 7.68,
@@ -2828,7 +9379,152 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 7.124511967920712e-30,
+                    "velocityY": -7.12047614876129e-30,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.30430205284905,
+                    "y": 5.6386850261363115,
+                    "heading": 3.0976627193075768,
+                    "angularVelocity": -0.4082995079820314,
+                    "velocityX": 0.3983158006634516,
+                    "velocityY": 0.758836111615579,
+                    "timestamp": 0.10369172596279416
+                },
+                {
+                    "x": 4.388754424241646,
+                    "y": 5.795256557860491,
+                    "heading": 3.0179562679096885,
+                    "angularVelocity": -0.7686867072352654,
+                    "velocityX": 0.8144562221183627,
+                    "velocityY": 1.5099713142045914,
+                    "timestamp": 0.20738345192558832
+                },
+                {
+                    "x": 4.5199632474011375,
+                    "y": 6.027943066067755,
+                    "heading": 2.911091818022606,
+                    "angularVelocity": -1.030597657574192,
+                    "velocityX": 1.265374087866657,
+                    "velocityY": 2.2440219414493656,
+                    "timestamp": 0.31107517788838246
+                },
+                {
+                    "x": 4.707776355466448,
+                    "y": 6.330145783951522,
+                    "heading": 2.807461071652614,
+                    "angularVelocity": -0.9994119145743484,
+                    "velocityX": 1.8112641709976698,
+                    "velocityY": 2.914434252856334,
+                    "timestamp": 0.41476690385117665
+                },
+                {
+                    "x": 4.978743687535899,
+                    "y": 6.613772866928766,
+                    "heading": 2.807461069210919,
+                    "angularVelocity": -2.3547634644306893e-8,
+                    "velocityX": 2.613201097324575,
+                    "velocityY": 2.7352913681754196,
+                    "timestamp": 0.5184586298139708
+                },
+                {
+                    "x": 5.307032554871728,
+                    "y": 6.82846808577305,
+                    "heading": 2.80746110336737,
+                    "angularVelocity": 3.2940382214109427e-7,
+                    "velocityX": 3.166008322145415,
+                    "velocityY": 2.0705144682548786,
+                    "timestamp": 0.622150355776765
+                },
+                {
+                    "x": 5.675494511223277,
+                    "y": 6.963016991691847,
+                    "heading": 2.8074611309590716,
+                    "angularVelocity": 2.660935682689419e-7,
+                    "velocityX": 3.5534364283198543,
+                    "velocityY": 1.2975857491953324,
+                    "timestamp": 0.7258420817395592
+                },
+                {
+                    "x": 6.055822077771958,
+                    "y": 7.05903034533374,
+                    "heading": 2.8074611577219364,
+                    "angularVelocity": 2.5810029119060704e-7,
+                    "velocityX": 3.6678680291727868,
+                    "velocityY": 0.9259499998712609,
+                    "timestamp": 0.8295338077023534
+                },
+                {
+                    "x": 6.436149809853972,
+                    "y": 7.155043043263379,
+                    "heading": 2.807461184484807,
+                    "angularVelocity": 2.58100350606906e-7,
+                    "velocityX": 3.6678696255714556,
+                    "velocityY": 0.9259436762012891,
+                    "timestamp": 0.9332255336651476
+                },
+                {
+                    "x": 6.816477543748484,
+                    "y": 7.2510557340128905,
+                    "heading": 2.8074612112477313,
+                    "angularVelocity": 2.581008643735431e-7,
+                    "velocityX": 3.667869643051154,
+                    "velocityY": 0.9259436069563147,
+                    "timestamp": 1.0369172596279417
+                },
+                {
+                    "x": 7.162445200777026,
+                    "y": 7.337344185673276,
+                    "heading": 2.898451236968193,
+                    "angularVelocity": 0.8775051709825819,
+                    "velocityX": 3.3365020575767024,
+                    "velocityY": 0.8321633270078459,
+                    "timestamp": 1.1406089855907358
+                },
+                {
+                    "x": 7.421534649307807,
+                    "y": 7.402207433353085,
+                    "heading": 3.009161772576331,
+                    "angularVelocity": 1.0676891968008626,
+                    "velocityX": 2.498651132721501,
+                    "velocityY": 0.6255392807626672,
+                    "timestamp": 1.2443007115535298
+                },
+                {
+                    "x": 7.593924390045372,
+                    "y": 7.445415282758517,
+                    "heading": 3.094199643650422,
+                    "angularVelocity": 0.8201027640778757,
+                    "velocityX": 1.6625216634876054,
+                    "velocityY": 0.416695247419558,
+                    "timestamp": 1.347992437516324
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "angularVelocity": 0.4416973092532584,
+                    "velocityX": 0.8301106877660893,
+                    "velocityY": 0.20816238751031255,
+                    "timestamp": 1.451684163479118
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": -4.6860665144292015e-31,
+                    "velocityY": 3.7247780913146357e-28,
+                    "timestamp": 1.555375889441912
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -2846,7 +9542,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S2 To C2": {
             "waypoints": [
@@ -2857,7 +9569,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 14
                 },
                 {
                     "x": 7.68,
@@ -2887,7 +9599,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S2 To C3": {
             "waypoints": [
@@ -2898,7 +9626,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 15
                 },
                 {
                     "x": 7.68,
@@ -2910,7 +9638,152 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": -3.269880825493717e-29,
+                    "velocityX": -1.6106277248271234e-30,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.257789422547788,
+                    "y": 5.465225005289041,
+                    "heading": 3.082306365857339,
+                    "angularVelocity": -0.5369837298016524,
+                    "velocityX": -0.04849747041052485,
+                    "velocityY": -0.882118641130872,
+                    "timestamp": 0.1074401903461163
+                },
+                {
+                    "x": 4.273795168733638,
+                    "y": 5.277874820636679,
+                    "heading": 2.968634839381067,
+                    "angularVelocity": -1.057998185875108,
+                    "velocityX": 0.14897354643815672,
+                    "velocityY": -1.7437625905987038,
+                    "timestamp": 0.2148803806922326
+                },
+                {
+                    "x": 4.354522429535891,
+                    "y": 5.020377016639425,
+                    "heading": 2.810559381522931,
+                    "angularVelocity": -1.471287954245977,
+                    "velocityX": 0.7513693017686587,
+                    "velocityY": -2.396661837322988,
+                    "timestamp": 0.3223205710383489
+                },
+                {
+                    "x": 4.529578110420142,
+                    "y": 4.7442561245273325,
+                    "heading": 2.6367348361471494,
+                    "angularVelocity": -1.6178726491066266,
+                    "velocityX": 1.6293314477600391,
+                    "velocityY": -2.5699963042002807,
+                    "timestamp": 0.4297607613844652
+                },
+                {
+                    "x": 4.793919720832893,
+                    "y": 4.487710124588879,
+                    "heading": 2.5690751936264027,
+                    "angularVelocity": -0.6297423925143462,
+                    "velocityX": 2.4603605928208117,
+                    "velocityY": -2.3878029172509625,
+                    "timestamp": 0.5372009517305815
+                },
+                {
+                    "x": 5.141097516271185,
+                    "y": 4.276379117165763,
+                    "heading": 2.5690757140293337,
+                    "angularVelocity": 0.000004843652357933311,
+                    "velocityX": 3.231358715205786,
+                    "velocityY": -1.9669641941467053,
+                    "timestamp": 0.6446411420766978
+                },
+                {
+                    "x": 5.528234411762885,
+                    "y": 4.152611717586108,
+                    "heading": 2.5690757988249406,
+                    "angularVelocity": 7.892354497964203e-7,
+                    "velocityX": 3.6032781982659046,
+                    "velocityY": -1.1519655650361502,
+                    "timestamp": 0.752081332422814
+                },
+                {
+                    "x": 5.933619437652518,
+                    "y": 4.123348539284839,
+                    "heading": 2.569075852374573,
+                    "angularVelocity": 4.984134150731162e-7,
+                    "velocityX": 3.773122744698165,
+                    "velocityY": -0.2723671487085231,
+                    "timestamp": 0.8595215227689303
+                },
+                {
+                    "x": 6.340054515622032,
+                    "y": 4.125318524745646,
+                    "heading": 2.569075903811562,
+                    "angularVelocity": 4.787499761820812e-7,
+                    "velocityX": 3.782896108618124,
+                    "velocityY": 0.01833564753065316,
+                    "timestamp": 0.9669617131150466
+                },
+                {
+                    "x": 6.746489593269827,
+                    "y": 4.127288577706153,
+                    "heading": 2.5690759552486404,
+                    "angularVelocity": 4.787508167496477e-7,
+                    "velocityX": 3.7828961056237245,
+                    "velocityY": 0.01833627578441685,
+                    "timestamp": 1.074401903461163
+                },
+                {
+                    "x": 7.118708956476355,
+                    "y": 4.125866161972181,
+                    "heading": 2.656486209298221,
+                    "angularVelocity": 0.8135712880626333,
+                    "velocityX": 3.464433207046916,
+                    "velocityY": -0.013239140115027938,
+                    "timestamp": 1.1818420938072793
+                },
+                {
+                    "x": 7.400847427365447,
+                    "y": 4.126314118090732,
+                    "heading": 2.8696985577774003,
+                    "angularVelocity": 1.9844747835268808,
+                    "velocityX": 2.6260049426586924,
+                    "velocityY": 0.004169353359354839,
+                    "timestamp": 1.2892822841533955
+                },
+                {
+                    "x": 7.587357907068713,
+                    "y": 4.126749409200305,
+                    "heading": 3.0436879317659473,
+                    "angularVelocity": 1.6194067920769941,
+                    "velocityX": 1.7359470334371772,
+                    "velocityY": 0.004051473737811161,
+                    "timestamp": 1.3967224744995117
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": 0.8964249590751834,
+                    "velocityX": 0.8622666493129096,
+                    "velocityY": 0.002332374867239492,
+                    "timestamp": 1.5041626648456279
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 4.233852791919794e-30,
+                    "velocityY": -1.3542182850561966e-29,
+                    "timestamp": 1.611602855191744
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -2928,7 +9801,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S2 To C4": {
             "waypoints": [
@@ -2969,7 +9858,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S2 To C5": {
             "waypoints": [
@@ -3010,7 +9915,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S3 To W1": {
             "waypoints": [
@@ -3051,7 +9972,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S3 To W2": {
             "waypoints": [
@@ -3092,7 +10029,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S3 To W3": {
             "waypoints": [
@@ -3133,7 +10086,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S3 To C1": {
             "waypoints": [
@@ -3144,7 +10113,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 23
                 },
                 {
                     "x": 7.68,
@@ -3174,7 +10143,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S3 To C2": {
             "waypoints": [
@@ -3185,7 +10170,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 20
                 },
                 {
                     "x": 7.68,
@@ -3197,7 +10182,197 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": 1.5794772183943222e-24,
+                    "velocityX": -1.3909195862275997e-25,
+                    "velocityY": -1.589027573571451e-24,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.294802050577596,
+                    "y": 3.0477360619772367,
+                    "heading": 2.544164914123716,
+                    "angularVelocity": -0.3578657180774014,
+                    "velocityX": 0.38145905506951994,
+                    "velocityY": 0.5725842442186713,
+                    "timestamp": 0.08336949970108862
+                },
+                {
+                    "x": 4.359925633327058,
+                    "y": 3.142202439962799,
+                    "heading": 2.4853835413264154,
+                    "angularVelocity": -0.7050704755102848,
+                    "velocityX": 0.781143979308431,
+                    "velocityY": 1.13310477241989,
+                    "timestamp": 0.16673899940217723
+                },
+                {
+                    "x": 4.461027910894954,
+                    "y": 3.2814650300922756,
+                    "heading": 2.399327847716391,
+                    "angularVelocity": -1.0322203433937678,
+                    "velocityX": 1.2127010229206814,
+                    "velocityY": 1.6704261226082242,
+                    "timestamp": 0.2501084991032658
+                },
+                {
+                    "x": 4.603798889123446,
+                    "y": 3.46048671868814,
+                    "heading": 2.2901235890618308,
+                    "angularVelocity": -1.309882619496323,
+                    "velocityX": 1.7125085161885507,
+                    "velocityY": 2.147328330357326,
+                    "timestamp": 0.33347799880435447
+                },
+                {
+                    "x": 4.803010081194681,
+                    "y": 3.6526247699506835,
+                    "heading": 2.177140187929377,
+                    "angularVelocity": -1.3552126561577384,
+                    "velocityX": 2.389497271609915,
+                    "velocityY": 2.3046564025384755,
+                    "timestamp": 0.4168474985054431
+                },
+                {
+                    "x": 5.0510307536076,
+                    "y": 3.8467573295012585,
+                    "heading": 2.175769825428097,
+                    "angularVelocity": -0.016437216322438507,
+                    "velocityX": 2.9749569483104428,
+                    "velocityY": 2.328580119187639,
+                    "timestamp": 0.5002169982065318
+                },
+                {
+                    "x": 5.320458962981737,
+                    "y": 4.010690590396415,
+                    "heading": 2.175769541029465,
+                    "angularVelocity": -0.000003411303085546954,
+                    "velocityX": 3.231735950679083,
+                    "velocityY": 1.9663457437422465,
+                    "timestamp": 0.5835864979076204
+                },
+                {
+                    "x": 5.614141443438261,
+                    "y": 4.125652797356739,
+                    "heading": 2.175769455965937,
+                    "angularVelocity": -0.0000010203195273976758,
+                    "velocityX": 3.522660943264475,
+                    "velocityY": 1.3789480250272361,
+                    "timestamp": 0.666955997608709
+                },
+                {
+                    "x": 5.906454716700063,
+                    "y": 4.244053209777629,
+                    "heading": 2.1757695445063954,
+                    "angularVelocity": 0.000001062024591118681,
+                    "velocityX": 3.506237584606561,
+                    "velocityY": 1.4201885922957453,
+                    "timestamp": 0.7503254973097977
+                },
+                {
+                    "x": 6.172161728896083,
+                    "y": 4.41393857041553,
+                    "heading": 2.1757932909481013,
+                    "angularVelocity": 0.00028483368367179416,
+                    "velocityX": 3.187100955969282,
+                    "velocityY": 2.0377399558232225,
+                    "timestamp": 0.8336949970108863
+                },
+                {
+                    "x": 6.426803794432408,
+                    "y": 4.599225132728127,
+                    "heading": 2.1773610490246513,
+                    "angularVelocity": 0.018804935643986023,
+                    "velocityX": 3.0543791968203453,
+                    "velocityY": 2.2224742019194084,
+                    "timestamp": 0.917064496711975
+                },
+                {
+                    "x": 6.681572955411179,
+                    "y": 4.785089288083454,
+                    "heading": 2.177430344851842,
+                    "angularVelocity": 0.0008311891931529928,
+                    "velocityX": 3.0559036805092323,
+                    "velocityY": 2.2294023116574087,
+                    "timestamp": 1.0004339964130635
+                },
+                {
+                    "x": 6.9013556581674305,
+                    "y": 5.011277814262196,
+                    "heading": 2.1774305498100937,
+                    "angularVelocity": 0.000002458432073886186,
+                    "velocityX": 2.636248310764209,
+                    "velocityY": 2.7130848450538085,
+                    "timestamp": 1.083803496114152
+                },
+                {
+                    "x": 7.121935686700776,
+                    "y": 5.230936165800854,
+                    "heading": 2.189684453697612,
+                    "angularVelocity": 0.14698305653090518,
+                    "velocityX": 2.645812069452359,
+                    "velocityY": 2.634756743487918,
+                    "timestamp": 1.1671729958152406
+                },
+                {
+                    "x": 7.311158558493755,
+                    "y": 5.417630564055526,
+                    "heading": 2.349466738853841,
+                    "angularVelocity": 1.9165556435999858,
+                    "velocityX": 2.2696894244467765,
+                    "velocityY": 2.239360904455968,
+                    "timestamp": 1.2505424955163291
+                },
+                {
+                    "x": 7.462052071844248,
+                    "y": 5.571824642162805,
+                    "heading": 2.618524466568701,
+                    "angularVelocity": 3.2272920993832837,
+                    "velocityX": 1.8099366541901185,
+                    "velocityY": 1.8495262495291969,
+                    "timestamp": 1.3339119952174177
+                },
+                {
+                    "x": 7.571973798709058,
+                    "y": 5.685339226077728,
+                    "heading": 2.8696843432959835,
+                    "angularVelocity": 3.0126110583341195,
+                    "velocityX": 1.3184885030967077,
+                    "velocityY": 1.3615840843703815,
+                    "timestamp": 1.4172814949185062
+                },
+                {
+                    "x": 7.644303858666078,
+                    "y": 5.759924476296826,
+                    "heading": 3.0477755113688096,
+                    "angularVelocity": 2.1361669280894184,
+                    "velocityX": 0.8675841910572869,
+                    "velocityY": 0.8946347343634476,
+                    "timestamp": 1.5006509946195947
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": 1.1062137707656932,
+                    "velocityX": 0.4281678726861332,
+                    "velocityY": 0.44471328046952424,
+                    "timestamp": 1.5840204943206833
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": 1.4380080051319566e-21,
+                    "velocityX": 0,
+                    "velocityY": 1.143279687808587e-23,
+                    "timestamp": 1.6673899940217718
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -3215,7 +10390,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S3 To C3": {
             "waypoints": [
@@ -3226,7 +10417,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 17
                 },
                 {
                     "x": 7.68,
@@ -3238,7 +10429,170 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": -2.0886910556164006e-30,
+                    "velocityX": -6.72399315077418e-33,
+                    "velocityY": 1.0977042871870097e-32,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.300222432415914,
+                    "y": 3.053065889088155,
+                    "heading": 2.5374285401398833,
+                    "angularVelocity": -0.4122912034410164,
+                    "velocityX": 0.41962999329145273,
+                    "velocityY": 0.5982424370672392,
+                    "timestamp": 0.08870298360694638
+                },
+                {
+                    "x": 4.375819084559984,
+                    "y": 3.158500650682638,
+                    "heading": 2.466843265689978,
+                    "angularVelocity": -0.7957485935611535,
+                    "velocityX": 0.8522447506280879,
+                    "velocityY": 1.1886270033675244,
+                    "timestamp": 0.17740596721389276
+                },
+                {
+                    "x": 4.491836015930185,
+                    "y": 3.3149606373409832,
+                    "heading": 2.3668866050755812,
+                    "angularVelocity": -1.1268692049561326,
+                    "velocityX": 1.307925919203419,
+                    "velocityY": 1.7638638554892148,
+                    "timestamp": 0.26610895082083913
+                },
+                {
+                    "x": 4.652875991060298,
+                    "y": 3.5188568125430604,
+                    "heading": 2.248368974552935,
+                    "angularVelocity": -1.3361177460254572,
+                    "velocityX": 1.8154967125311139,
+                    "velocityY": 2.2986394246394894,
+                    "timestamp": 0.3548119344277855
+                },
+                {
+                    "x": 4.870873285433555,
+                    "y": 3.7498811407551353,
+                    "heading": 2.196009009563727,
+                    "angularVelocity": -0.5902841467116942,
+                    "velocityX": 2.4576094907836294,
+                    "velocityY": 2.604470772209555,
+                    "timestamp": 0.4435149180347319
+                },
+                {
+                    "x": 5.1428014530518755,
+                    "y": 3.946488109616269,
+                    "heading": 2.1960088759837526,
+                    "angularVelocity": -0.0000015059242530161288,
+                    "velocityX": 3.0656033941684258,
+                    "velocityY": 2.21646398876866,
+                    "timestamp": 0.5322179016416783
+                },
+                {
+                    "x": 5.447787504833652,
+                    "y": 4.086426434262152,
+                    "heading": 2.1960087423061556,
+                    "angularVelocity": -0.0000015070248085665752,
+                    "velocityX": 3.4382840281135,
+                    "velocityY": 1.577605610945025,
+                    "timestamp": 0.6209208852486247
+                },
+                {
+                    "x": 5.774173248105482,
+                    "y": 4.1643468008186435,
+                    "heading": 2.1960088209928723,
+                    "angularVelocity": 8.870808373208906e-7,
+                    "velocityX": 3.679535118210733,
+                    "velocityY": 0.8784413261876967,
+                    "timestamp": 0.709623868855571
+                },
+                {
+                    "x": 6.1094823510079035,
+                    "y": 4.1772711464289065,
+                    "heading": 2.1960089213757183,
+                    "angularVelocity": 0.0000011316738380089979,
+                    "velocityX": 3.780133308572978,
+                    "velocityY": 0.1457036176768579,
+                    "timestamp": 0.7983268524625174
+                },
+                {
+                    "x": 6.444914319541054,
+                    "y": 4.1680717704970895,
+                    "heading": 2.1960090177915097,
+                    "angularVelocity": 0.000001086950938877831,
+                    "velocityX": 3.78151844383826,
+                    "velocityY": -0.10370988164931202,
+                    "timestamp": 0.8870298360694638
+                },
+                {
+                    "x": 6.780346283198341,
+                    "y": 4.1588722167865155,
+                    "heading": 2.196009114207664,
+                    "angularVelocity": 0.0000010869550292397467,
+                    "velocityX": 3.7815183888698267,
+                    "velocityY": -0.10371188585197635,
+                    "timestamp": 0.9757328196764102
+                },
+                {
+                    "x": 7.07908088896125,
+                    "y": 4.144071317525266,
+                    "heading": 2.2852514937378463,
+                    "angularVelocity": 1.0060809219859659,
+                    "velocityX": 3.3678078641259517,
+                    "velocityY": -0.16685909153668751,
+                    "timestamp": 1.0644358032833565
+                },
+                {
+                    "x": 7.324007491183461,
+                    "y": 4.136590325759102,
+                    "heading": 2.509537504382542,
+                    "angularVelocity": 2.5285058238687177,
+                    "velocityX": 2.7611991419308914,
+                    "velocityY": -0.08433754381152235,
+                    "timestamp": 1.153138786890303
+                },
+                {
+                    "x": 7.505234177737953,
+                    "y": 4.13149769076231,
+                    "heading": 2.790057598116992,
+                    "angularVelocity": 3.162465142970483,
+                    "velocityX": 2.043073177307424,
+                    "velocityY": -0.05741221760204461,
+                    "timestamp": 1.2418417704972493
+                },
+                {
+                    "x": 7.622663550770296,
+                    "y": 4.128429038475573,
+                    "heading": 3.015163183857666,
+                    "angularVelocity": 2.5377453675982737,
+                    "velocityX": 1.3238491903799634,
+                    "velocityY": -0.03459469075285244,
+                    "timestamp": 1.3305447541041957
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": 1.4073575776830842,
+                    "velocityX": 0.6463869297087906,
+                    "velocityY": -0.016110376646462816,
+                    "timestamp": 1.419247737711142
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": 1.404904480201356e-31,
+                    "velocityX": 0,
+                    "velocityY": -1.2280872780983036e-31,
+                    "timestamp": 1.5079507213180885
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -3256,7 +10610,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S3 To C4": {
             "waypoints": [
@@ -3297,7 +10667,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S3 To C5": {
             "waypoints": [
@@ -3308,19 +10694,200 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
+                    "controlIntervalCount": 19
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
                     "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": -5.037969685186626e-28,
+                    "velocityX": 1.6468312353259754e-28,
+                    "velocityY": 5.118071700544948e-29,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.2573202524069735,
+                    "y": 2.93910307994174,
+                    "heading": 2.524484159587002,
+                    "angularVelocity": -0.5709623504386324,
+                    "velocityX": -0.06549261829272982,
+                    "velocityY": -0.7021964753285136,
+                    "timestamp": 0.08672347725723033
+                },
+                {
+                    "x": 4.260125495463358,
+                    "y": 2.8166106869531333,
+                    "heading": 2.4459027831938824,
+                    "angularVelocity": -0.9061142251024153,
+                    "velocityX": 0.032346985442753974,
+                    "velocityY": -1.412447896032608,
+                    "timestamp": 0.17344695451446066
+                },
+                {
+                    "x": 4.2940460497089665,
+                    "y": 2.639724962337947,
+                    "heading": 2.375950811095946,
+                    "angularVelocity": -0.8066094016324238,
+                    "velocityX": 0.3911346191181556,
+                    "velocityY": -2.039652124309153,
+                    "timestamp": 0.26017043177169097
+                },
+                {
+                    "x": 4.380083643145815,
+                    "y": 2.4322611848370723,
+                    "heading": 2.367349671900665,
+                    "angularVelocity": -0.09917890134603644,
+                    "velocityX": 0.9920911402301427,
+                    "velocityY": -2.3922446846257825,
+                    "timestamp": 0.3468939090289213
+                },
+                {
+                    "x": 4.5223038363919486,
+                    "y": 2.2062304273482725,
+                    "heading": 2.4344770790327512,
+                    "angularVelocity": 0.774039617126779,
+                    "velocityX": 1.639927246278367,
+                    "velocityY": -2.6063387289968842,
+                    "timestamp": 0.43361738628615165
+                },
+                {
+                    "x": 4.727106314265492,
+                    "y": 1.976782887261895,
+                    "heading": 2.494103363003819,
+                    "angularVelocity": 0.6875448939185098,
+                    "velocityX": 2.361557496894169,
+                    "velocityY": -2.6457373175410677,
+                    "timestamp": 0.5203408635433819
+                },
+                {
+                    "x": 4.985728714890376,
+                    "y": 1.77493098646381,
+                    "heading": 2.4941035609909017,
+                    "angularVelocity": 0.0000022829698365674285,
+                    "velocityX": 2.982149803077939,
+                    "velocityY": -2.327534678981717,
+                    "timestamp": 0.6070643408006122
+                },
+                {
+                    "x": 5.274471193088088,
+                    "y": 1.6191826835790144,
+                    "heading": 2.494103670963996,
+                    "angularVelocity": 0.000001268089077272548,
+                    "velocityX": 3.329461494507118,
+                    "velocityY": -1.795918565659344,
+                    "timestamp": 0.6937878180578425
+                },
+                {
+                    "x": 5.576873114142296,
+                    "y": 1.4919708990806198,
+                    "heading": 2.4941037354476974,
+                    "angularVelocity": 7.435553047089643e-7,
+                    "velocityX": 3.486967204477438,
+                    "velocityY": -1.4668667415291887,
+                    "timestamp": 0.7805112953150728
+                },
+                {
+                    "x": 5.888127945966446,
+                    "y": 1.388287773871042,
+                    "heading": 2.4941037912285227,
+                    "angularVelocity": 6.432032813789173e-7,
+                    "velocityX": 3.589049259417249,
+                    "velocityY": -1.1955600546555811,
+                    "timestamp": 0.8672347725723031
+                },
+                {
+                    "x": 6.199389301961685,
+                    "y": 1.2846242361365592,
+                    "heading": 2.4941038470080463,
+                    "angularVelocity": 6.431882742929459e-7,
+                    "velocityX": 3.58912448900094,
+                    "velocityY": -1.1953341933810824,
+                    "timestamp": 0.9539582498295334
+                },
+                {
+                    "x": 6.510650764865791,
+                    "y": 1.1809610194297586,
+                    "heading": 2.494103902787595,
+                    "angularVelocity": 6.431885596372589e-7,
+                    "velocityX": 3.589125721756674,
+                    "velocityY": -1.1953304916421486,
+                    "timestamp": 1.0406817270867637
+                },
+                {
+                    "x": 6.821912914234967,
+                    "y": 1.0772998639276916,
+                    "heading": 2.4941039585563476,
+                    "angularVelocity": 6.430640741979831e-7,
+                    "velocityX": 3.589133637318779,
+                    "velocityY": -1.1953067240904078,
+                    "timestamp": 1.127405204343994
+                },
+                {
+                    "x": 7.104835642474277,
+                    "y": 0.9795860470438206,
+                    "heading": 2.575393099111341,
+                    "angularVelocity": 0.9373371908725603,
+                    "velocityX": 3.2623545225260457,
+                    "velocityY": -1.126728539655348,
+                    "timestamp": 1.2141286816012242
+                },
+                {
+                    "x": 7.336162807105594,
+                    "y": 0.9014195336331526,
+                    "heading": 2.773123816483908,
+                    "angularVelocity": 2.280013712850493,
+                    "velocityX": 2.6674110857568296,
+                    "velocityY": -0.9013304803129645,
+                    "timestamp": 1.3008521588584545
+                },
+                {
+                    "x": 7.508575525064269,
+                    "y": 0.8437553503848311,
+                    "heading": 2.9477959992299394,
+                    "angularVelocity": 2.0141279878335374,
+                    "velocityX": 1.9880743186447822,
+                    "velocityY": -0.664920100900538,
+                    "timestamp": 1.3875756361156848
+                },
+                {
+                    "x": 7.62299427085809,
+                    "y": 0.8057956185147762,
+                    "heading": 3.0737109392018835,
+                    "angularVelocity": 1.451912953149597,
+                    "velocityX": 1.31935145375275,
+                    "velocityY": -0.4377099843155908,
+                    "timestamp": 1.474299113372915
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": 0.7643727269087626,
+                    "velocityX": 0.6573275305004772,
+                    "velocityY": -0.2167304530355382,
+                    "timestamp": 1.5610225906301454
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
                     "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "angularVelocity": -1.052133339029093e-28,
+                    "velocityX": 0,
+                    "velocityY": -1.789975640452255e-29,
+                    "timestamp": 1.6477460678873757
                 }
             ],
-            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -3338,7 +10905,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S4 To W1": {
             "waypoints": [
@@ -3379,7 +10962,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S4 To W2": {
             "waypoints": [
@@ -3420,7 +11019,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S4 To W3": {
             "waypoints": [
@@ -3461,7 +11076,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S4 To C1": {
             "waypoints": [
@@ -3502,7 +11133,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S4 To C2": {
             "waypoints": [
@@ -3543,7 +11190,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S4 To C3": {
             "waypoints": [
@@ -3584,7 +11247,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S4 To C4": {
             "waypoints": [
@@ -3595,7 +11274,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 15
                 },
                 {
                     "x": 7.68,
@@ -3607,7 +11286,152 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": 0,
+                    "velocityX": -5.177514657860589e-36,
+                    "velocityY": 7.381711039090797e-35,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.22399403974827,
+                    "y": 1.6219999714389142,
+                    "heading": 2.5657089279494847,
+                    "angularVelocity": 0.8361720983637341,
+                    "velocityX": 0.6107431391380216,
+                    "velocityY": 0.02545042761966764,
+                    "timestamp": 0.07858301906756712
+                },
+                {
+                    "x": 5.320421642922389,
+                    "y": 1.6267873270311852,
+                    "heading": 2.6934140350122218,
+                    "angularVelocity": 1.6250979992628447,
+                    "velocityX": 1.2270793909204332,
+                    "velocityY": 0.06092099348021541,
+                    "timestamp": 0.15716603813513425
+                },
+                {
+                    "x": 5.466088696265102,
+                    "y": 1.6359772771500425,
+                    "heading": 2.8734313337183015,
+                    "angularVelocity": 2.2907913292475777,
+                    "velocityX": 1.8536708702609945,
+                    "velocityY": 0.11694575021297698,
+                    "timestamp": 0.23574905720270137
+                },
+                {
+                    "x": 5.662207065544692,
+                    "y": 1.6523107767500078,
+                    "heading": 3.0800991109745315,
+                    "angularVelocity": 2.629929210005703,
+                    "velocityX": 2.495683820838744,
+                    "velocityY": 0.2078502428867176,
+                    "timestamp": 0.3143320762702685
+                },
+                {
+                    "x": 5.9065811613932215,
+                    "y": 1.6816988713551873,
+                    "heading": 3.2417008687725852,
+                    "angularVelocity": 2.056446287194764,
+                    "velocityX": 3.1097570282762126,
+                    "velocityY": 0.3739751278824109,
+                    "timestamp": 0.39291509533783564
+                },
+                {
+                    "x": 6.188716953254853,
+                    "y": 1.7271916925648765,
+                    "heading": 3.2799906959816494,
+                    "angularVelocity": 0.48725319621713953,
+                    "velocityX": 3.5902895461301405,
+                    "velocityY": 0.5789141439141413,
+                    "timestamp": 0.4714981144054028
+                },
+                {
+                    "x": 6.470102604778816,
+                    "y": 1.8230794962925625,
+                    "heading": 3.2799906961506404,
+                    "angularVelocity": 2.150475348001427e-9,
+                    "velocityX": 3.580743713626258,
+                    "velocityY": 1.22021022945479,
+                    "timestamp": 0.5500811334729699
+                },
+                {
+                    "x": 6.733423435664353,
+                    "y": 1.9610450600579923,
+                    "heading": 3.279990689877103,
+                    "angularVelocity": -7.983324169828431e-8,
+                    "velocityX": 3.3508617257263804,
+                    "velocityY": 1.755666369178364,
+                    "timestamp": 0.6286641525405371
+                },
+                {
+                    "x": 6.996743409287627,
+                    "y": 2.0990122599800607,
+                    "heading": 3.2799906836034283,
+                    "angularVelocity": -7.983499950790008e-8,
+                    "velocityX": 3.3508508167250133,
+                    "velocityY": 1.7556871899187498,
+                    "timestamp": 0.7072471716081042
+                },
+                {
+                    "x": 7.224495559877957,
+                    "y": 2.2183414399847847,
+                    "heading": 3.23332230759167,
+                    "angularVelocity": -0.5938735437439961,
+                    "velocityX": 2.8982362002979842,
+                    "velocityY": 1.5185110144740412,
+                    "timestamp": 0.7858301906756714
+                },
+                {
+                    "x": 7.406697323512462,
+                    "y": 2.313804835379604,
+                    "heading": 3.1959916534176562,
+                    "angularVelocity": -0.4750473399592355,
+                    "velocityX": 2.31858950949497,
+                    "velocityY": 1.2148094655505426,
+                    "timestamp": 0.8644132097432385
+                },
+                {
+                    "x": 7.543348661054632,
+                    "y": 2.3854024089057084,
+                    "heading": 3.1679955366885673,
+                    "angularVelocity": -0.356261658832645,
+                    "velocityX": 1.7389423206643992,
+                    "velocityY": 0.9111074424939519,
+                    "timestamp": 0.9429962288108057
+                },
+                {
+                    "x": 7.634449555023231,
+                    "y": 2.4331341352600573,
+                    "heading": 3.149331937782722,
+                    "angularVelocity": -0.2375016781907786,
+                    "velocityX": 1.1592949093781795,
+                    "velocityY": 0.607405097446157,
+                    "timestamp": 1.0215792478783727
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": -0.11875259939679916,
+                    "velocityX": 0.5796474291424605,
+                    "velocityY": 0.30370256860992423,
+                    "timestamp": 1.1001622669459399
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": -1.0945332293260443e-31,
+                    "velocityX": 7.198086319769953e-33,
+                    "velocityY": -6.46561486115161e-33,
+                    "timestamp": 1.178745286013507
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -3625,7 +11449,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "S4 To C5": {
             "waypoints": [
@@ -3636,7 +11476,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 15
                 },
                 {
                     "x": 7.68,
@@ -3648,7 +11488,152 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.222452071774302,
+                    "y": 1.6045080698057588,
+                    "heading": 2.5267186887396895,
+                    "angularVelocity": 0.3466731100007665,
+                    "velocityX": 0.6027123690413302,
+                    "velocityY": -0.20100670630495998,
+                    "timestamp": 0.0770717080988209
+                },
+                {
+                    "x": 5.315452291782514,
+                    "y": 1.5735030960792171,
+                    "heading": 2.577725213280899,
+                    "angularVelocity": 0.6618060738424182,
+                    "velocityX": 1.2066713233988244,
+                    "velocityY": -0.40228735668849247,
+                    "timestamp": 0.1541434161976418
+                },
+                {
+                    "x": 5.4551316607074885,
+                    "y": 1.5269515495858468,
+                    "heading": 2.6491768389859613,
+                    "angularVelocity": 0.9270798256274155,
+                    "velocityX": 1.8123300024164322,
+                    "velocityY": -0.6040030465353413,
+                    "timestamp": 0.23121512429646268
+                },
+                {
+                    "x": 5.641664886749133,
+                    "y": 1.4648000786644693,
+                    "heading": 2.734238612850838,
+                    "angularVelocity": 1.1036705421892394,
+                    "velocityX": 2.420255508058452,
+                    "velocityY": -0.8064109704392095,
+                    "timestamp": 0.3082868323952836
+                },
+                {
+                    "x": 5.8751883475378746,
+                    "y": 1.3870009970649684,
+                    "heading": 2.8177772109144708,
+                    "angularVelocity": 1.0839074431374935,
+                    "velocityX": 3.02995050387775,
+                    "velocityY": -1.0094376200894257,
+                    "timestamp": 0.3853585404941045
+                },
+                {
+                    "x": 6.15120111527276,
+                    "y": 1.295041169269926,
+                    "heading": 2.8199508970154183,
+                    "angularVelocity": 0.028203424506439225,
+                    "velocityX": 3.5812462775702794,
+                    "velocityY": -1.193172307497486,
+                    "timestamp": 0.4624302485929254
+                },
+                {
+                    "x": 6.427838786740925,
+                    "y": 1.202967940667996,
+                    "heading": 2.8199509259126705,
+                    "angularVelocity": 3.7493981751466555e-7,
+                    "velocityX": 3.5893543596239312,
+                    "velocityY": -1.1946436750029477,
+                    "timestamp": 0.5395019566917463
+                },
+                {
+                    "x": 6.7044764770342145,
+                    "y": 1.110894768626946,
+                    "heading": 2.8199509548099355,
+                    "angularVelocity": 3.749399855143739e-7,
+                    "velocityX": 3.5893546038785895,
+                    "velocityY": -1.1946429411295039,
+                    "timestamp": 0.6165736647905672
+                },
+                {
+                    "x": 6.980650518345068,
+                    "y": 1.0194616665370815,
+                    "heading": 2.8221688917599996,
+                    "angularVelocity": 0.02877757616608584,
+                    "velocityX": 3.583338790892565,
+                    "velocityY": -1.1863380784636202,
+                    "timestamp": 0.6936453728893881
+                },
+                {
+                    "x": 7.214215740363564,
+                    "y": 0.9418010792136509,
+                    "heading": 2.905688153737692,
+                    "angularVelocity": 1.083656558780357,
+                    "velocityX": 3.030492352901014,
+                    "velocityY": -1.0076406665835769,
+                    "timestamp": 0.770717080988209
+                },
+                {
+                    "x": 7.400786993851664,
+                    "y": 0.8797765728247763,
+                    "heading": 2.990770900783412,
+                    "angularVelocity": 1.1039426677377822,
+                    "velocityX": 2.420748911505631,
+                    "velocityY": -0.8047636145464304,
+                    "timestamp": 0.8477887890870299
+                },
+                {
+                    "x": 7.540501987432905,
+                    "y": 0.8333423582814848,
+                    "heading": 3.0622172150737166,
+                    "angularVelocity": 0.9270109103939473,
+                    "velocityX": 1.8127922298296504,
+                    "velocityY": -0.6024806727230407,
+                    "timestamp": 0.9248604971858508
+                },
+                {
+                    "x": 7.633531379147319,
+                    "y": 0.802434096261962,
+                    "heading": 3.113237732512233,
+                    "angularVelocity": 0.661987630702283,
+                    "velocityX": 1.2070498242381353,
+                    "velocityY": -0.4010325290817799,
+                    "timestamp": 1.0019322052846718
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": 0.3472385411966328,
+                    "velocityX": 0.6029270921710836,
+                    "velocityY": -0.20025631509518968,
+                    "timestamp": 1.0790039133834926
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": 1.7638718006253152e-30,
+                    "velocityX": 1.1888064162036181e-31,
+                    "velocityY": -2.2168268180884255e-31,
+                    "timestamp": 1.1560756214823134
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -3666,7 +11651,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C1 To W1": {
             "waypoints": [
@@ -3677,19 +11678,218 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 21
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 3.5331853071795862,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "angularVelocity": -3.261859244163768e-33,
+                    "velocityX": 7.471195721597621e-34,
+                    "velocityY": -1.6825458997868564e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.6132701032534955,
+                    "y": 7.458186361668427,
+                    "heading": 3.162690215100173,
+                    "angularVelocity": 0.25198683492477825,
+                    "velocityX": -0.7410707832329344,
+                    "velocityY": -0.09788011341186789,
+                    "timestamp": 0.09004524028783478
+                },
+                {
+                    "x": 7.4797323779606675,
+                    "y": 7.440537712219312,
+                    "heading": 3.2047640757495373,
+                    "angularVelocity": 0.46725246681415944,
+                    "velocityX": -1.4830070403051525,
+                    "velocityY": -0.19599758291165675,
+                    "timestamp": 0.18009048057566956
+                },
+                {
+                    "x": 7.27928179650239,
+                    "y": 7.414023208632297,
+                    "heading": 3.2605597645105227,
+                    "angularVelocity": 0.6196406226762411,
+                    "velocityX": -2.226109684615486,
+                    "velocityY": -0.29445758045912396,
+                    "timestamp": 0.27013572086350435
+                },
+                {
+                    "x": 7.011816476608683,
+                    "y": 7.378594826276994,
+                    "heading": 3.318124935630623,
+                    "angularVelocity": 0.6392916597933375,
+                    "velocityX": -2.9703437853987498,
+                    "velocityY": -0.3934509169174634,
+                    "timestamp": 0.36018096115133913
+                },
+                {
+                    "x": 6.67842119537258,
+                    "y": 7.334255965416845,
+                    "heading": 3.332157072604317,
+                    "angularVelocity": 0.15583429983460975,
+                    "velocityX": -3.70253086304603,
+                    "velocityY": -0.49240649165261274,
+                    "timestamp": 0.4502262014391739
+                },
+                {
+                    "x": 6.34069723545727,
+                    "y": 7.289811993684431,
+                    "heading": 3.332157092024104,
+                    "angularVelocity": 2.1566700178855977e-7,
+                    "velocityX": -3.7506031283358707,
+                    "velocityY": -0.4935738034608694,
+                    "timestamp": 0.5402714417270087
+                },
+                {
+                    "x": 6.002973284663831,
+                    "y": 7.245367952636118,
+                    "heading": 3.3321571114438866,
+                    "angularVelocity": 2.156669548142136e-7,
+                    "velocityX": -3.750603027032689,
+                    "velocityY": -0.49357457325056325,
+                    "timestamp": 0.6303166820148435
+                },
+                {
+                    "x": 5.665249344531261,
+                    "y": 7.200923830577433,
+                    "heading": 3.3321571308636764,
+                    "angularVelocity": 2.1566703381132295e-7,
+                    "velocityX": -3.750602908638108,
+                    "velocityY": -0.4935754729135791,
+                    "timestamp": 0.7203619223026783
+                },
+                {
+                    "x": 5.32752541585805,
+                    "y": 7.156479621440943,
+                    "heading": 3.332157150283477,
+                    "angularVelocity": 2.1566715012677575e-7,
+                    "velocityX": -3.750602781375866,
+                    "velocityY": -0.4935764399586428,
+                    "timestamp": 0.810407162590513
+                },
+                {
+                    "x": 4.9898014989996415,
+                    "y": 7.112035322525874,
+                    "heading": 3.332157169703281,
+                    "angularVelocity": 2.1566719028685347e-7,
+                    "velocityX": -3.750602650166234,
+                    "velocityY": -0.4935774369972257,
+                    "timestamp": 0.9004524028783478
+                },
+                {
+                    "x": 4.652077593988678,
+                    "y": 7.06759093358437,
+                    "heading": 3.33215718912308,
+                    "angularVelocity": 2.1566713427464117e-7,
+                    "velocityX": -3.750602518594089,
+                    "velocityY": -0.49357843678839325,
+                    "timestamp": 0.9904976431661826
+                },
+                {
+                    "x": 4.314353700505922,
+                    "y": 7.023146457042436,
+                    "heading": 3.332157208542871,
+                    "angularVelocity": 2.156670494902004e-7,
+                    "velocityX": -3.7506023905672445,
+                    "velocityY": -0.4935794096374718,
+                    "timestamp": 1.0805428834540174
+                },
+                {
+                    "x": 3.9766298178690414,
+                    "y": 6.978701898085155,
+                    "heading": 3.3321572279626577,
+                    "angularVelocity": 2.1566699709425413e-7,
+                    "velocityX": -3.750602270118079,
+                    "velocityY": -0.4935803249034738,
+                    "timestamp": 1.1705881237418523
+                },
+                {
+                    "x": 3.6389059453045984,
+                    "y": 6.934257262589842,
+                    "heading": 3.332157247382445,
+                    "angularVelocity": 2.1566700145288475e-7,
+                    "velocityX": -3.7506021582583307,
+                    "velocityY": -0.49358117489878034,
+                    "timestamp": 1.2606333640296872
+                },
+                {
+                    "x": 3.301182082389383,
+                    "y": 6.889812553772521,
+                    "heading": 3.332157266802242,
+                    "angularVelocity": 2.156671157814786e-7,
+                    "velocityX": -3.750602051098551,
+                    "velocityY": -0.4935819891784481,
+                    "timestamp": 1.350678604317522
+                },
+                {
+                    "x": 2.9679743508365415,
+                    "y": 6.845442796163545,
+                    "heading": 3.346601240828058,
+                    "angularVelocity": 0.16040796803523288,
+                    "velocityX": -3.7004480246565397,
+                    "velocityY": -0.4927496163833412,
+                    "timestamp": 1.440723844605357
+                },
+                {
+                    "x": 2.7005730136886035,
+                    "y": 6.810002161458836,
+                    "heading": 3.406941333368848,
+                    "angularVelocity": 0.670108629261352,
+                    "velocityX": -2.969633223179528,
+                    "velocityY": -0.393586985735394,
+                    "timestamp": 1.5307690848931919
+                },
+                {
+                    "x": 2.500185866956939,
+                    "y": 6.783477694334395,
+                    "heading": 3.465398232800302,
+                    "angularVelocity": 0.6491947741445663,
+                    "velocityX": -2.2254052084387275,
+                    "velocityY": -0.294568230809919,
+                    "timestamp": 1.6208143251810267
+                },
+                {
+                    "x": 2.366700027158657,
+                    "y": 6.765819972243713,
+                    "heading": 3.5094432939271947,
+                    "angularVelocity": 0.4891436902839078,
+                    "velocityX": -1.4824308244565385,
+                    "velocityY": -0.19609833939293356,
+                    "timestamp": 1.7108595654688616
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 3.5331853071795862,
+                    "angularVelocity": 0.26366760948717133,
+                    "velocityX": -0.7407390656679514,
+                    "velocityY": -0.09795045485490664,
+                    "timestamp": 1.8009048057566965
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 3.5331853071795862,
+                    "angularVelocity": 1.378321455008778e-32,
+                    "velocityX": 0,
+                    "velocityY": 4.004611385179353e-34,
+                    "timestamp": 1.8909500460445314
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -3707,7 +11907,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C1 To W2": {
             "waypoints": [
@@ -3718,7 +11934,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 20
                 },
                 {
                     "x": 2.25,
@@ -3748,7 +11964,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C1 To W3": {
             "waypoints": [
@@ -3759,7 +11991,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 23
                 },
                 {
                     "x": 2.3,
@@ -3789,7 +12021,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C1 To S1": {
             "waypoints": [
@@ -3800,19 +12048,146 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 13
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": -2.88,
+                    "heading": 3.4031853071795863,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "angularVelocity": -8.261119414836203e-30,
+                    "velocityX": 2.821294333158614e-31,
+                    "velocityY": -2.62095350113427e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.617797825979087,
+                    "y": 7.446201794633709,
+                    "heading": 3.1534584721408314,
+                    "angularVelocity": 0.15162222909690662,
+                    "velocityX": -0.7007654495821862,
+                    "velocityY": -0.2343111629647132,
+                    "timestamp": 0.08876318611043306
+                },
+                {
+                    "x": 7.493368421156802,
+                    "y": 7.404602777573554,
+                    "heading": 3.178746831508521,
+                    "angularVelocity": 0.28489693164482327,
+                    "velocityX": -1.4018131871435613,
+                    "velocityY": -0.4686516886383534,
+                    "timestamp": 0.17752637222086612
+                },
+                {
+                    "x": 7.306676117263916,
+                    "y": 7.342201100203473,
+                    "heading": 3.213107467103745,
+                    "angularVelocity": 0.38710457678121596,
+                    "velocityX": -2.103262760992579,
+                    "velocityY": -0.7030130406946671,
+                    "timestamp": 0.2662895583312992
+                },
+                {
+                    "x": 7.057674416387051,
+                    "y": 7.2590025582832345,
+                    "heading": 3.2508668949579036,
+                    "angularVelocity": 0.4253951385364178,
+                    "velocityX": -2.8052361771613255,
+                    "velocityY": -0.9373090981171329,
+                    "timestamp": 0.35505274444173224
+                },
+                {
+                    "x": 6.74646520076521,
+                    "y": 7.155112122369572,
+                    "heading": 3.2734870599198675,
+                    "angularVelocity": 0.25483723550559106,
+                    "velocityX": -3.5060617949738107,
+                    "velocityY": -1.1704225644085642,
+                    "timestamp": 0.4438159305521653
+                },
+                {
+                    "x": 6.4280476263908515,
+                    "y": 7.048517625465743,
+                    "heading": 3.2734870708664827,
+                    "angularVelocity": 1.233247689161178e-7,
+                    "velocityX": -3.5872706729821253,
+                    "velocityY": -1.200886331076399,
+                    "timestamp": 0.5325791166625984
+                },
+                {
+                    "x": 6.109630072909297,
+                    "y": 6.941923066153303,
+                    "heading": 3.273487081813265,
+                    "angularVelocity": 1.2332664336181238e-7,
+                    "velocityX": -3.5872704376046,
+                    "velocityY": -1.2008870341690594,
+                    "timestamp": 0.6213423027730315
+                },
+                {
+                    "x": 5.798381254717505,
+                    "y": 6.838018859230789,
+                    "heading": 3.296331970538473,
+                    "angularVelocity": 0.25736895809035,
+                    "velocityX": -3.5065079548134683,
+                    "velocityY": -1.1705777076875172,
+                    "timestamp": 0.7101054888834646
+                },
+                {
+                    "x": 5.549362929627812,
+                    "y": 6.754814274993791,
+                    "heading": 3.3329076351356512,
+                    "angularVelocity": 0.4120589424672841,
+                    "velocityX": -2.8054234643993468,
+                    "velocityY": -0.9373771704727861,
+                    "timestamp": 0.7988686749938977
+                },
+                {
+                    "x": 5.36265371808689,
+                    "y": 6.692406142204076,
+                    "heading": 3.3659874710813016,
+                    "angularVelocity": 0.37267517533582245,
+                    "velocityX": -2.103453241394186,
+                    "velocityY": -0.7030857670337234,
+                    "timestamp": 0.8876318611043308
+                },
+                {
+                    "x": 5.238210282238424,
+                    "y": 6.650801541740841,
+                    "heading": 3.3902750965399324,
+                    "angularVelocity": 0.27362273171986246,
+                    "velocityX": -1.4019712597217382,
+                    "velocityY": -0.4687145908921929,
+                    "timestamp": 0.9763950472147639
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": 0.14544555244060076,
+                    "velocityX": -0.7008567962045305,
+                    "velocityY": -0.2343487503412182,
+                    "timestamp": 1.065158233325197
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": 0,
+                    "velocityX": 1.430834166779729e-32,
+                    "velocityY": -6.582148241954714e-30,
+                    "timestamp": 1.15392141943563
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -3830,7 +12205,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C1 To S2": {
             "waypoints": [
@@ -3841,7 +12232,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 15
                 },
                 {
                     "x": 4.263,
@@ -3853,7 +12244,152 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "angularVelocity": -3.436405776355784e-29,
+                    "velocityX": 4.0035032875632384e-30,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.593933573152452,
+                    "y": 7.445362365518837,
+                    "heading": 3.094092714332116,
+                    "angularVelocity": -0.44270408674328476,
+                    "velocityX": -0.8299762955364275,
+                    "velocityY": -0.2086611977356942,
+                    "timestamp": 0.10369745173495665
+                },
+                {
+                    "x": 7.421555540437275,
+                    "y": 7.402033887236261,
+                    "heading": 3.009154332069554,
+                    "angularVelocity": -0.8190980669386346,
+                    "velocityX": -1.6623169598782952,
+                    "velocityY": -0.41783551627973,
+                    "timestamp": 0.2073949034699133
+                },
+                {
+                    "x": 7.1624840188793355,
+                    "y": 7.336952212665302,
+                    "heading": 2.898930851860923,
+                    "angularVelocity": -1.0629333543342216,
+                    "velocityX": -2.4983402892108755,
+                    "velocityY": -0.6276111271981535,
+                    "timestamp": 0.31109235520486994
+                },
+                {
+                    "x": 6.816593659112812,
+                    "y": 7.250270000971659,
+                    "heading": 2.8098268582890107,
+                    "angularVelocity": -0.8592688834789594,
+                    "velocityX": -3.335572417445675,
+                    "velocityY": -0.8359145788384311,
+                    "timestamp": 0.4147898069398266
+                },
+                {
+                    "x": 6.436271393844499,
+                    "y": 7.15414721538473,
+                    "heading": 2.8098268314636434,
+                    "angularVelocity": -2.586887820849056e-7,
+                    "velocityX": -3.6676143811169655,
+                    "velocityY": -0.9269541727276405,
+                    "timestamp": 0.5184872586747833
+                },
+                {
+                    "x": 6.055949159763292,
+                    "y": 7.058024306401983,
+                    "heading": 2.8098268046376322,
+                    "angularVelocity": -2.5869499105961153e-7,
+                    "velocityX": -3.6676140803660213,
+                    "velocityY": -0.9269553626876206,
+                    "timestamp": 0.62218471040974
+                },
+                {
+                    "x": 5.675627085616213,
+                    "y": 6.961900764622196,
+                    "heading": 2.809826777811619,
+                    "angularVelocity": -2.5869501157187357e-7,
+                    "velocityX": -3.6676125380511024,
+                    "velocityY": -0.9269614650269816,
+                    "timestamp": 0.7258821621446967
+                },
+                {
+                    "x": 5.306947068096014,
+                    "y": 6.827887110574328,
+                    "heading": 2.8098267501768928,
+                    "angularVelocity": -2.6649378350721345e-7,
+                    "velocityX": -3.555343080778155,
+                    "velocityY": -1.2923524330219798,
+                    "timestamp": 0.8295796138796534
+                },
+                {
+                    "x": 4.978327022444636,
+                    "y": 6.61365948285698,
+                    "heading": 2.809826716038805,
+                    "angularVelocity": -3.2920855048251957e-7,
+                    "velocityX": -3.1690272051362296,
+                    "velocityY": -2.065890956172196,
+                    "timestamp": 0.9332770656146101
+                },
+                {
+                    "x": 4.706934591498499,
+                    "y": 6.330409136170975,
+                    "heading": 2.809826716485968,
+                    "angularVelocity": 4.312190502568527e-9,
+                    "velocityX": -2.617156221348633,
+                    "velocityY": -2.731507302705652,
+                    "timestamp": 1.0369745173495668
+                },
+                {
+                    "x": 4.519067005656564,
+                    "y": 6.028361651029434,
+                    "heading": 2.9086869877879162,
+                    "angularVelocity": 0.9533529479069416,
+                    "velocityX": -1.8116895130858792,
+                    "velocityY": -2.9127763516651948,
+                    "timestamp": 1.1406719690845235
+                },
+                {
+                    "x": 4.388194794926867,
+                    "y": 5.795490351049463,
+                    "heading": 3.0157821623071808,
+                    "angularVelocity": 1.0327657307624416,
+                    "velocityX": -1.2620581175340149,
+                    "velocityY": -2.245680063336305,
+                    "timestamp": 1.2443694208194802
+                },
+                {
+                    "x": 4.304089821797064,
+                    "y": 5.638764448430372,
+                    "heading": 3.096687373599028,
+                    "angularVelocity": 0.7802044306606039,
+                    "velocityX": -0.8110611372086162,
+                    "velocityY": -1.5113766056631632,
+                    "timestamp": 1.348066872554437
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0.41768264963403856,
+                    "velocityX": -0.39624717010474564,
+                    "velocityY": -0.7595601156303902,
+                    "timestamp": 1.4517643242893936
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 1.3748988870988578e-26,
+                    "velocityX": 7.122460517165917e-28,
+                    "velocityY": 4.07656513453054e-28,
+                    "timestamp": 1.5554617760243503
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -3862,16 +12398,32 @@
                     "type": "StopPoint"
                 },
                 {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
                 }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            ]
         },
         "C1 To S3": {
             "waypoints": [
@@ -3912,7 +12464,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C1 To S4": {
             "waypoints": [
@@ -3953,7 +12521,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C1 To C2": {
             "waypoints": [
@@ -3964,7 +12548,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 9
                 },
                 {
                     "x": 7.68,
@@ -3994,7 +12578,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C1 To C3": {
             "waypoints": [
@@ -4005,7 +12605,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 14
                 },
                 {
                     "x": 7.68,
@@ -4035,7 +12635,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C1 To C4": {
             "waypoints": [
@@ -4046,7 +12662,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 18
                 },
                 {
                     "x": 7.68,
@@ -4058,7 +12674,179 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "angularVelocity": 1.0688205894738666e-32,
+                    "velocityX": 4.722903296583099e-32,
+                    "velocityY": -7.052966104933725e-38,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.680000100019751,
+                    "y": 7.385155846048263,
+                    "heading": 3.1400000209558985,
+                    "angularVelocity": 2.1151775994912985e-7,
+                    "velocityX": 0.0000010095466114596713,
+                    "velocityY": -0.8260917454272356,
+                    "timestamp": 0.09907392781101554
+                },
+                {
+                    "x": 7.680000295060445,
+                    "y": 7.221467541258139,
+                    "heading": 3.1400000632436256,
+                    "angularVelocity": 4.2682995289468236e-7,
+                    "velocityX": 0.000001968637858711507,
+                    "velocityY": -1.6521834594299558,
+                    "timestamp": 0.19814785562203108
+                },
+                {
+                    "x": 7.680000579860811,
+                    "y": 6.975935091856332,
+                    "heading": 3.140000126701719,
+                    "angularVelocity": 6.405123969521912e-7,
+                    "velocityX": 0.0000028746245916429767,
+                    "velocityY": -2.4782751105836276,
+                    "timestamp": 0.2972217834330466
+                },
+                {
+                    "x": 7.680000948789776,
+                    "y": 6.648558516522933,
+                    "heading": 3.140000210348385,
+                    "angularVelocity": 8.442850564566377e-7,
+                    "velocityX": 0.0000037237742032736077,
+                    "velocityY": -3.3043665731903222,
+                    "timestamp": 0.39629571124406215
+                },
+                {
+                    "x": 7.68000127512063,
+                    "y": 6.273767735073521,
+                    "heading": 3.1400002103483864,
+                    "angularVelocity": 1.8797601612350415e-14,
+                    "velocityX": 0.0000032938116008237123,
+                    "velocityY": -3.782940575085686,
+                    "timestamp": 0.4953696390550777
+                },
+                {
+                    "x": 7.680001534050369,
+                    "y": 5.898976953624012,
+                    "heading": 3.140000210348386,
+                    "angularVelocity": -4.168435621307512e-16,
+                    "velocityX": 0.0000026135000838056266,
+                    "velocityY": -3.782940575086706,
+                    "timestamp": 0.5944435668660932
+                },
+                {
+                    "x": 7.680001718421092,
+                    "y": 5.524186172174471,
+                    "heading": 3.14000021034838,
+                    "angularVelocity": -6.61889670232267e-14,
+                    "velocityX": 0.000001860940720346917,
+                    "velocityY": -3.782940575087075,
+                    "timestamp": 0.6935174946771088
+                },
+                {
+                    "x": 7.680001822997413,
+                    "y": 5.14939539072491,
+                    "heading": 3.1400002103483726,
+                    "angularVelocity": -8.462091393548897e-14,
+                    "velocityX": 0.000001055538133316693,
+                    "velocityY": -3.7829405750872986,
+                    "timestamp": 0.7925914224881243
+                },
+                {
+                    "x": 7.6800018440805555,
+                    "y": 4.774604609275343,
+                    "heading": 3.140000210348366,
+                    "angularVelocity": -6.932568582141466e-14,
+                    "velocityX": 2.128020680091996e-7,
+                    "velocityY": -3.782940575087379,
+                    "timestamp": 0.8916653502991398
+                },
+                {
+                    "x": 7.680001779518267,
+                    "y": 4.39981382782578,
+                    "heading": 3.1400002103483597,
+                    "angularVelocity": -6.86156871514602e-14,
+                    "velocityX": -6.516576918751784e-7,
+                    "velocityY": -3.782940575087321,
+                    "timestamp": 0.9907392781101554
+                },
+                {
+                    "x": 7.680001628799847,
+                    "y": 4.025023046376237,
+                    "heading": 3.140000210348353,
+                    "angularVelocity": -7.135901447410535e-14,
+                    "velocityX": -0.0000015212721994966025,
+                    "velocityY": -3.782940575087123,
+                    "timestamp": 1.089813205921171
+                },
+                {
+                    "x": 7.680001391869683,
+                    "y": 3.6502322649267236,
+                    "heading": 3.140000210348346,
+                    "angularVelocity": -7.415296718915011e-14,
+                    "velocityX": -0.0000023914480342122818,
+                    "velocityY": -3.7829405750867786,
+                    "timestamp": 1.1888871337321865
+                },
+                {
+                    "x": 7.680001068770943,
+                    "y": 3.27544148347729,
+                    "heading": 3.1400002103483433,
+                    "angularVelocity": -3.3132922253880277e-14,
+                    "velocityX": -0.0000032611884784431762,
+                    "velocityY": -3.782940575085905,
+                    "timestamp": 1.287961061543202
+                },
+                {
+                    "x": 7.680000658927351,
+                    "y": 2.9480649081437575,
+                    "heading": 3.140000122293031,
+                    "angularVelocity": -8.887836013747625e-7,
+                    "velocityX": -0.000004136744686858471,
+                    "velocityY": -3.3043665731916545,
+                    "timestamp": 1.3870349893542175
+                },
+                {
+                    "x": 7.680000338206625,
+                    "y": 2.7025324587418944,
+                    "heading": 3.140000057972934,
+                    "angularVelocity": -6.492129951120484e-7,
+                    "velocityX": -0.0000032371856464875894,
+                    "velocityY": -2.4782751105841743,
+                    "timestamp": 1.486108917165233
+                },
+                {
+                    "x": 7.680000115622722,
+                    "y": 2.5388441539517457,
+                    "heading": 3.140000017927834,
+                    "angularVelocity": -4.0419405265238827e-7,
+                    "velocityX": -0.000002246644408273398,
+                    "velocityY": -1.6521834594301972,
+                    "timestamp": 1.5851828449762486
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": -1.8095406608446286e-7,
+                    "velocityX": -0.0000011670347327659162,
+                    "velocityY": -0.8260917454273138,
+                    "timestamp": 1.6842567727872642
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": -2.451701531139976e-31,
+                    "velocityX": 0,
+                    "velocityY": -1.4987552972984166e-37,
+                    "timestamp": 1.7833307005982797
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -4076,7 +12864,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C1 To C5": {
             "waypoints": [
@@ -4087,7 +12891,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 23
                 },
                 {
                     "x": 7.68,
@@ -4099,7 +12903,224 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "angularVelocity": 1.8987898957165807e-31,
+                    "velocityX": -1.39380671571913e-32,
+                    "velocityY": -1.373126916943459e-36,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.680000133861116,
+                    "y": 7.389044729512878,
+                    "heading": 3.1400000003198203,
+                    "angularVelocity": 3.307649058744193e-9,
+                    "velocityX": 0.000001384414398977881,
+                    "velocityY": -0.8062267267090539,
+                    "timestamp": 0.0966914986871212
+                },
+                {
+                    "x": 7.6800003968709785,
+                    "y": 7.233134191633238,
+                    "heading": 3.1400000013587985,
+                    "angularVelocity": 1.0745310107378493e-8,
+                    "velocityX": 0.0000027200926202754936,
+                    "velocityY": -1.6124534214132102,
+                    "timestamp": 0.1933829973742424
+                },
+                {
+                    "x": 7.680000784081611,
+                    "y": 6.9992683925502845,
+                    "heading": 3.1400000030373074,
+                    "angularVelocity": 1.735946006644181e-8,
+                    "velocityX": 0.0000040045979798072784,
+                    "velocityY": -2.4186800521075664,
+                    "timestamp": 0.2900744960613636
+                },
+                {
+                    "x": 7.680001290213695,
+                    "y": 6.687447350831601,
+                    "heading": 3.1400000045486194,
+                    "angularVelocity": 1.5630280874799268e-8,
+                    "velocityX": 0.000005234503549587981,
+                    "velocityY": -3.2249064907728675,
+                    "timestamp": 0.3867659947484848
+                },
+                {
+                    "x": 7.680001791921761,
+                    "y": 6.32166915785587,
+                    "heading": 3.140000004548618,
+                    "angularVelocity": -2.2141915496959812e-14,
+                    "velocityX": 0.0000051887503514693284,
+                    "velocityY": -3.782940568118399,
+                    "timestamp": 0.48345749343560596
+                },
+                {
+                    "x": 7.6800022292578305,
+                    "y": 5.955890964879955,
+                    "heading": 3.1400000045486127,
+                    "angularVelocity": -6.389600617895948e-14,
+                    "velocityX": 0.000004523003602297052,
+                    "velocityY": -3.782940568120494,
+                    "timestamp": 0.5801489921227272
+                },
+                {
+                    "x": 7.680002595183966,
+                    "y": 5.590112771903998,
+                    "heading": 3.140000004548604,
+                    "angularVelocity": -1.1343594252599792e-13,
+                    "velocityX": 0.0000037844699136287606,
+                    "velocityY": -3.782940568121032,
+                    "timestamp": 0.6768404908098484
+                },
+                {
+                    "x": 7.680002884490285,
+                    "y": 5.224334578928017,
+                    "heading": 3.140000004548593,
+                    "angularVelocity": -1.3664646403255707e-13,
+                    "velocityX": 0.0000029920547840648683,
+                    "velocityY": -3.7829405681213992,
+                    "timestamp": 0.7735319894969697
+                },
+                {
+                    "x": 7.680003093405051,
+                    "y": 4.858556385952021,
+                    "heading": 3.140000004548582,
+                    "angularVelocity": -1.389367975129708e-13,
+                    "velocityX": 0.0000021606317619867176,
+                    "velocityY": -3.782940568121637,
+                    "timestamp": 0.8702234881840909
+                },
+                {
+                    "x": 7.6800032195393335,
+                    "y": 4.492778192976016,
+                    "heading": 3.1400000045485705,
+                    "angularVelocity": -1.422935768803712e-13,
+                    "velocityX": 0.0000013045020379807613,
+                    "velocityY": -3.782940568121783,
+                    "timestamp": 0.9669149868712121
+                },
+                {
+                    "x": 7.680003262132589,
+                    "y": 4.127000000000006,
+                    "heading": 3.1400000045485568,
+                    "angularVelocity": -1.5798635695024144e-13,
+                    "velocityX": 4.405066330821885e-7,
+                    "velocityY": -3.782940568121863,
+                    "timestamp": 1.0636064855583334
+                },
+                {
+                    "x": 7.680003221112365,
+                    "y": 3.7612218070239956,
+                    "heading": 3.1400000045485426,
+                    "angularVelocity": -1.6204973501388964e-13,
+                    "velocityX": -4.2423808567477835e-7,
+                    "velocityY": -3.782940568121861,
+                    "timestamp": 1.1602979842454546
+                },
+                {
+                    "x": 7.680003096442586,
+                    "y": 3.3954436140479913,
+                    "heading": 3.14000000454853,
+                    "angularVelocity": -1.5384845387063427e-13,
+                    "velocityX": -0.0000012893559013190512,
+                    "velocityY": -3.782940568121779,
+                    "timestamp": 1.2569894829325758
+                },
+                {
+                    "x": 7.680002888734547,
+                    "y": 3.029665421071995,
+                    "heading": 3.140000004548518,
+                    "angularVelocity": -1.4981778939731422e-13,
+                    "velocityX": -0.0000021481515920122195,
+                    "velocityY": -3.7829405681216324,
+                    "timestamp": 1.353680981619697
+                },
+                {
+                    "x": 7.680002600247316,
+                    "y": 2.663887228096014,
+                    "heading": 3.1400000045485066,
+                    "angularVelocity": -1.4657528609032921e-13,
+                    "velocityX": -0.0000029835836472893606,
+                    "velocityY": -3.782940568121397,
+                    "timestamp": 1.4503724803068183
+                },
+                {
+                    "x": 7.680002234659543,
+                    "y": 2.2981090351200577,
+                    "heading": 3.140000004548497,
+                    "angularVelocity": -1.235449081893301e-13,
+                    "velocityX": -0.000003780970508619144,
+                    "velocityY": -3.782940568121035,
+                    "timestamp": 1.5470639789939395
+                },
+                {
+                    "x": 7.680001797111122,
+                    "y": 1.9323308421441414,
+                    "heading": 3.1400000045484906,
+                    "angularVelocity": -7.160301715418214e-14,
+                    "velocityX": -0.0000045251997748623165,
+                    "velocityY": -3.7829405681205035,
+                    "timestamp": 1.6437554776810608
+                },
+                {
+                    "x": 7.6800012945835014,
+                    "y": 1.566552649168411,
+                    "heading": 3.140000004548489,
+                    "angularVelocity": -2.2663051081772477e-14,
+                    "velocityX": -0.000005197226332013039,
+                    "velocityY": -3.7829405681183954,
+                    "timestamp": 1.740446976368182
+                },
+                {
+                    "x": 7.680000786831981,
+                    "y": 1.2547316074497201,
+                    "heading": 3.140000001978014,
+                    "angularVelocity": -2.6584233523758634e-8,
+                    "velocityX": -0.0000052512520234957685,
+                    "velocityY": -3.2249064907729323,
+                    "timestamp": 1.8371384750553033
+                },
+                {
+                    "x": 7.680000398309784,
+                    "y": 1.0208658083667639,
+                    "heading": 3.140000000933386,
+                    "angularVelocity": -1.0803673621572709e-8,
+                    "velocityX": -0.000004018162386759126,
+                    "velocityY": -2.4186800521075957,
+                    "timestamp": 1.9338299737424245
+                },
+                {
+                    "x": 7.680000134361733,
+                    "y": 0.8649552704871222,
+                    "heading": 3.1400000003740773,
+                    "angularVelocity": -5.784435922761806e-9,
+                    "velocityX": -0.0000027297955176854786,
+                    "velocityY": -1.612453421413225,
+                    "timestamp": 2.0305214724295455
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": -3.8687549359618056e-9,
+                    "velocityX": -0.000001389591862211489,
+                    "velocityY": -0.8062267267090597,
+                    "timestamp": 2.1272129711166667
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": 5.6019143334006085e-31,
+                    "velocityX": 0,
+                    "velocityY": 9.268974451742519e-38,
+                    "timestamp": 2.223904469803788
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -4117,7 +13138,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C2 To W1": {
             "waypoints": [
@@ -4128,19 +13165,218 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 21
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 3.5331853071795862,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": -3.1814997382550796e-29,
+                    "velocityX": -1.8229862739599686e-32,
+                    "velocityY": -3.244948758863147e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.617836558696546,
+                    "y": 5.830157448001668,
+                    "heading": 3.1302887872729133,
+                    "angularVelocity": -0.10560741421513488,
+                    "velocityX": -0.676014466912156,
+                    "velocityY": 0.36058033572491816,
+                    "timestamp": 0.0919557854839981
+                },
+                {
+                    "x": 7.493509680194616,
+                    "y": 5.896472329123028,
+                    "heading": 3.1108653372490442,
+                    "angularVelocity": -0.21122597041214858,
+                    "velocityX": -1.3520288891835412,
+                    "velocityY": 0.7211605096114337,
+                    "timestamp": 0.1839115709679962
+                },
+                {
+                    "x": 7.307019376427131,
+                    "y": 5.9959446237909395,
+                    "heading": 3.0817278950569205,
+                    "angularVelocity": -0.316863610470641,
+                    "velocityX": -2.028043181686891,
+                    "velocityY": 1.0817404706441378,
+                    "timestamp": 0.27586735645199434
+                },
+                {
+                    "x": 7.058365675689159,
+                    "y": 6.128574306111159,
+                    "heading": 3.0428740808433132,
+                    "angularVelocity": -0.4225271309369522,
+                    "velocityX": -2.704057166487276,
+                    "velocityY": 1.442320150082345,
+                    "timestamp": 0.3678231419359924
+                },
+                {
+                    "x": 6.75143523206277,
+                    "y": 6.292289169474125,
+                    "heading": 3.0428740759198862,
+                    "angularVelocity": -5.354123851149769e-8,
+                    "velocityX": -3.3378045982740363,
+                    "velocityY": 1.7803650145694723,
+                    "timestamp": 0.4597789274199905
+                },
+                {
+                    "x": 6.443010139077041,
+                    "y": 6.453170540992539,
+                    "heading": 3.042874070986951,
+                    "angularVelocity": -5.3644645461672824e-8,
+                    "velocityX": -3.354058598514181,
+                    "velocityY": 1.7495513813691466,
+                    "timestamp": 0.5517347129039886
+                },
+                {
+                    "x": 6.108481192244814,
+                    "y": 6.548560466077805,
+                    "heading": 3.0428740880877947,
+                    "angularVelocity": 1.8596810871163974e-7,
+                    "velocityX": -3.637932568043149,
+                    "velocityY": 1.037345552356419,
+                    "timestamp": 0.6436904983879866
+                },
+                {
+                    "x": 5.76231566928878,
+                    "y": 6.582886564044467,
+                    "heading": 3.0428741422485728,
+                    "angularVelocity": 5.889871735366024e-7,
+                    "velocityX": -3.764477907877501,
+                    "velocityY": 0.3732891605024142,
+                    "timestamp": 0.7356462838719847
+                },
+                {
+                    "x": 5.41614197674267,
+                    "y": 6.617130174737203,
+                    "heading": 3.042874196415433,
+                    "angularVelocity": 5.890533147506573e-7,
+                    "velocityX": -3.764566750466717,
+                    "velocityY": 0.37239212859200393,
+                    "timestamp": 0.8276020693559828
+                },
+                {
+                    "x": 5.068621146465333,
+                    "y": 6.632561444279093,
+                    "heading": 3.0428742397901987,
+                    "angularVelocity": 4.716915377702693e-7,
+                    "velocityX": -3.779216592498277,
+                    "velocityY": 0.16781183979527767,
+                    "timestamp": 0.9195578548399809
+                },
+                {
+                    "x": 4.721100288864093,
+                    "y": 6.647992098459583,
+                    "heading": 3.042874283164768,
+                    "angularVelocity": 4.716894023860314e-7,
+                    "velocityX": -3.779216889640001,
+                    "velocityY": 0.16780514786832676,
+                    "timestamp": 1.011513640323979
+                },
+                {
+                    "x": 4.3735794260416885,
+                    "y": 6.663422635051171,
+                    "heading": 3.042874326539347,
+                    "angularVelocity": 4.716895035423872e-7,
+                    "velocityX": -3.779216946419108,
+                    "velocityY": 0.16780386911352246,
+                    "timestamp": 1.1034694258079771
+                },
+                {
+                    "x": 4.026058559390409,
+                    "y": 6.678853085409858,
+                    "heading": 3.042874369913944,
+                    "angularVelocity": 4.716897058235115e-7,
+                    "velocityX": -3.779216988057318,
+                    "velocityY": 0.1678029313486884,
+                    "timestamp": 1.1954252112919752
+                },
+                {
+                    "x": 3.678537689463977,
+                    "y": 6.69428346200604,
+                    "heading": 3.042874413288561,
+                    "angularVelocity": 4.7168991769457604e-7,
+                    "velocityX": -3.779217023673906,
+                    "velocityY": 0.16780212919682533,
+                    "timestamp": 1.2873809967759733
+                },
+                {
+                    "x": 3.3310168167092282,
+                    "y": 6.7097137749028235,
+                    "heading": 3.042874456663209,
+                    "angularVelocity": 4.7169025361622227e-7,
+                    "velocityX": -3.7792170544312707,
+                    "velocityY": 0.16780143647915202,
+                    "timestamp": 1.3793367822599714
+                },
+                {
+                    "x": 2.9915174484895957,
+                    "y": 6.725380176084037,
+                    "heading": 3.069608362610464,
+                    "angularVelocity": 0.2907256547975106,
+                    "velocityX": -3.6919848646033437,
+                    "velocityY": 0.1703688473624042,
+                    "timestamp": 1.4712925677439694
+                },
+                {
+                    "x": 2.7138500007590802,
+                    "y": 6.7384126132729225,
+                    "heading": 3.2277640151767573,
+                    "angularVelocity": 1.7199097559099776,
+                    "velocityX": -3.019575617445345,
+                    "velocityY": 0.14172503796570737,
+                    "timestamp": 1.5632483532279675
+                },
+                {
+                    "x": 2.5064920875678816,
+                    "y": 6.747862712816966,
+                    "heading": 3.3721576212980104,
+                    "angularVelocity": 1.570250369362352,
+                    "velocityX": -2.2549740845537376,
+                    "velocityY": 0.1027678627756077,
+                    "timestamp": 1.6552041387119656
+                },
+                {
+                    "x": 2.368714185355249,
+                    "y": 6.753998249997193,
+                    "heading": 3.477486882189963,
+                    "angularVelocity": 1.14543375751254,
+                    "velocityX": -1.4983059683243996,
+                    "velocityY": 0.06672268795197085,
+                    "timestamp": 1.7471599241959637
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 3.5331853071795862,
+                    "angularVelocity": 0.6057087620584267,
+                    "velocityX": -0.7472524430472809,
+                    "velocityY": 0.03264340559983963,
+                    "timestamp": 1.8391157096799617
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 3.5331853071795862,
+                    "angularVelocity": -1.2870903265044466e-29,
+                    "velocityX": 0,
+                    "velocityY": 1.2655145174510225e-29,
+                    "timestamp": 1.9310714951639598
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -4158,7 +13394,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C2 To W2": {
             "waypoints": [
@@ -4169,7 +13421,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 19
                 },
                 {
                     "x": 2.25,
@@ -4181,7 +13433,188 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": -1.4170612284410768e-31,
+                    "velocityX": -1.067311144931875e-32,
+                    "velocityY": -8.392488596494602e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.598760740640866,
+                    "y": 5.839328600808308,
+                    "heading": 3.1292747695902707,
+                    "angularVelocity": -0.10229725511121834,
+                    "velocityX": -0.7748601122982537,
+                    "velocityY": 0.40373022396426267,
+                    "timestamp": 0.10484377511467942
+                },
+                {
+                    "x": 7.436282224463055,
+                    "y": 5.923985787899839,
+                    "heading": 3.1078233746588477,
+                    "angularVelocity": -0.2046034197830007,
+                    "velocityX": -1.5497202003656476,
+                    "velocityY": 0.8074603093882498,
+                    "timestamp": 0.20968755022935884
+                },
+                {
+                    "x": 7.19256446073334,
+                    "y": 6.050971542878038,
+                    "heading": 3.0756442181443964,
+                    "angularVelocity": -0.30692481722690457,
+                    "velocityX": -2.3245802000465203,
+                    "velocityY": 1.2111902193458808,
+                    "timestamp": 0.31453132534403827
+                },
+                {
+                    "x": 6.8676074741032105,
+                    "y": 6.220285840922999,
+                    "heading": 3.032735095907097,
+                    "angularVelocity": -0.4092672377579469,
+                    "velocityX": -3.099439964601499,
+                    "velocityY": 1.614919892571246,
+                    "timestamp": 0.4193751004587177
+                },
+                {
+                    "x": 6.515870869646968,
+                    "y": 6.403553456428483,
+                    "heading": 3.032735091775592,
+                    "angularVelocity": -3.94062953033438e-8,
+                    "velocityX": -3.3548639780617218,
+                    "velocityY": 1.7480066442192022,
+                    "timestamp": 0.5242188755733971
+                },
+                {
+                    "x": 6.1437635937584405,
+                    "y": 6.5408190150249235,
+                    "heading": 3.032735095078602,
+                    "angularVelocity": 3.1504111682057e-8,
+                    "velocityX": -3.5491594563579234,
+                    "velocityY": 1.3092389934099258,
+                    "timestamp": 0.6290626506880765
+                },
+                {
+                    "x": 5.750083988314003,
+                    "y": 6.589006339833596,
+                    "heading": 3.032735167102088,
+                    "angularVelocity": 6.86960058704524e-7,
+                    "velocityX": -3.754916350673425,
+                    "velocityY": 0.45961073755656895,
+                    "timestamp": 0.733906425802756
+                },
+                {
+                    "x": 5.355289339691467,
+                    "y": 6.626991015951962,
+                    "heading": 3.032735238530673,
+                    "angularVelocity": 6.812858975549085e-7,
+                    "velocityX": -3.7655516332820365,
+                    "velocityY": 0.36229786724885477,
+                    "timestamp": 0.8387502009174355
+                },
+                {
+                    "x": 4.960619519356147,
+                    "y": 6.587730487852754,
+                    "heading": 3.032735268670057,
+                    "angularVelocity": 2.874694691913672e-7,
+                    "velocityX": -3.76436102099172,
+                    "velocityY": -0.3744669443299256,
+                    "timestamp": 0.9435939760321149
+                },
+                {
+                    "x": 4.585499557692294,
+                    "y": 6.45892571115183,
+                    "heading": 3.032735273052689,
+                    "angularVelocity": 4.180154788476782e-8,
+                    "velocityX": -3.577894455379368,
+                    "velocityY": -1.2285400498030161,
+                    "timestamp": 1.0484377511467944
+                },
+                {
+                    "x": 4.215352940634833,
+                    "y": 6.316457686051506,
+                    "heading": 3.0327352773918506,
+                    "angularVelocity": 4.1386927047063915e-8,
+                    "velocityX": -3.530458691062866,
+                    "velocityY": -1.358860122544141,
+                    "timestamp": 1.1532815262614737
+                },
+                {
+                    "x": 3.8452063807699624,
+                    "y": 6.173989512359033,
+                    "heading": 3.0327352817310285,
+                    "angularVelocity": 4.138708047875789e-8,
+                    "velocityX": -3.530458145559901,
+                    "velocityY": -1.3588615398161574,
+                    "timestamp": 1.258125301376153
+                },
+                {
+                    "x": 3.47505985812915,
+                    "y": 6.031521241954782,
+                    "heading": 3.0327352860702175,
+                    "angularVelocity": 4.138718836240866e-8,
+                    "velocityX": -3.530457790516812,
+                    "velocityY": -1.3588624622531746,
+                    "timestamp": 1.3629690764908324
+                },
+                {
+                    "x": 3.1049133713898427,
+                    "y": 5.889052878274973,
+                    "heading": 3.0327352904094194,
+                    "angularVelocity": 4.138730707919649e-8,
+                    "velocityX": -3.53045744808823,
+                    "velocityY": -1.3588633519155102,
+                    "timestamp": 1.4678128516055118
+                },
+                {
+                    "x": 2.762948024328836,
+                    "y": 5.75743175203549,
+                    "heading": 3.07563992564996,
+                    "angularVelocity": 0.4092244407797366,
+                    "velocityX": -3.2616657182265767,
+                    "velocityY": -1.255402393661561,
+                    "timestamp": 1.5726566267201911
+                },
+                {
+                    "x": 2.5064740111386725,
+                    "y": 5.658715887693948,
+                    "heading": 3.107819702204835,
+                    "angularVelocity": 0.30693073117289776,
+                    "velocityX": -2.4462493162768064,
+                    "velocityY": -0.9415519827816698,
+                    "timestamp": 1.6775004018348705
+                },
+                {
+                    "x": 2.3354913370033428,
+                    "y": 5.592905299768155,
+                    "heading": 3.129273261906622,
+                    "angularVelocity": 0.20462406736424307,
+                    "velocityX": -1.6308328648821255,
+                    "velocityY": -0.6277014334309162,
+                    "timestamp": 1.7823441769495498
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0.1023116353989093,
+                    "velocityX": -0.8154164318275585,
+                    "velocityY": -0.3138507720860239,
+                    "timestamp": 1.8871879520642292
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 6.542152862475765e-30,
+                    "velocityX": 0,
+                    "velocityY": -3.777031189999863e-33,
+                    "timestamp": 1.9920317271789085
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -4199,7 +13632,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C2 To W3": {
             "waypoints": [
@@ -4210,7 +13659,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 22
                 },
                 {
                     "x": 2.3,
@@ -4222,7 +13671,215 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": 1.7201358648458989e-31,
+                    "velocityX": -4.9530751339705636e-33,
+                    "velocityY": 8.276147677005227e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.605093588171019,
+                    "y": 5.8310278116994025,
+                    "heading": 3.10842986087817,
+                    "angularVelocity": -0.3169118675748513,
+                    "velocityX": -0.7519362133453025,
+                    "velocityY": 0.3415828265288231,
+                    "timestamp": 0.0996180400671603
+                },
+                {
+                    "x": 7.455282578249206,
+                    "y": 5.899083098798643,
+                    "heading": 3.0452020176570493,
+                    "angularVelocity": -0.6347027423797291,
+                    "velocityX": -1.5038542197860265,
+                    "velocityY": 0.6831622771674665,
+                    "timestamp": 0.1992360801343206
+                },
+                {
+                    "x": 7.230570445252572,
+                    "y": 6.001166640275022,
+                    "heading": 2.9501837123513917,
+                    "angularVelocity": -0.9538262873029698,
+                    "velocityX": -2.2557373428059657,
+                    "velocityY": 1.0247495474469857,
+                    "timestamp": 0.2988541202014809
+                },
+                {
+                    "x": 6.930961848120999,
+                    "y": 6.137281948011585,
+                    "heading": 2.82324695361152,
+                    "angularVelocity": -1.274234653224393,
+                    "velocityX": -3.007573697791919,
+                    "velocityY": 1.3663720712111798,
+                    "timestamp": 0.3984721602686412
+                },
+                {
+                    "x": 6.58788061707114,
+                    "y": 6.293200302520464,
+                    "heading": 2.8232469405560607,
+                    "angularVelocity": -1.3105517366390336e-7,
+                    "velocityX": -3.4439668840961075,
+                    "velocityY": 1.565161836187118,
+                    "timestamp": 0.4980902003358015
+                },
+                {
+                    "x": 6.242826706460482,
+                    "y": 6.444702986624836,
+                    "heading": 2.8232469291979005,
+                    "angularVelocity": -1.1401709907689636e-7,
+                    "velocityX": -3.46376931706376,
+                    "velocityY": 1.5208358245377296,
+                    "timestamp": 0.5977082404029618
+                },
+                {
+                    "x": 5.895869664946667,
+                    "y": 6.591795052657303,
+                    "heading": 2.823246919432399,
+                    "angularVelocity": -9.802945216969962e-8,
+                    "velocityX": -3.482873596789339,
+                    "velocityY": 1.4765605299331361,
+                    "timestamp": 0.6973262804701221
+                },
+                {
+                    "x": 5.529327027819563,
+                    "y": 6.679326525812769,
+                    "heading": 2.8232469090031302,
+                    "angularVelocity": -1.0469256638864389e-7,
+                    "velocityX": -3.679480512565681,
+                    "velocityY": 0.8786709023431315,
+                    "timestamp": 0.7969443205372824
+                },
+                {
+                    "x": 5.152516966350426,
+                    "y": 6.684752095540444,
+                    "heading": 2.823246873859642,
+                    "angularVelocity": -3.5278236840852576e-7,
+                    "velocityX": -3.7825484341500837,
+                    "velocityY": 0.05446372689140277,
+                    "timestamp": 0.8965623606044426
+                },
+                {
+                    "x": 4.783606202844696,
+                    "y": 6.607810054904123,
+                    "heading": 2.823246710781253,
+                    "angularVelocity": -0.0000016370367136474411,
+                    "velocityX": -3.703252576109883,
+                    "velocityY": -0.7723705524064612,
+                    "timestamp": 0.9961804006716029
+                },
+                {
+                    "x": 4.4403804832702125,
+                    "y": 6.452210088653612,
+                    "heading": 2.8232466000560583,
+                    "angularVelocity": -0.0000011114974222563995,
+                    "velocityX": -3.445417309385819,
+                    "velocityY": -1.5619657458188145,
+                    "timestamp": 1.095798440738763
+                },
+                {
+                    "x": 4.124524669473561,
+                    "y": 6.246661186403647,
+                    "heading": 2.823246495691381,
+                    "angularVelocity": -0.0000010476483740088647,
+                    "velocityX": -3.170668822471396,
+                    "velocityY": -2.0633702701979306,
+                    "timestamp": 1.1954164808059233
+                },
+                {
+                    "x": 3.808669145932633,
+                    "y": 6.041111838134177,
+                    "heading": 2.82324639132671,
+                    "angularVelocity": -0.0000010476483076103719,
+                    "velocityX": -3.1706659087850397,
+                    "velocityY": -2.063374747494463,
+                    "timestamp": 1.2950345208730836
+                },
+                {
+                    "x": 3.492813679718565,
+                    "y": 5.835562401773946,
+                    "heading": 2.8232462869620267,
+                    "angularVelocity": -0.0000010476484251301315,
+                    "velocityX": -3.1706653333184023,
+                    "velocityY": -2.063375631779686,
+                    "timestamp": 1.3946525609402438
+                },
+                {
+                    "x": 3.176958352678971,
+                    "y": 5.630012751552842,
+                    "heading": 2.8232461825973036,
+                    "angularVelocity": -0.000001047648830467493,
+                    "velocityX": -3.1706639362373616,
+                    "velocityY": -2.0633777785883685,
+                    "timestamp": 1.494270601007404
+                },
+                {
+                    "x": 2.880984417024366,
+                    "y": 5.396743220702011,
+                    "heading": 2.8232460713596375,
+                    "angularVelocity": -0.0000011166417889330504,
+                    "velocityX": -2.971087721210592,
+                    "velocityY": -2.3416394329136074,
+                    "timestamp": 1.5938886410745643
+                },
+                {
+                    "x": 2.6437210391842343,
+                    "y": 5.10516988637377,
+                    "heading": 2.820727987751454,
+                    "angularVelocity": -0.02527738556676537,
+                    "velocityX": -2.3817310366694042,
+                    "velocityY": -2.926912978127948,
+                    "timestamp": 1.6935066811417245
+                },
+                {
+                    "x": 2.4878211519832623,
+                    "y": 4.818636614610494,
+                    "heading": 2.7673750452255876,
+                    "angularVelocity": -0.5355751075798869,
+                    "velocityX": -1.5649764550262995,
+                    "velocityY": -2.876319104151233,
+                    "timestamp": 1.7931247212088848
+                },
+                {
+                    "x": 2.3880615291520813,
+                    "y": 4.591997027963517,
+                    "heading": 2.751203678024379,
+                    "angularVelocity": -0.1623337217867963,
+                    "velocityX": -1.0014212562696991,
+                    "velocityY": -2.275085782597012,
+                    "timestamp": 1.892742761276045
+                },
+                {
+                    "x": 2.3279705887355506,
+                    "y": 4.437873884538665,
+                    "heading": 2.748674861705227,
+                    "angularVelocity": -0.025385124194849635,
+                    "velocityX": -0.6032134378072334,
+                    "velocityY": -1.547140892562696,
+                    "timestamp": 1.9923608013432053
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 0.013302191991328724,
+                    "velocityX": -0.28077834814551317,
+                    "velocityY": -0.7817247206044561,
+                    "timestamp": 2.0919788414103655
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": -2.062592729534739e-30,
+                    "velocityX": 0,
+                    "velocityY": 2.2969932039584724e-33,
+                    "timestamp": 2.1915968814775257
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -4240,7 +13897,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C2 To S1": {
             "waypoints": [
@@ -4251,19 +13924,146 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 13
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": -2.88,
+                    "heading": 3.4031853071795863,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": 6.88462418020868e-33,
+                    "velocityX": -2.6678679284923953e-34,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.620477294727877,
+                    "y": 5.828094267433712,
+                    "heading": 3.125579827082933,
+                    "angularVelocity": -0.16053625399389732,
+                    "velocityX": -0.6626517023703261,
+                    "velocityY": 0.34616486523434487,
+                    "timestamp": 0.08982502430644557
+                },
+                {
+                    "x": 7.501400723607879,
+                    "y": 5.890296797165295,
+                    "heading": 3.0989291927764526,
+                    "angularVelocity": -0.29669498574873204,
+                    "velocityX": -1.3256503078001867,
+                    "velocityY": 0.6924855318644141,
+                    "timestamp": 0.17965004861289113
+                },
+                {
+                    "x": 7.322728500258778,
+                    "y": 5.983625073051111,
+                    "heading": 3.0637763178084882,
+                    "angularVelocity": -0.3913483490751664,
+                    "velocityX": -1.9891141107799197,
+                    "velocityY": 1.0390008419860823,
+                    "timestamp": 0.2694750729193367
+                },
+                {
+                    "x": 7.084419480032935,
+                    "y": 6.108090729851432,
+                    "heading": 3.027860333619382,
+                    "angularVelocity": -0.3998438571703182,
+                    "velocityX": -2.6530359670466983,
+                    "velocityY": 1.3856456790448122,
+                    "timestamp": 0.35930009722578227
+                },
+                {
+                    "x": 6.786779796078207,
+                    "y": 6.263490017806672,
+                    "heading": 3.0170090626873085,
+                    "angularVelocity": -0.1208045421179424,
+                    "velocityX": -3.3135497179416755,
+                    "velocityY": 1.7300222199225983,
+                    "timestamp": 0.44912512153222783
+                },
+                {
+                    "x": 6.48562142682328,
+                    "y": 6.4208733949571855,
+                    "heading": 3.0170090501527196,
+                    "angularVelocity": -1.3954451104022202e-7,
+                    "velocityX": -3.3527223797624646,
+                    "velocityY": 1.752110599086374,
+                    "timestamp": 0.5389501458386734
+                },
+                {
+                    "x": 6.164454252506572,
+                    "y": 6.531857797451653,
+                    "heading": 3.017009051216684,
+                    "angularVelocity": 1.1844854204542515e-8,
+                    "velocityX": -3.5754755069257693,
+                    "velocityY": 1.2355621760349855,
+                    "timestamp": 0.628775170145119
+                },
+                {
+                    "x": 5.838539791831514,
+                    "y": 6.576460283854247,
+                    "heading": 3.052457229239417,
+                    "angularVelocity": 0.3946358856725586,
+                    "velocityX": -3.6283258834773524,
+                    "velocityY": 0.4965485592347737,
+                    "timestamp": 0.7186001944515645
+                },
+                {
+                    "x": 5.573157198508507,
+                    "y": 6.601492716526358,
+                    "heading": 3.1680842973805725,
+                    "angularVelocity": 1.287247835822277,
+                    "velocityX": -2.9544394267863754,
+                    "velocityY": 0.2786799432050252,
+                    "timestamp": 0.8084252187580101
+                },
+                {
+                    "x": 5.374340211248987,
+                    "y": 6.616920258488131,
+                    "heading": 3.277639232341259,
+                    "angularVelocity": 1.2196482640175015,
+                    "velocityX": -2.2133808345126464,
+                    "velocityY": 0.17175104689246343,
+                    "timestamp": 0.8982502430644557
+                },
+                {
+                    "x": 5.242040057703165,
+                    "y": 6.625927417066914,
+                    "heading": 3.3593576896006887,
+                    "angularVelocity": 0.9097515741341804,
+                    "velocityX": -1.4728652128661812,
+                    "velocityY": 0.10027449085964446,
+                    "timestamp": 0.9880752673709012
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": 0.48792213436398174,
+                    "velocityX": -0.7352077910701558,
+                    "velocityY": 0.04533906853386082,
+                    "timestamp": 1.0779002916773468
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": 0,
+                    "velocityX": 2.612853322689367e-35,
+                    "velocityY": 8.19047982527468e-33,
+                    "timestamp": 1.1677253159837924
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -4281,7 +14081,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C2 To S2": {
             "waypoints": [
@@ -4292,7 +14108,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 14
                 },
                 {
                     "x": 4.263,
@@ -4304,7 +14120,143 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": 7.320432516399354e-29,
+                    "velocityX": -1.2956627863132083e-30,
+                    "velocityY": -2.1869206977044484e-30,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.583655523801243,
+                    "y": 5.855694285619672,
+                    "heading": 3.1309164320392644,
+                    "angularVelocity": -0.078082149385061,
+                    "velocityX": -0.8281749875703897,
+                    "velocityY": 0.5045347816645432,
+                    "timestamp": 0.1163334773987831
+                },
+                {
+                    "x": 7.390966571410279,
+                    "y": 5.973082843106357,
+                    "heading": 3.112749003492932,
+                    "angularVelocity": -0.15616681416695535,
+                    "velocityX": -1.6563499750844712,
+                    "velocityY": 1.0090694451115323,
+                    "timestamp": 0.2326669547975662
+                },
+                {
+                    "x": 7.1019331530012675,
+                    "y": 6.149165651208255,
+                    "heading": 3.0854972394904423,
+                    "angularVelocity": -0.2342555609256986,
+                    "velocityX": -2.484524875141701,
+                    "velocityY": 1.5136039258785212,
+                    "timestamp": 0.3490004321963493
+                },
+                {
+                    "x": 6.726101496816599,
+                    "y": 6.378126994339933,
+                    "heading": 3.085497236816664,
+                    "angularVelocity": -2.298373345534558e-8,
+                    "velocityX": -3.2306406082605346,
+                    "velocityY": 1.968146644037928,
+                    "timestamp": 0.4653339095951324
+                },
+                {
+                    "x": 6.345661043001768,
+                    "y": 6.599345871138349,
+                    "heading": 3.085497234127535,
+                    "angularVelocity": -2.3115694536288248e-8,
+                    "velocityX": -3.2702577308052767,
+                    "velocityY": 1.9015925745956355,
+                    "timestamp": 0.5816673869939155
+                },
+                {
+                    "x": 5.9214720079184415,
+                    "y": 6.716548218753983,
+                    "heading": 3.08549723097445,
+                    "angularVelocity": -2.7103849507961225e-8,
+                    "velocityX": -3.6463195682635403,
+                    "velocityY": 1.0074687891764202,
+                    "timestamp": 0.6980008643926986
+                },
+                {
+                    "x": 5.481423496557165,
+                    "y": 6.7220267434353165,
+                    "heading": 3.0854971884524733,
+                    "angularVelocity": -3.6551797104544465e-7,
+                    "velocityX": -3.7826472757521072,
+                    "velocityY": 0.047093277050021864,
+                    "timestamp": 0.8143343417914817
+                },
+                {
+                    "x": 5.071345998787579,
+                    "y": 6.619502331936359,
+                    "heading": 3.038730840809692,
+                    "angularVelocity": -0.40200249049953485,
+                    "velocityX": -3.5250171054705812,
+                    "velocityY": -0.8812975747944856,
+                    "timestamp": 0.9306678191902648
+                },
+                {
+                    "x": 4.728494542469685,
+                    "y": 6.427075201621358,
+                    "heading": 2.9510390674106386,
+                    "angularVelocity": -0.7537965455846517,
+                    "velocityX": -2.947143539280811,
+                    "velocityY": -1.6540993583074475,
+                    "timestamp": 1.0470012965890478
+                },
+                {
+                    "x": 4.474342078218714,
+                    "y": 6.168607565995357,
+                    "heading": 2.943927263065388,
+                    "angularVelocity": -0.061132912935043464,
+                    "velocityX": -2.1846889642931684,
+                    "velocityY": -2.221782082039817,
+                    "timestamp": 1.163334773987831
+                },
+                {
+                    "x": 4.328922284450032,
+                    "y": 5.887474207261646,
+                    "heading": 3.0089644652983214,
+                    "angularVelocity": 0.5590583526527744,
+                    "velocityX": -1.2500253325205173,
+                    "velocityY": -2.4166161368150747,
+                    "timestamp": 1.2796682513866142
+                },
+                {
+                    "x": 4.275692020371012,
+                    "y": 5.671261483228584,
+                    "heading": 3.090017233953519,
+                    "angularVelocity": 0.696727807571276,
+                    "velocityX": -0.45756617329120264,
+                    "velocityY": -1.8585597960929203,
+                    "timestamp": 1.3960017287853974
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0.42965075199414443,
+                    "velocityX": -0.10910032653373211,
+                    "velocityY": -0.9564012502367466,
+                    "timestamp": 1.5123352061841806
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": -1.1779881542407281e-29,
+                    "velocityX": 2.2171306210967267e-31,
+                    "velocityY": -6.290632939767308e-32,
+                    "timestamp": 1.6286686835829638
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -4322,7 +14274,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C2 To S3": {
             "waypoints": [
@@ -4333,7 +14301,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 20
                 },
                 {
                     "x": 4.263,
@@ -4345,7 +14313,197 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": 7.712459531346682e-31,
+                    "velocityX": 8.463399539512725e-31,
+                    "velocityY": -2.525463677436265e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.643960549352244,
+                    "y": 5.758768623179225,
+                    "heading": 3.0545306432351627,
+                    "angularVelocity": -1.025550451563212,
+                    "velocityX": -0.43243890307482463,
+                    "velocityY": -0.45873991856878515,
+                    "timestamp": 0.0833399825767314
+                },
+                {
+                    "x": 7.572007137321917,
+                    "y": 5.681963480289103,
+                    "heading": 2.882145272333543,
+                    "angularVelocity": -2.0684594065387945,
+                    "velocityX": -0.8633720551126874,
+                    "velocityY": -0.9215881803119774,
+                    "timestamp": 0.1666799651534628
+                },
+                {
+                    "x": 7.462165154490984,
+                    "y": 5.566757202432317,
+                    "heading": 2.631422178575744,
+                    "angularVelocity": -3.008437079128932,
+                    "velocityX": -1.3179986296469572,
+                    "velocityY": -1.382365034102495,
+                    "timestamp": 0.2500199477301942
+                },
+                {
+                    "x": 7.311173429444628,
+                    "y": 5.412128972668698,
+                    "heading": 2.3433148642731845,
+                    "angularVelocity": -3.457011933465413,
+                    "velocityX": -1.8117561388657581,
+                    "velocityY": -1.8553907138299572,
+                    "timestamp": 0.3333599303069256
+                },
+                {
+                    "x": 7.123843168628933,
+                    "y": 5.227542500531133,
+                    "heading": 2.191575313033137,
+                    "angularVelocity": -1.820729337210275,
+                    "velocityX": -2.2477837770510787,
+                    "velocityY": -2.21486093985698,
+                    "timestamp": 0.41669991288365704
+                },
+                {
+                    "x": 6.902601176392792,
+                    "y": 5.010431192670728,
+                    "heading": 2.175785118248912,
+                    "angularVelocity": -0.1894672196467848,
+                    "velocityX": -2.6546920865077652,
+                    "velocityY": -2.6051278287766304,
+                    "timestamp": 0.5000398954603884
+                },
+                {
+                    "x": 6.682324869385203,
+                    "y": 4.7848793693995555,
+                    "heading": 2.175784967765777,
+                    "angularVelocity": -0.0000018056535426780284,
+                    "velocityX": -2.643104788326184,
+                    "velocityY": -2.7064059326328107,
+                    "timestamp": 0.5833798780371198
+                },
+                {
+                    "x": 6.427295318194512,
+                    "y": 4.59952808942922,
+                    "heading": 2.175784213925318,
+                    "angularVelocity": -0.00000904536377316099,
+                    "velocityX": -3.060110445258255,
+                    "velocityY": -2.224037901611934,
+                    "timestamp": 0.6667198606138511
+                },
+                {
+                    "x": 6.1722637689115825,
+                    "y": 4.4141795587676,
+                    "heading": 2.1757834600140606,
+                    "angularVelocity": -0.000009046213285018581,
+                    "velocityX": -3.0601344204519796,
+                    "velocityY": -2.224004912539731,
+                    "timestamp": 0.7500598431905825
+                },
+                {
+                    "x": 5.906721685974762,
+                    "y": 4.244230836092675,
+                    "heading": 2.175782696018566,
+                    "angularVelocity": -0.000009167214470514032,
+                    "velocityX": -3.1862507613597577,
+                    "velocityY": -2.039221960701194,
+                    "timestamp": 0.8333998257673139
+                },
+                {
+                    "x": 5.614573321214709,
+                    "y": 4.1257208216835135,
+                    "heading": 2.175782627659757,
+                    "angularVelocity": -8.202402673559022e-7,
+                    "velocityX": -3.5055006699943965,
+                    "velocityY": -1.4220067096852238,
+                    "timestamp": 0.9167398083440452
+                },
+                {
+                    "x": 5.32091408424666,
+                    "y": 4.0110057741909,
+                    "heading": 2.1757827056250556,
+                    "angularVelocity": 9.355089415031885e-7,
+                    "velocityX": -3.523629689959162,
+                    "velocityY": -1.3764707400435987,
+                    "timestamp": 1.0000797909207766
+                },
+                {
+                    "x": 5.051276753694785,
+                    "y": 3.8476320535360697,
+                    "heading": 2.1757829434200273,
+                    "angularVelocity": 0.0000028533119900283243,
+                    "velocityX": -3.235389811890331,
+                    "velocityY": -1.9603282314633543,
+                    "timestamp": 1.083419773497508
+                },
+                {
+                    "x": 4.804772075990701,
+                    "y": 3.6532031340051674,
+                    "heading": 2.180086334431566,
+                    "angularVelocity": 0.05163657200885012,
+                    "velocityX": -2.957820125257733,
+                    "velocityY": -2.332960885273652,
+                    "timestamp": 1.1667597560742393
+                },
+                {
+                    "x": 4.605798608727452,
+                    "y": 3.4589597133454815,
+                    "heading": 2.2977487638281207,
+                    "angularVelocity": 1.411836501024247,
+                    "velocityX": -2.387491107045202,
+                    "velocityY": -2.330735076418397,
+                    "timestamp": 1.2500997386509707
+                },
+                {
+                    "x": 4.462499334629702,
+                    "y": 3.280406115781069,
+                    "heading": 2.4052460747163282,
+                    "angularVelocity": 1.28986481115753,
+                    "velocityX": -1.7194540923477422,
+                    "velocityY": -2.142472220941705,
+                    "timestamp": 1.333439721227702
+                },
+                {
+                    "x": 4.360731419840454,
+                    "y": 3.1416564242722327,
+                    "heading": 2.488750866660265,
+                    "angularVelocity": 1.0019775546155592,
+                    "velocityX": -1.2211175433778025,
+                    "velocityY": -1.6648634571178227,
+                    "timestamp": 1.4167797038044334
+                },
+                {
+                    "x": 4.295085549107569,
+                    "y": 3.0475525606989393,
+                    "heading": 2.5453815690352877,
+                    "angularVelocity": 0.6795142094358206,
+                    "velocityX": -0.7876875984758661,
+                    "velocityY": -1.129156266461315,
+                    "timestamp": 1.5001196863811648
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": 0.3433937718713025,
+                    "velocityX": -0.3849958701158514,
+                    "velocityY": -0.5705852008687156,
+                    "timestamp": 1.5834596689578961
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": -1.6359372913753935e-30,
+                    "velocityX": 0,
+                    "velocityY": -3.7530428699783824e-32,
+                    "timestamp": 1.6667996515346275
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -4363,7 +14521,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C2 To S4": {
             "waypoints": [
@@ -4374,7 +14548,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 21
                 },
                 {
                     "x": 5.176,
@@ -4404,7 +14578,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C2 To C1": {
             "waypoints": [
@@ -4415,7 +14605,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 9
                 },
                 {
                     "x": 7.68,
@@ -4427,7 +14617,98 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 1.6870437677917242e-24,
+                    "velocityY": 1.6813073596046228e-23,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.680000027670122,
+                    "y": 5.880500005575848,
+                    "heading": 3.140000018022814,
+                    "angularVelocity": 1.8010003546528393e-7,
+                    "velocityX": 2.7650455327562645e-7,
+                    "velocityY": 0.8344065481228156,
+                    "timestamp": 0.10007112931184436
+                },
+                {
+                    "x": 7.680000074983718,
+                    "y": 6.047500013629854,
+                    "heading": 3.1400000460729856,
+                    "angularVelocity": 2.8030233382034975e-7,
+                    "velocityX": 4.7279965639225817e-7,
+                    "velocityY": 1.6688130652907482,
+                    "timestamp": 0.20014225862368873
+                },
+                {
+                    "x": 7.680000134184045,
+                    "y": 6.298000017966634,
+                    "heading": 3.1400000770424787,
+                    "angularVelocity": 3.0947480383704467e-7,
+                    "velocityX": 5.915824876001831e-7,
+                    "velocityY": 2.5032195205488788,
+                    "timestamp": 0.3002133879355331
+                },
+                {
+                    "x": 7.680000197955803,
+                    "y": 6.632000000000009,
+                    "heading": 3.1400001051570197,
+                    "angularVelocity": 2.8094557567454647e-7,
+                    "velocityX": 6.372642912914395e-7,
+                    "velocityY": 3.3376257900773374,
+                    "timestamp": 0.40028451724737746
+                },
+                {
+                    "x": 7.680000129458281,
+                    "y": 6.96599998203339,
+                    "heading": 3.140000063575334,
+                    "angularVelocity": -4.1552129568712905e-7,
+                    "velocityX": -6.844883427144649e-7,
+                    "velocityY": 3.337625790077381,
+                    "timestamp": 0.5003556465592218
+                },
+                {
+                    "x": 7.680000069924504,
+                    "y": 7.216499986370157,
+                    "heading": 3.1400000316818817,
+                    "angularVelocity": -3.1870782840594973e-7,
+                    "velocityX": -5.949146101339814e-7,
+                    "velocityY": 2.5032195205487504,
+                    "timestamp": 0.6004267758710662
+                },
+                {
+                    "x": 7.6800000249972635,
+                    "y": 7.383499994424155,
+                    "heading": 3.14000001043482,
+                    "angularVelocity": -2.1231960430791816e-7,
+                    "velocityX": -4.4895306909597897e-7,
+                    "velocityY": 1.6688130652906694,
+                    "timestamp": 0.7004979051829106
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "angularVelocity": -1.0427402507810036e-7,
+                    "velocityX": -2.497949654636061e-7,
+                    "velocityY": 0.8344065481227817,
+                    "timestamp": 0.8005690344947549
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 4.128313998274909e-23,
+                    "velocityY": -7.415182378687901e-21,
+                    "timestamp": 0.9006401638065993
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -4445,7 +14726,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C2 To C3": {
             "waypoints": [
@@ -4456,7 +14753,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 9
                 },
                 {
                     "x": 7.68,
@@ -4486,7 +14783,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C2 To C4": {
             "waypoints": [
@@ -4497,7 +14810,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 14
                 },
                 {
                     "x": 7.68,
@@ -4509,7 +14822,143 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": -1.429570475254817e-33,
+                    "velocityX": -4.923067503198675e-33,
+                    "velocityY": 6.357653017323465e-38,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.680000074935176,
+                    "y": 5.7205546279944945,
+                    "heading": 3.1400000039815823,
+                    "angularVelocity": 4.158287610282007e-8,
+                    "velocityX": 7.826085092728229e-7,
+                    "velocityY": -0.7983807350943761,
+                    "timestamp": 0.09575052183150115
+                },
+                {
+                    "x": 7.68000021698637,
+                    "y": 5.567663887017397,
+                    "heading": 3.1400000079801687,
+                    "angularVelocity": 4.176046399562833e-8,
+                    "velocityX": 0.000001483555234342966,
+                    "velocityY": -1.5967614385031572,
+                    "timestamp": 0.1915010436630023
+                },
+                {
+                    "x": 7.680000418191477,
+                    "y": 5.338327783136532,
+                    "heading": 3.140000009761107,
+                    "angularVelocity": 1.8599774209907478e-8,
+                    "velocityX": 0.000002101347243657271,
+                    "velocityY": -2.395142078540755,
+                    "timestamp": 0.2872515654945035
+                },
+                {
+                    "x": 7.680000670419164,
+                    "y": 5.032546334555315,
+                    "heading": 3.1400000093900653,
+                    "angularVelocity": -3.875097409582088e-9,
+                    "velocityX": 0.000002634217150547891,
+                    "velocityY": -3.193522528465398,
+                    "timestamp": 0.3830020873260046
+                },
+                {
+                    "x": 7.68000083273763,
+                    "y": 4.670327800733128,
+                    "heading": 3.1400000093900697,
+                    "angularVelocity": 4.937002430391119e-14,
+                    "velocityX": 0.000001695223016776456,
+                    "velocityY": -3.7829405719543154,
+                    "timestamp": 0.4787526091575057
+                },
+                {
+                    "x": 7.6800009141474845,
+                    "y": 4.308109266911048,
+                    "heading": 3.1400000093900733,
+                    "angularVelocity": 3.662638141198336e-14,
+                    "velocityX": 8.50228817967247e-7,
+                    "velocityY": -3.782940571953249,
+                    "timestamp": 0.5745031309890068
+                },
+                {
+                    "x": 7.680000914400225,
+                    "y": 3.9458907330889583,
+                    "heading": 3.1400000093900764,
+                    "angularVelocity": 3.286235025790432e-14,
+                    "velocityX": 2.639571649715247e-9,
+                    "velocityY": -3.7829405719533424,
+                    "timestamp": 0.670253652820508
+                },
+                {
+                    "x": 7.680000833444056,
+                    "y": 3.5836721992668767,
+                    "heading": 3.140000009390079,
+                    "angularVelocity": 2.6535816526961207e-14,
+                    "velocityX": -8.454906188658589e-7,
+                    "velocityY": -3.78294057195325,
+                    "timestamp": 0.7660041746520091
+                },
+                {
+                    "x": 7.680000671408071,
+                    "y": 3.221453665444688,
+                    "heading": 3.140000009390085,
+                    "angularVelocity": 5.86589821918151e-14,
+                    "velocityX": -0.0000016922728517221796,
+                    "velocityY": -3.7829405719543456,
+                    "timestamp": 0.8617546964835102
+                },
+                {
+                    "x": 7.680000418877801,
+                    "y": 2.9156722168634683,
+                    "heading": 3.140000001402919,
+                    "angularVelocity": -8.341641379215641e-8,
+                    "velocityX": -0.000002637377255306636,
+                    "velocityY": -3.193522528465426,
+                    "timestamp": 0.9575052183150113
+                },
+                {
+                    "x": 7.680000217377681,
+                    "y": 2.686336112982602,
+                    "heading": 3.139999997550481,
+                    "angularVelocity": -4.023411698701823e-8,
+                    "velocityX": -0.0000021044283112699676,
+                    "velocityY": -2.395142078540766,
+                    "timestamp": 1.0532557401465126
+                },
+                {
+                    "x": 7.680000075082089,
+                    "y": 2.5334453720055046,
+                    "heading": 3.139999997845116,
+                    "angularVelocity": 3.0771112395983046e-9,
+                    "velocityX": -0.0000014861076817541395,
+                    "velocityY": -1.5967614385031628,
+                    "timestamp": 1.1490062619780137
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": 2.250519944152171e-8,
+                    "velocityX": -7.841428415253779e-7,
+                    "velocityY": -0.7983807350943782,
+                    "timestamp": 1.2447567838095148
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 5.079076909387901e-38,
+                    "timestamp": 1.340507305641016
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -4527,7 +14976,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C2 To C5": {
             "waypoints": [
@@ -4538,7 +15003,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 18
                 },
                 {
                     "x": 7.68,
@@ -4550,7 +15015,179 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": -8.977040531488188e-32,
+                    "velocityX": -1.4835352698294527e-33,
+                    "velocityY": -6.682715069575163e-38,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.680000115631693,
+                    "y": 5.715155846048254,
+                    "heading": 3.139999983275777,
+                    "angularVelocity": -1.688054668223312e-7,
+                    "velocityX": 0.0000011671252822265972,
+                    "velocityY": -0.8260917454273096,
+                    "timestamp": 0.09907392781101573
+                },
+                {
+                    "x": 7.680000338287477,
+                    "y": 5.551467541258106,
+                    "heading": 3.1399999456936927,
+                    "angularVelocity": -3.793336766253788e-7,
+                    "velocityX": 0.0000022473699405935927,
+                    "velocityY": -1.6521834594301872,
+                    "timestamp": 0.19814785562203147
+                },
+                {
+                    "x": 7.680000659193895,
+                    "y": 5.305935091856244,
+                    "heading": 3.1399998851536046,
+                    "angularVelocity": -6.110596046233074e-7,
+                    "velocityX": 0.0000032390599337968516,
+                    "velocityY": -2.4782751105841556,
+                    "timestamp": 0.29722178343304717
+                },
+                {
+                    "x": 7.680001069383311,
+                    "y": 4.978558516522714,
+                    "heading": 3.1399998022537425,
+                    "angularVelocity": -8.367472472020738e-7,
+                    "velocityX": 0.0000041402352562216615,
+                    "velocityY": -3.3043665731916145,
+                    "timestamp": 0.39629571124406293
+                },
+                {
+                    "x": 7.680001393532974,
+                    "y": 4.603767735073282,
+                    "heading": 3.1399998022537403,
+                    "angularVelocity": -2.4397007486697484e-14,
+                    "velocityX": 0.0000032717959136681545,
+                    "velocityY": -3.782940575085891,
+                    "timestamp": 0.4953696390550787
+                },
+                {
+                    "x": 7.680001631666853,
+                    "y": 4.228976953623769,
+                    "heading": 3.1399998022537345,
+                    "angularVelocity": -6.162047103580494e-14,
+                    "velocityX": 0.000002403597704835693,
+                    "velocityY": -3.7829405750867746,
+                    "timestamp": 0.5944435668660945
+                },
+                {
+                    "x": 7.68000178361998,
+                    "y": 3.854186172174224,
+                    "heading": 3.139999802253728,
+                    "angularVelocity": -7.021738392770843e-14,
+                    "velocityX": 0.0000015337346806740877,
+                    "velocityY": -3.78294057508712,
+                    "timestamp": 0.6935174946771102
+                },
+                {
+                    "x": 7.680001849334011,
+                    "y": 3.4793953907246618,
+                    "heading": 3.1399998022537208,
+                    "angularVelocity": -7.676689036353258e-14,
+                    "velocityX": 6.632827646851356e-7,
+                    "velocityY": -3.782940575087319,
+                    "timestamp": 0.792591422488126
+                },
+                {
+                    "x": 7.680001829174184,
+                    "y": 3.104604609275094,
+                    "heading": 3.139999802253714,
+                    "angularVelocity": -7.698791271549556e-14,
+                    "velocityX": -2.0348261115822963e-7,
+                    "velocityY": -3.7829405750873777,
+                    "timestamp": 0.8916653502991417
+                },
+                {
+                    "x": 7.680001725145299,
+                    "y": 2.729813827825533,
+                    "heading": 3.1399998022537057,
+                    "angularVelocity": -9.316120157306394e-14,
+                    "velocityX": -0.000001050012614805773,
+                    "velocityY": -3.7829405750872973,
+                    "timestamp": 0.9907392781101575
+                },
+                {
+                    "x": 7.680001540841657,
+                    "y": 2.355023046375991,
+                    "heading": 3.1399998022536986,
+                    "angularVelocity": -7.445499842222973e-14,
+                    "velocityX": -0.0000018602636449128091,
+                    "velocityY": -3.7829405750870753,
+                    "timestamp": 1.0898132059211731
+                },
+                {
+                    "x": 7.680001281423707,
+                    "y": 1.9802322649264825,
+                    "heading": 3.139999802253698,
+                    "angularVelocity": -7.093550187375142e-15,
+                    "velocityX": -0.000002618427817479533,
+                    "velocityY": -3.782940575086706,
+                    "timestamp": 1.188887133732189
+                },
+                {
+                    "x": 7.680000953984531,
+                    "y": 1.6054414834770696,
+                    "heading": 3.139999802253699,
+                    "angularVelocity": 1.1174094242032294e-14,
+                    "velocityX": -0.0000033049984216512085,
+                    "velocityY": -3.782940575085678,
+                    "timestamp": 1.2879610615432047
+                },
+                {
+                    "x": 7.680000583113676,
+                    "y": 1.2780649081436686,
+                    "heading": 3.139999880388909,
+                    "angularVelocity": 7.886553669648788e-7,
+                    "velocityX": -0.0000037433746003660557,
+                    "velocityY": -3.304366573190331,
+                    "timestamp": 1.3870349893542204
+                },
+                {
+                    "x": 7.6800002967542085,
+                    "y": 1.0325324587418607,
+                    "heading": 3.139999940073013,
+                    "angularVelocity": 6.024197635453253e-7,
+                    "velocityX": -0.0000028903613408431902,
+                    "velocityY": -2.47827511058363,
+                    "timestamp": 1.4861089171652362
+                },
+                {
+                    "x": 7.680000100606573,
+                    "y": 0.8688441539517371,
+                    "heading": 3.1399999800752636,
+                    "angularVelocity": 4.0376157345960537e-7,
+                    "velocityX": -0.000001979810751989867,
+                    "velocityY": -1.6521834594299558,
+                    "timestamp": 1.585182844976252
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": 2.0110976517179292e-7,
+                    "velocityX": -0.00000101546967480447,
+                    "velocityY": -0.8260917454272348,
+                    "timestamp": 1.6842567727872677
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": -8.944233053536814e-32,
+                    "velocityX": 0,
+                    "velocityY": -1.1208522306082827e-37,
+                    "timestamp": 1.7833307005982835
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -4568,7 +15205,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C3 To W1": {
             "waypoints": [
@@ -4579,12 +15232,12 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 23
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 3.5331853071795862,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -4609,7 +15262,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C3 To W2": {
             "waypoints": [
@@ -4620,7 +15289,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 20
                 },
                 {
                     "x": 2.25,
@@ -4632,7 +15301,197 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": 8.742415967937999e-33,
+                    "velocityX": -1.6780440317133427e-34,
+                    "velocityY": 4.640205301974453e-35,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.599500768056594,
+                    "y": 4.12633480373168,
+                    "heading": 3.0835764078705523,
+                    "angularVelocity": -0.5690015744722524,
+                    "velocityX": -0.8117914508973499,
+                    "velocityY": -0.006708146534495369,
+                    "timestamp": 0.09916245342867555
+                },
+                {
+                    "x": 7.438156123912552,
+                    "y": 4.125079622260014,
+                    "heading": 2.9779167441714383,
+                    "angularVelocity": -1.065520870508837,
+                    "velocityX": -1.6270739434668355,
+                    "velocityY": -0.012657829937315014,
+                    "timestamp": 0.1983249068573511
+                },
+                {
+                    "x": 7.195408805362699,
+                    "y": 4.123338544846998,
+                    "heading": 2.8371936481283773,
+                    "angularVelocity": -1.4191167238946776,
+                    "velocityX": -2.4479761256053836,
+                    "velocityY": -0.017557829125994403,
+                    "timestamp": 0.2974873602860266
+                },
+                {
+                    "x": 6.870725465009593,
+                    "y": 4.121560782802172,
+                    "heading": 2.707127276825701,
+                    "angularVelocity": -1.311649387499567,
+                    "velocityX": -3.2742568293415686,
+                    "velocityY": -0.017927773903902572,
+                    "timestamp": 0.3966498137147022
+                },
+                {
+                    "x": 6.495621806376681,
+                    "y": 4.117497642893674,
+                    "heading": 2.7071272422206913,
+                    "angularVelocity": -3.489729050319063e-7,
+                    "velocityX": -3.7827186164036664,
+                    "velocityY": -0.04097458027721316,
+                    "timestamp": 0.4958122671433778
+                },
+                {
+                    "x": 6.120518146053048,
+                    "y": 4.113434659065473,
+                    "heading": 2.7071272076158857,
+                    "angularVelocity": -3.4897084992850824e-7,
+                    "velocityX": -3.7827186334536713,
+                    "velocityY": -0.04097300629137556,
+                    "timestamp": 0.5949747205720534
+                },
+                {
+                    "x": 5.746992350808683,
+                    "y": 4.148043089964446,
+                    "heading": 2.7071271713462406,
+                    "angularVelocity": -3.657598578804205e-7,
+                    "velocityX": -3.76680671291609,
+                    "velocityY": 0.3490074085739077,
+                    "timestamp": 0.6941371740007289
+                },
+                {
+                    "x": 5.389907487037217,
+                    "y": 4.262976321781226,
+                    "heading": 2.7071270362524373,
+                    "angularVelocity": -0.000001362348332205964,
+                    "velocityX": -3.6010087631434655,
+                    "velocityY": 1.1590398164104305,
+                    "timestamp": 0.7932996274294045
+                },
+                {
+                    "x": 5.0459552426535295,
+                    "y": 4.412696400521408,
+                    "heading": 2.70712690066141,
+                    "angularVelocity": -0.000001367362571417024,
+                    "velocityX": -3.468573361096618,
+                    "velocityY": 1.509846454614715,
+                    "timestamp": 0.8924620808580801
+                },
+                {
+                    "x": 4.713262422237293,
+                    "y": 4.586001722574957,
+                    "heading": 2.70712676137173,
+                    "angularVelocity": -0.0000014046614974130796,
+                    "velocityX": -3.355028127208762,
+                    "velocityY": 1.747690946132164,
+                    "timestamp": 0.9916245342867557
+                },
+                {
+                    "x": 4.426168065896197,
+                    "y": 4.827447572944867,
+                    "heading": 2.7071267614112156,
+                    "angularVelocity": 3.9818977543377344e-10,
+                    "velocityX": -2.8951921459627346,
+                    "velocityY": 2.4348515191142828,
+                    "timestamp": 1.0907869877154313
+                },
+                {
+                    "x": 4.093475286723252,
+                    "y": 5.000753014560729,
+                    "heading": 2.707126833350956,
+                    "angularVelocity": 7.254735825966592e-7,
+                    "velocityX": -3.355027711292369,
+                    "velocityY": 1.7476921518537738,
+                    "timestamp": 1.1899494411441067
+                },
+                {
+                    "x": 3.752079510529913,
+                    "y": 5.156214217951438,
+                    "heading": 2.707126904178975,
+                    "angularVelocity": 7.142624716180523e-7,
+                    "velocityX": -3.4427927546074075,
+                    "velocityY": 1.5677426083706831,
+                    "timestamp": 1.2891118945727822
+                },
+                {
+                    "x": 3.4085800456948445,
+                    "y": 5.306970245158155,
+                    "heading": 2.7071269730653125,
+                    "angularVelocity": 6.946816574949046e-7,
+                    "velocityX": -3.4640073229141812,
+                    "velocityY": 1.5202934376281008,
+                    "timestamp": 1.3882743480014577
+                },
+                {
+                    "x": 3.041842891660795,
+                    "y": 5.385857046749469,
+                    "heading": 2.7071269977260264,
+                    "angularVelocity": 2.486900331372695e-7,
+                    "velocityX": -3.698346918149141,
+                    "velocityY": 0.7955309581771803,
+                    "timestamp": 1.4874368014301331
+                },
+                {
+                    "x": 2.7246821450713132,
+                    "y": 5.456194623849801,
+                    "heading": 2.863679707498997,
+                    "angularVelocity": 1.5787498630777044,
+                    "velocityX": -3.1983955178923207,
+                    "velocityY": 0.7093166281018212,
+                    "timestamp": 1.5865992548588086
+                },
+                {
+                    "x": 2.4872060579091175,
+                    "y": 5.508276609290408,
+                    "heading": 2.997740485854431,
+                    "angularVelocity": 1.3519308339003542,
+                    "velocityX": -2.394818592634002,
+                    "velocityY": 0.5252188065119572,
+                    "timestamp": 1.685761708287484
+                },
+                {
+                    "x": 2.3290362313275734,
+                    "y": 5.542798533700601,
+                    "heading": 3.0916641126613666,
+                    "angularVelocity": 0.9471692516612843,
+                    "velocityX": -1.5950576161904975,
+                    "velocityY": 0.34813503716932903,
+                    "timestamp": 1.7849241617161595
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0.4874414223060736,
+                    "velocityX": -0.7970378766840579,
+                    "velocityY": 0.17346753438054688,
+                    "timestamp": 1.884086615144835
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": -6.536266088535149e-32,
+                    "velocityX": 0,
+                    "velocityY": 2.2958668874154396e-32,
+                    "timestamp": 1.9832490685735105
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -4650,7 +15509,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C3 To W3": {
             "waypoints": [
@@ -4661,7 +15536,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 21
                 },
                 {
                     "x": 2.3,
@@ -4691,7 +15566,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C3 To S1": {
             "waypoints": [
@@ -4702,19 +15593,173 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 16
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": -2.88,
+                    "heading": 3.4031853071795863,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": -7.693901589446952e-31,
+                    "velocityX": -6.624376358473947e-32,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.653617802747577,
+                    "y": 4.193887846119818,
+                    "heading": 3.0831794581771232,
+                    "angularVelocity": -0.6046893294524585,
+                    "velocityX": -0.2807617219803893,
+                    "velocityY": 0.7118264895254428,
+                    "timestamp": 0.0939665032196413
+                },
+                {
+                    "x": 7.6005882437737835,
+                    "y": 4.328100423051846,
+                    "heading": 2.9787754661598114,
+                    "angularVelocity": -1.1110766969084016,
+                    "velocityX": -0.5643453481485778,
+                    "velocityY": 1.4283023453401595,
+                    "timestamp": 0.1879330064392826
+                },
+                {
+                    "x": 7.520633934300323,
+                    "y": 4.5302283764630555,
+                    "heading": 2.841927884917239,
+                    "angularVelocity": -1.4563442988050657,
+                    "velocityX": -0.8508809707068956,
+                    "velocityY": 2.1510639055999357,
+                    "timestamp": 0.2818995096589239
+                },
+                {
+                    "x": 7.4136169796865685,
+                    "y": 4.800820969540026,
+                    "heading": 2.7078159162812883,
+                    "angularVelocity": -1.4272316627817019,
+                    "velocityX": -1.1388840804643727,
+                    "velocityY": 2.879670774217013,
+                    "timestamp": 0.3758660128785652
+                },
+                {
+                    "x": 7.279364393815875,
+                    "y": 5.128465147908359,
+                    "heading": 2.7044182580824483,
+                    "angularVelocity": -0.03615818491083086,
+                    "velocityX": -1.4287281240729475,
+                    "velocityY": 3.486818889093733,
+                    "timestamp": 0.4698325160982065
+                },
+                {
+                    "x": 7.14774134031491,
+                    "y": 5.45866822544814,
+                    "heading": 2.7044182151788783,
+                    "angularVelocity": -4.565836605912663e-7,
+                    "velocityX": -1.4007444034956136,
+                    "velocityY": 3.514050924806152,
+                    "timestamp": 0.5637990193178478
+                },
+                {
+                    "x": 6.993945263950606,
+                    "y": 5.779145162484634,
+                    "heading": 2.704418171322215,
+                    "angularVelocity": -4.667265617625592e-7,
+                    "velocityX": -1.636711712096111,
+                    "velocityY": 3.410544460587175,
+                    "timestamp": 0.6577655225374891
+                },
+                {
+                    "x": 6.777429472030302,
+                    "y": 6.061066444299655,
+                    "heading": 2.704418118850798,
+                    "angularVelocity": -5.58405553736054e-7,
+                    "velocityX": -2.3041805803309763,
+                    "velocityY": 3.000231701248326,
+                    "timestamp": 0.7517320257571304
+                },
+                {
+                    "x": 6.5074813547317545,
+                    "y": 6.2923383909446455,
+                    "heading": 2.7044180981669834,
+                    "angularVelocity": -2.2011901843785396e-7,
+                    "velocityX": -2.872812204872187,
+                    "velocityY": 2.461216909438525,
+                    "timestamp": 0.8456985289767717
+                },
+                {
+                    "x": 6.195734686238664,
+                    "y": 6.463002989375195,
+                    "heading": 2.7046305201623446,
+                    "angularVelocity": 0.002260614028208905,
+                    "velocityX": -3.317636155560676,
+                    "velocityY": 1.81622804491969,
+                    "timestamp": 0.939665032196413
+                },
+                {
+                    "x": 5.885885567596677,
+                    "y": 6.564828261025872,
+                    "heading": 2.8003077522097084,
+                    "angularVelocity": 1.018205730436987,
+                    "velocityX": -3.297442258947648,
+                    "velocityY": 1.0836337222495915,
+                    "timestamp": 1.0336315354160543
+                },
+                {
+                    "x": 5.602675613806244,
+                    "y": 6.605201169261095,
+                    "heading": 3.005504879522894,
+                    "angularVelocity": 2.183726331004879,
+                    "velocityX": -3.0139458646071584,
+                    "velocityY": 0.4296521297685472,
+                    "timestamp": 1.1275980386356956
+                },
+                {
+                    "x": 5.389000568947429,
+                    "y": 6.621873348579134,
+                    "heading": 3.193372437152187,
+                    "angularVelocity": 1.999303487862736,
+                    "velocityX": -2.273949093959171,
+                    "velocityY": 0.17742683559341804,
+                    "timestamp": 1.221564541855337
+                },
+                {
+                    "x": 5.246855829881342,
+                    "y": 6.628281724607146,
+                    "heading": 3.330497456216328,
+                    "angularVelocity": 1.459296817118111,
+                    "velocityX": -1.5127171299950581,
+                    "velocityY": 0.06819851551815755,
+                    "timestamp": 1.3155310450749782
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": 0.7735506640420036,
+                    "velocityX": -0.7540541304992547,
+                    "velocityY": 0.018286041663562246,
+                    "timestamp": 1.4094975482946195
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": 3.878626979187187e-32,
+                    "velocityX": -5.758991747455131e-33,
+                    "velocityY": 3.749392123602624e-30,
+                    "timestamp": 1.5034640515142608
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -4732,7 +15777,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C3 To S2": {
             "waypoints": [
@@ -4773,7 +15834,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C3 To S3": {
             "waypoints": [
@@ -4814,7 +15891,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C3 To S4": {
             "waypoints": [
@@ -4851,11 +15944,27 @@
                     ],
                     "type": "StopPoint"
                 }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C3 To C1": {
             "waypoints": [
@@ -4896,7 +16005,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C3 To C2": {
             "waypoints": [
@@ -4907,7 +16032,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 9
                 },
                 {
                     "x": 7.68,
@@ -4937,7 +16062,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C3 To C4": {
             "waypoints": [
@@ -4948,7 +16089,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 9
                 },
                 {
                     "x": 7.68,
@@ -4960,7 +16101,98 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": 2.0369445518795058e-34,
+                    "velocityX": 0,
+                    "velocityY": 7.181654629664687e-43,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.680000037559162,
+                    "y": 4.043499994424155,
+                    "heading": 3.140000006758021,
+                    "angularVelocity": 6.75321744601227e-8,
+                    "velocityX": 3.753246582974315e-7,
+                    "velocityY": -0.8344065481227707,
+                    "timestamp": 0.10007112931184514
+                },
+                {
+                    "x": 7.6800001031226985,
+                    "y": 3.8764999863701557,
+                    "heading": 3.1400000189870485,
+                    "angularVelocity": 1.2220335610640094e-7,
+                    "velocityX": 6.551693493497348e-7,
+                    "velocityY": -1.6688130652906727,
+                    "timestamp": 0.20014225862369028
+                },
+                {
+                    "x": 7.680000187135912,
+                    "y": 3.625999982033383,
+                    "heading": 3.1400000350867425,
+                    "angularVelocity": 1.6088250383461684e-7,
+                    "velocityX": 8.39534968426326e-7,
+                    "velocityY": -2.503219520548785,
+                    "timestamp": 0.3002133879355354
+                },
+                {
+                    "x": 7.680000280051002,
+                    "y": 3.2919999999999914,
+                    "heading": 3.1400000531130368,
+                    "angularVelocity": 1.801348130208338e-7,
+                    "velocityX": 9.284904765306952e-7,
+                    "velocityY": -3.3376257900774653,
+                    "timestamp": 0.40028451724738057
+                },
+                {
+                    "x": 7.680000185977535,
+                    "y": 2.958000017966607,
+                    "heading": 3.1400000281215186,
+                    "angularVelocity": -2.4973754429186553e-7,
+                    "velocityX": -9.400660019914908e-7,
+                    "velocityY": -3.337625790077398,
+                    "timestamp": 0.5003556465592257
+                },
+                {
+                    "x": 7.680000101870251,
+                    "y": 2.7075000136298395,
+                    "heading": 3.1400000109296125,
+                    "angularVelocity": -1.7179686113327866e-7,
+                    "velocityX": -8.404750236739033e-7,
+                    "velocityY": -2.503219520548736,
+                    "timestamp": 0.6004267758710709
+                },
+                {
+                    "x": 7.6800000368892665,
+                    "y": 2.5405000055758427,
+                    "heading": 3.140000002248845,
+                    "angularVelocity": -8.674597729312873e-8,
+                    "velocityX": -6.493479632352893e-7,
+                    "velocityY": -1.6688130652906423,
+                    "timestamp": 0.700497905182916
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": -2.2472459301603394e-8,
+                    "velocityX": -3.686304669837251e-7,
+                    "velocityY": -0.8344065481227574,
+                    "timestamp": 0.8005690344947611
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": 7.368205253143154e-34,
+                    "velocityX": 0,
+                    "velocityY": -2.1562414433932607e-41,
+                    "timestamp": 0.9006401638066063
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -4978,7 +16210,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C3 To C5": {
             "waypoints": [
@@ -4989,7 +16237,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 14
                 },
                 {
                     "x": 7.68,
@@ -5001,7 +16249,143 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": 9.207800442360542e-32,
+                    "velocityX": -6.095364969086891e-33,
+                    "velocityY": -9.366947302592518e-40,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.680000070435844,
+                    "y": 4.0505546279944795,
+                    "heading": 3.1399999875764033,
+                    "angularVelocity": -1.2974964830179728e-7,
+                    "velocityX": 7.356183706178639e-7,
+                    "velocityY": -0.7983807350945191,
+                    "timestamp": 0.0957505218315028
+                },
+                {
+                    "x": 7.680000202739516,
+                    "y": 3.8976638870173534,
+                    "heading": 3.1399999616102225,
+                    "angularVelocity": -2.711857701387203e-7,
+                    "velocityX": 0.0000013817539912349048,
+                    "velocityY": -1.5967614385034457,
+                    "timestamp": 0.1915010436630056
+                },
+                {
+                    "x": 7.680000388342391,
+                    "y": 3.6683277831364425,
+                    "heading": 3.139999920718594,
+                    "angularVelocity": -4.2706425730703774e-7,
+                    "velocityX": 0.00000193840056979922,
+                    "velocityY": -2.395142078541202,
+                    "timestamp": 0.2872515654945084
+                },
+                {
+                    "x": 7.680000618679091,
+                    "y": 3.362546334555153,
+                    "heading": 3.139999863216847,
+                    "angularVelocity": -6.005370836912587e-7,
+                    "velocityX": 0.000002405591946532482,
+                    "velocityY": -3.1935225284661044,
+                    "timestamp": 0.3830020873260112
+                },
+                {
+                    "x": 7.680000750151485,
+                    "y": 3.000327800732898,
+                    "heading": 3.139999863216849,
+                    "angularVelocity": 2.234907225706593e-14,
+                    "velocityX": 0.0000013730725434542363,
+                    "velocityY": -3.7829405719549856,
+                    "timestamp": 0.478752609157514
+                },
+                {
+                    "x": 7.6800008034310885,
+                    "y": 2.6381092669108033,
+                    "heading": 3.139999863216839,
+                    "angularVelocity": -1.1397010502093565e-13,
+                    "velocityX": 5.56441925019577e-7,
+                    "velocityY": -3.7829405719533264,
+                    "timestamp": 0.5745031309890168
+                },
+                {
+                    "x": 7.68000078216065,
+                    "y": 2.275890733088706,
+                    "heading": 3.1399998632168313,
+                    "angularVelocity": -8.126780055977152e-14,
+                    "velocityX": -2.2214435413644554e-7,
+                    "velocityY": -3.7829405719533535,
+                    "timestamp": 0.6702536528205196
+                },
+                {
+                    "x": 7.6800006913830785,
+                    "y": 1.9136721992666206,
+                    "heading": 3.1399998632168296,
+                    "angularVelocity": -1.6383854112995832e-14,
+                    "velocityX": -9.4806343038794e-7,
+                    "velocityY": -3.7829405719532305,
+                    "timestamp": 0.7660041746520224
+                },
+                {
+                    "x": 7.680000537903045,
+                    "y": 1.55145366544455,
+                    "heading": 3.13999986321682,
+                    "angularVelocity": -1.0277752214408611e-13,
+                    "velocityX": -0.0000016029159878741863,
+                    "velocityY": -3.7829405719530524,
+                    "timestamp": 0.8617546964835252
+                },
+                {
+                    "x": 7.680000332408874,
+                    "y": 1.2456722168634402,
+                    "heading": 3.139999917268044,
+                    "angularVelocity": 5.645005022977776e-7,
+                    "velocityX": -0.0000021461414302094452,
+                    "velocityY": -3.1935225284642264,
+                    "timestamp": 0.957505218315028
+                },
+                {
+                    "x": 7.680000170899522,
+                    "y": 1.0163361129826003,
+                    "heading": 3.139999958602803,
+                    "angularVelocity": 4.3169222333319617e-7,
+                    "velocityX": -0.0000016867725067406415,
+                    "velocityY": -2.3951420785404567,
+                    "timestamp": 1.0532557401465308
+                },
+                {
+                    "x": 7.680000058494392,
+                    "y": 0.8634453720055071,
+                    "heading": 3.1399999862669987,
+                    "angularVelocity": 2.889195077791466e-7,
+                    "velocityX": -0.0000011739374912229912,
+                    "velocityY": -1.5967614385030957,
+                    "timestamp": 1.1490062619780337
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": 1.4342481443578566e-7,
+                    "velocityX": -6.10904148268517e-7,
+                    "velocityY": -0.7983807350943894,
+                    "timestamp": 1.2447567838095366
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": -3.045628877095783e-32,
+                    "velocityY": -7.047678165048749e-38,
+                    "timestamp": 1.3405073056410395
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -5019,7 +16403,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C4 To W1": {
             "waypoints": [
@@ -5030,19 +16430,254 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 25
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 3.5331853071795862,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": 1.0740254772261542e-28,
+                    "velocityX": 2.0063238328605694e-29,
+                    "velocityY": 1.599232652510583e-28,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.635665892651778,
+                    "y": 2.515360941496967,
+                    "heading": 3.1393295631597837,
+                    "angularVelocity": -0.0071510111194275595,
+                    "velocityX": -0.47287630332947994,
+                    "velocityY": 0.6224892734875569,
+                    "timestamp": 0.09375413197081223
+                },
+                {
+                    "x": 7.546997676041258,
+                    "y": 2.6320828188122176,
+                    "heading": 3.137988695350318,
+                    "angularVelocity": -0.014301959617960866,
+                    "velocityX": -0.9457526270748597,
+                    "velocityY": 1.2449784864051567,
+                    "timestamp": 0.18750826394162445
+                },
+                {
+                    "x": 7.413995349999322,
+                    "y": 2.8071656233620255,
+                    "heading": 3.1359774017076236,
+                    "angularVelocity": -0.02145285333469997,
+                    "velocityX": -1.4186289526241012,
+                    "velocityY": 1.8674676077670485,
+                    "timestamp": 0.2812623959124367
+                },
+                {
+                    "x": 7.236658921990214,
+                    "y": 3.0406093354422112,
+                    "heading": 3.133295687975677,
+                    "angularVelocity": -0.028603685785057682,
+                    "velocityX": -1.8915051985582343,
+                    "velocityY": 2.489956518960288,
+                    "timestamp": 0.3750165278832489
+                },
+                {
+                    "x": 7.022133976710729,
+                    "y": 3.323040275054587,
+                    "heading": 3.133295687627681,
+                    "angularVelocity": -3.7117965182714086e-9,
+                    "velocityX": -2.288165233573672,
+                    "velocityY": 3.0124639167935428,
+                    "timestamp": 0.46877065985406113
+                },
+                {
+                    "x": 6.786563329839658,
+                    "y": 3.588171672312515,
+                    "heading": 3.133295687271953,
+                    "angularVelocity": -3.794263922237942e-9,
+                    "velocityX": -2.512642823512138,
+                    "velocityY": 2.8279435976269225,
+                    "timestamp": 0.5625247918248734
+                },
+                {
+                    "x": 6.501527367007155,
+                    "y": 3.7992230523695487,
+                    "heading": 3.133295686856248,
+                    "angularVelocity": -4.433993924393781e-9,
+                    "velocityX": -3.040249606505238,
+                    "velocityY": 2.251115504143757,
+                    "timestamp": 0.6562789237956856
+                },
+                {
+                    "x": 6.179197632360147,
+                    "y": 3.947181584790984,
+                    "heading": 3.133295705650933,
+                    "angularVelocity": 2.004678114437141e-7,
+                    "velocityX": -3.438032307177196,
+                    "velocityY": 1.5781547896737034,
+                    "timestamp": 0.7500330557664978
+                },
+                {
+                    "x": 5.833338645417163,
+                    "y": 4.025728972736192,
+                    "heading": 3.133296009616361,
+                    "angularVelocity": 0.000003242155002257518,
+                    "velocityX": -3.6889999370977806,
+                    "velocityY": 0.837801879171189,
+                    "timestamp": 0.84378718773731
+                },
+                {
+                    "x": 5.481107394947439,
+                    "y": 4.067217134926503,
+                    "heading": 3.1332963782586343,
+                    "angularVelocity": 0.000003932010945944395,
+                    "velocityX": -3.7569677524120277,
+                    "velocityY": 0.4425208928736066,
+                    "timestamp": 0.9375413197081222
+                },
+                {
+                    "x": 5.149070300488837,
+                    "y": 4.179082889717707,
+                    "heading": 3.1449832895383,
+                    "angularVelocity": 0.12465489289866781,
+                    "velocityX": -3.541572915015461,
+                    "velocityY": 1.1931821290397846,
+                    "timestamp": 1.0312954516789343
+                },
+                {
+                    "x": 4.859012780941325,
+                    "y": 4.350314048380944,
+                    "heading": 3.189517171995987,
+                    "angularVelocity": 0.4750071439148605,
+                    "velocityX": -3.093810517469455,
+                    "velocityY": 1.826385195657799,
+                    "timestamp": 1.1250495836497465
+                },
+                {
+                    "x": 4.618219764951525,
+                    "y": 4.5756090572292365,
+                    "heading": 3.250644921045485,
+                    "angularVelocity": 0.6520005866891553,
+                    "velocityX": -2.5683456390462376,
+                    "velocityY": 2.4030408485721884,
+                    "timestamp": 1.2188037156205587
+                },
+                {
+                    "x": 4.427454911100163,
+                    "y": 4.854246931015481,
+                    "heading": 3.2945713874044515,
+                    "angularVelocity": 0.46852832441237513,
+                    "velocityX": -2.034735428095452,
+                    "velocityY": 2.9720063311233567,
+                    "timestamp": 1.3125578475913708
+                },
+                {
+                    "x": 4.197108920054306,
+                    "y": 5.123929924649606,
+                    "heading": 3.2945713966505874,
+                    "angularVelocity": 9.862110665096438e-8,
+                    "velocityX": -2.4569156175171605,
+                    "velocityY": 2.8764918192414846,
+                    "timestamp": 1.406311979562183
+                },
+                {
+                    "x": 3.9283154974180072,
+                    "y": 5.355313346500023,
+                    "heading": 3.2945714071744505,
+                    "angularVelocity": 1.1224958813512746e-7,
+                    "velocityX": -2.8670034801237065,
+                    "velocityY": 2.4679810583970268,
+                    "timestamp": 1.5000661115329952
+                },
+                {
+                    "x": 3.6595153829213625,
+                    "y": 5.586688994322917,
+                    "heading": 3.2945714176982936,
+                    "angularVelocity": 1.1224938145766723e-7,
+                    "velocityX": -2.867074856821552,
+                    "velocityY": 2.4678981390913592,
+                    "timestamp": 1.5938202435038074
+                },
+                {
+                    "x": 3.3907152487550194,
+                    "y": 5.818064619294552,
+                    "heading": 3.2945714282221594,
+                    "angularVelocity": 1.1224961995364469e-7,
+                    "velocityX": -2.867075066622423,
+                    "velocityY": 2.4678978953553523,
+                    "timestamp": 1.6875743754746195
+                },
+                {
+                    "x": 3.121915101893736,
+                    "y": 6.049440229517853,
+                    "heading": 3.294571438746038,
+                    "angularVelocity": 1.1224975583094217e-7,
+                    "velocityX": -2.8670752020291417,
+                    "velocityY": 2.467897738046713,
+                    "timestamp": 1.7813285074454317
+                },
+                {
+                    "x": 2.853115082553889,
+                    "y": 6.2808159878868715,
+                    "heading": 3.2945714492725595,
+                    "angularVelocity": 1.1227794602420482e-7,
+                    "velocityX": -2.8670738418604556,
+                    "velocityY": 2.467899318198048,
+                    "timestamp": 1.8750826394162439
+                },
+                {
+                    "x": 2.6318681155961685,
+                    "y": 6.471289698509282,
+                    "heading": 3.3899753271617903,
+                    "angularVelocity": 1.0175965142415218,
+                    "velocityX": -2.3598636380805007,
+                    "velocityY": 2.031630037188212,
+                    "timestamp": 1.968836771387056
+                },
+                {
+                    "x": 2.465933868110727,
+                    "y": 6.614144871792939,
+                    "heading": 3.461571184706241,
+                    "angularVelocity": 0.7636554895173272,
+                    "velocityX": -1.7698872998696302,
+                    "velocityY": 1.5237213579891205,
+                    "timestamp": 2.062590903357868
+                },
+                {
+                    "x": 2.3553113103707712,
+                    "y": 6.709381606049684,
+                    "heading": 3.5093149725923682,
+                    "angularVelocity": 0.5092446261566541,
+                    "velocityX": -1.1799219449271308,
+                    "velocityY": 1.0158137274034427,
+                    "timestamp": 2.1563450353286804
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 3.5331853071795862,
+                    "angularVelocity": 0.2546056806823578,
+                    "velocityX": -0.5899613084572273,
+                    "velocityY": 0.5079071497898447,
+                    "timestamp": 2.2500991672994926
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 3.5331853071795862,
+                    "angularVelocity": 0,
+                    "velocityX": 7.663961659075425e-29,
+                    "velocityY": 7.569331315704837e-30,
+                    "timestamp": 2.3438532992703047
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -5060,7 +16695,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C4 To W2": {
             "waypoints": [
@@ -5071,7 +16722,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 22
                 },
                 {
                     "x": 2.25,
@@ -5083,7 +16734,215 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": 1.048713449145388e-30,
+                    "velocityX": -1.8824383693720305e-31,
+                    "velocityY": -8.291277152276166e-32,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.627273403539296,
+                    "y": 2.514475736726478,
+                    "heading": 3.079415345929856,
+                    "angularVelocity": -0.619115297628915,
+                    "velocityX": -0.5388137138314486,
+                    "velocityY": 0.5873452344657393,
+                    "timestamp": 0.09785681972674802
+                },
+                {
+                    "x": 7.521466473904217,
+                    "y": 2.629566524398062,
+                    "heading": 2.9639191611810785,
+                    "angularVelocity": -1.1802568801161224,
+                    "velocityX": -1.08124226733027,
+                    "velocityY": 1.1761141225819405,
+                    "timestamp": 0.19571363945349604
+                },
+                {
+                    "x": 7.362002347569612,
+                    "y": 2.8026163659937295,
+                    "heading": 2.8066032187897654,
+                    "angularVelocity": -1.6076134788622511,
+                    "velocityX": -1.6295657960261463,
+                    "velocityY": 1.7683983812153936,
+                    "timestamp": 0.29357045918024405
+                },
+                {
+                    "x": 7.14775766144854,
+                    "y": 3.033681724074784,
+                    "heading": 2.6597575656629697,
+                    "angularVelocity": -1.5006174688370466,
+                    "velocityX": -2.18936898541483,
+                    "velocityY": 2.361259631431653,
+                    "timestamp": 0.3914272789069921
+                },
+                {
+                    "x": 6.898594719017662,
+                    "y": 3.307462468622087,
+                    "heading": 2.6597575308458405,
+                    "angularVelocity": -3.5579665549162934e-7,
+                    "velocityX": -2.546199060286565,
+                    "velocityY": 2.7977686717369177,
+                    "timestamp": 0.4892840986337401
+                },
+                {
+                    "x": 6.632320724562211,
+                    "y": 3.5646321159756953,
+                    "heading": 2.6597574950521046,
+                    "angularVelocity": -3.657766164691694e-7,
+                    "velocityX": -2.721057103623286,
+                    "velocityY": 2.628019672739413,
+                    "timestamp": 0.5871409183604881
+                },
+                {
+                    "x": 6.3170953901288,
+                    "y": 3.7587224487937174,
+                    "heading": 2.6597575172185417,
+                    "angularVelocity": 2.265190860696483e-7,
+                    "velocityX": -3.221291426735886,
+                    "velocityY": 1.983411410262395,
+                    "timestamp": 0.6849977380872361
+                },
+                {
+                    "x": 5.989980622324306,
+                    "y": 3.9320256447233213,
+                    "heading": 2.65975753898433,
+                    "angularVelocity": 2.224248489911636e-7,
+                    "velocityX": -3.342789687197243,
+                    "velocityY": 1.770987412155123,
+                    "timestamp": 0.782854557813984
+                },
+                {
+                    "x": 5.662865689540743,
+                    "y": 4.1053285292498645,
+                    "heading": 2.659757560750018,
+                    "angularVelocity": 2.2242382298690162e-7,
+                    "velocityX": -3.3427913731203196,
+                    "velocityY": 1.7709842299235545,
+                    "timestamp": 0.880711377540732
+                },
+                {
+                    "x": 5.335750711483487,
+                    "y": 4.278631328320837,
+                    "heading": 2.659757582515508,
+                    "angularVelocity": 2.2242180190175268e-7,
+                    "velocityX": -3.3427918357727204,
+                    "velocityY": 1.7709833566520703,
+                    "timestamp": 0.97856819726748
+                },
+                {
+                    "x": 5.008636210362509,
+                    "y": 4.451935027622489,
+                    "heading": 2.659757604280525,
+                    "angularVelocity": 2.224169652952219e-7,
+                    "velocityX": -3.342786961955248,
+                    "velocityY": 1.7709925561200404,
+                    "timestamp": 1.076425016994228
+                },
+                {
+                    "x": 4.6815226569833905,
+                    "y": 4.625240515795651,
+                    "heading": 2.6597576260457285,
+                    "angularVelocity": 2.224188696537283e-7,
+                    "velocityX": -3.3427772769699504,
+                    "velocityY": 1.7710108366192054,
+                    "timestamp": 1.174281836720976
+                },
+                {
+                    "x": 4.378427967662734,
+                    "y": 4.837776808087965,
+                    "heading": 2.65975767630399,
+                    "angularVelocity": 5.135897752366777e-7,
+                    "velocityX": -3.097328220628949,
+                    "velocityY": 2.171910888641101,
+                    "timestamp": 1.272138656447724
+                },
+                {
+                    "x": 4.050949875066163,
+                    "y": 5.0103924451271915,
+                    "heading": 2.6597577704243744,
+                    "angularVelocity": 9.61817319833198e-7,
+                    "velocityX": -3.3465025075514396,
+                    "velocityY": 1.7639612397095272,
+                    "timestamp": 1.369995476174472
+                },
+                {
+                    "x": 3.722909272369472,
+                    "y": 5.181936673224133,
+                    "heading": 2.659757864941058,
+                    "angularVelocity": 9.65867108780206e-7,
+                    "velocityX": -3.352250804928058,
+                    "velocityY": 1.7530124990363987,
+                    "timestamp": 1.46785229590122
+                },
+                {
+                    "x": 3.383528958103272,
+                    "y": 5.329784787197771,
+                    "heading": 2.659757940242804,
+                    "angularVelocity": 7.6950943654844e-7,
+                    "velocityX": -3.468131451787155,
+                    "velocityY": 1.5108616281060843,
+                    "timestamp": 1.5657091156279679
+                },
+                {
+                    "x": 3.020395743291134,
+                    "y": 5.401703742215627,
+                    "heading": 2.6597579736142767,
+                    "angularVelocity": 3.4102347755826357e-7,
+                    "velocityX": -3.7108626238430706,
+                    "velocityY": 0.7349406532797214,
+                    "timestamp": 1.6635659353547159
+                },
+                {
+                    "x": 2.7114854008089204,
+                    "y": 5.466141485650731,
+                    "heading": 2.823345730065705,
+                    "angularVelocity": 1.6717052210385108,
+                    "velocityX": -3.156758449179158,
+                    "velocityY": 0.6584900634931665,
+                    "timestamp": 1.7614227550814638
+                },
+                {
+                    "x": 2.4804696774793777,
+                    "y": 5.513370093880283,
+                    "heading": 2.974776278106282,
+                    "angularVelocity": 1.5474705642736624,
+                    "velocityX": -2.3607524133179756,
+                    "velocityY": 0.48262970696813046,
+                    "timestamp": 1.8592795748082118
+                },
+                {
+                    "x": 2.3267564356504185,
+                    "y": 5.544523353838769,
+                    "heading": 3.083396984959406,
+                    "angularVelocity": 1.1099962900534985,
+                    "velocityX": -1.5707974391379447,
+                    "velocityY": 0.3183555325574442,
+                    "timestamp": 1.9571363945349598
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0.5784268812194232,
+                    "velocityX": -0.7843749251687325,
+                    "velocityY": 0.15815603045805554,
+                    "timestamp": 2.054993214261708
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 1.5606110446115462e-29,
+                    "velocityX": 0,
+                    "velocityY": 3.583632692315791e-31,
+                    "timestamp": 2.152850033988456
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -5101,7 +16960,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C4 To W3": {
             "waypoints": [
@@ -5112,7 +16987,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 22
                 },
                 {
                     "x": 2.3,
@@ -5142,7 +17017,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C4 To S3": {
             "waypoints": [
@@ -5183,7 +17074,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C4 To S4": {
             "waypoints": [
@@ -5194,7 +17101,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 15
                 },
                 {
                     "x": 5.176,
@@ -5206,7 +17113,152 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": -1.3041266298020504e-30,
+                    "velocityX": -3.538115329170266e-32,
+                    "velocityY": -5.678450231579595e-32,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.634416744984149,
+                    "y": 2.432986632964242,
+                    "heading": 3.1489798678517915,
+                    "angularVelocity": 0.11416974002165144,
+                    "velocityX": -0.5795439822048055,
+                    "velocityY": -0.3053051466642632,
+                    "timestamp": 0.07865365945555033
+                },
+                {
+                    "x": 7.543250241722733,
+                    "y": 2.384959916525659,
+                    "heading": 3.166940966282971,
+                    "angularVelocity": 0.22835680571645545,
+                    "velocityX": -1.1590878783324414,
+                    "velocityY": -0.6106100691440082,
+                    "timestamp": 0.15730731891110067
+                },
+                {
+                    "x": 7.406500507389113,
+                    "y": 2.312919874583363,
+                    "heading": 3.193885700427137,
+                    "angularVelocity": 0.34257445019952903,
+                    "velocityX": -1.7386315561185093,
+                    "velocityY": -0.9159146877712426,
+                    "timestamp": 0.235960978366651
+                },
+                {
+                    "x": 7.224167573256533,
+                    "y": 2.216866534550912,
+                    "heading": 3.2298175365571806,
+                    "angularVelocity": 0.45683616476039074,
+                    "velocityX": -2.3181748362976387,
+                    "velocityY": -1.2212189578634138,
+                    "timestamp": 0.31461463782220134
+                },
+                {
+                    "x": 6.996251494378262,
+                    "y": 2.0967999260644565,
+                    "heading": 3.2747409040255966,
+                    "angularVelocity": 0.5711541939609761,
+                    "velocityX": -2.8977174165313135,
+                    "velocityY": -1.5265228511625537,
+                    "timestamp": 0.3932682972777517
+                },
+                {
+                    "x": 6.733004702063821,
+                    "y": 1.9581188703597256,
+                    "heading": 3.2747409100209213,
+                    "angularVelocity": 7.622435906772787e-8,
+                    "velocityX": -3.3469109274338362,
+                    "velocityY": -1.7631863115422364,
+                    "timestamp": 0.47192195673330206
+                },
+                {
+                    "x": 6.469756571981624,
+                    "y": 1.819440354058334,
+                    "heading": 3.2747409160161256,
+                    "angularVelocity": 7.622282910230258e-8,
+                    "velocityX": -3.3469279357684036,
+                    "velocityY": -1.7631540256529774,
+                    "timestamp": 0.5505756161888524
+                },
+                {
+                    "x": 6.187670135263903,
+                    "y": 1.7247907391748545,
+                    "heading": 3.2747409182109637,
+                    "angularVelocity": 2.790510194041111e-8,
+                    "velocityX": -3.5864375373041195,
+                    "velocityY": -1.2033720431910602,
+                    "timestamp": 0.6292292756444028
+                },
+                {
+                    "x": 5.905373163009314,
+                    "y": 1.6806498188741994,
+                    "heading": 3.236681586369982,
+                    "angularVelocity": -0.48388507419022697,
+                    "velocityX": -3.5891142790899004,
+                    "velocityY": -0.561206186796692,
+                    "timestamp": 0.7078829350999531
+                },
+                {
+                    "x": 5.661530976081841,
+                    "y": 1.6531145750479088,
+                    "heading": 3.0865007795970714,
+                    "angularVelocity": -1.9093937626358353,
+                    "velocityX": -3.1002014224814998,
+                    "velocityY": -0.3500821705803985,
+                    "timestamp": 0.7865365945555035
+                },
+                {
+                    "x": 5.4654145585609575,
+                    "y": 1.6369706610279315,
+                    "heading": 2.879148606187945,
+                    "angularVelocity": -2.6362686090442846,
+                    "velocityX": -2.4934175838533594,
+                    "velocityY": -0.20525318378988838,
+                    "timestamp": 0.8651902540110539
+                },
+                {
+                    "x": 5.319930503793118,
+                    "y": 1.6274302419537983,
+                    "heading": 2.697897950588003,
+                    "angularVelocity": -2.304414783171942,
+                    "velocityX": -1.8496794144722244,
+                    "velocityY": -0.12129656954518526,
+                    "timestamp": 0.9438439134666042
+                },
+                {
+                    "x": 5.223761655113151,
+                    "y": 1.6221627992964307,
+                    "heading": 2.568061248027121,
+                    "angularVelocity": -1.6507395009924064,
+                    "velocityX": -1.2226875309509793,
+                    "velocityY": -0.0669700900610273,
+                    "timestamp": 1.0224975729221546
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": -0.8653284347892848,
+                    "velocityX": -0.6072400883031053,
+                    "velocityY": -0.027497758036964814,
+                    "timestamp": 1.1011512323777048
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": 0,
+                    "velocityX": -1.7743974032793004e-34,
+                    "velocityY": 1.752346488498292e-33,
+                    "timestamp": 1.179804891833255
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -5224,7 +17276,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C4 To C1": {
             "waypoints": [
@@ -5235,7 +17303,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 18
                 },
                 {
                     "x": 7.68,
@@ -5247,7 +17315,179 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": 1.7868231110584375e-31,
+                    "velocityX": 3.500790942409583e-35,
+                    "velocityY": -4.724665555223456e-38,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.6800001156222075,
+                    "y": 2.5388441539517457,
+                    "heading": 3.140000017932032,
+                    "angularVelocity": 1.8099644459911076e-7,
+                    "velocityX": 0.0000011670295392084272,
+                    "velocityY": 0.8260917454273137,
+                    "timestamp": 0.09907392781101554
+                },
+                {
+                    "x": 7.680000338204962,
+                    "y": 2.7025324587418944,
+                    "heading": 3.1400000579849436,
+                    "angularVelocity": 4.042728942464055e-7,
+                    "velocityX": 0.000002246632826435788,
+                    "velocityY": 1.6521834594301972,
+                    "timestamp": 0.19814785562203108
+                },
+                {
+                    "x": 7.680000658923826,
+                    "y": 2.948064908143757,
+                    "heading": 3.1400001223157976,
+                    "angularVelocity": 6.493215714833696e-7,
+                    "velocityX": 0.0000032371668547106284,
+                    "velocityY": 2.4782751105841747,
+                    "timestamp": 0.2972217834330466
+                },
+                {
+                    "x": 7.6800010687648195,
+                    "y": 3.2754414834772896,
+                    "heading": 3.140000210384731,
+                    "angularVelocity": 8.889210892181502e-7,
+                    "velocityX": 0.0000041367184645969174,
+                    "velocityY": 3.3043665731916554,
+                    "timestamp": 0.39629571124406215
+                },
+                {
+                    "x": 7.6800013918601255,
+                    "y": 3.650232264926723,
+                    "heading": 3.140000210384734,
+                    "angularVelocity": 3.2741652225293424e-14,
+                    "velocityX": 0.000003261153797251531,
+                    "velocityY": 3.7829405750859055,
+                    "timestamp": 0.4953696390550777
+                },
+                {
+                    "x": 7.680001628786808,
+                    "y": 4.025023046376236,
+                    "heading": 3.140000210384741,
+                    "angularVelocity": 7.304626800025654e-14,
+                    "velocityX": 0.00000239141289972485,
+                    "velocityY": 3.782940575086779,
+                    "timestamp": 0.5944435668660932
+                },
+                {
+                    "x": 7.6800017795018,
+                    "y": 4.399813827825781,
+                    "heading": 3.1400002103847475,
+                    "angularVelocity": 7.220243449496737e-14,
+                    "velocityX": 0.0000015212375955225791,
+                    "velocityY": 3.782940575087123,
+                    "timestamp": 0.6935174946771088
+                },
+                {
+                    "x": 7.680001844060805,
+                    "y": 4.774604609275342,
+                    "heading": 3.140000210384754,
+                    "angularVelocity": 6.886052815923548e-14,
+                    "velocityX": 6.516245436906888e-7,
+                    "velocityY": 3.782940575087321,
+                    "timestamp": 0.7925914224881243
+                },
+                {
+                    "x": 7.680001822974652,
+                    "y": 5.14939539072491,
+                    "heading": 3.1400002103847604,
+                    "angularVelocity": 6.934745116017498e-14,
+                    "velocityX": -2.1283245913448782e-7,
+                    "velocityY": 3.782940575087379,
+                    "timestamp": 0.8916653502991398
+                },
+                {
+                    "x": 7.680001718396276,
+                    "y": 5.5241861721744705,
+                    "heading": 3.140000210384768,
+                    "angularVelocity": 8.52485669839133e-14,
+                    "velocityX": -0.0000010555588768470353,
+                    "velocityY": 3.7829405750872986,
+                    "timestamp": 0.9907392781101554
+                },
+                {
+                    "x": 7.680001534024895,
+                    "y": 5.898976953624011,
+                    "heading": 3.140000210384774,
+                    "angularVelocity": 6.71024495132872e-14,
+                    "velocityX": -0.000001860947354605527,
+                    "velocityY": 3.7829405750870757,
+                    "timestamp": 1.089813205921171
+                },
+                {
+                    "x": 7.680001275096042,
+                    "y": 6.273767735073519,
+                    "heading": 3.140000210384774,
+                    "angularVelocity": 2.6424584851850765e-16,
+                    "velocityX": -0.000002613491149837329,
+                    "velocityY": 3.7829405750867053,
+                    "timestamp": 1.1888871337321865
+                },
+                {
+                    "x": 7.680000948769078,
+                    "y": 6.648558516522932,
+                    "heading": 3.1400002103847724,
+                    "angularVelocity": -2.22727936711479e-14,
+                    "velocityX": -0.000003293772326272547,
+                    "velocityY": 3.782940575085685,
+                    "timestamp": 1.287961061543202
+                },
+                {
+                    "x": 7.680000579847807,
+                    "y": 6.975935091856332,
+                    "heading": 3.140000126724757,
+                    "angularVelocity": -8.444197945421442e-7,
+                    "velocityX": -0.0000037236965492526806,
+                    "velocityY": 3.3043665731903227,
+                    "timestamp": 1.3870349893542175
+                },
+                {
+                    "x": 7.680000295053659,
+                    "y": 7.221467541258139,
+                    "heading": 3.14000006325574,
+                    "angularVelocity": -6.40622649068188e-7,
+                    "velocityX": -0.000002874561832669146,
+                    "velocityY": 2.4782751105836276,
+                    "timestamp": 1.486108917165233
+                },
+                {
+                    "x": 7.680000100017398,
+                    "y": 7.385155846048263,
+                    "heading": 3.14000002096013,
+                    "angularVelocity": -4.269095158176857e-7,
+                    "velocityX": -0.000001968593124089932,
+                    "velocityY": 1.6521834594299558,
+                    "timestamp": 1.5851828449762486
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "angularVelocity": -2.115604748661088e-7,
+                    "velocityX": -0.0000010095228578568797,
+                    "velocityY": 0.8260917454272356,
+                    "timestamp": 1.6842567727872642
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "angularVelocity": 8.944588154710766e-32,
+                    "velocityX": 0,
+                    "velocityY": -4.821363548294539e-38,
+                    "timestamp": 1.7833307005982797
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -5265,7 +17505,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C4 To C2": {
             "waypoints": [
@@ -5276,7 +17532,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 14
                 },
                 {
                     "x": 7.68,
@@ -5288,7 +17544,143 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": -9.54519608761012e-33,
+                    "velocityX": 3.049131565114831e-33,
+                    "velocityY": -1.5169993504833503e-38,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.68000007508226,
+                    "y": 2.533445372005505,
+                    "heading": 3.1399999978468434,
+                    "angularVelocity": -2.248715497169588e-8,
+                    "velocityX": 7.84144633669789e-7,
+                    "velocityY": 0.7983807350943783,
+                    "timestamp": 0.09575052183150116
+                },
+                {
+                    "x": 7.680000217378094,
+                    "y": 2.6863361129826027,
+                    "heading": 3.1399999975552166,
+                    "angularVelocity": -3.0456922527341925e-9,
+                    "velocityX": 0.0000014861101970902313,
+                    "velocityY": 1.5967614385031632,
+                    "timestamp": 0.19150104366300233
+                },
+                {
+                    "x": 7.680000418878438,
+                    "y": 2.915672216863469,
+                    "heading": 3.1400000014113987,
+                    "angularVelocity": 4.027321769334868e-8,
+                    "velocityX": 0.000002104430661113429,
+                    "velocityY": 2.395142078540767,
+                    "timestamp": 0.2872515654945035
+                },
+                {
+                    "x": 7.680000671408876,
+                    "y": 3.2214536654446886,
+                    "heading": 3.14000000940269,
+                    "angularVelocity": 8.345949630994641e-8,
+                    "velocityX": 0.0000026373790047185906,
+                    "velocityY": 3.1935225284654267,
+                    "timestamp": 0.38300208732600466
+                },
+                {
+                    "x": 7.680000833444467,
+                    "y": 3.5836721992668776,
+                    "heading": 3.1400000094026845,
+                    "angularVelocity": -5.801733416228813e-14,
+                    "velocityX": 0.0000016922687336334932,
+                    "velocityY": 3.7829405719543456,
+                    "timestamp": 0.47875260915750584
+                },
+                {
+                    "x": 7.680000914400183,
+                    "y": 3.9458907330889588,
+                    "heading": 3.140000009402682,
+                    "angularVelocity": -2.5236519267762366e-14,
+                    "velocityX": 8.45485885682039e-7,
+                    "velocityY": 3.78294057195325,
+                    "timestamp": 0.574503130989007
+                },
+                {
+                    "x": 7.68000091414698,
+                    "y": 4.308109266911049,
+                    "heading": 3.1400000094026788,
+                    "angularVelocity": -3.379642410236896e-14,
+                    "velocityX": -2.644395591425324e-9,
+                    "velocityY": 3.782940571953342,
+                    "timestamp": 0.6702536528205081
+                },
+                {
+                    "x": 7.680000832736705,
+                    "y": 4.67032780073313,
+                    "heading": 3.140000009402675,
+                    "angularVelocity": -3.7410067203640955e-14,
+                    "velocityX": -8.502332108268853e-7,
+                    "velocityY": 3.782940571953249,
+                    "timestamp": 0.7660041746520092
+                },
+                {
+                    "x": 7.680000670417917,
+                    "y": 5.032546334555316,
+                    "heading": 3.1400000094026708,
+                    "angularVelocity": -4.92147091530942e-14,
+                    "velocityX": -0.0000016952263834753921,
+                    "velocityY": 3.7829405719543154,
+                    "timestamp": 0.8617546964835103
+                },
+                {
+                    "x": 7.680000418190559,
+                    "y": 5.338327783136532,
+                    "heading": 3.1400000097695924,
+                    "angularVelocity": 3.832069362065189e-9,
+                    "velocityX": -0.00000263421371002401,
+                    "velocityY": 3.1935225284653987,
+                    "timestamp": 0.9575052183150115
+                },
+                {
+                    "x": 7.68000021698581,
+                    "y": 5.567663887017397,
+                    "heading": 3.140000007984905,
+                    "angularVelocity": -1.863892342341337e-8,
+                    "velocityX": -0.0000021013435063173737,
+                    "velocityY": 2.3951420785407556,
+                    "timestamp": 1.0532557401465126
+                },
+                {
+                    "x": 7.680000074934953,
+                    "y": 5.7205546279944945,
+                    "heading": 3.140000003983309,
+                    "angularVelocity": -4.1791894261559745e-8,
+                    "velocityX": -0.0000014835517222041492,
+                    "velocityY": 1.5967614385031577,
+                    "timestamp": 1.1490062619780137
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": -4.160090541448797e-8,
+                    "velocityX": -7.826061846711994e-7,
+                    "velocityY": 0.7983807350943762,
+                    "timestamp": 1.2447567838095148
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 3.04245898705875e-32,
+                    "velocityY": 2.558785218352301e-38,
+                    "timestamp": 1.340507305641016
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -5302,11 +17694,27 @@
                     ],
                     "type": "StopPoint"
                 }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C4 To C3": {
             "waypoints": [
@@ -5317,7 +17725,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 9
                 },
                 {
                     "x": 7.68,
@@ -5329,7 +17737,98 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": -3.7532975982701904e-32,
+                    "velocityY": 5.0637303451538295e-36,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.680000036889488,
+                    "y": 2.540500005575837,
+                    "heading": 3.1400000022500048,
+                    "angularVelocity": 2.2484053179234407e-8,
+                    "velocityX": 3.6863266642951275e-7,
+                    "velocityY": 0.8344065481227464,
+                    "timestamp": 0.1000711293118395
+                },
+                {
+                    "x": 7.680000101870814,
+                    "y": 2.707500013629824,
+                    "heading": 3.140000010932795,
+                    "angularVelocity": 8.676618158835649e-8,
+                    "velocityX": 6.49351381942043e-7,
+                    "velocityY": 1.6688130652906412,
+                    "timestamp": 0.200142258623679
+                },
+                {
+                    "x": 7.680000185978468,
+                    "y": 2.958000017966584,
+                    "heading": 3.1400000281272393,
+                    "angularVelocity": 1.7182222560804709e-7,
+                    "velocityX": 8.404786987428781e-7,
+                    "velocityY": 2.503219520548802,
+                    "timestamp": 0.3002133879355185
+                },
+                {
+                    "x": 7.68000028005223,
+                    "y": 3.2919999999999923,
+                    "heading": 3.1400000531214163,
+                    "angularVelocity": 2.497641049380253e-7,
+                    "velocityX": 9.400689316139974e-7,
+                    "velocityY": 3.3376257900778246,
+                    "timestamp": 0.400284517247358
+                },
+                {
+                    "x": 7.680000187136683,
+                    "y": 3.6259999820334055,
+                    "heading": 3.140000035091715,
+                    "angularVelocity": -1.801688496007097e-7,
+                    "velocityX": -9.284950198395514e-7,
+                    "velocityY": 3.337625790077875,
+                    "timestamp": 0.5003556465591975
+                },
+                {
+                    "x": 7.680000103123094,
+                    "y": 3.8764999863701703,
+                    "heading": 3.1400000189895447,
+                    "angularVelocity": -1.609072425985215e-7,
+                    "velocityX": -8.395387126580118e-7,
+                    "velocityY": 2.5032195205488503,
+                    "timestamp": 0.600426775871037
+                },
+                {
+                    "x": 7.680000037559297,
+                    "y": 4.043499994424161,
+                    "heading": 3.140000006758861,
+                    "angularVelocity": -1.222199010476452e-7,
+                    "velocityX": -6.55171951368837e-7,
+                    "velocityY": 1.6688130652906763,
+                    "timestamp": 0.7004979051828765
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": -6.754056559007022e-8,
+                    "velocityX": -3.753259946110928e-7,
+                    "velocityY": 0.8344065481227645,
+                    "timestamp": 0.800569034494716
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": 3.7078876706224725e-32,
+                    "velocityX": 0,
+                    "velocityY": -1.600749389553648e-35,
+                    "timestamp": 0.9006401638065554
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -5347,7 +17846,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C4 To C5": {
             "waypoints": [
@@ -5358,7 +17873,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 9
                 },
                 {
                     "x": 7.68,
@@ -5388,7 +17903,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C5 To W1": {
             "waypoints": [
@@ -5399,19 +17930,281 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 28
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 3.5331853071795862,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": -1.8495960722802552e-34,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.66217209907118,
+                    "y": 0.8633707808076446,
+                    "heading": 3.200268562588625,
+                    "angularVelocity": 0.6146717037531945,
+                    "velocityX": -0.18182458262791865,
+                    "velocityY": 0.7788962593386439,
+                    "timestamp": 0.09805000331172546
+                },
+                {
+                    "x": 7.626250868838738,
+                    "y": 1.0164003464766511,
+                    "heading": 3.3145470628036264,
+                    "angularVelocity": 1.1655124564522605,
+                    "velocityX": -0.3663562368094907,
+                    "velocityY": 1.5607298368209865,
+                    "timestamp": 0.19610000662345092
+                },
+                {
+                    "x": 7.571819523432209,
+                    "y": 1.2466141084056221,
+                    "heading": 3.4692056792172115,
+                    "angularVelocity": 1.5773443262606215,
+                    "velocityX": -0.555138639143922,
+                    "velocityY": 2.347922020941335,
+                    "timestamp": 0.29415000993517637
+                },
+                {
+                    "x": 7.4978548812850345,
+                    "y": 1.55456588878344,
+                    "heading": 3.620198528248394,
+                    "angularVelocity": 1.5399576127615076,
+                    "velocityX": -0.7543563452213444,
+                    "velocityY": 3.1407625698773547,
+                    "timestamp": 0.39220001324690185
+                },
+                {
+                    "x": 7.414564781883039,
+                    "y": 1.916010778899352,
+                    "heading": 3.6201985877633485,
+                    "angularVelocity": 6.069857444827113e-7,
+                    "velocityX": -0.8494655439959167,
+                    "velocityY": 3.6863322581110816,
+                    "timestamp": 0.4902500165586273
+                },
+                {
+                    "x": 7.3312746982625,
+                    "y": 2.27745567265221,
+                    "heading": 3.620198647277482,
+                    "angularVelocity": 6.069773758970038e-7,
+                    "velocityX": -0.8494653830427515,
+                    "velocityY": 3.686332295203854,
+                    "timestamp": 0.5883000198703527
+                },
+                {
+                    "x": 7.247984475288188,
+                    "y": 2.6389005342928713,
+                    "heading": 3.6201987067915797,
+                    "angularVelocity": 6.069770050551867e-7,
+                    "velocityX": -0.8494668042948591,
+                    "velocityY": 3.686331967695479,
+                    "timestamp": 0.6863500231820782
+                },
+                {
+                    "x": 7.16025127777078,
+                    "y": 2.999292730685755,
+                    "heading": 3.620198766578442,
+                    "angularVelocity": 6.097589007693709e-7,
+                    "velocityX": -0.8947801586348038,
+                    "velocityY": 3.675595963491274,
+                    "timestamp": 0.7844000264938036
+                },
+                {
+                    "x": 6.997140841431304,
+                    "y": 3.3324212817743484,
+                    "heading": 3.6201988348555445,
+                    "angularVelocity": 6.963498230184926e-7,
+                    "velocityX": -1.6635434046943107,
+                    "velocityY": 3.3975373772247037,
+                    "timestamp": 0.882450029805529
+                },
+                {
+                    "x": 6.766266862430273,
+                    "y": 3.622726019325249,
+                    "heading": 3.620198931073779,
+                    "angularVelocity": 9.813180151910862e-7,
+                    "velocityX": -2.354655494166845,
+                    "velocityY": 2.9607825369260774,
+                    "timestamp": 0.9805000331172544
+                },
+                {
+                    "x": 6.478412432979381,
+                    "y": 3.856647861556034,
+                    "heading": 3.62019912006771,
+                    "angularVelocity": 0.000001927525998631886,
+                    "velocityX": -2.935792144093369,
+                    "velocityY": 2.38574027873399,
+                    "timestamp": 1.07855003642898
+                },
+                {
+                    "x": 6.147022412583985,
+                    "y": 4.023260998419484,
+                    "heading": 3.6202003231426523,
+                    "angularVelocity": 0.000012270014292625344,
+                    "velocityX": -3.3798063151698643,
+                    "velocityY": 1.6992670192345174,
+                    "timestamp": 1.1766000397407055
+                },
+                {
+                    "x": 5.787574046048744,
+                    "y": 4.114783912760558,
+                    "heading": 3.6201999548728017,
+                    "angularVelocity": -0.0000037559391933502495,
+                    "velocityX": -3.6659699581290637,
+                    "velocityY": 0.9334310173361262,
+                    "timestamp": 1.274650043052431
+                },
+                {
+                    "x": 5.417217638191157,
+                    "y": 4.135174501302961,
+                    "heading": 3.620199996859582,
+                    "angularVelocity": 4.2821804063172526e-7,
+                    "velocityX": -3.7772197383831925,
+                    "velocityY": 0.2079611203844201,
+                    "timestamp": 1.3727000463641565
+                },
+                {
+                    "x": 5.066383608065517,
+                    "y": 4.23317852630269,
+                    "heading": 3.636482368230358,
+                    "angularVelocity": 0.16606191556170027,
+                    "velocityX": -3.578113393940955,
+                    "velocityY": 0.9995310728154599,
+                    "timestamp": 1.470750049675882
+                },
+                {
+                    "x": 4.742245396435021,
+                    "y": 4.406733140775732,
+                    "heading": 3.644586480575013,
+                    "angularVelocity": 0.08265285130985205,
+                    "velocityX": -3.305846003900474,
+                    "velocityY": 1.7700623009799354,
+                    "timestamp": 1.5688000529876076
+                },
+                {
+                    "x": 4.460670694136194,
+                    "y": 4.648177372257919,
+                    "heading": 3.644586687046199,
+                    "angularVelocity": 0.000002105774391224538,
+                    "velocityX": -2.87174597438442,
+                    "velocityY": 2.4624602073145714,
+                    "timestamp": 1.6668500562993331
+                },
+                {
+                    "x": 4.237545979665016,
+                    "y": 4.944479621179968,
+                    "heading": 3.644586791013843,
+                    "angularVelocity": 0.0000010603532960842864,
+                    "velocityX": -2.2756216923502683,
+                    "velocityY": 3.0219504223781635,
+                    "timestamp": 1.7649000596110587
+                },
+                {
+                    "x": 3.9950087307179025,
+                    "y": 5.225113475172281,
+                    "heading": 3.6445867953690434,
+                    "angularVelocity": 4.441815614419627e-8,
+                    "velocityX": -2.473607758849603,
+                    "velocityY": 2.862150377497781,
+                    "timestamp": 1.8629500629227842
+                },
+                {
+                    "x": 3.7198234606165768,
+                    "y": 5.473815593691706,
+                    "heading": 3.644586791690717,
+                    "angularVelocity": -3.7514802268686446e-8,
+                    "velocityX": -2.8065809363253424,
+                    "velocityY": 2.536482510140642,
+                    "timestamp": 1.9610000662345097
+                },
+                {
+                    "x": 3.4446380366180587,
+                    "y": 5.722517541926013,
+                    "heading": 3.6445867880123934,
+                    "angularVelocity": -3.751477003592028e-8,
+                    "velocityX": -2.8065825059040024,
+                    "velocityY": 2.5364807734235555,
+                    "timestamp": 2.059050069546235
+                },
+                {
+                    "x": 3.1694525996169167,
+                    "y": 5.97121947577308,
+                    "heading": 3.6445867843340722,
+                    "angularVelocity": -3.75147461397837e-8,
+                    "velocityX": -2.806582638516174,
+                    "velocityY": 2.5364806266898405,
+                    "timestamp": 2.1571000728579603
+                },
+                {
+                    "x": 2.894267154657697,
+                    "y": 6.2199214008146,
+                    "heading": 3.6445867806557373,
+                    "angularVelocity": -3.751489444763293e-8,
+                    "velocityX": -2.806582719679637,
+                    "velocityY": 2.53648053688316,
+                    "timestamp": 2.2551500761696857
+                },
+                {
+                    "x": 2.6565603153930746,
+                    "y": 6.434752825744944,
+                    "heading": 3.60002452914154,
+                    "angularVelocity": -0.45448495674724715,
+                    "velocityX": -2.4243430008757167,
+                    "velocityY": 2.191039445938014,
+                    "timestamp": 2.353200079481411
+                },
+                {
+                    "x": 2.478280163108621,
+                    "y": 6.595876410073326,
+                    "heading": 3.56660458464388,
+                    "angularVelocity": -0.3408459293102701,
+                    "velocityX": -1.8182574835582244,
+                    "velocityY": 1.6432797438683373,
+                    "timestamp": 2.4512500827931363
+                },
+                {
+                    "x": 2.3594267213059505,
+                    "y": 6.703292136714203,
+                    "heading": 3.544325120911587,
+                    "angularVelocity": -0.2272255275857591,
+                    "velocityX": -1.2121717265506466,
+                    "velocityY": 1.0955198675452942,
+                    "timestamp": 2.5493000861048616
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 3.5331853071795862,
+                    "angularVelocity": -0.1136135987327294,
+                    "velocityX": -0.6060858674019433,
+                    "velocityY": 0.5477599334193345,
+                    "timestamp": 2.647350089416587
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 3.5331853071795862,
+                    "angularVelocity": 0,
+                    "velocityX": -1.9773011985788902e-32,
+                    "velocityY": -8.72670829182521e-33,
+                    "timestamp": 2.745400092728312
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -5429,7 +18222,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C5 To W2": {
             "waypoints": [
@@ -5470,7 +18279,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C5 To W3": {
             "waypoints": [
@@ -5511,7 +18336,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C5 To S3": {
             "waypoints": [
@@ -5522,7 +18363,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 19
                 },
                 {
                     "x": 4.263,
@@ -5534,7 +18375,188 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": 9.969319932712997e-31,
+                    "velocityX": 3.5015968931749707e-32,
+                    "velocityY": -1.3449299836267862e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.622998015019161,
+                    "y": 0.8057692963239994,
+                    "heading": 3.0732063030265673,
+                    "angularVelocity": -0.7701918865802806,
+                    "velocityX": -0.6572845693610088,
+                    "velocityY": 0.21642700435214937,
+                    "timestamp": 0.08672344923029876
+                },
+                {
+                    "x": 7.508611650111109,
+                    "y": 0.8437411084542301,
+                    "heading": 2.9459376863662614,
+                    "angularVelocity": -1.4675225419406142,
+                    "velocityX": -1.318978499163396,
+                    "velocityY": 0.43784942212566424,
+                    "timestamp": 0.17344689846059752
+                },
+                {
+                    "x": 7.336139052754301,
+                    "y": 0.9014260823029367,
+                    "heading": 2.7711998622663616,
+                    "angularVelocity": -2.014885543076985,
+                    "velocityX": -1.9887654248944688,
+                    "velocityY": 0.6651600502595449,
+                    "timestamp": 0.2601703476908963
+                },
+                {
+                    "x": 7.104576647516153,
+                    "y": 0.9793814888777497,
+                    "heading": 2.5768200425700405,
+                    "angularVelocity": -2.241375561298711,
+                    "velocityX": -2.6701244852844987,
+                    "velocityY": 0.8988965183776096,
+                    "timestamp": 0.34689379692119504
+                },
+                {
+                    "x": 6.822163575098835,
+                    "y": 1.078219287033468,
+                    "heading": 2.4981665441447336,
+                    "angularVelocity": -0.9069461503593836,
+                    "velocityX": -3.2564787831184523,
+                    "velocityY": 1.1396894269420643,
+                    "timestamp": 0.4336172461514938
+                },
+                {
+                    "x": 6.510888238397409,
+                    "y": 1.1818405008088049,
+                    "heading": 2.498166488214478,
+                    "angularVelocity": -6.449265521718259e-7,
+                    "velocityX": -3.5892868591379226,
+                    "velocityY": 1.194846546061207,
+                    "timestamp": 0.5203406953817925
+                },
+                {
+                    "x": 6.199612902811833,
+                    "y": 1.2854617179365646,
+                    "heading": 2.498166432284442,
+                    "angularVelocity": -6.449240327985389e-7,
+                    "velocityX": -3.589286846271159,
+                    "velocityY": 1.194846584717682,
+                    "timestamp": 0.6070641446120912
+                },
+                {
+                    "x": 5.8883375891897005,
+                    "y": 1.389083001041933,
+                    "heading": 2.4981663763544435,
+                    "angularVelocity": -6.449235898174935e-7,
+                    "velocityX": -3.5892865930127487,
+                    "velocityY": 1.1948473454993283,
+                    "timestamp": 0.69378759384239
+                },
+                {
+                    "x": 5.577064983818878,
+                    "y": 1.4927124193076864,
+                    "heading": 2.4981663204239246,
+                    "angularVelocity": -6.449295917693127e-7,
+                    "velocityX": -3.589255364419621,
+                    "velocityY": 1.194941151274556,
+                    "timestamp": 0.7805110430726887
+                },
+                {
+                    "x": 5.275080475665308,
+                    "y": 1.6209116670289339,
+                    "heading": 2.4981662550669967,
+                    "angularVelocity": -7.536246362770858e-7,
+                    "velocityX": -3.4821551821772507,
+                    "velocityY": 1.4782535618573864,
+                    "timestamp": 0.8672344923029874
+                },
+                {
+                    "x": 4.987205237443244,
+                    "y": 1.7782569230783876,
+                    "heading": 2.49816614132643,
+                    "angularVelocity": -0.000001311531862273131,
+                    "velocityX": -3.319462507281,
+                    "velocityY": 1.8143334639702267,
+                    "timestamp": 0.9539579415332862
+                },
+                {
+                    "x": 4.728299565339017,
+                    "y": 1.9797451797609154,
+                    "heading": 2.498165933556362,
+                    "angularVelocity": -0.0000023957772582798155,
+                    "velocityX": -2.9854171438302592,
+                    "velocityY": 2.3233422848238554,
+                    "timestamp": 1.040681390763585
+                },
+                {
+                    "x": 4.523329416895473,
+                    "y": 2.207660623196326,
+                    "heading": 2.432888839209935,
+                    "angularVelocity": -0.7527040832183707,
+                    "velocityX": -2.3634916537882953,
+                    "velocityY": 2.6280717090733883,
+                    "timestamp": 1.1274048399938839
+                },
+                {
+                    "x": 4.381013836981984,
+                    "y": 2.433217084695142,
+                    "heading": 2.3632823552491717,
+                    "angularVelocity": -0.8026258708405342,
+                    "velocityX": -1.6410276710231155,
+                    "velocityY": 2.600870508503859,
+                    "timestamp": 1.2141282892241827
+                },
+                {
+                    "x": 4.294783443237854,
+                    "y": 2.6401553266407616,
+                    "heading": 2.3696134751420326,
+                    "angularVelocity": 0.07300355266138053,
+                    "velocityX": -0.9943146232011729,
+                    "velocityY": 2.386185556297287,
+                    "timestamp": 1.3008517384544815
+                },
+                {
+                    "x": 4.26053013180991,
+                    "y": 2.816849773520389,
+                    "heading": 2.4414538337655816,
+                    "angularVelocity": 0.8283844711108445,
+                    "velocityX": -0.39497173753989284,
+                    "velocityY": 2.0374471777570338,
+                    "timestamp": 1.3875751876847804
+                },
+                {
+                    "x": 4.257469858369506,
+                    "y": 2.9392038098412794,
+                    "heading": 2.522566294218638,
+                    "angularVelocity": 0.9353002120298306,
+                    "velocityX": -0.035287727455096035,
+                    "velocityY": 1.4108529746778473,
+                    "timestamp": 1.4742986369150792
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": 0.5930772615463757,
+                    "velocityX": 0.06376754706571511,
+                    "velocityY": 0.7010351951900964,
+                    "timestamp": 1.561022086145378
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": -5.347216163770528e-32,
+                    "velocityX": 0,
+                    "velocityY": -5.6953276102603335e-33,
+                    "timestamp": 1.647745535375677
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -5552,7 +18574,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C5 To S4": {
             "waypoints": [
@@ -5563,7 +18601,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 15
                 },
                 {
                     "x": 5.176,
@@ -5575,7 +18613,152 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": 7.117315018424497e-31,
+                    "velocityX": 3.987995408855834e-32,
+                    "velocityY": -2.0629863005203966e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.633523057017953,
+                    "y": 0.8024232011655344,
+                    "heading": 3.1133262335807257,
+                    "angularVelocity": -0.3460902136380606,
+                    "velocityX": -0.6030350147430711,
+                    "velocityY": 0.20011493324412974,
+                    "timestamp": 0.07707171531631223
+                },
+                {
+                    "x": 7.540480034234958,
+                    "y": 0.8333108599665054,
+                    "heading": 3.0624083977372165,
+                    "angularVelocity": -0.6606552823501594,
+                    "velocityX": -1.2072265733432221,
+                    "velocityY": 0.4007651662377571,
+                    "timestamp": 0.15414343063262445
+                },
+                {
+                    "x": 7.40074665357272,
+                    "y": 0.8797155912265175,
+                    "heading": 2.9910865272302494,
+                    "angularVelocity": -0.9253961743845064,
+                    "velocityX": -1.813030631130417,
+                    "velocityY": 0.6020980728086949,
+                    "timestamp": 0.23121514594893666
+                },
+                {
+                    "x": 7.214152290317413,
+                    "y": 0.941701209252225,
+                    "heading": 2.9061891700546396,
+                    "angularVelocity": -1.101537144037597,
+                    "velocityX": -2.4210485323896127,
+                    "velocityY": 0.804258965449392,
+                    "timestamp": 0.3082868612652489
+                },
+                {
+                    "x": 6.98056152298191,
+                    "y": 1.0193157827445936,
+                    "heading": 2.8228339071694943,
+                    "angularVelocity": -1.0815285807905615,
+                    "velocityX": -3.0308235177693503,
+                    "velocityY": 1.007043546050951,
+                    "timestamp": 0.38535857658156114
+                },
+                {
+                    "x": 6.704462217906169,
+                    "y": 1.1110743550526192,
+                    "heading": 2.820724227783145,
+                    "angularVelocity": -0.027372939316202514,
+                    "velocityX": -3.582368758014451,
+                    "velocityY": 1.1905609201953868,
+                    "timestamp": 0.4624302918978734
+                },
+                {
+                    "x": 6.4277914338210325,
+                    "y": 1.2030481222509408,
+                    "heading": 2.8207241988506935,
+                    "angularVelocity": -3.753964944317793e-7,
+                    "velocityX": -3.589783657333227,
+                    "velocityY": 1.1933530585228214,
+                    "timestamp": 0.5395020072141856
+                },
+                {
+                    "x": 6.151120668434435,
+                    "y": 1.2950219456972574,
+                    "heading": 2.8207241699182544,
+                    "angularVelocity": -3.753963293976773e-7,
+                    "velocityX": -3.5897834147210324,
+                    "velocityY": 1.1933537883365417,
+                    "timestamp": 0.6165737225304978
+                },
+                {
+                    "x": 5.875226516165077,
+                    "y": 1.387237765393963,
+                    "heading": 2.8184428738821614,
+                    "angularVelocity": -0.029599653085833715,
+                    "velocityX": -3.5797069150083383,
+                    "velocityY": 1.1964936723964186,
+                    "timestamp": 0.69364543784681
+                },
+                {
+                    "x": 5.641686532929837,
+                    "y": 1.4649564505690207,
+                    "heading": 2.7347434741228605,
+                    "angularVelocity": -1.0859937321465907,
+                    "velocityX": -3.0301645976966087,
+                    "velocityY": 1.0083943876958055,
+                    "timestamp": 0.7707171531631222
+                },
+                {
+                    "x": 5.455142642963306,
+                    "y": 1.5270452896537086,
+                    "heading": 2.6494963329119967,
+                    "angularVelocity": -1.1060755669054276,
+                    "velocityX": -2.4203936450737,
+                    "velocityY": 0.8055982513152498,
+                    "timestamp": 0.8477888684794344
+                },
+                {
+                    "x": 5.315456646631149,
+                    "y": 1.5735510161760224,
+                    "heading": 2.577919230350482,
+                    "angularVelocity": -0.928707792057741,
+                    "velocityX": -1.812415822832897,
+                    "velocityY": 0.6034084791216642,
+                    "timestamp": 0.9248605837957465
+                },
+                {
+                    "x": 5.222452869215715,
+                    "y": 1.6045247487520136,
+                    "heading": 2.5268085106209885,
+                    "angularVelocity": -0.6631579369906109,
+                    "velocityX": -1.2067173675029015,
+                    "velocityY": 0.4018819673192824,
+                    "timestamp": 1.0019322991120587
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": -0.3478385100287815,
+                    "velocityX": -0.6027226593448243,
+                    "velocityY": 0.200790279345336,
+                    "timestamp": 1.079004014428371
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": -3.134288783198311e-31,
+                    "velocityX": -2.9694883024269136e-32,
+                    "velocityY": 4.283237987843841e-31,
+                    "timestamp": 1.1560757297446833
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -5593,7 +18776,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C5 To C1": {
             "waypoints": [
@@ -5604,7 +18803,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 23
                 },
                 {
                     "x": 7.68,
@@ -5616,7 +18815,224 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": 3.6158431875986385e-31,
+                    "velocityX": 5.569083161704914e-32,
+                    "velocityY": 2.1158895574058437e-37,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.680000134363673,
+                    "y": 0.8649552704871222,
+                    "heading": 3.140000000380936,
+                    "angularVelocity": 3.939689465162077e-9,
+                    "velocityX": 0.0000013896119285392994,
+                    "velocityY": 0.8062267267090596,
+                    "timestamp": 0.09669149868712118
+                },
+                {
+                    "x": 7.680000398315331,
+                    "y": 1.0208658083667637,
+                    "heading": 3.14000000095338,
+                    "angularVelocity": 5.920286837104207e-9,
+                    "velocityX": 0.0000027298328198052245,
+                    "velocityY": 1.6124534214132247,
+                    "timestamp": 0.19338299737424236
+                },
+                {
+                    "x": 7.680000786842507,
+                    "y": 1.2547316074497201,
+                    "heading": 3.1400000020167984,
+                    "angularVelocity": 1.0998009739093907e-8,
+                    "velocityX": 0.000004018213888590017,
+                    "velocityY": 2.4186800521075957,
+                    "timestamp": 0.29007449606136354
+                },
+                {
+                    "x": 7.680001294600068,
+                    "y": 1.566552649168411,
+                    "heading": 3.140000004611078,
+                    "angularVelocity": 2.6830422686000607e-8,
+                    "velocityX": 0.00000525131448771192,
+                    "velocityY": 3.2249064907729323,
+                    "timestamp": 0.3867659947484847
+                },
+                {
+                    "x": 7.680001797129468,
+                    "y": 1.9323308421441416,
+                    "heading": 3.1400000046110796,
+                    "angularVelocity": 2.2904858507221613e-14,
+                    "velocityX": 0.000005197244743376452,
+                    "velocityY": 3.782940568118395,
+                    "timestamp": 0.4834574934356059
+                },
+                {
+                    "x": 7.680002234677101,
+                    "y": 2.2981090351200577,
+                    "heading": 3.1400000046110854,
+                    "angularVelocity": 7.187274756998544e-14,
+                    "velocityX": 0.00000452519161992871,
+                    "velocityY": 3.782940568120504,
+                    "timestamp": 0.5801489921227271
+                },
+                {
+                    "x": 7.680002600262594,
+                    "y": 2.663887228096014,
+                    "heading": 3.140000004611095,
+                    "angularVelocity": 1.2305801027843294e-13,
+                    "velocityX": 0.000003780946939519626,
+                    "velocityY": 3.782940568121035,
+                    "timestamp": 0.6768404908098482
+                },
+                {
+                    "x": 7.680002888746174,
+                    "y": 3.029665421071995,
+                    "heading": 3.1400000046111063,
+                    "angularVelocity": 1.4680337346800463e-13,
+                    "velocityX": 0.000002983545883280996,
+                    "velocityY": 3.782940568121397,
+                    "timestamp": 0.7735319894969694
+                },
+                {
+                    "x": 7.680003096449986,
+                    "y": 3.395443614047991,
+                    "heading": 3.1400000046111183,
+                    "angularVelocity": 1.4951605959877598e-13,
+                    "velocityX": 0.0000021481078702221435,
+                    "velocityY": 3.782940568121633,
+                    "timestamp": 0.8702234881840907
+                },
+                {
+                    "x": 7.680003221115393,
+                    "y": 3.7612218070239956,
+                    "heading": 3.1400000046111316,
+                    "angularVelocity": 1.5542579572657036e-13,
+                    "velocityX": 0.0000012893106851068564,
+                    "velocityY": 3.7829405681217785,
+                    "timestamp": 0.9669149868712119
+                },
+                {
+                    "x": 7.68000326213119,
+                    "y": 4.127000000000005,
+                    "heading": 3.1400000046111454,
+                    "angularVelocity": 1.6157627281901116e-13,
+                    "velocityX": 4.241922977706735e-7,
+                    "velocityY": 3.782940568121862,
+                    "timestamp": 1.0636064855583331
+                },
+                {
+                    "x": 7.6800032195335275,
+                    "y": 4.492778192976015,
+                    "heading": 3.140000004611159,
+                    "angularVelocity": 1.571537708778215e-13,
+                    "velocityX": -4.4055220975717935e-7,
+                    "velocityY": 3.7829405681218624,
+                    "timestamp": 1.1602979842454544
+                },
+                {
+                    "x": 7.680003093394937,
+                    "y": 4.858556385952021,
+                    "heading": 3.1400000046111707,
+                    "angularVelocity": 1.4279777513481356e-13,
+                    "velocityX": -0.00000130454658943142,
+                    "velocityY": 3.7829405681217825,
+                    "timestamp": 1.2569894829325756
+                },
+                {
+                    "x": 7.680002884476087,
+                    "y": 5.224334578928017,
+                    "heading": 3.1400000046111822,
+                    "angularVelocity": 1.3985874807595513e-13,
+                    "velocityX": -0.0000021606740038572212,
+                    "velocityY": 3.7829405681216364,
+                    "timestamp": 1.3536809816196969
+                },
+                {
+                    "x": 7.680002595166353,
+                    "y": 5.590112771903999,
+                    "heading": 3.140000004611193,
+                    "angularVelocity": 1.3691025375631502e-13,
+                    "velocityX": -0.0000029920901128748315,
+                    "velocityY": 3.7829405681213992,
+                    "timestamp": 1.450372480306818
+                },
+                {
+                    "x": 7.680002229238247,
+                    "y": 5.955890964879955,
+                    "heading": 3.1400000046112018,
+                    "angularVelocity": 1.1312968357611626e-13,
+                    "velocityX": -0.000003784490284850132,
+                    "velocityY": 3.782940568121032,
+                    "timestamp": 1.5470639789939393
+                },
+                {
+                    "x": 7.680001791901768,
+                    "y": 6.321669157855871,
+                    "heading": 3.1400000046112067,
+                    "angularVelocity": 6.287792233292802e-14,
+                    "velocityX": -0.000004523007841510769,
+                    "velocityY": 3.7829405681204946,
+                    "timestamp": 1.6437554776810606
+                },
+                {
+                    "x": 7.680001290195951,
+                    "y": 6.687447350831601,
+                    "heading": 3.1400000046112084,
+                    "angularVelocity": 2.135816498669109e-14,
+                    "velocityX": -0.00000518872709755724,
+                    "velocityY": 3.7829405681183994,
+                    "timestamp": 1.7404469763681818
+                },
+                {
+                    "x": 7.68000078407037,
+                    "y": 6.999268392550285,
+                    "heading": 3.1400000030761217,
+                    "angularVelocity": -1.5876160780832585e-8,
+                    "velocityX": -0.000005234436287123649,
+                    "velocityY": 3.224906490772867,
+                    "timestamp": 1.837138475055303
+                },
+                {
+                    "x": 7.68000039686507,
+                    "y": 7.233134191633239,
+                    "heading": 3.1400000013788225,
+                    "angularVelocity": -1.7553792269593633e-8,
+                    "velocityX": -0.000004004542818722395,
+                    "velocityY": 2.4186800521075664,
+                    "timestamp": 1.9338299737424243
+                },
+                {
+                    "x": 7.680000133859054,
+                    "y": 7.389044729512878,
+                    "heading": 3.1400000003266944,
+                    "angularVelocity": -1.08813147296025e-8,
+                    "velocityX": -0.0000027200528426415456,
+                    "velocityY": 1.6124534214132105,
+                    "timestamp": 2.0305214724295455
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "angularVelocity": -3.378739660348149e-9,
+                    "velocityX": -0.000001384393079062506,
+                    "velocityY": 0.806226726709054,
+                    "timestamp": 2.1272129711166667
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "angularVelocity": 1.4001703039309738e-31,
+                    "velocityX": 0,
+                    "velocityY": 2.3509592524815357e-38,
+                    "timestamp": 2.223904469803788
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -5634,7 +19050,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C5 To C2": {
             "waypoints": [
@@ -5645,7 +19077,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 18
                 },
                 {
                     "x": 7.68,
@@ -5657,7 +19089,179 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": -9.959658814138394e-32,
+                    "velocityX": -4.727301624016019e-32,
+                    "velocityY": 3.595241492576121e-38,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.680000100608845,
+                    "y": 0.8688441539517371,
+                    "heading": 3.13999998007937,
+                    "angularVelocity": -2.0106831809452372e-7,
+                    "velocityX": 0.0000010154926233848571,
+                    "velocityY": 0.826091745427235,
+                    "timestamp": 0.09907392781101573
+                },
+                {
+                    "x": 7.680000296760758,
+                    "y": 1.0325324587418605,
+                    "heading": 3.139999940084766,
+                    "angularVelocity": -4.036843910631592e-7,
+                    "velocityX": 0.0000019798539067481827,
+                    "velocityY": 1.6521834594299558,
+                    "timestamp": 0.19814785562203147
+                },
+                {
+                    "x": 7.680000583126213,
+                    "y": 1.2780649081436684,
+                    "heading": 3.1399998804112537,
+                    "angularVelocity": -6.023128542087543e-7,
+                    "velocityX": 0.0000028904217813218476,
+                    "velocityY": 2.47827511058363,
+                    "timestamp": 0.29722178343304717
+                },
+                {
+                    "x": 7.680000954004463,
+                    "y": 1.6054414834770696,
+                    "heading": 3.13999980228898,
+                    "angularVelocity": -7.885247914488142e-7,
+                    "velocityX": 0.0000037434492396662855,
+                    "velocityY": 3.3043665731903316,
+                    "timestamp": 0.39629571124406293
+                },
+                {
+                    "x": 7.680001281447094,
+                    "y": 1.9802322649264825,
+                    "heading": 3.1399998022889792,
+                    "angularVelocity": -1.0474954246602775e-14,
+                    "velocityX": 0.0000033050332881388687,
+                    "velocityY": 3.7829405750856777,
+                    "timestamp": 0.4953696390550787
+                },
+                {
+                    "x": 7.6800015408657325,
+                    "y": 2.355023046375991,
+                    "heading": 3.13999980228898,
+                    "angularVelocity": 7.833153823506449e-15,
+                    "velocityX": 0.0000026184347759635413,
+                    "velocityY": 3.7829405750867062,
+                    "timestamp": 0.5944435668660945
+                },
+                {
+                    "x": 7.680001725168594,
+                    "y": 2.729813827825533,
+                    "heading": 3.139999802288987,
+                    "angularVelocity": 7.465414071871633e-14,
+                    "velocityX": 0.0000018602557564557753,
+                    "velocityY": 3.7829405750870757,
+                    "timestamp": 0.6935174946771102
+                },
+                {
+                    "x": 7.680001829195369,
+                    "y": 3.104604609275094,
+                    "heading": 3.1399998022889952,
+                    "angularVelocity": 9.203271146236807e-14,
+                    "velocityX": 0.0000010499913044241082,
+                    "velocityY": 3.7829405750872978,
+                    "timestamp": 0.792591422488126
+                },
+                {
+                    "x": 7.680001849352207,
+                    "y": 3.479395390724662,
+                    "heading": 3.1399998022890023,
+                    "angularVelocity": 7.71401741801888e-14,
+                    "velocityX": 2.034524499813149e-7,
+                    "velocityY": 3.7829405750873777,
+                    "timestamp": 0.8916653502991417
+                },
+                {
+                    "x": 7.6800017836349985,
+                    "y": 3.8541861721742245,
+                    "heading": 3.1399998022890094,
+                    "angularVelocity": 7.688638756481334e-14,
+                    "velocityX": -6.633148330797741e-7,
+                    "velocityY": 3.7829405750873186,
+                    "timestamp": 0.9907392781101575
+                },
+                {
+                    "x": 7.68000163167861,
+                    "y": 4.22897695362377,
+                    "heading": 3.1399998022890157,
+                    "angularVelocity": 6.981254883852486e-14,
+                    "velocityX": -0.000001533767606062718,
+                    "velocityY": 3.7829405750871206,
+                    "timestamp": 1.0898132059211731
+                },
+                {
+                    "x": 7.680001393541454,
+                    "y": 4.603767735073283,
+                    "heading": 3.139999802289022,
+                    "angularVelocity": 6.191049012077391e-14,
+                    "velocityX": -0.00000240363076954954,
+                    "velocityY": 3.782940575086774,
+                    "timestamp": 1.188887133732189
+                },
+                {
+                    "x": 7.680001069388574,
+                    "y": 4.978558516522716,
+                    "heading": 3.139999802289024,
+                    "angularVelocity": 2.5336752196891655e-14,
+                    "velocityX": -0.000003271828396587149,
+                    "velocityY": 3.782940575085891,
+                    "timestamp": 1.2879610615432047
+                },
+                {
+                    "x": 7.680000659196884,
+                    "y": 5.305935091856245,
+                    "heading": 3.139999885175708,
+                    "angularVelocity": 8.366142293360243e-7,
+                    "velocityX": -0.000004140258200407122,
+                    "velocityY": 3.304366573191614,
+                    "timestamp": 1.3870349893542204
+                },
+                {
+                    "x": 7.6800003382888615,
+                    "y": 5.5514675412581065,
+                    "heading": 3.1399999457053744,
+                    "angularVelocity": 6.10954418213692e-7,
+                    "velocityX": -0.0000032390761240661895,
+                    "velocityY": 2.478275110584155,
+                    "timestamp": 1.4861089171652362
+                },
+                {
+                    "x": 7.6800001156321125,
+                    "y": 5.715155846048254,
+                    "heading": 3.139999983279866,
+                    "angularVelocity": 3.792570390671995e-7,
+                    "velocityX": -0.000002247379691011521,
+                    "velocityY": 1.6521834594301872,
+                    "timestamp": 1.585182844976252
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": 1.687641935617777e-7,
+                    "velocityX": -0.0000011671295149488337,
+                    "velocityY": 0.8260917454273095,
+                    "timestamp": 1.6842567727872677
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": -8.926921159411167e-32,
+                    "velocityX": 0,
+                    "velocityY": 7.102031651059941e-38,
+                    "timestamp": 1.7833307005982835
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -5675,7 +19279,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C5 To C3": {
             "waypoints": [
@@ -5686,7 +19306,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 14
                 },
                 {
                     "x": 7.68,
@@ -5716,7 +19336,23 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         },
         "C5 To C4": {
             "waypoints": [
@@ -5727,7 +19363,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 9
                 },
                 {
                     "x": 7.68,
@@ -5739,7 +19375,98 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": -4.029789856147219e-33,
+                    "velocityY": -3.379656030870708e-35,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.680000025226674,
+                    "y": 0.87050000557584,
+                    "heading": 3.139999988557334,
+                    "angularVelocity": -1.1434532837257691e-7,
+                    "velocityX": 2.5208743257476917e-7,
+                    "velocityY": 0.8344065481227664,
+                    "timestamp": 0.10007112931184067
+                },
+                {
+                    "x": 7.680000070528317,
+                    "y": 1.03750001362983,
+                    "heading": 3.1399999655365143,
+                    "angularVelocity": -2.300445661244102e-7,
+                    "velocityX": 4.5269443024398455e-7,
+                    "velocityY": 1.6688130652906519,
+                    "timestamp": 0.20014225862368135
+                },
+                {
+                    "x": 7.6800001304795185,
+                    "y": 1.2880000179665891,
+                    "heading": 3.139999931372676,
+                    "angularVelocity": -3.413955478564011e-7,
+                    "velocityX": 5.990858804609274e-7,
+                    "velocityY": 2.5032195205487646,
+                    "timestamp": 0.300213387935522
+                },
+                {
+                    "x": 7.680000199269358,
+                    "y": 1.6219999999999795,
+                    "heading": 3.1399998873718746,
+                    "angularVelocity": -4.396952561787762e-7,
+                    "velocityX": 6.874094371094506e-7,
+                    "velocityY": 3.337625790077605,
+                    "timestamp": 0.4002845172473627
+                },
+                {
+                    "x": 7.680000135199738,
+                    "y": 1.9559999820333853,
+                    "heading": 3.1399999178431197,
+                    "angularVelocity": 3.0449586057484084e-7,
+                    "velocityX": -6.402407947941681e-7,
+                    "velocityY": 3.3376257900777593,
+                    "timestamp": 0.5003556465592034
+                },
+                {
+                    "x": 7.680000075581297,
+                    "y": 2.206499986370157,
+                    "heading": 3.139999951084868,
+                    "angularVelocity": 3.3218120651110665e-7,
+                    "velocityX": -5.957606422672805e-7,
+                    "velocityY": 2.503219520548894,
+                    "timestamp": 0.6004267758710441
+                },
+                {
+                    "x": 7.680000027896081,
+                    "y": 2.373499994424156,
+                    "heading": 3.13999998094686,
+                    "angularVelocity": 2.984076655658666e-7,
+                    "velocityX": -4.765132174434968e-7,
+                    "velocityY": 1.6688130652907383,
+                    "timestamp": 0.7004979051828848
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": 1.903959699368805e-7,
+                    "velocityX": -2.787625301130355e-7,
+                    "velocityY": 0.8344065481228072,
+                    "timestamp": 0.8005690344947255
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": -2.2348525624284448e-32,
+                    "velocityX": 0,
+                    "velocityY": 2.3896717289528774e-36,
+                    "timestamp": 0.9006401638065662
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -5757,9 +19484,25 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         }
     },
     "splitTrajectoriesAtStopPoints": false,
-    "usesObstacles": true
+    "usesObstacles": false
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.1.traj
new file mode 100644
index 00000000..d3385742
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.1.traj	
@@ -0,0 +1,274 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": -2.1,
+            "angularVelocity": 0,
+            "velocityX": -2.7669422223227046e-34,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7613814086171108,
+            "y": 6.725487327512705,
+            "heading": -2.1469081960592686,
+            "angularVelocity": -0.5864829148967596,
+            "velocityX": 0.642410513062366,
+            "velocityY": 0.06860685562450702,
+            "timestamp": 0.07998220385929865
+        },
+        {
+            "x": 0.8643773558424356,
+            "y": 6.73664563014898,
+            "heading": -2.237067397875382,
+            "angularVelocity": -1.1272407793953514,
+            "velocityX": 1.2877357994099667,
+            "velocityY": 0.13950981715763813,
+            "timestamp": 0.1599644077185973
+        },
+        {
+            "x": 1.019344149471572,
+            "y": 6.7536581703311604,
+            "heading": -2.3645059003180053,
+            "angularVelocity": -1.5933357208661998,
+            "velocityX": 1.9375159241891806,
+            "velocityY": 0.21270406867141206,
+            "timestamp": 0.23994661157789593
+        },
+        {
+            "x": 1.2269206708549203,
+            "y": 6.776747585260753,
+            "heading": -2.5146569204728233,
+            "angularVelocity": -1.8773053618147075,
+            "velocityX": 2.59528384274717,
+            "velocityY": 0.28868190441726393,
+            "timestamp": 0.3199288154371946
+        },
+        {
+            "x": 1.4861241127588112,
+            "y": 6.80668700452381,
+            "heading": -2.6232938505110392,
+            "angularVelocity": -1.3582637736430165,
+            "velocityX": 3.240763937436266,
+            "velocityY": 0.37432601026755163,
+            "timestamp": 0.39991101929649325
+        },
+        {
+            "x": 1.7870157378738414,
+            "y": 6.83849222949431,
+            "heading": -2.623293892215126,
+            "angularVelocity": -5.214170797715324e-7,
+            "velocityX": 3.7619821734888195,
+            "velocityY": 0.39765377091197274,
+            "timestamp": 0.4798932231557919
+        },
+        {
+            "x": 2.0879074285185357,
+            "y": 6.870296834526242,
+            "heading": -2.623293933917092,
+            "angularVelocity": -5.213905536622117e-7,
+            "velocityX": 3.7619829927918795,
+            "velocityY": 0.3976460199556515,
+            "timestamp": 0.5598754270150905
+        },
+        {
+            "x": 2.3887991234818986,
+            "y": 6.9021013987008635,
+            "heading": -2.623293975619048,
+            "angularVelocity": -5.213904366102477e-7,
+            "velocityX": 3.7619830467872393,
+            "velocityY": 0.3976455091256453,
+            "timestamp": 0.6398576308743892
+        },
+        {
+            "x": 2.689690823126622,
+            "y": 6.933905918586809,
+            "heading": -2.623294017320994,
+            "angularVelocity": -5.213903027395266e-7,
+            "velocityX": 3.761983105317272,
+            "velocityY": 0.39764495539400585,
+            "timestamp": 0.7198398347336878
+        },
+        {
+            "x": 2.9905825278407376,
+            "y": 6.965710390512973,
+            "heading": -2.62329405902293,
+            "angularVelocity": -5.213901895916786e-7,
+            "velocityX": 3.7619831686987752,
+            "velocityY": 0.39764435576335205,
+            "timestamp": 0.7998220385929865
+        },
+        {
+            "x": 3.2914742379996778,
+            "y": 6.997514810927433,
+            "heading": -2.623294100724858,
+            "angularVelocity": -5.21390082509775e-7,
+            "velocityX": 3.761983236774224,
+            "velocityY": 0.39764371172379265,
+            "timestamp": 0.8798042424522852
+        },
+        {
+            "x": 3.5923659539723536,
+            "y": 7.029319176339941,
+            "heading": -2.6232941424267775,
+            "angularVelocity": -5.213899786160023e-7,
+            "velocityX": 3.7619833094620927,
+            "velocityY": 0.3976430240464025,
+            "timestamp": 0.9597864463115838
+        },
+        {
+            "x": 3.8932576761280617,
+            "y": 7.0611234832565914,
+            "heading": -2.6232941841286896,
+            "angularVelocity": -5.213898862684442e-7,
+            "velocityX": 3.761983386767183,
+            "velocityY": 0.39764229268549517,
+            "timestamp": 1.0397686501708825
+        },
+        {
+            "x": 4.194149404850362,
+            "y": 7.092927728048495,
+            "heading": -2.623294225830597,
+            "angularVelocity": -5.213898264324783e-7,
+            "velocityX": 3.7619834688678555,
+            "velocityY": 0.3976415159533747,
+            "timestamp": 1.119750854030181
+        },
+        {
+            "x": 4.49504114052058,
+            "y": 7.124731907107895,
+            "heading": -2.623294267532503,
+            "angularVelocity": -5.213898089255833e-7,
+            "velocityX": 3.761983555736148,
+            "velocityY": 0.3976406941142751,
+            "timestamp": 1.1997330578894796
+        },
+        {
+            "x": 4.795932883453666,
+            "y": 7.15653601745498,
+            "heading": -2.6232943092344048,
+            "angularVelocity": -5.213897595490136e-7,
+            "velocityX": 3.761983646542196,
+            "velocityY": 0.39763983501921824,
+            "timestamp": 1.2797152617487781
+        },
+        {
+            "x": 5.0968246339465235,
+            "y": 7.188340056280654,
+            "heading": -2.6232943509362996,
+            "angularVelocity": -5.21389667495749e-7,
+            "velocityX": 3.7619837410603636,
+            "velocityY": 0.3976389408026617,
+            "timestamp": 1.3596974656080767
+        },
+        {
+            "x": 5.3977163922506195,
+            "y": 7.220144021205662,
+            "heading": -2.6232943926381824,
+            "angularVelocity": -5.213895176915956e-7,
+            "velocityX": 3.7619838387225806,
+            "velocityY": 0.39763801683879874,
+            "timestamp": 1.4396796694673752
+        },
+        {
+            "x": 5.698608158576608,
+            "y": 7.251947910236858,
+            "heading": -2.623294434340057,
+            "angularVelocity": -5.213894134163355e-7,
+            "velocityX": 3.7619839390185463,
+            "velocityY": 0.3976370679550778,
+            "timestamp": 1.5196618733266738
+        },
+        {
+            "x": 5.999499932773201,
+            "y": 7.283751724805354,
+            "heading": -2.62329447604193,
+            "angularVelocity": -5.213893967687742e-7,
+            "velocityX": 3.761984037422991,
+            "velocityY": 0.3976361369642237,
+            "timestamp": 1.5996440771859723
+        },
+        {
+            "x": 6.300391714248171,
+            "y": 7.315555470513951,
+            "heading": -2.6232945177438096,
+            "angularVelocity": -5.213894781911953e-7,
+            "velocityX": 3.7619841284229465,
+            "velocityY": 0.39763527602395277,
+            "timestamp": 1.679626281045271
+        },
+        {
+            "x": 6.601283502174795,
+            "y": 7.347359155184038,
+            "heading": -2.6232945594457013,
+            "angularVelocity": -5.213896319500141e-7,
+            "velocityX": 3.76198420908656,
+            "velocityY": 0.39763451287282225,
+            "timestamp": 1.7596084849045694
+        },
+        {
+            "x": 6.902175248741792,
+            "y": 7.379163231145718,
+            "heading": -2.6232946011491864,
+            "angularVelocity": -5.214095570695983e-7,
+            "velocityX": 3.7619836919762037,
+            "velocityY": 0.39763940510601253,
+            "timestamp": 1.839590688763868
+        },
+        {
+            "x": 7.162101881336522,
+            "y": 7.409614310010791,
+            "heading": -2.734801003168492,
+            "angularVelocity": -1.3941401541705845,
+            "velocityX": 3.249805832457199,
+            "velocityY": 0.3807231783540282,
+            "timestamp": 1.9195728926231665
+        },
+        {
+            "x": 7.370045087720824,
+            "y": 7.432975674976622,
+            "heading": -2.8811468250983516,
+            "angularVelocity": -1.8297298007354832,
+            "velocityX": 2.5998684251074975,
+            "velocityY": 0.29208203623556095,
+            "timestamp": 1.999555096482465
+        },
+        {
+            "x": 7.525332087472029,
+            "y": 7.4501357007114875,
+            "heading": -3.00655027342712,
+            "angularVelocity": -1.5678918844168468,
+            "velocityX": 1.9415193912933517,
+            "velocityY": 0.21454804827649476,
+            "timestamp": 2.0795373003417637
+        },
+        {
+            "x": 7.62852758064907,
+            "y": 7.461415013154985,
+            "heading": -3.096236840109313,
+            "angularVelocity": -1.1213315256974696,
+            "velocityX": 1.2902306787967257,
+            "velocityY": 0.14102277630833499,
+            "timestamp": 2.159519504201062
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.5869864145386997,
+            "velocityX": 0.6435484003601272,
+            "velocityY": 0.06982786889492548,
+            "timestamp": 2.2395017080603608
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -3.143185307179586,
+            "angularVelocity": -1.1375235030887645e-31,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 2.3194839119196593
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.traj
new file mode 100644
index 00000000..d3385742
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.traj	
@@ -0,0 +1,274 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": -2.1,
+            "angularVelocity": 0,
+            "velocityX": -2.7669422223227046e-34,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7613814086171108,
+            "y": 6.725487327512705,
+            "heading": -2.1469081960592686,
+            "angularVelocity": -0.5864829148967596,
+            "velocityX": 0.642410513062366,
+            "velocityY": 0.06860685562450702,
+            "timestamp": 0.07998220385929865
+        },
+        {
+            "x": 0.8643773558424356,
+            "y": 6.73664563014898,
+            "heading": -2.237067397875382,
+            "angularVelocity": -1.1272407793953514,
+            "velocityX": 1.2877357994099667,
+            "velocityY": 0.13950981715763813,
+            "timestamp": 0.1599644077185973
+        },
+        {
+            "x": 1.019344149471572,
+            "y": 6.7536581703311604,
+            "heading": -2.3645059003180053,
+            "angularVelocity": -1.5933357208661998,
+            "velocityX": 1.9375159241891806,
+            "velocityY": 0.21270406867141206,
+            "timestamp": 0.23994661157789593
+        },
+        {
+            "x": 1.2269206708549203,
+            "y": 6.776747585260753,
+            "heading": -2.5146569204728233,
+            "angularVelocity": -1.8773053618147075,
+            "velocityX": 2.59528384274717,
+            "velocityY": 0.28868190441726393,
+            "timestamp": 0.3199288154371946
+        },
+        {
+            "x": 1.4861241127588112,
+            "y": 6.80668700452381,
+            "heading": -2.6232938505110392,
+            "angularVelocity": -1.3582637736430165,
+            "velocityX": 3.240763937436266,
+            "velocityY": 0.37432601026755163,
+            "timestamp": 0.39991101929649325
+        },
+        {
+            "x": 1.7870157378738414,
+            "y": 6.83849222949431,
+            "heading": -2.623293892215126,
+            "angularVelocity": -5.214170797715324e-7,
+            "velocityX": 3.7619821734888195,
+            "velocityY": 0.39765377091197274,
+            "timestamp": 0.4798932231557919
+        },
+        {
+            "x": 2.0879074285185357,
+            "y": 6.870296834526242,
+            "heading": -2.623293933917092,
+            "angularVelocity": -5.213905536622117e-7,
+            "velocityX": 3.7619829927918795,
+            "velocityY": 0.3976460199556515,
+            "timestamp": 0.5598754270150905
+        },
+        {
+            "x": 2.3887991234818986,
+            "y": 6.9021013987008635,
+            "heading": -2.623293975619048,
+            "angularVelocity": -5.213904366102477e-7,
+            "velocityX": 3.7619830467872393,
+            "velocityY": 0.3976455091256453,
+            "timestamp": 0.6398576308743892
+        },
+        {
+            "x": 2.689690823126622,
+            "y": 6.933905918586809,
+            "heading": -2.623294017320994,
+            "angularVelocity": -5.213903027395266e-7,
+            "velocityX": 3.761983105317272,
+            "velocityY": 0.39764495539400585,
+            "timestamp": 0.7198398347336878
+        },
+        {
+            "x": 2.9905825278407376,
+            "y": 6.965710390512973,
+            "heading": -2.62329405902293,
+            "angularVelocity": -5.213901895916786e-7,
+            "velocityX": 3.7619831686987752,
+            "velocityY": 0.39764435576335205,
+            "timestamp": 0.7998220385929865
+        },
+        {
+            "x": 3.2914742379996778,
+            "y": 6.997514810927433,
+            "heading": -2.623294100724858,
+            "angularVelocity": -5.21390082509775e-7,
+            "velocityX": 3.761983236774224,
+            "velocityY": 0.39764371172379265,
+            "timestamp": 0.8798042424522852
+        },
+        {
+            "x": 3.5923659539723536,
+            "y": 7.029319176339941,
+            "heading": -2.6232941424267775,
+            "angularVelocity": -5.213899786160023e-7,
+            "velocityX": 3.7619833094620927,
+            "velocityY": 0.3976430240464025,
+            "timestamp": 0.9597864463115838
+        },
+        {
+            "x": 3.8932576761280617,
+            "y": 7.0611234832565914,
+            "heading": -2.6232941841286896,
+            "angularVelocity": -5.213898862684442e-7,
+            "velocityX": 3.761983386767183,
+            "velocityY": 0.39764229268549517,
+            "timestamp": 1.0397686501708825
+        },
+        {
+            "x": 4.194149404850362,
+            "y": 7.092927728048495,
+            "heading": -2.623294225830597,
+            "angularVelocity": -5.213898264324783e-7,
+            "velocityX": 3.7619834688678555,
+            "velocityY": 0.3976415159533747,
+            "timestamp": 1.119750854030181
+        },
+        {
+            "x": 4.49504114052058,
+            "y": 7.124731907107895,
+            "heading": -2.623294267532503,
+            "angularVelocity": -5.213898089255833e-7,
+            "velocityX": 3.761983555736148,
+            "velocityY": 0.3976406941142751,
+            "timestamp": 1.1997330578894796
+        },
+        {
+            "x": 4.795932883453666,
+            "y": 7.15653601745498,
+            "heading": -2.6232943092344048,
+            "angularVelocity": -5.213897595490136e-7,
+            "velocityX": 3.761983646542196,
+            "velocityY": 0.39763983501921824,
+            "timestamp": 1.2797152617487781
+        },
+        {
+            "x": 5.0968246339465235,
+            "y": 7.188340056280654,
+            "heading": -2.6232943509362996,
+            "angularVelocity": -5.21389667495749e-7,
+            "velocityX": 3.7619837410603636,
+            "velocityY": 0.3976389408026617,
+            "timestamp": 1.3596974656080767
+        },
+        {
+            "x": 5.3977163922506195,
+            "y": 7.220144021205662,
+            "heading": -2.6232943926381824,
+            "angularVelocity": -5.213895176915956e-7,
+            "velocityX": 3.7619838387225806,
+            "velocityY": 0.39763801683879874,
+            "timestamp": 1.4396796694673752
+        },
+        {
+            "x": 5.698608158576608,
+            "y": 7.251947910236858,
+            "heading": -2.623294434340057,
+            "angularVelocity": -5.213894134163355e-7,
+            "velocityX": 3.7619839390185463,
+            "velocityY": 0.3976370679550778,
+            "timestamp": 1.5196618733266738
+        },
+        {
+            "x": 5.999499932773201,
+            "y": 7.283751724805354,
+            "heading": -2.62329447604193,
+            "angularVelocity": -5.213893967687742e-7,
+            "velocityX": 3.761984037422991,
+            "velocityY": 0.3976361369642237,
+            "timestamp": 1.5996440771859723
+        },
+        {
+            "x": 6.300391714248171,
+            "y": 7.315555470513951,
+            "heading": -2.6232945177438096,
+            "angularVelocity": -5.213894781911953e-7,
+            "velocityX": 3.7619841284229465,
+            "velocityY": 0.39763527602395277,
+            "timestamp": 1.679626281045271
+        },
+        {
+            "x": 6.601283502174795,
+            "y": 7.347359155184038,
+            "heading": -2.6232945594457013,
+            "angularVelocity": -5.213896319500141e-7,
+            "velocityX": 3.76198420908656,
+            "velocityY": 0.39763451287282225,
+            "timestamp": 1.7596084849045694
+        },
+        {
+            "x": 6.902175248741792,
+            "y": 7.379163231145718,
+            "heading": -2.6232946011491864,
+            "angularVelocity": -5.214095570695983e-7,
+            "velocityX": 3.7619836919762037,
+            "velocityY": 0.39763940510601253,
+            "timestamp": 1.839590688763868
+        },
+        {
+            "x": 7.162101881336522,
+            "y": 7.409614310010791,
+            "heading": -2.734801003168492,
+            "angularVelocity": -1.3941401541705845,
+            "velocityX": 3.249805832457199,
+            "velocityY": 0.3807231783540282,
+            "timestamp": 1.9195728926231665
+        },
+        {
+            "x": 7.370045087720824,
+            "y": 7.432975674976622,
+            "heading": -2.8811468250983516,
+            "angularVelocity": -1.8297298007354832,
+            "velocityX": 2.5998684251074975,
+            "velocityY": 0.29208203623556095,
+            "timestamp": 1.999555096482465
+        },
+        {
+            "x": 7.525332087472029,
+            "y": 7.4501357007114875,
+            "heading": -3.00655027342712,
+            "angularVelocity": -1.5678918844168468,
+            "velocityX": 1.9415193912933517,
+            "velocityY": 0.21454804827649476,
+            "timestamp": 2.0795373003417637
+        },
+        {
+            "x": 7.62852758064907,
+            "y": 7.461415013154985,
+            "heading": -3.096236840109313,
+            "angularVelocity": -1.1213315256974696,
+            "velocityX": 1.2902306787967257,
+            "velocityY": 0.14102277630833499,
+            "timestamp": 2.159519504201062
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.5869864145386997,
+            "velocityX": 0.6435484003601272,
+            "velocityY": 0.06982786889492548,
+            "timestamp": 2.2395017080603608
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -3.143185307179586,
+            "angularVelocity": -1.1375235030887645e-31,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 2.3194839119196593
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.1.traj
new file mode 100644
index 00000000..6b188abb
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.1.traj	
@@ -0,0 +1,274 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": -2.1,
+            "angularVelocity": 0,
+            "velocityX": -7.879763479482e-33,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7634290430022197,
+            "y": 6.720160003423765,
+            "heading": -2.158070133372329,
+            "angularVelocity": -0.7094408010373536,
+            "velocityX": 0.6527407612984071,
+            "velocityY": 0.0019547562668324166,
+            "timestamp": 0.08185338831290496
+        },
+        {
+            "x": 0.8707119576691547,
+            "y": 6.720819673735276,
+            "heading": -2.2686879013575907,
+            "angularVelocity": -1.351413426679394,
+            "velocityX": 1.3106716396005418,
+            "velocityY": 0.008059169267236224,
+            "timestamp": 0.16370677662580993
+        },
+        {
+            "x": 1.0324112516405533,
+            "y": 6.722281472922437,
+            "heading": -2.4240599050623355,
+            "angularVelocity": -1.898174368919177,
+            "velocityX": 1.9754746541861241,
+            "velocityY": 0.017858749861073935,
+            "timestamp": 0.2455601649387149
+        },
+        {
+            "x": 1.249511925601742,
+            "y": 6.724737672202976,
+            "heading": -2.6048330548635152,
+            "angularVelocity": -2.2084992878991065,
+            "velocityX": 2.652311387908187,
+            "velocityY": 0.030007301239015387,
+            "timestamp": 0.32741355325161986
+        },
+        {
+            "x": 1.518512933103673,
+            "y": 6.730171304043105,
+            "heading": -2.7105117995327306,
+            "angularVelocity": -1.2910735504953308,
+            "velocityX": 3.2863759588498382,
+            "velocityY": 0.0663824913314231,
+            "timestamp": 0.4092669415645248
+        },
+        {
+            "x": 1.8281517964730614,
+            "y": 6.7323448579922625,
+            "heading": -2.71051185012585,
+            "angularVelocity": -6.180943842017169e-7,
+            "velocityX": 3.782847231512476,
+            "velocityY": 0.026554233049558205,
+            "timestamp": 0.49112032987742976
+        },
+        {
+            "x": 2.1377906615261124,
+            "y": 6.734518172124384,
+            "heading": -2.710511900718092,
+            "angularVelocity": -6.180836584503726e-7,
+            "velocityX": 3.7828472520817247,
+            "velocityY": 0.026551303213165345,
+            "timestamp": 0.5729737181903347
+        },
+        {
+            "x": 2.4474295268603647,
+            "y": 6.736691446193322,
+            "heading": -2.7105119513103224,
+            "angularVelocity": -6.180835185678023e-7,
+            "velocityX": 3.7828472555171566,
+            "velocityY": 0.026550813762651566,
+            "timestamp": 0.6548271065032397
+        },
+        {
+            "x": 2.7570683925033053,
+            "y": 6.738864676282315,
+            "heading": -2.7105120019025377,
+            "angularVelocity": -6.180833387834062e-7,
+            "velocityX": 3.7828472592883804,
+            "velocityY": 0.026550276461191975,
+            "timestamp": 0.7366804948161447
+        },
+        {
+            "x": 3.0667072584849335,
+            "y": 6.741037858115924,
+            "heading": -2.7105120524947366,
+            "angularVelocity": -6.180831373401399e-7,
+            "velocityX": 3.7828472634261248,
+            "velocityY": 0.026549686926837145,
+            "timestamp": 0.8185338831290497
+        },
+        {
+            "x": 3.376346124838015,
+            "y": 6.7432109870244785,
+            "heading": -2.7105121030869213,
+            "angularVelocity": -6.18082958211415e-7,
+            "velocityX": 3.7828472679641574,
+            "velocityY": 0.02654904034329684,
+            "timestamp": 0.9003872714419547
+        },
+        {
+            "x": 3.6859849915958844,
+            "y": 6.745384058256853,
+            "heading": -2.710512153679093,
+            "angularVelocity": -6.180828069034381e-7,
+            "velocityX": 3.782847272909432,
+            "velocityY": 0.026548335715409193,
+            "timestamp": 0.9822406597548597
+        },
+        {
+            "x": 3.99562385879327,
+            "y": 6.747557066862843,
+            "heading": -2.7105122042712537,
+            "angularVelocity": -6.180826661377271e-7,
+            "velocityX": 3.7828472782789873,
+            "velocityY": 0.026547570611047785,
+            "timestamp": 1.0640940480677648
+        },
+        {
+            "x": 4.305262726470487,
+            "y": 6.749730007095753,
+            "heading": -2.710512254863404,
+            "angularVelocity": -6.180825475741077e-7,
+            "velocityX": 3.782847284141071,
+            "velocityY": 0.026546735299496697,
+            "timestamp": 1.1459474363806696
+        },
+        {
+            "x": 4.614901594678237,
+            "y": 6.751902871728125,
+            "heading": -2.710512305455549,
+            "angularVelocity": -6.180824717562797e-7,
+            "velocityX": 3.78284729062258,
+            "velocityY": 0.0265458116903545,
+            "timestamp": 1.2278008246935745
+        },
+        {
+            "x": 4.924540463476948,
+            "y": 6.754075652145792,
+            "heading": -2.7105123560476914,
+            "angularVelocity": -6.180824480187367e-7,
+            "velocityX": 3.782847297842324,
+            "velocityY": 0.02654478284224149,
+            "timestamp": 1.3096542130064794
+        },
+        {
+            "x": 5.234179332930726,
+            "y": 6.756248339209459,
+            "heading": -2.7105124066398263,
+            "angularVelocity": -6.180823518950674e-7,
+            "velocityX": 3.782847305845261,
+            "velocityY": 0.026543642339650734,
+            "timestamp": 1.3915076013193843
+        },
+        {
+            "x": 5.543818204521741,
+            "y": 6.758420721665928,
+            "heading": -2.710512457231931,
+            "angularVelocity": -6.180819838461299e-7,
+            "velocityX": 3.782847331955805,
+            "velocityY": 0.026539920964112583,
+            "timestamp": 1.4733609896322892
+        },
+        {
+            "x": 5.852559225022806,
+            "y": 6.734757839446224,
+            "heading": -2.710512508060742,
+            "angularVelocity": -6.209738209329952e-7,
+            "velocityX": 3.771878316396938,
+            "velocityY": -0.2890886096156145,
+            "timestamp": 1.5552143779451941
+        },
+        {
+            "x": 6.152023822280739,
+            "y": 6.6560056480568655,
+            "heading": -2.710512532930908,
+            "angularVelocity": -3.03837945152059e-7,
+            "velocityX": 3.6585485760608294,
+            "velocityY": -0.9621127849748639,
+            "timestamp": 1.637067766258099
+        },
+        {
+            "x": 6.432464531176267,
+            "y": 6.5247274697787905,
+            "heading": -2.710512568697855,
+            "angularVelocity": -4.369635492236228e-7,
+            "velocityX": 3.4261343931600465,
+            "velocityY": -1.603820941122575,
+            "timestamp": 1.718921154571004
+        },
+        {
+            "x": 6.699924709540006,
+            "y": 6.368695406956045,
+            "heading": -2.710512603773811,
+            "angularVelocity": -4.2852173297406095e-7,
+            "velocityX": 3.267551702823897,
+            "velocityY": -1.9062382882218016,
+            "timestamp": 1.8007745428839088
+        },
+        {
+            "x": 6.967384720466867,
+            "y": 6.212663057124264,
+            "heading": -2.710512638850014,
+            "angularVelocity": -4.2852475398494205e-7,
+            "velocityX": 3.2675496572533906,
+            "velocityY": -1.9062417946012,
+            "timestamp": 1.8826279311968137
+        },
+        {
+            "x": 7.2050885186595135,
+            "y": 6.073759568822601,
+            "heading": -2.7948440651741784,
+            "angularVelocity": -1.0302741042531607,
+            "velocityX": 2.9040190405309123,
+            "velocityY": -1.6969790886440652,
+            "timestamp": 1.9644813195097186
+        },
+        {
+            "x": 7.39556651196216,
+            "y": 5.9626310397123286,
+            "heading": -2.9179406119000064,
+            "angularVelocity": -1.5038662328217944,
+            "velocityX": 2.3270630236404783,
+            "velocityY": -1.3576533775908837,
+            "timestamp": 2.0463347078226235
+        },
+        {
+            "x": 7.537983690499959,
+            "y": 5.879640169020095,
+            "heading": -3.0253464428320265,
+            "angularVelocity": -1.3121733033388265,
+            "velocityX": 1.7399057201318795,
+            "velocityY": -1.0138965826922115,
+            "timestamp": 2.1281880961355286
+        },
+        {
+            "x": 7.632713372468284,
+            "y": 5.824499681694904,
+            "heading": -3.1026583555829736,
+            "angularVelocity": -0.9445169509098753,
+            "velocityX": 1.1573092320405494,
+            "velocityY": -0.6736494171066164,
+            "timestamp": 2.2100414844484337
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.49511635904048823,
+            "velocityX": 0.5776990849902374,
+            "velocityY": -0.33596265544659853,
+            "timestamp": 2.291894872761339
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -3.143185307179586,
+            "angularVelocity": -7.614661899058019e-31,
+            "velocityX": 0,
+            "velocityY": -2.3591575038755638e-32,
+            "timestamp": 2.373748261074244
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.traj
new file mode 100644
index 00000000..6b188abb
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.traj	
@@ -0,0 +1,274 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": -2.1,
+            "angularVelocity": 0,
+            "velocityX": -7.879763479482e-33,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7634290430022197,
+            "y": 6.720160003423765,
+            "heading": -2.158070133372329,
+            "angularVelocity": -0.7094408010373536,
+            "velocityX": 0.6527407612984071,
+            "velocityY": 0.0019547562668324166,
+            "timestamp": 0.08185338831290496
+        },
+        {
+            "x": 0.8707119576691547,
+            "y": 6.720819673735276,
+            "heading": -2.2686879013575907,
+            "angularVelocity": -1.351413426679394,
+            "velocityX": 1.3106716396005418,
+            "velocityY": 0.008059169267236224,
+            "timestamp": 0.16370677662580993
+        },
+        {
+            "x": 1.0324112516405533,
+            "y": 6.722281472922437,
+            "heading": -2.4240599050623355,
+            "angularVelocity": -1.898174368919177,
+            "velocityX": 1.9754746541861241,
+            "velocityY": 0.017858749861073935,
+            "timestamp": 0.2455601649387149
+        },
+        {
+            "x": 1.249511925601742,
+            "y": 6.724737672202976,
+            "heading": -2.6048330548635152,
+            "angularVelocity": -2.2084992878991065,
+            "velocityX": 2.652311387908187,
+            "velocityY": 0.030007301239015387,
+            "timestamp": 0.32741355325161986
+        },
+        {
+            "x": 1.518512933103673,
+            "y": 6.730171304043105,
+            "heading": -2.7105117995327306,
+            "angularVelocity": -1.2910735504953308,
+            "velocityX": 3.2863759588498382,
+            "velocityY": 0.0663824913314231,
+            "timestamp": 0.4092669415645248
+        },
+        {
+            "x": 1.8281517964730614,
+            "y": 6.7323448579922625,
+            "heading": -2.71051185012585,
+            "angularVelocity": -6.180943842017169e-7,
+            "velocityX": 3.782847231512476,
+            "velocityY": 0.026554233049558205,
+            "timestamp": 0.49112032987742976
+        },
+        {
+            "x": 2.1377906615261124,
+            "y": 6.734518172124384,
+            "heading": -2.710511900718092,
+            "angularVelocity": -6.180836584503726e-7,
+            "velocityX": 3.7828472520817247,
+            "velocityY": 0.026551303213165345,
+            "timestamp": 0.5729737181903347
+        },
+        {
+            "x": 2.4474295268603647,
+            "y": 6.736691446193322,
+            "heading": -2.7105119513103224,
+            "angularVelocity": -6.180835185678023e-7,
+            "velocityX": 3.7828472555171566,
+            "velocityY": 0.026550813762651566,
+            "timestamp": 0.6548271065032397
+        },
+        {
+            "x": 2.7570683925033053,
+            "y": 6.738864676282315,
+            "heading": -2.7105120019025377,
+            "angularVelocity": -6.180833387834062e-7,
+            "velocityX": 3.7828472592883804,
+            "velocityY": 0.026550276461191975,
+            "timestamp": 0.7366804948161447
+        },
+        {
+            "x": 3.0667072584849335,
+            "y": 6.741037858115924,
+            "heading": -2.7105120524947366,
+            "angularVelocity": -6.180831373401399e-7,
+            "velocityX": 3.7828472634261248,
+            "velocityY": 0.026549686926837145,
+            "timestamp": 0.8185338831290497
+        },
+        {
+            "x": 3.376346124838015,
+            "y": 6.7432109870244785,
+            "heading": -2.7105121030869213,
+            "angularVelocity": -6.18082958211415e-7,
+            "velocityX": 3.7828472679641574,
+            "velocityY": 0.02654904034329684,
+            "timestamp": 0.9003872714419547
+        },
+        {
+            "x": 3.6859849915958844,
+            "y": 6.745384058256853,
+            "heading": -2.710512153679093,
+            "angularVelocity": -6.180828069034381e-7,
+            "velocityX": 3.782847272909432,
+            "velocityY": 0.026548335715409193,
+            "timestamp": 0.9822406597548597
+        },
+        {
+            "x": 3.99562385879327,
+            "y": 6.747557066862843,
+            "heading": -2.7105122042712537,
+            "angularVelocity": -6.180826661377271e-7,
+            "velocityX": 3.7828472782789873,
+            "velocityY": 0.026547570611047785,
+            "timestamp": 1.0640940480677648
+        },
+        {
+            "x": 4.305262726470487,
+            "y": 6.749730007095753,
+            "heading": -2.710512254863404,
+            "angularVelocity": -6.180825475741077e-7,
+            "velocityX": 3.782847284141071,
+            "velocityY": 0.026546735299496697,
+            "timestamp": 1.1459474363806696
+        },
+        {
+            "x": 4.614901594678237,
+            "y": 6.751902871728125,
+            "heading": -2.710512305455549,
+            "angularVelocity": -6.180824717562797e-7,
+            "velocityX": 3.78284729062258,
+            "velocityY": 0.0265458116903545,
+            "timestamp": 1.2278008246935745
+        },
+        {
+            "x": 4.924540463476948,
+            "y": 6.754075652145792,
+            "heading": -2.7105123560476914,
+            "angularVelocity": -6.180824480187367e-7,
+            "velocityX": 3.782847297842324,
+            "velocityY": 0.02654478284224149,
+            "timestamp": 1.3096542130064794
+        },
+        {
+            "x": 5.234179332930726,
+            "y": 6.756248339209459,
+            "heading": -2.7105124066398263,
+            "angularVelocity": -6.180823518950674e-7,
+            "velocityX": 3.782847305845261,
+            "velocityY": 0.026543642339650734,
+            "timestamp": 1.3915076013193843
+        },
+        {
+            "x": 5.543818204521741,
+            "y": 6.758420721665928,
+            "heading": -2.710512457231931,
+            "angularVelocity": -6.180819838461299e-7,
+            "velocityX": 3.782847331955805,
+            "velocityY": 0.026539920964112583,
+            "timestamp": 1.4733609896322892
+        },
+        {
+            "x": 5.852559225022806,
+            "y": 6.734757839446224,
+            "heading": -2.710512508060742,
+            "angularVelocity": -6.209738209329952e-7,
+            "velocityX": 3.771878316396938,
+            "velocityY": -0.2890886096156145,
+            "timestamp": 1.5552143779451941
+        },
+        {
+            "x": 6.152023822280739,
+            "y": 6.6560056480568655,
+            "heading": -2.710512532930908,
+            "angularVelocity": -3.03837945152059e-7,
+            "velocityX": 3.6585485760608294,
+            "velocityY": -0.9621127849748639,
+            "timestamp": 1.637067766258099
+        },
+        {
+            "x": 6.432464531176267,
+            "y": 6.5247274697787905,
+            "heading": -2.710512568697855,
+            "angularVelocity": -4.369635492236228e-7,
+            "velocityX": 3.4261343931600465,
+            "velocityY": -1.603820941122575,
+            "timestamp": 1.718921154571004
+        },
+        {
+            "x": 6.699924709540006,
+            "y": 6.368695406956045,
+            "heading": -2.710512603773811,
+            "angularVelocity": -4.2852173297406095e-7,
+            "velocityX": 3.267551702823897,
+            "velocityY": -1.9062382882218016,
+            "timestamp": 1.8007745428839088
+        },
+        {
+            "x": 6.967384720466867,
+            "y": 6.212663057124264,
+            "heading": -2.710512638850014,
+            "angularVelocity": -4.2852475398494205e-7,
+            "velocityX": 3.2675496572533906,
+            "velocityY": -1.9062417946012,
+            "timestamp": 1.8826279311968137
+        },
+        {
+            "x": 7.2050885186595135,
+            "y": 6.073759568822601,
+            "heading": -2.7948440651741784,
+            "angularVelocity": -1.0302741042531607,
+            "velocityX": 2.9040190405309123,
+            "velocityY": -1.6969790886440652,
+            "timestamp": 1.9644813195097186
+        },
+        {
+            "x": 7.39556651196216,
+            "y": 5.9626310397123286,
+            "heading": -2.9179406119000064,
+            "angularVelocity": -1.5038662328217944,
+            "velocityX": 2.3270630236404783,
+            "velocityY": -1.3576533775908837,
+            "timestamp": 2.0463347078226235
+        },
+        {
+            "x": 7.537983690499959,
+            "y": 5.879640169020095,
+            "heading": -3.0253464428320265,
+            "angularVelocity": -1.3121733033388265,
+            "velocityX": 1.7399057201318795,
+            "velocityY": -1.0138965826922115,
+            "timestamp": 2.1281880961355286
+        },
+        {
+            "x": 7.632713372468284,
+            "y": 5.824499681694904,
+            "heading": -3.1026583555829736,
+            "angularVelocity": -0.9445169509098753,
+            "velocityX": 1.1573092320405494,
+            "velocityY": -0.6736494171066164,
+            "timestamp": 2.2100414844484337
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.49511635904048823,
+            "velocityX": 0.5776990849902374,
+            "velocityY": -0.33596265544659853,
+            "timestamp": 2.291894872761339
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -3.143185307179586,
+            "angularVelocity": -7.614661899058019e-31,
+            "velocityX": 0,
+            "velocityY": -2.3591575038755638e-32,
+            "timestamp": 2.373748261074244
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.1.traj
new file mode 100644
index 00000000..9e5becca
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.1.traj	
@@ -0,0 +1,679 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": -2.1,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7185162341797315,
+            "y": 6.716270626486824,
+            "heading": -2.0163378917371135,
+            "angularVelocity": 1.6978541106369687,
+            "velocityX": 0.17283001240859905,
+            "velocityY": -0.07568458745445246,
+            "timestamp": 0.049275204352804955
+        },
+        {
+            "x": 0.7365174505438988,
+            "y": 6.709427263698689,
+            "heading": -1.8516035946552112,
+            "angularVelocity": 3.3431479228867977,
+            "velocityX": 0.3653199738205181,
+            "velocityY": -0.1388804547442707,
+            "timestamp": 0.09855040870560991
+        },
+        {
+            "x": 0.7651610538016371,
+            "y": 6.700606345332281,
+            "heading": -1.6098229565949118,
+            "angularVelocity": 4.906740443513477,
+            "velocityX": 0.5812985178641444,
+            "velocityY": -0.1790133289605956,
+            "timestamp": 0.14782561305841485
+        },
+        {
+            "x": 0.8095297793953646,
+            "y": 6.688220030792632,
+            "heading": -1.3224284097332135,
+            "angularVelocity": 5.832437442653418,
+            "velocityX": 0.9004270236212996,
+            "velocityY": -0.2513701303187648,
+            "timestamp": 0.19710081741121982
+        },
+        {
+            "x": 0.8714701737870825,
+            "y": 6.666054185298926,
+            "heading": -1.026684817124186,
+            "angularVelocity": 6.001874502468547,
+            "velocityX": 1.2570296806530035,
+            "velocityY": -0.4498377182771496,
+            "timestamp": 0.2463760217640248
+        },
+        {
+            "x": 0.9484340695976169,
+            "y": 6.633599911893754,
+            "heading": -0.7648347305022277,
+            "angularVelocity": 5.314033499427846,
+            "velocityX": 1.5619193633268658,
+            "velocityY": -0.6586329540675864,
+            "timestamp": 0.29565122611682976
+        },
+        {
+            "x": 1.0425919145867164,
+            "y": 6.594089868440422,
+            "heading": -0.5334353297349962,
+            "angularVelocity": 4.696061717176001,
+            "velocityX": 1.9108565093903993,
+            "velocityY": -0.8018240405548321,
+            "timestamp": 0.3449264304696347
+        },
+        {
+            "x": 1.155214051879495,
+            "y": 6.555173696166533,
+            "heading": -0.3327017892852748,
+            "angularVelocity": 4.073723145062813,
+            "velocityX": 2.285574230934019,
+            "velocityY": -0.7897719103355402,
+            "timestamp": 0.3942016348224397
+        },
+        {
+            "x": 1.2837470785266114,
+            "y": 6.519808002504125,
+            "heading": -0.17776538289371885,
+            "angularVelocity": 3.1443077390857384,
+            "velocityX": 2.6084727265022423,
+            "velocityY": -0.7177178487012317,
+            "timestamp": 0.44347683917524466
+        },
+        {
+            "x": 1.4286059942579405,
+            "y": 6.483992998763445,
+            "heading": -0.07145024720477738,
+            "angularVelocity": 2.1575787880601562,
+            "velocityX": 2.9397933024114007,
+            "velocityY": -0.7268362295212014,
+            "timestamp": 0.49275204352804963
+        },
+        {
+            "x": 1.5885837184337814,
+            "y": 6.442409682842055,
+            "heading": -0.012288104865979272,
+            "angularVelocity": 1.2006473258883674,
+            "velocityX": 3.246617163277867,
+            "velocityY": -0.8438994108204557,
+            "timestamp": 0.5420272478808545
+        },
+        {
+            "x": 1.7606060854656245,
+            "y": 6.389861829992508,
+            "heading": 0.005689657356259434,
+            "angularVelocity": 0.3648439911790104,
+            "velocityX": 3.491053346023324,
+            "velocityY": -1.0664157265246377,
+            "timestamp": 0.5913024522336595
+        },
+        {
+            "x": 1.937365788040802,
+            "y": 6.330680677458663,
+            "heading": 0.005693236516864234,
+            "angularVelocity": 0.00007263613924707546,
+            "velocityX": 3.587193699078299,
+            "velocityY": -1.2010331222599455,
+            "timestamp": 0.6405776565864645
+        },
+        {
+            "x": 2.11412543100989,
+            "y": 6.27149934643189,
+            "heading": 0.005696815267395046,
+            "angularVelocity": 0.00007262781713070066,
+            "velocityX": 3.5871924894214278,
+            "velocityY": -1.20103674462801,
+            "timestamp": 0.6898528609392695
+        },
+        {
+            "x": 2.2908850692554568,
+            "y": 6.212318001247489,
+            "heading": 0.005700393968496334,
+            "angularVelocity": 0.00007262681399886251,
+            "velocityX": 3.5871923935614243,
+            "velocityY": -1.2010370319454915,
+            "timestamp": 0.7391280652920744
+        },
+        {
+            "x": 2.467644702239147,
+            "y": 6.153136640294885,
+            "heading": 0.005703972617858713,
+            "angularVelocity": 0.00007262576400001342,
+            "velocityX": 3.5871922867759345,
+            "velocityY": -1.2010373519482989,
+            "timestamp": 0.7884032696448794
+        },
+        {
+            "x": 2.644404329357872,
+            "y": 6.0939552617700175,
+            "heading": 0.005707551213044099,
+            "angularVelocity": 0.00007262466452220377,
+            "velocityX": 3.5871921677512737,
+            "velocityY": -1.2010377085630215,
+            "timestamp": 0.8376784739976844
+        },
+        {
+            "x": 2.821163949952127,
+            "y": 6.0347738637002735,
+            "heading": 0.00571112975156944,
+            "angularVelocity": 0.00007262351465291134,
+            "velocityX": 3.5871920353424867,
+            "velocityY": -1.201038105210319,
+            "timestamp": 0.8869536783504893
+        },
+        {
+            "x": 2.9979235633249197,
+            "y": 5.975592444001196,
+            "heading": 0.005714708231053879,
+            "angularVelocity": 0.0000726223164660635,
+            "velocityX": 3.587191888788799,
+            "velocityY": -1.2010385441599662,
+            "timestamp": 0.9362288827032943
+        },
+        {
+            "x": 3.174683168778421,
+            "y": 5.916411000586595,
+            "heading": 0.005718286649774776,
+            "angularVelocity": 0.0000726210833196382,
+            "velocityX": 3.587191728073259,
+            "velocityY": -1.2010390254471126,
+            "timestamp": 0.9855040870560993
+        },
+        {
+            "x": 3.3514425126695637,
+            "y": 5.8572287763527235,
+            "heading": 0.005721865395381687,
+            "angularVelocity": 0.00007262771720412379,
+            "velocityX": 3.5871864198789636,
+            "velocityY": -1.2010548715360532,
+            "timestamp": 1.0347792914089042
+        },
+        {
+            "x": 3.5167193258876743,
+            "y": 5.790065395689035,
+            "heading": 0.026683569824856986,
+            "angularVelocity": 0.4254006595161298,
+            "velocityX": 3.354157844475837,
+            "velocityY": -1.3630259183261217,
+            "timestamp": 1.084054495761709
+        },
+        {
+            "x": 3.666994209555769,
+            "y": 5.710547155721769,
+            "heading": 0.06778616903847971,
+            "angularVelocity": 0.8341436581233173,
+            "velocityX": 3.0497059452487165,
+            "velocityY": -1.6137576903369897,
+            "timestamp": 1.133329700114514
+        },
+        {
+            "x": 3.802461838655518,
+            "y": 5.617737963132636,
+            "heading": 0.12219899767661042,
+            "angularVelocity": 1.104263885920004,
+            "velocityX": 2.749204815667845,
+            "velocityY": -1.8834867111788967,
+            "timestamp": 1.1826049044673188
+        },
+        {
+            "x": 3.923178434371948,
+            "y": 5.511256217956543,
+            "heading": 0.18485588032163774,
+            "angularVelocity": 1.2715702241722033,
+            "velocityX": 2.4498446490878596,
+            "velocityY": -2.1609599914329216,
+            "timestamp": 1.2318801088201237
+        },
+        {
+            "x": 4.036364016344018,
+            "y": 5.380109892116248,
+            "heading": 0.2569375572143931,
+            "angularVelocity": 1.3544496582801502,
+            "velocityX": 2.1268119643276364,
+            "velocityY": -2.4643030500437595,
+            "timestamp": 1.2850985333308458
+        },
+        {
+            "x": 4.130923397861632,
+            "y": 5.234522406039544,
+            "heading": 0.33431047652627577,
+            "angularVelocity": 1.4538746688431972,
+            "velocityX": 1.7768166267034577,
+            "velocityY": -2.7356594528154665,
+            "timestamp": 1.3383169578415677
+        },
+        {
+            "x": 4.205236217563619,
+            "y": 5.077100065980883,
+            "heading": 0.4216368931025466,
+            "angularVelocity": 1.640905708485922,
+            "velocityX": 1.396373913455762,
+            "velocityY": -2.958042097374491,
+            "timestamp": 1.3915353823522896
+        },
+        {
+            "x": 4.2828034673506234,
+            "y": 4.901292660688841,
+            "heading": 0.4516847731002138,
+            "angularVelocity": 0.5646142341476765,
+            "velocityX": 1.4575262326936578,
+            "velocityY": -3.303506387277969,
+            "timestamp": 1.4447538068630115
+        },
+        {
+            "x": 4.371277738622277,
+            "y": 4.723874668487275,
+            "heading": 0.4413575215812008,
+            "angularVelocity": -0.19405406330532018,
+            "velocityX": 1.6624744547600938,
+            "velocityY": -3.333770096215863,
+            "timestamp": 1.4979722313737334
+        },
+        {
+            "x": 4.466886857140796,
+            "y": 4.565900844635089,
+            "heading": 0.39184092001837434,
+            "angularVelocity": -0.9304409519460731,
+            "velocityX": 1.796541693925137,
+            "velocityY": -2.968404745246797,
+            "timestamp": 1.5511906558844553
+        },
+        {
+            "x": 4.573967443469787,
+            "y": 4.427674107970891,
+            "heading": 0.321042696188799,
+            "angularVelocity": -1.330332952928953,
+            "velocityX": 2.012096136130751,
+            "velocityY": -2.5973474024273386,
+            "timestamp": 1.6044090803951772
+        },
+        {
+            "x": 4.693065628013639,
+            "y": 4.309440309299518,
+            "heading": 0.2371568341960054,
+            "angularVelocity": -1.5762560196777895,
+            "velocityX": 2.2379126334312645,
+            "velocityY": -2.221670403781889,
+            "timestamp": 1.657627504905899
+        },
+        {
+            "x": 4.8242268562316895,
+            "y": 4.211382865905762,
+            "heading": 0.14523841868535878,
+            "angularVelocity": -1.7271915949358416,
+            "velocityX": 2.464583072947316,
+            "velocityY": -1.8425469054237094,
+            "timestamp": 1.710845929416621
+        },
+        {
+            "x": 4.982290420935857,
+            "y": 4.128393361272933,
+            "heading": 0.04004394713112955,
+            "angularVelocity": -1.8026614827604082,
+            "velocityX": 2.7086508987606095,
+            "velocityY": -1.4221468225908107,
+            "timestamp": 1.7692010185779787
+        },
+        {
+            "x": 5.153780525115527,
+            "y": 4.07042189977071,
+            "heading": -0.06589739756263054,
+            "angularVelocity": -1.8154602489051344,
+            "velocityX": 2.9387343356717652,
+            "velocityY": -0.9934259776713893,
+            "timestamp": 1.8275561077393365
+        },
+        {
+            "x": 5.3372218147923,
+            "y": 4.038159807995649,
+            "heading": -0.16628826181080308,
+            "angularVelocity": -1.7203446295931681,
+            "velocityX": 3.143535419328013,
+            "velocityY": -0.5528582380510605,
+            "timestamp": 1.8859111969006943
+        },
+        {
+            "x": 5.52929807317918,
+            "y": 4.032409431901968,
+            "heading": -0.2472171752765858,
+            "angularVelocity": -1.3868355721641668,
+            "velocityX": 3.2915082668414595,
+            "velocityY": -0.0985411242844827,
+            "timestamp": 1.944266286062052
+        },
+        {
+            "x": 5.726840626298333,
+            "y": 4.053774421636682,
+            "heading": -0.3047520267300494,
+            "angularVelocity": -0.985944024425596,
+            "velocityX": 3.385181240541464,
+            "velocityY": 0.3661204196884676,
+            "timestamp": 2.00262137522341
+        },
+        {
+            "x": 5.927911696641962,
+            "y": 4.1025958147259995,
+            "heading": -0.3393222015882671,
+            "angularVelocity": -0.5924106252777295,
+            "velocityX": 3.4456475559080273,
+            "velocityY": 0.8366261416261735,
+            "timestamp": 2.060976464384768
+        },
+        {
+            "x": 6.112967212419423,
+            "y": 4.131783461785041,
+            "heading": -0.42258796983730795,
+            "angularVelocity": -1.4268810046507232,
+            "velocityX": 3.1711975499816933,
+            "velocityY": 0.500173120776742,
+            "timestamp": 2.119331553546126
+        },
+        {
+            "x": 6.288399006454155,
+            "y": 4.135519035273829,
+            "heading": -0.5357406933321943,
+            "angularVelocity": -1.9390377963780898,
+            "velocityX": 3.0062809697650312,
+            "velocityY": 0.06401452799529236,
+            "timestamp": 2.177686642707484
+        },
+        {
+            "x": 6.454763850529513,
+            "y": 4.112891662498818,
+            "heading": -0.6682840950132551,
+            "angularVelocity": -2.2713254933869544,
+            "velocityX": 2.850905490270817,
+            "velocityY": -0.3877532037084742,
+            "timestamp": 2.236041731868842
+        },
+        {
+            "x": 6.611553192138672,
+            "y": 4.063669681549072,
+            "heading": -0.8104452528667577,
+            "angularVelocity": -2.436139844811345,
+            "velocityX": 2.686815218045847,
+            "velocityY": -0.8434908018671928,
+            "timestamp": 2.2943968210302
+        },
+        {
+            "x": 6.724757817470915,
+            "y": 4.010978330480739,
+            "heading": -0.9191384597303142,
+            "angularVelocity": -2.4505521656643996,
+            "velocityX": 2.5522647438252197,
+            "velocityY": -1.1879574464517886,
+            "timestamp": 2.3387513989030966
+        },
+        {
+            "x": 6.831367598021963,
+            "y": 3.9432896977899077,
+            "heading": -1.0248995406249544,
+            "angularVelocity": -2.384445664159214,
+            "velocityX": 2.40358009620903,
+            "velocityY": -1.526079965968843,
+            "timestamp": 2.383105976775993
+        },
+        {
+            "x": 6.930834934158794,
+            "y": 3.86094565265281,
+            "heading": -1.1241964811981286,
+            "angularVelocity": -2.238707825328021,
+            "velocityX": 2.2425494933548396,
+            "velocityY": -1.856494844186423,
+            "timestamp": 2.4274605546488894
+        },
+        {
+            "x": 7.022708800988577,
+            "y": 3.764327451214886,
+            "heading": -1.2136436923328575,
+            "angularVelocity": -2.016639892077224,
+            "velocityX": 2.0713502694819663,
+            "velocityY": -2.1783140787585773,
+            "timestamp": 2.471815132521786
+        },
+        {
+            "x": 7.106642839511368,
+            "y": 3.6538333663708773,
+            "heading": -1.2900644922719695,
+            "angularVelocity": -1.7229518034892648,
+            "velocityX": 1.892342178597992,
+            "velocityY": -2.4911540170812514,
+            "timestamp": 2.5161697103946823
+        },
+        {
+            "x": 7.182389770268718,
+            "y": 3.5298642170672982,
+            "heading": -1.3504994507692183,
+            "angularVelocity": -1.3625416224325844,
+            "velocityX": 1.7077590271383551,
+            "velocityY": -2.794957256922336,
+            "timestamp": 2.5605242882675787
+        },
+        {
+            "x": 7.249751518053722,
+            "y": 3.39283877736109,
+            "heading": -1.392210624169865,
+            "angularVelocity": -0.9404028941539149,
+            "velocityX": 1.518710153843384,
+            "velocityY": -3.0893189897753475,
+            "timestamp": 2.604878866140475
+        },
+        {
+            "x": 7.30834760453933,
+            "y": 3.243309493178288,
+            "heading": -1.412928953137589,
+            "angularVelocity": -0.46710689090751717,
+            "velocityX": 1.3210831732751818,
+            "velocityY": -3.371225504868904,
+            "timestamp": 2.6492334440133716
+        },
+        {
+            "x": 7.356500151413166,
+            "y": 3.0825770019538052,
+            "heading": -1.4129301457519094,
+            "angularVelocity": -0.000026888190071743586,
+            "velocityX": 1.0856274410236224,
+            "velocityY": -3.623808385350466,
+            "timestamp": 2.693588021886268
+        },
+        {
+            "x": 7.388728975781619,
+            "y": 2.917910705433503,
+            "heading": -1.4129305799992817,
+            "angularVelocity": -0.00000979036196658053,
+            "velocityX": 0.7266177678617259,
+            "velocityY": -3.7124983353955976,
+            "timestamp": 2.7379425997591644
+        },
+        {
+            "x": 7.4166210769822385,
+            "y": 2.7524546215220593,
+            "heading": -1.4129310308638345,
+            "angularVelocity": -0.000010165006058865363,
+            "velocityX": 0.6288437978273086,
+            "velocityY": -3.73030455583591,
+            "timestamp": 2.782297177632061
+        },
+        {
+            "x": 7.4445129710491305,
+            "y": 2.5869985026929103,
+            "heading": -1.412931481728963,
+            "angularVelocity": -0.000010165019038462572,
+            "velocityX": 0.628839127875817,
+            "velocityY": -3.730305343076067,
+            "timestamp": 2.8266517555049573
+        },
+        {
+            "x": 7.4724048295219,
+            "y": 2.4215423778635246,
+            "heading": -1.4129319325942993,
+            "angularVelocity": -0.000010165023720794904,
+            "velocityX": 0.6288383253854289,
+            "velocityY": -3.7303054783549245,
+            "timestamp": 2.8710063333778537
+        },
+        {
+            "x": 7.500296654307517,
+            "y": 2.2560862473554004,
+            "heading": -1.4129323834599168,
+            "angularVelocity": -0.000010165030059818064,
+            "velocityX": 0.6288375658887922,
+            "velocityY": -3.73030560638543,
+            "timestamp": 2.91536091125075
+        },
+        {
+            "x": 7.528188447597349,
+            "y": 2.090630111537975,
+            "heading": -1.4129328343258867,
+            "angularVelocity": -0.000010165038009449021,
+            "velocityX": 0.6288368557978095,
+            "velocityY": -3.7303057260867334,
+            "timestamp": 2.9597154891236466
+        },
+        {
+            "x": 7.55608021118008,
+            "y": 1.9251739707128015,
+            "heading": -1.4129332851922731,
+            "angularVelocity": -0.000010165047399002056,
+            "velocityX": 0.6288361860338073,
+            "velocityY": -3.7303058389893544,
+            "timestamp": 3.004070066996543
+        },
+        {
+            "x": 7.583956606011812,
+            "y": 1.7597152400598883,
+            "heading": -1.412933736829767,
+            "angularVelocity": -0.000010182432471034155,
+            "velocityX": 0.6284896885190855,
+            "velocityY": -3.730364228176306,
+            "timestamp": 3.0484246448694394
+        },
+        {
+            "x": 7.603176305958169,
+            "y": 1.6036265149225075,
+            "heading": -1.4482789360266273,
+            "angularVelocity": -0.7968782680819577,
+            "velocityX": 0.4333194197323582,
+            "velocityY": -3.5191119524273815,
+            "timestamp": 3.092779222742336
+        },
+        {
+            "x": 7.616234477472668,
+            "y": 1.4592226194785036,
+            "heading": -1.5202643791491903,
+            "angularVelocity": -1.6229540799339002,
+            "velocityX": 0.2944041436245525,
+            "velocityY": -3.2556706064887218,
+            "timestamp": 3.1371338006152323
+        },
+        {
+            "x": 7.626690890740831,
+            "y": 1.32844105514062,
+            "heading": -1.6264074068747738,
+            "angularVelocity": -2.393056879714865,
+            "velocityX": 0.23574597639338804,
+            "velocityY": -2.9485471536366448,
+            "timestamp": 3.1814883784881287
+        },
+        {
+            "x": 7.6362242266749725,
+            "y": 1.2119757666324047,
+            "heading": -1.7630703603278275,
+            "angularVelocity": -3.0811465243718166,
+            "velocityX": 0.2149346559324737,
+            "velocityY": -2.6257783095571594,
+            "timestamp": 3.225842956361025
+        },
+        {
+            "x": 7.645916498443832,
+            "y": 1.110010534092232,
+            "heading": -1.9285530962892936,
+            "angularVelocity": -3.7309054419518417,
+            "velocityX": 0.21851795764202436,
+            "velocityY": -2.298866034355394,
+            "timestamp": 3.2701975342339216
+        },
+        {
+            "x": 7.656628280193831,
+            "y": 1.0222888536217107,
+            "heading": -2.123550329375603,
+            "angularVelocity": -4.396327108446363,
+            "velocityX": 0.2415034087506259,
+            "velocityY": -1.9777367901437044,
+            "timestamp": 3.314552112106818
+        },
+        {
+            "x": 7.664811531196715,
+            "y": 0.9478358295186623,
+            "heading": -2.3514420654429586,
+            "angularVelocity": -5.137952991468154,
+            "velocityX": 0.18449619848336826,
+            "velocityY": -1.6785871419271048,
+            "timestamp": 3.3589066899797144
+        },
+        {
+            "x": 7.671315625708808,
+            "y": 0.8895973140434956,
+            "heading": -2.585878637991588,
+            "angularVelocity": -5.285510172601276,
+            "velocityX": 0.146638629517147,
+            "velocityY": -1.313021524904517,
+            "timestamp": 3.403261267852611
+        },
+        {
+            "x": 7.675675621987506,
+            "y": 0.8463977620596795,
+            "heading": -2.797067073731653,
+            "angularVelocity": -4.761367278598646,
+            "velocityX": 0.0982986759831574,
+            "velocityY": -0.9739592631815782,
+            "timestamp": 3.4476158457255073
+        },
+        {
+            "x": 7.67812133477644,
+            "y": 0.8158415850765369,
+            "heading": -2.965913260384183,
+            "angularVelocity": -3.8067364125610696,
+            "velocityX": 0.05514003077524084,
+            "velocityY": -0.6889069505002431,
+            "timestamp": 3.4919704235984037
+        },
+        {
+            "x": 7.679402410293105,
+            "y": 0.7963789610313945,
+            "heading": -3.08300713864879,
+            "angularVelocity": -2.6399502346782358,
+            "velocityX": 0.02888259967968837,
+            "velocityY": -0.4387962861672373,
+            "timestamp": 3.5363250014713
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -3.143185307179586,
+            "angularVelocity": -1.3567521418700865,
+            "velocityX": 0.01347301080414647,
+            "velocityY": -0.2114541831120807,
+            "timestamp": 3.5806795793441966
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -3.143185307179586,
+            "angularVelocity": 1.5420512157892309e-32,
+            "velocityX": -2.431181718338826e-33,
+            "velocityY": 1.2990424762087674e-33,
+            "timestamp": 3.625034157217093
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.traj
new file mode 100644
index 00000000..9e5becca
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.traj	
@@ -0,0 +1,679 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": -2.1,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7185162341797315,
+            "y": 6.716270626486824,
+            "heading": -2.0163378917371135,
+            "angularVelocity": 1.6978541106369687,
+            "velocityX": 0.17283001240859905,
+            "velocityY": -0.07568458745445246,
+            "timestamp": 0.049275204352804955
+        },
+        {
+            "x": 0.7365174505438988,
+            "y": 6.709427263698689,
+            "heading": -1.8516035946552112,
+            "angularVelocity": 3.3431479228867977,
+            "velocityX": 0.3653199738205181,
+            "velocityY": -0.1388804547442707,
+            "timestamp": 0.09855040870560991
+        },
+        {
+            "x": 0.7651610538016371,
+            "y": 6.700606345332281,
+            "heading": -1.6098229565949118,
+            "angularVelocity": 4.906740443513477,
+            "velocityX": 0.5812985178641444,
+            "velocityY": -0.1790133289605956,
+            "timestamp": 0.14782561305841485
+        },
+        {
+            "x": 0.8095297793953646,
+            "y": 6.688220030792632,
+            "heading": -1.3224284097332135,
+            "angularVelocity": 5.832437442653418,
+            "velocityX": 0.9004270236212996,
+            "velocityY": -0.2513701303187648,
+            "timestamp": 0.19710081741121982
+        },
+        {
+            "x": 0.8714701737870825,
+            "y": 6.666054185298926,
+            "heading": -1.026684817124186,
+            "angularVelocity": 6.001874502468547,
+            "velocityX": 1.2570296806530035,
+            "velocityY": -0.4498377182771496,
+            "timestamp": 0.2463760217640248
+        },
+        {
+            "x": 0.9484340695976169,
+            "y": 6.633599911893754,
+            "heading": -0.7648347305022277,
+            "angularVelocity": 5.314033499427846,
+            "velocityX": 1.5619193633268658,
+            "velocityY": -0.6586329540675864,
+            "timestamp": 0.29565122611682976
+        },
+        {
+            "x": 1.0425919145867164,
+            "y": 6.594089868440422,
+            "heading": -0.5334353297349962,
+            "angularVelocity": 4.696061717176001,
+            "velocityX": 1.9108565093903993,
+            "velocityY": -0.8018240405548321,
+            "timestamp": 0.3449264304696347
+        },
+        {
+            "x": 1.155214051879495,
+            "y": 6.555173696166533,
+            "heading": -0.3327017892852748,
+            "angularVelocity": 4.073723145062813,
+            "velocityX": 2.285574230934019,
+            "velocityY": -0.7897719103355402,
+            "timestamp": 0.3942016348224397
+        },
+        {
+            "x": 1.2837470785266114,
+            "y": 6.519808002504125,
+            "heading": -0.17776538289371885,
+            "angularVelocity": 3.1443077390857384,
+            "velocityX": 2.6084727265022423,
+            "velocityY": -0.7177178487012317,
+            "timestamp": 0.44347683917524466
+        },
+        {
+            "x": 1.4286059942579405,
+            "y": 6.483992998763445,
+            "heading": -0.07145024720477738,
+            "angularVelocity": 2.1575787880601562,
+            "velocityX": 2.9397933024114007,
+            "velocityY": -0.7268362295212014,
+            "timestamp": 0.49275204352804963
+        },
+        {
+            "x": 1.5885837184337814,
+            "y": 6.442409682842055,
+            "heading": -0.012288104865979272,
+            "angularVelocity": 1.2006473258883674,
+            "velocityX": 3.246617163277867,
+            "velocityY": -0.8438994108204557,
+            "timestamp": 0.5420272478808545
+        },
+        {
+            "x": 1.7606060854656245,
+            "y": 6.389861829992508,
+            "heading": 0.005689657356259434,
+            "angularVelocity": 0.3648439911790104,
+            "velocityX": 3.491053346023324,
+            "velocityY": -1.0664157265246377,
+            "timestamp": 0.5913024522336595
+        },
+        {
+            "x": 1.937365788040802,
+            "y": 6.330680677458663,
+            "heading": 0.005693236516864234,
+            "angularVelocity": 0.00007263613924707546,
+            "velocityX": 3.587193699078299,
+            "velocityY": -1.2010331222599455,
+            "timestamp": 0.6405776565864645
+        },
+        {
+            "x": 2.11412543100989,
+            "y": 6.27149934643189,
+            "heading": 0.005696815267395046,
+            "angularVelocity": 0.00007262781713070066,
+            "velocityX": 3.5871924894214278,
+            "velocityY": -1.20103674462801,
+            "timestamp": 0.6898528609392695
+        },
+        {
+            "x": 2.2908850692554568,
+            "y": 6.212318001247489,
+            "heading": 0.005700393968496334,
+            "angularVelocity": 0.00007262681399886251,
+            "velocityX": 3.5871923935614243,
+            "velocityY": -1.2010370319454915,
+            "timestamp": 0.7391280652920744
+        },
+        {
+            "x": 2.467644702239147,
+            "y": 6.153136640294885,
+            "heading": 0.005703972617858713,
+            "angularVelocity": 0.00007262576400001342,
+            "velocityX": 3.5871922867759345,
+            "velocityY": -1.2010373519482989,
+            "timestamp": 0.7884032696448794
+        },
+        {
+            "x": 2.644404329357872,
+            "y": 6.0939552617700175,
+            "heading": 0.005707551213044099,
+            "angularVelocity": 0.00007262466452220377,
+            "velocityX": 3.5871921677512737,
+            "velocityY": -1.2010377085630215,
+            "timestamp": 0.8376784739976844
+        },
+        {
+            "x": 2.821163949952127,
+            "y": 6.0347738637002735,
+            "heading": 0.00571112975156944,
+            "angularVelocity": 0.00007262351465291134,
+            "velocityX": 3.5871920353424867,
+            "velocityY": -1.201038105210319,
+            "timestamp": 0.8869536783504893
+        },
+        {
+            "x": 2.9979235633249197,
+            "y": 5.975592444001196,
+            "heading": 0.005714708231053879,
+            "angularVelocity": 0.0000726223164660635,
+            "velocityX": 3.587191888788799,
+            "velocityY": -1.2010385441599662,
+            "timestamp": 0.9362288827032943
+        },
+        {
+            "x": 3.174683168778421,
+            "y": 5.916411000586595,
+            "heading": 0.005718286649774776,
+            "angularVelocity": 0.0000726210833196382,
+            "velocityX": 3.587191728073259,
+            "velocityY": -1.2010390254471126,
+            "timestamp": 0.9855040870560993
+        },
+        {
+            "x": 3.3514425126695637,
+            "y": 5.8572287763527235,
+            "heading": 0.005721865395381687,
+            "angularVelocity": 0.00007262771720412379,
+            "velocityX": 3.5871864198789636,
+            "velocityY": -1.2010548715360532,
+            "timestamp": 1.0347792914089042
+        },
+        {
+            "x": 3.5167193258876743,
+            "y": 5.790065395689035,
+            "heading": 0.026683569824856986,
+            "angularVelocity": 0.4254006595161298,
+            "velocityX": 3.354157844475837,
+            "velocityY": -1.3630259183261217,
+            "timestamp": 1.084054495761709
+        },
+        {
+            "x": 3.666994209555769,
+            "y": 5.710547155721769,
+            "heading": 0.06778616903847971,
+            "angularVelocity": 0.8341436581233173,
+            "velocityX": 3.0497059452487165,
+            "velocityY": -1.6137576903369897,
+            "timestamp": 1.133329700114514
+        },
+        {
+            "x": 3.802461838655518,
+            "y": 5.617737963132636,
+            "heading": 0.12219899767661042,
+            "angularVelocity": 1.104263885920004,
+            "velocityX": 2.749204815667845,
+            "velocityY": -1.8834867111788967,
+            "timestamp": 1.1826049044673188
+        },
+        {
+            "x": 3.923178434371948,
+            "y": 5.511256217956543,
+            "heading": 0.18485588032163774,
+            "angularVelocity": 1.2715702241722033,
+            "velocityX": 2.4498446490878596,
+            "velocityY": -2.1609599914329216,
+            "timestamp": 1.2318801088201237
+        },
+        {
+            "x": 4.036364016344018,
+            "y": 5.380109892116248,
+            "heading": 0.2569375572143931,
+            "angularVelocity": 1.3544496582801502,
+            "velocityX": 2.1268119643276364,
+            "velocityY": -2.4643030500437595,
+            "timestamp": 1.2850985333308458
+        },
+        {
+            "x": 4.130923397861632,
+            "y": 5.234522406039544,
+            "heading": 0.33431047652627577,
+            "angularVelocity": 1.4538746688431972,
+            "velocityX": 1.7768166267034577,
+            "velocityY": -2.7356594528154665,
+            "timestamp": 1.3383169578415677
+        },
+        {
+            "x": 4.205236217563619,
+            "y": 5.077100065980883,
+            "heading": 0.4216368931025466,
+            "angularVelocity": 1.640905708485922,
+            "velocityX": 1.396373913455762,
+            "velocityY": -2.958042097374491,
+            "timestamp": 1.3915353823522896
+        },
+        {
+            "x": 4.2828034673506234,
+            "y": 4.901292660688841,
+            "heading": 0.4516847731002138,
+            "angularVelocity": 0.5646142341476765,
+            "velocityX": 1.4575262326936578,
+            "velocityY": -3.303506387277969,
+            "timestamp": 1.4447538068630115
+        },
+        {
+            "x": 4.371277738622277,
+            "y": 4.723874668487275,
+            "heading": 0.4413575215812008,
+            "angularVelocity": -0.19405406330532018,
+            "velocityX": 1.6624744547600938,
+            "velocityY": -3.333770096215863,
+            "timestamp": 1.4979722313737334
+        },
+        {
+            "x": 4.466886857140796,
+            "y": 4.565900844635089,
+            "heading": 0.39184092001837434,
+            "angularVelocity": -0.9304409519460731,
+            "velocityX": 1.796541693925137,
+            "velocityY": -2.968404745246797,
+            "timestamp": 1.5511906558844553
+        },
+        {
+            "x": 4.573967443469787,
+            "y": 4.427674107970891,
+            "heading": 0.321042696188799,
+            "angularVelocity": -1.330332952928953,
+            "velocityX": 2.012096136130751,
+            "velocityY": -2.5973474024273386,
+            "timestamp": 1.6044090803951772
+        },
+        {
+            "x": 4.693065628013639,
+            "y": 4.309440309299518,
+            "heading": 0.2371568341960054,
+            "angularVelocity": -1.5762560196777895,
+            "velocityX": 2.2379126334312645,
+            "velocityY": -2.221670403781889,
+            "timestamp": 1.657627504905899
+        },
+        {
+            "x": 4.8242268562316895,
+            "y": 4.211382865905762,
+            "heading": 0.14523841868535878,
+            "angularVelocity": -1.7271915949358416,
+            "velocityX": 2.464583072947316,
+            "velocityY": -1.8425469054237094,
+            "timestamp": 1.710845929416621
+        },
+        {
+            "x": 4.982290420935857,
+            "y": 4.128393361272933,
+            "heading": 0.04004394713112955,
+            "angularVelocity": -1.8026614827604082,
+            "velocityX": 2.7086508987606095,
+            "velocityY": -1.4221468225908107,
+            "timestamp": 1.7692010185779787
+        },
+        {
+            "x": 5.153780525115527,
+            "y": 4.07042189977071,
+            "heading": -0.06589739756263054,
+            "angularVelocity": -1.8154602489051344,
+            "velocityX": 2.9387343356717652,
+            "velocityY": -0.9934259776713893,
+            "timestamp": 1.8275561077393365
+        },
+        {
+            "x": 5.3372218147923,
+            "y": 4.038159807995649,
+            "heading": -0.16628826181080308,
+            "angularVelocity": -1.7203446295931681,
+            "velocityX": 3.143535419328013,
+            "velocityY": -0.5528582380510605,
+            "timestamp": 1.8859111969006943
+        },
+        {
+            "x": 5.52929807317918,
+            "y": 4.032409431901968,
+            "heading": -0.2472171752765858,
+            "angularVelocity": -1.3868355721641668,
+            "velocityX": 3.2915082668414595,
+            "velocityY": -0.0985411242844827,
+            "timestamp": 1.944266286062052
+        },
+        {
+            "x": 5.726840626298333,
+            "y": 4.053774421636682,
+            "heading": -0.3047520267300494,
+            "angularVelocity": -0.985944024425596,
+            "velocityX": 3.385181240541464,
+            "velocityY": 0.3661204196884676,
+            "timestamp": 2.00262137522341
+        },
+        {
+            "x": 5.927911696641962,
+            "y": 4.1025958147259995,
+            "heading": -0.3393222015882671,
+            "angularVelocity": -0.5924106252777295,
+            "velocityX": 3.4456475559080273,
+            "velocityY": 0.8366261416261735,
+            "timestamp": 2.060976464384768
+        },
+        {
+            "x": 6.112967212419423,
+            "y": 4.131783461785041,
+            "heading": -0.42258796983730795,
+            "angularVelocity": -1.4268810046507232,
+            "velocityX": 3.1711975499816933,
+            "velocityY": 0.500173120776742,
+            "timestamp": 2.119331553546126
+        },
+        {
+            "x": 6.288399006454155,
+            "y": 4.135519035273829,
+            "heading": -0.5357406933321943,
+            "angularVelocity": -1.9390377963780898,
+            "velocityX": 3.0062809697650312,
+            "velocityY": 0.06401452799529236,
+            "timestamp": 2.177686642707484
+        },
+        {
+            "x": 6.454763850529513,
+            "y": 4.112891662498818,
+            "heading": -0.6682840950132551,
+            "angularVelocity": -2.2713254933869544,
+            "velocityX": 2.850905490270817,
+            "velocityY": -0.3877532037084742,
+            "timestamp": 2.236041731868842
+        },
+        {
+            "x": 6.611553192138672,
+            "y": 4.063669681549072,
+            "heading": -0.8104452528667577,
+            "angularVelocity": -2.436139844811345,
+            "velocityX": 2.686815218045847,
+            "velocityY": -0.8434908018671928,
+            "timestamp": 2.2943968210302
+        },
+        {
+            "x": 6.724757817470915,
+            "y": 4.010978330480739,
+            "heading": -0.9191384597303142,
+            "angularVelocity": -2.4505521656643996,
+            "velocityX": 2.5522647438252197,
+            "velocityY": -1.1879574464517886,
+            "timestamp": 2.3387513989030966
+        },
+        {
+            "x": 6.831367598021963,
+            "y": 3.9432896977899077,
+            "heading": -1.0248995406249544,
+            "angularVelocity": -2.384445664159214,
+            "velocityX": 2.40358009620903,
+            "velocityY": -1.526079965968843,
+            "timestamp": 2.383105976775993
+        },
+        {
+            "x": 6.930834934158794,
+            "y": 3.86094565265281,
+            "heading": -1.1241964811981286,
+            "angularVelocity": -2.238707825328021,
+            "velocityX": 2.2425494933548396,
+            "velocityY": -1.856494844186423,
+            "timestamp": 2.4274605546488894
+        },
+        {
+            "x": 7.022708800988577,
+            "y": 3.764327451214886,
+            "heading": -1.2136436923328575,
+            "angularVelocity": -2.016639892077224,
+            "velocityX": 2.0713502694819663,
+            "velocityY": -2.1783140787585773,
+            "timestamp": 2.471815132521786
+        },
+        {
+            "x": 7.106642839511368,
+            "y": 3.6538333663708773,
+            "heading": -1.2900644922719695,
+            "angularVelocity": -1.7229518034892648,
+            "velocityX": 1.892342178597992,
+            "velocityY": -2.4911540170812514,
+            "timestamp": 2.5161697103946823
+        },
+        {
+            "x": 7.182389770268718,
+            "y": 3.5298642170672982,
+            "heading": -1.3504994507692183,
+            "angularVelocity": -1.3625416224325844,
+            "velocityX": 1.7077590271383551,
+            "velocityY": -2.794957256922336,
+            "timestamp": 2.5605242882675787
+        },
+        {
+            "x": 7.249751518053722,
+            "y": 3.39283877736109,
+            "heading": -1.392210624169865,
+            "angularVelocity": -0.9404028941539149,
+            "velocityX": 1.518710153843384,
+            "velocityY": -3.0893189897753475,
+            "timestamp": 2.604878866140475
+        },
+        {
+            "x": 7.30834760453933,
+            "y": 3.243309493178288,
+            "heading": -1.412928953137589,
+            "angularVelocity": -0.46710689090751717,
+            "velocityX": 1.3210831732751818,
+            "velocityY": -3.371225504868904,
+            "timestamp": 2.6492334440133716
+        },
+        {
+            "x": 7.356500151413166,
+            "y": 3.0825770019538052,
+            "heading": -1.4129301457519094,
+            "angularVelocity": -0.000026888190071743586,
+            "velocityX": 1.0856274410236224,
+            "velocityY": -3.623808385350466,
+            "timestamp": 2.693588021886268
+        },
+        {
+            "x": 7.388728975781619,
+            "y": 2.917910705433503,
+            "heading": -1.4129305799992817,
+            "angularVelocity": -0.00000979036196658053,
+            "velocityX": 0.7266177678617259,
+            "velocityY": -3.7124983353955976,
+            "timestamp": 2.7379425997591644
+        },
+        {
+            "x": 7.4166210769822385,
+            "y": 2.7524546215220593,
+            "heading": -1.4129310308638345,
+            "angularVelocity": -0.000010165006058865363,
+            "velocityX": 0.6288437978273086,
+            "velocityY": -3.73030455583591,
+            "timestamp": 2.782297177632061
+        },
+        {
+            "x": 7.4445129710491305,
+            "y": 2.5869985026929103,
+            "heading": -1.412931481728963,
+            "angularVelocity": -0.000010165019038462572,
+            "velocityX": 0.628839127875817,
+            "velocityY": -3.730305343076067,
+            "timestamp": 2.8266517555049573
+        },
+        {
+            "x": 7.4724048295219,
+            "y": 2.4215423778635246,
+            "heading": -1.4129319325942993,
+            "angularVelocity": -0.000010165023720794904,
+            "velocityX": 0.6288383253854289,
+            "velocityY": -3.7303054783549245,
+            "timestamp": 2.8710063333778537
+        },
+        {
+            "x": 7.500296654307517,
+            "y": 2.2560862473554004,
+            "heading": -1.4129323834599168,
+            "angularVelocity": -0.000010165030059818064,
+            "velocityX": 0.6288375658887922,
+            "velocityY": -3.73030560638543,
+            "timestamp": 2.91536091125075
+        },
+        {
+            "x": 7.528188447597349,
+            "y": 2.090630111537975,
+            "heading": -1.4129328343258867,
+            "angularVelocity": -0.000010165038009449021,
+            "velocityX": 0.6288368557978095,
+            "velocityY": -3.7303057260867334,
+            "timestamp": 2.9597154891236466
+        },
+        {
+            "x": 7.55608021118008,
+            "y": 1.9251739707128015,
+            "heading": -1.4129332851922731,
+            "angularVelocity": -0.000010165047399002056,
+            "velocityX": 0.6288361860338073,
+            "velocityY": -3.7303058389893544,
+            "timestamp": 3.004070066996543
+        },
+        {
+            "x": 7.583956606011812,
+            "y": 1.7597152400598883,
+            "heading": -1.412933736829767,
+            "angularVelocity": -0.000010182432471034155,
+            "velocityX": 0.6284896885190855,
+            "velocityY": -3.730364228176306,
+            "timestamp": 3.0484246448694394
+        },
+        {
+            "x": 7.603176305958169,
+            "y": 1.6036265149225075,
+            "heading": -1.4482789360266273,
+            "angularVelocity": -0.7968782680819577,
+            "velocityX": 0.4333194197323582,
+            "velocityY": -3.5191119524273815,
+            "timestamp": 3.092779222742336
+        },
+        {
+            "x": 7.616234477472668,
+            "y": 1.4592226194785036,
+            "heading": -1.5202643791491903,
+            "angularVelocity": -1.6229540799339002,
+            "velocityX": 0.2944041436245525,
+            "velocityY": -3.2556706064887218,
+            "timestamp": 3.1371338006152323
+        },
+        {
+            "x": 7.626690890740831,
+            "y": 1.32844105514062,
+            "heading": -1.6264074068747738,
+            "angularVelocity": -2.393056879714865,
+            "velocityX": 0.23574597639338804,
+            "velocityY": -2.9485471536366448,
+            "timestamp": 3.1814883784881287
+        },
+        {
+            "x": 7.6362242266749725,
+            "y": 1.2119757666324047,
+            "heading": -1.7630703603278275,
+            "angularVelocity": -3.0811465243718166,
+            "velocityX": 0.2149346559324737,
+            "velocityY": -2.6257783095571594,
+            "timestamp": 3.225842956361025
+        },
+        {
+            "x": 7.645916498443832,
+            "y": 1.110010534092232,
+            "heading": -1.9285530962892936,
+            "angularVelocity": -3.7309054419518417,
+            "velocityX": 0.21851795764202436,
+            "velocityY": -2.298866034355394,
+            "timestamp": 3.2701975342339216
+        },
+        {
+            "x": 7.656628280193831,
+            "y": 1.0222888536217107,
+            "heading": -2.123550329375603,
+            "angularVelocity": -4.396327108446363,
+            "velocityX": 0.2415034087506259,
+            "velocityY": -1.9777367901437044,
+            "timestamp": 3.314552112106818
+        },
+        {
+            "x": 7.664811531196715,
+            "y": 0.9478358295186623,
+            "heading": -2.3514420654429586,
+            "angularVelocity": -5.137952991468154,
+            "velocityX": 0.18449619848336826,
+            "velocityY": -1.6785871419271048,
+            "timestamp": 3.3589066899797144
+        },
+        {
+            "x": 7.671315625708808,
+            "y": 0.8895973140434956,
+            "heading": -2.585878637991588,
+            "angularVelocity": -5.285510172601276,
+            "velocityX": 0.146638629517147,
+            "velocityY": -1.313021524904517,
+            "timestamp": 3.403261267852611
+        },
+        {
+            "x": 7.675675621987506,
+            "y": 0.8463977620596795,
+            "heading": -2.797067073731653,
+            "angularVelocity": -4.761367278598646,
+            "velocityX": 0.0982986759831574,
+            "velocityY": -0.9739592631815782,
+            "timestamp": 3.4476158457255073
+        },
+        {
+            "x": 7.67812133477644,
+            "y": 0.8158415850765369,
+            "heading": -2.965913260384183,
+            "angularVelocity": -3.8067364125610696,
+            "velocityX": 0.05514003077524084,
+            "velocityY": -0.6889069505002431,
+            "timestamp": 3.4919704235984037
+        },
+        {
+            "x": 7.679402410293105,
+            "y": 0.7963789610313945,
+            "heading": -3.08300713864879,
+            "angularVelocity": -2.6399502346782358,
+            "velocityX": 0.02888259967968837,
+            "velocityY": -0.4387962861672373,
+            "timestamp": 3.5363250014713
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -3.143185307179586,
+            "angularVelocity": -1.3567521418700865,
+            "velocityX": 0.01347301080414647,
+            "velocityY": -0.2114541831120807,
+            "timestamp": 3.5806795793441966
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -3.143185307179586,
+            "angularVelocity": 1.5420512157892309e-32,
+            "velocityX": -2.431181718338826e-33,
+            "velocityY": 1.2990424762087674e-33,
+            "timestamp": 3.625034157217093
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj
new file mode 100644
index 00000000..9e6c02b6
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj	
@@ -0,0 +1,121 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": -2.1,
+            "angularVelocity": -8.4685465984255e-32,
+            "velocityX": -1.4242674774019307e-33,
+            "velocityY": -4.454347432487034e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7541194456754691,
+            "y": 6.720987374332373,
+            "heading": -2.1237071075059206,
+            "angularVelocity": -0.324046498361879,
+            "velocityX": 0.6030576221600736,
+            "velocityY": 0.013496171766179704,
+            "timestamp": 0.07315958550932472
+        },
+        {
+            "x": 0.8424134938705199,
+            "y": 6.722973032705717,
+            "heading": -2.1697540257675185,
+            "angularVelocity": -0.6294037608468737,
+            "velocityX": 1.2068691693694522,
+            "velocityY": 0.027141465599130124,
+            "timestamp": 0.14631917101864944
+        },
+        {
+            "x": 0.9749625148381232,
+            "y": 6.725970459573204,
+            "heading": -2.2360020755513283,
+            "angularVelocity": -0.9055279540276729,
+            "velocityX": 1.811779277381365,
+            "velocityY": 0.040971075035846904,
+            "timestamp": 0.21947875652797416
+        },
+        {
+            "x": 1.1518930933890432,
+            "y": 6.729994686272588,
+            "heading": -2.318545216019314,
+            "angularVelocity": -1.1282614560115307,
+            "velocityX": 2.418419641379311,
+            "velocityY": 0.055006144053120966,
+            "timestamp": 0.2926383420372989
+        },
+        {
+            "x": 1.3734035763726509,
+            "y": 6.735067561069991,
+            "heading": -2.4074942006161044,
+            "angularVelocity": -1.215821330554877,
+            "velocityX": 3.0277711586456673,
+            "velocityY": 0.0693398515327658,
+            "timestamp": 0.3657979275466236
+        },
+        {
+            "x": 1.6366126950009532,
+            "y": 6.741789773246868,
+            "heading": -2.44063463913809,
+            "angularVelocity": -0.45298833080218254,
+            "velocityX": 3.59773933649109,
+            "velocityY": 0.09188422993442831,
+            "timestamp": 0.4389575130559483
+        },
+        {
+            "x": 1.858120407547834,
+            "y": 6.746837523733308,
+            "heading": -2.530120705195818,
+            "angularVelocity": -1.2231625621549276,
+            "velocityX": 3.027733290241823,
+            "velocityY": 0.06899643363609939,
+            "timestamp": 0.512117098565273
+        },
+        {
+            "x": 2.0350481983019497,
+            "y": 6.750889776022306,
+            "heading": -2.613093703885261,
+            "angularVelocity": -1.134137080080631,
+            "velocityX": 2.418381535684925,
+            "velocityY": 0.05538921879868258,
+            "timestamp": 0.5852766840745978
+        },
+        {
+            "x": 2.1675926697007295,
+            "y": 6.753939244710426,
+            "heading": -2.6797699231631307,
+            "angularVelocity": -0.9113804953059623,
+            "velocityX": 1.8117170904677296,
+            "velocityY": 0.041682421611463606,
+            "timestamp": 0.6584362695839225
+        },
+        {
+            "x": 2.255882529656213,
+            "y": 6.7559780839356005,
+            "heading": -2.7261390096327216,
+            "angularVelocity": -0.6338073971683945,
+            "velocityX": 1.2068119213746804,
+            "velocityY": 0.027868381306137554,
+            "timestamp": 0.7315958550932473
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": -0.32614988454563837,
+            "velocityX": 0.603030621847418,
+            "velocityY": 0.013968314025900756,
+            "timestamp": 0.8047554406025721
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": -2.091519469848374e-32,
+            "velocityX": -8.675585045811285e-34,
+            "velocityY": 3.70068025566541e-32,
+            "timestamp": 0.8779150261118969
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.traj
new file mode 100644
index 00000000..9e6c02b6
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.traj	
@@ -0,0 +1,121 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": -2.1,
+            "angularVelocity": -8.4685465984255e-32,
+            "velocityX": -1.4242674774019307e-33,
+            "velocityY": -4.454347432487034e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7541194456754691,
+            "y": 6.720987374332373,
+            "heading": -2.1237071075059206,
+            "angularVelocity": -0.324046498361879,
+            "velocityX": 0.6030576221600736,
+            "velocityY": 0.013496171766179704,
+            "timestamp": 0.07315958550932472
+        },
+        {
+            "x": 0.8424134938705199,
+            "y": 6.722973032705717,
+            "heading": -2.1697540257675185,
+            "angularVelocity": -0.6294037608468737,
+            "velocityX": 1.2068691693694522,
+            "velocityY": 0.027141465599130124,
+            "timestamp": 0.14631917101864944
+        },
+        {
+            "x": 0.9749625148381232,
+            "y": 6.725970459573204,
+            "heading": -2.2360020755513283,
+            "angularVelocity": -0.9055279540276729,
+            "velocityX": 1.811779277381365,
+            "velocityY": 0.040971075035846904,
+            "timestamp": 0.21947875652797416
+        },
+        {
+            "x": 1.1518930933890432,
+            "y": 6.729994686272588,
+            "heading": -2.318545216019314,
+            "angularVelocity": -1.1282614560115307,
+            "velocityX": 2.418419641379311,
+            "velocityY": 0.055006144053120966,
+            "timestamp": 0.2926383420372989
+        },
+        {
+            "x": 1.3734035763726509,
+            "y": 6.735067561069991,
+            "heading": -2.4074942006161044,
+            "angularVelocity": -1.215821330554877,
+            "velocityX": 3.0277711586456673,
+            "velocityY": 0.0693398515327658,
+            "timestamp": 0.3657979275466236
+        },
+        {
+            "x": 1.6366126950009532,
+            "y": 6.741789773246868,
+            "heading": -2.44063463913809,
+            "angularVelocity": -0.45298833080218254,
+            "velocityX": 3.59773933649109,
+            "velocityY": 0.09188422993442831,
+            "timestamp": 0.4389575130559483
+        },
+        {
+            "x": 1.858120407547834,
+            "y": 6.746837523733308,
+            "heading": -2.530120705195818,
+            "angularVelocity": -1.2231625621549276,
+            "velocityX": 3.027733290241823,
+            "velocityY": 0.06899643363609939,
+            "timestamp": 0.512117098565273
+        },
+        {
+            "x": 2.0350481983019497,
+            "y": 6.750889776022306,
+            "heading": -2.613093703885261,
+            "angularVelocity": -1.134137080080631,
+            "velocityX": 2.418381535684925,
+            "velocityY": 0.05538921879868258,
+            "timestamp": 0.5852766840745978
+        },
+        {
+            "x": 2.1675926697007295,
+            "y": 6.753939244710426,
+            "heading": -2.6797699231631307,
+            "angularVelocity": -0.9113804953059623,
+            "velocityX": 1.8117170904677296,
+            "velocityY": 0.041682421611463606,
+            "timestamp": 0.6584362695839225
+        },
+        {
+            "x": 2.255882529656213,
+            "y": 6.7559780839356005,
+            "heading": -2.7261390096327216,
+            "angularVelocity": -0.6338073971683945,
+            "velocityX": 1.2068119213746804,
+            "velocityY": 0.027868381306137554,
+            "timestamp": 0.7315958550932473
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": -0.32614988454563837,
+            "velocityX": 0.603030621847418,
+            "velocityY": 0.013968314025900756,
+            "timestamp": 0.8047554406025721
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": -2.091519469848374e-32,
+            "velocityX": -8.675585045811285e-34,
+            "velocityY": 3.70068025566541e-32,
+            "timestamp": 0.8779150261118969
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.1.traj
new file mode 100644
index 00000000..d2e16138
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.1.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": -2.1,
+            "angularVelocity": 2.001911519996272e-31,
+            "velocityX": 6.781624829737293e-33,
+            "velocityY": -4.0943813533671796e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7269029739475109,
+            "y": 6.6955056066592755,
+            "heading": -2.1380447151842032,
+            "angularVelocity": -0.6185663850269976,
+            "velocityX": 0.27482428085724375,
+            "velocityY": -0.3982526421565261,
+            "timestamp": 0.061504659976863815
+        },
+        {
+            "x": 0.7606767634590716,
+            "y": 6.646279647618601,
+            "heading": -2.2124384537514556,
+            "angularVelocity": -1.2095626346887691,
+            "velocityX": 0.5491257007886129,
+            "velocityY": -0.8003614532490962,
+            "timestamp": 0.12300931995372763
+        },
+        {
+            "x": 0.8112924785066278,
+            "y": 6.571981628339174,
+            "heading": -2.320229893429466,
+            "angularVelocity": -1.752573540257899,
+            "velocityX": 0.8229573997579402,
+            "velocityY": -1.208006341428056,
+            "timestamp": 0.18451397993059143
+        },
+        {
+            "x": 0.8787853408643597,
+            "y": 6.472118688114863,
+            "heading": -2.4555147279599576,
+            "angularVelocity": -2.199586739954034,
+            "velocityX": 1.0973617671103395,
+            "velocityY": -1.6236646176383343,
+            "timestamp": 0.24601863990745526
+        },
+        {
+            "x": 0.9632675519056847,
+            "y": 6.346052032301826,
+            "heading": -2.6067469489099335,
+            "angularVelocity": -2.4588741894819854,
+            "velocityX": 1.37359040880975,
+            "velocityY": -2.049709011649839,
+            "timestamp": 0.3075232998843191
+        },
+        {
+            "x": 1.0643460613698084,
+            "y": 6.193349880124978,
+            "heading": -2.74665413649709,
+            "angularVelocity": -2.274741257650809,
+            "velocityX": 1.643428473584707,
+            "velocityY": -2.482773699330916,
+            "timestamp": 0.3690279598611829
+        },
+        {
+            "x": 1.1777092396516182,
+            "y": 6.018032409257481,
+            "heading": -2.813593357981312,
+            "angularVelocity": -1.0883601585538758,
+            "velocityX": 1.8431640517068764,
+            "velocityY": -2.850474597102823,
+            "timestamp": 0.43053261983804675
+        },
+        {
+            "x": 1.3085884905177934,
+            "y": 5.825664975179578,
+            "heading": -2.813593427712779,
+            "angularVelocity": -0.0000011337590858279882,
+            "velocityX": 2.1279566607702214,
+            "velocityY": -3.1276887661888684,
+            "timestamp": 0.4920372798149106
+        },
+        {
+            "x": 1.4394691966966826,
+            "y": 5.633298531244646,
+            "heading": -2.8135934974374672,
+            "angularVelocity": -0.000001133648866416165,
+            "velocityX": 2.127980322598685,
+            "velocityY": -3.127672667522972,
+            "timestamp": 0.5535419397917744
+        },
+        {
+            "x": 1.5703499198247641,
+            "y": 5.440932098841455,
+            "heading": -2.813593567162125,
+            "angularVelocity": -0.0000011336483749077044,
+            "velocityX": 2.1279805981744278,
+            "velocityY": -3.1276724800292155,
+            "timestamp": 0.6150465997686382
+        },
+        {
+            "x": 1.7012306619349875,
+            "y": 5.248565679353167,
+            "heading": -2.81359363688675,
+            "angularVelocity": -0.0000011336478448323905,
+            "velocityX": 2.1279809068037574,
+            "velocityY": -3.1276722700467015,
+            "timestamp": 0.676551259745502
+        },
+        {
+            "x": 1.8321095906543323,
+            "y": 5.056198026100399,
+            "heading": -2.8135937066202716,
+            "angularVelocity": -0.0000011337924880961462,
+            "velocityX": 2.1279514230072554,
+            "velocityY": -3.127692329737781,
+            "timestamp": 0.7380559197223658
+        },
+        {
+            "x": 1.945432077761735,
+            "y": 4.881276454017442,
+            "heading": -2.879360901618447,
+            "angularVelocity": -1.0693042612204542,
+            "velocityX": 1.8425024567249244,
+            "velocityY": -2.8440377062283995,
+            "timestamp": 0.7995605796992297
+        },
+        {
+            "x": 2.046464590922967,
+            "y": 4.730069111144926,
+            "heading": -3.0091287979589167,
+            "angularVelocity": -2.109887224631182,
+            "velocityX": 1.6426806228867414,
+            "velocityY": -2.458469698546365,
+            "timestamp": 0.8610652396760935
+        },
+        {
+            "x": 2.131137367212944,
+            "y": 4.6049715874663555,
+            "heading": -3.1629399624477488,
+            "angularVelocity": -2.500805053579533,
+            "velocityX": 1.3766887959681082,
+            "velocityY": -2.0339519595039928,
+            "timestamp": 0.9225698996529573
+        },
+        {
+            "x": 2.1987047903626262,
+            "y": 4.5060589736155245,
+            "heading": -3.302229643809037,
+            "angularVelocity": -2.2647012667606727,
+            "velocityX": 1.0985740458544027,
+            "velocityY": -1.6082133270558499,
+            "timestamp": 0.9840745596298212
+        },
+        {
+            "x": 2.2493460059898798,
+            "y": 4.432630316522509,
+            "heading": -3.4143130823982606,
+            "angularVelocity": -1.8223568528203613,
+            "velocityX": 0.8233720119142732,
+            "velocityY": -1.1938714419466272,
+            "timestamp": 1.0455792196066849
+        },
+        {
+            "x": 2.2831132091553847,
+            "y": 4.384103436110321,
+            "heading": -3.4927231006079147,
+            "angularVelocity": -1.2748630467862023,
+            "velocityX": 0.5490186138449845,
+            "velocityY": -0.7889951823234699,
+            "timestamp": 1.1070838795835487
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -3.5331853071795862,
+            "angularVelocity": -0.6578722097950352,
+            "velocityX": 0.2745611609098736,
+            "velocityY": -0.39189609566798916,
+            "timestamp": 1.1685885395604125
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -3.5331853071795862,
+            "angularVelocity": 8.150679561734844e-31,
+            "velocityX": 0,
+            "velocityY": -5.044788235947052e-32,
+            "timestamp": 1.2300931995372764
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.traj
new file mode 100644
index 00000000..d2e16138
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": -2.1,
+            "angularVelocity": 2.001911519996272e-31,
+            "velocityX": 6.781624829737293e-33,
+            "velocityY": -4.0943813533671796e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7269029739475109,
+            "y": 6.6955056066592755,
+            "heading": -2.1380447151842032,
+            "angularVelocity": -0.6185663850269976,
+            "velocityX": 0.27482428085724375,
+            "velocityY": -0.3982526421565261,
+            "timestamp": 0.061504659976863815
+        },
+        {
+            "x": 0.7606767634590716,
+            "y": 6.646279647618601,
+            "heading": -2.2124384537514556,
+            "angularVelocity": -1.2095626346887691,
+            "velocityX": 0.5491257007886129,
+            "velocityY": -0.8003614532490962,
+            "timestamp": 0.12300931995372763
+        },
+        {
+            "x": 0.8112924785066278,
+            "y": 6.571981628339174,
+            "heading": -2.320229893429466,
+            "angularVelocity": -1.752573540257899,
+            "velocityX": 0.8229573997579402,
+            "velocityY": -1.208006341428056,
+            "timestamp": 0.18451397993059143
+        },
+        {
+            "x": 0.8787853408643597,
+            "y": 6.472118688114863,
+            "heading": -2.4555147279599576,
+            "angularVelocity": -2.199586739954034,
+            "velocityX": 1.0973617671103395,
+            "velocityY": -1.6236646176383343,
+            "timestamp": 0.24601863990745526
+        },
+        {
+            "x": 0.9632675519056847,
+            "y": 6.346052032301826,
+            "heading": -2.6067469489099335,
+            "angularVelocity": -2.4588741894819854,
+            "velocityX": 1.37359040880975,
+            "velocityY": -2.049709011649839,
+            "timestamp": 0.3075232998843191
+        },
+        {
+            "x": 1.0643460613698084,
+            "y": 6.193349880124978,
+            "heading": -2.74665413649709,
+            "angularVelocity": -2.274741257650809,
+            "velocityX": 1.643428473584707,
+            "velocityY": -2.482773699330916,
+            "timestamp": 0.3690279598611829
+        },
+        {
+            "x": 1.1777092396516182,
+            "y": 6.018032409257481,
+            "heading": -2.813593357981312,
+            "angularVelocity": -1.0883601585538758,
+            "velocityX": 1.8431640517068764,
+            "velocityY": -2.850474597102823,
+            "timestamp": 0.43053261983804675
+        },
+        {
+            "x": 1.3085884905177934,
+            "y": 5.825664975179578,
+            "heading": -2.813593427712779,
+            "angularVelocity": -0.0000011337590858279882,
+            "velocityX": 2.1279566607702214,
+            "velocityY": -3.1276887661888684,
+            "timestamp": 0.4920372798149106
+        },
+        {
+            "x": 1.4394691966966826,
+            "y": 5.633298531244646,
+            "heading": -2.8135934974374672,
+            "angularVelocity": -0.000001133648866416165,
+            "velocityX": 2.127980322598685,
+            "velocityY": -3.127672667522972,
+            "timestamp": 0.5535419397917744
+        },
+        {
+            "x": 1.5703499198247641,
+            "y": 5.440932098841455,
+            "heading": -2.813593567162125,
+            "angularVelocity": -0.0000011336483749077044,
+            "velocityX": 2.1279805981744278,
+            "velocityY": -3.1276724800292155,
+            "timestamp": 0.6150465997686382
+        },
+        {
+            "x": 1.7012306619349875,
+            "y": 5.248565679353167,
+            "heading": -2.81359363688675,
+            "angularVelocity": -0.0000011336478448323905,
+            "velocityX": 2.1279809068037574,
+            "velocityY": -3.1276722700467015,
+            "timestamp": 0.676551259745502
+        },
+        {
+            "x": 1.8321095906543323,
+            "y": 5.056198026100399,
+            "heading": -2.8135937066202716,
+            "angularVelocity": -0.0000011337924880961462,
+            "velocityX": 2.1279514230072554,
+            "velocityY": -3.127692329737781,
+            "timestamp": 0.7380559197223658
+        },
+        {
+            "x": 1.945432077761735,
+            "y": 4.881276454017442,
+            "heading": -2.879360901618447,
+            "angularVelocity": -1.0693042612204542,
+            "velocityX": 1.8425024567249244,
+            "velocityY": -2.8440377062283995,
+            "timestamp": 0.7995605796992297
+        },
+        {
+            "x": 2.046464590922967,
+            "y": 4.730069111144926,
+            "heading": -3.0091287979589167,
+            "angularVelocity": -2.109887224631182,
+            "velocityX": 1.6426806228867414,
+            "velocityY": -2.458469698546365,
+            "timestamp": 0.8610652396760935
+        },
+        {
+            "x": 2.131137367212944,
+            "y": 4.6049715874663555,
+            "heading": -3.1629399624477488,
+            "angularVelocity": -2.500805053579533,
+            "velocityX": 1.3766887959681082,
+            "velocityY": -2.0339519595039928,
+            "timestamp": 0.9225698996529573
+        },
+        {
+            "x": 2.1987047903626262,
+            "y": 4.5060589736155245,
+            "heading": -3.302229643809037,
+            "angularVelocity": -2.2647012667606727,
+            "velocityX": 1.0985740458544027,
+            "velocityY": -1.6082133270558499,
+            "timestamp": 0.9840745596298212
+        },
+        {
+            "x": 2.2493460059898798,
+            "y": 4.432630316522509,
+            "heading": -3.4143130823982606,
+            "angularVelocity": -1.8223568528203613,
+            "velocityX": 0.8233720119142732,
+            "velocityY": -1.1938714419466272,
+            "timestamp": 1.0455792196066849
+        },
+        {
+            "x": 2.2831132091553847,
+            "y": 4.384103436110321,
+            "heading": -3.4927231006079147,
+            "angularVelocity": -1.2748630467862023,
+            "velocityX": 0.5490186138449845,
+            "velocityY": -0.7889951823234699,
+            "timestamp": 1.1070838795835487
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -3.5331853071795862,
+            "angularVelocity": -0.6578722097950352,
+            "velocityX": 0.2745611609098736,
+            "velocityY": -0.39189609566798916,
+            "timestamp": 1.1685885395604125
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -3.5331853071795862,
+            "angularVelocity": 8.150679561734844e-31,
+            "velocityX": 0,
+            "velocityY": -5.044788235947052e-32,
+            "timestamp": 1.2300931995372764
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.1.traj
new file mode 100644
index 00000000..c222ae0b
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.1.traj	
@@ -0,0 +1,175 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 1.0688205894738666e-32,
+            "velocityX": 4.722903296583099e-32,
+            "velocityY": -7.052966104933725e-38,
+            "timestamp": 0
+        },
+        {
+            "x": 7.680000100019751,
+            "y": 7.385155846048263,
+            "heading": 3.1400000209558985,
+            "angularVelocity": 2.1151775994912985e-7,
+            "velocityX": 0.0000010095466114596713,
+            "velocityY": -0.8260917454272356,
+            "timestamp": 0.09907392781101554
+        },
+        {
+            "x": 7.680000295060445,
+            "y": 7.221467541258139,
+            "heading": 3.1400000632436256,
+            "angularVelocity": 4.2682995289468236e-7,
+            "velocityX": 0.000001968637858711507,
+            "velocityY": -1.6521834594299558,
+            "timestamp": 0.19814785562203108
+        },
+        {
+            "x": 7.680000579860811,
+            "y": 6.975935091856332,
+            "heading": 3.140000126701719,
+            "angularVelocity": 6.405123969521912e-7,
+            "velocityX": 0.0000028746245916429767,
+            "velocityY": -2.4782751105836276,
+            "timestamp": 0.2972217834330466
+        },
+        {
+            "x": 7.680000948789776,
+            "y": 6.648558516522933,
+            "heading": 3.140000210348385,
+            "angularVelocity": 8.442850564566377e-7,
+            "velocityX": 0.0000037237742032736077,
+            "velocityY": -3.3043665731903222,
+            "timestamp": 0.39629571124406215
+        },
+        {
+            "x": 7.68000127512063,
+            "y": 6.273767735073521,
+            "heading": 3.1400002103483864,
+            "angularVelocity": 1.8797601612350415e-14,
+            "velocityX": 0.0000032938116008237123,
+            "velocityY": -3.782940575085686,
+            "timestamp": 0.4953696390550777
+        },
+        {
+            "x": 7.680001534050369,
+            "y": 5.898976953624012,
+            "heading": 3.140000210348386,
+            "angularVelocity": -4.168435621307512e-16,
+            "velocityX": 0.0000026135000838056266,
+            "velocityY": -3.782940575086706,
+            "timestamp": 0.5944435668660932
+        },
+        {
+            "x": 7.680001718421092,
+            "y": 5.524186172174471,
+            "heading": 3.14000021034838,
+            "angularVelocity": -6.61889670232267e-14,
+            "velocityX": 0.000001860940720346917,
+            "velocityY": -3.782940575087075,
+            "timestamp": 0.6935174946771088
+        },
+        {
+            "x": 7.680001822997413,
+            "y": 5.14939539072491,
+            "heading": 3.1400002103483726,
+            "angularVelocity": -8.462091393548897e-14,
+            "velocityX": 0.000001055538133316693,
+            "velocityY": -3.7829405750872986,
+            "timestamp": 0.7925914224881243
+        },
+        {
+            "x": 7.6800018440805555,
+            "y": 4.774604609275343,
+            "heading": 3.140000210348366,
+            "angularVelocity": -6.932568582141466e-14,
+            "velocityX": 2.128020680091996e-7,
+            "velocityY": -3.782940575087379,
+            "timestamp": 0.8916653502991398
+        },
+        {
+            "x": 7.680001779518267,
+            "y": 4.39981382782578,
+            "heading": 3.1400002103483597,
+            "angularVelocity": -6.86156871514602e-14,
+            "velocityX": -6.516576918751784e-7,
+            "velocityY": -3.782940575087321,
+            "timestamp": 0.9907392781101554
+        },
+        {
+            "x": 7.680001628799847,
+            "y": 4.025023046376237,
+            "heading": 3.140000210348353,
+            "angularVelocity": -7.135901447410535e-14,
+            "velocityX": -0.0000015212721994966025,
+            "velocityY": -3.782940575087123,
+            "timestamp": 1.089813205921171
+        },
+        {
+            "x": 7.680001391869683,
+            "y": 3.6502322649267236,
+            "heading": 3.140000210348346,
+            "angularVelocity": -7.415296718915011e-14,
+            "velocityX": -0.0000023914480342122818,
+            "velocityY": -3.7829405750867786,
+            "timestamp": 1.1888871337321865
+        },
+        {
+            "x": 7.680001068770943,
+            "y": 3.27544148347729,
+            "heading": 3.1400002103483433,
+            "angularVelocity": -3.3132922253880277e-14,
+            "velocityX": -0.0000032611884784431762,
+            "velocityY": -3.782940575085905,
+            "timestamp": 1.287961061543202
+        },
+        {
+            "x": 7.680000658927351,
+            "y": 2.9480649081437575,
+            "heading": 3.140000122293031,
+            "angularVelocity": -8.887836013747625e-7,
+            "velocityX": -0.000004136744686858471,
+            "velocityY": -3.3043665731916545,
+            "timestamp": 1.3870349893542175
+        },
+        {
+            "x": 7.680000338206625,
+            "y": 2.7025324587418944,
+            "heading": 3.140000057972934,
+            "angularVelocity": -6.492129951120484e-7,
+            "velocityX": -0.0000032371856464875894,
+            "velocityY": -2.4782751105841743,
+            "timestamp": 1.486108917165233
+        },
+        {
+            "x": 7.680000115622722,
+            "y": 2.5388441539517457,
+            "heading": 3.140000017927834,
+            "angularVelocity": -4.0419405265238827e-7,
+            "velocityX": -0.000002246644408273398,
+            "velocityY": -1.6521834594301972,
+            "timestamp": 1.5851828449762486
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -1.8095406608446286e-7,
+            "velocityX": -0.0000011670347327659162,
+            "velocityY": -0.8260917454273138,
+            "timestamp": 1.6842567727872642
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -2.451701531139976e-31,
+            "velocityX": 0,
+            "velocityY": -1.4987552972984166e-37,
+            "timestamp": 1.7833307005982797
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.traj
new file mode 100644
index 00000000..c222ae0b
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.traj	
@@ -0,0 +1,175 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 1.0688205894738666e-32,
+            "velocityX": 4.722903296583099e-32,
+            "velocityY": -7.052966104933725e-38,
+            "timestamp": 0
+        },
+        {
+            "x": 7.680000100019751,
+            "y": 7.385155846048263,
+            "heading": 3.1400000209558985,
+            "angularVelocity": 2.1151775994912985e-7,
+            "velocityX": 0.0000010095466114596713,
+            "velocityY": -0.8260917454272356,
+            "timestamp": 0.09907392781101554
+        },
+        {
+            "x": 7.680000295060445,
+            "y": 7.221467541258139,
+            "heading": 3.1400000632436256,
+            "angularVelocity": 4.2682995289468236e-7,
+            "velocityX": 0.000001968637858711507,
+            "velocityY": -1.6521834594299558,
+            "timestamp": 0.19814785562203108
+        },
+        {
+            "x": 7.680000579860811,
+            "y": 6.975935091856332,
+            "heading": 3.140000126701719,
+            "angularVelocity": 6.405123969521912e-7,
+            "velocityX": 0.0000028746245916429767,
+            "velocityY": -2.4782751105836276,
+            "timestamp": 0.2972217834330466
+        },
+        {
+            "x": 7.680000948789776,
+            "y": 6.648558516522933,
+            "heading": 3.140000210348385,
+            "angularVelocity": 8.442850564566377e-7,
+            "velocityX": 0.0000037237742032736077,
+            "velocityY": -3.3043665731903222,
+            "timestamp": 0.39629571124406215
+        },
+        {
+            "x": 7.68000127512063,
+            "y": 6.273767735073521,
+            "heading": 3.1400002103483864,
+            "angularVelocity": 1.8797601612350415e-14,
+            "velocityX": 0.0000032938116008237123,
+            "velocityY": -3.782940575085686,
+            "timestamp": 0.4953696390550777
+        },
+        {
+            "x": 7.680001534050369,
+            "y": 5.898976953624012,
+            "heading": 3.140000210348386,
+            "angularVelocity": -4.168435621307512e-16,
+            "velocityX": 0.0000026135000838056266,
+            "velocityY": -3.782940575086706,
+            "timestamp": 0.5944435668660932
+        },
+        {
+            "x": 7.680001718421092,
+            "y": 5.524186172174471,
+            "heading": 3.14000021034838,
+            "angularVelocity": -6.61889670232267e-14,
+            "velocityX": 0.000001860940720346917,
+            "velocityY": -3.782940575087075,
+            "timestamp": 0.6935174946771088
+        },
+        {
+            "x": 7.680001822997413,
+            "y": 5.14939539072491,
+            "heading": 3.1400002103483726,
+            "angularVelocity": -8.462091393548897e-14,
+            "velocityX": 0.000001055538133316693,
+            "velocityY": -3.7829405750872986,
+            "timestamp": 0.7925914224881243
+        },
+        {
+            "x": 7.6800018440805555,
+            "y": 4.774604609275343,
+            "heading": 3.140000210348366,
+            "angularVelocity": -6.932568582141466e-14,
+            "velocityX": 2.128020680091996e-7,
+            "velocityY": -3.782940575087379,
+            "timestamp": 0.8916653502991398
+        },
+        {
+            "x": 7.680001779518267,
+            "y": 4.39981382782578,
+            "heading": 3.1400002103483597,
+            "angularVelocity": -6.86156871514602e-14,
+            "velocityX": -6.516576918751784e-7,
+            "velocityY": -3.782940575087321,
+            "timestamp": 0.9907392781101554
+        },
+        {
+            "x": 7.680001628799847,
+            "y": 4.025023046376237,
+            "heading": 3.140000210348353,
+            "angularVelocity": -7.135901447410535e-14,
+            "velocityX": -0.0000015212721994966025,
+            "velocityY": -3.782940575087123,
+            "timestamp": 1.089813205921171
+        },
+        {
+            "x": 7.680001391869683,
+            "y": 3.6502322649267236,
+            "heading": 3.140000210348346,
+            "angularVelocity": -7.415296718915011e-14,
+            "velocityX": -0.0000023914480342122818,
+            "velocityY": -3.7829405750867786,
+            "timestamp": 1.1888871337321865
+        },
+        {
+            "x": 7.680001068770943,
+            "y": 3.27544148347729,
+            "heading": 3.1400002103483433,
+            "angularVelocity": -3.3132922253880277e-14,
+            "velocityX": -0.0000032611884784431762,
+            "velocityY": -3.782940575085905,
+            "timestamp": 1.287961061543202
+        },
+        {
+            "x": 7.680000658927351,
+            "y": 2.9480649081437575,
+            "heading": 3.140000122293031,
+            "angularVelocity": -8.887836013747625e-7,
+            "velocityX": -0.000004136744686858471,
+            "velocityY": -3.3043665731916545,
+            "timestamp": 1.3870349893542175
+        },
+        {
+            "x": 7.680000338206625,
+            "y": 2.7025324587418944,
+            "heading": 3.140000057972934,
+            "angularVelocity": -6.492129951120484e-7,
+            "velocityX": -0.0000032371856464875894,
+            "velocityY": -2.4782751105841743,
+            "timestamp": 1.486108917165233
+        },
+        {
+            "x": 7.680000115622722,
+            "y": 2.5388441539517457,
+            "heading": 3.140000017927834,
+            "angularVelocity": -4.0419405265238827e-7,
+            "velocityX": -0.000002246644408273398,
+            "velocityY": -1.6521834594301972,
+            "timestamp": 1.5851828449762486
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -1.8095406608446286e-7,
+            "velocityX": -0.0000011670347327659162,
+            "velocityY": -0.8260917454273138,
+            "timestamp": 1.6842567727872642
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -2.451701531139976e-31,
+            "velocityX": 0,
+            "velocityY": -1.4987552972984166e-37,
+            "timestamp": 1.7833307005982797
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.1.traj
new file mode 100644
index 00000000..eb7ef7de
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.1.traj	
@@ -0,0 +1,220 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 1.8987898957165807e-31,
+            "velocityX": -1.39380671571913e-32,
+            "velocityY": -1.373126916943459e-36,
+            "timestamp": 0
+        },
+        {
+            "x": 7.680000133861116,
+            "y": 7.389044729512878,
+            "heading": 3.1400000003198203,
+            "angularVelocity": 3.307649058744193e-9,
+            "velocityX": 0.000001384414398977881,
+            "velocityY": -0.8062267267090539,
+            "timestamp": 0.0966914986871212
+        },
+        {
+            "x": 7.6800003968709785,
+            "y": 7.233134191633238,
+            "heading": 3.1400000013587985,
+            "angularVelocity": 1.0745310107378493e-8,
+            "velocityX": 0.0000027200926202754936,
+            "velocityY": -1.6124534214132102,
+            "timestamp": 0.1933829973742424
+        },
+        {
+            "x": 7.680000784081611,
+            "y": 6.9992683925502845,
+            "heading": 3.1400000030373074,
+            "angularVelocity": 1.735946006644181e-8,
+            "velocityX": 0.0000040045979798072784,
+            "velocityY": -2.4186800521075664,
+            "timestamp": 0.2900744960613636
+        },
+        {
+            "x": 7.680001290213695,
+            "y": 6.687447350831601,
+            "heading": 3.1400000045486194,
+            "angularVelocity": 1.5630280874799268e-8,
+            "velocityX": 0.000005234503549587981,
+            "velocityY": -3.2249064907728675,
+            "timestamp": 0.3867659947484848
+        },
+        {
+            "x": 7.680001791921761,
+            "y": 6.32166915785587,
+            "heading": 3.140000004548618,
+            "angularVelocity": -2.2141915496959812e-14,
+            "velocityX": 0.0000051887503514693284,
+            "velocityY": -3.782940568118399,
+            "timestamp": 0.48345749343560596
+        },
+        {
+            "x": 7.6800022292578305,
+            "y": 5.955890964879955,
+            "heading": 3.1400000045486127,
+            "angularVelocity": -6.389600617895948e-14,
+            "velocityX": 0.000004523003602297052,
+            "velocityY": -3.782940568120494,
+            "timestamp": 0.5801489921227272
+        },
+        {
+            "x": 7.680002595183966,
+            "y": 5.590112771903998,
+            "heading": 3.140000004548604,
+            "angularVelocity": -1.1343594252599792e-13,
+            "velocityX": 0.0000037844699136287606,
+            "velocityY": -3.782940568121032,
+            "timestamp": 0.6768404908098484
+        },
+        {
+            "x": 7.680002884490285,
+            "y": 5.224334578928017,
+            "heading": 3.140000004548593,
+            "angularVelocity": -1.3664646403255707e-13,
+            "velocityX": 0.0000029920547840648683,
+            "velocityY": -3.7829405681213992,
+            "timestamp": 0.7735319894969697
+        },
+        {
+            "x": 7.680003093405051,
+            "y": 4.858556385952021,
+            "heading": 3.140000004548582,
+            "angularVelocity": -1.389367975129708e-13,
+            "velocityX": 0.0000021606317619867176,
+            "velocityY": -3.782940568121637,
+            "timestamp": 0.8702234881840909
+        },
+        {
+            "x": 7.6800032195393335,
+            "y": 4.492778192976016,
+            "heading": 3.1400000045485705,
+            "angularVelocity": -1.422935768803712e-13,
+            "velocityX": 0.0000013045020379807613,
+            "velocityY": -3.782940568121783,
+            "timestamp": 0.9669149868712121
+        },
+        {
+            "x": 7.680003262132589,
+            "y": 4.127000000000006,
+            "heading": 3.1400000045485568,
+            "angularVelocity": -1.5798635695024144e-13,
+            "velocityX": 4.405066330821885e-7,
+            "velocityY": -3.782940568121863,
+            "timestamp": 1.0636064855583334
+        },
+        {
+            "x": 7.680003221112365,
+            "y": 3.7612218070239956,
+            "heading": 3.1400000045485426,
+            "angularVelocity": -1.6204973501388964e-13,
+            "velocityX": -4.2423808567477835e-7,
+            "velocityY": -3.782940568121861,
+            "timestamp": 1.1602979842454546
+        },
+        {
+            "x": 7.680003096442586,
+            "y": 3.3954436140479913,
+            "heading": 3.14000000454853,
+            "angularVelocity": -1.5384845387063427e-13,
+            "velocityX": -0.0000012893559013190512,
+            "velocityY": -3.782940568121779,
+            "timestamp": 1.2569894829325758
+        },
+        {
+            "x": 7.680002888734547,
+            "y": 3.029665421071995,
+            "heading": 3.140000004548518,
+            "angularVelocity": -1.4981778939731422e-13,
+            "velocityX": -0.0000021481515920122195,
+            "velocityY": -3.7829405681216324,
+            "timestamp": 1.353680981619697
+        },
+        {
+            "x": 7.680002600247316,
+            "y": 2.663887228096014,
+            "heading": 3.1400000045485066,
+            "angularVelocity": -1.4657528609032921e-13,
+            "velocityX": -0.0000029835836472893606,
+            "velocityY": -3.782940568121397,
+            "timestamp": 1.4503724803068183
+        },
+        {
+            "x": 7.680002234659543,
+            "y": 2.2981090351200577,
+            "heading": 3.140000004548497,
+            "angularVelocity": -1.235449081893301e-13,
+            "velocityX": -0.000003780970508619144,
+            "velocityY": -3.782940568121035,
+            "timestamp": 1.5470639789939395
+        },
+        {
+            "x": 7.680001797111122,
+            "y": 1.9323308421441414,
+            "heading": 3.1400000045484906,
+            "angularVelocity": -7.160301715418214e-14,
+            "velocityX": -0.0000045251997748623165,
+            "velocityY": -3.7829405681205035,
+            "timestamp": 1.6437554776810608
+        },
+        {
+            "x": 7.6800012945835014,
+            "y": 1.566552649168411,
+            "heading": 3.140000004548489,
+            "angularVelocity": -2.2663051081772477e-14,
+            "velocityX": -0.000005197226332013039,
+            "velocityY": -3.7829405681183954,
+            "timestamp": 1.740446976368182
+        },
+        {
+            "x": 7.680000786831981,
+            "y": 1.2547316074497201,
+            "heading": 3.140000001978014,
+            "angularVelocity": -2.6584233523758634e-8,
+            "velocityX": -0.0000052512520234957685,
+            "velocityY": -3.2249064907729323,
+            "timestamp": 1.8371384750553033
+        },
+        {
+            "x": 7.680000398309784,
+            "y": 1.0208658083667639,
+            "heading": 3.140000000933386,
+            "angularVelocity": -1.0803673621572709e-8,
+            "velocityX": -0.000004018162386759126,
+            "velocityY": -2.4186800521075957,
+            "timestamp": 1.9338299737424245
+        },
+        {
+            "x": 7.680000134361733,
+            "y": 0.8649552704871222,
+            "heading": 3.1400000003740773,
+            "angularVelocity": -5.784435922761806e-9,
+            "velocityX": -0.0000027297955176854786,
+            "velocityY": -1.612453421413225,
+            "timestamp": 2.0305214724295455
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": -3.8687549359618056e-9,
+            "velocityX": -0.000001389591862211489,
+            "velocityY": -0.8062267267090597,
+            "timestamp": 2.1272129711166667
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 5.6019143334006085e-31,
+            "velocityX": 0,
+            "velocityY": 9.268974451742519e-38,
+            "timestamp": 2.223904469803788
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.traj
new file mode 100644
index 00000000..eb7ef7de
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.traj	
@@ -0,0 +1,220 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 1.8987898957165807e-31,
+            "velocityX": -1.39380671571913e-32,
+            "velocityY": -1.373126916943459e-36,
+            "timestamp": 0
+        },
+        {
+            "x": 7.680000133861116,
+            "y": 7.389044729512878,
+            "heading": 3.1400000003198203,
+            "angularVelocity": 3.307649058744193e-9,
+            "velocityX": 0.000001384414398977881,
+            "velocityY": -0.8062267267090539,
+            "timestamp": 0.0966914986871212
+        },
+        {
+            "x": 7.6800003968709785,
+            "y": 7.233134191633238,
+            "heading": 3.1400000013587985,
+            "angularVelocity": 1.0745310107378493e-8,
+            "velocityX": 0.0000027200926202754936,
+            "velocityY": -1.6124534214132102,
+            "timestamp": 0.1933829973742424
+        },
+        {
+            "x": 7.680000784081611,
+            "y": 6.9992683925502845,
+            "heading": 3.1400000030373074,
+            "angularVelocity": 1.735946006644181e-8,
+            "velocityX": 0.0000040045979798072784,
+            "velocityY": -2.4186800521075664,
+            "timestamp": 0.2900744960613636
+        },
+        {
+            "x": 7.680001290213695,
+            "y": 6.687447350831601,
+            "heading": 3.1400000045486194,
+            "angularVelocity": 1.5630280874799268e-8,
+            "velocityX": 0.000005234503549587981,
+            "velocityY": -3.2249064907728675,
+            "timestamp": 0.3867659947484848
+        },
+        {
+            "x": 7.680001791921761,
+            "y": 6.32166915785587,
+            "heading": 3.140000004548618,
+            "angularVelocity": -2.2141915496959812e-14,
+            "velocityX": 0.0000051887503514693284,
+            "velocityY": -3.782940568118399,
+            "timestamp": 0.48345749343560596
+        },
+        {
+            "x": 7.6800022292578305,
+            "y": 5.955890964879955,
+            "heading": 3.1400000045486127,
+            "angularVelocity": -6.389600617895948e-14,
+            "velocityX": 0.000004523003602297052,
+            "velocityY": -3.782940568120494,
+            "timestamp": 0.5801489921227272
+        },
+        {
+            "x": 7.680002595183966,
+            "y": 5.590112771903998,
+            "heading": 3.140000004548604,
+            "angularVelocity": -1.1343594252599792e-13,
+            "velocityX": 0.0000037844699136287606,
+            "velocityY": -3.782940568121032,
+            "timestamp": 0.6768404908098484
+        },
+        {
+            "x": 7.680002884490285,
+            "y": 5.224334578928017,
+            "heading": 3.140000004548593,
+            "angularVelocity": -1.3664646403255707e-13,
+            "velocityX": 0.0000029920547840648683,
+            "velocityY": -3.7829405681213992,
+            "timestamp": 0.7735319894969697
+        },
+        {
+            "x": 7.680003093405051,
+            "y": 4.858556385952021,
+            "heading": 3.140000004548582,
+            "angularVelocity": -1.389367975129708e-13,
+            "velocityX": 0.0000021606317619867176,
+            "velocityY": -3.782940568121637,
+            "timestamp": 0.8702234881840909
+        },
+        {
+            "x": 7.6800032195393335,
+            "y": 4.492778192976016,
+            "heading": 3.1400000045485705,
+            "angularVelocity": -1.422935768803712e-13,
+            "velocityX": 0.0000013045020379807613,
+            "velocityY": -3.782940568121783,
+            "timestamp": 0.9669149868712121
+        },
+        {
+            "x": 7.680003262132589,
+            "y": 4.127000000000006,
+            "heading": 3.1400000045485568,
+            "angularVelocity": -1.5798635695024144e-13,
+            "velocityX": 4.405066330821885e-7,
+            "velocityY": -3.782940568121863,
+            "timestamp": 1.0636064855583334
+        },
+        {
+            "x": 7.680003221112365,
+            "y": 3.7612218070239956,
+            "heading": 3.1400000045485426,
+            "angularVelocity": -1.6204973501388964e-13,
+            "velocityX": -4.2423808567477835e-7,
+            "velocityY": -3.782940568121861,
+            "timestamp": 1.1602979842454546
+        },
+        {
+            "x": 7.680003096442586,
+            "y": 3.3954436140479913,
+            "heading": 3.14000000454853,
+            "angularVelocity": -1.5384845387063427e-13,
+            "velocityX": -0.0000012893559013190512,
+            "velocityY": -3.782940568121779,
+            "timestamp": 1.2569894829325758
+        },
+        {
+            "x": 7.680002888734547,
+            "y": 3.029665421071995,
+            "heading": 3.140000004548518,
+            "angularVelocity": -1.4981778939731422e-13,
+            "velocityX": -0.0000021481515920122195,
+            "velocityY": -3.7829405681216324,
+            "timestamp": 1.353680981619697
+        },
+        {
+            "x": 7.680002600247316,
+            "y": 2.663887228096014,
+            "heading": 3.1400000045485066,
+            "angularVelocity": -1.4657528609032921e-13,
+            "velocityX": -0.0000029835836472893606,
+            "velocityY": -3.782940568121397,
+            "timestamp": 1.4503724803068183
+        },
+        {
+            "x": 7.680002234659543,
+            "y": 2.2981090351200577,
+            "heading": 3.140000004548497,
+            "angularVelocity": -1.235449081893301e-13,
+            "velocityX": -0.000003780970508619144,
+            "velocityY": -3.782940568121035,
+            "timestamp": 1.5470639789939395
+        },
+        {
+            "x": 7.680001797111122,
+            "y": 1.9323308421441414,
+            "heading": 3.1400000045484906,
+            "angularVelocity": -7.160301715418214e-14,
+            "velocityX": -0.0000045251997748623165,
+            "velocityY": -3.7829405681205035,
+            "timestamp": 1.6437554776810608
+        },
+        {
+            "x": 7.6800012945835014,
+            "y": 1.566552649168411,
+            "heading": 3.140000004548489,
+            "angularVelocity": -2.2663051081772477e-14,
+            "velocityX": -0.000005197226332013039,
+            "velocityY": -3.7829405681183954,
+            "timestamp": 1.740446976368182
+        },
+        {
+            "x": 7.680000786831981,
+            "y": 1.2547316074497201,
+            "heading": 3.140000001978014,
+            "angularVelocity": -2.6584233523758634e-8,
+            "velocityX": -0.0000052512520234957685,
+            "velocityY": -3.2249064907729323,
+            "timestamp": 1.8371384750553033
+        },
+        {
+            "x": 7.680000398309784,
+            "y": 1.0208658083667639,
+            "heading": 3.140000000933386,
+            "angularVelocity": -1.0803673621572709e-8,
+            "velocityX": -0.000004018162386759126,
+            "velocityY": -2.4186800521075957,
+            "timestamp": 1.9338299737424245
+        },
+        {
+            "x": 7.680000134361733,
+            "y": 0.8649552704871222,
+            "heading": 3.1400000003740773,
+            "angularVelocity": -5.784435922761806e-9,
+            "velocityX": -0.0000027297955176854786,
+            "velocityY": -1.612453421413225,
+            "timestamp": 2.0305214724295455
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": -3.8687549359618056e-9,
+            "velocityX": -0.000001389591862211489,
+            "velocityY": -0.8062267267090597,
+            "timestamp": 2.1272129711166667
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 5.6019143334006085e-31,
+            "velocityX": 0,
+            "velocityY": 9.268974451742519e-38,
+            "timestamp": 2.223904469803788
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.1.traj
new file mode 100644
index 00000000..b2a982fb
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": -8.261119414836203e-30,
+            "velocityX": 2.821294333158614e-31,
+            "velocityY": -2.62095350113427e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 7.617797825979087,
+            "y": 7.446201794633709,
+            "heading": 3.1534584721408314,
+            "angularVelocity": 0.15162222909690662,
+            "velocityX": -0.7007654495821862,
+            "velocityY": -0.2343111629647132,
+            "timestamp": 0.08876318611043306
+        },
+        {
+            "x": 7.493368421156802,
+            "y": 7.404602777573554,
+            "heading": 3.178746831508521,
+            "angularVelocity": 0.28489693164482327,
+            "velocityX": -1.4018131871435613,
+            "velocityY": -0.4686516886383534,
+            "timestamp": 0.17752637222086612
+        },
+        {
+            "x": 7.306676117263916,
+            "y": 7.342201100203473,
+            "heading": 3.213107467103745,
+            "angularVelocity": 0.38710457678121596,
+            "velocityX": -2.103262760992579,
+            "velocityY": -0.7030130406946671,
+            "timestamp": 0.2662895583312992
+        },
+        {
+            "x": 7.057674416387051,
+            "y": 7.2590025582832345,
+            "heading": 3.2508668949579036,
+            "angularVelocity": 0.4253951385364178,
+            "velocityX": -2.8052361771613255,
+            "velocityY": -0.9373090981171329,
+            "timestamp": 0.35505274444173224
+        },
+        {
+            "x": 6.74646520076521,
+            "y": 7.155112122369572,
+            "heading": 3.2734870599198675,
+            "angularVelocity": 0.25483723550559106,
+            "velocityX": -3.5060617949738107,
+            "velocityY": -1.1704225644085642,
+            "timestamp": 0.4438159305521653
+        },
+        {
+            "x": 6.4280476263908515,
+            "y": 7.048517625465743,
+            "heading": 3.2734870708664827,
+            "angularVelocity": 1.233247689161178e-7,
+            "velocityX": -3.5872706729821253,
+            "velocityY": -1.200886331076399,
+            "timestamp": 0.5325791166625984
+        },
+        {
+            "x": 6.109630072909297,
+            "y": 6.941923066153303,
+            "heading": 3.273487081813265,
+            "angularVelocity": 1.2332664336181238e-7,
+            "velocityX": -3.5872704376046,
+            "velocityY": -1.2008870341690594,
+            "timestamp": 0.6213423027730315
+        },
+        {
+            "x": 5.798381254717505,
+            "y": 6.838018859230789,
+            "heading": 3.296331970538473,
+            "angularVelocity": 0.25736895809035,
+            "velocityX": -3.5065079548134683,
+            "velocityY": -1.1705777076875172,
+            "timestamp": 0.7101054888834646
+        },
+        {
+            "x": 5.549362929627812,
+            "y": 6.754814274993791,
+            "heading": 3.3329076351356512,
+            "angularVelocity": 0.4120589424672841,
+            "velocityX": -2.8054234643993468,
+            "velocityY": -0.9373771704727861,
+            "timestamp": 0.7988686749938977
+        },
+        {
+            "x": 5.36265371808689,
+            "y": 6.692406142204076,
+            "heading": 3.3659874710813016,
+            "angularVelocity": 0.37267517533582245,
+            "velocityX": -2.103453241394186,
+            "velocityY": -0.7030857670337234,
+            "timestamp": 0.8876318611043308
+        },
+        {
+            "x": 5.238210282238424,
+            "y": 6.650801541740841,
+            "heading": 3.3902750965399324,
+            "angularVelocity": 0.27362273171986246,
+            "velocityX": -1.4019712597217382,
+            "velocityY": -0.4687145908921929,
+            "timestamp": 0.9763950472147639
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.14544555244060076,
+            "velocityX": -0.7008567962045305,
+            "velocityY": -0.2343487503412182,
+            "timestamp": 1.065158233325197
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0,
+            "velocityX": 1.430834166779729e-32,
+            "velocityY": -6.582148241954714e-30,
+            "timestamp": 1.15392141943563
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.traj
new file mode 100644
index 00000000..b2a982fb
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": -8.261119414836203e-30,
+            "velocityX": 2.821294333158614e-31,
+            "velocityY": -2.62095350113427e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 7.617797825979087,
+            "y": 7.446201794633709,
+            "heading": 3.1534584721408314,
+            "angularVelocity": 0.15162222909690662,
+            "velocityX": -0.7007654495821862,
+            "velocityY": -0.2343111629647132,
+            "timestamp": 0.08876318611043306
+        },
+        {
+            "x": 7.493368421156802,
+            "y": 7.404602777573554,
+            "heading": 3.178746831508521,
+            "angularVelocity": 0.28489693164482327,
+            "velocityX": -1.4018131871435613,
+            "velocityY": -0.4686516886383534,
+            "timestamp": 0.17752637222086612
+        },
+        {
+            "x": 7.306676117263916,
+            "y": 7.342201100203473,
+            "heading": 3.213107467103745,
+            "angularVelocity": 0.38710457678121596,
+            "velocityX": -2.103262760992579,
+            "velocityY": -0.7030130406946671,
+            "timestamp": 0.2662895583312992
+        },
+        {
+            "x": 7.057674416387051,
+            "y": 7.2590025582832345,
+            "heading": 3.2508668949579036,
+            "angularVelocity": 0.4253951385364178,
+            "velocityX": -2.8052361771613255,
+            "velocityY": -0.9373090981171329,
+            "timestamp": 0.35505274444173224
+        },
+        {
+            "x": 6.74646520076521,
+            "y": 7.155112122369572,
+            "heading": 3.2734870599198675,
+            "angularVelocity": 0.25483723550559106,
+            "velocityX": -3.5060617949738107,
+            "velocityY": -1.1704225644085642,
+            "timestamp": 0.4438159305521653
+        },
+        {
+            "x": 6.4280476263908515,
+            "y": 7.048517625465743,
+            "heading": 3.2734870708664827,
+            "angularVelocity": 1.233247689161178e-7,
+            "velocityX": -3.5872706729821253,
+            "velocityY": -1.200886331076399,
+            "timestamp": 0.5325791166625984
+        },
+        {
+            "x": 6.109630072909297,
+            "y": 6.941923066153303,
+            "heading": 3.273487081813265,
+            "angularVelocity": 1.2332664336181238e-7,
+            "velocityX": -3.5872704376046,
+            "velocityY": -1.2008870341690594,
+            "timestamp": 0.6213423027730315
+        },
+        {
+            "x": 5.798381254717505,
+            "y": 6.838018859230789,
+            "heading": 3.296331970538473,
+            "angularVelocity": 0.25736895809035,
+            "velocityX": -3.5065079548134683,
+            "velocityY": -1.1705777076875172,
+            "timestamp": 0.7101054888834646
+        },
+        {
+            "x": 5.549362929627812,
+            "y": 6.754814274993791,
+            "heading": 3.3329076351356512,
+            "angularVelocity": 0.4120589424672841,
+            "velocityX": -2.8054234643993468,
+            "velocityY": -0.9373771704727861,
+            "timestamp": 0.7988686749938977
+        },
+        {
+            "x": 5.36265371808689,
+            "y": 6.692406142204076,
+            "heading": 3.3659874710813016,
+            "angularVelocity": 0.37267517533582245,
+            "velocityX": -2.103453241394186,
+            "velocityY": -0.7030857670337234,
+            "timestamp": 0.8876318611043308
+        },
+        {
+            "x": 5.238210282238424,
+            "y": 6.650801541740841,
+            "heading": 3.3902750965399324,
+            "angularVelocity": 0.27362273171986246,
+            "velocityX": -1.4019712597217382,
+            "velocityY": -0.4687145908921929,
+            "timestamp": 0.9763950472147639
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.14544555244060076,
+            "velocityX": -0.7008567962045305,
+            "velocityY": -0.2343487503412182,
+            "timestamp": 1.065158233325197
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0,
+            "velocityX": 1.430834166779729e-32,
+            "velocityY": -6.582148241954714e-30,
+            "timestamp": 1.15392141943563
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.1.traj
new file mode 100644
index 00000000..c20ce3b6
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": -3.436405776355784e-29,
+            "velocityX": 4.0035032875632384e-30,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.593933573152452,
+            "y": 7.445362365518837,
+            "heading": 3.094092714332116,
+            "angularVelocity": -0.44270408674328476,
+            "velocityX": -0.8299762955364275,
+            "velocityY": -0.2086611977356942,
+            "timestamp": 0.10369745173495665
+        },
+        {
+            "x": 7.421555540437275,
+            "y": 7.402033887236261,
+            "heading": 3.009154332069554,
+            "angularVelocity": -0.8190980669386346,
+            "velocityX": -1.6623169598782952,
+            "velocityY": -0.41783551627973,
+            "timestamp": 0.2073949034699133
+        },
+        {
+            "x": 7.1624840188793355,
+            "y": 7.336952212665302,
+            "heading": 2.898930851860923,
+            "angularVelocity": -1.0629333543342216,
+            "velocityX": -2.4983402892108755,
+            "velocityY": -0.6276111271981535,
+            "timestamp": 0.31109235520486994
+        },
+        {
+            "x": 6.816593659112812,
+            "y": 7.250270000971659,
+            "heading": 2.8098268582890107,
+            "angularVelocity": -0.8592688834789594,
+            "velocityX": -3.335572417445675,
+            "velocityY": -0.8359145788384311,
+            "timestamp": 0.4147898069398266
+        },
+        {
+            "x": 6.436271393844499,
+            "y": 7.15414721538473,
+            "heading": 2.8098268314636434,
+            "angularVelocity": -2.586887820849056e-7,
+            "velocityX": -3.6676143811169655,
+            "velocityY": -0.9269541727276405,
+            "timestamp": 0.5184872586747833
+        },
+        {
+            "x": 6.055949159763292,
+            "y": 7.058024306401983,
+            "heading": 2.8098268046376322,
+            "angularVelocity": -2.5869499105961153e-7,
+            "velocityX": -3.6676140803660213,
+            "velocityY": -0.9269553626876206,
+            "timestamp": 0.62218471040974
+        },
+        {
+            "x": 5.675627085616213,
+            "y": 6.961900764622196,
+            "heading": 2.809826777811619,
+            "angularVelocity": -2.5869501157187357e-7,
+            "velocityX": -3.6676125380511024,
+            "velocityY": -0.9269614650269816,
+            "timestamp": 0.7258821621446967
+        },
+        {
+            "x": 5.306947068096014,
+            "y": 6.827887110574328,
+            "heading": 2.8098267501768928,
+            "angularVelocity": -2.6649378350721345e-7,
+            "velocityX": -3.555343080778155,
+            "velocityY": -1.2923524330219798,
+            "timestamp": 0.8295796138796534
+        },
+        {
+            "x": 4.978327022444636,
+            "y": 6.61365948285698,
+            "heading": 2.809826716038805,
+            "angularVelocity": -3.2920855048251957e-7,
+            "velocityX": -3.1690272051362296,
+            "velocityY": -2.065890956172196,
+            "timestamp": 0.9332770656146101
+        },
+        {
+            "x": 4.706934591498499,
+            "y": 6.330409136170975,
+            "heading": 2.809826716485968,
+            "angularVelocity": 4.312190502568527e-9,
+            "velocityX": -2.617156221348633,
+            "velocityY": -2.731507302705652,
+            "timestamp": 1.0369745173495668
+        },
+        {
+            "x": 4.519067005656564,
+            "y": 6.028361651029434,
+            "heading": 2.9086869877879162,
+            "angularVelocity": 0.9533529479069416,
+            "velocityX": -1.8116895130858792,
+            "velocityY": -2.9127763516651948,
+            "timestamp": 1.1406719690845235
+        },
+        {
+            "x": 4.388194794926867,
+            "y": 5.795490351049463,
+            "heading": 3.0157821623071808,
+            "angularVelocity": 1.0327657307624416,
+            "velocityX": -1.2620581175340149,
+            "velocityY": -2.245680063336305,
+            "timestamp": 1.2443694208194802
+        },
+        {
+            "x": 4.304089821797064,
+            "y": 5.638764448430372,
+            "heading": 3.096687373599028,
+            "angularVelocity": 0.7802044306606039,
+            "velocityX": -0.8110611372086162,
+            "velocityY": -1.5113766056631632,
+            "timestamp": 1.348066872554437
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0.41768264963403856,
+            "velocityX": -0.39624717010474564,
+            "velocityY": -0.7595601156303902,
+            "timestamp": 1.4517643242893936
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 1.3748988870988578e-26,
+            "velocityX": 7.122460517165917e-28,
+            "velocityY": 4.07656513453054e-28,
+            "timestamp": 1.5554617760243503
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.traj
new file mode 100644
index 00000000..c20ce3b6
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": -3.436405776355784e-29,
+            "velocityX": 4.0035032875632384e-30,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.593933573152452,
+            "y": 7.445362365518837,
+            "heading": 3.094092714332116,
+            "angularVelocity": -0.44270408674328476,
+            "velocityX": -0.8299762955364275,
+            "velocityY": -0.2086611977356942,
+            "timestamp": 0.10369745173495665
+        },
+        {
+            "x": 7.421555540437275,
+            "y": 7.402033887236261,
+            "heading": 3.009154332069554,
+            "angularVelocity": -0.8190980669386346,
+            "velocityX": -1.6623169598782952,
+            "velocityY": -0.41783551627973,
+            "timestamp": 0.2073949034699133
+        },
+        {
+            "x": 7.1624840188793355,
+            "y": 7.336952212665302,
+            "heading": 2.898930851860923,
+            "angularVelocity": -1.0629333543342216,
+            "velocityX": -2.4983402892108755,
+            "velocityY": -0.6276111271981535,
+            "timestamp": 0.31109235520486994
+        },
+        {
+            "x": 6.816593659112812,
+            "y": 7.250270000971659,
+            "heading": 2.8098268582890107,
+            "angularVelocity": -0.8592688834789594,
+            "velocityX": -3.335572417445675,
+            "velocityY": -0.8359145788384311,
+            "timestamp": 0.4147898069398266
+        },
+        {
+            "x": 6.436271393844499,
+            "y": 7.15414721538473,
+            "heading": 2.8098268314636434,
+            "angularVelocity": -2.586887820849056e-7,
+            "velocityX": -3.6676143811169655,
+            "velocityY": -0.9269541727276405,
+            "timestamp": 0.5184872586747833
+        },
+        {
+            "x": 6.055949159763292,
+            "y": 7.058024306401983,
+            "heading": 2.8098268046376322,
+            "angularVelocity": -2.5869499105961153e-7,
+            "velocityX": -3.6676140803660213,
+            "velocityY": -0.9269553626876206,
+            "timestamp": 0.62218471040974
+        },
+        {
+            "x": 5.675627085616213,
+            "y": 6.961900764622196,
+            "heading": 2.809826777811619,
+            "angularVelocity": -2.5869501157187357e-7,
+            "velocityX": -3.6676125380511024,
+            "velocityY": -0.9269614650269816,
+            "timestamp": 0.7258821621446967
+        },
+        {
+            "x": 5.306947068096014,
+            "y": 6.827887110574328,
+            "heading": 2.8098267501768928,
+            "angularVelocity": -2.6649378350721345e-7,
+            "velocityX": -3.555343080778155,
+            "velocityY": -1.2923524330219798,
+            "timestamp": 0.8295796138796534
+        },
+        {
+            "x": 4.978327022444636,
+            "y": 6.61365948285698,
+            "heading": 2.809826716038805,
+            "angularVelocity": -3.2920855048251957e-7,
+            "velocityX": -3.1690272051362296,
+            "velocityY": -2.065890956172196,
+            "timestamp": 0.9332770656146101
+        },
+        {
+            "x": 4.706934591498499,
+            "y": 6.330409136170975,
+            "heading": 2.809826716485968,
+            "angularVelocity": 4.312190502568527e-9,
+            "velocityX": -2.617156221348633,
+            "velocityY": -2.731507302705652,
+            "timestamp": 1.0369745173495668
+        },
+        {
+            "x": 4.519067005656564,
+            "y": 6.028361651029434,
+            "heading": 2.9086869877879162,
+            "angularVelocity": 0.9533529479069416,
+            "velocityX": -1.8116895130858792,
+            "velocityY": -2.9127763516651948,
+            "timestamp": 1.1406719690845235
+        },
+        {
+            "x": 4.388194794926867,
+            "y": 5.795490351049463,
+            "heading": 3.0157821623071808,
+            "angularVelocity": 1.0327657307624416,
+            "velocityX": -1.2620581175340149,
+            "velocityY": -2.245680063336305,
+            "timestamp": 1.2443694208194802
+        },
+        {
+            "x": 4.304089821797064,
+            "y": 5.638764448430372,
+            "heading": 3.096687373599028,
+            "angularVelocity": 0.7802044306606039,
+            "velocityX": -0.8110611372086162,
+            "velocityY": -1.5113766056631632,
+            "timestamp": 1.348066872554437
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0.41768264963403856,
+            "velocityX": -0.39624717010474564,
+            "velocityY": -0.7595601156303902,
+            "timestamp": 1.4517643242893936
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 1.3748988870988578e-26,
+            "velocityX": 7.122460517165917e-28,
+            "velocityY": 4.07656513453054e-28,
+            "timestamp": 1.5554617760243503
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.1.traj
new file mode 100644
index 00000000..60c01c11
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": -3.261859244163768e-33,
+            "velocityX": 7.471195721597621e-34,
+            "velocityY": -1.6825458997868564e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6132701032534955,
+            "y": 7.458186361668427,
+            "heading": 3.162690215100173,
+            "angularVelocity": 0.25198683492477825,
+            "velocityX": -0.7410707832329344,
+            "velocityY": -0.09788011341186789,
+            "timestamp": 0.09004524028783478
+        },
+        {
+            "x": 7.4797323779606675,
+            "y": 7.440537712219312,
+            "heading": 3.2047640757495373,
+            "angularVelocity": 0.46725246681415944,
+            "velocityX": -1.4830070403051525,
+            "velocityY": -0.19599758291165675,
+            "timestamp": 0.18009048057566956
+        },
+        {
+            "x": 7.27928179650239,
+            "y": 7.414023208632297,
+            "heading": 3.2605597645105227,
+            "angularVelocity": 0.6196406226762411,
+            "velocityX": -2.226109684615486,
+            "velocityY": -0.29445758045912396,
+            "timestamp": 0.27013572086350435
+        },
+        {
+            "x": 7.011816476608683,
+            "y": 7.378594826276994,
+            "heading": 3.318124935630623,
+            "angularVelocity": 0.6392916597933375,
+            "velocityX": -2.9703437853987498,
+            "velocityY": -0.3934509169174634,
+            "timestamp": 0.36018096115133913
+        },
+        {
+            "x": 6.67842119537258,
+            "y": 7.334255965416845,
+            "heading": 3.332157072604317,
+            "angularVelocity": 0.15583429983460975,
+            "velocityX": -3.70253086304603,
+            "velocityY": -0.49240649165261274,
+            "timestamp": 0.4502262014391739
+        },
+        {
+            "x": 6.34069723545727,
+            "y": 7.289811993684431,
+            "heading": 3.332157092024104,
+            "angularVelocity": 2.1566700178855977e-7,
+            "velocityX": -3.7506031283358707,
+            "velocityY": -0.4935738034608694,
+            "timestamp": 0.5402714417270087
+        },
+        {
+            "x": 6.002973284663831,
+            "y": 7.245367952636118,
+            "heading": 3.3321571114438866,
+            "angularVelocity": 2.156669548142136e-7,
+            "velocityX": -3.750603027032689,
+            "velocityY": -0.49357457325056325,
+            "timestamp": 0.6303166820148435
+        },
+        {
+            "x": 5.665249344531261,
+            "y": 7.200923830577433,
+            "heading": 3.3321571308636764,
+            "angularVelocity": 2.1566703381132295e-7,
+            "velocityX": -3.750602908638108,
+            "velocityY": -0.4935754729135791,
+            "timestamp": 0.7203619223026783
+        },
+        {
+            "x": 5.32752541585805,
+            "y": 7.156479621440943,
+            "heading": 3.332157150283477,
+            "angularVelocity": 2.1566715012677575e-7,
+            "velocityX": -3.750602781375866,
+            "velocityY": -0.4935764399586428,
+            "timestamp": 0.810407162590513
+        },
+        {
+            "x": 4.9898014989996415,
+            "y": 7.112035322525874,
+            "heading": 3.332157169703281,
+            "angularVelocity": 2.1566719028685347e-7,
+            "velocityX": -3.750602650166234,
+            "velocityY": -0.4935774369972257,
+            "timestamp": 0.9004524028783478
+        },
+        {
+            "x": 4.652077593988678,
+            "y": 7.06759093358437,
+            "heading": 3.33215718912308,
+            "angularVelocity": 2.1566713427464117e-7,
+            "velocityX": -3.750602518594089,
+            "velocityY": -0.49357843678839325,
+            "timestamp": 0.9904976431661826
+        },
+        {
+            "x": 4.314353700505922,
+            "y": 7.023146457042436,
+            "heading": 3.332157208542871,
+            "angularVelocity": 2.156670494902004e-7,
+            "velocityX": -3.7506023905672445,
+            "velocityY": -0.4935794096374718,
+            "timestamp": 1.0805428834540174
+        },
+        {
+            "x": 3.9766298178690414,
+            "y": 6.978701898085155,
+            "heading": 3.3321572279626577,
+            "angularVelocity": 2.1566699709425413e-7,
+            "velocityX": -3.750602270118079,
+            "velocityY": -0.4935803249034738,
+            "timestamp": 1.1705881237418523
+        },
+        {
+            "x": 3.6389059453045984,
+            "y": 6.934257262589842,
+            "heading": 3.332157247382445,
+            "angularVelocity": 2.1566700145288475e-7,
+            "velocityX": -3.7506021582583307,
+            "velocityY": -0.49358117489878034,
+            "timestamp": 1.2606333640296872
+        },
+        {
+            "x": 3.301182082389383,
+            "y": 6.889812553772521,
+            "heading": 3.332157266802242,
+            "angularVelocity": 2.156671157814786e-7,
+            "velocityX": -3.750602051098551,
+            "velocityY": -0.4935819891784481,
+            "timestamp": 1.350678604317522
+        },
+        {
+            "x": 2.9679743508365415,
+            "y": 6.845442796163545,
+            "heading": 3.346601240828058,
+            "angularVelocity": 0.16040796803523288,
+            "velocityX": -3.7004480246565397,
+            "velocityY": -0.4927496163833412,
+            "timestamp": 1.440723844605357
+        },
+        {
+            "x": 2.7005730136886035,
+            "y": 6.810002161458836,
+            "heading": 3.406941333368848,
+            "angularVelocity": 0.670108629261352,
+            "velocityX": -2.969633223179528,
+            "velocityY": -0.393586985735394,
+            "timestamp": 1.5307690848931919
+        },
+        {
+            "x": 2.500185866956939,
+            "y": 6.783477694334395,
+            "heading": 3.465398232800302,
+            "angularVelocity": 0.6491947741445663,
+            "velocityX": -2.2254052084387275,
+            "velocityY": -0.294568230809919,
+            "timestamp": 1.6208143251810267
+        },
+        {
+            "x": 2.366700027158657,
+            "y": 6.765819972243713,
+            "heading": 3.5094432939271947,
+            "angularVelocity": 0.4891436902839078,
+            "velocityX": -1.4824308244565385,
+            "velocityY": -0.19609833939293356,
+            "timestamp": 1.7108595654688616
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 0.26366760948717133,
+            "velocityX": -0.7407390656679514,
+            "velocityY": -0.09795045485490664,
+            "timestamp": 1.8009048057566965
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 1.378321455008778e-32,
+            "velocityX": 0,
+            "velocityY": 4.004611385179353e-34,
+            "timestamp": 1.8909500460445314
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.traj
new file mode 100644
index 00000000..60c01c11
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": -3.261859244163768e-33,
+            "velocityX": 7.471195721597621e-34,
+            "velocityY": -1.6825458997868564e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6132701032534955,
+            "y": 7.458186361668427,
+            "heading": 3.162690215100173,
+            "angularVelocity": 0.25198683492477825,
+            "velocityX": -0.7410707832329344,
+            "velocityY": -0.09788011341186789,
+            "timestamp": 0.09004524028783478
+        },
+        {
+            "x": 7.4797323779606675,
+            "y": 7.440537712219312,
+            "heading": 3.2047640757495373,
+            "angularVelocity": 0.46725246681415944,
+            "velocityX": -1.4830070403051525,
+            "velocityY": -0.19599758291165675,
+            "timestamp": 0.18009048057566956
+        },
+        {
+            "x": 7.27928179650239,
+            "y": 7.414023208632297,
+            "heading": 3.2605597645105227,
+            "angularVelocity": 0.6196406226762411,
+            "velocityX": -2.226109684615486,
+            "velocityY": -0.29445758045912396,
+            "timestamp": 0.27013572086350435
+        },
+        {
+            "x": 7.011816476608683,
+            "y": 7.378594826276994,
+            "heading": 3.318124935630623,
+            "angularVelocity": 0.6392916597933375,
+            "velocityX": -2.9703437853987498,
+            "velocityY": -0.3934509169174634,
+            "timestamp": 0.36018096115133913
+        },
+        {
+            "x": 6.67842119537258,
+            "y": 7.334255965416845,
+            "heading": 3.332157072604317,
+            "angularVelocity": 0.15583429983460975,
+            "velocityX": -3.70253086304603,
+            "velocityY": -0.49240649165261274,
+            "timestamp": 0.4502262014391739
+        },
+        {
+            "x": 6.34069723545727,
+            "y": 7.289811993684431,
+            "heading": 3.332157092024104,
+            "angularVelocity": 2.1566700178855977e-7,
+            "velocityX": -3.7506031283358707,
+            "velocityY": -0.4935738034608694,
+            "timestamp": 0.5402714417270087
+        },
+        {
+            "x": 6.002973284663831,
+            "y": 7.245367952636118,
+            "heading": 3.3321571114438866,
+            "angularVelocity": 2.156669548142136e-7,
+            "velocityX": -3.750603027032689,
+            "velocityY": -0.49357457325056325,
+            "timestamp": 0.6303166820148435
+        },
+        {
+            "x": 5.665249344531261,
+            "y": 7.200923830577433,
+            "heading": 3.3321571308636764,
+            "angularVelocity": 2.1566703381132295e-7,
+            "velocityX": -3.750602908638108,
+            "velocityY": -0.4935754729135791,
+            "timestamp": 0.7203619223026783
+        },
+        {
+            "x": 5.32752541585805,
+            "y": 7.156479621440943,
+            "heading": 3.332157150283477,
+            "angularVelocity": 2.1566715012677575e-7,
+            "velocityX": -3.750602781375866,
+            "velocityY": -0.4935764399586428,
+            "timestamp": 0.810407162590513
+        },
+        {
+            "x": 4.9898014989996415,
+            "y": 7.112035322525874,
+            "heading": 3.332157169703281,
+            "angularVelocity": 2.1566719028685347e-7,
+            "velocityX": -3.750602650166234,
+            "velocityY": -0.4935774369972257,
+            "timestamp": 0.9004524028783478
+        },
+        {
+            "x": 4.652077593988678,
+            "y": 7.06759093358437,
+            "heading": 3.33215718912308,
+            "angularVelocity": 2.1566713427464117e-7,
+            "velocityX": -3.750602518594089,
+            "velocityY": -0.49357843678839325,
+            "timestamp": 0.9904976431661826
+        },
+        {
+            "x": 4.314353700505922,
+            "y": 7.023146457042436,
+            "heading": 3.332157208542871,
+            "angularVelocity": 2.156670494902004e-7,
+            "velocityX": -3.7506023905672445,
+            "velocityY": -0.4935794096374718,
+            "timestamp": 1.0805428834540174
+        },
+        {
+            "x": 3.9766298178690414,
+            "y": 6.978701898085155,
+            "heading": 3.3321572279626577,
+            "angularVelocity": 2.1566699709425413e-7,
+            "velocityX": -3.750602270118079,
+            "velocityY": -0.4935803249034738,
+            "timestamp": 1.1705881237418523
+        },
+        {
+            "x": 3.6389059453045984,
+            "y": 6.934257262589842,
+            "heading": 3.332157247382445,
+            "angularVelocity": 2.1566700145288475e-7,
+            "velocityX": -3.7506021582583307,
+            "velocityY": -0.49358117489878034,
+            "timestamp": 1.2606333640296872
+        },
+        {
+            "x": 3.301182082389383,
+            "y": 6.889812553772521,
+            "heading": 3.332157266802242,
+            "angularVelocity": 2.156671157814786e-7,
+            "velocityX": -3.750602051098551,
+            "velocityY": -0.4935819891784481,
+            "timestamp": 1.350678604317522
+        },
+        {
+            "x": 2.9679743508365415,
+            "y": 6.845442796163545,
+            "heading": 3.346601240828058,
+            "angularVelocity": 0.16040796803523288,
+            "velocityX": -3.7004480246565397,
+            "velocityY": -0.4927496163833412,
+            "timestamp": 1.440723844605357
+        },
+        {
+            "x": 2.7005730136886035,
+            "y": 6.810002161458836,
+            "heading": 3.406941333368848,
+            "angularVelocity": 0.670108629261352,
+            "velocityX": -2.969633223179528,
+            "velocityY": -0.393586985735394,
+            "timestamp": 1.5307690848931919
+        },
+        {
+            "x": 2.500185866956939,
+            "y": 6.783477694334395,
+            "heading": 3.465398232800302,
+            "angularVelocity": 0.6491947741445663,
+            "velocityX": -2.2254052084387275,
+            "velocityY": -0.294568230809919,
+            "timestamp": 1.6208143251810267
+        },
+        {
+            "x": 2.366700027158657,
+            "y": 6.765819972243713,
+            "heading": 3.5094432939271947,
+            "angularVelocity": 0.4891436902839078,
+            "velocityX": -1.4824308244565385,
+            "velocityY": -0.19609833939293356,
+            "timestamp": 1.7108595654688616
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 0.26366760948717133,
+            "velocityX": -0.7407390656679514,
+            "velocityY": -0.09795045485490664,
+            "timestamp": 1.8009048057566965
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 1.378321455008778e-32,
+            "velocityX": 0,
+            "velocityY": 4.004611385179353e-34,
+            "timestamp": 1.8909500460445314
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.1.traj
new file mode 100644
index 00000000..88fa5a42
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.1.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 1.6870437677917242e-24,
+            "velocityY": 1.6813073596046228e-23,
+            "timestamp": 0
+        },
+        {
+            "x": 7.680000027670122,
+            "y": 5.880500005575848,
+            "heading": 3.140000018022814,
+            "angularVelocity": 1.8010003546528393e-7,
+            "velocityX": 2.7650455327562645e-7,
+            "velocityY": 0.8344065481228156,
+            "timestamp": 0.10007112931184436
+        },
+        {
+            "x": 7.680000074983718,
+            "y": 6.047500013629854,
+            "heading": 3.1400000460729856,
+            "angularVelocity": 2.8030233382034975e-7,
+            "velocityX": 4.7279965639225817e-7,
+            "velocityY": 1.6688130652907482,
+            "timestamp": 0.20014225862368873
+        },
+        {
+            "x": 7.680000134184045,
+            "y": 6.298000017966634,
+            "heading": 3.1400000770424787,
+            "angularVelocity": 3.0947480383704467e-7,
+            "velocityX": 5.915824876001831e-7,
+            "velocityY": 2.5032195205488788,
+            "timestamp": 0.3002133879355331
+        },
+        {
+            "x": 7.680000197955803,
+            "y": 6.632000000000009,
+            "heading": 3.1400001051570197,
+            "angularVelocity": 2.8094557567454647e-7,
+            "velocityX": 6.372642912914395e-7,
+            "velocityY": 3.3376257900773374,
+            "timestamp": 0.40028451724737746
+        },
+        {
+            "x": 7.680000129458281,
+            "y": 6.96599998203339,
+            "heading": 3.140000063575334,
+            "angularVelocity": -4.1552129568712905e-7,
+            "velocityX": -6.844883427144649e-7,
+            "velocityY": 3.337625790077381,
+            "timestamp": 0.5003556465592218
+        },
+        {
+            "x": 7.680000069924504,
+            "y": 7.216499986370157,
+            "heading": 3.1400000316818817,
+            "angularVelocity": -3.1870782840594973e-7,
+            "velocityX": -5.949146101339814e-7,
+            "velocityY": 2.5032195205487504,
+            "timestamp": 0.6004267758710662
+        },
+        {
+            "x": 7.6800000249972635,
+            "y": 7.383499994424155,
+            "heading": 3.14000001043482,
+            "angularVelocity": -2.1231960430791816e-7,
+            "velocityX": -4.4895306909597897e-7,
+            "velocityY": 1.6688130652906694,
+            "timestamp": 0.7004979051829106
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": -1.0427402507810036e-7,
+            "velocityX": -2.497949654636061e-7,
+            "velocityY": 0.8344065481227817,
+            "timestamp": 0.8005690344947549
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 4.128313998274909e-23,
+            "velocityY": -7.415182378687901e-21,
+            "timestamp": 0.9006401638065993
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.traj
new file mode 100644
index 00000000..88fa5a42
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 1.6870437677917242e-24,
+            "velocityY": 1.6813073596046228e-23,
+            "timestamp": 0
+        },
+        {
+            "x": 7.680000027670122,
+            "y": 5.880500005575848,
+            "heading": 3.140000018022814,
+            "angularVelocity": 1.8010003546528393e-7,
+            "velocityX": 2.7650455327562645e-7,
+            "velocityY": 0.8344065481228156,
+            "timestamp": 0.10007112931184436
+        },
+        {
+            "x": 7.680000074983718,
+            "y": 6.047500013629854,
+            "heading": 3.1400000460729856,
+            "angularVelocity": 2.8030233382034975e-7,
+            "velocityX": 4.7279965639225817e-7,
+            "velocityY": 1.6688130652907482,
+            "timestamp": 0.20014225862368873
+        },
+        {
+            "x": 7.680000134184045,
+            "y": 6.298000017966634,
+            "heading": 3.1400000770424787,
+            "angularVelocity": 3.0947480383704467e-7,
+            "velocityX": 5.915824876001831e-7,
+            "velocityY": 2.5032195205488788,
+            "timestamp": 0.3002133879355331
+        },
+        {
+            "x": 7.680000197955803,
+            "y": 6.632000000000009,
+            "heading": 3.1400001051570197,
+            "angularVelocity": 2.8094557567454647e-7,
+            "velocityX": 6.372642912914395e-7,
+            "velocityY": 3.3376257900773374,
+            "timestamp": 0.40028451724737746
+        },
+        {
+            "x": 7.680000129458281,
+            "y": 6.96599998203339,
+            "heading": 3.140000063575334,
+            "angularVelocity": -4.1552129568712905e-7,
+            "velocityX": -6.844883427144649e-7,
+            "velocityY": 3.337625790077381,
+            "timestamp": 0.5003556465592218
+        },
+        {
+            "x": 7.680000069924504,
+            "y": 7.216499986370157,
+            "heading": 3.1400000316818817,
+            "angularVelocity": -3.1870782840594973e-7,
+            "velocityX": -5.949146101339814e-7,
+            "velocityY": 2.5032195205487504,
+            "timestamp": 0.6004267758710662
+        },
+        {
+            "x": 7.6800000249972635,
+            "y": 7.383499994424155,
+            "heading": 3.14000001043482,
+            "angularVelocity": -2.1231960430791816e-7,
+            "velocityX": -4.4895306909597897e-7,
+            "velocityY": 1.6688130652906694,
+            "timestamp": 0.7004979051829106
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": -1.0427402507810036e-7,
+            "velocityX": -2.497949654636061e-7,
+            "velocityY": 0.8344065481227817,
+            "timestamp": 0.8005690344947549
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 4.128313998274909e-23,
+            "velocityY": -7.415182378687901e-21,
+            "timestamp": 0.9006401638065993
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.1.traj
new file mode 100644
index 00000000..46b2cfe7
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.1.traj	
@@ -0,0 +1,139 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": -1.429570475254817e-33,
+            "velocityX": -4.923067503198675e-33,
+            "velocityY": 6.357653017323465e-38,
+            "timestamp": 0
+        },
+        {
+            "x": 7.680000074935176,
+            "y": 5.7205546279944945,
+            "heading": 3.1400000039815823,
+            "angularVelocity": 4.158287610282007e-8,
+            "velocityX": 7.826085092728229e-7,
+            "velocityY": -0.7983807350943761,
+            "timestamp": 0.09575052183150115
+        },
+        {
+            "x": 7.68000021698637,
+            "y": 5.567663887017397,
+            "heading": 3.1400000079801687,
+            "angularVelocity": 4.176046399562833e-8,
+            "velocityX": 0.000001483555234342966,
+            "velocityY": -1.5967614385031572,
+            "timestamp": 0.1915010436630023
+        },
+        {
+            "x": 7.680000418191477,
+            "y": 5.338327783136532,
+            "heading": 3.140000009761107,
+            "angularVelocity": 1.8599774209907478e-8,
+            "velocityX": 0.000002101347243657271,
+            "velocityY": -2.395142078540755,
+            "timestamp": 0.2872515654945035
+        },
+        {
+            "x": 7.680000670419164,
+            "y": 5.032546334555315,
+            "heading": 3.1400000093900653,
+            "angularVelocity": -3.875097409582088e-9,
+            "velocityX": 0.000002634217150547891,
+            "velocityY": -3.193522528465398,
+            "timestamp": 0.3830020873260046
+        },
+        {
+            "x": 7.68000083273763,
+            "y": 4.670327800733128,
+            "heading": 3.1400000093900697,
+            "angularVelocity": 4.937002430391119e-14,
+            "velocityX": 0.000001695223016776456,
+            "velocityY": -3.7829405719543154,
+            "timestamp": 0.4787526091575057
+        },
+        {
+            "x": 7.6800009141474845,
+            "y": 4.308109266911048,
+            "heading": 3.1400000093900733,
+            "angularVelocity": 3.662638141198336e-14,
+            "velocityX": 8.50228817967247e-7,
+            "velocityY": -3.782940571953249,
+            "timestamp": 0.5745031309890068
+        },
+        {
+            "x": 7.680000914400225,
+            "y": 3.9458907330889583,
+            "heading": 3.1400000093900764,
+            "angularVelocity": 3.286235025790432e-14,
+            "velocityX": 2.639571649715247e-9,
+            "velocityY": -3.7829405719533424,
+            "timestamp": 0.670253652820508
+        },
+        {
+            "x": 7.680000833444056,
+            "y": 3.5836721992668767,
+            "heading": 3.140000009390079,
+            "angularVelocity": 2.6535816526961207e-14,
+            "velocityX": -8.454906188658589e-7,
+            "velocityY": -3.78294057195325,
+            "timestamp": 0.7660041746520091
+        },
+        {
+            "x": 7.680000671408071,
+            "y": 3.221453665444688,
+            "heading": 3.140000009390085,
+            "angularVelocity": 5.86589821918151e-14,
+            "velocityX": -0.0000016922728517221796,
+            "velocityY": -3.7829405719543456,
+            "timestamp": 0.8617546964835102
+        },
+        {
+            "x": 7.680000418877801,
+            "y": 2.9156722168634683,
+            "heading": 3.140000001402919,
+            "angularVelocity": -8.341641379215641e-8,
+            "velocityX": -0.000002637377255306636,
+            "velocityY": -3.193522528465426,
+            "timestamp": 0.9575052183150113
+        },
+        {
+            "x": 7.680000217377681,
+            "y": 2.686336112982602,
+            "heading": 3.139999997550481,
+            "angularVelocity": -4.023411698701823e-8,
+            "velocityX": -0.0000021044283112699676,
+            "velocityY": -2.395142078540766,
+            "timestamp": 1.0532557401465126
+        },
+        {
+            "x": 7.680000075082089,
+            "y": 2.5334453720055046,
+            "heading": 3.139999997845116,
+            "angularVelocity": 3.0771112395983046e-9,
+            "velocityX": -0.0000014861076817541395,
+            "velocityY": -1.5967614385031628,
+            "timestamp": 1.1490062619780137
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 2.250519944152171e-8,
+            "velocityX": -7.841428415253779e-7,
+            "velocityY": -0.7983807350943782,
+            "timestamp": 1.2447567838095148
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 5.079076909387901e-38,
+            "timestamp": 1.340507305641016
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.traj
new file mode 100644
index 00000000..46b2cfe7
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.traj	
@@ -0,0 +1,139 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": -1.429570475254817e-33,
+            "velocityX": -4.923067503198675e-33,
+            "velocityY": 6.357653017323465e-38,
+            "timestamp": 0
+        },
+        {
+            "x": 7.680000074935176,
+            "y": 5.7205546279944945,
+            "heading": 3.1400000039815823,
+            "angularVelocity": 4.158287610282007e-8,
+            "velocityX": 7.826085092728229e-7,
+            "velocityY": -0.7983807350943761,
+            "timestamp": 0.09575052183150115
+        },
+        {
+            "x": 7.68000021698637,
+            "y": 5.567663887017397,
+            "heading": 3.1400000079801687,
+            "angularVelocity": 4.176046399562833e-8,
+            "velocityX": 0.000001483555234342966,
+            "velocityY": -1.5967614385031572,
+            "timestamp": 0.1915010436630023
+        },
+        {
+            "x": 7.680000418191477,
+            "y": 5.338327783136532,
+            "heading": 3.140000009761107,
+            "angularVelocity": 1.8599774209907478e-8,
+            "velocityX": 0.000002101347243657271,
+            "velocityY": -2.395142078540755,
+            "timestamp": 0.2872515654945035
+        },
+        {
+            "x": 7.680000670419164,
+            "y": 5.032546334555315,
+            "heading": 3.1400000093900653,
+            "angularVelocity": -3.875097409582088e-9,
+            "velocityX": 0.000002634217150547891,
+            "velocityY": -3.193522528465398,
+            "timestamp": 0.3830020873260046
+        },
+        {
+            "x": 7.68000083273763,
+            "y": 4.670327800733128,
+            "heading": 3.1400000093900697,
+            "angularVelocity": 4.937002430391119e-14,
+            "velocityX": 0.000001695223016776456,
+            "velocityY": -3.7829405719543154,
+            "timestamp": 0.4787526091575057
+        },
+        {
+            "x": 7.6800009141474845,
+            "y": 4.308109266911048,
+            "heading": 3.1400000093900733,
+            "angularVelocity": 3.662638141198336e-14,
+            "velocityX": 8.50228817967247e-7,
+            "velocityY": -3.782940571953249,
+            "timestamp": 0.5745031309890068
+        },
+        {
+            "x": 7.680000914400225,
+            "y": 3.9458907330889583,
+            "heading": 3.1400000093900764,
+            "angularVelocity": 3.286235025790432e-14,
+            "velocityX": 2.639571649715247e-9,
+            "velocityY": -3.7829405719533424,
+            "timestamp": 0.670253652820508
+        },
+        {
+            "x": 7.680000833444056,
+            "y": 3.5836721992668767,
+            "heading": 3.140000009390079,
+            "angularVelocity": 2.6535816526961207e-14,
+            "velocityX": -8.454906188658589e-7,
+            "velocityY": -3.78294057195325,
+            "timestamp": 0.7660041746520091
+        },
+        {
+            "x": 7.680000671408071,
+            "y": 3.221453665444688,
+            "heading": 3.140000009390085,
+            "angularVelocity": 5.86589821918151e-14,
+            "velocityX": -0.0000016922728517221796,
+            "velocityY": -3.7829405719543456,
+            "timestamp": 0.8617546964835102
+        },
+        {
+            "x": 7.680000418877801,
+            "y": 2.9156722168634683,
+            "heading": 3.140000001402919,
+            "angularVelocity": -8.341641379215641e-8,
+            "velocityX": -0.000002637377255306636,
+            "velocityY": -3.193522528465426,
+            "timestamp": 0.9575052183150113
+        },
+        {
+            "x": 7.680000217377681,
+            "y": 2.686336112982602,
+            "heading": 3.139999997550481,
+            "angularVelocity": -4.023411698701823e-8,
+            "velocityX": -0.0000021044283112699676,
+            "velocityY": -2.395142078540766,
+            "timestamp": 1.0532557401465126
+        },
+        {
+            "x": 7.680000075082089,
+            "y": 2.5334453720055046,
+            "heading": 3.139999997845116,
+            "angularVelocity": 3.0771112395983046e-9,
+            "velocityX": -0.0000014861076817541395,
+            "velocityY": -1.5967614385031628,
+            "timestamp": 1.1490062619780137
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 2.250519944152171e-8,
+            "velocityX": -7.841428415253779e-7,
+            "velocityY": -0.7983807350943782,
+            "timestamp": 1.2447567838095148
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 5.079076909387901e-38,
+            "timestamp": 1.340507305641016
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.1.traj
new file mode 100644
index 00000000..8470d032
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.1.traj	
@@ -0,0 +1,175 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": -8.977040531488188e-32,
+            "velocityX": -1.4835352698294527e-33,
+            "velocityY": -6.682715069575163e-38,
+            "timestamp": 0
+        },
+        {
+            "x": 7.680000115631693,
+            "y": 5.715155846048254,
+            "heading": 3.139999983275777,
+            "angularVelocity": -1.688054668223312e-7,
+            "velocityX": 0.0000011671252822265972,
+            "velocityY": -0.8260917454273096,
+            "timestamp": 0.09907392781101573
+        },
+        {
+            "x": 7.680000338287477,
+            "y": 5.551467541258106,
+            "heading": 3.1399999456936927,
+            "angularVelocity": -3.793336766253788e-7,
+            "velocityX": 0.0000022473699405935927,
+            "velocityY": -1.6521834594301872,
+            "timestamp": 0.19814785562203147
+        },
+        {
+            "x": 7.680000659193895,
+            "y": 5.305935091856244,
+            "heading": 3.1399998851536046,
+            "angularVelocity": -6.110596046233074e-7,
+            "velocityX": 0.0000032390599337968516,
+            "velocityY": -2.4782751105841556,
+            "timestamp": 0.29722178343304717
+        },
+        {
+            "x": 7.680001069383311,
+            "y": 4.978558516522714,
+            "heading": 3.1399998022537425,
+            "angularVelocity": -8.367472472020738e-7,
+            "velocityX": 0.0000041402352562216615,
+            "velocityY": -3.3043665731916145,
+            "timestamp": 0.39629571124406293
+        },
+        {
+            "x": 7.680001393532974,
+            "y": 4.603767735073282,
+            "heading": 3.1399998022537403,
+            "angularVelocity": -2.4397007486697484e-14,
+            "velocityX": 0.0000032717959136681545,
+            "velocityY": -3.782940575085891,
+            "timestamp": 0.4953696390550787
+        },
+        {
+            "x": 7.680001631666853,
+            "y": 4.228976953623769,
+            "heading": 3.1399998022537345,
+            "angularVelocity": -6.162047103580494e-14,
+            "velocityX": 0.000002403597704835693,
+            "velocityY": -3.7829405750867746,
+            "timestamp": 0.5944435668660945
+        },
+        {
+            "x": 7.68000178361998,
+            "y": 3.854186172174224,
+            "heading": 3.139999802253728,
+            "angularVelocity": -7.021738392770843e-14,
+            "velocityX": 0.0000015337346806740877,
+            "velocityY": -3.78294057508712,
+            "timestamp": 0.6935174946771102
+        },
+        {
+            "x": 7.680001849334011,
+            "y": 3.4793953907246618,
+            "heading": 3.1399998022537208,
+            "angularVelocity": -7.676689036353258e-14,
+            "velocityX": 6.632827646851356e-7,
+            "velocityY": -3.782940575087319,
+            "timestamp": 0.792591422488126
+        },
+        {
+            "x": 7.680001829174184,
+            "y": 3.104604609275094,
+            "heading": 3.139999802253714,
+            "angularVelocity": -7.698791271549556e-14,
+            "velocityX": -2.0348261115822963e-7,
+            "velocityY": -3.7829405750873777,
+            "timestamp": 0.8916653502991417
+        },
+        {
+            "x": 7.680001725145299,
+            "y": 2.729813827825533,
+            "heading": 3.1399998022537057,
+            "angularVelocity": -9.316120157306394e-14,
+            "velocityX": -0.000001050012614805773,
+            "velocityY": -3.7829405750872973,
+            "timestamp": 0.9907392781101575
+        },
+        {
+            "x": 7.680001540841657,
+            "y": 2.355023046375991,
+            "heading": 3.1399998022536986,
+            "angularVelocity": -7.445499842222973e-14,
+            "velocityX": -0.0000018602636449128091,
+            "velocityY": -3.7829405750870753,
+            "timestamp": 1.0898132059211731
+        },
+        {
+            "x": 7.680001281423707,
+            "y": 1.9802322649264825,
+            "heading": 3.139999802253698,
+            "angularVelocity": -7.093550187375142e-15,
+            "velocityX": -0.000002618427817479533,
+            "velocityY": -3.782940575086706,
+            "timestamp": 1.188887133732189
+        },
+        {
+            "x": 7.680000953984531,
+            "y": 1.6054414834770696,
+            "heading": 3.139999802253699,
+            "angularVelocity": 1.1174094242032294e-14,
+            "velocityX": -0.0000033049984216512085,
+            "velocityY": -3.782940575085678,
+            "timestamp": 1.2879610615432047
+        },
+        {
+            "x": 7.680000583113676,
+            "y": 1.2780649081436686,
+            "heading": 3.139999880388909,
+            "angularVelocity": 7.886553669648788e-7,
+            "velocityX": -0.0000037433746003660557,
+            "velocityY": -3.304366573190331,
+            "timestamp": 1.3870349893542204
+        },
+        {
+            "x": 7.6800002967542085,
+            "y": 1.0325324587418607,
+            "heading": 3.139999940073013,
+            "angularVelocity": 6.024197635453253e-7,
+            "velocityX": -0.0000028903613408431902,
+            "velocityY": -2.47827511058363,
+            "timestamp": 1.4861089171652362
+        },
+        {
+            "x": 7.680000100606573,
+            "y": 0.8688441539517371,
+            "heading": 3.1399999800752636,
+            "angularVelocity": 4.0376157345960537e-7,
+            "velocityX": -0.000001979810751989867,
+            "velocityY": -1.6521834594299558,
+            "timestamp": 1.585182844976252
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 2.0110976517179292e-7,
+            "velocityX": -0.00000101546967480447,
+            "velocityY": -0.8260917454272348,
+            "timestamp": 1.6842567727872677
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": -8.944233053536814e-32,
+            "velocityX": 0,
+            "velocityY": -1.1208522306082827e-37,
+            "timestamp": 1.7833307005982835
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.traj
new file mode 100644
index 00000000..8470d032
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.traj	
@@ -0,0 +1,175 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": -8.977040531488188e-32,
+            "velocityX": -1.4835352698294527e-33,
+            "velocityY": -6.682715069575163e-38,
+            "timestamp": 0
+        },
+        {
+            "x": 7.680000115631693,
+            "y": 5.715155846048254,
+            "heading": 3.139999983275777,
+            "angularVelocity": -1.688054668223312e-7,
+            "velocityX": 0.0000011671252822265972,
+            "velocityY": -0.8260917454273096,
+            "timestamp": 0.09907392781101573
+        },
+        {
+            "x": 7.680000338287477,
+            "y": 5.551467541258106,
+            "heading": 3.1399999456936927,
+            "angularVelocity": -3.793336766253788e-7,
+            "velocityX": 0.0000022473699405935927,
+            "velocityY": -1.6521834594301872,
+            "timestamp": 0.19814785562203147
+        },
+        {
+            "x": 7.680000659193895,
+            "y": 5.305935091856244,
+            "heading": 3.1399998851536046,
+            "angularVelocity": -6.110596046233074e-7,
+            "velocityX": 0.0000032390599337968516,
+            "velocityY": -2.4782751105841556,
+            "timestamp": 0.29722178343304717
+        },
+        {
+            "x": 7.680001069383311,
+            "y": 4.978558516522714,
+            "heading": 3.1399998022537425,
+            "angularVelocity": -8.367472472020738e-7,
+            "velocityX": 0.0000041402352562216615,
+            "velocityY": -3.3043665731916145,
+            "timestamp": 0.39629571124406293
+        },
+        {
+            "x": 7.680001393532974,
+            "y": 4.603767735073282,
+            "heading": 3.1399998022537403,
+            "angularVelocity": -2.4397007486697484e-14,
+            "velocityX": 0.0000032717959136681545,
+            "velocityY": -3.782940575085891,
+            "timestamp": 0.4953696390550787
+        },
+        {
+            "x": 7.680001631666853,
+            "y": 4.228976953623769,
+            "heading": 3.1399998022537345,
+            "angularVelocity": -6.162047103580494e-14,
+            "velocityX": 0.000002403597704835693,
+            "velocityY": -3.7829405750867746,
+            "timestamp": 0.5944435668660945
+        },
+        {
+            "x": 7.68000178361998,
+            "y": 3.854186172174224,
+            "heading": 3.139999802253728,
+            "angularVelocity": -7.021738392770843e-14,
+            "velocityX": 0.0000015337346806740877,
+            "velocityY": -3.78294057508712,
+            "timestamp": 0.6935174946771102
+        },
+        {
+            "x": 7.680001849334011,
+            "y": 3.4793953907246618,
+            "heading": 3.1399998022537208,
+            "angularVelocity": -7.676689036353258e-14,
+            "velocityX": 6.632827646851356e-7,
+            "velocityY": -3.782940575087319,
+            "timestamp": 0.792591422488126
+        },
+        {
+            "x": 7.680001829174184,
+            "y": 3.104604609275094,
+            "heading": 3.139999802253714,
+            "angularVelocity": -7.698791271549556e-14,
+            "velocityX": -2.0348261115822963e-7,
+            "velocityY": -3.7829405750873777,
+            "timestamp": 0.8916653502991417
+        },
+        {
+            "x": 7.680001725145299,
+            "y": 2.729813827825533,
+            "heading": 3.1399998022537057,
+            "angularVelocity": -9.316120157306394e-14,
+            "velocityX": -0.000001050012614805773,
+            "velocityY": -3.7829405750872973,
+            "timestamp": 0.9907392781101575
+        },
+        {
+            "x": 7.680001540841657,
+            "y": 2.355023046375991,
+            "heading": 3.1399998022536986,
+            "angularVelocity": -7.445499842222973e-14,
+            "velocityX": -0.0000018602636449128091,
+            "velocityY": -3.7829405750870753,
+            "timestamp": 1.0898132059211731
+        },
+        {
+            "x": 7.680001281423707,
+            "y": 1.9802322649264825,
+            "heading": 3.139999802253698,
+            "angularVelocity": -7.093550187375142e-15,
+            "velocityX": -0.000002618427817479533,
+            "velocityY": -3.782940575086706,
+            "timestamp": 1.188887133732189
+        },
+        {
+            "x": 7.680000953984531,
+            "y": 1.6054414834770696,
+            "heading": 3.139999802253699,
+            "angularVelocity": 1.1174094242032294e-14,
+            "velocityX": -0.0000033049984216512085,
+            "velocityY": -3.782940575085678,
+            "timestamp": 1.2879610615432047
+        },
+        {
+            "x": 7.680000583113676,
+            "y": 1.2780649081436686,
+            "heading": 3.139999880388909,
+            "angularVelocity": 7.886553669648788e-7,
+            "velocityX": -0.0000037433746003660557,
+            "velocityY": -3.304366573190331,
+            "timestamp": 1.3870349893542204
+        },
+        {
+            "x": 7.6800002967542085,
+            "y": 1.0325324587418607,
+            "heading": 3.139999940073013,
+            "angularVelocity": 6.024197635453253e-7,
+            "velocityX": -0.0000028903613408431902,
+            "velocityY": -2.47827511058363,
+            "timestamp": 1.4861089171652362
+        },
+        {
+            "x": 7.680000100606573,
+            "y": 0.8688441539517371,
+            "heading": 3.1399999800752636,
+            "angularVelocity": 4.0376157345960537e-7,
+            "velocityX": -0.000001979810751989867,
+            "velocityY": -1.6521834594299558,
+            "timestamp": 1.585182844976252
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 2.0110976517179292e-7,
+            "velocityX": -0.00000101546967480447,
+            "velocityY": -0.8260917454272348,
+            "timestamp": 1.6842567727872677
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": -8.944233053536814e-32,
+            "velocityX": 0,
+            "velocityY": -1.1208522306082827e-37,
+            "timestamp": 1.7833307005982835
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.1.traj
new file mode 100644
index 00000000..1d87c7a2
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 6.88462418020868e-33,
+            "velocityX": -2.6678679284923953e-34,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.620477294727877,
+            "y": 5.828094267433712,
+            "heading": 3.125579827082933,
+            "angularVelocity": -0.16053625399389732,
+            "velocityX": -0.6626517023703261,
+            "velocityY": 0.34616486523434487,
+            "timestamp": 0.08982502430644557
+        },
+        {
+            "x": 7.501400723607879,
+            "y": 5.890296797165295,
+            "heading": 3.0989291927764526,
+            "angularVelocity": -0.29669498574873204,
+            "velocityX": -1.3256503078001867,
+            "velocityY": 0.6924855318644141,
+            "timestamp": 0.17965004861289113
+        },
+        {
+            "x": 7.322728500258778,
+            "y": 5.983625073051111,
+            "heading": 3.0637763178084882,
+            "angularVelocity": -0.3913483490751664,
+            "velocityX": -1.9891141107799197,
+            "velocityY": 1.0390008419860823,
+            "timestamp": 0.2694750729193367
+        },
+        {
+            "x": 7.084419480032935,
+            "y": 6.108090729851432,
+            "heading": 3.027860333619382,
+            "angularVelocity": -0.3998438571703182,
+            "velocityX": -2.6530359670466983,
+            "velocityY": 1.3856456790448122,
+            "timestamp": 0.35930009722578227
+        },
+        {
+            "x": 6.786779796078207,
+            "y": 6.263490017806672,
+            "heading": 3.0170090626873085,
+            "angularVelocity": -0.1208045421179424,
+            "velocityX": -3.3135497179416755,
+            "velocityY": 1.7300222199225983,
+            "timestamp": 0.44912512153222783
+        },
+        {
+            "x": 6.48562142682328,
+            "y": 6.4208733949571855,
+            "heading": 3.0170090501527196,
+            "angularVelocity": -1.3954451104022202e-7,
+            "velocityX": -3.3527223797624646,
+            "velocityY": 1.752110599086374,
+            "timestamp": 0.5389501458386734
+        },
+        {
+            "x": 6.164454252506572,
+            "y": 6.531857797451653,
+            "heading": 3.017009051216684,
+            "angularVelocity": 1.1844854204542515e-8,
+            "velocityX": -3.5754755069257693,
+            "velocityY": 1.2355621760349855,
+            "timestamp": 0.628775170145119
+        },
+        {
+            "x": 5.838539791831514,
+            "y": 6.576460283854247,
+            "heading": 3.052457229239417,
+            "angularVelocity": 0.3946358856725586,
+            "velocityX": -3.6283258834773524,
+            "velocityY": 0.4965485592347737,
+            "timestamp": 0.7186001944515645
+        },
+        {
+            "x": 5.573157198508507,
+            "y": 6.601492716526358,
+            "heading": 3.1680842973805725,
+            "angularVelocity": 1.287247835822277,
+            "velocityX": -2.9544394267863754,
+            "velocityY": 0.2786799432050252,
+            "timestamp": 0.8084252187580101
+        },
+        {
+            "x": 5.374340211248987,
+            "y": 6.616920258488131,
+            "heading": 3.277639232341259,
+            "angularVelocity": 1.2196482640175015,
+            "velocityX": -2.2133808345126464,
+            "velocityY": 0.17175104689246343,
+            "timestamp": 0.8982502430644557
+        },
+        {
+            "x": 5.242040057703165,
+            "y": 6.625927417066914,
+            "heading": 3.3593576896006887,
+            "angularVelocity": 0.9097515741341804,
+            "velocityX": -1.4728652128661812,
+            "velocityY": 0.10027449085964446,
+            "timestamp": 0.9880752673709012
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.48792213436398174,
+            "velocityX": -0.7352077910701558,
+            "velocityY": 0.04533906853386082,
+            "timestamp": 1.0779002916773468
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0,
+            "velocityX": 2.612853322689367e-35,
+            "velocityY": 8.19047982527468e-33,
+            "timestamp": 1.1677253159837924
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.traj
new file mode 100644
index 00000000..1d87c7a2
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 6.88462418020868e-33,
+            "velocityX": -2.6678679284923953e-34,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.620477294727877,
+            "y": 5.828094267433712,
+            "heading": 3.125579827082933,
+            "angularVelocity": -0.16053625399389732,
+            "velocityX": -0.6626517023703261,
+            "velocityY": 0.34616486523434487,
+            "timestamp": 0.08982502430644557
+        },
+        {
+            "x": 7.501400723607879,
+            "y": 5.890296797165295,
+            "heading": 3.0989291927764526,
+            "angularVelocity": -0.29669498574873204,
+            "velocityX": -1.3256503078001867,
+            "velocityY": 0.6924855318644141,
+            "timestamp": 0.17965004861289113
+        },
+        {
+            "x": 7.322728500258778,
+            "y": 5.983625073051111,
+            "heading": 3.0637763178084882,
+            "angularVelocity": -0.3913483490751664,
+            "velocityX": -1.9891141107799197,
+            "velocityY": 1.0390008419860823,
+            "timestamp": 0.2694750729193367
+        },
+        {
+            "x": 7.084419480032935,
+            "y": 6.108090729851432,
+            "heading": 3.027860333619382,
+            "angularVelocity": -0.3998438571703182,
+            "velocityX": -2.6530359670466983,
+            "velocityY": 1.3856456790448122,
+            "timestamp": 0.35930009722578227
+        },
+        {
+            "x": 6.786779796078207,
+            "y": 6.263490017806672,
+            "heading": 3.0170090626873085,
+            "angularVelocity": -0.1208045421179424,
+            "velocityX": -3.3135497179416755,
+            "velocityY": 1.7300222199225983,
+            "timestamp": 0.44912512153222783
+        },
+        {
+            "x": 6.48562142682328,
+            "y": 6.4208733949571855,
+            "heading": 3.0170090501527196,
+            "angularVelocity": -1.3954451104022202e-7,
+            "velocityX": -3.3527223797624646,
+            "velocityY": 1.752110599086374,
+            "timestamp": 0.5389501458386734
+        },
+        {
+            "x": 6.164454252506572,
+            "y": 6.531857797451653,
+            "heading": 3.017009051216684,
+            "angularVelocity": 1.1844854204542515e-8,
+            "velocityX": -3.5754755069257693,
+            "velocityY": 1.2355621760349855,
+            "timestamp": 0.628775170145119
+        },
+        {
+            "x": 5.838539791831514,
+            "y": 6.576460283854247,
+            "heading": 3.052457229239417,
+            "angularVelocity": 0.3946358856725586,
+            "velocityX": -3.6283258834773524,
+            "velocityY": 0.4965485592347737,
+            "timestamp": 0.7186001944515645
+        },
+        {
+            "x": 5.573157198508507,
+            "y": 6.601492716526358,
+            "heading": 3.1680842973805725,
+            "angularVelocity": 1.287247835822277,
+            "velocityX": -2.9544394267863754,
+            "velocityY": 0.2786799432050252,
+            "timestamp": 0.8084252187580101
+        },
+        {
+            "x": 5.374340211248987,
+            "y": 6.616920258488131,
+            "heading": 3.277639232341259,
+            "angularVelocity": 1.2196482640175015,
+            "velocityX": -2.2133808345126464,
+            "velocityY": 0.17175104689246343,
+            "timestamp": 0.8982502430644557
+        },
+        {
+            "x": 5.242040057703165,
+            "y": 6.625927417066914,
+            "heading": 3.3593576896006887,
+            "angularVelocity": 0.9097515741341804,
+            "velocityX": -1.4728652128661812,
+            "velocityY": 0.10027449085964446,
+            "timestamp": 0.9880752673709012
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.48792213436398174,
+            "velocityX": -0.7352077910701558,
+            "velocityY": 0.04533906853386082,
+            "timestamp": 1.0779002916773468
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0,
+            "velocityX": 2.612853322689367e-35,
+            "velocityY": 8.19047982527468e-33,
+            "timestamp": 1.1677253159837924
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.1.traj
new file mode 100644
index 00000000..7d7772cd
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.1.traj	
@@ -0,0 +1,139 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 7.320432516399354e-29,
+            "velocityX": -1.2956627863132083e-30,
+            "velocityY": -2.1869206977044484e-30,
+            "timestamp": 0
+        },
+        {
+            "x": 7.583655523801243,
+            "y": 5.855694285619672,
+            "heading": 3.1309164320392644,
+            "angularVelocity": -0.078082149385061,
+            "velocityX": -0.8281749875703897,
+            "velocityY": 0.5045347816645432,
+            "timestamp": 0.1163334773987831
+        },
+        {
+            "x": 7.390966571410279,
+            "y": 5.973082843106357,
+            "heading": 3.112749003492932,
+            "angularVelocity": -0.15616681416695535,
+            "velocityX": -1.6563499750844712,
+            "velocityY": 1.0090694451115323,
+            "timestamp": 0.2326669547975662
+        },
+        {
+            "x": 7.1019331530012675,
+            "y": 6.149165651208255,
+            "heading": 3.0854972394904423,
+            "angularVelocity": -0.2342555609256986,
+            "velocityX": -2.484524875141701,
+            "velocityY": 1.5136039258785212,
+            "timestamp": 0.3490004321963493
+        },
+        {
+            "x": 6.726101496816599,
+            "y": 6.378126994339933,
+            "heading": 3.085497236816664,
+            "angularVelocity": -2.298373345534558e-8,
+            "velocityX": -3.2306406082605346,
+            "velocityY": 1.968146644037928,
+            "timestamp": 0.4653339095951324
+        },
+        {
+            "x": 6.345661043001768,
+            "y": 6.599345871138349,
+            "heading": 3.085497234127535,
+            "angularVelocity": -2.3115694536288248e-8,
+            "velocityX": -3.2702577308052767,
+            "velocityY": 1.9015925745956355,
+            "timestamp": 0.5816673869939155
+        },
+        {
+            "x": 5.9214720079184415,
+            "y": 6.716548218753983,
+            "heading": 3.08549723097445,
+            "angularVelocity": -2.7103849507961225e-8,
+            "velocityX": -3.6463195682635403,
+            "velocityY": 1.0074687891764202,
+            "timestamp": 0.6980008643926986
+        },
+        {
+            "x": 5.481423496557165,
+            "y": 6.7220267434353165,
+            "heading": 3.0854971884524733,
+            "angularVelocity": -3.6551797104544465e-7,
+            "velocityX": -3.7826472757521072,
+            "velocityY": 0.047093277050021864,
+            "timestamp": 0.8143343417914817
+        },
+        {
+            "x": 5.071345998787579,
+            "y": 6.619502331936359,
+            "heading": 3.038730840809692,
+            "angularVelocity": -0.40200249049953485,
+            "velocityX": -3.5250171054705812,
+            "velocityY": -0.8812975747944856,
+            "timestamp": 0.9306678191902648
+        },
+        {
+            "x": 4.728494542469685,
+            "y": 6.427075201621358,
+            "heading": 2.9510390674106386,
+            "angularVelocity": -0.7537965455846517,
+            "velocityX": -2.947143539280811,
+            "velocityY": -1.6540993583074475,
+            "timestamp": 1.0470012965890478
+        },
+        {
+            "x": 4.474342078218714,
+            "y": 6.168607565995357,
+            "heading": 2.943927263065388,
+            "angularVelocity": -0.061132912935043464,
+            "velocityX": -2.1846889642931684,
+            "velocityY": -2.221782082039817,
+            "timestamp": 1.163334773987831
+        },
+        {
+            "x": 4.328922284450032,
+            "y": 5.887474207261646,
+            "heading": 3.0089644652983214,
+            "angularVelocity": 0.5590583526527744,
+            "velocityX": -1.2500253325205173,
+            "velocityY": -2.4166161368150747,
+            "timestamp": 1.2796682513866142
+        },
+        {
+            "x": 4.275692020371012,
+            "y": 5.671261483228584,
+            "heading": 3.090017233953519,
+            "angularVelocity": 0.696727807571276,
+            "velocityX": -0.45756617329120264,
+            "velocityY": -1.8585597960929203,
+            "timestamp": 1.3960017287853974
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0.42965075199414443,
+            "velocityX": -0.10910032653373211,
+            "velocityY": -0.9564012502367466,
+            "timestamp": 1.5123352061841806
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -1.1779881542407281e-29,
+            "velocityX": 2.2171306210967267e-31,
+            "velocityY": -6.290632939767308e-32,
+            "timestamp": 1.6286686835829638
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.traj
new file mode 100644
index 00000000..7d7772cd
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.traj	
@@ -0,0 +1,139 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 7.320432516399354e-29,
+            "velocityX": -1.2956627863132083e-30,
+            "velocityY": -2.1869206977044484e-30,
+            "timestamp": 0
+        },
+        {
+            "x": 7.583655523801243,
+            "y": 5.855694285619672,
+            "heading": 3.1309164320392644,
+            "angularVelocity": -0.078082149385061,
+            "velocityX": -0.8281749875703897,
+            "velocityY": 0.5045347816645432,
+            "timestamp": 0.1163334773987831
+        },
+        {
+            "x": 7.390966571410279,
+            "y": 5.973082843106357,
+            "heading": 3.112749003492932,
+            "angularVelocity": -0.15616681416695535,
+            "velocityX": -1.6563499750844712,
+            "velocityY": 1.0090694451115323,
+            "timestamp": 0.2326669547975662
+        },
+        {
+            "x": 7.1019331530012675,
+            "y": 6.149165651208255,
+            "heading": 3.0854972394904423,
+            "angularVelocity": -0.2342555609256986,
+            "velocityX": -2.484524875141701,
+            "velocityY": 1.5136039258785212,
+            "timestamp": 0.3490004321963493
+        },
+        {
+            "x": 6.726101496816599,
+            "y": 6.378126994339933,
+            "heading": 3.085497236816664,
+            "angularVelocity": -2.298373345534558e-8,
+            "velocityX": -3.2306406082605346,
+            "velocityY": 1.968146644037928,
+            "timestamp": 0.4653339095951324
+        },
+        {
+            "x": 6.345661043001768,
+            "y": 6.599345871138349,
+            "heading": 3.085497234127535,
+            "angularVelocity": -2.3115694536288248e-8,
+            "velocityX": -3.2702577308052767,
+            "velocityY": 1.9015925745956355,
+            "timestamp": 0.5816673869939155
+        },
+        {
+            "x": 5.9214720079184415,
+            "y": 6.716548218753983,
+            "heading": 3.08549723097445,
+            "angularVelocity": -2.7103849507961225e-8,
+            "velocityX": -3.6463195682635403,
+            "velocityY": 1.0074687891764202,
+            "timestamp": 0.6980008643926986
+        },
+        {
+            "x": 5.481423496557165,
+            "y": 6.7220267434353165,
+            "heading": 3.0854971884524733,
+            "angularVelocity": -3.6551797104544465e-7,
+            "velocityX": -3.7826472757521072,
+            "velocityY": 0.047093277050021864,
+            "timestamp": 0.8143343417914817
+        },
+        {
+            "x": 5.071345998787579,
+            "y": 6.619502331936359,
+            "heading": 3.038730840809692,
+            "angularVelocity": -0.40200249049953485,
+            "velocityX": -3.5250171054705812,
+            "velocityY": -0.8812975747944856,
+            "timestamp": 0.9306678191902648
+        },
+        {
+            "x": 4.728494542469685,
+            "y": 6.427075201621358,
+            "heading": 2.9510390674106386,
+            "angularVelocity": -0.7537965455846517,
+            "velocityX": -2.947143539280811,
+            "velocityY": -1.6540993583074475,
+            "timestamp": 1.0470012965890478
+        },
+        {
+            "x": 4.474342078218714,
+            "y": 6.168607565995357,
+            "heading": 2.943927263065388,
+            "angularVelocity": -0.061132912935043464,
+            "velocityX": -2.1846889642931684,
+            "velocityY": -2.221782082039817,
+            "timestamp": 1.163334773987831
+        },
+        {
+            "x": 4.328922284450032,
+            "y": 5.887474207261646,
+            "heading": 3.0089644652983214,
+            "angularVelocity": 0.5590583526527744,
+            "velocityX": -1.2500253325205173,
+            "velocityY": -2.4166161368150747,
+            "timestamp": 1.2796682513866142
+        },
+        {
+            "x": 4.275692020371012,
+            "y": 5.671261483228584,
+            "heading": 3.090017233953519,
+            "angularVelocity": 0.696727807571276,
+            "velocityX": -0.45756617329120264,
+            "velocityY": -1.8585597960929203,
+            "timestamp": 1.3960017287853974
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0.42965075199414443,
+            "velocityX": -0.10910032653373211,
+            "velocityY": -0.9564012502367466,
+            "timestamp": 1.5123352061841806
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -1.1779881542407281e-29,
+            "velocityX": 2.2171306210967267e-31,
+            "velocityY": -6.290632939767308e-32,
+            "timestamp": 1.6286686835829638
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.1.traj
new file mode 100644
index 00000000..9d9edbcb
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.1.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 7.712459531346682e-31,
+            "velocityX": 8.463399539512725e-31,
+            "velocityY": -2.525463677436265e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 7.643960549352244,
+            "y": 5.758768623179225,
+            "heading": 3.0545306432351627,
+            "angularVelocity": -1.025550451563212,
+            "velocityX": -0.43243890307482463,
+            "velocityY": -0.45873991856878515,
+            "timestamp": 0.0833399825767314
+        },
+        {
+            "x": 7.572007137321917,
+            "y": 5.681963480289103,
+            "heading": 2.882145272333543,
+            "angularVelocity": -2.0684594065387945,
+            "velocityX": -0.8633720551126874,
+            "velocityY": -0.9215881803119774,
+            "timestamp": 0.1666799651534628
+        },
+        {
+            "x": 7.462165154490984,
+            "y": 5.566757202432317,
+            "heading": 2.631422178575744,
+            "angularVelocity": -3.008437079128932,
+            "velocityX": -1.3179986296469572,
+            "velocityY": -1.382365034102495,
+            "timestamp": 0.2500199477301942
+        },
+        {
+            "x": 7.311173429444628,
+            "y": 5.412128972668698,
+            "heading": 2.3433148642731845,
+            "angularVelocity": -3.457011933465413,
+            "velocityX": -1.8117561388657581,
+            "velocityY": -1.8553907138299572,
+            "timestamp": 0.3333599303069256
+        },
+        {
+            "x": 7.123843168628933,
+            "y": 5.227542500531133,
+            "heading": 2.191575313033137,
+            "angularVelocity": -1.820729337210275,
+            "velocityX": -2.2477837770510787,
+            "velocityY": -2.21486093985698,
+            "timestamp": 0.41669991288365704
+        },
+        {
+            "x": 6.902601176392792,
+            "y": 5.010431192670728,
+            "heading": 2.175785118248912,
+            "angularVelocity": -0.1894672196467848,
+            "velocityX": -2.6546920865077652,
+            "velocityY": -2.6051278287766304,
+            "timestamp": 0.5000398954603884
+        },
+        {
+            "x": 6.682324869385203,
+            "y": 4.7848793693995555,
+            "heading": 2.175784967765777,
+            "angularVelocity": -0.0000018056535426780284,
+            "velocityX": -2.643104788326184,
+            "velocityY": -2.7064059326328107,
+            "timestamp": 0.5833798780371198
+        },
+        {
+            "x": 6.427295318194512,
+            "y": 4.59952808942922,
+            "heading": 2.175784213925318,
+            "angularVelocity": -0.00000904536377316099,
+            "velocityX": -3.060110445258255,
+            "velocityY": -2.224037901611934,
+            "timestamp": 0.6667198606138511
+        },
+        {
+            "x": 6.1722637689115825,
+            "y": 4.4141795587676,
+            "heading": 2.1757834600140606,
+            "angularVelocity": -0.000009046213285018581,
+            "velocityX": -3.0601344204519796,
+            "velocityY": -2.224004912539731,
+            "timestamp": 0.7500598431905825
+        },
+        {
+            "x": 5.906721685974762,
+            "y": 4.244230836092675,
+            "heading": 2.175782696018566,
+            "angularVelocity": -0.000009167214470514032,
+            "velocityX": -3.1862507613597577,
+            "velocityY": -2.039221960701194,
+            "timestamp": 0.8333998257673139
+        },
+        {
+            "x": 5.614573321214709,
+            "y": 4.1257208216835135,
+            "heading": 2.175782627659757,
+            "angularVelocity": -8.202402673559022e-7,
+            "velocityX": -3.5055006699943965,
+            "velocityY": -1.4220067096852238,
+            "timestamp": 0.9167398083440452
+        },
+        {
+            "x": 5.32091408424666,
+            "y": 4.0110057741909,
+            "heading": 2.1757827056250556,
+            "angularVelocity": 9.355089415031885e-7,
+            "velocityX": -3.523629689959162,
+            "velocityY": -1.3764707400435987,
+            "timestamp": 1.0000797909207766
+        },
+        {
+            "x": 5.051276753694785,
+            "y": 3.8476320535360697,
+            "heading": 2.1757829434200273,
+            "angularVelocity": 0.0000028533119900283243,
+            "velocityX": -3.235389811890331,
+            "velocityY": -1.9603282314633543,
+            "timestamp": 1.083419773497508
+        },
+        {
+            "x": 4.804772075990701,
+            "y": 3.6532031340051674,
+            "heading": 2.180086334431566,
+            "angularVelocity": 0.05163657200885012,
+            "velocityX": -2.957820125257733,
+            "velocityY": -2.332960885273652,
+            "timestamp": 1.1667597560742393
+        },
+        {
+            "x": 4.605798608727452,
+            "y": 3.4589597133454815,
+            "heading": 2.2977487638281207,
+            "angularVelocity": 1.411836501024247,
+            "velocityX": -2.387491107045202,
+            "velocityY": -2.330735076418397,
+            "timestamp": 1.2500997386509707
+        },
+        {
+            "x": 4.462499334629702,
+            "y": 3.280406115781069,
+            "heading": 2.4052460747163282,
+            "angularVelocity": 1.28986481115753,
+            "velocityX": -1.7194540923477422,
+            "velocityY": -2.142472220941705,
+            "timestamp": 1.333439721227702
+        },
+        {
+            "x": 4.360731419840454,
+            "y": 3.1416564242722327,
+            "heading": 2.488750866660265,
+            "angularVelocity": 1.0019775546155592,
+            "velocityX": -1.2211175433778025,
+            "velocityY": -1.6648634571178227,
+            "timestamp": 1.4167797038044334
+        },
+        {
+            "x": 4.295085549107569,
+            "y": 3.0475525606989393,
+            "heading": 2.5453815690352877,
+            "angularVelocity": 0.6795142094358206,
+            "velocityX": -0.7876875984758661,
+            "velocityY": -1.129156266461315,
+            "timestamp": 1.5001196863811648
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0.3433937718713025,
+            "velocityX": -0.3849958701158514,
+            "velocityY": -0.5705852008687156,
+            "timestamp": 1.5834596689578961
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -1.6359372913753935e-30,
+            "velocityX": 0,
+            "velocityY": -3.7530428699783824e-32,
+            "timestamp": 1.6667996515346275
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.traj
new file mode 100644
index 00000000..9d9edbcb
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 7.712459531346682e-31,
+            "velocityX": 8.463399539512725e-31,
+            "velocityY": -2.525463677436265e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 7.643960549352244,
+            "y": 5.758768623179225,
+            "heading": 3.0545306432351627,
+            "angularVelocity": -1.025550451563212,
+            "velocityX": -0.43243890307482463,
+            "velocityY": -0.45873991856878515,
+            "timestamp": 0.0833399825767314
+        },
+        {
+            "x": 7.572007137321917,
+            "y": 5.681963480289103,
+            "heading": 2.882145272333543,
+            "angularVelocity": -2.0684594065387945,
+            "velocityX": -0.8633720551126874,
+            "velocityY": -0.9215881803119774,
+            "timestamp": 0.1666799651534628
+        },
+        {
+            "x": 7.462165154490984,
+            "y": 5.566757202432317,
+            "heading": 2.631422178575744,
+            "angularVelocity": -3.008437079128932,
+            "velocityX": -1.3179986296469572,
+            "velocityY": -1.382365034102495,
+            "timestamp": 0.2500199477301942
+        },
+        {
+            "x": 7.311173429444628,
+            "y": 5.412128972668698,
+            "heading": 2.3433148642731845,
+            "angularVelocity": -3.457011933465413,
+            "velocityX": -1.8117561388657581,
+            "velocityY": -1.8553907138299572,
+            "timestamp": 0.3333599303069256
+        },
+        {
+            "x": 7.123843168628933,
+            "y": 5.227542500531133,
+            "heading": 2.191575313033137,
+            "angularVelocity": -1.820729337210275,
+            "velocityX": -2.2477837770510787,
+            "velocityY": -2.21486093985698,
+            "timestamp": 0.41669991288365704
+        },
+        {
+            "x": 6.902601176392792,
+            "y": 5.010431192670728,
+            "heading": 2.175785118248912,
+            "angularVelocity": -0.1894672196467848,
+            "velocityX": -2.6546920865077652,
+            "velocityY": -2.6051278287766304,
+            "timestamp": 0.5000398954603884
+        },
+        {
+            "x": 6.682324869385203,
+            "y": 4.7848793693995555,
+            "heading": 2.175784967765777,
+            "angularVelocity": -0.0000018056535426780284,
+            "velocityX": -2.643104788326184,
+            "velocityY": -2.7064059326328107,
+            "timestamp": 0.5833798780371198
+        },
+        {
+            "x": 6.427295318194512,
+            "y": 4.59952808942922,
+            "heading": 2.175784213925318,
+            "angularVelocity": -0.00000904536377316099,
+            "velocityX": -3.060110445258255,
+            "velocityY": -2.224037901611934,
+            "timestamp": 0.6667198606138511
+        },
+        {
+            "x": 6.1722637689115825,
+            "y": 4.4141795587676,
+            "heading": 2.1757834600140606,
+            "angularVelocity": -0.000009046213285018581,
+            "velocityX": -3.0601344204519796,
+            "velocityY": -2.224004912539731,
+            "timestamp": 0.7500598431905825
+        },
+        {
+            "x": 5.906721685974762,
+            "y": 4.244230836092675,
+            "heading": 2.175782696018566,
+            "angularVelocity": -0.000009167214470514032,
+            "velocityX": -3.1862507613597577,
+            "velocityY": -2.039221960701194,
+            "timestamp": 0.8333998257673139
+        },
+        {
+            "x": 5.614573321214709,
+            "y": 4.1257208216835135,
+            "heading": 2.175782627659757,
+            "angularVelocity": -8.202402673559022e-7,
+            "velocityX": -3.5055006699943965,
+            "velocityY": -1.4220067096852238,
+            "timestamp": 0.9167398083440452
+        },
+        {
+            "x": 5.32091408424666,
+            "y": 4.0110057741909,
+            "heading": 2.1757827056250556,
+            "angularVelocity": 9.355089415031885e-7,
+            "velocityX": -3.523629689959162,
+            "velocityY": -1.3764707400435987,
+            "timestamp": 1.0000797909207766
+        },
+        {
+            "x": 5.051276753694785,
+            "y": 3.8476320535360697,
+            "heading": 2.1757829434200273,
+            "angularVelocity": 0.0000028533119900283243,
+            "velocityX": -3.235389811890331,
+            "velocityY": -1.9603282314633543,
+            "timestamp": 1.083419773497508
+        },
+        {
+            "x": 4.804772075990701,
+            "y": 3.6532031340051674,
+            "heading": 2.180086334431566,
+            "angularVelocity": 0.05163657200885012,
+            "velocityX": -2.957820125257733,
+            "velocityY": -2.332960885273652,
+            "timestamp": 1.1667597560742393
+        },
+        {
+            "x": 4.605798608727452,
+            "y": 3.4589597133454815,
+            "heading": 2.2977487638281207,
+            "angularVelocity": 1.411836501024247,
+            "velocityX": -2.387491107045202,
+            "velocityY": -2.330735076418397,
+            "timestamp": 1.2500997386509707
+        },
+        {
+            "x": 4.462499334629702,
+            "y": 3.280406115781069,
+            "heading": 2.4052460747163282,
+            "angularVelocity": 1.28986481115753,
+            "velocityX": -1.7194540923477422,
+            "velocityY": -2.142472220941705,
+            "timestamp": 1.333439721227702
+        },
+        {
+            "x": 4.360731419840454,
+            "y": 3.1416564242722327,
+            "heading": 2.488750866660265,
+            "angularVelocity": 1.0019775546155592,
+            "velocityX": -1.2211175433778025,
+            "velocityY": -1.6648634571178227,
+            "timestamp": 1.4167797038044334
+        },
+        {
+            "x": 4.295085549107569,
+            "y": 3.0475525606989393,
+            "heading": 2.5453815690352877,
+            "angularVelocity": 0.6795142094358206,
+            "velocityX": -0.7876875984758661,
+            "velocityY": -1.129156266461315,
+            "timestamp": 1.5001196863811648
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0.3433937718713025,
+            "velocityX": -0.3849958701158514,
+            "velocityY": -0.5705852008687156,
+            "timestamp": 1.5834596689578961
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -1.6359372913753935e-30,
+            "velocityX": 0,
+            "velocityY": -3.7530428699783824e-32,
+            "timestamp": 1.6667996515346275
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.1.traj
new file mode 100644
index 00000000..16009ccb
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": -3.1814997382550796e-29,
+            "velocityX": -1.8229862739599686e-32,
+            "velocityY": -3.244948758863147e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 7.617836558696546,
+            "y": 5.830157448001668,
+            "heading": 3.1302887872729133,
+            "angularVelocity": -0.10560741421513488,
+            "velocityX": -0.676014466912156,
+            "velocityY": 0.36058033572491816,
+            "timestamp": 0.0919557854839981
+        },
+        {
+            "x": 7.493509680194616,
+            "y": 5.896472329123028,
+            "heading": 3.1108653372490442,
+            "angularVelocity": -0.21122597041214858,
+            "velocityX": -1.3520288891835412,
+            "velocityY": 0.7211605096114337,
+            "timestamp": 0.1839115709679962
+        },
+        {
+            "x": 7.307019376427131,
+            "y": 5.9959446237909395,
+            "heading": 3.0817278950569205,
+            "angularVelocity": -0.316863610470641,
+            "velocityX": -2.028043181686891,
+            "velocityY": 1.0817404706441378,
+            "timestamp": 0.27586735645199434
+        },
+        {
+            "x": 7.058365675689159,
+            "y": 6.128574306111159,
+            "heading": 3.0428740808433132,
+            "angularVelocity": -0.4225271309369522,
+            "velocityX": -2.704057166487276,
+            "velocityY": 1.442320150082345,
+            "timestamp": 0.3678231419359924
+        },
+        {
+            "x": 6.75143523206277,
+            "y": 6.292289169474125,
+            "heading": 3.0428740759198862,
+            "angularVelocity": -5.354123851149769e-8,
+            "velocityX": -3.3378045982740363,
+            "velocityY": 1.7803650145694723,
+            "timestamp": 0.4597789274199905
+        },
+        {
+            "x": 6.443010139077041,
+            "y": 6.453170540992539,
+            "heading": 3.042874070986951,
+            "angularVelocity": -5.3644645461672824e-8,
+            "velocityX": -3.354058598514181,
+            "velocityY": 1.7495513813691466,
+            "timestamp": 0.5517347129039886
+        },
+        {
+            "x": 6.108481192244814,
+            "y": 6.548560466077805,
+            "heading": 3.0428740880877947,
+            "angularVelocity": 1.8596810871163974e-7,
+            "velocityX": -3.637932568043149,
+            "velocityY": 1.037345552356419,
+            "timestamp": 0.6436904983879866
+        },
+        {
+            "x": 5.76231566928878,
+            "y": 6.582886564044467,
+            "heading": 3.0428741422485728,
+            "angularVelocity": 5.889871735366024e-7,
+            "velocityX": -3.764477907877501,
+            "velocityY": 0.3732891605024142,
+            "timestamp": 0.7356462838719847
+        },
+        {
+            "x": 5.41614197674267,
+            "y": 6.617130174737203,
+            "heading": 3.042874196415433,
+            "angularVelocity": 5.890533147506573e-7,
+            "velocityX": -3.764566750466717,
+            "velocityY": 0.37239212859200393,
+            "timestamp": 0.8276020693559828
+        },
+        {
+            "x": 5.068621146465333,
+            "y": 6.632561444279093,
+            "heading": 3.0428742397901987,
+            "angularVelocity": 4.716915377702693e-7,
+            "velocityX": -3.779216592498277,
+            "velocityY": 0.16781183979527767,
+            "timestamp": 0.9195578548399809
+        },
+        {
+            "x": 4.721100288864093,
+            "y": 6.647992098459583,
+            "heading": 3.042874283164768,
+            "angularVelocity": 4.716894023860314e-7,
+            "velocityX": -3.779216889640001,
+            "velocityY": 0.16780514786832676,
+            "timestamp": 1.011513640323979
+        },
+        {
+            "x": 4.3735794260416885,
+            "y": 6.663422635051171,
+            "heading": 3.042874326539347,
+            "angularVelocity": 4.716895035423872e-7,
+            "velocityX": -3.779216946419108,
+            "velocityY": 0.16780386911352246,
+            "timestamp": 1.1034694258079771
+        },
+        {
+            "x": 4.026058559390409,
+            "y": 6.678853085409858,
+            "heading": 3.042874369913944,
+            "angularVelocity": 4.716897058235115e-7,
+            "velocityX": -3.779216988057318,
+            "velocityY": 0.1678029313486884,
+            "timestamp": 1.1954252112919752
+        },
+        {
+            "x": 3.678537689463977,
+            "y": 6.69428346200604,
+            "heading": 3.042874413288561,
+            "angularVelocity": 4.7168991769457604e-7,
+            "velocityX": -3.779217023673906,
+            "velocityY": 0.16780212919682533,
+            "timestamp": 1.2873809967759733
+        },
+        {
+            "x": 3.3310168167092282,
+            "y": 6.7097137749028235,
+            "heading": 3.042874456663209,
+            "angularVelocity": 4.7169025361622227e-7,
+            "velocityX": -3.7792170544312707,
+            "velocityY": 0.16780143647915202,
+            "timestamp": 1.3793367822599714
+        },
+        {
+            "x": 2.9915174484895957,
+            "y": 6.725380176084037,
+            "heading": 3.069608362610464,
+            "angularVelocity": 0.2907256547975106,
+            "velocityX": -3.6919848646033437,
+            "velocityY": 0.1703688473624042,
+            "timestamp": 1.4712925677439694
+        },
+        {
+            "x": 2.7138500007590802,
+            "y": 6.7384126132729225,
+            "heading": 3.2277640151767573,
+            "angularVelocity": 1.7199097559099776,
+            "velocityX": -3.019575617445345,
+            "velocityY": 0.14172503796570737,
+            "timestamp": 1.5632483532279675
+        },
+        {
+            "x": 2.5064920875678816,
+            "y": 6.747862712816966,
+            "heading": 3.3721576212980104,
+            "angularVelocity": 1.570250369362352,
+            "velocityX": -2.2549740845537376,
+            "velocityY": 0.1027678627756077,
+            "timestamp": 1.6552041387119656
+        },
+        {
+            "x": 2.368714185355249,
+            "y": 6.753998249997193,
+            "heading": 3.477486882189963,
+            "angularVelocity": 1.14543375751254,
+            "velocityX": -1.4983059683243996,
+            "velocityY": 0.06672268795197085,
+            "timestamp": 1.7471599241959637
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 0.6057087620584267,
+            "velocityX": -0.7472524430472809,
+            "velocityY": 0.03264340559983963,
+            "timestamp": 1.8391157096799617
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": -1.2870903265044466e-29,
+            "velocityX": 0,
+            "velocityY": 1.2655145174510225e-29,
+            "timestamp": 1.9310714951639598
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.traj
new file mode 100644
index 00000000..16009ccb
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": -3.1814997382550796e-29,
+            "velocityX": -1.8229862739599686e-32,
+            "velocityY": -3.244948758863147e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 7.617836558696546,
+            "y": 5.830157448001668,
+            "heading": 3.1302887872729133,
+            "angularVelocity": -0.10560741421513488,
+            "velocityX": -0.676014466912156,
+            "velocityY": 0.36058033572491816,
+            "timestamp": 0.0919557854839981
+        },
+        {
+            "x": 7.493509680194616,
+            "y": 5.896472329123028,
+            "heading": 3.1108653372490442,
+            "angularVelocity": -0.21122597041214858,
+            "velocityX": -1.3520288891835412,
+            "velocityY": 0.7211605096114337,
+            "timestamp": 0.1839115709679962
+        },
+        {
+            "x": 7.307019376427131,
+            "y": 5.9959446237909395,
+            "heading": 3.0817278950569205,
+            "angularVelocity": -0.316863610470641,
+            "velocityX": -2.028043181686891,
+            "velocityY": 1.0817404706441378,
+            "timestamp": 0.27586735645199434
+        },
+        {
+            "x": 7.058365675689159,
+            "y": 6.128574306111159,
+            "heading": 3.0428740808433132,
+            "angularVelocity": -0.4225271309369522,
+            "velocityX": -2.704057166487276,
+            "velocityY": 1.442320150082345,
+            "timestamp": 0.3678231419359924
+        },
+        {
+            "x": 6.75143523206277,
+            "y": 6.292289169474125,
+            "heading": 3.0428740759198862,
+            "angularVelocity": -5.354123851149769e-8,
+            "velocityX": -3.3378045982740363,
+            "velocityY": 1.7803650145694723,
+            "timestamp": 0.4597789274199905
+        },
+        {
+            "x": 6.443010139077041,
+            "y": 6.453170540992539,
+            "heading": 3.042874070986951,
+            "angularVelocity": -5.3644645461672824e-8,
+            "velocityX": -3.354058598514181,
+            "velocityY": 1.7495513813691466,
+            "timestamp": 0.5517347129039886
+        },
+        {
+            "x": 6.108481192244814,
+            "y": 6.548560466077805,
+            "heading": 3.0428740880877947,
+            "angularVelocity": 1.8596810871163974e-7,
+            "velocityX": -3.637932568043149,
+            "velocityY": 1.037345552356419,
+            "timestamp": 0.6436904983879866
+        },
+        {
+            "x": 5.76231566928878,
+            "y": 6.582886564044467,
+            "heading": 3.0428741422485728,
+            "angularVelocity": 5.889871735366024e-7,
+            "velocityX": -3.764477907877501,
+            "velocityY": 0.3732891605024142,
+            "timestamp": 0.7356462838719847
+        },
+        {
+            "x": 5.41614197674267,
+            "y": 6.617130174737203,
+            "heading": 3.042874196415433,
+            "angularVelocity": 5.890533147506573e-7,
+            "velocityX": -3.764566750466717,
+            "velocityY": 0.37239212859200393,
+            "timestamp": 0.8276020693559828
+        },
+        {
+            "x": 5.068621146465333,
+            "y": 6.632561444279093,
+            "heading": 3.0428742397901987,
+            "angularVelocity": 4.716915377702693e-7,
+            "velocityX": -3.779216592498277,
+            "velocityY": 0.16781183979527767,
+            "timestamp": 0.9195578548399809
+        },
+        {
+            "x": 4.721100288864093,
+            "y": 6.647992098459583,
+            "heading": 3.042874283164768,
+            "angularVelocity": 4.716894023860314e-7,
+            "velocityX": -3.779216889640001,
+            "velocityY": 0.16780514786832676,
+            "timestamp": 1.011513640323979
+        },
+        {
+            "x": 4.3735794260416885,
+            "y": 6.663422635051171,
+            "heading": 3.042874326539347,
+            "angularVelocity": 4.716895035423872e-7,
+            "velocityX": -3.779216946419108,
+            "velocityY": 0.16780386911352246,
+            "timestamp": 1.1034694258079771
+        },
+        {
+            "x": 4.026058559390409,
+            "y": 6.678853085409858,
+            "heading": 3.042874369913944,
+            "angularVelocity": 4.716897058235115e-7,
+            "velocityX": -3.779216988057318,
+            "velocityY": 0.1678029313486884,
+            "timestamp": 1.1954252112919752
+        },
+        {
+            "x": 3.678537689463977,
+            "y": 6.69428346200604,
+            "heading": 3.042874413288561,
+            "angularVelocity": 4.7168991769457604e-7,
+            "velocityX": -3.779217023673906,
+            "velocityY": 0.16780212919682533,
+            "timestamp": 1.2873809967759733
+        },
+        {
+            "x": 3.3310168167092282,
+            "y": 6.7097137749028235,
+            "heading": 3.042874456663209,
+            "angularVelocity": 4.7169025361622227e-7,
+            "velocityX": -3.7792170544312707,
+            "velocityY": 0.16780143647915202,
+            "timestamp": 1.3793367822599714
+        },
+        {
+            "x": 2.9915174484895957,
+            "y": 6.725380176084037,
+            "heading": 3.069608362610464,
+            "angularVelocity": 0.2907256547975106,
+            "velocityX": -3.6919848646033437,
+            "velocityY": 0.1703688473624042,
+            "timestamp": 1.4712925677439694
+        },
+        {
+            "x": 2.7138500007590802,
+            "y": 6.7384126132729225,
+            "heading": 3.2277640151767573,
+            "angularVelocity": 1.7199097559099776,
+            "velocityX": -3.019575617445345,
+            "velocityY": 0.14172503796570737,
+            "timestamp": 1.5632483532279675
+        },
+        {
+            "x": 2.5064920875678816,
+            "y": 6.747862712816966,
+            "heading": 3.3721576212980104,
+            "angularVelocity": 1.570250369362352,
+            "velocityX": -2.2549740845537376,
+            "velocityY": 0.1027678627756077,
+            "timestamp": 1.6552041387119656
+        },
+        {
+            "x": 2.368714185355249,
+            "y": 6.753998249997193,
+            "heading": 3.477486882189963,
+            "angularVelocity": 1.14543375751254,
+            "velocityX": -1.4983059683243996,
+            "velocityY": 0.06672268795197085,
+            "timestamp": 1.7471599241959637
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 0.6057087620584267,
+            "velocityX": -0.7472524430472809,
+            "velocityY": 0.03264340559983963,
+            "timestamp": 1.8391157096799617
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": -1.2870903265044466e-29,
+            "velocityX": 0,
+            "velocityY": 1.2655145174510225e-29,
+            "timestamp": 1.9310714951639598
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.1.traj
new file mode 100644
index 00000000..66484d18
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.1.traj	
@@ -0,0 +1,184 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": -1.4170612284410768e-31,
+            "velocityX": -1.067311144931875e-32,
+            "velocityY": -8.392488596494602e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 7.598760740640866,
+            "y": 5.839328600808308,
+            "heading": 3.1292747695902707,
+            "angularVelocity": -0.10229725511121834,
+            "velocityX": -0.7748601122982537,
+            "velocityY": 0.40373022396426267,
+            "timestamp": 0.10484377511467942
+        },
+        {
+            "x": 7.436282224463055,
+            "y": 5.923985787899839,
+            "heading": 3.1078233746588477,
+            "angularVelocity": -0.2046034197830007,
+            "velocityX": -1.5497202003656476,
+            "velocityY": 0.8074603093882498,
+            "timestamp": 0.20968755022935884
+        },
+        {
+            "x": 7.19256446073334,
+            "y": 6.050971542878038,
+            "heading": 3.0756442181443964,
+            "angularVelocity": -0.30692481722690457,
+            "velocityX": -2.3245802000465203,
+            "velocityY": 1.2111902193458808,
+            "timestamp": 0.31453132534403827
+        },
+        {
+            "x": 6.8676074741032105,
+            "y": 6.220285840922999,
+            "heading": 3.032735095907097,
+            "angularVelocity": -0.4092672377579469,
+            "velocityX": -3.099439964601499,
+            "velocityY": 1.614919892571246,
+            "timestamp": 0.4193751004587177
+        },
+        {
+            "x": 6.515870869646968,
+            "y": 6.403553456428483,
+            "heading": 3.032735091775592,
+            "angularVelocity": -3.94062953033438e-8,
+            "velocityX": -3.3548639780617218,
+            "velocityY": 1.7480066442192022,
+            "timestamp": 0.5242188755733971
+        },
+        {
+            "x": 6.1437635937584405,
+            "y": 6.5408190150249235,
+            "heading": 3.032735095078602,
+            "angularVelocity": 3.1504111682057e-8,
+            "velocityX": -3.5491594563579234,
+            "velocityY": 1.3092389934099258,
+            "timestamp": 0.6290626506880765
+        },
+        {
+            "x": 5.750083988314003,
+            "y": 6.589006339833596,
+            "heading": 3.032735167102088,
+            "angularVelocity": 6.86960058704524e-7,
+            "velocityX": -3.754916350673425,
+            "velocityY": 0.45961073755656895,
+            "timestamp": 0.733906425802756
+        },
+        {
+            "x": 5.355289339691467,
+            "y": 6.626991015951962,
+            "heading": 3.032735238530673,
+            "angularVelocity": 6.812858975549085e-7,
+            "velocityX": -3.7655516332820365,
+            "velocityY": 0.36229786724885477,
+            "timestamp": 0.8387502009174355
+        },
+        {
+            "x": 4.960619519356147,
+            "y": 6.587730487852754,
+            "heading": 3.032735268670057,
+            "angularVelocity": 2.874694691913672e-7,
+            "velocityX": -3.76436102099172,
+            "velocityY": -0.3744669443299256,
+            "timestamp": 0.9435939760321149
+        },
+        {
+            "x": 4.585499557692294,
+            "y": 6.45892571115183,
+            "heading": 3.032735273052689,
+            "angularVelocity": 4.180154788476782e-8,
+            "velocityX": -3.577894455379368,
+            "velocityY": -1.2285400498030161,
+            "timestamp": 1.0484377511467944
+        },
+        {
+            "x": 4.215352940634833,
+            "y": 6.316457686051506,
+            "heading": 3.0327352773918506,
+            "angularVelocity": 4.1386927047063915e-8,
+            "velocityX": -3.530458691062866,
+            "velocityY": -1.358860122544141,
+            "timestamp": 1.1532815262614737
+        },
+        {
+            "x": 3.8452063807699624,
+            "y": 6.173989512359033,
+            "heading": 3.0327352817310285,
+            "angularVelocity": 4.138708047875789e-8,
+            "velocityX": -3.530458145559901,
+            "velocityY": -1.3588615398161574,
+            "timestamp": 1.258125301376153
+        },
+        {
+            "x": 3.47505985812915,
+            "y": 6.031521241954782,
+            "heading": 3.0327352860702175,
+            "angularVelocity": 4.138718836240866e-8,
+            "velocityX": -3.530457790516812,
+            "velocityY": -1.3588624622531746,
+            "timestamp": 1.3629690764908324
+        },
+        {
+            "x": 3.1049133713898427,
+            "y": 5.889052878274973,
+            "heading": 3.0327352904094194,
+            "angularVelocity": 4.138730707919649e-8,
+            "velocityX": -3.53045744808823,
+            "velocityY": -1.3588633519155102,
+            "timestamp": 1.4678128516055118
+        },
+        {
+            "x": 2.762948024328836,
+            "y": 5.75743175203549,
+            "heading": 3.07563992564996,
+            "angularVelocity": 0.4092244407797366,
+            "velocityX": -3.2616657182265767,
+            "velocityY": -1.255402393661561,
+            "timestamp": 1.5726566267201911
+        },
+        {
+            "x": 2.5064740111386725,
+            "y": 5.658715887693948,
+            "heading": 3.107819702204835,
+            "angularVelocity": 0.30693073117289776,
+            "velocityX": -2.4462493162768064,
+            "velocityY": -0.9415519827816698,
+            "timestamp": 1.6775004018348705
+        },
+        {
+            "x": 2.3354913370033428,
+            "y": 5.592905299768155,
+            "heading": 3.129273261906622,
+            "angularVelocity": 0.20462406736424307,
+            "velocityX": -1.6308328648821255,
+            "velocityY": -0.6277014334309162,
+            "timestamp": 1.7823441769495498
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0.1023116353989093,
+            "velocityX": -0.8154164318275585,
+            "velocityY": -0.3138507720860239,
+            "timestamp": 1.8871879520642292
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 6.542152862475765e-30,
+            "velocityX": 0,
+            "velocityY": -3.777031189999863e-33,
+            "timestamp": 1.9920317271789085
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.traj
new file mode 100644
index 00000000..66484d18
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.traj	
@@ -0,0 +1,184 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": -1.4170612284410768e-31,
+            "velocityX": -1.067311144931875e-32,
+            "velocityY": -8.392488596494602e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 7.598760740640866,
+            "y": 5.839328600808308,
+            "heading": 3.1292747695902707,
+            "angularVelocity": -0.10229725511121834,
+            "velocityX": -0.7748601122982537,
+            "velocityY": 0.40373022396426267,
+            "timestamp": 0.10484377511467942
+        },
+        {
+            "x": 7.436282224463055,
+            "y": 5.923985787899839,
+            "heading": 3.1078233746588477,
+            "angularVelocity": -0.2046034197830007,
+            "velocityX": -1.5497202003656476,
+            "velocityY": 0.8074603093882498,
+            "timestamp": 0.20968755022935884
+        },
+        {
+            "x": 7.19256446073334,
+            "y": 6.050971542878038,
+            "heading": 3.0756442181443964,
+            "angularVelocity": -0.30692481722690457,
+            "velocityX": -2.3245802000465203,
+            "velocityY": 1.2111902193458808,
+            "timestamp": 0.31453132534403827
+        },
+        {
+            "x": 6.8676074741032105,
+            "y": 6.220285840922999,
+            "heading": 3.032735095907097,
+            "angularVelocity": -0.4092672377579469,
+            "velocityX": -3.099439964601499,
+            "velocityY": 1.614919892571246,
+            "timestamp": 0.4193751004587177
+        },
+        {
+            "x": 6.515870869646968,
+            "y": 6.403553456428483,
+            "heading": 3.032735091775592,
+            "angularVelocity": -3.94062953033438e-8,
+            "velocityX": -3.3548639780617218,
+            "velocityY": 1.7480066442192022,
+            "timestamp": 0.5242188755733971
+        },
+        {
+            "x": 6.1437635937584405,
+            "y": 6.5408190150249235,
+            "heading": 3.032735095078602,
+            "angularVelocity": 3.1504111682057e-8,
+            "velocityX": -3.5491594563579234,
+            "velocityY": 1.3092389934099258,
+            "timestamp": 0.6290626506880765
+        },
+        {
+            "x": 5.750083988314003,
+            "y": 6.589006339833596,
+            "heading": 3.032735167102088,
+            "angularVelocity": 6.86960058704524e-7,
+            "velocityX": -3.754916350673425,
+            "velocityY": 0.45961073755656895,
+            "timestamp": 0.733906425802756
+        },
+        {
+            "x": 5.355289339691467,
+            "y": 6.626991015951962,
+            "heading": 3.032735238530673,
+            "angularVelocity": 6.812858975549085e-7,
+            "velocityX": -3.7655516332820365,
+            "velocityY": 0.36229786724885477,
+            "timestamp": 0.8387502009174355
+        },
+        {
+            "x": 4.960619519356147,
+            "y": 6.587730487852754,
+            "heading": 3.032735268670057,
+            "angularVelocity": 2.874694691913672e-7,
+            "velocityX": -3.76436102099172,
+            "velocityY": -0.3744669443299256,
+            "timestamp": 0.9435939760321149
+        },
+        {
+            "x": 4.585499557692294,
+            "y": 6.45892571115183,
+            "heading": 3.032735273052689,
+            "angularVelocity": 4.180154788476782e-8,
+            "velocityX": -3.577894455379368,
+            "velocityY": -1.2285400498030161,
+            "timestamp": 1.0484377511467944
+        },
+        {
+            "x": 4.215352940634833,
+            "y": 6.316457686051506,
+            "heading": 3.0327352773918506,
+            "angularVelocity": 4.1386927047063915e-8,
+            "velocityX": -3.530458691062866,
+            "velocityY": -1.358860122544141,
+            "timestamp": 1.1532815262614737
+        },
+        {
+            "x": 3.8452063807699624,
+            "y": 6.173989512359033,
+            "heading": 3.0327352817310285,
+            "angularVelocity": 4.138708047875789e-8,
+            "velocityX": -3.530458145559901,
+            "velocityY": -1.3588615398161574,
+            "timestamp": 1.258125301376153
+        },
+        {
+            "x": 3.47505985812915,
+            "y": 6.031521241954782,
+            "heading": 3.0327352860702175,
+            "angularVelocity": 4.138718836240866e-8,
+            "velocityX": -3.530457790516812,
+            "velocityY": -1.3588624622531746,
+            "timestamp": 1.3629690764908324
+        },
+        {
+            "x": 3.1049133713898427,
+            "y": 5.889052878274973,
+            "heading": 3.0327352904094194,
+            "angularVelocity": 4.138730707919649e-8,
+            "velocityX": -3.53045744808823,
+            "velocityY": -1.3588633519155102,
+            "timestamp": 1.4678128516055118
+        },
+        {
+            "x": 2.762948024328836,
+            "y": 5.75743175203549,
+            "heading": 3.07563992564996,
+            "angularVelocity": 0.4092244407797366,
+            "velocityX": -3.2616657182265767,
+            "velocityY": -1.255402393661561,
+            "timestamp": 1.5726566267201911
+        },
+        {
+            "x": 2.5064740111386725,
+            "y": 5.658715887693948,
+            "heading": 3.107819702204835,
+            "angularVelocity": 0.30693073117289776,
+            "velocityX": -2.4462493162768064,
+            "velocityY": -0.9415519827816698,
+            "timestamp": 1.6775004018348705
+        },
+        {
+            "x": 2.3354913370033428,
+            "y": 5.592905299768155,
+            "heading": 3.129273261906622,
+            "angularVelocity": 0.20462406736424307,
+            "velocityX": -1.6308328648821255,
+            "velocityY": -0.6277014334309162,
+            "timestamp": 1.7823441769495498
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0.1023116353989093,
+            "velocityX": -0.8154164318275585,
+            "velocityY": -0.3138507720860239,
+            "timestamp": 1.8871879520642292
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 6.542152862475765e-30,
+            "velocityX": 0,
+            "velocityY": -3.777031189999863e-33,
+            "timestamp": 1.9920317271789085
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.1.traj
new file mode 100644
index 00000000..4813b980
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.1.traj	
@@ -0,0 +1,211 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 1.7201358648458989e-31,
+            "velocityX": -4.9530751339705636e-33,
+            "velocityY": 8.276147677005227e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 7.605093588171019,
+            "y": 5.8310278116994025,
+            "heading": 3.10842986087817,
+            "angularVelocity": -0.3169118675748513,
+            "velocityX": -0.7519362133453025,
+            "velocityY": 0.3415828265288231,
+            "timestamp": 0.0996180400671603
+        },
+        {
+            "x": 7.455282578249206,
+            "y": 5.899083098798643,
+            "heading": 3.0452020176570493,
+            "angularVelocity": -0.6347027423797291,
+            "velocityX": -1.5038542197860265,
+            "velocityY": 0.6831622771674665,
+            "timestamp": 0.1992360801343206
+        },
+        {
+            "x": 7.230570445252572,
+            "y": 6.001166640275022,
+            "heading": 2.9501837123513917,
+            "angularVelocity": -0.9538262873029698,
+            "velocityX": -2.2557373428059657,
+            "velocityY": 1.0247495474469857,
+            "timestamp": 0.2988541202014809
+        },
+        {
+            "x": 6.930961848120999,
+            "y": 6.137281948011585,
+            "heading": 2.82324695361152,
+            "angularVelocity": -1.274234653224393,
+            "velocityX": -3.007573697791919,
+            "velocityY": 1.3663720712111798,
+            "timestamp": 0.3984721602686412
+        },
+        {
+            "x": 6.58788061707114,
+            "y": 6.293200302520464,
+            "heading": 2.8232469405560607,
+            "angularVelocity": -1.3105517366390336e-7,
+            "velocityX": -3.4439668840961075,
+            "velocityY": 1.565161836187118,
+            "timestamp": 0.4980902003358015
+        },
+        {
+            "x": 6.242826706460482,
+            "y": 6.444702986624836,
+            "heading": 2.8232469291979005,
+            "angularVelocity": -1.1401709907689636e-7,
+            "velocityX": -3.46376931706376,
+            "velocityY": 1.5208358245377296,
+            "timestamp": 0.5977082404029618
+        },
+        {
+            "x": 5.895869664946667,
+            "y": 6.591795052657303,
+            "heading": 2.823246919432399,
+            "angularVelocity": -9.802945216969962e-8,
+            "velocityX": -3.482873596789339,
+            "velocityY": 1.4765605299331361,
+            "timestamp": 0.6973262804701221
+        },
+        {
+            "x": 5.529327027819563,
+            "y": 6.679326525812769,
+            "heading": 2.8232469090031302,
+            "angularVelocity": -1.0469256638864389e-7,
+            "velocityX": -3.679480512565681,
+            "velocityY": 0.8786709023431315,
+            "timestamp": 0.7969443205372824
+        },
+        {
+            "x": 5.152516966350426,
+            "y": 6.684752095540444,
+            "heading": 2.823246873859642,
+            "angularVelocity": -3.5278236840852576e-7,
+            "velocityX": -3.7825484341500837,
+            "velocityY": 0.05446372689140277,
+            "timestamp": 0.8965623606044426
+        },
+        {
+            "x": 4.783606202844696,
+            "y": 6.607810054904123,
+            "heading": 2.823246710781253,
+            "angularVelocity": -0.0000016370367136474411,
+            "velocityX": -3.703252576109883,
+            "velocityY": -0.7723705524064612,
+            "timestamp": 0.9961804006716029
+        },
+        {
+            "x": 4.4403804832702125,
+            "y": 6.452210088653612,
+            "heading": 2.8232466000560583,
+            "angularVelocity": -0.0000011114974222563995,
+            "velocityX": -3.445417309385819,
+            "velocityY": -1.5619657458188145,
+            "timestamp": 1.095798440738763
+        },
+        {
+            "x": 4.124524669473561,
+            "y": 6.246661186403647,
+            "heading": 2.823246495691381,
+            "angularVelocity": -0.0000010476483740088647,
+            "velocityX": -3.170668822471396,
+            "velocityY": -2.0633702701979306,
+            "timestamp": 1.1954164808059233
+        },
+        {
+            "x": 3.808669145932633,
+            "y": 6.041111838134177,
+            "heading": 2.82324639132671,
+            "angularVelocity": -0.0000010476483076103719,
+            "velocityX": -3.1706659087850397,
+            "velocityY": -2.063374747494463,
+            "timestamp": 1.2950345208730836
+        },
+        {
+            "x": 3.492813679718565,
+            "y": 5.835562401773946,
+            "heading": 2.8232462869620267,
+            "angularVelocity": -0.0000010476484251301315,
+            "velocityX": -3.1706653333184023,
+            "velocityY": -2.063375631779686,
+            "timestamp": 1.3946525609402438
+        },
+        {
+            "x": 3.176958352678971,
+            "y": 5.630012751552842,
+            "heading": 2.8232461825973036,
+            "angularVelocity": -0.000001047648830467493,
+            "velocityX": -3.1706639362373616,
+            "velocityY": -2.0633777785883685,
+            "timestamp": 1.494270601007404
+        },
+        {
+            "x": 2.880984417024366,
+            "y": 5.396743220702011,
+            "heading": 2.8232460713596375,
+            "angularVelocity": -0.0000011166417889330504,
+            "velocityX": -2.971087721210592,
+            "velocityY": -2.3416394329136074,
+            "timestamp": 1.5938886410745643
+        },
+        {
+            "x": 2.6437210391842343,
+            "y": 5.10516988637377,
+            "heading": 2.820727987751454,
+            "angularVelocity": -0.02527738556676537,
+            "velocityX": -2.3817310366694042,
+            "velocityY": -2.926912978127948,
+            "timestamp": 1.6935066811417245
+        },
+        {
+            "x": 2.4878211519832623,
+            "y": 4.818636614610494,
+            "heading": 2.7673750452255876,
+            "angularVelocity": -0.5355751075798869,
+            "velocityX": -1.5649764550262995,
+            "velocityY": -2.876319104151233,
+            "timestamp": 1.7931247212088848
+        },
+        {
+            "x": 2.3880615291520813,
+            "y": 4.591997027963517,
+            "heading": 2.751203678024379,
+            "angularVelocity": -0.1623337217867963,
+            "velocityX": -1.0014212562696991,
+            "velocityY": -2.275085782597012,
+            "timestamp": 1.892742761276045
+        },
+        {
+            "x": 2.3279705887355506,
+            "y": 4.437873884538665,
+            "heading": 2.748674861705227,
+            "angularVelocity": -0.025385124194849635,
+            "velocityX": -0.6032134378072334,
+            "velocityY": -1.547140892562696,
+            "timestamp": 1.9923608013432053
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0.013302191991328724,
+            "velocityX": -0.28077834814551317,
+            "velocityY": -0.7817247206044561,
+            "timestamp": 2.0919788414103655
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": -2.062592729534739e-30,
+            "velocityX": 0,
+            "velocityY": 2.2969932039584724e-33,
+            "timestamp": 2.1915968814775257
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.traj
new file mode 100644
index 00000000..4813b980
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.traj	
@@ -0,0 +1,211 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 1.7201358648458989e-31,
+            "velocityX": -4.9530751339705636e-33,
+            "velocityY": 8.276147677005227e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 7.605093588171019,
+            "y": 5.8310278116994025,
+            "heading": 3.10842986087817,
+            "angularVelocity": -0.3169118675748513,
+            "velocityX": -0.7519362133453025,
+            "velocityY": 0.3415828265288231,
+            "timestamp": 0.0996180400671603
+        },
+        {
+            "x": 7.455282578249206,
+            "y": 5.899083098798643,
+            "heading": 3.0452020176570493,
+            "angularVelocity": -0.6347027423797291,
+            "velocityX": -1.5038542197860265,
+            "velocityY": 0.6831622771674665,
+            "timestamp": 0.1992360801343206
+        },
+        {
+            "x": 7.230570445252572,
+            "y": 6.001166640275022,
+            "heading": 2.9501837123513917,
+            "angularVelocity": -0.9538262873029698,
+            "velocityX": -2.2557373428059657,
+            "velocityY": 1.0247495474469857,
+            "timestamp": 0.2988541202014809
+        },
+        {
+            "x": 6.930961848120999,
+            "y": 6.137281948011585,
+            "heading": 2.82324695361152,
+            "angularVelocity": -1.274234653224393,
+            "velocityX": -3.007573697791919,
+            "velocityY": 1.3663720712111798,
+            "timestamp": 0.3984721602686412
+        },
+        {
+            "x": 6.58788061707114,
+            "y": 6.293200302520464,
+            "heading": 2.8232469405560607,
+            "angularVelocity": -1.3105517366390336e-7,
+            "velocityX": -3.4439668840961075,
+            "velocityY": 1.565161836187118,
+            "timestamp": 0.4980902003358015
+        },
+        {
+            "x": 6.242826706460482,
+            "y": 6.444702986624836,
+            "heading": 2.8232469291979005,
+            "angularVelocity": -1.1401709907689636e-7,
+            "velocityX": -3.46376931706376,
+            "velocityY": 1.5208358245377296,
+            "timestamp": 0.5977082404029618
+        },
+        {
+            "x": 5.895869664946667,
+            "y": 6.591795052657303,
+            "heading": 2.823246919432399,
+            "angularVelocity": -9.802945216969962e-8,
+            "velocityX": -3.482873596789339,
+            "velocityY": 1.4765605299331361,
+            "timestamp": 0.6973262804701221
+        },
+        {
+            "x": 5.529327027819563,
+            "y": 6.679326525812769,
+            "heading": 2.8232469090031302,
+            "angularVelocity": -1.0469256638864389e-7,
+            "velocityX": -3.679480512565681,
+            "velocityY": 0.8786709023431315,
+            "timestamp": 0.7969443205372824
+        },
+        {
+            "x": 5.152516966350426,
+            "y": 6.684752095540444,
+            "heading": 2.823246873859642,
+            "angularVelocity": -3.5278236840852576e-7,
+            "velocityX": -3.7825484341500837,
+            "velocityY": 0.05446372689140277,
+            "timestamp": 0.8965623606044426
+        },
+        {
+            "x": 4.783606202844696,
+            "y": 6.607810054904123,
+            "heading": 2.823246710781253,
+            "angularVelocity": -0.0000016370367136474411,
+            "velocityX": -3.703252576109883,
+            "velocityY": -0.7723705524064612,
+            "timestamp": 0.9961804006716029
+        },
+        {
+            "x": 4.4403804832702125,
+            "y": 6.452210088653612,
+            "heading": 2.8232466000560583,
+            "angularVelocity": -0.0000011114974222563995,
+            "velocityX": -3.445417309385819,
+            "velocityY": -1.5619657458188145,
+            "timestamp": 1.095798440738763
+        },
+        {
+            "x": 4.124524669473561,
+            "y": 6.246661186403647,
+            "heading": 2.823246495691381,
+            "angularVelocity": -0.0000010476483740088647,
+            "velocityX": -3.170668822471396,
+            "velocityY": -2.0633702701979306,
+            "timestamp": 1.1954164808059233
+        },
+        {
+            "x": 3.808669145932633,
+            "y": 6.041111838134177,
+            "heading": 2.82324639132671,
+            "angularVelocity": -0.0000010476483076103719,
+            "velocityX": -3.1706659087850397,
+            "velocityY": -2.063374747494463,
+            "timestamp": 1.2950345208730836
+        },
+        {
+            "x": 3.492813679718565,
+            "y": 5.835562401773946,
+            "heading": 2.8232462869620267,
+            "angularVelocity": -0.0000010476484251301315,
+            "velocityX": -3.1706653333184023,
+            "velocityY": -2.063375631779686,
+            "timestamp": 1.3946525609402438
+        },
+        {
+            "x": 3.176958352678971,
+            "y": 5.630012751552842,
+            "heading": 2.8232461825973036,
+            "angularVelocity": -0.000001047648830467493,
+            "velocityX": -3.1706639362373616,
+            "velocityY": -2.0633777785883685,
+            "timestamp": 1.494270601007404
+        },
+        {
+            "x": 2.880984417024366,
+            "y": 5.396743220702011,
+            "heading": 2.8232460713596375,
+            "angularVelocity": -0.0000011166417889330504,
+            "velocityX": -2.971087721210592,
+            "velocityY": -2.3416394329136074,
+            "timestamp": 1.5938886410745643
+        },
+        {
+            "x": 2.6437210391842343,
+            "y": 5.10516988637377,
+            "heading": 2.820727987751454,
+            "angularVelocity": -0.02527738556676537,
+            "velocityX": -2.3817310366694042,
+            "velocityY": -2.926912978127948,
+            "timestamp": 1.6935066811417245
+        },
+        {
+            "x": 2.4878211519832623,
+            "y": 4.818636614610494,
+            "heading": 2.7673750452255876,
+            "angularVelocity": -0.5355751075798869,
+            "velocityX": -1.5649764550262995,
+            "velocityY": -2.876319104151233,
+            "timestamp": 1.7931247212088848
+        },
+        {
+            "x": 2.3880615291520813,
+            "y": 4.591997027963517,
+            "heading": 2.751203678024379,
+            "angularVelocity": -0.1623337217867963,
+            "velocityX": -1.0014212562696991,
+            "velocityY": -2.275085782597012,
+            "timestamp": 1.892742761276045
+        },
+        {
+            "x": 2.3279705887355506,
+            "y": 4.437873884538665,
+            "heading": 2.748674861705227,
+            "angularVelocity": -0.025385124194849635,
+            "velocityX": -0.6032134378072334,
+            "velocityY": -1.547140892562696,
+            "timestamp": 1.9923608013432053
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0.013302191991328724,
+            "velocityX": -0.28077834814551317,
+            "velocityY": -0.7817247206044561,
+            "timestamp": 2.0919788414103655
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": -2.062592729534739e-30,
+            "velocityX": 0,
+            "velocityY": 2.2969932039584724e-33,
+            "timestamp": 2.1915968814775257
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.1.traj
new file mode 100644
index 00000000..e53f5f73
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.1.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 2.0369445518795058e-34,
+            "velocityX": 0,
+            "velocityY": 7.181654629664687e-43,
+            "timestamp": 0
+        },
+        {
+            "x": 7.680000037559162,
+            "y": 4.043499994424155,
+            "heading": 3.140000006758021,
+            "angularVelocity": 6.75321744601227e-8,
+            "velocityX": 3.753246582974315e-7,
+            "velocityY": -0.8344065481227707,
+            "timestamp": 0.10007112931184514
+        },
+        {
+            "x": 7.6800001031226985,
+            "y": 3.8764999863701557,
+            "heading": 3.1400000189870485,
+            "angularVelocity": 1.2220335610640094e-7,
+            "velocityX": 6.551693493497348e-7,
+            "velocityY": -1.6688130652906727,
+            "timestamp": 0.20014225862369028
+        },
+        {
+            "x": 7.680000187135912,
+            "y": 3.625999982033383,
+            "heading": 3.1400000350867425,
+            "angularVelocity": 1.6088250383461684e-7,
+            "velocityX": 8.39534968426326e-7,
+            "velocityY": -2.503219520548785,
+            "timestamp": 0.3002133879355354
+        },
+        {
+            "x": 7.680000280051002,
+            "y": 3.2919999999999914,
+            "heading": 3.1400000531130368,
+            "angularVelocity": 1.801348130208338e-7,
+            "velocityX": 9.284904765306952e-7,
+            "velocityY": -3.3376257900774653,
+            "timestamp": 0.40028451724738057
+        },
+        {
+            "x": 7.680000185977535,
+            "y": 2.958000017966607,
+            "heading": 3.1400000281215186,
+            "angularVelocity": -2.4973754429186553e-7,
+            "velocityX": -9.400660019914908e-7,
+            "velocityY": -3.337625790077398,
+            "timestamp": 0.5003556465592257
+        },
+        {
+            "x": 7.680000101870251,
+            "y": 2.7075000136298395,
+            "heading": 3.1400000109296125,
+            "angularVelocity": -1.7179686113327866e-7,
+            "velocityX": -8.404750236739033e-7,
+            "velocityY": -2.503219520548736,
+            "timestamp": 0.6004267758710709
+        },
+        {
+            "x": 7.6800000368892665,
+            "y": 2.5405000055758427,
+            "heading": 3.140000002248845,
+            "angularVelocity": -8.674597729312873e-8,
+            "velocityX": -6.493479632352893e-7,
+            "velocityY": -1.6688130652906423,
+            "timestamp": 0.700497905182916
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -2.2472459301603394e-8,
+            "velocityX": -3.686304669837251e-7,
+            "velocityY": -0.8344065481227574,
+            "timestamp": 0.8005690344947611
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 7.368205253143154e-34,
+            "velocityX": 0,
+            "velocityY": -2.1562414433932607e-41,
+            "timestamp": 0.9006401638066063
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.traj
new file mode 100644
index 00000000..e53f5f73
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 2.0369445518795058e-34,
+            "velocityX": 0,
+            "velocityY": 7.181654629664687e-43,
+            "timestamp": 0
+        },
+        {
+            "x": 7.680000037559162,
+            "y": 4.043499994424155,
+            "heading": 3.140000006758021,
+            "angularVelocity": 6.75321744601227e-8,
+            "velocityX": 3.753246582974315e-7,
+            "velocityY": -0.8344065481227707,
+            "timestamp": 0.10007112931184514
+        },
+        {
+            "x": 7.6800001031226985,
+            "y": 3.8764999863701557,
+            "heading": 3.1400000189870485,
+            "angularVelocity": 1.2220335610640094e-7,
+            "velocityX": 6.551693493497348e-7,
+            "velocityY": -1.6688130652906727,
+            "timestamp": 0.20014225862369028
+        },
+        {
+            "x": 7.680000187135912,
+            "y": 3.625999982033383,
+            "heading": 3.1400000350867425,
+            "angularVelocity": 1.6088250383461684e-7,
+            "velocityX": 8.39534968426326e-7,
+            "velocityY": -2.503219520548785,
+            "timestamp": 0.3002133879355354
+        },
+        {
+            "x": 7.680000280051002,
+            "y": 3.2919999999999914,
+            "heading": 3.1400000531130368,
+            "angularVelocity": 1.801348130208338e-7,
+            "velocityX": 9.284904765306952e-7,
+            "velocityY": -3.3376257900774653,
+            "timestamp": 0.40028451724738057
+        },
+        {
+            "x": 7.680000185977535,
+            "y": 2.958000017966607,
+            "heading": 3.1400000281215186,
+            "angularVelocity": -2.4973754429186553e-7,
+            "velocityX": -9.400660019914908e-7,
+            "velocityY": -3.337625790077398,
+            "timestamp": 0.5003556465592257
+        },
+        {
+            "x": 7.680000101870251,
+            "y": 2.7075000136298395,
+            "heading": 3.1400000109296125,
+            "angularVelocity": -1.7179686113327866e-7,
+            "velocityX": -8.404750236739033e-7,
+            "velocityY": -2.503219520548736,
+            "timestamp": 0.6004267758710709
+        },
+        {
+            "x": 7.6800000368892665,
+            "y": 2.5405000055758427,
+            "heading": 3.140000002248845,
+            "angularVelocity": -8.674597729312873e-8,
+            "velocityX": -6.493479632352893e-7,
+            "velocityY": -1.6688130652906423,
+            "timestamp": 0.700497905182916
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -2.2472459301603394e-8,
+            "velocityX": -3.686304669837251e-7,
+            "velocityY": -0.8344065481227574,
+            "timestamp": 0.8005690344947611
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 7.368205253143154e-34,
+            "velocityX": 0,
+            "velocityY": -2.1562414433932607e-41,
+            "timestamp": 0.9006401638066063
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.1.traj
new file mode 100644
index 00000000..7ea9ac6c
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.1.traj	
@@ -0,0 +1,139 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 9.207800442360542e-32,
+            "velocityX": -6.095364969086891e-33,
+            "velocityY": -9.366947302592518e-40,
+            "timestamp": 0
+        },
+        {
+            "x": 7.680000070435844,
+            "y": 4.0505546279944795,
+            "heading": 3.1399999875764033,
+            "angularVelocity": -1.2974964830179728e-7,
+            "velocityX": 7.356183706178639e-7,
+            "velocityY": -0.7983807350945191,
+            "timestamp": 0.0957505218315028
+        },
+        {
+            "x": 7.680000202739516,
+            "y": 3.8976638870173534,
+            "heading": 3.1399999616102225,
+            "angularVelocity": -2.711857701387203e-7,
+            "velocityX": 0.0000013817539912349048,
+            "velocityY": -1.5967614385034457,
+            "timestamp": 0.1915010436630056
+        },
+        {
+            "x": 7.680000388342391,
+            "y": 3.6683277831364425,
+            "heading": 3.139999920718594,
+            "angularVelocity": -4.2706425730703774e-7,
+            "velocityX": 0.00000193840056979922,
+            "velocityY": -2.395142078541202,
+            "timestamp": 0.2872515654945084
+        },
+        {
+            "x": 7.680000618679091,
+            "y": 3.362546334555153,
+            "heading": 3.139999863216847,
+            "angularVelocity": -6.005370836912587e-7,
+            "velocityX": 0.000002405591946532482,
+            "velocityY": -3.1935225284661044,
+            "timestamp": 0.3830020873260112
+        },
+        {
+            "x": 7.680000750151485,
+            "y": 3.000327800732898,
+            "heading": 3.139999863216849,
+            "angularVelocity": 2.234907225706593e-14,
+            "velocityX": 0.0000013730725434542363,
+            "velocityY": -3.7829405719549856,
+            "timestamp": 0.478752609157514
+        },
+        {
+            "x": 7.6800008034310885,
+            "y": 2.6381092669108033,
+            "heading": 3.139999863216839,
+            "angularVelocity": -1.1397010502093565e-13,
+            "velocityX": 5.56441925019577e-7,
+            "velocityY": -3.7829405719533264,
+            "timestamp": 0.5745031309890168
+        },
+        {
+            "x": 7.68000078216065,
+            "y": 2.275890733088706,
+            "heading": 3.1399998632168313,
+            "angularVelocity": -8.126780055977152e-14,
+            "velocityX": -2.2214435413644554e-7,
+            "velocityY": -3.7829405719533535,
+            "timestamp": 0.6702536528205196
+        },
+        {
+            "x": 7.6800006913830785,
+            "y": 1.9136721992666206,
+            "heading": 3.1399998632168296,
+            "angularVelocity": -1.6383854112995832e-14,
+            "velocityX": -9.4806343038794e-7,
+            "velocityY": -3.7829405719532305,
+            "timestamp": 0.7660041746520224
+        },
+        {
+            "x": 7.680000537903045,
+            "y": 1.55145366544455,
+            "heading": 3.13999986321682,
+            "angularVelocity": -1.0277752214408611e-13,
+            "velocityX": -0.0000016029159878741863,
+            "velocityY": -3.7829405719530524,
+            "timestamp": 0.8617546964835252
+        },
+        {
+            "x": 7.680000332408874,
+            "y": 1.2456722168634402,
+            "heading": 3.139999917268044,
+            "angularVelocity": 5.645005022977776e-7,
+            "velocityX": -0.0000021461414302094452,
+            "velocityY": -3.1935225284642264,
+            "timestamp": 0.957505218315028
+        },
+        {
+            "x": 7.680000170899522,
+            "y": 1.0163361129826003,
+            "heading": 3.139999958602803,
+            "angularVelocity": 4.3169222333319617e-7,
+            "velocityX": -0.0000016867725067406415,
+            "velocityY": -2.3951420785404567,
+            "timestamp": 1.0532557401465308
+        },
+        {
+            "x": 7.680000058494392,
+            "y": 0.8634453720055071,
+            "heading": 3.1399999862669987,
+            "angularVelocity": 2.889195077791466e-7,
+            "velocityX": -0.0000011739374912229912,
+            "velocityY": -1.5967614385030957,
+            "timestamp": 1.1490062619780337
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 1.4342481443578566e-7,
+            "velocityX": -6.10904148268517e-7,
+            "velocityY": -0.7983807350943894,
+            "timestamp": 1.2447567838095366
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -3.045628877095783e-32,
+            "velocityY": -7.047678165048749e-38,
+            "timestamp": 1.3405073056410395
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.traj
new file mode 100644
index 00000000..7ea9ac6c
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.traj	
@@ -0,0 +1,139 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 9.207800442360542e-32,
+            "velocityX": -6.095364969086891e-33,
+            "velocityY": -9.366947302592518e-40,
+            "timestamp": 0
+        },
+        {
+            "x": 7.680000070435844,
+            "y": 4.0505546279944795,
+            "heading": 3.1399999875764033,
+            "angularVelocity": -1.2974964830179728e-7,
+            "velocityX": 7.356183706178639e-7,
+            "velocityY": -0.7983807350945191,
+            "timestamp": 0.0957505218315028
+        },
+        {
+            "x": 7.680000202739516,
+            "y": 3.8976638870173534,
+            "heading": 3.1399999616102225,
+            "angularVelocity": -2.711857701387203e-7,
+            "velocityX": 0.0000013817539912349048,
+            "velocityY": -1.5967614385034457,
+            "timestamp": 0.1915010436630056
+        },
+        {
+            "x": 7.680000388342391,
+            "y": 3.6683277831364425,
+            "heading": 3.139999920718594,
+            "angularVelocity": -4.2706425730703774e-7,
+            "velocityX": 0.00000193840056979922,
+            "velocityY": -2.395142078541202,
+            "timestamp": 0.2872515654945084
+        },
+        {
+            "x": 7.680000618679091,
+            "y": 3.362546334555153,
+            "heading": 3.139999863216847,
+            "angularVelocity": -6.005370836912587e-7,
+            "velocityX": 0.000002405591946532482,
+            "velocityY": -3.1935225284661044,
+            "timestamp": 0.3830020873260112
+        },
+        {
+            "x": 7.680000750151485,
+            "y": 3.000327800732898,
+            "heading": 3.139999863216849,
+            "angularVelocity": 2.234907225706593e-14,
+            "velocityX": 0.0000013730725434542363,
+            "velocityY": -3.7829405719549856,
+            "timestamp": 0.478752609157514
+        },
+        {
+            "x": 7.6800008034310885,
+            "y": 2.6381092669108033,
+            "heading": 3.139999863216839,
+            "angularVelocity": -1.1397010502093565e-13,
+            "velocityX": 5.56441925019577e-7,
+            "velocityY": -3.7829405719533264,
+            "timestamp": 0.5745031309890168
+        },
+        {
+            "x": 7.68000078216065,
+            "y": 2.275890733088706,
+            "heading": 3.1399998632168313,
+            "angularVelocity": -8.126780055977152e-14,
+            "velocityX": -2.2214435413644554e-7,
+            "velocityY": -3.7829405719533535,
+            "timestamp": 0.6702536528205196
+        },
+        {
+            "x": 7.6800006913830785,
+            "y": 1.9136721992666206,
+            "heading": 3.1399998632168296,
+            "angularVelocity": -1.6383854112995832e-14,
+            "velocityX": -9.4806343038794e-7,
+            "velocityY": -3.7829405719532305,
+            "timestamp": 0.7660041746520224
+        },
+        {
+            "x": 7.680000537903045,
+            "y": 1.55145366544455,
+            "heading": 3.13999986321682,
+            "angularVelocity": -1.0277752214408611e-13,
+            "velocityX": -0.0000016029159878741863,
+            "velocityY": -3.7829405719530524,
+            "timestamp": 0.8617546964835252
+        },
+        {
+            "x": 7.680000332408874,
+            "y": 1.2456722168634402,
+            "heading": 3.139999917268044,
+            "angularVelocity": 5.645005022977776e-7,
+            "velocityX": -0.0000021461414302094452,
+            "velocityY": -3.1935225284642264,
+            "timestamp": 0.957505218315028
+        },
+        {
+            "x": 7.680000170899522,
+            "y": 1.0163361129826003,
+            "heading": 3.139999958602803,
+            "angularVelocity": 4.3169222333319617e-7,
+            "velocityX": -0.0000016867725067406415,
+            "velocityY": -2.3951420785404567,
+            "timestamp": 1.0532557401465308
+        },
+        {
+            "x": 7.680000058494392,
+            "y": 0.8634453720055071,
+            "heading": 3.1399999862669987,
+            "angularVelocity": 2.889195077791466e-7,
+            "velocityX": -0.0000011739374912229912,
+            "velocityY": -1.5967614385030957,
+            "timestamp": 1.1490062619780337
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 1.4342481443578566e-7,
+            "velocityX": -6.10904148268517e-7,
+            "velocityY": -0.7983807350943894,
+            "timestamp": 1.2447567838095366
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -3.045628877095783e-32,
+            "velocityY": -7.047678165048749e-38,
+            "timestamp": 1.3405073056410395
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.1.traj
new file mode 100644
index 00000000..626b4f65
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.1.traj	
@@ -0,0 +1,157 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": -7.693901589446952e-31,
+            "velocityX": -6.624376358473947e-32,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.653617802747577,
+            "y": 4.193887846119818,
+            "heading": 3.0831794581771232,
+            "angularVelocity": -0.6046893294524585,
+            "velocityX": -0.2807617219803893,
+            "velocityY": 0.7118264895254428,
+            "timestamp": 0.0939665032196413
+        },
+        {
+            "x": 7.6005882437737835,
+            "y": 4.328100423051846,
+            "heading": 2.9787754661598114,
+            "angularVelocity": -1.1110766969084016,
+            "velocityX": -0.5643453481485778,
+            "velocityY": 1.4283023453401595,
+            "timestamp": 0.1879330064392826
+        },
+        {
+            "x": 7.520633934300323,
+            "y": 4.5302283764630555,
+            "heading": 2.841927884917239,
+            "angularVelocity": -1.4563442988050657,
+            "velocityX": -0.8508809707068956,
+            "velocityY": 2.1510639055999357,
+            "timestamp": 0.2818995096589239
+        },
+        {
+            "x": 7.4136169796865685,
+            "y": 4.800820969540026,
+            "heading": 2.7078159162812883,
+            "angularVelocity": -1.4272316627817019,
+            "velocityX": -1.1388840804643727,
+            "velocityY": 2.879670774217013,
+            "timestamp": 0.3758660128785652
+        },
+        {
+            "x": 7.279364393815875,
+            "y": 5.128465147908359,
+            "heading": 2.7044182580824483,
+            "angularVelocity": -0.03615818491083086,
+            "velocityX": -1.4287281240729475,
+            "velocityY": 3.486818889093733,
+            "timestamp": 0.4698325160982065
+        },
+        {
+            "x": 7.14774134031491,
+            "y": 5.45866822544814,
+            "heading": 2.7044182151788783,
+            "angularVelocity": -4.565836605912663e-7,
+            "velocityX": -1.4007444034956136,
+            "velocityY": 3.514050924806152,
+            "timestamp": 0.5637990193178478
+        },
+        {
+            "x": 6.993945263950606,
+            "y": 5.779145162484634,
+            "heading": 2.704418171322215,
+            "angularVelocity": -4.667265617625592e-7,
+            "velocityX": -1.636711712096111,
+            "velocityY": 3.410544460587175,
+            "timestamp": 0.6577655225374891
+        },
+        {
+            "x": 6.777429472030302,
+            "y": 6.061066444299655,
+            "heading": 2.704418118850798,
+            "angularVelocity": -5.58405553736054e-7,
+            "velocityX": -2.3041805803309763,
+            "velocityY": 3.000231701248326,
+            "timestamp": 0.7517320257571304
+        },
+        {
+            "x": 6.5074813547317545,
+            "y": 6.2923383909446455,
+            "heading": 2.7044180981669834,
+            "angularVelocity": -2.2011901843785396e-7,
+            "velocityX": -2.872812204872187,
+            "velocityY": 2.461216909438525,
+            "timestamp": 0.8456985289767717
+        },
+        {
+            "x": 6.195734686238664,
+            "y": 6.463002989375195,
+            "heading": 2.7046305201623446,
+            "angularVelocity": 0.002260614028208905,
+            "velocityX": -3.317636155560676,
+            "velocityY": 1.81622804491969,
+            "timestamp": 0.939665032196413
+        },
+        {
+            "x": 5.885885567596677,
+            "y": 6.564828261025872,
+            "heading": 2.8003077522097084,
+            "angularVelocity": 1.018205730436987,
+            "velocityX": -3.297442258947648,
+            "velocityY": 1.0836337222495915,
+            "timestamp": 1.0336315354160543
+        },
+        {
+            "x": 5.602675613806244,
+            "y": 6.605201169261095,
+            "heading": 3.005504879522894,
+            "angularVelocity": 2.183726331004879,
+            "velocityX": -3.0139458646071584,
+            "velocityY": 0.4296521297685472,
+            "timestamp": 1.1275980386356956
+        },
+        {
+            "x": 5.389000568947429,
+            "y": 6.621873348579134,
+            "heading": 3.193372437152187,
+            "angularVelocity": 1.999303487862736,
+            "velocityX": -2.273949093959171,
+            "velocityY": 0.17742683559341804,
+            "timestamp": 1.221564541855337
+        },
+        {
+            "x": 5.246855829881342,
+            "y": 6.628281724607146,
+            "heading": 3.330497456216328,
+            "angularVelocity": 1.459296817118111,
+            "velocityX": -1.5127171299950581,
+            "velocityY": 0.06819851551815755,
+            "timestamp": 1.3155310450749782
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.7735506640420036,
+            "velocityX": -0.7540541304992547,
+            "velocityY": 0.018286041663562246,
+            "timestamp": 1.4094975482946195
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 3.878626979187187e-32,
+            "velocityX": -5.758991747455131e-33,
+            "velocityY": 3.749392123602624e-30,
+            "timestamp": 1.5034640515142608
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.traj
new file mode 100644
index 00000000..626b4f65
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.traj	
@@ -0,0 +1,157 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": -7.693901589446952e-31,
+            "velocityX": -6.624376358473947e-32,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.653617802747577,
+            "y": 4.193887846119818,
+            "heading": 3.0831794581771232,
+            "angularVelocity": -0.6046893294524585,
+            "velocityX": -0.2807617219803893,
+            "velocityY": 0.7118264895254428,
+            "timestamp": 0.0939665032196413
+        },
+        {
+            "x": 7.6005882437737835,
+            "y": 4.328100423051846,
+            "heading": 2.9787754661598114,
+            "angularVelocity": -1.1110766969084016,
+            "velocityX": -0.5643453481485778,
+            "velocityY": 1.4283023453401595,
+            "timestamp": 0.1879330064392826
+        },
+        {
+            "x": 7.520633934300323,
+            "y": 4.5302283764630555,
+            "heading": 2.841927884917239,
+            "angularVelocity": -1.4563442988050657,
+            "velocityX": -0.8508809707068956,
+            "velocityY": 2.1510639055999357,
+            "timestamp": 0.2818995096589239
+        },
+        {
+            "x": 7.4136169796865685,
+            "y": 4.800820969540026,
+            "heading": 2.7078159162812883,
+            "angularVelocity": -1.4272316627817019,
+            "velocityX": -1.1388840804643727,
+            "velocityY": 2.879670774217013,
+            "timestamp": 0.3758660128785652
+        },
+        {
+            "x": 7.279364393815875,
+            "y": 5.128465147908359,
+            "heading": 2.7044182580824483,
+            "angularVelocity": -0.03615818491083086,
+            "velocityX": -1.4287281240729475,
+            "velocityY": 3.486818889093733,
+            "timestamp": 0.4698325160982065
+        },
+        {
+            "x": 7.14774134031491,
+            "y": 5.45866822544814,
+            "heading": 2.7044182151788783,
+            "angularVelocity": -4.565836605912663e-7,
+            "velocityX": -1.4007444034956136,
+            "velocityY": 3.514050924806152,
+            "timestamp": 0.5637990193178478
+        },
+        {
+            "x": 6.993945263950606,
+            "y": 5.779145162484634,
+            "heading": 2.704418171322215,
+            "angularVelocity": -4.667265617625592e-7,
+            "velocityX": -1.636711712096111,
+            "velocityY": 3.410544460587175,
+            "timestamp": 0.6577655225374891
+        },
+        {
+            "x": 6.777429472030302,
+            "y": 6.061066444299655,
+            "heading": 2.704418118850798,
+            "angularVelocity": -5.58405553736054e-7,
+            "velocityX": -2.3041805803309763,
+            "velocityY": 3.000231701248326,
+            "timestamp": 0.7517320257571304
+        },
+        {
+            "x": 6.5074813547317545,
+            "y": 6.2923383909446455,
+            "heading": 2.7044180981669834,
+            "angularVelocity": -2.2011901843785396e-7,
+            "velocityX": -2.872812204872187,
+            "velocityY": 2.461216909438525,
+            "timestamp": 0.8456985289767717
+        },
+        {
+            "x": 6.195734686238664,
+            "y": 6.463002989375195,
+            "heading": 2.7046305201623446,
+            "angularVelocity": 0.002260614028208905,
+            "velocityX": -3.317636155560676,
+            "velocityY": 1.81622804491969,
+            "timestamp": 0.939665032196413
+        },
+        {
+            "x": 5.885885567596677,
+            "y": 6.564828261025872,
+            "heading": 2.8003077522097084,
+            "angularVelocity": 1.018205730436987,
+            "velocityX": -3.297442258947648,
+            "velocityY": 1.0836337222495915,
+            "timestamp": 1.0336315354160543
+        },
+        {
+            "x": 5.602675613806244,
+            "y": 6.605201169261095,
+            "heading": 3.005504879522894,
+            "angularVelocity": 2.183726331004879,
+            "velocityX": -3.0139458646071584,
+            "velocityY": 0.4296521297685472,
+            "timestamp": 1.1275980386356956
+        },
+        {
+            "x": 5.389000568947429,
+            "y": 6.621873348579134,
+            "heading": 3.193372437152187,
+            "angularVelocity": 1.999303487862736,
+            "velocityX": -2.273949093959171,
+            "velocityY": 0.17742683559341804,
+            "timestamp": 1.221564541855337
+        },
+        {
+            "x": 5.246855829881342,
+            "y": 6.628281724607146,
+            "heading": 3.330497456216328,
+            "angularVelocity": 1.459296817118111,
+            "velocityX": -1.5127171299950581,
+            "velocityY": 0.06819851551815755,
+            "timestamp": 1.3155310450749782
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.7735506640420036,
+            "velocityX": -0.7540541304992547,
+            "velocityY": 0.018286041663562246,
+            "timestamp": 1.4094975482946195
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 3.878626979187187e-32,
+            "velocityX": -5.758991747455131e-33,
+            "velocityY": 3.749392123602624e-30,
+            "timestamp": 1.5034640515142608
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.1.traj
new file mode 100644
index 00000000..8079765c
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.1.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 8.742415967937999e-33,
+            "velocityX": -1.6780440317133427e-34,
+            "velocityY": 4.640205301974453e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 7.599500768056594,
+            "y": 4.12633480373168,
+            "heading": 3.0835764078705523,
+            "angularVelocity": -0.5690015744722524,
+            "velocityX": -0.8117914508973499,
+            "velocityY": -0.006708146534495369,
+            "timestamp": 0.09916245342867555
+        },
+        {
+            "x": 7.438156123912552,
+            "y": 4.125079622260014,
+            "heading": 2.9779167441714383,
+            "angularVelocity": -1.065520870508837,
+            "velocityX": -1.6270739434668355,
+            "velocityY": -0.012657829937315014,
+            "timestamp": 0.1983249068573511
+        },
+        {
+            "x": 7.195408805362699,
+            "y": 4.123338544846998,
+            "heading": 2.8371936481283773,
+            "angularVelocity": -1.4191167238946776,
+            "velocityX": -2.4479761256053836,
+            "velocityY": -0.017557829125994403,
+            "timestamp": 0.2974873602860266
+        },
+        {
+            "x": 6.870725465009593,
+            "y": 4.121560782802172,
+            "heading": 2.707127276825701,
+            "angularVelocity": -1.311649387499567,
+            "velocityX": -3.2742568293415686,
+            "velocityY": -0.017927773903902572,
+            "timestamp": 0.3966498137147022
+        },
+        {
+            "x": 6.495621806376681,
+            "y": 4.117497642893674,
+            "heading": 2.7071272422206913,
+            "angularVelocity": -3.489729050319063e-7,
+            "velocityX": -3.7827186164036664,
+            "velocityY": -0.04097458027721316,
+            "timestamp": 0.4958122671433778
+        },
+        {
+            "x": 6.120518146053048,
+            "y": 4.113434659065473,
+            "heading": 2.7071272076158857,
+            "angularVelocity": -3.4897084992850824e-7,
+            "velocityX": -3.7827186334536713,
+            "velocityY": -0.04097300629137556,
+            "timestamp": 0.5949747205720534
+        },
+        {
+            "x": 5.746992350808683,
+            "y": 4.148043089964446,
+            "heading": 2.7071271713462406,
+            "angularVelocity": -3.657598578804205e-7,
+            "velocityX": -3.76680671291609,
+            "velocityY": 0.3490074085739077,
+            "timestamp": 0.6941371740007289
+        },
+        {
+            "x": 5.389907487037217,
+            "y": 4.262976321781226,
+            "heading": 2.7071270362524373,
+            "angularVelocity": -0.000001362348332205964,
+            "velocityX": -3.6010087631434655,
+            "velocityY": 1.1590398164104305,
+            "timestamp": 0.7932996274294045
+        },
+        {
+            "x": 5.0459552426535295,
+            "y": 4.412696400521408,
+            "heading": 2.70712690066141,
+            "angularVelocity": -0.000001367362571417024,
+            "velocityX": -3.468573361096618,
+            "velocityY": 1.509846454614715,
+            "timestamp": 0.8924620808580801
+        },
+        {
+            "x": 4.713262422237293,
+            "y": 4.586001722574957,
+            "heading": 2.70712676137173,
+            "angularVelocity": -0.0000014046614974130796,
+            "velocityX": -3.355028127208762,
+            "velocityY": 1.747690946132164,
+            "timestamp": 0.9916245342867557
+        },
+        {
+            "x": 4.426168065896197,
+            "y": 4.827447572944867,
+            "heading": 2.7071267614112156,
+            "angularVelocity": 3.9818977543377344e-10,
+            "velocityX": -2.8951921459627346,
+            "velocityY": 2.4348515191142828,
+            "timestamp": 1.0907869877154313
+        },
+        {
+            "x": 4.093475286723252,
+            "y": 5.000753014560729,
+            "heading": 2.707126833350956,
+            "angularVelocity": 7.254735825966592e-7,
+            "velocityX": -3.355027711292369,
+            "velocityY": 1.7476921518537738,
+            "timestamp": 1.1899494411441067
+        },
+        {
+            "x": 3.752079510529913,
+            "y": 5.156214217951438,
+            "heading": 2.707126904178975,
+            "angularVelocity": 7.142624716180523e-7,
+            "velocityX": -3.4427927546074075,
+            "velocityY": 1.5677426083706831,
+            "timestamp": 1.2891118945727822
+        },
+        {
+            "x": 3.4085800456948445,
+            "y": 5.306970245158155,
+            "heading": 2.7071269730653125,
+            "angularVelocity": 6.946816574949046e-7,
+            "velocityX": -3.4640073229141812,
+            "velocityY": 1.5202934376281008,
+            "timestamp": 1.3882743480014577
+        },
+        {
+            "x": 3.041842891660795,
+            "y": 5.385857046749469,
+            "heading": 2.7071269977260264,
+            "angularVelocity": 2.486900331372695e-7,
+            "velocityX": -3.698346918149141,
+            "velocityY": 0.7955309581771803,
+            "timestamp": 1.4874368014301331
+        },
+        {
+            "x": 2.7246821450713132,
+            "y": 5.456194623849801,
+            "heading": 2.863679707498997,
+            "angularVelocity": 1.5787498630777044,
+            "velocityX": -3.1983955178923207,
+            "velocityY": 0.7093166281018212,
+            "timestamp": 1.5865992548588086
+        },
+        {
+            "x": 2.4872060579091175,
+            "y": 5.508276609290408,
+            "heading": 2.997740485854431,
+            "angularVelocity": 1.3519308339003542,
+            "velocityX": -2.394818592634002,
+            "velocityY": 0.5252188065119572,
+            "timestamp": 1.685761708287484
+        },
+        {
+            "x": 2.3290362313275734,
+            "y": 5.542798533700601,
+            "heading": 3.0916641126613666,
+            "angularVelocity": 0.9471692516612843,
+            "velocityX": -1.5950576161904975,
+            "velocityY": 0.34813503716932903,
+            "timestamp": 1.7849241617161595
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0.4874414223060736,
+            "velocityX": -0.7970378766840579,
+            "velocityY": 0.17346753438054688,
+            "timestamp": 1.884086615144835
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -6.536266088535149e-32,
+            "velocityX": 0,
+            "velocityY": 2.2958668874154396e-32,
+            "timestamp": 1.9832490685735105
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.traj
new file mode 100644
index 00000000..8079765c
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 8.742415967937999e-33,
+            "velocityX": -1.6780440317133427e-34,
+            "velocityY": 4.640205301974453e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 7.599500768056594,
+            "y": 4.12633480373168,
+            "heading": 3.0835764078705523,
+            "angularVelocity": -0.5690015744722524,
+            "velocityX": -0.8117914508973499,
+            "velocityY": -0.006708146534495369,
+            "timestamp": 0.09916245342867555
+        },
+        {
+            "x": 7.438156123912552,
+            "y": 4.125079622260014,
+            "heading": 2.9779167441714383,
+            "angularVelocity": -1.065520870508837,
+            "velocityX": -1.6270739434668355,
+            "velocityY": -0.012657829937315014,
+            "timestamp": 0.1983249068573511
+        },
+        {
+            "x": 7.195408805362699,
+            "y": 4.123338544846998,
+            "heading": 2.8371936481283773,
+            "angularVelocity": -1.4191167238946776,
+            "velocityX": -2.4479761256053836,
+            "velocityY": -0.017557829125994403,
+            "timestamp": 0.2974873602860266
+        },
+        {
+            "x": 6.870725465009593,
+            "y": 4.121560782802172,
+            "heading": 2.707127276825701,
+            "angularVelocity": -1.311649387499567,
+            "velocityX": -3.2742568293415686,
+            "velocityY": -0.017927773903902572,
+            "timestamp": 0.3966498137147022
+        },
+        {
+            "x": 6.495621806376681,
+            "y": 4.117497642893674,
+            "heading": 2.7071272422206913,
+            "angularVelocity": -3.489729050319063e-7,
+            "velocityX": -3.7827186164036664,
+            "velocityY": -0.04097458027721316,
+            "timestamp": 0.4958122671433778
+        },
+        {
+            "x": 6.120518146053048,
+            "y": 4.113434659065473,
+            "heading": 2.7071272076158857,
+            "angularVelocity": -3.4897084992850824e-7,
+            "velocityX": -3.7827186334536713,
+            "velocityY": -0.04097300629137556,
+            "timestamp": 0.5949747205720534
+        },
+        {
+            "x": 5.746992350808683,
+            "y": 4.148043089964446,
+            "heading": 2.7071271713462406,
+            "angularVelocity": -3.657598578804205e-7,
+            "velocityX": -3.76680671291609,
+            "velocityY": 0.3490074085739077,
+            "timestamp": 0.6941371740007289
+        },
+        {
+            "x": 5.389907487037217,
+            "y": 4.262976321781226,
+            "heading": 2.7071270362524373,
+            "angularVelocity": -0.000001362348332205964,
+            "velocityX": -3.6010087631434655,
+            "velocityY": 1.1590398164104305,
+            "timestamp": 0.7932996274294045
+        },
+        {
+            "x": 5.0459552426535295,
+            "y": 4.412696400521408,
+            "heading": 2.70712690066141,
+            "angularVelocity": -0.000001367362571417024,
+            "velocityX": -3.468573361096618,
+            "velocityY": 1.509846454614715,
+            "timestamp": 0.8924620808580801
+        },
+        {
+            "x": 4.713262422237293,
+            "y": 4.586001722574957,
+            "heading": 2.70712676137173,
+            "angularVelocity": -0.0000014046614974130796,
+            "velocityX": -3.355028127208762,
+            "velocityY": 1.747690946132164,
+            "timestamp": 0.9916245342867557
+        },
+        {
+            "x": 4.426168065896197,
+            "y": 4.827447572944867,
+            "heading": 2.7071267614112156,
+            "angularVelocity": 3.9818977543377344e-10,
+            "velocityX": -2.8951921459627346,
+            "velocityY": 2.4348515191142828,
+            "timestamp": 1.0907869877154313
+        },
+        {
+            "x": 4.093475286723252,
+            "y": 5.000753014560729,
+            "heading": 2.707126833350956,
+            "angularVelocity": 7.254735825966592e-7,
+            "velocityX": -3.355027711292369,
+            "velocityY": 1.7476921518537738,
+            "timestamp": 1.1899494411441067
+        },
+        {
+            "x": 3.752079510529913,
+            "y": 5.156214217951438,
+            "heading": 2.707126904178975,
+            "angularVelocity": 7.142624716180523e-7,
+            "velocityX": -3.4427927546074075,
+            "velocityY": 1.5677426083706831,
+            "timestamp": 1.2891118945727822
+        },
+        {
+            "x": 3.4085800456948445,
+            "y": 5.306970245158155,
+            "heading": 2.7071269730653125,
+            "angularVelocity": 6.946816574949046e-7,
+            "velocityX": -3.4640073229141812,
+            "velocityY": 1.5202934376281008,
+            "timestamp": 1.3882743480014577
+        },
+        {
+            "x": 3.041842891660795,
+            "y": 5.385857046749469,
+            "heading": 2.7071269977260264,
+            "angularVelocity": 2.486900331372695e-7,
+            "velocityX": -3.698346918149141,
+            "velocityY": 0.7955309581771803,
+            "timestamp": 1.4874368014301331
+        },
+        {
+            "x": 2.7246821450713132,
+            "y": 5.456194623849801,
+            "heading": 2.863679707498997,
+            "angularVelocity": 1.5787498630777044,
+            "velocityX": -3.1983955178923207,
+            "velocityY": 0.7093166281018212,
+            "timestamp": 1.5865992548588086
+        },
+        {
+            "x": 2.4872060579091175,
+            "y": 5.508276609290408,
+            "heading": 2.997740485854431,
+            "angularVelocity": 1.3519308339003542,
+            "velocityX": -2.394818592634002,
+            "velocityY": 0.5252188065119572,
+            "timestamp": 1.685761708287484
+        },
+        {
+            "x": 2.3290362313275734,
+            "y": 5.542798533700601,
+            "heading": 3.0916641126613666,
+            "angularVelocity": 0.9471692516612843,
+            "velocityX": -1.5950576161904975,
+            "velocityY": 0.34813503716932903,
+            "timestamp": 1.7849241617161595
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0.4874414223060736,
+            "velocityX": -0.7970378766840579,
+            "velocityY": 0.17346753438054688,
+            "timestamp": 1.884086615144835
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -6.536266088535149e-32,
+            "velocityX": 0,
+            "velocityY": 2.2958668874154396e-32,
+            "timestamp": 1.9832490685735105
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.1.traj
new file mode 100644
index 00000000..28199c95
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.1.traj	
@@ -0,0 +1,175 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 1.7868231110584375e-31,
+            "velocityX": 3.500790942409583e-35,
+            "velocityY": -4.724665555223456e-38,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6800001156222075,
+            "y": 2.5388441539517457,
+            "heading": 3.140000017932032,
+            "angularVelocity": 1.8099644459911076e-7,
+            "velocityX": 0.0000011670295392084272,
+            "velocityY": 0.8260917454273137,
+            "timestamp": 0.09907392781101554
+        },
+        {
+            "x": 7.680000338204962,
+            "y": 2.7025324587418944,
+            "heading": 3.1400000579849436,
+            "angularVelocity": 4.042728942464055e-7,
+            "velocityX": 0.000002246632826435788,
+            "velocityY": 1.6521834594301972,
+            "timestamp": 0.19814785562203108
+        },
+        {
+            "x": 7.680000658923826,
+            "y": 2.948064908143757,
+            "heading": 3.1400001223157976,
+            "angularVelocity": 6.493215714833696e-7,
+            "velocityX": 0.0000032371668547106284,
+            "velocityY": 2.4782751105841747,
+            "timestamp": 0.2972217834330466
+        },
+        {
+            "x": 7.6800010687648195,
+            "y": 3.2754414834772896,
+            "heading": 3.140000210384731,
+            "angularVelocity": 8.889210892181502e-7,
+            "velocityX": 0.0000041367184645969174,
+            "velocityY": 3.3043665731916554,
+            "timestamp": 0.39629571124406215
+        },
+        {
+            "x": 7.6800013918601255,
+            "y": 3.650232264926723,
+            "heading": 3.140000210384734,
+            "angularVelocity": 3.2741652225293424e-14,
+            "velocityX": 0.000003261153797251531,
+            "velocityY": 3.7829405750859055,
+            "timestamp": 0.4953696390550777
+        },
+        {
+            "x": 7.680001628786808,
+            "y": 4.025023046376236,
+            "heading": 3.140000210384741,
+            "angularVelocity": 7.304626800025654e-14,
+            "velocityX": 0.00000239141289972485,
+            "velocityY": 3.782940575086779,
+            "timestamp": 0.5944435668660932
+        },
+        {
+            "x": 7.6800017795018,
+            "y": 4.399813827825781,
+            "heading": 3.1400002103847475,
+            "angularVelocity": 7.220243449496737e-14,
+            "velocityX": 0.0000015212375955225791,
+            "velocityY": 3.782940575087123,
+            "timestamp": 0.6935174946771088
+        },
+        {
+            "x": 7.680001844060805,
+            "y": 4.774604609275342,
+            "heading": 3.140000210384754,
+            "angularVelocity": 6.886052815923548e-14,
+            "velocityX": 6.516245436906888e-7,
+            "velocityY": 3.782940575087321,
+            "timestamp": 0.7925914224881243
+        },
+        {
+            "x": 7.680001822974652,
+            "y": 5.14939539072491,
+            "heading": 3.1400002103847604,
+            "angularVelocity": 6.934745116017498e-14,
+            "velocityX": -2.1283245913448782e-7,
+            "velocityY": 3.782940575087379,
+            "timestamp": 0.8916653502991398
+        },
+        {
+            "x": 7.680001718396276,
+            "y": 5.5241861721744705,
+            "heading": 3.140000210384768,
+            "angularVelocity": 8.52485669839133e-14,
+            "velocityX": -0.0000010555588768470353,
+            "velocityY": 3.7829405750872986,
+            "timestamp": 0.9907392781101554
+        },
+        {
+            "x": 7.680001534024895,
+            "y": 5.898976953624011,
+            "heading": 3.140000210384774,
+            "angularVelocity": 6.71024495132872e-14,
+            "velocityX": -0.000001860947354605527,
+            "velocityY": 3.7829405750870757,
+            "timestamp": 1.089813205921171
+        },
+        {
+            "x": 7.680001275096042,
+            "y": 6.273767735073519,
+            "heading": 3.140000210384774,
+            "angularVelocity": 2.6424584851850765e-16,
+            "velocityX": -0.000002613491149837329,
+            "velocityY": 3.7829405750867053,
+            "timestamp": 1.1888871337321865
+        },
+        {
+            "x": 7.680000948769078,
+            "y": 6.648558516522932,
+            "heading": 3.1400002103847724,
+            "angularVelocity": -2.22727936711479e-14,
+            "velocityX": -0.000003293772326272547,
+            "velocityY": 3.782940575085685,
+            "timestamp": 1.287961061543202
+        },
+        {
+            "x": 7.680000579847807,
+            "y": 6.975935091856332,
+            "heading": 3.140000126724757,
+            "angularVelocity": -8.444197945421442e-7,
+            "velocityX": -0.0000037236965492526806,
+            "velocityY": 3.3043665731903227,
+            "timestamp": 1.3870349893542175
+        },
+        {
+            "x": 7.680000295053659,
+            "y": 7.221467541258139,
+            "heading": 3.14000006325574,
+            "angularVelocity": -6.40622649068188e-7,
+            "velocityX": -0.000002874561832669146,
+            "velocityY": 2.4782751105836276,
+            "timestamp": 1.486108917165233
+        },
+        {
+            "x": 7.680000100017398,
+            "y": 7.385155846048263,
+            "heading": 3.14000002096013,
+            "angularVelocity": -4.269095158176857e-7,
+            "velocityX": -0.000001968593124089932,
+            "velocityY": 1.6521834594299558,
+            "timestamp": 1.5851828449762486
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": -2.115604748661088e-7,
+            "velocityX": -0.0000010095228578568797,
+            "velocityY": 0.8260917454272356,
+            "timestamp": 1.6842567727872642
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 8.944588154710766e-32,
+            "velocityX": 0,
+            "velocityY": -4.821363548294539e-38,
+            "timestamp": 1.7833307005982797
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.traj
new file mode 100644
index 00000000..28199c95
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.traj	
@@ -0,0 +1,175 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 1.7868231110584375e-31,
+            "velocityX": 3.500790942409583e-35,
+            "velocityY": -4.724665555223456e-38,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6800001156222075,
+            "y": 2.5388441539517457,
+            "heading": 3.140000017932032,
+            "angularVelocity": 1.8099644459911076e-7,
+            "velocityX": 0.0000011670295392084272,
+            "velocityY": 0.8260917454273137,
+            "timestamp": 0.09907392781101554
+        },
+        {
+            "x": 7.680000338204962,
+            "y": 2.7025324587418944,
+            "heading": 3.1400000579849436,
+            "angularVelocity": 4.042728942464055e-7,
+            "velocityX": 0.000002246632826435788,
+            "velocityY": 1.6521834594301972,
+            "timestamp": 0.19814785562203108
+        },
+        {
+            "x": 7.680000658923826,
+            "y": 2.948064908143757,
+            "heading": 3.1400001223157976,
+            "angularVelocity": 6.493215714833696e-7,
+            "velocityX": 0.0000032371668547106284,
+            "velocityY": 2.4782751105841747,
+            "timestamp": 0.2972217834330466
+        },
+        {
+            "x": 7.6800010687648195,
+            "y": 3.2754414834772896,
+            "heading": 3.140000210384731,
+            "angularVelocity": 8.889210892181502e-7,
+            "velocityX": 0.0000041367184645969174,
+            "velocityY": 3.3043665731916554,
+            "timestamp": 0.39629571124406215
+        },
+        {
+            "x": 7.6800013918601255,
+            "y": 3.650232264926723,
+            "heading": 3.140000210384734,
+            "angularVelocity": 3.2741652225293424e-14,
+            "velocityX": 0.000003261153797251531,
+            "velocityY": 3.7829405750859055,
+            "timestamp": 0.4953696390550777
+        },
+        {
+            "x": 7.680001628786808,
+            "y": 4.025023046376236,
+            "heading": 3.140000210384741,
+            "angularVelocity": 7.304626800025654e-14,
+            "velocityX": 0.00000239141289972485,
+            "velocityY": 3.782940575086779,
+            "timestamp": 0.5944435668660932
+        },
+        {
+            "x": 7.6800017795018,
+            "y": 4.399813827825781,
+            "heading": 3.1400002103847475,
+            "angularVelocity": 7.220243449496737e-14,
+            "velocityX": 0.0000015212375955225791,
+            "velocityY": 3.782940575087123,
+            "timestamp": 0.6935174946771088
+        },
+        {
+            "x": 7.680001844060805,
+            "y": 4.774604609275342,
+            "heading": 3.140000210384754,
+            "angularVelocity": 6.886052815923548e-14,
+            "velocityX": 6.516245436906888e-7,
+            "velocityY": 3.782940575087321,
+            "timestamp": 0.7925914224881243
+        },
+        {
+            "x": 7.680001822974652,
+            "y": 5.14939539072491,
+            "heading": 3.1400002103847604,
+            "angularVelocity": 6.934745116017498e-14,
+            "velocityX": -2.1283245913448782e-7,
+            "velocityY": 3.782940575087379,
+            "timestamp": 0.8916653502991398
+        },
+        {
+            "x": 7.680001718396276,
+            "y": 5.5241861721744705,
+            "heading": 3.140000210384768,
+            "angularVelocity": 8.52485669839133e-14,
+            "velocityX": -0.0000010555588768470353,
+            "velocityY": 3.7829405750872986,
+            "timestamp": 0.9907392781101554
+        },
+        {
+            "x": 7.680001534024895,
+            "y": 5.898976953624011,
+            "heading": 3.140000210384774,
+            "angularVelocity": 6.71024495132872e-14,
+            "velocityX": -0.000001860947354605527,
+            "velocityY": 3.7829405750870757,
+            "timestamp": 1.089813205921171
+        },
+        {
+            "x": 7.680001275096042,
+            "y": 6.273767735073519,
+            "heading": 3.140000210384774,
+            "angularVelocity": 2.6424584851850765e-16,
+            "velocityX": -0.000002613491149837329,
+            "velocityY": 3.7829405750867053,
+            "timestamp": 1.1888871337321865
+        },
+        {
+            "x": 7.680000948769078,
+            "y": 6.648558516522932,
+            "heading": 3.1400002103847724,
+            "angularVelocity": -2.22727936711479e-14,
+            "velocityX": -0.000003293772326272547,
+            "velocityY": 3.782940575085685,
+            "timestamp": 1.287961061543202
+        },
+        {
+            "x": 7.680000579847807,
+            "y": 6.975935091856332,
+            "heading": 3.140000126724757,
+            "angularVelocity": -8.444197945421442e-7,
+            "velocityX": -0.0000037236965492526806,
+            "velocityY": 3.3043665731903227,
+            "timestamp": 1.3870349893542175
+        },
+        {
+            "x": 7.680000295053659,
+            "y": 7.221467541258139,
+            "heading": 3.14000006325574,
+            "angularVelocity": -6.40622649068188e-7,
+            "velocityX": -0.000002874561832669146,
+            "velocityY": 2.4782751105836276,
+            "timestamp": 1.486108917165233
+        },
+        {
+            "x": 7.680000100017398,
+            "y": 7.385155846048263,
+            "heading": 3.14000002096013,
+            "angularVelocity": -4.269095158176857e-7,
+            "velocityX": -0.000001968593124089932,
+            "velocityY": 1.6521834594299558,
+            "timestamp": 1.5851828449762486
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": -2.115604748661088e-7,
+            "velocityX": -0.0000010095228578568797,
+            "velocityY": 0.8260917454272356,
+            "timestamp": 1.6842567727872642
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 8.944588154710766e-32,
+            "velocityX": 0,
+            "velocityY": -4.821363548294539e-38,
+            "timestamp": 1.7833307005982797
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.1.traj
new file mode 100644
index 00000000..da777257
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.1.traj	
@@ -0,0 +1,139 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -9.54519608761012e-33,
+            "velocityX": 3.049131565114831e-33,
+            "velocityY": -1.5169993504833503e-38,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68000007508226,
+            "y": 2.533445372005505,
+            "heading": 3.1399999978468434,
+            "angularVelocity": -2.248715497169588e-8,
+            "velocityX": 7.84144633669789e-7,
+            "velocityY": 0.7983807350943783,
+            "timestamp": 0.09575052183150116
+        },
+        {
+            "x": 7.680000217378094,
+            "y": 2.6863361129826027,
+            "heading": 3.1399999975552166,
+            "angularVelocity": -3.0456922527341925e-9,
+            "velocityX": 0.0000014861101970902313,
+            "velocityY": 1.5967614385031632,
+            "timestamp": 0.19150104366300233
+        },
+        {
+            "x": 7.680000418878438,
+            "y": 2.915672216863469,
+            "heading": 3.1400000014113987,
+            "angularVelocity": 4.027321769334868e-8,
+            "velocityX": 0.000002104430661113429,
+            "velocityY": 2.395142078540767,
+            "timestamp": 0.2872515654945035
+        },
+        {
+            "x": 7.680000671408876,
+            "y": 3.2214536654446886,
+            "heading": 3.14000000940269,
+            "angularVelocity": 8.345949630994641e-8,
+            "velocityX": 0.0000026373790047185906,
+            "velocityY": 3.1935225284654267,
+            "timestamp": 0.38300208732600466
+        },
+        {
+            "x": 7.680000833444467,
+            "y": 3.5836721992668776,
+            "heading": 3.1400000094026845,
+            "angularVelocity": -5.801733416228813e-14,
+            "velocityX": 0.0000016922687336334932,
+            "velocityY": 3.7829405719543456,
+            "timestamp": 0.47875260915750584
+        },
+        {
+            "x": 7.680000914400183,
+            "y": 3.9458907330889588,
+            "heading": 3.140000009402682,
+            "angularVelocity": -2.5236519267762366e-14,
+            "velocityX": 8.45485885682039e-7,
+            "velocityY": 3.78294057195325,
+            "timestamp": 0.574503130989007
+        },
+        {
+            "x": 7.68000091414698,
+            "y": 4.308109266911049,
+            "heading": 3.1400000094026788,
+            "angularVelocity": -3.379642410236896e-14,
+            "velocityX": -2.644395591425324e-9,
+            "velocityY": 3.782940571953342,
+            "timestamp": 0.6702536528205081
+        },
+        {
+            "x": 7.680000832736705,
+            "y": 4.67032780073313,
+            "heading": 3.140000009402675,
+            "angularVelocity": -3.7410067203640955e-14,
+            "velocityX": -8.502332108268853e-7,
+            "velocityY": 3.782940571953249,
+            "timestamp": 0.7660041746520092
+        },
+        {
+            "x": 7.680000670417917,
+            "y": 5.032546334555316,
+            "heading": 3.1400000094026708,
+            "angularVelocity": -4.92147091530942e-14,
+            "velocityX": -0.0000016952263834753921,
+            "velocityY": 3.7829405719543154,
+            "timestamp": 0.8617546964835103
+        },
+        {
+            "x": 7.680000418190559,
+            "y": 5.338327783136532,
+            "heading": 3.1400000097695924,
+            "angularVelocity": 3.832069362065189e-9,
+            "velocityX": -0.00000263421371002401,
+            "velocityY": 3.1935225284653987,
+            "timestamp": 0.9575052183150115
+        },
+        {
+            "x": 7.68000021698581,
+            "y": 5.567663887017397,
+            "heading": 3.140000007984905,
+            "angularVelocity": -1.863892342341337e-8,
+            "velocityX": -0.0000021013435063173737,
+            "velocityY": 2.3951420785407556,
+            "timestamp": 1.0532557401465126
+        },
+        {
+            "x": 7.680000074934953,
+            "y": 5.7205546279944945,
+            "heading": 3.140000003983309,
+            "angularVelocity": -4.1791894261559745e-8,
+            "velocityX": -0.0000014835517222041492,
+            "velocityY": 1.5967614385031577,
+            "timestamp": 1.1490062619780137
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": -4.160090541448797e-8,
+            "velocityX": -7.826061846711994e-7,
+            "velocityY": 0.7983807350943762,
+            "timestamp": 1.2447567838095148
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 3.04245898705875e-32,
+            "velocityY": 2.558785218352301e-38,
+            "timestamp": 1.340507305641016
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.traj
new file mode 100644
index 00000000..da777257
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.traj	
@@ -0,0 +1,139 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -9.54519608761012e-33,
+            "velocityX": 3.049131565114831e-33,
+            "velocityY": -1.5169993504833503e-38,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68000007508226,
+            "y": 2.533445372005505,
+            "heading": 3.1399999978468434,
+            "angularVelocity": -2.248715497169588e-8,
+            "velocityX": 7.84144633669789e-7,
+            "velocityY": 0.7983807350943783,
+            "timestamp": 0.09575052183150116
+        },
+        {
+            "x": 7.680000217378094,
+            "y": 2.6863361129826027,
+            "heading": 3.1399999975552166,
+            "angularVelocity": -3.0456922527341925e-9,
+            "velocityX": 0.0000014861101970902313,
+            "velocityY": 1.5967614385031632,
+            "timestamp": 0.19150104366300233
+        },
+        {
+            "x": 7.680000418878438,
+            "y": 2.915672216863469,
+            "heading": 3.1400000014113987,
+            "angularVelocity": 4.027321769334868e-8,
+            "velocityX": 0.000002104430661113429,
+            "velocityY": 2.395142078540767,
+            "timestamp": 0.2872515654945035
+        },
+        {
+            "x": 7.680000671408876,
+            "y": 3.2214536654446886,
+            "heading": 3.14000000940269,
+            "angularVelocity": 8.345949630994641e-8,
+            "velocityX": 0.0000026373790047185906,
+            "velocityY": 3.1935225284654267,
+            "timestamp": 0.38300208732600466
+        },
+        {
+            "x": 7.680000833444467,
+            "y": 3.5836721992668776,
+            "heading": 3.1400000094026845,
+            "angularVelocity": -5.801733416228813e-14,
+            "velocityX": 0.0000016922687336334932,
+            "velocityY": 3.7829405719543456,
+            "timestamp": 0.47875260915750584
+        },
+        {
+            "x": 7.680000914400183,
+            "y": 3.9458907330889588,
+            "heading": 3.140000009402682,
+            "angularVelocity": -2.5236519267762366e-14,
+            "velocityX": 8.45485885682039e-7,
+            "velocityY": 3.78294057195325,
+            "timestamp": 0.574503130989007
+        },
+        {
+            "x": 7.68000091414698,
+            "y": 4.308109266911049,
+            "heading": 3.1400000094026788,
+            "angularVelocity": -3.379642410236896e-14,
+            "velocityX": -2.644395591425324e-9,
+            "velocityY": 3.782940571953342,
+            "timestamp": 0.6702536528205081
+        },
+        {
+            "x": 7.680000832736705,
+            "y": 4.67032780073313,
+            "heading": 3.140000009402675,
+            "angularVelocity": -3.7410067203640955e-14,
+            "velocityX": -8.502332108268853e-7,
+            "velocityY": 3.782940571953249,
+            "timestamp": 0.7660041746520092
+        },
+        {
+            "x": 7.680000670417917,
+            "y": 5.032546334555316,
+            "heading": 3.1400000094026708,
+            "angularVelocity": -4.92147091530942e-14,
+            "velocityX": -0.0000016952263834753921,
+            "velocityY": 3.7829405719543154,
+            "timestamp": 0.8617546964835103
+        },
+        {
+            "x": 7.680000418190559,
+            "y": 5.338327783136532,
+            "heading": 3.1400000097695924,
+            "angularVelocity": 3.832069362065189e-9,
+            "velocityX": -0.00000263421371002401,
+            "velocityY": 3.1935225284653987,
+            "timestamp": 0.9575052183150115
+        },
+        {
+            "x": 7.68000021698581,
+            "y": 5.567663887017397,
+            "heading": 3.140000007984905,
+            "angularVelocity": -1.863892342341337e-8,
+            "velocityX": -0.0000021013435063173737,
+            "velocityY": 2.3951420785407556,
+            "timestamp": 1.0532557401465126
+        },
+        {
+            "x": 7.680000074934953,
+            "y": 5.7205546279944945,
+            "heading": 3.140000003983309,
+            "angularVelocity": -4.1791894261559745e-8,
+            "velocityX": -0.0000014835517222041492,
+            "velocityY": 1.5967614385031577,
+            "timestamp": 1.1490062619780137
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": -4.160090541448797e-8,
+            "velocityX": -7.826061846711994e-7,
+            "velocityY": 0.7983807350943762,
+            "timestamp": 1.2447567838095148
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 3.04245898705875e-32,
+            "velocityY": 2.558785218352301e-38,
+            "timestamp": 1.340507305641016
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.1.traj
new file mode 100644
index 00000000..63f25781
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.1.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -3.7532975982701904e-32,
+            "velocityY": 5.0637303451538295e-36,
+            "timestamp": 0
+        },
+        {
+            "x": 7.680000036889488,
+            "y": 2.540500005575837,
+            "heading": 3.1400000022500048,
+            "angularVelocity": 2.2484053179234407e-8,
+            "velocityX": 3.6863266642951275e-7,
+            "velocityY": 0.8344065481227464,
+            "timestamp": 0.1000711293118395
+        },
+        {
+            "x": 7.680000101870814,
+            "y": 2.707500013629824,
+            "heading": 3.140000010932795,
+            "angularVelocity": 8.676618158835649e-8,
+            "velocityX": 6.49351381942043e-7,
+            "velocityY": 1.6688130652906412,
+            "timestamp": 0.200142258623679
+        },
+        {
+            "x": 7.680000185978468,
+            "y": 2.958000017966584,
+            "heading": 3.1400000281272393,
+            "angularVelocity": 1.7182222560804709e-7,
+            "velocityX": 8.404786987428781e-7,
+            "velocityY": 2.503219520548802,
+            "timestamp": 0.3002133879355185
+        },
+        {
+            "x": 7.68000028005223,
+            "y": 3.2919999999999923,
+            "heading": 3.1400000531214163,
+            "angularVelocity": 2.497641049380253e-7,
+            "velocityX": 9.400689316139974e-7,
+            "velocityY": 3.3376257900778246,
+            "timestamp": 0.400284517247358
+        },
+        {
+            "x": 7.680000187136683,
+            "y": 3.6259999820334055,
+            "heading": 3.140000035091715,
+            "angularVelocity": -1.801688496007097e-7,
+            "velocityX": -9.284950198395514e-7,
+            "velocityY": 3.337625790077875,
+            "timestamp": 0.5003556465591975
+        },
+        {
+            "x": 7.680000103123094,
+            "y": 3.8764999863701703,
+            "heading": 3.1400000189895447,
+            "angularVelocity": -1.609072425985215e-7,
+            "velocityX": -8.395387126580118e-7,
+            "velocityY": 2.5032195205488503,
+            "timestamp": 0.600426775871037
+        },
+        {
+            "x": 7.680000037559297,
+            "y": 4.043499994424161,
+            "heading": 3.140000006758861,
+            "angularVelocity": -1.222199010476452e-7,
+            "velocityX": -6.55171951368837e-7,
+            "velocityY": 1.6688130652906763,
+            "timestamp": 0.7004979051828765
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": -6.754056559007022e-8,
+            "velocityX": -3.753259946110928e-7,
+            "velocityY": 0.8344065481227645,
+            "timestamp": 0.800569034494716
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 3.7078876706224725e-32,
+            "velocityX": 0,
+            "velocityY": -1.600749389553648e-35,
+            "timestamp": 0.9006401638065554
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.traj
new file mode 100644
index 00000000..63f25781
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -3.7532975982701904e-32,
+            "velocityY": 5.0637303451538295e-36,
+            "timestamp": 0
+        },
+        {
+            "x": 7.680000036889488,
+            "y": 2.540500005575837,
+            "heading": 3.1400000022500048,
+            "angularVelocity": 2.2484053179234407e-8,
+            "velocityX": 3.6863266642951275e-7,
+            "velocityY": 0.8344065481227464,
+            "timestamp": 0.1000711293118395
+        },
+        {
+            "x": 7.680000101870814,
+            "y": 2.707500013629824,
+            "heading": 3.140000010932795,
+            "angularVelocity": 8.676618158835649e-8,
+            "velocityX": 6.49351381942043e-7,
+            "velocityY": 1.6688130652906412,
+            "timestamp": 0.200142258623679
+        },
+        {
+            "x": 7.680000185978468,
+            "y": 2.958000017966584,
+            "heading": 3.1400000281272393,
+            "angularVelocity": 1.7182222560804709e-7,
+            "velocityX": 8.404786987428781e-7,
+            "velocityY": 2.503219520548802,
+            "timestamp": 0.3002133879355185
+        },
+        {
+            "x": 7.68000028005223,
+            "y": 3.2919999999999923,
+            "heading": 3.1400000531214163,
+            "angularVelocity": 2.497641049380253e-7,
+            "velocityX": 9.400689316139974e-7,
+            "velocityY": 3.3376257900778246,
+            "timestamp": 0.400284517247358
+        },
+        {
+            "x": 7.680000187136683,
+            "y": 3.6259999820334055,
+            "heading": 3.140000035091715,
+            "angularVelocity": -1.801688496007097e-7,
+            "velocityX": -9.284950198395514e-7,
+            "velocityY": 3.337625790077875,
+            "timestamp": 0.5003556465591975
+        },
+        {
+            "x": 7.680000103123094,
+            "y": 3.8764999863701703,
+            "heading": 3.1400000189895447,
+            "angularVelocity": -1.609072425985215e-7,
+            "velocityX": -8.395387126580118e-7,
+            "velocityY": 2.5032195205488503,
+            "timestamp": 0.600426775871037
+        },
+        {
+            "x": 7.680000037559297,
+            "y": 4.043499994424161,
+            "heading": 3.140000006758861,
+            "angularVelocity": -1.222199010476452e-7,
+            "velocityX": -6.55171951368837e-7,
+            "velocityY": 1.6688130652906763,
+            "timestamp": 0.7004979051828765
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": -6.754056559007022e-8,
+            "velocityX": -3.753259946110928e-7,
+            "velocityY": 0.8344065481227645,
+            "timestamp": 0.800569034494716
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 3.7078876706224725e-32,
+            "velocityX": 0,
+            "velocityY": -1.600749389553648e-35,
+            "timestamp": 0.9006401638065554
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.1.traj
new file mode 100644
index 00000000..12702aa5
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -1.3041266298020504e-30,
+            "velocityX": -3.538115329170266e-32,
+            "velocityY": -5.678450231579595e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.634416744984149,
+            "y": 2.432986632964242,
+            "heading": 3.1489798678517915,
+            "angularVelocity": 0.11416974002165144,
+            "velocityX": -0.5795439822048055,
+            "velocityY": -0.3053051466642632,
+            "timestamp": 0.07865365945555033
+        },
+        {
+            "x": 7.543250241722733,
+            "y": 2.384959916525659,
+            "heading": 3.166940966282971,
+            "angularVelocity": 0.22835680571645545,
+            "velocityX": -1.1590878783324414,
+            "velocityY": -0.6106100691440082,
+            "timestamp": 0.15730731891110067
+        },
+        {
+            "x": 7.406500507389113,
+            "y": 2.312919874583363,
+            "heading": 3.193885700427137,
+            "angularVelocity": 0.34257445019952903,
+            "velocityX": -1.7386315561185093,
+            "velocityY": -0.9159146877712426,
+            "timestamp": 0.235960978366651
+        },
+        {
+            "x": 7.224167573256533,
+            "y": 2.216866534550912,
+            "heading": 3.2298175365571806,
+            "angularVelocity": 0.45683616476039074,
+            "velocityX": -2.3181748362976387,
+            "velocityY": -1.2212189578634138,
+            "timestamp": 0.31461463782220134
+        },
+        {
+            "x": 6.996251494378262,
+            "y": 2.0967999260644565,
+            "heading": 3.2747409040255966,
+            "angularVelocity": 0.5711541939609761,
+            "velocityX": -2.8977174165313135,
+            "velocityY": -1.5265228511625537,
+            "timestamp": 0.3932682972777517
+        },
+        {
+            "x": 6.733004702063821,
+            "y": 1.9581188703597256,
+            "heading": 3.2747409100209213,
+            "angularVelocity": 7.622435906772787e-8,
+            "velocityX": -3.3469109274338362,
+            "velocityY": -1.7631863115422364,
+            "timestamp": 0.47192195673330206
+        },
+        {
+            "x": 6.469756571981624,
+            "y": 1.819440354058334,
+            "heading": 3.2747409160161256,
+            "angularVelocity": 7.622282910230258e-8,
+            "velocityX": -3.3469279357684036,
+            "velocityY": -1.7631540256529774,
+            "timestamp": 0.5505756161888524
+        },
+        {
+            "x": 6.187670135263903,
+            "y": 1.7247907391748545,
+            "heading": 3.2747409182109637,
+            "angularVelocity": 2.790510194041111e-8,
+            "velocityX": -3.5864375373041195,
+            "velocityY": -1.2033720431910602,
+            "timestamp": 0.6292292756444028
+        },
+        {
+            "x": 5.905373163009314,
+            "y": 1.6806498188741994,
+            "heading": 3.236681586369982,
+            "angularVelocity": -0.48388507419022697,
+            "velocityX": -3.5891142790899004,
+            "velocityY": -0.561206186796692,
+            "timestamp": 0.7078829350999531
+        },
+        {
+            "x": 5.661530976081841,
+            "y": 1.6531145750479088,
+            "heading": 3.0865007795970714,
+            "angularVelocity": -1.9093937626358353,
+            "velocityX": -3.1002014224814998,
+            "velocityY": -0.3500821705803985,
+            "timestamp": 0.7865365945555035
+        },
+        {
+            "x": 5.4654145585609575,
+            "y": 1.6369706610279315,
+            "heading": 2.879148606187945,
+            "angularVelocity": -2.6362686090442846,
+            "velocityX": -2.4934175838533594,
+            "velocityY": -0.20525318378988838,
+            "timestamp": 0.8651902540110539
+        },
+        {
+            "x": 5.319930503793118,
+            "y": 1.6274302419537983,
+            "heading": 2.697897950588003,
+            "angularVelocity": -2.304414783171942,
+            "velocityX": -1.8496794144722244,
+            "velocityY": -0.12129656954518526,
+            "timestamp": 0.9438439134666042
+        },
+        {
+            "x": 5.223761655113151,
+            "y": 1.6221627992964307,
+            "heading": 2.568061248027121,
+            "angularVelocity": -1.6507395009924064,
+            "velocityX": -1.2226875309509793,
+            "velocityY": -0.0669700900610273,
+            "timestamp": 1.0224975729221546
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.8653284347892848,
+            "velocityX": -0.6072400883031053,
+            "velocityY": -0.027497758036964814,
+            "timestamp": 1.1011512323777048
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": -1.7743974032793004e-34,
+            "velocityY": 1.752346488498292e-33,
+            "timestamp": 1.179804891833255
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.traj
new file mode 100644
index 00000000..12702aa5
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -1.3041266298020504e-30,
+            "velocityX": -3.538115329170266e-32,
+            "velocityY": -5.678450231579595e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.634416744984149,
+            "y": 2.432986632964242,
+            "heading": 3.1489798678517915,
+            "angularVelocity": 0.11416974002165144,
+            "velocityX": -0.5795439822048055,
+            "velocityY": -0.3053051466642632,
+            "timestamp": 0.07865365945555033
+        },
+        {
+            "x": 7.543250241722733,
+            "y": 2.384959916525659,
+            "heading": 3.166940966282971,
+            "angularVelocity": 0.22835680571645545,
+            "velocityX": -1.1590878783324414,
+            "velocityY": -0.6106100691440082,
+            "timestamp": 0.15730731891110067
+        },
+        {
+            "x": 7.406500507389113,
+            "y": 2.312919874583363,
+            "heading": 3.193885700427137,
+            "angularVelocity": 0.34257445019952903,
+            "velocityX": -1.7386315561185093,
+            "velocityY": -0.9159146877712426,
+            "timestamp": 0.235960978366651
+        },
+        {
+            "x": 7.224167573256533,
+            "y": 2.216866534550912,
+            "heading": 3.2298175365571806,
+            "angularVelocity": 0.45683616476039074,
+            "velocityX": -2.3181748362976387,
+            "velocityY": -1.2212189578634138,
+            "timestamp": 0.31461463782220134
+        },
+        {
+            "x": 6.996251494378262,
+            "y": 2.0967999260644565,
+            "heading": 3.2747409040255966,
+            "angularVelocity": 0.5711541939609761,
+            "velocityX": -2.8977174165313135,
+            "velocityY": -1.5265228511625537,
+            "timestamp": 0.3932682972777517
+        },
+        {
+            "x": 6.733004702063821,
+            "y": 1.9581188703597256,
+            "heading": 3.2747409100209213,
+            "angularVelocity": 7.622435906772787e-8,
+            "velocityX": -3.3469109274338362,
+            "velocityY": -1.7631863115422364,
+            "timestamp": 0.47192195673330206
+        },
+        {
+            "x": 6.469756571981624,
+            "y": 1.819440354058334,
+            "heading": 3.2747409160161256,
+            "angularVelocity": 7.622282910230258e-8,
+            "velocityX": -3.3469279357684036,
+            "velocityY": -1.7631540256529774,
+            "timestamp": 0.5505756161888524
+        },
+        {
+            "x": 6.187670135263903,
+            "y": 1.7247907391748545,
+            "heading": 3.2747409182109637,
+            "angularVelocity": 2.790510194041111e-8,
+            "velocityX": -3.5864375373041195,
+            "velocityY": -1.2033720431910602,
+            "timestamp": 0.6292292756444028
+        },
+        {
+            "x": 5.905373163009314,
+            "y": 1.6806498188741994,
+            "heading": 3.236681586369982,
+            "angularVelocity": -0.48388507419022697,
+            "velocityX": -3.5891142790899004,
+            "velocityY": -0.561206186796692,
+            "timestamp": 0.7078829350999531
+        },
+        {
+            "x": 5.661530976081841,
+            "y": 1.6531145750479088,
+            "heading": 3.0865007795970714,
+            "angularVelocity": -1.9093937626358353,
+            "velocityX": -3.1002014224814998,
+            "velocityY": -0.3500821705803985,
+            "timestamp": 0.7865365945555035
+        },
+        {
+            "x": 5.4654145585609575,
+            "y": 1.6369706610279315,
+            "heading": 2.879148606187945,
+            "angularVelocity": -2.6362686090442846,
+            "velocityX": -2.4934175838533594,
+            "velocityY": -0.20525318378988838,
+            "timestamp": 0.8651902540110539
+        },
+        {
+            "x": 5.319930503793118,
+            "y": 1.6274302419537983,
+            "heading": 2.697897950588003,
+            "angularVelocity": -2.304414783171942,
+            "velocityX": -1.8496794144722244,
+            "velocityY": -0.12129656954518526,
+            "timestamp": 0.9438439134666042
+        },
+        {
+            "x": 5.223761655113151,
+            "y": 1.6221627992964307,
+            "heading": 2.568061248027121,
+            "angularVelocity": -1.6507395009924064,
+            "velocityX": -1.2226875309509793,
+            "velocityY": -0.0669700900610273,
+            "timestamp": 1.0224975729221546
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.8653284347892848,
+            "velocityX": -0.6072400883031053,
+            "velocityY": -0.027497758036964814,
+            "timestamp": 1.1011512323777048
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": -1.7743974032793004e-34,
+            "velocityY": 1.752346488498292e-33,
+            "timestamp": 1.179804891833255
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.1.traj
new file mode 100644
index 00000000..5b28f5d1
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.1.traj	
@@ -0,0 +1,238 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 1.0740254772261542e-28,
+            "velocityX": 2.0063238328605694e-29,
+            "velocityY": 1.599232652510583e-28,
+            "timestamp": 0
+        },
+        {
+            "x": 7.635665892651778,
+            "y": 2.515360941496967,
+            "heading": 3.1393295631597837,
+            "angularVelocity": -0.0071510111194275595,
+            "velocityX": -0.47287630332947994,
+            "velocityY": 0.6224892734875569,
+            "timestamp": 0.09375413197081223
+        },
+        {
+            "x": 7.546997676041258,
+            "y": 2.6320828188122176,
+            "heading": 3.137988695350318,
+            "angularVelocity": -0.014301959617960866,
+            "velocityX": -0.9457526270748597,
+            "velocityY": 1.2449784864051567,
+            "timestamp": 0.18750826394162445
+        },
+        {
+            "x": 7.413995349999322,
+            "y": 2.8071656233620255,
+            "heading": 3.1359774017076236,
+            "angularVelocity": -0.02145285333469997,
+            "velocityX": -1.4186289526241012,
+            "velocityY": 1.8674676077670485,
+            "timestamp": 0.2812623959124367
+        },
+        {
+            "x": 7.236658921990214,
+            "y": 3.0406093354422112,
+            "heading": 3.133295687975677,
+            "angularVelocity": -0.028603685785057682,
+            "velocityX": -1.8915051985582343,
+            "velocityY": 2.489956518960288,
+            "timestamp": 0.3750165278832489
+        },
+        {
+            "x": 7.022133976710729,
+            "y": 3.323040275054587,
+            "heading": 3.133295687627681,
+            "angularVelocity": -3.7117965182714086e-9,
+            "velocityX": -2.288165233573672,
+            "velocityY": 3.0124639167935428,
+            "timestamp": 0.46877065985406113
+        },
+        {
+            "x": 6.786563329839658,
+            "y": 3.588171672312515,
+            "heading": 3.133295687271953,
+            "angularVelocity": -3.794263922237942e-9,
+            "velocityX": -2.512642823512138,
+            "velocityY": 2.8279435976269225,
+            "timestamp": 0.5625247918248734
+        },
+        {
+            "x": 6.501527367007155,
+            "y": 3.7992230523695487,
+            "heading": 3.133295686856248,
+            "angularVelocity": -4.433993924393781e-9,
+            "velocityX": -3.040249606505238,
+            "velocityY": 2.251115504143757,
+            "timestamp": 0.6562789237956856
+        },
+        {
+            "x": 6.179197632360147,
+            "y": 3.947181584790984,
+            "heading": 3.133295705650933,
+            "angularVelocity": 2.004678114437141e-7,
+            "velocityX": -3.438032307177196,
+            "velocityY": 1.5781547896737034,
+            "timestamp": 0.7500330557664978
+        },
+        {
+            "x": 5.833338645417163,
+            "y": 4.025728972736192,
+            "heading": 3.133296009616361,
+            "angularVelocity": 0.000003242155002257518,
+            "velocityX": -3.6889999370977806,
+            "velocityY": 0.837801879171189,
+            "timestamp": 0.84378718773731
+        },
+        {
+            "x": 5.481107394947439,
+            "y": 4.067217134926503,
+            "heading": 3.1332963782586343,
+            "angularVelocity": 0.000003932010945944395,
+            "velocityX": -3.7569677524120277,
+            "velocityY": 0.4425208928736066,
+            "timestamp": 0.9375413197081222
+        },
+        {
+            "x": 5.149070300488837,
+            "y": 4.179082889717707,
+            "heading": 3.1449832895383,
+            "angularVelocity": 0.12465489289866781,
+            "velocityX": -3.541572915015461,
+            "velocityY": 1.1931821290397846,
+            "timestamp": 1.0312954516789343
+        },
+        {
+            "x": 4.859012780941325,
+            "y": 4.350314048380944,
+            "heading": 3.189517171995987,
+            "angularVelocity": 0.4750071439148605,
+            "velocityX": -3.093810517469455,
+            "velocityY": 1.826385195657799,
+            "timestamp": 1.1250495836497465
+        },
+        {
+            "x": 4.618219764951525,
+            "y": 4.5756090572292365,
+            "heading": 3.250644921045485,
+            "angularVelocity": 0.6520005866891553,
+            "velocityX": -2.5683456390462376,
+            "velocityY": 2.4030408485721884,
+            "timestamp": 1.2188037156205587
+        },
+        {
+            "x": 4.427454911100163,
+            "y": 4.854246931015481,
+            "heading": 3.2945713874044515,
+            "angularVelocity": 0.46852832441237513,
+            "velocityX": -2.034735428095452,
+            "velocityY": 2.9720063311233567,
+            "timestamp": 1.3125578475913708
+        },
+        {
+            "x": 4.197108920054306,
+            "y": 5.123929924649606,
+            "heading": 3.2945713966505874,
+            "angularVelocity": 9.862110665096438e-8,
+            "velocityX": -2.4569156175171605,
+            "velocityY": 2.8764918192414846,
+            "timestamp": 1.406311979562183
+        },
+        {
+            "x": 3.9283154974180072,
+            "y": 5.355313346500023,
+            "heading": 3.2945714071744505,
+            "angularVelocity": 1.1224958813512746e-7,
+            "velocityX": -2.8670034801237065,
+            "velocityY": 2.4679810583970268,
+            "timestamp": 1.5000661115329952
+        },
+        {
+            "x": 3.6595153829213625,
+            "y": 5.586688994322917,
+            "heading": 3.2945714176982936,
+            "angularVelocity": 1.1224938145766723e-7,
+            "velocityX": -2.867074856821552,
+            "velocityY": 2.4678981390913592,
+            "timestamp": 1.5938202435038074
+        },
+        {
+            "x": 3.3907152487550194,
+            "y": 5.818064619294552,
+            "heading": 3.2945714282221594,
+            "angularVelocity": 1.1224961995364469e-7,
+            "velocityX": -2.867075066622423,
+            "velocityY": 2.4678978953553523,
+            "timestamp": 1.6875743754746195
+        },
+        {
+            "x": 3.121915101893736,
+            "y": 6.049440229517853,
+            "heading": 3.294571438746038,
+            "angularVelocity": 1.1224975583094217e-7,
+            "velocityX": -2.8670752020291417,
+            "velocityY": 2.467897738046713,
+            "timestamp": 1.7813285074454317
+        },
+        {
+            "x": 2.853115082553889,
+            "y": 6.2808159878868715,
+            "heading": 3.2945714492725595,
+            "angularVelocity": 1.1227794602420482e-7,
+            "velocityX": -2.8670738418604556,
+            "velocityY": 2.467899318198048,
+            "timestamp": 1.8750826394162439
+        },
+        {
+            "x": 2.6318681155961685,
+            "y": 6.471289698509282,
+            "heading": 3.3899753271617903,
+            "angularVelocity": 1.0175965142415218,
+            "velocityX": -2.3598636380805007,
+            "velocityY": 2.031630037188212,
+            "timestamp": 1.968836771387056
+        },
+        {
+            "x": 2.465933868110727,
+            "y": 6.614144871792939,
+            "heading": 3.461571184706241,
+            "angularVelocity": 0.7636554895173272,
+            "velocityX": -1.7698872998696302,
+            "velocityY": 1.5237213579891205,
+            "timestamp": 2.062590903357868
+        },
+        {
+            "x": 2.3553113103707712,
+            "y": 6.709381606049684,
+            "heading": 3.5093149725923682,
+            "angularVelocity": 0.5092446261566541,
+            "velocityX": -1.1799219449271308,
+            "velocityY": 1.0158137274034427,
+            "timestamp": 2.1563450353286804
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 0.2546056806823578,
+            "velocityX": -0.5899613084572273,
+            "velocityY": 0.5079071497898447,
+            "timestamp": 2.2500991672994926
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 0,
+            "velocityX": 7.663961659075425e-29,
+            "velocityY": 7.569331315704837e-30,
+            "timestamp": 2.3438532992703047
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.traj
new file mode 100644
index 00000000..5b28f5d1
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.traj	
@@ -0,0 +1,238 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 1.0740254772261542e-28,
+            "velocityX": 2.0063238328605694e-29,
+            "velocityY": 1.599232652510583e-28,
+            "timestamp": 0
+        },
+        {
+            "x": 7.635665892651778,
+            "y": 2.515360941496967,
+            "heading": 3.1393295631597837,
+            "angularVelocity": -0.0071510111194275595,
+            "velocityX": -0.47287630332947994,
+            "velocityY": 0.6224892734875569,
+            "timestamp": 0.09375413197081223
+        },
+        {
+            "x": 7.546997676041258,
+            "y": 2.6320828188122176,
+            "heading": 3.137988695350318,
+            "angularVelocity": -0.014301959617960866,
+            "velocityX": -0.9457526270748597,
+            "velocityY": 1.2449784864051567,
+            "timestamp": 0.18750826394162445
+        },
+        {
+            "x": 7.413995349999322,
+            "y": 2.8071656233620255,
+            "heading": 3.1359774017076236,
+            "angularVelocity": -0.02145285333469997,
+            "velocityX": -1.4186289526241012,
+            "velocityY": 1.8674676077670485,
+            "timestamp": 0.2812623959124367
+        },
+        {
+            "x": 7.236658921990214,
+            "y": 3.0406093354422112,
+            "heading": 3.133295687975677,
+            "angularVelocity": -0.028603685785057682,
+            "velocityX": -1.8915051985582343,
+            "velocityY": 2.489956518960288,
+            "timestamp": 0.3750165278832489
+        },
+        {
+            "x": 7.022133976710729,
+            "y": 3.323040275054587,
+            "heading": 3.133295687627681,
+            "angularVelocity": -3.7117965182714086e-9,
+            "velocityX": -2.288165233573672,
+            "velocityY": 3.0124639167935428,
+            "timestamp": 0.46877065985406113
+        },
+        {
+            "x": 6.786563329839658,
+            "y": 3.588171672312515,
+            "heading": 3.133295687271953,
+            "angularVelocity": -3.794263922237942e-9,
+            "velocityX": -2.512642823512138,
+            "velocityY": 2.8279435976269225,
+            "timestamp": 0.5625247918248734
+        },
+        {
+            "x": 6.501527367007155,
+            "y": 3.7992230523695487,
+            "heading": 3.133295686856248,
+            "angularVelocity": -4.433993924393781e-9,
+            "velocityX": -3.040249606505238,
+            "velocityY": 2.251115504143757,
+            "timestamp": 0.6562789237956856
+        },
+        {
+            "x": 6.179197632360147,
+            "y": 3.947181584790984,
+            "heading": 3.133295705650933,
+            "angularVelocity": 2.004678114437141e-7,
+            "velocityX": -3.438032307177196,
+            "velocityY": 1.5781547896737034,
+            "timestamp": 0.7500330557664978
+        },
+        {
+            "x": 5.833338645417163,
+            "y": 4.025728972736192,
+            "heading": 3.133296009616361,
+            "angularVelocity": 0.000003242155002257518,
+            "velocityX": -3.6889999370977806,
+            "velocityY": 0.837801879171189,
+            "timestamp": 0.84378718773731
+        },
+        {
+            "x": 5.481107394947439,
+            "y": 4.067217134926503,
+            "heading": 3.1332963782586343,
+            "angularVelocity": 0.000003932010945944395,
+            "velocityX": -3.7569677524120277,
+            "velocityY": 0.4425208928736066,
+            "timestamp": 0.9375413197081222
+        },
+        {
+            "x": 5.149070300488837,
+            "y": 4.179082889717707,
+            "heading": 3.1449832895383,
+            "angularVelocity": 0.12465489289866781,
+            "velocityX": -3.541572915015461,
+            "velocityY": 1.1931821290397846,
+            "timestamp": 1.0312954516789343
+        },
+        {
+            "x": 4.859012780941325,
+            "y": 4.350314048380944,
+            "heading": 3.189517171995987,
+            "angularVelocity": 0.4750071439148605,
+            "velocityX": -3.093810517469455,
+            "velocityY": 1.826385195657799,
+            "timestamp": 1.1250495836497465
+        },
+        {
+            "x": 4.618219764951525,
+            "y": 4.5756090572292365,
+            "heading": 3.250644921045485,
+            "angularVelocity": 0.6520005866891553,
+            "velocityX": -2.5683456390462376,
+            "velocityY": 2.4030408485721884,
+            "timestamp": 1.2188037156205587
+        },
+        {
+            "x": 4.427454911100163,
+            "y": 4.854246931015481,
+            "heading": 3.2945713874044515,
+            "angularVelocity": 0.46852832441237513,
+            "velocityX": -2.034735428095452,
+            "velocityY": 2.9720063311233567,
+            "timestamp": 1.3125578475913708
+        },
+        {
+            "x": 4.197108920054306,
+            "y": 5.123929924649606,
+            "heading": 3.2945713966505874,
+            "angularVelocity": 9.862110665096438e-8,
+            "velocityX": -2.4569156175171605,
+            "velocityY": 2.8764918192414846,
+            "timestamp": 1.406311979562183
+        },
+        {
+            "x": 3.9283154974180072,
+            "y": 5.355313346500023,
+            "heading": 3.2945714071744505,
+            "angularVelocity": 1.1224958813512746e-7,
+            "velocityX": -2.8670034801237065,
+            "velocityY": 2.4679810583970268,
+            "timestamp": 1.5000661115329952
+        },
+        {
+            "x": 3.6595153829213625,
+            "y": 5.586688994322917,
+            "heading": 3.2945714176982936,
+            "angularVelocity": 1.1224938145766723e-7,
+            "velocityX": -2.867074856821552,
+            "velocityY": 2.4678981390913592,
+            "timestamp": 1.5938202435038074
+        },
+        {
+            "x": 3.3907152487550194,
+            "y": 5.818064619294552,
+            "heading": 3.2945714282221594,
+            "angularVelocity": 1.1224961995364469e-7,
+            "velocityX": -2.867075066622423,
+            "velocityY": 2.4678978953553523,
+            "timestamp": 1.6875743754746195
+        },
+        {
+            "x": 3.121915101893736,
+            "y": 6.049440229517853,
+            "heading": 3.294571438746038,
+            "angularVelocity": 1.1224975583094217e-7,
+            "velocityX": -2.8670752020291417,
+            "velocityY": 2.467897738046713,
+            "timestamp": 1.7813285074454317
+        },
+        {
+            "x": 2.853115082553889,
+            "y": 6.2808159878868715,
+            "heading": 3.2945714492725595,
+            "angularVelocity": 1.1227794602420482e-7,
+            "velocityX": -2.8670738418604556,
+            "velocityY": 2.467899318198048,
+            "timestamp": 1.8750826394162439
+        },
+        {
+            "x": 2.6318681155961685,
+            "y": 6.471289698509282,
+            "heading": 3.3899753271617903,
+            "angularVelocity": 1.0175965142415218,
+            "velocityX": -2.3598636380805007,
+            "velocityY": 2.031630037188212,
+            "timestamp": 1.968836771387056
+        },
+        {
+            "x": 2.465933868110727,
+            "y": 6.614144871792939,
+            "heading": 3.461571184706241,
+            "angularVelocity": 0.7636554895173272,
+            "velocityX": -1.7698872998696302,
+            "velocityY": 1.5237213579891205,
+            "timestamp": 2.062590903357868
+        },
+        {
+            "x": 2.3553113103707712,
+            "y": 6.709381606049684,
+            "heading": 3.5093149725923682,
+            "angularVelocity": 0.5092446261566541,
+            "velocityX": -1.1799219449271308,
+            "velocityY": 1.0158137274034427,
+            "timestamp": 2.1563450353286804
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 0.2546056806823578,
+            "velocityX": -0.5899613084572273,
+            "velocityY": 0.5079071497898447,
+            "timestamp": 2.2500991672994926
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 0,
+            "velocityX": 7.663961659075425e-29,
+            "velocityY": 7.569331315704837e-30,
+            "timestamp": 2.3438532992703047
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.1.traj
new file mode 100644
index 00000000..03ea08d7
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.1.traj	
@@ -0,0 +1,211 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 1.048713449145388e-30,
+            "velocityX": -1.8824383693720305e-31,
+            "velocityY": -8.291277152276166e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.627273403539296,
+            "y": 2.514475736726478,
+            "heading": 3.079415345929856,
+            "angularVelocity": -0.619115297628915,
+            "velocityX": -0.5388137138314486,
+            "velocityY": 0.5873452344657393,
+            "timestamp": 0.09785681972674802
+        },
+        {
+            "x": 7.521466473904217,
+            "y": 2.629566524398062,
+            "heading": 2.9639191611810785,
+            "angularVelocity": -1.1802568801161224,
+            "velocityX": -1.08124226733027,
+            "velocityY": 1.1761141225819405,
+            "timestamp": 0.19571363945349604
+        },
+        {
+            "x": 7.362002347569612,
+            "y": 2.8026163659937295,
+            "heading": 2.8066032187897654,
+            "angularVelocity": -1.6076134788622511,
+            "velocityX": -1.6295657960261463,
+            "velocityY": 1.7683983812153936,
+            "timestamp": 0.29357045918024405
+        },
+        {
+            "x": 7.14775766144854,
+            "y": 3.033681724074784,
+            "heading": 2.6597575656629697,
+            "angularVelocity": -1.5006174688370466,
+            "velocityX": -2.18936898541483,
+            "velocityY": 2.361259631431653,
+            "timestamp": 0.3914272789069921
+        },
+        {
+            "x": 6.898594719017662,
+            "y": 3.307462468622087,
+            "heading": 2.6597575308458405,
+            "angularVelocity": -3.5579665549162934e-7,
+            "velocityX": -2.546199060286565,
+            "velocityY": 2.7977686717369177,
+            "timestamp": 0.4892840986337401
+        },
+        {
+            "x": 6.632320724562211,
+            "y": 3.5646321159756953,
+            "heading": 2.6597574950521046,
+            "angularVelocity": -3.657766164691694e-7,
+            "velocityX": -2.721057103623286,
+            "velocityY": 2.628019672739413,
+            "timestamp": 0.5871409183604881
+        },
+        {
+            "x": 6.3170953901288,
+            "y": 3.7587224487937174,
+            "heading": 2.6597575172185417,
+            "angularVelocity": 2.265190860696483e-7,
+            "velocityX": -3.221291426735886,
+            "velocityY": 1.983411410262395,
+            "timestamp": 0.6849977380872361
+        },
+        {
+            "x": 5.989980622324306,
+            "y": 3.9320256447233213,
+            "heading": 2.65975753898433,
+            "angularVelocity": 2.224248489911636e-7,
+            "velocityX": -3.342789687197243,
+            "velocityY": 1.770987412155123,
+            "timestamp": 0.782854557813984
+        },
+        {
+            "x": 5.662865689540743,
+            "y": 4.1053285292498645,
+            "heading": 2.659757560750018,
+            "angularVelocity": 2.2242382298690162e-7,
+            "velocityX": -3.3427913731203196,
+            "velocityY": 1.7709842299235545,
+            "timestamp": 0.880711377540732
+        },
+        {
+            "x": 5.335750711483487,
+            "y": 4.278631328320837,
+            "heading": 2.659757582515508,
+            "angularVelocity": 2.2242180190175268e-7,
+            "velocityX": -3.3427918357727204,
+            "velocityY": 1.7709833566520703,
+            "timestamp": 0.97856819726748
+        },
+        {
+            "x": 5.008636210362509,
+            "y": 4.451935027622489,
+            "heading": 2.659757604280525,
+            "angularVelocity": 2.224169652952219e-7,
+            "velocityX": -3.342786961955248,
+            "velocityY": 1.7709925561200404,
+            "timestamp": 1.076425016994228
+        },
+        {
+            "x": 4.6815226569833905,
+            "y": 4.625240515795651,
+            "heading": 2.6597576260457285,
+            "angularVelocity": 2.224188696537283e-7,
+            "velocityX": -3.3427772769699504,
+            "velocityY": 1.7710108366192054,
+            "timestamp": 1.174281836720976
+        },
+        {
+            "x": 4.378427967662734,
+            "y": 4.837776808087965,
+            "heading": 2.65975767630399,
+            "angularVelocity": 5.135897752366777e-7,
+            "velocityX": -3.097328220628949,
+            "velocityY": 2.171910888641101,
+            "timestamp": 1.272138656447724
+        },
+        {
+            "x": 4.050949875066163,
+            "y": 5.0103924451271915,
+            "heading": 2.6597577704243744,
+            "angularVelocity": 9.61817319833198e-7,
+            "velocityX": -3.3465025075514396,
+            "velocityY": 1.7639612397095272,
+            "timestamp": 1.369995476174472
+        },
+        {
+            "x": 3.722909272369472,
+            "y": 5.181936673224133,
+            "heading": 2.659757864941058,
+            "angularVelocity": 9.65867108780206e-7,
+            "velocityX": -3.352250804928058,
+            "velocityY": 1.7530124990363987,
+            "timestamp": 1.46785229590122
+        },
+        {
+            "x": 3.383528958103272,
+            "y": 5.329784787197771,
+            "heading": 2.659757940242804,
+            "angularVelocity": 7.6950943654844e-7,
+            "velocityX": -3.468131451787155,
+            "velocityY": 1.5108616281060843,
+            "timestamp": 1.5657091156279679
+        },
+        {
+            "x": 3.020395743291134,
+            "y": 5.401703742215627,
+            "heading": 2.6597579736142767,
+            "angularVelocity": 3.4102347755826357e-7,
+            "velocityX": -3.7108626238430706,
+            "velocityY": 0.7349406532797214,
+            "timestamp": 1.6635659353547159
+        },
+        {
+            "x": 2.7114854008089204,
+            "y": 5.466141485650731,
+            "heading": 2.823345730065705,
+            "angularVelocity": 1.6717052210385108,
+            "velocityX": -3.156758449179158,
+            "velocityY": 0.6584900634931665,
+            "timestamp": 1.7614227550814638
+        },
+        {
+            "x": 2.4804696774793777,
+            "y": 5.513370093880283,
+            "heading": 2.974776278106282,
+            "angularVelocity": 1.5474705642736624,
+            "velocityX": -2.3607524133179756,
+            "velocityY": 0.48262970696813046,
+            "timestamp": 1.8592795748082118
+        },
+        {
+            "x": 2.3267564356504185,
+            "y": 5.544523353838769,
+            "heading": 3.083396984959406,
+            "angularVelocity": 1.1099962900534985,
+            "velocityX": -1.5707974391379447,
+            "velocityY": 0.3183555325574442,
+            "timestamp": 1.9571363945349598
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0.5784268812194232,
+            "velocityX": -0.7843749251687325,
+            "velocityY": 0.15815603045805554,
+            "timestamp": 2.054993214261708
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 1.5606110446115462e-29,
+            "velocityX": 0,
+            "velocityY": 3.583632692315791e-31,
+            "timestamp": 2.152850033988456
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.traj
new file mode 100644
index 00000000..03ea08d7
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.traj	
@@ -0,0 +1,211 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 1.048713449145388e-30,
+            "velocityX": -1.8824383693720305e-31,
+            "velocityY": -8.291277152276166e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.627273403539296,
+            "y": 2.514475736726478,
+            "heading": 3.079415345929856,
+            "angularVelocity": -0.619115297628915,
+            "velocityX": -0.5388137138314486,
+            "velocityY": 0.5873452344657393,
+            "timestamp": 0.09785681972674802
+        },
+        {
+            "x": 7.521466473904217,
+            "y": 2.629566524398062,
+            "heading": 2.9639191611810785,
+            "angularVelocity": -1.1802568801161224,
+            "velocityX": -1.08124226733027,
+            "velocityY": 1.1761141225819405,
+            "timestamp": 0.19571363945349604
+        },
+        {
+            "x": 7.362002347569612,
+            "y": 2.8026163659937295,
+            "heading": 2.8066032187897654,
+            "angularVelocity": -1.6076134788622511,
+            "velocityX": -1.6295657960261463,
+            "velocityY": 1.7683983812153936,
+            "timestamp": 0.29357045918024405
+        },
+        {
+            "x": 7.14775766144854,
+            "y": 3.033681724074784,
+            "heading": 2.6597575656629697,
+            "angularVelocity": -1.5006174688370466,
+            "velocityX": -2.18936898541483,
+            "velocityY": 2.361259631431653,
+            "timestamp": 0.3914272789069921
+        },
+        {
+            "x": 6.898594719017662,
+            "y": 3.307462468622087,
+            "heading": 2.6597575308458405,
+            "angularVelocity": -3.5579665549162934e-7,
+            "velocityX": -2.546199060286565,
+            "velocityY": 2.7977686717369177,
+            "timestamp": 0.4892840986337401
+        },
+        {
+            "x": 6.632320724562211,
+            "y": 3.5646321159756953,
+            "heading": 2.6597574950521046,
+            "angularVelocity": -3.657766164691694e-7,
+            "velocityX": -2.721057103623286,
+            "velocityY": 2.628019672739413,
+            "timestamp": 0.5871409183604881
+        },
+        {
+            "x": 6.3170953901288,
+            "y": 3.7587224487937174,
+            "heading": 2.6597575172185417,
+            "angularVelocity": 2.265190860696483e-7,
+            "velocityX": -3.221291426735886,
+            "velocityY": 1.983411410262395,
+            "timestamp": 0.6849977380872361
+        },
+        {
+            "x": 5.989980622324306,
+            "y": 3.9320256447233213,
+            "heading": 2.65975753898433,
+            "angularVelocity": 2.224248489911636e-7,
+            "velocityX": -3.342789687197243,
+            "velocityY": 1.770987412155123,
+            "timestamp": 0.782854557813984
+        },
+        {
+            "x": 5.662865689540743,
+            "y": 4.1053285292498645,
+            "heading": 2.659757560750018,
+            "angularVelocity": 2.2242382298690162e-7,
+            "velocityX": -3.3427913731203196,
+            "velocityY": 1.7709842299235545,
+            "timestamp": 0.880711377540732
+        },
+        {
+            "x": 5.335750711483487,
+            "y": 4.278631328320837,
+            "heading": 2.659757582515508,
+            "angularVelocity": 2.2242180190175268e-7,
+            "velocityX": -3.3427918357727204,
+            "velocityY": 1.7709833566520703,
+            "timestamp": 0.97856819726748
+        },
+        {
+            "x": 5.008636210362509,
+            "y": 4.451935027622489,
+            "heading": 2.659757604280525,
+            "angularVelocity": 2.224169652952219e-7,
+            "velocityX": -3.342786961955248,
+            "velocityY": 1.7709925561200404,
+            "timestamp": 1.076425016994228
+        },
+        {
+            "x": 4.6815226569833905,
+            "y": 4.625240515795651,
+            "heading": 2.6597576260457285,
+            "angularVelocity": 2.224188696537283e-7,
+            "velocityX": -3.3427772769699504,
+            "velocityY": 1.7710108366192054,
+            "timestamp": 1.174281836720976
+        },
+        {
+            "x": 4.378427967662734,
+            "y": 4.837776808087965,
+            "heading": 2.65975767630399,
+            "angularVelocity": 5.135897752366777e-7,
+            "velocityX": -3.097328220628949,
+            "velocityY": 2.171910888641101,
+            "timestamp": 1.272138656447724
+        },
+        {
+            "x": 4.050949875066163,
+            "y": 5.0103924451271915,
+            "heading": 2.6597577704243744,
+            "angularVelocity": 9.61817319833198e-7,
+            "velocityX": -3.3465025075514396,
+            "velocityY": 1.7639612397095272,
+            "timestamp": 1.369995476174472
+        },
+        {
+            "x": 3.722909272369472,
+            "y": 5.181936673224133,
+            "heading": 2.659757864941058,
+            "angularVelocity": 9.65867108780206e-7,
+            "velocityX": -3.352250804928058,
+            "velocityY": 1.7530124990363987,
+            "timestamp": 1.46785229590122
+        },
+        {
+            "x": 3.383528958103272,
+            "y": 5.329784787197771,
+            "heading": 2.659757940242804,
+            "angularVelocity": 7.6950943654844e-7,
+            "velocityX": -3.468131451787155,
+            "velocityY": 1.5108616281060843,
+            "timestamp": 1.5657091156279679
+        },
+        {
+            "x": 3.020395743291134,
+            "y": 5.401703742215627,
+            "heading": 2.6597579736142767,
+            "angularVelocity": 3.4102347755826357e-7,
+            "velocityX": -3.7108626238430706,
+            "velocityY": 0.7349406532797214,
+            "timestamp": 1.6635659353547159
+        },
+        {
+            "x": 2.7114854008089204,
+            "y": 5.466141485650731,
+            "heading": 2.823345730065705,
+            "angularVelocity": 1.6717052210385108,
+            "velocityX": -3.156758449179158,
+            "velocityY": 0.6584900634931665,
+            "timestamp": 1.7614227550814638
+        },
+        {
+            "x": 2.4804696774793777,
+            "y": 5.513370093880283,
+            "heading": 2.974776278106282,
+            "angularVelocity": 1.5474705642736624,
+            "velocityX": -2.3607524133179756,
+            "velocityY": 0.48262970696813046,
+            "timestamp": 1.8592795748082118
+        },
+        {
+            "x": 2.3267564356504185,
+            "y": 5.544523353838769,
+            "heading": 3.083396984959406,
+            "angularVelocity": 1.1099962900534985,
+            "velocityX": -1.5707974391379447,
+            "velocityY": 0.3183555325574442,
+            "timestamp": 1.9571363945349598
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0.5784268812194232,
+            "velocityX": -0.7843749251687325,
+            "velocityY": 0.15815603045805554,
+            "timestamp": 2.054993214261708
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 1.5606110446115462e-29,
+            "velocityX": 0,
+            "velocityY": 3.583632692315791e-31,
+            "timestamp": 2.152850033988456
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.1.traj
new file mode 100644
index 00000000..85d8cdac
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.1.traj	
@@ -0,0 +1,220 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 3.6158431875986385e-31,
+            "velocityX": 5.569083161704914e-32,
+            "velocityY": 2.1158895574058437e-37,
+            "timestamp": 0
+        },
+        {
+            "x": 7.680000134363673,
+            "y": 0.8649552704871222,
+            "heading": 3.140000000380936,
+            "angularVelocity": 3.939689465162077e-9,
+            "velocityX": 0.0000013896119285392994,
+            "velocityY": 0.8062267267090596,
+            "timestamp": 0.09669149868712118
+        },
+        {
+            "x": 7.680000398315331,
+            "y": 1.0208658083667637,
+            "heading": 3.14000000095338,
+            "angularVelocity": 5.920286837104207e-9,
+            "velocityX": 0.0000027298328198052245,
+            "velocityY": 1.6124534214132247,
+            "timestamp": 0.19338299737424236
+        },
+        {
+            "x": 7.680000786842507,
+            "y": 1.2547316074497201,
+            "heading": 3.1400000020167984,
+            "angularVelocity": 1.0998009739093907e-8,
+            "velocityX": 0.000004018213888590017,
+            "velocityY": 2.4186800521075957,
+            "timestamp": 0.29007449606136354
+        },
+        {
+            "x": 7.680001294600068,
+            "y": 1.566552649168411,
+            "heading": 3.140000004611078,
+            "angularVelocity": 2.6830422686000607e-8,
+            "velocityX": 0.00000525131448771192,
+            "velocityY": 3.2249064907729323,
+            "timestamp": 0.3867659947484847
+        },
+        {
+            "x": 7.680001797129468,
+            "y": 1.9323308421441416,
+            "heading": 3.1400000046110796,
+            "angularVelocity": 2.2904858507221613e-14,
+            "velocityX": 0.000005197244743376452,
+            "velocityY": 3.782940568118395,
+            "timestamp": 0.4834574934356059
+        },
+        {
+            "x": 7.680002234677101,
+            "y": 2.2981090351200577,
+            "heading": 3.1400000046110854,
+            "angularVelocity": 7.187274756998544e-14,
+            "velocityX": 0.00000452519161992871,
+            "velocityY": 3.782940568120504,
+            "timestamp": 0.5801489921227271
+        },
+        {
+            "x": 7.680002600262594,
+            "y": 2.663887228096014,
+            "heading": 3.140000004611095,
+            "angularVelocity": 1.2305801027843294e-13,
+            "velocityX": 0.000003780946939519626,
+            "velocityY": 3.782940568121035,
+            "timestamp": 0.6768404908098482
+        },
+        {
+            "x": 7.680002888746174,
+            "y": 3.029665421071995,
+            "heading": 3.1400000046111063,
+            "angularVelocity": 1.4680337346800463e-13,
+            "velocityX": 0.000002983545883280996,
+            "velocityY": 3.782940568121397,
+            "timestamp": 0.7735319894969694
+        },
+        {
+            "x": 7.680003096449986,
+            "y": 3.395443614047991,
+            "heading": 3.1400000046111183,
+            "angularVelocity": 1.4951605959877598e-13,
+            "velocityX": 0.0000021481078702221435,
+            "velocityY": 3.782940568121633,
+            "timestamp": 0.8702234881840907
+        },
+        {
+            "x": 7.680003221115393,
+            "y": 3.7612218070239956,
+            "heading": 3.1400000046111316,
+            "angularVelocity": 1.5542579572657036e-13,
+            "velocityX": 0.0000012893106851068564,
+            "velocityY": 3.7829405681217785,
+            "timestamp": 0.9669149868712119
+        },
+        {
+            "x": 7.68000326213119,
+            "y": 4.127000000000005,
+            "heading": 3.1400000046111454,
+            "angularVelocity": 1.6157627281901116e-13,
+            "velocityX": 4.241922977706735e-7,
+            "velocityY": 3.782940568121862,
+            "timestamp": 1.0636064855583331
+        },
+        {
+            "x": 7.6800032195335275,
+            "y": 4.492778192976015,
+            "heading": 3.140000004611159,
+            "angularVelocity": 1.571537708778215e-13,
+            "velocityX": -4.4055220975717935e-7,
+            "velocityY": 3.7829405681218624,
+            "timestamp": 1.1602979842454544
+        },
+        {
+            "x": 7.680003093394937,
+            "y": 4.858556385952021,
+            "heading": 3.1400000046111707,
+            "angularVelocity": 1.4279777513481356e-13,
+            "velocityX": -0.00000130454658943142,
+            "velocityY": 3.7829405681217825,
+            "timestamp": 1.2569894829325756
+        },
+        {
+            "x": 7.680002884476087,
+            "y": 5.224334578928017,
+            "heading": 3.1400000046111822,
+            "angularVelocity": 1.3985874807595513e-13,
+            "velocityX": -0.0000021606740038572212,
+            "velocityY": 3.7829405681216364,
+            "timestamp": 1.3536809816196969
+        },
+        {
+            "x": 7.680002595166353,
+            "y": 5.590112771903999,
+            "heading": 3.140000004611193,
+            "angularVelocity": 1.3691025375631502e-13,
+            "velocityX": -0.0000029920901128748315,
+            "velocityY": 3.7829405681213992,
+            "timestamp": 1.450372480306818
+        },
+        {
+            "x": 7.680002229238247,
+            "y": 5.955890964879955,
+            "heading": 3.1400000046112018,
+            "angularVelocity": 1.1312968357611626e-13,
+            "velocityX": -0.000003784490284850132,
+            "velocityY": 3.782940568121032,
+            "timestamp": 1.5470639789939393
+        },
+        {
+            "x": 7.680001791901768,
+            "y": 6.321669157855871,
+            "heading": 3.1400000046112067,
+            "angularVelocity": 6.287792233292802e-14,
+            "velocityX": -0.000004523007841510769,
+            "velocityY": 3.7829405681204946,
+            "timestamp": 1.6437554776810606
+        },
+        {
+            "x": 7.680001290195951,
+            "y": 6.687447350831601,
+            "heading": 3.1400000046112084,
+            "angularVelocity": 2.135816498669109e-14,
+            "velocityX": -0.00000518872709755724,
+            "velocityY": 3.7829405681183994,
+            "timestamp": 1.7404469763681818
+        },
+        {
+            "x": 7.68000078407037,
+            "y": 6.999268392550285,
+            "heading": 3.1400000030761217,
+            "angularVelocity": -1.5876160780832585e-8,
+            "velocityX": -0.000005234436287123649,
+            "velocityY": 3.224906490772867,
+            "timestamp": 1.837138475055303
+        },
+        {
+            "x": 7.68000039686507,
+            "y": 7.233134191633239,
+            "heading": 3.1400000013788225,
+            "angularVelocity": -1.7553792269593633e-8,
+            "velocityX": -0.000004004542818722395,
+            "velocityY": 2.4186800521075664,
+            "timestamp": 1.9338299737424243
+        },
+        {
+            "x": 7.680000133859054,
+            "y": 7.389044729512878,
+            "heading": 3.1400000003266944,
+            "angularVelocity": -1.08813147296025e-8,
+            "velocityX": -0.0000027200528426415456,
+            "velocityY": 1.6124534214132105,
+            "timestamp": 2.0305214724295455
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": -3.378739660348149e-9,
+            "velocityX": -0.000001384393079062506,
+            "velocityY": 0.806226726709054,
+            "timestamp": 2.1272129711166667
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 1.4001703039309738e-31,
+            "velocityX": 0,
+            "velocityY": 2.3509592524815357e-38,
+            "timestamp": 2.223904469803788
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.traj
new file mode 100644
index 00000000..85d8cdac
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.traj	
@@ -0,0 +1,220 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 3.6158431875986385e-31,
+            "velocityX": 5.569083161704914e-32,
+            "velocityY": 2.1158895574058437e-37,
+            "timestamp": 0
+        },
+        {
+            "x": 7.680000134363673,
+            "y": 0.8649552704871222,
+            "heading": 3.140000000380936,
+            "angularVelocity": 3.939689465162077e-9,
+            "velocityX": 0.0000013896119285392994,
+            "velocityY": 0.8062267267090596,
+            "timestamp": 0.09669149868712118
+        },
+        {
+            "x": 7.680000398315331,
+            "y": 1.0208658083667637,
+            "heading": 3.14000000095338,
+            "angularVelocity": 5.920286837104207e-9,
+            "velocityX": 0.0000027298328198052245,
+            "velocityY": 1.6124534214132247,
+            "timestamp": 0.19338299737424236
+        },
+        {
+            "x": 7.680000786842507,
+            "y": 1.2547316074497201,
+            "heading": 3.1400000020167984,
+            "angularVelocity": 1.0998009739093907e-8,
+            "velocityX": 0.000004018213888590017,
+            "velocityY": 2.4186800521075957,
+            "timestamp": 0.29007449606136354
+        },
+        {
+            "x": 7.680001294600068,
+            "y": 1.566552649168411,
+            "heading": 3.140000004611078,
+            "angularVelocity": 2.6830422686000607e-8,
+            "velocityX": 0.00000525131448771192,
+            "velocityY": 3.2249064907729323,
+            "timestamp": 0.3867659947484847
+        },
+        {
+            "x": 7.680001797129468,
+            "y": 1.9323308421441416,
+            "heading": 3.1400000046110796,
+            "angularVelocity": 2.2904858507221613e-14,
+            "velocityX": 0.000005197244743376452,
+            "velocityY": 3.782940568118395,
+            "timestamp": 0.4834574934356059
+        },
+        {
+            "x": 7.680002234677101,
+            "y": 2.2981090351200577,
+            "heading": 3.1400000046110854,
+            "angularVelocity": 7.187274756998544e-14,
+            "velocityX": 0.00000452519161992871,
+            "velocityY": 3.782940568120504,
+            "timestamp": 0.5801489921227271
+        },
+        {
+            "x": 7.680002600262594,
+            "y": 2.663887228096014,
+            "heading": 3.140000004611095,
+            "angularVelocity": 1.2305801027843294e-13,
+            "velocityX": 0.000003780946939519626,
+            "velocityY": 3.782940568121035,
+            "timestamp": 0.6768404908098482
+        },
+        {
+            "x": 7.680002888746174,
+            "y": 3.029665421071995,
+            "heading": 3.1400000046111063,
+            "angularVelocity": 1.4680337346800463e-13,
+            "velocityX": 0.000002983545883280996,
+            "velocityY": 3.782940568121397,
+            "timestamp": 0.7735319894969694
+        },
+        {
+            "x": 7.680003096449986,
+            "y": 3.395443614047991,
+            "heading": 3.1400000046111183,
+            "angularVelocity": 1.4951605959877598e-13,
+            "velocityX": 0.0000021481078702221435,
+            "velocityY": 3.782940568121633,
+            "timestamp": 0.8702234881840907
+        },
+        {
+            "x": 7.680003221115393,
+            "y": 3.7612218070239956,
+            "heading": 3.1400000046111316,
+            "angularVelocity": 1.5542579572657036e-13,
+            "velocityX": 0.0000012893106851068564,
+            "velocityY": 3.7829405681217785,
+            "timestamp": 0.9669149868712119
+        },
+        {
+            "x": 7.68000326213119,
+            "y": 4.127000000000005,
+            "heading": 3.1400000046111454,
+            "angularVelocity": 1.6157627281901116e-13,
+            "velocityX": 4.241922977706735e-7,
+            "velocityY": 3.782940568121862,
+            "timestamp": 1.0636064855583331
+        },
+        {
+            "x": 7.6800032195335275,
+            "y": 4.492778192976015,
+            "heading": 3.140000004611159,
+            "angularVelocity": 1.571537708778215e-13,
+            "velocityX": -4.4055220975717935e-7,
+            "velocityY": 3.7829405681218624,
+            "timestamp": 1.1602979842454544
+        },
+        {
+            "x": 7.680003093394937,
+            "y": 4.858556385952021,
+            "heading": 3.1400000046111707,
+            "angularVelocity": 1.4279777513481356e-13,
+            "velocityX": -0.00000130454658943142,
+            "velocityY": 3.7829405681217825,
+            "timestamp": 1.2569894829325756
+        },
+        {
+            "x": 7.680002884476087,
+            "y": 5.224334578928017,
+            "heading": 3.1400000046111822,
+            "angularVelocity": 1.3985874807595513e-13,
+            "velocityX": -0.0000021606740038572212,
+            "velocityY": 3.7829405681216364,
+            "timestamp": 1.3536809816196969
+        },
+        {
+            "x": 7.680002595166353,
+            "y": 5.590112771903999,
+            "heading": 3.140000004611193,
+            "angularVelocity": 1.3691025375631502e-13,
+            "velocityX": -0.0000029920901128748315,
+            "velocityY": 3.7829405681213992,
+            "timestamp": 1.450372480306818
+        },
+        {
+            "x": 7.680002229238247,
+            "y": 5.955890964879955,
+            "heading": 3.1400000046112018,
+            "angularVelocity": 1.1312968357611626e-13,
+            "velocityX": -0.000003784490284850132,
+            "velocityY": 3.782940568121032,
+            "timestamp": 1.5470639789939393
+        },
+        {
+            "x": 7.680001791901768,
+            "y": 6.321669157855871,
+            "heading": 3.1400000046112067,
+            "angularVelocity": 6.287792233292802e-14,
+            "velocityX": -0.000004523007841510769,
+            "velocityY": 3.7829405681204946,
+            "timestamp": 1.6437554776810606
+        },
+        {
+            "x": 7.680001290195951,
+            "y": 6.687447350831601,
+            "heading": 3.1400000046112084,
+            "angularVelocity": 2.135816498669109e-14,
+            "velocityX": -0.00000518872709755724,
+            "velocityY": 3.7829405681183994,
+            "timestamp": 1.7404469763681818
+        },
+        {
+            "x": 7.68000078407037,
+            "y": 6.999268392550285,
+            "heading": 3.1400000030761217,
+            "angularVelocity": -1.5876160780832585e-8,
+            "velocityX": -0.000005234436287123649,
+            "velocityY": 3.224906490772867,
+            "timestamp": 1.837138475055303
+        },
+        {
+            "x": 7.68000039686507,
+            "y": 7.233134191633239,
+            "heading": 3.1400000013788225,
+            "angularVelocity": -1.7553792269593633e-8,
+            "velocityX": -0.000004004542818722395,
+            "velocityY": 2.4186800521075664,
+            "timestamp": 1.9338299737424243
+        },
+        {
+            "x": 7.680000133859054,
+            "y": 7.389044729512878,
+            "heading": 3.1400000003266944,
+            "angularVelocity": -1.08813147296025e-8,
+            "velocityX": -0.0000027200528426415456,
+            "velocityY": 1.6124534214132105,
+            "timestamp": 2.0305214724295455
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": -3.378739660348149e-9,
+            "velocityX": -0.000001384393079062506,
+            "velocityY": 0.806226726709054,
+            "timestamp": 2.1272129711166667
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 1.4001703039309738e-31,
+            "velocityX": 0,
+            "velocityY": 2.3509592524815357e-38,
+            "timestamp": 2.223904469803788
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.1.traj
new file mode 100644
index 00000000..de1957d8
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.1.traj	
@@ -0,0 +1,175 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": -9.959658814138394e-32,
+            "velocityX": -4.727301624016019e-32,
+            "velocityY": 3.595241492576121e-38,
+            "timestamp": 0
+        },
+        {
+            "x": 7.680000100608845,
+            "y": 0.8688441539517371,
+            "heading": 3.13999998007937,
+            "angularVelocity": -2.0106831809452372e-7,
+            "velocityX": 0.0000010154926233848571,
+            "velocityY": 0.826091745427235,
+            "timestamp": 0.09907392781101573
+        },
+        {
+            "x": 7.680000296760758,
+            "y": 1.0325324587418605,
+            "heading": 3.139999940084766,
+            "angularVelocity": -4.036843910631592e-7,
+            "velocityX": 0.0000019798539067481827,
+            "velocityY": 1.6521834594299558,
+            "timestamp": 0.19814785562203147
+        },
+        {
+            "x": 7.680000583126213,
+            "y": 1.2780649081436684,
+            "heading": 3.1399998804112537,
+            "angularVelocity": -6.023128542087543e-7,
+            "velocityX": 0.0000028904217813218476,
+            "velocityY": 2.47827511058363,
+            "timestamp": 0.29722178343304717
+        },
+        {
+            "x": 7.680000954004463,
+            "y": 1.6054414834770696,
+            "heading": 3.13999980228898,
+            "angularVelocity": -7.885247914488142e-7,
+            "velocityX": 0.0000037434492396662855,
+            "velocityY": 3.3043665731903316,
+            "timestamp": 0.39629571124406293
+        },
+        {
+            "x": 7.680001281447094,
+            "y": 1.9802322649264825,
+            "heading": 3.1399998022889792,
+            "angularVelocity": -1.0474954246602775e-14,
+            "velocityX": 0.0000033050332881388687,
+            "velocityY": 3.7829405750856777,
+            "timestamp": 0.4953696390550787
+        },
+        {
+            "x": 7.6800015408657325,
+            "y": 2.355023046375991,
+            "heading": 3.13999980228898,
+            "angularVelocity": 7.833153823506449e-15,
+            "velocityX": 0.0000026184347759635413,
+            "velocityY": 3.7829405750867062,
+            "timestamp": 0.5944435668660945
+        },
+        {
+            "x": 7.680001725168594,
+            "y": 2.729813827825533,
+            "heading": 3.139999802288987,
+            "angularVelocity": 7.465414071871633e-14,
+            "velocityX": 0.0000018602557564557753,
+            "velocityY": 3.7829405750870757,
+            "timestamp": 0.6935174946771102
+        },
+        {
+            "x": 7.680001829195369,
+            "y": 3.104604609275094,
+            "heading": 3.1399998022889952,
+            "angularVelocity": 9.203271146236807e-14,
+            "velocityX": 0.0000010499913044241082,
+            "velocityY": 3.7829405750872978,
+            "timestamp": 0.792591422488126
+        },
+        {
+            "x": 7.680001849352207,
+            "y": 3.479395390724662,
+            "heading": 3.1399998022890023,
+            "angularVelocity": 7.71401741801888e-14,
+            "velocityX": 2.034524499813149e-7,
+            "velocityY": 3.7829405750873777,
+            "timestamp": 0.8916653502991417
+        },
+        {
+            "x": 7.6800017836349985,
+            "y": 3.8541861721742245,
+            "heading": 3.1399998022890094,
+            "angularVelocity": 7.688638756481334e-14,
+            "velocityX": -6.633148330797741e-7,
+            "velocityY": 3.7829405750873186,
+            "timestamp": 0.9907392781101575
+        },
+        {
+            "x": 7.68000163167861,
+            "y": 4.22897695362377,
+            "heading": 3.1399998022890157,
+            "angularVelocity": 6.981254883852486e-14,
+            "velocityX": -0.000001533767606062718,
+            "velocityY": 3.7829405750871206,
+            "timestamp": 1.0898132059211731
+        },
+        {
+            "x": 7.680001393541454,
+            "y": 4.603767735073283,
+            "heading": 3.139999802289022,
+            "angularVelocity": 6.191049012077391e-14,
+            "velocityX": -0.00000240363076954954,
+            "velocityY": 3.782940575086774,
+            "timestamp": 1.188887133732189
+        },
+        {
+            "x": 7.680001069388574,
+            "y": 4.978558516522716,
+            "heading": 3.139999802289024,
+            "angularVelocity": 2.5336752196891655e-14,
+            "velocityX": -0.000003271828396587149,
+            "velocityY": 3.782940575085891,
+            "timestamp": 1.2879610615432047
+        },
+        {
+            "x": 7.680000659196884,
+            "y": 5.305935091856245,
+            "heading": 3.139999885175708,
+            "angularVelocity": 8.366142293360243e-7,
+            "velocityX": -0.000004140258200407122,
+            "velocityY": 3.304366573191614,
+            "timestamp": 1.3870349893542204
+        },
+        {
+            "x": 7.6800003382888615,
+            "y": 5.5514675412581065,
+            "heading": 3.1399999457053744,
+            "angularVelocity": 6.10954418213692e-7,
+            "velocityX": -0.0000032390761240661895,
+            "velocityY": 2.478275110584155,
+            "timestamp": 1.4861089171652362
+        },
+        {
+            "x": 7.6800001156321125,
+            "y": 5.715155846048254,
+            "heading": 3.139999983279866,
+            "angularVelocity": 3.792570390671995e-7,
+            "velocityX": -0.000002247379691011521,
+            "velocityY": 1.6521834594301872,
+            "timestamp": 1.585182844976252
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 1.687641935617777e-7,
+            "velocityX": -0.0000011671295149488337,
+            "velocityY": 0.8260917454273095,
+            "timestamp": 1.6842567727872677
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": -8.926921159411167e-32,
+            "velocityX": 0,
+            "velocityY": 7.102031651059941e-38,
+            "timestamp": 1.7833307005982835
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.traj
new file mode 100644
index 00000000..de1957d8
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.traj	
@@ -0,0 +1,175 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": -9.959658814138394e-32,
+            "velocityX": -4.727301624016019e-32,
+            "velocityY": 3.595241492576121e-38,
+            "timestamp": 0
+        },
+        {
+            "x": 7.680000100608845,
+            "y": 0.8688441539517371,
+            "heading": 3.13999998007937,
+            "angularVelocity": -2.0106831809452372e-7,
+            "velocityX": 0.0000010154926233848571,
+            "velocityY": 0.826091745427235,
+            "timestamp": 0.09907392781101573
+        },
+        {
+            "x": 7.680000296760758,
+            "y": 1.0325324587418605,
+            "heading": 3.139999940084766,
+            "angularVelocity": -4.036843910631592e-7,
+            "velocityX": 0.0000019798539067481827,
+            "velocityY": 1.6521834594299558,
+            "timestamp": 0.19814785562203147
+        },
+        {
+            "x": 7.680000583126213,
+            "y": 1.2780649081436684,
+            "heading": 3.1399998804112537,
+            "angularVelocity": -6.023128542087543e-7,
+            "velocityX": 0.0000028904217813218476,
+            "velocityY": 2.47827511058363,
+            "timestamp": 0.29722178343304717
+        },
+        {
+            "x": 7.680000954004463,
+            "y": 1.6054414834770696,
+            "heading": 3.13999980228898,
+            "angularVelocity": -7.885247914488142e-7,
+            "velocityX": 0.0000037434492396662855,
+            "velocityY": 3.3043665731903316,
+            "timestamp": 0.39629571124406293
+        },
+        {
+            "x": 7.680001281447094,
+            "y": 1.9802322649264825,
+            "heading": 3.1399998022889792,
+            "angularVelocity": -1.0474954246602775e-14,
+            "velocityX": 0.0000033050332881388687,
+            "velocityY": 3.7829405750856777,
+            "timestamp": 0.4953696390550787
+        },
+        {
+            "x": 7.6800015408657325,
+            "y": 2.355023046375991,
+            "heading": 3.13999980228898,
+            "angularVelocity": 7.833153823506449e-15,
+            "velocityX": 0.0000026184347759635413,
+            "velocityY": 3.7829405750867062,
+            "timestamp": 0.5944435668660945
+        },
+        {
+            "x": 7.680001725168594,
+            "y": 2.729813827825533,
+            "heading": 3.139999802288987,
+            "angularVelocity": 7.465414071871633e-14,
+            "velocityX": 0.0000018602557564557753,
+            "velocityY": 3.7829405750870757,
+            "timestamp": 0.6935174946771102
+        },
+        {
+            "x": 7.680001829195369,
+            "y": 3.104604609275094,
+            "heading": 3.1399998022889952,
+            "angularVelocity": 9.203271146236807e-14,
+            "velocityX": 0.0000010499913044241082,
+            "velocityY": 3.7829405750872978,
+            "timestamp": 0.792591422488126
+        },
+        {
+            "x": 7.680001849352207,
+            "y": 3.479395390724662,
+            "heading": 3.1399998022890023,
+            "angularVelocity": 7.71401741801888e-14,
+            "velocityX": 2.034524499813149e-7,
+            "velocityY": 3.7829405750873777,
+            "timestamp": 0.8916653502991417
+        },
+        {
+            "x": 7.6800017836349985,
+            "y": 3.8541861721742245,
+            "heading": 3.1399998022890094,
+            "angularVelocity": 7.688638756481334e-14,
+            "velocityX": -6.633148330797741e-7,
+            "velocityY": 3.7829405750873186,
+            "timestamp": 0.9907392781101575
+        },
+        {
+            "x": 7.68000163167861,
+            "y": 4.22897695362377,
+            "heading": 3.1399998022890157,
+            "angularVelocity": 6.981254883852486e-14,
+            "velocityX": -0.000001533767606062718,
+            "velocityY": 3.7829405750871206,
+            "timestamp": 1.0898132059211731
+        },
+        {
+            "x": 7.680001393541454,
+            "y": 4.603767735073283,
+            "heading": 3.139999802289022,
+            "angularVelocity": 6.191049012077391e-14,
+            "velocityX": -0.00000240363076954954,
+            "velocityY": 3.782940575086774,
+            "timestamp": 1.188887133732189
+        },
+        {
+            "x": 7.680001069388574,
+            "y": 4.978558516522716,
+            "heading": 3.139999802289024,
+            "angularVelocity": 2.5336752196891655e-14,
+            "velocityX": -0.000003271828396587149,
+            "velocityY": 3.782940575085891,
+            "timestamp": 1.2879610615432047
+        },
+        {
+            "x": 7.680000659196884,
+            "y": 5.305935091856245,
+            "heading": 3.139999885175708,
+            "angularVelocity": 8.366142293360243e-7,
+            "velocityX": -0.000004140258200407122,
+            "velocityY": 3.304366573191614,
+            "timestamp": 1.3870349893542204
+        },
+        {
+            "x": 7.6800003382888615,
+            "y": 5.5514675412581065,
+            "heading": 3.1399999457053744,
+            "angularVelocity": 6.10954418213692e-7,
+            "velocityX": -0.0000032390761240661895,
+            "velocityY": 2.478275110584155,
+            "timestamp": 1.4861089171652362
+        },
+        {
+            "x": 7.6800001156321125,
+            "y": 5.715155846048254,
+            "heading": 3.139999983279866,
+            "angularVelocity": 3.792570390671995e-7,
+            "velocityX": -0.000002247379691011521,
+            "velocityY": 1.6521834594301872,
+            "timestamp": 1.585182844976252
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 1.687641935617777e-7,
+            "velocityX": -0.0000011671295149488337,
+            "velocityY": 0.8260917454273095,
+            "timestamp": 1.6842567727872677
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": -8.926921159411167e-32,
+            "velocityX": 0,
+            "velocityY": 7.102031651059941e-38,
+            "timestamp": 1.7833307005982835
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.1.traj
new file mode 100644
index 00000000..b9d634dc
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.1.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -4.029789856147219e-33,
+            "velocityY": -3.379656030870708e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 7.680000025226674,
+            "y": 0.87050000557584,
+            "heading": 3.139999988557334,
+            "angularVelocity": -1.1434532837257691e-7,
+            "velocityX": 2.5208743257476917e-7,
+            "velocityY": 0.8344065481227664,
+            "timestamp": 0.10007112931184067
+        },
+        {
+            "x": 7.680000070528317,
+            "y": 1.03750001362983,
+            "heading": 3.1399999655365143,
+            "angularVelocity": -2.300445661244102e-7,
+            "velocityX": 4.5269443024398455e-7,
+            "velocityY": 1.6688130652906519,
+            "timestamp": 0.20014225862368135
+        },
+        {
+            "x": 7.6800001304795185,
+            "y": 1.2880000179665891,
+            "heading": 3.139999931372676,
+            "angularVelocity": -3.413955478564011e-7,
+            "velocityX": 5.990858804609274e-7,
+            "velocityY": 2.5032195205487646,
+            "timestamp": 0.300213387935522
+        },
+        {
+            "x": 7.680000199269358,
+            "y": 1.6219999999999795,
+            "heading": 3.1399998873718746,
+            "angularVelocity": -4.396952561787762e-7,
+            "velocityX": 6.874094371094506e-7,
+            "velocityY": 3.337625790077605,
+            "timestamp": 0.4002845172473627
+        },
+        {
+            "x": 7.680000135199738,
+            "y": 1.9559999820333853,
+            "heading": 3.1399999178431197,
+            "angularVelocity": 3.0449586057484084e-7,
+            "velocityX": -6.402407947941681e-7,
+            "velocityY": 3.3376257900777593,
+            "timestamp": 0.5003556465592034
+        },
+        {
+            "x": 7.680000075581297,
+            "y": 2.206499986370157,
+            "heading": 3.139999951084868,
+            "angularVelocity": 3.3218120651110665e-7,
+            "velocityX": -5.957606422672805e-7,
+            "velocityY": 2.503219520548894,
+            "timestamp": 0.6004267758710441
+        },
+        {
+            "x": 7.680000027896081,
+            "y": 2.373499994424156,
+            "heading": 3.13999998094686,
+            "angularVelocity": 2.984076655658666e-7,
+            "velocityX": -4.765132174434968e-7,
+            "velocityY": 1.6688130652907383,
+            "timestamp": 0.7004979051828848
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 1.903959699368805e-7,
+            "velocityX": -2.787625301130355e-7,
+            "velocityY": 0.8344065481228072,
+            "timestamp": 0.8005690344947255
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -2.2348525624284448e-32,
+            "velocityX": 0,
+            "velocityY": 2.3896717289528774e-36,
+            "timestamp": 0.9006401638065662
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.traj
new file mode 100644
index 00000000..b9d634dc
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -4.029789856147219e-33,
+            "velocityY": -3.379656030870708e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 7.680000025226674,
+            "y": 0.87050000557584,
+            "heading": 3.139999988557334,
+            "angularVelocity": -1.1434532837257691e-7,
+            "velocityX": 2.5208743257476917e-7,
+            "velocityY": 0.8344065481227664,
+            "timestamp": 0.10007112931184067
+        },
+        {
+            "x": 7.680000070528317,
+            "y": 1.03750001362983,
+            "heading": 3.1399999655365143,
+            "angularVelocity": -2.300445661244102e-7,
+            "velocityX": 4.5269443024398455e-7,
+            "velocityY": 1.6688130652906519,
+            "timestamp": 0.20014225862368135
+        },
+        {
+            "x": 7.6800001304795185,
+            "y": 1.2880000179665891,
+            "heading": 3.139999931372676,
+            "angularVelocity": -3.413955478564011e-7,
+            "velocityX": 5.990858804609274e-7,
+            "velocityY": 2.5032195205487646,
+            "timestamp": 0.300213387935522
+        },
+        {
+            "x": 7.680000199269358,
+            "y": 1.6219999999999795,
+            "heading": 3.1399998873718746,
+            "angularVelocity": -4.396952561787762e-7,
+            "velocityX": 6.874094371094506e-7,
+            "velocityY": 3.337625790077605,
+            "timestamp": 0.4002845172473627
+        },
+        {
+            "x": 7.680000135199738,
+            "y": 1.9559999820333853,
+            "heading": 3.1399999178431197,
+            "angularVelocity": 3.0449586057484084e-7,
+            "velocityX": -6.402407947941681e-7,
+            "velocityY": 3.3376257900777593,
+            "timestamp": 0.5003556465592034
+        },
+        {
+            "x": 7.680000075581297,
+            "y": 2.206499986370157,
+            "heading": 3.139999951084868,
+            "angularVelocity": 3.3218120651110665e-7,
+            "velocityX": -5.957606422672805e-7,
+            "velocityY": 2.503219520548894,
+            "timestamp": 0.6004267758710441
+        },
+        {
+            "x": 7.680000027896081,
+            "y": 2.373499994424156,
+            "heading": 3.13999998094686,
+            "angularVelocity": 2.984076655658666e-7,
+            "velocityX": -4.765132174434968e-7,
+            "velocityY": 1.6688130652907383,
+            "timestamp": 0.7004979051828848
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 1.903959699368805e-7,
+            "velocityX": -2.787625301130355e-7,
+            "velocityY": 0.8344065481228072,
+            "timestamp": 0.8005690344947255
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -2.2348525624284448e-32,
+            "velocityX": 0,
+            "velocityY": 2.3896717289528774e-36,
+            "timestamp": 0.9006401638065662
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.1.traj
new file mode 100644
index 00000000..9d086628
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.1.traj	
@@ -0,0 +1,184 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 9.969319932712997e-31,
+            "velocityX": 3.5015968931749707e-32,
+            "velocityY": -1.3449299836267862e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 7.622998015019161,
+            "y": 0.8057692963239994,
+            "heading": 3.0732063030265673,
+            "angularVelocity": -0.7701918865802806,
+            "velocityX": -0.6572845693610088,
+            "velocityY": 0.21642700435214937,
+            "timestamp": 0.08672344923029876
+        },
+        {
+            "x": 7.508611650111109,
+            "y": 0.8437411084542301,
+            "heading": 2.9459376863662614,
+            "angularVelocity": -1.4675225419406142,
+            "velocityX": -1.318978499163396,
+            "velocityY": 0.43784942212566424,
+            "timestamp": 0.17344689846059752
+        },
+        {
+            "x": 7.336139052754301,
+            "y": 0.9014260823029367,
+            "heading": 2.7711998622663616,
+            "angularVelocity": -2.014885543076985,
+            "velocityX": -1.9887654248944688,
+            "velocityY": 0.6651600502595449,
+            "timestamp": 0.2601703476908963
+        },
+        {
+            "x": 7.104576647516153,
+            "y": 0.9793814888777497,
+            "heading": 2.5768200425700405,
+            "angularVelocity": -2.241375561298711,
+            "velocityX": -2.6701244852844987,
+            "velocityY": 0.8988965183776096,
+            "timestamp": 0.34689379692119504
+        },
+        {
+            "x": 6.822163575098835,
+            "y": 1.078219287033468,
+            "heading": 2.4981665441447336,
+            "angularVelocity": -0.9069461503593836,
+            "velocityX": -3.2564787831184523,
+            "velocityY": 1.1396894269420643,
+            "timestamp": 0.4336172461514938
+        },
+        {
+            "x": 6.510888238397409,
+            "y": 1.1818405008088049,
+            "heading": 2.498166488214478,
+            "angularVelocity": -6.449265521718259e-7,
+            "velocityX": -3.5892868591379226,
+            "velocityY": 1.194846546061207,
+            "timestamp": 0.5203406953817925
+        },
+        {
+            "x": 6.199612902811833,
+            "y": 1.2854617179365646,
+            "heading": 2.498166432284442,
+            "angularVelocity": -6.449240327985389e-7,
+            "velocityX": -3.589286846271159,
+            "velocityY": 1.194846584717682,
+            "timestamp": 0.6070641446120912
+        },
+        {
+            "x": 5.8883375891897005,
+            "y": 1.389083001041933,
+            "heading": 2.4981663763544435,
+            "angularVelocity": -6.449235898174935e-7,
+            "velocityX": -3.5892865930127487,
+            "velocityY": 1.1948473454993283,
+            "timestamp": 0.69378759384239
+        },
+        {
+            "x": 5.577064983818878,
+            "y": 1.4927124193076864,
+            "heading": 2.4981663204239246,
+            "angularVelocity": -6.449295917693127e-7,
+            "velocityX": -3.589255364419621,
+            "velocityY": 1.194941151274556,
+            "timestamp": 0.7805110430726887
+        },
+        {
+            "x": 5.275080475665308,
+            "y": 1.6209116670289339,
+            "heading": 2.4981662550669967,
+            "angularVelocity": -7.536246362770858e-7,
+            "velocityX": -3.4821551821772507,
+            "velocityY": 1.4782535618573864,
+            "timestamp": 0.8672344923029874
+        },
+        {
+            "x": 4.987205237443244,
+            "y": 1.7782569230783876,
+            "heading": 2.49816614132643,
+            "angularVelocity": -0.000001311531862273131,
+            "velocityX": -3.319462507281,
+            "velocityY": 1.8143334639702267,
+            "timestamp": 0.9539579415332862
+        },
+        {
+            "x": 4.728299565339017,
+            "y": 1.9797451797609154,
+            "heading": 2.498165933556362,
+            "angularVelocity": -0.0000023957772582798155,
+            "velocityX": -2.9854171438302592,
+            "velocityY": 2.3233422848238554,
+            "timestamp": 1.040681390763585
+        },
+        {
+            "x": 4.523329416895473,
+            "y": 2.207660623196326,
+            "heading": 2.432888839209935,
+            "angularVelocity": -0.7527040832183707,
+            "velocityX": -2.3634916537882953,
+            "velocityY": 2.6280717090733883,
+            "timestamp": 1.1274048399938839
+        },
+        {
+            "x": 4.381013836981984,
+            "y": 2.433217084695142,
+            "heading": 2.3632823552491717,
+            "angularVelocity": -0.8026258708405342,
+            "velocityX": -1.6410276710231155,
+            "velocityY": 2.600870508503859,
+            "timestamp": 1.2141282892241827
+        },
+        {
+            "x": 4.294783443237854,
+            "y": 2.6401553266407616,
+            "heading": 2.3696134751420326,
+            "angularVelocity": 0.07300355266138053,
+            "velocityX": -0.9943146232011729,
+            "velocityY": 2.386185556297287,
+            "timestamp": 1.3008517384544815
+        },
+        {
+            "x": 4.26053013180991,
+            "y": 2.816849773520389,
+            "heading": 2.4414538337655816,
+            "angularVelocity": 0.8283844711108445,
+            "velocityX": -0.39497173753989284,
+            "velocityY": 2.0374471777570338,
+            "timestamp": 1.3875751876847804
+        },
+        {
+            "x": 4.257469858369506,
+            "y": 2.9392038098412794,
+            "heading": 2.522566294218638,
+            "angularVelocity": 0.9353002120298306,
+            "velocityX": -0.035287727455096035,
+            "velocityY": 1.4108529746778473,
+            "timestamp": 1.4742986369150792
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0.5930772615463757,
+            "velocityX": 0.06376754706571511,
+            "velocityY": 0.7010351951900964,
+            "timestamp": 1.561022086145378
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -5.347216163770528e-32,
+            "velocityX": 0,
+            "velocityY": -5.6953276102603335e-33,
+            "timestamp": 1.647745535375677
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.traj
new file mode 100644
index 00000000..9d086628
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.traj	
@@ -0,0 +1,184 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 9.969319932712997e-31,
+            "velocityX": 3.5015968931749707e-32,
+            "velocityY": -1.3449299836267862e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 7.622998015019161,
+            "y": 0.8057692963239994,
+            "heading": 3.0732063030265673,
+            "angularVelocity": -0.7701918865802806,
+            "velocityX": -0.6572845693610088,
+            "velocityY": 0.21642700435214937,
+            "timestamp": 0.08672344923029876
+        },
+        {
+            "x": 7.508611650111109,
+            "y": 0.8437411084542301,
+            "heading": 2.9459376863662614,
+            "angularVelocity": -1.4675225419406142,
+            "velocityX": -1.318978499163396,
+            "velocityY": 0.43784942212566424,
+            "timestamp": 0.17344689846059752
+        },
+        {
+            "x": 7.336139052754301,
+            "y": 0.9014260823029367,
+            "heading": 2.7711998622663616,
+            "angularVelocity": -2.014885543076985,
+            "velocityX": -1.9887654248944688,
+            "velocityY": 0.6651600502595449,
+            "timestamp": 0.2601703476908963
+        },
+        {
+            "x": 7.104576647516153,
+            "y": 0.9793814888777497,
+            "heading": 2.5768200425700405,
+            "angularVelocity": -2.241375561298711,
+            "velocityX": -2.6701244852844987,
+            "velocityY": 0.8988965183776096,
+            "timestamp": 0.34689379692119504
+        },
+        {
+            "x": 6.822163575098835,
+            "y": 1.078219287033468,
+            "heading": 2.4981665441447336,
+            "angularVelocity": -0.9069461503593836,
+            "velocityX": -3.2564787831184523,
+            "velocityY": 1.1396894269420643,
+            "timestamp": 0.4336172461514938
+        },
+        {
+            "x": 6.510888238397409,
+            "y": 1.1818405008088049,
+            "heading": 2.498166488214478,
+            "angularVelocity": -6.449265521718259e-7,
+            "velocityX": -3.5892868591379226,
+            "velocityY": 1.194846546061207,
+            "timestamp": 0.5203406953817925
+        },
+        {
+            "x": 6.199612902811833,
+            "y": 1.2854617179365646,
+            "heading": 2.498166432284442,
+            "angularVelocity": -6.449240327985389e-7,
+            "velocityX": -3.589286846271159,
+            "velocityY": 1.194846584717682,
+            "timestamp": 0.6070641446120912
+        },
+        {
+            "x": 5.8883375891897005,
+            "y": 1.389083001041933,
+            "heading": 2.4981663763544435,
+            "angularVelocity": -6.449235898174935e-7,
+            "velocityX": -3.5892865930127487,
+            "velocityY": 1.1948473454993283,
+            "timestamp": 0.69378759384239
+        },
+        {
+            "x": 5.577064983818878,
+            "y": 1.4927124193076864,
+            "heading": 2.4981663204239246,
+            "angularVelocity": -6.449295917693127e-7,
+            "velocityX": -3.589255364419621,
+            "velocityY": 1.194941151274556,
+            "timestamp": 0.7805110430726887
+        },
+        {
+            "x": 5.275080475665308,
+            "y": 1.6209116670289339,
+            "heading": 2.4981662550669967,
+            "angularVelocity": -7.536246362770858e-7,
+            "velocityX": -3.4821551821772507,
+            "velocityY": 1.4782535618573864,
+            "timestamp": 0.8672344923029874
+        },
+        {
+            "x": 4.987205237443244,
+            "y": 1.7782569230783876,
+            "heading": 2.49816614132643,
+            "angularVelocity": -0.000001311531862273131,
+            "velocityX": -3.319462507281,
+            "velocityY": 1.8143334639702267,
+            "timestamp": 0.9539579415332862
+        },
+        {
+            "x": 4.728299565339017,
+            "y": 1.9797451797609154,
+            "heading": 2.498165933556362,
+            "angularVelocity": -0.0000023957772582798155,
+            "velocityX": -2.9854171438302592,
+            "velocityY": 2.3233422848238554,
+            "timestamp": 1.040681390763585
+        },
+        {
+            "x": 4.523329416895473,
+            "y": 2.207660623196326,
+            "heading": 2.432888839209935,
+            "angularVelocity": -0.7527040832183707,
+            "velocityX": -2.3634916537882953,
+            "velocityY": 2.6280717090733883,
+            "timestamp": 1.1274048399938839
+        },
+        {
+            "x": 4.381013836981984,
+            "y": 2.433217084695142,
+            "heading": 2.3632823552491717,
+            "angularVelocity": -0.8026258708405342,
+            "velocityX": -1.6410276710231155,
+            "velocityY": 2.600870508503859,
+            "timestamp": 1.2141282892241827
+        },
+        {
+            "x": 4.294783443237854,
+            "y": 2.6401553266407616,
+            "heading": 2.3696134751420326,
+            "angularVelocity": 0.07300355266138053,
+            "velocityX": -0.9943146232011729,
+            "velocityY": 2.386185556297287,
+            "timestamp": 1.3008517384544815
+        },
+        {
+            "x": 4.26053013180991,
+            "y": 2.816849773520389,
+            "heading": 2.4414538337655816,
+            "angularVelocity": 0.8283844711108445,
+            "velocityX": -0.39497173753989284,
+            "velocityY": 2.0374471777570338,
+            "timestamp": 1.3875751876847804
+        },
+        {
+            "x": 4.257469858369506,
+            "y": 2.9392038098412794,
+            "heading": 2.522566294218638,
+            "angularVelocity": 0.9353002120298306,
+            "velocityX": -0.035287727455096035,
+            "velocityY": 1.4108529746778473,
+            "timestamp": 1.4742986369150792
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0.5930772615463757,
+            "velocityX": 0.06376754706571511,
+            "velocityY": 0.7010351951900964,
+            "timestamp": 1.561022086145378
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -5.347216163770528e-32,
+            "velocityX": 0,
+            "velocityY": -5.6953276102603335e-33,
+            "timestamp": 1.647745535375677
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.1.traj
new file mode 100644
index 00000000..0acd095d
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 7.117315018424497e-31,
+            "velocityX": 3.987995408855834e-32,
+            "velocityY": -2.0629863005203966e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 7.633523057017953,
+            "y": 0.8024232011655344,
+            "heading": 3.1133262335807257,
+            "angularVelocity": -0.3460902136380606,
+            "velocityX": -0.6030350147430711,
+            "velocityY": 0.20011493324412974,
+            "timestamp": 0.07707171531631223
+        },
+        {
+            "x": 7.540480034234958,
+            "y": 0.8333108599665054,
+            "heading": 3.0624083977372165,
+            "angularVelocity": -0.6606552823501594,
+            "velocityX": -1.2072265733432221,
+            "velocityY": 0.4007651662377571,
+            "timestamp": 0.15414343063262445
+        },
+        {
+            "x": 7.40074665357272,
+            "y": 0.8797155912265175,
+            "heading": 2.9910865272302494,
+            "angularVelocity": -0.9253961743845064,
+            "velocityX": -1.813030631130417,
+            "velocityY": 0.6020980728086949,
+            "timestamp": 0.23121514594893666
+        },
+        {
+            "x": 7.214152290317413,
+            "y": 0.941701209252225,
+            "heading": 2.9061891700546396,
+            "angularVelocity": -1.101537144037597,
+            "velocityX": -2.4210485323896127,
+            "velocityY": 0.804258965449392,
+            "timestamp": 0.3082868612652489
+        },
+        {
+            "x": 6.98056152298191,
+            "y": 1.0193157827445936,
+            "heading": 2.8228339071694943,
+            "angularVelocity": -1.0815285807905615,
+            "velocityX": -3.0308235177693503,
+            "velocityY": 1.007043546050951,
+            "timestamp": 0.38535857658156114
+        },
+        {
+            "x": 6.704462217906169,
+            "y": 1.1110743550526192,
+            "heading": 2.820724227783145,
+            "angularVelocity": -0.027372939316202514,
+            "velocityX": -3.582368758014451,
+            "velocityY": 1.1905609201953868,
+            "timestamp": 0.4624302918978734
+        },
+        {
+            "x": 6.4277914338210325,
+            "y": 1.2030481222509408,
+            "heading": 2.8207241988506935,
+            "angularVelocity": -3.753964944317793e-7,
+            "velocityX": -3.589783657333227,
+            "velocityY": 1.1933530585228214,
+            "timestamp": 0.5395020072141856
+        },
+        {
+            "x": 6.151120668434435,
+            "y": 1.2950219456972574,
+            "heading": 2.8207241699182544,
+            "angularVelocity": -3.753963293976773e-7,
+            "velocityX": -3.5897834147210324,
+            "velocityY": 1.1933537883365417,
+            "timestamp": 0.6165737225304978
+        },
+        {
+            "x": 5.875226516165077,
+            "y": 1.387237765393963,
+            "heading": 2.8184428738821614,
+            "angularVelocity": -0.029599653085833715,
+            "velocityX": -3.5797069150083383,
+            "velocityY": 1.1964936723964186,
+            "timestamp": 0.69364543784681
+        },
+        {
+            "x": 5.641686532929837,
+            "y": 1.4649564505690207,
+            "heading": 2.7347434741228605,
+            "angularVelocity": -1.0859937321465907,
+            "velocityX": -3.0301645976966087,
+            "velocityY": 1.0083943876958055,
+            "timestamp": 0.7707171531631222
+        },
+        {
+            "x": 5.455142642963306,
+            "y": 1.5270452896537086,
+            "heading": 2.6494963329119967,
+            "angularVelocity": -1.1060755669054276,
+            "velocityX": -2.4203936450737,
+            "velocityY": 0.8055982513152498,
+            "timestamp": 0.8477888684794344
+        },
+        {
+            "x": 5.315456646631149,
+            "y": 1.5735510161760224,
+            "heading": 2.577919230350482,
+            "angularVelocity": -0.928707792057741,
+            "velocityX": -1.812415822832897,
+            "velocityY": 0.6034084791216642,
+            "timestamp": 0.9248605837957465
+        },
+        {
+            "x": 5.222452869215715,
+            "y": 1.6045247487520136,
+            "heading": 2.5268085106209885,
+            "angularVelocity": -0.6631579369906109,
+            "velocityX": -1.2067173675029015,
+            "velocityY": 0.4018819673192824,
+            "timestamp": 1.0019322991120587
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.3478385100287815,
+            "velocityX": -0.6027226593448243,
+            "velocityY": 0.200790279345336,
+            "timestamp": 1.079004014428371
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -3.134288783198311e-31,
+            "velocityX": -2.9694883024269136e-32,
+            "velocityY": 4.283237987843841e-31,
+            "timestamp": 1.1560757297446833
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.traj
new file mode 100644
index 00000000..0acd095d
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 7.117315018424497e-31,
+            "velocityX": 3.987995408855834e-32,
+            "velocityY": -2.0629863005203966e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 7.633523057017953,
+            "y": 0.8024232011655344,
+            "heading": 3.1133262335807257,
+            "angularVelocity": -0.3460902136380606,
+            "velocityX": -0.6030350147430711,
+            "velocityY": 0.20011493324412974,
+            "timestamp": 0.07707171531631223
+        },
+        {
+            "x": 7.540480034234958,
+            "y": 0.8333108599665054,
+            "heading": 3.0624083977372165,
+            "angularVelocity": -0.6606552823501594,
+            "velocityX": -1.2072265733432221,
+            "velocityY": 0.4007651662377571,
+            "timestamp": 0.15414343063262445
+        },
+        {
+            "x": 7.40074665357272,
+            "y": 0.8797155912265175,
+            "heading": 2.9910865272302494,
+            "angularVelocity": -0.9253961743845064,
+            "velocityX": -1.813030631130417,
+            "velocityY": 0.6020980728086949,
+            "timestamp": 0.23121514594893666
+        },
+        {
+            "x": 7.214152290317413,
+            "y": 0.941701209252225,
+            "heading": 2.9061891700546396,
+            "angularVelocity": -1.101537144037597,
+            "velocityX": -2.4210485323896127,
+            "velocityY": 0.804258965449392,
+            "timestamp": 0.3082868612652489
+        },
+        {
+            "x": 6.98056152298191,
+            "y": 1.0193157827445936,
+            "heading": 2.8228339071694943,
+            "angularVelocity": -1.0815285807905615,
+            "velocityX": -3.0308235177693503,
+            "velocityY": 1.007043546050951,
+            "timestamp": 0.38535857658156114
+        },
+        {
+            "x": 6.704462217906169,
+            "y": 1.1110743550526192,
+            "heading": 2.820724227783145,
+            "angularVelocity": -0.027372939316202514,
+            "velocityX": -3.582368758014451,
+            "velocityY": 1.1905609201953868,
+            "timestamp": 0.4624302918978734
+        },
+        {
+            "x": 6.4277914338210325,
+            "y": 1.2030481222509408,
+            "heading": 2.8207241988506935,
+            "angularVelocity": -3.753964944317793e-7,
+            "velocityX": -3.589783657333227,
+            "velocityY": 1.1933530585228214,
+            "timestamp": 0.5395020072141856
+        },
+        {
+            "x": 6.151120668434435,
+            "y": 1.2950219456972574,
+            "heading": 2.8207241699182544,
+            "angularVelocity": -3.753963293976773e-7,
+            "velocityX": -3.5897834147210324,
+            "velocityY": 1.1933537883365417,
+            "timestamp": 0.6165737225304978
+        },
+        {
+            "x": 5.875226516165077,
+            "y": 1.387237765393963,
+            "heading": 2.8184428738821614,
+            "angularVelocity": -0.029599653085833715,
+            "velocityX": -3.5797069150083383,
+            "velocityY": 1.1964936723964186,
+            "timestamp": 0.69364543784681
+        },
+        {
+            "x": 5.641686532929837,
+            "y": 1.4649564505690207,
+            "heading": 2.7347434741228605,
+            "angularVelocity": -1.0859937321465907,
+            "velocityX": -3.0301645976966087,
+            "velocityY": 1.0083943876958055,
+            "timestamp": 0.7707171531631222
+        },
+        {
+            "x": 5.455142642963306,
+            "y": 1.5270452896537086,
+            "heading": 2.6494963329119967,
+            "angularVelocity": -1.1060755669054276,
+            "velocityX": -2.4203936450737,
+            "velocityY": 0.8055982513152498,
+            "timestamp": 0.8477888684794344
+        },
+        {
+            "x": 5.315456646631149,
+            "y": 1.5735510161760224,
+            "heading": 2.577919230350482,
+            "angularVelocity": -0.928707792057741,
+            "velocityX": -1.812415822832897,
+            "velocityY": 0.6034084791216642,
+            "timestamp": 0.9248605837957465
+        },
+        {
+            "x": 5.222452869215715,
+            "y": 1.6045247487520136,
+            "heading": 2.5268085106209885,
+            "angularVelocity": -0.6631579369906109,
+            "velocityX": -1.2067173675029015,
+            "velocityY": 0.4018819673192824,
+            "timestamp": 1.0019322991120587
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.3478385100287815,
+            "velocityX": -0.6027226593448243,
+            "velocityY": 0.200790279345336,
+            "timestamp": 1.079004014428371
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -3.134288783198311e-31,
+            "velocityX": -2.9694883024269136e-32,
+            "velocityY": 4.283237987843841e-31,
+            "timestamp": 1.1560757297446833
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.1.traj
new file mode 100644
index 00000000..6f37c8f6
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.1.traj	
@@ -0,0 +1,265 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -1.8495960722802552e-34,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.66217209907118,
+            "y": 0.8633707808076446,
+            "heading": 3.200268562588625,
+            "angularVelocity": 0.6146717037531945,
+            "velocityX": -0.18182458262791865,
+            "velocityY": 0.7788962593386439,
+            "timestamp": 0.09805000331172546
+        },
+        {
+            "x": 7.626250868838738,
+            "y": 1.0164003464766511,
+            "heading": 3.3145470628036264,
+            "angularVelocity": 1.1655124564522605,
+            "velocityX": -0.3663562368094907,
+            "velocityY": 1.5607298368209865,
+            "timestamp": 0.19610000662345092
+        },
+        {
+            "x": 7.571819523432209,
+            "y": 1.2466141084056221,
+            "heading": 3.4692056792172115,
+            "angularVelocity": 1.5773443262606215,
+            "velocityX": -0.555138639143922,
+            "velocityY": 2.347922020941335,
+            "timestamp": 0.29415000993517637
+        },
+        {
+            "x": 7.4978548812850345,
+            "y": 1.55456588878344,
+            "heading": 3.620198528248394,
+            "angularVelocity": 1.5399576127615076,
+            "velocityX": -0.7543563452213444,
+            "velocityY": 3.1407625698773547,
+            "timestamp": 0.39220001324690185
+        },
+        {
+            "x": 7.414564781883039,
+            "y": 1.916010778899352,
+            "heading": 3.6201985877633485,
+            "angularVelocity": 6.069857444827113e-7,
+            "velocityX": -0.8494655439959167,
+            "velocityY": 3.6863322581110816,
+            "timestamp": 0.4902500165586273
+        },
+        {
+            "x": 7.3312746982625,
+            "y": 2.27745567265221,
+            "heading": 3.620198647277482,
+            "angularVelocity": 6.069773758970038e-7,
+            "velocityX": -0.8494653830427515,
+            "velocityY": 3.686332295203854,
+            "timestamp": 0.5883000198703527
+        },
+        {
+            "x": 7.247984475288188,
+            "y": 2.6389005342928713,
+            "heading": 3.6201987067915797,
+            "angularVelocity": 6.069770050551867e-7,
+            "velocityX": -0.8494668042948591,
+            "velocityY": 3.686331967695479,
+            "timestamp": 0.6863500231820782
+        },
+        {
+            "x": 7.16025127777078,
+            "y": 2.999292730685755,
+            "heading": 3.620198766578442,
+            "angularVelocity": 6.097589007693709e-7,
+            "velocityX": -0.8947801586348038,
+            "velocityY": 3.675595963491274,
+            "timestamp": 0.7844000264938036
+        },
+        {
+            "x": 6.997140841431304,
+            "y": 3.3324212817743484,
+            "heading": 3.6201988348555445,
+            "angularVelocity": 6.963498230184926e-7,
+            "velocityX": -1.6635434046943107,
+            "velocityY": 3.3975373772247037,
+            "timestamp": 0.882450029805529
+        },
+        {
+            "x": 6.766266862430273,
+            "y": 3.622726019325249,
+            "heading": 3.620198931073779,
+            "angularVelocity": 9.813180151910862e-7,
+            "velocityX": -2.354655494166845,
+            "velocityY": 2.9607825369260774,
+            "timestamp": 0.9805000331172544
+        },
+        {
+            "x": 6.478412432979381,
+            "y": 3.856647861556034,
+            "heading": 3.62019912006771,
+            "angularVelocity": 0.000001927525998631886,
+            "velocityX": -2.935792144093369,
+            "velocityY": 2.38574027873399,
+            "timestamp": 1.07855003642898
+        },
+        {
+            "x": 6.147022412583985,
+            "y": 4.023260998419484,
+            "heading": 3.6202003231426523,
+            "angularVelocity": 0.000012270014292625344,
+            "velocityX": -3.3798063151698643,
+            "velocityY": 1.6992670192345174,
+            "timestamp": 1.1766000397407055
+        },
+        {
+            "x": 5.787574046048744,
+            "y": 4.114783912760558,
+            "heading": 3.6201999548728017,
+            "angularVelocity": -0.0000037559391933502495,
+            "velocityX": -3.6659699581290637,
+            "velocityY": 0.9334310173361262,
+            "timestamp": 1.274650043052431
+        },
+        {
+            "x": 5.417217638191157,
+            "y": 4.135174501302961,
+            "heading": 3.620199996859582,
+            "angularVelocity": 4.2821804063172526e-7,
+            "velocityX": -3.7772197383831925,
+            "velocityY": 0.2079611203844201,
+            "timestamp": 1.3727000463641565
+        },
+        {
+            "x": 5.066383608065517,
+            "y": 4.23317852630269,
+            "heading": 3.636482368230358,
+            "angularVelocity": 0.16606191556170027,
+            "velocityX": -3.578113393940955,
+            "velocityY": 0.9995310728154599,
+            "timestamp": 1.470750049675882
+        },
+        {
+            "x": 4.742245396435021,
+            "y": 4.406733140775732,
+            "heading": 3.644586480575013,
+            "angularVelocity": 0.08265285130985205,
+            "velocityX": -3.305846003900474,
+            "velocityY": 1.7700623009799354,
+            "timestamp": 1.5688000529876076
+        },
+        {
+            "x": 4.460670694136194,
+            "y": 4.648177372257919,
+            "heading": 3.644586687046199,
+            "angularVelocity": 0.000002105774391224538,
+            "velocityX": -2.87174597438442,
+            "velocityY": 2.4624602073145714,
+            "timestamp": 1.6668500562993331
+        },
+        {
+            "x": 4.237545979665016,
+            "y": 4.944479621179968,
+            "heading": 3.644586791013843,
+            "angularVelocity": 0.0000010603532960842864,
+            "velocityX": -2.2756216923502683,
+            "velocityY": 3.0219504223781635,
+            "timestamp": 1.7649000596110587
+        },
+        {
+            "x": 3.9950087307179025,
+            "y": 5.225113475172281,
+            "heading": 3.6445867953690434,
+            "angularVelocity": 4.441815614419627e-8,
+            "velocityX": -2.473607758849603,
+            "velocityY": 2.862150377497781,
+            "timestamp": 1.8629500629227842
+        },
+        {
+            "x": 3.7198234606165768,
+            "y": 5.473815593691706,
+            "heading": 3.644586791690717,
+            "angularVelocity": -3.7514802268686446e-8,
+            "velocityX": -2.8065809363253424,
+            "velocityY": 2.536482510140642,
+            "timestamp": 1.9610000662345097
+        },
+        {
+            "x": 3.4446380366180587,
+            "y": 5.722517541926013,
+            "heading": 3.6445867880123934,
+            "angularVelocity": -3.751477003592028e-8,
+            "velocityX": -2.8065825059040024,
+            "velocityY": 2.5364807734235555,
+            "timestamp": 2.059050069546235
+        },
+        {
+            "x": 3.1694525996169167,
+            "y": 5.97121947577308,
+            "heading": 3.6445867843340722,
+            "angularVelocity": -3.75147461397837e-8,
+            "velocityX": -2.806582638516174,
+            "velocityY": 2.5364806266898405,
+            "timestamp": 2.1571000728579603
+        },
+        {
+            "x": 2.894267154657697,
+            "y": 6.2199214008146,
+            "heading": 3.6445867806557373,
+            "angularVelocity": -3.751489444763293e-8,
+            "velocityX": -2.806582719679637,
+            "velocityY": 2.53648053688316,
+            "timestamp": 2.2551500761696857
+        },
+        {
+            "x": 2.6565603153930746,
+            "y": 6.434752825744944,
+            "heading": 3.60002452914154,
+            "angularVelocity": -0.45448495674724715,
+            "velocityX": -2.4243430008757167,
+            "velocityY": 2.191039445938014,
+            "timestamp": 2.353200079481411
+        },
+        {
+            "x": 2.478280163108621,
+            "y": 6.595876410073326,
+            "heading": 3.56660458464388,
+            "angularVelocity": -0.3408459293102701,
+            "velocityX": -1.8182574835582244,
+            "velocityY": 1.6432797438683373,
+            "timestamp": 2.4512500827931363
+        },
+        {
+            "x": 2.3594267213059505,
+            "y": 6.703292136714203,
+            "heading": 3.544325120911587,
+            "angularVelocity": -0.2272255275857591,
+            "velocityX": -1.2121717265506466,
+            "velocityY": 1.0955198675452942,
+            "timestamp": 2.5493000861048616
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": -0.1136135987327294,
+            "velocityX": -0.6060858674019433,
+            "velocityY": 0.5477599334193345,
+            "timestamp": 2.647350089416587
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 0,
+            "velocityX": -1.9773011985788902e-32,
+            "velocityY": -8.72670829182521e-33,
+            "timestamp": 2.745400092728312
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.traj
new file mode 100644
index 00000000..6f37c8f6
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.traj	
@@ -0,0 +1,265 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -1.8495960722802552e-34,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.66217209907118,
+            "y": 0.8633707808076446,
+            "heading": 3.200268562588625,
+            "angularVelocity": 0.6146717037531945,
+            "velocityX": -0.18182458262791865,
+            "velocityY": 0.7788962593386439,
+            "timestamp": 0.09805000331172546
+        },
+        {
+            "x": 7.626250868838738,
+            "y": 1.0164003464766511,
+            "heading": 3.3145470628036264,
+            "angularVelocity": 1.1655124564522605,
+            "velocityX": -0.3663562368094907,
+            "velocityY": 1.5607298368209865,
+            "timestamp": 0.19610000662345092
+        },
+        {
+            "x": 7.571819523432209,
+            "y": 1.2466141084056221,
+            "heading": 3.4692056792172115,
+            "angularVelocity": 1.5773443262606215,
+            "velocityX": -0.555138639143922,
+            "velocityY": 2.347922020941335,
+            "timestamp": 0.29415000993517637
+        },
+        {
+            "x": 7.4978548812850345,
+            "y": 1.55456588878344,
+            "heading": 3.620198528248394,
+            "angularVelocity": 1.5399576127615076,
+            "velocityX": -0.7543563452213444,
+            "velocityY": 3.1407625698773547,
+            "timestamp": 0.39220001324690185
+        },
+        {
+            "x": 7.414564781883039,
+            "y": 1.916010778899352,
+            "heading": 3.6201985877633485,
+            "angularVelocity": 6.069857444827113e-7,
+            "velocityX": -0.8494655439959167,
+            "velocityY": 3.6863322581110816,
+            "timestamp": 0.4902500165586273
+        },
+        {
+            "x": 7.3312746982625,
+            "y": 2.27745567265221,
+            "heading": 3.620198647277482,
+            "angularVelocity": 6.069773758970038e-7,
+            "velocityX": -0.8494653830427515,
+            "velocityY": 3.686332295203854,
+            "timestamp": 0.5883000198703527
+        },
+        {
+            "x": 7.247984475288188,
+            "y": 2.6389005342928713,
+            "heading": 3.6201987067915797,
+            "angularVelocity": 6.069770050551867e-7,
+            "velocityX": -0.8494668042948591,
+            "velocityY": 3.686331967695479,
+            "timestamp": 0.6863500231820782
+        },
+        {
+            "x": 7.16025127777078,
+            "y": 2.999292730685755,
+            "heading": 3.620198766578442,
+            "angularVelocity": 6.097589007693709e-7,
+            "velocityX": -0.8947801586348038,
+            "velocityY": 3.675595963491274,
+            "timestamp": 0.7844000264938036
+        },
+        {
+            "x": 6.997140841431304,
+            "y": 3.3324212817743484,
+            "heading": 3.6201988348555445,
+            "angularVelocity": 6.963498230184926e-7,
+            "velocityX": -1.6635434046943107,
+            "velocityY": 3.3975373772247037,
+            "timestamp": 0.882450029805529
+        },
+        {
+            "x": 6.766266862430273,
+            "y": 3.622726019325249,
+            "heading": 3.620198931073779,
+            "angularVelocity": 9.813180151910862e-7,
+            "velocityX": -2.354655494166845,
+            "velocityY": 2.9607825369260774,
+            "timestamp": 0.9805000331172544
+        },
+        {
+            "x": 6.478412432979381,
+            "y": 3.856647861556034,
+            "heading": 3.62019912006771,
+            "angularVelocity": 0.000001927525998631886,
+            "velocityX": -2.935792144093369,
+            "velocityY": 2.38574027873399,
+            "timestamp": 1.07855003642898
+        },
+        {
+            "x": 6.147022412583985,
+            "y": 4.023260998419484,
+            "heading": 3.6202003231426523,
+            "angularVelocity": 0.000012270014292625344,
+            "velocityX": -3.3798063151698643,
+            "velocityY": 1.6992670192345174,
+            "timestamp": 1.1766000397407055
+        },
+        {
+            "x": 5.787574046048744,
+            "y": 4.114783912760558,
+            "heading": 3.6201999548728017,
+            "angularVelocity": -0.0000037559391933502495,
+            "velocityX": -3.6659699581290637,
+            "velocityY": 0.9334310173361262,
+            "timestamp": 1.274650043052431
+        },
+        {
+            "x": 5.417217638191157,
+            "y": 4.135174501302961,
+            "heading": 3.620199996859582,
+            "angularVelocity": 4.2821804063172526e-7,
+            "velocityX": -3.7772197383831925,
+            "velocityY": 0.2079611203844201,
+            "timestamp": 1.3727000463641565
+        },
+        {
+            "x": 5.066383608065517,
+            "y": 4.23317852630269,
+            "heading": 3.636482368230358,
+            "angularVelocity": 0.16606191556170027,
+            "velocityX": -3.578113393940955,
+            "velocityY": 0.9995310728154599,
+            "timestamp": 1.470750049675882
+        },
+        {
+            "x": 4.742245396435021,
+            "y": 4.406733140775732,
+            "heading": 3.644586480575013,
+            "angularVelocity": 0.08265285130985205,
+            "velocityX": -3.305846003900474,
+            "velocityY": 1.7700623009799354,
+            "timestamp": 1.5688000529876076
+        },
+        {
+            "x": 4.460670694136194,
+            "y": 4.648177372257919,
+            "heading": 3.644586687046199,
+            "angularVelocity": 0.000002105774391224538,
+            "velocityX": -2.87174597438442,
+            "velocityY": 2.4624602073145714,
+            "timestamp": 1.6668500562993331
+        },
+        {
+            "x": 4.237545979665016,
+            "y": 4.944479621179968,
+            "heading": 3.644586791013843,
+            "angularVelocity": 0.0000010603532960842864,
+            "velocityX": -2.2756216923502683,
+            "velocityY": 3.0219504223781635,
+            "timestamp": 1.7649000596110587
+        },
+        {
+            "x": 3.9950087307179025,
+            "y": 5.225113475172281,
+            "heading": 3.6445867953690434,
+            "angularVelocity": 4.441815614419627e-8,
+            "velocityX": -2.473607758849603,
+            "velocityY": 2.862150377497781,
+            "timestamp": 1.8629500629227842
+        },
+        {
+            "x": 3.7198234606165768,
+            "y": 5.473815593691706,
+            "heading": 3.644586791690717,
+            "angularVelocity": -3.7514802268686446e-8,
+            "velocityX": -2.8065809363253424,
+            "velocityY": 2.536482510140642,
+            "timestamp": 1.9610000662345097
+        },
+        {
+            "x": 3.4446380366180587,
+            "y": 5.722517541926013,
+            "heading": 3.6445867880123934,
+            "angularVelocity": -3.751477003592028e-8,
+            "velocityX": -2.8065825059040024,
+            "velocityY": 2.5364807734235555,
+            "timestamp": 2.059050069546235
+        },
+        {
+            "x": 3.1694525996169167,
+            "y": 5.97121947577308,
+            "heading": 3.6445867843340722,
+            "angularVelocity": -3.75147461397837e-8,
+            "velocityX": -2.806582638516174,
+            "velocityY": 2.5364806266898405,
+            "timestamp": 2.1571000728579603
+        },
+        {
+            "x": 2.894267154657697,
+            "y": 6.2199214008146,
+            "heading": 3.6445867806557373,
+            "angularVelocity": -3.751489444763293e-8,
+            "velocityX": -2.806582719679637,
+            "velocityY": 2.53648053688316,
+            "timestamp": 2.2551500761696857
+        },
+        {
+            "x": 2.6565603153930746,
+            "y": 6.434752825744944,
+            "heading": 3.60002452914154,
+            "angularVelocity": -0.45448495674724715,
+            "velocityX": -2.4243430008757167,
+            "velocityY": 2.191039445938014,
+            "timestamp": 2.353200079481411
+        },
+        {
+            "x": 2.478280163108621,
+            "y": 6.595876410073326,
+            "heading": 3.56660458464388,
+            "angularVelocity": -0.3408459293102701,
+            "velocityX": -1.8182574835582244,
+            "velocityY": 1.6432797438683373,
+            "timestamp": 2.4512500827931363
+        },
+        {
+            "x": 2.3594267213059505,
+            "y": 6.703292136714203,
+            "heading": 3.544325120911587,
+            "angularVelocity": -0.2272255275857591,
+            "velocityX": -1.2121717265506466,
+            "velocityY": 1.0955198675452942,
+            "timestamp": 2.5493000861048616
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": -0.1136135987327294,
+            "velocityX": -0.6060858674019433,
+            "velocityY": 0.5477599334193345,
+            "timestamp": 2.647350089416587
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 0,
+            "velocityX": -1.9773011985788902e-32,
+            "velocityY": -8.72670829182521e-33,
+            "timestamp": 2.745400092728312
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.1.traj
new file mode 100644
index 00000000..db86197d
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.1.traj	
@@ -0,0 +1,220 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -5.5460688640051806e-30,
+            "velocityX": 1.8487232872526845e-30,
+            "velocityY": -2.9628257998243394e-30,
+            "timestamp": 0
+        },
+        {
+            "x": 1.423394920733784,
+            "y": 5.582111402692338,
+            "heading": 3.1399999953305415,
+            "angularVelocity": -4.870437841651369e-8,
+            "velocityX": 0.7654820737524797,
+            "velocityY": 0.23061381110796328,
+            "timestamp": 0.09588065253313424
+        },
+        {
+            "x": 1.5701847607820971,
+            "y": 5.626334202094221,
+            "heading": 3.1399999893931145,
+            "angularVelocity": -6.193245197617188e-8,
+            "velocityX": 1.530964132702707,
+            "velocityY": 0.46122755981751334,
+            "timestamp": 0.19176130506626848
+        },
+        {
+            "x": 1.7903695158505508,
+            "y": 5.692668391072556,
+            "heading": 3.1399999853590668,
+            "angularVelocity": -4.208451353086861e-8,
+            "velocityX": 2.2964461468641186,
+            "velocityY": 0.6918412341313931,
+            "timestamp": 0.2876419575994027
+        },
+        {
+            "x": 2.0839491699386645,
+            "y": 5.781113958575535,
+            "heading": 3.139999985983334,
+            "angularVelocity": 6.496408603804424e-9,
+            "velocityX": 3.0619279941465973,
+            "velocityY": 0.922454793178729,
+            "timestamp": 0.38352261013253697
+        },
+        {
+            "x": 2.431241902978436,
+            "y": 5.885741341465996,
+            "heading": 3.1399999859835575,
+            "angularVelocity": 2.3334348335212476e-12,
+            "velocityX": 3.6221356849840607,
+            "velocityY": 1.0912251859537307,
+            "timestamp": 0.47940326266567124
+        },
+        {
+            "x": 2.7785346564747577,
+            "y": 5.990368656458575,
+            "heading": 3.139999985983492,
+            "angularVelocity": -6.768139199697334e-13,
+            "velocityX": 3.62213589833913,
+            "velocityY": 1.0912244778026317,
+            "timestamp": 0.5752839151988055
+        },
+        {
+            "x": 3.125827431847794,
+            "y": 6.094995898834488,
+            "heading": 3.139999985983408,
+            "angularVelocity": -8.748780481443456e-13,
+            "velocityX": 3.6221361265054535,
+            "velocityY": 1.091223720437653,
+            "timestamp": 0.6711645677319398
+        },
+        {
+            "x": 3.473120229952072,
+            "y": 6.199623065757117,
+            "heading": 3.1399999859833065,
+            "angularVelocity": -1.0615094861925232e-12,
+            "velocityX": 3.6221363635841364,
+            "velocityY": 1.0912229334878236,
+            "timestamp": 0.767045220265074
+        },
+        {
+            "x": 3.820413051729017,
+            "y": 6.304250154101482,
+            "heading": 3.139999985983188,
+            "angularVelocity": -1.2288425429953676e-12,
+            "velocityX": 3.6221366104815065,
+            "velocityY": 1.0912221139455949,
+            "timestamp": 0.8629258727982083
+        },
+        {
+            "x": 4.167705898643341,
+            "y": 6.408877159005614,
+            "heading": 3.139999985983049,
+            "angularVelocity": -1.4178096575705385e-12,
+            "velocityX": 3.6221368726552323,
+            "velocityY": 1.0912212436948225,
+            "timestamp": 0.9588065253313426
+        },
+        {
+            "x": 4.5149987734968,
+            "y": 6.5135040711691525,
+            "heading": 3.139999985982883,
+            "angularVelocity": -1.676018359549876e-12,
+            "velocityX": 3.622137164050193,
+            "velocityY": 1.0912202764447858,
+            "timestamp": 1.0546871778644769
+        },
+        {
+            "x": 4.862291683371807,
+            "y": 6.6181308670796435,
+            "heading": 3.1399999859826795,
+            "angularVelocity": -2.066469238283253e-12,
+            "velocityX": 3.6221375293116886,
+            "velocityY": 1.0912190639696435,
+            "timestamp": 1.150567830397611
+        },
+        {
+            "x": 5.209584643601777,
+            "y": 6.722757495842404,
+            "heading": 3.139999985982697,
+            "angularVelocity": 1.7634954198261724e-13,
+            "velocityX": 3.6221380544925466,
+            "velocityY": 1.0912173206895408,
+            "timestamp": 1.2464484829307452
+        },
+        {
+            "x": 5.55687764566899,
+            "y": 6.827383985745101,
+            "heading": 3.139999985982843,
+            "angularVelocity": 1.4429880633934061e-12,
+            "velocityX": 3.6221384908382315,
+            "velocityY": 1.091215872433919,
+            "timestamp": 1.3423291354638793
+        },
+        {
+            "x": 5.904170680203765,
+            "y": 6.932010367880933,
+            "heading": 3.1399999859829473,
+            "angularVelocity": 9.965559645934633e-13,
+            "velocityX": 3.6221388294623984,
+            "velocityY": 1.0912147484660204,
+            "timestamp": 1.4382097879970135
+        },
+        {
+            "x": 6.251463742058712,
+            "y": 7.036636659333844,
+            "heading": 3.1399999859830086,
+            "angularVelocity": 5.792335690532441e-13,
+            "velocityX": 3.6221391144013104,
+            "velocityY": 1.0912138026768703,
+            "timestamp": 1.5340904405301476
+        },
+        {
+            "x": 6.598756826003656,
+            "y": 7.141262877462233,
+            "heading": 3.139999985983042,
+            "angularVelocity": 3.176331549030912e-13,
+            "velocityX": 3.6221393447915826,
+            "velocityY": 1.0912130379289529,
+            "timestamp": 1.6299710930632818
+        },
+        {
+            "x": 6.9460499272931315,
+            "y": 7.245889038012359,
+            "heading": 3.139999985982778,
+            "angularVelocity": -2.760419154137832e-12,
+            "velocityX": 3.622139525688641,
+            "velocityY": 1.0912124374075387,
+            "timestamp": 1.725851745596416
+        },
+        {
+            "x": 7.239629936933939,
+            "y": 7.334333425317839,
+            "heading": 3.139999987228266,
+            "angularVelocity": 1.3004483336236485e-8,
+            "velocityX": 3.061931702425989,
+            "velocityY": 0.9224424841673509,
+            "timestamp": 1.8217323981295501
+        },
+        {
+            "x": 7.459814962920636,
+            "y": 7.4006667150284455,
+            "heading": 3.13999999169634,
+            "angularVelocity": 4.661129161255315e-8,
+            "velocityX": 2.2964489724386126,
+            "velocityY": 0.6918318551100047,
+            "timestamp": 1.9176130506626843
+        },
+        {
+            "x": 7.606604986293984,
+            "y": 7.444888905913496,
+            "heading": 3.1399999966584766,
+            "angularVelocity": 5.176056299229228e-8,
+            "velocityX": 1.530966044713388,
+            "velocityY": 0.46122121321737414,
+            "timestamp": 2.0134937031958184
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 3.485452160899378e-8,
+            "velocityX": 0.7654830434176825,
+            "velocityY": 0.23061059246584142,
+            "timestamp": 2.109374355728953
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": -1.7922203019823127e-29,
+            "velocityX": 0,
+            "velocityY": -5.07505109843192e-30,
+            "timestamp": 2.205255008262087
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.traj
new file mode 100644
index 00000000..db86197d
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.traj	
@@ -0,0 +1,220 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -5.5460688640051806e-30,
+            "velocityX": 1.8487232872526845e-30,
+            "velocityY": -2.9628257998243394e-30,
+            "timestamp": 0
+        },
+        {
+            "x": 1.423394920733784,
+            "y": 5.582111402692338,
+            "heading": 3.1399999953305415,
+            "angularVelocity": -4.870437841651369e-8,
+            "velocityX": 0.7654820737524797,
+            "velocityY": 0.23061381110796328,
+            "timestamp": 0.09588065253313424
+        },
+        {
+            "x": 1.5701847607820971,
+            "y": 5.626334202094221,
+            "heading": 3.1399999893931145,
+            "angularVelocity": -6.193245197617188e-8,
+            "velocityX": 1.530964132702707,
+            "velocityY": 0.46122755981751334,
+            "timestamp": 0.19176130506626848
+        },
+        {
+            "x": 1.7903695158505508,
+            "y": 5.692668391072556,
+            "heading": 3.1399999853590668,
+            "angularVelocity": -4.208451353086861e-8,
+            "velocityX": 2.2964461468641186,
+            "velocityY": 0.6918412341313931,
+            "timestamp": 0.2876419575994027
+        },
+        {
+            "x": 2.0839491699386645,
+            "y": 5.781113958575535,
+            "heading": 3.139999985983334,
+            "angularVelocity": 6.496408603804424e-9,
+            "velocityX": 3.0619279941465973,
+            "velocityY": 0.922454793178729,
+            "timestamp": 0.38352261013253697
+        },
+        {
+            "x": 2.431241902978436,
+            "y": 5.885741341465996,
+            "heading": 3.1399999859835575,
+            "angularVelocity": 2.3334348335212476e-12,
+            "velocityX": 3.6221356849840607,
+            "velocityY": 1.0912251859537307,
+            "timestamp": 0.47940326266567124
+        },
+        {
+            "x": 2.7785346564747577,
+            "y": 5.990368656458575,
+            "heading": 3.139999985983492,
+            "angularVelocity": -6.768139199697334e-13,
+            "velocityX": 3.62213589833913,
+            "velocityY": 1.0912244778026317,
+            "timestamp": 0.5752839151988055
+        },
+        {
+            "x": 3.125827431847794,
+            "y": 6.094995898834488,
+            "heading": 3.139999985983408,
+            "angularVelocity": -8.748780481443456e-13,
+            "velocityX": 3.6221361265054535,
+            "velocityY": 1.091223720437653,
+            "timestamp": 0.6711645677319398
+        },
+        {
+            "x": 3.473120229952072,
+            "y": 6.199623065757117,
+            "heading": 3.1399999859833065,
+            "angularVelocity": -1.0615094861925232e-12,
+            "velocityX": 3.6221363635841364,
+            "velocityY": 1.0912229334878236,
+            "timestamp": 0.767045220265074
+        },
+        {
+            "x": 3.820413051729017,
+            "y": 6.304250154101482,
+            "heading": 3.139999985983188,
+            "angularVelocity": -1.2288425429953676e-12,
+            "velocityX": 3.6221366104815065,
+            "velocityY": 1.0912221139455949,
+            "timestamp": 0.8629258727982083
+        },
+        {
+            "x": 4.167705898643341,
+            "y": 6.408877159005614,
+            "heading": 3.139999985983049,
+            "angularVelocity": -1.4178096575705385e-12,
+            "velocityX": 3.6221368726552323,
+            "velocityY": 1.0912212436948225,
+            "timestamp": 0.9588065253313426
+        },
+        {
+            "x": 4.5149987734968,
+            "y": 6.5135040711691525,
+            "heading": 3.139999985982883,
+            "angularVelocity": -1.676018359549876e-12,
+            "velocityX": 3.622137164050193,
+            "velocityY": 1.0912202764447858,
+            "timestamp": 1.0546871778644769
+        },
+        {
+            "x": 4.862291683371807,
+            "y": 6.6181308670796435,
+            "heading": 3.1399999859826795,
+            "angularVelocity": -2.066469238283253e-12,
+            "velocityX": 3.6221375293116886,
+            "velocityY": 1.0912190639696435,
+            "timestamp": 1.150567830397611
+        },
+        {
+            "x": 5.209584643601777,
+            "y": 6.722757495842404,
+            "heading": 3.139999985982697,
+            "angularVelocity": 1.7634954198261724e-13,
+            "velocityX": 3.6221380544925466,
+            "velocityY": 1.0912173206895408,
+            "timestamp": 1.2464484829307452
+        },
+        {
+            "x": 5.55687764566899,
+            "y": 6.827383985745101,
+            "heading": 3.139999985982843,
+            "angularVelocity": 1.4429880633934061e-12,
+            "velocityX": 3.6221384908382315,
+            "velocityY": 1.091215872433919,
+            "timestamp": 1.3423291354638793
+        },
+        {
+            "x": 5.904170680203765,
+            "y": 6.932010367880933,
+            "heading": 3.1399999859829473,
+            "angularVelocity": 9.965559645934633e-13,
+            "velocityX": 3.6221388294623984,
+            "velocityY": 1.0912147484660204,
+            "timestamp": 1.4382097879970135
+        },
+        {
+            "x": 6.251463742058712,
+            "y": 7.036636659333844,
+            "heading": 3.1399999859830086,
+            "angularVelocity": 5.792335690532441e-13,
+            "velocityX": 3.6221391144013104,
+            "velocityY": 1.0912138026768703,
+            "timestamp": 1.5340904405301476
+        },
+        {
+            "x": 6.598756826003656,
+            "y": 7.141262877462233,
+            "heading": 3.139999985983042,
+            "angularVelocity": 3.176331549030912e-13,
+            "velocityX": 3.6221393447915826,
+            "velocityY": 1.0912130379289529,
+            "timestamp": 1.6299710930632818
+        },
+        {
+            "x": 6.9460499272931315,
+            "y": 7.245889038012359,
+            "heading": 3.139999985982778,
+            "angularVelocity": -2.760419154137832e-12,
+            "velocityX": 3.622139525688641,
+            "velocityY": 1.0912124374075387,
+            "timestamp": 1.725851745596416
+        },
+        {
+            "x": 7.239629936933939,
+            "y": 7.334333425317839,
+            "heading": 3.139999987228266,
+            "angularVelocity": 1.3004483336236485e-8,
+            "velocityX": 3.061931702425989,
+            "velocityY": 0.9224424841673509,
+            "timestamp": 1.8217323981295501
+        },
+        {
+            "x": 7.459814962920636,
+            "y": 7.4006667150284455,
+            "heading": 3.13999999169634,
+            "angularVelocity": 4.661129161255315e-8,
+            "velocityX": 2.2964489724386126,
+            "velocityY": 0.6918318551100047,
+            "timestamp": 1.9176130506626843
+        },
+        {
+            "x": 7.606604986293984,
+            "y": 7.444888905913496,
+            "heading": 3.1399999966584766,
+            "angularVelocity": 5.176056299229228e-8,
+            "velocityX": 1.530966044713388,
+            "velocityY": 0.46122121321737414,
+            "timestamp": 2.0134937031958184
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 3.485452160899378e-8,
+            "velocityX": 0.7654830434176825,
+            "velocityY": 0.23061059246584142,
+            "timestamp": 2.109374355728953
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": -1.7922203019823127e-29,
+            "velocityX": 0,
+            "velocityY": -5.07505109843192e-30,
+            "timestamp": 2.205255008262087
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.1.traj
new file mode 100644
index 00000000..9be2eeb8
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.1.traj	
@@ -0,0 +1,211 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 4.242495082651476e-31,
+            "velocityX": 1.1828951034939025e-32,
+            "velocityY": -1.8669507935571478e-37,
+            "timestamp": 0
+        },
+        {
+            "x": 1.458096319535533,
+            "y": 5.590985596762502,
+            "heading": 3.1146996746822455,
+            "angularVelocity": -0.21764643682588541,
+            "velocityX": 0.9299002477407399,
+            "velocityY": 0.2665540716802577,
+            "timestamp": 0.11624507015474053
+        },
+        {
+            "x": 1.674289226072187,
+            "y": 5.652956746779397,
+            "heading": 3.0641193580719652,
+            "angularVelocity": -0.4351179498876838,
+            "velocityX": 1.8598027963583075,
+            "velocityY": 0.5331077690813153,
+            "timestamp": 0.23249014030948106
+        },
+        {
+            "x": 1.9985791294926345,
+            "y": 5.745913563807996,
+            "heading": 2.9882935824900567,
+            "angularVelocity": -0.652292398128992,
+            "velocityX": 2.789708870997854,
+            "velocityY": 0.7996624450813951,
+            "timestamp": 0.3487352104642216
+        },
+        {
+            "x": 2.421300960273809,
+            "y": 5.867094149195328,
+            "heading": 2.9882935750965625,
+            "angularVelocity": -6.360264782367115e-8,
+            "velocityX": 3.6364710367369995,
+            "velocityY": 1.0424578455329059,
+            "timestamp": 0.4649802806189621
+        },
+        {
+            "x": 2.844022813063846,
+            "y": 5.988274657808269,
+            "heading": 2.988293567703214,
+            "angularVelocity": -6.360139701488917e-8,
+            "velocityX": 3.636471226068578,
+            "velocityY": 1.0424571850800302,
+            "timestamp": 0.5812253507737026
+        },
+        {
+            "x": 3.266744690508405,
+            "y": 6.109455080417214,
+            "heading": 2.988293560309863,
+            "angularVelocity": -6.360141400691824e-8,
+            "velocityX": 3.636471438159484,
+            "velocityY": 1.0424564452293308,
+            "timestamp": 0.6974704209284431
+        },
+        {
+            "x": 3.6894665932724995,
+            "y": 6.2306354147022835,
+            "heading": 2.9882935529165104,
+            "angularVelocity": -6.360143032479277e-8,
+            "velocityX": 3.6364716559711723,
+            "velocityY": 1.0424556854218385,
+            "timestamp": 0.8137154910831836
+        },
+        {
+            "x": 4.112188522356607,
+            "y": 6.35181565717337,
+            "heading": 2.988293545523163,
+            "angularVelocity": -6.360138139725806e-8,
+            "velocityX": 3.6364718823894826,
+            "velocityY": 1.042454895590637,
+            "timestamp": 0.9299605612379241
+        },
+        {
+            "x": 4.534910480631917,
+            "y": 6.472995797814601,
+            "heading": 2.988293538129828,
+            "angularVelocity": -6.360128257953913e-8,
+            "velocityX": 3.636472133507264,
+            "velocityY": 1.0424540195977474,
+            "timestamp": 1.0462056313926646
+        },
+        {
+            "x": 4.95763256393494,
+            "y": 6.594175502311251,
+            "heading": 2.9882935307365046,
+            "angularVelocity": -6.360117492960731e-8,
+            "velocityX": 3.6364732090600738,
+            "velocityY": 1.0424502676572964,
+            "timestamp": 1.1624507015474053
+        },
+        {
+            "x": 5.395072294009931,
+            "y": 6.63917509969073,
+            "heading": 2.988293514020312,
+            "angularVelocity": -1.4380130211846583e-7,
+            "velocityX": 3.763081991285213,
+            "velocityY": 0.3871097270583396,
+            "timestamp": 1.2786957717021459
+        },
+        {
+            "x": 5.834408437796079,
+            "y": 6.6201427711176,
+            "heading": 2.9882934665190843,
+            "angularVelocity": -4.0863004174605414e-7,
+            "velocityX": 3.7793959193394016,
+            "velocityY": -0.16372589863635184,
+            "timestamp": 1.3949408418568865
+        },
+        {
+            "x": 6.258700069994256,
+            "y": 6.504578567848497,
+            "heading": 2.988293412737735,
+            "angularVelocity": -4.6265488113342876e-7,
+            "velocityX": 3.649975277518237,
+            "velocityY": -0.9941428321671584,
+            "timestamp": 1.511185912011627
+        },
+        {
+            "x": 6.6396986873456,
+            "y": 6.284991393688008,
+            "heading": 2.988293385419437,
+            "angularVelocity": -2.350060774071174e-7,
+            "velocityX": 3.277546452887644,
+            "velocityY": -1.8890020356836066,
+            "timestamp": 1.6274309821663677
+        },
+        {
+            "x": 6.952392117761244,
+            "y": 5.9757968201953435,
+            "heading": 2.988293399279684,
+            "angularVelocity": 1.1923298633596667e-7,
+            "velocityX": 2.689950034004883,
+            "velocityY": -2.6598510636285697,
+            "timestamp": 1.7436760523211083
+        },
+        {
+            "x": 7.176252511109016,
+            "y": 5.597293189983828,
+            "heading": 2.988293408698133,
+            "angularVelocity": 8.102235350708823e-8,
+            "velocityX": 1.9257624693226023,
+            "velocityY": -3.2560832877271055,
+            "timestamp": 1.859921122475849
+        },
+        {
+            "x": 7.318788154943764,
+            "y": 5.181285899414456,
+            "heading": 2.98829341726252,
+            "angularVelocity": 7.367527341392295e-8,
+            "velocityX": 1.226165063559341,
+            "velocityY": -3.5787091015180317,
+            "timestamp": 1.9761661926305896
+        },
+        {
+            "x": 7.461323091390308,
+            "y": 4.765278366475121,
+            "heading": 2.9882934258271017,
+            "angularVelocity": 7.36769447232769e-8,
+            "velocityX": 1.2261589782414601,
+            "velocityY": -3.578711186509359,
+            "timestamp": 2.09241126278533
+        },
+        {
+            "x": 7.570661558741158,
+            "y": 4.44613912293617,
+            "heading": 3.0641409244839792,
+            "angularVelocity": 0.6524792712147914,
+            "velocityX": 0.9405858433850398,
+            "velocityY": -2.745400240320956,
+            "timestamp": 2.2086563329400706
+        },
+        {
+            "x": 7.643553854632525,
+            "y": 4.2333797165247065,
+            "heading": 3.1147139591088293,
+            "angularVelocity": 0.43505530649626106,
+            "velocityX": 0.6270570940714855,
+            "velocityY": -1.8302660588379978,
+            "timestamp": 2.324901403094811
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.21752355482697977,
+            "velocityX": 0.31352852485666055,
+            "velocityY": -0.9151331439956911,
+            "timestamp": 2.4411464732495514
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": -2.3792206080273036e-31,
+            "velocityX": 0,
+            "velocityY": -4.1394011967092386e-32,
+            "timestamp": 2.5573915434042918
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.traj
new file mode 100644
index 00000000..9be2eeb8
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.traj	
@@ -0,0 +1,211 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 4.242495082651476e-31,
+            "velocityX": 1.1828951034939025e-32,
+            "velocityY": -1.8669507935571478e-37,
+            "timestamp": 0
+        },
+        {
+            "x": 1.458096319535533,
+            "y": 5.590985596762502,
+            "heading": 3.1146996746822455,
+            "angularVelocity": -0.21764643682588541,
+            "velocityX": 0.9299002477407399,
+            "velocityY": 0.2665540716802577,
+            "timestamp": 0.11624507015474053
+        },
+        {
+            "x": 1.674289226072187,
+            "y": 5.652956746779397,
+            "heading": 3.0641193580719652,
+            "angularVelocity": -0.4351179498876838,
+            "velocityX": 1.8598027963583075,
+            "velocityY": 0.5331077690813153,
+            "timestamp": 0.23249014030948106
+        },
+        {
+            "x": 1.9985791294926345,
+            "y": 5.745913563807996,
+            "heading": 2.9882935824900567,
+            "angularVelocity": -0.652292398128992,
+            "velocityX": 2.789708870997854,
+            "velocityY": 0.7996624450813951,
+            "timestamp": 0.3487352104642216
+        },
+        {
+            "x": 2.421300960273809,
+            "y": 5.867094149195328,
+            "heading": 2.9882935750965625,
+            "angularVelocity": -6.360264782367115e-8,
+            "velocityX": 3.6364710367369995,
+            "velocityY": 1.0424578455329059,
+            "timestamp": 0.4649802806189621
+        },
+        {
+            "x": 2.844022813063846,
+            "y": 5.988274657808269,
+            "heading": 2.988293567703214,
+            "angularVelocity": -6.360139701488917e-8,
+            "velocityX": 3.636471226068578,
+            "velocityY": 1.0424571850800302,
+            "timestamp": 0.5812253507737026
+        },
+        {
+            "x": 3.266744690508405,
+            "y": 6.109455080417214,
+            "heading": 2.988293560309863,
+            "angularVelocity": -6.360141400691824e-8,
+            "velocityX": 3.636471438159484,
+            "velocityY": 1.0424564452293308,
+            "timestamp": 0.6974704209284431
+        },
+        {
+            "x": 3.6894665932724995,
+            "y": 6.2306354147022835,
+            "heading": 2.9882935529165104,
+            "angularVelocity": -6.360143032479277e-8,
+            "velocityX": 3.6364716559711723,
+            "velocityY": 1.0424556854218385,
+            "timestamp": 0.8137154910831836
+        },
+        {
+            "x": 4.112188522356607,
+            "y": 6.35181565717337,
+            "heading": 2.988293545523163,
+            "angularVelocity": -6.360138139725806e-8,
+            "velocityX": 3.6364718823894826,
+            "velocityY": 1.042454895590637,
+            "timestamp": 0.9299605612379241
+        },
+        {
+            "x": 4.534910480631917,
+            "y": 6.472995797814601,
+            "heading": 2.988293538129828,
+            "angularVelocity": -6.360128257953913e-8,
+            "velocityX": 3.636472133507264,
+            "velocityY": 1.0424540195977474,
+            "timestamp": 1.0462056313926646
+        },
+        {
+            "x": 4.95763256393494,
+            "y": 6.594175502311251,
+            "heading": 2.9882935307365046,
+            "angularVelocity": -6.360117492960731e-8,
+            "velocityX": 3.6364732090600738,
+            "velocityY": 1.0424502676572964,
+            "timestamp": 1.1624507015474053
+        },
+        {
+            "x": 5.395072294009931,
+            "y": 6.63917509969073,
+            "heading": 2.988293514020312,
+            "angularVelocity": -1.4380130211846583e-7,
+            "velocityX": 3.763081991285213,
+            "velocityY": 0.3871097270583396,
+            "timestamp": 1.2786957717021459
+        },
+        {
+            "x": 5.834408437796079,
+            "y": 6.6201427711176,
+            "heading": 2.9882934665190843,
+            "angularVelocity": -4.0863004174605414e-7,
+            "velocityX": 3.7793959193394016,
+            "velocityY": -0.16372589863635184,
+            "timestamp": 1.3949408418568865
+        },
+        {
+            "x": 6.258700069994256,
+            "y": 6.504578567848497,
+            "heading": 2.988293412737735,
+            "angularVelocity": -4.6265488113342876e-7,
+            "velocityX": 3.649975277518237,
+            "velocityY": -0.9941428321671584,
+            "timestamp": 1.511185912011627
+        },
+        {
+            "x": 6.6396986873456,
+            "y": 6.284991393688008,
+            "heading": 2.988293385419437,
+            "angularVelocity": -2.350060774071174e-7,
+            "velocityX": 3.277546452887644,
+            "velocityY": -1.8890020356836066,
+            "timestamp": 1.6274309821663677
+        },
+        {
+            "x": 6.952392117761244,
+            "y": 5.9757968201953435,
+            "heading": 2.988293399279684,
+            "angularVelocity": 1.1923298633596667e-7,
+            "velocityX": 2.689950034004883,
+            "velocityY": -2.6598510636285697,
+            "timestamp": 1.7436760523211083
+        },
+        {
+            "x": 7.176252511109016,
+            "y": 5.597293189983828,
+            "heading": 2.988293408698133,
+            "angularVelocity": 8.102235350708823e-8,
+            "velocityX": 1.9257624693226023,
+            "velocityY": -3.2560832877271055,
+            "timestamp": 1.859921122475849
+        },
+        {
+            "x": 7.318788154943764,
+            "y": 5.181285899414456,
+            "heading": 2.98829341726252,
+            "angularVelocity": 7.367527341392295e-8,
+            "velocityX": 1.226165063559341,
+            "velocityY": -3.5787091015180317,
+            "timestamp": 1.9761661926305896
+        },
+        {
+            "x": 7.461323091390308,
+            "y": 4.765278366475121,
+            "heading": 2.9882934258271017,
+            "angularVelocity": 7.36769447232769e-8,
+            "velocityX": 1.2261589782414601,
+            "velocityY": -3.578711186509359,
+            "timestamp": 2.09241126278533
+        },
+        {
+            "x": 7.570661558741158,
+            "y": 4.44613912293617,
+            "heading": 3.0641409244839792,
+            "angularVelocity": 0.6524792712147914,
+            "velocityX": 0.9405858433850398,
+            "velocityY": -2.745400240320956,
+            "timestamp": 2.2086563329400706
+        },
+        {
+            "x": 7.643553854632525,
+            "y": 4.2333797165247065,
+            "heading": 3.1147139591088293,
+            "angularVelocity": 0.43505530649626106,
+            "velocityX": 0.6270570940714855,
+            "velocityY": -1.8302660588379978,
+            "timestamp": 2.324901403094811
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.21752355482697977,
+            "velocityX": 0.31352852485666055,
+            "velocityY": -0.9151331439956911,
+            "timestamp": 2.4411464732495514
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": -2.3792206080273036e-31,
+            "velocityX": 0,
+            "velocityY": -4.1394011967092386e-32,
+            "timestamp": 2.5573915434042918
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.1.traj
new file mode 100644
index 00000000..e5085a74
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.1.traj	
@@ -0,0 +1,76 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 1.425000004529508,
+            "y": 5.5600000086998405,
+            "heading": 3.139999994805454,
+            "angularVelocity": -5.477109235911092e-8,
+            "velocityX": 0.7907971475606559,
+            "velocityY": 9.173079093903646e-8,
+            "timestamp": 0.09484101550044495
+        },
+        {
+            "x": 1.5750000090590133,
+            "y": 5.560000023679525,
+            "heading": 3.1399999868961004,
+            "angularVelocity": -8.339591750663206e-8,
+            "velocityX": 1.5815942473623312,
+            "velocityY": 1.5794521377308047e-7,
+            "timestamp": 0.1896820310008899
+        },
+        {
+            "x": 1.7999999999999887,
+            "y": 5.5600000423644245,
+            "heading": 3.1399999785762156,
+            "angularVelocity": -8.772454789703223e-8,
+            "velocityX": 2.372391203887096,
+            "velocityY": 1.9701287329918457e-7,
+            "timestamp": 0.28452304650133486
+        },
+        {
+            "x": 2.0249999909409757,
+            "y": 5.560000024808709,
+            "heading": 3.1399999889274675,
+            "angularVelocity": 1.0914320422483106e-7,
+            "velocityX": 2.372391203887217,
+            "velocityY": -1.851067857722626e-7,
+            "timestamp": 0.3793640620017798
+        },
+        {
+            "x": 2.174999995470489,
+            "y": 5.560000009522542,
+            "heading": 3.1399999962990623,
+            "angularVelocity": 7.772581212467168e-8,
+            "velocityX": 1.5815942473624114,
+            "velocityY": -1.6117675426775267e-7,
+            "timestamp": 0.47420507750222474
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 3.902254149386954e-8,
+            "velocityX": 0.7907971475606953,
+            "velocityY": -1.0040532392087884e-7,
+            "timestamp": 0.5690460930026697
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 1.1924381690495433e-31,
+            "velocityX": 0,
+            "velocityY": -3.802893138506909e-33,
+            "timestamp": 0.6638871085031147
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.traj
new file mode 100644
index 00000000..e5085a74
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.traj	
@@ -0,0 +1,76 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 1.425000004529508,
+            "y": 5.5600000086998405,
+            "heading": 3.139999994805454,
+            "angularVelocity": -5.477109235911092e-8,
+            "velocityX": 0.7907971475606559,
+            "velocityY": 9.173079093903646e-8,
+            "timestamp": 0.09484101550044495
+        },
+        {
+            "x": 1.5750000090590133,
+            "y": 5.560000023679525,
+            "heading": 3.1399999868961004,
+            "angularVelocity": -8.339591750663206e-8,
+            "velocityX": 1.5815942473623312,
+            "velocityY": 1.5794521377308047e-7,
+            "timestamp": 0.1896820310008899
+        },
+        {
+            "x": 1.7999999999999887,
+            "y": 5.5600000423644245,
+            "heading": 3.1399999785762156,
+            "angularVelocity": -8.772454789703223e-8,
+            "velocityX": 2.372391203887096,
+            "velocityY": 1.9701287329918457e-7,
+            "timestamp": 0.28452304650133486
+        },
+        {
+            "x": 2.0249999909409757,
+            "y": 5.560000024808709,
+            "heading": 3.1399999889274675,
+            "angularVelocity": 1.0914320422483106e-7,
+            "velocityX": 2.372391203887217,
+            "velocityY": -1.851067857722626e-7,
+            "timestamp": 0.3793640620017798
+        },
+        {
+            "x": 2.174999995470489,
+            "y": 5.560000009522542,
+            "heading": 3.1399999962990623,
+            "angularVelocity": 7.772581212467168e-8,
+            "velocityX": 1.5815942473624114,
+            "velocityY": -1.6117675426775267e-7,
+            "timestamp": 0.47420507750222474
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 3.902254149386954e-8,
+            "velocityX": 0.7907971475606953,
+            "velocityY": -1.0040532392087884e-7,
+            "timestamp": 0.5690460930026697
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 1.1924381690495433e-31,
+            "velocityX": 0,
+            "velocityY": -3.802893138506909e-33,
+            "timestamp": 0.6638871085031147
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.1.traj
new file mode 100644
index 00000000..93f4af7e
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.1.traj	
@@ -0,0 +1,112 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 3.0431878595519687e-30,
+            "velocityX": -9.811045254051387e-32,
+            "velocityY": 1.5487452756550695e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 1.3816658692579147,
+            "y": 5.519999048484489,
+            "heading": 3.127016326666322,
+            "angularVelocity": -0.16577538674563452,
+            "velocityX": 0.4043094421708381,
+            "velocityY": -0.5107316733298447,
+            "timestamp": 0.07832087494146221
+        },
+        {
+            "x": 1.4449977551622273,
+            "y": 5.439997223821588,
+            "heading": 3.1010513131375346,
+            "angularVelocity": -0.33152098400576074,
+            "velocityX": 0.8086207661960841,
+            "velocityY": -1.0214623460564694,
+            "timestamp": 0.15664174988292442
+        },
+        {
+            "x": 1.5399958776695735,
+            "y": 5.319994658705983,
+            "heading": 3.0621074441034875,
+            "angularVelocity": -0.49723485677543305,
+            "velocityX": 1.2129348986250355,
+            "velocityY": -1.5321913245389087,
+            "timestamp": 0.23496262482438662
+        },
+        {
+            "x": 1.6666605105444467,
+            "y": 5.159991566646187,
+            "heading": 3.010185320857369,
+            "angularVelocity": -0.6629410522408352,
+            "velocityX": 1.6172525264757869,
+            "velocityY": -2.042917576947189,
+            "timestamp": 0.31328349976584885
+        },
+        {
+            "x": 1.8249919396490273,
+            "y": 4.959988272167279,
+            "heading": 2.9452809657766084,
+            "angularVelocity": -0.8286980339439337,
+            "velocityX": 2.021573804211457,
+            "velocityY": -2.5536396858231227,
+            "timestamp": 0.3916043747073111
+        },
+        {
+            "x": 1.9833278878841576,
+            "y": 4.759991849616898,
+            "heading": 2.880203589095056,
+            "angularVelocity": -0.8309071716856798,
+            "velocityX": 2.021631504416561,
+            "velocityY": -2.553551945121407,
+            "timestamp": 0.4699252496487733
+        },
+        {
+            "x": 2.109996705593866,
+            "y": 4.599994984440838,
+            "heading": 2.828128420495832,
+            "angularVelocity": -0.6648951334896099,
+            "velocityX": 1.6173059583971898,
+            "velocityY": -2.0428380721696886,
+            "timestamp": 0.5482461245902355
+        },
+        {
+            "x": 2.2049983397447894,
+            "y": 4.479997467084651,
+            "heading": 2.789065384524658,
+            "angularVelocity": -0.4987563788117872,
+            "velocityX": 1.212979735248467,
+            "velocityY": -1.5321268748066883,
+            "timestamp": 0.6265669995316977
+        },
+        {
+            "x": 2.268332774342349,
+            "y": 4.3999991596507995,
+            "heading": 2.7630214971574416,
+            "angularVelocity": -0.3325280442370214,
+            "velocityX": 0.8086533078811499,
+            "velocityY": -1.0214174381177892,
+            "timestamp": 0.7048878744731599
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": -0.1662583208777211,
+            "velocityX": 0.4043267606665432,
+            "velocityY": -0.510708794822514,
+            "timestamp": 0.7832087494146222
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 6.867412911994789e-31,
+            "velocityX": 0,
+            "velocityY": 9.317443122449736e-32,
+            "timestamp": 0.8615296243560844
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.traj
new file mode 100644
index 00000000..93f4af7e
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.traj	
@@ -0,0 +1,112 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 3.0431878595519687e-30,
+            "velocityX": -9.811045254051387e-32,
+            "velocityY": 1.5487452756550695e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 1.3816658692579147,
+            "y": 5.519999048484489,
+            "heading": 3.127016326666322,
+            "angularVelocity": -0.16577538674563452,
+            "velocityX": 0.4043094421708381,
+            "velocityY": -0.5107316733298447,
+            "timestamp": 0.07832087494146221
+        },
+        {
+            "x": 1.4449977551622273,
+            "y": 5.439997223821588,
+            "heading": 3.1010513131375346,
+            "angularVelocity": -0.33152098400576074,
+            "velocityX": 0.8086207661960841,
+            "velocityY": -1.0214623460564694,
+            "timestamp": 0.15664174988292442
+        },
+        {
+            "x": 1.5399958776695735,
+            "y": 5.319994658705983,
+            "heading": 3.0621074441034875,
+            "angularVelocity": -0.49723485677543305,
+            "velocityX": 1.2129348986250355,
+            "velocityY": -1.5321913245389087,
+            "timestamp": 0.23496262482438662
+        },
+        {
+            "x": 1.6666605105444467,
+            "y": 5.159991566646187,
+            "heading": 3.010185320857369,
+            "angularVelocity": -0.6629410522408352,
+            "velocityX": 1.6172525264757869,
+            "velocityY": -2.042917576947189,
+            "timestamp": 0.31328349976584885
+        },
+        {
+            "x": 1.8249919396490273,
+            "y": 4.959988272167279,
+            "heading": 2.9452809657766084,
+            "angularVelocity": -0.8286980339439337,
+            "velocityX": 2.021573804211457,
+            "velocityY": -2.5536396858231227,
+            "timestamp": 0.3916043747073111
+        },
+        {
+            "x": 1.9833278878841576,
+            "y": 4.759991849616898,
+            "heading": 2.880203589095056,
+            "angularVelocity": -0.8309071716856798,
+            "velocityX": 2.021631504416561,
+            "velocityY": -2.553551945121407,
+            "timestamp": 0.4699252496487733
+        },
+        {
+            "x": 2.109996705593866,
+            "y": 4.599994984440838,
+            "heading": 2.828128420495832,
+            "angularVelocity": -0.6648951334896099,
+            "velocityX": 1.6173059583971898,
+            "velocityY": -2.0428380721696886,
+            "timestamp": 0.5482461245902355
+        },
+        {
+            "x": 2.2049983397447894,
+            "y": 4.479997467084651,
+            "heading": 2.789065384524658,
+            "angularVelocity": -0.4987563788117872,
+            "velocityX": 1.212979735248467,
+            "velocityY": -1.5321268748066883,
+            "timestamp": 0.6265669995316977
+        },
+        {
+            "x": 2.268332774342349,
+            "y": 4.3999991596507995,
+            "heading": 2.7630214971574416,
+            "angularVelocity": -0.3325280442370214,
+            "velocityX": 0.8086533078811499,
+            "velocityY": -1.0214174381177892,
+            "timestamp": 0.7048878744731599
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": -0.1662583208777211,
+            "velocityX": 0.4043267606665432,
+            "velocityY": -0.510708794822514,
+            "timestamp": 0.7832087494146222
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 6.867412911994789e-31,
+            "velocityX": 0,
+            "velocityY": 9.317443122449736e-32,
+            "timestamp": 0.8615296243560844
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.1.traj
new file mode 100644
index 00000000..cfc8d5b6
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0,
+            "velocityX": 1.8796456462447601e-29,
+            "velocityY": -4.3119341309057906e-30,
+            "timestamp": 0
+        },
+        {
+            "x": 5.238209828475023,
+            "y": 6.650800952122015,
+            "heading": -2.892998872920493,
+            "angularVelocity": -0.1464435777830739,
+            "velocityX": 0.7008476743058382,
+            "velocityY": 0.23434076697564932,
+            "timestamp": 0.08876369396038813
+        },
+        {
+            "x": 5.362652804152687,
+            "y": 6.692404857453675,
+            "heading": -2.917433036424788,
+            "angularVelocity": -0.27527204439886777,
+            "velocityX": 1.401958054303432,
+            "velocityY": 0.46870407793850294,
+            "timestamp": 0.17752738792077627
+        },
+        {
+            "x": 5.5493620218671325,
+            "y": 6.7548129941209,
+            "heading": -2.950667481161702,
+            "angularVelocity": -0.37441484524641305,
+            "velocityX": 2.1034412763117816,
+            "velocityY": 0.70308178809801,
+            "timestamp": 0.2662910818811644
+        },
+        {
+            "x": 5.798380867010647,
+            "y": 6.838019600580292,
+            "heading": -2.987306905639368,
+            "angularVelocity": -0.4127748952720924,
+            "velocityX": 2.8054132723935825,
+            "velocityY": 0.9373945894737359,
+            "timestamp": 0.35505477584155254
+        },
+        {
+            "x": 6.109622963136531,
+            "y": 6.941929172833481,
+            "heading": -3.009821564076527,
+            "angularVelocity": -0.25364715496791174,
+            "velocityX": 3.5064121628854164,
+            "velocityY": 1.1706314554748125,
+            "timestamp": 0.4438184698019407
+        },
+        {
+            "x": 6.428045089115211,
+            "y": 7.048516124767363,
+            "heading": -3.0098215751251205,
+            "angularVelocity": -1.2447253578662016e-7,
+            "velocityX": 3.5873014266485415,
+            "velocityY": 1.2007944597359943,
+            "timestamp": 0.5325821637623288
+        },
+        {
+            "x": 6.746467236003722,
+            "y": 7.1551030142348395,
+            "heading": -3.0098215861735884,
+            "angularVelocity": -1.2447113137335796e-7,
+            "velocityX": 3.5873016622160976,
+            "velocityY": 1.2007937559974857,
+            "timestamp": 0.6213458577227169
+        },
+        {
+            "x": 7.057671617899985,
+            "y": 7.2589999980659305,
+            "heading": -3.0321320878151,
+            "angularVelocity": -0.2513471515963816,
+            "velocityX": 3.5059872793849554,
+            "velocityY": 1.1704896359626165,
+            "timestamp": 0.7101095516831051
+        },
+        {
+            "x": 7.306674739642628,
+            "y": 7.342201198643625,
+            "heading": -3.06987813605104,
+            "angularVelocity": -0.42524197169930134,
+            "velocityX": 2.8052361346509134,
+            "velocityY": 0.9373336875121538,
+            "timestamp": 0.7988732456434933
+        },
+        {
+            "x": 7.493368024895104,
+            "y": 7.4046034302940065,
+            "heading": -3.1043079780358913,
+            "angularVelocity": -0.3878820320283419,
+            "velocityX": 2.1032617833134357,
+            "velocityY": 0.7030152629492495,
+            "timestamp": 0.8876369396038815
+        },
+        {
+            "x": 7.617797798391618,
+            "y": 7.446202168700309,
+            "heading": -3.129675012430027,
+            "angularVelocity": -0.28578164406378687,
+            "velocityX": 1.4018093202853217,
+            "velocityY": 0.4686458680340631,
+            "timestamp": 0.9764006335642696
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.15220518825005472,
+            "velocityX": 0.7007617510398297,
+            "velocityY": 0.23430560820061222,
+            "timestamp": 1.0651643275246578
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0,
+            "velocityX": 1.9101182509980996e-31,
+            "velocityY": -4.716397884209056e-30,
+            "timestamp": 1.153928021485046
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.traj
new file mode 100644
index 00000000..cfc8d5b6
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0,
+            "velocityX": 1.8796456462447601e-29,
+            "velocityY": -4.3119341309057906e-30,
+            "timestamp": 0
+        },
+        {
+            "x": 5.238209828475023,
+            "y": 6.650800952122015,
+            "heading": -2.892998872920493,
+            "angularVelocity": -0.1464435777830739,
+            "velocityX": 0.7008476743058382,
+            "velocityY": 0.23434076697564932,
+            "timestamp": 0.08876369396038813
+        },
+        {
+            "x": 5.362652804152687,
+            "y": 6.692404857453675,
+            "heading": -2.917433036424788,
+            "angularVelocity": -0.27527204439886777,
+            "velocityX": 1.401958054303432,
+            "velocityY": 0.46870407793850294,
+            "timestamp": 0.17752738792077627
+        },
+        {
+            "x": 5.5493620218671325,
+            "y": 6.7548129941209,
+            "heading": -2.950667481161702,
+            "angularVelocity": -0.37441484524641305,
+            "velocityX": 2.1034412763117816,
+            "velocityY": 0.70308178809801,
+            "timestamp": 0.2662910818811644
+        },
+        {
+            "x": 5.798380867010647,
+            "y": 6.838019600580292,
+            "heading": -2.987306905639368,
+            "angularVelocity": -0.4127748952720924,
+            "velocityX": 2.8054132723935825,
+            "velocityY": 0.9373945894737359,
+            "timestamp": 0.35505477584155254
+        },
+        {
+            "x": 6.109622963136531,
+            "y": 6.941929172833481,
+            "heading": -3.009821564076527,
+            "angularVelocity": -0.25364715496791174,
+            "velocityX": 3.5064121628854164,
+            "velocityY": 1.1706314554748125,
+            "timestamp": 0.4438184698019407
+        },
+        {
+            "x": 6.428045089115211,
+            "y": 7.048516124767363,
+            "heading": -3.0098215751251205,
+            "angularVelocity": -1.2447253578662016e-7,
+            "velocityX": 3.5873014266485415,
+            "velocityY": 1.2007944597359943,
+            "timestamp": 0.5325821637623288
+        },
+        {
+            "x": 6.746467236003722,
+            "y": 7.1551030142348395,
+            "heading": -3.0098215861735884,
+            "angularVelocity": -1.2447113137335796e-7,
+            "velocityX": 3.5873016622160976,
+            "velocityY": 1.2007937559974857,
+            "timestamp": 0.6213458577227169
+        },
+        {
+            "x": 7.057671617899985,
+            "y": 7.2589999980659305,
+            "heading": -3.0321320878151,
+            "angularVelocity": -0.2513471515963816,
+            "velocityX": 3.5059872793849554,
+            "velocityY": 1.1704896359626165,
+            "timestamp": 0.7101095516831051
+        },
+        {
+            "x": 7.306674739642628,
+            "y": 7.342201198643625,
+            "heading": -3.06987813605104,
+            "angularVelocity": -0.42524197169930134,
+            "velocityX": 2.8052361346509134,
+            "velocityY": 0.9373336875121538,
+            "timestamp": 0.7988732456434933
+        },
+        {
+            "x": 7.493368024895104,
+            "y": 7.4046034302940065,
+            "heading": -3.1043079780358913,
+            "angularVelocity": -0.3878820320283419,
+            "velocityX": 2.1032617833134357,
+            "velocityY": 0.7030152629492495,
+            "timestamp": 0.8876369396038815
+        },
+        {
+            "x": 7.617797798391618,
+            "y": 7.446202168700309,
+            "heading": -3.129675012430027,
+            "angularVelocity": -0.28578164406378687,
+            "velocityX": 1.4018093202853217,
+            "velocityY": 0.4686458680340631,
+            "timestamp": 0.9764006335642696
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.15220518825005472,
+            "velocityX": 0.7007617510398297,
+            "velocityY": 0.23430560820061222,
+            "timestamp": 1.0651643275246578
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0,
+            "velocityX": 1.9101182509980996e-31,
+            "velocityY": -4.716397884209056e-30,
+            "timestamp": 1.153928021485046
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.1.traj
new file mode 100644
index 00000000..4c23c37c
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0,
+            "velocityX": -2.1431055300877995e-28,
+            "velocityY": 1.1784110457685481e-29,
+            "timestamp": 0
+        },
+        {
+            "x": 5.242060097443899,
+            "y": 6.625920772567306,
+            "heading": -2.9233556616628746,
+            "angularVelocity": -0.48269065593589344,
+            "velocityX": 0.7354654627191669,
+            "velocityY": -0.04541517508160964,
+            "timestamp": 0.08982080164534181
+        },
+        {
+            "x": 5.3743817426165945,
+            "y": 6.616908903759451,
+            "heading": -3.004544486756046,
+            "angularVelocity": -0.9038978010210977,
+            "velocityX": 1.4731737275644603,
+            "velocityY": -0.1003316452622891,
+            "timestamp": 0.17964160329068363
+        },
+        {
+            "x": 5.573213116332803,
+            "y": 6.601503149315071,
+            "heading": -3.1137348179578126,
+            "angularVelocity": -1.215646367006541,
+            "velocityX": 2.2136450585388348,
+            "velocityY": -0.17151655476435285,
+            "timestamp": 0.26946240493602547
+        },
+        {
+            "x": 5.83860703608144,
+            "y": 6.576562673459042,
+            "heading": -3.229878025812627,
+            "angularVelocity": -1.2930546791757207,
+            "velocityX": 2.954704421327105,
+            "velocityY": -0.2776692636802449,
+            "timestamp": 0.35928320658136725
+        },
+        {
+            "x": 6.164587325578701,
+            "y": 6.531858569137778,
+            "heading": -3.2661345564867124,
+            "angularVelocity": -0.40365405351480704,
+            "velocityX": 3.629229349170066,
+            "velocityY": -0.49770324359594226,
+            "timestamp": 0.44910400822670904
+        },
+        {
+            "x": 6.48571912004567,
+            "y": 6.420820709470225,
+            "heading": -3.2661345579017715,
+            "angularVelocity": -1.5754250259377596e-8,
+            "velocityX": 3.5752497036818096,
+            "velocityY": -1.2362154159565475,
+            "timestamp": 0.5389248098720508
+        },
+        {
+            "x": 6.786864453228811,
+            "y": 6.263446876676794,
+            "heading": -3.2661345453374624,
+            "angularVelocity": 1.3988195638330222e-7,
+            "velocityX": 3.3527348639373855,
+            "velocityY": -1.7520867094330927,
+            "timestamp": 0.6287456115173926
+        },
+        {
+            "x": 7.084475527096153,
+            "y": 6.108061043338325,
+            "heading": -3.255293907795362,
+            "angularVelocity": 0.12069183689660036,
+            "velocityX": 3.313386970675918,
+            "velocityY": -1.7299537578389734,
+            "timestamp": 0.7185664131627344
+        },
+        {
+            "x": 7.322762050188167,
+            "y": 5.983606942670793,
+            "heading": -3.219406300389853,
+            "angularVelocity": 0.3995467280198762,
+            "velocityX": 2.652910224881898,
+            "velocityY": -1.385582163460734,
+            "timestamp": 0.8083872148080762
+        },
+        {
+            "x": 7.5014175228899544,
+            "y": 5.890287602383411,
+            "heading": -3.184260886361704,
+            "angularVelocity": 0.39128368244731204,
+            "velocityX": 1.9890211335143717,
+            "velocityY": -1.0389502050521982,
+            "timestamp": 0.898208016453418
+        },
+        {
+            "x": 7.620482912490659,
+            "y": 5.8280911603292145,
+            "heading": -3.1576094170182762,
+            "angularVelocity": 0.296718230690803,
+            "velocityX": 1.325588142386374,
+            "velocityY": -0.6924503112294588,
+            "timestamp": 0.9880288180987598
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0.16058763197915146,
+            "velocityX": 0.6626203108757032,
+            "velocityY": -0.3461465468987679,
+            "timestamp": 1.0778496197441017
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -3.143185307179586,
+            "angularVelocity": 2.6110530197350993e-29,
+            "velocityX": -1.9135813261242845e-30,
+            "velocityY": -4.114109248291899e-30,
+            "timestamp": 1.1676704213894435
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.traj
new file mode 100644
index 00000000..4c23c37c
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0,
+            "velocityX": -2.1431055300877995e-28,
+            "velocityY": 1.1784110457685481e-29,
+            "timestamp": 0
+        },
+        {
+            "x": 5.242060097443899,
+            "y": 6.625920772567306,
+            "heading": -2.9233556616628746,
+            "angularVelocity": -0.48269065593589344,
+            "velocityX": 0.7354654627191669,
+            "velocityY": -0.04541517508160964,
+            "timestamp": 0.08982080164534181
+        },
+        {
+            "x": 5.3743817426165945,
+            "y": 6.616908903759451,
+            "heading": -3.004544486756046,
+            "angularVelocity": -0.9038978010210977,
+            "velocityX": 1.4731737275644603,
+            "velocityY": -0.1003316452622891,
+            "timestamp": 0.17964160329068363
+        },
+        {
+            "x": 5.573213116332803,
+            "y": 6.601503149315071,
+            "heading": -3.1137348179578126,
+            "angularVelocity": -1.215646367006541,
+            "velocityX": 2.2136450585388348,
+            "velocityY": -0.17151655476435285,
+            "timestamp": 0.26946240493602547
+        },
+        {
+            "x": 5.83860703608144,
+            "y": 6.576562673459042,
+            "heading": -3.229878025812627,
+            "angularVelocity": -1.2930546791757207,
+            "velocityX": 2.954704421327105,
+            "velocityY": -0.2776692636802449,
+            "timestamp": 0.35928320658136725
+        },
+        {
+            "x": 6.164587325578701,
+            "y": 6.531858569137778,
+            "heading": -3.2661345564867124,
+            "angularVelocity": -0.40365405351480704,
+            "velocityX": 3.629229349170066,
+            "velocityY": -0.49770324359594226,
+            "timestamp": 0.44910400822670904
+        },
+        {
+            "x": 6.48571912004567,
+            "y": 6.420820709470225,
+            "heading": -3.2661345579017715,
+            "angularVelocity": -1.5754250259377596e-8,
+            "velocityX": 3.5752497036818096,
+            "velocityY": -1.2362154159565475,
+            "timestamp": 0.5389248098720508
+        },
+        {
+            "x": 6.786864453228811,
+            "y": 6.263446876676794,
+            "heading": -3.2661345453374624,
+            "angularVelocity": 1.3988195638330222e-7,
+            "velocityX": 3.3527348639373855,
+            "velocityY": -1.7520867094330927,
+            "timestamp": 0.6287456115173926
+        },
+        {
+            "x": 7.084475527096153,
+            "y": 6.108061043338325,
+            "heading": -3.255293907795362,
+            "angularVelocity": 0.12069183689660036,
+            "velocityX": 3.313386970675918,
+            "velocityY": -1.7299537578389734,
+            "timestamp": 0.7185664131627344
+        },
+        {
+            "x": 7.322762050188167,
+            "y": 5.983606942670793,
+            "heading": -3.219406300389853,
+            "angularVelocity": 0.3995467280198762,
+            "velocityX": 2.652910224881898,
+            "velocityY": -1.385582163460734,
+            "timestamp": 0.8083872148080762
+        },
+        {
+            "x": 7.5014175228899544,
+            "y": 5.890287602383411,
+            "heading": -3.184260886361704,
+            "angularVelocity": 0.39128368244731204,
+            "velocityX": 1.9890211335143717,
+            "velocityY": -1.0389502050521982,
+            "timestamp": 0.898208016453418
+        },
+        {
+            "x": 7.620482912490659,
+            "y": 5.8280911603292145,
+            "heading": -3.1576094170182762,
+            "angularVelocity": 0.296718230690803,
+            "velocityX": 1.325588142386374,
+            "velocityY": -0.6924503112294588,
+            "timestamp": 0.9880288180987598
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0.16058763197915146,
+            "velocityX": 0.6626203108757032,
+            "velocityY": -0.3461465468987679,
+            "timestamp": 1.0778496197441017
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -3.143185307179586,
+            "angularVelocity": 2.6110530197350993e-29,
+            "velocityX": -1.9135813261242845e-30,
+            "velocityY": -4.114109248291899e-30,
+            "timestamp": 1.1676704213894435
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj
new file mode 100644
index 00000000..877ee8fe
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": -6.0746648103992426e-33,
+            "velocityX": -3.2850296304780664e-35,
+            "velocityY": -2.0665663458795258e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 5.102979054151253,
+            "y": 6.633224554548809,
+            "heading": -2.8734987043090925,
+            "angularVelocity": 0.06942752469656709,
+            "velocityX": -0.7797927924375683,
+            "velocityY": 0.03443511127885569,
+            "timestamp": 0.09364147316685267
+        },
+        {
+            "x": 4.9569371637209425,
+            "y": 6.639673650012151,
+            "heading": -2.8604963348705175,
+            "angularVelocity": 0.1388526792548496,
+            "velocityX": -1.5595855713428277,
+            "velocityY": 0.0688700769568818,
+            "timestamp": 0.18728294633370535
+        },
+        {
+            "x": 4.737874332155748,
+            "y": 6.649347273486247,
+            "heading": -2.8409932661051047,
+            "angularVelocity": 0.2082738353622199,
+            "velocityX": -2.3393783134408976,
+            "velocityY": 0.10330490483508797,
+            "timestamp": 0.280924419500558
+        },
+        {
+            "x": 4.445790573832511,
+            "y": 6.662245412808618,
+            "heading": -2.8149900541710062,
+            "angularVelocity": 0.27768905224050994,
+            "velocityX": -3.119170902008226,
+            "velocityY": 0.13773960282991304,
+            "timestamp": 0.3745658926674107
+        },
+        {
+            "x": 4.091895329085733,
+            "y": 6.677873079084158,
+            "heading": -2.814990051178798,
+            "angularVelocity": 3.195386469208516e-8,
+            "velocityX": -3.7792575530737516,
+            "velocityY": 0.16688830009804687,
+            "timestamp": 0.46820736583426337
+        },
+        {
+            "x": 3.738000080960507,
+            "y": 6.693500668821502,
+            "heading": -2.8149900481866714,
+            "angularVelocity": 3.195300400483476e-8,
+            "velocityX": -3.7792575891522513,
+            "velocityY": 0.16688748274491266,
+            "timestamp": 0.561848839001116
+        },
+        {
+            "x": 3.384104829826614,
+            "y": 6.709128190425974,
+            "heading": -2.8149900451945484,
+            "angularVelocity": 3.1952965397811645e-8,
+            "velocityX": -3.7792576212818934,
+            "velocityY": 0.1668867551519713,
+            "timestamp": 0.6554903121679687
+        },
+        {
+            "x": 3.0302095759842613,
+            "y": 6.724755650696981,
+            "heading": -2.8149900422022327,
+            "angularVelocity": 3.195501290053148e-8,
+            "velocityX": -3.779257650205636,
+            "velocityY": 0.16688610017037783,
+            "timestamp": 0.7491317853348214
+        },
+        {
+            "x": 2.738125764107778,
+            "y": 6.7376533993595,
+            "heading": -2.7889938425825695,
+            "angularVelocity": 0.2776141675314341,
+            "velocityX": -3.1191714739049314,
+            "velocityY": 0.1377354309613996,
+            "timestamp": 0.8427732585016741
+        },
+        {
+            "x": 2.519062886855627,
+            "y": 6.747326704296012,
+            "heading": -2.7694968873701145,
+            "angularVelocity": 0.20820854855310947,
+            "velocityX": -2.339378801333246,
+            "velocityY": 0.10330150316261463,
+            "timestamp": 0.9364147316685267
+        },
+        {
+            "x": 2.3730209632759087,
+            "y": 6.753775569660814,
+            "heading": -2.756498972823295,
+            "angularVelocity": 0.13880510533677407,
+            "velocityX": -1.559585925346323,
+            "velocityY": 0.0688676197276826,
+            "timestamp": 1.0300562048353794
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0.06940271872607656,
+            "velocityX": -0.779792978542731,
+            "velocityY": 0.034433784840577714,
+            "timestamp": 1.123697678002232
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": -2.0910816279231796e-36,
+            "velocityY": 1.251790530664498e-31,
+            "timestamp": 1.2173391511690848
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.traj
new file mode 100644
index 00000000..877ee8fe
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": -6.0746648103992426e-33,
+            "velocityX": -3.2850296304780664e-35,
+            "velocityY": -2.0665663458795258e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 5.102979054151253,
+            "y": 6.633224554548809,
+            "heading": -2.8734987043090925,
+            "angularVelocity": 0.06942752469656709,
+            "velocityX": -0.7797927924375683,
+            "velocityY": 0.03443511127885569,
+            "timestamp": 0.09364147316685267
+        },
+        {
+            "x": 4.9569371637209425,
+            "y": 6.639673650012151,
+            "heading": -2.8604963348705175,
+            "angularVelocity": 0.1388526792548496,
+            "velocityX": -1.5595855713428277,
+            "velocityY": 0.0688700769568818,
+            "timestamp": 0.18728294633370535
+        },
+        {
+            "x": 4.737874332155748,
+            "y": 6.649347273486247,
+            "heading": -2.8409932661051047,
+            "angularVelocity": 0.2082738353622199,
+            "velocityX": -2.3393783134408976,
+            "velocityY": 0.10330490483508797,
+            "timestamp": 0.280924419500558
+        },
+        {
+            "x": 4.445790573832511,
+            "y": 6.662245412808618,
+            "heading": -2.8149900541710062,
+            "angularVelocity": 0.27768905224050994,
+            "velocityX": -3.119170902008226,
+            "velocityY": 0.13773960282991304,
+            "timestamp": 0.3745658926674107
+        },
+        {
+            "x": 4.091895329085733,
+            "y": 6.677873079084158,
+            "heading": -2.814990051178798,
+            "angularVelocity": 3.195386469208516e-8,
+            "velocityX": -3.7792575530737516,
+            "velocityY": 0.16688830009804687,
+            "timestamp": 0.46820736583426337
+        },
+        {
+            "x": 3.738000080960507,
+            "y": 6.693500668821502,
+            "heading": -2.8149900481866714,
+            "angularVelocity": 3.195300400483476e-8,
+            "velocityX": -3.7792575891522513,
+            "velocityY": 0.16688748274491266,
+            "timestamp": 0.561848839001116
+        },
+        {
+            "x": 3.384104829826614,
+            "y": 6.709128190425974,
+            "heading": -2.8149900451945484,
+            "angularVelocity": 3.1952965397811645e-8,
+            "velocityX": -3.7792576212818934,
+            "velocityY": 0.1668867551519713,
+            "timestamp": 0.6554903121679687
+        },
+        {
+            "x": 3.0302095759842613,
+            "y": 6.724755650696981,
+            "heading": -2.8149900422022327,
+            "angularVelocity": 3.195501290053148e-8,
+            "velocityX": -3.779257650205636,
+            "velocityY": 0.16688610017037783,
+            "timestamp": 0.7491317853348214
+        },
+        {
+            "x": 2.738125764107778,
+            "y": 6.7376533993595,
+            "heading": -2.7889938425825695,
+            "angularVelocity": 0.2776141675314341,
+            "velocityX": -3.1191714739049314,
+            "velocityY": 0.1377354309613996,
+            "timestamp": 0.8427732585016741
+        },
+        {
+            "x": 2.519062886855627,
+            "y": 6.747326704296012,
+            "heading": -2.7694968873701145,
+            "angularVelocity": 0.20820854855310947,
+            "velocityX": -2.339378801333246,
+            "velocityY": 0.10330150316261463,
+            "timestamp": 0.9364147316685267
+        },
+        {
+            "x": 2.3730209632759087,
+            "y": 6.753775569660814,
+            "heading": -2.756498972823295,
+            "angularVelocity": 0.13880510533677407,
+            "velocityX": -1.559585925346323,
+            "velocityY": 0.0688676197276826,
+            "timestamp": 1.0300562048353794
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0.06940271872607656,
+            "velocityX": -0.779792978542731,
+            "velocityY": 0.034433784840577714,
+            "timestamp": 1.123697678002232
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": -2.0910816279231796e-36,
+            "velocityY": 1.251790530664498e-31,
+            "timestamp": 1.2173391511690848
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj
new file mode 100644
index 00000000..75b837e7
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0,
+            "velocityX": -8.07520618943781e-35,
+            "velocityY": -1.6093142036511268e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 5.118924952955758,
+            "y": 6.609128496946765,
+            "heading": -2.888779485010293,
+            "angularVelocity": -0.10279588776958423,
+            "velocityX": -0.6682715584712982,
+            "velocityY": -0.24437705434061835,
+            "timestamp": 0.08540696715389907
+        },
+        {
+            "x": 5.004774858765426,
+            "y": 6.567385488245384,
+            "heading": -2.906338067516999,
+            "angularVelocity": -0.20558723827608577,
+            "velocityX": -1.3365431181350638,
+            "velocityY": -0.48875413906409965,
+            "timestamp": 0.17081393430779815
+        },
+        {
+            "x": 4.833549713525512,
+            "y": 6.504770974662745,
+            "heading": -2.9326749312069125,
+            "angularVelocity": -0.3083690308596943,
+            "velocityX": -2.004814723503466,
+            "velocityY": -0.7331312148084055,
+            "timestamp": 0.25622090146169724
+        },
+        {
+            "x": 4.6052495100619275,
+            "y": 6.421284959128349,
+            "heading": -2.96778862782457,
+            "angularVelocity": -0.4111338663318194,
+            "velocityX": -2.673086412870736,
+            "velocityY": -0.9775082562521819,
+            "timestamp": 0.3416278686155963
+        },
+        {
+            "x": 4.319874242645404,
+            "y": 6.316927450773692,
+            "heading": -3.011676691839701,
+            "angularVelocity": -0.513869833781204,
+            "velocityX": -3.3413581693199683,
+            "velocityY": -1.2218851907785282,
+            "timestamp": 0.42703483576949536
+        },
+        {
+            "x": 4.016437344871394,
+            "y": 6.205964133135793,
+            "heading": -3.0116766970908038,
+            "angularVelocity": -6.148330724628331e-8,
+            "velocityX": -3.5528354171297467,
+            "velocityY": -1.2992302775246547,
+            "timestamp": 0.5124418029233945
+        },
+        {
+            "x": 3.7130004778936962,
+            "y": 6.095000731283241,
+            "heading": -3.011676702341878,
+            "angularVelocity": -6.148297185895772e-8,
+            "velocityX": -3.552835056546619,
+            "velocityY": -1.299231263564264,
+            "timestamp": 0.5978487700772935
+        },
+        {
+            "x": 3.4095636397411098,
+            "y": 5.984037250606526,
+            "heading": -3.011676707592942,
+            "angularVelocity": -6.148285239694146e-8,
+            "velocityX": -3.5528347190435716,
+            "velocityY": -1.2992321864885386,
+            "timestamp": 0.6832557372311926
+        },
+        {
+            "x": 3.106126829490984,
+            "y": 5.873073693628802,
+            "heading": -3.0116767128440194,
+            "angularVelocity": -6.14830072681285e-8,
+            "velocityX": -3.5528343923435184,
+            "velocityY": -1.2992330798700915,
+            "timestamp": 0.7686627043850917
+        },
+        {
+            "x": 2.8207512525311835,
+            "y": 5.768715824967853,
+            "heading": -3.055515171895212,
+            "angularVelocity": -0.5132890267862834,
+            "velocityX": -3.3413617936528297,
+            "velocityY": -1.221889409477577,
+            "timestamp": 0.8540696715389907
+        },
+        {
+            "x": 2.5924507605120093,
+            "y": 5.68522951062146,
+            "heading": -3.090584080219249,
+            "angularVelocity": -0.4106094560276795,
+            "velocityX": -2.6730897914661687,
+            "velocityY": -0.9775117549362862,
+            "timestamp": 0.9394766386928898
+        },
+        {
+            "x": 2.421225380941837,
+            "y": 5.6226147624914855,
+            "heading": -3.116884817520286,
+            "angularVelocity": -0.3079460397375414,
+            "velocityX": -2.004817467193667,
+            "velocityY": -0.7331339610402823,
+            "timestamp": 1.024883605846789
+        },
+        {
+            "x": 2.307075126087855,
+            "y": 5.580871589665102,
+            "heading": -3.1344184120174683,
+            "angularVelocity": -0.20529466250203854,
+            "velocityX": -1.3365449992889762,
+            "velocityY": -0.48875606074577516,
+            "timestamp": 1.110290573000688
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.10264847768590626,
+            "velocityX": -0.6682724839650215,
+            "velocityY": -0.24437806844836182,
+            "timestamp": 1.195697540154587
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0,
+            "velocityX": -3.560921302709756e-34,
+            "velocityY": 7.627797313302701e-34,
+            "timestamp": 1.2811045073084861
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.traj
new file mode 100644
index 00000000..75b837e7
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 0,
+            "velocityX": -8.07520618943781e-35,
+            "velocityY": -1.6093142036511268e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 5.118924952955758,
+            "y": 6.609128496946765,
+            "heading": -2.888779485010293,
+            "angularVelocity": -0.10279588776958423,
+            "velocityX": -0.6682715584712982,
+            "velocityY": -0.24437705434061835,
+            "timestamp": 0.08540696715389907
+        },
+        {
+            "x": 5.004774858765426,
+            "y": 6.567385488245384,
+            "heading": -2.906338067516999,
+            "angularVelocity": -0.20558723827608577,
+            "velocityX": -1.3365431181350638,
+            "velocityY": -0.48875413906409965,
+            "timestamp": 0.17081393430779815
+        },
+        {
+            "x": 4.833549713525512,
+            "y": 6.504770974662745,
+            "heading": -2.9326749312069125,
+            "angularVelocity": -0.3083690308596943,
+            "velocityX": -2.004814723503466,
+            "velocityY": -0.7331312148084055,
+            "timestamp": 0.25622090146169724
+        },
+        {
+            "x": 4.6052495100619275,
+            "y": 6.421284959128349,
+            "heading": -2.96778862782457,
+            "angularVelocity": -0.4111338663318194,
+            "velocityX": -2.673086412870736,
+            "velocityY": -0.9775082562521819,
+            "timestamp": 0.3416278686155963
+        },
+        {
+            "x": 4.319874242645404,
+            "y": 6.316927450773692,
+            "heading": -3.011676691839701,
+            "angularVelocity": -0.513869833781204,
+            "velocityX": -3.3413581693199683,
+            "velocityY": -1.2218851907785282,
+            "timestamp": 0.42703483576949536
+        },
+        {
+            "x": 4.016437344871394,
+            "y": 6.205964133135793,
+            "heading": -3.0116766970908038,
+            "angularVelocity": -6.148330724628331e-8,
+            "velocityX": -3.5528354171297467,
+            "velocityY": -1.2992302775246547,
+            "timestamp": 0.5124418029233945
+        },
+        {
+            "x": 3.7130004778936962,
+            "y": 6.095000731283241,
+            "heading": -3.011676702341878,
+            "angularVelocity": -6.148297185895772e-8,
+            "velocityX": -3.552835056546619,
+            "velocityY": -1.299231263564264,
+            "timestamp": 0.5978487700772935
+        },
+        {
+            "x": 3.4095636397411098,
+            "y": 5.984037250606526,
+            "heading": -3.011676707592942,
+            "angularVelocity": -6.148285239694146e-8,
+            "velocityX": -3.5528347190435716,
+            "velocityY": -1.2992321864885386,
+            "timestamp": 0.6832557372311926
+        },
+        {
+            "x": 3.106126829490984,
+            "y": 5.873073693628802,
+            "heading": -3.0116767128440194,
+            "angularVelocity": -6.14830072681285e-8,
+            "velocityX": -3.5528343923435184,
+            "velocityY": -1.2992330798700915,
+            "timestamp": 0.7686627043850917
+        },
+        {
+            "x": 2.8207512525311835,
+            "y": 5.768715824967853,
+            "heading": -3.055515171895212,
+            "angularVelocity": -0.5132890267862834,
+            "velocityX": -3.3413617936528297,
+            "velocityY": -1.221889409477577,
+            "timestamp": 0.8540696715389907
+        },
+        {
+            "x": 2.5924507605120093,
+            "y": 5.68522951062146,
+            "heading": -3.090584080219249,
+            "angularVelocity": -0.4106094560276795,
+            "velocityX": -2.6730897914661687,
+            "velocityY": -0.9775117549362862,
+            "timestamp": 0.9394766386928898
+        },
+        {
+            "x": 2.421225380941837,
+            "y": 5.6226147624914855,
+            "heading": -3.116884817520286,
+            "angularVelocity": -0.3079460397375414,
+            "velocityX": -2.004817467193667,
+            "velocityY": -0.7331339610402823,
+            "timestamp": 1.024883605846789
+        },
+        {
+            "x": 2.307075126087855,
+            "y": 5.580871589665102,
+            "heading": -3.1344184120174683,
+            "angularVelocity": -0.20529466250203854,
+            "velocityX": -1.3365449992889762,
+            "velocityY": -0.48875606074577516,
+            "timestamp": 1.110290573000688
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.10264847768590626,
+            "velocityX": -0.6682724839650215,
+            "velocityY": -0.24437806844836182,
+            "timestamp": 1.195697540154587
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0,
+            "velocityX": -3.560921302709756e-34,
+            "velocityY": 7.627797313302701e-34,
+            "timestamp": 1.2811045073084861
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.1.traj
new file mode 100644
index 00000000..a50593ec
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 7.124511967920712e-30,
+            "velocityY": -7.12047614876129e-30,
+            "timestamp": 0
+        },
+        {
+            "x": 4.30430205284905,
+            "y": 5.6386850261363115,
+            "heading": 3.0976627193075768,
+            "angularVelocity": -0.4082995079820314,
+            "velocityX": 0.3983158006634516,
+            "velocityY": 0.758836111615579,
+            "timestamp": 0.10369172596279416
+        },
+        {
+            "x": 4.388754424241646,
+            "y": 5.795256557860491,
+            "heading": 3.0179562679096885,
+            "angularVelocity": -0.7686867072352654,
+            "velocityX": 0.8144562221183627,
+            "velocityY": 1.5099713142045914,
+            "timestamp": 0.20738345192558832
+        },
+        {
+            "x": 4.5199632474011375,
+            "y": 6.027943066067755,
+            "heading": 2.911091818022606,
+            "angularVelocity": -1.030597657574192,
+            "velocityX": 1.265374087866657,
+            "velocityY": 2.2440219414493656,
+            "timestamp": 0.31107517788838246
+        },
+        {
+            "x": 4.707776355466448,
+            "y": 6.330145783951522,
+            "heading": 2.807461071652614,
+            "angularVelocity": -0.9994119145743484,
+            "velocityX": 1.8112641709976698,
+            "velocityY": 2.914434252856334,
+            "timestamp": 0.41476690385117665
+        },
+        {
+            "x": 4.978743687535899,
+            "y": 6.613772866928766,
+            "heading": 2.807461069210919,
+            "angularVelocity": -2.3547634644306893e-8,
+            "velocityX": 2.613201097324575,
+            "velocityY": 2.7352913681754196,
+            "timestamp": 0.5184586298139708
+        },
+        {
+            "x": 5.307032554871728,
+            "y": 6.82846808577305,
+            "heading": 2.80746110336737,
+            "angularVelocity": 3.2940382214109427e-7,
+            "velocityX": 3.166008322145415,
+            "velocityY": 2.0705144682548786,
+            "timestamp": 0.622150355776765
+        },
+        {
+            "x": 5.675494511223277,
+            "y": 6.963016991691847,
+            "heading": 2.8074611309590716,
+            "angularVelocity": 2.660935682689419e-7,
+            "velocityX": 3.5534364283198543,
+            "velocityY": 1.2975857491953324,
+            "timestamp": 0.7258420817395592
+        },
+        {
+            "x": 6.055822077771958,
+            "y": 7.05903034533374,
+            "heading": 2.8074611577219364,
+            "angularVelocity": 2.5810029119060704e-7,
+            "velocityX": 3.6678680291727868,
+            "velocityY": 0.9259499998712609,
+            "timestamp": 0.8295338077023534
+        },
+        {
+            "x": 6.436149809853972,
+            "y": 7.155043043263379,
+            "heading": 2.807461184484807,
+            "angularVelocity": 2.58100350606906e-7,
+            "velocityX": 3.6678696255714556,
+            "velocityY": 0.9259436762012891,
+            "timestamp": 0.9332255336651476
+        },
+        {
+            "x": 6.816477543748484,
+            "y": 7.2510557340128905,
+            "heading": 2.8074612112477313,
+            "angularVelocity": 2.581008643735431e-7,
+            "velocityX": 3.667869643051154,
+            "velocityY": 0.9259436069563147,
+            "timestamp": 1.0369172596279417
+        },
+        {
+            "x": 7.162445200777026,
+            "y": 7.337344185673276,
+            "heading": 2.898451236968193,
+            "angularVelocity": 0.8775051709825819,
+            "velocityX": 3.3365020575767024,
+            "velocityY": 0.8321633270078459,
+            "timestamp": 1.1406089855907358
+        },
+        {
+            "x": 7.421534649307807,
+            "y": 7.402207433353085,
+            "heading": 3.009161772576331,
+            "angularVelocity": 1.0676891968008626,
+            "velocityX": 2.498651132721501,
+            "velocityY": 0.6255392807626672,
+            "timestamp": 1.2443007115535298
+        },
+        {
+            "x": 7.593924390045372,
+            "y": 7.445415282758517,
+            "heading": 3.094199643650422,
+            "angularVelocity": 0.8201027640778757,
+            "velocityX": 1.6625216634876054,
+            "velocityY": 0.416695247419558,
+            "timestamp": 1.347992437516324
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 0.4416973092532584,
+            "velocityX": 0.8301106877660893,
+            "velocityY": 0.20816238751031255,
+            "timestamp": 1.451684163479118
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -4.6860665144292015e-31,
+            "velocityY": 3.7247780913146357e-28,
+            "timestamp": 1.555375889441912
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.traj
new file mode 100644
index 00000000..a50593ec
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 7.124511967920712e-30,
+            "velocityY": -7.12047614876129e-30,
+            "timestamp": 0
+        },
+        {
+            "x": 4.30430205284905,
+            "y": 5.6386850261363115,
+            "heading": 3.0976627193075768,
+            "angularVelocity": -0.4082995079820314,
+            "velocityX": 0.3983158006634516,
+            "velocityY": 0.758836111615579,
+            "timestamp": 0.10369172596279416
+        },
+        {
+            "x": 4.388754424241646,
+            "y": 5.795256557860491,
+            "heading": 3.0179562679096885,
+            "angularVelocity": -0.7686867072352654,
+            "velocityX": 0.8144562221183627,
+            "velocityY": 1.5099713142045914,
+            "timestamp": 0.20738345192558832
+        },
+        {
+            "x": 4.5199632474011375,
+            "y": 6.027943066067755,
+            "heading": 2.911091818022606,
+            "angularVelocity": -1.030597657574192,
+            "velocityX": 1.265374087866657,
+            "velocityY": 2.2440219414493656,
+            "timestamp": 0.31107517788838246
+        },
+        {
+            "x": 4.707776355466448,
+            "y": 6.330145783951522,
+            "heading": 2.807461071652614,
+            "angularVelocity": -0.9994119145743484,
+            "velocityX": 1.8112641709976698,
+            "velocityY": 2.914434252856334,
+            "timestamp": 0.41476690385117665
+        },
+        {
+            "x": 4.978743687535899,
+            "y": 6.613772866928766,
+            "heading": 2.807461069210919,
+            "angularVelocity": -2.3547634644306893e-8,
+            "velocityX": 2.613201097324575,
+            "velocityY": 2.7352913681754196,
+            "timestamp": 0.5184586298139708
+        },
+        {
+            "x": 5.307032554871728,
+            "y": 6.82846808577305,
+            "heading": 2.80746110336737,
+            "angularVelocity": 3.2940382214109427e-7,
+            "velocityX": 3.166008322145415,
+            "velocityY": 2.0705144682548786,
+            "timestamp": 0.622150355776765
+        },
+        {
+            "x": 5.675494511223277,
+            "y": 6.963016991691847,
+            "heading": 2.8074611309590716,
+            "angularVelocity": 2.660935682689419e-7,
+            "velocityX": 3.5534364283198543,
+            "velocityY": 1.2975857491953324,
+            "timestamp": 0.7258420817395592
+        },
+        {
+            "x": 6.055822077771958,
+            "y": 7.05903034533374,
+            "heading": 2.8074611577219364,
+            "angularVelocity": 2.5810029119060704e-7,
+            "velocityX": 3.6678680291727868,
+            "velocityY": 0.9259499998712609,
+            "timestamp": 0.8295338077023534
+        },
+        {
+            "x": 6.436149809853972,
+            "y": 7.155043043263379,
+            "heading": 2.807461184484807,
+            "angularVelocity": 2.58100350606906e-7,
+            "velocityX": 3.6678696255714556,
+            "velocityY": 0.9259436762012891,
+            "timestamp": 0.9332255336651476
+        },
+        {
+            "x": 6.816477543748484,
+            "y": 7.2510557340128905,
+            "heading": 2.8074612112477313,
+            "angularVelocity": 2.581008643735431e-7,
+            "velocityX": 3.667869643051154,
+            "velocityY": 0.9259436069563147,
+            "timestamp": 1.0369172596279417
+        },
+        {
+            "x": 7.162445200777026,
+            "y": 7.337344185673276,
+            "heading": 2.898451236968193,
+            "angularVelocity": 0.8775051709825819,
+            "velocityX": 3.3365020575767024,
+            "velocityY": 0.8321633270078459,
+            "timestamp": 1.1406089855907358
+        },
+        {
+            "x": 7.421534649307807,
+            "y": 7.402207433353085,
+            "heading": 3.009161772576331,
+            "angularVelocity": 1.0676891968008626,
+            "velocityX": 2.498651132721501,
+            "velocityY": 0.6255392807626672,
+            "timestamp": 1.2443007115535298
+        },
+        {
+            "x": 7.593924390045372,
+            "y": 7.445415282758517,
+            "heading": 3.094199643650422,
+            "angularVelocity": 0.8201027640778757,
+            "velocityX": 1.6625216634876054,
+            "velocityY": 0.416695247419558,
+            "timestamp": 1.347992437516324
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 0.4416973092532584,
+            "velocityX": 0.8301106877660893,
+            "velocityY": 0.20816238751031255,
+            "timestamp": 1.451684163479118
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -4.6860665144292015e-31,
+            "velocityY": 3.7247780913146357e-28,
+            "timestamp": 1.555375889441912
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.1.traj
new file mode 100644
index 00000000..d6c47c76
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -3.269880825493717e-29,
+            "velocityX": -1.6106277248271234e-30,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.257789422547788,
+            "y": 5.465225005289041,
+            "heading": 3.082306365857339,
+            "angularVelocity": -0.5369837298016524,
+            "velocityX": -0.04849747041052485,
+            "velocityY": -0.882118641130872,
+            "timestamp": 0.1074401903461163
+        },
+        {
+            "x": 4.273795168733638,
+            "y": 5.277874820636679,
+            "heading": 2.968634839381067,
+            "angularVelocity": -1.057998185875108,
+            "velocityX": 0.14897354643815672,
+            "velocityY": -1.7437625905987038,
+            "timestamp": 0.2148803806922326
+        },
+        {
+            "x": 4.354522429535891,
+            "y": 5.020377016639425,
+            "heading": 2.810559381522931,
+            "angularVelocity": -1.471287954245977,
+            "velocityX": 0.7513693017686587,
+            "velocityY": -2.396661837322988,
+            "timestamp": 0.3223205710383489
+        },
+        {
+            "x": 4.529578110420142,
+            "y": 4.7442561245273325,
+            "heading": 2.6367348361471494,
+            "angularVelocity": -1.6178726491066266,
+            "velocityX": 1.6293314477600391,
+            "velocityY": -2.5699963042002807,
+            "timestamp": 0.4297607613844652
+        },
+        {
+            "x": 4.793919720832893,
+            "y": 4.487710124588879,
+            "heading": 2.5690751936264027,
+            "angularVelocity": -0.6297423925143462,
+            "velocityX": 2.4603605928208117,
+            "velocityY": -2.3878029172509625,
+            "timestamp": 0.5372009517305815
+        },
+        {
+            "x": 5.141097516271185,
+            "y": 4.276379117165763,
+            "heading": 2.5690757140293337,
+            "angularVelocity": 0.000004843652357933311,
+            "velocityX": 3.231358715205786,
+            "velocityY": -1.9669641941467053,
+            "timestamp": 0.6446411420766978
+        },
+        {
+            "x": 5.528234411762885,
+            "y": 4.152611717586108,
+            "heading": 2.5690757988249406,
+            "angularVelocity": 7.892354497964203e-7,
+            "velocityX": 3.6032781982659046,
+            "velocityY": -1.1519655650361502,
+            "timestamp": 0.752081332422814
+        },
+        {
+            "x": 5.933619437652518,
+            "y": 4.123348539284839,
+            "heading": 2.569075852374573,
+            "angularVelocity": 4.984134150731162e-7,
+            "velocityX": 3.773122744698165,
+            "velocityY": -0.2723671487085231,
+            "timestamp": 0.8595215227689303
+        },
+        {
+            "x": 6.340054515622032,
+            "y": 4.125318524745646,
+            "heading": 2.569075903811562,
+            "angularVelocity": 4.787499761820812e-7,
+            "velocityX": 3.782896108618124,
+            "velocityY": 0.01833564753065316,
+            "timestamp": 0.9669617131150466
+        },
+        {
+            "x": 6.746489593269827,
+            "y": 4.127288577706153,
+            "heading": 2.5690759552486404,
+            "angularVelocity": 4.787508167496477e-7,
+            "velocityX": 3.7828961056237245,
+            "velocityY": 0.01833627578441685,
+            "timestamp": 1.074401903461163
+        },
+        {
+            "x": 7.118708956476355,
+            "y": 4.125866161972181,
+            "heading": 2.656486209298221,
+            "angularVelocity": 0.8135712880626333,
+            "velocityX": 3.464433207046916,
+            "velocityY": -0.013239140115027938,
+            "timestamp": 1.1818420938072793
+        },
+        {
+            "x": 7.400847427365447,
+            "y": 4.126314118090732,
+            "heading": 2.8696985577774003,
+            "angularVelocity": 1.9844747835268808,
+            "velocityX": 2.6260049426586924,
+            "velocityY": 0.004169353359354839,
+            "timestamp": 1.2892822841533955
+        },
+        {
+            "x": 7.587357907068713,
+            "y": 4.126749409200305,
+            "heading": 3.0436879317659473,
+            "angularVelocity": 1.6194067920769941,
+            "velocityX": 1.7359470334371772,
+            "velocityY": 0.004051473737811161,
+            "timestamp": 1.3967224744995117
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.8964249590751834,
+            "velocityX": 0.8622666493129096,
+            "velocityY": 0.002332374867239492,
+            "timestamp": 1.5041626648456279
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 4.233852791919794e-30,
+            "velocityY": -1.3542182850561966e-29,
+            "timestamp": 1.611602855191744
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.traj
new file mode 100644
index 00000000..d6c47c76
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -3.269880825493717e-29,
+            "velocityX": -1.6106277248271234e-30,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.257789422547788,
+            "y": 5.465225005289041,
+            "heading": 3.082306365857339,
+            "angularVelocity": -0.5369837298016524,
+            "velocityX": -0.04849747041052485,
+            "velocityY": -0.882118641130872,
+            "timestamp": 0.1074401903461163
+        },
+        {
+            "x": 4.273795168733638,
+            "y": 5.277874820636679,
+            "heading": 2.968634839381067,
+            "angularVelocity": -1.057998185875108,
+            "velocityX": 0.14897354643815672,
+            "velocityY": -1.7437625905987038,
+            "timestamp": 0.2148803806922326
+        },
+        {
+            "x": 4.354522429535891,
+            "y": 5.020377016639425,
+            "heading": 2.810559381522931,
+            "angularVelocity": -1.471287954245977,
+            "velocityX": 0.7513693017686587,
+            "velocityY": -2.396661837322988,
+            "timestamp": 0.3223205710383489
+        },
+        {
+            "x": 4.529578110420142,
+            "y": 4.7442561245273325,
+            "heading": 2.6367348361471494,
+            "angularVelocity": -1.6178726491066266,
+            "velocityX": 1.6293314477600391,
+            "velocityY": -2.5699963042002807,
+            "timestamp": 0.4297607613844652
+        },
+        {
+            "x": 4.793919720832893,
+            "y": 4.487710124588879,
+            "heading": 2.5690751936264027,
+            "angularVelocity": -0.6297423925143462,
+            "velocityX": 2.4603605928208117,
+            "velocityY": -2.3878029172509625,
+            "timestamp": 0.5372009517305815
+        },
+        {
+            "x": 5.141097516271185,
+            "y": 4.276379117165763,
+            "heading": 2.5690757140293337,
+            "angularVelocity": 0.000004843652357933311,
+            "velocityX": 3.231358715205786,
+            "velocityY": -1.9669641941467053,
+            "timestamp": 0.6446411420766978
+        },
+        {
+            "x": 5.528234411762885,
+            "y": 4.152611717586108,
+            "heading": 2.5690757988249406,
+            "angularVelocity": 7.892354497964203e-7,
+            "velocityX": 3.6032781982659046,
+            "velocityY": -1.1519655650361502,
+            "timestamp": 0.752081332422814
+        },
+        {
+            "x": 5.933619437652518,
+            "y": 4.123348539284839,
+            "heading": 2.569075852374573,
+            "angularVelocity": 4.984134150731162e-7,
+            "velocityX": 3.773122744698165,
+            "velocityY": -0.2723671487085231,
+            "timestamp": 0.8595215227689303
+        },
+        {
+            "x": 6.340054515622032,
+            "y": 4.125318524745646,
+            "heading": 2.569075903811562,
+            "angularVelocity": 4.787499761820812e-7,
+            "velocityX": 3.782896108618124,
+            "velocityY": 0.01833564753065316,
+            "timestamp": 0.9669617131150466
+        },
+        {
+            "x": 6.746489593269827,
+            "y": 4.127288577706153,
+            "heading": 2.5690759552486404,
+            "angularVelocity": 4.787508167496477e-7,
+            "velocityX": 3.7828961056237245,
+            "velocityY": 0.01833627578441685,
+            "timestamp": 1.074401903461163
+        },
+        {
+            "x": 7.118708956476355,
+            "y": 4.125866161972181,
+            "heading": 2.656486209298221,
+            "angularVelocity": 0.8135712880626333,
+            "velocityX": 3.464433207046916,
+            "velocityY": -0.013239140115027938,
+            "timestamp": 1.1818420938072793
+        },
+        {
+            "x": 7.400847427365447,
+            "y": 4.126314118090732,
+            "heading": 2.8696985577774003,
+            "angularVelocity": 1.9844747835268808,
+            "velocityX": 2.6260049426586924,
+            "velocityY": 0.004169353359354839,
+            "timestamp": 1.2892822841533955
+        },
+        {
+            "x": 7.587357907068713,
+            "y": 4.126749409200305,
+            "heading": 3.0436879317659473,
+            "angularVelocity": 1.6194067920769941,
+            "velocityX": 1.7359470334371772,
+            "velocityY": 0.004051473737811161,
+            "timestamp": 1.3967224744995117
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.8964249590751834,
+            "velocityX": 0.8622666493129096,
+            "velocityY": 0.002332374867239492,
+            "timestamp": 1.5041626648456279
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 4.233852791919794e-30,
+            "velocityY": -1.3542182850561966e-29,
+            "timestamp": 1.611602855191744
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj
new file mode 100644
index 00000000..84804d4a
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -7.11323283877368e-33,
+            "velocityX": -2.26183607094562e-34,
+            "velocityY": 8.143994372388985e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 4.215224482174133,
+            "y": 5.589131482897013,
+            "heading": 3.153116118672446,
+            "angularVelocity": 0.15993604804856262,
+            "velocityX": -0.5825677325255438,
+            "velocityY": 0.35522507570249656,
+            "timestamp": 0.0820085204835332
+        },
+        {
+            "x": 4.119673447940142,
+            "y": 5.647394549238196,
+            "heading": 3.179344081068732,
+            "angularVelocity": 0.3198199679940867,
+            "velocityX": -1.1651354477633493,
+            "velocityY": 0.7104513774624363,
+            "timestamp": 0.1640170409670664
+        },
+        {
+            "x": 3.976346906814042,
+            "y": 5.734789369483009,
+            "heading": 3.218677503841872,
+            "angularVelocity": 0.47962605033262895,
+            "velocityX": -1.7477030469642283,
+            "velocityY": 1.0656797577803132,
+            "timestamp": 0.24602556145059962
+        },
+        {
+            "x": 3.785244904014577,
+            "y": 5.851316186750812,
+            "heading": 3.271109373786434,
+            "angularVelocity": 0.6393466146617031,
+            "velocityX": -2.3302700947743262,
+            "velocityY": 1.4209111026603802,
+            "timestamp": 0.3280340819341328
+        },
+        {
+            "x": 3.5463675589434582,
+            "y": 5.996975305481239,
+            "heading": 3.3366342258406214,
+            "angularVelocity": 0.7990005388201682,
+            "velocityX": -2.9128356866172687,
+            "velocityY": 1.7761461598331643,
+            "timestamp": 0.410042602417666
+        },
+        {
+            "x": 3.2814945861155147,
+            "y": 6.158490139171684,
+            "heading": 3.3366342341249666,
+            "angularVelocity": 1.010181030485091e-7,
+            "velocityX": -3.2298225997276515,
+            "velocityY": 1.9694884475190098,
+            "timestamp": 0.4920511229011992
+        },
+        {
+            "x": 3.0166215989993184,
+            "y": 6.32000494943038,
+            "heading": 3.336634242409321,
+            "angularVelocity": 1.0101821933668142e-7,
+            "velocityX": -3.2298227739565295,
+            "velocityY": 1.969488161795665,
+            "timestamp": 0.5740596433847324
+        },
+        {
+            "x": 2.7777475013239767,
+            "y": 6.46566975957478,
+            "heading": 3.402143020545178,
+            "angularVelocity": 0.7988045357922341,
+            "velocityX": -2.9127960883443285,
+            "velocityY": 1.776215560109378,
+            "timestamp": 0.6560681638682656
+        },
+        {
+            "x": 2.5866484088099964,
+            "y": 6.582201778902395,
+            "heading": 3.4545564874050534,
+            "angularVelocity": 0.6391222101171739,
+            "velocityX": -2.3302346071753846,
+            "velocityY": 1.4209745358229475,
+            "timestamp": 0.7380766843517989
+        },
+        {
+            "x": 2.443324185159995,
+            "y": 6.669600869933244,
+            "heading": 3.493870163524806,
+            "angularVelocity": 0.47938526250630853,
+            "velocityX": -1.7476747879969323,
+            "velocityY": 1.0657318351255574,
+            "timestamp": 0.8200852048353321
+        },
+        {
+            "x": 2.347774731346008,
+            "y": 6.727866955511231,
+            "heading": 3.5200803945158796,
+            "angularVelocity": 0.3196037538116112,
+            "velocityX": -1.1651161763511244,
+            "velocityY": 0.710488193597954,
+            "timestamp": 0.9020937253188653
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 0.1597994036038963,
+            "velocityX": -0.5825581423042643,
+            "velocityY": 0.35524411752579316,
+            "timestamp": 0.9841022458023986
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 0,
+            "velocityX": -5.266786892384168e-34,
+            "velocityY": -2.2308589105860002e-32,
+            "timestamp": 1.0661107662859317
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.traj
new file mode 100644
index 00000000..84804d4a
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -7.11323283877368e-33,
+            "velocityX": -2.26183607094562e-34,
+            "velocityY": 8.143994372388985e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 4.215224482174133,
+            "y": 5.589131482897013,
+            "heading": 3.153116118672446,
+            "angularVelocity": 0.15993604804856262,
+            "velocityX": -0.5825677325255438,
+            "velocityY": 0.35522507570249656,
+            "timestamp": 0.0820085204835332
+        },
+        {
+            "x": 4.119673447940142,
+            "y": 5.647394549238196,
+            "heading": 3.179344081068732,
+            "angularVelocity": 0.3198199679940867,
+            "velocityX": -1.1651354477633493,
+            "velocityY": 0.7104513774624363,
+            "timestamp": 0.1640170409670664
+        },
+        {
+            "x": 3.976346906814042,
+            "y": 5.734789369483009,
+            "heading": 3.218677503841872,
+            "angularVelocity": 0.47962605033262895,
+            "velocityX": -1.7477030469642283,
+            "velocityY": 1.0656797577803132,
+            "timestamp": 0.24602556145059962
+        },
+        {
+            "x": 3.785244904014577,
+            "y": 5.851316186750812,
+            "heading": 3.271109373786434,
+            "angularVelocity": 0.6393466146617031,
+            "velocityX": -2.3302700947743262,
+            "velocityY": 1.4209111026603802,
+            "timestamp": 0.3280340819341328
+        },
+        {
+            "x": 3.5463675589434582,
+            "y": 5.996975305481239,
+            "heading": 3.3366342258406214,
+            "angularVelocity": 0.7990005388201682,
+            "velocityX": -2.9128356866172687,
+            "velocityY": 1.7761461598331643,
+            "timestamp": 0.410042602417666
+        },
+        {
+            "x": 3.2814945861155147,
+            "y": 6.158490139171684,
+            "heading": 3.3366342341249666,
+            "angularVelocity": 1.010181030485091e-7,
+            "velocityX": -3.2298225997276515,
+            "velocityY": 1.9694884475190098,
+            "timestamp": 0.4920511229011992
+        },
+        {
+            "x": 3.0166215989993184,
+            "y": 6.32000494943038,
+            "heading": 3.336634242409321,
+            "angularVelocity": 1.0101821933668142e-7,
+            "velocityX": -3.2298227739565295,
+            "velocityY": 1.969488161795665,
+            "timestamp": 0.5740596433847324
+        },
+        {
+            "x": 2.7777475013239767,
+            "y": 6.46566975957478,
+            "heading": 3.402143020545178,
+            "angularVelocity": 0.7988045357922341,
+            "velocityX": -2.9127960883443285,
+            "velocityY": 1.776215560109378,
+            "timestamp": 0.6560681638682656
+        },
+        {
+            "x": 2.5866484088099964,
+            "y": 6.582201778902395,
+            "heading": 3.4545564874050534,
+            "angularVelocity": 0.6391222101171739,
+            "velocityX": -2.3302346071753846,
+            "velocityY": 1.4209745358229475,
+            "timestamp": 0.7380766843517989
+        },
+        {
+            "x": 2.443324185159995,
+            "y": 6.669600869933244,
+            "heading": 3.493870163524806,
+            "angularVelocity": 0.47938526250630853,
+            "velocityX": -1.7476747879969323,
+            "velocityY": 1.0657318351255574,
+            "timestamp": 0.8200852048353321
+        },
+        {
+            "x": 2.347774731346008,
+            "y": 6.727866955511231,
+            "heading": 3.5200803945158796,
+            "angularVelocity": 0.3196037538116112,
+            "velocityX": -1.1651161763511244,
+            "velocityY": 0.710488193597954,
+            "timestamp": 0.9020937253188653
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 0.1597994036038963,
+            "velocityX": -0.5825581423042643,
+            "velocityY": 0.35524411752579316,
+            "timestamp": 0.9841022458023986
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 0,
+            "velocityX": -5.266786892384168e-34,
+            "velocityY": -2.2308589105860002e-32,
+            "timestamp": 1.0661107662859317
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj
new file mode 100644
index 00000000..b9cb5044
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 9.713847301484863e-36,
+            "velocityY": -1.0645735433948216e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 4.181096344253042,
+            "y": 5.560000047628424,
+            "heading": 3.139999989014019,
+            "angularVelocity": -1.10846415392014e-7,
+            "velocityX": -0.826391981328299,
+            "velocityY": 4.805615505681296e-7,
+            "timestamp": 0.09910993523353247
+        },
+        {
+            "x": 4.017289035836252,
+            "y": 5.560000133123364,
+            "heading": 3.139999981407119,
+            "angularVelocity": -7.675215101799687e-8,
+            "velocityX": -1.6527839316089914,
+            "velocityY": 8.626273272073532e-7,
+            "timestamp": 0.19821987046706493
+        },
+        {
+            "x": 3.7715780809038826,
+            "y": 5.560000246490977,
+            "heading": 3.1399999920859667,
+            "angularVelocity": 1.0774748441307597e-7,
+            "velocityX": -2.4791758197944644,
+            "velocityY": 0.0000011438571635435649,
+            "timestamp": 0.2973298057005974
+        },
+        {
+            "x": 3.4439634979186673,
+            "y": 5.56000037760455,
+            "heading": 3.1400000352544852,
+            "angularVelocity": 4.355619377078705e-7,
+            "velocityX": -3.305567521694553,
+            "velocityY": 0.0000013229104502120412,
+            "timestamp": 0.39643974093412987
+        },
+        {
+            "x": 3.069036502081214,
+            "y": 5.560000367749523,
+            "heading": 3.140000035254493,
+            "angularVelocity": 8.147365661806906e-14,
+            "velocityX": -3.782940579610041,
+            "velocityY": -9.943529898465916e-8,
+            "timestamp": 0.4955496761676623
+        },
+        {
+            "x": 2.741421919096052,
+            "y": 5.560000236043854,
+            "heading": 3.1400000358317235,
+            "angularVelocity": 5.824165114605996e-9,
+            "velocityX": -3.305567521694012,
+            "velocityY": -0.0000013288845794272055,
+            "timestamp": 0.5946596114011948
+        },
+        {
+            "x": 2.4957109641637163,
+            "y": 5.560000125368319,
+            "heading": 3.1400000237983328,
+            "angularVelocity": -1.214145560764502e-7,
+            "velocityX": -2.479175819794127,
+            "velocityY": -0.0000011166946380844406,
+            "timestamp": 0.6937695466347273
+        },
+        {
+            "x": 2.3319036557469475,
+            "y": 5.560000044140379,
+            "heading": 3.140000009519778,
+            "angularVelocity": -1.4406783354118398e-7,
+            "velocityX": -1.6527839316087705,
+            "velocityY": -8.195741267361974e-7,
+            "timestamp": 0.7928794818682598
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -9.605270403416775e-8,
+            "velocityX": -0.8263919813281883,
+            "velocityY": -4.4536784825580755e-7,
+            "timestamp": 0.8919894171017924
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 4.934081445690025e-38,
+            "velocityY": -9.331512134237052e-34,
+            "timestamp": 0.9910993523353249
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.traj
new file mode 100644
index 00000000..b9cb5044
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 9.713847301484863e-36,
+            "velocityY": -1.0645735433948216e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 4.181096344253042,
+            "y": 5.560000047628424,
+            "heading": 3.139999989014019,
+            "angularVelocity": -1.10846415392014e-7,
+            "velocityX": -0.826391981328299,
+            "velocityY": 4.805615505681296e-7,
+            "timestamp": 0.09910993523353247
+        },
+        {
+            "x": 4.017289035836252,
+            "y": 5.560000133123364,
+            "heading": 3.139999981407119,
+            "angularVelocity": -7.675215101799687e-8,
+            "velocityX": -1.6527839316089914,
+            "velocityY": 8.626273272073532e-7,
+            "timestamp": 0.19821987046706493
+        },
+        {
+            "x": 3.7715780809038826,
+            "y": 5.560000246490977,
+            "heading": 3.1399999920859667,
+            "angularVelocity": 1.0774748441307597e-7,
+            "velocityX": -2.4791758197944644,
+            "velocityY": 0.0000011438571635435649,
+            "timestamp": 0.2973298057005974
+        },
+        {
+            "x": 3.4439634979186673,
+            "y": 5.56000037760455,
+            "heading": 3.1400000352544852,
+            "angularVelocity": 4.355619377078705e-7,
+            "velocityX": -3.305567521694553,
+            "velocityY": 0.0000013229104502120412,
+            "timestamp": 0.39643974093412987
+        },
+        {
+            "x": 3.069036502081214,
+            "y": 5.560000367749523,
+            "heading": 3.140000035254493,
+            "angularVelocity": 8.147365661806906e-14,
+            "velocityX": -3.782940579610041,
+            "velocityY": -9.943529898465916e-8,
+            "timestamp": 0.4955496761676623
+        },
+        {
+            "x": 2.741421919096052,
+            "y": 5.560000236043854,
+            "heading": 3.1400000358317235,
+            "angularVelocity": 5.824165114605996e-9,
+            "velocityX": -3.305567521694012,
+            "velocityY": -0.0000013288845794272055,
+            "timestamp": 0.5946596114011948
+        },
+        {
+            "x": 2.4957109641637163,
+            "y": 5.560000125368319,
+            "heading": 3.1400000237983328,
+            "angularVelocity": -1.214145560764502e-7,
+            "velocityX": -2.479175819794127,
+            "velocityY": -0.0000011166946380844406,
+            "timestamp": 0.6937695466347273
+        },
+        {
+            "x": 2.3319036557469475,
+            "y": 5.560000044140379,
+            "heading": 3.140000009519778,
+            "angularVelocity": -1.4406783354118398e-7,
+            "velocityX": -1.6527839316087705,
+            "velocityY": -8.195741267361974e-7,
+            "timestamp": 0.7928794818682598
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -9.605270403416775e-8,
+            "velocityX": -0.8263919813281883,
+            "velocityY": -4.4536784825580755e-7,
+            "timestamp": 0.8919894171017924
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 4.934081445690025e-38,
+            "velocityY": -9.331512134237052e-34,
+            "timestamp": 0.9910993523353249
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj
new file mode 100644
index 00000000..ee5c9ea2
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 1.4062915610922596e-31,
+            "velocityX": -1.6151999437241794e-32,
+            "velocityY": -1.528021229010722e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 4.206513702340257,
+            "y": 5.554388398990967,
+            "heading": 3.0326218488137218,
+            "angularVelocity": -1.231959178646129,
+            "velocityX": -0.6480723694798501,
+            "velocityY": -0.0643824026918128,
+            "timestamp": 0.0871604782426985
+        },
+        {
+            "x": 4.0911807616230185,
+            "y": 5.5411300283093805,
+            "heading": 2.844661289825114,
+            "angularVelocity": -2.156488385311873,
+            "velocityX": -1.323225193832629,
+            "velocityY": -0.15211447836104172,
+            "timestamp": 0.174320956485397
+        },
+        {
+            "x": 3.915187082588679,
+            "y": 5.516242186917599,
+            "heading": 2.6068500185118766,
+            "angularVelocity": -2.728430087901214,
+            "velocityX": -2.0191912961317713,
+            "velocityY": -0.2855404409608899,
+            "timestamp": 0.2614814347280955
+        },
+        {
+            "x": 3.678761658477574,
+            "y": 5.47299330729491,
+            "heading": 2.386294579571215,
+            "angularVelocity": -2.530452372307143,
+            "velocityX": -2.7125301384049063,
+            "velocityY": -0.4961982826925549,
+            "timestamp": 0.348641912970794
+        },
+        {
+            "x": 3.3968919941686,
+            "y": 5.398640264151657,
+            "heading": 2.288226309782543,
+            "angularVelocity": -1.1251460726913511,
+            "velocityX": -3.233915990273797,
+            "velocityY": -0.8530591460984852,
+            "timestamp": 0.4358023912134925
+        },
+        {
+            "x": 3.0985878200503763,
+            "y": 5.268158980231371,
+            "heading": 2.276780497851342,
+            "angularVelocity": -0.1313188289230101,
+            "velocityX": -3.4224705982864747,
+            "velocityY": -1.4970234967844136,
+            "timestamp": 0.522962869456191
+        },
+        {
+            "x": 2.8320780769343714,
+            "y": 5.083465560010488,
+            "heading": 2.2937556690853027,
+            "angularVelocity": 0.19475766512768708,
+            "velocityX": -3.0576902340290975,
+            "velocityY": -2.11900420861165,
+            "timestamp": 0.6101233476988894
+        },
+        {
+            "x": 2.620143221340131,
+            "y": 4.878958491159276,
+            "heading": 2.4039835676400654,
+            "angularVelocity": 1.264654586311841,
+            "velocityX": -2.4315476448409066,
+            "velocityY": -2.34632798000215,
+            "timestamp": 0.6972838259415879
+        },
+        {
+            "x": 2.469192165605932,
+            "y": 4.691124569407488,
+            "heading": 2.528247411458178,
+            "angularVelocity": 1.42569024773017,
+            "velocityX": -1.7318750284260176,
+            "velocityY": -2.155035464912952,
+            "timestamp": 0.7844443041842863
+        },
+        {
+            "x": 2.3748255822885,
+            "y": 4.531333386407196,
+            "heading": 2.6349251655798476,
+            "angularVelocity": 1.2239234601791131,
+            "velocityX": -1.0826762911358505,
+            "velocityY": -1.833298603013082,
+            "timestamp": 0.8716047824269848
+        },
+        {
+            "x": 2.3223557063303257,
+            "y": 4.418269595148307,
+            "heading": 2.710801114054306,
+            "angularVelocity": 0.8705315758270817,
+            "velocityX": -0.6019916023415092,
+            "velocityY": -1.2971910381682688,
+            "timestamp": 0.9587652606696833
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0.44973234126302364,
+            "velocityX": -0.2564890278375538,
+            "velocityY": -0.6685323018312901,
+            "timestamp": 1.0459257389123817
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 6.1103463204495e-32,
+            "velocityY": 1.8635567714760387e-30,
+            "timestamp": 1.1330862171550802
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.traj
new file mode 100644
index 00000000..ee5c9ea2
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 1.4062915610922596e-31,
+            "velocityX": -1.6151999437241794e-32,
+            "velocityY": -1.528021229010722e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 4.206513702340257,
+            "y": 5.554388398990967,
+            "heading": 3.0326218488137218,
+            "angularVelocity": -1.231959178646129,
+            "velocityX": -0.6480723694798501,
+            "velocityY": -0.0643824026918128,
+            "timestamp": 0.0871604782426985
+        },
+        {
+            "x": 4.0911807616230185,
+            "y": 5.5411300283093805,
+            "heading": 2.844661289825114,
+            "angularVelocity": -2.156488385311873,
+            "velocityX": -1.323225193832629,
+            "velocityY": -0.15211447836104172,
+            "timestamp": 0.174320956485397
+        },
+        {
+            "x": 3.915187082588679,
+            "y": 5.516242186917599,
+            "heading": 2.6068500185118766,
+            "angularVelocity": -2.728430087901214,
+            "velocityX": -2.0191912961317713,
+            "velocityY": -0.2855404409608899,
+            "timestamp": 0.2614814347280955
+        },
+        {
+            "x": 3.678761658477574,
+            "y": 5.47299330729491,
+            "heading": 2.386294579571215,
+            "angularVelocity": -2.530452372307143,
+            "velocityX": -2.7125301384049063,
+            "velocityY": -0.4961982826925549,
+            "timestamp": 0.348641912970794
+        },
+        {
+            "x": 3.3968919941686,
+            "y": 5.398640264151657,
+            "heading": 2.288226309782543,
+            "angularVelocity": -1.1251460726913511,
+            "velocityX": -3.233915990273797,
+            "velocityY": -0.8530591460984852,
+            "timestamp": 0.4358023912134925
+        },
+        {
+            "x": 3.0985878200503763,
+            "y": 5.268158980231371,
+            "heading": 2.276780497851342,
+            "angularVelocity": -0.1313188289230101,
+            "velocityX": -3.4224705982864747,
+            "velocityY": -1.4970234967844136,
+            "timestamp": 0.522962869456191
+        },
+        {
+            "x": 2.8320780769343714,
+            "y": 5.083465560010488,
+            "heading": 2.2937556690853027,
+            "angularVelocity": 0.19475766512768708,
+            "velocityX": -3.0576902340290975,
+            "velocityY": -2.11900420861165,
+            "timestamp": 0.6101233476988894
+        },
+        {
+            "x": 2.620143221340131,
+            "y": 4.878958491159276,
+            "heading": 2.4039835676400654,
+            "angularVelocity": 1.264654586311841,
+            "velocityX": -2.4315476448409066,
+            "velocityY": -2.34632798000215,
+            "timestamp": 0.6972838259415879
+        },
+        {
+            "x": 2.469192165605932,
+            "y": 4.691124569407488,
+            "heading": 2.528247411458178,
+            "angularVelocity": 1.42569024773017,
+            "velocityX": -1.7318750284260176,
+            "velocityY": -2.155035464912952,
+            "timestamp": 0.7844443041842863
+        },
+        {
+            "x": 2.3748255822885,
+            "y": 4.531333386407196,
+            "heading": 2.6349251655798476,
+            "angularVelocity": 1.2239234601791131,
+            "velocityX": -1.0826762911358505,
+            "velocityY": -1.833298603013082,
+            "timestamp": 0.8716047824269848
+        },
+        {
+            "x": 2.3223557063303257,
+            "y": 4.418269595148307,
+            "heading": 2.710801114054306,
+            "angularVelocity": 0.8705315758270817,
+            "velocityX": -0.6019916023415092,
+            "velocityY": -1.2971910381682688,
+            "timestamp": 0.9587652606696833
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0.44973234126302364,
+            "velocityX": -0.2564890278375538,
+            "velocityY": -0.6685323018312901,
+            "timestamp": 1.0459257389123817
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 6.1103463204495e-32,
+            "velocityY": 1.8635567714760387e-30,
+            "timestamp": 1.1330862171550802
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.1.traj
new file mode 100644
index 00000000..8d9b9558
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.1.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 1.5794772183943222e-24,
+            "velocityX": -1.3909195862275997e-25,
+            "velocityY": -1.589027573571451e-24,
+            "timestamp": 0
+        },
+        {
+            "x": 4.294802050577596,
+            "y": 3.0477360619772367,
+            "heading": 2.544164914123716,
+            "angularVelocity": -0.3578657180774014,
+            "velocityX": 0.38145905506951994,
+            "velocityY": 0.5725842442186713,
+            "timestamp": 0.08336949970108862
+        },
+        {
+            "x": 4.359925633327058,
+            "y": 3.142202439962799,
+            "heading": 2.4853835413264154,
+            "angularVelocity": -0.7050704755102848,
+            "velocityX": 0.781143979308431,
+            "velocityY": 1.13310477241989,
+            "timestamp": 0.16673899940217723
+        },
+        {
+            "x": 4.461027910894954,
+            "y": 3.2814650300922756,
+            "heading": 2.399327847716391,
+            "angularVelocity": -1.0322203433937678,
+            "velocityX": 1.2127010229206814,
+            "velocityY": 1.6704261226082242,
+            "timestamp": 0.2501084991032658
+        },
+        {
+            "x": 4.603798889123446,
+            "y": 3.46048671868814,
+            "heading": 2.2901235890618308,
+            "angularVelocity": -1.309882619496323,
+            "velocityX": 1.7125085161885507,
+            "velocityY": 2.147328330357326,
+            "timestamp": 0.33347799880435447
+        },
+        {
+            "x": 4.803010081194681,
+            "y": 3.6526247699506835,
+            "heading": 2.177140187929377,
+            "angularVelocity": -1.3552126561577384,
+            "velocityX": 2.389497271609915,
+            "velocityY": 2.3046564025384755,
+            "timestamp": 0.4168474985054431
+        },
+        {
+            "x": 5.0510307536076,
+            "y": 3.8467573295012585,
+            "heading": 2.175769825428097,
+            "angularVelocity": -0.016437216322438507,
+            "velocityX": 2.9749569483104428,
+            "velocityY": 2.328580119187639,
+            "timestamp": 0.5002169982065318
+        },
+        {
+            "x": 5.320458962981737,
+            "y": 4.010690590396415,
+            "heading": 2.175769541029465,
+            "angularVelocity": -0.000003411303085546954,
+            "velocityX": 3.231735950679083,
+            "velocityY": 1.9663457437422465,
+            "timestamp": 0.5835864979076204
+        },
+        {
+            "x": 5.614141443438261,
+            "y": 4.125652797356739,
+            "heading": 2.175769455965937,
+            "angularVelocity": -0.0000010203195273976758,
+            "velocityX": 3.522660943264475,
+            "velocityY": 1.3789480250272361,
+            "timestamp": 0.666955997608709
+        },
+        {
+            "x": 5.906454716700063,
+            "y": 4.244053209777629,
+            "heading": 2.1757695445063954,
+            "angularVelocity": 0.000001062024591118681,
+            "velocityX": 3.506237584606561,
+            "velocityY": 1.4201885922957453,
+            "timestamp": 0.7503254973097977
+        },
+        {
+            "x": 6.172161728896083,
+            "y": 4.41393857041553,
+            "heading": 2.1757932909481013,
+            "angularVelocity": 0.00028483368367179416,
+            "velocityX": 3.187100955969282,
+            "velocityY": 2.0377399558232225,
+            "timestamp": 0.8336949970108863
+        },
+        {
+            "x": 6.426803794432408,
+            "y": 4.599225132728127,
+            "heading": 2.1773610490246513,
+            "angularVelocity": 0.018804935643986023,
+            "velocityX": 3.0543791968203453,
+            "velocityY": 2.2224742019194084,
+            "timestamp": 0.917064496711975
+        },
+        {
+            "x": 6.681572955411179,
+            "y": 4.785089288083454,
+            "heading": 2.177430344851842,
+            "angularVelocity": 0.0008311891931529928,
+            "velocityX": 3.0559036805092323,
+            "velocityY": 2.2294023116574087,
+            "timestamp": 1.0004339964130635
+        },
+        {
+            "x": 6.9013556581674305,
+            "y": 5.011277814262196,
+            "heading": 2.1774305498100937,
+            "angularVelocity": 0.000002458432073886186,
+            "velocityX": 2.636248310764209,
+            "velocityY": 2.7130848450538085,
+            "timestamp": 1.083803496114152
+        },
+        {
+            "x": 7.121935686700776,
+            "y": 5.230936165800854,
+            "heading": 2.189684453697612,
+            "angularVelocity": 0.14698305653090518,
+            "velocityX": 2.645812069452359,
+            "velocityY": 2.634756743487918,
+            "timestamp": 1.1671729958152406
+        },
+        {
+            "x": 7.311158558493755,
+            "y": 5.417630564055526,
+            "heading": 2.349466738853841,
+            "angularVelocity": 1.9165556435999858,
+            "velocityX": 2.2696894244467765,
+            "velocityY": 2.239360904455968,
+            "timestamp": 1.2505424955163291
+        },
+        {
+            "x": 7.462052071844248,
+            "y": 5.571824642162805,
+            "heading": 2.618524466568701,
+            "angularVelocity": 3.2272920993832837,
+            "velocityX": 1.8099366541901185,
+            "velocityY": 1.8495262495291969,
+            "timestamp": 1.3339119952174177
+        },
+        {
+            "x": 7.571973798709058,
+            "y": 5.685339226077728,
+            "heading": 2.8696843432959835,
+            "angularVelocity": 3.0126110583341195,
+            "velocityX": 1.3184885030967077,
+            "velocityY": 1.3615840843703815,
+            "timestamp": 1.4172814949185062
+        },
+        {
+            "x": 7.644303858666078,
+            "y": 5.759924476296826,
+            "heading": 3.0477755113688096,
+            "angularVelocity": 2.1361669280894184,
+            "velocityX": 0.8675841910572869,
+            "velocityY": 0.8946347343634476,
+            "timestamp": 1.5006509946195947
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 1.1062137707656932,
+            "velocityX": 0.4281678726861332,
+            "velocityY": 0.44471328046952424,
+            "timestamp": 1.5840204943206833
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 1.4380080051319566e-21,
+            "velocityX": 0,
+            "velocityY": 1.143279687808587e-23,
+            "timestamp": 1.6673899940217718
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.traj
new file mode 100644
index 00000000..8d9b9558
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 1.5794772183943222e-24,
+            "velocityX": -1.3909195862275997e-25,
+            "velocityY": -1.589027573571451e-24,
+            "timestamp": 0
+        },
+        {
+            "x": 4.294802050577596,
+            "y": 3.0477360619772367,
+            "heading": 2.544164914123716,
+            "angularVelocity": -0.3578657180774014,
+            "velocityX": 0.38145905506951994,
+            "velocityY": 0.5725842442186713,
+            "timestamp": 0.08336949970108862
+        },
+        {
+            "x": 4.359925633327058,
+            "y": 3.142202439962799,
+            "heading": 2.4853835413264154,
+            "angularVelocity": -0.7050704755102848,
+            "velocityX": 0.781143979308431,
+            "velocityY": 1.13310477241989,
+            "timestamp": 0.16673899940217723
+        },
+        {
+            "x": 4.461027910894954,
+            "y": 3.2814650300922756,
+            "heading": 2.399327847716391,
+            "angularVelocity": -1.0322203433937678,
+            "velocityX": 1.2127010229206814,
+            "velocityY": 1.6704261226082242,
+            "timestamp": 0.2501084991032658
+        },
+        {
+            "x": 4.603798889123446,
+            "y": 3.46048671868814,
+            "heading": 2.2901235890618308,
+            "angularVelocity": -1.309882619496323,
+            "velocityX": 1.7125085161885507,
+            "velocityY": 2.147328330357326,
+            "timestamp": 0.33347799880435447
+        },
+        {
+            "x": 4.803010081194681,
+            "y": 3.6526247699506835,
+            "heading": 2.177140187929377,
+            "angularVelocity": -1.3552126561577384,
+            "velocityX": 2.389497271609915,
+            "velocityY": 2.3046564025384755,
+            "timestamp": 0.4168474985054431
+        },
+        {
+            "x": 5.0510307536076,
+            "y": 3.8467573295012585,
+            "heading": 2.175769825428097,
+            "angularVelocity": -0.016437216322438507,
+            "velocityX": 2.9749569483104428,
+            "velocityY": 2.328580119187639,
+            "timestamp": 0.5002169982065318
+        },
+        {
+            "x": 5.320458962981737,
+            "y": 4.010690590396415,
+            "heading": 2.175769541029465,
+            "angularVelocity": -0.000003411303085546954,
+            "velocityX": 3.231735950679083,
+            "velocityY": 1.9663457437422465,
+            "timestamp": 0.5835864979076204
+        },
+        {
+            "x": 5.614141443438261,
+            "y": 4.125652797356739,
+            "heading": 2.175769455965937,
+            "angularVelocity": -0.0000010203195273976758,
+            "velocityX": 3.522660943264475,
+            "velocityY": 1.3789480250272361,
+            "timestamp": 0.666955997608709
+        },
+        {
+            "x": 5.906454716700063,
+            "y": 4.244053209777629,
+            "heading": 2.1757695445063954,
+            "angularVelocity": 0.000001062024591118681,
+            "velocityX": 3.506237584606561,
+            "velocityY": 1.4201885922957453,
+            "timestamp": 0.7503254973097977
+        },
+        {
+            "x": 6.172161728896083,
+            "y": 4.41393857041553,
+            "heading": 2.1757932909481013,
+            "angularVelocity": 0.00028483368367179416,
+            "velocityX": 3.187100955969282,
+            "velocityY": 2.0377399558232225,
+            "timestamp": 0.8336949970108863
+        },
+        {
+            "x": 6.426803794432408,
+            "y": 4.599225132728127,
+            "heading": 2.1773610490246513,
+            "angularVelocity": 0.018804935643986023,
+            "velocityX": 3.0543791968203453,
+            "velocityY": 2.2224742019194084,
+            "timestamp": 0.917064496711975
+        },
+        {
+            "x": 6.681572955411179,
+            "y": 4.785089288083454,
+            "heading": 2.177430344851842,
+            "angularVelocity": 0.0008311891931529928,
+            "velocityX": 3.0559036805092323,
+            "velocityY": 2.2294023116574087,
+            "timestamp": 1.0004339964130635
+        },
+        {
+            "x": 6.9013556581674305,
+            "y": 5.011277814262196,
+            "heading": 2.1774305498100937,
+            "angularVelocity": 0.000002458432073886186,
+            "velocityX": 2.636248310764209,
+            "velocityY": 2.7130848450538085,
+            "timestamp": 1.083803496114152
+        },
+        {
+            "x": 7.121935686700776,
+            "y": 5.230936165800854,
+            "heading": 2.189684453697612,
+            "angularVelocity": 0.14698305653090518,
+            "velocityX": 2.645812069452359,
+            "velocityY": 2.634756743487918,
+            "timestamp": 1.1671729958152406
+        },
+        {
+            "x": 7.311158558493755,
+            "y": 5.417630564055526,
+            "heading": 2.349466738853841,
+            "angularVelocity": 1.9165556435999858,
+            "velocityX": 2.2696894244467765,
+            "velocityY": 2.239360904455968,
+            "timestamp": 1.2505424955163291
+        },
+        {
+            "x": 7.462052071844248,
+            "y": 5.571824642162805,
+            "heading": 2.618524466568701,
+            "angularVelocity": 3.2272920993832837,
+            "velocityX": 1.8099366541901185,
+            "velocityY": 1.8495262495291969,
+            "timestamp": 1.3339119952174177
+        },
+        {
+            "x": 7.571973798709058,
+            "y": 5.685339226077728,
+            "heading": 2.8696843432959835,
+            "angularVelocity": 3.0126110583341195,
+            "velocityX": 1.3184885030967077,
+            "velocityY": 1.3615840843703815,
+            "timestamp": 1.4172814949185062
+        },
+        {
+            "x": 7.644303858666078,
+            "y": 5.759924476296826,
+            "heading": 3.0477755113688096,
+            "angularVelocity": 2.1361669280894184,
+            "velocityX": 0.8675841910572869,
+            "velocityY": 0.8946347343634476,
+            "timestamp": 1.5006509946195947
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 1.1062137707656932,
+            "velocityX": 0.4281678726861332,
+            "velocityY": 0.44471328046952424,
+            "timestamp": 1.5840204943206833
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 1.4380080051319566e-21,
+            "velocityX": 0,
+            "velocityY": 1.143279687808587e-23,
+            "timestamp": 1.6673899940217718
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.1.traj
new file mode 100644
index 00000000..0e209e38
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.1.traj	
@@ -0,0 +1,166 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -2.0886910556164006e-30,
+            "velocityX": -6.72399315077418e-33,
+            "velocityY": 1.0977042871870097e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 4.300222432415914,
+            "y": 3.053065889088155,
+            "heading": 2.5374285401398833,
+            "angularVelocity": -0.4122912034410164,
+            "velocityX": 0.41962999329145273,
+            "velocityY": 0.5982424370672392,
+            "timestamp": 0.08870298360694638
+        },
+        {
+            "x": 4.375819084559984,
+            "y": 3.158500650682638,
+            "heading": 2.466843265689978,
+            "angularVelocity": -0.7957485935611535,
+            "velocityX": 0.8522447506280879,
+            "velocityY": 1.1886270033675244,
+            "timestamp": 0.17740596721389276
+        },
+        {
+            "x": 4.491836015930185,
+            "y": 3.3149606373409832,
+            "heading": 2.3668866050755812,
+            "angularVelocity": -1.1268692049561326,
+            "velocityX": 1.307925919203419,
+            "velocityY": 1.7638638554892148,
+            "timestamp": 0.26610895082083913
+        },
+        {
+            "x": 4.652875991060298,
+            "y": 3.5188568125430604,
+            "heading": 2.248368974552935,
+            "angularVelocity": -1.3361177460254572,
+            "velocityX": 1.8154967125311139,
+            "velocityY": 2.2986394246394894,
+            "timestamp": 0.3548119344277855
+        },
+        {
+            "x": 4.870873285433555,
+            "y": 3.7498811407551353,
+            "heading": 2.196009009563727,
+            "angularVelocity": -0.5902841467116942,
+            "velocityX": 2.4576094907836294,
+            "velocityY": 2.604470772209555,
+            "timestamp": 0.4435149180347319
+        },
+        {
+            "x": 5.1428014530518755,
+            "y": 3.946488109616269,
+            "heading": 2.1960088759837526,
+            "angularVelocity": -0.0000015059242530161288,
+            "velocityX": 3.0656033941684258,
+            "velocityY": 2.21646398876866,
+            "timestamp": 0.5322179016416783
+        },
+        {
+            "x": 5.447787504833652,
+            "y": 4.086426434262152,
+            "heading": 2.1960087423061556,
+            "angularVelocity": -0.0000015070248085665752,
+            "velocityX": 3.4382840281135,
+            "velocityY": 1.577605610945025,
+            "timestamp": 0.6209208852486247
+        },
+        {
+            "x": 5.774173248105482,
+            "y": 4.1643468008186435,
+            "heading": 2.1960088209928723,
+            "angularVelocity": 8.870808373208906e-7,
+            "velocityX": 3.679535118210733,
+            "velocityY": 0.8784413261876967,
+            "timestamp": 0.709623868855571
+        },
+        {
+            "x": 6.1094823510079035,
+            "y": 4.1772711464289065,
+            "heading": 2.1960089213757183,
+            "angularVelocity": 0.0000011316738380089979,
+            "velocityX": 3.780133308572978,
+            "velocityY": 0.1457036176768579,
+            "timestamp": 0.7983268524625174
+        },
+        {
+            "x": 6.444914319541054,
+            "y": 4.1680717704970895,
+            "heading": 2.1960090177915097,
+            "angularVelocity": 0.000001086950938877831,
+            "velocityX": 3.78151844383826,
+            "velocityY": -0.10370988164931202,
+            "timestamp": 0.8870298360694638
+        },
+        {
+            "x": 6.780346283198341,
+            "y": 4.1588722167865155,
+            "heading": 2.196009114207664,
+            "angularVelocity": 0.0000010869550292397467,
+            "velocityX": 3.7815183888698267,
+            "velocityY": -0.10371188585197635,
+            "timestamp": 0.9757328196764102
+        },
+        {
+            "x": 7.07908088896125,
+            "y": 4.144071317525266,
+            "heading": 2.2852514937378463,
+            "angularVelocity": 1.0060809219859659,
+            "velocityX": 3.3678078641259517,
+            "velocityY": -0.16685909153668751,
+            "timestamp": 1.0644358032833565
+        },
+        {
+            "x": 7.324007491183461,
+            "y": 4.136590325759102,
+            "heading": 2.509537504382542,
+            "angularVelocity": 2.5285058238687177,
+            "velocityX": 2.7611991419308914,
+            "velocityY": -0.08433754381152235,
+            "timestamp": 1.153138786890303
+        },
+        {
+            "x": 7.505234177737953,
+            "y": 4.13149769076231,
+            "heading": 2.790057598116992,
+            "angularVelocity": 3.162465142970483,
+            "velocityX": 2.043073177307424,
+            "velocityY": -0.05741221760204461,
+            "timestamp": 1.2418417704972493
+        },
+        {
+            "x": 7.622663550770296,
+            "y": 4.128429038475573,
+            "heading": 3.015163183857666,
+            "angularVelocity": 2.5377453675982737,
+            "velocityX": 1.3238491903799634,
+            "velocityY": -0.03459469075285244,
+            "timestamp": 1.3305447541041957
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 1.4073575776830842,
+            "velocityX": 0.6463869297087906,
+            "velocityY": -0.016110376646462816,
+            "timestamp": 1.419247737711142
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 1.404904480201356e-31,
+            "velocityX": 0,
+            "velocityY": -1.2280872780983036e-31,
+            "timestamp": 1.5079507213180885
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.traj
new file mode 100644
index 00000000..0e209e38
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.traj	
@@ -0,0 +1,166 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -2.0886910556164006e-30,
+            "velocityX": -6.72399315077418e-33,
+            "velocityY": 1.0977042871870097e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 4.300222432415914,
+            "y": 3.053065889088155,
+            "heading": 2.5374285401398833,
+            "angularVelocity": -0.4122912034410164,
+            "velocityX": 0.41962999329145273,
+            "velocityY": 0.5982424370672392,
+            "timestamp": 0.08870298360694638
+        },
+        {
+            "x": 4.375819084559984,
+            "y": 3.158500650682638,
+            "heading": 2.466843265689978,
+            "angularVelocity": -0.7957485935611535,
+            "velocityX": 0.8522447506280879,
+            "velocityY": 1.1886270033675244,
+            "timestamp": 0.17740596721389276
+        },
+        {
+            "x": 4.491836015930185,
+            "y": 3.3149606373409832,
+            "heading": 2.3668866050755812,
+            "angularVelocity": -1.1268692049561326,
+            "velocityX": 1.307925919203419,
+            "velocityY": 1.7638638554892148,
+            "timestamp": 0.26610895082083913
+        },
+        {
+            "x": 4.652875991060298,
+            "y": 3.5188568125430604,
+            "heading": 2.248368974552935,
+            "angularVelocity": -1.3361177460254572,
+            "velocityX": 1.8154967125311139,
+            "velocityY": 2.2986394246394894,
+            "timestamp": 0.3548119344277855
+        },
+        {
+            "x": 4.870873285433555,
+            "y": 3.7498811407551353,
+            "heading": 2.196009009563727,
+            "angularVelocity": -0.5902841467116942,
+            "velocityX": 2.4576094907836294,
+            "velocityY": 2.604470772209555,
+            "timestamp": 0.4435149180347319
+        },
+        {
+            "x": 5.1428014530518755,
+            "y": 3.946488109616269,
+            "heading": 2.1960088759837526,
+            "angularVelocity": -0.0000015059242530161288,
+            "velocityX": 3.0656033941684258,
+            "velocityY": 2.21646398876866,
+            "timestamp": 0.5322179016416783
+        },
+        {
+            "x": 5.447787504833652,
+            "y": 4.086426434262152,
+            "heading": 2.1960087423061556,
+            "angularVelocity": -0.0000015070248085665752,
+            "velocityX": 3.4382840281135,
+            "velocityY": 1.577605610945025,
+            "timestamp": 0.6209208852486247
+        },
+        {
+            "x": 5.774173248105482,
+            "y": 4.1643468008186435,
+            "heading": 2.1960088209928723,
+            "angularVelocity": 8.870808373208906e-7,
+            "velocityX": 3.679535118210733,
+            "velocityY": 0.8784413261876967,
+            "timestamp": 0.709623868855571
+        },
+        {
+            "x": 6.1094823510079035,
+            "y": 4.1772711464289065,
+            "heading": 2.1960089213757183,
+            "angularVelocity": 0.0000011316738380089979,
+            "velocityX": 3.780133308572978,
+            "velocityY": 0.1457036176768579,
+            "timestamp": 0.7983268524625174
+        },
+        {
+            "x": 6.444914319541054,
+            "y": 4.1680717704970895,
+            "heading": 2.1960090177915097,
+            "angularVelocity": 0.000001086950938877831,
+            "velocityX": 3.78151844383826,
+            "velocityY": -0.10370988164931202,
+            "timestamp": 0.8870298360694638
+        },
+        {
+            "x": 6.780346283198341,
+            "y": 4.1588722167865155,
+            "heading": 2.196009114207664,
+            "angularVelocity": 0.0000010869550292397467,
+            "velocityX": 3.7815183888698267,
+            "velocityY": -0.10371188585197635,
+            "timestamp": 0.9757328196764102
+        },
+        {
+            "x": 7.07908088896125,
+            "y": 4.144071317525266,
+            "heading": 2.2852514937378463,
+            "angularVelocity": 1.0060809219859659,
+            "velocityX": 3.3678078641259517,
+            "velocityY": -0.16685909153668751,
+            "timestamp": 1.0644358032833565
+        },
+        {
+            "x": 7.324007491183461,
+            "y": 4.136590325759102,
+            "heading": 2.509537504382542,
+            "angularVelocity": 2.5285058238687177,
+            "velocityX": 2.7611991419308914,
+            "velocityY": -0.08433754381152235,
+            "timestamp": 1.153138786890303
+        },
+        {
+            "x": 7.505234177737953,
+            "y": 4.13149769076231,
+            "heading": 2.790057598116992,
+            "angularVelocity": 3.162465142970483,
+            "velocityX": 2.043073177307424,
+            "velocityY": -0.05741221760204461,
+            "timestamp": 1.2418417704972493
+        },
+        {
+            "x": 7.622663550770296,
+            "y": 4.128429038475573,
+            "heading": 3.015163183857666,
+            "angularVelocity": 2.5377453675982737,
+            "velocityX": 1.3238491903799634,
+            "velocityY": -0.03459469075285244,
+            "timestamp": 1.3305447541041957
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 1.4073575776830842,
+            "velocityX": 0.6463869297087906,
+            "velocityY": -0.016110376646462816,
+            "timestamp": 1.419247737711142
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 1.404904480201356e-31,
+            "velocityX": 0,
+            "velocityY": -1.2280872780983036e-31,
+            "timestamp": 1.5079507213180885
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.1.traj
new file mode 100644
index 00000000..c6e6fe7d
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.1.traj	
@@ -0,0 +1,184 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -5.037969685186626e-28,
+            "velocityX": 1.6468312353259754e-28,
+            "velocityY": 5.118071700544948e-29,
+            "timestamp": 0
+        },
+        {
+            "x": 4.2573202524069735,
+            "y": 2.93910307994174,
+            "heading": 2.524484159587002,
+            "angularVelocity": -0.5709623504386324,
+            "velocityX": -0.06549261829272982,
+            "velocityY": -0.7021964753285136,
+            "timestamp": 0.08672347725723033
+        },
+        {
+            "x": 4.260125495463358,
+            "y": 2.8166106869531333,
+            "heading": 2.4459027831938824,
+            "angularVelocity": -0.9061142251024153,
+            "velocityX": 0.032346985442753974,
+            "velocityY": -1.412447896032608,
+            "timestamp": 0.17344695451446066
+        },
+        {
+            "x": 4.2940460497089665,
+            "y": 2.639724962337947,
+            "heading": 2.375950811095946,
+            "angularVelocity": -0.8066094016324238,
+            "velocityX": 0.3911346191181556,
+            "velocityY": -2.039652124309153,
+            "timestamp": 0.26017043177169097
+        },
+        {
+            "x": 4.380083643145815,
+            "y": 2.4322611848370723,
+            "heading": 2.367349671900665,
+            "angularVelocity": -0.09917890134603644,
+            "velocityX": 0.9920911402301427,
+            "velocityY": -2.3922446846257825,
+            "timestamp": 0.3468939090289213
+        },
+        {
+            "x": 4.5223038363919486,
+            "y": 2.2062304273482725,
+            "heading": 2.4344770790327512,
+            "angularVelocity": 0.774039617126779,
+            "velocityX": 1.639927246278367,
+            "velocityY": -2.6063387289968842,
+            "timestamp": 0.43361738628615165
+        },
+        {
+            "x": 4.727106314265492,
+            "y": 1.976782887261895,
+            "heading": 2.494103363003819,
+            "angularVelocity": 0.6875448939185098,
+            "velocityX": 2.361557496894169,
+            "velocityY": -2.6457373175410677,
+            "timestamp": 0.5203408635433819
+        },
+        {
+            "x": 4.985728714890376,
+            "y": 1.77493098646381,
+            "heading": 2.4941035609909017,
+            "angularVelocity": 0.0000022829698365674285,
+            "velocityX": 2.982149803077939,
+            "velocityY": -2.327534678981717,
+            "timestamp": 0.6070643408006122
+        },
+        {
+            "x": 5.274471193088088,
+            "y": 1.6191826835790144,
+            "heading": 2.494103670963996,
+            "angularVelocity": 0.000001268089077272548,
+            "velocityX": 3.329461494507118,
+            "velocityY": -1.795918565659344,
+            "timestamp": 0.6937878180578425
+        },
+        {
+            "x": 5.576873114142296,
+            "y": 1.4919708990806198,
+            "heading": 2.4941037354476974,
+            "angularVelocity": 7.435553047089643e-7,
+            "velocityX": 3.486967204477438,
+            "velocityY": -1.4668667415291887,
+            "timestamp": 0.7805112953150728
+        },
+        {
+            "x": 5.888127945966446,
+            "y": 1.388287773871042,
+            "heading": 2.4941037912285227,
+            "angularVelocity": 6.432032813789173e-7,
+            "velocityX": 3.589049259417249,
+            "velocityY": -1.1955600546555811,
+            "timestamp": 0.8672347725723031
+        },
+        {
+            "x": 6.199389301961685,
+            "y": 1.2846242361365592,
+            "heading": 2.4941038470080463,
+            "angularVelocity": 6.431882742929459e-7,
+            "velocityX": 3.58912448900094,
+            "velocityY": -1.1953341933810824,
+            "timestamp": 0.9539582498295334
+        },
+        {
+            "x": 6.510650764865791,
+            "y": 1.1809610194297586,
+            "heading": 2.494103902787595,
+            "angularVelocity": 6.431885596372589e-7,
+            "velocityX": 3.589125721756674,
+            "velocityY": -1.1953304916421486,
+            "timestamp": 1.0406817270867637
+        },
+        {
+            "x": 6.821912914234967,
+            "y": 1.0772998639276916,
+            "heading": 2.4941039585563476,
+            "angularVelocity": 6.430640741979831e-7,
+            "velocityX": 3.589133637318779,
+            "velocityY": -1.1953067240904078,
+            "timestamp": 1.127405204343994
+        },
+        {
+            "x": 7.104835642474277,
+            "y": 0.9795860470438206,
+            "heading": 2.575393099111341,
+            "angularVelocity": 0.9373371908725603,
+            "velocityX": 3.2623545225260457,
+            "velocityY": -1.126728539655348,
+            "timestamp": 1.2141286816012242
+        },
+        {
+            "x": 7.336162807105594,
+            "y": 0.9014195336331526,
+            "heading": 2.773123816483908,
+            "angularVelocity": 2.280013712850493,
+            "velocityX": 2.6674110857568296,
+            "velocityY": -0.9013304803129645,
+            "timestamp": 1.3008521588584545
+        },
+        {
+            "x": 7.508575525064269,
+            "y": 0.8437553503848311,
+            "heading": 2.9477959992299394,
+            "angularVelocity": 2.0141279878335374,
+            "velocityX": 1.9880743186447822,
+            "velocityY": -0.664920100900538,
+            "timestamp": 1.3875756361156848
+        },
+        {
+            "x": 7.62299427085809,
+            "y": 0.8057956185147762,
+            "heading": 3.0737109392018835,
+            "angularVelocity": 1.451912953149597,
+            "velocityX": 1.31935145375275,
+            "velocityY": -0.4377099843155908,
+            "timestamp": 1.474299113372915
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 0.7643727269087626,
+            "velocityX": 0.6573275305004772,
+            "velocityY": -0.2167304530355382,
+            "timestamp": 1.5610225906301454
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": -1.052133339029093e-28,
+            "velocityX": 0,
+            "velocityY": -1.789975640452255e-29,
+            "timestamp": 1.6477460678873757
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.traj
new file mode 100644
index 00000000..c6e6fe7d
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.traj	
@@ -0,0 +1,184 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -5.037969685186626e-28,
+            "velocityX": 1.6468312353259754e-28,
+            "velocityY": 5.118071700544948e-29,
+            "timestamp": 0
+        },
+        {
+            "x": 4.2573202524069735,
+            "y": 2.93910307994174,
+            "heading": 2.524484159587002,
+            "angularVelocity": -0.5709623504386324,
+            "velocityX": -0.06549261829272982,
+            "velocityY": -0.7021964753285136,
+            "timestamp": 0.08672347725723033
+        },
+        {
+            "x": 4.260125495463358,
+            "y": 2.8166106869531333,
+            "heading": 2.4459027831938824,
+            "angularVelocity": -0.9061142251024153,
+            "velocityX": 0.032346985442753974,
+            "velocityY": -1.412447896032608,
+            "timestamp": 0.17344695451446066
+        },
+        {
+            "x": 4.2940460497089665,
+            "y": 2.639724962337947,
+            "heading": 2.375950811095946,
+            "angularVelocity": -0.8066094016324238,
+            "velocityX": 0.3911346191181556,
+            "velocityY": -2.039652124309153,
+            "timestamp": 0.26017043177169097
+        },
+        {
+            "x": 4.380083643145815,
+            "y": 2.4322611848370723,
+            "heading": 2.367349671900665,
+            "angularVelocity": -0.09917890134603644,
+            "velocityX": 0.9920911402301427,
+            "velocityY": -2.3922446846257825,
+            "timestamp": 0.3468939090289213
+        },
+        {
+            "x": 4.5223038363919486,
+            "y": 2.2062304273482725,
+            "heading": 2.4344770790327512,
+            "angularVelocity": 0.774039617126779,
+            "velocityX": 1.639927246278367,
+            "velocityY": -2.6063387289968842,
+            "timestamp": 0.43361738628615165
+        },
+        {
+            "x": 4.727106314265492,
+            "y": 1.976782887261895,
+            "heading": 2.494103363003819,
+            "angularVelocity": 0.6875448939185098,
+            "velocityX": 2.361557496894169,
+            "velocityY": -2.6457373175410677,
+            "timestamp": 0.5203408635433819
+        },
+        {
+            "x": 4.985728714890376,
+            "y": 1.77493098646381,
+            "heading": 2.4941035609909017,
+            "angularVelocity": 0.0000022829698365674285,
+            "velocityX": 2.982149803077939,
+            "velocityY": -2.327534678981717,
+            "timestamp": 0.6070643408006122
+        },
+        {
+            "x": 5.274471193088088,
+            "y": 1.6191826835790144,
+            "heading": 2.494103670963996,
+            "angularVelocity": 0.000001268089077272548,
+            "velocityX": 3.329461494507118,
+            "velocityY": -1.795918565659344,
+            "timestamp": 0.6937878180578425
+        },
+        {
+            "x": 5.576873114142296,
+            "y": 1.4919708990806198,
+            "heading": 2.4941037354476974,
+            "angularVelocity": 7.435553047089643e-7,
+            "velocityX": 3.486967204477438,
+            "velocityY": -1.4668667415291887,
+            "timestamp": 0.7805112953150728
+        },
+        {
+            "x": 5.888127945966446,
+            "y": 1.388287773871042,
+            "heading": 2.4941037912285227,
+            "angularVelocity": 6.432032813789173e-7,
+            "velocityX": 3.589049259417249,
+            "velocityY": -1.1955600546555811,
+            "timestamp": 0.8672347725723031
+        },
+        {
+            "x": 6.199389301961685,
+            "y": 1.2846242361365592,
+            "heading": 2.4941038470080463,
+            "angularVelocity": 6.431882742929459e-7,
+            "velocityX": 3.58912448900094,
+            "velocityY": -1.1953341933810824,
+            "timestamp": 0.9539582498295334
+        },
+        {
+            "x": 6.510650764865791,
+            "y": 1.1809610194297586,
+            "heading": 2.494103902787595,
+            "angularVelocity": 6.431885596372589e-7,
+            "velocityX": 3.589125721756674,
+            "velocityY": -1.1953304916421486,
+            "timestamp": 1.0406817270867637
+        },
+        {
+            "x": 6.821912914234967,
+            "y": 1.0772998639276916,
+            "heading": 2.4941039585563476,
+            "angularVelocity": 6.430640741979831e-7,
+            "velocityX": 3.589133637318779,
+            "velocityY": -1.1953067240904078,
+            "timestamp": 1.127405204343994
+        },
+        {
+            "x": 7.104835642474277,
+            "y": 0.9795860470438206,
+            "heading": 2.575393099111341,
+            "angularVelocity": 0.9373371908725603,
+            "velocityX": 3.2623545225260457,
+            "velocityY": -1.126728539655348,
+            "timestamp": 1.2141286816012242
+        },
+        {
+            "x": 7.336162807105594,
+            "y": 0.9014195336331526,
+            "heading": 2.773123816483908,
+            "angularVelocity": 2.280013712850493,
+            "velocityX": 2.6674110857568296,
+            "velocityY": -0.9013304803129645,
+            "timestamp": 1.3008521588584545
+        },
+        {
+            "x": 7.508575525064269,
+            "y": 0.8437553503848311,
+            "heading": 2.9477959992299394,
+            "angularVelocity": 2.0141279878335374,
+            "velocityX": 1.9880743186447822,
+            "velocityY": -0.664920100900538,
+            "timestamp": 1.3875756361156848
+        },
+        {
+            "x": 7.62299427085809,
+            "y": 0.8057956185147762,
+            "heading": 3.0737109392018835,
+            "angularVelocity": 1.451912953149597,
+            "velocityX": 1.31935145375275,
+            "velocityY": -0.4377099843155908,
+            "timestamp": 1.474299113372915
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 0.7643727269087626,
+            "velocityX": 0.6573275305004772,
+            "velocityY": -0.2167304530355382,
+            "timestamp": 1.5610225906301454
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": -1.052133339029093e-28,
+            "velocityX": 0,
+            "velocityY": -1.789975640452255e-29,
+            "timestamp": 1.6477460678873757
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.1.traj
new file mode 100644
index 00000000..2987bdd4
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": -5.177514657860589e-36,
+            "velocityY": 7.381711039090797e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 5.22399403974827,
+            "y": 1.6219999714389142,
+            "heading": 2.5657089279494847,
+            "angularVelocity": 0.8361720983637341,
+            "velocityX": 0.6107431391380216,
+            "velocityY": 0.02545042761966764,
+            "timestamp": 0.07858301906756712
+        },
+        {
+            "x": 5.320421642922389,
+            "y": 1.6267873270311852,
+            "heading": 2.6934140350122218,
+            "angularVelocity": 1.6250979992628447,
+            "velocityX": 1.2270793909204332,
+            "velocityY": 0.06092099348021541,
+            "timestamp": 0.15716603813513425
+        },
+        {
+            "x": 5.466088696265102,
+            "y": 1.6359772771500425,
+            "heading": 2.8734313337183015,
+            "angularVelocity": 2.2907913292475777,
+            "velocityX": 1.8536708702609945,
+            "velocityY": 0.11694575021297698,
+            "timestamp": 0.23574905720270137
+        },
+        {
+            "x": 5.662207065544692,
+            "y": 1.6523107767500078,
+            "heading": 3.0800991109745315,
+            "angularVelocity": 2.629929210005703,
+            "velocityX": 2.495683820838744,
+            "velocityY": 0.2078502428867176,
+            "timestamp": 0.3143320762702685
+        },
+        {
+            "x": 5.9065811613932215,
+            "y": 1.6816988713551873,
+            "heading": 3.2417008687725852,
+            "angularVelocity": 2.056446287194764,
+            "velocityX": 3.1097570282762126,
+            "velocityY": 0.3739751278824109,
+            "timestamp": 0.39291509533783564
+        },
+        {
+            "x": 6.188716953254853,
+            "y": 1.7271916925648765,
+            "heading": 3.2799906959816494,
+            "angularVelocity": 0.48725319621713953,
+            "velocityX": 3.5902895461301405,
+            "velocityY": 0.5789141439141413,
+            "timestamp": 0.4714981144054028
+        },
+        {
+            "x": 6.470102604778816,
+            "y": 1.8230794962925625,
+            "heading": 3.2799906961506404,
+            "angularVelocity": 2.150475348001427e-9,
+            "velocityX": 3.580743713626258,
+            "velocityY": 1.22021022945479,
+            "timestamp": 0.5500811334729699
+        },
+        {
+            "x": 6.733423435664353,
+            "y": 1.9610450600579923,
+            "heading": 3.279990689877103,
+            "angularVelocity": -7.983324169828431e-8,
+            "velocityX": 3.3508617257263804,
+            "velocityY": 1.755666369178364,
+            "timestamp": 0.6286641525405371
+        },
+        {
+            "x": 6.996743409287627,
+            "y": 2.0990122599800607,
+            "heading": 3.2799906836034283,
+            "angularVelocity": -7.983499950790008e-8,
+            "velocityX": 3.3508508167250133,
+            "velocityY": 1.7556871899187498,
+            "timestamp": 0.7072471716081042
+        },
+        {
+            "x": 7.224495559877957,
+            "y": 2.2183414399847847,
+            "heading": 3.23332230759167,
+            "angularVelocity": -0.5938735437439961,
+            "velocityX": 2.8982362002979842,
+            "velocityY": 1.5185110144740412,
+            "timestamp": 0.7858301906756714
+        },
+        {
+            "x": 7.406697323512462,
+            "y": 2.313804835379604,
+            "heading": 3.1959916534176562,
+            "angularVelocity": -0.4750473399592355,
+            "velocityX": 2.31858950949497,
+            "velocityY": 1.2148094655505426,
+            "timestamp": 0.8644132097432385
+        },
+        {
+            "x": 7.543348661054632,
+            "y": 2.3854024089057084,
+            "heading": 3.1679955366885673,
+            "angularVelocity": -0.356261658832645,
+            "velocityX": 1.7389423206643992,
+            "velocityY": 0.9111074424939519,
+            "timestamp": 0.9429962288108057
+        },
+        {
+            "x": 7.634449555023231,
+            "y": 2.4331341352600573,
+            "heading": 3.149331937782722,
+            "angularVelocity": -0.2375016781907786,
+            "velocityX": 1.1592949093781795,
+            "velocityY": 0.607405097446157,
+            "timestamp": 1.0215792478783727
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -0.11875259939679916,
+            "velocityX": 0.5796474291424605,
+            "velocityY": 0.30370256860992423,
+            "timestamp": 1.1001622669459399
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -1.0945332293260443e-31,
+            "velocityX": 7.198086319769953e-33,
+            "velocityY": -6.46561486115161e-33,
+            "timestamp": 1.178745286013507
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.traj
new file mode 100644
index 00000000..2987bdd4
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": -5.177514657860589e-36,
+            "velocityY": 7.381711039090797e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 5.22399403974827,
+            "y": 1.6219999714389142,
+            "heading": 2.5657089279494847,
+            "angularVelocity": 0.8361720983637341,
+            "velocityX": 0.6107431391380216,
+            "velocityY": 0.02545042761966764,
+            "timestamp": 0.07858301906756712
+        },
+        {
+            "x": 5.320421642922389,
+            "y": 1.6267873270311852,
+            "heading": 2.6934140350122218,
+            "angularVelocity": 1.6250979992628447,
+            "velocityX": 1.2270793909204332,
+            "velocityY": 0.06092099348021541,
+            "timestamp": 0.15716603813513425
+        },
+        {
+            "x": 5.466088696265102,
+            "y": 1.6359772771500425,
+            "heading": 2.8734313337183015,
+            "angularVelocity": 2.2907913292475777,
+            "velocityX": 1.8536708702609945,
+            "velocityY": 0.11694575021297698,
+            "timestamp": 0.23574905720270137
+        },
+        {
+            "x": 5.662207065544692,
+            "y": 1.6523107767500078,
+            "heading": 3.0800991109745315,
+            "angularVelocity": 2.629929210005703,
+            "velocityX": 2.495683820838744,
+            "velocityY": 0.2078502428867176,
+            "timestamp": 0.3143320762702685
+        },
+        {
+            "x": 5.9065811613932215,
+            "y": 1.6816988713551873,
+            "heading": 3.2417008687725852,
+            "angularVelocity": 2.056446287194764,
+            "velocityX": 3.1097570282762126,
+            "velocityY": 0.3739751278824109,
+            "timestamp": 0.39291509533783564
+        },
+        {
+            "x": 6.188716953254853,
+            "y": 1.7271916925648765,
+            "heading": 3.2799906959816494,
+            "angularVelocity": 0.48725319621713953,
+            "velocityX": 3.5902895461301405,
+            "velocityY": 0.5789141439141413,
+            "timestamp": 0.4714981144054028
+        },
+        {
+            "x": 6.470102604778816,
+            "y": 1.8230794962925625,
+            "heading": 3.2799906961506404,
+            "angularVelocity": 2.150475348001427e-9,
+            "velocityX": 3.580743713626258,
+            "velocityY": 1.22021022945479,
+            "timestamp": 0.5500811334729699
+        },
+        {
+            "x": 6.733423435664353,
+            "y": 1.9610450600579923,
+            "heading": 3.279990689877103,
+            "angularVelocity": -7.983324169828431e-8,
+            "velocityX": 3.3508617257263804,
+            "velocityY": 1.755666369178364,
+            "timestamp": 0.6286641525405371
+        },
+        {
+            "x": 6.996743409287627,
+            "y": 2.0990122599800607,
+            "heading": 3.2799906836034283,
+            "angularVelocity": -7.983499950790008e-8,
+            "velocityX": 3.3508508167250133,
+            "velocityY": 1.7556871899187498,
+            "timestamp": 0.7072471716081042
+        },
+        {
+            "x": 7.224495559877957,
+            "y": 2.2183414399847847,
+            "heading": 3.23332230759167,
+            "angularVelocity": -0.5938735437439961,
+            "velocityX": 2.8982362002979842,
+            "velocityY": 1.5185110144740412,
+            "timestamp": 0.7858301906756714
+        },
+        {
+            "x": 7.406697323512462,
+            "y": 2.313804835379604,
+            "heading": 3.1959916534176562,
+            "angularVelocity": -0.4750473399592355,
+            "velocityX": 2.31858950949497,
+            "velocityY": 1.2148094655505426,
+            "timestamp": 0.8644132097432385
+        },
+        {
+            "x": 7.543348661054632,
+            "y": 2.3854024089057084,
+            "heading": 3.1679955366885673,
+            "angularVelocity": -0.356261658832645,
+            "velocityX": 1.7389423206643992,
+            "velocityY": 0.9111074424939519,
+            "timestamp": 0.9429962288108057
+        },
+        {
+            "x": 7.634449555023231,
+            "y": 2.4331341352600573,
+            "heading": 3.149331937782722,
+            "angularVelocity": -0.2375016781907786,
+            "velocityX": 1.1592949093781795,
+            "velocityY": 0.607405097446157,
+            "timestamp": 1.0215792478783727
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -0.11875259939679916,
+            "velocityX": 0.5796474291424605,
+            "velocityY": 0.30370256860992423,
+            "timestamp": 1.1001622669459399
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -1.0945332293260443e-31,
+            "velocityX": 7.198086319769953e-33,
+            "velocityY": -6.46561486115161e-33,
+            "timestamp": 1.178745286013507
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.1.traj
new file mode 100644
index 00000000..89c7b3cf
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.222452071774302,
+            "y": 1.6045080698057588,
+            "heading": 2.5267186887396895,
+            "angularVelocity": 0.3466731100007665,
+            "velocityX": 0.6027123690413302,
+            "velocityY": -0.20100670630495998,
+            "timestamp": 0.0770717080988209
+        },
+        {
+            "x": 5.315452291782514,
+            "y": 1.5735030960792171,
+            "heading": 2.577725213280899,
+            "angularVelocity": 0.6618060738424182,
+            "velocityX": 1.2066713233988244,
+            "velocityY": -0.40228735668849247,
+            "timestamp": 0.1541434161976418
+        },
+        {
+            "x": 5.4551316607074885,
+            "y": 1.5269515495858468,
+            "heading": 2.6491768389859613,
+            "angularVelocity": 0.9270798256274155,
+            "velocityX": 1.8123300024164322,
+            "velocityY": -0.6040030465353413,
+            "timestamp": 0.23121512429646268
+        },
+        {
+            "x": 5.641664886749133,
+            "y": 1.4648000786644693,
+            "heading": 2.734238612850838,
+            "angularVelocity": 1.1036705421892394,
+            "velocityX": 2.420255508058452,
+            "velocityY": -0.8064109704392095,
+            "timestamp": 0.3082868323952836
+        },
+        {
+            "x": 5.8751883475378746,
+            "y": 1.3870009970649684,
+            "heading": 2.8177772109144708,
+            "angularVelocity": 1.0839074431374935,
+            "velocityX": 3.02995050387775,
+            "velocityY": -1.0094376200894257,
+            "timestamp": 0.3853585404941045
+        },
+        {
+            "x": 6.15120111527276,
+            "y": 1.295041169269926,
+            "heading": 2.8199508970154183,
+            "angularVelocity": 0.028203424506439225,
+            "velocityX": 3.5812462775702794,
+            "velocityY": -1.193172307497486,
+            "timestamp": 0.4624302485929254
+        },
+        {
+            "x": 6.427838786740925,
+            "y": 1.202967940667996,
+            "heading": 2.8199509259126705,
+            "angularVelocity": 3.7493981751466555e-7,
+            "velocityX": 3.5893543596239312,
+            "velocityY": -1.1946436750029477,
+            "timestamp": 0.5395019566917463
+        },
+        {
+            "x": 6.7044764770342145,
+            "y": 1.110894768626946,
+            "heading": 2.8199509548099355,
+            "angularVelocity": 3.749399855143739e-7,
+            "velocityX": 3.5893546038785895,
+            "velocityY": -1.1946429411295039,
+            "timestamp": 0.6165736647905672
+        },
+        {
+            "x": 6.980650518345068,
+            "y": 1.0194616665370815,
+            "heading": 2.8221688917599996,
+            "angularVelocity": 0.02877757616608584,
+            "velocityX": 3.583338790892565,
+            "velocityY": -1.1863380784636202,
+            "timestamp": 0.6936453728893881
+        },
+        {
+            "x": 7.214215740363564,
+            "y": 0.9418010792136509,
+            "heading": 2.905688153737692,
+            "angularVelocity": 1.083656558780357,
+            "velocityX": 3.030492352901014,
+            "velocityY": -1.0076406665835769,
+            "timestamp": 0.770717080988209
+        },
+        {
+            "x": 7.400786993851664,
+            "y": 0.8797765728247763,
+            "heading": 2.990770900783412,
+            "angularVelocity": 1.1039426677377822,
+            "velocityX": 2.420748911505631,
+            "velocityY": -0.8047636145464304,
+            "timestamp": 0.8477887890870299
+        },
+        {
+            "x": 7.540501987432905,
+            "y": 0.8333423582814848,
+            "heading": 3.0622172150737166,
+            "angularVelocity": 0.9270109103939473,
+            "velocityX": 1.8127922298296504,
+            "velocityY": -0.6024806727230407,
+            "timestamp": 0.9248604971858508
+        },
+        {
+            "x": 7.633531379147319,
+            "y": 0.802434096261962,
+            "heading": 3.113237732512233,
+            "angularVelocity": 0.661987630702283,
+            "velocityX": 1.2070498242381353,
+            "velocityY": -0.4010325290817799,
+            "timestamp": 1.0019322052846718
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 0.3472385411966328,
+            "velocityX": 0.6029270921710836,
+            "velocityY": -0.20025631509518968,
+            "timestamp": 1.0790039133834926
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 1.7638718006253152e-30,
+            "velocityX": 1.1888064162036181e-31,
+            "velocityY": -2.2168268180884255e-31,
+            "timestamp": 1.1560756214823134
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.traj
new file mode 100644
index 00000000..89c7b3cf
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.222452071774302,
+            "y": 1.6045080698057588,
+            "heading": 2.5267186887396895,
+            "angularVelocity": 0.3466731100007665,
+            "velocityX": 0.6027123690413302,
+            "velocityY": -0.20100670630495998,
+            "timestamp": 0.0770717080988209
+        },
+        {
+            "x": 5.315452291782514,
+            "y": 1.5735030960792171,
+            "heading": 2.577725213280899,
+            "angularVelocity": 0.6618060738424182,
+            "velocityX": 1.2066713233988244,
+            "velocityY": -0.40228735668849247,
+            "timestamp": 0.1541434161976418
+        },
+        {
+            "x": 5.4551316607074885,
+            "y": 1.5269515495858468,
+            "heading": 2.6491768389859613,
+            "angularVelocity": 0.9270798256274155,
+            "velocityX": 1.8123300024164322,
+            "velocityY": -0.6040030465353413,
+            "timestamp": 0.23121512429646268
+        },
+        {
+            "x": 5.641664886749133,
+            "y": 1.4648000786644693,
+            "heading": 2.734238612850838,
+            "angularVelocity": 1.1036705421892394,
+            "velocityX": 2.420255508058452,
+            "velocityY": -0.8064109704392095,
+            "timestamp": 0.3082868323952836
+        },
+        {
+            "x": 5.8751883475378746,
+            "y": 1.3870009970649684,
+            "heading": 2.8177772109144708,
+            "angularVelocity": 1.0839074431374935,
+            "velocityX": 3.02995050387775,
+            "velocityY": -1.0094376200894257,
+            "timestamp": 0.3853585404941045
+        },
+        {
+            "x": 6.15120111527276,
+            "y": 1.295041169269926,
+            "heading": 2.8199508970154183,
+            "angularVelocity": 0.028203424506439225,
+            "velocityX": 3.5812462775702794,
+            "velocityY": -1.193172307497486,
+            "timestamp": 0.4624302485929254
+        },
+        {
+            "x": 6.427838786740925,
+            "y": 1.202967940667996,
+            "heading": 2.8199509259126705,
+            "angularVelocity": 3.7493981751466555e-7,
+            "velocityX": 3.5893543596239312,
+            "velocityY": -1.1946436750029477,
+            "timestamp": 0.5395019566917463
+        },
+        {
+            "x": 6.7044764770342145,
+            "y": 1.110894768626946,
+            "heading": 2.8199509548099355,
+            "angularVelocity": 3.749399855143739e-7,
+            "velocityX": 3.5893546038785895,
+            "velocityY": -1.1946429411295039,
+            "timestamp": 0.6165736647905672
+        },
+        {
+            "x": 6.980650518345068,
+            "y": 1.0194616665370815,
+            "heading": 2.8221688917599996,
+            "angularVelocity": 0.02877757616608584,
+            "velocityX": 3.583338790892565,
+            "velocityY": -1.1863380784636202,
+            "timestamp": 0.6936453728893881
+        },
+        {
+            "x": 7.214215740363564,
+            "y": 0.9418010792136509,
+            "heading": 2.905688153737692,
+            "angularVelocity": 1.083656558780357,
+            "velocityX": 3.030492352901014,
+            "velocityY": -1.0076406665835769,
+            "timestamp": 0.770717080988209
+        },
+        {
+            "x": 7.400786993851664,
+            "y": 0.8797765728247763,
+            "heading": 2.990770900783412,
+            "angularVelocity": 1.1039426677377822,
+            "velocityX": 2.420748911505631,
+            "velocityY": -0.8047636145464304,
+            "timestamp": 0.8477887890870299
+        },
+        {
+            "x": 7.540501987432905,
+            "y": 0.8333423582814848,
+            "heading": 3.0622172150737166,
+            "angularVelocity": 0.9270109103939473,
+            "velocityX": 1.8127922298296504,
+            "velocityY": -0.6024806727230407,
+            "timestamp": 0.9248604971858508
+        },
+        {
+            "x": 7.633531379147319,
+            "y": 0.802434096261962,
+            "heading": 3.113237732512233,
+            "angularVelocity": 0.661987630702283,
+            "velocityX": 1.2070498242381353,
+            "velocityY": -0.4010325290817799,
+            "timestamp": 1.0019322052846718
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 0.3472385411966328,
+            "velocityX": 0.6029270921710836,
+            "velocityY": -0.20025631509518968,
+            "timestamp": 1.0790039133834926
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 1.7638718006253152e-30,
+            "velocityX": 1.1888064162036181e-31,
+            "velocityY": -2.2168268180884255e-31,
+            "timestamp": 1.1560756214823134
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.1.traj
new file mode 100644
index 00000000..c3926d1c
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.1.traj	
@@ -0,0 +1,274 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": 2.1,
+            "angularVelocity": 0,
+            "velocityX": 1.276910697826558e-30,
+            "velocityY": -1.1746574795316565e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 0.761216454009274,
+            "y": 4.38433482667126,
+            "heading": 2.165113129644946,
+            "angularVelocity": 0.7716161812421117,
+            "velocityX": 0.6069351123943075,
+            "velocityY": 0.28837726169296907,
+            "timestamp": 0.08438538644967974
+        },
+        {
+            "x": 0.8642342955307559,
+            "y": 4.433284699389541,
+            "heading": 2.2862365234671036,
+            "angularVelocity": 1.435359828498015,
+            "velocityX": 1.2208019167265607,
+            "velocityY": 0.58007523313856,
+            "timestamp": 0.1687707728993595
+        },
+        {
+            "x": 1.0198907802831574,
+            "y": 4.507085180578877,
+            "heading": 2.44934046785083,
+            "angularVelocity": 1.9328458545483174,
+            "velocityX": 1.844590530437629,
+            "velocityY": 0.8745647119048293,
+            "timestamp": 0.25315615934903923
+        },
+        {
+            "x": 1.22914875569361,
+            "y": 4.606070154859929,
+            "heading": 2.62757889489394,
+            "angularVelocity": 2.112195423189735,
+            "velocityX": 2.4797892646404183,
+            "velocityY": 1.1730108546707563,
+            "timestamp": 0.337541545798719
+        },
+        {
+            "x": 1.4854756632487267,
+            "y": 4.729647702991699,
+            "heading": 2.7127404908574917,
+            "angularVelocity": 1.0091983878551618,
+            "velocityX": 3.0375746126133176,
+            "velocityY": 1.4644425217566792,
+            "timestamp": 0.4219269322483987
+        },
+        {
+            "x": 1.7744042397686677,
+            "y": 4.865385720511678,
+            "heading": 2.7127405410221264,
+            "angularVelocity": 5.944706378546851e-7,
+            "velocityX": 3.4239172050510245,
+            "velocityY": 1.6085488640966181,
+            "timestamp": 0.5063123186980785
+        },
+        {
+            "x": 2.0633325652631687,
+            "y": 5.001124272354809,
+            "heading": 2.712740591190551,
+            "angularVelocity": 5.945155495227871e-7,
+            "velocityX": 3.423914230300861,
+            "velocityY": 1.608555196036035,
+            "timestamp": 0.5906977051477582
+        },
+        {
+            "x": 2.352241240419818,
+            "y": 5.1369046433555585,
+            "heading": 2.712740641359129,
+            "angularVelocity": 5.945173720540621e-7,
+            "velocityX": 3.4236813660732097,
+            "velocityY": 1.6090507694920106,
+            "timestamp": 0.675083091597438
+        },
+        {
+            "x": 2.647177658207551,
+            "y": 5.259039982285096,
+            "heading": 2.712740691973867,
+            "angularVelocity": 5.998045355791158e-7,
+            "velocityX": 3.4951124856626223,
+            "velocityY": 1.4473517758000545,
+            "timestamp": 0.7594684780471177
+        },
+        {
+            "x": 2.9596305957770053,
+            "y": 5.324444044648143,
+            "heading": 2.712740749087516,
+            "angularVelocity": 6.768191931683843e-7,
+            "velocityX": 3.7026901305450077,
+            "velocityY": 0.7750638483126594,
+            "timestamp": 0.8438538644967974
+        },
+        {
+            "x": 3.2787911260313356,
+            "y": 5.330853358998101,
+            "heading": 2.712740828995151,
+            "angularVelocity": 9.469368813287509e-7,
+            "velocityX": 3.7821777405102064,
+            "velocityY": 0.0759528944478236,
+            "timestamp": 0.9282392509464772
+        },
+        {
+            "x": 3.5936179112160946,
+            "y": 5.278046121689486,
+            "heading": 2.7127406682187845,
+            "angularVelocity": -0.000001905263138772926,
+            "velocityX": 3.730821157908589,
+            "velocityY": -0.6257865198035278,
+            "timestamp": 1.012624637396157
+        },
+        {
+            "x": 3.893219599027341,
+            "y": 5.167849261276542,
+            "heading": 2.7127405436852854,
+            "angularVelocity": -0.0000014757709178373628,
+            "velocityX": 3.550397769285573,
+            "velocityY": -1.3058761125501486,
+            "timestamp": 1.0970100238458367
+        },
+        {
+            "x": 4.167231798281599,
+            "y": 5.0040755200168485,
+            "heading": 2.7127404397173622,
+            "angularVelocity": -0.0000012320607563537726,
+            "velocityX": 3.247152271058871,
+            "velocityY": -1.940783210815242,
+            "timestamp": 1.1813954102955164
+        },
+        {
+            "x": 4.431867133754474,
+            "y": 4.825546352606267,
+            "heading": 2.712740340652136,
+            "angularVelocity": -0.0000011739618702897745,
+            "velocityX": 3.136032749351509,
+            "velocityY": -2.115640810829663,
+            "timestamp": 1.2657807967451962
+        },
+        {
+            "x": 4.666216273140183,
+            "y": 4.608786081760519,
+            "heading": 2.712740321249029,
+            "angularVelocity": -2.2993444839053286e-7,
+            "velocityX": 2.7771294206901915,
+            "velocityY": -2.5686944145832347,
+            "timestamp": 1.350166183194876
+        },
+        {
+            "x": 4.936653737211477,
+            "y": 4.439174492795635,
+            "heading": 2.7127404800965853,
+            "angularVelocity": 0.0000018824059850759979,
+            "velocityX": 3.2047902539685085,
+            "velocityY": -2.009964000887954,
+            "timestamp": 1.4345515696445557
+        },
+        {
+            "x": 5.233678140767672,
+            "y": 4.322208601201109,
+            "heading": 2.7127406283303626,
+            "angularVelocity": 0.0000017566285300639338,
+            "velocityX": 3.5198559377731047,
+            "velocityY": -1.3860917928520893,
+            "timestamp": 1.5189369560942354
+        },
+        {
+            "x": 5.5320155723758075,
+            "y": 4.208633545762192,
+            "heading": 2.7127407751281813,
+            "angularVelocity": 0.000001739611855911675,
+            "velocityX": 3.5354158363194896,
+            "velocityY": -1.3459090515232621,
+            "timestamp": 1.6033223425439151
+        },
+        {
+            "x": 5.844921751101867,
+            "y": 4.1454334406216455,
+            "heading": 2.712740834966484,
+            "angularVelocity": 7.091074065603108e-7,
+            "velocityX": 3.708061216412759,
+            "velocityY": -0.748946088885059,
+            "timestamp": 1.687707728993595
+        },
+        {
+            "x": 6.164108889380454,
+            "y": 4.140523996019331,
+            "heading": 2.7127408721494533,
+            "angularVelocity": 4.406328022097576e-7,
+            "velocityX": 3.7824930560568397,
+            "velocityY": -0.05817884836341004,
+            "timestamp": 1.7720931154432746
+        },
+        {
+            "x": 6.483326965734431,
+            "y": 4.138437864876922,
+            "heading": 2.712740909220737,
+            "angularVelocity": 4.393092878808321e-7,
+            "velocityX": 3.7828596844114597,
+            "velocityY": -0.02472147406577555,
+            "timestamp": 1.8564785018929544
+        },
+        {
+            "x": 6.802545327798344,
+            "y": 4.1363959875067975,
+            "heading": 2.712740946289213,
+            "angularVelocity": 4.392760075001551e-7,
+            "velocityX": 3.782863070186549,
+            "velocityY": -0.024197049465056585,
+            "timestamp": 1.9408638883426341
+        },
+        {
+            "x": 7.094929199701602,
+            "y": 4.132589002725807,
+            "heading": 2.784661628236826,
+            "angularVelocity": 0.8522883519706954,
+            "velocityX": 3.464863813565768,
+            "velocityY": -0.04511426611993158,
+            "timestamp": 2.025249274792314
+        },
+        {
+            "x": 7.329645788590582,
+            "y": 4.130189422490391,
+            "heading": 2.9078456854173678,
+            "angularVelocity": 1.4597794993094293,
+            "velocityX": 2.7814838417425016,
+            "velocityY": -0.02843596902640405,
+            "timestamp": 2.1096346612419934
+        },
+        {
+            "x": 7.5051104174827215,
+            "y": 4.128539949714214,
+            "heading": 3.0176429032166108,
+            "angularVelocity": 1.3011401904845583,
+            "velocityX": 2.0793248247641927,
+            "velocityY": -0.01954690077910012,
+            "timestamp": 2.194020047691673
+        },
+        {
+            "x": 7.621781937801462,
+            "y": 4.1275005025427935,
+            "heading": 3.0976594405390174,
+            "angularVelocity": 0.9482274205158485,
+            "velocityX": 1.3826033775209736,
+            "velocityY": -0.01231785757139544,
+            "timestamp": 2.2784054341413524
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.5017522730222338,
+            "velocityX": 0.6899069216594048,
+            "velocityY": -0.00593115187164541,
+            "timestamp": 2.362790820591032
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 3.889752365812458e-29,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 2.4471762070407115
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.traj
new file mode 100644
index 00000000..c3926d1c
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.traj	
@@ -0,0 +1,274 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": 2.1,
+            "angularVelocity": 0,
+            "velocityX": 1.276910697826558e-30,
+            "velocityY": -1.1746574795316565e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 0.761216454009274,
+            "y": 4.38433482667126,
+            "heading": 2.165113129644946,
+            "angularVelocity": 0.7716161812421117,
+            "velocityX": 0.6069351123943075,
+            "velocityY": 0.28837726169296907,
+            "timestamp": 0.08438538644967974
+        },
+        {
+            "x": 0.8642342955307559,
+            "y": 4.433284699389541,
+            "heading": 2.2862365234671036,
+            "angularVelocity": 1.435359828498015,
+            "velocityX": 1.2208019167265607,
+            "velocityY": 0.58007523313856,
+            "timestamp": 0.1687707728993595
+        },
+        {
+            "x": 1.0198907802831574,
+            "y": 4.507085180578877,
+            "heading": 2.44934046785083,
+            "angularVelocity": 1.9328458545483174,
+            "velocityX": 1.844590530437629,
+            "velocityY": 0.8745647119048293,
+            "timestamp": 0.25315615934903923
+        },
+        {
+            "x": 1.22914875569361,
+            "y": 4.606070154859929,
+            "heading": 2.62757889489394,
+            "angularVelocity": 2.112195423189735,
+            "velocityX": 2.4797892646404183,
+            "velocityY": 1.1730108546707563,
+            "timestamp": 0.337541545798719
+        },
+        {
+            "x": 1.4854756632487267,
+            "y": 4.729647702991699,
+            "heading": 2.7127404908574917,
+            "angularVelocity": 1.0091983878551618,
+            "velocityX": 3.0375746126133176,
+            "velocityY": 1.4644425217566792,
+            "timestamp": 0.4219269322483987
+        },
+        {
+            "x": 1.7744042397686677,
+            "y": 4.865385720511678,
+            "heading": 2.7127405410221264,
+            "angularVelocity": 5.944706378546851e-7,
+            "velocityX": 3.4239172050510245,
+            "velocityY": 1.6085488640966181,
+            "timestamp": 0.5063123186980785
+        },
+        {
+            "x": 2.0633325652631687,
+            "y": 5.001124272354809,
+            "heading": 2.712740591190551,
+            "angularVelocity": 5.945155495227871e-7,
+            "velocityX": 3.423914230300861,
+            "velocityY": 1.608555196036035,
+            "timestamp": 0.5906977051477582
+        },
+        {
+            "x": 2.352241240419818,
+            "y": 5.1369046433555585,
+            "heading": 2.712740641359129,
+            "angularVelocity": 5.945173720540621e-7,
+            "velocityX": 3.4236813660732097,
+            "velocityY": 1.6090507694920106,
+            "timestamp": 0.675083091597438
+        },
+        {
+            "x": 2.647177658207551,
+            "y": 5.259039982285096,
+            "heading": 2.712740691973867,
+            "angularVelocity": 5.998045355791158e-7,
+            "velocityX": 3.4951124856626223,
+            "velocityY": 1.4473517758000545,
+            "timestamp": 0.7594684780471177
+        },
+        {
+            "x": 2.9596305957770053,
+            "y": 5.324444044648143,
+            "heading": 2.712740749087516,
+            "angularVelocity": 6.768191931683843e-7,
+            "velocityX": 3.7026901305450077,
+            "velocityY": 0.7750638483126594,
+            "timestamp": 0.8438538644967974
+        },
+        {
+            "x": 3.2787911260313356,
+            "y": 5.330853358998101,
+            "heading": 2.712740828995151,
+            "angularVelocity": 9.469368813287509e-7,
+            "velocityX": 3.7821777405102064,
+            "velocityY": 0.0759528944478236,
+            "timestamp": 0.9282392509464772
+        },
+        {
+            "x": 3.5936179112160946,
+            "y": 5.278046121689486,
+            "heading": 2.7127406682187845,
+            "angularVelocity": -0.000001905263138772926,
+            "velocityX": 3.730821157908589,
+            "velocityY": -0.6257865198035278,
+            "timestamp": 1.012624637396157
+        },
+        {
+            "x": 3.893219599027341,
+            "y": 5.167849261276542,
+            "heading": 2.7127405436852854,
+            "angularVelocity": -0.0000014757709178373628,
+            "velocityX": 3.550397769285573,
+            "velocityY": -1.3058761125501486,
+            "timestamp": 1.0970100238458367
+        },
+        {
+            "x": 4.167231798281599,
+            "y": 5.0040755200168485,
+            "heading": 2.7127404397173622,
+            "angularVelocity": -0.0000012320607563537726,
+            "velocityX": 3.247152271058871,
+            "velocityY": -1.940783210815242,
+            "timestamp": 1.1813954102955164
+        },
+        {
+            "x": 4.431867133754474,
+            "y": 4.825546352606267,
+            "heading": 2.712740340652136,
+            "angularVelocity": -0.0000011739618702897745,
+            "velocityX": 3.136032749351509,
+            "velocityY": -2.115640810829663,
+            "timestamp": 1.2657807967451962
+        },
+        {
+            "x": 4.666216273140183,
+            "y": 4.608786081760519,
+            "heading": 2.712740321249029,
+            "angularVelocity": -2.2993444839053286e-7,
+            "velocityX": 2.7771294206901915,
+            "velocityY": -2.5686944145832347,
+            "timestamp": 1.350166183194876
+        },
+        {
+            "x": 4.936653737211477,
+            "y": 4.439174492795635,
+            "heading": 2.7127404800965853,
+            "angularVelocity": 0.0000018824059850759979,
+            "velocityX": 3.2047902539685085,
+            "velocityY": -2.009964000887954,
+            "timestamp": 1.4345515696445557
+        },
+        {
+            "x": 5.233678140767672,
+            "y": 4.322208601201109,
+            "heading": 2.7127406283303626,
+            "angularVelocity": 0.0000017566285300639338,
+            "velocityX": 3.5198559377731047,
+            "velocityY": -1.3860917928520893,
+            "timestamp": 1.5189369560942354
+        },
+        {
+            "x": 5.5320155723758075,
+            "y": 4.208633545762192,
+            "heading": 2.7127407751281813,
+            "angularVelocity": 0.000001739611855911675,
+            "velocityX": 3.5354158363194896,
+            "velocityY": -1.3459090515232621,
+            "timestamp": 1.6033223425439151
+        },
+        {
+            "x": 5.844921751101867,
+            "y": 4.1454334406216455,
+            "heading": 2.712740834966484,
+            "angularVelocity": 7.091074065603108e-7,
+            "velocityX": 3.708061216412759,
+            "velocityY": -0.748946088885059,
+            "timestamp": 1.687707728993595
+        },
+        {
+            "x": 6.164108889380454,
+            "y": 4.140523996019331,
+            "heading": 2.7127408721494533,
+            "angularVelocity": 4.406328022097576e-7,
+            "velocityX": 3.7824930560568397,
+            "velocityY": -0.05817884836341004,
+            "timestamp": 1.7720931154432746
+        },
+        {
+            "x": 6.483326965734431,
+            "y": 4.138437864876922,
+            "heading": 2.712740909220737,
+            "angularVelocity": 4.393092878808321e-7,
+            "velocityX": 3.7828596844114597,
+            "velocityY": -0.02472147406577555,
+            "timestamp": 1.8564785018929544
+        },
+        {
+            "x": 6.802545327798344,
+            "y": 4.1363959875067975,
+            "heading": 2.712740946289213,
+            "angularVelocity": 4.392760075001551e-7,
+            "velocityX": 3.782863070186549,
+            "velocityY": -0.024197049465056585,
+            "timestamp": 1.9408638883426341
+        },
+        {
+            "x": 7.094929199701602,
+            "y": 4.132589002725807,
+            "heading": 2.784661628236826,
+            "angularVelocity": 0.8522883519706954,
+            "velocityX": 3.464863813565768,
+            "velocityY": -0.04511426611993158,
+            "timestamp": 2.025249274792314
+        },
+        {
+            "x": 7.329645788590582,
+            "y": 4.130189422490391,
+            "heading": 2.9078456854173678,
+            "angularVelocity": 1.4597794993094293,
+            "velocityX": 2.7814838417425016,
+            "velocityY": -0.02843596902640405,
+            "timestamp": 2.1096346612419934
+        },
+        {
+            "x": 7.5051104174827215,
+            "y": 4.128539949714214,
+            "heading": 3.0176429032166108,
+            "angularVelocity": 1.3011401904845583,
+            "velocityX": 2.0793248247641927,
+            "velocityY": -0.01954690077910012,
+            "timestamp": 2.194020047691673
+        },
+        {
+            "x": 7.621781937801462,
+            "y": 4.1275005025427935,
+            "heading": 3.0976594405390174,
+            "angularVelocity": 0.9482274205158485,
+            "velocityX": 1.3826033775209736,
+            "velocityY": -0.01231785757139544,
+            "timestamp": 2.2784054341413524
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 0.5017522730222338,
+            "velocityX": 0.6899069216594048,
+            "velocityY": -0.00593115187164541,
+            "timestamp": 2.362790820591032
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 3.889752365812458e-29,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 2.4471762070407115
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.1.traj
new file mode 100644
index 00000000..d8bd6114
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.1.traj	
@@ -0,0 +1,292 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": 2.1,
+            "angularVelocity": 0,
+            "velocityX": 8.526478185532948e-32,
+            "velocityY": -7.785685234118549e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 0.755735101962148,
+            "y": 4.33059726025757,
+            "heading": 2.1511666979053023,
+            "angularVelocity": 0.6232717514717016,
+            "velocityX": 0.5571083980530026,
+            "velocityY": -0.35816063665566134,
+            "timestamp": 0.08209372201529226
+        },
+        {
+            "x": 0.8475193720501324,
+            "y": 4.271815657764732,
+            "heading": 2.250112545454498,
+            "angularVelocity": 1.2052791019849762,
+            "velocityX": 1.1180424986808006,
+            "velocityY": -0.7160304229096619,
+            "timestamp": 0.16418744403058452
+        },
+        {
+            "x": 0.9858871774715989,
+            "y": 4.183669855686051,
+            "heading": 2.389658659666248,
+            "angularVelocity": 1.6998390472021188,
+            "velocityX": 1.6854858328349602,
+            "velocityY": -1.0737215942317955,
+            "timestamp": 0.2462811660458768
+        },
+        {
+            "x": 1.17173213163872,
+            "y": 4.066157035949835,
+            "heading": 2.5522635706365153,
+            "angularVelocity": 1.9807228491842224,
+            "velocityX": 2.2638144501780824,
+            "velocityY": -1.4314470930472174,
+            "timestamp": 0.32837488806116905
+        },
+        {
+            "x": 1.4047988716632624,
+            "y": 3.9214804758601747,
+            "heading": 2.664319566325619,
+            "angularVelocity": 1.3649764310629042,
+            "velocityX": 2.839032441252049,
+            "velocityY": -1.76233403161705,
+            "timestamp": 0.4104686100764613
+        },
+        {
+            "x": 1.6657539489096052,
+            "y": 3.7531140130982656,
+            "heading": 2.6643196178388515,
+            "angularVelocity": 6.274929563351661e-7,
+            "velocityX": 3.178745838783293,
+            "velocityY": -2.050905460597173,
+            "timestamp": 0.49256233209175354
+        },
+        {
+            "x": 1.9267089834441824,
+            "y": 3.584747484136064,
+            "heading": 2.664319669351516,
+            "angularVelocity": 6.274860319306578e-7,
+            "velocityX": 3.1787453185027585,
+            "velocityY": -2.05090626699614,
+            "timestamp": 0.5746560541070458
+        },
+        {
+            "x": 2.1876640582968827,
+            "y": 3.416381017663811,
+            "heading": 2.6643197208641496,
+            "angularVelocity": 6.274856686006067e-7,
+            "velocityX": 3.1787458096258594,
+            "velocityY": -2.0509055057936005,
+            "timestamp": 0.6567497761223381
+        },
+        {
+            "x": 2.4486192865071836,
+            "y": 3.248014788884269,
+            "heading": 2.6643197723767935,
+            "angularVelocity": 6.274857907572903e-7,
+            "velocityX": 3.178747677705374,
+            "velocityY": -2.050902610411298,
+            "timestamp": 0.7388434981376304
+        },
+        {
+            "x": 2.7195077051073344,
+            "y": 3.0961455009278414,
+            "heading": 2.664319841917229,
+            "angularVelocity": 8.470859127420906e-7,
+            "velocityX": 3.2997458508421667,
+            "velocityY": -1.849950084223712,
+            "timestamp": 0.8209372201529227
+        },
+        {
+            "x": 2.9903964357661867,
+            "y": 2.9442767695902643,
+            "heading": 2.664319911457928,
+            "angularVelocity": 8.470891223849179e-7,
+            "velocityX": 3.2997496520914464,
+            "velocityY": -1.8499433039384836,
+            "timestamp": 0.903030942168215
+        },
+        {
+            "x": 3.2612853045995296,
+            "y": 2.79240828471517,
+            "heading": 2.6643199809985263,
+            "angularVelocity": 8.470878995035866e-7,
+            "velocityX": 3.299751335222467,
+            "velocityY": -1.8499403017298282,
+            "timestamp": 0.9851246641835073
+        },
+        {
+            "x": 3.5321743153682585,
+            "y": 2.6405400530113767,
+            "heading": 2.664320050538881,
+            "angularVelocity": 8.47084928778718e-7,
+            "velocityX": 3.299753064165714,
+            "velocityY": -1.8499372177997218,
+            "timestamp": 1.0672183861987996
+        },
+        {
+            "x": 3.8030633934021476,
+            "y": 2.4886719412892284,
+            "heading": 2.6643201200792013,
+            "angularVelocity": 8.470845077302885e-7,
+            "velocityX": 3.299753883536049,
+            "velocityY": -1.8499357562793664,
+            "timestamp": 1.1493121082140918
+        },
+        {
+            "x": 4.073952519237506,
+            "y": 2.3368039148311976,
+            "heading": 2.664320189619518,
+            "angularVelocity": 8.470844698893479e-7,
+            "velocityX": 3.2997544658152695,
+            "velocityY": -1.8499347176601568,
+            "timestamp": 1.231405830229384
+        },
+        {
+            "x": 4.3448416924892905,
+            "y": 2.1849359729505413,
+            "heading": 2.6643202591598345,
+            "angularVelocity": 8.470844641828709e-7,
+            "velocityX": 3.2997550434042244,
+            "velocityY": -1.8499336874062895,
+            "timestamp": 1.3134995522446762
+        },
+        {
+            "x": 4.615730948551723,
+            "y": 2.0330681787806033,
+            "heading": 2.664320328700174,
+            "angularVelocity": 8.470847405524923e-7,
+            "velocityX": 3.29975605213723,
+            "velocityY": -1.8499318881125704,
+            "timestamp": 1.3955932742599684
+        },
+        {
+            "x": 4.889033051365783,
+            "y": 1.8855867361226037,
+            "heading": 2.6643204031763625,
+            "angularVelocity": 9.072093065949494e-7,
+            "velocityX": 3.3291474196181556,
+            "velocityY": -1.7965008655659056,
+            "timestamp": 1.4776869962752606
+        },
+        {
+            "x": 5.16486669963939,
+            "y": 1.742896114789829,
+            "heading": 2.6643204810266874,
+            "angularVelocity": 9.483103351737687e-7,
+            "velocityX": 3.359984679732577,
+            "velocityY": -1.738142915559301,
+            "timestamp": 1.5597807182905528
+        },
+        {
+            "x": 5.440736180585003,
+            "y": 1.6002747825074959,
+            "heading": 2.6643205588558216,
+            "angularVelocity": 9.480522059758943e-7,
+            "velocityX": 3.3604211646564863,
+            "velocityY": -1.7372988918174013,
+            "timestamp": 1.641874440305845
+        },
+        {
+            "x": 5.732995703254166,
+            "y": 1.4952548268359402,
+            "heading": 2.664320597611399,
+            "angularVelocity": 4.720894158515159e-7,
+            "velocityX": 3.5600715315931244,
+            "velocityY": -1.2792690243961995,
+            "timestamp": 1.7239681623211371
+        },
+        {
+            "x": 6.025255587661767,
+            "y": 1.3902358778499353,
+            "heading": 2.6643206363668037,
+            "angularVelocity": 4.720873108891009e-7,
+            "velocityX": 3.560075938001194,
+            "velocityY": -1.2792567617587365,
+            "timestamp": 1.8060618843364293
+        },
+        {
+            "x": 6.317515503776411,
+            "y": 1.2852170171023363,
+            "heading": 2.664320675122227,
+            "angularVelocity": 4.7208754093652347e-7,
+            "velocityX": 3.5600763242310083,
+            "velocityY": -1.2792556869091232,
+            "timestamp": 1.8881556063517215
+        },
+        {
+            "x": 6.60977544021274,
+            "y": 1.1801982129085715,
+            "heading": 2.6643207138776748,
+            "angularVelocity": 4.720878420422611e-7,
+            "velocityX": 3.5600765717735112,
+            "velocityY": -1.2792549980155836,
+            "timestamp": 1.9702493283670137
+        },
+        {
+            "x": 6.902035364394445,
+            "y": 1.0751793746113998,
+            "heading": 2.664320752633444,
+            "angularVelocity": 4.720917543940102e-7,
+            "velocityX": 3.5600764224974903,
+            "velocityY": -1.2792554134359897,
+            "timestamp": 2.052343050382306
+        },
+        {
+            "x": 7.1614900474496475,
+            "y": 0.9773985940984262,
+            "heading": 2.7732250681220454,
+            "angularVelocity": 1.3265851859940632,
+            "velocityX": 3.1604692379140946,
+            "velocityY": -1.1910871880648686,
+            "timestamp": 2.1344367723975983
+        },
+        {
+            "x": 7.369273961287434,
+            "y": 0.9006112993544645,
+            "heading": 2.9067869208138672,
+            "angularVelocity": 1.626943564173427,
+            "velocityX": 2.5310572932614863,
+            "velocityY": -0.935361351135448,
+            "timestamp": 2.2165304944128907
+        },
+        {
+            "x": 7.524785907770447,
+            "y": 0.8435839044250819,
+            "heading": 3.0191766997290386,
+            "angularVelocity": 1.3690423111067538,
+            "velocityX": 1.8943220341994518,
+            "velocityY": -0.6946620707337358,
+            "timestamp": 2.298624216428183
+        },
+        {
+            "x": 7.62830248590124,
+            "y": 0.8058044628378582,
+            "heading": 3.098670404043058,
+            "angularVelocity": 0.9683286658535433,
+            "velocityX": 1.2609560827502735,
+            "velocityY": -0.460198912411186,
+            "timestamp": 2.3807179384434756
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 0.5034440508038269,
+            "velocityX": 0.6297377293860565,
+            "velocityY": -0.22906091204337628,
+            "timestamp": 2.462811660458768
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 1.633672716236069e-30,
+            "velocityX": -5.394468608823283e-31,
+            "velocityY": 5.5679704387753575e-31,
+            "timestamp": 2.5449053824740604
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.traj
new file mode 100644
index 00000000..d8bd6114
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.traj	
@@ -0,0 +1,292 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": 2.1,
+            "angularVelocity": 0,
+            "velocityX": 8.526478185532948e-32,
+            "velocityY": -7.785685234118549e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 0.755735101962148,
+            "y": 4.33059726025757,
+            "heading": 2.1511666979053023,
+            "angularVelocity": 0.6232717514717016,
+            "velocityX": 0.5571083980530026,
+            "velocityY": -0.35816063665566134,
+            "timestamp": 0.08209372201529226
+        },
+        {
+            "x": 0.8475193720501324,
+            "y": 4.271815657764732,
+            "heading": 2.250112545454498,
+            "angularVelocity": 1.2052791019849762,
+            "velocityX": 1.1180424986808006,
+            "velocityY": -0.7160304229096619,
+            "timestamp": 0.16418744403058452
+        },
+        {
+            "x": 0.9858871774715989,
+            "y": 4.183669855686051,
+            "heading": 2.389658659666248,
+            "angularVelocity": 1.6998390472021188,
+            "velocityX": 1.6854858328349602,
+            "velocityY": -1.0737215942317955,
+            "timestamp": 0.2462811660458768
+        },
+        {
+            "x": 1.17173213163872,
+            "y": 4.066157035949835,
+            "heading": 2.5522635706365153,
+            "angularVelocity": 1.9807228491842224,
+            "velocityX": 2.2638144501780824,
+            "velocityY": -1.4314470930472174,
+            "timestamp": 0.32837488806116905
+        },
+        {
+            "x": 1.4047988716632624,
+            "y": 3.9214804758601747,
+            "heading": 2.664319566325619,
+            "angularVelocity": 1.3649764310629042,
+            "velocityX": 2.839032441252049,
+            "velocityY": -1.76233403161705,
+            "timestamp": 0.4104686100764613
+        },
+        {
+            "x": 1.6657539489096052,
+            "y": 3.7531140130982656,
+            "heading": 2.6643196178388515,
+            "angularVelocity": 6.274929563351661e-7,
+            "velocityX": 3.178745838783293,
+            "velocityY": -2.050905460597173,
+            "timestamp": 0.49256233209175354
+        },
+        {
+            "x": 1.9267089834441824,
+            "y": 3.584747484136064,
+            "heading": 2.664319669351516,
+            "angularVelocity": 6.274860319306578e-7,
+            "velocityX": 3.1787453185027585,
+            "velocityY": -2.05090626699614,
+            "timestamp": 0.5746560541070458
+        },
+        {
+            "x": 2.1876640582968827,
+            "y": 3.416381017663811,
+            "heading": 2.6643197208641496,
+            "angularVelocity": 6.274856686006067e-7,
+            "velocityX": 3.1787458096258594,
+            "velocityY": -2.0509055057936005,
+            "timestamp": 0.6567497761223381
+        },
+        {
+            "x": 2.4486192865071836,
+            "y": 3.248014788884269,
+            "heading": 2.6643197723767935,
+            "angularVelocity": 6.274857907572903e-7,
+            "velocityX": 3.178747677705374,
+            "velocityY": -2.050902610411298,
+            "timestamp": 0.7388434981376304
+        },
+        {
+            "x": 2.7195077051073344,
+            "y": 3.0961455009278414,
+            "heading": 2.664319841917229,
+            "angularVelocity": 8.470859127420906e-7,
+            "velocityX": 3.2997458508421667,
+            "velocityY": -1.849950084223712,
+            "timestamp": 0.8209372201529227
+        },
+        {
+            "x": 2.9903964357661867,
+            "y": 2.9442767695902643,
+            "heading": 2.664319911457928,
+            "angularVelocity": 8.470891223849179e-7,
+            "velocityX": 3.2997496520914464,
+            "velocityY": -1.8499433039384836,
+            "timestamp": 0.903030942168215
+        },
+        {
+            "x": 3.2612853045995296,
+            "y": 2.79240828471517,
+            "heading": 2.6643199809985263,
+            "angularVelocity": 8.470878995035866e-7,
+            "velocityX": 3.299751335222467,
+            "velocityY": -1.8499403017298282,
+            "timestamp": 0.9851246641835073
+        },
+        {
+            "x": 3.5321743153682585,
+            "y": 2.6405400530113767,
+            "heading": 2.664320050538881,
+            "angularVelocity": 8.47084928778718e-7,
+            "velocityX": 3.299753064165714,
+            "velocityY": -1.8499372177997218,
+            "timestamp": 1.0672183861987996
+        },
+        {
+            "x": 3.8030633934021476,
+            "y": 2.4886719412892284,
+            "heading": 2.6643201200792013,
+            "angularVelocity": 8.470845077302885e-7,
+            "velocityX": 3.299753883536049,
+            "velocityY": -1.8499357562793664,
+            "timestamp": 1.1493121082140918
+        },
+        {
+            "x": 4.073952519237506,
+            "y": 2.3368039148311976,
+            "heading": 2.664320189619518,
+            "angularVelocity": 8.470844698893479e-7,
+            "velocityX": 3.2997544658152695,
+            "velocityY": -1.8499347176601568,
+            "timestamp": 1.231405830229384
+        },
+        {
+            "x": 4.3448416924892905,
+            "y": 2.1849359729505413,
+            "heading": 2.6643202591598345,
+            "angularVelocity": 8.470844641828709e-7,
+            "velocityX": 3.2997550434042244,
+            "velocityY": -1.8499336874062895,
+            "timestamp": 1.3134995522446762
+        },
+        {
+            "x": 4.615730948551723,
+            "y": 2.0330681787806033,
+            "heading": 2.664320328700174,
+            "angularVelocity": 8.470847405524923e-7,
+            "velocityX": 3.29975605213723,
+            "velocityY": -1.8499318881125704,
+            "timestamp": 1.3955932742599684
+        },
+        {
+            "x": 4.889033051365783,
+            "y": 1.8855867361226037,
+            "heading": 2.6643204031763625,
+            "angularVelocity": 9.072093065949494e-7,
+            "velocityX": 3.3291474196181556,
+            "velocityY": -1.7965008655659056,
+            "timestamp": 1.4776869962752606
+        },
+        {
+            "x": 5.16486669963939,
+            "y": 1.742896114789829,
+            "heading": 2.6643204810266874,
+            "angularVelocity": 9.483103351737687e-7,
+            "velocityX": 3.359984679732577,
+            "velocityY": -1.738142915559301,
+            "timestamp": 1.5597807182905528
+        },
+        {
+            "x": 5.440736180585003,
+            "y": 1.6002747825074959,
+            "heading": 2.6643205588558216,
+            "angularVelocity": 9.480522059758943e-7,
+            "velocityX": 3.3604211646564863,
+            "velocityY": -1.7372988918174013,
+            "timestamp": 1.641874440305845
+        },
+        {
+            "x": 5.732995703254166,
+            "y": 1.4952548268359402,
+            "heading": 2.664320597611399,
+            "angularVelocity": 4.720894158515159e-7,
+            "velocityX": 3.5600715315931244,
+            "velocityY": -1.2792690243961995,
+            "timestamp": 1.7239681623211371
+        },
+        {
+            "x": 6.025255587661767,
+            "y": 1.3902358778499353,
+            "heading": 2.6643206363668037,
+            "angularVelocity": 4.720873108891009e-7,
+            "velocityX": 3.560075938001194,
+            "velocityY": -1.2792567617587365,
+            "timestamp": 1.8060618843364293
+        },
+        {
+            "x": 6.317515503776411,
+            "y": 1.2852170171023363,
+            "heading": 2.664320675122227,
+            "angularVelocity": 4.7208754093652347e-7,
+            "velocityX": 3.5600763242310083,
+            "velocityY": -1.2792556869091232,
+            "timestamp": 1.8881556063517215
+        },
+        {
+            "x": 6.60977544021274,
+            "y": 1.1801982129085715,
+            "heading": 2.6643207138776748,
+            "angularVelocity": 4.720878420422611e-7,
+            "velocityX": 3.5600765717735112,
+            "velocityY": -1.2792549980155836,
+            "timestamp": 1.9702493283670137
+        },
+        {
+            "x": 6.902035364394445,
+            "y": 1.0751793746113998,
+            "heading": 2.664320752633444,
+            "angularVelocity": 4.720917543940102e-7,
+            "velocityX": 3.5600764224974903,
+            "velocityY": -1.2792554134359897,
+            "timestamp": 2.052343050382306
+        },
+        {
+            "x": 7.1614900474496475,
+            "y": 0.9773985940984262,
+            "heading": 2.7732250681220454,
+            "angularVelocity": 1.3265851859940632,
+            "velocityX": 3.1604692379140946,
+            "velocityY": -1.1910871880648686,
+            "timestamp": 2.1344367723975983
+        },
+        {
+            "x": 7.369273961287434,
+            "y": 0.9006112993544645,
+            "heading": 2.9067869208138672,
+            "angularVelocity": 1.626943564173427,
+            "velocityX": 2.5310572932614863,
+            "velocityY": -0.935361351135448,
+            "timestamp": 2.2165304944128907
+        },
+        {
+            "x": 7.524785907770447,
+            "y": 0.8435839044250819,
+            "heading": 3.0191766997290386,
+            "angularVelocity": 1.3690423111067538,
+            "velocityX": 1.8943220341994518,
+            "velocityY": -0.6946620707337358,
+            "timestamp": 2.298624216428183
+        },
+        {
+            "x": 7.62830248590124,
+            "y": 0.8058044628378582,
+            "heading": 3.098670404043058,
+            "angularVelocity": 0.9683286658535433,
+            "velocityX": 1.2609560827502735,
+            "velocityY": -0.460198912411186,
+            "timestamp": 2.3807179384434756
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 0.5034440508038269,
+            "velocityX": 0.6297377293860565,
+            "velocityY": -0.22906091204337628,
+            "timestamp": 2.462811660458768
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 1.633672716236069e-30,
+            "velocityX": -5.394468608823283e-31,
+            "velocityY": 5.5679704387753575e-31,
+            "timestamp": 2.5449053824740604
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.1.traj
new file mode 100644
index 00000000..5a8bacb1
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.1.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": 2.1,
+            "angularVelocity": 1.7952667234276065e-30,
+            "velocityX": 8.378323076428447e-32,
+            "velocityY": 1.4697485073236794e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7269607278962015,
+            "y": 4.384959045157939,
+            "heading": 2.138406592301522,
+            "angularVelocity": 0.6202480809957288,
+            "velocityX": 0.2739076366713438,
+            "velocityY": 0.4030766317709414,
+            "timestamp": 0.06192133999006512
+        },
+        {
+            "x": 0.7608491761808356,
+            "y": 4.435120384688249,
+            "heading": 2.213462658415359,
+            "angularVelocity": 1.2121195394976814,
+            "velocityX": 0.5472822178924311,
+            "velocityY": 0.8100816219151135,
+            "timestamp": 0.12384267998013024
+        },
+        {
+            "x": 0.8116352150421299,
+            "y": 4.5108343869767795,
+            "heading": 2.3220976741276163,
+            "angularVelocity": 1.7544035017602526,
+            "velocityX": 0.8201702170760925,
+            "velocityY": 1.2227448937745646,
+            "timestamp": 0.18576401997019537
+        },
+        {
+            "x": 0.879352990275961,
+            "y": 4.612608489350272,
+            "heading": 2.458161516132951,
+            "angularVelocity": 2.197365916615584,
+            "velocityX": 1.093609654518071,
+            "velocityY": 1.6436030355580518,
+            "timestamp": 0.24768535996026048
+        },
+        {
+            "x": 0.9641073070497683,
+            "y": 4.741095934499406,
+            "heading": 2.609605752410294,
+            "angularVelocity": 2.445751921738789,
+            "velocityX": 1.3687416452454926,
+            "velocityY": 2.075010734098273,
+            "timestamp": 0.3096066999503256
+        },
+        {
+            "x": 1.065422526066935,
+            "y": 4.896666065794743,
+            "heading": 2.7474397378967583,
+            "angularVelocity": 2.2259528864940465,
+            "velocityX": 1.6361922890141296,
+            "velocityY": 2.5123831512738084,
+            "timestamp": 0.3715280399403907
+        },
+        {
+            "x": 1.1791068919270769,
+            "y": 5.075184614148285,
+            "heading": 2.811143860874752,
+            "angularVelocity": 1.0287910918629104,
+            "velocityX": 1.8359480895985416,
+            "velocityY": 2.882989101692323,
+            "timestamp": 0.4334493799304558
+        },
+        {
+            "x": 1.309522757298702,
+            "y": 5.269766971770384,
+            "heading": 2.8111439317105513,
+            "angularVelocity": 0.0000011439642533999806,
+            "velocityX": 2.1061537975849616,
+            "velocityY": 3.142411931869009,
+            "timestamp": 0.4953707199205209
+        },
+        {
+            "x": 1.4399391293197474,
+            "y": 5.464348989818422,
+            "heading": 2.811144002543952,
+            "angularVelocity": 0.0000011439255168645237,
+            "velocityX": 2.1061619797305715,
+            "velocityY": 3.142406447910498,
+            "timestamp": 0.5572920599105861
+        },
+        {
+            "x": 1.570355539071906,
+            "y": 5.658930982577611,
+            "heading": 2.811144073377348,
+            "angularVelocity": 0.0000011439254352819296,
+            "velocityX": 2.1061625890699887,
+            "velocityY": 3.1424060395076645,
+            "timestamp": 0.6192133999006512
+        },
+        {
+            "x": 1.7007719870139304,
+            "y": 5.853512949740463,
+            "heading": 2.811144144210739,
+            "angularVelocity": 0.0000011439253582531217,
+            "velocityX": 2.1061632058180395,
+            "velocityY": 3.1424056261390936,
+            "timestamp": 0.6811347398907163
+        },
+        {
+            "x": 1.8311879242943474,
+            "y": 6.048095259166571,
+            "heading": 2.811144215047033,
+            "angularVelocity": 0.0000011439722415453888,
+            "velocityX": 2.1061549588775286,
+            "velocityY": 3.1424111535268193,
+            "timestamp": 0.7430560798807814
+        },
+        {
+            "x": 1.9446720292676574,
+            "y": 6.226098534814738,
+            "heading": 2.87431577200777,
+            "angularVelocity": 1.0201904056157727,
+            "velocityX": 1.8327139721381605,
+            "velocityY": 2.8746676941540104,
+            "timestamp": 0.8049774198708465
+        },
+        {
+            "x": 2.045803800007245,
+            "y": 6.3800371688336455,
+            "heading": 3.00267788576032,
+            "angularVelocity": 2.072986692037753,
+            "velocityX": 1.6332296871452403,
+            "velocityY": 2.486035251233359,
+            "timestamp": 0.8668987598609116
+        },
+        {
+            "x": 2.1306920593857335,
+            "y": 6.5074855802636336,
+            "heading": 3.157852124619465,
+            "angularVelocity": 2.5059896779372313,
+            "velocityX": 1.370904754194726,
+            "velocityY": 2.0582308369042988,
+            "timestamp": 0.9288200998509767
+        },
+        {
+            "x": 2.1984321789198162,
+            "y": 6.6082437599743225,
+            "heading": 3.2989222381105456,
+            "angularVelocity": 2.278214804681454,
+            "velocityX": 1.0939705042712429,
+            "velocityY": 1.6271963708610766,
+            "timestamp": 0.9907414398410418
+        },
+        {
+            "x": 2.249205760326253,
+            "y": 6.6830338505565665,
+            "heading": 3.412569338792277,
+            "angularVelocity": 1.8353462748055114,
+            "velocityX": 0.8199690351433494,
+            "velocityY": 1.2078241619810486,
+            "timestamp": 1.052662779831107
+        },
+        {
+            "x": 2.2830654942840813,
+            "y": 6.732454760660193,
+            "heading": 3.492119269429147,
+            "angularVelocity": 1.2846933003974643,
+            "velocityX": 0.5468184952596451,
+            "velocityY": 0.7981240411069244,
+            "timestamp": 1.1145841198211721
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 0.66319685195811,
+            "velocityX": 0.2734841610119513,
+            "velocityY": 0.3963938658908936,
+            "timestamp": 1.1765054598112372
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": -1.1911850535673714e-30,
+            "velocityX": 0,
+            "velocityY": -7.498214836328326e-32,
+            "timestamp": 1.2384267998013023
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.traj
new file mode 100644
index 00000000..5a8bacb1
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": 2.1,
+            "angularVelocity": 1.7952667234276065e-30,
+            "velocityX": 8.378323076428447e-32,
+            "velocityY": 1.4697485073236794e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7269607278962015,
+            "y": 4.384959045157939,
+            "heading": 2.138406592301522,
+            "angularVelocity": 0.6202480809957288,
+            "velocityX": 0.2739076366713438,
+            "velocityY": 0.4030766317709414,
+            "timestamp": 0.06192133999006512
+        },
+        {
+            "x": 0.7608491761808356,
+            "y": 4.435120384688249,
+            "heading": 2.213462658415359,
+            "angularVelocity": 1.2121195394976814,
+            "velocityX": 0.5472822178924311,
+            "velocityY": 0.8100816219151135,
+            "timestamp": 0.12384267998013024
+        },
+        {
+            "x": 0.8116352150421299,
+            "y": 4.5108343869767795,
+            "heading": 2.3220976741276163,
+            "angularVelocity": 1.7544035017602526,
+            "velocityX": 0.8201702170760925,
+            "velocityY": 1.2227448937745646,
+            "timestamp": 0.18576401997019537
+        },
+        {
+            "x": 0.879352990275961,
+            "y": 4.612608489350272,
+            "heading": 2.458161516132951,
+            "angularVelocity": 2.197365916615584,
+            "velocityX": 1.093609654518071,
+            "velocityY": 1.6436030355580518,
+            "timestamp": 0.24768535996026048
+        },
+        {
+            "x": 0.9641073070497683,
+            "y": 4.741095934499406,
+            "heading": 2.609605752410294,
+            "angularVelocity": 2.445751921738789,
+            "velocityX": 1.3687416452454926,
+            "velocityY": 2.075010734098273,
+            "timestamp": 0.3096066999503256
+        },
+        {
+            "x": 1.065422526066935,
+            "y": 4.896666065794743,
+            "heading": 2.7474397378967583,
+            "angularVelocity": 2.2259528864940465,
+            "velocityX": 1.6361922890141296,
+            "velocityY": 2.5123831512738084,
+            "timestamp": 0.3715280399403907
+        },
+        {
+            "x": 1.1791068919270769,
+            "y": 5.075184614148285,
+            "heading": 2.811143860874752,
+            "angularVelocity": 1.0287910918629104,
+            "velocityX": 1.8359480895985416,
+            "velocityY": 2.882989101692323,
+            "timestamp": 0.4334493799304558
+        },
+        {
+            "x": 1.309522757298702,
+            "y": 5.269766971770384,
+            "heading": 2.8111439317105513,
+            "angularVelocity": 0.0000011439642533999806,
+            "velocityX": 2.1061537975849616,
+            "velocityY": 3.142411931869009,
+            "timestamp": 0.4953707199205209
+        },
+        {
+            "x": 1.4399391293197474,
+            "y": 5.464348989818422,
+            "heading": 2.811144002543952,
+            "angularVelocity": 0.0000011439255168645237,
+            "velocityX": 2.1061619797305715,
+            "velocityY": 3.142406447910498,
+            "timestamp": 0.5572920599105861
+        },
+        {
+            "x": 1.570355539071906,
+            "y": 5.658930982577611,
+            "heading": 2.811144073377348,
+            "angularVelocity": 0.0000011439254352819296,
+            "velocityX": 2.1061625890699887,
+            "velocityY": 3.1424060395076645,
+            "timestamp": 0.6192133999006512
+        },
+        {
+            "x": 1.7007719870139304,
+            "y": 5.853512949740463,
+            "heading": 2.811144144210739,
+            "angularVelocity": 0.0000011439253582531217,
+            "velocityX": 2.1061632058180395,
+            "velocityY": 3.1424056261390936,
+            "timestamp": 0.6811347398907163
+        },
+        {
+            "x": 1.8311879242943474,
+            "y": 6.048095259166571,
+            "heading": 2.811144215047033,
+            "angularVelocity": 0.0000011439722415453888,
+            "velocityX": 2.1061549588775286,
+            "velocityY": 3.1424111535268193,
+            "timestamp": 0.7430560798807814
+        },
+        {
+            "x": 1.9446720292676574,
+            "y": 6.226098534814738,
+            "heading": 2.87431577200777,
+            "angularVelocity": 1.0201904056157727,
+            "velocityX": 1.8327139721381605,
+            "velocityY": 2.8746676941540104,
+            "timestamp": 0.8049774198708465
+        },
+        {
+            "x": 2.045803800007245,
+            "y": 6.3800371688336455,
+            "heading": 3.00267788576032,
+            "angularVelocity": 2.072986692037753,
+            "velocityX": 1.6332296871452403,
+            "velocityY": 2.486035251233359,
+            "timestamp": 0.8668987598609116
+        },
+        {
+            "x": 2.1306920593857335,
+            "y": 6.5074855802636336,
+            "heading": 3.157852124619465,
+            "angularVelocity": 2.5059896779372313,
+            "velocityX": 1.370904754194726,
+            "velocityY": 2.0582308369042988,
+            "timestamp": 0.9288200998509767
+        },
+        {
+            "x": 2.1984321789198162,
+            "y": 6.6082437599743225,
+            "heading": 3.2989222381105456,
+            "angularVelocity": 2.278214804681454,
+            "velocityX": 1.0939705042712429,
+            "velocityY": 1.6271963708610766,
+            "timestamp": 0.9907414398410418
+        },
+        {
+            "x": 2.249205760326253,
+            "y": 6.6830338505565665,
+            "heading": 3.412569338792277,
+            "angularVelocity": 1.8353462748055114,
+            "velocityX": 0.8199690351433494,
+            "velocityY": 1.2078241619810486,
+            "timestamp": 1.052662779831107
+        },
+        {
+            "x": 2.2830654942840813,
+            "y": 6.732454760660193,
+            "heading": 3.492119269429147,
+            "angularVelocity": 1.2846933003974643,
+            "velocityX": 0.5468184952596451,
+            "velocityY": 0.7981240411069244,
+            "timestamp": 1.1145841198211721
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 0.66319685195811,
+            "velocityX": 0.2734841610119513,
+            "velocityY": 0.3963938658908936,
+            "timestamp": 1.1765054598112372
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": -1.1911850535673714e-30,
+            "velocityX": 0,
+            "velocityY": -7.498214836328326e-32,
+            "timestamp": 1.2384267998013023
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.1.traj
new file mode 100644
index 00000000..67ac392d
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": 2.1,
+            "angularVelocity": 8.845615831720266e-31,
+            "velocityX": 5.520878611448458e-31,
+            "velocityY": 4.542986326160959e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7374422286617038,
+            "y": 4.3811878915004145,
+            "heading": 2.1328463142733396,
+            "angularVelocity": 0.5012247413747498,
+            "velocityX": 0.41876004258027855,
+            "velocityY": 0.3233207643693322,
+            "timestamp": 0.06553210877669326
+        },
+        {
+            "x": 0.7924456920447251,
+            "y": 4.423718037322063,
+            "heading": 2.19563146481745,
+            "angularVelocity": 0.9580822548845105,
+            "velocityX": 0.8393360813467762,
+            "velocityY": 0.6489970583210544,
+            "timestamp": 0.13106421755338651
+        },
+        {
+            "x": 0.8751785876629536,
+            "y": 4.487773457465306,
+            "heading": 2.284091563401004,
+            "angularVelocity": 1.3498741339912115,
+            "velocityX": 1.262478762893885,
+            "velocityY": 0.9774661816777632,
+            "timestamp": 0.19659632633007978
+        },
+        {
+            "x": 0.985868892669873,
+            "y": 4.573562834319481,
+            "heading": 2.3916808862774843,
+            "angularVelocity": 1.641780264436542,
+            "velocityX": 1.6891003062957237,
+            "velocityY": 1.309119734671004,
+            "timestamp": 0.26212843510677303
+        },
+        {
+            "x": 1.1247521909380378,
+            "y": 4.681315175779475,
+            "heading": 2.507240555710109,
+            "angularVelocity": 1.7634053228228717,
+            "velocityX": 2.1193167877661256,
+            "velocityY": 1.6442678783186102,
+            "timestamp": 0.32766054388346627
+        },
+        {
+            "x": 1.291492862531941,
+            "y": 4.810942835643569,
+            "heading": 2.6039577624805075,
+            "angularVelocity": 1.4758750874318616,
+            "velocityX": 2.5444118113470626,
+            "velocityY": 1.9780785676502115,
+            "timestamp": 0.3931926526601595
+        },
+        {
+            "x": 1.4800025601939901,
+            "y": 4.960730002903915,
+            "heading": 2.6228917229305413,
+            "angularVelocity": 0.2889264637363189,
+            "velocityX": 2.8766005120392,
+            "velocityY": 2.28570650413168,
+            "timestamp": 0.45872476143685276
+        },
+        {
+            "x": 1.6682727616271493,
+            "y": 5.110599963848934,
+            "heading": 2.6420440088534156,
+            "angularVelocity": 0.2922580438871624,
+            "velocityX": 2.8729458726059685,
+            "velocityY": 2.2869699105167616,
+            "timestamp": 0.5242568702135461
+        },
+        {
+            "x": 1.8350043100801503,
+            "y": 5.239747952271642,
+            "heading": 2.7356929069056295,
+            "angularVelocity": 1.429053631881899,
+            "velocityX": 2.544272594998539,
+            "velocityY": 1.9707589277004427,
+            "timestamp": 0.5897889789902393
+        },
+        {
+            "x": 1.9739563679504606,
+            "y": 5.347167856813287,
+            "heading": 2.8514085217331835,
+            "angularVelocity": 1.765785002003577,
+            "velocityX": 2.1203660383309844,
+            "velocityY": 1.6391949923004236,
+            "timestamp": 0.6553210877669325
+        },
+        {
+            "x": 2.0846918002256074,
+            "y": 5.4326634298654595,
+            "heading": 2.9588833399365435,
+            "angularVelocity": 1.640032958035698,
+            "velocityX": 1.6897889346501833,
+            "velocityY": 1.304636378229562,
+            "timestamp": 0.7208531965436258
+        },
+        {
+            "x": 2.1674771737861436,
+            "y": 5.496490038501947,
+            "heading": 3.046413104094783,
+            "angularVelocity": 1.3356775143083182,
+            "velocityX": 1.2632795602936622,
+            "velocityY": 0.9739745878464279,
+            "timestamp": 0.786385305320319
+        },
+        {
+            "x": 2.2225294464291743,
+            "y": 5.538874362150552,
+            "heading": 3.1080101238304096,
+            "angularVelocity": 0.9399517409934369,
+            "velocityX": 0.8400808957732031,
+            "velocityY": 0.6467718564197722,
+            "timestamp": 0.8519174140970123
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0.4881557570289305,
+            "velocityX": 0.41919227205757975,
+            "velocityY": 0.3223707926359362,
+            "timestamp": 0.9174495228737055
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 7.973690679262731e-27,
+            "velocityX": -1.618419533015103e-29,
+            "velocityY": -2.1923403816587234e-30,
+            "timestamp": 0.9829816316503988
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.traj
new file mode 100644
index 00000000..67ac392d
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": 2.1,
+            "angularVelocity": 8.845615831720266e-31,
+            "velocityX": 5.520878611448458e-31,
+            "velocityY": 4.542986326160959e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7374422286617038,
+            "y": 4.3811878915004145,
+            "heading": 2.1328463142733396,
+            "angularVelocity": 0.5012247413747498,
+            "velocityX": 0.41876004258027855,
+            "velocityY": 0.3233207643693322,
+            "timestamp": 0.06553210877669326
+        },
+        {
+            "x": 0.7924456920447251,
+            "y": 4.423718037322063,
+            "heading": 2.19563146481745,
+            "angularVelocity": 0.9580822548845105,
+            "velocityX": 0.8393360813467762,
+            "velocityY": 0.6489970583210544,
+            "timestamp": 0.13106421755338651
+        },
+        {
+            "x": 0.8751785876629536,
+            "y": 4.487773457465306,
+            "heading": 2.284091563401004,
+            "angularVelocity": 1.3498741339912115,
+            "velocityX": 1.262478762893885,
+            "velocityY": 0.9774661816777632,
+            "timestamp": 0.19659632633007978
+        },
+        {
+            "x": 0.985868892669873,
+            "y": 4.573562834319481,
+            "heading": 2.3916808862774843,
+            "angularVelocity": 1.641780264436542,
+            "velocityX": 1.6891003062957237,
+            "velocityY": 1.309119734671004,
+            "timestamp": 0.26212843510677303
+        },
+        {
+            "x": 1.1247521909380378,
+            "y": 4.681315175779475,
+            "heading": 2.507240555710109,
+            "angularVelocity": 1.7634053228228717,
+            "velocityX": 2.1193167877661256,
+            "velocityY": 1.6442678783186102,
+            "timestamp": 0.32766054388346627
+        },
+        {
+            "x": 1.291492862531941,
+            "y": 4.810942835643569,
+            "heading": 2.6039577624805075,
+            "angularVelocity": 1.4758750874318616,
+            "velocityX": 2.5444118113470626,
+            "velocityY": 1.9780785676502115,
+            "timestamp": 0.3931926526601595
+        },
+        {
+            "x": 1.4800025601939901,
+            "y": 4.960730002903915,
+            "heading": 2.6228917229305413,
+            "angularVelocity": 0.2889264637363189,
+            "velocityX": 2.8766005120392,
+            "velocityY": 2.28570650413168,
+            "timestamp": 0.45872476143685276
+        },
+        {
+            "x": 1.6682727616271493,
+            "y": 5.110599963848934,
+            "heading": 2.6420440088534156,
+            "angularVelocity": 0.2922580438871624,
+            "velocityX": 2.8729458726059685,
+            "velocityY": 2.2869699105167616,
+            "timestamp": 0.5242568702135461
+        },
+        {
+            "x": 1.8350043100801503,
+            "y": 5.239747952271642,
+            "heading": 2.7356929069056295,
+            "angularVelocity": 1.429053631881899,
+            "velocityX": 2.544272594998539,
+            "velocityY": 1.9707589277004427,
+            "timestamp": 0.5897889789902393
+        },
+        {
+            "x": 1.9739563679504606,
+            "y": 5.347167856813287,
+            "heading": 2.8514085217331835,
+            "angularVelocity": 1.765785002003577,
+            "velocityX": 2.1203660383309844,
+            "velocityY": 1.6391949923004236,
+            "timestamp": 0.6553210877669325
+        },
+        {
+            "x": 2.0846918002256074,
+            "y": 5.4326634298654595,
+            "heading": 2.9588833399365435,
+            "angularVelocity": 1.640032958035698,
+            "velocityX": 1.6897889346501833,
+            "velocityY": 1.304636378229562,
+            "timestamp": 0.7208531965436258
+        },
+        {
+            "x": 2.1674771737861436,
+            "y": 5.496490038501947,
+            "heading": 3.046413104094783,
+            "angularVelocity": 1.3356775143083182,
+            "velocityX": 1.2632795602936622,
+            "velocityY": 0.9739745878464279,
+            "timestamp": 0.786385305320319
+        },
+        {
+            "x": 2.2225294464291743,
+            "y": 5.538874362150552,
+            "heading": 3.1080101238304096,
+            "angularVelocity": 0.9399517409934369,
+            "velocityX": 0.8400808957732031,
+            "velocityY": 0.6467718564197722,
+            "timestamp": 0.8519174140970123
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0.4881557570289305,
+            "velocityX": 0.41919227205757975,
+            "velocityY": 0.3223707926359362,
+            "timestamp": 0.9174495228737055
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 7.973690679262731e-27,
+            "velocityX": -1.618419533015103e-29,
+            "velocityY": -2.1923403816587234e-30,
+            "timestamp": 0.9829816316503988
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.1.traj
new file mode 100644
index 00000000..d9a920a4
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.1.traj	
@@ -0,0 +1,121 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": 2.1,
+            "angularVelocity": -5.8020277899403155e-31,
+            "velocityX": 1.3777732804976719e-32,
+            "velocityY": 5.338313673616136e-29,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7541175647990941,
+            "y": 4.360036481014467,
+            "heading": 2.123748591233672,
+            "angularVelocity": 0.32465550054626485,
+            "velocityX": 0.603109883101075,
+            "velocityY": 0.0004987142982711039,
+            "timestamp": 0.07315012742330113
+        },
+        {
+            "x": 0.8424087590063745,
+            "y": 4.360099698646948,
+            "heading": 2.1698601870465493,
+            "angularVelocity": 0.6303693163234944,
+            "velocityX": 1.2069861983474726,
+            "velocityY": 0.0008642176672118523,
+            "timestamp": 0.14630025484660225
+        },
+        {
+            "x": 0.9749550737228709,
+            "y": 4.360177642554779,
+            "heading": 2.23617090700284,
+            "angularVelocity": 0.906501769606733,
+            "velocityX": 1.8119765390084532,
+            "velocityY": 0.0010655334525686096,
+            "timestamp": 0.21945038226990338
+        },
+        {
+            "x": 1.1518842000389728,
+            "y": 4.360257435104989,
+            "heading": 2.318743489919177,
+            "angularVelocity": 1.1288098302053016,
+            "velocityX": 2.4187124827857356,
+            "velocityY": 0.0010908053480699021,
+            "timestamp": 0.2926005096932045
+        },
+        {
+            "x": 1.3733949578522506,
+            "y": 4.360320997027151,
+            "heading": 2.4076375641139776,
+            "angularVelocity": 1.2152278789650626,
+            "velocityX": 3.028166397188238,
+            "velocityY": 0.0008689242848102628,
+            "timestamp": 0.36575063711650563
+        },
+        {
+            "x": 1.6366157123115315,
+            "y": 4.3597910751743765,
+            "heading": 2.4407985098803153,
+            "angularVelocity": 0.45332724541279523,
+            "velocityX": 3.5983635809144485,
+            "velocityY": -0.007244305258449855,
+            "timestamp": 0.43890076453980675
+        },
+        {
+            "x": 1.8581231700320338,
+            "y": 4.359897336997892,
+            "heading": 2.530170275911139,
+            "angularVelocity": 1.2217581729403981,
+            "velocityX": 3.0281212832164655,
+            "velocityY": 0.0014526539769375909,
+            "timestamp": 0.5120508919631079
+        },
+        {
+            "x": 2.0350502325675994,
+            "y": 4.359957426055228,
+            "heading": 2.6131109520937392,
+            "angularVelocity": 1.1338418551586247,
+            "velocityX": 2.4186842698404125,
+            "velocityY": 0.000821448430104839,
+            "timestamp": 0.585201019386409
+        },
+        {
+            "x": 2.1675938018574246,
+            "y": 4.359986783385126,
+            "heading": 2.6797755559230114,
+            "angularVelocity": 0.9113395448181003,
+            "velocityX": 1.8119390076086361,
+            "velocityY": 0.00040132985313166364,
+            "timestamp": 0.6583511468097101
+        },
+        {
+            "x": 2.255882928551814,
+            "y": 4.359997887713372,
+            "heading": 2.726140403287784,
+            "angularVelocity": 0.6338313957609201,
+            "velocityX": 1.2069579343789958,
+            "velocityY": 0.00015180189885440488,
+            "timestamp": 0.7315012742330111
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0.3261730027365948,
+            "velocityX": 0.6031031387394808,
+            "velocityY": 0.000028876048490911067,
+            "timestamp": 0.8046514016563122
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 1.0612763050961144e-31,
+            "velocityX": 9.233550959236252e-32,
+            "velocityY": 1.483165202160369e-28,
+            "timestamp": 0.8778015290796133
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.traj
new file mode 100644
index 00000000..d9a920a4
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.traj	
@@ -0,0 +1,121 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": 2.1,
+            "angularVelocity": -5.8020277899403155e-31,
+            "velocityX": 1.3777732804976719e-32,
+            "velocityY": 5.338313673616136e-29,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7541175647990941,
+            "y": 4.360036481014467,
+            "heading": 2.123748591233672,
+            "angularVelocity": 0.32465550054626485,
+            "velocityX": 0.603109883101075,
+            "velocityY": 0.0004987142982711039,
+            "timestamp": 0.07315012742330113
+        },
+        {
+            "x": 0.8424087590063745,
+            "y": 4.360099698646948,
+            "heading": 2.1698601870465493,
+            "angularVelocity": 0.6303693163234944,
+            "velocityX": 1.2069861983474726,
+            "velocityY": 0.0008642176672118523,
+            "timestamp": 0.14630025484660225
+        },
+        {
+            "x": 0.9749550737228709,
+            "y": 4.360177642554779,
+            "heading": 2.23617090700284,
+            "angularVelocity": 0.906501769606733,
+            "velocityX": 1.8119765390084532,
+            "velocityY": 0.0010655334525686096,
+            "timestamp": 0.21945038226990338
+        },
+        {
+            "x": 1.1518842000389728,
+            "y": 4.360257435104989,
+            "heading": 2.318743489919177,
+            "angularVelocity": 1.1288098302053016,
+            "velocityX": 2.4187124827857356,
+            "velocityY": 0.0010908053480699021,
+            "timestamp": 0.2926005096932045
+        },
+        {
+            "x": 1.3733949578522506,
+            "y": 4.360320997027151,
+            "heading": 2.4076375641139776,
+            "angularVelocity": 1.2152278789650626,
+            "velocityX": 3.028166397188238,
+            "velocityY": 0.0008689242848102628,
+            "timestamp": 0.36575063711650563
+        },
+        {
+            "x": 1.6366157123115315,
+            "y": 4.3597910751743765,
+            "heading": 2.4407985098803153,
+            "angularVelocity": 0.45332724541279523,
+            "velocityX": 3.5983635809144485,
+            "velocityY": -0.007244305258449855,
+            "timestamp": 0.43890076453980675
+        },
+        {
+            "x": 1.8581231700320338,
+            "y": 4.359897336997892,
+            "heading": 2.530170275911139,
+            "angularVelocity": 1.2217581729403981,
+            "velocityX": 3.0281212832164655,
+            "velocityY": 0.0014526539769375909,
+            "timestamp": 0.5120508919631079
+        },
+        {
+            "x": 2.0350502325675994,
+            "y": 4.359957426055228,
+            "heading": 2.6131109520937392,
+            "angularVelocity": 1.1338418551586247,
+            "velocityX": 2.4186842698404125,
+            "velocityY": 0.000821448430104839,
+            "timestamp": 0.585201019386409
+        },
+        {
+            "x": 2.1675938018574246,
+            "y": 4.359986783385126,
+            "heading": 2.6797755559230114,
+            "angularVelocity": 0.9113395448181003,
+            "velocityX": 1.8119390076086361,
+            "velocityY": 0.00040132985313166364,
+            "timestamp": 0.6583511468097101
+        },
+        {
+            "x": 2.255882928551814,
+            "y": 4.359997887713372,
+            "heading": 2.726140403287784,
+            "angularVelocity": 0.6338313957609201,
+            "velocityX": 1.2069579343789958,
+            "velocityY": 0.00015180189885440488,
+            "timestamp": 0.7315012742330111
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0.3261730027365948,
+            "velocityX": 0.6031031387394808,
+            "velocityY": 0.000028876048490911067,
+            "timestamp": 0.8046514016563122
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 1.0612763050961144e-31,
+            "velocityX": 9.233550959236252e-32,
+            "velocityY": 1.483165202160369e-28,
+            "timestamp": 0.8778015290796133
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.1.traj
new file mode 100644
index 00000000..0c6c619f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": -1.8545515236340358e-32,
+            "velocityX": 2.346645959584709e-34,
+            "velocityY": 1.5264694726605652e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3666998117674303,
+            "y": 6.765820939449683,
+            "heading": -2.7737296132590643,
+            "angularVelocity": -0.2635310200357621,
+            "velocityX": 0.7407398190339184,
+            "velocityY": 0.09796161216242502,
+            "timestamp": 0.09004485792922705
+        },
+        {
+            "x": 2.500184310330562,
+            "y": 6.783480740643308,
+            "heading": -2.817788411123353,
+            "angularVelocity": -0.4892983217200214,
+            "velocityX": 1.4824222241324077,
+            "velocityY": 0.19612226172321479,
+            "timestamp": 0.1800897158584541
+        },
+        {
+            "x": 2.700568444033807,
+            "y": 6.810009643084086,
+            "heading": -2.8763096511057067,
+            "angularVelocity": -0.6499120696969705,
+            "velocityX": 2.2253811967890735,
+            "velocityY": 0.294618738380691,
+            "timestamp": 0.27013457378768113
+        },
+        {
+            "x": 2.9679652961316036,
+            "y": 6.845459467588075,
+            "heading": -2.9368283914330156,
+            "angularVelocity": -0.6720954612963594,
+            "velocityX": 2.969596024105716,
+            "velocityY": 0.3936907150417343,
+            "timestamp": 0.3601794317169082
+        },
+        {
+            "x": 3.3011724137612752,
+            "y": 6.889825844077578,
+            "heading": -2.9514563142993295,
+            "angularVelocity": -0.1624515069790128,
+            "velocityX": 3.7004569199449846,
+            "velocityY": 0.49271415947342084,
+            "timestamp": 0.45022428964613526
+        },
+        {
+            "x": 3.638895175262513,
+            "y": 6.93426783648319,
+            "heading": -2.9514563336020103,
+            "angularVelocity": -2.143673866142303e-7,
+            "velocityX": 3.750605745490536,
+            "velocityY": 0.49355391776554697,
+            "timestamp": 0.5402691475753623
+        },
+        {
+            "x": 3.976617946407746,
+            "y": 6.9787097556023205,
+            "heading": -2.9514563529046813,
+            "angularVelocity": -2.1436727225160337e-7,
+            "velocityX": 3.75060585259265,
+            "velocityY": 0.4935531038769741,
+            "timestamp": 0.6303140055045893
+        },
+        {
+            "x": 4.314340727620448,
+            "y": 7.023151598216748,
+            "heading": -2.9514563722073515,
+            "angularVelocity": -2.1436726735785024e-7,
+            "velocityX": 3.7506059643976966,
+            "velocityY": 0.49355225424817656,
+            "timestamp": 0.7203588634338163
+        },
+        {
+            "x": 4.65206351967383,
+            "y": 7.06759335845053,
+            "heading": -2.951456391510026,
+            "angularVelocity": -2.1436731957684306e-7,
+            "velocityX": 3.7506060847896903,
+            "velocityY": 0.49355133936368123,
+            "timestamp": 0.8104037213630433
+        },
+        {
+            "x": 4.989786323250196,
+            "y": 7.11203503111852,
+            "heading": -2.9514564108127086,
+            "angularVelocity": -2.143674043301644e-7,
+            "velocityX": 3.7506062127590543,
+            "velocityY": 0.4935503668951324,
+            "timestamp": 0.9004485792922703
+        },
+        {
+            "x": 5.327509138668733,
+            "y": 7.15647661379494,
+            "heading": -2.951456430115396,
+            "angularVelocity": -2.1436746122645403e-7,
+            "velocityX": 3.7506063442731943,
+            "velocityY": 0.4935493674869138,
+            "timestamp": 0.9904934372214973
+        },
+        {
+            "x": 5.66523196589638,
+            "y": 7.200918106730859,
+            "heading": -2.95145644941808,
+            "angularVelocity": -2.143674220103917e-7,
+            "velocityX": 3.7506064754201436,
+            "velocityY": 0.49354837086698544,
+            "timestamp": 1.0805382951507243
+        },
+        {
+            "x": 6.002954804577347,
+            "y": 7.24535951262982,
+            "heading": -2.9514564687207536,
+            "angularVelocity": -2.1436730521441728e-7,
+            "velocityX": 3.7506066026158704,
+            "velocityY": 0.4935474042714428,
+            "timestamp": 1.1705831530799513
+        },
+        {
+            "x": 6.340677653913631,
+            "y": 7.289800837555904,
+            "heading": -2.95145648802342,
+            "angularVelocity": -2.1436722572774826e-7,
+            "velocityX": 3.750606720949308,
+            "velocityY": 0.49354650502102115,
+            "timestamp": 1.2606280110091783
+        },
+        {
+            "x": 6.678400512367262,
+            "y": 7.334242093196452,
+            "heading": -2.9514565073260908,
+            "angularVelocity": -2.1436727271528376e-7,
+            "velocityX": 3.750606822202691,
+            "velocityY": 0.4935457355652348,
+            "timestamp": 1.3506728689384053
+        },
+        {
+            "x": 7.0118090549698895,
+            "y": 7.378580230758345,
+            "heading": -2.965622818302132,
+            "angularVelocity": -0.15732504111646,
+            "velocityX": 3.7026938602610375,
+            "velocityY": 0.4924005499208098,
+            "timestamp": 1.4407177268676323
+        },
+        {
+            "x": 7.2792751287326976,
+            "y": 7.414016497420869,
+            "heading": -3.0230864740546695,
+            "angularVelocity": -0.6381669878107006,
+            "velocityX": 2.9703647705572496,
+            "velocityY": 0.39354014740493215,
+            "timestamp": 1.5307625847968593
+        },
+        {
+            "x": 7.479727754094387,
+            "y": 7.440534879064414,
+            "heading": -3.078696937244319,
+            "angularVelocity": -0.617586217231393,
+            "velocityX": 2.2261418360973004,
+            "velocityY": 0.2945018988689779,
+            "timestamp": 1.6208074427260863
+        },
+        {
+            "x": 7.613268208977269,
+            "y": 7.458185411749662,
+            "heading": -3.120596791778004,
+            "angularVelocity": -0.4653220127974119,
+            "velocityX": 1.48304365128591,
+            "velocityY": 0.1960193296003781,
+            "timestamp": 1.7108523006553134
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.2508584712225979,
+            "velocityX": 0.7410949670794122,
+            "velocityY": 0.09789107843632346,
+            "timestamp": 1.8008971585845404
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -3.143185307179586,
+            "angularVelocity": 4.6252031231996675e-32,
+            "velocityX": 0,
+            "velocityY": -8.558401294468068e-35,
+            "timestamp": 1.8909420165137674
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.traj
new file mode 100644
index 00000000..0c6c619f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": -1.8545515236340358e-32,
+            "velocityX": 2.346645959584709e-34,
+            "velocityY": 1.5264694726605652e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3666998117674303,
+            "y": 6.765820939449683,
+            "heading": -2.7737296132590643,
+            "angularVelocity": -0.2635310200357621,
+            "velocityX": 0.7407398190339184,
+            "velocityY": 0.09796161216242502,
+            "timestamp": 0.09004485792922705
+        },
+        {
+            "x": 2.500184310330562,
+            "y": 6.783480740643308,
+            "heading": -2.817788411123353,
+            "angularVelocity": -0.4892983217200214,
+            "velocityX": 1.4824222241324077,
+            "velocityY": 0.19612226172321479,
+            "timestamp": 0.1800897158584541
+        },
+        {
+            "x": 2.700568444033807,
+            "y": 6.810009643084086,
+            "heading": -2.8763096511057067,
+            "angularVelocity": -0.6499120696969705,
+            "velocityX": 2.2253811967890735,
+            "velocityY": 0.294618738380691,
+            "timestamp": 0.27013457378768113
+        },
+        {
+            "x": 2.9679652961316036,
+            "y": 6.845459467588075,
+            "heading": -2.9368283914330156,
+            "angularVelocity": -0.6720954612963594,
+            "velocityX": 2.969596024105716,
+            "velocityY": 0.3936907150417343,
+            "timestamp": 0.3601794317169082
+        },
+        {
+            "x": 3.3011724137612752,
+            "y": 6.889825844077578,
+            "heading": -2.9514563142993295,
+            "angularVelocity": -0.1624515069790128,
+            "velocityX": 3.7004569199449846,
+            "velocityY": 0.49271415947342084,
+            "timestamp": 0.45022428964613526
+        },
+        {
+            "x": 3.638895175262513,
+            "y": 6.93426783648319,
+            "heading": -2.9514563336020103,
+            "angularVelocity": -2.143673866142303e-7,
+            "velocityX": 3.750605745490536,
+            "velocityY": 0.49355391776554697,
+            "timestamp": 0.5402691475753623
+        },
+        {
+            "x": 3.976617946407746,
+            "y": 6.9787097556023205,
+            "heading": -2.9514563529046813,
+            "angularVelocity": -2.1436727225160337e-7,
+            "velocityX": 3.75060585259265,
+            "velocityY": 0.4935531038769741,
+            "timestamp": 0.6303140055045893
+        },
+        {
+            "x": 4.314340727620448,
+            "y": 7.023151598216748,
+            "heading": -2.9514563722073515,
+            "angularVelocity": -2.1436726735785024e-7,
+            "velocityX": 3.7506059643976966,
+            "velocityY": 0.49355225424817656,
+            "timestamp": 0.7203588634338163
+        },
+        {
+            "x": 4.65206351967383,
+            "y": 7.06759335845053,
+            "heading": -2.951456391510026,
+            "angularVelocity": -2.1436731957684306e-7,
+            "velocityX": 3.7506060847896903,
+            "velocityY": 0.49355133936368123,
+            "timestamp": 0.8104037213630433
+        },
+        {
+            "x": 4.989786323250196,
+            "y": 7.11203503111852,
+            "heading": -2.9514564108127086,
+            "angularVelocity": -2.143674043301644e-7,
+            "velocityX": 3.7506062127590543,
+            "velocityY": 0.4935503668951324,
+            "timestamp": 0.9004485792922703
+        },
+        {
+            "x": 5.327509138668733,
+            "y": 7.15647661379494,
+            "heading": -2.951456430115396,
+            "angularVelocity": -2.1436746122645403e-7,
+            "velocityX": 3.7506063442731943,
+            "velocityY": 0.4935493674869138,
+            "timestamp": 0.9904934372214973
+        },
+        {
+            "x": 5.66523196589638,
+            "y": 7.200918106730859,
+            "heading": -2.95145644941808,
+            "angularVelocity": -2.143674220103917e-7,
+            "velocityX": 3.7506064754201436,
+            "velocityY": 0.49354837086698544,
+            "timestamp": 1.0805382951507243
+        },
+        {
+            "x": 6.002954804577347,
+            "y": 7.24535951262982,
+            "heading": -2.9514564687207536,
+            "angularVelocity": -2.1436730521441728e-7,
+            "velocityX": 3.7506066026158704,
+            "velocityY": 0.4935474042714428,
+            "timestamp": 1.1705831530799513
+        },
+        {
+            "x": 6.340677653913631,
+            "y": 7.289800837555904,
+            "heading": -2.95145648802342,
+            "angularVelocity": -2.1436722572774826e-7,
+            "velocityX": 3.750606720949308,
+            "velocityY": 0.49354650502102115,
+            "timestamp": 1.2606280110091783
+        },
+        {
+            "x": 6.678400512367262,
+            "y": 7.334242093196452,
+            "heading": -2.9514565073260908,
+            "angularVelocity": -2.1436727271528376e-7,
+            "velocityX": 3.750606822202691,
+            "velocityY": 0.4935457355652348,
+            "timestamp": 1.3506728689384053
+        },
+        {
+            "x": 7.0118090549698895,
+            "y": 7.378580230758345,
+            "heading": -2.965622818302132,
+            "angularVelocity": -0.15732504111646,
+            "velocityX": 3.7026938602610375,
+            "velocityY": 0.4924005499208098,
+            "timestamp": 1.4407177268676323
+        },
+        {
+            "x": 7.2792751287326976,
+            "y": 7.414016497420869,
+            "heading": -3.0230864740546695,
+            "angularVelocity": -0.6381669878107006,
+            "velocityX": 2.9703647705572496,
+            "velocityY": 0.39354014740493215,
+            "timestamp": 1.5307625847968593
+        },
+        {
+            "x": 7.479727754094387,
+            "y": 7.440534879064414,
+            "heading": -3.078696937244319,
+            "angularVelocity": -0.617586217231393,
+            "velocityX": 2.2261418360973004,
+            "velocityY": 0.2945018988689779,
+            "timestamp": 1.6208074427260863
+        },
+        {
+            "x": 7.613268208977269,
+            "y": 7.458185411749662,
+            "heading": -3.120596791778004,
+            "angularVelocity": -0.4653220127974119,
+            "velocityX": 1.48304365128591,
+            "velocityY": 0.1960193296003781,
+            "timestamp": 1.7108523006553134
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.2508584712225979,
+            "velocityX": 0.7410949670794122,
+            "velocityY": 0.09789107843632346,
+            "timestamp": 1.8008971585845404
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -3.143185307179586,
+            "angularVelocity": 4.6252031231996675e-32,
+            "velocityX": 0,
+            "velocityY": -8.558401294468068e-35,
+            "timestamp": 1.8909420165137674
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.1.traj
new file mode 100644
index 00000000..c369c20d
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 1.5124244158252309e-30,
+            "velocityX": -1.0784945790349375e-32,
+            "velocityY": 7.7332582255010576e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 2.368718302927459,
+            "y": 6.754046521321464,
+            "heading": -2.805815501863529,
+            "angularVelocity": -0.6069900647837034,
+            "velocityX": 0.7473072131063252,
+            "velocityY": -0.0321188944749646,
+            "timestamp": 0.09195455593398905
+        },
+        {
+            "x": 2.506490387717156,
+            "y": 6.748005444139933,
+            "heading": -2.9118057011244183,
+            "angularVelocity": -1.1526367365307737,
+            "velocityX": 1.498262738483551,
+            "velocityY": -0.06569633358752333,
+            "timestamp": 0.1839091118679781
+        },
+        {
+            "x": 2.7138594941009426,
+            "y": 6.738708328412372,
+            "heading": -3.056707583490325,
+            "angularVelocity": -1.5757988377424839,
+            "velocityX": 2.255125961704928,
+            "velocityY": -0.10110554755149347,
+            "timestamp": 0.27586366780196714
+        },
+        {
+            "x": 2.99155716708356,
+            "y": 6.725962728573371,
+            "heading": -3.2144148322825616,
+            "angularVelocity": -1.7150563905223961,
+            "velocityX": 3.019944690744492,
+            "velocityY": -0.13860759490970812,
+            "timestamp": 0.3678182237359562
+        },
+        {
+            "x": 3.331057359197656,
+            "y": 6.709255401861718,
+            "heading": -3.240411366891808,
+            "angularVelocity": -0.28271067534607036,
+            "velocityX": 3.6920431909628384,
+            "velocityY": -0.1816911249470588,
+            "timestamp": 0.45977277966994523
+        },
+        {
+            "x": 3.6785771644429963,
+            "y": 6.69390611896786,
+            "heading": -3.2404114102003327,
+            "angularVelocity": -4.7097747760964064e-7,
+            "velocityX": 3.7792559782988104,
+            "velocityY": -0.1669224840243556,
+            "timestamp": 0.5517273356039343
+        },
+        {
+            "x": 4.026096967074563,
+            "y": 6.678556776895953,
+            "heading": -3.2404114535088273,
+            "angularVelocity": -4.70977144522105e-7,
+            "velocityX": 3.779255949874186,
+            "velocityY": -0.1669231275818996,
+            "timestamp": 0.6436818915379233
+        },
+        {
+            "x": 4.373616766444988,
+            "y": 6.663207360989526,
+            "heading": -3.240411496817302,
+            "angularVelocity": -4.7097693251045375e-7,
+            "velocityX": 3.779255914409469,
+            "velocityY": -0.16692393052765972,
+            "timestamp": 0.7356364474719124
+        },
+        {
+            "x": 4.721136562002901,
+            "y": 6.647857858765673,
+            "heading": -3.240411540125758,
+            "angularVelocity": -4.709767302327839e-7,
+            "velocityX": 3.779255872948645,
+            "velocityY": -0.1669248692242324,
+            "timestamp": 0.8275910034059014
+        },
+        {
+            "x": 5.068656352361164,
+            "y": 6.632508238819668,
+            "heading": -3.2404115834342044,
+            "angularVelocity": -4.709766303037568e-7,
+            "velocityX": 3.7792558164027836,
+            "velocityY": -0.166926149445212,
+            "timestamp": 0.9195455593398905
+        },
+        {
+            "x": 5.4161761153722905,
+            "y": 6.617157999739619,
+            "heading": -3.2404116267428487,
+            "angularVelocity": -4.709787784210011e-7,
+            "velocityX": 3.7792555190043995,
+            "velocityY": -0.16693288248891244,
+            "timestamp": 1.0115001152738796
+        },
+        {
+            "x": 5.762341737151603,
+            "y": 6.582880068913748,
+            "heading": -3.240411680913547,
+            "angularVelocity": -5.891029283022056e-7,
+            "velocityX": 3.7645293184582718,
+            "velocityY": -0.37277033723568115,
+            "timestamp": 1.1034546712078686
+        },
+        {
+            "x": 6.108499177819837,
+            "y": 6.548519619057327,
+            "heading": -3.2404117350781716,
+            "angularVelocity": -5.89036882514826e-7,
+            "velocityX": 3.7644403493909295,
+            "velocityY": -0.37366772649185126,
+            "timestamp": 1.1954092271418575
+        },
+        {
+            "x": 6.443025175700078,
+            "y": 6.453136314569308,
+            "heading": -3.2404117521954174,
+            "angularVelocity": -1.8614896803626685e-7,
+            "velocityX": 3.637949142186984,
+            "velocityY": -1.0372874244152857,
+            "timestamp": 1.2873637830758464
+        },
+        {
+            "x": 6.751449154461275,
+            "y": 6.292262864409884,
+            "heading": -3.240411747251824,
+            "angularVelocity": 5.3761265862029046e-8,
+            "velocityX": 3.3540913294454215,
+            "velocityY": -1.7494886308287982,
+            "timestamp": 1.3793183390098354
+        },
+        {
+            "x": 7.05837809298162,
+            "y": 6.128555062623798,
+            "heading": -3.240411742317762,
+            "angularVelocity": 5.3657614685044897e-8,
+            "velocityX": 3.337832861056693,
+            "velocityY": -1.780312026123053,
+            "timestamp": 1.4712728949438243
+        },
+        {
+            "x": 7.307026842216431,
+            "y": 5.995933066242413,
+            "heading": -3.201520178002944,
+            "angularVelocity": 0.422943310636366,
+            "velocityX": 2.7040394759048985,
+            "velocityY": -1.44225585164687,
+            "timestamp": 1.5632274508778132
+        },
+        {
+            "x": 7.493513419749655,
+            "y": 5.8964665430996925,
+            "heading": -3.172352554188821,
+            "angularVelocity": 0.3171960705792624,
+            "velocityX": 2.0280297766550737,
+            "velocityY": -1.0816921699249264,
+            "timestamp": 1.6551820068118022
+        },
+        {
+            "x": 7.617837807165489,
+            "y": 5.830155516567485,
+            "heading": -3.1529077720147476,
+            "angularVelocity": 0.21146078056243686,
+            "velocityX": 1.3520198771345535,
+            "velocityY": -0.721128234035621,
+            "timestamp": 1.747136562745791
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0.10573119228742708,
+            "velocityX": 0.6760099290690327,
+            "velocityY": -0.3605641529201271,
+            "timestamp": 1.83909111867978
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -3.143185307179586,
+            "angularVelocity": 3.5257869003902484e-31,
+            "velocityX": 0,
+            "velocityY": -2.4831219594454356e-32,
+            "timestamp": 1.931045674613769
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.traj
new file mode 100644
index 00000000..c369c20d
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 1.5124244158252309e-30,
+            "velocityX": -1.0784945790349375e-32,
+            "velocityY": 7.7332582255010576e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 2.368718302927459,
+            "y": 6.754046521321464,
+            "heading": -2.805815501863529,
+            "angularVelocity": -0.6069900647837034,
+            "velocityX": 0.7473072131063252,
+            "velocityY": -0.0321188944749646,
+            "timestamp": 0.09195455593398905
+        },
+        {
+            "x": 2.506490387717156,
+            "y": 6.748005444139933,
+            "heading": -2.9118057011244183,
+            "angularVelocity": -1.1526367365307737,
+            "velocityX": 1.498262738483551,
+            "velocityY": -0.06569633358752333,
+            "timestamp": 0.1839091118679781
+        },
+        {
+            "x": 2.7138594941009426,
+            "y": 6.738708328412372,
+            "heading": -3.056707583490325,
+            "angularVelocity": -1.5757988377424839,
+            "velocityX": 2.255125961704928,
+            "velocityY": -0.10110554755149347,
+            "timestamp": 0.27586366780196714
+        },
+        {
+            "x": 2.99155716708356,
+            "y": 6.725962728573371,
+            "heading": -3.2144148322825616,
+            "angularVelocity": -1.7150563905223961,
+            "velocityX": 3.019944690744492,
+            "velocityY": -0.13860759490970812,
+            "timestamp": 0.3678182237359562
+        },
+        {
+            "x": 3.331057359197656,
+            "y": 6.709255401861718,
+            "heading": -3.240411366891808,
+            "angularVelocity": -0.28271067534607036,
+            "velocityX": 3.6920431909628384,
+            "velocityY": -0.1816911249470588,
+            "timestamp": 0.45977277966994523
+        },
+        {
+            "x": 3.6785771644429963,
+            "y": 6.69390611896786,
+            "heading": -3.2404114102003327,
+            "angularVelocity": -4.7097747760964064e-7,
+            "velocityX": 3.7792559782988104,
+            "velocityY": -0.1669224840243556,
+            "timestamp": 0.5517273356039343
+        },
+        {
+            "x": 4.026096967074563,
+            "y": 6.678556776895953,
+            "heading": -3.2404114535088273,
+            "angularVelocity": -4.70977144522105e-7,
+            "velocityX": 3.779255949874186,
+            "velocityY": -0.1669231275818996,
+            "timestamp": 0.6436818915379233
+        },
+        {
+            "x": 4.373616766444988,
+            "y": 6.663207360989526,
+            "heading": -3.240411496817302,
+            "angularVelocity": -4.7097693251045375e-7,
+            "velocityX": 3.779255914409469,
+            "velocityY": -0.16692393052765972,
+            "timestamp": 0.7356364474719124
+        },
+        {
+            "x": 4.721136562002901,
+            "y": 6.647857858765673,
+            "heading": -3.240411540125758,
+            "angularVelocity": -4.709767302327839e-7,
+            "velocityX": 3.779255872948645,
+            "velocityY": -0.1669248692242324,
+            "timestamp": 0.8275910034059014
+        },
+        {
+            "x": 5.068656352361164,
+            "y": 6.632508238819668,
+            "heading": -3.2404115834342044,
+            "angularVelocity": -4.709766303037568e-7,
+            "velocityX": 3.7792558164027836,
+            "velocityY": -0.166926149445212,
+            "timestamp": 0.9195455593398905
+        },
+        {
+            "x": 5.4161761153722905,
+            "y": 6.617157999739619,
+            "heading": -3.2404116267428487,
+            "angularVelocity": -4.709787784210011e-7,
+            "velocityX": 3.7792555190043995,
+            "velocityY": -0.16693288248891244,
+            "timestamp": 1.0115001152738796
+        },
+        {
+            "x": 5.762341737151603,
+            "y": 6.582880068913748,
+            "heading": -3.240411680913547,
+            "angularVelocity": -5.891029283022056e-7,
+            "velocityX": 3.7645293184582718,
+            "velocityY": -0.37277033723568115,
+            "timestamp": 1.1034546712078686
+        },
+        {
+            "x": 6.108499177819837,
+            "y": 6.548519619057327,
+            "heading": -3.2404117350781716,
+            "angularVelocity": -5.89036882514826e-7,
+            "velocityX": 3.7644403493909295,
+            "velocityY": -0.37366772649185126,
+            "timestamp": 1.1954092271418575
+        },
+        {
+            "x": 6.443025175700078,
+            "y": 6.453136314569308,
+            "heading": -3.2404117521954174,
+            "angularVelocity": -1.8614896803626685e-7,
+            "velocityX": 3.637949142186984,
+            "velocityY": -1.0372874244152857,
+            "timestamp": 1.2873637830758464
+        },
+        {
+            "x": 6.751449154461275,
+            "y": 6.292262864409884,
+            "heading": -3.240411747251824,
+            "angularVelocity": 5.3761265862029046e-8,
+            "velocityX": 3.3540913294454215,
+            "velocityY": -1.7494886308287982,
+            "timestamp": 1.3793183390098354
+        },
+        {
+            "x": 7.05837809298162,
+            "y": 6.128555062623798,
+            "heading": -3.240411742317762,
+            "angularVelocity": 5.3657614685044897e-8,
+            "velocityX": 3.337832861056693,
+            "velocityY": -1.780312026123053,
+            "timestamp": 1.4712728949438243
+        },
+        {
+            "x": 7.307026842216431,
+            "y": 5.995933066242413,
+            "heading": -3.201520178002944,
+            "angularVelocity": 0.422943310636366,
+            "velocityX": 2.7040394759048985,
+            "velocityY": -1.44225585164687,
+            "timestamp": 1.5632274508778132
+        },
+        {
+            "x": 7.493513419749655,
+            "y": 5.8964665430996925,
+            "heading": -3.172352554188821,
+            "angularVelocity": 0.3171960705792624,
+            "velocityX": 2.0280297766550737,
+            "velocityY": -1.0816921699249264,
+            "timestamp": 1.6551820068118022
+        },
+        {
+            "x": 7.617837807165489,
+            "y": 5.830155516567485,
+            "heading": -3.1529077720147476,
+            "angularVelocity": 0.21146078056243686,
+            "velocityX": 1.3520198771345535,
+            "velocityY": -0.721128234035621,
+            "timestamp": 1.747136562745791
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0.10573119228742708,
+            "velocityX": 0.6760099290690327,
+            "velocityY": -0.3605641529201271,
+            "timestamp": 1.83909111867978
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -3.143185307179586,
+            "angularVelocity": 3.5257869003902484e-31,
+            "velocityX": 0,
+            "velocityY": -2.4831219594454356e-32,
+            "timestamp": 1.931045674613769
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.1.traj
new file mode 100644
index 00000000..f5c62fb3
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": -1.6054364331739777e-34,
+            "velocityY": -2.5654655421376103e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 2.373020964607517,
+            "y": 6.753775569864816,
+            "heading": -2.7564987511657213,
+            "angularVelocity": -0.06940035185307017,
+            "velocityX": 0.7797929951747273,
+            "velocityY": -0.03443378276852441,
+            "timestamp": 0.09364147287724231
+        },
+        {
+            "x": 2.5190628897265572,
+            "y": 6.7473267051761905,
+            "heading": -2.769496449901771,
+            "angularVelocity": -0.13880280111661264,
+            "velocityX": 1.5595859466082067,
+            "velocityY": -0.06886761271975776,
+            "timestamp": 0.18728294575448462
+        },
+        {
+            "x": 2.738125767578475,
+            "y": 6.73765340144405,
+            "heading": -2.7889934253789708,
+            "angularVelocity": -0.20820876560484325,
+            "velocityX": 2.339378814973306,
+            "velocityY": -0.10330149062058414,
+            "timestamp": 0.2809244186317269
+        },
+        {
+            "x": 3.030209577950943,
+            "y": 6.724755654214012,
+            "heading": -2.814990111841214,
+            "angularVelocity": -0.2776193673960349,
+            "velocityX": 3.1191714674903457,
+            "velocityY": -0.13773541608988796,
+            "timestamp": 0.37456589150896924
+        },
+        {
+            "x": 3.3841048307253856,
+            "y": 6.709128194593673,
+            "heading": -2.814990114833307,
+            "angularVelocity": -3.1952637479967674e-8,
+            "velocityX": 3.779257650489721,
+            "velocityY": -0.16688609373799812,
+            "timestamp": 0.4682073643862116
+        },
+        {
+            "x": 3.7380000807913594,
+            "y": 6.693500673639671,
+            "heading": -2.814990117825208,
+            "angularVelocity": -3.1950597051067127e-8,
+            "velocityX": 3.779257621565883,
+            "velocityY": -0.16688674872173656,
+            "timestamp": 0.5618488372634539
+        },
+        {
+            "x": 4.091895327848636,
+            "y": 6.677873084552109,
+            "heading": -2.8149901208171126,
+            "angularVelocity": -3.1950635675887034e-8,
+            "velocityX": 3.779257589435919,
+            "velocityY": -0.16688747632201045,
+            "timestamp": 0.6554903101406963
+        },
+        {
+            "x": 4.445790571527361,
+            "y": 6.662245418924005,
+            "heading": -2.814990123809099,
+            "angularVelocity": -3.195149337938281e-8,
+            "velocityX": 3.779257553356315,
+            "velocityY": -0.16688829370019806,
+            "timestamp": 0.7491317830179386
+        },
+        {
+            "x": 4.737874328479073,
+            "y": 6.649347276687054,
+            "heading": -2.8409937626507493,
+            "angularVelocity": -0.27769361205743737,
+            "velocityX": 3.119170897008512,
+            "velocityY": -0.13773963438078918,
+            "timestamp": 0.8427732558951809
+        },
+        {
+            "x": 4.956937160751323,
+            "y": 6.639673651230963,
+            "heading": -2.8604968062706018,
+            "angularVelocity": -0.20827356747619122,
+            "velocityX": 2.3393783282266916,
+            "velocityY": -0.10330492632038867,
+            "timestamp": 0.9364147287724233
+        },
+        {
+            "x": 5.102979052789527,
+            "y": 6.633224554793233,
+            "heading": -2.8734989345094433,
+            "angularVelocity": -0.1388501039053446,
+            "velocityX": 1.5595855933369822,
+            "velocityY": -0.06887008757539573,
+            "timestamp": 1.0300562016496655
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": -0.06942506659508625,
+            "velocityX": 0.7797928093911877,
+            "velocityY": -0.0344351139955655,
+            "timestamp": 1.1236976745269078
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 1.756012262603739e-32,
+            "velocityX": 4.952933956711668e-34,
+            "velocityY": 0,
+            "timestamp": 1.2173391474041502
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.traj
new file mode 100644
index 00000000..f5c62fb3
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": -1.6054364331739777e-34,
+            "velocityY": -2.5654655421376103e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 2.373020964607517,
+            "y": 6.753775569864816,
+            "heading": -2.7564987511657213,
+            "angularVelocity": -0.06940035185307017,
+            "velocityX": 0.7797929951747273,
+            "velocityY": -0.03443378276852441,
+            "timestamp": 0.09364147287724231
+        },
+        {
+            "x": 2.5190628897265572,
+            "y": 6.7473267051761905,
+            "heading": -2.769496449901771,
+            "angularVelocity": -0.13880280111661264,
+            "velocityX": 1.5595859466082067,
+            "velocityY": -0.06886761271975776,
+            "timestamp": 0.18728294575448462
+        },
+        {
+            "x": 2.738125767578475,
+            "y": 6.73765340144405,
+            "heading": -2.7889934253789708,
+            "angularVelocity": -0.20820876560484325,
+            "velocityX": 2.339378814973306,
+            "velocityY": -0.10330149062058414,
+            "timestamp": 0.2809244186317269
+        },
+        {
+            "x": 3.030209577950943,
+            "y": 6.724755654214012,
+            "heading": -2.814990111841214,
+            "angularVelocity": -0.2776193673960349,
+            "velocityX": 3.1191714674903457,
+            "velocityY": -0.13773541608988796,
+            "timestamp": 0.37456589150896924
+        },
+        {
+            "x": 3.3841048307253856,
+            "y": 6.709128194593673,
+            "heading": -2.814990114833307,
+            "angularVelocity": -3.1952637479967674e-8,
+            "velocityX": 3.779257650489721,
+            "velocityY": -0.16688609373799812,
+            "timestamp": 0.4682073643862116
+        },
+        {
+            "x": 3.7380000807913594,
+            "y": 6.693500673639671,
+            "heading": -2.814990117825208,
+            "angularVelocity": -3.1950597051067127e-8,
+            "velocityX": 3.779257621565883,
+            "velocityY": -0.16688674872173656,
+            "timestamp": 0.5618488372634539
+        },
+        {
+            "x": 4.091895327848636,
+            "y": 6.677873084552109,
+            "heading": -2.8149901208171126,
+            "angularVelocity": -3.1950635675887034e-8,
+            "velocityX": 3.779257589435919,
+            "velocityY": -0.16688747632201045,
+            "timestamp": 0.6554903101406963
+        },
+        {
+            "x": 4.445790571527361,
+            "y": 6.662245418924005,
+            "heading": -2.814990123809099,
+            "angularVelocity": -3.195149337938281e-8,
+            "velocityX": 3.779257553356315,
+            "velocityY": -0.16688829370019806,
+            "timestamp": 0.7491317830179386
+        },
+        {
+            "x": 4.737874328479073,
+            "y": 6.649347276687054,
+            "heading": -2.8409937626507493,
+            "angularVelocity": -0.27769361205743737,
+            "velocityX": 3.119170897008512,
+            "velocityY": -0.13773963438078918,
+            "timestamp": 0.8427732558951809
+        },
+        {
+            "x": 4.956937160751323,
+            "y": 6.639673651230963,
+            "heading": -2.8604968062706018,
+            "angularVelocity": -0.20827356747619122,
+            "velocityX": 2.3393783282266916,
+            "velocityY": -0.10330492632038867,
+            "timestamp": 0.9364147287724233
+        },
+        {
+            "x": 5.102979052789527,
+            "y": 6.633224554793233,
+            "heading": -2.8734989345094433,
+            "angularVelocity": -0.1388501039053446,
+            "velocityX": 1.5595855933369822,
+            "velocityY": -0.06887008757539573,
+            "timestamp": 1.0300562016496655
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": -0.06942506659508625,
+            "velocityX": 0.7797928093911877,
+            "velocityY": -0.0344351139955655,
+            "timestamp": 1.1236976745269078
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": -2.88,
+            "angularVelocity": 1.756012262603739e-32,
+            "velocityX": 4.952933956711668e-34,
+            "velocityY": 0,
+            "timestamp": 1.2173391474041502
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.1.traj
new file mode 100644
index 00000000..02347e71
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.347774752133698,
+            "y": 6.727866832421063,
+            "heading": -2.7630995428589378,
+            "angularVelocity": -0.15973393577530476,
+            "velocityX": 0.5825584351440611,
+            "velocityY": -0.3552456424698836,
+            "timestamp": 0.08200851494302656
+        },
+        {
+            "x": 2.4433243460965994,
+            "y": 6.669600579952885,
+            "heading": -2.78929580141764,
+            "angularVelocity": -0.31943339757952466,
+            "velocityX": 1.1651179640221794,
+            "velocityY": -0.7104902766335507,
+            "timestamp": 0.16401702988605313
+        },
+        {
+            "x": 2.586648917905952,
+            "y": 6.582201380168291,
+            "heading": -2.8285851612262256,
+            "angularVelocity": -0.4790887853033408,
+            "velocityX": 1.74767915147499,
+            "velocityY": -1.065733233254043,
+            "timestamp": 0.24602554482907968
+        },
+        {
+            "x": 2.7777486316597817,
+            "y": 6.465669444023111,
+            "heading": -2.8809649184638944,
+            "angularVelocity": -0.6387112030264004,
+            "velocityX": 2.3302423399154564,
+            "velocityY": -1.4209736175095709,
+            "timestamp": 0.32803405977210626
+        },
+        {
+            "x": 3.016623643228877,
+            "y": 6.320005072616726,
+            "heading": -2.9464355230657095,
+            "angularVelocity": -0.7983391071927027,
+            "velocityX": 2.9128074290218198,
+            "velocityY": -1.776210330202685,
+            "timestamp": 0.41004257471513283
+        },
+        {
+            "x": 3.2814961744061293,
+            "y": 6.158489554910389,
+            "heading": -2.9464355313484747,
+            "angularVelocity": -1.0099884469874073e-7,
+            "velocityX": 3.229817432510106,
+            "velocityY": -1.9694969213690323,
+            "timestamp": 0.4920510896581594
+        },
+        {
+            "x": 3.546368691279801,
+            "y": 5.996974013747309,
+            "heading": -2.9464355396312305,
+            "angularVelocity": -1.009987284542081e-7,
+            "velocityX": 3.229817258094308,
+            "velocityY": -1.9694972073971664,
+            "timestamp": 0.574059604601186
+        },
+        {
+            "x": 3.785245806593771,
+            "y": 5.851315740974219,
+            "heading": -3.0120060162505475,
+            "angularVelocity": -0.7995569321657724,
+            "velocityX": 2.912833081783328,
+            "velocityY": -1.7761359643481167,
+            "timestamp": 0.6560681195442125
+        },
+        {
+            "x": 3.976347491680443,
+            "y": 5.7347893459358685,
+            "heading": -3.0644727513052605,
+            "angularVelocity": -0.6397717979793073,
+            "velocityX": 2.3302663780637296,
+            "velocityY": -1.4209060500523003,
+            "timestamp": 0.738076634487239
+        },
+        {
+            "x": 4.119673744072056,
+            "y": 5.64739465388186,
+            "heading": -3.1038280635353432,
+            "angularVelocity": -0.4798929996162412,
+            "velocityX": 1.7476996442526245,
+            "velocityY": -1.0656782666376068,
+            "timestamp": 0.8200851494302656
+        },
+        {
+            "x": 4.2152245778751,
+            "y": 5.589131554648123,
+            "heading": -3.130066448240266,
+            "angularVelocity": -0.3199470777290782,
+            "velocityX": 1.1651330824539987,
+            "velocityY": -0.7104518265476871,
+            "timestamp": 0.9020936643732921
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.15996947327279604,
+            "velocityX": 0.5825666049201219,
+            "velocityY": -0.3552259746242433,
+            "timestamp": 0.9841021793163186
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0,
+            "velocityX": -4.515906316961801e-34,
+            "velocityY": 2.878377535752905e-33,
+            "timestamp": 1.0661106942593452
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.traj
new file mode 100644
index 00000000..02347e71
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.347774752133698,
+            "y": 6.727866832421063,
+            "heading": -2.7630995428589378,
+            "angularVelocity": -0.15973393577530476,
+            "velocityX": 0.5825584351440611,
+            "velocityY": -0.3552456424698836,
+            "timestamp": 0.08200851494302656
+        },
+        {
+            "x": 2.4433243460965994,
+            "y": 6.669600579952885,
+            "heading": -2.78929580141764,
+            "angularVelocity": -0.31943339757952466,
+            "velocityX": 1.1651179640221794,
+            "velocityY": -0.7104902766335507,
+            "timestamp": 0.16401702988605313
+        },
+        {
+            "x": 2.586648917905952,
+            "y": 6.582201380168291,
+            "heading": -2.8285851612262256,
+            "angularVelocity": -0.4790887853033408,
+            "velocityX": 1.74767915147499,
+            "velocityY": -1.065733233254043,
+            "timestamp": 0.24602554482907968
+        },
+        {
+            "x": 2.7777486316597817,
+            "y": 6.465669444023111,
+            "heading": -2.8809649184638944,
+            "angularVelocity": -0.6387112030264004,
+            "velocityX": 2.3302423399154564,
+            "velocityY": -1.4209736175095709,
+            "timestamp": 0.32803405977210626
+        },
+        {
+            "x": 3.016623643228877,
+            "y": 6.320005072616726,
+            "heading": -2.9464355230657095,
+            "angularVelocity": -0.7983391071927027,
+            "velocityX": 2.9128074290218198,
+            "velocityY": -1.776210330202685,
+            "timestamp": 0.41004257471513283
+        },
+        {
+            "x": 3.2814961744061293,
+            "y": 6.158489554910389,
+            "heading": -2.9464355313484747,
+            "angularVelocity": -1.0099884469874073e-7,
+            "velocityX": 3.229817432510106,
+            "velocityY": -1.9694969213690323,
+            "timestamp": 0.4920510896581594
+        },
+        {
+            "x": 3.546368691279801,
+            "y": 5.996974013747309,
+            "heading": -2.9464355396312305,
+            "angularVelocity": -1.009987284542081e-7,
+            "velocityX": 3.229817258094308,
+            "velocityY": -1.9694972073971664,
+            "timestamp": 0.574059604601186
+        },
+        {
+            "x": 3.785245806593771,
+            "y": 5.851315740974219,
+            "heading": -3.0120060162505475,
+            "angularVelocity": -0.7995569321657724,
+            "velocityX": 2.912833081783328,
+            "velocityY": -1.7761359643481167,
+            "timestamp": 0.6560681195442125
+        },
+        {
+            "x": 3.976347491680443,
+            "y": 5.7347893459358685,
+            "heading": -3.0644727513052605,
+            "angularVelocity": -0.6397717979793073,
+            "velocityX": 2.3302663780637296,
+            "velocityY": -1.4209060500523003,
+            "timestamp": 0.738076634487239
+        },
+        {
+            "x": 4.119673744072056,
+            "y": 5.64739465388186,
+            "heading": -3.1038280635353432,
+            "angularVelocity": -0.4798929996162412,
+            "velocityX": 1.7476996442526245,
+            "velocityY": -1.0656782666376068,
+            "timestamp": 0.8200851494302656
+        },
+        {
+            "x": 4.2152245778751,
+            "y": 5.589131554648123,
+            "heading": -3.130066448240266,
+            "angularVelocity": -0.3199470777290782,
+            "velocityX": 1.1651330824539987,
+            "velocityY": -0.7104518265476871,
+            "timestamp": 0.9020936643732921
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.15996947327279604,
+            "velocityX": 0.5825666049201219,
+            "velocityY": -0.3552259746242433,
+            "timestamp": 0.9841021793163186
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0,
+            "velocityX": -4.515906316961801e-34,
+            "velocityY": 2.878377535752905e-33,
+            "timestamp": 1.0661106942593452
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.1.traj
new file mode 100644
index 00000000..e9619fbb
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": -3.577903969873187e-35,
+            "velocityX": -4.511566930271669e-37,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3001399949944257,
+            "y": 6.713175615322878,
+            "heading": -2.7771678602975163,
+            "angularVelocity": -0.3719998319011172,
+            "velocityX": 0.001916901582349506,
+            "velocityY": -0.6000716859748281,
+            "timestamp": 0.07303191552177325
+        },
+        {
+            "x": 2.3004163850099326,
+            "y": 6.6254379467440225,
+            "heading": -2.829600510901429,
+            "angularVelocity": -0.7179416044247195,
+            "velocityX": 0.0037845100122603365,
+            "velocityY": -1.2013606373599366,
+            "timestamp": 0.1460638310435465
+        },
+        {
+            "x": 2.3008288453246886,
+            "y": 6.493659773055691,
+            "heading": -2.9042529721112227,
+            "angularVelocity": -1.022189554750722,
+            "velocityX": 0.005647672141821651,
+            "velocityY": -1.8043915834173219,
+            "timestamp": 0.21909574656531977
+        },
+        {
+            "x": 2.301391538953172,
+            "y": 6.317653855065593,
+            "heading": -2.995610403714753,
+            "angularVelocity": -1.2509247628359643,
+            "velocityX": 0.007704763382735362,
+            "velocityY": -2.4099863290265735,
+            "timestamp": 0.292127662087093
+        },
+        {
+            "x": 2.3021600106867623,
+            "y": 6.097194240566218,
+            "heading": -3.0915567903699093,
+            "angularVelocity": -1.3137596894408665,
+            "velocityX": 0.010522409662952134,
+            "velocityY": -3.0186749577128253,
+            "timestamp": 0.36515957760886625
+        },
+        {
+            "x": 2.303349915614448,
+            "y": 5.834382556129677,
+            "heading": -3.1370632737304405,
+            "angularVelocity": -0.6231040639617954,
+            "velocityX": 0.016292944244779627,
+            "velocityY": -3.598586762498201,
+            "timestamp": 0.4381914931306395
+        },
+        {
+            "x": 2.3036783877036533,
+            "y": 5.558107366997832,
+            "heading": -3.137063302917444,
+            "angularVelocity": -3.99647241237663e-7,
+            "velocityX": 0.004497651292023743,
+            "velocityY": -3.7829377356188716,
+            "timestamp": 0.5112234086524128
+        },
+        {
+            "x": 2.3040068828134284,
+            "y": 5.281832177893351,
+            "heading": -3.1370633321044417,
+            "angularVelocity": -3.996471578074284e-7,
+            "velocityX": 0.0044979665044766115,
+            "velocityY": -3.782937735244173,
+            "timestamp": 0.584255324174186
+        },
+        {
+            "x": 2.302092596714716,
+            "y": 5.019360768744023,
+            "heading": -3.182327854705923,
+            "angularVelocity": -0.6197909814920075,
+            "velocityX": -0.026211637542794358,
+            "velocityY": -3.5939274942210355,
+            "timestamp": 0.6572872396959593
+        },
+        {
+            "x": 2.3012286144088434,
+            "y": 4.799012511457164,
+            "heading": -3.280568867162049,
+            "angularVelocity": -1.3451791830222102,
+            "velocityX": -0.01183020190145165,
+            "velocityY": -3.017150183075313,
+            "timestamp": 0.7303191552177325
+        },
+        {
+            "x": 2.3006858203530896,
+            "y": 4.623118549812072,
+            "heading": -3.3743826060457494,
+            "angularVelocity": -1.284558103309387,
+            "velocityX": -0.007432285623012945,
+            "velocityY": -2.408453350681352,
+            "timestamp": 0.8033510707395057
+        },
+        {
+            "x": 2.3003278174317203,
+            "y": 4.491440647891757,
+            "heading": -3.4511285642395753,
+            "angularVelocity": -1.0508550630983442,
+            "velocityX": -0.00490200645582827,
+            "velocityY": -1.8030185978218984,
+            "timestamp": 0.876382986261279
+        },
+        {
+            "x": 2.3001060473289097,
+            "y": 4.403780156277999,
+            "heading": -3.505127434733723,
+            "angularVelocity": -0.7393872953811381,
+            "velocityX": -0.0030366190072672575,
+            "velocityY": -1.2003038806728776,
+            "timestamp": 0.9494149017830522
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -3.5331853071795862,
+            "angularVelocity": -0.38418645116186434,
+            "velocityX": -0.001452068293050597,
+            "velocityY": -0.5994660822629829,
+            "timestamp": 1.0224468173048256
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -3.5331853071795862,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 8.704854669398703e-34,
+            "timestamp": 1.095478732826599
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.traj
new file mode 100644
index 00000000..e9619fbb
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": -3.577903969873187e-35,
+            "velocityX": -4.511566930271669e-37,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3001399949944257,
+            "y": 6.713175615322878,
+            "heading": -2.7771678602975163,
+            "angularVelocity": -0.3719998319011172,
+            "velocityX": 0.001916901582349506,
+            "velocityY": -0.6000716859748281,
+            "timestamp": 0.07303191552177325
+        },
+        {
+            "x": 2.3004163850099326,
+            "y": 6.6254379467440225,
+            "heading": -2.829600510901429,
+            "angularVelocity": -0.7179416044247195,
+            "velocityX": 0.0037845100122603365,
+            "velocityY": -1.2013606373599366,
+            "timestamp": 0.1460638310435465
+        },
+        {
+            "x": 2.3008288453246886,
+            "y": 6.493659773055691,
+            "heading": -2.9042529721112227,
+            "angularVelocity": -1.022189554750722,
+            "velocityX": 0.005647672141821651,
+            "velocityY": -1.8043915834173219,
+            "timestamp": 0.21909574656531977
+        },
+        {
+            "x": 2.301391538953172,
+            "y": 6.317653855065593,
+            "heading": -2.995610403714753,
+            "angularVelocity": -1.2509247628359643,
+            "velocityX": 0.007704763382735362,
+            "velocityY": -2.4099863290265735,
+            "timestamp": 0.292127662087093
+        },
+        {
+            "x": 2.3021600106867623,
+            "y": 6.097194240566218,
+            "heading": -3.0915567903699093,
+            "angularVelocity": -1.3137596894408665,
+            "velocityX": 0.010522409662952134,
+            "velocityY": -3.0186749577128253,
+            "timestamp": 0.36515957760886625
+        },
+        {
+            "x": 2.303349915614448,
+            "y": 5.834382556129677,
+            "heading": -3.1370632737304405,
+            "angularVelocity": -0.6231040639617954,
+            "velocityX": 0.016292944244779627,
+            "velocityY": -3.598586762498201,
+            "timestamp": 0.4381914931306395
+        },
+        {
+            "x": 2.3036783877036533,
+            "y": 5.558107366997832,
+            "heading": -3.137063302917444,
+            "angularVelocity": -3.99647241237663e-7,
+            "velocityX": 0.004497651292023743,
+            "velocityY": -3.7829377356188716,
+            "timestamp": 0.5112234086524128
+        },
+        {
+            "x": 2.3040068828134284,
+            "y": 5.281832177893351,
+            "heading": -3.1370633321044417,
+            "angularVelocity": -3.996471578074284e-7,
+            "velocityX": 0.0044979665044766115,
+            "velocityY": -3.782937735244173,
+            "timestamp": 0.584255324174186
+        },
+        {
+            "x": 2.302092596714716,
+            "y": 5.019360768744023,
+            "heading": -3.182327854705923,
+            "angularVelocity": -0.6197909814920075,
+            "velocityX": -0.026211637542794358,
+            "velocityY": -3.5939274942210355,
+            "timestamp": 0.6572872396959593
+        },
+        {
+            "x": 2.3012286144088434,
+            "y": 4.799012511457164,
+            "heading": -3.280568867162049,
+            "angularVelocity": -1.3451791830222102,
+            "velocityX": -0.01183020190145165,
+            "velocityY": -3.017150183075313,
+            "timestamp": 0.7303191552177325
+        },
+        {
+            "x": 2.3006858203530896,
+            "y": 4.623118549812072,
+            "heading": -3.3743826060457494,
+            "angularVelocity": -1.284558103309387,
+            "velocityX": -0.007432285623012945,
+            "velocityY": -2.408453350681352,
+            "timestamp": 0.8033510707395057
+        },
+        {
+            "x": 2.3003278174317203,
+            "y": 4.491440647891757,
+            "heading": -3.4511285642395753,
+            "angularVelocity": -1.0508550630983442,
+            "velocityX": -0.00490200645582827,
+            "velocityY": -1.8030185978218984,
+            "timestamp": 0.876382986261279
+        },
+        {
+            "x": 2.3001060473289097,
+            "y": 4.403780156277999,
+            "heading": -3.505127434733723,
+            "angularVelocity": -0.7393872953811381,
+            "velocityX": -0.0030366190072672575,
+            "velocityY": -1.2003038806728776,
+            "timestamp": 0.9494149017830522
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -3.5331853071795862,
+            "angularVelocity": -0.38418645116186434,
+            "velocityX": -0.001452068293050597,
+            "velocityY": -0.5994660822629829,
+            "timestamp": 1.0224468173048256
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -3.5331853071795862,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 8.704854669398703e-34,
+            "timestamp": 1.095478732826599
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.1.traj
new file mode 100644
index 00000000..e091b127
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.1.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 3.909956769253128e-30,
+            "velocityX": -2.302876892990051e-31,
+            "velocityY": 1.6352823102584027e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 2.326889522840126,
+            "y": 5.587767009829938,
+            "heading": 3.1412161105492817,
+            "angularVelocity": 0.012281818163829663,
+            "velocityX": 0.7765273796780119,
+            "velocityY": 0.2804262868111325,
+            "timestamp": 0.0990171433131028
+        },
+        {
+            "x": 2.4806685686648633,
+            "y": 5.6433010199081535,
+            "heading": 3.143648327345736,
+            "angularVelocity": 0.024563592879525555,
+            "velocityX": 1.5530547608152194,
+            "velocityY": 0.560852476854545,
+            "timestamp": 0.1980342866262056
+        },
+        {
+            "x": 2.711337134772754,
+            "y": 5.72660201934395,
+            "heading": 3.147296644122033,
+            "angularVelocity": 0.03684530429983614,
+            "velocityX": 2.3295821146697007,
+            "velocityY": 0.8412785569099963,
+            "timestamp": 0.2970514299393084
+        },
+        {
+            "x": 3.0188952067968695,
+            "y": 5.837669992778753,
+            "heading": 3.1521610500975985,
+            "angularVelocity": 0.04912690684429555,
+            "velocityX": 3.106109323428804,
+            "velocityY": 1.1217044818551853,
+            "timestamp": 0.3960685732524112
+        },
+        {
+            "x": 3.3712021943007997,
+            "y": 5.964897910508787,
+            "heading": 3.1521610506223987,
+            "angularVelocity": 5.300094570798509e-9,
+            "velocityX": 3.558040312169958,
+            "velocityY": 1.2849079813151831,
+            "timestamp": 0.495085716565514
+        },
+        {
+            "x": 3.7235092123405478,
+            "y": 6.092125743682096,
+            "heading": 3.1521610511472007,
+            "angularVelocity": 5.300112966968611e-9,
+            "velocityX": 3.558040620559158,
+            "velocityY": 1.2849071273547275,
+            "timestamp": 0.5941028598786168
+        },
+        {
+            "x": 4.075816261929694,
+            "y": 6.219353489491835,
+            "heading": 3.1521610516720138,
+            "angularVelocity": 5.300218907643243e-9,
+            "velocityX": 3.5580409391847763,
+            "velocityY": 1.284906245047206,
+            "timestamp": 0.6931200031917195
+        },
+        {
+            "x": 4.428123345781717,
+            "y": 6.346581140424019,
+            "heading": 3.152161052196838,
+            "angularVelocity": 5.300337345645622e-9,
+            "velocityX": 3.558041285214527,
+            "velocityY": 1.284905286853995,
+            "timestamp": 0.7921371465048224
+        },
+        {
+            "x": 4.780430481129625,
+            "y": 6.473808648758581,
+            "heading": 3.152161052721672,
+            "angularVelocity": 5.300431772031768e-9,
+            "velocityX": 3.558041805284913,
+            "velocityY": 1.2849038467233598,
+            "timestamp": 0.8911542898179252
+        },
+        {
+            "x": 5.134794229593029,
+            "y": 6.595190238737374,
+            "heading": 3.1521610521993333,
+            "angularVelocity": -5.2752356524836585e-9,
+            "velocityX": 3.5788120784587116,
+            "velocityY": 1.225864389916352,
+            "timestamp": 0.9901714331310281
+        },
+        {
+            "x": 5.489158282910728,
+            "y": 6.716570938689376,
+            "heading": 3.152161051676551,
+            "angularVelocity": -5.279713305479767e-9,
+            "velocityX": 3.578815157261821,
+            "velocityY": 1.225855401303425,
+            "timestamp": 1.089188576444131
+        },
+        {
+            "x": 5.843522381964551,
+            "y": 6.837951505117218,
+            "heading": 3.1521610511537586,
+            "angularVelocity": -5.279818233241214e-9,
+            "velocityX": 3.5788156191628895,
+            "velocityY": 1.2258540528080475,
+            "timestamp": 1.1882057197572338
+        },
+        {
+            "x": 6.19788651637458,
+            "y": 6.959331968324431,
+            "heading": 3.1521610506309807,
+            "angularVelocity": -5.279669747527449e-9,
+            "velocityX": 3.5788159762344476,
+            "velocityY": 1.2258530103559488,
+            "timestamp": 1.2872228630703366
+        },
+        {
+            "x": 6.5522506780354695,
+            "y": 7.080712351974079,
+            "heading": 3.1521610501082176,
+            "angularVelocity": -5.2795212284316414e-9,
+            "velocityX": 3.5788162514480155,
+            "velocityY": 1.2258522068833249,
+            "timestamp": 1.3862400063834395
+        },
+        {
+            "x": 6.9066148601448845,
+            "y": 7.202092675925132,
+            "heading": 3.1521610495854597,
+            "angularVelocity": -5.279467664817609e-9,
+            "velocityX": 3.5788164579630073,
+            "velocityY": 1.2258516039716054,
+            "timestamp": 1.4852571496965423
+        },
+        {
+            "x": 7.215968896352917,
+            "y": 7.308055607481538,
+            "heading": 3.147296630745593,
+            "angularVelocity": -0.049127036764239344,
+            "velocityX": 3.1242472349441757,
+            "velocityY": 1.0701473301581856,
+            "timestamp": 1.5842742930096452
+        },
+        {
+            "x": 7.447984442479348,
+            "y": 7.3875278059692215,
+            "heading": 3.1436483149588383,
+            "angularVelocity": -0.0368452943061847,
+            "velocityX": 2.3431856177952266,
+            "velocityY": 0.8026104958045841,
+            "timestamp": 1.683291436322748
+        },
+        {
+            "x": 7.6026614794307905,
+            "y": 7.440509269529631,
+            "heading": 3.141216104773647,
+            "angularVelocity": -0.024563526110654314,
+            "velocityX": 1.5621238078171693,
+            "velocityY": 0.5350736426810189,
+            "timestamp": 1.7823085796358509
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": -0.012281759834182485,
+            "velocityX": 0.7810619250512634,
+            "velocityY": 0.2675368081121253,
+            "timestamp": 1.8813257229489537
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 1.8685058153122784e-28,
+            "velocityX": 0,
+            "velocityY": -1.820396114239373e-31,
+            "timestamp": 1.9803428662620566
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.traj
new file mode 100644
index 00000000..e091b127
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 3.909956769253128e-30,
+            "velocityX": -2.302876892990051e-31,
+            "velocityY": 1.6352823102584027e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 2.326889522840126,
+            "y": 5.587767009829938,
+            "heading": 3.1412161105492817,
+            "angularVelocity": 0.012281818163829663,
+            "velocityX": 0.7765273796780119,
+            "velocityY": 0.2804262868111325,
+            "timestamp": 0.0990171433131028
+        },
+        {
+            "x": 2.4806685686648633,
+            "y": 5.6433010199081535,
+            "heading": 3.143648327345736,
+            "angularVelocity": 0.024563592879525555,
+            "velocityX": 1.5530547608152194,
+            "velocityY": 0.560852476854545,
+            "timestamp": 0.1980342866262056
+        },
+        {
+            "x": 2.711337134772754,
+            "y": 5.72660201934395,
+            "heading": 3.147296644122033,
+            "angularVelocity": 0.03684530429983614,
+            "velocityX": 2.3295821146697007,
+            "velocityY": 0.8412785569099963,
+            "timestamp": 0.2970514299393084
+        },
+        {
+            "x": 3.0188952067968695,
+            "y": 5.837669992778753,
+            "heading": 3.1521610500975985,
+            "angularVelocity": 0.04912690684429555,
+            "velocityX": 3.106109323428804,
+            "velocityY": 1.1217044818551853,
+            "timestamp": 0.3960685732524112
+        },
+        {
+            "x": 3.3712021943007997,
+            "y": 5.964897910508787,
+            "heading": 3.1521610506223987,
+            "angularVelocity": 5.300094570798509e-9,
+            "velocityX": 3.558040312169958,
+            "velocityY": 1.2849079813151831,
+            "timestamp": 0.495085716565514
+        },
+        {
+            "x": 3.7235092123405478,
+            "y": 6.092125743682096,
+            "heading": 3.1521610511472007,
+            "angularVelocity": 5.300112966968611e-9,
+            "velocityX": 3.558040620559158,
+            "velocityY": 1.2849071273547275,
+            "timestamp": 0.5941028598786168
+        },
+        {
+            "x": 4.075816261929694,
+            "y": 6.219353489491835,
+            "heading": 3.1521610516720138,
+            "angularVelocity": 5.300218907643243e-9,
+            "velocityX": 3.5580409391847763,
+            "velocityY": 1.284906245047206,
+            "timestamp": 0.6931200031917195
+        },
+        {
+            "x": 4.428123345781717,
+            "y": 6.346581140424019,
+            "heading": 3.152161052196838,
+            "angularVelocity": 5.300337345645622e-9,
+            "velocityX": 3.558041285214527,
+            "velocityY": 1.284905286853995,
+            "timestamp": 0.7921371465048224
+        },
+        {
+            "x": 4.780430481129625,
+            "y": 6.473808648758581,
+            "heading": 3.152161052721672,
+            "angularVelocity": 5.300431772031768e-9,
+            "velocityX": 3.558041805284913,
+            "velocityY": 1.2849038467233598,
+            "timestamp": 0.8911542898179252
+        },
+        {
+            "x": 5.134794229593029,
+            "y": 6.595190238737374,
+            "heading": 3.1521610521993333,
+            "angularVelocity": -5.2752356524836585e-9,
+            "velocityX": 3.5788120784587116,
+            "velocityY": 1.225864389916352,
+            "timestamp": 0.9901714331310281
+        },
+        {
+            "x": 5.489158282910728,
+            "y": 6.716570938689376,
+            "heading": 3.152161051676551,
+            "angularVelocity": -5.279713305479767e-9,
+            "velocityX": 3.578815157261821,
+            "velocityY": 1.225855401303425,
+            "timestamp": 1.089188576444131
+        },
+        {
+            "x": 5.843522381964551,
+            "y": 6.837951505117218,
+            "heading": 3.1521610511537586,
+            "angularVelocity": -5.279818233241214e-9,
+            "velocityX": 3.5788156191628895,
+            "velocityY": 1.2258540528080475,
+            "timestamp": 1.1882057197572338
+        },
+        {
+            "x": 6.19788651637458,
+            "y": 6.959331968324431,
+            "heading": 3.1521610506309807,
+            "angularVelocity": -5.279669747527449e-9,
+            "velocityX": 3.5788159762344476,
+            "velocityY": 1.2258530103559488,
+            "timestamp": 1.2872228630703366
+        },
+        {
+            "x": 6.5522506780354695,
+            "y": 7.080712351974079,
+            "heading": 3.1521610501082176,
+            "angularVelocity": -5.2795212284316414e-9,
+            "velocityX": 3.5788162514480155,
+            "velocityY": 1.2258522068833249,
+            "timestamp": 1.3862400063834395
+        },
+        {
+            "x": 6.9066148601448845,
+            "y": 7.202092675925132,
+            "heading": 3.1521610495854597,
+            "angularVelocity": -5.279467664817609e-9,
+            "velocityX": 3.5788164579630073,
+            "velocityY": 1.2258516039716054,
+            "timestamp": 1.4852571496965423
+        },
+        {
+            "x": 7.215968896352917,
+            "y": 7.308055607481538,
+            "heading": 3.147296630745593,
+            "angularVelocity": -0.049127036764239344,
+            "velocityX": 3.1242472349441757,
+            "velocityY": 1.0701473301581856,
+            "timestamp": 1.5842742930096452
+        },
+        {
+            "x": 7.447984442479348,
+            "y": 7.3875278059692215,
+            "heading": 3.1436483149588383,
+            "angularVelocity": -0.0368452943061847,
+            "velocityX": 2.3431856177952266,
+            "velocityY": 0.8026104958045841,
+            "timestamp": 1.683291436322748
+        },
+        {
+            "x": 7.6026614794307905,
+            "y": 7.440509269529631,
+            "heading": 3.141216104773647,
+            "angularVelocity": -0.024563526110654314,
+            "velocityX": 1.5621238078171693,
+            "velocityY": 0.5350736426810189,
+            "timestamp": 1.7823085796358509
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": -0.012281759834182485,
+            "velocityX": 0.7810619250512634,
+            "velocityY": 0.2675368081121253,
+            "timestamp": 1.8813257229489537
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 1.8685058153122784e-28,
+            "velocityX": 0,
+            "velocityY": -1.820396114239373e-31,
+            "timestamp": 1.9803428662620566
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.1.traj
new file mode 100644
index 00000000..31e55043
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.1.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 1.3281175985971763e-33,
+            "velocityX": 1.9314152785727527e-39,
+            "velocityY": 8.278387842337126e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3319036557469475,
+            "y": 5.560000044140862,
+            "heading": 3.140000009517521,
+            "angularVelocity": 9.602993340973177e-8,
+            "velocityX": 0.8263919813281884,
+            "velocityY": 4.4537273180015373e-7,
+            "timestamp": 0.09910993523353247
+        },
+        {
+            "x": 2.495710964163716,
+            "y": 5.5600001253696005,
+            "heading": 3.140000023792121,
+            "angularVelocity": 1.44027928091657e-7,
+            "velocityX": 1.6527839316087705,
+            "velocityY": 8.195821841849307e-7,
+            "timestamp": 0.19821987046706493
+        },
+        {
+            "x": 2.741421919096052,
+            "y": 5.5600002360460925,
+            "heading": 3.140000035820423,
+            "angularVelocity": 1.213632154532524e-7,
+            "velocityX": 2.479175819794127,
+            "velocityY": 0.0000011167042923727,
+            "timestamp": 0.2973298057005974
+        },
+        {
+            "x": 3.0690365020812136,
+            "y": 5.5600003677528065,
+            "heading": 3.140000035237491,
+            "angularVelocity": -5.88169262513392e-9,
+            "velocityX": 3.305567521694012,
+            "velocityY": 0.0000013288951255827482,
+            "timestamp": 0.39643974093412987
+        },
+        {
+            "x": 3.443963497918667,
+            "y": 5.560000377607592,
+            "heading": 3.140000035237483,
+            "angularVelocity": -8.264095134625168e-14,
+            "velocityX": 3.782940579610041,
+            "velocityY": 9.943285507786426e-8,
+            "timestamp": 0.4955496761676623
+        },
+        {
+            "x": 3.771578080903882,
+            "y": 5.560000246492901,
+            "heading": 3.1399999920753214,
+            "angularVelocity": -4.354978004755772e-7,
+            "velocityX": 3.305567521694553,
+            "velocityY": -0.0000013229217178930416,
+            "timestamp": 0.5946596114011948
+        },
+        {
+            "x": 4.017289035836251,
+            "y": 5.560000133124366,
+            "heading": 3.1399999814016817,
+            "angularVelocity": -1.0769493378117993e-7,
+            "velocityX": 2.4791758197944644,
+            "velocityY": -0.000001143866475595614,
+            "timestamp": 0.6937695466347273
+        },
+        {
+            "x": 4.181096344253041,
+            "y": 5.560000047628769,
+            "heading": 3.1399999890121935,
+            "angularVelocity": 7.678859424954975e-8,
+            "velocityX": 1.6527839316089916,
+            "velocityY": -8.626339566508034e-7,
+            "timestamp": 0.7928794818682598
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 1.1086483608499845e-7,
+            "velocityX": 0.8263919813282989,
+            "velocityY": -4.805650223133441e-7,
+            "timestamp": 0.8919894171017924
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 2.938735877055719e-39,
+            "velocityY": -3.0539806085263666e-33,
+            "timestamp": 0.9910993523353249
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.traj
new file mode 100644
index 00000000..31e55043
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 1.3281175985971763e-33,
+            "velocityX": 1.9314152785727527e-39,
+            "velocityY": 8.278387842337126e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3319036557469475,
+            "y": 5.560000044140862,
+            "heading": 3.140000009517521,
+            "angularVelocity": 9.602993340973177e-8,
+            "velocityX": 0.8263919813281884,
+            "velocityY": 4.4537273180015373e-7,
+            "timestamp": 0.09910993523353247
+        },
+        {
+            "x": 2.495710964163716,
+            "y": 5.5600001253696005,
+            "heading": 3.140000023792121,
+            "angularVelocity": 1.44027928091657e-7,
+            "velocityX": 1.6527839316087705,
+            "velocityY": 8.195821841849307e-7,
+            "timestamp": 0.19821987046706493
+        },
+        {
+            "x": 2.741421919096052,
+            "y": 5.5600002360460925,
+            "heading": 3.140000035820423,
+            "angularVelocity": 1.213632154532524e-7,
+            "velocityX": 2.479175819794127,
+            "velocityY": 0.0000011167042923727,
+            "timestamp": 0.2973298057005974
+        },
+        {
+            "x": 3.0690365020812136,
+            "y": 5.5600003677528065,
+            "heading": 3.140000035237491,
+            "angularVelocity": -5.88169262513392e-9,
+            "velocityX": 3.305567521694012,
+            "velocityY": 0.0000013288951255827482,
+            "timestamp": 0.39643974093412987
+        },
+        {
+            "x": 3.443963497918667,
+            "y": 5.560000377607592,
+            "heading": 3.140000035237483,
+            "angularVelocity": -8.264095134625168e-14,
+            "velocityX": 3.782940579610041,
+            "velocityY": 9.943285507786426e-8,
+            "timestamp": 0.4955496761676623
+        },
+        {
+            "x": 3.771578080903882,
+            "y": 5.560000246492901,
+            "heading": 3.1399999920753214,
+            "angularVelocity": -4.354978004755772e-7,
+            "velocityX": 3.305567521694553,
+            "velocityY": -0.0000013229217178930416,
+            "timestamp": 0.5946596114011948
+        },
+        {
+            "x": 4.017289035836251,
+            "y": 5.560000133124366,
+            "heading": 3.1399999814016817,
+            "angularVelocity": -1.0769493378117993e-7,
+            "velocityX": 2.4791758197944644,
+            "velocityY": -0.000001143866475595614,
+            "timestamp": 0.6937695466347273
+        },
+        {
+            "x": 4.181096344253041,
+            "y": 5.560000047628769,
+            "heading": 3.1399999890121935,
+            "angularVelocity": 7.678859424954975e-8,
+            "velocityX": 1.6527839316089916,
+            "velocityY": -8.626339566508034e-7,
+            "timestamp": 0.7928794818682598
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 1.1086483608499845e-7,
+            "velocityX": 0.8263919813282989,
+            "velocityY": -4.805650223133441e-7,
+            "timestamp": 0.8919894171017924
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 2.938735877055719e-39,
+            "velocityY": -3.0539806085263666e-33,
+            "timestamp": 0.9910993523353249
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj
new file mode 100644
index 00000000..ca906489
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 7.733955963828693e-33,
+            "velocityY": 1.1033116642945717e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2520020198561768,
+            "y": 5.607878032622388,
+            "heading": 3.1558282209608954,
+            "angularVelocity": 0.2083481053076329,
+            "velocityX": 0.026352743296497748,
+            "velocityY": 0.6302222724446542,
+            "timestamp": 0.07597007391799751
+        },
+        {
+            "x": 2.2560055614843093,
+            "y": 5.703634205053828,
+            "heading": 3.187479734547443,
+            "angularVelocity": 0.41663133855460127,
+            "velocityX": 0.052698930271593884,
+            "velocityY": 1.2604459557957022,
+            "timestamp": 0.15194014783599502
+        },
+        {
+            "x": 2.2620099218556464,
+            "y": 5.847268779856728,
+            "heading": 3.234941916913622,
+            "angularVelocity": 0.6247484031331855,
+            "velocityX": 0.07903586322448838,
+            "velocityY": 1.8906730952761694,
+            "timestamp": 0.22791022175399253
+        },
+        {
+            "x": 2.270014272624616,
+            "y": 6.038782285632903,
+            "heading": 3.2981885303415055,
+            "angularVelocity": 0.8325200985871338,
+            "velocityX": 0.10536189260009911,
+            "velocityY": 2.520907192783506,
+            "timestamp": 0.30388029567199004
+        },
+        {
+            "x": 2.2800095231252646,
+            "y": 6.278187238938093,
+            "heading": 3.376569786131263,
+            "angularVelocity": 1.0317385747756829,
+            "velocityX": 0.13156826083172354,
+            "velocityY": 3.151306046688924,
+            "timestamp": 0.3798503695899875
+        },
+        {
+            "x": 2.2880056735413383,
+            "y": 6.469712014906391,
+            "heading": 3.4392336964848362,
+            "angularVelocity": 0.8248499326354866,
+            "velocityX": 0.10525395071624018,
+            "velocityY": 2.5210555432002106,
+            "timestamp": 0.455820443507985
+        },
+        {
+            "x": 2.2940028218748654,
+            "y": 6.613355955414726,
+            "heading": 3.486212731686583,
+            "angularVelocity": 0.6183886993772851,
+            "velocityX": 0.07894093060907224,
+            "velocityY": 1.8907963767862817,
+            "timestamp": 0.5317905174259825
+        },
+        {
+            "x": 2.29800094256259,
+            "y": 6.709118670546653,
+            "heading": 3.517527025119772,
+            "angularVelocity": 0.412192483411159,
+            "velocityX": 0.052627574010790594,
+            "velocityY": 1.2605320778717877,
+            "timestamp": 0.60776059134398
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 0.20611118631681047,
+            "velocityX": 0.026313748747538647,
+            "velocityY": 0.6302656688873288,
+            "timestamp": 0.6837306652619775
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": -1.7034359069625615e-32,
+            "velocityX": 1.3795062534340913e-33,
+            "velocityY": 0,
+            "timestamp": 0.7597007391799749
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.traj
new file mode 100644
index 00000000..ca906489
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 7.733955963828693e-33,
+            "velocityY": 1.1033116642945717e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2520020198561768,
+            "y": 5.607878032622388,
+            "heading": 3.1558282209608954,
+            "angularVelocity": 0.2083481053076329,
+            "velocityX": 0.026352743296497748,
+            "velocityY": 0.6302222724446542,
+            "timestamp": 0.07597007391799751
+        },
+        {
+            "x": 2.2560055614843093,
+            "y": 5.703634205053828,
+            "heading": 3.187479734547443,
+            "angularVelocity": 0.41663133855460127,
+            "velocityX": 0.052698930271593884,
+            "velocityY": 1.2604459557957022,
+            "timestamp": 0.15194014783599502
+        },
+        {
+            "x": 2.2620099218556464,
+            "y": 5.847268779856728,
+            "heading": 3.234941916913622,
+            "angularVelocity": 0.6247484031331855,
+            "velocityX": 0.07903586322448838,
+            "velocityY": 1.8906730952761694,
+            "timestamp": 0.22791022175399253
+        },
+        {
+            "x": 2.270014272624616,
+            "y": 6.038782285632903,
+            "heading": 3.2981885303415055,
+            "angularVelocity": 0.8325200985871338,
+            "velocityX": 0.10536189260009911,
+            "velocityY": 2.520907192783506,
+            "timestamp": 0.30388029567199004
+        },
+        {
+            "x": 2.2800095231252646,
+            "y": 6.278187238938093,
+            "heading": 3.376569786131263,
+            "angularVelocity": 1.0317385747756829,
+            "velocityX": 0.13156826083172354,
+            "velocityY": 3.151306046688924,
+            "timestamp": 0.3798503695899875
+        },
+        {
+            "x": 2.2880056735413383,
+            "y": 6.469712014906391,
+            "heading": 3.4392336964848362,
+            "angularVelocity": 0.8248499326354866,
+            "velocityX": 0.10525395071624018,
+            "velocityY": 2.5210555432002106,
+            "timestamp": 0.455820443507985
+        },
+        {
+            "x": 2.2940028218748654,
+            "y": 6.613355955414726,
+            "heading": 3.486212731686583,
+            "angularVelocity": 0.6183886993772851,
+            "velocityX": 0.07894093060907224,
+            "velocityY": 1.8907963767862817,
+            "timestamp": 0.5317905174259825
+        },
+        {
+            "x": 2.29800094256259,
+            "y": 6.709118670546653,
+            "heading": 3.517527025119772,
+            "angularVelocity": 0.412192483411159,
+            "velocityX": 0.052627574010790594,
+            "velocityY": 1.2605320778717877,
+            "timestamp": 0.60776059134398
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 0.20611118631681047,
+            "velocityX": 0.026313748747538647,
+            "velocityY": 0.6302656688873288,
+            "timestamp": 0.6837306652619775
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": -1.7034359069625615e-32,
+            "velocityX": 1.3795062534340913e-33,
+            "velocityY": 0,
+            "timestamp": 0.7597007391799749
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj
new file mode 100644
index 00000000..5e33e1a6
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -1.4697340738263578e-34,
+            "velocityY": 1.0017538611600725e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2520018980434946,
+            "y": 5.512001884856013,
+            "heading": 3.124302281529173,
+            "angularVelocity": -0.20638131373671467,
+            "velocityX": 0.026319388320742283,
+            "velocityY": -0.6310416433261579,
+            "timestamp": 0.07606172374138878
+        },
+        {
+            "x": 2.2560052212232096,
+            "y": 5.416005549224728,
+            "heading": 3.092911780798619,
+            "angularVelocity": -0.4126977300341266,
+            "velocityX": 0.05263255922684882,
+            "velocityY": -1.2620846716237228,
+            "timestamp": 0.15212344748277756
+        },
+        {
+            "x": 2.262009303064208,
+            "y": 5.272010738006526,
+            "heading": 3.045840934307484,
+            "angularVelocity": -0.618850640976494,
+            "velocityX": 0.07893696784222709,
+            "velocityY": -1.8931310537713693,
+            "timestamp": 0.22818517122416634
+        },
+        {
+            "x": 2.2700133593824035,
+            "y": 5.080016941067703,
+            "heading": 2.9831153695364567,
+            "angularVelocity": -0.8246666218648364,
+            "velocityX": 0.10523106661912582,
+            "velocityY": -2.524184142757597,
+            "timestamp": 0.3042468949655551
+        },
+        {
+            "x": 2.2800088926734445,
+            "y": 4.840012206333479,
+            "heading": 2.90536283495199,
+            "angularVelocity": -1.0222294573395003,
+            "velocityX": 0.1314134468609495,
+            "velocityY": -3.1553943682665504,
+            "timestamp": 0.3803086187069439
+        },
+        {
+            "x": 2.288005283531118,
+            "y": 4.648007638478312,
+            "heading": 2.8432005737453654,
+            "angularVelocity": -0.8172607475736078,
+            "velocityX": 0.10513028714497022,
+            "velocityY": -2.524325750333843,
+            "timestamp": 0.45637034244833263
+        },
+        {
+            "x": 2.2940026200964168,
+            "y": 4.504003869225784,
+            "heading": 2.7965971516354546,
+            "angularVelocity": -0.6127053111281423,
+            "velocityX": 0.07884828623776054,
+            "velocityY": -1.8932488269940233,
+            "timestamp": 0.5324320661897214
+        },
+        {
+            "x": 2.2980008723577208,
+            "y": 4.408001271885206,
+            "heading": 2.7655331089053763,
+            "angularVelocity": -0.40840571580650337,
+            "velocityX": 0.052565890761270766,
+            "velocityY": -1.262166995675615,
+            "timestamp": 0.6084937899311101
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": -0.20421715603224325,
+            "velocityX": 0.026282965254326718,
+            "velocityY": -0.6310831456885047,
+            "timestamp": 0.6845555136724989
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 2.533367966722586e-34,
+            "velocityY": 7.359145603780692e-32,
+            "timestamp": 0.7606172374138876
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.traj
new file mode 100644
index 00000000..5e33e1a6
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -1.4697340738263578e-34,
+            "velocityY": 1.0017538611600725e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2520018980434946,
+            "y": 5.512001884856013,
+            "heading": 3.124302281529173,
+            "angularVelocity": -0.20638131373671467,
+            "velocityX": 0.026319388320742283,
+            "velocityY": -0.6310416433261579,
+            "timestamp": 0.07606172374138878
+        },
+        {
+            "x": 2.2560052212232096,
+            "y": 5.416005549224728,
+            "heading": 3.092911780798619,
+            "angularVelocity": -0.4126977300341266,
+            "velocityX": 0.05263255922684882,
+            "velocityY": -1.2620846716237228,
+            "timestamp": 0.15212344748277756
+        },
+        {
+            "x": 2.262009303064208,
+            "y": 5.272010738006526,
+            "heading": 3.045840934307484,
+            "angularVelocity": -0.618850640976494,
+            "velocityX": 0.07893696784222709,
+            "velocityY": -1.8931310537713693,
+            "timestamp": 0.22818517122416634
+        },
+        {
+            "x": 2.2700133593824035,
+            "y": 5.080016941067703,
+            "heading": 2.9831153695364567,
+            "angularVelocity": -0.8246666218648364,
+            "velocityX": 0.10523106661912582,
+            "velocityY": -2.524184142757597,
+            "timestamp": 0.3042468949655551
+        },
+        {
+            "x": 2.2800088926734445,
+            "y": 4.840012206333479,
+            "heading": 2.90536283495199,
+            "angularVelocity": -1.0222294573395003,
+            "velocityX": 0.1314134468609495,
+            "velocityY": -3.1553943682665504,
+            "timestamp": 0.3803086187069439
+        },
+        {
+            "x": 2.288005283531118,
+            "y": 4.648007638478312,
+            "heading": 2.8432005737453654,
+            "angularVelocity": -0.8172607475736078,
+            "velocityX": 0.10513028714497022,
+            "velocityY": -2.524325750333843,
+            "timestamp": 0.45637034244833263
+        },
+        {
+            "x": 2.2940026200964168,
+            "y": 4.504003869225784,
+            "heading": 2.7965971516354546,
+            "angularVelocity": -0.6127053111281423,
+            "velocityX": 0.07884828623776054,
+            "velocityY": -1.8932488269940233,
+            "timestamp": 0.5324320661897214
+        },
+        {
+            "x": 2.2980008723577208,
+            "y": 4.408001271885206,
+            "heading": 2.7655331089053763,
+            "angularVelocity": -0.40840571580650337,
+            "velocityX": 0.052565890761270766,
+            "velocityY": -1.262166995675615,
+            "timestamp": 0.6084937899311101
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": -0.20421715603224325,
+            "velocityX": 0.026282965254326718,
+            "velocityY": -0.6310831456885047,
+            "timestamp": 0.6845555136724989
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 2.533367966722586e-34,
+            "velocityY": 7.359145603780692e-32,
+            "timestamp": 0.7606172374138876
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.1.traj
new file mode 100644
index 00000000..22f8f664
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.1.traj	
@@ -0,0 +1,175 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": -1.3064230645361168e-30,
+            "velocityX": 1.1529790698064783e-31,
+            "velocityY": -2.8761439718242387e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 2.322180658668465,
+            "y": 4.411221384877813,
+            "heading": 2.7352853365602194,
+            "angularVelocity": -0.1796018847298344,
+            "velocityX": 0.2707291347647005,
+            "velocityY": 0.6251897843383167,
+            "timestamp": 0.08192933755631272
+        },
+        {
+            "x": 2.368773081487876,
+            "y": 4.512701566380937,
+            "heading": 2.7089075651304313,
+            "angularVelocity": -0.32195758218669895,
+            "velocityX": 0.5686903398600793,
+            "velocityY": 1.238630562994284,
+            "timestamp": 0.16385867511262545
+        },
+        {
+            "x": 2.4433012157913003,
+            "y": 4.662644128727173,
+            "heading": 2.675827445796147,
+            "angularVelocity": -0.4037640278922902,
+            "velocityX": 0.9096635774968738,
+            "velocityY": 1.8301449373147467,
+            "timestamp": 0.24578801266893818
+        },
+        {
+            "x": 2.5519618748459716,
+            "y": 4.856937153180571,
+            "heading": 2.6453653517833016,
+            "angularVelocity": -0.3718093532967731,
+            "velocityX": 1.3262728870471487,
+            "velocityY": 2.371470711817401,
+            "timestamp": 0.3277173502252509
+        },
+        {
+            "x": 2.7016003249644123,
+            "y": 5.089209181124471,
+            "heading": 2.6267184772011043,
+            "angularVelocity": -0.22759703835491757,
+            "velocityX": 1.8264330529413755,
+            "velocityY": 2.835028756143084,
+            "timestamp": 0.4096466877815636
+        },
+        {
+            "x": 2.9069631472288275,
+            "y": 5.3213406432232695,
+            "heading": 2.6267188101864813,
+            "angularVelocity": 0.00000406429963372636,
+            "velocityX": 2.5065846788186557,
+            "velocityY": 2.833313060040874,
+            "timestamp": 0.4915760253378763
+        },
+        {
+            "x": 3.1507251830977756,
+            "y": 5.512753871430069,
+            "heading": 2.626718909792111,
+            "angularVelocity": 0.0000012157504557439165,
+            "velocityX": 2.975271656522333,
+            "velocityY": 2.3363209555458044,
+            "timestamp": 0.5735053628941891
+        },
+        {
+            "x": 3.4078762515345984,
+            "y": 5.685763359739643,
+            "heading": 2.6267190032534695,
+            "angularVelocity": 0.0000011407556990966525,
+            "velocityX": 3.1386933680511517,
+            "velocityY": 2.1116915316282108,
+            "timestamp": 0.6554347004505018
+        },
+        {
+            "x": 3.665027465532842,
+            "y": 5.858772631695159,
+            "heading": 2.626719096714767,
+            "angularVelocity": 0.0000011407549466335633,
+            "velocityX": 3.1386951447214493,
+            "velocityY": 2.111688890888465,
+            "timestamp": 0.7373640380068145
+        },
+        {
+            "x": 3.922178727707041,
+            "y": 6.031781832044647,
+            "heading": 2.6267191901760296,
+            "angularVelocity": 0.0000011407545266438845,
+            "velocityX": 3.138695732739822,
+            "velocityY": 2.1116880168910543,
+            "timestamp": 0.8192933755631272
+        },
+        {
+            "x": 4.179330140012749,
+            "y": 6.204790809246775,
+            "heading": 2.6267192836372875,
+            "angularVelocity": 0.0000011407544682834688,
+            "velocityX": 3.1386975651909763,
+            "velocityY": 2.111685293234714,
+            "timestamp": 0.9012227131194399
+        },
+        {
+            "x": 4.443624087689148,
+            "y": 6.354175617474429,
+            "heading": 2.642647782574834,
+            "angularVelocity": 0.1944175238399595,
+            "velocityX": 3.225876780643333,
+            "velocityY": 1.823337191332418,
+            "timestamp": 0.9831520506757526
+        },
+        {
+            "x": 4.674751705763445,
+            "y": 6.469415592948305,
+            "heading": 2.769232553851288,
+            "angularVelocity": 1.5450481482222196,
+            "velocityX": 2.821060501257377,
+            "velocityY": 1.4065776547341065,
+            "timestamp": 1.0650813882320653
+        },
+        {
+            "x": 4.870834347570915,
+            "y": 6.549065174445882,
+            "heading": 2.9849459088839683,
+            "angularVelocity": 2.6329195556404166,
+            "velocityX": 2.39331413698659,
+            "velocityY": 0.9721741182495338,
+            "timestamp": 1.1470107257883781
+        },
+        {
+            "x": 5.022947063703052,
+            "y": 6.5945122449585005,
+            "heading": 3.181337324435562,
+            "angularVelocity": 2.397082918150139,
+            "velocityX": 1.856633053179344,
+            "velocityY": 0.5547105819253249,
+            "timestamp": 1.228940063344691
+        },
+        {
+            "x": 5.124965104320653,
+            "y": 6.619330120195266,
+            "heading": 3.326379958974982,
+            "angularVelocity": 1.7703381824578692,
+            "velocityX": 1.2451954777185934,
+            "velocityY": 0.3029180508106365,
+            "timestamp": 1.3108694009010038
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.93745843058738,
+            "velocityX": 0.6229135643171693,
+            "velocityY": 0.13023271178533075,
+            "timestamp": 1.3927987384573166
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": -5.432893499766539e-33,
+            "velocityX": 0,
+            "velocityY": 6.79272724985994e-33,
+            "timestamp": 1.4747280760136294
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.traj
new file mode 100644
index 00000000..22f8f664
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.traj	
@@ -0,0 +1,175 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": -1.3064230645361168e-30,
+            "velocityX": 1.1529790698064783e-31,
+            "velocityY": -2.8761439718242387e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 2.322180658668465,
+            "y": 4.411221384877813,
+            "heading": 2.7352853365602194,
+            "angularVelocity": -0.1796018847298344,
+            "velocityX": 0.2707291347647005,
+            "velocityY": 0.6251897843383167,
+            "timestamp": 0.08192933755631272
+        },
+        {
+            "x": 2.368773081487876,
+            "y": 4.512701566380937,
+            "heading": 2.7089075651304313,
+            "angularVelocity": -0.32195758218669895,
+            "velocityX": 0.5686903398600793,
+            "velocityY": 1.238630562994284,
+            "timestamp": 0.16385867511262545
+        },
+        {
+            "x": 2.4433012157913003,
+            "y": 4.662644128727173,
+            "heading": 2.675827445796147,
+            "angularVelocity": -0.4037640278922902,
+            "velocityX": 0.9096635774968738,
+            "velocityY": 1.8301449373147467,
+            "timestamp": 0.24578801266893818
+        },
+        {
+            "x": 2.5519618748459716,
+            "y": 4.856937153180571,
+            "heading": 2.6453653517833016,
+            "angularVelocity": -0.3718093532967731,
+            "velocityX": 1.3262728870471487,
+            "velocityY": 2.371470711817401,
+            "timestamp": 0.3277173502252509
+        },
+        {
+            "x": 2.7016003249644123,
+            "y": 5.089209181124471,
+            "heading": 2.6267184772011043,
+            "angularVelocity": -0.22759703835491757,
+            "velocityX": 1.8264330529413755,
+            "velocityY": 2.835028756143084,
+            "timestamp": 0.4096466877815636
+        },
+        {
+            "x": 2.9069631472288275,
+            "y": 5.3213406432232695,
+            "heading": 2.6267188101864813,
+            "angularVelocity": 0.00000406429963372636,
+            "velocityX": 2.5065846788186557,
+            "velocityY": 2.833313060040874,
+            "timestamp": 0.4915760253378763
+        },
+        {
+            "x": 3.1507251830977756,
+            "y": 5.512753871430069,
+            "heading": 2.626718909792111,
+            "angularVelocity": 0.0000012157504557439165,
+            "velocityX": 2.975271656522333,
+            "velocityY": 2.3363209555458044,
+            "timestamp": 0.5735053628941891
+        },
+        {
+            "x": 3.4078762515345984,
+            "y": 5.685763359739643,
+            "heading": 2.6267190032534695,
+            "angularVelocity": 0.0000011407556990966525,
+            "velocityX": 3.1386933680511517,
+            "velocityY": 2.1116915316282108,
+            "timestamp": 0.6554347004505018
+        },
+        {
+            "x": 3.665027465532842,
+            "y": 5.858772631695159,
+            "heading": 2.626719096714767,
+            "angularVelocity": 0.0000011407549466335633,
+            "velocityX": 3.1386951447214493,
+            "velocityY": 2.111688890888465,
+            "timestamp": 0.7373640380068145
+        },
+        {
+            "x": 3.922178727707041,
+            "y": 6.031781832044647,
+            "heading": 2.6267191901760296,
+            "angularVelocity": 0.0000011407545266438845,
+            "velocityX": 3.138695732739822,
+            "velocityY": 2.1116880168910543,
+            "timestamp": 0.8192933755631272
+        },
+        {
+            "x": 4.179330140012749,
+            "y": 6.204790809246775,
+            "heading": 2.6267192836372875,
+            "angularVelocity": 0.0000011407544682834688,
+            "velocityX": 3.1386975651909763,
+            "velocityY": 2.111685293234714,
+            "timestamp": 0.9012227131194399
+        },
+        {
+            "x": 4.443624087689148,
+            "y": 6.354175617474429,
+            "heading": 2.642647782574834,
+            "angularVelocity": 0.1944175238399595,
+            "velocityX": 3.225876780643333,
+            "velocityY": 1.823337191332418,
+            "timestamp": 0.9831520506757526
+        },
+        {
+            "x": 4.674751705763445,
+            "y": 6.469415592948305,
+            "heading": 2.769232553851288,
+            "angularVelocity": 1.5450481482222196,
+            "velocityX": 2.821060501257377,
+            "velocityY": 1.4065776547341065,
+            "timestamp": 1.0650813882320653
+        },
+        {
+            "x": 4.870834347570915,
+            "y": 6.549065174445882,
+            "heading": 2.9849459088839683,
+            "angularVelocity": 2.6329195556404166,
+            "velocityX": 2.39331413698659,
+            "velocityY": 0.9721741182495338,
+            "timestamp": 1.1470107257883781
+        },
+        {
+            "x": 5.022947063703052,
+            "y": 6.5945122449585005,
+            "heading": 3.181337324435562,
+            "angularVelocity": 2.397082918150139,
+            "velocityX": 1.856633053179344,
+            "velocityY": 0.5547105819253249,
+            "timestamp": 1.228940063344691
+        },
+        {
+            "x": 5.124965104320653,
+            "y": 6.619330120195266,
+            "heading": 3.326379958974982,
+            "angularVelocity": 1.7703381824578692,
+            "velocityX": 1.2451954777185934,
+            "velocityY": 0.3029180508106365,
+            "timestamp": 1.3108694009010038
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.93745843058738,
+            "velocityX": 0.6229135643171693,
+            "velocityY": 0.13023271178533075,
+            "timestamp": 1.3927987384573166
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": -5.432893499766539e-33,
+            "velocityX": 0,
+            "velocityY": 6.79272724985994e-33,
+            "timestamp": 1.4747280760136294
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.1.traj
new file mode 100644
index 00000000..06c38028
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": -1.3306255005182603e-31,
+            "velocityY": -4.9176166375307755e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 2.322374290257025,
+            "y": 4.418269020876138,
+            "heading": 2.7110352663465282,
+            "angularVelocity": -0.44706114894751375,
+            "velocityX": 0.25671100431863964,
+            "velocityY": 0.6685485303865802,
+            "timestamp": 0.08715750349857801
+        },
+        {
+            "x": 2.3748782855305732,
+            "y": 4.5313296521256,
+            "heading": 2.6355865457570187,
+            "angularVelocity": -0.8656594964395732,
+            "velocityX": 0.6024036160513037,
+            "velocityY": 1.297199055859919,
+            "timestamp": 0.17431500699715602
+        },
+        {
+            "x": 2.469236718208719,
+            "y": 4.691158857574848,
+            "heading": 2.529430325019821,
+            "angularVelocity": -1.2179814298941005,
+            "velocityX": 1.0826197273959968,
+            "velocityY": 1.8337974245884079,
+            "timestamp": 0.26147251049573406
+        },
+        {
+            "x": 2.6203114191501853,
+            "y": 4.878748173786945,
+            "heading": 2.4055559201698435,
+            "angularVelocity": -1.421270686717166,
+            "velocityX": 1.7333527794761936,
+            "velocityY": 2.1523025405971743,
+            "timestamp": 0.34863001399431204
+        },
+        {
+            "x": 2.832356831160347,
+            "y": 5.08316808870822,
+            "heading": 2.2936588957805637,
+            "angularVelocity": -1.2838484341294274,
+            "velocityX": 2.4328991021825375,
+            "velocityY": 2.345408102752848,
+            "timestamp": 0.43578751749289
+        },
+        {
+            "x": 3.098575041354179,
+            "y": 5.2672072069550175,
+            "heading": 2.274563036259226,
+            "angularVelocity": -0.2190959900732964,
+            "velocityX": 3.054449697474129,
+            "velocityY": 2.1115694100829816,
+            "timestamp": 0.522945020991468
+        },
+        {
+            "x": 3.397459622153223,
+            "y": 5.3975878258937575,
+            "heading": 2.2845714271399826,
+            "angularVelocity": 0.11483108715843302,
+            "velocityX": 3.429246694794569,
+            "velocityY": 1.495919613402737,
+            "timestamp": 0.610102524490046
+        },
+        {
+            "x": 3.6781811959965687,
+            "y": 5.472346031669266,
+            "heading": 2.382872565425927,
+            "angularVelocity": 1.1278562870671076,
+            "velocityX": 3.220853771332781,
+            "velocityY": 0.8577368875271776,
+            "timestamp": 0.697260027988624
+        },
+        {
+            "x": 3.9150228109281926,
+            "y": 5.514569108720165,
+            "heading": 2.601398031866265,
+            "angularVelocity": 2.5072478865104606,
+            "velocityX": 2.717397876540693,
+            "velocityY": 0.484445691489864,
+            "timestamp": 0.784417531487202
+        },
+        {
+            "x": 4.091172675855641,
+            "y": 5.53970946369802,
+            "heading": 2.8415186627639684,
+            "angularVelocity": 2.7550196054160887,
+            "velocityX": 2.0210522084346136,
+            "velocityY": 0.28844739659465746,
+            "timestamp": 0.8715750349857799
+        },
+        {
+            "x": 4.206403236830247,
+            "y": 5.5538325492768035,
+            "heading": 3.0331865406048246,
+            "angularVelocity": 2.1990978418052483,
+            "velocityX": 1.322095704318632,
+            "velocityY": 0.16204096046663827,
+            "timestamp": 0.9587325384843579
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 1.2255222454474992,
+            "velocityX": 0.649361912605446,
+            "velocityY": 0.0707621314933191,
+            "timestamp": 1.045890041982936
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 2.336425691005573e-30,
+            "velocityX": -1.4618072975561068e-30,
+            "velocityY": 3.4366383722351896e-30,
+            "timestamp": 1.133047545481514
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.traj
new file mode 100644
index 00000000..06c38028
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": -1.3306255005182603e-31,
+            "velocityY": -4.9176166375307755e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 2.322374290257025,
+            "y": 4.418269020876138,
+            "heading": 2.7110352663465282,
+            "angularVelocity": -0.44706114894751375,
+            "velocityX": 0.25671100431863964,
+            "velocityY": 0.6685485303865802,
+            "timestamp": 0.08715750349857801
+        },
+        {
+            "x": 2.3748782855305732,
+            "y": 4.5313296521256,
+            "heading": 2.6355865457570187,
+            "angularVelocity": -0.8656594964395732,
+            "velocityX": 0.6024036160513037,
+            "velocityY": 1.297199055859919,
+            "timestamp": 0.17431500699715602
+        },
+        {
+            "x": 2.469236718208719,
+            "y": 4.691158857574848,
+            "heading": 2.529430325019821,
+            "angularVelocity": -1.2179814298941005,
+            "velocityX": 1.0826197273959968,
+            "velocityY": 1.8337974245884079,
+            "timestamp": 0.26147251049573406
+        },
+        {
+            "x": 2.6203114191501853,
+            "y": 4.878748173786945,
+            "heading": 2.4055559201698435,
+            "angularVelocity": -1.421270686717166,
+            "velocityX": 1.7333527794761936,
+            "velocityY": 2.1523025405971743,
+            "timestamp": 0.34863001399431204
+        },
+        {
+            "x": 2.832356831160347,
+            "y": 5.08316808870822,
+            "heading": 2.2936588957805637,
+            "angularVelocity": -1.2838484341294274,
+            "velocityX": 2.4328991021825375,
+            "velocityY": 2.345408102752848,
+            "timestamp": 0.43578751749289
+        },
+        {
+            "x": 3.098575041354179,
+            "y": 5.2672072069550175,
+            "heading": 2.274563036259226,
+            "angularVelocity": -0.2190959900732964,
+            "velocityX": 3.054449697474129,
+            "velocityY": 2.1115694100829816,
+            "timestamp": 0.522945020991468
+        },
+        {
+            "x": 3.397459622153223,
+            "y": 5.3975878258937575,
+            "heading": 2.2845714271399826,
+            "angularVelocity": 0.11483108715843302,
+            "velocityX": 3.429246694794569,
+            "velocityY": 1.495919613402737,
+            "timestamp": 0.610102524490046
+        },
+        {
+            "x": 3.6781811959965687,
+            "y": 5.472346031669266,
+            "heading": 2.382872565425927,
+            "angularVelocity": 1.1278562870671076,
+            "velocityX": 3.220853771332781,
+            "velocityY": 0.8577368875271776,
+            "timestamp": 0.697260027988624
+        },
+        {
+            "x": 3.9150228109281926,
+            "y": 5.514569108720165,
+            "heading": 2.601398031866265,
+            "angularVelocity": 2.5072478865104606,
+            "velocityX": 2.717397876540693,
+            "velocityY": 0.484445691489864,
+            "timestamp": 0.784417531487202
+        },
+        {
+            "x": 4.091172675855641,
+            "y": 5.53970946369802,
+            "heading": 2.8415186627639684,
+            "angularVelocity": 2.7550196054160887,
+            "velocityX": 2.0210522084346136,
+            "velocityY": 0.28844739659465746,
+            "timestamp": 0.8715750349857799
+        },
+        {
+            "x": 4.206403236830247,
+            "y": 5.5538325492768035,
+            "heading": 3.0331865406048246,
+            "angularVelocity": 2.1990978418052483,
+            "velocityX": 1.322095704318632,
+            "velocityY": 0.16204096046663827,
+            "timestamp": 0.9587325384843579
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 1.2255222454474992,
+            "velocityX": 0.649361912605446,
+            "velocityY": 0.0707621314933191,
+            "timestamp": 1.045890041982936
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 2.336425691005573e-30,
+            "velocityX": -1.4618072975561068e-30,
+            "velocityY": 3.4366383722351896e-30,
+            "timestamp": 1.133047545481514
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.1.traj
new file mode 100644
index 00000000..3d841ff3
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.1.traj	
@@ -0,0 +1,121 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": -2.857623496877536e-30,
+            "velocityX": -2.5600502945380354e-30,
+            "velocityY": 7.096305433334873e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2589562715242293,
+            "y": 4.250539211803029,
+            "heading": 2.836221335933998,
+            "angularVelocity": 0.7208664352087386,
+            "velocityX": -0.3431522593974162,
+            "velocityY": -0.9151633679526462,
+            "timestamp": 0.1196079214161022
+        },
+        {
+            "x": 2.2003066234183892,
+            "y": 4.024396154786088,
+            "heading": 2.9853349315252267,
+            "angularVelocity": 1.2466866226394515,
+            "velocityX": -0.49034919603525046,
+            "velocityY": -1.8907030097966573,
+            "timestamp": 0.2392158428322044
+        },
+        {
+            "x": 2.1964987876971427,
+            "y": 3.692388997223788,
+            "heading": 3.120971884356915,
+            "angularVelocity": 1.1340131257679336,
+            "velocityX": -0.03183598273568803,
+            "velocityY": -2.7757957301782654,
+            "timestamp": 0.3588237642483066
+        },
+        {
+            "x": 2.306992116857372,
+            "y": 3.3594977499513337,
+            "heading": 3.1303317506759742,
+            "angularVelocity": 0.07825456883065958,
+            "velocityX": 0.9237960818316977,
+            "velocityY": -2.7831872950485823,
+            "timestamp": 0.4784316856644088
+        },
+        {
+            "x": 2.522086295400282,
+            "y": 3.073270003946497,
+            "heading": 3.020615571964542,
+            "angularVelocity": -0.9172985987240034,
+            "velocityX": 1.7983272010439872,
+            "velocityY": -2.39305008076411,
+            "timestamp": 0.598039607080511
+        },
+        {
+            "x": 2.8292729817758957,
+            "y": 2.8628580598282483,
+            "heading": 2.911598621970614,
+            "angularVelocity": -0.9114525919611488,
+            "velocityX": 2.568280451149536,
+            "velocityY": -1.7591806765559896,
+            "timestamp": 0.7176475284966132
+        },
+        {
+            "x": 3.212139121131616,
+            "y": 2.7446490517974147,
+            "heading": 2.8046662811978735,
+            "angularVelocity": -0.8940239033224252,
+            "velocityX": 3.2010098898363166,
+            "velocityY": -0.9883041744332437,
+            "timestamp": 0.8372554499127154
+        },
+        {
+            "x": 3.61962025055875,
+            "y": 2.7431499033487023,
+            "heading": 2.702792215888405,
+            "angularVelocity": -0.8517334312273422,
+            "velocityX": 3.406807213124051,
+            "velocityY": -0.012533855876376997,
+            "timestamp": 0.9568633713288176
+        },
+        {
+            "x": 3.9493476486905155,
+            "y": 2.8320906325242436,
+            "heading": 2.608128750394254,
+            "angularVelocity": -0.7914481279620891,
+            "velocityX": 2.7567354588888784,
+            "velocityY": 0.7436023310372396,
+            "timestamp": 1.0764712927449198
+        },
+        {
+            "x": 4.162172137163363,
+            "y": 2.936272219295665,
+            "heading": 2.5785368758755864,
+            "angularVelocity": -0.2474073135651704,
+            "velocityX": 1.7793511161560736,
+            "velocityY": 0.8710258111499355,
+            "timestamp": 1.196079214161022
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -0.037931232495828804,
+            "velocityX": 0.842986498242598,
+            "velocityY": 0.5328056866955708,
+            "timestamp": 1.315687135577124
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 1.3483931579260095e-31,
+            "velocityY": -3.3355137434500586e-29,
+            "timestamp": 1.4352950569932261
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.traj
new file mode 100644
index 00000000..3d841ff3
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.traj	
@@ -0,0 +1,121 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": -2.857623496877536e-30,
+            "velocityX": -2.5600502945380354e-30,
+            "velocityY": 7.096305433334873e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2589562715242293,
+            "y": 4.250539211803029,
+            "heading": 2.836221335933998,
+            "angularVelocity": 0.7208664352087386,
+            "velocityX": -0.3431522593974162,
+            "velocityY": -0.9151633679526462,
+            "timestamp": 0.1196079214161022
+        },
+        {
+            "x": 2.2003066234183892,
+            "y": 4.024396154786088,
+            "heading": 2.9853349315252267,
+            "angularVelocity": 1.2466866226394515,
+            "velocityX": -0.49034919603525046,
+            "velocityY": -1.8907030097966573,
+            "timestamp": 0.2392158428322044
+        },
+        {
+            "x": 2.1964987876971427,
+            "y": 3.692388997223788,
+            "heading": 3.120971884356915,
+            "angularVelocity": 1.1340131257679336,
+            "velocityX": -0.03183598273568803,
+            "velocityY": -2.7757957301782654,
+            "timestamp": 0.3588237642483066
+        },
+        {
+            "x": 2.306992116857372,
+            "y": 3.3594977499513337,
+            "heading": 3.1303317506759742,
+            "angularVelocity": 0.07825456883065958,
+            "velocityX": 0.9237960818316977,
+            "velocityY": -2.7831872950485823,
+            "timestamp": 0.4784316856644088
+        },
+        {
+            "x": 2.522086295400282,
+            "y": 3.073270003946497,
+            "heading": 3.020615571964542,
+            "angularVelocity": -0.9172985987240034,
+            "velocityX": 1.7983272010439872,
+            "velocityY": -2.39305008076411,
+            "timestamp": 0.598039607080511
+        },
+        {
+            "x": 2.8292729817758957,
+            "y": 2.8628580598282483,
+            "heading": 2.911598621970614,
+            "angularVelocity": -0.9114525919611488,
+            "velocityX": 2.568280451149536,
+            "velocityY": -1.7591806765559896,
+            "timestamp": 0.7176475284966132
+        },
+        {
+            "x": 3.212139121131616,
+            "y": 2.7446490517974147,
+            "heading": 2.8046662811978735,
+            "angularVelocity": -0.8940239033224252,
+            "velocityX": 3.2010098898363166,
+            "velocityY": -0.9883041744332437,
+            "timestamp": 0.8372554499127154
+        },
+        {
+            "x": 3.61962025055875,
+            "y": 2.7431499033487023,
+            "heading": 2.702792215888405,
+            "angularVelocity": -0.8517334312273422,
+            "velocityX": 3.406807213124051,
+            "velocityY": -0.012533855876376997,
+            "timestamp": 0.9568633713288176
+        },
+        {
+            "x": 3.9493476486905155,
+            "y": 2.8320906325242436,
+            "heading": 2.608128750394254,
+            "angularVelocity": -0.7914481279620891,
+            "velocityX": 2.7567354588888784,
+            "velocityY": 0.7436023310372396,
+            "timestamp": 1.0764712927449198
+        },
+        {
+            "x": 4.162172137163363,
+            "y": 2.936272219295665,
+            "heading": 2.5785368758755864,
+            "angularVelocity": -0.2474073135651704,
+            "velocityX": 1.7793511161560736,
+            "velocityY": 0.8710258111499355,
+            "timestamp": 1.196079214161022
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -0.037931232495828804,
+            "velocityX": 0.842986498242598,
+            "velocityY": 0.5328056866955708,
+            "timestamp": 1.315687135577124
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": 0,
+            "velocityX": 1.3483931579260095e-31,
+            "velocityY": -3.3355137434500586e-29,
+            "timestamp": 1.4352950569932261
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.1.traj
new file mode 100644
index 00000000..1aabb2f0
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.1.traj	
@@ -0,0 +1,166 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": -2.979591758162217e-32,
+            "velocityX": -5.0524169564327655e-33,
+            "velocityY": 3.4190943066430706e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2859321803383907,
+            "y": 4.28632531036449,
+            "heading": 2.8392348343623115,
+            "angularVelocity": 0.9181215337942725,
+            "velocityX": -0.14474132503475426,
+            "velocityY": -0.7580259383384993,
+            "timestamp": 0.09719283458425766
+        },
+        {
+            "x": 2.269559639424535,
+            "y": 4.136180116377401,
+            "heading": 2.9969853519762393,
+            "angularVelocity": 1.623067361793753,
+            "velocityX": -0.1684541971009417,
+            "velocityY": -1.5448175231161345,
+            "timestamp": 0.19438566916851532
+        },
+        {
+            "x": 2.2724010855339243,
+            "y": 3.9101733835281753,
+            "heading": 3.1867879122348075,
+            "angularVelocity": 1.952845197595569,
+            "velocityX": 0.029235139828396976,
+            "velocityY": -2.325343568957208,
+            "timestamp": 0.291578503752773
+        },
+        {
+            "x": 2.3226715061490353,
+            "y": 3.6216059722814986,
+            "heading": 3.3538116791494788,
+            "angularVelocity": 1.7184781947055583,
+            "velocityX": 0.5172235260978133,
+            "velocityY": -2.969019398199714,
+            "timestamp": 0.38877133833703065
+        },
+        {
+            "x": 2.437426080371791,
+            "y": 3.296473518840458,
+            "heading": 3.427293312254084,
+            "angularVelocity": 0.7560396136085858,
+            "velocityX": 1.1806896538579041,
+            "velocityY": -3.3452306935155747,
+            "timestamp": 0.4859641729212883
+        },
+        {
+            "x": 2.6276283995685685,
+            "y": 2.9818185735083116,
+            "heading": 3.4272931426736677,
+            "angularVelocity": -0.000001744783111288276,
+            "velocityX": 1.9569582470803324,
+            "velocityY": -3.2374294532933714,
+            "timestamp": 0.583157007505546
+        },
+        {
+            "x": 2.8804858808256992,
+            "y": 2.7148958271590775,
+            "heading": 3.4272929522201836,
+            "angularVelocity": -0.00000195954243555236,
+            "velocityX": 2.6016062021313475,
+            "velocityY": -2.7463212436492483,
+            "timestamp": 0.6803498420898036
+        },
+        {
+            "x": 3.1843940400703103,
+            "y": 2.5079549973523347,
+            "heading": 3.4272928289705424,
+            "angularVelocity": -0.0000012680939050558275,
+            "velocityX": 3.126857659256241,
+            "velocityY": -2.1291778420902356,
+            "timestamp": 0.7775426766740613
+        },
+        {
+            "x": 3.5145691798617946,
+            "y": 2.3461864714628624,
+            "heading": 3.4272927145634307,
+            "angularVelocity": -0.0000011771146716870812,
+            "velocityX": 3.397114007465763,
+            "velocityY": -1.66440794304886,
+            "timestamp": 0.8747355112583189
+        },
+        {
+            "x": 3.844744525773579,
+            "y": 2.1844183662726744,
+            "heading": 3.427292600156351,
+            "angularVelocity": -0.0000011771143448425205,
+            "velocityX": 3.397116128201317,
+            "velocityY": -1.6644036145478303,
+            "timestamp": 0.9719283458425766
+        },
+        {
+            "x": 4.174919952487512,
+            "y": 2.022650426003138,
+            "heading": 3.42729248574922,
+            "angularVelocity": -0.0000011771148700868944,
+            "velocityX": 3.3971169595604263,
+            "velocityY": -1.6644019177082188,
+            "timestamp": 1.0691211804268343
+        },
+        {
+            "x": 4.503263696345804,
+            "y": 1.8851757396047895,
+            "heading": 3.3983831121261034,
+            "angularVelocity": -0.29744346634992885,
+            "velocityX": 3.3782710964525586,
+            "velocityY": -1.4144528965164656,
+            "timestamp": 1.166314015011092
+        },
+        {
+            "x": 4.772302673813676,
+            "y": 1.7735532891774939,
+            "heading": 3.2030632550848326,
+            "angularVelocity": -2.009611694902728,
+            "velocityX": 2.7680947738450667,
+            "velocityY": -1.1484637823843746,
+            "timestamp": 1.2635068495953496
+        },
+        {
+            "x": 4.977265937404966,
+            "y": 1.6951772631841453,
+            "heading": 2.8890004427677543,
+            "angularVelocity": -3.2313371007284832,
+            "velocityX": 2.1088310107223527,
+            "velocityY": -0.8063971621838382,
+            "timestamp": 1.3606996841796073
+        },
+        {
+            "x": 5.110692645903806,
+            "y": 1.6448171116124384,
+            "heading": 2.6382240127177106,
+            "angularVelocity": -2.5801946318649924,
+            "velocityX": 1.3728039630655127,
+            "velocityY": -0.5181467521461082,
+            "timestamp": 1.457892518763865
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -1.4221625833731872,
+            "velocityX": 0.6719358929652187,
+            "velocityY": -0.2553389014590782,
+            "timestamp": 1.5550853533481226
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 5.001833480552254e-32,
+            "velocityX": 0,
+            "velocityY": -3.22512482003026e-32,
+            "timestamp": 1.6522781879323802
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.traj
new file mode 100644
index 00000000..1aabb2f0
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.traj	
@@ -0,0 +1,166 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": -2.979591758162217e-32,
+            "velocityX": -5.0524169564327655e-33,
+            "velocityY": 3.4190943066430706e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2859321803383907,
+            "y": 4.28632531036449,
+            "heading": 2.8392348343623115,
+            "angularVelocity": 0.9181215337942725,
+            "velocityX": -0.14474132503475426,
+            "velocityY": -0.7580259383384993,
+            "timestamp": 0.09719283458425766
+        },
+        {
+            "x": 2.269559639424535,
+            "y": 4.136180116377401,
+            "heading": 2.9969853519762393,
+            "angularVelocity": 1.623067361793753,
+            "velocityX": -0.1684541971009417,
+            "velocityY": -1.5448175231161345,
+            "timestamp": 0.19438566916851532
+        },
+        {
+            "x": 2.2724010855339243,
+            "y": 3.9101733835281753,
+            "heading": 3.1867879122348075,
+            "angularVelocity": 1.952845197595569,
+            "velocityX": 0.029235139828396976,
+            "velocityY": -2.325343568957208,
+            "timestamp": 0.291578503752773
+        },
+        {
+            "x": 2.3226715061490353,
+            "y": 3.6216059722814986,
+            "heading": 3.3538116791494788,
+            "angularVelocity": 1.7184781947055583,
+            "velocityX": 0.5172235260978133,
+            "velocityY": -2.969019398199714,
+            "timestamp": 0.38877133833703065
+        },
+        {
+            "x": 2.437426080371791,
+            "y": 3.296473518840458,
+            "heading": 3.427293312254084,
+            "angularVelocity": 0.7560396136085858,
+            "velocityX": 1.1806896538579041,
+            "velocityY": -3.3452306935155747,
+            "timestamp": 0.4859641729212883
+        },
+        {
+            "x": 2.6276283995685685,
+            "y": 2.9818185735083116,
+            "heading": 3.4272931426736677,
+            "angularVelocity": -0.000001744783111288276,
+            "velocityX": 1.9569582470803324,
+            "velocityY": -3.2374294532933714,
+            "timestamp": 0.583157007505546
+        },
+        {
+            "x": 2.8804858808256992,
+            "y": 2.7148958271590775,
+            "heading": 3.4272929522201836,
+            "angularVelocity": -0.00000195954243555236,
+            "velocityX": 2.6016062021313475,
+            "velocityY": -2.7463212436492483,
+            "timestamp": 0.6803498420898036
+        },
+        {
+            "x": 3.1843940400703103,
+            "y": 2.5079549973523347,
+            "heading": 3.4272928289705424,
+            "angularVelocity": -0.0000012680939050558275,
+            "velocityX": 3.126857659256241,
+            "velocityY": -2.1291778420902356,
+            "timestamp": 0.7775426766740613
+        },
+        {
+            "x": 3.5145691798617946,
+            "y": 2.3461864714628624,
+            "heading": 3.4272927145634307,
+            "angularVelocity": -0.0000011771146716870812,
+            "velocityX": 3.397114007465763,
+            "velocityY": -1.66440794304886,
+            "timestamp": 0.8747355112583189
+        },
+        {
+            "x": 3.844744525773579,
+            "y": 2.1844183662726744,
+            "heading": 3.427292600156351,
+            "angularVelocity": -0.0000011771143448425205,
+            "velocityX": 3.397116128201317,
+            "velocityY": -1.6644036145478303,
+            "timestamp": 0.9719283458425766
+        },
+        {
+            "x": 4.174919952487512,
+            "y": 2.022650426003138,
+            "heading": 3.42729248574922,
+            "angularVelocity": -0.0000011771148700868944,
+            "velocityX": 3.3971169595604263,
+            "velocityY": -1.6644019177082188,
+            "timestamp": 1.0691211804268343
+        },
+        {
+            "x": 4.503263696345804,
+            "y": 1.8851757396047895,
+            "heading": 3.3983831121261034,
+            "angularVelocity": -0.29744346634992885,
+            "velocityX": 3.3782710964525586,
+            "velocityY": -1.4144528965164656,
+            "timestamp": 1.166314015011092
+        },
+        {
+            "x": 4.772302673813676,
+            "y": 1.7735532891774939,
+            "heading": 3.2030632550848326,
+            "angularVelocity": -2.009611694902728,
+            "velocityX": 2.7680947738450667,
+            "velocityY": -1.1484637823843746,
+            "timestamp": 1.2635068495953496
+        },
+        {
+            "x": 4.977265937404966,
+            "y": 1.6951772631841453,
+            "heading": 2.8890004427677543,
+            "angularVelocity": -3.2313371007284832,
+            "velocityX": 2.1088310107223527,
+            "velocityY": -0.8063971621838382,
+            "timestamp": 1.3606996841796073
+        },
+        {
+            "x": 5.110692645903806,
+            "y": 1.6448171116124384,
+            "heading": 2.6382240127177106,
+            "angularVelocity": -2.5801946318649924,
+            "velocityX": 1.3728039630655127,
+            "velocityY": -0.5181467521461082,
+            "timestamp": 1.457892518763865
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -1.4221625833731872,
+            "velocityX": 0.6719358929652187,
+            "velocityY": -0.2553389014590782,
+            "timestamp": 1.5550853533481226
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 5.001833480552254e-32,
+            "velocityX": 0,
+            "velocityY": -3.22512482003026e-32,
+            "timestamp": 1.6522781879323802
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.1.traj
new file mode 100644
index 00000000..edd74832
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 1.6975195374214185e-34,
+            "velocityY": -4.870070626129674e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 2.300139974954616,
+            "y": 4.403824386953961,
+            "heading": 2.777167813185812,
+            "angularVelocity": 0.371999186817317,
+            "velocityX": 0.0019166271843757909,
+            "velocityY": 0.6000717171507387,
+            "timestamp": 0.07303191552177725
+        },
+        {
+            "x": 2.3004163368424435,
+            "y": 4.4915620593167,
+            "heading": 2.8296003827219796,
+            "angularVelocity": 0.7179404943929278,
+            "velocityX": 0.003784124869971189,
+            "velocityY": 1.2013606891712643,
+            "timestamp": 0.1460638310435545
+        },
+        {
+            "x": 2.3008287718098006,
+            "y": 4.623340237169809,
+            "heading": 2.9042527526949042,
+            "angularVelocity": 1.022188305476718,
+            "velocityX": 0.005647325068915905,
+            "velocityY": 1.8043916404440152,
+            "timestamp": 0.21909574656533176
+        },
+        {
+            "x": 2.30139145489797,
+            "y": 4.799346158732557,
+            "heading": 2.995610111682823,
+            "angularVelocity": 1.2509237685362016,
+            "velocityX": 0.007704619057970502,
+            "velocityY": 2.409986377945481,
+            "timestamp": 0.292127662087109
+        },
+        {
+            "x": 2.3021599471353418,
+            "y": 5.019805776345521,
+            "heading": 3.0915564646710374,
+            "angularVelocity": 1.3137592284513517,
+            "velocityX": 0.01052269041392973,
+            "velocityY": 3.0186750003459215,
+            "timestamp": 0.36515957760888623
+        },
+        {
+            "x": 2.303349941373319,
+            "y": 5.282617464741668,
+            "heading": 3.1370629336033304,
+            "angularVelocity": 0.6231038664010262,
+            "velocityX": 0.0162941671387857,
+            "velocityY": 3.598586816715496,
+            "timestamp": 0.43819149313066347
+        },
+        {
+            "x": 2.30367850771832,
+            "y": 5.558892653761466,
+            "heading": 3.1370629627902624,
+            "angularVelocity": 3.996462650884406e-7,
+            "velocityX": 0.004498941903047958,
+            "velocityY": 3.7829377340844195,
+            "timestamp": 0.5112234086524408
+        },
+        {
+            "x": 2.304007097232836,
+            "y": 5.835167842753699,
+            "heading": 3.137062991977204,
+            "angularVelocity": 3.996463893760175e-7,
+            "velocityX": 0.004499259154964363,
+            "velocityY": 3.7829377337070014,
+            "timestamp": 0.584255324174218
+        },
+        {
+            "x": 2.3020927945039733,
+            "y": 6.097639248519623,
+            "heading": 3.182327539673564,
+            "angularVelocity": 0.6197913251072079,
+            "velocityX": -0.026211865253511206,
+            "velocityY": 3.5939274478930847,
+            "timestamp": 0.6572872396959952
+        },
+        {
+            "x": 2.3012287765145363,
+            "y": 6.317987502538012,
+            "heading": 3.2805685838685283,
+            "angularVelocity": 1.3451796176107471,
+            "velocityX": -0.011830690503794171,
+            "velocityY": 3.0171501383211403,
+            "timestamp": 0.7303191552177725
+        },
+        {
+            "x": 2.3006859363348666,
+            "y": 6.493881460544365,
+            "heading": 3.3743823922179033,
+            "angularVelocity": 1.284559054478048,
+            "velocityX": -0.007432917181360783,
+            "velocityY": 2.40845330085727,
+            "timestamp": 0.8033510707395497
+        },
+        {
+            "x": 2.3003278852317357,
+            "y": 6.625559358254098,
+            "heading": 3.451128438683188,
+            "angularVelocity": 1.050856271767924,
+            "velocityX": -0.004902666191527756,
+            "velocityY": 1.80301854016781,
+            "timestamp": 0.8763829862613269
+        },
+        {
+            "x": 2.300106073487947,
+            "y": 6.713219846040187,
+            "heading": 3.5051273882405054,
+            "angularVelocity": 0.739388377964907,
+            "velocityX": -0.0030371891823418365,
+            "velocityY": 1.2003038282618885,
+            "timestamp": 0.9494149017831042
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 0.3841870877769111,
+            "velocityX": -0.0014524264794266268,
+            "velocityY": 0.5994660505208633,
+            "timestamp": 1.0224468173048815
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": -9.351204897899337e-33,
+            "velocityX": 1.378221818744695e-30,
+            "velocityY": 0,
+            "timestamp": 1.0954787328266589
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.traj
new file mode 100644
index 00000000..edd74832
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 1.6975195374214185e-34,
+            "velocityY": -4.870070626129674e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 2.300139974954616,
+            "y": 4.403824386953961,
+            "heading": 2.777167813185812,
+            "angularVelocity": 0.371999186817317,
+            "velocityX": 0.0019166271843757909,
+            "velocityY": 0.6000717171507387,
+            "timestamp": 0.07303191552177725
+        },
+        {
+            "x": 2.3004163368424435,
+            "y": 4.4915620593167,
+            "heading": 2.8296003827219796,
+            "angularVelocity": 0.7179404943929278,
+            "velocityX": 0.003784124869971189,
+            "velocityY": 1.2013606891712643,
+            "timestamp": 0.1460638310435545
+        },
+        {
+            "x": 2.3008287718098006,
+            "y": 4.623340237169809,
+            "heading": 2.9042527526949042,
+            "angularVelocity": 1.022188305476718,
+            "velocityX": 0.005647325068915905,
+            "velocityY": 1.8043916404440152,
+            "timestamp": 0.21909574656533176
+        },
+        {
+            "x": 2.30139145489797,
+            "y": 4.799346158732557,
+            "heading": 2.995610111682823,
+            "angularVelocity": 1.2509237685362016,
+            "velocityX": 0.007704619057970502,
+            "velocityY": 2.409986377945481,
+            "timestamp": 0.292127662087109
+        },
+        {
+            "x": 2.3021599471353418,
+            "y": 5.019805776345521,
+            "heading": 3.0915564646710374,
+            "angularVelocity": 1.3137592284513517,
+            "velocityX": 0.01052269041392973,
+            "velocityY": 3.0186750003459215,
+            "timestamp": 0.36515957760888623
+        },
+        {
+            "x": 2.303349941373319,
+            "y": 5.282617464741668,
+            "heading": 3.1370629336033304,
+            "angularVelocity": 0.6231038664010262,
+            "velocityX": 0.0162941671387857,
+            "velocityY": 3.598586816715496,
+            "timestamp": 0.43819149313066347
+        },
+        {
+            "x": 2.30367850771832,
+            "y": 5.558892653761466,
+            "heading": 3.1370629627902624,
+            "angularVelocity": 3.996462650884406e-7,
+            "velocityX": 0.004498941903047958,
+            "velocityY": 3.7829377340844195,
+            "timestamp": 0.5112234086524408
+        },
+        {
+            "x": 2.304007097232836,
+            "y": 5.835167842753699,
+            "heading": 3.137062991977204,
+            "angularVelocity": 3.996463893760175e-7,
+            "velocityX": 0.004499259154964363,
+            "velocityY": 3.7829377337070014,
+            "timestamp": 0.584255324174218
+        },
+        {
+            "x": 2.3020927945039733,
+            "y": 6.097639248519623,
+            "heading": 3.182327539673564,
+            "angularVelocity": 0.6197913251072079,
+            "velocityX": -0.026211865253511206,
+            "velocityY": 3.5939274478930847,
+            "timestamp": 0.6572872396959952
+        },
+        {
+            "x": 2.3012287765145363,
+            "y": 6.317987502538012,
+            "heading": 3.2805685838685283,
+            "angularVelocity": 1.3451796176107471,
+            "velocityX": -0.011830690503794171,
+            "velocityY": 3.0171501383211403,
+            "timestamp": 0.7303191552177725
+        },
+        {
+            "x": 2.3006859363348666,
+            "y": 6.493881460544365,
+            "heading": 3.3743823922179033,
+            "angularVelocity": 1.284559054478048,
+            "velocityX": -0.007432917181360783,
+            "velocityY": 2.40845330085727,
+            "timestamp": 0.8033510707395497
+        },
+        {
+            "x": 2.3003278852317357,
+            "y": 6.625559358254098,
+            "heading": 3.451128438683188,
+            "angularVelocity": 1.050856271767924,
+            "velocityX": -0.004902666191527756,
+            "velocityY": 1.80301854016781,
+            "timestamp": 0.8763829862613269
+        },
+        {
+            "x": 2.300106073487947,
+            "y": 6.713219846040187,
+            "heading": 3.5051273882405054,
+            "angularVelocity": 0.739388377964907,
+            "velocityX": -0.0030371891823418365,
+            "velocityY": 1.2003038282618885,
+            "timestamp": 0.9494149017831042
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 0.3841870877769111,
+            "velocityX": -0.0014524264794266268,
+            "velocityY": 0.5994660505208633,
+            "timestamp": 1.0224468173048815
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": -9.351204897899337e-33,
+            "velocityX": 1.378221818744695e-30,
+            "velocityY": 0,
+            "timestamp": 1.0954787328266589
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj
new file mode 100644
index 00000000..5479ee40
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": -7.080178799155261e-34,
+            "velocityY": 4.018324582530293e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 2.298001228420339,
+            "y": 4.408001448490065,
+            "heading": 2.765522898073599,
+            "angularVelocity": 0.20408324361211075,
+            "velocityX": -0.026278326719846846,
+            "velocityY": 0.6310864929657463,
+            "timestamp": 0.07606160015322963
+        },
+        {
+            "x": 2.294003714931991,
+            "y": 4.50400409446738,
+            "heading": 2.7965834623359793,
+            "angularVelocity": 0.40836064715713705,
+            "velocityX": -0.052556263348324604,
+            "velocityY": 1.2621696859376152,
+            "timestamp": 0.15212320030645926
+        },
+        {
+            "x": 2.288007388147497,
+            "y": 4.648007477823444,
+            "heading": 2.8432069040547985,
+            "angularVelocity": 0.6129695092516301,
+            "velocityX": -0.07883513852474495,
+            "velocityY": 1.8932468297532885,
+            "timestamp": 0.2281848004596889
+        },
+        {
+            "x": 2.2800119392615206,
+            "y": 4.840010948654735,
+            "heading": 2.9054273803096926,
+            "angularVelocity": 0.8180274426195046,
+            "velocityX": -0.10511807363859851,
+            "velocityY": 2.5243154291323253,
+            "timestamp": 0.3042464006129185
+        },
+        {
+            "x": 2.270016643226651,
+            "y": 5.0800137105269405,
+            "heading": 2.98328446819424,
+            "angularVelocity": 1.0236057054768744,
+            "velocityX": -0.13141054112368558,
+            "velocityY": 3.155373557599477,
+            "timestamp": 0.3803080007661481
+        },
+        {
+            "x": 2.262010502624602,
+            "y": 5.272008371939251,
+            "heading": 3.045963904195738,
+            "angularVelocity": 0.8240614958826565,
+            "velocityX": -0.10525864018007076,
+            "velocityY": 2.524199609599701,
+            "timestamp": 0.4563696009193777
+        },
+        {
+            "x": 2.2560053812396474,
+            "y": 5.416004211679494,
+            "heading": 3.092980743508332,
+            "angularVelocity": 0.6181416012531585,
+            "velocityX": -0.07895076323476027,
+            "velocityY": 1.8931476520367212,
+            "timestamp": 0.5324312010726073
+        },
+        {
+            "x": 2.2520017956431593,
+            "y": 5.512001402686665,
+            "heading": 3.124326930482601,
+            "angularVelocity": 0.412115797079227,
+            "velocityX": -0.05263609480240691,
+            "velocityY": 1.2620979681439777,
+            "timestamp": 0.6084928012258369
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0.20605758340377522,
+            "velocityX": -0.026318084803984385,
+            "velocityY": 0.6310490078651869,
+            "timestamp": 0.6845544013790665
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -1.7962813810508635e-31,
+            "velocityX": -5.242761228396242e-32,
+            "velocityY": 0,
+            "timestamp": 0.7606160015322961
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.traj
new file mode 100644
index 00000000..5479ee40
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": -7.080178799155261e-34,
+            "velocityY": 4.018324582530293e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 2.298001228420339,
+            "y": 4.408001448490065,
+            "heading": 2.765522898073599,
+            "angularVelocity": 0.20408324361211075,
+            "velocityX": -0.026278326719846846,
+            "velocityY": 0.6310864929657463,
+            "timestamp": 0.07606160015322963
+        },
+        {
+            "x": 2.294003714931991,
+            "y": 4.50400409446738,
+            "heading": 2.7965834623359793,
+            "angularVelocity": 0.40836064715713705,
+            "velocityX": -0.052556263348324604,
+            "velocityY": 1.2621696859376152,
+            "timestamp": 0.15212320030645926
+        },
+        {
+            "x": 2.288007388147497,
+            "y": 4.648007477823444,
+            "heading": 2.8432069040547985,
+            "angularVelocity": 0.6129695092516301,
+            "velocityX": -0.07883513852474495,
+            "velocityY": 1.8932468297532885,
+            "timestamp": 0.2281848004596889
+        },
+        {
+            "x": 2.2800119392615206,
+            "y": 4.840010948654735,
+            "heading": 2.9054273803096926,
+            "angularVelocity": 0.8180274426195046,
+            "velocityX": -0.10511807363859851,
+            "velocityY": 2.5243154291323253,
+            "timestamp": 0.3042464006129185
+        },
+        {
+            "x": 2.270016643226651,
+            "y": 5.0800137105269405,
+            "heading": 2.98328446819424,
+            "angularVelocity": 1.0236057054768744,
+            "velocityX": -0.13141054112368558,
+            "velocityY": 3.155373557599477,
+            "timestamp": 0.3803080007661481
+        },
+        {
+            "x": 2.262010502624602,
+            "y": 5.272008371939251,
+            "heading": 3.045963904195738,
+            "angularVelocity": 0.8240614958826565,
+            "velocityX": -0.10525864018007076,
+            "velocityY": 2.524199609599701,
+            "timestamp": 0.4563696009193777
+        },
+        {
+            "x": 2.2560053812396474,
+            "y": 5.416004211679494,
+            "heading": 3.092980743508332,
+            "angularVelocity": 0.6181416012531585,
+            "velocityX": -0.07895076323476027,
+            "velocityY": 1.8931476520367212,
+            "timestamp": 0.5324312010726073
+        },
+        {
+            "x": 2.2520017956431593,
+            "y": 5.512001402686665,
+            "heading": 3.124326930482601,
+            "angularVelocity": 0.412115797079227,
+            "velocityX": -0.05263609480240691,
+            "velocityY": 1.2620979681439777,
+            "timestamp": 0.6084928012258369
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0.20605758340377522,
+            "velocityX": -0.026318084803984385,
+            "velocityY": 0.6310490078651869,
+            "timestamp": 0.6845544013790665
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -1.7962813810508635e-31,
+            "velocityX": -5.242761228396242e-32,
+            "velocityY": 0,
+            "timestamp": 0.7606160015322961
+        }
+    ]
+}
\ No newline at end of file

From 56de999fef5c9b8fabcba6551d9ec5850a7ec50c Mon Sep 17 00:00:00 2001
From: Lewis-Seiden <lewy@seiden.us>
Date: Sun, 18 Feb 2024 17:33:10 -0800
Subject: [PATCH 32/62] worked on elevator tuning

---
 src/main/java/frc/robot/Robot.java               | 10 ++++++----
 .../subsystems/elevator/ElevatorIOReal.java      | 16 ++++++++--------
 .../subsystems/elevator/ElevatorSubsystem.java   |  6 +++---
 .../robot/subsystems/intake/IntakeIOReal.java    |  2 +-
 .../robot/subsystems/intake/IntakeSubsystem.java | 12 ++++++------
 5 files changed, 24 insertions(+), 22 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 099cc73a..c620217e 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -26,6 +26,7 @@
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.subsystems.carriage.CarriageIOReal;
 import frc.robot.subsystems.carriage.CarriageSubsystem;
+import frc.robot.subsystems.elevator.ElevatorIOReal;
 import frc.robot.subsystems.elevator.ElevatorIOSim;
 import frc.robot.subsystems.elevator.ElevatorSubsystem;
 import frc.robot.subsystems.feeder.FeederIOReal;
@@ -91,7 +92,8 @@ public void setYaw(Rotation2d yaw) {}
               : SwerveSubsystem.createSimModules());
   private final IntakeSubsystem intake = new IntakeSubsystem(new IntakeIOReal());
   private final FeederSubsystem feeder = new FeederSubsystem(new FeederIOReal());
-  private final ElevatorSubsystem elevator = new ElevatorSubsystem(new ElevatorIOSim());
+  private final ElevatorSubsystem elevator =
+      new ElevatorSubsystem(mode == RobotMode.REAL ? new ElevatorIOReal() : new ElevatorIOSim());
   private final ShooterSubystem shooter =
       new ShooterSubystem(mode == RobotMode.REAL ? new ShooterIOReal() : new ShooterIOSim());
   private final CarriageSubsystem carriage = new CarriageSubsystem(new CarriageIOReal());
@@ -163,10 +165,10 @@ public void robotInit() {
         carriage
             .runVoltageCmd(CarriageSubsystem.INDEXING_VOLTAGE)
             .withInterruptBehavior(InterruptionBehavior.kCancelSelf));
-    intake.setDefaultCommand(intake.runVoltageCmd(0.0));
+    intake.setDefaultCommand(intake.runVoltageCmd(0.0, 0.0));
     shooter.setDefaultCommand(
         shooter.runStateCmd(
-            () -> Rotation2d.fromDegrees(0.0), () -> flywheelIdleSpeed, () -> flywheelIdleSpeed));
+            () -> Rotation2d.fromDegrees(5.0), () -> flywheelIdleSpeed, () -> flywheelIdleSpeed));
     // reactionBarRelease.setDefaultCommand(
     //     reactionBarRelease.setRotationCmd(Rotation2d.fromDegrees(0.0)));
     leds.setDefaultCommand(
@@ -198,7 +200,7 @@ public void robotInit() {
                 () -> feeder.getFirstBeambreak()));
 
     // ---- Controller bindings here ----
-    controller.leftTrigger().whileTrue(intake.runVelocityCmd(80.0));
+    controller.leftTrigger().whileTrue(intake.runVelocityCmd(80.0, 30.0));
     controller
         .rightTrigger()
         .and(() -> currentTarget == Target.SPEAKER)
diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java
index 255801bc..ff5f69d2 100644
--- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java
+++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java
@@ -32,15 +32,15 @@ public class ElevatorIOReal implements ElevatorIO {
   public ElevatorIOReal() {
     var config = new TalonFXConfiguration();
 
-    config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+    config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
 
     config.Slot0.GravityType = GravityTypeValue.Elevator_Static;
-    config.Slot0.kG = 0.0;
-    config.Slot0.kS = 0.0;
-    config.Slot0.kV = 0.0;
-    config.Slot0.kA = 0.0;
-    config.Slot0.kP = 0.0;
-    config.Slot0.kD = 0.0;
+    config.Slot0.kG = 0.11591;
+    config.Slot0.kS = 0.16898;
+    config.Slot0.kV = 10.911;
+    config.Slot0.kA = 0.28688;
+    config.Slot0.kP = 69.785;
+    config.Slot0.kD = 17.53;
 
     config.CurrentLimits.StatorCurrentLimit = 60.0;
     config.CurrentLimits.StatorCurrentLimitEnable = true;
@@ -52,7 +52,7 @@ public ElevatorIOReal() {
 
     // Carriage position meters in direction of elevator
     config.Feedback.SensorToMechanismRatio =
-        ElevatorSubsystem.GEAR_RATIO * 2 * Math.PI * ElevatorSubsystem.DRUM_RADIUS_METERS;
+        ElevatorSubsystem.GEAR_RATIO / (2 * Math.PI * ElevatorSubsystem.DRUM_RADIUS_METERS);
 
     motor.getConfigurator().apply(config);
     motor.setPosition(0.0); // Assume we boot 0ed
diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java
index e24b6678..511a3dbd 100644
--- a/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorSubsystem.java
@@ -32,10 +32,10 @@ public class ElevatorSubsystem extends SubsystemBase {
   public static final double DRUM_RADIUS_METERS = Units.inchesToMeters(1.751 / 2.0);
   public static final Rotation2d ELEVATOR_ANGLE = Rotation2d.fromDegrees(65.0);
 
-  public static final double MAX_EXTENSION_METERS = Units.inchesToMeters(31.3);
+  public static final double MAX_EXTENSION_METERS = Units.inchesToMeters(30);
 
-  public static final double CLIMB_EXTENSION_METERS = 0.6;
-  public static final double TRAP_EXTENSION_METERS = 0.9;
+  public static final double CLIMB_EXTENSION_METERS = 0.65;
+  public static final double TRAP_EXTENSION_METERS = Units.inchesToMeters(29);
   public static final double AMP_EXTENSION_METERS = 0.6;
 
   private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged();
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java
index 3d4c9eca..cf0c143b 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java
@@ -51,7 +51,7 @@ public IntakeIOReal() {
     centeringConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
 
     centeringConfig.Slot0.kV = (12.0 * 60.0) / 5800;
-    centeringConfig.Slot0.kP = 1.0;
+    centeringConfig.Slot0.kP = 0.1;
 
     centeringMotor.getConfigurator().apply(centeringConfig);
 
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java
index fa2491c7..8c90b0f2 100644
--- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java
@@ -25,19 +25,19 @@ public void periodic() {
   }
 
   /** Run the intake and centering motors at the specified voltage */
-  public Command runVoltageCmd(double voltage) {
+  public Command runVoltageCmd(double intakeVoltage, double centeringVoltage) {
     return this.run(
         () -> {
-          io.setIntakeVoltage(voltage);
-          io.setCenteringVoltage(voltage);
+          io.setIntakeVoltage(intakeVoltage);
+          io.setCenteringVoltage(centeringVoltage);
         });
   }
 
-  public Command runVelocityCmd(double velocity) {
+  public Command runVelocityCmd(double intakeVelocity, double centeringVelocity) {
     return this.run(
         () -> {
-          io.setIntakeSpeed(velocity);
-          io.setCenteringSpeed(velocity);
+          io.setIntakeSpeed(intakeVelocity);
+          io.setCenteringSpeed(centeringVelocity);
         });
   }
 }

From 770ec7340e9fc61717b3d4e35a524016d5eb7fe2 Mon Sep 17 00:00:00 2001
From: stoopipoopy <anotherguywithabeanie@gmail.com>
Date: Sun, 18 Feb 2024 18:34:46 -0800
Subject: [PATCH 33/62] inverted all the choreo angles

---
 .../robot/utils/dynamicauto/ChoreoBuilder.py  | 30 +++++++++----------
 .../robot/utils/dynamicauto/DynamicAuto.java  | 24 +++++++--------
 2 files changed, 27 insertions(+), 27 deletions(-)

diff --git a/src/main/java/frc/robot/utils/dynamicauto/ChoreoBuilder.py b/src/main/java/frc/robot/utils/dynamicauto/ChoreoBuilder.py
index 7078fb9f..f34e3980 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/ChoreoBuilder.py
+++ b/src/main/java/frc/robot/utils/dynamicauto/ChoreoBuilder.py
@@ -3,22 +3,22 @@
 # Assuming locations is a list of dictionaries where each dictionary represents a location
 # L,M, and R were change to S1, S2, and S4 with an S3 being added.
 locations = [
-    {"name": "Amp Side", "x": 0.71, "y": 6.72, "angle": -2.10, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]},
-    {"name": "Center", "x": 1.35, "y": 5.56, "angle": 3.14, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]},
-    {"name": "Stage Side", "x": 0.71, "y": 4.36, "angle": 2.10, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]},
-    {"name": "W1", "x": 2.3, "y": 6.757, "angle": -2.75, "allowed_destinations": [ "W2", "W3", "S1", "S2", "S3", "S4", "C1", "C2", "C3", "C4", "C5"]},
-    {"name": "W2", "x": 2.25, "y": 5.56, "angle": 3.14, "allowed_destinations": ["W1", "W3", "S1", "S2", "S3", "S4", "C1", "C2", "C3", "C4", "C5"]},
-    {"name": "W3", "x": 2.3, "y": 4.36, "angle": 2.75, "allowed_destinations": ["W1", "W2", "S1", "S2", "S3", "S4", "C1", "C2", "C3", "C4", "C5"]},
-    {"name": "S1", "x": 5.176, "y": 6.63, "angle": -2.88, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]},
-    {"name": "S2", "x": 4.263, "y": 5.56, "angle": 3.14, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]},
-    {"name": "S3", "x": 4.263, "y": 3, "angle": 2.574, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]},
-    {"name": "S4", "x": 5.176, "y": 1.62, "angle": 2.5, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]},
+    {"name": "Amp Side", "x": 0.71, "y": 6.72, "angle": 1.04, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]},
+    {"name": "Center", "x": 1.35, "y": 5.56, "angle": 0, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]},
+    {"name": "Stage Side", "x": 0.71, "y": 4.36, "angle": -1.04, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]},
+    {"name": "W1", "x": 2.3, "y": 6.757, "angle": 0.39, "allowed_destinations": [ "W2", "W3", "S1", "S2", "S3", "S4", "C1", "C2", "C3", "C4", "C5"]},
+    {"name": "W2", "x": 2.25, "y": 5.56, "angle": 0, "allowed_destinations": ["W1", "W3", "S1", "S2", "S3", "S4", "C1", "C2", "C3", "C4", "C5"]},
+    {"name": "W3", "x": 2.3, "y": 4.36, "angle": -0.39, "allowed_destinations": ["W1", "W2", "S1", "S2", "S3", "S4", "C1", "C2", "C3", "C4", "C5"]},
+    {"name": "S1", "x": 5.176, "y": 6.63, "angle": 0.26, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]},
+    {"name": "S2", "x": 4.263, "y": 5.56, "angle": 0, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]},
+    {"name": "S3", "x": 4.263, "y": 3, "angle": -0.544, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]},
+    {"name": "S4", "x": 5.176, "y": 1.62, "angle": -2.89, "allowed_destinations": ["W1", "W2", "W3", "C1", "C2", "C3", "C4", "C5"]},
 
-    {"name": "C1", "x": 7.68, "y": 7.467, "angle": 3.14, "allowed_destinations": ["W1", "W2", "W3", "S1", "S2", "S3", "S4", "C2", "C3", "C4", "C5"]},
-    {"name": "C2", "x": 7.68, "y": 5.797, "angle": 3.14, "allowed_destinations": ["W1", "W2", "W3", "S1", "S2", "S3", "S4", "C1",  "C3", "C4", "C5"]},
-    {"name": "C3", "x": 7.68, "y": 4.127, "angle": 3.14, "allowed_destinations": ["W1", "W2", "W3", "S1", "S2", "S3", "S4", "C1", "C2",  "C4", "C5"]},
-    {"name": "C4", "x": 7.68, "y": 2.457, "angle": 3.14, "allowed_destinations": ["W1", "W2", "W3", "S3", "S4", "S3", "S4", "C1", "C2", "C3",  "C5"]},
-    {"name": "C5", "x": 7.68, "y": 0.787, "angle": 3.14, "allowed_destinations": ["W1", "W2", "W3", "S3", "S4", "S3", "S4", "C1", "C2", "C3", "C4"]},
+    {"name": "C1", "x": 7.68, "y": 7.467, "angle": 0, "allowed_destinations": ["W1", "W2", "W3", "S1", "S2", "S3", "S4", "C2", "C3", "C4", "C5"]},
+    {"name": "C2", "x": 7.68, "y": 5.797, "angle": 0, "allowed_destinations": ["W1", "W2", "W3", "S1", "S2", "S3", "S4", "C1",  "C3", "C4", "C5"]},
+    {"name": "C3", "x": 7.68, "y": 4.127, "angle": 0, "allowed_destinations": ["W1", "W2", "W3", "S1", "S2", "S3", "S4", "C1", "C2",  "C4", "C5"]},
+    {"name": "C4", "x": 7.68, "y": 2.457, "angle": 0, "allowed_destinations": ["W1", "W2", "W3", "S3", "S4", "S3", "S4", "C1", "C2", "C3",  "C5"]},
+    {"name": "C5", "x": 7.68, "y": 0.787, "angle": 0, "allowed_destinations": ["W1", "W2", "W3", "S3", "S4", "S3", "S4", "C1", "C2", "C3", "C4"]},
 ]
 
 obastacles = [
diff --git a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
index 4117eaf1..f665e5d7 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
@@ -9,21 +9,21 @@
 public class DynamicAuto {
 
   public static final Note[] notes = {
-    new Note(new Pose2d(2.204, 7.0, Rotation2d.fromRadians(-Math.PI)), true, 0, "W1"), // w1
-    new Note(new Pose2d(2.204, 5.5, Rotation2d.fromRadians(-Math.PI)), false, 1, "W2"), // w2
-    new Note(new Pose2d(2.204, 4.1, Rotation2d.fromRadians(-Math.PI)), false, 2, "W3"), // w3
-    new Note(new Pose2d(7.538, 7.3, Rotation2d.fromRadians(-Math.PI)), true, 0, "C1"), // c1
-    new Note(new Pose2d(7.538, 5.7, Rotation2d.fromRadians(-Math.PI)), false, 0, "C2"), // c2
-    new Note(new Pose2d(7.538, 4.1, Rotation2d.fromRadians(-Math.PI)), false, 1, "C3"), // c3
-    new Note(new Pose2d(7.538, 2.5, Rotation2d.fromRadians(-Math.PI)), false, 2, "C4"), // c4
-    new Note(new Pose2d(7.538, 0.7, Rotation2d.fromRadians(-Math.PI)), true, 3, "C5") // c5
+    new Note(new Pose2d(2.204, 7.0, Rotation2d.fromRadians(Math.PI)), true, 0, "W1"), // w1
+    new Note(new Pose2d(2.204, 5.5, Rotation2d.fromRadians(Math.PI)), false, 1, "W2"), // w2
+    new Note(new Pose2d(2.204, 4.1, Rotation2d.fromRadians(Math.PI)), false, 2, "W3"), // w3
+    new Note(new Pose2d(7.538, 7.3, Rotation2d.fromRadians(Math.PI)), true, 0, "C1"), // c1
+    new Note(new Pose2d(7.538, 5.7, Rotation2d.fromRadians(Math.PI)), false, 0, "C2"), // c2
+    new Note(new Pose2d(7.538, 4.1, Rotation2d.fromRadians(Math.PI)), false, 1, "C3"), // c3
+    new Note(new Pose2d(7.538, 2.5, Rotation2d.fromRadians(Math.PI)), false, 2, "C4"), // c4
+    new Note(new Pose2d(7.538, 0.7, Rotation2d.fromRadians(Math.PI)), true, 3, "C5") // c5
   };
 
   public static final ShootingLocation[] shootingLocations = {
-    new ShootingLocation(new Pose2d(5.639, 6.463, Rotation2d.fromRadians(0.1)), "S1"), // Left
-    new ShootingLocation(new Pose2d(4.216, 5.216, Rotation2d.fromRadians(0)), "S2"),
-    new ShootingLocation(new Pose2d(4.263, 3.000, Rotation2d.fromRadians(2.574)), "S3"), // Middle
-    new ShootingLocation(new Pose2d(5.176, 1.620, Rotation2d.fromRadians(-0.179)), "S4") // Right
+    new ShootingLocation(new Pose2d(5.639, 6.463, Rotation2d.fromRadians(-3.04)), "S1"), // Left
+    new ShootingLocation(new Pose2d(4.216, 5.216, Rotation2d.fromRadians(-Math.PI)), "S2"),
+    new ShootingLocation(new Pose2d(4.263, 3.000, Rotation2d.fromRadians(-0.566)), "S3"), // Middle
+    new ShootingLocation(new Pose2d(5.176, 1.620, Rotation2d.fromRadians(2.961)), "S4") // Right
   };
 
   public static ShootingLocation closestShootingLocation(Supplier<Pose2d> curPose) {

From d0f0c758c69e28e7bb3125d84fbdd89b834322d6 Mon Sep 17 00:00:00 2001
From: Awesomeplayer165 <dev@jacob2.dev>
Date: Sun, 18 Feb 2024 18:41:05 -0800
Subject: [PATCH 34/62] Fine turned till W3 to C5

---
 .../frc/robot/utils/dynamicauto/data.chor     | 15449 +++++++++++++++-
 .../deploy/choreo/Amp Side To C3.1.traj       |   526 +
 .../deploy/choreo/Amp Side To C3.traj         |   526 +
 .../deploy/choreo/Amp Side To C4.1.traj       |   193 +
 .../deploy/choreo/Amp Side To C4.traj         |   193 +
 .../deploy/choreo/Amp Side To C5.1.traj       |  1294 +-
 .../deploy/choreo/Amp Side To C5.traj         |  1294 +-
 .../deploy/choreo/Amp Side To W2.1.traj       |   148 +
 .../deploy/choreo/Amp Side To W2.traj         |   148 +
 .../deploy/choreo/Center To C2.1.traj         |   211 +
 .../deploy/choreo/Center To C2.traj           |   211 +
 .../deploy/choreo/Center To C4.1.traj         |  1093 ++
 .../deploy/choreo/Center To C4.traj           |  1093 ++
 .../deploy/choreo/Center To C5.1.traj         |   625 +
 .../deploy/choreo/Center To C5.traj           |   625 +
 .../deploy/choreo/Center To W1.1.traj         |   112 +
 .../deploy/choreo/Center To W1.traj           |   112 +
 .../dynamicauto/deploy/choreo/New Path.1.traj |   625 +
 .../dynamicauto/deploy/choreo/New Path.traj   |   625 +
 .../deploy/choreo/Stage Side To C1.1.traj     |   526 +
 .../deploy/choreo/Stage Side To C1.traj       |   526 +
 .../deploy/choreo/Stage Side To C2.1.traj     |   517 +
 .../deploy/choreo/Stage Side To C2.traj       |   517 +
 .../deploy/choreo/Stage Side To C4.1.traj     |   580 +
 .../deploy/choreo/Stage Side To C4.traj       |   580 +
 .../dynamicauto/deploy/choreo/W1 To C3.1.traj |   526 +
 .../dynamicauto/deploy/choreo/W1 To C3.traj   |   526 +
 .../dynamicauto/deploy/choreo/W1 To C4.1.traj |   580 +
 .../dynamicauto/deploy/choreo/W1 To C4.traj   |   580 +
 .../dynamicauto/deploy/choreo/W1 To C5.1.traj |   616 +
 .../dynamicauto/deploy/choreo/W1 To C5.traj   |   616 +
 .../dynamicauto/deploy/choreo/W1 To S3.1.traj |   544 +
 .../dynamicauto/deploy/choreo/W1 To S3.traj   |   544 +
 .../dynamicauto/deploy/choreo/W1 To S4.1.traj |   562 +
 .../dynamicauto/deploy/choreo/W1 To S4.traj   |   562 +
 .../dynamicauto/deploy/choreo/W1 To W2.1.traj |   103 +
 .../dynamicauto/deploy/choreo/W1 To W2.traj   |   103 +
 .../dynamicauto/deploy/choreo/W2 To C2.1.traj |   526 +
 .../dynamicauto/deploy/choreo/W2 To C2.traj   |   526 +
 .../dynamicauto/deploy/choreo/W2 To C3.1.traj |   571 +
 .../dynamicauto/deploy/choreo/W2 To C3.traj   |   571 +
 .../dynamicauto/deploy/choreo/W2 To C4.1.traj |   589 +
 .../dynamicauto/deploy/choreo/W2 To C4.traj   |   589 +
 .../dynamicauto/deploy/choreo/W2 To C5.1.traj |   625 +
 .../dynamicauto/deploy/choreo/W2 To C5.traj   |   625 +
 .../dynamicauto/deploy/choreo/W2 To S1.1.traj |   148 +
 .../dynamicauto/deploy/choreo/W2 To S1.traj   |   148 +
 .../dynamicauto/deploy/choreo/W2 To S2.1.traj |   132 +-
 .../dynamicauto/deploy/choreo/W2 To S2.traj   |   132 +-
 .../dynamicauto/deploy/choreo/W2 To S3.1.traj |   472 +
 .../dynamicauto/deploy/choreo/W2 To S3.traj   |   472 +
 .../dynamicauto/deploy/choreo/W2 To S4.1.traj |   499 +
 .../dynamicauto/deploy/choreo/W2 To S4.traj   |   499 +
 .../dynamicauto/deploy/choreo/W3 To C1.1.traj |   517 +
 .../dynamicauto/deploy/choreo/W3 To C1.traj   |   517 +
 .../dynamicauto/deploy/choreo/W3 To C2.1.traj |   517 +
 .../dynamicauto/deploy/choreo/W3 To C2.traj   |   517 +
 .../dynamicauto/deploy/choreo/W3 To C3.1.traj |   868 +
 .../dynamicauto/deploy/choreo/W3 To C3.traj   |   868 +
 .../dynamicauto/deploy/choreo/W3 To C4.1.traj |   589 +
 .../dynamicauto/deploy/choreo/W3 To C4.traj   |   589 +
 .../dynamicauto/deploy/choreo/W3 To C5.1.traj |   535 +
 .../dynamicauto/deploy/choreo/W3 To C5.traj   |   535 +
 63 files changed, 45482 insertions(+), 1905 deletions(-)
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.traj

diff --git a/src/main/java/frc/robot/utils/dynamicauto/data.chor b/src/main/java/frc/robot/utils/dynamicauto/data.chor
index 480e8e58..ff6fdd81 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/data.chor
+++ b/src/main/java/frc/robot/utils/dynamicauto/data.chor
@@ -209,7 +209,152 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 0.71,
+                    "y": 6.72,
+                    "heading": -2.1,
+                    "angularVelocity": -1.5217589356437866e-30,
+                    "velocityX": -1.7239506448843928e-31,
+                    "velocityY": 1.9880283347693254e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 0.737435952483546,
+                    "y": 6.69951466757637,
+                    "heading": -2.132336617905336,
+                    "angularVelocity": -0.4965864735710579,
+                    "velocityX": 0.4213280106395778,
+                    "velocityY": -0.3145888360360173,
+                    "timestamp": 0.06511779846276568
+                },
+                {
+                    "x": 0.7924253707255817,
+                    "y": 6.658399914346492,
+                    "heading": -2.1942155291625705,
+                    "angularVelocity": -0.9502611070707117,
+                    "velocityX": 0.8444606473217692,
+                    "velocityY": -0.6313904063170618,
+                    "timestamp": 0.13023559692553136
+                },
+                {
+                    "x": 0.8751335704497958,
+                    "y": 6.596485085823342,
+                    "heading": -2.281554213555253,
+                    "angularVelocity": -1.3412413572707518,
+                    "velocityX": 1.2701320019519164,
+                    "velocityY": -0.950812680784239,
+                    "timestamp": 0.19535339538829705
+                },
+                {
+                    "x": 0.9857847196454437,
+                    "y": 6.513575479492505,
+                    "heading": -2.388116786100485,
+                    "angularVelocity": -1.6364584654403693,
+                    "velocityX": 1.6992458560913724,
+                    "velocityY": -1.2732249598125673,
+                    "timestamp": 0.2604711938510627
+                },
+                {
+                    "x": 1.1246179227996527,
+                    "y": 6.409453213658469,
+                    "heading": -2.503336368742031,
+                    "angularVelocity": -1.7694023041554334,
+                    "velocityX": 2.132031586319575,
+                    "velocityY": -1.5989832010916685,
+                    "timestamp": 0.3255889923138284
+                },
+                {
+                    "x": 1.2913924480076884,
+                    "y": 6.28415287204147,
+                    "heading": -2.6022121669144127,
+                    "angularVelocity": -1.518414327672898,
+                    "velocityX": 2.5611204485575056,
+                    "velocityY": -1.9242103476309298,
+                    "timestamp": 0.39070679077659404
+                },
+                {
+                    "x": 1.4799082257130864,
+                    "y": 6.139182928400833,
+                    "heading": -2.6246781987943484,
+                    "angularVelocity": -0.3450060108033547,
+                    "velocityX": 2.8949961785515774,
+                    "velocityY": -2.2262721876804705,
+                    "timestamp": 0.4558245892393597
+                },
+                {
+                    "x": 1.6682193277242308,
+                    "y": 5.99414498496963,
+                    "heading": -2.64727359752809,
+                    "angularVelocity": -0.34699266970246656,
+                    "velocityX": 2.8918530180165907,
+                    "velocityY": -2.2273164458122023,
+                    "timestamp": 0.5209423877021254
+                },
+                {
+                    "x": 1.8350272177323106,
+                    "y": 5.869345925023006,
+                    "heading": -2.74292245957107,
+                    "angularVelocity": -1.4688589648446377,
+                    "velocityX": 2.5616328246026367,
+                    "velocityY": -1.9165122730305941,
+                    "timestamp": 0.5860601861648911
+                },
+                {
+                    "x": 1.9739579805969827,
+                    "y": 5.765588033107393,
+                    "heading": -2.8581454024211346,
+                    "angularVelocity": -1.7694539061536165,
+                    "velocityX": 2.133529789771879,
+                    "velocityY": -1.5933875893384517,
+                    "timestamp": 0.6511779846276567
+                },
+                {
+                    "x": 2.084683421379683,
+                    "y": 5.683004349697254,
+                    "heading": -2.964498621197243,
+                    "angularVelocity": -1.6332434647175749,
+                    "velocityX": 1.7003867359860632,
+                    "velocityY": -1.2682198317463163,
+                    "timestamp": 0.7162957830904224
+                },
+                {
+                    "x": 2.1674690836499857,
+                    "y": 5.621349763098201,
+                    "heading": -3.050914836384586,
+                    "angularVelocity": -1.327075196449642,
+                    "velocityX": 1.2713215775812727,
+                    "velocityY": -0.9468162016304505,
+                    "timestamp": 0.7814135815531881
+                },
+                {
+                    "x": 2.22252551206485,
+                    "y": 5.580407151857498,
+                    "heading": -3.1116606802742157,
+                    "angularVelocity": -0.9328608356494681,
+                    "velocityX": 0.8454897081071593,
+                    "velocityY": -0.6287468588808828,
+                    "timestamp": 0.8465313800159537
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -0.48411690274504937,
+                    "velocityX": 0.42191979126659057,
+                    "velocityY": -0.3133882339275879,
+                    "timestamp": 0.9116491784787194
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -1.0063297310782738e-31,
+                    "velocityX": -3.625667697405941e-32,
+                    "velocityY": 0,
+                    "timestamp": 0.9767669769414851
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -1157,7 +1302,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 30
+                    "controlIntervalCount": 29
+                },
+                {
+                    "x": 4.900439739227295,
+                    "y": 4.161101341247559,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 28
                 },
                 {
                     "x": 7.68,
@@ -1169,7 +1323,530 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 0.71,
+                    "y": 6.72,
+                    "heading": -2.1,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 2.27986083101055e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 0.7292945467072102,
+                    "y": 6.711630943578302,
+                    "heading": -2.0466837783195437,
+                    "angularVelocity": 0.9750423416679173,
+                    "velocityX": 0.3528569619125016,
+                    "velocityY": -0.1530525628742106,
+                    "timestamp": 0.05468092963968429
+                },
+                {
+                    "x": 0.7686895561493499,
+                    "y": 6.695016697545084,
+                    "heading": -1.9453579324448191,
+                    "angularVelocity": 1.853038098335252,
+                    "velocityX": 0.720452444787058,
+                    "velocityY": -0.30383986049058775,
+                    "timestamp": 0.10936185927936858
+                },
+                {
+                    "x": 0.8291498926090706,
+                    "y": 6.670065411203325,
+                    "heading": -1.8043277140562832,
+                    "angularVelocity": 2.579148147587168,
+                    "velocityX": 1.1056932802371746,
+                    "velocityY": -0.4563069155219838,
+                    "timestamp": 0.16404278891905286
+                },
+                {
+                    "x": 0.9116442238767206,
+                    "y": 6.636292399159329,
+                    "heading": -1.6353628554936872,
+                    "angularVelocity": 3.09001437385898,
+                    "velocityX": 1.50864902647486,
+                    "velocityY": -0.6176378541210026,
+                    "timestamp": 0.21872371855873715
+                },
+                {
+                    "x": 1.0169609100629697,
+                    "y": 6.593200359597327,
+                    "heading": -1.4531033706452814,
+                    "angularVelocity": 3.333145322316033,
+                    "velocityX": 1.926022232617937,
+                    "velocityY": -0.7880634042974968,
+                    "timestamp": 0.2734046481984214
+                },
+                {
+                    "x": 1.1451022790503944,
+                    "y": 6.540887351226898,
+                    "heading": -1.2848862020086818,
+                    "angularVelocity": 3.0763406866901017,
+                    "velocityX": 2.3434380108714725,
+                    "velocityY": -0.9566956654750047,
+                    "timestamp": 0.3280855778381057
+                },
+                {
+                    "x": 1.291419394840941,
+                    "y": 6.479714636695398,
+                    "heading": -1.166754837939456,
+                    "angularVelocity": 2.1603759271768683,
+                    "velocityX": 2.6758344591924805,
+                    "velocityY": -1.1187211873425027,
+                    "timestamp": 0.38276650747779
+                },
+                {
+                    "x": 1.45556402161766,
+                    "y": 6.408096144298377,
+                    "heading": -1.0985152891132095,
+                    "angularVelocity": 1.24795882725304,
+                    "velocityX": 3.0018624017246447,
+                    "velocityY": -1.3097526481159927,
+                    "timestamp": 0.4374474371174743
+                },
+                {
+                    "x": 1.6376170855458871,
+                    "y": 6.326295801964295,
+                    "heading": -1.0805867815133028,
+                    "angularVelocity": 0.3278749596622653,
+                    "velocityX": 3.32937031480356,
+                    "velocityY": -1.495957418301032,
+                    "timestamp": 0.4921283667571586
+                },
+                {
+                    "x": 1.828511449146643,
+                    "y": 6.246620230558847,
+                    "heading": -1.0805866805891169,
+                    "angularVelocity": 0.000001845692576742716,
+                    "velocityX": 3.491059220438268,
+                    "velocityY": -1.4570997956776681,
+                    "timestamp": 0.5468092963968428
+                },
+                {
+                    "x": 2.0194058587075467,
+                    "y": 6.16694476926885,
+                    "heading": -1.0805865796652752,
+                    "angularVelocity": 0.0000018456862813728727,
+                    "velocityX": 3.491060060953374,
+                    "velocityY": -1.4570977818960094,
+                    "timestamp": 0.6014902260365271
+                },
+                {
+                    "x": 2.2103002634541666,
+                    "y": 6.087269296444259,
+                    "heading": -1.0805864787414845,
+                    "angularVelocity": 0.000001845685351855892,
+                    "velocityX": 3.491059972910188,
+                    "velocityY": -1.4570979928396652,
+                    "timestamp": 0.6561711556762114
+                },
+                {
+                    "x": 2.4011946628107936,
+                    "y": 6.007593810705732,
+                    "heading": -1.0805863778177442,
+                    "angularVelocity": 0.000001845684424329729,
+                    "velocityX": 3.4910598743384673,
+                    "velocityY": -1.457098229008597,
+                    "timestamp": 0.7108520853158957
+                },
+                {
+                    "x": 2.5920890560965946,
+                    "y": 5.927918310422065,
+                    "heading": -1.0805862768940542,
+                    "angularVelocity": 0.0000018456835100189827,
+                    "velocityX": 3.4910597633157407,
+                    "velocityY": -1.45709849500883,
+                    "timestamp": 0.76553301495558
+                },
+                {
+                    "x": 2.7829834425031943,
+                    "y": 5.8482427936564765,
+                    "heading": -1.0805861759704127,
+                    "angularVelocity": 0.0000018456826171919115,
+                    "velocityX": 3.4910596375095175,
+                    "velocityY": -1.4570987964287467,
+                    "timestamp": 0.8202139445952643
+                },
+                {
+                    "x": 2.9738778210651113,
+                    "y": 5.768567258095778,
+                    "heading": -1.080586075046818,
+                    "angularVelocity": 0.0000018456817639254375,
+                    "velocityX": 3.4910594940466586,
+                    "velocityY": -1.4570991401520532,
+                    "timestamp": 0.8748948742349486
+                },
+                {
+                    "x": 3.164772190605788,
+                    "y": 5.688891700921064,
+                    "heading": -1.0805859741232662,
+                    "angularVelocity": 0.0000018456809789441398,
+                    "velocityX": 3.491059329067008,
+                    "velocityY": -1.4570995354272234,
+                    "timestamp": 0.9295758038746329
+                },
+                {
+                    "x": 3.3556665493271205,
+                    "y": 5.609216117824277,
+                    "heading": -1.0805858731997562,
+                    "angularVelocity": 0.000001845680216876217,
+                    "velocityX": 3.491059131203806,
+                    "velocityY": -1.4571000094878162,
+                    "timestamp": 0.9842567335143172
+                },
+                {
+                    "x": 3.5465606286609406,
+                    "y": 5.529539865347568,
+                    "heading": -1.0805857722761907,
+                    "angularVelocity": 0.0000018456812299162406,
+                    "velocityX": 3.491054021789706,
+                    "velocityY": -1.4571122510486052,
+                    "timestamp": 1.0389376631540015
+                },
+                {
+                    "x": 3.7302882443981766,
+                    "y": 5.43449767788976,
+                    "heading": -1.0805856670205962,
+                    "angularVelocity": 0.0000019249049955796687,
+                    "velocityX": 3.3599943700279993,
+                    "velocityY": -1.7381231095389325,
+                    "timestamp": 1.0936185927936857
+                },
+                {
+                    "x": 3.901247591247985,
+                    "y": 5.318042735383766,
+                    "heading": -1.0805855451412456,
+                    "angularVelocity": 0.000002228918769474178,
+                    "velocityX": 3.1264893990707807,
+                    "velocityY": -2.12971767805275,
+                    "timestamp": 1.1482995224333699
+                },
+                {
+                    "x": 4.056955156553094,
+                    "y": 5.181866377235877,
+                    "heading": -1.0805853830111598,
+                    "angularVelocity": 0.0000029650206572180867,
+                    "velocityX": 2.8475661685185734,
+                    "velocityY": -2.490381181249336,
+                    "timestamp": 1.202980452073054
+                },
+                {
+                    "x": 4.195149083685748,
+                    "y": 5.0279466523676755,
+                    "heading": -1.080585195813012,
+                    "angularVelocity": 0.0000034234631562085443,
+                    "velocityX": 2.527278304214503,
+                    "velocityY": -2.81487030089728,
+                    "timestamp": 1.2576613817127382
+                },
+                {
+                    "x": 4.313735776578777,
+                    "y": 4.858628961688981,
+                    "heading": -1.081017907841774,
+                    "angularVelocity": -0.007913399271258946,
+                    "velocityX": 2.168702940393416,
+                    "velocityY": -3.0964669363597066,
+                    "timestamp": 1.3123423113524224
+                },
+                {
+                    "x": 4.409971486167556,
+                    "y": 4.678531583119963,
+                    "heading": -1.0900704798968306,
+                    "angularVelocity": -0.16555263626108413,
+                    "velocityX": 1.759950136600006,
+                    "velocityY": -3.293604914103617,
+                    "timestamp": 1.3670232409921066
+                },
+                {
+                    "x": 4.510748059482075,
+                    "y": 4.519168046099015,
+                    "heading": -1.1448684085004224,
+                    "angularVelocity": -1.0021396666932787,
+                    "velocityX": 1.8429930503848142,
+                    "velocityY": -2.9144262555714096,
+                    "timestamp": 1.4217041706317908
+                },
+                {
+                    "x": 4.62613024170022,
+                    "y": 4.379607058549792,
+                    "heading": -1.2139690573229496,
+                    "angularVelocity": -1.2637065477463625,
+                    "velocityX": 2.110099132886858,
+                    "velocityY": -2.552278984078161,
+                    "timestamp": 1.476385100271475
+                },
+                {
+                    "x": 4.756158352198169,
+                    "y": 4.26013085900244,
+                    "heading": -1.289886392340967,
+                    "angularVelocity": -1.3883695013648907,
+                    "velocityX": 2.3779425725707126,
+                    "velocityY": -2.1849701593340063,
+                    "timestamp": 1.5310660299111591
+                },
+                {
+                    "x": 4.900439739227295,
+                    "y": 4.161101341247559,
+                    "heading": -1.3678659526433012,
+                    "angularVelocity": -1.4260832947825601,
+                    "velocityX": 2.638605231839619,
+                    "velocityY": -1.8110430529880912,
+                    "timestamp": 1.5857469595508433
+                },
+                {
+                    "x": 5.00826704552834,
+                    "y": 4.101917336684186,
+                    "heading": -1.4217535551168057,
+                    "angularVelocity": -1.4073230082403518,
+                    "velocityX": 2.816006690010969,
+                    "velocityY": -1.5456432930522144,
+                    "timestamp": 1.6240378154225759
+                },
+                {
+                    "x": 5.122657163782486,
+                    "y": 4.053031768762854,
+                    "heading": -1.4735948904430307,
+                    "angularVelocity": -1.353882908752006,
+                    "velocityX": 2.9874004027836745,
+                    "velocityY": -1.2766903953541875,
+                    "timestamp": 1.6623286712943084
+                },
+                {
+                    "x": 5.24335031560154,
+                    "y": 4.014573699061021,
+                    "heading": -1.5218759194168912,
+                    "angularVelocity": -1.2609023192271587,
+                    "velocityX": 3.152009770253094,
+                    "velocityY": -1.0043669389543197,
+                    "timestamp": 1.700619527166041
+                },
+                {
+                    "x": 5.370053209601022,
+                    "y": 3.986652038058385,
+                    "heading": -1.5647151394478649,
+                    "angularVelocity": -1.1187846042009912,
+                    "velocityX": 3.308959570502,
+                    "velocityY": -0.729199187820931,
+                    "timestamp": 1.7389103830377735
+                },
+                {
+                    "x": 5.5024723935607405,
+                    "y": 3.969300942477628,
+                    "heading": -1.599514624372743,
+                    "angularVelocity": -0.9088197203387187,
+                    "velocityX": 3.458245603161718,
+                    "velocityY": -0.4531394032789495,
+                    "timestamp": 1.777201238909506
+                },
+                {
+                    "x": 5.64052782398134,
+                    "y": 3.9622378493580968,
+                    "heading": -1.6219128460749601,
+                    "angularVelocity": -0.5849496228876974,
+                    "velocityX": 3.6054412281370607,
+                    "velocityY": -0.1844590035592553,
+                    "timestamp": 1.8154920947812385
+                },
+                {
+                    "x": 5.78519636080909,
+                    "y": 3.9633730625174985,
+                    "heading": -1.6224984270976561,
+                    "angularVelocity": -0.015292972940008887,
+                    "velocityX": 3.778148425628394,
+                    "velocityY": 0.02964710852127253,
+                    "timestamp": 1.853782950652971
+                },
+                {
+                    "x": 5.929431807514691,
+                    "y": 3.9767231173381155,
+                    "heading": -1.6224986694398071,
+                    "angularVelocity": -0.000006328982347971965,
+                    "velocityX": 3.7668378891493703,
+                    "velocityY": 0.3486486399086302,
+                    "timestamp": 1.8920738065247036
+                },
+                {
+                    "x": 6.073330473121851,
+                    "y": 3.9933141696465464,
+                    "heading": -1.6224988987872904,
+                    "angularVelocity": -0.000005989614964631434,
+                    "velocityX": 3.758042549093057,
+                    "velocityY": 0.4332901924158613,
+                    "timestamp": 1.9303646623964361
+                },
+                {
+                    "x": 6.217229120638352,
+                    "y": 4.009905378860974,
+                    "heading": -1.6224991281340264,
+                    "angularVelocity": -0.000005989595448400927,
+                    "velocityX": 3.7580420766392404,
+                    "velocityY": 0.4332942901565158,
+                    "timestamp": 1.9686555182681686
+                },
+                {
+                    "x": 6.361127770496839,
+                    "y": 4.0264965677626225,
+                    "heading": -1.6224993574808346,
+                    "angularVelocity": -0.000005989597330541617,
+                    "velocityX": 3.7580421378023496,
+                    "velocityY": 0.4332937596700765,
+                    "timestamp": 2.0069463741399014
+                },
+                {
+                    "x": 6.505026421139362,
+                    "y": 4.0430877498636315,
+                    "heading": -1.622499586827807,
+                    "angularVelocity": -0.000005989601616416828,
+                    "velocityX": 3.7580421582781125,
+                    "velocityY": 0.43329358206529056,
+                    "timestamp": 2.045237230011634
+                },
+                {
+                    "x": 6.648925182384827,
+                    "y": 4.0596779726347245,
+                    "heading": -1.6224998161827902,
+                    "angularVelocity": -0.000005989810829077631,
+                    "velocityX": 3.7580450467730504,
+                    "velocityY": 0.43326852830525053,
+                    "timestamp": 2.0835280858833665
+                },
+                {
+                    "x": 6.790105794643628,
+                    "y": 4.068502844708353,
+                    "heading": -1.6329532702813698,
+                    "angularVelocity": -0.27300131743194306,
+                    "velocityX": 3.6870581512131766,
+                    "velocityY": 0.2304694390532754,
+                    "timestamp": 2.121818941755099
+                },
+                {
+                    "x": 6.921555592389516,
+                    "y": 4.075747346171521,
+                    "heading": -1.6719954127909697,
+                    "angularVelocity": -1.0196205235104698,
+                    "velocityX": 3.4329292138629897,
+                    "velocityY": 0.18919664494924698,
+                    "timestamp": 2.1601097976268315
+                },
+                {
+                    "x": 7.042894308610419,
+                    "y": 4.08202615388359,
+                    "heading": -1.737655401763884,
+                    "angularVelocity": -1.714769426749386,
+                    "velocityX": 3.168869262869602,
+                    "velocityY": 0.16397668762232157,
+                    "timestamp": 2.198400653498564
+                },
+                {
+                    "x": 7.153820883415079,
+                    "y": 4.087877633041295,
+                    "heading": -1.8279052524624368,
+                    "angularVelocity": -2.3569556920031554,
+                    "velocityX": 2.8969468631425612,
+                    "velocityY": 0.15281661964688553,
+                    "timestamp": 2.2366915093702966
+                },
+                {
+                    "x": 7.254173862078464,
+                    "y": 4.093777241980607,
+                    "heading": -1.9412919882105817,
+                    "angularVelocity": -2.96119616986284,
+                    "velocityX": 2.6208079286488037,
+                    "velocityY": 0.15407357200568295,
+                    "timestamp": 2.274982365242029
+                },
+                {
+                    "x": 7.343968095562701,
+                    "y": 4.100172778216793,
+                    "heading": -2.0772958326900723,
+                    "angularVelocity": -3.5518622235835817,
+                    "velocityX": 2.3450568403336534,
+                    "velocityY": 0.16702515758877745,
+                    "timestamp": 2.3132732211137617
+                },
+                {
+                    "x": 7.423449152791422,
+                    "y": 4.107548146969254,
+                    "heading": -2.2364797071963527,
+                    "angularVelocity": -4.15722947116975,
+                    "velocityX": 2.0757190044267455,
+                    "velocityY": 0.19261436143311772,
+                    "timestamp": 2.351564076985494
+                },
+                {
+                    "x": 7.492711517186474,
+                    "y": 4.113210958649556,
+                    "heading": -2.4189397780167137,
+                    "angularVelocity": -4.765108187489175,
+                    "velocityX": 1.8088486877145706,
+                    "velocityY": 0.147889399476235,
+                    "timestamp": 2.3898549328572267
+                },
+                {
+                    "x": 7.549783547522053,
+                    "y": 4.117992264935679,
+                    "heading": -2.6004162007304275,
+                    "angularVelocity": -4.739419336084478,
+                    "velocityX": 1.490487194299341,
+                    "velocityY": 0.12486809650168224,
+                    "timestamp": 2.4281457887289593
+                },
+                {
+                    "x": 7.595268726062492,
+                    "y": 4.121696461876066,
+                    "heading": -2.768130415473744,
+                    "angularVelocity": -4.380006947484507,
+                    "velocityX": 1.1878861807843983,
+                    "velocityY": 0.09673842112058428,
+                    "timestamp": 2.466436644600692
+                },
+                {
+                    "x": 7.630218683798498,
+                    "y": 4.124204213311165,
+                    "heading": -2.9120023323717077,
+                    "angularVelocity": -3.7573439825922996,
+                    "velocityX": 0.9127494525868564,
+                    "velocityY": 0.06549217503781085,
+                    "timestamp": 2.5047275004724243
+                },
+                {
+                    "x": 7.655568399114973,
+                    "y": 4.125734158703857,
+                    "heading": -3.0250833006842397,
+                    "angularVelocity": -2.9532107793916214,
+                    "velocityX": 0.6620305224148624,
+                    "velocityY": 0.03995589437375436,
+                    "timestamp": 2.543018356344157
+                },
+                {
+                    "x": 7.671992151604379,
+                    "y": 4.126594890877869,
+                    "heading": -3.103103884407084,
+                    "angularVelocity": -2.037577430606375,
+                    "velocityX": 0.42892100778377035,
+                    "velocityY": 0.022478791722372646,
+                    "timestamp": 2.5813092122158894
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -1.0467622585080842,
+                    "velocityX": 0.20913213385579937,
+                    "velocityY": 0.010579787599633836,
+                    "timestamp": 2.619600068087622
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": 7.101458819208695e-33,
+                    "velocityX": -7.030931331746161e-34,
+                    "velocityY": 2.5921613146305674e-34,
+                    "timestamp": 2.6578909239593544
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -1214,50 +1891,246 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 10
+                },
+                {
+                    "x": 5.018874645233154,
+                    "y": 4.29060697555542,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 10
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
+                    "heading": -3.143185307179586,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 10
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 0.71,
+                    "y": 6.72,
+                    "heading": -2.1,
+                    "angularVelocity": 0,
+                    "velocityX": -3.0738838344554265e-34,
+                    "velocityY": -1.6532575942515022e-34,
+                    "timestamp": 0
+                },
+                {
+                    "x": 0.8675349003884936,
+                    "y": 6.638058463961624,
+                    "heading": -2.192308349350307,
+                    "angularVelocity": -0.6292858581626611,
+                    "velocityX": 1.0739492763036018,
+                    "velocityY": -0.5586130635852894,
+                    "timestamp": 0.14668746826731788
+                },
+                {
+                    "x": 1.183655074306953,
+                    "y": 6.473510793186018,
+                    "heading": -2.3371750269963787,
+                    "angularVelocity": -0.9875872789764978,
+                    "velocityX": 2.155059172078514,
+                    "velocityY": -1.12175683934867,
+                    "timestamp": 0.29337493653463576
+                },
+                {
+                    "x": 1.6571019357484182,
+                    "y": 6.226344402054212,
+                    "heading": -2.393383986431118,
+                    "angularVelocity": -0.3831885579503364,
+                    "velocityX": 3.2275890165250725,
+                    "velocityY": -1.6849864139817283,
+                    "timestamp": 0.44006240480195363
+                },
+                {
+                    "x": 2.149819339361714,
+                    "y": 5.971090303282803,
+                    "heading": -2.3933840219605123,
+                    "angularVelocity": -2.422115180983058e-7,
+                    "velocityX": 3.3589604444967707,
+                    "velocityY": -1.7401220553227135,
+                    "timestamp": 0.5867498730692715
+                },
+                {
+                    "x": 2.6425367239446045,
+                    "y": 5.71583616777695,
+                    "heading": -2.3933840574898846,
+                    "angularVelocity": -2.422113667505868e-7,
+                    "velocityX": 3.358960314762406,
+                    "velocityY": -1.740122305749301,
+                    "timestamp": 0.7334373413365893
+                },
+                {
+                    "x": 3.135254076780422,
+                    "y": 5.4605819709896695,
+                    "heading": -2.3933840930192347,
+                    "angularVelocity": -2.4221121497556687e-7,
+                    "velocityX": 3.358960098335784,
+                    "velocityY": -1.7401227235179682,
+                    "timestamp": 0.8801248096039072
+                },
+                {
+                    "x": 3.627971327740651,
+                    "y": 5.20532757755194,
+                    "heading": -2.393384128548559,
+                    "angularVelocity": -2.4221104201937785e-7,
+                    "velocityX": 3.358959403828,
+                    "velocityY": -1.7401240641263462,
+                    "timestamp": 1.026812277871225
+                },
+                {
+                    "x": 4.091605856430787,
+                    "y": 4.900420846350941,
+                    "heading": -2.393384184524006,
+                    "angularVelocity": -3.815966513199635e-7,
+                    "velocityX": 3.160696235108687,
+                    "velocityY": -2.07861472287018,
+                    "timestamp": 1.1734997461385428
+                },
+                {
+                    "x": 4.555240236344247,
+                    "y": 4.595513888923491,
+                    "heading": -2.393384240499462,
+                    "angularVelocity": -3.815967160070403e-7,
+                    "velocityX": 3.1606952208661028,
+                    "velocityY": -2.078616265104517,
+                    "timestamp": 1.3201872144058606
+                },
+                {
+                    "x": 5.018874645233154,
+                    "y": 4.29060697555542,
+                    "heading": -2.393384296474878,
+                    "angularVelocity": -3.8159644312835243e-7,
+                    "velocityX": 3.1606954183979536,
+                    "velocityY": -2.078615964742264,
+                    "timestamp": 1.4668746826731784
+                },
+                {
+                    "x": 5.414956983036457,
+                    "y": 4.102744630717356,
+                    "heading": -2.393384340957434,
+                    "angularVelocity": -3.838597081329049e-7,
+                    "velocityX": 3.4179702017234304,
+                    "velocityY": -1.621147512518649,
+                    "timestamp": 1.5827570088571772
+                },
+                {
+                    "x": 5.811039262323732,
+                    "y": 3.9148821625062062,
+                    "heading": -2.3933843854399766,
+                    "angularVelocity": -3.838595921618151e-7,
+                    "velocityX": 3.4179696967626736,
+                    "velocityY": -1.6211485771597347,
+                    "timestamp": 1.698639335041176
+                },
+                {
+                    "x": 6.175303264444137,
+                    "y": 3.6709862524403962,
+                    "heading": -2.39338444094527,
+                    "angularVelocity": -4.789798009551726e-7,
+                    "velocityX": 3.143395667964284,
+                    "velocityY": -2.1046860043053552,
+                    "timestamp": 1.8145216612251747
+                },
+                {
+                    "x": 6.517947229314261,
+                    "y": 3.397547803025246,
+                    "heading": -2.3933845251610077,
+                    "angularVelocity": -7.267349611095117e-7,
+                    "velocityX": 2.9568267755177056,
+                    "velocityY": -2.3596216819205247,
+                    "timestamp": 1.9304039874091734
+                },
+                {
+                    "x": 6.860591017465818,
+                    "y": 3.124109132165372,
+                    "heading": -2.3933846093768096,
+                    "angularVelocity": -7.267355129115313e-7,
+                    "velocityX": 2.9568252505347843,
+                    "velocityY": -2.3596235928652884,
+                    "timestamp": 2.046286313593172
+                },
+                {
+                    "x": 7.1795790741631205,
+                    "y": 2.8553069978284444,
+                    "heading": -2.4556792266097736,
+                    "angularVelocity": -0.5375678870482116,
+                    "velocityX": 2.7526894497337926,
+                    "velocityY": -2.3196128623628227,
+                    "timestamp": 2.1621686397771707
+                },
+                {
+                    "x": 7.431770863942688,
+                    "y": 2.653048744111035,
+                    "heading": -2.7460349787453167,
+                    "angularVelocity": -2.5056085918961837,
+                    "velocityX": 2.1762748305478063,
+                    "velocityY": -1.7453761965069832,
+                    "timestamp": 2.278050965961169
+                },
+                {
+                    "x": 7.5976853712302646,
+                    "y": 2.521507979737877,
+                    "heading": -3.002083319178936,
+                    "angularVelocity": -2.2095547169726615,
+                    "velocityX": 1.4317498858638353,
+                    "velocityY": -1.135123609482061,
+                    "timestamp": 2.3939332921451677
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -1.2176316496840747,
+                    "velocityX": 0.7103294478144603,
+                    "velocityY": -0.5566679739881255,
+                    "timestamp": 2.509815618329166
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": 2.6221634517625535e-33,
+                    "velocityX": -1.3293034908554191e-34,
+                    "velocityY": 0,
+                    "timestamp": 2.6256979445131647
                 }
             ],
-            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
                         "first"
                     ],
-                    "type": "StopPoint"
+                    "type": "StopPoint",
+                    "direction": 0
                 },
                 {
                     "scope": [
                         "last"
                     ],
-                    "type": "StopPoint"
+                    "type": "StopPoint",
+                    "direction": 0
                 }
             ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
+            "usesControlIntervalGuessing": false,
+            "defaultControlIntervalCount": 10,
             "usesDefaultFieldObstacles": true,
             "circleObstacles": [
                 {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
+                    "x": 3.2201673221588134,
+                    "y": 3.984490909576416,
                     "radius": 0.5
                 },
                 {
-                    "x": 5.61,
-                    "y": 5.44,
+                    "x": 5.587796077728272,
+                    "y": 2.6101673221588135,
                     "radius": 0.5
                 }
             ]
@@ -1271,19 +2144,704 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 26
+                },
+                {
+                    "x": 4.092995643615723,
+                    "y": 5.023272514343262,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 10
+                },
+                {
+                    "x": 5.7832255363464355,
+                    "y": 3.9654417037963867,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 9
+                },
+                {
+                    "x": 7.140008449554443,
+                    "y": 3.2525558471679688,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 27
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
+                    "heading": -3.143185307179586,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 25
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 0.71,
+                    "y": 6.72,
+                    "heading": -2.1,
+                    "angularVelocity": 0,
+                    "velocityX": 3.044182207072049e-31,
+                    "velocityY": 6.458172299259412e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 0.7256414580836799,
+                    "y": 6.712155644836975,
+                    "heading": -2.1163027262729814,
+                    "angularVelocity": -0.3503504178414695,
+                    "velocityX": 0.3361395685300024,
+                    "velocityY": -0.16857751660924442,
+                    "timestamp": 0.04653262974092379
+                },
+                {
+                    "x": 0.7569790078652245,
+                    "y": 6.696436652979446,
+                    "heading": -2.1479259826479233,
+                    "angularVelocity": -0.6795931489587892,
+                    "velocityX": 0.6734532296158987,
+                    "velocityY": -0.3378057923018942,
+                    "timestamp": 0.09306525948184757
+                },
+                {
+                    "x": 0.8040786472875676,
+                    "y": 6.672808979329537,
+                    "heading": -2.1936219074673273,
+                    "angularVelocity": -0.9820189633351775,
+                    "velocityX": 1.0121852060495677,
+                    "velocityY": -0.5077657072349027,
+                    "timestamp": 0.13959788922277136
+                },
+                {
+                    "x": 0.8670199327112733,
+                    "y": 6.641236276400096,
+                    "heading": -2.2517861808528257,
+                    "angularVelocity": -1.2499674681912987,
+                    "velocityX": 1.3526268722430308,
+                    "velocityY": -0.6785067404360078,
+                    "timestamp": 0.18613051896369515
+                },
+                {
+                    "x": 0.9458962124147239,
+                    "y": 6.601679900322021,
+                    "heading": -2.32031104809549,
+                    "angularVelocity": -1.4726196998587933,
+                    "velocityX": 1.6950746205965188,
+                    "velocityY": -0.8500782418336558,
+                    "timestamp": 0.23266314870461893
+                },
+                {
+                    "x": 1.0408105422873288,
+                    "y": 6.554096827806623,
+                    "heading": -2.396269750321632,
+                    "angularVelocity": -1.6323750161792245,
+                    "velocityX": 2.039737070547226,
+                    "velocityY": -1.022574326452373,
+                    "timestamp": 0.2791957784455427
+                },
+                {
+                    "x": 1.1518552587221624,
+                    "y": 6.498443176244561,
+                    "heading": -2.4750808863817895,
+                    "angularVelocity": -1.6936746644023601,
+                    "velocityX": 2.3863838569428775,
+                    "velocityY": -1.19601346134745,
+                    "timestamp": 0.32572840818646653
+                },
+                {
+                    "x": 1.2789760591669448,
+                    "y": 6.434740942054882,
+                    "heading": -2.5479819963693036,
+                    "angularVelocity": -1.5666664530556718,
+                    "velocityX": 2.7318636654010686,
+                    "velocityY": -1.3689798866801088,
+                    "timestamp": 0.3722610379273903
+                },
+                {
+                    "x": 1.4202169645474811,
+                    "y": 6.363052674678261,
+                    "heading": -2.59222819945715,
+                    "angularVelocity": -0.9508640137943395,
+                    "velocityX": 3.0353089040296184,
+                    "velocityY": -1.5406021060002604,
+                    "timestamp": 0.41879366766831405
+                },
+                {
+                    "x": 1.573995110361942,
+                    "y": 6.28311594811204,
+                    "heading": -2.599064012600599,
+                    "angularVelocity": -0.14690364979431952,
+                    "velocityX": 3.304737915536797,
+                    "velocityY": -1.7178639378700842,
+                    "timestamp": 0.4653262974092378
+                },
+                {
+                    "x": 1.7314466383055478,
+                    "y": 6.204403629832344,
+                    "heading": -2.599064059608806,
+                    "angularVelocity": -0.0000010102203007383576,
+                    "velocityX": 3.3836799858560402,
+                    "velocityY": -1.6915510410205123,
+                    "timestamp": 0.5118589271501616
+                },
+                {
+                    "x": 1.8889032791179137,
+                    "y": 6.1257015398581975,
+                    "heading": -2.5990641066080484,
+                    "angularVelocity": -0.0000010100276450499987,
+                    "velocityX": 3.3837898629208136,
+                    "velocityY": -1.69133123170322,
+                    "timestamp": 0.5583915568910853
+                },
+                {
+                    "x": 2.0463595988203984,
+                    "y": 6.046998807454145,
+                    "heading": -2.5990641536073187,
+                    "angularVelocity": -0.0000010100282384452147,
+                    "velocityX": 3.3837829621742297,
+                    "velocityY": -1.691345037712453,
+                    "timestamp": 0.6049241866320091
+                },
+                {
+                    "x": 2.2038159116861142,
+                    "y": 5.968296061372362,
+                    "heading": -2.599064200606589,
+                    "angularVelocity": -0.0000010100282469186477,
+                    "velocityX": 3.3837828152499863,
+                    "velocityY": -1.6913453316510296,
+                    "timestamp": 0.6514568163729328
+                },
+                {
+                    "x": 2.3612722547062472,
+                    "y": 5.889593375618833,
+                    "heading": -2.5990642476058565,
+                    "angularVelocity": -0.0000010100281851545483,
+                    "velocityX": 3.3837834632773647,
+                    "velocityY": -1.6913440351789604,
+                    "timestamp": 0.6979894461138566
+                },
+                {
+                    "x": 2.5187286297414215,
+                    "y": 5.8108907539159365,
+                    "heading": -2.599064294605121,
+                    "angularVelocity": -0.0000010100281210334782,
+                    "velocityX": 3.383784151290135,
+                    "velocityY": -1.6913426587117795,
+                    "timestamp": 0.7445220758547804
+                },
+                {
+                    "x": 2.6761850576997652,
+                    "y": 5.732188238093708,
+                    "heading": -2.599064341604381,
+                    "angularVelocity": -0.000001010028016565385,
+                    "velocityX": 3.3837852886247464,
+                    "velocityY": -1.6913403833048126,
+                    "timestamp": 0.7910547055957041
+                },
+                {
+                    "x": 2.83364151622281,
+                    "y": 5.653485783421201,
+                    "heading": -2.5990643886036375,
+                    "angularVelocity": -0.0000010100279552603614,
+                    "velocityX": 3.3837859454691737,
+                    "velocityY": -1.6913390691793195,
+                    "timestamp": 0.8375873353366279
+                },
+                {
+                    "x": 2.9910977149954676,
+                    "y": 5.574782809080547,
+                    "heading": -2.5990644356029167,
+                    "angularVelocity": -0.000001010028434200447,
+                    "velocityX": 3.3837803633561943,
+                    "velocityY": -1.691350237002125,
+                    "timestamp": 0.8841199650775516
+                },
+                {
+                    "x": 3.1485530895378893,
+                    "y": 5.496078185774728,
+                    "heading": -2.599064482602267,
+                    "angularVelocity": -0.0000010100299667197812,
+                    "velocityX": 3.3837626504042433,
+                    "velocityY": -1.6913856737520705,
+                    "timestamp": 0.9306525948184754
+                },
+                {
+                    "x": 3.3060077005580313,
+                    "y": 5.417372034978031,
+                    "heading": -2.5990645296016837,
+                    "angularVelocity": -0.0000010100313828443218,
+                    "velocityX": 3.383746242085582,
+                    "velocityY": -1.6914184999808708,
+                    "timestamp": 0.9771852245593992
+                },
+                {
+                    "x": 3.463466207634218,
+                    "y": 5.3386736786731355,
+                    "heading": -2.5990645766007607,
+                    "angularVelocity": -0.0000010100240886605546,
+                    "velocityX": 3.383829969488029,
+                    "velocityY": -1.6912509940353067,
+                    "timestamp": 1.023717854300323
+                },
+                {
+                    "x": 3.620938146457774,
+                    "y": 5.260002202075332,
+                    "heading": -2.5990646235986756,
+                    "angularVelocity": -0.0000010099991153726974,
+                    "velocityX": 3.3841186217193853,
+                    "velocityY": -1.690673341164269,
+                    "timestamp": 1.0702504840412468
+                },
+                {
+                    "x": 3.7783857783864416,
+                    "y": 5.181282090499446,
+                    "heading": -2.5990646705986817,
+                    "angularVelocity": -0.0000010100440573860445,
+                    "velocityX": 3.383596259333376,
+                    "velocityY": -1.691718521279299,
+                    "timestamp": 1.1167831137821707
+                },
+                {
+                    "x": 3.9357445242921885,
+                    "y": 5.102384448798495,
+                    "heading": -2.599064717606569,
+                    "angularVelocity": -0.0000010102134222869035,
+                    "velocityX": 3.381686072364613,
+                    "velocityY": -1.6955336962675904,
+                    "timestamp": 1.1633157435230945
+                },
+                {
+                    "x": 4.092995643615723,
+                    "y": 5.023272514343262,
+                    "heading": -2.5990647646424785,
+                    "angularVelocity": -0.000001010815630420508,
+                    "velocityX": 3.379373145230901,
+                    "velocityY": -1.700138911036495,
+                    "timestamp": 1.2098483732640184
+                },
+                {
+                    "x": 4.261229917640389,
+                    "y": 4.916068911394467,
+                    "heading": -2.599064809945939,
+                    "angularVelocity": -8.591015508014648e-7,
+                    "velocityX": 3.1902711911232386,
+                    "velocityY": -2.0329303767326627,
+                    "timestamp": 1.2625819071694577
+                },
+                {
+                    "x": 4.429297438166161,
+                    "y": 4.8086040706119935,
+                    "heading": -2.5990648552466906,
+                    "angularVelocity": -8.590501714319945e-7,
+                    "velocityX": 3.187109000265223,
+                    "velocityY": -2.0378842991085575,
+                    "timestamp": 1.315315441074897
+                },
+                {
+                    "x": 4.597219271125054,
+                    "y": 4.700911724294517,
+                    "heading": -2.599064900558894,
+                    "angularVelocity": -8.592673362873697e-7,
+                    "velocityX": 3.184346288265075,
+                    "velocityY": -2.0421985469549115,
+                    "timestamp": 1.3680489749803364
+                },
+                {
+                    "x": 4.765178923625143,
+                    "y": 4.59327837178587,
+                    "heading": -2.5990649458681174,
+                    "angularVelocity": -8.592108353148986e-7,
+                    "velocityX": 3.1850634702628065,
+                    "velocityY": -2.0410798317000203,
+                    "timestamp": 1.4207825088857757
+                },
+                {
+                    "x": 4.93313403793343,
+                    "y": 4.485637937706628,
+                    "heading": -2.599064991177699,
+                    "angularVelocity": -8.592176153657431e-7,
+                    "velocityX": 3.184977411326241,
+                    "velocityY": -2.041214121404157,
+                    "timestamp": 1.473516042791215
+                },
+                {
+                    "x": 5.101087827105617,
+                    "y": 4.377995436130629,
+                    "heading": -2.599065036487385,
+                    "angularVelocity": -8.592196078701955e-7,
+                    "velocityX": 3.184952282419374,
+                    "velocityY": -2.041253327892839,
+                    "timestamp": 1.5262495766966544
+                },
+                {
+                    "x": 5.269016802174646,
+                    "y": 4.270314227125004,
+                    "heading": -2.599065081799025,
+                    "angularVelocity": -8.592566538542498e-7,
+                    "velocityX": 3.1844817259951634,
+                    "velocityY": -2.041987347154346,
+                    "timestamp": 1.5789831106020937
+                },
+                {
+                    "x": 5.437055950944385,
+                    "y": 4.162805027649924,
+                    "heading": -2.599065127101977,
+                    "angularVelocity": -8.590919076920599e-7,
+                    "velocityX": 3.1865709791255945,
+                    "velocityY": -2.038725484769403,
+                    "timestamp": 1.631716644507533
+                },
+                {
+                    "x": 5.605213465892237,
+                    "y": 4.055481061993006,
+                    "heading": -2.599065172406386,
+                    "angularVelocity": -8.591195260284143e-7,
+                    "velocityX": 3.188815588376687,
+                    "velocityY": -2.035212846712725,
+                    "timestamp": 1.6844501784129724
+                },
+                {
+                    "x": 5.7832255363464355,
+                    "y": 3.9654417037963863,
+                    "heading": -2.599065217938776,
+                    "angularVelocity": -8.634427952488677e-7,
+                    "velocityX": 3.3756901400427615,
+                    "velocityY": -1.707440247757134,
+                    "timestamp": 1.7371837123184117
+                },
+                {
+                    "x": 5.945200300467497,
+                    "y": 3.903126168683373,
+                    "heading": -2.599065262330506,
+                    "angularVelocity": -9.676334890696444e-7,
+                    "velocityX": 3.5306622744884395,
+                    "velocityY": -1.3583295529523924,
+                    "timestamp": 1.7830603075655527
+                },
+                {
+                    "x": 6.112434090508488,
+                    "y": 3.856737486793876,
+                    "heading": -2.5990653054733555,
+                    "angularVelocity": -9.404108896808373e-7,
+                    "velocityX": 3.6452964554166534,
+                    "velocityY": -1.01116226345067,
+                    "timestamp": 1.8289369028126936
+                },
+                {
+                    "x": 6.280097327658818,
+                    "y": 3.8119257778274935,
+                    "heading": -2.599065348543951,
+                    "angularVelocity": -9.38835914358075e-7,
+                    "velocityX": 3.6546573747925137,
+                    "velocityY": -0.9767880272056341,
+                    "timestamp": 1.8748134980598345
+                },
+                {
+                    "x": 6.444605868501446,
+                    "y": 3.756644766145867,
+                    "heading": -2.599065392143275,
+                    "angularVelocity": -9.503609402316767e-7,
+                    "velocityX": 3.5858925440393525,
+                    "velocityY": -1.2049937747953314,
+                    "timestamp": 1.9206900933069755
+                },
+                {
+                    "x": 6.602719051918594,
+                    "y": 3.685095684621415,
+                    "heading": -2.5990654382603977,
+                    "angularVelocity": -0.0000010052429246959388,
+                    "velocityX": 3.446489055375297,
+                    "velocityY": -1.5595987701135843,
+                    "timestamp": 1.9665666885541164
+                },
+                {
+                    "x": 6.752801915063028,
+                    "y": 3.597951074081566,
+                    "heading": -2.599065489714602,
+                    "angularVelocity": -0.0000011215785414644313,
+                    "velocityX": 3.2714472888827597,
+                    "velocityY": -1.8995439846919482,
+                    "timestamp": 2.0124432838012574
+                },
+                {
+                    "x": 6.893316861258978,
+                    "y": 3.49609530674715,
+                    "heading": -2.599065550773928,
+                    "angularVelocity": -0.0000013309472018502562,
+                    "velocityX": 3.06288959411619,
+                    "velocityY": -2.2202120010370177,
+                    "timestamp": 2.058319879048398
+                },
+                {
+                    "x": 7.022827120260116,
+                    "y": 3.380569821579126,
+                    "heading": -2.5990656290967817,
+                    "angularVelocity": -0.0000017072507862805843,
+                    "velocityX": 2.8230137459733338,
+                    "velocityY": -2.5181791400535034,
+                    "timestamp": 2.104196474295539
+                },
+                {
+                    "x": 7.140008449554443,
+                    "y": 3.2525558471679688,
+                    "heading": -2.5990657325552275,
+                    "angularVelocity": -0.000002255146554794306,
+                    "velocityX": 2.5542725797993215,
+                    "velocityY": -2.790398322315311,
+                    "timestamp": 2.1500730695426795
+                },
+                {
+                    "x": 7.220411639481594,
+                    "y": 3.1500923106303724,
+                    "heading": -2.599065847631647,
+                    "angularVelocity": -0.000003342401256012092,
+                    "velocityX": 2.3353152959116907,
+                    "velocityY": -2.9760593375246422,
+                    "timestamp": 2.1845023348619734
+                },
+                {
+                    "x": 7.292813286750944,
+                    "y": 3.0418266490558983,
+                    "heading": -2.5990659368308053,
+                    "angularVelocity": -0.0000025907947068596104,
+                    "velocityX": 2.1029100272080186,
+                    "velocityY": -3.1445823943795888,
+                    "timestamp": 2.2189316001812673
+                },
+                {
+                    "x": 7.356796447286593,
+                    "y": 2.9283823501608603,
+                    "heading": -2.599066009494182,
+                    "angularVelocity": -0.0000021105119622632264,
+                    "velocityX": 1.8583945937350983,
+                    "velocityY": -3.294996214498541,
+                    "timestamp": 2.2533608655005612
+                },
+                {
+                    "x": 7.411992663456803,
+                    "y": 2.8104127239306367,
+                    "heading": -2.5990660711538616,
+                    "angularVelocity": -0.0000017909089651975303,
+                    "velocityX": 1.603177287064451,
+                    "velocityY": -3.426434608353826,
+                    "timestamp": 2.287790130819855
+                },
+                {
+                    "x": 7.458084080553435,
+                    "y": 2.688597137512427,
+                    "heading": -2.599066125344508,
+                    "angularVelocity": -0.0000015739704607850277,
+                    "velocityX": 1.3387278720350797,
+                    "velocityY": -3.538140744175163,
+                    "timestamp": 2.322219396139149
+                },
+                {
+                    "x": 7.494809849686971,
+                    "y": 2.5636384472618623,
+                    "heading": -2.599066174481706,
+                    "angularVelocity": -0.0000014271927566927072,
+                    "velocityX": 1.0667020859416116,
+                    "velocityY": -3.6294323765468546,
+                    "timestamp": 2.356648661458443
+                },
+                {
+                    "x": 7.5219737035667364,
+                    "y": 2.4362587715435033,
+                    "heading": -2.599066220329768,
+                    "angularVelocity": -0.0000013316596016825942,
+                    "velocityX": 0.7889757050532209,
+                    "velocityY": -3.699750039306788,
+                    "timestamp": 2.391077926777737
+                },
+                {
+                    "x": 7.539529775464085,
+                    "y": 2.307203584640024,
+                    "heading": -2.5990662642857343,
+                    "angularVelocity": -0.0000012767035893272692,
+                    "velocityX": 0.5099171223818962,
+                    "velocityY": -3.748415358464242,
+                    "timestamp": 2.425507192097031
+                },
+                {
+                    "x": 7.55111544342237,
+                    "y": 2.1774760653156817,
+                    "heading": -2.599066307607684,
+                    "angularVelocity": -0.0000012582885205698109,
+                    "velocityX": 0.3365063950923211,
+                    "velocityY": -3.7679432924653384,
+                    "timestamp": 2.4599364574163247
+                },
+                {
+                    "x": 7.56271571323303,
+                    "y": 2.047749850875278,
+                    "heading": -2.5990663509314866,
+                    "angularVelocity": -0.0000012583423688964546,
+                    "velocityX": 0.3369305067394331,
+                    "velocityY": -3.767905392035856,
+                    "timestamp": 2.4943657227356186
+                },
+                {
+                    "x": 7.5743766744470316,
+                    "y": 1.9180290778400821,
+                    "heading": -2.599066394258993,
+                    "angularVelocity": -0.0000012584499309396356,
+                    "velocityX": 0.33869329205287857,
+                    "velocityY": -3.7677473461074933,
+                    "timestamp": 2.5287949880549125
+                },
+                {
+                    "x": 7.586111158859003,
+                    "y": 1.7883149350094998,
+                    "heading": -2.5990664375910546,
+                    "angularVelocity": -0.0000012585822270319525,
+                    "velocityX": 0.34082877758442714,
+                    "velocityY": -3.7675547714315485,
+                    "timestamp": 2.5632242533742065
+                },
+                {
+                    "x": 7.597929633339714,
+                    "y": 1.6586084182080805,
+                    "heading": -2.59906648228782,
+                    "angularVelocity": -0.0000012982201369267655,
+                    "velocityX": 0.3432682739856076,
+                    "velocityY": -3.7673332729737243,
+                    "timestamp": 2.5976535186935004
+                },
+                {
+                    "x": 7.610690036465753,
+                    "y": 1.5360359058677395,
+                    "heading": -2.6173573406008535,
+                    "angularVelocity": -0.5312590362708309,
+                    "velocityX": 0.37062664589906397,
+                    "velocityY": -3.5601257013070047,
+                    "timestamp": 2.6320827840127943
+                },
+                {
+                    "x": 7.621968158889347,
+                    "y": 1.4215047209508034,
+                    "heading": -2.657213150288673,
+                    "angularVelocity": -1.1576142946476244,
+                    "velocityX": 0.3275737172727147,
+                    "velocityY": -3.3265648817881672,
+                    "timestamp": 2.666512049332088
+                },
+                {
+                    "x": 7.631829723236215,
+                    "y": 1.3151734834169941,
+                    "heading": -2.7178665355180023,
+                    "angularVelocity": -1.76168107762032,
+                    "velocityX": 0.28642970610680407,
+                    "velocityY": -3.08839693637669,
+                    "timestamp": 2.700941314651382
+                },
+                {
+                    "x": 7.640728077188491,
+                    "y": 1.2186118736781408,
+                    "heading": -2.7826314980185702,
+                    "angularVelocity": -1.8811020769678346,
+                    "velocityX": 0.2584532045561674,
+                    "velocityY": -2.8046375327311788,
+                    "timestamp": 2.735370579970676
+                },
+                {
+                    "x": 7.648685947205252,
+                    "y": 1.1318864826516366,
+                    "heading": -2.8465688471146957,
+                    "angularVelocity": -1.8570639978286094,
+                    "velocityX": 0.23113679432217057,
+                    "velocityY": -2.518943992043375,
+                    "timestamp": 2.76979984528997
+                },
+                {
+                    "x": 7.655720752960135,
+                    "y": 1.0549563243440254,
+                    "heading": -2.9071249866023474,
+                    "angularVelocity": -1.7588565694352556,
+                    "velocityX": 0.20432633951521018,
+                    "velocityY": -2.2344408919030423,
+                    "timestamp": 2.804229110609264
+                },
+                {
+                    "x": 7.661844928847109,
+                    "y": 0.9877692823605362,
+                    "heading": -2.962675956286548,
+                    "angularVelocity": -1.6134811233710036,
+                    "velocityX": 0.17787704239939275,
+                    "velocityY": -1.9514515154593546,
+                    "timestamp": 2.8386583759285577
+                },
+                {
+                    "x": 7.667068746842824,
+                    "y": 0.930277282093812,
+                    "heading": -3.0120810880108007,
+                    "angularVelocity": -1.4349749048108316,
+                    "velocityX": 0.1517260954387245,
+                    "velocityY": -1.6698584687633535,
+                    "timestamp": 2.8730876412478517
+                },
+                {
+                    "x": 7.6714018244195765,
+                    "y": 0.8824389383291659,
+                    "heading": -3.0544961336196454,
+                    "angularVelocity": -1.231947449807498,
+                    "velocityX": 0.12585448851644476,
+                    "velocityY": -1.3894674580185522,
+                    "timestamp": 2.9075169065671456
+                },
+                {
+                    "x": 7.674853405168496,
+                    "y": 0.8442193530437802,
+                    "heading": -3.0892755187362573,
+                    "angularVelocity": -1.0101692497377945,
+                    "velocityX": 0.10025136223247667,
+                    "velocityY": -1.1100900623623653,
+                    "timestamp": 2.9419461718864395
+                },
+                {
+                    "x": 7.677432109437281,
+                    "y": 0.8155891364108041,
+                    "heading": -3.1159120874216906,
+                    "angularVelocity": -0.773660676125647,
+                    "velocityX": 0.07489861444537417,
+                    "velocityY": -0.8315662959247566,
+                    "timestamp": 2.9763754372057334
+                },
+                {
+                    "x": 7.679145555938743,
+                    "y": 0.7965232635297996,
+                    "heading": -3.1339959816499734,
+                    "angularVelocity": -0.525247752473809,
+                    "velocityX": 0.04976715261198055,
+                    "velocityY": -0.5537693791659258,
+                    "timestamp": 3.0108047025250273
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -0.2669044908275344,
+                    "velocityX": 0.024817377116055982,
+                    "velocityY": -0.2766037393328496,
+                    "timestamp": 3.045233967844321
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -4.433819854347959e-31,
+                    "velocityX": 0,
+                    "velocityY": 2.2179802932100695e-31,
+                    "timestamp": 3.079663233163615
                 }
             ],
-            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -1299,25 +2857,9 @@
                 }
             ],
             "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
+            "defaultControlIntervalCount": 25,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "Center To W1": {
             "waypoints": [
@@ -1340,7 +2882,116 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 1.35,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": -7.821039013033948e-33,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 1.3816658469394465,
+                    "y": 5.599900983994057,
+                    "heading": 3.1530893621447014,
+                    "angularVelocity": 0.16724918062405764,
+                    "velocityX": 0.4046100104681557,
+                    "velocityY": 0.5098343834730725,
+                    "timestamp": 0.07826263839297289
+                },
+                {
+                    "x": 1.444997693090777,
+                    "y": 5.6797028720542775,
+                    "heading": 3.179265627435073,
+                    "angularVelocity": 0.3344669414150786,
+                    "velocityX": 0.8092219665957109,
+                    "velocityY": 1.0196677456683545,
+                    "timestamp": 0.15652527678594577
+                },
+                {
+                    "x": 1.539995766815641,
+                    "y": 5.799405527661274,
+                    "heading": 3.2185261574836668,
+                    "angularVelocity": 0.5016509902395458,
+                    "velocityX": 1.213836840611724,
+                    "velocityY": 1.5294993634886844,
+                    "timestamp": 0.23478791517891867
+                },
+                {
+                    "x": 1.6666603529158708,
+                    "y": 5.959008729778703,
+                    "heading": 3.270870260258294,
+                    "angularVelocity": 0.6688261966303347,
+                    "velocityX": 1.6184553536800128,
+                    "velocityY": 2.039328157019548,
+                    "timestamp": 0.31305055357189154
+                },
+                {
+                    "x": 1.8249917488860277,
+                    "y": 6.158512141354908,
+                    "heading": 3.336302010504542,
+                    "angularVelocity": 0.8360534680379705,
+                    "velocityX": 2.0230776679817777,
+                    "velocityY": 2.5491526438766083,
+                    "timestamp": 0.3913131919648644
+                },
+                {
+                    "x": 1.9833277736824528,
+                    "y": 6.358008431463026,
+                    "heading": 3.401912848694198,
+                    "angularVelocity": 0.8383417622622024,
+                    "velocityX": 2.023136812758436,
+                    "velocityY": 2.549061649396556,
+                    "timestamp": 0.4695758303578373
+                },
+                {
+                    "x": 2.1099966456081427,
+                    "y": 6.517605183327805,
+                    "heading": 3.454415313467435,
+                    "angularVelocity": 0.6708496653232114,
+                    "velocityX": 1.618510115767619,
+                    "velocityY": 2.0392457389873657,
+                    "timestamp": 0.5478384687508102
+                },
+                {
+                    "x": 2.20499831408803,
+                    "y": 6.637302614330095,
+                    "heading": 3.493799088289958,
+                    "angularVelocity": 0.5032257489808813,
+                    "velocityX": 1.2138827725544437,
+                    "velocityY": 1.5294326061595858,
+                    "timestamp": 0.6261011071437831
+                },
+                {
+                    "x": 2.268332767215697,
+                    "y": 6.717100865980731,
+                    "heading": 3.520056877037348,
+                    "angularVelocity": 0.3355086064891452,
+                    "velocityX": 0.8092552772071353,
+                    "velocityY": 1.0196212814849352,
+                    "timestamp": 0.704363745536756
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 3.5331853071795862,
+                    "angularVelocity": 0.16774837153225197,
+                    "velocityX": 0.4046277180855394,
+                    "velocityY": 0.5098107454405937,
+                    "timestamp": 0.7826263839297288
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 3.5331853071795862,
+                    "angularVelocity": 3.497795149569243e-31,
+                    "velocityX": 5.486835585676403e-32,
+                    "velocityY": -3.2429311821904986e-32,
+                    "timestamp": 0.8608890223227017
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -1358,23 +3009,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "Center To W2": {
             "waypoints": [
@@ -1967,7 +3602,215 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 1.35,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": -2.32021994810819e-32,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 1.430775984981732,
+                    "y": 5.581186654302167,
+                    "heading": 3.138045514038262,
+                    "angularVelocity": -0.019529788750833752,
+                    "velocityX": 0.8071369934195644,
+                    "velocityY": 0.2117031746247163,
+                    "timestamp": 0.10007716860989323
+                },
+                {
+                    "x": 1.592327952146185,
+                    "y": 5.62355996139984,
+                    "heading": 3.134136547157813,
+                    "angularVelocity": -0.0390595271102885,
+                    "velocityX": 1.6142739588706034,
+                    "velocityY": 0.42340633419443324,
+                    "timestamp": 0.20015433721978645
+                },
+                {
+                    "x": 1.8346558957131356,
+                    "y": 5.687119918958483,
+                    "heading": 3.1282731070819607,
+                    "angularVelocity": -0.05858918829621809,
+                    "velocityX": 2.4214108665639773,
+                    "velocityY": 0.6351094704367861,
+                    "timestamp": 0.30023150582967967
+                },
+                {
+                    "x": 2.1577597979432506,
+                    "y": 5.771866521439831,
+                    "heading": 3.1204552085550783,
+                    "angularVelocity": -0.078118702152105,
+                    "velocityX": 3.2285475970008037,
+                    "velocityY": 0.8468125513392032,
+                    "timestamp": 0.4003086744395729
+                },
+                {
+                    "x": 2.523958853996556,
+                    "y": 5.867916494702879,
+                    "heading": 3.120455207736121,
+                    "angularVelocity": -8.183261644434384e-9,
+                    "velocityX": 3.659166832354859,
+                    "velocityY": 0.9597590998747719,
+                    "timestamp": 0.5003858430494661
+                },
+                {
+                    "x": 2.890157912923332,
+                    "y": 5.963966457010659,
+                    "heading": 3.1204552069171707,
+                    "angularVelocity": -8.18318468791684e-9,
+                    "velocityX": 3.6591668610673986,
+                    "velocityY": 0.9597589904065732,
+                    "timestamp": 0.6004630116593593
+                },
+                {
+                    "x": 3.256356975588488,
+                    "y": 6.06001640506554,
+                    "heading": 3.1204552060982262,
+                    "angularVelocity": -8.183132612245063e-9,
+                    "velocityX": 3.659166898422379,
+                    "velocityY": 0.9597588479874845,
+                    "timestamp": 0.7005401802692526
+                },
+                {
+                    "x": 3.6225560432776467,
+                    "y": 6.156066333965972,
+                    "heading": 3.1204552052792867,
+                    "angularVelocity": -8.183078308583848e-9,
+                    "velocityX": 3.6591669486236644,
+                    "velocityY": 0.9597586565906886,
+                    "timestamp": 0.8006173488791458
+                },
+                {
+                    "x": 3.9887551180209555,
+                    "y": 6.252116235971829,
+                    "heading": 3.1204552044603537,
+                    "angularVelocity": -8.183012061558546e-9,
+                    "velocityX": 3.659167019110774,
+                    "velocityY": 0.9597583878523045,
+                    "timestamp": 0.9006945174890391
+                },
+                {
+                    "x": 4.354954203462548,
+                    "y": 6.348166097189504,
+                    "heading": 3.1204552036414284,
+                    "angularVelocity": -8.182937495729244e-9,
+                    "velocityX": 3.659167126011114,
+                    "velocityY": 0.9597579802850231,
+                    "timestamp": 1.0007716860989322
+                },
+                {
+                    "x": 4.721153326841163,
+                    "y": 6.444215813768638,
+                    "heading": 3.120455202822509,
+                    "angularVelocity": -8.182878073364452e-9,
+                    "velocityX": 3.659167505088806,
+                    "velocityY": 0.9597565350148991,
+                    "timestamp": 1.1008488547088253
+                },
+                {
+                    "x": 5.092057106129702,
+                    "y": 6.520095539851432,
+                    "heading": 3.1204551978641732,
+                    "angularVelocity": -4.954512725191857e-8,
+                    "velocityX": 3.7061777870070114,
+                    "velocityY": 0.7582121590446533,
+                    "timestamp": 1.2009260233187184
+                },
+                {
+                    "x": 5.470573222615853,
+                    "y": 6.51282289206487,
+                    "heading": 3.120455142790982,
+                    "angularVelocity": -5.503072477147126e-7,
+                    "velocityX": 3.7822424609316347,
+                    "velocityY": -0.07267039912792687,
+                    "timestamp": 1.3010031919286116
+                },
+                {
+                    "x": 5.849058511568644,
+                    "y": 6.504092056642741,
+                    "heading": 3.1204550877725445,
+                    "angularVelocity": -5.497601318399077e-7,
+                    "velocityX": 3.7819344233063643,
+                    "velocityY": -0.08724103153050607,
+                    "timestamp": 1.4010803605385047
+                },
+                {
+                    "x": 6.221366644651584,
+                    "y": 6.4354334883920234,
+                    "heading": 3.1204550635505535,
+                    "angularVelocity": -2.420331372889176e-7,
+                    "velocityX": 3.720210496104441,
+                    "velocityY": -0.6860562624264548,
+                    "timestamp": 1.5011575291483978
+                },
+                {
+                    "x": 6.569565662964383,
+                    "y": 6.286823971409196,
+                    "heading": 3.1204550644036275,
+                    "angularVelocity": 8.52416239462205e-9,
+                    "velocityX": 3.479305251631334,
+                    "velocityY": -1.4849492551305274,
+                    "timestamp": 1.601234697758291
+                },
+                {
+                    "x": 6.915949214018099,
+                    "y": 6.134030710794496,
+                    "heading": 3.1204550652542946,
+                    "angularVelocity": 8.500112010852639e-9,
+                    "velocityX": 3.4611645779462337,
+                    "velocityY": -1.5267544309761423,
+                    "timestamp": 1.701311866368184
+                },
+                {
+                    "x": 7.22156951411921,
+                    "y": 5.999218440367679,
+                    "heading": 3.1282730490164057,
+                    "angularVelocity": 0.07811955384714984,
+                    "velocityX": 3.0538463902035207,
+                    "velocityY": -1.347083178904912,
+                    "timestamp": 1.8013890349780772
+                },
+                {
+                    "x": 7.4507847539347845,
+                    "y": 5.898109225088713,
+                    "heading": 3.1341365308407623,
+                    "angularVelocity": 0.05858960545933885,
+                    "velocityX": 2.290384939936379,
+                    "velocityY": -1.010312508671161,
+                    "timestamp": 1.9014662035879704
+                },
+                {
+                    "x": 7.603594917403085,
+                    "y": 5.830703076393403,
+                    "heading": 3.1380455124480764,
+                    "angularVelocity": 0.03905967426538248,
+                    "velocityX": 1.526923329175751,
+                    "velocityY": -0.6735417241675008,
+                    "timestamp": 2.0015433721978635
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": 0.019529804640431036,
+                    "velocityX": 0.7634616732088615,
+                    "velocityY": -0.33677088252547477,
+                    "timestamp": 2.101620540807757
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 2.2237121345666e-32,
+                    "velocityY": 0,
+                    "timestamp": 2.20169770941765
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -1987,18 +3830,8 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": [
                 {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
+                    "x": 5.472021923065186,
+                    "y": 5.359512748718262,
                     "radius": 0.5
                 }
             ]
@@ -2280,16 +4113,1124 @@
                     "controlIntervalCount": 40
                 },
                 {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 3.14,
+                    "x": 4.018431663513184,
+                    "y": 5.088562488555908,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 6.893649101257324,
+                    "y": 3.5118300914764404,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 40
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 1.35,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 4.411137018023184e-34,
+                    "velocityY": 2.784782344800776e-32,
+                    "timestamp": 0
+                },
+                {
+                    "x": 1.3544641699489606,
+                    "y": 5.55925259826767,
+                    "heading": 3.1400000009005375,
+                    "angularVelocity": 3.865132870171629e-8,
+                    "velocityX": 0.19160342420680077,
+                    "velocityY": -0.032078691629102596,
+                    "timestamp": 0.023299009229303495
+                },
+                {
+                    "x": 1.3633925116373848,
+                    "y": 5.557757806179583,
+                    "heading": 3.1400000027300097,
+                    "angularVelocity": 7.852144487332144e-8,
+                    "velocityX": 0.383206925262499,
+                    "velocityY": -0.06415689497258387,
+                    "timestamp": 0.04659801845860699
+                },
+                {
+                    "x": 1.3767850270669844,
+                    "y": 5.555515636454451,
+                    "heading": 3.140000005520194,
+                    "angularVelocity": 1.1975551924808857e-7,
+                    "velocityX": 0.5748105122322142,
+                    "velocityY": -0.09623455242535102,
+                    "timestamp": 0.06989702768791048
+                },
+                {
+                    "x": 1.3946417184903905,
+                    "y": 5.55252610340551,
+                    "heading": 3.1400000093068963,
+                    "angularVelocity": 1.6252630014020525e-7,
+                    "velocityX": 0.7664141958854979,
+                    "velocityY": -0.12831159554996177,
+                    "timestamp": 0.09319603691721398
+                },
+                {
+                    "x": 1.4169625884614865,
+                    "y": 5.548789223260415,
+                    "heading": 3.14000001413076,
+                    "angularVelocity": 2.070415703537556e-7,
+                    "velocityX": 0.9580179891521703,
+                    "velocityY": -0.16038794217918884,
+                    "timestamp": 0.11649504614651748
+                },
+                {
+                    "x": 1.443747639900165,
+                    "y": 5.544305014572816,
+                    "heading": 3.1400000200383236,
+                    "angularVelocity": 2.5355430687602407e-7,
+                    "velocityX": 1.1496219077414547,
+                    "velocityY": -0.1924634924804626,
+                    "timestamp": 0.13979405537582099
+                },
+                {
+                    "x": 1.474996876177077,
+                    "y": 5.539073498761034,
+                    "heading": 3.1400000270834036,
+                    "angularVelocity": 3.0237681109940727e-7,
+                    "velocityX": 1.3412259710000187,
+                    "velocityY": -0.22453812350142868,
+                    "timestamp": 0.1630930646051245
+                },
+                {
+                    "x": 1.5107103012267313,
+                    "y": 5.5330947008269575,
+                    "heading": 3.1400000353289426,
+                    "angularVelocity": 3.539008296544223e-7,
+                    "velocityX": 1.5328302031287921,
+                    "velocityY": -0.25661168143439866,
+                    "timestamp": 0.186392073834428
+                },
+                {
+                    "x": 1.5508879197018797,
+                    "y": 5.526368650337421,
+                    "heading": 3.140000044849547,
+                    "angularVelocity": 4.086269951598913e-7,
+                    "velocityX": 1.7244346349549924,
+                    "velocityY": -0.2886839703505207,
+                    "timestamp": 0.2096910830637315
+                },
+                {
+                    "x": 1.5955297371899198,
+                    "y": 5.5188953827998875,
+                    "heading": 3.1400000557350607,
+                    "angularVelocity": 4.6720929680542457e-7,
+                    "velocityX": 1.9160393065938472,
+                    "velocityY": -0.3207547352763359,
+                    "timestamp": 0.232990092293035
+                },
+                {
+                    "x": 1.6446357605259214,
+                    "y": 5.510674941652546,
+                    "heading": 3.140000068095764,
+                    "angularVelocity": 5.305248328425052e-7,
+                    "velocityX": 2.107644271595818,
+                    "velocityY": -0.3528236358230067,
+                    "timestamp": 0.25628910152233847
+                },
+                {
+                    "x": 1.6982059982629796,
+                    "y": 5.501707381254886,
+                    "heading": 3.1400000820702374,
+                    "angularVelocity": 5.997883043629641e-7,
+                    "velocityX": 2.299249603699023,
+                    "velocityY": -0.38489020324511874,
+                    "timestamp": 0.27958811075164197
+                },
+                {
+                    "x": 1.7562404614129117,
+                    "y": 5.491992771597749,
+                    "heading": 3.1400000978378597,
+                    "angularVelocity": 6.767507519595272e-7,
+                    "velocityX": 2.490855408432506,
+                    "velocityY": -0.41695376664068867,
+                    "timestamp": 0.3028871199809455
+                },
+                {
+                    "x": 1.8187391646840712,
+                    "y": 5.4815312061757675,
+                    "heading": 3.140000115639928,
+                    "angularVelocity": 7.640697380946438e-7,
+                    "velocityX": 2.6824618444526434,
+                    "velocityY": -0.4490133172205749,
+                    "timestamp": 0.326186129210249
+                },
+                {
+                    "x": 1.8857021287172593,
+                    "y": 5.47032281621074,
+                    "heading": 3.140000135818395,
+                    "angularVelocity": 8.660654300433757e-7,
+                    "velocityX": 2.874069166381749,
+                    "velocityY": -0.48106723572405463,
+                    "timestamp": 0.3494851384395525
+                },
+                {
+                    "x": 1.9571293845792233,
+                    "y": 5.458367799245384,
+                    "heading": 3.14000015889542,
+                    "angularVelocity": 9.904723735244152e-7,
+                    "velocityX": 3.0656778217043392,
+                    "velocityY": -0.5131126756388134,
+                    "timestamp": 0.372784147668856
+                },
+                {
+                    "x": 2.033020984323074,
+                    "y": 5.445666486377279,
+                    "heading": 3.140000185766696,
+                    "angularVelocity": 0.000001153322655289093,
+                    "velocityX": 3.2572887111610362,
+                    "velocityY": -0.545143904752752,
+                    "timestamp": 0.3960831568981595
+                },
+                {
+                    "x": 2.113377033060567,
+                    "y": 5.432219546645115,
+                    "heading": 3.1400002183250084,
+                    "angularVelocity": 0.0000013974118596484493,
+                    "velocityX": 3.448904112043669,
+                    "velocityY": -0.5771464185388335,
+                    "timestamp": 0.419382166127463
+                },
+                {
+                    "x": 2.1981978506136755,
+                    "y": 5.41802904044786,
+                    "heading": 3.1400002631776545,
+                    "angularVelocity": 0.0000019250880740696254,
+                    "velocityX": 3.640533239775037,
+                    "velocityY": -0.6090604994243719,
+                    "timestamp": 0.4426811753567665
+                },
+                {
+                    "x": 2.2851188032420633,
+                    "y": 5.403428173451014,
+                    "heading": 3.140000263177585,
+                    "angularVelocity": -2.9753453619857404e-12,
+                    "velocityX": 3.73067162525748,
+                    "velocityY": -0.6266732998482398,
+                    "timestamp": 0.46598018458607
+                },
+                {
+                    "x": 2.372043080680341,
+                    "y": 5.388847120989839,
+                    "heading": 3.140000263177669,
+                    "angularVelocity": 3.609665835664611e-12,
+                    "velocityX": 3.730814327029464,
+                    "velocityY": -0.6258228544259767,
+                    "timestamp": 0.4892791938153735
+                },
+                {
+                    "x": 2.458969517114403,
+                    "y": 5.374278945126162,
+                    "heading": 3.140000263177672,
+                    "angularVelocity": 1.1048975247425585e-13,
+                    "velocityX": 3.7309069917332867,
+                    "velocityY": -0.6252701872544597,
+                    "timestamp": 0.5125782030446769
+                },
+                {
+                    "x": 2.5458971210754204,
+                    "y": 5.359717737461923,
+                    "heading": 3.1400002631776736,
+                    "angularVelocity": 8.62983817390415e-14,
+                    "velocityX": 3.7309571023169674,
+                    "velocityY": -0.6249711101842599,
+                    "timestamp": 0.5358772122739804
+                },
+                {
+                    "x": 2.632825281306951,
+                    "y": 5.345159851008328,
+                    "heading": 3.140000263177678,
+                    "angularVelocity": 1.7012303031669384e-13,
+                    "velocityX": 3.7309809776027962,
+                    "velocityY": -0.6248285628956001,
+                    "timestamp": 0.5591762215032839
+                },
+                {
+                    "x": 2.7197537163641496,
+                    "y": 5.330603605663881,
+                    "heading": 3.1400002631776815,
+                    "angularVelocity": 1.5893105684431692e-13,
+                    "velocityX": 3.7309927731970562,
+                    "velocityY": -0.6247581260336735,
+                    "timestamp": 0.5824752307325874
+                },
+                {
+                    "x": 2.8066823211500065,
+                    "y": 5.316048373923756,
+                    "heading": 3.140000263177685,
+                    "angularVelocity": 1.4661255756791518e-13,
+                    "velocityX": 3.7310000579983225,
+                    "velocityY": -0.6247146218539804,
+                    "timestamp": 0.605774239961891
+                },
+                {
+                    "x": 2.8936112175364808,
+                    "y": 5.301494883804396,
+                    "heading": 3.140000263177688,
+                    "angularVelocity": 1.4712645864153266e-13,
+                    "velocityX": 3.731012573578327,
+                    "velocityY": -0.6246398710039542,
+                    "timestamp": 0.6290732491911945
+                },
+                {
+                    "x": 2.9805406318812673,
+                    "y": 5.286944487835434,
+                    "heading": 3.1400002631776913,
+                    "angularVelocity": 1.2989281986974027e-13,
+                    "velocityX": 3.7310348044942168,
+                    "velocityY": -0.6245070692018166,
+                    "timestamp": 0.652372258420498
+                },
+                {
+                    "x": 3.067470472751592,
+                    "y": 5.272396640371872,
+                    "heading": 3.140000263177696,
+                    "angularVelocity": 2.2497040560944553e-13,
+                    "velocityX": 3.7310531110902496,
+                    "velocityY": -0.6243976866335479,
+                    "timestamp": 0.6756712676498015
+                },
+                {
+                    "x": 3.1544000489958055,
+                    "y": 5.257847211710117,
+                    "heading": 3.1400002631777,
+                    "angularVelocity": 1.6698909622359586e-13,
+                    "velocityX": 3.73104175326474,
+                    "velocityY": -0.6244655520950864,
+                    "timestamp": 0.698970276879105
+                },
+                {
+                    "x": 3.24132763546162,
+                    "y": 5.243285899348815,
+                    "heading": 3.1400002631776998,
+                    "angularVelocity": -2.5660759099238358e-14,
+                    "velocityX": 3.7309563514197523,
+                    "velocityY": -0.6249756038008819,
+                    "timestamp": 0.7222692861084085
+                },
+                {
+                    "x": 3.3282514237632603,
+                    "y": 5.228701930838363,
+                    "heading": 3.1400002631777038,
+                    "angularVelocity": 1.8102189988940347e-13,
+                    "velocityX": 3.730793333150296,
+                    "velocityY": -0.62594801206375,
+                    "timestamp": 0.745568295337712
+                },
+                {
+                    "x": 3.4151697732878015,
+                    "y": 5.214085582937486,
+                    "heading": 3.140000263177707,
+                    "angularVelocity": 1.4033623034653362e-13,
+                    "velocityX": 3.7305598993143207,
+                    "velocityY": -0.6273377445673929,
+                    "timestamp": 0.7688673045670155
+                },
+                {
+                    "x": 3.502081162717683,
+                    "y": 5.199427905968822,
+                    "heading": 3.1400002631777144,
+                    "angularVelocity": 3.3137702069013046e-13,
+                    "velocityX": 3.7302611701022674,
+                    "velocityY": -0.6291115997313443,
+                    "timestamp": 0.792166313796319
+                },
+                {
+                    "x": 3.588983960416736,
+                    "y": 5.18471937597994,
+                    "heading": 3.140000263177713,
+                    "angularVelocity": -6.829626544199834e-14,
+                    "velocityX": 3.729892410608752,
+                    "velocityY": -0.6312942256021684,
+                    "timestamp": 0.8154653230256225
+                },
+                {
+                    "x": 3.675876953607923,
+                    "y": 5.169953034745646,
+                    "heading": 3.140000263177716,
+                    "angularVelocity": 1.3540589961163151e-13,
+                    "velocityX": 3.7294715983848317,
+                    "velocityY": -0.6337755004484098,
+                    "timestamp": 0.838764332254926
+                },
+                {
+                    "x": 3.7627586434088305,
+                    "y": 5.155120331788361,
+                    "heading": 3.1400002631777193,
+                    "angularVelocity": 1.3533863072281271e-13,
+                    "velocityX": 3.728986453707805,
+                    "velocityY": -0.6366237641880743,
+                    "timestamp": 0.8620633414842295
+                },
+                {
+                    "x": 3.8490562562014863,
+                    "y": 5.137199419063008,
+                    "heading": 3.1400002631777224,
+                    "angularVelocity": 1.3559815009211054e-13,
+                    "velocityX": 3.7039177049679806,
+                    "velocityY": -0.7691705921529395,
+                    "timestamp": 0.885362350713533
+                },
+                {
+                    "x": 3.9343637094417163,
+                    "y": 5.1150391964944655,
+                    "heading": 3.1400002631777255,
+                    "angularVelocity": 1.370480919312873e-13,
+                    "velocityX": 3.6614197797262853,
+                    "velocityY": -0.9511229576576579,
+                    "timestamp": 0.9086613599428365
+                },
+                {
+                    "x": 4.018431663513184,
+                    "y": 5.088562488555908,
+                    "heading": 3.140000263177729,
+                    "angularVelocity": 1.413301961445022e-13,
+                    "velocityX": 3.6082201283346866,
+                    "velocityY": -1.1363877183834126,
+                    "timestamp": 0.93196036917214
+                },
+                {
+                    "x": 4.095482183944048,
+                    "y": 5.0601896166081985,
+                    "heading": 3.140000263177732,
+                    "angularVelocity": 1.478757127486931e-13,
+                    "velocityX": 3.549906540385796,
+                    "velocityY": -1.3072078310833697,
+                    "timestamp": 0.9536653125429956
+                },
+                {
+                    "x": 4.171099571738343,
+                    "y": 5.028192533992809,
+                    "heading": 3.1400002631777353,
+                    "angularVelocity": 1.4523452590479803e-13,
+                    "velocityX": 3.483878603243662,
+                    "velocityY": -1.4741841095171686,
+                    "timestamp": 0.9753702559138512
+                },
+                {
+                    "x": 4.2451557679063034,
+                    "y": 4.992731534520922,
+                    "heading": 3.1400002631777384,
+                    "angularVelocity": 1.4237189069796376e-13,
+                    "velocityX": 3.4119506742159467,
+                    "velocityY": -1.6337752587458831,
+                    "timestamp": 0.9970751992847068
+                },
+                {
+                    "x": 4.31781550635648,
+                    "y": 4.954490651976443,
+                    "heading": 3.1400002631777415,
+                    "angularVelocity": 1.4182458601020346e-13,
+                    "velocityX": 3.347612440573966,
+                    "velocityY": -1.761851293089538,
+                    "timestamp": 1.0187801426555625
+                },
+                {
+                    "x": 4.390182790341615,
+                    "y": 4.91569917224677,
+                    "heading": 3.140000263177745,
+                    "angularVelocity": 1.421722636666585e-13,
+                    "velocityX": 3.3341383457540337,
+                    "velocityY": -1.787218656449447,
+                    "timestamp": 1.040485086026418
+                },
+                {
+                    "x": 4.462533664586848,
+                    "y": 4.87687709490227,
+                    "heading": 3.1400002631777486,
+                    "angularVelocity": 1.635268036057102e-13,
+                    "velocityX": 3.333382308772387,
+                    "velocityY": -1.7886283636415046,
+                    "timestamp": 1.0621900293972737
+                },
+                {
+                    "x": 4.534860115355159,
+                    "y": 4.838009534998386,
+                    "heading": 3.140000263177751,
+                    "angularVelocity": 1.1808318022164126e-13,
+                    "velocityX": 3.3322570592611274,
+                    "velocityY": -1.7907238567558525,
+                    "timestamp": 1.0838949727681293
+                },
+                {
+                    "x": 4.607177418550424,
+                    "y": 4.799124957707118,
+                    "heading": 3.140000263177754,
+                    "angularVelocity": 1.4385253669729274e-13,
+                    "velocityX": 3.3318356081243334,
+                    "velocityY": -1.7915078895502723,
+                    "timestamp": 1.105599916138985
+                },
+                {
+                    "x": 4.679489672949611,
+                    "y": 4.7602309921876325,
+                    "heading": 3.1400002631777575,
+                    "angularVelocity": 1.4120580598782794e-13,
+                    "velocityX": 3.331602997698768,
+                    "velocityY": -1.7919404282640008,
+                    "timestamp": 1.1273048595098405
+                },
+                {
+                    "x": 4.751800053049455,
+                    "y": 4.721333542151952,
+                    "heading": 3.1400002631777606,
+                    "angularVelocity": 1.4195129118241079e-13,
+                    "velocityX": 3.3315166441264745,
+                    "velocityY": -1.7921009684797435,
+                    "timestamp": 1.149009802880696
+                },
+                {
+                    "x": 4.824110260922509,
+                    "y": 4.682435771933003,
+                    "heading": 3.1400002631777637,
+                    "angularVelocity": 1.420416383867913e-13,
+                    "velocityX": 3.3315087092142113,
+                    "velocityY": -1.7921157201114153,
+                    "timestamp": 1.1707147462515517
+                },
+                {
+                    "x": 4.896420645274053,
+                    "y": 4.643538329777489,
+                    "heading": 3.140000263177767,
+                    "angularVelocity": 1.4171119036284518e-13,
+                    "velocityX": 3.3315168400108037,
+                    "velocityY": -1.7921006054248252,
+                    "timestamp": 1.1924196896224073
+                },
+                {
+                    "x": 4.96873125432361,
+                    "y": 4.6046413053457185,
+                    "heading": 3.14000026317777,
+                    "angularVelocity": 1.4137748376763704e-13,
+                    "velocityX": 3.3315271924000274,
+                    "velocityY": -1.792081359866257,
+                    "timestamp": 1.214124632993263
+                },
+                {
+                    "x": 5.04104400801269,
+                    "y": 4.56574826813338,
+                    "heading": 3.1400002631777735,
+                    "angularVelocity": 1.421351986736705e-13,
+                    "velocityX": 3.331626001206414,
+                    "velocityY": -1.7918976588811946,
+                    "timestamp": 1.2358295763641185
+                },
+                {
+                    "x": 5.11336222581346,
+                    "y": 4.5268653919090065,
+                    "heading": 3.1400002631777766,
+                    "angularVelocity": 1.4239328375117255e-13,
+                    "velocityX": 3.3318777462404188,
+                    "velocityY": -1.7914295172313686,
+                    "timestamp": 1.257534519734974
+                },
+                {
+                    "x": 5.185674775789392,
+                    "y": 4.487971975945349,
+                    "heading": 3.1400002631777797,
+                    "angularVelocity": 1.4189340221530015e-13,
+                    "velocityX": 3.331616615642773,
+                    "velocityY": -1.7919151088820495,
+                    "timestamp": 1.2792394631058297
+                },
+                {
+                    "x": 5.25798679998857,
+                    "y": 4.4490775824474165,
+                    "heading": 3.140000263177783,
+                    "angularVelocity": 1.4182847689207558e-13,
+                    "velocityX": 3.331592391818018,
+                    "velocityY": -1.791960146283848,
+                    "timestamp": 1.3009444064766853
+                },
+                {
+                    "x": 5.330301009987797,
+                    "y": 4.410187253034054,
+                    "heading": 3.1400002631777864,
+                    "angularVelocity": 1.4221031722558925e-13,
+                    "velocityX": 3.331693096990619,
+                    "velocityY": -1.7917729039338035,
+                    "timestamp": 1.322649349847541
+                },
+                {
+                    "x": 5.4026181052176305,
+                    "y": 4.3713022890025295,
+                    "heading": 3.1400002631777895,
+                    "angularVelocity": 1.4161705945188678e-13,
+                    "velocityX": 3.3318260266449937,
+                    "velocityY": -1.791525707625898,
+                    "timestamp": 1.3443542932183965
+                },
+                {
+                    "x": 5.474936056465776,
+                    "y": 4.332418917012381,
+                    "heading": 3.1400002631777926,
+                    "angularVelocity": 1.4174219306640175e-13,
+                    "velocityX": 3.331865465508907,
+                    "velocityY": -1.7914523583735324,
+                    "timestamp": 1.366059236589252
+                },
+                {
+                    "x": 5.54725002868687,
+                    "y": 4.293528145471586,
+                    "heading": 3.1400002631777957,
+                    "angularVelocity": 1.4114123611773667e-13,
+                    "velocityX": 3.331682141968083,
+                    "velocityY": -1.7917932738304505,
+                    "timestamp": 1.3877641799601077
+                },
+                {
+                    "x": 5.619560804995751,
+                    "y": 4.2546314319979865,
+                    "heading": 3.1400002631777992,
+                    "angularVelocity": 1.438802546332647e-13,
+                    "velocityX": 3.331534898449109,
+                    "velocityY": -1.7920670332572342,
+                    "timestamp": 1.4094691233309633
+                },
+                {
+                    "x": 5.691871264235437,
+                    "y": 4.215734129079228,
+                    "heading": 3.1400002631778023,
+                    "angularVelocity": 1.4227513855970737e-13,
+                    "velocityX": 3.331520290292617,
+                    "velocityY": -1.7920941904433432,
+                    "timestamp": 1.431174066701819
+                },
+                {
+                    "x": 5.7641829192535345,
+                    "y": 4.1768390492074925,
+                    "heading": 3.1400002631778055,
+                    "angularVelocity": 1.4198857424426595e-13,
+                    "velocityX": 3.3315753827383325,
+                    "velocityY": -1.79199176921174,
+                    "timestamp": 1.4528790100726745
+                },
+                {
+                    "x": 5.836496897649906,
+                    "y": 4.13794828914952,
+                    "heading": 3.1400002631778086,
+                    "angularVelocity": 1.420236059365936e-13,
+                    "velocityX": 3.331682426477163,
+                    "velocityY": -1.7917927447907471,
+                    "timestamp": 1.47458395344353
+                },
+                {
+                    "x": 5.90881236073979,
+                    "y": 4.099060289876852,
+                    "heading": 3.1400002631778117,
+                    "angularVelocity": 1.4199973949506742e-13,
+                    "velocityX": 3.3317508299509484,
+                    "velocityY": -1.7916655486362405,
+                    "timestamp": 1.4962888968143857
+                },
+                {
+                    "x": 5.981129099244504,
+                    "y": 4.06017466243704,
+                    "heading": 3.140000263177815,
+                    "angularVelocity": 1.4813660843765197e-13,
+                    "velocityX": 3.3318095914418606,
+                    "velocityY": -1.7915562724764487,
+                    "timestamp": 1.5179938401852413
+                },
+                {
+                    "x": 6.053446077627243,
+                    "y": 4.021289481108035,
+                    "heading": 3.140000263177829,
+                    "angularVelocity": 6.425453029046258e-13,
+                    "velocityX": 3.331820643210356,
+                    "velocityY": -1.7915357190582988,
+                    "timestamp": 1.539698783556097
+                },
+                {
+                    "x": 6.125761091363011,
+                    "y": 3.9824006462217088,
+                    "heading": 3.140000263177869,
+                    "angularVelocity": 1.8383501574993634e-12,
+                    "velocityX": 3.331730127102014,
+                    "velocityY": -1.7917040474119204,
+                    "timestamp": 1.5614037269269525
+                },
+                {
+                    "x": 6.198074029937981,
+                    "y": 3.9435079527230394,
+                    "heading": 3.140000263178238,
+                    "angularVelocity": 1.7022488728392152e-11,
+                    "velocityX": 3.331634519350639,
+                    "velocityY": -1.79188182315355,
+                    "timestamp": 1.583108670297808
+                },
+                {
+                    "x": 6.270380104650612,
+                    "y": 3.9046024999630506,
+                    "heading": 3.140000263177847,
+                    "angularVelocity": -1.8008440986287348e-11,
+                    "velocityX": 3.33131828436938,
+                    "velocityY": -1.7924696736324577,
+                    "timestamp": 1.6048136136686637
+                },
+                {
+                    "x": 6.342678298341779,
+                    "y": 3.8656824038223503,
+                    "heading": 3.1400002631782598,
+                    "angularVelocity": 1.900200591260141e-11,
+                    "velocityX": 3.3309551863724884,
+                    "velocityY": -1.7931443301131518,
+                    "timestamp": 1.6265185570395193
+                },
+                {
+                    "x": 6.414967377910046,
+                    "y": 3.8267453820171355,
+                    "heading": 3.1400002631778587,
+                    "angularVelocity": -1.8477715999059415e-11,
+                    "velocityX": 3.3305352763678626,
+                    "velocityY": -1.7939241370065324,
+                    "timestamp": 1.648223500410375
+                },
+                {
+                    "x": 6.487241234316261,
+                    "y": 3.787780110681628,
+                    "heading": 3.1400002631778627,
+                    "angularVelocity": 1.954226890690534e-13,
+                    "velocityX": 3.3298339079393355,
+                    "velocityY": -1.795225662182324,
+                    "timestamp": 1.6699284437812305
+                },
+                {
+                    "x": 6.559496489740858,
+                    "y": 3.7487803574447534,
+                    "heading": 3.1400002631778654,
+                    "angularVelocity": 1.160046399416623e-13,
+                    "velocityX": 3.3289769150756516,
+                    "velocityY": -1.79681432798537,
+                    "timestamp": 1.691633387152086
+                },
+                {
+                    "x": 6.630585131325424,
+                    "y": 3.7076925524537225,
+                    "heading": 3.140000263177869,
+                    "angularVelocity": 1.5938094021424355e-13,
+                    "velocityX": 3.275228152865242,
+                    "velocityY": -1.8930159958948245,
+                    "timestamp": 1.7133383305229417
+                },
+                {
+                    "x": 6.699761336489547,
+                    "y": 3.6634607167510884,
+                    "heading": 3.140000263177872,
+                    "angularVelocity": 1.431049344650049e-13,
+                    "velocityX": 3.1871175142993193,
+                    "velocityY": -2.037869205501672,
+                    "timestamp": 1.7350432738937973
+                },
+                {
+                    "x": 6.76677589557052,
+                    "y": 3.6160175775021286,
+                    "heading": 3.140000263178055,
+                    "angularVelocity": 8.443311888840044e-12,
+                    "velocityX": 3.087525175069758,
+                    "velocityY": -2.1858218396761058,
+                    "timestamp": 1.756748217264653
+                },
+                {
+                    "x": 6.8314564099651545,
+                    "y": 3.5654385941456423,
+                    "heading": 3.140000263178058,
+                    "angularVelocity": 1.5118918552164865e-13,
+                    "velocityX": 2.9799900091650118,
+                    "velocityY": -2.3302978723451035,
+                    "timestamp": 1.7784531606355085
+                },
+                {
+                    "x": 6.893649101257324,
+                    "y": 3.5118300914764404,
+                    "heading": 3.1400002631780617,
+                    "angularVelocity": 1.5501587028498165e-13,
+                    "velocityX": 2.865369894292817,
+                    "velocityY": -2.469875260820684,
+                    "timestamp": 1.8001581040063641
+                },
+                {
+                    "x": 6.9342196158595195,
+                    "y": 3.4745269537623855,
+                    "heading": 3.140000263178065,
+                    "angularVelocity": 2.3326761756772743e-13,
+                    "velocityX": 2.784724807966257,
+                    "velocityY": -2.5604549024296013,
+                    "timestamp": 1.8147270533280853
+                },
+                {
+                    "x": 6.973574770579654,
+                    "y": 3.435943763171971,
+                    "heading": 3.140000263178068,
+                    "angularVelocity": 2.2697466808256025e-13,
+                    "velocityX": 2.701303563563437,
+                    "velocityY": -2.648316617649584,
+                    "timestamp": 1.8292960026498064
+                },
+                {
+                    "x": 7.011676569760775,
+                    "y": 3.396122365046946,
+                    "heading": 3.140000263178071,
+                    "angularVelocity": 2.224027473304724e-13,
+                    "velocityX": 2.615274330340327,
+                    "velocityY": -2.7333061050358776,
+                    "timestamp": 1.8438649519715276
+                },
+                {
+                    "x": 7.048491012484172,
+                    "y": 3.355107883496788,
+                    "heading": 3.1400002631780746,
+                    "angularVelocity": 2.1692590269440246e-13,
+                    "velocityX": 2.5269113036529633,
+                    "velocityY": -2.815198312825684,
+                    "timestamp": 1.8584339012932487
+                },
+                {
+                    "x": 7.083992093604447,
+                    "y": 3.312951458599195,
+                    "heading": 3.1400002631780777,
+                    "angularVelocity": 2.1487378521690259e-13,
+                    "velocityX": 2.4367633064212786,
+                    "velocityY": -2.893580310196796,
+                    "timestamp": 1.8730028506149698
+                },
+                {
+                    "x": 7.118173429008876,
+                    "y": 3.269718054236438,
+                    "heading": 3.140000263178081,
+                    "angularVelocity": 2.1169602046875272e-13,
+                    "velocityX": 2.3461771092494,
+                    "velocityY": -2.967503243236169,
+                    "timestamp": 1.887571799936691
+                },
+                {
+                    "x": 7.151084190942531,
+                    "y": 3.2255097664946186,
+                    "heading": 3.140000263178084,
+                    "angularVelocity": 2.115299519607656e-13,
+                    "velocityX": 2.258966052177459,
+                    "velocityY": -3.034418389794455,
+                    "timestamp": 1.9021407492584121
+                },
+                {
+                    "x": 7.1829258498520225,
+                    "y": 3.1805253278057903,
+                    "heading": 3.1400002631783006,
+                    "angularVelocity": 1.4861478341138462e-11,
+                    "velocityX": 2.1855837511923784,
+                    "velocityY": -3.0876927152018316,
+                    "timestamp": 1.9167096985801333
+                },
+                {
+                    "x": 7.213982568749707,
+                    "y": 3.1352979487738755,
+                    "heading": 3.140000221251036,
+                    "angularVelocity": -0.000002877850999701821,
+                    "velocityX": 2.1317061520275473,
+                    "velocityY": -3.104367928885527,
+                    "timestamp": 1.9312786479018544
+                },
+                {
+                    "x": 7.244041792187231,
+                    "y": 3.091532444769153,
+                    "heading": 3.1400001920667906,
+                    "angularVelocity": -0.000002003181212693182,
+                    "velocityX": 2.063238931905195,
+                    "velocityY": -3.0040260994983523,
+                    "timestamp": 1.9458475972235756
+                },
+                {
+                    "x": 7.273101015062598,
+                    "y": 3.049227126183516,
+                    "heading": 3.140000168751395,
+                    "angularVelocity": -0.0000016003484573673396,
+                    "velocityX": 1.9945997637621933,
+                    "velocityY": -2.9038002433409447,
+                    "timestamp": 1.9604165465452967
+                },
+                {
+                    "x": 7.301159421127851,
+                    "y": 3.008381441393539,
+                    "heading": 3.1400001491208855,
+                    "angularVelocity": -0.0000013474210949527603,
+                    "velocityX": 1.92590456907015,
+                    "velocityY": -2.8036122501352616,
+                    "timestamp": 1.9749854958670179
+                },
+                {
+                    "x": 7.328216598566981,
+                    "y": 2.9689951117692965,
+                    "heading": 3.1400001321528324,
+                    "angularVelocity": -0.0000011646724122855417,
+                    "velocityX": 1.8571811076855331,
+                    "velocityY": -2.703443381845065,
+                    "timestamp": 1.989554445188739
+                },
+                {
+                    "x": 7.35427229894417,
+                    "y": 2.9310679691270813,
+                    "heading": 3.140000117255664,
+                    "angularVelocity": -0.0000010225286768555236,
+                    "velocityX": 1.7884405938827321,
+                    "velocityY": -2.603286057538151,
+                    "timestamp": 2.00412339451046
+                },
+                {
+                    "x": 7.379326356096679,
+                    "y": 2.8945999009401007,
+                    "heading": 3.1400001040435335,
+                    "angularVelocity": -9.068691499457651e-7,
+                    "velocityX": 1.7196886748142857,
+                    "velocityY": -2.50313645697232,
+                    "timestamp": 2.0186923438321815
+                },
+                {
+                    "x": 7.403378651082293,
+                    "y": 2.8595908266403365,
+                    "heading": 3.1400000922448146,
+                    "angularVelocity": -8.098538018281321e-7,
+                    "velocityX": 1.650928591655569,
+                    "velocityY": -2.4029923865251464,
+                    "timestamp": 2.033261293153903
+                },
+                {
+                    "x": 7.426429094558334,
+                    "y": 2.826040685699055,
+                    "heading": 3.1400000816578917,
+                    "angularVelocity": -7.266771822683861e-7,
+                    "velocityX": 1.5821623760933374,
+                    "velocityY": -2.3028524707174887,
+                    "timestamp": 2.0478302424756243
+                },
+                {
+                    "x": 7.448477616956692,
+                    "y": 2.793949430978311,
+                    "heading": 3.1400000721271697,
+                    "angularVelocity": -6.541804651955507e-7,
+                    "velocityX": 1.513391385436516,
+                    "velocityY": -2.202715790417496,
+                    "timestamp": 2.0623991917973457
+                },
+                {
+                    "x": 7.4695241625734585,
+                    "y": 2.763317024730304,
+                    "heading": 3.1400000635289262,
+                    "angularVelocity": -5.901759591769764e-7,
+                    "velocityX": 1.4446165713119874,
+                    "velocityY": -2.102581701093514,
+                    "timestamp": 2.076968141119067
+                },
+                {
+                    "x": 7.489568685800359,
+                    "y": 2.734143436045934,
+                    "heading": 3.1400000557624246,
+                    "angularVelocity": -5.330859058092693e-7,
+                    "velocityX": 1.3758386266751614,
+                    "velocityY": -2.0024497333432913,
+                    "timestamp": 2.0915370904407884
+                },
+                {
+                    "x": 7.508611148608458,
+                    "y": 2.7064286391508827,
+                    "heading": 3.140000048744052,
+                    "angularVelocity": -4.817349694037634e-7,
+                    "velocityX": 1.3070580717652986,
+                    "velocityY": -1.902319534719876,
+                    "timestamp": 2.1061060397625098
+                },
+                {
+                    "x": 7.526651518803911,
+                    "y": 2.680172612224319,
+                    "heading": 3.1400000424032948,
+                    "angularVelocity": -4.3522407744534453e-7,
+                    "velocityX": 1.2382753070979928,
+                    "velocityY": -1.8021908338592867,
+                    "timestamp": 2.120674989084231
+                },
+                {
+                    "x": 7.543689768780771,
+                    "y": 2.6553753365541346,
+                    "heading": 3.1400000366798815,
+                    "angularVelocity": -3.9285011556875146e-7,
+                    "velocityX": 1.1694906475824893,
+                    "velocityY": -1.7020634173815092,
+                    "timestamp": 2.1352439384059525
+                },
+                {
+                    "x": 7.559725874605572,
+                    "y": 2.6320367959168407,
+                    "heading": 3.1400000315217036,
+                    "angularVelocity": -3.5405282918878005e-7,
+                    "velocityX": 1.100704345294715,
+                    "velocityY": -1.6019371144699315,
+                    "timestamp": 2.149812887727674
+                },
+                {
+                    "x": 7.574759815330251,
+                    "y": 2.6101569761120964,
+                    "heading": 3.140000026883262,
+                    "angularVelocity": -3.1837857149022665e-7,
+                    "velocityX": 1.0319166051501305,
+                    "velocityY": -1.5018117862573288,
+                    "timestamp": 2.1643818370493952
+                },
+                {
+                    "x": 7.588791572466392,
+                    "y": 2.5897358646065065,
+                    "heading": 3.1400000227244846,
+                    "angularVelocity": -2.854548657017254e-7,
+                    "velocityX": 0.9631275959768569,
+                    "velocityY": -1.4016873183261147,
+                    "timestamp": 2.1789507863711166
+                },
+                {
+                    "x": 7.601821129576078,
+                    "y": 2.570773450256394,
+                    "heading": 3.140000019009809,
+                    "angularVelocity": -2.5497212819286766e-7,
+                    "velocityX": 0.8943374585191634,
+                    "velocityY": -1.3015636152871453,
+                    "timestamp": 2.193519735692838
+                },
+                {
+                    "x": 7.61384847194871,
+                    "y": 2.5532697230888193,
+                    "heading": 3.1400000157074612,
+                    "angularVelocity": -2.2667026217565558e-7,
+                    "velocityX": 0.8255463113387961,
+                    "velocityY": -1.201440596782113,
+                    "timestamp": 2.2080886850145593
+                },
+                {
+                    "x": 7.624873586342355,
+                    "y": 2.5372246741263043,
+                    "heading": 3.1400000127888834,
+                    "angularVelocity": -2.0032864319885596e-7,
+                    "velocityX": 0.7567542552436427,
+                    "velocityY": -1.1013181944833246,
+                    "timestamp": 2.2226576343362807
+                },
+                {
+                    "x": 7.634896460774273,
+                    "y": 2.5226382952448834,
+                    "heading": 3.140000010228267,
+                    "angularVelocity": -1.757585156167035e-7,
+                    "velocityX": 0.6879613766637577,
+                    "velocityY": -1.0011963498064516,
+                    "timestamp": 2.237226583658002
+                },
+                {
+                    "x": 7.643917084349478,
+                    "y": 2.509510579057917,
+                    "heading": 3.140000008002173,
+                    "angularVelocity": -1.527971366621834e-7,
+                    "velocityX": 0.6191677502613596,
+                    "velocityY": -0.9010750121421621,
+                    "timestamp": 2.2517955329797235
+                },
+                {
+                    "x": 7.6519354471190555,
+                    "y": 2.4978415188200733,
+                    "heading": 3.140000006089223,
+                    "angularVelocity": -1.3130320584948841e-7,
+                    "velocityX": 0.5503734409743167,
+                    "velocityY": -0.8009541374715159,
+                    "timestamp": 2.266364482301445
+                },
+                {
+                    "x": 7.658951539962077,
+                    "y": 2.487631108347293,
+                    "heading": 3.1400000044698366,
+                    "angularVelocity": -1.1115325658440658e-7,
+                    "velocityX": 0.4815785056345368,
+                    "velocityY": -0.7008336872693404,
+                    "timestamp": 2.280933431623166
+                },
+                {
+                    "x": 7.664965354486367,
+                    "y": 2.478879341949535,
+                    "heading": 3.1400000031260147,
+                    "angularVelocity": -9.223877588817121e-8,
+                    "velocityX": 0.4127829942631362,
+                    "velocityY": -0.600713627626585,
+                    "timestamp": 2.2955023809448876
+                },
+                {
+                    "x": 7.669976882944524,
+                    "y": 2.4715862143738576,
+                    "heading": 3.140000002041154,
+                    "angularVelocity": -7.44638832915239e-8,
+                    "velocityX": 0.34398695111687533,
+                    "velocityY": -0.5005939285411971,
+                    "timestamp": 2.310071330266609
+                },
+                {
+                    "x": 7.673986118162365,
+                    "y": 2.4657517207559176,
+                    "heading": 3.1400000011998928,
+                    "angularVelocity": -5.774344306809202e-8,
+                    "velocityX": 0.27519041554106066,
+                    "velocityY": -0.4004745633401276,
+                    "timestamp": 2.3246402795883303
+                },
+                {
+                    "x": 7.676993053477586,
+                    "y": 2.4613758565783885,
+                    "heading": 3.1400000005879747,
+                    "angularVelocity": -4.2001516695103525e-8,
+                    "velocityX": 0.20639342267033312,
+                    "velocityY": -0.30035550820440443,
+                    "timestamp": 2.3392092289100517
+                },
+                {
+                    "x": 7.6789976826868624,
+                    "y": 2.458458617635108,
+                    "heading": 3.1400000001921353,
+                    "angularVelocity": -2.7170084001525683e-8,
+                    "velocityX": 0.13759600400877475,
+                    "velocityY": -0.2002367417759812,
+                    "timestamp": 2.353778178231773
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": -1.3187979232574289e-8,
+                    "velocityX": 0.06879818791344262,
+                    "velocityY": -0.10011824483002574,
+                    "timestamp": 2.3683471275534944
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": 2.7611774763020037e-32,
+                    "velocityX": -1.1210620234419235e-31,
+                    "velocityY": 4.358756882663372e-31,
+                    "timestamp": 2.382916076875216
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -2304,26 +5245,10 @@
                     "type": "StopPoint"
                 }
             ],
-            "usesControlIntervalGuessing": true,
+            "usesControlIntervalGuessing": false,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "Center To C5": {
             "waypoints": [
@@ -2334,7 +5259,25 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 26
+                    "controlIntervalCount": 28
+                },
+                {
+                    "x": 2.7507288455963135,
+                    "y": 3.0107133388519287,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 5.311619758605957,
+                    "y": 1.6392407417297363,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 27
                 },
                 {
                     "x": 7.68,
@@ -2346,7 +5289,629 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 1.35,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 3.295386080925321e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 1.3546830312136446,
+                    "y": 5.550921973415328,
+                    "heading": 3.14,
+                    "angularVelocity": -6.105321890596023e-17,
+                    "velocityX": 0.13379730014070298,
+                    "velocityY": -0.2593652256885782,
+                    "timestamp": 0.03500093954603884
+                },
+                {
+                    "x": 1.364049093509165,
+                    "y": 5.532765920501418,
+                    "heading": 3.14,
+                    "angularVelocity": -9.872410216704159e-17,
+                    "velocityX": 0.2675945965166781,
+                    "velocityY": -0.5187304440792354,
+                    "timestamp": 0.07000187909207768
+                },
+                {
+                    "x": 1.3780981867284383,
+                    "y": 5.505531841564791,
+                    "heading": 3.14,
+                    "angularVelocity": -1.396517144912572e-16,
+                    "velocityX": 0.40139188837498685,
+                    "velocityY": -0.7780956537124009,
+                    "timestamp": 0.10500281863811653
+                },
+                {
+                    "x": 1.3968303106782043,
+                    "y": 5.46921973698008,
+                    "heading": 3.14,
+                    "angularVelocity": -1.3147784440878345e-16,
+                    "velocityX": 0.5351891747117138,
+                    "velocityY": -1.0374608526419855,
+                    "timestamp": 0.14000375818415536
+                },
+                {
+                    "x": 1.4202454651168874,
+                    "y": 5.423829607215582,
+                    "heading": 3.14,
+                    "angularVelocity": -7.833997421538468e-18,
+                    "velocityX": 0.6689864541464796,
+                    "velocityY": -1.2968260381921248,
+                    "timestamp": 0.1750046977301942
+                },
+                {
+                    "x": 1.4483436497338924,
+                    "y": 5.369361452873384,
+                    "heading": 3.14,
+                    "angularVelocity": -2.0049167233544137e-17,
+                    "velocityX": 0.802783724707322,
+                    "velocityY": -1.5561912065401715,
+                    "timestamp": 0.21000563727623303
+                },
+                {
+                    "x": 1.4811248641150931,
+                    "y": 5.305815274756268,
+                    "heading": 3.14,
+                    "angularVelocity": -8.129804229935506e-17,
+                    "velocityX": 0.9365809834363131,
+                    "velocityY": -1.8155563519521838,
+                    "timestamp": 0.24500657682227187
+                },
+                {
+                    "x": 1.5185891076807168,
+                    "y": 5.233191073988126,
+                    "heading": 3.14,
+                    "angularVelocity": -1.3088931274135507e-16,
+                    "velocityX": 1.0703782256008025,
+                    "velocityY": -2.0749214652539205,
+                    "timestamp": 0.2800075163683107
+                },
+                {
+                    "x": 1.56073637956111,
+                    "y": 5.151488852254778,
+                    "heading": 3.14,
+                    "angularVelocity": -1.390272479709275e-16,
+                    "velocityX": 1.204175442918738,
+                    "velocityY": -2.334286530390624,
+                    "timestamp": 0.31500845591434956
+                },
+                {
+                    "x": 1.6075666783068707,
+                    "y": 5.060708612365892,
+                    "heading": 3.14,
+                    "angularVelocity": -2.002620206188659e-16,
+                    "velocityX": 1.3379726188262815,
+                    "velocityY": -2.593651515253289,
+                    "timestamp": 0.3500093954603884
+                },
+                {
+                    "x": 1.6590800010192643,
+                    "y": 4.9608503599406655,
+                    "heading": 3.14,
+                    "angularVelocity": -2.3956924314672257e-16,
+                    "velocityX": 1.471769711915058,
+                    "velocityY": -2.8530163395717247,
+                    "timestamp": 0.38501033500642723
+                },
+                {
+                    "x": 1.7152763390026582,
+                    "y": 4.851914111835608,
+                    "heading": 3.14,
+                    "angularVelocity": -2.6938408534488506e-16,
+                    "velocityX": 1.6055665565638675,
+                    "velocityY": -3.112380682288517,
+                    "timestamp": 0.42001127455246606
+                },
+                {
+                    "x": 1.7759790338056225,
+                    "y": 4.734242319347403,
+                    "heading": 3.14,
+                    "angularVelocity": 3.279195870571412e-16,
+                    "velocityX": 1.734316152375285,
+                    "velocityY": -3.3619609648879023,
+                    "timestamp": 0.4550122140985049
+                },
+                {
+                    "x": 1.8366817286107124,
+                    "y": 4.616570526856246,
+                    "heading": 3.14,
+                    "angularVelocity": -3.1071543812317194e-16,
+                    "velocityX": 1.7343161524360031,
+                    "velocityY": -3.36196096497222,
+                    "timestamp": 0.49001315364454373
+                },
+                {
+                    "x": 1.8973844234157842,
+                    "y": 4.498898734365081,
+                    "heading": 3.14,
+                    "angularVelocity": -2.903673566166758e-16,
+                    "velocityX": 1.7343161524354895,
+                    "velocityY": -3.361960964972484,
+                    "timestamp": 0.5250140931905826
+                },
+                {
+                    "x": 1.958087118220856,
+                    "y": 4.381226941873915,
+                    "heading": 3.14,
+                    "angularVelocity": 3.614460545323391e-16,
+                    "velocityX": 1.7343161524354895,
+                    "velocityY": -3.3619609649724835,
+                    "timestamp": 0.5600150327366215
+                },
+                {
+                    "x": 2.0187898130259274,
+                    "y": 4.26355514938275,
+                    "heading": 3.14,
+                    "angularVelocity": -3.3191632817458094e-16,
+                    "velocityX": 1.7343161524354893,
+                    "velocityY": -3.361960964972483,
+                    "timestamp": 0.5950159722826603
+                },
+                {
+                    "x": 2.079492507830999,
+                    "y": 4.145883356891584,
+                    "heading": 3.14,
+                    "angularVelocity": 3.2426066858348807e-16,
+                    "velocityX": 1.7343161524354893,
+                    "velocityY": -3.361960964972483,
+                    "timestamp": 0.6300169118286992
+                },
+                {
+                    "x": 2.140195202636071,
+                    "y": 4.028211564400418,
+                    "heading": 3.14,
+                    "angularVelocity": -2.798848785469217e-16,
+                    "velocityX": 1.734316152435488,
+                    "velocityY": -3.3619609649724826,
+                    "timestamp": 0.6650178513747381
+                },
+                {
+                    "x": 2.200897897441142,
+                    "y": 3.9105397719092525,
+                    "heading": 3.14,
+                    "angularVelocity": -1.795170282800533e-16,
+                    "velocityX": 1.734316152435471,
+                    "velocityY": -3.361960964972468,
+                    "timestamp": 0.700018790920777
+                },
+                {
+                    "x": 2.2616005922462064,
+                    "y": 3.792867979418094,
+                    "heading": 3.14,
+                    "angularVelocity": -3.385716636422731e-16,
+                    "velocityX": 1.7343161524352515,
+                    "velocityY": -3.3619609649722846,
+                    "timestamp": 0.7350197304668159
+                },
+                {
+                    "x": 2.3223032870512124,
+                    "y": 3.6751961869269927,
+                    "heading": 3.14,
+                    "angularVelocity": -3.759822562526031e-16,
+                    "velocityX": 1.7343161524334345,
+                    "velocityY": -3.361960964970739,
+                    "timestamp": 0.7700206700128548
+                },
+                {
+                    "x": 2.383005981879538,
+                    "y": 3.5575243944476096,
+                    "heading": 3.14,
+                    "angularVelocity": -2.6556763706975105e-16,
+                    "velocityX": 1.7343161531001825,
+                    "velocityY": -3.3619609646356636,
+                    "timestamp": 0.8050216095588937
+                },
+                {
+                    "x": 2.443708981028518,
+                    "y": 3.4398527589551926,
+                    "heading": 3.14,
+                    "angularVelocity": 5.092773468950765e-17,
+                    "velocityX": 1.7343248477441577,
+                    "velocityY": -3.3619564794134043,
+                    "timestamp": 0.8400225491049326
+                },
+                {
+                    "x": 2.507799181067132,
+                    "y": 3.3239911422386283,
+                    "heading": 3.14,
+                    "angularVelocity": -2.1081778211927598e-16,
+                    "velocityX": 1.8310994181462568,
+                    "velocityY": -3.310243045471397,
+                    "timestamp": 0.8750234886509715
+                },
+                {
+                    "x": 2.5806292795037855,
+                    "y": 3.2134142705817266,
+                    "heading": 3.14,
+                    "angularVelocity": -3.875593095285532e-16,
+                    "velocityX": 2.0808040978720626,
+                    "velocityY": -3.159254382625257,
+                    "timestamp": 0.9100244281970103
+                },
+                {
+                    "x": 2.661766753531331,
+                    "y": 3.108780729790167,
+                    "heading": 3.14,
+                    "angularVelocity": -7.347130647585027e-16,
+                    "velocityX": 2.318151314794784,
+                    "velocityY": -2.9894494876003863,
+                    "timestamp": 0.9450253677430492
+                },
+                {
+                    "x": 2.7507288455963135,
+                    "y": 3.0107133388519287,
+                    "heading": 3.14,
+                    "angularVelocity": -3.52994704864956e-16,
+                    "velocityX": 2.541705829014059,
+                    "velocityY": -2.801850242022933,
+                    "timestamp": 0.9800263072890881
+                },
+                {
+                    "x": 2.9215337593923705,
+                    "y": 2.8658352980219064,
+                    "heading": 3.14,
+                    "angularVelocity": 1.5913396610629706e-16,
+                    "velocityX": 2.884920936848367,
+                    "velocityY": -2.4470121145413266,
+                    "timestamp": 1.0392324060123015
+                },
+                {
+                    "x": 3.109750180151373,
+                    "y": 2.744432888397421,
+                    "heading": 3.14,
+                    "angularVelocity": -6.286818756935585e-16,
+                    "velocityX": 3.179003934013425,
+                    "velocityY": -2.0505051378666,
+                    "timestamp": 1.098438504735515
+                },
+                {
+                    "x": 3.3097585780303125,
+                    "y": 2.6436330516151556,
+                    "heading": 3.14,
+                    "angularVelocity": -3.4341251732407706e-17,
+                    "velocityX": 3.3781722186064402,
+                    "velocityY": -1.702524553319067,
+                    "timestamp": 1.1576446034587284
+                },
+                {
+                    "x": 3.5097675494852,
+                    "y": 2.5428343529427293,
+                    "heading": 3.14,
+                    "angularVelocity": -2.2516273301091217e-16,
+                    "velocityX": 3.3781819063939547,
+                    "velocityY": -1.7025053304654465,
+                    "timestamp": 1.216850702181942
+                },
+                {
+                    "x": 3.709776520948374,
+                    "y": 2.442035654287211,
+                    "heading": 3.14,
+                    "angularVelocity": -3.629334864774022e-16,
+                    "velocityX": 3.378181906533671,
+                    "velocityY": -1.7025053301803685,
+                    "timestamp": 1.2760568009051554
+                },
+                {
+                    "x": 3.909785492411555,
+                    "y": 2.3412369556316897,
+                    "heading": 3.14,
+                    "angularVelocity": -2.39059813931354e-16,
+                    "velocityX": 3.3781819065337895,
+                    "velocityY": -1.7025053301803958,
+                    "timestamp": 1.3352628996283689
+                },
+                {
+                    "x": 4.109794463874735,
+                    "y": 2.240438256976168,
+                    "heading": 3.14,
+                    "angularVelocity": 3.7738636092274995e-16,
+                    "velocityX": 3.378181906533792,
+                    "velocityY": -1.7025053301803967,
+                    "timestamp": 1.3944689983515823
+                },
+                {
+                    "x": 4.3098034353379155,
+                    "y": 2.139639558320647,
+                    "heading": 3.14,
+                    "angularVelocity": 3.725508803000677e-16,
+                    "velocityX": 3.3781819065337926,
+                    "velocityY": -1.7025053301803965,
+                    "timestamp": 1.4536750970747958
+                },
+                {
+                    "x": 4.509812406801097,
+                    "y": 2.0388408596651253,
+                    "heading": 3.14,
+                    "angularVelocity": -2.2963176234131617e-16,
+                    "velocityX": 3.3781819065337926,
+                    "velocityY": -1.7025053301803967,
+                    "timestamp": 1.5128811957980093
+                },
+                {
+                    "x": 4.709821378264278,
+                    "y": 1.9380421610096037,
+                    "heading": 3.14,
+                    "angularVelocity": -2.0546569648664528e-16,
+                    "velocityX": 3.3781819065338015,
+                    "velocityY": -1.702505330180399,
+                    "timestamp": 1.5720872945212228
+                },
+                {
+                    "x": 4.9098303497278835,
+                    "y": 1.8372434623549267,
+                    "heading": 3.14,
+                    "angularVelocity": 1.0642243284106308e-16,
+                    "velocityX": 3.3781819065409677,
+                    "velocityY": -1.7025053301661326,
+                    "timestamp": 1.6312933932444362
+                },
+                {
+                    "x": 5.109839349236117,
+                    "y": 1.736444819347842,
+                    "heading": 3.14,
+                    "angularVelocity": 1.2679224790319894e-16,
+                    "velocityX": 3.378182380218786,
+                    "velocityY": -1.7025043902702677,
+                    "timestamp": 1.6904994919676497
+                },
+                {
+                    "x": 5.311619758605957,
+                    "y": 1.6392407417297363,
+                    "heading": 3.14,
+                    "angularVelocity": 3.145701878302232e-16,
+                    "velocityX": 3.408101761838499,
+                    "velocityY": -1.6417916348765778,
+                    "timestamp": 1.7497055906908632
+                },
+                {
+                    "x": 5.430192834880249,
+                    "y": 1.5926107881863976,
+                    "heading": 3.14,
+                    "angularVelocity": 2.0229336550458753e-17,
+                    "velocityX": 3.5204949744296554,
+                    "velocityY": -1.384467049902731,
+                    "timestamp": 1.78338638760593
+                },
+                {
+                    "x": 5.550146685206584,
+                    "y": 1.5496573158737061,
+                    "heading": 3.14,
+                    "angularVelocity": -2.618637002687426e-16,
+                    "velocityX": 3.561490858687975,
+                    "velocityY": -1.2753104512640405,
+                    "timestamp": 1.8170671845209967
+                },
+                {
+                    "x": 5.670100671705041,
+                    "y": 1.506704223843596,
+                    "heading": 3.14,
+                    "angularVelocity": -1.632751475559818e-17,
+                    "velocityX": 3.5614949017075617,
+                    "velocityY": -1.2752991604806918,
+                    "timestamp": 1.8507479814360634
+                },
+                {
+                    "x": 5.790054658213967,
+                    "y": 1.4637511318444056,
+                    "heading": 3.14,
+                    "angularVelocity": -2.9492764372093784e-16,
+                    "velocityX": 3.561494902018399,
+                    "velocityY": -1.2752991595627796,
+                    "timestamp": 1.8844287783511302
+                },
+                {
+                    "x": 5.910008644722922,
+                    "y": 1.4207980398452127,
+                    "heading": 3.14,
+                    "angularVelocity": -2.6299005747305263e-17,
+                    "velocityX": 3.5614949020192235,
+                    "velocityY": -1.2752991595628407,
+                    "timestamp": 1.918109575266197
+                },
+                {
+                    "x": 6.02996263123188,
+                    "y": 1.3778449478460184,
+                    "heading": 3.14,
+                    "angularVelocity": -3.0549772131007975e-16,
+                    "velocityX": 3.5614949020193487,
+                    "velocityY": -1.2752991595628858,
+                    "timestamp": 1.9517903721812637
+                },
+                {
+                    "x": 6.149916617740839,
+                    "y": 1.3348918558468237,
+                    "heading": 3.14,
+                    "angularVelocity": 3.2741509193878177e-16,
+                    "velocityX": 3.56149490201936,
+                    "velocityY": -1.27529915956289,
+                    "timestamp": 1.9854711690963305
+                },
+                {
+                    "x": 6.269870604249798,
+                    "y": 1.2919387638476292,
+                    "heading": 3.14,
+                    "angularVelocity": 3.4990315690965786e-16,
+                    "velocityX": 3.5614949020193607,
+                    "velocityY": -1.2752991595628904,
+                    "timestamp": 2.019151966011397
+                },
+                {
+                    "x": 6.3898245907587565,
+                    "y": 1.2489856718484345,
+                    "heading": 3.14,
+                    "angularVelocity": 2.004607715018742e-16,
+                    "velocityX": 3.5614949020193603,
+                    "velocityY": -1.27529915956289,
+                    "timestamp": 2.0528327629264638
+                },
+                {
+                    "x": 6.509778577267715,
+                    "y": 1.2060325798492397,
+                    "heading": 3.14,
+                    "angularVelocity": 3.130071946862803e-16,
+                    "velocityX": 3.5614949020193603,
+                    "velocityY": -1.2752991595628902,
+                    "timestamp": 2.0865135598415305
+                },
+                {
+                    "x": 6.629732563776674,
+                    "y": 1.1630794878500452,
+                    "heading": 3.14,
+                    "angularVelocity": -2.481040891393771e-16,
+                    "velocityX": 3.5614949020193607,
+                    "velocityY": -1.2752991595628902,
+                    "timestamp": 2.1201943567565973
+                },
+                {
+                    "x": 6.749686550285633,
+                    "y": 1.1201263958508503,
+                    "heading": 3.14,
+                    "angularVelocity": 5.765266318677121e-16,
+                    "velocityX": 3.561494902019357,
+                    "velocityY": -1.2752991595628989,
+                    "timestamp": 2.153875153671664
+                },
+                {
+                    "x": 6.869640536794461,
+                    "y": 1.077173303851719,
+                    "heading": 3.14,
+                    "angularVelocity": 5.94200130731494e-16,
+                    "velocityX": 3.5614949020154905,
+                    "velocityY": -1.2752991595610093,
+                    "timestamp": 2.187555950586731
+                },
+                {
+                    "x": 6.985406128017608,
+                    "y": 1.0357199913369377,
+                    "heading": 3.14,
+                    "angularVelocity": -5.216843428237177e-16,
+                    "velocityX": 3.437139314579594,
+                    "velocityY": -1.2307699434580792,
+                    "timestamp": 2.2212367475017976
+                },
+                {
+                    "x": 7.092266700241795,
+                    "y": 0.9974553856875887,
+                    "heading": 3.14,
+                    "angularVelocity": -4.51325473252053e-16,
+                    "velocityX": 3.172744768885923,
+                    "velocityY": -1.1360956139440452,
+                    "timestamp": 2.2549175444168643
+                },
+                {
+                    "x": 7.190222235939472,
+                    "y": 0.9623794931799317,
+                    "heading": 3.14,
+                    "angularVelocity": -3.932803198593372e-16,
+                    "velocityX": 2.908349702790401,
+                    "velocityY": -1.0414210980846788,
+                    "timestamp": 2.288598341331931
+                },
+                {
+                    "x": 7.279272729268112,
+                    "y": 0.9304923159060571,
+                    "heading": 3.14,
+                    "angularVelocity": -3.6674368533996665e-16,
+                    "velocityX": 2.643954463227235,
+                    "velocityY": -0.9467465201100715,
+                    "timestamp": 2.322279138246998
+                },
+                {
+                    "x": 7.35941817730645,
+                    "y": 0.9017938549120107,
+                    "heading": 3.14,
+                    "angularVelocity": -3.61350987503525e-16,
+                    "velocityX": 2.3795591369302214,
+                    "velocityY": -0.8520719110778341,
+                    "timestamp": 2.3559599351620646
+                },
+                {
+                    "x": 7.430658578301727,
+                    "y": 0.8762841108254199,
+                    "heading": 3.14,
+                    "angularVelocity": -3.11296740830968e-16,
+                    "velocityX": 2.115163758592898,
+                    "velocityY": -0.7573972834110186,
+                    "timestamp": 2.3896407320771313
+                },
+                {
+                    "x": 7.492993931085436,
+                    "y": 0.8539630840647031,
+                    "heading": 3.14,
+                    "angularVelocity": -3.0797715285651212e-16,
+                    "velocityX": 1.8507683455620387,
+                    "velocityY": -0.6627226433211519,
+                    "timestamp": 2.423321528992198
+                },
+                {
+                    "x": 7.546424234822929,
+                    "y": 0.8348307749287305,
+                    "heading": 3.14,
+                    "angularVelocity": -2.384214973522538e-16,
+                    "velocityX": 1.5863729077500837,
+                    "velocityY": -0.5680479943576783,
+                    "timestamp": 2.457002325907265
+                },
+                {
+                    "x": 7.590949488888224,
+                    "y": 0.8188871836416546,
+                    "heading": 3.14,
+                    "angularVelocity": -1.6873896303460275e-16,
+                    "velocityX": 1.3219774513523088,
+                    "velocityY": -0.4733733387389998,
+                    "timestamp": 2.4906831228223316
+                },
+                {
+                    "x": 7.626569692794441,
+                    "y": 0.8061323103778162,
+                    "heading": 3.14,
+                    "angularVelocity": -1.2706181170586285e-16,
+                    "velocityX": 1.0575819804988977,
+                    "velocityY": -0.3786986779440515,
+                    "timestamp": 2.5243639197373984
+                },
+                {
+                    "x": 7.653284846152079,
+                    "y": 0.7965661552766882,
+                    "heading": 3.14,
+                    "angularVelocity": -1.0566459548829111e-16,
+                    "velocityX": 0.7931864980809785,
+                    "velocityY": -0.28402401300808755,
+                    "timestamp": 2.558044716652465
+                },
+                {
+                    "x": 7.671094948642455,
+                    "y": 0.7901887184523844,
+                    "heading": 3.14,
+                    "angularVelocity": -6.428789472113484e-17,
+                    "velocityX": 0.5287910062011897,
+                    "velocityY": -0.18934934468402026,
+                    "timestamp": 2.591725513567532
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": -8.910267594003032e-17,
+                    "velocityX": 0.26439550643651,
+                    "velocityY": -0.09467467353653364,
+                    "timestamp": 2.6254063104825986
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": -3.385083394775927e-32,
+                    "velocityX": 1.13191163707898e-33,
+                    "velocityY": 3.1068609037049197e-32,
+                    "timestamp": 2.6590871073976654
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -2364,23 +5929,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "Stage Side To W1": {
             "waypoints": [
@@ -3015,7 +6564,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 30
+                    "controlIntervalCount": 25
+                },
+                {
+                    "x": 3.69740891456604,
+                    "y": 6.1177659034729,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 32
                 },
                 {
                     "x": 7.68,
@@ -3027,7 +6585,530 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 0.71,
+                    "y": 4.36,
+                    "heading": 2.1,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 3.1166835341708484e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 0.7240863087605185,
+                    "y": 4.368337736173442,
+                    "heading": 2.1163001020338092,
+                    "angularVelocity": 0.3614478974621331,
+                    "velocityX": 0.31235796401343735,
+                    "velocityY": 0.1848857880296264,
+                    "timestamp": 0.045096685160595056
+                },
+                {
+                    "x": 0.7523050752299821,
+                    "y": 4.38505134365204,
+                    "heading": 2.148025504039376,
+                    "angularVelocity": 0.7034974276399452,
+                    "velocityX": 0.6257392615216195,
+                    "velocityY": 0.3706172065436647,
+                    "timestamp": 0.09019337032119011
+                },
+                {
+                    "x": 0.7947129681201482,
+                    "y": 4.410184705499472,
+                    "heading": 2.1940580950016866,
+                    "angularVelocity": 1.020753316976243,
+                    "velocityX": 0.9403771638457716,
+                    "velocityY": 0.5573217135124126,
+                    "timestamp": 0.13529005548178516
+                },
+                {
+                    "x": 0.8513807226870724,
+                    "y": 4.44378606946023,
+                    "heading": 2.252941060288208,
+                    "angularVelocity": 1.3057049554048448,
+                    "velocityX": 1.25658359068129,
+                    "velocityY": 0.7450960938946717,
+                    "timestamp": 0.18038674064238022
+                },
+                {
+                    "x": 0.922394851892513,
+                    "y": 4.485907452258627,
+                    "heading": 2.32274249990232,
+                    "angularVelocity": 1.5478175250695256,
+                    "velocityX": 1.5747084060070078,
+                    "velocityY": 0.9340239232306716,
+                    "timestamp": 0.22548342580297528
+                },
+                {
+                    "x": 1.0078549434585182,
+                    "y": 4.536606503403333,
+                    "heading": 2.4007952890388493,
+                    "angularVelocity": 1.7307877254963087,
+                    "velocityX": 1.8950415371256468,
+                    "velocityY": 1.1242301061410762,
+                    "timestamp": 0.2705801109635703
+                },
+                {
+                    "x": 1.1078580204781514,
+                    "y": 4.595947756684704,
+                    "heading": 2.4830211624583747,
+                    "angularVelocity": 1.823324111887792,
+                    "velocityX": 2.2175261144695955,
+                    "velocityY": 1.315867298672826,
+                    "timestamp": 0.3156767961241654
+                },
+                {
+                    "x": 1.2224032158634355,
+                    "y": 4.663956921963176,
+                    "heading": 2.561776708325237,
+                    "angularVelocity": 1.7463710600103812,
+                    "velocityX": 2.5399914645028585,
+                    "velocityY": 1.5080745965314675,
+                    "timestamp": 0.36077348128476044
+                },
+                {
+                    "x": 1.3501227544876644,
+                    "y": 4.740405284602277,
+                    "heading": 2.616849964946217,
+                    "angularVelocity": 1.2212262702869785,
+                    "velocityX": 2.8321269771692363,
+                    "velocityY": 1.695210243654476,
+                    "timestamp": 0.4058701664453555
+                },
+                {
+                    "x": 1.4890564849563437,
+                    "y": 4.825393782343559,
+                    "heading": 2.6364418869926514,
+                    "angularVelocity": 0.43444261982150123,
+                    "velocityX": 3.0807969582225083,
+                    "velocityY": 1.884584142684248,
+                    "timestamp": 0.45096685160595057
+                },
+                {
+                    "x": 1.6359707555642846,
+                    "y": 4.9121110990754175,
+                    "heading": 2.6364419350884667,
+                    "angularVelocity": 0.0000010665044506613552,
+                    "velocityX": 3.257762074634985,
+                    "velocityY": 1.9229199756711164,
+                    "timestamp": 0.49606353676654563
+                },
+                {
+                    "x": 1.7828851010176074,
+                    "y": 4.99882828900635,
+                    "heading": 2.63644198318385,
+                    "angularVelocity": 0.000001066494868779716,
+                    "velocityX": 3.257763734299801,
+                    "velocityY": 1.9229171639139742,
+                    "timestamp": 0.5411602219271406
+                },
+                {
+                    "x": 1.929799446473537,
+                    "y": 5.085545478932866,
+                    "heading": 2.6364420312792327,
+                    "angularVelocity": 0.0000010664948614258307,
+                    "velocityX": 3.257763734357611,
+                    "velocityY": 1.9229171638160338,
+                    "timestamp": 0.5862569070877357
+                },
+                {
+                    "x": 2.076713791929467,
+                    "y": 5.172262668859381,
+                    "heading": 2.636442079374616,
+                    "angularVelocity": 0.000001066494855048045,
+                    "velocityX": 3.257763734357614,
+                    "velocityY": 1.922917163816031,
+                    "timestamp": 0.6313535922483308
+                },
+                {
+                    "x": 2.2236281373853966,
+                    "y": 5.258979858785897,
+                    "heading": 2.6364421274699983,
+                    "angularVelocity": 0.000001066494848970006,
+                    "velocityX": 3.257763734357614,
+                    "velocityY": 1.9229171638160298,
+                    "timestamp": 0.6764502774089258
+                },
+                {
+                    "x": 2.3705424828413264,
+                    "y": 5.345697048712413,
+                    "heading": 2.6364421755653806,
+                    "angularVelocity": 0.0000010664948432023413,
+                    "velocityX": 3.257763734357615,
+                    "velocityY": 1.9229171638160298,
+                    "timestamp": 0.7215469625695209
+                },
+                {
+                    "x": 2.5174568282972563,
+                    "y": 5.432414238638929,
+                    "heading": 2.6364422236607625,
+                    "angularVelocity": 0.000001066494835980498,
+                    "velocityX": 3.2577637343576162,
+                    "velocityY": 1.9229171638160294,
+                    "timestamp": 0.766643647730116
+                },
+                {
+                    "x": 2.6643711737531857,
+                    "y": 5.519131428565444,
+                    "heading": 2.636442271756144,
+                    "angularVelocity": 0.0000010664948304508311,
+                    "velocityX": 3.2577637343576167,
+                    "velocityY": 1.9229171638160292,
+                    "timestamp": 0.811740332890711
+                },
+                {
+                    "x": 2.8112855192091155,
+                    "y": 5.60584861849196,
+                    "heading": 2.6364423198515254,
+                    "angularVelocity": 0.0000010664948245159241,
+                    "velocityX": 3.257763734357617,
+                    "velocityY": 1.922917163816029,
+                    "timestamp": 0.8568370180513061
+                },
+                {
+                    "x": 2.958199864665046,
+                    "y": 5.692565808418475,
+                    "heading": 2.6364423679469065,
+                    "angularVelocity": 0.0000010664948181966642,
+                    "velocityX": 3.2577637343576185,
+                    "velocityY": 1.922917163816028,
+                    "timestamp": 0.9019337032119011
+                },
+                {
+                    "x": 3.1051142101209757,
+                    "y": 5.779282998344991,
+                    "heading": 2.636442416042287,
+                    "angularVelocity": 0.0000010664948111950998,
+                    "velocityX": 3.257763734357619,
+                    "velocityY": 1.9229171638160278,
+                    "timestamp": 0.9470303883724962
+                },
+                {
+                    "x": 3.252028555576906,
+                    "y": 5.8660001882715065,
+                    "heading": 2.6364424641376676,
+                    "angularVelocity": 0.0000010664948058295197,
+                    "velocityX": 3.2577637343576273,
+                    "velocityY": 1.922917163816015,
+                    "timestamp": 0.9921270735330913
+                },
+                {
+                    "x": 3.3989429010423815,
+                    "y": 5.952717378181851,
+                    "heading": 2.636442512233048,
+                    "angularVelocity": 0.00000106649479979568,
+                    "velocityX": 3.257763734569281,
+                    "velocityY": 1.9229171634574358,
+                    "timestamp": 1.0372237586936863
+                },
+                {
+                    "x": 3.5458575205338394,
+                    "y": 6.039434103841755,
+                    "heading": 2.636442560328411,
+                    "angularVelocity": 0.0000010664944172141213,
+                    "velocityX": 3.2577698109800433,
+                    "velocityY": 1.9229068688994477,
+                    "timestamp": 1.0823204438542813
+                },
+                {
+                    "x": 3.69740891456604,
+                    "y": 6.1177659034729,
+                    "heading": 2.6364426084288106,
+                    "angularVelocity": 0.0000010666061013072793,
+                    "velocityX": 3.3605883335439555,
+                    "velocityY": 1.736974665703161,
+                    "timestamp": 1.1274171290148762
+                },
+                {
+                    "x": 3.8471798789241873,
+                    "y": 6.1780975155460816,
+                    "heading": 2.636442653750165,
+                    "angularVelocity": 0.0000010618213127102396,
+                    "velocityX": 3.5089419861830207,
+                    "velocityY": 1.4134924456482445,
+                    "timestamp": 1.1700997851966806
+                },
+                {
+                    "x": 4.000192250358795,
+                    "y": 6.2296577661788755,
+                    "heading": 2.6364426985309986,
+                    "angularVelocity": 0.0000010491576146038598,
+                    "velocityX": 3.584884004942437,
+                    "velocityY": 1.2079906745535305,
+                    "timestamp": 1.212782441378485
+                },
+                {
+                    "x": 4.153204837360183,
+                    "y": 6.281217377082217,
+                    "heading": 2.6364427433118154,
+                    "angularVelocity": 0.0000010491572166464704,
+                    "velocityX": 3.5848890553961605,
+                    "velocityY": 1.2079756865112976,
+                    "timestamp": 1.2554650975602892
+                },
+                {
+                    "x": 4.306217424370213,
+                    "y": 6.3327769879599085,
+                    "heading": 2.636442788092632,
+                    "angularVelocity": 0.0000010491572158103502,
+                    "velocityX": 3.5848890555986537,
+                    "velocityY": 1.2079756859103623,
+                    "timestamp": 1.2981477537420936
+                },
+                {
+                    "x": 4.459230011380243,
+                    "y": 6.384336598837598,
+                    "heading": 2.6364428328734486,
+                    "angularVelocity": 0.0000010491572148390834,
+                    "velocityX": 3.584889055598662,
+                    "velocityY": 1.2079756859103368,
+                    "timestamp": 1.340830409923898
+                },
+                {
+                    "x": 4.6122425983902735,
+                    "y": 6.435896209715288,
+                    "heading": 2.636442877654265,
+                    "angularVelocity": 0.0000010491572125659184,
+                    "velocityX": 3.584889055598663,
+                    "velocityY": 1.2079756859103368,
+                    "timestamp": 1.3835130661057022
+                },
+                {
+                    "x": 4.765255185400304,
+                    "y": 6.487455820592977,
+                    "heading": 2.636442922435082,
+                    "angularVelocity": 0.0000010491572116366236,
+                    "velocityX": 3.584889055598663,
+                    "velocityY": 1.2079756859103352,
+                    "timestamp": 1.4261957222875066
+                },
+                {
+                    "x": 4.918267772410334,
+                    "y": 6.539015431470667,
+                    "heading": 2.636442967215898,
+                    "angularVelocity": 0.0000010491572113687884,
+                    "velocityX": 3.584889055598663,
+                    "velocityY": 1.2079756859103348,
+                    "timestamp": 1.468878378469311
+                },
+                {
+                    "x": 5.071280359420366,
+                    "y": 6.590575042348357,
+                    "heading": 2.6364430119967146,
+                    "angularVelocity": 0.0000010491572106337029,
+                    "velocityX": 3.584889055598664,
+                    "velocityY": 1.2079756859103339,
+                    "timestamp": 1.5115610346511152
+                },
+                {
+                    "x": 5.224292946430397,
+                    "y": 6.642134653226047,
+                    "heading": 2.6364430567775314,
+                    "angularVelocity": 0.0000010491572087267192,
+                    "velocityX": 3.584889055598664,
+                    "velocityY": 1.2079756859103334,
+                    "timestamp": 1.5542436908329196
+                },
+                {
+                    "x": 5.3773055334404285,
+                    "y": 6.693694264103736,
+                    "heading": 2.636443101558348,
+                    "angularVelocity": 0.0000010491572072659829,
+                    "velocityX": 3.5848890555986643,
+                    "velocityY": 1.2079756859103326,
+                    "timestamp": 1.596926347014724
+                },
+                {
+                    "x": 5.53031812045046,
+                    "y": 6.745253874981426,
+                    "heading": 2.6364431463391638,
+                    "angularVelocity": 0.0000010491572066940514,
+                    "velocityX": 3.5848890555986643,
+                    "velocityY": 1.2079756859103314,
+                    "timestamp": 1.6396090031965282
+                },
+                {
+                    "x": 5.683330707460491,
+                    "y": 6.796813485859116,
+                    "heading": 2.63644319111998,
+                    "angularVelocity": 0.000001049157205644206,
+                    "velocityX": 3.5848890555986648,
+                    "velocityY": 1.2079756859103312,
+                    "timestamp": 1.6822916593783326
+                },
+                {
+                    "x": 5.836343294470522,
+                    "y": 6.848373096736806,
+                    "heading": 2.636443235900796,
+                    "angularVelocity": 0.0000010491572052803454,
+                    "velocityX": 3.5848890555986657,
+                    "velocityY": 1.2079756859103297,
+                    "timestamp": 1.724974315560137
+                },
+                {
+                    "x": 5.989355881480554,
+                    "y": 6.899932707614496,
+                    "heading": 2.6364432806816125,
+                    "angularVelocity": 0.0000010491572041084738,
+                    "velocityX": 3.5848890555986657,
+                    "velocityY": 1.2079756859103294,
+                    "timestamp": 1.7676569717419413
+                },
+                {
+                    "x": 6.142368468490585,
+                    "y": 6.951492318492185,
+                    "heading": 2.6364433254624284,
+                    "angularVelocity": 0.0000010491572019949161,
+                    "velocityX": 3.584889055598666,
+                    "velocityY": 1.2079756859103283,
+                    "timestamp": 1.8103396279237456
+                },
+                {
+                    "x": 6.295381055500616,
+                    "y": 7.0030519293698745,
+                    "heading": 2.6364433702432444,
+                    "angularVelocity": 0.0000010491572021090905,
+                    "velocityX": 3.584889055598666,
+                    "velocityY": 1.2079756859103277,
+                    "timestamp": 1.85302228410555
+                },
+                {
+                    "x": 6.448393642510648,
+                    "y": 7.054611540247564,
+                    "heading": 2.6364434150240608,
+                    "angularVelocity": 0.0000010491572009028897,
+                    "velocityX": 3.584889055598666,
+                    "velocityY": 1.2079756859103268,
+                    "timestamp": 1.8957049402873543
+                },
+                {
+                    "x": 6.601406229520499,
+                    "y": 7.106171151125787,
+                    "heading": 2.6364434598048767,
+                    "angularVelocity": 0.0000010491571991066208,
+                    "velocityX": 3.58488905559446,
+                    "velocityY": 1.2079756859228103,
+                    "timestamp": 1.9383875964691586
+                },
+                {
+                    "x": 6.75441881205228,
+                    "y": 7.157730775293412,
+                    "heading": 2.636443504585761,
+                    "angularVelocity": 0.0000010491588061157563,
+                    "velocityX": 3.5848889506789767,
+                    "velocityY": 1.207975997276489,
+                    "timestamp": 1.981070252650963
+                },
+                {
+                    "x": 6.905487720036347,
+                    "y": 7.20899283211694,
+                    "heading": 2.6411872457126164,
+                    "angularVelocity": 0.11113978255358876,
+                    "velocityX": 3.539351143954066,
+                    "velocityY": 1.2010043752942743,
+                    "timestamp": 2.0237529088327673
+                },
+                {
+                    "x": 7.045499588568849,
+                    "y": 7.254368377570732,
+                    "heading": 2.676148381759387,
+                    "angularVelocity": 0.81909466687958,
+                    "velocityX": 3.2802988627542176,
+                    "velocityY": 1.0630909487103528,
+                    "timestamp": 2.0664355650145714
+                },
+                {
+                    "x": 7.172970608020284,
+                    "y": 7.297222729151116,
+                    "heading": 2.7400991643229964,
+                    "angularVelocity": 1.4982849776549703,
+                    "velocityX": 2.9864828212302403,
+                    "velocityY": 1.0040226034164315,
+                    "timestamp": 2.1091182211963755
+                },
+                {
+                    "x": 7.286231301322222,
+                    "y": 7.335261893158222,
+                    "heading": 2.8124985936039937,
+                    "angularVelocity": 1.6962259558687312,
+                    "velocityX": 2.6535530689447047,
+                    "velocityY": 0.8912089220755359,
+                    "timestamp": 2.1518008773781796
+                },
+                {
+                    "x": 7.3851039731372365,
+                    "y": 7.3684303034498875,
+                    "heading": 2.8845195362089706,
+                    "angularVelocity": 1.6873584975173053,
+                    "velocityX": 2.316460142355517,
+                    "velocityY": 0.7770933971491014,
+                    "timestamp": 2.1944835335599837
+                },
+                {
+                    "x": 7.46964567597808,
+                    "y": 7.396754020582906,
+                    "heading": 2.951617256319432,
+                    "angularVelocity": 1.5720136962578515,
+                    "velocityX": 1.9807038831122041,
+                    "velocityY": 0.6635884377105193,
+                    "timestamp": 2.237166189741788
+                },
+                {
+                    "x": 7.539938623640353,
+                    "y": 7.420270719849242,
+                    "heading": 3.0110324789568197,
+                    "angularVelocity": 1.3920226141576573,
+                    "velocityX": 1.6468737878651336,
+                    "velocityY": 0.5509661621378036,
+                    "timestamp": 2.279848845923592
+                },
+                {
+                    "x": 7.596058846559568,
+                    "y": 7.439019303920545,
+                    "heading": 3.060856486484029,
+                    "angularVelocity": 1.1673127210028025,
+                    "velocityX": 1.3148249884022007,
+                    "velocityY": 0.4392553263659099,
+                    "timestamp": 2.322531502105396
+                },
+                {
+                    "x": 7.638072702511279,
+                    "y": 7.453036150115233,
+                    "heading": 3.09965963024843,
+                    "angularVelocity": 0.9091079898851933,
+                    "velocityX": 0.9843308666816342,
+                    "velocityY": 0.32839676460117173,
+                    "timestamp": 2.3652141582872
+                },
+                {
+                    "x": 7.666037142481598,
+                    "y": 7.462353623790155,
+                    "heading": 3.126330643718343,
+                    "angularVelocity": 0.6248677063655441,
+                    "velocityX": 0.6551710336677993,
+                    "velocityY": 0.2182964817192845,
+                    "timestamp": 2.4078968144690043
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "angularVelocity": 0.32025552073032726,
+                    "velocityX": 0.3271318790219412,
+                    "velocityY": 0.10885864717634237,
+                    "timestamp": 2.4505794706508084
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": -9.011054390829982e-34,
+                    "velocityY": 6.746182534290948e-34,
+                    "timestamp": 2.4932621268326125
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -3040,35 +7121,12 @@
                         "last"
                     ],
                     "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        0,
-                        1
-                    ],
-                    "type": "StraightLine"
                 }
             ],
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "Stage Side To C2": {
             "waypoints": [
@@ -3079,7 +7137,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 29
+                    "controlIntervalCount": 30
+                },
+                {
+                    "x": 5.7195634841918945,
+                    "y": 6.649363040924072,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 26
                 },
                 {
                     "x": 7.68,
@@ -3091,7 +7158,521 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 0.71,
+                    "y": 4.36,
+                    "heading": 2.1,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 1.2156956395995823e-32,
+                    "timestamp": 0
+                },
+                {
+                    "x": 0.7323642464210136,
+                    "y": 4.371312176942183,
+                    "heading": 2.122836377232617,
+                    "angularVelocity": 0.41032613171248355,
+                    "velocityX": 0.4018428417574259,
+                    "velocityY": 0.20325823832090237,
+                    "timestamp": 0.05565421129117423
+                },
+                {
+                    "x": 0.7771770070003539,
+                    "y": 4.393987109637107,
+                    "heading": 2.1669272728918627,
+                    "angularVelocity": 0.7922292785458643,
+                    "velocityX": 0.8051998139886801,
+                    "velocityY": 0.40742528137344314,
+                    "timestamp": 0.11130842258234847
+                },
+                {
+                    "x": 0.8445465122121998,
+                    "y": 4.4280825394621655,
+                    "heading": 2.2301328847833437,
+                    "angularVelocity": 1.135684262252843,
+                    "velocityX": 1.2105014813592279,
+                    "velocityY": 0.612629826818977,
+                    "timestamp": 0.1669626338735227
+                },
+                {
+                    "x": 0.9346100694611125,
+                    "y": 4.4736618275321725,
+                    "heading": 2.3094955096415575,
+                    "angularVelocity": 1.4259949609743032,
+                    "velocityX": 1.6182703008351718,
+                    "velocityY": 0.8189728506175846,
+                    "timestamp": 0.22261684516469693
+                },
+                {
+                    "x": 1.047532617883247,
+                    "y": 4.530798800054343,
+                    "heading": 2.4006762269852384,
+                    "angularVelocity": 1.638343536424179,
+                    "velocityX": 2.0290027619175355,
+                    "velocityY": 1.0266423905144981,
+                    "timestamp": 0.27827105645587114
+                },
+                {
+                    "x": 1.1834754046687317,
+                    "y": 4.5995806121594685,
+                    "heading": 2.4961863960775177,
+                    "angularVelocity": 1.7161355246341592,
+                    "velocityX": 2.4426325273796983,
+                    "velocityY": 1.2358779418375128,
+                    "timestamp": 0.33392526774704534
+                },
+                {
+                    "x": 1.342193634237211,
+                    "y": 4.6799334714255165,
+                    "heading": 2.578155037608781,
+                    "angularVelocity": 1.4728201088398516,
+                    "velocityX": 2.851863783282992,
+                    "velocityY": 1.4437875841174588,
+                    "timestamp": 0.38957947903821954
+                },
+                {
+                    "x": 1.518721607083693,
+                    "y": 4.772275835878884,
+                    "heading": 2.6065642311000707,
+                    "angularVelocity": 0.5104590080821917,
+                    "velocityX": 3.17187089262509,
+                    "velocityY": 1.6592161187991088,
+                    "timestamp": 0.44523369032939375
+                },
+                {
+                    "x": 1.7067559979094145,
+                    "y": 4.866979097734677,
+                    "heading": 2.6065642752111695,
+                    "angularVelocity": 7.925922929779288e-7,
+                    "velocityX": 3.378619271809538,
+                    "velocityY": 1.7016369410091843,
+                    "timestamp": 0.500887901620568
+                },
+                {
+                    "x": 1.894790428298913,
+                    "y": 4.9616822810363175,
+                    "heading": 2.6065643193220347,
+                    "angularVelocity": 7.92588092448351e-7,
+                    "velocityX": 3.378619982695133,
+                    "velocityY": 1.7016355295409376,
+                    "timestamp": 0.5565421129117422
+                },
+                {
+                    "x": 2.082824858689142,
+                    "y": 5.056385464336507,
+                    "heading": 2.6065643634329,
+                    "angularVelocity": 7.925880881331428e-7,
+                    "velocityX": 3.378619982708264,
+                    "velocityY": 1.7016355295148662,
+                    "timestamp": 0.6121963242029164
+                },
+                {
+                    "x": 2.270859289079371,
+                    "y": 5.151088647636696,
+                    "heading": 2.606564407543764,
+                    "angularVelocity": 7.925880850764007e-7,
+                    "velocityX": 3.3786199827082646,
+                    "velocityY": 1.7016355295148653,
+                    "timestamp": 0.6678505354940906
+                },
+                {
+                    "x": 2.4588937194696,
+                    "y": 5.245791830936885,
+                    "heading": 2.606564451654629,
+                    "angularVelocity": 7.92588082043664e-7,
+                    "velocityX": 3.378619982708265,
+                    "velocityY": 1.7016355295148649,
+                    "timestamp": 0.7235047467852648
+                },
+                {
+                    "x": 2.6469281498598294,
+                    "y": 5.340495014237074,
+                    "heading": 2.6065644957654936,
+                    "angularVelocity": 7.92588079438066e-7,
+                    "velocityX": 3.378619982708265,
+                    "velocityY": 1.7016355295148646,
+                    "timestamp": 0.779158958076439
+                },
+                {
+                    "x": 2.8349625802500587,
+                    "y": 5.435198197537264,
+                    "heading": 2.606564539876358,
+                    "angularVelocity": 7.925880759204128e-7,
+                    "velocityX": 3.3786199827082655,
+                    "velocityY": 1.7016355295148642,
+                    "timestamp": 0.8348131693676132
+                },
+                {
+                    "x": 3.022997010640288,
+                    "y": 5.529901380837453,
+                    "heading": 2.606564583987222,
+                    "angularVelocity": 7.925880718883517e-7,
+                    "velocityX": 3.3786199827082664,
+                    "velocityY": 1.7016355295148637,
+                    "timestamp": 0.8904673806587874
+                },
+                {
+                    "x": 3.2110314410305167,
+                    "y": 5.624604564137642,
+                    "heading": 2.6065646280980856,
+                    "angularVelocity": 7.925880686816096e-7,
+                    "velocityX": 3.3786199827082664,
+                    "velocityY": 1.7016355295148633,
+                    "timestamp": 0.9461215919499616
+                },
+                {
+                    "x": 3.399065871420746,
+                    "y": 5.719307747437831,
+                    "heading": 2.6065646722089495,
+                    "angularVelocity": 7.92588065491887e-7,
+                    "velocityX": 3.378619982708267,
+                    "velocityY": 1.7016355295148633,
+                    "timestamp": 1.001775803241136
+                },
+                {
+                    "x": 3.5871003018109753,
+                    "y": 5.8140109307380206,
+                    "heading": 2.606564716319813,
+                    "angularVelocity": 7.92588062190758e-7,
+                    "velocityX": 3.378619982708267,
+                    "velocityY": 1.7016355295148633,
+                    "timestamp": 1.0574300145323101
+                },
+                {
+                    "x": 3.7751347322012045,
+                    "y": 5.90871411403821,
+                    "heading": 2.6065647604306763,
+                    "angularVelocity": 7.92588058976371e-7,
+                    "velocityX": 3.378619982708267,
+                    "velocityY": 1.7016355295148629,
+                    "timestamp": 1.1130842258234843
+                },
+                {
+                    "x": 3.963169162591434,
+                    "y": 6.003417297338399,
+                    "heading": 2.6065648045415397,
+                    "angularVelocity": 7.925880551764629e-7,
+                    "velocityX": 3.3786199827082677,
+                    "velocityY": 1.7016355295148622,
+                    "timestamp": 1.1687384371146585
+                },
+                {
+                    "x": 4.151203592981663,
+                    "y": 6.098120480638587,
+                    "heading": 2.6065648486524027,
+                    "angularVelocity": 7.925880525134244e-7,
+                    "velocityX": 3.378619982708268,
+                    "velocityY": 1.7016355295148615,
+                    "timestamp": 1.2243926484058327
+                },
+                {
+                    "x": 4.339238023371892,
+                    "y": 6.192823663938777,
+                    "heading": 2.606564892763265,
+                    "angularVelocity": 7.925880485176538e-7,
+                    "velocityX": 3.3786199827082686,
+                    "velocityY": 1.701635529514862,
+                    "timestamp": 1.280046859697007
+                },
+                {
+                    "x": 4.527272453762121,
+                    "y": 6.287526847238966,
+                    "heading": 2.6065649368741277,
+                    "angularVelocity": 7.925880449736749e-7,
+                    "velocityX": 3.37861998270827,
+                    "velocityY": 1.7016355295148586,
+                    "timestamp": 1.3357010709881811
+                },
+                {
+                    "x": 4.715306884156566,
+                    "y": 6.382230030530785,
+                    "heading": 2.6065649809849902,
+                    "angularVelocity": 7.925880433141827e-7,
+                    "velocityX": 3.3786199827840138,
+                    "velocityY": 1.7016355293644703,
+                    "timestamp": 1.3913552822793553
+                },
+                {
+                    "x": 4.903341542764552,
+                    "y": 6.476932760700215,
+                    "heading": 2.6065650250958465,
+                    "angularVelocity": 7.925879324126063e-7,
+                    "velocityX": 3.378624083345972,
+                    "velocityY": 1.7016273876195969,
+                    "timestamp": 1.4470094935705295
+                },
+                {
+                    "x": 5.097883205540706,
+                    "y": 6.557426122164244,
+                    "heading": 2.6065650695055442,
+                    "angularVelocity": 7.97957544404275e-7,
+                    "velocityX": 3.495542534208987,
+                    "velocityY": 1.4463121405656068,
+                    "timestamp": 1.5026637048617038
+                },
+                {
+                    "x": 5.300816520591001,
+                    "y": 6.613494907557912,
+                    "heading": 2.6065651167952333,
+                    "angularVelocity": 8.497055004082428e-7,
+                    "velocityX": 3.6463245160115534,
+                    "velocityY": 1.0074491056988386,
+                    "timestamp": 1.558317916152878
+                },
+                {
+                    "x": 5.5090879488774736,
+                    "y": 6.644295063089685,
+                    "heading": 2.606565171305544,
+                    "angularVelocity": 9.79446290291877e-7,
+                    "velocityX": 3.742240226832587,
+                    "velocityY": 0.5534200344809682,
+                    "timestamp": 1.6139721274440522
+                },
+                {
+                    "x": 5.7195634841918945,
+                    "y": 6.649363040924072,
+                    "heading": 2.6065652359371767,
+                    "angularVelocity": 0.0000011613071303259608,
+                    "velocityX": 3.781843825137414,
+                    "velocityY": 0.0910618930142201,
+                    "timestamp": 1.6696263387352264
+                },
+                {
+                    "x": 5.838739218913105,
+                    "y": 6.643942809110191,
+                    "heading": 2.606565305952835,
+                    "angularVelocity": 0.0000022201789880280334,
+                    "velocityX": 3.77903269949585,
+                    "velocityY": -0.1718741932778599,
+                    "timestamp": 1.7011623809444163
+                },
+                {
+                    "x": 5.957250522969574,
+                    "y": 6.630256808888294,
+                    "heading": 2.606565367221186,
+                    "angularVelocity": 0.00000194280407444938,
+                    "velocityX": 3.7579637695288737,
+                    "velocityY": -0.4339796392683931,
+                    "timestamp": 1.7326984231536062
+                },
+                {
+                    "x": 6.074524798614125,
+                    "y": 6.608371177242311,
+                    "heading": 2.60656542248837,
+                    "angularVelocity": 0.000001752508552376974,
+                    "velocityX": 3.718737908410616,
+                    "velocityY": -0.693987898063049,
+                    "timestamp": 1.7642344653627962
+                },
+                {
+                    "x": 6.189995430266423,
+                    "y": 6.578391677512442,
+                    "heading": 2.606565473629249,
+                    "angularVelocity": 0.0000016216644704538906,
+                    "velocityX": 3.6615448091532925,
+                    "velocityY": -0.9506424278292124,
+                    "timestamp": 1.795770507571986
+                },
+                {
+                    "x": 6.303104529085719,
+                    "y": 6.5404632008330585,
+                    "heading": 2.606565522006597,
+                    "angularVelocity": 0.000001534033588555687,
+                    "velocityX": 3.586661194483535,
+                    "velocityY": -1.2027024960136288,
+                    "timestamp": 1.827306549781176
+                },
+                {
+                    "x": 6.413305655265644,
+                    "y": 6.4947691134226035,
+                    "heading": 2.6065655686762406,
+                    "angularVelocity": 0.000001479882707645635,
+                    "velocityX": 3.4944501104140286,
+                    "velocityY": -1.4489480673367345,
+                    "timestamp": 1.858842591990366
+                },
+                {
+                    "x": 6.520066647831161,
+                    "y": 6.441530702158678,
+                    "heading": 2.6065656145154117,
+                    "angularVelocity": 0.000001453548632665294,
+                    "velocityX": 3.3853643351100575,
+                    "velocityY": -1.6881766871941624,
+                    "timestamp": 1.8903786341995559
+                },
+                {
+                    "x": 6.6242681306818785,
+                    "y": 6.383442071865126,
+                    "heading": 2.6065656601861753,
+                    "angularVelocity": 0.0000014482084799406948,
+                    "velocityX": 3.3042029231034076,
+                    "velocityY": -1.8419759178475867,
+                    "timestamp": 1.9219146764087458
+                },
+                {
+                    "x": 6.728469078482098,
+                    "y": 6.325352481789039,
+                    "heading": 2.6065657058569283,
+                    "angularVelocity": 0.0000014482081475491236,
+                    "velocityX": 3.3041859567861076,
+                    "velocityY": -1.8420063523113561,
+                    "timestamp": 1.9534507186179357
+                },
+                {
+                    "x": 6.832670029211484,
+                    "y": 6.267262896967356,
+                    "heading": 2.6065657515277794,
+                    "angularVelocity": 0.0000014482112413699588,
+                    "velocityX": 3.3041860496692763,
+                    "velocityY": -1.8420061856955277,
+                    "timestamp": 1.9849867608271257
+                },
+                {
+                    "x": 6.936347380984222,
+                    "y": 6.209534885446302,
+                    "heading": 2.608700420086438,
+                    "angularVelocity": 0.06768980535028488,
+                    "velocityX": 3.2875828578934874,
+                    "velocityY": -1.8305407868915176,
+                    "timestamp": 2.0165228030363154
+                },
+                {
+                    "x": 7.0344207678165205,
+                    "y": 6.154966008244285,
+                    "heading": 2.632443469248245,
+                    "angularVelocity": 0.7528861422848001,
+                    "velocityX": 3.1098825331898827,
+                    "velocityY": -1.730365428865226,
+                    "timestamp": 2.0480588452455053
+                },
+                {
+                    "x": 7.1267043537102115,
+                    "y": 6.103735031776186,
+                    "heading": 2.6761348400547162,
+                    "angularVelocity": 1.3854424254207447,
+                    "velocityX": 2.9262893955285,
+                    "velocityY": -1.6245214325965975,
+                    "timestamp": 2.0795948874546952
+                },
+                {
+                    "x": 7.212236583644832,
+                    "y": 6.056223391759097,
+                    "heading": 2.7313687787461847,
+                    "angularVelocity": 1.7514543621257885,
+                    "velocityX": 2.7122055889973176,
+                    "velocityY": -1.5065822052725362,
+                    "timestamp": 2.111130929663885
+                },
+                {
+                    "x": 7.290555468694817,
+                    "y": 6.012738407550467,
+                    "heading": 2.789179732571192,
+                    "angularVelocity": 1.8331708665763002,
+                    "velocityX": 2.4834722293453213,
+                    "velocityY": -1.3788979580943836,
+                    "timestamp": 2.142666971873075
+                },
+                {
+                    "x": 7.361635905599927,
+                    "y": 5.973292872851013,
+                    "heading": 2.8461736255132477,
+                    "angularVelocity": 1.8072620706173155,
+                    "velocityX": 2.2539428515984796,
+                    "velocityY": -1.2508080258708043,
+                    "timestamp": 2.174203014082265
+                },
+                {
+                    "x": 7.425505527447213,
+                    "y": 5.937869720935748,
+                    "heading": 2.900420841886284,
+                    "angularVelocity": 1.7201656445407778,
+                    "velocityX": 2.0252897121209994,
+                    "velocityY": -1.1232592752220105,
+                    "timestamp": 2.205739056291455
+                },
+                {
+                    "x": 7.482197651528883,
+                    "y": 5.90644694625493,
+                    "heading": 2.9506667136326055,
+                    "angularVelocity": 1.5932840085963447,
+                    "velocityX": 1.7976930556348818,
+                    "velocityY": -0.9964083150440386,
+                    "timestamp": 2.237275098500645
+                },
+                {
+                    "x": 7.531741810313519,
+                    "y": 5.879003134115031,
+                    "heading": 2.9960449007561833,
+                    "angularVelocity": 1.4389309483596011,
+                    "velocityX": 1.5710328663309023,
+                    "velocityY": -0.8702364094344716,
+                    "timestamp": 2.268811140709835
+                },
+                {
+                    "x": 7.5741623718487,
+                    "y": 5.855519184527709,
+                    "heading": 3.0359339398400818,
+                    "angularVelocity": 1.264871438822304,
+                    "velocityX": 1.3451453817124663,
+                    "velocityY": -0.7446701596714312,
+                    "timestamp": 2.3003471829190247
+                },
+                {
+                    "x": 7.609479178893684,
+                    "y": 5.835978699422795,
+                    "heading": 3.0698731008846027,
+                    "angularVelocity": 1.076202296388058,
+                    "velocityX": 1.1198871060203273,
+                    "velocityY": -0.6196238886063927,
+                    "timestamp": 2.3318832251282147
+                },
+                {
+                    "x": 7.637708552445183,
+                    "y": 5.820367770747114,
+                    "heading": 3.0975089102886857,
+                    "angularVelocity": 0.8763245945944945,
+                    "velocityX": 0.895146365046179,
+                    "velocityY": -0.4950186384241787,
+                    "timestamp": 2.3634192673374046
+                },
+                {
+                    "x": 7.658864191629111,
+                    "y": 5.808674501363959,
+                    "heading": 3.118559676862925,
+                    "angularVelocity": 0.6675145357366632,
+                    "velocityX": 0.6708400199236877,
+                    "velocityY": -0.3707906434672271,
+                    "timestamp": 2.3949553095465945
+                },
+                {
+                    "x": 7.672957876154406,
+                    "y": 5.800888435996016,
+                    "heading": 3.1327912648609026,
+                    "angularVelocity": 0.45128009100108973,
+                    "velocityX": 0.4469072064213547,
+                    "velocityY": -0.24689418273527672,
+                    "timestamp": 2.4264913517557845
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": 0.22858718577555334,
+                    "velocityX": 0.2233039833876712,
+                    "velocityY": -0.12330133154384845,
+                    "timestamp": 2.4580273939649744
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": 3.1728947336241785e-32,
+                    "velocityX": 2.0932493862769896e-33,
+                    "velocityY": -2.0877321220317972e-33,
+                    "timestamp": 2.4895634361741643
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -3109,23 +7690,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "Stage Side To C3": {
             "waypoints": [
@@ -3464,7 +8029,25 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 29
+                    "controlIntervalCount": 22
+                },
+                {
+                    "x": 2.4943342208862305,
+                    "y": 2.6289942264556885,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 5.236664772033691,
+                    "y": 1.4238771200180054,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 28
                 },
                 {
                     "x": 7.68,
@@ -3476,7 +8059,584 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 0.71,
+                    "y": 4.36,
+                    "heading": 2.1,
+                    "angularVelocity": 0,
+                    "velocityX": 4.814287914507576e-35,
+                    "velocityY": 2.4063306961726914e-34,
+                    "timestamp": 0
+                },
+                {
+                    "x": 0.7189874993597535,
+                    "y": 4.351109558323372,
+                    "heading": 2.1116856791570084,
+                    "angularVelocity": 0.2954657391089134,
+                    "velocityX": 0.22724380032956057,
+                    "velocityY": -0.22478975211418176,
+                    "timestamp": 0.039550031053517704
+                },
+                {
+                    "x": 0.7369922825493326,
+                    "y": 4.3333146846737405,
+                    "heading": 2.134597544316128,
+                    "angularVelocity": 0.5793134556106004,
+                    "velocityX": 0.45524068401401757,
+                    "velocityY": -0.4499332409006739,
+                    "timestamp": 0.07910006210703541
+                },
+                {
+                    "x": 0.7640484897224145,
+                    "y": 4.306598997837255,
+                    "heading": 2.168179876708578,
+                    "angularVelocity": 0.849110139686295,
+                    "velocityX": 0.6841007820315073,
+                    "velocityY": -0.67549091934555,
+                    "timestamp": 0.11865009316055311
+                },
+                {
+                    "x": 0.800195666694539,
+                    "y": 4.270941917715144,
+                    "heading": 2.2117258914758198,
+                    "angularVelocity": 1.1010361713323769,
+                    "velocityX": 0.9139607734621368,
+                    "velocityY": -0.9015689538615157,
+                    "timestamp": 0.15820012421407081
+                },
+                {
+                    "x": 0.8454794987248587,
+                    "y": 4.226316705338956,
+                    "heading": 2.264304991238856,
+                    "angularVelocity": 1.3294325784950254,
+                    "velocityX": 1.14497589063946,
+                    "velocityY": -1.1283230679592324,
+                    "timestamp": 0.19775015526758852
+                },
+                {
+                    "x": 0.8999520283363762,
+                    "y": 4.172689027996317,
+                    "heading": 2.324662422471035,
+                    "angularVelocity": 1.5261032576815274,
+                    "velocityX": 1.3773068733576257,
+                    "velocityY": -1.3559452651268766,
+                    "timestamp": 0.23730018632110622
+                },
+                {
+                    "x": 0.963671089377297,
+                    "y": 4.110017781664974,
+                    "heading": 2.391063304695866,
+                    "angularVelocity": 1.6789084725364454,
+                    "velocityX": 1.6111001519745567,
+                    "velocityY": -1.584606754076596,
+                    "timestamp": 0.2768502173746239
+                },
+                {
+                    "x": 1.0366970385232381,
+                    "y": 4.038262577868419,
+                    "heading": 2.4609901353872035,
+                    "angularVelocity": 1.7680600704640437,
+                    "velocityX": 1.846419514743869,
+                    "velocityY": -1.8142894426418532,
+                    "timestamp": 0.31640024842814163
+                },
+                {
+                    "x": 1.1190717343496346,
+                    "y": 3.9574142704891733,
+                    "heading": 2.5303614875414775,
+                    "angularVelocity": 1.7540151121601602,
+                    "velocityX": 2.082797247742485,
+                    "velocityY": -2.0442033855762225,
+                    "timestamp": 0.35595027948165936
+                },
+                {
+                    "x": 1.21066072775428,
+                    "y": 3.867689453216591,
+                    "heading": 2.590569114237573,
+                    "angularVelocity": 1.522315535343692,
+                    "velocityX": 2.3157755117994827,
+                    "velocityY": -2.268640880488067,
+                    "timestamp": 0.3955003105351771
+                },
+                {
+                    "x": 1.310358949370952,
+                    "y": 3.7710393719100375,
+                    "heading": 2.620822405939027,
+                    "angularVelocity": 0.764937242666538,
+                    "velocityX": 2.5208127265883533,
+                    "velocityY": -2.4437422356450176,
+                    "timestamp": 0.4350503415886948
+                },
+                {
+                    "x": 1.418053038225142,
+                    "y": 3.6673441481868845,
+                    "heading": 2.6211471035683176,
+                    "angularVelocity": 0.008209794547368329,
+                    "velocityX": 2.722983673728656,
+                    "velocityY": -2.621874647401313,
+                    "timestamp": 0.47460037264221255
+                },
+                {
+                    "x": 1.5245836891775304,
+                    "y": 3.5622918530292758,
+                    "heading": 2.6211471537170588,
+                    "angularVelocity": 0.0000012679823435232559,
+                    "velocityX": 2.6935668092961826,
+                    "velocityY": -2.656187425376634,
+                    "timestamp": 0.5141504036957303
+                },
+                {
+                    "x": 1.6311142725711645,
+                    "y": 3.4572394893622254,
+                    "heading": 2.621147203865785,
+                    "angularVelocity": 0.0000012679819635220034,
+                    "velocityX": 2.6935651011115636,
+                    "velocityY": -2.6561891575988192,
+                    "timestamp": 0.553700434749248
+                },
+                {
+                    "x": 1.7376448559612427,
+                    "y": 3.3521871256915694,
+                    "heading": 2.621147254014511,
+                    "angularVelocity": 0.0000012679819538520794,
+                    "velocityX": 2.6935651010216595,
+                    "velocityY": -2.656189157689989,
+                    "timestamp": 0.5932504658027657
+                },
+                {
+                    "x": 1.8441754393513208,
+                    "y": 3.2471347620209134,
+                    "heading": 2.621147304163236,
+                    "angularVelocity": 0.000001267981944384733,
+                    "velocityX": 2.693565101021656,
+                    "velocityY": -2.656189157689993,
+                    "timestamp": 0.6328004968562835
+                },
+                {
+                    "x": 1.9507060227413988,
+                    "y": 3.142082398350257,
+                    "heading": 2.621147354311961,
+                    "angularVelocity": 0.0000012679819349678633,
+                    "velocityX": 2.6935651010216572,
+                    "velocityY": -2.656189157689994,
+                    "timestamp": 0.6723505279098012
+                },
+                {
+                    "x": 2.057236606131477,
+                    "y": 3.037030034679601,
+                    "heading": 2.6211474044606855,
+                    "angularVelocity": 0.0000012679819252039202,
+                    "velocityX": 2.693565101021662,
+                    "velocityY": -2.6561891576899894,
+                    "timestamp": 0.7119005589633189
+                },
+                {
+                    "x": 2.1637671895248896,
+                    "y": 2.931977671012326,
+                    "heading": 2.6211474546094093,
+                    "angularVelocity": 0.0000012679819154308074,
+                    "velocityX": 2.69356510110597,
+                    "velocityY": -2.656189157604496,
+                    "timestamp": 0.7514505900168367
+                },
+                {
+                    "x": 2.2702978362708315,
+                    "y": 2.8269253715890703,
+                    "heading": 2.6211475047581403,
+                    "angularVelocity": 0.0000012679820770656017,
+                    "velocityX": 2.693566702938569,
+                    "velocityY": -2.6561875332310767,
+                    "timestamp": 0.7910006210703544
+                },
+                {
+                    "x": 2.3779994803856632,
+                    "y": 2.7230739385096507,
+                    "heading": 2.6211475550861425,
+                    "angularVelocity": 0.0000012725148543062649,
+                    "velocityX": 2.723174704189075,
+                    "velocityY": -2.6258243119680853,
+                    "timestamp": 0.8305506521238721
+                },
+                {
+                    "x": 2.4943342208862305,
+                    "y": 2.6289942264556885,
+                    "heading": 2.6211476076664186,
+                    "angularVelocity": 0.0000013294623338808287,
+                    "velocityX": 2.941457627255649,
+                    "velocityY": -2.378751913662396,
+                    "timestamp": 0.8701006831773899
+                },
+                {
+                    "x": 2.6932881359026166,
+                    "y": 2.508539731368457,
+                    "heading": 2.6211476558145916,
+                    "angularVelocity": 7.831464659210102e-7,
+                    "velocityX": 3.236053335597206,
+                    "velocityY": -1.9592334766702564,
+                    "timestamp": 0.9315811021889849
+                },
+                {
+                    "x": 2.899938569108162,
+                    "y": 2.401827293887261,
+                    "heading": 2.621147703190774,
+                    "angularVelocity": 7.705897835721374e-7,
+                    "velocityX": 3.361239830954497,
+                    "velocityY": -1.7357142192064432,
+                    "timestamp": 0.99306152120058
+                },
+                {
+                    "x": 3.106589142147435,
+                    "y": 2.295115127196865,
+                    "heading": 2.6211477505669496,
+                    "angularVelocity": 7.705896615935058e-7,
+                    "velocityX": 3.361242105397809,
+                    "velocityY": -1.7357098147016614,
+                    "timestamp": 1.0545419402121752
+                },
+                {
+                    "x": 3.313239715188592,
+                    "y": 2.188402960510117,
+                    "heading": 2.621147797943125,
+                    "angularVelocity": 7.705896625567414e-7,
+                    "velocityX": 3.361242105428448,
+                    "velocityY": -1.735709814642326,
+                    "timestamp": 1.1160223592237704
+                },
+                {
+                    "x": 3.519890288229749,
+                    "y": 2.0816907938233693,
+                    "heading": 2.6211478453193005,
+                    "angularVelocity": 7.705896648559933e-7,
+                    "velocityX": 3.361242105428449,
+                    "velocityY": -1.735709814642324,
+                    "timestamp": 1.1775027782353655
+                },
+                {
+                    "x": 3.726540861270906,
+                    "y": 1.9749786271366212,
+                    "heading": 2.621147892695476,
+                    "angularVelocity": 7.705896659187971e-7,
+                    "velocityX": 3.361242105428449,
+                    "velocityY": -1.7357098146423233,
+                    "timestamp": 1.2389831972469607
+                },
+                {
+                    "x": 3.933191434312063,
+                    "y": 1.868266460449873,
+                    "heading": 2.6211479400716513,
+                    "angularVelocity": 7.705896660607392e-7,
+                    "velocityX": 3.36124210542845,
+                    "velocityY": -1.7357098146423227,
+                    "timestamp": 1.3004636162585559
+                },
+                {
+                    "x": 4.139842007353606,
+                    "y": 1.7615542937638724,
+                    "heading": 2.6211479874478267,
+                    "angularVelocity": 7.705896681379808e-7,
+                    "velocityX": 3.361242105434726,
+                    "velocityY": -1.7357098146301677,
+                    "timestamp": 1.361944035270151
+                },
+                {
+                    "x": 4.3464926090376705,
+                    "y": 1.6548421825447925,
+                    "heading": 2.621148034824002,
+                    "angularVelocity": 7.705896639238046e-7,
+                    "velocityX": 3.361242571315101,
+                    "velocityY": -1.7357089124417728,
+                    "timestamp": 1.4234244542817462
+                },
+                {
+                    "x": 4.555173710801168,
+                    "y": 1.5521575021408582,
+                    "heading": 2.6211480823085673,
+                    "angularVelocity": 7.723526622271063e-7,
+                    "velocityX": 3.3942693481666213,
+                    "velocityY": -1.6702013755724032,
+                    "timestamp": 1.4849048732933414
+                },
+                {
+                    "x": 4.775821132533127,
+                    "y": 1.478627504505032,
+                    "heading": 2.621148131521485,
+                    "angularVelocity": 8.004649014817169e-7,
+                    "velocityX": 3.5889056268524353,
+                    "velocityY": -1.1959905091401257,
+                    "timestamp": 1.5463852923049366
+                },
+                {
+                    "x": 5.004383859827403,
+                    "y": 1.4356039834024816,
+                    "heading": 2.621148186381714,
+                    "angularVelocity": 8.923203458439086e-7,
+                    "velocityX": 3.7176507735116338,
+                    "velocityY": -0.699792255716998,
+                    "timestamp": 1.6078657113165318
+                },
+                {
+                    "x": 5.236664772033691,
+                    "y": 1.4238771200180054,
+                    "heading": 2.6211482490896194,
+                    "angularVelocity": 0.0000010199654837334,
+                    "velocityX": 3.7781283202133062,
+                    "velocityY": -0.1907414356148787,
+                    "timestamp": 1.669346130328127
+                },
+                {
+                    "x": 5.366075062772529,
+                    "y": 1.4271130159839853,
+                    "heading": 2.621148315155484,
+                    "angularVelocity": 0.0000019306429598932118,
+                    "velocityX": 3.781757302483639,
+                    "velocityY": 0.09456259722124823,
+                    "timestamp": 1.703565748454824
+                },
+                {
+                    "x": 5.494873366116125,
+                    "y": 1.4400934888882522,
+                    "heading": 2.621148371231603,
+                    "angularVelocity": 0.00000163871257714578,
+                    "velocityX": 3.7638731930533114,
+                    "velocityY": 0.3793283974183189,
+                    "timestamp": 1.7377853665815208
+                },
+                {
+                    "x": 5.622326965929087,
+                    "y": 1.4627446781635232,
+                    "heading": 2.6211484208273035,
+                    "angularVelocity": 0.0000014493353027620846,
+                    "velocityX": 3.724576917868275,
+                    "velocityY": 0.6619357700429461,
+                    "timestamp": 1.7720049847082178
+                },
+                {
+                    "x": 5.747710804313215,
+                    "y": 1.4949376920061848,
+                    "heading": 2.6211484662381075,
+                    "angularVelocity": 0.000001327040057798138,
+                    "velocityX": 3.6640922736162436,
+                    "velocityY": 0.9407765371158774,
+                    "timestamp": 1.8062246028349147
+                },
+                {
+                    "x": 5.8703116218973515,
+                    "y": 1.5364893102203447,
+                    "heading": 2.62114850910788,
+                    "angularVelocity": 0.000001252783485820186,
+                    "velocityX": 3.5827640486872516,
+                    "velocityY": 1.2142630598715765,
+                    "timestamp": 1.8404442209616116
+                },
+                {
+                    "x": 5.989432110510927,
+                    "y": 1.5871628422758242,
+                    "heading": 2.621148550731445,
+                    "angularVelocity": 0.00000121636555358505,
+                    "velocityX": 3.481058385062543,
+                    "velocityY": 1.4808327745757486,
+                    "timestamp": 1.8746638390883086
+                },
+                {
+                    "x": 6.104583123843471,
+                    "y": 1.6463045521525654,
+                    "heading": 2.621148592213924,
+                    "angularVelocity": 0.0000012122426079588706,
+                    "velocityX": 3.3650583973847925,
+                    "velocityY": 1.7282983596652888,
+                    "timestamp": 1.9088834572150055
+                },
+                {
+                    "x": 6.2197290464685,
+                    "y": 1.7054561727682278,
+                    "heading": 2.621148633696581,
+                    "angularVelocity": 0.0000012122477903456845,
+                    "velocityX": 3.3649096316243146,
+                    "velocityY": 1.7285879812175449,
+                    "timestamp": 1.9431030753417025
+                },
+                {
+                    "x": 6.334874968704583,
+                    "y": 1.7646077941410194,
+                    "heading": 2.6211486751792368,
+                    "angularVelocity": 0.0000012122477871677539,
+                    "velocityX": 3.364909620258179,
+                    "velocityY": 1.7285880033431293,
+                    "timestamp": 1.9773226934683994
+                },
+                {
+                    "x": 6.450020890940638,
+                    "y": 1.8237594155138686,
+                    "heading": 2.6211487166618928,
+                    "angularVelocity": 0.000001212247782062354,
+                    "velocityX": 3.364909620257311,
+                    "velocityY": 1.7285880033448198,
+                    "timestamp": 2.0115423115950963
+                },
+                {
+                    "x": 6.565166813176692,
+                    "y": 1.882911036886718,
+                    "heading": 2.621148758144549,
+                    "angularVelocity": 0.0000012122477787509118,
+                    "velocityX": 3.3649096202573117,
+                    "velocityY": 1.7285880033448189,
+                    "timestamp": 2.0457619297217935
+                },
+                {
+                    "x": 6.68031273541274,
+                    "y": 1.9420626582595795,
+                    "heading": 2.6211487996272047,
+                    "angularVelocity": 0.0000012122477734983372,
+                    "velocityX": 3.3649096202571287,
+                    "velocityY": 1.7285880033451764,
+                    "timestamp": 2.0799815478484907
+                },
+                {
+                    "x": 6.79545865756654,
+                    "y": 2.001214279792547,
+                    "heading": 2.6211488411098602,
+                    "angularVelocity": 0.0000012122477688304903,
+                    "velocityX": 3.3649096178535927,
+                    "velocityY": 1.7285880080239562,
+                    "timestamp": 2.114201165975188
+                },
+                {
+                    "x": 6.910603503099618,
+                    "y": 2.0603679970417836,
+                    "heading": 2.621148882606884,
+                    "angularVelocity": 0.000001212667636144746,
+                    "velocityX": 3.3648781557631158,
+                    "velocityY": 1.728649251146572,
+                    "timestamp": 2.148420784101885
+                },
+                {
+                    "x": 7.018224220241307,
+                    "y": 2.1177284432893,
+                    "heading": 2.639872440534831,
+                    "angularVelocity": 0.5471585877616616,
+                    "velocityX": 3.1450005299073442,
+                    "velocityY": 1.6762444874498934,
+                    "timestamp": 2.182640402228582
+                },
+                {
+                    "x": 7.119181986421491,
+                    "y": 2.170426181119355,
+                    "heading": 2.6783971426308466,
+                    "angularVelocity": 1.1258074813511751,
+                    "velocityX": 2.9502890945887246,
+                    "velocityY": 1.5399861458109638,
+                    "timestamp": 2.2168600203552793
+                },
+                {
+                    "x": 7.213084203441603,
+                    "y": 2.2185446189369,
+                    "heading": 2.734533820088199,
+                    "angularVelocity": 1.6404822885371884,
+                    "velocityX": 2.7441047609719855,
+                    "velocityY": 1.4061652482324039,
+                    "timestamp": 2.2510796384819765
+                },
+                {
+                    "x": 7.29840370402033,
+                    "y": 2.262230718496702,
+                    "heading": 2.7957315713049304,
+                    "angularVelocity": 1.788382061721109,
+                    "velocityX": 2.493292013453601,
+                    "velocityY": 1.2766390144406516,
+                    "timestamp": 2.2852992566086736
+                },
+                {
+                    "x": 7.375044913403133,
+                    "y": 2.301443411516585,
+                    "heading": 2.8566177755066406,
+                    "angularVelocity": 1.7792777223954108,
+                    "velocityX": 2.239686284605582,
+                    "velocityY": 1.1459126421194965,
+                    "timestamp": 2.3195188747353708
+                },
+                {
+                    "x": 7.443040169771806,
+                    "y": 2.336203371638747,
+                    "heading": 2.9144119075075787,
+                    "angularVelocity": 1.6889180874829506,
+                    "velocityX": 1.9870255745380796,
+                    "velocityY": 1.0157904157043582,
+                    "timestamp": 2.353738492862068
+                },
+                {
+                    "x": 7.502434435273893,
+                    "y": 2.3665382868697598,
+                    "heading": 2.967461608910358,
+                    "angularVelocity": 1.5502715783199053,
+                    "velocityX": 1.7356787934389564,
+                    "velocityY": 0.8864773159857819,
+                    "timestamp": 2.387958110988765
+                },
+                {
+                    "x": 7.553267406161016,
+                    "y": 2.3924752684695187,
+                    "heading": 3.014662356421872,
+                    "angularVelocity": 1.379347581751362,
+                    "velocityX": 1.48549205601638,
+                    "velocityY": 0.7579564886939567,
+                    "timestamp": 2.4221777291154623
+                },
+                {
+                    "x": 7.595572130014844,
+                    "y": 2.41403971266414,
+                    "heading": 3.055206797581657,
+                    "angularVelocity": 1.184830321883501,
+                    "velocityX": 1.2362710681690243,
+                    "velocityY": 0.6301778153917356,
+                    "timestamp": 2.4563973472421594
+                },
+                {
+                    "x": 7.629376218234675,
+                    "y": 2.4312548848965747,
+                    "heading": 3.088468435996466,
+                    "angularVelocity": 0.9720049560944556,
+                    "velocityX": 0.9878569683236498,
+                    "velocityY": 0.5030790281965085,
+                    "timestamp": 2.4906169653688566
+                },
+                {
+                    "x": 7.654702978974042,
+                    "y": 2.4441416202766635,
+                    "heading": 3.113945152945951,
+                    "angularVelocity": 0.7445061734809163,
+                    "velocityX": 0.7401240027166469,
+                    "velocityY": 0.3765891054767559,
+                    "timestamp": 2.5248365834955537
+                },
+                {
+                    "x": 7.671572100944035,
+                    "y": 2.4527181446432627,
+                    "heading": 3.1312307619473794,
+                    "angularVelocity": 0.5051374021015854,
+                    "velocityX": 0.4929664003711423,
+                    "velocityY": 0.250631796498847,
+                    "timestamp": 2.559056201622251
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": 0.2562634691057309,
+                    "velocityX": 0.24628851861412304,
+                    "velocityY": 0.1251286715381821,
+                    "timestamp": 2.593275819748948
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": 3.214441635047838e-33,
+                    "velocityX": 1.400444892944232e-33,
+                    "velocityY": -3.598995466574735e-33,
+                    "timestamp": 2.627495437875645
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -3494,23 +8654,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "Stage Side To C5": {
             "waypoints": [
@@ -3873,13 +9017,113 @@
                     "x": 2.25,
                     "y": 5.56,
                     "heading": -3.143185307179586,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.2980013304879616,
+                    "y": 6.709118488011682,
+                    "heading": -2.76564785007466,
+                    "angularVelocity": -0.20597421909793953,
+                    "velocityX": -0.026308687136743093,
+                    "velocityY": -0.6302691420205844,
+                    "timestamp": 0.07596994489499284
+                },
+                {
+                    "x": 2.294004010549845,
+                    "y": 6.6133557271698225,
+                    "heading": -2.796958855018623,
+                    "angularVelocity": -0.41214989674195696,
+                    "velocityX": -0.05261712304308655,
+                    "velocityY": -1.2605348203717124,
+                    "timestamp": 0.15193988978998568
+                },
+                {
+                    "x": 2.2880079544710785,
+                    "y": 6.469712199120552,
+                    "heading": -2.8439591282551087,
+                    "angularVelocity": -0.6186693079934498,
+                    "velocityX": -0.07892668721893853,
+                    "velocityY": -1.8907941587665607,
+                    "timestamp": 0.22790983468497852
+                },
+                {
+                    "x": 2.2800128266474267,
+                    "y": 6.278188582998367,
+                    "heading": -2.9066839984131234,
+                    "angularVelocity": -0.8256537535299542,
+                    "velocityX": -0.10524066898696272,
+                    "velocityY": -2.5210445576459732,
+                    "timestamp": 0.30387977957997137
+                },
+                {
+                    "x": 2.2700178539327394,
+                    "y": 6.038785706839934,
+                    "heading": -2.985174219418862,
+                    "angularVelocity": -1.0331746470822045,
+                    "velocityX": -0.13156482775526857,
+                    "velocityY": -3.1512840569956957,
+                    "timestamp": 0.37984972447496423
+                },
+                {
+                    "x": 2.2620112641506833,
+                    "y": 5.847271276792759,
+                    "heading": -3.048371938278793,
+                    "angularVelocity": -0.8318779083923777,
+                    "velocityX": -0.10539154389440776,
+                    "velocityY": -2.520923640419774,
+                    "timestamp": 0.4558196693699571
+                },
+                {
+                    "x": 2.256005772930322,
+                    "y": 5.703635612537123,
+                    "heading": -3.0957774424626403,
+                    "angularVelocity": -0.624003403574559,
+                    "velocityX": -0.0790508829335374,
+                    "velocityY": -1.8906906468626763,
+                    "timestamp": 0.53178961426495
+                },
+                {
+                    "x": 2.25200192771716,
+                    "y": 5.607878538669833,
+                    "heading": -3.127382702013982,
+                    "angularVelocity": -0.4160231996354262,
+                    "velocityX": -0.05270301589261462,
+                    "velocityY": -1.260459962155393,
+                    "timestamp": 0.6077595591599428
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -0.20801127587293636,
+                    "velocityX": -0.026351575217375103,
+                    "velocityY": -0.6302300039313167,
+                    "timestamp": 0.6837295040549357
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": 2.276477315177118e-32,
+                    "velocityX": 0,
+                    "velocityY": -9.616776104992252e-34,
+                    "timestamp": 0.7596994489499286
                 }
             ],
-            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -3897,23 +9141,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "W1 To W3": {
             "waypoints": [
@@ -4494,19 +9722,569 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 30
+                },
+                {
+                    "x": 2.1914827823638916,
+                    "y": 2.376894474029541,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 29
                 },
                 {
                     "x": 4.263,
                     "y": 3,
-                    "heading": 2.574,
+                    "heading": -3.7091853071795864,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": -7.419385593511551e-32,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.293773644442264,
+                    "y": 6.737033643205664,
+                    "heading": -2.75526766821854,
+                    "angularVelocity": -0.10504784252909477,
+                    "velocityX": -0.1241659859778435,
+                    "velocityY": -0.3981690982414253,
+                    "timestamp": 0.050145420331514534
+                },
+                {
+                    "x": 2.2813193646388377,
+                    "y": 6.697095621507987,
+                    "heading": -2.7655160845701583,
+                    "angularVelocity": -0.20437392455513897,
+                    "velocityX": -0.24836325473174134,
+                    "velocityY": -0.7964440507955471,
+                    "timestamp": 0.10029084066302907
+                },
+                {
+                    "x": 2.2626352666399625,
+                    "y": 6.637179497704236,
+                    "heading": -2.7803732946846287,
+                    "angularVelocity": -0.2962824923243045,
+                    "velocityX": -0.3725982926327797,
+                    "velocityY": -1.1948473740501462,
+                    "timestamp": 0.1504362609945436
+                },
+                {
+                    "x": 2.2377190441074863,
+                    "y": 6.557277385764274,
+                    "heading": -2.799337789044294,
+                    "angularVelocity": -0.3781899570148323,
+                    "velocityX": -0.4968793235305074,
+                    "velocityY": -1.5934079605220988,
+                    "timestamp": 0.20058168132605814
+                },
+                {
+                    "x": 2.2065679165597203,
+                    "y": 6.457379637066711,
+                    "heading": -2.8216967909576067,
+                    "angularVelocity": -0.44588322852808987,
+                    "velocityX": -0.6212158027956237,
+                    "velocityY": -1.9921609598071555,
+                    "timestamp": 0.25072710165757267
+                },
+                {
+                    "x": 2.1691787676944525,
+                    "y": 6.337475004416767,
+                    "heading": -2.846358557659031,
+                    "angularVelocity": -0.49180496520688655,
+                    "velocityX": -0.7456144273611754,
+                    "velocityY": -2.3911382506567254,
+                    "timestamp": 0.3008725219890872
+                },
+                {
+                    "x": 2.1255494213188793,
+                    "y": 6.197554115260485,
+                    "heading": -2.8714458003624506,
+                    "angularVelocity": -0.5002898078741331,
+                    "velocityX": -0.8700564495648131,
+                    "velocityY": -2.7903024489824935,
+                    "timestamp": 0.35101794232060174
+                },
+                {
+                    "x": 2.0756883391523315,
+                    "y": 6.037638592000563,
+                    "heading": -2.8929930081362487,
+                    "angularVelocity": -0.4296944293486671,
+                    "velocityX": -0.9943297281568805,
+                    "velocityY": -3.1890354533417224,
+                    "timestamp": 0.4011633626521163
+                },
+                {
+                    "x": 2.0198170203632526,
+                    "y": 5.858213938792853,
+                    "heading": -2.897462778158027,
+                    "angularVelocity": -0.08913615624774217,
+                    "velocityX": -1.1141858702092737,
+                    "velocityY": -3.578086533556236,
+                    "timestamp": 0.4513087829836308
+                },
+                {
+                    "x": 1.9633339655295539,
+                    "y": 5.677120968466723,
+                    "heading": -2.897462791142581,
+                    "angularVelocity": -2.5893798802100903e-7,
+                    "velocityX": -1.1263851107496088,
+                    "velocityY": -3.611356114454979,
+                    "timestamp": 0.5014542033151453
+                },
+                {
+                    "x": 1.9068509267595308,
+                    "y": 5.496027993130308,
+                    "heading": -2.897462804127121,
+                    "angularVelocity": -2.5893770297899326e-7,
+                    "velocityX": -1.1263847904077713,
+                    "velocityY": -3.611356214370084,
+                    "timestamp": 0.5515996236466598
+                },
+                {
+                    "x": 1.8503679083113405,
+                    "y": 5.314935011455497,
+                    "heading": -2.897462817111663,
+                    "angularVelocity": -2.589377492115138e-7,
+                    "velocityX": -1.1263843851497735,
+                    "velocityY": -3.611356340770398,
+                    "timestamp": 0.6017450439781744
+                },
+                {
+                    "x": 1.7938849123636174,
+                    "y": 5.133842022762774,
+                    "heading": -2.8974628300962064,
+                    "angularVelocity": -2.589377769209582e-7,
+                    "velocityX": -1.1263839364454482,
+                    "velocityY": -3.6113564807215806,
+                    "timestamp": 0.651890464309689
+                },
+                {
+                    "x": 1.737401940953919,
+                    "y": 4.952749026416629,
+                    "heading": -2.89746284308075,
+                    "angularVelocity": -2.5893777760216377e-7,
+                    "velocityX": -1.126383447108141,
+                    "velocityY": -3.611356633346137,
+                    "timestamp": 0.7020358846412036
+                },
+                {
+                    "x": 1.680918995692082,
+                    "y": 4.771656021914957,
+                    "heading": -2.897462856065292,
+                    "angularVelocity": -2.5893774300527816e-7,
+                    "velocityX": -1.1263829256674796,
+                    "velocityY": -3.6113567959836606,
+                    "timestamp": 0.7521813049727182
+                },
+                {
+                    "x": 1.6244360774524895,
+                    "y": 4.590563008985036,
+                    "heading": -2.8974628690498303,
+                    "angularVelocity": -2.589376688920553e-7,
+                    "velocityX": -1.1263823867898586,
+                    "velocityY": -3.611356964059789,
+                    "timestamp": 0.8023267253042328
+                },
+                {
+                    "x": 1.567953186210211,
+                    "y": 4.40946998763471,
+                    "heading": -2.897462882034363,
+                    "angularVelocity": -2.589375607053774e-7,
+                    "velocityX": -1.1263818484094146,
+                    "velocityY": -3.6113571319795557,
+                    "timestamp": 0.8524721456357474
+                },
+                {
+                    "x": 1.5114708900262606,
+                    "y": 4.228376780686611,
+                    "heading": -2.8974628950188928,
+                    "angularVelocity": -2.589374956200649e-7,
+                    "velocityX": -1.1263699817559003,
+                    "velocityY": -3.611360833170398,
+                    "timestamp": 0.902617565967262
+                },
+                {
+                    "x": 1.4675217320832292,
+                    "y": 4.043840955415942,
+                    "heading": -2.8974629081797687,
+                    "angularVelocity": -2.624542007539591e-7,
+                    "velocityX": -0.8764341320200526,
+                    "velocityY": -3.6800135296641385,
+                    "timestamp": 0.9527629862987765
+                },
+                {
+                    "x": 1.4442056001491934,
+                    "y": 3.855582205665926,
+                    "heading": -2.8974629221278954,
+                    "angularVelocity": -2.7815354458428775e-7,
+                    "velocityX": -0.4649703159309848,
+                    "velocityY": -3.75425609169144,
+                    "timestamp": 1.0029084066302911
+                },
+                {
+                    "x": 1.4418077217209881,
+                    "y": 3.665900239807532,
+                    "heading": -2.897462937731955,
+                    "angularVelocity": -3.1117616956346287e-7,
+                    "velocityX": -0.04781849294218802,
+                    "velocityY": -3.782637868112273,
+                    "timestamp": 1.0530538269618057
+                },
+                {
+                    "x": 1.4603574724576953,
+                    "y": 3.47711225483097,
+                    "heading": -2.8974629563063408,
+                    "angularVelocity": -3.7041041196117577e-7,
+                    "velocityX": 0.3699191394562724,
+                    "velocityY": -3.7648101008720767,
+                    "timestamp": 1.1031992472933203
+                },
+                {
+                    "x": 1.4996282585911056,
+                    "y": 3.2915246350140723,
+                    "heading": -2.897463288823342,
+                    "angularVelocity": -0.000006631054220103306,
+                    "velocityX": 0.7831380388037097,
+                    "velocityY": -3.7009884170870753,
+                    "timestamp": 1.153344667624835
+                },
+                {
+                    "x": 1.5575649679963268,
+                    "y": 3.1148801932866275,
+                    "heading": -2.9095242713797083,
+                    "angularVelocity": -0.24052012081323423,
+                    "velocityX": 1.1553738910193128,
+                    "velocityY": -3.5226435546783237,
+                    "timestamp": 1.2034900879563495
+                },
+                {
+                    "x": 1.6317457698728626,
+                    "y": 2.9507691275120527,
+                    "heading": -2.937069238173179,
+                    "angularVelocity": -0.549301743037936,
+                    "velocityX": 1.4793135920712512,
+                    "velocityY": -3.2727029644905956,
+                    "timestamp": 1.253635508287864
+                },
+                {
+                    "x": 1.7203254363653924,
+                    "y": 2.801423061648199,
+                    "heading": -2.9780621265122655,
+                    "angularVelocity": -0.8174802019421967,
+                    "velocityX": 1.7664557582112876,
+                    "velocityY": -2.9782593281004814,
+                    "timestamp": 1.3037809286193787
+                },
+                {
+                    "x": 1.821600608747065,
+                    "y": 2.668597377909185,
+                    "heading": -3.0274133717272194,
+                    "angularVelocity": -0.9841625593860135,
+                    "velocityX": 2.019629543677879,
+                    "velocityY": -2.648809858625076,
+                    "timestamp": 1.3539263489508933
+                },
+                {
+                    "x": 1.9344033083274008,
+                    "y": 2.553226314276784,
+                    "heading": -3.081880110008977,
+                    "angularVelocity": -1.0861757249549924,
+                    "velocityX": 2.2495114974525996,
+                    "velocityY": -2.300729814800146,
+                    "timestamp": 1.4040717692824078
+                },
+                {
+                    "x": 2.057902367887511,
+                    "y": 2.4558701645332204,
+                    "heading": -3.139236203843767,
+                    "angularVelocity": -1.1437952549924524,
+                    "velocityX": 2.4628183140883095,
+                    "velocityY": -1.9414763920600462,
+                    "timestamp": 1.4542171896139224
+                },
+                {
+                    "x": 2.1914827823638916,
+                    "y": 2.376894474029541,
+                    "heading": -3.1978794252899205,
+                    "angularVelocity": -1.1694631545305318,
+                    "velocityX": 2.663860699407278,
+                    "velocityY": -1.5749332637270959,
+                    "timestamp": 1.504362609945437
+                },
+                {
+                    "x": 2.278019971764992,
+                    "y": 2.3350837736524683,
+                    "heading": -3.2342809833466872,
+                    "angularVelocity": -1.1705395923861364,
+                    "velocityX": 2.7827162301626456,
+                    "velocityY": -1.3444776209968852,
+                    "timestamp": 1.535460710593682
+                },
+                {
+                    "x": 2.368131283216315,
+                    "y": 2.300501166585177,
+                    "heading": -3.270450856694399,
+                    "angularVelocity": -1.1630894682876274,
+                    "velocityX": 2.8976467878404213,
+                    "velocityY": -1.112048850135951,
+                    "timestamp": 1.5665588112419269
+                },
+                {
+                    "x": 2.461670879436653,
+                    "y": 2.2732156435859956,
+                    "heading": -3.306075840058796,
+                    "angularVelocity": -1.1455678199567971,
+                    "velocityX": 3.007887757467237,
+                    "velocityY": -0.8774015914287735,
+                    "timestamp": 1.5976569118901718
+                },
+                {
+                    "x": 2.5584617966487637,
+                    "y": 2.2533048266167017,
+                    "heading": -3.3407803732359653,
+                    "angularVelocity": -1.1159695432761514,
+                    "velocityX": 3.112438225952366,
+                    "velocityY": -0.6402582972673794,
+                    "timestamp": 1.6287550125384167
+                },
+                {
+                    "x": 2.658285404214264,
+                    "y": 2.240855747544563,
+                    "heading": -3.37410574490051,
+                    "angularVelocity": -1.0716208054472351,
+                    "velocityX": 3.209958341013189,
+                    "velocityY": -0.40031637986360386,
+                    "timestamp": 1.6598531131866616
+                },
+                {
+                    "x": 2.7608658878900645,
+                    "y": 2.2359648026671843,
+                    "heading": -3.405478910014421,
+                    "angularVelocity": -1.0088450567700136,
+                    "velocityX": 3.2986092892329157,
+                    "velocityY": -0.1572747137422285,
+                    "timestamp": 1.6909512138349065
+                },
+                {
+                    "x": 2.8658468441128373,
+                    "y": 2.238735383081805,
+                    "heading": -3.4341645997548764,
+                    "angularVelocity": -0.9224257797905216,
+                    "velocityX": 3.3757996158745374,
+                    "velocityY": 0.08909162800513673,
+                    "timestamp": 1.7220493144831515
+                },
+                {
+                    "x": 2.972755430801249,
+                    "y": 2.249269494915596,
+                    "heading": -3.459191954885324,
+                    "angularVelocity": -0.8047872573808069,
+                    "velocityX": 3.4377850884743864,
+                    "velocityY": 0.3387381098589673,
+                    "timestamp": 1.7531474151313964
+                },
+                {
+                    "x": 3.0809481205317057,
+                    "y": 2.267644679606423,
+                    "heading": -3.4792519494186864,
+                    "angularVelocity": -0.6450552964717784,
+                    "velocityX": 3.4790770971590295,
+                    "velocityY": 0.5908780378155715,
+                    "timestamp": 1.7842455157796413
+                },
+                {
+                    "x": 3.1896989899339894,
+                    "y": 2.293908495297157,
+                    "heading": -3.4936234234211128,
+                    "angularVelocity": -0.4621334969934767,
+                    "velocityX": 3.4970260927630066,
+                    "velocityY": 0.8445472599052618,
+                    "timestamp": 1.8153436164278862
+                },
+                {
+                    "x": 3.298236220991847,
+                    "y": 2.3281164444820805,
+                    "heading": -3.5030903221852148,
+                    "angularVelocity": -0.30442048121156556,
+                    "velocityX": 3.4901562730643,
+                    "velocityY": 1.1000012371126273,
+                    "timestamp": 1.8464417170761311
+                },
+                {
+                    "x": 3.405452310949792,
+                    "y": 2.3702201329887522,
+                    "heading": -3.5091052020695845,
+                    "angularVelocity": -0.1934163102887616,
+                    "velocityX": 3.447673257305325,
+                    "velocityY": 1.3538990365653494,
+                    "timestamp": 1.877539817724376
+                },
+                {
+                    "x": 3.5097674187642585,
+                    "y": 2.419844212041495,
+                    "heading": -3.514308835252089,
+                    "angularVelocity": -0.1673296141576457,
+                    "velocityX": 3.354388391573783,
+                    "velocityY": 1.5957270064190303,
+                    "timestamp": 1.908637918372621
+                },
+                {
+                    "x": 3.6091747208997034,
+                    "y": 2.4757866931575614,
+                    "heading": -3.523108085575375,
+                    "angularVelocity": -0.2829513745169274,
+                    "velocityX": 3.196571496756511,
+                    "velocityY": 1.7989034683770408,
+                    "timestamp": 1.939736019020866
+                },
+                {
+                    "x": 3.701528766735335,
+                    "y": 2.535299893209444,
+                    "heading": -3.5376794547025563,
+                    "angularVelocity": -0.4685613855325914,
+                    "velocityX": 2.969764838060721,
+                    "velocityY": 1.913724594471066,
+                    "timestamp": 1.9708341196691108
+                },
+                {
+                    "x": 3.7858722644846305,
+                    "y": 2.59510740434591,
+                    "heading": -3.555385401955535,
+                    "angularVelocity": -0.5693578348478764,
+                    "velocityX": 2.7121752129918284,
+                    "velocityY": 1.923188551383177,
+                    "timestamp": 2.001932220317356
+                },
+                {
+                    "x": 3.862373728468494,
+                    "y": 2.653074903297687,
+                    "heading": -3.5744288715420867,
+                    "angularVelocity": -0.6123676105476805,
+                    "velocityX": 2.460004385772028,
+                    "velocityY": 1.8640205589227477,
+                    "timestamp": 2.033030320965601
+                },
+                {
+                    "x": 3.931417292290283,
+                    "y": 2.707976512433947,
+                    "heading": -3.5936932545527167,
+                    "angularVelocity": -0.6194713699243786,
+                    "velocityX": 2.220185875746878,
+                    "velocityY": 1.7654328718419663,
+                    "timestamp": 2.064128421613846
+                },
+                {
+                    "x": 3.9933519727900295,
+                    "y": 2.7590743829972735,
+                    "heading": -3.612463672222599,
+                    "angularVelocity": -0.6035872699166261,
+                    "velocityX": 1.991590457574803,
+                    "velocityY": 1.643118695295981,
+                    "timestamp": 2.0952265222620907
+                },
+                {
+                    "x": 4.04845853802061,
+                    "y": 2.8058913864006936,
+                    "heading": -3.6302579260771157,
+                    "angularVelocity": -0.5721974488337824,
+                    "velocityX": 1.7720235024607334,
+                    "velocityY": 1.5054618265268112,
+                    "timestamp": 2.1263246229103356
+                },
+                {
+                    "x": 4.096959063236219,
+                    "y": 2.8480989028223083,
+                    "heading": -3.64673270512686,
+                    "angularVelocity": -0.5297680149691044,
+                    "velocityX": 1.5595976668866187,
+                    "velocityY": 1.3572377586345543,
+                    "timestamp": 2.1574227235585806
+                },
+                {
+                    "x": 4.139030722594851,
+                    "y": 2.8854588640311274,
+                    "heading": -3.6616322031984536,
+                    "angularVelocity": -0.4791127998494546,
+                    "velocityX": 1.352869097521714,
+                    "velocityY": 1.2013582961674927,
+                    "timestamp": 2.1885208242068255
+                },
+                {
+                    "x": 4.174817002989322,
+                    "y": 2.917791762586731,
+                    "heading": -3.6747587890471625,
+                    "angularVelocity": -0.4221024942061285,
+                    "velocityX": 1.150754536402508,
+                    "velocityY": 1.0397065377505372,
+                    "timestamp": 2.2196189248550704
+                },
+                {
+                    "x": 4.204435949248215,
+                    "y": 2.9449578523068656,
+                    "heading": -3.68595534704571,
+                    "angularVelocity": -0.360039930579135,
+                    "velocityX": 0.9524358607593557,
+                    "velocityY": 0.8735610585165674,
+                    "timestamp": 2.2507170255033153
+                },
+                {
+                    "x": 4.227986113342503,
+                    "y": 2.9668454867866734,
+                    "heading": -3.6950940909562924,
+                    "angularVelocity": -0.2938682337530664,
+                    "velocityX": 0.7572862523234876,
+                    "velocityY": 0.7038254434694367,
+                    "timestamp": 2.2818151261515602
+                },
+                {
+                    "x": 4.245550869753413,
+                    "y": 2.983363547472538,
+                    "heading": -3.702069156081572,
+                    "angularVelocity": -0.22429231946250391,
+                    "velocityX": 0.5648176591100355,
+                    "velocityY": 0.5311597924485464,
+                    "timestamp": 2.312913226799805
+                },
+                {
+                    "x": 4.25720158771329,
+                    "y": 2.99443633378999,
+                    "heading": -3.7067915044128146,
+                    "angularVelocity": -0.15185327183332542,
+                    "velocityX": 0.3746440366779525,
+                    "velocityY": 0.3560598906890862,
+                    "timestamp": 2.34401132744805
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": -3.7091853071795864,
+                    "angularVelocity": -0.07697585115715987,
+                    "velocityX": 0.1864555122609252,
+                    "velocityY": 0.17890694589168776,
+                    "timestamp": 2.375109428096295
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": -3.7091853071795864,
+                    "angularVelocity": 7.12380386376873e-29,
+                    "velocityX": -3.695914984045945e-30,
+                    "velocityY": 3.729410348522699e-30,
+                    "timestamp": 2.40620752874454
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -4529,16 +10307,6 @@
                     "x": 3.42,
                     "y": 4.04,
                     "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
                 }
             ]
         },
@@ -4551,19 +10319,587 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 28
+                },
+                {
+                    "x": 2.170377492904663,
+                    "y": 3.170344352722168,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 33
                 },
                 {
                     "x": 5.176,
                     "y": 1.62,
-                    "heading": 2.5,
+                    "heading": -3.7831853071795862,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": -1.7629358050488447e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.2980097531788157,
+                    "y": 6.74279910356074,
+                    "heading": -2.762722782141496,
+                    "angularVelocity": -0.3022479537187966,
+                    "velocityX": -0.047281170298139656,
+                    "velocityY": -0.3373626807410066,
+                    "timestamp": 0.04209385699707004
+                },
+                {
+                    "x": 2.2940230158560544,
+                    "y": 6.7143715511474085,
+                    "heading": -2.7877986560948775,
+                    "angularVelocity": -0.5957133829557797,
+                    "velocityX": -0.09471066818701897,
+                    "velocityY": -0.6753373162100508,
+                    "timestamp": 0.08418771399414007
+                },
+                {
+                    "x": 2.2880322840288483,
+                    "y": 6.671686949257674,
+                    "heading": -2.8247754513459067,
+                    "angularVelocity": -0.8784368525222962,
+                    "velocityX": -0.14231843443624606,
+                    "velocityY": -1.0140339929578401,
+                    "timestamp": 0.12628157099121012
+                },
+                {
+                    "x": 2.2800276497015446,
+                    "y": 6.614707345454749,
+                    "heading": -2.873051459668216,
+                    "angularVelocity": -1.1468658794006534,
+                    "velocityX": -0.19016157934542763,
+                    "velocityY": -1.353632284323334,
+                    "timestamp": 0.16837542798828015
+                },
+                {
+                    "x": 2.269996639934327,
+                    "y": 6.543383180902105,
+                    "heading": -2.9317676826482244,
+                    "angularVelocity": -1.3948881658455714,
+                    "velocityX": -0.23830103684524737,
+                    "velocityY": -1.694407916993876,
+                    "timestamp": 0.21046928498535017
+                },
+                {
+                    "x": 2.2579259581538498,
+                    "y": 6.457648474415292,
+                    "heading": -2.9996334069438224,
+                    "angularVelocity": -1.6122477039897383,
+                    "velocityX": -0.28675637353255107,
+                    "velocityY": -2.0367510274190597,
+                    "timestamp": 0.2525631419824202
+                },
+                {
+                    "x": 2.2438060571207714,
+                    "y": 6.357417527376881,
+                    "heading": -3.0746299268265957,
+                    "angularVelocity": -1.7816499896408666,
+                    "velocityX": -0.3354385185957493,
+                    "velocityY": -2.3811300315242536,
+                    "timestamp": 0.2946569989794902
+                },
+                {
+                    "x": 2.227639514948982,
+                    "y": 6.242593213913433,
+                    "heading": -3.1534176716951676,
+                    "angularVelocity": -1.8717159815993158,
+                    "velocityX": -0.3840594168625274,
+                    "velocityY": -2.727816400180199,
+                    "timestamp": 0.33675085597656024
+                },
+                {
+                    "x": 2.2094628615660015,
+                    "y": 6.1131506228921415,
+                    "heading": -3.2295701531339893,
+                    "angularVelocity": -1.8091115158233875,
+                    "velocityX": -0.43181249426122753,
+                    "velocityY": -3.07509456855668,
+                    "timestamp": 0.37884471297363026
+                },
+                {
+                    "x": 2.189573480043473,
+                    "y": 5.97059741661809,
+                    "heading": -3.280208967362098,
+                    "angularVelocity": -1.2029977255739333,
+                    "velocityX": -0.4725008098904457,
+                    "velocityY": -3.386556054580001,
+                    "timestamp": 0.4209385699707003
+                },
+                {
+                    "x": 2.167754438435126,
+                    "y": 5.816653814130607,
+                    "heading": -3.2929329955032975,
+                    "angularVelocity": -0.3022775542304222,
+                    "velocityX": -0.5183426553158416,
+                    "velocityY": -3.657151267896363,
+                    "timestamp": 0.4630324269677703
+                },
+                {
+                    "x": 2.1447655867486377,
+                    "y": 5.659083438978025,
+                    "heading": -3.2929330394086493,
+                    "angularVelocity": -0.0000010430346734701328,
+                    "velocityX": -0.5461331730206714,
+                    "velocityY": -3.74331045890019,
+                    "timestamp": 0.5051262839648404
+                },
+                {
+                    "x": 2.1217767257990308,
+                    "y": 5.50151306517681,
+                    "heading": -3.2929330833137715,
+                    "angularVelocity": -0.0000010430292064009232,
+                    "velocityX": -0.5461333930793576,
+                    "velocityY": -3.7433104267965445,
+                    "timestamp": 0.5472201409619104
+                },
+                {
+                    "x": 2.098787889918124,
+                    "y": 5.3439426877181715,
+                    "heading": -3.292933127218878,
+                    "angularVelocity": -0.0000010430288283584586,
+                    "velocityX": -0.5461327975363938,
+                    "velocityY": -3.7433105136838924,
+                    "timestamp": 0.5893139979589804
+                },
+                {
+                    "x": 2.075799081922294,
+                    "y": 5.186372306191217,
+                    "heading": -3.292933171123966,
+                    "angularVelocity": -0.000001043028392528962,
+                    "velocityX": -0.5461321350863673,
+                    "velocityY": -3.7433106103325895,
+                    "timestamp": 0.6314078549560505
+                },
+                {
+                    "x": 2.052810304885704,
+                    "y": 5.028801920147444,
+                    "heading": -3.292933215029033,
+                    "angularVelocity": -0.0000010430278884550004,
+                    "velocityX": -0.5461313996051773,
+                    "velocityY": -3.7433107176360743,
+                    "timestamp": 0.6735017119531205
+                },
+                {
+                    "x": 2.029821561935122,
+                    "y": 4.8712315291306805,
+                    "heading": -3.2929332589340747,
+                    "angularVelocity": -0.0000010430273082107483,
+                    "velocityX": -0.546130589843121,
+                    "velocityY": -3.7433108357766267,
+                    "timestamp": 0.7155955689501905
+                },
+                {
+                    "x": 2.0068328558598987,
+                    "y": 4.71366113273398,
+                    "heading": -3.29293330283909,
+                    "angularVelocity": -0.0000010430266595669302,
+                    "velocityX": -0.5461297138160472,
+                    "velocityY": -3.7433109635847206,
+                    "timestamp": 0.7576894259472605
+                },
+                {
+                    "x": 1.9838441886370295,
+                    "y": 4.556090730668921,
+                    "heading": -3.2929333467440762,
+                    "angularVelocity": -0.0000010430259761326365,
+                    "velocityX": -0.5461287908225987,
+                    "velocityY": -3.7433110982447326,
+                    "timestamp": 0.7997832829443305
+                },
+                {
+                    "x": 1.960855561483052,
+                    "y": 4.398520322758029,
+                    "heading": -3.2929333906490403,
+                    "angularVelocity": -0.0000010430254501600513,
+                    "velocityX": -0.5461278389285532,
+                    "velocityY": -3.7433112371209067,
+                    "timestamp": 0.8418771399414006
+                },
+                {
+                    "x": 1.9378696268054536,
+                    "y": 4.240949522053656,
+                    "heading": -3.2929334345540515,
+                    "angularVelocity": -0.0000010430265803117889,
+                    "velocityX": -0.5460638752870397,
+                    "velocityY": -3.7433205684938873,
+                    "timestamp": 0.8839709969384706
+                },
+                {
+                    "x": 1.9275923145038192,
+                    "y": 4.0820429860952725,
+                    "heading": -3.292933478993091,
+                    "angularVelocity": -0.0000010557131812143582,
+                    "velocityX": -0.24415230712523545,
+                    "velocityY": -3.7750528769422123,
+                    "timestamp": 0.9260648539355406
+                },
+                {
+                    "x": 1.9320863630886849,
+                    "y": 3.9228678829456647,
+                    "heading": -3.29293352534017,
+                    "angularVelocity": -0.0000011010413859861484,
+                    "velocityX": 0.10676257547932101,
+                    "velocityY": -3.781433076106252,
+                    "timestamp": 0.9681587109326106
+                },
+                {
+                    "x": 1.9513134143780595,
+                    "y": 3.764794388276326,
+                    "heading": -3.2929335753664444,
+                    "angularVelocity": -0.0000011884459622031428,
+                    "velocityX": 0.4567662044063322,
+                    "velocityY": -3.75526278526441,
+                    "timestamp": 1.0102525679296808
+                },
+                {
+                    "x": 1.9851080356036048,
+                    "y": 3.609183233024402,
+                    "heading": -3.292933631555695,
+                    "angularVelocity": -0.000001334856299032327,
+                    "velocityX": 0.8028397404376079,
+                    "velocityY": -3.696766377639273,
+                    "timestamp": 1.052346424926751
+                },
+                {
+                    "x": 2.033179344139022,
+                    "y": 3.457373959395419,
+                    "heading": -3.2929336978163843,
+                    "angularVelocity": -0.000001574117798079096,
+                    "velocityX": 1.142002942110131,
+                    "velocityY": -3.6064472219675583,
+                    "timestamp": 1.094440281923821
+                },
+                {
+                    "x": 2.095113543139509,
+                    "y": 3.310673385447386,
+                    "heading": -3.29293378107099,
+                    "angularVelocity": -0.0000019778326800211228,
+                    "velocityX": 1.4713358057162051,
+                    "velocityY": -3.485082727350082,
+                    "timestamp": 1.1365341389208912
+                },
+                {
+                    "x": 2.170377492904663,
+                    "y": 3.170344352722168,
+                    "heading": -3.292933888363931,
+                    "angularVelocity": -0.00000254889782501678,
+                    "velocityX": 1.7880031704006887,
+                    "velocityY": -3.3337176190574715,
+                    "timestamp": 1.1786279959179613
+                },
+                {
+                    "x": 2.2421588223512248,
+                    "y": 3.0583154434355615,
+                    "heading": -3.2929339938603395,
+                    "angularVelocity": -0.000002999460293233564,
+                    "velocityX": 2.0408775039380207,
+                    "velocityY": -3.1851915047625172,
+                    "timestamp": 1.2137997930042903
+                },
+                {
+                    "x": 2.3224028083906,
+                    "y": 2.952183724176483,
+                    "heading": -3.2929340733758186,
+                    "angularVelocity": -0.000002260773842517862,
+                    "velocityX": 2.2814866650804433,
+                    "velocityY": -3.0175233582344014,
+                    "timestamp": 1.2489715900906193
+                },
+                {
+                    "x": 2.4106271869517015,
+                    "y": 2.852587036133821,
+                    "heading": -3.292934137641087,
+                    "angularVelocity": -0.0000018271818267107102,
+                    "velocityX": 2.508384156332853,
+                    "velocityY": -2.8317201932617277,
+                    "timestamp": 1.2841433871769483
+                },
+                {
+                    "x": 2.506301732241236,
+                    "y": 2.7601239405733753,
+                    "heading": -3.29293419233114,
+                    "angularVelocity": -0.0000015549405448920558,
+                    "velocityX": 2.720206336193231,
+                    "velocityY": -2.628898811553339,
+                    "timestamp": 1.3193151842632773
+                },
+                {
+                    "x": 2.6088514418801845,
+                    "y": 2.6753501198214895,
+                    "heading": -3.292934240814094,
+                    "angularVelocity": -0.0000013784610795710298,
+                    "velocityX": 2.9156801225493263,
+                    "velocityY": -2.410278341587403,
+                    "timestamp": 1.3544869813496063
+                },
+                {
+                    "x": 2.717659989787404,
+                    "y": 2.5987750311541573,
+                    "heading": -3.29293428528508,
+                    "angularVelocity": -0.0000012643933456255718,
+                    "velocityX": 3.093630605230363,
+                    "velocityY": -2.177173048035584,
+                    "timestamp": 1.3896587784359353
+                },
+                {
+                    "x": 2.8320734217114962,
+                    "y": 2.5308588229326605,
+                    "heading": -3.2929343273033043,
+                    "angularVelocity": -0.0000011946567396881703,
+                    "velocityX": 3.252987944951041,
+                    "velocityY": -1.9309848756034937,
+                    "timestamp": 1.4248305755222643
+                },
+                {
+                    "x": 2.951404044154876,
+                    "y": 2.472009459742262,
+                    "heading": -3.29293436807755,
+                    "angularVelocity": -0.000001159288093356335,
+                    "velocityX": 3.392792871813832,
+                    "velocityY": -1.6731975066827869,
+                    "timestamp": 1.4600023726085933
+                },
+                {
+                    "x": 3.0749338856232122,
+                    "y": 2.4225784637746113,
+                    "heading": -3.2929344086367345,
+                    "angularVelocity": -0.0000011531735020269185,
+                    "velocityX": 3.512184525719075,
+                    "velocityY": -1.4054157041314157,
+                    "timestamp": 1.4951741696949223
+                },
+                {
+                    "x": 3.199149697823942,
+                    "y": 2.374897470692157,
+                    "heading": -3.2929344492220305,
+                    "angularVelocity": -0.000001153915896405561,
+                    "velocityX": 3.531687957139154,
+                    "velocityY": -1.3556598477303206,
+                    "timestamp": 1.5303459667812513
+                },
+                {
+                    "x": 3.3233655700450013,
+                    "y": 2.3272166339715414,
+                    "heading": -3.2929344898073354,
+                    "angularVelocity": -0.0000011539161555742895,
+                    "velocityX": 3.5316896636294053,
+                    "velocityY": -1.3556554020706744,
+                    "timestamp": 1.5655177638675803
+                },
+                {
+                    "x": 3.4475814520083037,
+                    "y": 2.279535822630966,
+                    "heading": -3.2929345303926483,
+                    "angularVelocity": -0.0000011539163817435631,
+                    "velocityX": 3.531689940619614,
+                    "velocityY": -1.3556546804686425,
+                    "timestamp": 1.6006895609539094
+                },
+                {
+                    "x": 3.571797343695941,
+                    "y": 2.231855036623794,
+                    "heading": -3.2929345709779727,
+                    "angularVelocity": -0.0000011539167138358367,
+                    "velocityX": 3.53169021710065,
+                    "velocityY": -1.3556539601925897,
+                    "timestamp": 1.6358613580402384
+                },
+                {
+                    "x": 3.696013245125145,
+                    "y": 2.1841742759949323,
+                    "heading": -3.2929346115633122,
+                    "angularVelocity": -0.0000011539171376230348,
+                    "velocityX": 3.531690494071608,
+                    "velocityY": -1.355653238639736,
+                    "timestamp": 1.6710331551265674
+                },
+                {
+                    "x": 3.8202291563227684,
+                    "y": 2.1364935408143535,
+                    "heading": -3.292934652148666,
+                    "angularVelocity": -0.0000011539175501305985,
+                    "velocityX": 3.531690771806052,
+                    "velocityY": -1.3556525150974317,
+                    "timestamp": 1.7062049522128964
+                },
+                {
+                    "x": 3.9444450773804367,
+                    "y": 2.0888128313207726,
+                    "heading": -3.292934692734033,
+                    "angularVelocity": -0.0000011539179284682577,
+                    "velocityX": 3.5316910521455807,
+                    "velocityY": -1.355651784768035,
+                    "timestamp": 1.7413767492992254
+                },
+                {
+                    "x": 4.068661008518651,
+                    "y": 2.041132148088645,
+                    "heading": -3.292934733319412,
+                    "angularVelocity": -0.0000011539182628370223,
+                    "velocityX": 3.531691338754367,
+                    "velocityY": -1.3556510381057705,
+                    "timestamp": 1.7765485463855544
+                },
+                {
+                    "x": 4.192876950169643,
+                    "y": 1.9934514922440194,
+                    "heading": -3.2929347739048014,
+                    "angularVelocity": -0.0000011539185452860982,
+                    "velocityX": 3.5316916376522904,
+                    "velocityY": -1.3556502594278441,
+                    "timestamp": 1.8117203434718834
+                },
+                {
+                    "x": 4.317092949197956,
+                    "y": 1.945770985877502,
+                    "heading": -3.292934814490671,
+                    "angularVelocity": -0.0000011539322132251563,
+                    "velocityX": 3.5316932689969183,
+                    "velocityY": -1.3556460094855505,
+                    "timestamp": 1.8468921405582124
+                },
+                {
+                    "x": 4.4381547246220405,
+                    "y": 1.900646588966398,
+                    "heading": -3.304224160857315,
+                    "angularVelocity": -0.3209772403421673,
+                    "velocityX": 3.442012790160775,
+                    "velocityY": -1.2829710350126964,
+                    "timestamp": 1.8820639376445414
+                },
+                {
+                    "x": 4.551299156991204,
+                    "y": 1.858106592529644,
+                    "heading": -3.340115682679576,
+                    "angularVelocity": -1.0204631208967,
+                    "velocityX": 3.2169079132195413,
+                    "velocityY": -1.2094916939370357,
+                    "timestamp": 1.9172357347308704
+                },
+                {
+                    "x": 4.6559354752004225,
+                    "y": 1.8184604535185338,
+                    "heading": -3.3958506942974975,
+                    "angularVelocity": -1.5846506643126916,
+                    "velocityX": 2.9750063083893172,
+                    "velocityY": -1.127213912721008,
+                    "timestamp": 1.9524075318171994
+                },
+                {
+                    "x": 4.750941844820644,
+                    "y": 1.7823637508813854,
+                    "heading": -3.4552749718768117,
+                    "angularVelocity": -1.6895433984637673,
+                    "velocityX": 2.7012088517123067,
+                    "velocityY": -1.0262967953712765,
+                    "timestamp": 1.9875793289035284
+                },
+                {
+                    "x": 4.836288167632038,
+                    "y": 1.7498748942497964,
+                    "heading": -3.5138218394398413,
+                    "angularVelocity": -1.664596990006722,
+                    "velocityX": 2.4265556463296942,
+                    "velocityY": -0.9237189829068263,
+                    "timestamp": 2.0227511259898576
+                },
+                {
+                    "x": 4.9120167698263195,
+                    "y": 1.7210024537073532,
+                    "heading": -3.5691368807810755,
+                    "angularVelocity": -1.57271012355337,
+                    "velocityX": 2.153105853772738,
+                    "velocityY": -0.820897506930789,
+                    "timestamp": 2.0579229230761866
+                },
+                {
+                    "x": 4.9781740579406755,
+                    "y": 1.695744953740728,
+                    "heading": -3.6197442140788145,
+                    "angularVelocity": -1.438861175433386,
+                    "velocityX": 1.8809754858977845,
+                    "velocityY": -0.718117982559453,
+                    "timestamp": 2.0930947201625156
+                },
+                {
+                    "x": 5.034800882324283,
+                    "y": 1.6740995356540391,
+                    "heading": -3.6646343712421663,
+                    "angularVelocity": -1.2763111607055304,
+                    "velocityX": 1.6100065698837365,
+                    "velocityY": -0.6154197362608602,
+                    "timestamp": 2.1282665172488446
+                },
+                {
+                    "x": 5.081931555960545,
+                    "y": 1.6560639982794174,
+                    "heading": -3.703081117816373,
+                    "angularVelocity": -1.0931129415946466,
+                    "velocityX": 1.3400132361897832,
+                    "velocityY": -0.5127840732833052,
+                    "timestamp": 2.1634383143351736
+                },
+                {
+                    "x": 5.11959462557562,
+                    "y": 1.64163698634227,
+                    "heading": -3.734541789094988,
+                    "angularVelocity": -0.8944857495167222,
+                    "velocityX": 1.0708315393333863,
+                    "velocityY": -0.41018694329825167,
+                    "timestamp": 2.1986101114215026
+                },
+                {
+                    "x": 5.1478139750030305,
+                    "y": 1.6308177057465072,
+                    "heading": -3.758596048910276,
+                    "angularVelocity": -0.6839076137124058,
+                    "velocityX": 0.8023289045523005,
+                    "velocityY": -0.30761239095081433,
+                    "timestamp": 2.2337819085078316
+                },
+                {
+                    "x": 5.166609893194281,
+                    "y": 1.6236055874501054,
+                    "heading": -3.7749054122717336,
+                    "angularVelocity": -0.4637057162995195,
+                    "velocityX": 0.5344031226245449,
+                    "velocityY": -0.20505401753284183,
+                    "timestamp": 2.2689537055941607
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": -3.7831853071795862,
+                    "angularVelocity": -0.2354129044793956,
+                    "velocityX": 0.2669783060180454,
+                    "velocityY": -0.10251359750698616,
+                    "timestamp": 2.3041255026804897
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": -3.7831853071795862,
+                    "angularVelocity": -9.348588807340857e-32,
+                    "velocityX": 3.3761299345246814e-33,
+                    "velocityY": 7.633245142283867e-33,
+                    "timestamp": 2.3392972997668187
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -4591,11 +10927,6 @@
                     "x": 5.61,
                     "y": 2.81,
                     "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
                 }
             ]
         },
@@ -5120,19 +11451,551 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 28
+                },
+                {
+                    "x": 4.748738765716553,
+                    "y": 4.2041850090026855,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 29
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
+                    "heading": -3.143185307179586,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": -1.6129974497596655e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.3086679912938655,
+                    "y": 6.746243793862482,
+                    "heading": -2.736085320412874,
+                    "angularVelocity": 0.33564007387910216,
+                    "velocityX": 0.20908316429709453,
+                    "velocityY": -0.25945360797208683,
+                    "timestamp": 0.04145714612176501
+                },
+                {
+                    "x": 2.3260411538964876,
+                    "y": 6.724713219688379,
+                    "heading": -2.708761093447192,
+                    "angularVelocity": 0.6590957053683163,
+                    "velocityX": 0.4190631586553287,
+                    "velocityY": -0.5193453044467995,
+                    "timestamp": 0.08291429224353002
+                },
+                {
+                    "x": 2.352163012722991,
+                    "y": 6.692386750887991,
+                    "heading": -2.6686499617636756,
+                    "angularVelocity": 0.9675323903315748,
+                    "velocityX": 0.6300930302770934,
+                    "velocityY": -0.7797562501152615,
+                    "timestamp": 0.12437143836529502
+                },
+                {
+                    "x": 2.387085245084356,
+                    "y": 6.649236203863692,
+                    "heading": -2.6165769378043913,
+                    "angularVelocity": 1.2560687078251613,
+                    "velocityX": 0.8423694254976876,
+                    "velocityY": -1.0408470206212592,
+                    "timestamp": 0.16582858448706003
+                },
+                {
+                    "x": 2.4308688712635975,
+                    "y": 6.595222928663028,
+                    "heading": -2.553684267520584,
+                    "angularVelocity": 1.5170525751841024,
+                    "velocityX": 1.056117708890124,
+                    "velocityY": -1.3028700779841438,
+                    "timestamp": 0.20728573060882505
+                },
+                {
+                    "x": 2.4835846779423907,
+                    "y": 6.530294843671354,
+                    "heading": -2.481592483057959,
+                    "angularVelocity": 1.7389471106110828,
+                    "velocityX": 1.271573458625441,
+                    "velocityY": -1.5661494112733225,
+                    "timestamp": 0.24874287673059006
+                },
+                {
+                    "x": 2.5453132269093315,
+                    "y": 6.454388220972069,
+                    "heading": -2.4026634058172087,
+                    "angularVelocity": 1.9038714582264231,
+                    "velocityX": 1.4889724629292092,
+                    "velocityY": -1.830965944359449,
+                    "timestamp": 0.2902000228523551
+                },
+                {
+                    "x": 2.6161417178055544,
+                    "y": 6.3674459995028085,
+                    "heading": -2.3205870553837444,
+                    "angularVelocity": 1.9797877594467097,
+                    "velocityX": 1.708474835392443,
+                    "velocityY": -2.0971588640930596,
+                    "timestamp": 0.33165716897412006
+                },
+                {
+                    "x": 2.696110411470073,
+                    "y": 6.269518903939401,
+                    "heading": -2.2423584912738157,
+                    "angularVelocity": 1.8869741752160536,
+                    "velocityX": 1.9289483513804893,
+                    "velocityY": -2.3621282390202,
+                    "timestamp": 0.37311431509588505
+                },
+                {
+                    "x": 2.7844874173560816,
+                    "y": 6.1625113811609245,
+                    "heading": -2.1922364761983766,
+                    "angularVelocity": 1.2090078494121173,
+                    "velocityX": 2.131767720489823,
+                    "velocityY": -2.581159891329275,
+                    "timestamp": 0.41457146121765004
+                },
+                {
+                    "x": 2.8809289045046786,
+                    "y": 6.046946295339885,
+                    "heading": -2.1746156662329965,
+                    "angularVelocity": 0.42503673344096826,
+                    "velocityX": 2.3262934420361696,
+                    "velocityY": -2.7875793833374174,
+                    "timestamp": 0.456028607339415
+                },
+                {
+                    "x": 2.9795919491542358,
+                    "y": 5.925039697257722,
+                    "heading": -2.1746156143142836,
+                    "angularVelocity": 0.000001252346527187571,
+                    "velocityX": 2.3798802831186356,
+                    "velocityY": -2.940544863462319,
+                    "timestamp": 0.49748575346118
+                },
+                {
+                    "x": 3.0782548699283607,
+                    "y": 5.803132998918896,
+                    "heading": -2.174615562396172,
+                    "angularVelocity": 0.0000012523320226734394,
+                    "velocityX": 2.37987729508295,
+                    "velocityY": -2.940547281782763,
+                    "timestamp": 0.5389428995829451
+                },
+                {
+                    "x": 3.1769177990519513,
+                    "y": 5.6812263073375435,
+                    "heading": -2.1746155104780764,
+                    "angularVelocity": 0.0000012523316371956637,
+                    "velocityX": 2.379877496482862,
+                    "velocityY": -2.940547118783714,
+                    "timestamp": 0.58040004570471
+                },
+                {
+                    "x": 3.2755807374460058,
+                    "y": 5.559319623259062,
+                    "heading": -2.174615458559997,
+                    "angularVelocity": 0.0000012523312531723852,
+                    "velocityX": 2.3798777200984556,
+                    "velocityY": -2.9405469378047924,
+                    "timestamp": 0.621857191826475
+                },
+                {
+                    "x": 3.374243686147731,
+                    "y": 5.437412947522895,
+                    "heading": -2.1746154066419328,
+                    "angularVelocity": 0.0000012523308771614933,
+                    "velocityX": 2.379877968732799,
+                    "velocityY": -2.9405467365773754,
+                    "timestamp": 0.66331433794824
+                },
+                {
+                    "x": 3.472906646327713,
+                    "y": 5.315506281076437,
+                    "heading": -2.174615354723884,
+                    "angularVelocity": 0.000001252330513608687,
+                    "velocityX": 2.3798782456032144,
+                    "velocityY": -2.9405465124975687,
+                    "timestamp": 0.704771484070005
+                },
+                {
+                    "x": 3.5715696192957083,
+                    "y": 5.193599624979717,
+                    "heading": -2.1746153028058486,
+                    "angularVelocity": 0.0000012523301754013102,
+                    "velocityX": 2.379878554066626,
+                    "velocityY": -2.940546262848488,
+                    "timestamp": 0.74622863019177
+                },
+                {
+                    "x": 3.6702326064938156,
+                    "y": 5.071692980399876,
+                    "heading": -2.174615250887826,
+                    "angularVelocity": 0.0000012523298755846902,
+                    "velocityX": 2.3798788973153537,
+                    "velocityY": -2.940545985046467,
+                    "timestamp": 0.787685776313535
+                },
+                {
+                    "x": 3.7688956094655266,
+                    "y": 4.949786348586114,
+                    "heading": -2.1746151989698133,
+                    "angularVelocity": 0.0000012523296313000306,
+                    "velocityX": 2.379879277795066,
+                    "velocityY": -2.940545677112118,
+                    "timestamp": 0.8291429224353
+                },
+                {
+                    "x": 3.8675586330203298,
+                    "y": 4.82787973343092,
+                    "heading": -2.1746151470518074,
+                    "angularVelocity": 0.0000012523294719911187,
+                    "velocityX": 2.379879774285891,
+                    "velocityY": -2.940545275285949,
+                    "timestamp": 0.870600068557065
+                },
+                {
+                    "x": 3.9662940139338945,
+                    "y": 4.706031714187132,
+                    "heading": -2.1746150951231753,
+                    "angularVelocity": 0.0000012525857863060412,
+                    "velocityX": 2.381625127392215,
+                    "velocityY": -2.939131866093794,
+                    "timestamp": 0.9120572146788299
+                },
+                {
+                    "x": 4.07569184834889,
+                    "y": 4.593658547130379,
+                    "heading": -2.174615041084707,
+                    "angularVelocity": 0.0000013034777659582994,
+                    "velocityX": 2.638817300488551,
+                    "velocityY": -2.7105861731702134,
+                    "timestamp": 0.9535143608005949
+                },
+                {
+                    "x": 4.1948903308489776,
+                    "y": 4.491740265556163,
+                    "heading": -2.17461498286675,
+                    "angularVelocity": 0.0000014042924363783103,
+                    "velocityX": 2.8752216119746037,
+                    "velocityY": -2.458400809232485,
+                    "timestamp": 0.9949715069223599
+                },
+                {
+                    "x": 4.322894391111494,
+                    "y": 4.401128047699223,
+                    "heading": -2.1746149179117107,
+                    "angularVelocity": 0.00000156679957607106,
+                    "velocityX": 3.087623539897125,
+                    "velocityY": -2.1856839250535947,
+                    "timestamp": 1.036428653044125
+                },
+                {
+                    "x": 4.458635270784365,
+                    "y": 4.322578536823241,
+                    "heading": -2.1746148427021956,
+                    "angularVelocity": 0.0000018141508159566887,
+                    "velocityX": 3.2742456336522188,
+                    "velocityY": -1.8947158264409445,
+                    "timestamp": 1.07788579916589
+                },
+                {
+                    "x": 4.600979576445416,
+                    "y": 4.2567476280747965,
+                    "heading": -2.174614751821203,
+                    "angularVelocity": 0.000002192167120840163,
+                    "velocityX": 3.433528811726906,
+                    "velocityY": -1.5879266883226886,
+                    "timestamp": 1.1193429452876549
+                },
+                {
+                    "x": 4.748738765716553,
+                    "y": 4.2041850090026855,
+                    "heading": -2.174614640016373,
+                    "angularVelocity": 0.000002696877156656919,
+                    "velocityX": 3.564142809954843,
+                    "velocityY": -1.2678783753644691,
+                    "timestamp": 1.1608000914094199
+                },
+                {
+                    "x": 4.880414686886391,
+                    "y": 4.168830160718546,
+                    "heading": -2.174614553815122,
+                    "angularVelocity": 0.0000023917765874926725,
+                    "velocityX": 3.65353612769536,
+                    "velocityY": -0.9809706615128672,
+                    "timestamp": 1.1968407699651018
+                },
+                {
+                    "x": 5.014481469919241,
+                    "y": 4.144038758633067,
+                    "heading": -2.1746144885447847,
+                    "angularVelocity": 0.0000018110185423275369,
+                    "velocityX": 3.7198739980913533,
+                    "velocityY": -0.6878727892755006,
+                    "timestamp": 1.2328814485207837
+                },
+                {
+                    "x": 5.150093083338604,
+                    "y": 4.129967234831094,
+                    "heading": -2.1746144356224972,
+                    "angularVelocity": 0.0000014684043018781928,
+                    "velocityX": 3.7627375192130574,
+                    "velocityY": -0.3904344858611157,
+                    "timestamp": 1.2689221270764657
+                },
+                {
+                    "x": 5.286393751264718,
+                    "y": 4.1267043465211,
+                    "heading": -2.1746143904583044,
+                    "angularVelocity": 0.000001253144907070333,
+                    "velocityX": 3.7818563187020433,
+                    "velocityY": -0.09053348718039304,
+                    "timestamp": 1.3049628056321476
+                },
+                {
+                    "x": 5.422523381587792,
+                    "y": 4.134270240486654,
+                    "heading": -2.1746143488300143,
+                    "angularVelocity": 0.0000011550362392666718,
+                    "velocityX": 3.7771106366034917,
+                    "velocityY": 0.20992651272823276,
+                    "timestamp": 1.3410034841878296
+                },
+                {
+                    "x": 5.558824040120251,
+                    "y": 4.131006987177355,
+                    "heading": -2.1746143024581284,
+                    "angularVelocity": 0.0000012866540842201377,
+                    "velocityX": 3.7818560580616727,
+                    "velocityY": -0.09054361460641977,
+                    "timestamp": 1.3770441627435115
+                },
+                {
+                    "x": 5.69443561806916,
+                    "y": 4.116935078707767,
+                    "heading": -2.1746143409291885,
+                    "angularVelocity": -0.000001067434403137199,
+                    "velocityX": 3.762736535034787,
+                    "velocityY": -0.3904451590124025,
+                    "timestamp": 1.4130848412991934
+                },
+                {
+                    "x": 5.830736232387668,
+                    "y": 4.113670630261612,
+                    "heading": -2.1746144425807508,
+                    "angularVelocity": -0.000002820467492063081,
+                    "velocityX": 3.7818548312825957,
+                    "velocityY": -0.09057677538205054,
+                    "timestamp": 1.4491255198548754
+                },
+                {
+                    "x": 5.967072945403984,
+                    "y": 4.1145733334601475,
+                    "heading": -2.174614542773063,
+                    "angularVelocity": -0.000002779978522071658,
+                    "velocityX": 3.7828564411094536,
+                    "velocityY": 0.02504678698377033,
+                    "timestamp": 1.4851661984105573
+                },
+                {
+                    "x": 6.103409655835918,
+                    "y": 4.115476426894617,
+                    "heading": -2.1746146429653797,
+                    "angularVelocity": -0.0000027799786444267726,
+                    "velocityX": 3.7828563694020714,
+                    "velocityY": 0.025057614636036382,
+                    "timestamp": 1.5212068769662392
+                },
+                {
+                    "x": 6.239746366170269,
+                    "y": 4.1163795350566765,
+                    "heading": -2.1746147431577554,
+                    "angularVelocity": -0.000002779980281350245,
+                    "velocityX": 3.782856366694493,
+                    "velocityY": 0.025058023274013706,
+                    "timestamp": 1.5572475555219212
+                },
+                {
+                    "x": 6.37608307646628,
+                    "y": 4.117282649003582,
+                    "heading": -2.174614843350174,
+                    "angularVelocity": -0.0000027799814898706564,
+                    "velocityX": 3.7828563656306846,
+                    "velocityY": 0.025058183782793973,
+                    "timestamp": 1.5932882340776031
+                },
+                {
+                    "x": 6.512419786749522,
+                    "y": 4.118185764875291,
+                    "heading": -2.174614943542629,
+                    "angularVelocity": -0.00000277998246882532,
+                    "velocityX": 3.782856365276422,
+                    "velocityY": 0.025058237189222135,
+                    "timestamp": 1.629328912633285
+                },
+                {
+                    "x": 6.648756497843916,
+                    "y": 4.11908875827432,
+                    "heading": -2.1746150437352676,
+                    "angularVelocity": -0.000002779987571113984,
+                    "velocityX": 3.782856387782983,
+                    "velocityY": 0.02505483901017122,
+                    "timestamp": 1.665369591188967
+                },
+                {
+                    "x": 6.784343394669118,
+                    "y": 4.118261735488966,
+                    "heading": -2.17648334857758,
+                    "angularVelocity": -0.05183878098815612,
+                    "velocityX": 3.762051722076281,
+                    "velocityY": -0.02294692604294446,
+                    "timestamp": 1.701410269744649
+                },
+                {
+                    "x": 6.911008509970103,
+                    "y": 4.118512712423575,
+                    "heading": -2.2004225516217266,
+                    "angularVelocity": -0.6642273121234696,
+                    "velocityX": 3.5145041763098894,
+                    "velocityY": 0.006963712800855175,
+                    "timestamp": 1.7374509483003309
+                },
+                {
+                    "x": 7.028751474127586,
+                    "y": 4.119997003379487,
+                    "heading": -2.2461001757447607,
+                    "angularVelocity": -1.267390791559704,
+                    "velocityX": 3.2669463749294456,
+                    "velocityY": 0.04118376832493557,
+                    "timestamp": 1.7734916268560128
+                },
+                {
+                    "x": 7.137616008718106,
+                    "y": 4.122892854321661,
+                    "heading": -2.313255173533344,
+                    "angularVelocity": -1.863311138407951,
+                    "velocityX": 3.0206016910121933,
+                    "velocityY": 0.08034951222410198,
+                    "timestamp": 1.8095323054116947
+                },
+                {
+                    "x": 7.237654743603063,
+                    "y": 4.124394361652757,
+                    "heading": -2.4021910734424403,
+                    "angularVelocity": -2.467653314897855,
+                    "velocityX": 2.775717297619687,
+                    "velocityY": 0.04166146119519181,
+                    "timestamp": 1.8455729839673767
+                },
+                {
+                    "x": 7.328556844177467,
+                    "y": 4.1248711523025925,
+                    "heading": -2.5119123449992884,
+                    "angularVelocity": -3.04437307936175,
+                    "velocityX": 2.5222083550386993,
+                    "velocityY": 0.013229236211493491,
+                    "timestamp": 1.8816136625230586
+                },
+                {
+                    "x": 7.408737931676895,
+                    "y": 4.125345359362809,
+                    "heading": -2.6271223888576807,
+                    "angularVelocity": -3.1966668907300306,
+                    "velocityX": 2.224738565217367,
+                    "velocityY": 0.013157550834778516,
+                    "timestamp": 1.9176543410787406
+                },
+                {
+                    "x": 7.478113482970281,
+                    "y": 4.125802607297277,
+                    "heading": -2.739701188642133,
+                    "angularVelocity": -3.123659273243732,
+                    "velocityX": 1.9249235606428023,
+                    "velocityY": 0.012686995716837342,
+                    "timestamp": 1.9536950196344225
+                },
+                {
+                    "x": 7.536867447368059,
+                    "y": 4.126215703233864,
+                    "heading": -2.8446379232047505,
+                    "angularVelocity": -2.911619280433174,
+                    "velocityX": 1.630212491893156,
+                    "velocityY": 0.011461935600054532,
+                    "timestamp": 1.9897356981901044
+                },
+                {
+                    "x": 7.585249192103714,
+                    "y": 4.126543507918265,
+                    "heading": -2.93809966154793,
+                    "angularVelocity": -2.593229153518397,
+                    "velocityX": 1.3424204724921138,
+                    "velocityY": 0.009095408231394075,
+                    "timestamp": 2.025776376745786
+                },
+                {
+                    "x": 7.623522580110136,
+                    "y": 4.126765972225253,
+                    "heading": -3.01693933623456,
+                    "angularVelocity": -2.1875191546359196,
+                    "velocityX": 1.0619497062823264,
+                    "velocityY": 0.006172589304754541,
+                    "timestamp": 2.061817055301468
+                },
+                {
+                    "x": 7.651933909825069,
+                    "y": 4.126896328898384,
+                    "heading": -3.078642707410718,
+                    "angularVelocity": -1.7120479871328622,
+                    "velocityX": 0.7883128413089711,
+                    "velocityY": 0.003616931710359869,
+                    "timestamp": 2.0978577338571496
+                },
+                {
+                    "x": 7.670698144632144,
+                    "y": 4.126965664579503,
+                    "heading": -3.1212443544252966,
+                    "angularVelocity": -1.1820434220948564,
+                    "velocityX": 0.5206404418297583,
+                    "velocityY": 0.0019238173058132363,
+                    "timestamp": 2.1338984124128313
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -0.6087830094650188,
+                    "velocityX": 0.25809323632697007,
+                    "velocityY": 0.0009526851844440906,
+                    "timestamp": 2.169939090968513
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -7.326485384728368e-34,
+                    "velocityX": -1.4740993581424338e-35,
+                    "velocityY": 9.762457034283277e-35,
+                    "timestamp": 2.2059797695241947
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -5151,11 +12014,6 @@
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
             "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
                 {
                     "x": 5.61,
                     "y": 2.81,
@@ -5177,19 +12035,614 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 26
+                },
+                {
+                    "x": 4.169802665710449,
+                    "y": 4.898907661437988,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 10
+                },
+                {
+                    "x": 5.703982353210449,
+                    "y": 3.987083911895752,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 27
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
+                    "heading": -3.143185307179586,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "angularVelocity": 0,
+                    "velocityX": -2.2987344072087424e-31,
+                    "velocityY": -2.682409549792184e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.30727109352441,
+                    "y": 6.7499479361039905,
+                    "heading": -2.752438811293409,
+                    "angularVelocity": -0.06989284029038495,
+                    "velocityX": 0.20837913118223148,
+                    "velocityY": -0.20210205559294603,
+                    "timestamp": 0.03489357827320871
+                },
+                {
+                    "x": 2.321814327969628,
+                    "y": 6.735842827712052,
+                    "heading": -2.7572390739653767,
+                    "angularVelocity": -0.1375686561688486,
+                    "velocityX": 0.41678827924577844,
+                    "velocityY": -0.4042322137758234,
+                    "timestamp": 0.06978715654641741
+                },
+                {
+                    "x": 2.3436309003249467,
+                    "y": 6.714683550648929,
+                    "heading": -2.7643090195026687,
+                    "angularVelocity": -0.202614517832932,
+                    "velocityX": 0.6252317313088365,
+                    "velocityY": -0.6063945892120965,
+                    "timestamp": 0.10468073481962611
+                },
+                {
+                    "x": 2.3727221871027773,
+                    "y": 6.6864688072553555,
+                    "heading": -2.7735381226601596,
+                    "angularVelocity": -0.26449288419860933,
+                    "velocityX": 0.8337146322470192,
+                    "velocityY": -0.8085941537052478,
+                    "timestamp": 0.13957431309283483
+                },
+                {
+                    "x": 2.409089780647816,
+                    "y": 6.651197089471134,
+                    "heading": -2.7847907497118745,
+                    "angularVelocity": -0.32248418214977886,
+                    "velocityX": 1.042243167504621,
+                    "velocityY": -1.0108369370447357,
+                    "timestamp": 0.17446789136604354
+                },
+                {
+                    "x": 2.452735530672034,
+                    "y": 6.6088666355169075,
+                    "heading": -2.7978965041034827,
+                    "angularVelocity": -0.3755921587918882,
+                    "velocityX": 1.2508247128592573,
+                    "velocityY": -1.213130210458454,
+                    "timestamp": 0.20936146963925226
+                },
+                {
+                    "x": 2.5036615834922005,
+                    "y": 6.559475386735866,
+                    "heading": -2.8126348139869735,
+                    "angularVelocity": -0.42237886203854363,
+                    "velocityX": 1.459467768579872,
+                    "velocityY": -1.4154824820291676,
+                    "timestamp": 0.24425504791246097
+                },
+                {
+                    "x": 2.5618703896153354,
+                    "y": 6.503020972116864,
+                    "heading": -2.8287087438800844,
+                    "angularVelocity": -0.4606558194535241,
+                    "velocityX": 1.6681810523235565,
+                    "velocityY": -1.6179027033850466,
+                    "timestamp": 0.27914862618566966
+                },
+                {
+                    "x": 2.6273645712551357,
+                    "y": 6.439500824983286,
+                    "heading": -2.8456966886741912,
+                    "angularVelocity": -0.48685017802115704,
+                    "velocityX": 1.8769694849578602,
+                    "velocityY": -1.8203964820182346,
+                    "timestamp": 0.31404220445887837
+                },
+                {
+                    "x": 2.700146215046118,
+                    "y": 6.368912855338095,
+                    "heading": -2.8629525699896186,
+                    "angularVelocity": -0.4945288551468594,
+                    "velocityX": 2.085817717550196,
+                    "velocityY": -2.0229501569744945,
+                    "timestamp": 0.3489357827320871
+                },
+                {
+                    "x": 2.780213462242076,
+                    "y": 6.2912587361998735,
+                    "heading": -2.8793619429160175,
+                    "angularVelocity": -0.47026913657055547,
+                    "velocityX": 2.294612681137229,
+                    "velocityY": -2.225455885613285,
+                    "timestamp": 0.3838293610052958
+                },
+                {
+                    "x": 2.8675389350998404,
+                    "y": 6.20656386499432,
+                    "heading": -2.892555231685043,
+                    "angularVelocity": -0.37810076873529364,
+                    "velocityX": 2.502623037798732,
+                    "velocityY": -2.42723376039027,
+                    "timestamp": 0.4187229392785045
+                },
+                {
+                    "x": 2.9617036478615395,
+                    "y": 6.11507787860357,
+                    "heading": -2.8943452360571387,
+                    "angularVelocity": -0.05129896275126653,
+                    "velocityX": 2.698625862455626,
+                    "velocityY": -2.621857399502999,
+                    "timestamp": 0.4536165175517132
+                },
+                {
+                    "x": 3.05646121242224,
+                    "y": 6.023180735189561,
+                    "heading": -2.894345248240927,
+                    "angularVelocity": -3.4916993473204957e-7,
+                    "velocityX": 2.7156161462940758,
+                    "velocityY": -2.633640571181149,
+                    "timestamp": 0.48851009582492194
+                },
+                {
+                    "x": 3.151218783278769,
+                    "y": 5.931283598267322,
+                    "heading": -2.894345260424686,
+                    "angularVelocity": -3.4916908908792776e-7,
+                    "velocityX": 2.7156163267235134,
+                    "velocityY": -2.633640385136315,
+                    "timestamp": 0.5234036740981306
+                },
+                {
+                    "x": 3.245976354135778,
+                    "y": 5.839386461345578,
+                    "heading": -2.894345272608445,
+                    "angularVelocity": -3.491690897148369e-7,
+                    "velocityX": 2.715616326737254,
+                    "velocityY": -2.633640385122147,
+                    "timestamp": 0.5582972523713393
+                },
+                {
+                    "x": 3.340733924992786,
+                    "y": 5.747489324423833,
+                    "heading": -2.894345284792204,
+                    "angularVelocity": -3.491690888486973e-7,
+                    "velocityX": 2.715616326737255,
+                    "velocityY": -2.633640385122146,
+                    "timestamp": 0.593190830644548
+                },
+                {
+                    "x": 3.435491495849795,
+                    "y": 5.655592187502088,
+                    "heading": -2.894345296975963,
+                    "angularVelocity": -3.4916908897748007e-7,
+                    "velocityX": 2.715616326737255,
+                    "velocityY": -2.633640385122146,
+                    "timestamp": 0.6280844089177567
+                },
+                {
+                    "x": 3.530249066706803,
+                    "y": 5.5636950505803435,
+                    "heading": -2.8943453091597218,
+                    "angularVelocity": -3.491690889850577e-7,
+                    "velocityX": 2.715616326737255,
+                    "velocityY": -2.633640385122146,
+                    "timestamp": 0.6629779871909655
+                },
+                {
+                    "x": 3.625006637563812,
+                    "y": 5.471797913658599,
+                    "heading": -2.8943453213434807,
+                    "angularVelocity": -3.49169089086754e-7,
+                    "velocityX": 2.715616326737255,
+                    "velocityY": -2.633640385122146,
+                    "timestamp": 0.6978715654641742
+                },
+                {
+                    "x": 3.7197642084208202,
+                    "y": 5.379900776736854,
+                    "heading": -2.8943453335272395,
+                    "angularVelocity": -3.4916908901998635e-7,
+                    "velocityX": 2.71561632673725,
+                    "velocityY": -2.6336403851221513,
+                    "timestamp": 0.7327651437373829
+                },
+                {
+                    "x": 3.814521779275631,
+                    "y": 5.288003639812843,
+                    "heading": -2.8943453457109984,
+                    "angularVelocity": -3.4916908849611257e-7,
+                    "velocityX": 2.715616326674264,
+                    "velocityY": -2.633640385187098,
+                    "timestamp": 0.7676587220105916
+                },
+                {
+                    "x": 3.9092793212702794,
+                    "y": 5.196106473130367,
+                    "heading": -2.8943453578947573,
+                    "angularVelocity": -3.4916909136087424e-7,
+                    "velocityX": 2.715615499583321,
+                    "velocityY": -2.6336412380221117,
+                    "timestamp": 0.8025523002838003
+                },
+                {
+                    "x": 4.0036585134090545,
+                    "y": 5.103820780001642,
+                    "heading": -2.8943453700832222,
+                    "angularVelocity": -3.49303962122221e-7,
+                    "velocityX": 2.704772534355369,
+                    "velocityY": -2.644775849761548,
+                    "timestamp": 0.837445878557009
+                },
+                {
+                    "x": 4.09067331484616,
+                    "y": 5.004560993237775,
+                    "heading": -2.894345382446594,
+                    "angularVelocity": -3.5431651835190834e-7,
+                    "velocityX": 2.4937196396368213,
+                    "velocityY": -2.8446433892988883,
+                    "timestamp": 0.8723394568302177
+                },
+                {
+                    "x": 4.169802665710449,
+                    "y": 4.898907661437988,
+                    "heading": -2.8943453954133593,
+                    "angularVelocity": -3.71608932660535e-7,
+                    "velocityX": 2.2677339149549134,
+                    "velocityY": -3.0278732370909647,
+                    "timestamp": 0.9072330351034265
+                },
+                {
+                    "x": 4.267711504415231,
+                    "y": 4.731769329108682,
+                    "heading": -2.8943454067066363,
+                    "angularVelocity": -2.2055157567695873e-7,
+                    "velocityX": 1.9121064876426594,
+                    "velocityY": -3.2641209293092457,
+                    "timestamp": 0.9584377349135761
+                },
+                {
+                    "x": 4.3838302459464416,
+                    "y": 4.576728047661677,
+                    "heading": -2.8943454207852617,
+                    "angularVelocity": -2.749479155903796e-7,
+                    "velocityX": 2.2677360078614184,
+                    "velocityY": -3.027872090293428,
+                    "timestamp": 1.0096424347237256
+                },
+                {
+                    "x": 4.5166798451311,
+                    "y": 4.43575875391577,
+                    "heading": -2.8943454409364353,
+                    "angularVelocity": -3.9354148044055934e-7,
+                    "velocityX": 2.594480578486377,
+                    "velocityY": -2.7530538069469372,
+                    "timestamp": 1.0608471345338752
+                },
+                {
+                    "x": 4.664567965806816,
+                    "y": 4.3106572284696325,
+                    "heading": -2.8943459031991594,
+                    "angularVelocity": -0.000009027740145972362,
+                    "velocityX": 2.888174742241181,
+                    "velocityY": -2.443164903025968,
+                    "timestamp": 1.1120518343440249
+                },
+                {
+                    "x": 4.823636896940825,
+                    "y": 4.2042733124430525,
+                    "heading": -2.9000775312757443,
+                    "angularVelocity": -0.1119355859488654,
+                    "velocityX": 3.1065299029929774,
+                    "velocityY": -2.077620148560901,
+                    "timestamp": 1.1632565341541745
+                },
+                {
+                    "x": 4.99080976041859,
+                    "y": 4.118113444137051,
+                    "heading": -2.9122463824645286,
+                    "angularVelocity": -0.23765106003750752,
+                    "velocityX": 3.2647953038995774,
+                    "velocityY": -1.6826554715769175,
+                    "timestamp": 1.2144612339643241
+                },
+                {
+                    "x": 5.163718201552664,
+                    "y": 4.0530282335458665,
+                    "heading": -2.927811187303136,
+                    "angularVelocity": -0.30397219193387703,
+                    "velocityX": 3.3768080229971553,
+                    "velocityY": -1.2710788430065951,
+                    "timestamp": 1.2656659337744738
+                },
+                {
+                    "x": 5.340853772484394,
+                    "y": 4.009388376225937,
+                    "heading": -2.9448561503180763,
+                    "angularVelocity": -0.3328788778791379,
+                    "velocityX": 3.459361574005724,
+                    "velocityY": -0.852262731384641,
+                    "timestamp": 1.3168706335846234
+                },
+                {
+                    "x": 5.521188530660567,
+                    "y": 3.987375039379581,
+                    "heading": -2.9620889053684105,
+                    "angularVelocity": -0.3365463544211233,
+                    "velocityX": 3.5218399647844,
+                    "velocityY": -0.42990852261559487,
+                    "timestamp": 1.368075333394773
+                },
+                {
+                    "x": 5.703982353210449,
+                    "y": 3.987083911895752,
+                    "heading": -2.978585149802318,
+                    "angularVelocity": -0.3221627017650738,
+                    "velocityX": 3.569864157540669,
+                    "velocityY": -0.0056855617728103595,
+                    "timestamp": 1.4192800332049227
+                },
+                {
+                    "x": 5.82977348163753,
+                    "y": 3.977187586416487,
+                    "heading": -2.987958123186903,
+                    "angularVelocity": -0.27093528448013743,
+                    "velocityX": 3.636119912761043,
+                    "velocityY": -0.2860633065962091,
+                    "timestamp": 1.4538749122092192
+                },
+                {
+                    "x": 5.956799020313408,
+                    "y": 3.9574239534344695,
+                    "heading": -2.994346009010497,
+                    "angularVelocity": -0.18464830655430406,
+                    "velocityX": 3.6718017906668177,
+                    "velocityY": -0.5712878192047837,
+                    "timestamp": 1.4884697912135156
+                },
+                {
+                    "x": 6.083502386416879,
+                    "y": 3.927762346295825,
+                    "heading": -2.9963234364674123,
+                    "angularVelocity": -0.057159542505398835,
+                    "velocityX": 3.662489066307642,
+                    "velocityY": -0.8573987824891895,
+                    "timestamp": 1.523064670217812
+                },
+                {
+                    "x": 6.208288544878107,
+                    "y": 3.888323197622386,
+                    "heading": -2.9963235773653807,
+                    "angularVelocity": -0.000004072798431777223,
+                    "velocityX": 3.6070702385092757,
+                    "velocityY": -1.1400285189186714,
+                    "timestamp": 1.5576595492221086
+                },
+                {
+                    "x": 6.329706848437896,
+                    "y": 3.8394904257798843,
+                    "heading": -2.9963235979992118,
+                    "angularVelocity": -5.964417674236108e-7,
+                    "velocityX": 3.5097189831104894,
+                    "velocityY": -1.4115607063241151,
+                    "timestamp": 1.592254428226405
+                },
+                {
+                    "x": 6.447051291726402,
+                    "y": 3.7815479813892945,
+                    "heading": -2.996323615206392,
+                    "angularVelocity": -4.973909609914855e-7,
+                    "velocityX": 3.3919599277665977,
+                    "velocityY": -1.6748850135707645,
+                    "timestamp": 1.6268493072307015
+                },
+                {
+                    "x": 6.559639598402797,
+                    "y": 3.714832771082226,
+                    "heading": -2.996323630138998,
+                    "angularVelocity": -4.316421016363579e-7,
+                    "velocityX": 3.2544789840835127,
+                    "velocityY": -1.9284706935608398,
+                    "timestamp": 1.661444186234998
+                },
+                {
+                    "x": 6.666817153320071,
+                    "y": 3.639732715012558,
+                    "heading": -2.996323643537738,
+                    "angularVelocity": -3.8730413434828734e-7,
+                    "velocityX": 3.0980757268717096,
+                    "velocityY": -2.1708431487891944,
+                    "timestamp": 1.6960390652392945
+                },
+                {
+                    "x": 6.767960818192567,
+                    "y": 3.556684498992451,
+                    "heading": -2.996323655916169,
+                    "angularVelocity": -3.578110804784623e-7,
+                    "velocityX": 2.9236600266743116,
+                    "velocityY": -2.400592758534959,
+                    "timestamp": 1.730633944243591
+                },
+                {
+                    "x": 6.862482590576338,
+                    "y": 3.466171063796537,
+                    "heading": -2.99632366765796,
+                    "angularVelocity": -3.394083586636284e-7,
+                    "velocityX": 2.7322475205660415,
+                    "velocityY": -2.6163824762813186,
+                    "timestamp": 1.7652288232478874
+                },
+                {
+                    "x": 6.949833264778536,
+                    "y": 3.3687189870150447,
+                    "heading": -2.9963236790740537,
+                    "angularVelocity": -3.299937460810232e-7,
+                    "velocityX": 2.524959667913537,
+                    "velocityY": -2.8169509356974314,
+                    "timestamp": 1.799823702252184
+                },
+                {
+                    "x": 7.031655596531464,
+                    "y": 3.2665810723307764,
+                    "heading": -2.9963236904142594,
+                    "angularVelocity": -3.2780012680929783e-7,
+                    "velocityX": 2.365157332759168,
+                    "velocityY": -2.9523998240197193,
+                    "timestamp": 1.8344185812564804
+                },
+                {
+                    "x": 7.1134768697956385,
+                    "y": 3.164442309703858,
+                    "heading": -2.9963237017545317,
+                    "angularVelocity": -3.2780205084524347e-7,
+                    "velocityX": 2.3651267360701933,
+                    "velocityY": -2.952424334660453,
+                    "timestamp": 1.8690134602607769
+                },
+                {
+                    "x": 7.194171018365882,
+                    "y": 3.0635712609157633,
+                    "heading": -3.000602693910309,
+                    "angularVelocity": -0.1236886001319993,
+                    "velocityX": 2.3325460557391247,
+                    "velocityY": -2.9157797827698024,
+                    "timestamp": 1.9036083392650733
+                },
+                {
+                    "x": 7.268926895683992,
+                    "y": 2.9702293971794824,
+                    "heading": -3.0147524025112875,
+                    "angularVelocity": -0.4090116516731078,
+                    "velocityX": 2.1608943135435097,
+                    "velocityY": -2.698141066621111,
+                    "timestamp": 1.9382032182693698
+                },
+                {
+                    "x": 7.337464009959094,
+                    "y": 2.8846562411973626,
+                    "heading": -3.031563935832344,
+                    "angularVelocity": -0.4859543899248224,
+                    "velocityX": 1.9811346721747336,
+                    "velocityY": -2.473578704278531,
+                    "timestamp": 1.9727980972736663
+                },
+                {
+                    "x": 7.399764505912731,
+                    "y": 2.806871719273257,
+                    "heading": -3.0489337906921676,
+                    "angularVelocity": -0.5020932392238406,
+                    "velocityX": 1.8008589059062745,
+                    "velocityY": -2.2484403519505154,
+                    "timestamp": 2.0073929762779628
+                },
+                {
+                    "x": 7.455827645980218,
+                    "y": 2.7368758607336336,
+                    "heading": -3.0658680766465114,
+                    "angularVelocity": -0.48950267905953376,
+                    "velocityX": 1.6205618195839024,
+                    "velocityY": -2.023301151911255,
+                    "timestamp": 2.041987855282259
+                },
+                {
+                    "x": 7.505655239118239,
+                    "y": 2.674665896616921,
+                    "heading": -3.081789323798935,
+                    "angularVelocity": -0.4602197669327373,
+                    "velocityX": 1.4403170229857634,
+                    "velocityY": -1.7982419915094008,
+                    "timestamp": 2.0765827342865553
+                },
+                {
+                    "x": 7.549249359478674,
+                    "y": 2.6202389173200062,
+                    "heading": -3.0963205890594514,
+                    "angularVelocity": -0.42004093318875263,
+                    "velocityX": 1.2601321818475304,
+                    "velocityY": -1.5732669361310743,
+                    "timestamp": 2.1111776132908515
+                },
+                {
+                    "x": 7.586611891972334,
+                    "y": 2.573592354245129,
+                    "heading": -3.1091966615790723,
+                    "angularVelocity": -0.37219591136657937,
+                    "velocityX": 1.0800018259644524,
+                    "velocityY": -1.348366128671367,
+                    "timestamp": 2.1457724922951478
+                },
+                {
+                    "x": 7.617744461907291,
+                    "y": 2.5347240241708393,
+                    "heading": -3.1202208856007014,
+                    "angularVelocity": -0.318666355799661,
+                    "velocityX": 0.8999184512566408,
+                    "velocityY": -1.1235284294378556,
+                    "timestamp": 2.180367371299444
+                },
+                {
+                    "x": 7.642648454713618,
+                    "y": 2.5036320816555615,
+                    "heading": -3.1292416557703198,
+                    "angularVelocity": -0.26075449399601563,
+                    "velocityX": 0.7198751238076218,
+                    "velocityY": -0.8987440745613371,
+                    "timestamp": 2.2149622503037403
+                },
+                {
+                    "x": 7.66132505338278,
+                    "y": 2.48031495979902,
+                    "heading": -3.136138533441162,
+                    "angularVelocity": -0.19936123118063845,
+                    "velocityX": 0.5398659919244735,
+                    "velocityY": -0.6740050125235452,
+                    "timestamp": 2.2495571293080365
+                },
+                {
+                    "x": 7.673775274710525,
+                    "y": 2.4647713180548445,
+                    "heading": -3.1408135216098865,
+                    "angularVelocity": -0.1351352657757358,
+                    "velocityX": 0.3598862515518098,
+                    "velocityY": -0.4493047003357149,
+                    "timestamp": 2.284152008312333
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -0.06855886298677888,
+                    "velocityX": 0.17993198613883732,
+                    "velocityY": -0.22463781572640673,
+                    "timestamp": 2.318746887316629
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -2.3105673781771525e-33,
+                    "velocityX": 0,
+                    "velocityY": -9.473748472540581e-34,
+                    "timestamp": 2.3533417663209253
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -5202,28 +12655,19 @@
                         "last"
                     ],
                     "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        2
+                    ],
+                    "type": "WptVelocityDirection",
+                    "direction": 3.14
                 }
             ],
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "W1 To C5": {
             "waypoints": [
@@ -5234,19 +12678,641 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 32
+                },
+                {
+                    "x": 2.382448196411133,
+                    "y": 1.9443897008895874,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 35
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
+                    "heading": -3.143185307179586,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": -2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": -2.713469488843566e-34,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.2985074122681866,
+                    "y": 6.738728579173207,
+                    "heading": -2.7557122969387127,
+                    "angularVelocity": -0.12159424626473993,
+                    "velocityX": -0.03177182176995015,
+                    "velocityY": -0.38893280014262377,
+                    "timestamp": 0.046978349010656624
+                },
+                {
+                    "x": 2.2955230171267877,
+                    "y": 6.702182881977796,
+                    "heading": -2.7670222648124514,
+                    "angularVelocity": -0.24074851738984976,
+                    "velocityX": -0.06352703328764883,
+                    "velocityY": -0.777926384495132,
+                    "timestamp": 0.09395669802131325
+                },
+                {
+                    "x": 2.2910478333318625,
+                    "y": 6.647359316893119,
+                    "heading": -2.7837824265023454,
+                    "angularVelocity": -0.3567635313470153,
+                    "velocityX": -0.09526055915480924,
+                    "velocityY": -1.1669964193982714,
+                    "timestamp": 0.14093504703196988
+                },
+                {
+                    "x": 2.2850832407725172,
+                    "y": 6.574253215154707,
+                    "heading": -2.805794841014387,
+                    "angularVelocity": -0.46856509382754397,
+                    "velocityX": -0.12696471214840174,
+                    "velocityY": -1.556165835496437,
+                    "timestamp": 0.1879133960426265
+                },
+                {
+                    "x": 2.2776312180066953,
+                    "y": 6.482858263005064,
+                    "heading": -2.832779187520957,
+                    "angularVelocity": -0.5743996346156903,
+                    "velocityX": -0.15862674876316962,
+                    "velocityY": -1.945469648771827,
+                    "timestamp": 0.2348917450532831
+                },
+                {
+                    "x": 2.2686948542910046,
+                    "y": 6.373165502071328,
+                    "heading": -2.864308391554441,
+                    "angularVelocity": -0.6711432968053561,
+                    "velocityX": -0.1902230262213726,
+                    "velocityY": -2.3349641535689947,
+                    "timestamp": 0.28187009406393976
+                },
+                {
+                    "x": 2.2582796451376828,
+                    "y": 6.245161498589565,
+                    "heading": -2.8996563968568023,
+                    "angularVelocity": -0.7524318339570105,
+                    "velocityX": -0.22170232399947937,
+                    "velocityY": -2.7247446148592136,
+                    "timestamp": 0.3288484430745964
+                },
+                {
+                    "x": 2.2463978083089957,
+                    "y": 6.098825641295113,
+                    "heading": -2.9373287044127423,
+                    "angularVelocity": -0.8019078649910463,
+                    "velocityX": -0.2529215495842953,
+                    "velocityY": -3.1149638158049613,
+                    "timestamp": 0.375826792085253
+                },
+                {
+                    "x": 2.23309481763283,
+                    "y": 5.934156164582871,
+                    "heading": -2.9725027104068498,
+                    "angularVelocity": -0.7487280148165356,
+                    "velocityX": -0.2831728010098399,
+                    "velocityY": -3.505220600129828,
+                    "timestamp": 0.4228051410959096
+                },
+                {
+                    "x": 2.2185299400469667,
+                    "y": 5.757037726236599,
+                    "heading": -2.9725027230810714,
+                    "angularVelocity": -2.697885773041044e-7,
+                    "velocityX": -0.31003383244820454,
+                    "velocityY": -3.77021419603515,
+                    "timestamp": 0.4697834901065662
+                },
+                {
+                    "x": 2.2039649895163667,
+                    "y": 5.57991929388838,
+                    "heading": -2.972502735754471,
+                    "angularVelocity": -2.6977108201545366e-7,
+                    "velocityX": -0.3100353851791582,
+                    "velocityY": -3.7702140683582024,
+                    "timestamp": 0.5167618391172228
+                },
+                {
+                    "x": 2.189400055868242,
+                    "y": 5.402800860151866,
+                    "heading": -2.972502748427869,
+                    "angularVelocity": -2.6977103310705994e-7,
+                    "velocityX": -0.31003502581200326,
+                    "velocityY": -3.770214097910002,
+                    "timestamp": 0.5637401881278795
+                },
+                {
+                    "x": 2.174835140412249,
+                    "y": 5.225682424919363,
+                    "heading": -2.9725027611012638,
+                    "angularVelocity": -2.69770971237986e-7,
+                    "velocityX": -0.3100346385669985,
+                    "velocityY": -3.7702141297542338,
+                    "timestamp": 0.6107185371385362
+                },
+                {
+                    "x": 2.1602702444212443,
+                    "y": 5.048563988086202,
+                    "heading": -2.972502773774655,
+                    "angularVelocity": -2.697708934411251e-7,
+                    "velocityX": -0.31003422422742555,
+                    "velocityY": -3.770214163826477,
+                    "timestamp": 0.6576968861491929
+                },
+                {
+                    "x": 2.145705369054558,
+                    "y": 4.87144554955705,
+                    "heading": -2.972502786448042,
+                    "angularVelocity": -2.697707995800925e-7,
+                    "velocityX": -0.31003378520991653,
+                    "velocityY": -3.7702141999279966,
+                    "timestamp": 0.7046752351598495
+                },
+                {
+                    "x": 2.1311405152628695,
+                    "y": 4.694327109253736,
+                    "heading": -2.9725027991214237,
+                    "angularVelocity": -2.697706911872684e-7,
+                    "velocityX": -0.31003332595584043,
+                    "velocityY": -3.7702142376935592,
+                    "timestamp": 0.7516535841705062
+                },
+                {
+                    "x": 2.116575683697044,
+                    "y": 4.5172086671227385,
+                    "heading": -2.9725028117947994,
+                    "angularVelocity": -2.6977056864766263e-7,
+                    "velocityX": -0.3100328528472012,
+                    "velocityY": -3.77021427659835,
+                    "timestamp": 0.7986319331811629
+                },
+                {
+                    "x": 2.102010874647989,
+                    "y": 4.340090223140139,
+                    "heading": -2.9725028244681693,
+                    "angularVelocity": -2.697704390106331e-7,
+                    "velocityX": -0.3100323735461829,
+                    "velocityY": -3.7702143160122867,
+                    "timestamp": 0.8456102821918196
+                },
+                {
+                    "x": 2.087446088035901,
+                    "y": 4.162971777312504,
+                    "heading": -2.9725028371415325,
+                    "angularVelocity": -2.697703054465384e-7,
+                    "velocityX": -0.3100318959438893,
+                    "velocityY": -3.7702143552864653,
+                    "timestamp": 0.8925886312024762
+                },
+                {
+                    "x": 2.0728813234455363,
+                    "y": 3.9858533296739824,
+                    "heading": -2.97250284981489,
+                    "angularVelocity": -2.69770174050259e-7,
+                    "velocityX": -0.31003142718065585,
+                    "velocityY": -3.770214393833725,
+                    "timestamp": 0.9395669802131329
+                },
+                {
+                    "x": 2.0583165802454695,
+                    "y": 3.8087348802765,
+                    "heading": -2.972502862488243,
+                    "angularVelocity": -2.697700772639611e-7,
+                    "velocityX": -0.3100309718581832,
+                    "velocityY": -3.770214431275663,
+                    "timestamp": 0.9865453292237896
+                },
+                {
+                    "x": 2.0437518576774885,
+                    "y": 3.631616429182409,
+                    "heading": -2.972502875161594,
+                    "angularVelocity": -2.6977003935784156e-7,
+                    "velocityX": -0.31003053267531316,
+                    "velocityY": -3.7702144673903546,
+                    "timestamp": 1.0335236782344461
+                },
+                {
+                    "x": 2.02918715486261,
+                    "y": 3.4544979764639927,
+                    "heading": -2.972502887834946,
+                    "angularVelocity": -2.697700540709368e-7,
+                    "velocityX": -0.31003011220283416,
+                    "velocityY": -3.770214501966405,
+                    "timestamp": 1.0805020272451027
+                },
+                {
+                    "x": 2.014622470919118,
+                    "y": 3.2773795221937574,
+                    "heading": -2.972502900508301,
+                    "angularVelocity": -2.6977012019750386e-7,
+                    "velocityX": -0.3100297104989377,
+                    "velocityY": -3.770214534999038,
+                    "timestamp": 1.1274803762557593
+                },
+                {
+                    "x": 2.000060409435934,
+                    "y": 3.1002608522940482,
+                    "heading": -2.972502913181681,
+                    "angularVelocity": -2.697706573776715e-7,
+                    "velocityX": -0.30997388775603324,
+                    "velocityY": -3.77021912497458,
+                    "timestamp": 1.1744587252664158
+                },
+                {
+                    "x": 2.001608653690626,
+                    "y": 2.9225513149097844,
+                    "heading": -2.972502926099156,
+                    "angularVelocity": -2.749665584566826e-7,
+                    "velocityX": 0.03295654886341899,
+                    "velocityY": -3.782796567498611,
+                    "timestamp": 1.2214370742770724
+                },
+                {
+                    "x": 2.0215247790201643,
+                    "y": 2.7459545305120523,
+                    "heading": -2.9725029398381673,
+                    "angularVelocity": -2.9245411249129853e-7,
+                    "velocityX": 0.4239426405772788,
+                    "velocityY": -3.759110060629698,
+                    "timestamp": 1.268415423287729
+                },
+                {
+                    "x": 2.0595955339413297,
+                    "y": 2.572363939972743,
+                    "heading": -2.9725029551587165,
+                    "angularVelocity": -3.2611936426906016e-7,
+                    "velocityX": 0.8103893755936399,
+                    "velocityY": -3.695119011098735,
+                    "timestamp": 1.3153937722983855
+                },
+                {
+                    "x": 2.1154127951527757,
+                    "y": 2.4036407715704846,
+                    "heading": -2.972502973192088,
+                    "angularVelocity": -3.8386557912539916e-7,
+                    "velocityX": 1.1881486341460188,
+                    "velocityY": -3.5915091091001314,
+                    "timestamp": 1.362372121309042
+                },
+                {
+                    "x": 2.1883781192124445,
+                    "y": 2.241594069488506,
+                    "heading": -2.9725029958666975,
+                    "angularVelocity": -4.826608466149902e-7,
+                    "velocityX": 1.5531691853010408,
+                    "velocityY": -3.4493911662416843,
+                    "timestamp": 1.4093504703196986
+                },
+                {
+                    "x": 2.277709186746539,
+                    "y": 2.0879612953868114,
+                    "heading": -2.9725030269682566,
+                    "angularVelocity": -6.620402725580348e-7,
+                    "velocityX": 1.901536972145005,
+                    "velocityY": -3.2702889168549634,
+                    "timestamp": 1.4563288193303552
+                },
+                {
+                    "x": 2.382448196411133,
+                    "y": 1.9443897008895872,
+                    "heading": -2.9725030765065807,
+                    "angularVelocity": -0.0000010544926575738783,
+                    "velocityX": 2.2295166150014207,
+                    "velocityY": -3.0561226079838564,
+                    "timestamp": 1.5033071683410117
+                },
+                {
+                    "x": 2.5069799488821,
+                    "y": 1.8074960632068364,
+                    "heading": -2.9725031217478985,
+                    "angularVelocity": -9.247979619471774e-7,
+                    "velocityX": 2.5456091262787446,
+                    "velocityY": -2.798303938555362,
+                    "timestamp": 1.5522273859759206
+                },
+                {
+                    "x": 2.645527130072252,
+                    "y": 1.6848065781413901,
+                    "heading": -2.9725031505695743,
+                    "angularVelocity": -5.891567373541898e-7,
+                    "velocityX": 2.8321047593068385,
+                    "velocityY": -2.507950516105164,
+                    "timestamp": 1.6011476036108294
+                },
+                {
+                    "x": 2.7964788901009507,
+                    "y": 1.5777477131393498,
+                    "heading": -2.972503171586243,
+                    "angularVelocity": -4.2961111591525744e-7,
+                    "velocityX": 3.085672291060715,
+                    "velocityY": -2.188438036008342,
+                    "timestamp": 1.6500678212457383
+                },
+                {
+                    "x": 2.9580801560836862,
+                    "y": 1.4875641931626062,
+                    "heading": -2.9725031883859017,
+                    "angularVelocity": -3.434093106618729e-7,
+                    "velocityX": 3.3033635947567466,
+                    "velocityY": -1.8434815774897553,
+                    "timestamp": 1.698988038880647
+                },
+                {
+                    "x": 3.1284520354522702,
+                    "y": 1.4153045196097347,
+                    "heading": -2.9725032027821,
+                    "angularVelocity": -2.9427911753262145e-7,
+                    "velocityX": 3.482647616984601,
+                    "velocityY": -1.4770922339746126,
+                    "timestamp": 1.747908256515556
+                },
+                {
+                    "x": 3.305613655866368,
+                    "y": 1.3618087375272205,
+                    "heading": -2.9725032158448177,
+                    "angularVelocity": -2.670208303687875e-7,
+                    "velocityX": 3.6214397437119468,
+                    "velocityY": -1.0935311547825342,
+                    "timestamp": 1.7968284741504648
+                },
+                {
+                    "x": 3.4875051596049977,
+                    "y": 1.327698333409946,
+                    "heading": -2.9725032283101815,
+                    "angularVelocity": -2.548100678766224e-7,
+                    "velocityX": 3.7181253995246735,
+                    "velocityY": -0.6972659928016005,
+                    "timestamp": 1.8457486917853736
+                },
+                {
+                    "x": 3.671044139168993,
+                    "y": 1.3040027251815136,
+                    "heading": -2.972503240670167,
+                    "angularVelocity": -2.5265597445839956e-7,
+                    "velocityX": 3.751802188079885,
+                    "velocityY": -0.48437250229081197,
+                    "timestamp": 1.8946689094202824
+                },
+                {
+                    "x": 3.854583175253026,
+                    "y": 1.2803075547441987,
+                    "heading": -2.972503253030148,
+                    "angularVelocity": -2.5265589039445086e-7,
+                    "velocityX": 3.751803343431197,
+                    "velocityY": -0.48436355320721586,
+                    "timestamp": 1.9435891270551913
+                },
+                {
+                    "x": 4.038122213757311,
+                    "y": 1.256612403053765,
+                    "heading": -2.972503265390126,
+                    "angularVelocity": -2.5265581959003427e-7,
+                    "velocityX": 3.751803392904627,
+                    "velocityY": -0.4843631699938504,
+                    "timestamp": 1.9925093446901
+                },
+                {
+                    "x": 4.221661254747456,
+                    "y": 1.2329172706184417,
+                    "heading": -2.9725032777500995,
+                    "angularVelocity": -2.526557374147453e-7,
+                    "velocityX": 3.7518034437192385,
+                    "velocityY": -0.48436277639154923,
+                    "timestamp": 2.041429562325009
+                },
+                {
+                    "x": 4.405200298259638,
+                    "y": 1.2092221577184428,
+                    "heading": -2.972503290110069,
+                    "angularVelocity": -2.526556474640549e-7,
+                    "velocityX": 3.7518034952732924,
+                    "velocityY": -0.4843623770612628,
+                    "timestamp": 2.090349779959918
+                },
+                {
+                    "x": 4.5887393442969575,
+                    "y": 1.1855270643778184,
+                    "heading": -2.9725033024700336,
+                    "angularVelocity": -2.526555535451317e-7,
+                    "velocityX": 3.7518035468907667,
+                    "velocityY": -0.4843619772393562,
+                    "timestamp": 2.1392699975948273
+                },
+                {
+                    "x": 4.772278392830339,
+                    "y": 1.1618319903713676,
+                    "heading": -2.9725033148299937,
+                    "angularVelocity": -2.5265545524016583e-7,
+                    "velocityX": 3.7518035979138844,
+                    "velocityY": -0.48436158202088675,
+                    "timestamp": 2.1881902152297363
+                },
+                {
+                    "x": 4.955817443801929,
+                    "y": 1.1381369352509825,
+                    "heading": -2.972503327189949,
+                    "angularVelocity": -2.526553571562985e-7,
+                    "velocityX": 3.751803647754394,
+                    "velocityY": -0.4843611959623946,
+                    "timestamp": 2.2371104328646454
+                },
+                {
+                    "x": 5.139356497135768,
+                    "y": 1.114441898428302,
+                    "heading": -2.9725033395499,
+                    "angularVelocity": -2.526552791781282e-7,
+                    "velocityX": 3.751803696042169,
+                    "velocityY": -0.48436082193084695,
+                    "timestamp": 2.2860306504995545
+                },
+                {
+                    "x": 5.322895552747796,
+                    "y": 1.0907468792522317,
+                    "heading": -2.97250335190985,
+                    "angularVelocity": -2.5265524884530903e-7,
+                    "velocityX": 3.7518037426116595,
+                    "velocityY": -0.4843604612086112,
+                    "timestamp": 2.3349508681344635
+                },
+                {
+                    "x": 5.506434610543511,
+                    "y": 1.067051876990783,
+                    "heading": -2.9725033642697998,
+                    "angularVelocity": -2.5265526208757776e-7,
+                    "velocityX": 3.7518037872493903,
+                    "velocityY": -0.4843601154492814,
+                    "timestamp": 2.3838710857693726
+                },
+                {
+                    "x": 5.689973670421635,
+                    "y": 1.043356890859468,
+                    "heading": -2.972503376629753,
+                    "angularVelocity": -2.52655316177873e-7,
+                    "velocityX": 3.751803829816819,
+                    "velocityY": -0.48435978572601984,
+                    "timestamp": 2.4327913034042816
+                },
+                {
+                    "x": 5.873512732277465,
+                    "y": 1.0196619200472952,
+                    "heading": -2.9725033889897103,
+                    "angularVelocity": -2.526554076637863e-7,
+                    "velocityX": 3.751803870244009,
+                    "velocityY": -0.4843594725805982,
+                    "timestamp": 2.4817115210391907
+                },
+                {
+                    "x": 6.057051796000616,
+                    "y": 0.9959669636992331,
+                    "heading": -2.9725034013496736,
+                    "angularVelocity": -2.5265551720355475e-7,
+                    "velocityX": 3.751803908414743,
+                    "velocityY": -0.48435917691325625,
+                    "timestamp": 2.5306317386740997
+                },
+                {
+                    "x": 6.240590861474878,
+                    "y": 0.972272020915138,
+                    "heading": -2.9725034137096418,
+                    "angularVelocity": -2.526556193143771e-7,
+                    "velocityX": 3.7518039442099766,
+                    "velocityY": -0.48435889964615636,
+                    "timestamp": 2.579551956309009
+                },
+                {
+                    "x": 6.424129928585653,
+                    "y": 0.9485770908073501,
+                    "heading": -2.972503426069614,
+                    "angularVelocity": -2.5265571512549996e-7,
+                    "velocityX": 3.7518039776626577,
+                    "velocityY": -0.4843586405241034,
+                    "timestamp": 2.628472173943918
+                },
+                {
+                    "x": 6.607668997222064,
+                    "y": 0.9248821725170546,
+                    "heading": -2.972503438429591,
+                    "angularVelocity": -2.526558029491111e-7,
+                    "velocityX": 3.7518040088489135,
+                    "velocityY": -0.4843583989574683,
+                    "timestamp": 2.677392391578827
+                },
+                {
+                    "x": 6.791208068059123,
+                    "y": 0.9011872712731527,
+                    "heading": -2.972503450789653,
+                    "angularVelocity": -2.5265754822248404e-7,
+                    "velocityX": 3.751804053833296,
+                    "velocityY": -0.48435805050452835,
+                    "timestamp": 2.726312609213736
+                },
+                {
+                    "x": 6.968893183591214,
+                    "y": 0.8784347773708095,
+                    "heading": -2.9889996672823953,
+                    "angularVelocity": -0.3372065229932763,
+                    "velocityX": 3.632140741035789,
+                    "velocityY": -0.4650938814733942,
+                    "timestamp": 2.775232826848645
+                },
+                {
+                    "x": 7.126993065244261,
+                    "y": 0.8581341417430071,
+                    "heading": -3.0160355228350078,
+                    "angularVelocity": -0.5526519884760304,
+                    "velocityX": 3.231790235132359,
+                    "velocityY": -0.4149743523077879,
+                    "timestamp": 2.824153044483554
+                },
+                {
+                    "x": 7.265300475634767,
+                    "y": 0.8403578347966174,
+                    "heading": -3.044055057812944,
+                    "angularVelocity": -0.5727598185896393,
+                    "velocityX": 2.827203497390216,
+                    "velocityY": -0.363373423214384,
+                    "timestamp": 2.873073262118463
+                },
+                {
+                    "x": 7.3838209572244,
+                    "y": 0.8251160468222167,
+                    "heading": -3.0703114433567618,
+                    "angularVelocity": -0.5367184941769645,
+                    "velocityX": 2.4227300555805176,
+                    "velocityY": -0.31156418984375384,
+                    "timestamp": 2.921993479753372
+                },
+                {
+                    "x": 7.4825676028202075,
+                    "y": 0.812412258635403,
+                    "heading": -3.093486935010673,
+                    "angularVelocity": -0.47374056728180086,
+                    "velocityX": 2.018524249682452,
+                    "velocityY": -0.25968380356812476,
+                    "timestamp": 2.9709136973882813
+                },
+                {
+                    "x": 7.561551267898866,
+                    "y": 0.8022480583211733,
+                    "heading": -3.1128054575324193,
+                    "angularVelocity": -0.39489854002532226,
+                    "velocityX": 1.6145403454276093,
+                    "velocityY": -0.20777095453836575,
+                    "timestamp": 3.0198339150231903
+                },
+                {
+                    "x": 7.62078038482921,
+                    "y": 0.7946243025369127,
+                    "heading": -3.1277548992054633,
+                    "angularVelocity": -0.30558820863414116,
+                    "velocityX": 1.210728810987095,
+                    "velocityY": -0.1558405941927034,
+                    "timestamp": 3.0687541326580994
+                },
+                {
+                    "x": 7.6602615463029124,
+                    "y": 0.7895415063368129,
+                    "heading": -3.137971889882548,
+                    "angularVelocity": -0.208850065903912,
+                    "velocityX": 0.8070520406992162,
+                    "velocityY": -0.10389970539445781,
+                    "timestamp": 3.1176743502930084
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": -0.10656978952845035,
+                    "velocityX": 0.40348254058055355,
+                    "velocityY": -0.051952065213203134,
+                    "timestamp": 3.1665945679279175
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": -3.143185307179586,
+                    "angularVelocity": 1.1636456429625425e-30,
+                    "velocityX": -1.050271820667376e-32,
+                    "velocityY": -1.190420457247715e-32,
+                    "timestamp": 3.2155147855628265
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -5266,19 +13332,9 @@
             "usesDefaultFieldObstacles": true,
             "circleObstacles": [
                 {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
+                    "x": 4.816418571472168,
+                    "y": 4.104055919647217,
+                    "radius": 1.7841171466850674
                 }
             ]
         },
@@ -5602,22 +13658,167 @@
                     "x": 2.25,
                     "y": 5.56,
                     "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 3.1925891483075246e-34,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.3070751616194327,
+                    "y": 5.580871513694386,
+                    "heading": 3.1487661024318196,
+                    "angularVelocity": 0.1026391987797076,
+                    "velocityX": 0.668272918826128,
+                    "velocityY": 0.24437718582154933,
+                    "timestamp": 0.08540696474681245
+                },
+                {
+                    "x": 2.4212254758610405,
+                    "y": 5.622614541188738,
+                    "heading": 3.166299183933703,
+                    "angularVelocity": 0.20528866180714583,
+                    "velocityX": 1.3365457323065477,
+                    "velocityY": 0.48875437287928064,
+                    "timestamp": 0.1708139294936249
+                },
+                {
+                    "x": 2.592450926566031,
+                    "y": 5.685229083532584,
+                    "heading": 3.1926007136716263,
+                    "angularVelocity": 0.307955326780356,
+                    "velocityX": 2.0048183565893676,
+                    "velocityY": 0.7331315722255889,
+                    "timestamp": 0.25622089424043737
+                },
+                {
+                    "x": 2.820751488987388,
+                    "y": 5.768715144253157,
+                    "heading": 3.2276726330952217,
+                    "angularVelocity": 0.4106447235019468,
+                    "velocityX": 2.6730906911181096,
+                    "velocityY": 0.9775088128710195,
+                    "timestamp": 0.3416278589872498
+                },
+                {
+                    "x": 3.106127123163594,
+                    "y": 5.873072729004609,
+                    "heading": 3.271517021699339,
+                    "angularVelocity": 0.5133584682946327,
+                    "velocityX": 3.341362557751549,
+                    "velocityY": 1.2218861197188997,
+                    "timestamp": 0.4270348237340622
+                },
+                {
+                    "x": 3.4095640006306436,
+                    "y": 5.984036075660027,
+                    "heading": 3.2715170269491542,
+                    "angularVelocity": 6.146823646853408e-8,
+                    "velocityX": 3.5528352794948703,
+                    "velocityY": 1.2992306538976923,
+                    "timestamp": 0.5124417884808746
+                },
+                {
+                    "x": 3.7130008780817154,
+                    "y": 6.094999422359182,
+                    "heading": 3.271517032198949,
+                    "angularVelocity": 6.146799253439994e-8,
+                    "velocityX": 3.5528352793077906,
+                    "velocityY": 1.299230654409779,
+                    "timestamp": 0.597848753227687
+                },
+                {
+                    "x": 4.016437755532787,
+                    "y": 6.205962769058336,
+                    "heading": 3.271517037448744,
+                    "angularVelocity": 6.146799251320457e-8,
+                    "velocityX": 3.55283527930779,
+                    "velocityY": 1.2992306544097794,
+                    "timestamp": 0.6832557179744995
+                },
+                {
+                    "x": 4.319874632996578,
+                    "y": 6.3169261157226675,
+                    "heading": 3.2715170426985596,
+                    "angularVelocity": 6.146823670561877e-8,
+                    "velocityX": 3.5528352794567044,
+                    "velocityY": 1.2992306540020593,
+                    "timestamp": 0.7686626827213119
+                },
+                {
+                    "x": 4.605249758095724,
+                    "y": 6.421284043313642,
+                    "heading": 3.315404813019647,
+                    "angularVelocity": 0.5138664095040972,
+                    "velocityX": 3.3413565971479624,
+                    "velocityY": 1.221890133905845,
+                    "timestamp": 0.8540696474681243
+                },
+                {
+                    "x": 4.833549856620357,
+                    "y": 6.504770412469219,
+                    "heading": 3.3505164790016466,
+                    "angularVelocity": 0.41111010192303765,
+                    "velocityX": 2.673085259515143,
+                    "velocityY": 0.9775124242275821,
+                    "timestamp": 0.9394766122149367
+                },
+                {
+                    "x": 5.004774927843149,
+                    "y": 6.567385202446666,
+                    "heading": 3.3768507903488065,
+                    "angularVelocity": 0.3083391550704053,
+                    "velocityX": 2.0048139133662572,
+                    "velocityY": 0.7331344716800138,
+                    "timestamp": 1.0248835769617493
+                },
+                {
+                    "x": 5.118924975241942,
+                    "y": 6.609128400529722,
+                    "heading": 3.3944071451755278,
+                    "angularVelocity": 0.20556116095176463,
+                    "velocityX": 1.3365426079382265,
+                    "velocityY": 0.4887563702422027,
+                    "timestamp": 1.1102905417085618
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
                     "heading": 3.4031853071795863,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "angularVelocity": 0.10278040005380541,
+                    "velocityX": 0.6682713163646097,
+                    "velocityY": 0.24437819014118428,
+                    "timestamp": 1.1956975064553743
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 3.4031853071795863,
+                    "angularVelocity": -1.3221581062655587e-34,
+                    "velocityX": 2.469071735487476e-35,
+                    "velocityY": 0,
+                    "timestamp": 1.2811044712021868
                 }
             ],
-            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -5635,23 +13836,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "W2 To S2": {
             "waypoints": [
@@ -5679,100 +13864,100 @@
                     "x": 2.25,
                     "y": 5.56,
                     "heading": 3.14,
-                    "angularVelocity": 1.3281175985971763e-33,
-                    "velocityX": 1.9314152785727527e-39,
-                    "velocityY": 8.278387842337126e-35,
+                    "angularVelocity": -1.2603829918079947e-39,
+                    "velocityX": 0,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.3319036557469475,
-                    "y": 5.560000044140862,
-                    "heading": 3.140000009517521,
-                    "angularVelocity": 9.602993340973177e-8,
-                    "velocityX": 0.8263919813281884,
-                    "velocityY": 4.4537273180015373e-7,
-                    "timestamp": 0.09910993523353247
+                    "x": 2.3319036557469652,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 1.854813108024866e-16,
+                    "velocityX": 0.8263919813283321,
+                    "velocityY": 1.2609253605375368e-21,
+                    "timestamp": 0.099109935233537
                 },
                 {
-                    "x": 2.495710964163716,
-                    "y": 5.5600001253696005,
-                    "heading": 3.140000023792121,
-                    "angularVelocity": 1.44027928091657e-7,
-                    "velocityX": 1.6527839316087705,
-                    "velocityY": 8.195821841849307e-7,
-                    "timestamp": 0.19821987046706493
+                    "x": 2.4957109641637625,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 3.709616393980982e-16,
+                    "velocityX": 1.6527839316089838,
+                    "velocityY": 2.5218434283644083e-21,
+                    "timestamp": 0.198219870467074
                 },
                 {
-                    "x": 2.741421919096052,
-                    "y": 5.5600002360460925,
-                    "heading": 3.140000035820423,
-                    "angularVelocity": 1.213632154532524e-7,
-                    "velocityX": 2.479175819794127,
-                    "velocityY": 0.0000011167042923727,
-                    "timestamp": 0.2973298057005974
+                    "x": 2.741421919096125,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 5.564379296417211e-16,
+                    "velocityX": 2.4791758197942864,
+                    "velocityY": 3.782763291910568e-21,
+                    "timestamp": 0.297329805700611
                 },
                 {
-                    "x": 3.0690365020812136,
-                    "y": 5.5600003677528065,
-                    "heading": 3.140000035237491,
-                    "angularVelocity": -5.88169262513392e-9,
-                    "velocityX": 3.305567521694012,
-                    "velocityY": 0.0000013288951255827482,
-                    "timestamp": 0.39643974093412987
+                    "x": 3.0690365020812695,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 7.41869431396362e-16,
+                    "velocityX": 3.3055675216936846,
+                    "velocityY": 5.0436652946129374e-21,
+                    "timestamp": 0.396439740934148
                 },
                 {
-                    "x": 3.443963497918667,
-                    "y": 5.560000377607592,
-                    "heading": 3.140000035237483,
-                    "angularVelocity": -8.264095134625168e-14,
-                    "velocityX": 3.782940579610041,
-                    "velocityY": 9.943285507786426e-8,
-                    "timestamp": 0.4955496761676623
+                    "x": 3.4439634979187304,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 5.10358456113492e-16,
+                    "velocityX": 3.7829405796099507,
+                    "velocityY": -2.5218394750934443e-20,
+                    "timestamp": 0.495549676167685
                 },
                 {
-                    "x": 3.771578080903882,
-                    "y": 5.560000246492901,
-                    "heading": 3.1399999920753214,
-                    "angularVelocity": -4.354978004755772e-7,
-                    "velocityX": 3.305567521694553,
-                    "velocityY": -0.0000013229217178930416,
-                    "timestamp": 0.5946596114011948
+                    "x": 3.771578080903875,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 7.390704390186125e-16,
+                    "velocityX": 3.3055675216936846,
+                    "velocityY": 5.043665294641482e-21,
+                    "timestamp": 0.594659611401222
                 },
                 {
-                    "x": 4.017289035836251,
-                    "y": 5.560000133124366,
-                    "heading": 3.1399999814016817,
-                    "angularVelocity": -1.0769493378117993e-7,
-                    "velocityX": 2.4791758197944644,
-                    "velocityY": -0.000001143866475595614,
-                    "timestamp": 0.6937695466347273
+                    "x": 4.017289035836238,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 5.549819222123311e-16,
+                    "velocityX": 2.479175819794286,
+                    "velocityY": 3.782763291928162e-21,
+                    "timestamp": 0.6937695466347591
                 },
                 {
-                    "x": 4.181096344253041,
-                    "y": 5.560000047628769,
-                    "heading": 3.1399999890121935,
-                    "angularVelocity": 7.678859424954975e-8,
-                    "velocityX": 1.6527839316089916,
-                    "velocityY": -8.626339566508034e-7,
-                    "timestamp": 0.7928794818682598
+                    "x": 4.181096344253034,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 3.701778935205264e-16,
+                    "velocityX": 1.6527839316089838,
+                    "velocityY": 2.5218434283731754e-21,
+                    "timestamp": 0.792879481868296
                 },
                 {
                     "x": 4.263,
                     "y": 5.56,
                     "heading": 3.14,
-                    "angularVelocity": 1.1086483608499845e-7,
-                    "velocityX": 0.8263919813282989,
-                    "velocityY": -4.805650223133441e-7,
-                    "timestamp": 0.8919894171017924
+                    "angularVelocity": 1.8514488061077656e-16,
+                    "velocityX": 0.8263919813283321,
+                    "velocityY": 1.260925360546343e-21,
+                    "timestamp": 0.891989417101833
                 },
                 {
                     "x": 4.263,
                     "y": 5.56,
                     "heading": 3.14,
                     "angularVelocity": 0,
-                    "velocityX": 2.938735877055719e-39,
-                    "velocityY": -3.0539806085263666e-33,
-                    "timestamp": 0.9910993523353249
+                    "velocityX": 0,
+                    "velocityY": 3.337507404137172e-39,
+                    "timestamp": 0.9910993523353699
                 }
             ],
             "constraints": [
@@ -5792,23 +13977,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "W2 To S3": {
             "waypoints": [
@@ -5819,7 +13988,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 28
+                },
+                {
+                    "x": 2.2392399311065674,
+                    "y": 2.659268856048584,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 23
                 },
                 {
                     "x": 4.263,
@@ -5831,7 +14009,476 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 1.712760733525474e-32,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.2447404981946972,
+                    "y": 5.548452449977017,
+                    "heading": 3.1382782323729086,
+                    "angularVelocity": -0.0441204598358695,
+                    "velocityX": -0.13477523592974824,
+                    "velocityY": -0.2959070718806661,
+                    "timestamp": 0.03902424484008447
+                },
+                {
+                    "x": 2.2342214959104107,
+                    "y": 5.525357349752352,
+                    "heading": 3.134834682439341,
+                    "angularVelocity": -0.08824129583236773,
+                    "velocityX": -0.269550437872442,
+                    "velocityY": -0.5918141483404872,
+                    "timestamp": 0.07804848968016893
+                },
+                {
+                    "x": 2.218442994608338,
+                    "y": 5.490714699360087,
+                    "heading": 3.129669313928024,
+                    "angularVelocity": -0.13236306128370928,
+                    "velocityX": -0.40432560237181914,
+                    "velocityY": -0.8877212239269984,
+                    "timestamp": 0.11707273452025341
+                },
+                {
+                    "x": 2.1974049958845625,
+                    "y": 5.44452449907313,
+                    "heading": 3.1227820694139163,
+                    "angularVelocity": -0.17648629825721382,
+                    "velocityX": -0.539100725971382,
+                    "velocityY": -1.1836282925201553,
+                    "timestamp": 0.15609697936033787
+                },
+                {
+                    "x": 2.17110750147078,
+                    "y": 5.386786749443186,
+                    "heading": 3.114172871086463,
+                    "angularVelocity": -0.22061152913360044,
+                    "velocityX": -0.6738758051961007,
+                    "velocityY": -1.479535346975897,
+                    "timestamp": 0.19512122420042233
+                },
+                {
+                    "x": 2.139550513239859,
+                    "y": 5.317501451364342,
+                    "heading": 3.10384162215273,
+                    "angularVelocity": -0.2647392403381948,
+                    "velocityX": -0.8086508364284021,
+                    "velocityY": -1.775442378520474,
+                    "timestamp": 0.2341454690405068
+                },
+                {
+                    "x": 2.1027340332257456,
+                    "y": 5.236668606182096,
+                    "heading": 3.091788209481888,
+                    "angularVelocity": -0.30886985053106986,
+                    "velocityX": -0.9434258155405923,
+                    "velocityY": -2.0713493755866326,
+                    "timestamp": 0.27316971388059125
+                },
+                {
+                    "x": 2.0606580636823133,
+                    "y": 5.1442882159029235,
+                    "heading": 3.078012508890752,
+                    "angularVelocity": -0.35300364292993813,
+                    "velocityX": -1.078200736897071,
+                    "velocityY": -2.3672563212366358,
+                    "timestamp": 0.31219395872067573
+                },
+                {
+                    "x": 2.013322607256609,
+                    "y": 5.040360283671343,
+                    "heading": 3.0625143970359345,
+                    "angularVelocity": -0.3971405960148595,
+                    "velocityX": -1.212975590422751,
+                    "velocityY": -2.6631631863080094,
+                    "timestamp": 0.3512182035607602
+                },
+                {
+                    "x": 1.960727667582776,
+                    "y": 4.92488481518758,
+                    "heading": 3.0452937850632114,
+                    "angularVelocity": -0.44127982599779075,
+                    "velocityX": -1.3477503508231259,
+                    "velocityY": -2.9590699053106113,
+                    "timestamp": 0.3902424484008447
+                },
+                {
+                    "x": 1.902873252468991,
+                    "y": 4.797861825841284,
+                    "heading": 3.026350773365474,
+                    "angularVelocity": -0.4854164834032721,
+                    "velocityX": -1.482524911138253,
+                    "velocityY": -3.25497622995194,
+                    "timestamp": 0.4292666932409292
+                },
+                {
+                    "x": 1.8416825412927555,
+                    "y": 4.663514351826141,
+                    "heading": 3.02635076853487,
+                    "angularVelocity": -1.2378468871067517e-7,
+                    "velocityX": -1.5680178162827485,
+                    "velocityY": -3.4426668489212484,
+                    "timestamp": 0.4682909380810137
+                },
+                {
+                    "x": 1.7807776273702332,
+                    "y": 4.529037073556694,
+                    "heading": 3.0263507637038423,
+                    "angularVelocity": -1.237955425988301e-7,
+                    "velocityX": -1.5606942343751464,
+                    "velocityY": -3.445993095328429,
+                    "timestamp": 0.5073151829210981
+                },
+                {
+                    "x": 1.7316376139293153,
+                    "y": 4.389829293964471,
+                    "heading": 3.02635075879599,
+                    "angularVelocity": -1.2576418143070467e-7,
+                    "velocityX": -1.2592175362335267,
+                    "velocityY": -3.5672126433882343,
+                    "timestamp": 0.5463394277611826
+                },
+                {
+                    "x": 1.6946418503710443,
+                    "y": 4.246913724355564,
+                    "heading": 3.026350753656809,
+                    "angularVelocity": -1.3169201974173754e-7,
+                    "velocityX": -0.9480199734770174,
+                    "velocityY": -3.662225116579624,
+                    "timestamp": 0.5853636726012671
+                },
+                {
+                    "x": 1.6700643070030454,
+                    "y": 4.101347620951498,
+                    "heading": 3.02635074809997,
+                    "angularVelocity": -1.423945209571316e-7,
+                    "velocityX": -0.6298018954293696,
+                    "velocityY": -3.7301452981493113,
+                    "timestamp": 0.6243879174413516
+                },
+                {
+                    "x": 1.6580868892212808,
+                    "y": 3.9542079360929074,
+                    "heading": 3.0263507418753948,
+                    "angularVelocity": -1.595053312793056e-7,
+                    "velocityX": -0.3069224742421484,
+                    "velocityY": -3.7704684731645255,
+                    "timestamp": 0.6634121622814361
+                },
+                {
+                    "x": 1.6587982403438444,
+                    "y": 3.806583282470554,
+                    "heading": 3.0263507346134717,
+                    "angularVelocity": -1.8608747530006848e-7,
+                    "velocityX": 0.018228440434743125,
+                    "velocityY": -3.7828958440399596,
+                    "timestamp": 0.7024364071215206
+                },
+                {
+                    "x": 1.6721931105563068,
+                    "y": 3.659565885100826,
+                    "heading": 3.0263506736368497,
+                    "angularVelocity": -0.0000015625317616489644,
+                    "velocityX": 0.34324482811563534,
+                    "velocityY": -3.767334844586533,
+                    "timestamp": 0.741460651961605
+                },
+                {
+                    "x": 1.6979565964079348,
+                    "y": 3.5152981288336416,
+                    "heading": 3.0233678381587312,
+                    "angularVelocity": -0.07643544392291168,
+                    "velocityX": 0.6601917848050372,
+                    "velocityY": -3.6968750288025483,
+                    "timestamp": 0.7804848968016895
+                },
+                {
+                    "x": 1.7349981432705297,
+                    "y": 3.376556575471776,
+                    "heading": 3.013707601859302,
+                    "angularVelocity": -0.2475444775167341,
+                    "velocityX": 0.9491931750224177,
+                    "velocityY": -3.5552655517207494,
+                    "timestamp": 0.819509141641774
+                },
+                {
+                    "x": 1.7820959712007873,
+                    "y": 3.2454658997474337,
+                    "heading": 2.9995738602901865,
+                    "angularVelocity": -0.3621784771756839,
+                    "velocityX": 1.2068863375389995,
+                    "velocityY": -3.3592110817655803,
+                    "timestamp": 0.8585333864818585
+                },
+                {
+                    "x": 1.8383454622663515,
+                    "y": 3.1231345959204413,
+                    "heading": 2.9823282080134397,
+                    "angularVelocity": -0.44192148617905314,
+                    "velocityX": 1.441398579167467,
+                    "velocityY": -3.1347513405650798,
+                    "timestamp": 0.897557631321943
+                },
+                {
+                    "x": 1.9031002792591356,
+                    "y": 3.0102116491438333,
+                    "heading": 2.962874002379933,
+                    "angularVelocity": -0.4985158768152219,
+                    "velocityX": 1.6593483681274654,
+                    "velocityY": -2.893661292853978,
+                    "timestamp": 0.9365818761620275
+                },
+                {
+                    "x": 1.9758873013923517,
+                    "y": 2.9071136453404876,
+                    "heading": 2.9418434761111434,
+                    "angularVelocity": -0.5389092435989264,
+                    "velocityX": 1.8651743917531916,
+                    "velocityY": -2.6418961910942005,
+                    "timestamp": 0.975606121002112
+                },
+                {
+                    "x": 2.0563490115304868,
+                    "y": 2.814127161728483,
+                    "heading": 2.919699686688593,
+                    "angularVelocity": -0.5674367182066591,
+                    "velocityX": 2.0618390046462407,
+                    "velocityY": -2.3827875207591935,
+                    "timestamp": 1.0146303658421965
+                },
+                {
+                    "x": 2.1442072150434264,
+                    "y": 2.731459952378173,
+                    "heading": 2.8967947458212753,
+                    "angularVelocity": -0.586941296652354,
+                    "velocityX": 2.2513748535810034,
+                    "velocityY": -2.11835513253537,
+                    "timestamp": 1.0536546106822808
+                },
+                {
+                    "x": 2.2392399311065674,
+                    "y": 2.659268856048584,
+                    "heading": 2.8734047162081335,
+                    "angularVelocity": -0.5993717420795943,
+                    "velocityX": 2.4352224226909684,
+                    "velocityY": -1.849903736136748,
+                    "timestamp": 1.0926788555223652
+                },
+                {
+                    "x": 2.338491380229597,
+                    "y": 2.598983086121257,
+                    "heading": 2.850361085284195,
+                    "angularVelocity": -0.6059434386623828,
+                    "velocityX": 2.609864937185211,
+                    "velocityY": -1.5852435257599564,
+                    "timestamp": 1.1307081986155205
+                },
+                {
+                    "x": 2.444184622569516,
+                    "y": 2.5488913725009676,
+                    "heading": 2.8272194988809463,
+                    "angularVelocity": -0.608519225445519,
+                    "velocityX": 2.779255010558571,
+                    "velocityY": -1.3171858766423765,
+                    "timestamp": 1.1687375417086758
+                },
+                {
+                    "x": 2.5560703795596873,
+                    "y": 2.509147043381333,
+                    "heading": 2.804166943788384,
+                    "angularVelocity": -0.6061781039996946,
+                    "velocityX": 2.9420901832588977,
+                    "velocityY": -1.0450963883935758,
+                    "timestamp": 1.2067668848018311
+                },
+                {
+                    "x": 2.67382962144658,
+                    "y": 2.4799341019260455,
+                    "heading": 2.7814386686411225,
+                    "angularVelocity": -0.5976510057401518,
+                    "velocityX": 3.096536314035744,
+                    "velocityY": -0.7681684478149349,
+                    "timestamp": 1.2447962278949865
+                },
+                {
+                    "x": 2.7970412331199452,
+                    "y": 2.46147554775192,
+                    "heading": 2.7593387310325537,
+                    "angularVelocity": -0.5811285657613138,
+                    "velocityX": 3.239909018978801,
+                    "velocityY": -0.48537662427964845,
+                    "timestamp": 1.2828255709881418
+                },
+                {
+                    "x": 2.925127627515394,
+                    "y": 2.4540428102885845,
+                    "heading": 2.738273276977094,
+                    "angularVelocity": -0.5539263195754057,
+                    "velocityX": 3.3680937922509844,
+                    "velocityY": -0.19544743239797485,
+                    "timestamp": 1.320854914081297
+                },
+                {
+                    "x": 3.057257978674903,
+                    "y": 2.457961811901398,
+                    "heading": 2.7188066654897645,
+                    "angularVelocity": -0.5118839796850494,
+                    "velocityX": 3.47443159446217,
+                    "velocityY": 0.10305204597435572,
+                    "timestamp": 1.3588842571744524
+                },
+                {
+                    "x": 3.192166604008838,
+                    "y": 2.4735957350107154,
+                    "heading": 2.701757241393799,
+                    "angularVelocity": -0.4483228662195614,
+                    "velocityX": 3.5474876598173757,
+                    "velocityY": 0.41110158203408753,
+                    "timestamp": 1.3969136002676077
+                },
+                {
+                    "x": 3.3278072233432696,
+                    "y": 2.5012232638000396,
+                    "heading": 2.6883398256989803,
+                    "angularVelocity": -0.3528174457801884,
+                    "velocityX": 3.566735796675976,
+                    "velocityY": 0.7264792537080078,
+                    "timestamp": 1.434942943360763
+                },
+                {
+                    "x": 3.4613865978283758,
+                    "y": 2.5407156767305445,
+                    "heading": 2.677154917488752,
+                    "angularVelocity": -0.2941125799315481,
+                    "velocityX": 3.512534364790241,
+                    "velocityY": 1.0384721301591493,
+                    "timestamp": 1.4729722864539183
+                },
+                {
+                    "x": 3.5888930381916024,
+                    "y": 2.590617982563406,
+                    "heading": 2.6645014610802713,
+                    "angularVelocity": -0.3327287662446667,
+                    "velocityX": 3.35284362001453,
+                    "velocityY": 1.3122053071130373,
+                    "timestamp": 1.5110016295470736
+                },
+                {
+                    "x": 3.7058440395321557,
+                    "y": 2.646348954214802,
+                    "heading": 2.6512815676345496,
+                    "angularVelocity": -0.34762350255007635,
+                    "velocityX": 3.0752832373169756,
+                    "velocityY": 1.4654728985163021,
+                    "timestamp": 1.549030972640229
+                },
+                {
+                    "x": 3.810776558765862,
+                    "y": 2.7030629098701224,
+                    "heading": 2.6383327014335145,
+                    "angularVelocity": -0.34049670985152497,
+                    "velocityX": 2.7592514279479117,
+                    "velocityY": 1.491320939107423,
+                    "timestamp": 1.5870603157333842
+                },
+                {
+                    "x": 3.9038282779644375,
+                    "y": 2.757723211117941,
+                    "heading": 2.626141706261417,
+                    "angularVelocity": -0.3205681239939803,
+                    "velocityX": 2.446840035354867,
+                    "velocityY": 1.4373191015665727,
+                    "timestamp": 1.6250896588265396
+                },
+                {
+                    "x": 3.9854403181685942,
+                    "y": 2.8085799309132606,
+                    "heading": 2.6149760707773027,
+                    "angularVelocity": -0.2936057942615943,
+                    "velocityX": 2.1460281342290126,
+                    "velocityY": 1.337302084623056,
+                    "timestamp": 1.6631190019196949
+                },
+                {
+                    "x": 4.056026736802803,
+                    "y": 2.8545641040147816,
+                    "heading": 2.6049941853375604,
+                    "angularVelocity": -0.26247851337555306,
+                    "velocityX": 1.8561040736703007,
+                    "velocityY": 1.209176108797926,
+                    "timestamp": 1.7011483450128502
+                },
+                {
+                    "x": 4.115926253855544,
+                    "y": 2.894973650540783,
+                    "heading": 2.5962978923918545,
+                    "angularVelocity": -0.22867323593790462,
+                    "velocityX": 1.5750868193019991,
+                    "velocityY": 1.0625886023594207,
+                    "timestamp": 1.7391776881060055
+                },
+                {
+                    "x": 4.1654099219914915,
+                    "y": 2.9293183863277776,
+                    "heading": 2.588957208164297,
+                    "angularVelocity": -0.19302684796776065,
+                    "velocityX": 1.301197026063159,
+                    "velocityY": 0.9031114658716605,
+                    "timestamp": 1.7772070311991608
+                },
+                {
+                    "x": 4.20469589755594,
+                    "y": 2.957239109446396,
+                    "heading": 2.5830228338283105,
+                    "angularVelocity": -0.15604724808021303,
+                    "velocityX": 1.0330437596099378,
+                    "velocityY": 0.7341889406352728,
+                    "timestamp": 1.815236374292316
+                },
+                {
+                    "x": 4.23396207855711,
+                    "y": 2.9784623659467853,
+                    "heading": 2.578532998453876,
+                    "angularVelocity": -0.11806239627740928,
+                    "velocityX": 0.769568407465833,
+                    "velocityY": 0.5580758113124009,
+                    "timestamp": 1.8532657173854714
+                },
+                {
+                    "x": 4.253355635131929,
+                    "y": 2.992773543728212,
+                    "heading": 2.575517463166205,
+                    "angularVelocity": -0.0792949612693492,
+                    "velocityX": 0.5099629653689784,
+                    "velocityY": 0.3763193528315992,
+                    "timestamp": 1.8912950604786267
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": -0.03990242909239911,
+                    "velocityX": 0.25360324643122656,
+                    "velocityY": 0.190023168532958,
+                    "timestamp": 1.929324403571782
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": 2.574,
+                    "angularVelocity": -5.006919316497686e-30,
+                    "velocityX": -1.2207915195710053e-31,
+                    "velocityY": 7.406976698260565e-32,
+                    "timestamp": 1.9673537466649373
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -5859,11 +14506,6 @@
                     "x": 5.61,
                     "y": 2.81,
                     "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
                 }
             ]
         },
@@ -5876,7 +14518,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 27
+                },
+                {
+                    "x": 2.008638858795166,
+                    "y": 3.158904790878296,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 27
                 },
                 {
                     "x": 5.176,
@@ -5888,7 +14539,503 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": -4.120593510226553e-35,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.2473912878554576,
+                    "y": 5.551785895834254,
+                    "heading": 3.1358246059620716,
+                    "angularVelocity": -0.12928713365744599,
+                    "velocityX": -0.08077630820983339,
+                    "velocityY": -0.2543419791057163,
+                    "timestamp": 0.03229551092834574
+                },
+                {
+                    "x": 2.242172157117942,
+                    "y": 5.535355099551312,
+                    "heading": 3.127553990435098,
+                    "angularVelocity": -0.25609180004378934,
+                    "velocityX": -0.16160545498398599,
+                    "velocityY": -0.5087640916843489,
+                    "timestamp": 0.06459102185669148
+                },
+                {
+                    "x": 2.2343406441985514,
+                    "y": 5.510704667985898,
+                    "heading": 3.115281361261821,
+                    "angularVelocity": -0.38001037359361073,
+                    "velocityX": -0.24249540243429488,
+                    "velocityY": -0.7632773365965662,
+                    "timestamp": 0.09688653278503723
+                },
+                {
+                    "x": 2.2238944664634275,
+                    "y": 5.477831199999642,
+                    "heading": 3.0991173688763074,
+                    "angularVelocity": -0.5005027609371739,
+                    "velocityX": -0.3234560294866074,
+                    "velocityY": -1.017895894546866,
+                    "timestamp": 0.12918204371338296
+                },
+                {
+                    "x": 2.2108309419755985,
+                    "y": 5.43673070422605,
+                    "heading": 3.0791962484868605,
+                    "angularVelocity": -0.6168386818107923,
+                    "velocityX": -0.40449970018474013,
+                    "velocityY": -1.272638041391628,
+                    "timestamp": 0.1614775546417287
+                },
+                {
+                    "x": 2.19514688330712,
+                    "y": 5.387398428523646,
+                    "heading": 3.0556846451348814,
+                    "angularVelocity": -0.7280145963364562,
+                    "velocityX": -0.4856420665793569,
+                    "velocityY": -1.5275273338098838,
+                    "timestamp": 0.19377306557007443
+                },
+                {
+                    "x": 2.17683845193207,
+                    "y": 5.329828642238818,
+                    "heading": 3.028794799017408,
+                    "angularVelocity": -0.832618693574285,
+                    "velocityX": -0.5669032893045367,
+                    "velocityY": -1.782594070505913,
+                    "timestamp": 0.22606857649842016
+                },
+                {
+                    "x": 2.1559009488672944,
+                    "y": 5.264014369161548,
+                    "heading": 2.9988052787110364,
+                    "angularVelocity": -0.9285971778836174,
+                    "velocityX": -0.6483100116059459,
+                    "velocityY": -2.037876818957682,
+                    "timestamp": 0.2583640874267659
+                },
+                {
+                    "x": 2.132328497947263,
+                    "y": 5.189947104033879,
+                    "heading": 2.966095877804453,
+                    "angularVelocity": -1.012815712349497,
+                    "velocityX": -0.7298986838242586,
+                    "velocityY": -2.293422924690741,
+                    "timestamp": 0.2906595983551117
+                },
+                {
+                    "x": 2.1061135495185,
+                    "y": 5.107616699879427,
+                    "heading": 2.9312120529633434,
+                    "angularVelocity": -1.0801446962243946,
+                    "velocityX": -0.8117211239334865,
+                    "velocityY": -2.549283221966035,
+                    "timestamp": 0.32295510928345744
+                },
+                {
+                    "x": 2.0772461397099717,
+                    "y": 5.017012347335229,
+                    "heading": 2.89500074789377,
+                    "angularVelocity": -1.1212488679902226,
+                    "velocityX": -0.8938520859006283,
+                    "velocityY": -2.805478220958382,
+                    "timestamp": 0.3552506202118032
+                },
+                {
+                    "x": 2.0457134444911373,
+                    "y": 4.918129964629122,
+                    "heading": 2.858963174705018,
+                    "angularVelocity": -1.1158694243515237,
+                    "velocityX": -0.976380131864019,
+                    "velocityY": -3.0617996081715195,
+                    "timestamp": 0.38754613114014896
+                },
+                {
+                    "x": 2.0115103463196724,
+                    "y": 4.811035711883079,
+                    "heading": 2.8266343874462354,
+                    "angularVelocity": -1.0010303701499224,
+                    "velocityX": -1.0590666376931392,
+                    "velocityY": -3.3160724096811367,
+                    "timestamp": 0.4198416420684947
+                },
+                {
+                    "x": 1.9751456955721198,
+                    "y": 4.697500624139868,
+                    "heading": 2.816667462304015,
+                    "angularVelocity": -0.30861642549437257,
+                    "velocityX": -1.1259970721081056,
+                    "velocityY": -3.5155067834385165,
+                    "timestamp": 0.4521371529968405
+                },
+                {
+                    "x": 1.9382198046413848,
+                    "y": 4.581042609670271,
+                    "heading": 2.816667439698893,
+                    "angularVelocity": -6.999462561903041e-7,
+                    "velocityX": -1.1433753444143555,
+                    "velocityY": -3.6060124494695995,
+                    "timestamp": 0.48443266392518625
+                },
+                {
+                    "x": 1.901294488957654,
+                    "y": 4.4645844128058085,
+                    "heading": 2.816667417094136,
+                    "angularVelocity": -6.999349526609492e-7,
+                    "velocityX": -1.1433575324340735,
+                    "velocityY": -3.6060180971544047,
+                    "timestamp": 0.516728174853532
+                },
+                {
+                    "x": 1.8682421277834205,
+                    "y": 4.346968376660799,
+                    "heading": 2.816667394414886,
+                    "angularVelocity": -7.022415636891642e-7,
+                    "velocityX": -1.0234351531879067,
+                    "velocityY": -3.6418694971559726,
+                    "timestamp": 0.5490236857818777
+                },
+                {
+                    "x": 1.8436394280164332,
+                    "y": 4.227299259504863,
+                    "heading": 2.81666737130494,
+                    "angularVelocity": -7.15577656003266e-7,
+                    "velocityX": -0.7617993665303343,
+                    "velocityY": -3.705441211982887,
+                    "timestamp": 0.5813191967102234
+                },
+                {
+                    "x": 1.8276118980147091,
+                    "y": 4.1061831643448095,
+                    "heading": 2.8166673472723573,
+                    "angularVelocity": -7.441462228176694e-7,
+                    "velocityX": -0.4962773320813651,
+                    "velocityY": -3.7502455195328888,
+                    "timestamp": 0.6136147076385691
+                },
+                {
+                    "x": 1.8202409034217624,
+                    "y": 3.984233755462536,
+                    "heading": 2.8166673217380835,
+                    "angularVelocity": -7.906446726868519e-7,
+                    "velocityX": -0.22823588731266858,
+                    "velocityY": -3.776048292062723,
+                    "timestamp": 0.6459102185669148
+                },
+                {
+                    "x": 1.821563847305773,
+                    "y": 3.8620689519300933,
+                    "heading": 2.816667293968721,
+                    "angularVelocity": -8.598520920997921e-7,
+                    "velocityX": 0.04096370814339873,
+                    "velocityY": -3.782717784013104,
+                    "timestamp": 0.6782057294952605
+                },
+                {
+                    "x": 1.831574050917961,
+                    "y": 3.740307773577626,
+                    "heading": 2.816667262971477,
+                    "angularVelocity": -9.5980040013024e-7,
+                    "velocityX": 0.30995650245006834,
+                    "velocityY": -3.7702199114489767,
+                    "timestamp": 0.7105012404236062
+                },
+                {
+                    "x": 1.8502208046317663,
+                    "y": 3.6195671988548175,
+                    "heading": 2.816667227314634,
+                    "angularVelocity": -0.00000110408047182699,
+                    "velocityX": 0.5773791210542256,
+                    "velocityY": -3.738617883788745,
+                    "timestamp": 0.7427967513519519
+                },
+                {
+                    "x": 1.8774096306565946,
+                    "y": 3.5004590367763506,
+                    "heading": 2.8166671847943117,
+                    "angularVelocity": -0.0000013166016324392044,
+                    "velocityX": 0.841876324085786,
+                    "velocityY": -3.688071767705825,
+                    "timestamp": 0.7750922622802976
+                },
+                {
+                    "x": 1.913002764036886,
+                    "y": 3.383586826300032,
+                    "heading": 2.8166671317574883,
+                    "angularVelocity": -0.0000016422351652599994,
+                    "velocityX": 1.1021077653566822,
+                    "velocityY": -3.618837637702142,
+                    "timestamp": 0.8073877732086433
+                },
+                {
+                    "x": 1.9568198515105035,
+                    "y": 3.2695427783414357,
+                    "heading": 2.8166670615606066,
+                    "angularVelocity": -0.0000021735801416730847,
+                    "velocityX": 1.3567547381688734,
+                    "velocityY": -3.53126625590866,
+                    "timestamp": 0.839683284136989
+                },
+                {
+                    "x": 2.008638858795166,
+                    "y": 3.158904790878296,
+                    "heading": 2.8166669305303302,
+                    "angularVelocity": -0.000004057228785819985,
+                    "velocityX": 1.6045266290919962,
+                    "velocityY": -3.4258008089301617,
+                    "timestamp": 0.8719787950653347
+                },
+                {
+                    "x": 2.095074231369689,
+                    "y": 3.013583476455127,
+                    "heading": 2.8166668472664824,
+                    "angularVelocity": -0.000001862874616466948,
+                    "velocityX": 1.9338316217798797,
+                    "velocityY": -3.2512956765222865,
+                    "timestamp": 0.9166752265441874
+                },
+                {
+                    "x": 2.195389426610098,
+                    "y": 2.8774723973475123,
+                    "heading": 2.8166667934576224,
+                    "angularVelocity": -0.0000012038737383971035,
+                    "velocityX": 2.2443669868336342,
+                    "velocityY": -3.0452336932538677,
+                    "timestamp": 0.9613716580230401
+                },
+                {
+                    "x": 2.308610813706186,
+                    "y": 2.751892601119491,
+                    "heading": 2.8166667540699692,
+                    "angularVelocity": -8.812259002769856e-7,
+                    "velocityX": 2.5331191629841037,
+                    "velocityY": -2.8096157136713913,
+                    "timestamp": 1.006068089501893
+                },
+                {
+                    "x": 2.433639499231453,
+                    "y": 2.638062914068913,
+                    "heading": 2.816666722710724,
+                    "angularVelocity": -7.016051243778922e-7,
+                    "velocityX": 2.7972856308321177,
+                    "velocityY": -2.5467287495744175,
+                    "timestamp": 1.0507645209807457
+                },
+                {
+                    "x": 2.5692619900937426,
+                    "y": 2.537088109244684,
+                    "heading": 2.8166666961232654,
+                    "angularVelocity": -5.94845213173575e-7,
+                    "velocityX": 3.034302434780658,
+                    "velocityY": -2.259124531496504,
+                    "timestamp": 1.0954609524595984
+                },
+                {
+                    "x": 2.7141619661921816,
+                    "y": 2.449948169817928,
+                    "heading": 2.8166666724078224,
+                    "angularVelocity": -5.305891904122224e-7,
+                    "velocityX": 3.241869010661312,
+                    "velocityY": -1.9495950021868031,
+                    "timestamp": 1.1401573839384511
+                },
+                {
+                    "x": 2.866933035141265,
+                    "y": 2.377488711333149,
+                    "heading": 2.8166666503106814,
+                    "angularVelocity": -4.943826596232375e-7,
+                    "velocityX": 3.4179701576705006,
+                    "velocityY": -1.6211463888132076,
+                    "timestamp": 1.1848538154173038
+                },
+                {
+                    "x": 3.0260921573816075,
+                    "y": 2.3204120378316904,
+                    "heading": 2.8166666288878814,
+                    "angularVelocity": -4.79295537569982e-7,
+                    "velocityX": 3.560891037031571,
+                    "velocityY": -1.2769850212418752,
+                    "timestamp": 1.2295502468961566
+                },
+                {
+                    "x": 3.1869067054641667,
+                    "y": 2.2681812614436785,
+                    "heading": 2.8166666075502422,
+                    "angularVelocity": -4.773902106314294e-7,
+                    "velocityX": 3.597928128974802,
+                    "velocityY": -1.1685670345455579,
+                    "timestamp": 1.2742466783750093
+                },
+                {
+                    "x": 3.3477213293158035,
+                    "y": 2.2159507183434,
+                    "heading": 2.8166665862126,
+                    "angularVelocity": -4.773902944556367e-7,
+                    "velocityX": 3.597929824167796,
+                    "velocityY": -1.1685618151639285,
+                    "timestamp": 1.318943109853862
+                },
+                {
+                    "x": 3.508535966619386,
+                    "y": 2.163720216660845,
+                    "heading": 2.8166665648749514,
+                    "angularVelocity": -4.773904335055608e-7,
+                    "velocityX": 3.5979301251302083,
+                    "velocityY": -1.1685608885189713,
+                    "timestamp": 1.3636395413327147
+                },
+                {
+                    "x": 3.669350617545261,
+                    "y": 2.111489756920536,
+                    "heading": 2.816666543537298,
+                    "angularVelocity": -4.773905387074157e-7,
+                    "velocityX": 3.5979304299038075,
+                    "velocityY": -1.1685599501387545,
+                    "timestamp": 1.4083359728115674
+                },
+                {
+                    "x": 3.830165282253442,
+                    "y": 2.0592593396151875,
+                    "heading": 2.816666522199642,
+                    "angularVelocity": -4.773905991181995e-7,
+                    "velocityX": 3.597930738257429,
+                    "velocityY": -1.168559000734998,
+                    "timestamp": 1.4530324042904201
+                },
+                {
+                    "x": 3.990979960989088,
+                    "y": 2.0070289654996674,
+                    "heading": 2.816666500861985,
+                    "angularVelocity": -4.773906042282552e-7,
+                    "velocityX": 3.597931052096018,
+                    "velocityY": -1.1685580344424535,
+                    "timestamp": 1.4977288357692728
+                },
+                {
+                    "x": 4.151794654203474,
+                    "y": 1.9547986359634688,
+                    "heading": 2.8166664795243315,
+                    "angularVelocity": -4.773905411161549e-7,
+                    "velocityX": 3.597931376031047,
+                    "velocityY": -1.1685570370625784,
+                    "timestamp": 1.5424252672481256
+                },
+                {
+                    "x": 4.312609353229998,
+                    "y": 1.902568324322763,
+                    "heading": 2.8166664581865346,
+                    "angularVelocity": -4.773937498526183e-7,
+                    "velocityX": 3.597931506066887,
+                    "velocityY": -1.1685566366840177,
+                    "timestamp": 1.5871216987269783
+                },
+                {
+                    "x": 4.468360661654424,
+                    "y": 1.8512590257414445,
+                    "heading": 2.7996529762164895,
+                    "angularVelocity": -0.38064519710247374,
+                    "velocityX": 3.4846475047592103,
+                    "velocityY": -1.1479506726525697,
+                    "timestamp": 1.631818130205831
+                },
+                {
+                    "x": 4.61007924771562,
+                    "y": 1.8048518324023994,
+                    "heading": 2.753515893557521,
+                    "angularVelocity": -1.0322319060481902,
+                    "velocityX": 3.1706912917253294,
+                    "velocityY": -1.0382751330159734,
+                    "timestamp": 1.6765145616846837
+                },
+                {
+                    "x": 4.736004026753032,
+                    "y": 1.7636749139398393,
+                    "heading": 2.704438737111309,
+                    "angularVelocity": -1.0980106201416129,
+                    "velocityX": 2.8173340660761905,
+                    "velocityY": -0.9212574046776457,
+                    "timestamp": 1.7212109931635364
+                },
+                {
+                    "x": 4.846114017481673,
+                    "y": 1.7276966103545506,
+                    "heading": 2.657457847080447,
+                    "angularVelocity": -1.05111053559365,
+                    "velocityX": 2.4635074229748755,
+                    "velocityY": -0.8049480102748526,
+                    "timestamp": 1.7659074246423891
+                },
+                {
+                    "x": 4.9404369366183385,
+                    "y": 1.6968910625978524,
+                    "heading": 2.6148397238370804,
+                    "angularVelocity": -0.9535016965175586,
+                    "velocityX": 2.110300890157008,
+                    "velocityY": -0.6892171642667541,
+                    "timestamp": 1.8106038561212419
+                },
+                {
+                    "x": 5.018999070193965,
+                    "y": 1.6712409213417598,
+                    "heading": 2.5778779941552346,
+                    "angularVelocity": -0.8269503505964041,
+                    "velocityX": 1.7576824586722803,
+                    "velocityY": -0.5738744773892818,
+                    "timestamp": 1.8553002876000946
+                },
+                {
+                    "x": 5.08182145352524,
+                    "y": 1.6507342817906896,
+                    "heading": 2.547405196833757,
+                    "angularVelocity": -0.6817724886134384,
+                    "velocityX": 1.4055346534990782,
+                    "velocityY": -0.45879813829819405,
+                    "timestamp": 1.8999967190789473
+                },
+                {
+                    "x": 5.128920654811687,
+                    "y": 1.6353626111579855,
+                    "heading": 2.524000017522932,
+                    "angularVelocity": -0.52364760533286,
+                    "velocityX": 1.0537575311516838,
+                    "velocityY": -0.34391270452937267,
+                    "timestamp": 1.9446931505578
+                },
+                {
+                    "x": 5.160309902680238,
+                    "y": 1.6251195157583214,
+                    "heading": 2.5080885464702414,
+                    "angularVelocity": -0.35598974070712264,
+                    "velocityX": 0.702276374869034,
+                    "velocityY": -0.2291703176462827,
+                    "timestamp": 1.9893895820366527
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": -0.18096626962419662,
+                    "velocityX": 0.35103691280557286,
+                    "velocityY": -0.1145396978893815,
+                    "timestamp": 2.0340860135155054
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": 2.5,
+                    "angularVelocity": 7.15236707545134e-33,
+                    "velocityX": -4.116164438326626e-35,
+                    "velocityY": 1.5643849141244448e-34,
+                    "timestamp": 2.078782444994358
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -6180,7 +15327,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 19
+                    "controlIntervalCount": 30
+                },
+                {
+                    "x": 5.403600215911865,
+                    "y": 6.822902202606201,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 27
                 },
                 {
                     "x": 7.68,
@@ -6192,7 +15348,530 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 4.173822195239149e-34,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.260404640394534,
+                    "y": 5.564834920701341,
+                    "heading": 3.1398005529486164,
+                    "angularVelocity": -0.005376730415880204,
+                    "velocityX": 0.2804902157612115,
+                    "velocityY": 0.1303406844718941,
+                    "timestamp": 0.037094486045788126
+                },
+                {
+                    "x": 2.2812139214883365,
+                    "y": 5.574504760754136,
+                    "heading": 3.139401659951885,
+                    "angularVelocity": -0.010753431015026859,
+                    "velocityX": 0.5609804397374861,
+                    "velocityY": 0.26068133255328624,
+                    "timestamp": 0.07418897209157625
+                },
+                {
+                    "x": 2.3124278435365335,
+                    "y": 5.58900951877657,
+                    "heading": 3.1388033223810634,
+                    "angularVelocity": -0.016130094647582943,
+                    "velocityX": 0.8414706705915215,
+                    "velocityY": 0.3910219433834154,
+                    "timestamp": 0.11128345813736437
+                },
+                {
+                    "x": 2.3540464067273468,
+                    "y": 5.608349193345913,
+                    "heading": 3.1380055419662973,
+                    "angularVelocity": -0.02150671163853717,
+                    "velocityX": 1.1219609065196614,
+                    "velocityY": 0.5213625158593963,
+                    "timestamp": 0.1483779441831525
+                },
+                {
+                    "x": 2.4060696111557784,
+                    "y": 5.632523782985854,
+                    "heading": 3.1370083209408497,
+                    "angularVelocity": -0.026883268424770904,
+                    "velocityX": 1.4024511450088724,
+                    "velocityY": 0.651703048536671,
+                    "timestamp": 0.18547243022894064
+                },
+                {
+                    "x": 2.4684974567819364,
+                    "y": 5.661533286147284,
+                    "heading": 3.1358116622714935,
+                    "angularVelocity": -0.032259745232204366,
+                    "velocityX": 1.6829413824226938,
+                    "velocityY": 0.7820435394527967,
+                    "timestamp": 0.22256691627472877
+                },
+                {
+                    "x": 2.541329943361364,
+                    "y": 5.695377701176762,
+                    "heading": 3.1344155700455096,
+                    "angularVelocity": -0.03763611185395848,
+                    "velocityX": 1.9634316132463963,
+                    "velocityY": 0.9123839857951219,
+                    "timestamp": 0.2596614023205169
+                },
+                {
+                    "x": 2.6245670703196087,
+                    "y": 5.734057026259781,
+                    "heading": 3.1328200501674663,
+                    "angularVelocity": -0.04301231929927239,
+                    "velocityX": 2.243921828583893,
+                    "velocityY": 1.0427243832216457,
+                    "timestamp": 0.296755888366305
+                },
+                {
+                    "x": 2.7182088365016934,
+                    "y": 5.777571259306566,
+                    "heading": 3.1310251117440115,
+                    "angularVelocity": -0.04838828124588956,
+                    "velocityX": 2.5244120127853087,
+                    "velocityY": 1.17306472431166,
+                    "timestamp": 0.33385037441209314
+                },
+                {
+                    "x": 2.822255239588457,
+                    "y": 5.825920397683519,
+                    "heading": 3.1290307702747278,
+                    "angularVelocity": -0.053763825351902786,
+                    "velocityX": 2.804902134466352,
+                    "velocityY": 1.3034049944046322,
+                    "timestamp": 0.3709448604578813
+                },
+                {
+                    "x": 2.9367062743473644,
+                    "y": 5.879104437402994,
+                    "heading": 3.126837057015217,
+                    "angularVelocity": -0.05913852686363322,
+                    "velocityX": 3.0853921150877586,
+                    "velocityY": 1.4337451569978974,
+                    "timestamp": 0.4080393465036694
+                },
+                {
+                    "x": 3.0615619238881426,
+                    "y": 5.937123369060452,
+                    "heading": 3.1244440632726094,
+                    "angularVelocity": -0.06451076690090568,
+                    "velocityX": 3.3658816403780625,
+                    "velocityY": 1.5640850660618872,
+                    "timestamp": 0.44513383254945754
+                },
+                {
+                    "x": 3.188819545942614,
+                    "y": 5.996258327229638,
+                    "heading": 3.124444062637546,
+                    "angularVelocity": -1.7120162809754954e-8,
+                    "velocityX": 3.430634458646746,
+                    "velocityY": 1.594171114709378,
+                    "timestamp": 0.4822283185952457
+                },
+                {
+                    "x": 3.3160771824859174,
+                    "y": 6.055393254219169,
+                    "heading": 3.1244440620025067,
+                    "angularVelocity": -1.711950789942117e-8,
+                    "velocityX": 3.4306348492393433,
+                    "velocityY": 1.5941702741624886,
+                    "timestamp": 0.5193228046410338
+                },
+                {
+                    "x": 3.4433348335743874,
+                    "y": 6.114528149907673,
+                    "heading": 3.1244440613674698,
+                    "angularVelocity": -1.7119436749370044e-8,
+                    "velocityX": 3.4306352413506347,
+                    "velocityY": 1.5941694303436482,
+                    "timestamp": 0.5564172906868219
+                },
+                {
+                    "x": 3.570592499306144,
+                    "y": 6.173663014083979,
+                    "heading": 3.124444060732438,
+                    "angularVelocity": -1.7119304165961875e-8,
+                    "velocityX": 3.4306356361070582,
+                    "velocityY": 1.5941685808319566,
+                    "timestamp": 0.59351177673261
+                },
+                {
+                    "x": 3.6978501798346053,
+                    "y": 6.232797846417909,
+                    "heading": 3.1244440600974133,
+                    "angularVelocity": -1.7119116923870704e-8,
+                    "velocityX": 3.430636034999362,
+                    "velocityY": 1.5941677224193291,
+                    "timestamp": 0.6306062627783982
+                },
+                {
+                    "x": 3.825107875386636,
+                    "y": 6.291932646421232,
+                    "heading": 3.1244440594623972,
+                    "angularVelocity": -1.7118879018616895e-8,
+                    "velocityX": 3.430636440007508,
+                    "velocityY": 1.594166850844881,
+                    "timestamp": 0.6677007488241863
+                },
+                {
+                    "x": 3.9523655862834066,
+                    "y": 6.351067413402769,
+                    "heading": 3.1244440588273914,
+                    "angularVelocity": -1.711859691944282e-8,
+                    "velocityX": 3.4306368536738447,
+                    "velocityY": 1.5941659606374654,
+                    "timestamp": 0.7047952348699744
+                },
+                {
+                    "x": 4.079623312940078,
+                    "y": 6.410202146469073,
+                    "heading": 3.1244440581923967,
+                    "angularVelocity": -1.711830453291075e-8,
+                    "velocityX": 3.4306372785321515,
+                    "velocityY": 1.5941650463443597,
+                    "timestamp": 0.7418897209157626
+                },
+                {
+                    "x": 4.206881055880688,
+                    "y": 6.469336844492392,
+                    "heading": 3.124444057557412,
+                    "angularVelocity": -1.711804273623487e-8,
+                    "velocityX": 3.430637717517568,
+                    "velocityY": 1.5941641016490944,
+                    "timestamp": 0.7789842069615507
+                },
+                {
+                    "x": 4.334138815794606,
+                    "y": 6.528471505989175,
+                    "heading": 3.1244440569224348,
+                    "angularVelocity": -1.7117826690263867e-8,
+                    "velocityX": 3.4306381750871577,
+                    "velocityY": 1.59416311696,
+                    "timestamp": 0.8160786930073388
+                },
+                {
+                    "x": 4.461396593547393,
+                    "y": 6.587606129096708,
+                    "heading": 3.1244440562874636,
+                    "angularVelocity": -1.7117677202487012e-8,
+                    "velocityX": 3.430638655990672,
+                    "velocityY": 1.5941620820555098,
+                    "timestamp": 0.853173179053127
+                },
+                {
+                    "x": 4.5886544601407655,
+                    "y": 6.646740561018822,
+                    "heading": 3.124444055652494,
+                    "angularVelocity": -1.711762346285072e-8,
+                    "velocityX": 3.4306410509715453,
+                    "velocityY": 1.5941569280437173,
+                    "timestamp": 0.8902676650989151
+                },
+                {
+                    "x": 4.7169368908362825,
+                    "y": 6.703618180644166,
+                    "heading": 3.1244440550164043,
+                    "angularVelocity": -1.7147829637187305e-8,
+                    "velocityX": 3.458261439103553,
+                    "velocityY": 1.5333173656899308,
+                    "timestamp": 0.9273621511447032
+                },
+                {
+                    "x": 4.8494363294608345,
+                    "y": 6.749827966380956,
+                    "heading": 3.124444054367944,
+                    "angularVelocity": -1.7481320274300667e-8,
+                    "velocityX": 3.5719443170340583,
+                    "velocityY": 1.245731931148746,
+                    "timestamp": 0.9644566371904914
+                },
+                {
+                    "x": 4.985267831359121,
+                    "y": 6.785059321241209,
+                    "heading": 3.124444053688418,
+                    "angularVelocity": -1.8318771799358465e-8,
+                    "velocityX": 3.6617706936448973,
+                    "velocityY": 0.9497733656900947,
+                    "timestamp": 1.0015511232362795
+                },
+                {
+                    "x": 5.12352345692124,
+                    "y": 6.809076540845587,
+                    "heading": 3.1244440529552464,
+                    "angularVelocity": -1.9764980034334986e-8,
+                    "velocityX": 3.7271206667066523,
+                    "velocityY": 0.6474606380779142,
+                    "timestamp": 1.0386456092820675
+                },
+                {
+                    "x": 5.26327900081687,
+                    "y": 6.821719014416903,
+                    "heading": 3.124444052138408,
+                    "angularVelocity": -2.2020481247534803e-8,
+                    "velocityX": 3.767555741926746,
+                    "velocityY": 0.3408181354961927,
+                    "timestamp": 1.0757400953278555
+                },
+                {
+                    "x": 5.403600215911865,
+                    "y": 6.822902202606201,
+                    "heading": 3.124444051205612,
+                    "angularVelocity": -2.5146473765828148e-8,
+                    "velocityX": 3.782805210504538,
+                    "velocityY": 0.03189660554501852,
+                    "timestamp": 1.1128345813736435
+                },
+                {
+                    "x": 5.530809022252107,
+                    "y": 6.8145114887459926,
+                    "heading": 3.1244440503418645,
+                    "angularVelocity": -2.5630462052464063e-8,
+                    "velocityX": 3.774737065106383,
+                    "velocityY": -0.24898227978112425,
+                    "timestamp": 1.1465346256803473
+                },
+                {
+                    "x": 5.657044071820609,
+                    "y": 6.79670145003808,
+                    "heading": 3.1244440495837504,
+                    "angularVelocity": -2.2495928655681543e-8,
+                    "velocityX": 3.745842243399973,
+                    "velocityY": -0.5284871006643926,
+                    "timestamp": 1.180234669987051
+                },
+                {
+                    "x": 5.781608875607717,
+                    "y": 6.769570371843818,
+                    "heading": 3.124444048897901,
+                    "angularVelocity": -2.0351591481890114e-8,
+                    "velocityX": 3.6962801192023655,
+                    "velocityY": -0.8050754458167036,
+                    "timestamp": 1.2139347142937547
+                },
+                {
+                    "x": 5.903816171014302,
+                    "y": 6.733267988087913,
+                    "heading": 3.1244440482605893,
+                    "angularVelocity": -1.891130244410514e-8,
+                    "velocityX": 3.6263244728813384,
+                    "velocityY": -1.0772206536441533,
+                    "timestamp": 1.2476347586004584
+                },
+                {
+                    "x": 6.022991737743217,
+                    "y": 6.687994699591429,
+                    "heading": 3.124444047653726,
+                    "angularVelocity": -1.8007800496265653e-8,
+                    "velocityX": 3.5363623158563073,
+                    "velocityY": -1.3434192573888535,
+                    "timestamp": 1.281334802907162
+                },
+                {
+                    "x": 6.1384782869774295,
+                    "y": 6.6340007793146,
+                    "heading": 3.1244440470624193,
+                    "angularVelocity": -1.754616693571922e-8,
+                    "velocityX": 3.4268960652742955,
+                    "velocityY": -1.6021913735611486,
+                    "timestamp": 1.3150348472138658
+                },
+                {
+                    "x": 6.251164558797792,
+                    "y": 6.57438290738676,
+                    "heading": 3.1244440464747822,
+                    "angularVelocity": -1.7437275134789687e-8,
+                    "velocityX": 3.343801889244,
+                    "velocityY": -1.769073992462997,
+                    "timestamp": 1.3487348915205695
+                },
+                {
+                    "x": 6.363850200907552,
+                    "y": 6.514763845231331,
+                    "heading": 3.124444045887316,
+                    "angularVelocity": -1.7432210000565644e-8,
+                    "velocityX": 3.343783203493965,
+                    "velocityY": -1.769109310742662,
+                    "timestamp": 1.3824349358272732
+                },
+                {
+                    "x": 6.475774212207488,
+                    "y": 6.453726953025987,
+                    "heading": 3.124444046521184,
+                    "angularVelocity": 1.880910439494107e-8,
+                    "velocityX": 3.3211829124412344,
+                    "velocityY": -1.8111813637349317,
+                    "timestamp": 1.4161349801339769
+                },
+                {
+                    "x": 6.587697896511305,
+                    "y": 6.392689461208172,
+                    "heading": 3.1244440471552553,
+                    "angularVelocity": 1.881516364731064e-8,
+                    "velocityX": 3.3211732093049866,
+                    "velocityY": -1.8111991563664802,
+                    "timestamp": 1.4498350244406806
+                },
+                {
+                    "x": 6.699621529072565,
+                    "y": 6.331651874510795,
+                    "heading": 3.124444047789331,
+                    "angularVelocity": 1.881528213317004e-8,
+                    "velocityX": 3.321171673919603,
+                    "velocityY": -1.811201971780052,
+                    "timestamp": 1.4835350687473843
+                },
+                {
+                    "x": 6.811545132450604,
+                    "y": 6.2706142343006634,
+                    "heading": 3.124444048423398,
+                    "angularVelocity": 1.881501023244474e-8,
+                    "velocityX": 3.3211708079497897,
+                    "velocityY": -1.8112035596935343,
+                    "timestamp": 1.517235113054088
+                },
+                {
+                    "x": 6.923468714015748,
+                    "y": 6.209576554092949,
+                    "heading": 3.1244440490575007,
+                    "angularVelocity": 1.8816081178773256e-8,
+                    "velocityX": 3.3211701606836845,
+                    "velocityY": -1.8112047465638819,
+                    "timestamp": 1.5509351573607917
+                },
+                {
+                    "x": 7.031544581287166,
+                    "y": 6.150637092992946,
+                    "heading": 3.126666281190466,
+                    "angularVelocity": 0.06594151962352889,
+                    "velocityX": 3.2069948124643886,
+                    "velocityY": -1.7489431338307682,
+                    "timestamp": 1.5846352016674954
+                },
+                {
+                    "x": 7.131306941921243,
+                    "y": 6.096231417458986,
+                    "heading": 3.1287176070894986,
+                    "angularVelocity": 0.060870124690714414,
+                    "velocityX": 2.960303545186476,
+                    "velocityY": -1.6144096143855096,
+                    "timestamp": 1.618335245974199
+                },
+                {
+                    "x": 7.2227557801672635,
+                    "y": 6.046359537502576,
+                    "heading": 3.1305980016068635,
+                    "angularVelocity": 0.05579798353529268,
+                    "velocityX": 2.7136118105289606,
+                    "velocityY": -1.4798757978632517,
+                    "timestamp": 1.6520352902809028
+                },
+                {
+                    "x": 7.305891091138876,
+                    "y": 6.001021457128046,
+                    "heading": 3.132307456329253,
+                    "angularVelocity": 0.05072559272714715,
+                    "velocityX": 2.466919930876006,
+                    "velocityY": -1.3453418625182645,
+                    "timestamp": 1.6857353345876065
+                },
+                {
+                    "x": 7.380712872646774,
+                    "y": 5.9602171788044025,
+                    "heading": 3.1338459674980714,
+                    "angularVelocity": 0.04565309038812635,
+                    "velocityX": 2.2202279862586183,
+                    "velocityY": -1.2108078539091887,
+                    "timestamp": 1.7194353788943102
+                },
+                {
+                    "x": 7.447221123569205,
+                    "y": 5.923946704366036,
+                    "heading": 3.135213533279876,
+                    "angularVelocity": 0.04058053364434269,
+                    "velocityX": 1.9735360083547677,
+                    "velocityY": -1.0762737908671516,
+                    "timestamp": 1.753135423201014
+                },
+                {
+                    "x": 7.505415843310863,
+                    "y": 5.892210035316634,
+                    "heading": 3.136410152813217,
+                    "angularVelocity": 0.035507951338280094,
+                    "velocityX": 1.7268440127861169,
+                    "velocityY": -0.941739683205353,
+                    "timestamp": 1.7868354675077176
+                },
+                {
+                    "x": 7.555297031572268,
+                    "y": 5.865007172961733,
+                    "heading": 3.1374358257913886,
+                    "angularVelocity": 0.030435359931185808,
+                    "velocityX": 1.4801520083308155,
+                    "velocityY": -0.807205536803692,
+                    "timestamp": 1.8205355118144213
+                },
+                {
+                    "x": 7.596864688235311,
+                    "y": 5.8423381184765635,
+                    "heading": 3.138290552251957,
+                    "angularVelocity": 0.025362769638798365,
+                    "velocityX": 1.2334600003707947,
+                    "velocityY": -0.6726713555287516,
+                    "timestamp": 1.854235556121125
+                },
+                {
+                    "x": 7.6301188133003475,
+                    "y": 5.824202872944982,
+                    "heading": 3.1389743324564656,
+                    "angularVelocity": 0.020290187107322288,
+                    "velocityX": 0.9867679924213419,
+                    "velocityY": -0.5381371420919633,
+                    "timestamp": 1.8879356004278287
+                },
+                {
+                    "x": 7.655059406849112,
+                    "y": 5.8106014373839985,
+                    "heading": 3.1394871668082627,
+                    "angularVelocity": 0.015217616544653215,
+                    "velocityX": 0.7400759868972333,
+                    "velocityY": -0.4036028984768356,
+                    "timestamp": 1.9216356447345324
+                },
+                {
+                    "x": 7.671686469021436,
+                    "y": 5.801533812759989,
+                    "heading": 3.1398290558033333,
+                    "angularVelocity": 0.010145060699586378,
+                    "velocityX": 0.4933839855224485,
+                    "velocityY": -0.2690686261859067,
+                    "timestamp": 1.9553356890412361
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": 0.0050725214219634815,
+                    "velocityX": 0.24669198956838576,
+                    "velocityY": -0.1345343263862727,
+                    "timestamp": 1.9890357333479398
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": 1.6384700465306213e-30,
+                    "velocityX": 7.518277191788514e-33,
+                    "velocityY": 3.939051930235097e-33,
+                    "timestamp": 2.0227357776546437
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -6237,7 +15916,25 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 26
+                },
+                {
+                    "x": 4.148105144500732,
+                    "y": 5.054959774017334,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 9
+                },
+                {
+                    "x": 5.480467319488525,
+                    "y": 4.235044002532959,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 27
                 },
                 {
                     "x": 7.68,
@@ -6249,7 +15946,575 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 1.2492921998824363e-31,
+                    "velocityX": 1.8348820495773462e-32,
+                    "velocityY": 2.100679182506497e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.256418341264554,
+                    "y": 5.5583910221605555,
+                    "heading": 3.140000000000001,
+                    "angularVelocity": 2.3717958634313657e-14,
+                    "velocityX": 0.2278392560297752,
+                    "velocityY": -0.05711574046896039,
+                    "timestamp": 0.028170480260500258
+                },
+                {
+                    "x": 2.269255024758466,
+                    "y": 5.555173071001971,
+                    "heading": 3.1400000000000023,
+                    "angularVelocity": 4.731676431156196e-14,
+                    "velocityX": 0.4556785463083383,
+                    "velocityY": -0.11423132047543481,
+                    "timestamp": 0.056340960521000516
+                },
+                {
+                    "x": 2.2885100515714525,
+                    "y": 5.550346151638775,
+                    "heading": 3.140000000000004,
+                    "angularVelocity": 7.082238243214026e-14,
+                    "velocityX": 0.6835178752697619,
+                    "velocityY": -0.17134671892564213,
+                    "timestamp": 0.08451144078150077
+                },
+                {
+                    "x": 2.314183422941926,
+                    "y": 5.543910269896753,
+                    "heading": 3.140000000000007,
+                    "angularVelocity": 9.417090371236357e-14,
+                    "velocityX": 0.9113572481926052,
+                    "velocityY": -0.22846191057115275,
+                    "timestamp": 0.11268192104200103
+                },
+                {
+                    "x": 2.3462751402864916,
+                    "y": 5.535865432460115,
+                    "heading": 3.1400000000000103,
+                    "angularVelocity": 1.1735309251828409e-13,
+                    "velocityX": 1.1391966714022896,
+                    "velocityY": -0.28557686493967693,
+                    "timestamp": 0.1408524013025013
+                },
+                {
+                    "x": 2.384785205236557,
+                    "y": 5.526211647057785,
+                    "heading": 3.140000000000014,
+                    "angularVelocity": 1.4030928785894893e-13,
+                    "velocityX": 1.367036152523929,
+                    "velocityY": -0.3426915449455824,
+                    "timestamp": 0.16902288156300155
+                },
+                {
+                    "x": 2.4297136196836084,
+                    "y": 5.514948922700363,
+                    "heading": 3.1400000000000188,
+                    "angularVelocity": 1.629004634033803e-13,
+                    "velocityX": 1.5948757007898307,
+                    "velocityY": -0.39980590509187397,
+                    "timestamp": 0.1971933618235018
+                },
+                {
+                    "x": 2.481060385834354,
+                    "y": 5.50207726998147,
+                    "heading": 3.1400000000000237,
+                    "angularVelocity": 1.8496997642686924e-13,
+                    "velocityX": 1.822715327389804,
+                    "velocityY": -0.4569198891841572,
+                    "timestamp": 0.22536384208400206
+                },
+                {
+                    "x": 2.5388255062752263,
+                    "y": 5.487596701457088,
+                    "heading": 3.14000000000003,
+                    "angularVelocity": 2.0646227541738248e-13,
+                    "velocityX": 2.0505550458033337,
+                    "velocityY": -0.5140334275623145,
+                    "timestamp": 0.25353432234450235
+                },
+                {
+                    "x": 2.6030089840400077,
+                    "y": 5.471507232107069,
+                    "heading": 3.140000000000036,
+                    "angularVelocity": 2.2698477398227208e-13,
+                    "velocityX": 2.278394871910569,
+                    "velocityY": -0.5711464341833905,
+                    "timestamp": 0.28170480260500264
+                },
+                {
+                    "x": 2.673610822657147,
+                    "y": 5.453808879843062,
+                    "heading": 3.140000000000043,
+                    "angularVelocity": 2.462514676500777e-13,
+                    "velocityX": 2.506234823271173,
+                    "velocityY": -0.6282588049740611,
+                    "timestamp": 0.3098752828655029
+                },
+                {
+                    "x": 2.7506310261005877,
+                    "y": 5.434501665888006,
+                    "heading": 3.1400000000000508,
+                    "angularVelocity": 2.636502833251031e-13,
+                    "velocityX": 2.7340749156994817,
+                    "velocityY": -0.6853704223895046,
+                    "timestamp": 0.3380457631260032
+                },
+                {
+                    "x": 2.834069598399475,
+                    "y": 5.413585614377554,
+                    "heading": 3.1400000000000587,
+                    "angularVelocity": 2.7876299092334793e-13,
+                    "velocityX": 2.9619151511549577,
+                    "velocityY": -0.7424811830340069,
+                    "timestamp": 0.3662162433865035
+                },
+                {
+                    "x": 2.9239265420507494,
+                    "y": 5.391060748829609,
+                    "heading": 3.1400000000000667,
+                    "angularVelocity": 2.92402388167881e-13,
+                    "velocityX": 3.189755475247231,
+                    "velocityY": -0.7995911088362938,
+                    "timestamp": 0.3943867236470038
+                },
+                {
+                    "x": 3.0202018518398366,
+                    "y": 5.366927076318611,
+                    "heading": 3.1400000000000756,
+                    "angularVelocity": 3.135357172375438e-13,
+                    "velocityX": 3.41759561423172,
+                    "velocityY": -0.8567007835090982,
+                    "timestamp": 0.42255720390750406
+                },
+                {
+                    "x": 3.122895489704816,
+                    "y": 5.341184519185279,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": 4.18703156863577e-13,
+                    "velocityX": 3.645434402088397,
+                    "velocityY": -0.9138132149428461,
+                    "timestamp": 0.45072768416800435
+                },
+                {
+                    "x": 3.2262642847828022,
+                    "y": 5.315271868839329,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": 1.5556336116797473e-16,
+                    "velocityX": 3.669401235694458,
+                    "velocityY": -0.9198512097177582,
+                    "timestamp": 0.47889816442850464
+                },
+                {
+                    "x": 3.3296318094364086,
+                    "y": 5.289354150507015,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": -1.6083670828215077e-16,
+                    "velocityX": 3.669356137976281,
+                    "velocityY": -0.9200311138697771,
+                    "timestamp": 0.5070686446890049
+                },
+                {
+                    "x": 3.4329975109343875,
+                    "y": 5.263429161959932,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": 1.7787924056306163e-16,
+                    "velocityX": 3.669291419320054,
+                    "velocityY": -0.9202891930612488,
+                    "timestamp": 0.5352391249495052
+                },
+                {
+                    "x": 3.536360974744539,
+                    "y": 5.23749525314096,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": 1.5501276329034833e-16,
+                    "velocityX": 3.669211985536661,
+                    "velocityY": -0.9206058462317519,
+                    "timestamp": 0.5634096052100055
+                },
+                {
+                    "x": 3.6397250084453354,
+                    "y": 5.211563615834452,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": 2.0671720135979797e-16,
+                    "velocityX": 3.6692322156015877,
+                    "velocityY": -0.9205252117362434,
+                    "timestamp": 0.5915800854705058
+                },
+                {
+                    "x": 3.743083009106784,
+                    "y": 5.185607942562298,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": 1.5977153188511187e-16,
+                    "velocityX": 3.6690180538517057,
+                    "velocityY": -0.921378444106565,
+                    "timestamp": 0.6197505657310061
+                },
+                {
+                    "x": 3.8464299042889807,
+                    "y": 5.159608086186739,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": -2.0588084435766592e-17,
+                    "velocityX": 3.6686238298573155,
+                    "velocityY": -0.9229468626423776,
+                    "timestamp": 0.6479210459915064
+                },
+                {
+                    "x": 3.949030591223734,
+                    "y": 5.130804053392629,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": 7.696524756540132e-18,
+                    "velocityX": 3.642134815806339,
+                    "velocityY": -1.0224899443584623,
+                    "timestamp": 0.6760915262520066
+                },
+                {
+                    "x": 4.049738686087192,
+                    "y": 5.095955052913767,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": 3.5716950666970086e-16,
+                    "velocityX": 3.5749512941271244,
+                    "velocityY": -1.2370751281698067,
+                    "timestamp": 0.7042620065125069
+                },
+                {
+                    "x": 4.148105144500732,
+                    "y": 5.054959774017334,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": 3.8010173489100904e-16,
+                    "velocityX": 3.4918275266846264,
+                    "velocityY": -1.455256655809122,
+                    "timestamp": 0.7324324867730072
+                },
+                {
+                    "x": 4.301558125078007,
+                    "y": 4.971822103513501,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": -3.218591761776222e-17,
+                    "velocityX": 3.32615174670464,
+                    "velocityY": -1.8020406441293784,
+                    "timestamp": 0.7785677783240725
+                },
+                {
+                    "x": 4.445898119119771,
+                    "y": 4.873711676762074,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": -4.812512433810467e-17,
+                    "velocityX": 3.128624295828435,
+                    "velocityY": -2.126580833303964,
+                    "timestamp": 0.8247030698751379
+                },
+                {
+                    "x": 4.5875715760336115,
+                    "y": 4.7717885618775915,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": -6.437963156007962e-17,
+                    "velocityX": 3.0708260888967014,
+                    "velocityY": -2.2092222994111217,
+                    "timestamp": 0.8708383614262032
+                },
+                {
+                    "x": 4.729219222551534,
+                    "y": 4.669829579874243,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": -5.401066324195286e-16,
+                    "velocityX": 3.0702666386935125,
+                    "velocityY": -2.2099997329304943,
+                    "timestamp": 0.9169736529772685
+                },
+                {
+                    "x": 4.870877206247071,
+                    "y": 4.567884960570754,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": -5.491807811904997e-16,
+                    "velocityX": 3.0704907009776297,
+                    "velocityY": -2.209688415909936,
+                    "timestamp": 0.9631089445283338
+                },
+                {
+                    "x": 5.012587244990002,
+                    "y": 4.46601271378786,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": 2.3992830315072076e-16,
+                    "velocityX": 3.0716190139795567,
+                    "velocityY": -2.2081197139288955,
+                    "timestamp": 1.0092442360793992
+                },
+                {
+                    "x": 5.160453670190759,
+                    "y": 4.373302358536228,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": 3.2225913830486856e-16,
+                    "velocityX": 3.205061033110239,
+                    "velocityY": -2.009532228659069,
+                    "timestamp": 1.0553795276304645
+                },
+                {
+                    "x": 5.316942928230888,
+                    "y": 4.296031397168284,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": 3.540598605731943e-16,
+                    "velocityX": 3.391964216104365,
+                    "velocityY": -1.674877491153309,
+                    "timestamp": 1.1015148191815298
+                },
+                {
+                    "x": 5.480467319488525,
+                    "y": 4.235044002532959,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": 3.7214299347620075e-16,
+                    "velocityX": 3.544453405635226,
+                    "velocityY": -1.3219249859178432,
+                    "timestamp": 1.1476501107325952
+                },
+                {
+                    "x": 5.591417678770796,
+                    "y": 4.201991970754989,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": 2.3864185821028e-16,
+                    "velocityX": 3.6254884971019536,
+                    "velocityY": -1.0800304009303623,
+                    "timestamp": 1.1782529824203136
+                },
+                {
+                    "x": 5.704342870000873,
+                    "y": 4.176490587908602,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": 1.1130552624117688e-16,
+                    "velocityX": 3.6900194328910065,
+                    "velocityY": -0.8333003225044783,
+                    "timestamp": 1.208855854108032
+                },
+                {
+                    "x": 5.818728492964508,
+                    "y": 4.158648255815286,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": 1.0429128791162552e-16,
+                    "velocityX": 3.737741481609135,
+                    "velocityY": -0.583028033314901,
+                    "timestamp": 1.2394587257957506
+                },
+                {
+                    "x": 5.934052260793966,
+                    "y": 4.148506842860091,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": 7.090230424723168e-16,
+                    "velocityX": 3.7683969336671863,
+                    "velocityY": -0.3313876246402188,
+                    "timestamp": 1.270061597483469
+                },
+                {
+                    "x": 6.049784939893549,
+                    "y": 4.145614314312393,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": 5.904438564058107e-16,
+                    "velocityX": 3.7817587931139536,
+                    "velocityY": -0.0945182065847883,
+                    "timestamp": 1.3006644691711875
+                },
+                {
+                    "x": 6.1655453259799495,
+                    "y": 4.144216905752427,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": -4.945243197986353e-16,
+                    "velocityX": 3.7826641652342365,
+                    "velocityY": -0.04566266116445697,
+                    "timestamp": 1.331267340858906
+                },
+                {
+                    "x": 6.281306292238703,
+                    "y": 4.142868414104831,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": 1.3561359601509856e-16,
+                    "velocityX": 3.782683123335836,
+                    "velocityY": -0.04406421926917252,
+                    "timestamp": 1.3618702125466244
+                },
+                {
+                    "x": 6.397067393015407,
+                    "y": 4.141531519361414,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": -2.815905629868065e-16,
+                    "velocityX": 3.782687518934728,
+                    "velocityY": -0.04368527104047988,
+                    "timestamp": 1.392473084234343
+                },
+                {
+                    "x": 6.512828617959603,
+                    "y": 4.140205420378013,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": 4.8810263606010396e-17,
+                    "velocityX": 3.782691576315187,
+                    "velocityY": -0.043332501527048845,
+                    "timestamp": 1.4230759559220614
+                },
+                {
+                    "x": 6.628589931635543,
+                    "y": 4.1388870909429585,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": -3.260693519621353e-16,
+                    "velocityX": 3.7826944757734187,
+                    "velocityY": -0.04307861853318547,
+                    "timestamp": 1.4536788276097798
+                },
+                {
+                    "x": 6.744351296534275,
+                    "y": 4.137573267441446,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": -5.381950425280429e-16,
+                    "velocityX": 3.782696149563972,
+                    "velocityY": -0.04293137960615145,
+                    "timestamp": 1.4842816992974983
+                },
+                {
+                    "x": 6.860112695906051,
+                    "y": 4.136261713626517,
+                    "heading": 3.140000000000087,
+                    "angularVelocity": 6.589038089157793e-16,
+                    "velocityX": 3.7826972760284443,
+                    "velocityY": -0.04285721378647441,
+                    "timestamp": 1.5148845709852168
+                },
+                {
+                    "x": 6.969430224754608,
+                    "y": 4.135029589996691,
+                    "heading": 3.1400000000000587,
+                    "angularVelocity": -9.14507649037293e-13,
+                    "velocityX": 3.5721330326141576,
+                    "velocityY": -0.04026169968457299,
+                    "timestamp": 1.5454874426729353
+                },
+                {
+                    "x": 7.070940187047598,
+                    "y": 4.13388425582119,
+                    "heading": 3.140000000000041,
+                    "angularVelocity": -5.92710722161247e-13,
+                    "velocityX": 3.3170077412616483,
+                    "velocityY": -0.037425709147242185,
+                    "timestamp": 1.5760903143606537
+                },
+                {
+                    "x": 7.16464169000894,
+                    "y": 4.132826276442613,
+                    "heading": 3.1400000000000285,
+                    "angularVelocity": -4.0082806789240517e-13,
+                    "velocityX": 3.06185327695782,
+                    "velocityY": -0.034571245100923735,
+                    "timestamp": 1.6066931860483722
+                },
+                {
+                    "x": 7.250534738512908,
+                    "y": 4.131855992104498,
+                    "heading": 3.14000000000002,
+                    "angularVelocity": -2.818757978482064e-13,
+                    "velocityX": 2.8066989719287347,
+                    "velocityY": -0.031705663029917695,
+                    "timestamp": 1.6372960577360907
+                },
+                {
+                    "x": 7.328619329487215,
+                    "y": 4.130973602817362,
+                    "heading": 3.140000000000014,
+                    "angularVelocity": -2.020529664357193e-13,
+                    "velocityX": 2.5515445665075394,
+                    "velocityY": -0.028833545281456593,
+                    "timestamp": 1.6678989294238091
+                },
+                {
+                    "x": 7.398895462248533,
+                    "y": 4.130179235854988,
+                    "heading": 3.1400000000000095,
+                    "angularVelocity": -1.455432238063259e-13,
+                    "velocityX": 2.296390138757389,
+                    "velocityY": -0.02595726864966503,
+                    "timestamp": 1.6985018011115276
+                },
+                {
+                    "x": 7.4613631364100685,
+                    "y": 4.129472977966136,
+                    "heading": 3.1400000000000063,
+                    "angularVelocity": -1.0415995882589759e-13,
+                    "velocityX": 2.0412356983681157,
+                    "velocityY": -0.0230781573571433,
+                    "timestamp": 1.729104672799246
+                },
+                {
+                    "x": 7.5160223517191325,
+                    "y": 4.128854891515294,
+                    "heading": 3.1400000000000037,
+                    "angularVelocity": -7.336162887662554e-14,
+                    "velocityX": 1.7860812497217864,
+                    "velocityY": -0.02019700820070875,
+                    "timestamp": 1.7597075444869645
+                },
+                {
+                    "x": 7.562873107995996,
+                    "y": 4.12832502323739,
+                    "heading": 3.1400000000000023,
+                    "angularVelocity": -5.027990878360554e-14,
+                    "velocityX": 1.5309267952025598,
+                    "velocityY": -0.017314331902335514,
+                    "timestamp": 1.790310416174683
+                },
+                {
+                    "x": 7.6019154051050695,
+                    "y": 4.127883409326492,
+                    "heading": 3.1400000000000015,
+                    "angularVelocity": -3.297949891981295e-14,
+                    "velocityX": 1.2757723362526898,
+                    "velocityY": -0.014430472902237576,
+                    "timestamp": 1.8209132878624015
+                },
+                {
+                    "x": 7.63314924293966,
+                    "y": 4.127530078568267,
+                    "heading": 3.1400000000000006,
+                    "angularVelocity": -2.0267021915169143e-14,
+                    "velocityX": 1.0206178738164648,
+                    "velocityY": -0.011545673282216062,
+                    "timestamp": 1.85151615955012
+                },
+                {
+                    "x": 7.656574621413162,
+                    "y": 4.127265054361888,
+                    "heading": 3.14,
+                    "angularVelocity": -1.117466715568672e-14,
+                    "velocityX": 0.7654634085502764,
+                    "velocityY": -0.008660109055131773,
+                    "timestamp": 1.8821190312378384
+                },
+                {
+                    "x": 7.672191540453593,
+                    "y": 4.127088356077733,
+                    "heading": 3.14,
+                    "angularVelocity": -5.057381822634304e-15,
+                    "velocityX": 0.5103089409318914,
+                    "velocityY": -0.005773911868497247,
+                    "timestamp": 1.9127219029255569
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": -1.4274059532547784e-15,
+                    "velocityX": 0.2551544713218168,
+                    "velocityY": -0.002887182568422342,
+                    "timestamp": 1.9433247746132754
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": 3.6920170793948963e-32,
+                    "velocityX": 1.993299363399013e-33,
+                    "velocityY": 1.7597210552293873e-31,
+                    "timestamp": 1.9739276463009938
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -6267,23 +16532,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "W2 To C4": {
             "waypoints": [
@@ -6294,19 +16543,623 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
+                    "controlIntervalCount": 26
+                },
+                {
+                    "x": 4.032804489135742,
+                    "y": 5.285560607910156,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 10
+                },
+                {
+                    "x": 5.390789031982422,
+                    "y": 4.09412145614624,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 28
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
                     "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.254338747446347,
+                    "y": 5.559909413116517,
+                    "heading": 3.1115254109417156,
+                    "angularVelocity": -0.9384273980850008,
+                    "velocityX": 0.14299063170637197,
+                    "velocityY": -0.0029854412716235888,
+                    "timestamp": 0.030342879072361703
+                },
+                {
+                    "x": 2.263150716185563,
+                    "y": 5.559590842447939,
+                    "heading": 3.055209315133754,
+                    "angularVelocity": -1.855990516709349,
+                    "velocityX": 0.29041307247742426,
+                    "velocityY": -0.010499025745671282,
+                    "timestamp": 0.060685758144723406
+                },
+                {
+                    "x": 2.2766003091950555,
+                    "y": 5.558840744374995,
+                    "heading": 2.9719321573579647,
+                    "angularVelocity": -2.744537114529888,
+                    "velocityX": 0.44325368655420766,
+                    "velocityY": -0.024720728417179556,
+                    "timestamp": 0.09102863721708511
+                },
+                {
+                    "x": 2.2949251974733853,
+                    "y": 5.557404975825004,
+                    "heading": 2.863139887890429,
+                    "angularVelocity": -3.5854300182948253,
+                    "velocityX": 0.6039271433217751,
+                    "velocityY": -0.04731813835356722,
+                    "timestamp": 0.12137151628944681
+                },
+                {
+                    "x": 2.3185983013070146,
+                    "y": 5.55505416569458,
+                    "heading": 2.7318619262997976,
+                    "angularVelocity": -4.326483366247482,
+                    "velocityX": 0.7801864739721567,
+                    "velocityY": -0.07747485414348045,
+                    "timestamp": 0.15171439536180853
+                },
+                {
+                    "x": 2.348656357948328,
+                    "y": 5.552070493546886,
+                    "heading": 2.5849479304647764,
+                    "angularVelocity": -4.841794856864455,
+                    "velocityX": 0.9906132035009321,
+                    "velocityY": -0.0983318735370822,
+                    "timestamp": 0.18205727443417025
+                },
+                {
+                    "x": 2.38579846451748,
+                    "y": 5.549354804791822,
+                    "heading": 2.427729518710298,
+                    "angularVelocity": -5.181394006137112,
+                    "velocityX": 1.2240798403004254,
+                    "velocityY": -0.08950003553016357,
+                    "timestamp": 0.21240015350653196
+                },
+                {
+                    "x": 2.430432513687245,
+                    "y": 5.547202777187511,
+                    "heading": 2.264898570795434,
+                    "angularVelocity": -5.366364461544502,
+                    "velocityX": 1.470989257918541,
+                    "velocityY": -0.07092364568235572,
+                    "timestamp": 0.24274303257889368
+                },
+                {
+                    "x": 2.4823970400851527,
+                    "y": 5.546086320842798,
+                    "heading": 2.109408164193531,
+                    "angularVelocity": -5.12444472494155,
+                    "velocityX": 1.7125773158830133,
+                    "velocityY": -0.03679467403373135,
+                    "timestamp": 0.2730859116512554
+                },
+                {
+                    "x": 2.541153647389717,
+                    "y": 5.546054192747997,
+                    "heading": 1.9668933595072364,
+                    "angularVelocity": -4.696812202508044,
+                    "velocityX": 1.936421628430232,
+                    "velocityY": -0.0010588347507973917,
+                    "timestamp": 0.3034287907236171
+                },
+                {
+                    "x": 2.60683848805178,
+                    "y": 5.546562966972454,
+                    "heading": 1.8368003827209543,
+                    "angularVelocity": -4.287430222954007,
+                    "velocityX": 2.164753071236843,
+                    "velocityY": 0.016767499987179104,
+                    "timestamp": 0.33377166979597883
+                },
+                {
+                    "x": 2.679458734246464,
+                    "y": 5.546832906614029,
+                    "heading": 1.7192242968736162,
+                    "angularVelocity": -3.874915282987571,
+                    "velocityX": 2.3933208849924683,
+                    "velocityY": 0.008896309441567017,
+                    "timestamp": 0.36411454886834055
+                },
+                {
+                    "x": 2.7588038922481295,
+                    "y": 5.545750285790584,
+                    "heading": 1.6147732040322635,
+                    "angularVelocity": -3.442359328930443,
+                    "velocityX": 2.6149515282463223,
+                    "velocityY": -0.035679568206544436,
+                    "timestamp": 0.39445742794070227
+                },
+                {
+                    "x": 2.844227039552491,
+                    "y": 5.54174873240555,
+                    "heading": 1.5239846755378514,
+                    "angularVelocity": -2.992086818060329,
+                    "velocityX": 2.8152617653929393,
+                    "velocityY": -0.13187784110699777,
+                    "timestamp": 0.424800307013064
+                },
+                {
+                    "x": 2.9352842549562403,
+                    "y": 5.535535053694752,
+                    "heading": 1.4520400757805025,
+                    "angularVelocity": -2.371053833941581,
+                    "velocityX": 3.0009418416293347,
+                    "velocityY": -0.20478210706307357,
+                    "timestamp": 0.4551431860854257
+                },
+                {
+                    "x": 3.0322214391168845,
+                    "y": 5.5275999113119605,
+                    "heading": 1.3988891918450945,
+                    "angularVelocity": -1.7516756998785012,
+                    "velocityX": 3.1947259826421948,
+                    "velocityY": -0.26151580289620013,
+                    "timestamp": 0.4854860651577874
+                },
+                {
+                    "x": 3.1352234766500766,
+                    "y": 5.5186341666131185,
+                    "heading": 1.36445468986127,
+                    "angularVelocity": -1.1348462320172443,
+                    "velocityX": 3.394603303383066,
+                    "velocityY": -0.2954810147534299,
+                    "timestamp": 0.5158289442301491
+                },
+                {
+                    "x": 3.2443643044344928,
+                    "y": 5.509491432490049,
+                    "heading": 1.3485232387156947,
+                    "angularVelocity": -0.525047445484061,
+                    "velocityX": 3.5969173368201925,
+                    "velocityY": -0.3013139953287366,
+                    "timestamp": 0.5461718233025108
+                },
+                {
+                    "x": 3.358782949449409,
+                    "y": 5.500338621504484,
+                    "heading": 1.3485196823103536,
+                    "angularVelocity": -0.00011720724762443023,
+                    "velocityX": 3.7708565736972712,
+                    "velocityY": -0.3016460950767756,
+                    "timestamp": 0.5765147023748725
+                },
+                {
+                    "x": 3.4684760810380606,
+                    "y": 5.486343295947389,
+                    "heading": 1.335475696910559,
+                    "angularVelocity": -0.42988621378634684,
+                    "velocityX": 3.615119426441224,
+                    "velocityY": -0.4612392094935691,
+                    "timestamp": 0.6068575814472341
+                },
+                {
+                    "x": 3.573383189765519,
+                    "y": 5.467459869277216,
+                    "heading": 1.309845724977452,
+                    "angularVelocity": -0.8446783138799892,
+                    "velocityX": 3.457388090209756,
+                    "velocityY": -0.6223347041373206,
+                    "timestamp": 0.6372004605195958
+                },
+                {
+                    "x": 3.6735704123100494,
+                    "y": 5.443371550860819,
+                    "heading": 1.272922802934567,
+                    "angularVelocity": -1.216856250022656,
+                    "velocityX": 3.301836398108563,
+                    "velocityY": -0.7938705605012504,
+                    "timestamp": 0.6675433395919574
+                },
+                {
+                    "x": 3.7692345180845526,
+                    "y": 5.4136136865914954,
+                    "heading": 1.2267696953368106,
+                    "angularVelocity": -1.521052352602741,
+                    "velocityX": 3.152769568977396,
+                    "velocityY": -0.9807198650582027,
+                    "timestamp": 0.6978862186643191
+                },
+                {
+                    "x": 3.860739036058752,
+                    "y": 5.377644621585128,
+                    "heading": 1.1742567314728194,
+                    "angularVelocity": -1.730651983905632,
+                    "velocityX": 3.0156834411124778,
+                    "velocityY": -1.1854203063785596,
+                    "timestamp": 0.7282290977366808
+                },
+                {
+                    "x": 3.9485394410884225,
+                    "y": 5.335005141499111,
+                    "heading": 1.1187025848702756,
+                    "angularVelocity": -1.830879214528657,
+                    "velocityX": 2.893608243973284,
+                    "velocityY": -1.4052549194270891,
+                    "timestamp": 0.7585719768090424
+                },
+                {
+                    "x": 4.032804489135742,
+                    "y": 5.285560607910156,
+                    "heading": 1.064415461383156,
+                    "angularVelocity": -1.7891223623722634,
+                    "velocityX": 2.7770946799861798,
+                    "velocityY": -1.6295267654410455,
+                    "timestamp": 0.7889148558814041
+                },
+                {
+                    "x": 4.1733297943826075,
+                    "y": 5.174158598481438,
+                    "heading": 0.9768931525481981,
+                    "angularVelocity": -1.5934853179105337,
+                    "velocityX": 2.55849067153879,
+                    "velocityY": -2.0282539248952025,
+                    "timestamp": 0.8438399360507789
+                },
+                {
+                    "x": 4.303490854924698,
+                    "y": 5.0407757547823575,
+                    "heading": 0.9131112872795231,
+                    "angularVelocity": -1.1612521105474607,
+                    "velocityX": 2.369792818521288,
+                    "velocityY": -2.428450596480911,
+                    "timestamp": 0.8987650162201537
+                },
+                {
+                    "x": 4.424699657845323,
+                    "y": 4.885599604328108,
+                    "heading": 0.881895788498386,
+                    "angularVelocity": -0.5683286885495016,
+                    "velocityX": 2.206802476152012,
+                    "velocityY": -2.8252330260734433,
+                    "timestamp": 0.9536900963895285
+                },
+                {
+                    "x": 4.534874604263042,
+                    "y": 4.709437065112464,
+                    "heading": 0.8818953110956026,
+                    "angularVelocity": -0.000008691890515565163,
+                    "velocityX": 2.005913256347883,
+                    "velocityY": -3.2073242073093726,
+                    "timestamp": 1.0086151765589033
+                },
+                {
+                    "x": 4.656392865207935,
+                    "y": 4.550190331352248,
+                    "heading": 0.8602662015486029,
+                    "angularVelocity": -0.3937929536070082,
+                    "velocityX": 2.2124366604502335,
+                    "velocityY": -2.899344584826112,
+                    "timestamp": 1.0635402567282781
+                },
+                {
+                    "x": 4.786084989350276,
+                    "y": 4.412405730848589,
+                    "heading": 0.8009192530896463,
+                    "angularVelocity": -1.0805072705573824,
+                    "velocityX": 2.3612550722257284,
+                    "velocityY": -2.508591704896303,
+                    "timestamp": 1.118465336897653
+                },
+                {
+                    "x": 4.924876209285355,
+                    "y": 4.297327162575611,
+                    "heading": 0.7205421769770328,
+                    "angularVelocity": -1.463394789133692,
+                    "velocityX": 2.5269188412121024,
+                    "velocityY": -2.0951916304556226,
+                    "timestamp": 1.1733904170670277
+                },
+                {
+                    "x": 5.072286801869267,
+                    "y": 4.205735025902594,
+                    "heading": 0.6308477205816693,
+                    "angularVelocity": -1.6330327806307947,
+                    "velocityX": 2.6838484737634416,
+                    "velocityY": -1.6675831221469524,
+                    "timestamp": 1.2283154972364025
+                },
+                {
+                    "x": 5.227765920434763,
+                    "y": 4.1379313687941846,
+                    "heading": 0.5364496515071664,
+                    "angularVelocity": -1.7186696639022372,
+                    "velocityX": 2.830749050998892,
+                    "velocityY": -1.2344753416712253,
+                    "timestamp": 1.2832405774057773
+                },
+                {
+                    "x": 5.390789031982422,
+                    "y": 4.09412145614624,
+                    "heading": 0.44081122980213755,
+                    "angularVelocity": -1.7412522914869581,
+                    "velocityX": 2.968099655839153,
+                    "velocityY": -0.7976303814732078,
+                    "timestamp": 1.3381656575751522
+                },
+                {
+                    "x": 5.527208209694737,
+                    "y": 4.074474240030864,
+                    "heading": 0.3646632920053755,
+                    "angularVelocity": -1.7150127332494942,
+                    "velocityX": 3.0724486257327652,
+                    "velocityY": -0.44249689204593257,
+                    "timestamp": 1.3825664578260528
+                },
+                {
+                    "x": 5.669753286288942,
+                    "y": 4.0699039581909835,
+                    "heading": 0.2967653403322419,
+                    "angularVelocity": -1.529205583896121,
+                    "velocityX": 3.2104168345775173,
+                    "velocityY": -0.10293242045310311,
+                    "timestamp": 1.4269672580769535
+                },
+                {
+                    "x": 5.825688318788907,
+                    "y": 4.069335183916362,
+                    "heading": 0.2632559067133074,
+                    "angularVelocity": -0.7547033708757247,
+                    "velocityX": 3.5119869826400643,
+                    "velocityY": -0.012810000527210512,
+                    "timestamp": 1.4713680583278541
+                },
+                {
+                    "x": 5.986467377363025,
+                    "y": 4.053794613625281,
+                    "heading": 0.24734267435752091,
+                    "angularVelocity": -0.35839967446225596,
+                    "velocityX": 3.6210847026536497,
+                    "velocityY": -0.3500065359917586,
+                    "timestamp": 1.5157688585787548
+                },
+                {
+                    "x": 6.146135354603106,
+                    "y": 4.023716272835588,
+                    "heading": 0.25998224016695326,
+                    "angularVelocity": 0.28466977482406836,
+                    "velocityX": 3.5960607993060463,
+                    "velocityY": -0.6774278981398296,
+                    "timestamp": 1.5601696588296554
+                },
+                {
+                    "x": 6.299693722526358,
+                    "y": 3.978509002442476,
+                    "heading": 0.279745523920822,
+                    "angularVelocity": 0.4451109809325518,
+                    "velocityX": 3.4584594659447885,
+                    "velocityY": -1.0181634145703302,
+                    "timestamp": 1.604570459080556
+                },
+                {
+                    "x": 6.446348354717166,
+                    "y": 3.918416502871273,
+                    "heading": 0.30303477341760615,
+                    "angularVelocity": 0.5245231924915958,
+                    "velocityX": 3.3029727248628453,
+                    "velocityY": -1.3534102816082356,
+                    "timestamp": 1.6489712593314567
+                },
+                {
+                    "x": 6.585501043822193,
+                    "y": 3.8437018977675175,
+                    "heading": 0.32757979886137184,
+                    "angularVelocity": 0.5528059247821282,
+                    "velocityX": 3.134013088023211,
+                    "velocityY": -1.6827310472234156,
+                    "timestamp": 1.6933720595823574
+                },
+                {
+                    "x": 6.716485001612935,
+                    "y": 3.754724015839551,
+                    "heading": 0.3514161297676369,
+                    "angularVelocity": 0.5368446237808859,
+                    "velocityX": 2.950035968959475,
+                    "velocityY": -2.0039702308329863,
+                    "timestamp": 1.737772859833258
+                },
+                {
+                    "x": 6.838464987128356,
+                    "y": 3.6520072183603935,
+                    "heading": 0.37305697715168934,
+                    "angularVelocity": 0.48739768791922733,
+                    "velocityX": 2.747247455589403,
+                    "velocityY": -2.313399688715609,
+                    "timestamp": 1.7821736600841587
+                },
+                {
+                    "x": 6.950225204946805,
+                    "y": 3.536435013339574,
+                    "heading": 0.39217088238591646,
+                    "angularVelocity": 0.430485602201265,
+                    "velocityX": 2.5170766559817053,
+                    "velocityY": -2.60293067619823,
+                    "timestamp": 1.8265744603350593
+                },
+                {
+                    "x": 7.049675913816151,
+                    "y": 3.409971379240331,
+                    "heading": 0.4121286773514249,
+                    "angularVelocity": 0.44949178511942617,
+                    "velocityX": 2.23984046024776,
+                    "velocityY": -2.848228711748902,
+                    "timestamp": 1.87097526058596
+                },
+                {
+                    "x": 7.134078905097545,
+                    "y": 3.2784202019721485,
+                    "heading": 0.44760621801548245,
+                    "angularVelocity": 0.799029307210242,
+                    "velocityX": 1.9009340102981855,
+                    "velocityY": -2.9628109521632653,
+                    "timestamp": 1.9153760608368606
+                },
+                {
+                    "x": 7.206535248844668,
+                    "y": 3.1515334712387078,
+                    "heading": 0.5188452438191252,
+                    "angularVelocity": 1.6044536450037898,
+                    "velocityX": 1.6318702216556016,
+                    "velocityY": -2.857757743473708,
+                    "timestamp": 1.9597768610877613
+                },
+                {
+                    "x": 7.286286308378336,
+                    "y": 3.038022763329368,
+                    "heading": 0.612459613040285,
+                    "angularVelocity": 2.1083937382245868,
+                    "velocityX": 1.796162661100898,
+                    "velocityY": -2.556501397900756,
+                    "timestamp": 2.004177661338662
+                },
+                {
+                    "x": 7.36964311440034,
+                    "y": 2.9389690479632176,
+                    "heading": 0.7307138885874557,
+                    "angularVelocity": 2.6633365812989322,
+                    "velocityX": 1.8773717039100728,
+                    "velocityY": -2.2308993262827714,
+                    "timestamp": 2.0485784615895626
+                },
+                {
+                    "x": 7.443994240643498,
+                    "y": 2.8513361754602453,
+                    "heading": 0.8783950719218284,
+                    "angularVelocity": 3.326092829405186,
+                    "velocityX": 1.674544733946552,
+                    "velocityY": -1.9736777717467908,
+                    "timestamp": 2.0929792618404632
+                },
+                {
+                    "x": 7.506362936213548,
+                    "y": 2.772668193478103,
+                    "heading": 1.052033337356815,
+                    "angularVelocity": 3.9107012588464345,
+                    "velocityX": 1.4046750332790265,
+                    "velocityY": -1.7717694622079865,
+                    "timestamp": 2.137380062091364
+                },
+                {
+                    "x": 7.555798075190431,
+                    "y": 2.7014303099088472,
+                    "heading": 1.2519057769914284,
+                    "angularVelocity": 4.501550388848195,
+                    "velocityX": 1.1133839637469243,
+                    "velocityY": -1.6044279194677442,
+                    "timestamp": 2.1817808623422645
+                },
+                {
+                    "x": 7.592358210662926,
+                    "y": 2.6364422153707756,
+                    "heading": 1.482218675635827,
+                    "angularVelocity": 5.187133955760781,
+                    "velocityX": 0.8234116337070656,
+                    "velocityY": -1.4636694422360859,
+                    "timestamp": 2.226181662593165
+                },
+                {
+                    "x": 7.621254126448735,
+                    "y": 2.5813646495023557,
+                    "heading": 1.7524007538852981,
+                    "angularVelocity": 6.085072267227659,
+                    "velocityX": 0.6507971843417887,
+                    "velocityY": -1.2404633600562773,
+                    "timestamp": 2.270582462844066
+                },
+                {
+                    "x": 7.645052051647252,
+                    "y": 2.5375067096061756,
+                    "heading": 2.067951038154778,
+                    "angularVelocity": 7.10686029275066,
+                    "velocityX": 0.5359796459532341,
+                    "velocityY": -0.9877736357981667,
+                    "timestamp": 2.3149832630949665
+                },
+                {
+                    "x": 7.661427436541854,
+                    "y": 2.5075370043722347,
+                    "heading": 2.400674072151774,
+                    "angularVelocity": 7.493626964307851,
+                    "velocityX": 0.36880832782444795,
+                    "velocityY": -0.6749811954871879,
+                    "timestamp": 2.359384063345867
+                },
+                {
+                    "x": 7.6692908711764884,
+                    "y": 2.4875264930527843,
+                    "heading": 2.6956822674348517,
+                    "angularVelocity": 6.644208969569037,
+                    "velocityX": 0.17710119164969557,
+                    "velocityY": -0.4506790689891867,
+                    "timestamp": 2.403784863596768
+                },
+                {
+                    "x": 7.674629616295112,
+                    "y": 2.472122256604734,
+                    "heading": 2.9172973649743583,
+                    "angularVelocity": 4.991241065188027,
+                    "velocityX": 0.12023984001312674,
+                    "velocityY": -0.3469360092836942,
+                    "timestamp": 2.4481856638476684
+                },
+                {
+                    "x": 7.678206340030146,
+                    "y": 2.4619495353215974,
+                    "heading": 3.0655099943755024,
+                    "angularVelocity": 3.3380621196830216,
+                    "velocityX": 0.0805553889754595,
+                    "velocityY": -0.22911121479010477,
+                    "timestamp": 2.492586464098569
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
                     "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "angularVelocity": 1.6776725915652129,
+                    "velocityX": 0.04039701896629419,
+                    "velocityY": -0.11147401158601906,
+                    "timestamp": 2.5369872643494698
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": -5.831834401931518e-35,
+                    "velocityX": -1.3540056730700436e-35,
+                    "velocityY": -1.938442767179852e-35,
+                    "timestamp": 2.5813880646003704
                 }
             ],
-            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -6351,7 +17204,25 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 27
+                },
+                {
+                    "x": 2.1495614051818848,
+                    "y": 3.005171298980713,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 4.929586887359619,
+                    "y": 1.5703192949295044,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 28
                 },
                 {
                     "x": 7.68,
@@ -6363,7 +17234,629 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 3.14,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": -2.730529759762368e-29,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.2482118663055717,
+                    "y": 5.55085896880981,
+                    "heading": 3.1399999903330857,
+                    "angularVelocity": -2.8923280730678937e-7,
+                    "velocityX": -0.05350072563170815,
+                    "velocityY": -0.2734984543883791,
+                    "timestamp": 0.03342260639319256
+                },
+                {
+                    "x": 2.24463557685663,
+                    "y": 5.5325769109933445,
+                    "heading": 3.1399999700354155,
+                    "angularVelocity": -6.073036282303753e-7,
+                    "velocityX": -0.10700211129830166,
+                    "velocityY": -0.5469967722250598,
+                    "timestamp": 0.06684521278638512
+                },
+                {
+                    "x": 2.2392711054637595,
+                    "y": 5.505153831967315,
+                    "heading": 3.139999938021822,
+                    "angularVelocity": -9.578425018869592e-7,
+                    "velocityX": -0.16050428054878876,
+                    "velocityY": -0.8204949279944175,
+                    "timestamp": 0.1002678191795777
+                },
+                {
+                    "x": 2.2321184205712044,
+                    "y": 5.468589738256669,
+                    "heading": 3.139999893047122,
+                    "angularVelocity": -0.0000013456371104363374,
+                    "velocityX": -0.2140073939433933,
+                    "velocityY": -1.0939928885381913,
+                    "timestamp": 0.13369042557277025
+                },
+                {
+                    "x": 2.2231774834707245,
+                    "y": 5.422884637863401,
+                    "heading": 3.1399998336497137,
+                    "angularVelocity": -0.000001777162647778402,
+                    "velocityX": -0.2675116654835123,
+                    "velocityY": -1.3674906096633432,
+                    "timestamp": 0.1671130319659628
+                },
+                {
+                    "x": 2.2124482456246803,
+                    "y": 5.368038540819271,
+                    "heading": 3.139999758066302,
+                    "angularVelocity": -0.0000022614457562380146,
+                    "velocityX": -0.3210173892634857,
+                    "velocityY": -1.6409880306395483,
+                    "timestamp": 0.20053563835915536
+                },
+                {
+                    "x": 2.199930644465073,
+                    "y": 5.30405146005317,
+                    "heading": 3.139999664095155,
+                    "angularVelocity": -0.0000028116043956655657,
+                    "velocityX": -0.3745249850698391,
+                    "velocityY": -1.9144850647849765,
+                    "timestamp": 0.23395824475234792
+                },
+                {
+                    "x": 2.1856245964060905,
+                    "y": 5.230923412833771,
+                    "heading": 3.1399995488597034,
+                    "angularVelocity": -0.0000034478296167107145,
+                    "velocityX": -0.4280350817528267,
+                    "velocityY": -2.1879815822539745,
+                    "timestamp": 0.2673808511455405
+                },
+                {
+                    "x": 2.1695299843090114,
+                    "y": 5.148654423357543,
+                    "heading": 3.139999408375343,
+                    "angularVelocity": -0.000004203273662517842,
+                    "velocityX": -0.4815486832110918,
+                    "velocityY": -2.461477375767798,
+                    "timestamp": 0.30080345753873305
+                },
+                {
+                    "x": 2.15164663254415,
+                    "y": 5.057244527897619,
+                    "heading": 3.139999236662074,
+                    "angularVelocity": -0.000005137638535462322,
+                    "velocityX": -0.535067539486715,
+                    "velocityY": -2.7349720839942284,
+                    "timestamp": 0.3342260639319256
+                },
+                {
+                    "x": 2.1319742493327887,
+                    "y": 4.956693786709351,
+                    "heading": 3.1399990236359914,
+                    "angularVelocity": -0.000006373712524248939,
+                    "velocityX": -0.5885951257435309,
+                    "velocityY": -3.008464989388312,
+                    "timestamp": 0.36764867032511817
+                },
+                {
+                    "x": 2.110512257081521,
+                    "y": 4.847002319075526,
+                    "heading": 3.13999874874802,
+                    "angularVelocity": -0.000008224612190539215,
+                    "velocityX": -0.6421399935196433,
+                    "velocityY": -3.28195432585699,
+                    "timestamp": 0.4010712767183107
+                },
+                {
+                    "x": 2.087258999095236,
+                    "y": 4.728170467468909,
+                    "heading": 3.139998349832889,
+                    "angularVelocity": -0.00001193548864433131,
+                    "velocityX": -0.6957344293958355,
+                    "velocityY": -3.555433415585576,
+                    "timestamp": 0.4344938831115033
+                },
+                {
+                    "x": 2.0629644962421034,
+                    "y": 4.604090796237193,
+                    "heading": 3.1399983498325463,
+                    "angularVelocity": -1.0248884056674418e-11,
+                    "velocityX": -0.7268883391614586,
+                    "velocityY": -3.712447490549055,
+                    "timestamp": 0.46791648950469583
+                },
+                {
+                    "x": 2.0387448816492295,
+                    "y": 4.479996482972041,
+                    "heading": 3.139998349833487,
+                    "angularVelocity": 2.8141899837441888e-11,
+                    "velocityX": -0.7246476922026518,
+                    "velocityY": -3.712885578275693,
+                    "timestamp": 0.5013390958978884
+                },
+                {
+                    "x": 2.0146234361555044,
+                    "y": 4.355883050210464,
+                    "heading": 3.139998349833582,
+                    "angularVelocity": 2.8336640394810287e-12,
+                    "velocityX": -0.7217104856241171,
+                    "velocityY": -3.713457631102453,
+                    "timestamp": 0.534761702291081
+                },
+                {
+                    "x": 1.9906204010106838,
+                    "y": 4.231746663125483,
+                    "heading": 3.1399983498337,
+                    "angularVelocity": 3.5336881649516105e-12,
+                    "velocityX": -0.7181676635393196,
+                    "velocityY": -3.714144421443354,
+                    "timestamp": 0.5681843086842736
+                },
+                {
+                    "x": 1.9667619480914416,
+                    "y": 4.107582407046251,
+                    "heading": 3.139998349833824,
+                    "angularVelocity": 3.708930519775865e-12,
+                    "velocityX": -0.7138417823660727,
+                    "velocityY": -3.7149782580858504,
+                    "timestamp": 0.6016069150774662
+                },
+                {
+                    "x": 1.9503914931986956,
+                    "y": 3.982210973196183,
+                    "heading": 3.1399983498339594,
+                    "angularVelocity": 4.046203674508551e-12,
+                    "velocityX": -0.4898018634439243,
+                    "velocityY": -3.751096858670778,
+                    "timestamp": 0.6350295214706588
+                },
+                {
+                    "x": 1.943224502199385,
+                    "y": 3.8559785593201936,
+                    "heading": 3.1399983498340998,
+                    "angularVelocity": 4.203331220184306e-12,
+                    "velocityX": -0.21443543076194166,
+                    "velocityY": -3.7768572681307018,
+                    "timestamp": 0.6684521278638514
+                },
+                {
+                    "x": 1.9453593714612354,
+                    "y": 3.7295608780305693,
+                    "heading": 3.1399983498342436,
+                    "angularVelocity": 4.304427970302576e-12,
+                    "velocityX": 0.06387500833733431,
+                    "velocityY": -3.7824004448488933,
+                    "timestamp": 0.701874734257044
+                },
+                {
+                    "x": 1.9567930300665735,
+                    "y": 3.6036432110345795,
+                    "heading": 3.139998349834389,
+                    "angularVelocity": 4.350609924565741e-12,
+                    "velocityX": 0.3420935659816486,
+                    "velocityY": -3.767440082758668,
+                    "timestamp": 0.7352973406502367
+                },
+                {
+                    "x": 1.9774653925716308,
+                    "y": 3.4789089323576285,
+                    "heading": 3.1399983498345336,
+                    "angularVelocity": 4.327887690330789e-12,
+                    "velocityX": 0.6185143750313882,
+                    "velocityY": -3.732033259450771,
+                    "timestamp": 0.7687199470434293
+                },
+                {
+                    "x": 2.0072643339986427,
+                    "y": 3.3560349759367867,
+                    "heading": 3.1399983498346824,
+                    "angularVelocity": 4.434032141487508e-12,
+                    "velocityX": 0.8915804194501273,
+                    "velocityY": -3.6763726615238776,
+                    "timestamp": 0.8021425534366219
+                },
+                {
+                    "x": 2.0460281463567345,
+                    "y": 3.235688177587346,
+                    "heading": 3.1399983498348414,
+                    "angularVelocity": 4.757708405328401e-12,
+                    "velocityX": 1.1598081819894186,
+                    "velocityY": -3.600760423457161,
+                    "timestamp": 0.8355651598298145
+                },
+                {
+                    "x": 2.093546464867386,
+                    "y": 3.118521659475836,
+                    "heading": 3.139998349835024,
+                    "angularVelocity": 5.453307843013083e-12,
+                    "velocityX": 1.421741857944163,
+                    "velocityY": -3.5056068558230677,
+                    "timestamp": 0.8689877662230071
+                },
+                {
+                    "x": 2.1495614051818848,
+                    "y": 3.005171298980713,
+                    "heading": 3.1399983498353623,
+                    "angularVelocity": 1.0122205936259764e-11,
+                    "velocityX": 1.67595966800623,
+                    "velocityY": -3.391427920419207,
+                    "timestamp": 0.9024103726161997
+                },
+                {
+                    "x": 2.2887605263975965,
+                    "y": 2.8026901614733335,
+                    "heading": 3.1399983498355466,
+                    "angularVelocity": 2.834267287592998e-12,
+                    "velocityX": 2.1430749184844986,
+                    "velocityY": -3.117349042649137,
+                    "timestamp": 0.9673633614337364
+                },
+                {
+                    "x": 2.4554470535268806,
+                    "y": 2.6221614552311943,
+                    "heading": 3.1399983498356696,
+                    "angularVelocity": 1.896559244966036e-12,
+                    "velocityX": 2.566264157563543,
+                    "velocityY": -2.7793748914229504,
+                    "timestamp": 1.0323163502512729
+                },
+                {
+                    "x": 2.646204500588454,
+                    "y": 2.4672853384780784,
+                    "heading": 3.139998349835769,
+                    "angularVelocity": 1.528915692972437e-12,
+                    "velocityX": 2.9368540314174165,
+                    "velocityY": -2.384434027946127,
+                    "timestamp": 1.0972693390688095
+                },
+                {
+                    "x": 2.8571229789617445,
+                    "y": 2.341236093184693,
+                    "heading": 3.1399983498358592,
+                    "angularVelocity": 1.3872590523351098e-12,
+                    "velocityX": 3.247248236192457,
+                    "velocityY": -1.940622711722675,
+                    "timestamp": 1.1622223278863462
+                },
+                {
+                    "x": 3.0838578351814623,
+                    "y": 2.2465454263799622,
+                    "heading": 3.139998349835948,
+                    "angularVelocity": 1.37081210529667e-12,
+                    "velocityX": 3.490753240880161,
+                    "velocityY": -1.4578338661343044,
+                    "timestamp": 1.2271753167038828
+                },
+                {
+                    "x": 3.3145391093778356,
+                    "y": 2.1619217463740292,
+                    "heading": 3.1399983498360386,
+                    "angularVelocity": 1.3907396736325547e-12,
+                    "velocityX": 3.551511306806079,
+                    "velocityY": -1.3028450506530576,
+                    "timestamp": 1.2921283055214194
+                },
+                {
+                    "x": 3.5452163544414024,
+                    "y": 2.0772870838321214,
+                    "heading": 3.1399983498361257,
+                    "angularVelocity": 1.3371940783811797e-12,
+                    "velocityX": 3.5514492752906865,
+                    "velocityY": -1.3030141350149058,
+                    "timestamp": 1.357081294338956
+                },
+                {
+                    "x": 3.7758941865765565,
+                    "y": 1.9926540214142978,
+                    "heading": 3.139998349836204,
+                    "angularVelocity": 1.2081341732104707e-12,
+                    "velocityX": 3.5514583136980007,
+                    "velocityY": -1.302989499904737,
+                    "timestamp": 1.4220342831564927
+                },
+                {
+                    "x": 4.006572310105761,
+                    "y": 1.908021753240333,
+                    "heading": 3.1399983498362696,
+                    "angularVelocity": 1.0139841411422692e-12,
+                    "velocityX": 3.5514627999275414,
+                    "velocityY": -1.3029772719259731,
+                    "timestamp": 1.4869872719740294
+                },
+                {
+                    "x": 4.237248920507999,
+                    "y": 1.8233853608613613,
+                    "heading": 3.1399983498363224,
+                    "angularVelocity": 8.135622214219814e-13,
+                    "velocityX": 3.5514395042053684,
+                    "velocityY": -1.3030407671488682,
+                    "timestamp": 1.551940260791566
+                },
+                {
+                    "x": 4.46792193477352,
+                    "y": 1.7387391679278503,
+                    "heading": 3.1399983498363646,
+                    "angularVelocity": 6.458697792259404e-13,
+                    "velocityX": 3.551384138968564,
+                    "velocityY": -1.3031916540771904,
+                    "timestamp": 1.6168932496091026
+                },
+                {
+                    "x": 4.698662422545312,
+                    "y": 1.6542770762611563,
+                    "heading": 3.139998349836401,
+                    "angularVelocity": 5.573297491220617e-13,
+                    "velocityX": 3.5524229442068638,
+                    "velocityY": -1.3003572769873404,
+                    "timestamp": 1.6818462384266393
+                },
+                {
+                    "x": 4.929586887359619,
+                    "y": 1.5703192949295044,
+                    "heading": 3.139998349836444,
+                    "angularVelocity": 6.621983947605345e-13,
+                    "velocityX": 3.5552554088546553,
+                    "velocityY": -1.2925930408850796,
+                    "timestamp": 1.746799227244176
+                },
+                {
+                    "x": 5.059491461224377,
+                    "y": 1.5327116535816125,
+                    "heading": 3.1399983498365303,
+                    "angularVelocity": 2.411376289586005e-12,
+                    "velocityX": 3.6337293619618993,
+                    "velocityY": -1.0519721248789429,
+                    "timestamp": 1.7825488830178295
+                },
+                {
+                    "x": 5.189524196366832,
+                    "y": 1.4955495654455135,
+                    "heading": 3.1399983498366377,
+                    "angularVelocity": 3.0099354211020166e-12,
+                    "velocityX": 3.6373143273250914,
+                    "velocityY": -1.0395089779716884,
+                    "timestamp": 1.8182985387914832
+                },
+                {
+                    "x": 5.319583815273274,
+                    "y": 1.4584816742976305,
+                    "heading": 3.1399983498367723,
+                    "angularVelocity": 3.767098328405853e-12,
+                    "velocityX": 3.6380663279640872,
+                    "velocityY": -1.036874071811995,
+                    "timestamp": 1.8540481945651368
+                },
+                {
+                    "x": 5.4496589768626915,
+                    "y": 1.4214683610582035,
+                    "heading": 3.139998349836922,
+                    "angularVelocity": 4.1821295987378e-12,
+                    "velocityX": 3.6385010925132435,
+                    "velocityY": -1.0353474023301,
+                    "timestamp": 1.8897978503387904
+                },
+                {
+                    "x": 5.579738820656374,
+                    "y": 1.3844715066642421,
+                    "heading": 3.139998349837078,
+                    "angularVelocity": 4.36086747938318e-12,
+                    "velocityX": 3.6386320645231356,
+                    "velocityY": -1.0348870106108623,
+                    "timestamp": 1.925547506112444
+                },
+                {
+                    "x": 5.709817887256256,
+                    "y": 1.3474719197526863,
+                    "heading": 3.1399983498372332,
+                    "angularVelocity": 4.346123552895358e-12,
+                    "velocityX": 3.6386103246271904,
+                    "velocityY": -1.0349634454051337,
+                    "timestamp": 1.9612971618860977
+                },
+                {
+                    "x": 5.839896082809478,
+                    "y": 1.3104692706317995,
+                    "heading": 3.139998349837383,
+                    "angularVelocity": 4.183764792386501e-12,
+                    "velocityX": 3.638585959451041,
+                    "velocityY": -1.0350491024349304,
+                    "timestamp": 1.9970468176597513
+                },
+                {
+                    "x": 5.969973859581082,
+                    "y": 1.2734651493862033,
+                    "heading": 3.139998349837523,
+                    "angularVelocity": 3.914068769730236e-12,
+                    "velocityX": 3.6385742451671077,
+                    "velocityY": -1.0350902811450293,
+                    "timestamp": 2.032796473433405
+                },
+                {
+                    "x": 6.100051488327663,
+                    "y": 1.2364605078211643,
+                    "heading": 3.1399983498376502,
+                    "angularVelocity": 3.5614211180657343e-12,
+                    "velocityX": 3.6385701045671635,
+                    "velocityY": -1.0351048356752235,
+                    "timestamp": 2.068546129207059
+                },
+                {
+                    "x": 6.230129108868512,
+                    "y": 1.199455837420786,
+                    "heading": 3.1399983498377644,
+                    "angularVelocity": 3.192303325422517e-12,
+                    "velocityX": 3.6385698750339617,
+                    "velocityY": -1.0351056422659504,
+                    "timestamp": 2.1042957849807125
+                },
+                {
+                    "x": 6.360206785724816,
+                    "y": 1.162451364982862,
+                    "heading": 3.1399983498378674,
+                    "angularVelocity": 2.8800276139093587e-12,
+                    "velocityX": 3.638571450306583,
+                    "velocityY": -1.0351001048013342,
+                    "timestamp": 2.140045440754366
+                },
+                {
+                    "x": 6.490284555094277,
+                    "y": 1.1254472177490595,
+                    "heading": 3.1399983498379616,
+                    "angularVelocity": 2.621534784795507e-12,
+                    "velocityX": 3.638574038112148,
+                    "velocityY": -1.0350910080949738,
+                    "timestamp": 2.1757950965280197
+                },
+                {
+                    "x": 6.6203624349981105,
+                    "y": 1.0884434590721859,
+                    "heading": 3.1399983498380477,
+                    "angularVelocity": 2.4107623793093176e-12,
+                    "velocityX": 3.638577130012462,
+                    "velocityY": -1.0350801392650475,
+                    "timestamp": 2.2115447523016734
+                },
+                {
+                    "x": 6.75044029323431,
+                    "y": 1.0514396242316386,
+                    "heading": 3.1399983498381276,
+                    "angularVelocity": 2.2461604882641728e-12,
+                    "velocityX": 3.6385765239189465,
+                    "velocityY": -1.0350822697380073,
+                    "timestamp": 2.247294408075327
+                },
+                {
+                    "x": 6.880516952547443,
+                    "y": 1.0144315751292232,
+                    "heading": 3.139998349838195,
+                    "angularVelocity": 1.892677955933963e-12,
+                    "velocityX": 3.6385429872864736,
+                    "velocityY": -1.0352001523239789,
+                    "timestamp": 2.2830440638489806
+                },
+                {
+                    "x": 7.003514384307231,
+                    "y": 0.97944241190024,
+                    "heading": 3.1399986104136794,
+                    "angularVelocity": 0.000007288894915666013,
+                    "velocityX": 3.440520729445303,
+                    "velocityY": -0.9787272764389812,
+                    "timestamp": 2.3187937196226343
+                },
+                {
+                    "x": 7.116262004659963,
+                    "y": 0.9473688227429191,
+                    "heading": 3.139998845492451,
+                    "angularVelocity": 0.000006575693298715729,
+                    "velocityX": 3.1538099574045058,
+                    "velocityY": -0.8971719716796405,
+                    "timestamp": 2.354543375396288
+                },
+                {
+                    "x": 7.218759831907218,
+                    "y": 0.9182109395297621,
+                    "heading": 3.1399990575434797,
+                    "angularVelocity": 0.00000593155441577883,
+                    "velocityX": 2.8670996973009437,
+                    "velocityY": -0.8156129781434808,
+                    "timestamp": 2.3902930311699415
+                },
+                {
+                    "x": 7.3110078714654865,
+                    "y": 0.8919688038160335,
+                    "heading": 3.1399992473559544,
+                    "angularVelocity": 0.000005309490974163329,
+                    "velocityX": 2.5803895887090773,
+                    "velocityY": -0.7340528222110998,
+                    "timestamp": 2.426042686943595
+                },
+                {
+                    "x": 7.393006125886103,
+                    "y": 0.8686424358274811,
+                    "heading": 3.139999415401417,
+                    "angularVelocity": 0.000004700617645814799,
+                    "velocityX": 2.293679551483878,
+                    "velocityY": -0.6524921005180893,
+                    "timestamp": 2.461792342717249
+                },
+                {
+                    "x": 7.464754596626899,
+                    "y": 0.8482318474429659,
+                    "heading": 3.1399995619997028,
+                    "angularVelocity": 0.000004100690828648421,
+                    "velocityX": 2.0069695550376028,
+                    "velocityY": -0.5709310465460533,
+                    "timestamp": 2.4975419984909024
+                },
+                {
+                    "x": 7.526253284614184,
+                    "y": 0.8307370464214682,
+                    "heading": 3.1399996873763563,
+                    "angularVelocity": 0.00000350707302580956,
+                    "velocityX": 1.7202595845023039,
+                    "velocityY": -0.4893697755375005,
+                    "timestamp": 2.533291654264556
+                },
+                {
+                    "x": 7.577502190476697,
+                    "y": 0.8161580381894111,
+                    "heading": 3.139999791690666,
+                    "angularVelocity": 0.0000029179108844478315,
+                    "velocityX": 1.433549631554256,
+                    "velocityY": -0.40780835273943755,
+                    "timestamp": 2.5690413100382097
+                },
+                {
+                    "x": 7.618501314660242,
+                    "y": 0.8044948267260937,
+                    "heading": 3.1399998750527174,
+                    "angularVelocity": 0.000002331828092755085,
+                    "velocityX": 1.1468396910764296,
+                    "velocityY": -0.3262468186312345,
+                    "timestamp": 2.6047909658118633
+                },
+                {
+                    "x": 7.649250657490367,
+                    "y": 0.7957474150520292,
+                    "heading": 3.139999937534455,
+                    "angularVelocity": 0.0000017477577406037154,
+                    "velocityX": 0.860129759704899,
+                    "velocityY": -0.24468520003233896,
+                    "timestamp": 2.640540621585517
+                },
+                {
+                    "x": 7.669750219209449,
+                    "y": 0.789915805520138,
+                    "heading": 3.1399999791776474,
+                    "angularVelocity": 0.0000011648557571115021,
+                    "velocityX": 0.5734198351132136,
+                    "velocityY": -0.16312351561685162,
+                    "timestamp": 2.6762902773591706
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": 5.824490440069194e-7,
+                    "velocityX": 0.2867099156267607,
+                    "velocityY": -0.0815617789049158,
+                    "timestamp": 2.712039933132824
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": -6.025226692032396e-31,
+                    "velocityX": 4.372989448335815e-32,
+                    "velocityY": 7.524777583668013e-32,
+                    "timestamp": 2.747789588906478
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -6391,11 +17884,6 @@
                     "x": 5.61,
                     "y": 2.81,
                     "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
                 }
             ]
         },
@@ -7575,7 +19063,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 23
+                    "controlIntervalCount": 26
+                },
+                {
+                    "x": 4.276216983795166,
+                    "y": 6.32326602935791,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 30
                 },
                 {
                     "x": 7.68,
@@ -7587,7 +19084,521 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": -5.253317174178206e-37,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.3042260943612694,
+                    "y": 4.3705069747477046,
+                    "heading": 2.7457396895687083,
+                    "angularVelocity": -0.11528375442815593,
+                    "velocityX": 0.11435786954779384,
+                    "velocityY": 0.2843181303644528,
+                    "timestamp": 0.03695499381005128
+                },
+                {
+                    "x": 2.312934956515594,
+                    "y": 4.391423172843284,
+                    "heading": 2.737520345774514,
+                    "angularVelocity": -0.2224149687709801,
+                    "velocityX": 0.23566130734830049,
+                    "velocityY": 0.5659911134903584,
+                    "timestamp": 0.07390998762010256
+                },
+                {
+                    "x": 2.326431561960596,
+                    "y": 4.422622192917349,
+                    "heading": 2.725704837211404,
+                    "angularVelocity": -0.31972697989997184,
+                    "velocityX": 0.3652173645157235,
+                    "velocityY": 0.8442436828545633,
+                    "timestamp": 0.11086498143015384
+                },
+                {
+                    "x": 2.3450829408251197,
+                    "y": 4.463936524002756,
+                    "heading": 2.7107373323748916,
+                    "angularVelocity": -0.40501981717128094,
+                    "velocityX": 0.5047052357900914,
+                    "velocityY": 1.1179634151141484,
+                    "timestamp": 0.14781997524020513
+                },
+                {
+                    "x": 2.369336863920227,
+                    "y": 4.515137741117879,
+                    "heading": 2.6931719166575077,
+                    "angularVelocity": -0.4753191356943592,
+                    "velocityX": 0.6563097593730617,
+                    "velocityY": 1.3855019805522522,
+                    "timestamp": 0.18477496905025642
+                },
+                {
+                    "x": 2.3997470229979276,
+                    "y": 4.575903866240714,
+                    "heading": 2.6737152145246563,
+                    "angularVelocity": -0.5264972369596066,
+                    "velocityX": 0.8228971498144095,
+                    "velocityY": 1.6443278392948362,
+                    "timestamp": 0.22172996286030772
+                },
+                {
+                    "x": 2.4370055110126323,
+                    "y": 4.645763622209906,
+                    "heading": 2.6532916288606927,
+                    "angularVelocity": -0.5526610495171655,
+                    "velocityX": 1.0082125356646991,
+                    "velocityY": 1.890400965246281,
+                    "timestamp": 0.258684956670359
+                },
+                {
+                    "x": 2.4818450920186503,
+                    "y": 4.724119837508184,
+                    "heading": 2.6331026993833615,
+                    "angularVelocity": -0.5463112666478134,
+                    "velocityX": 1.213356474540179,
+                    "velocityY": 2.1203146644004023,
+                    "timestamp": 0.2956399504804103
+                },
+                {
+                    "x": 2.5345067531707874,
+                    "y": 4.810746963179403,
+                    "heading": 2.6138856788459637,
+                    "angularVelocity": -0.5200114668175445,
+                    "velocityX": 1.425021511918485,
+                    "velocityY": 2.3441250218165925,
+                    "timestamp": 0.3325949442904616
+                },
+                {
+                    "x": 2.595354080808564,
+                    "y": 4.90526717496031,
+                    "heading": 2.5968144984302115,
+                    "angularVelocity": -0.46194515695220434,
+                    "velocityX": 1.6465251746633045,
+                    "velocityY": 2.557711476471661,
+                    "timestamp": 0.3695499381005129
+                },
+                {
+                    "x": 2.6649832778781826,
+                    "y": 5.00693656288579,
+                    "heading": 2.584029518775602,
+                    "angularVelocity": -0.3459608116923114,
+                    "velocityX": 1.8841620547283313,
+                    "velocityY": 2.7511677703982533,
+                    "timestamp": 0.4065049319105642
+                },
+                {
+                    "x": 2.7444162884451204,
+                    "y": 5.113834606219336,
+                    "heading": 2.580154938465408,
+                    "angularVelocity": -0.10484591960993231,
+                    "velocityX": 2.149452682233515,
+                    "velocityY": 2.8926548840192674,
+                    "timestamp": 0.4434599257206155
+                },
+                {
+                    "x": 2.835170549378822,
+                    "y": 5.2201703041601135,
+                    "heading": 2.5801550588658806,
+                    "angularVelocity": 0.0000032580298363436347,
+                    "velocityX": 2.4558050638616957,
+                    "velocityY": 2.877437850141254,
+                    "timestamp": 0.4804149195306668
+                },
+                {
+                    "x": 2.934277899482491,
+                    "y": 5.318767249922712,
+                    "heading": 2.580155115984828,
+                    "angularVelocity": 0.0000015456354283753369,
+                    "velocityX": 2.681839174783285,
+                    "velocityY": 2.6680276627669444,
+                    "timestamp": 0.517369913340718
+                },
+                {
+                    "x": 3.041080419735732,
+                    "y": 5.408971720433865,
+                    "heading": 2.5801551701001046,
+                    "angularVelocity": 0.0000014643562563831181,
+                    "velocityX": 2.8900700349783843,
+                    "velocityY": 2.440927766753384,
+                    "timestamp": 0.5543249071507693
+                },
+                {
+                    "x": 3.1521380355083095,
+                    "y": 5.493882443444743,
+                    "heading": 2.5801552231562916,
+                    "angularVelocity": 0.0000014356973685406667,
+                    "velocityX": 3.005212674189958,
+                    "velocityY": 2.2976792648733673,
+                    "timestamp": 0.5912799009608206
+                },
+                {
+                    "x": 3.263196121755215,
+                    "y": 5.578792551102765,
+                    "heading": 2.580155276212398,
+                    "angularVelocity": 0.0000014356951704765205,
+                    "velocityX": 3.005225405198131,
+                    "velocityY": 2.29766261346058,
+                    "timestamp": 0.6282348947708719
+                },
+                {
+                    "x": 3.374254224320857,
+                    "y": 5.6637026374167005,
+                    "heading": 2.580155329268507,
+                    "angularVelocity": 0.0000014356952512853246,
+                    "velocityX": 3.0052258467821833,
+                    "velocityY": 2.297662035890833,
+                    "timestamp": 0.6651898885809232
+                },
+                {
+                    "x": 3.485312339954463,
+                    "y": 5.748612706638388,
+                    "heading": 2.5801553823246177,
+                    "angularVelocity": 0.0000014356952943350777,
+                    "velocityX": 3.0052262004005508,
+                    "velocityY": 2.2976615733755907,
+                    "timestamp": 0.7021448823909745
+                },
+                {
+                    "x": 3.5963704664420417,
+                    "y": 5.833522761663614,
+                    "heading": 2.58015543538073,
+                    "angularVelocity": 0.0000014356953270064253,
+                    "velocityX": 3.0052264941084177,
+                    "velocityY": 2.297661189220169,
+                    "timestamp": 0.7390998762010258
+                },
+                {
+                    "x": 3.70742860215257,
+                    "y": 5.918432804625673,
+                    "heading": 2.5801554884368434,
+                    "angularVelocity": 0.0000014356953601191765,
+                    "velocityX": 3.0052267436809124,
+                    "velocityY": 2.2976608627915387,
+                    "timestamp": 0.7760548700110771
+                },
+                {
+                    "x": 3.8184867458225393,
+                    "y": 6.00334283717717,
+                    "heading": 2.580155541492958,
+                    "angularVelocity": 0.0000014356953984229,
+                    "velocityX": 3.005226959062899,
+                    "velocityY": 2.2976605810823423,
+                    "timestamp": 0.8130098638211284
+                },
+                {
+                    "x": 3.9295448964619117,
+                    "y": 6.088252860613022,
+                    "heading": 2.580155594549074,
+                    "angularVelocity": 0.0000014356954384844398,
+                    "velocityX": 3.0052271476545696,
+                    "velocityY": 2.2976603344135205,
+                    "timestamp": 0.8499648576311797
+                },
+                {
+                    "x": 4.040603429478696,
+                    "y": 6.173162383915615,
+                    "heading": 2.5801556476051415,
+                    "angularVelocity": 0.0000014356941260173984,
+                    "velocityX": 3.005237494765267,
+                    "velocityY": 2.2976468008363797,
+                    "timestamp": 0.886919851441231
+                },
+                {
+                    "x": 4.155351262344264,
+                    "y": 6.253015747837148,
+                    "heading": 2.580155700374003,
+                    "angularVelocity": 0.0000014279223609881846,
+                    "velocityX": 3.105069735781099,
+                    "velocityY": 2.1608274197522537,
+                    "timestamp": 0.9238748452512823
+                },
+                {
+                    "x": 4.276216983795166,
+                    "y": 6.32326602935791,
+                    "heading": 2.5801557537673054,
+                    "angularVelocity": 0.0000014448196748631945,
+                    "velocityX": 3.2706194478654775,
+                    "velocityY": 1.9009685641363763,
+                    "timestamp": 0.9608298390613336
+                },
+                {
+                    "x": 4.413687737524023,
+                    "y": 6.387653258704535,
+                    "heading": 2.580155801751457,
+                    "angularVelocity": 0.0000011957738462706154,
+                    "velocityX": 3.425796355950964,
+                    "velocityY": 1.604541545618551,
+                    "timestamp": 1.0009579551094623
+                },
+                {
+                    "x": 4.556309721411339,
+                    "y": 6.439642616989784,
+                    "heading": 2.580155847910912,
+                    "angularVelocity": 0.0000011503020593677978,
+                    "velocityX": 3.55416595476994,
+                    "velocityY": 1.2955843285264883,
+                    "timestamp": 1.041086071157591
+                },
+                {
+                    "x": 4.700491299069936,
+                    "y": 6.48713573606665,
+                    "heading": 2.580155893776208,
+                    "angularVelocity": 0.000001142971587913107,
+                    "velocityX": 3.593031317136076,
+                    "velocityY": 1.1835372241224518,
+                    "timestamp": 1.08121418720572
+                },
+                {
+                    "x": 4.844672963256385,
+                    "y": 6.5346285924578735,
+                    "heading": 2.5801559396414997,
+                    "angularVelocity": 0.0000011429714605909094,
+                    "velocityX": 3.5930334734259763,
+                    "velocityY": 1.1835306779481511,
+                    "timestamp": 1.1213423032538488
+                },
+                {
+                    "x": 4.988854630036052,
+                    "y": 6.58212144097644,
+                    "heading": 2.5801559855067953,
+                    "angularVelocity": 0.0000011429715641749761,
+                    "velocityX": 3.5930335380494514,
+                    "velocityY": 1.18353048176008,
+                    "timestamp": 1.1614704193019776
+                },
+                {
+                    "x": 5.133036299227264,
+                    "y": 6.62961428217388,
+                    "heading": 2.580156031372096,
+                    "angularVelocity": 0.0000011429716937026712,
+                    "velocityX": 3.593033598145609,
+                    "velocityY": 1.1835302993162664,
+                    "timestamp": 1.2015985353501064
+                },
+                {
+                    "x": 5.277217970668299,
+                    "y": 6.677107116541164,
+                    "heading": 2.580156077237403,
+                    "angularVelocity": 0.0000011429718454732932,
+                    "velocityX": 3.593033654211569,
+                    "velocityY": 1.183530129107538,
+                    "timestamp": 1.2417266513982352
+                },
+                {
+                    "x": 5.421399644212107,
+                    "y": 6.724599944524706,
+                    "heading": 2.5801561231027166,
+                    "angularVelocity": 0.0000011429720157589239,
+                    "velocityX": 3.593033706613086,
+                    "velocityY": 1.183529970023511,
+                    "timestamp": 1.281854767446364
+                },
+                {
+                    "x": 5.565581319724973,
+                    "y": 6.772092766530448,
+                    "heading": 2.580156168968038,
+                    "angularVelocity": 0.0000011429722040135848,
+                    "velocityX": 3.593033755682377,
+                    "velocityY": 1.1835298210556366,
+                    "timestamp": 1.3219828834944929
+                },
+                {
+                    "x": 5.709762997085268,
+                    "y": 6.819585582927637,
+                    "heading": 2.5801562148333677,
+                    "angularVelocity": 0.0000011429724102352075,
+                    "velocityX": 3.5930338017206545,
+                    "velocityY": 1.1835296812894733,
+                    "timestamp": 1.3621109995426217
+                },
+                {
+                    "x": 5.853944676177738,
+                    "y": 6.867078394066171,
+                    "heading": 2.580156260698706,
+                    "angularVelocity": 0.0000011429726261958928,
+                    "velocityX": 3.5930338448867536,
+                    "velocityY": 1.183529550242829,
+                    "timestamp": 1.4022391155907505
+                },
+                {
+                    "x": 5.998126356881069,
+                    "y": 6.91457120031434,
+                    "heading": 2.5801563065640525,
+                    "angularVelocity": 0.0000011429728355759127,
+                    "velocityX": 3.593033885029711,
+                    "velocityY": 1.183529428374024,
+                    "timestamp": 1.4423672316388794
+                },
+                {
+                    "x": 6.142308039084736,
+                    "y": 6.962064002007683,
+                    "heading": 2.580156352429407,
+                    "angularVelocity": 0.0000011429730341793388,
+                    "velocityX": 3.5930339224183525,
+                    "velocityY": 1.1835293148669426,
+                    "timestamp": 1.4824953476870082
+                },
+                {
+                    "x": 6.286489722687973,
+                    "y": 7.009556799452112,
+                    "heading": 2.5801563982947693,
+                    "angularVelocity": 0.0000011429732271830896,
+                    "velocityX": 3.5930339572958836,
+                    "velocityY": 1.1835292089832443,
+                    "timestamp": 1.522623463735137
+                },
+                {
+                    "x": 6.430671407598782,
+                    "y": 7.057049592926921,
+                    "heading": 2.580156444160139,
+                    "angularVelocity": 0.000001142973413162616,
+                    "velocityX": 3.5930339898808183,
+                    "velocityY": 1.183529110059557,
+                    "timestamp": 1.5627515797832658
+                },
+                {
+                    "x": 6.574853093733082,
+                    "y": 7.104542382687364,
+                    "heading": 2.580156490025516,
+                    "angularVelocity": 0.0000011429735908015805,
+                    "velocityX": 3.593034020370455,
+                    "velocityY": 1.1835290174969095,
+                    "timestamp": 1.6028796958313947
+                },
+                {
+                    "x": 6.719034781004445,
+                    "y": 7.15203516899583,
+                    "heading": 2.5801565358908998,
+                    "angularVelocity": 0.0000011429737623342271,
+                    "velocityX": 3.593034048706264,
+                    "velocityY": 1.1835289314729884,
+                    "timestamp": 1.6430078118795235
+                },
+                {
+                    "x": 6.863216267597302,
+                    "y": 7.199528564522877,
+                    "heading": 2.580156581764359,
+                    "angularVelocity": 0.0000011431750045992354,
+                    "velocityX": 3.593029047761165,
+                    "velocityY": 1.1835441133115774,
+                    "timestamp": 1.6831359279276523
+                },
+                {
+                    "x": 6.997116821056572,
+                    "y": 7.244893026525659,
+                    "heading": 2.605743326369695,
+                    "angularVelocity": 0.6376263608948962,
+                    "velocityX": 3.3368263114738097,
+                    "velocityY": 1.1304907000461517,
+                    "timestamp": 1.7232640439757811
+                },
+                {
+                    "x": 7.1211832753718545,
+                    "y": 7.2851403173258245,
+                    "heading": 2.65804915297094,
+                    "angularVelocity": 1.3034707769113922,
+                    "velocityX": 3.091758760029488,
+                    "velocityY": 1.0029698566435126,
+                    "timestamp": 1.76339216002391
+                },
+                {
+                    "x": 7.233669414925018,
+                    "y": 7.321896904745842,
+                    "heading": 2.730695495833792,
+                    "angularVelocity": 1.8103601668147513,
+                    "velocityX": 2.8031751956221984,
+                    "velocityY": 0.9159808892082741,
+                    "timestamp": 1.8035202760720388
+                },
+                {
+                    "x": 7.333463334152891,
+                    "y": 7.354467297173912,
+                    "heading": 2.807930546248393,
+                    "angularVelocity": 1.9247115992678774,
+                    "velocityX": 2.486882740973504,
+                    "velocityY": 0.811660143451645,
+                    "timestamp": 1.8436483921201676
+                },
+                {
+                    "x": 7.42052379329864,
+                    "y": 7.382839226266775,
+                    "heading": 2.882629127741458,
+                    "angularVelocity": 1.8615023292763984,
+                    "velocityX": 2.169562584032867,
+                    "velocityY": 0.7070336683345496,
+                    "timestamp": 1.8837765081682964
+                },
+                {
+                    "x": 7.494937832171454,
+                    "y": 7.407046736601957,
+                    "heading": 2.951143873381968,
+                    "angularVelocity": 1.7074000074744335,
+                    "velocityX": 1.8544114750755487,
+                    "velocityY": 0.6032555903234524,
+                    "timestamp": 1.9239046242164253
+                },
+                {
+                    "x": 7.556793519582923,
+                    "y": 7.427131383067914,
+                    "heading": 3.011211531798935,
+                    "angularVelocity": 1.4968970470710155,
+                    "velocityX": 1.5414550570298573,
+                    "velocityY": 0.5005130677420416,
+                    "timestamp": 1.964032740264554
+                },
+                {
+                    "x": 7.606167226555021,
+                    "y": 7.443134327538265,
+                    "heading": 3.0612259719014916,
+                    "angularVelocity": 1.2463690057756673,
+                    "velocityX": 1.2304018188364594,
+                    "velocityY": 0.3987963065885285,
+                    "timestamp": 2.0041608563126827
+                },
+                {
+                    "x": 7.643124291158215,
+                    "y": 7.455092771789357,
+                    "heading": 3.0999636114018587,
+                    "angularVelocity": 0.9653490698119509,
+                    "velocityX": 0.9209768173235189,
+                    "velocityY": 0.2980066205139035,
+                    "timestamp": 2.0442889723608113
+                },
+                {
+                    "x": 7.667720023098554,
+                    "y": 7.46303884043931,
+                    "heading": 3.1264689002845456,
+                    "angularVelocity": 0.6605166524861815,
+                    "velocityX": 0.6129301438133489,
+                    "velocityY": 0.1980174858052883,
+                    "timestamp": 2.08441708840894
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "angularVelocity": 0.3371974826634153,
+                    "velocityX": 0.30601927303833204,
+                    "velocityY": 0.0987128216021636,
+                    "timestamp": 2.1245452044570685
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 3.14,
+                    "angularVelocity": -8.758785880018889e-35,
+                    "velocityX": 1.2314161274227302e-35,
+                    "velocityY": -4.94083755736297e-36,
+                    "timestamp": 2.164673320505197
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -7610,16 +19621,6 @@
                     "x": 3.42,
                     "y": 4.04,
                     "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
                 }
             ]
         },
@@ -7632,7 +19633,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 29
+                },
+                {
+                    "x": 5.493278503417969,
+                    "y": 6.720412731170654,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 27
                 },
                 {
                     "x": 7.68,
@@ -7644,7 +19654,521 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 3.853182471863691e-34,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.30610760298278,
+                    "y": 4.37532703674674,
+                    "heading": 2.7433864075571766,
+                    "angularVelocity": -0.14821621401149343,
+                    "velocityX": 0.13687656120621225,
+                    "velocityY": 0.3434918886001163,
+                    "timestamp": 0.04462124799861899
+                },
+                {
+                    "x": 2.318848696543319,
+                    "y": 4.405778929719923,
+                    "heading": 2.7307174870785778,
+                    "angularVelocity": -0.28392124933375007,
+                    "velocityX": 0.28553870929233327,
+                    "velocityY": 0.6824527403206025,
+                    "timestamp": 0.08924249599723798
+                },
+                {
+                    "x": 2.3388726580644836,
+                    "y": 4.451074836672454,
+                    "heading": 2.7126983162955036,
+                    "angularVelocity": -0.4038248948938342,
+                    "velocityX": 0.44875395510641974,
+                    "velocityY": 1.0151196791701815,
+                    "timestamp": 0.13386374399585696
+                },
+                {
+                    "x": 2.3669952727104597,
+                    "y": 4.510809884172449,
+                    "heading": 2.6902411170995073,
+                    "angularVelocity": -0.5032848744322687,
+                    "velocityX": 0.6302516381174824,
+                    "velocityY": 1.3387130611372482,
+                    "timestamp": 0.17848499199447596
+                },
+                {
+                    "x": 2.404258116570724,
+                    "y": 4.5843745316547775,
+                    "heading": 2.664559931990299,
+                    "angularVelocity": -0.5755371322201343,
+                    "velocityX": 0.8350919243992831,
+                    "velocityY": 1.648646122237679,
+                    "timestamp": 0.22310623999309495
+                },
+                {
+                    "x": 2.452003610186855,
+                    "y": 4.670805411588522,
+                    "heading": 2.637322930497274,
+                    "angularVelocity": -0.6104042964882519,
+                    "velocityX": 1.070016993195909,
+                    "velocityY": 1.9369893001742093,
+                    "timestamp": 0.2677274879917139
+                },
+                {
+                    "x": 2.5115645242184845,
+                    "y": 4.768897077167795,
+                    "heading": 2.6107796946069493,
+                    "angularVelocity": -0.5948564211190585,
+                    "velocityX": 1.3348105824712122,
+                    "velocityY": 2.1983173931466227,
+                    "timestamp": 0.3123487359903329
+                },
+                {
+                    "x": 2.5833793756558516,
+                    "y": 4.878178047966119,
+                    "heading": 2.586043040896354,
+                    "angularVelocity": -0.5543693827514722,
+                    "velocityX": 1.60943170929665,
+                    "velocityY": 2.449079210014641,
+                    "timestamp": 0.3569699839889519
+                },
+                {
+                    "x": 2.6682042395216055,
+                    "y": 4.997714430694696,
+                    "heading": 2.5651891379462763,
+                    "angularVelocity": -0.46735364619839126,
+                    "velocityX": 1.9009971184216836,
+                    "velocityY": 2.678911686474495,
+                    "timestamp": 0.4015912319875709
+                },
+                {
+                    "x": 2.76752425780781,
+                    "y": 5.124957228979585,
+                    "heading": 2.5531720535122697,
+                    "angularVelocity": -0.269313050911911,
+                    "velocityX": 2.2258458187740118,
+                    "velocityY": 2.8516189930148728,
+                    "timestamp": 0.4462124799861899
+                },
+                {
+                    "x": 2.882656696791853,
+                    "y": 5.248398665617137,
+                    "heading": 2.553172127817037,
+                    "angularVelocity": 0.0000016652328226055909,
+                    "velocityX": 2.580215573253489,
+                    "velocityY": 2.76642725549436,
+                    "timestamp": 0.4908337279848089
+                },
+                {
+                    "x": 3.0093581189008844,
+                    "y": 5.359933509101588,
+                    "heading": 2.5531721828704046,
+                    "angularVelocity": 0.000001233792641064896,
+                    "velocityX": 2.8394862939053795,
+                    "velocityY": 2.4995904078680997,
+                    "timestamp": 0.5354549759834278
+                },
+                {
+                    "x": 3.146402398989035,
+                    "y": 5.458483685796272,
+                    "heading": 2.553172234700022,
+                    "angularVelocity": 0.0000011615456718217746,
+                    "velocityX": 3.071278510461917,
+                    "velocityY": 2.20859301599395,
+                    "timestamp": 0.5800762239820468
+                },
+                {
+                    "x": 3.286524028997536,
+                    "y": 5.552607013873938,
+                    "heading": 2.5531722858926313,
+                    "angularVelocity": 0.0000011472697806901915,
+                    "velocityX": 3.140244531323671,
+                    "velocityY": 2.1093835851605776,
+                    "timestamp": 0.6246974719806658
+                },
+                {
+                    "x": 3.426645803515369,
+                    "y": 5.646730126819845,
+                    "heading": 2.5531723370852246,
+                    "angularVelocity": 0.000001147269414679425,
+                    "velocityX": 3.140247769900325,
+                    "velocityY": 2.109378763875461,
+                    "timestamp": 0.6693187199792848
+                },
+                {
+                    "x": 3.5667675932030476,
+                    "y": 5.740853217182279,
+                    "heading": 2.5531723882778183,
+                    "angularVelocity": 0.0000011472694285852317,
+                    "velocityX": 3.1402481098694217,
+                    "velocityY": 2.109378257760682,
+                    "timestamp": 0.7139399679779038
+                },
+                {
+                    "x": 3.70688939501189,
+                    "y": 5.834976289499829,
+                    "heading": 2.553172439470412,
+                    "angularVelocity": 0.0000011472694368681445,
+                    "velocityX": 3.1402483815149935,
+                    "velocityY": 2.109377853359542,
+                    "timestamp": 0.7585612159765228
+                },
+                {
+                    "x": 3.8470112068261555,
+                    "y": 5.929099346922214,
+                    "heading": 2.553172490663007,
+                    "angularVelocity": 0.0000011472694500175128,
+                    "velocityX": 3.140248605744995,
+                    "velocityY": 2.1093775195462685,
+                    "timestamp": 0.8031824639751418
+                },
+                {
+                    "x": 3.9871330270690666,
+                    "y": 6.023222391796794,
+                    "heading": 2.5531725418556026,
+                    "angularVelocity": 0.000001147269470343035,
+                    "velocityX": 3.1402487946380906,
+                    "velocityY": 2.109377238339292,
+                    "timestamp": 0.8478037119737608
+                },
+                {
+                    "x": 4.127254854511192,
+                    "y": 6.117345425953829,
+                    "heading": 2.553172593048199,
+                    "angularVelocity": 0.000001147269495912955,
+                    "velocityX": 3.1402489559785947,
+                    "velocityY": 2.109376998150064,
+                    "timestamp": 0.8924249599723798
+                },
+                {
+                    "x": 4.267376688165873,
+                    "y": 6.21146845086217,
+                    "heading": 2.5531726442407967,
+                    "angularVelocity": 0.0000011472695239009516,
+                    "velocityX": 3.1402490952072233,
+                    "velocityY": 2.1093767908789607,
+                    "timestamp": 0.9370462079709988
+                },
+                {
+                    "x": 4.407498527228544,
+                    "y": 6.305591467719579,
+                    "heading": 2.553172695433396,
+                    "angularVelocity": 0.0000011472695542073644,
+                    "velocityX": 3.140249216404848,
+                    "velocityY": 2.1093766104507625,
+                    "timestamp": 0.9816674559696178
+                },
+                {
+                    "x": 4.547620436764314,
+                    "y": 6.399714379662917,
+                    "heading": 2.5531727466259886,
+                    "angularVelocity": 0.0000011472694062933976,
+                    "velocityX": 3.1402507957667765,
+                    "velocityY": 2.1093742592374776,
+                    "timestamp": 1.0262887039682367
+                },
+                {
+                    "x": 4.689466118908105,
+                    "y": 6.4912188851781,
+                    "heading": 2.553172797709037,
+                    "angularVelocity": 0.0000011448144250292167,
+                    "velocityX": 3.1788820014218735,
+                    "velocityY": 2.050693551153348,
+                    "timestamp": 1.0709099519668557
+                },
+                {
+                    "x": 4.839613537732198,
+                    "y": 6.568348804669705,
+                    "heading": 2.5531728491363963,
+                    "angularVelocity": 0.0000011525307241082326,
+                    "velocityX": 3.364930959096914,
+                    "velocityY": 1.7285468907994308,
+                    "timestamp": 1.1155311999654747
+                },
+                {
+                    "x": 4.996611329828,
+                    "y": 6.630356582585623,
+                    "heading": 2.5531729033694757,
+                    "angularVelocity": 0.0000012154092882901391,
+                    "velocityX": 3.5184536322395523,
+                    "velocityY": 1.3896468767040242,
+                    "timestamp": 1.1601524479640937
+                },
+                {
+                    "x": 5.158940949366324,
+                    "y": 6.676642268737186,
+                    "heading": 2.553172963573569,
+                    "angularVelocity": 0.0000013492247785707482,
+                    "velocityX": 3.637944405843334,
+                    "velocityY": 1.037301470209733,
+                    "timestamp": 1.2047736959627127
+                },
+                {
+                    "x": 5.325032205172685,
+                    "y": 6.706758095236814,
+                    "heading": 2.5531730345823234,
+                    "angularVelocity": 0.0000015913663950616823,
+                    "velocityX": 3.7222458639324056,
+                    "velocityY": 0.6749212057125495,
+                    "timestamp": 1.2493949439613317
+                },
+                {
+                    "x": 5.493278503417969,
+                    "y": 6.720412731170654,
+                    "heading": 2.553173120074835,
+                    "angularVelocity": 0.000001915959677785522,
+                    "velocityX": 3.7705421921522753,
+                    "velocityY": 0.30601196842950656,
+                    "timestamp": 1.2940161919599507
+                },
+                {
+                    "x": 5.617518941656642,
+                    "y": 6.721477956029719,
+                    "heading": 2.5531732061618477,
+                    "angularVelocity": 0.0000026211268995160273,
+                    "velocityX": 3.782800031375301,
+                    "velocityY": 0.03243334205370069,
+                    "timestamp": 1.32685970396381
+                },
+                {
+                    "x": 5.741510905915031,
+                    "y": 6.713552321996202,
+                    "heading": 2.5531732789431283,
+                    "angularVelocity": 0.0000022160017651615355,
+                    "velocityX": 3.7752346412837734,
+                    "velocityY": -0.24131505889453328,
+                    "timestamp": 1.3597032159676692
+                },
+                {
+                    "x": 5.8646046105180325,
+                    "y": 6.696677372566761,
+                    "heading": 2.553173343002795,
+                    "angularVelocity": 0.0000019504511878675683,
+                    "velocityX": 3.7478849578735156,
+                    "velocityY": -0.5137985678101988,
+                    "timestamp": 1.3925467279715285
+                },
+                {
+                    "x": 5.986154981292517,
+                    "y": 6.670941556957854,
+                    "heading": 2.5531734012532086,
+                    "angularVelocity": 0.0000017735744392757528,
+                    "velocityX": 3.7008944341945598,
+                    "velocityY": -0.7835890268336883,
+                    "timestamp": 1.4253902399753877
+                },
+                {
+                    "x": 6.105525040034572,
+                    "y": 6.636479774106562,
+                    "heading": 2.553173455691931,
+                    "angularVelocity": 0.000001657518306287931,
+                    "velocityX": 3.634509571571689,
+                    "velocityY": -1.0492721621013206,
+                    "timestamp": 1.458233751979247
+                },
+                {
+                    "x": 6.222089256538776,
+                    "y": 6.593472690949968,
+                    "heading": 2.5531735077894595,
+                    "angularVelocity": 0.0000015862350106976908,
+                    "velocityX": 3.549078931951851,
+                    "velocityY": -1.3094544563790795,
+                    "timestamp": 1.4910772639831062
+                },
+                {
+                    "x": 6.335236901536747,
+                    "y": 6.54214592976541,
+                    "heading": 2.553173558707783,
+                    "angularVelocity": 0.000001550331263346088,
+                    "velocityX": 3.4450531655894774,
+                    "velocityY": -1.5627671358204458,
+                    "timestamp": 1.5239207759869655
+                },
+                {
+                    "x": 6.444377333823394,
+                    "y": 6.482772601626653,
+                    "heading": 2.5531736094378257,
+                    "angularVelocity": 0.0000015445985995778538,
+                    "velocityX": 3.3230439020592755,
+                    "velocityY": -1.8077642893915407,
+                    "timestamp": 1.5567642879908248
+                },
+                {
+                    "x": 6.553087073787246,
+                    "y": 6.422614322949958,
+                    "heading": 2.5531736601729174,
+                    "angularVelocity": 0.0000015447523320156895,
+                    "velocityX": 3.3099304346952643,
+                    "velocityY": -1.8316640032170186,
+                    "timestamp": 1.589607799994684
+                },
+                {
+                    "x": 6.661796772216766,
+                    "y": 6.362455969218214,
+                    "heading": 2.553173710908019,
+                    "angularVelocity": 0.0000015447526253443646,
+                    "velocityX": 3.3099291700822753,
+                    "velocityY": -1.8316662884491148,
+                    "timestamp": 1.6224513119985433
+                },
+                {
+                    "x": 6.770506467400396,
+                    "y": 6.302297609620959,
+                    "heading": 2.5531737616431265,
+                    "angularVelocity": 0.0000015447528238732798,
+                    "velocityX": 3.3099290712533134,
+                    "velocityY": -1.831666467038753,
+                    "timestamp": 1.6552948240024026
+                },
+                {
+                    "x": 6.879216090079623,
+                    "y": 6.242139119005013,
+                    "heading": 2.5531738123794403,
+                    "angularVelocity": 0.0000015447895469442916,
+                    "velocityX": 3.309926863681755,
+                    "velocityY": -1.831670456219097,
+                    "timestamp": 1.6881383360062618
+                },
+                {
+                    "x": 6.983740227457298,
+                    "y": 6.1833650705733145,
+                    "heading": 2.56712091506752,
+                    "angularVelocity": 0.4246532065888785,
+                    "velocityX": 3.18248966083146,
+                    "velocityY": -1.7895177721795115,
+                    "timestamp": 1.720981848010121
+                },
+                {
+                    "x": 7.082197224690116,
+                    "y": 6.128185260935312,
+                    "heading": 2.6040317014746788,
+                    "angularVelocity": 1.1238379867177672,
+                    "velocityX": 2.9977609343741736,
+                    "velocityY": -1.6800824964004346,
+                    "timestamp": 1.7538253600139804
+                },
+                {
+                    "x": 7.174469097455313,
+                    "y": 6.076712532186898,
+                    "heading": 2.662828331449144,
+                    "angularVelocity": 1.7902053217559901,
+                    "velocityX": 2.809439890422065,
+                    "velocityY": -1.567211470635817,
+                    "timestamp": 1.7866688720178396
+                },
+                {
+                    "x": 7.2591302790362535,
+                    "y": 6.029614181571086,
+                    "heading": 2.728764792304138,
+                    "angularVelocity": 2.0075947069012146,
+                    "velocityX": 2.577714026776202,
+                    "velocityY": -1.4340229695982882,
+                    "timestamp": 1.819512384021699
+                },
+                {
+                    "x": 7.335970518862824,
+                    "y": 5.986956845665554,
+                    "heading": 2.7950149925408634,
+                    "angularVelocity": 2.0171472596761757,
+                    "velocityX": 2.339586577024463,
+                    "velocityY": -1.2988055571072747,
+                    "timestamp": 1.8523558960255582
+                },
+                {
+                    "x": 7.405004233411928,
+                    "y": 5.94870169343351,
+                    "heading": 2.858549177078574,
+                    "angularVelocity": 1.9344516058527972,
+                    "velocityX": 2.1018980717102513,
+                    "velocityY": -1.1647704492609365,
+                    "timestamp": 1.8851994080294174
+                },
+                {
+                    "x": 7.466271161714403,
+                    "y": 5.914805217934882,
+                    "heading": 2.9175764455342432,
+                    "angularVelocity": 1.7972276670270115,
+                    "velocityX": 1.8654195171112133,
+                    "velocityY": -1.0320600152214254,
+                    "timestamp": 1.9180429200332767
+                },
+                {
+                    "x": 7.519808154158724,
+                    "y": 5.885228450835136,
+                    "heading": 2.9709348371498114,
+                    "angularVelocity": 1.6246250282034163,
+                    "velocityX": 1.6300629615380646,
+                    "velocityY": -0.9005360661877405,
+                    "timestamp": 1.950886432037136
+                },
+                {
+                    "x": 7.565644888760801,
+                    "y": 5.8599385495006775,
+                    "heading": 3.0178335837303445,
+                    "angularVelocity": 1.427945542943834,
+                    "velocityX": 1.3956100247954721,
+                    "velocityY": -0.770012090408821,
+                    "timestamp": 1.9837299440409952
+                },
+                {
+                    "x": 7.603804643651593,
+                    "y": 5.838908591345399,
+                    "heading": 3.057713507827927,
+                    "angularVelocity": 1.214240550550639,
+                    "velocityX": 1.1618658469383054,
+                    "velocityY": -0.6403078377491017,
+                    "timestamp": 2.0165734560448545
+                },
+                {
+                    "x": 7.6343059239967275,
+                    "y": 5.822116657822096,
+                    "heading": 3.090162525369235,
+                    "angularVelocity": 0.987988663864437,
+                    "velocityX": 0.9286851035160462,
+                    "velocityY": -0.5112709481656601,
+                    "timestamp": 2.0494169680487135
+                },
+                {
+                    "x": 7.657163897668815,
+                    "y": 5.809544651656199,
+                    "heading": 3.1148617613725005,
+                    "angularVelocity": 0.7520278586639367,
+                    "velocityX": 0.6959661825873334,
+                    "velocityY": -0.3827850737892687,
+                    "timestamp": 2.0822604800525726
+                },
+                {
+                    "x": 7.67239146004521,
+                    "y": 5.801177089276775,
+                    "heading": 3.1315500981004414,
+                    "angularVelocity": 0.5081166936708882,
+                    "velocityX": 0.4636398925488508,
+                    "velocityY": -0.2547706341039437,
+                    "timestamp": 2.1151039920564316
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": 0.257277659544025,
+                    "velocityX": 0.2316603642721254,
+                    "velocityY": -0.1271815656097946,
+                    "timestamp": 2.1479475040602907
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 3.14,
+                    "angularVelocity": -3.3673783800968066e-32,
+                    "velocityX": -2.4380081043450124e-32,
+                    "velocityY": -4.2407952262773785e-32,
+                    "timestamp": 2.1807910160641497
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -7667,16 +20191,6 @@
                     "x": 3.42,
                     "y": 4.04,
                     "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
                 }
             ]
         },
@@ -7689,7 +20203,25 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 11
+                },
+                {
+                    "x": 3.558790922164917,
+                    "y": 5.477728366851807,
+                    "heading": 3.141592653589793,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 42
+                },
+                {
+                    "x": 5.390789031982422,
+                    "y": 4.081310272216797,
+                    "heading": -3.131789038956166,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 42
                 },
                 {
                     "x": 7.68,
@@ -7701,7 +20233,872 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 0,
+                    "velocityX": -5.720774693888146e-34,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.3121576423431627,
+                    "y": 4.3971322791414,
+                    "heading": 2.7106310928070285,
+                    "angularVelocity": -0.5647989987388474,
+                    "velocityX": 0.1744174454420521,
+                    "velocityY": 0.5327116136893348,
+                    "timestamp": 0.06970427936465769
+                },
+                {
+                    "x": 2.3388751368329244,
+                    "y": 4.470343590887415,
+                    "heading": 2.629622228062893,
+                    "angularVelocity": -1.1621792160039144,
+                    "velocityX": 0.38329776497637447,
+                    "velocityY": 1.0503130139687884,
+                    "timestamp": 0.13940855872931537
+                },
+                {
+                    "x": 2.3840539747737246,
+                    "y": 4.577503337755321,
+                    "heading": 2.5035786127839184,
+                    "angularVelocity": -1.8082622247563562,
+                    "velocityX": 0.6481501329989676,
+                    "velocityY": 1.5373481778256302,
+                    "timestamp": 0.20911283809397307
+                },
+                {
+                    "x": 2.4545399331182174,
+                    "y": 4.713290189810374,
+                    "heading": 2.3271901249556994,
+                    "angularVelocity": -2.5305259510028497,
+                    "velocityX": 1.011214217935542,
+                    "velocityY": 1.948041831760208,
+                    "timestamp": 0.27881711745863075
+                },
+                {
+                    "x": 2.555797664071962,
+                    "y": 4.87530128954077,
+                    "heading": 2.141593101929859,
+                    "angularVelocity": -2.662634557268572,
+                    "velocityX": 1.4526759601661592,
+                    "velocityY": 2.3242633193700435,
+                    "timestamp": 0.3485213968232884
+                },
+                {
+                    "x": 2.6889975632138237,
+                    "y": 5.05237912689222,
+                    "heading": 2.0338949096526036,
+                    "angularVelocity": -1.545072888765306,
+                    "velocityX": 1.910928573624968,
+                    "velocityY": 2.54041558087227,
+                    "timestamp": 0.4182256761879461
+                },
+                {
+                    "x": 2.8587006072828576,
+                    "y": 5.214124992637136,
+                    "heading": 1.956646376370765,
+                    "angularVelocity": -1.108232291990471,
+                    "velocityX": 2.4346144256256212,
+                    "velocityY": 2.3204581873480494,
+                    "timestamp": 0.48792995555260377
+                },
+                {
+                    "x": 3.035891081202655,
+                    "y": 5.338211520127435,
+                    "heading": 1.8312106553523118,
+                    "angularVelocity": -1.7995411782716633,
+                    "velocityX": 2.5420315012917163,
+                    "velocityY": 1.7801852141837777,
+                    "timestamp": 0.5576342349172615
+                },
+                {
+                    "x": 3.2127166120567576,
+                    "y": 5.42406293198425,
+                    "heading": 1.6557539162164756,
+                    "angularVelocity": -2.5171587847273322,
+                    "velocityX": 2.536795910750906,
+                    "velocityY": 1.231651953643812,
+                    "timestamp": 0.6273385142819192
+                },
+                {
+                    "x": 3.3872389413542425,
+                    "y": 5.4708692312967075,
+                    "heading": 1.441472502737752,
+                    "angularVelocity": -3.0741500440412146,
+                    "velocityX": 2.503753440796252,
+                    "velocityY": 0.6714982170261123,
+                    "timestamp": 0.697042793646577
+                },
+                {
+                    "x": 3.558790922164917,
+                    "y": 5.477728366851807,
+                    "heading": 1.205442867585498,
+                    "angularVelocity": -3.386157023695285,
+                    "velocityX": 2.461139866509503,
+                    "velocityY": 0.09840336371911824,
+                    "timestamp": 0.7667470730112347
+                },
+                {
+                    "x": 3.6181705627536003,
+                    "y": 5.4752230037777725,
+                    "heading": 1.1222490393160696,
+                    "angularVelocity": -3.4253809174942864,
+                    "velocityX": 2.44486750990022,
+                    "velocityY": -0.10315456138644599,
+                    "timestamp": 0.7910345406749146
+                },
+                {
+                    "x": 3.6770128805090945,
+                    "y": 5.467829320967843,
+                    "heading": 1.0387507075885118,
+                    "angularVelocity": -3.4379183900025705,
+                    "velocityX": 2.4227440493307753,
+                    "velocityY": -0.3044237839989221,
+                    "timestamp": 0.8153220083385946
+                },
+                {
+                    "x": 3.735181283810321,
+                    "y": 5.455564580406075,
+                    "heading": 0.9556144237478981,
+                    "angularVelocity": -3.4230116120725995,
+                    "velocityX": 2.394996633931211,
+                    "velocityY": -0.5049822703463023,
+                    "timestamp": 0.8396094760022745
+                },
+                {
+                    "x": 3.7925490950380945,
+                    "y": 5.438455755513165,
+                    "heading": 0.8735217241735449,
+                    "angularVelocity": -3.380043597427691,
+                    "velocityX": 2.362033457838118,
+                    "velocityY": -0.7044301666120085,
+                    "timestamp": 0.8638969436659545
+                },
+                {
+                    "x": 3.8490034281899477,
+                    "y": 5.416538531692629,
+                    "heading": 0.7931634356424634,
+                    "angularVelocity": -3.308631827897467,
+                    "velocityX": 2.3244223701541378,
+                    "velocityY": -0.902408769989304,
+                    "timestamp": 0.8881844113296344
+                },
+                {
+                    "x": 3.9044487728906376,
+                    "y": 5.389855903636327,
+                    "heading": 0.7152329727062241,
+                    "angularVelocity": -3.2086697557513846,
+                    "velocityX": 2.282878786231156,
+                    "velocityY": -1.0986171315094786,
+                    "timestamp": 0.9124718789933144
+                },
+                {
+                    "x": 3.958810912211869,
+                    "y": 5.358456406059338,
+                    "heading": 0.6404209450438546,
+                    "angularVelocity": -3.080272867403366,
+                    "velocityX": 2.2382794317633223,
+                    "velocityY": -1.2928271490380632,
+                    "timestamp": 0.9367593466569943
+                },
+                {
+                    "x": 4.012042256408639,
+                    "y": 5.322391998548557,
+                    "heading": 0.5694141014307214,
+                    "angularVelocity": -2.923600129762354,
+                    "velocityX": 2.1917206410273016,
+                    "velocityY": -1.4848978086221774,
+                    "timestamp": 0.9610468143206743
+                },
+                {
+                    "x": 4.064130365340956,
+                    "y": 5.281715631109981,
+                    "heading": 0.5029022798709881,
+                    "angularVelocity": -2.738524348472814,
+                    "velocityX": 2.1446496462128213,
+                    "velocityY": -1.6747883312431175,
+                    "timestamp": 0.9853342819843542
+                },
+                {
+                    "x": 4.1151123432623455,
+                    "y": 5.236478617606072,
+                    "heading": 0.44159733021441666,
+                    "angularVelocity": -2.524139218855187,
+                    "velocityX": 2.09910636330477,
+                    "velocityY": -1.8625660826533157,
+                    "timestamp": 1.0096217496480342
+                },
+                {
+                    "x": 4.165095928896697,
+                    "y": 5.186728422848365,
+                    "heading": 0.38626165615570274,
+                    "angularVelocity": -2.278363262278863,
+                    "velocityX": 2.057999060523636,
+                    "velocityY": -2.0483895417431537,
+                    "timestamp": 1.0339092173117141
+                },
+                {
+                    "x": 4.214269146284126,
+                    "y": 5.132508462649398,
+                    "heading": 0.33770705635311316,
+                    "angularVelocity": -1.9991627153126146,
+                    "velocityX": 2.0246333651722517,
+                    "velocityY": -2.232425419964549,
+                    "timestamp": 1.058196684975394
+                },
+                {
+                    "x": 4.262857394718322,
+                    "y": 5.073859999949952,
+                    "heading": 0.2966997330272638,
+                    "angularVelocity": -1.6884149427884858,
+                    "velocityX": 2.000548147177036,
+                    "velocityY": -2.4147623585784754,
+                    "timestamp": 1.082484152639074
+                },
+                {
+                    "x": 4.311032470035773,
+                    "y": 5.010821065356263,
+                    "heading": 0.26381895203232264,
+                    "angularVelocity": -1.353816768806731,
+                    "velocityX": 1.9835363647027058,
+                    "velocityY": -2.59553344410457,
+                    "timestamp": 1.106771620302754
+                },
+                {
+                    "x": 4.358621953046075,
+                    "y": 4.943414538605821,
+                    "heading": 0.23906687218938943,
+                    "angularVelocity": -1.019129708608845,
+                    "velocityX": 1.9594254810463076,
+                    "velocityY": -2.77536249080602,
+                    "timestamp": 1.131059087966434
+                },
+                {
+                    "x": 4.401785346523604,
+                    "y": 4.873960680394997,
+                    "heading": 0.21204412295947933,
+                    "angularVelocity": -1.1126211099529368,
+                    "velocityX": 1.7771878927533025,
+                    "velocityY": -2.8596582884878865,
+                    "timestamp": 1.155346555630114
+                },
+                {
+                    "x": 4.442527933324705,
+                    "y": 4.807784869243466,
+                    "heading": 0.1747999499890042,
+                    "angularVelocity": -1.5334728793554109,
+                    "velocityX": 1.6775148140301646,
+                    "velocityY": -2.7246896246203156,
+                    "timestamp": 1.1796340232937939
+                },
+                {
+                    "x": 4.4813722294916225,
+                    "y": 4.745319922619003,
+                    "heading": 0.12759770745686402,
+                    "angularVelocity": -1.9434814360134838,
+                    "velocityX": 1.5993555484998627,
+                    "velocityY": -2.571900351631775,
+                    "timestamp": 1.2039214909574738
+                },
+                {
+                    "x": 4.518612325734112,
+                    "y": 4.686765196550011,
+                    "heading": 0.07078885768669944,
+                    "angularVelocity": -2.3390190594105262,
+                    "velocityX": 1.5333050262040608,
+                    "velocityY": -2.410902893616876,
+                    "timestamp": 1.2282089586211538
+                },
+                {
+                    "x": 4.554475342160425,
+                    "y": 4.632242142038935,
+                    "heading": 0.004632855038261056,
+                    "angularVelocity": -2.723874039258911,
+                    "velocityX": 1.47660583321928,
+                    "velocityY": -2.244904873002102,
+                    "timestamp": 1.2524964262848337
+                },
+                {
+                    "x": 4.589156761961767,
+                    "y": 4.581819399142938,
+                    "heading": -0.07076666263842435,
+                    "angularVelocity": -3.1044618863019378,
+                    "velocityX": 1.4279553670061842,
+                    "velocityY": -2.076080701134665,
+                    "timestamp": 1.2767838939485137
+                },
+                {
+                    "x": 4.6228372561227475,
+                    "y": 4.535518314545829,
+                    "heading": -0.1555015238515939,
+                    "angularVelocity": -3.488830634240387,
+                    "velocityX": 1.3867437572072365,
+                    "velocityY": -1.9063776116251314,
+                    "timestamp": 1.3010713616121936
+                },
+                {
+                    "x": 4.654626762222228,
+                    "y": 4.492711334026638,
+                    "heading": -0.25016165034544124,
+                    "angularVelocity": -3.8974885239025583,
+                    "velocityX": 1.3088851641383707,
+                    "velocityY": -1.762513124544663,
+                    "timestamp": 1.3253588292758736
+                },
+                {
+                    "x": 4.685559266058307,
+                    "y": 4.453815281857176,
+                    "heading": -0.35503604485130436,
+                    "angularVelocity": -4.318045666930297,
+                    "velocityX": 1.2735993832052854,
+                    "velocityY": -1.6014865241643912,
+                    "timestamp": 1.3496462969395535
+                },
+                {
+                    "x": 4.716254412162635,
+                    "y": 4.418834840133045,
+                    "heading": -0.4707020428721612,
+                    "angularVelocity": -4.762373732104905,
+                    "velocityX": 1.2638265351241147,
+                    "velocityY": -1.440267145530449,
+                    "timestamp": 1.3739337646032335
+                },
+                {
+                    "x": 4.747074366156251,
+                    "y": 4.387541660715592,
+                    "heading": -0.5981615568154537,
+                    "angularVelocity": -5.247954035730924,
+                    "velocityX": 1.268965312497503,
+                    "velocityY": -1.2884496585146104,
+                    "timestamp": 1.3982212322669134
+                },
+                {
+                    "x": 4.7760218641280225,
+                    "y": 4.359163482704458,
+                    "heading": -0.7400182505648043,
+                    "angularVelocity": -5.840736288925144,
+                    "velocityX": 1.191869748325327,
+                    "velocityY": -1.1684288540946337,
+                    "timestamp": 1.4225086999305934
+                },
+                {
+                    "x": 4.800681197551656,
+                    "y": 4.328735146997818,
+                    "heading": -0.8864907082583147,
+                    "angularVelocity": -6.030783436206011,
+                    "velocityX": 1.015311014104143,
+                    "velocityY": -1.2528410177625973,
+                    "timestamp": 1.4467961675942733
+                },
+                {
+                    "x": 4.821257943768876,
+                    "y": 4.295565486099031,
+                    "heading": -1.0326334280053908,
+                    "angularVelocity": -6.017206971545368,
+                    "velocityX": 0.8472166181404436,
+                    "velocityY": -1.3657109649348727,
+                    "timestamp": 1.4710836352579533
+                },
+                {
+                    "x": 4.842407281192705,
+                    "y": 4.263513715444916,
+                    "heading": -1.1735548575556736,
+                    "angularVelocity": -5.802228190344444,
+                    "velocityX": 0.8707922010105955,
+                    "velocityY": -1.319683513240253,
+                    "timestamp": 1.4953711029216332
+                },
+                {
+                    "x": 4.867138629271623,
+                    "y": 4.234341103924059,
+                    "heading": -1.3080892458596534,
+                    "angularVelocity": -5.539251360699312,
+                    "velocityX": 1.0182761093657138,
+                    "velocityY": -1.2011384605766136,
+                    "timestamp": 1.5196585705853132
+                },
+                {
+                    "x": 4.89576478236693,
+                    "y": 4.207846540583478,
+                    "heading": -1.4377333776779964,
+                    "angularVelocity": -5.337902395325306,
+                    "velocityX": 1.1786388557138734,
+                    "velocityY": -1.0908738493227768,
+                    "timestamp": 1.5439460382489931
+                },
+                {
+                    "x": 4.928534232838641,
+                    "y": 4.183793595288781,
+                    "heading": -1.5636936940682342,
+                    "angularVelocity": -5.1862268283573245,
+                    "velocityX": 1.3492329017369804,
+                    "velocityY": -0.9903438937220402,
+                    "timestamp": 1.568233505912673
+                },
+                {
+                    "x": 4.965642273050373,
+                    "y": 4.1619364014492675,
+                    "heading": -1.6870176099857392,
+                    "angularVelocity": -5.077676998904523,
+                    "velocityX": 1.5278678174928029,
+                    "velocityY": -0.8999371256888635,
+                    "timestamp": 1.592520973576353
+                },
+                {
+                    "x": 5.007242132688007,
+                    "y": 4.142028453113074,
+                    "heading": -1.8086864696957439,
+                    "angularVelocity": -5.009532545542041,
+                    "velocityX": 1.712811735405568,
+                    "velocityY": -0.8196798699587677,
+                    "timestamp": 1.616808441240033
+                },
+                {
+                    "x": 5.0534540304279405,
+                    "y": 4.123819388018015,
+                    "heading": -1.9296932957732547,
+                    "angularVelocity": -4.982274305133403,
+                    "velocityX": 1.902705476744277,
+                    "velocityY": -0.749730904316916,
+                    "timestamp": 1.641095908903713
+                },
+                {
+                    "x": 5.102543724383187,
+                    "y": 4.108450898288225,
+                    "heading": -2.039853637710442,
+                    "angularVelocity": -4.535686612644417,
+                    "velocityX": 2.0211944133087334,
+                    "velocityY": -0.6327744803453944,
+                    "timestamp": 1.6653833765673929
+                },
+                {
+                    "x": 5.15444764406018,
+                    "y": 4.096180097409251,
+                    "heading": -2.1398530735226235,
+                    "angularVelocity": -4.117326565161934,
+                    "velocityX": 2.1370659302868042,
+                    "velocityY": -0.5052317948043931,
+                    "timestamp": 1.6896708442310728
+                },
+                {
+                    "x": 5.20922556245635,
+                    "y": 4.087205112082737,
+                    "heading": -2.231114838342211,
+                    "angularVelocity": -3.757566086482643,
+                    "velocityX": 2.2553985106519345,
+                    "velocityY": -0.36953153991984866,
+                    "timestamp": 1.7139583118947528
+                },
+                {
+                    "x": 5.266890046930524,
+                    "y": 4.081668558801882,
+                    "heading": -2.314693981916533,
+                    "angularVelocity": -3.4412456964094225,
+                    "velocityX": 2.3742485331398995,
+                    "velocityY": -0.22795926514540626,
+                    "timestamp": 1.7382457795584327
+                },
+                {
+                    "x": 5.327424168986788,
+                    "y": 4.079676285408984,
+                    "heading": -2.3913241555270592,
+                    "angularVelocity": -3.1551322958679826,
+                    "velocityX": 2.492401550236051,
+                    "velocityY": -0.08202886445327343,
+                    "timestamp": 1.7625332472221127
+                },
+                {
+                    "x": 5.390789031982422,
+                    "y": 4.081310272216797,
+                    "heading": -2.461469590575517,
+                    "angularVelocity": -2.888132925992722,
+                    "velocityX": 2.608953056493045,
+                    "velocityY": 0.06727695247771216,
+                    "timestamp": 1.7868207148857926
+                },
+                {
+                    "x": 5.47852793656002,
+                    "y": 4.0897392429132315,
+                    "heading": -2.541079365075053,
+                    "angularVelocity": -2.4976756873239485,
+                    "velocityX": 2.752718873700339,
+                    "velocityY": 0.2644503807478311,
+                    "timestamp": 1.8186942583180192
+                },
+                {
+                    "x": 5.570818505990881,
+                    "y": 4.101985641066581,
+                    "heading": -2.595195903422616,
+                    "angularVelocity": -1.6978513375091853,
+                    "velocityX": 2.8955227280299005,
+                    "velocityY": 0.384218283711974,
+                    "timestamp": 1.8505678017502458
+                },
+                {
+                    "x": 5.666172128712358,
+                    "y": 4.1161647975520586,
+                    "heading": -2.6133567142303673,
+                    "angularVelocity": -0.5697769639691046,
+                    "velocityX": 2.9916229089567223,
+                    "velocityY": 0.4448566101734819,
+                    "timestamp": 1.8824413451824724
+                },
+                {
+                    "x": 5.762848661651843,
+                    "y": 4.1299008615103485,
+                    "heading": -2.5899687129898252,
+                    "angularVelocity": 0.7337747461392797,
+                    "velocityX": 3.033127871240633,
+                    "velocityY": 0.43095503289420706,
+                    "timestamp": 1.914314888614699
+                },
+                {
+                    "x": 5.85803719097752,
+                    "y": 4.141633583817035,
+                    "heading": -2.5274756720169904,
+                    "angularVelocity": 1.960655585900415,
+                    "velocityX": 2.9864432716142737,
+                    "velocityY": 0.36810222658906844,
+                    "timestamp": 1.9461884320469256
+                },
+                {
+                    "x": 5.9473700887935115,
+                    "y": 4.148469896281193,
+                    "heading": -2.4508224753320094,
+                    "angularVelocity": 2.404916066140258,
+                    "velocityX": 2.8027287899740645,
+                    "velocityY": 0.21448234893285298,
+                    "timestamp": 1.9780619754791522
+                },
+                {
+                    "x": 6.0307522620060645,
+                    "y": 4.150565536032173,
+                    "heading": -2.3598343948212093,
+                    "angularVelocity": 2.854658463194389,
+                    "velocityX": 2.6160308592563855,
+                    "velocityY": 0.06574856527754983,
+                    "timestamp": 2.009935518911379
+                },
+                {
+                    "x": 6.108380396897077,
+                    "y": 4.1477533333139265,
+                    "heading": -2.2542021077222087,
+                    "angularVelocity": 3.3141055472419727,
+                    "velocityX": 2.4355037605428165,
+                    "velocityY": -0.0882299994108409,
+                    "timestamp": 2.0418090623436056
+                },
+                {
+                    "x": 6.180764914708381,
+                    "y": 4.139596420595998,
+                    "heading": -2.1333489972804904,
+                    "angularVelocity": 3.7916433953661524,
+                    "velocityX": 2.270990609036518,
+                    "velocityY": -0.2559148384387137,
+                    "timestamp": 2.0736826057758324
+                },
+                {
+                    "x": 6.2487782508806635,
+                    "y": 4.1255186451287145,
+                    "heading": -1.9962477740611584,
+                    "angularVelocity": 4.301411404441215,
+                    "velocityX": 2.133849231946881,
+                    "velocityY": -0.4416758838633167,
+                    "timestamp": 2.1055561492080592
+                },
+                {
+                    "x": 6.318999325259612,
+                    "y": 4.108403885933248,
+                    "heading": -1.8570700146176022,
+                    "angularVelocity": 4.366560616000945,
+                    "velocityX": 2.2031147722330044,
+                    "velocityY": -0.5369581587895442,
+                    "timestamp": 2.137429692640286
+                },
+                {
+                    "x": 6.393124517437766,
+                    "y": 4.09868995620756,
+                    "heading": -1.7226225348946564,
+                    "angularVelocity": 4.218152901914545,
+                    "velocityX": 2.3256024964958177,
+                    "velocityY": -0.3047646630925519,
+                    "timestamp": 2.169303236072513
+                },
+                {
+                    "x": 6.468812945370671,
+                    "y": 4.097296283093324,
+                    "heading": -1.5892075925845424,
+                    "angularVelocity": 4.185758090994726,
+                    "velocityX": 2.3746474280101166,
+                    "velocityY": -0.04372507616542974,
+                    "timestamp": 2.2011767795047397
+                },
+                {
+                    "x": 6.543776258333572,
+                    "y": 4.104341851159788,
+                    "heading": -1.4561024343589095,
+                    "angularVelocity": 4.176038930489708,
+                    "velocityX": 2.351897683490888,
+                    "velocityY": 0.22104753057800555,
+                    "timestamp": 2.2330503229369665
+                },
+                {
+                    "x": 6.614153245906366,
+                    "y": 4.118263151934315,
+                    "heading": -1.3162963129525131,
+                    "angularVelocity": 4.386274833347886,
+                    "velocityX": 2.208006390078291,
+                    "velocityY": 0.43676664956088845,
+                    "timestamp": 2.2649238663691933
+                },
+                {
+                    "x": 6.676725935567768,
+                    "y": 4.132111873679476,
+                    "heading": -1.164406226014857,
+                    "angularVelocity": 4.765396958785676,
+                    "velocityX": 1.9631544824770328,
+                    "velocityY": 0.4344895563497056,
+                    "timestamp": 2.29679740980142
+                },
+                {
+                    "x": 6.732923820729643,
+                    "y": 4.140964584087909,
+                    "heading": -1.0033332436559739,
+                    "angularVelocity": 5.053500960800803,
+                    "velocityX": 1.7631514764390552,
+                    "velocityY": 0.27774478313828127,
+                    "timestamp": 2.328670953233647
+                },
+                {
+                    "x": 6.785034236650818,
+                    "y": 4.142631098888223,
+                    "heading": -0.8355937264762496,
+                    "angularVelocity": 5.262656708890626,
+                    "velocityX": 1.6349112872241036,
+                    "velocityY": 0.0522852065023047,
+                    "timestamp": 2.3605444966658737
+                },
+                {
+                    "x": 6.835668705020557,
+                    "y": 4.136059975961784,
+                    "heading": -0.6631765444972689,
+                    "angularVelocity": 5.409413683345085,
+                    "velocityX": 1.5886049342899025,
+                    "velocityY": -0.2061622969661811,
+                    "timestamp": 2.3924180400981006
+                },
+                {
+                    "x": 6.8889189304898775,
+                    "y": 4.121434248751472,
+                    "heading": -0.4913644813841438,
+                    "angularVelocity": 5.390428694520651,
+                    "velocityX": 1.6706716522606933,
+                    "velocityY": -0.45886731236551676,
+                    "timestamp": 2.4242915835303274
+                },
+                {
+                    "x": 6.949941444507073,
+                    "y": 4.105368374325079,
+                    "heading": -0.330980540513716,
+                    "angularVelocity": 5.031882985067382,
+                    "velocityX": 1.9145192986449624,
+                    "velocityY": -0.5040504661979253,
+                    "timestamp": 2.456165126962554
+                },
+                {
+                    "x": 7.016508340627581,
+                    "y": 4.095339734393401,
+                    "heading": -0.17821539872753786,
+                    "angularVelocity": 4.792850914458417,
+                    "velocityX": 2.0884686467963878,
+                    "velocityY": -0.31463837564850267,
+                    "timestamp": 2.488038670394781
+                },
+                {
+                    "x": 7.085360516906702,
+                    "y": 4.093448303803928,
+                    "heading": -0.02746406984555507,
+                    "angularVelocity": 4.7296695832557045,
+                    "velocityX": 2.1601669869408497,
+                    "velocityY": -0.05934171057873482,
+                    "timestamp": 2.519912213827008
+                },
+                {
+                    "x": 7.153846633279139,
+                    "y": 4.100009116269512,
+                    "heading": 0.121772089508303,
+                    "angularVelocity": 4.682132680703696,
+                    "velocityX": 2.1486822297640367,
+                    "velocityY": 0.20583881674575163,
+                    "timestamp": 2.5517857572592346
+                },
+                {
+                    "x": 7.2178667451999,
+                    "y": 4.113588367220929,
+                    "heading": 0.27676556654047163,
+                    "angularVelocity": 4.862762665899784,
+                    "velocityX": 2.008565883391336,
+                    "velocityY": 0.4260351843305824,
+                    "timestamp": 2.5836593006914614
+                },
+                {
+                    "x": 7.274040071387795,
+                    "y": 4.126189441281968,
+                    "heading": 0.44297350868055696,
+                    "angularVelocity": 5.214605100104267,
+                    "velocityX": 1.7623809636144747,
+                    "velocityY": 0.3953458795013916,
+                    "timestamp": 2.6155328441236882
+                },
+                {
+                    "x": 7.324153742240104,
+                    "y": 4.133335840122352,
+                    "heading": 0.617641408977622,
+                    "angularVelocity": 5.480027680902935,
+                    "velocityX": 1.5722654419916315,
+                    "velocityY": 0.22421099353388252,
+                    "timestamp": 2.647406387555915
+                },
+                {
+                    "x": 7.370062418422902,
+                    "y": 4.133468677579737,
+                    "heading": 0.8003841200365157,
+                    "angularVelocity": 5.733366653991959,
+                    "velocityX": 1.440338012007226,
+                    "velocityY": 0.00416764008895702,
+                    "timestamp": 2.679279930988142
+                },
+                {
+                    "x": 7.413378960747577,
+                    "y": 4.125978597308371,
+                    "heading": 0.9928213817572759,
+                    "angularVelocity": 6.037523318671553,
+                    "velocityX": 1.3590124492050755,
+                    "velocityY": -0.2349936488013014,
+                    "timestamp": 2.7111534744203687
+                },
+                {
+                    "x": 7.454276397546973,
+                    "y": 4.11151825545799,
+                    "heading": 1.2015436063625509,
+                    "angularVelocity": 6.548447462362883,
+                    "velocityX": 1.2831154743229476,
+                    "velocityY": -0.4536785149454069,
+                    "timestamp": 2.7430270178525955
+                },
+                {
+                    "x": 7.496754891759898,
+                    "y": 4.105364802951648,
+                    "heading": 1.412047118709339,
+                    "angularVelocity": 6.604333553135948,
+                    "velocityX": 1.3327195422513116,
+                    "velocityY": -0.19305831243477098,
+                    "timestamp": 2.7749005612848223
+                },
+                {
+                    "x": 7.53888503669894,
+                    "y": 4.1076484340015265,
+                    "heading": 1.6249494045277855,
+                    "angularVelocity": 6.679592630519619,
+                    "velocityX": 1.3217904381614993,
+                    "velocityY": 0.07164660103553838,
+                    "timestamp": 2.806774104717049
+                },
+                {
+                    "x": 7.576001000454075,
+                    "y": 4.116500503183424,
+                    "heading": 1.8445622032463762,
+                    "angularVelocity": 6.890128146108359,
+                    "velocityX": 1.1644756044791265,
+                    "velocityY": 0.277724665308084,
+                    "timestamp": 2.838647648149276
+                },
+                {
+                    "x": 7.60515186851165,
+                    "y": 4.124308875688622,
+                    "heading": 2.0739714188941045,
+                    "angularVelocity": 7.1974807612943055,
+                    "velocityX": 0.9145788299176564,
+                    "velocityY": 0.24497974383681875,
+                    "timestamp": 2.8705211915815028
+                },
+                {
+                    "x": 7.62773108442525,
+                    "y": 4.127593662867362,
+                    "heading": 2.3140115714104335,
+                    "angularVelocity": 7.53101559061771,
+                    "velocityX": 0.708399929289633,
+                    "velocityY": 0.10305685609521825,
+                    "timestamp": 2.9023947350137296
+                },
+                {
+                    "x": 7.642808233685486,
+                    "y": 4.127168273515703,
+                    "heading": 2.549384793893863,
+                    "angularVelocity": 7.384595408537156,
+                    "velocityX": 0.473030219946994,
+                    "velocityY": -0.013346158156635379,
+                    "timestamp": 2.9342682784459564
+                },
+                {
+                    "x": 7.655297430086512,
+                    "y": 4.12694395453313,
+                    "heading": 2.7459505912038105,
+                    "angularVelocity": 6.167051922793274,
+                    "velocityX": 0.39183583173240866,
+                    "velocityY": -0.007037779876922506,
+                    "timestamp": 2.966141821878183
+                },
+                {
+                    "x": 7.665239437445753,
+                    "y": 4.126897699920467,
+                    "heading": 2.903400180799407,
+                    "angularVelocity": 4.939820698956328,
+                    "velocityX": 0.31192036682023955,
+                    "velocityY": -0.001451191417126478,
+                    "timestamp": 2.99801536531041
+                },
+                {
+                    "x": 7.672649475189336,
+                    "y": 4.12693483442264,
+                    "heading": 3.0216231319761504,
+                    "angularVelocity": 3.7091248241076054,
+                    "velocityX": 0.23248239591994224,
+                    "velocityY": 0.001165057228464201,
+                    "timestamp": 3.029888908742637
+                },
+                {
+                    "x": 7.677558550585934,
+                    "y": 4.1269805608298435,
+                    "heading": 3.1005189939079423,
+                    "angularVelocity": 2.4752774067793823,
+                    "velocityX": 0.15401724653037796,
+                    "velocityY": 0.0014346195082003044,
+                    "timestamp": 3.0617624521748636
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": 1.238676401825463,
+                    "velocityX": 0.07659799166218657,
+                    "velocityY": 0.0006098841880359634,
+                    "timestamp": 3.0936359956070905
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.14,
+                    "angularVelocity": -2.349124968959552e-31,
+                    "velocityX": 6.522838948908763e-32,
+                    "velocityY": -6.176555916296272e-32,
+                    "timestamp": 3.1255095390393173
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -7746,7 +21143,25 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 23
+                },
+                {
+                    "x": 2.6104869842529297,
+                    "y": 2.6966493129730225,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 14
+                },
+                {
+                    "x": 5.646764755249023,
+                    "y": 1.5066063404083252,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 27
                 },
                 {
                     "x": 7.68,
@@ -7758,7 +21173,593 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.291384520253252,
+                    "y": 4.354032854764245,
+                    "heading": 2.74598551663602,
+                    "angularVelocity": -0.1129108392120503,
+                    "velocityX": -0.24231786763597,
+                    "velocityY": -0.1678311541441644,
+                    "timestamp": 0.03555445510807641
+                },
+                {
+                    "x": 2.274618850931762,
+                    "y": 4.341572242021881,
+                    "heading": 2.7363109696234753,
+                    "angularVelocity": -0.2721050564025352,
+                    "velocityX": -0.47154904415004567,
+                    "velocityY": -0.3504655803186264,
+                    "timestamp": 0.07110891021615282
+                },
+                {
+                    "x": 2.250443983775186,
+                    "y": 4.321957452674385,
+                    "heading": 2.718567267740621,
+                    "angularVelocity": -0.4990570613139176,
+                    "velocityX": -0.6799391829544631,
+                    "velocityY": -0.5516830250350497,
+                    "timestamp": 0.10666336532422922
+                },
+                {
+                    "x": 2.2201183125080237,
+                    "y": 4.294438762451095,
+                    "heading": 2.688674932435733,
+                    "angularVelocity": -0.8407479516708394,
+                    "velocityX": -0.8529359028279262,
+                    "velocityY": -0.7739871174973922,
+                    "timestamp": 0.14221782043230563
+                },
+                {
+                    "x": 2.1856076192251765,
+                    "y": 4.258557396291605,
+                    "heading": 2.63987728987899,
+                    "angularVelocity": -1.3724761751631571,
+                    "velocityX": -0.9706432900727432,
+                    "velocityY": -1.009194657896466,
+                    "timestamp": 0.17777227554038205
+                },
+                {
+                    "x": 2.1489602975624615,
+                    "y": 4.2143768004023965,
+                    "heading": 2.56722210682279,
+                    "angularVelocity": -2.0434902696538804,
+                    "velocityX": -1.0307378231874669,
+                    "velocityY": -1.2426177185084084,
+                    "timestamp": 0.21332673064845847
+                },
+                {
+                    "x": 2.1162123400405237,
+                    "y": 4.163459966768879,
+                    "heading": 2.4666019501957965,
+                    "angularVelocity": -2.8300294947885973,
+                    "velocityX": -0.9210648123390461,
+                    "velocityY": -1.4320802689492316,
+                    "timestamp": 0.2488811857565349
+                },
+                {
+                    "x": 2.0883593879928255,
+                    "y": 4.106985689754014,
+                    "heading": 2.334003256666977,
+                    "angularVelocity": -3.7294536824078297,
+                    "velocityX": -0.7833885222831486,
+                    "velocityY": -1.588388201793505,
+                    "timestamp": 0.2844356408646113
+                },
+                {
+                    "x": 2.0671900205502793,
+                    "y": 4.044210359842601,
+                    "heading": 2.181383778591042,
+                    "angularVelocity": -4.2925556758502195,
+                    "velocityX": -0.5954068872155824,
+                    "velocityY": -1.765610799563443,
+                    "timestamp": 0.31999009597268774
+                },
+                {
+                    "x": 2.0546372297458078,
+                    "y": 3.9756937078562617,
+                    "heading": 2.021719977202011,
+                    "angularVelocity": -4.49068340110105,
+                    "velocityX": -0.35305816855621736,
+                    "velocityY": -1.927090480730638,
+                    "timestamp": 0.35554455108076416
+                },
+                {
+                    "x": 2.0513227865299846,
+                    "y": 3.902144544449752,
+                    "heading": 1.8643846688260233,
+                    "angularVelocity": -4.42519250816047,
+                    "velocityX": -0.0932216006615165,
+                    "velocityY": -2.068634245214532,
+                    "timestamp": 0.3910990061888406
+                },
+                {
+                    "x": 2.056389020104272,
+                    "y": 3.822243076047335,
+                    "heading": 1.7096006131055106,
+                    "angularVelocity": -4.353436306364677,
+                    "velocityX": 0.14249222942348805,
+                    "velocityY": -2.247298352893811,
+                    "timestamp": 0.426653461296917
+                },
+                {
+                    "x": 2.0690751873782185,
+                    "y": 3.7373642087091223,
+                    "heading": 1.5737353715537044,
+                    "angularVelocity": -3.821328189078159,
+                    "velocityX": 0.35680949786416494,
+                    "velocityY": -2.3872920307793386,
+                    "timestamp": 0.4622079164049934
+                },
+                {
+                    "x": 2.089274580938585,
+                    "y": 3.648200269765939,
+                    "heading": 1.460663871168444,
+                    "angularVelocity": -3.1802343768608408,
+                    "velocityX": 0.5681255274188619,
+                    "velocityY": -2.507813399815797,
+                    "timestamp": 0.49776237151306985
+                },
+                {
+                    "x": 2.1169367654773477,
+                    "y": 3.55409496646177,
+                    "heading": 1.3686138295927854,
+                    "angularVelocity": -2.5889875487010094,
+                    "velocityX": 0.7780230200315752,
+                    "velocityY": -2.646793573916784,
+                    "timestamp": 0.5333168266211462
+                },
+                {
+                    "x": 2.1519045434306308,
+                    "y": 3.4545849603291003,
+                    "heading": 1.296733638399589,
+                    "angularVelocity": -2.0216929488779694,
+                    "velocityX": 0.9834991943200934,
+                    "velocityY": -2.798805545751856,
+                    "timestamp": 0.5688712817292226
+                },
+                {
+                    "x": 2.1939849352834964,
+                    "y": 3.3493550759903967,
+                    "heading": 1.244789692250972,
+                    "angularVelocity": -1.4609687025358928,
+                    "velocityX": 1.1835476517626813,
+                    "velocityY": -2.959682099439628,
+                    "timestamp": 0.604425736837299
+                },
+                {
+                    "x": 2.243083202431903,
+                    "y": 3.2382839213654937,
+                    "heading": 1.2128218877185057,
+                    "angularVelocity": -0.8991223303884791,
+                    "velocityX": 1.3809315034968324,
+                    "velocityY": -3.123972911053636,
+                    "timestamp": 0.6399801919453755
+                },
+                {
+                    "x": 2.299610340542871,
+                    "y": 3.1217794185343526,
+                    "heading": 1.2005607333325377,
+                    "angularVelocity": -0.3448556404168553,
+                    "velocityX": 1.5898749661368734,
+                    "velocityY": -3.276790559073325,
+                    "timestamp": 0.6755346470534519
+                },
+                {
+                    "x": 2.3661237313117724,
+                    "y": 3.0048767909387704,
+                    "heading": 1.2005598156629014,
+                    "angularVelocity": -0.000025810257353301716,
+                    "velocityX": 1.8707470151551455,
+                    "velocityY": -3.2879881646400775,
+                    "timestamp": 0.7110891021615283
+                },
+                {
+                    "x": 2.4413914156279892,
+                    "y": 2.893838891515528,
+                    "heading": 1.1996733189380515,
+                    "angularVelocity": -0.02493349208011108,
+                    "velocityX": 2.116969141769213,
+                    "velocityY": -3.123037579558325,
+                    "timestamp": 0.7466435572696047
+                },
+                {
+                    "x": 2.522968201294396,
+                    "y": 2.7909984002833275,
+                    "heading": 1.191481222958301,
+                    "angularVelocity": -0.2304098306344059,
+                    "velocityX": 2.2944181093040097,
+                    "velocityY": -2.89247833835709,
+                    "timestamp": 0.7821980123776812
+                },
+                {
+                    "x": 2.6104869842529297,
+                    "y": 2.6966493129730225,
+                    "heading": 1.1763516697745318,
+                    "angularVelocity": -0.4255318535406987,
+                    "velocityX": 2.4615419556423808,
+                    "velocityY": -2.653650211302861,
+                    "timestamp": 0.8177524674857576
+                },
+                {
+                    "x": 2.7952393085826728,
+                    "y": 2.5500987298885027,
+                    "heading": 1.131439771707853,
+                    "angularVelocity": -0.6752506092749339,
+                    "velocityX": 2.777752108882206,
+                    "velocityY": -2.203388740562692,
+                    "timestamp": 0.884263918863645
+                },
+                {
+                    "x": 2.9911597283478026,
+                    "y": 2.437065890505019,
+                    "heading": 1.0475109017935547,
+                    "angularVelocity": -1.2618709737283185,
+                    "velocityX": 2.9456644789180824,
+                    "velocityY": -1.6994492984566387,
+                    "timestamp": 0.9507753702415324
+                },
+                {
+                    "x": 3.2086737929320703,
+                    "y": 2.3383672587643702,
+                    "heading": 1.0088878551520015,
+                    "angularVelocity": -0.5806976970343763,
+                    "velocityX": 3.2703250354356728,
+                    "velocityY": -1.4839344157427674,
+                    "timestamp": 1.0172868216194197
+                },
+                {
+                    "x": 3.4256073991681717,
+                    "y": 2.2401993070344908,
+                    "heading": 0.9691349707945731,
+                    "angularVelocity": -0.5976848126733959,
+                    "velocityX": 3.2615978413038147,
+                    "velocityY": -1.4759556391595554,
+                    "timestamp": 1.083798272997307
+                },
+                {
+                    "x": 3.641898030119061,
+                    "y": 2.142612874932996,
+                    "heading": 0.9280602960666862,
+                    "angularVelocity": -0.6175579374222298,
+                    "velocityX": 3.251930704714679,
+                    "velocityY": -1.4672124886743745,
+                    "timestamp": 1.1503097243751943
+                },
+                {
+                    "x": 3.8574674811397935,
+                    "y": 2.045673041019181,
+                    "heading": 0.8854247472051884,
+                    "angularVelocity": -0.6410256877310082,
+                    "velocityX": 3.2410877609025053,
+                    "velocityY": -1.4574908817287378,
+                    "timestamp": 1.2168211757530816
+                },
+                {
+                    "x": 4.072209689171901,
+                    "y": 1.949463725749032,
+                    "heading": 0.8409091268268268,
+                    "angularVelocity": -0.6692925722736718,
+                    "velocityX": 3.2286501584823513,
+                    "velocityY": -1.446507530312815,
+                    "timestamp": 1.2833326271309689
+                },
+                {
+                    "x": 4.28597464141242,
+                    "y": 1.8540976935596791,
+                    "heading": 0.7940685595150717,
+                    "angularVelocity": -0.7042481609013259,
+                    "velocityX": 3.2139571128286692,
+                    "velocityY": -1.4338287650276498,
+                    "timestamp": 1.3498440785088561
+                },
+                {
+                    "x": 4.498537652945423,
+                    "y": 1.7597351759166577,
+                    "heading": 0.7442459258691415,
+                    "angularVelocity": -0.7490835429655718,
+                    "velocityX": 3.195885928354179,
+                    "velocityY": -1.4187409188666942,
+                    "timestamp": 1.4163555298867434
+                },
+                {
+                    "x": 4.709533812148138,
+                    "y": 1.6666229170102642,
+                    "heading": 0.6903878152301356,
+                    "angularVelocity": -0.8097569594897124,
+                    "velocityX": 3.1723282958288084,
+                    "velocityY": -1.399943272585839,
+                    "timestamp": 1.4828669812646307
+                },
+                {
+                    "x": 4.924860666444789,
+                    "y": 1.5757431293365876,
+                    "heading": 0.6452055578690052,
+                    "angularVelocity": -0.679315462602457,
+                    "velocityX": 3.2374403179576423,
+                    "velocityY": -1.3663780565746433,
+                    "timestamp": 1.549378432642518
+                },
+                {
+                    "x": 5.15821860461426,
+                    "y": 1.5166188222705783,
+                    "heading": 0.6185000496396688,
+                    "angularVelocity": -0.4015174481399904,
+                    "velocityX": 3.5085377530500543,
+                    "velocityY": -0.8889342487820968,
+                    "timestamp": 1.6158898840204052
+                },
+                {
+                    "x": 5.39961745545984,
+                    "y": 1.4934726786236598,
+                    "heading": 0.5961273041444063,
+                    "angularVelocity": -0.33637433903149927,
+                    "velocityX": 3.6294329148534703,
+                    "velocityY": -0.3480023840618476,
+                    "timestamp": 1.6824013353982925
+                },
+                {
+                    "x": 5.646764755249023,
+                    "y": 1.5066063404083252,
+                    "heading": 0.585736957474559,
+                    "angularVelocity": -0.15621891350429284,
+                    "velocityX": 3.7158608731150236,
+                    "velocityY": 0.19746466980619687,
+                    "timestamp": 1.7489127867761798
+                },
+                {
+                    "x": 5.78386589002804,
+                    "y": 1.5248142647082106,
+                    "heading": 0.5861133793107512,
+                    "angularVelocity": 0.010285140296820981,
+                    "velocityX": 3.7460749363537476,
+                    "velocityY": 0.4975031678094133,
+                    "timestamp": 1.7855113965499783
+                },
+                {
+                    "x": 5.9191942381460025,
+                    "y": 1.554048616452813,
+                    "heading": 0.586114299955562,
+                    "angularVelocity": 0.00002515518530364309,
+                    "velocityX": 3.69763630242715,
+                    "velocityY": 0.7987831211428086,
+                    "timestamp": 1.8221100063237767
+                },
+                {
+                    "x": 6.0517259817313,
+                    "y": 1.5940956419716419,
+                    "heading": 0.5861150757223451,
+                    "angularVelocity": 0.000021196618886827452,
+                    "velocityX": 3.6212234400274688,
+                    "velocityY": 1.0942225884082484,
+                    "timestamp": 1.8587086160975752
+                },
+                {
+                    "x": 6.180598624537076,
+                    "y": 1.644694580267861,
+                    "heading": 0.5861160659567196,
+                    "angularVelocity": 0.00002705661173256264,
+                    "velocityX": 3.521244211249743,
+                    "velocityY": 1.3825371676397282,
+                    "timestamp": 1.8953072258713737
+                },
+                {
+                    "x": 6.304973627271485,
+                    "y": 1.7055153509385155,
+                    "heading": 0.5861182648046722,
+                    "angularVelocity": 0.00006008009501406976,
+                    "velocityX": 3.3983532025703247,
+                    "velocityY": 1.6618328140485057,
+                    "timestamp": 1.9319058356451722
+                },
+                {
+                    "x": 6.427040241028168,
+                    "y": 1.7708422617715522,
+                    "heading": 0.5861256207319434,
+                    "angularVelocity": 0.0002009892538702712,
+                    "velocityX": 3.335280069683768,
+                    "velocityY": 1.784956074473775,
+                    "timestamp": 1.9685044454189706
+                },
+                {
+                    "x": 6.5491066329086145,
+                    "y": 1.8361695861725622,
+                    "heading": 0.5861329781592151,
+                    "angularVelocity": 0.0002010302390486887,
+                    "velocityX": 3.3352740072612326,
+                    "velocityY": 1.7849673745744064,
+                    "timestamp": 2.005103055192769
+                },
+                {
+                    "x": 6.6711729055545455,
+                    "y": 1.9014971320931282,
+                    "heading": 0.586140337461326,
+                    "angularVelocity": 0.00020108146611328895,
+                    "velocityX": 3.3352707493638243,
+                    "velocityY": 1.7849734272511966,
+                    "timestamp": 2.0417016649665674
+                },
+                {
+                    "x": 6.789839361593639,
+                    "y": 1.9681130226753558,
+                    "heading": 0.59368067703471,
+                    "angularVelocity": 0.20602803275828666,
+                    "velocityX": 3.2423760567006434,
+                    "velocityY": 1.820175438191616,
+                    "timestamp": 2.0783002747403656
+                },
+                {
+                    "x": 6.899503815249364,
+                    "y": 2.0364566539738247,
+                    "heading": 0.622995969771871,
+                    "angularVelocity": 0.8009947076773526,
+                    "velocityX": 2.9964103645881037,
+                    "velocityY": 1.8673832618472042,
+                    "timestamp": 2.114898884514164
+                },
+                {
+                    "x": 7.0002752102658246,
+                    "y": 2.1033967313761632,
+                    "heading": 0.6764261486140803,
+                    "angularVelocity": 1.4598964051487269,
+                    "velocityX": 2.753421390574382,
+                    "velocityY": 1.829033338043951,
+                    "timestamp": 2.151497494287962
+                },
+                {
+                    "x": 7.092625779879376,
+                    "y": 2.166299323632935,
+                    "heading": 0.7535384043827013,
+                    "angularVelocity": 2.1069722660292705,
+                    "velocityX": 2.5233354541151716,
+                    "velocityY": 1.7187153458983286,
+                    "timestamp": 2.1880961040617604
+                },
+                {
+                    "x": 7.177366976931162,
+                    "y": 2.223194562450384,
+                    "heading": 0.8522168962516827,
+                    "angularVelocity": 2.696236072322813,
+                    "velocityX": 2.3154212024865664,
+                    "velocityY": 1.5545737712196184,
+                    "timestamp": 2.2246947138355586
+                },
+                {
+                    "x": 7.2553185898803205,
+                    "y": 2.2728300819136558,
+                    "heading": 0.9705292362405307,
+                    "angularVelocity": 3.2327003872576037,
+                    "velocityX": 2.1299063934654243,
+                    "velocityY": 1.3562132488105336,
+                    "timestamp": 2.261293323609357
+                },
+                {
+                    "x": 7.327198001580843,
+                    "y": 2.3144960913605033,
+                    "heading": 1.1077214572134357,
+                    "angularVelocity": 3.748563724710752,
+                    "velocityX": 1.9639929534148923,
+                    "velocityY": 1.1384588022432733,
+                    "timestamp": 2.297891933383155
+                },
+                {
+                    "x": 7.393635990725555,
+                    "y": 2.347893157970517,
+                    "heading": 1.2645826825431576,
+                    "angularVelocity": 4.285988629055024,
+                    "velocityX": 1.8153145585403265,
+                    "velocityY": 0.9125228203046082,
+                    "timestamp": 2.3344905431569534
+                },
+                {
+                    "x": 7.455288023435831,
+                    "y": 2.373088898730826,
+                    "heading": 1.443531300083093,
+                    "angularVelocity": 4.889492214211044,
+                    "velocityX": 1.684545754369454,
+                    "velocityY": 0.6884343672077653,
+                    "timestamp": 2.3710891529307516
+                },
+                {
+                    "x": 7.509792212794572,
+                    "y": 2.3938656260636906,
+                    "heading": 1.6501307836273436,
+                    "angularVelocity": 5.64500905420071,
+                    "velocityX": 1.4892420694559032,
+                    "velocityY": 0.5676917090916096,
+                    "timestamp": 2.40768776270455
+                },
+                {
+                    "x": 7.556730666272478,
+                    "y": 2.4114332825613682,
+                    "heading": 1.888437501864436,
+                    "angularVelocity": 6.51135984973125,
+                    "velocityX": 1.2825201221580125,
+                    "velocityY": 0.48000884750149375,
+                    "timestamp": 2.444286372478348
+                },
+                {
+                    "x": 7.593462104085248,
+                    "y": 2.4287428163755616,
+                    "heading": 2.1429512418884697,
+                    "angularVelocity": 6.954191473312333,
+                    "velocityX": 1.0036293192499064,
+                    "velocityY": 0.4729560472700227,
+                    "timestamp": 2.4808849822521464
+                },
+                {
+                    "x": 7.620818492305724,
+                    "y": 2.4405113575302453,
+                    "heading": 2.4064355131818194,
+                    "angularVelocity": 7.199297266257987,
+                    "velocityX": 0.7474706932737378,
+                    "velocityY": 0.32155705441874516,
+                    "timestamp": 2.5174835920259446
+                },
+                {
+                    "x": 7.6400541302749785,
+                    "y": 2.446864161128428,
+                    "heading": 2.650140298596015,
+                    "angularVelocity": 6.658853626420207,
+                    "velocityX": 0.5255838428875319,
+                    "velocityY": 0.17358046213904924,
+                    "timestamp": 2.554082201799743
+                },
+                {
+                    "x": 7.6561677382990485,
+                    "y": 2.4508382476782002,
+                    "heading": 2.845393638086096,
+                    "angularVelocity": 5.334993342557683,
+                    "velocityX": 0.4402792380273905,
+                    "velocityY": 0.10858572427572141,
+                    "timestamp": 2.590680811573541
+                },
+                {
+                    "x": 7.668208670546753,
+                    "y": 2.453890313590812,
+                    "heading": 2.992258715743694,
+                    "angularVelocity": 4.01285946557297,
+                    "velocityX": 0.3289997167139778,
+                    "velocityY": 0.08339294665767145,
+                    "timestamp": 2.6272794213473394
+                },
+                {
+                    "x": 7.676124567266394,
+                    "y": 2.4559646616612736,
+                    "heading": 3.0905802273604146,
+                    "angularVelocity": 2.686482143021458,
+                    "velocityX": 0.21628954674959436,
+                    "velocityY": 0.056678329676522185,
+                    "timestamp": 2.6638780311211376
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": 1.350318302936367,
+                    "velocityX": 0.10589016242853574,
+                    "velocityY": 0.028289007290860937,
+                    "timestamp": 2.700476640894936
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 3.14,
+                    "angularVelocity": -2.3806959148386727e-33,
+                    "velocityX": 4.3916568461332404e-35,
+                    "velocityY": 1.2700164208802373e-34,
+                    "timestamp": 2.737075250668734
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -7786,11 +21787,6 @@
                     "x": 5.61,
                     "y": 2.81,
                     "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
                 }
             ]
         },
@@ -7803,7 +21799,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 23
+                },
+                {
+                    "x": 2.7430825233459473,
+                    "y": 2.7833950519561768,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 35
                 },
                 {
                     "x": 7.68,
@@ -7815,7 +21820,539 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": 2.75,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 2.0996459321900828e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.2983894677520005,
+                    "y": 4.3533419612939515,
+                    "heading": 2.758567124910339,
+                    "angularVelocity": 0.29099245314259387,
+                    "velocityX": -0.05470361814673814,
+                    "velocityY": -0.226148099446212,
+                    "timestamp": 0.02944105531884974
+                },
+                {
+                    "x": 2.295387593245339,
+                    "y": 4.339939574817211,
+                    "heading": 2.775252952592925,
+                    "angularVelocity": 0.5667537220346529,
+                    "velocityX": -0.10196219103394874,
+                    "velocityY": -0.45522778757728155,
+                    "timestamp": 0.05888211063769948
+                },
+                {
+                    "x": 2.2912544329244677,
+                    "y": 4.31970469118274,
+                    "heading": 2.7995524599349486,
+                    "angularVelocity": 0.8253612881351188,
+                    "velocityX": -0.14038764154710928,
+                    "velocityY": -0.687301573103468,
+                    "timestamp": 0.08832316595654921
+                },
+                {
+                    "x": 2.286299680545296,
+                    "y": 4.292550560538348,
+                    "heading": 2.8308875928825827,
+                    "angularVelocity": 1.064334569813193,
+                    "velocityX": -0.1682939801413744,
+                    "velocityY": -0.922321919180889,
+                    "timestamp": 0.11776422127539896
+                },
+                {
+                    "x": 2.280893923712392,
+                    "y": 4.258397506462172,
+                    "heading": 2.868587408224874,
+                    "angularVelocity": 1.2805184778194243,
+                    "velocityX": -0.18361287577360647,
+                    "velocityY": -1.1600485684461586,
+                    "timestamp": 0.1472052765942487
+                },
+                {
+                    "x": 2.2754825429318224,
+                    "y": 4.217182700090041,
+                    "heading": 2.9118655493454413,
+                    "angularVelocity": 1.4699928603733934,
+                    "velocityX": -0.18380389975713432,
+                    "velocityY": -1.3999092738276546,
+                    "timestamp": 0.17664633191309845
+                },
+                {
+                    "x": 2.2706016921590138,
+                    "y": 4.168876446645306,
+                    "heading": 2.959795755168493,
+                    "angularVelocity": 1.6280056982999451,
+                    "velocityX": -0.16578382534010866,
+                    "velocityY": -1.6407785971519722,
+                    "timestamp": 0.2060873872319482
+                },
+                {
+                    "x": 2.2668942123670868,
+                    "y": 4.113508113408168,
+                    "heading": 3.0112851676807657,
+                    "angularVelocity": 1.748898331076693,
+                    "velocityX": -0.12592890274395677,
+                    "velocityY": -1.880650426334687,
+                    "timestamp": 0.23552844255079794
+                },
+                {
+                    "x": 2.2651203704726766,
+                    "y": 4.051204935167073,
+                    "heading": 3.0650426993920417,
+                    "angularVelocity": 1.8259376618492857,
+                    "velocityX": -0.06025062196987639,
+                    "velocityY": -2.1162005765875382,
+                    "timestamp": 0.2649694978696477
+                },
+                {
+                    "x": 2.2661533431015752,
+                    "y": 3.9822449997835685,
+                    "heading": 3.1195407017354904,
+                    "angularVelocity": 1.8510886159897915,
+                    "velocityX": 0.03508612778011459,
+                    "velocityY": -2.3423051462205073,
+                    "timestamp": 0.2944105531884974
+                },
+                {
+                    "x": 2.2709425124463385,
+                    "y": 3.9071174668882147,
+                    "heading": 3.1729959824105403,
+                    "angularVelocity": 1.8156713506402375,
+                    "velocityX": 0.16266975802653263,
+                    "velocityY": -2.5517948348561,
+                    "timestamp": 0.3238516085073471
+                },
+                {
+                    "x": 2.2797199160454222,
+                    "y": 3.825989687229236,
+                    "heading": 3.2244176609184314,
+                    "angularVelocity": 1.7465976661158726,
+                    "velocityX": 0.2981348156179574,
+                    "velocityY": -2.7556002589022834,
+                    "timestamp": 0.35329266382619684
+                },
+                {
+                    "x": 2.292782229397934,
+                    "y": 3.7391196184482327,
+                    "heading": 3.272453259481913,
+                    "angularVelocity": 1.6315854864321535,
+                    "velocityX": 0.4436768047559834,
+                    "velocityY": -2.9506438488767315,
+                    "timestamp": 0.38273371914504656
+                },
+                {
+                    "x": 2.31050581940653,
+                    "y": 3.6469279173399998,
+                    "heading": 3.3152109067306803,
+                    "angularVelocity": 1.4523136750940884,
+                    "velocityX": 0.6020025374990144,
+                    "velocityY": -3.131399337074948,
+                    "timestamp": 0.4121747744638963
+                },
+                {
+                    "x": 2.3333478257637266,
+                    "y": 3.5501341924969156,
+                    "heading": 3.350152812747203,
+                    "angularVelocity": 1.186842850505799,
+                    "velocityX": 0.7758555564606926,
+                    "velocityY": -3.287712474800853,
+                    "timestamp": 0.441615829782746
+                },
+                {
+                    "x": 2.3619457015355163,
+                    "y": 3.450260447984075,
+                    "heading": 3.374288635448379,
+                    "angularVelocity": 0.8198015471858178,
+                    "velocityX": 0.9713604170119489,
+                    "velocityY": -3.3923289580212845,
+                    "timestamp": 0.4710568851015957
+                },
+                {
+                    "x": 2.396816597338257,
+                    "y": 3.3487121183154565,
+                    "heading": 3.38694435959391,
+                    "angularVelocity": 0.42986652511155254,
+                    "velocityX": 1.184430905247282,
+                    "velocityY": -3.4492082083620734,
+                    "timestamp": 0.5004979404204455
+                },
+                {
+                    "x": 2.4384933115651135,
+                    "y": 3.2467782441206525,
+                    "heading": 3.390761607070638,
+                    "angularVelocity": 0.12965729099677573,
+                    "velocityX": 1.4155985162723614,
+                    "velocityY": -3.462303680722349,
+                    "timestamp": 0.5299389957392953
+                },
+                {
+                    "x": 2.487106839227099,
+                    "y": 3.1465743963564163,
+                    "heading": 3.3907619234819757,
+                    "angularVelocity": 0.000010747282459786006,
+                    "velocityX": 1.6512155266004023,
+                    "velocityY": -3.403541302409769,
+                    "timestamp": 0.559380051058145
+                },
+                {
+                    "x": 2.5421170689241235,
+                    "y": 3.049734421677047,
+                    "heading": 3.39076189132422,
+                    "angularVelocity": -0.0000010922759119129131,
+                    "velocityX": 1.8684870192748764,
+                    "velocityY": -3.2892834047754955,
+                    "timestamp": 0.5888211063769948
+                },
+                {
+                    "x": 2.603292311228317,
+                    "y": 2.9566661889623873,
+                    "heading": 3.3907618600542158,
+                    "angularVelocity": -0.0000010621224075785883,
+                    "velocityX": 2.077888908589026,
+                    "velocityY": -3.161171761905971,
+                    "timestamp": 0.6182621616958446
+                },
+                {
+                    "x": 2.670374957735173,
+                    "y": 2.8677616065314258,
+                    "heading": 3.3907618290531647,
+                    "angularVelocity": -0.0000010529870817317445,
+                    "velocityX": 2.2785408260792437,
+                    "velocityY": -3.019748492984228,
+                    "timestamp": 0.6477032170146944
+                },
+                {
+                    "x": 2.7430825233459473,
+                    "y": 2.7833950519561768,
+                    "heading": 3.3907617949090247,
+                    "angularVelocity": -0.0000011597457911507082,
+                    "velocityX": 2.4695978056269863,
+                    "velocityY": -2.8656090504076794,
+                    "timestamp": 0.6771442723335441
+                },
+                {
+                    "x": 2.8743019890864545,
+                    "y": 2.6600421338324565,
+                    "heading": 3.3907617697237464,
+                    "angularVelocity": -5.290207073739654e-7,
+                    "velocityX": 2.7562853520145434,
+                    "velocityY": -2.5910472919087915,
+                    "timestamp": 0.7247516311451417
+                },
+                {
+                    "x": 3.0177249974626466,
+                    "y": 2.5511186165163586,
+                    "heading": 3.390761748438384,
+                    "angularVelocity": -4.4710235312140834e-7,
+                    "velocityX": 3.0126226691923277,
+                    "velocityY": -2.287955476529456,
+                    "timestamp": 0.7723589899567394
+                },
+                {
+                    "x": 3.171772290811784,
+                    "y": 2.457823792449377,
+                    "heading": 3.3907617293701717,
+                    "angularVelocity": -4.005307751758197e-7,
+                    "velocityX": 3.235787432753145,
+                    "velocityY": -1.95967233629129,
+                    "timestamp": 0.819966348768337
+                },
+                {
+                    "x": 3.3347475520740515,
+                    "y": 2.3811846546739925,
+                    "heading": 3.3907617114029063,
+                    "angularVelocity": -3.7740521255041523e-7,
+                    "velocityX": 3.4233207917967023,
+                    "velocityY": -1.6098170469543955,
+                    "timestamp": 0.8675737075799346
+                },
+                {
+                    "x": 3.5038153046646756,
+                    "y": 2.3191315453490344,
+                    "heading": 3.3907616937093077,
+                    "angularVelocity": -3.7165679655198125e-7,
+                    "velocityX": 3.551294522758467,
+                    "velocityY": -1.3034352434994065,
+                    "timestamp": 0.9151810663915322
+                },
+                {
+                    "x": 3.6728836971491297,
+                    "y": 2.257080179484392,
+                    "heading": 3.3907616760157264,
+                    "angularVelocity": -3.7165643530673955e-7,
+                    "velocityX": 3.551307963828218,
+                    "velocityY": -1.303398621843431,
+                    "timestamp": 0.9627884252031298
+                },
+                {
+                    "x": 3.8419520974106347,
+                    "y": 2.195028834809519,
+                    "heading": 3.3907616583221465,
+                    "angularVelocity": -3.71656403732769e-7,
+                    "velocityX": 3.5513081271863745,
+                    "velocityY": -1.3033981767490248,
+                    "timestamp": 1.0103957840147275
+                },
+                {
+                    "x": 4.011020504671481,
+                    "y": 2.1329775092054373,
+                    "heading": 3.390761640628565,
+                    "angularVelocity": -3.7165644168556193e-7,
+                    "velocityX": 3.5513082742086395,
+                    "velocityY": -1.3033977761640818,
+                    "timestamp": 1.0580031428263252
+                },
+                {
+                    "x": 4.180088918243229,
+                    "y": 2.070926200796389,
+                    "heading": 3.390761622934979,
+                    "angularVelocity": -3.716565361157419e-7,
+                    "velocityX": 3.55130840677009,
+                    "velocityY": -1.3033974149797176,
+                    "timestamp": 1.105610501637923
+                },
+                {
+                    "x": 4.349157337505363,
+                    "y": 2.008874907891698,
+                    "heading": 3.3907616052413863,
+                    "angularVelocity": -3.716566778801587e-7,
+                    "velocityX": 3.551308526297583,
+                    "velocityY": -1.3033970893082818,
+                    "timestamp": 1.1532178604495207
+                },
+                {
+                    "x": 4.518225761904485,
+                    "y": 1.9468236289835406,
+                    "heading": 3.390761587547785,
+                    "angularVelocity": -3.716568562531517e-7,
+                    "velocityX": 3.551308634200803,
+                    "velocityY": -1.303396795308895,
+                    "timestamp": 1.2008252192611184
+                },
+                {
+                    "x": 4.687294190885785,
+                    "y": 1.8847723625602526,
+                    "heading": 3.3907615698541753,
+                    "angularVelocity": -3.716570319734318e-7,
+                    "velocityX": 3.5513087304501405,
+                    "velocityY": -1.3033965330622725,
+                    "timestamp": 1.2484325780727161
+                },
+                {
+                    "x": 4.8563626239355635,
+                    "y": 1.8227211072221874,
+                    "heading": 3.390761552160558,
+                    "angularVelocity": -3.716571921571891e-7,
+                    "velocityX": 3.5513088159091875,
+                    "velocityY": -1.3033963002154438,
+                    "timestamp": 1.2960399368843138
+                },
+                {
+                    "x": 5.025431060602707,
+                    "y": 1.7606698617402123,
+                    "heading": 3.3907615344669337,
+                    "angularVelocity": -3.716573365191525e-7,
+                    "velocityX": 3.551308891892495,
+                    "velocityY": -1.3033960931867248,
+                    "timestamp": 1.3436472956959116
+                },
+                {
+                    "x": 5.1944995004938015,
+                    "y": 1.6986186250424111,
+                    "heading": 3.3907615167733036,
+                    "angularVelocity": -3.7165746535883894e-7,
+                    "velocityX": 3.551308959612086,
+                    "velocityY": -1.3033959086737847,
+                    "timestamp": 1.3912546545075093
+                },
+                {
+                    "x": 5.363567943266909,
+                    "y": 1.6365673961971205,
+                    "heading": 3.3907614990796673,
+                    "angularVelocity": -3.716575836762919e-7,
+                    "velocityX": 3.551309020149218,
+                    "velocityY": -1.3033957437305734,
+                    "timestamp": 1.438862013319107
+                },
+                {
+                    "x": 5.532636388625086,
+                    "y": 1.574516174395274,
+                    "heading": 3.390761481386026,
+                    "angularVelocity": -3.716576912991928e-7,
+                    "velocityX": 3.5513090744490245,
+                    "velocityY": -1.3033955957819334,
+                    "timestamp": 1.4864693721307047
+                },
+                {
+                    "x": 5.701704836310244,
+                    "y": 1.5124649589336634,
+                    "heading": 3.3907614636923804,
+                    "angularVelocity": -3.7165778948932697e-7,
+                    "velocityX": 3.551309123327601,
+                    "velocityY": -1.3033954626042876,
+                    "timestamp": 1.5340767309423025
+                },
+                {
+                    "x": 5.870773286097621,
+                    "y": 1.4504137491998943,
+                    "heading": 3.3907614459987303,
+                    "angularVelocity": -3.716578803925389e-7,
+                    "velocityX": 3.551309167485058,
+                    "velocityY": -1.3033953422900804,
+                    "timestamp": 1.5816840897539002
+                },
+                {
+                    "x": 6.039841737790961,
+                    "y": 1.388362544659232,
+                    "heading": 3.3907614283050767,
+                    "angularVelocity": -3.716579611998838e-7,
+                    "velocityX": 3.551309207520092,
+                    "velocityY": -1.3033952332080676,
+                    "timestamp": 1.629291448565498
+                },
+                {
+                    "x": 6.208910191218329,
+                    "y": 1.3263113448432136,
+                    "heading": 3.390761410611419,
+                    "angularVelocity": -3.7165803844263857e-7,
+                    "velocityX": 3.551309243943619,
+                    "velocityY": -1.303395133966182,
+                    "timestamp": 1.6768988073770956
+                },
+                {
+                    "x": 6.377978646228594,
+                    "y": 1.2642601493400643,
+                    "heading": 3.3907613929177582,
+                    "angularVelocity": -3.7165810831214996e-7,
+                    "velocityX": 3.5513092771926584,
+                    "velocityY": -1.3033950433736952,
+                    "timestamp": 1.7245061661886933
+                },
+                {
+                    "x": 6.547047102688449,
+                    "y": 1.2022089577865573,
+                    "heading": 3.390761375224094,
+                    "angularVelocity": -3.716581745376039e-7,
+                    "velocityX": 3.551309307641484,
+                    "velocityY": -1.3033949604108395,
+                    "timestamp": 1.772113525000291
+                },
+                {
+                    "x": 6.716115560480058,
+                    "y": 1.1401577698616354,
+                    "heading": 3.3907613575304274,
+                    "angularVelocity": -3.716582357126045e-7,
+                    "velocityX": 3.5513093356151892,
+                    "velocityY": -1.3033948841918472,
+                    "timestamp": 1.8197208838118888
+                },
+                {
+                    "x": 6.885184026346865,
+                    "y": 1.0781066039393965,
+                    "heading": 3.3907613398363963,
+                    "angularVelocity": -3.7166587620925834e-7,
+                    "velocityX": 3.5513095052359795,
+                    "velocityY": -1.3033944220220548,
+                    "timestamp": 1.8673282426234865
+                },
+                {
+                    "x": 7.044084736711506,
+                    "y": 1.0199905851581765,
+                    "heading": 3.36350523152736,
+                    "angularVelocity": -0.5725188077939848,
+                    "velocityX": 3.337734214441043,
+                    "velocityY": -1.220736042325084,
+                    "timestamp": 1.9149356014350842
+                },
+                {
+                    "x": 7.185551132214723,
+                    "y": 0.9681799151008591,
+                    "heading": 3.323163851941908,
+                    "angularVelocity": -0.8473769726461788,
+                    "velocityX": 2.9715237105057524,
+                    "velocityY": -1.0882912085577785,
+                    "timestamp": 1.962542960246682
+                },
+                {
+                    "x": 7.3092683658418975,
+                    "y": 0.9228556276324137,
+                    "heading": 3.282206369227669,
+                    "angularVelocity": -0.8603183149967459,
+                    "velocityX": 2.598699795902896,
+                    "velocityY": -0.952043730210126,
+                    "timestamp": 2.0101503190582797
+                },
+                {
+                    "x": 7.415256126479253,
+                    "y": 0.8840204066687376,
+                    "heading": 3.244221221054594,
+                    "angularVelocity": -0.7978839641870918,
+                    "velocityX": 2.226289449427224,
+                    "velocityY": -0.815739875790283,
+                    "timestamp": 2.057757677869877
+                },
+                {
+                    "x": 7.503541109453659,
+                    "y": 0.8516687069454374,
+                    "heading": 3.210907204397792,
+                    "angularVelocity": -0.6997661178524867,
+                    "velocityX": 1.8544398424576745,
+                    "velocityY": -0.6795525005142472,
+                    "timestamp": 2.1053650366814747
+                },
+                {
+                    "x": 7.574144027018988,
+                    "y": 0.8257950136289538,
+                    "heading": 3.1832598866385293,
+                    "angularVelocity": -0.5807362233362916,
+                    "velocityX": 1.4830252996124917,
+                    "velocityY": -0.5434809651776052,
+                    "timestamp": 2.152972395493072
+                },
+                {
+                    "x": 7.627080539080189,
+                    "y": 0.8063947169930168,
+                    "heading": 3.161935751906697,
+                    "angularVelocity": -0.44791677724069034,
+                    "velocityX": 1.1119396955141558,
+                    "velocityY": -0.407506257860514,
+                    "timestamp": 2.2005797543046697
+                },
+                {
+                    "x": 7.662362703782539,
+                    "y": 0.7934640657363813,
+                    "heading": 3.147400394630243,
+                    "angularVelocity": -0.3053174475395025,
+                    "velocityX": 0.7411073746387762,
+                    "velocityY": -0.27161034721139543,
+                    "timestamp": 2.248187113116267
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": -0.1554464438896817,
+                    "velocityX": 0.370474158989983,
+                    "velocityY": -0.13577870937899197,
+                    "timestamp": 2.2957944719278647
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 3.14,
+                    "angularVelocity": 1.845639156363882e-34,
+                    "velocityX": 1.6232927575664752e-34,
+                    "velocityY": 1.3237538522972492e-34,
+                    "timestamp": 2.343401830739462
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -7838,16 +22375,6 @@
                     "x": 3.42,
                     "y": 4.04,
                     "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
                 }
             ]
         },
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.1.traj
new file mode 100644
index 00000000..38312c6d
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.1.traj	
@@ -0,0 +1,526 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": -2.1,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.27986083101055e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7292945467072102,
+            "y": 6.711630943578302,
+            "heading": -2.0466837783195437,
+            "angularVelocity": 0.9750423416679173,
+            "velocityX": 0.3528569619125016,
+            "velocityY": -0.1530525628742106,
+            "timestamp": 0.05468092963968429
+        },
+        {
+            "x": 0.7686895561493499,
+            "y": 6.695016697545084,
+            "heading": -1.9453579324448191,
+            "angularVelocity": 1.853038098335252,
+            "velocityX": 0.720452444787058,
+            "velocityY": -0.30383986049058775,
+            "timestamp": 0.10936185927936858
+        },
+        {
+            "x": 0.8291498926090706,
+            "y": 6.670065411203325,
+            "heading": -1.8043277140562832,
+            "angularVelocity": 2.579148147587168,
+            "velocityX": 1.1056932802371746,
+            "velocityY": -0.4563069155219838,
+            "timestamp": 0.16404278891905286
+        },
+        {
+            "x": 0.9116442238767206,
+            "y": 6.636292399159329,
+            "heading": -1.6353628554936872,
+            "angularVelocity": 3.09001437385898,
+            "velocityX": 1.50864902647486,
+            "velocityY": -0.6176378541210026,
+            "timestamp": 0.21872371855873715
+        },
+        {
+            "x": 1.0169609100629697,
+            "y": 6.593200359597327,
+            "heading": -1.4531033706452814,
+            "angularVelocity": 3.333145322316033,
+            "velocityX": 1.926022232617937,
+            "velocityY": -0.7880634042974968,
+            "timestamp": 0.2734046481984214
+        },
+        {
+            "x": 1.1451022790503944,
+            "y": 6.540887351226898,
+            "heading": -1.2848862020086818,
+            "angularVelocity": 3.0763406866901017,
+            "velocityX": 2.3434380108714725,
+            "velocityY": -0.9566956654750047,
+            "timestamp": 0.3280855778381057
+        },
+        {
+            "x": 1.291419394840941,
+            "y": 6.479714636695398,
+            "heading": -1.166754837939456,
+            "angularVelocity": 2.1603759271768683,
+            "velocityX": 2.6758344591924805,
+            "velocityY": -1.1187211873425027,
+            "timestamp": 0.38276650747779
+        },
+        {
+            "x": 1.45556402161766,
+            "y": 6.408096144298377,
+            "heading": -1.0985152891132095,
+            "angularVelocity": 1.24795882725304,
+            "velocityX": 3.0018624017246447,
+            "velocityY": -1.3097526481159927,
+            "timestamp": 0.4374474371174743
+        },
+        {
+            "x": 1.6376170855458871,
+            "y": 6.326295801964295,
+            "heading": -1.0805867815133028,
+            "angularVelocity": 0.3278749596622653,
+            "velocityX": 3.32937031480356,
+            "velocityY": -1.495957418301032,
+            "timestamp": 0.4921283667571586
+        },
+        {
+            "x": 1.828511449146643,
+            "y": 6.246620230558847,
+            "heading": -1.0805866805891169,
+            "angularVelocity": 0.000001845692576742716,
+            "velocityX": 3.491059220438268,
+            "velocityY": -1.4570997956776681,
+            "timestamp": 0.5468092963968428
+        },
+        {
+            "x": 2.0194058587075467,
+            "y": 6.16694476926885,
+            "heading": -1.0805865796652752,
+            "angularVelocity": 0.0000018456862813728727,
+            "velocityX": 3.491060060953374,
+            "velocityY": -1.4570977818960094,
+            "timestamp": 0.6014902260365271
+        },
+        {
+            "x": 2.2103002634541666,
+            "y": 6.087269296444259,
+            "heading": -1.0805864787414845,
+            "angularVelocity": 0.000001845685351855892,
+            "velocityX": 3.491059972910188,
+            "velocityY": -1.4570979928396652,
+            "timestamp": 0.6561711556762114
+        },
+        {
+            "x": 2.4011946628107936,
+            "y": 6.007593810705732,
+            "heading": -1.0805863778177442,
+            "angularVelocity": 0.000001845684424329729,
+            "velocityX": 3.4910598743384673,
+            "velocityY": -1.457098229008597,
+            "timestamp": 0.7108520853158957
+        },
+        {
+            "x": 2.5920890560965946,
+            "y": 5.927918310422065,
+            "heading": -1.0805862768940542,
+            "angularVelocity": 0.0000018456835100189827,
+            "velocityX": 3.4910597633157407,
+            "velocityY": -1.45709849500883,
+            "timestamp": 0.76553301495558
+        },
+        {
+            "x": 2.7829834425031943,
+            "y": 5.8482427936564765,
+            "heading": -1.0805861759704127,
+            "angularVelocity": 0.0000018456826171919115,
+            "velocityX": 3.4910596375095175,
+            "velocityY": -1.4570987964287467,
+            "timestamp": 0.8202139445952643
+        },
+        {
+            "x": 2.9738778210651113,
+            "y": 5.768567258095778,
+            "heading": -1.080586075046818,
+            "angularVelocity": 0.0000018456817639254375,
+            "velocityX": 3.4910594940466586,
+            "velocityY": -1.4570991401520532,
+            "timestamp": 0.8748948742349486
+        },
+        {
+            "x": 3.164772190605788,
+            "y": 5.688891700921064,
+            "heading": -1.0805859741232662,
+            "angularVelocity": 0.0000018456809789441398,
+            "velocityX": 3.491059329067008,
+            "velocityY": -1.4570995354272234,
+            "timestamp": 0.9295758038746329
+        },
+        {
+            "x": 3.3556665493271205,
+            "y": 5.609216117824277,
+            "heading": -1.0805858731997562,
+            "angularVelocity": 0.000001845680216876217,
+            "velocityX": 3.491059131203806,
+            "velocityY": -1.4571000094878162,
+            "timestamp": 0.9842567335143172
+        },
+        {
+            "x": 3.5465606286609406,
+            "y": 5.529539865347568,
+            "heading": -1.0805857722761907,
+            "angularVelocity": 0.0000018456812299162406,
+            "velocityX": 3.491054021789706,
+            "velocityY": -1.4571122510486052,
+            "timestamp": 1.0389376631540015
+        },
+        {
+            "x": 3.7302882443981766,
+            "y": 5.43449767788976,
+            "heading": -1.0805856670205962,
+            "angularVelocity": 0.0000019249049955796687,
+            "velocityX": 3.3599943700279993,
+            "velocityY": -1.7381231095389325,
+            "timestamp": 1.0936185927936857
+        },
+        {
+            "x": 3.901247591247985,
+            "y": 5.318042735383766,
+            "heading": -1.0805855451412456,
+            "angularVelocity": 0.000002228918769474178,
+            "velocityX": 3.1264893990707807,
+            "velocityY": -2.12971767805275,
+            "timestamp": 1.1482995224333699
+        },
+        {
+            "x": 4.056955156553094,
+            "y": 5.181866377235877,
+            "heading": -1.0805853830111598,
+            "angularVelocity": 0.0000029650206572180867,
+            "velocityX": 2.8475661685185734,
+            "velocityY": -2.490381181249336,
+            "timestamp": 1.202980452073054
+        },
+        {
+            "x": 4.195149083685748,
+            "y": 5.0279466523676755,
+            "heading": -1.080585195813012,
+            "angularVelocity": 0.0000034234631562085443,
+            "velocityX": 2.527278304214503,
+            "velocityY": -2.81487030089728,
+            "timestamp": 1.2576613817127382
+        },
+        {
+            "x": 4.313735776578777,
+            "y": 4.858628961688981,
+            "heading": -1.081017907841774,
+            "angularVelocity": -0.007913399271258946,
+            "velocityX": 2.168702940393416,
+            "velocityY": -3.0964669363597066,
+            "timestamp": 1.3123423113524224
+        },
+        {
+            "x": 4.409971486167556,
+            "y": 4.678531583119963,
+            "heading": -1.0900704798968306,
+            "angularVelocity": -0.16555263626108413,
+            "velocityX": 1.759950136600006,
+            "velocityY": -3.293604914103617,
+            "timestamp": 1.3670232409921066
+        },
+        {
+            "x": 4.510748059482075,
+            "y": 4.519168046099015,
+            "heading": -1.1448684085004224,
+            "angularVelocity": -1.0021396666932787,
+            "velocityX": 1.8429930503848142,
+            "velocityY": -2.9144262555714096,
+            "timestamp": 1.4217041706317908
+        },
+        {
+            "x": 4.62613024170022,
+            "y": 4.379607058549792,
+            "heading": -1.2139690573229496,
+            "angularVelocity": -1.2637065477463625,
+            "velocityX": 2.110099132886858,
+            "velocityY": -2.552278984078161,
+            "timestamp": 1.476385100271475
+        },
+        {
+            "x": 4.756158352198169,
+            "y": 4.26013085900244,
+            "heading": -1.289886392340967,
+            "angularVelocity": -1.3883695013648907,
+            "velocityX": 2.3779425725707126,
+            "velocityY": -2.1849701593340063,
+            "timestamp": 1.5310660299111591
+        },
+        {
+            "x": 4.900439739227295,
+            "y": 4.161101341247559,
+            "heading": -1.3678659526433012,
+            "angularVelocity": -1.4260832947825601,
+            "velocityX": 2.638605231839619,
+            "velocityY": -1.8110430529880912,
+            "timestamp": 1.5857469595508433
+        },
+        {
+            "x": 5.00826704552834,
+            "y": 4.101917336684186,
+            "heading": -1.4217535551168057,
+            "angularVelocity": -1.4073230082403518,
+            "velocityX": 2.816006690010969,
+            "velocityY": -1.5456432930522144,
+            "timestamp": 1.6240378154225759
+        },
+        {
+            "x": 5.122657163782486,
+            "y": 4.053031768762854,
+            "heading": -1.4735948904430307,
+            "angularVelocity": -1.353882908752006,
+            "velocityX": 2.9874004027836745,
+            "velocityY": -1.2766903953541875,
+            "timestamp": 1.6623286712943084
+        },
+        {
+            "x": 5.24335031560154,
+            "y": 4.014573699061021,
+            "heading": -1.5218759194168912,
+            "angularVelocity": -1.2609023192271587,
+            "velocityX": 3.152009770253094,
+            "velocityY": -1.0043669389543197,
+            "timestamp": 1.700619527166041
+        },
+        {
+            "x": 5.370053209601022,
+            "y": 3.986652038058385,
+            "heading": -1.5647151394478649,
+            "angularVelocity": -1.1187846042009912,
+            "velocityX": 3.308959570502,
+            "velocityY": -0.729199187820931,
+            "timestamp": 1.7389103830377735
+        },
+        {
+            "x": 5.5024723935607405,
+            "y": 3.969300942477628,
+            "heading": -1.599514624372743,
+            "angularVelocity": -0.9088197203387187,
+            "velocityX": 3.458245603161718,
+            "velocityY": -0.4531394032789495,
+            "timestamp": 1.777201238909506
+        },
+        {
+            "x": 5.64052782398134,
+            "y": 3.9622378493580968,
+            "heading": -1.6219128460749601,
+            "angularVelocity": -0.5849496228876974,
+            "velocityX": 3.6054412281370607,
+            "velocityY": -0.1844590035592553,
+            "timestamp": 1.8154920947812385
+        },
+        {
+            "x": 5.78519636080909,
+            "y": 3.9633730625174985,
+            "heading": -1.6224984270976561,
+            "angularVelocity": -0.015292972940008887,
+            "velocityX": 3.778148425628394,
+            "velocityY": 0.02964710852127253,
+            "timestamp": 1.853782950652971
+        },
+        {
+            "x": 5.929431807514691,
+            "y": 3.9767231173381155,
+            "heading": -1.6224986694398071,
+            "angularVelocity": -0.000006328982347971965,
+            "velocityX": 3.7668378891493703,
+            "velocityY": 0.3486486399086302,
+            "timestamp": 1.8920738065247036
+        },
+        {
+            "x": 6.073330473121851,
+            "y": 3.9933141696465464,
+            "heading": -1.6224988987872904,
+            "angularVelocity": -0.000005989614964631434,
+            "velocityX": 3.758042549093057,
+            "velocityY": 0.4332901924158613,
+            "timestamp": 1.9303646623964361
+        },
+        {
+            "x": 6.217229120638352,
+            "y": 4.009905378860974,
+            "heading": -1.6224991281340264,
+            "angularVelocity": -0.000005989595448400927,
+            "velocityX": 3.7580420766392404,
+            "velocityY": 0.4332942901565158,
+            "timestamp": 1.9686555182681686
+        },
+        {
+            "x": 6.361127770496839,
+            "y": 4.0264965677626225,
+            "heading": -1.6224993574808346,
+            "angularVelocity": -0.000005989597330541617,
+            "velocityX": 3.7580421378023496,
+            "velocityY": 0.4332937596700765,
+            "timestamp": 2.0069463741399014
+        },
+        {
+            "x": 6.505026421139362,
+            "y": 4.0430877498636315,
+            "heading": -1.622499586827807,
+            "angularVelocity": -0.000005989601616416828,
+            "velocityX": 3.7580421582781125,
+            "velocityY": 0.43329358206529056,
+            "timestamp": 2.045237230011634
+        },
+        {
+            "x": 6.648925182384827,
+            "y": 4.0596779726347245,
+            "heading": -1.6224998161827902,
+            "angularVelocity": -0.000005989810829077631,
+            "velocityX": 3.7580450467730504,
+            "velocityY": 0.43326852830525053,
+            "timestamp": 2.0835280858833665
+        },
+        {
+            "x": 6.790105794643628,
+            "y": 4.068502844708353,
+            "heading": -1.6329532702813698,
+            "angularVelocity": -0.27300131743194306,
+            "velocityX": 3.6870581512131766,
+            "velocityY": 0.2304694390532754,
+            "timestamp": 2.121818941755099
+        },
+        {
+            "x": 6.921555592389516,
+            "y": 4.075747346171521,
+            "heading": -1.6719954127909697,
+            "angularVelocity": -1.0196205235104698,
+            "velocityX": 3.4329292138629897,
+            "velocityY": 0.18919664494924698,
+            "timestamp": 2.1601097976268315
+        },
+        {
+            "x": 7.042894308610419,
+            "y": 4.08202615388359,
+            "heading": -1.737655401763884,
+            "angularVelocity": -1.714769426749386,
+            "velocityX": 3.168869262869602,
+            "velocityY": 0.16397668762232157,
+            "timestamp": 2.198400653498564
+        },
+        {
+            "x": 7.153820883415079,
+            "y": 4.087877633041295,
+            "heading": -1.8279052524624368,
+            "angularVelocity": -2.3569556920031554,
+            "velocityX": 2.8969468631425612,
+            "velocityY": 0.15281661964688553,
+            "timestamp": 2.2366915093702966
+        },
+        {
+            "x": 7.254173862078464,
+            "y": 4.093777241980607,
+            "heading": -1.9412919882105817,
+            "angularVelocity": -2.96119616986284,
+            "velocityX": 2.6208079286488037,
+            "velocityY": 0.15407357200568295,
+            "timestamp": 2.274982365242029
+        },
+        {
+            "x": 7.343968095562701,
+            "y": 4.100172778216793,
+            "heading": -2.0772958326900723,
+            "angularVelocity": -3.5518622235835817,
+            "velocityX": 2.3450568403336534,
+            "velocityY": 0.16702515758877745,
+            "timestamp": 2.3132732211137617
+        },
+        {
+            "x": 7.423449152791422,
+            "y": 4.107548146969254,
+            "heading": -2.2364797071963527,
+            "angularVelocity": -4.15722947116975,
+            "velocityX": 2.0757190044267455,
+            "velocityY": 0.19261436143311772,
+            "timestamp": 2.351564076985494
+        },
+        {
+            "x": 7.492711517186474,
+            "y": 4.113210958649556,
+            "heading": -2.4189397780167137,
+            "angularVelocity": -4.765108187489175,
+            "velocityX": 1.8088486877145706,
+            "velocityY": 0.147889399476235,
+            "timestamp": 2.3898549328572267
+        },
+        {
+            "x": 7.549783547522053,
+            "y": 4.117992264935679,
+            "heading": -2.6004162007304275,
+            "angularVelocity": -4.739419336084478,
+            "velocityX": 1.490487194299341,
+            "velocityY": 0.12486809650168224,
+            "timestamp": 2.4281457887289593
+        },
+        {
+            "x": 7.595268726062492,
+            "y": 4.121696461876066,
+            "heading": -2.768130415473744,
+            "angularVelocity": -4.380006947484507,
+            "velocityX": 1.1878861807843983,
+            "velocityY": 0.09673842112058428,
+            "timestamp": 2.466436644600692
+        },
+        {
+            "x": 7.630218683798498,
+            "y": 4.124204213311165,
+            "heading": -2.9120023323717077,
+            "angularVelocity": -3.7573439825922996,
+            "velocityX": 0.9127494525868564,
+            "velocityY": 0.06549217503781085,
+            "timestamp": 2.5047275004724243
+        },
+        {
+            "x": 7.655568399114973,
+            "y": 4.125734158703857,
+            "heading": -3.0250833006842397,
+            "angularVelocity": -2.9532107793916214,
+            "velocityX": 0.6620305224148624,
+            "velocityY": 0.03995589437375436,
+            "timestamp": 2.543018356344157
+        },
+        {
+            "x": 7.671992151604379,
+            "y": 4.126594890877869,
+            "heading": -3.103103884407084,
+            "angularVelocity": -2.037577430606375,
+            "velocityX": 0.42892100778377035,
+            "velocityY": 0.022478791722372646,
+            "timestamp": 2.5813092122158894
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -3.143185307179586,
+            "angularVelocity": -1.0467622585080842,
+            "velocityX": 0.20913213385579937,
+            "velocityY": 0.010579787599633836,
+            "timestamp": 2.619600068087622
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -3.143185307179586,
+            "angularVelocity": 7.101458819208695e-33,
+            "velocityX": -7.030931331746161e-34,
+            "velocityY": 2.5921613146305674e-34,
+            "timestamp": 2.6578909239593544
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.traj
new file mode 100644
index 00000000..38312c6d
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.traj	
@@ -0,0 +1,526 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": -2.1,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.27986083101055e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7292945467072102,
+            "y": 6.711630943578302,
+            "heading": -2.0466837783195437,
+            "angularVelocity": 0.9750423416679173,
+            "velocityX": 0.3528569619125016,
+            "velocityY": -0.1530525628742106,
+            "timestamp": 0.05468092963968429
+        },
+        {
+            "x": 0.7686895561493499,
+            "y": 6.695016697545084,
+            "heading": -1.9453579324448191,
+            "angularVelocity": 1.853038098335252,
+            "velocityX": 0.720452444787058,
+            "velocityY": -0.30383986049058775,
+            "timestamp": 0.10936185927936858
+        },
+        {
+            "x": 0.8291498926090706,
+            "y": 6.670065411203325,
+            "heading": -1.8043277140562832,
+            "angularVelocity": 2.579148147587168,
+            "velocityX": 1.1056932802371746,
+            "velocityY": -0.4563069155219838,
+            "timestamp": 0.16404278891905286
+        },
+        {
+            "x": 0.9116442238767206,
+            "y": 6.636292399159329,
+            "heading": -1.6353628554936872,
+            "angularVelocity": 3.09001437385898,
+            "velocityX": 1.50864902647486,
+            "velocityY": -0.6176378541210026,
+            "timestamp": 0.21872371855873715
+        },
+        {
+            "x": 1.0169609100629697,
+            "y": 6.593200359597327,
+            "heading": -1.4531033706452814,
+            "angularVelocity": 3.333145322316033,
+            "velocityX": 1.926022232617937,
+            "velocityY": -0.7880634042974968,
+            "timestamp": 0.2734046481984214
+        },
+        {
+            "x": 1.1451022790503944,
+            "y": 6.540887351226898,
+            "heading": -1.2848862020086818,
+            "angularVelocity": 3.0763406866901017,
+            "velocityX": 2.3434380108714725,
+            "velocityY": -0.9566956654750047,
+            "timestamp": 0.3280855778381057
+        },
+        {
+            "x": 1.291419394840941,
+            "y": 6.479714636695398,
+            "heading": -1.166754837939456,
+            "angularVelocity": 2.1603759271768683,
+            "velocityX": 2.6758344591924805,
+            "velocityY": -1.1187211873425027,
+            "timestamp": 0.38276650747779
+        },
+        {
+            "x": 1.45556402161766,
+            "y": 6.408096144298377,
+            "heading": -1.0985152891132095,
+            "angularVelocity": 1.24795882725304,
+            "velocityX": 3.0018624017246447,
+            "velocityY": -1.3097526481159927,
+            "timestamp": 0.4374474371174743
+        },
+        {
+            "x": 1.6376170855458871,
+            "y": 6.326295801964295,
+            "heading": -1.0805867815133028,
+            "angularVelocity": 0.3278749596622653,
+            "velocityX": 3.32937031480356,
+            "velocityY": -1.495957418301032,
+            "timestamp": 0.4921283667571586
+        },
+        {
+            "x": 1.828511449146643,
+            "y": 6.246620230558847,
+            "heading": -1.0805866805891169,
+            "angularVelocity": 0.000001845692576742716,
+            "velocityX": 3.491059220438268,
+            "velocityY": -1.4570997956776681,
+            "timestamp": 0.5468092963968428
+        },
+        {
+            "x": 2.0194058587075467,
+            "y": 6.16694476926885,
+            "heading": -1.0805865796652752,
+            "angularVelocity": 0.0000018456862813728727,
+            "velocityX": 3.491060060953374,
+            "velocityY": -1.4570977818960094,
+            "timestamp": 0.6014902260365271
+        },
+        {
+            "x": 2.2103002634541666,
+            "y": 6.087269296444259,
+            "heading": -1.0805864787414845,
+            "angularVelocity": 0.000001845685351855892,
+            "velocityX": 3.491059972910188,
+            "velocityY": -1.4570979928396652,
+            "timestamp": 0.6561711556762114
+        },
+        {
+            "x": 2.4011946628107936,
+            "y": 6.007593810705732,
+            "heading": -1.0805863778177442,
+            "angularVelocity": 0.000001845684424329729,
+            "velocityX": 3.4910598743384673,
+            "velocityY": -1.457098229008597,
+            "timestamp": 0.7108520853158957
+        },
+        {
+            "x": 2.5920890560965946,
+            "y": 5.927918310422065,
+            "heading": -1.0805862768940542,
+            "angularVelocity": 0.0000018456835100189827,
+            "velocityX": 3.4910597633157407,
+            "velocityY": -1.45709849500883,
+            "timestamp": 0.76553301495558
+        },
+        {
+            "x": 2.7829834425031943,
+            "y": 5.8482427936564765,
+            "heading": -1.0805861759704127,
+            "angularVelocity": 0.0000018456826171919115,
+            "velocityX": 3.4910596375095175,
+            "velocityY": -1.4570987964287467,
+            "timestamp": 0.8202139445952643
+        },
+        {
+            "x": 2.9738778210651113,
+            "y": 5.768567258095778,
+            "heading": -1.080586075046818,
+            "angularVelocity": 0.0000018456817639254375,
+            "velocityX": 3.4910594940466586,
+            "velocityY": -1.4570991401520532,
+            "timestamp": 0.8748948742349486
+        },
+        {
+            "x": 3.164772190605788,
+            "y": 5.688891700921064,
+            "heading": -1.0805859741232662,
+            "angularVelocity": 0.0000018456809789441398,
+            "velocityX": 3.491059329067008,
+            "velocityY": -1.4570995354272234,
+            "timestamp": 0.9295758038746329
+        },
+        {
+            "x": 3.3556665493271205,
+            "y": 5.609216117824277,
+            "heading": -1.0805858731997562,
+            "angularVelocity": 0.000001845680216876217,
+            "velocityX": 3.491059131203806,
+            "velocityY": -1.4571000094878162,
+            "timestamp": 0.9842567335143172
+        },
+        {
+            "x": 3.5465606286609406,
+            "y": 5.529539865347568,
+            "heading": -1.0805857722761907,
+            "angularVelocity": 0.0000018456812299162406,
+            "velocityX": 3.491054021789706,
+            "velocityY": -1.4571122510486052,
+            "timestamp": 1.0389376631540015
+        },
+        {
+            "x": 3.7302882443981766,
+            "y": 5.43449767788976,
+            "heading": -1.0805856670205962,
+            "angularVelocity": 0.0000019249049955796687,
+            "velocityX": 3.3599943700279993,
+            "velocityY": -1.7381231095389325,
+            "timestamp": 1.0936185927936857
+        },
+        {
+            "x": 3.901247591247985,
+            "y": 5.318042735383766,
+            "heading": -1.0805855451412456,
+            "angularVelocity": 0.000002228918769474178,
+            "velocityX": 3.1264893990707807,
+            "velocityY": -2.12971767805275,
+            "timestamp": 1.1482995224333699
+        },
+        {
+            "x": 4.056955156553094,
+            "y": 5.181866377235877,
+            "heading": -1.0805853830111598,
+            "angularVelocity": 0.0000029650206572180867,
+            "velocityX": 2.8475661685185734,
+            "velocityY": -2.490381181249336,
+            "timestamp": 1.202980452073054
+        },
+        {
+            "x": 4.195149083685748,
+            "y": 5.0279466523676755,
+            "heading": -1.080585195813012,
+            "angularVelocity": 0.0000034234631562085443,
+            "velocityX": 2.527278304214503,
+            "velocityY": -2.81487030089728,
+            "timestamp": 1.2576613817127382
+        },
+        {
+            "x": 4.313735776578777,
+            "y": 4.858628961688981,
+            "heading": -1.081017907841774,
+            "angularVelocity": -0.007913399271258946,
+            "velocityX": 2.168702940393416,
+            "velocityY": -3.0964669363597066,
+            "timestamp": 1.3123423113524224
+        },
+        {
+            "x": 4.409971486167556,
+            "y": 4.678531583119963,
+            "heading": -1.0900704798968306,
+            "angularVelocity": -0.16555263626108413,
+            "velocityX": 1.759950136600006,
+            "velocityY": -3.293604914103617,
+            "timestamp": 1.3670232409921066
+        },
+        {
+            "x": 4.510748059482075,
+            "y": 4.519168046099015,
+            "heading": -1.1448684085004224,
+            "angularVelocity": -1.0021396666932787,
+            "velocityX": 1.8429930503848142,
+            "velocityY": -2.9144262555714096,
+            "timestamp": 1.4217041706317908
+        },
+        {
+            "x": 4.62613024170022,
+            "y": 4.379607058549792,
+            "heading": -1.2139690573229496,
+            "angularVelocity": -1.2637065477463625,
+            "velocityX": 2.110099132886858,
+            "velocityY": -2.552278984078161,
+            "timestamp": 1.476385100271475
+        },
+        {
+            "x": 4.756158352198169,
+            "y": 4.26013085900244,
+            "heading": -1.289886392340967,
+            "angularVelocity": -1.3883695013648907,
+            "velocityX": 2.3779425725707126,
+            "velocityY": -2.1849701593340063,
+            "timestamp": 1.5310660299111591
+        },
+        {
+            "x": 4.900439739227295,
+            "y": 4.161101341247559,
+            "heading": -1.3678659526433012,
+            "angularVelocity": -1.4260832947825601,
+            "velocityX": 2.638605231839619,
+            "velocityY": -1.8110430529880912,
+            "timestamp": 1.5857469595508433
+        },
+        {
+            "x": 5.00826704552834,
+            "y": 4.101917336684186,
+            "heading": -1.4217535551168057,
+            "angularVelocity": -1.4073230082403518,
+            "velocityX": 2.816006690010969,
+            "velocityY": -1.5456432930522144,
+            "timestamp": 1.6240378154225759
+        },
+        {
+            "x": 5.122657163782486,
+            "y": 4.053031768762854,
+            "heading": -1.4735948904430307,
+            "angularVelocity": -1.353882908752006,
+            "velocityX": 2.9874004027836745,
+            "velocityY": -1.2766903953541875,
+            "timestamp": 1.6623286712943084
+        },
+        {
+            "x": 5.24335031560154,
+            "y": 4.014573699061021,
+            "heading": -1.5218759194168912,
+            "angularVelocity": -1.2609023192271587,
+            "velocityX": 3.152009770253094,
+            "velocityY": -1.0043669389543197,
+            "timestamp": 1.700619527166041
+        },
+        {
+            "x": 5.370053209601022,
+            "y": 3.986652038058385,
+            "heading": -1.5647151394478649,
+            "angularVelocity": -1.1187846042009912,
+            "velocityX": 3.308959570502,
+            "velocityY": -0.729199187820931,
+            "timestamp": 1.7389103830377735
+        },
+        {
+            "x": 5.5024723935607405,
+            "y": 3.969300942477628,
+            "heading": -1.599514624372743,
+            "angularVelocity": -0.9088197203387187,
+            "velocityX": 3.458245603161718,
+            "velocityY": -0.4531394032789495,
+            "timestamp": 1.777201238909506
+        },
+        {
+            "x": 5.64052782398134,
+            "y": 3.9622378493580968,
+            "heading": -1.6219128460749601,
+            "angularVelocity": -0.5849496228876974,
+            "velocityX": 3.6054412281370607,
+            "velocityY": -0.1844590035592553,
+            "timestamp": 1.8154920947812385
+        },
+        {
+            "x": 5.78519636080909,
+            "y": 3.9633730625174985,
+            "heading": -1.6224984270976561,
+            "angularVelocity": -0.015292972940008887,
+            "velocityX": 3.778148425628394,
+            "velocityY": 0.02964710852127253,
+            "timestamp": 1.853782950652971
+        },
+        {
+            "x": 5.929431807514691,
+            "y": 3.9767231173381155,
+            "heading": -1.6224986694398071,
+            "angularVelocity": -0.000006328982347971965,
+            "velocityX": 3.7668378891493703,
+            "velocityY": 0.3486486399086302,
+            "timestamp": 1.8920738065247036
+        },
+        {
+            "x": 6.073330473121851,
+            "y": 3.9933141696465464,
+            "heading": -1.6224988987872904,
+            "angularVelocity": -0.000005989614964631434,
+            "velocityX": 3.758042549093057,
+            "velocityY": 0.4332901924158613,
+            "timestamp": 1.9303646623964361
+        },
+        {
+            "x": 6.217229120638352,
+            "y": 4.009905378860974,
+            "heading": -1.6224991281340264,
+            "angularVelocity": -0.000005989595448400927,
+            "velocityX": 3.7580420766392404,
+            "velocityY": 0.4332942901565158,
+            "timestamp": 1.9686555182681686
+        },
+        {
+            "x": 6.361127770496839,
+            "y": 4.0264965677626225,
+            "heading": -1.6224993574808346,
+            "angularVelocity": -0.000005989597330541617,
+            "velocityX": 3.7580421378023496,
+            "velocityY": 0.4332937596700765,
+            "timestamp": 2.0069463741399014
+        },
+        {
+            "x": 6.505026421139362,
+            "y": 4.0430877498636315,
+            "heading": -1.622499586827807,
+            "angularVelocity": -0.000005989601616416828,
+            "velocityX": 3.7580421582781125,
+            "velocityY": 0.43329358206529056,
+            "timestamp": 2.045237230011634
+        },
+        {
+            "x": 6.648925182384827,
+            "y": 4.0596779726347245,
+            "heading": -1.6224998161827902,
+            "angularVelocity": -0.000005989810829077631,
+            "velocityX": 3.7580450467730504,
+            "velocityY": 0.43326852830525053,
+            "timestamp": 2.0835280858833665
+        },
+        {
+            "x": 6.790105794643628,
+            "y": 4.068502844708353,
+            "heading": -1.6329532702813698,
+            "angularVelocity": -0.27300131743194306,
+            "velocityX": 3.6870581512131766,
+            "velocityY": 0.2304694390532754,
+            "timestamp": 2.121818941755099
+        },
+        {
+            "x": 6.921555592389516,
+            "y": 4.075747346171521,
+            "heading": -1.6719954127909697,
+            "angularVelocity": -1.0196205235104698,
+            "velocityX": 3.4329292138629897,
+            "velocityY": 0.18919664494924698,
+            "timestamp": 2.1601097976268315
+        },
+        {
+            "x": 7.042894308610419,
+            "y": 4.08202615388359,
+            "heading": -1.737655401763884,
+            "angularVelocity": -1.714769426749386,
+            "velocityX": 3.168869262869602,
+            "velocityY": 0.16397668762232157,
+            "timestamp": 2.198400653498564
+        },
+        {
+            "x": 7.153820883415079,
+            "y": 4.087877633041295,
+            "heading": -1.8279052524624368,
+            "angularVelocity": -2.3569556920031554,
+            "velocityX": 2.8969468631425612,
+            "velocityY": 0.15281661964688553,
+            "timestamp": 2.2366915093702966
+        },
+        {
+            "x": 7.254173862078464,
+            "y": 4.093777241980607,
+            "heading": -1.9412919882105817,
+            "angularVelocity": -2.96119616986284,
+            "velocityX": 2.6208079286488037,
+            "velocityY": 0.15407357200568295,
+            "timestamp": 2.274982365242029
+        },
+        {
+            "x": 7.343968095562701,
+            "y": 4.100172778216793,
+            "heading": -2.0772958326900723,
+            "angularVelocity": -3.5518622235835817,
+            "velocityX": 2.3450568403336534,
+            "velocityY": 0.16702515758877745,
+            "timestamp": 2.3132732211137617
+        },
+        {
+            "x": 7.423449152791422,
+            "y": 4.107548146969254,
+            "heading": -2.2364797071963527,
+            "angularVelocity": -4.15722947116975,
+            "velocityX": 2.0757190044267455,
+            "velocityY": 0.19261436143311772,
+            "timestamp": 2.351564076985494
+        },
+        {
+            "x": 7.492711517186474,
+            "y": 4.113210958649556,
+            "heading": -2.4189397780167137,
+            "angularVelocity": -4.765108187489175,
+            "velocityX": 1.8088486877145706,
+            "velocityY": 0.147889399476235,
+            "timestamp": 2.3898549328572267
+        },
+        {
+            "x": 7.549783547522053,
+            "y": 4.117992264935679,
+            "heading": -2.6004162007304275,
+            "angularVelocity": -4.739419336084478,
+            "velocityX": 1.490487194299341,
+            "velocityY": 0.12486809650168224,
+            "timestamp": 2.4281457887289593
+        },
+        {
+            "x": 7.595268726062492,
+            "y": 4.121696461876066,
+            "heading": -2.768130415473744,
+            "angularVelocity": -4.380006947484507,
+            "velocityX": 1.1878861807843983,
+            "velocityY": 0.09673842112058428,
+            "timestamp": 2.466436644600692
+        },
+        {
+            "x": 7.630218683798498,
+            "y": 4.124204213311165,
+            "heading": -2.9120023323717077,
+            "angularVelocity": -3.7573439825922996,
+            "velocityX": 0.9127494525868564,
+            "velocityY": 0.06549217503781085,
+            "timestamp": 2.5047275004724243
+        },
+        {
+            "x": 7.655568399114973,
+            "y": 4.125734158703857,
+            "heading": -3.0250833006842397,
+            "angularVelocity": -2.9532107793916214,
+            "velocityX": 0.6620305224148624,
+            "velocityY": 0.03995589437375436,
+            "timestamp": 2.543018356344157
+        },
+        {
+            "x": 7.671992151604379,
+            "y": 4.126594890877869,
+            "heading": -3.103103884407084,
+            "angularVelocity": -2.037577430606375,
+            "velocityX": 0.42892100778377035,
+            "velocityY": 0.022478791722372646,
+            "timestamp": 2.5813092122158894
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -3.143185307179586,
+            "angularVelocity": -1.0467622585080842,
+            "velocityX": 0.20913213385579937,
+            "velocityY": 0.010579787599633836,
+            "timestamp": 2.619600068087622
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -3.143185307179586,
+            "angularVelocity": 7.101458819208695e-33,
+            "velocityX": -7.030931331746161e-34,
+            "velocityY": 2.5921613146305674e-34,
+            "timestamp": 2.6578909239593544
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.1.traj
new file mode 100644
index 00000000..00ddf5d5
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.1.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": -2.1,
+            "angularVelocity": 0,
+            "velocityX": -3.0738838344554265e-34,
+            "velocityY": -1.6532575942515022e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 0.8675349003884936,
+            "y": 6.638058463961624,
+            "heading": -2.192308349350307,
+            "angularVelocity": -0.6292858581626611,
+            "velocityX": 1.0739492763036018,
+            "velocityY": -0.5586130635852894,
+            "timestamp": 0.14668746826731788
+        },
+        {
+            "x": 1.183655074306953,
+            "y": 6.473510793186018,
+            "heading": -2.3371750269963787,
+            "angularVelocity": -0.9875872789764978,
+            "velocityX": 2.155059172078514,
+            "velocityY": -1.12175683934867,
+            "timestamp": 0.29337493653463576
+        },
+        {
+            "x": 1.6571019357484182,
+            "y": 6.226344402054212,
+            "heading": -2.393383986431118,
+            "angularVelocity": -0.3831885579503364,
+            "velocityX": 3.2275890165250725,
+            "velocityY": -1.6849864139817283,
+            "timestamp": 0.44006240480195363
+        },
+        {
+            "x": 2.149819339361714,
+            "y": 5.971090303282803,
+            "heading": -2.3933840219605123,
+            "angularVelocity": -2.422115180983058e-7,
+            "velocityX": 3.3589604444967707,
+            "velocityY": -1.7401220553227135,
+            "timestamp": 0.5867498730692715
+        },
+        {
+            "x": 2.6425367239446045,
+            "y": 5.71583616777695,
+            "heading": -2.3933840574898846,
+            "angularVelocity": -2.422113667505868e-7,
+            "velocityX": 3.358960314762406,
+            "velocityY": -1.740122305749301,
+            "timestamp": 0.7334373413365893
+        },
+        {
+            "x": 3.135254076780422,
+            "y": 5.4605819709896695,
+            "heading": -2.3933840930192347,
+            "angularVelocity": -2.4221121497556687e-7,
+            "velocityX": 3.358960098335784,
+            "velocityY": -1.7401227235179682,
+            "timestamp": 0.8801248096039072
+        },
+        {
+            "x": 3.627971327740651,
+            "y": 5.20532757755194,
+            "heading": -2.393384128548559,
+            "angularVelocity": -2.4221104201937785e-7,
+            "velocityX": 3.358959403828,
+            "velocityY": -1.7401240641263462,
+            "timestamp": 1.026812277871225
+        },
+        {
+            "x": 4.091605856430787,
+            "y": 4.900420846350941,
+            "heading": -2.393384184524006,
+            "angularVelocity": -3.815966513199635e-7,
+            "velocityX": 3.160696235108687,
+            "velocityY": -2.07861472287018,
+            "timestamp": 1.1734997461385428
+        },
+        {
+            "x": 4.555240236344247,
+            "y": 4.595513888923491,
+            "heading": -2.393384240499462,
+            "angularVelocity": -3.815967160070403e-7,
+            "velocityX": 3.1606952208661028,
+            "velocityY": -2.078616265104517,
+            "timestamp": 1.3201872144058606
+        },
+        {
+            "x": 5.018874645233154,
+            "y": 4.29060697555542,
+            "heading": -2.393384296474878,
+            "angularVelocity": -3.8159644312835243e-7,
+            "velocityX": 3.1606954183979536,
+            "velocityY": -2.078615964742264,
+            "timestamp": 1.4668746826731784
+        },
+        {
+            "x": 5.414956983036457,
+            "y": 4.102744630717356,
+            "heading": -2.393384340957434,
+            "angularVelocity": -3.838597081329049e-7,
+            "velocityX": 3.4179702017234304,
+            "velocityY": -1.621147512518649,
+            "timestamp": 1.5827570088571772
+        },
+        {
+            "x": 5.811039262323732,
+            "y": 3.9148821625062062,
+            "heading": -2.3933843854399766,
+            "angularVelocity": -3.838595921618151e-7,
+            "velocityX": 3.4179696967626736,
+            "velocityY": -1.6211485771597347,
+            "timestamp": 1.698639335041176
+        },
+        {
+            "x": 6.175303264444137,
+            "y": 3.6709862524403962,
+            "heading": -2.39338444094527,
+            "angularVelocity": -4.789798009551726e-7,
+            "velocityX": 3.143395667964284,
+            "velocityY": -2.1046860043053552,
+            "timestamp": 1.8145216612251747
+        },
+        {
+            "x": 6.517947229314261,
+            "y": 3.397547803025246,
+            "heading": -2.3933845251610077,
+            "angularVelocity": -7.267349611095117e-7,
+            "velocityX": 2.9568267755177056,
+            "velocityY": -2.3596216819205247,
+            "timestamp": 1.9304039874091734
+        },
+        {
+            "x": 6.860591017465818,
+            "y": 3.124109132165372,
+            "heading": -2.3933846093768096,
+            "angularVelocity": -7.267355129115313e-7,
+            "velocityX": 2.9568252505347843,
+            "velocityY": -2.3596235928652884,
+            "timestamp": 2.046286313593172
+        },
+        {
+            "x": 7.1795790741631205,
+            "y": 2.8553069978284444,
+            "heading": -2.4556792266097736,
+            "angularVelocity": -0.5375678870482116,
+            "velocityX": 2.7526894497337926,
+            "velocityY": -2.3196128623628227,
+            "timestamp": 2.1621686397771707
+        },
+        {
+            "x": 7.431770863942688,
+            "y": 2.653048744111035,
+            "heading": -2.7460349787453167,
+            "angularVelocity": -2.5056085918961837,
+            "velocityX": 2.1762748305478063,
+            "velocityY": -1.7453761965069832,
+            "timestamp": 2.278050965961169
+        },
+        {
+            "x": 7.5976853712302646,
+            "y": 2.521507979737877,
+            "heading": -3.002083319178936,
+            "angularVelocity": -2.2095547169726615,
+            "velocityX": 1.4317498858638353,
+            "velocityY": -1.135123609482061,
+            "timestamp": 2.3939332921451677
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -3.143185307179586,
+            "angularVelocity": -1.2176316496840747,
+            "velocityX": 0.7103294478144603,
+            "velocityY": -0.5566679739881255,
+            "timestamp": 2.509815618329166
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -3.143185307179586,
+            "angularVelocity": 2.6221634517625535e-33,
+            "velocityX": -1.3293034908554191e-34,
+            "velocityY": 0,
+            "timestamp": 2.6256979445131647
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.traj
new file mode 100644
index 00000000..00ddf5d5
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": -2.1,
+            "angularVelocity": 0,
+            "velocityX": -3.0738838344554265e-34,
+            "velocityY": -1.6532575942515022e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 0.8675349003884936,
+            "y": 6.638058463961624,
+            "heading": -2.192308349350307,
+            "angularVelocity": -0.6292858581626611,
+            "velocityX": 1.0739492763036018,
+            "velocityY": -0.5586130635852894,
+            "timestamp": 0.14668746826731788
+        },
+        {
+            "x": 1.183655074306953,
+            "y": 6.473510793186018,
+            "heading": -2.3371750269963787,
+            "angularVelocity": -0.9875872789764978,
+            "velocityX": 2.155059172078514,
+            "velocityY": -1.12175683934867,
+            "timestamp": 0.29337493653463576
+        },
+        {
+            "x": 1.6571019357484182,
+            "y": 6.226344402054212,
+            "heading": -2.393383986431118,
+            "angularVelocity": -0.3831885579503364,
+            "velocityX": 3.2275890165250725,
+            "velocityY": -1.6849864139817283,
+            "timestamp": 0.44006240480195363
+        },
+        {
+            "x": 2.149819339361714,
+            "y": 5.971090303282803,
+            "heading": -2.3933840219605123,
+            "angularVelocity": -2.422115180983058e-7,
+            "velocityX": 3.3589604444967707,
+            "velocityY": -1.7401220553227135,
+            "timestamp": 0.5867498730692715
+        },
+        {
+            "x": 2.6425367239446045,
+            "y": 5.71583616777695,
+            "heading": -2.3933840574898846,
+            "angularVelocity": -2.422113667505868e-7,
+            "velocityX": 3.358960314762406,
+            "velocityY": -1.740122305749301,
+            "timestamp": 0.7334373413365893
+        },
+        {
+            "x": 3.135254076780422,
+            "y": 5.4605819709896695,
+            "heading": -2.3933840930192347,
+            "angularVelocity": -2.4221121497556687e-7,
+            "velocityX": 3.358960098335784,
+            "velocityY": -1.7401227235179682,
+            "timestamp": 0.8801248096039072
+        },
+        {
+            "x": 3.627971327740651,
+            "y": 5.20532757755194,
+            "heading": -2.393384128548559,
+            "angularVelocity": -2.4221104201937785e-7,
+            "velocityX": 3.358959403828,
+            "velocityY": -1.7401240641263462,
+            "timestamp": 1.026812277871225
+        },
+        {
+            "x": 4.091605856430787,
+            "y": 4.900420846350941,
+            "heading": -2.393384184524006,
+            "angularVelocity": -3.815966513199635e-7,
+            "velocityX": 3.160696235108687,
+            "velocityY": -2.07861472287018,
+            "timestamp": 1.1734997461385428
+        },
+        {
+            "x": 4.555240236344247,
+            "y": 4.595513888923491,
+            "heading": -2.393384240499462,
+            "angularVelocity": -3.815967160070403e-7,
+            "velocityX": 3.1606952208661028,
+            "velocityY": -2.078616265104517,
+            "timestamp": 1.3201872144058606
+        },
+        {
+            "x": 5.018874645233154,
+            "y": 4.29060697555542,
+            "heading": -2.393384296474878,
+            "angularVelocity": -3.8159644312835243e-7,
+            "velocityX": 3.1606954183979536,
+            "velocityY": -2.078615964742264,
+            "timestamp": 1.4668746826731784
+        },
+        {
+            "x": 5.414956983036457,
+            "y": 4.102744630717356,
+            "heading": -2.393384340957434,
+            "angularVelocity": -3.838597081329049e-7,
+            "velocityX": 3.4179702017234304,
+            "velocityY": -1.621147512518649,
+            "timestamp": 1.5827570088571772
+        },
+        {
+            "x": 5.811039262323732,
+            "y": 3.9148821625062062,
+            "heading": -2.3933843854399766,
+            "angularVelocity": -3.838595921618151e-7,
+            "velocityX": 3.4179696967626736,
+            "velocityY": -1.6211485771597347,
+            "timestamp": 1.698639335041176
+        },
+        {
+            "x": 6.175303264444137,
+            "y": 3.6709862524403962,
+            "heading": -2.39338444094527,
+            "angularVelocity": -4.789798009551726e-7,
+            "velocityX": 3.143395667964284,
+            "velocityY": -2.1046860043053552,
+            "timestamp": 1.8145216612251747
+        },
+        {
+            "x": 6.517947229314261,
+            "y": 3.397547803025246,
+            "heading": -2.3933845251610077,
+            "angularVelocity": -7.267349611095117e-7,
+            "velocityX": 2.9568267755177056,
+            "velocityY": -2.3596216819205247,
+            "timestamp": 1.9304039874091734
+        },
+        {
+            "x": 6.860591017465818,
+            "y": 3.124109132165372,
+            "heading": -2.3933846093768096,
+            "angularVelocity": -7.267355129115313e-7,
+            "velocityX": 2.9568252505347843,
+            "velocityY": -2.3596235928652884,
+            "timestamp": 2.046286313593172
+        },
+        {
+            "x": 7.1795790741631205,
+            "y": 2.8553069978284444,
+            "heading": -2.4556792266097736,
+            "angularVelocity": -0.5375678870482116,
+            "velocityX": 2.7526894497337926,
+            "velocityY": -2.3196128623628227,
+            "timestamp": 2.1621686397771707
+        },
+        {
+            "x": 7.431770863942688,
+            "y": 2.653048744111035,
+            "heading": -2.7460349787453167,
+            "angularVelocity": -2.5056085918961837,
+            "velocityX": 2.1762748305478063,
+            "velocityY": -1.7453761965069832,
+            "timestamp": 2.278050965961169
+        },
+        {
+            "x": 7.5976853712302646,
+            "y": 2.521507979737877,
+            "heading": -3.002083319178936,
+            "angularVelocity": -2.2095547169726615,
+            "velocityX": 1.4317498858638353,
+            "velocityY": -1.135123609482061,
+            "timestamp": 2.3939332921451677
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -3.143185307179586,
+            "angularVelocity": -1.2176316496840747,
+            "velocityX": 0.7103294478144603,
+            "velocityY": -0.5566679739881255,
+            "timestamp": 2.509815618329166
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -3.143185307179586,
+            "angularVelocity": 2.6221634517625535e-33,
+            "velocityX": -1.3293034908554191e-34,
+            "velocityY": 0,
+            "timestamp": 2.6256979445131647
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.1.traj
index 9e5becca..19f07e68 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.1.traj	
@@ -5,675 +5,657 @@
             "y": 6.72,
             "heading": -2.1,
             "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
+            "velocityX": 3.044182207072049e-31,
+            "velocityY": 6.458172299259412e-31,
             "timestamp": 0
         },
         {
-            "x": 0.7185162341797315,
-            "y": 6.716270626486824,
-            "heading": -2.0163378917371135,
-            "angularVelocity": 1.6978541106369687,
-            "velocityX": 0.17283001240859905,
-            "velocityY": -0.07568458745445246,
-            "timestamp": 0.049275204352804955
-        },
-        {
-            "x": 0.7365174505438988,
-            "y": 6.709427263698689,
-            "heading": -1.8516035946552112,
-            "angularVelocity": 3.3431479228867977,
-            "velocityX": 0.3653199738205181,
-            "velocityY": -0.1388804547442707,
-            "timestamp": 0.09855040870560991
-        },
-        {
-            "x": 0.7651610538016371,
-            "y": 6.700606345332281,
-            "heading": -1.6098229565949118,
-            "angularVelocity": 4.906740443513477,
-            "velocityX": 0.5812985178641444,
-            "velocityY": -0.1790133289605956,
-            "timestamp": 0.14782561305841485
-        },
-        {
-            "x": 0.8095297793953646,
-            "y": 6.688220030792632,
-            "heading": -1.3224284097332135,
-            "angularVelocity": 5.832437442653418,
-            "velocityX": 0.9004270236212996,
-            "velocityY": -0.2513701303187648,
-            "timestamp": 0.19710081741121982
-        },
-        {
-            "x": 0.8714701737870825,
-            "y": 6.666054185298926,
-            "heading": -1.026684817124186,
-            "angularVelocity": 6.001874502468547,
-            "velocityX": 1.2570296806530035,
-            "velocityY": -0.4498377182771496,
-            "timestamp": 0.2463760217640248
-        },
-        {
-            "x": 0.9484340695976169,
-            "y": 6.633599911893754,
-            "heading": -0.7648347305022277,
-            "angularVelocity": 5.314033499427846,
-            "velocityX": 1.5619193633268658,
-            "velocityY": -0.6586329540675864,
-            "timestamp": 0.29565122611682976
-        },
-        {
-            "x": 1.0425919145867164,
-            "y": 6.594089868440422,
-            "heading": -0.5334353297349962,
-            "angularVelocity": 4.696061717176001,
-            "velocityX": 1.9108565093903993,
-            "velocityY": -0.8018240405548321,
-            "timestamp": 0.3449264304696347
-        },
-        {
-            "x": 1.155214051879495,
-            "y": 6.555173696166533,
-            "heading": -0.3327017892852748,
-            "angularVelocity": 4.073723145062813,
-            "velocityX": 2.285574230934019,
-            "velocityY": -0.7897719103355402,
-            "timestamp": 0.3942016348224397
-        },
-        {
-            "x": 1.2837470785266114,
-            "y": 6.519808002504125,
-            "heading": -0.17776538289371885,
-            "angularVelocity": 3.1443077390857384,
-            "velocityX": 2.6084727265022423,
-            "velocityY": -0.7177178487012317,
-            "timestamp": 0.44347683917524466
-        },
-        {
-            "x": 1.4286059942579405,
-            "y": 6.483992998763445,
-            "heading": -0.07145024720477738,
-            "angularVelocity": 2.1575787880601562,
-            "velocityX": 2.9397933024114007,
-            "velocityY": -0.7268362295212014,
-            "timestamp": 0.49275204352804963
-        },
-        {
-            "x": 1.5885837184337814,
-            "y": 6.442409682842055,
-            "heading": -0.012288104865979272,
-            "angularVelocity": 1.2006473258883674,
-            "velocityX": 3.246617163277867,
-            "velocityY": -0.8438994108204557,
-            "timestamp": 0.5420272478808545
-        },
-        {
-            "x": 1.7606060854656245,
-            "y": 6.389861829992508,
-            "heading": 0.005689657356259434,
-            "angularVelocity": 0.3648439911790104,
-            "velocityX": 3.491053346023324,
-            "velocityY": -1.0664157265246377,
-            "timestamp": 0.5913024522336595
-        },
-        {
-            "x": 1.937365788040802,
-            "y": 6.330680677458663,
-            "heading": 0.005693236516864234,
-            "angularVelocity": 0.00007263613924707546,
-            "velocityX": 3.587193699078299,
-            "velocityY": -1.2010331222599455,
-            "timestamp": 0.6405776565864645
-        },
-        {
-            "x": 2.11412543100989,
-            "y": 6.27149934643189,
-            "heading": 0.005696815267395046,
-            "angularVelocity": 0.00007262781713070066,
-            "velocityX": 3.5871924894214278,
-            "velocityY": -1.20103674462801,
-            "timestamp": 0.6898528609392695
-        },
-        {
-            "x": 2.2908850692554568,
-            "y": 6.212318001247489,
-            "heading": 0.005700393968496334,
-            "angularVelocity": 0.00007262681399886251,
-            "velocityX": 3.5871923935614243,
-            "velocityY": -1.2010370319454915,
-            "timestamp": 0.7391280652920744
-        },
-        {
-            "x": 2.467644702239147,
-            "y": 6.153136640294885,
-            "heading": 0.005703972617858713,
-            "angularVelocity": 0.00007262576400001342,
-            "velocityX": 3.5871922867759345,
-            "velocityY": -1.2010373519482989,
-            "timestamp": 0.7884032696448794
-        },
-        {
-            "x": 2.644404329357872,
-            "y": 6.0939552617700175,
-            "heading": 0.005707551213044099,
-            "angularVelocity": 0.00007262466452220377,
-            "velocityX": 3.5871921677512737,
-            "velocityY": -1.2010377085630215,
-            "timestamp": 0.8376784739976844
-        },
-        {
-            "x": 2.821163949952127,
-            "y": 6.0347738637002735,
-            "heading": 0.00571112975156944,
-            "angularVelocity": 0.00007262351465291134,
-            "velocityX": 3.5871920353424867,
-            "velocityY": -1.201038105210319,
-            "timestamp": 0.8869536783504893
-        },
-        {
-            "x": 2.9979235633249197,
-            "y": 5.975592444001196,
-            "heading": 0.005714708231053879,
-            "angularVelocity": 0.0000726223164660635,
-            "velocityX": 3.587191888788799,
-            "velocityY": -1.2010385441599662,
-            "timestamp": 0.9362288827032943
-        },
-        {
-            "x": 3.174683168778421,
-            "y": 5.916411000586595,
-            "heading": 0.005718286649774776,
-            "angularVelocity": 0.0000726210833196382,
-            "velocityX": 3.587191728073259,
-            "velocityY": -1.2010390254471126,
-            "timestamp": 0.9855040870560993
-        },
-        {
-            "x": 3.3514425126695637,
-            "y": 5.8572287763527235,
-            "heading": 0.005721865395381687,
-            "angularVelocity": 0.00007262771720412379,
-            "velocityX": 3.5871864198789636,
-            "velocityY": -1.2010548715360532,
-            "timestamp": 1.0347792914089042
-        },
-        {
-            "x": 3.5167193258876743,
-            "y": 5.790065395689035,
-            "heading": 0.026683569824856986,
-            "angularVelocity": 0.4254006595161298,
-            "velocityX": 3.354157844475837,
-            "velocityY": -1.3630259183261217,
-            "timestamp": 1.084054495761709
-        },
-        {
-            "x": 3.666994209555769,
-            "y": 5.710547155721769,
-            "heading": 0.06778616903847971,
-            "angularVelocity": 0.8341436581233173,
-            "velocityX": 3.0497059452487165,
-            "velocityY": -1.6137576903369897,
-            "timestamp": 1.133329700114514
-        },
-        {
-            "x": 3.802461838655518,
-            "y": 5.617737963132636,
-            "heading": 0.12219899767661042,
-            "angularVelocity": 1.104263885920004,
-            "velocityX": 2.749204815667845,
-            "velocityY": -1.8834867111788967,
-            "timestamp": 1.1826049044673188
-        },
-        {
-            "x": 3.923178434371948,
-            "y": 5.511256217956543,
-            "heading": 0.18485588032163774,
-            "angularVelocity": 1.2715702241722033,
-            "velocityX": 2.4498446490878596,
-            "velocityY": -2.1609599914329216,
-            "timestamp": 1.2318801088201237
-        },
-        {
-            "x": 4.036364016344018,
-            "y": 5.380109892116248,
-            "heading": 0.2569375572143931,
-            "angularVelocity": 1.3544496582801502,
-            "velocityX": 2.1268119643276364,
-            "velocityY": -2.4643030500437595,
-            "timestamp": 1.2850985333308458
-        },
-        {
-            "x": 4.130923397861632,
-            "y": 5.234522406039544,
-            "heading": 0.33431047652627577,
-            "angularVelocity": 1.4538746688431972,
-            "velocityX": 1.7768166267034577,
-            "velocityY": -2.7356594528154665,
-            "timestamp": 1.3383169578415677
-        },
-        {
-            "x": 4.205236217563619,
-            "y": 5.077100065980883,
-            "heading": 0.4216368931025466,
-            "angularVelocity": 1.640905708485922,
-            "velocityX": 1.396373913455762,
-            "velocityY": -2.958042097374491,
-            "timestamp": 1.3915353823522896
-        },
-        {
-            "x": 4.2828034673506234,
-            "y": 4.901292660688841,
-            "heading": 0.4516847731002138,
-            "angularVelocity": 0.5646142341476765,
-            "velocityX": 1.4575262326936578,
-            "velocityY": -3.303506387277969,
-            "timestamp": 1.4447538068630115
-        },
-        {
-            "x": 4.371277738622277,
-            "y": 4.723874668487275,
-            "heading": 0.4413575215812008,
-            "angularVelocity": -0.19405406330532018,
-            "velocityX": 1.6624744547600938,
-            "velocityY": -3.333770096215863,
-            "timestamp": 1.4979722313737334
-        },
-        {
-            "x": 4.466886857140796,
-            "y": 4.565900844635089,
-            "heading": 0.39184092001837434,
-            "angularVelocity": -0.9304409519460731,
-            "velocityX": 1.796541693925137,
-            "velocityY": -2.968404745246797,
-            "timestamp": 1.5511906558844553
-        },
-        {
-            "x": 4.573967443469787,
-            "y": 4.427674107970891,
-            "heading": 0.321042696188799,
-            "angularVelocity": -1.330332952928953,
-            "velocityX": 2.012096136130751,
-            "velocityY": -2.5973474024273386,
-            "timestamp": 1.6044090803951772
-        },
-        {
-            "x": 4.693065628013639,
-            "y": 4.309440309299518,
-            "heading": 0.2371568341960054,
-            "angularVelocity": -1.5762560196777895,
-            "velocityX": 2.2379126334312645,
-            "velocityY": -2.221670403781889,
-            "timestamp": 1.657627504905899
-        },
-        {
-            "x": 4.8242268562316895,
-            "y": 4.211382865905762,
-            "heading": 0.14523841868535878,
-            "angularVelocity": -1.7271915949358416,
-            "velocityX": 2.464583072947316,
-            "velocityY": -1.8425469054237094,
-            "timestamp": 1.710845929416621
-        },
-        {
-            "x": 4.982290420935857,
-            "y": 4.128393361272933,
-            "heading": 0.04004394713112955,
-            "angularVelocity": -1.8026614827604082,
-            "velocityX": 2.7086508987606095,
-            "velocityY": -1.4221468225908107,
-            "timestamp": 1.7692010185779787
-        },
-        {
-            "x": 5.153780525115527,
-            "y": 4.07042189977071,
-            "heading": -0.06589739756263054,
-            "angularVelocity": -1.8154602489051344,
-            "velocityX": 2.9387343356717652,
-            "velocityY": -0.9934259776713893,
-            "timestamp": 1.8275561077393365
-        },
-        {
-            "x": 5.3372218147923,
-            "y": 4.038159807995649,
-            "heading": -0.16628826181080308,
-            "angularVelocity": -1.7203446295931681,
-            "velocityX": 3.143535419328013,
-            "velocityY": -0.5528582380510605,
-            "timestamp": 1.8859111969006943
-        },
-        {
-            "x": 5.52929807317918,
-            "y": 4.032409431901968,
-            "heading": -0.2472171752765858,
-            "angularVelocity": -1.3868355721641668,
-            "velocityX": 3.2915082668414595,
-            "velocityY": -0.0985411242844827,
-            "timestamp": 1.944266286062052
-        },
-        {
-            "x": 5.726840626298333,
-            "y": 4.053774421636682,
-            "heading": -0.3047520267300494,
-            "angularVelocity": -0.985944024425596,
-            "velocityX": 3.385181240541464,
-            "velocityY": 0.3661204196884676,
-            "timestamp": 2.00262137522341
-        },
-        {
-            "x": 5.927911696641962,
-            "y": 4.1025958147259995,
-            "heading": -0.3393222015882671,
-            "angularVelocity": -0.5924106252777295,
-            "velocityX": 3.4456475559080273,
-            "velocityY": 0.8366261416261735,
-            "timestamp": 2.060976464384768
-        },
-        {
-            "x": 6.112967212419423,
-            "y": 4.131783461785041,
-            "heading": -0.42258796983730795,
-            "angularVelocity": -1.4268810046507232,
-            "velocityX": 3.1711975499816933,
-            "velocityY": 0.500173120776742,
-            "timestamp": 2.119331553546126
-        },
-        {
-            "x": 6.288399006454155,
-            "y": 4.135519035273829,
-            "heading": -0.5357406933321943,
-            "angularVelocity": -1.9390377963780898,
-            "velocityX": 3.0062809697650312,
-            "velocityY": 0.06401452799529236,
-            "timestamp": 2.177686642707484
-        },
-        {
-            "x": 6.454763850529513,
-            "y": 4.112891662498818,
-            "heading": -0.6682840950132551,
-            "angularVelocity": -2.2713254933869544,
-            "velocityX": 2.850905490270817,
-            "velocityY": -0.3877532037084742,
-            "timestamp": 2.236041731868842
-        },
-        {
-            "x": 6.611553192138672,
-            "y": 4.063669681549072,
-            "heading": -0.8104452528667577,
-            "angularVelocity": -2.436139844811345,
-            "velocityX": 2.686815218045847,
-            "velocityY": -0.8434908018671928,
-            "timestamp": 2.2943968210302
-        },
-        {
-            "x": 6.724757817470915,
-            "y": 4.010978330480739,
-            "heading": -0.9191384597303142,
-            "angularVelocity": -2.4505521656643996,
-            "velocityX": 2.5522647438252197,
-            "velocityY": -1.1879574464517886,
-            "timestamp": 2.3387513989030966
-        },
-        {
-            "x": 6.831367598021963,
-            "y": 3.9432896977899077,
-            "heading": -1.0248995406249544,
-            "angularVelocity": -2.384445664159214,
-            "velocityX": 2.40358009620903,
-            "velocityY": -1.526079965968843,
-            "timestamp": 2.383105976775993
-        },
-        {
-            "x": 6.930834934158794,
-            "y": 3.86094565265281,
-            "heading": -1.1241964811981286,
-            "angularVelocity": -2.238707825328021,
-            "velocityX": 2.2425494933548396,
-            "velocityY": -1.856494844186423,
-            "timestamp": 2.4274605546488894
-        },
-        {
-            "x": 7.022708800988577,
-            "y": 3.764327451214886,
-            "heading": -1.2136436923328575,
-            "angularVelocity": -2.016639892077224,
-            "velocityX": 2.0713502694819663,
-            "velocityY": -2.1783140787585773,
-            "timestamp": 2.471815132521786
-        },
-        {
-            "x": 7.106642839511368,
-            "y": 3.6538333663708773,
-            "heading": -1.2900644922719695,
-            "angularVelocity": -1.7229518034892648,
-            "velocityX": 1.892342178597992,
-            "velocityY": -2.4911540170812514,
-            "timestamp": 2.5161697103946823
-        },
-        {
-            "x": 7.182389770268718,
-            "y": 3.5298642170672982,
-            "heading": -1.3504994507692183,
-            "angularVelocity": -1.3625416224325844,
-            "velocityX": 1.7077590271383551,
-            "velocityY": -2.794957256922336,
-            "timestamp": 2.5605242882675787
-        },
-        {
-            "x": 7.249751518053722,
-            "y": 3.39283877736109,
-            "heading": -1.392210624169865,
-            "angularVelocity": -0.9404028941539149,
-            "velocityX": 1.518710153843384,
-            "velocityY": -3.0893189897753475,
-            "timestamp": 2.604878866140475
-        },
-        {
-            "x": 7.30834760453933,
-            "y": 3.243309493178288,
-            "heading": -1.412928953137589,
-            "angularVelocity": -0.46710689090751717,
-            "velocityX": 1.3210831732751818,
-            "velocityY": -3.371225504868904,
-            "timestamp": 2.6492334440133716
-        },
-        {
-            "x": 7.356500151413166,
-            "y": 3.0825770019538052,
-            "heading": -1.4129301457519094,
-            "angularVelocity": -0.000026888190071743586,
-            "velocityX": 1.0856274410236224,
-            "velocityY": -3.623808385350466,
-            "timestamp": 2.693588021886268
-        },
-        {
-            "x": 7.388728975781619,
-            "y": 2.917910705433503,
-            "heading": -1.4129305799992817,
-            "angularVelocity": -0.00000979036196658053,
-            "velocityX": 0.7266177678617259,
-            "velocityY": -3.7124983353955976,
-            "timestamp": 2.7379425997591644
-        },
-        {
-            "x": 7.4166210769822385,
-            "y": 2.7524546215220593,
-            "heading": -1.4129310308638345,
-            "angularVelocity": -0.000010165006058865363,
-            "velocityX": 0.6288437978273086,
-            "velocityY": -3.73030455583591,
-            "timestamp": 2.782297177632061
-        },
-        {
-            "x": 7.4445129710491305,
-            "y": 2.5869985026929103,
-            "heading": -1.412931481728963,
-            "angularVelocity": -0.000010165019038462572,
-            "velocityX": 0.628839127875817,
-            "velocityY": -3.730305343076067,
-            "timestamp": 2.8266517555049573
-        },
-        {
-            "x": 7.4724048295219,
-            "y": 2.4215423778635246,
-            "heading": -1.4129319325942993,
-            "angularVelocity": -0.000010165023720794904,
-            "velocityX": 0.6288383253854289,
-            "velocityY": -3.7303054783549245,
-            "timestamp": 2.8710063333778537
-        },
-        {
-            "x": 7.500296654307517,
-            "y": 2.2560862473554004,
-            "heading": -1.4129323834599168,
-            "angularVelocity": -0.000010165030059818064,
-            "velocityX": 0.6288375658887922,
-            "velocityY": -3.73030560638543,
-            "timestamp": 2.91536091125075
-        },
-        {
-            "x": 7.528188447597349,
-            "y": 2.090630111537975,
-            "heading": -1.4129328343258867,
-            "angularVelocity": -0.000010165038009449021,
-            "velocityX": 0.6288368557978095,
-            "velocityY": -3.7303057260867334,
-            "timestamp": 2.9597154891236466
-        },
-        {
-            "x": 7.55608021118008,
-            "y": 1.9251739707128015,
-            "heading": -1.4129332851922731,
-            "angularVelocity": -0.000010165047399002056,
-            "velocityX": 0.6288361860338073,
-            "velocityY": -3.7303058389893544,
-            "timestamp": 3.004070066996543
-        },
-        {
-            "x": 7.583956606011812,
-            "y": 1.7597152400598883,
-            "heading": -1.412933736829767,
-            "angularVelocity": -0.000010182432471034155,
-            "velocityX": 0.6284896885190855,
-            "velocityY": -3.730364228176306,
-            "timestamp": 3.0484246448694394
-        },
-        {
-            "x": 7.603176305958169,
-            "y": 1.6036265149225075,
-            "heading": -1.4482789360266273,
-            "angularVelocity": -0.7968782680819577,
-            "velocityX": 0.4333194197323582,
-            "velocityY": -3.5191119524273815,
-            "timestamp": 3.092779222742336
-        },
-        {
-            "x": 7.616234477472668,
-            "y": 1.4592226194785036,
-            "heading": -1.5202643791491903,
-            "angularVelocity": -1.6229540799339002,
-            "velocityX": 0.2944041436245525,
-            "velocityY": -3.2556706064887218,
-            "timestamp": 3.1371338006152323
-        },
-        {
-            "x": 7.626690890740831,
-            "y": 1.32844105514062,
-            "heading": -1.6264074068747738,
-            "angularVelocity": -2.393056879714865,
-            "velocityX": 0.23574597639338804,
-            "velocityY": -2.9485471536366448,
-            "timestamp": 3.1814883784881287
-        },
-        {
-            "x": 7.6362242266749725,
-            "y": 1.2119757666324047,
-            "heading": -1.7630703603278275,
-            "angularVelocity": -3.0811465243718166,
-            "velocityX": 0.2149346559324737,
-            "velocityY": -2.6257783095571594,
-            "timestamp": 3.225842956361025
-        },
-        {
-            "x": 7.645916498443832,
-            "y": 1.110010534092232,
-            "heading": -1.9285530962892936,
-            "angularVelocity": -3.7309054419518417,
-            "velocityX": 0.21851795764202436,
-            "velocityY": -2.298866034355394,
-            "timestamp": 3.2701975342339216
-        },
-        {
-            "x": 7.656628280193831,
-            "y": 1.0222888536217107,
-            "heading": -2.123550329375603,
-            "angularVelocity": -4.396327108446363,
-            "velocityX": 0.2415034087506259,
-            "velocityY": -1.9777367901437044,
-            "timestamp": 3.314552112106818
-        },
-        {
-            "x": 7.664811531196715,
-            "y": 0.9478358295186623,
-            "heading": -2.3514420654429586,
-            "angularVelocity": -5.137952991468154,
-            "velocityX": 0.18449619848336826,
-            "velocityY": -1.6785871419271048,
-            "timestamp": 3.3589066899797144
-        },
-        {
-            "x": 7.671315625708808,
-            "y": 0.8895973140434956,
-            "heading": -2.585878637991588,
-            "angularVelocity": -5.285510172601276,
-            "velocityX": 0.146638629517147,
-            "velocityY": -1.313021524904517,
-            "timestamp": 3.403261267852611
-        },
-        {
-            "x": 7.675675621987506,
-            "y": 0.8463977620596795,
-            "heading": -2.797067073731653,
-            "angularVelocity": -4.761367278598646,
-            "velocityX": 0.0982986759831574,
-            "velocityY": -0.9739592631815782,
-            "timestamp": 3.4476158457255073
-        },
-        {
-            "x": 7.67812133477644,
-            "y": 0.8158415850765369,
-            "heading": -2.965913260384183,
-            "angularVelocity": -3.8067364125610696,
-            "velocityX": 0.05514003077524084,
-            "velocityY": -0.6889069505002431,
-            "timestamp": 3.4919704235984037
-        },
-        {
-            "x": 7.679402410293105,
-            "y": 0.7963789610313945,
-            "heading": -3.08300713864879,
-            "angularVelocity": -2.6399502346782358,
-            "velocityX": 0.02888259967968837,
-            "velocityY": -0.4387962861672373,
-            "timestamp": 3.5363250014713
+            "x": 0.7256414580836799,
+            "y": 6.712155644836975,
+            "heading": -2.1163027262729814,
+            "angularVelocity": -0.3503504178414695,
+            "velocityX": 0.3361395685300024,
+            "velocityY": -0.16857751660924442,
+            "timestamp": 0.04653262974092379
+        },
+        {
+            "x": 0.7569790078652245,
+            "y": 6.696436652979446,
+            "heading": -2.1479259826479233,
+            "angularVelocity": -0.6795931489587892,
+            "velocityX": 0.6734532296158987,
+            "velocityY": -0.3378057923018942,
+            "timestamp": 0.09306525948184757
+        },
+        {
+            "x": 0.8040786472875676,
+            "y": 6.672808979329537,
+            "heading": -2.1936219074673273,
+            "angularVelocity": -0.9820189633351775,
+            "velocityX": 1.0121852060495677,
+            "velocityY": -0.5077657072349027,
+            "timestamp": 0.13959788922277136
+        },
+        {
+            "x": 0.8670199327112733,
+            "y": 6.641236276400096,
+            "heading": -2.2517861808528257,
+            "angularVelocity": -1.2499674681912987,
+            "velocityX": 1.3526268722430308,
+            "velocityY": -0.6785067404360078,
+            "timestamp": 0.18613051896369515
+        },
+        {
+            "x": 0.9458962124147239,
+            "y": 6.601679900322021,
+            "heading": -2.32031104809549,
+            "angularVelocity": -1.4726196998587933,
+            "velocityX": 1.6950746205965188,
+            "velocityY": -0.8500782418336558,
+            "timestamp": 0.23266314870461893
+        },
+        {
+            "x": 1.0408105422873288,
+            "y": 6.554096827806623,
+            "heading": -2.396269750321632,
+            "angularVelocity": -1.6323750161792245,
+            "velocityX": 2.039737070547226,
+            "velocityY": -1.022574326452373,
+            "timestamp": 0.2791957784455427
+        },
+        {
+            "x": 1.1518552587221624,
+            "y": 6.498443176244561,
+            "heading": -2.4750808863817895,
+            "angularVelocity": -1.6936746644023601,
+            "velocityX": 2.3863838569428775,
+            "velocityY": -1.19601346134745,
+            "timestamp": 0.32572840818646653
+        },
+        {
+            "x": 1.2789760591669448,
+            "y": 6.434740942054882,
+            "heading": -2.5479819963693036,
+            "angularVelocity": -1.5666664530556718,
+            "velocityX": 2.7318636654010686,
+            "velocityY": -1.3689798866801088,
+            "timestamp": 0.3722610379273903
+        },
+        {
+            "x": 1.4202169645474811,
+            "y": 6.363052674678261,
+            "heading": -2.59222819945715,
+            "angularVelocity": -0.9508640137943395,
+            "velocityX": 3.0353089040296184,
+            "velocityY": -1.5406021060002604,
+            "timestamp": 0.41879366766831405
+        },
+        {
+            "x": 1.573995110361942,
+            "y": 6.28311594811204,
+            "heading": -2.599064012600599,
+            "angularVelocity": -0.14690364979431952,
+            "velocityX": 3.304737915536797,
+            "velocityY": -1.7178639378700842,
+            "timestamp": 0.4653262974092378
+        },
+        {
+            "x": 1.7314466383055478,
+            "y": 6.204403629832344,
+            "heading": -2.599064059608806,
+            "angularVelocity": -0.0000010102203007383576,
+            "velocityX": 3.3836799858560402,
+            "velocityY": -1.6915510410205123,
+            "timestamp": 0.5118589271501616
+        },
+        {
+            "x": 1.8889032791179137,
+            "y": 6.1257015398581975,
+            "heading": -2.5990641066080484,
+            "angularVelocity": -0.0000010100276450499987,
+            "velocityX": 3.3837898629208136,
+            "velocityY": -1.69133123170322,
+            "timestamp": 0.5583915568910853
+        },
+        {
+            "x": 2.0463595988203984,
+            "y": 6.046998807454145,
+            "heading": -2.5990641536073187,
+            "angularVelocity": -0.0000010100282384452147,
+            "velocityX": 3.3837829621742297,
+            "velocityY": -1.691345037712453,
+            "timestamp": 0.6049241866320091
+        },
+        {
+            "x": 2.2038159116861142,
+            "y": 5.968296061372362,
+            "heading": -2.599064200606589,
+            "angularVelocity": -0.0000010100282469186477,
+            "velocityX": 3.3837828152499863,
+            "velocityY": -1.6913453316510296,
+            "timestamp": 0.6514568163729328
+        },
+        {
+            "x": 2.3612722547062472,
+            "y": 5.889593375618833,
+            "heading": -2.5990642476058565,
+            "angularVelocity": -0.0000010100281851545483,
+            "velocityX": 3.3837834632773647,
+            "velocityY": -1.6913440351789604,
+            "timestamp": 0.6979894461138566
+        },
+        {
+            "x": 2.5187286297414215,
+            "y": 5.8108907539159365,
+            "heading": -2.599064294605121,
+            "angularVelocity": -0.0000010100281210334782,
+            "velocityX": 3.383784151290135,
+            "velocityY": -1.6913426587117795,
+            "timestamp": 0.7445220758547804
+        },
+        {
+            "x": 2.6761850576997652,
+            "y": 5.732188238093708,
+            "heading": -2.599064341604381,
+            "angularVelocity": -0.000001010028016565385,
+            "velocityX": 3.3837852886247464,
+            "velocityY": -1.6913403833048126,
+            "timestamp": 0.7910547055957041
+        },
+        {
+            "x": 2.83364151622281,
+            "y": 5.653485783421201,
+            "heading": -2.5990643886036375,
+            "angularVelocity": -0.0000010100279552603614,
+            "velocityX": 3.3837859454691737,
+            "velocityY": -1.6913390691793195,
+            "timestamp": 0.8375873353366279
+        },
+        {
+            "x": 2.9910977149954676,
+            "y": 5.574782809080547,
+            "heading": -2.5990644356029167,
+            "angularVelocity": -0.000001010028434200447,
+            "velocityX": 3.3837803633561943,
+            "velocityY": -1.691350237002125,
+            "timestamp": 0.8841199650775516
+        },
+        {
+            "x": 3.1485530895378893,
+            "y": 5.496078185774728,
+            "heading": -2.599064482602267,
+            "angularVelocity": -0.0000010100299667197812,
+            "velocityX": 3.3837626504042433,
+            "velocityY": -1.6913856737520705,
+            "timestamp": 0.9306525948184754
+        },
+        {
+            "x": 3.3060077005580313,
+            "y": 5.417372034978031,
+            "heading": -2.5990645296016837,
+            "angularVelocity": -0.0000010100313828443218,
+            "velocityX": 3.383746242085582,
+            "velocityY": -1.6914184999808708,
+            "timestamp": 0.9771852245593992
+        },
+        {
+            "x": 3.463466207634218,
+            "y": 5.3386736786731355,
+            "heading": -2.5990645766007607,
+            "angularVelocity": -0.0000010100240886605546,
+            "velocityX": 3.383829969488029,
+            "velocityY": -1.6912509940353067,
+            "timestamp": 1.023717854300323
+        },
+        {
+            "x": 3.620938146457774,
+            "y": 5.260002202075332,
+            "heading": -2.5990646235986756,
+            "angularVelocity": -0.0000010099991153726974,
+            "velocityX": 3.3841186217193853,
+            "velocityY": -1.690673341164269,
+            "timestamp": 1.0702504840412468
+        },
+        {
+            "x": 3.7783857783864416,
+            "y": 5.181282090499446,
+            "heading": -2.5990646705986817,
+            "angularVelocity": -0.0000010100440573860445,
+            "velocityX": 3.383596259333376,
+            "velocityY": -1.691718521279299,
+            "timestamp": 1.1167831137821707
+        },
+        {
+            "x": 3.9357445242921885,
+            "y": 5.102384448798495,
+            "heading": -2.599064717606569,
+            "angularVelocity": -0.0000010102134222869035,
+            "velocityX": 3.381686072364613,
+            "velocityY": -1.6955336962675904,
+            "timestamp": 1.1633157435230945
+        },
+        {
+            "x": 4.092995643615723,
+            "y": 5.023272514343262,
+            "heading": -2.5990647646424785,
+            "angularVelocity": -0.000001010815630420508,
+            "velocityX": 3.379373145230901,
+            "velocityY": -1.700138911036495,
+            "timestamp": 1.2098483732640184
+        },
+        {
+            "x": 4.261229917640389,
+            "y": 4.916068911394467,
+            "heading": -2.599064809945939,
+            "angularVelocity": -8.591015508014648e-7,
+            "velocityX": 3.1902711911232386,
+            "velocityY": -2.0329303767326627,
+            "timestamp": 1.2625819071694577
+        },
+        {
+            "x": 4.429297438166161,
+            "y": 4.8086040706119935,
+            "heading": -2.5990648552466906,
+            "angularVelocity": -8.590501714319945e-7,
+            "velocityX": 3.187109000265223,
+            "velocityY": -2.0378842991085575,
+            "timestamp": 1.315315441074897
+        },
+        {
+            "x": 4.597219271125054,
+            "y": 4.700911724294517,
+            "heading": -2.599064900558894,
+            "angularVelocity": -8.592673362873697e-7,
+            "velocityX": 3.184346288265075,
+            "velocityY": -2.0421985469549115,
+            "timestamp": 1.3680489749803364
+        },
+        {
+            "x": 4.765178923625143,
+            "y": 4.59327837178587,
+            "heading": -2.5990649458681174,
+            "angularVelocity": -8.592108353148986e-7,
+            "velocityX": 3.1850634702628065,
+            "velocityY": -2.0410798317000203,
+            "timestamp": 1.4207825088857757
+        },
+        {
+            "x": 4.93313403793343,
+            "y": 4.485637937706628,
+            "heading": -2.599064991177699,
+            "angularVelocity": -8.592176153657431e-7,
+            "velocityX": 3.184977411326241,
+            "velocityY": -2.041214121404157,
+            "timestamp": 1.473516042791215
+        },
+        {
+            "x": 5.101087827105617,
+            "y": 4.377995436130629,
+            "heading": -2.599065036487385,
+            "angularVelocity": -8.592196078701955e-7,
+            "velocityX": 3.184952282419374,
+            "velocityY": -2.041253327892839,
+            "timestamp": 1.5262495766966544
+        },
+        {
+            "x": 5.269016802174646,
+            "y": 4.270314227125004,
+            "heading": -2.599065081799025,
+            "angularVelocity": -8.592566538542498e-7,
+            "velocityX": 3.1844817259951634,
+            "velocityY": -2.041987347154346,
+            "timestamp": 1.5789831106020937
+        },
+        {
+            "x": 5.437055950944385,
+            "y": 4.162805027649924,
+            "heading": -2.599065127101977,
+            "angularVelocity": -8.590919076920599e-7,
+            "velocityX": 3.1865709791255945,
+            "velocityY": -2.038725484769403,
+            "timestamp": 1.631716644507533
+        },
+        {
+            "x": 5.605213465892237,
+            "y": 4.055481061993006,
+            "heading": -2.599065172406386,
+            "angularVelocity": -8.591195260284143e-7,
+            "velocityX": 3.188815588376687,
+            "velocityY": -2.035212846712725,
+            "timestamp": 1.6844501784129724
+        },
+        {
+            "x": 5.7832255363464355,
+            "y": 3.9654417037963863,
+            "heading": -2.599065217938776,
+            "angularVelocity": -8.634427952488677e-7,
+            "velocityX": 3.3756901400427615,
+            "velocityY": -1.707440247757134,
+            "timestamp": 1.7371837123184117
+        },
+        {
+            "x": 5.945200300467497,
+            "y": 3.903126168683373,
+            "heading": -2.599065262330506,
+            "angularVelocity": -9.676334890696444e-7,
+            "velocityX": 3.5306622744884395,
+            "velocityY": -1.3583295529523924,
+            "timestamp": 1.7830603075655527
+        },
+        {
+            "x": 6.112434090508488,
+            "y": 3.856737486793876,
+            "heading": -2.5990653054733555,
+            "angularVelocity": -9.404108896808373e-7,
+            "velocityX": 3.6452964554166534,
+            "velocityY": -1.01116226345067,
+            "timestamp": 1.8289369028126936
+        },
+        {
+            "x": 6.280097327658818,
+            "y": 3.8119257778274935,
+            "heading": -2.599065348543951,
+            "angularVelocity": -9.38835914358075e-7,
+            "velocityX": 3.6546573747925137,
+            "velocityY": -0.9767880272056341,
+            "timestamp": 1.8748134980598345
+        },
+        {
+            "x": 6.444605868501446,
+            "y": 3.756644766145867,
+            "heading": -2.599065392143275,
+            "angularVelocity": -9.503609402316767e-7,
+            "velocityX": 3.5858925440393525,
+            "velocityY": -1.2049937747953314,
+            "timestamp": 1.9206900933069755
+        },
+        {
+            "x": 6.602719051918594,
+            "y": 3.685095684621415,
+            "heading": -2.5990654382603977,
+            "angularVelocity": -0.0000010052429246959388,
+            "velocityX": 3.446489055375297,
+            "velocityY": -1.5595987701135843,
+            "timestamp": 1.9665666885541164
+        },
+        {
+            "x": 6.752801915063028,
+            "y": 3.597951074081566,
+            "heading": -2.599065489714602,
+            "angularVelocity": -0.0000011215785414644313,
+            "velocityX": 3.2714472888827597,
+            "velocityY": -1.8995439846919482,
+            "timestamp": 2.0124432838012574
+        },
+        {
+            "x": 6.893316861258978,
+            "y": 3.49609530674715,
+            "heading": -2.599065550773928,
+            "angularVelocity": -0.0000013309472018502562,
+            "velocityX": 3.06288959411619,
+            "velocityY": -2.2202120010370177,
+            "timestamp": 2.058319879048398
+        },
+        {
+            "x": 7.022827120260116,
+            "y": 3.380569821579126,
+            "heading": -2.5990656290967817,
+            "angularVelocity": -0.0000017072507862805843,
+            "velocityX": 2.8230137459733338,
+            "velocityY": -2.5181791400535034,
+            "timestamp": 2.104196474295539
+        },
+        {
+            "x": 7.140008449554443,
+            "y": 3.2525558471679688,
+            "heading": -2.5990657325552275,
+            "angularVelocity": -0.000002255146554794306,
+            "velocityX": 2.5542725797993215,
+            "velocityY": -2.790398322315311,
+            "timestamp": 2.1500730695426795
+        },
+        {
+            "x": 7.220411639481594,
+            "y": 3.1500923106303724,
+            "heading": -2.599065847631647,
+            "angularVelocity": -0.000003342401256012092,
+            "velocityX": 2.3353152959116907,
+            "velocityY": -2.9760593375246422,
+            "timestamp": 2.1845023348619734
+        },
+        {
+            "x": 7.292813286750944,
+            "y": 3.0418266490558983,
+            "heading": -2.5990659368308053,
+            "angularVelocity": -0.0000025907947068596104,
+            "velocityX": 2.1029100272080186,
+            "velocityY": -3.1445823943795888,
+            "timestamp": 2.2189316001812673
+        },
+        {
+            "x": 7.356796447286593,
+            "y": 2.9283823501608603,
+            "heading": -2.599066009494182,
+            "angularVelocity": -0.0000021105119622632264,
+            "velocityX": 1.8583945937350983,
+            "velocityY": -3.294996214498541,
+            "timestamp": 2.2533608655005612
+        },
+        {
+            "x": 7.411992663456803,
+            "y": 2.8104127239306367,
+            "heading": -2.5990660711538616,
+            "angularVelocity": -0.0000017909089651975303,
+            "velocityX": 1.603177287064451,
+            "velocityY": -3.426434608353826,
+            "timestamp": 2.287790130819855
+        },
+        {
+            "x": 7.458084080553435,
+            "y": 2.688597137512427,
+            "heading": -2.599066125344508,
+            "angularVelocity": -0.0000015739704607850277,
+            "velocityX": 1.3387278720350797,
+            "velocityY": -3.538140744175163,
+            "timestamp": 2.322219396139149
+        },
+        {
+            "x": 7.494809849686971,
+            "y": 2.5636384472618623,
+            "heading": -2.599066174481706,
+            "angularVelocity": -0.0000014271927566927072,
+            "velocityX": 1.0667020859416116,
+            "velocityY": -3.6294323765468546,
+            "timestamp": 2.356648661458443
+        },
+        {
+            "x": 7.5219737035667364,
+            "y": 2.4362587715435033,
+            "heading": -2.599066220329768,
+            "angularVelocity": -0.0000013316596016825942,
+            "velocityX": 0.7889757050532209,
+            "velocityY": -3.699750039306788,
+            "timestamp": 2.391077926777737
+        },
+        {
+            "x": 7.539529775464085,
+            "y": 2.307203584640024,
+            "heading": -2.5990662642857343,
+            "angularVelocity": -0.0000012767035893272692,
+            "velocityX": 0.5099171223818962,
+            "velocityY": -3.748415358464242,
+            "timestamp": 2.425507192097031
+        },
+        {
+            "x": 7.55111544342237,
+            "y": 2.1774760653156817,
+            "heading": -2.599066307607684,
+            "angularVelocity": -0.0000012582885205698109,
+            "velocityX": 0.3365063950923211,
+            "velocityY": -3.7679432924653384,
+            "timestamp": 2.4599364574163247
+        },
+        {
+            "x": 7.56271571323303,
+            "y": 2.047749850875278,
+            "heading": -2.5990663509314866,
+            "angularVelocity": -0.0000012583423688964546,
+            "velocityX": 0.3369305067394331,
+            "velocityY": -3.767905392035856,
+            "timestamp": 2.4943657227356186
+        },
+        {
+            "x": 7.5743766744470316,
+            "y": 1.9180290778400821,
+            "heading": -2.599066394258993,
+            "angularVelocity": -0.0000012584499309396356,
+            "velocityX": 0.33869329205287857,
+            "velocityY": -3.7677473461074933,
+            "timestamp": 2.5287949880549125
+        },
+        {
+            "x": 7.586111158859003,
+            "y": 1.7883149350094998,
+            "heading": -2.5990664375910546,
+            "angularVelocity": -0.0000012585822270319525,
+            "velocityX": 0.34082877758442714,
+            "velocityY": -3.7675547714315485,
+            "timestamp": 2.5632242533742065
+        },
+        {
+            "x": 7.597929633339714,
+            "y": 1.6586084182080805,
+            "heading": -2.59906648228782,
+            "angularVelocity": -0.0000012982201369267655,
+            "velocityX": 0.3432682739856076,
+            "velocityY": -3.7673332729737243,
+            "timestamp": 2.5976535186935004
+        },
+        {
+            "x": 7.610690036465753,
+            "y": 1.5360359058677395,
+            "heading": -2.6173573406008535,
+            "angularVelocity": -0.5312590362708309,
+            "velocityX": 0.37062664589906397,
+            "velocityY": -3.5601257013070047,
+            "timestamp": 2.6320827840127943
+        },
+        {
+            "x": 7.621968158889347,
+            "y": 1.4215047209508034,
+            "heading": -2.657213150288673,
+            "angularVelocity": -1.1576142946476244,
+            "velocityX": 0.3275737172727147,
+            "velocityY": -3.3265648817881672,
+            "timestamp": 2.666512049332088
+        },
+        {
+            "x": 7.631829723236215,
+            "y": 1.3151734834169941,
+            "heading": -2.7178665355180023,
+            "angularVelocity": -1.76168107762032,
+            "velocityX": 0.28642970610680407,
+            "velocityY": -3.08839693637669,
+            "timestamp": 2.700941314651382
+        },
+        {
+            "x": 7.640728077188491,
+            "y": 1.2186118736781408,
+            "heading": -2.7826314980185702,
+            "angularVelocity": -1.8811020769678346,
+            "velocityX": 0.2584532045561674,
+            "velocityY": -2.8046375327311788,
+            "timestamp": 2.735370579970676
+        },
+        {
+            "x": 7.648685947205252,
+            "y": 1.1318864826516366,
+            "heading": -2.8465688471146957,
+            "angularVelocity": -1.8570639978286094,
+            "velocityX": 0.23113679432217057,
+            "velocityY": -2.518943992043375,
+            "timestamp": 2.76979984528997
+        },
+        {
+            "x": 7.655720752960135,
+            "y": 1.0549563243440254,
+            "heading": -2.9071249866023474,
+            "angularVelocity": -1.7588565694352556,
+            "velocityX": 0.20432633951521018,
+            "velocityY": -2.2344408919030423,
+            "timestamp": 2.804229110609264
+        },
+        {
+            "x": 7.661844928847109,
+            "y": 0.9877692823605362,
+            "heading": -2.962675956286548,
+            "angularVelocity": -1.6134811233710036,
+            "velocityX": 0.17787704239939275,
+            "velocityY": -1.9514515154593546,
+            "timestamp": 2.8386583759285577
+        },
+        {
+            "x": 7.667068746842824,
+            "y": 0.930277282093812,
+            "heading": -3.0120810880108007,
+            "angularVelocity": -1.4349749048108316,
+            "velocityX": 0.1517260954387245,
+            "velocityY": -1.6698584687633535,
+            "timestamp": 2.8730876412478517
+        },
+        {
+            "x": 7.6714018244195765,
+            "y": 0.8824389383291659,
+            "heading": -3.0544961336196454,
+            "angularVelocity": -1.231947449807498,
+            "velocityX": 0.12585448851644476,
+            "velocityY": -1.3894674580185522,
+            "timestamp": 2.9075169065671456
+        },
+        {
+            "x": 7.674853405168496,
+            "y": 0.8442193530437802,
+            "heading": -3.0892755187362573,
+            "angularVelocity": -1.0101692497377945,
+            "velocityX": 0.10025136223247667,
+            "velocityY": -1.1100900623623653,
+            "timestamp": 2.9419461718864395
+        },
+        {
+            "x": 7.677432109437281,
+            "y": 0.8155891364108041,
+            "heading": -3.1159120874216906,
+            "angularVelocity": -0.773660676125647,
+            "velocityX": 0.07489861444537417,
+            "velocityY": -0.8315662959247566,
+            "timestamp": 2.9763754372057334
+        },
+        {
+            "x": 7.679145555938743,
+            "y": 0.7965232635297996,
+            "heading": -3.1339959816499734,
+            "angularVelocity": -0.525247752473809,
+            "velocityX": 0.04976715261198055,
+            "velocityY": -0.5537693791659258,
+            "timestamp": 3.0108047025250273
         },
         {
             "x": 7.68,
             "y": 0.787,
             "heading": -3.143185307179586,
-            "angularVelocity": -1.3567521418700865,
-            "velocityX": 0.01347301080414647,
-            "velocityY": -0.2114541831120807,
-            "timestamp": 3.5806795793441966
+            "angularVelocity": -0.2669044908275344,
+            "velocityX": 0.024817377116055982,
+            "velocityY": -0.2766037393328496,
+            "timestamp": 3.045233967844321
         },
         {
             "x": 7.68,
             "y": 0.787,
             "heading": -3.143185307179586,
-            "angularVelocity": 1.5420512157892309e-32,
-            "velocityX": -2.431181718338826e-33,
-            "velocityY": 1.2990424762087674e-33,
-            "timestamp": 3.625034157217093
+            "angularVelocity": -4.433819854347959e-31,
+            "velocityX": 0,
+            "velocityY": 2.2179802932100695e-31,
+            "timestamp": 3.079663233163615
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.traj
index 9e5becca..19f07e68 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.traj	
@@ -5,675 +5,657 @@
             "y": 6.72,
             "heading": -2.1,
             "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
+            "velocityX": 3.044182207072049e-31,
+            "velocityY": 6.458172299259412e-31,
             "timestamp": 0
         },
         {
-            "x": 0.7185162341797315,
-            "y": 6.716270626486824,
-            "heading": -2.0163378917371135,
-            "angularVelocity": 1.6978541106369687,
-            "velocityX": 0.17283001240859905,
-            "velocityY": -0.07568458745445246,
-            "timestamp": 0.049275204352804955
-        },
-        {
-            "x": 0.7365174505438988,
-            "y": 6.709427263698689,
-            "heading": -1.8516035946552112,
-            "angularVelocity": 3.3431479228867977,
-            "velocityX": 0.3653199738205181,
-            "velocityY": -0.1388804547442707,
-            "timestamp": 0.09855040870560991
-        },
-        {
-            "x": 0.7651610538016371,
-            "y": 6.700606345332281,
-            "heading": -1.6098229565949118,
-            "angularVelocity": 4.906740443513477,
-            "velocityX": 0.5812985178641444,
-            "velocityY": -0.1790133289605956,
-            "timestamp": 0.14782561305841485
-        },
-        {
-            "x": 0.8095297793953646,
-            "y": 6.688220030792632,
-            "heading": -1.3224284097332135,
-            "angularVelocity": 5.832437442653418,
-            "velocityX": 0.9004270236212996,
-            "velocityY": -0.2513701303187648,
-            "timestamp": 0.19710081741121982
-        },
-        {
-            "x": 0.8714701737870825,
-            "y": 6.666054185298926,
-            "heading": -1.026684817124186,
-            "angularVelocity": 6.001874502468547,
-            "velocityX": 1.2570296806530035,
-            "velocityY": -0.4498377182771496,
-            "timestamp": 0.2463760217640248
-        },
-        {
-            "x": 0.9484340695976169,
-            "y": 6.633599911893754,
-            "heading": -0.7648347305022277,
-            "angularVelocity": 5.314033499427846,
-            "velocityX": 1.5619193633268658,
-            "velocityY": -0.6586329540675864,
-            "timestamp": 0.29565122611682976
-        },
-        {
-            "x": 1.0425919145867164,
-            "y": 6.594089868440422,
-            "heading": -0.5334353297349962,
-            "angularVelocity": 4.696061717176001,
-            "velocityX": 1.9108565093903993,
-            "velocityY": -0.8018240405548321,
-            "timestamp": 0.3449264304696347
-        },
-        {
-            "x": 1.155214051879495,
-            "y": 6.555173696166533,
-            "heading": -0.3327017892852748,
-            "angularVelocity": 4.073723145062813,
-            "velocityX": 2.285574230934019,
-            "velocityY": -0.7897719103355402,
-            "timestamp": 0.3942016348224397
-        },
-        {
-            "x": 1.2837470785266114,
-            "y": 6.519808002504125,
-            "heading": -0.17776538289371885,
-            "angularVelocity": 3.1443077390857384,
-            "velocityX": 2.6084727265022423,
-            "velocityY": -0.7177178487012317,
-            "timestamp": 0.44347683917524466
-        },
-        {
-            "x": 1.4286059942579405,
-            "y": 6.483992998763445,
-            "heading": -0.07145024720477738,
-            "angularVelocity": 2.1575787880601562,
-            "velocityX": 2.9397933024114007,
-            "velocityY": -0.7268362295212014,
-            "timestamp": 0.49275204352804963
-        },
-        {
-            "x": 1.5885837184337814,
-            "y": 6.442409682842055,
-            "heading": -0.012288104865979272,
-            "angularVelocity": 1.2006473258883674,
-            "velocityX": 3.246617163277867,
-            "velocityY": -0.8438994108204557,
-            "timestamp": 0.5420272478808545
-        },
-        {
-            "x": 1.7606060854656245,
-            "y": 6.389861829992508,
-            "heading": 0.005689657356259434,
-            "angularVelocity": 0.3648439911790104,
-            "velocityX": 3.491053346023324,
-            "velocityY": -1.0664157265246377,
-            "timestamp": 0.5913024522336595
-        },
-        {
-            "x": 1.937365788040802,
-            "y": 6.330680677458663,
-            "heading": 0.005693236516864234,
-            "angularVelocity": 0.00007263613924707546,
-            "velocityX": 3.587193699078299,
-            "velocityY": -1.2010331222599455,
-            "timestamp": 0.6405776565864645
-        },
-        {
-            "x": 2.11412543100989,
-            "y": 6.27149934643189,
-            "heading": 0.005696815267395046,
-            "angularVelocity": 0.00007262781713070066,
-            "velocityX": 3.5871924894214278,
-            "velocityY": -1.20103674462801,
-            "timestamp": 0.6898528609392695
-        },
-        {
-            "x": 2.2908850692554568,
-            "y": 6.212318001247489,
-            "heading": 0.005700393968496334,
-            "angularVelocity": 0.00007262681399886251,
-            "velocityX": 3.5871923935614243,
-            "velocityY": -1.2010370319454915,
-            "timestamp": 0.7391280652920744
-        },
-        {
-            "x": 2.467644702239147,
-            "y": 6.153136640294885,
-            "heading": 0.005703972617858713,
-            "angularVelocity": 0.00007262576400001342,
-            "velocityX": 3.5871922867759345,
-            "velocityY": -1.2010373519482989,
-            "timestamp": 0.7884032696448794
-        },
-        {
-            "x": 2.644404329357872,
-            "y": 6.0939552617700175,
-            "heading": 0.005707551213044099,
-            "angularVelocity": 0.00007262466452220377,
-            "velocityX": 3.5871921677512737,
-            "velocityY": -1.2010377085630215,
-            "timestamp": 0.8376784739976844
-        },
-        {
-            "x": 2.821163949952127,
-            "y": 6.0347738637002735,
-            "heading": 0.00571112975156944,
-            "angularVelocity": 0.00007262351465291134,
-            "velocityX": 3.5871920353424867,
-            "velocityY": -1.201038105210319,
-            "timestamp": 0.8869536783504893
-        },
-        {
-            "x": 2.9979235633249197,
-            "y": 5.975592444001196,
-            "heading": 0.005714708231053879,
-            "angularVelocity": 0.0000726223164660635,
-            "velocityX": 3.587191888788799,
-            "velocityY": -1.2010385441599662,
-            "timestamp": 0.9362288827032943
-        },
-        {
-            "x": 3.174683168778421,
-            "y": 5.916411000586595,
-            "heading": 0.005718286649774776,
-            "angularVelocity": 0.0000726210833196382,
-            "velocityX": 3.587191728073259,
-            "velocityY": -1.2010390254471126,
-            "timestamp": 0.9855040870560993
-        },
-        {
-            "x": 3.3514425126695637,
-            "y": 5.8572287763527235,
-            "heading": 0.005721865395381687,
-            "angularVelocity": 0.00007262771720412379,
-            "velocityX": 3.5871864198789636,
-            "velocityY": -1.2010548715360532,
-            "timestamp": 1.0347792914089042
-        },
-        {
-            "x": 3.5167193258876743,
-            "y": 5.790065395689035,
-            "heading": 0.026683569824856986,
-            "angularVelocity": 0.4254006595161298,
-            "velocityX": 3.354157844475837,
-            "velocityY": -1.3630259183261217,
-            "timestamp": 1.084054495761709
-        },
-        {
-            "x": 3.666994209555769,
-            "y": 5.710547155721769,
-            "heading": 0.06778616903847971,
-            "angularVelocity": 0.8341436581233173,
-            "velocityX": 3.0497059452487165,
-            "velocityY": -1.6137576903369897,
-            "timestamp": 1.133329700114514
-        },
-        {
-            "x": 3.802461838655518,
-            "y": 5.617737963132636,
-            "heading": 0.12219899767661042,
-            "angularVelocity": 1.104263885920004,
-            "velocityX": 2.749204815667845,
-            "velocityY": -1.8834867111788967,
-            "timestamp": 1.1826049044673188
-        },
-        {
-            "x": 3.923178434371948,
-            "y": 5.511256217956543,
-            "heading": 0.18485588032163774,
-            "angularVelocity": 1.2715702241722033,
-            "velocityX": 2.4498446490878596,
-            "velocityY": -2.1609599914329216,
-            "timestamp": 1.2318801088201237
-        },
-        {
-            "x": 4.036364016344018,
-            "y": 5.380109892116248,
-            "heading": 0.2569375572143931,
-            "angularVelocity": 1.3544496582801502,
-            "velocityX": 2.1268119643276364,
-            "velocityY": -2.4643030500437595,
-            "timestamp": 1.2850985333308458
-        },
-        {
-            "x": 4.130923397861632,
-            "y": 5.234522406039544,
-            "heading": 0.33431047652627577,
-            "angularVelocity": 1.4538746688431972,
-            "velocityX": 1.7768166267034577,
-            "velocityY": -2.7356594528154665,
-            "timestamp": 1.3383169578415677
-        },
-        {
-            "x": 4.205236217563619,
-            "y": 5.077100065980883,
-            "heading": 0.4216368931025466,
-            "angularVelocity": 1.640905708485922,
-            "velocityX": 1.396373913455762,
-            "velocityY": -2.958042097374491,
-            "timestamp": 1.3915353823522896
-        },
-        {
-            "x": 4.2828034673506234,
-            "y": 4.901292660688841,
-            "heading": 0.4516847731002138,
-            "angularVelocity": 0.5646142341476765,
-            "velocityX": 1.4575262326936578,
-            "velocityY": -3.303506387277969,
-            "timestamp": 1.4447538068630115
-        },
-        {
-            "x": 4.371277738622277,
-            "y": 4.723874668487275,
-            "heading": 0.4413575215812008,
-            "angularVelocity": -0.19405406330532018,
-            "velocityX": 1.6624744547600938,
-            "velocityY": -3.333770096215863,
-            "timestamp": 1.4979722313737334
-        },
-        {
-            "x": 4.466886857140796,
-            "y": 4.565900844635089,
-            "heading": 0.39184092001837434,
-            "angularVelocity": -0.9304409519460731,
-            "velocityX": 1.796541693925137,
-            "velocityY": -2.968404745246797,
-            "timestamp": 1.5511906558844553
-        },
-        {
-            "x": 4.573967443469787,
-            "y": 4.427674107970891,
-            "heading": 0.321042696188799,
-            "angularVelocity": -1.330332952928953,
-            "velocityX": 2.012096136130751,
-            "velocityY": -2.5973474024273386,
-            "timestamp": 1.6044090803951772
-        },
-        {
-            "x": 4.693065628013639,
-            "y": 4.309440309299518,
-            "heading": 0.2371568341960054,
-            "angularVelocity": -1.5762560196777895,
-            "velocityX": 2.2379126334312645,
-            "velocityY": -2.221670403781889,
-            "timestamp": 1.657627504905899
-        },
-        {
-            "x": 4.8242268562316895,
-            "y": 4.211382865905762,
-            "heading": 0.14523841868535878,
-            "angularVelocity": -1.7271915949358416,
-            "velocityX": 2.464583072947316,
-            "velocityY": -1.8425469054237094,
-            "timestamp": 1.710845929416621
-        },
-        {
-            "x": 4.982290420935857,
-            "y": 4.128393361272933,
-            "heading": 0.04004394713112955,
-            "angularVelocity": -1.8026614827604082,
-            "velocityX": 2.7086508987606095,
-            "velocityY": -1.4221468225908107,
-            "timestamp": 1.7692010185779787
-        },
-        {
-            "x": 5.153780525115527,
-            "y": 4.07042189977071,
-            "heading": -0.06589739756263054,
-            "angularVelocity": -1.8154602489051344,
-            "velocityX": 2.9387343356717652,
-            "velocityY": -0.9934259776713893,
-            "timestamp": 1.8275561077393365
-        },
-        {
-            "x": 5.3372218147923,
-            "y": 4.038159807995649,
-            "heading": -0.16628826181080308,
-            "angularVelocity": -1.7203446295931681,
-            "velocityX": 3.143535419328013,
-            "velocityY": -0.5528582380510605,
-            "timestamp": 1.8859111969006943
-        },
-        {
-            "x": 5.52929807317918,
-            "y": 4.032409431901968,
-            "heading": -0.2472171752765858,
-            "angularVelocity": -1.3868355721641668,
-            "velocityX": 3.2915082668414595,
-            "velocityY": -0.0985411242844827,
-            "timestamp": 1.944266286062052
-        },
-        {
-            "x": 5.726840626298333,
-            "y": 4.053774421636682,
-            "heading": -0.3047520267300494,
-            "angularVelocity": -0.985944024425596,
-            "velocityX": 3.385181240541464,
-            "velocityY": 0.3661204196884676,
-            "timestamp": 2.00262137522341
-        },
-        {
-            "x": 5.927911696641962,
-            "y": 4.1025958147259995,
-            "heading": -0.3393222015882671,
-            "angularVelocity": -0.5924106252777295,
-            "velocityX": 3.4456475559080273,
-            "velocityY": 0.8366261416261735,
-            "timestamp": 2.060976464384768
-        },
-        {
-            "x": 6.112967212419423,
-            "y": 4.131783461785041,
-            "heading": -0.42258796983730795,
-            "angularVelocity": -1.4268810046507232,
-            "velocityX": 3.1711975499816933,
-            "velocityY": 0.500173120776742,
-            "timestamp": 2.119331553546126
-        },
-        {
-            "x": 6.288399006454155,
-            "y": 4.135519035273829,
-            "heading": -0.5357406933321943,
-            "angularVelocity": -1.9390377963780898,
-            "velocityX": 3.0062809697650312,
-            "velocityY": 0.06401452799529236,
-            "timestamp": 2.177686642707484
-        },
-        {
-            "x": 6.454763850529513,
-            "y": 4.112891662498818,
-            "heading": -0.6682840950132551,
-            "angularVelocity": -2.2713254933869544,
-            "velocityX": 2.850905490270817,
-            "velocityY": -0.3877532037084742,
-            "timestamp": 2.236041731868842
-        },
-        {
-            "x": 6.611553192138672,
-            "y": 4.063669681549072,
-            "heading": -0.8104452528667577,
-            "angularVelocity": -2.436139844811345,
-            "velocityX": 2.686815218045847,
-            "velocityY": -0.8434908018671928,
-            "timestamp": 2.2943968210302
-        },
-        {
-            "x": 6.724757817470915,
-            "y": 4.010978330480739,
-            "heading": -0.9191384597303142,
-            "angularVelocity": -2.4505521656643996,
-            "velocityX": 2.5522647438252197,
-            "velocityY": -1.1879574464517886,
-            "timestamp": 2.3387513989030966
-        },
-        {
-            "x": 6.831367598021963,
-            "y": 3.9432896977899077,
-            "heading": -1.0248995406249544,
-            "angularVelocity": -2.384445664159214,
-            "velocityX": 2.40358009620903,
-            "velocityY": -1.526079965968843,
-            "timestamp": 2.383105976775993
-        },
-        {
-            "x": 6.930834934158794,
-            "y": 3.86094565265281,
-            "heading": -1.1241964811981286,
-            "angularVelocity": -2.238707825328021,
-            "velocityX": 2.2425494933548396,
-            "velocityY": -1.856494844186423,
-            "timestamp": 2.4274605546488894
-        },
-        {
-            "x": 7.022708800988577,
-            "y": 3.764327451214886,
-            "heading": -1.2136436923328575,
-            "angularVelocity": -2.016639892077224,
-            "velocityX": 2.0713502694819663,
-            "velocityY": -2.1783140787585773,
-            "timestamp": 2.471815132521786
-        },
-        {
-            "x": 7.106642839511368,
-            "y": 3.6538333663708773,
-            "heading": -1.2900644922719695,
-            "angularVelocity": -1.7229518034892648,
-            "velocityX": 1.892342178597992,
-            "velocityY": -2.4911540170812514,
-            "timestamp": 2.5161697103946823
-        },
-        {
-            "x": 7.182389770268718,
-            "y": 3.5298642170672982,
-            "heading": -1.3504994507692183,
-            "angularVelocity": -1.3625416224325844,
-            "velocityX": 1.7077590271383551,
-            "velocityY": -2.794957256922336,
-            "timestamp": 2.5605242882675787
-        },
-        {
-            "x": 7.249751518053722,
-            "y": 3.39283877736109,
-            "heading": -1.392210624169865,
-            "angularVelocity": -0.9404028941539149,
-            "velocityX": 1.518710153843384,
-            "velocityY": -3.0893189897753475,
-            "timestamp": 2.604878866140475
-        },
-        {
-            "x": 7.30834760453933,
-            "y": 3.243309493178288,
-            "heading": -1.412928953137589,
-            "angularVelocity": -0.46710689090751717,
-            "velocityX": 1.3210831732751818,
-            "velocityY": -3.371225504868904,
-            "timestamp": 2.6492334440133716
-        },
-        {
-            "x": 7.356500151413166,
-            "y": 3.0825770019538052,
-            "heading": -1.4129301457519094,
-            "angularVelocity": -0.000026888190071743586,
-            "velocityX": 1.0856274410236224,
-            "velocityY": -3.623808385350466,
-            "timestamp": 2.693588021886268
-        },
-        {
-            "x": 7.388728975781619,
-            "y": 2.917910705433503,
-            "heading": -1.4129305799992817,
-            "angularVelocity": -0.00000979036196658053,
-            "velocityX": 0.7266177678617259,
-            "velocityY": -3.7124983353955976,
-            "timestamp": 2.7379425997591644
-        },
-        {
-            "x": 7.4166210769822385,
-            "y": 2.7524546215220593,
-            "heading": -1.4129310308638345,
-            "angularVelocity": -0.000010165006058865363,
-            "velocityX": 0.6288437978273086,
-            "velocityY": -3.73030455583591,
-            "timestamp": 2.782297177632061
-        },
-        {
-            "x": 7.4445129710491305,
-            "y": 2.5869985026929103,
-            "heading": -1.412931481728963,
-            "angularVelocity": -0.000010165019038462572,
-            "velocityX": 0.628839127875817,
-            "velocityY": -3.730305343076067,
-            "timestamp": 2.8266517555049573
-        },
-        {
-            "x": 7.4724048295219,
-            "y": 2.4215423778635246,
-            "heading": -1.4129319325942993,
-            "angularVelocity": -0.000010165023720794904,
-            "velocityX": 0.6288383253854289,
-            "velocityY": -3.7303054783549245,
-            "timestamp": 2.8710063333778537
-        },
-        {
-            "x": 7.500296654307517,
-            "y": 2.2560862473554004,
-            "heading": -1.4129323834599168,
-            "angularVelocity": -0.000010165030059818064,
-            "velocityX": 0.6288375658887922,
-            "velocityY": -3.73030560638543,
-            "timestamp": 2.91536091125075
-        },
-        {
-            "x": 7.528188447597349,
-            "y": 2.090630111537975,
-            "heading": -1.4129328343258867,
-            "angularVelocity": -0.000010165038009449021,
-            "velocityX": 0.6288368557978095,
-            "velocityY": -3.7303057260867334,
-            "timestamp": 2.9597154891236466
-        },
-        {
-            "x": 7.55608021118008,
-            "y": 1.9251739707128015,
-            "heading": -1.4129332851922731,
-            "angularVelocity": -0.000010165047399002056,
-            "velocityX": 0.6288361860338073,
-            "velocityY": -3.7303058389893544,
-            "timestamp": 3.004070066996543
-        },
-        {
-            "x": 7.583956606011812,
-            "y": 1.7597152400598883,
-            "heading": -1.412933736829767,
-            "angularVelocity": -0.000010182432471034155,
-            "velocityX": 0.6284896885190855,
-            "velocityY": -3.730364228176306,
-            "timestamp": 3.0484246448694394
-        },
-        {
-            "x": 7.603176305958169,
-            "y": 1.6036265149225075,
-            "heading": -1.4482789360266273,
-            "angularVelocity": -0.7968782680819577,
-            "velocityX": 0.4333194197323582,
-            "velocityY": -3.5191119524273815,
-            "timestamp": 3.092779222742336
-        },
-        {
-            "x": 7.616234477472668,
-            "y": 1.4592226194785036,
-            "heading": -1.5202643791491903,
-            "angularVelocity": -1.6229540799339002,
-            "velocityX": 0.2944041436245525,
-            "velocityY": -3.2556706064887218,
-            "timestamp": 3.1371338006152323
-        },
-        {
-            "x": 7.626690890740831,
-            "y": 1.32844105514062,
-            "heading": -1.6264074068747738,
-            "angularVelocity": -2.393056879714865,
-            "velocityX": 0.23574597639338804,
-            "velocityY": -2.9485471536366448,
-            "timestamp": 3.1814883784881287
-        },
-        {
-            "x": 7.6362242266749725,
-            "y": 1.2119757666324047,
-            "heading": -1.7630703603278275,
-            "angularVelocity": -3.0811465243718166,
-            "velocityX": 0.2149346559324737,
-            "velocityY": -2.6257783095571594,
-            "timestamp": 3.225842956361025
-        },
-        {
-            "x": 7.645916498443832,
-            "y": 1.110010534092232,
-            "heading": -1.9285530962892936,
-            "angularVelocity": -3.7309054419518417,
-            "velocityX": 0.21851795764202436,
-            "velocityY": -2.298866034355394,
-            "timestamp": 3.2701975342339216
-        },
-        {
-            "x": 7.656628280193831,
-            "y": 1.0222888536217107,
-            "heading": -2.123550329375603,
-            "angularVelocity": -4.396327108446363,
-            "velocityX": 0.2415034087506259,
-            "velocityY": -1.9777367901437044,
-            "timestamp": 3.314552112106818
-        },
-        {
-            "x": 7.664811531196715,
-            "y": 0.9478358295186623,
-            "heading": -2.3514420654429586,
-            "angularVelocity": -5.137952991468154,
-            "velocityX": 0.18449619848336826,
-            "velocityY": -1.6785871419271048,
-            "timestamp": 3.3589066899797144
-        },
-        {
-            "x": 7.671315625708808,
-            "y": 0.8895973140434956,
-            "heading": -2.585878637991588,
-            "angularVelocity": -5.285510172601276,
-            "velocityX": 0.146638629517147,
-            "velocityY": -1.313021524904517,
-            "timestamp": 3.403261267852611
-        },
-        {
-            "x": 7.675675621987506,
-            "y": 0.8463977620596795,
-            "heading": -2.797067073731653,
-            "angularVelocity": -4.761367278598646,
-            "velocityX": 0.0982986759831574,
-            "velocityY": -0.9739592631815782,
-            "timestamp": 3.4476158457255073
-        },
-        {
-            "x": 7.67812133477644,
-            "y": 0.8158415850765369,
-            "heading": -2.965913260384183,
-            "angularVelocity": -3.8067364125610696,
-            "velocityX": 0.05514003077524084,
-            "velocityY": -0.6889069505002431,
-            "timestamp": 3.4919704235984037
-        },
-        {
-            "x": 7.679402410293105,
-            "y": 0.7963789610313945,
-            "heading": -3.08300713864879,
-            "angularVelocity": -2.6399502346782358,
-            "velocityX": 0.02888259967968837,
-            "velocityY": -0.4387962861672373,
-            "timestamp": 3.5363250014713
+            "x": 0.7256414580836799,
+            "y": 6.712155644836975,
+            "heading": -2.1163027262729814,
+            "angularVelocity": -0.3503504178414695,
+            "velocityX": 0.3361395685300024,
+            "velocityY": -0.16857751660924442,
+            "timestamp": 0.04653262974092379
+        },
+        {
+            "x": 0.7569790078652245,
+            "y": 6.696436652979446,
+            "heading": -2.1479259826479233,
+            "angularVelocity": -0.6795931489587892,
+            "velocityX": 0.6734532296158987,
+            "velocityY": -0.3378057923018942,
+            "timestamp": 0.09306525948184757
+        },
+        {
+            "x": 0.8040786472875676,
+            "y": 6.672808979329537,
+            "heading": -2.1936219074673273,
+            "angularVelocity": -0.9820189633351775,
+            "velocityX": 1.0121852060495677,
+            "velocityY": -0.5077657072349027,
+            "timestamp": 0.13959788922277136
+        },
+        {
+            "x": 0.8670199327112733,
+            "y": 6.641236276400096,
+            "heading": -2.2517861808528257,
+            "angularVelocity": -1.2499674681912987,
+            "velocityX": 1.3526268722430308,
+            "velocityY": -0.6785067404360078,
+            "timestamp": 0.18613051896369515
+        },
+        {
+            "x": 0.9458962124147239,
+            "y": 6.601679900322021,
+            "heading": -2.32031104809549,
+            "angularVelocity": -1.4726196998587933,
+            "velocityX": 1.6950746205965188,
+            "velocityY": -0.8500782418336558,
+            "timestamp": 0.23266314870461893
+        },
+        {
+            "x": 1.0408105422873288,
+            "y": 6.554096827806623,
+            "heading": -2.396269750321632,
+            "angularVelocity": -1.6323750161792245,
+            "velocityX": 2.039737070547226,
+            "velocityY": -1.022574326452373,
+            "timestamp": 0.2791957784455427
+        },
+        {
+            "x": 1.1518552587221624,
+            "y": 6.498443176244561,
+            "heading": -2.4750808863817895,
+            "angularVelocity": -1.6936746644023601,
+            "velocityX": 2.3863838569428775,
+            "velocityY": -1.19601346134745,
+            "timestamp": 0.32572840818646653
+        },
+        {
+            "x": 1.2789760591669448,
+            "y": 6.434740942054882,
+            "heading": -2.5479819963693036,
+            "angularVelocity": -1.5666664530556718,
+            "velocityX": 2.7318636654010686,
+            "velocityY": -1.3689798866801088,
+            "timestamp": 0.3722610379273903
+        },
+        {
+            "x": 1.4202169645474811,
+            "y": 6.363052674678261,
+            "heading": -2.59222819945715,
+            "angularVelocity": -0.9508640137943395,
+            "velocityX": 3.0353089040296184,
+            "velocityY": -1.5406021060002604,
+            "timestamp": 0.41879366766831405
+        },
+        {
+            "x": 1.573995110361942,
+            "y": 6.28311594811204,
+            "heading": -2.599064012600599,
+            "angularVelocity": -0.14690364979431952,
+            "velocityX": 3.304737915536797,
+            "velocityY": -1.7178639378700842,
+            "timestamp": 0.4653262974092378
+        },
+        {
+            "x": 1.7314466383055478,
+            "y": 6.204403629832344,
+            "heading": -2.599064059608806,
+            "angularVelocity": -0.0000010102203007383576,
+            "velocityX": 3.3836799858560402,
+            "velocityY": -1.6915510410205123,
+            "timestamp": 0.5118589271501616
+        },
+        {
+            "x": 1.8889032791179137,
+            "y": 6.1257015398581975,
+            "heading": -2.5990641066080484,
+            "angularVelocity": -0.0000010100276450499987,
+            "velocityX": 3.3837898629208136,
+            "velocityY": -1.69133123170322,
+            "timestamp": 0.5583915568910853
+        },
+        {
+            "x": 2.0463595988203984,
+            "y": 6.046998807454145,
+            "heading": -2.5990641536073187,
+            "angularVelocity": -0.0000010100282384452147,
+            "velocityX": 3.3837829621742297,
+            "velocityY": -1.691345037712453,
+            "timestamp": 0.6049241866320091
+        },
+        {
+            "x": 2.2038159116861142,
+            "y": 5.968296061372362,
+            "heading": -2.599064200606589,
+            "angularVelocity": -0.0000010100282469186477,
+            "velocityX": 3.3837828152499863,
+            "velocityY": -1.6913453316510296,
+            "timestamp": 0.6514568163729328
+        },
+        {
+            "x": 2.3612722547062472,
+            "y": 5.889593375618833,
+            "heading": -2.5990642476058565,
+            "angularVelocity": -0.0000010100281851545483,
+            "velocityX": 3.3837834632773647,
+            "velocityY": -1.6913440351789604,
+            "timestamp": 0.6979894461138566
+        },
+        {
+            "x": 2.5187286297414215,
+            "y": 5.8108907539159365,
+            "heading": -2.599064294605121,
+            "angularVelocity": -0.0000010100281210334782,
+            "velocityX": 3.383784151290135,
+            "velocityY": -1.6913426587117795,
+            "timestamp": 0.7445220758547804
+        },
+        {
+            "x": 2.6761850576997652,
+            "y": 5.732188238093708,
+            "heading": -2.599064341604381,
+            "angularVelocity": -0.000001010028016565385,
+            "velocityX": 3.3837852886247464,
+            "velocityY": -1.6913403833048126,
+            "timestamp": 0.7910547055957041
+        },
+        {
+            "x": 2.83364151622281,
+            "y": 5.653485783421201,
+            "heading": -2.5990643886036375,
+            "angularVelocity": -0.0000010100279552603614,
+            "velocityX": 3.3837859454691737,
+            "velocityY": -1.6913390691793195,
+            "timestamp": 0.8375873353366279
+        },
+        {
+            "x": 2.9910977149954676,
+            "y": 5.574782809080547,
+            "heading": -2.5990644356029167,
+            "angularVelocity": -0.000001010028434200447,
+            "velocityX": 3.3837803633561943,
+            "velocityY": -1.691350237002125,
+            "timestamp": 0.8841199650775516
+        },
+        {
+            "x": 3.1485530895378893,
+            "y": 5.496078185774728,
+            "heading": -2.599064482602267,
+            "angularVelocity": -0.0000010100299667197812,
+            "velocityX": 3.3837626504042433,
+            "velocityY": -1.6913856737520705,
+            "timestamp": 0.9306525948184754
+        },
+        {
+            "x": 3.3060077005580313,
+            "y": 5.417372034978031,
+            "heading": -2.5990645296016837,
+            "angularVelocity": -0.0000010100313828443218,
+            "velocityX": 3.383746242085582,
+            "velocityY": -1.6914184999808708,
+            "timestamp": 0.9771852245593992
+        },
+        {
+            "x": 3.463466207634218,
+            "y": 5.3386736786731355,
+            "heading": -2.5990645766007607,
+            "angularVelocity": -0.0000010100240886605546,
+            "velocityX": 3.383829969488029,
+            "velocityY": -1.6912509940353067,
+            "timestamp": 1.023717854300323
+        },
+        {
+            "x": 3.620938146457774,
+            "y": 5.260002202075332,
+            "heading": -2.5990646235986756,
+            "angularVelocity": -0.0000010099991153726974,
+            "velocityX": 3.3841186217193853,
+            "velocityY": -1.690673341164269,
+            "timestamp": 1.0702504840412468
+        },
+        {
+            "x": 3.7783857783864416,
+            "y": 5.181282090499446,
+            "heading": -2.5990646705986817,
+            "angularVelocity": -0.0000010100440573860445,
+            "velocityX": 3.383596259333376,
+            "velocityY": -1.691718521279299,
+            "timestamp": 1.1167831137821707
+        },
+        {
+            "x": 3.9357445242921885,
+            "y": 5.102384448798495,
+            "heading": -2.599064717606569,
+            "angularVelocity": -0.0000010102134222869035,
+            "velocityX": 3.381686072364613,
+            "velocityY": -1.6955336962675904,
+            "timestamp": 1.1633157435230945
+        },
+        {
+            "x": 4.092995643615723,
+            "y": 5.023272514343262,
+            "heading": -2.5990647646424785,
+            "angularVelocity": -0.000001010815630420508,
+            "velocityX": 3.379373145230901,
+            "velocityY": -1.700138911036495,
+            "timestamp": 1.2098483732640184
+        },
+        {
+            "x": 4.261229917640389,
+            "y": 4.916068911394467,
+            "heading": -2.599064809945939,
+            "angularVelocity": -8.591015508014648e-7,
+            "velocityX": 3.1902711911232386,
+            "velocityY": -2.0329303767326627,
+            "timestamp": 1.2625819071694577
+        },
+        {
+            "x": 4.429297438166161,
+            "y": 4.8086040706119935,
+            "heading": -2.5990648552466906,
+            "angularVelocity": -8.590501714319945e-7,
+            "velocityX": 3.187109000265223,
+            "velocityY": -2.0378842991085575,
+            "timestamp": 1.315315441074897
+        },
+        {
+            "x": 4.597219271125054,
+            "y": 4.700911724294517,
+            "heading": -2.599064900558894,
+            "angularVelocity": -8.592673362873697e-7,
+            "velocityX": 3.184346288265075,
+            "velocityY": -2.0421985469549115,
+            "timestamp": 1.3680489749803364
+        },
+        {
+            "x": 4.765178923625143,
+            "y": 4.59327837178587,
+            "heading": -2.5990649458681174,
+            "angularVelocity": -8.592108353148986e-7,
+            "velocityX": 3.1850634702628065,
+            "velocityY": -2.0410798317000203,
+            "timestamp": 1.4207825088857757
+        },
+        {
+            "x": 4.93313403793343,
+            "y": 4.485637937706628,
+            "heading": -2.599064991177699,
+            "angularVelocity": -8.592176153657431e-7,
+            "velocityX": 3.184977411326241,
+            "velocityY": -2.041214121404157,
+            "timestamp": 1.473516042791215
+        },
+        {
+            "x": 5.101087827105617,
+            "y": 4.377995436130629,
+            "heading": -2.599065036487385,
+            "angularVelocity": -8.592196078701955e-7,
+            "velocityX": 3.184952282419374,
+            "velocityY": -2.041253327892839,
+            "timestamp": 1.5262495766966544
+        },
+        {
+            "x": 5.269016802174646,
+            "y": 4.270314227125004,
+            "heading": -2.599065081799025,
+            "angularVelocity": -8.592566538542498e-7,
+            "velocityX": 3.1844817259951634,
+            "velocityY": -2.041987347154346,
+            "timestamp": 1.5789831106020937
+        },
+        {
+            "x": 5.437055950944385,
+            "y": 4.162805027649924,
+            "heading": -2.599065127101977,
+            "angularVelocity": -8.590919076920599e-7,
+            "velocityX": 3.1865709791255945,
+            "velocityY": -2.038725484769403,
+            "timestamp": 1.631716644507533
+        },
+        {
+            "x": 5.605213465892237,
+            "y": 4.055481061993006,
+            "heading": -2.599065172406386,
+            "angularVelocity": -8.591195260284143e-7,
+            "velocityX": 3.188815588376687,
+            "velocityY": -2.035212846712725,
+            "timestamp": 1.6844501784129724
+        },
+        {
+            "x": 5.7832255363464355,
+            "y": 3.9654417037963863,
+            "heading": -2.599065217938776,
+            "angularVelocity": -8.634427952488677e-7,
+            "velocityX": 3.3756901400427615,
+            "velocityY": -1.707440247757134,
+            "timestamp": 1.7371837123184117
+        },
+        {
+            "x": 5.945200300467497,
+            "y": 3.903126168683373,
+            "heading": -2.599065262330506,
+            "angularVelocity": -9.676334890696444e-7,
+            "velocityX": 3.5306622744884395,
+            "velocityY": -1.3583295529523924,
+            "timestamp": 1.7830603075655527
+        },
+        {
+            "x": 6.112434090508488,
+            "y": 3.856737486793876,
+            "heading": -2.5990653054733555,
+            "angularVelocity": -9.404108896808373e-7,
+            "velocityX": 3.6452964554166534,
+            "velocityY": -1.01116226345067,
+            "timestamp": 1.8289369028126936
+        },
+        {
+            "x": 6.280097327658818,
+            "y": 3.8119257778274935,
+            "heading": -2.599065348543951,
+            "angularVelocity": -9.38835914358075e-7,
+            "velocityX": 3.6546573747925137,
+            "velocityY": -0.9767880272056341,
+            "timestamp": 1.8748134980598345
+        },
+        {
+            "x": 6.444605868501446,
+            "y": 3.756644766145867,
+            "heading": -2.599065392143275,
+            "angularVelocity": -9.503609402316767e-7,
+            "velocityX": 3.5858925440393525,
+            "velocityY": -1.2049937747953314,
+            "timestamp": 1.9206900933069755
+        },
+        {
+            "x": 6.602719051918594,
+            "y": 3.685095684621415,
+            "heading": -2.5990654382603977,
+            "angularVelocity": -0.0000010052429246959388,
+            "velocityX": 3.446489055375297,
+            "velocityY": -1.5595987701135843,
+            "timestamp": 1.9665666885541164
+        },
+        {
+            "x": 6.752801915063028,
+            "y": 3.597951074081566,
+            "heading": -2.599065489714602,
+            "angularVelocity": -0.0000011215785414644313,
+            "velocityX": 3.2714472888827597,
+            "velocityY": -1.8995439846919482,
+            "timestamp": 2.0124432838012574
+        },
+        {
+            "x": 6.893316861258978,
+            "y": 3.49609530674715,
+            "heading": -2.599065550773928,
+            "angularVelocity": -0.0000013309472018502562,
+            "velocityX": 3.06288959411619,
+            "velocityY": -2.2202120010370177,
+            "timestamp": 2.058319879048398
+        },
+        {
+            "x": 7.022827120260116,
+            "y": 3.380569821579126,
+            "heading": -2.5990656290967817,
+            "angularVelocity": -0.0000017072507862805843,
+            "velocityX": 2.8230137459733338,
+            "velocityY": -2.5181791400535034,
+            "timestamp": 2.104196474295539
+        },
+        {
+            "x": 7.140008449554443,
+            "y": 3.2525558471679688,
+            "heading": -2.5990657325552275,
+            "angularVelocity": -0.000002255146554794306,
+            "velocityX": 2.5542725797993215,
+            "velocityY": -2.790398322315311,
+            "timestamp": 2.1500730695426795
+        },
+        {
+            "x": 7.220411639481594,
+            "y": 3.1500923106303724,
+            "heading": -2.599065847631647,
+            "angularVelocity": -0.000003342401256012092,
+            "velocityX": 2.3353152959116907,
+            "velocityY": -2.9760593375246422,
+            "timestamp": 2.1845023348619734
+        },
+        {
+            "x": 7.292813286750944,
+            "y": 3.0418266490558983,
+            "heading": -2.5990659368308053,
+            "angularVelocity": -0.0000025907947068596104,
+            "velocityX": 2.1029100272080186,
+            "velocityY": -3.1445823943795888,
+            "timestamp": 2.2189316001812673
+        },
+        {
+            "x": 7.356796447286593,
+            "y": 2.9283823501608603,
+            "heading": -2.599066009494182,
+            "angularVelocity": -0.0000021105119622632264,
+            "velocityX": 1.8583945937350983,
+            "velocityY": -3.294996214498541,
+            "timestamp": 2.2533608655005612
+        },
+        {
+            "x": 7.411992663456803,
+            "y": 2.8104127239306367,
+            "heading": -2.5990660711538616,
+            "angularVelocity": -0.0000017909089651975303,
+            "velocityX": 1.603177287064451,
+            "velocityY": -3.426434608353826,
+            "timestamp": 2.287790130819855
+        },
+        {
+            "x": 7.458084080553435,
+            "y": 2.688597137512427,
+            "heading": -2.599066125344508,
+            "angularVelocity": -0.0000015739704607850277,
+            "velocityX": 1.3387278720350797,
+            "velocityY": -3.538140744175163,
+            "timestamp": 2.322219396139149
+        },
+        {
+            "x": 7.494809849686971,
+            "y": 2.5636384472618623,
+            "heading": -2.599066174481706,
+            "angularVelocity": -0.0000014271927566927072,
+            "velocityX": 1.0667020859416116,
+            "velocityY": -3.6294323765468546,
+            "timestamp": 2.356648661458443
+        },
+        {
+            "x": 7.5219737035667364,
+            "y": 2.4362587715435033,
+            "heading": -2.599066220329768,
+            "angularVelocity": -0.0000013316596016825942,
+            "velocityX": 0.7889757050532209,
+            "velocityY": -3.699750039306788,
+            "timestamp": 2.391077926777737
+        },
+        {
+            "x": 7.539529775464085,
+            "y": 2.307203584640024,
+            "heading": -2.5990662642857343,
+            "angularVelocity": -0.0000012767035893272692,
+            "velocityX": 0.5099171223818962,
+            "velocityY": -3.748415358464242,
+            "timestamp": 2.425507192097031
+        },
+        {
+            "x": 7.55111544342237,
+            "y": 2.1774760653156817,
+            "heading": -2.599066307607684,
+            "angularVelocity": -0.0000012582885205698109,
+            "velocityX": 0.3365063950923211,
+            "velocityY": -3.7679432924653384,
+            "timestamp": 2.4599364574163247
+        },
+        {
+            "x": 7.56271571323303,
+            "y": 2.047749850875278,
+            "heading": -2.5990663509314866,
+            "angularVelocity": -0.0000012583423688964546,
+            "velocityX": 0.3369305067394331,
+            "velocityY": -3.767905392035856,
+            "timestamp": 2.4943657227356186
+        },
+        {
+            "x": 7.5743766744470316,
+            "y": 1.9180290778400821,
+            "heading": -2.599066394258993,
+            "angularVelocity": -0.0000012584499309396356,
+            "velocityX": 0.33869329205287857,
+            "velocityY": -3.7677473461074933,
+            "timestamp": 2.5287949880549125
+        },
+        {
+            "x": 7.586111158859003,
+            "y": 1.7883149350094998,
+            "heading": -2.5990664375910546,
+            "angularVelocity": -0.0000012585822270319525,
+            "velocityX": 0.34082877758442714,
+            "velocityY": -3.7675547714315485,
+            "timestamp": 2.5632242533742065
+        },
+        {
+            "x": 7.597929633339714,
+            "y": 1.6586084182080805,
+            "heading": -2.59906648228782,
+            "angularVelocity": -0.0000012982201369267655,
+            "velocityX": 0.3432682739856076,
+            "velocityY": -3.7673332729737243,
+            "timestamp": 2.5976535186935004
+        },
+        {
+            "x": 7.610690036465753,
+            "y": 1.5360359058677395,
+            "heading": -2.6173573406008535,
+            "angularVelocity": -0.5312590362708309,
+            "velocityX": 0.37062664589906397,
+            "velocityY": -3.5601257013070047,
+            "timestamp": 2.6320827840127943
+        },
+        {
+            "x": 7.621968158889347,
+            "y": 1.4215047209508034,
+            "heading": -2.657213150288673,
+            "angularVelocity": -1.1576142946476244,
+            "velocityX": 0.3275737172727147,
+            "velocityY": -3.3265648817881672,
+            "timestamp": 2.666512049332088
+        },
+        {
+            "x": 7.631829723236215,
+            "y": 1.3151734834169941,
+            "heading": -2.7178665355180023,
+            "angularVelocity": -1.76168107762032,
+            "velocityX": 0.28642970610680407,
+            "velocityY": -3.08839693637669,
+            "timestamp": 2.700941314651382
+        },
+        {
+            "x": 7.640728077188491,
+            "y": 1.2186118736781408,
+            "heading": -2.7826314980185702,
+            "angularVelocity": -1.8811020769678346,
+            "velocityX": 0.2584532045561674,
+            "velocityY": -2.8046375327311788,
+            "timestamp": 2.735370579970676
+        },
+        {
+            "x": 7.648685947205252,
+            "y": 1.1318864826516366,
+            "heading": -2.8465688471146957,
+            "angularVelocity": -1.8570639978286094,
+            "velocityX": 0.23113679432217057,
+            "velocityY": -2.518943992043375,
+            "timestamp": 2.76979984528997
+        },
+        {
+            "x": 7.655720752960135,
+            "y": 1.0549563243440254,
+            "heading": -2.9071249866023474,
+            "angularVelocity": -1.7588565694352556,
+            "velocityX": 0.20432633951521018,
+            "velocityY": -2.2344408919030423,
+            "timestamp": 2.804229110609264
+        },
+        {
+            "x": 7.661844928847109,
+            "y": 0.9877692823605362,
+            "heading": -2.962675956286548,
+            "angularVelocity": -1.6134811233710036,
+            "velocityX": 0.17787704239939275,
+            "velocityY": -1.9514515154593546,
+            "timestamp": 2.8386583759285577
+        },
+        {
+            "x": 7.667068746842824,
+            "y": 0.930277282093812,
+            "heading": -3.0120810880108007,
+            "angularVelocity": -1.4349749048108316,
+            "velocityX": 0.1517260954387245,
+            "velocityY": -1.6698584687633535,
+            "timestamp": 2.8730876412478517
+        },
+        {
+            "x": 7.6714018244195765,
+            "y": 0.8824389383291659,
+            "heading": -3.0544961336196454,
+            "angularVelocity": -1.231947449807498,
+            "velocityX": 0.12585448851644476,
+            "velocityY": -1.3894674580185522,
+            "timestamp": 2.9075169065671456
+        },
+        {
+            "x": 7.674853405168496,
+            "y": 0.8442193530437802,
+            "heading": -3.0892755187362573,
+            "angularVelocity": -1.0101692497377945,
+            "velocityX": 0.10025136223247667,
+            "velocityY": -1.1100900623623653,
+            "timestamp": 2.9419461718864395
+        },
+        {
+            "x": 7.677432109437281,
+            "y": 0.8155891364108041,
+            "heading": -3.1159120874216906,
+            "angularVelocity": -0.773660676125647,
+            "velocityX": 0.07489861444537417,
+            "velocityY": -0.8315662959247566,
+            "timestamp": 2.9763754372057334
+        },
+        {
+            "x": 7.679145555938743,
+            "y": 0.7965232635297996,
+            "heading": -3.1339959816499734,
+            "angularVelocity": -0.525247752473809,
+            "velocityX": 0.04976715261198055,
+            "velocityY": -0.5537693791659258,
+            "timestamp": 3.0108047025250273
         },
         {
             "x": 7.68,
             "y": 0.787,
             "heading": -3.143185307179586,
-            "angularVelocity": -1.3567521418700865,
-            "velocityX": 0.01347301080414647,
-            "velocityY": -0.2114541831120807,
-            "timestamp": 3.5806795793441966
+            "angularVelocity": -0.2669044908275344,
+            "velocityX": 0.024817377116055982,
+            "velocityY": -0.2766037393328496,
+            "timestamp": 3.045233967844321
         },
         {
             "x": 7.68,
             "y": 0.787,
             "heading": -3.143185307179586,
-            "angularVelocity": 1.5420512157892309e-32,
-            "velocityX": -2.431181718338826e-33,
-            "velocityY": 1.2990424762087674e-33,
-            "timestamp": 3.625034157217093
+            "angularVelocity": -4.433819854347959e-31,
+            "velocityX": 0,
+            "velocityY": 2.2179802932100695e-31,
+            "timestamp": 3.079663233163615
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.1.traj
new file mode 100644
index 00000000..66f9741c
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": -2.1,
+            "angularVelocity": -1.5217589356437866e-30,
+            "velocityX": -1.7239506448843928e-31,
+            "velocityY": 1.9880283347693254e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 0.737435952483546,
+            "y": 6.69951466757637,
+            "heading": -2.132336617905336,
+            "angularVelocity": -0.4965864735710579,
+            "velocityX": 0.4213280106395778,
+            "velocityY": -0.3145888360360173,
+            "timestamp": 0.06511779846276568
+        },
+        {
+            "x": 0.7924253707255817,
+            "y": 6.658399914346492,
+            "heading": -2.1942155291625705,
+            "angularVelocity": -0.9502611070707117,
+            "velocityX": 0.8444606473217692,
+            "velocityY": -0.6313904063170618,
+            "timestamp": 0.13023559692553136
+        },
+        {
+            "x": 0.8751335704497958,
+            "y": 6.596485085823342,
+            "heading": -2.281554213555253,
+            "angularVelocity": -1.3412413572707518,
+            "velocityX": 1.2701320019519164,
+            "velocityY": -0.950812680784239,
+            "timestamp": 0.19535339538829705
+        },
+        {
+            "x": 0.9857847196454437,
+            "y": 6.513575479492505,
+            "heading": -2.388116786100485,
+            "angularVelocity": -1.6364584654403693,
+            "velocityX": 1.6992458560913724,
+            "velocityY": -1.2732249598125673,
+            "timestamp": 0.2604711938510627
+        },
+        {
+            "x": 1.1246179227996527,
+            "y": 6.409453213658469,
+            "heading": -2.503336368742031,
+            "angularVelocity": -1.7694023041554334,
+            "velocityX": 2.132031586319575,
+            "velocityY": -1.5989832010916685,
+            "timestamp": 0.3255889923138284
+        },
+        {
+            "x": 1.2913924480076884,
+            "y": 6.28415287204147,
+            "heading": -2.6022121669144127,
+            "angularVelocity": -1.518414327672898,
+            "velocityX": 2.5611204485575056,
+            "velocityY": -1.9242103476309298,
+            "timestamp": 0.39070679077659404
+        },
+        {
+            "x": 1.4799082257130864,
+            "y": 6.139182928400833,
+            "heading": -2.6246781987943484,
+            "angularVelocity": -0.3450060108033547,
+            "velocityX": 2.8949961785515774,
+            "velocityY": -2.2262721876804705,
+            "timestamp": 0.4558245892393597
+        },
+        {
+            "x": 1.6682193277242308,
+            "y": 5.99414498496963,
+            "heading": -2.64727359752809,
+            "angularVelocity": -0.34699266970246656,
+            "velocityX": 2.8918530180165907,
+            "velocityY": -2.2273164458122023,
+            "timestamp": 0.5209423877021254
+        },
+        {
+            "x": 1.8350272177323106,
+            "y": 5.869345925023006,
+            "heading": -2.74292245957107,
+            "angularVelocity": -1.4688589648446377,
+            "velocityX": 2.5616328246026367,
+            "velocityY": -1.9165122730305941,
+            "timestamp": 0.5860601861648911
+        },
+        {
+            "x": 1.9739579805969827,
+            "y": 5.765588033107393,
+            "heading": -2.8581454024211346,
+            "angularVelocity": -1.7694539061536165,
+            "velocityX": 2.133529789771879,
+            "velocityY": -1.5933875893384517,
+            "timestamp": 0.6511779846276567
+        },
+        {
+            "x": 2.084683421379683,
+            "y": 5.683004349697254,
+            "heading": -2.964498621197243,
+            "angularVelocity": -1.6332434647175749,
+            "velocityX": 1.7003867359860632,
+            "velocityY": -1.2682198317463163,
+            "timestamp": 0.7162957830904224
+        },
+        {
+            "x": 2.1674690836499857,
+            "y": 5.621349763098201,
+            "heading": -3.050914836384586,
+            "angularVelocity": -1.327075196449642,
+            "velocityX": 1.2713215775812727,
+            "velocityY": -0.9468162016304505,
+            "timestamp": 0.7814135815531881
+        },
+        {
+            "x": 2.22252551206485,
+            "y": 5.580407151857498,
+            "heading": -3.1116606802742157,
+            "angularVelocity": -0.9328608356494681,
+            "velocityX": 0.8454897081071593,
+            "velocityY": -0.6287468588808828,
+            "timestamp": 0.8465313800159537
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.48411690274504937,
+            "velocityX": 0.42191979126659057,
+            "velocityY": -0.3133882339275879,
+            "timestamp": 0.9116491784787194
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": -1.0063297310782738e-31,
+            "velocityX": -3.625667697405941e-32,
+            "velocityY": 0,
+            "timestamp": 0.9767669769414851
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.traj
new file mode 100644
index 00000000..66f9741c
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": -2.1,
+            "angularVelocity": -1.5217589356437866e-30,
+            "velocityX": -1.7239506448843928e-31,
+            "velocityY": 1.9880283347693254e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 0.737435952483546,
+            "y": 6.69951466757637,
+            "heading": -2.132336617905336,
+            "angularVelocity": -0.4965864735710579,
+            "velocityX": 0.4213280106395778,
+            "velocityY": -0.3145888360360173,
+            "timestamp": 0.06511779846276568
+        },
+        {
+            "x": 0.7924253707255817,
+            "y": 6.658399914346492,
+            "heading": -2.1942155291625705,
+            "angularVelocity": -0.9502611070707117,
+            "velocityX": 0.8444606473217692,
+            "velocityY": -0.6313904063170618,
+            "timestamp": 0.13023559692553136
+        },
+        {
+            "x": 0.8751335704497958,
+            "y": 6.596485085823342,
+            "heading": -2.281554213555253,
+            "angularVelocity": -1.3412413572707518,
+            "velocityX": 1.2701320019519164,
+            "velocityY": -0.950812680784239,
+            "timestamp": 0.19535339538829705
+        },
+        {
+            "x": 0.9857847196454437,
+            "y": 6.513575479492505,
+            "heading": -2.388116786100485,
+            "angularVelocity": -1.6364584654403693,
+            "velocityX": 1.6992458560913724,
+            "velocityY": -1.2732249598125673,
+            "timestamp": 0.2604711938510627
+        },
+        {
+            "x": 1.1246179227996527,
+            "y": 6.409453213658469,
+            "heading": -2.503336368742031,
+            "angularVelocity": -1.7694023041554334,
+            "velocityX": 2.132031586319575,
+            "velocityY": -1.5989832010916685,
+            "timestamp": 0.3255889923138284
+        },
+        {
+            "x": 1.2913924480076884,
+            "y": 6.28415287204147,
+            "heading": -2.6022121669144127,
+            "angularVelocity": -1.518414327672898,
+            "velocityX": 2.5611204485575056,
+            "velocityY": -1.9242103476309298,
+            "timestamp": 0.39070679077659404
+        },
+        {
+            "x": 1.4799082257130864,
+            "y": 6.139182928400833,
+            "heading": -2.6246781987943484,
+            "angularVelocity": -0.3450060108033547,
+            "velocityX": 2.8949961785515774,
+            "velocityY": -2.2262721876804705,
+            "timestamp": 0.4558245892393597
+        },
+        {
+            "x": 1.6682193277242308,
+            "y": 5.99414498496963,
+            "heading": -2.64727359752809,
+            "angularVelocity": -0.34699266970246656,
+            "velocityX": 2.8918530180165907,
+            "velocityY": -2.2273164458122023,
+            "timestamp": 0.5209423877021254
+        },
+        {
+            "x": 1.8350272177323106,
+            "y": 5.869345925023006,
+            "heading": -2.74292245957107,
+            "angularVelocity": -1.4688589648446377,
+            "velocityX": 2.5616328246026367,
+            "velocityY": -1.9165122730305941,
+            "timestamp": 0.5860601861648911
+        },
+        {
+            "x": 1.9739579805969827,
+            "y": 5.765588033107393,
+            "heading": -2.8581454024211346,
+            "angularVelocity": -1.7694539061536165,
+            "velocityX": 2.133529789771879,
+            "velocityY": -1.5933875893384517,
+            "timestamp": 0.6511779846276567
+        },
+        {
+            "x": 2.084683421379683,
+            "y": 5.683004349697254,
+            "heading": -2.964498621197243,
+            "angularVelocity": -1.6332434647175749,
+            "velocityX": 1.7003867359860632,
+            "velocityY": -1.2682198317463163,
+            "timestamp": 0.7162957830904224
+        },
+        {
+            "x": 2.1674690836499857,
+            "y": 5.621349763098201,
+            "heading": -3.050914836384586,
+            "angularVelocity": -1.327075196449642,
+            "velocityX": 1.2713215775812727,
+            "velocityY": -0.9468162016304505,
+            "timestamp": 0.7814135815531881
+        },
+        {
+            "x": 2.22252551206485,
+            "y": 5.580407151857498,
+            "heading": -3.1116606802742157,
+            "angularVelocity": -0.9328608356494681,
+            "velocityX": 0.8454897081071593,
+            "velocityY": -0.6287468588808828,
+            "timestamp": 0.8465313800159537
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.48411690274504937,
+            "velocityX": 0.42191979126659057,
+            "velocityY": -0.3133882339275879,
+            "timestamp": 0.9116491784787194
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": -1.0063297310782738e-31,
+            "velocityX": -3.625667697405941e-32,
+            "velocityY": 0,
+            "timestamp": 0.9767669769414851
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.1.traj
new file mode 100644
index 00000000..c5a01bdd
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.1.traj	
@@ -0,0 +1,211 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -2.32021994810819e-32,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 1.430775984981732,
+            "y": 5.581186654302167,
+            "heading": 3.138045514038262,
+            "angularVelocity": -0.019529788750833752,
+            "velocityX": 0.8071369934195644,
+            "velocityY": 0.2117031746247163,
+            "timestamp": 0.10007716860989323
+        },
+        {
+            "x": 1.592327952146185,
+            "y": 5.62355996139984,
+            "heading": 3.134136547157813,
+            "angularVelocity": -0.0390595271102885,
+            "velocityX": 1.6142739588706034,
+            "velocityY": 0.42340633419443324,
+            "timestamp": 0.20015433721978645
+        },
+        {
+            "x": 1.8346558957131356,
+            "y": 5.687119918958483,
+            "heading": 3.1282731070819607,
+            "angularVelocity": -0.05858918829621809,
+            "velocityX": 2.4214108665639773,
+            "velocityY": 0.6351094704367861,
+            "timestamp": 0.30023150582967967
+        },
+        {
+            "x": 2.1577597979432506,
+            "y": 5.771866521439831,
+            "heading": 3.1204552085550783,
+            "angularVelocity": -0.078118702152105,
+            "velocityX": 3.2285475970008037,
+            "velocityY": 0.8468125513392032,
+            "timestamp": 0.4003086744395729
+        },
+        {
+            "x": 2.523958853996556,
+            "y": 5.867916494702879,
+            "heading": 3.120455207736121,
+            "angularVelocity": -8.183261644434384e-9,
+            "velocityX": 3.659166832354859,
+            "velocityY": 0.9597590998747719,
+            "timestamp": 0.5003858430494661
+        },
+        {
+            "x": 2.890157912923332,
+            "y": 5.963966457010659,
+            "heading": 3.1204552069171707,
+            "angularVelocity": -8.18318468791684e-9,
+            "velocityX": 3.6591668610673986,
+            "velocityY": 0.9597589904065732,
+            "timestamp": 0.6004630116593593
+        },
+        {
+            "x": 3.256356975588488,
+            "y": 6.06001640506554,
+            "heading": 3.1204552060982262,
+            "angularVelocity": -8.183132612245063e-9,
+            "velocityX": 3.659166898422379,
+            "velocityY": 0.9597588479874845,
+            "timestamp": 0.7005401802692526
+        },
+        {
+            "x": 3.6225560432776467,
+            "y": 6.156066333965972,
+            "heading": 3.1204552052792867,
+            "angularVelocity": -8.183078308583848e-9,
+            "velocityX": 3.6591669486236644,
+            "velocityY": 0.9597586565906886,
+            "timestamp": 0.8006173488791458
+        },
+        {
+            "x": 3.9887551180209555,
+            "y": 6.252116235971829,
+            "heading": 3.1204552044603537,
+            "angularVelocity": -8.183012061558546e-9,
+            "velocityX": 3.659167019110774,
+            "velocityY": 0.9597583878523045,
+            "timestamp": 0.9006945174890391
+        },
+        {
+            "x": 4.354954203462548,
+            "y": 6.348166097189504,
+            "heading": 3.1204552036414284,
+            "angularVelocity": -8.182937495729244e-9,
+            "velocityX": 3.659167126011114,
+            "velocityY": 0.9597579802850231,
+            "timestamp": 1.0007716860989322
+        },
+        {
+            "x": 4.721153326841163,
+            "y": 6.444215813768638,
+            "heading": 3.120455202822509,
+            "angularVelocity": -8.182878073364452e-9,
+            "velocityX": 3.659167505088806,
+            "velocityY": 0.9597565350148991,
+            "timestamp": 1.1008488547088253
+        },
+        {
+            "x": 5.092057106129702,
+            "y": 6.520095539851432,
+            "heading": 3.1204551978641732,
+            "angularVelocity": -4.954512725191857e-8,
+            "velocityX": 3.7061777870070114,
+            "velocityY": 0.7582121590446533,
+            "timestamp": 1.2009260233187184
+        },
+        {
+            "x": 5.470573222615853,
+            "y": 6.51282289206487,
+            "heading": 3.120455142790982,
+            "angularVelocity": -5.503072477147126e-7,
+            "velocityX": 3.7822424609316347,
+            "velocityY": -0.07267039912792687,
+            "timestamp": 1.3010031919286116
+        },
+        {
+            "x": 5.849058511568644,
+            "y": 6.504092056642741,
+            "heading": 3.1204550877725445,
+            "angularVelocity": -5.497601318399077e-7,
+            "velocityX": 3.7819344233063643,
+            "velocityY": -0.08724103153050607,
+            "timestamp": 1.4010803605385047
+        },
+        {
+            "x": 6.221366644651584,
+            "y": 6.4354334883920234,
+            "heading": 3.1204550635505535,
+            "angularVelocity": -2.420331372889176e-7,
+            "velocityX": 3.720210496104441,
+            "velocityY": -0.6860562624264548,
+            "timestamp": 1.5011575291483978
+        },
+        {
+            "x": 6.569565662964383,
+            "y": 6.286823971409196,
+            "heading": 3.1204550644036275,
+            "angularVelocity": 8.52416239462205e-9,
+            "velocityX": 3.479305251631334,
+            "velocityY": -1.4849492551305274,
+            "timestamp": 1.601234697758291
+        },
+        {
+            "x": 6.915949214018099,
+            "y": 6.134030710794496,
+            "heading": 3.1204550652542946,
+            "angularVelocity": 8.500112010852639e-9,
+            "velocityX": 3.4611645779462337,
+            "velocityY": -1.5267544309761423,
+            "timestamp": 1.701311866368184
+        },
+        {
+            "x": 7.22156951411921,
+            "y": 5.999218440367679,
+            "heading": 3.1282730490164057,
+            "angularVelocity": 0.07811955384714984,
+            "velocityX": 3.0538463902035207,
+            "velocityY": -1.347083178904912,
+            "timestamp": 1.8013890349780772
+        },
+        {
+            "x": 7.4507847539347845,
+            "y": 5.898109225088713,
+            "heading": 3.1341365308407623,
+            "angularVelocity": 0.05858960545933885,
+            "velocityX": 2.290384939936379,
+            "velocityY": -1.010312508671161,
+            "timestamp": 1.9014662035879704
+        },
+        {
+            "x": 7.603594917403085,
+            "y": 5.830703076393403,
+            "heading": 3.1380455124480764,
+            "angularVelocity": 0.03905967426538248,
+            "velocityX": 1.526923329175751,
+            "velocityY": -0.6735417241675008,
+            "timestamp": 2.0015433721978635
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 0.019529804640431036,
+            "velocityX": 0.7634616732088615,
+            "velocityY": -0.33677088252547477,
+            "timestamp": 2.101620540807757
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 2.2237121345666e-32,
+            "velocityY": 0,
+            "timestamp": 2.20169770941765
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.traj
new file mode 100644
index 00000000..c5a01bdd
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.traj	
@@ -0,0 +1,211 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -2.32021994810819e-32,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 1.430775984981732,
+            "y": 5.581186654302167,
+            "heading": 3.138045514038262,
+            "angularVelocity": -0.019529788750833752,
+            "velocityX": 0.8071369934195644,
+            "velocityY": 0.2117031746247163,
+            "timestamp": 0.10007716860989323
+        },
+        {
+            "x": 1.592327952146185,
+            "y": 5.62355996139984,
+            "heading": 3.134136547157813,
+            "angularVelocity": -0.0390595271102885,
+            "velocityX": 1.6142739588706034,
+            "velocityY": 0.42340633419443324,
+            "timestamp": 0.20015433721978645
+        },
+        {
+            "x": 1.8346558957131356,
+            "y": 5.687119918958483,
+            "heading": 3.1282731070819607,
+            "angularVelocity": -0.05858918829621809,
+            "velocityX": 2.4214108665639773,
+            "velocityY": 0.6351094704367861,
+            "timestamp": 0.30023150582967967
+        },
+        {
+            "x": 2.1577597979432506,
+            "y": 5.771866521439831,
+            "heading": 3.1204552085550783,
+            "angularVelocity": -0.078118702152105,
+            "velocityX": 3.2285475970008037,
+            "velocityY": 0.8468125513392032,
+            "timestamp": 0.4003086744395729
+        },
+        {
+            "x": 2.523958853996556,
+            "y": 5.867916494702879,
+            "heading": 3.120455207736121,
+            "angularVelocity": -8.183261644434384e-9,
+            "velocityX": 3.659166832354859,
+            "velocityY": 0.9597590998747719,
+            "timestamp": 0.5003858430494661
+        },
+        {
+            "x": 2.890157912923332,
+            "y": 5.963966457010659,
+            "heading": 3.1204552069171707,
+            "angularVelocity": -8.18318468791684e-9,
+            "velocityX": 3.6591668610673986,
+            "velocityY": 0.9597589904065732,
+            "timestamp": 0.6004630116593593
+        },
+        {
+            "x": 3.256356975588488,
+            "y": 6.06001640506554,
+            "heading": 3.1204552060982262,
+            "angularVelocity": -8.183132612245063e-9,
+            "velocityX": 3.659166898422379,
+            "velocityY": 0.9597588479874845,
+            "timestamp": 0.7005401802692526
+        },
+        {
+            "x": 3.6225560432776467,
+            "y": 6.156066333965972,
+            "heading": 3.1204552052792867,
+            "angularVelocity": -8.183078308583848e-9,
+            "velocityX": 3.6591669486236644,
+            "velocityY": 0.9597586565906886,
+            "timestamp": 0.8006173488791458
+        },
+        {
+            "x": 3.9887551180209555,
+            "y": 6.252116235971829,
+            "heading": 3.1204552044603537,
+            "angularVelocity": -8.183012061558546e-9,
+            "velocityX": 3.659167019110774,
+            "velocityY": 0.9597583878523045,
+            "timestamp": 0.9006945174890391
+        },
+        {
+            "x": 4.354954203462548,
+            "y": 6.348166097189504,
+            "heading": 3.1204552036414284,
+            "angularVelocity": -8.182937495729244e-9,
+            "velocityX": 3.659167126011114,
+            "velocityY": 0.9597579802850231,
+            "timestamp": 1.0007716860989322
+        },
+        {
+            "x": 4.721153326841163,
+            "y": 6.444215813768638,
+            "heading": 3.120455202822509,
+            "angularVelocity": -8.182878073364452e-9,
+            "velocityX": 3.659167505088806,
+            "velocityY": 0.9597565350148991,
+            "timestamp": 1.1008488547088253
+        },
+        {
+            "x": 5.092057106129702,
+            "y": 6.520095539851432,
+            "heading": 3.1204551978641732,
+            "angularVelocity": -4.954512725191857e-8,
+            "velocityX": 3.7061777870070114,
+            "velocityY": 0.7582121590446533,
+            "timestamp": 1.2009260233187184
+        },
+        {
+            "x": 5.470573222615853,
+            "y": 6.51282289206487,
+            "heading": 3.120455142790982,
+            "angularVelocity": -5.503072477147126e-7,
+            "velocityX": 3.7822424609316347,
+            "velocityY": -0.07267039912792687,
+            "timestamp": 1.3010031919286116
+        },
+        {
+            "x": 5.849058511568644,
+            "y": 6.504092056642741,
+            "heading": 3.1204550877725445,
+            "angularVelocity": -5.497601318399077e-7,
+            "velocityX": 3.7819344233063643,
+            "velocityY": -0.08724103153050607,
+            "timestamp": 1.4010803605385047
+        },
+        {
+            "x": 6.221366644651584,
+            "y": 6.4354334883920234,
+            "heading": 3.1204550635505535,
+            "angularVelocity": -2.420331372889176e-7,
+            "velocityX": 3.720210496104441,
+            "velocityY": -0.6860562624264548,
+            "timestamp": 1.5011575291483978
+        },
+        {
+            "x": 6.569565662964383,
+            "y": 6.286823971409196,
+            "heading": 3.1204550644036275,
+            "angularVelocity": 8.52416239462205e-9,
+            "velocityX": 3.479305251631334,
+            "velocityY": -1.4849492551305274,
+            "timestamp": 1.601234697758291
+        },
+        {
+            "x": 6.915949214018099,
+            "y": 6.134030710794496,
+            "heading": 3.1204550652542946,
+            "angularVelocity": 8.500112010852639e-9,
+            "velocityX": 3.4611645779462337,
+            "velocityY": -1.5267544309761423,
+            "timestamp": 1.701311866368184
+        },
+        {
+            "x": 7.22156951411921,
+            "y": 5.999218440367679,
+            "heading": 3.1282730490164057,
+            "angularVelocity": 0.07811955384714984,
+            "velocityX": 3.0538463902035207,
+            "velocityY": -1.347083178904912,
+            "timestamp": 1.8013890349780772
+        },
+        {
+            "x": 7.4507847539347845,
+            "y": 5.898109225088713,
+            "heading": 3.1341365308407623,
+            "angularVelocity": 0.05858960545933885,
+            "velocityX": 2.290384939936379,
+            "velocityY": -1.010312508671161,
+            "timestamp": 1.9014662035879704
+        },
+        {
+            "x": 7.603594917403085,
+            "y": 5.830703076393403,
+            "heading": 3.1380455124480764,
+            "angularVelocity": 0.03905967426538248,
+            "velocityX": 1.526923329175751,
+            "velocityY": -0.6735417241675008,
+            "timestamp": 2.0015433721978635
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 0.019529804640431036,
+            "velocityX": 0.7634616732088615,
+            "velocityY": -0.33677088252547477,
+            "timestamp": 2.101620540807757
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 2.2237121345666e-32,
+            "velocityY": 0,
+            "timestamp": 2.20169770941765
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.1.traj
new file mode 100644
index 00000000..92207f04
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.1.traj	
@@ -0,0 +1,1093 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 4.411137018023184e-34,
+            "velocityY": 2.784782344800776e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 1.3544641699489606,
+            "y": 5.55925259826767,
+            "heading": 3.1400000009005375,
+            "angularVelocity": 3.865132870171629e-8,
+            "velocityX": 0.19160342420680077,
+            "velocityY": -0.032078691629102596,
+            "timestamp": 0.023299009229303495
+        },
+        {
+            "x": 1.3633925116373848,
+            "y": 5.557757806179583,
+            "heading": 3.1400000027300097,
+            "angularVelocity": 7.852144487332144e-8,
+            "velocityX": 0.383206925262499,
+            "velocityY": -0.06415689497258387,
+            "timestamp": 0.04659801845860699
+        },
+        {
+            "x": 1.3767850270669844,
+            "y": 5.555515636454451,
+            "heading": 3.140000005520194,
+            "angularVelocity": 1.1975551924808857e-7,
+            "velocityX": 0.5748105122322142,
+            "velocityY": -0.09623455242535102,
+            "timestamp": 0.06989702768791048
+        },
+        {
+            "x": 1.3946417184903905,
+            "y": 5.55252610340551,
+            "heading": 3.1400000093068963,
+            "angularVelocity": 1.6252630014020525e-7,
+            "velocityX": 0.7664141958854979,
+            "velocityY": -0.12831159554996177,
+            "timestamp": 0.09319603691721398
+        },
+        {
+            "x": 1.4169625884614865,
+            "y": 5.548789223260415,
+            "heading": 3.14000001413076,
+            "angularVelocity": 2.070415703537556e-7,
+            "velocityX": 0.9580179891521703,
+            "velocityY": -0.16038794217918884,
+            "timestamp": 0.11649504614651748
+        },
+        {
+            "x": 1.443747639900165,
+            "y": 5.544305014572816,
+            "heading": 3.1400000200383236,
+            "angularVelocity": 2.5355430687602407e-7,
+            "velocityX": 1.1496219077414547,
+            "velocityY": -0.1924634924804626,
+            "timestamp": 0.13979405537582099
+        },
+        {
+            "x": 1.474996876177077,
+            "y": 5.539073498761034,
+            "heading": 3.1400000270834036,
+            "angularVelocity": 3.0237681109940727e-7,
+            "velocityX": 1.3412259710000187,
+            "velocityY": -0.22453812350142868,
+            "timestamp": 0.1630930646051245
+        },
+        {
+            "x": 1.5107103012267313,
+            "y": 5.5330947008269575,
+            "heading": 3.1400000353289426,
+            "angularVelocity": 3.539008296544223e-7,
+            "velocityX": 1.5328302031287921,
+            "velocityY": -0.25661168143439866,
+            "timestamp": 0.186392073834428
+        },
+        {
+            "x": 1.5508879197018797,
+            "y": 5.526368650337421,
+            "heading": 3.140000044849547,
+            "angularVelocity": 4.086269951598913e-7,
+            "velocityX": 1.7244346349549924,
+            "velocityY": -0.2886839703505207,
+            "timestamp": 0.2096910830637315
+        },
+        {
+            "x": 1.5955297371899198,
+            "y": 5.5188953827998875,
+            "heading": 3.1400000557350607,
+            "angularVelocity": 4.6720929680542457e-7,
+            "velocityX": 1.9160393065938472,
+            "velocityY": -0.3207547352763359,
+            "timestamp": 0.232990092293035
+        },
+        {
+            "x": 1.6446357605259214,
+            "y": 5.510674941652546,
+            "heading": 3.140000068095764,
+            "angularVelocity": 5.305248328425052e-7,
+            "velocityX": 2.107644271595818,
+            "velocityY": -0.3528236358230067,
+            "timestamp": 0.25628910152233847
+        },
+        {
+            "x": 1.6982059982629796,
+            "y": 5.501707381254886,
+            "heading": 3.1400000820702374,
+            "angularVelocity": 5.997883043629641e-7,
+            "velocityX": 2.299249603699023,
+            "velocityY": -0.38489020324511874,
+            "timestamp": 0.27958811075164197
+        },
+        {
+            "x": 1.7562404614129117,
+            "y": 5.491992771597749,
+            "heading": 3.1400000978378597,
+            "angularVelocity": 6.767507519595272e-7,
+            "velocityX": 2.490855408432506,
+            "velocityY": -0.41695376664068867,
+            "timestamp": 0.3028871199809455
+        },
+        {
+            "x": 1.8187391646840712,
+            "y": 5.4815312061757675,
+            "heading": 3.140000115639928,
+            "angularVelocity": 7.640697380946438e-7,
+            "velocityX": 2.6824618444526434,
+            "velocityY": -0.4490133172205749,
+            "timestamp": 0.326186129210249
+        },
+        {
+            "x": 1.8857021287172593,
+            "y": 5.47032281621074,
+            "heading": 3.140000135818395,
+            "angularVelocity": 8.660654300433757e-7,
+            "velocityX": 2.874069166381749,
+            "velocityY": -0.48106723572405463,
+            "timestamp": 0.3494851384395525
+        },
+        {
+            "x": 1.9571293845792233,
+            "y": 5.458367799245384,
+            "heading": 3.14000015889542,
+            "angularVelocity": 9.904723735244152e-7,
+            "velocityX": 3.0656778217043392,
+            "velocityY": -0.5131126756388134,
+            "timestamp": 0.372784147668856
+        },
+        {
+            "x": 2.033020984323074,
+            "y": 5.445666486377279,
+            "heading": 3.140000185766696,
+            "angularVelocity": 0.000001153322655289093,
+            "velocityX": 3.2572887111610362,
+            "velocityY": -0.545143904752752,
+            "timestamp": 0.3960831568981595
+        },
+        {
+            "x": 2.113377033060567,
+            "y": 5.432219546645115,
+            "heading": 3.1400002183250084,
+            "angularVelocity": 0.0000013974118596484493,
+            "velocityX": 3.448904112043669,
+            "velocityY": -0.5771464185388335,
+            "timestamp": 0.419382166127463
+        },
+        {
+            "x": 2.1981978506136755,
+            "y": 5.41802904044786,
+            "heading": 3.1400002631776545,
+            "angularVelocity": 0.0000019250880740696254,
+            "velocityX": 3.640533239775037,
+            "velocityY": -0.6090604994243719,
+            "timestamp": 0.4426811753567665
+        },
+        {
+            "x": 2.2851188032420633,
+            "y": 5.403428173451014,
+            "heading": 3.140000263177585,
+            "angularVelocity": -2.9753453619857404e-12,
+            "velocityX": 3.73067162525748,
+            "velocityY": -0.6266732998482398,
+            "timestamp": 0.46598018458607
+        },
+        {
+            "x": 2.372043080680341,
+            "y": 5.388847120989839,
+            "heading": 3.140000263177669,
+            "angularVelocity": 3.609665835664611e-12,
+            "velocityX": 3.730814327029464,
+            "velocityY": -0.6258228544259767,
+            "timestamp": 0.4892791938153735
+        },
+        {
+            "x": 2.458969517114403,
+            "y": 5.374278945126162,
+            "heading": 3.140000263177672,
+            "angularVelocity": 1.1048975247425585e-13,
+            "velocityX": 3.7309069917332867,
+            "velocityY": -0.6252701872544597,
+            "timestamp": 0.5125782030446769
+        },
+        {
+            "x": 2.5458971210754204,
+            "y": 5.359717737461923,
+            "heading": 3.1400002631776736,
+            "angularVelocity": 8.62983817390415e-14,
+            "velocityX": 3.7309571023169674,
+            "velocityY": -0.6249711101842599,
+            "timestamp": 0.5358772122739804
+        },
+        {
+            "x": 2.632825281306951,
+            "y": 5.345159851008328,
+            "heading": 3.140000263177678,
+            "angularVelocity": 1.7012303031669384e-13,
+            "velocityX": 3.7309809776027962,
+            "velocityY": -0.6248285628956001,
+            "timestamp": 0.5591762215032839
+        },
+        {
+            "x": 2.7197537163641496,
+            "y": 5.330603605663881,
+            "heading": 3.1400002631776815,
+            "angularVelocity": 1.5893105684431692e-13,
+            "velocityX": 3.7309927731970562,
+            "velocityY": -0.6247581260336735,
+            "timestamp": 0.5824752307325874
+        },
+        {
+            "x": 2.8066823211500065,
+            "y": 5.316048373923756,
+            "heading": 3.140000263177685,
+            "angularVelocity": 1.4661255756791518e-13,
+            "velocityX": 3.7310000579983225,
+            "velocityY": -0.6247146218539804,
+            "timestamp": 0.605774239961891
+        },
+        {
+            "x": 2.8936112175364808,
+            "y": 5.301494883804396,
+            "heading": 3.140000263177688,
+            "angularVelocity": 1.4712645864153266e-13,
+            "velocityX": 3.731012573578327,
+            "velocityY": -0.6246398710039542,
+            "timestamp": 0.6290732491911945
+        },
+        {
+            "x": 2.9805406318812673,
+            "y": 5.286944487835434,
+            "heading": 3.1400002631776913,
+            "angularVelocity": 1.2989281986974027e-13,
+            "velocityX": 3.7310348044942168,
+            "velocityY": -0.6245070692018166,
+            "timestamp": 0.652372258420498
+        },
+        {
+            "x": 3.067470472751592,
+            "y": 5.272396640371872,
+            "heading": 3.140000263177696,
+            "angularVelocity": 2.2497040560944553e-13,
+            "velocityX": 3.7310531110902496,
+            "velocityY": -0.6243976866335479,
+            "timestamp": 0.6756712676498015
+        },
+        {
+            "x": 3.1544000489958055,
+            "y": 5.257847211710117,
+            "heading": 3.1400002631777,
+            "angularVelocity": 1.6698909622359586e-13,
+            "velocityX": 3.73104175326474,
+            "velocityY": -0.6244655520950864,
+            "timestamp": 0.698970276879105
+        },
+        {
+            "x": 3.24132763546162,
+            "y": 5.243285899348815,
+            "heading": 3.1400002631776998,
+            "angularVelocity": -2.5660759099238358e-14,
+            "velocityX": 3.7309563514197523,
+            "velocityY": -0.6249756038008819,
+            "timestamp": 0.7222692861084085
+        },
+        {
+            "x": 3.3282514237632603,
+            "y": 5.228701930838363,
+            "heading": 3.1400002631777038,
+            "angularVelocity": 1.8102189988940347e-13,
+            "velocityX": 3.730793333150296,
+            "velocityY": -0.62594801206375,
+            "timestamp": 0.745568295337712
+        },
+        {
+            "x": 3.4151697732878015,
+            "y": 5.214085582937486,
+            "heading": 3.140000263177707,
+            "angularVelocity": 1.4033623034653362e-13,
+            "velocityX": 3.7305598993143207,
+            "velocityY": -0.6273377445673929,
+            "timestamp": 0.7688673045670155
+        },
+        {
+            "x": 3.502081162717683,
+            "y": 5.199427905968822,
+            "heading": 3.1400002631777144,
+            "angularVelocity": 3.3137702069013046e-13,
+            "velocityX": 3.7302611701022674,
+            "velocityY": -0.6291115997313443,
+            "timestamp": 0.792166313796319
+        },
+        {
+            "x": 3.588983960416736,
+            "y": 5.18471937597994,
+            "heading": 3.140000263177713,
+            "angularVelocity": -6.829626544199834e-14,
+            "velocityX": 3.729892410608752,
+            "velocityY": -0.6312942256021684,
+            "timestamp": 0.8154653230256225
+        },
+        {
+            "x": 3.675876953607923,
+            "y": 5.169953034745646,
+            "heading": 3.140000263177716,
+            "angularVelocity": 1.3540589961163151e-13,
+            "velocityX": 3.7294715983848317,
+            "velocityY": -0.6337755004484098,
+            "timestamp": 0.838764332254926
+        },
+        {
+            "x": 3.7627586434088305,
+            "y": 5.155120331788361,
+            "heading": 3.1400002631777193,
+            "angularVelocity": 1.3533863072281271e-13,
+            "velocityX": 3.728986453707805,
+            "velocityY": -0.6366237641880743,
+            "timestamp": 0.8620633414842295
+        },
+        {
+            "x": 3.8490562562014863,
+            "y": 5.137199419063008,
+            "heading": 3.1400002631777224,
+            "angularVelocity": 1.3559815009211054e-13,
+            "velocityX": 3.7039177049679806,
+            "velocityY": -0.7691705921529395,
+            "timestamp": 0.885362350713533
+        },
+        {
+            "x": 3.9343637094417163,
+            "y": 5.1150391964944655,
+            "heading": 3.1400002631777255,
+            "angularVelocity": 1.370480919312873e-13,
+            "velocityX": 3.6614197797262853,
+            "velocityY": -0.9511229576576579,
+            "timestamp": 0.9086613599428365
+        },
+        {
+            "x": 4.018431663513184,
+            "y": 5.088562488555908,
+            "heading": 3.140000263177729,
+            "angularVelocity": 1.413301961445022e-13,
+            "velocityX": 3.6082201283346866,
+            "velocityY": -1.1363877183834126,
+            "timestamp": 0.93196036917214
+        },
+        {
+            "x": 4.095482183944048,
+            "y": 5.0601896166081985,
+            "heading": 3.140000263177732,
+            "angularVelocity": 1.478757127486931e-13,
+            "velocityX": 3.549906540385796,
+            "velocityY": -1.3072078310833697,
+            "timestamp": 0.9536653125429956
+        },
+        {
+            "x": 4.171099571738343,
+            "y": 5.028192533992809,
+            "heading": 3.1400002631777353,
+            "angularVelocity": 1.4523452590479803e-13,
+            "velocityX": 3.483878603243662,
+            "velocityY": -1.4741841095171686,
+            "timestamp": 0.9753702559138512
+        },
+        {
+            "x": 4.2451557679063034,
+            "y": 4.992731534520922,
+            "heading": 3.1400002631777384,
+            "angularVelocity": 1.4237189069796376e-13,
+            "velocityX": 3.4119506742159467,
+            "velocityY": -1.6337752587458831,
+            "timestamp": 0.9970751992847068
+        },
+        {
+            "x": 4.31781550635648,
+            "y": 4.954490651976443,
+            "heading": 3.1400002631777415,
+            "angularVelocity": 1.4182458601020346e-13,
+            "velocityX": 3.347612440573966,
+            "velocityY": -1.761851293089538,
+            "timestamp": 1.0187801426555625
+        },
+        {
+            "x": 4.390182790341615,
+            "y": 4.91569917224677,
+            "heading": 3.140000263177745,
+            "angularVelocity": 1.421722636666585e-13,
+            "velocityX": 3.3341383457540337,
+            "velocityY": -1.787218656449447,
+            "timestamp": 1.040485086026418
+        },
+        {
+            "x": 4.462533664586848,
+            "y": 4.87687709490227,
+            "heading": 3.1400002631777486,
+            "angularVelocity": 1.635268036057102e-13,
+            "velocityX": 3.333382308772387,
+            "velocityY": -1.7886283636415046,
+            "timestamp": 1.0621900293972737
+        },
+        {
+            "x": 4.534860115355159,
+            "y": 4.838009534998386,
+            "heading": 3.140000263177751,
+            "angularVelocity": 1.1808318022164126e-13,
+            "velocityX": 3.3322570592611274,
+            "velocityY": -1.7907238567558525,
+            "timestamp": 1.0838949727681293
+        },
+        {
+            "x": 4.607177418550424,
+            "y": 4.799124957707118,
+            "heading": 3.140000263177754,
+            "angularVelocity": 1.4385253669729274e-13,
+            "velocityX": 3.3318356081243334,
+            "velocityY": -1.7915078895502723,
+            "timestamp": 1.105599916138985
+        },
+        {
+            "x": 4.679489672949611,
+            "y": 4.7602309921876325,
+            "heading": 3.1400002631777575,
+            "angularVelocity": 1.4120580598782794e-13,
+            "velocityX": 3.331602997698768,
+            "velocityY": -1.7919404282640008,
+            "timestamp": 1.1273048595098405
+        },
+        {
+            "x": 4.751800053049455,
+            "y": 4.721333542151952,
+            "heading": 3.1400002631777606,
+            "angularVelocity": 1.4195129118241079e-13,
+            "velocityX": 3.3315166441264745,
+            "velocityY": -1.7921009684797435,
+            "timestamp": 1.149009802880696
+        },
+        {
+            "x": 4.824110260922509,
+            "y": 4.682435771933003,
+            "heading": 3.1400002631777637,
+            "angularVelocity": 1.420416383867913e-13,
+            "velocityX": 3.3315087092142113,
+            "velocityY": -1.7921157201114153,
+            "timestamp": 1.1707147462515517
+        },
+        {
+            "x": 4.896420645274053,
+            "y": 4.643538329777489,
+            "heading": 3.140000263177767,
+            "angularVelocity": 1.4171119036284518e-13,
+            "velocityX": 3.3315168400108037,
+            "velocityY": -1.7921006054248252,
+            "timestamp": 1.1924196896224073
+        },
+        {
+            "x": 4.96873125432361,
+            "y": 4.6046413053457185,
+            "heading": 3.14000026317777,
+            "angularVelocity": 1.4137748376763704e-13,
+            "velocityX": 3.3315271924000274,
+            "velocityY": -1.792081359866257,
+            "timestamp": 1.214124632993263
+        },
+        {
+            "x": 5.04104400801269,
+            "y": 4.56574826813338,
+            "heading": 3.1400002631777735,
+            "angularVelocity": 1.421351986736705e-13,
+            "velocityX": 3.331626001206414,
+            "velocityY": -1.7918976588811946,
+            "timestamp": 1.2358295763641185
+        },
+        {
+            "x": 5.11336222581346,
+            "y": 4.5268653919090065,
+            "heading": 3.1400002631777766,
+            "angularVelocity": 1.4239328375117255e-13,
+            "velocityX": 3.3318777462404188,
+            "velocityY": -1.7914295172313686,
+            "timestamp": 1.257534519734974
+        },
+        {
+            "x": 5.185674775789392,
+            "y": 4.487971975945349,
+            "heading": 3.1400002631777797,
+            "angularVelocity": 1.4189340221530015e-13,
+            "velocityX": 3.331616615642773,
+            "velocityY": -1.7919151088820495,
+            "timestamp": 1.2792394631058297
+        },
+        {
+            "x": 5.25798679998857,
+            "y": 4.4490775824474165,
+            "heading": 3.140000263177783,
+            "angularVelocity": 1.4182847689207558e-13,
+            "velocityX": 3.331592391818018,
+            "velocityY": -1.791960146283848,
+            "timestamp": 1.3009444064766853
+        },
+        {
+            "x": 5.330301009987797,
+            "y": 4.410187253034054,
+            "heading": 3.1400002631777864,
+            "angularVelocity": 1.4221031722558925e-13,
+            "velocityX": 3.331693096990619,
+            "velocityY": -1.7917729039338035,
+            "timestamp": 1.322649349847541
+        },
+        {
+            "x": 5.4026181052176305,
+            "y": 4.3713022890025295,
+            "heading": 3.1400002631777895,
+            "angularVelocity": 1.4161705945188678e-13,
+            "velocityX": 3.3318260266449937,
+            "velocityY": -1.791525707625898,
+            "timestamp": 1.3443542932183965
+        },
+        {
+            "x": 5.474936056465776,
+            "y": 4.332418917012381,
+            "heading": 3.1400002631777926,
+            "angularVelocity": 1.4174219306640175e-13,
+            "velocityX": 3.331865465508907,
+            "velocityY": -1.7914523583735324,
+            "timestamp": 1.366059236589252
+        },
+        {
+            "x": 5.54725002868687,
+            "y": 4.293528145471586,
+            "heading": 3.1400002631777957,
+            "angularVelocity": 1.4114123611773667e-13,
+            "velocityX": 3.331682141968083,
+            "velocityY": -1.7917932738304505,
+            "timestamp": 1.3877641799601077
+        },
+        {
+            "x": 5.619560804995751,
+            "y": 4.2546314319979865,
+            "heading": 3.1400002631777992,
+            "angularVelocity": 1.438802546332647e-13,
+            "velocityX": 3.331534898449109,
+            "velocityY": -1.7920670332572342,
+            "timestamp": 1.4094691233309633
+        },
+        {
+            "x": 5.691871264235437,
+            "y": 4.215734129079228,
+            "heading": 3.1400002631778023,
+            "angularVelocity": 1.4227513855970737e-13,
+            "velocityX": 3.331520290292617,
+            "velocityY": -1.7920941904433432,
+            "timestamp": 1.431174066701819
+        },
+        {
+            "x": 5.7641829192535345,
+            "y": 4.1768390492074925,
+            "heading": 3.1400002631778055,
+            "angularVelocity": 1.4198857424426595e-13,
+            "velocityX": 3.3315753827383325,
+            "velocityY": -1.79199176921174,
+            "timestamp": 1.4528790100726745
+        },
+        {
+            "x": 5.836496897649906,
+            "y": 4.13794828914952,
+            "heading": 3.1400002631778086,
+            "angularVelocity": 1.420236059365936e-13,
+            "velocityX": 3.331682426477163,
+            "velocityY": -1.7917927447907471,
+            "timestamp": 1.47458395344353
+        },
+        {
+            "x": 5.90881236073979,
+            "y": 4.099060289876852,
+            "heading": 3.1400002631778117,
+            "angularVelocity": 1.4199973949506742e-13,
+            "velocityX": 3.3317508299509484,
+            "velocityY": -1.7916655486362405,
+            "timestamp": 1.4962888968143857
+        },
+        {
+            "x": 5.981129099244504,
+            "y": 4.06017466243704,
+            "heading": 3.140000263177815,
+            "angularVelocity": 1.4813660843765197e-13,
+            "velocityX": 3.3318095914418606,
+            "velocityY": -1.7915562724764487,
+            "timestamp": 1.5179938401852413
+        },
+        {
+            "x": 6.053446077627243,
+            "y": 4.021289481108035,
+            "heading": 3.140000263177829,
+            "angularVelocity": 6.425453029046258e-13,
+            "velocityX": 3.331820643210356,
+            "velocityY": -1.7915357190582988,
+            "timestamp": 1.539698783556097
+        },
+        {
+            "x": 6.125761091363011,
+            "y": 3.9824006462217088,
+            "heading": 3.140000263177869,
+            "angularVelocity": 1.8383501574993634e-12,
+            "velocityX": 3.331730127102014,
+            "velocityY": -1.7917040474119204,
+            "timestamp": 1.5614037269269525
+        },
+        {
+            "x": 6.198074029937981,
+            "y": 3.9435079527230394,
+            "heading": 3.140000263178238,
+            "angularVelocity": 1.7022488728392152e-11,
+            "velocityX": 3.331634519350639,
+            "velocityY": -1.79188182315355,
+            "timestamp": 1.583108670297808
+        },
+        {
+            "x": 6.270380104650612,
+            "y": 3.9046024999630506,
+            "heading": 3.140000263177847,
+            "angularVelocity": -1.8008440986287348e-11,
+            "velocityX": 3.33131828436938,
+            "velocityY": -1.7924696736324577,
+            "timestamp": 1.6048136136686637
+        },
+        {
+            "x": 6.342678298341779,
+            "y": 3.8656824038223503,
+            "heading": 3.1400002631782598,
+            "angularVelocity": 1.900200591260141e-11,
+            "velocityX": 3.3309551863724884,
+            "velocityY": -1.7931443301131518,
+            "timestamp": 1.6265185570395193
+        },
+        {
+            "x": 6.414967377910046,
+            "y": 3.8267453820171355,
+            "heading": 3.1400002631778587,
+            "angularVelocity": -1.8477715999059415e-11,
+            "velocityX": 3.3305352763678626,
+            "velocityY": -1.7939241370065324,
+            "timestamp": 1.648223500410375
+        },
+        {
+            "x": 6.487241234316261,
+            "y": 3.787780110681628,
+            "heading": 3.1400002631778627,
+            "angularVelocity": 1.954226890690534e-13,
+            "velocityX": 3.3298339079393355,
+            "velocityY": -1.795225662182324,
+            "timestamp": 1.6699284437812305
+        },
+        {
+            "x": 6.559496489740858,
+            "y": 3.7487803574447534,
+            "heading": 3.1400002631778654,
+            "angularVelocity": 1.160046399416623e-13,
+            "velocityX": 3.3289769150756516,
+            "velocityY": -1.79681432798537,
+            "timestamp": 1.691633387152086
+        },
+        {
+            "x": 6.630585131325424,
+            "y": 3.7076925524537225,
+            "heading": 3.140000263177869,
+            "angularVelocity": 1.5938094021424355e-13,
+            "velocityX": 3.275228152865242,
+            "velocityY": -1.8930159958948245,
+            "timestamp": 1.7133383305229417
+        },
+        {
+            "x": 6.699761336489547,
+            "y": 3.6634607167510884,
+            "heading": 3.140000263177872,
+            "angularVelocity": 1.431049344650049e-13,
+            "velocityX": 3.1871175142993193,
+            "velocityY": -2.037869205501672,
+            "timestamp": 1.7350432738937973
+        },
+        {
+            "x": 6.76677589557052,
+            "y": 3.6160175775021286,
+            "heading": 3.140000263178055,
+            "angularVelocity": 8.443311888840044e-12,
+            "velocityX": 3.087525175069758,
+            "velocityY": -2.1858218396761058,
+            "timestamp": 1.756748217264653
+        },
+        {
+            "x": 6.8314564099651545,
+            "y": 3.5654385941456423,
+            "heading": 3.140000263178058,
+            "angularVelocity": 1.5118918552164865e-13,
+            "velocityX": 2.9799900091650118,
+            "velocityY": -2.3302978723451035,
+            "timestamp": 1.7784531606355085
+        },
+        {
+            "x": 6.893649101257324,
+            "y": 3.5118300914764404,
+            "heading": 3.1400002631780617,
+            "angularVelocity": 1.5501587028498165e-13,
+            "velocityX": 2.865369894292817,
+            "velocityY": -2.469875260820684,
+            "timestamp": 1.8001581040063641
+        },
+        {
+            "x": 6.9342196158595195,
+            "y": 3.4745269537623855,
+            "heading": 3.140000263178065,
+            "angularVelocity": 2.3326761756772743e-13,
+            "velocityX": 2.784724807966257,
+            "velocityY": -2.5604549024296013,
+            "timestamp": 1.8147270533280853
+        },
+        {
+            "x": 6.973574770579654,
+            "y": 3.435943763171971,
+            "heading": 3.140000263178068,
+            "angularVelocity": 2.2697466808256025e-13,
+            "velocityX": 2.701303563563437,
+            "velocityY": -2.648316617649584,
+            "timestamp": 1.8292960026498064
+        },
+        {
+            "x": 7.011676569760775,
+            "y": 3.396122365046946,
+            "heading": 3.140000263178071,
+            "angularVelocity": 2.224027473304724e-13,
+            "velocityX": 2.615274330340327,
+            "velocityY": -2.7333061050358776,
+            "timestamp": 1.8438649519715276
+        },
+        {
+            "x": 7.048491012484172,
+            "y": 3.355107883496788,
+            "heading": 3.1400002631780746,
+            "angularVelocity": 2.1692590269440246e-13,
+            "velocityX": 2.5269113036529633,
+            "velocityY": -2.815198312825684,
+            "timestamp": 1.8584339012932487
+        },
+        {
+            "x": 7.083992093604447,
+            "y": 3.312951458599195,
+            "heading": 3.1400002631780777,
+            "angularVelocity": 2.1487378521690259e-13,
+            "velocityX": 2.4367633064212786,
+            "velocityY": -2.893580310196796,
+            "timestamp": 1.8730028506149698
+        },
+        {
+            "x": 7.118173429008876,
+            "y": 3.269718054236438,
+            "heading": 3.140000263178081,
+            "angularVelocity": 2.1169602046875272e-13,
+            "velocityX": 2.3461771092494,
+            "velocityY": -2.967503243236169,
+            "timestamp": 1.887571799936691
+        },
+        {
+            "x": 7.151084190942531,
+            "y": 3.2255097664946186,
+            "heading": 3.140000263178084,
+            "angularVelocity": 2.115299519607656e-13,
+            "velocityX": 2.258966052177459,
+            "velocityY": -3.034418389794455,
+            "timestamp": 1.9021407492584121
+        },
+        {
+            "x": 7.1829258498520225,
+            "y": 3.1805253278057903,
+            "heading": 3.1400002631783006,
+            "angularVelocity": 1.4861478341138462e-11,
+            "velocityX": 2.1855837511923784,
+            "velocityY": -3.0876927152018316,
+            "timestamp": 1.9167096985801333
+        },
+        {
+            "x": 7.213982568749707,
+            "y": 3.1352979487738755,
+            "heading": 3.140000221251036,
+            "angularVelocity": -0.000002877850999701821,
+            "velocityX": 2.1317061520275473,
+            "velocityY": -3.104367928885527,
+            "timestamp": 1.9312786479018544
+        },
+        {
+            "x": 7.244041792187231,
+            "y": 3.091532444769153,
+            "heading": 3.1400001920667906,
+            "angularVelocity": -0.000002003181212693182,
+            "velocityX": 2.063238931905195,
+            "velocityY": -3.0040260994983523,
+            "timestamp": 1.9458475972235756
+        },
+        {
+            "x": 7.273101015062598,
+            "y": 3.049227126183516,
+            "heading": 3.140000168751395,
+            "angularVelocity": -0.0000016003484573673396,
+            "velocityX": 1.9945997637621933,
+            "velocityY": -2.9038002433409447,
+            "timestamp": 1.9604165465452967
+        },
+        {
+            "x": 7.301159421127851,
+            "y": 3.008381441393539,
+            "heading": 3.1400001491208855,
+            "angularVelocity": -0.0000013474210949527603,
+            "velocityX": 1.92590456907015,
+            "velocityY": -2.8036122501352616,
+            "timestamp": 1.9749854958670179
+        },
+        {
+            "x": 7.328216598566981,
+            "y": 2.9689951117692965,
+            "heading": 3.1400001321528324,
+            "angularVelocity": -0.0000011646724122855417,
+            "velocityX": 1.8571811076855331,
+            "velocityY": -2.703443381845065,
+            "timestamp": 1.989554445188739
+        },
+        {
+            "x": 7.35427229894417,
+            "y": 2.9310679691270813,
+            "heading": 3.140000117255664,
+            "angularVelocity": -0.0000010225286768555236,
+            "velocityX": 1.7884405938827321,
+            "velocityY": -2.603286057538151,
+            "timestamp": 2.00412339451046
+        },
+        {
+            "x": 7.379326356096679,
+            "y": 2.8945999009401007,
+            "heading": 3.1400001040435335,
+            "angularVelocity": -9.068691499457651e-7,
+            "velocityX": 1.7196886748142857,
+            "velocityY": -2.50313645697232,
+            "timestamp": 2.0186923438321815
+        },
+        {
+            "x": 7.403378651082293,
+            "y": 2.8595908266403365,
+            "heading": 3.1400000922448146,
+            "angularVelocity": -8.098538018281321e-7,
+            "velocityX": 1.650928591655569,
+            "velocityY": -2.4029923865251464,
+            "timestamp": 2.033261293153903
+        },
+        {
+            "x": 7.426429094558334,
+            "y": 2.826040685699055,
+            "heading": 3.1400000816578917,
+            "angularVelocity": -7.266771822683861e-7,
+            "velocityX": 1.5821623760933374,
+            "velocityY": -2.3028524707174887,
+            "timestamp": 2.0478302424756243
+        },
+        {
+            "x": 7.448477616956692,
+            "y": 2.793949430978311,
+            "heading": 3.1400000721271697,
+            "angularVelocity": -6.541804651955507e-7,
+            "velocityX": 1.513391385436516,
+            "velocityY": -2.202715790417496,
+            "timestamp": 2.0623991917973457
+        },
+        {
+            "x": 7.4695241625734585,
+            "y": 2.763317024730304,
+            "heading": 3.1400000635289262,
+            "angularVelocity": -5.901759591769764e-7,
+            "velocityX": 1.4446165713119874,
+            "velocityY": -2.102581701093514,
+            "timestamp": 2.076968141119067
+        },
+        {
+            "x": 7.489568685800359,
+            "y": 2.734143436045934,
+            "heading": 3.1400000557624246,
+            "angularVelocity": -5.330859058092693e-7,
+            "velocityX": 1.3758386266751614,
+            "velocityY": -2.0024497333432913,
+            "timestamp": 2.0915370904407884
+        },
+        {
+            "x": 7.508611148608458,
+            "y": 2.7064286391508827,
+            "heading": 3.140000048744052,
+            "angularVelocity": -4.817349694037634e-7,
+            "velocityX": 1.3070580717652986,
+            "velocityY": -1.902319534719876,
+            "timestamp": 2.1061060397625098
+        },
+        {
+            "x": 7.526651518803911,
+            "y": 2.680172612224319,
+            "heading": 3.1400000424032948,
+            "angularVelocity": -4.3522407744534453e-7,
+            "velocityX": 1.2382753070979928,
+            "velocityY": -1.8021908338592867,
+            "timestamp": 2.120674989084231
+        },
+        {
+            "x": 7.543689768780771,
+            "y": 2.6553753365541346,
+            "heading": 3.1400000366798815,
+            "angularVelocity": -3.9285011556875146e-7,
+            "velocityX": 1.1694906475824893,
+            "velocityY": -1.7020634173815092,
+            "timestamp": 2.1352439384059525
+        },
+        {
+            "x": 7.559725874605572,
+            "y": 2.6320367959168407,
+            "heading": 3.1400000315217036,
+            "angularVelocity": -3.5405282918878005e-7,
+            "velocityX": 1.100704345294715,
+            "velocityY": -1.6019371144699315,
+            "timestamp": 2.149812887727674
+        },
+        {
+            "x": 7.574759815330251,
+            "y": 2.6101569761120964,
+            "heading": 3.140000026883262,
+            "angularVelocity": -3.1837857149022665e-7,
+            "velocityX": 1.0319166051501305,
+            "velocityY": -1.5018117862573288,
+            "timestamp": 2.1643818370493952
+        },
+        {
+            "x": 7.588791572466392,
+            "y": 2.5897358646065065,
+            "heading": 3.1400000227244846,
+            "angularVelocity": -2.854548657017254e-7,
+            "velocityX": 0.9631275959768569,
+            "velocityY": -1.4016873183261147,
+            "timestamp": 2.1789507863711166
+        },
+        {
+            "x": 7.601821129576078,
+            "y": 2.570773450256394,
+            "heading": 3.140000019009809,
+            "angularVelocity": -2.5497212819286766e-7,
+            "velocityX": 0.8943374585191634,
+            "velocityY": -1.3015636152871453,
+            "timestamp": 2.193519735692838
+        },
+        {
+            "x": 7.61384847194871,
+            "y": 2.5532697230888193,
+            "heading": 3.1400000157074612,
+            "angularVelocity": -2.2667026217565558e-7,
+            "velocityX": 0.8255463113387961,
+            "velocityY": -1.201440596782113,
+            "timestamp": 2.2080886850145593
+        },
+        {
+            "x": 7.624873586342355,
+            "y": 2.5372246741263043,
+            "heading": 3.1400000127888834,
+            "angularVelocity": -2.0032864319885596e-7,
+            "velocityX": 0.7567542552436427,
+            "velocityY": -1.1013181944833246,
+            "timestamp": 2.2226576343362807
+        },
+        {
+            "x": 7.634896460774273,
+            "y": 2.5226382952448834,
+            "heading": 3.140000010228267,
+            "angularVelocity": -1.757585156167035e-7,
+            "velocityX": 0.6879613766637577,
+            "velocityY": -1.0011963498064516,
+            "timestamp": 2.237226583658002
+        },
+        {
+            "x": 7.643917084349478,
+            "y": 2.509510579057917,
+            "heading": 3.140000008002173,
+            "angularVelocity": -1.527971366621834e-7,
+            "velocityX": 0.6191677502613596,
+            "velocityY": -0.9010750121421621,
+            "timestamp": 2.2517955329797235
+        },
+        {
+            "x": 7.6519354471190555,
+            "y": 2.4978415188200733,
+            "heading": 3.140000006089223,
+            "angularVelocity": -1.3130320584948841e-7,
+            "velocityX": 0.5503734409743167,
+            "velocityY": -0.8009541374715159,
+            "timestamp": 2.266364482301445
+        },
+        {
+            "x": 7.658951539962077,
+            "y": 2.487631108347293,
+            "heading": 3.1400000044698366,
+            "angularVelocity": -1.1115325658440658e-7,
+            "velocityX": 0.4815785056345368,
+            "velocityY": -0.7008336872693404,
+            "timestamp": 2.280933431623166
+        },
+        {
+            "x": 7.664965354486367,
+            "y": 2.478879341949535,
+            "heading": 3.1400000031260147,
+            "angularVelocity": -9.223877588817121e-8,
+            "velocityX": 0.4127829942631362,
+            "velocityY": -0.600713627626585,
+            "timestamp": 2.2955023809448876
+        },
+        {
+            "x": 7.669976882944524,
+            "y": 2.4715862143738576,
+            "heading": 3.140000002041154,
+            "angularVelocity": -7.44638832915239e-8,
+            "velocityX": 0.34398695111687533,
+            "velocityY": -0.5005939285411971,
+            "timestamp": 2.310071330266609
+        },
+        {
+            "x": 7.673986118162365,
+            "y": 2.4657517207559176,
+            "heading": 3.1400000011998928,
+            "angularVelocity": -5.774344306809202e-8,
+            "velocityX": 0.27519041554106066,
+            "velocityY": -0.4004745633401276,
+            "timestamp": 2.3246402795883303
+        },
+        {
+            "x": 7.676993053477586,
+            "y": 2.4613758565783885,
+            "heading": 3.1400000005879747,
+            "angularVelocity": -4.2001516695103525e-8,
+            "velocityX": 0.20639342267033312,
+            "velocityY": -0.30035550820440443,
+            "timestamp": 2.3392092289100517
+        },
+        {
+            "x": 7.6789976826868624,
+            "y": 2.458458617635108,
+            "heading": 3.1400000001921353,
+            "angularVelocity": -2.7170084001525683e-8,
+            "velocityX": 0.13759600400877475,
+            "velocityY": -0.2002367417759812,
+            "timestamp": 2.353778178231773
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -1.3187979232574289e-8,
+            "velocityX": 0.06879818791344262,
+            "velocityY": -0.10011824483002574,
+            "timestamp": 2.3683471275534944
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 2.7611774763020037e-32,
+            "velocityX": -1.1210620234419235e-31,
+            "velocityY": 4.358756882663372e-31,
+            "timestamp": 2.382916076875216
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.traj
new file mode 100644
index 00000000..92207f04
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.traj	
@@ -0,0 +1,1093 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 4.411137018023184e-34,
+            "velocityY": 2.784782344800776e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 1.3544641699489606,
+            "y": 5.55925259826767,
+            "heading": 3.1400000009005375,
+            "angularVelocity": 3.865132870171629e-8,
+            "velocityX": 0.19160342420680077,
+            "velocityY": -0.032078691629102596,
+            "timestamp": 0.023299009229303495
+        },
+        {
+            "x": 1.3633925116373848,
+            "y": 5.557757806179583,
+            "heading": 3.1400000027300097,
+            "angularVelocity": 7.852144487332144e-8,
+            "velocityX": 0.383206925262499,
+            "velocityY": -0.06415689497258387,
+            "timestamp": 0.04659801845860699
+        },
+        {
+            "x": 1.3767850270669844,
+            "y": 5.555515636454451,
+            "heading": 3.140000005520194,
+            "angularVelocity": 1.1975551924808857e-7,
+            "velocityX": 0.5748105122322142,
+            "velocityY": -0.09623455242535102,
+            "timestamp": 0.06989702768791048
+        },
+        {
+            "x": 1.3946417184903905,
+            "y": 5.55252610340551,
+            "heading": 3.1400000093068963,
+            "angularVelocity": 1.6252630014020525e-7,
+            "velocityX": 0.7664141958854979,
+            "velocityY": -0.12831159554996177,
+            "timestamp": 0.09319603691721398
+        },
+        {
+            "x": 1.4169625884614865,
+            "y": 5.548789223260415,
+            "heading": 3.14000001413076,
+            "angularVelocity": 2.070415703537556e-7,
+            "velocityX": 0.9580179891521703,
+            "velocityY": -0.16038794217918884,
+            "timestamp": 0.11649504614651748
+        },
+        {
+            "x": 1.443747639900165,
+            "y": 5.544305014572816,
+            "heading": 3.1400000200383236,
+            "angularVelocity": 2.5355430687602407e-7,
+            "velocityX": 1.1496219077414547,
+            "velocityY": -0.1924634924804626,
+            "timestamp": 0.13979405537582099
+        },
+        {
+            "x": 1.474996876177077,
+            "y": 5.539073498761034,
+            "heading": 3.1400000270834036,
+            "angularVelocity": 3.0237681109940727e-7,
+            "velocityX": 1.3412259710000187,
+            "velocityY": -0.22453812350142868,
+            "timestamp": 0.1630930646051245
+        },
+        {
+            "x": 1.5107103012267313,
+            "y": 5.5330947008269575,
+            "heading": 3.1400000353289426,
+            "angularVelocity": 3.539008296544223e-7,
+            "velocityX": 1.5328302031287921,
+            "velocityY": -0.25661168143439866,
+            "timestamp": 0.186392073834428
+        },
+        {
+            "x": 1.5508879197018797,
+            "y": 5.526368650337421,
+            "heading": 3.140000044849547,
+            "angularVelocity": 4.086269951598913e-7,
+            "velocityX": 1.7244346349549924,
+            "velocityY": -0.2886839703505207,
+            "timestamp": 0.2096910830637315
+        },
+        {
+            "x": 1.5955297371899198,
+            "y": 5.5188953827998875,
+            "heading": 3.1400000557350607,
+            "angularVelocity": 4.6720929680542457e-7,
+            "velocityX": 1.9160393065938472,
+            "velocityY": -0.3207547352763359,
+            "timestamp": 0.232990092293035
+        },
+        {
+            "x": 1.6446357605259214,
+            "y": 5.510674941652546,
+            "heading": 3.140000068095764,
+            "angularVelocity": 5.305248328425052e-7,
+            "velocityX": 2.107644271595818,
+            "velocityY": -0.3528236358230067,
+            "timestamp": 0.25628910152233847
+        },
+        {
+            "x": 1.6982059982629796,
+            "y": 5.501707381254886,
+            "heading": 3.1400000820702374,
+            "angularVelocity": 5.997883043629641e-7,
+            "velocityX": 2.299249603699023,
+            "velocityY": -0.38489020324511874,
+            "timestamp": 0.27958811075164197
+        },
+        {
+            "x": 1.7562404614129117,
+            "y": 5.491992771597749,
+            "heading": 3.1400000978378597,
+            "angularVelocity": 6.767507519595272e-7,
+            "velocityX": 2.490855408432506,
+            "velocityY": -0.41695376664068867,
+            "timestamp": 0.3028871199809455
+        },
+        {
+            "x": 1.8187391646840712,
+            "y": 5.4815312061757675,
+            "heading": 3.140000115639928,
+            "angularVelocity": 7.640697380946438e-7,
+            "velocityX": 2.6824618444526434,
+            "velocityY": -0.4490133172205749,
+            "timestamp": 0.326186129210249
+        },
+        {
+            "x": 1.8857021287172593,
+            "y": 5.47032281621074,
+            "heading": 3.140000135818395,
+            "angularVelocity": 8.660654300433757e-7,
+            "velocityX": 2.874069166381749,
+            "velocityY": -0.48106723572405463,
+            "timestamp": 0.3494851384395525
+        },
+        {
+            "x": 1.9571293845792233,
+            "y": 5.458367799245384,
+            "heading": 3.14000015889542,
+            "angularVelocity": 9.904723735244152e-7,
+            "velocityX": 3.0656778217043392,
+            "velocityY": -0.5131126756388134,
+            "timestamp": 0.372784147668856
+        },
+        {
+            "x": 2.033020984323074,
+            "y": 5.445666486377279,
+            "heading": 3.140000185766696,
+            "angularVelocity": 0.000001153322655289093,
+            "velocityX": 3.2572887111610362,
+            "velocityY": -0.545143904752752,
+            "timestamp": 0.3960831568981595
+        },
+        {
+            "x": 2.113377033060567,
+            "y": 5.432219546645115,
+            "heading": 3.1400002183250084,
+            "angularVelocity": 0.0000013974118596484493,
+            "velocityX": 3.448904112043669,
+            "velocityY": -0.5771464185388335,
+            "timestamp": 0.419382166127463
+        },
+        {
+            "x": 2.1981978506136755,
+            "y": 5.41802904044786,
+            "heading": 3.1400002631776545,
+            "angularVelocity": 0.0000019250880740696254,
+            "velocityX": 3.640533239775037,
+            "velocityY": -0.6090604994243719,
+            "timestamp": 0.4426811753567665
+        },
+        {
+            "x": 2.2851188032420633,
+            "y": 5.403428173451014,
+            "heading": 3.140000263177585,
+            "angularVelocity": -2.9753453619857404e-12,
+            "velocityX": 3.73067162525748,
+            "velocityY": -0.6266732998482398,
+            "timestamp": 0.46598018458607
+        },
+        {
+            "x": 2.372043080680341,
+            "y": 5.388847120989839,
+            "heading": 3.140000263177669,
+            "angularVelocity": 3.609665835664611e-12,
+            "velocityX": 3.730814327029464,
+            "velocityY": -0.6258228544259767,
+            "timestamp": 0.4892791938153735
+        },
+        {
+            "x": 2.458969517114403,
+            "y": 5.374278945126162,
+            "heading": 3.140000263177672,
+            "angularVelocity": 1.1048975247425585e-13,
+            "velocityX": 3.7309069917332867,
+            "velocityY": -0.6252701872544597,
+            "timestamp": 0.5125782030446769
+        },
+        {
+            "x": 2.5458971210754204,
+            "y": 5.359717737461923,
+            "heading": 3.1400002631776736,
+            "angularVelocity": 8.62983817390415e-14,
+            "velocityX": 3.7309571023169674,
+            "velocityY": -0.6249711101842599,
+            "timestamp": 0.5358772122739804
+        },
+        {
+            "x": 2.632825281306951,
+            "y": 5.345159851008328,
+            "heading": 3.140000263177678,
+            "angularVelocity": 1.7012303031669384e-13,
+            "velocityX": 3.7309809776027962,
+            "velocityY": -0.6248285628956001,
+            "timestamp": 0.5591762215032839
+        },
+        {
+            "x": 2.7197537163641496,
+            "y": 5.330603605663881,
+            "heading": 3.1400002631776815,
+            "angularVelocity": 1.5893105684431692e-13,
+            "velocityX": 3.7309927731970562,
+            "velocityY": -0.6247581260336735,
+            "timestamp": 0.5824752307325874
+        },
+        {
+            "x": 2.8066823211500065,
+            "y": 5.316048373923756,
+            "heading": 3.140000263177685,
+            "angularVelocity": 1.4661255756791518e-13,
+            "velocityX": 3.7310000579983225,
+            "velocityY": -0.6247146218539804,
+            "timestamp": 0.605774239961891
+        },
+        {
+            "x": 2.8936112175364808,
+            "y": 5.301494883804396,
+            "heading": 3.140000263177688,
+            "angularVelocity": 1.4712645864153266e-13,
+            "velocityX": 3.731012573578327,
+            "velocityY": -0.6246398710039542,
+            "timestamp": 0.6290732491911945
+        },
+        {
+            "x": 2.9805406318812673,
+            "y": 5.286944487835434,
+            "heading": 3.1400002631776913,
+            "angularVelocity": 1.2989281986974027e-13,
+            "velocityX": 3.7310348044942168,
+            "velocityY": -0.6245070692018166,
+            "timestamp": 0.652372258420498
+        },
+        {
+            "x": 3.067470472751592,
+            "y": 5.272396640371872,
+            "heading": 3.140000263177696,
+            "angularVelocity": 2.2497040560944553e-13,
+            "velocityX": 3.7310531110902496,
+            "velocityY": -0.6243976866335479,
+            "timestamp": 0.6756712676498015
+        },
+        {
+            "x": 3.1544000489958055,
+            "y": 5.257847211710117,
+            "heading": 3.1400002631777,
+            "angularVelocity": 1.6698909622359586e-13,
+            "velocityX": 3.73104175326474,
+            "velocityY": -0.6244655520950864,
+            "timestamp": 0.698970276879105
+        },
+        {
+            "x": 3.24132763546162,
+            "y": 5.243285899348815,
+            "heading": 3.1400002631776998,
+            "angularVelocity": -2.5660759099238358e-14,
+            "velocityX": 3.7309563514197523,
+            "velocityY": -0.6249756038008819,
+            "timestamp": 0.7222692861084085
+        },
+        {
+            "x": 3.3282514237632603,
+            "y": 5.228701930838363,
+            "heading": 3.1400002631777038,
+            "angularVelocity": 1.8102189988940347e-13,
+            "velocityX": 3.730793333150296,
+            "velocityY": -0.62594801206375,
+            "timestamp": 0.745568295337712
+        },
+        {
+            "x": 3.4151697732878015,
+            "y": 5.214085582937486,
+            "heading": 3.140000263177707,
+            "angularVelocity": 1.4033623034653362e-13,
+            "velocityX": 3.7305598993143207,
+            "velocityY": -0.6273377445673929,
+            "timestamp": 0.7688673045670155
+        },
+        {
+            "x": 3.502081162717683,
+            "y": 5.199427905968822,
+            "heading": 3.1400002631777144,
+            "angularVelocity": 3.3137702069013046e-13,
+            "velocityX": 3.7302611701022674,
+            "velocityY": -0.6291115997313443,
+            "timestamp": 0.792166313796319
+        },
+        {
+            "x": 3.588983960416736,
+            "y": 5.18471937597994,
+            "heading": 3.140000263177713,
+            "angularVelocity": -6.829626544199834e-14,
+            "velocityX": 3.729892410608752,
+            "velocityY": -0.6312942256021684,
+            "timestamp": 0.8154653230256225
+        },
+        {
+            "x": 3.675876953607923,
+            "y": 5.169953034745646,
+            "heading": 3.140000263177716,
+            "angularVelocity": 1.3540589961163151e-13,
+            "velocityX": 3.7294715983848317,
+            "velocityY": -0.6337755004484098,
+            "timestamp": 0.838764332254926
+        },
+        {
+            "x": 3.7627586434088305,
+            "y": 5.155120331788361,
+            "heading": 3.1400002631777193,
+            "angularVelocity": 1.3533863072281271e-13,
+            "velocityX": 3.728986453707805,
+            "velocityY": -0.6366237641880743,
+            "timestamp": 0.8620633414842295
+        },
+        {
+            "x": 3.8490562562014863,
+            "y": 5.137199419063008,
+            "heading": 3.1400002631777224,
+            "angularVelocity": 1.3559815009211054e-13,
+            "velocityX": 3.7039177049679806,
+            "velocityY": -0.7691705921529395,
+            "timestamp": 0.885362350713533
+        },
+        {
+            "x": 3.9343637094417163,
+            "y": 5.1150391964944655,
+            "heading": 3.1400002631777255,
+            "angularVelocity": 1.370480919312873e-13,
+            "velocityX": 3.6614197797262853,
+            "velocityY": -0.9511229576576579,
+            "timestamp": 0.9086613599428365
+        },
+        {
+            "x": 4.018431663513184,
+            "y": 5.088562488555908,
+            "heading": 3.140000263177729,
+            "angularVelocity": 1.413301961445022e-13,
+            "velocityX": 3.6082201283346866,
+            "velocityY": -1.1363877183834126,
+            "timestamp": 0.93196036917214
+        },
+        {
+            "x": 4.095482183944048,
+            "y": 5.0601896166081985,
+            "heading": 3.140000263177732,
+            "angularVelocity": 1.478757127486931e-13,
+            "velocityX": 3.549906540385796,
+            "velocityY": -1.3072078310833697,
+            "timestamp": 0.9536653125429956
+        },
+        {
+            "x": 4.171099571738343,
+            "y": 5.028192533992809,
+            "heading": 3.1400002631777353,
+            "angularVelocity": 1.4523452590479803e-13,
+            "velocityX": 3.483878603243662,
+            "velocityY": -1.4741841095171686,
+            "timestamp": 0.9753702559138512
+        },
+        {
+            "x": 4.2451557679063034,
+            "y": 4.992731534520922,
+            "heading": 3.1400002631777384,
+            "angularVelocity": 1.4237189069796376e-13,
+            "velocityX": 3.4119506742159467,
+            "velocityY": -1.6337752587458831,
+            "timestamp": 0.9970751992847068
+        },
+        {
+            "x": 4.31781550635648,
+            "y": 4.954490651976443,
+            "heading": 3.1400002631777415,
+            "angularVelocity": 1.4182458601020346e-13,
+            "velocityX": 3.347612440573966,
+            "velocityY": -1.761851293089538,
+            "timestamp": 1.0187801426555625
+        },
+        {
+            "x": 4.390182790341615,
+            "y": 4.91569917224677,
+            "heading": 3.140000263177745,
+            "angularVelocity": 1.421722636666585e-13,
+            "velocityX": 3.3341383457540337,
+            "velocityY": -1.787218656449447,
+            "timestamp": 1.040485086026418
+        },
+        {
+            "x": 4.462533664586848,
+            "y": 4.87687709490227,
+            "heading": 3.1400002631777486,
+            "angularVelocity": 1.635268036057102e-13,
+            "velocityX": 3.333382308772387,
+            "velocityY": -1.7886283636415046,
+            "timestamp": 1.0621900293972737
+        },
+        {
+            "x": 4.534860115355159,
+            "y": 4.838009534998386,
+            "heading": 3.140000263177751,
+            "angularVelocity": 1.1808318022164126e-13,
+            "velocityX": 3.3322570592611274,
+            "velocityY": -1.7907238567558525,
+            "timestamp": 1.0838949727681293
+        },
+        {
+            "x": 4.607177418550424,
+            "y": 4.799124957707118,
+            "heading": 3.140000263177754,
+            "angularVelocity": 1.4385253669729274e-13,
+            "velocityX": 3.3318356081243334,
+            "velocityY": -1.7915078895502723,
+            "timestamp": 1.105599916138985
+        },
+        {
+            "x": 4.679489672949611,
+            "y": 4.7602309921876325,
+            "heading": 3.1400002631777575,
+            "angularVelocity": 1.4120580598782794e-13,
+            "velocityX": 3.331602997698768,
+            "velocityY": -1.7919404282640008,
+            "timestamp": 1.1273048595098405
+        },
+        {
+            "x": 4.751800053049455,
+            "y": 4.721333542151952,
+            "heading": 3.1400002631777606,
+            "angularVelocity": 1.4195129118241079e-13,
+            "velocityX": 3.3315166441264745,
+            "velocityY": -1.7921009684797435,
+            "timestamp": 1.149009802880696
+        },
+        {
+            "x": 4.824110260922509,
+            "y": 4.682435771933003,
+            "heading": 3.1400002631777637,
+            "angularVelocity": 1.420416383867913e-13,
+            "velocityX": 3.3315087092142113,
+            "velocityY": -1.7921157201114153,
+            "timestamp": 1.1707147462515517
+        },
+        {
+            "x": 4.896420645274053,
+            "y": 4.643538329777489,
+            "heading": 3.140000263177767,
+            "angularVelocity": 1.4171119036284518e-13,
+            "velocityX": 3.3315168400108037,
+            "velocityY": -1.7921006054248252,
+            "timestamp": 1.1924196896224073
+        },
+        {
+            "x": 4.96873125432361,
+            "y": 4.6046413053457185,
+            "heading": 3.14000026317777,
+            "angularVelocity": 1.4137748376763704e-13,
+            "velocityX": 3.3315271924000274,
+            "velocityY": -1.792081359866257,
+            "timestamp": 1.214124632993263
+        },
+        {
+            "x": 5.04104400801269,
+            "y": 4.56574826813338,
+            "heading": 3.1400002631777735,
+            "angularVelocity": 1.421351986736705e-13,
+            "velocityX": 3.331626001206414,
+            "velocityY": -1.7918976588811946,
+            "timestamp": 1.2358295763641185
+        },
+        {
+            "x": 5.11336222581346,
+            "y": 4.5268653919090065,
+            "heading": 3.1400002631777766,
+            "angularVelocity": 1.4239328375117255e-13,
+            "velocityX": 3.3318777462404188,
+            "velocityY": -1.7914295172313686,
+            "timestamp": 1.257534519734974
+        },
+        {
+            "x": 5.185674775789392,
+            "y": 4.487971975945349,
+            "heading": 3.1400002631777797,
+            "angularVelocity": 1.4189340221530015e-13,
+            "velocityX": 3.331616615642773,
+            "velocityY": -1.7919151088820495,
+            "timestamp": 1.2792394631058297
+        },
+        {
+            "x": 5.25798679998857,
+            "y": 4.4490775824474165,
+            "heading": 3.140000263177783,
+            "angularVelocity": 1.4182847689207558e-13,
+            "velocityX": 3.331592391818018,
+            "velocityY": -1.791960146283848,
+            "timestamp": 1.3009444064766853
+        },
+        {
+            "x": 5.330301009987797,
+            "y": 4.410187253034054,
+            "heading": 3.1400002631777864,
+            "angularVelocity": 1.4221031722558925e-13,
+            "velocityX": 3.331693096990619,
+            "velocityY": -1.7917729039338035,
+            "timestamp": 1.322649349847541
+        },
+        {
+            "x": 5.4026181052176305,
+            "y": 4.3713022890025295,
+            "heading": 3.1400002631777895,
+            "angularVelocity": 1.4161705945188678e-13,
+            "velocityX": 3.3318260266449937,
+            "velocityY": -1.791525707625898,
+            "timestamp": 1.3443542932183965
+        },
+        {
+            "x": 5.474936056465776,
+            "y": 4.332418917012381,
+            "heading": 3.1400002631777926,
+            "angularVelocity": 1.4174219306640175e-13,
+            "velocityX": 3.331865465508907,
+            "velocityY": -1.7914523583735324,
+            "timestamp": 1.366059236589252
+        },
+        {
+            "x": 5.54725002868687,
+            "y": 4.293528145471586,
+            "heading": 3.1400002631777957,
+            "angularVelocity": 1.4114123611773667e-13,
+            "velocityX": 3.331682141968083,
+            "velocityY": -1.7917932738304505,
+            "timestamp": 1.3877641799601077
+        },
+        {
+            "x": 5.619560804995751,
+            "y": 4.2546314319979865,
+            "heading": 3.1400002631777992,
+            "angularVelocity": 1.438802546332647e-13,
+            "velocityX": 3.331534898449109,
+            "velocityY": -1.7920670332572342,
+            "timestamp": 1.4094691233309633
+        },
+        {
+            "x": 5.691871264235437,
+            "y": 4.215734129079228,
+            "heading": 3.1400002631778023,
+            "angularVelocity": 1.4227513855970737e-13,
+            "velocityX": 3.331520290292617,
+            "velocityY": -1.7920941904433432,
+            "timestamp": 1.431174066701819
+        },
+        {
+            "x": 5.7641829192535345,
+            "y": 4.1768390492074925,
+            "heading": 3.1400002631778055,
+            "angularVelocity": 1.4198857424426595e-13,
+            "velocityX": 3.3315753827383325,
+            "velocityY": -1.79199176921174,
+            "timestamp": 1.4528790100726745
+        },
+        {
+            "x": 5.836496897649906,
+            "y": 4.13794828914952,
+            "heading": 3.1400002631778086,
+            "angularVelocity": 1.420236059365936e-13,
+            "velocityX": 3.331682426477163,
+            "velocityY": -1.7917927447907471,
+            "timestamp": 1.47458395344353
+        },
+        {
+            "x": 5.90881236073979,
+            "y": 4.099060289876852,
+            "heading": 3.1400002631778117,
+            "angularVelocity": 1.4199973949506742e-13,
+            "velocityX": 3.3317508299509484,
+            "velocityY": -1.7916655486362405,
+            "timestamp": 1.4962888968143857
+        },
+        {
+            "x": 5.981129099244504,
+            "y": 4.06017466243704,
+            "heading": 3.140000263177815,
+            "angularVelocity": 1.4813660843765197e-13,
+            "velocityX": 3.3318095914418606,
+            "velocityY": -1.7915562724764487,
+            "timestamp": 1.5179938401852413
+        },
+        {
+            "x": 6.053446077627243,
+            "y": 4.021289481108035,
+            "heading": 3.140000263177829,
+            "angularVelocity": 6.425453029046258e-13,
+            "velocityX": 3.331820643210356,
+            "velocityY": -1.7915357190582988,
+            "timestamp": 1.539698783556097
+        },
+        {
+            "x": 6.125761091363011,
+            "y": 3.9824006462217088,
+            "heading": 3.140000263177869,
+            "angularVelocity": 1.8383501574993634e-12,
+            "velocityX": 3.331730127102014,
+            "velocityY": -1.7917040474119204,
+            "timestamp": 1.5614037269269525
+        },
+        {
+            "x": 6.198074029937981,
+            "y": 3.9435079527230394,
+            "heading": 3.140000263178238,
+            "angularVelocity": 1.7022488728392152e-11,
+            "velocityX": 3.331634519350639,
+            "velocityY": -1.79188182315355,
+            "timestamp": 1.583108670297808
+        },
+        {
+            "x": 6.270380104650612,
+            "y": 3.9046024999630506,
+            "heading": 3.140000263177847,
+            "angularVelocity": -1.8008440986287348e-11,
+            "velocityX": 3.33131828436938,
+            "velocityY": -1.7924696736324577,
+            "timestamp": 1.6048136136686637
+        },
+        {
+            "x": 6.342678298341779,
+            "y": 3.8656824038223503,
+            "heading": 3.1400002631782598,
+            "angularVelocity": 1.900200591260141e-11,
+            "velocityX": 3.3309551863724884,
+            "velocityY": -1.7931443301131518,
+            "timestamp": 1.6265185570395193
+        },
+        {
+            "x": 6.414967377910046,
+            "y": 3.8267453820171355,
+            "heading": 3.1400002631778587,
+            "angularVelocity": -1.8477715999059415e-11,
+            "velocityX": 3.3305352763678626,
+            "velocityY": -1.7939241370065324,
+            "timestamp": 1.648223500410375
+        },
+        {
+            "x": 6.487241234316261,
+            "y": 3.787780110681628,
+            "heading": 3.1400002631778627,
+            "angularVelocity": 1.954226890690534e-13,
+            "velocityX": 3.3298339079393355,
+            "velocityY": -1.795225662182324,
+            "timestamp": 1.6699284437812305
+        },
+        {
+            "x": 6.559496489740858,
+            "y": 3.7487803574447534,
+            "heading": 3.1400002631778654,
+            "angularVelocity": 1.160046399416623e-13,
+            "velocityX": 3.3289769150756516,
+            "velocityY": -1.79681432798537,
+            "timestamp": 1.691633387152086
+        },
+        {
+            "x": 6.630585131325424,
+            "y": 3.7076925524537225,
+            "heading": 3.140000263177869,
+            "angularVelocity": 1.5938094021424355e-13,
+            "velocityX": 3.275228152865242,
+            "velocityY": -1.8930159958948245,
+            "timestamp": 1.7133383305229417
+        },
+        {
+            "x": 6.699761336489547,
+            "y": 3.6634607167510884,
+            "heading": 3.140000263177872,
+            "angularVelocity": 1.431049344650049e-13,
+            "velocityX": 3.1871175142993193,
+            "velocityY": -2.037869205501672,
+            "timestamp": 1.7350432738937973
+        },
+        {
+            "x": 6.76677589557052,
+            "y": 3.6160175775021286,
+            "heading": 3.140000263178055,
+            "angularVelocity": 8.443311888840044e-12,
+            "velocityX": 3.087525175069758,
+            "velocityY": -2.1858218396761058,
+            "timestamp": 1.756748217264653
+        },
+        {
+            "x": 6.8314564099651545,
+            "y": 3.5654385941456423,
+            "heading": 3.140000263178058,
+            "angularVelocity": 1.5118918552164865e-13,
+            "velocityX": 2.9799900091650118,
+            "velocityY": -2.3302978723451035,
+            "timestamp": 1.7784531606355085
+        },
+        {
+            "x": 6.893649101257324,
+            "y": 3.5118300914764404,
+            "heading": 3.1400002631780617,
+            "angularVelocity": 1.5501587028498165e-13,
+            "velocityX": 2.865369894292817,
+            "velocityY": -2.469875260820684,
+            "timestamp": 1.8001581040063641
+        },
+        {
+            "x": 6.9342196158595195,
+            "y": 3.4745269537623855,
+            "heading": 3.140000263178065,
+            "angularVelocity": 2.3326761756772743e-13,
+            "velocityX": 2.784724807966257,
+            "velocityY": -2.5604549024296013,
+            "timestamp": 1.8147270533280853
+        },
+        {
+            "x": 6.973574770579654,
+            "y": 3.435943763171971,
+            "heading": 3.140000263178068,
+            "angularVelocity": 2.2697466808256025e-13,
+            "velocityX": 2.701303563563437,
+            "velocityY": -2.648316617649584,
+            "timestamp": 1.8292960026498064
+        },
+        {
+            "x": 7.011676569760775,
+            "y": 3.396122365046946,
+            "heading": 3.140000263178071,
+            "angularVelocity": 2.224027473304724e-13,
+            "velocityX": 2.615274330340327,
+            "velocityY": -2.7333061050358776,
+            "timestamp": 1.8438649519715276
+        },
+        {
+            "x": 7.048491012484172,
+            "y": 3.355107883496788,
+            "heading": 3.1400002631780746,
+            "angularVelocity": 2.1692590269440246e-13,
+            "velocityX": 2.5269113036529633,
+            "velocityY": -2.815198312825684,
+            "timestamp": 1.8584339012932487
+        },
+        {
+            "x": 7.083992093604447,
+            "y": 3.312951458599195,
+            "heading": 3.1400002631780777,
+            "angularVelocity": 2.1487378521690259e-13,
+            "velocityX": 2.4367633064212786,
+            "velocityY": -2.893580310196796,
+            "timestamp": 1.8730028506149698
+        },
+        {
+            "x": 7.118173429008876,
+            "y": 3.269718054236438,
+            "heading": 3.140000263178081,
+            "angularVelocity": 2.1169602046875272e-13,
+            "velocityX": 2.3461771092494,
+            "velocityY": -2.967503243236169,
+            "timestamp": 1.887571799936691
+        },
+        {
+            "x": 7.151084190942531,
+            "y": 3.2255097664946186,
+            "heading": 3.140000263178084,
+            "angularVelocity": 2.115299519607656e-13,
+            "velocityX": 2.258966052177459,
+            "velocityY": -3.034418389794455,
+            "timestamp": 1.9021407492584121
+        },
+        {
+            "x": 7.1829258498520225,
+            "y": 3.1805253278057903,
+            "heading": 3.1400002631783006,
+            "angularVelocity": 1.4861478341138462e-11,
+            "velocityX": 2.1855837511923784,
+            "velocityY": -3.0876927152018316,
+            "timestamp": 1.9167096985801333
+        },
+        {
+            "x": 7.213982568749707,
+            "y": 3.1352979487738755,
+            "heading": 3.140000221251036,
+            "angularVelocity": -0.000002877850999701821,
+            "velocityX": 2.1317061520275473,
+            "velocityY": -3.104367928885527,
+            "timestamp": 1.9312786479018544
+        },
+        {
+            "x": 7.244041792187231,
+            "y": 3.091532444769153,
+            "heading": 3.1400001920667906,
+            "angularVelocity": -0.000002003181212693182,
+            "velocityX": 2.063238931905195,
+            "velocityY": -3.0040260994983523,
+            "timestamp": 1.9458475972235756
+        },
+        {
+            "x": 7.273101015062598,
+            "y": 3.049227126183516,
+            "heading": 3.140000168751395,
+            "angularVelocity": -0.0000016003484573673396,
+            "velocityX": 1.9945997637621933,
+            "velocityY": -2.9038002433409447,
+            "timestamp": 1.9604165465452967
+        },
+        {
+            "x": 7.301159421127851,
+            "y": 3.008381441393539,
+            "heading": 3.1400001491208855,
+            "angularVelocity": -0.0000013474210949527603,
+            "velocityX": 1.92590456907015,
+            "velocityY": -2.8036122501352616,
+            "timestamp": 1.9749854958670179
+        },
+        {
+            "x": 7.328216598566981,
+            "y": 2.9689951117692965,
+            "heading": 3.1400001321528324,
+            "angularVelocity": -0.0000011646724122855417,
+            "velocityX": 1.8571811076855331,
+            "velocityY": -2.703443381845065,
+            "timestamp": 1.989554445188739
+        },
+        {
+            "x": 7.35427229894417,
+            "y": 2.9310679691270813,
+            "heading": 3.140000117255664,
+            "angularVelocity": -0.0000010225286768555236,
+            "velocityX": 1.7884405938827321,
+            "velocityY": -2.603286057538151,
+            "timestamp": 2.00412339451046
+        },
+        {
+            "x": 7.379326356096679,
+            "y": 2.8945999009401007,
+            "heading": 3.1400001040435335,
+            "angularVelocity": -9.068691499457651e-7,
+            "velocityX": 1.7196886748142857,
+            "velocityY": -2.50313645697232,
+            "timestamp": 2.0186923438321815
+        },
+        {
+            "x": 7.403378651082293,
+            "y": 2.8595908266403365,
+            "heading": 3.1400000922448146,
+            "angularVelocity": -8.098538018281321e-7,
+            "velocityX": 1.650928591655569,
+            "velocityY": -2.4029923865251464,
+            "timestamp": 2.033261293153903
+        },
+        {
+            "x": 7.426429094558334,
+            "y": 2.826040685699055,
+            "heading": 3.1400000816578917,
+            "angularVelocity": -7.266771822683861e-7,
+            "velocityX": 1.5821623760933374,
+            "velocityY": -2.3028524707174887,
+            "timestamp": 2.0478302424756243
+        },
+        {
+            "x": 7.448477616956692,
+            "y": 2.793949430978311,
+            "heading": 3.1400000721271697,
+            "angularVelocity": -6.541804651955507e-7,
+            "velocityX": 1.513391385436516,
+            "velocityY": -2.202715790417496,
+            "timestamp": 2.0623991917973457
+        },
+        {
+            "x": 7.4695241625734585,
+            "y": 2.763317024730304,
+            "heading": 3.1400000635289262,
+            "angularVelocity": -5.901759591769764e-7,
+            "velocityX": 1.4446165713119874,
+            "velocityY": -2.102581701093514,
+            "timestamp": 2.076968141119067
+        },
+        {
+            "x": 7.489568685800359,
+            "y": 2.734143436045934,
+            "heading": 3.1400000557624246,
+            "angularVelocity": -5.330859058092693e-7,
+            "velocityX": 1.3758386266751614,
+            "velocityY": -2.0024497333432913,
+            "timestamp": 2.0915370904407884
+        },
+        {
+            "x": 7.508611148608458,
+            "y": 2.7064286391508827,
+            "heading": 3.140000048744052,
+            "angularVelocity": -4.817349694037634e-7,
+            "velocityX": 1.3070580717652986,
+            "velocityY": -1.902319534719876,
+            "timestamp": 2.1061060397625098
+        },
+        {
+            "x": 7.526651518803911,
+            "y": 2.680172612224319,
+            "heading": 3.1400000424032948,
+            "angularVelocity": -4.3522407744534453e-7,
+            "velocityX": 1.2382753070979928,
+            "velocityY": -1.8021908338592867,
+            "timestamp": 2.120674989084231
+        },
+        {
+            "x": 7.543689768780771,
+            "y": 2.6553753365541346,
+            "heading": 3.1400000366798815,
+            "angularVelocity": -3.9285011556875146e-7,
+            "velocityX": 1.1694906475824893,
+            "velocityY": -1.7020634173815092,
+            "timestamp": 2.1352439384059525
+        },
+        {
+            "x": 7.559725874605572,
+            "y": 2.6320367959168407,
+            "heading": 3.1400000315217036,
+            "angularVelocity": -3.5405282918878005e-7,
+            "velocityX": 1.100704345294715,
+            "velocityY": -1.6019371144699315,
+            "timestamp": 2.149812887727674
+        },
+        {
+            "x": 7.574759815330251,
+            "y": 2.6101569761120964,
+            "heading": 3.140000026883262,
+            "angularVelocity": -3.1837857149022665e-7,
+            "velocityX": 1.0319166051501305,
+            "velocityY": -1.5018117862573288,
+            "timestamp": 2.1643818370493952
+        },
+        {
+            "x": 7.588791572466392,
+            "y": 2.5897358646065065,
+            "heading": 3.1400000227244846,
+            "angularVelocity": -2.854548657017254e-7,
+            "velocityX": 0.9631275959768569,
+            "velocityY": -1.4016873183261147,
+            "timestamp": 2.1789507863711166
+        },
+        {
+            "x": 7.601821129576078,
+            "y": 2.570773450256394,
+            "heading": 3.140000019009809,
+            "angularVelocity": -2.5497212819286766e-7,
+            "velocityX": 0.8943374585191634,
+            "velocityY": -1.3015636152871453,
+            "timestamp": 2.193519735692838
+        },
+        {
+            "x": 7.61384847194871,
+            "y": 2.5532697230888193,
+            "heading": 3.1400000157074612,
+            "angularVelocity": -2.2667026217565558e-7,
+            "velocityX": 0.8255463113387961,
+            "velocityY": -1.201440596782113,
+            "timestamp": 2.2080886850145593
+        },
+        {
+            "x": 7.624873586342355,
+            "y": 2.5372246741263043,
+            "heading": 3.1400000127888834,
+            "angularVelocity": -2.0032864319885596e-7,
+            "velocityX": 0.7567542552436427,
+            "velocityY": -1.1013181944833246,
+            "timestamp": 2.2226576343362807
+        },
+        {
+            "x": 7.634896460774273,
+            "y": 2.5226382952448834,
+            "heading": 3.140000010228267,
+            "angularVelocity": -1.757585156167035e-7,
+            "velocityX": 0.6879613766637577,
+            "velocityY": -1.0011963498064516,
+            "timestamp": 2.237226583658002
+        },
+        {
+            "x": 7.643917084349478,
+            "y": 2.509510579057917,
+            "heading": 3.140000008002173,
+            "angularVelocity": -1.527971366621834e-7,
+            "velocityX": 0.6191677502613596,
+            "velocityY": -0.9010750121421621,
+            "timestamp": 2.2517955329797235
+        },
+        {
+            "x": 7.6519354471190555,
+            "y": 2.4978415188200733,
+            "heading": 3.140000006089223,
+            "angularVelocity": -1.3130320584948841e-7,
+            "velocityX": 0.5503734409743167,
+            "velocityY": -0.8009541374715159,
+            "timestamp": 2.266364482301445
+        },
+        {
+            "x": 7.658951539962077,
+            "y": 2.487631108347293,
+            "heading": 3.1400000044698366,
+            "angularVelocity": -1.1115325658440658e-7,
+            "velocityX": 0.4815785056345368,
+            "velocityY": -0.7008336872693404,
+            "timestamp": 2.280933431623166
+        },
+        {
+            "x": 7.664965354486367,
+            "y": 2.478879341949535,
+            "heading": 3.1400000031260147,
+            "angularVelocity": -9.223877588817121e-8,
+            "velocityX": 0.4127829942631362,
+            "velocityY": -0.600713627626585,
+            "timestamp": 2.2955023809448876
+        },
+        {
+            "x": 7.669976882944524,
+            "y": 2.4715862143738576,
+            "heading": 3.140000002041154,
+            "angularVelocity": -7.44638832915239e-8,
+            "velocityX": 0.34398695111687533,
+            "velocityY": -0.5005939285411971,
+            "timestamp": 2.310071330266609
+        },
+        {
+            "x": 7.673986118162365,
+            "y": 2.4657517207559176,
+            "heading": 3.1400000011998928,
+            "angularVelocity": -5.774344306809202e-8,
+            "velocityX": 0.27519041554106066,
+            "velocityY": -0.4004745633401276,
+            "timestamp": 2.3246402795883303
+        },
+        {
+            "x": 7.676993053477586,
+            "y": 2.4613758565783885,
+            "heading": 3.1400000005879747,
+            "angularVelocity": -4.2001516695103525e-8,
+            "velocityX": 0.20639342267033312,
+            "velocityY": -0.30035550820440443,
+            "timestamp": 2.3392092289100517
+        },
+        {
+            "x": 7.6789976826868624,
+            "y": 2.458458617635108,
+            "heading": 3.1400000001921353,
+            "angularVelocity": -2.7170084001525683e-8,
+            "velocityX": 0.13759600400877475,
+            "velocityY": -0.2002367417759812,
+            "timestamp": 2.353778178231773
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -1.3187979232574289e-8,
+            "velocityX": 0.06879818791344262,
+            "velocityY": -0.10011824483002574,
+            "timestamp": 2.3683471275534944
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 2.7611774763020037e-32,
+            "velocityX": -1.1210620234419235e-31,
+            "velocityY": 4.358756882663372e-31,
+            "timestamp": 2.382916076875216
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.1.traj
new file mode 100644
index 00000000..24664a13
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.1.traj	
@@ -0,0 +1,625 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 3.295386080925321e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 1.3546830312136446,
+            "y": 5.550921973415328,
+            "heading": 3.14,
+            "angularVelocity": -6.105321890596023e-17,
+            "velocityX": 0.13379730014070298,
+            "velocityY": -0.2593652256885782,
+            "timestamp": 0.03500093954603884
+        },
+        {
+            "x": 1.364049093509165,
+            "y": 5.532765920501418,
+            "heading": 3.14,
+            "angularVelocity": -9.872410216704159e-17,
+            "velocityX": 0.2675945965166781,
+            "velocityY": -0.5187304440792354,
+            "timestamp": 0.07000187909207768
+        },
+        {
+            "x": 1.3780981867284383,
+            "y": 5.505531841564791,
+            "heading": 3.14,
+            "angularVelocity": -1.396517144912572e-16,
+            "velocityX": 0.40139188837498685,
+            "velocityY": -0.7780956537124009,
+            "timestamp": 0.10500281863811653
+        },
+        {
+            "x": 1.3968303106782043,
+            "y": 5.46921973698008,
+            "heading": 3.14,
+            "angularVelocity": -1.3147784440878345e-16,
+            "velocityX": 0.5351891747117138,
+            "velocityY": -1.0374608526419855,
+            "timestamp": 0.14000375818415536
+        },
+        {
+            "x": 1.4202454651168874,
+            "y": 5.423829607215582,
+            "heading": 3.14,
+            "angularVelocity": -7.833997421538468e-18,
+            "velocityX": 0.6689864541464796,
+            "velocityY": -1.2968260381921248,
+            "timestamp": 0.1750046977301942
+        },
+        {
+            "x": 1.4483436497338924,
+            "y": 5.369361452873384,
+            "heading": 3.14,
+            "angularVelocity": -2.0049167233544137e-17,
+            "velocityX": 0.802783724707322,
+            "velocityY": -1.5561912065401715,
+            "timestamp": 0.21000563727623303
+        },
+        {
+            "x": 1.4811248641150931,
+            "y": 5.305815274756268,
+            "heading": 3.14,
+            "angularVelocity": -8.129804229935506e-17,
+            "velocityX": 0.9365809834363131,
+            "velocityY": -1.8155563519521838,
+            "timestamp": 0.24500657682227187
+        },
+        {
+            "x": 1.5185891076807168,
+            "y": 5.233191073988126,
+            "heading": 3.14,
+            "angularVelocity": -1.3088931274135507e-16,
+            "velocityX": 1.0703782256008025,
+            "velocityY": -2.0749214652539205,
+            "timestamp": 0.2800075163683107
+        },
+        {
+            "x": 1.56073637956111,
+            "y": 5.151488852254778,
+            "heading": 3.14,
+            "angularVelocity": -1.390272479709275e-16,
+            "velocityX": 1.204175442918738,
+            "velocityY": -2.334286530390624,
+            "timestamp": 0.31500845591434956
+        },
+        {
+            "x": 1.6075666783068707,
+            "y": 5.060708612365892,
+            "heading": 3.14,
+            "angularVelocity": -2.002620206188659e-16,
+            "velocityX": 1.3379726188262815,
+            "velocityY": -2.593651515253289,
+            "timestamp": 0.3500093954603884
+        },
+        {
+            "x": 1.6590800010192643,
+            "y": 4.9608503599406655,
+            "heading": 3.14,
+            "angularVelocity": -2.3956924314672257e-16,
+            "velocityX": 1.471769711915058,
+            "velocityY": -2.8530163395717247,
+            "timestamp": 0.38501033500642723
+        },
+        {
+            "x": 1.7152763390026582,
+            "y": 4.851914111835608,
+            "heading": 3.14,
+            "angularVelocity": -2.6938408534488506e-16,
+            "velocityX": 1.6055665565638675,
+            "velocityY": -3.112380682288517,
+            "timestamp": 0.42001127455246606
+        },
+        {
+            "x": 1.7759790338056225,
+            "y": 4.734242319347403,
+            "heading": 3.14,
+            "angularVelocity": 3.279195870571412e-16,
+            "velocityX": 1.734316152375285,
+            "velocityY": -3.3619609648879023,
+            "timestamp": 0.4550122140985049
+        },
+        {
+            "x": 1.8366817286107124,
+            "y": 4.616570526856246,
+            "heading": 3.14,
+            "angularVelocity": -3.1071543812317194e-16,
+            "velocityX": 1.7343161524360031,
+            "velocityY": -3.36196096497222,
+            "timestamp": 0.49001315364454373
+        },
+        {
+            "x": 1.8973844234157842,
+            "y": 4.498898734365081,
+            "heading": 3.14,
+            "angularVelocity": -2.903673566166758e-16,
+            "velocityX": 1.7343161524354895,
+            "velocityY": -3.361960964972484,
+            "timestamp": 0.5250140931905826
+        },
+        {
+            "x": 1.958087118220856,
+            "y": 4.381226941873915,
+            "heading": 3.14,
+            "angularVelocity": 3.614460545323391e-16,
+            "velocityX": 1.7343161524354895,
+            "velocityY": -3.3619609649724835,
+            "timestamp": 0.5600150327366215
+        },
+        {
+            "x": 2.0187898130259274,
+            "y": 4.26355514938275,
+            "heading": 3.14,
+            "angularVelocity": -3.3191632817458094e-16,
+            "velocityX": 1.7343161524354893,
+            "velocityY": -3.361960964972483,
+            "timestamp": 0.5950159722826603
+        },
+        {
+            "x": 2.079492507830999,
+            "y": 4.145883356891584,
+            "heading": 3.14,
+            "angularVelocity": 3.2426066858348807e-16,
+            "velocityX": 1.7343161524354893,
+            "velocityY": -3.361960964972483,
+            "timestamp": 0.6300169118286992
+        },
+        {
+            "x": 2.140195202636071,
+            "y": 4.028211564400418,
+            "heading": 3.14,
+            "angularVelocity": -2.798848785469217e-16,
+            "velocityX": 1.734316152435488,
+            "velocityY": -3.3619609649724826,
+            "timestamp": 0.6650178513747381
+        },
+        {
+            "x": 2.200897897441142,
+            "y": 3.9105397719092525,
+            "heading": 3.14,
+            "angularVelocity": -1.795170282800533e-16,
+            "velocityX": 1.734316152435471,
+            "velocityY": -3.361960964972468,
+            "timestamp": 0.700018790920777
+        },
+        {
+            "x": 2.2616005922462064,
+            "y": 3.792867979418094,
+            "heading": 3.14,
+            "angularVelocity": -3.385716636422731e-16,
+            "velocityX": 1.7343161524352515,
+            "velocityY": -3.3619609649722846,
+            "timestamp": 0.7350197304668159
+        },
+        {
+            "x": 2.3223032870512124,
+            "y": 3.6751961869269927,
+            "heading": 3.14,
+            "angularVelocity": -3.759822562526031e-16,
+            "velocityX": 1.7343161524334345,
+            "velocityY": -3.361960964970739,
+            "timestamp": 0.7700206700128548
+        },
+        {
+            "x": 2.383005981879538,
+            "y": 3.5575243944476096,
+            "heading": 3.14,
+            "angularVelocity": -2.6556763706975105e-16,
+            "velocityX": 1.7343161531001825,
+            "velocityY": -3.3619609646356636,
+            "timestamp": 0.8050216095588937
+        },
+        {
+            "x": 2.443708981028518,
+            "y": 3.4398527589551926,
+            "heading": 3.14,
+            "angularVelocity": 5.092773468950765e-17,
+            "velocityX": 1.7343248477441577,
+            "velocityY": -3.3619564794134043,
+            "timestamp": 0.8400225491049326
+        },
+        {
+            "x": 2.507799181067132,
+            "y": 3.3239911422386283,
+            "heading": 3.14,
+            "angularVelocity": -2.1081778211927598e-16,
+            "velocityX": 1.8310994181462568,
+            "velocityY": -3.310243045471397,
+            "timestamp": 0.8750234886509715
+        },
+        {
+            "x": 2.5806292795037855,
+            "y": 3.2134142705817266,
+            "heading": 3.14,
+            "angularVelocity": -3.875593095285532e-16,
+            "velocityX": 2.0808040978720626,
+            "velocityY": -3.159254382625257,
+            "timestamp": 0.9100244281970103
+        },
+        {
+            "x": 2.661766753531331,
+            "y": 3.108780729790167,
+            "heading": 3.14,
+            "angularVelocity": -7.347130647585027e-16,
+            "velocityX": 2.318151314794784,
+            "velocityY": -2.9894494876003863,
+            "timestamp": 0.9450253677430492
+        },
+        {
+            "x": 2.7507288455963135,
+            "y": 3.0107133388519287,
+            "heading": 3.14,
+            "angularVelocity": -3.52994704864956e-16,
+            "velocityX": 2.541705829014059,
+            "velocityY": -2.801850242022933,
+            "timestamp": 0.9800263072890881
+        },
+        {
+            "x": 2.9215337593923705,
+            "y": 2.8658352980219064,
+            "heading": 3.14,
+            "angularVelocity": 1.5913396610629706e-16,
+            "velocityX": 2.884920936848367,
+            "velocityY": -2.4470121145413266,
+            "timestamp": 1.0392324060123015
+        },
+        {
+            "x": 3.109750180151373,
+            "y": 2.744432888397421,
+            "heading": 3.14,
+            "angularVelocity": -6.286818756935585e-16,
+            "velocityX": 3.179003934013425,
+            "velocityY": -2.0505051378666,
+            "timestamp": 1.098438504735515
+        },
+        {
+            "x": 3.3097585780303125,
+            "y": 2.6436330516151556,
+            "heading": 3.14,
+            "angularVelocity": -3.4341251732407706e-17,
+            "velocityX": 3.3781722186064402,
+            "velocityY": -1.702524553319067,
+            "timestamp": 1.1576446034587284
+        },
+        {
+            "x": 3.5097675494852,
+            "y": 2.5428343529427293,
+            "heading": 3.14,
+            "angularVelocity": -2.2516273301091217e-16,
+            "velocityX": 3.3781819063939547,
+            "velocityY": -1.7025053304654465,
+            "timestamp": 1.216850702181942
+        },
+        {
+            "x": 3.709776520948374,
+            "y": 2.442035654287211,
+            "heading": 3.14,
+            "angularVelocity": -3.629334864774022e-16,
+            "velocityX": 3.378181906533671,
+            "velocityY": -1.7025053301803685,
+            "timestamp": 1.2760568009051554
+        },
+        {
+            "x": 3.909785492411555,
+            "y": 2.3412369556316897,
+            "heading": 3.14,
+            "angularVelocity": -2.39059813931354e-16,
+            "velocityX": 3.3781819065337895,
+            "velocityY": -1.7025053301803958,
+            "timestamp": 1.3352628996283689
+        },
+        {
+            "x": 4.109794463874735,
+            "y": 2.240438256976168,
+            "heading": 3.14,
+            "angularVelocity": 3.7738636092274995e-16,
+            "velocityX": 3.378181906533792,
+            "velocityY": -1.7025053301803967,
+            "timestamp": 1.3944689983515823
+        },
+        {
+            "x": 4.3098034353379155,
+            "y": 2.139639558320647,
+            "heading": 3.14,
+            "angularVelocity": 3.725508803000677e-16,
+            "velocityX": 3.3781819065337926,
+            "velocityY": -1.7025053301803965,
+            "timestamp": 1.4536750970747958
+        },
+        {
+            "x": 4.509812406801097,
+            "y": 2.0388408596651253,
+            "heading": 3.14,
+            "angularVelocity": -2.2963176234131617e-16,
+            "velocityX": 3.3781819065337926,
+            "velocityY": -1.7025053301803967,
+            "timestamp": 1.5128811957980093
+        },
+        {
+            "x": 4.709821378264278,
+            "y": 1.9380421610096037,
+            "heading": 3.14,
+            "angularVelocity": -2.0546569648664528e-16,
+            "velocityX": 3.3781819065338015,
+            "velocityY": -1.702505330180399,
+            "timestamp": 1.5720872945212228
+        },
+        {
+            "x": 4.9098303497278835,
+            "y": 1.8372434623549267,
+            "heading": 3.14,
+            "angularVelocity": 1.0642243284106308e-16,
+            "velocityX": 3.3781819065409677,
+            "velocityY": -1.7025053301661326,
+            "timestamp": 1.6312933932444362
+        },
+        {
+            "x": 5.109839349236117,
+            "y": 1.736444819347842,
+            "heading": 3.14,
+            "angularVelocity": 1.2679224790319894e-16,
+            "velocityX": 3.378182380218786,
+            "velocityY": -1.7025043902702677,
+            "timestamp": 1.6904994919676497
+        },
+        {
+            "x": 5.311619758605957,
+            "y": 1.6392407417297363,
+            "heading": 3.14,
+            "angularVelocity": 3.145701878302232e-16,
+            "velocityX": 3.408101761838499,
+            "velocityY": -1.6417916348765778,
+            "timestamp": 1.7497055906908632
+        },
+        {
+            "x": 5.430192834880249,
+            "y": 1.5926107881863976,
+            "heading": 3.14,
+            "angularVelocity": 2.0229336550458753e-17,
+            "velocityX": 3.5204949744296554,
+            "velocityY": -1.384467049902731,
+            "timestamp": 1.78338638760593
+        },
+        {
+            "x": 5.550146685206584,
+            "y": 1.5496573158737061,
+            "heading": 3.14,
+            "angularVelocity": -2.618637002687426e-16,
+            "velocityX": 3.561490858687975,
+            "velocityY": -1.2753104512640405,
+            "timestamp": 1.8170671845209967
+        },
+        {
+            "x": 5.670100671705041,
+            "y": 1.506704223843596,
+            "heading": 3.14,
+            "angularVelocity": -1.632751475559818e-17,
+            "velocityX": 3.5614949017075617,
+            "velocityY": -1.2752991604806918,
+            "timestamp": 1.8507479814360634
+        },
+        {
+            "x": 5.790054658213967,
+            "y": 1.4637511318444056,
+            "heading": 3.14,
+            "angularVelocity": -2.9492764372093784e-16,
+            "velocityX": 3.561494902018399,
+            "velocityY": -1.2752991595627796,
+            "timestamp": 1.8844287783511302
+        },
+        {
+            "x": 5.910008644722922,
+            "y": 1.4207980398452127,
+            "heading": 3.14,
+            "angularVelocity": -2.6299005747305263e-17,
+            "velocityX": 3.5614949020192235,
+            "velocityY": -1.2752991595628407,
+            "timestamp": 1.918109575266197
+        },
+        {
+            "x": 6.02996263123188,
+            "y": 1.3778449478460184,
+            "heading": 3.14,
+            "angularVelocity": -3.0549772131007975e-16,
+            "velocityX": 3.5614949020193487,
+            "velocityY": -1.2752991595628858,
+            "timestamp": 1.9517903721812637
+        },
+        {
+            "x": 6.149916617740839,
+            "y": 1.3348918558468237,
+            "heading": 3.14,
+            "angularVelocity": 3.2741509193878177e-16,
+            "velocityX": 3.56149490201936,
+            "velocityY": -1.27529915956289,
+            "timestamp": 1.9854711690963305
+        },
+        {
+            "x": 6.269870604249798,
+            "y": 1.2919387638476292,
+            "heading": 3.14,
+            "angularVelocity": 3.4990315690965786e-16,
+            "velocityX": 3.5614949020193607,
+            "velocityY": -1.2752991595628904,
+            "timestamp": 2.019151966011397
+        },
+        {
+            "x": 6.3898245907587565,
+            "y": 1.2489856718484345,
+            "heading": 3.14,
+            "angularVelocity": 2.004607715018742e-16,
+            "velocityX": 3.5614949020193603,
+            "velocityY": -1.27529915956289,
+            "timestamp": 2.0528327629264638
+        },
+        {
+            "x": 6.509778577267715,
+            "y": 1.2060325798492397,
+            "heading": 3.14,
+            "angularVelocity": 3.130071946862803e-16,
+            "velocityX": 3.5614949020193603,
+            "velocityY": -1.2752991595628902,
+            "timestamp": 2.0865135598415305
+        },
+        {
+            "x": 6.629732563776674,
+            "y": 1.1630794878500452,
+            "heading": 3.14,
+            "angularVelocity": -2.481040891393771e-16,
+            "velocityX": 3.5614949020193607,
+            "velocityY": -1.2752991595628902,
+            "timestamp": 2.1201943567565973
+        },
+        {
+            "x": 6.749686550285633,
+            "y": 1.1201263958508503,
+            "heading": 3.14,
+            "angularVelocity": 5.765266318677121e-16,
+            "velocityX": 3.561494902019357,
+            "velocityY": -1.2752991595628989,
+            "timestamp": 2.153875153671664
+        },
+        {
+            "x": 6.869640536794461,
+            "y": 1.077173303851719,
+            "heading": 3.14,
+            "angularVelocity": 5.94200130731494e-16,
+            "velocityX": 3.5614949020154905,
+            "velocityY": -1.2752991595610093,
+            "timestamp": 2.187555950586731
+        },
+        {
+            "x": 6.985406128017608,
+            "y": 1.0357199913369377,
+            "heading": 3.14,
+            "angularVelocity": -5.216843428237177e-16,
+            "velocityX": 3.437139314579594,
+            "velocityY": -1.2307699434580792,
+            "timestamp": 2.2212367475017976
+        },
+        {
+            "x": 7.092266700241795,
+            "y": 0.9974553856875887,
+            "heading": 3.14,
+            "angularVelocity": -4.51325473252053e-16,
+            "velocityX": 3.172744768885923,
+            "velocityY": -1.1360956139440452,
+            "timestamp": 2.2549175444168643
+        },
+        {
+            "x": 7.190222235939472,
+            "y": 0.9623794931799317,
+            "heading": 3.14,
+            "angularVelocity": -3.932803198593372e-16,
+            "velocityX": 2.908349702790401,
+            "velocityY": -1.0414210980846788,
+            "timestamp": 2.288598341331931
+        },
+        {
+            "x": 7.279272729268112,
+            "y": 0.9304923159060571,
+            "heading": 3.14,
+            "angularVelocity": -3.6674368533996665e-16,
+            "velocityX": 2.643954463227235,
+            "velocityY": -0.9467465201100715,
+            "timestamp": 2.322279138246998
+        },
+        {
+            "x": 7.35941817730645,
+            "y": 0.9017938549120107,
+            "heading": 3.14,
+            "angularVelocity": -3.61350987503525e-16,
+            "velocityX": 2.3795591369302214,
+            "velocityY": -0.8520719110778341,
+            "timestamp": 2.3559599351620646
+        },
+        {
+            "x": 7.430658578301727,
+            "y": 0.8762841108254199,
+            "heading": 3.14,
+            "angularVelocity": -3.11296740830968e-16,
+            "velocityX": 2.115163758592898,
+            "velocityY": -0.7573972834110186,
+            "timestamp": 2.3896407320771313
+        },
+        {
+            "x": 7.492993931085436,
+            "y": 0.8539630840647031,
+            "heading": 3.14,
+            "angularVelocity": -3.0797715285651212e-16,
+            "velocityX": 1.8507683455620387,
+            "velocityY": -0.6627226433211519,
+            "timestamp": 2.423321528992198
+        },
+        {
+            "x": 7.546424234822929,
+            "y": 0.8348307749287305,
+            "heading": 3.14,
+            "angularVelocity": -2.384214973522538e-16,
+            "velocityX": 1.5863729077500837,
+            "velocityY": -0.5680479943576783,
+            "timestamp": 2.457002325907265
+        },
+        {
+            "x": 7.590949488888224,
+            "y": 0.8188871836416546,
+            "heading": 3.14,
+            "angularVelocity": -1.6873896303460275e-16,
+            "velocityX": 1.3219774513523088,
+            "velocityY": -0.4733733387389998,
+            "timestamp": 2.4906831228223316
+        },
+        {
+            "x": 7.626569692794441,
+            "y": 0.8061323103778162,
+            "heading": 3.14,
+            "angularVelocity": -1.2706181170586285e-16,
+            "velocityX": 1.0575819804988977,
+            "velocityY": -0.3786986779440515,
+            "timestamp": 2.5243639197373984
+        },
+        {
+            "x": 7.653284846152079,
+            "y": 0.7965661552766882,
+            "heading": 3.14,
+            "angularVelocity": -1.0566459548829111e-16,
+            "velocityX": 0.7931864980809785,
+            "velocityY": -0.28402401300808755,
+            "timestamp": 2.558044716652465
+        },
+        {
+            "x": 7.671094948642455,
+            "y": 0.7901887184523844,
+            "heading": 3.14,
+            "angularVelocity": -6.428789472113484e-17,
+            "velocityX": 0.5287910062011897,
+            "velocityY": -0.18934934468402026,
+            "timestamp": 2.591725513567532
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": -8.910267594003032e-17,
+            "velocityX": 0.26439550643651,
+            "velocityY": -0.09467467353653364,
+            "timestamp": 2.6254063104825986
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": -3.385083394775927e-32,
+            "velocityX": 1.13191163707898e-33,
+            "velocityY": 3.1068609037049197e-32,
+            "timestamp": 2.6590871073976654
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.traj
new file mode 100644
index 00000000..24664a13
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.traj	
@@ -0,0 +1,625 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 3.295386080925321e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 1.3546830312136446,
+            "y": 5.550921973415328,
+            "heading": 3.14,
+            "angularVelocity": -6.105321890596023e-17,
+            "velocityX": 0.13379730014070298,
+            "velocityY": -0.2593652256885782,
+            "timestamp": 0.03500093954603884
+        },
+        {
+            "x": 1.364049093509165,
+            "y": 5.532765920501418,
+            "heading": 3.14,
+            "angularVelocity": -9.872410216704159e-17,
+            "velocityX": 0.2675945965166781,
+            "velocityY": -0.5187304440792354,
+            "timestamp": 0.07000187909207768
+        },
+        {
+            "x": 1.3780981867284383,
+            "y": 5.505531841564791,
+            "heading": 3.14,
+            "angularVelocity": -1.396517144912572e-16,
+            "velocityX": 0.40139188837498685,
+            "velocityY": -0.7780956537124009,
+            "timestamp": 0.10500281863811653
+        },
+        {
+            "x": 1.3968303106782043,
+            "y": 5.46921973698008,
+            "heading": 3.14,
+            "angularVelocity": -1.3147784440878345e-16,
+            "velocityX": 0.5351891747117138,
+            "velocityY": -1.0374608526419855,
+            "timestamp": 0.14000375818415536
+        },
+        {
+            "x": 1.4202454651168874,
+            "y": 5.423829607215582,
+            "heading": 3.14,
+            "angularVelocity": -7.833997421538468e-18,
+            "velocityX": 0.6689864541464796,
+            "velocityY": -1.2968260381921248,
+            "timestamp": 0.1750046977301942
+        },
+        {
+            "x": 1.4483436497338924,
+            "y": 5.369361452873384,
+            "heading": 3.14,
+            "angularVelocity": -2.0049167233544137e-17,
+            "velocityX": 0.802783724707322,
+            "velocityY": -1.5561912065401715,
+            "timestamp": 0.21000563727623303
+        },
+        {
+            "x": 1.4811248641150931,
+            "y": 5.305815274756268,
+            "heading": 3.14,
+            "angularVelocity": -8.129804229935506e-17,
+            "velocityX": 0.9365809834363131,
+            "velocityY": -1.8155563519521838,
+            "timestamp": 0.24500657682227187
+        },
+        {
+            "x": 1.5185891076807168,
+            "y": 5.233191073988126,
+            "heading": 3.14,
+            "angularVelocity": -1.3088931274135507e-16,
+            "velocityX": 1.0703782256008025,
+            "velocityY": -2.0749214652539205,
+            "timestamp": 0.2800075163683107
+        },
+        {
+            "x": 1.56073637956111,
+            "y": 5.151488852254778,
+            "heading": 3.14,
+            "angularVelocity": -1.390272479709275e-16,
+            "velocityX": 1.204175442918738,
+            "velocityY": -2.334286530390624,
+            "timestamp": 0.31500845591434956
+        },
+        {
+            "x": 1.6075666783068707,
+            "y": 5.060708612365892,
+            "heading": 3.14,
+            "angularVelocity": -2.002620206188659e-16,
+            "velocityX": 1.3379726188262815,
+            "velocityY": -2.593651515253289,
+            "timestamp": 0.3500093954603884
+        },
+        {
+            "x": 1.6590800010192643,
+            "y": 4.9608503599406655,
+            "heading": 3.14,
+            "angularVelocity": -2.3956924314672257e-16,
+            "velocityX": 1.471769711915058,
+            "velocityY": -2.8530163395717247,
+            "timestamp": 0.38501033500642723
+        },
+        {
+            "x": 1.7152763390026582,
+            "y": 4.851914111835608,
+            "heading": 3.14,
+            "angularVelocity": -2.6938408534488506e-16,
+            "velocityX": 1.6055665565638675,
+            "velocityY": -3.112380682288517,
+            "timestamp": 0.42001127455246606
+        },
+        {
+            "x": 1.7759790338056225,
+            "y": 4.734242319347403,
+            "heading": 3.14,
+            "angularVelocity": 3.279195870571412e-16,
+            "velocityX": 1.734316152375285,
+            "velocityY": -3.3619609648879023,
+            "timestamp": 0.4550122140985049
+        },
+        {
+            "x": 1.8366817286107124,
+            "y": 4.616570526856246,
+            "heading": 3.14,
+            "angularVelocity": -3.1071543812317194e-16,
+            "velocityX": 1.7343161524360031,
+            "velocityY": -3.36196096497222,
+            "timestamp": 0.49001315364454373
+        },
+        {
+            "x": 1.8973844234157842,
+            "y": 4.498898734365081,
+            "heading": 3.14,
+            "angularVelocity": -2.903673566166758e-16,
+            "velocityX": 1.7343161524354895,
+            "velocityY": -3.361960964972484,
+            "timestamp": 0.5250140931905826
+        },
+        {
+            "x": 1.958087118220856,
+            "y": 4.381226941873915,
+            "heading": 3.14,
+            "angularVelocity": 3.614460545323391e-16,
+            "velocityX": 1.7343161524354895,
+            "velocityY": -3.3619609649724835,
+            "timestamp": 0.5600150327366215
+        },
+        {
+            "x": 2.0187898130259274,
+            "y": 4.26355514938275,
+            "heading": 3.14,
+            "angularVelocity": -3.3191632817458094e-16,
+            "velocityX": 1.7343161524354893,
+            "velocityY": -3.361960964972483,
+            "timestamp": 0.5950159722826603
+        },
+        {
+            "x": 2.079492507830999,
+            "y": 4.145883356891584,
+            "heading": 3.14,
+            "angularVelocity": 3.2426066858348807e-16,
+            "velocityX": 1.7343161524354893,
+            "velocityY": -3.361960964972483,
+            "timestamp": 0.6300169118286992
+        },
+        {
+            "x": 2.140195202636071,
+            "y": 4.028211564400418,
+            "heading": 3.14,
+            "angularVelocity": -2.798848785469217e-16,
+            "velocityX": 1.734316152435488,
+            "velocityY": -3.3619609649724826,
+            "timestamp": 0.6650178513747381
+        },
+        {
+            "x": 2.200897897441142,
+            "y": 3.9105397719092525,
+            "heading": 3.14,
+            "angularVelocity": -1.795170282800533e-16,
+            "velocityX": 1.734316152435471,
+            "velocityY": -3.361960964972468,
+            "timestamp": 0.700018790920777
+        },
+        {
+            "x": 2.2616005922462064,
+            "y": 3.792867979418094,
+            "heading": 3.14,
+            "angularVelocity": -3.385716636422731e-16,
+            "velocityX": 1.7343161524352515,
+            "velocityY": -3.3619609649722846,
+            "timestamp": 0.7350197304668159
+        },
+        {
+            "x": 2.3223032870512124,
+            "y": 3.6751961869269927,
+            "heading": 3.14,
+            "angularVelocity": -3.759822562526031e-16,
+            "velocityX": 1.7343161524334345,
+            "velocityY": -3.361960964970739,
+            "timestamp": 0.7700206700128548
+        },
+        {
+            "x": 2.383005981879538,
+            "y": 3.5575243944476096,
+            "heading": 3.14,
+            "angularVelocity": -2.6556763706975105e-16,
+            "velocityX": 1.7343161531001825,
+            "velocityY": -3.3619609646356636,
+            "timestamp": 0.8050216095588937
+        },
+        {
+            "x": 2.443708981028518,
+            "y": 3.4398527589551926,
+            "heading": 3.14,
+            "angularVelocity": 5.092773468950765e-17,
+            "velocityX": 1.7343248477441577,
+            "velocityY": -3.3619564794134043,
+            "timestamp": 0.8400225491049326
+        },
+        {
+            "x": 2.507799181067132,
+            "y": 3.3239911422386283,
+            "heading": 3.14,
+            "angularVelocity": -2.1081778211927598e-16,
+            "velocityX": 1.8310994181462568,
+            "velocityY": -3.310243045471397,
+            "timestamp": 0.8750234886509715
+        },
+        {
+            "x": 2.5806292795037855,
+            "y": 3.2134142705817266,
+            "heading": 3.14,
+            "angularVelocity": -3.875593095285532e-16,
+            "velocityX": 2.0808040978720626,
+            "velocityY": -3.159254382625257,
+            "timestamp": 0.9100244281970103
+        },
+        {
+            "x": 2.661766753531331,
+            "y": 3.108780729790167,
+            "heading": 3.14,
+            "angularVelocity": -7.347130647585027e-16,
+            "velocityX": 2.318151314794784,
+            "velocityY": -2.9894494876003863,
+            "timestamp": 0.9450253677430492
+        },
+        {
+            "x": 2.7507288455963135,
+            "y": 3.0107133388519287,
+            "heading": 3.14,
+            "angularVelocity": -3.52994704864956e-16,
+            "velocityX": 2.541705829014059,
+            "velocityY": -2.801850242022933,
+            "timestamp": 0.9800263072890881
+        },
+        {
+            "x": 2.9215337593923705,
+            "y": 2.8658352980219064,
+            "heading": 3.14,
+            "angularVelocity": 1.5913396610629706e-16,
+            "velocityX": 2.884920936848367,
+            "velocityY": -2.4470121145413266,
+            "timestamp": 1.0392324060123015
+        },
+        {
+            "x": 3.109750180151373,
+            "y": 2.744432888397421,
+            "heading": 3.14,
+            "angularVelocity": -6.286818756935585e-16,
+            "velocityX": 3.179003934013425,
+            "velocityY": -2.0505051378666,
+            "timestamp": 1.098438504735515
+        },
+        {
+            "x": 3.3097585780303125,
+            "y": 2.6436330516151556,
+            "heading": 3.14,
+            "angularVelocity": -3.4341251732407706e-17,
+            "velocityX": 3.3781722186064402,
+            "velocityY": -1.702524553319067,
+            "timestamp": 1.1576446034587284
+        },
+        {
+            "x": 3.5097675494852,
+            "y": 2.5428343529427293,
+            "heading": 3.14,
+            "angularVelocity": -2.2516273301091217e-16,
+            "velocityX": 3.3781819063939547,
+            "velocityY": -1.7025053304654465,
+            "timestamp": 1.216850702181942
+        },
+        {
+            "x": 3.709776520948374,
+            "y": 2.442035654287211,
+            "heading": 3.14,
+            "angularVelocity": -3.629334864774022e-16,
+            "velocityX": 3.378181906533671,
+            "velocityY": -1.7025053301803685,
+            "timestamp": 1.2760568009051554
+        },
+        {
+            "x": 3.909785492411555,
+            "y": 2.3412369556316897,
+            "heading": 3.14,
+            "angularVelocity": -2.39059813931354e-16,
+            "velocityX": 3.3781819065337895,
+            "velocityY": -1.7025053301803958,
+            "timestamp": 1.3352628996283689
+        },
+        {
+            "x": 4.109794463874735,
+            "y": 2.240438256976168,
+            "heading": 3.14,
+            "angularVelocity": 3.7738636092274995e-16,
+            "velocityX": 3.378181906533792,
+            "velocityY": -1.7025053301803967,
+            "timestamp": 1.3944689983515823
+        },
+        {
+            "x": 4.3098034353379155,
+            "y": 2.139639558320647,
+            "heading": 3.14,
+            "angularVelocity": 3.725508803000677e-16,
+            "velocityX": 3.3781819065337926,
+            "velocityY": -1.7025053301803965,
+            "timestamp": 1.4536750970747958
+        },
+        {
+            "x": 4.509812406801097,
+            "y": 2.0388408596651253,
+            "heading": 3.14,
+            "angularVelocity": -2.2963176234131617e-16,
+            "velocityX": 3.3781819065337926,
+            "velocityY": -1.7025053301803967,
+            "timestamp": 1.5128811957980093
+        },
+        {
+            "x": 4.709821378264278,
+            "y": 1.9380421610096037,
+            "heading": 3.14,
+            "angularVelocity": -2.0546569648664528e-16,
+            "velocityX": 3.3781819065338015,
+            "velocityY": -1.702505330180399,
+            "timestamp": 1.5720872945212228
+        },
+        {
+            "x": 4.9098303497278835,
+            "y": 1.8372434623549267,
+            "heading": 3.14,
+            "angularVelocity": 1.0642243284106308e-16,
+            "velocityX": 3.3781819065409677,
+            "velocityY": -1.7025053301661326,
+            "timestamp": 1.6312933932444362
+        },
+        {
+            "x": 5.109839349236117,
+            "y": 1.736444819347842,
+            "heading": 3.14,
+            "angularVelocity": 1.2679224790319894e-16,
+            "velocityX": 3.378182380218786,
+            "velocityY": -1.7025043902702677,
+            "timestamp": 1.6904994919676497
+        },
+        {
+            "x": 5.311619758605957,
+            "y": 1.6392407417297363,
+            "heading": 3.14,
+            "angularVelocity": 3.145701878302232e-16,
+            "velocityX": 3.408101761838499,
+            "velocityY": -1.6417916348765778,
+            "timestamp": 1.7497055906908632
+        },
+        {
+            "x": 5.430192834880249,
+            "y": 1.5926107881863976,
+            "heading": 3.14,
+            "angularVelocity": 2.0229336550458753e-17,
+            "velocityX": 3.5204949744296554,
+            "velocityY": -1.384467049902731,
+            "timestamp": 1.78338638760593
+        },
+        {
+            "x": 5.550146685206584,
+            "y": 1.5496573158737061,
+            "heading": 3.14,
+            "angularVelocity": -2.618637002687426e-16,
+            "velocityX": 3.561490858687975,
+            "velocityY": -1.2753104512640405,
+            "timestamp": 1.8170671845209967
+        },
+        {
+            "x": 5.670100671705041,
+            "y": 1.506704223843596,
+            "heading": 3.14,
+            "angularVelocity": -1.632751475559818e-17,
+            "velocityX": 3.5614949017075617,
+            "velocityY": -1.2752991604806918,
+            "timestamp": 1.8507479814360634
+        },
+        {
+            "x": 5.790054658213967,
+            "y": 1.4637511318444056,
+            "heading": 3.14,
+            "angularVelocity": -2.9492764372093784e-16,
+            "velocityX": 3.561494902018399,
+            "velocityY": -1.2752991595627796,
+            "timestamp": 1.8844287783511302
+        },
+        {
+            "x": 5.910008644722922,
+            "y": 1.4207980398452127,
+            "heading": 3.14,
+            "angularVelocity": -2.6299005747305263e-17,
+            "velocityX": 3.5614949020192235,
+            "velocityY": -1.2752991595628407,
+            "timestamp": 1.918109575266197
+        },
+        {
+            "x": 6.02996263123188,
+            "y": 1.3778449478460184,
+            "heading": 3.14,
+            "angularVelocity": -3.0549772131007975e-16,
+            "velocityX": 3.5614949020193487,
+            "velocityY": -1.2752991595628858,
+            "timestamp": 1.9517903721812637
+        },
+        {
+            "x": 6.149916617740839,
+            "y": 1.3348918558468237,
+            "heading": 3.14,
+            "angularVelocity": 3.2741509193878177e-16,
+            "velocityX": 3.56149490201936,
+            "velocityY": -1.27529915956289,
+            "timestamp": 1.9854711690963305
+        },
+        {
+            "x": 6.269870604249798,
+            "y": 1.2919387638476292,
+            "heading": 3.14,
+            "angularVelocity": 3.4990315690965786e-16,
+            "velocityX": 3.5614949020193607,
+            "velocityY": -1.2752991595628904,
+            "timestamp": 2.019151966011397
+        },
+        {
+            "x": 6.3898245907587565,
+            "y": 1.2489856718484345,
+            "heading": 3.14,
+            "angularVelocity": 2.004607715018742e-16,
+            "velocityX": 3.5614949020193603,
+            "velocityY": -1.27529915956289,
+            "timestamp": 2.0528327629264638
+        },
+        {
+            "x": 6.509778577267715,
+            "y": 1.2060325798492397,
+            "heading": 3.14,
+            "angularVelocity": 3.130071946862803e-16,
+            "velocityX": 3.5614949020193603,
+            "velocityY": -1.2752991595628902,
+            "timestamp": 2.0865135598415305
+        },
+        {
+            "x": 6.629732563776674,
+            "y": 1.1630794878500452,
+            "heading": 3.14,
+            "angularVelocity": -2.481040891393771e-16,
+            "velocityX": 3.5614949020193607,
+            "velocityY": -1.2752991595628902,
+            "timestamp": 2.1201943567565973
+        },
+        {
+            "x": 6.749686550285633,
+            "y": 1.1201263958508503,
+            "heading": 3.14,
+            "angularVelocity": 5.765266318677121e-16,
+            "velocityX": 3.561494902019357,
+            "velocityY": -1.2752991595628989,
+            "timestamp": 2.153875153671664
+        },
+        {
+            "x": 6.869640536794461,
+            "y": 1.077173303851719,
+            "heading": 3.14,
+            "angularVelocity": 5.94200130731494e-16,
+            "velocityX": 3.5614949020154905,
+            "velocityY": -1.2752991595610093,
+            "timestamp": 2.187555950586731
+        },
+        {
+            "x": 6.985406128017608,
+            "y": 1.0357199913369377,
+            "heading": 3.14,
+            "angularVelocity": -5.216843428237177e-16,
+            "velocityX": 3.437139314579594,
+            "velocityY": -1.2307699434580792,
+            "timestamp": 2.2212367475017976
+        },
+        {
+            "x": 7.092266700241795,
+            "y": 0.9974553856875887,
+            "heading": 3.14,
+            "angularVelocity": -4.51325473252053e-16,
+            "velocityX": 3.172744768885923,
+            "velocityY": -1.1360956139440452,
+            "timestamp": 2.2549175444168643
+        },
+        {
+            "x": 7.190222235939472,
+            "y": 0.9623794931799317,
+            "heading": 3.14,
+            "angularVelocity": -3.932803198593372e-16,
+            "velocityX": 2.908349702790401,
+            "velocityY": -1.0414210980846788,
+            "timestamp": 2.288598341331931
+        },
+        {
+            "x": 7.279272729268112,
+            "y": 0.9304923159060571,
+            "heading": 3.14,
+            "angularVelocity": -3.6674368533996665e-16,
+            "velocityX": 2.643954463227235,
+            "velocityY": -0.9467465201100715,
+            "timestamp": 2.322279138246998
+        },
+        {
+            "x": 7.35941817730645,
+            "y": 0.9017938549120107,
+            "heading": 3.14,
+            "angularVelocity": -3.61350987503525e-16,
+            "velocityX": 2.3795591369302214,
+            "velocityY": -0.8520719110778341,
+            "timestamp": 2.3559599351620646
+        },
+        {
+            "x": 7.430658578301727,
+            "y": 0.8762841108254199,
+            "heading": 3.14,
+            "angularVelocity": -3.11296740830968e-16,
+            "velocityX": 2.115163758592898,
+            "velocityY": -0.7573972834110186,
+            "timestamp": 2.3896407320771313
+        },
+        {
+            "x": 7.492993931085436,
+            "y": 0.8539630840647031,
+            "heading": 3.14,
+            "angularVelocity": -3.0797715285651212e-16,
+            "velocityX": 1.8507683455620387,
+            "velocityY": -0.6627226433211519,
+            "timestamp": 2.423321528992198
+        },
+        {
+            "x": 7.546424234822929,
+            "y": 0.8348307749287305,
+            "heading": 3.14,
+            "angularVelocity": -2.384214973522538e-16,
+            "velocityX": 1.5863729077500837,
+            "velocityY": -0.5680479943576783,
+            "timestamp": 2.457002325907265
+        },
+        {
+            "x": 7.590949488888224,
+            "y": 0.8188871836416546,
+            "heading": 3.14,
+            "angularVelocity": -1.6873896303460275e-16,
+            "velocityX": 1.3219774513523088,
+            "velocityY": -0.4733733387389998,
+            "timestamp": 2.4906831228223316
+        },
+        {
+            "x": 7.626569692794441,
+            "y": 0.8061323103778162,
+            "heading": 3.14,
+            "angularVelocity": -1.2706181170586285e-16,
+            "velocityX": 1.0575819804988977,
+            "velocityY": -0.3786986779440515,
+            "timestamp": 2.5243639197373984
+        },
+        {
+            "x": 7.653284846152079,
+            "y": 0.7965661552766882,
+            "heading": 3.14,
+            "angularVelocity": -1.0566459548829111e-16,
+            "velocityX": 0.7931864980809785,
+            "velocityY": -0.28402401300808755,
+            "timestamp": 2.558044716652465
+        },
+        {
+            "x": 7.671094948642455,
+            "y": 0.7901887184523844,
+            "heading": 3.14,
+            "angularVelocity": -6.428789472113484e-17,
+            "velocityX": 0.5287910062011897,
+            "velocityY": -0.18934934468402026,
+            "timestamp": 2.591725513567532
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": -8.910267594003032e-17,
+            "velocityX": 0.26439550643651,
+            "velocityY": -0.09467467353653364,
+            "timestamp": 2.6254063104825986
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": -3.385083394775927e-32,
+            "velocityX": 1.13191163707898e-33,
+            "velocityY": 3.1068609037049197e-32,
+            "timestamp": 2.6590871073976654
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.1.traj
new file mode 100644
index 00000000..e00feed7
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.1.traj	
@@ -0,0 +1,112 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -7.821039013033948e-33,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 1.3816658469394465,
+            "y": 5.599900983994057,
+            "heading": 3.1530893621447014,
+            "angularVelocity": 0.16724918062405764,
+            "velocityX": 0.4046100104681557,
+            "velocityY": 0.5098343834730725,
+            "timestamp": 0.07826263839297289
+        },
+        {
+            "x": 1.444997693090777,
+            "y": 5.6797028720542775,
+            "heading": 3.179265627435073,
+            "angularVelocity": 0.3344669414150786,
+            "velocityX": 0.8092219665957109,
+            "velocityY": 1.0196677456683545,
+            "timestamp": 0.15652527678594577
+        },
+        {
+            "x": 1.539995766815641,
+            "y": 5.799405527661274,
+            "heading": 3.2185261574836668,
+            "angularVelocity": 0.5016509902395458,
+            "velocityX": 1.213836840611724,
+            "velocityY": 1.5294993634886844,
+            "timestamp": 0.23478791517891867
+        },
+        {
+            "x": 1.6666603529158708,
+            "y": 5.959008729778703,
+            "heading": 3.270870260258294,
+            "angularVelocity": 0.6688261966303347,
+            "velocityX": 1.6184553536800128,
+            "velocityY": 2.039328157019548,
+            "timestamp": 0.31305055357189154
+        },
+        {
+            "x": 1.8249917488860277,
+            "y": 6.158512141354908,
+            "heading": 3.336302010504542,
+            "angularVelocity": 0.8360534680379705,
+            "velocityX": 2.0230776679817777,
+            "velocityY": 2.5491526438766083,
+            "timestamp": 0.3913131919648644
+        },
+        {
+            "x": 1.9833277736824528,
+            "y": 6.358008431463026,
+            "heading": 3.401912848694198,
+            "angularVelocity": 0.8383417622622024,
+            "velocityX": 2.023136812758436,
+            "velocityY": 2.549061649396556,
+            "timestamp": 0.4695758303578373
+        },
+        {
+            "x": 2.1099966456081427,
+            "y": 6.517605183327805,
+            "heading": 3.454415313467435,
+            "angularVelocity": 0.6708496653232114,
+            "velocityX": 1.618510115767619,
+            "velocityY": 2.0392457389873657,
+            "timestamp": 0.5478384687508102
+        },
+        {
+            "x": 2.20499831408803,
+            "y": 6.637302614330095,
+            "heading": 3.493799088289958,
+            "angularVelocity": 0.5032257489808813,
+            "velocityX": 1.2138827725544437,
+            "velocityY": 1.5294326061595858,
+            "timestamp": 0.6261011071437831
+        },
+        {
+            "x": 2.268332767215697,
+            "y": 6.717100865980731,
+            "heading": 3.520056877037348,
+            "angularVelocity": 0.3355086064891452,
+            "velocityX": 0.8092552772071353,
+            "velocityY": 1.0196212814849352,
+            "timestamp": 0.704363745536756
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 0.16774837153225197,
+            "velocityX": 0.4046277180855394,
+            "velocityY": 0.5098107454405937,
+            "timestamp": 0.7826263839297288
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 3.497795149569243e-31,
+            "velocityX": 5.486835585676403e-32,
+            "velocityY": -3.2429311821904986e-32,
+            "timestamp": 0.8608890223227017
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.traj
new file mode 100644
index 00000000..e00feed7
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.traj	
@@ -0,0 +1,112 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": -7.821039013033948e-33,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 1.3816658469394465,
+            "y": 5.599900983994057,
+            "heading": 3.1530893621447014,
+            "angularVelocity": 0.16724918062405764,
+            "velocityX": 0.4046100104681557,
+            "velocityY": 0.5098343834730725,
+            "timestamp": 0.07826263839297289
+        },
+        {
+            "x": 1.444997693090777,
+            "y": 5.6797028720542775,
+            "heading": 3.179265627435073,
+            "angularVelocity": 0.3344669414150786,
+            "velocityX": 0.8092219665957109,
+            "velocityY": 1.0196677456683545,
+            "timestamp": 0.15652527678594577
+        },
+        {
+            "x": 1.539995766815641,
+            "y": 5.799405527661274,
+            "heading": 3.2185261574836668,
+            "angularVelocity": 0.5016509902395458,
+            "velocityX": 1.213836840611724,
+            "velocityY": 1.5294993634886844,
+            "timestamp": 0.23478791517891867
+        },
+        {
+            "x": 1.6666603529158708,
+            "y": 5.959008729778703,
+            "heading": 3.270870260258294,
+            "angularVelocity": 0.6688261966303347,
+            "velocityX": 1.6184553536800128,
+            "velocityY": 2.039328157019548,
+            "timestamp": 0.31305055357189154
+        },
+        {
+            "x": 1.8249917488860277,
+            "y": 6.158512141354908,
+            "heading": 3.336302010504542,
+            "angularVelocity": 0.8360534680379705,
+            "velocityX": 2.0230776679817777,
+            "velocityY": 2.5491526438766083,
+            "timestamp": 0.3913131919648644
+        },
+        {
+            "x": 1.9833277736824528,
+            "y": 6.358008431463026,
+            "heading": 3.401912848694198,
+            "angularVelocity": 0.8383417622622024,
+            "velocityX": 2.023136812758436,
+            "velocityY": 2.549061649396556,
+            "timestamp": 0.4695758303578373
+        },
+        {
+            "x": 2.1099966456081427,
+            "y": 6.517605183327805,
+            "heading": 3.454415313467435,
+            "angularVelocity": 0.6708496653232114,
+            "velocityX": 1.618510115767619,
+            "velocityY": 2.0392457389873657,
+            "timestamp": 0.5478384687508102
+        },
+        {
+            "x": 2.20499831408803,
+            "y": 6.637302614330095,
+            "heading": 3.493799088289958,
+            "angularVelocity": 0.5032257489808813,
+            "velocityX": 1.2138827725544437,
+            "velocityY": 1.5294326061595858,
+            "timestamp": 0.6261011071437831
+        },
+        {
+            "x": 2.268332767215697,
+            "y": 6.717100865980731,
+            "heading": 3.520056877037348,
+            "angularVelocity": 0.3355086064891452,
+            "velocityX": 0.8092552772071353,
+            "velocityY": 1.0196212814849352,
+            "timestamp": 0.704363745536756
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 0.16774837153225197,
+            "velocityX": 0.4046277180855394,
+            "velocityY": 0.5098107454405937,
+            "timestamp": 0.7826263839297288
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 3.5331853071795862,
+            "angularVelocity": 3.497795149569243e-31,
+            "velocityX": 5.486835585676403e-32,
+            "velocityY": -3.2429311821904986e-32,
+            "timestamp": 0.8608890223227017
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.1.traj
new file mode 100644
index 00000000..79c52dac
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.1.traj	
@@ -0,0 +1,625 @@
+{
+    "samples": [
+        {
+            "x": 0.7328571677207947,
+            "y": 6.758700847625732,
+            "heading": -2.1130989969635645,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7582517080124825,
+            "y": 6.75986365233427,
+            "heading": -2.1364061905138043,
+            "angularVelocity": -0.4153761780165196,
+            "velocityX": 0.4525764573976041,
+            "velocityY": 0.020723274750874442,
+            "timestamp": 0.05611105013661352
+        },
+        {
+            "x": 0.8091370757597145,
+            "y": 6.7622371099463585,
+            "heading": -2.1815821515780387,
+            "angularVelocity": -0.8051170126783445,
+            "velocityX": 0.9068689255207545,
+            "velocityY": 0.04229929053754837,
+            "timestamp": 0.11222210027322704
+        },
+        {
+            "x": 0.8856327770155913,
+            "y": 6.7658721035419465,
+            "heading": -2.246734552492055,
+            "angularVelocity": -1.1611331592509972,
+            "velocityX": 1.3632912068056597,
+            "velocityY": 0.06478213447706996,
+            "timestamp": 0.16833315040984057
+        },
+        {
+            "x": 0.9878912539433133,
+            "y": 6.770811955024787,
+            "heading": -2.329199596325353,
+            "angularVelocity": -1.4696756455728817,
+            "velocityX": 1.8224302820701674,
+            "velocityY": 0.08803705278779066,
+            "timestamp": 0.2244442005464541
+        },
+        {
+            "x": 1.116115460670533,
+            "y": 6.777089415314351,
+            "heading": -2.4245891589263606,
+            "angularVelocity": -1.7000138541118799,
+            "velocityX": 2.285186365520377,
+            "velocityY": 0.1118756514857201,
+            "timestamp": 0.28055525068306764
+        },
+        {
+            "x": 1.270529175155293,
+            "y": 6.784749415960288,
+            "heading": -2.524162862737573,
+            "angularVelocity": -1.774582788395185,
+            "velocityX": 2.751930575328903,
+            "velocityY": 0.13651501134463712,
+            "timestamp": 0.3366663008196812
+        },
+        {
+            "x": 1.4506438070602947,
+            "y": 6.793909267096616,
+            "heading": -2.6053474913460852,
+            "angularVelocity": -1.4468563395419025,
+            "velocityX": 3.2099672251094336,
+            "velocityY": 0.16324504913117607,
+            "timestamp": 0.39277735095629474
+        },
+        {
+            "x": 1.6524388072374525,
+            "y": 6.806354930258429,
+            "heading": -2.6314264655034,
+            "angularVelocity": -0.46477430192125535,
+            "velocityX": 3.5963504458720204,
+            "velocityY": 0.22180413896261764,
+            "timestamp": 0.4488884010929083
+        },
+        {
+            "x": 1.8644608843714687,
+            "y": 6.816501923570639,
+            "heading": -2.6314265116901563,
+            "angularVelocity": -8.231311961492043e-7,
+            "velocityX": 3.7786153817796047,
+            "velocityY": 0.18083770108638533,
+            "timestamp": 0.5049994512295218
+        },
+        {
+            "x": 2.0764830467529034,
+            "y": 6.826647135513074,
+            "heading": -2.631426557872052,
+            "angularVelocity": -8.230445785018772e-7,
+            "velocityX": 3.7786169010421835,
+            "velocityY": 0.1808059538676366,
+            "timestamp": 0.5611105013661354
+        },
+        {
+            "x": 2.288505209106376,
+            "y": 6.836792348039794,
+            "heading": -2.631426604053948,
+            "angularVelocity": -8.230445836057129e-7,
+            "velocityX": 3.778616900543848,
+            "velocityY": 0.18080596428064635,
+            "timestamp": 0.6172215515027489
+        },
+        {
+            "x": 2.5005273714588676,
+            "y": 6.846937560587016,
+            "heading": -2.631426650235844,
+            "angularVelocity": -8.230445885587366e-7,
+            "velocityX": 3.7786169005263646,
+            "velocityY": 0.18080596464604262,
+            "timestamp": 0.6733326016393625
+        },
+        {
+            "x": 2.7125495338113623,
+            "y": 6.857082773134173,
+            "heading": -2.6314266964177406,
+            "angularVelocity": -8.230445944915301e-7,
+            "velocityX": 3.778616900526421,
+            "velocityY": 0.18080596464485496,
+            "timestamp": 0.729443651775976
+        },
+        {
+            "x": 2.9245716961638615,
+            "y": 6.867227985681239,
+            "heading": -2.631426742599637,
+            "angularVelocity": -8.230445986306635e-7,
+            "velocityX": 3.7786169005264982,
+            "velocityY": 0.18080596464325174,
+            "timestamp": 0.7855547019125896
+        },
+        {
+            "x": 3.1365938585163606,
+            "y": 6.877373198228301,
+            "heading": -2.631426788781534,
+            "angularVelocity": -8.230446041948432e-7,
+            "velocityX": 3.7786169005265,
+            "velocityY": 0.18080596464320411,
+            "timestamp": 0.8416657520492031
+        },
+        {
+            "x": 3.34861602086886,
+            "y": 6.887518410775364,
+            "heading": -2.6314268349634315,
+            "angularVelocity": -8.230446102544191e-7,
+            "velocityX": 3.7786169005265,
+            "velocityY": 0.18080596464319879,
+            "timestamp": 0.8977768021858167
+        },
+        {
+            "x": 3.560638183221359,
+            "y": 6.897663623322427,
+            "heading": -2.631426881145329,
+            "angularVelocity": -8.230446143262805e-7,
+            "velocityX": 3.7786169005264996,
+            "velocityY": 0.1808059646431973,
+            "timestamp": 0.9538878523224302
+        },
+        {
+            "x": 3.772660345573858,
+            "y": 6.90780883586949,
+            "heading": -2.631426927327227,
+            "angularVelocity": -8.230446193740569e-7,
+            "velocityX": 3.778616900526499,
+            "velocityY": 0.1808059646432043,
+            "timestamp": 1.0099989024590437
+        },
+        {
+            "x": 3.984682507926357,
+            "y": 6.917954048416554,
+            "heading": -2.631426973509125,
+            "angularVelocity": -8.230446249324534e-7,
+            "velocityX": 3.778616900526498,
+            "velocityY": 0.18080596464321827,
+            "timestamp": 1.0661099525956572
+        },
+        {
+            "x": 4.196704670278859,
+            "y": 6.928099260963566,
+            "heading": -2.6314270196910234,
+            "angularVelocity": -8.230446298795515e-7,
+            "velocityX": 3.7786169005265413,
+            "velocityY": 0.18080596464229606,
+            "timestamp": 1.1222210027322708
+        },
+        {
+            "x": 4.40872683263124,
+            "y": 6.938244473513073,
+            "heading": -2.631427065872922,
+            "angularVelocity": -8.230446358036557e-7,
+            "velocityX": 3.778616900524414,
+            "velocityY": 0.18080596468676038,
+            "timestamp": 1.1783320528688843
+        },
+        {
+            "x": 4.620748994963782,
+            "y": 6.948389686477181,
+            "heading": -2.6314271120548214,
+            "angularVelocity": -8.230446409621988e-7,
+            "velocityX": 3.778616900170846,
+            "velocityY": 0.18080597207568838,
+            "timestamp": 1.2344431030054979
+        },
+        {
+            "x": 4.832771157387297,
+            "y": 6.958534897538403,
+            "heading": -2.631427158236721,
+            "angularVelocity": -8.230446451889797e-7,
+            "velocityX": 3.778616901792118,
+            "velocityY": 0.18080593816277588,
+            "timestamp": 1.2905541531421114
+        },
+        {
+            "x": 5.044793317425079,
+            "y": 6.968680158463288,
+            "heading": -2.6314272044186224,
+            "angularVelocity": -8.230446869472616e-7,
+            "velocityX": 3.7786168592740705,
+            "velocityY": 0.18080682682325677,
+            "timestamp": 1.346665203278725
+        },
+        {
+            "x": 5.256813933209308,
+            "y": 6.978857641482405,
+            "heading": -2.6314272506013574,
+            "angularVelocity": -8.230595289243604e-7,
+            "velocityX": 3.778589337893981,
+            "velocityY": 0.18138108258631616,
+            "timestamp": 1.4027762534153385
+        },
+        {
+            "x": 5.469038653323493,
+            "y": 6.974735659993926,
+            "heading": -2.6314272968552426,
+            "angularVelocity": -8.243275644517052e-7,
+            "velocityX": 3.7822268447565213,
+            "velocityY": -0.07346113606385067,
+            "timestamp": 1.458887303551952
+        },
+        {
+            "x": 5.679131625937882,
+            "y": 6.944449281942863,
+            "heading": -2.631427345537303,
+            "angularVelocity": -8.676020260335946e-7,
+            "velocityX": 3.7442352638718064,
+            "velocityY": -0.5397578191342853,
+            "timestamp": 1.5149983536885656
+        },
+        {
+            "x": 5.8838793004435885,
+            "y": 6.888460800908395,
+            "heading": -2.6314274009810084,
+            "angularVelocity": -9.881067120494687e-7,
+            "velocityX": 3.648972421781526,
+            "velocityY": -0.9978155977862404,
+            "timestamp": 1.5711094038251792
+        },
+        {
+            "x": 6.080149873036162,
+            "y": 6.807626528914084,
+            "heading": -2.6314274705096907,
+            "angularVelocity": -0.0000012391263740452789,
+            "velocityX": 3.497895193811386,
+            "velocityY": -1.440612353493913,
+            "timestamp": 1.6272204539617927
+        },
+        {
+            "x": 6.264941198917061,
+            "y": 6.703182873189846,
+            "heading": -2.631427570246947,
+            "angularVelocity": -0.0000017774975926495223,
+            "velocityX": 3.2933143370331135,
+            "velocityY": -1.8613741049215575,
+            "timestamp": 1.6833315040984063
+        },
+        {
+            "x": 6.435426712036133,
+            "y": 6.576727390289307,
+            "heading": -2.6314277398800354,
+            "angularVelocity": -0.0000030231672442365436,
+            "velocityX": 3.0383589810560285,
+            "velocityY": -2.253664520494037,
+            "timestamp": 1.7394425542350198
+        },
+        {
+            "x": 6.574927462247643,
+            "y": 6.446962020265696,
+            "heading": -2.631427921819672,
+            "angularVelocity": -0.0000036124868348171132,
+            "velocityX": 2.7698451702830824,
+            "velocityY": -2.5765451647028184,
+            "timestamp": 1.7898066510113226
+        },
+        {
+            "x": 6.6991857061041244,
+            "y": 6.302534147821866,
+            "heading": -2.631428035463349,
+            "angularVelocity": -0.0000022564422659644148,
+            "velocityX": 2.4671988938546927,
+            "velocityY": -2.867675222794474,
+            "timestamp": 1.8401707477876255
+        },
+        {
+            "x": 6.806670192415463,
+            "y": 6.145223588911882,
+            "heading": -2.631428117085094,
+            "angularVelocity": -0.0000016206335538188846,
+            "velocityX": 2.134148990872305,
+            "velocityY": -3.123466297999793,
+            "timestamp": 1.8905348445639283
+        },
+        {
+            "x": 6.896056392277072,
+            "y": 5.976968910136802,
+            "heading": -2.631428181121382,
+            "angularVelocity": -0.000001271467013700371,
+            "velocityX": 1.7748000179300458,
+            "velocityY": -3.340766330475413,
+            "timestamp": 1.940898941340231
+        },
+        {
+            "x": 6.966242833371282,
+            "y": 5.799843545392681,
+            "heading": -2.631428234939726,
+            "angularVelocity": -0.0000010685855124139772,
+            "velocityX": 1.393580855940955,
+            "velocityY": -3.5168974742233234,
+            "timestamp": 1.991263038116534
+        },
+        {
+            "x": 7.016364754971619,
+            "y": 5.616030256280146,
+            "heading": -2.6314282829523696,
+            "angularVelocity": -9.53310925320319e-7,
+            "velocityX": 0.9951915115833975,
+            "velocityY": -3.6496889823907637,
+            "timestamp": 2.041627134892837
+        },
+        {
+            "x": 7.046069338416312,
+            "y": 5.427835754816808,
+            "heading": -2.631428328230437,
+            "angularVelocity": -8.990147867657557e-7,
+            "velocityX": 0.5897968065726167,
+            "velocityY": -3.7366797681141786,
+            "timestamp": 2.09199123166914
+        },
+        {
+            "x": 7.072076786928083,
+            "y": 5.239094807157974,
+            "heading": -2.6314283732011274,
+            "angularVelocity": -8.929116824307271e-7,
+            "velocityX": 0.5163886612943162,
+            "velocityY": -3.747529683638862,
+            "timestamp": 2.142355328445443
+        },
+        {
+            "x": 7.098039162505348,
+            "y": 5.0503476542439945,
+            "heading": -2.631428418169392,
+            "angularVelocity": -8.928635140632744e-7,
+            "velocityX": 0.5154937195157807,
+            "velocityY": -3.747652891549277,
+            "timestamp": 2.192719425221746
+        },
+        {
+            "x": 7.124001613673454,
+            "y": 4.861600511727296,
+            "heading": -2.6314284631376603,
+            "angularVelocity": -8.928635936943803e-7,
+            "velocityX": 0.5154952204033956,
+            "velocityY": -3.7476526851069147,
+            "timestamp": 2.243083521998049
+        },
+        {
+            "x": 7.149964068909211,
+            "y": 4.67285336977021,
+            "heading": -2.631428508105929,
+            "angularVelocity": -8.928636032719525e-7,
+            "velocityX": 0.5154953011682646,
+            "velocityY": -3.747652673995615,
+            "timestamp": 2.293447618774352
+        },
+        {
+            "x": 7.175926523495577,
+            "y": 4.484106227723796,
+            "heading": -2.6314285530741985,
+            "angularVelocity": -8.928636081094573e-7,
+            "velocityX": 0.5154952882743342,
+            "velocityY": -3.7476526757692454,
+            "timestamp": 2.3438117155506553
+        },
+        {
+            "x": 7.2018889780951465,
+            "y": 4.295359085679197,
+            "heading": -2.6314285980424676,
+            "angularVelocity": -8.928636141069267e-7,
+            "velocityX": 0.5154952885364941,
+            "velocityY": -3.747652675733191,
+            "timestamp": 2.3941758123269583
+        },
+        {
+            "x": 7.227851432696769,
+            "y": 4.106611943634882,
+            "heading": -2.6314286430107376,
+            "angularVelocity": -8.928636206980889e-7,
+            "velocityX": 0.5154952885772718,
+            "velocityY": -3.7476526757275823,
+            "timestamp": 2.4445399091032614
+        },
+        {
+            "x": 7.253813887298451,
+            "y": 3.9178648015905746,
+            "heading": -2.6314286879790076,
+            "angularVelocity": -8.928636265842637e-7,
+            "velocityX": 0.5154952885784285,
+            "velocityY": -3.747652675727423,
+            "timestamp": 2.4949040058795644
+        },
+        {
+            "x": 7.2797763419001384,
+            "y": 3.729117659546268,
+            "heading": -2.631428732947278,
+            "angularVelocity": -8.928636327298174e-7,
+            "velocityX": 0.5154952885785545,
+            "velocityY": -3.747652675727405,
+            "timestamp": 2.5452681026558674
+        },
+        {
+            "x": 7.30573879650183,
+            "y": 3.5403705175019615,
+            "heading": -2.6314287779155485,
+            "angularVelocity": -8.928636380137632e-7,
+            "velocityX": 0.5154952885786028,
+            "velocityY": -3.747652675727398,
+            "timestamp": 2.5956321994321705
+        },
+        {
+            "x": 7.33170125110352,
+            "y": 3.351623375457655,
+            "heading": -2.63142882288382,
+            "angularVelocity": -8.928636442498648e-7,
+            "velocityX": 0.5154952885786169,
+            "velocityY": -3.7476526757273954,
+            "timestamp": 2.6459962962084735
+        },
+        {
+            "x": 7.357663705705211,
+            "y": 3.162876233413349,
+            "heading": -2.631428867852091,
+            "angularVelocity": -8.92863650023284e-7,
+            "velocityX": 0.5154952885786185,
+            "velocityY": -3.747652675727395,
+            "timestamp": 2.6963603929847766
+        },
+        {
+            "x": 7.3836261603069016,
+            "y": 2.974129091369042,
+            "heading": -2.6314289128203625,
+            "angularVelocity": -8.928636550825091e-7,
+            "velocityX": 0.5154952885786133,
+            "velocityY": -3.7476526757273954,
+            "timestamp": 2.7467244897610796
+        },
+        {
+            "x": 7.409588614908591,
+            "y": 2.785381949324736,
+            "heading": -2.6314289577886343,
+            "angularVelocity": -8.928636615837126e-7,
+            "velocityX": 0.5154952885785878,
+            "velocityY": -3.7476526757273985,
+            "timestamp": 2.7970885865373827
+        },
+        {
+            "x": 7.435551069510267,
+            "y": 2.5966348072804273,
+            "heading": -2.6314290027569065,
+            "angularVelocity": -8.928636688471597e-7,
+            "velocityX": 0.5154952885783288,
+            "velocityY": -3.7476526757274335,
+            "timestamp": 2.8474526833136857
+        },
+        {
+            "x": 7.461513524111708,
+            "y": 2.4078876652360863,
+            "heading": -2.631429047725179,
+            "angularVelocity": -8.928636741117398e-7,
+            "velocityX": 0.5154952885736368,
+            "velocityY": -3.747652675728079,
+            "timestamp": 2.8978167800899888
+        },
+        {
+            "x": 7.487475978714219,
+            "y": 2.219140523191893,
+            "heading": -2.631429092693452,
+            "angularVelocity": -8.928636797177715e-7,
+            "velocityX": 0.5154952885949088,
+            "velocityY": -3.7476526757251514,
+            "timestamp": 2.948180876866292
+        },
+        {
+            "x": 7.513438433344531,
+            "y": 2.030393381151524,
+            "heading": -2.631429137661725,
+            "angularVelocity": -8.928636858438467e-7,
+            "velocityX": 0.5154952891469065,
+            "velocityY": -3.747652675649225,
+            "timestamp": 2.998544973642595
+        },
+        {
+            "x": 7.539400885537171,
+            "y": 1.8416462387758443,
+            "heading": -2.631429182629998,
+            "angularVelocity": -8.928636898911743e-7,
+            "velocityX": 0.5154952407459272,
+            "velocityY": -3.747652682306939,
+            "timestamp": 3.048909070418898
+        },
+        {
+            "x": 7.565366905970307,
+            "y": 1.6528995872554086,
+            "heading": -2.631429227608021,
+            "angularVelocity": -8.930572773269882e-7,
+            "velocityX": 0.5155660896386092,
+            "velocityY": -3.747642936172811,
+            "timestamp": 3.099273167195201
+        },
+        {
+            "x": 7.5925510306015145,
+            "y": 1.4714803587358116,
+            "heading": -2.650528472115119,
+            "angularVelocity": -0.3792234097224001,
+            "velocityX": 0.5397520529745963,
+            "velocityY": -3.6021539178074944,
+            "timestamp": 3.149637263971504
+        },
+        {
+            "x": 7.6161137082008885,
+            "y": 1.3070490604011487,
+            "heading": -2.717467314284565,
+            "angularVelocity": -1.3290984342827263,
+            "velocityX": 0.46784672231967417,
+            "velocityY": -3.2648515283615525,
+            "timestamp": 3.200001360747807
+        },
+        {
+            "x": 7.636512760583371,
+            "y": 1.162828952012704,
+            "heading": -2.804054032355297,
+            "angularVelocity": -1.7192151475547452,
+            "velocityX": 0.4050316334093296,
+            "velocityY": -2.863549981428464,
+            "timestamp": 3.25036545752411
+        },
+        {
+            "x": 7.65384671106135,
+            "y": 1.039535414639525,
+            "heading": -2.8903428318832396,
+            "angularVelocity": -1.7132998515034186,
+            "velocityX": 0.344172765670314,
+            "velocityY": -2.4480442470913055,
+            "timestamp": 3.300729554300413
+        },
+        {
+            "x": 7.668186632114738,
+            "y": 0.9370698417317626,
+            "heading": -2.968979200240837,
+            "angularVelocity": -1.561357661329035,
+            "velocityX": 0.28472507145489134,
+            "velocityY": -2.034496386639724,
+            "timestamp": 3.351093651076716
+        },
+        {
+            "x": 7.6795847479840615,
+            "y": 0.8552933598147935,
+            "heading": -3.035849646004894,
+            "angularVelocity": -1.3277403953270235,
+            "velocityX": 0.22631431116405423,
+            "velocityY": -1.6237059165418328,
+            "timestamp": 3.4014577478530192
+        },
+        {
+            "x": 7.688084380345824,
+            "y": 0.7940870720973938,
+            "heading": -3.088311607549879,
+            "angularVelocity": -1.0416539738218624,
+            "velocityX": 0.16876372070203405,
+            "velocityY": -1.2152761914753087,
+            "timestamp": 3.4518218446293223
+        },
+        {
+            "x": 7.693722705516469,
+            "y": 0.7533557615174019,
+            "heading": -3.124538922216378,
+            "angularVelocity": -0.7193083364009395,
+            "velocityX": 0.11195128139975212,
+            "velocityY": -0.8087370406125588,
+            "timestamp": 3.5021859414056253
+        },
+        {
+            "x": 7.696530342102051,
+            "y": 0.7330225110054016,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.37023169592473243,
+            "velocityX": 0.05574678720146357,
+            "velocityY": -0.40372511001859746,
+            "timestamp": 3.5525500381819284
+        },
+        {
+            "x": 7.696530342102051,
+            "y": 0.7330225110054016,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0,
+            "velocityX": -4.6862983235773636e-32,
+            "velocityY": -2.8797435391634997e-33,
+            "timestamp": 3.6029141349582314
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.traj
new file mode 100644
index 00000000..79c52dac
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.traj	
@@ -0,0 +1,625 @@
+{
+    "samples": [
+        {
+            "x": 0.7328571677207947,
+            "y": 6.758700847625732,
+            "heading": -2.1130989969635645,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7582517080124825,
+            "y": 6.75986365233427,
+            "heading": -2.1364061905138043,
+            "angularVelocity": -0.4153761780165196,
+            "velocityX": 0.4525764573976041,
+            "velocityY": 0.020723274750874442,
+            "timestamp": 0.05611105013661352
+        },
+        {
+            "x": 0.8091370757597145,
+            "y": 6.7622371099463585,
+            "heading": -2.1815821515780387,
+            "angularVelocity": -0.8051170126783445,
+            "velocityX": 0.9068689255207545,
+            "velocityY": 0.04229929053754837,
+            "timestamp": 0.11222210027322704
+        },
+        {
+            "x": 0.8856327770155913,
+            "y": 6.7658721035419465,
+            "heading": -2.246734552492055,
+            "angularVelocity": -1.1611331592509972,
+            "velocityX": 1.3632912068056597,
+            "velocityY": 0.06478213447706996,
+            "timestamp": 0.16833315040984057
+        },
+        {
+            "x": 0.9878912539433133,
+            "y": 6.770811955024787,
+            "heading": -2.329199596325353,
+            "angularVelocity": -1.4696756455728817,
+            "velocityX": 1.8224302820701674,
+            "velocityY": 0.08803705278779066,
+            "timestamp": 0.2244442005464541
+        },
+        {
+            "x": 1.116115460670533,
+            "y": 6.777089415314351,
+            "heading": -2.4245891589263606,
+            "angularVelocity": -1.7000138541118799,
+            "velocityX": 2.285186365520377,
+            "velocityY": 0.1118756514857201,
+            "timestamp": 0.28055525068306764
+        },
+        {
+            "x": 1.270529175155293,
+            "y": 6.784749415960288,
+            "heading": -2.524162862737573,
+            "angularVelocity": -1.774582788395185,
+            "velocityX": 2.751930575328903,
+            "velocityY": 0.13651501134463712,
+            "timestamp": 0.3366663008196812
+        },
+        {
+            "x": 1.4506438070602947,
+            "y": 6.793909267096616,
+            "heading": -2.6053474913460852,
+            "angularVelocity": -1.4468563395419025,
+            "velocityX": 3.2099672251094336,
+            "velocityY": 0.16324504913117607,
+            "timestamp": 0.39277735095629474
+        },
+        {
+            "x": 1.6524388072374525,
+            "y": 6.806354930258429,
+            "heading": -2.6314264655034,
+            "angularVelocity": -0.46477430192125535,
+            "velocityX": 3.5963504458720204,
+            "velocityY": 0.22180413896261764,
+            "timestamp": 0.4488884010929083
+        },
+        {
+            "x": 1.8644608843714687,
+            "y": 6.816501923570639,
+            "heading": -2.6314265116901563,
+            "angularVelocity": -8.231311961492043e-7,
+            "velocityX": 3.7786153817796047,
+            "velocityY": 0.18083770108638533,
+            "timestamp": 0.5049994512295218
+        },
+        {
+            "x": 2.0764830467529034,
+            "y": 6.826647135513074,
+            "heading": -2.631426557872052,
+            "angularVelocity": -8.230445785018772e-7,
+            "velocityX": 3.7786169010421835,
+            "velocityY": 0.1808059538676366,
+            "timestamp": 0.5611105013661354
+        },
+        {
+            "x": 2.288505209106376,
+            "y": 6.836792348039794,
+            "heading": -2.631426604053948,
+            "angularVelocity": -8.230445836057129e-7,
+            "velocityX": 3.778616900543848,
+            "velocityY": 0.18080596428064635,
+            "timestamp": 0.6172215515027489
+        },
+        {
+            "x": 2.5005273714588676,
+            "y": 6.846937560587016,
+            "heading": -2.631426650235844,
+            "angularVelocity": -8.230445885587366e-7,
+            "velocityX": 3.7786169005263646,
+            "velocityY": 0.18080596464604262,
+            "timestamp": 0.6733326016393625
+        },
+        {
+            "x": 2.7125495338113623,
+            "y": 6.857082773134173,
+            "heading": -2.6314266964177406,
+            "angularVelocity": -8.230445944915301e-7,
+            "velocityX": 3.778616900526421,
+            "velocityY": 0.18080596464485496,
+            "timestamp": 0.729443651775976
+        },
+        {
+            "x": 2.9245716961638615,
+            "y": 6.867227985681239,
+            "heading": -2.631426742599637,
+            "angularVelocity": -8.230445986306635e-7,
+            "velocityX": 3.7786169005264982,
+            "velocityY": 0.18080596464325174,
+            "timestamp": 0.7855547019125896
+        },
+        {
+            "x": 3.1365938585163606,
+            "y": 6.877373198228301,
+            "heading": -2.631426788781534,
+            "angularVelocity": -8.230446041948432e-7,
+            "velocityX": 3.7786169005265,
+            "velocityY": 0.18080596464320411,
+            "timestamp": 0.8416657520492031
+        },
+        {
+            "x": 3.34861602086886,
+            "y": 6.887518410775364,
+            "heading": -2.6314268349634315,
+            "angularVelocity": -8.230446102544191e-7,
+            "velocityX": 3.7786169005265,
+            "velocityY": 0.18080596464319879,
+            "timestamp": 0.8977768021858167
+        },
+        {
+            "x": 3.560638183221359,
+            "y": 6.897663623322427,
+            "heading": -2.631426881145329,
+            "angularVelocity": -8.230446143262805e-7,
+            "velocityX": 3.7786169005264996,
+            "velocityY": 0.1808059646431973,
+            "timestamp": 0.9538878523224302
+        },
+        {
+            "x": 3.772660345573858,
+            "y": 6.90780883586949,
+            "heading": -2.631426927327227,
+            "angularVelocity": -8.230446193740569e-7,
+            "velocityX": 3.778616900526499,
+            "velocityY": 0.1808059646432043,
+            "timestamp": 1.0099989024590437
+        },
+        {
+            "x": 3.984682507926357,
+            "y": 6.917954048416554,
+            "heading": -2.631426973509125,
+            "angularVelocity": -8.230446249324534e-7,
+            "velocityX": 3.778616900526498,
+            "velocityY": 0.18080596464321827,
+            "timestamp": 1.0661099525956572
+        },
+        {
+            "x": 4.196704670278859,
+            "y": 6.928099260963566,
+            "heading": -2.6314270196910234,
+            "angularVelocity": -8.230446298795515e-7,
+            "velocityX": 3.7786169005265413,
+            "velocityY": 0.18080596464229606,
+            "timestamp": 1.1222210027322708
+        },
+        {
+            "x": 4.40872683263124,
+            "y": 6.938244473513073,
+            "heading": -2.631427065872922,
+            "angularVelocity": -8.230446358036557e-7,
+            "velocityX": 3.778616900524414,
+            "velocityY": 0.18080596468676038,
+            "timestamp": 1.1783320528688843
+        },
+        {
+            "x": 4.620748994963782,
+            "y": 6.948389686477181,
+            "heading": -2.6314271120548214,
+            "angularVelocity": -8.230446409621988e-7,
+            "velocityX": 3.778616900170846,
+            "velocityY": 0.18080597207568838,
+            "timestamp": 1.2344431030054979
+        },
+        {
+            "x": 4.832771157387297,
+            "y": 6.958534897538403,
+            "heading": -2.631427158236721,
+            "angularVelocity": -8.230446451889797e-7,
+            "velocityX": 3.778616901792118,
+            "velocityY": 0.18080593816277588,
+            "timestamp": 1.2905541531421114
+        },
+        {
+            "x": 5.044793317425079,
+            "y": 6.968680158463288,
+            "heading": -2.6314272044186224,
+            "angularVelocity": -8.230446869472616e-7,
+            "velocityX": 3.7786168592740705,
+            "velocityY": 0.18080682682325677,
+            "timestamp": 1.346665203278725
+        },
+        {
+            "x": 5.256813933209308,
+            "y": 6.978857641482405,
+            "heading": -2.6314272506013574,
+            "angularVelocity": -8.230595289243604e-7,
+            "velocityX": 3.778589337893981,
+            "velocityY": 0.18138108258631616,
+            "timestamp": 1.4027762534153385
+        },
+        {
+            "x": 5.469038653323493,
+            "y": 6.974735659993926,
+            "heading": -2.6314272968552426,
+            "angularVelocity": -8.243275644517052e-7,
+            "velocityX": 3.7822268447565213,
+            "velocityY": -0.07346113606385067,
+            "timestamp": 1.458887303551952
+        },
+        {
+            "x": 5.679131625937882,
+            "y": 6.944449281942863,
+            "heading": -2.631427345537303,
+            "angularVelocity": -8.676020260335946e-7,
+            "velocityX": 3.7442352638718064,
+            "velocityY": -0.5397578191342853,
+            "timestamp": 1.5149983536885656
+        },
+        {
+            "x": 5.8838793004435885,
+            "y": 6.888460800908395,
+            "heading": -2.6314274009810084,
+            "angularVelocity": -9.881067120494687e-7,
+            "velocityX": 3.648972421781526,
+            "velocityY": -0.9978155977862404,
+            "timestamp": 1.5711094038251792
+        },
+        {
+            "x": 6.080149873036162,
+            "y": 6.807626528914084,
+            "heading": -2.6314274705096907,
+            "angularVelocity": -0.0000012391263740452789,
+            "velocityX": 3.497895193811386,
+            "velocityY": -1.440612353493913,
+            "timestamp": 1.6272204539617927
+        },
+        {
+            "x": 6.264941198917061,
+            "y": 6.703182873189846,
+            "heading": -2.631427570246947,
+            "angularVelocity": -0.0000017774975926495223,
+            "velocityX": 3.2933143370331135,
+            "velocityY": -1.8613741049215575,
+            "timestamp": 1.6833315040984063
+        },
+        {
+            "x": 6.435426712036133,
+            "y": 6.576727390289307,
+            "heading": -2.6314277398800354,
+            "angularVelocity": -0.0000030231672442365436,
+            "velocityX": 3.0383589810560285,
+            "velocityY": -2.253664520494037,
+            "timestamp": 1.7394425542350198
+        },
+        {
+            "x": 6.574927462247643,
+            "y": 6.446962020265696,
+            "heading": -2.631427921819672,
+            "angularVelocity": -0.0000036124868348171132,
+            "velocityX": 2.7698451702830824,
+            "velocityY": -2.5765451647028184,
+            "timestamp": 1.7898066510113226
+        },
+        {
+            "x": 6.6991857061041244,
+            "y": 6.302534147821866,
+            "heading": -2.631428035463349,
+            "angularVelocity": -0.0000022564422659644148,
+            "velocityX": 2.4671988938546927,
+            "velocityY": -2.867675222794474,
+            "timestamp": 1.8401707477876255
+        },
+        {
+            "x": 6.806670192415463,
+            "y": 6.145223588911882,
+            "heading": -2.631428117085094,
+            "angularVelocity": -0.0000016206335538188846,
+            "velocityX": 2.134148990872305,
+            "velocityY": -3.123466297999793,
+            "timestamp": 1.8905348445639283
+        },
+        {
+            "x": 6.896056392277072,
+            "y": 5.976968910136802,
+            "heading": -2.631428181121382,
+            "angularVelocity": -0.000001271467013700371,
+            "velocityX": 1.7748000179300458,
+            "velocityY": -3.340766330475413,
+            "timestamp": 1.940898941340231
+        },
+        {
+            "x": 6.966242833371282,
+            "y": 5.799843545392681,
+            "heading": -2.631428234939726,
+            "angularVelocity": -0.0000010685855124139772,
+            "velocityX": 1.393580855940955,
+            "velocityY": -3.5168974742233234,
+            "timestamp": 1.991263038116534
+        },
+        {
+            "x": 7.016364754971619,
+            "y": 5.616030256280146,
+            "heading": -2.6314282829523696,
+            "angularVelocity": -9.53310925320319e-7,
+            "velocityX": 0.9951915115833975,
+            "velocityY": -3.6496889823907637,
+            "timestamp": 2.041627134892837
+        },
+        {
+            "x": 7.046069338416312,
+            "y": 5.427835754816808,
+            "heading": -2.631428328230437,
+            "angularVelocity": -8.990147867657557e-7,
+            "velocityX": 0.5897968065726167,
+            "velocityY": -3.7366797681141786,
+            "timestamp": 2.09199123166914
+        },
+        {
+            "x": 7.072076786928083,
+            "y": 5.239094807157974,
+            "heading": -2.6314283732011274,
+            "angularVelocity": -8.929116824307271e-7,
+            "velocityX": 0.5163886612943162,
+            "velocityY": -3.747529683638862,
+            "timestamp": 2.142355328445443
+        },
+        {
+            "x": 7.098039162505348,
+            "y": 5.0503476542439945,
+            "heading": -2.631428418169392,
+            "angularVelocity": -8.928635140632744e-7,
+            "velocityX": 0.5154937195157807,
+            "velocityY": -3.747652891549277,
+            "timestamp": 2.192719425221746
+        },
+        {
+            "x": 7.124001613673454,
+            "y": 4.861600511727296,
+            "heading": -2.6314284631376603,
+            "angularVelocity": -8.928635936943803e-7,
+            "velocityX": 0.5154952204033956,
+            "velocityY": -3.7476526851069147,
+            "timestamp": 2.243083521998049
+        },
+        {
+            "x": 7.149964068909211,
+            "y": 4.67285336977021,
+            "heading": -2.631428508105929,
+            "angularVelocity": -8.928636032719525e-7,
+            "velocityX": 0.5154953011682646,
+            "velocityY": -3.747652673995615,
+            "timestamp": 2.293447618774352
+        },
+        {
+            "x": 7.175926523495577,
+            "y": 4.484106227723796,
+            "heading": -2.6314285530741985,
+            "angularVelocity": -8.928636081094573e-7,
+            "velocityX": 0.5154952882743342,
+            "velocityY": -3.7476526757692454,
+            "timestamp": 2.3438117155506553
+        },
+        {
+            "x": 7.2018889780951465,
+            "y": 4.295359085679197,
+            "heading": -2.6314285980424676,
+            "angularVelocity": -8.928636141069267e-7,
+            "velocityX": 0.5154952885364941,
+            "velocityY": -3.747652675733191,
+            "timestamp": 2.3941758123269583
+        },
+        {
+            "x": 7.227851432696769,
+            "y": 4.106611943634882,
+            "heading": -2.6314286430107376,
+            "angularVelocity": -8.928636206980889e-7,
+            "velocityX": 0.5154952885772718,
+            "velocityY": -3.7476526757275823,
+            "timestamp": 2.4445399091032614
+        },
+        {
+            "x": 7.253813887298451,
+            "y": 3.9178648015905746,
+            "heading": -2.6314286879790076,
+            "angularVelocity": -8.928636265842637e-7,
+            "velocityX": 0.5154952885784285,
+            "velocityY": -3.747652675727423,
+            "timestamp": 2.4949040058795644
+        },
+        {
+            "x": 7.2797763419001384,
+            "y": 3.729117659546268,
+            "heading": -2.631428732947278,
+            "angularVelocity": -8.928636327298174e-7,
+            "velocityX": 0.5154952885785545,
+            "velocityY": -3.747652675727405,
+            "timestamp": 2.5452681026558674
+        },
+        {
+            "x": 7.30573879650183,
+            "y": 3.5403705175019615,
+            "heading": -2.6314287779155485,
+            "angularVelocity": -8.928636380137632e-7,
+            "velocityX": 0.5154952885786028,
+            "velocityY": -3.747652675727398,
+            "timestamp": 2.5956321994321705
+        },
+        {
+            "x": 7.33170125110352,
+            "y": 3.351623375457655,
+            "heading": -2.63142882288382,
+            "angularVelocity": -8.928636442498648e-7,
+            "velocityX": 0.5154952885786169,
+            "velocityY": -3.7476526757273954,
+            "timestamp": 2.6459962962084735
+        },
+        {
+            "x": 7.357663705705211,
+            "y": 3.162876233413349,
+            "heading": -2.631428867852091,
+            "angularVelocity": -8.92863650023284e-7,
+            "velocityX": 0.5154952885786185,
+            "velocityY": -3.747652675727395,
+            "timestamp": 2.6963603929847766
+        },
+        {
+            "x": 7.3836261603069016,
+            "y": 2.974129091369042,
+            "heading": -2.6314289128203625,
+            "angularVelocity": -8.928636550825091e-7,
+            "velocityX": 0.5154952885786133,
+            "velocityY": -3.7476526757273954,
+            "timestamp": 2.7467244897610796
+        },
+        {
+            "x": 7.409588614908591,
+            "y": 2.785381949324736,
+            "heading": -2.6314289577886343,
+            "angularVelocity": -8.928636615837126e-7,
+            "velocityX": 0.5154952885785878,
+            "velocityY": -3.7476526757273985,
+            "timestamp": 2.7970885865373827
+        },
+        {
+            "x": 7.435551069510267,
+            "y": 2.5966348072804273,
+            "heading": -2.6314290027569065,
+            "angularVelocity": -8.928636688471597e-7,
+            "velocityX": 0.5154952885783288,
+            "velocityY": -3.7476526757274335,
+            "timestamp": 2.8474526833136857
+        },
+        {
+            "x": 7.461513524111708,
+            "y": 2.4078876652360863,
+            "heading": -2.631429047725179,
+            "angularVelocity": -8.928636741117398e-7,
+            "velocityX": 0.5154952885736368,
+            "velocityY": -3.747652675728079,
+            "timestamp": 2.8978167800899888
+        },
+        {
+            "x": 7.487475978714219,
+            "y": 2.219140523191893,
+            "heading": -2.631429092693452,
+            "angularVelocity": -8.928636797177715e-7,
+            "velocityX": 0.5154952885949088,
+            "velocityY": -3.7476526757251514,
+            "timestamp": 2.948180876866292
+        },
+        {
+            "x": 7.513438433344531,
+            "y": 2.030393381151524,
+            "heading": -2.631429137661725,
+            "angularVelocity": -8.928636858438467e-7,
+            "velocityX": 0.5154952891469065,
+            "velocityY": -3.747652675649225,
+            "timestamp": 2.998544973642595
+        },
+        {
+            "x": 7.539400885537171,
+            "y": 1.8416462387758443,
+            "heading": -2.631429182629998,
+            "angularVelocity": -8.928636898911743e-7,
+            "velocityX": 0.5154952407459272,
+            "velocityY": -3.747652682306939,
+            "timestamp": 3.048909070418898
+        },
+        {
+            "x": 7.565366905970307,
+            "y": 1.6528995872554086,
+            "heading": -2.631429227608021,
+            "angularVelocity": -8.930572773269882e-7,
+            "velocityX": 0.5155660896386092,
+            "velocityY": -3.747642936172811,
+            "timestamp": 3.099273167195201
+        },
+        {
+            "x": 7.5925510306015145,
+            "y": 1.4714803587358116,
+            "heading": -2.650528472115119,
+            "angularVelocity": -0.3792234097224001,
+            "velocityX": 0.5397520529745963,
+            "velocityY": -3.6021539178074944,
+            "timestamp": 3.149637263971504
+        },
+        {
+            "x": 7.6161137082008885,
+            "y": 1.3070490604011487,
+            "heading": -2.717467314284565,
+            "angularVelocity": -1.3290984342827263,
+            "velocityX": 0.46784672231967417,
+            "velocityY": -3.2648515283615525,
+            "timestamp": 3.200001360747807
+        },
+        {
+            "x": 7.636512760583371,
+            "y": 1.162828952012704,
+            "heading": -2.804054032355297,
+            "angularVelocity": -1.7192151475547452,
+            "velocityX": 0.4050316334093296,
+            "velocityY": -2.863549981428464,
+            "timestamp": 3.25036545752411
+        },
+        {
+            "x": 7.65384671106135,
+            "y": 1.039535414639525,
+            "heading": -2.8903428318832396,
+            "angularVelocity": -1.7132998515034186,
+            "velocityX": 0.344172765670314,
+            "velocityY": -2.4480442470913055,
+            "timestamp": 3.300729554300413
+        },
+        {
+            "x": 7.668186632114738,
+            "y": 0.9370698417317626,
+            "heading": -2.968979200240837,
+            "angularVelocity": -1.561357661329035,
+            "velocityX": 0.28472507145489134,
+            "velocityY": -2.034496386639724,
+            "timestamp": 3.351093651076716
+        },
+        {
+            "x": 7.6795847479840615,
+            "y": 0.8552933598147935,
+            "heading": -3.035849646004894,
+            "angularVelocity": -1.3277403953270235,
+            "velocityX": 0.22631431116405423,
+            "velocityY": -1.6237059165418328,
+            "timestamp": 3.4014577478530192
+        },
+        {
+            "x": 7.688084380345824,
+            "y": 0.7940870720973938,
+            "heading": -3.088311607549879,
+            "angularVelocity": -1.0416539738218624,
+            "velocityX": 0.16876372070203405,
+            "velocityY": -1.2152761914753087,
+            "timestamp": 3.4518218446293223
+        },
+        {
+            "x": 7.693722705516469,
+            "y": 0.7533557615174019,
+            "heading": -3.124538922216378,
+            "angularVelocity": -0.7193083364009395,
+            "velocityX": 0.11195128139975212,
+            "velocityY": -0.8087370406125588,
+            "timestamp": 3.5021859414056253
+        },
+        {
+            "x": 7.696530342102051,
+            "y": 0.7330225110054016,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.37023169592473243,
+            "velocityX": 0.05574678720146357,
+            "velocityY": -0.40372511001859746,
+            "timestamp": 3.5525500381819284
+        },
+        {
+            "x": 7.696530342102051,
+            "y": 0.7330225110054016,
+            "heading": -3.143185307179586,
+            "angularVelocity": 0,
+            "velocityX": -4.6862983235773636e-32,
+            "velocityY": -2.8797435391634997e-33,
+            "timestamp": 3.6029141349582314
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.1.traj
new file mode 100644
index 00000000..5004d1ca
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.1.traj	
@@ -0,0 +1,526 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": 2.1,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 3.1166835341708484e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7240863087605185,
+            "y": 4.368337736173442,
+            "heading": 2.1163001020338092,
+            "angularVelocity": 0.3614478974621331,
+            "velocityX": 0.31235796401343735,
+            "velocityY": 0.1848857880296264,
+            "timestamp": 0.045096685160595056
+        },
+        {
+            "x": 0.7523050752299821,
+            "y": 4.38505134365204,
+            "heading": 2.148025504039376,
+            "angularVelocity": 0.7034974276399452,
+            "velocityX": 0.6257392615216195,
+            "velocityY": 0.3706172065436647,
+            "timestamp": 0.09019337032119011
+        },
+        {
+            "x": 0.7947129681201482,
+            "y": 4.410184705499472,
+            "heading": 2.1940580950016866,
+            "angularVelocity": 1.020753316976243,
+            "velocityX": 0.9403771638457716,
+            "velocityY": 0.5573217135124126,
+            "timestamp": 0.13529005548178516
+        },
+        {
+            "x": 0.8513807226870724,
+            "y": 4.44378606946023,
+            "heading": 2.252941060288208,
+            "angularVelocity": 1.3057049554048448,
+            "velocityX": 1.25658359068129,
+            "velocityY": 0.7450960938946717,
+            "timestamp": 0.18038674064238022
+        },
+        {
+            "x": 0.922394851892513,
+            "y": 4.485907452258627,
+            "heading": 2.32274249990232,
+            "angularVelocity": 1.5478175250695256,
+            "velocityX": 1.5747084060070078,
+            "velocityY": 0.9340239232306716,
+            "timestamp": 0.22548342580297528
+        },
+        {
+            "x": 1.0078549434585182,
+            "y": 4.536606503403333,
+            "heading": 2.4007952890388493,
+            "angularVelocity": 1.7307877254963087,
+            "velocityX": 1.8950415371256468,
+            "velocityY": 1.1242301061410762,
+            "timestamp": 0.2705801109635703
+        },
+        {
+            "x": 1.1078580204781514,
+            "y": 4.595947756684704,
+            "heading": 2.4830211624583747,
+            "angularVelocity": 1.823324111887792,
+            "velocityX": 2.2175261144695955,
+            "velocityY": 1.315867298672826,
+            "timestamp": 0.3156767961241654
+        },
+        {
+            "x": 1.2224032158634355,
+            "y": 4.663956921963176,
+            "heading": 2.561776708325237,
+            "angularVelocity": 1.7463710600103812,
+            "velocityX": 2.5399914645028585,
+            "velocityY": 1.5080745965314675,
+            "timestamp": 0.36077348128476044
+        },
+        {
+            "x": 1.3501227544876644,
+            "y": 4.740405284602277,
+            "heading": 2.616849964946217,
+            "angularVelocity": 1.2212262702869785,
+            "velocityX": 2.8321269771692363,
+            "velocityY": 1.695210243654476,
+            "timestamp": 0.4058701664453555
+        },
+        {
+            "x": 1.4890564849563437,
+            "y": 4.825393782343559,
+            "heading": 2.6364418869926514,
+            "angularVelocity": 0.43444261982150123,
+            "velocityX": 3.0807969582225083,
+            "velocityY": 1.884584142684248,
+            "timestamp": 0.45096685160595057
+        },
+        {
+            "x": 1.6359707555642846,
+            "y": 4.9121110990754175,
+            "heading": 2.6364419350884667,
+            "angularVelocity": 0.0000010665044506613552,
+            "velocityX": 3.257762074634985,
+            "velocityY": 1.9229199756711164,
+            "timestamp": 0.49606353676654563
+        },
+        {
+            "x": 1.7828851010176074,
+            "y": 4.99882828900635,
+            "heading": 2.63644198318385,
+            "angularVelocity": 0.000001066494868779716,
+            "velocityX": 3.257763734299801,
+            "velocityY": 1.9229171639139742,
+            "timestamp": 0.5411602219271406
+        },
+        {
+            "x": 1.929799446473537,
+            "y": 5.085545478932866,
+            "heading": 2.6364420312792327,
+            "angularVelocity": 0.0000010664948614258307,
+            "velocityX": 3.257763734357611,
+            "velocityY": 1.9229171638160338,
+            "timestamp": 0.5862569070877357
+        },
+        {
+            "x": 2.076713791929467,
+            "y": 5.172262668859381,
+            "heading": 2.636442079374616,
+            "angularVelocity": 0.000001066494855048045,
+            "velocityX": 3.257763734357614,
+            "velocityY": 1.922917163816031,
+            "timestamp": 0.6313535922483308
+        },
+        {
+            "x": 2.2236281373853966,
+            "y": 5.258979858785897,
+            "heading": 2.6364421274699983,
+            "angularVelocity": 0.000001066494848970006,
+            "velocityX": 3.257763734357614,
+            "velocityY": 1.9229171638160298,
+            "timestamp": 0.6764502774089258
+        },
+        {
+            "x": 2.3705424828413264,
+            "y": 5.345697048712413,
+            "heading": 2.6364421755653806,
+            "angularVelocity": 0.0000010664948432023413,
+            "velocityX": 3.257763734357615,
+            "velocityY": 1.9229171638160298,
+            "timestamp": 0.7215469625695209
+        },
+        {
+            "x": 2.5174568282972563,
+            "y": 5.432414238638929,
+            "heading": 2.6364422236607625,
+            "angularVelocity": 0.000001066494835980498,
+            "velocityX": 3.2577637343576162,
+            "velocityY": 1.9229171638160294,
+            "timestamp": 0.766643647730116
+        },
+        {
+            "x": 2.6643711737531857,
+            "y": 5.519131428565444,
+            "heading": 2.636442271756144,
+            "angularVelocity": 0.0000010664948304508311,
+            "velocityX": 3.2577637343576167,
+            "velocityY": 1.9229171638160292,
+            "timestamp": 0.811740332890711
+        },
+        {
+            "x": 2.8112855192091155,
+            "y": 5.60584861849196,
+            "heading": 2.6364423198515254,
+            "angularVelocity": 0.0000010664948245159241,
+            "velocityX": 3.257763734357617,
+            "velocityY": 1.922917163816029,
+            "timestamp": 0.8568370180513061
+        },
+        {
+            "x": 2.958199864665046,
+            "y": 5.692565808418475,
+            "heading": 2.6364423679469065,
+            "angularVelocity": 0.0000010664948181966642,
+            "velocityX": 3.2577637343576185,
+            "velocityY": 1.922917163816028,
+            "timestamp": 0.9019337032119011
+        },
+        {
+            "x": 3.1051142101209757,
+            "y": 5.779282998344991,
+            "heading": 2.636442416042287,
+            "angularVelocity": 0.0000010664948111950998,
+            "velocityX": 3.257763734357619,
+            "velocityY": 1.9229171638160278,
+            "timestamp": 0.9470303883724962
+        },
+        {
+            "x": 3.252028555576906,
+            "y": 5.8660001882715065,
+            "heading": 2.6364424641376676,
+            "angularVelocity": 0.0000010664948058295197,
+            "velocityX": 3.2577637343576273,
+            "velocityY": 1.922917163816015,
+            "timestamp": 0.9921270735330913
+        },
+        {
+            "x": 3.3989429010423815,
+            "y": 5.952717378181851,
+            "heading": 2.636442512233048,
+            "angularVelocity": 0.00000106649479979568,
+            "velocityX": 3.257763734569281,
+            "velocityY": 1.9229171634574358,
+            "timestamp": 1.0372237586936863
+        },
+        {
+            "x": 3.5458575205338394,
+            "y": 6.039434103841755,
+            "heading": 2.636442560328411,
+            "angularVelocity": 0.0000010664944172141213,
+            "velocityX": 3.2577698109800433,
+            "velocityY": 1.9229068688994477,
+            "timestamp": 1.0823204438542813
+        },
+        {
+            "x": 3.69740891456604,
+            "y": 6.1177659034729,
+            "heading": 2.6364426084288106,
+            "angularVelocity": 0.0000010666061013072793,
+            "velocityX": 3.3605883335439555,
+            "velocityY": 1.736974665703161,
+            "timestamp": 1.1274171290148762
+        },
+        {
+            "x": 3.8471798789241873,
+            "y": 6.1780975155460816,
+            "heading": 2.636442653750165,
+            "angularVelocity": 0.0000010618213127102396,
+            "velocityX": 3.5089419861830207,
+            "velocityY": 1.4134924456482445,
+            "timestamp": 1.1700997851966806
+        },
+        {
+            "x": 4.000192250358795,
+            "y": 6.2296577661788755,
+            "heading": 2.6364426985309986,
+            "angularVelocity": 0.0000010491576146038598,
+            "velocityX": 3.584884004942437,
+            "velocityY": 1.2079906745535305,
+            "timestamp": 1.212782441378485
+        },
+        {
+            "x": 4.153204837360183,
+            "y": 6.281217377082217,
+            "heading": 2.6364427433118154,
+            "angularVelocity": 0.0000010491572166464704,
+            "velocityX": 3.5848890553961605,
+            "velocityY": 1.2079756865112976,
+            "timestamp": 1.2554650975602892
+        },
+        {
+            "x": 4.306217424370213,
+            "y": 6.3327769879599085,
+            "heading": 2.636442788092632,
+            "angularVelocity": 0.0000010491572158103502,
+            "velocityX": 3.5848890555986537,
+            "velocityY": 1.2079756859103623,
+            "timestamp": 1.2981477537420936
+        },
+        {
+            "x": 4.459230011380243,
+            "y": 6.384336598837598,
+            "heading": 2.6364428328734486,
+            "angularVelocity": 0.0000010491572148390834,
+            "velocityX": 3.584889055598662,
+            "velocityY": 1.2079756859103368,
+            "timestamp": 1.340830409923898
+        },
+        {
+            "x": 4.6122425983902735,
+            "y": 6.435896209715288,
+            "heading": 2.636442877654265,
+            "angularVelocity": 0.0000010491572125659184,
+            "velocityX": 3.584889055598663,
+            "velocityY": 1.2079756859103368,
+            "timestamp": 1.3835130661057022
+        },
+        {
+            "x": 4.765255185400304,
+            "y": 6.487455820592977,
+            "heading": 2.636442922435082,
+            "angularVelocity": 0.0000010491572116366236,
+            "velocityX": 3.584889055598663,
+            "velocityY": 1.2079756859103352,
+            "timestamp": 1.4261957222875066
+        },
+        {
+            "x": 4.918267772410334,
+            "y": 6.539015431470667,
+            "heading": 2.636442967215898,
+            "angularVelocity": 0.0000010491572113687884,
+            "velocityX": 3.584889055598663,
+            "velocityY": 1.2079756859103348,
+            "timestamp": 1.468878378469311
+        },
+        {
+            "x": 5.071280359420366,
+            "y": 6.590575042348357,
+            "heading": 2.6364430119967146,
+            "angularVelocity": 0.0000010491572106337029,
+            "velocityX": 3.584889055598664,
+            "velocityY": 1.2079756859103339,
+            "timestamp": 1.5115610346511152
+        },
+        {
+            "x": 5.224292946430397,
+            "y": 6.642134653226047,
+            "heading": 2.6364430567775314,
+            "angularVelocity": 0.0000010491572087267192,
+            "velocityX": 3.584889055598664,
+            "velocityY": 1.2079756859103334,
+            "timestamp": 1.5542436908329196
+        },
+        {
+            "x": 5.3773055334404285,
+            "y": 6.693694264103736,
+            "heading": 2.636443101558348,
+            "angularVelocity": 0.0000010491572072659829,
+            "velocityX": 3.5848890555986643,
+            "velocityY": 1.2079756859103326,
+            "timestamp": 1.596926347014724
+        },
+        {
+            "x": 5.53031812045046,
+            "y": 6.745253874981426,
+            "heading": 2.6364431463391638,
+            "angularVelocity": 0.0000010491572066940514,
+            "velocityX": 3.5848890555986643,
+            "velocityY": 1.2079756859103314,
+            "timestamp": 1.6396090031965282
+        },
+        {
+            "x": 5.683330707460491,
+            "y": 6.796813485859116,
+            "heading": 2.63644319111998,
+            "angularVelocity": 0.000001049157205644206,
+            "velocityX": 3.5848890555986648,
+            "velocityY": 1.2079756859103312,
+            "timestamp": 1.6822916593783326
+        },
+        {
+            "x": 5.836343294470522,
+            "y": 6.848373096736806,
+            "heading": 2.636443235900796,
+            "angularVelocity": 0.0000010491572052803454,
+            "velocityX": 3.5848890555986657,
+            "velocityY": 1.2079756859103297,
+            "timestamp": 1.724974315560137
+        },
+        {
+            "x": 5.989355881480554,
+            "y": 6.899932707614496,
+            "heading": 2.6364432806816125,
+            "angularVelocity": 0.0000010491572041084738,
+            "velocityX": 3.5848890555986657,
+            "velocityY": 1.2079756859103294,
+            "timestamp": 1.7676569717419413
+        },
+        {
+            "x": 6.142368468490585,
+            "y": 6.951492318492185,
+            "heading": 2.6364433254624284,
+            "angularVelocity": 0.0000010491572019949161,
+            "velocityX": 3.584889055598666,
+            "velocityY": 1.2079756859103283,
+            "timestamp": 1.8103396279237456
+        },
+        {
+            "x": 6.295381055500616,
+            "y": 7.0030519293698745,
+            "heading": 2.6364433702432444,
+            "angularVelocity": 0.0000010491572021090905,
+            "velocityX": 3.584889055598666,
+            "velocityY": 1.2079756859103277,
+            "timestamp": 1.85302228410555
+        },
+        {
+            "x": 6.448393642510648,
+            "y": 7.054611540247564,
+            "heading": 2.6364434150240608,
+            "angularVelocity": 0.0000010491572009028897,
+            "velocityX": 3.584889055598666,
+            "velocityY": 1.2079756859103268,
+            "timestamp": 1.8957049402873543
+        },
+        {
+            "x": 6.601406229520499,
+            "y": 7.106171151125787,
+            "heading": 2.6364434598048767,
+            "angularVelocity": 0.0000010491571991066208,
+            "velocityX": 3.58488905559446,
+            "velocityY": 1.2079756859228103,
+            "timestamp": 1.9383875964691586
+        },
+        {
+            "x": 6.75441881205228,
+            "y": 7.157730775293412,
+            "heading": 2.636443504585761,
+            "angularVelocity": 0.0000010491588061157563,
+            "velocityX": 3.5848889506789767,
+            "velocityY": 1.207975997276489,
+            "timestamp": 1.981070252650963
+        },
+        {
+            "x": 6.905487720036347,
+            "y": 7.20899283211694,
+            "heading": 2.6411872457126164,
+            "angularVelocity": 0.11113978255358876,
+            "velocityX": 3.539351143954066,
+            "velocityY": 1.2010043752942743,
+            "timestamp": 2.0237529088327673
+        },
+        {
+            "x": 7.045499588568849,
+            "y": 7.254368377570732,
+            "heading": 2.676148381759387,
+            "angularVelocity": 0.81909466687958,
+            "velocityX": 3.2802988627542176,
+            "velocityY": 1.0630909487103528,
+            "timestamp": 2.0664355650145714
+        },
+        {
+            "x": 7.172970608020284,
+            "y": 7.297222729151116,
+            "heading": 2.7400991643229964,
+            "angularVelocity": 1.4982849776549703,
+            "velocityX": 2.9864828212302403,
+            "velocityY": 1.0040226034164315,
+            "timestamp": 2.1091182211963755
+        },
+        {
+            "x": 7.286231301322222,
+            "y": 7.335261893158222,
+            "heading": 2.8124985936039937,
+            "angularVelocity": 1.6962259558687312,
+            "velocityX": 2.6535530689447047,
+            "velocityY": 0.8912089220755359,
+            "timestamp": 2.1518008773781796
+        },
+        {
+            "x": 7.3851039731372365,
+            "y": 7.3684303034498875,
+            "heading": 2.8845195362089706,
+            "angularVelocity": 1.6873584975173053,
+            "velocityX": 2.316460142355517,
+            "velocityY": 0.7770933971491014,
+            "timestamp": 2.1944835335599837
+        },
+        {
+            "x": 7.46964567597808,
+            "y": 7.396754020582906,
+            "heading": 2.951617256319432,
+            "angularVelocity": 1.5720136962578515,
+            "velocityX": 1.9807038831122041,
+            "velocityY": 0.6635884377105193,
+            "timestamp": 2.237166189741788
+        },
+        {
+            "x": 7.539938623640353,
+            "y": 7.420270719849242,
+            "heading": 3.0110324789568197,
+            "angularVelocity": 1.3920226141576573,
+            "velocityX": 1.6468737878651336,
+            "velocityY": 0.5509661621378036,
+            "timestamp": 2.279848845923592
+        },
+        {
+            "x": 7.596058846559568,
+            "y": 7.439019303920545,
+            "heading": 3.060856486484029,
+            "angularVelocity": 1.1673127210028025,
+            "velocityX": 1.3148249884022007,
+            "velocityY": 0.4392553263659099,
+            "timestamp": 2.322531502105396
+        },
+        {
+            "x": 7.638072702511279,
+            "y": 7.453036150115233,
+            "heading": 3.09965963024843,
+            "angularVelocity": 0.9091079898851933,
+            "velocityX": 0.9843308666816342,
+            "velocityY": 0.32839676460117173,
+            "timestamp": 2.3652141582872
+        },
+        {
+            "x": 7.666037142481598,
+            "y": 7.462353623790155,
+            "heading": 3.126330643718343,
+            "angularVelocity": 0.6248677063655441,
+            "velocityX": 0.6551710336677993,
+            "velocityY": 0.2182964817192845,
+            "timestamp": 2.4078968144690043
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 0.32025552073032726,
+            "velocityX": 0.3271318790219412,
+            "velocityY": 0.10885864717634237,
+            "timestamp": 2.4505794706508084
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -9.011054390829982e-34,
+            "velocityY": 6.746182534290948e-34,
+            "timestamp": 2.4932621268326125
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.traj
new file mode 100644
index 00000000..5004d1ca
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.traj	
@@ -0,0 +1,526 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": 2.1,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 3.1166835341708484e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7240863087605185,
+            "y": 4.368337736173442,
+            "heading": 2.1163001020338092,
+            "angularVelocity": 0.3614478974621331,
+            "velocityX": 0.31235796401343735,
+            "velocityY": 0.1848857880296264,
+            "timestamp": 0.045096685160595056
+        },
+        {
+            "x": 0.7523050752299821,
+            "y": 4.38505134365204,
+            "heading": 2.148025504039376,
+            "angularVelocity": 0.7034974276399452,
+            "velocityX": 0.6257392615216195,
+            "velocityY": 0.3706172065436647,
+            "timestamp": 0.09019337032119011
+        },
+        {
+            "x": 0.7947129681201482,
+            "y": 4.410184705499472,
+            "heading": 2.1940580950016866,
+            "angularVelocity": 1.020753316976243,
+            "velocityX": 0.9403771638457716,
+            "velocityY": 0.5573217135124126,
+            "timestamp": 0.13529005548178516
+        },
+        {
+            "x": 0.8513807226870724,
+            "y": 4.44378606946023,
+            "heading": 2.252941060288208,
+            "angularVelocity": 1.3057049554048448,
+            "velocityX": 1.25658359068129,
+            "velocityY": 0.7450960938946717,
+            "timestamp": 0.18038674064238022
+        },
+        {
+            "x": 0.922394851892513,
+            "y": 4.485907452258627,
+            "heading": 2.32274249990232,
+            "angularVelocity": 1.5478175250695256,
+            "velocityX": 1.5747084060070078,
+            "velocityY": 0.9340239232306716,
+            "timestamp": 0.22548342580297528
+        },
+        {
+            "x": 1.0078549434585182,
+            "y": 4.536606503403333,
+            "heading": 2.4007952890388493,
+            "angularVelocity": 1.7307877254963087,
+            "velocityX": 1.8950415371256468,
+            "velocityY": 1.1242301061410762,
+            "timestamp": 0.2705801109635703
+        },
+        {
+            "x": 1.1078580204781514,
+            "y": 4.595947756684704,
+            "heading": 2.4830211624583747,
+            "angularVelocity": 1.823324111887792,
+            "velocityX": 2.2175261144695955,
+            "velocityY": 1.315867298672826,
+            "timestamp": 0.3156767961241654
+        },
+        {
+            "x": 1.2224032158634355,
+            "y": 4.663956921963176,
+            "heading": 2.561776708325237,
+            "angularVelocity": 1.7463710600103812,
+            "velocityX": 2.5399914645028585,
+            "velocityY": 1.5080745965314675,
+            "timestamp": 0.36077348128476044
+        },
+        {
+            "x": 1.3501227544876644,
+            "y": 4.740405284602277,
+            "heading": 2.616849964946217,
+            "angularVelocity": 1.2212262702869785,
+            "velocityX": 2.8321269771692363,
+            "velocityY": 1.695210243654476,
+            "timestamp": 0.4058701664453555
+        },
+        {
+            "x": 1.4890564849563437,
+            "y": 4.825393782343559,
+            "heading": 2.6364418869926514,
+            "angularVelocity": 0.43444261982150123,
+            "velocityX": 3.0807969582225083,
+            "velocityY": 1.884584142684248,
+            "timestamp": 0.45096685160595057
+        },
+        {
+            "x": 1.6359707555642846,
+            "y": 4.9121110990754175,
+            "heading": 2.6364419350884667,
+            "angularVelocity": 0.0000010665044506613552,
+            "velocityX": 3.257762074634985,
+            "velocityY": 1.9229199756711164,
+            "timestamp": 0.49606353676654563
+        },
+        {
+            "x": 1.7828851010176074,
+            "y": 4.99882828900635,
+            "heading": 2.63644198318385,
+            "angularVelocity": 0.000001066494868779716,
+            "velocityX": 3.257763734299801,
+            "velocityY": 1.9229171639139742,
+            "timestamp": 0.5411602219271406
+        },
+        {
+            "x": 1.929799446473537,
+            "y": 5.085545478932866,
+            "heading": 2.6364420312792327,
+            "angularVelocity": 0.0000010664948614258307,
+            "velocityX": 3.257763734357611,
+            "velocityY": 1.9229171638160338,
+            "timestamp": 0.5862569070877357
+        },
+        {
+            "x": 2.076713791929467,
+            "y": 5.172262668859381,
+            "heading": 2.636442079374616,
+            "angularVelocity": 0.000001066494855048045,
+            "velocityX": 3.257763734357614,
+            "velocityY": 1.922917163816031,
+            "timestamp": 0.6313535922483308
+        },
+        {
+            "x": 2.2236281373853966,
+            "y": 5.258979858785897,
+            "heading": 2.6364421274699983,
+            "angularVelocity": 0.000001066494848970006,
+            "velocityX": 3.257763734357614,
+            "velocityY": 1.9229171638160298,
+            "timestamp": 0.6764502774089258
+        },
+        {
+            "x": 2.3705424828413264,
+            "y": 5.345697048712413,
+            "heading": 2.6364421755653806,
+            "angularVelocity": 0.0000010664948432023413,
+            "velocityX": 3.257763734357615,
+            "velocityY": 1.9229171638160298,
+            "timestamp": 0.7215469625695209
+        },
+        {
+            "x": 2.5174568282972563,
+            "y": 5.432414238638929,
+            "heading": 2.6364422236607625,
+            "angularVelocity": 0.000001066494835980498,
+            "velocityX": 3.2577637343576162,
+            "velocityY": 1.9229171638160294,
+            "timestamp": 0.766643647730116
+        },
+        {
+            "x": 2.6643711737531857,
+            "y": 5.519131428565444,
+            "heading": 2.636442271756144,
+            "angularVelocity": 0.0000010664948304508311,
+            "velocityX": 3.2577637343576167,
+            "velocityY": 1.9229171638160292,
+            "timestamp": 0.811740332890711
+        },
+        {
+            "x": 2.8112855192091155,
+            "y": 5.60584861849196,
+            "heading": 2.6364423198515254,
+            "angularVelocity": 0.0000010664948245159241,
+            "velocityX": 3.257763734357617,
+            "velocityY": 1.922917163816029,
+            "timestamp": 0.8568370180513061
+        },
+        {
+            "x": 2.958199864665046,
+            "y": 5.692565808418475,
+            "heading": 2.6364423679469065,
+            "angularVelocity": 0.0000010664948181966642,
+            "velocityX": 3.2577637343576185,
+            "velocityY": 1.922917163816028,
+            "timestamp": 0.9019337032119011
+        },
+        {
+            "x": 3.1051142101209757,
+            "y": 5.779282998344991,
+            "heading": 2.636442416042287,
+            "angularVelocity": 0.0000010664948111950998,
+            "velocityX": 3.257763734357619,
+            "velocityY": 1.9229171638160278,
+            "timestamp": 0.9470303883724962
+        },
+        {
+            "x": 3.252028555576906,
+            "y": 5.8660001882715065,
+            "heading": 2.6364424641376676,
+            "angularVelocity": 0.0000010664948058295197,
+            "velocityX": 3.2577637343576273,
+            "velocityY": 1.922917163816015,
+            "timestamp": 0.9921270735330913
+        },
+        {
+            "x": 3.3989429010423815,
+            "y": 5.952717378181851,
+            "heading": 2.636442512233048,
+            "angularVelocity": 0.00000106649479979568,
+            "velocityX": 3.257763734569281,
+            "velocityY": 1.9229171634574358,
+            "timestamp": 1.0372237586936863
+        },
+        {
+            "x": 3.5458575205338394,
+            "y": 6.039434103841755,
+            "heading": 2.636442560328411,
+            "angularVelocity": 0.0000010664944172141213,
+            "velocityX": 3.2577698109800433,
+            "velocityY": 1.9229068688994477,
+            "timestamp": 1.0823204438542813
+        },
+        {
+            "x": 3.69740891456604,
+            "y": 6.1177659034729,
+            "heading": 2.6364426084288106,
+            "angularVelocity": 0.0000010666061013072793,
+            "velocityX": 3.3605883335439555,
+            "velocityY": 1.736974665703161,
+            "timestamp": 1.1274171290148762
+        },
+        {
+            "x": 3.8471798789241873,
+            "y": 6.1780975155460816,
+            "heading": 2.636442653750165,
+            "angularVelocity": 0.0000010618213127102396,
+            "velocityX": 3.5089419861830207,
+            "velocityY": 1.4134924456482445,
+            "timestamp": 1.1700997851966806
+        },
+        {
+            "x": 4.000192250358795,
+            "y": 6.2296577661788755,
+            "heading": 2.6364426985309986,
+            "angularVelocity": 0.0000010491576146038598,
+            "velocityX": 3.584884004942437,
+            "velocityY": 1.2079906745535305,
+            "timestamp": 1.212782441378485
+        },
+        {
+            "x": 4.153204837360183,
+            "y": 6.281217377082217,
+            "heading": 2.6364427433118154,
+            "angularVelocity": 0.0000010491572166464704,
+            "velocityX": 3.5848890553961605,
+            "velocityY": 1.2079756865112976,
+            "timestamp": 1.2554650975602892
+        },
+        {
+            "x": 4.306217424370213,
+            "y": 6.3327769879599085,
+            "heading": 2.636442788092632,
+            "angularVelocity": 0.0000010491572158103502,
+            "velocityX": 3.5848890555986537,
+            "velocityY": 1.2079756859103623,
+            "timestamp": 1.2981477537420936
+        },
+        {
+            "x": 4.459230011380243,
+            "y": 6.384336598837598,
+            "heading": 2.6364428328734486,
+            "angularVelocity": 0.0000010491572148390834,
+            "velocityX": 3.584889055598662,
+            "velocityY": 1.2079756859103368,
+            "timestamp": 1.340830409923898
+        },
+        {
+            "x": 4.6122425983902735,
+            "y": 6.435896209715288,
+            "heading": 2.636442877654265,
+            "angularVelocity": 0.0000010491572125659184,
+            "velocityX": 3.584889055598663,
+            "velocityY": 1.2079756859103368,
+            "timestamp": 1.3835130661057022
+        },
+        {
+            "x": 4.765255185400304,
+            "y": 6.487455820592977,
+            "heading": 2.636442922435082,
+            "angularVelocity": 0.0000010491572116366236,
+            "velocityX": 3.584889055598663,
+            "velocityY": 1.2079756859103352,
+            "timestamp": 1.4261957222875066
+        },
+        {
+            "x": 4.918267772410334,
+            "y": 6.539015431470667,
+            "heading": 2.636442967215898,
+            "angularVelocity": 0.0000010491572113687884,
+            "velocityX": 3.584889055598663,
+            "velocityY": 1.2079756859103348,
+            "timestamp": 1.468878378469311
+        },
+        {
+            "x": 5.071280359420366,
+            "y": 6.590575042348357,
+            "heading": 2.6364430119967146,
+            "angularVelocity": 0.0000010491572106337029,
+            "velocityX": 3.584889055598664,
+            "velocityY": 1.2079756859103339,
+            "timestamp": 1.5115610346511152
+        },
+        {
+            "x": 5.224292946430397,
+            "y": 6.642134653226047,
+            "heading": 2.6364430567775314,
+            "angularVelocity": 0.0000010491572087267192,
+            "velocityX": 3.584889055598664,
+            "velocityY": 1.2079756859103334,
+            "timestamp": 1.5542436908329196
+        },
+        {
+            "x": 5.3773055334404285,
+            "y": 6.693694264103736,
+            "heading": 2.636443101558348,
+            "angularVelocity": 0.0000010491572072659829,
+            "velocityX": 3.5848890555986643,
+            "velocityY": 1.2079756859103326,
+            "timestamp": 1.596926347014724
+        },
+        {
+            "x": 5.53031812045046,
+            "y": 6.745253874981426,
+            "heading": 2.6364431463391638,
+            "angularVelocity": 0.0000010491572066940514,
+            "velocityX": 3.5848890555986643,
+            "velocityY": 1.2079756859103314,
+            "timestamp": 1.6396090031965282
+        },
+        {
+            "x": 5.683330707460491,
+            "y": 6.796813485859116,
+            "heading": 2.63644319111998,
+            "angularVelocity": 0.000001049157205644206,
+            "velocityX": 3.5848890555986648,
+            "velocityY": 1.2079756859103312,
+            "timestamp": 1.6822916593783326
+        },
+        {
+            "x": 5.836343294470522,
+            "y": 6.848373096736806,
+            "heading": 2.636443235900796,
+            "angularVelocity": 0.0000010491572052803454,
+            "velocityX": 3.5848890555986657,
+            "velocityY": 1.2079756859103297,
+            "timestamp": 1.724974315560137
+        },
+        {
+            "x": 5.989355881480554,
+            "y": 6.899932707614496,
+            "heading": 2.6364432806816125,
+            "angularVelocity": 0.0000010491572041084738,
+            "velocityX": 3.5848890555986657,
+            "velocityY": 1.2079756859103294,
+            "timestamp": 1.7676569717419413
+        },
+        {
+            "x": 6.142368468490585,
+            "y": 6.951492318492185,
+            "heading": 2.6364433254624284,
+            "angularVelocity": 0.0000010491572019949161,
+            "velocityX": 3.584889055598666,
+            "velocityY": 1.2079756859103283,
+            "timestamp": 1.8103396279237456
+        },
+        {
+            "x": 6.295381055500616,
+            "y": 7.0030519293698745,
+            "heading": 2.6364433702432444,
+            "angularVelocity": 0.0000010491572021090905,
+            "velocityX": 3.584889055598666,
+            "velocityY": 1.2079756859103277,
+            "timestamp": 1.85302228410555
+        },
+        {
+            "x": 6.448393642510648,
+            "y": 7.054611540247564,
+            "heading": 2.6364434150240608,
+            "angularVelocity": 0.0000010491572009028897,
+            "velocityX": 3.584889055598666,
+            "velocityY": 1.2079756859103268,
+            "timestamp": 1.8957049402873543
+        },
+        {
+            "x": 6.601406229520499,
+            "y": 7.106171151125787,
+            "heading": 2.6364434598048767,
+            "angularVelocity": 0.0000010491571991066208,
+            "velocityX": 3.58488905559446,
+            "velocityY": 1.2079756859228103,
+            "timestamp": 1.9383875964691586
+        },
+        {
+            "x": 6.75441881205228,
+            "y": 7.157730775293412,
+            "heading": 2.636443504585761,
+            "angularVelocity": 0.0000010491588061157563,
+            "velocityX": 3.5848889506789767,
+            "velocityY": 1.207975997276489,
+            "timestamp": 1.981070252650963
+        },
+        {
+            "x": 6.905487720036347,
+            "y": 7.20899283211694,
+            "heading": 2.6411872457126164,
+            "angularVelocity": 0.11113978255358876,
+            "velocityX": 3.539351143954066,
+            "velocityY": 1.2010043752942743,
+            "timestamp": 2.0237529088327673
+        },
+        {
+            "x": 7.045499588568849,
+            "y": 7.254368377570732,
+            "heading": 2.676148381759387,
+            "angularVelocity": 0.81909466687958,
+            "velocityX": 3.2802988627542176,
+            "velocityY": 1.0630909487103528,
+            "timestamp": 2.0664355650145714
+        },
+        {
+            "x": 7.172970608020284,
+            "y": 7.297222729151116,
+            "heading": 2.7400991643229964,
+            "angularVelocity": 1.4982849776549703,
+            "velocityX": 2.9864828212302403,
+            "velocityY": 1.0040226034164315,
+            "timestamp": 2.1091182211963755
+        },
+        {
+            "x": 7.286231301322222,
+            "y": 7.335261893158222,
+            "heading": 2.8124985936039937,
+            "angularVelocity": 1.6962259558687312,
+            "velocityX": 2.6535530689447047,
+            "velocityY": 0.8912089220755359,
+            "timestamp": 2.1518008773781796
+        },
+        {
+            "x": 7.3851039731372365,
+            "y": 7.3684303034498875,
+            "heading": 2.8845195362089706,
+            "angularVelocity": 1.6873584975173053,
+            "velocityX": 2.316460142355517,
+            "velocityY": 0.7770933971491014,
+            "timestamp": 2.1944835335599837
+        },
+        {
+            "x": 7.46964567597808,
+            "y": 7.396754020582906,
+            "heading": 2.951617256319432,
+            "angularVelocity": 1.5720136962578515,
+            "velocityX": 1.9807038831122041,
+            "velocityY": 0.6635884377105193,
+            "timestamp": 2.237166189741788
+        },
+        {
+            "x": 7.539938623640353,
+            "y": 7.420270719849242,
+            "heading": 3.0110324789568197,
+            "angularVelocity": 1.3920226141576573,
+            "velocityX": 1.6468737878651336,
+            "velocityY": 0.5509661621378036,
+            "timestamp": 2.279848845923592
+        },
+        {
+            "x": 7.596058846559568,
+            "y": 7.439019303920545,
+            "heading": 3.060856486484029,
+            "angularVelocity": 1.1673127210028025,
+            "velocityX": 1.3148249884022007,
+            "velocityY": 0.4392553263659099,
+            "timestamp": 2.322531502105396
+        },
+        {
+            "x": 7.638072702511279,
+            "y": 7.453036150115233,
+            "heading": 3.09965963024843,
+            "angularVelocity": 0.9091079898851933,
+            "velocityX": 0.9843308666816342,
+            "velocityY": 0.32839676460117173,
+            "timestamp": 2.3652141582872
+        },
+        {
+            "x": 7.666037142481598,
+            "y": 7.462353623790155,
+            "heading": 3.126330643718343,
+            "angularVelocity": 0.6248677063655441,
+            "velocityX": 0.6551710336677993,
+            "velocityY": 0.2182964817192845,
+            "timestamp": 2.4078968144690043
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 0.32025552073032726,
+            "velocityX": 0.3271318790219412,
+            "velocityY": 0.10885864717634237,
+            "timestamp": 2.4505794706508084
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": -9.011054390829982e-34,
+            "velocityY": 6.746182534290948e-34,
+            "timestamp": 2.4932621268326125
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.1.traj
new file mode 100644
index 00000000..8d55d74b
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.1.traj	
@@ -0,0 +1,517 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": 2.1,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 1.2156956395995823e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7323642464210136,
+            "y": 4.371312176942183,
+            "heading": 2.122836377232617,
+            "angularVelocity": 0.41032613171248355,
+            "velocityX": 0.4018428417574259,
+            "velocityY": 0.20325823832090237,
+            "timestamp": 0.05565421129117423
+        },
+        {
+            "x": 0.7771770070003539,
+            "y": 4.393987109637107,
+            "heading": 2.1669272728918627,
+            "angularVelocity": 0.7922292785458643,
+            "velocityX": 0.8051998139886801,
+            "velocityY": 0.40742528137344314,
+            "timestamp": 0.11130842258234847
+        },
+        {
+            "x": 0.8445465122121998,
+            "y": 4.4280825394621655,
+            "heading": 2.2301328847833437,
+            "angularVelocity": 1.135684262252843,
+            "velocityX": 1.2105014813592279,
+            "velocityY": 0.612629826818977,
+            "timestamp": 0.1669626338735227
+        },
+        {
+            "x": 0.9346100694611125,
+            "y": 4.4736618275321725,
+            "heading": 2.3094955096415575,
+            "angularVelocity": 1.4259949609743032,
+            "velocityX": 1.6182703008351718,
+            "velocityY": 0.8189728506175846,
+            "timestamp": 0.22261684516469693
+        },
+        {
+            "x": 1.047532617883247,
+            "y": 4.530798800054343,
+            "heading": 2.4006762269852384,
+            "angularVelocity": 1.638343536424179,
+            "velocityX": 2.0290027619175355,
+            "velocityY": 1.0266423905144981,
+            "timestamp": 0.27827105645587114
+        },
+        {
+            "x": 1.1834754046687317,
+            "y": 4.5995806121594685,
+            "heading": 2.4961863960775177,
+            "angularVelocity": 1.7161355246341592,
+            "velocityX": 2.4426325273796983,
+            "velocityY": 1.2358779418375128,
+            "timestamp": 0.33392526774704534
+        },
+        {
+            "x": 1.342193634237211,
+            "y": 4.6799334714255165,
+            "heading": 2.578155037608781,
+            "angularVelocity": 1.4728201088398516,
+            "velocityX": 2.851863783282992,
+            "velocityY": 1.4437875841174588,
+            "timestamp": 0.38957947903821954
+        },
+        {
+            "x": 1.518721607083693,
+            "y": 4.772275835878884,
+            "heading": 2.6065642311000707,
+            "angularVelocity": 0.5104590080821917,
+            "velocityX": 3.17187089262509,
+            "velocityY": 1.6592161187991088,
+            "timestamp": 0.44523369032939375
+        },
+        {
+            "x": 1.7067559979094145,
+            "y": 4.866979097734677,
+            "heading": 2.6065642752111695,
+            "angularVelocity": 7.925922929779288e-7,
+            "velocityX": 3.378619271809538,
+            "velocityY": 1.7016369410091843,
+            "timestamp": 0.500887901620568
+        },
+        {
+            "x": 1.894790428298913,
+            "y": 4.9616822810363175,
+            "heading": 2.6065643193220347,
+            "angularVelocity": 7.92588092448351e-7,
+            "velocityX": 3.378619982695133,
+            "velocityY": 1.7016355295409376,
+            "timestamp": 0.5565421129117422
+        },
+        {
+            "x": 2.082824858689142,
+            "y": 5.056385464336507,
+            "heading": 2.6065643634329,
+            "angularVelocity": 7.925880881331428e-7,
+            "velocityX": 3.378619982708264,
+            "velocityY": 1.7016355295148662,
+            "timestamp": 0.6121963242029164
+        },
+        {
+            "x": 2.270859289079371,
+            "y": 5.151088647636696,
+            "heading": 2.606564407543764,
+            "angularVelocity": 7.925880850764007e-7,
+            "velocityX": 3.3786199827082646,
+            "velocityY": 1.7016355295148653,
+            "timestamp": 0.6678505354940906
+        },
+        {
+            "x": 2.4588937194696,
+            "y": 5.245791830936885,
+            "heading": 2.606564451654629,
+            "angularVelocity": 7.92588082043664e-7,
+            "velocityX": 3.378619982708265,
+            "velocityY": 1.7016355295148649,
+            "timestamp": 0.7235047467852648
+        },
+        {
+            "x": 2.6469281498598294,
+            "y": 5.340495014237074,
+            "heading": 2.6065644957654936,
+            "angularVelocity": 7.92588079438066e-7,
+            "velocityX": 3.378619982708265,
+            "velocityY": 1.7016355295148646,
+            "timestamp": 0.779158958076439
+        },
+        {
+            "x": 2.8349625802500587,
+            "y": 5.435198197537264,
+            "heading": 2.606564539876358,
+            "angularVelocity": 7.925880759204128e-7,
+            "velocityX": 3.3786199827082655,
+            "velocityY": 1.7016355295148642,
+            "timestamp": 0.8348131693676132
+        },
+        {
+            "x": 3.022997010640288,
+            "y": 5.529901380837453,
+            "heading": 2.606564583987222,
+            "angularVelocity": 7.925880718883517e-7,
+            "velocityX": 3.3786199827082664,
+            "velocityY": 1.7016355295148637,
+            "timestamp": 0.8904673806587874
+        },
+        {
+            "x": 3.2110314410305167,
+            "y": 5.624604564137642,
+            "heading": 2.6065646280980856,
+            "angularVelocity": 7.925880686816096e-7,
+            "velocityX": 3.3786199827082664,
+            "velocityY": 1.7016355295148633,
+            "timestamp": 0.9461215919499616
+        },
+        {
+            "x": 3.399065871420746,
+            "y": 5.719307747437831,
+            "heading": 2.6065646722089495,
+            "angularVelocity": 7.92588065491887e-7,
+            "velocityX": 3.378619982708267,
+            "velocityY": 1.7016355295148633,
+            "timestamp": 1.001775803241136
+        },
+        {
+            "x": 3.5871003018109753,
+            "y": 5.8140109307380206,
+            "heading": 2.606564716319813,
+            "angularVelocity": 7.92588062190758e-7,
+            "velocityX": 3.378619982708267,
+            "velocityY": 1.7016355295148633,
+            "timestamp": 1.0574300145323101
+        },
+        {
+            "x": 3.7751347322012045,
+            "y": 5.90871411403821,
+            "heading": 2.6065647604306763,
+            "angularVelocity": 7.92588058976371e-7,
+            "velocityX": 3.378619982708267,
+            "velocityY": 1.7016355295148629,
+            "timestamp": 1.1130842258234843
+        },
+        {
+            "x": 3.963169162591434,
+            "y": 6.003417297338399,
+            "heading": 2.6065648045415397,
+            "angularVelocity": 7.925880551764629e-7,
+            "velocityX": 3.3786199827082677,
+            "velocityY": 1.7016355295148622,
+            "timestamp": 1.1687384371146585
+        },
+        {
+            "x": 4.151203592981663,
+            "y": 6.098120480638587,
+            "heading": 2.6065648486524027,
+            "angularVelocity": 7.925880525134244e-7,
+            "velocityX": 3.378619982708268,
+            "velocityY": 1.7016355295148615,
+            "timestamp": 1.2243926484058327
+        },
+        {
+            "x": 4.339238023371892,
+            "y": 6.192823663938777,
+            "heading": 2.606564892763265,
+            "angularVelocity": 7.925880485176538e-7,
+            "velocityX": 3.3786199827082686,
+            "velocityY": 1.701635529514862,
+            "timestamp": 1.280046859697007
+        },
+        {
+            "x": 4.527272453762121,
+            "y": 6.287526847238966,
+            "heading": 2.6065649368741277,
+            "angularVelocity": 7.925880449736749e-7,
+            "velocityX": 3.37861998270827,
+            "velocityY": 1.7016355295148586,
+            "timestamp": 1.3357010709881811
+        },
+        {
+            "x": 4.715306884156566,
+            "y": 6.382230030530785,
+            "heading": 2.6065649809849902,
+            "angularVelocity": 7.925880433141827e-7,
+            "velocityX": 3.3786199827840138,
+            "velocityY": 1.7016355293644703,
+            "timestamp": 1.3913552822793553
+        },
+        {
+            "x": 4.903341542764552,
+            "y": 6.476932760700215,
+            "heading": 2.6065650250958465,
+            "angularVelocity": 7.925879324126063e-7,
+            "velocityX": 3.378624083345972,
+            "velocityY": 1.7016273876195969,
+            "timestamp": 1.4470094935705295
+        },
+        {
+            "x": 5.097883205540706,
+            "y": 6.557426122164244,
+            "heading": 2.6065650695055442,
+            "angularVelocity": 7.97957544404275e-7,
+            "velocityX": 3.495542534208987,
+            "velocityY": 1.4463121405656068,
+            "timestamp": 1.5026637048617038
+        },
+        {
+            "x": 5.300816520591001,
+            "y": 6.613494907557912,
+            "heading": 2.6065651167952333,
+            "angularVelocity": 8.497055004082428e-7,
+            "velocityX": 3.6463245160115534,
+            "velocityY": 1.0074491056988386,
+            "timestamp": 1.558317916152878
+        },
+        {
+            "x": 5.5090879488774736,
+            "y": 6.644295063089685,
+            "heading": 2.606565171305544,
+            "angularVelocity": 9.79446290291877e-7,
+            "velocityX": 3.742240226832587,
+            "velocityY": 0.5534200344809682,
+            "timestamp": 1.6139721274440522
+        },
+        {
+            "x": 5.7195634841918945,
+            "y": 6.649363040924072,
+            "heading": 2.6065652359371767,
+            "angularVelocity": 0.0000011613071303259608,
+            "velocityX": 3.781843825137414,
+            "velocityY": 0.0910618930142201,
+            "timestamp": 1.6696263387352264
+        },
+        {
+            "x": 5.838739218913105,
+            "y": 6.643942809110191,
+            "heading": 2.606565305952835,
+            "angularVelocity": 0.0000022201789880280334,
+            "velocityX": 3.77903269949585,
+            "velocityY": -0.1718741932778599,
+            "timestamp": 1.7011623809444163
+        },
+        {
+            "x": 5.957250522969574,
+            "y": 6.630256808888294,
+            "heading": 2.606565367221186,
+            "angularVelocity": 0.00000194280407444938,
+            "velocityX": 3.7579637695288737,
+            "velocityY": -0.4339796392683931,
+            "timestamp": 1.7326984231536062
+        },
+        {
+            "x": 6.074524798614125,
+            "y": 6.608371177242311,
+            "heading": 2.60656542248837,
+            "angularVelocity": 0.000001752508552376974,
+            "velocityX": 3.718737908410616,
+            "velocityY": -0.693987898063049,
+            "timestamp": 1.7642344653627962
+        },
+        {
+            "x": 6.189995430266423,
+            "y": 6.578391677512442,
+            "heading": 2.606565473629249,
+            "angularVelocity": 0.0000016216644704538906,
+            "velocityX": 3.6615448091532925,
+            "velocityY": -0.9506424278292124,
+            "timestamp": 1.795770507571986
+        },
+        {
+            "x": 6.303104529085719,
+            "y": 6.5404632008330585,
+            "heading": 2.606565522006597,
+            "angularVelocity": 0.000001534033588555687,
+            "velocityX": 3.586661194483535,
+            "velocityY": -1.2027024960136288,
+            "timestamp": 1.827306549781176
+        },
+        {
+            "x": 6.413305655265644,
+            "y": 6.4947691134226035,
+            "heading": 2.6065655686762406,
+            "angularVelocity": 0.000001479882707645635,
+            "velocityX": 3.4944501104140286,
+            "velocityY": -1.4489480673367345,
+            "timestamp": 1.858842591990366
+        },
+        {
+            "x": 6.520066647831161,
+            "y": 6.441530702158678,
+            "heading": 2.6065656145154117,
+            "angularVelocity": 0.000001453548632665294,
+            "velocityX": 3.3853643351100575,
+            "velocityY": -1.6881766871941624,
+            "timestamp": 1.8903786341995559
+        },
+        {
+            "x": 6.6242681306818785,
+            "y": 6.383442071865126,
+            "heading": 2.6065656601861753,
+            "angularVelocity": 0.0000014482084799406948,
+            "velocityX": 3.3042029231034076,
+            "velocityY": -1.8419759178475867,
+            "timestamp": 1.9219146764087458
+        },
+        {
+            "x": 6.728469078482098,
+            "y": 6.325352481789039,
+            "heading": 2.6065657058569283,
+            "angularVelocity": 0.0000014482081475491236,
+            "velocityX": 3.3041859567861076,
+            "velocityY": -1.8420063523113561,
+            "timestamp": 1.9534507186179357
+        },
+        {
+            "x": 6.832670029211484,
+            "y": 6.267262896967356,
+            "heading": 2.6065657515277794,
+            "angularVelocity": 0.0000014482112413699588,
+            "velocityX": 3.3041860496692763,
+            "velocityY": -1.8420061856955277,
+            "timestamp": 1.9849867608271257
+        },
+        {
+            "x": 6.936347380984222,
+            "y": 6.209534885446302,
+            "heading": 2.608700420086438,
+            "angularVelocity": 0.06768980535028488,
+            "velocityX": 3.2875828578934874,
+            "velocityY": -1.8305407868915176,
+            "timestamp": 2.0165228030363154
+        },
+        {
+            "x": 7.0344207678165205,
+            "y": 6.154966008244285,
+            "heading": 2.632443469248245,
+            "angularVelocity": 0.7528861422848001,
+            "velocityX": 3.1098825331898827,
+            "velocityY": -1.730365428865226,
+            "timestamp": 2.0480588452455053
+        },
+        {
+            "x": 7.1267043537102115,
+            "y": 6.103735031776186,
+            "heading": 2.6761348400547162,
+            "angularVelocity": 1.3854424254207447,
+            "velocityX": 2.9262893955285,
+            "velocityY": -1.6245214325965975,
+            "timestamp": 2.0795948874546952
+        },
+        {
+            "x": 7.212236583644832,
+            "y": 6.056223391759097,
+            "heading": 2.7313687787461847,
+            "angularVelocity": 1.7514543621257885,
+            "velocityX": 2.7122055889973176,
+            "velocityY": -1.5065822052725362,
+            "timestamp": 2.111130929663885
+        },
+        {
+            "x": 7.290555468694817,
+            "y": 6.012738407550467,
+            "heading": 2.789179732571192,
+            "angularVelocity": 1.8331708665763002,
+            "velocityX": 2.4834722293453213,
+            "velocityY": -1.3788979580943836,
+            "timestamp": 2.142666971873075
+        },
+        {
+            "x": 7.361635905599927,
+            "y": 5.973292872851013,
+            "heading": 2.8461736255132477,
+            "angularVelocity": 1.8072620706173155,
+            "velocityX": 2.2539428515984796,
+            "velocityY": -1.2508080258708043,
+            "timestamp": 2.174203014082265
+        },
+        {
+            "x": 7.425505527447213,
+            "y": 5.937869720935748,
+            "heading": 2.900420841886284,
+            "angularVelocity": 1.7201656445407778,
+            "velocityX": 2.0252897121209994,
+            "velocityY": -1.1232592752220105,
+            "timestamp": 2.205739056291455
+        },
+        {
+            "x": 7.482197651528883,
+            "y": 5.90644694625493,
+            "heading": 2.9506667136326055,
+            "angularVelocity": 1.5932840085963447,
+            "velocityX": 1.7976930556348818,
+            "velocityY": -0.9964083150440386,
+            "timestamp": 2.237275098500645
+        },
+        {
+            "x": 7.531741810313519,
+            "y": 5.879003134115031,
+            "heading": 2.9960449007561833,
+            "angularVelocity": 1.4389309483596011,
+            "velocityX": 1.5710328663309023,
+            "velocityY": -0.8702364094344716,
+            "timestamp": 2.268811140709835
+        },
+        {
+            "x": 7.5741623718487,
+            "y": 5.855519184527709,
+            "heading": 3.0359339398400818,
+            "angularVelocity": 1.264871438822304,
+            "velocityX": 1.3451453817124663,
+            "velocityY": -0.7446701596714312,
+            "timestamp": 2.3003471829190247
+        },
+        {
+            "x": 7.609479178893684,
+            "y": 5.835978699422795,
+            "heading": 3.0698731008846027,
+            "angularVelocity": 1.076202296388058,
+            "velocityX": 1.1198871060203273,
+            "velocityY": -0.6196238886063927,
+            "timestamp": 2.3318832251282147
+        },
+        {
+            "x": 7.637708552445183,
+            "y": 5.820367770747114,
+            "heading": 3.0975089102886857,
+            "angularVelocity": 0.8763245945944945,
+            "velocityX": 0.895146365046179,
+            "velocityY": -0.4950186384241787,
+            "timestamp": 2.3634192673374046
+        },
+        {
+            "x": 7.658864191629111,
+            "y": 5.808674501363959,
+            "heading": 3.118559676862925,
+            "angularVelocity": 0.6675145357366632,
+            "velocityX": 0.6708400199236877,
+            "velocityY": -0.3707906434672271,
+            "timestamp": 2.3949553095465945
+        },
+        {
+            "x": 7.672957876154406,
+            "y": 5.800888435996016,
+            "heading": 3.1327912648609026,
+            "angularVelocity": 0.45128009100108973,
+            "velocityX": 0.4469072064213547,
+            "velocityY": -0.24689418273527672,
+            "timestamp": 2.4264913517557845
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 0.22858718577555334,
+            "velocityX": 0.2233039833876712,
+            "velocityY": -0.12330133154384845,
+            "timestamp": 2.4580273939649744
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 3.1728947336241785e-32,
+            "velocityX": 2.0932493862769896e-33,
+            "velocityY": -2.0877321220317972e-33,
+            "timestamp": 2.4895634361741643
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.traj
new file mode 100644
index 00000000..8d55d74b
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.traj	
@@ -0,0 +1,517 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": 2.1,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 1.2156956395995823e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7323642464210136,
+            "y": 4.371312176942183,
+            "heading": 2.122836377232617,
+            "angularVelocity": 0.41032613171248355,
+            "velocityX": 0.4018428417574259,
+            "velocityY": 0.20325823832090237,
+            "timestamp": 0.05565421129117423
+        },
+        {
+            "x": 0.7771770070003539,
+            "y": 4.393987109637107,
+            "heading": 2.1669272728918627,
+            "angularVelocity": 0.7922292785458643,
+            "velocityX": 0.8051998139886801,
+            "velocityY": 0.40742528137344314,
+            "timestamp": 0.11130842258234847
+        },
+        {
+            "x": 0.8445465122121998,
+            "y": 4.4280825394621655,
+            "heading": 2.2301328847833437,
+            "angularVelocity": 1.135684262252843,
+            "velocityX": 1.2105014813592279,
+            "velocityY": 0.612629826818977,
+            "timestamp": 0.1669626338735227
+        },
+        {
+            "x": 0.9346100694611125,
+            "y": 4.4736618275321725,
+            "heading": 2.3094955096415575,
+            "angularVelocity": 1.4259949609743032,
+            "velocityX": 1.6182703008351718,
+            "velocityY": 0.8189728506175846,
+            "timestamp": 0.22261684516469693
+        },
+        {
+            "x": 1.047532617883247,
+            "y": 4.530798800054343,
+            "heading": 2.4006762269852384,
+            "angularVelocity": 1.638343536424179,
+            "velocityX": 2.0290027619175355,
+            "velocityY": 1.0266423905144981,
+            "timestamp": 0.27827105645587114
+        },
+        {
+            "x": 1.1834754046687317,
+            "y": 4.5995806121594685,
+            "heading": 2.4961863960775177,
+            "angularVelocity": 1.7161355246341592,
+            "velocityX": 2.4426325273796983,
+            "velocityY": 1.2358779418375128,
+            "timestamp": 0.33392526774704534
+        },
+        {
+            "x": 1.342193634237211,
+            "y": 4.6799334714255165,
+            "heading": 2.578155037608781,
+            "angularVelocity": 1.4728201088398516,
+            "velocityX": 2.851863783282992,
+            "velocityY": 1.4437875841174588,
+            "timestamp": 0.38957947903821954
+        },
+        {
+            "x": 1.518721607083693,
+            "y": 4.772275835878884,
+            "heading": 2.6065642311000707,
+            "angularVelocity": 0.5104590080821917,
+            "velocityX": 3.17187089262509,
+            "velocityY": 1.6592161187991088,
+            "timestamp": 0.44523369032939375
+        },
+        {
+            "x": 1.7067559979094145,
+            "y": 4.866979097734677,
+            "heading": 2.6065642752111695,
+            "angularVelocity": 7.925922929779288e-7,
+            "velocityX": 3.378619271809538,
+            "velocityY": 1.7016369410091843,
+            "timestamp": 0.500887901620568
+        },
+        {
+            "x": 1.894790428298913,
+            "y": 4.9616822810363175,
+            "heading": 2.6065643193220347,
+            "angularVelocity": 7.92588092448351e-7,
+            "velocityX": 3.378619982695133,
+            "velocityY": 1.7016355295409376,
+            "timestamp": 0.5565421129117422
+        },
+        {
+            "x": 2.082824858689142,
+            "y": 5.056385464336507,
+            "heading": 2.6065643634329,
+            "angularVelocity": 7.925880881331428e-7,
+            "velocityX": 3.378619982708264,
+            "velocityY": 1.7016355295148662,
+            "timestamp": 0.6121963242029164
+        },
+        {
+            "x": 2.270859289079371,
+            "y": 5.151088647636696,
+            "heading": 2.606564407543764,
+            "angularVelocity": 7.925880850764007e-7,
+            "velocityX": 3.3786199827082646,
+            "velocityY": 1.7016355295148653,
+            "timestamp": 0.6678505354940906
+        },
+        {
+            "x": 2.4588937194696,
+            "y": 5.245791830936885,
+            "heading": 2.606564451654629,
+            "angularVelocity": 7.92588082043664e-7,
+            "velocityX": 3.378619982708265,
+            "velocityY": 1.7016355295148649,
+            "timestamp": 0.7235047467852648
+        },
+        {
+            "x": 2.6469281498598294,
+            "y": 5.340495014237074,
+            "heading": 2.6065644957654936,
+            "angularVelocity": 7.92588079438066e-7,
+            "velocityX": 3.378619982708265,
+            "velocityY": 1.7016355295148646,
+            "timestamp": 0.779158958076439
+        },
+        {
+            "x": 2.8349625802500587,
+            "y": 5.435198197537264,
+            "heading": 2.606564539876358,
+            "angularVelocity": 7.925880759204128e-7,
+            "velocityX": 3.3786199827082655,
+            "velocityY": 1.7016355295148642,
+            "timestamp": 0.8348131693676132
+        },
+        {
+            "x": 3.022997010640288,
+            "y": 5.529901380837453,
+            "heading": 2.606564583987222,
+            "angularVelocity": 7.925880718883517e-7,
+            "velocityX": 3.3786199827082664,
+            "velocityY": 1.7016355295148637,
+            "timestamp": 0.8904673806587874
+        },
+        {
+            "x": 3.2110314410305167,
+            "y": 5.624604564137642,
+            "heading": 2.6065646280980856,
+            "angularVelocity": 7.925880686816096e-7,
+            "velocityX": 3.3786199827082664,
+            "velocityY": 1.7016355295148633,
+            "timestamp": 0.9461215919499616
+        },
+        {
+            "x": 3.399065871420746,
+            "y": 5.719307747437831,
+            "heading": 2.6065646722089495,
+            "angularVelocity": 7.92588065491887e-7,
+            "velocityX": 3.378619982708267,
+            "velocityY": 1.7016355295148633,
+            "timestamp": 1.001775803241136
+        },
+        {
+            "x": 3.5871003018109753,
+            "y": 5.8140109307380206,
+            "heading": 2.606564716319813,
+            "angularVelocity": 7.92588062190758e-7,
+            "velocityX": 3.378619982708267,
+            "velocityY": 1.7016355295148633,
+            "timestamp": 1.0574300145323101
+        },
+        {
+            "x": 3.7751347322012045,
+            "y": 5.90871411403821,
+            "heading": 2.6065647604306763,
+            "angularVelocity": 7.92588058976371e-7,
+            "velocityX": 3.378619982708267,
+            "velocityY": 1.7016355295148629,
+            "timestamp": 1.1130842258234843
+        },
+        {
+            "x": 3.963169162591434,
+            "y": 6.003417297338399,
+            "heading": 2.6065648045415397,
+            "angularVelocity": 7.925880551764629e-7,
+            "velocityX": 3.3786199827082677,
+            "velocityY": 1.7016355295148622,
+            "timestamp": 1.1687384371146585
+        },
+        {
+            "x": 4.151203592981663,
+            "y": 6.098120480638587,
+            "heading": 2.6065648486524027,
+            "angularVelocity": 7.925880525134244e-7,
+            "velocityX": 3.378619982708268,
+            "velocityY": 1.7016355295148615,
+            "timestamp": 1.2243926484058327
+        },
+        {
+            "x": 4.339238023371892,
+            "y": 6.192823663938777,
+            "heading": 2.606564892763265,
+            "angularVelocity": 7.925880485176538e-7,
+            "velocityX": 3.3786199827082686,
+            "velocityY": 1.701635529514862,
+            "timestamp": 1.280046859697007
+        },
+        {
+            "x": 4.527272453762121,
+            "y": 6.287526847238966,
+            "heading": 2.6065649368741277,
+            "angularVelocity": 7.925880449736749e-7,
+            "velocityX": 3.37861998270827,
+            "velocityY": 1.7016355295148586,
+            "timestamp": 1.3357010709881811
+        },
+        {
+            "x": 4.715306884156566,
+            "y": 6.382230030530785,
+            "heading": 2.6065649809849902,
+            "angularVelocity": 7.925880433141827e-7,
+            "velocityX": 3.3786199827840138,
+            "velocityY": 1.7016355293644703,
+            "timestamp": 1.3913552822793553
+        },
+        {
+            "x": 4.903341542764552,
+            "y": 6.476932760700215,
+            "heading": 2.6065650250958465,
+            "angularVelocity": 7.925879324126063e-7,
+            "velocityX": 3.378624083345972,
+            "velocityY": 1.7016273876195969,
+            "timestamp": 1.4470094935705295
+        },
+        {
+            "x": 5.097883205540706,
+            "y": 6.557426122164244,
+            "heading": 2.6065650695055442,
+            "angularVelocity": 7.97957544404275e-7,
+            "velocityX": 3.495542534208987,
+            "velocityY": 1.4463121405656068,
+            "timestamp": 1.5026637048617038
+        },
+        {
+            "x": 5.300816520591001,
+            "y": 6.613494907557912,
+            "heading": 2.6065651167952333,
+            "angularVelocity": 8.497055004082428e-7,
+            "velocityX": 3.6463245160115534,
+            "velocityY": 1.0074491056988386,
+            "timestamp": 1.558317916152878
+        },
+        {
+            "x": 5.5090879488774736,
+            "y": 6.644295063089685,
+            "heading": 2.606565171305544,
+            "angularVelocity": 9.79446290291877e-7,
+            "velocityX": 3.742240226832587,
+            "velocityY": 0.5534200344809682,
+            "timestamp": 1.6139721274440522
+        },
+        {
+            "x": 5.7195634841918945,
+            "y": 6.649363040924072,
+            "heading": 2.6065652359371767,
+            "angularVelocity": 0.0000011613071303259608,
+            "velocityX": 3.781843825137414,
+            "velocityY": 0.0910618930142201,
+            "timestamp": 1.6696263387352264
+        },
+        {
+            "x": 5.838739218913105,
+            "y": 6.643942809110191,
+            "heading": 2.606565305952835,
+            "angularVelocity": 0.0000022201789880280334,
+            "velocityX": 3.77903269949585,
+            "velocityY": -0.1718741932778599,
+            "timestamp": 1.7011623809444163
+        },
+        {
+            "x": 5.957250522969574,
+            "y": 6.630256808888294,
+            "heading": 2.606565367221186,
+            "angularVelocity": 0.00000194280407444938,
+            "velocityX": 3.7579637695288737,
+            "velocityY": -0.4339796392683931,
+            "timestamp": 1.7326984231536062
+        },
+        {
+            "x": 6.074524798614125,
+            "y": 6.608371177242311,
+            "heading": 2.60656542248837,
+            "angularVelocity": 0.000001752508552376974,
+            "velocityX": 3.718737908410616,
+            "velocityY": -0.693987898063049,
+            "timestamp": 1.7642344653627962
+        },
+        {
+            "x": 6.189995430266423,
+            "y": 6.578391677512442,
+            "heading": 2.606565473629249,
+            "angularVelocity": 0.0000016216644704538906,
+            "velocityX": 3.6615448091532925,
+            "velocityY": -0.9506424278292124,
+            "timestamp": 1.795770507571986
+        },
+        {
+            "x": 6.303104529085719,
+            "y": 6.5404632008330585,
+            "heading": 2.606565522006597,
+            "angularVelocity": 0.000001534033588555687,
+            "velocityX": 3.586661194483535,
+            "velocityY": -1.2027024960136288,
+            "timestamp": 1.827306549781176
+        },
+        {
+            "x": 6.413305655265644,
+            "y": 6.4947691134226035,
+            "heading": 2.6065655686762406,
+            "angularVelocity": 0.000001479882707645635,
+            "velocityX": 3.4944501104140286,
+            "velocityY": -1.4489480673367345,
+            "timestamp": 1.858842591990366
+        },
+        {
+            "x": 6.520066647831161,
+            "y": 6.441530702158678,
+            "heading": 2.6065656145154117,
+            "angularVelocity": 0.000001453548632665294,
+            "velocityX": 3.3853643351100575,
+            "velocityY": -1.6881766871941624,
+            "timestamp": 1.8903786341995559
+        },
+        {
+            "x": 6.6242681306818785,
+            "y": 6.383442071865126,
+            "heading": 2.6065656601861753,
+            "angularVelocity": 0.0000014482084799406948,
+            "velocityX": 3.3042029231034076,
+            "velocityY": -1.8419759178475867,
+            "timestamp": 1.9219146764087458
+        },
+        {
+            "x": 6.728469078482098,
+            "y": 6.325352481789039,
+            "heading": 2.6065657058569283,
+            "angularVelocity": 0.0000014482081475491236,
+            "velocityX": 3.3041859567861076,
+            "velocityY": -1.8420063523113561,
+            "timestamp": 1.9534507186179357
+        },
+        {
+            "x": 6.832670029211484,
+            "y": 6.267262896967356,
+            "heading": 2.6065657515277794,
+            "angularVelocity": 0.0000014482112413699588,
+            "velocityX": 3.3041860496692763,
+            "velocityY": -1.8420061856955277,
+            "timestamp": 1.9849867608271257
+        },
+        {
+            "x": 6.936347380984222,
+            "y": 6.209534885446302,
+            "heading": 2.608700420086438,
+            "angularVelocity": 0.06768980535028488,
+            "velocityX": 3.2875828578934874,
+            "velocityY": -1.8305407868915176,
+            "timestamp": 2.0165228030363154
+        },
+        {
+            "x": 7.0344207678165205,
+            "y": 6.154966008244285,
+            "heading": 2.632443469248245,
+            "angularVelocity": 0.7528861422848001,
+            "velocityX": 3.1098825331898827,
+            "velocityY": -1.730365428865226,
+            "timestamp": 2.0480588452455053
+        },
+        {
+            "x": 7.1267043537102115,
+            "y": 6.103735031776186,
+            "heading": 2.6761348400547162,
+            "angularVelocity": 1.3854424254207447,
+            "velocityX": 2.9262893955285,
+            "velocityY": -1.6245214325965975,
+            "timestamp": 2.0795948874546952
+        },
+        {
+            "x": 7.212236583644832,
+            "y": 6.056223391759097,
+            "heading": 2.7313687787461847,
+            "angularVelocity": 1.7514543621257885,
+            "velocityX": 2.7122055889973176,
+            "velocityY": -1.5065822052725362,
+            "timestamp": 2.111130929663885
+        },
+        {
+            "x": 7.290555468694817,
+            "y": 6.012738407550467,
+            "heading": 2.789179732571192,
+            "angularVelocity": 1.8331708665763002,
+            "velocityX": 2.4834722293453213,
+            "velocityY": -1.3788979580943836,
+            "timestamp": 2.142666971873075
+        },
+        {
+            "x": 7.361635905599927,
+            "y": 5.973292872851013,
+            "heading": 2.8461736255132477,
+            "angularVelocity": 1.8072620706173155,
+            "velocityX": 2.2539428515984796,
+            "velocityY": -1.2508080258708043,
+            "timestamp": 2.174203014082265
+        },
+        {
+            "x": 7.425505527447213,
+            "y": 5.937869720935748,
+            "heading": 2.900420841886284,
+            "angularVelocity": 1.7201656445407778,
+            "velocityX": 2.0252897121209994,
+            "velocityY": -1.1232592752220105,
+            "timestamp": 2.205739056291455
+        },
+        {
+            "x": 7.482197651528883,
+            "y": 5.90644694625493,
+            "heading": 2.9506667136326055,
+            "angularVelocity": 1.5932840085963447,
+            "velocityX": 1.7976930556348818,
+            "velocityY": -0.9964083150440386,
+            "timestamp": 2.237275098500645
+        },
+        {
+            "x": 7.531741810313519,
+            "y": 5.879003134115031,
+            "heading": 2.9960449007561833,
+            "angularVelocity": 1.4389309483596011,
+            "velocityX": 1.5710328663309023,
+            "velocityY": -0.8702364094344716,
+            "timestamp": 2.268811140709835
+        },
+        {
+            "x": 7.5741623718487,
+            "y": 5.855519184527709,
+            "heading": 3.0359339398400818,
+            "angularVelocity": 1.264871438822304,
+            "velocityX": 1.3451453817124663,
+            "velocityY": -0.7446701596714312,
+            "timestamp": 2.3003471829190247
+        },
+        {
+            "x": 7.609479178893684,
+            "y": 5.835978699422795,
+            "heading": 3.0698731008846027,
+            "angularVelocity": 1.076202296388058,
+            "velocityX": 1.1198871060203273,
+            "velocityY": -0.6196238886063927,
+            "timestamp": 2.3318832251282147
+        },
+        {
+            "x": 7.637708552445183,
+            "y": 5.820367770747114,
+            "heading": 3.0975089102886857,
+            "angularVelocity": 0.8763245945944945,
+            "velocityX": 0.895146365046179,
+            "velocityY": -0.4950186384241787,
+            "timestamp": 2.3634192673374046
+        },
+        {
+            "x": 7.658864191629111,
+            "y": 5.808674501363959,
+            "heading": 3.118559676862925,
+            "angularVelocity": 0.6675145357366632,
+            "velocityX": 0.6708400199236877,
+            "velocityY": -0.3707906434672271,
+            "timestamp": 2.3949553095465945
+        },
+        {
+            "x": 7.672957876154406,
+            "y": 5.800888435996016,
+            "heading": 3.1327912648609026,
+            "angularVelocity": 0.45128009100108973,
+            "velocityX": 0.4469072064213547,
+            "velocityY": -0.24689418273527672,
+            "timestamp": 2.4264913517557845
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 0.22858718577555334,
+            "velocityX": 0.2233039833876712,
+            "velocityY": -0.12330133154384845,
+            "timestamp": 2.4580273939649744
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 3.1728947336241785e-32,
+            "velocityX": 2.0932493862769896e-33,
+            "velocityY": -2.0877321220317972e-33,
+            "timestamp": 2.4895634361741643
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.1.traj
new file mode 100644
index 00000000..0e00ab8a
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.1.traj	
@@ -0,0 +1,580 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": 2.1,
+            "angularVelocity": 0,
+            "velocityX": 4.814287914507576e-35,
+            "velocityY": 2.4063306961726914e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7189874993597535,
+            "y": 4.351109558323372,
+            "heading": 2.1116856791570084,
+            "angularVelocity": 0.2954657391089134,
+            "velocityX": 0.22724380032956057,
+            "velocityY": -0.22478975211418176,
+            "timestamp": 0.039550031053517704
+        },
+        {
+            "x": 0.7369922825493326,
+            "y": 4.3333146846737405,
+            "heading": 2.134597544316128,
+            "angularVelocity": 0.5793134556106004,
+            "velocityX": 0.45524068401401757,
+            "velocityY": -0.4499332409006739,
+            "timestamp": 0.07910006210703541
+        },
+        {
+            "x": 0.7640484897224145,
+            "y": 4.306598997837255,
+            "heading": 2.168179876708578,
+            "angularVelocity": 0.849110139686295,
+            "velocityX": 0.6841007820315073,
+            "velocityY": -0.67549091934555,
+            "timestamp": 0.11865009316055311
+        },
+        {
+            "x": 0.800195666694539,
+            "y": 4.270941917715144,
+            "heading": 2.2117258914758198,
+            "angularVelocity": 1.1010361713323769,
+            "velocityX": 0.9139607734621368,
+            "velocityY": -0.9015689538615157,
+            "timestamp": 0.15820012421407081
+        },
+        {
+            "x": 0.8454794987248587,
+            "y": 4.226316705338956,
+            "heading": 2.264304991238856,
+            "angularVelocity": 1.3294325784950254,
+            "velocityX": 1.14497589063946,
+            "velocityY": -1.1283230679592324,
+            "timestamp": 0.19775015526758852
+        },
+        {
+            "x": 0.8999520283363762,
+            "y": 4.172689027996317,
+            "heading": 2.324662422471035,
+            "angularVelocity": 1.5261032576815274,
+            "velocityX": 1.3773068733576257,
+            "velocityY": -1.3559452651268766,
+            "timestamp": 0.23730018632110622
+        },
+        {
+            "x": 0.963671089377297,
+            "y": 4.110017781664974,
+            "heading": 2.391063304695866,
+            "angularVelocity": 1.6789084725364454,
+            "velocityX": 1.6111001519745567,
+            "velocityY": -1.584606754076596,
+            "timestamp": 0.2768502173746239
+        },
+        {
+            "x": 1.0366970385232381,
+            "y": 4.038262577868419,
+            "heading": 2.4609901353872035,
+            "angularVelocity": 1.7680600704640437,
+            "velocityX": 1.846419514743869,
+            "velocityY": -1.8142894426418532,
+            "timestamp": 0.31640024842814163
+        },
+        {
+            "x": 1.1190717343496346,
+            "y": 3.9574142704891733,
+            "heading": 2.5303614875414775,
+            "angularVelocity": 1.7540151121601602,
+            "velocityX": 2.082797247742485,
+            "velocityY": -2.0442033855762225,
+            "timestamp": 0.35595027948165936
+        },
+        {
+            "x": 1.21066072775428,
+            "y": 3.867689453216591,
+            "heading": 2.590569114237573,
+            "angularVelocity": 1.522315535343692,
+            "velocityX": 2.3157755117994827,
+            "velocityY": -2.268640880488067,
+            "timestamp": 0.3955003105351771
+        },
+        {
+            "x": 1.310358949370952,
+            "y": 3.7710393719100375,
+            "heading": 2.620822405939027,
+            "angularVelocity": 0.764937242666538,
+            "velocityX": 2.5208127265883533,
+            "velocityY": -2.4437422356450176,
+            "timestamp": 0.4350503415886948
+        },
+        {
+            "x": 1.418053038225142,
+            "y": 3.6673441481868845,
+            "heading": 2.6211471035683176,
+            "angularVelocity": 0.008209794547368329,
+            "velocityX": 2.722983673728656,
+            "velocityY": -2.621874647401313,
+            "timestamp": 0.47460037264221255
+        },
+        {
+            "x": 1.5245836891775304,
+            "y": 3.5622918530292758,
+            "heading": 2.6211471537170588,
+            "angularVelocity": 0.0000012679823435232559,
+            "velocityX": 2.6935668092961826,
+            "velocityY": -2.656187425376634,
+            "timestamp": 0.5141504036957303
+        },
+        {
+            "x": 1.6311142725711645,
+            "y": 3.4572394893622254,
+            "heading": 2.621147203865785,
+            "angularVelocity": 0.0000012679819635220034,
+            "velocityX": 2.6935651011115636,
+            "velocityY": -2.6561891575988192,
+            "timestamp": 0.553700434749248
+        },
+        {
+            "x": 1.7376448559612427,
+            "y": 3.3521871256915694,
+            "heading": 2.621147254014511,
+            "angularVelocity": 0.0000012679819538520794,
+            "velocityX": 2.6935651010216595,
+            "velocityY": -2.656189157689989,
+            "timestamp": 0.5932504658027657
+        },
+        {
+            "x": 1.8441754393513208,
+            "y": 3.2471347620209134,
+            "heading": 2.621147304163236,
+            "angularVelocity": 0.000001267981944384733,
+            "velocityX": 2.693565101021656,
+            "velocityY": -2.656189157689993,
+            "timestamp": 0.6328004968562835
+        },
+        {
+            "x": 1.9507060227413988,
+            "y": 3.142082398350257,
+            "heading": 2.621147354311961,
+            "angularVelocity": 0.0000012679819349678633,
+            "velocityX": 2.6935651010216572,
+            "velocityY": -2.656189157689994,
+            "timestamp": 0.6723505279098012
+        },
+        {
+            "x": 2.057236606131477,
+            "y": 3.037030034679601,
+            "heading": 2.6211474044606855,
+            "angularVelocity": 0.0000012679819252039202,
+            "velocityX": 2.693565101021662,
+            "velocityY": -2.6561891576899894,
+            "timestamp": 0.7119005589633189
+        },
+        {
+            "x": 2.1637671895248896,
+            "y": 2.931977671012326,
+            "heading": 2.6211474546094093,
+            "angularVelocity": 0.0000012679819154308074,
+            "velocityX": 2.69356510110597,
+            "velocityY": -2.656189157604496,
+            "timestamp": 0.7514505900168367
+        },
+        {
+            "x": 2.2702978362708315,
+            "y": 2.8269253715890703,
+            "heading": 2.6211475047581403,
+            "angularVelocity": 0.0000012679820770656017,
+            "velocityX": 2.693566702938569,
+            "velocityY": -2.6561875332310767,
+            "timestamp": 0.7910006210703544
+        },
+        {
+            "x": 2.3779994803856632,
+            "y": 2.7230739385096507,
+            "heading": 2.6211475550861425,
+            "angularVelocity": 0.0000012725148543062649,
+            "velocityX": 2.723174704189075,
+            "velocityY": -2.6258243119680853,
+            "timestamp": 0.8305506521238721
+        },
+        {
+            "x": 2.4943342208862305,
+            "y": 2.6289942264556885,
+            "heading": 2.6211476076664186,
+            "angularVelocity": 0.0000013294623338808287,
+            "velocityX": 2.941457627255649,
+            "velocityY": -2.378751913662396,
+            "timestamp": 0.8701006831773899
+        },
+        {
+            "x": 2.6932881359026166,
+            "y": 2.508539731368457,
+            "heading": 2.6211476558145916,
+            "angularVelocity": 7.831464659210102e-7,
+            "velocityX": 3.236053335597206,
+            "velocityY": -1.9592334766702564,
+            "timestamp": 0.9315811021889849
+        },
+        {
+            "x": 2.899938569108162,
+            "y": 2.401827293887261,
+            "heading": 2.621147703190774,
+            "angularVelocity": 7.705897835721374e-7,
+            "velocityX": 3.361239830954497,
+            "velocityY": -1.7357142192064432,
+            "timestamp": 0.99306152120058
+        },
+        {
+            "x": 3.106589142147435,
+            "y": 2.295115127196865,
+            "heading": 2.6211477505669496,
+            "angularVelocity": 7.705896615935058e-7,
+            "velocityX": 3.361242105397809,
+            "velocityY": -1.7357098147016614,
+            "timestamp": 1.0545419402121752
+        },
+        {
+            "x": 3.313239715188592,
+            "y": 2.188402960510117,
+            "heading": 2.621147797943125,
+            "angularVelocity": 7.705896625567414e-7,
+            "velocityX": 3.361242105428448,
+            "velocityY": -1.735709814642326,
+            "timestamp": 1.1160223592237704
+        },
+        {
+            "x": 3.519890288229749,
+            "y": 2.0816907938233693,
+            "heading": 2.6211478453193005,
+            "angularVelocity": 7.705896648559933e-7,
+            "velocityX": 3.361242105428449,
+            "velocityY": -1.735709814642324,
+            "timestamp": 1.1775027782353655
+        },
+        {
+            "x": 3.726540861270906,
+            "y": 1.9749786271366212,
+            "heading": 2.621147892695476,
+            "angularVelocity": 7.705896659187971e-7,
+            "velocityX": 3.361242105428449,
+            "velocityY": -1.7357098146423233,
+            "timestamp": 1.2389831972469607
+        },
+        {
+            "x": 3.933191434312063,
+            "y": 1.868266460449873,
+            "heading": 2.6211479400716513,
+            "angularVelocity": 7.705896660607392e-7,
+            "velocityX": 3.36124210542845,
+            "velocityY": -1.7357098146423227,
+            "timestamp": 1.3004636162585559
+        },
+        {
+            "x": 4.139842007353606,
+            "y": 1.7615542937638724,
+            "heading": 2.6211479874478267,
+            "angularVelocity": 7.705896681379808e-7,
+            "velocityX": 3.361242105434726,
+            "velocityY": -1.7357098146301677,
+            "timestamp": 1.361944035270151
+        },
+        {
+            "x": 4.3464926090376705,
+            "y": 1.6548421825447925,
+            "heading": 2.621148034824002,
+            "angularVelocity": 7.705896639238046e-7,
+            "velocityX": 3.361242571315101,
+            "velocityY": -1.7357089124417728,
+            "timestamp": 1.4234244542817462
+        },
+        {
+            "x": 4.555173710801168,
+            "y": 1.5521575021408582,
+            "heading": 2.6211480823085673,
+            "angularVelocity": 7.723526622271063e-7,
+            "velocityX": 3.3942693481666213,
+            "velocityY": -1.6702013755724032,
+            "timestamp": 1.4849048732933414
+        },
+        {
+            "x": 4.775821132533127,
+            "y": 1.478627504505032,
+            "heading": 2.621148131521485,
+            "angularVelocity": 8.004649014817169e-7,
+            "velocityX": 3.5889056268524353,
+            "velocityY": -1.1959905091401257,
+            "timestamp": 1.5463852923049366
+        },
+        {
+            "x": 5.004383859827403,
+            "y": 1.4356039834024816,
+            "heading": 2.621148186381714,
+            "angularVelocity": 8.923203458439086e-7,
+            "velocityX": 3.7176507735116338,
+            "velocityY": -0.699792255716998,
+            "timestamp": 1.6078657113165318
+        },
+        {
+            "x": 5.236664772033691,
+            "y": 1.4238771200180054,
+            "heading": 2.6211482490896194,
+            "angularVelocity": 0.0000010199654837334,
+            "velocityX": 3.7781283202133062,
+            "velocityY": -0.1907414356148787,
+            "timestamp": 1.669346130328127
+        },
+        {
+            "x": 5.366075062772529,
+            "y": 1.4271130159839853,
+            "heading": 2.621148315155484,
+            "angularVelocity": 0.0000019306429598932118,
+            "velocityX": 3.781757302483639,
+            "velocityY": 0.09456259722124823,
+            "timestamp": 1.703565748454824
+        },
+        {
+            "x": 5.494873366116125,
+            "y": 1.4400934888882522,
+            "heading": 2.621148371231603,
+            "angularVelocity": 0.00000163871257714578,
+            "velocityX": 3.7638731930533114,
+            "velocityY": 0.3793283974183189,
+            "timestamp": 1.7377853665815208
+        },
+        {
+            "x": 5.622326965929087,
+            "y": 1.4627446781635232,
+            "heading": 2.6211484208273035,
+            "angularVelocity": 0.0000014493353027620846,
+            "velocityX": 3.724576917868275,
+            "velocityY": 0.6619357700429461,
+            "timestamp": 1.7720049847082178
+        },
+        {
+            "x": 5.747710804313215,
+            "y": 1.4949376920061848,
+            "heading": 2.6211484662381075,
+            "angularVelocity": 0.000001327040057798138,
+            "velocityX": 3.6640922736162436,
+            "velocityY": 0.9407765371158774,
+            "timestamp": 1.8062246028349147
+        },
+        {
+            "x": 5.8703116218973515,
+            "y": 1.5364893102203447,
+            "heading": 2.62114850910788,
+            "angularVelocity": 0.000001252783485820186,
+            "velocityX": 3.5827640486872516,
+            "velocityY": 1.2142630598715765,
+            "timestamp": 1.8404442209616116
+        },
+        {
+            "x": 5.989432110510927,
+            "y": 1.5871628422758242,
+            "heading": 2.621148550731445,
+            "angularVelocity": 0.00000121636555358505,
+            "velocityX": 3.481058385062543,
+            "velocityY": 1.4808327745757486,
+            "timestamp": 1.8746638390883086
+        },
+        {
+            "x": 6.104583123843471,
+            "y": 1.6463045521525654,
+            "heading": 2.621148592213924,
+            "angularVelocity": 0.0000012122426079588706,
+            "velocityX": 3.3650583973847925,
+            "velocityY": 1.7282983596652888,
+            "timestamp": 1.9088834572150055
+        },
+        {
+            "x": 6.2197290464685,
+            "y": 1.7054561727682278,
+            "heading": 2.621148633696581,
+            "angularVelocity": 0.0000012122477903456845,
+            "velocityX": 3.3649096316243146,
+            "velocityY": 1.7285879812175449,
+            "timestamp": 1.9431030753417025
+        },
+        {
+            "x": 6.334874968704583,
+            "y": 1.7646077941410194,
+            "heading": 2.6211486751792368,
+            "angularVelocity": 0.0000012122477871677539,
+            "velocityX": 3.364909620258179,
+            "velocityY": 1.7285880033431293,
+            "timestamp": 1.9773226934683994
+        },
+        {
+            "x": 6.450020890940638,
+            "y": 1.8237594155138686,
+            "heading": 2.6211487166618928,
+            "angularVelocity": 0.000001212247782062354,
+            "velocityX": 3.364909620257311,
+            "velocityY": 1.7285880033448198,
+            "timestamp": 2.0115423115950963
+        },
+        {
+            "x": 6.565166813176692,
+            "y": 1.882911036886718,
+            "heading": 2.621148758144549,
+            "angularVelocity": 0.0000012122477787509118,
+            "velocityX": 3.3649096202573117,
+            "velocityY": 1.7285880033448189,
+            "timestamp": 2.0457619297217935
+        },
+        {
+            "x": 6.68031273541274,
+            "y": 1.9420626582595795,
+            "heading": 2.6211487996272047,
+            "angularVelocity": 0.0000012122477734983372,
+            "velocityX": 3.3649096202571287,
+            "velocityY": 1.7285880033451764,
+            "timestamp": 2.0799815478484907
+        },
+        {
+            "x": 6.79545865756654,
+            "y": 2.001214279792547,
+            "heading": 2.6211488411098602,
+            "angularVelocity": 0.0000012122477688304903,
+            "velocityX": 3.3649096178535927,
+            "velocityY": 1.7285880080239562,
+            "timestamp": 2.114201165975188
+        },
+        {
+            "x": 6.910603503099618,
+            "y": 2.0603679970417836,
+            "heading": 2.621148882606884,
+            "angularVelocity": 0.000001212667636144746,
+            "velocityX": 3.3648781557631158,
+            "velocityY": 1.728649251146572,
+            "timestamp": 2.148420784101885
+        },
+        {
+            "x": 7.018224220241307,
+            "y": 2.1177284432893,
+            "heading": 2.639872440534831,
+            "angularVelocity": 0.5471585877616616,
+            "velocityX": 3.1450005299073442,
+            "velocityY": 1.6762444874498934,
+            "timestamp": 2.182640402228582
+        },
+        {
+            "x": 7.119181986421491,
+            "y": 2.170426181119355,
+            "heading": 2.6783971426308466,
+            "angularVelocity": 1.1258074813511751,
+            "velocityX": 2.9502890945887246,
+            "velocityY": 1.5399861458109638,
+            "timestamp": 2.2168600203552793
+        },
+        {
+            "x": 7.213084203441603,
+            "y": 2.2185446189369,
+            "heading": 2.734533820088199,
+            "angularVelocity": 1.6404822885371884,
+            "velocityX": 2.7441047609719855,
+            "velocityY": 1.4061652482324039,
+            "timestamp": 2.2510796384819765
+        },
+        {
+            "x": 7.29840370402033,
+            "y": 2.262230718496702,
+            "heading": 2.7957315713049304,
+            "angularVelocity": 1.788382061721109,
+            "velocityX": 2.493292013453601,
+            "velocityY": 1.2766390144406516,
+            "timestamp": 2.2852992566086736
+        },
+        {
+            "x": 7.375044913403133,
+            "y": 2.301443411516585,
+            "heading": 2.8566177755066406,
+            "angularVelocity": 1.7792777223954108,
+            "velocityX": 2.239686284605582,
+            "velocityY": 1.1459126421194965,
+            "timestamp": 2.3195188747353708
+        },
+        {
+            "x": 7.443040169771806,
+            "y": 2.336203371638747,
+            "heading": 2.9144119075075787,
+            "angularVelocity": 1.6889180874829506,
+            "velocityX": 1.9870255745380796,
+            "velocityY": 1.0157904157043582,
+            "timestamp": 2.353738492862068
+        },
+        {
+            "x": 7.502434435273893,
+            "y": 2.3665382868697598,
+            "heading": 2.967461608910358,
+            "angularVelocity": 1.5502715783199053,
+            "velocityX": 1.7356787934389564,
+            "velocityY": 0.8864773159857819,
+            "timestamp": 2.387958110988765
+        },
+        {
+            "x": 7.553267406161016,
+            "y": 2.3924752684695187,
+            "heading": 3.014662356421872,
+            "angularVelocity": 1.379347581751362,
+            "velocityX": 1.48549205601638,
+            "velocityY": 0.7579564886939567,
+            "timestamp": 2.4221777291154623
+        },
+        {
+            "x": 7.595572130014844,
+            "y": 2.41403971266414,
+            "heading": 3.055206797581657,
+            "angularVelocity": 1.184830321883501,
+            "velocityX": 1.2362710681690243,
+            "velocityY": 0.6301778153917356,
+            "timestamp": 2.4563973472421594
+        },
+        {
+            "x": 7.629376218234675,
+            "y": 2.4312548848965747,
+            "heading": 3.088468435996466,
+            "angularVelocity": 0.9720049560944556,
+            "velocityX": 0.9878569683236498,
+            "velocityY": 0.5030790281965085,
+            "timestamp": 2.4906169653688566
+        },
+        {
+            "x": 7.654702978974042,
+            "y": 2.4441416202766635,
+            "heading": 3.113945152945951,
+            "angularVelocity": 0.7445061734809163,
+            "velocityX": 0.7401240027166469,
+            "velocityY": 0.3765891054767559,
+            "timestamp": 2.5248365834955537
+        },
+        {
+            "x": 7.671572100944035,
+            "y": 2.4527181446432627,
+            "heading": 3.1312307619473794,
+            "angularVelocity": 0.5051374021015854,
+            "velocityX": 0.4929664003711423,
+            "velocityY": 0.250631796498847,
+            "timestamp": 2.559056201622251
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 0.2562634691057309,
+            "velocityX": 0.24628851861412304,
+            "velocityY": 0.1251286715381821,
+            "timestamp": 2.593275819748948
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 3.214441635047838e-33,
+            "velocityX": 1.400444892944232e-33,
+            "velocityY": -3.598995466574735e-33,
+            "timestamp": 2.627495437875645
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.traj
new file mode 100644
index 00000000..0e00ab8a
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.traj	
@@ -0,0 +1,580 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": 2.1,
+            "angularVelocity": 0,
+            "velocityX": 4.814287914507576e-35,
+            "velocityY": 2.4063306961726914e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7189874993597535,
+            "y": 4.351109558323372,
+            "heading": 2.1116856791570084,
+            "angularVelocity": 0.2954657391089134,
+            "velocityX": 0.22724380032956057,
+            "velocityY": -0.22478975211418176,
+            "timestamp": 0.039550031053517704
+        },
+        {
+            "x": 0.7369922825493326,
+            "y": 4.3333146846737405,
+            "heading": 2.134597544316128,
+            "angularVelocity": 0.5793134556106004,
+            "velocityX": 0.45524068401401757,
+            "velocityY": -0.4499332409006739,
+            "timestamp": 0.07910006210703541
+        },
+        {
+            "x": 0.7640484897224145,
+            "y": 4.306598997837255,
+            "heading": 2.168179876708578,
+            "angularVelocity": 0.849110139686295,
+            "velocityX": 0.6841007820315073,
+            "velocityY": -0.67549091934555,
+            "timestamp": 0.11865009316055311
+        },
+        {
+            "x": 0.800195666694539,
+            "y": 4.270941917715144,
+            "heading": 2.2117258914758198,
+            "angularVelocity": 1.1010361713323769,
+            "velocityX": 0.9139607734621368,
+            "velocityY": -0.9015689538615157,
+            "timestamp": 0.15820012421407081
+        },
+        {
+            "x": 0.8454794987248587,
+            "y": 4.226316705338956,
+            "heading": 2.264304991238856,
+            "angularVelocity": 1.3294325784950254,
+            "velocityX": 1.14497589063946,
+            "velocityY": -1.1283230679592324,
+            "timestamp": 0.19775015526758852
+        },
+        {
+            "x": 0.8999520283363762,
+            "y": 4.172689027996317,
+            "heading": 2.324662422471035,
+            "angularVelocity": 1.5261032576815274,
+            "velocityX": 1.3773068733576257,
+            "velocityY": -1.3559452651268766,
+            "timestamp": 0.23730018632110622
+        },
+        {
+            "x": 0.963671089377297,
+            "y": 4.110017781664974,
+            "heading": 2.391063304695866,
+            "angularVelocity": 1.6789084725364454,
+            "velocityX": 1.6111001519745567,
+            "velocityY": -1.584606754076596,
+            "timestamp": 0.2768502173746239
+        },
+        {
+            "x": 1.0366970385232381,
+            "y": 4.038262577868419,
+            "heading": 2.4609901353872035,
+            "angularVelocity": 1.7680600704640437,
+            "velocityX": 1.846419514743869,
+            "velocityY": -1.8142894426418532,
+            "timestamp": 0.31640024842814163
+        },
+        {
+            "x": 1.1190717343496346,
+            "y": 3.9574142704891733,
+            "heading": 2.5303614875414775,
+            "angularVelocity": 1.7540151121601602,
+            "velocityX": 2.082797247742485,
+            "velocityY": -2.0442033855762225,
+            "timestamp": 0.35595027948165936
+        },
+        {
+            "x": 1.21066072775428,
+            "y": 3.867689453216591,
+            "heading": 2.590569114237573,
+            "angularVelocity": 1.522315535343692,
+            "velocityX": 2.3157755117994827,
+            "velocityY": -2.268640880488067,
+            "timestamp": 0.3955003105351771
+        },
+        {
+            "x": 1.310358949370952,
+            "y": 3.7710393719100375,
+            "heading": 2.620822405939027,
+            "angularVelocity": 0.764937242666538,
+            "velocityX": 2.5208127265883533,
+            "velocityY": -2.4437422356450176,
+            "timestamp": 0.4350503415886948
+        },
+        {
+            "x": 1.418053038225142,
+            "y": 3.6673441481868845,
+            "heading": 2.6211471035683176,
+            "angularVelocity": 0.008209794547368329,
+            "velocityX": 2.722983673728656,
+            "velocityY": -2.621874647401313,
+            "timestamp": 0.47460037264221255
+        },
+        {
+            "x": 1.5245836891775304,
+            "y": 3.5622918530292758,
+            "heading": 2.6211471537170588,
+            "angularVelocity": 0.0000012679823435232559,
+            "velocityX": 2.6935668092961826,
+            "velocityY": -2.656187425376634,
+            "timestamp": 0.5141504036957303
+        },
+        {
+            "x": 1.6311142725711645,
+            "y": 3.4572394893622254,
+            "heading": 2.621147203865785,
+            "angularVelocity": 0.0000012679819635220034,
+            "velocityX": 2.6935651011115636,
+            "velocityY": -2.6561891575988192,
+            "timestamp": 0.553700434749248
+        },
+        {
+            "x": 1.7376448559612427,
+            "y": 3.3521871256915694,
+            "heading": 2.621147254014511,
+            "angularVelocity": 0.0000012679819538520794,
+            "velocityX": 2.6935651010216595,
+            "velocityY": -2.656189157689989,
+            "timestamp": 0.5932504658027657
+        },
+        {
+            "x": 1.8441754393513208,
+            "y": 3.2471347620209134,
+            "heading": 2.621147304163236,
+            "angularVelocity": 0.000001267981944384733,
+            "velocityX": 2.693565101021656,
+            "velocityY": -2.656189157689993,
+            "timestamp": 0.6328004968562835
+        },
+        {
+            "x": 1.9507060227413988,
+            "y": 3.142082398350257,
+            "heading": 2.621147354311961,
+            "angularVelocity": 0.0000012679819349678633,
+            "velocityX": 2.6935651010216572,
+            "velocityY": -2.656189157689994,
+            "timestamp": 0.6723505279098012
+        },
+        {
+            "x": 2.057236606131477,
+            "y": 3.037030034679601,
+            "heading": 2.6211474044606855,
+            "angularVelocity": 0.0000012679819252039202,
+            "velocityX": 2.693565101021662,
+            "velocityY": -2.6561891576899894,
+            "timestamp": 0.7119005589633189
+        },
+        {
+            "x": 2.1637671895248896,
+            "y": 2.931977671012326,
+            "heading": 2.6211474546094093,
+            "angularVelocity": 0.0000012679819154308074,
+            "velocityX": 2.69356510110597,
+            "velocityY": -2.656189157604496,
+            "timestamp": 0.7514505900168367
+        },
+        {
+            "x": 2.2702978362708315,
+            "y": 2.8269253715890703,
+            "heading": 2.6211475047581403,
+            "angularVelocity": 0.0000012679820770656017,
+            "velocityX": 2.693566702938569,
+            "velocityY": -2.6561875332310767,
+            "timestamp": 0.7910006210703544
+        },
+        {
+            "x": 2.3779994803856632,
+            "y": 2.7230739385096507,
+            "heading": 2.6211475550861425,
+            "angularVelocity": 0.0000012725148543062649,
+            "velocityX": 2.723174704189075,
+            "velocityY": -2.6258243119680853,
+            "timestamp": 0.8305506521238721
+        },
+        {
+            "x": 2.4943342208862305,
+            "y": 2.6289942264556885,
+            "heading": 2.6211476076664186,
+            "angularVelocity": 0.0000013294623338808287,
+            "velocityX": 2.941457627255649,
+            "velocityY": -2.378751913662396,
+            "timestamp": 0.8701006831773899
+        },
+        {
+            "x": 2.6932881359026166,
+            "y": 2.508539731368457,
+            "heading": 2.6211476558145916,
+            "angularVelocity": 7.831464659210102e-7,
+            "velocityX": 3.236053335597206,
+            "velocityY": -1.9592334766702564,
+            "timestamp": 0.9315811021889849
+        },
+        {
+            "x": 2.899938569108162,
+            "y": 2.401827293887261,
+            "heading": 2.621147703190774,
+            "angularVelocity": 7.705897835721374e-7,
+            "velocityX": 3.361239830954497,
+            "velocityY": -1.7357142192064432,
+            "timestamp": 0.99306152120058
+        },
+        {
+            "x": 3.106589142147435,
+            "y": 2.295115127196865,
+            "heading": 2.6211477505669496,
+            "angularVelocity": 7.705896615935058e-7,
+            "velocityX": 3.361242105397809,
+            "velocityY": -1.7357098147016614,
+            "timestamp": 1.0545419402121752
+        },
+        {
+            "x": 3.313239715188592,
+            "y": 2.188402960510117,
+            "heading": 2.621147797943125,
+            "angularVelocity": 7.705896625567414e-7,
+            "velocityX": 3.361242105428448,
+            "velocityY": -1.735709814642326,
+            "timestamp": 1.1160223592237704
+        },
+        {
+            "x": 3.519890288229749,
+            "y": 2.0816907938233693,
+            "heading": 2.6211478453193005,
+            "angularVelocity": 7.705896648559933e-7,
+            "velocityX": 3.361242105428449,
+            "velocityY": -1.735709814642324,
+            "timestamp": 1.1775027782353655
+        },
+        {
+            "x": 3.726540861270906,
+            "y": 1.9749786271366212,
+            "heading": 2.621147892695476,
+            "angularVelocity": 7.705896659187971e-7,
+            "velocityX": 3.361242105428449,
+            "velocityY": -1.7357098146423233,
+            "timestamp": 1.2389831972469607
+        },
+        {
+            "x": 3.933191434312063,
+            "y": 1.868266460449873,
+            "heading": 2.6211479400716513,
+            "angularVelocity": 7.705896660607392e-7,
+            "velocityX": 3.36124210542845,
+            "velocityY": -1.7357098146423227,
+            "timestamp": 1.3004636162585559
+        },
+        {
+            "x": 4.139842007353606,
+            "y": 1.7615542937638724,
+            "heading": 2.6211479874478267,
+            "angularVelocity": 7.705896681379808e-7,
+            "velocityX": 3.361242105434726,
+            "velocityY": -1.7357098146301677,
+            "timestamp": 1.361944035270151
+        },
+        {
+            "x": 4.3464926090376705,
+            "y": 1.6548421825447925,
+            "heading": 2.621148034824002,
+            "angularVelocity": 7.705896639238046e-7,
+            "velocityX": 3.361242571315101,
+            "velocityY": -1.7357089124417728,
+            "timestamp": 1.4234244542817462
+        },
+        {
+            "x": 4.555173710801168,
+            "y": 1.5521575021408582,
+            "heading": 2.6211480823085673,
+            "angularVelocity": 7.723526622271063e-7,
+            "velocityX": 3.3942693481666213,
+            "velocityY": -1.6702013755724032,
+            "timestamp": 1.4849048732933414
+        },
+        {
+            "x": 4.775821132533127,
+            "y": 1.478627504505032,
+            "heading": 2.621148131521485,
+            "angularVelocity": 8.004649014817169e-7,
+            "velocityX": 3.5889056268524353,
+            "velocityY": -1.1959905091401257,
+            "timestamp": 1.5463852923049366
+        },
+        {
+            "x": 5.004383859827403,
+            "y": 1.4356039834024816,
+            "heading": 2.621148186381714,
+            "angularVelocity": 8.923203458439086e-7,
+            "velocityX": 3.7176507735116338,
+            "velocityY": -0.699792255716998,
+            "timestamp": 1.6078657113165318
+        },
+        {
+            "x": 5.236664772033691,
+            "y": 1.4238771200180054,
+            "heading": 2.6211482490896194,
+            "angularVelocity": 0.0000010199654837334,
+            "velocityX": 3.7781283202133062,
+            "velocityY": -0.1907414356148787,
+            "timestamp": 1.669346130328127
+        },
+        {
+            "x": 5.366075062772529,
+            "y": 1.4271130159839853,
+            "heading": 2.621148315155484,
+            "angularVelocity": 0.0000019306429598932118,
+            "velocityX": 3.781757302483639,
+            "velocityY": 0.09456259722124823,
+            "timestamp": 1.703565748454824
+        },
+        {
+            "x": 5.494873366116125,
+            "y": 1.4400934888882522,
+            "heading": 2.621148371231603,
+            "angularVelocity": 0.00000163871257714578,
+            "velocityX": 3.7638731930533114,
+            "velocityY": 0.3793283974183189,
+            "timestamp": 1.7377853665815208
+        },
+        {
+            "x": 5.622326965929087,
+            "y": 1.4627446781635232,
+            "heading": 2.6211484208273035,
+            "angularVelocity": 0.0000014493353027620846,
+            "velocityX": 3.724576917868275,
+            "velocityY": 0.6619357700429461,
+            "timestamp": 1.7720049847082178
+        },
+        {
+            "x": 5.747710804313215,
+            "y": 1.4949376920061848,
+            "heading": 2.6211484662381075,
+            "angularVelocity": 0.000001327040057798138,
+            "velocityX": 3.6640922736162436,
+            "velocityY": 0.9407765371158774,
+            "timestamp": 1.8062246028349147
+        },
+        {
+            "x": 5.8703116218973515,
+            "y": 1.5364893102203447,
+            "heading": 2.62114850910788,
+            "angularVelocity": 0.000001252783485820186,
+            "velocityX": 3.5827640486872516,
+            "velocityY": 1.2142630598715765,
+            "timestamp": 1.8404442209616116
+        },
+        {
+            "x": 5.989432110510927,
+            "y": 1.5871628422758242,
+            "heading": 2.621148550731445,
+            "angularVelocity": 0.00000121636555358505,
+            "velocityX": 3.481058385062543,
+            "velocityY": 1.4808327745757486,
+            "timestamp": 1.8746638390883086
+        },
+        {
+            "x": 6.104583123843471,
+            "y": 1.6463045521525654,
+            "heading": 2.621148592213924,
+            "angularVelocity": 0.0000012122426079588706,
+            "velocityX": 3.3650583973847925,
+            "velocityY": 1.7282983596652888,
+            "timestamp": 1.9088834572150055
+        },
+        {
+            "x": 6.2197290464685,
+            "y": 1.7054561727682278,
+            "heading": 2.621148633696581,
+            "angularVelocity": 0.0000012122477903456845,
+            "velocityX": 3.3649096316243146,
+            "velocityY": 1.7285879812175449,
+            "timestamp": 1.9431030753417025
+        },
+        {
+            "x": 6.334874968704583,
+            "y": 1.7646077941410194,
+            "heading": 2.6211486751792368,
+            "angularVelocity": 0.0000012122477871677539,
+            "velocityX": 3.364909620258179,
+            "velocityY": 1.7285880033431293,
+            "timestamp": 1.9773226934683994
+        },
+        {
+            "x": 6.450020890940638,
+            "y": 1.8237594155138686,
+            "heading": 2.6211487166618928,
+            "angularVelocity": 0.000001212247782062354,
+            "velocityX": 3.364909620257311,
+            "velocityY": 1.7285880033448198,
+            "timestamp": 2.0115423115950963
+        },
+        {
+            "x": 6.565166813176692,
+            "y": 1.882911036886718,
+            "heading": 2.621148758144549,
+            "angularVelocity": 0.0000012122477787509118,
+            "velocityX": 3.3649096202573117,
+            "velocityY": 1.7285880033448189,
+            "timestamp": 2.0457619297217935
+        },
+        {
+            "x": 6.68031273541274,
+            "y": 1.9420626582595795,
+            "heading": 2.6211487996272047,
+            "angularVelocity": 0.0000012122477734983372,
+            "velocityX": 3.3649096202571287,
+            "velocityY": 1.7285880033451764,
+            "timestamp": 2.0799815478484907
+        },
+        {
+            "x": 6.79545865756654,
+            "y": 2.001214279792547,
+            "heading": 2.6211488411098602,
+            "angularVelocity": 0.0000012122477688304903,
+            "velocityX": 3.3649096178535927,
+            "velocityY": 1.7285880080239562,
+            "timestamp": 2.114201165975188
+        },
+        {
+            "x": 6.910603503099618,
+            "y": 2.0603679970417836,
+            "heading": 2.621148882606884,
+            "angularVelocity": 0.000001212667636144746,
+            "velocityX": 3.3648781557631158,
+            "velocityY": 1.728649251146572,
+            "timestamp": 2.148420784101885
+        },
+        {
+            "x": 7.018224220241307,
+            "y": 2.1177284432893,
+            "heading": 2.639872440534831,
+            "angularVelocity": 0.5471585877616616,
+            "velocityX": 3.1450005299073442,
+            "velocityY": 1.6762444874498934,
+            "timestamp": 2.182640402228582
+        },
+        {
+            "x": 7.119181986421491,
+            "y": 2.170426181119355,
+            "heading": 2.6783971426308466,
+            "angularVelocity": 1.1258074813511751,
+            "velocityX": 2.9502890945887246,
+            "velocityY": 1.5399861458109638,
+            "timestamp": 2.2168600203552793
+        },
+        {
+            "x": 7.213084203441603,
+            "y": 2.2185446189369,
+            "heading": 2.734533820088199,
+            "angularVelocity": 1.6404822885371884,
+            "velocityX": 2.7441047609719855,
+            "velocityY": 1.4061652482324039,
+            "timestamp": 2.2510796384819765
+        },
+        {
+            "x": 7.29840370402033,
+            "y": 2.262230718496702,
+            "heading": 2.7957315713049304,
+            "angularVelocity": 1.788382061721109,
+            "velocityX": 2.493292013453601,
+            "velocityY": 1.2766390144406516,
+            "timestamp": 2.2852992566086736
+        },
+        {
+            "x": 7.375044913403133,
+            "y": 2.301443411516585,
+            "heading": 2.8566177755066406,
+            "angularVelocity": 1.7792777223954108,
+            "velocityX": 2.239686284605582,
+            "velocityY": 1.1459126421194965,
+            "timestamp": 2.3195188747353708
+        },
+        {
+            "x": 7.443040169771806,
+            "y": 2.336203371638747,
+            "heading": 2.9144119075075787,
+            "angularVelocity": 1.6889180874829506,
+            "velocityX": 1.9870255745380796,
+            "velocityY": 1.0157904157043582,
+            "timestamp": 2.353738492862068
+        },
+        {
+            "x": 7.502434435273893,
+            "y": 2.3665382868697598,
+            "heading": 2.967461608910358,
+            "angularVelocity": 1.5502715783199053,
+            "velocityX": 1.7356787934389564,
+            "velocityY": 0.8864773159857819,
+            "timestamp": 2.387958110988765
+        },
+        {
+            "x": 7.553267406161016,
+            "y": 2.3924752684695187,
+            "heading": 3.014662356421872,
+            "angularVelocity": 1.379347581751362,
+            "velocityX": 1.48549205601638,
+            "velocityY": 0.7579564886939567,
+            "timestamp": 2.4221777291154623
+        },
+        {
+            "x": 7.595572130014844,
+            "y": 2.41403971266414,
+            "heading": 3.055206797581657,
+            "angularVelocity": 1.184830321883501,
+            "velocityX": 1.2362710681690243,
+            "velocityY": 0.6301778153917356,
+            "timestamp": 2.4563973472421594
+        },
+        {
+            "x": 7.629376218234675,
+            "y": 2.4312548848965747,
+            "heading": 3.088468435996466,
+            "angularVelocity": 0.9720049560944556,
+            "velocityX": 0.9878569683236498,
+            "velocityY": 0.5030790281965085,
+            "timestamp": 2.4906169653688566
+        },
+        {
+            "x": 7.654702978974042,
+            "y": 2.4441416202766635,
+            "heading": 3.113945152945951,
+            "angularVelocity": 0.7445061734809163,
+            "velocityX": 0.7401240027166469,
+            "velocityY": 0.3765891054767559,
+            "timestamp": 2.5248365834955537
+        },
+        {
+            "x": 7.671572100944035,
+            "y": 2.4527181446432627,
+            "heading": 3.1312307619473794,
+            "angularVelocity": 0.5051374021015854,
+            "velocityX": 0.4929664003711423,
+            "velocityY": 0.250631796498847,
+            "timestamp": 2.559056201622251
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 0.2562634691057309,
+            "velocityX": 0.24628851861412304,
+            "velocityY": 0.1251286715381821,
+            "timestamp": 2.593275819748948
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 3.214441635047838e-33,
+            "velocityX": 1.400444892944232e-33,
+            "velocityY": -3.598995466574735e-33,
+            "timestamp": 2.627495437875645
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.1.traj
new file mode 100644
index 00000000..241d7413
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.1.traj	
@@ -0,0 +1,526 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -1.6129974497596655e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3086679912938655,
+            "y": 6.746243793862482,
+            "heading": -2.736085320412874,
+            "angularVelocity": 0.33564007387910216,
+            "velocityX": 0.20908316429709453,
+            "velocityY": -0.25945360797208683,
+            "timestamp": 0.04145714612176501
+        },
+        {
+            "x": 2.3260411538964876,
+            "y": 6.724713219688379,
+            "heading": -2.708761093447192,
+            "angularVelocity": 0.6590957053683163,
+            "velocityX": 0.4190631586553287,
+            "velocityY": -0.5193453044467995,
+            "timestamp": 0.08291429224353002
+        },
+        {
+            "x": 2.352163012722991,
+            "y": 6.692386750887991,
+            "heading": -2.6686499617636756,
+            "angularVelocity": 0.9675323903315748,
+            "velocityX": 0.6300930302770934,
+            "velocityY": -0.7797562501152615,
+            "timestamp": 0.12437143836529502
+        },
+        {
+            "x": 2.387085245084356,
+            "y": 6.649236203863692,
+            "heading": -2.6165769378043913,
+            "angularVelocity": 1.2560687078251613,
+            "velocityX": 0.8423694254976876,
+            "velocityY": -1.0408470206212592,
+            "timestamp": 0.16582858448706003
+        },
+        {
+            "x": 2.4308688712635975,
+            "y": 6.595222928663028,
+            "heading": -2.553684267520584,
+            "angularVelocity": 1.5170525751841024,
+            "velocityX": 1.056117708890124,
+            "velocityY": -1.3028700779841438,
+            "timestamp": 0.20728573060882505
+        },
+        {
+            "x": 2.4835846779423907,
+            "y": 6.530294843671354,
+            "heading": -2.481592483057959,
+            "angularVelocity": 1.7389471106110828,
+            "velocityX": 1.271573458625441,
+            "velocityY": -1.5661494112733225,
+            "timestamp": 0.24874287673059006
+        },
+        {
+            "x": 2.5453132269093315,
+            "y": 6.454388220972069,
+            "heading": -2.4026634058172087,
+            "angularVelocity": 1.9038714582264231,
+            "velocityX": 1.4889724629292092,
+            "velocityY": -1.830965944359449,
+            "timestamp": 0.2902000228523551
+        },
+        {
+            "x": 2.6161417178055544,
+            "y": 6.3674459995028085,
+            "heading": -2.3205870553837444,
+            "angularVelocity": 1.9797877594467097,
+            "velocityX": 1.708474835392443,
+            "velocityY": -2.0971588640930596,
+            "timestamp": 0.33165716897412006
+        },
+        {
+            "x": 2.696110411470073,
+            "y": 6.269518903939401,
+            "heading": -2.2423584912738157,
+            "angularVelocity": 1.8869741752160536,
+            "velocityX": 1.9289483513804893,
+            "velocityY": -2.3621282390202,
+            "timestamp": 0.37311431509588505
+        },
+        {
+            "x": 2.7844874173560816,
+            "y": 6.1625113811609245,
+            "heading": -2.1922364761983766,
+            "angularVelocity": 1.2090078494121173,
+            "velocityX": 2.131767720489823,
+            "velocityY": -2.581159891329275,
+            "timestamp": 0.41457146121765004
+        },
+        {
+            "x": 2.8809289045046786,
+            "y": 6.046946295339885,
+            "heading": -2.1746156662329965,
+            "angularVelocity": 0.42503673344096826,
+            "velocityX": 2.3262934420361696,
+            "velocityY": -2.7875793833374174,
+            "timestamp": 0.456028607339415
+        },
+        {
+            "x": 2.9795919491542358,
+            "y": 5.925039697257722,
+            "heading": -2.1746156143142836,
+            "angularVelocity": 0.000001252346527187571,
+            "velocityX": 2.3798802831186356,
+            "velocityY": -2.940544863462319,
+            "timestamp": 0.49748575346118
+        },
+        {
+            "x": 3.0782548699283607,
+            "y": 5.803132998918896,
+            "heading": -2.174615562396172,
+            "angularVelocity": 0.0000012523320226734394,
+            "velocityX": 2.37987729508295,
+            "velocityY": -2.940547281782763,
+            "timestamp": 0.5389428995829451
+        },
+        {
+            "x": 3.1769177990519513,
+            "y": 5.6812263073375435,
+            "heading": -2.1746155104780764,
+            "angularVelocity": 0.0000012523316371956637,
+            "velocityX": 2.379877496482862,
+            "velocityY": -2.940547118783714,
+            "timestamp": 0.58040004570471
+        },
+        {
+            "x": 3.2755807374460058,
+            "y": 5.559319623259062,
+            "heading": -2.174615458559997,
+            "angularVelocity": 0.0000012523312531723852,
+            "velocityX": 2.3798777200984556,
+            "velocityY": -2.9405469378047924,
+            "timestamp": 0.621857191826475
+        },
+        {
+            "x": 3.374243686147731,
+            "y": 5.437412947522895,
+            "heading": -2.1746154066419328,
+            "angularVelocity": 0.0000012523308771614933,
+            "velocityX": 2.379877968732799,
+            "velocityY": -2.9405467365773754,
+            "timestamp": 0.66331433794824
+        },
+        {
+            "x": 3.472906646327713,
+            "y": 5.315506281076437,
+            "heading": -2.174615354723884,
+            "angularVelocity": 0.000001252330513608687,
+            "velocityX": 2.3798782456032144,
+            "velocityY": -2.9405465124975687,
+            "timestamp": 0.704771484070005
+        },
+        {
+            "x": 3.5715696192957083,
+            "y": 5.193599624979717,
+            "heading": -2.1746153028058486,
+            "angularVelocity": 0.0000012523301754013102,
+            "velocityX": 2.379878554066626,
+            "velocityY": -2.940546262848488,
+            "timestamp": 0.74622863019177
+        },
+        {
+            "x": 3.6702326064938156,
+            "y": 5.071692980399876,
+            "heading": -2.174615250887826,
+            "angularVelocity": 0.0000012523298755846902,
+            "velocityX": 2.3798788973153537,
+            "velocityY": -2.940545985046467,
+            "timestamp": 0.787685776313535
+        },
+        {
+            "x": 3.7688956094655266,
+            "y": 4.949786348586114,
+            "heading": -2.1746151989698133,
+            "angularVelocity": 0.0000012523296313000306,
+            "velocityX": 2.379879277795066,
+            "velocityY": -2.940545677112118,
+            "timestamp": 0.8291429224353
+        },
+        {
+            "x": 3.8675586330203298,
+            "y": 4.82787973343092,
+            "heading": -2.1746151470518074,
+            "angularVelocity": 0.0000012523294719911187,
+            "velocityX": 2.379879774285891,
+            "velocityY": -2.940545275285949,
+            "timestamp": 0.870600068557065
+        },
+        {
+            "x": 3.9662940139338945,
+            "y": 4.706031714187132,
+            "heading": -2.1746150951231753,
+            "angularVelocity": 0.0000012525857863060412,
+            "velocityX": 2.381625127392215,
+            "velocityY": -2.939131866093794,
+            "timestamp": 0.9120572146788299
+        },
+        {
+            "x": 4.07569184834889,
+            "y": 4.593658547130379,
+            "heading": -2.174615041084707,
+            "angularVelocity": 0.0000013034777659582994,
+            "velocityX": 2.638817300488551,
+            "velocityY": -2.7105861731702134,
+            "timestamp": 0.9535143608005949
+        },
+        {
+            "x": 4.1948903308489776,
+            "y": 4.491740265556163,
+            "heading": -2.17461498286675,
+            "angularVelocity": 0.0000014042924363783103,
+            "velocityX": 2.8752216119746037,
+            "velocityY": -2.458400809232485,
+            "timestamp": 0.9949715069223599
+        },
+        {
+            "x": 4.322894391111494,
+            "y": 4.401128047699223,
+            "heading": -2.1746149179117107,
+            "angularVelocity": 0.00000156679957607106,
+            "velocityX": 3.087623539897125,
+            "velocityY": -2.1856839250535947,
+            "timestamp": 1.036428653044125
+        },
+        {
+            "x": 4.458635270784365,
+            "y": 4.322578536823241,
+            "heading": -2.1746148427021956,
+            "angularVelocity": 0.0000018141508159566887,
+            "velocityX": 3.2742456336522188,
+            "velocityY": -1.8947158264409445,
+            "timestamp": 1.07788579916589
+        },
+        {
+            "x": 4.600979576445416,
+            "y": 4.2567476280747965,
+            "heading": -2.174614751821203,
+            "angularVelocity": 0.000002192167120840163,
+            "velocityX": 3.433528811726906,
+            "velocityY": -1.5879266883226886,
+            "timestamp": 1.1193429452876549
+        },
+        {
+            "x": 4.748738765716553,
+            "y": 4.2041850090026855,
+            "heading": -2.174614640016373,
+            "angularVelocity": 0.000002696877156656919,
+            "velocityX": 3.564142809954843,
+            "velocityY": -1.2678783753644691,
+            "timestamp": 1.1608000914094199
+        },
+        {
+            "x": 4.880414686886391,
+            "y": 4.168830160718546,
+            "heading": -2.174614553815122,
+            "angularVelocity": 0.0000023917765874926725,
+            "velocityX": 3.65353612769536,
+            "velocityY": -0.9809706615128672,
+            "timestamp": 1.1968407699651018
+        },
+        {
+            "x": 5.014481469919241,
+            "y": 4.144038758633067,
+            "heading": -2.1746144885447847,
+            "angularVelocity": 0.0000018110185423275369,
+            "velocityX": 3.7198739980913533,
+            "velocityY": -0.6878727892755006,
+            "timestamp": 1.2328814485207837
+        },
+        {
+            "x": 5.150093083338604,
+            "y": 4.129967234831094,
+            "heading": -2.1746144356224972,
+            "angularVelocity": 0.0000014684043018781928,
+            "velocityX": 3.7627375192130574,
+            "velocityY": -0.3904344858611157,
+            "timestamp": 1.2689221270764657
+        },
+        {
+            "x": 5.286393751264718,
+            "y": 4.1267043465211,
+            "heading": -2.1746143904583044,
+            "angularVelocity": 0.000001253144907070333,
+            "velocityX": 3.7818563187020433,
+            "velocityY": -0.09053348718039304,
+            "timestamp": 1.3049628056321476
+        },
+        {
+            "x": 5.422523381587792,
+            "y": 4.134270240486654,
+            "heading": -2.1746143488300143,
+            "angularVelocity": 0.0000011550362392666718,
+            "velocityX": 3.7771106366034917,
+            "velocityY": 0.20992651272823276,
+            "timestamp": 1.3410034841878296
+        },
+        {
+            "x": 5.558824040120251,
+            "y": 4.131006987177355,
+            "heading": -2.1746143024581284,
+            "angularVelocity": 0.0000012866540842201377,
+            "velocityX": 3.7818560580616727,
+            "velocityY": -0.09054361460641977,
+            "timestamp": 1.3770441627435115
+        },
+        {
+            "x": 5.69443561806916,
+            "y": 4.116935078707767,
+            "heading": -2.1746143409291885,
+            "angularVelocity": -0.000001067434403137199,
+            "velocityX": 3.762736535034787,
+            "velocityY": -0.3904451590124025,
+            "timestamp": 1.4130848412991934
+        },
+        {
+            "x": 5.830736232387668,
+            "y": 4.113670630261612,
+            "heading": -2.1746144425807508,
+            "angularVelocity": -0.000002820467492063081,
+            "velocityX": 3.7818548312825957,
+            "velocityY": -0.09057677538205054,
+            "timestamp": 1.4491255198548754
+        },
+        {
+            "x": 5.967072945403984,
+            "y": 4.1145733334601475,
+            "heading": -2.174614542773063,
+            "angularVelocity": -0.000002779978522071658,
+            "velocityX": 3.7828564411094536,
+            "velocityY": 0.02504678698377033,
+            "timestamp": 1.4851661984105573
+        },
+        {
+            "x": 6.103409655835918,
+            "y": 4.115476426894617,
+            "heading": -2.1746146429653797,
+            "angularVelocity": -0.0000027799786444267726,
+            "velocityX": 3.7828563694020714,
+            "velocityY": 0.025057614636036382,
+            "timestamp": 1.5212068769662392
+        },
+        {
+            "x": 6.239746366170269,
+            "y": 4.1163795350566765,
+            "heading": -2.1746147431577554,
+            "angularVelocity": -0.000002779980281350245,
+            "velocityX": 3.782856366694493,
+            "velocityY": 0.025058023274013706,
+            "timestamp": 1.5572475555219212
+        },
+        {
+            "x": 6.37608307646628,
+            "y": 4.117282649003582,
+            "heading": -2.174614843350174,
+            "angularVelocity": -0.0000027799814898706564,
+            "velocityX": 3.7828563656306846,
+            "velocityY": 0.025058183782793973,
+            "timestamp": 1.5932882340776031
+        },
+        {
+            "x": 6.512419786749522,
+            "y": 4.118185764875291,
+            "heading": -2.174614943542629,
+            "angularVelocity": -0.00000277998246882532,
+            "velocityX": 3.782856365276422,
+            "velocityY": 0.025058237189222135,
+            "timestamp": 1.629328912633285
+        },
+        {
+            "x": 6.648756497843916,
+            "y": 4.11908875827432,
+            "heading": -2.1746150437352676,
+            "angularVelocity": -0.000002779987571113984,
+            "velocityX": 3.782856387782983,
+            "velocityY": 0.02505483901017122,
+            "timestamp": 1.665369591188967
+        },
+        {
+            "x": 6.784343394669118,
+            "y": 4.118261735488966,
+            "heading": -2.17648334857758,
+            "angularVelocity": -0.05183878098815612,
+            "velocityX": 3.762051722076281,
+            "velocityY": -0.02294692604294446,
+            "timestamp": 1.701410269744649
+        },
+        {
+            "x": 6.911008509970103,
+            "y": 4.118512712423575,
+            "heading": -2.2004225516217266,
+            "angularVelocity": -0.6642273121234696,
+            "velocityX": 3.5145041763098894,
+            "velocityY": 0.006963712800855175,
+            "timestamp": 1.7374509483003309
+        },
+        {
+            "x": 7.028751474127586,
+            "y": 4.119997003379487,
+            "heading": -2.2461001757447607,
+            "angularVelocity": -1.267390791559704,
+            "velocityX": 3.2669463749294456,
+            "velocityY": 0.04118376832493557,
+            "timestamp": 1.7734916268560128
+        },
+        {
+            "x": 7.137616008718106,
+            "y": 4.122892854321661,
+            "heading": -2.313255173533344,
+            "angularVelocity": -1.863311138407951,
+            "velocityX": 3.0206016910121933,
+            "velocityY": 0.08034951222410198,
+            "timestamp": 1.8095323054116947
+        },
+        {
+            "x": 7.237654743603063,
+            "y": 4.124394361652757,
+            "heading": -2.4021910734424403,
+            "angularVelocity": -2.467653314897855,
+            "velocityX": 2.775717297619687,
+            "velocityY": 0.04166146119519181,
+            "timestamp": 1.8455729839673767
+        },
+        {
+            "x": 7.328556844177467,
+            "y": 4.1248711523025925,
+            "heading": -2.5119123449992884,
+            "angularVelocity": -3.04437307936175,
+            "velocityX": 2.5222083550386993,
+            "velocityY": 0.013229236211493491,
+            "timestamp": 1.8816136625230586
+        },
+        {
+            "x": 7.408737931676895,
+            "y": 4.125345359362809,
+            "heading": -2.6271223888576807,
+            "angularVelocity": -3.1966668907300306,
+            "velocityX": 2.224738565217367,
+            "velocityY": 0.013157550834778516,
+            "timestamp": 1.9176543410787406
+        },
+        {
+            "x": 7.478113482970281,
+            "y": 4.125802607297277,
+            "heading": -2.739701188642133,
+            "angularVelocity": -3.123659273243732,
+            "velocityX": 1.9249235606428023,
+            "velocityY": 0.012686995716837342,
+            "timestamp": 1.9536950196344225
+        },
+        {
+            "x": 7.536867447368059,
+            "y": 4.126215703233864,
+            "heading": -2.8446379232047505,
+            "angularVelocity": -2.911619280433174,
+            "velocityX": 1.630212491893156,
+            "velocityY": 0.011461935600054532,
+            "timestamp": 1.9897356981901044
+        },
+        {
+            "x": 7.585249192103714,
+            "y": 4.126543507918265,
+            "heading": -2.93809966154793,
+            "angularVelocity": -2.593229153518397,
+            "velocityX": 1.3424204724921138,
+            "velocityY": 0.009095408231394075,
+            "timestamp": 2.025776376745786
+        },
+        {
+            "x": 7.623522580110136,
+            "y": 4.126765972225253,
+            "heading": -3.01693933623456,
+            "angularVelocity": -2.1875191546359196,
+            "velocityX": 1.0619497062823264,
+            "velocityY": 0.006172589304754541,
+            "timestamp": 2.061817055301468
+        },
+        {
+            "x": 7.651933909825069,
+            "y": 4.126896328898384,
+            "heading": -3.078642707410718,
+            "angularVelocity": -1.7120479871328622,
+            "velocityX": 0.7883128413089711,
+            "velocityY": 0.003616931710359869,
+            "timestamp": 2.0978577338571496
+        },
+        {
+            "x": 7.670698144632144,
+            "y": 4.126965664579503,
+            "heading": -3.1212443544252966,
+            "angularVelocity": -1.1820434220948564,
+            "velocityX": 0.5206404418297583,
+            "velocityY": 0.0019238173058132363,
+            "timestamp": 2.1338984124128313
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.6087830094650188,
+            "velocityX": 0.25809323632697007,
+            "velocityY": 0.0009526851844440906,
+            "timestamp": 2.169939090968513
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -3.143185307179586,
+            "angularVelocity": -7.326485384728368e-34,
+            "velocityX": -1.4740993581424338e-35,
+            "velocityY": 9.762457034283277e-35,
+            "timestamp": 2.2059797695241947
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.traj
new file mode 100644
index 00000000..241d7413
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.traj	
@@ -0,0 +1,526 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -1.6129974497596655e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3086679912938655,
+            "y": 6.746243793862482,
+            "heading": -2.736085320412874,
+            "angularVelocity": 0.33564007387910216,
+            "velocityX": 0.20908316429709453,
+            "velocityY": -0.25945360797208683,
+            "timestamp": 0.04145714612176501
+        },
+        {
+            "x": 2.3260411538964876,
+            "y": 6.724713219688379,
+            "heading": -2.708761093447192,
+            "angularVelocity": 0.6590957053683163,
+            "velocityX": 0.4190631586553287,
+            "velocityY": -0.5193453044467995,
+            "timestamp": 0.08291429224353002
+        },
+        {
+            "x": 2.352163012722991,
+            "y": 6.692386750887991,
+            "heading": -2.6686499617636756,
+            "angularVelocity": 0.9675323903315748,
+            "velocityX": 0.6300930302770934,
+            "velocityY": -0.7797562501152615,
+            "timestamp": 0.12437143836529502
+        },
+        {
+            "x": 2.387085245084356,
+            "y": 6.649236203863692,
+            "heading": -2.6165769378043913,
+            "angularVelocity": 1.2560687078251613,
+            "velocityX": 0.8423694254976876,
+            "velocityY": -1.0408470206212592,
+            "timestamp": 0.16582858448706003
+        },
+        {
+            "x": 2.4308688712635975,
+            "y": 6.595222928663028,
+            "heading": -2.553684267520584,
+            "angularVelocity": 1.5170525751841024,
+            "velocityX": 1.056117708890124,
+            "velocityY": -1.3028700779841438,
+            "timestamp": 0.20728573060882505
+        },
+        {
+            "x": 2.4835846779423907,
+            "y": 6.530294843671354,
+            "heading": -2.481592483057959,
+            "angularVelocity": 1.7389471106110828,
+            "velocityX": 1.271573458625441,
+            "velocityY": -1.5661494112733225,
+            "timestamp": 0.24874287673059006
+        },
+        {
+            "x": 2.5453132269093315,
+            "y": 6.454388220972069,
+            "heading": -2.4026634058172087,
+            "angularVelocity": 1.9038714582264231,
+            "velocityX": 1.4889724629292092,
+            "velocityY": -1.830965944359449,
+            "timestamp": 0.2902000228523551
+        },
+        {
+            "x": 2.6161417178055544,
+            "y": 6.3674459995028085,
+            "heading": -2.3205870553837444,
+            "angularVelocity": 1.9797877594467097,
+            "velocityX": 1.708474835392443,
+            "velocityY": -2.0971588640930596,
+            "timestamp": 0.33165716897412006
+        },
+        {
+            "x": 2.696110411470073,
+            "y": 6.269518903939401,
+            "heading": -2.2423584912738157,
+            "angularVelocity": 1.8869741752160536,
+            "velocityX": 1.9289483513804893,
+            "velocityY": -2.3621282390202,
+            "timestamp": 0.37311431509588505
+        },
+        {
+            "x": 2.7844874173560816,
+            "y": 6.1625113811609245,
+            "heading": -2.1922364761983766,
+            "angularVelocity": 1.2090078494121173,
+            "velocityX": 2.131767720489823,
+            "velocityY": -2.581159891329275,
+            "timestamp": 0.41457146121765004
+        },
+        {
+            "x": 2.8809289045046786,
+            "y": 6.046946295339885,
+            "heading": -2.1746156662329965,
+            "angularVelocity": 0.42503673344096826,
+            "velocityX": 2.3262934420361696,
+            "velocityY": -2.7875793833374174,
+            "timestamp": 0.456028607339415
+        },
+        {
+            "x": 2.9795919491542358,
+            "y": 5.925039697257722,
+            "heading": -2.1746156143142836,
+            "angularVelocity": 0.000001252346527187571,
+            "velocityX": 2.3798802831186356,
+            "velocityY": -2.940544863462319,
+            "timestamp": 0.49748575346118
+        },
+        {
+            "x": 3.0782548699283607,
+            "y": 5.803132998918896,
+            "heading": -2.174615562396172,
+            "angularVelocity": 0.0000012523320226734394,
+            "velocityX": 2.37987729508295,
+            "velocityY": -2.940547281782763,
+            "timestamp": 0.5389428995829451
+        },
+        {
+            "x": 3.1769177990519513,
+            "y": 5.6812263073375435,
+            "heading": -2.1746155104780764,
+            "angularVelocity": 0.0000012523316371956637,
+            "velocityX": 2.379877496482862,
+            "velocityY": -2.940547118783714,
+            "timestamp": 0.58040004570471
+        },
+        {
+            "x": 3.2755807374460058,
+            "y": 5.559319623259062,
+            "heading": -2.174615458559997,
+            "angularVelocity": 0.0000012523312531723852,
+            "velocityX": 2.3798777200984556,
+            "velocityY": -2.9405469378047924,
+            "timestamp": 0.621857191826475
+        },
+        {
+            "x": 3.374243686147731,
+            "y": 5.437412947522895,
+            "heading": -2.1746154066419328,
+            "angularVelocity": 0.0000012523308771614933,
+            "velocityX": 2.379877968732799,
+            "velocityY": -2.9405467365773754,
+            "timestamp": 0.66331433794824
+        },
+        {
+            "x": 3.472906646327713,
+            "y": 5.315506281076437,
+            "heading": -2.174615354723884,
+            "angularVelocity": 0.000001252330513608687,
+            "velocityX": 2.3798782456032144,
+            "velocityY": -2.9405465124975687,
+            "timestamp": 0.704771484070005
+        },
+        {
+            "x": 3.5715696192957083,
+            "y": 5.193599624979717,
+            "heading": -2.1746153028058486,
+            "angularVelocity": 0.0000012523301754013102,
+            "velocityX": 2.379878554066626,
+            "velocityY": -2.940546262848488,
+            "timestamp": 0.74622863019177
+        },
+        {
+            "x": 3.6702326064938156,
+            "y": 5.071692980399876,
+            "heading": -2.174615250887826,
+            "angularVelocity": 0.0000012523298755846902,
+            "velocityX": 2.3798788973153537,
+            "velocityY": -2.940545985046467,
+            "timestamp": 0.787685776313535
+        },
+        {
+            "x": 3.7688956094655266,
+            "y": 4.949786348586114,
+            "heading": -2.1746151989698133,
+            "angularVelocity": 0.0000012523296313000306,
+            "velocityX": 2.379879277795066,
+            "velocityY": -2.940545677112118,
+            "timestamp": 0.8291429224353
+        },
+        {
+            "x": 3.8675586330203298,
+            "y": 4.82787973343092,
+            "heading": -2.1746151470518074,
+            "angularVelocity": 0.0000012523294719911187,
+            "velocityX": 2.379879774285891,
+            "velocityY": -2.940545275285949,
+            "timestamp": 0.870600068557065
+        },
+        {
+            "x": 3.9662940139338945,
+            "y": 4.706031714187132,
+            "heading": -2.1746150951231753,
+            "angularVelocity": 0.0000012525857863060412,
+            "velocityX": 2.381625127392215,
+            "velocityY": -2.939131866093794,
+            "timestamp": 0.9120572146788299
+        },
+        {
+            "x": 4.07569184834889,
+            "y": 4.593658547130379,
+            "heading": -2.174615041084707,
+            "angularVelocity": 0.0000013034777659582994,
+            "velocityX": 2.638817300488551,
+            "velocityY": -2.7105861731702134,
+            "timestamp": 0.9535143608005949
+        },
+        {
+            "x": 4.1948903308489776,
+            "y": 4.491740265556163,
+            "heading": -2.17461498286675,
+            "angularVelocity": 0.0000014042924363783103,
+            "velocityX": 2.8752216119746037,
+            "velocityY": -2.458400809232485,
+            "timestamp": 0.9949715069223599
+        },
+        {
+            "x": 4.322894391111494,
+            "y": 4.401128047699223,
+            "heading": -2.1746149179117107,
+            "angularVelocity": 0.00000156679957607106,
+            "velocityX": 3.087623539897125,
+            "velocityY": -2.1856839250535947,
+            "timestamp": 1.036428653044125
+        },
+        {
+            "x": 4.458635270784365,
+            "y": 4.322578536823241,
+            "heading": -2.1746148427021956,
+            "angularVelocity": 0.0000018141508159566887,
+            "velocityX": 3.2742456336522188,
+            "velocityY": -1.8947158264409445,
+            "timestamp": 1.07788579916589
+        },
+        {
+            "x": 4.600979576445416,
+            "y": 4.2567476280747965,
+            "heading": -2.174614751821203,
+            "angularVelocity": 0.000002192167120840163,
+            "velocityX": 3.433528811726906,
+            "velocityY": -1.5879266883226886,
+            "timestamp": 1.1193429452876549
+        },
+        {
+            "x": 4.748738765716553,
+            "y": 4.2041850090026855,
+            "heading": -2.174614640016373,
+            "angularVelocity": 0.000002696877156656919,
+            "velocityX": 3.564142809954843,
+            "velocityY": -1.2678783753644691,
+            "timestamp": 1.1608000914094199
+        },
+        {
+            "x": 4.880414686886391,
+            "y": 4.168830160718546,
+            "heading": -2.174614553815122,
+            "angularVelocity": 0.0000023917765874926725,
+            "velocityX": 3.65353612769536,
+            "velocityY": -0.9809706615128672,
+            "timestamp": 1.1968407699651018
+        },
+        {
+            "x": 5.014481469919241,
+            "y": 4.144038758633067,
+            "heading": -2.1746144885447847,
+            "angularVelocity": 0.0000018110185423275369,
+            "velocityX": 3.7198739980913533,
+            "velocityY": -0.6878727892755006,
+            "timestamp": 1.2328814485207837
+        },
+        {
+            "x": 5.150093083338604,
+            "y": 4.129967234831094,
+            "heading": -2.1746144356224972,
+            "angularVelocity": 0.0000014684043018781928,
+            "velocityX": 3.7627375192130574,
+            "velocityY": -0.3904344858611157,
+            "timestamp": 1.2689221270764657
+        },
+        {
+            "x": 5.286393751264718,
+            "y": 4.1267043465211,
+            "heading": -2.1746143904583044,
+            "angularVelocity": 0.000001253144907070333,
+            "velocityX": 3.7818563187020433,
+            "velocityY": -0.09053348718039304,
+            "timestamp": 1.3049628056321476
+        },
+        {
+            "x": 5.422523381587792,
+            "y": 4.134270240486654,
+            "heading": -2.1746143488300143,
+            "angularVelocity": 0.0000011550362392666718,
+            "velocityX": 3.7771106366034917,
+            "velocityY": 0.20992651272823276,
+            "timestamp": 1.3410034841878296
+        },
+        {
+            "x": 5.558824040120251,
+            "y": 4.131006987177355,
+            "heading": -2.1746143024581284,
+            "angularVelocity": 0.0000012866540842201377,
+            "velocityX": 3.7818560580616727,
+            "velocityY": -0.09054361460641977,
+            "timestamp": 1.3770441627435115
+        },
+        {
+            "x": 5.69443561806916,
+            "y": 4.116935078707767,
+            "heading": -2.1746143409291885,
+            "angularVelocity": -0.000001067434403137199,
+            "velocityX": 3.762736535034787,
+            "velocityY": -0.3904451590124025,
+            "timestamp": 1.4130848412991934
+        },
+        {
+            "x": 5.830736232387668,
+            "y": 4.113670630261612,
+            "heading": -2.1746144425807508,
+            "angularVelocity": -0.000002820467492063081,
+            "velocityX": 3.7818548312825957,
+            "velocityY": -0.09057677538205054,
+            "timestamp": 1.4491255198548754
+        },
+        {
+            "x": 5.967072945403984,
+            "y": 4.1145733334601475,
+            "heading": -2.174614542773063,
+            "angularVelocity": -0.000002779978522071658,
+            "velocityX": 3.7828564411094536,
+            "velocityY": 0.02504678698377033,
+            "timestamp": 1.4851661984105573
+        },
+        {
+            "x": 6.103409655835918,
+            "y": 4.115476426894617,
+            "heading": -2.1746146429653797,
+            "angularVelocity": -0.0000027799786444267726,
+            "velocityX": 3.7828563694020714,
+            "velocityY": 0.025057614636036382,
+            "timestamp": 1.5212068769662392
+        },
+        {
+            "x": 6.239746366170269,
+            "y": 4.1163795350566765,
+            "heading": -2.1746147431577554,
+            "angularVelocity": -0.000002779980281350245,
+            "velocityX": 3.782856366694493,
+            "velocityY": 0.025058023274013706,
+            "timestamp": 1.5572475555219212
+        },
+        {
+            "x": 6.37608307646628,
+            "y": 4.117282649003582,
+            "heading": -2.174614843350174,
+            "angularVelocity": -0.0000027799814898706564,
+            "velocityX": 3.7828563656306846,
+            "velocityY": 0.025058183782793973,
+            "timestamp": 1.5932882340776031
+        },
+        {
+            "x": 6.512419786749522,
+            "y": 4.118185764875291,
+            "heading": -2.174614943542629,
+            "angularVelocity": -0.00000277998246882532,
+            "velocityX": 3.782856365276422,
+            "velocityY": 0.025058237189222135,
+            "timestamp": 1.629328912633285
+        },
+        {
+            "x": 6.648756497843916,
+            "y": 4.11908875827432,
+            "heading": -2.1746150437352676,
+            "angularVelocity": -0.000002779987571113984,
+            "velocityX": 3.782856387782983,
+            "velocityY": 0.02505483901017122,
+            "timestamp": 1.665369591188967
+        },
+        {
+            "x": 6.784343394669118,
+            "y": 4.118261735488966,
+            "heading": -2.17648334857758,
+            "angularVelocity": -0.05183878098815612,
+            "velocityX": 3.762051722076281,
+            "velocityY": -0.02294692604294446,
+            "timestamp": 1.701410269744649
+        },
+        {
+            "x": 6.911008509970103,
+            "y": 4.118512712423575,
+            "heading": -2.2004225516217266,
+            "angularVelocity": -0.6642273121234696,
+            "velocityX": 3.5145041763098894,
+            "velocityY": 0.006963712800855175,
+            "timestamp": 1.7374509483003309
+        },
+        {
+            "x": 7.028751474127586,
+            "y": 4.119997003379487,
+            "heading": -2.2461001757447607,
+            "angularVelocity": -1.267390791559704,
+            "velocityX": 3.2669463749294456,
+            "velocityY": 0.04118376832493557,
+            "timestamp": 1.7734916268560128
+        },
+        {
+            "x": 7.137616008718106,
+            "y": 4.122892854321661,
+            "heading": -2.313255173533344,
+            "angularVelocity": -1.863311138407951,
+            "velocityX": 3.0206016910121933,
+            "velocityY": 0.08034951222410198,
+            "timestamp": 1.8095323054116947
+        },
+        {
+            "x": 7.237654743603063,
+            "y": 4.124394361652757,
+            "heading": -2.4021910734424403,
+            "angularVelocity": -2.467653314897855,
+            "velocityX": 2.775717297619687,
+            "velocityY": 0.04166146119519181,
+            "timestamp": 1.8455729839673767
+        },
+        {
+            "x": 7.328556844177467,
+            "y": 4.1248711523025925,
+            "heading": -2.5119123449992884,
+            "angularVelocity": -3.04437307936175,
+            "velocityX": 2.5222083550386993,
+            "velocityY": 0.013229236211493491,
+            "timestamp": 1.8816136625230586
+        },
+        {
+            "x": 7.408737931676895,
+            "y": 4.125345359362809,
+            "heading": -2.6271223888576807,
+            "angularVelocity": -3.1966668907300306,
+            "velocityX": 2.224738565217367,
+            "velocityY": 0.013157550834778516,
+            "timestamp": 1.9176543410787406
+        },
+        {
+            "x": 7.478113482970281,
+            "y": 4.125802607297277,
+            "heading": -2.739701188642133,
+            "angularVelocity": -3.123659273243732,
+            "velocityX": 1.9249235606428023,
+            "velocityY": 0.012686995716837342,
+            "timestamp": 1.9536950196344225
+        },
+        {
+            "x": 7.536867447368059,
+            "y": 4.126215703233864,
+            "heading": -2.8446379232047505,
+            "angularVelocity": -2.911619280433174,
+            "velocityX": 1.630212491893156,
+            "velocityY": 0.011461935600054532,
+            "timestamp": 1.9897356981901044
+        },
+        {
+            "x": 7.585249192103714,
+            "y": 4.126543507918265,
+            "heading": -2.93809966154793,
+            "angularVelocity": -2.593229153518397,
+            "velocityX": 1.3424204724921138,
+            "velocityY": 0.009095408231394075,
+            "timestamp": 2.025776376745786
+        },
+        {
+            "x": 7.623522580110136,
+            "y": 4.126765972225253,
+            "heading": -3.01693933623456,
+            "angularVelocity": -2.1875191546359196,
+            "velocityX": 1.0619497062823264,
+            "velocityY": 0.006172589304754541,
+            "timestamp": 2.061817055301468
+        },
+        {
+            "x": 7.651933909825069,
+            "y": 4.126896328898384,
+            "heading": -3.078642707410718,
+            "angularVelocity": -1.7120479871328622,
+            "velocityX": 0.7883128413089711,
+            "velocityY": 0.003616931710359869,
+            "timestamp": 2.0978577338571496
+        },
+        {
+            "x": 7.670698144632144,
+            "y": 4.126965664579503,
+            "heading": -3.1212443544252966,
+            "angularVelocity": -1.1820434220948564,
+            "velocityX": 0.5206404418297583,
+            "velocityY": 0.0019238173058132363,
+            "timestamp": 2.1338984124128313
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.6087830094650188,
+            "velocityX": 0.25809323632697007,
+            "velocityY": 0.0009526851844440906,
+            "timestamp": 2.169939090968513
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -3.143185307179586,
+            "angularVelocity": -7.326485384728368e-34,
+            "velocityX": -1.4740993581424338e-35,
+            "velocityY": 9.762457034283277e-35,
+            "timestamp": 2.2059797695241947
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.1.traj
new file mode 100644
index 00000000..5ed5f209
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.1.traj	
@@ -0,0 +1,580 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": -2.2987344072087424e-31,
+            "velocityY": -2.682409549792184e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 2.30727109352441,
+            "y": 6.7499479361039905,
+            "heading": -2.752438811293409,
+            "angularVelocity": -0.06989284029038495,
+            "velocityX": 0.20837913118223148,
+            "velocityY": -0.20210205559294603,
+            "timestamp": 0.03489357827320871
+        },
+        {
+            "x": 2.321814327969628,
+            "y": 6.735842827712052,
+            "heading": -2.7572390739653767,
+            "angularVelocity": -0.1375686561688486,
+            "velocityX": 0.41678827924577844,
+            "velocityY": -0.4042322137758234,
+            "timestamp": 0.06978715654641741
+        },
+        {
+            "x": 2.3436309003249467,
+            "y": 6.714683550648929,
+            "heading": -2.7643090195026687,
+            "angularVelocity": -0.202614517832932,
+            "velocityX": 0.6252317313088365,
+            "velocityY": -0.6063945892120965,
+            "timestamp": 0.10468073481962611
+        },
+        {
+            "x": 2.3727221871027773,
+            "y": 6.6864688072553555,
+            "heading": -2.7735381226601596,
+            "angularVelocity": -0.26449288419860933,
+            "velocityX": 0.8337146322470192,
+            "velocityY": -0.8085941537052478,
+            "timestamp": 0.13957431309283483
+        },
+        {
+            "x": 2.409089780647816,
+            "y": 6.651197089471134,
+            "heading": -2.7847907497118745,
+            "angularVelocity": -0.32248418214977886,
+            "velocityX": 1.042243167504621,
+            "velocityY": -1.0108369370447357,
+            "timestamp": 0.17446789136604354
+        },
+        {
+            "x": 2.452735530672034,
+            "y": 6.6088666355169075,
+            "heading": -2.7978965041034827,
+            "angularVelocity": -0.3755921587918882,
+            "velocityX": 1.2508247128592573,
+            "velocityY": -1.213130210458454,
+            "timestamp": 0.20936146963925226
+        },
+        {
+            "x": 2.5036615834922005,
+            "y": 6.559475386735866,
+            "heading": -2.8126348139869735,
+            "angularVelocity": -0.42237886203854363,
+            "velocityX": 1.459467768579872,
+            "velocityY": -1.4154824820291676,
+            "timestamp": 0.24425504791246097
+        },
+        {
+            "x": 2.5618703896153354,
+            "y": 6.503020972116864,
+            "heading": -2.8287087438800844,
+            "angularVelocity": -0.4606558194535241,
+            "velocityX": 1.6681810523235565,
+            "velocityY": -1.6179027033850466,
+            "timestamp": 0.27914862618566966
+        },
+        {
+            "x": 2.6273645712551357,
+            "y": 6.439500824983286,
+            "heading": -2.8456966886741912,
+            "angularVelocity": -0.48685017802115704,
+            "velocityX": 1.8769694849578602,
+            "velocityY": -1.8203964820182346,
+            "timestamp": 0.31404220445887837
+        },
+        {
+            "x": 2.700146215046118,
+            "y": 6.368912855338095,
+            "heading": -2.8629525699896186,
+            "angularVelocity": -0.4945288551468594,
+            "velocityX": 2.085817717550196,
+            "velocityY": -2.0229501569744945,
+            "timestamp": 0.3489357827320871
+        },
+        {
+            "x": 2.780213462242076,
+            "y": 6.2912587361998735,
+            "heading": -2.8793619429160175,
+            "angularVelocity": -0.47026913657055547,
+            "velocityX": 2.294612681137229,
+            "velocityY": -2.225455885613285,
+            "timestamp": 0.3838293610052958
+        },
+        {
+            "x": 2.8675389350998404,
+            "y": 6.20656386499432,
+            "heading": -2.892555231685043,
+            "angularVelocity": -0.37810076873529364,
+            "velocityX": 2.502623037798732,
+            "velocityY": -2.42723376039027,
+            "timestamp": 0.4187229392785045
+        },
+        {
+            "x": 2.9617036478615395,
+            "y": 6.11507787860357,
+            "heading": -2.8943452360571387,
+            "angularVelocity": -0.05129896275126653,
+            "velocityX": 2.698625862455626,
+            "velocityY": -2.621857399502999,
+            "timestamp": 0.4536165175517132
+        },
+        {
+            "x": 3.05646121242224,
+            "y": 6.023180735189561,
+            "heading": -2.894345248240927,
+            "angularVelocity": -3.4916993473204957e-7,
+            "velocityX": 2.7156161462940758,
+            "velocityY": -2.633640571181149,
+            "timestamp": 0.48851009582492194
+        },
+        {
+            "x": 3.151218783278769,
+            "y": 5.931283598267322,
+            "heading": -2.894345260424686,
+            "angularVelocity": -3.4916908908792776e-7,
+            "velocityX": 2.7156163267235134,
+            "velocityY": -2.633640385136315,
+            "timestamp": 0.5234036740981306
+        },
+        {
+            "x": 3.245976354135778,
+            "y": 5.839386461345578,
+            "heading": -2.894345272608445,
+            "angularVelocity": -3.491690897148369e-7,
+            "velocityX": 2.715616326737254,
+            "velocityY": -2.633640385122147,
+            "timestamp": 0.5582972523713393
+        },
+        {
+            "x": 3.340733924992786,
+            "y": 5.747489324423833,
+            "heading": -2.894345284792204,
+            "angularVelocity": -3.491690888486973e-7,
+            "velocityX": 2.715616326737255,
+            "velocityY": -2.633640385122146,
+            "timestamp": 0.593190830644548
+        },
+        {
+            "x": 3.435491495849795,
+            "y": 5.655592187502088,
+            "heading": -2.894345296975963,
+            "angularVelocity": -3.4916908897748007e-7,
+            "velocityX": 2.715616326737255,
+            "velocityY": -2.633640385122146,
+            "timestamp": 0.6280844089177567
+        },
+        {
+            "x": 3.530249066706803,
+            "y": 5.5636950505803435,
+            "heading": -2.8943453091597218,
+            "angularVelocity": -3.491690889850577e-7,
+            "velocityX": 2.715616326737255,
+            "velocityY": -2.633640385122146,
+            "timestamp": 0.6629779871909655
+        },
+        {
+            "x": 3.625006637563812,
+            "y": 5.471797913658599,
+            "heading": -2.8943453213434807,
+            "angularVelocity": -3.49169089086754e-7,
+            "velocityX": 2.715616326737255,
+            "velocityY": -2.633640385122146,
+            "timestamp": 0.6978715654641742
+        },
+        {
+            "x": 3.7197642084208202,
+            "y": 5.379900776736854,
+            "heading": -2.8943453335272395,
+            "angularVelocity": -3.4916908901998635e-7,
+            "velocityX": 2.71561632673725,
+            "velocityY": -2.6336403851221513,
+            "timestamp": 0.7327651437373829
+        },
+        {
+            "x": 3.814521779275631,
+            "y": 5.288003639812843,
+            "heading": -2.8943453457109984,
+            "angularVelocity": -3.4916908849611257e-7,
+            "velocityX": 2.715616326674264,
+            "velocityY": -2.633640385187098,
+            "timestamp": 0.7676587220105916
+        },
+        {
+            "x": 3.9092793212702794,
+            "y": 5.196106473130367,
+            "heading": -2.8943453578947573,
+            "angularVelocity": -3.4916909136087424e-7,
+            "velocityX": 2.715615499583321,
+            "velocityY": -2.6336412380221117,
+            "timestamp": 0.8025523002838003
+        },
+        {
+            "x": 4.0036585134090545,
+            "y": 5.103820780001642,
+            "heading": -2.8943453700832222,
+            "angularVelocity": -3.49303962122221e-7,
+            "velocityX": 2.704772534355369,
+            "velocityY": -2.644775849761548,
+            "timestamp": 0.837445878557009
+        },
+        {
+            "x": 4.09067331484616,
+            "y": 5.004560993237775,
+            "heading": -2.894345382446594,
+            "angularVelocity": -3.5431651835190834e-7,
+            "velocityX": 2.4937196396368213,
+            "velocityY": -2.8446433892988883,
+            "timestamp": 0.8723394568302177
+        },
+        {
+            "x": 4.169802665710449,
+            "y": 4.898907661437988,
+            "heading": -2.8943453954133593,
+            "angularVelocity": -3.71608932660535e-7,
+            "velocityX": 2.2677339149549134,
+            "velocityY": -3.0278732370909647,
+            "timestamp": 0.9072330351034265
+        },
+        {
+            "x": 4.267711504415231,
+            "y": 4.731769329108682,
+            "heading": -2.8943454067066363,
+            "angularVelocity": -2.2055157567695873e-7,
+            "velocityX": 1.9121064876426594,
+            "velocityY": -3.2641209293092457,
+            "timestamp": 0.9584377349135761
+        },
+        {
+            "x": 4.3838302459464416,
+            "y": 4.576728047661677,
+            "heading": -2.8943454207852617,
+            "angularVelocity": -2.749479155903796e-7,
+            "velocityX": 2.2677360078614184,
+            "velocityY": -3.027872090293428,
+            "timestamp": 1.0096424347237256
+        },
+        {
+            "x": 4.5166798451311,
+            "y": 4.43575875391577,
+            "heading": -2.8943454409364353,
+            "angularVelocity": -3.9354148044055934e-7,
+            "velocityX": 2.594480578486377,
+            "velocityY": -2.7530538069469372,
+            "timestamp": 1.0608471345338752
+        },
+        {
+            "x": 4.664567965806816,
+            "y": 4.3106572284696325,
+            "heading": -2.8943459031991594,
+            "angularVelocity": -0.000009027740145972362,
+            "velocityX": 2.888174742241181,
+            "velocityY": -2.443164903025968,
+            "timestamp": 1.1120518343440249
+        },
+        {
+            "x": 4.823636896940825,
+            "y": 4.2042733124430525,
+            "heading": -2.9000775312757443,
+            "angularVelocity": -0.1119355859488654,
+            "velocityX": 3.1065299029929774,
+            "velocityY": -2.077620148560901,
+            "timestamp": 1.1632565341541745
+        },
+        {
+            "x": 4.99080976041859,
+            "y": 4.118113444137051,
+            "heading": -2.9122463824645286,
+            "angularVelocity": -0.23765106003750752,
+            "velocityX": 3.2647953038995774,
+            "velocityY": -1.6826554715769175,
+            "timestamp": 1.2144612339643241
+        },
+        {
+            "x": 5.163718201552664,
+            "y": 4.0530282335458665,
+            "heading": -2.927811187303136,
+            "angularVelocity": -0.30397219193387703,
+            "velocityX": 3.3768080229971553,
+            "velocityY": -1.2710788430065951,
+            "timestamp": 1.2656659337744738
+        },
+        {
+            "x": 5.340853772484394,
+            "y": 4.009388376225937,
+            "heading": -2.9448561503180763,
+            "angularVelocity": -0.3328788778791379,
+            "velocityX": 3.459361574005724,
+            "velocityY": -0.852262731384641,
+            "timestamp": 1.3168706335846234
+        },
+        {
+            "x": 5.521188530660567,
+            "y": 3.987375039379581,
+            "heading": -2.9620889053684105,
+            "angularVelocity": -0.3365463544211233,
+            "velocityX": 3.5218399647844,
+            "velocityY": -0.42990852261559487,
+            "timestamp": 1.368075333394773
+        },
+        {
+            "x": 5.703982353210449,
+            "y": 3.987083911895752,
+            "heading": -2.978585149802318,
+            "angularVelocity": -0.3221627017650738,
+            "velocityX": 3.569864157540669,
+            "velocityY": -0.0056855617728103595,
+            "timestamp": 1.4192800332049227
+        },
+        {
+            "x": 5.82977348163753,
+            "y": 3.977187586416487,
+            "heading": -2.987958123186903,
+            "angularVelocity": -0.27093528448013743,
+            "velocityX": 3.636119912761043,
+            "velocityY": -0.2860633065962091,
+            "timestamp": 1.4538749122092192
+        },
+        {
+            "x": 5.956799020313408,
+            "y": 3.9574239534344695,
+            "heading": -2.994346009010497,
+            "angularVelocity": -0.18464830655430406,
+            "velocityX": 3.6718017906668177,
+            "velocityY": -0.5712878192047837,
+            "timestamp": 1.4884697912135156
+        },
+        {
+            "x": 6.083502386416879,
+            "y": 3.927762346295825,
+            "heading": -2.9963234364674123,
+            "angularVelocity": -0.057159542505398835,
+            "velocityX": 3.662489066307642,
+            "velocityY": -0.8573987824891895,
+            "timestamp": 1.523064670217812
+        },
+        {
+            "x": 6.208288544878107,
+            "y": 3.888323197622386,
+            "heading": -2.9963235773653807,
+            "angularVelocity": -0.000004072798431777223,
+            "velocityX": 3.6070702385092757,
+            "velocityY": -1.1400285189186714,
+            "timestamp": 1.5576595492221086
+        },
+        {
+            "x": 6.329706848437896,
+            "y": 3.8394904257798843,
+            "heading": -2.9963235979992118,
+            "angularVelocity": -5.964417674236108e-7,
+            "velocityX": 3.5097189831104894,
+            "velocityY": -1.4115607063241151,
+            "timestamp": 1.592254428226405
+        },
+        {
+            "x": 6.447051291726402,
+            "y": 3.7815479813892945,
+            "heading": -2.996323615206392,
+            "angularVelocity": -4.973909609914855e-7,
+            "velocityX": 3.3919599277665977,
+            "velocityY": -1.6748850135707645,
+            "timestamp": 1.6268493072307015
+        },
+        {
+            "x": 6.559639598402797,
+            "y": 3.714832771082226,
+            "heading": -2.996323630138998,
+            "angularVelocity": -4.316421016363579e-7,
+            "velocityX": 3.2544789840835127,
+            "velocityY": -1.9284706935608398,
+            "timestamp": 1.661444186234998
+        },
+        {
+            "x": 6.666817153320071,
+            "y": 3.639732715012558,
+            "heading": -2.996323643537738,
+            "angularVelocity": -3.8730413434828734e-7,
+            "velocityX": 3.0980757268717096,
+            "velocityY": -2.1708431487891944,
+            "timestamp": 1.6960390652392945
+        },
+        {
+            "x": 6.767960818192567,
+            "y": 3.556684498992451,
+            "heading": -2.996323655916169,
+            "angularVelocity": -3.578110804784623e-7,
+            "velocityX": 2.9236600266743116,
+            "velocityY": -2.400592758534959,
+            "timestamp": 1.730633944243591
+        },
+        {
+            "x": 6.862482590576338,
+            "y": 3.466171063796537,
+            "heading": -2.99632366765796,
+            "angularVelocity": -3.394083586636284e-7,
+            "velocityX": 2.7322475205660415,
+            "velocityY": -2.6163824762813186,
+            "timestamp": 1.7652288232478874
+        },
+        {
+            "x": 6.949833264778536,
+            "y": 3.3687189870150447,
+            "heading": -2.9963236790740537,
+            "angularVelocity": -3.299937460810232e-7,
+            "velocityX": 2.524959667913537,
+            "velocityY": -2.8169509356974314,
+            "timestamp": 1.799823702252184
+        },
+        {
+            "x": 7.031655596531464,
+            "y": 3.2665810723307764,
+            "heading": -2.9963236904142594,
+            "angularVelocity": -3.2780012680929783e-7,
+            "velocityX": 2.365157332759168,
+            "velocityY": -2.9523998240197193,
+            "timestamp": 1.8344185812564804
+        },
+        {
+            "x": 7.1134768697956385,
+            "y": 3.164442309703858,
+            "heading": -2.9963237017545317,
+            "angularVelocity": -3.2780205084524347e-7,
+            "velocityX": 2.3651267360701933,
+            "velocityY": -2.952424334660453,
+            "timestamp": 1.8690134602607769
+        },
+        {
+            "x": 7.194171018365882,
+            "y": 3.0635712609157633,
+            "heading": -3.000602693910309,
+            "angularVelocity": -0.1236886001319993,
+            "velocityX": 2.3325460557391247,
+            "velocityY": -2.9157797827698024,
+            "timestamp": 1.9036083392650733
+        },
+        {
+            "x": 7.268926895683992,
+            "y": 2.9702293971794824,
+            "heading": -3.0147524025112875,
+            "angularVelocity": -0.4090116516731078,
+            "velocityX": 2.1608943135435097,
+            "velocityY": -2.698141066621111,
+            "timestamp": 1.9382032182693698
+        },
+        {
+            "x": 7.337464009959094,
+            "y": 2.8846562411973626,
+            "heading": -3.031563935832344,
+            "angularVelocity": -0.4859543899248224,
+            "velocityX": 1.9811346721747336,
+            "velocityY": -2.473578704278531,
+            "timestamp": 1.9727980972736663
+        },
+        {
+            "x": 7.399764505912731,
+            "y": 2.806871719273257,
+            "heading": -3.0489337906921676,
+            "angularVelocity": -0.5020932392238406,
+            "velocityX": 1.8008589059062745,
+            "velocityY": -2.2484403519505154,
+            "timestamp": 2.0073929762779628
+        },
+        {
+            "x": 7.455827645980218,
+            "y": 2.7368758607336336,
+            "heading": -3.0658680766465114,
+            "angularVelocity": -0.48950267905953376,
+            "velocityX": 1.6205618195839024,
+            "velocityY": -2.023301151911255,
+            "timestamp": 2.041987855282259
+        },
+        {
+            "x": 7.505655239118239,
+            "y": 2.674665896616921,
+            "heading": -3.081789323798935,
+            "angularVelocity": -0.4602197669327373,
+            "velocityX": 1.4403170229857634,
+            "velocityY": -1.7982419915094008,
+            "timestamp": 2.0765827342865553
+        },
+        {
+            "x": 7.549249359478674,
+            "y": 2.6202389173200062,
+            "heading": -3.0963205890594514,
+            "angularVelocity": -0.42004093318875263,
+            "velocityX": 1.2601321818475304,
+            "velocityY": -1.5732669361310743,
+            "timestamp": 2.1111776132908515
+        },
+        {
+            "x": 7.586611891972334,
+            "y": 2.573592354245129,
+            "heading": -3.1091966615790723,
+            "angularVelocity": -0.37219591136657937,
+            "velocityX": 1.0800018259644524,
+            "velocityY": -1.348366128671367,
+            "timestamp": 2.1457724922951478
+        },
+        {
+            "x": 7.617744461907291,
+            "y": 2.5347240241708393,
+            "heading": -3.1202208856007014,
+            "angularVelocity": -0.318666355799661,
+            "velocityX": 0.8999184512566408,
+            "velocityY": -1.1235284294378556,
+            "timestamp": 2.180367371299444
+        },
+        {
+            "x": 7.642648454713618,
+            "y": 2.5036320816555615,
+            "heading": -3.1292416557703198,
+            "angularVelocity": -0.26075449399601563,
+            "velocityX": 0.7198751238076218,
+            "velocityY": -0.8987440745613371,
+            "timestamp": 2.2149622503037403
+        },
+        {
+            "x": 7.66132505338278,
+            "y": 2.48031495979902,
+            "heading": -3.136138533441162,
+            "angularVelocity": -0.19936123118063845,
+            "velocityX": 0.5398659919244735,
+            "velocityY": -0.6740050125235452,
+            "timestamp": 2.2495571293080365
+        },
+        {
+            "x": 7.673775274710525,
+            "y": 2.4647713180548445,
+            "heading": -3.1408135216098865,
+            "angularVelocity": -0.1351352657757358,
+            "velocityX": 0.3598862515518098,
+            "velocityY": -0.4493047003357149,
+            "timestamp": 2.284152008312333
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.06855886298677888,
+            "velocityX": 0.17993198613883732,
+            "velocityY": -0.22463781572640673,
+            "timestamp": 2.318746887316629
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -3.143185307179586,
+            "angularVelocity": -2.3105673781771525e-33,
+            "velocityX": 0,
+            "velocityY": -9.473748472540581e-34,
+            "timestamp": 2.3533417663209253
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.traj
new file mode 100644
index 00000000..5ed5f209
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.traj	
@@ -0,0 +1,580 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": -2.2987344072087424e-31,
+            "velocityY": -2.682409549792184e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 2.30727109352441,
+            "y": 6.7499479361039905,
+            "heading": -2.752438811293409,
+            "angularVelocity": -0.06989284029038495,
+            "velocityX": 0.20837913118223148,
+            "velocityY": -0.20210205559294603,
+            "timestamp": 0.03489357827320871
+        },
+        {
+            "x": 2.321814327969628,
+            "y": 6.735842827712052,
+            "heading": -2.7572390739653767,
+            "angularVelocity": -0.1375686561688486,
+            "velocityX": 0.41678827924577844,
+            "velocityY": -0.4042322137758234,
+            "timestamp": 0.06978715654641741
+        },
+        {
+            "x": 2.3436309003249467,
+            "y": 6.714683550648929,
+            "heading": -2.7643090195026687,
+            "angularVelocity": -0.202614517832932,
+            "velocityX": 0.6252317313088365,
+            "velocityY": -0.6063945892120965,
+            "timestamp": 0.10468073481962611
+        },
+        {
+            "x": 2.3727221871027773,
+            "y": 6.6864688072553555,
+            "heading": -2.7735381226601596,
+            "angularVelocity": -0.26449288419860933,
+            "velocityX": 0.8337146322470192,
+            "velocityY": -0.8085941537052478,
+            "timestamp": 0.13957431309283483
+        },
+        {
+            "x": 2.409089780647816,
+            "y": 6.651197089471134,
+            "heading": -2.7847907497118745,
+            "angularVelocity": -0.32248418214977886,
+            "velocityX": 1.042243167504621,
+            "velocityY": -1.0108369370447357,
+            "timestamp": 0.17446789136604354
+        },
+        {
+            "x": 2.452735530672034,
+            "y": 6.6088666355169075,
+            "heading": -2.7978965041034827,
+            "angularVelocity": -0.3755921587918882,
+            "velocityX": 1.2508247128592573,
+            "velocityY": -1.213130210458454,
+            "timestamp": 0.20936146963925226
+        },
+        {
+            "x": 2.5036615834922005,
+            "y": 6.559475386735866,
+            "heading": -2.8126348139869735,
+            "angularVelocity": -0.42237886203854363,
+            "velocityX": 1.459467768579872,
+            "velocityY": -1.4154824820291676,
+            "timestamp": 0.24425504791246097
+        },
+        {
+            "x": 2.5618703896153354,
+            "y": 6.503020972116864,
+            "heading": -2.8287087438800844,
+            "angularVelocity": -0.4606558194535241,
+            "velocityX": 1.6681810523235565,
+            "velocityY": -1.6179027033850466,
+            "timestamp": 0.27914862618566966
+        },
+        {
+            "x": 2.6273645712551357,
+            "y": 6.439500824983286,
+            "heading": -2.8456966886741912,
+            "angularVelocity": -0.48685017802115704,
+            "velocityX": 1.8769694849578602,
+            "velocityY": -1.8203964820182346,
+            "timestamp": 0.31404220445887837
+        },
+        {
+            "x": 2.700146215046118,
+            "y": 6.368912855338095,
+            "heading": -2.8629525699896186,
+            "angularVelocity": -0.4945288551468594,
+            "velocityX": 2.085817717550196,
+            "velocityY": -2.0229501569744945,
+            "timestamp": 0.3489357827320871
+        },
+        {
+            "x": 2.780213462242076,
+            "y": 6.2912587361998735,
+            "heading": -2.8793619429160175,
+            "angularVelocity": -0.47026913657055547,
+            "velocityX": 2.294612681137229,
+            "velocityY": -2.225455885613285,
+            "timestamp": 0.3838293610052958
+        },
+        {
+            "x": 2.8675389350998404,
+            "y": 6.20656386499432,
+            "heading": -2.892555231685043,
+            "angularVelocity": -0.37810076873529364,
+            "velocityX": 2.502623037798732,
+            "velocityY": -2.42723376039027,
+            "timestamp": 0.4187229392785045
+        },
+        {
+            "x": 2.9617036478615395,
+            "y": 6.11507787860357,
+            "heading": -2.8943452360571387,
+            "angularVelocity": -0.05129896275126653,
+            "velocityX": 2.698625862455626,
+            "velocityY": -2.621857399502999,
+            "timestamp": 0.4536165175517132
+        },
+        {
+            "x": 3.05646121242224,
+            "y": 6.023180735189561,
+            "heading": -2.894345248240927,
+            "angularVelocity": -3.4916993473204957e-7,
+            "velocityX": 2.7156161462940758,
+            "velocityY": -2.633640571181149,
+            "timestamp": 0.48851009582492194
+        },
+        {
+            "x": 3.151218783278769,
+            "y": 5.931283598267322,
+            "heading": -2.894345260424686,
+            "angularVelocity": -3.4916908908792776e-7,
+            "velocityX": 2.7156163267235134,
+            "velocityY": -2.633640385136315,
+            "timestamp": 0.5234036740981306
+        },
+        {
+            "x": 3.245976354135778,
+            "y": 5.839386461345578,
+            "heading": -2.894345272608445,
+            "angularVelocity": -3.491690897148369e-7,
+            "velocityX": 2.715616326737254,
+            "velocityY": -2.633640385122147,
+            "timestamp": 0.5582972523713393
+        },
+        {
+            "x": 3.340733924992786,
+            "y": 5.747489324423833,
+            "heading": -2.894345284792204,
+            "angularVelocity": -3.491690888486973e-7,
+            "velocityX": 2.715616326737255,
+            "velocityY": -2.633640385122146,
+            "timestamp": 0.593190830644548
+        },
+        {
+            "x": 3.435491495849795,
+            "y": 5.655592187502088,
+            "heading": -2.894345296975963,
+            "angularVelocity": -3.4916908897748007e-7,
+            "velocityX": 2.715616326737255,
+            "velocityY": -2.633640385122146,
+            "timestamp": 0.6280844089177567
+        },
+        {
+            "x": 3.530249066706803,
+            "y": 5.5636950505803435,
+            "heading": -2.8943453091597218,
+            "angularVelocity": -3.491690889850577e-7,
+            "velocityX": 2.715616326737255,
+            "velocityY": -2.633640385122146,
+            "timestamp": 0.6629779871909655
+        },
+        {
+            "x": 3.625006637563812,
+            "y": 5.471797913658599,
+            "heading": -2.8943453213434807,
+            "angularVelocity": -3.49169089086754e-7,
+            "velocityX": 2.715616326737255,
+            "velocityY": -2.633640385122146,
+            "timestamp": 0.6978715654641742
+        },
+        {
+            "x": 3.7197642084208202,
+            "y": 5.379900776736854,
+            "heading": -2.8943453335272395,
+            "angularVelocity": -3.4916908901998635e-7,
+            "velocityX": 2.71561632673725,
+            "velocityY": -2.6336403851221513,
+            "timestamp": 0.7327651437373829
+        },
+        {
+            "x": 3.814521779275631,
+            "y": 5.288003639812843,
+            "heading": -2.8943453457109984,
+            "angularVelocity": -3.4916908849611257e-7,
+            "velocityX": 2.715616326674264,
+            "velocityY": -2.633640385187098,
+            "timestamp": 0.7676587220105916
+        },
+        {
+            "x": 3.9092793212702794,
+            "y": 5.196106473130367,
+            "heading": -2.8943453578947573,
+            "angularVelocity": -3.4916909136087424e-7,
+            "velocityX": 2.715615499583321,
+            "velocityY": -2.6336412380221117,
+            "timestamp": 0.8025523002838003
+        },
+        {
+            "x": 4.0036585134090545,
+            "y": 5.103820780001642,
+            "heading": -2.8943453700832222,
+            "angularVelocity": -3.49303962122221e-7,
+            "velocityX": 2.704772534355369,
+            "velocityY": -2.644775849761548,
+            "timestamp": 0.837445878557009
+        },
+        {
+            "x": 4.09067331484616,
+            "y": 5.004560993237775,
+            "heading": -2.894345382446594,
+            "angularVelocity": -3.5431651835190834e-7,
+            "velocityX": 2.4937196396368213,
+            "velocityY": -2.8446433892988883,
+            "timestamp": 0.8723394568302177
+        },
+        {
+            "x": 4.169802665710449,
+            "y": 4.898907661437988,
+            "heading": -2.8943453954133593,
+            "angularVelocity": -3.71608932660535e-7,
+            "velocityX": 2.2677339149549134,
+            "velocityY": -3.0278732370909647,
+            "timestamp": 0.9072330351034265
+        },
+        {
+            "x": 4.267711504415231,
+            "y": 4.731769329108682,
+            "heading": -2.8943454067066363,
+            "angularVelocity": -2.2055157567695873e-7,
+            "velocityX": 1.9121064876426594,
+            "velocityY": -3.2641209293092457,
+            "timestamp": 0.9584377349135761
+        },
+        {
+            "x": 4.3838302459464416,
+            "y": 4.576728047661677,
+            "heading": -2.8943454207852617,
+            "angularVelocity": -2.749479155903796e-7,
+            "velocityX": 2.2677360078614184,
+            "velocityY": -3.027872090293428,
+            "timestamp": 1.0096424347237256
+        },
+        {
+            "x": 4.5166798451311,
+            "y": 4.43575875391577,
+            "heading": -2.8943454409364353,
+            "angularVelocity": -3.9354148044055934e-7,
+            "velocityX": 2.594480578486377,
+            "velocityY": -2.7530538069469372,
+            "timestamp": 1.0608471345338752
+        },
+        {
+            "x": 4.664567965806816,
+            "y": 4.3106572284696325,
+            "heading": -2.8943459031991594,
+            "angularVelocity": -0.000009027740145972362,
+            "velocityX": 2.888174742241181,
+            "velocityY": -2.443164903025968,
+            "timestamp": 1.1120518343440249
+        },
+        {
+            "x": 4.823636896940825,
+            "y": 4.2042733124430525,
+            "heading": -2.9000775312757443,
+            "angularVelocity": -0.1119355859488654,
+            "velocityX": 3.1065299029929774,
+            "velocityY": -2.077620148560901,
+            "timestamp": 1.1632565341541745
+        },
+        {
+            "x": 4.99080976041859,
+            "y": 4.118113444137051,
+            "heading": -2.9122463824645286,
+            "angularVelocity": -0.23765106003750752,
+            "velocityX": 3.2647953038995774,
+            "velocityY": -1.6826554715769175,
+            "timestamp": 1.2144612339643241
+        },
+        {
+            "x": 5.163718201552664,
+            "y": 4.0530282335458665,
+            "heading": -2.927811187303136,
+            "angularVelocity": -0.30397219193387703,
+            "velocityX": 3.3768080229971553,
+            "velocityY": -1.2710788430065951,
+            "timestamp": 1.2656659337744738
+        },
+        {
+            "x": 5.340853772484394,
+            "y": 4.009388376225937,
+            "heading": -2.9448561503180763,
+            "angularVelocity": -0.3328788778791379,
+            "velocityX": 3.459361574005724,
+            "velocityY": -0.852262731384641,
+            "timestamp": 1.3168706335846234
+        },
+        {
+            "x": 5.521188530660567,
+            "y": 3.987375039379581,
+            "heading": -2.9620889053684105,
+            "angularVelocity": -0.3365463544211233,
+            "velocityX": 3.5218399647844,
+            "velocityY": -0.42990852261559487,
+            "timestamp": 1.368075333394773
+        },
+        {
+            "x": 5.703982353210449,
+            "y": 3.987083911895752,
+            "heading": -2.978585149802318,
+            "angularVelocity": -0.3221627017650738,
+            "velocityX": 3.569864157540669,
+            "velocityY": -0.0056855617728103595,
+            "timestamp": 1.4192800332049227
+        },
+        {
+            "x": 5.82977348163753,
+            "y": 3.977187586416487,
+            "heading": -2.987958123186903,
+            "angularVelocity": -0.27093528448013743,
+            "velocityX": 3.636119912761043,
+            "velocityY": -0.2860633065962091,
+            "timestamp": 1.4538749122092192
+        },
+        {
+            "x": 5.956799020313408,
+            "y": 3.9574239534344695,
+            "heading": -2.994346009010497,
+            "angularVelocity": -0.18464830655430406,
+            "velocityX": 3.6718017906668177,
+            "velocityY": -0.5712878192047837,
+            "timestamp": 1.4884697912135156
+        },
+        {
+            "x": 6.083502386416879,
+            "y": 3.927762346295825,
+            "heading": -2.9963234364674123,
+            "angularVelocity": -0.057159542505398835,
+            "velocityX": 3.662489066307642,
+            "velocityY": -0.8573987824891895,
+            "timestamp": 1.523064670217812
+        },
+        {
+            "x": 6.208288544878107,
+            "y": 3.888323197622386,
+            "heading": -2.9963235773653807,
+            "angularVelocity": -0.000004072798431777223,
+            "velocityX": 3.6070702385092757,
+            "velocityY": -1.1400285189186714,
+            "timestamp": 1.5576595492221086
+        },
+        {
+            "x": 6.329706848437896,
+            "y": 3.8394904257798843,
+            "heading": -2.9963235979992118,
+            "angularVelocity": -5.964417674236108e-7,
+            "velocityX": 3.5097189831104894,
+            "velocityY": -1.4115607063241151,
+            "timestamp": 1.592254428226405
+        },
+        {
+            "x": 6.447051291726402,
+            "y": 3.7815479813892945,
+            "heading": -2.996323615206392,
+            "angularVelocity": -4.973909609914855e-7,
+            "velocityX": 3.3919599277665977,
+            "velocityY": -1.6748850135707645,
+            "timestamp": 1.6268493072307015
+        },
+        {
+            "x": 6.559639598402797,
+            "y": 3.714832771082226,
+            "heading": -2.996323630138998,
+            "angularVelocity": -4.316421016363579e-7,
+            "velocityX": 3.2544789840835127,
+            "velocityY": -1.9284706935608398,
+            "timestamp": 1.661444186234998
+        },
+        {
+            "x": 6.666817153320071,
+            "y": 3.639732715012558,
+            "heading": -2.996323643537738,
+            "angularVelocity": -3.8730413434828734e-7,
+            "velocityX": 3.0980757268717096,
+            "velocityY": -2.1708431487891944,
+            "timestamp": 1.6960390652392945
+        },
+        {
+            "x": 6.767960818192567,
+            "y": 3.556684498992451,
+            "heading": -2.996323655916169,
+            "angularVelocity": -3.578110804784623e-7,
+            "velocityX": 2.9236600266743116,
+            "velocityY": -2.400592758534959,
+            "timestamp": 1.730633944243591
+        },
+        {
+            "x": 6.862482590576338,
+            "y": 3.466171063796537,
+            "heading": -2.99632366765796,
+            "angularVelocity": -3.394083586636284e-7,
+            "velocityX": 2.7322475205660415,
+            "velocityY": -2.6163824762813186,
+            "timestamp": 1.7652288232478874
+        },
+        {
+            "x": 6.949833264778536,
+            "y": 3.3687189870150447,
+            "heading": -2.9963236790740537,
+            "angularVelocity": -3.299937460810232e-7,
+            "velocityX": 2.524959667913537,
+            "velocityY": -2.8169509356974314,
+            "timestamp": 1.799823702252184
+        },
+        {
+            "x": 7.031655596531464,
+            "y": 3.2665810723307764,
+            "heading": -2.9963236904142594,
+            "angularVelocity": -3.2780012680929783e-7,
+            "velocityX": 2.365157332759168,
+            "velocityY": -2.9523998240197193,
+            "timestamp": 1.8344185812564804
+        },
+        {
+            "x": 7.1134768697956385,
+            "y": 3.164442309703858,
+            "heading": -2.9963237017545317,
+            "angularVelocity": -3.2780205084524347e-7,
+            "velocityX": 2.3651267360701933,
+            "velocityY": -2.952424334660453,
+            "timestamp": 1.8690134602607769
+        },
+        {
+            "x": 7.194171018365882,
+            "y": 3.0635712609157633,
+            "heading": -3.000602693910309,
+            "angularVelocity": -0.1236886001319993,
+            "velocityX": 2.3325460557391247,
+            "velocityY": -2.9157797827698024,
+            "timestamp": 1.9036083392650733
+        },
+        {
+            "x": 7.268926895683992,
+            "y": 2.9702293971794824,
+            "heading": -3.0147524025112875,
+            "angularVelocity": -0.4090116516731078,
+            "velocityX": 2.1608943135435097,
+            "velocityY": -2.698141066621111,
+            "timestamp": 1.9382032182693698
+        },
+        {
+            "x": 7.337464009959094,
+            "y": 2.8846562411973626,
+            "heading": -3.031563935832344,
+            "angularVelocity": -0.4859543899248224,
+            "velocityX": 1.9811346721747336,
+            "velocityY": -2.473578704278531,
+            "timestamp": 1.9727980972736663
+        },
+        {
+            "x": 7.399764505912731,
+            "y": 2.806871719273257,
+            "heading": -3.0489337906921676,
+            "angularVelocity": -0.5020932392238406,
+            "velocityX": 1.8008589059062745,
+            "velocityY": -2.2484403519505154,
+            "timestamp": 2.0073929762779628
+        },
+        {
+            "x": 7.455827645980218,
+            "y": 2.7368758607336336,
+            "heading": -3.0658680766465114,
+            "angularVelocity": -0.48950267905953376,
+            "velocityX": 1.6205618195839024,
+            "velocityY": -2.023301151911255,
+            "timestamp": 2.041987855282259
+        },
+        {
+            "x": 7.505655239118239,
+            "y": 2.674665896616921,
+            "heading": -3.081789323798935,
+            "angularVelocity": -0.4602197669327373,
+            "velocityX": 1.4403170229857634,
+            "velocityY": -1.7982419915094008,
+            "timestamp": 2.0765827342865553
+        },
+        {
+            "x": 7.549249359478674,
+            "y": 2.6202389173200062,
+            "heading": -3.0963205890594514,
+            "angularVelocity": -0.42004093318875263,
+            "velocityX": 1.2601321818475304,
+            "velocityY": -1.5732669361310743,
+            "timestamp": 2.1111776132908515
+        },
+        {
+            "x": 7.586611891972334,
+            "y": 2.573592354245129,
+            "heading": -3.1091966615790723,
+            "angularVelocity": -0.37219591136657937,
+            "velocityX": 1.0800018259644524,
+            "velocityY": -1.348366128671367,
+            "timestamp": 2.1457724922951478
+        },
+        {
+            "x": 7.617744461907291,
+            "y": 2.5347240241708393,
+            "heading": -3.1202208856007014,
+            "angularVelocity": -0.318666355799661,
+            "velocityX": 0.8999184512566408,
+            "velocityY": -1.1235284294378556,
+            "timestamp": 2.180367371299444
+        },
+        {
+            "x": 7.642648454713618,
+            "y": 2.5036320816555615,
+            "heading": -3.1292416557703198,
+            "angularVelocity": -0.26075449399601563,
+            "velocityX": 0.7198751238076218,
+            "velocityY": -0.8987440745613371,
+            "timestamp": 2.2149622503037403
+        },
+        {
+            "x": 7.66132505338278,
+            "y": 2.48031495979902,
+            "heading": -3.136138533441162,
+            "angularVelocity": -0.19936123118063845,
+            "velocityX": 0.5398659919244735,
+            "velocityY": -0.6740050125235452,
+            "timestamp": 2.2495571293080365
+        },
+        {
+            "x": 7.673775274710525,
+            "y": 2.4647713180548445,
+            "heading": -3.1408135216098865,
+            "angularVelocity": -0.1351352657757358,
+            "velocityX": 0.3598862515518098,
+            "velocityY": -0.4493047003357149,
+            "timestamp": 2.284152008312333
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.06855886298677888,
+            "velocityX": 0.17993198613883732,
+            "velocityY": -0.22463781572640673,
+            "timestamp": 2.318746887316629
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -3.143185307179586,
+            "angularVelocity": -2.3105673781771525e-33,
+            "velocityX": 0,
+            "velocityY": -9.473748472540581e-34,
+            "timestamp": 2.3533417663209253
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.1.traj
new file mode 100644
index 00000000..998ccf57
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.1.traj	
@@ -0,0 +1,616 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -2.713469488843566e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2985074122681866,
+            "y": 6.738728579173207,
+            "heading": -2.7557122969387127,
+            "angularVelocity": -0.12159424626473993,
+            "velocityX": -0.03177182176995015,
+            "velocityY": -0.38893280014262377,
+            "timestamp": 0.046978349010656624
+        },
+        {
+            "x": 2.2955230171267877,
+            "y": 6.702182881977796,
+            "heading": -2.7670222648124514,
+            "angularVelocity": -0.24074851738984976,
+            "velocityX": -0.06352703328764883,
+            "velocityY": -0.777926384495132,
+            "timestamp": 0.09395669802131325
+        },
+        {
+            "x": 2.2910478333318625,
+            "y": 6.647359316893119,
+            "heading": -2.7837824265023454,
+            "angularVelocity": -0.3567635313470153,
+            "velocityX": -0.09526055915480924,
+            "velocityY": -1.1669964193982714,
+            "timestamp": 0.14093504703196988
+        },
+        {
+            "x": 2.2850832407725172,
+            "y": 6.574253215154707,
+            "heading": -2.805794841014387,
+            "angularVelocity": -0.46856509382754397,
+            "velocityX": -0.12696471214840174,
+            "velocityY": -1.556165835496437,
+            "timestamp": 0.1879133960426265
+        },
+        {
+            "x": 2.2776312180066953,
+            "y": 6.482858263005064,
+            "heading": -2.832779187520957,
+            "angularVelocity": -0.5743996346156903,
+            "velocityX": -0.15862674876316962,
+            "velocityY": -1.945469648771827,
+            "timestamp": 0.2348917450532831
+        },
+        {
+            "x": 2.2686948542910046,
+            "y": 6.373165502071328,
+            "heading": -2.864308391554441,
+            "angularVelocity": -0.6711432968053561,
+            "velocityX": -0.1902230262213726,
+            "velocityY": -2.3349641535689947,
+            "timestamp": 0.28187009406393976
+        },
+        {
+            "x": 2.2582796451376828,
+            "y": 6.245161498589565,
+            "heading": -2.8996563968568023,
+            "angularVelocity": -0.7524318339570105,
+            "velocityX": -0.22170232399947937,
+            "velocityY": -2.7247446148592136,
+            "timestamp": 0.3288484430745964
+        },
+        {
+            "x": 2.2463978083089957,
+            "y": 6.098825641295113,
+            "heading": -2.9373287044127423,
+            "angularVelocity": -0.8019078649910463,
+            "velocityX": -0.2529215495842953,
+            "velocityY": -3.1149638158049613,
+            "timestamp": 0.375826792085253
+        },
+        {
+            "x": 2.23309481763283,
+            "y": 5.934156164582871,
+            "heading": -2.9725027104068498,
+            "angularVelocity": -0.7487280148165356,
+            "velocityX": -0.2831728010098399,
+            "velocityY": -3.505220600129828,
+            "timestamp": 0.4228051410959096
+        },
+        {
+            "x": 2.2185299400469667,
+            "y": 5.757037726236599,
+            "heading": -2.9725027230810714,
+            "angularVelocity": -2.697885773041044e-7,
+            "velocityX": -0.31003383244820454,
+            "velocityY": -3.77021419603515,
+            "timestamp": 0.4697834901065662
+        },
+        {
+            "x": 2.2039649895163667,
+            "y": 5.57991929388838,
+            "heading": -2.972502735754471,
+            "angularVelocity": -2.6977108201545366e-7,
+            "velocityX": -0.3100353851791582,
+            "velocityY": -3.7702140683582024,
+            "timestamp": 0.5167618391172228
+        },
+        {
+            "x": 2.189400055868242,
+            "y": 5.402800860151866,
+            "heading": -2.972502748427869,
+            "angularVelocity": -2.6977103310705994e-7,
+            "velocityX": -0.31003502581200326,
+            "velocityY": -3.770214097910002,
+            "timestamp": 0.5637401881278795
+        },
+        {
+            "x": 2.174835140412249,
+            "y": 5.225682424919363,
+            "heading": -2.9725027611012638,
+            "angularVelocity": -2.69770971237986e-7,
+            "velocityX": -0.3100346385669985,
+            "velocityY": -3.7702141297542338,
+            "timestamp": 0.6107185371385362
+        },
+        {
+            "x": 2.1602702444212443,
+            "y": 5.048563988086202,
+            "heading": -2.972502773774655,
+            "angularVelocity": -2.697708934411251e-7,
+            "velocityX": -0.31003422422742555,
+            "velocityY": -3.770214163826477,
+            "timestamp": 0.6576968861491929
+        },
+        {
+            "x": 2.145705369054558,
+            "y": 4.87144554955705,
+            "heading": -2.972502786448042,
+            "angularVelocity": -2.697707995800925e-7,
+            "velocityX": -0.31003378520991653,
+            "velocityY": -3.7702141999279966,
+            "timestamp": 0.7046752351598495
+        },
+        {
+            "x": 2.1311405152628695,
+            "y": 4.694327109253736,
+            "heading": -2.9725027991214237,
+            "angularVelocity": -2.697706911872684e-7,
+            "velocityX": -0.31003332595584043,
+            "velocityY": -3.7702142376935592,
+            "timestamp": 0.7516535841705062
+        },
+        {
+            "x": 2.116575683697044,
+            "y": 4.5172086671227385,
+            "heading": -2.9725028117947994,
+            "angularVelocity": -2.6977056864766263e-7,
+            "velocityX": -0.3100328528472012,
+            "velocityY": -3.77021427659835,
+            "timestamp": 0.7986319331811629
+        },
+        {
+            "x": 2.102010874647989,
+            "y": 4.340090223140139,
+            "heading": -2.9725028244681693,
+            "angularVelocity": -2.697704390106331e-7,
+            "velocityX": -0.3100323735461829,
+            "velocityY": -3.7702143160122867,
+            "timestamp": 0.8456102821918196
+        },
+        {
+            "x": 2.087446088035901,
+            "y": 4.162971777312504,
+            "heading": -2.9725028371415325,
+            "angularVelocity": -2.697703054465384e-7,
+            "velocityX": -0.3100318959438893,
+            "velocityY": -3.7702143552864653,
+            "timestamp": 0.8925886312024762
+        },
+        {
+            "x": 2.0728813234455363,
+            "y": 3.9858533296739824,
+            "heading": -2.97250284981489,
+            "angularVelocity": -2.69770174050259e-7,
+            "velocityX": -0.31003142718065585,
+            "velocityY": -3.770214393833725,
+            "timestamp": 0.9395669802131329
+        },
+        {
+            "x": 2.0583165802454695,
+            "y": 3.8087348802765,
+            "heading": -2.972502862488243,
+            "angularVelocity": -2.697700772639611e-7,
+            "velocityX": -0.3100309718581832,
+            "velocityY": -3.770214431275663,
+            "timestamp": 0.9865453292237896
+        },
+        {
+            "x": 2.0437518576774885,
+            "y": 3.631616429182409,
+            "heading": -2.972502875161594,
+            "angularVelocity": -2.6977003935784156e-7,
+            "velocityX": -0.31003053267531316,
+            "velocityY": -3.7702144673903546,
+            "timestamp": 1.0335236782344461
+        },
+        {
+            "x": 2.02918715486261,
+            "y": 3.4544979764639927,
+            "heading": -2.972502887834946,
+            "angularVelocity": -2.697700540709368e-7,
+            "velocityX": -0.31003011220283416,
+            "velocityY": -3.770214501966405,
+            "timestamp": 1.0805020272451027
+        },
+        {
+            "x": 2.014622470919118,
+            "y": 3.2773795221937574,
+            "heading": -2.972502900508301,
+            "angularVelocity": -2.6977012019750386e-7,
+            "velocityX": -0.3100297104989377,
+            "velocityY": -3.770214534999038,
+            "timestamp": 1.1274803762557593
+        },
+        {
+            "x": 2.000060409435934,
+            "y": 3.1002608522940482,
+            "heading": -2.972502913181681,
+            "angularVelocity": -2.697706573776715e-7,
+            "velocityX": -0.30997388775603324,
+            "velocityY": -3.77021912497458,
+            "timestamp": 1.1744587252664158
+        },
+        {
+            "x": 2.001608653690626,
+            "y": 2.9225513149097844,
+            "heading": -2.972502926099156,
+            "angularVelocity": -2.749665584566826e-7,
+            "velocityX": 0.03295654886341899,
+            "velocityY": -3.782796567498611,
+            "timestamp": 1.2214370742770724
+        },
+        {
+            "x": 2.0215247790201643,
+            "y": 2.7459545305120523,
+            "heading": -2.9725029398381673,
+            "angularVelocity": -2.9245411249129853e-7,
+            "velocityX": 0.4239426405772788,
+            "velocityY": -3.759110060629698,
+            "timestamp": 1.268415423287729
+        },
+        {
+            "x": 2.0595955339413297,
+            "y": 2.572363939972743,
+            "heading": -2.9725029551587165,
+            "angularVelocity": -3.2611936426906016e-7,
+            "velocityX": 0.8103893755936399,
+            "velocityY": -3.695119011098735,
+            "timestamp": 1.3153937722983855
+        },
+        {
+            "x": 2.1154127951527757,
+            "y": 2.4036407715704846,
+            "heading": -2.972502973192088,
+            "angularVelocity": -3.8386557912539916e-7,
+            "velocityX": 1.1881486341460188,
+            "velocityY": -3.5915091091001314,
+            "timestamp": 1.362372121309042
+        },
+        {
+            "x": 2.1883781192124445,
+            "y": 2.241594069488506,
+            "heading": -2.9725029958666975,
+            "angularVelocity": -4.826608466149902e-7,
+            "velocityX": 1.5531691853010408,
+            "velocityY": -3.4493911662416843,
+            "timestamp": 1.4093504703196986
+        },
+        {
+            "x": 2.277709186746539,
+            "y": 2.0879612953868114,
+            "heading": -2.9725030269682566,
+            "angularVelocity": -6.620402725580348e-7,
+            "velocityX": 1.901536972145005,
+            "velocityY": -3.2702889168549634,
+            "timestamp": 1.4563288193303552
+        },
+        {
+            "x": 2.382448196411133,
+            "y": 1.9443897008895872,
+            "heading": -2.9725030765065807,
+            "angularVelocity": -0.0000010544926575738783,
+            "velocityX": 2.2295166150014207,
+            "velocityY": -3.0561226079838564,
+            "timestamp": 1.5033071683410117
+        },
+        {
+            "x": 2.5069799488821,
+            "y": 1.8074960632068364,
+            "heading": -2.9725031217478985,
+            "angularVelocity": -9.247979619471774e-7,
+            "velocityX": 2.5456091262787446,
+            "velocityY": -2.798303938555362,
+            "timestamp": 1.5522273859759206
+        },
+        {
+            "x": 2.645527130072252,
+            "y": 1.6848065781413901,
+            "heading": -2.9725031505695743,
+            "angularVelocity": -5.891567373541898e-7,
+            "velocityX": 2.8321047593068385,
+            "velocityY": -2.507950516105164,
+            "timestamp": 1.6011476036108294
+        },
+        {
+            "x": 2.7964788901009507,
+            "y": 1.5777477131393498,
+            "heading": -2.972503171586243,
+            "angularVelocity": -4.2961111591525744e-7,
+            "velocityX": 3.085672291060715,
+            "velocityY": -2.188438036008342,
+            "timestamp": 1.6500678212457383
+        },
+        {
+            "x": 2.9580801560836862,
+            "y": 1.4875641931626062,
+            "heading": -2.9725031883859017,
+            "angularVelocity": -3.434093106618729e-7,
+            "velocityX": 3.3033635947567466,
+            "velocityY": -1.8434815774897553,
+            "timestamp": 1.698988038880647
+        },
+        {
+            "x": 3.1284520354522702,
+            "y": 1.4153045196097347,
+            "heading": -2.9725032027821,
+            "angularVelocity": -2.9427911753262145e-7,
+            "velocityX": 3.482647616984601,
+            "velocityY": -1.4770922339746126,
+            "timestamp": 1.747908256515556
+        },
+        {
+            "x": 3.305613655866368,
+            "y": 1.3618087375272205,
+            "heading": -2.9725032158448177,
+            "angularVelocity": -2.670208303687875e-7,
+            "velocityX": 3.6214397437119468,
+            "velocityY": -1.0935311547825342,
+            "timestamp": 1.7968284741504648
+        },
+        {
+            "x": 3.4875051596049977,
+            "y": 1.327698333409946,
+            "heading": -2.9725032283101815,
+            "angularVelocity": -2.548100678766224e-7,
+            "velocityX": 3.7181253995246735,
+            "velocityY": -0.6972659928016005,
+            "timestamp": 1.8457486917853736
+        },
+        {
+            "x": 3.671044139168993,
+            "y": 1.3040027251815136,
+            "heading": -2.972503240670167,
+            "angularVelocity": -2.5265597445839956e-7,
+            "velocityX": 3.751802188079885,
+            "velocityY": -0.48437250229081197,
+            "timestamp": 1.8946689094202824
+        },
+        {
+            "x": 3.854583175253026,
+            "y": 1.2803075547441987,
+            "heading": -2.972503253030148,
+            "angularVelocity": -2.5265589039445086e-7,
+            "velocityX": 3.751803343431197,
+            "velocityY": -0.48436355320721586,
+            "timestamp": 1.9435891270551913
+        },
+        {
+            "x": 4.038122213757311,
+            "y": 1.256612403053765,
+            "heading": -2.972503265390126,
+            "angularVelocity": -2.5265581959003427e-7,
+            "velocityX": 3.751803392904627,
+            "velocityY": -0.4843631699938504,
+            "timestamp": 1.9925093446901
+        },
+        {
+            "x": 4.221661254747456,
+            "y": 1.2329172706184417,
+            "heading": -2.9725032777500995,
+            "angularVelocity": -2.526557374147453e-7,
+            "velocityX": 3.7518034437192385,
+            "velocityY": -0.48436277639154923,
+            "timestamp": 2.041429562325009
+        },
+        {
+            "x": 4.405200298259638,
+            "y": 1.2092221577184428,
+            "heading": -2.972503290110069,
+            "angularVelocity": -2.526556474640549e-7,
+            "velocityX": 3.7518034952732924,
+            "velocityY": -0.4843623770612628,
+            "timestamp": 2.090349779959918
+        },
+        {
+            "x": 4.5887393442969575,
+            "y": 1.1855270643778184,
+            "heading": -2.9725033024700336,
+            "angularVelocity": -2.526555535451317e-7,
+            "velocityX": 3.7518035468907667,
+            "velocityY": -0.4843619772393562,
+            "timestamp": 2.1392699975948273
+        },
+        {
+            "x": 4.772278392830339,
+            "y": 1.1618319903713676,
+            "heading": -2.9725033148299937,
+            "angularVelocity": -2.5265545524016583e-7,
+            "velocityX": 3.7518035979138844,
+            "velocityY": -0.48436158202088675,
+            "timestamp": 2.1881902152297363
+        },
+        {
+            "x": 4.955817443801929,
+            "y": 1.1381369352509825,
+            "heading": -2.972503327189949,
+            "angularVelocity": -2.526553571562985e-7,
+            "velocityX": 3.751803647754394,
+            "velocityY": -0.4843611959623946,
+            "timestamp": 2.2371104328646454
+        },
+        {
+            "x": 5.139356497135768,
+            "y": 1.114441898428302,
+            "heading": -2.9725033395499,
+            "angularVelocity": -2.526552791781282e-7,
+            "velocityX": 3.751803696042169,
+            "velocityY": -0.48436082193084695,
+            "timestamp": 2.2860306504995545
+        },
+        {
+            "x": 5.322895552747796,
+            "y": 1.0907468792522317,
+            "heading": -2.97250335190985,
+            "angularVelocity": -2.5265524884530903e-7,
+            "velocityX": 3.7518037426116595,
+            "velocityY": -0.4843604612086112,
+            "timestamp": 2.3349508681344635
+        },
+        {
+            "x": 5.506434610543511,
+            "y": 1.067051876990783,
+            "heading": -2.9725033642697998,
+            "angularVelocity": -2.5265526208757776e-7,
+            "velocityX": 3.7518037872493903,
+            "velocityY": -0.4843601154492814,
+            "timestamp": 2.3838710857693726
+        },
+        {
+            "x": 5.689973670421635,
+            "y": 1.043356890859468,
+            "heading": -2.972503376629753,
+            "angularVelocity": -2.52655316177873e-7,
+            "velocityX": 3.751803829816819,
+            "velocityY": -0.48435978572601984,
+            "timestamp": 2.4327913034042816
+        },
+        {
+            "x": 5.873512732277465,
+            "y": 1.0196619200472952,
+            "heading": -2.9725033889897103,
+            "angularVelocity": -2.526554076637863e-7,
+            "velocityX": 3.751803870244009,
+            "velocityY": -0.4843594725805982,
+            "timestamp": 2.4817115210391907
+        },
+        {
+            "x": 6.057051796000616,
+            "y": 0.9959669636992331,
+            "heading": -2.9725034013496736,
+            "angularVelocity": -2.5265551720355475e-7,
+            "velocityX": 3.751803908414743,
+            "velocityY": -0.48435917691325625,
+            "timestamp": 2.5306317386740997
+        },
+        {
+            "x": 6.240590861474878,
+            "y": 0.972272020915138,
+            "heading": -2.9725034137096418,
+            "angularVelocity": -2.526556193143771e-7,
+            "velocityX": 3.7518039442099766,
+            "velocityY": -0.48435889964615636,
+            "timestamp": 2.579551956309009
+        },
+        {
+            "x": 6.424129928585653,
+            "y": 0.9485770908073501,
+            "heading": -2.972503426069614,
+            "angularVelocity": -2.5265571512549996e-7,
+            "velocityX": 3.7518039776626577,
+            "velocityY": -0.4843586405241034,
+            "timestamp": 2.628472173943918
+        },
+        {
+            "x": 6.607668997222064,
+            "y": 0.9248821725170546,
+            "heading": -2.972503438429591,
+            "angularVelocity": -2.526558029491111e-7,
+            "velocityX": 3.7518040088489135,
+            "velocityY": -0.4843583989574683,
+            "timestamp": 2.677392391578827
+        },
+        {
+            "x": 6.791208068059123,
+            "y": 0.9011872712731527,
+            "heading": -2.972503450789653,
+            "angularVelocity": -2.5265754822248404e-7,
+            "velocityX": 3.751804053833296,
+            "velocityY": -0.48435805050452835,
+            "timestamp": 2.726312609213736
+        },
+        {
+            "x": 6.968893183591214,
+            "y": 0.8784347773708095,
+            "heading": -2.9889996672823953,
+            "angularVelocity": -0.3372065229932763,
+            "velocityX": 3.632140741035789,
+            "velocityY": -0.4650938814733942,
+            "timestamp": 2.775232826848645
+        },
+        {
+            "x": 7.126993065244261,
+            "y": 0.8581341417430071,
+            "heading": -3.0160355228350078,
+            "angularVelocity": -0.5526519884760304,
+            "velocityX": 3.231790235132359,
+            "velocityY": -0.4149743523077879,
+            "timestamp": 2.824153044483554
+        },
+        {
+            "x": 7.265300475634767,
+            "y": 0.8403578347966174,
+            "heading": -3.044055057812944,
+            "angularVelocity": -0.5727598185896393,
+            "velocityX": 2.827203497390216,
+            "velocityY": -0.363373423214384,
+            "timestamp": 2.873073262118463
+        },
+        {
+            "x": 7.3838209572244,
+            "y": 0.8251160468222167,
+            "heading": -3.0703114433567618,
+            "angularVelocity": -0.5367184941769645,
+            "velocityX": 2.4227300555805176,
+            "velocityY": -0.31156418984375384,
+            "timestamp": 2.921993479753372
+        },
+        {
+            "x": 7.4825676028202075,
+            "y": 0.812412258635403,
+            "heading": -3.093486935010673,
+            "angularVelocity": -0.47374056728180086,
+            "velocityX": 2.018524249682452,
+            "velocityY": -0.25968380356812476,
+            "timestamp": 2.9709136973882813
+        },
+        {
+            "x": 7.561551267898866,
+            "y": 0.8022480583211733,
+            "heading": -3.1128054575324193,
+            "angularVelocity": -0.39489854002532226,
+            "velocityX": 1.6145403454276093,
+            "velocityY": -0.20777095453836575,
+            "timestamp": 3.0198339150231903
+        },
+        {
+            "x": 7.62078038482921,
+            "y": 0.7946243025369127,
+            "heading": -3.1277548992054633,
+            "angularVelocity": -0.30558820863414116,
+            "velocityX": 1.210728810987095,
+            "velocityY": -0.1558405941927034,
+            "timestamp": 3.0687541326580994
+        },
+        {
+            "x": 7.6602615463029124,
+            "y": 0.7895415063368129,
+            "heading": -3.137971889882548,
+            "angularVelocity": -0.208850065903912,
+            "velocityX": 0.8070520406992162,
+            "velocityY": -0.10389970539445781,
+            "timestamp": 3.1176743502930084
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.10656978952845035,
+            "velocityX": 0.40348254058055355,
+            "velocityY": -0.051952065213203134,
+            "timestamp": 3.1665945679279175
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -3.143185307179586,
+            "angularVelocity": 1.1636456429625425e-30,
+            "velocityX": -1.050271820667376e-32,
+            "velocityY": -1.190420457247715e-32,
+            "timestamp": 3.2155147855628265
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.traj
new file mode 100644
index 00000000..998ccf57
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.traj	
@@ -0,0 +1,616 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -2.713469488843566e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2985074122681866,
+            "y": 6.738728579173207,
+            "heading": -2.7557122969387127,
+            "angularVelocity": -0.12159424626473993,
+            "velocityX": -0.03177182176995015,
+            "velocityY": -0.38893280014262377,
+            "timestamp": 0.046978349010656624
+        },
+        {
+            "x": 2.2955230171267877,
+            "y": 6.702182881977796,
+            "heading": -2.7670222648124514,
+            "angularVelocity": -0.24074851738984976,
+            "velocityX": -0.06352703328764883,
+            "velocityY": -0.777926384495132,
+            "timestamp": 0.09395669802131325
+        },
+        {
+            "x": 2.2910478333318625,
+            "y": 6.647359316893119,
+            "heading": -2.7837824265023454,
+            "angularVelocity": -0.3567635313470153,
+            "velocityX": -0.09526055915480924,
+            "velocityY": -1.1669964193982714,
+            "timestamp": 0.14093504703196988
+        },
+        {
+            "x": 2.2850832407725172,
+            "y": 6.574253215154707,
+            "heading": -2.805794841014387,
+            "angularVelocity": -0.46856509382754397,
+            "velocityX": -0.12696471214840174,
+            "velocityY": -1.556165835496437,
+            "timestamp": 0.1879133960426265
+        },
+        {
+            "x": 2.2776312180066953,
+            "y": 6.482858263005064,
+            "heading": -2.832779187520957,
+            "angularVelocity": -0.5743996346156903,
+            "velocityX": -0.15862674876316962,
+            "velocityY": -1.945469648771827,
+            "timestamp": 0.2348917450532831
+        },
+        {
+            "x": 2.2686948542910046,
+            "y": 6.373165502071328,
+            "heading": -2.864308391554441,
+            "angularVelocity": -0.6711432968053561,
+            "velocityX": -0.1902230262213726,
+            "velocityY": -2.3349641535689947,
+            "timestamp": 0.28187009406393976
+        },
+        {
+            "x": 2.2582796451376828,
+            "y": 6.245161498589565,
+            "heading": -2.8996563968568023,
+            "angularVelocity": -0.7524318339570105,
+            "velocityX": -0.22170232399947937,
+            "velocityY": -2.7247446148592136,
+            "timestamp": 0.3288484430745964
+        },
+        {
+            "x": 2.2463978083089957,
+            "y": 6.098825641295113,
+            "heading": -2.9373287044127423,
+            "angularVelocity": -0.8019078649910463,
+            "velocityX": -0.2529215495842953,
+            "velocityY": -3.1149638158049613,
+            "timestamp": 0.375826792085253
+        },
+        {
+            "x": 2.23309481763283,
+            "y": 5.934156164582871,
+            "heading": -2.9725027104068498,
+            "angularVelocity": -0.7487280148165356,
+            "velocityX": -0.2831728010098399,
+            "velocityY": -3.505220600129828,
+            "timestamp": 0.4228051410959096
+        },
+        {
+            "x": 2.2185299400469667,
+            "y": 5.757037726236599,
+            "heading": -2.9725027230810714,
+            "angularVelocity": -2.697885773041044e-7,
+            "velocityX": -0.31003383244820454,
+            "velocityY": -3.77021419603515,
+            "timestamp": 0.4697834901065662
+        },
+        {
+            "x": 2.2039649895163667,
+            "y": 5.57991929388838,
+            "heading": -2.972502735754471,
+            "angularVelocity": -2.6977108201545366e-7,
+            "velocityX": -0.3100353851791582,
+            "velocityY": -3.7702140683582024,
+            "timestamp": 0.5167618391172228
+        },
+        {
+            "x": 2.189400055868242,
+            "y": 5.402800860151866,
+            "heading": -2.972502748427869,
+            "angularVelocity": -2.6977103310705994e-7,
+            "velocityX": -0.31003502581200326,
+            "velocityY": -3.770214097910002,
+            "timestamp": 0.5637401881278795
+        },
+        {
+            "x": 2.174835140412249,
+            "y": 5.225682424919363,
+            "heading": -2.9725027611012638,
+            "angularVelocity": -2.69770971237986e-7,
+            "velocityX": -0.3100346385669985,
+            "velocityY": -3.7702141297542338,
+            "timestamp": 0.6107185371385362
+        },
+        {
+            "x": 2.1602702444212443,
+            "y": 5.048563988086202,
+            "heading": -2.972502773774655,
+            "angularVelocity": -2.697708934411251e-7,
+            "velocityX": -0.31003422422742555,
+            "velocityY": -3.770214163826477,
+            "timestamp": 0.6576968861491929
+        },
+        {
+            "x": 2.145705369054558,
+            "y": 4.87144554955705,
+            "heading": -2.972502786448042,
+            "angularVelocity": -2.697707995800925e-7,
+            "velocityX": -0.31003378520991653,
+            "velocityY": -3.7702141999279966,
+            "timestamp": 0.7046752351598495
+        },
+        {
+            "x": 2.1311405152628695,
+            "y": 4.694327109253736,
+            "heading": -2.9725027991214237,
+            "angularVelocity": -2.697706911872684e-7,
+            "velocityX": -0.31003332595584043,
+            "velocityY": -3.7702142376935592,
+            "timestamp": 0.7516535841705062
+        },
+        {
+            "x": 2.116575683697044,
+            "y": 4.5172086671227385,
+            "heading": -2.9725028117947994,
+            "angularVelocity": -2.6977056864766263e-7,
+            "velocityX": -0.3100328528472012,
+            "velocityY": -3.77021427659835,
+            "timestamp": 0.7986319331811629
+        },
+        {
+            "x": 2.102010874647989,
+            "y": 4.340090223140139,
+            "heading": -2.9725028244681693,
+            "angularVelocity": -2.697704390106331e-7,
+            "velocityX": -0.3100323735461829,
+            "velocityY": -3.7702143160122867,
+            "timestamp": 0.8456102821918196
+        },
+        {
+            "x": 2.087446088035901,
+            "y": 4.162971777312504,
+            "heading": -2.9725028371415325,
+            "angularVelocity": -2.697703054465384e-7,
+            "velocityX": -0.3100318959438893,
+            "velocityY": -3.7702143552864653,
+            "timestamp": 0.8925886312024762
+        },
+        {
+            "x": 2.0728813234455363,
+            "y": 3.9858533296739824,
+            "heading": -2.97250284981489,
+            "angularVelocity": -2.69770174050259e-7,
+            "velocityX": -0.31003142718065585,
+            "velocityY": -3.770214393833725,
+            "timestamp": 0.9395669802131329
+        },
+        {
+            "x": 2.0583165802454695,
+            "y": 3.8087348802765,
+            "heading": -2.972502862488243,
+            "angularVelocity": -2.697700772639611e-7,
+            "velocityX": -0.3100309718581832,
+            "velocityY": -3.770214431275663,
+            "timestamp": 0.9865453292237896
+        },
+        {
+            "x": 2.0437518576774885,
+            "y": 3.631616429182409,
+            "heading": -2.972502875161594,
+            "angularVelocity": -2.6977003935784156e-7,
+            "velocityX": -0.31003053267531316,
+            "velocityY": -3.7702144673903546,
+            "timestamp": 1.0335236782344461
+        },
+        {
+            "x": 2.02918715486261,
+            "y": 3.4544979764639927,
+            "heading": -2.972502887834946,
+            "angularVelocity": -2.697700540709368e-7,
+            "velocityX": -0.31003011220283416,
+            "velocityY": -3.770214501966405,
+            "timestamp": 1.0805020272451027
+        },
+        {
+            "x": 2.014622470919118,
+            "y": 3.2773795221937574,
+            "heading": -2.972502900508301,
+            "angularVelocity": -2.6977012019750386e-7,
+            "velocityX": -0.3100297104989377,
+            "velocityY": -3.770214534999038,
+            "timestamp": 1.1274803762557593
+        },
+        {
+            "x": 2.000060409435934,
+            "y": 3.1002608522940482,
+            "heading": -2.972502913181681,
+            "angularVelocity": -2.697706573776715e-7,
+            "velocityX": -0.30997388775603324,
+            "velocityY": -3.77021912497458,
+            "timestamp": 1.1744587252664158
+        },
+        {
+            "x": 2.001608653690626,
+            "y": 2.9225513149097844,
+            "heading": -2.972502926099156,
+            "angularVelocity": -2.749665584566826e-7,
+            "velocityX": 0.03295654886341899,
+            "velocityY": -3.782796567498611,
+            "timestamp": 1.2214370742770724
+        },
+        {
+            "x": 2.0215247790201643,
+            "y": 2.7459545305120523,
+            "heading": -2.9725029398381673,
+            "angularVelocity": -2.9245411249129853e-7,
+            "velocityX": 0.4239426405772788,
+            "velocityY": -3.759110060629698,
+            "timestamp": 1.268415423287729
+        },
+        {
+            "x": 2.0595955339413297,
+            "y": 2.572363939972743,
+            "heading": -2.9725029551587165,
+            "angularVelocity": -3.2611936426906016e-7,
+            "velocityX": 0.8103893755936399,
+            "velocityY": -3.695119011098735,
+            "timestamp": 1.3153937722983855
+        },
+        {
+            "x": 2.1154127951527757,
+            "y": 2.4036407715704846,
+            "heading": -2.972502973192088,
+            "angularVelocity": -3.8386557912539916e-7,
+            "velocityX": 1.1881486341460188,
+            "velocityY": -3.5915091091001314,
+            "timestamp": 1.362372121309042
+        },
+        {
+            "x": 2.1883781192124445,
+            "y": 2.241594069488506,
+            "heading": -2.9725029958666975,
+            "angularVelocity": -4.826608466149902e-7,
+            "velocityX": 1.5531691853010408,
+            "velocityY": -3.4493911662416843,
+            "timestamp": 1.4093504703196986
+        },
+        {
+            "x": 2.277709186746539,
+            "y": 2.0879612953868114,
+            "heading": -2.9725030269682566,
+            "angularVelocity": -6.620402725580348e-7,
+            "velocityX": 1.901536972145005,
+            "velocityY": -3.2702889168549634,
+            "timestamp": 1.4563288193303552
+        },
+        {
+            "x": 2.382448196411133,
+            "y": 1.9443897008895872,
+            "heading": -2.9725030765065807,
+            "angularVelocity": -0.0000010544926575738783,
+            "velocityX": 2.2295166150014207,
+            "velocityY": -3.0561226079838564,
+            "timestamp": 1.5033071683410117
+        },
+        {
+            "x": 2.5069799488821,
+            "y": 1.8074960632068364,
+            "heading": -2.9725031217478985,
+            "angularVelocity": -9.247979619471774e-7,
+            "velocityX": 2.5456091262787446,
+            "velocityY": -2.798303938555362,
+            "timestamp": 1.5522273859759206
+        },
+        {
+            "x": 2.645527130072252,
+            "y": 1.6848065781413901,
+            "heading": -2.9725031505695743,
+            "angularVelocity": -5.891567373541898e-7,
+            "velocityX": 2.8321047593068385,
+            "velocityY": -2.507950516105164,
+            "timestamp": 1.6011476036108294
+        },
+        {
+            "x": 2.7964788901009507,
+            "y": 1.5777477131393498,
+            "heading": -2.972503171586243,
+            "angularVelocity": -4.2961111591525744e-7,
+            "velocityX": 3.085672291060715,
+            "velocityY": -2.188438036008342,
+            "timestamp": 1.6500678212457383
+        },
+        {
+            "x": 2.9580801560836862,
+            "y": 1.4875641931626062,
+            "heading": -2.9725031883859017,
+            "angularVelocity": -3.434093106618729e-7,
+            "velocityX": 3.3033635947567466,
+            "velocityY": -1.8434815774897553,
+            "timestamp": 1.698988038880647
+        },
+        {
+            "x": 3.1284520354522702,
+            "y": 1.4153045196097347,
+            "heading": -2.9725032027821,
+            "angularVelocity": -2.9427911753262145e-7,
+            "velocityX": 3.482647616984601,
+            "velocityY": -1.4770922339746126,
+            "timestamp": 1.747908256515556
+        },
+        {
+            "x": 3.305613655866368,
+            "y": 1.3618087375272205,
+            "heading": -2.9725032158448177,
+            "angularVelocity": -2.670208303687875e-7,
+            "velocityX": 3.6214397437119468,
+            "velocityY": -1.0935311547825342,
+            "timestamp": 1.7968284741504648
+        },
+        {
+            "x": 3.4875051596049977,
+            "y": 1.327698333409946,
+            "heading": -2.9725032283101815,
+            "angularVelocity": -2.548100678766224e-7,
+            "velocityX": 3.7181253995246735,
+            "velocityY": -0.6972659928016005,
+            "timestamp": 1.8457486917853736
+        },
+        {
+            "x": 3.671044139168993,
+            "y": 1.3040027251815136,
+            "heading": -2.972503240670167,
+            "angularVelocity": -2.5265597445839956e-7,
+            "velocityX": 3.751802188079885,
+            "velocityY": -0.48437250229081197,
+            "timestamp": 1.8946689094202824
+        },
+        {
+            "x": 3.854583175253026,
+            "y": 1.2803075547441987,
+            "heading": -2.972503253030148,
+            "angularVelocity": -2.5265589039445086e-7,
+            "velocityX": 3.751803343431197,
+            "velocityY": -0.48436355320721586,
+            "timestamp": 1.9435891270551913
+        },
+        {
+            "x": 4.038122213757311,
+            "y": 1.256612403053765,
+            "heading": -2.972503265390126,
+            "angularVelocity": -2.5265581959003427e-7,
+            "velocityX": 3.751803392904627,
+            "velocityY": -0.4843631699938504,
+            "timestamp": 1.9925093446901
+        },
+        {
+            "x": 4.221661254747456,
+            "y": 1.2329172706184417,
+            "heading": -2.9725032777500995,
+            "angularVelocity": -2.526557374147453e-7,
+            "velocityX": 3.7518034437192385,
+            "velocityY": -0.48436277639154923,
+            "timestamp": 2.041429562325009
+        },
+        {
+            "x": 4.405200298259638,
+            "y": 1.2092221577184428,
+            "heading": -2.972503290110069,
+            "angularVelocity": -2.526556474640549e-7,
+            "velocityX": 3.7518034952732924,
+            "velocityY": -0.4843623770612628,
+            "timestamp": 2.090349779959918
+        },
+        {
+            "x": 4.5887393442969575,
+            "y": 1.1855270643778184,
+            "heading": -2.9725033024700336,
+            "angularVelocity": -2.526555535451317e-7,
+            "velocityX": 3.7518035468907667,
+            "velocityY": -0.4843619772393562,
+            "timestamp": 2.1392699975948273
+        },
+        {
+            "x": 4.772278392830339,
+            "y": 1.1618319903713676,
+            "heading": -2.9725033148299937,
+            "angularVelocity": -2.5265545524016583e-7,
+            "velocityX": 3.7518035979138844,
+            "velocityY": -0.48436158202088675,
+            "timestamp": 2.1881902152297363
+        },
+        {
+            "x": 4.955817443801929,
+            "y": 1.1381369352509825,
+            "heading": -2.972503327189949,
+            "angularVelocity": -2.526553571562985e-7,
+            "velocityX": 3.751803647754394,
+            "velocityY": -0.4843611959623946,
+            "timestamp": 2.2371104328646454
+        },
+        {
+            "x": 5.139356497135768,
+            "y": 1.114441898428302,
+            "heading": -2.9725033395499,
+            "angularVelocity": -2.526552791781282e-7,
+            "velocityX": 3.751803696042169,
+            "velocityY": -0.48436082193084695,
+            "timestamp": 2.2860306504995545
+        },
+        {
+            "x": 5.322895552747796,
+            "y": 1.0907468792522317,
+            "heading": -2.97250335190985,
+            "angularVelocity": -2.5265524884530903e-7,
+            "velocityX": 3.7518037426116595,
+            "velocityY": -0.4843604612086112,
+            "timestamp": 2.3349508681344635
+        },
+        {
+            "x": 5.506434610543511,
+            "y": 1.067051876990783,
+            "heading": -2.9725033642697998,
+            "angularVelocity": -2.5265526208757776e-7,
+            "velocityX": 3.7518037872493903,
+            "velocityY": -0.4843601154492814,
+            "timestamp": 2.3838710857693726
+        },
+        {
+            "x": 5.689973670421635,
+            "y": 1.043356890859468,
+            "heading": -2.972503376629753,
+            "angularVelocity": -2.52655316177873e-7,
+            "velocityX": 3.751803829816819,
+            "velocityY": -0.48435978572601984,
+            "timestamp": 2.4327913034042816
+        },
+        {
+            "x": 5.873512732277465,
+            "y": 1.0196619200472952,
+            "heading": -2.9725033889897103,
+            "angularVelocity": -2.526554076637863e-7,
+            "velocityX": 3.751803870244009,
+            "velocityY": -0.4843594725805982,
+            "timestamp": 2.4817115210391907
+        },
+        {
+            "x": 6.057051796000616,
+            "y": 0.9959669636992331,
+            "heading": -2.9725034013496736,
+            "angularVelocity": -2.5265551720355475e-7,
+            "velocityX": 3.751803908414743,
+            "velocityY": -0.48435917691325625,
+            "timestamp": 2.5306317386740997
+        },
+        {
+            "x": 6.240590861474878,
+            "y": 0.972272020915138,
+            "heading": -2.9725034137096418,
+            "angularVelocity": -2.526556193143771e-7,
+            "velocityX": 3.7518039442099766,
+            "velocityY": -0.48435889964615636,
+            "timestamp": 2.579551956309009
+        },
+        {
+            "x": 6.424129928585653,
+            "y": 0.9485770908073501,
+            "heading": -2.972503426069614,
+            "angularVelocity": -2.5265571512549996e-7,
+            "velocityX": 3.7518039776626577,
+            "velocityY": -0.4843586405241034,
+            "timestamp": 2.628472173943918
+        },
+        {
+            "x": 6.607668997222064,
+            "y": 0.9248821725170546,
+            "heading": -2.972503438429591,
+            "angularVelocity": -2.526558029491111e-7,
+            "velocityX": 3.7518040088489135,
+            "velocityY": -0.4843583989574683,
+            "timestamp": 2.677392391578827
+        },
+        {
+            "x": 6.791208068059123,
+            "y": 0.9011872712731527,
+            "heading": -2.972503450789653,
+            "angularVelocity": -2.5265754822248404e-7,
+            "velocityX": 3.751804053833296,
+            "velocityY": -0.48435805050452835,
+            "timestamp": 2.726312609213736
+        },
+        {
+            "x": 6.968893183591214,
+            "y": 0.8784347773708095,
+            "heading": -2.9889996672823953,
+            "angularVelocity": -0.3372065229932763,
+            "velocityX": 3.632140741035789,
+            "velocityY": -0.4650938814733942,
+            "timestamp": 2.775232826848645
+        },
+        {
+            "x": 7.126993065244261,
+            "y": 0.8581341417430071,
+            "heading": -3.0160355228350078,
+            "angularVelocity": -0.5526519884760304,
+            "velocityX": 3.231790235132359,
+            "velocityY": -0.4149743523077879,
+            "timestamp": 2.824153044483554
+        },
+        {
+            "x": 7.265300475634767,
+            "y": 0.8403578347966174,
+            "heading": -3.044055057812944,
+            "angularVelocity": -0.5727598185896393,
+            "velocityX": 2.827203497390216,
+            "velocityY": -0.363373423214384,
+            "timestamp": 2.873073262118463
+        },
+        {
+            "x": 7.3838209572244,
+            "y": 0.8251160468222167,
+            "heading": -3.0703114433567618,
+            "angularVelocity": -0.5367184941769645,
+            "velocityX": 2.4227300555805176,
+            "velocityY": -0.31156418984375384,
+            "timestamp": 2.921993479753372
+        },
+        {
+            "x": 7.4825676028202075,
+            "y": 0.812412258635403,
+            "heading": -3.093486935010673,
+            "angularVelocity": -0.47374056728180086,
+            "velocityX": 2.018524249682452,
+            "velocityY": -0.25968380356812476,
+            "timestamp": 2.9709136973882813
+        },
+        {
+            "x": 7.561551267898866,
+            "y": 0.8022480583211733,
+            "heading": -3.1128054575324193,
+            "angularVelocity": -0.39489854002532226,
+            "velocityX": 1.6145403454276093,
+            "velocityY": -0.20777095453836575,
+            "timestamp": 3.0198339150231903
+        },
+        {
+            "x": 7.62078038482921,
+            "y": 0.7946243025369127,
+            "heading": -3.1277548992054633,
+            "angularVelocity": -0.30558820863414116,
+            "velocityX": 1.210728810987095,
+            "velocityY": -0.1558405941927034,
+            "timestamp": 3.0687541326580994
+        },
+        {
+            "x": 7.6602615463029124,
+            "y": 0.7895415063368129,
+            "heading": -3.137971889882548,
+            "angularVelocity": -0.208850065903912,
+            "velocityX": 0.8070520406992162,
+            "velocityY": -0.10389970539445781,
+            "timestamp": 3.1176743502930084
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.10656978952845035,
+            "velocityX": 0.40348254058055355,
+            "velocityY": -0.051952065213203134,
+            "timestamp": 3.1665945679279175
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -3.143185307179586,
+            "angularVelocity": 1.1636456429625425e-30,
+            "velocityX": -1.050271820667376e-32,
+            "velocityY": -1.190420457247715e-32,
+            "timestamp": 3.2155147855628265
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.1.traj
new file mode 100644
index 00000000..96fad438
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.1.traj	
@@ -0,0 +1,544 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -7.419385593511551e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 2.293773644442264,
+            "y": 6.737033643205664,
+            "heading": -2.75526766821854,
+            "angularVelocity": -0.10504784252909477,
+            "velocityX": -0.1241659859778435,
+            "velocityY": -0.3981690982414253,
+            "timestamp": 0.050145420331514534
+        },
+        {
+            "x": 2.2813193646388377,
+            "y": 6.697095621507987,
+            "heading": -2.7655160845701583,
+            "angularVelocity": -0.20437392455513897,
+            "velocityX": -0.24836325473174134,
+            "velocityY": -0.7964440507955471,
+            "timestamp": 0.10029084066302907
+        },
+        {
+            "x": 2.2626352666399625,
+            "y": 6.637179497704236,
+            "heading": -2.7803732946846287,
+            "angularVelocity": -0.2962824923243045,
+            "velocityX": -0.3725982926327797,
+            "velocityY": -1.1948473740501462,
+            "timestamp": 0.1504362609945436
+        },
+        {
+            "x": 2.2377190441074863,
+            "y": 6.557277385764274,
+            "heading": -2.799337789044294,
+            "angularVelocity": -0.3781899570148323,
+            "velocityX": -0.4968793235305074,
+            "velocityY": -1.5934079605220988,
+            "timestamp": 0.20058168132605814
+        },
+        {
+            "x": 2.2065679165597203,
+            "y": 6.457379637066711,
+            "heading": -2.8216967909576067,
+            "angularVelocity": -0.44588322852808987,
+            "velocityX": -0.6212158027956237,
+            "velocityY": -1.9921609598071555,
+            "timestamp": 0.25072710165757267
+        },
+        {
+            "x": 2.1691787676944525,
+            "y": 6.337475004416767,
+            "heading": -2.846358557659031,
+            "angularVelocity": -0.49180496520688655,
+            "velocityX": -0.7456144273611754,
+            "velocityY": -2.3911382506567254,
+            "timestamp": 0.3008725219890872
+        },
+        {
+            "x": 2.1255494213188793,
+            "y": 6.197554115260485,
+            "heading": -2.8714458003624506,
+            "angularVelocity": -0.5002898078741331,
+            "velocityX": -0.8700564495648131,
+            "velocityY": -2.7903024489824935,
+            "timestamp": 0.35101794232060174
+        },
+        {
+            "x": 2.0756883391523315,
+            "y": 6.037638592000563,
+            "heading": -2.8929930081362487,
+            "angularVelocity": -0.4296944293486671,
+            "velocityX": -0.9943297281568805,
+            "velocityY": -3.1890354533417224,
+            "timestamp": 0.4011633626521163
+        },
+        {
+            "x": 2.0198170203632526,
+            "y": 5.858213938792853,
+            "heading": -2.897462778158027,
+            "angularVelocity": -0.08913615624774217,
+            "velocityX": -1.1141858702092737,
+            "velocityY": -3.578086533556236,
+            "timestamp": 0.4513087829836308
+        },
+        {
+            "x": 1.9633339655295539,
+            "y": 5.677120968466723,
+            "heading": -2.897462791142581,
+            "angularVelocity": -2.5893798802100903e-7,
+            "velocityX": -1.1263851107496088,
+            "velocityY": -3.611356114454979,
+            "timestamp": 0.5014542033151453
+        },
+        {
+            "x": 1.9068509267595308,
+            "y": 5.496027993130308,
+            "heading": -2.897462804127121,
+            "angularVelocity": -2.5893770297899326e-7,
+            "velocityX": -1.1263847904077713,
+            "velocityY": -3.611356214370084,
+            "timestamp": 0.5515996236466598
+        },
+        {
+            "x": 1.8503679083113405,
+            "y": 5.314935011455497,
+            "heading": -2.897462817111663,
+            "angularVelocity": -2.589377492115138e-7,
+            "velocityX": -1.1263843851497735,
+            "velocityY": -3.611356340770398,
+            "timestamp": 0.6017450439781744
+        },
+        {
+            "x": 1.7938849123636174,
+            "y": 5.133842022762774,
+            "heading": -2.8974628300962064,
+            "angularVelocity": -2.589377769209582e-7,
+            "velocityX": -1.1263839364454482,
+            "velocityY": -3.6113564807215806,
+            "timestamp": 0.651890464309689
+        },
+        {
+            "x": 1.737401940953919,
+            "y": 4.952749026416629,
+            "heading": -2.89746284308075,
+            "angularVelocity": -2.5893777760216377e-7,
+            "velocityX": -1.126383447108141,
+            "velocityY": -3.611356633346137,
+            "timestamp": 0.7020358846412036
+        },
+        {
+            "x": 1.680918995692082,
+            "y": 4.771656021914957,
+            "heading": -2.897462856065292,
+            "angularVelocity": -2.5893774300527816e-7,
+            "velocityX": -1.1263829256674796,
+            "velocityY": -3.6113567959836606,
+            "timestamp": 0.7521813049727182
+        },
+        {
+            "x": 1.6244360774524895,
+            "y": 4.590563008985036,
+            "heading": -2.8974628690498303,
+            "angularVelocity": -2.589376688920553e-7,
+            "velocityX": -1.1263823867898586,
+            "velocityY": -3.611356964059789,
+            "timestamp": 0.8023267253042328
+        },
+        {
+            "x": 1.567953186210211,
+            "y": 4.40946998763471,
+            "heading": -2.897462882034363,
+            "angularVelocity": -2.589375607053774e-7,
+            "velocityX": -1.1263818484094146,
+            "velocityY": -3.6113571319795557,
+            "timestamp": 0.8524721456357474
+        },
+        {
+            "x": 1.5114708900262606,
+            "y": 4.228376780686611,
+            "heading": -2.8974628950188928,
+            "angularVelocity": -2.589374956200649e-7,
+            "velocityX": -1.1263699817559003,
+            "velocityY": -3.611360833170398,
+            "timestamp": 0.902617565967262
+        },
+        {
+            "x": 1.4675217320832292,
+            "y": 4.043840955415942,
+            "heading": -2.8974629081797687,
+            "angularVelocity": -2.624542007539591e-7,
+            "velocityX": -0.8764341320200526,
+            "velocityY": -3.6800135296641385,
+            "timestamp": 0.9527629862987765
+        },
+        {
+            "x": 1.4442056001491934,
+            "y": 3.855582205665926,
+            "heading": -2.8974629221278954,
+            "angularVelocity": -2.7815354458428775e-7,
+            "velocityX": -0.4649703159309848,
+            "velocityY": -3.75425609169144,
+            "timestamp": 1.0029084066302911
+        },
+        {
+            "x": 1.4418077217209881,
+            "y": 3.665900239807532,
+            "heading": -2.897462937731955,
+            "angularVelocity": -3.1117616956346287e-7,
+            "velocityX": -0.04781849294218802,
+            "velocityY": -3.782637868112273,
+            "timestamp": 1.0530538269618057
+        },
+        {
+            "x": 1.4603574724576953,
+            "y": 3.47711225483097,
+            "heading": -2.8974629563063408,
+            "angularVelocity": -3.7041041196117577e-7,
+            "velocityX": 0.3699191394562724,
+            "velocityY": -3.7648101008720767,
+            "timestamp": 1.1031992472933203
+        },
+        {
+            "x": 1.4996282585911056,
+            "y": 3.2915246350140723,
+            "heading": -2.897463288823342,
+            "angularVelocity": -0.000006631054220103306,
+            "velocityX": 0.7831380388037097,
+            "velocityY": -3.7009884170870753,
+            "timestamp": 1.153344667624835
+        },
+        {
+            "x": 1.5575649679963268,
+            "y": 3.1148801932866275,
+            "heading": -2.9095242713797083,
+            "angularVelocity": -0.24052012081323423,
+            "velocityX": 1.1553738910193128,
+            "velocityY": -3.5226435546783237,
+            "timestamp": 1.2034900879563495
+        },
+        {
+            "x": 1.6317457698728626,
+            "y": 2.9507691275120527,
+            "heading": -2.937069238173179,
+            "angularVelocity": -0.549301743037936,
+            "velocityX": 1.4793135920712512,
+            "velocityY": -3.2727029644905956,
+            "timestamp": 1.253635508287864
+        },
+        {
+            "x": 1.7203254363653924,
+            "y": 2.801423061648199,
+            "heading": -2.9780621265122655,
+            "angularVelocity": -0.8174802019421967,
+            "velocityX": 1.7664557582112876,
+            "velocityY": -2.9782593281004814,
+            "timestamp": 1.3037809286193787
+        },
+        {
+            "x": 1.821600608747065,
+            "y": 2.668597377909185,
+            "heading": -3.0274133717272194,
+            "angularVelocity": -0.9841625593860135,
+            "velocityX": 2.019629543677879,
+            "velocityY": -2.648809858625076,
+            "timestamp": 1.3539263489508933
+        },
+        {
+            "x": 1.9344033083274008,
+            "y": 2.553226314276784,
+            "heading": -3.081880110008977,
+            "angularVelocity": -1.0861757249549924,
+            "velocityX": 2.2495114974525996,
+            "velocityY": -2.300729814800146,
+            "timestamp": 1.4040717692824078
+        },
+        {
+            "x": 2.057902367887511,
+            "y": 2.4558701645332204,
+            "heading": -3.139236203843767,
+            "angularVelocity": -1.1437952549924524,
+            "velocityX": 2.4628183140883095,
+            "velocityY": -1.9414763920600462,
+            "timestamp": 1.4542171896139224
+        },
+        {
+            "x": 2.1914827823638916,
+            "y": 2.376894474029541,
+            "heading": -3.1978794252899205,
+            "angularVelocity": -1.1694631545305318,
+            "velocityX": 2.663860699407278,
+            "velocityY": -1.5749332637270959,
+            "timestamp": 1.504362609945437
+        },
+        {
+            "x": 2.278019971764992,
+            "y": 2.3350837736524683,
+            "heading": -3.2342809833466872,
+            "angularVelocity": -1.1705395923861364,
+            "velocityX": 2.7827162301626456,
+            "velocityY": -1.3444776209968852,
+            "timestamp": 1.535460710593682
+        },
+        {
+            "x": 2.368131283216315,
+            "y": 2.300501166585177,
+            "heading": -3.270450856694399,
+            "angularVelocity": -1.1630894682876274,
+            "velocityX": 2.8976467878404213,
+            "velocityY": -1.112048850135951,
+            "timestamp": 1.5665588112419269
+        },
+        {
+            "x": 2.461670879436653,
+            "y": 2.2732156435859956,
+            "heading": -3.306075840058796,
+            "angularVelocity": -1.1455678199567971,
+            "velocityX": 3.007887757467237,
+            "velocityY": -0.8774015914287735,
+            "timestamp": 1.5976569118901718
+        },
+        {
+            "x": 2.5584617966487637,
+            "y": 2.2533048266167017,
+            "heading": -3.3407803732359653,
+            "angularVelocity": -1.1159695432761514,
+            "velocityX": 3.112438225952366,
+            "velocityY": -0.6402582972673794,
+            "timestamp": 1.6287550125384167
+        },
+        {
+            "x": 2.658285404214264,
+            "y": 2.240855747544563,
+            "heading": -3.37410574490051,
+            "angularVelocity": -1.0716208054472351,
+            "velocityX": 3.209958341013189,
+            "velocityY": -0.40031637986360386,
+            "timestamp": 1.6598531131866616
+        },
+        {
+            "x": 2.7608658878900645,
+            "y": 2.2359648026671843,
+            "heading": -3.405478910014421,
+            "angularVelocity": -1.0088450567700136,
+            "velocityX": 3.2986092892329157,
+            "velocityY": -0.1572747137422285,
+            "timestamp": 1.6909512138349065
+        },
+        {
+            "x": 2.8658468441128373,
+            "y": 2.238735383081805,
+            "heading": -3.4341645997548764,
+            "angularVelocity": -0.9224257797905216,
+            "velocityX": 3.3757996158745374,
+            "velocityY": 0.08909162800513673,
+            "timestamp": 1.7220493144831515
+        },
+        {
+            "x": 2.972755430801249,
+            "y": 2.249269494915596,
+            "heading": -3.459191954885324,
+            "angularVelocity": -0.8047872573808069,
+            "velocityX": 3.4377850884743864,
+            "velocityY": 0.3387381098589673,
+            "timestamp": 1.7531474151313964
+        },
+        {
+            "x": 3.0809481205317057,
+            "y": 2.267644679606423,
+            "heading": -3.4792519494186864,
+            "angularVelocity": -0.6450552964717784,
+            "velocityX": 3.4790770971590295,
+            "velocityY": 0.5908780378155715,
+            "timestamp": 1.7842455157796413
+        },
+        {
+            "x": 3.1896989899339894,
+            "y": 2.293908495297157,
+            "heading": -3.4936234234211128,
+            "angularVelocity": -0.4621334969934767,
+            "velocityX": 3.4970260927630066,
+            "velocityY": 0.8445472599052618,
+            "timestamp": 1.8153436164278862
+        },
+        {
+            "x": 3.298236220991847,
+            "y": 2.3281164444820805,
+            "heading": -3.5030903221852148,
+            "angularVelocity": -0.30442048121156556,
+            "velocityX": 3.4901562730643,
+            "velocityY": 1.1000012371126273,
+            "timestamp": 1.8464417170761311
+        },
+        {
+            "x": 3.405452310949792,
+            "y": 2.3702201329887522,
+            "heading": -3.5091052020695845,
+            "angularVelocity": -0.1934163102887616,
+            "velocityX": 3.447673257305325,
+            "velocityY": 1.3538990365653494,
+            "timestamp": 1.877539817724376
+        },
+        {
+            "x": 3.5097674187642585,
+            "y": 2.419844212041495,
+            "heading": -3.514308835252089,
+            "angularVelocity": -0.1673296141576457,
+            "velocityX": 3.354388391573783,
+            "velocityY": 1.5957270064190303,
+            "timestamp": 1.908637918372621
+        },
+        {
+            "x": 3.6091747208997034,
+            "y": 2.4757866931575614,
+            "heading": -3.523108085575375,
+            "angularVelocity": -0.2829513745169274,
+            "velocityX": 3.196571496756511,
+            "velocityY": 1.7989034683770408,
+            "timestamp": 1.939736019020866
+        },
+        {
+            "x": 3.701528766735335,
+            "y": 2.535299893209444,
+            "heading": -3.5376794547025563,
+            "angularVelocity": -0.4685613855325914,
+            "velocityX": 2.969764838060721,
+            "velocityY": 1.913724594471066,
+            "timestamp": 1.9708341196691108
+        },
+        {
+            "x": 3.7858722644846305,
+            "y": 2.59510740434591,
+            "heading": -3.555385401955535,
+            "angularVelocity": -0.5693578348478764,
+            "velocityX": 2.7121752129918284,
+            "velocityY": 1.923188551383177,
+            "timestamp": 2.001932220317356
+        },
+        {
+            "x": 3.862373728468494,
+            "y": 2.653074903297687,
+            "heading": -3.5744288715420867,
+            "angularVelocity": -0.6123676105476805,
+            "velocityX": 2.460004385772028,
+            "velocityY": 1.8640205589227477,
+            "timestamp": 2.033030320965601
+        },
+        {
+            "x": 3.931417292290283,
+            "y": 2.707976512433947,
+            "heading": -3.5936932545527167,
+            "angularVelocity": -0.6194713699243786,
+            "velocityX": 2.220185875746878,
+            "velocityY": 1.7654328718419663,
+            "timestamp": 2.064128421613846
+        },
+        {
+            "x": 3.9933519727900295,
+            "y": 2.7590743829972735,
+            "heading": -3.612463672222599,
+            "angularVelocity": -0.6035872699166261,
+            "velocityX": 1.991590457574803,
+            "velocityY": 1.643118695295981,
+            "timestamp": 2.0952265222620907
+        },
+        {
+            "x": 4.04845853802061,
+            "y": 2.8058913864006936,
+            "heading": -3.6302579260771157,
+            "angularVelocity": -0.5721974488337824,
+            "velocityX": 1.7720235024607334,
+            "velocityY": 1.5054618265268112,
+            "timestamp": 2.1263246229103356
+        },
+        {
+            "x": 4.096959063236219,
+            "y": 2.8480989028223083,
+            "heading": -3.64673270512686,
+            "angularVelocity": -0.5297680149691044,
+            "velocityX": 1.5595976668866187,
+            "velocityY": 1.3572377586345543,
+            "timestamp": 2.1574227235585806
+        },
+        {
+            "x": 4.139030722594851,
+            "y": 2.8854588640311274,
+            "heading": -3.6616322031984536,
+            "angularVelocity": -0.4791127998494546,
+            "velocityX": 1.352869097521714,
+            "velocityY": 1.2013582961674927,
+            "timestamp": 2.1885208242068255
+        },
+        {
+            "x": 4.174817002989322,
+            "y": 2.917791762586731,
+            "heading": -3.6747587890471625,
+            "angularVelocity": -0.4221024942061285,
+            "velocityX": 1.150754536402508,
+            "velocityY": 1.0397065377505372,
+            "timestamp": 2.2196189248550704
+        },
+        {
+            "x": 4.204435949248215,
+            "y": 2.9449578523068656,
+            "heading": -3.68595534704571,
+            "angularVelocity": -0.360039930579135,
+            "velocityX": 0.9524358607593557,
+            "velocityY": 0.8735610585165674,
+            "timestamp": 2.2507170255033153
+        },
+        {
+            "x": 4.227986113342503,
+            "y": 2.9668454867866734,
+            "heading": -3.6950940909562924,
+            "angularVelocity": -0.2938682337530664,
+            "velocityX": 0.7572862523234876,
+            "velocityY": 0.7038254434694367,
+            "timestamp": 2.2818151261515602
+        },
+        {
+            "x": 4.245550869753413,
+            "y": 2.983363547472538,
+            "heading": -3.702069156081572,
+            "angularVelocity": -0.22429231946250391,
+            "velocityX": 0.5648176591100355,
+            "velocityY": 0.5311597924485464,
+            "timestamp": 2.312913226799805
+        },
+        {
+            "x": 4.25720158771329,
+            "y": 2.99443633378999,
+            "heading": -3.7067915044128146,
+            "angularVelocity": -0.15185327183332542,
+            "velocityX": 0.3746440366779525,
+            "velocityY": 0.3560598906890862,
+            "timestamp": 2.34401132744805
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -3.7091853071795864,
+            "angularVelocity": -0.07697585115715987,
+            "velocityX": 0.1864555122609252,
+            "velocityY": 0.17890694589168776,
+            "timestamp": 2.375109428096295
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -3.7091853071795864,
+            "angularVelocity": 7.12380386376873e-29,
+            "velocityX": -3.695914984045945e-30,
+            "velocityY": 3.729410348522699e-30,
+            "timestamp": 2.40620752874454
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.traj
new file mode 100644
index 00000000..96fad438
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.traj	
@@ -0,0 +1,544 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -7.419385593511551e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 2.293773644442264,
+            "y": 6.737033643205664,
+            "heading": -2.75526766821854,
+            "angularVelocity": -0.10504784252909477,
+            "velocityX": -0.1241659859778435,
+            "velocityY": -0.3981690982414253,
+            "timestamp": 0.050145420331514534
+        },
+        {
+            "x": 2.2813193646388377,
+            "y": 6.697095621507987,
+            "heading": -2.7655160845701583,
+            "angularVelocity": -0.20437392455513897,
+            "velocityX": -0.24836325473174134,
+            "velocityY": -0.7964440507955471,
+            "timestamp": 0.10029084066302907
+        },
+        {
+            "x": 2.2626352666399625,
+            "y": 6.637179497704236,
+            "heading": -2.7803732946846287,
+            "angularVelocity": -0.2962824923243045,
+            "velocityX": -0.3725982926327797,
+            "velocityY": -1.1948473740501462,
+            "timestamp": 0.1504362609945436
+        },
+        {
+            "x": 2.2377190441074863,
+            "y": 6.557277385764274,
+            "heading": -2.799337789044294,
+            "angularVelocity": -0.3781899570148323,
+            "velocityX": -0.4968793235305074,
+            "velocityY": -1.5934079605220988,
+            "timestamp": 0.20058168132605814
+        },
+        {
+            "x": 2.2065679165597203,
+            "y": 6.457379637066711,
+            "heading": -2.8216967909576067,
+            "angularVelocity": -0.44588322852808987,
+            "velocityX": -0.6212158027956237,
+            "velocityY": -1.9921609598071555,
+            "timestamp": 0.25072710165757267
+        },
+        {
+            "x": 2.1691787676944525,
+            "y": 6.337475004416767,
+            "heading": -2.846358557659031,
+            "angularVelocity": -0.49180496520688655,
+            "velocityX": -0.7456144273611754,
+            "velocityY": -2.3911382506567254,
+            "timestamp": 0.3008725219890872
+        },
+        {
+            "x": 2.1255494213188793,
+            "y": 6.197554115260485,
+            "heading": -2.8714458003624506,
+            "angularVelocity": -0.5002898078741331,
+            "velocityX": -0.8700564495648131,
+            "velocityY": -2.7903024489824935,
+            "timestamp": 0.35101794232060174
+        },
+        {
+            "x": 2.0756883391523315,
+            "y": 6.037638592000563,
+            "heading": -2.8929930081362487,
+            "angularVelocity": -0.4296944293486671,
+            "velocityX": -0.9943297281568805,
+            "velocityY": -3.1890354533417224,
+            "timestamp": 0.4011633626521163
+        },
+        {
+            "x": 2.0198170203632526,
+            "y": 5.858213938792853,
+            "heading": -2.897462778158027,
+            "angularVelocity": -0.08913615624774217,
+            "velocityX": -1.1141858702092737,
+            "velocityY": -3.578086533556236,
+            "timestamp": 0.4513087829836308
+        },
+        {
+            "x": 1.9633339655295539,
+            "y": 5.677120968466723,
+            "heading": -2.897462791142581,
+            "angularVelocity": -2.5893798802100903e-7,
+            "velocityX": -1.1263851107496088,
+            "velocityY": -3.611356114454979,
+            "timestamp": 0.5014542033151453
+        },
+        {
+            "x": 1.9068509267595308,
+            "y": 5.496027993130308,
+            "heading": -2.897462804127121,
+            "angularVelocity": -2.5893770297899326e-7,
+            "velocityX": -1.1263847904077713,
+            "velocityY": -3.611356214370084,
+            "timestamp": 0.5515996236466598
+        },
+        {
+            "x": 1.8503679083113405,
+            "y": 5.314935011455497,
+            "heading": -2.897462817111663,
+            "angularVelocity": -2.589377492115138e-7,
+            "velocityX": -1.1263843851497735,
+            "velocityY": -3.611356340770398,
+            "timestamp": 0.6017450439781744
+        },
+        {
+            "x": 1.7938849123636174,
+            "y": 5.133842022762774,
+            "heading": -2.8974628300962064,
+            "angularVelocity": -2.589377769209582e-7,
+            "velocityX": -1.1263839364454482,
+            "velocityY": -3.6113564807215806,
+            "timestamp": 0.651890464309689
+        },
+        {
+            "x": 1.737401940953919,
+            "y": 4.952749026416629,
+            "heading": -2.89746284308075,
+            "angularVelocity": -2.5893777760216377e-7,
+            "velocityX": -1.126383447108141,
+            "velocityY": -3.611356633346137,
+            "timestamp": 0.7020358846412036
+        },
+        {
+            "x": 1.680918995692082,
+            "y": 4.771656021914957,
+            "heading": -2.897462856065292,
+            "angularVelocity": -2.5893774300527816e-7,
+            "velocityX": -1.1263829256674796,
+            "velocityY": -3.6113567959836606,
+            "timestamp": 0.7521813049727182
+        },
+        {
+            "x": 1.6244360774524895,
+            "y": 4.590563008985036,
+            "heading": -2.8974628690498303,
+            "angularVelocity": -2.589376688920553e-7,
+            "velocityX": -1.1263823867898586,
+            "velocityY": -3.611356964059789,
+            "timestamp": 0.8023267253042328
+        },
+        {
+            "x": 1.567953186210211,
+            "y": 4.40946998763471,
+            "heading": -2.897462882034363,
+            "angularVelocity": -2.589375607053774e-7,
+            "velocityX": -1.1263818484094146,
+            "velocityY": -3.6113571319795557,
+            "timestamp": 0.8524721456357474
+        },
+        {
+            "x": 1.5114708900262606,
+            "y": 4.228376780686611,
+            "heading": -2.8974628950188928,
+            "angularVelocity": -2.589374956200649e-7,
+            "velocityX": -1.1263699817559003,
+            "velocityY": -3.611360833170398,
+            "timestamp": 0.902617565967262
+        },
+        {
+            "x": 1.4675217320832292,
+            "y": 4.043840955415942,
+            "heading": -2.8974629081797687,
+            "angularVelocity": -2.624542007539591e-7,
+            "velocityX": -0.8764341320200526,
+            "velocityY": -3.6800135296641385,
+            "timestamp": 0.9527629862987765
+        },
+        {
+            "x": 1.4442056001491934,
+            "y": 3.855582205665926,
+            "heading": -2.8974629221278954,
+            "angularVelocity": -2.7815354458428775e-7,
+            "velocityX": -0.4649703159309848,
+            "velocityY": -3.75425609169144,
+            "timestamp": 1.0029084066302911
+        },
+        {
+            "x": 1.4418077217209881,
+            "y": 3.665900239807532,
+            "heading": -2.897462937731955,
+            "angularVelocity": -3.1117616956346287e-7,
+            "velocityX": -0.04781849294218802,
+            "velocityY": -3.782637868112273,
+            "timestamp": 1.0530538269618057
+        },
+        {
+            "x": 1.4603574724576953,
+            "y": 3.47711225483097,
+            "heading": -2.8974629563063408,
+            "angularVelocity": -3.7041041196117577e-7,
+            "velocityX": 0.3699191394562724,
+            "velocityY": -3.7648101008720767,
+            "timestamp": 1.1031992472933203
+        },
+        {
+            "x": 1.4996282585911056,
+            "y": 3.2915246350140723,
+            "heading": -2.897463288823342,
+            "angularVelocity": -0.000006631054220103306,
+            "velocityX": 0.7831380388037097,
+            "velocityY": -3.7009884170870753,
+            "timestamp": 1.153344667624835
+        },
+        {
+            "x": 1.5575649679963268,
+            "y": 3.1148801932866275,
+            "heading": -2.9095242713797083,
+            "angularVelocity": -0.24052012081323423,
+            "velocityX": 1.1553738910193128,
+            "velocityY": -3.5226435546783237,
+            "timestamp": 1.2034900879563495
+        },
+        {
+            "x": 1.6317457698728626,
+            "y": 2.9507691275120527,
+            "heading": -2.937069238173179,
+            "angularVelocity": -0.549301743037936,
+            "velocityX": 1.4793135920712512,
+            "velocityY": -3.2727029644905956,
+            "timestamp": 1.253635508287864
+        },
+        {
+            "x": 1.7203254363653924,
+            "y": 2.801423061648199,
+            "heading": -2.9780621265122655,
+            "angularVelocity": -0.8174802019421967,
+            "velocityX": 1.7664557582112876,
+            "velocityY": -2.9782593281004814,
+            "timestamp": 1.3037809286193787
+        },
+        {
+            "x": 1.821600608747065,
+            "y": 2.668597377909185,
+            "heading": -3.0274133717272194,
+            "angularVelocity": -0.9841625593860135,
+            "velocityX": 2.019629543677879,
+            "velocityY": -2.648809858625076,
+            "timestamp": 1.3539263489508933
+        },
+        {
+            "x": 1.9344033083274008,
+            "y": 2.553226314276784,
+            "heading": -3.081880110008977,
+            "angularVelocity": -1.0861757249549924,
+            "velocityX": 2.2495114974525996,
+            "velocityY": -2.300729814800146,
+            "timestamp": 1.4040717692824078
+        },
+        {
+            "x": 2.057902367887511,
+            "y": 2.4558701645332204,
+            "heading": -3.139236203843767,
+            "angularVelocity": -1.1437952549924524,
+            "velocityX": 2.4628183140883095,
+            "velocityY": -1.9414763920600462,
+            "timestamp": 1.4542171896139224
+        },
+        {
+            "x": 2.1914827823638916,
+            "y": 2.376894474029541,
+            "heading": -3.1978794252899205,
+            "angularVelocity": -1.1694631545305318,
+            "velocityX": 2.663860699407278,
+            "velocityY": -1.5749332637270959,
+            "timestamp": 1.504362609945437
+        },
+        {
+            "x": 2.278019971764992,
+            "y": 2.3350837736524683,
+            "heading": -3.2342809833466872,
+            "angularVelocity": -1.1705395923861364,
+            "velocityX": 2.7827162301626456,
+            "velocityY": -1.3444776209968852,
+            "timestamp": 1.535460710593682
+        },
+        {
+            "x": 2.368131283216315,
+            "y": 2.300501166585177,
+            "heading": -3.270450856694399,
+            "angularVelocity": -1.1630894682876274,
+            "velocityX": 2.8976467878404213,
+            "velocityY": -1.112048850135951,
+            "timestamp": 1.5665588112419269
+        },
+        {
+            "x": 2.461670879436653,
+            "y": 2.2732156435859956,
+            "heading": -3.306075840058796,
+            "angularVelocity": -1.1455678199567971,
+            "velocityX": 3.007887757467237,
+            "velocityY": -0.8774015914287735,
+            "timestamp": 1.5976569118901718
+        },
+        {
+            "x": 2.5584617966487637,
+            "y": 2.2533048266167017,
+            "heading": -3.3407803732359653,
+            "angularVelocity": -1.1159695432761514,
+            "velocityX": 3.112438225952366,
+            "velocityY": -0.6402582972673794,
+            "timestamp": 1.6287550125384167
+        },
+        {
+            "x": 2.658285404214264,
+            "y": 2.240855747544563,
+            "heading": -3.37410574490051,
+            "angularVelocity": -1.0716208054472351,
+            "velocityX": 3.209958341013189,
+            "velocityY": -0.40031637986360386,
+            "timestamp": 1.6598531131866616
+        },
+        {
+            "x": 2.7608658878900645,
+            "y": 2.2359648026671843,
+            "heading": -3.405478910014421,
+            "angularVelocity": -1.0088450567700136,
+            "velocityX": 3.2986092892329157,
+            "velocityY": -0.1572747137422285,
+            "timestamp": 1.6909512138349065
+        },
+        {
+            "x": 2.8658468441128373,
+            "y": 2.238735383081805,
+            "heading": -3.4341645997548764,
+            "angularVelocity": -0.9224257797905216,
+            "velocityX": 3.3757996158745374,
+            "velocityY": 0.08909162800513673,
+            "timestamp": 1.7220493144831515
+        },
+        {
+            "x": 2.972755430801249,
+            "y": 2.249269494915596,
+            "heading": -3.459191954885324,
+            "angularVelocity": -0.8047872573808069,
+            "velocityX": 3.4377850884743864,
+            "velocityY": 0.3387381098589673,
+            "timestamp": 1.7531474151313964
+        },
+        {
+            "x": 3.0809481205317057,
+            "y": 2.267644679606423,
+            "heading": -3.4792519494186864,
+            "angularVelocity": -0.6450552964717784,
+            "velocityX": 3.4790770971590295,
+            "velocityY": 0.5908780378155715,
+            "timestamp": 1.7842455157796413
+        },
+        {
+            "x": 3.1896989899339894,
+            "y": 2.293908495297157,
+            "heading": -3.4936234234211128,
+            "angularVelocity": -0.4621334969934767,
+            "velocityX": 3.4970260927630066,
+            "velocityY": 0.8445472599052618,
+            "timestamp": 1.8153436164278862
+        },
+        {
+            "x": 3.298236220991847,
+            "y": 2.3281164444820805,
+            "heading": -3.5030903221852148,
+            "angularVelocity": -0.30442048121156556,
+            "velocityX": 3.4901562730643,
+            "velocityY": 1.1000012371126273,
+            "timestamp": 1.8464417170761311
+        },
+        {
+            "x": 3.405452310949792,
+            "y": 2.3702201329887522,
+            "heading": -3.5091052020695845,
+            "angularVelocity": -0.1934163102887616,
+            "velocityX": 3.447673257305325,
+            "velocityY": 1.3538990365653494,
+            "timestamp": 1.877539817724376
+        },
+        {
+            "x": 3.5097674187642585,
+            "y": 2.419844212041495,
+            "heading": -3.514308835252089,
+            "angularVelocity": -0.1673296141576457,
+            "velocityX": 3.354388391573783,
+            "velocityY": 1.5957270064190303,
+            "timestamp": 1.908637918372621
+        },
+        {
+            "x": 3.6091747208997034,
+            "y": 2.4757866931575614,
+            "heading": -3.523108085575375,
+            "angularVelocity": -0.2829513745169274,
+            "velocityX": 3.196571496756511,
+            "velocityY": 1.7989034683770408,
+            "timestamp": 1.939736019020866
+        },
+        {
+            "x": 3.701528766735335,
+            "y": 2.535299893209444,
+            "heading": -3.5376794547025563,
+            "angularVelocity": -0.4685613855325914,
+            "velocityX": 2.969764838060721,
+            "velocityY": 1.913724594471066,
+            "timestamp": 1.9708341196691108
+        },
+        {
+            "x": 3.7858722644846305,
+            "y": 2.59510740434591,
+            "heading": -3.555385401955535,
+            "angularVelocity": -0.5693578348478764,
+            "velocityX": 2.7121752129918284,
+            "velocityY": 1.923188551383177,
+            "timestamp": 2.001932220317356
+        },
+        {
+            "x": 3.862373728468494,
+            "y": 2.653074903297687,
+            "heading": -3.5744288715420867,
+            "angularVelocity": -0.6123676105476805,
+            "velocityX": 2.460004385772028,
+            "velocityY": 1.8640205589227477,
+            "timestamp": 2.033030320965601
+        },
+        {
+            "x": 3.931417292290283,
+            "y": 2.707976512433947,
+            "heading": -3.5936932545527167,
+            "angularVelocity": -0.6194713699243786,
+            "velocityX": 2.220185875746878,
+            "velocityY": 1.7654328718419663,
+            "timestamp": 2.064128421613846
+        },
+        {
+            "x": 3.9933519727900295,
+            "y": 2.7590743829972735,
+            "heading": -3.612463672222599,
+            "angularVelocity": -0.6035872699166261,
+            "velocityX": 1.991590457574803,
+            "velocityY": 1.643118695295981,
+            "timestamp": 2.0952265222620907
+        },
+        {
+            "x": 4.04845853802061,
+            "y": 2.8058913864006936,
+            "heading": -3.6302579260771157,
+            "angularVelocity": -0.5721974488337824,
+            "velocityX": 1.7720235024607334,
+            "velocityY": 1.5054618265268112,
+            "timestamp": 2.1263246229103356
+        },
+        {
+            "x": 4.096959063236219,
+            "y": 2.8480989028223083,
+            "heading": -3.64673270512686,
+            "angularVelocity": -0.5297680149691044,
+            "velocityX": 1.5595976668866187,
+            "velocityY": 1.3572377586345543,
+            "timestamp": 2.1574227235585806
+        },
+        {
+            "x": 4.139030722594851,
+            "y": 2.8854588640311274,
+            "heading": -3.6616322031984536,
+            "angularVelocity": -0.4791127998494546,
+            "velocityX": 1.352869097521714,
+            "velocityY": 1.2013582961674927,
+            "timestamp": 2.1885208242068255
+        },
+        {
+            "x": 4.174817002989322,
+            "y": 2.917791762586731,
+            "heading": -3.6747587890471625,
+            "angularVelocity": -0.4221024942061285,
+            "velocityX": 1.150754536402508,
+            "velocityY": 1.0397065377505372,
+            "timestamp": 2.2196189248550704
+        },
+        {
+            "x": 4.204435949248215,
+            "y": 2.9449578523068656,
+            "heading": -3.68595534704571,
+            "angularVelocity": -0.360039930579135,
+            "velocityX": 0.9524358607593557,
+            "velocityY": 0.8735610585165674,
+            "timestamp": 2.2507170255033153
+        },
+        {
+            "x": 4.227986113342503,
+            "y": 2.9668454867866734,
+            "heading": -3.6950940909562924,
+            "angularVelocity": -0.2938682337530664,
+            "velocityX": 0.7572862523234876,
+            "velocityY": 0.7038254434694367,
+            "timestamp": 2.2818151261515602
+        },
+        {
+            "x": 4.245550869753413,
+            "y": 2.983363547472538,
+            "heading": -3.702069156081572,
+            "angularVelocity": -0.22429231946250391,
+            "velocityX": 0.5648176591100355,
+            "velocityY": 0.5311597924485464,
+            "timestamp": 2.312913226799805
+        },
+        {
+            "x": 4.25720158771329,
+            "y": 2.99443633378999,
+            "heading": -3.7067915044128146,
+            "angularVelocity": -0.15185327183332542,
+            "velocityX": 0.3746440366779525,
+            "velocityY": 0.3560598906890862,
+            "timestamp": 2.34401132744805
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -3.7091853071795864,
+            "angularVelocity": -0.07697585115715987,
+            "velocityX": 0.1864555122609252,
+            "velocityY": 0.17890694589168776,
+            "timestamp": 2.375109428096295
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -3.7091853071795864,
+            "angularVelocity": 7.12380386376873e-29,
+            "velocityX": -3.695914984045945e-30,
+            "velocityY": 3.729410348522699e-30,
+            "timestamp": 2.40620752874454
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.1.traj
new file mode 100644
index 00000000..40c9a79c
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.1.traj	
@@ -0,0 +1,562 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -1.7629358050488447e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2980097531788157,
+            "y": 6.74279910356074,
+            "heading": -2.762722782141496,
+            "angularVelocity": -0.3022479537187966,
+            "velocityX": -0.047281170298139656,
+            "velocityY": -0.3373626807410066,
+            "timestamp": 0.04209385699707004
+        },
+        {
+            "x": 2.2940230158560544,
+            "y": 6.7143715511474085,
+            "heading": -2.7877986560948775,
+            "angularVelocity": -0.5957133829557797,
+            "velocityX": -0.09471066818701897,
+            "velocityY": -0.6753373162100508,
+            "timestamp": 0.08418771399414007
+        },
+        {
+            "x": 2.2880322840288483,
+            "y": 6.671686949257674,
+            "heading": -2.8247754513459067,
+            "angularVelocity": -0.8784368525222962,
+            "velocityX": -0.14231843443624606,
+            "velocityY": -1.0140339929578401,
+            "timestamp": 0.12628157099121012
+        },
+        {
+            "x": 2.2800276497015446,
+            "y": 6.614707345454749,
+            "heading": -2.873051459668216,
+            "angularVelocity": -1.1468658794006534,
+            "velocityX": -0.19016157934542763,
+            "velocityY": -1.353632284323334,
+            "timestamp": 0.16837542798828015
+        },
+        {
+            "x": 2.269996639934327,
+            "y": 6.543383180902105,
+            "heading": -2.9317676826482244,
+            "angularVelocity": -1.3948881658455714,
+            "velocityX": -0.23830103684524737,
+            "velocityY": -1.694407916993876,
+            "timestamp": 0.21046928498535017
+        },
+        {
+            "x": 2.2579259581538498,
+            "y": 6.457648474415292,
+            "heading": -2.9996334069438224,
+            "angularVelocity": -1.6122477039897383,
+            "velocityX": -0.28675637353255107,
+            "velocityY": -2.0367510274190597,
+            "timestamp": 0.2525631419824202
+        },
+        {
+            "x": 2.2438060571207714,
+            "y": 6.357417527376881,
+            "heading": -3.0746299268265957,
+            "angularVelocity": -1.7816499896408666,
+            "velocityX": -0.3354385185957493,
+            "velocityY": -2.3811300315242536,
+            "timestamp": 0.2946569989794902
+        },
+        {
+            "x": 2.227639514948982,
+            "y": 6.242593213913433,
+            "heading": -3.1534176716951676,
+            "angularVelocity": -1.8717159815993158,
+            "velocityX": -0.3840594168625274,
+            "velocityY": -2.727816400180199,
+            "timestamp": 0.33675085597656024
+        },
+        {
+            "x": 2.2094628615660015,
+            "y": 6.1131506228921415,
+            "heading": -3.2295701531339893,
+            "angularVelocity": -1.8091115158233875,
+            "velocityX": -0.43181249426122753,
+            "velocityY": -3.07509456855668,
+            "timestamp": 0.37884471297363026
+        },
+        {
+            "x": 2.189573480043473,
+            "y": 5.97059741661809,
+            "heading": -3.280208967362098,
+            "angularVelocity": -1.2029977255739333,
+            "velocityX": -0.4725008098904457,
+            "velocityY": -3.386556054580001,
+            "timestamp": 0.4209385699707003
+        },
+        {
+            "x": 2.167754438435126,
+            "y": 5.816653814130607,
+            "heading": -3.2929329955032975,
+            "angularVelocity": -0.3022775542304222,
+            "velocityX": -0.5183426553158416,
+            "velocityY": -3.657151267896363,
+            "timestamp": 0.4630324269677703
+        },
+        {
+            "x": 2.1447655867486377,
+            "y": 5.659083438978025,
+            "heading": -3.2929330394086493,
+            "angularVelocity": -0.0000010430346734701328,
+            "velocityX": -0.5461331730206714,
+            "velocityY": -3.74331045890019,
+            "timestamp": 0.5051262839648404
+        },
+        {
+            "x": 2.1217767257990308,
+            "y": 5.50151306517681,
+            "heading": -3.2929330833137715,
+            "angularVelocity": -0.0000010430292064009232,
+            "velocityX": -0.5461333930793576,
+            "velocityY": -3.7433104267965445,
+            "timestamp": 0.5472201409619104
+        },
+        {
+            "x": 2.098787889918124,
+            "y": 5.3439426877181715,
+            "heading": -3.292933127218878,
+            "angularVelocity": -0.0000010430288283584586,
+            "velocityX": -0.5461327975363938,
+            "velocityY": -3.7433105136838924,
+            "timestamp": 0.5893139979589804
+        },
+        {
+            "x": 2.075799081922294,
+            "y": 5.186372306191217,
+            "heading": -3.292933171123966,
+            "angularVelocity": -0.000001043028392528962,
+            "velocityX": -0.5461321350863673,
+            "velocityY": -3.7433106103325895,
+            "timestamp": 0.6314078549560505
+        },
+        {
+            "x": 2.052810304885704,
+            "y": 5.028801920147444,
+            "heading": -3.292933215029033,
+            "angularVelocity": -0.0000010430278884550004,
+            "velocityX": -0.5461313996051773,
+            "velocityY": -3.7433107176360743,
+            "timestamp": 0.6735017119531205
+        },
+        {
+            "x": 2.029821561935122,
+            "y": 4.8712315291306805,
+            "heading": -3.2929332589340747,
+            "angularVelocity": -0.0000010430273082107483,
+            "velocityX": -0.546130589843121,
+            "velocityY": -3.7433108357766267,
+            "timestamp": 0.7155955689501905
+        },
+        {
+            "x": 2.0068328558598987,
+            "y": 4.71366113273398,
+            "heading": -3.29293330283909,
+            "angularVelocity": -0.0000010430266595669302,
+            "velocityX": -0.5461297138160472,
+            "velocityY": -3.7433109635847206,
+            "timestamp": 0.7576894259472605
+        },
+        {
+            "x": 1.9838441886370295,
+            "y": 4.556090730668921,
+            "heading": -3.2929333467440762,
+            "angularVelocity": -0.0000010430259761326365,
+            "velocityX": -0.5461287908225987,
+            "velocityY": -3.7433110982447326,
+            "timestamp": 0.7997832829443305
+        },
+        {
+            "x": 1.960855561483052,
+            "y": 4.398520322758029,
+            "heading": -3.2929333906490403,
+            "angularVelocity": -0.0000010430254501600513,
+            "velocityX": -0.5461278389285532,
+            "velocityY": -3.7433112371209067,
+            "timestamp": 0.8418771399414006
+        },
+        {
+            "x": 1.9378696268054536,
+            "y": 4.240949522053656,
+            "heading": -3.2929334345540515,
+            "angularVelocity": -0.0000010430265803117889,
+            "velocityX": -0.5460638752870397,
+            "velocityY": -3.7433205684938873,
+            "timestamp": 0.8839709969384706
+        },
+        {
+            "x": 1.9275923145038192,
+            "y": 4.0820429860952725,
+            "heading": -3.292933478993091,
+            "angularVelocity": -0.0000010557131812143582,
+            "velocityX": -0.24415230712523545,
+            "velocityY": -3.7750528769422123,
+            "timestamp": 0.9260648539355406
+        },
+        {
+            "x": 1.9320863630886849,
+            "y": 3.9228678829456647,
+            "heading": -3.29293352534017,
+            "angularVelocity": -0.0000011010413859861484,
+            "velocityX": 0.10676257547932101,
+            "velocityY": -3.781433076106252,
+            "timestamp": 0.9681587109326106
+        },
+        {
+            "x": 1.9513134143780595,
+            "y": 3.764794388276326,
+            "heading": -3.2929335753664444,
+            "angularVelocity": -0.0000011884459622031428,
+            "velocityX": 0.4567662044063322,
+            "velocityY": -3.75526278526441,
+            "timestamp": 1.0102525679296808
+        },
+        {
+            "x": 1.9851080356036048,
+            "y": 3.609183233024402,
+            "heading": -3.292933631555695,
+            "angularVelocity": -0.000001334856299032327,
+            "velocityX": 0.8028397404376079,
+            "velocityY": -3.696766377639273,
+            "timestamp": 1.052346424926751
+        },
+        {
+            "x": 2.033179344139022,
+            "y": 3.457373959395419,
+            "heading": -3.2929336978163843,
+            "angularVelocity": -0.000001574117798079096,
+            "velocityX": 1.142002942110131,
+            "velocityY": -3.6064472219675583,
+            "timestamp": 1.094440281923821
+        },
+        {
+            "x": 2.095113543139509,
+            "y": 3.310673385447386,
+            "heading": -3.29293378107099,
+            "angularVelocity": -0.0000019778326800211228,
+            "velocityX": 1.4713358057162051,
+            "velocityY": -3.485082727350082,
+            "timestamp": 1.1365341389208912
+        },
+        {
+            "x": 2.170377492904663,
+            "y": 3.170344352722168,
+            "heading": -3.292933888363931,
+            "angularVelocity": -0.00000254889782501678,
+            "velocityX": 1.7880031704006887,
+            "velocityY": -3.3337176190574715,
+            "timestamp": 1.1786279959179613
+        },
+        {
+            "x": 2.2421588223512248,
+            "y": 3.0583154434355615,
+            "heading": -3.2929339938603395,
+            "angularVelocity": -0.000002999460293233564,
+            "velocityX": 2.0408775039380207,
+            "velocityY": -3.1851915047625172,
+            "timestamp": 1.2137997930042903
+        },
+        {
+            "x": 2.3224028083906,
+            "y": 2.952183724176483,
+            "heading": -3.2929340733758186,
+            "angularVelocity": -0.000002260773842517862,
+            "velocityX": 2.2814866650804433,
+            "velocityY": -3.0175233582344014,
+            "timestamp": 1.2489715900906193
+        },
+        {
+            "x": 2.4106271869517015,
+            "y": 2.852587036133821,
+            "heading": -3.292934137641087,
+            "angularVelocity": -0.0000018271818267107102,
+            "velocityX": 2.508384156332853,
+            "velocityY": -2.8317201932617277,
+            "timestamp": 1.2841433871769483
+        },
+        {
+            "x": 2.506301732241236,
+            "y": 2.7601239405733753,
+            "heading": -3.29293419233114,
+            "angularVelocity": -0.0000015549405448920558,
+            "velocityX": 2.720206336193231,
+            "velocityY": -2.628898811553339,
+            "timestamp": 1.3193151842632773
+        },
+        {
+            "x": 2.6088514418801845,
+            "y": 2.6753501198214895,
+            "heading": -3.292934240814094,
+            "angularVelocity": -0.0000013784610795710298,
+            "velocityX": 2.9156801225493263,
+            "velocityY": -2.410278341587403,
+            "timestamp": 1.3544869813496063
+        },
+        {
+            "x": 2.717659989787404,
+            "y": 2.5987750311541573,
+            "heading": -3.29293428528508,
+            "angularVelocity": -0.0000012643933456255718,
+            "velocityX": 3.093630605230363,
+            "velocityY": -2.177173048035584,
+            "timestamp": 1.3896587784359353
+        },
+        {
+            "x": 2.8320734217114962,
+            "y": 2.5308588229326605,
+            "heading": -3.2929343273033043,
+            "angularVelocity": -0.0000011946567396881703,
+            "velocityX": 3.252987944951041,
+            "velocityY": -1.9309848756034937,
+            "timestamp": 1.4248305755222643
+        },
+        {
+            "x": 2.951404044154876,
+            "y": 2.472009459742262,
+            "heading": -3.29293436807755,
+            "angularVelocity": -0.000001159288093356335,
+            "velocityX": 3.392792871813832,
+            "velocityY": -1.6731975066827869,
+            "timestamp": 1.4600023726085933
+        },
+        {
+            "x": 3.0749338856232122,
+            "y": 2.4225784637746113,
+            "heading": -3.2929344086367345,
+            "angularVelocity": -0.0000011531735020269185,
+            "velocityX": 3.512184525719075,
+            "velocityY": -1.4054157041314157,
+            "timestamp": 1.4951741696949223
+        },
+        {
+            "x": 3.199149697823942,
+            "y": 2.374897470692157,
+            "heading": -3.2929344492220305,
+            "angularVelocity": -0.000001153915896405561,
+            "velocityX": 3.531687957139154,
+            "velocityY": -1.3556598477303206,
+            "timestamp": 1.5303459667812513
+        },
+        {
+            "x": 3.3233655700450013,
+            "y": 2.3272166339715414,
+            "heading": -3.2929344898073354,
+            "angularVelocity": -0.0000011539161555742895,
+            "velocityX": 3.5316896636294053,
+            "velocityY": -1.3556554020706744,
+            "timestamp": 1.5655177638675803
+        },
+        {
+            "x": 3.4475814520083037,
+            "y": 2.279535822630966,
+            "heading": -3.2929345303926483,
+            "angularVelocity": -0.0000011539163817435631,
+            "velocityX": 3.531689940619614,
+            "velocityY": -1.3556546804686425,
+            "timestamp": 1.6006895609539094
+        },
+        {
+            "x": 3.571797343695941,
+            "y": 2.231855036623794,
+            "heading": -3.2929345709779727,
+            "angularVelocity": -0.0000011539167138358367,
+            "velocityX": 3.53169021710065,
+            "velocityY": -1.3556539601925897,
+            "timestamp": 1.6358613580402384
+        },
+        {
+            "x": 3.696013245125145,
+            "y": 2.1841742759949323,
+            "heading": -3.2929346115633122,
+            "angularVelocity": -0.0000011539171376230348,
+            "velocityX": 3.531690494071608,
+            "velocityY": -1.355653238639736,
+            "timestamp": 1.6710331551265674
+        },
+        {
+            "x": 3.8202291563227684,
+            "y": 2.1364935408143535,
+            "heading": -3.292934652148666,
+            "angularVelocity": -0.0000011539175501305985,
+            "velocityX": 3.531690771806052,
+            "velocityY": -1.3556525150974317,
+            "timestamp": 1.7062049522128964
+        },
+        {
+            "x": 3.9444450773804367,
+            "y": 2.0888128313207726,
+            "heading": -3.292934692734033,
+            "angularVelocity": -0.0000011539179284682577,
+            "velocityX": 3.5316910521455807,
+            "velocityY": -1.355651784768035,
+            "timestamp": 1.7413767492992254
+        },
+        {
+            "x": 4.068661008518651,
+            "y": 2.041132148088645,
+            "heading": -3.292934733319412,
+            "angularVelocity": -0.0000011539182628370223,
+            "velocityX": 3.531691338754367,
+            "velocityY": -1.3556510381057705,
+            "timestamp": 1.7765485463855544
+        },
+        {
+            "x": 4.192876950169643,
+            "y": 1.9934514922440194,
+            "heading": -3.2929347739048014,
+            "angularVelocity": -0.0000011539185452860982,
+            "velocityX": 3.5316916376522904,
+            "velocityY": -1.3556502594278441,
+            "timestamp": 1.8117203434718834
+        },
+        {
+            "x": 4.317092949197956,
+            "y": 1.945770985877502,
+            "heading": -3.292934814490671,
+            "angularVelocity": -0.0000011539322132251563,
+            "velocityX": 3.5316932689969183,
+            "velocityY": -1.3556460094855505,
+            "timestamp": 1.8468921405582124
+        },
+        {
+            "x": 4.4381547246220405,
+            "y": 1.900646588966398,
+            "heading": -3.304224160857315,
+            "angularVelocity": -0.3209772403421673,
+            "velocityX": 3.442012790160775,
+            "velocityY": -1.2829710350126964,
+            "timestamp": 1.8820639376445414
+        },
+        {
+            "x": 4.551299156991204,
+            "y": 1.858106592529644,
+            "heading": -3.340115682679576,
+            "angularVelocity": -1.0204631208967,
+            "velocityX": 3.2169079132195413,
+            "velocityY": -1.2094916939370357,
+            "timestamp": 1.9172357347308704
+        },
+        {
+            "x": 4.6559354752004225,
+            "y": 1.8184604535185338,
+            "heading": -3.3958506942974975,
+            "angularVelocity": -1.5846506643126916,
+            "velocityX": 2.9750063083893172,
+            "velocityY": -1.127213912721008,
+            "timestamp": 1.9524075318171994
+        },
+        {
+            "x": 4.750941844820644,
+            "y": 1.7823637508813854,
+            "heading": -3.4552749718768117,
+            "angularVelocity": -1.6895433984637673,
+            "velocityX": 2.7012088517123067,
+            "velocityY": -1.0262967953712765,
+            "timestamp": 1.9875793289035284
+        },
+        {
+            "x": 4.836288167632038,
+            "y": 1.7498748942497964,
+            "heading": -3.5138218394398413,
+            "angularVelocity": -1.664596990006722,
+            "velocityX": 2.4265556463296942,
+            "velocityY": -0.9237189829068263,
+            "timestamp": 2.0227511259898576
+        },
+        {
+            "x": 4.9120167698263195,
+            "y": 1.7210024537073532,
+            "heading": -3.5691368807810755,
+            "angularVelocity": -1.57271012355337,
+            "velocityX": 2.153105853772738,
+            "velocityY": -0.820897506930789,
+            "timestamp": 2.0579229230761866
+        },
+        {
+            "x": 4.9781740579406755,
+            "y": 1.695744953740728,
+            "heading": -3.6197442140788145,
+            "angularVelocity": -1.438861175433386,
+            "velocityX": 1.8809754858977845,
+            "velocityY": -0.718117982559453,
+            "timestamp": 2.0930947201625156
+        },
+        {
+            "x": 5.034800882324283,
+            "y": 1.6740995356540391,
+            "heading": -3.6646343712421663,
+            "angularVelocity": -1.2763111607055304,
+            "velocityX": 1.6100065698837365,
+            "velocityY": -0.6154197362608602,
+            "timestamp": 2.1282665172488446
+        },
+        {
+            "x": 5.081931555960545,
+            "y": 1.6560639982794174,
+            "heading": -3.703081117816373,
+            "angularVelocity": -1.0931129415946466,
+            "velocityX": 1.3400132361897832,
+            "velocityY": -0.5127840732833052,
+            "timestamp": 2.1634383143351736
+        },
+        {
+            "x": 5.11959462557562,
+            "y": 1.64163698634227,
+            "heading": -3.734541789094988,
+            "angularVelocity": -0.8944857495167222,
+            "velocityX": 1.0708315393333863,
+            "velocityY": -0.41018694329825167,
+            "timestamp": 2.1986101114215026
+        },
+        {
+            "x": 5.1478139750030305,
+            "y": 1.6308177057465072,
+            "heading": -3.758596048910276,
+            "angularVelocity": -0.6839076137124058,
+            "velocityX": 0.8023289045523005,
+            "velocityY": -0.30761239095081433,
+            "timestamp": 2.2337819085078316
+        },
+        {
+            "x": 5.166609893194281,
+            "y": 1.6236055874501054,
+            "heading": -3.7749054122717336,
+            "angularVelocity": -0.4637057162995195,
+            "velocityX": 0.5344031226245449,
+            "velocityY": -0.20505401753284183,
+            "timestamp": 2.2689537055941607
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -3.7831853071795862,
+            "angularVelocity": -0.2354129044793956,
+            "velocityX": 0.2669783060180454,
+            "velocityY": -0.10251359750698616,
+            "timestamp": 2.3041255026804897
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -3.7831853071795862,
+            "angularVelocity": -9.348588807340857e-32,
+            "velocityX": 3.3761299345246814e-33,
+            "velocityY": 7.633245142283867e-33,
+            "timestamp": 2.3392972997668187
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.traj
new file mode 100644
index 00000000..40c9a79c
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.traj	
@@ -0,0 +1,562 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -1.7629358050488447e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2980097531788157,
+            "y": 6.74279910356074,
+            "heading": -2.762722782141496,
+            "angularVelocity": -0.3022479537187966,
+            "velocityX": -0.047281170298139656,
+            "velocityY": -0.3373626807410066,
+            "timestamp": 0.04209385699707004
+        },
+        {
+            "x": 2.2940230158560544,
+            "y": 6.7143715511474085,
+            "heading": -2.7877986560948775,
+            "angularVelocity": -0.5957133829557797,
+            "velocityX": -0.09471066818701897,
+            "velocityY": -0.6753373162100508,
+            "timestamp": 0.08418771399414007
+        },
+        {
+            "x": 2.2880322840288483,
+            "y": 6.671686949257674,
+            "heading": -2.8247754513459067,
+            "angularVelocity": -0.8784368525222962,
+            "velocityX": -0.14231843443624606,
+            "velocityY": -1.0140339929578401,
+            "timestamp": 0.12628157099121012
+        },
+        {
+            "x": 2.2800276497015446,
+            "y": 6.614707345454749,
+            "heading": -2.873051459668216,
+            "angularVelocity": -1.1468658794006534,
+            "velocityX": -0.19016157934542763,
+            "velocityY": -1.353632284323334,
+            "timestamp": 0.16837542798828015
+        },
+        {
+            "x": 2.269996639934327,
+            "y": 6.543383180902105,
+            "heading": -2.9317676826482244,
+            "angularVelocity": -1.3948881658455714,
+            "velocityX": -0.23830103684524737,
+            "velocityY": -1.694407916993876,
+            "timestamp": 0.21046928498535017
+        },
+        {
+            "x": 2.2579259581538498,
+            "y": 6.457648474415292,
+            "heading": -2.9996334069438224,
+            "angularVelocity": -1.6122477039897383,
+            "velocityX": -0.28675637353255107,
+            "velocityY": -2.0367510274190597,
+            "timestamp": 0.2525631419824202
+        },
+        {
+            "x": 2.2438060571207714,
+            "y": 6.357417527376881,
+            "heading": -3.0746299268265957,
+            "angularVelocity": -1.7816499896408666,
+            "velocityX": -0.3354385185957493,
+            "velocityY": -2.3811300315242536,
+            "timestamp": 0.2946569989794902
+        },
+        {
+            "x": 2.227639514948982,
+            "y": 6.242593213913433,
+            "heading": -3.1534176716951676,
+            "angularVelocity": -1.8717159815993158,
+            "velocityX": -0.3840594168625274,
+            "velocityY": -2.727816400180199,
+            "timestamp": 0.33675085597656024
+        },
+        {
+            "x": 2.2094628615660015,
+            "y": 6.1131506228921415,
+            "heading": -3.2295701531339893,
+            "angularVelocity": -1.8091115158233875,
+            "velocityX": -0.43181249426122753,
+            "velocityY": -3.07509456855668,
+            "timestamp": 0.37884471297363026
+        },
+        {
+            "x": 2.189573480043473,
+            "y": 5.97059741661809,
+            "heading": -3.280208967362098,
+            "angularVelocity": -1.2029977255739333,
+            "velocityX": -0.4725008098904457,
+            "velocityY": -3.386556054580001,
+            "timestamp": 0.4209385699707003
+        },
+        {
+            "x": 2.167754438435126,
+            "y": 5.816653814130607,
+            "heading": -3.2929329955032975,
+            "angularVelocity": -0.3022775542304222,
+            "velocityX": -0.5183426553158416,
+            "velocityY": -3.657151267896363,
+            "timestamp": 0.4630324269677703
+        },
+        {
+            "x": 2.1447655867486377,
+            "y": 5.659083438978025,
+            "heading": -3.2929330394086493,
+            "angularVelocity": -0.0000010430346734701328,
+            "velocityX": -0.5461331730206714,
+            "velocityY": -3.74331045890019,
+            "timestamp": 0.5051262839648404
+        },
+        {
+            "x": 2.1217767257990308,
+            "y": 5.50151306517681,
+            "heading": -3.2929330833137715,
+            "angularVelocity": -0.0000010430292064009232,
+            "velocityX": -0.5461333930793576,
+            "velocityY": -3.7433104267965445,
+            "timestamp": 0.5472201409619104
+        },
+        {
+            "x": 2.098787889918124,
+            "y": 5.3439426877181715,
+            "heading": -3.292933127218878,
+            "angularVelocity": -0.0000010430288283584586,
+            "velocityX": -0.5461327975363938,
+            "velocityY": -3.7433105136838924,
+            "timestamp": 0.5893139979589804
+        },
+        {
+            "x": 2.075799081922294,
+            "y": 5.186372306191217,
+            "heading": -3.292933171123966,
+            "angularVelocity": -0.000001043028392528962,
+            "velocityX": -0.5461321350863673,
+            "velocityY": -3.7433106103325895,
+            "timestamp": 0.6314078549560505
+        },
+        {
+            "x": 2.052810304885704,
+            "y": 5.028801920147444,
+            "heading": -3.292933215029033,
+            "angularVelocity": -0.0000010430278884550004,
+            "velocityX": -0.5461313996051773,
+            "velocityY": -3.7433107176360743,
+            "timestamp": 0.6735017119531205
+        },
+        {
+            "x": 2.029821561935122,
+            "y": 4.8712315291306805,
+            "heading": -3.2929332589340747,
+            "angularVelocity": -0.0000010430273082107483,
+            "velocityX": -0.546130589843121,
+            "velocityY": -3.7433108357766267,
+            "timestamp": 0.7155955689501905
+        },
+        {
+            "x": 2.0068328558598987,
+            "y": 4.71366113273398,
+            "heading": -3.29293330283909,
+            "angularVelocity": -0.0000010430266595669302,
+            "velocityX": -0.5461297138160472,
+            "velocityY": -3.7433109635847206,
+            "timestamp": 0.7576894259472605
+        },
+        {
+            "x": 1.9838441886370295,
+            "y": 4.556090730668921,
+            "heading": -3.2929333467440762,
+            "angularVelocity": -0.0000010430259761326365,
+            "velocityX": -0.5461287908225987,
+            "velocityY": -3.7433110982447326,
+            "timestamp": 0.7997832829443305
+        },
+        {
+            "x": 1.960855561483052,
+            "y": 4.398520322758029,
+            "heading": -3.2929333906490403,
+            "angularVelocity": -0.0000010430254501600513,
+            "velocityX": -0.5461278389285532,
+            "velocityY": -3.7433112371209067,
+            "timestamp": 0.8418771399414006
+        },
+        {
+            "x": 1.9378696268054536,
+            "y": 4.240949522053656,
+            "heading": -3.2929334345540515,
+            "angularVelocity": -0.0000010430265803117889,
+            "velocityX": -0.5460638752870397,
+            "velocityY": -3.7433205684938873,
+            "timestamp": 0.8839709969384706
+        },
+        {
+            "x": 1.9275923145038192,
+            "y": 4.0820429860952725,
+            "heading": -3.292933478993091,
+            "angularVelocity": -0.0000010557131812143582,
+            "velocityX": -0.24415230712523545,
+            "velocityY": -3.7750528769422123,
+            "timestamp": 0.9260648539355406
+        },
+        {
+            "x": 1.9320863630886849,
+            "y": 3.9228678829456647,
+            "heading": -3.29293352534017,
+            "angularVelocity": -0.0000011010413859861484,
+            "velocityX": 0.10676257547932101,
+            "velocityY": -3.781433076106252,
+            "timestamp": 0.9681587109326106
+        },
+        {
+            "x": 1.9513134143780595,
+            "y": 3.764794388276326,
+            "heading": -3.2929335753664444,
+            "angularVelocity": -0.0000011884459622031428,
+            "velocityX": 0.4567662044063322,
+            "velocityY": -3.75526278526441,
+            "timestamp": 1.0102525679296808
+        },
+        {
+            "x": 1.9851080356036048,
+            "y": 3.609183233024402,
+            "heading": -3.292933631555695,
+            "angularVelocity": -0.000001334856299032327,
+            "velocityX": 0.8028397404376079,
+            "velocityY": -3.696766377639273,
+            "timestamp": 1.052346424926751
+        },
+        {
+            "x": 2.033179344139022,
+            "y": 3.457373959395419,
+            "heading": -3.2929336978163843,
+            "angularVelocity": -0.000001574117798079096,
+            "velocityX": 1.142002942110131,
+            "velocityY": -3.6064472219675583,
+            "timestamp": 1.094440281923821
+        },
+        {
+            "x": 2.095113543139509,
+            "y": 3.310673385447386,
+            "heading": -3.29293378107099,
+            "angularVelocity": -0.0000019778326800211228,
+            "velocityX": 1.4713358057162051,
+            "velocityY": -3.485082727350082,
+            "timestamp": 1.1365341389208912
+        },
+        {
+            "x": 2.170377492904663,
+            "y": 3.170344352722168,
+            "heading": -3.292933888363931,
+            "angularVelocity": -0.00000254889782501678,
+            "velocityX": 1.7880031704006887,
+            "velocityY": -3.3337176190574715,
+            "timestamp": 1.1786279959179613
+        },
+        {
+            "x": 2.2421588223512248,
+            "y": 3.0583154434355615,
+            "heading": -3.2929339938603395,
+            "angularVelocity": -0.000002999460293233564,
+            "velocityX": 2.0408775039380207,
+            "velocityY": -3.1851915047625172,
+            "timestamp": 1.2137997930042903
+        },
+        {
+            "x": 2.3224028083906,
+            "y": 2.952183724176483,
+            "heading": -3.2929340733758186,
+            "angularVelocity": -0.000002260773842517862,
+            "velocityX": 2.2814866650804433,
+            "velocityY": -3.0175233582344014,
+            "timestamp": 1.2489715900906193
+        },
+        {
+            "x": 2.4106271869517015,
+            "y": 2.852587036133821,
+            "heading": -3.292934137641087,
+            "angularVelocity": -0.0000018271818267107102,
+            "velocityX": 2.508384156332853,
+            "velocityY": -2.8317201932617277,
+            "timestamp": 1.2841433871769483
+        },
+        {
+            "x": 2.506301732241236,
+            "y": 2.7601239405733753,
+            "heading": -3.29293419233114,
+            "angularVelocity": -0.0000015549405448920558,
+            "velocityX": 2.720206336193231,
+            "velocityY": -2.628898811553339,
+            "timestamp": 1.3193151842632773
+        },
+        {
+            "x": 2.6088514418801845,
+            "y": 2.6753501198214895,
+            "heading": -3.292934240814094,
+            "angularVelocity": -0.0000013784610795710298,
+            "velocityX": 2.9156801225493263,
+            "velocityY": -2.410278341587403,
+            "timestamp": 1.3544869813496063
+        },
+        {
+            "x": 2.717659989787404,
+            "y": 2.5987750311541573,
+            "heading": -3.29293428528508,
+            "angularVelocity": -0.0000012643933456255718,
+            "velocityX": 3.093630605230363,
+            "velocityY": -2.177173048035584,
+            "timestamp": 1.3896587784359353
+        },
+        {
+            "x": 2.8320734217114962,
+            "y": 2.5308588229326605,
+            "heading": -3.2929343273033043,
+            "angularVelocity": -0.0000011946567396881703,
+            "velocityX": 3.252987944951041,
+            "velocityY": -1.9309848756034937,
+            "timestamp": 1.4248305755222643
+        },
+        {
+            "x": 2.951404044154876,
+            "y": 2.472009459742262,
+            "heading": -3.29293436807755,
+            "angularVelocity": -0.000001159288093356335,
+            "velocityX": 3.392792871813832,
+            "velocityY": -1.6731975066827869,
+            "timestamp": 1.4600023726085933
+        },
+        {
+            "x": 3.0749338856232122,
+            "y": 2.4225784637746113,
+            "heading": -3.2929344086367345,
+            "angularVelocity": -0.0000011531735020269185,
+            "velocityX": 3.512184525719075,
+            "velocityY": -1.4054157041314157,
+            "timestamp": 1.4951741696949223
+        },
+        {
+            "x": 3.199149697823942,
+            "y": 2.374897470692157,
+            "heading": -3.2929344492220305,
+            "angularVelocity": -0.000001153915896405561,
+            "velocityX": 3.531687957139154,
+            "velocityY": -1.3556598477303206,
+            "timestamp": 1.5303459667812513
+        },
+        {
+            "x": 3.3233655700450013,
+            "y": 2.3272166339715414,
+            "heading": -3.2929344898073354,
+            "angularVelocity": -0.0000011539161555742895,
+            "velocityX": 3.5316896636294053,
+            "velocityY": -1.3556554020706744,
+            "timestamp": 1.5655177638675803
+        },
+        {
+            "x": 3.4475814520083037,
+            "y": 2.279535822630966,
+            "heading": -3.2929345303926483,
+            "angularVelocity": -0.0000011539163817435631,
+            "velocityX": 3.531689940619614,
+            "velocityY": -1.3556546804686425,
+            "timestamp": 1.6006895609539094
+        },
+        {
+            "x": 3.571797343695941,
+            "y": 2.231855036623794,
+            "heading": -3.2929345709779727,
+            "angularVelocity": -0.0000011539167138358367,
+            "velocityX": 3.53169021710065,
+            "velocityY": -1.3556539601925897,
+            "timestamp": 1.6358613580402384
+        },
+        {
+            "x": 3.696013245125145,
+            "y": 2.1841742759949323,
+            "heading": -3.2929346115633122,
+            "angularVelocity": -0.0000011539171376230348,
+            "velocityX": 3.531690494071608,
+            "velocityY": -1.355653238639736,
+            "timestamp": 1.6710331551265674
+        },
+        {
+            "x": 3.8202291563227684,
+            "y": 2.1364935408143535,
+            "heading": -3.292934652148666,
+            "angularVelocity": -0.0000011539175501305985,
+            "velocityX": 3.531690771806052,
+            "velocityY": -1.3556525150974317,
+            "timestamp": 1.7062049522128964
+        },
+        {
+            "x": 3.9444450773804367,
+            "y": 2.0888128313207726,
+            "heading": -3.292934692734033,
+            "angularVelocity": -0.0000011539179284682577,
+            "velocityX": 3.5316910521455807,
+            "velocityY": -1.355651784768035,
+            "timestamp": 1.7413767492992254
+        },
+        {
+            "x": 4.068661008518651,
+            "y": 2.041132148088645,
+            "heading": -3.292934733319412,
+            "angularVelocity": -0.0000011539182628370223,
+            "velocityX": 3.531691338754367,
+            "velocityY": -1.3556510381057705,
+            "timestamp": 1.7765485463855544
+        },
+        {
+            "x": 4.192876950169643,
+            "y": 1.9934514922440194,
+            "heading": -3.2929347739048014,
+            "angularVelocity": -0.0000011539185452860982,
+            "velocityX": 3.5316916376522904,
+            "velocityY": -1.3556502594278441,
+            "timestamp": 1.8117203434718834
+        },
+        {
+            "x": 4.317092949197956,
+            "y": 1.945770985877502,
+            "heading": -3.292934814490671,
+            "angularVelocity": -0.0000011539322132251563,
+            "velocityX": 3.5316932689969183,
+            "velocityY": -1.3556460094855505,
+            "timestamp": 1.8468921405582124
+        },
+        {
+            "x": 4.4381547246220405,
+            "y": 1.900646588966398,
+            "heading": -3.304224160857315,
+            "angularVelocity": -0.3209772403421673,
+            "velocityX": 3.442012790160775,
+            "velocityY": -1.2829710350126964,
+            "timestamp": 1.8820639376445414
+        },
+        {
+            "x": 4.551299156991204,
+            "y": 1.858106592529644,
+            "heading": -3.340115682679576,
+            "angularVelocity": -1.0204631208967,
+            "velocityX": 3.2169079132195413,
+            "velocityY": -1.2094916939370357,
+            "timestamp": 1.9172357347308704
+        },
+        {
+            "x": 4.6559354752004225,
+            "y": 1.8184604535185338,
+            "heading": -3.3958506942974975,
+            "angularVelocity": -1.5846506643126916,
+            "velocityX": 2.9750063083893172,
+            "velocityY": -1.127213912721008,
+            "timestamp": 1.9524075318171994
+        },
+        {
+            "x": 4.750941844820644,
+            "y": 1.7823637508813854,
+            "heading": -3.4552749718768117,
+            "angularVelocity": -1.6895433984637673,
+            "velocityX": 2.7012088517123067,
+            "velocityY": -1.0262967953712765,
+            "timestamp": 1.9875793289035284
+        },
+        {
+            "x": 4.836288167632038,
+            "y": 1.7498748942497964,
+            "heading": -3.5138218394398413,
+            "angularVelocity": -1.664596990006722,
+            "velocityX": 2.4265556463296942,
+            "velocityY": -0.9237189829068263,
+            "timestamp": 2.0227511259898576
+        },
+        {
+            "x": 4.9120167698263195,
+            "y": 1.7210024537073532,
+            "heading": -3.5691368807810755,
+            "angularVelocity": -1.57271012355337,
+            "velocityX": 2.153105853772738,
+            "velocityY": -0.820897506930789,
+            "timestamp": 2.0579229230761866
+        },
+        {
+            "x": 4.9781740579406755,
+            "y": 1.695744953740728,
+            "heading": -3.6197442140788145,
+            "angularVelocity": -1.438861175433386,
+            "velocityX": 1.8809754858977845,
+            "velocityY": -0.718117982559453,
+            "timestamp": 2.0930947201625156
+        },
+        {
+            "x": 5.034800882324283,
+            "y": 1.6740995356540391,
+            "heading": -3.6646343712421663,
+            "angularVelocity": -1.2763111607055304,
+            "velocityX": 1.6100065698837365,
+            "velocityY": -0.6154197362608602,
+            "timestamp": 2.1282665172488446
+        },
+        {
+            "x": 5.081931555960545,
+            "y": 1.6560639982794174,
+            "heading": -3.703081117816373,
+            "angularVelocity": -1.0931129415946466,
+            "velocityX": 1.3400132361897832,
+            "velocityY": -0.5127840732833052,
+            "timestamp": 2.1634383143351736
+        },
+        {
+            "x": 5.11959462557562,
+            "y": 1.64163698634227,
+            "heading": -3.734541789094988,
+            "angularVelocity": -0.8944857495167222,
+            "velocityX": 1.0708315393333863,
+            "velocityY": -0.41018694329825167,
+            "timestamp": 2.1986101114215026
+        },
+        {
+            "x": 5.1478139750030305,
+            "y": 1.6308177057465072,
+            "heading": -3.758596048910276,
+            "angularVelocity": -0.6839076137124058,
+            "velocityX": 0.8023289045523005,
+            "velocityY": -0.30761239095081433,
+            "timestamp": 2.2337819085078316
+        },
+        {
+            "x": 5.166609893194281,
+            "y": 1.6236055874501054,
+            "heading": -3.7749054122717336,
+            "angularVelocity": -0.4637057162995195,
+            "velocityX": 0.5344031226245449,
+            "velocityY": -0.20505401753284183,
+            "timestamp": 2.2689537055941607
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -3.7831853071795862,
+            "angularVelocity": -0.2354129044793956,
+            "velocityX": 0.2669783060180454,
+            "velocityY": -0.10251359750698616,
+            "timestamp": 2.3041255026804897
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -3.7831853071795862,
+            "angularVelocity": -9.348588807340857e-32,
+            "velocityX": 3.3761299345246814e-33,
+            "velocityY": 7.633245142283867e-33,
+            "timestamp": 2.3392972997668187
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.1.traj
new file mode 100644
index 00000000..f0ca2992
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.1.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2980013304879616,
+            "y": 6.709118488011682,
+            "heading": -2.76564785007466,
+            "angularVelocity": -0.20597421909793953,
+            "velocityX": -0.026308687136743093,
+            "velocityY": -0.6302691420205844,
+            "timestamp": 0.07596994489499284
+        },
+        {
+            "x": 2.294004010549845,
+            "y": 6.6133557271698225,
+            "heading": -2.796958855018623,
+            "angularVelocity": -0.41214989674195696,
+            "velocityX": -0.05261712304308655,
+            "velocityY": -1.2605348203717124,
+            "timestamp": 0.15193988978998568
+        },
+        {
+            "x": 2.2880079544710785,
+            "y": 6.469712199120552,
+            "heading": -2.8439591282551087,
+            "angularVelocity": -0.6186693079934498,
+            "velocityX": -0.07892668721893853,
+            "velocityY": -1.8907941587665607,
+            "timestamp": 0.22790983468497852
+        },
+        {
+            "x": 2.2800128266474267,
+            "y": 6.278188582998367,
+            "heading": -2.9066839984131234,
+            "angularVelocity": -0.8256537535299542,
+            "velocityX": -0.10524066898696272,
+            "velocityY": -2.5210445576459732,
+            "timestamp": 0.30387977957997137
+        },
+        {
+            "x": 2.2700178539327394,
+            "y": 6.038785706839934,
+            "heading": -2.985174219418862,
+            "angularVelocity": -1.0331746470822045,
+            "velocityX": -0.13156482775526857,
+            "velocityY": -3.1512840569956957,
+            "timestamp": 0.37984972447496423
+        },
+        {
+            "x": 2.2620112641506833,
+            "y": 5.847271276792759,
+            "heading": -3.048371938278793,
+            "angularVelocity": -0.8318779083923777,
+            "velocityX": -0.10539154389440776,
+            "velocityY": -2.520923640419774,
+            "timestamp": 0.4558196693699571
+        },
+        {
+            "x": 2.256005772930322,
+            "y": 5.703635612537123,
+            "heading": -3.0957774424626403,
+            "angularVelocity": -0.624003403574559,
+            "velocityX": -0.0790508829335374,
+            "velocityY": -1.8906906468626763,
+            "timestamp": 0.53178961426495
+        },
+        {
+            "x": 2.25200192771716,
+            "y": 5.607878538669833,
+            "heading": -3.127382702013982,
+            "angularVelocity": -0.4160231996354262,
+            "velocityX": -0.05270301589261462,
+            "velocityY": -1.260459962155393,
+            "timestamp": 0.6077595591599428
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.20801127587293636,
+            "velocityX": -0.026351575217375103,
+            "velocityY": -0.6302300039313167,
+            "timestamp": 0.6837295040549357
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": 2.276477315177118e-32,
+            "velocityX": 0,
+            "velocityY": -9.616776104992252e-34,
+            "timestamp": 0.7596994489499286
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.traj
new file mode 100644
index 00000000..f0ca2992
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": -2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2980013304879616,
+            "y": 6.709118488011682,
+            "heading": -2.76564785007466,
+            "angularVelocity": -0.20597421909793953,
+            "velocityX": -0.026308687136743093,
+            "velocityY": -0.6302691420205844,
+            "timestamp": 0.07596994489499284
+        },
+        {
+            "x": 2.294004010549845,
+            "y": 6.6133557271698225,
+            "heading": -2.796958855018623,
+            "angularVelocity": -0.41214989674195696,
+            "velocityX": -0.05261712304308655,
+            "velocityY": -1.2605348203717124,
+            "timestamp": 0.15193988978998568
+        },
+        {
+            "x": 2.2880079544710785,
+            "y": 6.469712199120552,
+            "heading": -2.8439591282551087,
+            "angularVelocity": -0.6186693079934498,
+            "velocityX": -0.07892668721893853,
+            "velocityY": -1.8907941587665607,
+            "timestamp": 0.22790983468497852
+        },
+        {
+            "x": 2.2800128266474267,
+            "y": 6.278188582998367,
+            "heading": -2.9066839984131234,
+            "angularVelocity": -0.8256537535299542,
+            "velocityX": -0.10524066898696272,
+            "velocityY": -2.5210445576459732,
+            "timestamp": 0.30387977957997137
+        },
+        {
+            "x": 2.2700178539327394,
+            "y": 6.038785706839934,
+            "heading": -2.985174219418862,
+            "angularVelocity": -1.0331746470822045,
+            "velocityX": -0.13156482775526857,
+            "velocityY": -3.1512840569956957,
+            "timestamp": 0.37984972447496423
+        },
+        {
+            "x": 2.2620112641506833,
+            "y": 5.847271276792759,
+            "heading": -3.048371938278793,
+            "angularVelocity": -0.8318779083923777,
+            "velocityX": -0.10539154389440776,
+            "velocityY": -2.520923640419774,
+            "timestamp": 0.4558196693699571
+        },
+        {
+            "x": 2.256005772930322,
+            "y": 5.703635612537123,
+            "heading": -3.0957774424626403,
+            "angularVelocity": -0.624003403574559,
+            "velocityX": -0.0790508829335374,
+            "velocityY": -1.8906906468626763,
+            "timestamp": 0.53178961426495
+        },
+        {
+            "x": 2.25200192771716,
+            "y": 5.607878538669833,
+            "heading": -3.127382702013982,
+            "angularVelocity": -0.4160231996354262,
+            "velocityX": -0.05270301589261462,
+            "velocityY": -1.260459962155393,
+            "timestamp": 0.6077595591599428
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": -0.20801127587293636,
+            "velocityX": -0.026351575217375103,
+            "velocityY": -0.6302300039313167,
+            "timestamp": 0.6837295040549357
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -3.143185307179586,
+            "angularVelocity": 2.276477315177118e-32,
+            "velocityX": 0,
+            "velocityY": -9.616776104992252e-34,
+            "timestamp": 0.7596994489499286
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.1.traj
new file mode 100644
index 00000000..ae5af9d6
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.1.traj	
@@ -0,0 +1,526 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 4.173822195239149e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 2.260404640394534,
+            "y": 5.564834920701341,
+            "heading": 3.1398005529486164,
+            "angularVelocity": -0.005376730415880204,
+            "velocityX": 0.2804902157612115,
+            "velocityY": 0.1303406844718941,
+            "timestamp": 0.037094486045788126
+        },
+        {
+            "x": 2.2812139214883365,
+            "y": 5.574504760754136,
+            "heading": 3.139401659951885,
+            "angularVelocity": -0.010753431015026859,
+            "velocityX": 0.5609804397374861,
+            "velocityY": 0.26068133255328624,
+            "timestamp": 0.07418897209157625
+        },
+        {
+            "x": 2.3124278435365335,
+            "y": 5.58900951877657,
+            "heading": 3.1388033223810634,
+            "angularVelocity": -0.016130094647582943,
+            "velocityX": 0.8414706705915215,
+            "velocityY": 0.3910219433834154,
+            "timestamp": 0.11128345813736437
+        },
+        {
+            "x": 2.3540464067273468,
+            "y": 5.608349193345913,
+            "heading": 3.1380055419662973,
+            "angularVelocity": -0.02150671163853717,
+            "velocityX": 1.1219609065196614,
+            "velocityY": 0.5213625158593963,
+            "timestamp": 0.1483779441831525
+        },
+        {
+            "x": 2.4060696111557784,
+            "y": 5.632523782985854,
+            "heading": 3.1370083209408497,
+            "angularVelocity": -0.026883268424770904,
+            "velocityX": 1.4024511450088724,
+            "velocityY": 0.651703048536671,
+            "timestamp": 0.18547243022894064
+        },
+        {
+            "x": 2.4684974567819364,
+            "y": 5.661533286147284,
+            "heading": 3.1358116622714935,
+            "angularVelocity": -0.032259745232204366,
+            "velocityX": 1.6829413824226938,
+            "velocityY": 0.7820435394527967,
+            "timestamp": 0.22256691627472877
+        },
+        {
+            "x": 2.541329943361364,
+            "y": 5.695377701176762,
+            "heading": 3.1344155700455096,
+            "angularVelocity": -0.03763611185395848,
+            "velocityX": 1.9634316132463963,
+            "velocityY": 0.9123839857951219,
+            "timestamp": 0.2596614023205169
+        },
+        {
+            "x": 2.6245670703196087,
+            "y": 5.734057026259781,
+            "heading": 3.1328200501674663,
+            "angularVelocity": -0.04301231929927239,
+            "velocityX": 2.243921828583893,
+            "velocityY": 1.0427243832216457,
+            "timestamp": 0.296755888366305
+        },
+        {
+            "x": 2.7182088365016934,
+            "y": 5.777571259306566,
+            "heading": 3.1310251117440115,
+            "angularVelocity": -0.04838828124588956,
+            "velocityX": 2.5244120127853087,
+            "velocityY": 1.17306472431166,
+            "timestamp": 0.33385037441209314
+        },
+        {
+            "x": 2.822255239588457,
+            "y": 5.825920397683519,
+            "heading": 3.1290307702747278,
+            "angularVelocity": -0.053763825351902786,
+            "velocityX": 2.804902134466352,
+            "velocityY": 1.3034049944046322,
+            "timestamp": 0.3709448604578813
+        },
+        {
+            "x": 2.9367062743473644,
+            "y": 5.879104437402994,
+            "heading": 3.126837057015217,
+            "angularVelocity": -0.05913852686363322,
+            "velocityX": 3.0853921150877586,
+            "velocityY": 1.4337451569978974,
+            "timestamp": 0.4080393465036694
+        },
+        {
+            "x": 3.0615619238881426,
+            "y": 5.937123369060452,
+            "heading": 3.1244440632726094,
+            "angularVelocity": -0.06451076690090568,
+            "velocityX": 3.3658816403780625,
+            "velocityY": 1.5640850660618872,
+            "timestamp": 0.44513383254945754
+        },
+        {
+            "x": 3.188819545942614,
+            "y": 5.996258327229638,
+            "heading": 3.124444062637546,
+            "angularVelocity": -1.7120162809754954e-8,
+            "velocityX": 3.430634458646746,
+            "velocityY": 1.594171114709378,
+            "timestamp": 0.4822283185952457
+        },
+        {
+            "x": 3.3160771824859174,
+            "y": 6.055393254219169,
+            "heading": 3.1244440620025067,
+            "angularVelocity": -1.711950789942117e-8,
+            "velocityX": 3.4306348492393433,
+            "velocityY": 1.5941702741624886,
+            "timestamp": 0.5193228046410338
+        },
+        {
+            "x": 3.4433348335743874,
+            "y": 6.114528149907673,
+            "heading": 3.1244440613674698,
+            "angularVelocity": -1.7119436749370044e-8,
+            "velocityX": 3.4306352413506347,
+            "velocityY": 1.5941694303436482,
+            "timestamp": 0.5564172906868219
+        },
+        {
+            "x": 3.570592499306144,
+            "y": 6.173663014083979,
+            "heading": 3.124444060732438,
+            "angularVelocity": -1.7119304165961875e-8,
+            "velocityX": 3.4306356361070582,
+            "velocityY": 1.5941685808319566,
+            "timestamp": 0.59351177673261
+        },
+        {
+            "x": 3.6978501798346053,
+            "y": 6.232797846417909,
+            "heading": 3.1244440600974133,
+            "angularVelocity": -1.7119116923870704e-8,
+            "velocityX": 3.430636034999362,
+            "velocityY": 1.5941677224193291,
+            "timestamp": 0.6306062627783982
+        },
+        {
+            "x": 3.825107875386636,
+            "y": 6.291932646421232,
+            "heading": 3.1244440594623972,
+            "angularVelocity": -1.7118879018616895e-8,
+            "velocityX": 3.430636440007508,
+            "velocityY": 1.594166850844881,
+            "timestamp": 0.6677007488241863
+        },
+        {
+            "x": 3.9523655862834066,
+            "y": 6.351067413402769,
+            "heading": 3.1244440588273914,
+            "angularVelocity": -1.711859691944282e-8,
+            "velocityX": 3.4306368536738447,
+            "velocityY": 1.5941659606374654,
+            "timestamp": 0.7047952348699744
+        },
+        {
+            "x": 4.079623312940078,
+            "y": 6.410202146469073,
+            "heading": 3.1244440581923967,
+            "angularVelocity": -1.711830453291075e-8,
+            "velocityX": 3.4306372785321515,
+            "velocityY": 1.5941650463443597,
+            "timestamp": 0.7418897209157626
+        },
+        {
+            "x": 4.206881055880688,
+            "y": 6.469336844492392,
+            "heading": 3.124444057557412,
+            "angularVelocity": -1.711804273623487e-8,
+            "velocityX": 3.430637717517568,
+            "velocityY": 1.5941641016490944,
+            "timestamp": 0.7789842069615507
+        },
+        {
+            "x": 4.334138815794606,
+            "y": 6.528471505989175,
+            "heading": 3.1244440569224348,
+            "angularVelocity": -1.7117826690263867e-8,
+            "velocityX": 3.4306381750871577,
+            "velocityY": 1.59416311696,
+            "timestamp": 0.8160786930073388
+        },
+        {
+            "x": 4.461396593547393,
+            "y": 6.587606129096708,
+            "heading": 3.1244440562874636,
+            "angularVelocity": -1.7117677202487012e-8,
+            "velocityX": 3.430638655990672,
+            "velocityY": 1.5941620820555098,
+            "timestamp": 0.853173179053127
+        },
+        {
+            "x": 4.5886544601407655,
+            "y": 6.646740561018822,
+            "heading": 3.124444055652494,
+            "angularVelocity": -1.711762346285072e-8,
+            "velocityX": 3.4306410509715453,
+            "velocityY": 1.5941569280437173,
+            "timestamp": 0.8902676650989151
+        },
+        {
+            "x": 4.7169368908362825,
+            "y": 6.703618180644166,
+            "heading": 3.1244440550164043,
+            "angularVelocity": -1.7147829637187305e-8,
+            "velocityX": 3.458261439103553,
+            "velocityY": 1.5333173656899308,
+            "timestamp": 0.9273621511447032
+        },
+        {
+            "x": 4.8494363294608345,
+            "y": 6.749827966380956,
+            "heading": 3.124444054367944,
+            "angularVelocity": -1.7481320274300667e-8,
+            "velocityX": 3.5719443170340583,
+            "velocityY": 1.245731931148746,
+            "timestamp": 0.9644566371904914
+        },
+        {
+            "x": 4.985267831359121,
+            "y": 6.785059321241209,
+            "heading": 3.124444053688418,
+            "angularVelocity": -1.8318771799358465e-8,
+            "velocityX": 3.6617706936448973,
+            "velocityY": 0.9497733656900947,
+            "timestamp": 1.0015511232362795
+        },
+        {
+            "x": 5.12352345692124,
+            "y": 6.809076540845587,
+            "heading": 3.1244440529552464,
+            "angularVelocity": -1.9764980034334986e-8,
+            "velocityX": 3.7271206667066523,
+            "velocityY": 0.6474606380779142,
+            "timestamp": 1.0386456092820675
+        },
+        {
+            "x": 5.26327900081687,
+            "y": 6.821719014416903,
+            "heading": 3.124444052138408,
+            "angularVelocity": -2.2020481247534803e-8,
+            "velocityX": 3.767555741926746,
+            "velocityY": 0.3408181354961927,
+            "timestamp": 1.0757400953278555
+        },
+        {
+            "x": 5.403600215911865,
+            "y": 6.822902202606201,
+            "heading": 3.124444051205612,
+            "angularVelocity": -2.5146473765828148e-8,
+            "velocityX": 3.782805210504538,
+            "velocityY": 0.03189660554501852,
+            "timestamp": 1.1128345813736435
+        },
+        {
+            "x": 5.530809022252107,
+            "y": 6.8145114887459926,
+            "heading": 3.1244440503418645,
+            "angularVelocity": -2.5630462052464063e-8,
+            "velocityX": 3.774737065106383,
+            "velocityY": -0.24898227978112425,
+            "timestamp": 1.1465346256803473
+        },
+        {
+            "x": 5.657044071820609,
+            "y": 6.79670145003808,
+            "heading": 3.1244440495837504,
+            "angularVelocity": -2.2495928655681543e-8,
+            "velocityX": 3.745842243399973,
+            "velocityY": -0.5284871006643926,
+            "timestamp": 1.180234669987051
+        },
+        {
+            "x": 5.781608875607717,
+            "y": 6.769570371843818,
+            "heading": 3.124444048897901,
+            "angularVelocity": -2.0351591481890114e-8,
+            "velocityX": 3.6962801192023655,
+            "velocityY": -0.8050754458167036,
+            "timestamp": 1.2139347142937547
+        },
+        {
+            "x": 5.903816171014302,
+            "y": 6.733267988087913,
+            "heading": 3.1244440482605893,
+            "angularVelocity": -1.891130244410514e-8,
+            "velocityX": 3.6263244728813384,
+            "velocityY": -1.0772206536441533,
+            "timestamp": 1.2476347586004584
+        },
+        {
+            "x": 6.022991737743217,
+            "y": 6.687994699591429,
+            "heading": 3.124444047653726,
+            "angularVelocity": -1.8007800496265653e-8,
+            "velocityX": 3.5363623158563073,
+            "velocityY": -1.3434192573888535,
+            "timestamp": 1.281334802907162
+        },
+        {
+            "x": 6.1384782869774295,
+            "y": 6.6340007793146,
+            "heading": 3.1244440470624193,
+            "angularVelocity": -1.754616693571922e-8,
+            "velocityX": 3.4268960652742955,
+            "velocityY": -1.6021913735611486,
+            "timestamp": 1.3150348472138658
+        },
+        {
+            "x": 6.251164558797792,
+            "y": 6.57438290738676,
+            "heading": 3.1244440464747822,
+            "angularVelocity": -1.7437275134789687e-8,
+            "velocityX": 3.343801889244,
+            "velocityY": -1.769073992462997,
+            "timestamp": 1.3487348915205695
+        },
+        {
+            "x": 6.363850200907552,
+            "y": 6.514763845231331,
+            "heading": 3.124444045887316,
+            "angularVelocity": -1.7432210000565644e-8,
+            "velocityX": 3.343783203493965,
+            "velocityY": -1.769109310742662,
+            "timestamp": 1.3824349358272732
+        },
+        {
+            "x": 6.475774212207488,
+            "y": 6.453726953025987,
+            "heading": 3.124444046521184,
+            "angularVelocity": 1.880910439494107e-8,
+            "velocityX": 3.3211829124412344,
+            "velocityY": -1.8111813637349317,
+            "timestamp": 1.4161349801339769
+        },
+        {
+            "x": 6.587697896511305,
+            "y": 6.392689461208172,
+            "heading": 3.1244440471552553,
+            "angularVelocity": 1.881516364731064e-8,
+            "velocityX": 3.3211732093049866,
+            "velocityY": -1.8111991563664802,
+            "timestamp": 1.4498350244406806
+        },
+        {
+            "x": 6.699621529072565,
+            "y": 6.331651874510795,
+            "heading": 3.124444047789331,
+            "angularVelocity": 1.881528213317004e-8,
+            "velocityX": 3.321171673919603,
+            "velocityY": -1.811201971780052,
+            "timestamp": 1.4835350687473843
+        },
+        {
+            "x": 6.811545132450604,
+            "y": 6.2706142343006634,
+            "heading": 3.124444048423398,
+            "angularVelocity": 1.881501023244474e-8,
+            "velocityX": 3.3211708079497897,
+            "velocityY": -1.8112035596935343,
+            "timestamp": 1.517235113054088
+        },
+        {
+            "x": 6.923468714015748,
+            "y": 6.209576554092949,
+            "heading": 3.1244440490575007,
+            "angularVelocity": 1.8816081178773256e-8,
+            "velocityX": 3.3211701606836845,
+            "velocityY": -1.8112047465638819,
+            "timestamp": 1.5509351573607917
+        },
+        {
+            "x": 7.031544581287166,
+            "y": 6.150637092992946,
+            "heading": 3.126666281190466,
+            "angularVelocity": 0.06594151962352889,
+            "velocityX": 3.2069948124643886,
+            "velocityY": -1.7489431338307682,
+            "timestamp": 1.5846352016674954
+        },
+        {
+            "x": 7.131306941921243,
+            "y": 6.096231417458986,
+            "heading": 3.1287176070894986,
+            "angularVelocity": 0.060870124690714414,
+            "velocityX": 2.960303545186476,
+            "velocityY": -1.6144096143855096,
+            "timestamp": 1.618335245974199
+        },
+        {
+            "x": 7.2227557801672635,
+            "y": 6.046359537502576,
+            "heading": 3.1305980016068635,
+            "angularVelocity": 0.05579798353529268,
+            "velocityX": 2.7136118105289606,
+            "velocityY": -1.4798757978632517,
+            "timestamp": 1.6520352902809028
+        },
+        {
+            "x": 7.305891091138876,
+            "y": 6.001021457128046,
+            "heading": 3.132307456329253,
+            "angularVelocity": 0.05072559272714715,
+            "velocityX": 2.466919930876006,
+            "velocityY": -1.3453418625182645,
+            "timestamp": 1.6857353345876065
+        },
+        {
+            "x": 7.380712872646774,
+            "y": 5.9602171788044025,
+            "heading": 3.1338459674980714,
+            "angularVelocity": 0.04565309038812635,
+            "velocityX": 2.2202279862586183,
+            "velocityY": -1.2108078539091887,
+            "timestamp": 1.7194353788943102
+        },
+        {
+            "x": 7.447221123569205,
+            "y": 5.923946704366036,
+            "heading": 3.135213533279876,
+            "angularVelocity": 0.04058053364434269,
+            "velocityX": 1.9735360083547677,
+            "velocityY": -1.0762737908671516,
+            "timestamp": 1.753135423201014
+        },
+        {
+            "x": 7.505415843310863,
+            "y": 5.892210035316634,
+            "heading": 3.136410152813217,
+            "angularVelocity": 0.035507951338280094,
+            "velocityX": 1.7268440127861169,
+            "velocityY": -0.941739683205353,
+            "timestamp": 1.7868354675077176
+        },
+        {
+            "x": 7.555297031572268,
+            "y": 5.865007172961733,
+            "heading": 3.1374358257913886,
+            "angularVelocity": 0.030435359931185808,
+            "velocityX": 1.4801520083308155,
+            "velocityY": -0.807205536803692,
+            "timestamp": 1.8205355118144213
+        },
+        {
+            "x": 7.596864688235311,
+            "y": 5.8423381184765635,
+            "heading": 3.138290552251957,
+            "angularVelocity": 0.025362769638798365,
+            "velocityX": 1.2334600003707947,
+            "velocityY": -0.6726713555287516,
+            "timestamp": 1.854235556121125
+        },
+        {
+            "x": 7.6301188133003475,
+            "y": 5.824202872944982,
+            "heading": 3.1389743324564656,
+            "angularVelocity": 0.020290187107322288,
+            "velocityX": 0.9867679924213419,
+            "velocityY": -0.5381371420919633,
+            "timestamp": 1.8879356004278287
+        },
+        {
+            "x": 7.655059406849112,
+            "y": 5.8106014373839985,
+            "heading": 3.1394871668082627,
+            "angularVelocity": 0.015217616544653215,
+            "velocityX": 0.7400759868972333,
+            "velocityY": -0.4036028984768356,
+            "timestamp": 1.9216356447345324
+        },
+        {
+            "x": 7.671686469021436,
+            "y": 5.801533812759989,
+            "heading": 3.1398290558033333,
+            "angularVelocity": 0.010145060699586378,
+            "velocityX": 0.4933839855224485,
+            "velocityY": -0.2690686261859067,
+            "timestamp": 1.9553356890412361
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 0.0050725214219634815,
+            "velocityX": 0.24669198956838576,
+            "velocityY": -0.1345343263862727,
+            "timestamp": 1.9890357333479398
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 1.6384700465306213e-30,
+            "velocityX": 7.518277191788514e-33,
+            "velocityY": 3.939051930235097e-33,
+            "timestamp": 2.0227357776546437
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.traj
new file mode 100644
index 00000000..ae5af9d6
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.traj	
@@ -0,0 +1,526 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 4.173822195239149e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 2.260404640394534,
+            "y": 5.564834920701341,
+            "heading": 3.1398005529486164,
+            "angularVelocity": -0.005376730415880204,
+            "velocityX": 0.2804902157612115,
+            "velocityY": 0.1303406844718941,
+            "timestamp": 0.037094486045788126
+        },
+        {
+            "x": 2.2812139214883365,
+            "y": 5.574504760754136,
+            "heading": 3.139401659951885,
+            "angularVelocity": -0.010753431015026859,
+            "velocityX": 0.5609804397374861,
+            "velocityY": 0.26068133255328624,
+            "timestamp": 0.07418897209157625
+        },
+        {
+            "x": 2.3124278435365335,
+            "y": 5.58900951877657,
+            "heading": 3.1388033223810634,
+            "angularVelocity": -0.016130094647582943,
+            "velocityX": 0.8414706705915215,
+            "velocityY": 0.3910219433834154,
+            "timestamp": 0.11128345813736437
+        },
+        {
+            "x": 2.3540464067273468,
+            "y": 5.608349193345913,
+            "heading": 3.1380055419662973,
+            "angularVelocity": -0.02150671163853717,
+            "velocityX": 1.1219609065196614,
+            "velocityY": 0.5213625158593963,
+            "timestamp": 0.1483779441831525
+        },
+        {
+            "x": 2.4060696111557784,
+            "y": 5.632523782985854,
+            "heading": 3.1370083209408497,
+            "angularVelocity": -0.026883268424770904,
+            "velocityX": 1.4024511450088724,
+            "velocityY": 0.651703048536671,
+            "timestamp": 0.18547243022894064
+        },
+        {
+            "x": 2.4684974567819364,
+            "y": 5.661533286147284,
+            "heading": 3.1358116622714935,
+            "angularVelocity": -0.032259745232204366,
+            "velocityX": 1.6829413824226938,
+            "velocityY": 0.7820435394527967,
+            "timestamp": 0.22256691627472877
+        },
+        {
+            "x": 2.541329943361364,
+            "y": 5.695377701176762,
+            "heading": 3.1344155700455096,
+            "angularVelocity": -0.03763611185395848,
+            "velocityX": 1.9634316132463963,
+            "velocityY": 0.9123839857951219,
+            "timestamp": 0.2596614023205169
+        },
+        {
+            "x": 2.6245670703196087,
+            "y": 5.734057026259781,
+            "heading": 3.1328200501674663,
+            "angularVelocity": -0.04301231929927239,
+            "velocityX": 2.243921828583893,
+            "velocityY": 1.0427243832216457,
+            "timestamp": 0.296755888366305
+        },
+        {
+            "x": 2.7182088365016934,
+            "y": 5.777571259306566,
+            "heading": 3.1310251117440115,
+            "angularVelocity": -0.04838828124588956,
+            "velocityX": 2.5244120127853087,
+            "velocityY": 1.17306472431166,
+            "timestamp": 0.33385037441209314
+        },
+        {
+            "x": 2.822255239588457,
+            "y": 5.825920397683519,
+            "heading": 3.1290307702747278,
+            "angularVelocity": -0.053763825351902786,
+            "velocityX": 2.804902134466352,
+            "velocityY": 1.3034049944046322,
+            "timestamp": 0.3709448604578813
+        },
+        {
+            "x": 2.9367062743473644,
+            "y": 5.879104437402994,
+            "heading": 3.126837057015217,
+            "angularVelocity": -0.05913852686363322,
+            "velocityX": 3.0853921150877586,
+            "velocityY": 1.4337451569978974,
+            "timestamp": 0.4080393465036694
+        },
+        {
+            "x": 3.0615619238881426,
+            "y": 5.937123369060452,
+            "heading": 3.1244440632726094,
+            "angularVelocity": -0.06451076690090568,
+            "velocityX": 3.3658816403780625,
+            "velocityY": 1.5640850660618872,
+            "timestamp": 0.44513383254945754
+        },
+        {
+            "x": 3.188819545942614,
+            "y": 5.996258327229638,
+            "heading": 3.124444062637546,
+            "angularVelocity": -1.7120162809754954e-8,
+            "velocityX": 3.430634458646746,
+            "velocityY": 1.594171114709378,
+            "timestamp": 0.4822283185952457
+        },
+        {
+            "x": 3.3160771824859174,
+            "y": 6.055393254219169,
+            "heading": 3.1244440620025067,
+            "angularVelocity": -1.711950789942117e-8,
+            "velocityX": 3.4306348492393433,
+            "velocityY": 1.5941702741624886,
+            "timestamp": 0.5193228046410338
+        },
+        {
+            "x": 3.4433348335743874,
+            "y": 6.114528149907673,
+            "heading": 3.1244440613674698,
+            "angularVelocity": -1.7119436749370044e-8,
+            "velocityX": 3.4306352413506347,
+            "velocityY": 1.5941694303436482,
+            "timestamp": 0.5564172906868219
+        },
+        {
+            "x": 3.570592499306144,
+            "y": 6.173663014083979,
+            "heading": 3.124444060732438,
+            "angularVelocity": -1.7119304165961875e-8,
+            "velocityX": 3.4306356361070582,
+            "velocityY": 1.5941685808319566,
+            "timestamp": 0.59351177673261
+        },
+        {
+            "x": 3.6978501798346053,
+            "y": 6.232797846417909,
+            "heading": 3.1244440600974133,
+            "angularVelocity": -1.7119116923870704e-8,
+            "velocityX": 3.430636034999362,
+            "velocityY": 1.5941677224193291,
+            "timestamp": 0.6306062627783982
+        },
+        {
+            "x": 3.825107875386636,
+            "y": 6.291932646421232,
+            "heading": 3.1244440594623972,
+            "angularVelocity": -1.7118879018616895e-8,
+            "velocityX": 3.430636440007508,
+            "velocityY": 1.594166850844881,
+            "timestamp": 0.6677007488241863
+        },
+        {
+            "x": 3.9523655862834066,
+            "y": 6.351067413402769,
+            "heading": 3.1244440588273914,
+            "angularVelocity": -1.711859691944282e-8,
+            "velocityX": 3.4306368536738447,
+            "velocityY": 1.5941659606374654,
+            "timestamp": 0.7047952348699744
+        },
+        {
+            "x": 4.079623312940078,
+            "y": 6.410202146469073,
+            "heading": 3.1244440581923967,
+            "angularVelocity": -1.711830453291075e-8,
+            "velocityX": 3.4306372785321515,
+            "velocityY": 1.5941650463443597,
+            "timestamp": 0.7418897209157626
+        },
+        {
+            "x": 4.206881055880688,
+            "y": 6.469336844492392,
+            "heading": 3.124444057557412,
+            "angularVelocity": -1.711804273623487e-8,
+            "velocityX": 3.430637717517568,
+            "velocityY": 1.5941641016490944,
+            "timestamp": 0.7789842069615507
+        },
+        {
+            "x": 4.334138815794606,
+            "y": 6.528471505989175,
+            "heading": 3.1244440569224348,
+            "angularVelocity": -1.7117826690263867e-8,
+            "velocityX": 3.4306381750871577,
+            "velocityY": 1.59416311696,
+            "timestamp": 0.8160786930073388
+        },
+        {
+            "x": 4.461396593547393,
+            "y": 6.587606129096708,
+            "heading": 3.1244440562874636,
+            "angularVelocity": -1.7117677202487012e-8,
+            "velocityX": 3.430638655990672,
+            "velocityY": 1.5941620820555098,
+            "timestamp": 0.853173179053127
+        },
+        {
+            "x": 4.5886544601407655,
+            "y": 6.646740561018822,
+            "heading": 3.124444055652494,
+            "angularVelocity": -1.711762346285072e-8,
+            "velocityX": 3.4306410509715453,
+            "velocityY": 1.5941569280437173,
+            "timestamp": 0.8902676650989151
+        },
+        {
+            "x": 4.7169368908362825,
+            "y": 6.703618180644166,
+            "heading": 3.1244440550164043,
+            "angularVelocity": -1.7147829637187305e-8,
+            "velocityX": 3.458261439103553,
+            "velocityY": 1.5333173656899308,
+            "timestamp": 0.9273621511447032
+        },
+        {
+            "x": 4.8494363294608345,
+            "y": 6.749827966380956,
+            "heading": 3.124444054367944,
+            "angularVelocity": -1.7481320274300667e-8,
+            "velocityX": 3.5719443170340583,
+            "velocityY": 1.245731931148746,
+            "timestamp": 0.9644566371904914
+        },
+        {
+            "x": 4.985267831359121,
+            "y": 6.785059321241209,
+            "heading": 3.124444053688418,
+            "angularVelocity": -1.8318771799358465e-8,
+            "velocityX": 3.6617706936448973,
+            "velocityY": 0.9497733656900947,
+            "timestamp": 1.0015511232362795
+        },
+        {
+            "x": 5.12352345692124,
+            "y": 6.809076540845587,
+            "heading": 3.1244440529552464,
+            "angularVelocity": -1.9764980034334986e-8,
+            "velocityX": 3.7271206667066523,
+            "velocityY": 0.6474606380779142,
+            "timestamp": 1.0386456092820675
+        },
+        {
+            "x": 5.26327900081687,
+            "y": 6.821719014416903,
+            "heading": 3.124444052138408,
+            "angularVelocity": -2.2020481247534803e-8,
+            "velocityX": 3.767555741926746,
+            "velocityY": 0.3408181354961927,
+            "timestamp": 1.0757400953278555
+        },
+        {
+            "x": 5.403600215911865,
+            "y": 6.822902202606201,
+            "heading": 3.124444051205612,
+            "angularVelocity": -2.5146473765828148e-8,
+            "velocityX": 3.782805210504538,
+            "velocityY": 0.03189660554501852,
+            "timestamp": 1.1128345813736435
+        },
+        {
+            "x": 5.530809022252107,
+            "y": 6.8145114887459926,
+            "heading": 3.1244440503418645,
+            "angularVelocity": -2.5630462052464063e-8,
+            "velocityX": 3.774737065106383,
+            "velocityY": -0.24898227978112425,
+            "timestamp": 1.1465346256803473
+        },
+        {
+            "x": 5.657044071820609,
+            "y": 6.79670145003808,
+            "heading": 3.1244440495837504,
+            "angularVelocity": -2.2495928655681543e-8,
+            "velocityX": 3.745842243399973,
+            "velocityY": -0.5284871006643926,
+            "timestamp": 1.180234669987051
+        },
+        {
+            "x": 5.781608875607717,
+            "y": 6.769570371843818,
+            "heading": 3.124444048897901,
+            "angularVelocity": -2.0351591481890114e-8,
+            "velocityX": 3.6962801192023655,
+            "velocityY": -0.8050754458167036,
+            "timestamp": 1.2139347142937547
+        },
+        {
+            "x": 5.903816171014302,
+            "y": 6.733267988087913,
+            "heading": 3.1244440482605893,
+            "angularVelocity": -1.891130244410514e-8,
+            "velocityX": 3.6263244728813384,
+            "velocityY": -1.0772206536441533,
+            "timestamp": 1.2476347586004584
+        },
+        {
+            "x": 6.022991737743217,
+            "y": 6.687994699591429,
+            "heading": 3.124444047653726,
+            "angularVelocity": -1.8007800496265653e-8,
+            "velocityX": 3.5363623158563073,
+            "velocityY": -1.3434192573888535,
+            "timestamp": 1.281334802907162
+        },
+        {
+            "x": 6.1384782869774295,
+            "y": 6.6340007793146,
+            "heading": 3.1244440470624193,
+            "angularVelocity": -1.754616693571922e-8,
+            "velocityX": 3.4268960652742955,
+            "velocityY": -1.6021913735611486,
+            "timestamp": 1.3150348472138658
+        },
+        {
+            "x": 6.251164558797792,
+            "y": 6.57438290738676,
+            "heading": 3.1244440464747822,
+            "angularVelocity": -1.7437275134789687e-8,
+            "velocityX": 3.343801889244,
+            "velocityY": -1.769073992462997,
+            "timestamp": 1.3487348915205695
+        },
+        {
+            "x": 6.363850200907552,
+            "y": 6.514763845231331,
+            "heading": 3.124444045887316,
+            "angularVelocity": -1.7432210000565644e-8,
+            "velocityX": 3.343783203493965,
+            "velocityY": -1.769109310742662,
+            "timestamp": 1.3824349358272732
+        },
+        {
+            "x": 6.475774212207488,
+            "y": 6.453726953025987,
+            "heading": 3.124444046521184,
+            "angularVelocity": 1.880910439494107e-8,
+            "velocityX": 3.3211829124412344,
+            "velocityY": -1.8111813637349317,
+            "timestamp": 1.4161349801339769
+        },
+        {
+            "x": 6.587697896511305,
+            "y": 6.392689461208172,
+            "heading": 3.1244440471552553,
+            "angularVelocity": 1.881516364731064e-8,
+            "velocityX": 3.3211732093049866,
+            "velocityY": -1.8111991563664802,
+            "timestamp": 1.4498350244406806
+        },
+        {
+            "x": 6.699621529072565,
+            "y": 6.331651874510795,
+            "heading": 3.124444047789331,
+            "angularVelocity": 1.881528213317004e-8,
+            "velocityX": 3.321171673919603,
+            "velocityY": -1.811201971780052,
+            "timestamp": 1.4835350687473843
+        },
+        {
+            "x": 6.811545132450604,
+            "y": 6.2706142343006634,
+            "heading": 3.124444048423398,
+            "angularVelocity": 1.881501023244474e-8,
+            "velocityX": 3.3211708079497897,
+            "velocityY": -1.8112035596935343,
+            "timestamp": 1.517235113054088
+        },
+        {
+            "x": 6.923468714015748,
+            "y": 6.209576554092949,
+            "heading": 3.1244440490575007,
+            "angularVelocity": 1.8816081178773256e-8,
+            "velocityX": 3.3211701606836845,
+            "velocityY": -1.8112047465638819,
+            "timestamp": 1.5509351573607917
+        },
+        {
+            "x": 7.031544581287166,
+            "y": 6.150637092992946,
+            "heading": 3.126666281190466,
+            "angularVelocity": 0.06594151962352889,
+            "velocityX": 3.2069948124643886,
+            "velocityY": -1.7489431338307682,
+            "timestamp": 1.5846352016674954
+        },
+        {
+            "x": 7.131306941921243,
+            "y": 6.096231417458986,
+            "heading": 3.1287176070894986,
+            "angularVelocity": 0.060870124690714414,
+            "velocityX": 2.960303545186476,
+            "velocityY": -1.6144096143855096,
+            "timestamp": 1.618335245974199
+        },
+        {
+            "x": 7.2227557801672635,
+            "y": 6.046359537502576,
+            "heading": 3.1305980016068635,
+            "angularVelocity": 0.05579798353529268,
+            "velocityX": 2.7136118105289606,
+            "velocityY": -1.4798757978632517,
+            "timestamp": 1.6520352902809028
+        },
+        {
+            "x": 7.305891091138876,
+            "y": 6.001021457128046,
+            "heading": 3.132307456329253,
+            "angularVelocity": 0.05072559272714715,
+            "velocityX": 2.466919930876006,
+            "velocityY": -1.3453418625182645,
+            "timestamp": 1.6857353345876065
+        },
+        {
+            "x": 7.380712872646774,
+            "y": 5.9602171788044025,
+            "heading": 3.1338459674980714,
+            "angularVelocity": 0.04565309038812635,
+            "velocityX": 2.2202279862586183,
+            "velocityY": -1.2108078539091887,
+            "timestamp": 1.7194353788943102
+        },
+        {
+            "x": 7.447221123569205,
+            "y": 5.923946704366036,
+            "heading": 3.135213533279876,
+            "angularVelocity": 0.04058053364434269,
+            "velocityX": 1.9735360083547677,
+            "velocityY": -1.0762737908671516,
+            "timestamp": 1.753135423201014
+        },
+        {
+            "x": 7.505415843310863,
+            "y": 5.892210035316634,
+            "heading": 3.136410152813217,
+            "angularVelocity": 0.035507951338280094,
+            "velocityX": 1.7268440127861169,
+            "velocityY": -0.941739683205353,
+            "timestamp": 1.7868354675077176
+        },
+        {
+            "x": 7.555297031572268,
+            "y": 5.865007172961733,
+            "heading": 3.1374358257913886,
+            "angularVelocity": 0.030435359931185808,
+            "velocityX": 1.4801520083308155,
+            "velocityY": -0.807205536803692,
+            "timestamp": 1.8205355118144213
+        },
+        {
+            "x": 7.596864688235311,
+            "y": 5.8423381184765635,
+            "heading": 3.138290552251957,
+            "angularVelocity": 0.025362769638798365,
+            "velocityX": 1.2334600003707947,
+            "velocityY": -0.6726713555287516,
+            "timestamp": 1.854235556121125
+        },
+        {
+            "x": 7.6301188133003475,
+            "y": 5.824202872944982,
+            "heading": 3.1389743324564656,
+            "angularVelocity": 0.020290187107322288,
+            "velocityX": 0.9867679924213419,
+            "velocityY": -0.5381371420919633,
+            "timestamp": 1.8879356004278287
+        },
+        {
+            "x": 7.655059406849112,
+            "y": 5.8106014373839985,
+            "heading": 3.1394871668082627,
+            "angularVelocity": 0.015217616544653215,
+            "velocityX": 0.7400759868972333,
+            "velocityY": -0.4036028984768356,
+            "timestamp": 1.9216356447345324
+        },
+        {
+            "x": 7.671686469021436,
+            "y": 5.801533812759989,
+            "heading": 3.1398290558033333,
+            "angularVelocity": 0.010145060699586378,
+            "velocityX": 0.4933839855224485,
+            "velocityY": -0.2690686261859067,
+            "timestamp": 1.9553356890412361
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 0.0050725214219634815,
+            "velocityX": 0.24669198956838576,
+            "velocityY": -0.1345343263862727,
+            "timestamp": 1.9890357333479398
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 1.6384700465306213e-30,
+            "velocityX": 7.518277191788514e-33,
+            "velocityY": 3.939051930235097e-33,
+            "timestamp": 2.0227357776546437
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.1.traj
new file mode 100644
index 00000000..14bb5c7a
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.1.traj	
@@ -0,0 +1,571 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 1.2492921998824363e-31,
+            "velocityX": 1.8348820495773462e-32,
+            "velocityY": 2.100679182506497e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 2.256418341264554,
+            "y": 5.5583910221605555,
+            "heading": 3.140000000000001,
+            "angularVelocity": 2.3717958634313657e-14,
+            "velocityX": 0.2278392560297752,
+            "velocityY": -0.05711574046896039,
+            "timestamp": 0.028170480260500258
+        },
+        {
+            "x": 2.269255024758466,
+            "y": 5.555173071001971,
+            "heading": 3.1400000000000023,
+            "angularVelocity": 4.731676431156196e-14,
+            "velocityX": 0.4556785463083383,
+            "velocityY": -0.11423132047543481,
+            "timestamp": 0.056340960521000516
+        },
+        {
+            "x": 2.2885100515714525,
+            "y": 5.550346151638775,
+            "heading": 3.140000000000004,
+            "angularVelocity": 7.082238243214026e-14,
+            "velocityX": 0.6835178752697619,
+            "velocityY": -0.17134671892564213,
+            "timestamp": 0.08451144078150077
+        },
+        {
+            "x": 2.314183422941926,
+            "y": 5.543910269896753,
+            "heading": 3.140000000000007,
+            "angularVelocity": 9.417090371236357e-14,
+            "velocityX": 0.9113572481926052,
+            "velocityY": -0.22846191057115275,
+            "timestamp": 0.11268192104200103
+        },
+        {
+            "x": 2.3462751402864916,
+            "y": 5.535865432460115,
+            "heading": 3.1400000000000103,
+            "angularVelocity": 1.1735309251828409e-13,
+            "velocityX": 1.1391966714022896,
+            "velocityY": -0.28557686493967693,
+            "timestamp": 0.1408524013025013
+        },
+        {
+            "x": 2.384785205236557,
+            "y": 5.526211647057785,
+            "heading": 3.140000000000014,
+            "angularVelocity": 1.4030928785894893e-13,
+            "velocityX": 1.367036152523929,
+            "velocityY": -0.3426915449455824,
+            "timestamp": 0.16902288156300155
+        },
+        {
+            "x": 2.4297136196836084,
+            "y": 5.514948922700363,
+            "heading": 3.1400000000000188,
+            "angularVelocity": 1.629004634033803e-13,
+            "velocityX": 1.5948757007898307,
+            "velocityY": -0.39980590509187397,
+            "timestamp": 0.1971933618235018
+        },
+        {
+            "x": 2.481060385834354,
+            "y": 5.50207726998147,
+            "heading": 3.1400000000000237,
+            "angularVelocity": 1.8496997642686924e-13,
+            "velocityX": 1.822715327389804,
+            "velocityY": -0.4569198891841572,
+            "timestamp": 0.22536384208400206
+        },
+        {
+            "x": 2.5388255062752263,
+            "y": 5.487596701457088,
+            "heading": 3.14000000000003,
+            "angularVelocity": 2.0646227541738248e-13,
+            "velocityX": 2.0505550458033337,
+            "velocityY": -0.5140334275623145,
+            "timestamp": 0.25353432234450235
+        },
+        {
+            "x": 2.6030089840400077,
+            "y": 5.471507232107069,
+            "heading": 3.140000000000036,
+            "angularVelocity": 2.2698477398227208e-13,
+            "velocityX": 2.278394871910569,
+            "velocityY": -0.5711464341833905,
+            "timestamp": 0.28170480260500264
+        },
+        {
+            "x": 2.673610822657147,
+            "y": 5.453808879843062,
+            "heading": 3.140000000000043,
+            "angularVelocity": 2.462514676500777e-13,
+            "velocityX": 2.506234823271173,
+            "velocityY": -0.6282588049740611,
+            "timestamp": 0.3098752828655029
+        },
+        {
+            "x": 2.7506310261005877,
+            "y": 5.434501665888006,
+            "heading": 3.1400000000000508,
+            "angularVelocity": 2.636502833251031e-13,
+            "velocityX": 2.7340749156994817,
+            "velocityY": -0.6853704223895046,
+            "timestamp": 0.3380457631260032
+        },
+        {
+            "x": 2.834069598399475,
+            "y": 5.413585614377554,
+            "heading": 3.1400000000000587,
+            "angularVelocity": 2.7876299092334793e-13,
+            "velocityX": 2.9619151511549577,
+            "velocityY": -0.7424811830340069,
+            "timestamp": 0.3662162433865035
+        },
+        {
+            "x": 2.9239265420507494,
+            "y": 5.391060748829609,
+            "heading": 3.1400000000000667,
+            "angularVelocity": 2.92402388167881e-13,
+            "velocityX": 3.189755475247231,
+            "velocityY": -0.7995911088362938,
+            "timestamp": 0.3943867236470038
+        },
+        {
+            "x": 3.0202018518398366,
+            "y": 5.366927076318611,
+            "heading": 3.1400000000000756,
+            "angularVelocity": 3.135357172375438e-13,
+            "velocityX": 3.41759561423172,
+            "velocityY": -0.8567007835090982,
+            "timestamp": 0.42255720390750406
+        },
+        {
+            "x": 3.122895489704816,
+            "y": 5.341184519185279,
+            "heading": 3.140000000000087,
+            "angularVelocity": 4.18703156863577e-13,
+            "velocityX": 3.645434402088397,
+            "velocityY": -0.9138132149428461,
+            "timestamp": 0.45072768416800435
+        },
+        {
+            "x": 3.2262642847828022,
+            "y": 5.315271868839329,
+            "heading": 3.140000000000087,
+            "angularVelocity": 1.5556336116797473e-16,
+            "velocityX": 3.669401235694458,
+            "velocityY": -0.9198512097177582,
+            "timestamp": 0.47889816442850464
+        },
+        {
+            "x": 3.3296318094364086,
+            "y": 5.289354150507015,
+            "heading": 3.140000000000087,
+            "angularVelocity": -1.6083670828215077e-16,
+            "velocityX": 3.669356137976281,
+            "velocityY": -0.9200311138697771,
+            "timestamp": 0.5070686446890049
+        },
+        {
+            "x": 3.4329975109343875,
+            "y": 5.263429161959932,
+            "heading": 3.140000000000087,
+            "angularVelocity": 1.7787924056306163e-16,
+            "velocityX": 3.669291419320054,
+            "velocityY": -0.9202891930612488,
+            "timestamp": 0.5352391249495052
+        },
+        {
+            "x": 3.536360974744539,
+            "y": 5.23749525314096,
+            "heading": 3.140000000000087,
+            "angularVelocity": 1.5501276329034833e-16,
+            "velocityX": 3.669211985536661,
+            "velocityY": -0.9206058462317519,
+            "timestamp": 0.5634096052100055
+        },
+        {
+            "x": 3.6397250084453354,
+            "y": 5.211563615834452,
+            "heading": 3.140000000000087,
+            "angularVelocity": 2.0671720135979797e-16,
+            "velocityX": 3.6692322156015877,
+            "velocityY": -0.9205252117362434,
+            "timestamp": 0.5915800854705058
+        },
+        {
+            "x": 3.743083009106784,
+            "y": 5.185607942562298,
+            "heading": 3.140000000000087,
+            "angularVelocity": 1.5977153188511187e-16,
+            "velocityX": 3.6690180538517057,
+            "velocityY": -0.921378444106565,
+            "timestamp": 0.6197505657310061
+        },
+        {
+            "x": 3.8464299042889807,
+            "y": 5.159608086186739,
+            "heading": 3.140000000000087,
+            "angularVelocity": -2.0588084435766592e-17,
+            "velocityX": 3.6686238298573155,
+            "velocityY": -0.9229468626423776,
+            "timestamp": 0.6479210459915064
+        },
+        {
+            "x": 3.949030591223734,
+            "y": 5.130804053392629,
+            "heading": 3.140000000000087,
+            "angularVelocity": 7.696524756540132e-18,
+            "velocityX": 3.642134815806339,
+            "velocityY": -1.0224899443584623,
+            "timestamp": 0.6760915262520066
+        },
+        {
+            "x": 4.049738686087192,
+            "y": 5.095955052913767,
+            "heading": 3.140000000000087,
+            "angularVelocity": 3.5716950666970086e-16,
+            "velocityX": 3.5749512941271244,
+            "velocityY": -1.2370751281698067,
+            "timestamp": 0.7042620065125069
+        },
+        {
+            "x": 4.148105144500732,
+            "y": 5.054959774017334,
+            "heading": 3.140000000000087,
+            "angularVelocity": 3.8010173489100904e-16,
+            "velocityX": 3.4918275266846264,
+            "velocityY": -1.455256655809122,
+            "timestamp": 0.7324324867730072
+        },
+        {
+            "x": 4.301558125078007,
+            "y": 4.971822103513501,
+            "heading": 3.140000000000087,
+            "angularVelocity": -3.218591761776222e-17,
+            "velocityX": 3.32615174670464,
+            "velocityY": -1.8020406441293784,
+            "timestamp": 0.7785677783240725
+        },
+        {
+            "x": 4.445898119119771,
+            "y": 4.873711676762074,
+            "heading": 3.140000000000087,
+            "angularVelocity": -4.812512433810467e-17,
+            "velocityX": 3.128624295828435,
+            "velocityY": -2.126580833303964,
+            "timestamp": 0.8247030698751379
+        },
+        {
+            "x": 4.5875715760336115,
+            "y": 4.7717885618775915,
+            "heading": 3.140000000000087,
+            "angularVelocity": -6.437963156007962e-17,
+            "velocityX": 3.0708260888967014,
+            "velocityY": -2.2092222994111217,
+            "timestamp": 0.8708383614262032
+        },
+        {
+            "x": 4.729219222551534,
+            "y": 4.669829579874243,
+            "heading": 3.140000000000087,
+            "angularVelocity": -5.401066324195286e-16,
+            "velocityX": 3.0702666386935125,
+            "velocityY": -2.2099997329304943,
+            "timestamp": 0.9169736529772685
+        },
+        {
+            "x": 4.870877206247071,
+            "y": 4.567884960570754,
+            "heading": 3.140000000000087,
+            "angularVelocity": -5.491807811904997e-16,
+            "velocityX": 3.0704907009776297,
+            "velocityY": -2.209688415909936,
+            "timestamp": 0.9631089445283338
+        },
+        {
+            "x": 5.012587244990002,
+            "y": 4.46601271378786,
+            "heading": 3.140000000000087,
+            "angularVelocity": 2.3992830315072076e-16,
+            "velocityX": 3.0716190139795567,
+            "velocityY": -2.2081197139288955,
+            "timestamp": 1.0092442360793992
+        },
+        {
+            "x": 5.160453670190759,
+            "y": 4.373302358536228,
+            "heading": 3.140000000000087,
+            "angularVelocity": 3.2225913830486856e-16,
+            "velocityX": 3.205061033110239,
+            "velocityY": -2.009532228659069,
+            "timestamp": 1.0553795276304645
+        },
+        {
+            "x": 5.316942928230888,
+            "y": 4.296031397168284,
+            "heading": 3.140000000000087,
+            "angularVelocity": 3.540598605731943e-16,
+            "velocityX": 3.391964216104365,
+            "velocityY": -1.674877491153309,
+            "timestamp": 1.1015148191815298
+        },
+        {
+            "x": 5.480467319488525,
+            "y": 4.235044002532959,
+            "heading": 3.140000000000087,
+            "angularVelocity": 3.7214299347620075e-16,
+            "velocityX": 3.544453405635226,
+            "velocityY": -1.3219249859178432,
+            "timestamp": 1.1476501107325952
+        },
+        {
+            "x": 5.591417678770796,
+            "y": 4.201991970754989,
+            "heading": 3.140000000000087,
+            "angularVelocity": 2.3864185821028e-16,
+            "velocityX": 3.6254884971019536,
+            "velocityY": -1.0800304009303623,
+            "timestamp": 1.1782529824203136
+        },
+        {
+            "x": 5.704342870000873,
+            "y": 4.176490587908602,
+            "heading": 3.140000000000087,
+            "angularVelocity": 1.1130552624117688e-16,
+            "velocityX": 3.6900194328910065,
+            "velocityY": -0.8333003225044783,
+            "timestamp": 1.208855854108032
+        },
+        {
+            "x": 5.818728492964508,
+            "y": 4.158648255815286,
+            "heading": 3.140000000000087,
+            "angularVelocity": 1.0429128791162552e-16,
+            "velocityX": 3.737741481609135,
+            "velocityY": -0.583028033314901,
+            "timestamp": 1.2394587257957506
+        },
+        {
+            "x": 5.934052260793966,
+            "y": 4.148506842860091,
+            "heading": 3.140000000000087,
+            "angularVelocity": 7.090230424723168e-16,
+            "velocityX": 3.7683969336671863,
+            "velocityY": -0.3313876246402188,
+            "timestamp": 1.270061597483469
+        },
+        {
+            "x": 6.049784939893549,
+            "y": 4.145614314312393,
+            "heading": 3.140000000000087,
+            "angularVelocity": 5.904438564058107e-16,
+            "velocityX": 3.7817587931139536,
+            "velocityY": -0.0945182065847883,
+            "timestamp": 1.3006644691711875
+        },
+        {
+            "x": 6.1655453259799495,
+            "y": 4.144216905752427,
+            "heading": 3.140000000000087,
+            "angularVelocity": -4.945243197986353e-16,
+            "velocityX": 3.7826641652342365,
+            "velocityY": -0.04566266116445697,
+            "timestamp": 1.331267340858906
+        },
+        {
+            "x": 6.281306292238703,
+            "y": 4.142868414104831,
+            "heading": 3.140000000000087,
+            "angularVelocity": 1.3561359601509856e-16,
+            "velocityX": 3.782683123335836,
+            "velocityY": -0.04406421926917252,
+            "timestamp": 1.3618702125466244
+        },
+        {
+            "x": 6.397067393015407,
+            "y": 4.141531519361414,
+            "heading": 3.140000000000087,
+            "angularVelocity": -2.815905629868065e-16,
+            "velocityX": 3.782687518934728,
+            "velocityY": -0.04368527104047988,
+            "timestamp": 1.392473084234343
+        },
+        {
+            "x": 6.512828617959603,
+            "y": 4.140205420378013,
+            "heading": 3.140000000000087,
+            "angularVelocity": 4.8810263606010396e-17,
+            "velocityX": 3.782691576315187,
+            "velocityY": -0.043332501527048845,
+            "timestamp": 1.4230759559220614
+        },
+        {
+            "x": 6.628589931635543,
+            "y": 4.1388870909429585,
+            "heading": 3.140000000000087,
+            "angularVelocity": -3.260693519621353e-16,
+            "velocityX": 3.7826944757734187,
+            "velocityY": -0.04307861853318547,
+            "timestamp": 1.4536788276097798
+        },
+        {
+            "x": 6.744351296534275,
+            "y": 4.137573267441446,
+            "heading": 3.140000000000087,
+            "angularVelocity": -5.381950425280429e-16,
+            "velocityX": 3.782696149563972,
+            "velocityY": -0.04293137960615145,
+            "timestamp": 1.4842816992974983
+        },
+        {
+            "x": 6.860112695906051,
+            "y": 4.136261713626517,
+            "heading": 3.140000000000087,
+            "angularVelocity": 6.589038089157793e-16,
+            "velocityX": 3.7826972760284443,
+            "velocityY": -0.04285721378647441,
+            "timestamp": 1.5148845709852168
+        },
+        {
+            "x": 6.969430224754608,
+            "y": 4.135029589996691,
+            "heading": 3.1400000000000587,
+            "angularVelocity": -9.14507649037293e-13,
+            "velocityX": 3.5721330326141576,
+            "velocityY": -0.04026169968457299,
+            "timestamp": 1.5454874426729353
+        },
+        {
+            "x": 7.070940187047598,
+            "y": 4.13388425582119,
+            "heading": 3.140000000000041,
+            "angularVelocity": -5.92710722161247e-13,
+            "velocityX": 3.3170077412616483,
+            "velocityY": -0.037425709147242185,
+            "timestamp": 1.5760903143606537
+        },
+        {
+            "x": 7.16464169000894,
+            "y": 4.132826276442613,
+            "heading": 3.1400000000000285,
+            "angularVelocity": -4.0082806789240517e-13,
+            "velocityX": 3.06185327695782,
+            "velocityY": -0.034571245100923735,
+            "timestamp": 1.6066931860483722
+        },
+        {
+            "x": 7.250534738512908,
+            "y": 4.131855992104498,
+            "heading": 3.14000000000002,
+            "angularVelocity": -2.818757978482064e-13,
+            "velocityX": 2.8066989719287347,
+            "velocityY": -0.031705663029917695,
+            "timestamp": 1.6372960577360907
+        },
+        {
+            "x": 7.328619329487215,
+            "y": 4.130973602817362,
+            "heading": 3.140000000000014,
+            "angularVelocity": -2.020529664357193e-13,
+            "velocityX": 2.5515445665075394,
+            "velocityY": -0.028833545281456593,
+            "timestamp": 1.6678989294238091
+        },
+        {
+            "x": 7.398895462248533,
+            "y": 4.130179235854988,
+            "heading": 3.1400000000000095,
+            "angularVelocity": -1.455432238063259e-13,
+            "velocityX": 2.296390138757389,
+            "velocityY": -0.02595726864966503,
+            "timestamp": 1.6985018011115276
+        },
+        {
+            "x": 7.4613631364100685,
+            "y": 4.129472977966136,
+            "heading": 3.1400000000000063,
+            "angularVelocity": -1.0415995882589759e-13,
+            "velocityX": 2.0412356983681157,
+            "velocityY": -0.0230781573571433,
+            "timestamp": 1.729104672799246
+        },
+        {
+            "x": 7.5160223517191325,
+            "y": 4.128854891515294,
+            "heading": 3.1400000000000037,
+            "angularVelocity": -7.336162887662554e-14,
+            "velocityX": 1.7860812497217864,
+            "velocityY": -0.02019700820070875,
+            "timestamp": 1.7597075444869645
+        },
+        {
+            "x": 7.562873107995996,
+            "y": 4.12832502323739,
+            "heading": 3.1400000000000023,
+            "angularVelocity": -5.027990878360554e-14,
+            "velocityX": 1.5309267952025598,
+            "velocityY": -0.017314331902335514,
+            "timestamp": 1.790310416174683
+        },
+        {
+            "x": 7.6019154051050695,
+            "y": 4.127883409326492,
+            "heading": 3.1400000000000015,
+            "angularVelocity": -3.297949891981295e-14,
+            "velocityX": 1.2757723362526898,
+            "velocityY": -0.014430472902237576,
+            "timestamp": 1.8209132878624015
+        },
+        {
+            "x": 7.63314924293966,
+            "y": 4.127530078568267,
+            "heading": 3.1400000000000006,
+            "angularVelocity": -2.0267021915169143e-14,
+            "velocityX": 1.0206178738164648,
+            "velocityY": -0.011545673282216062,
+            "timestamp": 1.85151615955012
+        },
+        {
+            "x": 7.656574621413162,
+            "y": 4.127265054361888,
+            "heading": 3.14,
+            "angularVelocity": -1.117466715568672e-14,
+            "velocityX": 0.7654634085502764,
+            "velocityY": -0.008660109055131773,
+            "timestamp": 1.8821190312378384
+        },
+        {
+            "x": 7.672191540453593,
+            "y": 4.127088356077733,
+            "heading": 3.14,
+            "angularVelocity": -5.057381822634304e-15,
+            "velocityX": 0.5103089409318914,
+            "velocityY": -0.005773911868497247,
+            "timestamp": 1.9127219029255569
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": -1.4274059532547784e-15,
+            "velocityX": 0.2551544713218168,
+            "velocityY": -0.002887182568422342,
+            "timestamp": 1.9433247746132754
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 3.6920170793948963e-32,
+            "velocityX": 1.993299363399013e-33,
+            "velocityY": 1.7597210552293873e-31,
+            "timestamp": 1.9739276463009938
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.traj
new file mode 100644
index 00000000..14bb5c7a
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.traj	
@@ -0,0 +1,571 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 1.2492921998824363e-31,
+            "velocityX": 1.8348820495773462e-32,
+            "velocityY": 2.100679182506497e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 2.256418341264554,
+            "y": 5.5583910221605555,
+            "heading": 3.140000000000001,
+            "angularVelocity": 2.3717958634313657e-14,
+            "velocityX": 0.2278392560297752,
+            "velocityY": -0.05711574046896039,
+            "timestamp": 0.028170480260500258
+        },
+        {
+            "x": 2.269255024758466,
+            "y": 5.555173071001971,
+            "heading": 3.1400000000000023,
+            "angularVelocity": 4.731676431156196e-14,
+            "velocityX": 0.4556785463083383,
+            "velocityY": -0.11423132047543481,
+            "timestamp": 0.056340960521000516
+        },
+        {
+            "x": 2.2885100515714525,
+            "y": 5.550346151638775,
+            "heading": 3.140000000000004,
+            "angularVelocity": 7.082238243214026e-14,
+            "velocityX": 0.6835178752697619,
+            "velocityY": -0.17134671892564213,
+            "timestamp": 0.08451144078150077
+        },
+        {
+            "x": 2.314183422941926,
+            "y": 5.543910269896753,
+            "heading": 3.140000000000007,
+            "angularVelocity": 9.417090371236357e-14,
+            "velocityX": 0.9113572481926052,
+            "velocityY": -0.22846191057115275,
+            "timestamp": 0.11268192104200103
+        },
+        {
+            "x": 2.3462751402864916,
+            "y": 5.535865432460115,
+            "heading": 3.1400000000000103,
+            "angularVelocity": 1.1735309251828409e-13,
+            "velocityX": 1.1391966714022896,
+            "velocityY": -0.28557686493967693,
+            "timestamp": 0.1408524013025013
+        },
+        {
+            "x": 2.384785205236557,
+            "y": 5.526211647057785,
+            "heading": 3.140000000000014,
+            "angularVelocity": 1.4030928785894893e-13,
+            "velocityX": 1.367036152523929,
+            "velocityY": -0.3426915449455824,
+            "timestamp": 0.16902288156300155
+        },
+        {
+            "x": 2.4297136196836084,
+            "y": 5.514948922700363,
+            "heading": 3.1400000000000188,
+            "angularVelocity": 1.629004634033803e-13,
+            "velocityX": 1.5948757007898307,
+            "velocityY": -0.39980590509187397,
+            "timestamp": 0.1971933618235018
+        },
+        {
+            "x": 2.481060385834354,
+            "y": 5.50207726998147,
+            "heading": 3.1400000000000237,
+            "angularVelocity": 1.8496997642686924e-13,
+            "velocityX": 1.822715327389804,
+            "velocityY": -0.4569198891841572,
+            "timestamp": 0.22536384208400206
+        },
+        {
+            "x": 2.5388255062752263,
+            "y": 5.487596701457088,
+            "heading": 3.14000000000003,
+            "angularVelocity": 2.0646227541738248e-13,
+            "velocityX": 2.0505550458033337,
+            "velocityY": -0.5140334275623145,
+            "timestamp": 0.25353432234450235
+        },
+        {
+            "x": 2.6030089840400077,
+            "y": 5.471507232107069,
+            "heading": 3.140000000000036,
+            "angularVelocity": 2.2698477398227208e-13,
+            "velocityX": 2.278394871910569,
+            "velocityY": -0.5711464341833905,
+            "timestamp": 0.28170480260500264
+        },
+        {
+            "x": 2.673610822657147,
+            "y": 5.453808879843062,
+            "heading": 3.140000000000043,
+            "angularVelocity": 2.462514676500777e-13,
+            "velocityX": 2.506234823271173,
+            "velocityY": -0.6282588049740611,
+            "timestamp": 0.3098752828655029
+        },
+        {
+            "x": 2.7506310261005877,
+            "y": 5.434501665888006,
+            "heading": 3.1400000000000508,
+            "angularVelocity": 2.636502833251031e-13,
+            "velocityX": 2.7340749156994817,
+            "velocityY": -0.6853704223895046,
+            "timestamp": 0.3380457631260032
+        },
+        {
+            "x": 2.834069598399475,
+            "y": 5.413585614377554,
+            "heading": 3.1400000000000587,
+            "angularVelocity": 2.7876299092334793e-13,
+            "velocityX": 2.9619151511549577,
+            "velocityY": -0.7424811830340069,
+            "timestamp": 0.3662162433865035
+        },
+        {
+            "x": 2.9239265420507494,
+            "y": 5.391060748829609,
+            "heading": 3.1400000000000667,
+            "angularVelocity": 2.92402388167881e-13,
+            "velocityX": 3.189755475247231,
+            "velocityY": -0.7995911088362938,
+            "timestamp": 0.3943867236470038
+        },
+        {
+            "x": 3.0202018518398366,
+            "y": 5.366927076318611,
+            "heading": 3.1400000000000756,
+            "angularVelocity": 3.135357172375438e-13,
+            "velocityX": 3.41759561423172,
+            "velocityY": -0.8567007835090982,
+            "timestamp": 0.42255720390750406
+        },
+        {
+            "x": 3.122895489704816,
+            "y": 5.341184519185279,
+            "heading": 3.140000000000087,
+            "angularVelocity": 4.18703156863577e-13,
+            "velocityX": 3.645434402088397,
+            "velocityY": -0.9138132149428461,
+            "timestamp": 0.45072768416800435
+        },
+        {
+            "x": 3.2262642847828022,
+            "y": 5.315271868839329,
+            "heading": 3.140000000000087,
+            "angularVelocity": 1.5556336116797473e-16,
+            "velocityX": 3.669401235694458,
+            "velocityY": -0.9198512097177582,
+            "timestamp": 0.47889816442850464
+        },
+        {
+            "x": 3.3296318094364086,
+            "y": 5.289354150507015,
+            "heading": 3.140000000000087,
+            "angularVelocity": -1.6083670828215077e-16,
+            "velocityX": 3.669356137976281,
+            "velocityY": -0.9200311138697771,
+            "timestamp": 0.5070686446890049
+        },
+        {
+            "x": 3.4329975109343875,
+            "y": 5.263429161959932,
+            "heading": 3.140000000000087,
+            "angularVelocity": 1.7787924056306163e-16,
+            "velocityX": 3.669291419320054,
+            "velocityY": -0.9202891930612488,
+            "timestamp": 0.5352391249495052
+        },
+        {
+            "x": 3.536360974744539,
+            "y": 5.23749525314096,
+            "heading": 3.140000000000087,
+            "angularVelocity": 1.5501276329034833e-16,
+            "velocityX": 3.669211985536661,
+            "velocityY": -0.9206058462317519,
+            "timestamp": 0.5634096052100055
+        },
+        {
+            "x": 3.6397250084453354,
+            "y": 5.211563615834452,
+            "heading": 3.140000000000087,
+            "angularVelocity": 2.0671720135979797e-16,
+            "velocityX": 3.6692322156015877,
+            "velocityY": -0.9205252117362434,
+            "timestamp": 0.5915800854705058
+        },
+        {
+            "x": 3.743083009106784,
+            "y": 5.185607942562298,
+            "heading": 3.140000000000087,
+            "angularVelocity": 1.5977153188511187e-16,
+            "velocityX": 3.6690180538517057,
+            "velocityY": -0.921378444106565,
+            "timestamp": 0.6197505657310061
+        },
+        {
+            "x": 3.8464299042889807,
+            "y": 5.159608086186739,
+            "heading": 3.140000000000087,
+            "angularVelocity": -2.0588084435766592e-17,
+            "velocityX": 3.6686238298573155,
+            "velocityY": -0.9229468626423776,
+            "timestamp": 0.6479210459915064
+        },
+        {
+            "x": 3.949030591223734,
+            "y": 5.130804053392629,
+            "heading": 3.140000000000087,
+            "angularVelocity": 7.696524756540132e-18,
+            "velocityX": 3.642134815806339,
+            "velocityY": -1.0224899443584623,
+            "timestamp": 0.6760915262520066
+        },
+        {
+            "x": 4.049738686087192,
+            "y": 5.095955052913767,
+            "heading": 3.140000000000087,
+            "angularVelocity": 3.5716950666970086e-16,
+            "velocityX": 3.5749512941271244,
+            "velocityY": -1.2370751281698067,
+            "timestamp": 0.7042620065125069
+        },
+        {
+            "x": 4.148105144500732,
+            "y": 5.054959774017334,
+            "heading": 3.140000000000087,
+            "angularVelocity": 3.8010173489100904e-16,
+            "velocityX": 3.4918275266846264,
+            "velocityY": -1.455256655809122,
+            "timestamp": 0.7324324867730072
+        },
+        {
+            "x": 4.301558125078007,
+            "y": 4.971822103513501,
+            "heading": 3.140000000000087,
+            "angularVelocity": -3.218591761776222e-17,
+            "velocityX": 3.32615174670464,
+            "velocityY": -1.8020406441293784,
+            "timestamp": 0.7785677783240725
+        },
+        {
+            "x": 4.445898119119771,
+            "y": 4.873711676762074,
+            "heading": 3.140000000000087,
+            "angularVelocity": -4.812512433810467e-17,
+            "velocityX": 3.128624295828435,
+            "velocityY": -2.126580833303964,
+            "timestamp": 0.8247030698751379
+        },
+        {
+            "x": 4.5875715760336115,
+            "y": 4.7717885618775915,
+            "heading": 3.140000000000087,
+            "angularVelocity": -6.437963156007962e-17,
+            "velocityX": 3.0708260888967014,
+            "velocityY": -2.2092222994111217,
+            "timestamp": 0.8708383614262032
+        },
+        {
+            "x": 4.729219222551534,
+            "y": 4.669829579874243,
+            "heading": 3.140000000000087,
+            "angularVelocity": -5.401066324195286e-16,
+            "velocityX": 3.0702666386935125,
+            "velocityY": -2.2099997329304943,
+            "timestamp": 0.9169736529772685
+        },
+        {
+            "x": 4.870877206247071,
+            "y": 4.567884960570754,
+            "heading": 3.140000000000087,
+            "angularVelocity": -5.491807811904997e-16,
+            "velocityX": 3.0704907009776297,
+            "velocityY": -2.209688415909936,
+            "timestamp": 0.9631089445283338
+        },
+        {
+            "x": 5.012587244990002,
+            "y": 4.46601271378786,
+            "heading": 3.140000000000087,
+            "angularVelocity": 2.3992830315072076e-16,
+            "velocityX": 3.0716190139795567,
+            "velocityY": -2.2081197139288955,
+            "timestamp": 1.0092442360793992
+        },
+        {
+            "x": 5.160453670190759,
+            "y": 4.373302358536228,
+            "heading": 3.140000000000087,
+            "angularVelocity": 3.2225913830486856e-16,
+            "velocityX": 3.205061033110239,
+            "velocityY": -2.009532228659069,
+            "timestamp": 1.0553795276304645
+        },
+        {
+            "x": 5.316942928230888,
+            "y": 4.296031397168284,
+            "heading": 3.140000000000087,
+            "angularVelocity": 3.540598605731943e-16,
+            "velocityX": 3.391964216104365,
+            "velocityY": -1.674877491153309,
+            "timestamp": 1.1015148191815298
+        },
+        {
+            "x": 5.480467319488525,
+            "y": 4.235044002532959,
+            "heading": 3.140000000000087,
+            "angularVelocity": 3.7214299347620075e-16,
+            "velocityX": 3.544453405635226,
+            "velocityY": -1.3219249859178432,
+            "timestamp": 1.1476501107325952
+        },
+        {
+            "x": 5.591417678770796,
+            "y": 4.201991970754989,
+            "heading": 3.140000000000087,
+            "angularVelocity": 2.3864185821028e-16,
+            "velocityX": 3.6254884971019536,
+            "velocityY": -1.0800304009303623,
+            "timestamp": 1.1782529824203136
+        },
+        {
+            "x": 5.704342870000873,
+            "y": 4.176490587908602,
+            "heading": 3.140000000000087,
+            "angularVelocity": 1.1130552624117688e-16,
+            "velocityX": 3.6900194328910065,
+            "velocityY": -0.8333003225044783,
+            "timestamp": 1.208855854108032
+        },
+        {
+            "x": 5.818728492964508,
+            "y": 4.158648255815286,
+            "heading": 3.140000000000087,
+            "angularVelocity": 1.0429128791162552e-16,
+            "velocityX": 3.737741481609135,
+            "velocityY": -0.583028033314901,
+            "timestamp": 1.2394587257957506
+        },
+        {
+            "x": 5.934052260793966,
+            "y": 4.148506842860091,
+            "heading": 3.140000000000087,
+            "angularVelocity": 7.090230424723168e-16,
+            "velocityX": 3.7683969336671863,
+            "velocityY": -0.3313876246402188,
+            "timestamp": 1.270061597483469
+        },
+        {
+            "x": 6.049784939893549,
+            "y": 4.145614314312393,
+            "heading": 3.140000000000087,
+            "angularVelocity": 5.904438564058107e-16,
+            "velocityX": 3.7817587931139536,
+            "velocityY": -0.0945182065847883,
+            "timestamp": 1.3006644691711875
+        },
+        {
+            "x": 6.1655453259799495,
+            "y": 4.144216905752427,
+            "heading": 3.140000000000087,
+            "angularVelocity": -4.945243197986353e-16,
+            "velocityX": 3.7826641652342365,
+            "velocityY": -0.04566266116445697,
+            "timestamp": 1.331267340858906
+        },
+        {
+            "x": 6.281306292238703,
+            "y": 4.142868414104831,
+            "heading": 3.140000000000087,
+            "angularVelocity": 1.3561359601509856e-16,
+            "velocityX": 3.782683123335836,
+            "velocityY": -0.04406421926917252,
+            "timestamp": 1.3618702125466244
+        },
+        {
+            "x": 6.397067393015407,
+            "y": 4.141531519361414,
+            "heading": 3.140000000000087,
+            "angularVelocity": -2.815905629868065e-16,
+            "velocityX": 3.782687518934728,
+            "velocityY": -0.04368527104047988,
+            "timestamp": 1.392473084234343
+        },
+        {
+            "x": 6.512828617959603,
+            "y": 4.140205420378013,
+            "heading": 3.140000000000087,
+            "angularVelocity": 4.8810263606010396e-17,
+            "velocityX": 3.782691576315187,
+            "velocityY": -0.043332501527048845,
+            "timestamp": 1.4230759559220614
+        },
+        {
+            "x": 6.628589931635543,
+            "y": 4.1388870909429585,
+            "heading": 3.140000000000087,
+            "angularVelocity": -3.260693519621353e-16,
+            "velocityX": 3.7826944757734187,
+            "velocityY": -0.04307861853318547,
+            "timestamp": 1.4536788276097798
+        },
+        {
+            "x": 6.744351296534275,
+            "y": 4.137573267441446,
+            "heading": 3.140000000000087,
+            "angularVelocity": -5.381950425280429e-16,
+            "velocityX": 3.782696149563972,
+            "velocityY": -0.04293137960615145,
+            "timestamp": 1.4842816992974983
+        },
+        {
+            "x": 6.860112695906051,
+            "y": 4.136261713626517,
+            "heading": 3.140000000000087,
+            "angularVelocity": 6.589038089157793e-16,
+            "velocityX": 3.7826972760284443,
+            "velocityY": -0.04285721378647441,
+            "timestamp": 1.5148845709852168
+        },
+        {
+            "x": 6.969430224754608,
+            "y": 4.135029589996691,
+            "heading": 3.1400000000000587,
+            "angularVelocity": -9.14507649037293e-13,
+            "velocityX": 3.5721330326141576,
+            "velocityY": -0.04026169968457299,
+            "timestamp": 1.5454874426729353
+        },
+        {
+            "x": 7.070940187047598,
+            "y": 4.13388425582119,
+            "heading": 3.140000000000041,
+            "angularVelocity": -5.92710722161247e-13,
+            "velocityX": 3.3170077412616483,
+            "velocityY": -0.037425709147242185,
+            "timestamp": 1.5760903143606537
+        },
+        {
+            "x": 7.16464169000894,
+            "y": 4.132826276442613,
+            "heading": 3.1400000000000285,
+            "angularVelocity": -4.0082806789240517e-13,
+            "velocityX": 3.06185327695782,
+            "velocityY": -0.034571245100923735,
+            "timestamp": 1.6066931860483722
+        },
+        {
+            "x": 7.250534738512908,
+            "y": 4.131855992104498,
+            "heading": 3.14000000000002,
+            "angularVelocity": -2.818757978482064e-13,
+            "velocityX": 2.8066989719287347,
+            "velocityY": -0.031705663029917695,
+            "timestamp": 1.6372960577360907
+        },
+        {
+            "x": 7.328619329487215,
+            "y": 4.130973602817362,
+            "heading": 3.140000000000014,
+            "angularVelocity": -2.020529664357193e-13,
+            "velocityX": 2.5515445665075394,
+            "velocityY": -0.028833545281456593,
+            "timestamp": 1.6678989294238091
+        },
+        {
+            "x": 7.398895462248533,
+            "y": 4.130179235854988,
+            "heading": 3.1400000000000095,
+            "angularVelocity": -1.455432238063259e-13,
+            "velocityX": 2.296390138757389,
+            "velocityY": -0.02595726864966503,
+            "timestamp": 1.6985018011115276
+        },
+        {
+            "x": 7.4613631364100685,
+            "y": 4.129472977966136,
+            "heading": 3.1400000000000063,
+            "angularVelocity": -1.0415995882589759e-13,
+            "velocityX": 2.0412356983681157,
+            "velocityY": -0.0230781573571433,
+            "timestamp": 1.729104672799246
+        },
+        {
+            "x": 7.5160223517191325,
+            "y": 4.128854891515294,
+            "heading": 3.1400000000000037,
+            "angularVelocity": -7.336162887662554e-14,
+            "velocityX": 1.7860812497217864,
+            "velocityY": -0.02019700820070875,
+            "timestamp": 1.7597075444869645
+        },
+        {
+            "x": 7.562873107995996,
+            "y": 4.12832502323739,
+            "heading": 3.1400000000000023,
+            "angularVelocity": -5.027990878360554e-14,
+            "velocityX": 1.5309267952025598,
+            "velocityY": -0.017314331902335514,
+            "timestamp": 1.790310416174683
+        },
+        {
+            "x": 7.6019154051050695,
+            "y": 4.127883409326492,
+            "heading": 3.1400000000000015,
+            "angularVelocity": -3.297949891981295e-14,
+            "velocityX": 1.2757723362526898,
+            "velocityY": -0.014430472902237576,
+            "timestamp": 1.8209132878624015
+        },
+        {
+            "x": 7.63314924293966,
+            "y": 4.127530078568267,
+            "heading": 3.1400000000000006,
+            "angularVelocity": -2.0267021915169143e-14,
+            "velocityX": 1.0206178738164648,
+            "velocityY": -0.011545673282216062,
+            "timestamp": 1.85151615955012
+        },
+        {
+            "x": 7.656574621413162,
+            "y": 4.127265054361888,
+            "heading": 3.14,
+            "angularVelocity": -1.117466715568672e-14,
+            "velocityX": 0.7654634085502764,
+            "velocityY": -0.008660109055131773,
+            "timestamp": 1.8821190312378384
+        },
+        {
+            "x": 7.672191540453593,
+            "y": 4.127088356077733,
+            "heading": 3.14,
+            "angularVelocity": -5.057381822634304e-15,
+            "velocityX": 0.5103089409318914,
+            "velocityY": -0.005773911868497247,
+            "timestamp": 1.9127219029255569
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": -1.4274059532547784e-15,
+            "velocityX": 0.2551544713218168,
+            "velocityY": -0.002887182568422342,
+            "timestamp": 1.9433247746132754
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 3.6920170793948963e-32,
+            "velocityX": 1.993299363399013e-33,
+            "velocityY": 1.7597210552293873e-31,
+            "timestamp": 1.9739276463009938
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.1.traj
new file mode 100644
index 00000000..135fb8ca
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.1.traj	
@@ -0,0 +1,589 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.254338747446347,
+            "y": 5.559909413116517,
+            "heading": 3.1115254109417156,
+            "angularVelocity": -0.9384273980850008,
+            "velocityX": 0.14299063170637197,
+            "velocityY": -0.0029854412716235888,
+            "timestamp": 0.030342879072361703
+        },
+        {
+            "x": 2.263150716185563,
+            "y": 5.559590842447939,
+            "heading": 3.055209315133754,
+            "angularVelocity": -1.855990516709349,
+            "velocityX": 0.29041307247742426,
+            "velocityY": -0.010499025745671282,
+            "timestamp": 0.060685758144723406
+        },
+        {
+            "x": 2.2766003091950555,
+            "y": 5.558840744374995,
+            "heading": 2.9719321573579647,
+            "angularVelocity": -2.744537114529888,
+            "velocityX": 0.44325368655420766,
+            "velocityY": -0.024720728417179556,
+            "timestamp": 0.09102863721708511
+        },
+        {
+            "x": 2.2949251974733853,
+            "y": 5.557404975825004,
+            "heading": 2.863139887890429,
+            "angularVelocity": -3.5854300182948253,
+            "velocityX": 0.6039271433217751,
+            "velocityY": -0.04731813835356722,
+            "timestamp": 0.12137151628944681
+        },
+        {
+            "x": 2.3185983013070146,
+            "y": 5.55505416569458,
+            "heading": 2.7318619262997976,
+            "angularVelocity": -4.326483366247482,
+            "velocityX": 0.7801864739721567,
+            "velocityY": -0.07747485414348045,
+            "timestamp": 0.15171439536180853
+        },
+        {
+            "x": 2.348656357948328,
+            "y": 5.552070493546886,
+            "heading": 2.5849479304647764,
+            "angularVelocity": -4.841794856864455,
+            "velocityX": 0.9906132035009321,
+            "velocityY": -0.0983318735370822,
+            "timestamp": 0.18205727443417025
+        },
+        {
+            "x": 2.38579846451748,
+            "y": 5.549354804791822,
+            "heading": 2.427729518710298,
+            "angularVelocity": -5.181394006137112,
+            "velocityX": 1.2240798403004254,
+            "velocityY": -0.08950003553016357,
+            "timestamp": 0.21240015350653196
+        },
+        {
+            "x": 2.430432513687245,
+            "y": 5.547202777187511,
+            "heading": 2.264898570795434,
+            "angularVelocity": -5.366364461544502,
+            "velocityX": 1.470989257918541,
+            "velocityY": -0.07092364568235572,
+            "timestamp": 0.24274303257889368
+        },
+        {
+            "x": 2.4823970400851527,
+            "y": 5.546086320842798,
+            "heading": 2.109408164193531,
+            "angularVelocity": -5.12444472494155,
+            "velocityX": 1.7125773158830133,
+            "velocityY": -0.03679467403373135,
+            "timestamp": 0.2730859116512554
+        },
+        {
+            "x": 2.541153647389717,
+            "y": 5.546054192747997,
+            "heading": 1.9668933595072364,
+            "angularVelocity": -4.696812202508044,
+            "velocityX": 1.936421628430232,
+            "velocityY": -0.0010588347507973917,
+            "timestamp": 0.3034287907236171
+        },
+        {
+            "x": 2.60683848805178,
+            "y": 5.546562966972454,
+            "heading": 1.8368003827209543,
+            "angularVelocity": -4.287430222954007,
+            "velocityX": 2.164753071236843,
+            "velocityY": 0.016767499987179104,
+            "timestamp": 0.33377166979597883
+        },
+        {
+            "x": 2.679458734246464,
+            "y": 5.546832906614029,
+            "heading": 1.7192242968736162,
+            "angularVelocity": -3.874915282987571,
+            "velocityX": 2.3933208849924683,
+            "velocityY": 0.008896309441567017,
+            "timestamp": 0.36411454886834055
+        },
+        {
+            "x": 2.7588038922481295,
+            "y": 5.545750285790584,
+            "heading": 1.6147732040322635,
+            "angularVelocity": -3.442359328930443,
+            "velocityX": 2.6149515282463223,
+            "velocityY": -0.035679568206544436,
+            "timestamp": 0.39445742794070227
+        },
+        {
+            "x": 2.844227039552491,
+            "y": 5.54174873240555,
+            "heading": 1.5239846755378514,
+            "angularVelocity": -2.992086818060329,
+            "velocityX": 2.8152617653929393,
+            "velocityY": -0.13187784110699777,
+            "timestamp": 0.424800307013064
+        },
+        {
+            "x": 2.9352842549562403,
+            "y": 5.535535053694752,
+            "heading": 1.4520400757805025,
+            "angularVelocity": -2.371053833941581,
+            "velocityX": 3.0009418416293347,
+            "velocityY": -0.20478210706307357,
+            "timestamp": 0.4551431860854257
+        },
+        {
+            "x": 3.0322214391168845,
+            "y": 5.5275999113119605,
+            "heading": 1.3988891918450945,
+            "angularVelocity": -1.7516756998785012,
+            "velocityX": 3.1947259826421948,
+            "velocityY": -0.26151580289620013,
+            "timestamp": 0.4854860651577874
+        },
+        {
+            "x": 3.1352234766500766,
+            "y": 5.5186341666131185,
+            "heading": 1.36445468986127,
+            "angularVelocity": -1.1348462320172443,
+            "velocityX": 3.394603303383066,
+            "velocityY": -0.2954810147534299,
+            "timestamp": 0.5158289442301491
+        },
+        {
+            "x": 3.2443643044344928,
+            "y": 5.509491432490049,
+            "heading": 1.3485232387156947,
+            "angularVelocity": -0.525047445484061,
+            "velocityX": 3.5969173368201925,
+            "velocityY": -0.3013139953287366,
+            "timestamp": 0.5461718233025108
+        },
+        {
+            "x": 3.358782949449409,
+            "y": 5.500338621504484,
+            "heading": 1.3485196823103536,
+            "angularVelocity": -0.00011720724762443023,
+            "velocityX": 3.7708565736972712,
+            "velocityY": -0.3016460950767756,
+            "timestamp": 0.5765147023748725
+        },
+        {
+            "x": 3.4684760810380606,
+            "y": 5.486343295947389,
+            "heading": 1.335475696910559,
+            "angularVelocity": -0.42988621378634684,
+            "velocityX": 3.615119426441224,
+            "velocityY": -0.4612392094935691,
+            "timestamp": 0.6068575814472341
+        },
+        {
+            "x": 3.573383189765519,
+            "y": 5.467459869277216,
+            "heading": 1.309845724977452,
+            "angularVelocity": -0.8446783138799892,
+            "velocityX": 3.457388090209756,
+            "velocityY": -0.6223347041373206,
+            "timestamp": 0.6372004605195958
+        },
+        {
+            "x": 3.6735704123100494,
+            "y": 5.443371550860819,
+            "heading": 1.272922802934567,
+            "angularVelocity": -1.216856250022656,
+            "velocityX": 3.301836398108563,
+            "velocityY": -0.7938705605012504,
+            "timestamp": 0.6675433395919574
+        },
+        {
+            "x": 3.7692345180845526,
+            "y": 5.4136136865914954,
+            "heading": 1.2267696953368106,
+            "angularVelocity": -1.521052352602741,
+            "velocityX": 3.152769568977396,
+            "velocityY": -0.9807198650582027,
+            "timestamp": 0.6978862186643191
+        },
+        {
+            "x": 3.860739036058752,
+            "y": 5.377644621585128,
+            "heading": 1.1742567314728194,
+            "angularVelocity": -1.730651983905632,
+            "velocityX": 3.0156834411124778,
+            "velocityY": -1.1854203063785596,
+            "timestamp": 0.7282290977366808
+        },
+        {
+            "x": 3.9485394410884225,
+            "y": 5.335005141499111,
+            "heading": 1.1187025848702756,
+            "angularVelocity": -1.830879214528657,
+            "velocityX": 2.893608243973284,
+            "velocityY": -1.4052549194270891,
+            "timestamp": 0.7585719768090424
+        },
+        {
+            "x": 4.032804489135742,
+            "y": 5.285560607910156,
+            "heading": 1.064415461383156,
+            "angularVelocity": -1.7891223623722634,
+            "velocityX": 2.7770946799861798,
+            "velocityY": -1.6295267654410455,
+            "timestamp": 0.7889148558814041
+        },
+        {
+            "x": 4.1733297943826075,
+            "y": 5.174158598481438,
+            "heading": 0.9768931525481981,
+            "angularVelocity": -1.5934853179105337,
+            "velocityX": 2.55849067153879,
+            "velocityY": -2.0282539248952025,
+            "timestamp": 0.8438399360507789
+        },
+        {
+            "x": 4.303490854924698,
+            "y": 5.0407757547823575,
+            "heading": 0.9131112872795231,
+            "angularVelocity": -1.1612521105474607,
+            "velocityX": 2.369792818521288,
+            "velocityY": -2.428450596480911,
+            "timestamp": 0.8987650162201537
+        },
+        {
+            "x": 4.424699657845323,
+            "y": 4.885599604328108,
+            "heading": 0.881895788498386,
+            "angularVelocity": -0.5683286885495016,
+            "velocityX": 2.206802476152012,
+            "velocityY": -2.8252330260734433,
+            "timestamp": 0.9536900963895285
+        },
+        {
+            "x": 4.534874604263042,
+            "y": 4.709437065112464,
+            "heading": 0.8818953110956026,
+            "angularVelocity": -0.000008691890515565163,
+            "velocityX": 2.005913256347883,
+            "velocityY": -3.2073242073093726,
+            "timestamp": 1.0086151765589033
+        },
+        {
+            "x": 4.656392865207935,
+            "y": 4.550190331352248,
+            "heading": 0.8602662015486029,
+            "angularVelocity": -0.3937929536070082,
+            "velocityX": 2.2124366604502335,
+            "velocityY": -2.899344584826112,
+            "timestamp": 1.0635402567282781
+        },
+        {
+            "x": 4.786084989350276,
+            "y": 4.412405730848589,
+            "heading": 0.8009192530896463,
+            "angularVelocity": -1.0805072705573824,
+            "velocityX": 2.3612550722257284,
+            "velocityY": -2.508591704896303,
+            "timestamp": 1.118465336897653
+        },
+        {
+            "x": 4.924876209285355,
+            "y": 4.297327162575611,
+            "heading": 0.7205421769770328,
+            "angularVelocity": -1.463394789133692,
+            "velocityX": 2.5269188412121024,
+            "velocityY": -2.0951916304556226,
+            "timestamp": 1.1733904170670277
+        },
+        {
+            "x": 5.072286801869267,
+            "y": 4.205735025902594,
+            "heading": 0.6308477205816693,
+            "angularVelocity": -1.6330327806307947,
+            "velocityX": 2.6838484737634416,
+            "velocityY": -1.6675831221469524,
+            "timestamp": 1.2283154972364025
+        },
+        {
+            "x": 5.227765920434763,
+            "y": 4.1379313687941846,
+            "heading": 0.5364496515071664,
+            "angularVelocity": -1.7186696639022372,
+            "velocityX": 2.830749050998892,
+            "velocityY": -1.2344753416712253,
+            "timestamp": 1.2832405774057773
+        },
+        {
+            "x": 5.390789031982422,
+            "y": 4.09412145614624,
+            "heading": 0.44081122980213755,
+            "angularVelocity": -1.7412522914869581,
+            "velocityX": 2.968099655839153,
+            "velocityY": -0.7976303814732078,
+            "timestamp": 1.3381656575751522
+        },
+        {
+            "x": 5.527208209694737,
+            "y": 4.074474240030864,
+            "heading": 0.3646632920053755,
+            "angularVelocity": -1.7150127332494942,
+            "velocityX": 3.0724486257327652,
+            "velocityY": -0.44249689204593257,
+            "timestamp": 1.3825664578260528
+        },
+        {
+            "x": 5.669753286288942,
+            "y": 4.0699039581909835,
+            "heading": 0.2967653403322419,
+            "angularVelocity": -1.529205583896121,
+            "velocityX": 3.2104168345775173,
+            "velocityY": -0.10293242045310311,
+            "timestamp": 1.4269672580769535
+        },
+        {
+            "x": 5.825688318788907,
+            "y": 4.069335183916362,
+            "heading": 0.2632559067133074,
+            "angularVelocity": -0.7547033708757247,
+            "velocityX": 3.5119869826400643,
+            "velocityY": -0.012810000527210512,
+            "timestamp": 1.4713680583278541
+        },
+        {
+            "x": 5.986467377363025,
+            "y": 4.053794613625281,
+            "heading": 0.24734267435752091,
+            "angularVelocity": -0.35839967446225596,
+            "velocityX": 3.6210847026536497,
+            "velocityY": -0.3500065359917586,
+            "timestamp": 1.5157688585787548
+        },
+        {
+            "x": 6.146135354603106,
+            "y": 4.023716272835588,
+            "heading": 0.25998224016695326,
+            "angularVelocity": 0.28466977482406836,
+            "velocityX": 3.5960607993060463,
+            "velocityY": -0.6774278981398296,
+            "timestamp": 1.5601696588296554
+        },
+        {
+            "x": 6.299693722526358,
+            "y": 3.978509002442476,
+            "heading": 0.279745523920822,
+            "angularVelocity": 0.4451109809325518,
+            "velocityX": 3.4584594659447885,
+            "velocityY": -1.0181634145703302,
+            "timestamp": 1.604570459080556
+        },
+        {
+            "x": 6.446348354717166,
+            "y": 3.918416502871273,
+            "heading": 0.30303477341760615,
+            "angularVelocity": 0.5245231924915958,
+            "velocityX": 3.3029727248628453,
+            "velocityY": -1.3534102816082356,
+            "timestamp": 1.6489712593314567
+        },
+        {
+            "x": 6.585501043822193,
+            "y": 3.8437018977675175,
+            "heading": 0.32757979886137184,
+            "angularVelocity": 0.5528059247821282,
+            "velocityX": 3.134013088023211,
+            "velocityY": -1.6827310472234156,
+            "timestamp": 1.6933720595823574
+        },
+        {
+            "x": 6.716485001612935,
+            "y": 3.754724015839551,
+            "heading": 0.3514161297676369,
+            "angularVelocity": 0.5368446237808859,
+            "velocityX": 2.950035968959475,
+            "velocityY": -2.0039702308329863,
+            "timestamp": 1.737772859833258
+        },
+        {
+            "x": 6.838464987128356,
+            "y": 3.6520072183603935,
+            "heading": 0.37305697715168934,
+            "angularVelocity": 0.48739768791922733,
+            "velocityX": 2.747247455589403,
+            "velocityY": -2.313399688715609,
+            "timestamp": 1.7821736600841587
+        },
+        {
+            "x": 6.950225204946805,
+            "y": 3.536435013339574,
+            "heading": 0.39217088238591646,
+            "angularVelocity": 0.430485602201265,
+            "velocityX": 2.5170766559817053,
+            "velocityY": -2.60293067619823,
+            "timestamp": 1.8265744603350593
+        },
+        {
+            "x": 7.049675913816151,
+            "y": 3.409971379240331,
+            "heading": 0.4121286773514249,
+            "angularVelocity": 0.44949178511942617,
+            "velocityX": 2.23984046024776,
+            "velocityY": -2.848228711748902,
+            "timestamp": 1.87097526058596
+        },
+        {
+            "x": 7.134078905097545,
+            "y": 3.2784202019721485,
+            "heading": 0.44760621801548245,
+            "angularVelocity": 0.799029307210242,
+            "velocityX": 1.9009340102981855,
+            "velocityY": -2.9628109521632653,
+            "timestamp": 1.9153760608368606
+        },
+        {
+            "x": 7.206535248844668,
+            "y": 3.1515334712387078,
+            "heading": 0.5188452438191252,
+            "angularVelocity": 1.6044536450037898,
+            "velocityX": 1.6318702216556016,
+            "velocityY": -2.857757743473708,
+            "timestamp": 1.9597768610877613
+        },
+        {
+            "x": 7.286286308378336,
+            "y": 3.038022763329368,
+            "heading": 0.612459613040285,
+            "angularVelocity": 2.1083937382245868,
+            "velocityX": 1.796162661100898,
+            "velocityY": -2.556501397900756,
+            "timestamp": 2.004177661338662
+        },
+        {
+            "x": 7.36964311440034,
+            "y": 2.9389690479632176,
+            "heading": 0.7307138885874557,
+            "angularVelocity": 2.6633365812989322,
+            "velocityX": 1.8773717039100728,
+            "velocityY": -2.2308993262827714,
+            "timestamp": 2.0485784615895626
+        },
+        {
+            "x": 7.443994240643498,
+            "y": 2.8513361754602453,
+            "heading": 0.8783950719218284,
+            "angularVelocity": 3.326092829405186,
+            "velocityX": 1.674544733946552,
+            "velocityY": -1.9736777717467908,
+            "timestamp": 2.0929792618404632
+        },
+        {
+            "x": 7.506362936213548,
+            "y": 2.772668193478103,
+            "heading": 1.052033337356815,
+            "angularVelocity": 3.9107012588464345,
+            "velocityX": 1.4046750332790265,
+            "velocityY": -1.7717694622079865,
+            "timestamp": 2.137380062091364
+        },
+        {
+            "x": 7.555798075190431,
+            "y": 2.7014303099088472,
+            "heading": 1.2519057769914284,
+            "angularVelocity": 4.501550388848195,
+            "velocityX": 1.1133839637469243,
+            "velocityY": -1.6044279194677442,
+            "timestamp": 2.1817808623422645
+        },
+        {
+            "x": 7.592358210662926,
+            "y": 2.6364422153707756,
+            "heading": 1.482218675635827,
+            "angularVelocity": 5.187133955760781,
+            "velocityX": 0.8234116337070656,
+            "velocityY": -1.4636694422360859,
+            "timestamp": 2.226181662593165
+        },
+        {
+            "x": 7.621254126448735,
+            "y": 2.5813646495023557,
+            "heading": 1.7524007538852981,
+            "angularVelocity": 6.085072267227659,
+            "velocityX": 0.6507971843417887,
+            "velocityY": -1.2404633600562773,
+            "timestamp": 2.270582462844066
+        },
+        {
+            "x": 7.645052051647252,
+            "y": 2.5375067096061756,
+            "heading": 2.067951038154778,
+            "angularVelocity": 7.10686029275066,
+            "velocityX": 0.5359796459532341,
+            "velocityY": -0.9877736357981667,
+            "timestamp": 2.3149832630949665
+        },
+        {
+            "x": 7.661427436541854,
+            "y": 2.5075370043722347,
+            "heading": 2.400674072151774,
+            "angularVelocity": 7.493626964307851,
+            "velocityX": 0.36880832782444795,
+            "velocityY": -0.6749811954871879,
+            "timestamp": 2.359384063345867
+        },
+        {
+            "x": 7.6692908711764884,
+            "y": 2.4875264930527843,
+            "heading": 2.6956822674348517,
+            "angularVelocity": 6.644208969569037,
+            "velocityX": 0.17710119164969557,
+            "velocityY": -0.4506790689891867,
+            "timestamp": 2.403784863596768
+        },
+        {
+            "x": 7.674629616295112,
+            "y": 2.472122256604734,
+            "heading": 2.9172973649743583,
+            "angularVelocity": 4.991241065188027,
+            "velocityX": 0.12023984001312674,
+            "velocityY": -0.3469360092836942,
+            "timestamp": 2.4481856638476684
+        },
+        {
+            "x": 7.678206340030146,
+            "y": 2.4619495353215974,
+            "heading": 3.0655099943755024,
+            "angularVelocity": 3.3380621196830216,
+            "velocityX": 0.0805553889754595,
+            "velocityY": -0.22911121479010477,
+            "timestamp": 2.492586464098569
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 1.6776725915652129,
+            "velocityX": 0.04039701896629419,
+            "velocityY": -0.11147401158601906,
+            "timestamp": 2.5369872643494698
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -5.831834401931518e-35,
+            "velocityX": -1.3540056730700436e-35,
+            "velocityY": -1.938442767179852e-35,
+            "timestamp": 2.5813880646003704
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.traj
new file mode 100644
index 00000000..135fb8ca
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.traj	
@@ -0,0 +1,589 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.254338747446347,
+            "y": 5.559909413116517,
+            "heading": 3.1115254109417156,
+            "angularVelocity": -0.9384273980850008,
+            "velocityX": 0.14299063170637197,
+            "velocityY": -0.0029854412716235888,
+            "timestamp": 0.030342879072361703
+        },
+        {
+            "x": 2.263150716185563,
+            "y": 5.559590842447939,
+            "heading": 3.055209315133754,
+            "angularVelocity": -1.855990516709349,
+            "velocityX": 0.29041307247742426,
+            "velocityY": -0.010499025745671282,
+            "timestamp": 0.060685758144723406
+        },
+        {
+            "x": 2.2766003091950555,
+            "y": 5.558840744374995,
+            "heading": 2.9719321573579647,
+            "angularVelocity": -2.744537114529888,
+            "velocityX": 0.44325368655420766,
+            "velocityY": -0.024720728417179556,
+            "timestamp": 0.09102863721708511
+        },
+        {
+            "x": 2.2949251974733853,
+            "y": 5.557404975825004,
+            "heading": 2.863139887890429,
+            "angularVelocity": -3.5854300182948253,
+            "velocityX": 0.6039271433217751,
+            "velocityY": -0.04731813835356722,
+            "timestamp": 0.12137151628944681
+        },
+        {
+            "x": 2.3185983013070146,
+            "y": 5.55505416569458,
+            "heading": 2.7318619262997976,
+            "angularVelocity": -4.326483366247482,
+            "velocityX": 0.7801864739721567,
+            "velocityY": -0.07747485414348045,
+            "timestamp": 0.15171439536180853
+        },
+        {
+            "x": 2.348656357948328,
+            "y": 5.552070493546886,
+            "heading": 2.5849479304647764,
+            "angularVelocity": -4.841794856864455,
+            "velocityX": 0.9906132035009321,
+            "velocityY": -0.0983318735370822,
+            "timestamp": 0.18205727443417025
+        },
+        {
+            "x": 2.38579846451748,
+            "y": 5.549354804791822,
+            "heading": 2.427729518710298,
+            "angularVelocity": -5.181394006137112,
+            "velocityX": 1.2240798403004254,
+            "velocityY": -0.08950003553016357,
+            "timestamp": 0.21240015350653196
+        },
+        {
+            "x": 2.430432513687245,
+            "y": 5.547202777187511,
+            "heading": 2.264898570795434,
+            "angularVelocity": -5.366364461544502,
+            "velocityX": 1.470989257918541,
+            "velocityY": -0.07092364568235572,
+            "timestamp": 0.24274303257889368
+        },
+        {
+            "x": 2.4823970400851527,
+            "y": 5.546086320842798,
+            "heading": 2.109408164193531,
+            "angularVelocity": -5.12444472494155,
+            "velocityX": 1.7125773158830133,
+            "velocityY": -0.03679467403373135,
+            "timestamp": 0.2730859116512554
+        },
+        {
+            "x": 2.541153647389717,
+            "y": 5.546054192747997,
+            "heading": 1.9668933595072364,
+            "angularVelocity": -4.696812202508044,
+            "velocityX": 1.936421628430232,
+            "velocityY": -0.0010588347507973917,
+            "timestamp": 0.3034287907236171
+        },
+        {
+            "x": 2.60683848805178,
+            "y": 5.546562966972454,
+            "heading": 1.8368003827209543,
+            "angularVelocity": -4.287430222954007,
+            "velocityX": 2.164753071236843,
+            "velocityY": 0.016767499987179104,
+            "timestamp": 0.33377166979597883
+        },
+        {
+            "x": 2.679458734246464,
+            "y": 5.546832906614029,
+            "heading": 1.7192242968736162,
+            "angularVelocity": -3.874915282987571,
+            "velocityX": 2.3933208849924683,
+            "velocityY": 0.008896309441567017,
+            "timestamp": 0.36411454886834055
+        },
+        {
+            "x": 2.7588038922481295,
+            "y": 5.545750285790584,
+            "heading": 1.6147732040322635,
+            "angularVelocity": -3.442359328930443,
+            "velocityX": 2.6149515282463223,
+            "velocityY": -0.035679568206544436,
+            "timestamp": 0.39445742794070227
+        },
+        {
+            "x": 2.844227039552491,
+            "y": 5.54174873240555,
+            "heading": 1.5239846755378514,
+            "angularVelocity": -2.992086818060329,
+            "velocityX": 2.8152617653929393,
+            "velocityY": -0.13187784110699777,
+            "timestamp": 0.424800307013064
+        },
+        {
+            "x": 2.9352842549562403,
+            "y": 5.535535053694752,
+            "heading": 1.4520400757805025,
+            "angularVelocity": -2.371053833941581,
+            "velocityX": 3.0009418416293347,
+            "velocityY": -0.20478210706307357,
+            "timestamp": 0.4551431860854257
+        },
+        {
+            "x": 3.0322214391168845,
+            "y": 5.5275999113119605,
+            "heading": 1.3988891918450945,
+            "angularVelocity": -1.7516756998785012,
+            "velocityX": 3.1947259826421948,
+            "velocityY": -0.26151580289620013,
+            "timestamp": 0.4854860651577874
+        },
+        {
+            "x": 3.1352234766500766,
+            "y": 5.5186341666131185,
+            "heading": 1.36445468986127,
+            "angularVelocity": -1.1348462320172443,
+            "velocityX": 3.394603303383066,
+            "velocityY": -0.2954810147534299,
+            "timestamp": 0.5158289442301491
+        },
+        {
+            "x": 3.2443643044344928,
+            "y": 5.509491432490049,
+            "heading": 1.3485232387156947,
+            "angularVelocity": -0.525047445484061,
+            "velocityX": 3.5969173368201925,
+            "velocityY": -0.3013139953287366,
+            "timestamp": 0.5461718233025108
+        },
+        {
+            "x": 3.358782949449409,
+            "y": 5.500338621504484,
+            "heading": 1.3485196823103536,
+            "angularVelocity": -0.00011720724762443023,
+            "velocityX": 3.7708565736972712,
+            "velocityY": -0.3016460950767756,
+            "timestamp": 0.5765147023748725
+        },
+        {
+            "x": 3.4684760810380606,
+            "y": 5.486343295947389,
+            "heading": 1.335475696910559,
+            "angularVelocity": -0.42988621378634684,
+            "velocityX": 3.615119426441224,
+            "velocityY": -0.4612392094935691,
+            "timestamp": 0.6068575814472341
+        },
+        {
+            "x": 3.573383189765519,
+            "y": 5.467459869277216,
+            "heading": 1.309845724977452,
+            "angularVelocity": -0.8446783138799892,
+            "velocityX": 3.457388090209756,
+            "velocityY": -0.6223347041373206,
+            "timestamp": 0.6372004605195958
+        },
+        {
+            "x": 3.6735704123100494,
+            "y": 5.443371550860819,
+            "heading": 1.272922802934567,
+            "angularVelocity": -1.216856250022656,
+            "velocityX": 3.301836398108563,
+            "velocityY": -0.7938705605012504,
+            "timestamp": 0.6675433395919574
+        },
+        {
+            "x": 3.7692345180845526,
+            "y": 5.4136136865914954,
+            "heading": 1.2267696953368106,
+            "angularVelocity": -1.521052352602741,
+            "velocityX": 3.152769568977396,
+            "velocityY": -0.9807198650582027,
+            "timestamp": 0.6978862186643191
+        },
+        {
+            "x": 3.860739036058752,
+            "y": 5.377644621585128,
+            "heading": 1.1742567314728194,
+            "angularVelocity": -1.730651983905632,
+            "velocityX": 3.0156834411124778,
+            "velocityY": -1.1854203063785596,
+            "timestamp": 0.7282290977366808
+        },
+        {
+            "x": 3.9485394410884225,
+            "y": 5.335005141499111,
+            "heading": 1.1187025848702756,
+            "angularVelocity": -1.830879214528657,
+            "velocityX": 2.893608243973284,
+            "velocityY": -1.4052549194270891,
+            "timestamp": 0.7585719768090424
+        },
+        {
+            "x": 4.032804489135742,
+            "y": 5.285560607910156,
+            "heading": 1.064415461383156,
+            "angularVelocity": -1.7891223623722634,
+            "velocityX": 2.7770946799861798,
+            "velocityY": -1.6295267654410455,
+            "timestamp": 0.7889148558814041
+        },
+        {
+            "x": 4.1733297943826075,
+            "y": 5.174158598481438,
+            "heading": 0.9768931525481981,
+            "angularVelocity": -1.5934853179105337,
+            "velocityX": 2.55849067153879,
+            "velocityY": -2.0282539248952025,
+            "timestamp": 0.8438399360507789
+        },
+        {
+            "x": 4.303490854924698,
+            "y": 5.0407757547823575,
+            "heading": 0.9131112872795231,
+            "angularVelocity": -1.1612521105474607,
+            "velocityX": 2.369792818521288,
+            "velocityY": -2.428450596480911,
+            "timestamp": 0.8987650162201537
+        },
+        {
+            "x": 4.424699657845323,
+            "y": 4.885599604328108,
+            "heading": 0.881895788498386,
+            "angularVelocity": -0.5683286885495016,
+            "velocityX": 2.206802476152012,
+            "velocityY": -2.8252330260734433,
+            "timestamp": 0.9536900963895285
+        },
+        {
+            "x": 4.534874604263042,
+            "y": 4.709437065112464,
+            "heading": 0.8818953110956026,
+            "angularVelocity": -0.000008691890515565163,
+            "velocityX": 2.005913256347883,
+            "velocityY": -3.2073242073093726,
+            "timestamp": 1.0086151765589033
+        },
+        {
+            "x": 4.656392865207935,
+            "y": 4.550190331352248,
+            "heading": 0.8602662015486029,
+            "angularVelocity": -0.3937929536070082,
+            "velocityX": 2.2124366604502335,
+            "velocityY": -2.899344584826112,
+            "timestamp": 1.0635402567282781
+        },
+        {
+            "x": 4.786084989350276,
+            "y": 4.412405730848589,
+            "heading": 0.8009192530896463,
+            "angularVelocity": -1.0805072705573824,
+            "velocityX": 2.3612550722257284,
+            "velocityY": -2.508591704896303,
+            "timestamp": 1.118465336897653
+        },
+        {
+            "x": 4.924876209285355,
+            "y": 4.297327162575611,
+            "heading": 0.7205421769770328,
+            "angularVelocity": -1.463394789133692,
+            "velocityX": 2.5269188412121024,
+            "velocityY": -2.0951916304556226,
+            "timestamp": 1.1733904170670277
+        },
+        {
+            "x": 5.072286801869267,
+            "y": 4.205735025902594,
+            "heading": 0.6308477205816693,
+            "angularVelocity": -1.6330327806307947,
+            "velocityX": 2.6838484737634416,
+            "velocityY": -1.6675831221469524,
+            "timestamp": 1.2283154972364025
+        },
+        {
+            "x": 5.227765920434763,
+            "y": 4.1379313687941846,
+            "heading": 0.5364496515071664,
+            "angularVelocity": -1.7186696639022372,
+            "velocityX": 2.830749050998892,
+            "velocityY": -1.2344753416712253,
+            "timestamp": 1.2832405774057773
+        },
+        {
+            "x": 5.390789031982422,
+            "y": 4.09412145614624,
+            "heading": 0.44081122980213755,
+            "angularVelocity": -1.7412522914869581,
+            "velocityX": 2.968099655839153,
+            "velocityY": -0.7976303814732078,
+            "timestamp": 1.3381656575751522
+        },
+        {
+            "x": 5.527208209694737,
+            "y": 4.074474240030864,
+            "heading": 0.3646632920053755,
+            "angularVelocity": -1.7150127332494942,
+            "velocityX": 3.0724486257327652,
+            "velocityY": -0.44249689204593257,
+            "timestamp": 1.3825664578260528
+        },
+        {
+            "x": 5.669753286288942,
+            "y": 4.0699039581909835,
+            "heading": 0.2967653403322419,
+            "angularVelocity": -1.529205583896121,
+            "velocityX": 3.2104168345775173,
+            "velocityY": -0.10293242045310311,
+            "timestamp": 1.4269672580769535
+        },
+        {
+            "x": 5.825688318788907,
+            "y": 4.069335183916362,
+            "heading": 0.2632559067133074,
+            "angularVelocity": -0.7547033708757247,
+            "velocityX": 3.5119869826400643,
+            "velocityY": -0.012810000527210512,
+            "timestamp": 1.4713680583278541
+        },
+        {
+            "x": 5.986467377363025,
+            "y": 4.053794613625281,
+            "heading": 0.24734267435752091,
+            "angularVelocity": -0.35839967446225596,
+            "velocityX": 3.6210847026536497,
+            "velocityY": -0.3500065359917586,
+            "timestamp": 1.5157688585787548
+        },
+        {
+            "x": 6.146135354603106,
+            "y": 4.023716272835588,
+            "heading": 0.25998224016695326,
+            "angularVelocity": 0.28466977482406836,
+            "velocityX": 3.5960607993060463,
+            "velocityY": -0.6774278981398296,
+            "timestamp": 1.5601696588296554
+        },
+        {
+            "x": 6.299693722526358,
+            "y": 3.978509002442476,
+            "heading": 0.279745523920822,
+            "angularVelocity": 0.4451109809325518,
+            "velocityX": 3.4584594659447885,
+            "velocityY": -1.0181634145703302,
+            "timestamp": 1.604570459080556
+        },
+        {
+            "x": 6.446348354717166,
+            "y": 3.918416502871273,
+            "heading": 0.30303477341760615,
+            "angularVelocity": 0.5245231924915958,
+            "velocityX": 3.3029727248628453,
+            "velocityY": -1.3534102816082356,
+            "timestamp": 1.6489712593314567
+        },
+        {
+            "x": 6.585501043822193,
+            "y": 3.8437018977675175,
+            "heading": 0.32757979886137184,
+            "angularVelocity": 0.5528059247821282,
+            "velocityX": 3.134013088023211,
+            "velocityY": -1.6827310472234156,
+            "timestamp": 1.6933720595823574
+        },
+        {
+            "x": 6.716485001612935,
+            "y": 3.754724015839551,
+            "heading": 0.3514161297676369,
+            "angularVelocity": 0.5368446237808859,
+            "velocityX": 2.950035968959475,
+            "velocityY": -2.0039702308329863,
+            "timestamp": 1.737772859833258
+        },
+        {
+            "x": 6.838464987128356,
+            "y": 3.6520072183603935,
+            "heading": 0.37305697715168934,
+            "angularVelocity": 0.48739768791922733,
+            "velocityX": 2.747247455589403,
+            "velocityY": -2.313399688715609,
+            "timestamp": 1.7821736600841587
+        },
+        {
+            "x": 6.950225204946805,
+            "y": 3.536435013339574,
+            "heading": 0.39217088238591646,
+            "angularVelocity": 0.430485602201265,
+            "velocityX": 2.5170766559817053,
+            "velocityY": -2.60293067619823,
+            "timestamp": 1.8265744603350593
+        },
+        {
+            "x": 7.049675913816151,
+            "y": 3.409971379240331,
+            "heading": 0.4121286773514249,
+            "angularVelocity": 0.44949178511942617,
+            "velocityX": 2.23984046024776,
+            "velocityY": -2.848228711748902,
+            "timestamp": 1.87097526058596
+        },
+        {
+            "x": 7.134078905097545,
+            "y": 3.2784202019721485,
+            "heading": 0.44760621801548245,
+            "angularVelocity": 0.799029307210242,
+            "velocityX": 1.9009340102981855,
+            "velocityY": -2.9628109521632653,
+            "timestamp": 1.9153760608368606
+        },
+        {
+            "x": 7.206535248844668,
+            "y": 3.1515334712387078,
+            "heading": 0.5188452438191252,
+            "angularVelocity": 1.6044536450037898,
+            "velocityX": 1.6318702216556016,
+            "velocityY": -2.857757743473708,
+            "timestamp": 1.9597768610877613
+        },
+        {
+            "x": 7.286286308378336,
+            "y": 3.038022763329368,
+            "heading": 0.612459613040285,
+            "angularVelocity": 2.1083937382245868,
+            "velocityX": 1.796162661100898,
+            "velocityY": -2.556501397900756,
+            "timestamp": 2.004177661338662
+        },
+        {
+            "x": 7.36964311440034,
+            "y": 2.9389690479632176,
+            "heading": 0.7307138885874557,
+            "angularVelocity": 2.6633365812989322,
+            "velocityX": 1.8773717039100728,
+            "velocityY": -2.2308993262827714,
+            "timestamp": 2.0485784615895626
+        },
+        {
+            "x": 7.443994240643498,
+            "y": 2.8513361754602453,
+            "heading": 0.8783950719218284,
+            "angularVelocity": 3.326092829405186,
+            "velocityX": 1.674544733946552,
+            "velocityY": -1.9736777717467908,
+            "timestamp": 2.0929792618404632
+        },
+        {
+            "x": 7.506362936213548,
+            "y": 2.772668193478103,
+            "heading": 1.052033337356815,
+            "angularVelocity": 3.9107012588464345,
+            "velocityX": 1.4046750332790265,
+            "velocityY": -1.7717694622079865,
+            "timestamp": 2.137380062091364
+        },
+        {
+            "x": 7.555798075190431,
+            "y": 2.7014303099088472,
+            "heading": 1.2519057769914284,
+            "angularVelocity": 4.501550388848195,
+            "velocityX": 1.1133839637469243,
+            "velocityY": -1.6044279194677442,
+            "timestamp": 2.1817808623422645
+        },
+        {
+            "x": 7.592358210662926,
+            "y": 2.6364422153707756,
+            "heading": 1.482218675635827,
+            "angularVelocity": 5.187133955760781,
+            "velocityX": 0.8234116337070656,
+            "velocityY": -1.4636694422360859,
+            "timestamp": 2.226181662593165
+        },
+        {
+            "x": 7.621254126448735,
+            "y": 2.5813646495023557,
+            "heading": 1.7524007538852981,
+            "angularVelocity": 6.085072267227659,
+            "velocityX": 0.6507971843417887,
+            "velocityY": -1.2404633600562773,
+            "timestamp": 2.270582462844066
+        },
+        {
+            "x": 7.645052051647252,
+            "y": 2.5375067096061756,
+            "heading": 2.067951038154778,
+            "angularVelocity": 7.10686029275066,
+            "velocityX": 0.5359796459532341,
+            "velocityY": -0.9877736357981667,
+            "timestamp": 2.3149832630949665
+        },
+        {
+            "x": 7.661427436541854,
+            "y": 2.5075370043722347,
+            "heading": 2.400674072151774,
+            "angularVelocity": 7.493626964307851,
+            "velocityX": 0.36880832782444795,
+            "velocityY": -0.6749811954871879,
+            "timestamp": 2.359384063345867
+        },
+        {
+            "x": 7.6692908711764884,
+            "y": 2.4875264930527843,
+            "heading": 2.6956822674348517,
+            "angularVelocity": 6.644208969569037,
+            "velocityX": 0.17710119164969557,
+            "velocityY": -0.4506790689891867,
+            "timestamp": 2.403784863596768
+        },
+        {
+            "x": 7.674629616295112,
+            "y": 2.472122256604734,
+            "heading": 2.9172973649743583,
+            "angularVelocity": 4.991241065188027,
+            "velocityX": 0.12023984001312674,
+            "velocityY": -0.3469360092836942,
+            "timestamp": 2.4481856638476684
+        },
+        {
+            "x": 7.678206340030146,
+            "y": 2.4619495353215974,
+            "heading": 3.0655099943755024,
+            "angularVelocity": 3.3380621196830216,
+            "velocityX": 0.0805553889754595,
+            "velocityY": -0.22911121479010477,
+            "timestamp": 2.492586464098569
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 1.6776725915652129,
+            "velocityX": 0.04039701896629419,
+            "velocityY": -0.11147401158601906,
+            "timestamp": 2.5369872643494698
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -5.831834401931518e-35,
+            "velocityX": -1.3540056730700436e-35,
+            "velocityY": -1.938442767179852e-35,
+            "timestamp": 2.5813880646003704
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.1.traj
new file mode 100644
index 00000000..ac17f8d7
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.1.traj	
@@ -0,0 +1,625 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -2.730529759762368e-29,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2482118663055717,
+            "y": 5.55085896880981,
+            "heading": 3.1399999903330857,
+            "angularVelocity": -2.8923280730678937e-7,
+            "velocityX": -0.05350072563170815,
+            "velocityY": -0.2734984543883791,
+            "timestamp": 0.03342260639319256
+        },
+        {
+            "x": 2.24463557685663,
+            "y": 5.5325769109933445,
+            "heading": 3.1399999700354155,
+            "angularVelocity": -6.073036282303753e-7,
+            "velocityX": -0.10700211129830166,
+            "velocityY": -0.5469967722250598,
+            "timestamp": 0.06684521278638512
+        },
+        {
+            "x": 2.2392711054637595,
+            "y": 5.505153831967315,
+            "heading": 3.139999938021822,
+            "angularVelocity": -9.578425018869592e-7,
+            "velocityX": -0.16050428054878876,
+            "velocityY": -0.8204949279944175,
+            "timestamp": 0.1002678191795777
+        },
+        {
+            "x": 2.2321184205712044,
+            "y": 5.468589738256669,
+            "heading": 3.139999893047122,
+            "angularVelocity": -0.0000013456371104363374,
+            "velocityX": -0.2140073939433933,
+            "velocityY": -1.0939928885381913,
+            "timestamp": 0.13369042557277025
+        },
+        {
+            "x": 2.2231774834707245,
+            "y": 5.422884637863401,
+            "heading": 3.1399998336497137,
+            "angularVelocity": -0.000001777162647778402,
+            "velocityX": -0.2675116654835123,
+            "velocityY": -1.3674906096633432,
+            "timestamp": 0.1671130319659628
+        },
+        {
+            "x": 2.2124482456246803,
+            "y": 5.368038540819271,
+            "heading": 3.139999758066302,
+            "angularVelocity": -0.0000022614457562380146,
+            "velocityX": -0.3210173892634857,
+            "velocityY": -1.6409880306395483,
+            "timestamp": 0.20053563835915536
+        },
+        {
+            "x": 2.199930644465073,
+            "y": 5.30405146005317,
+            "heading": 3.139999664095155,
+            "angularVelocity": -0.0000028116043956655657,
+            "velocityX": -0.3745249850698391,
+            "velocityY": -1.9144850647849765,
+            "timestamp": 0.23395824475234792
+        },
+        {
+            "x": 2.1856245964060905,
+            "y": 5.230923412833771,
+            "heading": 3.1399995488597034,
+            "angularVelocity": -0.0000034478296167107145,
+            "velocityX": -0.4280350817528267,
+            "velocityY": -2.1879815822539745,
+            "timestamp": 0.2673808511455405
+        },
+        {
+            "x": 2.1695299843090114,
+            "y": 5.148654423357543,
+            "heading": 3.139999408375343,
+            "angularVelocity": -0.000004203273662517842,
+            "velocityX": -0.4815486832110918,
+            "velocityY": -2.461477375767798,
+            "timestamp": 0.30080345753873305
+        },
+        {
+            "x": 2.15164663254415,
+            "y": 5.057244527897619,
+            "heading": 3.139999236662074,
+            "angularVelocity": -0.000005137638535462322,
+            "velocityX": -0.535067539486715,
+            "velocityY": -2.7349720839942284,
+            "timestamp": 0.3342260639319256
+        },
+        {
+            "x": 2.1319742493327887,
+            "y": 4.956693786709351,
+            "heading": 3.1399990236359914,
+            "angularVelocity": -0.000006373712524248939,
+            "velocityX": -0.5885951257435309,
+            "velocityY": -3.008464989388312,
+            "timestamp": 0.36764867032511817
+        },
+        {
+            "x": 2.110512257081521,
+            "y": 4.847002319075526,
+            "heading": 3.13999874874802,
+            "angularVelocity": -0.000008224612190539215,
+            "velocityX": -0.6421399935196433,
+            "velocityY": -3.28195432585699,
+            "timestamp": 0.4010712767183107
+        },
+        {
+            "x": 2.087258999095236,
+            "y": 4.728170467468909,
+            "heading": 3.139998349832889,
+            "angularVelocity": -0.00001193548864433131,
+            "velocityX": -0.6957344293958355,
+            "velocityY": -3.555433415585576,
+            "timestamp": 0.4344938831115033
+        },
+        {
+            "x": 2.0629644962421034,
+            "y": 4.604090796237193,
+            "heading": 3.1399983498325463,
+            "angularVelocity": -1.0248884056674418e-11,
+            "velocityX": -0.7268883391614586,
+            "velocityY": -3.712447490549055,
+            "timestamp": 0.46791648950469583
+        },
+        {
+            "x": 2.0387448816492295,
+            "y": 4.479996482972041,
+            "heading": 3.139998349833487,
+            "angularVelocity": 2.8141899837441888e-11,
+            "velocityX": -0.7246476922026518,
+            "velocityY": -3.712885578275693,
+            "timestamp": 0.5013390958978884
+        },
+        {
+            "x": 2.0146234361555044,
+            "y": 4.355883050210464,
+            "heading": 3.139998349833582,
+            "angularVelocity": 2.8336640394810287e-12,
+            "velocityX": -0.7217104856241171,
+            "velocityY": -3.713457631102453,
+            "timestamp": 0.534761702291081
+        },
+        {
+            "x": 1.9906204010106838,
+            "y": 4.231746663125483,
+            "heading": 3.1399983498337,
+            "angularVelocity": 3.5336881649516105e-12,
+            "velocityX": -0.7181676635393196,
+            "velocityY": -3.714144421443354,
+            "timestamp": 0.5681843086842736
+        },
+        {
+            "x": 1.9667619480914416,
+            "y": 4.107582407046251,
+            "heading": 3.139998349833824,
+            "angularVelocity": 3.708930519775865e-12,
+            "velocityX": -0.7138417823660727,
+            "velocityY": -3.7149782580858504,
+            "timestamp": 0.6016069150774662
+        },
+        {
+            "x": 1.9503914931986956,
+            "y": 3.982210973196183,
+            "heading": 3.1399983498339594,
+            "angularVelocity": 4.046203674508551e-12,
+            "velocityX": -0.4898018634439243,
+            "velocityY": -3.751096858670778,
+            "timestamp": 0.6350295214706588
+        },
+        {
+            "x": 1.943224502199385,
+            "y": 3.8559785593201936,
+            "heading": 3.1399983498340998,
+            "angularVelocity": 4.203331220184306e-12,
+            "velocityX": -0.21443543076194166,
+            "velocityY": -3.7768572681307018,
+            "timestamp": 0.6684521278638514
+        },
+        {
+            "x": 1.9453593714612354,
+            "y": 3.7295608780305693,
+            "heading": 3.1399983498342436,
+            "angularVelocity": 4.304427970302576e-12,
+            "velocityX": 0.06387500833733431,
+            "velocityY": -3.7824004448488933,
+            "timestamp": 0.701874734257044
+        },
+        {
+            "x": 1.9567930300665735,
+            "y": 3.6036432110345795,
+            "heading": 3.139998349834389,
+            "angularVelocity": 4.350609924565741e-12,
+            "velocityX": 0.3420935659816486,
+            "velocityY": -3.767440082758668,
+            "timestamp": 0.7352973406502367
+        },
+        {
+            "x": 1.9774653925716308,
+            "y": 3.4789089323576285,
+            "heading": 3.1399983498345336,
+            "angularVelocity": 4.327887690330789e-12,
+            "velocityX": 0.6185143750313882,
+            "velocityY": -3.732033259450771,
+            "timestamp": 0.7687199470434293
+        },
+        {
+            "x": 2.0072643339986427,
+            "y": 3.3560349759367867,
+            "heading": 3.1399983498346824,
+            "angularVelocity": 4.434032141487508e-12,
+            "velocityX": 0.8915804194501273,
+            "velocityY": -3.6763726615238776,
+            "timestamp": 0.8021425534366219
+        },
+        {
+            "x": 2.0460281463567345,
+            "y": 3.235688177587346,
+            "heading": 3.1399983498348414,
+            "angularVelocity": 4.757708405328401e-12,
+            "velocityX": 1.1598081819894186,
+            "velocityY": -3.600760423457161,
+            "timestamp": 0.8355651598298145
+        },
+        {
+            "x": 2.093546464867386,
+            "y": 3.118521659475836,
+            "heading": 3.139998349835024,
+            "angularVelocity": 5.453307843013083e-12,
+            "velocityX": 1.421741857944163,
+            "velocityY": -3.5056068558230677,
+            "timestamp": 0.8689877662230071
+        },
+        {
+            "x": 2.1495614051818848,
+            "y": 3.005171298980713,
+            "heading": 3.1399983498353623,
+            "angularVelocity": 1.0122205936259764e-11,
+            "velocityX": 1.67595966800623,
+            "velocityY": -3.391427920419207,
+            "timestamp": 0.9024103726161997
+        },
+        {
+            "x": 2.2887605263975965,
+            "y": 2.8026901614733335,
+            "heading": 3.1399983498355466,
+            "angularVelocity": 2.834267287592998e-12,
+            "velocityX": 2.1430749184844986,
+            "velocityY": -3.117349042649137,
+            "timestamp": 0.9673633614337364
+        },
+        {
+            "x": 2.4554470535268806,
+            "y": 2.6221614552311943,
+            "heading": 3.1399983498356696,
+            "angularVelocity": 1.896559244966036e-12,
+            "velocityX": 2.566264157563543,
+            "velocityY": -2.7793748914229504,
+            "timestamp": 1.0323163502512729
+        },
+        {
+            "x": 2.646204500588454,
+            "y": 2.4672853384780784,
+            "heading": 3.139998349835769,
+            "angularVelocity": 1.528915692972437e-12,
+            "velocityX": 2.9368540314174165,
+            "velocityY": -2.384434027946127,
+            "timestamp": 1.0972693390688095
+        },
+        {
+            "x": 2.8571229789617445,
+            "y": 2.341236093184693,
+            "heading": 3.1399983498358592,
+            "angularVelocity": 1.3872590523351098e-12,
+            "velocityX": 3.247248236192457,
+            "velocityY": -1.940622711722675,
+            "timestamp": 1.1622223278863462
+        },
+        {
+            "x": 3.0838578351814623,
+            "y": 2.2465454263799622,
+            "heading": 3.139998349835948,
+            "angularVelocity": 1.37081210529667e-12,
+            "velocityX": 3.490753240880161,
+            "velocityY": -1.4578338661343044,
+            "timestamp": 1.2271753167038828
+        },
+        {
+            "x": 3.3145391093778356,
+            "y": 2.1619217463740292,
+            "heading": 3.1399983498360386,
+            "angularVelocity": 1.3907396736325547e-12,
+            "velocityX": 3.551511306806079,
+            "velocityY": -1.3028450506530576,
+            "timestamp": 1.2921283055214194
+        },
+        {
+            "x": 3.5452163544414024,
+            "y": 2.0772870838321214,
+            "heading": 3.1399983498361257,
+            "angularVelocity": 1.3371940783811797e-12,
+            "velocityX": 3.5514492752906865,
+            "velocityY": -1.3030141350149058,
+            "timestamp": 1.357081294338956
+        },
+        {
+            "x": 3.7758941865765565,
+            "y": 1.9926540214142978,
+            "heading": 3.139998349836204,
+            "angularVelocity": 1.2081341732104707e-12,
+            "velocityX": 3.5514583136980007,
+            "velocityY": -1.302989499904737,
+            "timestamp": 1.4220342831564927
+        },
+        {
+            "x": 4.006572310105761,
+            "y": 1.908021753240333,
+            "heading": 3.1399983498362696,
+            "angularVelocity": 1.0139841411422692e-12,
+            "velocityX": 3.5514627999275414,
+            "velocityY": -1.3029772719259731,
+            "timestamp": 1.4869872719740294
+        },
+        {
+            "x": 4.237248920507999,
+            "y": 1.8233853608613613,
+            "heading": 3.1399983498363224,
+            "angularVelocity": 8.135622214219814e-13,
+            "velocityX": 3.5514395042053684,
+            "velocityY": -1.3030407671488682,
+            "timestamp": 1.551940260791566
+        },
+        {
+            "x": 4.46792193477352,
+            "y": 1.7387391679278503,
+            "heading": 3.1399983498363646,
+            "angularVelocity": 6.458697792259404e-13,
+            "velocityX": 3.551384138968564,
+            "velocityY": -1.3031916540771904,
+            "timestamp": 1.6168932496091026
+        },
+        {
+            "x": 4.698662422545312,
+            "y": 1.6542770762611563,
+            "heading": 3.139998349836401,
+            "angularVelocity": 5.573297491220617e-13,
+            "velocityX": 3.5524229442068638,
+            "velocityY": -1.3003572769873404,
+            "timestamp": 1.6818462384266393
+        },
+        {
+            "x": 4.929586887359619,
+            "y": 1.5703192949295044,
+            "heading": 3.139998349836444,
+            "angularVelocity": 6.621983947605345e-13,
+            "velocityX": 3.5552554088546553,
+            "velocityY": -1.2925930408850796,
+            "timestamp": 1.746799227244176
+        },
+        {
+            "x": 5.059491461224377,
+            "y": 1.5327116535816125,
+            "heading": 3.1399983498365303,
+            "angularVelocity": 2.411376289586005e-12,
+            "velocityX": 3.6337293619618993,
+            "velocityY": -1.0519721248789429,
+            "timestamp": 1.7825488830178295
+        },
+        {
+            "x": 5.189524196366832,
+            "y": 1.4955495654455135,
+            "heading": 3.1399983498366377,
+            "angularVelocity": 3.0099354211020166e-12,
+            "velocityX": 3.6373143273250914,
+            "velocityY": -1.0395089779716884,
+            "timestamp": 1.8182985387914832
+        },
+        {
+            "x": 5.319583815273274,
+            "y": 1.4584816742976305,
+            "heading": 3.1399983498367723,
+            "angularVelocity": 3.767098328405853e-12,
+            "velocityX": 3.6380663279640872,
+            "velocityY": -1.036874071811995,
+            "timestamp": 1.8540481945651368
+        },
+        {
+            "x": 5.4496589768626915,
+            "y": 1.4214683610582035,
+            "heading": 3.139998349836922,
+            "angularVelocity": 4.1821295987378e-12,
+            "velocityX": 3.6385010925132435,
+            "velocityY": -1.0353474023301,
+            "timestamp": 1.8897978503387904
+        },
+        {
+            "x": 5.579738820656374,
+            "y": 1.3844715066642421,
+            "heading": 3.139998349837078,
+            "angularVelocity": 4.36086747938318e-12,
+            "velocityX": 3.6386320645231356,
+            "velocityY": -1.0348870106108623,
+            "timestamp": 1.925547506112444
+        },
+        {
+            "x": 5.709817887256256,
+            "y": 1.3474719197526863,
+            "heading": 3.1399983498372332,
+            "angularVelocity": 4.346123552895358e-12,
+            "velocityX": 3.6386103246271904,
+            "velocityY": -1.0349634454051337,
+            "timestamp": 1.9612971618860977
+        },
+        {
+            "x": 5.839896082809478,
+            "y": 1.3104692706317995,
+            "heading": 3.139998349837383,
+            "angularVelocity": 4.183764792386501e-12,
+            "velocityX": 3.638585959451041,
+            "velocityY": -1.0350491024349304,
+            "timestamp": 1.9970468176597513
+        },
+        {
+            "x": 5.969973859581082,
+            "y": 1.2734651493862033,
+            "heading": 3.139998349837523,
+            "angularVelocity": 3.914068769730236e-12,
+            "velocityX": 3.6385742451671077,
+            "velocityY": -1.0350902811450293,
+            "timestamp": 2.032796473433405
+        },
+        {
+            "x": 6.100051488327663,
+            "y": 1.2364605078211643,
+            "heading": 3.1399983498376502,
+            "angularVelocity": 3.5614211180657343e-12,
+            "velocityX": 3.6385701045671635,
+            "velocityY": -1.0351048356752235,
+            "timestamp": 2.068546129207059
+        },
+        {
+            "x": 6.230129108868512,
+            "y": 1.199455837420786,
+            "heading": 3.1399983498377644,
+            "angularVelocity": 3.192303325422517e-12,
+            "velocityX": 3.6385698750339617,
+            "velocityY": -1.0351056422659504,
+            "timestamp": 2.1042957849807125
+        },
+        {
+            "x": 6.360206785724816,
+            "y": 1.162451364982862,
+            "heading": 3.1399983498378674,
+            "angularVelocity": 2.8800276139093587e-12,
+            "velocityX": 3.638571450306583,
+            "velocityY": -1.0351001048013342,
+            "timestamp": 2.140045440754366
+        },
+        {
+            "x": 6.490284555094277,
+            "y": 1.1254472177490595,
+            "heading": 3.1399983498379616,
+            "angularVelocity": 2.621534784795507e-12,
+            "velocityX": 3.638574038112148,
+            "velocityY": -1.0350910080949738,
+            "timestamp": 2.1757950965280197
+        },
+        {
+            "x": 6.6203624349981105,
+            "y": 1.0884434590721859,
+            "heading": 3.1399983498380477,
+            "angularVelocity": 2.4107623793093176e-12,
+            "velocityX": 3.638577130012462,
+            "velocityY": -1.0350801392650475,
+            "timestamp": 2.2115447523016734
+        },
+        {
+            "x": 6.75044029323431,
+            "y": 1.0514396242316386,
+            "heading": 3.1399983498381276,
+            "angularVelocity": 2.2461604882641728e-12,
+            "velocityX": 3.6385765239189465,
+            "velocityY": -1.0350822697380073,
+            "timestamp": 2.247294408075327
+        },
+        {
+            "x": 6.880516952547443,
+            "y": 1.0144315751292232,
+            "heading": 3.139998349838195,
+            "angularVelocity": 1.892677955933963e-12,
+            "velocityX": 3.6385429872864736,
+            "velocityY": -1.0352001523239789,
+            "timestamp": 2.2830440638489806
+        },
+        {
+            "x": 7.003514384307231,
+            "y": 0.97944241190024,
+            "heading": 3.1399986104136794,
+            "angularVelocity": 0.000007288894915666013,
+            "velocityX": 3.440520729445303,
+            "velocityY": -0.9787272764389812,
+            "timestamp": 2.3187937196226343
+        },
+        {
+            "x": 7.116262004659963,
+            "y": 0.9473688227429191,
+            "heading": 3.139998845492451,
+            "angularVelocity": 0.000006575693298715729,
+            "velocityX": 3.1538099574045058,
+            "velocityY": -0.8971719716796405,
+            "timestamp": 2.354543375396288
+        },
+        {
+            "x": 7.218759831907218,
+            "y": 0.9182109395297621,
+            "heading": 3.1399990575434797,
+            "angularVelocity": 0.00000593155441577883,
+            "velocityX": 2.8670996973009437,
+            "velocityY": -0.8156129781434808,
+            "timestamp": 2.3902930311699415
+        },
+        {
+            "x": 7.3110078714654865,
+            "y": 0.8919688038160335,
+            "heading": 3.1399992473559544,
+            "angularVelocity": 0.000005309490974163329,
+            "velocityX": 2.5803895887090773,
+            "velocityY": -0.7340528222110998,
+            "timestamp": 2.426042686943595
+        },
+        {
+            "x": 7.393006125886103,
+            "y": 0.8686424358274811,
+            "heading": 3.139999415401417,
+            "angularVelocity": 0.000004700617645814799,
+            "velocityX": 2.293679551483878,
+            "velocityY": -0.6524921005180893,
+            "timestamp": 2.461792342717249
+        },
+        {
+            "x": 7.464754596626899,
+            "y": 0.8482318474429659,
+            "heading": 3.1399995619997028,
+            "angularVelocity": 0.000004100690828648421,
+            "velocityX": 2.0069695550376028,
+            "velocityY": -0.5709310465460533,
+            "timestamp": 2.4975419984909024
+        },
+        {
+            "x": 7.526253284614184,
+            "y": 0.8307370464214682,
+            "heading": 3.1399996873763563,
+            "angularVelocity": 0.00000350707302580956,
+            "velocityX": 1.7202595845023039,
+            "velocityY": -0.4893697755375005,
+            "timestamp": 2.533291654264556
+        },
+        {
+            "x": 7.577502190476697,
+            "y": 0.8161580381894111,
+            "heading": 3.139999791690666,
+            "angularVelocity": 0.0000029179108844478315,
+            "velocityX": 1.433549631554256,
+            "velocityY": -0.40780835273943755,
+            "timestamp": 2.5690413100382097
+        },
+        {
+            "x": 7.618501314660242,
+            "y": 0.8044948267260937,
+            "heading": 3.1399998750527174,
+            "angularVelocity": 0.000002331828092755085,
+            "velocityX": 1.1468396910764296,
+            "velocityY": -0.3262468186312345,
+            "timestamp": 2.6047909658118633
+        },
+        {
+            "x": 7.649250657490367,
+            "y": 0.7957474150520292,
+            "heading": 3.139999937534455,
+            "angularVelocity": 0.0000017477577406037154,
+            "velocityX": 0.860129759704899,
+            "velocityY": -0.24468520003233896,
+            "timestamp": 2.640540621585517
+        },
+        {
+            "x": 7.669750219209449,
+            "y": 0.789915805520138,
+            "heading": 3.1399999791776474,
+            "angularVelocity": 0.0000011648557571115021,
+            "velocityX": 0.5734198351132136,
+            "velocityY": -0.16312351561685162,
+            "timestamp": 2.6762902773591706
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 5.824490440069194e-7,
+            "velocityX": 0.2867099156267607,
+            "velocityY": -0.0815617789049158,
+            "timestamp": 2.712039933132824
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": -6.025226692032396e-31,
+            "velocityX": 4.372989448335815e-32,
+            "velocityY": 7.524777583668013e-32,
+            "timestamp": 2.747789588906478
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.traj
new file mode 100644
index 00000000..ac17f8d7
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.traj	
@@ -0,0 +1,625 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -2.730529759762368e-29,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2482118663055717,
+            "y": 5.55085896880981,
+            "heading": 3.1399999903330857,
+            "angularVelocity": -2.8923280730678937e-7,
+            "velocityX": -0.05350072563170815,
+            "velocityY": -0.2734984543883791,
+            "timestamp": 0.03342260639319256
+        },
+        {
+            "x": 2.24463557685663,
+            "y": 5.5325769109933445,
+            "heading": 3.1399999700354155,
+            "angularVelocity": -6.073036282303753e-7,
+            "velocityX": -0.10700211129830166,
+            "velocityY": -0.5469967722250598,
+            "timestamp": 0.06684521278638512
+        },
+        {
+            "x": 2.2392711054637595,
+            "y": 5.505153831967315,
+            "heading": 3.139999938021822,
+            "angularVelocity": -9.578425018869592e-7,
+            "velocityX": -0.16050428054878876,
+            "velocityY": -0.8204949279944175,
+            "timestamp": 0.1002678191795777
+        },
+        {
+            "x": 2.2321184205712044,
+            "y": 5.468589738256669,
+            "heading": 3.139999893047122,
+            "angularVelocity": -0.0000013456371104363374,
+            "velocityX": -0.2140073939433933,
+            "velocityY": -1.0939928885381913,
+            "timestamp": 0.13369042557277025
+        },
+        {
+            "x": 2.2231774834707245,
+            "y": 5.422884637863401,
+            "heading": 3.1399998336497137,
+            "angularVelocity": -0.000001777162647778402,
+            "velocityX": -0.2675116654835123,
+            "velocityY": -1.3674906096633432,
+            "timestamp": 0.1671130319659628
+        },
+        {
+            "x": 2.2124482456246803,
+            "y": 5.368038540819271,
+            "heading": 3.139999758066302,
+            "angularVelocity": -0.0000022614457562380146,
+            "velocityX": -0.3210173892634857,
+            "velocityY": -1.6409880306395483,
+            "timestamp": 0.20053563835915536
+        },
+        {
+            "x": 2.199930644465073,
+            "y": 5.30405146005317,
+            "heading": 3.139999664095155,
+            "angularVelocity": -0.0000028116043956655657,
+            "velocityX": -0.3745249850698391,
+            "velocityY": -1.9144850647849765,
+            "timestamp": 0.23395824475234792
+        },
+        {
+            "x": 2.1856245964060905,
+            "y": 5.230923412833771,
+            "heading": 3.1399995488597034,
+            "angularVelocity": -0.0000034478296167107145,
+            "velocityX": -0.4280350817528267,
+            "velocityY": -2.1879815822539745,
+            "timestamp": 0.2673808511455405
+        },
+        {
+            "x": 2.1695299843090114,
+            "y": 5.148654423357543,
+            "heading": 3.139999408375343,
+            "angularVelocity": -0.000004203273662517842,
+            "velocityX": -0.4815486832110918,
+            "velocityY": -2.461477375767798,
+            "timestamp": 0.30080345753873305
+        },
+        {
+            "x": 2.15164663254415,
+            "y": 5.057244527897619,
+            "heading": 3.139999236662074,
+            "angularVelocity": -0.000005137638535462322,
+            "velocityX": -0.535067539486715,
+            "velocityY": -2.7349720839942284,
+            "timestamp": 0.3342260639319256
+        },
+        {
+            "x": 2.1319742493327887,
+            "y": 4.956693786709351,
+            "heading": 3.1399990236359914,
+            "angularVelocity": -0.000006373712524248939,
+            "velocityX": -0.5885951257435309,
+            "velocityY": -3.008464989388312,
+            "timestamp": 0.36764867032511817
+        },
+        {
+            "x": 2.110512257081521,
+            "y": 4.847002319075526,
+            "heading": 3.13999874874802,
+            "angularVelocity": -0.000008224612190539215,
+            "velocityX": -0.6421399935196433,
+            "velocityY": -3.28195432585699,
+            "timestamp": 0.4010712767183107
+        },
+        {
+            "x": 2.087258999095236,
+            "y": 4.728170467468909,
+            "heading": 3.139998349832889,
+            "angularVelocity": -0.00001193548864433131,
+            "velocityX": -0.6957344293958355,
+            "velocityY": -3.555433415585576,
+            "timestamp": 0.4344938831115033
+        },
+        {
+            "x": 2.0629644962421034,
+            "y": 4.604090796237193,
+            "heading": 3.1399983498325463,
+            "angularVelocity": -1.0248884056674418e-11,
+            "velocityX": -0.7268883391614586,
+            "velocityY": -3.712447490549055,
+            "timestamp": 0.46791648950469583
+        },
+        {
+            "x": 2.0387448816492295,
+            "y": 4.479996482972041,
+            "heading": 3.139998349833487,
+            "angularVelocity": 2.8141899837441888e-11,
+            "velocityX": -0.7246476922026518,
+            "velocityY": -3.712885578275693,
+            "timestamp": 0.5013390958978884
+        },
+        {
+            "x": 2.0146234361555044,
+            "y": 4.355883050210464,
+            "heading": 3.139998349833582,
+            "angularVelocity": 2.8336640394810287e-12,
+            "velocityX": -0.7217104856241171,
+            "velocityY": -3.713457631102453,
+            "timestamp": 0.534761702291081
+        },
+        {
+            "x": 1.9906204010106838,
+            "y": 4.231746663125483,
+            "heading": 3.1399983498337,
+            "angularVelocity": 3.5336881649516105e-12,
+            "velocityX": -0.7181676635393196,
+            "velocityY": -3.714144421443354,
+            "timestamp": 0.5681843086842736
+        },
+        {
+            "x": 1.9667619480914416,
+            "y": 4.107582407046251,
+            "heading": 3.139998349833824,
+            "angularVelocity": 3.708930519775865e-12,
+            "velocityX": -0.7138417823660727,
+            "velocityY": -3.7149782580858504,
+            "timestamp": 0.6016069150774662
+        },
+        {
+            "x": 1.9503914931986956,
+            "y": 3.982210973196183,
+            "heading": 3.1399983498339594,
+            "angularVelocity": 4.046203674508551e-12,
+            "velocityX": -0.4898018634439243,
+            "velocityY": -3.751096858670778,
+            "timestamp": 0.6350295214706588
+        },
+        {
+            "x": 1.943224502199385,
+            "y": 3.8559785593201936,
+            "heading": 3.1399983498340998,
+            "angularVelocity": 4.203331220184306e-12,
+            "velocityX": -0.21443543076194166,
+            "velocityY": -3.7768572681307018,
+            "timestamp": 0.6684521278638514
+        },
+        {
+            "x": 1.9453593714612354,
+            "y": 3.7295608780305693,
+            "heading": 3.1399983498342436,
+            "angularVelocity": 4.304427970302576e-12,
+            "velocityX": 0.06387500833733431,
+            "velocityY": -3.7824004448488933,
+            "timestamp": 0.701874734257044
+        },
+        {
+            "x": 1.9567930300665735,
+            "y": 3.6036432110345795,
+            "heading": 3.139998349834389,
+            "angularVelocity": 4.350609924565741e-12,
+            "velocityX": 0.3420935659816486,
+            "velocityY": -3.767440082758668,
+            "timestamp": 0.7352973406502367
+        },
+        {
+            "x": 1.9774653925716308,
+            "y": 3.4789089323576285,
+            "heading": 3.1399983498345336,
+            "angularVelocity": 4.327887690330789e-12,
+            "velocityX": 0.6185143750313882,
+            "velocityY": -3.732033259450771,
+            "timestamp": 0.7687199470434293
+        },
+        {
+            "x": 2.0072643339986427,
+            "y": 3.3560349759367867,
+            "heading": 3.1399983498346824,
+            "angularVelocity": 4.434032141487508e-12,
+            "velocityX": 0.8915804194501273,
+            "velocityY": -3.6763726615238776,
+            "timestamp": 0.8021425534366219
+        },
+        {
+            "x": 2.0460281463567345,
+            "y": 3.235688177587346,
+            "heading": 3.1399983498348414,
+            "angularVelocity": 4.757708405328401e-12,
+            "velocityX": 1.1598081819894186,
+            "velocityY": -3.600760423457161,
+            "timestamp": 0.8355651598298145
+        },
+        {
+            "x": 2.093546464867386,
+            "y": 3.118521659475836,
+            "heading": 3.139998349835024,
+            "angularVelocity": 5.453307843013083e-12,
+            "velocityX": 1.421741857944163,
+            "velocityY": -3.5056068558230677,
+            "timestamp": 0.8689877662230071
+        },
+        {
+            "x": 2.1495614051818848,
+            "y": 3.005171298980713,
+            "heading": 3.1399983498353623,
+            "angularVelocity": 1.0122205936259764e-11,
+            "velocityX": 1.67595966800623,
+            "velocityY": -3.391427920419207,
+            "timestamp": 0.9024103726161997
+        },
+        {
+            "x": 2.2887605263975965,
+            "y": 2.8026901614733335,
+            "heading": 3.1399983498355466,
+            "angularVelocity": 2.834267287592998e-12,
+            "velocityX": 2.1430749184844986,
+            "velocityY": -3.117349042649137,
+            "timestamp": 0.9673633614337364
+        },
+        {
+            "x": 2.4554470535268806,
+            "y": 2.6221614552311943,
+            "heading": 3.1399983498356696,
+            "angularVelocity": 1.896559244966036e-12,
+            "velocityX": 2.566264157563543,
+            "velocityY": -2.7793748914229504,
+            "timestamp": 1.0323163502512729
+        },
+        {
+            "x": 2.646204500588454,
+            "y": 2.4672853384780784,
+            "heading": 3.139998349835769,
+            "angularVelocity": 1.528915692972437e-12,
+            "velocityX": 2.9368540314174165,
+            "velocityY": -2.384434027946127,
+            "timestamp": 1.0972693390688095
+        },
+        {
+            "x": 2.8571229789617445,
+            "y": 2.341236093184693,
+            "heading": 3.1399983498358592,
+            "angularVelocity": 1.3872590523351098e-12,
+            "velocityX": 3.247248236192457,
+            "velocityY": -1.940622711722675,
+            "timestamp": 1.1622223278863462
+        },
+        {
+            "x": 3.0838578351814623,
+            "y": 2.2465454263799622,
+            "heading": 3.139998349835948,
+            "angularVelocity": 1.37081210529667e-12,
+            "velocityX": 3.490753240880161,
+            "velocityY": -1.4578338661343044,
+            "timestamp": 1.2271753167038828
+        },
+        {
+            "x": 3.3145391093778356,
+            "y": 2.1619217463740292,
+            "heading": 3.1399983498360386,
+            "angularVelocity": 1.3907396736325547e-12,
+            "velocityX": 3.551511306806079,
+            "velocityY": -1.3028450506530576,
+            "timestamp": 1.2921283055214194
+        },
+        {
+            "x": 3.5452163544414024,
+            "y": 2.0772870838321214,
+            "heading": 3.1399983498361257,
+            "angularVelocity": 1.3371940783811797e-12,
+            "velocityX": 3.5514492752906865,
+            "velocityY": -1.3030141350149058,
+            "timestamp": 1.357081294338956
+        },
+        {
+            "x": 3.7758941865765565,
+            "y": 1.9926540214142978,
+            "heading": 3.139998349836204,
+            "angularVelocity": 1.2081341732104707e-12,
+            "velocityX": 3.5514583136980007,
+            "velocityY": -1.302989499904737,
+            "timestamp": 1.4220342831564927
+        },
+        {
+            "x": 4.006572310105761,
+            "y": 1.908021753240333,
+            "heading": 3.1399983498362696,
+            "angularVelocity": 1.0139841411422692e-12,
+            "velocityX": 3.5514627999275414,
+            "velocityY": -1.3029772719259731,
+            "timestamp": 1.4869872719740294
+        },
+        {
+            "x": 4.237248920507999,
+            "y": 1.8233853608613613,
+            "heading": 3.1399983498363224,
+            "angularVelocity": 8.135622214219814e-13,
+            "velocityX": 3.5514395042053684,
+            "velocityY": -1.3030407671488682,
+            "timestamp": 1.551940260791566
+        },
+        {
+            "x": 4.46792193477352,
+            "y": 1.7387391679278503,
+            "heading": 3.1399983498363646,
+            "angularVelocity": 6.458697792259404e-13,
+            "velocityX": 3.551384138968564,
+            "velocityY": -1.3031916540771904,
+            "timestamp": 1.6168932496091026
+        },
+        {
+            "x": 4.698662422545312,
+            "y": 1.6542770762611563,
+            "heading": 3.139998349836401,
+            "angularVelocity": 5.573297491220617e-13,
+            "velocityX": 3.5524229442068638,
+            "velocityY": -1.3003572769873404,
+            "timestamp": 1.6818462384266393
+        },
+        {
+            "x": 4.929586887359619,
+            "y": 1.5703192949295044,
+            "heading": 3.139998349836444,
+            "angularVelocity": 6.621983947605345e-13,
+            "velocityX": 3.5552554088546553,
+            "velocityY": -1.2925930408850796,
+            "timestamp": 1.746799227244176
+        },
+        {
+            "x": 5.059491461224377,
+            "y": 1.5327116535816125,
+            "heading": 3.1399983498365303,
+            "angularVelocity": 2.411376289586005e-12,
+            "velocityX": 3.6337293619618993,
+            "velocityY": -1.0519721248789429,
+            "timestamp": 1.7825488830178295
+        },
+        {
+            "x": 5.189524196366832,
+            "y": 1.4955495654455135,
+            "heading": 3.1399983498366377,
+            "angularVelocity": 3.0099354211020166e-12,
+            "velocityX": 3.6373143273250914,
+            "velocityY": -1.0395089779716884,
+            "timestamp": 1.8182985387914832
+        },
+        {
+            "x": 5.319583815273274,
+            "y": 1.4584816742976305,
+            "heading": 3.1399983498367723,
+            "angularVelocity": 3.767098328405853e-12,
+            "velocityX": 3.6380663279640872,
+            "velocityY": -1.036874071811995,
+            "timestamp": 1.8540481945651368
+        },
+        {
+            "x": 5.4496589768626915,
+            "y": 1.4214683610582035,
+            "heading": 3.139998349836922,
+            "angularVelocity": 4.1821295987378e-12,
+            "velocityX": 3.6385010925132435,
+            "velocityY": -1.0353474023301,
+            "timestamp": 1.8897978503387904
+        },
+        {
+            "x": 5.579738820656374,
+            "y": 1.3844715066642421,
+            "heading": 3.139998349837078,
+            "angularVelocity": 4.36086747938318e-12,
+            "velocityX": 3.6386320645231356,
+            "velocityY": -1.0348870106108623,
+            "timestamp": 1.925547506112444
+        },
+        {
+            "x": 5.709817887256256,
+            "y": 1.3474719197526863,
+            "heading": 3.1399983498372332,
+            "angularVelocity": 4.346123552895358e-12,
+            "velocityX": 3.6386103246271904,
+            "velocityY": -1.0349634454051337,
+            "timestamp": 1.9612971618860977
+        },
+        {
+            "x": 5.839896082809478,
+            "y": 1.3104692706317995,
+            "heading": 3.139998349837383,
+            "angularVelocity": 4.183764792386501e-12,
+            "velocityX": 3.638585959451041,
+            "velocityY": -1.0350491024349304,
+            "timestamp": 1.9970468176597513
+        },
+        {
+            "x": 5.969973859581082,
+            "y": 1.2734651493862033,
+            "heading": 3.139998349837523,
+            "angularVelocity": 3.914068769730236e-12,
+            "velocityX": 3.6385742451671077,
+            "velocityY": -1.0350902811450293,
+            "timestamp": 2.032796473433405
+        },
+        {
+            "x": 6.100051488327663,
+            "y": 1.2364605078211643,
+            "heading": 3.1399983498376502,
+            "angularVelocity": 3.5614211180657343e-12,
+            "velocityX": 3.6385701045671635,
+            "velocityY": -1.0351048356752235,
+            "timestamp": 2.068546129207059
+        },
+        {
+            "x": 6.230129108868512,
+            "y": 1.199455837420786,
+            "heading": 3.1399983498377644,
+            "angularVelocity": 3.192303325422517e-12,
+            "velocityX": 3.6385698750339617,
+            "velocityY": -1.0351056422659504,
+            "timestamp": 2.1042957849807125
+        },
+        {
+            "x": 6.360206785724816,
+            "y": 1.162451364982862,
+            "heading": 3.1399983498378674,
+            "angularVelocity": 2.8800276139093587e-12,
+            "velocityX": 3.638571450306583,
+            "velocityY": -1.0351001048013342,
+            "timestamp": 2.140045440754366
+        },
+        {
+            "x": 6.490284555094277,
+            "y": 1.1254472177490595,
+            "heading": 3.1399983498379616,
+            "angularVelocity": 2.621534784795507e-12,
+            "velocityX": 3.638574038112148,
+            "velocityY": -1.0350910080949738,
+            "timestamp": 2.1757950965280197
+        },
+        {
+            "x": 6.6203624349981105,
+            "y": 1.0884434590721859,
+            "heading": 3.1399983498380477,
+            "angularVelocity": 2.4107623793093176e-12,
+            "velocityX": 3.638577130012462,
+            "velocityY": -1.0350801392650475,
+            "timestamp": 2.2115447523016734
+        },
+        {
+            "x": 6.75044029323431,
+            "y": 1.0514396242316386,
+            "heading": 3.1399983498381276,
+            "angularVelocity": 2.2461604882641728e-12,
+            "velocityX": 3.6385765239189465,
+            "velocityY": -1.0350822697380073,
+            "timestamp": 2.247294408075327
+        },
+        {
+            "x": 6.880516952547443,
+            "y": 1.0144315751292232,
+            "heading": 3.139998349838195,
+            "angularVelocity": 1.892677955933963e-12,
+            "velocityX": 3.6385429872864736,
+            "velocityY": -1.0352001523239789,
+            "timestamp": 2.2830440638489806
+        },
+        {
+            "x": 7.003514384307231,
+            "y": 0.97944241190024,
+            "heading": 3.1399986104136794,
+            "angularVelocity": 0.000007288894915666013,
+            "velocityX": 3.440520729445303,
+            "velocityY": -0.9787272764389812,
+            "timestamp": 2.3187937196226343
+        },
+        {
+            "x": 7.116262004659963,
+            "y": 0.9473688227429191,
+            "heading": 3.139998845492451,
+            "angularVelocity": 0.000006575693298715729,
+            "velocityX": 3.1538099574045058,
+            "velocityY": -0.8971719716796405,
+            "timestamp": 2.354543375396288
+        },
+        {
+            "x": 7.218759831907218,
+            "y": 0.9182109395297621,
+            "heading": 3.1399990575434797,
+            "angularVelocity": 0.00000593155441577883,
+            "velocityX": 2.8670996973009437,
+            "velocityY": -0.8156129781434808,
+            "timestamp": 2.3902930311699415
+        },
+        {
+            "x": 7.3110078714654865,
+            "y": 0.8919688038160335,
+            "heading": 3.1399992473559544,
+            "angularVelocity": 0.000005309490974163329,
+            "velocityX": 2.5803895887090773,
+            "velocityY": -0.7340528222110998,
+            "timestamp": 2.426042686943595
+        },
+        {
+            "x": 7.393006125886103,
+            "y": 0.8686424358274811,
+            "heading": 3.139999415401417,
+            "angularVelocity": 0.000004700617645814799,
+            "velocityX": 2.293679551483878,
+            "velocityY": -0.6524921005180893,
+            "timestamp": 2.461792342717249
+        },
+        {
+            "x": 7.464754596626899,
+            "y": 0.8482318474429659,
+            "heading": 3.1399995619997028,
+            "angularVelocity": 0.000004100690828648421,
+            "velocityX": 2.0069695550376028,
+            "velocityY": -0.5709310465460533,
+            "timestamp": 2.4975419984909024
+        },
+        {
+            "x": 7.526253284614184,
+            "y": 0.8307370464214682,
+            "heading": 3.1399996873763563,
+            "angularVelocity": 0.00000350707302580956,
+            "velocityX": 1.7202595845023039,
+            "velocityY": -0.4893697755375005,
+            "timestamp": 2.533291654264556
+        },
+        {
+            "x": 7.577502190476697,
+            "y": 0.8161580381894111,
+            "heading": 3.139999791690666,
+            "angularVelocity": 0.0000029179108844478315,
+            "velocityX": 1.433549631554256,
+            "velocityY": -0.40780835273943755,
+            "timestamp": 2.5690413100382097
+        },
+        {
+            "x": 7.618501314660242,
+            "y": 0.8044948267260937,
+            "heading": 3.1399998750527174,
+            "angularVelocity": 0.000002331828092755085,
+            "velocityX": 1.1468396910764296,
+            "velocityY": -0.3262468186312345,
+            "timestamp": 2.6047909658118633
+        },
+        {
+            "x": 7.649250657490367,
+            "y": 0.7957474150520292,
+            "heading": 3.139999937534455,
+            "angularVelocity": 0.0000017477577406037154,
+            "velocityX": 0.860129759704899,
+            "velocityY": -0.24468520003233896,
+            "timestamp": 2.640540621585517
+        },
+        {
+            "x": 7.669750219209449,
+            "y": 0.789915805520138,
+            "heading": 3.1399999791776474,
+            "angularVelocity": 0.0000011648557571115021,
+            "velocityX": 0.5734198351132136,
+            "velocityY": -0.16312351561685162,
+            "timestamp": 2.6762902773591706
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 5.824490440069194e-7,
+            "velocityX": 0.2867099156267607,
+            "velocityY": -0.0815617789049158,
+            "timestamp": 2.712039933132824
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": -6.025226692032396e-31,
+            "velocityX": 4.372989448335815e-32,
+            "velocityY": 7.524777583668013e-32,
+            "timestamp": 2.747789588906478
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.1.traj
new file mode 100644
index 00000000..a83dc4bd
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 3.1925891483075246e-34,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3070751616194327,
+            "y": 5.580871513694386,
+            "heading": 3.1487661024318196,
+            "angularVelocity": 0.1026391987797076,
+            "velocityX": 0.668272918826128,
+            "velocityY": 0.24437718582154933,
+            "timestamp": 0.08540696474681245
+        },
+        {
+            "x": 2.4212254758610405,
+            "y": 5.622614541188738,
+            "heading": 3.166299183933703,
+            "angularVelocity": 0.20528866180714583,
+            "velocityX": 1.3365457323065477,
+            "velocityY": 0.48875437287928064,
+            "timestamp": 0.1708139294936249
+        },
+        {
+            "x": 2.592450926566031,
+            "y": 5.685229083532584,
+            "heading": 3.1926007136716263,
+            "angularVelocity": 0.307955326780356,
+            "velocityX": 2.0048183565893676,
+            "velocityY": 0.7331315722255889,
+            "timestamp": 0.25622089424043737
+        },
+        {
+            "x": 2.820751488987388,
+            "y": 5.768715144253157,
+            "heading": 3.2276726330952217,
+            "angularVelocity": 0.4106447235019468,
+            "velocityX": 2.6730906911181096,
+            "velocityY": 0.9775088128710195,
+            "timestamp": 0.3416278589872498
+        },
+        {
+            "x": 3.106127123163594,
+            "y": 5.873072729004609,
+            "heading": 3.271517021699339,
+            "angularVelocity": 0.5133584682946327,
+            "velocityX": 3.341362557751549,
+            "velocityY": 1.2218861197188997,
+            "timestamp": 0.4270348237340622
+        },
+        {
+            "x": 3.4095640006306436,
+            "y": 5.984036075660027,
+            "heading": 3.2715170269491542,
+            "angularVelocity": 6.146823646853408e-8,
+            "velocityX": 3.5528352794948703,
+            "velocityY": 1.2992306538976923,
+            "timestamp": 0.5124417884808746
+        },
+        {
+            "x": 3.7130008780817154,
+            "y": 6.094999422359182,
+            "heading": 3.271517032198949,
+            "angularVelocity": 6.146799253439994e-8,
+            "velocityX": 3.5528352793077906,
+            "velocityY": 1.299230654409779,
+            "timestamp": 0.597848753227687
+        },
+        {
+            "x": 4.016437755532787,
+            "y": 6.205962769058336,
+            "heading": 3.271517037448744,
+            "angularVelocity": 6.146799251320457e-8,
+            "velocityX": 3.55283527930779,
+            "velocityY": 1.2992306544097794,
+            "timestamp": 0.6832557179744995
+        },
+        {
+            "x": 4.319874632996578,
+            "y": 6.3169261157226675,
+            "heading": 3.2715170426985596,
+            "angularVelocity": 6.146823670561877e-8,
+            "velocityX": 3.5528352794567044,
+            "velocityY": 1.2992306540020593,
+            "timestamp": 0.7686626827213119
+        },
+        {
+            "x": 4.605249758095724,
+            "y": 6.421284043313642,
+            "heading": 3.315404813019647,
+            "angularVelocity": 0.5138664095040972,
+            "velocityX": 3.3413565971479624,
+            "velocityY": 1.221890133905845,
+            "timestamp": 0.8540696474681243
+        },
+        {
+            "x": 4.833549856620357,
+            "y": 6.504770412469219,
+            "heading": 3.3505164790016466,
+            "angularVelocity": 0.41111010192303765,
+            "velocityX": 2.673085259515143,
+            "velocityY": 0.9775124242275821,
+            "timestamp": 0.9394766122149367
+        },
+        {
+            "x": 5.004774927843149,
+            "y": 6.567385202446666,
+            "heading": 3.3768507903488065,
+            "angularVelocity": 0.3083391550704053,
+            "velocityX": 2.0048139133662572,
+            "velocityY": 0.7331344716800138,
+            "timestamp": 1.0248835769617493
+        },
+        {
+            "x": 5.118924975241942,
+            "y": 6.609128400529722,
+            "heading": 3.3944071451755278,
+            "angularVelocity": 0.20556116095176463,
+            "velocityX": 1.3365426079382265,
+            "velocityY": 0.4887563702422027,
+            "timestamp": 1.1102905417085618
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.10278040005380541,
+            "velocityX": 0.6682713163646097,
+            "velocityY": 0.24437819014118428,
+            "timestamp": 1.1956975064553743
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": -1.3221581062655587e-34,
+            "velocityX": 2.469071735487476e-35,
+            "velocityY": 0,
+            "timestamp": 1.2811044712021868
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.traj
new file mode 100644
index 00000000..a83dc4bd
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 3.1925891483075246e-34,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3070751616194327,
+            "y": 5.580871513694386,
+            "heading": 3.1487661024318196,
+            "angularVelocity": 0.1026391987797076,
+            "velocityX": 0.668272918826128,
+            "velocityY": 0.24437718582154933,
+            "timestamp": 0.08540696474681245
+        },
+        {
+            "x": 2.4212254758610405,
+            "y": 5.622614541188738,
+            "heading": 3.166299183933703,
+            "angularVelocity": 0.20528866180714583,
+            "velocityX": 1.3365457323065477,
+            "velocityY": 0.48875437287928064,
+            "timestamp": 0.1708139294936249
+        },
+        {
+            "x": 2.592450926566031,
+            "y": 5.685229083532584,
+            "heading": 3.1926007136716263,
+            "angularVelocity": 0.307955326780356,
+            "velocityX": 2.0048183565893676,
+            "velocityY": 0.7331315722255889,
+            "timestamp": 0.25622089424043737
+        },
+        {
+            "x": 2.820751488987388,
+            "y": 5.768715144253157,
+            "heading": 3.2276726330952217,
+            "angularVelocity": 0.4106447235019468,
+            "velocityX": 2.6730906911181096,
+            "velocityY": 0.9775088128710195,
+            "timestamp": 0.3416278589872498
+        },
+        {
+            "x": 3.106127123163594,
+            "y": 5.873072729004609,
+            "heading": 3.271517021699339,
+            "angularVelocity": 0.5133584682946327,
+            "velocityX": 3.341362557751549,
+            "velocityY": 1.2218861197188997,
+            "timestamp": 0.4270348237340622
+        },
+        {
+            "x": 3.4095640006306436,
+            "y": 5.984036075660027,
+            "heading": 3.2715170269491542,
+            "angularVelocity": 6.146823646853408e-8,
+            "velocityX": 3.5528352794948703,
+            "velocityY": 1.2992306538976923,
+            "timestamp": 0.5124417884808746
+        },
+        {
+            "x": 3.7130008780817154,
+            "y": 6.094999422359182,
+            "heading": 3.271517032198949,
+            "angularVelocity": 6.146799253439994e-8,
+            "velocityX": 3.5528352793077906,
+            "velocityY": 1.299230654409779,
+            "timestamp": 0.597848753227687
+        },
+        {
+            "x": 4.016437755532787,
+            "y": 6.205962769058336,
+            "heading": 3.271517037448744,
+            "angularVelocity": 6.146799251320457e-8,
+            "velocityX": 3.55283527930779,
+            "velocityY": 1.2992306544097794,
+            "timestamp": 0.6832557179744995
+        },
+        {
+            "x": 4.319874632996578,
+            "y": 6.3169261157226675,
+            "heading": 3.2715170426985596,
+            "angularVelocity": 6.146823670561877e-8,
+            "velocityX": 3.5528352794567044,
+            "velocityY": 1.2992306540020593,
+            "timestamp": 0.7686626827213119
+        },
+        {
+            "x": 4.605249758095724,
+            "y": 6.421284043313642,
+            "heading": 3.315404813019647,
+            "angularVelocity": 0.5138664095040972,
+            "velocityX": 3.3413565971479624,
+            "velocityY": 1.221890133905845,
+            "timestamp": 0.8540696474681243
+        },
+        {
+            "x": 4.833549856620357,
+            "y": 6.504770412469219,
+            "heading": 3.3505164790016466,
+            "angularVelocity": 0.41111010192303765,
+            "velocityX": 2.673085259515143,
+            "velocityY": 0.9775124242275821,
+            "timestamp": 0.9394766122149367
+        },
+        {
+            "x": 5.004774927843149,
+            "y": 6.567385202446666,
+            "heading": 3.3768507903488065,
+            "angularVelocity": 0.3083391550704053,
+            "velocityX": 2.0048139133662572,
+            "velocityY": 0.7331344716800138,
+            "timestamp": 1.0248835769617493
+        },
+        {
+            "x": 5.118924975241942,
+            "y": 6.609128400529722,
+            "heading": 3.3944071451755278,
+            "angularVelocity": 0.20556116095176463,
+            "velocityX": 1.3365426079382265,
+            "velocityY": 0.4887563702422027,
+            "timestamp": 1.1102905417085618
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": 0.10278040005380541,
+            "velocityX": 0.6682713163646097,
+            "velocityY": 0.24437819014118428,
+            "timestamp": 1.1956975064553743
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 3.4031853071795863,
+            "angularVelocity": -1.3221581062655587e-34,
+            "velocityX": 2.469071735487476e-35,
+            "velocityY": 0,
+            "timestamp": 1.2811044712021868
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.1.traj
index 31e55043..fa09d8a2 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.1.traj	
@@ -4,100 +4,100 @@
             "x": 2.25,
             "y": 5.56,
             "heading": 3.14,
-            "angularVelocity": 1.3281175985971763e-33,
-            "velocityX": 1.9314152785727527e-39,
-            "velocityY": 8.278387842337126e-35,
+            "angularVelocity": -1.2603829918079947e-39,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 2.3319036557469475,
-            "y": 5.560000044140862,
-            "heading": 3.140000009517521,
-            "angularVelocity": 9.602993340973177e-8,
-            "velocityX": 0.8263919813281884,
-            "velocityY": 4.4537273180015373e-7,
-            "timestamp": 0.09910993523353247
+            "x": 2.3319036557469652,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 1.854813108024866e-16,
+            "velocityX": 0.8263919813283321,
+            "velocityY": 1.2609253605375368e-21,
+            "timestamp": 0.099109935233537
         },
         {
-            "x": 2.495710964163716,
-            "y": 5.5600001253696005,
-            "heading": 3.140000023792121,
-            "angularVelocity": 1.44027928091657e-7,
-            "velocityX": 1.6527839316087705,
-            "velocityY": 8.195821841849307e-7,
-            "timestamp": 0.19821987046706493
+            "x": 2.4957109641637625,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 3.709616393980982e-16,
+            "velocityX": 1.6527839316089838,
+            "velocityY": 2.5218434283644083e-21,
+            "timestamp": 0.198219870467074
         },
         {
-            "x": 2.741421919096052,
-            "y": 5.5600002360460925,
-            "heading": 3.140000035820423,
-            "angularVelocity": 1.213632154532524e-7,
-            "velocityX": 2.479175819794127,
-            "velocityY": 0.0000011167042923727,
-            "timestamp": 0.2973298057005974
+            "x": 2.741421919096125,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 5.564379296417211e-16,
+            "velocityX": 2.4791758197942864,
+            "velocityY": 3.782763291910568e-21,
+            "timestamp": 0.297329805700611
         },
         {
-            "x": 3.0690365020812136,
-            "y": 5.5600003677528065,
-            "heading": 3.140000035237491,
-            "angularVelocity": -5.88169262513392e-9,
-            "velocityX": 3.305567521694012,
-            "velocityY": 0.0000013288951255827482,
-            "timestamp": 0.39643974093412987
+            "x": 3.0690365020812695,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 7.41869431396362e-16,
+            "velocityX": 3.3055675216936846,
+            "velocityY": 5.0436652946129374e-21,
+            "timestamp": 0.396439740934148
         },
         {
-            "x": 3.443963497918667,
-            "y": 5.560000377607592,
-            "heading": 3.140000035237483,
-            "angularVelocity": -8.264095134625168e-14,
-            "velocityX": 3.782940579610041,
-            "velocityY": 9.943285507786426e-8,
-            "timestamp": 0.4955496761676623
+            "x": 3.4439634979187304,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 5.10358456113492e-16,
+            "velocityX": 3.7829405796099507,
+            "velocityY": -2.5218394750934443e-20,
+            "timestamp": 0.495549676167685
         },
         {
-            "x": 3.771578080903882,
-            "y": 5.560000246492901,
-            "heading": 3.1399999920753214,
-            "angularVelocity": -4.354978004755772e-7,
-            "velocityX": 3.305567521694553,
-            "velocityY": -0.0000013229217178930416,
-            "timestamp": 0.5946596114011948
+            "x": 3.771578080903875,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 7.390704390186125e-16,
+            "velocityX": 3.3055675216936846,
+            "velocityY": 5.043665294641482e-21,
+            "timestamp": 0.594659611401222
         },
         {
-            "x": 4.017289035836251,
-            "y": 5.560000133124366,
-            "heading": 3.1399999814016817,
-            "angularVelocity": -1.0769493378117993e-7,
-            "velocityX": 2.4791758197944644,
-            "velocityY": -0.000001143866475595614,
-            "timestamp": 0.6937695466347273
+            "x": 4.017289035836238,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 5.549819222123311e-16,
+            "velocityX": 2.479175819794286,
+            "velocityY": 3.782763291928162e-21,
+            "timestamp": 0.6937695466347591
         },
         {
-            "x": 4.181096344253041,
-            "y": 5.560000047628769,
-            "heading": 3.1399999890121935,
-            "angularVelocity": 7.678859424954975e-8,
-            "velocityX": 1.6527839316089916,
-            "velocityY": -8.626339566508034e-7,
-            "timestamp": 0.7928794818682598
+            "x": 4.181096344253034,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 3.701778935205264e-16,
+            "velocityX": 1.6527839316089838,
+            "velocityY": 2.5218434283731754e-21,
+            "timestamp": 0.792879481868296
         },
         {
             "x": 4.263,
             "y": 5.56,
             "heading": 3.14,
-            "angularVelocity": 1.1086483608499845e-7,
-            "velocityX": 0.8263919813282989,
-            "velocityY": -4.805650223133441e-7,
-            "timestamp": 0.8919894171017924
+            "angularVelocity": 1.8514488061077656e-16,
+            "velocityX": 0.8263919813283321,
+            "velocityY": 1.260925360546343e-21,
+            "timestamp": 0.891989417101833
         },
         {
             "x": 4.263,
             "y": 5.56,
             "heading": 3.14,
             "angularVelocity": 0,
-            "velocityX": 2.938735877055719e-39,
-            "velocityY": -3.0539806085263666e-33,
-            "timestamp": 0.9910993523353249
+            "velocityX": 0,
+            "velocityY": 3.337507404137172e-39,
+            "timestamp": 0.9910993523353699
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.traj
index 31e55043..fa09d8a2 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.traj	
@@ -4,100 +4,100 @@
             "x": 2.25,
             "y": 5.56,
             "heading": 3.14,
-            "angularVelocity": 1.3281175985971763e-33,
-            "velocityX": 1.9314152785727527e-39,
-            "velocityY": 8.278387842337126e-35,
+            "angularVelocity": -1.2603829918079947e-39,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 2.3319036557469475,
-            "y": 5.560000044140862,
-            "heading": 3.140000009517521,
-            "angularVelocity": 9.602993340973177e-8,
-            "velocityX": 0.8263919813281884,
-            "velocityY": 4.4537273180015373e-7,
-            "timestamp": 0.09910993523353247
+            "x": 2.3319036557469652,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 1.854813108024866e-16,
+            "velocityX": 0.8263919813283321,
+            "velocityY": 1.2609253605375368e-21,
+            "timestamp": 0.099109935233537
         },
         {
-            "x": 2.495710964163716,
-            "y": 5.5600001253696005,
-            "heading": 3.140000023792121,
-            "angularVelocity": 1.44027928091657e-7,
-            "velocityX": 1.6527839316087705,
-            "velocityY": 8.195821841849307e-7,
-            "timestamp": 0.19821987046706493
+            "x": 2.4957109641637625,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 3.709616393980982e-16,
+            "velocityX": 1.6527839316089838,
+            "velocityY": 2.5218434283644083e-21,
+            "timestamp": 0.198219870467074
         },
         {
-            "x": 2.741421919096052,
-            "y": 5.5600002360460925,
-            "heading": 3.140000035820423,
-            "angularVelocity": 1.213632154532524e-7,
-            "velocityX": 2.479175819794127,
-            "velocityY": 0.0000011167042923727,
-            "timestamp": 0.2973298057005974
+            "x": 2.741421919096125,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 5.564379296417211e-16,
+            "velocityX": 2.4791758197942864,
+            "velocityY": 3.782763291910568e-21,
+            "timestamp": 0.297329805700611
         },
         {
-            "x": 3.0690365020812136,
-            "y": 5.5600003677528065,
-            "heading": 3.140000035237491,
-            "angularVelocity": -5.88169262513392e-9,
-            "velocityX": 3.305567521694012,
-            "velocityY": 0.0000013288951255827482,
-            "timestamp": 0.39643974093412987
+            "x": 3.0690365020812695,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 7.41869431396362e-16,
+            "velocityX": 3.3055675216936846,
+            "velocityY": 5.0436652946129374e-21,
+            "timestamp": 0.396439740934148
         },
         {
-            "x": 3.443963497918667,
-            "y": 5.560000377607592,
-            "heading": 3.140000035237483,
-            "angularVelocity": -8.264095134625168e-14,
-            "velocityX": 3.782940579610041,
-            "velocityY": 9.943285507786426e-8,
-            "timestamp": 0.4955496761676623
+            "x": 3.4439634979187304,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 5.10358456113492e-16,
+            "velocityX": 3.7829405796099507,
+            "velocityY": -2.5218394750934443e-20,
+            "timestamp": 0.495549676167685
         },
         {
-            "x": 3.771578080903882,
-            "y": 5.560000246492901,
-            "heading": 3.1399999920753214,
-            "angularVelocity": -4.354978004755772e-7,
-            "velocityX": 3.305567521694553,
-            "velocityY": -0.0000013229217178930416,
-            "timestamp": 0.5946596114011948
+            "x": 3.771578080903875,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 7.390704390186125e-16,
+            "velocityX": 3.3055675216936846,
+            "velocityY": 5.043665294641482e-21,
+            "timestamp": 0.594659611401222
         },
         {
-            "x": 4.017289035836251,
-            "y": 5.560000133124366,
-            "heading": 3.1399999814016817,
-            "angularVelocity": -1.0769493378117993e-7,
-            "velocityX": 2.4791758197944644,
-            "velocityY": -0.000001143866475595614,
-            "timestamp": 0.6937695466347273
+            "x": 4.017289035836238,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 5.549819222123311e-16,
+            "velocityX": 2.479175819794286,
+            "velocityY": 3.782763291928162e-21,
+            "timestamp": 0.6937695466347591
         },
         {
-            "x": 4.181096344253041,
-            "y": 5.560000047628769,
-            "heading": 3.1399999890121935,
-            "angularVelocity": 7.678859424954975e-8,
-            "velocityX": 1.6527839316089916,
-            "velocityY": -8.626339566508034e-7,
-            "timestamp": 0.7928794818682598
+            "x": 4.181096344253034,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 3.701778935205264e-16,
+            "velocityX": 1.6527839316089838,
+            "velocityY": 2.5218434283731754e-21,
+            "timestamp": 0.792879481868296
         },
         {
             "x": 4.263,
             "y": 5.56,
             "heading": 3.14,
-            "angularVelocity": 1.1086483608499845e-7,
-            "velocityX": 0.8263919813282989,
-            "velocityY": -4.805650223133441e-7,
-            "timestamp": 0.8919894171017924
+            "angularVelocity": 1.8514488061077656e-16,
+            "velocityX": 0.8263919813283321,
+            "velocityY": 1.260925360546343e-21,
+            "timestamp": 0.891989417101833
         },
         {
             "x": 4.263,
             "y": 5.56,
             "heading": 3.14,
             "angularVelocity": 0,
-            "velocityX": 2.938735877055719e-39,
-            "velocityY": -3.0539806085263666e-33,
-            "timestamp": 0.9910993523353249
+            "velocityX": 0,
+            "velocityY": 3.337507404137172e-39,
+            "timestamp": 0.9910993523353699
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.1.traj
new file mode 100644
index 00000000..d6861c07
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.1.traj	
@@ -0,0 +1,472 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 1.712760733525474e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2447404981946972,
+            "y": 5.548452449977017,
+            "heading": 3.1382782323729086,
+            "angularVelocity": -0.0441204598358695,
+            "velocityX": -0.13477523592974824,
+            "velocityY": -0.2959070718806661,
+            "timestamp": 0.03902424484008447
+        },
+        {
+            "x": 2.2342214959104107,
+            "y": 5.525357349752352,
+            "heading": 3.134834682439341,
+            "angularVelocity": -0.08824129583236773,
+            "velocityX": -0.269550437872442,
+            "velocityY": -0.5918141483404872,
+            "timestamp": 0.07804848968016893
+        },
+        {
+            "x": 2.218442994608338,
+            "y": 5.490714699360087,
+            "heading": 3.129669313928024,
+            "angularVelocity": -0.13236306128370928,
+            "velocityX": -0.40432560237181914,
+            "velocityY": -0.8877212239269984,
+            "timestamp": 0.11707273452025341
+        },
+        {
+            "x": 2.1974049958845625,
+            "y": 5.44452449907313,
+            "heading": 3.1227820694139163,
+            "angularVelocity": -0.17648629825721382,
+            "velocityX": -0.539100725971382,
+            "velocityY": -1.1836282925201553,
+            "timestamp": 0.15609697936033787
+        },
+        {
+            "x": 2.17110750147078,
+            "y": 5.386786749443186,
+            "heading": 3.114172871086463,
+            "angularVelocity": -0.22061152913360044,
+            "velocityX": -0.6738758051961007,
+            "velocityY": -1.479535346975897,
+            "timestamp": 0.19512122420042233
+        },
+        {
+            "x": 2.139550513239859,
+            "y": 5.317501451364342,
+            "heading": 3.10384162215273,
+            "angularVelocity": -0.2647392403381948,
+            "velocityX": -0.8086508364284021,
+            "velocityY": -1.775442378520474,
+            "timestamp": 0.2341454690405068
+        },
+        {
+            "x": 2.1027340332257456,
+            "y": 5.236668606182096,
+            "heading": 3.091788209481888,
+            "angularVelocity": -0.30886985053106986,
+            "velocityX": -0.9434258155405923,
+            "velocityY": -2.0713493755866326,
+            "timestamp": 0.27316971388059125
+        },
+        {
+            "x": 2.0606580636823133,
+            "y": 5.1442882159029235,
+            "heading": 3.078012508890752,
+            "angularVelocity": -0.35300364292993813,
+            "velocityX": -1.078200736897071,
+            "velocityY": -2.3672563212366358,
+            "timestamp": 0.31219395872067573
+        },
+        {
+            "x": 2.013322607256609,
+            "y": 5.040360283671343,
+            "heading": 3.0625143970359345,
+            "angularVelocity": -0.3971405960148595,
+            "velocityX": -1.212975590422751,
+            "velocityY": -2.6631631863080094,
+            "timestamp": 0.3512182035607602
+        },
+        {
+            "x": 1.960727667582776,
+            "y": 4.92488481518758,
+            "heading": 3.0452937850632114,
+            "angularVelocity": -0.44127982599779075,
+            "velocityX": -1.3477503508231259,
+            "velocityY": -2.9590699053106113,
+            "timestamp": 0.3902424484008447
+        },
+        {
+            "x": 1.902873252468991,
+            "y": 4.797861825841284,
+            "heading": 3.026350773365474,
+            "angularVelocity": -0.4854164834032721,
+            "velocityX": -1.482524911138253,
+            "velocityY": -3.25497622995194,
+            "timestamp": 0.4292666932409292
+        },
+        {
+            "x": 1.8416825412927555,
+            "y": 4.663514351826141,
+            "heading": 3.02635076853487,
+            "angularVelocity": -1.2378468871067517e-7,
+            "velocityX": -1.5680178162827485,
+            "velocityY": -3.4426668489212484,
+            "timestamp": 0.4682909380810137
+        },
+        {
+            "x": 1.7807776273702332,
+            "y": 4.529037073556694,
+            "heading": 3.0263507637038423,
+            "angularVelocity": -1.237955425988301e-7,
+            "velocityX": -1.5606942343751464,
+            "velocityY": -3.445993095328429,
+            "timestamp": 0.5073151829210981
+        },
+        {
+            "x": 1.7316376139293153,
+            "y": 4.389829293964471,
+            "heading": 3.02635075879599,
+            "angularVelocity": -1.2576418143070467e-7,
+            "velocityX": -1.2592175362335267,
+            "velocityY": -3.5672126433882343,
+            "timestamp": 0.5463394277611826
+        },
+        {
+            "x": 1.6946418503710443,
+            "y": 4.246913724355564,
+            "heading": 3.026350753656809,
+            "angularVelocity": -1.3169201974173754e-7,
+            "velocityX": -0.9480199734770174,
+            "velocityY": -3.662225116579624,
+            "timestamp": 0.5853636726012671
+        },
+        {
+            "x": 1.6700643070030454,
+            "y": 4.101347620951498,
+            "heading": 3.02635074809997,
+            "angularVelocity": -1.423945209571316e-7,
+            "velocityX": -0.6298018954293696,
+            "velocityY": -3.7301452981493113,
+            "timestamp": 0.6243879174413516
+        },
+        {
+            "x": 1.6580868892212808,
+            "y": 3.9542079360929074,
+            "heading": 3.0263507418753948,
+            "angularVelocity": -1.595053312793056e-7,
+            "velocityX": -0.3069224742421484,
+            "velocityY": -3.7704684731645255,
+            "timestamp": 0.6634121622814361
+        },
+        {
+            "x": 1.6587982403438444,
+            "y": 3.806583282470554,
+            "heading": 3.0263507346134717,
+            "angularVelocity": -1.8608747530006848e-7,
+            "velocityX": 0.018228440434743125,
+            "velocityY": -3.7828958440399596,
+            "timestamp": 0.7024364071215206
+        },
+        {
+            "x": 1.6721931105563068,
+            "y": 3.659565885100826,
+            "heading": 3.0263506736368497,
+            "angularVelocity": -0.0000015625317616489644,
+            "velocityX": 0.34324482811563534,
+            "velocityY": -3.767334844586533,
+            "timestamp": 0.741460651961605
+        },
+        {
+            "x": 1.6979565964079348,
+            "y": 3.5152981288336416,
+            "heading": 3.0233678381587312,
+            "angularVelocity": -0.07643544392291168,
+            "velocityX": 0.6601917848050372,
+            "velocityY": -3.6968750288025483,
+            "timestamp": 0.7804848968016895
+        },
+        {
+            "x": 1.7349981432705297,
+            "y": 3.376556575471776,
+            "heading": 3.013707601859302,
+            "angularVelocity": -0.2475444775167341,
+            "velocityX": 0.9491931750224177,
+            "velocityY": -3.5552655517207494,
+            "timestamp": 0.819509141641774
+        },
+        {
+            "x": 1.7820959712007873,
+            "y": 3.2454658997474337,
+            "heading": 2.9995738602901865,
+            "angularVelocity": -0.3621784771756839,
+            "velocityX": 1.2068863375389995,
+            "velocityY": -3.3592110817655803,
+            "timestamp": 0.8585333864818585
+        },
+        {
+            "x": 1.8383454622663515,
+            "y": 3.1231345959204413,
+            "heading": 2.9823282080134397,
+            "angularVelocity": -0.44192148617905314,
+            "velocityX": 1.441398579167467,
+            "velocityY": -3.1347513405650798,
+            "timestamp": 0.897557631321943
+        },
+        {
+            "x": 1.9031002792591356,
+            "y": 3.0102116491438333,
+            "heading": 2.962874002379933,
+            "angularVelocity": -0.4985158768152219,
+            "velocityX": 1.6593483681274654,
+            "velocityY": -2.893661292853978,
+            "timestamp": 0.9365818761620275
+        },
+        {
+            "x": 1.9758873013923517,
+            "y": 2.9071136453404876,
+            "heading": 2.9418434761111434,
+            "angularVelocity": -0.5389092435989264,
+            "velocityX": 1.8651743917531916,
+            "velocityY": -2.6418961910942005,
+            "timestamp": 0.975606121002112
+        },
+        {
+            "x": 2.0563490115304868,
+            "y": 2.814127161728483,
+            "heading": 2.919699686688593,
+            "angularVelocity": -0.5674367182066591,
+            "velocityX": 2.0618390046462407,
+            "velocityY": -2.3827875207591935,
+            "timestamp": 1.0146303658421965
+        },
+        {
+            "x": 2.1442072150434264,
+            "y": 2.731459952378173,
+            "heading": 2.8967947458212753,
+            "angularVelocity": -0.586941296652354,
+            "velocityX": 2.2513748535810034,
+            "velocityY": -2.11835513253537,
+            "timestamp": 1.0536546106822808
+        },
+        {
+            "x": 2.2392399311065674,
+            "y": 2.659268856048584,
+            "heading": 2.8734047162081335,
+            "angularVelocity": -0.5993717420795943,
+            "velocityX": 2.4352224226909684,
+            "velocityY": -1.849903736136748,
+            "timestamp": 1.0926788555223652
+        },
+        {
+            "x": 2.338491380229597,
+            "y": 2.598983086121257,
+            "heading": 2.850361085284195,
+            "angularVelocity": -0.6059434386623828,
+            "velocityX": 2.609864937185211,
+            "velocityY": -1.5852435257599564,
+            "timestamp": 1.1307081986155205
+        },
+        {
+            "x": 2.444184622569516,
+            "y": 2.5488913725009676,
+            "heading": 2.8272194988809463,
+            "angularVelocity": -0.608519225445519,
+            "velocityX": 2.779255010558571,
+            "velocityY": -1.3171858766423765,
+            "timestamp": 1.1687375417086758
+        },
+        {
+            "x": 2.5560703795596873,
+            "y": 2.509147043381333,
+            "heading": 2.804166943788384,
+            "angularVelocity": -0.6061781039996946,
+            "velocityX": 2.9420901832588977,
+            "velocityY": -1.0450963883935758,
+            "timestamp": 1.2067668848018311
+        },
+        {
+            "x": 2.67382962144658,
+            "y": 2.4799341019260455,
+            "heading": 2.7814386686411225,
+            "angularVelocity": -0.5976510057401518,
+            "velocityX": 3.096536314035744,
+            "velocityY": -0.7681684478149349,
+            "timestamp": 1.2447962278949865
+        },
+        {
+            "x": 2.7970412331199452,
+            "y": 2.46147554775192,
+            "heading": 2.7593387310325537,
+            "angularVelocity": -0.5811285657613138,
+            "velocityX": 3.239909018978801,
+            "velocityY": -0.48537662427964845,
+            "timestamp": 1.2828255709881418
+        },
+        {
+            "x": 2.925127627515394,
+            "y": 2.4540428102885845,
+            "heading": 2.738273276977094,
+            "angularVelocity": -0.5539263195754057,
+            "velocityX": 3.3680937922509844,
+            "velocityY": -0.19544743239797485,
+            "timestamp": 1.320854914081297
+        },
+        {
+            "x": 3.057257978674903,
+            "y": 2.457961811901398,
+            "heading": 2.7188066654897645,
+            "angularVelocity": -0.5118839796850494,
+            "velocityX": 3.47443159446217,
+            "velocityY": 0.10305204597435572,
+            "timestamp": 1.3588842571744524
+        },
+        {
+            "x": 3.192166604008838,
+            "y": 2.4735957350107154,
+            "heading": 2.701757241393799,
+            "angularVelocity": -0.4483228662195614,
+            "velocityX": 3.5474876598173757,
+            "velocityY": 0.41110158203408753,
+            "timestamp": 1.3969136002676077
+        },
+        {
+            "x": 3.3278072233432696,
+            "y": 2.5012232638000396,
+            "heading": 2.6883398256989803,
+            "angularVelocity": -0.3528174457801884,
+            "velocityX": 3.566735796675976,
+            "velocityY": 0.7264792537080078,
+            "timestamp": 1.434942943360763
+        },
+        {
+            "x": 3.4613865978283758,
+            "y": 2.5407156767305445,
+            "heading": 2.677154917488752,
+            "angularVelocity": -0.2941125799315481,
+            "velocityX": 3.512534364790241,
+            "velocityY": 1.0384721301591493,
+            "timestamp": 1.4729722864539183
+        },
+        {
+            "x": 3.5888930381916024,
+            "y": 2.590617982563406,
+            "heading": 2.6645014610802713,
+            "angularVelocity": -0.3327287662446667,
+            "velocityX": 3.35284362001453,
+            "velocityY": 1.3122053071130373,
+            "timestamp": 1.5110016295470736
+        },
+        {
+            "x": 3.7058440395321557,
+            "y": 2.646348954214802,
+            "heading": 2.6512815676345496,
+            "angularVelocity": -0.34762350255007635,
+            "velocityX": 3.0752832373169756,
+            "velocityY": 1.4654728985163021,
+            "timestamp": 1.549030972640229
+        },
+        {
+            "x": 3.810776558765862,
+            "y": 2.7030629098701224,
+            "heading": 2.6383327014335145,
+            "angularVelocity": -0.34049670985152497,
+            "velocityX": 2.7592514279479117,
+            "velocityY": 1.491320939107423,
+            "timestamp": 1.5870603157333842
+        },
+        {
+            "x": 3.9038282779644375,
+            "y": 2.757723211117941,
+            "heading": 2.626141706261417,
+            "angularVelocity": -0.3205681239939803,
+            "velocityX": 2.446840035354867,
+            "velocityY": 1.4373191015665727,
+            "timestamp": 1.6250896588265396
+        },
+        {
+            "x": 3.9854403181685942,
+            "y": 2.8085799309132606,
+            "heading": 2.6149760707773027,
+            "angularVelocity": -0.2936057942615943,
+            "velocityX": 2.1460281342290126,
+            "velocityY": 1.337302084623056,
+            "timestamp": 1.6631190019196949
+        },
+        {
+            "x": 4.056026736802803,
+            "y": 2.8545641040147816,
+            "heading": 2.6049941853375604,
+            "angularVelocity": -0.26247851337555306,
+            "velocityX": 1.8561040736703007,
+            "velocityY": 1.209176108797926,
+            "timestamp": 1.7011483450128502
+        },
+        {
+            "x": 4.115926253855544,
+            "y": 2.894973650540783,
+            "heading": 2.5962978923918545,
+            "angularVelocity": -0.22867323593790462,
+            "velocityX": 1.5750868193019991,
+            "velocityY": 1.0625886023594207,
+            "timestamp": 1.7391776881060055
+        },
+        {
+            "x": 4.1654099219914915,
+            "y": 2.9293183863277776,
+            "heading": 2.588957208164297,
+            "angularVelocity": -0.19302684796776065,
+            "velocityX": 1.301197026063159,
+            "velocityY": 0.9031114658716605,
+            "timestamp": 1.7772070311991608
+        },
+        {
+            "x": 4.20469589755594,
+            "y": 2.957239109446396,
+            "heading": 2.5830228338283105,
+            "angularVelocity": -0.15604724808021303,
+            "velocityX": 1.0330437596099378,
+            "velocityY": 0.7341889406352728,
+            "timestamp": 1.815236374292316
+        },
+        {
+            "x": 4.23396207855711,
+            "y": 2.9784623659467853,
+            "heading": 2.578532998453876,
+            "angularVelocity": -0.11806239627740928,
+            "velocityX": 0.769568407465833,
+            "velocityY": 0.5580758113124009,
+            "timestamp": 1.8532657173854714
+        },
+        {
+            "x": 4.253355635131929,
+            "y": 2.992773543728212,
+            "heading": 2.575517463166205,
+            "angularVelocity": -0.0792949612693492,
+            "velocityX": 0.5099629653689784,
+            "velocityY": 0.3763193528315992,
+            "timestamp": 1.8912950604786267
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -0.03990242909239911,
+            "velocityX": 0.25360324643122656,
+            "velocityY": 0.190023168532958,
+            "timestamp": 1.929324403571782
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -5.006919316497686e-30,
+            "velocityX": -1.2207915195710053e-31,
+            "velocityY": 7.406976698260565e-32,
+            "timestamp": 1.9673537466649373
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.traj
new file mode 100644
index 00000000..d6861c07
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.traj	
@@ -0,0 +1,472 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 1.712760733525474e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2447404981946972,
+            "y": 5.548452449977017,
+            "heading": 3.1382782323729086,
+            "angularVelocity": -0.0441204598358695,
+            "velocityX": -0.13477523592974824,
+            "velocityY": -0.2959070718806661,
+            "timestamp": 0.03902424484008447
+        },
+        {
+            "x": 2.2342214959104107,
+            "y": 5.525357349752352,
+            "heading": 3.134834682439341,
+            "angularVelocity": -0.08824129583236773,
+            "velocityX": -0.269550437872442,
+            "velocityY": -0.5918141483404872,
+            "timestamp": 0.07804848968016893
+        },
+        {
+            "x": 2.218442994608338,
+            "y": 5.490714699360087,
+            "heading": 3.129669313928024,
+            "angularVelocity": -0.13236306128370928,
+            "velocityX": -0.40432560237181914,
+            "velocityY": -0.8877212239269984,
+            "timestamp": 0.11707273452025341
+        },
+        {
+            "x": 2.1974049958845625,
+            "y": 5.44452449907313,
+            "heading": 3.1227820694139163,
+            "angularVelocity": -0.17648629825721382,
+            "velocityX": -0.539100725971382,
+            "velocityY": -1.1836282925201553,
+            "timestamp": 0.15609697936033787
+        },
+        {
+            "x": 2.17110750147078,
+            "y": 5.386786749443186,
+            "heading": 3.114172871086463,
+            "angularVelocity": -0.22061152913360044,
+            "velocityX": -0.6738758051961007,
+            "velocityY": -1.479535346975897,
+            "timestamp": 0.19512122420042233
+        },
+        {
+            "x": 2.139550513239859,
+            "y": 5.317501451364342,
+            "heading": 3.10384162215273,
+            "angularVelocity": -0.2647392403381948,
+            "velocityX": -0.8086508364284021,
+            "velocityY": -1.775442378520474,
+            "timestamp": 0.2341454690405068
+        },
+        {
+            "x": 2.1027340332257456,
+            "y": 5.236668606182096,
+            "heading": 3.091788209481888,
+            "angularVelocity": -0.30886985053106986,
+            "velocityX": -0.9434258155405923,
+            "velocityY": -2.0713493755866326,
+            "timestamp": 0.27316971388059125
+        },
+        {
+            "x": 2.0606580636823133,
+            "y": 5.1442882159029235,
+            "heading": 3.078012508890752,
+            "angularVelocity": -0.35300364292993813,
+            "velocityX": -1.078200736897071,
+            "velocityY": -2.3672563212366358,
+            "timestamp": 0.31219395872067573
+        },
+        {
+            "x": 2.013322607256609,
+            "y": 5.040360283671343,
+            "heading": 3.0625143970359345,
+            "angularVelocity": -0.3971405960148595,
+            "velocityX": -1.212975590422751,
+            "velocityY": -2.6631631863080094,
+            "timestamp": 0.3512182035607602
+        },
+        {
+            "x": 1.960727667582776,
+            "y": 4.92488481518758,
+            "heading": 3.0452937850632114,
+            "angularVelocity": -0.44127982599779075,
+            "velocityX": -1.3477503508231259,
+            "velocityY": -2.9590699053106113,
+            "timestamp": 0.3902424484008447
+        },
+        {
+            "x": 1.902873252468991,
+            "y": 4.797861825841284,
+            "heading": 3.026350773365474,
+            "angularVelocity": -0.4854164834032721,
+            "velocityX": -1.482524911138253,
+            "velocityY": -3.25497622995194,
+            "timestamp": 0.4292666932409292
+        },
+        {
+            "x": 1.8416825412927555,
+            "y": 4.663514351826141,
+            "heading": 3.02635076853487,
+            "angularVelocity": -1.2378468871067517e-7,
+            "velocityX": -1.5680178162827485,
+            "velocityY": -3.4426668489212484,
+            "timestamp": 0.4682909380810137
+        },
+        {
+            "x": 1.7807776273702332,
+            "y": 4.529037073556694,
+            "heading": 3.0263507637038423,
+            "angularVelocity": -1.237955425988301e-7,
+            "velocityX": -1.5606942343751464,
+            "velocityY": -3.445993095328429,
+            "timestamp": 0.5073151829210981
+        },
+        {
+            "x": 1.7316376139293153,
+            "y": 4.389829293964471,
+            "heading": 3.02635075879599,
+            "angularVelocity": -1.2576418143070467e-7,
+            "velocityX": -1.2592175362335267,
+            "velocityY": -3.5672126433882343,
+            "timestamp": 0.5463394277611826
+        },
+        {
+            "x": 1.6946418503710443,
+            "y": 4.246913724355564,
+            "heading": 3.026350753656809,
+            "angularVelocity": -1.3169201974173754e-7,
+            "velocityX": -0.9480199734770174,
+            "velocityY": -3.662225116579624,
+            "timestamp": 0.5853636726012671
+        },
+        {
+            "x": 1.6700643070030454,
+            "y": 4.101347620951498,
+            "heading": 3.02635074809997,
+            "angularVelocity": -1.423945209571316e-7,
+            "velocityX": -0.6298018954293696,
+            "velocityY": -3.7301452981493113,
+            "timestamp": 0.6243879174413516
+        },
+        {
+            "x": 1.6580868892212808,
+            "y": 3.9542079360929074,
+            "heading": 3.0263507418753948,
+            "angularVelocity": -1.595053312793056e-7,
+            "velocityX": -0.3069224742421484,
+            "velocityY": -3.7704684731645255,
+            "timestamp": 0.6634121622814361
+        },
+        {
+            "x": 1.6587982403438444,
+            "y": 3.806583282470554,
+            "heading": 3.0263507346134717,
+            "angularVelocity": -1.8608747530006848e-7,
+            "velocityX": 0.018228440434743125,
+            "velocityY": -3.7828958440399596,
+            "timestamp": 0.7024364071215206
+        },
+        {
+            "x": 1.6721931105563068,
+            "y": 3.659565885100826,
+            "heading": 3.0263506736368497,
+            "angularVelocity": -0.0000015625317616489644,
+            "velocityX": 0.34324482811563534,
+            "velocityY": -3.767334844586533,
+            "timestamp": 0.741460651961605
+        },
+        {
+            "x": 1.6979565964079348,
+            "y": 3.5152981288336416,
+            "heading": 3.0233678381587312,
+            "angularVelocity": -0.07643544392291168,
+            "velocityX": 0.6601917848050372,
+            "velocityY": -3.6968750288025483,
+            "timestamp": 0.7804848968016895
+        },
+        {
+            "x": 1.7349981432705297,
+            "y": 3.376556575471776,
+            "heading": 3.013707601859302,
+            "angularVelocity": -0.2475444775167341,
+            "velocityX": 0.9491931750224177,
+            "velocityY": -3.5552655517207494,
+            "timestamp": 0.819509141641774
+        },
+        {
+            "x": 1.7820959712007873,
+            "y": 3.2454658997474337,
+            "heading": 2.9995738602901865,
+            "angularVelocity": -0.3621784771756839,
+            "velocityX": 1.2068863375389995,
+            "velocityY": -3.3592110817655803,
+            "timestamp": 0.8585333864818585
+        },
+        {
+            "x": 1.8383454622663515,
+            "y": 3.1231345959204413,
+            "heading": 2.9823282080134397,
+            "angularVelocity": -0.44192148617905314,
+            "velocityX": 1.441398579167467,
+            "velocityY": -3.1347513405650798,
+            "timestamp": 0.897557631321943
+        },
+        {
+            "x": 1.9031002792591356,
+            "y": 3.0102116491438333,
+            "heading": 2.962874002379933,
+            "angularVelocity": -0.4985158768152219,
+            "velocityX": 1.6593483681274654,
+            "velocityY": -2.893661292853978,
+            "timestamp": 0.9365818761620275
+        },
+        {
+            "x": 1.9758873013923517,
+            "y": 2.9071136453404876,
+            "heading": 2.9418434761111434,
+            "angularVelocity": -0.5389092435989264,
+            "velocityX": 1.8651743917531916,
+            "velocityY": -2.6418961910942005,
+            "timestamp": 0.975606121002112
+        },
+        {
+            "x": 2.0563490115304868,
+            "y": 2.814127161728483,
+            "heading": 2.919699686688593,
+            "angularVelocity": -0.5674367182066591,
+            "velocityX": 2.0618390046462407,
+            "velocityY": -2.3827875207591935,
+            "timestamp": 1.0146303658421965
+        },
+        {
+            "x": 2.1442072150434264,
+            "y": 2.731459952378173,
+            "heading": 2.8967947458212753,
+            "angularVelocity": -0.586941296652354,
+            "velocityX": 2.2513748535810034,
+            "velocityY": -2.11835513253537,
+            "timestamp": 1.0536546106822808
+        },
+        {
+            "x": 2.2392399311065674,
+            "y": 2.659268856048584,
+            "heading": 2.8734047162081335,
+            "angularVelocity": -0.5993717420795943,
+            "velocityX": 2.4352224226909684,
+            "velocityY": -1.849903736136748,
+            "timestamp": 1.0926788555223652
+        },
+        {
+            "x": 2.338491380229597,
+            "y": 2.598983086121257,
+            "heading": 2.850361085284195,
+            "angularVelocity": -0.6059434386623828,
+            "velocityX": 2.609864937185211,
+            "velocityY": -1.5852435257599564,
+            "timestamp": 1.1307081986155205
+        },
+        {
+            "x": 2.444184622569516,
+            "y": 2.5488913725009676,
+            "heading": 2.8272194988809463,
+            "angularVelocity": -0.608519225445519,
+            "velocityX": 2.779255010558571,
+            "velocityY": -1.3171858766423765,
+            "timestamp": 1.1687375417086758
+        },
+        {
+            "x": 2.5560703795596873,
+            "y": 2.509147043381333,
+            "heading": 2.804166943788384,
+            "angularVelocity": -0.6061781039996946,
+            "velocityX": 2.9420901832588977,
+            "velocityY": -1.0450963883935758,
+            "timestamp": 1.2067668848018311
+        },
+        {
+            "x": 2.67382962144658,
+            "y": 2.4799341019260455,
+            "heading": 2.7814386686411225,
+            "angularVelocity": -0.5976510057401518,
+            "velocityX": 3.096536314035744,
+            "velocityY": -0.7681684478149349,
+            "timestamp": 1.2447962278949865
+        },
+        {
+            "x": 2.7970412331199452,
+            "y": 2.46147554775192,
+            "heading": 2.7593387310325537,
+            "angularVelocity": -0.5811285657613138,
+            "velocityX": 3.239909018978801,
+            "velocityY": -0.48537662427964845,
+            "timestamp": 1.2828255709881418
+        },
+        {
+            "x": 2.925127627515394,
+            "y": 2.4540428102885845,
+            "heading": 2.738273276977094,
+            "angularVelocity": -0.5539263195754057,
+            "velocityX": 3.3680937922509844,
+            "velocityY": -0.19544743239797485,
+            "timestamp": 1.320854914081297
+        },
+        {
+            "x": 3.057257978674903,
+            "y": 2.457961811901398,
+            "heading": 2.7188066654897645,
+            "angularVelocity": -0.5118839796850494,
+            "velocityX": 3.47443159446217,
+            "velocityY": 0.10305204597435572,
+            "timestamp": 1.3588842571744524
+        },
+        {
+            "x": 3.192166604008838,
+            "y": 2.4735957350107154,
+            "heading": 2.701757241393799,
+            "angularVelocity": -0.4483228662195614,
+            "velocityX": 3.5474876598173757,
+            "velocityY": 0.41110158203408753,
+            "timestamp": 1.3969136002676077
+        },
+        {
+            "x": 3.3278072233432696,
+            "y": 2.5012232638000396,
+            "heading": 2.6883398256989803,
+            "angularVelocity": -0.3528174457801884,
+            "velocityX": 3.566735796675976,
+            "velocityY": 0.7264792537080078,
+            "timestamp": 1.434942943360763
+        },
+        {
+            "x": 3.4613865978283758,
+            "y": 2.5407156767305445,
+            "heading": 2.677154917488752,
+            "angularVelocity": -0.2941125799315481,
+            "velocityX": 3.512534364790241,
+            "velocityY": 1.0384721301591493,
+            "timestamp": 1.4729722864539183
+        },
+        {
+            "x": 3.5888930381916024,
+            "y": 2.590617982563406,
+            "heading": 2.6645014610802713,
+            "angularVelocity": -0.3327287662446667,
+            "velocityX": 3.35284362001453,
+            "velocityY": 1.3122053071130373,
+            "timestamp": 1.5110016295470736
+        },
+        {
+            "x": 3.7058440395321557,
+            "y": 2.646348954214802,
+            "heading": 2.6512815676345496,
+            "angularVelocity": -0.34762350255007635,
+            "velocityX": 3.0752832373169756,
+            "velocityY": 1.4654728985163021,
+            "timestamp": 1.549030972640229
+        },
+        {
+            "x": 3.810776558765862,
+            "y": 2.7030629098701224,
+            "heading": 2.6383327014335145,
+            "angularVelocity": -0.34049670985152497,
+            "velocityX": 2.7592514279479117,
+            "velocityY": 1.491320939107423,
+            "timestamp": 1.5870603157333842
+        },
+        {
+            "x": 3.9038282779644375,
+            "y": 2.757723211117941,
+            "heading": 2.626141706261417,
+            "angularVelocity": -0.3205681239939803,
+            "velocityX": 2.446840035354867,
+            "velocityY": 1.4373191015665727,
+            "timestamp": 1.6250896588265396
+        },
+        {
+            "x": 3.9854403181685942,
+            "y": 2.8085799309132606,
+            "heading": 2.6149760707773027,
+            "angularVelocity": -0.2936057942615943,
+            "velocityX": 2.1460281342290126,
+            "velocityY": 1.337302084623056,
+            "timestamp": 1.6631190019196949
+        },
+        {
+            "x": 4.056026736802803,
+            "y": 2.8545641040147816,
+            "heading": 2.6049941853375604,
+            "angularVelocity": -0.26247851337555306,
+            "velocityX": 1.8561040736703007,
+            "velocityY": 1.209176108797926,
+            "timestamp": 1.7011483450128502
+        },
+        {
+            "x": 4.115926253855544,
+            "y": 2.894973650540783,
+            "heading": 2.5962978923918545,
+            "angularVelocity": -0.22867323593790462,
+            "velocityX": 1.5750868193019991,
+            "velocityY": 1.0625886023594207,
+            "timestamp": 1.7391776881060055
+        },
+        {
+            "x": 4.1654099219914915,
+            "y": 2.9293183863277776,
+            "heading": 2.588957208164297,
+            "angularVelocity": -0.19302684796776065,
+            "velocityX": 1.301197026063159,
+            "velocityY": 0.9031114658716605,
+            "timestamp": 1.7772070311991608
+        },
+        {
+            "x": 4.20469589755594,
+            "y": 2.957239109446396,
+            "heading": 2.5830228338283105,
+            "angularVelocity": -0.15604724808021303,
+            "velocityX": 1.0330437596099378,
+            "velocityY": 0.7341889406352728,
+            "timestamp": 1.815236374292316
+        },
+        {
+            "x": 4.23396207855711,
+            "y": 2.9784623659467853,
+            "heading": 2.578532998453876,
+            "angularVelocity": -0.11806239627740928,
+            "velocityX": 0.769568407465833,
+            "velocityY": 0.5580758113124009,
+            "timestamp": 1.8532657173854714
+        },
+        {
+            "x": 4.253355635131929,
+            "y": 2.992773543728212,
+            "heading": 2.575517463166205,
+            "angularVelocity": -0.0792949612693492,
+            "velocityX": 0.5099629653689784,
+            "velocityY": 0.3763193528315992,
+            "timestamp": 1.8912950604786267
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -0.03990242909239911,
+            "velocityX": 0.25360324643122656,
+            "velocityY": 0.190023168532958,
+            "timestamp": 1.929324403571782
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": 2.574,
+            "angularVelocity": -5.006919316497686e-30,
+            "velocityX": -1.2207915195710053e-31,
+            "velocityY": 7.406976698260565e-32,
+            "timestamp": 1.9673537466649373
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.1.traj
new file mode 100644
index 00000000..9212064d
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.1.traj	
@@ -0,0 +1,499 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -4.120593510226553e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2473912878554576,
+            "y": 5.551785895834254,
+            "heading": 3.1358246059620716,
+            "angularVelocity": -0.12928713365744599,
+            "velocityX": -0.08077630820983339,
+            "velocityY": -0.2543419791057163,
+            "timestamp": 0.03229551092834574
+        },
+        {
+            "x": 2.242172157117942,
+            "y": 5.535355099551312,
+            "heading": 3.127553990435098,
+            "angularVelocity": -0.25609180004378934,
+            "velocityX": -0.16160545498398599,
+            "velocityY": -0.5087640916843489,
+            "timestamp": 0.06459102185669148
+        },
+        {
+            "x": 2.2343406441985514,
+            "y": 5.510704667985898,
+            "heading": 3.115281361261821,
+            "angularVelocity": -0.38001037359361073,
+            "velocityX": -0.24249540243429488,
+            "velocityY": -0.7632773365965662,
+            "timestamp": 0.09688653278503723
+        },
+        {
+            "x": 2.2238944664634275,
+            "y": 5.477831199999642,
+            "heading": 3.0991173688763074,
+            "angularVelocity": -0.5005027609371739,
+            "velocityX": -0.3234560294866074,
+            "velocityY": -1.017895894546866,
+            "timestamp": 0.12918204371338296
+        },
+        {
+            "x": 2.2108309419755985,
+            "y": 5.43673070422605,
+            "heading": 3.0791962484868605,
+            "angularVelocity": -0.6168386818107923,
+            "velocityX": -0.40449970018474013,
+            "velocityY": -1.272638041391628,
+            "timestamp": 0.1614775546417287
+        },
+        {
+            "x": 2.19514688330712,
+            "y": 5.387398428523646,
+            "heading": 3.0556846451348814,
+            "angularVelocity": -0.7280145963364562,
+            "velocityX": -0.4856420665793569,
+            "velocityY": -1.5275273338098838,
+            "timestamp": 0.19377306557007443
+        },
+        {
+            "x": 2.17683845193207,
+            "y": 5.329828642238818,
+            "heading": 3.028794799017408,
+            "angularVelocity": -0.832618693574285,
+            "velocityX": -0.5669032893045367,
+            "velocityY": -1.782594070505913,
+            "timestamp": 0.22606857649842016
+        },
+        {
+            "x": 2.1559009488672944,
+            "y": 5.264014369161548,
+            "heading": 2.9988052787110364,
+            "angularVelocity": -0.9285971778836174,
+            "velocityX": -0.6483100116059459,
+            "velocityY": -2.037876818957682,
+            "timestamp": 0.2583640874267659
+        },
+        {
+            "x": 2.132328497947263,
+            "y": 5.189947104033879,
+            "heading": 2.966095877804453,
+            "angularVelocity": -1.012815712349497,
+            "velocityX": -0.7298986838242586,
+            "velocityY": -2.293422924690741,
+            "timestamp": 0.2906595983551117
+        },
+        {
+            "x": 2.1061135495185,
+            "y": 5.107616699879427,
+            "heading": 2.9312120529633434,
+            "angularVelocity": -1.0801446962243946,
+            "velocityX": -0.8117211239334865,
+            "velocityY": -2.549283221966035,
+            "timestamp": 0.32295510928345744
+        },
+        {
+            "x": 2.0772461397099717,
+            "y": 5.017012347335229,
+            "heading": 2.89500074789377,
+            "angularVelocity": -1.1212488679902226,
+            "velocityX": -0.8938520859006283,
+            "velocityY": -2.805478220958382,
+            "timestamp": 0.3552506202118032
+        },
+        {
+            "x": 2.0457134444911373,
+            "y": 4.918129964629122,
+            "heading": 2.858963174705018,
+            "angularVelocity": -1.1158694243515237,
+            "velocityX": -0.976380131864019,
+            "velocityY": -3.0617996081715195,
+            "timestamp": 0.38754613114014896
+        },
+        {
+            "x": 2.0115103463196724,
+            "y": 4.811035711883079,
+            "heading": 2.8266343874462354,
+            "angularVelocity": -1.0010303701499224,
+            "velocityX": -1.0590666376931392,
+            "velocityY": -3.3160724096811367,
+            "timestamp": 0.4198416420684947
+        },
+        {
+            "x": 1.9751456955721198,
+            "y": 4.697500624139868,
+            "heading": 2.816667462304015,
+            "angularVelocity": -0.30861642549437257,
+            "velocityX": -1.1259970721081056,
+            "velocityY": -3.5155067834385165,
+            "timestamp": 0.4521371529968405
+        },
+        {
+            "x": 1.9382198046413848,
+            "y": 4.581042609670271,
+            "heading": 2.816667439698893,
+            "angularVelocity": -6.999462561903041e-7,
+            "velocityX": -1.1433753444143555,
+            "velocityY": -3.6060124494695995,
+            "timestamp": 0.48443266392518625
+        },
+        {
+            "x": 1.901294488957654,
+            "y": 4.4645844128058085,
+            "heading": 2.816667417094136,
+            "angularVelocity": -6.999349526609492e-7,
+            "velocityX": -1.1433575324340735,
+            "velocityY": -3.6060180971544047,
+            "timestamp": 0.516728174853532
+        },
+        {
+            "x": 1.8682421277834205,
+            "y": 4.346968376660799,
+            "heading": 2.816667394414886,
+            "angularVelocity": -7.022415636891642e-7,
+            "velocityX": -1.0234351531879067,
+            "velocityY": -3.6418694971559726,
+            "timestamp": 0.5490236857818777
+        },
+        {
+            "x": 1.8436394280164332,
+            "y": 4.227299259504863,
+            "heading": 2.81666737130494,
+            "angularVelocity": -7.15577656003266e-7,
+            "velocityX": -0.7617993665303343,
+            "velocityY": -3.705441211982887,
+            "timestamp": 0.5813191967102234
+        },
+        {
+            "x": 1.8276118980147091,
+            "y": 4.1061831643448095,
+            "heading": 2.8166673472723573,
+            "angularVelocity": -7.441462228176694e-7,
+            "velocityX": -0.4962773320813651,
+            "velocityY": -3.7502455195328888,
+            "timestamp": 0.6136147076385691
+        },
+        {
+            "x": 1.8202409034217624,
+            "y": 3.984233755462536,
+            "heading": 2.8166673217380835,
+            "angularVelocity": -7.906446726868519e-7,
+            "velocityX": -0.22823588731266858,
+            "velocityY": -3.776048292062723,
+            "timestamp": 0.6459102185669148
+        },
+        {
+            "x": 1.821563847305773,
+            "y": 3.8620689519300933,
+            "heading": 2.816667293968721,
+            "angularVelocity": -8.598520920997921e-7,
+            "velocityX": 0.04096370814339873,
+            "velocityY": -3.782717784013104,
+            "timestamp": 0.6782057294952605
+        },
+        {
+            "x": 1.831574050917961,
+            "y": 3.740307773577626,
+            "heading": 2.816667262971477,
+            "angularVelocity": -9.5980040013024e-7,
+            "velocityX": 0.30995650245006834,
+            "velocityY": -3.7702199114489767,
+            "timestamp": 0.7105012404236062
+        },
+        {
+            "x": 1.8502208046317663,
+            "y": 3.6195671988548175,
+            "heading": 2.816667227314634,
+            "angularVelocity": -0.00000110408047182699,
+            "velocityX": 0.5773791210542256,
+            "velocityY": -3.738617883788745,
+            "timestamp": 0.7427967513519519
+        },
+        {
+            "x": 1.8774096306565946,
+            "y": 3.5004590367763506,
+            "heading": 2.8166671847943117,
+            "angularVelocity": -0.0000013166016324392044,
+            "velocityX": 0.841876324085786,
+            "velocityY": -3.688071767705825,
+            "timestamp": 0.7750922622802976
+        },
+        {
+            "x": 1.913002764036886,
+            "y": 3.383586826300032,
+            "heading": 2.8166671317574883,
+            "angularVelocity": -0.0000016422351652599994,
+            "velocityX": 1.1021077653566822,
+            "velocityY": -3.618837637702142,
+            "timestamp": 0.8073877732086433
+        },
+        {
+            "x": 1.9568198515105035,
+            "y": 3.2695427783414357,
+            "heading": 2.8166670615606066,
+            "angularVelocity": -0.0000021735801416730847,
+            "velocityX": 1.3567547381688734,
+            "velocityY": -3.53126625590866,
+            "timestamp": 0.839683284136989
+        },
+        {
+            "x": 2.008638858795166,
+            "y": 3.158904790878296,
+            "heading": 2.8166669305303302,
+            "angularVelocity": -0.000004057228785819985,
+            "velocityX": 1.6045266290919962,
+            "velocityY": -3.4258008089301617,
+            "timestamp": 0.8719787950653347
+        },
+        {
+            "x": 2.095074231369689,
+            "y": 3.013583476455127,
+            "heading": 2.8166668472664824,
+            "angularVelocity": -0.000001862874616466948,
+            "velocityX": 1.9338316217798797,
+            "velocityY": -3.2512956765222865,
+            "timestamp": 0.9166752265441874
+        },
+        {
+            "x": 2.195389426610098,
+            "y": 2.8774723973475123,
+            "heading": 2.8166667934576224,
+            "angularVelocity": -0.0000012038737383971035,
+            "velocityX": 2.2443669868336342,
+            "velocityY": -3.0452336932538677,
+            "timestamp": 0.9613716580230401
+        },
+        {
+            "x": 2.308610813706186,
+            "y": 2.751892601119491,
+            "heading": 2.8166667540699692,
+            "angularVelocity": -8.812259002769856e-7,
+            "velocityX": 2.5331191629841037,
+            "velocityY": -2.8096157136713913,
+            "timestamp": 1.006068089501893
+        },
+        {
+            "x": 2.433639499231453,
+            "y": 2.638062914068913,
+            "heading": 2.816666722710724,
+            "angularVelocity": -7.016051243778922e-7,
+            "velocityX": 2.7972856308321177,
+            "velocityY": -2.5467287495744175,
+            "timestamp": 1.0507645209807457
+        },
+        {
+            "x": 2.5692619900937426,
+            "y": 2.537088109244684,
+            "heading": 2.8166666961232654,
+            "angularVelocity": -5.94845213173575e-7,
+            "velocityX": 3.034302434780658,
+            "velocityY": -2.259124531496504,
+            "timestamp": 1.0954609524595984
+        },
+        {
+            "x": 2.7141619661921816,
+            "y": 2.449948169817928,
+            "heading": 2.8166666724078224,
+            "angularVelocity": -5.305891904122224e-7,
+            "velocityX": 3.241869010661312,
+            "velocityY": -1.9495950021868031,
+            "timestamp": 1.1401573839384511
+        },
+        {
+            "x": 2.866933035141265,
+            "y": 2.377488711333149,
+            "heading": 2.8166666503106814,
+            "angularVelocity": -4.943826596232375e-7,
+            "velocityX": 3.4179701576705006,
+            "velocityY": -1.6211463888132076,
+            "timestamp": 1.1848538154173038
+        },
+        {
+            "x": 3.0260921573816075,
+            "y": 2.3204120378316904,
+            "heading": 2.8166666288878814,
+            "angularVelocity": -4.79295537569982e-7,
+            "velocityX": 3.560891037031571,
+            "velocityY": -1.2769850212418752,
+            "timestamp": 1.2295502468961566
+        },
+        {
+            "x": 3.1869067054641667,
+            "y": 2.2681812614436785,
+            "heading": 2.8166666075502422,
+            "angularVelocity": -4.773902106314294e-7,
+            "velocityX": 3.597928128974802,
+            "velocityY": -1.1685670345455579,
+            "timestamp": 1.2742466783750093
+        },
+        {
+            "x": 3.3477213293158035,
+            "y": 2.2159507183434,
+            "heading": 2.8166665862126,
+            "angularVelocity": -4.773902944556367e-7,
+            "velocityX": 3.597929824167796,
+            "velocityY": -1.1685618151639285,
+            "timestamp": 1.318943109853862
+        },
+        {
+            "x": 3.508535966619386,
+            "y": 2.163720216660845,
+            "heading": 2.8166665648749514,
+            "angularVelocity": -4.773904335055608e-7,
+            "velocityX": 3.5979301251302083,
+            "velocityY": -1.1685608885189713,
+            "timestamp": 1.3636395413327147
+        },
+        {
+            "x": 3.669350617545261,
+            "y": 2.111489756920536,
+            "heading": 2.816666543537298,
+            "angularVelocity": -4.773905387074157e-7,
+            "velocityX": 3.5979304299038075,
+            "velocityY": -1.1685599501387545,
+            "timestamp": 1.4083359728115674
+        },
+        {
+            "x": 3.830165282253442,
+            "y": 2.0592593396151875,
+            "heading": 2.816666522199642,
+            "angularVelocity": -4.773905991181995e-7,
+            "velocityX": 3.597930738257429,
+            "velocityY": -1.168559000734998,
+            "timestamp": 1.4530324042904201
+        },
+        {
+            "x": 3.990979960989088,
+            "y": 2.0070289654996674,
+            "heading": 2.816666500861985,
+            "angularVelocity": -4.773906042282552e-7,
+            "velocityX": 3.597931052096018,
+            "velocityY": -1.1685580344424535,
+            "timestamp": 1.4977288357692728
+        },
+        {
+            "x": 4.151794654203474,
+            "y": 1.9547986359634688,
+            "heading": 2.8166664795243315,
+            "angularVelocity": -4.773905411161549e-7,
+            "velocityX": 3.597931376031047,
+            "velocityY": -1.1685570370625784,
+            "timestamp": 1.5424252672481256
+        },
+        {
+            "x": 4.312609353229998,
+            "y": 1.902568324322763,
+            "heading": 2.8166664581865346,
+            "angularVelocity": -4.773937498526183e-7,
+            "velocityX": 3.597931506066887,
+            "velocityY": -1.1685566366840177,
+            "timestamp": 1.5871216987269783
+        },
+        {
+            "x": 4.468360661654424,
+            "y": 1.8512590257414445,
+            "heading": 2.7996529762164895,
+            "angularVelocity": -0.38064519710247374,
+            "velocityX": 3.4846475047592103,
+            "velocityY": -1.1479506726525697,
+            "timestamp": 1.631818130205831
+        },
+        {
+            "x": 4.61007924771562,
+            "y": 1.8048518324023994,
+            "heading": 2.753515893557521,
+            "angularVelocity": -1.0322319060481902,
+            "velocityX": 3.1706912917253294,
+            "velocityY": -1.0382751330159734,
+            "timestamp": 1.6765145616846837
+        },
+        {
+            "x": 4.736004026753032,
+            "y": 1.7636749139398393,
+            "heading": 2.704438737111309,
+            "angularVelocity": -1.0980106201416129,
+            "velocityX": 2.8173340660761905,
+            "velocityY": -0.9212574046776457,
+            "timestamp": 1.7212109931635364
+        },
+        {
+            "x": 4.846114017481673,
+            "y": 1.7276966103545506,
+            "heading": 2.657457847080447,
+            "angularVelocity": -1.05111053559365,
+            "velocityX": 2.4635074229748755,
+            "velocityY": -0.8049480102748526,
+            "timestamp": 1.7659074246423891
+        },
+        {
+            "x": 4.9404369366183385,
+            "y": 1.6968910625978524,
+            "heading": 2.6148397238370804,
+            "angularVelocity": -0.9535016965175586,
+            "velocityX": 2.110300890157008,
+            "velocityY": -0.6892171642667541,
+            "timestamp": 1.8106038561212419
+        },
+        {
+            "x": 5.018999070193965,
+            "y": 1.6712409213417598,
+            "heading": 2.5778779941552346,
+            "angularVelocity": -0.8269503505964041,
+            "velocityX": 1.7576824586722803,
+            "velocityY": -0.5738744773892818,
+            "timestamp": 1.8553002876000946
+        },
+        {
+            "x": 5.08182145352524,
+            "y": 1.6507342817906896,
+            "heading": 2.547405196833757,
+            "angularVelocity": -0.6817724886134384,
+            "velocityX": 1.4055346534990782,
+            "velocityY": -0.45879813829819405,
+            "timestamp": 1.8999967190789473
+        },
+        {
+            "x": 5.128920654811687,
+            "y": 1.6353626111579855,
+            "heading": 2.524000017522932,
+            "angularVelocity": -0.52364760533286,
+            "velocityX": 1.0537575311516838,
+            "velocityY": -0.34391270452937267,
+            "timestamp": 1.9446931505578
+        },
+        {
+            "x": 5.160309902680238,
+            "y": 1.6251195157583214,
+            "heading": 2.5080885464702414,
+            "angularVelocity": -0.35598974070712264,
+            "velocityX": 0.702276374869034,
+            "velocityY": -0.2291703176462827,
+            "timestamp": 1.9893895820366527
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.18096626962419662,
+            "velocityX": 0.35103691280557286,
+            "velocityY": -0.1145396978893815,
+            "timestamp": 2.0340860135155054
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 7.15236707545134e-33,
+            "velocityX": -4.116164438326626e-35,
+            "velocityY": 1.5643849141244448e-34,
+            "timestamp": 2.078782444994358
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.traj
new file mode 100644
index 00000000..9212064d
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.traj	
@@ -0,0 +1,499 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 3.14,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -4.120593510226553e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2473912878554576,
+            "y": 5.551785895834254,
+            "heading": 3.1358246059620716,
+            "angularVelocity": -0.12928713365744599,
+            "velocityX": -0.08077630820983339,
+            "velocityY": -0.2543419791057163,
+            "timestamp": 0.03229551092834574
+        },
+        {
+            "x": 2.242172157117942,
+            "y": 5.535355099551312,
+            "heading": 3.127553990435098,
+            "angularVelocity": -0.25609180004378934,
+            "velocityX": -0.16160545498398599,
+            "velocityY": -0.5087640916843489,
+            "timestamp": 0.06459102185669148
+        },
+        {
+            "x": 2.2343406441985514,
+            "y": 5.510704667985898,
+            "heading": 3.115281361261821,
+            "angularVelocity": -0.38001037359361073,
+            "velocityX": -0.24249540243429488,
+            "velocityY": -0.7632773365965662,
+            "timestamp": 0.09688653278503723
+        },
+        {
+            "x": 2.2238944664634275,
+            "y": 5.477831199999642,
+            "heading": 3.0991173688763074,
+            "angularVelocity": -0.5005027609371739,
+            "velocityX": -0.3234560294866074,
+            "velocityY": -1.017895894546866,
+            "timestamp": 0.12918204371338296
+        },
+        {
+            "x": 2.2108309419755985,
+            "y": 5.43673070422605,
+            "heading": 3.0791962484868605,
+            "angularVelocity": -0.6168386818107923,
+            "velocityX": -0.40449970018474013,
+            "velocityY": -1.272638041391628,
+            "timestamp": 0.1614775546417287
+        },
+        {
+            "x": 2.19514688330712,
+            "y": 5.387398428523646,
+            "heading": 3.0556846451348814,
+            "angularVelocity": -0.7280145963364562,
+            "velocityX": -0.4856420665793569,
+            "velocityY": -1.5275273338098838,
+            "timestamp": 0.19377306557007443
+        },
+        {
+            "x": 2.17683845193207,
+            "y": 5.329828642238818,
+            "heading": 3.028794799017408,
+            "angularVelocity": -0.832618693574285,
+            "velocityX": -0.5669032893045367,
+            "velocityY": -1.782594070505913,
+            "timestamp": 0.22606857649842016
+        },
+        {
+            "x": 2.1559009488672944,
+            "y": 5.264014369161548,
+            "heading": 2.9988052787110364,
+            "angularVelocity": -0.9285971778836174,
+            "velocityX": -0.6483100116059459,
+            "velocityY": -2.037876818957682,
+            "timestamp": 0.2583640874267659
+        },
+        {
+            "x": 2.132328497947263,
+            "y": 5.189947104033879,
+            "heading": 2.966095877804453,
+            "angularVelocity": -1.012815712349497,
+            "velocityX": -0.7298986838242586,
+            "velocityY": -2.293422924690741,
+            "timestamp": 0.2906595983551117
+        },
+        {
+            "x": 2.1061135495185,
+            "y": 5.107616699879427,
+            "heading": 2.9312120529633434,
+            "angularVelocity": -1.0801446962243946,
+            "velocityX": -0.8117211239334865,
+            "velocityY": -2.549283221966035,
+            "timestamp": 0.32295510928345744
+        },
+        {
+            "x": 2.0772461397099717,
+            "y": 5.017012347335229,
+            "heading": 2.89500074789377,
+            "angularVelocity": -1.1212488679902226,
+            "velocityX": -0.8938520859006283,
+            "velocityY": -2.805478220958382,
+            "timestamp": 0.3552506202118032
+        },
+        {
+            "x": 2.0457134444911373,
+            "y": 4.918129964629122,
+            "heading": 2.858963174705018,
+            "angularVelocity": -1.1158694243515237,
+            "velocityX": -0.976380131864019,
+            "velocityY": -3.0617996081715195,
+            "timestamp": 0.38754613114014896
+        },
+        {
+            "x": 2.0115103463196724,
+            "y": 4.811035711883079,
+            "heading": 2.8266343874462354,
+            "angularVelocity": -1.0010303701499224,
+            "velocityX": -1.0590666376931392,
+            "velocityY": -3.3160724096811367,
+            "timestamp": 0.4198416420684947
+        },
+        {
+            "x": 1.9751456955721198,
+            "y": 4.697500624139868,
+            "heading": 2.816667462304015,
+            "angularVelocity": -0.30861642549437257,
+            "velocityX": -1.1259970721081056,
+            "velocityY": -3.5155067834385165,
+            "timestamp": 0.4521371529968405
+        },
+        {
+            "x": 1.9382198046413848,
+            "y": 4.581042609670271,
+            "heading": 2.816667439698893,
+            "angularVelocity": -6.999462561903041e-7,
+            "velocityX": -1.1433753444143555,
+            "velocityY": -3.6060124494695995,
+            "timestamp": 0.48443266392518625
+        },
+        {
+            "x": 1.901294488957654,
+            "y": 4.4645844128058085,
+            "heading": 2.816667417094136,
+            "angularVelocity": -6.999349526609492e-7,
+            "velocityX": -1.1433575324340735,
+            "velocityY": -3.6060180971544047,
+            "timestamp": 0.516728174853532
+        },
+        {
+            "x": 1.8682421277834205,
+            "y": 4.346968376660799,
+            "heading": 2.816667394414886,
+            "angularVelocity": -7.022415636891642e-7,
+            "velocityX": -1.0234351531879067,
+            "velocityY": -3.6418694971559726,
+            "timestamp": 0.5490236857818777
+        },
+        {
+            "x": 1.8436394280164332,
+            "y": 4.227299259504863,
+            "heading": 2.81666737130494,
+            "angularVelocity": -7.15577656003266e-7,
+            "velocityX": -0.7617993665303343,
+            "velocityY": -3.705441211982887,
+            "timestamp": 0.5813191967102234
+        },
+        {
+            "x": 1.8276118980147091,
+            "y": 4.1061831643448095,
+            "heading": 2.8166673472723573,
+            "angularVelocity": -7.441462228176694e-7,
+            "velocityX": -0.4962773320813651,
+            "velocityY": -3.7502455195328888,
+            "timestamp": 0.6136147076385691
+        },
+        {
+            "x": 1.8202409034217624,
+            "y": 3.984233755462536,
+            "heading": 2.8166673217380835,
+            "angularVelocity": -7.906446726868519e-7,
+            "velocityX": -0.22823588731266858,
+            "velocityY": -3.776048292062723,
+            "timestamp": 0.6459102185669148
+        },
+        {
+            "x": 1.821563847305773,
+            "y": 3.8620689519300933,
+            "heading": 2.816667293968721,
+            "angularVelocity": -8.598520920997921e-7,
+            "velocityX": 0.04096370814339873,
+            "velocityY": -3.782717784013104,
+            "timestamp": 0.6782057294952605
+        },
+        {
+            "x": 1.831574050917961,
+            "y": 3.740307773577626,
+            "heading": 2.816667262971477,
+            "angularVelocity": -9.5980040013024e-7,
+            "velocityX": 0.30995650245006834,
+            "velocityY": -3.7702199114489767,
+            "timestamp": 0.7105012404236062
+        },
+        {
+            "x": 1.8502208046317663,
+            "y": 3.6195671988548175,
+            "heading": 2.816667227314634,
+            "angularVelocity": -0.00000110408047182699,
+            "velocityX": 0.5773791210542256,
+            "velocityY": -3.738617883788745,
+            "timestamp": 0.7427967513519519
+        },
+        {
+            "x": 1.8774096306565946,
+            "y": 3.5004590367763506,
+            "heading": 2.8166671847943117,
+            "angularVelocity": -0.0000013166016324392044,
+            "velocityX": 0.841876324085786,
+            "velocityY": -3.688071767705825,
+            "timestamp": 0.7750922622802976
+        },
+        {
+            "x": 1.913002764036886,
+            "y": 3.383586826300032,
+            "heading": 2.8166671317574883,
+            "angularVelocity": -0.0000016422351652599994,
+            "velocityX": 1.1021077653566822,
+            "velocityY": -3.618837637702142,
+            "timestamp": 0.8073877732086433
+        },
+        {
+            "x": 1.9568198515105035,
+            "y": 3.2695427783414357,
+            "heading": 2.8166670615606066,
+            "angularVelocity": -0.0000021735801416730847,
+            "velocityX": 1.3567547381688734,
+            "velocityY": -3.53126625590866,
+            "timestamp": 0.839683284136989
+        },
+        {
+            "x": 2.008638858795166,
+            "y": 3.158904790878296,
+            "heading": 2.8166669305303302,
+            "angularVelocity": -0.000004057228785819985,
+            "velocityX": 1.6045266290919962,
+            "velocityY": -3.4258008089301617,
+            "timestamp": 0.8719787950653347
+        },
+        {
+            "x": 2.095074231369689,
+            "y": 3.013583476455127,
+            "heading": 2.8166668472664824,
+            "angularVelocity": -0.000001862874616466948,
+            "velocityX": 1.9338316217798797,
+            "velocityY": -3.2512956765222865,
+            "timestamp": 0.9166752265441874
+        },
+        {
+            "x": 2.195389426610098,
+            "y": 2.8774723973475123,
+            "heading": 2.8166667934576224,
+            "angularVelocity": -0.0000012038737383971035,
+            "velocityX": 2.2443669868336342,
+            "velocityY": -3.0452336932538677,
+            "timestamp": 0.9613716580230401
+        },
+        {
+            "x": 2.308610813706186,
+            "y": 2.751892601119491,
+            "heading": 2.8166667540699692,
+            "angularVelocity": -8.812259002769856e-7,
+            "velocityX": 2.5331191629841037,
+            "velocityY": -2.8096157136713913,
+            "timestamp": 1.006068089501893
+        },
+        {
+            "x": 2.433639499231453,
+            "y": 2.638062914068913,
+            "heading": 2.816666722710724,
+            "angularVelocity": -7.016051243778922e-7,
+            "velocityX": 2.7972856308321177,
+            "velocityY": -2.5467287495744175,
+            "timestamp": 1.0507645209807457
+        },
+        {
+            "x": 2.5692619900937426,
+            "y": 2.537088109244684,
+            "heading": 2.8166666961232654,
+            "angularVelocity": -5.94845213173575e-7,
+            "velocityX": 3.034302434780658,
+            "velocityY": -2.259124531496504,
+            "timestamp": 1.0954609524595984
+        },
+        {
+            "x": 2.7141619661921816,
+            "y": 2.449948169817928,
+            "heading": 2.8166666724078224,
+            "angularVelocity": -5.305891904122224e-7,
+            "velocityX": 3.241869010661312,
+            "velocityY": -1.9495950021868031,
+            "timestamp": 1.1401573839384511
+        },
+        {
+            "x": 2.866933035141265,
+            "y": 2.377488711333149,
+            "heading": 2.8166666503106814,
+            "angularVelocity": -4.943826596232375e-7,
+            "velocityX": 3.4179701576705006,
+            "velocityY": -1.6211463888132076,
+            "timestamp": 1.1848538154173038
+        },
+        {
+            "x": 3.0260921573816075,
+            "y": 2.3204120378316904,
+            "heading": 2.8166666288878814,
+            "angularVelocity": -4.79295537569982e-7,
+            "velocityX": 3.560891037031571,
+            "velocityY": -1.2769850212418752,
+            "timestamp": 1.2295502468961566
+        },
+        {
+            "x": 3.1869067054641667,
+            "y": 2.2681812614436785,
+            "heading": 2.8166666075502422,
+            "angularVelocity": -4.773902106314294e-7,
+            "velocityX": 3.597928128974802,
+            "velocityY": -1.1685670345455579,
+            "timestamp": 1.2742466783750093
+        },
+        {
+            "x": 3.3477213293158035,
+            "y": 2.2159507183434,
+            "heading": 2.8166665862126,
+            "angularVelocity": -4.773902944556367e-7,
+            "velocityX": 3.597929824167796,
+            "velocityY": -1.1685618151639285,
+            "timestamp": 1.318943109853862
+        },
+        {
+            "x": 3.508535966619386,
+            "y": 2.163720216660845,
+            "heading": 2.8166665648749514,
+            "angularVelocity": -4.773904335055608e-7,
+            "velocityX": 3.5979301251302083,
+            "velocityY": -1.1685608885189713,
+            "timestamp": 1.3636395413327147
+        },
+        {
+            "x": 3.669350617545261,
+            "y": 2.111489756920536,
+            "heading": 2.816666543537298,
+            "angularVelocity": -4.773905387074157e-7,
+            "velocityX": 3.5979304299038075,
+            "velocityY": -1.1685599501387545,
+            "timestamp": 1.4083359728115674
+        },
+        {
+            "x": 3.830165282253442,
+            "y": 2.0592593396151875,
+            "heading": 2.816666522199642,
+            "angularVelocity": -4.773905991181995e-7,
+            "velocityX": 3.597930738257429,
+            "velocityY": -1.168559000734998,
+            "timestamp": 1.4530324042904201
+        },
+        {
+            "x": 3.990979960989088,
+            "y": 2.0070289654996674,
+            "heading": 2.816666500861985,
+            "angularVelocity": -4.773906042282552e-7,
+            "velocityX": 3.597931052096018,
+            "velocityY": -1.1685580344424535,
+            "timestamp": 1.4977288357692728
+        },
+        {
+            "x": 4.151794654203474,
+            "y": 1.9547986359634688,
+            "heading": 2.8166664795243315,
+            "angularVelocity": -4.773905411161549e-7,
+            "velocityX": 3.597931376031047,
+            "velocityY": -1.1685570370625784,
+            "timestamp": 1.5424252672481256
+        },
+        {
+            "x": 4.312609353229998,
+            "y": 1.902568324322763,
+            "heading": 2.8166664581865346,
+            "angularVelocity": -4.773937498526183e-7,
+            "velocityX": 3.597931506066887,
+            "velocityY": -1.1685566366840177,
+            "timestamp": 1.5871216987269783
+        },
+        {
+            "x": 4.468360661654424,
+            "y": 1.8512590257414445,
+            "heading": 2.7996529762164895,
+            "angularVelocity": -0.38064519710247374,
+            "velocityX": 3.4846475047592103,
+            "velocityY": -1.1479506726525697,
+            "timestamp": 1.631818130205831
+        },
+        {
+            "x": 4.61007924771562,
+            "y": 1.8048518324023994,
+            "heading": 2.753515893557521,
+            "angularVelocity": -1.0322319060481902,
+            "velocityX": 3.1706912917253294,
+            "velocityY": -1.0382751330159734,
+            "timestamp": 1.6765145616846837
+        },
+        {
+            "x": 4.736004026753032,
+            "y": 1.7636749139398393,
+            "heading": 2.704438737111309,
+            "angularVelocity": -1.0980106201416129,
+            "velocityX": 2.8173340660761905,
+            "velocityY": -0.9212574046776457,
+            "timestamp": 1.7212109931635364
+        },
+        {
+            "x": 4.846114017481673,
+            "y": 1.7276966103545506,
+            "heading": 2.657457847080447,
+            "angularVelocity": -1.05111053559365,
+            "velocityX": 2.4635074229748755,
+            "velocityY": -0.8049480102748526,
+            "timestamp": 1.7659074246423891
+        },
+        {
+            "x": 4.9404369366183385,
+            "y": 1.6968910625978524,
+            "heading": 2.6148397238370804,
+            "angularVelocity": -0.9535016965175586,
+            "velocityX": 2.110300890157008,
+            "velocityY": -0.6892171642667541,
+            "timestamp": 1.8106038561212419
+        },
+        {
+            "x": 5.018999070193965,
+            "y": 1.6712409213417598,
+            "heading": 2.5778779941552346,
+            "angularVelocity": -0.8269503505964041,
+            "velocityX": 1.7576824586722803,
+            "velocityY": -0.5738744773892818,
+            "timestamp": 1.8553002876000946
+        },
+        {
+            "x": 5.08182145352524,
+            "y": 1.6507342817906896,
+            "heading": 2.547405196833757,
+            "angularVelocity": -0.6817724886134384,
+            "velocityX": 1.4055346534990782,
+            "velocityY": -0.45879813829819405,
+            "timestamp": 1.8999967190789473
+        },
+        {
+            "x": 5.128920654811687,
+            "y": 1.6353626111579855,
+            "heading": 2.524000017522932,
+            "angularVelocity": -0.52364760533286,
+            "velocityX": 1.0537575311516838,
+            "velocityY": -0.34391270452937267,
+            "timestamp": 1.9446931505578
+        },
+        {
+            "x": 5.160309902680238,
+            "y": 1.6251195157583214,
+            "heading": 2.5080885464702414,
+            "angularVelocity": -0.35598974070712264,
+            "velocityX": 0.702276374869034,
+            "velocityY": -0.2291703176462827,
+            "timestamp": 1.9893895820366527
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": -0.18096626962419662,
+            "velocityX": 0.35103691280557286,
+            "velocityY": -0.1145396978893815,
+            "timestamp": 2.0340860135155054
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": 2.5,
+            "angularVelocity": 7.15236707545134e-33,
+            "velocityX": -4.116164438326626e-35,
+            "velocityY": 1.5643849141244448e-34,
+            "timestamp": 2.078782444994358
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.1.traj
new file mode 100644
index 00000000..ff4b41b1
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.1.traj	
@@ -0,0 +1,517 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -5.253317174178206e-37,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3042260943612694,
+            "y": 4.3705069747477046,
+            "heading": 2.7457396895687083,
+            "angularVelocity": -0.11528375442815593,
+            "velocityX": 0.11435786954779384,
+            "velocityY": 0.2843181303644528,
+            "timestamp": 0.03695499381005128
+        },
+        {
+            "x": 2.312934956515594,
+            "y": 4.391423172843284,
+            "heading": 2.737520345774514,
+            "angularVelocity": -0.2224149687709801,
+            "velocityX": 0.23566130734830049,
+            "velocityY": 0.5659911134903584,
+            "timestamp": 0.07390998762010256
+        },
+        {
+            "x": 2.326431561960596,
+            "y": 4.422622192917349,
+            "heading": 2.725704837211404,
+            "angularVelocity": -0.31972697989997184,
+            "velocityX": 0.3652173645157235,
+            "velocityY": 0.8442436828545633,
+            "timestamp": 0.11086498143015384
+        },
+        {
+            "x": 2.3450829408251197,
+            "y": 4.463936524002756,
+            "heading": 2.7107373323748916,
+            "angularVelocity": -0.40501981717128094,
+            "velocityX": 0.5047052357900914,
+            "velocityY": 1.1179634151141484,
+            "timestamp": 0.14781997524020513
+        },
+        {
+            "x": 2.369336863920227,
+            "y": 4.515137741117879,
+            "heading": 2.6931719166575077,
+            "angularVelocity": -0.4753191356943592,
+            "velocityX": 0.6563097593730617,
+            "velocityY": 1.3855019805522522,
+            "timestamp": 0.18477496905025642
+        },
+        {
+            "x": 2.3997470229979276,
+            "y": 4.575903866240714,
+            "heading": 2.6737152145246563,
+            "angularVelocity": -0.5264972369596066,
+            "velocityX": 0.8228971498144095,
+            "velocityY": 1.6443278392948362,
+            "timestamp": 0.22172996286030772
+        },
+        {
+            "x": 2.4370055110126323,
+            "y": 4.645763622209906,
+            "heading": 2.6532916288606927,
+            "angularVelocity": -0.5526610495171655,
+            "velocityX": 1.0082125356646991,
+            "velocityY": 1.890400965246281,
+            "timestamp": 0.258684956670359
+        },
+        {
+            "x": 2.4818450920186503,
+            "y": 4.724119837508184,
+            "heading": 2.6331026993833615,
+            "angularVelocity": -0.5463112666478134,
+            "velocityX": 1.213356474540179,
+            "velocityY": 2.1203146644004023,
+            "timestamp": 0.2956399504804103
+        },
+        {
+            "x": 2.5345067531707874,
+            "y": 4.810746963179403,
+            "heading": 2.6138856788459637,
+            "angularVelocity": -0.5200114668175445,
+            "velocityX": 1.425021511918485,
+            "velocityY": 2.3441250218165925,
+            "timestamp": 0.3325949442904616
+        },
+        {
+            "x": 2.595354080808564,
+            "y": 4.90526717496031,
+            "heading": 2.5968144984302115,
+            "angularVelocity": -0.46194515695220434,
+            "velocityX": 1.6465251746633045,
+            "velocityY": 2.557711476471661,
+            "timestamp": 0.3695499381005129
+        },
+        {
+            "x": 2.6649832778781826,
+            "y": 5.00693656288579,
+            "heading": 2.584029518775602,
+            "angularVelocity": -0.3459608116923114,
+            "velocityX": 1.8841620547283313,
+            "velocityY": 2.7511677703982533,
+            "timestamp": 0.4065049319105642
+        },
+        {
+            "x": 2.7444162884451204,
+            "y": 5.113834606219336,
+            "heading": 2.580154938465408,
+            "angularVelocity": -0.10484591960993231,
+            "velocityX": 2.149452682233515,
+            "velocityY": 2.8926548840192674,
+            "timestamp": 0.4434599257206155
+        },
+        {
+            "x": 2.835170549378822,
+            "y": 5.2201703041601135,
+            "heading": 2.5801550588658806,
+            "angularVelocity": 0.0000032580298363436347,
+            "velocityX": 2.4558050638616957,
+            "velocityY": 2.877437850141254,
+            "timestamp": 0.4804149195306668
+        },
+        {
+            "x": 2.934277899482491,
+            "y": 5.318767249922712,
+            "heading": 2.580155115984828,
+            "angularVelocity": 0.0000015456354283753369,
+            "velocityX": 2.681839174783285,
+            "velocityY": 2.6680276627669444,
+            "timestamp": 0.517369913340718
+        },
+        {
+            "x": 3.041080419735732,
+            "y": 5.408971720433865,
+            "heading": 2.5801551701001046,
+            "angularVelocity": 0.0000014643562563831181,
+            "velocityX": 2.8900700349783843,
+            "velocityY": 2.440927766753384,
+            "timestamp": 0.5543249071507693
+        },
+        {
+            "x": 3.1521380355083095,
+            "y": 5.493882443444743,
+            "heading": 2.5801552231562916,
+            "angularVelocity": 0.0000014356973685406667,
+            "velocityX": 3.005212674189958,
+            "velocityY": 2.2976792648733673,
+            "timestamp": 0.5912799009608206
+        },
+        {
+            "x": 3.263196121755215,
+            "y": 5.578792551102765,
+            "heading": 2.580155276212398,
+            "angularVelocity": 0.0000014356951704765205,
+            "velocityX": 3.005225405198131,
+            "velocityY": 2.29766261346058,
+            "timestamp": 0.6282348947708719
+        },
+        {
+            "x": 3.374254224320857,
+            "y": 5.6637026374167005,
+            "heading": 2.580155329268507,
+            "angularVelocity": 0.0000014356952512853246,
+            "velocityX": 3.0052258467821833,
+            "velocityY": 2.297662035890833,
+            "timestamp": 0.6651898885809232
+        },
+        {
+            "x": 3.485312339954463,
+            "y": 5.748612706638388,
+            "heading": 2.5801553823246177,
+            "angularVelocity": 0.0000014356952943350777,
+            "velocityX": 3.0052262004005508,
+            "velocityY": 2.2976615733755907,
+            "timestamp": 0.7021448823909745
+        },
+        {
+            "x": 3.5963704664420417,
+            "y": 5.833522761663614,
+            "heading": 2.58015543538073,
+            "angularVelocity": 0.0000014356953270064253,
+            "velocityX": 3.0052264941084177,
+            "velocityY": 2.297661189220169,
+            "timestamp": 0.7390998762010258
+        },
+        {
+            "x": 3.70742860215257,
+            "y": 5.918432804625673,
+            "heading": 2.5801554884368434,
+            "angularVelocity": 0.0000014356953601191765,
+            "velocityX": 3.0052267436809124,
+            "velocityY": 2.2976608627915387,
+            "timestamp": 0.7760548700110771
+        },
+        {
+            "x": 3.8184867458225393,
+            "y": 6.00334283717717,
+            "heading": 2.580155541492958,
+            "angularVelocity": 0.0000014356953984229,
+            "velocityX": 3.005226959062899,
+            "velocityY": 2.2976605810823423,
+            "timestamp": 0.8130098638211284
+        },
+        {
+            "x": 3.9295448964619117,
+            "y": 6.088252860613022,
+            "heading": 2.580155594549074,
+            "angularVelocity": 0.0000014356954384844398,
+            "velocityX": 3.0052271476545696,
+            "velocityY": 2.2976603344135205,
+            "timestamp": 0.8499648576311797
+        },
+        {
+            "x": 4.040603429478696,
+            "y": 6.173162383915615,
+            "heading": 2.5801556476051415,
+            "angularVelocity": 0.0000014356941260173984,
+            "velocityX": 3.005237494765267,
+            "velocityY": 2.2976468008363797,
+            "timestamp": 0.886919851441231
+        },
+        {
+            "x": 4.155351262344264,
+            "y": 6.253015747837148,
+            "heading": 2.580155700374003,
+            "angularVelocity": 0.0000014279223609881846,
+            "velocityX": 3.105069735781099,
+            "velocityY": 2.1608274197522537,
+            "timestamp": 0.9238748452512823
+        },
+        {
+            "x": 4.276216983795166,
+            "y": 6.32326602935791,
+            "heading": 2.5801557537673054,
+            "angularVelocity": 0.0000014448196748631945,
+            "velocityX": 3.2706194478654775,
+            "velocityY": 1.9009685641363763,
+            "timestamp": 0.9608298390613336
+        },
+        {
+            "x": 4.413687737524023,
+            "y": 6.387653258704535,
+            "heading": 2.580155801751457,
+            "angularVelocity": 0.0000011957738462706154,
+            "velocityX": 3.425796355950964,
+            "velocityY": 1.604541545618551,
+            "timestamp": 1.0009579551094623
+        },
+        {
+            "x": 4.556309721411339,
+            "y": 6.439642616989784,
+            "heading": 2.580155847910912,
+            "angularVelocity": 0.0000011503020593677978,
+            "velocityX": 3.55416595476994,
+            "velocityY": 1.2955843285264883,
+            "timestamp": 1.041086071157591
+        },
+        {
+            "x": 4.700491299069936,
+            "y": 6.48713573606665,
+            "heading": 2.580155893776208,
+            "angularVelocity": 0.000001142971587913107,
+            "velocityX": 3.593031317136076,
+            "velocityY": 1.1835372241224518,
+            "timestamp": 1.08121418720572
+        },
+        {
+            "x": 4.844672963256385,
+            "y": 6.5346285924578735,
+            "heading": 2.5801559396414997,
+            "angularVelocity": 0.0000011429714605909094,
+            "velocityX": 3.5930334734259763,
+            "velocityY": 1.1835306779481511,
+            "timestamp": 1.1213423032538488
+        },
+        {
+            "x": 4.988854630036052,
+            "y": 6.58212144097644,
+            "heading": 2.5801559855067953,
+            "angularVelocity": 0.0000011429715641749761,
+            "velocityX": 3.5930335380494514,
+            "velocityY": 1.18353048176008,
+            "timestamp": 1.1614704193019776
+        },
+        {
+            "x": 5.133036299227264,
+            "y": 6.62961428217388,
+            "heading": 2.580156031372096,
+            "angularVelocity": 0.0000011429716937026712,
+            "velocityX": 3.593033598145609,
+            "velocityY": 1.1835302993162664,
+            "timestamp": 1.2015985353501064
+        },
+        {
+            "x": 5.277217970668299,
+            "y": 6.677107116541164,
+            "heading": 2.580156077237403,
+            "angularVelocity": 0.0000011429718454732932,
+            "velocityX": 3.593033654211569,
+            "velocityY": 1.183530129107538,
+            "timestamp": 1.2417266513982352
+        },
+        {
+            "x": 5.421399644212107,
+            "y": 6.724599944524706,
+            "heading": 2.5801561231027166,
+            "angularVelocity": 0.0000011429720157589239,
+            "velocityX": 3.593033706613086,
+            "velocityY": 1.183529970023511,
+            "timestamp": 1.281854767446364
+        },
+        {
+            "x": 5.565581319724973,
+            "y": 6.772092766530448,
+            "heading": 2.580156168968038,
+            "angularVelocity": 0.0000011429722040135848,
+            "velocityX": 3.593033755682377,
+            "velocityY": 1.1835298210556366,
+            "timestamp": 1.3219828834944929
+        },
+        {
+            "x": 5.709762997085268,
+            "y": 6.819585582927637,
+            "heading": 2.5801562148333677,
+            "angularVelocity": 0.0000011429724102352075,
+            "velocityX": 3.5930338017206545,
+            "velocityY": 1.1835296812894733,
+            "timestamp": 1.3621109995426217
+        },
+        {
+            "x": 5.853944676177738,
+            "y": 6.867078394066171,
+            "heading": 2.580156260698706,
+            "angularVelocity": 0.0000011429726261958928,
+            "velocityX": 3.5930338448867536,
+            "velocityY": 1.183529550242829,
+            "timestamp": 1.4022391155907505
+        },
+        {
+            "x": 5.998126356881069,
+            "y": 6.91457120031434,
+            "heading": 2.5801563065640525,
+            "angularVelocity": 0.0000011429728355759127,
+            "velocityX": 3.593033885029711,
+            "velocityY": 1.183529428374024,
+            "timestamp": 1.4423672316388794
+        },
+        {
+            "x": 6.142308039084736,
+            "y": 6.962064002007683,
+            "heading": 2.580156352429407,
+            "angularVelocity": 0.0000011429730341793388,
+            "velocityX": 3.5930339224183525,
+            "velocityY": 1.1835293148669426,
+            "timestamp": 1.4824953476870082
+        },
+        {
+            "x": 6.286489722687973,
+            "y": 7.009556799452112,
+            "heading": 2.5801563982947693,
+            "angularVelocity": 0.0000011429732271830896,
+            "velocityX": 3.5930339572958836,
+            "velocityY": 1.1835292089832443,
+            "timestamp": 1.522623463735137
+        },
+        {
+            "x": 6.430671407598782,
+            "y": 7.057049592926921,
+            "heading": 2.580156444160139,
+            "angularVelocity": 0.000001142973413162616,
+            "velocityX": 3.5930339898808183,
+            "velocityY": 1.183529110059557,
+            "timestamp": 1.5627515797832658
+        },
+        {
+            "x": 6.574853093733082,
+            "y": 7.104542382687364,
+            "heading": 2.580156490025516,
+            "angularVelocity": 0.0000011429735908015805,
+            "velocityX": 3.593034020370455,
+            "velocityY": 1.1835290174969095,
+            "timestamp": 1.6028796958313947
+        },
+        {
+            "x": 6.719034781004445,
+            "y": 7.15203516899583,
+            "heading": 2.5801565358908998,
+            "angularVelocity": 0.0000011429737623342271,
+            "velocityX": 3.593034048706264,
+            "velocityY": 1.1835289314729884,
+            "timestamp": 1.6430078118795235
+        },
+        {
+            "x": 6.863216267597302,
+            "y": 7.199528564522877,
+            "heading": 2.580156581764359,
+            "angularVelocity": 0.0000011431750045992354,
+            "velocityX": 3.593029047761165,
+            "velocityY": 1.1835441133115774,
+            "timestamp": 1.6831359279276523
+        },
+        {
+            "x": 6.997116821056572,
+            "y": 7.244893026525659,
+            "heading": 2.605743326369695,
+            "angularVelocity": 0.6376263608948962,
+            "velocityX": 3.3368263114738097,
+            "velocityY": 1.1304907000461517,
+            "timestamp": 1.7232640439757811
+        },
+        {
+            "x": 7.1211832753718545,
+            "y": 7.2851403173258245,
+            "heading": 2.65804915297094,
+            "angularVelocity": 1.3034707769113922,
+            "velocityX": 3.091758760029488,
+            "velocityY": 1.0029698566435126,
+            "timestamp": 1.76339216002391
+        },
+        {
+            "x": 7.233669414925018,
+            "y": 7.321896904745842,
+            "heading": 2.730695495833792,
+            "angularVelocity": 1.8103601668147513,
+            "velocityX": 2.8031751956221984,
+            "velocityY": 0.9159808892082741,
+            "timestamp": 1.8035202760720388
+        },
+        {
+            "x": 7.333463334152891,
+            "y": 7.354467297173912,
+            "heading": 2.807930546248393,
+            "angularVelocity": 1.9247115992678774,
+            "velocityX": 2.486882740973504,
+            "velocityY": 0.811660143451645,
+            "timestamp": 1.8436483921201676
+        },
+        {
+            "x": 7.42052379329864,
+            "y": 7.382839226266775,
+            "heading": 2.882629127741458,
+            "angularVelocity": 1.8615023292763984,
+            "velocityX": 2.169562584032867,
+            "velocityY": 0.7070336683345496,
+            "timestamp": 1.8837765081682964
+        },
+        {
+            "x": 7.494937832171454,
+            "y": 7.407046736601957,
+            "heading": 2.951143873381968,
+            "angularVelocity": 1.7074000074744335,
+            "velocityX": 1.8544114750755487,
+            "velocityY": 0.6032555903234524,
+            "timestamp": 1.9239046242164253
+        },
+        {
+            "x": 7.556793519582923,
+            "y": 7.427131383067914,
+            "heading": 3.011211531798935,
+            "angularVelocity": 1.4968970470710155,
+            "velocityX": 1.5414550570298573,
+            "velocityY": 0.5005130677420416,
+            "timestamp": 1.964032740264554
+        },
+        {
+            "x": 7.606167226555021,
+            "y": 7.443134327538265,
+            "heading": 3.0612259719014916,
+            "angularVelocity": 1.2463690057756673,
+            "velocityX": 1.2304018188364594,
+            "velocityY": 0.3987963065885285,
+            "timestamp": 2.0041608563126827
+        },
+        {
+            "x": 7.643124291158215,
+            "y": 7.455092771789357,
+            "heading": 3.0999636114018587,
+            "angularVelocity": 0.9653490698119509,
+            "velocityX": 0.9209768173235189,
+            "velocityY": 0.2980066205139035,
+            "timestamp": 2.0442889723608113
+        },
+        {
+            "x": 7.667720023098554,
+            "y": 7.46303884043931,
+            "heading": 3.1264689002845456,
+            "angularVelocity": 0.6605166524861815,
+            "velocityX": 0.6129301438133489,
+            "velocityY": 0.1980174858052883,
+            "timestamp": 2.08441708840894
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 0.3371974826634153,
+            "velocityX": 0.30601927303833204,
+            "velocityY": 0.0987128216021636,
+            "timestamp": 2.1245452044570685
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": -8.758785880018889e-35,
+            "velocityX": 1.2314161274227302e-35,
+            "velocityY": -4.94083755736297e-36,
+            "timestamp": 2.164673320505197
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.traj
new file mode 100644
index 00000000..ff4b41b1
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.traj	
@@ -0,0 +1,517 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -5.253317174178206e-37,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3042260943612694,
+            "y": 4.3705069747477046,
+            "heading": 2.7457396895687083,
+            "angularVelocity": -0.11528375442815593,
+            "velocityX": 0.11435786954779384,
+            "velocityY": 0.2843181303644528,
+            "timestamp": 0.03695499381005128
+        },
+        {
+            "x": 2.312934956515594,
+            "y": 4.391423172843284,
+            "heading": 2.737520345774514,
+            "angularVelocity": -0.2224149687709801,
+            "velocityX": 0.23566130734830049,
+            "velocityY": 0.5659911134903584,
+            "timestamp": 0.07390998762010256
+        },
+        {
+            "x": 2.326431561960596,
+            "y": 4.422622192917349,
+            "heading": 2.725704837211404,
+            "angularVelocity": -0.31972697989997184,
+            "velocityX": 0.3652173645157235,
+            "velocityY": 0.8442436828545633,
+            "timestamp": 0.11086498143015384
+        },
+        {
+            "x": 2.3450829408251197,
+            "y": 4.463936524002756,
+            "heading": 2.7107373323748916,
+            "angularVelocity": -0.40501981717128094,
+            "velocityX": 0.5047052357900914,
+            "velocityY": 1.1179634151141484,
+            "timestamp": 0.14781997524020513
+        },
+        {
+            "x": 2.369336863920227,
+            "y": 4.515137741117879,
+            "heading": 2.6931719166575077,
+            "angularVelocity": -0.4753191356943592,
+            "velocityX": 0.6563097593730617,
+            "velocityY": 1.3855019805522522,
+            "timestamp": 0.18477496905025642
+        },
+        {
+            "x": 2.3997470229979276,
+            "y": 4.575903866240714,
+            "heading": 2.6737152145246563,
+            "angularVelocity": -0.5264972369596066,
+            "velocityX": 0.8228971498144095,
+            "velocityY": 1.6443278392948362,
+            "timestamp": 0.22172996286030772
+        },
+        {
+            "x": 2.4370055110126323,
+            "y": 4.645763622209906,
+            "heading": 2.6532916288606927,
+            "angularVelocity": -0.5526610495171655,
+            "velocityX": 1.0082125356646991,
+            "velocityY": 1.890400965246281,
+            "timestamp": 0.258684956670359
+        },
+        {
+            "x": 2.4818450920186503,
+            "y": 4.724119837508184,
+            "heading": 2.6331026993833615,
+            "angularVelocity": -0.5463112666478134,
+            "velocityX": 1.213356474540179,
+            "velocityY": 2.1203146644004023,
+            "timestamp": 0.2956399504804103
+        },
+        {
+            "x": 2.5345067531707874,
+            "y": 4.810746963179403,
+            "heading": 2.6138856788459637,
+            "angularVelocity": -0.5200114668175445,
+            "velocityX": 1.425021511918485,
+            "velocityY": 2.3441250218165925,
+            "timestamp": 0.3325949442904616
+        },
+        {
+            "x": 2.595354080808564,
+            "y": 4.90526717496031,
+            "heading": 2.5968144984302115,
+            "angularVelocity": -0.46194515695220434,
+            "velocityX": 1.6465251746633045,
+            "velocityY": 2.557711476471661,
+            "timestamp": 0.3695499381005129
+        },
+        {
+            "x": 2.6649832778781826,
+            "y": 5.00693656288579,
+            "heading": 2.584029518775602,
+            "angularVelocity": -0.3459608116923114,
+            "velocityX": 1.8841620547283313,
+            "velocityY": 2.7511677703982533,
+            "timestamp": 0.4065049319105642
+        },
+        {
+            "x": 2.7444162884451204,
+            "y": 5.113834606219336,
+            "heading": 2.580154938465408,
+            "angularVelocity": -0.10484591960993231,
+            "velocityX": 2.149452682233515,
+            "velocityY": 2.8926548840192674,
+            "timestamp": 0.4434599257206155
+        },
+        {
+            "x": 2.835170549378822,
+            "y": 5.2201703041601135,
+            "heading": 2.5801550588658806,
+            "angularVelocity": 0.0000032580298363436347,
+            "velocityX": 2.4558050638616957,
+            "velocityY": 2.877437850141254,
+            "timestamp": 0.4804149195306668
+        },
+        {
+            "x": 2.934277899482491,
+            "y": 5.318767249922712,
+            "heading": 2.580155115984828,
+            "angularVelocity": 0.0000015456354283753369,
+            "velocityX": 2.681839174783285,
+            "velocityY": 2.6680276627669444,
+            "timestamp": 0.517369913340718
+        },
+        {
+            "x": 3.041080419735732,
+            "y": 5.408971720433865,
+            "heading": 2.5801551701001046,
+            "angularVelocity": 0.0000014643562563831181,
+            "velocityX": 2.8900700349783843,
+            "velocityY": 2.440927766753384,
+            "timestamp": 0.5543249071507693
+        },
+        {
+            "x": 3.1521380355083095,
+            "y": 5.493882443444743,
+            "heading": 2.5801552231562916,
+            "angularVelocity": 0.0000014356973685406667,
+            "velocityX": 3.005212674189958,
+            "velocityY": 2.2976792648733673,
+            "timestamp": 0.5912799009608206
+        },
+        {
+            "x": 3.263196121755215,
+            "y": 5.578792551102765,
+            "heading": 2.580155276212398,
+            "angularVelocity": 0.0000014356951704765205,
+            "velocityX": 3.005225405198131,
+            "velocityY": 2.29766261346058,
+            "timestamp": 0.6282348947708719
+        },
+        {
+            "x": 3.374254224320857,
+            "y": 5.6637026374167005,
+            "heading": 2.580155329268507,
+            "angularVelocity": 0.0000014356952512853246,
+            "velocityX": 3.0052258467821833,
+            "velocityY": 2.297662035890833,
+            "timestamp": 0.6651898885809232
+        },
+        {
+            "x": 3.485312339954463,
+            "y": 5.748612706638388,
+            "heading": 2.5801553823246177,
+            "angularVelocity": 0.0000014356952943350777,
+            "velocityX": 3.0052262004005508,
+            "velocityY": 2.2976615733755907,
+            "timestamp": 0.7021448823909745
+        },
+        {
+            "x": 3.5963704664420417,
+            "y": 5.833522761663614,
+            "heading": 2.58015543538073,
+            "angularVelocity": 0.0000014356953270064253,
+            "velocityX": 3.0052264941084177,
+            "velocityY": 2.297661189220169,
+            "timestamp": 0.7390998762010258
+        },
+        {
+            "x": 3.70742860215257,
+            "y": 5.918432804625673,
+            "heading": 2.5801554884368434,
+            "angularVelocity": 0.0000014356953601191765,
+            "velocityX": 3.0052267436809124,
+            "velocityY": 2.2976608627915387,
+            "timestamp": 0.7760548700110771
+        },
+        {
+            "x": 3.8184867458225393,
+            "y": 6.00334283717717,
+            "heading": 2.580155541492958,
+            "angularVelocity": 0.0000014356953984229,
+            "velocityX": 3.005226959062899,
+            "velocityY": 2.2976605810823423,
+            "timestamp": 0.8130098638211284
+        },
+        {
+            "x": 3.9295448964619117,
+            "y": 6.088252860613022,
+            "heading": 2.580155594549074,
+            "angularVelocity": 0.0000014356954384844398,
+            "velocityX": 3.0052271476545696,
+            "velocityY": 2.2976603344135205,
+            "timestamp": 0.8499648576311797
+        },
+        {
+            "x": 4.040603429478696,
+            "y": 6.173162383915615,
+            "heading": 2.5801556476051415,
+            "angularVelocity": 0.0000014356941260173984,
+            "velocityX": 3.005237494765267,
+            "velocityY": 2.2976468008363797,
+            "timestamp": 0.886919851441231
+        },
+        {
+            "x": 4.155351262344264,
+            "y": 6.253015747837148,
+            "heading": 2.580155700374003,
+            "angularVelocity": 0.0000014279223609881846,
+            "velocityX": 3.105069735781099,
+            "velocityY": 2.1608274197522537,
+            "timestamp": 0.9238748452512823
+        },
+        {
+            "x": 4.276216983795166,
+            "y": 6.32326602935791,
+            "heading": 2.5801557537673054,
+            "angularVelocity": 0.0000014448196748631945,
+            "velocityX": 3.2706194478654775,
+            "velocityY": 1.9009685641363763,
+            "timestamp": 0.9608298390613336
+        },
+        {
+            "x": 4.413687737524023,
+            "y": 6.387653258704535,
+            "heading": 2.580155801751457,
+            "angularVelocity": 0.0000011957738462706154,
+            "velocityX": 3.425796355950964,
+            "velocityY": 1.604541545618551,
+            "timestamp": 1.0009579551094623
+        },
+        {
+            "x": 4.556309721411339,
+            "y": 6.439642616989784,
+            "heading": 2.580155847910912,
+            "angularVelocity": 0.0000011503020593677978,
+            "velocityX": 3.55416595476994,
+            "velocityY": 1.2955843285264883,
+            "timestamp": 1.041086071157591
+        },
+        {
+            "x": 4.700491299069936,
+            "y": 6.48713573606665,
+            "heading": 2.580155893776208,
+            "angularVelocity": 0.000001142971587913107,
+            "velocityX": 3.593031317136076,
+            "velocityY": 1.1835372241224518,
+            "timestamp": 1.08121418720572
+        },
+        {
+            "x": 4.844672963256385,
+            "y": 6.5346285924578735,
+            "heading": 2.5801559396414997,
+            "angularVelocity": 0.0000011429714605909094,
+            "velocityX": 3.5930334734259763,
+            "velocityY": 1.1835306779481511,
+            "timestamp": 1.1213423032538488
+        },
+        {
+            "x": 4.988854630036052,
+            "y": 6.58212144097644,
+            "heading": 2.5801559855067953,
+            "angularVelocity": 0.0000011429715641749761,
+            "velocityX": 3.5930335380494514,
+            "velocityY": 1.18353048176008,
+            "timestamp": 1.1614704193019776
+        },
+        {
+            "x": 5.133036299227264,
+            "y": 6.62961428217388,
+            "heading": 2.580156031372096,
+            "angularVelocity": 0.0000011429716937026712,
+            "velocityX": 3.593033598145609,
+            "velocityY": 1.1835302993162664,
+            "timestamp": 1.2015985353501064
+        },
+        {
+            "x": 5.277217970668299,
+            "y": 6.677107116541164,
+            "heading": 2.580156077237403,
+            "angularVelocity": 0.0000011429718454732932,
+            "velocityX": 3.593033654211569,
+            "velocityY": 1.183530129107538,
+            "timestamp": 1.2417266513982352
+        },
+        {
+            "x": 5.421399644212107,
+            "y": 6.724599944524706,
+            "heading": 2.5801561231027166,
+            "angularVelocity": 0.0000011429720157589239,
+            "velocityX": 3.593033706613086,
+            "velocityY": 1.183529970023511,
+            "timestamp": 1.281854767446364
+        },
+        {
+            "x": 5.565581319724973,
+            "y": 6.772092766530448,
+            "heading": 2.580156168968038,
+            "angularVelocity": 0.0000011429722040135848,
+            "velocityX": 3.593033755682377,
+            "velocityY": 1.1835298210556366,
+            "timestamp": 1.3219828834944929
+        },
+        {
+            "x": 5.709762997085268,
+            "y": 6.819585582927637,
+            "heading": 2.5801562148333677,
+            "angularVelocity": 0.0000011429724102352075,
+            "velocityX": 3.5930338017206545,
+            "velocityY": 1.1835296812894733,
+            "timestamp": 1.3621109995426217
+        },
+        {
+            "x": 5.853944676177738,
+            "y": 6.867078394066171,
+            "heading": 2.580156260698706,
+            "angularVelocity": 0.0000011429726261958928,
+            "velocityX": 3.5930338448867536,
+            "velocityY": 1.183529550242829,
+            "timestamp": 1.4022391155907505
+        },
+        {
+            "x": 5.998126356881069,
+            "y": 6.91457120031434,
+            "heading": 2.5801563065640525,
+            "angularVelocity": 0.0000011429728355759127,
+            "velocityX": 3.593033885029711,
+            "velocityY": 1.183529428374024,
+            "timestamp": 1.4423672316388794
+        },
+        {
+            "x": 6.142308039084736,
+            "y": 6.962064002007683,
+            "heading": 2.580156352429407,
+            "angularVelocity": 0.0000011429730341793388,
+            "velocityX": 3.5930339224183525,
+            "velocityY": 1.1835293148669426,
+            "timestamp": 1.4824953476870082
+        },
+        {
+            "x": 6.286489722687973,
+            "y": 7.009556799452112,
+            "heading": 2.5801563982947693,
+            "angularVelocity": 0.0000011429732271830896,
+            "velocityX": 3.5930339572958836,
+            "velocityY": 1.1835292089832443,
+            "timestamp": 1.522623463735137
+        },
+        {
+            "x": 6.430671407598782,
+            "y": 7.057049592926921,
+            "heading": 2.580156444160139,
+            "angularVelocity": 0.000001142973413162616,
+            "velocityX": 3.5930339898808183,
+            "velocityY": 1.183529110059557,
+            "timestamp": 1.5627515797832658
+        },
+        {
+            "x": 6.574853093733082,
+            "y": 7.104542382687364,
+            "heading": 2.580156490025516,
+            "angularVelocity": 0.0000011429735908015805,
+            "velocityX": 3.593034020370455,
+            "velocityY": 1.1835290174969095,
+            "timestamp": 1.6028796958313947
+        },
+        {
+            "x": 6.719034781004445,
+            "y": 7.15203516899583,
+            "heading": 2.5801565358908998,
+            "angularVelocity": 0.0000011429737623342271,
+            "velocityX": 3.593034048706264,
+            "velocityY": 1.1835289314729884,
+            "timestamp": 1.6430078118795235
+        },
+        {
+            "x": 6.863216267597302,
+            "y": 7.199528564522877,
+            "heading": 2.580156581764359,
+            "angularVelocity": 0.0000011431750045992354,
+            "velocityX": 3.593029047761165,
+            "velocityY": 1.1835441133115774,
+            "timestamp": 1.6831359279276523
+        },
+        {
+            "x": 6.997116821056572,
+            "y": 7.244893026525659,
+            "heading": 2.605743326369695,
+            "angularVelocity": 0.6376263608948962,
+            "velocityX": 3.3368263114738097,
+            "velocityY": 1.1304907000461517,
+            "timestamp": 1.7232640439757811
+        },
+        {
+            "x": 7.1211832753718545,
+            "y": 7.2851403173258245,
+            "heading": 2.65804915297094,
+            "angularVelocity": 1.3034707769113922,
+            "velocityX": 3.091758760029488,
+            "velocityY": 1.0029698566435126,
+            "timestamp": 1.76339216002391
+        },
+        {
+            "x": 7.233669414925018,
+            "y": 7.321896904745842,
+            "heading": 2.730695495833792,
+            "angularVelocity": 1.8103601668147513,
+            "velocityX": 2.8031751956221984,
+            "velocityY": 0.9159808892082741,
+            "timestamp": 1.8035202760720388
+        },
+        {
+            "x": 7.333463334152891,
+            "y": 7.354467297173912,
+            "heading": 2.807930546248393,
+            "angularVelocity": 1.9247115992678774,
+            "velocityX": 2.486882740973504,
+            "velocityY": 0.811660143451645,
+            "timestamp": 1.8436483921201676
+        },
+        {
+            "x": 7.42052379329864,
+            "y": 7.382839226266775,
+            "heading": 2.882629127741458,
+            "angularVelocity": 1.8615023292763984,
+            "velocityX": 2.169562584032867,
+            "velocityY": 0.7070336683345496,
+            "timestamp": 1.8837765081682964
+        },
+        {
+            "x": 7.494937832171454,
+            "y": 7.407046736601957,
+            "heading": 2.951143873381968,
+            "angularVelocity": 1.7074000074744335,
+            "velocityX": 1.8544114750755487,
+            "velocityY": 0.6032555903234524,
+            "timestamp": 1.9239046242164253
+        },
+        {
+            "x": 7.556793519582923,
+            "y": 7.427131383067914,
+            "heading": 3.011211531798935,
+            "angularVelocity": 1.4968970470710155,
+            "velocityX": 1.5414550570298573,
+            "velocityY": 0.5005130677420416,
+            "timestamp": 1.964032740264554
+        },
+        {
+            "x": 7.606167226555021,
+            "y": 7.443134327538265,
+            "heading": 3.0612259719014916,
+            "angularVelocity": 1.2463690057756673,
+            "velocityX": 1.2304018188364594,
+            "velocityY": 0.3987963065885285,
+            "timestamp": 2.0041608563126827
+        },
+        {
+            "x": 7.643124291158215,
+            "y": 7.455092771789357,
+            "heading": 3.0999636114018587,
+            "angularVelocity": 0.9653490698119509,
+            "velocityX": 0.9209768173235189,
+            "velocityY": 0.2980066205139035,
+            "timestamp": 2.0442889723608113
+        },
+        {
+            "x": 7.667720023098554,
+            "y": 7.46303884043931,
+            "heading": 3.1264689002845456,
+            "angularVelocity": 0.6605166524861815,
+            "velocityX": 0.6129301438133489,
+            "velocityY": 0.1980174858052883,
+            "timestamp": 2.08441708840894
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": 0.3371974826634153,
+            "velocityX": 0.30601927303833204,
+            "velocityY": 0.0987128216021636,
+            "timestamp": 2.1245452044570685
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 3.14,
+            "angularVelocity": -8.758785880018889e-35,
+            "velocityX": 1.2314161274227302e-35,
+            "velocityY": -4.94083755736297e-36,
+            "timestamp": 2.164673320505197
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.1.traj
new file mode 100644
index 00000000..66059810
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.1.traj	
@@ -0,0 +1,517 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 3.853182471863691e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 2.30610760298278,
+            "y": 4.37532703674674,
+            "heading": 2.7433864075571766,
+            "angularVelocity": -0.14821621401149343,
+            "velocityX": 0.13687656120621225,
+            "velocityY": 0.3434918886001163,
+            "timestamp": 0.04462124799861899
+        },
+        {
+            "x": 2.318848696543319,
+            "y": 4.405778929719923,
+            "heading": 2.7307174870785778,
+            "angularVelocity": -0.28392124933375007,
+            "velocityX": 0.28553870929233327,
+            "velocityY": 0.6824527403206025,
+            "timestamp": 0.08924249599723798
+        },
+        {
+            "x": 2.3388726580644836,
+            "y": 4.451074836672454,
+            "heading": 2.7126983162955036,
+            "angularVelocity": -0.4038248948938342,
+            "velocityX": 0.44875395510641974,
+            "velocityY": 1.0151196791701815,
+            "timestamp": 0.13386374399585696
+        },
+        {
+            "x": 2.3669952727104597,
+            "y": 4.510809884172449,
+            "heading": 2.6902411170995073,
+            "angularVelocity": -0.5032848744322687,
+            "velocityX": 0.6302516381174824,
+            "velocityY": 1.3387130611372482,
+            "timestamp": 0.17848499199447596
+        },
+        {
+            "x": 2.404258116570724,
+            "y": 4.5843745316547775,
+            "heading": 2.664559931990299,
+            "angularVelocity": -0.5755371322201343,
+            "velocityX": 0.8350919243992831,
+            "velocityY": 1.648646122237679,
+            "timestamp": 0.22310623999309495
+        },
+        {
+            "x": 2.452003610186855,
+            "y": 4.670805411588522,
+            "heading": 2.637322930497274,
+            "angularVelocity": -0.6104042964882519,
+            "velocityX": 1.070016993195909,
+            "velocityY": 1.9369893001742093,
+            "timestamp": 0.2677274879917139
+        },
+        {
+            "x": 2.5115645242184845,
+            "y": 4.768897077167795,
+            "heading": 2.6107796946069493,
+            "angularVelocity": -0.5948564211190585,
+            "velocityX": 1.3348105824712122,
+            "velocityY": 2.1983173931466227,
+            "timestamp": 0.3123487359903329
+        },
+        {
+            "x": 2.5833793756558516,
+            "y": 4.878178047966119,
+            "heading": 2.586043040896354,
+            "angularVelocity": -0.5543693827514722,
+            "velocityX": 1.60943170929665,
+            "velocityY": 2.449079210014641,
+            "timestamp": 0.3569699839889519
+        },
+        {
+            "x": 2.6682042395216055,
+            "y": 4.997714430694696,
+            "heading": 2.5651891379462763,
+            "angularVelocity": -0.46735364619839126,
+            "velocityX": 1.9009971184216836,
+            "velocityY": 2.678911686474495,
+            "timestamp": 0.4015912319875709
+        },
+        {
+            "x": 2.76752425780781,
+            "y": 5.124957228979585,
+            "heading": 2.5531720535122697,
+            "angularVelocity": -0.269313050911911,
+            "velocityX": 2.2258458187740118,
+            "velocityY": 2.8516189930148728,
+            "timestamp": 0.4462124799861899
+        },
+        {
+            "x": 2.882656696791853,
+            "y": 5.248398665617137,
+            "heading": 2.553172127817037,
+            "angularVelocity": 0.0000016652328226055909,
+            "velocityX": 2.580215573253489,
+            "velocityY": 2.76642725549436,
+            "timestamp": 0.4908337279848089
+        },
+        {
+            "x": 3.0093581189008844,
+            "y": 5.359933509101588,
+            "heading": 2.5531721828704046,
+            "angularVelocity": 0.000001233792641064896,
+            "velocityX": 2.8394862939053795,
+            "velocityY": 2.4995904078680997,
+            "timestamp": 0.5354549759834278
+        },
+        {
+            "x": 3.146402398989035,
+            "y": 5.458483685796272,
+            "heading": 2.553172234700022,
+            "angularVelocity": 0.0000011615456718217746,
+            "velocityX": 3.071278510461917,
+            "velocityY": 2.20859301599395,
+            "timestamp": 0.5800762239820468
+        },
+        {
+            "x": 3.286524028997536,
+            "y": 5.552607013873938,
+            "heading": 2.5531722858926313,
+            "angularVelocity": 0.0000011472697806901915,
+            "velocityX": 3.140244531323671,
+            "velocityY": 2.1093835851605776,
+            "timestamp": 0.6246974719806658
+        },
+        {
+            "x": 3.426645803515369,
+            "y": 5.646730126819845,
+            "heading": 2.5531723370852246,
+            "angularVelocity": 0.000001147269414679425,
+            "velocityX": 3.140247769900325,
+            "velocityY": 2.109378763875461,
+            "timestamp": 0.6693187199792848
+        },
+        {
+            "x": 3.5667675932030476,
+            "y": 5.740853217182279,
+            "heading": 2.5531723882778183,
+            "angularVelocity": 0.0000011472694285852317,
+            "velocityX": 3.1402481098694217,
+            "velocityY": 2.109378257760682,
+            "timestamp": 0.7139399679779038
+        },
+        {
+            "x": 3.70688939501189,
+            "y": 5.834976289499829,
+            "heading": 2.553172439470412,
+            "angularVelocity": 0.0000011472694368681445,
+            "velocityX": 3.1402483815149935,
+            "velocityY": 2.109377853359542,
+            "timestamp": 0.7585612159765228
+        },
+        {
+            "x": 3.8470112068261555,
+            "y": 5.929099346922214,
+            "heading": 2.553172490663007,
+            "angularVelocity": 0.0000011472694500175128,
+            "velocityX": 3.140248605744995,
+            "velocityY": 2.1093775195462685,
+            "timestamp": 0.8031824639751418
+        },
+        {
+            "x": 3.9871330270690666,
+            "y": 6.023222391796794,
+            "heading": 2.5531725418556026,
+            "angularVelocity": 0.000001147269470343035,
+            "velocityX": 3.1402487946380906,
+            "velocityY": 2.109377238339292,
+            "timestamp": 0.8478037119737608
+        },
+        {
+            "x": 4.127254854511192,
+            "y": 6.117345425953829,
+            "heading": 2.553172593048199,
+            "angularVelocity": 0.000001147269495912955,
+            "velocityX": 3.1402489559785947,
+            "velocityY": 2.109376998150064,
+            "timestamp": 0.8924249599723798
+        },
+        {
+            "x": 4.267376688165873,
+            "y": 6.21146845086217,
+            "heading": 2.5531726442407967,
+            "angularVelocity": 0.0000011472695239009516,
+            "velocityX": 3.1402490952072233,
+            "velocityY": 2.1093767908789607,
+            "timestamp": 0.9370462079709988
+        },
+        {
+            "x": 4.407498527228544,
+            "y": 6.305591467719579,
+            "heading": 2.553172695433396,
+            "angularVelocity": 0.0000011472695542073644,
+            "velocityX": 3.140249216404848,
+            "velocityY": 2.1093766104507625,
+            "timestamp": 0.9816674559696178
+        },
+        {
+            "x": 4.547620436764314,
+            "y": 6.399714379662917,
+            "heading": 2.5531727466259886,
+            "angularVelocity": 0.0000011472694062933976,
+            "velocityX": 3.1402507957667765,
+            "velocityY": 2.1093742592374776,
+            "timestamp": 1.0262887039682367
+        },
+        {
+            "x": 4.689466118908105,
+            "y": 6.4912188851781,
+            "heading": 2.553172797709037,
+            "angularVelocity": 0.0000011448144250292167,
+            "velocityX": 3.1788820014218735,
+            "velocityY": 2.050693551153348,
+            "timestamp": 1.0709099519668557
+        },
+        {
+            "x": 4.839613537732198,
+            "y": 6.568348804669705,
+            "heading": 2.5531728491363963,
+            "angularVelocity": 0.0000011525307241082326,
+            "velocityX": 3.364930959096914,
+            "velocityY": 1.7285468907994308,
+            "timestamp": 1.1155311999654747
+        },
+        {
+            "x": 4.996611329828,
+            "y": 6.630356582585623,
+            "heading": 2.5531729033694757,
+            "angularVelocity": 0.0000012154092882901391,
+            "velocityX": 3.5184536322395523,
+            "velocityY": 1.3896468767040242,
+            "timestamp": 1.1601524479640937
+        },
+        {
+            "x": 5.158940949366324,
+            "y": 6.676642268737186,
+            "heading": 2.553172963573569,
+            "angularVelocity": 0.0000013492247785707482,
+            "velocityX": 3.637944405843334,
+            "velocityY": 1.037301470209733,
+            "timestamp": 1.2047736959627127
+        },
+        {
+            "x": 5.325032205172685,
+            "y": 6.706758095236814,
+            "heading": 2.5531730345823234,
+            "angularVelocity": 0.0000015913663950616823,
+            "velocityX": 3.7222458639324056,
+            "velocityY": 0.6749212057125495,
+            "timestamp": 1.2493949439613317
+        },
+        {
+            "x": 5.493278503417969,
+            "y": 6.720412731170654,
+            "heading": 2.553173120074835,
+            "angularVelocity": 0.000001915959677785522,
+            "velocityX": 3.7705421921522753,
+            "velocityY": 0.30601196842950656,
+            "timestamp": 1.2940161919599507
+        },
+        {
+            "x": 5.617518941656642,
+            "y": 6.721477956029719,
+            "heading": 2.5531732061618477,
+            "angularVelocity": 0.0000026211268995160273,
+            "velocityX": 3.782800031375301,
+            "velocityY": 0.03243334205370069,
+            "timestamp": 1.32685970396381
+        },
+        {
+            "x": 5.741510905915031,
+            "y": 6.713552321996202,
+            "heading": 2.5531732789431283,
+            "angularVelocity": 0.0000022160017651615355,
+            "velocityX": 3.7752346412837734,
+            "velocityY": -0.24131505889453328,
+            "timestamp": 1.3597032159676692
+        },
+        {
+            "x": 5.8646046105180325,
+            "y": 6.696677372566761,
+            "heading": 2.553173343002795,
+            "angularVelocity": 0.0000019504511878675683,
+            "velocityX": 3.7478849578735156,
+            "velocityY": -0.5137985678101988,
+            "timestamp": 1.3925467279715285
+        },
+        {
+            "x": 5.986154981292517,
+            "y": 6.670941556957854,
+            "heading": 2.5531734012532086,
+            "angularVelocity": 0.0000017735744392757528,
+            "velocityX": 3.7008944341945598,
+            "velocityY": -0.7835890268336883,
+            "timestamp": 1.4253902399753877
+        },
+        {
+            "x": 6.105525040034572,
+            "y": 6.636479774106562,
+            "heading": 2.553173455691931,
+            "angularVelocity": 0.000001657518306287931,
+            "velocityX": 3.634509571571689,
+            "velocityY": -1.0492721621013206,
+            "timestamp": 1.458233751979247
+        },
+        {
+            "x": 6.222089256538776,
+            "y": 6.593472690949968,
+            "heading": 2.5531735077894595,
+            "angularVelocity": 0.0000015862350106976908,
+            "velocityX": 3.549078931951851,
+            "velocityY": -1.3094544563790795,
+            "timestamp": 1.4910772639831062
+        },
+        {
+            "x": 6.335236901536747,
+            "y": 6.54214592976541,
+            "heading": 2.553173558707783,
+            "angularVelocity": 0.000001550331263346088,
+            "velocityX": 3.4450531655894774,
+            "velocityY": -1.5627671358204458,
+            "timestamp": 1.5239207759869655
+        },
+        {
+            "x": 6.444377333823394,
+            "y": 6.482772601626653,
+            "heading": 2.5531736094378257,
+            "angularVelocity": 0.0000015445985995778538,
+            "velocityX": 3.3230439020592755,
+            "velocityY": -1.8077642893915407,
+            "timestamp": 1.5567642879908248
+        },
+        {
+            "x": 6.553087073787246,
+            "y": 6.422614322949958,
+            "heading": 2.5531736601729174,
+            "angularVelocity": 0.0000015447523320156895,
+            "velocityX": 3.3099304346952643,
+            "velocityY": -1.8316640032170186,
+            "timestamp": 1.589607799994684
+        },
+        {
+            "x": 6.661796772216766,
+            "y": 6.362455969218214,
+            "heading": 2.553173710908019,
+            "angularVelocity": 0.0000015447526253443646,
+            "velocityX": 3.3099291700822753,
+            "velocityY": -1.8316662884491148,
+            "timestamp": 1.6224513119985433
+        },
+        {
+            "x": 6.770506467400396,
+            "y": 6.302297609620959,
+            "heading": 2.5531737616431265,
+            "angularVelocity": 0.0000015447528238732798,
+            "velocityX": 3.3099290712533134,
+            "velocityY": -1.831666467038753,
+            "timestamp": 1.6552948240024026
+        },
+        {
+            "x": 6.879216090079623,
+            "y": 6.242139119005013,
+            "heading": 2.5531738123794403,
+            "angularVelocity": 0.0000015447895469442916,
+            "velocityX": 3.309926863681755,
+            "velocityY": -1.831670456219097,
+            "timestamp": 1.6881383360062618
+        },
+        {
+            "x": 6.983740227457298,
+            "y": 6.1833650705733145,
+            "heading": 2.56712091506752,
+            "angularVelocity": 0.4246532065888785,
+            "velocityX": 3.18248966083146,
+            "velocityY": -1.7895177721795115,
+            "timestamp": 1.720981848010121
+        },
+        {
+            "x": 7.082197224690116,
+            "y": 6.128185260935312,
+            "heading": 2.6040317014746788,
+            "angularVelocity": 1.1238379867177672,
+            "velocityX": 2.9977609343741736,
+            "velocityY": -1.6800824964004346,
+            "timestamp": 1.7538253600139804
+        },
+        {
+            "x": 7.174469097455313,
+            "y": 6.076712532186898,
+            "heading": 2.662828331449144,
+            "angularVelocity": 1.7902053217559901,
+            "velocityX": 2.809439890422065,
+            "velocityY": -1.567211470635817,
+            "timestamp": 1.7866688720178396
+        },
+        {
+            "x": 7.2591302790362535,
+            "y": 6.029614181571086,
+            "heading": 2.728764792304138,
+            "angularVelocity": 2.0075947069012146,
+            "velocityX": 2.577714026776202,
+            "velocityY": -1.4340229695982882,
+            "timestamp": 1.819512384021699
+        },
+        {
+            "x": 7.335970518862824,
+            "y": 5.986956845665554,
+            "heading": 2.7950149925408634,
+            "angularVelocity": 2.0171472596761757,
+            "velocityX": 2.339586577024463,
+            "velocityY": -1.2988055571072747,
+            "timestamp": 1.8523558960255582
+        },
+        {
+            "x": 7.405004233411928,
+            "y": 5.94870169343351,
+            "heading": 2.858549177078574,
+            "angularVelocity": 1.9344516058527972,
+            "velocityX": 2.1018980717102513,
+            "velocityY": -1.1647704492609365,
+            "timestamp": 1.8851994080294174
+        },
+        {
+            "x": 7.466271161714403,
+            "y": 5.914805217934882,
+            "heading": 2.9175764455342432,
+            "angularVelocity": 1.7972276670270115,
+            "velocityX": 1.8654195171112133,
+            "velocityY": -1.0320600152214254,
+            "timestamp": 1.9180429200332767
+        },
+        {
+            "x": 7.519808154158724,
+            "y": 5.885228450835136,
+            "heading": 2.9709348371498114,
+            "angularVelocity": 1.6246250282034163,
+            "velocityX": 1.6300629615380646,
+            "velocityY": -0.9005360661877405,
+            "timestamp": 1.950886432037136
+        },
+        {
+            "x": 7.565644888760801,
+            "y": 5.8599385495006775,
+            "heading": 3.0178335837303445,
+            "angularVelocity": 1.427945542943834,
+            "velocityX": 1.3956100247954721,
+            "velocityY": -0.770012090408821,
+            "timestamp": 1.9837299440409952
+        },
+        {
+            "x": 7.603804643651593,
+            "y": 5.838908591345399,
+            "heading": 3.057713507827927,
+            "angularVelocity": 1.214240550550639,
+            "velocityX": 1.1618658469383054,
+            "velocityY": -0.6403078377491017,
+            "timestamp": 2.0165734560448545
+        },
+        {
+            "x": 7.6343059239967275,
+            "y": 5.822116657822096,
+            "heading": 3.090162525369235,
+            "angularVelocity": 0.987988663864437,
+            "velocityX": 0.9286851035160462,
+            "velocityY": -0.5112709481656601,
+            "timestamp": 2.0494169680487135
+        },
+        {
+            "x": 7.657163897668815,
+            "y": 5.809544651656199,
+            "heading": 3.1148617613725005,
+            "angularVelocity": 0.7520278586639367,
+            "velocityX": 0.6959661825873334,
+            "velocityY": -0.3827850737892687,
+            "timestamp": 2.0822604800525726
+        },
+        {
+            "x": 7.67239146004521,
+            "y": 5.801177089276775,
+            "heading": 3.1315500981004414,
+            "angularVelocity": 0.5081166936708882,
+            "velocityX": 0.4636398925488508,
+            "velocityY": -0.2547706341039437,
+            "timestamp": 2.1151039920564316
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 0.257277659544025,
+            "velocityX": 0.2316603642721254,
+            "velocityY": -0.1271815656097946,
+            "timestamp": 2.1479475040602907
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": -3.3673783800968066e-32,
+            "velocityX": -2.4380081043450124e-32,
+            "velocityY": -4.2407952262773785e-32,
+            "timestamp": 2.1807910160641497
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.traj
new file mode 100644
index 00000000..66059810
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.traj	
@@ -0,0 +1,517 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 3.853182471863691e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 2.30610760298278,
+            "y": 4.37532703674674,
+            "heading": 2.7433864075571766,
+            "angularVelocity": -0.14821621401149343,
+            "velocityX": 0.13687656120621225,
+            "velocityY": 0.3434918886001163,
+            "timestamp": 0.04462124799861899
+        },
+        {
+            "x": 2.318848696543319,
+            "y": 4.405778929719923,
+            "heading": 2.7307174870785778,
+            "angularVelocity": -0.28392124933375007,
+            "velocityX": 0.28553870929233327,
+            "velocityY": 0.6824527403206025,
+            "timestamp": 0.08924249599723798
+        },
+        {
+            "x": 2.3388726580644836,
+            "y": 4.451074836672454,
+            "heading": 2.7126983162955036,
+            "angularVelocity": -0.4038248948938342,
+            "velocityX": 0.44875395510641974,
+            "velocityY": 1.0151196791701815,
+            "timestamp": 0.13386374399585696
+        },
+        {
+            "x": 2.3669952727104597,
+            "y": 4.510809884172449,
+            "heading": 2.6902411170995073,
+            "angularVelocity": -0.5032848744322687,
+            "velocityX": 0.6302516381174824,
+            "velocityY": 1.3387130611372482,
+            "timestamp": 0.17848499199447596
+        },
+        {
+            "x": 2.404258116570724,
+            "y": 4.5843745316547775,
+            "heading": 2.664559931990299,
+            "angularVelocity": -0.5755371322201343,
+            "velocityX": 0.8350919243992831,
+            "velocityY": 1.648646122237679,
+            "timestamp": 0.22310623999309495
+        },
+        {
+            "x": 2.452003610186855,
+            "y": 4.670805411588522,
+            "heading": 2.637322930497274,
+            "angularVelocity": -0.6104042964882519,
+            "velocityX": 1.070016993195909,
+            "velocityY": 1.9369893001742093,
+            "timestamp": 0.2677274879917139
+        },
+        {
+            "x": 2.5115645242184845,
+            "y": 4.768897077167795,
+            "heading": 2.6107796946069493,
+            "angularVelocity": -0.5948564211190585,
+            "velocityX": 1.3348105824712122,
+            "velocityY": 2.1983173931466227,
+            "timestamp": 0.3123487359903329
+        },
+        {
+            "x": 2.5833793756558516,
+            "y": 4.878178047966119,
+            "heading": 2.586043040896354,
+            "angularVelocity": -0.5543693827514722,
+            "velocityX": 1.60943170929665,
+            "velocityY": 2.449079210014641,
+            "timestamp": 0.3569699839889519
+        },
+        {
+            "x": 2.6682042395216055,
+            "y": 4.997714430694696,
+            "heading": 2.5651891379462763,
+            "angularVelocity": -0.46735364619839126,
+            "velocityX": 1.9009971184216836,
+            "velocityY": 2.678911686474495,
+            "timestamp": 0.4015912319875709
+        },
+        {
+            "x": 2.76752425780781,
+            "y": 5.124957228979585,
+            "heading": 2.5531720535122697,
+            "angularVelocity": -0.269313050911911,
+            "velocityX": 2.2258458187740118,
+            "velocityY": 2.8516189930148728,
+            "timestamp": 0.4462124799861899
+        },
+        {
+            "x": 2.882656696791853,
+            "y": 5.248398665617137,
+            "heading": 2.553172127817037,
+            "angularVelocity": 0.0000016652328226055909,
+            "velocityX": 2.580215573253489,
+            "velocityY": 2.76642725549436,
+            "timestamp": 0.4908337279848089
+        },
+        {
+            "x": 3.0093581189008844,
+            "y": 5.359933509101588,
+            "heading": 2.5531721828704046,
+            "angularVelocity": 0.000001233792641064896,
+            "velocityX": 2.8394862939053795,
+            "velocityY": 2.4995904078680997,
+            "timestamp": 0.5354549759834278
+        },
+        {
+            "x": 3.146402398989035,
+            "y": 5.458483685796272,
+            "heading": 2.553172234700022,
+            "angularVelocity": 0.0000011615456718217746,
+            "velocityX": 3.071278510461917,
+            "velocityY": 2.20859301599395,
+            "timestamp": 0.5800762239820468
+        },
+        {
+            "x": 3.286524028997536,
+            "y": 5.552607013873938,
+            "heading": 2.5531722858926313,
+            "angularVelocity": 0.0000011472697806901915,
+            "velocityX": 3.140244531323671,
+            "velocityY": 2.1093835851605776,
+            "timestamp": 0.6246974719806658
+        },
+        {
+            "x": 3.426645803515369,
+            "y": 5.646730126819845,
+            "heading": 2.5531723370852246,
+            "angularVelocity": 0.000001147269414679425,
+            "velocityX": 3.140247769900325,
+            "velocityY": 2.109378763875461,
+            "timestamp": 0.6693187199792848
+        },
+        {
+            "x": 3.5667675932030476,
+            "y": 5.740853217182279,
+            "heading": 2.5531723882778183,
+            "angularVelocity": 0.0000011472694285852317,
+            "velocityX": 3.1402481098694217,
+            "velocityY": 2.109378257760682,
+            "timestamp": 0.7139399679779038
+        },
+        {
+            "x": 3.70688939501189,
+            "y": 5.834976289499829,
+            "heading": 2.553172439470412,
+            "angularVelocity": 0.0000011472694368681445,
+            "velocityX": 3.1402483815149935,
+            "velocityY": 2.109377853359542,
+            "timestamp": 0.7585612159765228
+        },
+        {
+            "x": 3.8470112068261555,
+            "y": 5.929099346922214,
+            "heading": 2.553172490663007,
+            "angularVelocity": 0.0000011472694500175128,
+            "velocityX": 3.140248605744995,
+            "velocityY": 2.1093775195462685,
+            "timestamp": 0.8031824639751418
+        },
+        {
+            "x": 3.9871330270690666,
+            "y": 6.023222391796794,
+            "heading": 2.5531725418556026,
+            "angularVelocity": 0.000001147269470343035,
+            "velocityX": 3.1402487946380906,
+            "velocityY": 2.109377238339292,
+            "timestamp": 0.8478037119737608
+        },
+        {
+            "x": 4.127254854511192,
+            "y": 6.117345425953829,
+            "heading": 2.553172593048199,
+            "angularVelocity": 0.000001147269495912955,
+            "velocityX": 3.1402489559785947,
+            "velocityY": 2.109376998150064,
+            "timestamp": 0.8924249599723798
+        },
+        {
+            "x": 4.267376688165873,
+            "y": 6.21146845086217,
+            "heading": 2.5531726442407967,
+            "angularVelocity": 0.0000011472695239009516,
+            "velocityX": 3.1402490952072233,
+            "velocityY": 2.1093767908789607,
+            "timestamp": 0.9370462079709988
+        },
+        {
+            "x": 4.407498527228544,
+            "y": 6.305591467719579,
+            "heading": 2.553172695433396,
+            "angularVelocity": 0.0000011472695542073644,
+            "velocityX": 3.140249216404848,
+            "velocityY": 2.1093766104507625,
+            "timestamp": 0.9816674559696178
+        },
+        {
+            "x": 4.547620436764314,
+            "y": 6.399714379662917,
+            "heading": 2.5531727466259886,
+            "angularVelocity": 0.0000011472694062933976,
+            "velocityX": 3.1402507957667765,
+            "velocityY": 2.1093742592374776,
+            "timestamp": 1.0262887039682367
+        },
+        {
+            "x": 4.689466118908105,
+            "y": 6.4912188851781,
+            "heading": 2.553172797709037,
+            "angularVelocity": 0.0000011448144250292167,
+            "velocityX": 3.1788820014218735,
+            "velocityY": 2.050693551153348,
+            "timestamp": 1.0709099519668557
+        },
+        {
+            "x": 4.839613537732198,
+            "y": 6.568348804669705,
+            "heading": 2.5531728491363963,
+            "angularVelocity": 0.0000011525307241082326,
+            "velocityX": 3.364930959096914,
+            "velocityY": 1.7285468907994308,
+            "timestamp": 1.1155311999654747
+        },
+        {
+            "x": 4.996611329828,
+            "y": 6.630356582585623,
+            "heading": 2.5531729033694757,
+            "angularVelocity": 0.0000012154092882901391,
+            "velocityX": 3.5184536322395523,
+            "velocityY": 1.3896468767040242,
+            "timestamp": 1.1601524479640937
+        },
+        {
+            "x": 5.158940949366324,
+            "y": 6.676642268737186,
+            "heading": 2.553172963573569,
+            "angularVelocity": 0.0000013492247785707482,
+            "velocityX": 3.637944405843334,
+            "velocityY": 1.037301470209733,
+            "timestamp": 1.2047736959627127
+        },
+        {
+            "x": 5.325032205172685,
+            "y": 6.706758095236814,
+            "heading": 2.5531730345823234,
+            "angularVelocity": 0.0000015913663950616823,
+            "velocityX": 3.7222458639324056,
+            "velocityY": 0.6749212057125495,
+            "timestamp": 1.2493949439613317
+        },
+        {
+            "x": 5.493278503417969,
+            "y": 6.720412731170654,
+            "heading": 2.553173120074835,
+            "angularVelocity": 0.000001915959677785522,
+            "velocityX": 3.7705421921522753,
+            "velocityY": 0.30601196842950656,
+            "timestamp": 1.2940161919599507
+        },
+        {
+            "x": 5.617518941656642,
+            "y": 6.721477956029719,
+            "heading": 2.5531732061618477,
+            "angularVelocity": 0.0000026211268995160273,
+            "velocityX": 3.782800031375301,
+            "velocityY": 0.03243334205370069,
+            "timestamp": 1.32685970396381
+        },
+        {
+            "x": 5.741510905915031,
+            "y": 6.713552321996202,
+            "heading": 2.5531732789431283,
+            "angularVelocity": 0.0000022160017651615355,
+            "velocityX": 3.7752346412837734,
+            "velocityY": -0.24131505889453328,
+            "timestamp": 1.3597032159676692
+        },
+        {
+            "x": 5.8646046105180325,
+            "y": 6.696677372566761,
+            "heading": 2.553173343002795,
+            "angularVelocity": 0.0000019504511878675683,
+            "velocityX": 3.7478849578735156,
+            "velocityY": -0.5137985678101988,
+            "timestamp": 1.3925467279715285
+        },
+        {
+            "x": 5.986154981292517,
+            "y": 6.670941556957854,
+            "heading": 2.5531734012532086,
+            "angularVelocity": 0.0000017735744392757528,
+            "velocityX": 3.7008944341945598,
+            "velocityY": -0.7835890268336883,
+            "timestamp": 1.4253902399753877
+        },
+        {
+            "x": 6.105525040034572,
+            "y": 6.636479774106562,
+            "heading": 2.553173455691931,
+            "angularVelocity": 0.000001657518306287931,
+            "velocityX": 3.634509571571689,
+            "velocityY": -1.0492721621013206,
+            "timestamp": 1.458233751979247
+        },
+        {
+            "x": 6.222089256538776,
+            "y": 6.593472690949968,
+            "heading": 2.5531735077894595,
+            "angularVelocity": 0.0000015862350106976908,
+            "velocityX": 3.549078931951851,
+            "velocityY": -1.3094544563790795,
+            "timestamp": 1.4910772639831062
+        },
+        {
+            "x": 6.335236901536747,
+            "y": 6.54214592976541,
+            "heading": 2.553173558707783,
+            "angularVelocity": 0.000001550331263346088,
+            "velocityX": 3.4450531655894774,
+            "velocityY": -1.5627671358204458,
+            "timestamp": 1.5239207759869655
+        },
+        {
+            "x": 6.444377333823394,
+            "y": 6.482772601626653,
+            "heading": 2.5531736094378257,
+            "angularVelocity": 0.0000015445985995778538,
+            "velocityX": 3.3230439020592755,
+            "velocityY": -1.8077642893915407,
+            "timestamp": 1.5567642879908248
+        },
+        {
+            "x": 6.553087073787246,
+            "y": 6.422614322949958,
+            "heading": 2.5531736601729174,
+            "angularVelocity": 0.0000015447523320156895,
+            "velocityX": 3.3099304346952643,
+            "velocityY": -1.8316640032170186,
+            "timestamp": 1.589607799994684
+        },
+        {
+            "x": 6.661796772216766,
+            "y": 6.362455969218214,
+            "heading": 2.553173710908019,
+            "angularVelocity": 0.0000015447526253443646,
+            "velocityX": 3.3099291700822753,
+            "velocityY": -1.8316662884491148,
+            "timestamp": 1.6224513119985433
+        },
+        {
+            "x": 6.770506467400396,
+            "y": 6.302297609620959,
+            "heading": 2.5531737616431265,
+            "angularVelocity": 0.0000015447528238732798,
+            "velocityX": 3.3099290712533134,
+            "velocityY": -1.831666467038753,
+            "timestamp": 1.6552948240024026
+        },
+        {
+            "x": 6.879216090079623,
+            "y": 6.242139119005013,
+            "heading": 2.5531738123794403,
+            "angularVelocity": 0.0000015447895469442916,
+            "velocityX": 3.309926863681755,
+            "velocityY": -1.831670456219097,
+            "timestamp": 1.6881383360062618
+        },
+        {
+            "x": 6.983740227457298,
+            "y": 6.1833650705733145,
+            "heading": 2.56712091506752,
+            "angularVelocity": 0.4246532065888785,
+            "velocityX": 3.18248966083146,
+            "velocityY": -1.7895177721795115,
+            "timestamp": 1.720981848010121
+        },
+        {
+            "x": 7.082197224690116,
+            "y": 6.128185260935312,
+            "heading": 2.6040317014746788,
+            "angularVelocity": 1.1238379867177672,
+            "velocityX": 2.9977609343741736,
+            "velocityY": -1.6800824964004346,
+            "timestamp": 1.7538253600139804
+        },
+        {
+            "x": 7.174469097455313,
+            "y": 6.076712532186898,
+            "heading": 2.662828331449144,
+            "angularVelocity": 1.7902053217559901,
+            "velocityX": 2.809439890422065,
+            "velocityY": -1.567211470635817,
+            "timestamp": 1.7866688720178396
+        },
+        {
+            "x": 7.2591302790362535,
+            "y": 6.029614181571086,
+            "heading": 2.728764792304138,
+            "angularVelocity": 2.0075947069012146,
+            "velocityX": 2.577714026776202,
+            "velocityY": -1.4340229695982882,
+            "timestamp": 1.819512384021699
+        },
+        {
+            "x": 7.335970518862824,
+            "y": 5.986956845665554,
+            "heading": 2.7950149925408634,
+            "angularVelocity": 2.0171472596761757,
+            "velocityX": 2.339586577024463,
+            "velocityY": -1.2988055571072747,
+            "timestamp": 1.8523558960255582
+        },
+        {
+            "x": 7.405004233411928,
+            "y": 5.94870169343351,
+            "heading": 2.858549177078574,
+            "angularVelocity": 1.9344516058527972,
+            "velocityX": 2.1018980717102513,
+            "velocityY": -1.1647704492609365,
+            "timestamp": 1.8851994080294174
+        },
+        {
+            "x": 7.466271161714403,
+            "y": 5.914805217934882,
+            "heading": 2.9175764455342432,
+            "angularVelocity": 1.7972276670270115,
+            "velocityX": 1.8654195171112133,
+            "velocityY": -1.0320600152214254,
+            "timestamp": 1.9180429200332767
+        },
+        {
+            "x": 7.519808154158724,
+            "y": 5.885228450835136,
+            "heading": 2.9709348371498114,
+            "angularVelocity": 1.6246250282034163,
+            "velocityX": 1.6300629615380646,
+            "velocityY": -0.9005360661877405,
+            "timestamp": 1.950886432037136
+        },
+        {
+            "x": 7.565644888760801,
+            "y": 5.8599385495006775,
+            "heading": 3.0178335837303445,
+            "angularVelocity": 1.427945542943834,
+            "velocityX": 1.3956100247954721,
+            "velocityY": -0.770012090408821,
+            "timestamp": 1.9837299440409952
+        },
+        {
+            "x": 7.603804643651593,
+            "y": 5.838908591345399,
+            "heading": 3.057713507827927,
+            "angularVelocity": 1.214240550550639,
+            "velocityX": 1.1618658469383054,
+            "velocityY": -0.6403078377491017,
+            "timestamp": 2.0165734560448545
+        },
+        {
+            "x": 7.6343059239967275,
+            "y": 5.822116657822096,
+            "heading": 3.090162525369235,
+            "angularVelocity": 0.987988663864437,
+            "velocityX": 0.9286851035160462,
+            "velocityY": -0.5112709481656601,
+            "timestamp": 2.0494169680487135
+        },
+        {
+            "x": 7.657163897668815,
+            "y": 5.809544651656199,
+            "heading": 3.1148617613725005,
+            "angularVelocity": 0.7520278586639367,
+            "velocityX": 0.6959661825873334,
+            "velocityY": -0.3827850737892687,
+            "timestamp": 2.0822604800525726
+        },
+        {
+            "x": 7.67239146004521,
+            "y": 5.801177089276775,
+            "heading": 3.1315500981004414,
+            "angularVelocity": 0.5081166936708882,
+            "velocityX": 0.4636398925488508,
+            "velocityY": -0.2547706341039437,
+            "timestamp": 2.1151039920564316
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": 0.257277659544025,
+            "velocityX": 0.2316603642721254,
+            "velocityY": -0.1271815656097946,
+            "timestamp": 2.1479475040602907
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.14,
+            "angularVelocity": -3.3673783800968066e-32,
+            "velocityX": -2.4380081043450124e-32,
+            "velocityY": -4.2407952262773785e-32,
+            "timestamp": 2.1807910160641497
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.1.traj
new file mode 100644
index 00000000..af794028
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.1.traj	
@@ -0,0 +1,868 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": -5.720774693888146e-34,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3121576423431627,
+            "y": 4.3971322791414,
+            "heading": 2.7106310928070285,
+            "angularVelocity": -0.5647989987388474,
+            "velocityX": 0.1744174454420521,
+            "velocityY": 0.5327116136893348,
+            "timestamp": 0.06970427936465769
+        },
+        {
+            "x": 2.3388751368329244,
+            "y": 4.470343590887415,
+            "heading": 2.629622228062893,
+            "angularVelocity": -1.1621792160039144,
+            "velocityX": 0.38329776497637447,
+            "velocityY": 1.0503130139687884,
+            "timestamp": 0.13940855872931537
+        },
+        {
+            "x": 2.3840539747737246,
+            "y": 4.577503337755321,
+            "heading": 2.5035786127839184,
+            "angularVelocity": -1.8082622247563562,
+            "velocityX": 0.6481501329989676,
+            "velocityY": 1.5373481778256302,
+            "timestamp": 0.20911283809397307
+        },
+        {
+            "x": 2.4545399331182174,
+            "y": 4.713290189810374,
+            "heading": 2.3271901249556994,
+            "angularVelocity": -2.5305259510028497,
+            "velocityX": 1.011214217935542,
+            "velocityY": 1.948041831760208,
+            "timestamp": 0.27881711745863075
+        },
+        {
+            "x": 2.555797664071962,
+            "y": 4.87530128954077,
+            "heading": 2.141593101929859,
+            "angularVelocity": -2.662634557268572,
+            "velocityX": 1.4526759601661592,
+            "velocityY": 2.3242633193700435,
+            "timestamp": 0.3485213968232884
+        },
+        {
+            "x": 2.6889975632138237,
+            "y": 5.05237912689222,
+            "heading": 2.0338949096526036,
+            "angularVelocity": -1.545072888765306,
+            "velocityX": 1.910928573624968,
+            "velocityY": 2.54041558087227,
+            "timestamp": 0.4182256761879461
+        },
+        {
+            "x": 2.8587006072828576,
+            "y": 5.214124992637136,
+            "heading": 1.956646376370765,
+            "angularVelocity": -1.108232291990471,
+            "velocityX": 2.4346144256256212,
+            "velocityY": 2.3204581873480494,
+            "timestamp": 0.48792995555260377
+        },
+        {
+            "x": 3.035891081202655,
+            "y": 5.338211520127435,
+            "heading": 1.8312106553523118,
+            "angularVelocity": -1.7995411782716633,
+            "velocityX": 2.5420315012917163,
+            "velocityY": 1.7801852141837777,
+            "timestamp": 0.5576342349172615
+        },
+        {
+            "x": 3.2127166120567576,
+            "y": 5.42406293198425,
+            "heading": 1.6557539162164756,
+            "angularVelocity": -2.5171587847273322,
+            "velocityX": 2.536795910750906,
+            "velocityY": 1.231651953643812,
+            "timestamp": 0.6273385142819192
+        },
+        {
+            "x": 3.3872389413542425,
+            "y": 5.4708692312967075,
+            "heading": 1.441472502737752,
+            "angularVelocity": -3.0741500440412146,
+            "velocityX": 2.503753440796252,
+            "velocityY": 0.6714982170261123,
+            "timestamp": 0.697042793646577
+        },
+        {
+            "x": 3.558790922164917,
+            "y": 5.477728366851807,
+            "heading": 1.205442867585498,
+            "angularVelocity": -3.386157023695285,
+            "velocityX": 2.461139866509503,
+            "velocityY": 0.09840336371911824,
+            "timestamp": 0.7667470730112347
+        },
+        {
+            "x": 3.6181705627536003,
+            "y": 5.4752230037777725,
+            "heading": 1.1222490393160696,
+            "angularVelocity": -3.4253809174942864,
+            "velocityX": 2.44486750990022,
+            "velocityY": -0.10315456138644599,
+            "timestamp": 0.7910345406749146
+        },
+        {
+            "x": 3.6770128805090945,
+            "y": 5.467829320967843,
+            "heading": 1.0387507075885118,
+            "angularVelocity": -3.4379183900025705,
+            "velocityX": 2.4227440493307753,
+            "velocityY": -0.3044237839989221,
+            "timestamp": 0.8153220083385946
+        },
+        {
+            "x": 3.735181283810321,
+            "y": 5.455564580406075,
+            "heading": 0.9556144237478981,
+            "angularVelocity": -3.4230116120725995,
+            "velocityX": 2.394996633931211,
+            "velocityY": -0.5049822703463023,
+            "timestamp": 0.8396094760022745
+        },
+        {
+            "x": 3.7925490950380945,
+            "y": 5.438455755513165,
+            "heading": 0.8735217241735449,
+            "angularVelocity": -3.380043597427691,
+            "velocityX": 2.362033457838118,
+            "velocityY": -0.7044301666120085,
+            "timestamp": 0.8638969436659545
+        },
+        {
+            "x": 3.8490034281899477,
+            "y": 5.416538531692629,
+            "heading": 0.7931634356424634,
+            "angularVelocity": -3.308631827897467,
+            "velocityX": 2.3244223701541378,
+            "velocityY": -0.902408769989304,
+            "timestamp": 0.8881844113296344
+        },
+        {
+            "x": 3.9044487728906376,
+            "y": 5.389855903636327,
+            "heading": 0.7152329727062241,
+            "angularVelocity": -3.2086697557513846,
+            "velocityX": 2.282878786231156,
+            "velocityY": -1.0986171315094786,
+            "timestamp": 0.9124718789933144
+        },
+        {
+            "x": 3.958810912211869,
+            "y": 5.358456406059338,
+            "heading": 0.6404209450438546,
+            "angularVelocity": -3.080272867403366,
+            "velocityX": 2.2382794317633223,
+            "velocityY": -1.2928271490380632,
+            "timestamp": 0.9367593466569943
+        },
+        {
+            "x": 4.012042256408639,
+            "y": 5.322391998548557,
+            "heading": 0.5694141014307214,
+            "angularVelocity": -2.923600129762354,
+            "velocityX": 2.1917206410273016,
+            "velocityY": -1.4848978086221774,
+            "timestamp": 0.9610468143206743
+        },
+        {
+            "x": 4.064130365340956,
+            "y": 5.281715631109981,
+            "heading": 0.5029022798709881,
+            "angularVelocity": -2.738524348472814,
+            "velocityX": 2.1446496462128213,
+            "velocityY": -1.6747883312431175,
+            "timestamp": 0.9853342819843542
+        },
+        {
+            "x": 4.1151123432623455,
+            "y": 5.236478617606072,
+            "heading": 0.44159733021441666,
+            "angularVelocity": -2.524139218855187,
+            "velocityX": 2.09910636330477,
+            "velocityY": -1.8625660826533157,
+            "timestamp": 1.0096217496480342
+        },
+        {
+            "x": 4.165095928896697,
+            "y": 5.186728422848365,
+            "heading": 0.38626165615570274,
+            "angularVelocity": -2.278363262278863,
+            "velocityX": 2.057999060523636,
+            "velocityY": -2.0483895417431537,
+            "timestamp": 1.0339092173117141
+        },
+        {
+            "x": 4.214269146284126,
+            "y": 5.132508462649398,
+            "heading": 0.33770705635311316,
+            "angularVelocity": -1.9991627153126146,
+            "velocityX": 2.0246333651722517,
+            "velocityY": -2.232425419964549,
+            "timestamp": 1.058196684975394
+        },
+        {
+            "x": 4.262857394718322,
+            "y": 5.073859999949952,
+            "heading": 0.2966997330272638,
+            "angularVelocity": -1.6884149427884858,
+            "velocityX": 2.000548147177036,
+            "velocityY": -2.4147623585784754,
+            "timestamp": 1.082484152639074
+        },
+        {
+            "x": 4.311032470035773,
+            "y": 5.010821065356263,
+            "heading": 0.26381895203232264,
+            "angularVelocity": -1.353816768806731,
+            "velocityX": 1.9835363647027058,
+            "velocityY": -2.59553344410457,
+            "timestamp": 1.106771620302754
+        },
+        {
+            "x": 4.358621953046075,
+            "y": 4.943414538605821,
+            "heading": 0.23906687218938943,
+            "angularVelocity": -1.019129708608845,
+            "velocityX": 1.9594254810463076,
+            "velocityY": -2.77536249080602,
+            "timestamp": 1.131059087966434
+        },
+        {
+            "x": 4.401785346523604,
+            "y": 4.873960680394997,
+            "heading": 0.21204412295947933,
+            "angularVelocity": -1.1126211099529368,
+            "velocityX": 1.7771878927533025,
+            "velocityY": -2.8596582884878865,
+            "timestamp": 1.155346555630114
+        },
+        {
+            "x": 4.442527933324705,
+            "y": 4.807784869243466,
+            "heading": 0.1747999499890042,
+            "angularVelocity": -1.5334728793554109,
+            "velocityX": 1.6775148140301646,
+            "velocityY": -2.7246896246203156,
+            "timestamp": 1.1796340232937939
+        },
+        {
+            "x": 4.4813722294916225,
+            "y": 4.745319922619003,
+            "heading": 0.12759770745686402,
+            "angularVelocity": -1.9434814360134838,
+            "velocityX": 1.5993555484998627,
+            "velocityY": -2.571900351631775,
+            "timestamp": 1.2039214909574738
+        },
+        {
+            "x": 4.518612325734112,
+            "y": 4.686765196550011,
+            "heading": 0.07078885768669944,
+            "angularVelocity": -2.3390190594105262,
+            "velocityX": 1.5333050262040608,
+            "velocityY": -2.410902893616876,
+            "timestamp": 1.2282089586211538
+        },
+        {
+            "x": 4.554475342160425,
+            "y": 4.632242142038935,
+            "heading": 0.004632855038261056,
+            "angularVelocity": -2.723874039258911,
+            "velocityX": 1.47660583321928,
+            "velocityY": -2.244904873002102,
+            "timestamp": 1.2524964262848337
+        },
+        {
+            "x": 4.589156761961767,
+            "y": 4.581819399142938,
+            "heading": -0.07076666263842435,
+            "angularVelocity": -3.1044618863019378,
+            "velocityX": 1.4279553670061842,
+            "velocityY": -2.076080701134665,
+            "timestamp": 1.2767838939485137
+        },
+        {
+            "x": 4.6228372561227475,
+            "y": 4.535518314545829,
+            "heading": -0.1555015238515939,
+            "angularVelocity": -3.488830634240387,
+            "velocityX": 1.3867437572072365,
+            "velocityY": -1.9063776116251314,
+            "timestamp": 1.3010713616121936
+        },
+        {
+            "x": 4.654626762222228,
+            "y": 4.492711334026638,
+            "heading": -0.25016165034544124,
+            "angularVelocity": -3.8974885239025583,
+            "velocityX": 1.3088851641383707,
+            "velocityY": -1.762513124544663,
+            "timestamp": 1.3253588292758736
+        },
+        {
+            "x": 4.685559266058307,
+            "y": 4.453815281857176,
+            "heading": -0.35503604485130436,
+            "angularVelocity": -4.318045666930297,
+            "velocityX": 1.2735993832052854,
+            "velocityY": -1.6014865241643912,
+            "timestamp": 1.3496462969395535
+        },
+        {
+            "x": 4.716254412162635,
+            "y": 4.418834840133045,
+            "heading": -0.4707020428721612,
+            "angularVelocity": -4.762373732104905,
+            "velocityX": 1.2638265351241147,
+            "velocityY": -1.440267145530449,
+            "timestamp": 1.3739337646032335
+        },
+        {
+            "x": 4.747074366156251,
+            "y": 4.387541660715592,
+            "heading": -0.5981615568154537,
+            "angularVelocity": -5.247954035730924,
+            "velocityX": 1.268965312497503,
+            "velocityY": -1.2884496585146104,
+            "timestamp": 1.3982212322669134
+        },
+        {
+            "x": 4.7760218641280225,
+            "y": 4.359163482704458,
+            "heading": -0.7400182505648043,
+            "angularVelocity": -5.840736288925144,
+            "velocityX": 1.191869748325327,
+            "velocityY": -1.1684288540946337,
+            "timestamp": 1.4225086999305934
+        },
+        {
+            "x": 4.800681197551656,
+            "y": 4.328735146997818,
+            "heading": -0.8864907082583147,
+            "angularVelocity": -6.030783436206011,
+            "velocityX": 1.015311014104143,
+            "velocityY": -1.2528410177625973,
+            "timestamp": 1.4467961675942733
+        },
+        {
+            "x": 4.821257943768876,
+            "y": 4.295565486099031,
+            "heading": -1.0326334280053908,
+            "angularVelocity": -6.017206971545368,
+            "velocityX": 0.8472166181404436,
+            "velocityY": -1.3657109649348727,
+            "timestamp": 1.4710836352579533
+        },
+        {
+            "x": 4.842407281192705,
+            "y": 4.263513715444916,
+            "heading": -1.1735548575556736,
+            "angularVelocity": -5.802228190344444,
+            "velocityX": 0.8707922010105955,
+            "velocityY": -1.319683513240253,
+            "timestamp": 1.4953711029216332
+        },
+        {
+            "x": 4.867138629271623,
+            "y": 4.234341103924059,
+            "heading": -1.3080892458596534,
+            "angularVelocity": -5.539251360699312,
+            "velocityX": 1.0182761093657138,
+            "velocityY": -1.2011384605766136,
+            "timestamp": 1.5196585705853132
+        },
+        {
+            "x": 4.89576478236693,
+            "y": 4.207846540583478,
+            "heading": -1.4377333776779964,
+            "angularVelocity": -5.337902395325306,
+            "velocityX": 1.1786388557138734,
+            "velocityY": -1.0908738493227768,
+            "timestamp": 1.5439460382489931
+        },
+        {
+            "x": 4.928534232838641,
+            "y": 4.183793595288781,
+            "heading": -1.5636936940682342,
+            "angularVelocity": -5.1862268283573245,
+            "velocityX": 1.3492329017369804,
+            "velocityY": -0.9903438937220402,
+            "timestamp": 1.568233505912673
+        },
+        {
+            "x": 4.965642273050373,
+            "y": 4.1619364014492675,
+            "heading": -1.6870176099857392,
+            "angularVelocity": -5.077676998904523,
+            "velocityX": 1.5278678174928029,
+            "velocityY": -0.8999371256888635,
+            "timestamp": 1.592520973576353
+        },
+        {
+            "x": 5.007242132688007,
+            "y": 4.142028453113074,
+            "heading": -1.8086864696957439,
+            "angularVelocity": -5.009532545542041,
+            "velocityX": 1.712811735405568,
+            "velocityY": -0.8196798699587677,
+            "timestamp": 1.616808441240033
+        },
+        {
+            "x": 5.0534540304279405,
+            "y": 4.123819388018015,
+            "heading": -1.9296932957732547,
+            "angularVelocity": -4.982274305133403,
+            "velocityX": 1.902705476744277,
+            "velocityY": -0.749730904316916,
+            "timestamp": 1.641095908903713
+        },
+        {
+            "x": 5.102543724383187,
+            "y": 4.108450898288225,
+            "heading": -2.039853637710442,
+            "angularVelocity": -4.535686612644417,
+            "velocityX": 2.0211944133087334,
+            "velocityY": -0.6327744803453944,
+            "timestamp": 1.6653833765673929
+        },
+        {
+            "x": 5.15444764406018,
+            "y": 4.096180097409251,
+            "heading": -2.1398530735226235,
+            "angularVelocity": -4.117326565161934,
+            "velocityX": 2.1370659302868042,
+            "velocityY": -0.5052317948043931,
+            "timestamp": 1.6896708442310728
+        },
+        {
+            "x": 5.20922556245635,
+            "y": 4.087205112082737,
+            "heading": -2.231114838342211,
+            "angularVelocity": -3.757566086482643,
+            "velocityX": 2.2553985106519345,
+            "velocityY": -0.36953153991984866,
+            "timestamp": 1.7139583118947528
+        },
+        {
+            "x": 5.266890046930524,
+            "y": 4.081668558801882,
+            "heading": -2.314693981916533,
+            "angularVelocity": -3.4412456964094225,
+            "velocityX": 2.3742485331398995,
+            "velocityY": -0.22795926514540626,
+            "timestamp": 1.7382457795584327
+        },
+        {
+            "x": 5.327424168986788,
+            "y": 4.079676285408984,
+            "heading": -2.3913241555270592,
+            "angularVelocity": -3.1551322958679826,
+            "velocityX": 2.492401550236051,
+            "velocityY": -0.08202886445327343,
+            "timestamp": 1.7625332472221127
+        },
+        {
+            "x": 5.390789031982422,
+            "y": 4.081310272216797,
+            "heading": -2.461469590575517,
+            "angularVelocity": -2.888132925992722,
+            "velocityX": 2.608953056493045,
+            "velocityY": 0.06727695247771216,
+            "timestamp": 1.7868207148857926
+        },
+        {
+            "x": 5.47852793656002,
+            "y": 4.0897392429132315,
+            "heading": -2.541079365075053,
+            "angularVelocity": -2.4976756873239485,
+            "velocityX": 2.752718873700339,
+            "velocityY": 0.2644503807478311,
+            "timestamp": 1.8186942583180192
+        },
+        {
+            "x": 5.570818505990881,
+            "y": 4.101985641066581,
+            "heading": -2.595195903422616,
+            "angularVelocity": -1.6978513375091853,
+            "velocityX": 2.8955227280299005,
+            "velocityY": 0.384218283711974,
+            "timestamp": 1.8505678017502458
+        },
+        {
+            "x": 5.666172128712358,
+            "y": 4.1161647975520586,
+            "heading": -2.6133567142303673,
+            "angularVelocity": -0.5697769639691046,
+            "velocityX": 2.9916229089567223,
+            "velocityY": 0.4448566101734819,
+            "timestamp": 1.8824413451824724
+        },
+        {
+            "x": 5.762848661651843,
+            "y": 4.1299008615103485,
+            "heading": -2.5899687129898252,
+            "angularVelocity": 0.7337747461392797,
+            "velocityX": 3.033127871240633,
+            "velocityY": 0.43095503289420706,
+            "timestamp": 1.914314888614699
+        },
+        {
+            "x": 5.85803719097752,
+            "y": 4.141633583817035,
+            "heading": -2.5274756720169904,
+            "angularVelocity": 1.960655585900415,
+            "velocityX": 2.9864432716142737,
+            "velocityY": 0.36810222658906844,
+            "timestamp": 1.9461884320469256
+        },
+        {
+            "x": 5.9473700887935115,
+            "y": 4.148469896281193,
+            "heading": -2.4508224753320094,
+            "angularVelocity": 2.404916066140258,
+            "velocityX": 2.8027287899740645,
+            "velocityY": 0.21448234893285298,
+            "timestamp": 1.9780619754791522
+        },
+        {
+            "x": 6.0307522620060645,
+            "y": 4.150565536032173,
+            "heading": -2.3598343948212093,
+            "angularVelocity": 2.854658463194389,
+            "velocityX": 2.6160308592563855,
+            "velocityY": 0.06574856527754983,
+            "timestamp": 2.009935518911379
+        },
+        {
+            "x": 6.108380396897077,
+            "y": 4.1477533333139265,
+            "heading": -2.2542021077222087,
+            "angularVelocity": 3.3141055472419727,
+            "velocityX": 2.4355037605428165,
+            "velocityY": -0.0882299994108409,
+            "timestamp": 2.0418090623436056
+        },
+        {
+            "x": 6.180764914708381,
+            "y": 4.139596420595998,
+            "heading": -2.1333489972804904,
+            "angularVelocity": 3.7916433953661524,
+            "velocityX": 2.270990609036518,
+            "velocityY": -0.2559148384387137,
+            "timestamp": 2.0736826057758324
+        },
+        {
+            "x": 6.2487782508806635,
+            "y": 4.1255186451287145,
+            "heading": -1.9962477740611584,
+            "angularVelocity": 4.301411404441215,
+            "velocityX": 2.133849231946881,
+            "velocityY": -0.4416758838633167,
+            "timestamp": 2.1055561492080592
+        },
+        {
+            "x": 6.318999325259612,
+            "y": 4.108403885933248,
+            "heading": -1.8570700146176022,
+            "angularVelocity": 4.366560616000945,
+            "velocityX": 2.2031147722330044,
+            "velocityY": -0.5369581587895442,
+            "timestamp": 2.137429692640286
+        },
+        {
+            "x": 6.393124517437766,
+            "y": 4.09868995620756,
+            "heading": -1.7226225348946564,
+            "angularVelocity": 4.218152901914545,
+            "velocityX": 2.3256024964958177,
+            "velocityY": -0.3047646630925519,
+            "timestamp": 2.169303236072513
+        },
+        {
+            "x": 6.468812945370671,
+            "y": 4.097296283093324,
+            "heading": -1.5892075925845424,
+            "angularVelocity": 4.185758090994726,
+            "velocityX": 2.3746474280101166,
+            "velocityY": -0.04372507616542974,
+            "timestamp": 2.2011767795047397
+        },
+        {
+            "x": 6.543776258333572,
+            "y": 4.104341851159788,
+            "heading": -1.4561024343589095,
+            "angularVelocity": 4.176038930489708,
+            "velocityX": 2.351897683490888,
+            "velocityY": 0.22104753057800555,
+            "timestamp": 2.2330503229369665
+        },
+        {
+            "x": 6.614153245906366,
+            "y": 4.118263151934315,
+            "heading": -1.3162963129525131,
+            "angularVelocity": 4.386274833347886,
+            "velocityX": 2.208006390078291,
+            "velocityY": 0.43676664956088845,
+            "timestamp": 2.2649238663691933
+        },
+        {
+            "x": 6.676725935567768,
+            "y": 4.132111873679476,
+            "heading": -1.164406226014857,
+            "angularVelocity": 4.765396958785676,
+            "velocityX": 1.9631544824770328,
+            "velocityY": 0.4344895563497056,
+            "timestamp": 2.29679740980142
+        },
+        {
+            "x": 6.732923820729643,
+            "y": 4.140964584087909,
+            "heading": -1.0033332436559739,
+            "angularVelocity": 5.053500960800803,
+            "velocityX": 1.7631514764390552,
+            "velocityY": 0.27774478313828127,
+            "timestamp": 2.328670953233647
+        },
+        {
+            "x": 6.785034236650818,
+            "y": 4.142631098888223,
+            "heading": -0.8355937264762496,
+            "angularVelocity": 5.262656708890626,
+            "velocityX": 1.6349112872241036,
+            "velocityY": 0.0522852065023047,
+            "timestamp": 2.3605444966658737
+        },
+        {
+            "x": 6.835668705020557,
+            "y": 4.136059975961784,
+            "heading": -0.6631765444972689,
+            "angularVelocity": 5.409413683345085,
+            "velocityX": 1.5886049342899025,
+            "velocityY": -0.2061622969661811,
+            "timestamp": 2.3924180400981006
+        },
+        {
+            "x": 6.8889189304898775,
+            "y": 4.121434248751472,
+            "heading": -0.4913644813841438,
+            "angularVelocity": 5.390428694520651,
+            "velocityX": 1.6706716522606933,
+            "velocityY": -0.45886731236551676,
+            "timestamp": 2.4242915835303274
+        },
+        {
+            "x": 6.949941444507073,
+            "y": 4.105368374325079,
+            "heading": -0.330980540513716,
+            "angularVelocity": 5.031882985067382,
+            "velocityX": 1.9145192986449624,
+            "velocityY": -0.5040504661979253,
+            "timestamp": 2.456165126962554
+        },
+        {
+            "x": 7.016508340627581,
+            "y": 4.095339734393401,
+            "heading": -0.17821539872753786,
+            "angularVelocity": 4.792850914458417,
+            "velocityX": 2.0884686467963878,
+            "velocityY": -0.31463837564850267,
+            "timestamp": 2.488038670394781
+        },
+        {
+            "x": 7.085360516906702,
+            "y": 4.093448303803928,
+            "heading": -0.02746406984555507,
+            "angularVelocity": 4.7296695832557045,
+            "velocityX": 2.1601669869408497,
+            "velocityY": -0.05934171057873482,
+            "timestamp": 2.519912213827008
+        },
+        {
+            "x": 7.153846633279139,
+            "y": 4.100009116269512,
+            "heading": 0.121772089508303,
+            "angularVelocity": 4.682132680703696,
+            "velocityX": 2.1486822297640367,
+            "velocityY": 0.20583881674575163,
+            "timestamp": 2.5517857572592346
+        },
+        {
+            "x": 7.2178667451999,
+            "y": 4.113588367220929,
+            "heading": 0.27676556654047163,
+            "angularVelocity": 4.862762665899784,
+            "velocityX": 2.008565883391336,
+            "velocityY": 0.4260351843305824,
+            "timestamp": 2.5836593006914614
+        },
+        {
+            "x": 7.274040071387795,
+            "y": 4.126189441281968,
+            "heading": 0.44297350868055696,
+            "angularVelocity": 5.214605100104267,
+            "velocityX": 1.7623809636144747,
+            "velocityY": 0.3953458795013916,
+            "timestamp": 2.6155328441236882
+        },
+        {
+            "x": 7.324153742240104,
+            "y": 4.133335840122352,
+            "heading": 0.617641408977622,
+            "angularVelocity": 5.480027680902935,
+            "velocityX": 1.5722654419916315,
+            "velocityY": 0.22421099353388252,
+            "timestamp": 2.647406387555915
+        },
+        {
+            "x": 7.370062418422902,
+            "y": 4.133468677579737,
+            "heading": 0.8003841200365157,
+            "angularVelocity": 5.733366653991959,
+            "velocityX": 1.440338012007226,
+            "velocityY": 0.00416764008895702,
+            "timestamp": 2.679279930988142
+        },
+        {
+            "x": 7.413378960747577,
+            "y": 4.125978597308371,
+            "heading": 0.9928213817572759,
+            "angularVelocity": 6.037523318671553,
+            "velocityX": 1.3590124492050755,
+            "velocityY": -0.2349936488013014,
+            "timestamp": 2.7111534744203687
+        },
+        {
+            "x": 7.454276397546973,
+            "y": 4.11151825545799,
+            "heading": 1.2015436063625509,
+            "angularVelocity": 6.548447462362883,
+            "velocityX": 1.2831154743229476,
+            "velocityY": -0.4536785149454069,
+            "timestamp": 2.7430270178525955
+        },
+        {
+            "x": 7.496754891759898,
+            "y": 4.105364802951648,
+            "heading": 1.412047118709339,
+            "angularVelocity": 6.604333553135948,
+            "velocityX": 1.3327195422513116,
+            "velocityY": -0.19305831243477098,
+            "timestamp": 2.7749005612848223
+        },
+        {
+            "x": 7.53888503669894,
+            "y": 4.1076484340015265,
+            "heading": 1.6249494045277855,
+            "angularVelocity": 6.679592630519619,
+            "velocityX": 1.3217904381614993,
+            "velocityY": 0.07164660103553838,
+            "timestamp": 2.806774104717049
+        },
+        {
+            "x": 7.576001000454075,
+            "y": 4.116500503183424,
+            "heading": 1.8445622032463762,
+            "angularVelocity": 6.890128146108359,
+            "velocityX": 1.1644756044791265,
+            "velocityY": 0.277724665308084,
+            "timestamp": 2.838647648149276
+        },
+        {
+            "x": 7.60515186851165,
+            "y": 4.124308875688622,
+            "heading": 2.0739714188941045,
+            "angularVelocity": 7.1974807612943055,
+            "velocityX": 0.9145788299176564,
+            "velocityY": 0.24497974383681875,
+            "timestamp": 2.8705211915815028
+        },
+        {
+            "x": 7.62773108442525,
+            "y": 4.127593662867362,
+            "heading": 2.3140115714104335,
+            "angularVelocity": 7.53101559061771,
+            "velocityX": 0.708399929289633,
+            "velocityY": 0.10305685609521825,
+            "timestamp": 2.9023947350137296
+        },
+        {
+            "x": 7.642808233685486,
+            "y": 4.127168273515703,
+            "heading": 2.549384793893863,
+            "angularVelocity": 7.384595408537156,
+            "velocityX": 0.473030219946994,
+            "velocityY": -0.013346158156635379,
+            "timestamp": 2.9342682784459564
+        },
+        {
+            "x": 7.655297430086512,
+            "y": 4.12694395453313,
+            "heading": 2.7459505912038105,
+            "angularVelocity": 6.167051922793274,
+            "velocityX": 0.39183583173240866,
+            "velocityY": -0.007037779876922506,
+            "timestamp": 2.966141821878183
+        },
+        {
+            "x": 7.665239437445753,
+            "y": 4.126897699920467,
+            "heading": 2.903400180799407,
+            "angularVelocity": 4.939820698956328,
+            "velocityX": 0.31192036682023955,
+            "velocityY": -0.001451191417126478,
+            "timestamp": 2.99801536531041
+        },
+        {
+            "x": 7.672649475189336,
+            "y": 4.12693483442264,
+            "heading": 3.0216231319761504,
+            "angularVelocity": 3.7091248241076054,
+            "velocityX": 0.23248239591994224,
+            "velocityY": 0.001165057228464201,
+            "timestamp": 3.029888908742637
+        },
+        {
+            "x": 7.677558550585934,
+            "y": 4.1269805608298435,
+            "heading": 3.1005189939079423,
+            "angularVelocity": 2.4752774067793823,
+            "velocityX": 0.15401724653037796,
+            "velocityY": 0.0014346195082003044,
+            "timestamp": 3.0617624521748636
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 1.238676401825463,
+            "velocityX": 0.07659799166218657,
+            "velocityY": 0.0006098841880359634,
+            "timestamp": 3.0936359956070905
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": -2.349124968959552e-31,
+            "velocityX": 6.522838948908763e-32,
+            "velocityY": -6.176555916296272e-32,
+            "timestamp": 3.1255095390393173
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.traj
new file mode 100644
index 00000000..af794028
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.traj	
@@ -0,0 +1,868 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": -5.720774693888146e-34,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3121576423431627,
+            "y": 4.3971322791414,
+            "heading": 2.7106310928070285,
+            "angularVelocity": -0.5647989987388474,
+            "velocityX": 0.1744174454420521,
+            "velocityY": 0.5327116136893348,
+            "timestamp": 0.06970427936465769
+        },
+        {
+            "x": 2.3388751368329244,
+            "y": 4.470343590887415,
+            "heading": 2.629622228062893,
+            "angularVelocity": -1.1621792160039144,
+            "velocityX": 0.38329776497637447,
+            "velocityY": 1.0503130139687884,
+            "timestamp": 0.13940855872931537
+        },
+        {
+            "x": 2.3840539747737246,
+            "y": 4.577503337755321,
+            "heading": 2.5035786127839184,
+            "angularVelocity": -1.8082622247563562,
+            "velocityX": 0.6481501329989676,
+            "velocityY": 1.5373481778256302,
+            "timestamp": 0.20911283809397307
+        },
+        {
+            "x": 2.4545399331182174,
+            "y": 4.713290189810374,
+            "heading": 2.3271901249556994,
+            "angularVelocity": -2.5305259510028497,
+            "velocityX": 1.011214217935542,
+            "velocityY": 1.948041831760208,
+            "timestamp": 0.27881711745863075
+        },
+        {
+            "x": 2.555797664071962,
+            "y": 4.87530128954077,
+            "heading": 2.141593101929859,
+            "angularVelocity": -2.662634557268572,
+            "velocityX": 1.4526759601661592,
+            "velocityY": 2.3242633193700435,
+            "timestamp": 0.3485213968232884
+        },
+        {
+            "x": 2.6889975632138237,
+            "y": 5.05237912689222,
+            "heading": 2.0338949096526036,
+            "angularVelocity": -1.545072888765306,
+            "velocityX": 1.910928573624968,
+            "velocityY": 2.54041558087227,
+            "timestamp": 0.4182256761879461
+        },
+        {
+            "x": 2.8587006072828576,
+            "y": 5.214124992637136,
+            "heading": 1.956646376370765,
+            "angularVelocity": -1.108232291990471,
+            "velocityX": 2.4346144256256212,
+            "velocityY": 2.3204581873480494,
+            "timestamp": 0.48792995555260377
+        },
+        {
+            "x": 3.035891081202655,
+            "y": 5.338211520127435,
+            "heading": 1.8312106553523118,
+            "angularVelocity": -1.7995411782716633,
+            "velocityX": 2.5420315012917163,
+            "velocityY": 1.7801852141837777,
+            "timestamp": 0.5576342349172615
+        },
+        {
+            "x": 3.2127166120567576,
+            "y": 5.42406293198425,
+            "heading": 1.6557539162164756,
+            "angularVelocity": -2.5171587847273322,
+            "velocityX": 2.536795910750906,
+            "velocityY": 1.231651953643812,
+            "timestamp": 0.6273385142819192
+        },
+        {
+            "x": 3.3872389413542425,
+            "y": 5.4708692312967075,
+            "heading": 1.441472502737752,
+            "angularVelocity": -3.0741500440412146,
+            "velocityX": 2.503753440796252,
+            "velocityY": 0.6714982170261123,
+            "timestamp": 0.697042793646577
+        },
+        {
+            "x": 3.558790922164917,
+            "y": 5.477728366851807,
+            "heading": 1.205442867585498,
+            "angularVelocity": -3.386157023695285,
+            "velocityX": 2.461139866509503,
+            "velocityY": 0.09840336371911824,
+            "timestamp": 0.7667470730112347
+        },
+        {
+            "x": 3.6181705627536003,
+            "y": 5.4752230037777725,
+            "heading": 1.1222490393160696,
+            "angularVelocity": -3.4253809174942864,
+            "velocityX": 2.44486750990022,
+            "velocityY": -0.10315456138644599,
+            "timestamp": 0.7910345406749146
+        },
+        {
+            "x": 3.6770128805090945,
+            "y": 5.467829320967843,
+            "heading": 1.0387507075885118,
+            "angularVelocity": -3.4379183900025705,
+            "velocityX": 2.4227440493307753,
+            "velocityY": -0.3044237839989221,
+            "timestamp": 0.8153220083385946
+        },
+        {
+            "x": 3.735181283810321,
+            "y": 5.455564580406075,
+            "heading": 0.9556144237478981,
+            "angularVelocity": -3.4230116120725995,
+            "velocityX": 2.394996633931211,
+            "velocityY": -0.5049822703463023,
+            "timestamp": 0.8396094760022745
+        },
+        {
+            "x": 3.7925490950380945,
+            "y": 5.438455755513165,
+            "heading": 0.8735217241735449,
+            "angularVelocity": -3.380043597427691,
+            "velocityX": 2.362033457838118,
+            "velocityY": -0.7044301666120085,
+            "timestamp": 0.8638969436659545
+        },
+        {
+            "x": 3.8490034281899477,
+            "y": 5.416538531692629,
+            "heading": 0.7931634356424634,
+            "angularVelocity": -3.308631827897467,
+            "velocityX": 2.3244223701541378,
+            "velocityY": -0.902408769989304,
+            "timestamp": 0.8881844113296344
+        },
+        {
+            "x": 3.9044487728906376,
+            "y": 5.389855903636327,
+            "heading": 0.7152329727062241,
+            "angularVelocity": -3.2086697557513846,
+            "velocityX": 2.282878786231156,
+            "velocityY": -1.0986171315094786,
+            "timestamp": 0.9124718789933144
+        },
+        {
+            "x": 3.958810912211869,
+            "y": 5.358456406059338,
+            "heading": 0.6404209450438546,
+            "angularVelocity": -3.080272867403366,
+            "velocityX": 2.2382794317633223,
+            "velocityY": -1.2928271490380632,
+            "timestamp": 0.9367593466569943
+        },
+        {
+            "x": 4.012042256408639,
+            "y": 5.322391998548557,
+            "heading": 0.5694141014307214,
+            "angularVelocity": -2.923600129762354,
+            "velocityX": 2.1917206410273016,
+            "velocityY": -1.4848978086221774,
+            "timestamp": 0.9610468143206743
+        },
+        {
+            "x": 4.064130365340956,
+            "y": 5.281715631109981,
+            "heading": 0.5029022798709881,
+            "angularVelocity": -2.738524348472814,
+            "velocityX": 2.1446496462128213,
+            "velocityY": -1.6747883312431175,
+            "timestamp": 0.9853342819843542
+        },
+        {
+            "x": 4.1151123432623455,
+            "y": 5.236478617606072,
+            "heading": 0.44159733021441666,
+            "angularVelocity": -2.524139218855187,
+            "velocityX": 2.09910636330477,
+            "velocityY": -1.8625660826533157,
+            "timestamp": 1.0096217496480342
+        },
+        {
+            "x": 4.165095928896697,
+            "y": 5.186728422848365,
+            "heading": 0.38626165615570274,
+            "angularVelocity": -2.278363262278863,
+            "velocityX": 2.057999060523636,
+            "velocityY": -2.0483895417431537,
+            "timestamp": 1.0339092173117141
+        },
+        {
+            "x": 4.214269146284126,
+            "y": 5.132508462649398,
+            "heading": 0.33770705635311316,
+            "angularVelocity": -1.9991627153126146,
+            "velocityX": 2.0246333651722517,
+            "velocityY": -2.232425419964549,
+            "timestamp": 1.058196684975394
+        },
+        {
+            "x": 4.262857394718322,
+            "y": 5.073859999949952,
+            "heading": 0.2966997330272638,
+            "angularVelocity": -1.6884149427884858,
+            "velocityX": 2.000548147177036,
+            "velocityY": -2.4147623585784754,
+            "timestamp": 1.082484152639074
+        },
+        {
+            "x": 4.311032470035773,
+            "y": 5.010821065356263,
+            "heading": 0.26381895203232264,
+            "angularVelocity": -1.353816768806731,
+            "velocityX": 1.9835363647027058,
+            "velocityY": -2.59553344410457,
+            "timestamp": 1.106771620302754
+        },
+        {
+            "x": 4.358621953046075,
+            "y": 4.943414538605821,
+            "heading": 0.23906687218938943,
+            "angularVelocity": -1.019129708608845,
+            "velocityX": 1.9594254810463076,
+            "velocityY": -2.77536249080602,
+            "timestamp": 1.131059087966434
+        },
+        {
+            "x": 4.401785346523604,
+            "y": 4.873960680394997,
+            "heading": 0.21204412295947933,
+            "angularVelocity": -1.1126211099529368,
+            "velocityX": 1.7771878927533025,
+            "velocityY": -2.8596582884878865,
+            "timestamp": 1.155346555630114
+        },
+        {
+            "x": 4.442527933324705,
+            "y": 4.807784869243466,
+            "heading": 0.1747999499890042,
+            "angularVelocity": -1.5334728793554109,
+            "velocityX": 1.6775148140301646,
+            "velocityY": -2.7246896246203156,
+            "timestamp": 1.1796340232937939
+        },
+        {
+            "x": 4.4813722294916225,
+            "y": 4.745319922619003,
+            "heading": 0.12759770745686402,
+            "angularVelocity": -1.9434814360134838,
+            "velocityX": 1.5993555484998627,
+            "velocityY": -2.571900351631775,
+            "timestamp": 1.2039214909574738
+        },
+        {
+            "x": 4.518612325734112,
+            "y": 4.686765196550011,
+            "heading": 0.07078885768669944,
+            "angularVelocity": -2.3390190594105262,
+            "velocityX": 1.5333050262040608,
+            "velocityY": -2.410902893616876,
+            "timestamp": 1.2282089586211538
+        },
+        {
+            "x": 4.554475342160425,
+            "y": 4.632242142038935,
+            "heading": 0.004632855038261056,
+            "angularVelocity": -2.723874039258911,
+            "velocityX": 1.47660583321928,
+            "velocityY": -2.244904873002102,
+            "timestamp": 1.2524964262848337
+        },
+        {
+            "x": 4.589156761961767,
+            "y": 4.581819399142938,
+            "heading": -0.07076666263842435,
+            "angularVelocity": -3.1044618863019378,
+            "velocityX": 1.4279553670061842,
+            "velocityY": -2.076080701134665,
+            "timestamp": 1.2767838939485137
+        },
+        {
+            "x": 4.6228372561227475,
+            "y": 4.535518314545829,
+            "heading": -0.1555015238515939,
+            "angularVelocity": -3.488830634240387,
+            "velocityX": 1.3867437572072365,
+            "velocityY": -1.9063776116251314,
+            "timestamp": 1.3010713616121936
+        },
+        {
+            "x": 4.654626762222228,
+            "y": 4.492711334026638,
+            "heading": -0.25016165034544124,
+            "angularVelocity": -3.8974885239025583,
+            "velocityX": 1.3088851641383707,
+            "velocityY": -1.762513124544663,
+            "timestamp": 1.3253588292758736
+        },
+        {
+            "x": 4.685559266058307,
+            "y": 4.453815281857176,
+            "heading": -0.35503604485130436,
+            "angularVelocity": -4.318045666930297,
+            "velocityX": 1.2735993832052854,
+            "velocityY": -1.6014865241643912,
+            "timestamp": 1.3496462969395535
+        },
+        {
+            "x": 4.716254412162635,
+            "y": 4.418834840133045,
+            "heading": -0.4707020428721612,
+            "angularVelocity": -4.762373732104905,
+            "velocityX": 1.2638265351241147,
+            "velocityY": -1.440267145530449,
+            "timestamp": 1.3739337646032335
+        },
+        {
+            "x": 4.747074366156251,
+            "y": 4.387541660715592,
+            "heading": -0.5981615568154537,
+            "angularVelocity": -5.247954035730924,
+            "velocityX": 1.268965312497503,
+            "velocityY": -1.2884496585146104,
+            "timestamp": 1.3982212322669134
+        },
+        {
+            "x": 4.7760218641280225,
+            "y": 4.359163482704458,
+            "heading": -0.7400182505648043,
+            "angularVelocity": -5.840736288925144,
+            "velocityX": 1.191869748325327,
+            "velocityY": -1.1684288540946337,
+            "timestamp": 1.4225086999305934
+        },
+        {
+            "x": 4.800681197551656,
+            "y": 4.328735146997818,
+            "heading": -0.8864907082583147,
+            "angularVelocity": -6.030783436206011,
+            "velocityX": 1.015311014104143,
+            "velocityY": -1.2528410177625973,
+            "timestamp": 1.4467961675942733
+        },
+        {
+            "x": 4.821257943768876,
+            "y": 4.295565486099031,
+            "heading": -1.0326334280053908,
+            "angularVelocity": -6.017206971545368,
+            "velocityX": 0.8472166181404436,
+            "velocityY": -1.3657109649348727,
+            "timestamp": 1.4710836352579533
+        },
+        {
+            "x": 4.842407281192705,
+            "y": 4.263513715444916,
+            "heading": -1.1735548575556736,
+            "angularVelocity": -5.802228190344444,
+            "velocityX": 0.8707922010105955,
+            "velocityY": -1.319683513240253,
+            "timestamp": 1.4953711029216332
+        },
+        {
+            "x": 4.867138629271623,
+            "y": 4.234341103924059,
+            "heading": -1.3080892458596534,
+            "angularVelocity": -5.539251360699312,
+            "velocityX": 1.0182761093657138,
+            "velocityY": -1.2011384605766136,
+            "timestamp": 1.5196585705853132
+        },
+        {
+            "x": 4.89576478236693,
+            "y": 4.207846540583478,
+            "heading": -1.4377333776779964,
+            "angularVelocity": -5.337902395325306,
+            "velocityX": 1.1786388557138734,
+            "velocityY": -1.0908738493227768,
+            "timestamp": 1.5439460382489931
+        },
+        {
+            "x": 4.928534232838641,
+            "y": 4.183793595288781,
+            "heading": -1.5636936940682342,
+            "angularVelocity": -5.1862268283573245,
+            "velocityX": 1.3492329017369804,
+            "velocityY": -0.9903438937220402,
+            "timestamp": 1.568233505912673
+        },
+        {
+            "x": 4.965642273050373,
+            "y": 4.1619364014492675,
+            "heading": -1.6870176099857392,
+            "angularVelocity": -5.077676998904523,
+            "velocityX": 1.5278678174928029,
+            "velocityY": -0.8999371256888635,
+            "timestamp": 1.592520973576353
+        },
+        {
+            "x": 5.007242132688007,
+            "y": 4.142028453113074,
+            "heading": -1.8086864696957439,
+            "angularVelocity": -5.009532545542041,
+            "velocityX": 1.712811735405568,
+            "velocityY": -0.8196798699587677,
+            "timestamp": 1.616808441240033
+        },
+        {
+            "x": 5.0534540304279405,
+            "y": 4.123819388018015,
+            "heading": -1.9296932957732547,
+            "angularVelocity": -4.982274305133403,
+            "velocityX": 1.902705476744277,
+            "velocityY": -0.749730904316916,
+            "timestamp": 1.641095908903713
+        },
+        {
+            "x": 5.102543724383187,
+            "y": 4.108450898288225,
+            "heading": -2.039853637710442,
+            "angularVelocity": -4.535686612644417,
+            "velocityX": 2.0211944133087334,
+            "velocityY": -0.6327744803453944,
+            "timestamp": 1.6653833765673929
+        },
+        {
+            "x": 5.15444764406018,
+            "y": 4.096180097409251,
+            "heading": -2.1398530735226235,
+            "angularVelocity": -4.117326565161934,
+            "velocityX": 2.1370659302868042,
+            "velocityY": -0.5052317948043931,
+            "timestamp": 1.6896708442310728
+        },
+        {
+            "x": 5.20922556245635,
+            "y": 4.087205112082737,
+            "heading": -2.231114838342211,
+            "angularVelocity": -3.757566086482643,
+            "velocityX": 2.2553985106519345,
+            "velocityY": -0.36953153991984866,
+            "timestamp": 1.7139583118947528
+        },
+        {
+            "x": 5.266890046930524,
+            "y": 4.081668558801882,
+            "heading": -2.314693981916533,
+            "angularVelocity": -3.4412456964094225,
+            "velocityX": 2.3742485331398995,
+            "velocityY": -0.22795926514540626,
+            "timestamp": 1.7382457795584327
+        },
+        {
+            "x": 5.327424168986788,
+            "y": 4.079676285408984,
+            "heading": -2.3913241555270592,
+            "angularVelocity": -3.1551322958679826,
+            "velocityX": 2.492401550236051,
+            "velocityY": -0.08202886445327343,
+            "timestamp": 1.7625332472221127
+        },
+        {
+            "x": 5.390789031982422,
+            "y": 4.081310272216797,
+            "heading": -2.461469590575517,
+            "angularVelocity": -2.888132925992722,
+            "velocityX": 2.608953056493045,
+            "velocityY": 0.06727695247771216,
+            "timestamp": 1.7868207148857926
+        },
+        {
+            "x": 5.47852793656002,
+            "y": 4.0897392429132315,
+            "heading": -2.541079365075053,
+            "angularVelocity": -2.4976756873239485,
+            "velocityX": 2.752718873700339,
+            "velocityY": 0.2644503807478311,
+            "timestamp": 1.8186942583180192
+        },
+        {
+            "x": 5.570818505990881,
+            "y": 4.101985641066581,
+            "heading": -2.595195903422616,
+            "angularVelocity": -1.6978513375091853,
+            "velocityX": 2.8955227280299005,
+            "velocityY": 0.384218283711974,
+            "timestamp": 1.8505678017502458
+        },
+        {
+            "x": 5.666172128712358,
+            "y": 4.1161647975520586,
+            "heading": -2.6133567142303673,
+            "angularVelocity": -0.5697769639691046,
+            "velocityX": 2.9916229089567223,
+            "velocityY": 0.4448566101734819,
+            "timestamp": 1.8824413451824724
+        },
+        {
+            "x": 5.762848661651843,
+            "y": 4.1299008615103485,
+            "heading": -2.5899687129898252,
+            "angularVelocity": 0.7337747461392797,
+            "velocityX": 3.033127871240633,
+            "velocityY": 0.43095503289420706,
+            "timestamp": 1.914314888614699
+        },
+        {
+            "x": 5.85803719097752,
+            "y": 4.141633583817035,
+            "heading": -2.5274756720169904,
+            "angularVelocity": 1.960655585900415,
+            "velocityX": 2.9864432716142737,
+            "velocityY": 0.36810222658906844,
+            "timestamp": 1.9461884320469256
+        },
+        {
+            "x": 5.9473700887935115,
+            "y": 4.148469896281193,
+            "heading": -2.4508224753320094,
+            "angularVelocity": 2.404916066140258,
+            "velocityX": 2.8027287899740645,
+            "velocityY": 0.21448234893285298,
+            "timestamp": 1.9780619754791522
+        },
+        {
+            "x": 6.0307522620060645,
+            "y": 4.150565536032173,
+            "heading": -2.3598343948212093,
+            "angularVelocity": 2.854658463194389,
+            "velocityX": 2.6160308592563855,
+            "velocityY": 0.06574856527754983,
+            "timestamp": 2.009935518911379
+        },
+        {
+            "x": 6.108380396897077,
+            "y": 4.1477533333139265,
+            "heading": -2.2542021077222087,
+            "angularVelocity": 3.3141055472419727,
+            "velocityX": 2.4355037605428165,
+            "velocityY": -0.0882299994108409,
+            "timestamp": 2.0418090623436056
+        },
+        {
+            "x": 6.180764914708381,
+            "y": 4.139596420595998,
+            "heading": -2.1333489972804904,
+            "angularVelocity": 3.7916433953661524,
+            "velocityX": 2.270990609036518,
+            "velocityY": -0.2559148384387137,
+            "timestamp": 2.0736826057758324
+        },
+        {
+            "x": 6.2487782508806635,
+            "y": 4.1255186451287145,
+            "heading": -1.9962477740611584,
+            "angularVelocity": 4.301411404441215,
+            "velocityX": 2.133849231946881,
+            "velocityY": -0.4416758838633167,
+            "timestamp": 2.1055561492080592
+        },
+        {
+            "x": 6.318999325259612,
+            "y": 4.108403885933248,
+            "heading": -1.8570700146176022,
+            "angularVelocity": 4.366560616000945,
+            "velocityX": 2.2031147722330044,
+            "velocityY": -0.5369581587895442,
+            "timestamp": 2.137429692640286
+        },
+        {
+            "x": 6.393124517437766,
+            "y": 4.09868995620756,
+            "heading": -1.7226225348946564,
+            "angularVelocity": 4.218152901914545,
+            "velocityX": 2.3256024964958177,
+            "velocityY": -0.3047646630925519,
+            "timestamp": 2.169303236072513
+        },
+        {
+            "x": 6.468812945370671,
+            "y": 4.097296283093324,
+            "heading": -1.5892075925845424,
+            "angularVelocity": 4.185758090994726,
+            "velocityX": 2.3746474280101166,
+            "velocityY": -0.04372507616542974,
+            "timestamp": 2.2011767795047397
+        },
+        {
+            "x": 6.543776258333572,
+            "y": 4.104341851159788,
+            "heading": -1.4561024343589095,
+            "angularVelocity": 4.176038930489708,
+            "velocityX": 2.351897683490888,
+            "velocityY": 0.22104753057800555,
+            "timestamp": 2.2330503229369665
+        },
+        {
+            "x": 6.614153245906366,
+            "y": 4.118263151934315,
+            "heading": -1.3162963129525131,
+            "angularVelocity": 4.386274833347886,
+            "velocityX": 2.208006390078291,
+            "velocityY": 0.43676664956088845,
+            "timestamp": 2.2649238663691933
+        },
+        {
+            "x": 6.676725935567768,
+            "y": 4.132111873679476,
+            "heading": -1.164406226014857,
+            "angularVelocity": 4.765396958785676,
+            "velocityX": 1.9631544824770328,
+            "velocityY": 0.4344895563497056,
+            "timestamp": 2.29679740980142
+        },
+        {
+            "x": 6.732923820729643,
+            "y": 4.140964584087909,
+            "heading": -1.0033332436559739,
+            "angularVelocity": 5.053500960800803,
+            "velocityX": 1.7631514764390552,
+            "velocityY": 0.27774478313828127,
+            "timestamp": 2.328670953233647
+        },
+        {
+            "x": 6.785034236650818,
+            "y": 4.142631098888223,
+            "heading": -0.8355937264762496,
+            "angularVelocity": 5.262656708890626,
+            "velocityX": 1.6349112872241036,
+            "velocityY": 0.0522852065023047,
+            "timestamp": 2.3605444966658737
+        },
+        {
+            "x": 6.835668705020557,
+            "y": 4.136059975961784,
+            "heading": -0.6631765444972689,
+            "angularVelocity": 5.409413683345085,
+            "velocityX": 1.5886049342899025,
+            "velocityY": -0.2061622969661811,
+            "timestamp": 2.3924180400981006
+        },
+        {
+            "x": 6.8889189304898775,
+            "y": 4.121434248751472,
+            "heading": -0.4913644813841438,
+            "angularVelocity": 5.390428694520651,
+            "velocityX": 1.6706716522606933,
+            "velocityY": -0.45886731236551676,
+            "timestamp": 2.4242915835303274
+        },
+        {
+            "x": 6.949941444507073,
+            "y": 4.105368374325079,
+            "heading": -0.330980540513716,
+            "angularVelocity": 5.031882985067382,
+            "velocityX": 1.9145192986449624,
+            "velocityY": -0.5040504661979253,
+            "timestamp": 2.456165126962554
+        },
+        {
+            "x": 7.016508340627581,
+            "y": 4.095339734393401,
+            "heading": -0.17821539872753786,
+            "angularVelocity": 4.792850914458417,
+            "velocityX": 2.0884686467963878,
+            "velocityY": -0.31463837564850267,
+            "timestamp": 2.488038670394781
+        },
+        {
+            "x": 7.085360516906702,
+            "y": 4.093448303803928,
+            "heading": -0.02746406984555507,
+            "angularVelocity": 4.7296695832557045,
+            "velocityX": 2.1601669869408497,
+            "velocityY": -0.05934171057873482,
+            "timestamp": 2.519912213827008
+        },
+        {
+            "x": 7.153846633279139,
+            "y": 4.100009116269512,
+            "heading": 0.121772089508303,
+            "angularVelocity": 4.682132680703696,
+            "velocityX": 2.1486822297640367,
+            "velocityY": 0.20583881674575163,
+            "timestamp": 2.5517857572592346
+        },
+        {
+            "x": 7.2178667451999,
+            "y": 4.113588367220929,
+            "heading": 0.27676556654047163,
+            "angularVelocity": 4.862762665899784,
+            "velocityX": 2.008565883391336,
+            "velocityY": 0.4260351843305824,
+            "timestamp": 2.5836593006914614
+        },
+        {
+            "x": 7.274040071387795,
+            "y": 4.126189441281968,
+            "heading": 0.44297350868055696,
+            "angularVelocity": 5.214605100104267,
+            "velocityX": 1.7623809636144747,
+            "velocityY": 0.3953458795013916,
+            "timestamp": 2.6155328441236882
+        },
+        {
+            "x": 7.324153742240104,
+            "y": 4.133335840122352,
+            "heading": 0.617641408977622,
+            "angularVelocity": 5.480027680902935,
+            "velocityX": 1.5722654419916315,
+            "velocityY": 0.22421099353388252,
+            "timestamp": 2.647406387555915
+        },
+        {
+            "x": 7.370062418422902,
+            "y": 4.133468677579737,
+            "heading": 0.8003841200365157,
+            "angularVelocity": 5.733366653991959,
+            "velocityX": 1.440338012007226,
+            "velocityY": 0.00416764008895702,
+            "timestamp": 2.679279930988142
+        },
+        {
+            "x": 7.413378960747577,
+            "y": 4.125978597308371,
+            "heading": 0.9928213817572759,
+            "angularVelocity": 6.037523318671553,
+            "velocityX": 1.3590124492050755,
+            "velocityY": -0.2349936488013014,
+            "timestamp": 2.7111534744203687
+        },
+        {
+            "x": 7.454276397546973,
+            "y": 4.11151825545799,
+            "heading": 1.2015436063625509,
+            "angularVelocity": 6.548447462362883,
+            "velocityX": 1.2831154743229476,
+            "velocityY": -0.4536785149454069,
+            "timestamp": 2.7430270178525955
+        },
+        {
+            "x": 7.496754891759898,
+            "y": 4.105364802951648,
+            "heading": 1.412047118709339,
+            "angularVelocity": 6.604333553135948,
+            "velocityX": 1.3327195422513116,
+            "velocityY": -0.19305831243477098,
+            "timestamp": 2.7749005612848223
+        },
+        {
+            "x": 7.53888503669894,
+            "y": 4.1076484340015265,
+            "heading": 1.6249494045277855,
+            "angularVelocity": 6.679592630519619,
+            "velocityX": 1.3217904381614993,
+            "velocityY": 0.07164660103553838,
+            "timestamp": 2.806774104717049
+        },
+        {
+            "x": 7.576001000454075,
+            "y": 4.116500503183424,
+            "heading": 1.8445622032463762,
+            "angularVelocity": 6.890128146108359,
+            "velocityX": 1.1644756044791265,
+            "velocityY": 0.277724665308084,
+            "timestamp": 2.838647648149276
+        },
+        {
+            "x": 7.60515186851165,
+            "y": 4.124308875688622,
+            "heading": 2.0739714188941045,
+            "angularVelocity": 7.1974807612943055,
+            "velocityX": 0.9145788299176564,
+            "velocityY": 0.24497974383681875,
+            "timestamp": 2.8705211915815028
+        },
+        {
+            "x": 7.62773108442525,
+            "y": 4.127593662867362,
+            "heading": 2.3140115714104335,
+            "angularVelocity": 7.53101559061771,
+            "velocityX": 0.708399929289633,
+            "velocityY": 0.10305685609521825,
+            "timestamp": 2.9023947350137296
+        },
+        {
+            "x": 7.642808233685486,
+            "y": 4.127168273515703,
+            "heading": 2.549384793893863,
+            "angularVelocity": 7.384595408537156,
+            "velocityX": 0.473030219946994,
+            "velocityY": -0.013346158156635379,
+            "timestamp": 2.9342682784459564
+        },
+        {
+            "x": 7.655297430086512,
+            "y": 4.12694395453313,
+            "heading": 2.7459505912038105,
+            "angularVelocity": 6.167051922793274,
+            "velocityX": 0.39183583173240866,
+            "velocityY": -0.007037779876922506,
+            "timestamp": 2.966141821878183
+        },
+        {
+            "x": 7.665239437445753,
+            "y": 4.126897699920467,
+            "heading": 2.903400180799407,
+            "angularVelocity": 4.939820698956328,
+            "velocityX": 0.31192036682023955,
+            "velocityY": -0.001451191417126478,
+            "timestamp": 2.99801536531041
+        },
+        {
+            "x": 7.672649475189336,
+            "y": 4.12693483442264,
+            "heading": 3.0216231319761504,
+            "angularVelocity": 3.7091248241076054,
+            "velocityX": 0.23248239591994224,
+            "velocityY": 0.001165057228464201,
+            "timestamp": 3.029888908742637
+        },
+        {
+            "x": 7.677558550585934,
+            "y": 4.1269805608298435,
+            "heading": 3.1005189939079423,
+            "angularVelocity": 2.4752774067793823,
+            "velocityX": 0.15401724653037796,
+            "velocityY": 0.0014346195082003044,
+            "timestamp": 3.0617624521748636
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": 1.238676401825463,
+            "velocityX": 0.07659799166218657,
+            "velocityY": 0.0006098841880359634,
+            "timestamp": 3.0936359956070905
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.14,
+            "angularVelocity": -2.349124968959552e-31,
+            "velocityX": 6.522838948908763e-32,
+            "velocityY": -6.176555916296272e-32,
+            "timestamp": 3.1255095390393173
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.1.traj
new file mode 100644
index 00000000..f2e9d5e3
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.1.traj	
@@ -0,0 +1,589 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.291384520253252,
+            "y": 4.354032854764245,
+            "heading": 2.74598551663602,
+            "angularVelocity": -0.1129108392120503,
+            "velocityX": -0.24231786763597,
+            "velocityY": -0.1678311541441644,
+            "timestamp": 0.03555445510807641
+        },
+        {
+            "x": 2.274618850931762,
+            "y": 4.341572242021881,
+            "heading": 2.7363109696234753,
+            "angularVelocity": -0.2721050564025352,
+            "velocityX": -0.47154904415004567,
+            "velocityY": -0.3504655803186264,
+            "timestamp": 0.07110891021615282
+        },
+        {
+            "x": 2.250443983775186,
+            "y": 4.321957452674385,
+            "heading": 2.718567267740621,
+            "angularVelocity": -0.4990570613139176,
+            "velocityX": -0.6799391829544631,
+            "velocityY": -0.5516830250350497,
+            "timestamp": 0.10666336532422922
+        },
+        {
+            "x": 2.2201183125080237,
+            "y": 4.294438762451095,
+            "heading": 2.688674932435733,
+            "angularVelocity": -0.8407479516708394,
+            "velocityX": -0.8529359028279262,
+            "velocityY": -0.7739871174973922,
+            "timestamp": 0.14221782043230563
+        },
+        {
+            "x": 2.1856076192251765,
+            "y": 4.258557396291605,
+            "heading": 2.63987728987899,
+            "angularVelocity": -1.3724761751631571,
+            "velocityX": -0.9706432900727432,
+            "velocityY": -1.009194657896466,
+            "timestamp": 0.17777227554038205
+        },
+        {
+            "x": 2.1489602975624615,
+            "y": 4.2143768004023965,
+            "heading": 2.56722210682279,
+            "angularVelocity": -2.0434902696538804,
+            "velocityX": -1.0307378231874669,
+            "velocityY": -1.2426177185084084,
+            "timestamp": 0.21332673064845847
+        },
+        {
+            "x": 2.1162123400405237,
+            "y": 4.163459966768879,
+            "heading": 2.4666019501957965,
+            "angularVelocity": -2.8300294947885973,
+            "velocityX": -0.9210648123390461,
+            "velocityY": -1.4320802689492316,
+            "timestamp": 0.2488811857565349
+        },
+        {
+            "x": 2.0883593879928255,
+            "y": 4.106985689754014,
+            "heading": 2.334003256666977,
+            "angularVelocity": -3.7294536824078297,
+            "velocityX": -0.7833885222831486,
+            "velocityY": -1.588388201793505,
+            "timestamp": 0.2844356408646113
+        },
+        {
+            "x": 2.0671900205502793,
+            "y": 4.044210359842601,
+            "heading": 2.181383778591042,
+            "angularVelocity": -4.2925556758502195,
+            "velocityX": -0.5954068872155824,
+            "velocityY": -1.765610799563443,
+            "timestamp": 0.31999009597268774
+        },
+        {
+            "x": 2.0546372297458078,
+            "y": 3.9756937078562617,
+            "heading": 2.021719977202011,
+            "angularVelocity": -4.49068340110105,
+            "velocityX": -0.35305816855621736,
+            "velocityY": -1.927090480730638,
+            "timestamp": 0.35554455108076416
+        },
+        {
+            "x": 2.0513227865299846,
+            "y": 3.902144544449752,
+            "heading": 1.8643846688260233,
+            "angularVelocity": -4.42519250816047,
+            "velocityX": -0.0932216006615165,
+            "velocityY": -2.068634245214532,
+            "timestamp": 0.3910990061888406
+        },
+        {
+            "x": 2.056389020104272,
+            "y": 3.822243076047335,
+            "heading": 1.7096006131055106,
+            "angularVelocity": -4.353436306364677,
+            "velocityX": 0.14249222942348805,
+            "velocityY": -2.247298352893811,
+            "timestamp": 0.426653461296917
+        },
+        {
+            "x": 2.0690751873782185,
+            "y": 3.7373642087091223,
+            "heading": 1.5737353715537044,
+            "angularVelocity": -3.821328189078159,
+            "velocityX": 0.35680949786416494,
+            "velocityY": -2.3872920307793386,
+            "timestamp": 0.4622079164049934
+        },
+        {
+            "x": 2.089274580938585,
+            "y": 3.648200269765939,
+            "heading": 1.460663871168444,
+            "angularVelocity": -3.1802343768608408,
+            "velocityX": 0.5681255274188619,
+            "velocityY": -2.507813399815797,
+            "timestamp": 0.49776237151306985
+        },
+        {
+            "x": 2.1169367654773477,
+            "y": 3.55409496646177,
+            "heading": 1.3686138295927854,
+            "angularVelocity": -2.5889875487010094,
+            "velocityX": 0.7780230200315752,
+            "velocityY": -2.646793573916784,
+            "timestamp": 0.5333168266211462
+        },
+        {
+            "x": 2.1519045434306308,
+            "y": 3.4545849603291003,
+            "heading": 1.296733638399589,
+            "angularVelocity": -2.0216929488779694,
+            "velocityX": 0.9834991943200934,
+            "velocityY": -2.798805545751856,
+            "timestamp": 0.5688712817292226
+        },
+        {
+            "x": 2.1939849352834964,
+            "y": 3.3493550759903967,
+            "heading": 1.244789692250972,
+            "angularVelocity": -1.4609687025358928,
+            "velocityX": 1.1835476517626813,
+            "velocityY": -2.959682099439628,
+            "timestamp": 0.604425736837299
+        },
+        {
+            "x": 2.243083202431903,
+            "y": 3.2382839213654937,
+            "heading": 1.2128218877185057,
+            "angularVelocity": -0.8991223303884791,
+            "velocityX": 1.3809315034968324,
+            "velocityY": -3.123972911053636,
+            "timestamp": 0.6399801919453755
+        },
+        {
+            "x": 2.299610340542871,
+            "y": 3.1217794185343526,
+            "heading": 1.2005607333325377,
+            "angularVelocity": -0.3448556404168553,
+            "velocityX": 1.5898749661368734,
+            "velocityY": -3.276790559073325,
+            "timestamp": 0.6755346470534519
+        },
+        {
+            "x": 2.3661237313117724,
+            "y": 3.0048767909387704,
+            "heading": 1.2005598156629014,
+            "angularVelocity": -0.000025810257353301716,
+            "velocityX": 1.8707470151551455,
+            "velocityY": -3.2879881646400775,
+            "timestamp": 0.7110891021615283
+        },
+        {
+            "x": 2.4413914156279892,
+            "y": 2.893838891515528,
+            "heading": 1.1996733189380515,
+            "angularVelocity": -0.02493349208011108,
+            "velocityX": 2.116969141769213,
+            "velocityY": -3.123037579558325,
+            "timestamp": 0.7466435572696047
+        },
+        {
+            "x": 2.522968201294396,
+            "y": 2.7909984002833275,
+            "heading": 1.191481222958301,
+            "angularVelocity": -0.2304098306344059,
+            "velocityX": 2.2944181093040097,
+            "velocityY": -2.89247833835709,
+            "timestamp": 0.7821980123776812
+        },
+        {
+            "x": 2.6104869842529297,
+            "y": 2.6966493129730225,
+            "heading": 1.1763516697745318,
+            "angularVelocity": -0.4255318535406987,
+            "velocityX": 2.4615419556423808,
+            "velocityY": -2.653650211302861,
+            "timestamp": 0.8177524674857576
+        },
+        {
+            "x": 2.7952393085826728,
+            "y": 2.5500987298885027,
+            "heading": 1.131439771707853,
+            "angularVelocity": -0.6752506092749339,
+            "velocityX": 2.777752108882206,
+            "velocityY": -2.203388740562692,
+            "timestamp": 0.884263918863645
+        },
+        {
+            "x": 2.9911597283478026,
+            "y": 2.437065890505019,
+            "heading": 1.0475109017935547,
+            "angularVelocity": -1.2618709737283185,
+            "velocityX": 2.9456644789180824,
+            "velocityY": -1.6994492984566387,
+            "timestamp": 0.9507753702415324
+        },
+        {
+            "x": 3.2086737929320703,
+            "y": 2.3383672587643702,
+            "heading": 1.0088878551520015,
+            "angularVelocity": -0.5806976970343763,
+            "velocityX": 3.2703250354356728,
+            "velocityY": -1.4839344157427674,
+            "timestamp": 1.0172868216194197
+        },
+        {
+            "x": 3.4256073991681717,
+            "y": 2.2401993070344908,
+            "heading": 0.9691349707945731,
+            "angularVelocity": -0.5976848126733959,
+            "velocityX": 3.2615978413038147,
+            "velocityY": -1.4759556391595554,
+            "timestamp": 1.083798272997307
+        },
+        {
+            "x": 3.641898030119061,
+            "y": 2.142612874932996,
+            "heading": 0.9280602960666862,
+            "angularVelocity": -0.6175579374222298,
+            "velocityX": 3.251930704714679,
+            "velocityY": -1.4672124886743745,
+            "timestamp": 1.1503097243751943
+        },
+        {
+            "x": 3.8574674811397935,
+            "y": 2.045673041019181,
+            "heading": 0.8854247472051884,
+            "angularVelocity": -0.6410256877310082,
+            "velocityX": 3.2410877609025053,
+            "velocityY": -1.4574908817287378,
+            "timestamp": 1.2168211757530816
+        },
+        {
+            "x": 4.072209689171901,
+            "y": 1.949463725749032,
+            "heading": 0.8409091268268268,
+            "angularVelocity": -0.6692925722736718,
+            "velocityX": 3.2286501584823513,
+            "velocityY": -1.446507530312815,
+            "timestamp": 1.2833326271309689
+        },
+        {
+            "x": 4.28597464141242,
+            "y": 1.8540976935596791,
+            "heading": 0.7940685595150717,
+            "angularVelocity": -0.7042481609013259,
+            "velocityX": 3.2139571128286692,
+            "velocityY": -1.4338287650276498,
+            "timestamp": 1.3498440785088561
+        },
+        {
+            "x": 4.498537652945423,
+            "y": 1.7597351759166577,
+            "heading": 0.7442459258691415,
+            "angularVelocity": -0.7490835429655718,
+            "velocityX": 3.195885928354179,
+            "velocityY": -1.4187409188666942,
+            "timestamp": 1.4163555298867434
+        },
+        {
+            "x": 4.709533812148138,
+            "y": 1.6666229170102642,
+            "heading": 0.6903878152301356,
+            "angularVelocity": -0.8097569594897124,
+            "velocityX": 3.1723282958288084,
+            "velocityY": -1.399943272585839,
+            "timestamp": 1.4828669812646307
+        },
+        {
+            "x": 4.924860666444789,
+            "y": 1.5757431293365876,
+            "heading": 0.6452055578690052,
+            "angularVelocity": -0.679315462602457,
+            "velocityX": 3.2374403179576423,
+            "velocityY": -1.3663780565746433,
+            "timestamp": 1.549378432642518
+        },
+        {
+            "x": 5.15821860461426,
+            "y": 1.5166188222705783,
+            "heading": 0.6185000496396688,
+            "angularVelocity": -0.4015174481399904,
+            "velocityX": 3.5085377530500543,
+            "velocityY": -0.8889342487820968,
+            "timestamp": 1.6158898840204052
+        },
+        {
+            "x": 5.39961745545984,
+            "y": 1.4934726786236598,
+            "heading": 0.5961273041444063,
+            "angularVelocity": -0.33637433903149927,
+            "velocityX": 3.6294329148534703,
+            "velocityY": -0.3480023840618476,
+            "timestamp": 1.6824013353982925
+        },
+        {
+            "x": 5.646764755249023,
+            "y": 1.5066063404083252,
+            "heading": 0.585736957474559,
+            "angularVelocity": -0.15621891350429284,
+            "velocityX": 3.7158608731150236,
+            "velocityY": 0.19746466980619687,
+            "timestamp": 1.7489127867761798
+        },
+        {
+            "x": 5.78386589002804,
+            "y": 1.5248142647082106,
+            "heading": 0.5861133793107512,
+            "angularVelocity": 0.010285140296820981,
+            "velocityX": 3.7460749363537476,
+            "velocityY": 0.4975031678094133,
+            "timestamp": 1.7855113965499783
+        },
+        {
+            "x": 5.9191942381460025,
+            "y": 1.554048616452813,
+            "heading": 0.586114299955562,
+            "angularVelocity": 0.00002515518530364309,
+            "velocityX": 3.69763630242715,
+            "velocityY": 0.7987831211428086,
+            "timestamp": 1.8221100063237767
+        },
+        {
+            "x": 6.0517259817313,
+            "y": 1.5940956419716419,
+            "heading": 0.5861150757223451,
+            "angularVelocity": 0.000021196618886827452,
+            "velocityX": 3.6212234400274688,
+            "velocityY": 1.0942225884082484,
+            "timestamp": 1.8587086160975752
+        },
+        {
+            "x": 6.180598624537076,
+            "y": 1.644694580267861,
+            "heading": 0.5861160659567196,
+            "angularVelocity": 0.00002705661173256264,
+            "velocityX": 3.521244211249743,
+            "velocityY": 1.3825371676397282,
+            "timestamp": 1.8953072258713737
+        },
+        {
+            "x": 6.304973627271485,
+            "y": 1.7055153509385155,
+            "heading": 0.5861182648046722,
+            "angularVelocity": 0.00006008009501406976,
+            "velocityX": 3.3983532025703247,
+            "velocityY": 1.6618328140485057,
+            "timestamp": 1.9319058356451722
+        },
+        {
+            "x": 6.427040241028168,
+            "y": 1.7708422617715522,
+            "heading": 0.5861256207319434,
+            "angularVelocity": 0.0002009892538702712,
+            "velocityX": 3.335280069683768,
+            "velocityY": 1.784956074473775,
+            "timestamp": 1.9685044454189706
+        },
+        {
+            "x": 6.5491066329086145,
+            "y": 1.8361695861725622,
+            "heading": 0.5861329781592151,
+            "angularVelocity": 0.0002010302390486887,
+            "velocityX": 3.3352740072612326,
+            "velocityY": 1.7849673745744064,
+            "timestamp": 2.005103055192769
+        },
+        {
+            "x": 6.6711729055545455,
+            "y": 1.9014971320931282,
+            "heading": 0.586140337461326,
+            "angularVelocity": 0.00020108146611328895,
+            "velocityX": 3.3352707493638243,
+            "velocityY": 1.7849734272511966,
+            "timestamp": 2.0417016649665674
+        },
+        {
+            "x": 6.789839361593639,
+            "y": 1.9681130226753558,
+            "heading": 0.59368067703471,
+            "angularVelocity": 0.20602803275828666,
+            "velocityX": 3.2423760567006434,
+            "velocityY": 1.820175438191616,
+            "timestamp": 2.0783002747403656
+        },
+        {
+            "x": 6.899503815249364,
+            "y": 2.0364566539738247,
+            "heading": 0.622995969771871,
+            "angularVelocity": 0.8009947076773526,
+            "velocityX": 2.9964103645881037,
+            "velocityY": 1.8673832618472042,
+            "timestamp": 2.114898884514164
+        },
+        {
+            "x": 7.0002752102658246,
+            "y": 2.1033967313761632,
+            "heading": 0.6764261486140803,
+            "angularVelocity": 1.4598964051487269,
+            "velocityX": 2.753421390574382,
+            "velocityY": 1.829033338043951,
+            "timestamp": 2.151497494287962
+        },
+        {
+            "x": 7.092625779879376,
+            "y": 2.166299323632935,
+            "heading": 0.7535384043827013,
+            "angularVelocity": 2.1069722660292705,
+            "velocityX": 2.5233354541151716,
+            "velocityY": 1.7187153458983286,
+            "timestamp": 2.1880961040617604
+        },
+        {
+            "x": 7.177366976931162,
+            "y": 2.223194562450384,
+            "heading": 0.8522168962516827,
+            "angularVelocity": 2.696236072322813,
+            "velocityX": 2.3154212024865664,
+            "velocityY": 1.5545737712196184,
+            "timestamp": 2.2246947138355586
+        },
+        {
+            "x": 7.2553185898803205,
+            "y": 2.2728300819136558,
+            "heading": 0.9705292362405307,
+            "angularVelocity": 3.2327003872576037,
+            "velocityX": 2.1299063934654243,
+            "velocityY": 1.3562132488105336,
+            "timestamp": 2.261293323609357
+        },
+        {
+            "x": 7.327198001580843,
+            "y": 2.3144960913605033,
+            "heading": 1.1077214572134357,
+            "angularVelocity": 3.748563724710752,
+            "velocityX": 1.9639929534148923,
+            "velocityY": 1.1384588022432733,
+            "timestamp": 2.297891933383155
+        },
+        {
+            "x": 7.393635990725555,
+            "y": 2.347893157970517,
+            "heading": 1.2645826825431576,
+            "angularVelocity": 4.285988629055024,
+            "velocityX": 1.8153145585403265,
+            "velocityY": 0.9125228203046082,
+            "timestamp": 2.3344905431569534
+        },
+        {
+            "x": 7.455288023435831,
+            "y": 2.373088898730826,
+            "heading": 1.443531300083093,
+            "angularVelocity": 4.889492214211044,
+            "velocityX": 1.684545754369454,
+            "velocityY": 0.6884343672077653,
+            "timestamp": 2.3710891529307516
+        },
+        {
+            "x": 7.509792212794572,
+            "y": 2.3938656260636906,
+            "heading": 1.6501307836273436,
+            "angularVelocity": 5.64500905420071,
+            "velocityX": 1.4892420694559032,
+            "velocityY": 0.5676917090916096,
+            "timestamp": 2.40768776270455
+        },
+        {
+            "x": 7.556730666272478,
+            "y": 2.4114332825613682,
+            "heading": 1.888437501864436,
+            "angularVelocity": 6.51135984973125,
+            "velocityX": 1.2825201221580125,
+            "velocityY": 0.48000884750149375,
+            "timestamp": 2.444286372478348
+        },
+        {
+            "x": 7.593462104085248,
+            "y": 2.4287428163755616,
+            "heading": 2.1429512418884697,
+            "angularVelocity": 6.954191473312333,
+            "velocityX": 1.0036293192499064,
+            "velocityY": 0.4729560472700227,
+            "timestamp": 2.4808849822521464
+        },
+        {
+            "x": 7.620818492305724,
+            "y": 2.4405113575302453,
+            "heading": 2.4064355131818194,
+            "angularVelocity": 7.199297266257987,
+            "velocityX": 0.7474706932737378,
+            "velocityY": 0.32155705441874516,
+            "timestamp": 2.5174835920259446
+        },
+        {
+            "x": 7.6400541302749785,
+            "y": 2.446864161128428,
+            "heading": 2.650140298596015,
+            "angularVelocity": 6.658853626420207,
+            "velocityX": 0.5255838428875319,
+            "velocityY": 0.17358046213904924,
+            "timestamp": 2.554082201799743
+        },
+        {
+            "x": 7.6561677382990485,
+            "y": 2.4508382476782002,
+            "heading": 2.845393638086096,
+            "angularVelocity": 5.334993342557683,
+            "velocityX": 0.4402792380273905,
+            "velocityY": 0.10858572427572141,
+            "timestamp": 2.590680811573541
+        },
+        {
+            "x": 7.668208670546753,
+            "y": 2.453890313590812,
+            "heading": 2.992258715743694,
+            "angularVelocity": 4.01285946557297,
+            "velocityX": 0.3289997167139778,
+            "velocityY": 0.08339294665767145,
+            "timestamp": 2.6272794213473394
+        },
+        {
+            "x": 7.676124567266394,
+            "y": 2.4559646616612736,
+            "heading": 3.0905802273604146,
+            "angularVelocity": 2.686482143021458,
+            "velocityX": 0.21628954674959436,
+            "velocityY": 0.056678329676522185,
+            "timestamp": 2.6638780311211376
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 1.350318302936367,
+            "velocityX": 0.10589016242853574,
+            "velocityY": 0.028289007290860937,
+            "timestamp": 2.700476640894936
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -2.3806959148386727e-33,
+            "velocityX": 4.3916568461332404e-35,
+            "velocityY": 1.2700164208802373e-34,
+            "timestamp": 2.737075250668734
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.traj
new file mode 100644
index 00000000..f2e9d5e3
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.traj	
@@ -0,0 +1,589 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.291384520253252,
+            "y": 4.354032854764245,
+            "heading": 2.74598551663602,
+            "angularVelocity": -0.1129108392120503,
+            "velocityX": -0.24231786763597,
+            "velocityY": -0.1678311541441644,
+            "timestamp": 0.03555445510807641
+        },
+        {
+            "x": 2.274618850931762,
+            "y": 4.341572242021881,
+            "heading": 2.7363109696234753,
+            "angularVelocity": -0.2721050564025352,
+            "velocityX": -0.47154904415004567,
+            "velocityY": -0.3504655803186264,
+            "timestamp": 0.07110891021615282
+        },
+        {
+            "x": 2.250443983775186,
+            "y": 4.321957452674385,
+            "heading": 2.718567267740621,
+            "angularVelocity": -0.4990570613139176,
+            "velocityX": -0.6799391829544631,
+            "velocityY": -0.5516830250350497,
+            "timestamp": 0.10666336532422922
+        },
+        {
+            "x": 2.2201183125080237,
+            "y": 4.294438762451095,
+            "heading": 2.688674932435733,
+            "angularVelocity": -0.8407479516708394,
+            "velocityX": -0.8529359028279262,
+            "velocityY": -0.7739871174973922,
+            "timestamp": 0.14221782043230563
+        },
+        {
+            "x": 2.1856076192251765,
+            "y": 4.258557396291605,
+            "heading": 2.63987728987899,
+            "angularVelocity": -1.3724761751631571,
+            "velocityX": -0.9706432900727432,
+            "velocityY": -1.009194657896466,
+            "timestamp": 0.17777227554038205
+        },
+        {
+            "x": 2.1489602975624615,
+            "y": 4.2143768004023965,
+            "heading": 2.56722210682279,
+            "angularVelocity": -2.0434902696538804,
+            "velocityX": -1.0307378231874669,
+            "velocityY": -1.2426177185084084,
+            "timestamp": 0.21332673064845847
+        },
+        {
+            "x": 2.1162123400405237,
+            "y": 4.163459966768879,
+            "heading": 2.4666019501957965,
+            "angularVelocity": -2.8300294947885973,
+            "velocityX": -0.9210648123390461,
+            "velocityY": -1.4320802689492316,
+            "timestamp": 0.2488811857565349
+        },
+        {
+            "x": 2.0883593879928255,
+            "y": 4.106985689754014,
+            "heading": 2.334003256666977,
+            "angularVelocity": -3.7294536824078297,
+            "velocityX": -0.7833885222831486,
+            "velocityY": -1.588388201793505,
+            "timestamp": 0.2844356408646113
+        },
+        {
+            "x": 2.0671900205502793,
+            "y": 4.044210359842601,
+            "heading": 2.181383778591042,
+            "angularVelocity": -4.2925556758502195,
+            "velocityX": -0.5954068872155824,
+            "velocityY": -1.765610799563443,
+            "timestamp": 0.31999009597268774
+        },
+        {
+            "x": 2.0546372297458078,
+            "y": 3.9756937078562617,
+            "heading": 2.021719977202011,
+            "angularVelocity": -4.49068340110105,
+            "velocityX": -0.35305816855621736,
+            "velocityY": -1.927090480730638,
+            "timestamp": 0.35554455108076416
+        },
+        {
+            "x": 2.0513227865299846,
+            "y": 3.902144544449752,
+            "heading": 1.8643846688260233,
+            "angularVelocity": -4.42519250816047,
+            "velocityX": -0.0932216006615165,
+            "velocityY": -2.068634245214532,
+            "timestamp": 0.3910990061888406
+        },
+        {
+            "x": 2.056389020104272,
+            "y": 3.822243076047335,
+            "heading": 1.7096006131055106,
+            "angularVelocity": -4.353436306364677,
+            "velocityX": 0.14249222942348805,
+            "velocityY": -2.247298352893811,
+            "timestamp": 0.426653461296917
+        },
+        {
+            "x": 2.0690751873782185,
+            "y": 3.7373642087091223,
+            "heading": 1.5737353715537044,
+            "angularVelocity": -3.821328189078159,
+            "velocityX": 0.35680949786416494,
+            "velocityY": -2.3872920307793386,
+            "timestamp": 0.4622079164049934
+        },
+        {
+            "x": 2.089274580938585,
+            "y": 3.648200269765939,
+            "heading": 1.460663871168444,
+            "angularVelocity": -3.1802343768608408,
+            "velocityX": 0.5681255274188619,
+            "velocityY": -2.507813399815797,
+            "timestamp": 0.49776237151306985
+        },
+        {
+            "x": 2.1169367654773477,
+            "y": 3.55409496646177,
+            "heading": 1.3686138295927854,
+            "angularVelocity": -2.5889875487010094,
+            "velocityX": 0.7780230200315752,
+            "velocityY": -2.646793573916784,
+            "timestamp": 0.5333168266211462
+        },
+        {
+            "x": 2.1519045434306308,
+            "y": 3.4545849603291003,
+            "heading": 1.296733638399589,
+            "angularVelocity": -2.0216929488779694,
+            "velocityX": 0.9834991943200934,
+            "velocityY": -2.798805545751856,
+            "timestamp": 0.5688712817292226
+        },
+        {
+            "x": 2.1939849352834964,
+            "y": 3.3493550759903967,
+            "heading": 1.244789692250972,
+            "angularVelocity": -1.4609687025358928,
+            "velocityX": 1.1835476517626813,
+            "velocityY": -2.959682099439628,
+            "timestamp": 0.604425736837299
+        },
+        {
+            "x": 2.243083202431903,
+            "y": 3.2382839213654937,
+            "heading": 1.2128218877185057,
+            "angularVelocity": -0.8991223303884791,
+            "velocityX": 1.3809315034968324,
+            "velocityY": -3.123972911053636,
+            "timestamp": 0.6399801919453755
+        },
+        {
+            "x": 2.299610340542871,
+            "y": 3.1217794185343526,
+            "heading": 1.2005607333325377,
+            "angularVelocity": -0.3448556404168553,
+            "velocityX": 1.5898749661368734,
+            "velocityY": -3.276790559073325,
+            "timestamp": 0.6755346470534519
+        },
+        {
+            "x": 2.3661237313117724,
+            "y": 3.0048767909387704,
+            "heading": 1.2005598156629014,
+            "angularVelocity": -0.000025810257353301716,
+            "velocityX": 1.8707470151551455,
+            "velocityY": -3.2879881646400775,
+            "timestamp": 0.7110891021615283
+        },
+        {
+            "x": 2.4413914156279892,
+            "y": 2.893838891515528,
+            "heading": 1.1996733189380515,
+            "angularVelocity": -0.02493349208011108,
+            "velocityX": 2.116969141769213,
+            "velocityY": -3.123037579558325,
+            "timestamp": 0.7466435572696047
+        },
+        {
+            "x": 2.522968201294396,
+            "y": 2.7909984002833275,
+            "heading": 1.191481222958301,
+            "angularVelocity": -0.2304098306344059,
+            "velocityX": 2.2944181093040097,
+            "velocityY": -2.89247833835709,
+            "timestamp": 0.7821980123776812
+        },
+        {
+            "x": 2.6104869842529297,
+            "y": 2.6966493129730225,
+            "heading": 1.1763516697745318,
+            "angularVelocity": -0.4255318535406987,
+            "velocityX": 2.4615419556423808,
+            "velocityY": -2.653650211302861,
+            "timestamp": 0.8177524674857576
+        },
+        {
+            "x": 2.7952393085826728,
+            "y": 2.5500987298885027,
+            "heading": 1.131439771707853,
+            "angularVelocity": -0.6752506092749339,
+            "velocityX": 2.777752108882206,
+            "velocityY": -2.203388740562692,
+            "timestamp": 0.884263918863645
+        },
+        {
+            "x": 2.9911597283478026,
+            "y": 2.437065890505019,
+            "heading": 1.0475109017935547,
+            "angularVelocity": -1.2618709737283185,
+            "velocityX": 2.9456644789180824,
+            "velocityY": -1.6994492984566387,
+            "timestamp": 0.9507753702415324
+        },
+        {
+            "x": 3.2086737929320703,
+            "y": 2.3383672587643702,
+            "heading": 1.0088878551520015,
+            "angularVelocity": -0.5806976970343763,
+            "velocityX": 3.2703250354356728,
+            "velocityY": -1.4839344157427674,
+            "timestamp": 1.0172868216194197
+        },
+        {
+            "x": 3.4256073991681717,
+            "y": 2.2401993070344908,
+            "heading": 0.9691349707945731,
+            "angularVelocity": -0.5976848126733959,
+            "velocityX": 3.2615978413038147,
+            "velocityY": -1.4759556391595554,
+            "timestamp": 1.083798272997307
+        },
+        {
+            "x": 3.641898030119061,
+            "y": 2.142612874932996,
+            "heading": 0.9280602960666862,
+            "angularVelocity": -0.6175579374222298,
+            "velocityX": 3.251930704714679,
+            "velocityY": -1.4672124886743745,
+            "timestamp": 1.1503097243751943
+        },
+        {
+            "x": 3.8574674811397935,
+            "y": 2.045673041019181,
+            "heading": 0.8854247472051884,
+            "angularVelocity": -0.6410256877310082,
+            "velocityX": 3.2410877609025053,
+            "velocityY": -1.4574908817287378,
+            "timestamp": 1.2168211757530816
+        },
+        {
+            "x": 4.072209689171901,
+            "y": 1.949463725749032,
+            "heading": 0.8409091268268268,
+            "angularVelocity": -0.6692925722736718,
+            "velocityX": 3.2286501584823513,
+            "velocityY": -1.446507530312815,
+            "timestamp": 1.2833326271309689
+        },
+        {
+            "x": 4.28597464141242,
+            "y": 1.8540976935596791,
+            "heading": 0.7940685595150717,
+            "angularVelocity": -0.7042481609013259,
+            "velocityX": 3.2139571128286692,
+            "velocityY": -1.4338287650276498,
+            "timestamp": 1.3498440785088561
+        },
+        {
+            "x": 4.498537652945423,
+            "y": 1.7597351759166577,
+            "heading": 0.7442459258691415,
+            "angularVelocity": -0.7490835429655718,
+            "velocityX": 3.195885928354179,
+            "velocityY": -1.4187409188666942,
+            "timestamp": 1.4163555298867434
+        },
+        {
+            "x": 4.709533812148138,
+            "y": 1.6666229170102642,
+            "heading": 0.6903878152301356,
+            "angularVelocity": -0.8097569594897124,
+            "velocityX": 3.1723282958288084,
+            "velocityY": -1.399943272585839,
+            "timestamp": 1.4828669812646307
+        },
+        {
+            "x": 4.924860666444789,
+            "y": 1.5757431293365876,
+            "heading": 0.6452055578690052,
+            "angularVelocity": -0.679315462602457,
+            "velocityX": 3.2374403179576423,
+            "velocityY": -1.3663780565746433,
+            "timestamp": 1.549378432642518
+        },
+        {
+            "x": 5.15821860461426,
+            "y": 1.5166188222705783,
+            "heading": 0.6185000496396688,
+            "angularVelocity": -0.4015174481399904,
+            "velocityX": 3.5085377530500543,
+            "velocityY": -0.8889342487820968,
+            "timestamp": 1.6158898840204052
+        },
+        {
+            "x": 5.39961745545984,
+            "y": 1.4934726786236598,
+            "heading": 0.5961273041444063,
+            "angularVelocity": -0.33637433903149927,
+            "velocityX": 3.6294329148534703,
+            "velocityY": -0.3480023840618476,
+            "timestamp": 1.6824013353982925
+        },
+        {
+            "x": 5.646764755249023,
+            "y": 1.5066063404083252,
+            "heading": 0.585736957474559,
+            "angularVelocity": -0.15621891350429284,
+            "velocityX": 3.7158608731150236,
+            "velocityY": 0.19746466980619687,
+            "timestamp": 1.7489127867761798
+        },
+        {
+            "x": 5.78386589002804,
+            "y": 1.5248142647082106,
+            "heading": 0.5861133793107512,
+            "angularVelocity": 0.010285140296820981,
+            "velocityX": 3.7460749363537476,
+            "velocityY": 0.4975031678094133,
+            "timestamp": 1.7855113965499783
+        },
+        {
+            "x": 5.9191942381460025,
+            "y": 1.554048616452813,
+            "heading": 0.586114299955562,
+            "angularVelocity": 0.00002515518530364309,
+            "velocityX": 3.69763630242715,
+            "velocityY": 0.7987831211428086,
+            "timestamp": 1.8221100063237767
+        },
+        {
+            "x": 6.0517259817313,
+            "y": 1.5940956419716419,
+            "heading": 0.5861150757223451,
+            "angularVelocity": 0.000021196618886827452,
+            "velocityX": 3.6212234400274688,
+            "velocityY": 1.0942225884082484,
+            "timestamp": 1.8587086160975752
+        },
+        {
+            "x": 6.180598624537076,
+            "y": 1.644694580267861,
+            "heading": 0.5861160659567196,
+            "angularVelocity": 0.00002705661173256264,
+            "velocityX": 3.521244211249743,
+            "velocityY": 1.3825371676397282,
+            "timestamp": 1.8953072258713737
+        },
+        {
+            "x": 6.304973627271485,
+            "y": 1.7055153509385155,
+            "heading": 0.5861182648046722,
+            "angularVelocity": 0.00006008009501406976,
+            "velocityX": 3.3983532025703247,
+            "velocityY": 1.6618328140485057,
+            "timestamp": 1.9319058356451722
+        },
+        {
+            "x": 6.427040241028168,
+            "y": 1.7708422617715522,
+            "heading": 0.5861256207319434,
+            "angularVelocity": 0.0002009892538702712,
+            "velocityX": 3.335280069683768,
+            "velocityY": 1.784956074473775,
+            "timestamp": 1.9685044454189706
+        },
+        {
+            "x": 6.5491066329086145,
+            "y": 1.8361695861725622,
+            "heading": 0.5861329781592151,
+            "angularVelocity": 0.0002010302390486887,
+            "velocityX": 3.3352740072612326,
+            "velocityY": 1.7849673745744064,
+            "timestamp": 2.005103055192769
+        },
+        {
+            "x": 6.6711729055545455,
+            "y": 1.9014971320931282,
+            "heading": 0.586140337461326,
+            "angularVelocity": 0.00020108146611328895,
+            "velocityX": 3.3352707493638243,
+            "velocityY": 1.7849734272511966,
+            "timestamp": 2.0417016649665674
+        },
+        {
+            "x": 6.789839361593639,
+            "y": 1.9681130226753558,
+            "heading": 0.59368067703471,
+            "angularVelocity": 0.20602803275828666,
+            "velocityX": 3.2423760567006434,
+            "velocityY": 1.820175438191616,
+            "timestamp": 2.0783002747403656
+        },
+        {
+            "x": 6.899503815249364,
+            "y": 2.0364566539738247,
+            "heading": 0.622995969771871,
+            "angularVelocity": 0.8009947076773526,
+            "velocityX": 2.9964103645881037,
+            "velocityY": 1.8673832618472042,
+            "timestamp": 2.114898884514164
+        },
+        {
+            "x": 7.0002752102658246,
+            "y": 2.1033967313761632,
+            "heading": 0.6764261486140803,
+            "angularVelocity": 1.4598964051487269,
+            "velocityX": 2.753421390574382,
+            "velocityY": 1.829033338043951,
+            "timestamp": 2.151497494287962
+        },
+        {
+            "x": 7.092625779879376,
+            "y": 2.166299323632935,
+            "heading": 0.7535384043827013,
+            "angularVelocity": 2.1069722660292705,
+            "velocityX": 2.5233354541151716,
+            "velocityY": 1.7187153458983286,
+            "timestamp": 2.1880961040617604
+        },
+        {
+            "x": 7.177366976931162,
+            "y": 2.223194562450384,
+            "heading": 0.8522168962516827,
+            "angularVelocity": 2.696236072322813,
+            "velocityX": 2.3154212024865664,
+            "velocityY": 1.5545737712196184,
+            "timestamp": 2.2246947138355586
+        },
+        {
+            "x": 7.2553185898803205,
+            "y": 2.2728300819136558,
+            "heading": 0.9705292362405307,
+            "angularVelocity": 3.2327003872576037,
+            "velocityX": 2.1299063934654243,
+            "velocityY": 1.3562132488105336,
+            "timestamp": 2.261293323609357
+        },
+        {
+            "x": 7.327198001580843,
+            "y": 2.3144960913605033,
+            "heading": 1.1077214572134357,
+            "angularVelocity": 3.748563724710752,
+            "velocityX": 1.9639929534148923,
+            "velocityY": 1.1384588022432733,
+            "timestamp": 2.297891933383155
+        },
+        {
+            "x": 7.393635990725555,
+            "y": 2.347893157970517,
+            "heading": 1.2645826825431576,
+            "angularVelocity": 4.285988629055024,
+            "velocityX": 1.8153145585403265,
+            "velocityY": 0.9125228203046082,
+            "timestamp": 2.3344905431569534
+        },
+        {
+            "x": 7.455288023435831,
+            "y": 2.373088898730826,
+            "heading": 1.443531300083093,
+            "angularVelocity": 4.889492214211044,
+            "velocityX": 1.684545754369454,
+            "velocityY": 0.6884343672077653,
+            "timestamp": 2.3710891529307516
+        },
+        {
+            "x": 7.509792212794572,
+            "y": 2.3938656260636906,
+            "heading": 1.6501307836273436,
+            "angularVelocity": 5.64500905420071,
+            "velocityX": 1.4892420694559032,
+            "velocityY": 0.5676917090916096,
+            "timestamp": 2.40768776270455
+        },
+        {
+            "x": 7.556730666272478,
+            "y": 2.4114332825613682,
+            "heading": 1.888437501864436,
+            "angularVelocity": 6.51135984973125,
+            "velocityX": 1.2825201221580125,
+            "velocityY": 0.48000884750149375,
+            "timestamp": 2.444286372478348
+        },
+        {
+            "x": 7.593462104085248,
+            "y": 2.4287428163755616,
+            "heading": 2.1429512418884697,
+            "angularVelocity": 6.954191473312333,
+            "velocityX": 1.0036293192499064,
+            "velocityY": 0.4729560472700227,
+            "timestamp": 2.4808849822521464
+        },
+        {
+            "x": 7.620818492305724,
+            "y": 2.4405113575302453,
+            "heading": 2.4064355131818194,
+            "angularVelocity": 7.199297266257987,
+            "velocityX": 0.7474706932737378,
+            "velocityY": 0.32155705441874516,
+            "timestamp": 2.5174835920259446
+        },
+        {
+            "x": 7.6400541302749785,
+            "y": 2.446864161128428,
+            "heading": 2.650140298596015,
+            "angularVelocity": 6.658853626420207,
+            "velocityX": 0.5255838428875319,
+            "velocityY": 0.17358046213904924,
+            "timestamp": 2.554082201799743
+        },
+        {
+            "x": 7.6561677382990485,
+            "y": 2.4508382476782002,
+            "heading": 2.845393638086096,
+            "angularVelocity": 5.334993342557683,
+            "velocityX": 0.4402792380273905,
+            "velocityY": 0.10858572427572141,
+            "timestamp": 2.590680811573541
+        },
+        {
+            "x": 7.668208670546753,
+            "y": 2.453890313590812,
+            "heading": 2.992258715743694,
+            "angularVelocity": 4.01285946557297,
+            "velocityX": 0.3289997167139778,
+            "velocityY": 0.08339294665767145,
+            "timestamp": 2.6272794213473394
+        },
+        {
+            "x": 7.676124567266394,
+            "y": 2.4559646616612736,
+            "heading": 3.0905802273604146,
+            "angularVelocity": 2.686482143021458,
+            "velocityX": 0.21628954674959436,
+            "velocityY": 0.056678329676522185,
+            "timestamp": 2.6638780311211376
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": 1.350318302936367,
+            "velocityX": 0.10589016242853574,
+            "velocityY": 0.028289007290860937,
+            "timestamp": 2.700476640894936
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 3.14,
+            "angularVelocity": -2.3806959148386727e-33,
+            "velocityX": 4.3916568461332404e-35,
+            "velocityY": 1.2700164208802373e-34,
+            "timestamp": 2.737075250668734
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.1.traj
new file mode 100644
index 00000000..a8fe3b09
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.1.traj	
@@ -0,0 +1,535 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.0996459321900828e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2983894677520005,
+            "y": 4.3533419612939515,
+            "heading": 2.758567124910339,
+            "angularVelocity": 0.29099245314259387,
+            "velocityX": -0.05470361814673814,
+            "velocityY": -0.226148099446212,
+            "timestamp": 0.02944105531884974
+        },
+        {
+            "x": 2.295387593245339,
+            "y": 4.339939574817211,
+            "heading": 2.775252952592925,
+            "angularVelocity": 0.5667537220346529,
+            "velocityX": -0.10196219103394874,
+            "velocityY": -0.45522778757728155,
+            "timestamp": 0.05888211063769948
+        },
+        {
+            "x": 2.2912544329244677,
+            "y": 4.31970469118274,
+            "heading": 2.7995524599349486,
+            "angularVelocity": 0.8253612881351188,
+            "velocityX": -0.14038764154710928,
+            "velocityY": -0.687301573103468,
+            "timestamp": 0.08832316595654921
+        },
+        {
+            "x": 2.286299680545296,
+            "y": 4.292550560538348,
+            "heading": 2.8308875928825827,
+            "angularVelocity": 1.064334569813193,
+            "velocityX": -0.1682939801413744,
+            "velocityY": -0.922321919180889,
+            "timestamp": 0.11776422127539896
+        },
+        {
+            "x": 2.280893923712392,
+            "y": 4.258397506462172,
+            "heading": 2.868587408224874,
+            "angularVelocity": 1.2805184778194243,
+            "velocityX": -0.18361287577360647,
+            "velocityY": -1.1600485684461586,
+            "timestamp": 0.1472052765942487
+        },
+        {
+            "x": 2.2754825429318224,
+            "y": 4.217182700090041,
+            "heading": 2.9118655493454413,
+            "angularVelocity": 1.4699928603733934,
+            "velocityX": -0.18380389975713432,
+            "velocityY": -1.3999092738276546,
+            "timestamp": 0.17664633191309845
+        },
+        {
+            "x": 2.2706016921590138,
+            "y": 4.168876446645306,
+            "heading": 2.959795755168493,
+            "angularVelocity": 1.6280056982999451,
+            "velocityX": -0.16578382534010866,
+            "velocityY": -1.6407785971519722,
+            "timestamp": 0.2060873872319482
+        },
+        {
+            "x": 2.2668942123670868,
+            "y": 4.113508113408168,
+            "heading": 3.0112851676807657,
+            "angularVelocity": 1.748898331076693,
+            "velocityX": -0.12592890274395677,
+            "velocityY": -1.880650426334687,
+            "timestamp": 0.23552844255079794
+        },
+        {
+            "x": 2.2651203704726766,
+            "y": 4.051204935167073,
+            "heading": 3.0650426993920417,
+            "angularVelocity": 1.8259376618492857,
+            "velocityX": -0.06025062196987639,
+            "velocityY": -2.1162005765875382,
+            "timestamp": 0.2649694978696477
+        },
+        {
+            "x": 2.2661533431015752,
+            "y": 3.9822449997835685,
+            "heading": 3.1195407017354904,
+            "angularVelocity": 1.8510886159897915,
+            "velocityX": 0.03508612778011459,
+            "velocityY": -2.3423051462205073,
+            "timestamp": 0.2944105531884974
+        },
+        {
+            "x": 2.2709425124463385,
+            "y": 3.9071174668882147,
+            "heading": 3.1729959824105403,
+            "angularVelocity": 1.8156713506402375,
+            "velocityX": 0.16266975802653263,
+            "velocityY": -2.5517948348561,
+            "timestamp": 0.3238516085073471
+        },
+        {
+            "x": 2.2797199160454222,
+            "y": 3.825989687229236,
+            "heading": 3.2244176609184314,
+            "angularVelocity": 1.7465976661158726,
+            "velocityX": 0.2981348156179574,
+            "velocityY": -2.7556002589022834,
+            "timestamp": 0.35329266382619684
+        },
+        {
+            "x": 2.292782229397934,
+            "y": 3.7391196184482327,
+            "heading": 3.272453259481913,
+            "angularVelocity": 1.6315854864321535,
+            "velocityX": 0.4436768047559834,
+            "velocityY": -2.9506438488767315,
+            "timestamp": 0.38273371914504656
+        },
+        {
+            "x": 2.31050581940653,
+            "y": 3.6469279173399998,
+            "heading": 3.3152109067306803,
+            "angularVelocity": 1.4523136750940884,
+            "velocityX": 0.6020025374990144,
+            "velocityY": -3.131399337074948,
+            "timestamp": 0.4121747744638963
+        },
+        {
+            "x": 2.3333478257637266,
+            "y": 3.5501341924969156,
+            "heading": 3.350152812747203,
+            "angularVelocity": 1.186842850505799,
+            "velocityX": 0.7758555564606926,
+            "velocityY": -3.287712474800853,
+            "timestamp": 0.441615829782746
+        },
+        {
+            "x": 2.3619457015355163,
+            "y": 3.450260447984075,
+            "heading": 3.374288635448379,
+            "angularVelocity": 0.8198015471858178,
+            "velocityX": 0.9713604170119489,
+            "velocityY": -3.3923289580212845,
+            "timestamp": 0.4710568851015957
+        },
+        {
+            "x": 2.396816597338257,
+            "y": 3.3487121183154565,
+            "heading": 3.38694435959391,
+            "angularVelocity": 0.42986652511155254,
+            "velocityX": 1.184430905247282,
+            "velocityY": -3.4492082083620734,
+            "timestamp": 0.5004979404204455
+        },
+        {
+            "x": 2.4384933115651135,
+            "y": 3.2467782441206525,
+            "heading": 3.390761607070638,
+            "angularVelocity": 0.12965729099677573,
+            "velocityX": 1.4155985162723614,
+            "velocityY": -3.462303680722349,
+            "timestamp": 0.5299389957392953
+        },
+        {
+            "x": 2.487106839227099,
+            "y": 3.1465743963564163,
+            "heading": 3.3907619234819757,
+            "angularVelocity": 0.000010747282459786006,
+            "velocityX": 1.6512155266004023,
+            "velocityY": -3.403541302409769,
+            "timestamp": 0.559380051058145
+        },
+        {
+            "x": 2.5421170689241235,
+            "y": 3.049734421677047,
+            "heading": 3.39076189132422,
+            "angularVelocity": -0.0000010922759119129131,
+            "velocityX": 1.8684870192748764,
+            "velocityY": -3.2892834047754955,
+            "timestamp": 0.5888211063769948
+        },
+        {
+            "x": 2.603292311228317,
+            "y": 2.9566661889623873,
+            "heading": 3.3907618600542158,
+            "angularVelocity": -0.0000010621224075785883,
+            "velocityX": 2.077888908589026,
+            "velocityY": -3.161171761905971,
+            "timestamp": 0.6182621616958446
+        },
+        {
+            "x": 2.670374957735173,
+            "y": 2.8677616065314258,
+            "heading": 3.3907618290531647,
+            "angularVelocity": -0.0000010529870817317445,
+            "velocityX": 2.2785408260792437,
+            "velocityY": -3.019748492984228,
+            "timestamp": 0.6477032170146944
+        },
+        {
+            "x": 2.7430825233459473,
+            "y": 2.7833950519561768,
+            "heading": 3.3907617949090247,
+            "angularVelocity": -0.0000011597457911507082,
+            "velocityX": 2.4695978056269863,
+            "velocityY": -2.8656090504076794,
+            "timestamp": 0.6771442723335441
+        },
+        {
+            "x": 2.8743019890864545,
+            "y": 2.6600421338324565,
+            "heading": 3.3907617697237464,
+            "angularVelocity": -5.290207073739654e-7,
+            "velocityX": 2.7562853520145434,
+            "velocityY": -2.5910472919087915,
+            "timestamp": 0.7247516311451417
+        },
+        {
+            "x": 3.0177249974626466,
+            "y": 2.5511186165163586,
+            "heading": 3.390761748438384,
+            "angularVelocity": -4.4710235312140834e-7,
+            "velocityX": 3.0126226691923277,
+            "velocityY": -2.287955476529456,
+            "timestamp": 0.7723589899567394
+        },
+        {
+            "x": 3.171772290811784,
+            "y": 2.457823792449377,
+            "heading": 3.3907617293701717,
+            "angularVelocity": -4.005307751758197e-7,
+            "velocityX": 3.235787432753145,
+            "velocityY": -1.95967233629129,
+            "timestamp": 0.819966348768337
+        },
+        {
+            "x": 3.3347475520740515,
+            "y": 2.3811846546739925,
+            "heading": 3.3907617114029063,
+            "angularVelocity": -3.7740521255041523e-7,
+            "velocityX": 3.4233207917967023,
+            "velocityY": -1.6098170469543955,
+            "timestamp": 0.8675737075799346
+        },
+        {
+            "x": 3.5038153046646756,
+            "y": 2.3191315453490344,
+            "heading": 3.3907616937093077,
+            "angularVelocity": -3.7165679655198125e-7,
+            "velocityX": 3.551294522758467,
+            "velocityY": -1.3034352434994065,
+            "timestamp": 0.9151810663915322
+        },
+        {
+            "x": 3.6728836971491297,
+            "y": 2.257080179484392,
+            "heading": 3.3907616760157264,
+            "angularVelocity": -3.7165643530673955e-7,
+            "velocityX": 3.551307963828218,
+            "velocityY": -1.303398621843431,
+            "timestamp": 0.9627884252031298
+        },
+        {
+            "x": 3.8419520974106347,
+            "y": 2.195028834809519,
+            "heading": 3.3907616583221465,
+            "angularVelocity": -3.71656403732769e-7,
+            "velocityX": 3.5513081271863745,
+            "velocityY": -1.3033981767490248,
+            "timestamp": 1.0103957840147275
+        },
+        {
+            "x": 4.011020504671481,
+            "y": 2.1329775092054373,
+            "heading": 3.390761640628565,
+            "angularVelocity": -3.7165644168556193e-7,
+            "velocityX": 3.5513082742086395,
+            "velocityY": -1.3033977761640818,
+            "timestamp": 1.0580031428263252
+        },
+        {
+            "x": 4.180088918243229,
+            "y": 2.070926200796389,
+            "heading": 3.390761622934979,
+            "angularVelocity": -3.716565361157419e-7,
+            "velocityX": 3.55130840677009,
+            "velocityY": -1.3033974149797176,
+            "timestamp": 1.105610501637923
+        },
+        {
+            "x": 4.349157337505363,
+            "y": 2.008874907891698,
+            "heading": 3.3907616052413863,
+            "angularVelocity": -3.716566778801587e-7,
+            "velocityX": 3.551308526297583,
+            "velocityY": -1.3033970893082818,
+            "timestamp": 1.1532178604495207
+        },
+        {
+            "x": 4.518225761904485,
+            "y": 1.9468236289835406,
+            "heading": 3.390761587547785,
+            "angularVelocity": -3.716568562531517e-7,
+            "velocityX": 3.551308634200803,
+            "velocityY": -1.303396795308895,
+            "timestamp": 1.2008252192611184
+        },
+        {
+            "x": 4.687294190885785,
+            "y": 1.8847723625602526,
+            "heading": 3.3907615698541753,
+            "angularVelocity": -3.716570319734318e-7,
+            "velocityX": 3.5513087304501405,
+            "velocityY": -1.3033965330622725,
+            "timestamp": 1.2484325780727161
+        },
+        {
+            "x": 4.8563626239355635,
+            "y": 1.8227211072221874,
+            "heading": 3.390761552160558,
+            "angularVelocity": -3.716571921571891e-7,
+            "velocityX": 3.5513088159091875,
+            "velocityY": -1.3033963002154438,
+            "timestamp": 1.2960399368843138
+        },
+        {
+            "x": 5.025431060602707,
+            "y": 1.7606698617402123,
+            "heading": 3.3907615344669337,
+            "angularVelocity": -3.716573365191525e-7,
+            "velocityX": 3.551308891892495,
+            "velocityY": -1.3033960931867248,
+            "timestamp": 1.3436472956959116
+        },
+        {
+            "x": 5.1944995004938015,
+            "y": 1.6986186250424111,
+            "heading": 3.3907615167733036,
+            "angularVelocity": -3.7165746535883894e-7,
+            "velocityX": 3.551308959612086,
+            "velocityY": -1.3033959086737847,
+            "timestamp": 1.3912546545075093
+        },
+        {
+            "x": 5.363567943266909,
+            "y": 1.6365673961971205,
+            "heading": 3.3907614990796673,
+            "angularVelocity": -3.716575836762919e-7,
+            "velocityX": 3.551309020149218,
+            "velocityY": -1.3033957437305734,
+            "timestamp": 1.438862013319107
+        },
+        {
+            "x": 5.532636388625086,
+            "y": 1.574516174395274,
+            "heading": 3.390761481386026,
+            "angularVelocity": -3.716576912991928e-7,
+            "velocityX": 3.5513090744490245,
+            "velocityY": -1.3033955957819334,
+            "timestamp": 1.4864693721307047
+        },
+        {
+            "x": 5.701704836310244,
+            "y": 1.5124649589336634,
+            "heading": 3.3907614636923804,
+            "angularVelocity": -3.7165778948932697e-7,
+            "velocityX": 3.551309123327601,
+            "velocityY": -1.3033954626042876,
+            "timestamp": 1.5340767309423025
+        },
+        {
+            "x": 5.870773286097621,
+            "y": 1.4504137491998943,
+            "heading": 3.3907614459987303,
+            "angularVelocity": -3.716578803925389e-7,
+            "velocityX": 3.551309167485058,
+            "velocityY": -1.3033953422900804,
+            "timestamp": 1.5816840897539002
+        },
+        {
+            "x": 6.039841737790961,
+            "y": 1.388362544659232,
+            "heading": 3.3907614283050767,
+            "angularVelocity": -3.716579611998838e-7,
+            "velocityX": 3.551309207520092,
+            "velocityY": -1.3033952332080676,
+            "timestamp": 1.629291448565498
+        },
+        {
+            "x": 6.208910191218329,
+            "y": 1.3263113448432136,
+            "heading": 3.390761410611419,
+            "angularVelocity": -3.7165803844263857e-7,
+            "velocityX": 3.551309243943619,
+            "velocityY": -1.303395133966182,
+            "timestamp": 1.6768988073770956
+        },
+        {
+            "x": 6.377978646228594,
+            "y": 1.2642601493400643,
+            "heading": 3.3907613929177582,
+            "angularVelocity": -3.7165810831214996e-7,
+            "velocityX": 3.5513092771926584,
+            "velocityY": -1.3033950433736952,
+            "timestamp": 1.7245061661886933
+        },
+        {
+            "x": 6.547047102688449,
+            "y": 1.2022089577865573,
+            "heading": 3.390761375224094,
+            "angularVelocity": -3.716581745376039e-7,
+            "velocityX": 3.551309307641484,
+            "velocityY": -1.3033949604108395,
+            "timestamp": 1.772113525000291
+        },
+        {
+            "x": 6.716115560480058,
+            "y": 1.1401577698616354,
+            "heading": 3.3907613575304274,
+            "angularVelocity": -3.716582357126045e-7,
+            "velocityX": 3.5513093356151892,
+            "velocityY": -1.3033948841918472,
+            "timestamp": 1.8197208838118888
+        },
+        {
+            "x": 6.885184026346865,
+            "y": 1.0781066039393965,
+            "heading": 3.3907613398363963,
+            "angularVelocity": -3.7166587620925834e-7,
+            "velocityX": 3.5513095052359795,
+            "velocityY": -1.3033944220220548,
+            "timestamp": 1.8673282426234865
+        },
+        {
+            "x": 7.044084736711506,
+            "y": 1.0199905851581765,
+            "heading": 3.36350523152736,
+            "angularVelocity": -0.5725188077939848,
+            "velocityX": 3.337734214441043,
+            "velocityY": -1.220736042325084,
+            "timestamp": 1.9149356014350842
+        },
+        {
+            "x": 7.185551132214723,
+            "y": 0.9681799151008591,
+            "heading": 3.323163851941908,
+            "angularVelocity": -0.8473769726461788,
+            "velocityX": 2.9715237105057524,
+            "velocityY": -1.0882912085577785,
+            "timestamp": 1.962542960246682
+        },
+        {
+            "x": 7.3092683658418975,
+            "y": 0.9228556276324137,
+            "heading": 3.282206369227669,
+            "angularVelocity": -0.8603183149967459,
+            "velocityX": 2.598699795902896,
+            "velocityY": -0.952043730210126,
+            "timestamp": 2.0101503190582797
+        },
+        {
+            "x": 7.415256126479253,
+            "y": 0.8840204066687376,
+            "heading": 3.244221221054594,
+            "angularVelocity": -0.7978839641870918,
+            "velocityX": 2.226289449427224,
+            "velocityY": -0.815739875790283,
+            "timestamp": 2.057757677869877
+        },
+        {
+            "x": 7.503541109453659,
+            "y": 0.8516687069454374,
+            "heading": 3.210907204397792,
+            "angularVelocity": -0.6997661178524867,
+            "velocityX": 1.8544398424576745,
+            "velocityY": -0.6795525005142472,
+            "timestamp": 2.1053650366814747
+        },
+        {
+            "x": 7.574144027018988,
+            "y": 0.8257950136289538,
+            "heading": 3.1832598866385293,
+            "angularVelocity": -0.5807362233362916,
+            "velocityX": 1.4830252996124917,
+            "velocityY": -0.5434809651776052,
+            "timestamp": 2.152972395493072
+        },
+        {
+            "x": 7.627080539080189,
+            "y": 0.8063947169930168,
+            "heading": 3.161935751906697,
+            "angularVelocity": -0.44791677724069034,
+            "velocityX": 1.1119396955141558,
+            "velocityY": -0.407506257860514,
+            "timestamp": 2.2005797543046697
+        },
+        {
+            "x": 7.662362703782539,
+            "y": 0.7934640657363813,
+            "heading": 3.147400394630243,
+            "angularVelocity": -0.3053174475395025,
+            "velocityX": 0.7411073746387762,
+            "velocityY": -0.27161034721139543,
+            "timestamp": 2.248187113116267
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": -0.1554464438896817,
+            "velocityX": 0.370474158989983,
+            "velocityY": -0.13577870937899197,
+            "timestamp": 2.2957944719278647
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 1.845639156363882e-34,
+            "velocityX": 1.6232927575664752e-34,
+            "velocityY": 1.3237538522972492e-34,
+            "timestamp": 2.343401830739462
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.traj
new file mode 100644
index 00000000..a8fe3b09
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.traj	
@@ -0,0 +1,535 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": 2.75,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.0996459321900828e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2983894677520005,
+            "y": 4.3533419612939515,
+            "heading": 2.758567124910339,
+            "angularVelocity": 0.29099245314259387,
+            "velocityX": -0.05470361814673814,
+            "velocityY": -0.226148099446212,
+            "timestamp": 0.02944105531884974
+        },
+        {
+            "x": 2.295387593245339,
+            "y": 4.339939574817211,
+            "heading": 2.775252952592925,
+            "angularVelocity": 0.5667537220346529,
+            "velocityX": -0.10196219103394874,
+            "velocityY": -0.45522778757728155,
+            "timestamp": 0.05888211063769948
+        },
+        {
+            "x": 2.2912544329244677,
+            "y": 4.31970469118274,
+            "heading": 2.7995524599349486,
+            "angularVelocity": 0.8253612881351188,
+            "velocityX": -0.14038764154710928,
+            "velocityY": -0.687301573103468,
+            "timestamp": 0.08832316595654921
+        },
+        {
+            "x": 2.286299680545296,
+            "y": 4.292550560538348,
+            "heading": 2.8308875928825827,
+            "angularVelocity": 1.064334569813193,
+            "velocityX": -0.1682939801413744,
+            "velocityY": -0.922321919180889,
+            "timestamp": 0.11776422127539896
+        },
+        {
+            "x": 2.280893923712392,
+            "y": 4.258397506462172,
+            "heading": 2.868587408224874,
+            "angularVelocity": 1.2805184778194243,
+            "velocityX": -0.18361287577360647,
+            "velocityY": -1.1600485684461586,
+            "timestamp": 0.1472052765942487
+        },
+        {
+            "x": 2.2754825429318224,
+            "y": 4.217182700090041,
+            "heading": 2.9118655493454413,
+            "angularVelocity": 1.4699928603733934,
+            "velocityX": -0.18380389975713432,
+            "velocityY": -1.3999092738276546,
+            "timestamp": 0.17664633191309845
+        },
+        {
+            "x": 2.2706016921590138,
+            "y": 4.168876446645306,
+            "heading": 2.959795755168493,
+            "angularVelocity": 1.6280056982999451,
+            "velocityX": -0.16578382534010866,
+            "velocityY": -1.6407785971519722,
+            "timestamp": 0.2060873872319482
+        },
+        {
+            "x": 2.2668942123670868,
+            "y": 4.113508113408168,
+            "heading": 3.0112851676807657,
+            "angularVelocity": 1.748898331076693,
+            "velocityX": -0.12592890274395677,
+            "velocityY": -1.880650426334687,
+            "timestamp": 0.23552844255079794
+        },
+        {
+            "x": 2.2651203704726766,
+            "y": 4.051204935167073,
+            "heading": 3.0650426993920417,
+            "angularVelocity": 1.8259376618492857,
+            "velocityX": -0.06025062196987639,
+            "velocityY": -2.1162005765875382,
+            "timestamp": 0.2649694978696477
+        },
+        {
+            "x": 2.2661533431015752,
+            "y": 3.9822449997835685,
+            "heading": 3.1195407017354904,
+            "angularVelocity": 1.8510886159897915,
+            "velocityX": 0.03508612778011459,
+            "velocityY": -2.3423051462205073,
+            "timestamp": 0.2944105531884974
+        },
+        {
+            "x": 2.2709425124463385,
+            "y": 3.9071174668882147,
+            "heading": 3.1729959824105403,
+            "angularVelocity": 1.8156713506402375,
+            "velocityX": 0.16266975802653263,
+            "velocityY": -2.5517948348561,
+            "timestamp": 0.3238516085073471
+        },
+        {
+            "x": 2.2797199160454222,
+            "y": 3.825989687229236,
+            "heading": 3.2244176609184314,
+            "angularVelocity": 1.7465976661158726,
+            "velocityX": 0.2981348156179574,
+            "velocityY": -2.7556002589022834,
+            "timestamp": 0.35329266382619684
+        },
+        {
+            "x": 2.292782229397934,
+            "y": 3.7391196184482327,
+            "heading": 3.272453259481913,
+            "angularVelocity": 1.6315854864321535,
+            "velocityX": 0.4436768047559834,
+            "velocityY": -2.9506438488767315,
+            "timestamp": 0.38273371914504656
+        },
+        {
+            "x": 2.31050581940653,
+            "y": 3.6469279173399998,
+            "heading": 3.3152109067306803,
+            "angularVelocity": 1.4523136750940884,
+            "velocityX": 0.6020025374990144,
+            "velocityY": -3.131399337074948,
+            "timestamp": 0.4121747744638963
+        },
+        {
+            "x": 2.3333478257637266,
+            "y": 3.5501341924969156,
+            "heading": 3.350152812747203,
+            "angularVelocity": 1.186842850505799,
+            "velocityX": 0.7758555564606926,
+            "velocityY": -3.287712474800853,
+            "timestamp": 0.441615829782746
+        },
+        {
+            "x": 2.3619457015355163,
+            "y": 3.450260447984075,
+            "heading": 3.374288635448379,
+            "angularVelocity": 0.8198015471858178,
+            "velocityX": 0.9713604170119489,
+            "velocityY": -3.3923289580212845,
+            "timestamp": 0.4710568851015957
+        },
+        {
+            "x": 2.396816597338257,
+            "y": 3.3487121183154565,
+            "heading": 3.38694435959391,
+            "angularVelocity": 0.42986652511155254,
+            "velocityX": 1.184430905247282,
+            "velocityY": -3.4492082083620734,
+            "timestamp": 0.5004979404204455
+        },
+        {
+            "x": 2.4384933115651135,
+            "y": 3.2467782441206525,
+            "heading": 3.390761607070638,
+            "angularVelocity": 0.12965729099677573,
+            "velocityX": 1.4155985162723614,
+            "velocityY": -3.462303680722349,
+            "timestamp": 0.5299389957392953
+        },
+        {
+            "x": 2.487106839227099,
+            "y": 3.1465743963564163,
+            "heading": 3.3907619234819757,
+            "angularVelocity": 0.000010747282459786006,
+            "velocityX": 1.6512155266004023,
+            "velocityY": -3.403541302409769,
+            "timestamp": 0.559380051058145
+        },
+        {
+            "x": 2.5421170689241235,
+            "y": 3.049734421677047,
+            "heading": 3.39076189132422,
+            "angularVelocity": -0.0000010922759119129131,
+            "velocityX": 1.8684870192748764,
+            "velocityY": -3.2892834047754955,
+            "timestamp": 0.5888211063769948
+        },
+        {
+            "x": 2.603292311228317,
+            "y": 2.9566661889623873,
+            "heading": 3.3907618600542158,
+            "angularVelocity": -0.0000010621224075785883,
+            "velocityX": 2.077888908589026,
+            "velocityY": -3.161171761905971,
+            "timestamp": 0.6182621616958446
+        },
+        {
+            "x": 2.670374957735173,
+            "y": 2.8677616065314258,
+            "heading": 3.3907618290531647,
+            "angularVelocity": -0.0000010529870817317445,
+            "velocityX": 2.2785408260792437,
+            "velocityY": -3.019748492984228,
+            "timestamp": 0.6477032170146944
+        },
+        {
+            "x": 2.7430825233459473,
+            "y": 2.7833950519561768,
+            "heading": 3.3907617949090247,
+            "angularVelocity": -0.0000011597457911507082,
+            "velocityX": 2.4695978056269863,
+            "velocityY": -2.8656090504076794,
+            "timestamp": 0.6771442723335441
+        },
+        {
+            "x": 2.8743019890864545,
+            "y": 2.6600421338324565,
+            "heading": 3.3907617697237464,
+            "angularVelocity": -5.290207073739654e-7,
+            "velocityX": 2.7562853520145434,
+            "velocityY": -2.5910472919087915,
+            "timestamp": 0.7247516311451417
+        },
+        {
+            "x": 3.0177249974626466,
+            "y": 2.5511186165163586,
+            "heading": 3.390761748438384,
+            "angularVelocity": -4.4710235312140834e-7,
+            "velocityX": 3.0126226691923277,
+            "velocityY": -2.287955476529456,
+            "timestamp": 0.7723589899567394
+        },
+        {
+            "x": 3.171772290811784,
+            "y": 2.457823792449377,
+            "heading": 3.3907617293701717,
+            "angularVelocity": -4.005307751758197e-7,
+            "velocityX": 3.235787432753145,
+            "velocityY": -1.95967233629129,
+            "timestamp": 0.819966348768337
+        },
+        {
+            "x": 3.3347475520740515,
+            "y": 2.3811846546739925,
+            "heading": 3.3907617114029063,
+            "angularVelocity": -3.7740521255041523e-7,
+            "velocityX": 3.4233207917967023,
+            "velocityY": -1.6098170469543955,
+            "timestamp": 0.8675737075799346
+        },
+        {
+            "x": 3.5038153046646756,
+            "y": 2.3191315453490344,
+            "heading": 3.3907616937093077,
+            "angularVelocity": -3.7165679655198125e-7,
+            "velocityX": 3.551294522758467,
+            "velocityY": -1.3034352434994065,
+            "timestamp": 0.9151810663915322
+        },
+        {
+            "x": 3.6728836971491297,
+            "y": 2.257080179484392,
+            "heading": 3.3907616760157264,
+            "angularVelocity": -3.7165643530673955e-7,
+            "velocityX": 3.551307963828218,
+            "velocityY": -1.303398621843431,
+            "timestamp": 0.9627884252031298
+        },
+        {
+            "x": 3.8419520974106347,
+            "y": 2.195028834809519,
+            "heading": 3.3907616583221465,
+            "angularVelocity": -3.71656403732769e-7,
+            "velocityX": 3.5513081271863745,
+            "velocityY": -1.3033981767490248,
+            "timestamp": 1.0103957840147275
+        },
+        {
+            "x": 4.011020504671481,
+            "y": 2.1329775092054373,
+            "heading": 3.390761640628565,
+            "angularVelocity": -3.7165644168556193e-7,
+            "velocityX": 3.5513082742086395,
+            "velocityY": -1.3033977761640818,
+            "timestamp": 1.0580031428263252
+        },
+        {
+            "x": 4.180088918243229,
+            "y": 2.070926200796389,
+            "heading": 3.390761622934979,
+            "angularVelocity": -3.716565361157419e-7,
+            "velocityX": 3.55130840677009,
+            "velocityY": -1.3033974149797176,
+            "timestamp": 1.105610501637923
+        },
+        {
+            "x": 4.349157337505363,
+            "y": 2.008874907891698,
+            "heading": 3.3907616052413863,
+            "angularVelocity": -3.716566778801587e-7,
+            "velocityX": 3.551308526297583,
+            "velocityY": -1.3033970893082818,
+            "timestamp": 1.1532178604495207
+        },
+        {
+            "x": 4.518225761904485,
+            "y": 1.9468236289835406,
+            "heading": 3.390761587547785,
+            "angularVelocity": -3.716568562531517e-7,
+            "velocityX": 3.551308634200803,
+            "velocityY": -1.303396795308895,
+            "timestamp": 1.2008252192611184
+        },
+        {
+            "x": 4.687294190885785,
+            "y": 1.8847723625602526,
+            "heading": 3.3907615698541753,
+            "angularVelocity": -3.716570319734318e-7,
+            "velocityX": 3.5513087304501405,
+            "velocityY": -1.3033965330622725,
+            "timestamp": 1.2484325780727161
+        },
+        {
+            "x": 4.8563626239355635,
+            "y": 1.8227211072221874,
+            "heading": 3.390761552160558,
+            "angularVelocity": -3.716571921571891e-7,
+            "velocityX": 3.5513088159091875,
+            "velocityY": -1.3033963002154438,
+            "timestamp": 1.2960399368843138
+        },
+        {
+            "x": 5.025431060602707,
+            "y": 1.7606698617402123,
+            "heading": 3.3907615344669337,
+            "angularVelocity": -3.716573365191525e-7,
+            "velocityX": 3.551308891892495,
+            "velocityY": -1.3033960931867248,
+            "timestamp": 1.3436472956959116
+        },
+        {
+            "x": 5.1944995004938015,
+            "y": 1.6986186250424111,
+            "heading": 3.3907615167733036,
+            "angularVelocity": -3.7165746535883894e-7,
+            "velocityX": 3.551308959612086,
+            "velocityY": -1.3033959086737847,
+            "timestamp": 1.3912546545075093
+        },
+        {
+            "x": 5.363567943266909,
+            "y": 1.6365673961971205,
+            "heading": 3.3907614990796673,
+            "angularVelocity": -3.716575836762919e-7,
+            "velocityX": 3.551309020149218,
+            "velocityY": -1.3033957437305734,
+            "timestamp": 1.438862013319107
+        },
+        {
+            "x": 5.532636388625086,
+            "y": 1.574516174395274,
+            "heading": 3.390761481386026,
+            "angularVelocity": -3.716576912991928e-7,
+            "velocityX": 3.5513090744490245,
+            "velocityY": -1.3033955957819334,
+            "timestamp": 1.4864693721307047
+        },
+        {
+            "x": 5.701704836310244,
+            "y": 1.5124649589336634,
+            "heading": 3.3907614636923804,
+            "angularVelocity": -3.7165778948932697e-7,
+            "velocityX": 3.551309123327601,
+            "velocityY": -1.3033954626042876,
+            "timestamp": 1.5340767309423025
+        },
+        {
+            "x": 5.870773286097621,
+            "y": 1.4504137491998943,
+            "heading": 3.3907614459987303,
+            "angularVelocity": -3.716578803925389e-7,
+            "velocityX": 3.551309167485058,
+            "velocityY": -1.3033953422900804,
+            "timestamp": 1.5816840897539002
+        },
+        {
+            "x": 6.039841737790961,
+            "y": 1.388362544659232,
+            "heading": 3.3907614283050767,
+            "angularVelocity": -3.716579611998838e-7,
+            "velocityX": 3.551309207520092,
+            "velocityY": -1.3033952332080676,
+            "timestamp": 1.629291448565498
+        },
+        {
+            "x": 6.208910191218329,
+            "y": 1.3263113448432136,
+            "heading": 3.390761410611419,
+            "angularVelocity": -3.7165803844263857e-7,
+            "velocityX": 3.551309243943619,
+            "velocityY": -1.303395133966182,
+            "timestamp": 1.6768988073770956
+        },
+        {
+            "x": 6.377978646228594,
+            "y": 1.2642601493400643,
+            "heading": 3.3907613929177582,
+            "angularVelocity": -3.7165810831214996e-7,
+            "velocityX": 3.5513092771926584,
+            "velocityY": -1.3033950433736952,
+            "timestamp": 1.7245061661886933
+        },
+        {
+            "x": 6.547047102688449,
+            "y": 1.2022089577865573,
+            "heading": 3.390761375224094,
+            "angularVelocity": -3.716581745376039e-7,
+            "velocityX": 3.551309307641484,
+            "velocityY": -1.3033949604108395,
+            "timestamp": 1.772113525000291
+        },
+        {
+            "x": 6.716115560480058,
+            "y": 1.1401577698616354,
+            "heading": 3.3907613575304274,
+            "angularVelocity": -3.716582357126045e-7,
+            "velocityX": 3.5513093356151892,
+            "velocityY": -1.3033948841918472,
+            "timestamp": 1.8197208838118888
+        },
+        {
+            "x": 6.885184026346865,
+            "y": 1.0781066039393965,
+            "heading": 3.3907613398363963,
+            "angularVelocity": -3.7166587620925834e-7,
+            "velocityX": 3.5513095052359795,
+            "velocityY": -1.3033944220220548,
+            "timestamp": 1.8673282426234865
+        },
+        {
+            "x": 7.044084736711506,
+            "y": 1.0199905851581765,
+            "heading": 3.36350523152736,
+            "angularVelocity": -0.5725188077939848,
+            "velocityX": 3.337734214441043,
+            "velocityY": -1.220736042325084,
+            "timestamp": 1.9149356014350842
+        },
+        {
+            "x": 7.185551132214723,
+            "y": 0.9681799151008591,
+            "heading": 3.323163851941908,
+            "angularVelocity": -0.8473769726461788,
+            "velocityX": 2.9715237105057524,
+            "velocityY": -1.0882912085577785,
+            "timestamp": 1.962542960246682
+        },
+        {
+            "x": 7.3092683658418975,
+            "y": 0.9228556276324137,
+            "heading": 3.282206369227669,
+            "angularVelocity": -0.8603183149967459,
+            "velocityX": 2.598699795902896,
+            "velocityY": -0.952043730210126,
+            "timestamp": 2.0101503190582797
+        },
+        {
+            "x": 7.415256126479253,
+            "y": 0.8840204066687376,
+            "heading": 3.244221221054594,
+            "angularVelocity": -0.7978839641870918,
+            "velocityX": 2.226289449427224,
+            "velocityY": -0.815739875790283,
+            "timestamp": 2.057757677869877
+        },
+        {
+            "x": 7.503541109453659,
+            "y": 0.8516687069454374,
+            "heading": 3.210907204397792,
+            "angularVelocity": -0.6997661178524867,
+            "velocityX": 1.8544398424576745,
+            "velocityY": -0.6795525005142472,
+            "timestamp": 2.1053650366814747
+        },
+        {
+            "x": 7.574144027018988,
+            "y": 0.8257950136289538,
+            "heading": 3.1832598866385293,
+            "angularVelocity": -0.5807362233362916,
+            "velocityX": 1.4830252996124917,
+            "velocityY": -0.5434809651776052,
+            "timestamp": 2.152972395493072
+        },
+        {
+            "x": 7.627080539080189,
+            "y": 0.8063947169930168,
+            "heading": 3.161935751906697,
+            "angularVelocity": -0.44791677724069034,
+            "velocityX": 1.1119396955141558,
+            "velocityY": -0.407506257860514,
+            "timestamp": 2.2005797543046697
+        },
+        {
+            "x": 7.662362703782539,
+            "y": 0.7934640657363813,
+            "heading": 3.147400394630243,
+            "angularVelocity": -0.3053174475395025,
+            "velocityX": 0.7411073746387762,
+            "velocityY": -0.27161034721139543,
+            "timestamp": 2.248187113116267
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": -0.1554464438896817,
+            "velocityX": 0.370474158989983,
+            "velocityY": -0.13577870937899197,
+            "timestamp": 2.2957944719278647
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.14,
+            "angularVelocity": 1.845639156363882e-34,
+            "velocityX": 1.6232927575664752e-34,
+            "velocityY": 1.3237538522972492e-34,
+            "timestamp": 2.343401830739462
+        }
+    ]
+}
\ No newline at end of file

From 8b2364e9a1a05a2b66ad219381a52e666119d674 Mon Sep 17 00:00:00 2001
From: stoopipoopy <anotherguywithabeanie@gmail.com>
Date: Sun, 18 Feb 2024 19:11:40 -0800
Subject: [PATCH 35/62] update rotations

---
 src/main/java/frc/robot/utils/dynamicauto/ChoreoBuilder.py | 6 ++++--
 src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java | 6 +++---
 2 files changed, 7 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/utils/dynamicauto/ChoreoBuilder.py b/src/main/java/frc/robot/utils/dynamicauto/ChoreoBuilder.py
index f34e3980..f413d057 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/ChoreoBuilder.py
+++ b/src/main/java/frc/robot/utils/dynamicauto/ChoreoBuilder.py
@@ -111,5 +111,7 @@ def find_path(start, end_name, newPath, path_name):
 
 
 # Write the data to a JSON file
-with open('C:\\Users\\Robotics\\Desktop\\Crescendo\\src\\main\\java\\frc\\robot\\utils\\dynamicauto\\data.chor', 'w') as f:
-    json.dump(data, f, indent=4)
\ No newline at end of file
+# C:\\Users\\Robotics\\Desktop\\Crescendo\\src\\main\\java\\frc\\robot\\utils\\dynamicauto\\data.chor
+with open('C:\\Users\\stoopi_poopy\\Crescendo\\src\\main\\java\\frc\\robot\\utils\\dynamicauto\\data.chor', 'w') as f:
+    json.dump(data, f, indent=4)
+
diff --git a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
index f665e5d7..b57ad5d9 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
@@ -9,14 +9,14 @@
 public class DynamicAuto {
 
   public static final Note[] notes = {
-    new Note(new Pose2d(2.204, 7.0, Rotation2d.fromRadians(Math.PI)), true, 0, "W1"), // w1
+    new Note(new Pose2d(2.204, 7.0, Rotation2d.fromRadians(Math.PI)), false, 0, "W1"), // w1
     new Note(new Pose2d(2.204, 5.5, Rotation2d.fromRadians(Math.PI)), false, 1, "W2"), // w2
     new Note(new Pose2d(2.204, 4.1, Rotation2d.fromRadians(Math.PI)), false, 2, "W3"), // w3
-    new Note(new Pose2d(7.538, 7.3, Rotation2d.fromRadians(Math.PI)), true, 0, "C1"), // c1
+    new Note(new Pose2d(7.538, 7.3, Rotation2d.fromRadians(Math.PI)), false, 0, "C1"), // c1
     new Note(new Pose2d(7.538, 5.7, Rotation2d.fromRadians(Math.PI)), false, 0, "C2"), // c2
     new Note(new Pose2d(7.538, 4.1, Rotation2d.fromRadians(Math.PI)), false, 1, "C3"), // c3
     new Note(new Pose2d(7.538, 2.5, Rotation2d.fromRadians(Math.PI)), false, 2, "C4"), // c4
-    new Note(new Pose2d(7.538, 0.7, Rotation2d.fromRadians(Math.PI)), true, 3, "C5") // c5
+    new Note(new Pose2d(7.538, 0.7, Rotation2d.fromRadians(Math.PI)), false, 3, "C5") // c5
   };
 
   public static final ShootingLocation[] shootingLocations = {

From ef23b098a407a4b0102583a1c616baaac70a0b33 Mon Sep 17 00:00:00 2001
From: Awesomeplayer165 <dev@jacob2.dev>
Date: Sun, 18 Feb 2024 19:29:46 -0800
Subject: [PATCH 36/62] Choreo artifacts when opened

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

diff --git a/src/main/java/frc/robot/utils/dynamicauto/data.chor b/src/main/java/frc/robot/utils/dynamicauto/data.chor
index ff6fdd81..bce95e68 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/data.chor
+++ b/src/main/java/frc/robot/utils/dynamicauto/data.chor
@@ -2108,15 +2108,13 @@
                     "scope": [
                         "first"
                     ],
-                    "type": "StopPoint",
-                    "direction": 0
+                    "type": "StopPoint"
                 },
                 {
                     "scope": [
                         "last"
                     ],
-                    "type": "StopPoint",
-                    "direction": 0
+                    "type": "StopPoint"
                 }
             ],
             "usesControlIntervalGuessing": false,

From d953f0b4101d8e2689f6553f8bb95751d73a9a04 Mon Sep 17 00:00:00 2001
From: Awesomeplayer165 <dev@jacob2.dev>
Date: Sun, 18 Feb 2024 20:55:13 -0800
Subject: [PATCH 37/62] swapped headings and ran generation

---
 .../robot/utils/dynamicauto/SwapHeadings.py   |    26 +
 .../frc/robot/utils/dynamicauto/data.chor     | 46394 ++++++++--------
 .../deploy/choreo/Amp Side To C1.1.traj       |   398 +-
 .../deploy/choreo/Amp Side To C1.traj         |   398 +-
 .../deploy/choreo/Amp Side To C2.1.traj       |   400 +-
 .../deploy/choreo/Amp Side To C2.traj         |   400 +-
 .../deploy/choreo/Amp Side To C3.1.traj       |  1002 +-
 .../deploy/choreo/Amp Side To C3.traj         |  1002 +-
 .../deploy/choreo/Amp Side To C4.1.traj       |   551 +-
 .../deploy/choreo/Amp Side To C4.traj         |   551 +-
 .../deploy/choreo/Amp Side To C5.1.traj       |  1256 +-
 .../deploy/choreo/Amp Side To C5.traj         |  1256 +-
 .../deploy/choreo/Amp Side To W1.1.traj       |   168 +-
 .../deploy/choreo/Amp Side To W1.traj         |   168 +-
 .../deploy/choreo/Amp Side To W2.1.traj       |   210 +-
 .../deploy/choreo/Amp Side To W2.traj         |   210 +-
 .../deploy/choreo/Amp Side To W3.1.traj       |   346 +-
 .../deploy/choreo/Amp Side To W3.traj         |   346 +-
 .../dynamicauto/deploy/choreo/C1 To C4.1.traj |   175 -
 .../dynamicauto/deploy/choreo/C1 To C4.traj   |   175 -
 .../dynamicauto/deploy/choreo/C1 To C5.1.traj |   220 -
 .../dynamicauto/deploy/choreo/C1 To C5.traj   |   220 -
 .../dynamicauto/deploy/choreo/C1 To S1.1.traj |   180 +-
 .../dynamicauto/deploy/choreo/C1 To S1.traj   |   180 +-
 .../dynamicauto/deploy/choreo/C1 To S2.1.traj |   210 +-
 .../dynamicauto/deploy/choreo/C1 To S2.traj   |   210 +-
 .../dynamicauto/deploy/choreo/C1 To W1.1.traj |   364 +-
 .../dynamicauto/deploy/choreo/C1 To W1.traj   |   364 +-
 .../dynamicauto/deploy/choreo/C1 To W2.1.traj |   193 +
 .../dynamicauto/deploy/choreo/C1 To W2.traj   |   193 +
 .../dynamicauto/deploy/choreo/C2 To C1.1.traj |   124 +-
 .../dynamicauto/deploy/choreo/C2 To C1.traj   |   124 +-
 .../dynamicauto/deploy/choreo/C2 To C3.1.traj |    94 +
 .../dynamicauto/deploy/choreo/C2 To C3.traj   |    94 +
 .../dynamicauto/deploy/choreo/C2 To C4.1.traj |   176 +-
 .../dynamicauto/deploy/choreo/C2 To C4.traj   |   176 +-
 .../dynamicauto/deploy/choreo/C2 To C5.1.traj |   222 +-
 .../dynamicauto/deploy/choreo/C2 To C5.traj   |   222 +-
 .../dynamicauto/deploy/choreo/C2 To S1.1.traj |   180 +-
 .../dynamicauto/deploy/choreo/C2 To S1.traj   |   180 +-
 .../dynamicauto/deploy/choreo/C2 To S2.1.traj |   139 -
 .../dynamicauto/deploy/choreo/C2 To S2.traj   |   139 -
 .../dynamicauto/deploy/choreo/C2 To S3.1.traj |   346 +-
 .../dynamicauto/deploy/choreo/C2 To S3.traj   |   346 +-
 .../dynamicauto/deploy/choreo/C2 To W1.1.traj |   364 +-
 .../dynamicauto/deploy/choreo/C2 To W1.traj   |   364 +-
 .../dynamicauto/deploy/choreo/C2 To W2.1.traj |   184 -
 .../dynamicauto/deploy/choreo/C2 To W2.traj   |   184 -
 .../dynamicauto/deploy/choreo/C2 To W3.1.traj |   211 -
 .../dynamicauto/deploy/choreo/C2 To W3.traj   |   211 -
 .../dynamicauto/deploy/choreo/C3 To C1.1.traj |   139 +
 .../dynamicauto/deploy/choreo/C3 To C1.traj   |   139 +
 .../dynamicauto/deploy/choreo/C3 To C4.1.traj |    94 -
 .../dynamicauto/deploy/choreo/C3 To C4.traj   |    94 -
 .../dynamicauto/deploy/choreo/C3 To C5.1.traj |   180 +-
 .../dynamicauto/deploy/choreo/C3 To C5.traj   |   180 +-
 .../dynamicauto/deploy/choreo/C3 To S1.1.traj |   274 +-
 .../dynamicauto/deploy/choreo/C3 To S1.traj   |   274 +-
 .../dynamicauto/deploy/choreo/C3 To S2.1.traj |   148 +
 .../dynamicauto/deploy/choreo/C3 To S2.traj   |   148 +
 .../dynamicauto/deploy/choreo/C3 To S3.1.traj |   166 +
 .../dynamicauto/deploy/choreo/C3 To S3.traj   |   166 +
 .../dynamicauto/deploy/choreo/C3 To W2.1.traj |   346 +-
 .../dynamicauto/deploy/choreo/C3 To W2.traj   |   346 +-
 .../dynamicauto/deploy/choreo/C3 To W3.1.traj |   202 +
 .../dynamicauto/deploy/choreo/C3 To W3.traj   |   202 +
 .../dynamicauto/deploy/choreo/C4 To W1.1.traj |   436 +-
 .../dynamicauto/deploy/choreo/C4 To W1.traj   |   436 +-
 .../dynamicauto/deploy/choreo/C4 To W2.1.traj |   211 -
 .../dynamicauto/deploy/choreo/C4 To W2.traj   |   211 -
 .../deploy/choreo/Center To C1.1.traj         |   400 +-
 .../deploy/choreo/Center To C1.traj           |   400 +-
 .../deploy/choreo/Center To C2.1.traj         |   376 +-
 .../deploy/choreo/Center To C2.traj           |   376 +-
 .../deploy/choreo/Center To C4.1.traj         |  2130 +-
 .../deploy/choreo/Center To C4.traj           |  2130 +-
 .../deploy/choreo/Center To C5.1.traj         |   606 +-
 .../deploy/choreo/Center To C5.traj           |   606 +-
 .../deploy/choreo/Center To W1.1.traj         |   154 +-
 .../deploy/choreo/Center To W1.traj           |   154 +-
 .../deploy/choreo/Center To W2.1.traj         |    94 +-
 .../deploy/choreo/Center To W2.traj           |    94 +-
 .../deploy/choreo/Center To W3.1.traj         |   154 +-
 .../deploy/choreo/Center To W3.traj           |   154 +-
 .../dynamicauto/deploy/choreo/S1 To C1.1.traj |   180 +-
 .../dynamicauto/deploy/choreo/S1 To C1.traj   |   180 +-
 .../dynamicauto/deploy/choreo/S1 To C2.1.traj |   180 +-
 .../dynamicauto/deploy/choreo/S1 To C2.traj   |   180 +-
 .../dynamicauto/deploy/choreo/S1 To W1.1.traj |   180 +-
 .../dynamicauto/deploy/choreo/S1 To W1.traj   |   180 +-
 .../dynamicauto/deploy/choreo/S1 To W2.1.traj |   208 +-
 .../dynamicauto/deploy/choreo/S1 To W2.traj   |   208 +-
 .../dynamicauto/deploy/choreo/S2 To C1.1.traj |   208 +-
 .../dynamicauto/deploy/choreo/S2 To C1.traj   |   208 +-
 .../dynamicauto/deploy/choreo/S2 To C3.1.traj |   210 +-
 .../dynamicauto/deploy/choreo/S2 To C3.traj   |   210 +-
 .../dynamicauto/deploy/choreo/S2 To C4.1.traj |   166 +
 .../dynamicauto/deploy/choreo/S2 To C4.traj   |   166 +
 .../dynamicauto/deploy/choreo/S2 To C5.1.traj |   202 +
 .../dynamicauto/deploy/choreo/S2 To C5.traj   |   202 +
 .../dynamicauto/deploy/choreo/S2 To W1.1.traj |   182 +-
 .../dynamicauto/deploy/choreo/S2 To W1.traj   |   182 +-
 .../dynamicauto/deploy/choreo/S2 To W2.1.traj |   118 +-
 .../dynamicauto/deploy/choreo/S2 To W2.traj   |   118 +-
 .../dynamicauto/deploy/choreo/S2 To W3.1.traj |   180 +-
 .../dynamicauto/deploy/choreo/S2 To W3.traj   |   180 +-
 .../dynamicauto/deploy/choreo/S3 To C1.1.traj |   220 +
 .../dynamicauto/deploy/choreo/S3 To C1.traj   |   220 +
 .../dynamicauto/deploy/choreo/S3 To C2.1.traj |   346 +-
 .../dynamicauto/deploy/choreo/S3 To C2.traj   |   346 +-
 .../dynamicauto/deploy/choreo/S3 To C3.1.traj |   292 +-
 .../dynamicauto/deploy/choreo/S3 To C3.traj   |   292 +-
 .../dynamicauto/deploy/choreo/S3 To C5.1.traj |   328 +-
 .../dynamicauto/deploy/choreo/S3 To C5.traj   |   328 +-
 .../dynamicauto/deploy/choreo/S3 To W1.1.traj |   202 +
 .../dynamicauto/deploy/choreo/S3 To W1.traj   |   202 +
 .../dynamicauto/deploy/choreo/S3 To W2.1.traj |   157 +
 .../dynamicauto/deploy/choreo/S3 To W2.traj   |   157 +
 .../dynamicauto/deploy/choreo/S3 To W3.1.traj |   121 +
 .../dynamicauto/deploy/choreo/S3 To W3.traj   |   121 +
 .../dynamicauto/deploy/choreo/S4 To C2.1.traj |   202 +
 .../dynamicauto/deploy/choreo/S4 To C2.traj   |   202 +
 .../dynamicauto/deploy/choreo/S4 To C4.1.traj |   148 -
 .../dynamicauto/deploy/choreo/S4 To C4.traj   |   148 -
 .../dynamicauto/deploy/choreo/S4 To C5.1.traj |   208 +-
 .../dynamicauto/deploy/choreo/S4 To C5.traj   |   208 +-
 .../dynamicauto/deploy/choreo/S4 To W1.1.traj |   247 +
 .../dynamicauto/deploy/choreo/S4 To W1.traj   |   247 +
 .../dynamicauto/deploy/choreo/S4 To W2.1.traj |   202 +
 .../dynamicauto/deploy/choreo/S4 To W2.traj   |   202 +
 .../deploy/choreo/Stage Side To C1.1.traj     |  1090 +-
 .../deploy/choreo/Stage Side To C1.traj       |  1090 +-
 .../deploy/choreo/Stage Side To C2.1.traj     |  1074 +-
 .../deploy/choreo/Stage Side To C2.traj       |  1074 +-
 .../deploy/choreo/Stage Side To C3.1.traj     |   400 +-
 .../deploy/choreo/Stage Side To C3.traj       |   400 +-
 .../deploy/choreo/Stage Side To C4.1.traj     |  1203 +-
 .../deploy/choreo/Stage Side To C4.traj       |  1203 +-
 .../deploy/choreo/Stage Side To C5.1.traj     |   432 +-
 .../deploy/choreo/Stage Side To C5.traj       |   432 +-
 .../deploy/choreo/Stage Side To W1.1.traj     |   346 +-
 .../deploy/choreo/Stage Side To W1.traj       |   346 +-
 .../deploy/choreo/Stage Side To W2.1.traj     |   210 +-
 .../deploy/choreo/Stage Side To W2.traj       |   210 +-
 .../deploy/choreo/Stage Side To W3.1.traj     |   168 +-
 .../deploy/choreo/Stage Side To W3.traj       |   168 +-
 .../dynamicauto/deploy/choreo/W1 To C1.1.traj |   364 +-
 .../dynamicauto/deploy/choreo/W1 To C1.traj   |   364 +-
 .../dynamicauto/deploy/choreo/W1 To C2.1.traj |   364 +-
 .../dynamicauto/deploy/choreo/W1 To C2.traj   |   364 +-
 .../dynamicauto/deploy/choreo/W1 To C3.1.traj |   526 -
 .../dynamicauto/deploy/choreo/W1 To C3.traj   |   526 -
 .../dynamicauto/deploy/choreo/W1 To C4.1.traj |  1102 +-
 .../dynamicauto/deploy/choreo/W1 To C4.traj   |  1102 +-
 .../dynamicauto/deploy/choreo/W1 To C5.1.traj |   616 -
 .../dynamicauto/deploy/choreo/W1 To C5.traj   |   616 -
 .../dynamicauto/deploy/choreo/W1 To S1.1.traj |   180 +-
 .../dynamicauto/deploy/choreo/W1 To S1.traj   |   180 +-
 .../dynamicauto/deploy/choreo/W1 To S2.1.traj |   180 +-
 .../dynamicauto/deploy/choreo/W1 To S2.traj   |   180 +-
 .../dynamicauto/deploy/choreo/W1 To S3.1.traj |   544 -
 .../dynamicauto/deploy/choreo/W1 To S3.traj   |   544 -
 .../dynamicauto/deploy/choreo/W1 To S4.1.traj |  1074 +-
 .../dynamicauto/deploy/choreo/W1 To S4.traj   |  1074 +-
 .../dynamicauto/deploy/choreo/W1 To W2.1.traj |   140 +-
 .../dynamicauto/deploy/choreo/W1 To W2.traj   |   140 +-
 .../dynamicauto/deploy/choreo/W1 To W3.1.traj |   148 -
 .../dynamicauto/deploy/choreo/W1 To W3.traj   |   148 -
 .../dynamicauto/deploy/choreo/W2 To C1.1.traj |   346 +-
 .../dynamicauto/deploy/choreo/W2 To C1.traj   |   346 +-
 .../dynamicauto/deploy/choreo/W2 To C2.1.traj |  1002 +-
 .../dynamicauto/deploy/choreo/W2 To C2.traj   |  1002 +-
 .../dynamicauto/deploy/choreo/W2 To C3.1.traj |  1088 +-
 .../dynamicauto/deploy/choreo/W2 To C3.traj   |  1088 +-
 .../dynamicauto/deploy/choreo/W2 To C4.1.traj |  1118 +-
 .../dynamicauto/deploy/choreo/W2 To C4.traj   |  1118 +-
 .../dynamicauto/deploy/choreo/W2 To C5.1.traj |  1192 +-
 .../dynamicauto/deploy/choreo/W2 To C5.traj   |  1192 +-
 .../dynamicauto/deploy/choreo/W2 To S1.1.traj |   204 +-
 .../dynamicauto/deploy/choreo/W2 To S1.traj   |   204 +-
 .../dynamicauto/deploy/choreo/W2 To S2.1.traj |    66 +-
 .../dynamicauto/deploy/choreo/W2 To S2.traj   |    66 +-
 .../dynamicauto/deploy/choreo/W2 To S3.1.traj |   948 +-
 .../dynamicauto/deploy/choreo/W2 To S3.traj   |   948 +-
 .../dynamicauto/deploy/choreo/W2 To S4.1.traj |  1002 +-
 .../dynamicauto/deploy/choreo/W2 To S4.traj   |  1002 +-
 .../dynamicauto/deploy/choreo/W2 To W1.1.traj |   138 +-
 .../dynamicauto/deploy/choreo/W2 To W1.traj   |   138 +-
 .../dynamicauto/deploy/choreo/W2 To W3.1.traj |   136 +-
 .../dynamicauto/deploy/choreo/W2 To W3.traj   |   136 +-
 .../dynamicauto/deploy/choreo/W3 To C1.1.traj |  1020 +-
 .../dynamicauto/deploy/choreo/W3 To C1.traj   |  1020 +-
 .../dynamicauto/deploy/choreo/W3 To C2.1.traj |  1020 +-
 .../dynamicauto/deploy/choreo/W3 To C2.traj   |  1020 +-
 .../dynamicauto/deploy/choreo/W3 To C3.1.traj |  1122 +-
 .../dynamicauto/deploy/choreo/W3 To C3.traj   |  1122 +-
 .../dynamicauto/deploy/choreo/W3 To C4.1.traj |  1154 +-
 .../dynamicauto/deploy/choreo/W3 To C4.traj   |  1154 +-
 .../dynamicauto/deploy/choreo/W3 To C5.1.traj |  1056 +-
 .../dynamicauto/deploy/choreo/W3 To C5.traj   |  1056 +-
 .../dynamicauto/deploy/choreo/W3 To S1.1.traj |   310 +-
 .../dynamicauto/deploy/choreo/W3 To S1.traj   |   310 +-
 .../dynamicauto/deploy/choreo/W3 To S2.1.traj |   180 +-
 .../dynamicauto/deploy/choreo/W3 To S2.traj   |   180 +-
 .../dynamicauto/deploy/choreo/W3 To S3.1.traj |   166 +-
 .../dynamicauto/deploy/choreo/W3 To S3.traj   |   166 +-
 .../dynamicauto/deploy/choreo/W3 To S4.1.traj |   166 -
 .../dynamicauto/deploy/choreo/W3 To S4.traj   |   166 -
 .../dynamicauto/deploy/choreo/W3 To W1.1.traj |   208 +-
 .../dynamicauto/deploy/choreo/W3 To W1.traj   |   208 +-
 .../dynamicauto/deploy/choreo/W3 To W2.1.traj |   138 +-
 .../dynamicauto/deploy/choreo/W3 To W2.traj   |   138 +-
 212 files changed, 63944 insertions(+), 67378 deletions(-)
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/SwapHeadings.py
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C5.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.traj

diff --git a/src/main/java/frc/robot/utils/dynamicauto/SwapHeadings.py b/src/main/java/frc/robot/utils/dynamicauto/SwapHeadings.py
new file mode 100644
index 00000000..990a148d
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/SwapHeadings.py
@@ -0,0 +1,26 @@
+import json
+
+# open file in write mode
+with open('src/main/java/frc/robot/utils/dynamicauto/data.chor', 'r+') as file:
+    data = json.load(file)
+
+    """
+    Things to Change for each path
+    1. Clear Trajectory array
+    2. Reverse headings for each waypoint
+    """
+
+    for path in data["paths"]:
+        path = data["paths"][path]
+
+        waypoints = path["waypoints"]
+        path["trajectory"] = []
+
+        for waypoint in waypoints:
+            if waypoint["heading"] > 0:
+                waypoint["heading"] = round(waypoint["heading"] - 3.14, 3)
+            else:
+                waypoint["heading"] = round(waypoint["heading"] + 3.14, 3)
+
+    with open('src/main/java/frc/robot/utils/dynamicauto/data.chor', 'w') as file:
+        json.dump(data, file, indent=4)
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/data.chor b/src/main/java/frc/robot/utils/dynamicauto/data.chor
index bce95e68..a0af0f38 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/data.chor
+++ b/src/main/java/frc/robot/utils/dynamicauto/data.chor
@@ -18,7 +18,7 @@
                 {
                     "x": 0.71,
                     "y": 6.72,
-                    "heading": -2.1,
+                    "heading": 1.04,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -27,7 +27,7 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -38,119 +38,119 @@
                 {
                     "x": 0.71,
                     "y": 6.72,
-                    "heading": -2.1,
-                    "angularVelocity": -8.4685465984255e-32,
-                    "velocityX": -1.4242674774019307e-33,
-                    "velocityY": -4.454347432487034e-33,
+                    "heading": 1.04,
+                    "angularVelocity": 6.421381184701714e-32,
+                    "velocityX": 9.791473054297313e-34,
+                    "velocityY": 6.656933252086786e-33,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7541194456754691,
-                    "y": 6.720987374332373,
-                    "heading": -2.1237071075059206,
-                    "angularVelocity": -0.324046498361879,
-                    "velocityX": 0.6030576221600736,
-                    "velocityY": 0.013496171766179704,
-                    "timestamp": 0.07315958550932472
-                },
-                {
-                    "x": 0.8424134938705199,
-                    "y": 6.722973032705717,
-                    "heading": -2.1697540257675185,
-                    "angularVelocity": -0.6294037608468737,
-                    "velocityX": 1.2068691693694522,
-                    "velocityY": 0.027141465599130124,
-                    "timestamp": 0.14631917101864944
-                },
-                {
-                    "x": 0.9749625148381232,
-                    "y": 6.725970459573204,
-                    "heading": -2.2360020755513283,
-                    "angularVelocity": -0.9055279540276729,
-                    "velocityX": 1.811779277381365,
-                    "velocityY": 0.040971075035846904,
-                    "timestamp": 0.21947875652797416
-                },
-                {
-                    "x": 1.1518930933890432,
-                    "y": 6.729994686272588,
-                    "heading": -2.318545216019314,
-                    "angularVelocity": -1.1282614560115307,
-                    "velocityX": 2.418419641379311,
-                    "velocityY": 0.055006144053120966,
-                    "timestamp": 0.2926383420372989
-                },
-                {
-                    "x": 1.3734035763726509,
-                    "y": 6.735067561069991,
-                    "heading": -2.4074942006161044,
-                    "angularVelocity": -1.215821330554877,
-                    "velocityX": 3.0277711586456673,
-                    "velocityY": 0.0693398515327658,
-                    "timestamp": 0.3657979275466236
-                },
-                {
-                    "x": 1.6366126950009532,
-                    "y": 6.741789773246868,
-                    "heading": -2.44063463913809,
-                    "angularVelocity": -0.45298833080218254,
-                    "velocityX": 3.59773933649109,
-                    "velocityY": 0.09188422993442831,
-                    "timestamp": 0.4389575130559483
-                },
-                {
-                    "x": 1.858120407547834,
-                    "y": 6.746837523733308,
-                    "heading": -2.530120705195818,
-                    "angularVelocity": -1.2231625621549276,
-                    "velocityX": 3.027733290241823,
-                    "velocityY": 0.06899643363609939,
-                    "timestamp": 0.512117098565273
-                },
-                {
-                    "x": 2.0350481983019497,
-                    "y": 6.750889776022306,
-                    "heading": -2.613093703885261,
-                    "angularVelocity": -1.134137080080631,
-                    "velocityX": 2.418381535684925,
-                    "velocityY": 0.05538921879868258,
-                    "timestamp": 0.5852766840745978
-                },
-                {
-                    "x": 2.1675926697007295,
-                    "y": 6.753939244710426,
-                    "heading": -2.6797699231631307,
-                    "angularVelocity": -0.9113804953059623,
-                    "velocityX": 1.8117170904677296,
-                    "velocityY": 0.041682421611463606,
-                    "timestamp": 0.6584362695839225
-                },
-                {
-                    "x": 2.255882529656213,
-                    "y": 6.7559780839356005,
-                    "heading": -2.7261390096327216,
-                    "angularVelocity": -0.6338073971683945,
-                    "velocityX": 1.2068119213746804,
-                    "velocityY": 0.027868381306137554,
-                    "timestamp": 0.7315958550932473
+                    "x": 0.7541195074128881,
+                    "y": 6.720987207910964,
+                    "heading": 1.0162961446571532,
+                    "angularVelocity": -0.3240022105828226,
+                    "velocityX": 0.6030587735562275,
+                    "velocityY": 0.01349390387478939,
+                    "timestamp": 0.0731595482024364
+                },
+                {
+                    "x": 0.842413628953284,
+                    "y": 6.722972522190638,
+                    "heading": 0.9702543638919431,
+                    "angularVelocity": -0.6293338586206134,
+                    "velocityX": 1.2068707873384146,
+                    "velocityY": 0.027136776107259192,
+                    "timestamp": 0.1463190964048728
+                },
+                {
+                    "x": 0.9749626727340511,
+                    "y": 6.725969397195605,
+                    "heading": 0.9040114408742106,
+                    "angularVelocity": -0.9054583392778368,
+                    "velocityX": 1.811780513105384,
+                    "velocityY": 0.04096355265456321,
+                    "timestamp": 0.2194786446073092
+                },
+                {
+                    "x": 1.1518931623979327,
+                    "y": 6.729992809371002,
+                    "heading": 0.821470963167313,
+                    "angularVelocity": -1.1282256347246662,
+                    "velocityX": 2.418419659649969,
+                    "velocityY": 0.05499503857340178,
+                    "timestamp": 0.2926381928097456
+                },
+                {
+                    "x": 1.3734034405206448,
+                    "y": 6.735064554387548,
+                    "heading": 0.7325177503671355,
+                    "angularVelocity": -1.215879744828927,
+                    "velocityX": 3.027769902429938,
+                    "velocityY": 0.06932444419316391,
+                    "timestamp": 0.36579774101218204
+                },
+                {
+                    "x": 1.6366130242279704,
+                    "y": 6.741790766846534,
+                    "heading": 0.6993734477778637,
+                    "angularVelocity": -0.45304137879066375,
+                    "velocityX": 3.5977475281693874,
+                    "velocityY": 0.09193895566910278,
+                    "timestamp": 0.43895728921461846
+                },
+                {
+                    "x": 1.8581205064725355,
+                    "y": 6.746838576232807,
+                    "heading": 0.6098795976632514,
+                    "angularVelocity": -1.223269584265702,
+                    "velocityX": 3.0277316862542305,
+                    "velocityY": 0.06899727390750988,
+                    "timestamp": 0.5121168374170548
+                },
+                {
+                    "x": 2.0350481932432154,
+                    "y": 6.750890564225749,
+                    "heading": 0.5269052400796844,
+                    "angularVelocity": -1.134156232813029,
+                    "velocityX": 2.4183813475871814,
+                    "velocityY": 0.05538563444554272,
+                    "timestamp": 0.5852763856194912
+                },
+                {
+                    "x": 2.167592642728435,
+                    "y": 6.753939698452488,
+                    "heading": 0.4602292040948866,
+                    "angularVelocity": -0.9113784546659414,
+                    "velocityX": 1.8117177148013692,
+                    "velocityY": 0.04167787119598535,
+                    "timestamp": 0.6584359338219277
+                },
+                {
+                    "x": 2.255882514523324,
+                    "y": 6.755978249842539,
+                    "heading": 0.41386063175705684,
+                    "angularVelocity": -0.633800692829844,
+                    "velocityX": 1.2068126986047898,
+                    "velocityY": 0.02786446116929368,
+                    "timestamp": 0.7315954820243641
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
-                    "angularVelocity": -0.32614988454563837,
-                    "velocityX": 0.603030621847418,
-                    "velocityY": 0.013968314025900756,
-                    "timestamp": 0.8047554406025721
+                    "heading": 0.39,
+                    "angularVelocity": -0.3261451491065467,
+                    "velocityX": 0.6030311362038534,
+                    "velocityY": 0.013966053407445108,
+                    "timestamp": 0.8047550302268005
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
-                    "angularVelocity": -2.091519469848374e-32,
-                    "velocityX": -8.675585045811285e-34,
-                    "velocityY": 3.70068025566541e-32,
-                    "timestamp": 0.8779150261118969
+                    "heading": 0.39,
+                    "angularVelocity": -1.0168243354874858e-31,
+                    "velocityX": -4.086489815881427e-33,
+                    "velocityY": 0,
+                    "timestamp": 0.8779145784292369
                 }
             ],
             "constraints": [
@@ -193,7 +193,7 @@
                 {
                     "x": 0.71,
                     "y": 6.72,
-                    "heading": -2.1,
+                    "heading": 1.04,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -202,7 +202,7 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": -3.143185307179586,
+                    "heading": -0.003,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -213,146 +213,146 @@
                 {
                     "x": 0.71,
                     "y": 6.72,
-                    "heading": -2.1,
-                    "angularVelocity": -1.5217589356437866e-30,
-                    "velocityX": -1.7239506448843928e-31,
-                    "velocityY": 1.9880283347693254e-31,
+                    "heading": 1.04,
+                    "angularVelocity": 1.094777100362113e-30,
+                    "velocityX": 3.4815734426203924e-31,
+                    "velocityY": -2.2714927632651006e-31,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.737435952483546,
-                    "y": 6.69951466757637,
-                    "heading": -2.132336617905336,
-                    "angularVelocity": -0.4965864735710579,
-                    "velocityX": 0.4213280106395778,
-                    "velocityY": -0.3145888360360173,
-                    "timestamp": 0.06511779846276568
-                },
-                {
-                    "x": 0.7924253707255817,
-                    "y": 6.658399914346492,
-                    "heading": -2.1942155291625705,
-                    "angularVelocity": -0.9502611070707117,
-                    "velocityX": 0.8444606473217692,
-                    "velocityY": -0.6313904063170618,
-                    "timestamp": 0.13023559692553136
-                },
-                {
-                    "x": 0.8751335704497958,
-                    "y": 6.596485085823342,
-                    "heading": -2.281554213555253,
-                    "angularVelocity": -1.3412413572707518,
-                    "velocityX": 1.2701320019519164,
-                    "velocityY": -0.950812680784239,
-                    "timestamp": 0.19535339538829705
-                },
-                {
-                    "x": 0.9857847196454437,
-                    "y": 6.513575479492505,
-                    "heading": -2.388116786100485,
-                    "angularVelocity": -1.6364584654403693,
-                    "velocityX": 1.6992458560913724,
-                    "velocityY": -1.2732249598125673,
-                    "timestamp": 0.2604711938510627
-                },
-                {
-                    "x": 1.1246179227996527,
-                    "y": 6.409453213658469,
-                    "heading": -2.503336368742031,
-                    "angularVelocity": -1.7694023041554334,
-                    "velocityX": 2.132031586319575,
-                    "velocityY": -1.5989832010916685,
-                    "timestamp": 0.3255889923138284
-                },
-                {
-                    "x": 1.2913924480076884,
-                    "y": 6.28415287204147,
-                    "heading": -2.6022121669144127,
-                    "angularVelocity": -1.518414327672898,
-                    "velocityX": 2.5611204485575056,
-                    "velocityY": -1.9242103476309298,
-                    "timestamp": 0.39070679077659404
-                },
-                {
-                    "x": 1.4799082257130864,
-                    "y": 6.139182928400833,
-                    "heading": -2.6246781987943484,
-                    "angularVelocity": -0.3450060108033547,
-                    "velocityX": 2.8949961785515774,
-                    "velocityY": -2.2262721876804705,
-                    "timestamp": 0.4558245892393597
-                },
-                {
-                    "x": 1.6682193277242308,
-                    "y": 5.99414498496963,
-                    "heading": -2.64727359752809,
-                    "angularVelocity": -0.34699266970246656,
-                    "velocityX": 2.8918530180165907,
-                    "velocityY": -2.2273164458122023,
-                    "timestamp": 0.5209423877021254
-                },
-                {
-                    "x": 1.8350272177323106,
-                    "y": 5.869345925023006,
-                    "heading": -2.74292245957107,
-                    "angularVelocity": -1.4688589648446377,
-                    "velocityX": 2.5616328246026367,
-                    "velocityY": -1.9165122730305941,
-                    "timestamp": 0.5860601861648911
-                },
-                {
-                    "x": 1.9739579805969827,
-                    "y": 5.765588033107393,
-                    "heading": -2.8581454024211346,
-                    "angularVelocity": -1.7694539061536165,
-                    "velocityX": 2.133529789771879,
-                    "velocityY": -1.5933875893384517,
-                    "timestamp": 0.6511779846276567
-                },
-                {
-                    "x": 2.084683421379683,
-                    "y": 5.683004349697254,
-                    "heading": -2.964498621197243,
-                    "angularVelocity": -1.6332434647175749,
-                    "velocityX": 1.7003867359860632,
-                    "velocityY": -1.2682198317463163,
-                    "timestamp": 0.7162957830904224
-                },
-                {
-                    "x": 2.1674690836499857,
-                    "y": 5.621349763098201,
-                    "heading": -3.050914836384586,
-                    "angularVelocity": -1.327075196449642,
-                    "velocityX": 1.2713215775812727,
-                    "velocityY": -0.9468162016304505,
-                    "timestamp": 0.7814135815531881
-                },
-                {
-                    "x": 2.22252551206485,
-                    "y": 5.580407151857498,
-                    "heading": -3.1116606802742157,
-                    "angularVelocity": -0.9328608356494681,
-                    "velocityX": 0.8454897081071593,
-                    "velocityY": -0.6287468588808828,
-                    "timestamp": 0.8465313800159537
+                    "x": 0.7374353381803705,
+                    "y": 6.699514089758397,
+                    "heading": 1.0076617651006685,
+                    "angularVelocity": -0.49661202140938077,
+                    "velocityX": 0.42131918437160226,
+                    "velocityY": -0.3145981630464246,
+                    "timestamp": 0.06511770457661528
+                },
+                {
+                    "x": 0.7924238459364756,
+                    "y": 6.658398285396173,
+                    "heading": 0.9457835104036431,
+                    "angularVelocity": -0.9502523944808582,
+                    "velocityX": 0.8444478827015074,
+                    "velocityY": -0.6314074586865741,
+                    "timestamp": 0.13023540915323056
+                },
+                {
+                    "x": 0.8751312050328514,
+                    "y": 6.596482140589978,
+                    "heading": 0.858451871788505,
+                    "angularVelocity": -1.3411350904174961,
+                    "velocityX": 1.270120923858195,
+                    "velocityY": -0.9508342655621027,
+                    "timestamp": 0.19535311372984585
+                },
+                {
+                    "x": 0.9857818852205986,
+                    "y": 6.513571276882533,
+                    "heading": 0.7519056244975073,
+                    "angularVelocity": -1.6362101210990878,
+                    "velocityX": 1.6992411035858814,
+                    "velocityY": -1.2732461048269026,
+                    "timestamp": 0.2604708183064611
+                },
+                {
+                    "x": 1.1246151400642461,
+                    "y": 6.409448224375173,
+                    "heading": 0.63671471411989,
+                    "angularVelocity": -1.7689645408506078,
+                    "velocityX": 2.1320354540492286,
+                    "velocityY": -1.5989975872821531,
+                    "timestamp": 0.3255885228830764
+                },
+                {
+                    "x": 1.2913898302664586,
+                    "y": 6.284148790385846,
+                    "heading": 0.5378900355869111,
+                    "angularVelocity": -1.5176314824903545,
+                    "velocityX": 2.5611266749427672,
+                    "velocityY": -1.9241991836782961,
+                    "timestamp": 0.39070622745969164
+                },
+                {
+                    "x": 1.4799020803988825,
+                    "y": 6.139172930172668,
+                    "heading": 0.5154397477236047,
+                    "angularVelocity": -0.3447647304104786,
+                    "velocityX": 2.8949461802761682,
+                    "velocityY": -2.2263662571613483,
+                    "timestamp": 0.4558239320363069
+                },
+                {
+                    "x": 1.668209892858901,
+                    "y": 5.994133297119725,
+                    "heading": 0.4928531841519364,
+                    "angularVelocity": -0.3468574901176696,
+                    "velocityX": 2.891806670464904,
+                    "velocityY": -2.2273456043324518,
+                    "timestamp": 0.5209416366129221
+                },
+                {
+                    "x": 1.8350205833080036,
+                    "y": 5.869338176931447,
+                    "heading": 0.3972353467498499,
+                    "angularVelocity": -1.4683846432207315,
+                    "velocityX": 2.5616795237743473,
+                    "velocityY": -1.9164545341343717,
+                    "timestamp": 0.5860593411895374
+                },
+                {
+                    "x": 1.9739536541090807,
+                    "y": 5.765582598648054,
+                    "heading": 0.2820330218741829,
+                    "angularVelocity": -1.7691398310903903,
+                    "velocityX": 2.1335683084100903,
+                    "velocityY": -1.5933543566683568,
+                    "timestamp": 0.6511770457661527
+                },
+                {
+                    "x": 2.084680900392731,
+                    "y": 5.683000749842365,
+                    "heading": 0.17568947614699124,
+                    "angularVelocity": -1.6330972723719295,
+                    "velocityX": 1.7004169143181675,
+                    "velocityY": -1.2681934865889841,
+                    "timestamp": 0.7162947503427679
+                },
+                {
+                    "x": 2.1674678561101657,
+                    "y": 5.621347760780931,
+                    "heading": 0.08927495882603552,
+                    "angularVelocity": -1.3270510360094632,
+                    "velocityX": 1.271343273779417,
+                    "velocityY": -0.9467930336656067,
+                    "timestamp": 0.7814124549193832
+                },
+                {
+                    "x": 2.222525102580128,
+                    "y": 5.580406412641355,
+                    "heading": 0.02852690376362466,
+                    "angularVelocity": -0.9328961371931729,
+                    "velocityX": 0.8455034898409743,
+                    "velocityY": -0.6287283682029188,
+                    "timestamp": 0.8465301594959984
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -0.48411690274504937,
-                    "velocityX": 0.42191979126659057,
-                    "velocityY": -0.3133882339275879,
-                    "timestamp": 0.9116491784787194
+                    "heading": -0.003,
+                    "angularVelocity": -0.4841525660126885,
+                    "velocityX": 0.42192668796465593,
+                    "velocityY": -0.3133773337686722,
+                    "timestamp": 0.9116478640726137
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -1.0063297310782738e-31,
-                    "velocityX": -3.625667697405941e-32,
-                    "velocityY": 0,
-                    "timestamp": 0.9767669769414851
+                    "heading": -0.003,
+                    "angularVelocity": 5.587549482799738e-30,
+                    "velocityX": -2.981373275826657e-31,
+                    "velocityY": -1.0757932056005479e-30,
+                    "timestamp": 0.9767655686492289
                 }
             ],
             "constraints": [
@@ -395,7 +395,7 @@
                 {
                     "x": 0.71,
                     "y": 6.72,
-                    "heading": -2.1,
+                    "heading": 1.04,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -404,7 +404,7 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": -3.5331853071795862,
+                    "heading": -0.393,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -415,191 +415,191 @@
                 {
                     "x": 0.71,
                     "y": 6.72,
-                    "heading": -2.1,
-                    "angularVelocity": 2.001911519996272e-31,
-                    "velocityX": 6.781624829737293e-33,
-                    "velocityY": -4.0943813533671796e-32,
+                    "heading": 1.04,
+                    "angularVelocity": 1.9442039286246259e-31,
+                    "velocityX": 1.5017390020853034e-32,
+                    "velocityY": 2.127944688535994e-32,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7269029739475109,
-                    "y": 6.6955056066592755,
-                    "heading": -2.1380447151842032,
-                    "angularVelocity": -0.6185663850269976,
-                    "velocityX": 0.27482428085724375,
-                    "velocityY": -0.3982526421565261,
-                    "timestamp": 0.061504659976863815
-                },
-                {
-                    "x": 0.7606767634590716,
-                    "y": 6.646279647618601,
-                    "heading": -2.2124384537514556,
-                    "angularVelocity": -1.2095626346887691,
-                    "velocityX": 0.5491257007886129,
-                    "velocityY": -0.8003614532490962,
-                    "timestamp": 0.12300931995372763
-                },
-                {
-                    "x": 0.8112924785066278,
-                    "y": 6.571981628339174,
-                    "heading": -2.320229893429466,
-                    "angularVelocity": -1.752573540257899,
-                    "velocityX": 0.8229573997579402,
-                    "velocityY": -1.208006341428056,
-                    "timestamp": 0.18451397993059143
-                },
-                {
-                    "x": 0.8787853408643597,
-                    "y": 6.472118688114863,
-                    "heading": -2.4555147279599576,
-                    "angularVelocity": -2.199586739954034,
-                    "velocityX": 1.0973617671103395,
-                    "velocityY": -1.6236646176383343,
-                    "timestamp": 0.24601863990745526
-                },
-                {
-                    "x": 0.9632675519056847,
-                    "y": 6.346052032301826,
-                    "heading": -2.6067469489099335,
-                    "angularVelocity": -2.4588741894819854,
-                    "velocityX": 1.37359040880975,
-                    "velocityY": -2.049709011649839,
-                    "timestamp": 0.3075232998843191
-                },
-                {
-                    "x": 1.0643460613698084,
-                    "y": 6.193349880124978,
-                    "heading": -2.74665413649709,
-                    "angularVelocity": -2.274741257650809,
-                    "velocityX": 1.643428473584707,
-                    "velocityY": -2.482773699330916,
-                    "timestamp": 0.3690279598611829
-                },
-                {
-                    "x": 1.1777092396516182,
-                    "y": 6.018032409257481,
-                    "heading": -2.813593357981312,
-                    "angularVelocity": -1.0883601585538758,
-                    "velocityX": 1.8431640517068764,
-                    "velocityY": -2.850474597102823,
-                    "timestamp": 0.43053261983804675
-                },
-                {
-                    "x": 1.3085884905177934,
-                    "y": 5.825664975179578,
-                    "heading": -2.813593427712779,
-                    "angularVelocity": -0.0000011337590858279882,
-                    "velocityX": 2.1279566607702214,
-                    "velocityY": -3.1276887661888684,
-                    "timestamp": 0.4920372798149106
-                },
-                {
-                    "x": 1.4394691966966826,
-                    "y": 5.633298531244646,
-                    "heading": -2.8135934974374672,
-                    "angularVelocity": -0.000001133648866416165,
-                    "velocityX": 2.127980322598685,
-                    "velocityY": -3.127672667522972,
-                    "timestamp": 0.5535419397917744
-                },
-                {
-                    "x": 1.5703499198247641,
-                    "y": 5.440932098841455,
-                    "heading": -2.813593567162125,
-                    "angularVelocity": -0.0000011336483749077044,
-                    "velocityX": 2.1279805981744278,
-                    "velocityY": -3.1276724800292155,
-                    "timestamp": 0.6150465997686382
-                },
-                {
-                    "x": 1.7012306619349875,
-                    "y": 5.248565679353167,
-                    "heading": -2.81359363688675,
-                    "angularVelocity": -0.0000011336478448323905,
-                    "velocityX": 2.1279809068037574,
-                    "velocityY": -3.1276722700467015,
-                    "timestamp": 0.676551259745502
-                },
-                {
-                    "x": 1.8321095906543323,
-                    "y": 5.056198026100399,
-                    "heading": -2.8135937066202716,
-                    "angularVelocity": -0.0000011337924880961462,
-                    "velocityX": 2.1279514230072554,
-                    "velocityY": -3.127692329737781,
-                    "timestamp": 0.7380559197223658
-                },
-                {
-                    "x": 1.945432077761735,
-                    "y": 4.881276454017442,
-                    "heading": -2.879360901618447,
-                    "angularVelocity": -1.0693042612204542,
-                    "velocityX": 1.8425024567249244,
-                    "velocityY": -2.8440377062283995,
-                    "timestamp": 0.7995605796992297
-                },
-                {
-                    "x": 2.046464590922967,
-                    "y": 4.730069111144926,
-                    "heading": -3.0091287979589167,
-                    "angularVelocity": -2.109887224631182,
-                    "velocityX": 1.6426806228867414,
-                    "velocityY": -2.458469698546365,
-                    "timestamp": 0.8610652396760935
-                },
-                {
-                    "x": 2.131137367212944,
-                    "y": 4.6049715874663555,
-                    "heading": -3.1629399624477488,
-                    "angularVelocity": -2.500805053579533,
-                    "velocityX": 1.3766887959681082,
-                    "velocityY": -2.0339519595039928,
-                    "timestamp": 0.9225698996529573
-                },
-                {
-                    "x": 2.1987047903626262,
-                    "y": 4.5060589736155245,
-                    "heading": -3.302229643809037,
-                    "angularVelocity": -2.2647012667606727,
-                    "velocityX": 1.0985740458544027,
-                    "velocityY": -1.6082133270558499,
-                    "timestamp": 0.9840745596298212
-                },
-                {
-                    "x": 2.2493460059898798,
-                    "y": 4.432630316522509,
-                    "heading": -3.4143130823982606,
-                    "angularVelocity": -1.8223568528203613,
-                    "velocityX": 0.8233720119142732,
-                    "velocityY": -1.1938714419466272,
-                    "timestamp": 1.0455792196066849
-                },
-                {
-                    "x": 2.2831132091553847,
-                    "y": 4.384103436110321,
-                    "heading": -3.4927231006079147,
-                    "angularVelocity": -1.2748630467862023,
-                    "velocityX": 0.5490186138449845,
-                    "velocityY": -0.7889951823234699,
-                    "timestamp": 1.1070838795835487
+                    "x": 0.7269018921676205,
+                    "y": 6.6955064720122275,
+                    "heading": 1.0019343004430092,
+                    "angularVelocity": -0.6189082678684702,
+                    "velocityX": 0.2748070028110285,
+                    "velocityY": -0.3982390224617833,
+                    "timestamp": 0.06150459047524045
+                },
+                {
+                    "x": 0.7606735160812093,
+                    "y": 6.646281374376347,
+                    "heading": 0.9275074281520226,
+                    "angularVelocity": -1.2101027210472746,
+                    "velocityX": 0.5490911109664911,
+                    "velocityY": -0.8003483521396102,
+                    "timestamp": 0.1230091809504809
+                },
+                {
+                    "x": 0.8112864226090604,
+                    "y": 6.571983368952451,
+                    "heading": 0.8196845896852679,
+                    "angularVelocity": -1.7530860320118746,
+                    "velocityX": 0.8229126661403596,
+                    "velocityY": -1.2080074812270452,
+                    "timestamp": 0.18451377142572134
+                },
+                {
+                    "x": 0.8787769249457482,
+                    "y": 6.472119220055144,
+                    "heading": 0.6843882308491542,
+                    "angularVelocity": -2.199776598635823,
+                    "velocityX": 1.0973246356929591,
+                    "velocityY": -1.6236861041698774,
+                    "timestamp": 0.2460183619009618
+                },
+                {
+                    "x": 0.9632583450726888,
+                    "y": 6.346050730942796,
+                    "heading": 0.5331821377651369,
+                    "angularVelocity": -2.458452156427049,
+                    "velocityX": 1.373579101562342,
+                    "velocityY": -2.0497411353888846,
+                    "timestamp": 0.30752295237620225
+                },
+                {
+                    "x": 1.064336906516991,
+                    "y": 6.193349961542198,
+                    "heading": 0.3933800445651471,
+                    "angularVelocity": -2.273035103880077,
+                    "velocityX": 1.6434311758403277,
+                    "velocityY": -2.4827540224346163,
+                    "timestamp": 0.3690275428514427
+                },
+                {
+                    "x": 1.1776989215840556,
+                    "y": 6.018028084645807,
+                    "heading": 0.32650139843607295,
+                    "angularVelocity": -1.0873764968160713,
+                    "velocityX": 1.8431472218760627,
+                    "velocityY": -2.8505494556047704,
+                    "timestamp": 0.4305321333266831
+                },
+                {
+                    "x": 1.3085726451154573,
+                    "y": 5.825657208090679,
+                    "heading": 0.3265013287499931,
+                    "angularVelocity": -0.0000011330224187482564,
+                    "velocityX": 2.1278691967567314,
+                    "velocityY": -3.1277482716118437,
+                    "timestamp": 0.49203672380192354
+                },
+                {
+                    "x": 1.4394478126055321,
+                    "y": 5.633287313894505,
+                    "heading": 0.3265012590706379,
+                    "angularVelocity": -0.000001132913083706052,
+                    "velocityX": 2.1278926740071626,
+                    "velocityY": -3.127732299487714,
+                    "timestamp": 0.553541314277164
+                },
+                {
+                    "x": 1.570322997049186,
+                    "y": 5.440917431232361,
+                    "heading": 0.3265011893913128,
+                    "angularVelocity": -0.0000011329125928412157,
+                    "velocityX": 2.1278929496545396,
+                    "velocityY": -3.1277321119565196,
+                    "timestamp": 0.6150459047524045
+                },
+                {
+                    "x": 1.7011982004807327,
+                    "y": 5.248547561488253,
+                    "heading": 0.32650111971202045,
+                    "angularVelocity": -0.0000011329120610367875,
+                    "velocityX": 2.1278932583777177,
+                    "velocityY": -3.12773190192281,
+                    "timestamp": 0.676550495227645
+                },
+                {
+                    "x": 1.8320716066458687,
+                    "y": 5.056176469023738,
+                    "heading": 0.3265010500238972,
+                    "angularVelocity": -0.0000011330556420647947,
+                    "velocityX": 2.1278640367148025,
+                    "velocityY": -3.127751782071573,
+                    "timestamp": 0.7380550857028855
+                },
+                {
+                    "x": 1.9453996822937256,
+                    "y": 4.881259566133339,
+                    "heading": 0.2607759086629828,
+                    "angularVelocity": -1.0686217216156084,
+                    "velocityX": 1.8425954025899711,
+                    "velocityY": -2.843965003893719,
+                    "timestamp": 0.799559676178126
+                },
+                {
+                    "x": 2.0464428806975716,
+                    "y": 4.730058134054239,
+                    "heading": 0.1310608851796909,
+                    "angularVelocity": -2.109029951764503,
+                    "velocityX": 1.6428562099689532,
+                    "velocityY": -2.458376373385783,
+                    "timestamp": 0.8610642666533664
+                },
+                {
+                    "x": 2.1311236772079067,
+                    "y": 4.60496312733231,
+                    "heading": -0.022718365147026284,
+                    "angularVelocity": -2.500288988813334,
+                    "velocityX": 1.3768207520124016,
+                    "velocityY": -2.0339133348475604,
+                    "timestamp": 0.9225688571286069
+                },
+                {
+                    "x": 2.1986971663058994,
+                    "y": 4.506052833762169,
+                    "heading": -0.16200277924985632,
+                    "angularVelocity": -2.264618185839331,
+                    "velocityX": 1.0986739132129535,
+                    "velocityY": -1.6081774190490494,
+                    "timestamp": 0.9840734476038474
+                },
+                {
+                    "x": 2.2493424732278595,
+                    "y": 4.432626695040595,
+                    "heading": -0.27409854656488936,
+                    "angularVelocity": -1.822559364250361,
+                    "velocityX": 0.823439462495856,
+                    "velocityY": -1.1938318449763972,
+                    "timestamp": 1.0455780380790878
+                },
+                {
+                    "x": 2.2831120594312195,
+                    "y": 4.384102039370989,
+                    "heading": -0.35252594149925776,
+                    "angularVelocity": -1.2751470146921873,
+                    "velocityX": 0.5490579799398,
+                    "velocityY": -0.7889599019302513,
+                    "timestamp": 1.1070826285543283
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": -3.5331853071795862,
-                    "angularVelocity": -0.6578722097950352,
-                    "velocityX": 0.2745611609098736,
-                    "velocityY": -0.39189609566798916,
-                    "timestamp": 1.1685885395604125
+                    "heading": -0.393,
+                    "angularVelocity": -0.6580656531163417,
+                    "velocityX": 0.2745801644769678,
+                    "velocityY": -0.3918738290061591,
+                    "timestamp": 1.1685872190295687
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": -3.5331853071795862,
-                    "angularVelocity": 8.150679561734844e-31,
+                    "heading": -0.393,
+                    "angularVelocity": -1.5278905371426354e-30,
                     "velocityX": 0,
-                    "velocityY": -5.044788235947052e-32,
-                    "timestamp": 1.2300931995372764
+                    "velocityY": -1.0222462264365078e-31,
+                    "timestamp": 1.2300918095048092
                 }
             ],
             "constraints": [
@@ -642,7 +642,7 @@
                 {
                     "x": 0.71,
                     "y": 6.72,
-                    "heading": -2.1,
+                    "heading": 1.04,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -651,7 +651,7 @@
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": -3.143185307179586,
+                    "heading": -0.003,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -662,272 +662,272 @@
                 {
                     "x": 0.71,
                     "y": 6.72,
-                    "heading": -2.1,
+                    "heading": 1.04,
                     "angularVelocity": 0,
-                    "velocityX": -2.7669422223227046e-34,
+                    "velocityX": -1.8357629106361847e-33,
                     "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7613814086171108,
-                    "y": 6.725487327512705,
-                    "heading": -2.1469081960592686,
-                    "angularVelocity": -0.5864829148967596,
-                    "velocityX": 0.642410513062366,
-                    "velocityY": 0.06860685562450702,
-                    "timestamp": 0.07998220385929865
-                },
-                {
-                    "x": 0.8643773558424356,
-                    "y": 6.73664563014898,
-                    "heading": -2.237067397875382,
-                    "angularVelocity": -1.1272407793953514,
-                    "velocityX": 1.2877357994099667,
-                    "velocityY": 0.13950981715763813,
-                    "timestamp": 0.1599644077185973
-                },
-                {
-                    "x": 1.019344149471572,
-                    "y": 6.7536581703311604,
-                    "heading": -2.3645059003180053,
-                    "angularVelocity": -1.5933357208661998,
-                    "velocityX": 1.9375159241891806,
-                    "velocityY": 0.21270406867141206,
-                    "timestamp": 0.23994661157789593
-                },
-                {
-                    "x": 1.2269206708549203,
-                    "y": 6.776747585260753,
-                    "heading": -2.5146569204728233,
-                    "angularVelocity": -1.8773053618147075,
-                    "velocityX": 2.59528384274717,
-                    "velocityY": 0.28868190441726393,
-                    "timestamp": 0.3199288154371946
-                },
-                {
-                    "x": 1.4861241127588112,
-                    "y": 6.80668700452381,
-                    "heading": -2.6232938505110392,
-                    "angularVelocity": -1.3582637736430165,
-                    "velocityX": 3.240763937436266,
-                    "velocityY": 0.37432601026755163,
-                    "timestamp": 0.39991101929649325
-                },
-                {
-                    "x": 1.7870157378738414,
-                    "y": 6.83849222949431,
-                    "heading": -2.623293892215126,
-                    "angularVelocity": -5.214170797715324e-7,
-                    "velocityX": 3.7619821734888195,
-                    "velocityY": 0.39765377091197274,
-                    "timestamp": 0.4798932231557919
-                },
-                {
-                    "x": 2.0879074285185357,
-                    "y": 6.870296834526242,
-                    "heading": -2.623293933917092,
-                    "angularVelocity": -5.213905536622117e-7,
-                    "velocityX": 3.7619829927918795,
-                    "velocityY": 0.3976460199556515,
-                    "timestamp": 0.5598754270150905
-                },
-                {
-                    "x": 2.3887991234818986,
-                    "y": 6.9021013987008635,
-                    "heading": -2.623293975619048,
-                    "angularVelocity": -5.213904366102477e-7,
-                    "velocityX": 3.7619830467872393,
-                    "velocityY": 0.3976455091256453,
-                    "timestamp": 0.6398576308743892
-                },
-                {
-                    "x": 2.689690823126622,
-                    "y": 6.933905918586809,
-                    "heading": -2.623294017320994,
-                    "angularVelocity": -5.213903027395266e-7,
-                    "velocityX": 3.761983105317272,
-                    "velocityY": 0.39764495539400585,
-                    "timestamp": 0.7198398347336878
-                },
-                {
-                    "x": 2.9905825278407376,
-                    "y": 6.965710390512973,
-                    "heading": -2.62329405902293,
-                    "angularVelocity": -5.213901895916786e-7,
-                    "velocityX": 3.7619831686987752,
-                    "velocityY": 0.39764435576335205,
-                    "timestamp": 0.7998220385929865
-                },
-                {
-                    "x": 3.2914742379996778,
-                    "y": 6.997514810927433,
-                    "heading": -2.623294100724858,
-                    "angularVelocity": -5.21390082509775e-7,
-                    "velocityX": 3.761983236774224,
-                    "velocityY": 0.39764371172379265,
-                    "timestamp": 0.8798042424522852
-                },
-                {
-                    "x": 3.5923659539723536,
-                    "y": 7.029319176339941,
-                    "heading": -2.6232941424267775,
-                    "angularVelocity": -5.213899786160023e-7,
-                    "velocityX": 3.7619833094620927,
-                    "velocityY": 0.3976430240464025,
-                    "timestamp": 0.9597864463115838
-                },
-                {
-                    "x": 3.8932576761280617,
-                    "y": 7.0611234832565914,
-                    "heading": -2.6232941841286896,
-                    "angularVelocity": -5.213898862684442e-7,
-                    "velocityX": 3.761983386767183,
-                    "velocityY": 0.39764229268549517,
-                    "timestamp": 1.0397686501708825
-                },
-                {
-                    "x": 4.194149404850362,
-                    "y": 7.092927728048495,
-                    "heading": -2.623294225830597,
-                    "angularVelocity": -5.213898264324783e-7,
-                    "velocityX": 3.7619834688678555,
-                    "velocityY": 0.3976415159533747,
-                    "timestamp": 1.119750854030181
-                },
-                {
-                    "x": 4.49504114052058,
-                    "y": 7.124731907107895,
-                    "heading": -2.623294267532503,
-                    "angularVelocity": -5.213898089255833e-7,
-                    "velocityX": 3.761983555736148,
-                    "velocityY": 0.3976406941142751,
-                    "timestamp": 1.1997330578894796
-                },
-                {
-                    "x": 4.795932883453666,
-                    "y": 7.15653601745498,
-                    "heading": -2.6232943092344048,
-                    "angularVelocity": -5.213897595490136e-7,
-                    "velocityX": 3.761983646542196,
-                    "velocityY": 0.39763983501921824,
-                    "timestamp": 1.2797152617487781
-                },
-                {
-                    "x": 5.0968246339465235,
-                    "y": 7.188340056280654,
-                    "heading": -2.6232943509362996,
-                    "angularVelocity": -5.21389667495749e-7,
-                    "velocityX": 3.7619837410603636,
-                    "velocityY": 0.3976389408026617,
-                    "timestamp": 1.3596974656080767
-                },
-                {
-                    "x": 5.3977163922506195,
-                    "y": 7.220144021205662,
-                    "heading": -2.6232943926381824,
-                    "angularVelocity": -5.213895176915956e-7,
-                    "velocityX": 3.7619838387225806,
-                    "velocityY": 0.39763801683879874,
-                    "timestamp": 1.4396796694673752
-                },
-                {
-                    "x": 5.698608158576608,
-                    "y": 7.251947910236858,
-                    "heading": -2.623294434340057,
-                    "angularVelocity": -5.213894134163355e-7,
-                    "velocityX": 3.7619839390185463,
-                    "velocityY": 0.3976370679550778,
-                    "timestamp": 1.5196618733266738
-                },
-                {
-                    "x": 5.999499932773201,
-                    "y": 7.283751724805354,
-                    "heading": -2.62329447604193,
-                    "angularVelocity": -5.213893967687742e-7,
-                    "velocityX": 3.761984037422991,
-                    "velocityY": 0.3976361369642237,
-                    "timestamp": 1.5996440771859723
-                },
-                {
-                    "x": 6.300391714248171,
-                    "y": 7.315555470513951,
-                    "heading": -2.6232945177438096,
-                    "angularVelocity": -5.213894781911953e-7,
-                    "velocityX": 3.7619841284229465,
-                    "velocityY": 0.39763527602395277,
-                    "timestamp": 1.679626281045271
-                },
-                {
-                    "x": 6.601283502174795,
-                    "y": 7.347359155184038,
-                    "heading": -2.6232945594457013,
-                    "angularVelocity": -5.213896319500141e-7,
-                    "velocityX": 3.76198420908656,
-                    "velocityY": 0.39763451287282225,
-                    "timestamp": 1.7596084849045694
-                },
-                {
-                    "x": 6.902175248741792,
-                    "y": 7.379163231145718,
-                    "heading": -2.6232946011491864,
-                    "angularVelocity": -5.214095570695983e-7,
-                    "velocityX": 3.7619836919762037,
-                    "velocityY": 0.39763940510601253,
-                    "timestamp": 1.839590688763868
-                },
-                {
-                    "x": 7.162101881336522,
-                    "y": 7.409614310010791,
-                    "heading": -2.734801003168492,
-                    "angularVelocity": -1.3941401541705845,
-                    "velocityX": 3.249805832457199,
-                    "velocityY": 0.3807231783540282,
-                    "timestamp": 1.9195728926231665
-                },
-                {
-                    "x": 7.370045087720824,
-                    "y": 7.432975674976622,
-                    "heading": -2.8811468250983516,
-                    "angularVelocity": -1.8297298007354832,
-                    "velocityX": 2.5998684251074975,
-                    "velocityY": 0.29208203623556095,
-                    "timestamp": 1.999555096482465
-                },
-                {
-                    "x": 7.525332087472029,
-                    "y": 7.4501357007114875,
-                    "heading": -3.00655027342712,
-                    "angularVelocity": -1.5678918844168468,
-                    "velocityX": 1.9415193912933517,
-                    "velocityY": 0.21454804827649476,
-                    "timestamp": 2.0795373003417637
-                },
-                {
-                    "x": 7.62852758064907,
-                    "y": 7.461415013154985,
-                    "heading": -3.096236840109313,
-                    "angularVelocity": -1.1213315256974696,
-                    "velocityX": 1.2902306787967257,
-                    "velocityY": 0.14102277630833499,
-                    "timestamp": 2.159519504201062
+                    "x": 0.7613816250456615,
+                    "y": 6.725489542887092,
+                    "heading": 0.9931061774124983,
+                    "angularVelocity": -0.5863053943309177,
+                    "velocityX": 0.6424156162050443,
+                    "velocityY": 0.06863481007775876,
+                    "timestamp": 0.07998190540446273
+                },
+                {
+                    "x": 0.8643774278996095,
+                    "y": 6.736651981527731,
+                    "heading": 0.9029653992770575,
+                    "angularVelocity": -1.1270146376184127,
+                    "velocityX": 1.2877387995835512,
+                    "velocityY": 0.13956204949346018,
+                    "timestamp": 0.15996381080892547
+                },
+                {
+                    "x": 1.019343252130834,
+                    "y": 6.753670139482374,
+                    "heading": 0.7755395431364547,
+                    "angularVelocity": -1.5931835519073916,
+                    "velocityX": 1.9375110338716452,
+                    "velocityY": 0.21277510042531103,
+                    "timestamp": 0.2399457162133882
+                },
+                {
+                    "x": 1.2269177512246652,
+                    "y": 6.77676689021641,
+                    "heading": 0.6253872743553933,
+                    "angularVelocity": -1.8773279783940144,
+                    "velocityX": 2.5952682427874385,
+                    "velocityY": 0.2887746999429258,
+                    "timestamp": 0.31992762161785093
+                },
+                {
+                    "x": 1.4861215960194483,
+                    "y": 6.806717977426302,
+                    "heading": 0.5166968690593423,
+                    "angularVelocity": -1.358937433990995,
+                    "velocityX": 3.240781067717853,
+                    "velocityY": 0.37447328940753305,
+                    "timestamp": 0.39990952702231364
+                },
+                {
+                    "x": 1.7870123176597594,
+                    "y": 6.838521008932437,
+                    "heading": 0.5166968273679649,
+                    "angularVelocity": -5.212601175914625e-7,
+                    "velocityX": 3.7619849154471687,
+                    "velocityY": 0.397627830261236,
+                    "timestamp": 0.47989143242677634
+                },
+                {
+                    "x": 2.087903104978946,
+                    "y": 6.870323419047386,
+                    "heading": 0.5166967856787098,
+                    "angularVelocity": -5.212335818457529e-7,
+                    "velocityX": 3.7619857366188425,
+                    "velocityY": 0.3976200611141572,
+                    "timestamp": 0.559873337831239
+                },
+                {
+                    "x": 2.3887938966166886,
+                    "y": 6.9021257883033815,
+                    "heading": 0.5166967439894641,
+                    "angularVelocity": -5.212334661065162e-7,
+                    "velocityX": 3.761985790613012,
+                    "velocityY": 0.3976195502616874,
+                    "timestamp": 0.6398552432357018
+                },
+                {
+                    "x": 2.689684692935055,
+                    "y": 6.933928113274756,
+                    "heading": 0.5166967023002289,
+                    "angularVelocity": -5.212333328622992e-7,
+                    "velocityX": 3.76198584913404,
+                    "velocityY": 0.3976189965786884,
+                    "timestamp": 0.7198371486401645
+                },
+                {
+                    "x": 2.990575494322058,
+                    "y": 6.965730390290324,
+                    "heading": 0.5166966606110028,
+                    "angularVelocity": -5.212332194068621e-7,
+                    "velocityX": 3.761985912506342,
+                    "velocityY": 0.3976183969955037,
+                    "timestamp": 0.7998190540446272
+                },
+                {
+                    "x": 3.291466301153434,
+                    "y": 6.997532615795069,
+                    "heading": 0.5166966189217854,
+                    "angularVelocity": -5.21233112050001e-7,
+                    "velocityX": 3.76198598057639,
+                    "velocityY": 0.39761775296452984,
+                    "timestamp": 0.8798009594490899
+                },
+                {
+                    "x": 3.592357113798132,
+                    "y": 7.029334786298146,
+                    "heading": 0.5166965772325762,
+                    "angularVelocity": -5.212330086999819e-7,
+                    "velocityX": 3.7619860532593528,
+                    "velocityY": 0.39761706528816065,
+                    "timestamp": 0.9597828648535526
+                },
+                {
+                    "x": 3.8932479326255467,
+                    "y": 7.061136898304478,
+                    "heading": 0.5166965355433745,
+                    "angularVelocity": -5.212329156376395e-7,
+                    "velocityX": 3.7619861305608038,
+                    "velocityY": 0.3976163339134084,
+                    "timestamp": 1.0397647702580153
+                },
+                {
+                    "x": 4.194138758019097,
+                    "y": 7.092938948186284,
+                    "heading": 0.5166964938541775,
+                    "angularVelocity": -5.212328561470798e-7,
+                    "velocityX": 3.7619862126560695,
+                    "velocityY": 0.3976155571811608,
+                    "timestamp": 1.119746675662478
+                },
+                {
+                    "x": 4.495029590361164,
+                    "y": 7.1247409323255795,
+                    "heading": 0.516696452164982,
+                    "angularVelocity": -5.212328389753682e-7,
+                    "velocityX": 3.7619862995321793,
+                    "velocityY": 0.397614735213847,
+                    "timestamp": 1.199728581066941
+                },
+                {
+                    "x": 4.79592042996658,
+                    "y": 7.15654284774347,
+                    "heading": 0.5166964104757901,
+                    "angularVelocity": -5.212327914819509e-7,
+                    "velocityX": 3.7619863903445805,
+                    "velocityY": 0.39761387600196396,
+                    "timestamp": 1.2797104864714037
+                },
+                {
+                    "x": 5.096811277131865,
+                    "y": 7.188344691634321,
+                    "heading": 0.5166963687866055,
+                    "angularVelocity": -5.212326996843497e-7,
+                    "velocityX": 3.7619864848642997,
+                    "velocityY": 0.39761298171169474,
+                    "timestamp": 1.3596923918758665
+                },
+                {
+                    "x": 5.397702132107731,
+                    "y": 7.220146461625841,
+                    "heading": 0.5166963270974331,
+                    "angularVelocity": -5.212325496997691e-7,
+                    "velocityX": 3.761986582518669,
+                    "velocityY": 0.3976120577610798,
+                    "timestamp": 1.4396742972803294
+                },
+                {
+                    "x": 5.698592995103496,
+                    "y": 7.251948155737401,
+                    "heading": 0.516696285408269,
+                    "angularVelocity": -5.212324447299826e-7,
+                    "velocityX": 3.7619866827900856,
+                    "velocityY": 0.3976111090469843,
+                    "timestamp": 1.5196562026847922
+                },
+                {
+                    "x": 5.99948386596862,
+                    "y": 7.283749775393132,
+                    "heading": 0.5166962437191063,
+                    "angularVelocity": -5.212324282706134e-7,
+                    "velocityX": 3.7619867811793206,
+                    "velocityY": 0.3976101781385786,
+                    "timestamp": 1.599638108089255
+                },
+                {
+                    "x": 6.300374744112002,
+                    "y": 7.315551326185539,
+                    "heading": 0.5166962020299369,
+                    "angularVelocity": -5.212325087463018e-7,
+                    "velocityX": 3.761986872178135,
+                    "velocityY": 0.3976093171522739,
+                    "timestamp": 1.6796200134937178
+                },
+                {
+                    "x": 6.6012656287075195,
+                    "y": 7.347352815930844,
+                    "heading": 0.5166961603407556,
+                    "angularVelocity": -5.21232660707814e-7,
+                    "velocityX": 3.7619869528480785,
+                    "velocityY": 0.39760855389086563,
+                    "timestamp": 1.7596019188981806
+                },
+                {
+                    "x": 6.9021564720244495,
+                    "y": 7.37915469622686,
+                    "heading": 0.5166961186499832,
+                    "angularVelocity": -5.212525529907653e-7,
+                    "velocityX": 3.7619864367489835,
+                    "velocityY": 0.3976134368792068,
+                    "timestamp": 1.8395838243026434
+                },
+                {
+                    "x": 7.162090895853125,
+                    "y": 7.409612003973996,
+                    "heading": 0.405130711689902,
+                    "angularVelocity": -1.3948830850665912,
+                    "velocityX": 3.2499153716606957,
+                    "velocityY": 0.3808024776743751,
+                    "timestamp": 1.9195657297071063
+                },
+                {
+                    "x": 7.370036531434433,
+                    "y": 7.432975355945854,
+                    "heading": 0.2588526808330054,
+                    "angularVelocity": -1.8288890483063531,
+                    "velocityX": 2.5999084984252714,
+                    "velocityY": 0.2921079693427033,
+                    "timestamp": 1.999547635111569
+                },
+                {
+                    "x": 7.52532693851905,
+                    "y": 7.450135904743354,
+                    "heading": 0.13353023499549588,
+                    "angularVelocity": -1.5668849748422837,
+                    "velocityX": 1.9415692374334488,
+                    "velocityY": 0.21455538863097798,
+                    "timestamp": 2.079529540516032
+                },
+                {
+                    "x": 7.628525628712187,
+                    "y": 7.461415184148821,
+                    "heading": 0.04391070884179766,
+                    "angularVelocity": -1.120497513787634,
+                    "velocityX": 1.2902754650726174,
+                    "velocityY": 0.14102288947017258,
+                    "timestamp": 2.1595114459204945
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -0.5869864145386997,
-                    "velocityX": 0.6435484003601272,
-                    "velocityY": 0.06982786889492548,
-                    "timestamp": 2.2395017080603608
+                    "heading": -0.003,
+                    "angularVelocity": -0.5865165202625967,
+                    "velocityX": 0.6435752065109067,
+                    "velocityY": 0.06982599155316412,
+                    "timestamp": 2.239493351324957
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -1.1375235030887645e-31,
+                    "heading": -0.003,
+                    "angularVelocity": -3.4580717798993225e-32,
                     "velocityX": 0,
                     "velocityY": 0,
-                    "timestamp": 2.3194839119196593
+                    "timestamp": 2.3194752567294197
                 }
             ],
             "constraints": [
@@ -970,7 +970,7 @@
                 {
                     "x": 0.71,
                     "y": 6.72,
-                    "heading": -2.1,
+                    "heading": 1.04,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -979,7 +979,7 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": -3.143185307179586,
+                    "heading": -0.003,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -990,272 +990,272 @@
                 {
                     "x": 0.71,
                     "y": 6.72,
-                    "heading": -2.1,
+                    "heading": 1.04,
                     "angularVelocity": 0,
-                    "velocityX": -7.879763479482e-33,
+                    "velocityX": 8.672453699203877e-33,
                     "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7634290430022197,
-                    "y": 6.720160003423765,
-                    "heading": -2.158070133372329,
-                    "angularVelocity": -0.7094408010373536,
-                    "velocityX": 0.6527407612984071,
-                    "velocityY": 0.0019547562668324166,
-                    "timestamp": 0.08185338831290496
-                },
-                {
-                    "x": 0.8707119576691547,
-                    "y": 6.720819673735276,
-                    "heading": -2.2686879013575907,
-                    "angularVelocity": -1.351413426679394,
-                    "velocityX": 1.3106716396005418,
-                    "velocityY": 0.008059169267236224,
-                    "timestamp": 0.16370677662580993
-                },
-                {
-                    "x": 1.0324112516405533,
-                    "y": 6.722281472922437,
-                    "heading": -2.4240599050623355,
-                    "angularVelocity": -1.898174368919177,
-                    "velocityX": 1.9754746541861241,
-                    "velocityY": 0.017858749861073935,
-                    "timestamp": 0.2455601649387149
-                },
-                {
-                    "x": 1.249511925601742,
-                    "y": 6.724737672202976,
-                    "heading": -2.6048330548635152,
-                    "angularVelocity": -2.2084992878991065,
-                    "velocityX": 2.652311387908187,
-                    "velocityY": 0.030007301239015387,
-                    "timestamp": 0.32741355325161986
-                },
-                {
-                    "x": 1.518512933103673,
-                    "y": 6.730171304043105,
-                    "heading": -2.7105117995327306,
-                    "angularVelocity": -1.2910735504953308,
-                    "velocityX": 3.2863759588498382,
-                    "velocityY": 0.0663824913314231,
-                    "timestamp": 0.4092669415645248
-                },
-                {
-                    "x": 1.8281517964730614,
-                    "y": 6.7323448579922625,
-                    "heading": -2.71051185012585,
-                    "angularVelocity": -6.180943842017169e-7,
-                    "velocityX": 3.782847231512476,
-                    "velocityY": 0.026554233049558205,
-                    "timestamp": 0.49112032987742976
-                },
-                {
-                    "x": 2.1377906615261124,
-                    "y": 6.734518172124384,
-                    "heading": -2.710511900718092,
-                    "angularVelocity": -6.180836584503726e-7,
-                    "velocityX": 3.7828472520817247,
-                    "velocityY": 0.026551303213165345,
-                    "timestamp": 0.5729737181903347
-                },
-                {
-                    "x": 2.4474295268603647,
-                    "y": 6.736691446193322,
-                    "heading": -2.7105119513103224,
-                    "angularVelocity": -6.180835185678023e-7,
-                    "velocityX": 3.7828472555171566,
-                    "velocityY": 0.026550813762651566,
-                    "timestamp": 0.6548271065032397
-                },
-                {
-                    "x": 2.7570683925033053,
-                    "y": 6.738864676282315,
-                    "heading": -2.7105120019025377,
-                    "angularVelocity": -6.180833387834062e-7,
-                    "velocityX": 3.7828472592883804,
-                    "velocityY": 0.026550276461191975,
-                    "timestamp": 0.7366804948161447
-                },
-                {
-                    "x": 3.0667072584849335,
-                    "y": 6.741037858115924,
-                    "heading": -2.7105120524947366,
-                    "angularVelocity": -6.180831373401399e-7,
-                    "velocityX": 3.7828472634261248,
-                    "velocityY": 0.026549686926837145,
-                    "timestamp": 0.8185338831290497
-                },
-                {
-                    "x": 3.376346124838015,
-                    "y": 6.7432109870244785,
-                    "heading": -2.7105121030869213,
-                    "angularVelocity": -6.18082958211415e-7,
-                    "velocityX": 3.7828472679641574,
-                    "velocityY": 0.02654904034329684,
-                    "timestamp": 0.9003872714419547
-                },
-                {
-                    "x": 3.6859849915958844,
-                    "y": 6.745384058256853,
-                    "heading": -2.710512153679093,
-                    "angularVelocity": -6.180828069034381e-7,
-                    "velocityX": 3.782847272909432,
-                    "velocityY": 0.026548335715409193,
-                    "timestamp": 0.9822406597548597
-                },
-                {
-                    "x": 3.99562385879327,
-                    "y": 6.747557066862843,
-                    "heading": -2.7105122042712537,
-                    "angularVelocity": -6.180826661377271e-7,
-                    "velocityX": 3.7828472782789873,
-                    "velocityY": 0.026547570611047785,
-                    "timestamp": 1.0640940480677648
-                },
-                {
-                    "x": 4.305262726470487,
-                    "y": 6.749730007095753,
-                    "heading": -2.710512254863404,
-                    "angularVelocity": -6.180825475741077e-7,
-                    "velocityX": 3.782847284141071,
-                    "velocityY": 0.026546735299496697,
-                    "timestamp": 1.1459474363806696
-                },
-                {
-                    "x": 4.614901594678237,
-                    "y": 6.751902871728125,
-                    "heading": -2.710512305455549,
-                    "angularVelocity": -6.180824717562797e-7,
-                    "velocityX": 3.78284729062258,
-                    "velocityY": 0.0265458116903545,
-                    "timestamp": 1.2278008246935745
-                },
-                {
-                    "x": 4.924540463476948,
-                    "y": 6.754075652145792,
-                    "heading": -2.7105123560476914,
-                    "angularVelocity": -6.180824480187367e-7,
-                    "velocityX": 3.782847297842324,
-                    "velocityY": 0.02654478284224149,
-                    "timestamp": 1.3096542130064794
-                },
-                {
-                    "x": 5.234179332930726,
-                    "y": 6.756248339209459,
-                    "heading": -2.7105124066398263,
-                    "angularVelocity": -6.180823518950674e-7,
-                    "velocityX": 3.782847305845261,
-                    "velocityY": 0.026543642339650734,
-                    "timestamp": 1.3915076013193843
-                },
-                {
-                    "x": 5.543818204521741,
-                    "y": 6.758420721665928,
-                    "heading": -2.710512457231931,
-                    "angularVelocity": -6.180819838461299e-7,
-                    "velocityX": 3.782847331955805,
-                    "velocityY": 0.026539920964112583,
-                    "timestamp": 1.4733609896322892
-                },
-                {
-                    "x": 5.852559225022806,
-                    "y": 6.734757839446224,
-                    "heading": -2.710512508060742,
-                    "angularVelocity": -6.209738209329952e-7,
-                    "velocityX": 3.771878316396938,
-                    "velocityY": -0.2890886096156145,
-                    "timestamp": 1.5552143779451941
-                },
-                {
-                    "x": 6.152023822280739,
-                    "y": 6.6560056480568655,
-                    "heading": -2.710512532930908,
-                    "angularVelocity": -3.03837945152059e-7,
-                    "velocityX": 3.6585485760608294,
-                    "velocityY": -0.9621127849748639,
-                    "timestamp": 1.637067766258099
-                },
-                {
-                    "x": 6.432464531176267,
-                    "y": 6.5247274697787905,
-                    "heading": -2.710512568697855,
-                    "angularVelocity": -4.369635492236228e-7,
-                    "velocityX": 3.4261343931600465,
-                    "velocityY": -1.603820941122575,
-                    "timestamp": 1.718921154571004
-                },
-                {
-                    "x": 6.699924709540006,
-                    "y": 6.368695406956045,
-                    "heading": -2.710512603773811,
-                    "angularVelocity": -4.2852173297406095e-7,
-                    "velocityX": 3.267551702823897,
-                    "velocityY": -1.9062382882218016,
-                    "timestamp": 1.8007745428839088
-                },
-                {
-                    "x": 6.967384720466867,
-                    "y": 6.212663057124264,
-                    "heading": -2.710512638850014,
-                    "angularVelocity": -4.2852475398494205e-7,
-                    "velocityX": 3.2675496572533906,
-                    "velocityY": -1.9062417946012,
-                    "timestamp": 1.8826279311968137
-                },
-                {
-                    "x": 7.2050885186595135,
-                    "y": 6.073759568822601,
-                    "heading": -2.7948440651741784,
-                    "angularVelocity": -1.0302741042531607,
-                    "velocityX": 2.9040190405309123,
-                    "velocityY": -1.6969790886440652,
-                    "timestamp": 1.9644813195097186
-                },
-                {
-                    "x": 7.39556651196216,
-                    "y": 5.9626310397123286,
-                    "heading": -2.9179406119000064,
-                    "angularVelocity": -1.5038662328217944,
-                    "velocityX": 2.3270630236404783,
-                    "velocityY": -1.3576533775908837,
-                    "timestamp": 2.0463347078226235
-                },
-                {
-                    "x": 7.537983690499959,
-                    "y": 5.879640169020095,
-                    "heading": -3.0253464428320265,
-                    "angularVelocity": -1.3121733033388265,
-                    "velocityX": 1.7399057201318795,
-                    "velocityY": -1.0138965826922115,
-                    "timestamp": 2.1281880961355286
-                },
-                {
-                    "x": 7.632713372468284,
-                    "y": 5.824499681694904,
-                    "heading": -3.1026583555829736,
-                    "angularVelocity": -0.9445169509098753,
-                    "velocityX": 1.1573092320405494,
-                    "velocityY": -0.6736494171066164,
-                    "timestamp": 2.2100414844484337
+                    "x": 0.7586383226811528,
+                    "y": 6.716006460523898,
+                    "heading": 0.9415437629009579,
+                    "angularVelocity": -1.201643840291405,
+                    "velocityX": 0.5936235486342963,
+                    "velocityY": -0.048740559804155884,
+                    "timestamp": 0.08193462471805792
+                },
+                {
+                    "x": 0.8580797410236602,
+                    "y": 6.713039346170398,
+                    "heading": 0.7612656224060018,
+                    "angularVelocity": -2.2002680931938623,
+                    "velocityX": 1.2136678319415195,
+                    "velocityY": -0.03621319269735464,
+                    "timestamp": 0.16386924943611583
+                },
+                {
+                    "x": 1.0092416285400778,
+                    "y": 6.712497099188053,
+                    "heading": 0.5037500418751092,
+                    "angularVelocity": -3.142939647518002,
+                    "velocityX": 1.8449085235524658,
+                    "velocityY": -0.006618044376361366,
+                    "timestamp": 0.24580387415417376
+                },
+                {
+                    "x": 1.2158692368887885,
+                    "y": 6.714801206948958,
+                    "heading": 0.22114949007633808,
+                    "angularVelocity": -3.4490980189537326,
+                    "velocityX": 2.5218594588030294,
+                    "velocityY": 0.028121295103663166,
+                    "timestamp": 0.32773849887223166
+                },
+                {
+                    "x": 1.4708450332398146,
+                    "y": 6.7208733735889465,
+                    "heading": 0.04860948021713714,
+                    "angularVelocity": -2.105825351039585,
+                    "velocityX": 3.1119419565100066,
+                    "velocityY": 0.07410989750528489,
+                    "timestamp": 0.40967312359028957
+                },
+                {
+                    "x": 1.7704680006957136,
+                    "y": 6.724924507610315,
+                    "heading": 0.013522063773945665,
+                    "angularVelocity": -0.42823673830116915,
+                    "velocityX": 3.6568540893049817,
+                    "velocityY": 0.049443492727383076,
+                    "timestamp": 0.49160774830834747
+                },
+                {
+                    "x": 2.080344251704432,
+                    "y": 6.7179919202849705,
+                    "heading": 0.013521948546161235,
+                    "angularVelocity": -0.000001406338099756076,
+                    "velocityX": 3.78199390154067,
+                    "velocityY": -0.08461120496004566,
+                    "timestamp": 0.5735423730264054
+                },
+                {
+                    "x": 2.3902205002379273,
+                    "y": 6.711059222319389,
+                    "heading": 0.013521833318547862,
+                    "angularVelocity": -0.000001406336012025862,
+                    "velocityX": 3.781993871330935,
+                    "velocityY": -0.08461255530781114,
+                    "timestamp": 0.6554769977444633
+                },
+                {
+                    "x": 2.7000967477974154,
+                    "y": 6.704126480817366,
+                    "heading": 0.013521718090975673,
+                    "angularVelocity": -0.000001406335509399091,
+                    "velocityX": 3.7819938594433236,
+                    "velocityY": -0.08461308666366031,
+                    "timestamp": 0.7374116224625212
+                },
+                {
+                    "x": 3.009972994287386,
+                    "y": 6.6971936915101855,
+                    "heading": 0.013521602863440677,
+                    "angularVelocity": -0.0000014063350554339165,
+                    "velocityX": 3.7819938463900247,
+                    "velocityY": -0.08461367011854067,
+                    "timestamp": 0.8193462471805791
+                },
+                {
+                    "x": 3.319849239613809,
+                    "y": 6.690260850195287,
+                    "heading": 0.013521487635941729,
+                    "angularVelocity": -0.0000014063346155021318,
+                    "velocityX": 3.781993832189087,
+                    "velocityY": -0.08461430486506759,
+                    "timestamp": 0.901280871898637
+                },
+                {
+                    "x": 3.629725483679546,
+                    "y": 6.683327952531275,
+                    "heading": 0.013521372408489362,
+                    "angularVelocity": -0.0000014063340469636637,
+                    "velocityX": 3.7819938168026104,
+                    "velocityY": -0.0846149925976696,
+                    "timestamp": 0.9832154966166949
+                },
+                {
+                    "x": 3.939601726375346,
+                    "y": 6.6763949936353555,
+                    "heading": 0.013521257181093756,
+                    "angularVelocity": -0.0000014063333542144944,
+                    "velocityX": 3.7819938000827285,
+                    "velocityY": -0.08461573992406812,
+                    "timestamp": 1.065150121334753
+                },
+                {
+                    "x": 4.249477967564444,
+                    "y": 6.66946196739526,
+                    "heading": 0.013521141953758897,
+                    "angularVelocity": -0.0000014063326128127892,
+                    "velocityX": 3.781993781693661,
+                    "velocityY": -0.0846165618498062,
+                    "timestamp": 1.1470847460528109
+                },
+                {
+                    "x": 4.559354207050928,
+                    "y": 6.662528865055332,
+                    "heading": 0.013521026726476763,
+                    "angularVelocity": -0.0000014063319692972842,
+                    "velocityX": 3.7819937609134957,
+                    "velocityY": -0.08461749063701171,
+                    "timestamp": 1.2290193707708688
+                },
+                {
+                    "x": 4.869230444482983,
+                    "y": 6.655595670892424,
+                    "heading": 0.01352091149922989,
+                    "angularVelocity": -0.0000014063315389464776,
+                    "velocityX": 3.7819937358394955,
+                    "velocityY": -0.08461861132293771,
+                    "timestamp": 1.3109539954889267
+                },
+                {
+                    "x": 5.1791066790320945,
+                    "y": 6.648662347878877,
+                    "heading": 0.013520796271957977,
+                    "angularVelocity": -0.0000014063318445457383,
+                    "velocityX": 3.7819937006535858,
+                    "velocityY": -0.08462018392597198,
+                    "timestamp": 1.3928886202069846
+                },
+                {
+                    "x": 5.488982909387702,
+                    "y": 6.641728837446599,
+                    "heading": 0.013520681044501265,
+                    "angularVelocity": -0.000001406334100009416,
+                    "velocityX": 3.781993649472511,
+                    "velocityY": -0.08462247134390694,
+                    "timestamp": 1.4748232449250425
+                },
+                {
+                    "x": 5.798859128679326,
+                    "y": 6.634794832555163,
+                    "heading": 0.013520565816988437,
+                    "angularVelocity": -0.0000014063347848865157,
+                    "velocityX": 3.781993514438203,
+                    "velocityY": -0.08462850614495548,
+                    "timestamp": 1.5567578696431004
+                },
+                {
+                    "x": 6.106285584248806,
+                    "y": 6.595293875173847,
+                    "heading": 0.013520451149743993,
+                    "angularVelocity": -0.000001399496791973984,
+                    "velocityX": 3.7520945098284613,
+                    "velocityY": -0.48210335395128207,
+                    "timestamp": 1.6386924943611583
+                },
+                {
+                    "x": 6.401594301701064,
+                    "y": 6.501144389493677,
+                    "heading": 0.013520442621996222,
+                    "angularVelocity": -1.0407990272609098e-7,
+                    "velocityX": 3.604199304853526,
+                    "velocityY": -1.1490805749601392,
+                    "timestamp": 1.7206271190792162
+                },
+                {
+                    "x": 6.675154779128766,
+                    "y": 6.355418517983256,
+                    "heading": 0.013520441934185553,
+                    "angularVelocity": -8.394627700401504e-9,
+                    "velocityX": 3.3387652456948387,
+                    "velocityY": -1.7785627506304358,
+                    "timestamp": 1.802561743797274
+                },
+                {
+                    "x": 6.946083635654423,
+                    "y": 6.204856506903887,
+                    "heading": 0.013520441248790115,
+                    "angularVelocity": -8.365150168101951e-9,
+                    "velocityX": 3.3066467010490315,
+                    "velocityY": -1.8375871201883405,
+                    "timestamp": 1.884496368515332
+                },
+                {
+                    "x": 7.190722413199009,
+                    "y": 6.068904365786755,
+                    "heading": 0.008013663922991063,
+                    "angularVelocity": -0.06720940438487652,
+                    "velocityX": 2.985780167864364,
+                    "velocityY": -1.6592758139181236,
+                    "timestamp": 1.9664309932333899
+                },
+                {
+                    "x": 7.38643344699072,
+                    "y": 5.96014263272892,
+                    "heading": 0.0036082164005303732,
+                    "angularVelocity": -0.053767836706643944,
+                    "velocityX": 2.3886242777722417,
+                    "velocityY": -1.3274208972347252,
+                    "timestamp": 2.048365617951448
+                },
+                {
+                    "x": 7.533216723981145,
+                    "y": 5.878571322292618,
+                    "heading": 0.0003041162966361105,
+                    "angularVelocity": -0.04032605403715309,
+                    "velocityX": 1.7914682284261954,
+                    "velocityY": -0.9955658028213874,
+                    "timestamp": 2.130300242669506
+                },
+                {
+                    "x": 7.631072241649754,
+                    "y": 5.82419044260697,
+                    "heading": -0.0018986254325586427,
+                    "angularVelocity": -0.026884137649675257,
+                    "velocityX": 1.1943121483174686,
+                    "velocityY": -0.66371060919331,
+                    "timestamp": 2.212234867387564
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -0.49511635904048823,
-                    "velocityX": 0.5776990849902374,
-                    "velocityY": -0.33596265544659853,
-                    "timestamp": 2.291894872761339
+                    "heading": -0.003,
+                    "angularVelocity": -0.013442114017501823,
+                    "velocityX": 0.5971560682508651,
+                    "velocityY": -0.3318553383326643,
+                    "timestamp": 2.294169492105622
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -7.614661899058019e-31,
+                    "heading": -0.003,
+                    "angularVelocity": -2.3555736468302963e-30,
                     "velocityX": 0,
-                    "velocityY": -2.3591575038755638e-32,
-                    "timestamp": 2.373748261074244
+                    "velocityY": 5.757868192156219e-33,
+                    "timestamp": 2.3761041168236803
                 }
             ],
             "constraints": [
@@ -1298,7 +1298,7 @@
                 {
                     "x": 0.71,
                     "y": 6.72,
-                    "heading": -2.1,
+                    "heading": 1.04,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -1307,7 +1307,7 @@
                 {
                     "x": 4.900439739227295,
                     "y": 4.161101341247559,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -1316,7 +1316,7 @@
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": -3.143185307179586,
+                    "heading": -0.003,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -1327,524 +1327,524 @@
                 {
                     "x": 0.71,
                     "y": 6.72,
-                    "heading": -2.1,
+                    "heading": 1.04,
                     "angularVelocity": 0,
                     "velocityX": 0,
-                    "velocityY": 2.27986083101055e-33,
+                    "velocityY": 1.3928609100313025e-33,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7292945467072102,
-                    "y": 6.711630943578302,
-                    "heading": -2.0466837783195437,
-                    "angularVelocity": 0.9750423416679173,
-                    "velocityX": 0.3528569619125016,
-                    "velocityY": -0.1530525628742106,
-                    "timestamp": 0.05468092963968429
-                },
-                {
-                    "x": 0.7686895561493499,
-                    "y": 6.695016697545084,
-                    "heading": -1.9453579324448191,
-                    "angularVelocity": 1.853038098335252,
-                    "velocityX": 0.720452444787058,
-                    "velocityY": -0.30383986049058775,
-                    "timestamp": 0.10936185927936858
-                },
-                {
-                    "x": 0.8291498926090706,
-                    "y": 6.670065411203325,
-                    "heading": -1.8043277140562832,
-                    "angularVelocity": 2.579148147587168,
-                    "velocityX": 1.1056932802371746,
-                    "velocityY": -0.4563069155219838,
-                    "timestamp": 0.16404278891905286
-                },
-                {
-                    "x": 0.9116442238767206,
-                    "y": 6.636292399159329,
-                    "heading": -1.6353628554936872,
-                    "angularVelocity": 3.09001437385898,
-                    "velocityX": 1.50864902647486,
-                    "velocityY": -0.6176378541210026,
-                    "timestamp": 0.21872371855873715
-                },
-                {
-                    "x": 1.0169609100629697,
-                    "y": 6.593200359597327,
-                    "heading": -1.4531033706452814,
-                    "angularVelocity": 3.333145322316033,
-                    "velocityX": 1.926022232617937,
-                    "velocityY": -0.7880634042974968,
-                    "timestamp": 0.2734046481984214
-                },
-                {
-                    "x": 1.1451022790503944,
-                    "y": 6.540887351226898,
-                    "heading": -1.2848862020086818,
-                    "angularVelocity": 3.0763406866901017,
-                    "velocityX": 2.3434380108714725,
-                    "velocityY": -0.9566956654750047,
-                    "timestamp": 0.3280855778381057
-                },
-                {
-                    "x": 1.291419394840941,
-                    "y": 6.479714636695398,
-                    "heading": -1.166754837939456,
-                    "angularVelocity": 2.1603759271768683,
-                    "velocityX": 2.6758344591924805,
-                    "velocityY": -1.1187211873425027,
-                    "timestamp": 0.38276650747779
-                },
-                {
-                    "x": 1.45556402161766,
-                    "y": 6.408096144298377,
-                    "heading": -1.0985152891132095,
-                    "angularVelocity": 1.24795882725304,
-                    "velocityX": 3.0018624017246447,
-                    "velocityY": -1.3097526481159927,
-                    "timestamp": 0.4374474371174743
-                },
-                {
-                    "x": 1.6376170855458871,
-                    "y": 6.326295801964295,
-                    "heading": -1.0805867815133028,
-                    "angularVelocity": 0.3278749596622653,
-                    "velocityX": 3.32937031480356,
-                    "velocityY": -1.495957418301032,
-                    "timestamp": 0.4921283667571586
-                },
-                {
-                    "x": 1.828511449146643,
-                    "y": 6.246620230558847,
-                    "heading": -1.0805866805891169,
-                    "angularVelocity": 0.000001845692576742716,
-                    "velocityX": 3.491059220438268,
-                    "velocityY": -1.4570997956776681,
-                    "timestamp": 0.5468092963968428
-                },
-                {
-                    "x": 2.0194058587075467,
-                    "y": 6.16694476926885,
-                    "heading": -1.0805865796652752,
-                    "angularVelocity": 0.0000018456862813728727,
-                    "velocityX": 3.491060060953374,
-                    "velocityY": -1.4570977818960094,
-                    "timestamp": 0.6014902260365271
-                },
-                {
-                    "x": 2.2103002634541666,
-                    "y": 6.087269296444259,
-                    "heading": -1.0805864787414845,
-                    "angularVelocity": 0.000001845685351855892,
-                    "velocityX": 3.491059972910188,
-                    "velocityY": -1.4570979928396652,
-                    "timestamp": 0.6561711556762114
-                },
-                {
-                    "x": 2.4011946628107936,
-                    "y": 6.007593810705732,
-                    "heading": -1.0805863778177442,
-                    "angularVelocity": 0.000001845684424329729,
-                    "velocityX": 3.4910598743384673,
-                    "velocityY": -1.457098229008597,
-                    "timestamp": 0.7108520853158957
-                },
-                {
-                    "x": 2.5920890560965946,
-                    "y": 5.927918310422065,
-                    "heading": -1.0805862768940542,
-                    "angularVelocity": 0.0000018456835100189827,
-                    "velocityX": 3.4910597633157407,
-                    "velocityY": -1.45709849500883,
-                    "timestamp": 0.76553301495558
-                },
-                {
-                    "x": 2.7829834425031943,
-                    "y": 5.8482427936564765,
-                    "heading": -1.0805861759704127,
-                    "angularVelocity": 0.0000018456826171919115,
-                    "velocityX": 3.4910596375095175,
-                    "velocityY": -1.4570987964287467,
-                    "timestamp": 0.8202139445952643
-                },
-                {
-                    "x": 2.9738778210651113,
-                    "y": 5.768567258095778,
-                    "heading": -1.080586075046818,
-                    "angularVelocity": 0.0000018456817639254375,
-                    "velocityX": 3.4910594940466586,
-                    "velocityY": -1.4570991401520532,
-                    "timestamp": 0.8748948742349486
-                },
-                {
-                    "x": 3.164772190605788,
-                    "y": 5.688891700921064,
-                    "heading": -1.0805859741232662,
-                    "angularVelocity": 0.0000018456809789441398,
-                    "velocityX": 3.491059329067008,
-                    "velocityY": -1.4570995354272234,
-                    "timestamp": 0.9295758038746329
-                },
-                {
-                    "x": 3.3556665493271205,
-                    "y": 5.609216117824277,
-                    "heading": -1.0805858731997562,
-                    "angularVelocity": 0.000001845680216876217,
-                    "velocityX": 3.491059131203806,
-                    "velocityY": -1.4571000094878162,
-                    "timestamp": 0.9842567335143172
-                },
-                {
-                    "x": 3.5465606286609406,
-                    "y": 5.529539865347568,
-                    "heading": -1.0805857722761907,
-                    "angularVelocity": 0.0000018456812299162406,
-                    "velocityX": 3.491054021789706,
-                    "velocityY": -1.4571122510486052,
-                    "timestamp": 1.0389376631540015
-                },
-                {
-                    "x": 3.7302882443981766,
-                    "y": 5.43449767788976,
-                    "heading": -1.0805856670205962,
-                    "angularVelocity": 0.0000019249049955796687,
-                    "velocityX": 3.3599943700279993,
-                    "velocityY": -1.7381231095389325,
-                    "timestamp": 1.0936185927936857
-                },
-                {
-                    "x": 3.901247591247985,
-                    "y": 5.318042735383766,
-                    "heading": -1.0805855451412456,
-                    "angularVelocity": 0.000002228918769474178,
-                    "velocityX": 3.1264893990707807,
-                    "velocityY": -2.12971767805275,
-                    "timestamp": 1.1482995224333699
-                },
-                {
-                    "x": 4.056955156553094,
-                    "y": 5.181866377235877,
-                    "heading": -1.0805853830111598,
-                    "angularVelocity": 0.0000029650206572180867,
-                    "velocityX": 2.8475661685185734,
-                    "velocityY": -2.490381181249336,
-                    "timestamp": 1.202980452073054
-                },
-                {
-                    "x": 4.195149083685748,
-                    "y": 5.0279466523676755,
-                    "heading": -1.080585195813012,
-                    "angularVelocity": 0.0000034234631562085443,
-                    "velocityX": 2.527278304214503,
-                    "velocityY": -2.81487030089728,
-                    "timestamp": 1.2576613817127382
-                },
-                {
-                    "x": 4.313735776578777,
-                    "y": 4.858628961688981,
-                    "heading": -1.081017907841774,
-                    "angularVelocity": -0.007913399271258946,
-                    "velocityX": 2.168702940393416,
-                    "velocityY": -3.0964669363597066,
-                    "timestamp": 1.3123423113524224
-                },
-                {
-                    "x": 4.409971486167556,
-                    "y": 4.678531583119963,
-                    "heading": -1.0900704798968306,
-                    "angularVelocity": -0.16555263626108413,
-                    "velocityX": 1.759950136600006,
-                    "velocityY": -3.293604914103617,
-                    "timestamp": 1.3670232409921066
-                },
-                {
-                    "x": 4.510748059482075,
-                    "y": 4.519168046099015,
-                    "heading": -1.1448684085004224,
-                    "angularVelocity": -1.0021396666932787,
-                    "velocityX": 1.8429930503848142,
-                    "velocityY": -2.9144262555714096,
-                    "timestamp": 1.4217041706317908
-                },
-                {
-                    "x": 4.62613024170022,
-                    "y": 4.379607058549792,
-                    "heading": -1.2139690573229496,
-                    "angularVelocity": -1.2637065477463625,
-                    "velocityX": 2.110099132886858,
-                    "velocityY": -2.552278984078161,
-                    "timestamp": 1.476385100271475
-                },
-                {
-                    "x": 4.756158352198169,
-                    "y": 4.26013085900244,
-                    "heading": -1.289886392340967,
-                    "angularVelocity": -1.3883695013648907,
-                    "velocityX": 2.3779425725707126,
-                    "velocityY": -2.1849701593340063,
-                    "timestamp": 1.5310660299111591
+                    "x": 0.729279758485921,
+                    "y": 6.711628327586588,
+                    "heading": 1.093428142295198,
+                    "angularVelocity": 0.9770378814200095,
+                    "velocityX": 0.352568020824986,
+                    "velocityY": -0.15309237280887605,
+                    "timestamp": 0.05468379815278649
+                },
+                {
+                    "x": 0.7686493967347447,
+                    "y": 6.69501140584797,
+                    "heading": 1.1949690599508662,
+                    "angularVelocity": 1.8568739020644303,
+                    "velocityX": 0.7199506906748694,
+                    "velocityY": -0.30387285265355396,
+                    "timestamp": 0.10936759630557298
+                },
+                {
+                    "x": 0.8290790315170126,
+                    "y": 6.670059182269693,
+                    "heading": 1.3362954219692313,
+                    "angularVelocity": 2.584428419245852,
+                    "velocityX": 1.1050738394839994,
+                    "velocityY": -0.4563001185206737,
+                    "timestamp": 0.16405139445835948
+                },
+                {
+                    "x": 0.9115429372168562,
+                    "y": 6.636285478537955,
+                    "heading": 1.5055888656223098,
+                    "angularVelocity": 3.0958611027725738,
+                    "velocityX": 1.508013497333151,
+                    "velocityY": -0.6176181039468854,
+                    "timestamp": 0.21873519261114596
+                },
+                {
+                    "x": 1.0168329273736771,
+                    "y": 6.593191021214006,
+                    "heading": 1.6881423696003794,
+                    "angularVelocity": 3.3383471913932383,
+                    "velocityX": 1.9254330114861617,
+                    "velocityY": -0.7880662788554578,
+                    "timestamp": 0.27341899076393245
+                },
+                {
+                    "x": 1.1449422871140338,
+                    "y": 6.540878714080721,
+                    "heading": 1.8564615478628756,
+                    "angularVelocity": 3.078044758197163,
+                    "velocityX": 2.3427297310698694,
+                    "velocityY": -0.9566326572109004,
+                    "timestamp": 0.32810278891671896
+                },
+                {
+                    "x": 1.2912409780613738,
+                    "y": 6.479735868066328,
+                    "heading": 1.974688906048384,
+                    "angularVelocity": 2.1620180415263293,
+                    "velocityX": 2.675357160425865,
+                    "velocityY": -1.1181162991560991,
+                    "timestamp": 0.38278658706950547
+                },
+                {
+                    "x": 1.4553681499836075,
+                    "y": 6.408144093279595,
+                    "heading": 2.0430197361851268,
+                    "angularVelocity": 1.2495626208301445,
+                    "velocityX": 3.001385738855639,
+                    "velocityY": -1.3091953595963746,
+                    "timestamp": 0.437470385222292
+                },
+                {
+                    "x": 1.6374055328463037,
+                    "y": 6.326368400043195,
+                    "heading": 2.0610350683831213,
+                    "angularVelocity": 0.3294455178051113,
+                    "velocityX": 3.328908909254691,
+                    "velocityY": -1.4954281889476506,
+                    "timestamp": 0.4921541833750785
+                },
+                {
+                    "x": 1.8283093127689483,
+                    "y": 6.246687216839916,
+                    "heading": 2.061035169590908,
+                    "angularVelocity": 0.000001850781943801841,
+                    "velocityX": 3.4910482880003957,
+                    "velocityY": -1.45712598420193,
+                    "timestamp": 0.546837981527865
+                },
+                {
+                    "x": 2.01921313853121,
+                    "y": 6.167006143460992,
+                    "heading": 2.0610352707983473,
+                    "angularVelocity": 0.0000018507755948234292,
+                    "velocityX": 3.491049126267287,
+                    "velocityY": -1.4571239758492094,
+                    "timestamp": 0.6015217796806515
+                },
+                {
+                    "x": 2.210116959480156,
+                    "y": 6.087325058550032,
+                    "heading": 2.061035372005735,
+                    "angularVelocity": 0.000001850774661522582,
+                    "velocityX": 3.4910490382464,
+                    "velocityY": -1.4571241867349893,
+                    "timestamp": 0.6562055778334379
+                },
+                {
+                    "x": 2.40102077504018,
+                    "y": 6.007643960727976,
+                    "heading": 2.0610354732130722,
+                    "angularVelocity": 0.0000018507737318277825,
+                    "velocityX": 3.49104893969945,
+                    "velocityY": -1.4571244228395928,
+                    "timestamp": 0.7108893759862244
+                },
+                {
+                    "x": 2.5919245845305627,
+                    "y": 5.927962848363926,
+                    "heading": 2.0610355744203592,
+                    "angularVelocity": 0.0000018507728139380304,
+                    "velocityX": 3.491048828704212,
+                    "velocityY": -1.457124688768356,
+                    "timestamp": 0.7655731741390108
+                },
+                {
+                    "x": 2.78282838714305,
+                    "y": 5.848281719521431,
+                    "heading": 2.0610356756275974,
+                    "angularVelocity": 0.0000018507719179726602,
+                    "velocityX": 3.4910487029284663,
+                    "velocityY": -1.457124990108895,
+                    "timestamp": 0.8202569722917973
+                },
+                {
+                    "x": 2.973732181912291,
+                    "y": 5.768600571887663,
+                    "heading": 2.061035776834788,
+                    "angularVelocity": 0.0000018507710613284672,
+                    "velocityX": 3.491048559499401,
+                    "velocityY": -1.4571253337439838,
+                    "timestamp": 0.8749407704445837
+                },
+                {
+                    "x": 3.16463596766188,
+                    "y": 5.688919402644143,
+                    "heading": 2.061035878041936,
+                    "angularVelocity": 0.000001850770272196717,
+                    "velocityX": 3.4910483945574473,
+                    "velocityY": -1.4571257289205004,
+                    "timestamp": 0.9296245685973702
+                },
+                {
+                    "x": 3.3555397425941185,
+                    "y": 5.609238207483871,
+                    "heading": 2.061035979249042,
+                    "angularVelocity": 0.000001850769506382635,
+                    "velocityX": 3.4910481967410822,
+                    "velocityY": -1.457126202858873,
+                    "timestamp": 0.9843083667501566
+                },
+                {
+                    "x": 3.546443239175115,
+                    "y": 5.529556345440524,
+                    "heading": 2.0610360804562036,
+                    "angularVelocity": 0.0000018507705174022697,
+                    "velocityX": 3.491043106545231,
+                    "velocityY": -1.4571383981177848,
+                    "timestamp": 1.0389921649029432
+                },
+                {
+                    "x": 3.7301891812097523,
+                    "y": 5.434525969364101,
+                    "heading": 2.0610361860102304,
+                    "angularVelocity": 0.0000019302614304565207,
+                    "velocityX": 3.360153249071168,
+                    "velocityY": -1.7378159397580357,
+                    "timestamp": 1.0936759630557298
+                },
+                {
+                    "x": 3.9011674048607876,
+                    "y": 5.318079465137689,
+                    "heading": 2.0610363082632275,
+                    "angularVelocity": 0.0000022356347107367405,
+                    "velocityX": 3.126670593972313,
+                    "velocityY": -2.129451650396711,
+                    "timestamp": 1.1483597612085163
+                },
+                {
+                    "x": 4.05689386248419,
+                    "y": 5.181908226701039,
+                    "heading": 2.0610364709885136,
+                    "angularVelocity": 0.0000029757495174652245,
+                    "velocityX": 2.8477622784778513,
+                    "velocityY": -2.490156920998575,
+                    "timestamp": 1.203043559361303
+                },
+                {
+                    "x": 4.195106185477333,
+                    "y": 5.027990436285506,
+                    "heading": 2.061036659879246,
+                    "angularVelocity": 0.0000034542357869786317,
+                    "velocityX": 2.52748213660978,
+                    "velocityY": -2.814687267798894,
+                    "timestamp": 1.2577273575140895
+                },
+                {
+                    "x": 4.313711649853664,
+                    "y": 4.858669951905228,
+                    "heading": 2.0606108116890636,
+                    "angularVelocity": -0.007787465475470693,
+                    "velocityX": 2.168932451344132,
+                    "velocityY": -3.096355595256869,
+                    "timestamp": 1.312411155666876
+                },
+                {
+                    "x": 4.40996698448611,
+                    "y": 4.67856050613378,
+                    "heading": 2.051588761211157,
+                    "angularVelocity": -0.16498580535132465,
+                    "velocityX": 1.7602166982532683,
+                    "velocityY": -3.293652815925874,
+                    "timestamp": 1.3670949538196626
+                },
+                {
+                    "x": 4.510742037442817,
+                    "y": 4.51918682060045,
+                    "heading": 1.996778499947209,
+                    "angularVelocity": -1.002312624862085,
+                    "velocityX": 1.8428685709639585,
+                    "velocityY": -2.9144589607334965,
+                    "timestamp": 1.4217787519724492
+                },
+                {
+                    "x": 4.62612450472694,
+                    "y": 4.379617585881877,
+                    "heading": 1.9276644376664414,
+                    "angularVelocity": -1.263885549567404,
+                    "velocityX": 2.1099936577511538,
+                    "velocityY": -2.5522959163995464,
+                    "timestamp": 1.4764625501252358
+                },
+                {
+                    "x": 4.756154634809633,
+                    "y": 4.26013511834241,
+                    "heading": 1.8517335763640235,
+                    "angularVelocity": -1.3885440270675238,
+                    "velocityX": 2.3778547664042544,
+                    "velocityY": -2.1849701662205963,
+                    "timestamp": 1.5311463482780223
                 },
                 {
                     "x": 4.900439739227295,
                     "y": 4.161101341247559,
-                    "heading": -1.3678659526433012,
-                    "angularVelocity": -1.4260832947825601,
-                    "velocityX": 2.638605231839619,
-                    "velocityY": -1.8110430529880912,
-                    "timestamp": 1.5857469595508433
-                },
-                {
-                    "x": 5.00826704552834,
-                    "y": 4.101917336684186,
-                    "heading": -1.4217535551168057,
-                    "angularVelocity": -1.4073230082403518,
-                    "velocityX": 2.816006690010969,
-                    "velocityY": -1.5456432930522144,
-                    "timestamp": 1.6240378154225759
-                },
-                {
-                    "x": 5.122657163782486,
-                    "y": 4.053031768762854,
-                    "heading": -1.4735948904430307,
-                    "angularVelocity": -1.353882908752006,
-                    "velocityX": 2.9874004027836745,
-                    "velocityY": -1.2766903953541875,
-                    "timestamp": 1.6623286712943084
-                },
-                {
-                    "x": 5.24335031560154,
-                    "y": 4.014573699061021,
-                    "heading": -1.5218759194168912,
-                    "angularVelocity": -1.2609023192271587,
-                    "velocityX": 3.152009770253094,
-                    "velocityY": -1.0043669389543197,
-                    "timestamp": 1.700619527166041
-                },
-                {
-                    "x": 5.370053209601022,
-                    "y": 3.986652038058385,
-                    "heading": -1.5647151394478649,
-                    "angularVelocity": -1.1187846042009912,
-                    "velocityX": 3.308959570502,
-                    "velocityY": -0.729199187820931,
-                    "timestamp": 1.7389103830377735
-                },
-                {
-                    "x": 5.5024723935607405,
-                    "y": 3.969300942477628,
-                    "heading": -1.599514624372743,
-                    "angularVelocity": -0.9088197203387187,
-                    "velocityX": 3.458245603161718,
-                    "velocityY": -0.4531394032789495,
-                    "timestamp": 1.777201238909506
-                },
-                {
-                    "x": 5.64052782398134,
-                    "y": 3.9622378493580968,
-                    "heading": -1.6219128460749601,
-                    "angularVelocity": -0.5849496228876974,
-                    "velocityX": 3.6054412281370607,
-                    "velocityY": -0.1844590035592553,
-                    "timestamp": 1.8154920947812385
-                },
-                {
-                    "x": 5.78519636080909,
-                    "y": 3.9633730625174985,
-                    "heading": -1.6224984270976561,
-                    "angularVelocity": -0.015292972940008887,
-                    "velocityX": 3.778148425628394,
-                    "velocityY": 0.02964710852127253,
-                    "timestamp": 1.853782950652971
-                },
-                {
-                    "x": 5.929431807514691,
-                    "y": 3.9767231173381155,
-                    "heading": -1.6224986694398071,
-                    "angularVelocity": -0.000006328982347971965,
-                    "velocityX": 3.7668378891493703,
-                    "velocityY": 0.3486486399086302,
-                    "timestamp": 1.8920738065247036
-                },
-                {
-                    "x": 6.073330473121851,
-                    "y": 3.9933141696465464,
-                    "heading": -1.6224988987872904,
-                    "angularVelocity": -0.000005989614964631434,
-                    "velocityX": 3.758042549093057,
-                    "velocityY": 0.4332901924158613,
-                    "timestamp": 1.9303646623964361
-                },
-                {
-                    "x": 6.217229120638352,
-                    "y": 4.009905378860974,
-                    "heading": -1.6224991281340264,
-                    "angularVelocity": -0.000005989595448400927,
-                    "velocityX": 3.7580420766392404,
-                    "velocityY": 0.4332942901565158,
-                    "timestamp": 1.9686555182681686
-                },
-                {
-                    "x": 6.361127770496839,
-                    "y": 4.0264965677626225,
-                    "heading": -1.6224993574808346,
-                    "angularVelocity": -0.000005989597330541617,
-                    "velocityX": 3.7580421378023496,
-                    "velocityY": 0.4332937596700765,
-                    "timestamp": 2.0069463741399014
-                },
-                {
-                    "x": 6.505026421139362,
-                    "y": 4.0430877498636315,
-                    "heading": -1.622499586827807,
-                    "angularVelocity": -0.000005989601616416828,
-                    "velocityX": 3.7580421582781125,
-                    "velocityY": 0.43329358206529056,
-                    "timestamp": 2.045237230011634
-                },
-                {
-                    "x": 6.648925182384827,
-                    "y": 4.0596779726347245,
-                    "heading": -1.6224998161827902,
-                    "angularVelocity": -0.000005989810829077631,
-                    "velocityX": 3.7580450467730504,
-                    "velocityY": 0.43326852830525053,
-                    "timestamp": 2.0835280858833665
-                },
-                {
-                    "x": 6.790105794643628,
-                    "y": 4.068502844708353,
-                    "heading": -1.6329532702813698,
-                    "angularVelocity": -0.27300131743194306,
-                    "velocityX": 3.6870581512131766,
-                    "velocityY": 0.2304694390532754,
-                    "timestamp": 2.121818941755099
-                },
-                {
-                    "x": 6.921555592389516,
-                    "y": 4.075747346171521,
-                    "heading": -1.6719954127909697,
-                    "angularVelocity": -1.0196205235104698,
-                    "velocityX": 3.4329292138629897,
-                    "velocityY": 0.18919664494924698,
-                    "timestamp": 2.1601097976268315
-                },
-                {
-                    "x": 7.042894308610419,
-                    "y": 4.08202615388359,
-                    "heading": -1.737655401763884,
-                    "angularVelocity": -1.714769426749386,
-                    "velocityX": 3.168869262869602,
-                    "velocityY": 0.16397668762232157,
-                    "timestamp": 2.198400653498564
-                },
-                {
-                    "x": 7.153820883415079,
-                    "y": 4.087877633041295,
-                    "heading": -1.8279052524624368,
-                    "angularVelocity": -2.3569556920031554,
-                    "velocityX": 2.8969468631425612,
-                    "velocityY": 0.15281661964688553,
-                    "timestamp": 2.2366915093702966
-                },
-                {
-                    "x": 7.254173862078464,
-                    "y": 4.093777241980607,
-                    "heading": -1.9412919882105817,
-                    "angularVelocity": -2.96119616986284,
-                    "velocityX": 2.6208079286488037,
-                    "velocityY": 0.15407357200568295,
-                    "timestamp": 2.274982365242029
-                },
-                {
-                    "x": 7.343968095562701,
-                    "y": 4.100172778216793,
-                    "heading": -2.0772958326900723,
-                    "angularVelocity": -3.5518622235835817,
-                    "velocityX": 2.3450568403336534,
-                    "velocityY": 0.16702515758877745,
-                    "timestamp": 2.3132732211137617
-                },
-                {
-                    "x": 7.423449152791422,
-                    "y": 4.107548146969254,
-                    "heading": -2.2364797071963527,
-                    "angularVelocity": -4.15722947116975,
-                    "velocityX": 2.0757190044267455,
-                    "velocityY": 0.19261436143311772,
-                    "timestamp": 2.351564076985494
-                },
-                {
-                    "x": 7.492711517186474,
-                    "y": 4.113210958649556,
-                    "heading": -2.4189397780167137,
-                    "angularVelocity": -4.765108187489175,
-                    "velocityX": 1.8088486877145706,
-                    "velocityY": 0.147889399476235,
-                    "timestamp": 2.3898549328572267
-                },
-                {
-                    "x": 7.549783547522053,
-                    "y": 4.117992264935679,
-                    "heading": -2.6004162007304275,
-                    "angularVelocity": -4.739419336084478,
-                    "velocityX": 1.490487194299341,
-                    "velocityY": 0.12486809650168224,
-                    "timestamp": 2.4281457887289593
-                },
-                {
-                    "x": 7.595268726062492,
-                    "y": 4.121696461876066,
-                    "heading": -2.768130415473744,
-                    "angularVelocity": -4.380006947484507,
-                    "velocityX": 1.1878861807843983,
-                    "velocityY": 0.09673842112058428,
-                    "timestamp": 2.466436644600692
-                },
-                {
-                    "x": 7.630218683798498,
-                    "y": 4.124204213311165,
-                    "heading": -2.9120023323717077,
-                    "angularVelocity": -3.7573439825922996,
-                    "velocityX": 0.9127494525868564,
-                    "velocityY": 0.06549217503781085,
-                    "timestamp": 2.5047275004724243
-                },
-                {
-                    "x": 7.655568399114973,
-                    "y": 4.125734158703857,
-                    "heading": -3.0250833006842397,
-                    "angularVelocity": -2.9532107793916214,
-                    "velocityX": 0.6620305224148624,
-                    "velocityY": 0.03995589437375436,
-                    "timestamp": 2.543018356344157
-                },
-                {
-                    "x": 7.671992151604379,
-                    "y": 4.126594890877869,
-                    "heading": -3.103103884407084,
-                    "angularVelocity": -2.037577430606375,
-                    "velocityX": 0.42892100778377035,
-                    "velocityY": 0.022478791722372646,
-                    "timestamp": 2.5813092122158894
+                    "heading": 1.7737408289368037,
+                    "angularVelocity": -1.4262496399629772,
+                    "velocityX": 2.638534799915143,
+                    "velocityY": -1.8110259426046127,
+                    "timestamp": 1.5858301464308089
+                },
+                {
+                    "x": 5.008276114908647,
+                    "y": 4.1019129343639005,
+                    "heading": 1.719841693932476,
+                    "angularVelocity": -1.4074809230288232,
+                    "velocityX": 2.815956908545699,
+                    "velocityY": -1.5456009367595303,
+                    "timestamp": 1.6241248999315097
+                },
+                {
+                    "x": 5.122676890234505,
+                    "y": 4.053024888243358,
+                    "heading": 1.6679900254477509,
+                    "angularVelocity": -1.354014943163857,
+                    "velocityX": 2.987374636678728,
+                    "velocityY": -1.276625167978901,
+                    "timestamp": 1.6624196534322104
+                },
+                {
+                    "x": 5.243382451295884,
+                    "y": 4.014566134854959,
+                    "heading": 1.6197010617895011,
+                    "angularVelocity": -1.260981185252083,
+                    "velocityX": 3.1520130051018556,
+                    "velocityY": -1.0042825680467018,
+                    "timestamp": 1.7007144069329112
+                },
+                {
+                    "x": 5.370099790784461,
+                    "y": 3.9866453379407707,
+                    "heading": 1.576858310766692,
+                    "angularVelocity": -1.1187629402556432,
+                    "velocityX": 3.3090000040412137,
+                    "velocityY": -0.7291024059908604,
+                    "timestamp": 1.739009160433612
+                },
+                {
+                    "x": 5.502536097274536,
+                    "y": 3.9692960416426737,
+                    "heading": 1.5420636410872666,
+                    "angularVelocity": -0.9086014792806747,
+                    "velocityX": 3.4583407486263615,
+                    "velocityY": -0.45304629778540073,
+                    "timestamp": 1.7773039139343128
+                },
+                {
+                    "x": 5.640613600815518,
+                    "y": 3.9622331858505495,
+                    "heading": 1.5196895147171037,
+                    "angularVelocity": -0.5842608797508935,
+                    "velocityX": 3.6056506680074274,
+                    "velocityY": -0.18443403198808295,
+                    "timestamp": 1.8155986674350135
+                },
+                {
+                    "x": 5.785301546169388,
+                    "y": 3.963377277975263,
+                    "heading": 1.519119424890785,
+                    "angularVelocity": -0.014886891132695255,
+                    "velocityX": 3.778270706226711,
+                    "velocityY": 0.029875949578638192,
+                    "timestamp": 1.8538934209357143
+                },
+                {
+                    "x": 5.929550760809323,
+                    "y": 3.9767385598159253,
+                    "heading": 1.5191191822149777,
+                    "angularVelocity": -0.000006337051042848807,
+                    "velocityX": 3.7668140268168893,
+                    "velocityY": 0.3489063283934661,
+                    "timestamp": 1.892188174436415
+                },
+                {
+                    "x": 6.073462686179593,
+                    "y": 3.9933433315789473,
+                    "heading": 1.5191189525644635,
+                    "angularVelocity": -0.0000059969184602345635,
+                    "velocityX": 3.7580063119517533,
+                    "velocityY": 0.4336043516435729,
+                    "timestamp": 1.9304829279371158
+                },
+                {
+                    "x": 6.217374593429904,
+                    "y": 4.009948260386706,
+                    "heading": 1.519118722914699,
+                    "angularVelocity": -0.000005996898884562842,
+                    "velocityX": 3.7580058387809343,
+                    "velocityY": 0.43360845259010783,
+                    "timestamp": 1.9687776814378166
+                },
+                {
+                    "x": 6.361286503020876,
+                    "y": 4.026553168908082,
+                    "heading": 1.5191184932648618,
+                    "angularVelocity": -0.000005996900776462565,
+                    "velocityX": 3.7580058999032113,
+                    "velocityY": 0.4336079228469906,
+                    "timestamp": 2.0070724349385176
+                },
+                {
+                    "x": 6.5051984133944005,
+                    "y": 4.04315807064667,
+                    "heading": 1.5191182636148604,
+                    "angularVelocity": -0.000005996905070522029,
+                    "velocityX": 3.7580059203381437,
+                    "velocityY": 0.4336077457264339,
+                    "timestamp": 2.0453671884392186
+                },
+                {
+                    "x": 6.6491104352583505,
+                    "y": 4.059762006067292,
+                    "heading": 1.5191180339567425,
+                    "angularVelocity": -0.000005997117020238007,
+                    "velocityX": 3.7580088317141747,
+                    "velocityY": 0.4335825120357026,
+                    "timestamp": 2.0836619419399196
+                },
+                {
+                    "x": 6.790280334982046,
+                    "y": 4.068588148423426,
+                    "heading": 1.5085875786821552,
+                    "angularVelocity": -0.27498428144717973,
+                    "velocityX": 3.6864031445224414,
+                    "velocityY": 0.23047915312922457,
+                    "timestamp": 2.1219566954406206
+                },
+                {
+                    "x": 6.921716704437578,
+                    "y": 4.075833761978748,
+                    "heading": 1.4694677109965053,
+                    "angularVelocity": -1.0215464028234083,
+                    "velocityX": 3.43222915517987,
+                    "velocityY": 0.18920642889606504,
+                    "timestamp": 2.1602514489413216
+                },
+                {
+                    "x": 7.043039125371844,
+                    "y": 4.08211395301362,
+                    "heading": 1.4037305732475935,
+                    "angularVelocity": -1.716609502335841,
+                    "velocityX": 3.1681212135768497,
+                    "velocityY": 0.16399612115943957,
+                    "timestamp": 2.1985462024420226
+                },
+                {
+                    "x": 7.153946697571625,
+                    "y": 4.087967374487544,
+                    "heading": 1.3134036899027293,
+                    "angularVelocity": -2.3587273735346383,
+                    "velocityX": 2.896155793188521,
+                    "velocityY": 0.1528517861805002,
+                    "timestamp": 2.2368409559427236
+                },
+                {
+                    "x": 7.254278338500056,
+                    "y": 4.0938696715910385,
+                    "heading": 1.199938125135736,
+                    "angularVelocity": -2.9629532610757465,
+                    "velocityX": 2.619983986229206,
+                    "velocityY": 0.1541280871121784,
+                    "timestamp": 2.2751357094434246
+                },
+                {
+                    "x": 7.34404947628579,
+                    "y": 4.100268813517142,
+                    "heading": 1.0638507127340207,
+                    "angularVelocity": -3.5536829450860603,
+                    "velocityX": 2.344215057660344,
+                    "velocityY": 0.16710231405423262,
+                    "timestamp": 2.3134304629441256
+                },
+                {
+                    "x": 7.4235066081317465,
+                    "y": 4.107648882181887,
+                    "heading": 0.9045749783818139,
+                    "angularVelocity": -4.159205107542786,
+                    "velocityX": 2.0748829691383666,
+                    "velocityY": 0.192717487125464,
+                    "timestamp": 2.3517252164448266
+                },
+                {
+                    "x": 7.492760083799764,
+                    "y": 4.113297423743801,
+                    "heading": 0.7219171880631509,
+                    "angularVelocity": -4.769786292404788,
+                    "velocityX": 1.808432470175071,
+                    "velocityY": 0.1475017083426549,
+                    "timestamp": 2.3900199699455276
+                },
+                {
+                    "x": 7.549820937161571,
+                    "y": 4.1180620764150975,
+                    "heading": 0.5402485968001275,
+                    "angularVelocity": -4.7439551023536115,
+                    "velocityX": 1.4900436259698675,
+                    "velocityY": 0.1244205076606485,
+                    "timestamp": 2.4283147234462286
+                },
+                {
+                    "x": 7.595294548959434,
+                    "y": 4.121747510359626,
+                    "heading": 0.3723680212509228,
+                    "angularVelocity": -4.383905370904984,
+                    "velocityX": 1.1874632329734431,
+                    "velocityY": 0.09623861254154589,
+                    "timestamp": 2.4666094769469296
+                },
+                {
+                    "x": 7.630234770559726,
+                    "y": 4.124236647711556,
+                    "heading": 0.22836852122657442,
+                    "angularVelocity": -3.7602931697083015,
+                    "velocityX": 0.912402311184801,
+                    "velocityY": 0.06499943528517171,
+                    "timestamp": 2.5049042304476306
+                },
+                {
+                    "x": 7.655576852148959,
+                    "y": 4.125750923706981,
+                    "heading": 0.11519495737029227,
+                    "angularVelocity": -2.9553281718920323,
+                    "velocityX": 0.6617637998053468,
+                    "velocityY": 0.03954264897925492,
+                    "timestamp": 2.5431989839483315
+                },
+                {
+                    "x": 7.671995118301485,
+                    "y": 4.1266005721732935,
+                    "heading": 0.03711275718074835,
+                    "angularVelocity": -2.0389790520029063,
+                    "velocityX": 0.4287340863083462,
+                    "velocityY": 0.02218707234390751,
+                    "timestamp": 2.5814937374490325
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -1.0467622585080842,
-                    "velocityX": 0.20913213385579937,
-                    "velocityY": 0.010579787599633836,
-                    "timestamp": 2.619600068087622
+                    "heading": -0.003,
+                    "angularVelocity": -1.047473962197309,
+                    "velocityX": 0.20903337838088354,
+                    "velocityY": 0.010430353774151994,
+                    "timestamp": 2.6197884909497335
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": 7.101458819208695e-33,
-                    "velocityX": -7.030931331746161e-34,
-                    "velocityY": 2.5921613146305674e-34,
-                    "timestamp": 2.6578909239593544
+                    "heading": -0.003,
+                    "angularVelocity": 7.250240493925158e-33,
+                    "velocityX": 3.887711813813661e-34,
+                    "velocityY": -4.900942603355191e-34,
+                    "timestamp": 2.6580832444504345
                 }
             ],
             "constraints": [
@@ -1887,25 +1887,25 @@
                 {
                     "x": 0.71,
                     "y": 6.72,
-                    "heading": -2.1,
+                    "heading": 1.04,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 10
+                    "controlIntervalCount": 27
                 },
                 {
-                    "x": 5.018874645233154,
-                    "y": 4.29060697555542,
-                    "heading": 0,
+                    "x": 5.489266395568848,
+                    "y": 4.304305553436279,
+                    "heading": -0.5942133396987443,
                     "isInitialGuess": false,
                     "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 10
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": -3.143185307179586,
+                    "heading": -0.003,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -1916,191 +1916,398 @@
                 {
                     "x": 0.71,
                     "y": 6.72,
-                    "heading": -2.1,
+                    "heading": 1.04,
                     "angularVelocity": 0,
-                    "velocityX": -3.0738838344554265e-34,
-                    "velocityY": -1.6532575942515022e-34,
+                    "velocityX": 0,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.8675349003884936,
-                    "y": 6.638058463961624,
-                    "heading": -2.192308349350307,
-                    "angularVelocity": -0.6292858581626611,
-                    "velocityX": 1.0739492763036018,
-                    "velocityY": -0.5586130635852894,
-                    "timestamp": 0.14668746826731788
-                },
-                {
-                    "x": 1.183655074306953,
-                    "y": 6.473510793186018,
-                    "heading": -2.3371750269963787,
-                    "angularVelocity": -0.9875872789764978,
-                    "velocityX": 2.155059172078514,
-                    "velocityY": -1.12175683934867,
-                    "timestamp": 0.29337493653463576
-                },
-                {
-                    "x": 1.6571019357484182,
-                    "y": 6.226344402054212,
-                    "heading": -2.393383986431118,
-                    "angularVelocity": -0.3831885579503364,
-                    "velocityX": 3.2275890165250725,
-                    "velocityY": -1.6849864139817283,
-                    "timestamp": 0.44006240480195363
-                },
-                {
-                    "x": 2.149819339361714,
-                    "y": 5.971090303282803,
-                    "heading": -2.3933840219605123,
-                    "angularVelocity": -2.422115180983058e-7,
-                    "velocityX": 3.3589604444967707,
-                    "velocityY": -1.7401220553227135,
-                    "timestamp": 0.5867498730692715
-                },
-                {
-                    "x": 2.6425367239446045,
-                    "y": 5.71583616777695,
-                    "heading": -2.3933840574898846,
-                    "angularVelocity": -2.422113667505868e-7,
-                    "velocityX": 3.358960314762406,
-                    "velocityY": -1.740122305749301,
-                    "timestamp": 0.7334373413365893
-                },
-                {
-                    "x": 3.135254076780422,
-                    "y": 5.4605819709896695,
-                    "heading": -2.3933840930192347,
-                    "angularVelocity": -2.4221121497556687e-7,
-                    "velocityX": 3.358960098335784,
-                    "velocityY": -1.7401227235179682,
-                    "timestamp": 0.8801248096039072
-                },
-                {
-                    "x": 3.627971327740651,
-                    "y": 5.20532757755194,
-                    "heading": -2.393384128548559,
-                    "angularVelocity": -2.4221104201937785e-7,
-                    "velocityX": 3.358959403828,
-                    "velocityY": -1.7401240641263462,
-                    "timestamp": 1.026812277871225
-                },
-                {
-                    "x": 4.091605856430787,
-                    "y": 4.900420846350941,
-                    "heading": -2.393384184524006,
-                    "angularVelocity": -3.815966513199635e-7,
-                    "velocityX": 3.160696235108687,
-                    "velocityY": -2.07861472287018,
-                    "timestamp": 1.1734997461385428
-                },
-                {
-                    "x": 4.555240236344247,
-                    "y": 4.595513888923491,
-                    "heading": -2.393384240499462,
-                    "angularVelocity": -3.815967160070403e-7,
-                    "velocityX": 3.1606952208661028,
-                    "velocityY": -2.078616265104517,
-                    "timestamp": 1.3201872144058606
-                },
-                {
-                    "x": 5.018874645233154,
-                    "y": 4.29060697555542,
-                    "heading": -2.393384296474878,
-                    "angularVelocity": -3.8159644312835243e-7,
-                    "velocityX": 3.1606954183979536,
-                    "velocityY": -2.078615964742264,
-                    "timestamp": 1.4668746826731784
-                },
-                {
-                    "x": 5.414956983036457,
-                    "y": 4.102744630717356,
-                    "heading": -2.393384340957434,
-                    "angularVelocity": -3.838597081329049e-7,
-                    "velocityX": 3.4179702017234304,
-                    "velocityY": -1.621147512518649,
-                    "timestamp": 1.5827570088571772
-                },
-                {
-                    "x": 5.811039262323732,
-                    "y": 3.9148821625062062,
-                    "heading": -2.3933843854399766,
-                    "angularVelocity": -3.838595921618151e-7,
-                    "velocityX": 3.4179696967626736,
-                    "velocityY": -1.6211485771597347,
-                    "timestamp": 1.698639335041176
-                },
-                {
-                    "x": 6.175303264444137,
-                    "y": 3.6709862524403962,
-                    "heading": -2.39338444094527,
-                    "angularVelocity": -4.789798009551726e-7,
-                    "velocityX": 3.143395667964284,
-                    "velocityY": -2.1046860043053552,
-                    "timestamp": 1.8145216612251747
-                },
-                {
-                    "x": 6.517947229314261,
-                    "y": 3.397547803025246,
-                    "heading": -2.3933845251610077,
-                    "angularVelocity": -7.267349611095117e-7,
-                    "velocityX": 2.9568267755177056,
-                    "velocityY": -2.3596216819205247,
-                    "timestamp": 1.9304039874091734
-                },
-                {
-                    "x": 6.860591017465818,
-                    "y": 3.124109132165372,
-                    "heading": -2.3933846093768096,
-                    "angularVelocity": -7.267355129115313e-7,
-                    "velocityX": 2.9568252505347843,
-                    "velocityY": -2.3596235928652884,
-                    "timestamp": 2.046286313593172
-                },
-                {
-                    "x": 7.1795790741631205,
-                    "y": 2.8553069978284444,
-                    "heading": -2.4556792266097736,
-                    "angularVelocity": -0.5375678870482116,
-                    "velocityX": 2.7526894497337926,
-                    "velocityY": -2.3196128623628227,
-                    "timestamp": 2.1621686397771707
-                },
-                {
-                    "x": 7.431770863942688,
-                    "y": 2.653048744111035,
-                    "heading": -2.7460349787453167,
-                    "angularVelocity": -2.5056085918961837,
-                    "velocityX": 2.1762748305478063,
-                    "velocityY": -1.7453761965069832,
-                    "timestamp": 2.278050965961169
-                },
-                {
-                    "x": 7.5976853712302646,
-                    "y": 2.521507979737877,
-                    "heading": -3.002083319178936,
-                    "angularVelocity": -2.2095547169726615,
-                    "velocityX": 1.4317498858638353,
-                    "velocityY": -1.135123609482061,
-                    "timestamp": 2.3939332921451677
+                    "x": 0.7271139423991175,
+                    "y": 6.709732017673653,
+                    "heading": 0.9305052411067223,
+                    "angularVelocity": -1.767895408627961,
+                    "velocityX": 0.2763206248110281,
+                    "velocityY": -0.16578618916650492,
+                    "timestamp": 0.06193508867035021
+                },
+                {
+                    "x": 0.7615532933104725,
+                    "y": 6.686914726598375,
+                    "heading": 0.7206788937202419,
+                    "angularVelocity": -3.387842851138587,
+                    "velocityX": 0.5560555680263672,
+                    "velocityY": -0.36840652956392367,
+                    "timestamp": 0.12387017734070042
+                },
+                {
+                    "x": 0.8202948153873781,
+                    "y": 6.650989300229197,
+                    "heading": 0.4540271404715134,
+                    "angularVelocity": -4.305342237709284,
+                    "velocityX": 0.9484368770271357,
+                    "velocityY": -0.5800496477915925,
+                    "timestamp": 0.18580526601105063
+                },
+                {
+                    "x": 0.9068549514543394,
+                    "y": 6.604915057765644,
+                    "heading": 0.1437481054753499,
+                    "angularVelocity": -5.009745552276917,
+                    "velocityX": 1.3975944480789861,
+                    "velocityY": -0.7439117865606645,
+                    "timestamp": 0.24774035468140085
+                },
+                {
+                    "x": 1.022793030858839,
+                    "y": 6.550181878656492,
+                    "heading": -0.13251218552677876,
+                    "angularVelocity": -4.460481076769346,
+                    "velocityX": 1.871928851536495,
+                    "velocityY": -0.8837184265686551,
+                    "timestamp": 0.30967544335175107
+                },
+                {
+                    "x": 1.1650239086947571,
+                    "y": 6.483494014593417,
+                    "heading": -0.35594040398018906,
+                    "angularVelocity": -3.6074577957352734,
+                    "velocityX": 2.296450701684509,
+                    "velocityY": -1.0767380090149254,
+                    "timestamp": 0.37161053202210126
+                },
+                {
+                    "x": 1.3271753983931303,
+                    "y": 6.398307258999271,
+                    "heading": -0.5136957877872723,
+                    "angularVelocity": -2.5471083870846947,
+                    "velocityX": 2.61808763302859,
+                    "velocityY": -1.3754199343695408,
+                    "timestamp": 0.43354562069245145
+                },
+                {
+                    "x": 1.510544798478812,
+                    "y": 6.2989823057862875,
+                    "heading": -0.5925289533905876,
+                    "angularVelocity": -1.2728352747326335,
+                    "velocityX": 2.96067066379232,
+                    "velocityY": -1.6036943733404823,
+                    "timestamp": 0.49548070936280164
+                },
+                {
+                    "x": 1.7178378905511122,
+                    "y": 6.190936000456915,
+                    "heading": -0.5942013489806535,
+                    "angularVelocity": -0.027002392762643195,
+                    "velocityX": 3.3469410720572084,
+                    "velocityY": -1.7445087695675872,
+                    "timestamp": 0.5574157980331519
+                },
+                {
+                    "x": 1.927701821836241,
+                    "y": 6.086763021965953,
+                    "heading": -0.5942019920614379,
+                    "angularVelocity": -0.000010383141417040622,
+                    "velocityX": 3.388449678374247,
+                    "velocityY": -1.6819702809408126,
+                    "timestamp": 0.6193508867035021
+                },
+                {
+                    "x": 2.1375657815463693,
+                    "y": 5.982590100737573,
+                    "heading": -0.594202635139582,
+                    "angularVelocity": -0.0000103830987882079,
+                    "velocityX": 3.3884501373224802,
+                    "velocityY": -1.6819693563827811,
+                    "timestamp": 0.6812859753738524
+                },
+                {
+                    "x": 2.347429741257157,
+                    "y": 5.878417179509917,
+                    "heading": -0.5942032782166262,
+                    "angularVelocity": -0.000010383081029176535,
+                    "velocityX": 3.3884501373331317,
+                    "velocityY": -1.6819693563710962,
+                    "timestamp": 0.7432210640442026
+                },
+                {
+                    "x": 2.5572937009682177,
+                    "y": 5.774244258282216,
+                    "heading": -0.5942039212925836,
+                    "angularVelocity": -0.000010383063479788062,
+                    "velocityX": 3.3884501373375415,
+                    "velocityY": -1.6819693563718265,
+                    "timestamp": 0.8051561527145529
+                },
+                {
+                    "x": 2.7671576606795463,
+                    "y": 5.670071337054474,
+                    "heading": -0.594204564367474,
+                    "angularVelocity": -0.00001038304625561267,
+                    "velocityX": 3.388450137341862,
+                    "velocityY": -1.68196935637249,
+                    "timestamp": 0.8670912413849031
+                },
+                {
+                    "x": 2.977021620391137,
+                    "y": 5.5658984158266955,
+                    "heading": -0.5942052074413191,
+                    "angularVelocity": -0.000010383029375253952,
+                    "velocityX": 3.388450137346089,
+                    "velocityY": -1.6819693563730826,
+                    "timestamp": 0.9290263300552534
+                },
+                {
+                    "x": 3.186885580102983,
+                    "y": 5.461725494598884,
+                    "heading": -0.5942058505141404,
+                    "angularVelocity": -0.000010383012846871076,
+                    "velocityX": 3.3884501373502207,
+                    "velocityY": -1.6819693563736025,
+                    "timestamp": 0.9909614187256036
+                },
+                {
+                    "x": 3.3967495398150795,
+                    "y": 5.357552573371046,
+                    "heading": -0.594206493585961,
+                    "angularVelocity": -0.000010382996688205053,
+                    "velocityX": 3.3884501373542517,
+                    "velocityY": -1.681969356374048,
+                    "timestamp": 1.0528965073959538
+                },
+                {
+                    "x": 3.6066134995274184,
+                    "y": 5.253379652143185,
+                    "heading": -0.5942071366568044,
+                    "angularVelocity": -0.000010382980910469912,
+                    "velocityX": 3.38845013735818,
+                    "velocityY": -1.6819693563744145,
+                    "timestamp": 1.114831596066304
+                },
+                {
+                    "x": 3.8164774592399944,
+                    "y": 5.149206730915306,
+                    "heading": -0.5942077797266946,
+                    "angularVelocity": -0.000010382965523044611,
+                    "velocityX": 3.388450137362003,
+                    "velocityY": -1.681969356374701,
+                    "timestamp": 1.1767666847366542
+                },
+                {
+                    "x": 4.026341418952801,
+                    "y": 5.045033809687414,
+                    "heading": -0.594208422795657,
+                    "angularVelocity": -0.00001038295054018878,
+                    "velocityX": 3.388450137365716,
+                    "velocityY": -1.6819693563749047,
+                    "timestamp": 1.2387017734070045
+                },
+                {
+                    "x": 4.236205378665829,
+                    "y": 4.940860888459515,
+                    "heading": -0.594209065863717,
+                    "angularVelocity": -0.000010382935971395078,
+                    "velocityX": 3.388450137369317,
+                    "velocityY": -1.681969356375023,
+                    "timestamp": 1.3006368620773547
+                },
+                {
+                    "x": 4.446069338379075,
+                    "y": 4.836687967231613,
+                    "heading": -0.5942097089309012,
+                    "angularVelocity": -0.000010382921828372961,
+                    "velocityX": 3.3884501373728035,
+                    "velocityY": -1.6819693563750544,
+                    "timestamp": 1.362571950747705
+                },
+                {
+                    "x": 4.6559332980925285,
+                    "y": 4.732515046003717,
+                    "heading": -0.5942103519972364,
+                    "angularVelocity": -0.000010382908121631514,
+                    "velocityX": 3.388450137376173,
+                    "velocityY": -1.6819693563749973,
+                    "timestamp": 1.4245070394180552
+                },
+                {
+                    "x": 4.865797257806183,
+                    "y": 4.628342124775828,
+                    "heading": -0.5942109950627501,
+                    "angularVelocity": -0.00001038289485891648,
+                    "velocityX": 3.388450137379422,
+                    "velocityY": -1.6819693563748486,
+                    "timestamp": 1.4864421280884055
+                },
+                {
+                    "x": 5.075661217518705,
+                    "y": 4.524169203545277,
+                    "heading": -0.594211638127464,
+                    "angularVelocity": -0.000010382881945026341,
+                    "velocityX": 3.3884501373611275,
+                    "velocityY": -1.6819693564178315,
+                    "timestamp": 1.5483772167587557
+                },
+                {
+                    "x": 5.285525078653758,
+                    "y": 4.419996083725492,
+                    "heading": -0.5942122811951368,
+                    "angularVelocity": -0.000010382929719206097,
+                    "velocityX": 3.3884485457355824,
+                    "velocityY": -1.6819725628269917,
+                    "timestamp": 1.610312305429106
+                },
+                {
+                    "x": 5.489266395568848,
+                    "y": 4.304305553436279,
+                    "heading": -0.5942133396987443,
+                    "angularVelocity": -0.000017090531880803138,
+                    "velocityX": 3.2895943364108793,
+                    "velocityY": -1.8679319392755818,
+                    "timestamp": 1.6722473940994562
+                },
+                {
+                    "x": 5.681024172067428,
+                    "y": 4.156179263559471,
+                    "heading": -0.5942132880529166,
+                    "angularVelocity": 8.063055616728172e-7,
+                    "velocityX": 2.9937628707653796,
+                    "velocityY": -2.312578894659331,
+                    "timestamp": 1.7362998206864544
+                },
+                {
+                    "x": 5.864967854996084,
+                    "y": 3.998454299605836,
+                    "heading": -0.5942132372714615,
+                    "angularVelocity": 7.928107921043856e-7,
+                    "velocityX": 2.871767593048443,
+                    "velocityY": -2.4624354198261003,
+                    "timestamp": 1.8003522472734526
+                },
+                {
+                    "x": 6.0489114264021975,
+                    "y": 3.840729205591288,
+                    "heading": -0.5942131864900138,
+                    "angularVelocity": 7.928106775338953e-7,
+                    "velocityX": 2.871765851934987,
+                    "velocityY": -2.4624374503645132,
+                    "timestamp": 1.8644046738604507
+                },
+                {
+                    "x": 6.232854997807022,
+                    "y": 3.683004111575237,
+                    "heading": -0.5942131357085663,
+                    "angularVelocity": 7.928106743629292e-7,
+                    "velocityX": 2.87176585191486,
+                    "velocityY": -2.462437450387986,
+                    "timestamp": 1.928457100447449
+                },
+                {
+                    "x": 6.4167985692118465,
+                    "y": 3.5252790175591864,
+                    "heading": -0.594213084927119,
+                    "angularVelocity": 7.928106702567426e-7,
+                    "velocityX": 2.871765851914861,
+                    "velocityY": -2.462437450387986,
+                    "timestamp": 1.992509527034447
+                },
+                {
+                    "x": 6.6007421406166715,
+                    "y": 3.367553923543135,
+                    "heading": -0.5942130341456721,
+                    "angularVelocity": 7.928106666471698e-7,
+                    "velocityX": 2.8717658519148612,
+                    "velocityY": -2.462437450387985,
+                    "timestamp": 2.0565619536214452
+                },
+                {
+                    "x": 6.784685712022088,
+                    "y": 3.2098288295277735,
+                    "heading": -0.5942129833642253,
+                    "angularVelocity": 7.928106614555171e-7,
+                    "velocityX": 2.8717658519240943,
+                    "velocityY": -2.4624374503772177,
+                    "timestamp": 2.120614380208443
+                },
+                {
+                    "x": 6.968629334581004,
+                    "y": 3.052103795169223,
+                    "heading": -0.594212932582592,
+                    "angularVelocity": 7.928135744203515e-7,
+                    "velocityX": 2.871766650543336,
+                    "velocityY": -2.462436519002493,
+                    "timestamp": 2.184666806795441
+                },
+                {
+                    "x": 7.14554518368725,
+                    "y": 2.9064126516878526,
+                    "heading": -0.554455950225117,
+                    "angularVelocity": 0.6206943979472176,
+                    "velocityX": 2.7620475684235433,
+                    "velocityY": -2.2745608752775093,
+                    "timestamp": 2.248719233382439
+                },
+                {
+                    "x": 7.299470766691596,
+                    "y": 2.7775737815312107,
+                    "heading": -0.43989859569345574,
+                    "angularVelocity": 1.7884935924490803,
+                    "velocityX": 2.4031186827134454,
+                    "velocityY": -2.0114596280228136,
+                    "timestamp": 2.312771659969437
+                },
+                {
+                    "x": 7.4271138269599435,
+                    "y": 2.670230084319628,
+                    "heading": -0.3103960824325961,
+                    "angularVelocity": 2.021820564205559,
+                    "velocityX": 1.9927903917110543,
+                    "velocityY": -1.6758724521667427,
+                    "timestamp": 2.376824086556435
+                },
+                {
+                    "x": 7.52865057013146,
+                    "y": 2.5846878922538004,
+                    "heading": -0.1939655985882249,
+                    "angularVelocity": 1.8177372825404448,
+                    "velocityX": 1.5852130603296692,
+                    "velocityY": -1.3355027533522315,
+                    "timestamp": 2.440876513143433
+                },
+                {
+                    "x": 7.604480145165485,
+                    "y": 2.520737422138354,
+                    "heading": -0.10094853997619462,
+                    "angularVelocity": 1.4522019471922978,
+                    "velocityX": 1.1838673267285131,
+                    "velocityY": -0.9984082340516305,
+                    "timestamp": 2.504928939730431
+                },
+                {
+                    "x": 7.654868393442648,
+                    "y": 2.4782123742697575,
+                    "heading": -0.03632146129528005,
+                    "angularVelocity": 1.008971589751342,
+                    "velocityX": 0.7866719648587251,
+                    "velocityY": -0.6639100208145539,
+                    "timestamp": 2.568981366317429
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -1.2176316496840747,
-                    "velocityX": 0.7103294478144603,
-                    "velocityY": -0.5566679739881255,
-                    "timestamp": 2.509815618329166
+                    "heading": -0.003,
+                    "angularVelocity": 0.5202216851226045,
+                    "velocityX": 0.39235994475896174,
+                    "velocityY": -0.3311720632620588,
+                    "timestamp": 2.6330337929044267
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": 2.6221634517625535e-33,
-                    "velocityX": -1.3293034908554191e-34,
+                    "heading": -0.003,
+                    "angularVelocity": -9.133242422473854e-34,
+                    "velocityX": 1.8081868884139647e-34,
                     "velocityY": 0,
-                    "timestamp": 2.6256979445131647
+                    "timestamp": 2.6970862194914247
                 }
             ],
             "constraints": [
@@ -2117,28 +2324,17 @@
                     "type": "StopPoint"
                 }
             ],
-            "usesControlIntervalGuessing": false,
+            "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 10,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.2201673221588134,
-                    "y": 3.984490909576416,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.587796077728272,
-                    "y": 2.6101673221588135,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "Amp Side To C5": {
             "waypoints": [
                 {
                     "x": 0.71,
                     "y": 6.72,
-                    "heading": -2.1,
+                    "heading": 1.04,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -2147,7 +2343,7 @@
                 {
                     "x": 4.092995643615723,
                     "y": 5.023272514343262,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -2156,7 +2352,7 @@
                 {
                     "x": 5.7832255363464355,
                     "y": 3.9654417037963867,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -2165,7 +2361,7 @@
                 {
                     "x": 7.140008449554443,
                     "y": 3.2525558471679688,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -2174,7 +2370,7 @@
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": -3.143185307179586,
+                    "heading": -0.003,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -2185,659 +2381,659 @@
                 {
                     "x": 0.71,
                     "y": 6.72,
-                    "heading": -2.1,
+                    "heading": 1.04,
                     "angularVelocity": 0,
-                    "velocityX": 3.044182207072049e-31,
-                    "velocityY": 6.458172299259412e-31,
+                    "velocityX": -2.70270630963226e-31,
+                    "velocityY": -3.181766396851851e-31,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7256414580836799,
-                    "y": 6.712155644836975,
-                    "heading": -2.1163027262729814,
-                    "angularVelocity": -0.3503504178414695,
-                    "velocityX": 0.3361395685300024,
-                    "velocityY": -0.16857751660924442,
-                    "timestamp": 0.04653262974092379
-                },
-                {
-                    "x": 0.7569790078652245,
-                    "y": 6.696436652979446,
-                    "heading": -2.1479259826479233,
-                    "angularVelocity": -0.6795931489587892,
-                    "velocityX": 0.6734532296158987,
-                    "velocityY": -0.3378057923018942,
-                    "timestamp": 0.09306525948184757
-                },
-                {
-                    "x": 0.8040786472875676,
-                    "y": 6.672808979329537,
-                    "heading": -2.1936219074673273,
-                    "angularVelocity": -0.9820189633351775,
-                    "velocityX": 1.0121852060495677,
-                    "velocityY": -0.5077657072349027,
-                    "timestamp": 0.13959788922277136
-                },
-                {
-                    "x": 0.8670199327112733,
-                    "y": 6.641236276400096,
-                    "heading": -2.2517861808528257,
-                    "angularVelocity": -1.2499674681912987,
-                    "velocityX": 1.3526268722430308,
-                    "velocityY": -0.6785067404360078,
-                    "timestamp": 0.18613051896369515
-                },
-                {
-                    "x": 0.9458962124147239,
-                    "y": 6.601679900322021,
-                    "heading": -2.32031104809549,
-                    "angularVelocity": -1.4726196998587933,
-                    "velocityX": 1.6950746205965188,
-                    "velocityY": -0.8500782418336558,
-                    "timestamp": 0.23266314870461893
-                },
-                {
-                    "x": 1.0408105422873288,
-                    "y": 6.554096827806623,
-                    "heading": -2.396269750321632,
-                    "angularVelocity": -1.6323750161792245,
-                    "velocityX": 2.039737070547226,
-                    "velocityY": -1.022574326452373,
-                    "timestamp": 0.2791957784455427
-                },
-                {
-                    "x": 1.1518552587221624,
-                    "y": 6.498443176244561,
-                    "heading": -2.4750808863817895,
-                    "angularVelocity": -1.6936746644023601,
-                    "velocityX": 2.3863838569428775,
-                    "velocityY": -1.19601346134745,
-                    "timestamp": 0.32572840818646653
-                },
-                {
-                    "x": 1.2789760591669448,
-                    "y": 6.434740942054882,
-                    "heading": -2.5479819963693036,
-                    "angularVelocity": -1.5666664530556718,
-                    "velocityX": 2.7318636654010686,
-                    "velocityY": -1.3689798866801088,
-                    "timestamp": 0.3722610379273903
-                },
-                {
-                    "x": 1.4202169645474811,
-                    "y": 6.363052674678261,
-                    "heading": -2.59222819945715,
-                    "angularVelocity": -0.9508640137943395,
-                    "velocityX": 3.0353089040296184,
-                    "velocityY": -1.5406021060002604,
-                    "timestamp": 0.41879366766831405
-                },
-                {
-                    "x": 1.573995110361942,
-                    "y": 6.28311594811204,
-                    "heading": -2.599064012600599,
-                    "angularVelocity": -0.14690364979431952,
-                    "velocityX": 3.304737915536797,
-                    "velocityY": -1.7178639378700842,
-                    "timestamp": 0.4653262974092378
-                },
-                {
-                    "x": 1.7314466383055478,
-                    "y": 6.204403629832344,
-                    "heading": -2.599064059608806,
-                    "angularVelocity": -0.0000010102203007383576,
-                    "velocityX": 3.3836799858560402,
-                    "velocityY": -1.6915510410205123,
-                    "timestamp": 0.5118589271501616
-                },
-                {
-                    "x": 1.8889032791179137,
-                    "y": 6.1257015398581975,
-                    "heading": -2.5990641066080484,
-                    "angularVelocity": -0.0000010100276450499987,
-                    "velocityX": 3.3837898629208136,
-                    "velocityY": -1.69133123170322,
-                    "timestamp": 0.5583915568910853
-                },
-                {
-                    "x": 2.0463595988203984,
-                    "y": 6.046998807454145,
-                    "heading": -2.5990641536073187,
-                    "angularVelocity": -0.0000010100282384452147,
-                    "velocityX": 3.3837829621742297,
-                    "velocityY": -1.691345037712453,
-                    "timestamp": 0.6049241866320091
-                },
-                {
-                    "x": 2.2038159116861142,
-                    "y": 5.968296061372362,
-                    "heading": -2.599064200606589,
-                    "angularVelocity": -0.0000010100282469186477,
-                    "velocityX": 3.3837828152499863,
-                    "velocityY": -1.6913453316510296,
-                    "timestamp": 0.6514568163729328
-                },
-                {
-                    "x": 2.3612722547062472,
-                    "y": 5.889593375618833,
-                    "heading": -2.5990642476058565,
-                    "angularVelocity": -0.0000010100281851545483,
-                    "velocityX": 3.3837834632773647,
-                    "velocityY": -1.6913440351789604,
-                    "timestamp": 0.6979894461138566
-                },
-                {
-                    "x": 2.5187286297414215,
-                    "y": 5.8108907539159365,
-                    "heading": -2.599064294605121,
-                    "angularVelocity": -0.0000010100281210334782,
-                    "velocityX": 3.383784151290135,
-                    "velocityY": -1.6913426587117795,
-                    "timestamp": 0.7445220758547804
-                },
-                {
-                    "x": 2.6761850576997652,
-                    "y": 5.732188238093708,
-                    "heading": -2.599064341604381,
-                    "angularVelocity": -0.000001010028016565385,
-                    "velocityX": 3.3837852886247464,
-                    "velocityY": -1.6913403833048126,
-                    "timestamp": 0.7910547055957041
-                },
-                {
-                    "x": 2.83364151622281,
-                    "y": 5.653485783421201,
-                    "heading": -2.5990643886036375,
-                    "angularVelocity": -0.0000010100279552603614,
-                    "velocityX": 3.3837859454691737,
-                    "velocityY": -1.6913390691793195,
-                    "timestamp": 0.8375873353366279
-                },
-                {
-                    "x": 2.9910977149954676,
-                    "y": 5.574782809080547,
-                    "heading": -2.5990644356029167,
-                    "angularVelocity": -0.000001010028434200447,
-                    "velocityX": 3.3837803633561943,
-                    "velocityY": -1.691350237002125,
-                    "timestamp": 0.8841199650775516
-                },
-                {
-                    "x": 3.1485530895378893,
-                    "y": 5.496078185774728,
-                    "heading": -2.599064482602267,
-                    "angularVelocity": -0.0000010100299667197812,
-                    "velocityX": 3.3837626504042433,
-                    "velocityY": -1.6913856737520705,
-                    "timestamp": 0.9306525948184754
-                },
-                {
-                    "x": 3.3060077005580313,
-                    "y": 5.417372034978031,
-                    "heading": -2.5990645296016837,
-                    "angularVelocity": -0.0000010100313828443218,
-                    "velocityX": 3.383746242085582,
-                    "velocityY": -1.6914184999808708,
-                    "timestamp": 0.9771852245593992
-                },
-                {
-                    "x": 3.463466207634218,
-                    "y": 5.3386736786731355,
-                    "heading": -2.5990645766007607,
-                    "angularVelocity": -0.0000010100240886605546,
-                    "velocityX": 3.383829969488029,
-                    "velocityY": -1.6912509940353067,
-                    "timestamp": 1.023717854300323
-                },
-                {
-                    "x": 3.620938146457774,
-                    "y": 5.260002202075332,
-                    "heading": -2.5990646235986756,
-                    "angularVelocity": -0.0000010099991153726974,
-                    "velocityX": 3.3841186217193853,
-                    "velocityY": -1.690673341164269,
-                    "timestamp": 1.0702504840412468
-                },
-                {
-                    "x": 3.7783857783864416,
-                    "y": 5.181282090499446,
-                    "heading": -2.5990646705986817,
-                    "angularVelocity": -0.0000010100440573860445,
-                    "velocityX": 3.383596259333376,
-                    "velocityY": -1.691718521279299,
-                    "timestamp": 1.1167831137821707
-                },
-                {
-                    "x": 3.9357445242921885,
-                    "y": 5.102384448798495,
-                    "heading": -2.599064717606569,
-                    "angularVelocity": -0.0000010102134222869035,
-                    "velocityX": 3.381686072364613,
-                    "velocityY": -1.6955336962675904,
-                    "timestamp": 1.1633157435230945
+                    "x": 0.7256414720586181,
+                    "y": 6.712155337793633,
+                    "heading": 1.0237015987984905,
+                    "angularVelocity": -0.3502584296953155,
+                    "velocityX": 0.33614078912617174,
+                    "velocityY": -0.16858457660462928,
+                    "timestamp": 0.046532502346056014
+                },
+                {
+                    "x": 0.756979104040484,
+                    "y": 6.696435827024302,
+                    "heading": 0.9920873730999193,
+                    "angularVelocity": -0.6794009370802031,
+                    "velocityX": 0.6734568398839507,
+                    "velocityY": -0.33781786873231273,
+                    "timestamp": 0.09306500469211203
+                },
+                {
+                    "x": 0.8040789517219918,
+                    "y": 6.672807532790365,
+                    "heading": 0.9464055818688567,
+                    "angularVelocity": -0.9817179160357762,
+                    "velocityX": 1.0121924527341073,
+                    "velocityY": -0.5077804339471543,
+                    "timestamp": 0.13959750703816803
+                },
+                {
+                    "x": 0.8670206146686451,
+                    "y": 6.641234227942618,
+                    "heading": 0.8882606987188267,
+                    "angularVelocity": -1.2495541872573583,
+                    "velocityX": 1.352638688514273,
+                    "velocityY": -0.6785215334633973,
+                    "timestamp": 0.18613000938422405
+                },
+                {
+                    "x": 0.9458974559747272,
+                    "y": 6.601677384330224,
+                    "heading": 0.8197602775960398,
+                    "angularVelocity": -1.4720983757420398,
+                    "velocityX": 1.6950913303456163,
+                    "velocityY": -0.8500906166232347,
+                    "timestamp": 0.23266251173028007
+                },
+                {
+                    "x": 1.04081251945038,
+                    "y": 6.554094078416389,
+                    "heading": 0.743830819371293,
+                    "angularVelocity": -1.6317510212566968,
+                    "velocityX": 2.039758420249741,
+                    "velocityY": -1.0225821418316814,
+                    "timestamp": 0.27919501407633607
+                },
+                {
+                    "x": 1.15185810993152,
+                    "y": 6.498440545281296,
+                    "heading": 0.6650543509456113,
+                    "angularVelocity": -1.6929342815026889,
+                    "velocityX": 2.3864091738569346,
+                    "velocityY": -1.1960141907087913,
+                    "timestamp": 0.32572751642239206
+                },
+                {
+                    "x": 1.2789797685079123,
+                    "y": 6.43473905676892,
+                    "heading": 0.5921954143923897,
+                    "angularVelocity": -1.5657644201343583,
+                    "velocityX": 2.7318895861437724,
+                    "velocityY": -1.3689676097497174,
+                    "timestamp": 0.37226001876844805
+                },
+                {
+                    "x": 1.420223914518488,
+                    "y": 6.363052522149474,
+                    "heading": 0.5479689699773799,
+                    "angularVelocity": -0.9504419961367557,
+                    "velocityX": 3.035386856270174,
+                    "velocityY": -1.5405690862335149,
+                    "timestamp": 0.41879252111450405
+                },
+                {
+                    "x": 1.5740028534513755,
+                    "y": 6.283114192872816,
+                    "heading": 0.5411402298477218,
+                    "angularVelocity": -0.14675205040326236,
+                    "velocityX": 3.304764007515849,
+                    "velocityY": -1.717903083787275,
+                    "timestamp": 0.46532502346056004
+                },
+                {
+                    "x": 1.7314538549626566,
+                    "y": 6.20440189931456,
+                    "heading": 0.5411401828527235,
+                    "angularVelocity": -0.0000010099392058430412,
+                    "velocityX": 3.383677936345371,
+                    "velocityY": -1.6915551408103213,
+                    "timestamp": 0.511857525806616
+                },
+                {
+                    "x": 1.888910012901083,
+                    "y": 6.125699921176338,
+                    "heading": 0.5411401358667378,
+                    "angularVelocity": -0.0000010097455172737006,
+                    "velocityX": 3.383788749796141,
+                    "velocityY": -1.6913334587700644,
+                    "timestamp": 0.558390028152672
+                },
+                {
+                    "x": 2.0463658388325006,
+                    "y": 6.046997278808062,
+                    "heading": 0.5411400888807237,
+                    "angularVelocity": -0.0000010097461271838877,
+                    "velocityX": 3.3837816148475093,
+                    "velocityY": -1.691347733310028,
+                    "timestamp": 0.604922530498728
+                },
+                {
+                    "x": 2.2038216598445532,
+                    "y": 5.9682946265981345,
+                    "heading": 0.5411400418947097,
+                    "angularVelocity": -0.0000010097461301639238,
+                    "velocityX": 3.3837815091285663,
+                    "velocityY": -1.6913479448107236,
+                    "timestamp": 0.651455032844784
+                },
+                {
+                    "x": 2.3612775122085363,
+                    "y": 5.8895920371121315,
+                    "heading": 0.5411399949086985,
+                    "angularVelocity": -0.0000010097460671631127,
+                    "velocityX": 3.3837821828926944,
+                    "velocityY": -1.6913465968512624,
+                    "timestamp": 0.69798753519084
+                },
+                {
+                    "x": 2.5187333978232895,
+                    "y": 5.810889514148883,
+                    "heading": 0.5411399479226903,
+                    "angularVelocity": -0.0000010097460006294838,
+                    "velocityX": 3.383782897463604,
+                    "velocityY": -1.6913451672535464,
+                    "timestamp": 0.744520037536896
+                },
+                {
+                    "x": 2.676189337500118,
+                    "y": 5.732187099344657,
+                    "heading": 0.541139900936687,
+                    "angularVelocity": -0.0000010097458968738495,
+                    "velocityX": 3.3837840592768114,
+                    "velocityY": -1.6913428428777157,
+                    "timestamp": 0.791052539882952
+                },
+                {
+                    "x": 2.833645305906935,
+                    "y": 5.653484742019454,
+                    "heading": 0.5411398539506863,
+                    "angularVelocity": -0.0000010097458381135432,
+                    "velocityX": 3.3837846766944186,
+                    "velocityY": -1.6913416076332204,
+                    "timestamp": 0.837585042229008
+                },
+                {
+                    "x": 2.991101010217502,
+                    "y": 5.57478185633263,
+                    "heading": 0.5411398069646632,
+                    "angularVelocity": -0.0000010097463224464638,
+                    "velocityX": 3.383779001172013,
+                    "velocityY": -1.6913529623124643,
+                    "timestamp": 0.884117544575064
+                },
+                {
+                    "x": 3.14855589978306,
+                    "y": 5.496077340659936,
+                    "heading": 0.5411397599785701,
+                    "angularVelocity": -0.0000010097478275290572,
+                    "velocityX": 3.3837614920124643,
+                    "velocityY": -1.6913879912875969,
+                    "timestamp": 0.93065004692112
+                },
+                {
+                    "x": 3.306010013541662,
+                    "y": 5.417371272923184,
+                    "heading": 0.5411397129924104,
+                    "angularVelocity": -0.0000010097492581811278,
+                    "velocityX": 3.3837448196468105,
+                    "velocityY": -1.6914213456927205,
+                    "timestamp": 0.977182549267176
+                },
+                {
+                    "x": 3.4634680562159517,
+                    "y": 5.338673065406879,
+                    "heading": 0.5411396660065909,
+                    "angularVelocity": -0.0000010097419454671228,
+                    "velocityX": 3.383829253443873,
+                    "velocityY": -1.6912524267565827,
+                    "timestamp": 1.023715051613232
+                },
+                {
+                    "x": 3.6209395624979943,
+                    "y": 5.2600018013447745,
+                    "heading": 0.5411396190219294,
+                    "angularVelocity": -0.0000010097170609305348,
+                    "velocityX": 3.3841185911564384,
+                    "velocityY": -1.690673402356174,
+                    "timestamp": 1.0702475539592882
+                },
+                {
+                    "x": 3.778386684395729,
+                    "y": 5.181281747313384,
+                    "heading": 0.5411395720351826,
+                    "angularVelocity": -0.0000010097618775946008,
+                    "velocityX": 3.3835945620651766,
+                    "velocityY": -1.6917219161486032,
+                    "timestamp": 1.1167800563053443
+                },
+                {
+                    "x": 3.9357449151310644,
+                    "y": 5.10238415335025,
+                    "heading": 0.5411395250405994,
+                    "angularVelocity": -0.000001009930280464059,
+                    "velocityX": 3.3816842594175633,
+                    "velocityY": -1.6955373123309678,
+                    "timestamp": 1.1633125586514004
                 },
                 {
                     "x": 4.092995643615723,
                     "y": 5.023272514343262,
-                    "heading": -2.5990647646424785,
-                    "angularVelocity": -0.000001010815630420508,
-                    "velocityX": 3.379373145230901,
-                    "velocityY": -1.700138911036495,
-                    "timestamp": 1.2098483732640184
-                },
-                {
-                    "x": 4.261229917640389,
-                    "y": 4.916068911394467,
-                    "heading": -2.599064809945939,
-                    "angularVelocity": -8.591015508014648e-7,
-                    "velocityX": 3.1902711911232386,
-                    "velocityY": -2.0329303767326627,
-                    "timestamp": 1.2625819071694577
-                },
-                {
-                    "x": 4.429297438166161,
-                    "y": 4.8086040706119935,
-                    "heading": -2.5990648552466906,
-                    "angularVelocity": -8.590501714319945e-7,
-                    "velocityX": 3.187109000265223,
-                    "velocityY": -2.0378842991085575,
-                    "timestamp": 1.315315441074897
-                },
-                {
-                    "x": 4.597219271125054,
-                    "y": 4.700911724294517,
-                    "heading": -2.599064900558894,
-                    "angularVelocity": -8.592673362873697e-7,
-                    "velocityX": 3.184346288265075,
-                    "velocityY": -2.0421985469549115,
-                    "timestamp": 1.3680489749803364
-                },
-                {
-                    "x": 4.765178923625143,
-                    "y": 4.59327837178587,
-                    "heading": -2.5990649458681174,
-                    "angularVelocity": -8.592108353148986e-7,
-                    "velocityX": 3.1850634702628065,
-                    "velocityY": -2.0410798317000203,
-                    "timestamp": 1.4207825088857757
-                },
-                {
-                    "x": 4.93313403793343,
-                    "y": 4.485637937706628,
-                    "heading": -2.599064991177699,
-                    "angularVelocity": -8.592176153657431e-7,
-                    "velocityX": 3.184977411326241,
-                    "velocityY": -2.041214121404157,
-                    "timestamp": 1.473516042791215
-                },
-                {
-                    "x": 5.101087827105617,
-                    "y": 4.377995436130629,
-                    "heading": -2.599065036487385,
-                    "angularVelocity": -8.592196078701955e-7,
-                    "velocityX": 3.184952282419374,
-                    "velocityY": -2.041253327892839,
-                    "timestamp": 1.5262495766966544
-                },
-                {
-                    "x": 5.269016802174646,
-                    "y": 4.270314227125004,
-                    "heading": -2.599065081799025,
-                    "angularVelocity": -8.592566538542498e-7,
-                    "velocityX": 3.1844817259951634,
-                    "velocityY": -2.041987347154346,
-                    "timestamp": 1.5789831106020937
-                },
-                {
-                    "x": 5.437055950944385,
-                    "y": 4.162805027649924,
-                    "heading": -2.599065127101977,
-                    "angularVelocity": -8.590919076920599e-7,
-                    "velocityX": 3.1865709791255945,
-                    "velocityY": -2.038725484769403,
-                    "timestamp": 1.631716644507533
-                },
-                {
-                    "x": 5.605213465892237,
-                    "y": 4.055481061993006,
-                    "heading": -2.599065172406386,
-                    "angularVelocity": -8.591195260284143e-7,
-                    "velocityX": 3.188815588376687,
-                    "velocityY": -2.035212846712725,
-                    "timestamp": 1.6844501784129724
+                    "heading": 0.5411394780180799,
+                    "angularVelocity": -0.0000010105306414990813,
+                    "velocityX": 3.379373997880459,
+                    "velocityY": -1.7001372163227824,
+                    "timestamp": 1.2098450609974565
+                },
+                {
+                    "x": 4.2612300154414715,
+                    "y": 4.916069066513277,
+                    "heading": 0.541139432730096,
+                    "angularVelocity": -8.588080638060829e-7,
+                    "velocityX": 3.1902730598822937,
+                    "velocityY": -2.0329274441782497,
+                    "timestamp": 1.2625785946693153
+                },
+                {
+                    "x": 4.429297905294805,
+                    "y": 4.808604804974877,
+                    "heading": 0.5411393874448718,
+                    "angularVelocity": -8.58755730533247e-7,
+                    "velocityX": 3.1871160180389406,
+                    "velocityY": -2.0378733237761697,
+                    "timestamp": 1.3153121283411742
+                },
+                {
+                    "x": 4.5972191269209945,
+                    "y": 4.700911507061467,
+                    "heading": 0.5411393421481548,
+                    "angularVelocity": -8.589736709635531e-7,
+                    "velocityX": 3.1843347095055985,
+                    "velocityY": -2.042216601367096,
+                    "timestamp": 1.368045662013033
+                },
+                {
+                    "x": 4.765178956046694,
+                    "y": 4.593278431809082,
+                    "heading": 0.5411392968544698,
+                    "angularVelocity": -8.589161734970966e-7,
+                    "velocityX": 3.185066833769243,
+                    "velocityY": -2.0410745830567376,
+                    "timestamp": 1.4207791956848919
+                },
+                {
+                    "x": 4.9331339491528,
+                    "y": 4.485637810244685,
+                    "heading": 0.5411392515604048,
+                    "angularVelocity": -8.58923380695705e-7,
+                    "velocityX": 3.1849751270448214,
+                    "velocityY": -2.041217685776372,
+                    "timestamp": 1.4735127293567507
+                },
+                {
+                    "x": 5.1010877481253685,
+                    "y": 4.377995325600118,
+                    "heading": 0.5411392062662455,
+                    "angularVelocity": -8.589251678490757e-7,
+                    "velocityX": 3.1849524823742743,
+                    "velocityY": -2.041253015859115,
+                    "timestamp": 1.5262462630286096
+                },
+                {
+                    "x": 5.269016703536996,
+                    "y": 4.270314087571009,
+                    "heading": 0.5411391609701367,
+                    "angularVelocity": -8.589621362197366e-7,
+                    "velocityX": 3.184481367332135,
+                    "velocityY": -2.0419879065793034,
+                    "timestamp": 1.5789797967004684
+                },
+                {
+                    "x": 5.437056059940434,
+                    "y": 4.162805214274757,
+                    "heading": 0.5411391156827058,
+                    "angularVelocity": -8.587975760545075e-7,
+                    "velocityX": 3.18657493065327,
+                    "velocityY": -2.038719308384206,
+                    "timestamp": 1.6317133303723272
+                },
+                {
+                    "x": 5.605213513282879,
+                    "y": 4.0554811537322735,
+                    "heading": 0.5411390703937885,
+                    "angularVelocity": -8.588257618591655e-7,
+                    "velocityX": 3.188814434261697,
+                    "velocityY": -2.03521465506778,
+                    "timestamp": 1.684446864044186
                 },
                 {
                     "x": 5.7832255363464355,
                     "y": 3.9654417037963863,
-                    "heading": -2.599065217938776,
-                    "angularVelocity": -8.634427952488677e-7,
-                    "velocityX": 3.3756901400427615,
-                    "velocityY": -1.707440247757134,
-                    "timestamp": 1.7371837123184117
-                },
-                {
-                    "x": 5.945200300467497,
-                    "y": 3.903126168683373,
-                    "heading": -2.599065262330506,
-                    "angularVelocity": -9.676334890696444e-7,
-                    "velocityX": 3.5306622744884395,
-                    "velocityY": -1.3583295529523924,
-                    "timestamp": 1.7830603075655527
-                },
-                {
-                    "x": 6.112434090508488,
-                    "y": 3.856737486793876,
-                    "heading": -2.5990653054733555,
-                    "angularVelocity": -9.404108896808373e-7,
-                    "velocityX": 3.6452964554166534,
-                    "velocityY": -1.01116226345067,
-                    "timestamp": 1.8289369028126936
-                },
-                {
-                    "x": 6.280097327658818,
-                    "y": 3.8119257778274935,
-                    "heading": -2.599065348543951,
-                    "angularVelocity": -9.38835914358075e-7,
-                    "velocityX": 3.6546573747925137,
-                    "velocityY": -0.9767880272056341,
-                    "timestamp": 1.8748134980598345
-                },
-                {
-                    "x": 6.444605868501446,
-                    "y": 3.756644766145867,
-                    "heading": -2.599065392143275,
-                    "angularVelocity": -9.503609402316767e-7,
-                    "velocityX": 3.5858925440393525,
-                    "velocityY": -1.2049937747953314,
-                    "timestamp": 1.9206900933069755
-                },
-                {
-                    "x": 6.602719051918594,
-                    "y": 3.685095684621415,
-                    "heading": -2.5990654382603977,
-                    "angularVelocity": -0.0000010052429246959388,
-                    "velocityX": 3.446489055375297,
-                    "velocityY": -1.5595987701135843,
-                    "timestamp": 1.9665666885541164
-                },
-                {
-                    "x": 6.752801915063028,
-                    "y": 3.597951074081566,
-                    "heading": -2.599065489714602,
-                    "angularVelocity": -0.0000011215785414644313,
-                    "velocityX": 3.2714472888827597,
-                    "velocityY": -1.8995439846919482,
-                    "timestamp": 2.0124432838012574
-                },
-                {
-                    "x": 6.893316861258978,
-                    "y": 3.49609530674715,
-                    "heading": -2.599065550773928,
-                    "angularVelocity": -0.0000013309472018502562,
-                    "velocityX": 3.06288959411619,
-                    "velocityY": -2.2202120010370177,
-                    "timestamp": 2.058319879048398
-                },
-                {
-                    "x": 7.022827120260116,
-                    "y": 3.380569821579126,
-                    "heading": -2.5990656290967817,
-                    "angularVelocity": -0.0000017072507862805843,
-                    "velocityX": 2.8230137459733338,
-                    "velocityY": -2.5181791400535034,
-                    "timestamp": 2.104196474295539
+                    "heading": 0.5411390248741499,
+                    "angularVelocity": -8.632009947472993e-7,
+                    "velocityX": 3.3756892563138687,
+                    "velocityY": -1.7074419949962474,
+                    "timestamp": 1.737180397716045
+                },
+                {
+                    "x": 5.9452002659111205,
+                    "y": 3.9031260890359523,
+                    "heading": 0.54113898049095,
+                    "angularVelocity": -9.674475759968245e-7,
+                    "velocityX": 3.530661595583403,
+                    "velocityY": -1.358331317676469,
+                    "timestamp": 1.7830569919972126
+                },
+                {
+                    "x": 6.112434032055133,
+                    "y": 3.8567373346649623,
+                    "heading": 0.5411389373525121,
+                    "angularVelocity": -9.403147416321739e-7,
+                    "velocityX": 3.6452960112747332,
+                    "velocityY": -1.0111638646647567,
+                    "timestamp": 1.8289335862783802
+                },
+                {
+                    "x": 6.280097272848707,
+                    "y": 3.8119256534794217,
+                    "heading": 0.5411388942859083,
+                    "angularVelocity": -9.387489265836176e-7,
+                    "velocityX": 3.65465753115864,
+                    "velocityY": -0.9767874422169035,
+                    "timestamp": 1.8748101805595478
+                },
+                {
+                    "x": 6.4446058180798245,
+                    "y": 3.756644666326502,
+                    "heading": 0.5411388506931323,
+                    "angularVelocity": -9.502182266145116e-7,
+                    "velocityX": 3.5858927152021427,
+                    "velocityY": -1.204993265500441,
+                    "timestamp": 1.9206867748407155
+                },
+                {
+                    "x": 6.602719007178768,
+                    "y": 3.68509560621824,
+                    "heading": 0.5411388045872843,
+                    "angularVelocity": -0.0000010049971826881646,
+                    "velocityX": 3.4464892517938135,
+                    "velocityY": -1.5595983361306547,
+                    "timestamp": 1.9665633691218831
+                },
+                {
+                    "x": 6.75280187814694,
+                    "y": 3.5979510164257396,
+                    "heading": 0.5411387531507533,
+                    "angularVelocity": -0.000001121193317368561,
+                    "velocityX": 3.271447528304838,
+                    "velocityY": -1.899543572446005,
+                    "timestamp": 2.0124399634030508
+                },
+                {
+                    "x": 6.893316834439751,
+                    "y": 3.4960952692408354,
+                    "heading": 0.5411386921189019,
+                    "angularVelocity": -0.0000013303483490867941,
+                    "velocityX": 3.0628898786954792,
+                    "velocityY": -2.2202116085744548,
+                    "timestamp": 2.0583165576842184
+                },
+                {
+                    "x": 7.022827105769335,
+                    "y": 3.380569803374169,
+                    "heading": 0.5411386138405815,
+                    "angularVelocity": -0.0000017062801116380342,
+                    "velocityX": 2.8230140741451093,
+                    "velocityY": -2.5181787723526305,
+                    "timestamp": 2.104193151965386
                 },
                 {
                     "x": 7.140008449554443,
                     "y": 3.2525558471679688,
-                    "heading": -2.5990657325552275,
-                    "angularVelocity": -0.000002255146554794306,
-                    "velocityX": 2.5542725797993215,
-                    "velocityY": -2.790398322315311,
-                    "timestamp": 2.1500730695426795
-                },
-                {
-                    "x": 7.220411639481594,
-                    "y": 3.1500923106303724,
-                    "heading": -2.599065847631647,
-                    "angularVelocity": -0.000003342401256012092,
-                    "velocityX": 2.3353152959116907,
-                    "velocityY": -2.9760593375246422,
-                    "timestamp": 2.1845023348619734
-                },
-                {
-                    "x": 7.292813286750944,
-                    "y": 3.0418266490558983,
-                    "heading": -2.5990659368308053,
-                    "angularVelocity": -0.0000025907947068596104,
-                    "velocityX": 2.1029100272080186,
-                    "velocityY": -3.1445823943795888,
-                    "timestamp": 2.2189316001812673
-                },
-                {
-                    "x": 7.356796447286593,
-                    "y": 2.9283823501608603,
-                    "heading": -2.599066009494182,
-                    "angularVelocity": -0.0000021105119622632264,
-                    "velocityX": 1.8583945937350983,
-                    "velocityY": -3.294996214498541,
-                    "timestamp": 2.2533608655005612
-                },
-                {
-                    "x": 7.411992663456803,
-                    "y": 2.8104127239306367,
-                    "heading": -2.5990660711538616,
-                    "angularVelocity": -0.0000017909089651975303,
-                    "velocityX": 1.603177287064451,
-                    "velocityY": -3.426434608353826,
-                    "timestamp": 2.287790130819855
-                },
-                {
-                    "x": 7.458084080553435,
-                    "y": 2.688597137512427,
-                    "heading": -2.599066125344508,
-                    "angularVelocity": -0.0000015739704607850277,
-                    "velocityX": 1.3387278720350797,
-                    "velocityY": -3.538140744175163,
-                    "timestamp": 2.322219396139149
-                },
-                {
-                    "x": 7.494809849686971,
-                    "y": 2.5636384472618623,
-                    "heading": -2.599066174481706,
-                    "angularVelocity": -0.0000014271927566927072,
-                    "velocityX": 1.0667020859416116,
-                    "velocityY": -3.6294323765468546,
-                    "timestamp": 2.356648661458443
-                },
-                {
-                    "x": 7.5219737035667364,
-                    "y": 2.4362587715435033,
-                    "heading": -2.599066220329768,
-                    "angularVelocity": -0.0000013316596016825942,
-                    "velocityX": 0.7889757050532209,
-                    "velocityY": -3.699750039306788,
-                    "timestamp": 2.391077926777737
-                },
-                {
-                    "x": 7.539529775464085,
-                    "y": 2.307203584640024,
-                    "heading": -2.5990662642857343,
-                    "angularVelocity": -0.0000012767035893272692,
-                    "velocityX": 0.5099171223818962,
-                    "velocityY": -3.748415358464242,
-                    "timestamp": 2.425507192097031
-                },
-                {
-                    "x": 7.55111544342237,
-                    "y": 2.1774760653156817,
-                    "heading": -2.599066307607684,
-                    "angularVelocity": -0.0000012582885205698109,
-                    "velocityX": 0.3365063950923211,
-                    "velocityY": -3.7679432924653384,
-                    "timestamp": 2.4599364574163247
-                },
-                {
-                    "x": 7.56271571323303,
-                    "y": 2.047749850875278,
-                    "heading": -2.5990663509314866,
-                    "angularVelocity": -0.0000012583423688964546,
-                    "velocityX": 0.3369305067394331,
-                    "velocityY": -3.767905392035856,
-                    "timestamp": 2.4943657227356186
-                },
-                {
-                    "x": 7.5743766744470316,
-                    "y": 1.9180290778400821,
-                    "heading": -2.599066394258993,
-                    "angularVelocity": -0.0000012584499309396356,
-                    "velocityX": 0.33869329205287857,
-                    "velocityY": -3.7677473461074933,
-                    "timestamp": 2.5287949880549125
-                },
-                {
-                    "x": 7.586111158859003,
-                    "y": 1.7883149350094998,
-                    "heading": -2.5990664375910546,
-                    "angularVelocity": -0.0000012585822270319525,
-                    "velocityX": 0.34082877758442714,
-                    "velocityY": -3.7675547714315485,
-                    "timestamp": 2.5632242533742065
-                },
-                {
-                    "x": 7.597929633339714,
-                    "y": 1.6586084182080805,
-                    "heading": -2.59906648228782,
-                    "angularVelocity": -0.0000012982201369267655,
-                    "velocityX": 0.3432682739856076,
-                    "velocityY": -3.7673332729737243,
-                    "timestamp": 2.5976535186935004
-                },
-                {
-                    "x": 7.610690036465753,
-                    "y": 1.5360359058677395,
-                    "heading": -2.6173573406008535,
-                    "angularVelocity": -0.5312590362708309,
-                    "velocityX": 0.37062664589906397,
-                    "velocityY": -3.5601257013070047,
-                    "timestamp": 2.6320827840127943
-                },
-                {
-                    "x": 7.621968158889347,
-                    "y": 1.4215047209508034,
-                    "heading": -2.657213150288673,
-                    "angularVelocity": -1.1576142946476244,
-                    "velocityX": 0.3275737172727147,
-                    "velocityY": -3.3265648817881672,
-                    "timestamp": 2.666512049332088
-                },
-                {
-                    "x": 7.631829723236215,
-                    "y": 1.3151734834169941,
-                    "heading": -2.7178665355180023,
-                    "angularVelocity": -1.76168107762032,
-                    "velocityX": 0.28642970610680407,
-                    "velocityY": -3.08839693637669,
-                    "timestamp": 2.700941314651382
-                },
-                {
-                    "x": 7.640728077188491,
-                    "y": 1.2186118736781408,
-                    "heading": -2.7826314980185702,
-                    "angularVelocity": -1.8811020769678346,
-                    "velocityX": 0.2584532045561674,
-                    "velocityY": -2.8046375327311788,
-                    "timestamp": 2.735370579970676
-                },
-                {
-                    "x": 7.648685947205252,
-                    "y": 1.1318864826516366,
-                    "heading": -2.8465688471146957,
-                    "angularVelocity": -1.8570639978286094,
-                    "velocityX": 0.23113679432217057,
-                    "velocityY": -2.518943992043375,
-                    "timestamp": 2.76979984528997
-                },
-                {
-                    "x": 7.655720752960135,
-                    "y": 1.0549563243440254,
-                    "heading": -2.9071249866023474,
-                    "angularVelocity": -1.7588565694352556,
-                    "velocityX": 0.20432633951521018,
-                    "velocityY": -2.2344408919030423,
-                    "timestamp": 2.804229110609264
-                },
-                {
-                    "x": 7.661844928847109,
-                    "y": 0.9877692823605362,
-                    "heading": -2.962675956286548,
-                    "angularVelocity": -1.6134811233710036,
-                    "velocityX": 0.17787704239939275,
-                    "velocityY": -1.9514515154593546,
-                    "timestamp": 2.8386583759285577
-                },
-                {
-                    "x": 7.667068746842824,
-                    "y": 0.930277282093812,
-                    "heading": -3.0120810880108007,
-                    "angularVelocity": -1.4349749048108316,
-                    "velocityX": 0.1517260954387245,
-                    "velocityY": -1.6698584687633535,
-                    "timestamp": 2.8730876412478517
-                },
-                {
-                    "x": 7.6714018244195765,
-                    "y": 0.8824389383291659,
-                    "heading": -3.0544961336196454,
-                    "angularVelocity": -1.231947449807498,
-                    "velocityX": 0.12585448851644476,
-                    "velocityY": -1.3894674580185522,
-                    "timestamp": 2.9075169065671456
-                },
-                {
-                    "x": 7.674853405168496,
-                    "y": 0.8442193530437802,
-                    "heading": -3.0892755187362573,
-                    "angularVelocity": -1.0101692497377945,
-                    "velocityX": 0.10025136223247667,
-                    "velocityY": -1.1100900623623653,
-                    "timestamp": 2.9419461718864395
-                },
-                {
-                    "x": 7.677432109437281,
-                    "y": 0.8155891364108041,
-                    "heading": -3.1159120874216906,
-                    "angularVelocity": -0.773660676125647,
-                    "velocityX": 0.07489861444537417,
-                    "velocityY": -0.8315662959247566,
-                    "timestamp": 2.9763754372057334
-                },
-                {
-                    "x": 7.679145555938743,
-                    "y": 0.7965232635297996,
-                    "heading": -3.1339959816499734,
-                    "angularVelocity": -0.525247752473809,
-                    "velocityX": 0.04976715261198055,
-                    "velocityY": -0.5537693791659258,
-                    "timestamp": 3.0108047025250273
+                    "heading": 0.541138510449912,
+                    "angularVelocity": -0.0000022536692406151377,
+                    "velocityX": 2.5542729494461978,
+                    "velocityY": -2.790397984245173,
+                    "timestamp": 2.1500697462465537
+                },
+                {
+                    "x": 7.220411568965462,
+                    "y": 3.1500924469852043,
+                    "heading": 0.5411383954402629,
+                    "angularVelocity": -0.0000033404657713126234,
+                    "velocityX": 2.3353159519018676,
+                    "velocityY": -2.97605882316215,
+                    "timestamp": 2.1844989716991257
+                },
+                {
+                    "x": 7.292813165778826,
+                    "y": 3.0418269330928753,
+                    "heading": 0.5411383062644725,
+                    "angularVelocity": -0.000002590118983726798,
+                    "velocityX": 2.102910996737314,
+                    "velocityY": -3.1445817461525896,
+                    "timestamp": 2.2189281971516976
+                },
+                {
+                    "x": 7.3567962971772936,
+                    "y": 2.928382790909777,
+                    "heading": 0.5411382336043034,
+                    "angularVelocity": -0.000002110421251450144,
+                    "velocityX": 1.8583958993386591,
+                    "velocityY": -3.2949954781694024,
+                    "timestamp": 2.2533574226042696
+                },
+                {
+                    "x": 7.411992506551136,
+                    "y": 2.810413328004449,
+                    "heading": 0.5411381719390687,
+                    "angularVelocity": -0.00000179107237982498,
+                    "velocityX": 1.603178946034542,
+                    "velocityY": -3.426433832147572,
+                    "timestamp": 2.2877866480568416
+                },
+                {
+                    "x": 7.458083940034777,
+                    "y": 2.688597909034967,
+                    "heading": 0.5411381177395469,
+                    "angularVelocity": -0.0000015742300651441758,
+                    "velocityX": 1.3387298981538813,
+                    "velocityY": -3.5381399775400135,
+                    "timestamp": 2.3222158735094136
+                },
+                {
+                    "x": 7.4948097486817264,
+                    "y": 2.563639387590104,
+                    "heading": 0.5411380685927316,
+                    "angularVelocity": -0.0000014274737405214791,
+                    "velocityX": 1.0667044687814087,
+                    "velocityY": -3.6294316762078314,
+                    "timestamp": 2.3566450989619856
+                },
+                {
+                    "x": 7.521973664613143,
+                    "y": 2.436259879309271,
+                    "heading": 0.5411380227355477,
+                    "angularVelocity": -0.0000013319260973088958,
+                    "velocityX": 0.7889784209297904,
+                    "velocityY": -3.6997494601296452,
+                    "timestamp": 2.3910743244145576
+                },
+                {
+                    "x": 7.539529818670366,
+                    "y": 2.3072048557852676,
+                    "heading": 0.5411379787715688,
+                    "angularVelocity": -0.0000012769377868531526,
+                    "velocityX": 0.5099200991725963,
+                    "velocityY": -3.748414953504679,
+                    "timestamp": 2.4255035498671296
+                },
+                {
+                    "x": 7.551115587366525,
+                    "y": 2.177477496871536,
+                    "heading": 0.5411379354425048,
+                    "angularVelocity": -0.0000012584966258013745,
+                    "velocityX": 0.336509710684848,
+                    "velocityY": -3.7679429963488027,
+                    "timestamp": 2.4599327753197016
+                },
+                {
+                    "x": 7.56271584344406,
+                    "y": 2.0477514326184285,
+                    "heading": 0.5411378921115911,
+                    "angularVelocity": -0.0000012585503448264627,
+                    "velocityX": 0.3369304980023622,
+                    "velocityY": -3.76790539281253,
+                    "timestamp": 2.4943620007722735
+                },
+                {
+                    "x": 7.574376716473137,
+                    "y": 1.9180308030778739,
+                    "heading": 0.5411378487769694,
+                    "angularVelocity": -0.0000012586580504181092,
+                    "velocityX": 0.3386911228976962,
+                    "velocityY": -3.767747541089928,
+                    "timestamp": 2.5287912262248455
+                },
+                {
+                    "x": 7.58611108718596,
+                    "y": 1.788316801391074,
+                    "heading": 0.541137805437783,
+                    "angularVelocity": -0.0000012587906282413006,
+                    "velocityX": 0.34082586983899804,
+                    "velocityY": -3.7675550344719086,
+                    "timestamp": 2.5632204516774175
+                },
+                {
+                    "x": 7.597929211011114,
+                    "y": 1.6586104040760536,
+                    "heading": 0.541137760738121,
+                    "angularVelocity": -0.0000012983057658513822,
+                    "velocityX": 0.3432584866444477,
+                    "velocityY": -3.7673341647982834,
+                    "timestamp": 2.5976496771299895
+                },
+                {
+                    "x": 7.610687440254598,
+                    "y": 1.5360368797463901,
+                    "heading": 0.5228402741207003,
+                    "angularVelocity": -0.5314521711397164,
+                    "velocityX": 0.37056393444221536,
+                    "velocityY": -3.560159217014913,
+                    "timestamp": 2.6320789025825615
+                },
+                {
+                    "x": 7.6219652115761445,
+                    "y": 1.4215043504075173,
+                    "heading": 0.4829676933785713,
+                    "angularVelocity": -1.1581027518921772,
+                    "velocityX": 0.3275638987899306,
+                    "velocityY": -3.3266077825840545,
+                    "timestamp": 2.6665081280351335
+                },
+                {
+                    "x": 7.631828440236478,
+                    "y": 1.3151733520485158,
+                    "heading": 0.42230189460591644,
+                    "angularVelocity": -1.7620436700286786,
+                    "velocityX": 0.2864783779098223,
+                    "velocityY": -3.0883935656779764,
+                    "timestamp": 2.7009373534877055
+                },
+                {
+                    "x": 7.640727843462309,
+                    "y": 1.2186121100176637,
+                    "heading": 0.3575309788065375,
+                    "angularVelocity": -1.8812771692652814,
+                    "velocityX": 0.2584839800738238,
+                    "velocityY": -2.804630100199854,
+                    "timestamp": 2.7353665789402775
+                },
+                {
+                    "x": 7.648686345492233,
+                    "y": 1.1318869979940107,
+                    "heading": 0.2935915741251458,
+                    "angularVelocity": -1.8571258528454382,
+                    "velocityX": 0.23115541884308086,
+                    "velocityY": -2.518938805147378,
+                    "timestamp": 2.7697958043928494
+                },
+                {
+                    "x": 7.655721476158885,
+                    "y": 1.054956995812666,
+                    "heading": 0.2330360616068616,
+                    "angularVelocity": -1.7588403956899903,
+                    "velocityX": 0.20433601320340974,
+                    "velocityY": -2.234438944533275,
+                    "timestamp": 2.8042250298454214
+                },
+                {
+                    "x": 7.661845751852373,
+                    "y": 0.9877699914439897,
+                    "heading": 0.17748768247912494,
+                    "angularVelocity": -1.6134077487241032,
+                    "velocityX": 0.17788014725831017,
+                    "velocityY": -1.9514526825829648,
+                    "timestamp": 2.8386542552979934
+                },
+                {
+                    "x": 7.667069516180101,
+                    "y": 0.9302779311076093,
+                    "heading": 0.1280864585154153,
+                    "angularVelocity": -1.4348630651528973,
+                    "velocityX": 0.15172471233573487,
+                    "velocityY": -1.6698621470755346,
+                    "timestamp": 2.8730834807505654
+                },
+                {
+                    "x": 7.6714024480246685,
+                    "y": 0.88243945985842,
+                    "heading": 0.08567598072175592,
+                    "angularVelocity": -1.2318162037100373,
+                    "velocityX": 0.1258504014428196,
+                    "velocityY": -1.3894727697283988,
+                    "timestamp": 2.9075127062031374
+                },
+                {
+                    "x": 7.674853841393155,
+                    "y": 0.8442197139951643,
+                    "heading": 0.050901182135293016,
+                    "angularVelocity": -1.0100372032583045,
+                    "velocityX": 0.10024603583499626,
+                    "velocityY": -1.110096011770723,
+                    "timestamp": 2.9419419316557094
+                },
+                {
+                    "x": 7.6774323573426795,
+                    "y": 0.8155893376478138,
+                    "heading": 0.02426863930506587,
+                    "angularVelocity": -0.7735446406401408,
+                    "velocityX": 0.07489323142279661,
+                    "velocityY": -0.8315718977410657,
+                    "timestamp": 2.9763711571082814
+                },
+                {
+                    "x": 7.6791456483845035,
+                    "y": 0.7965233362757584,
+                    "heading": 0.006187731070844568,
+                    "angularVelocity": -0.5251616322048499,
+                    "velocityX": 0.04976269489958636,
+                    "velocityY": -0.5537737524278505,
+                    "timestamp": 3.0108003825608534
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -0.2669044908275344,
-                    "velocityX": 0.024817377116055982,
-                    "velocityY": -0.2766037393328496,
-                    "timestamp": 3.045233967844321
+                    "heading": -0.003,
+                    "angularVelocity": -0.26685848868430057,
+                    "velocityX": 0.024814720757314154,
+                    "velocityY": -0.2766061725343536,
+                    "timestamp": 3.0452296080134253
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -4.433819854347959e-31,
+                    "heading": -0.003,
+                    "angularVelocity": -2.388488097950269e-30,
                     "velocityX": 0,
-                    "velocityY": 2.2179802932100695e-31,
-                    "timestamp": 3.079663233163615
+                    "velocityY": 9.563090319474696e-31,
+                    "timestamp": 3.0796588334659973
                 }
             ],
             "constraints": [
@@ -2864,7 +3060,7 @@
                 {
                     "x": 1.35,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -2873,7 +3069,7 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": 3.5331853071795862,
+                    "heading": 0.393,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -2884,110 +3080,110 @@
                 {
                     "x": 1.35,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": -7.821039013033948e-33,
-                    "velocityX": 0,
-                    "velocityY": 0,
+                    "heading": 0,
+                    "angularVelocity": -1.3982338019676807e-32,
+                    "velocityX": 1.9225496754980614e-32,
+                    "velocityY": -8.567103847838605e-38,
                     "timestamp": 0
                 },
                 {
-                    "x": 1.3816658469394465,
-                    "y": 5.599900983994057,
-                    "heading": 3.1530893621447014,
-                    "angularVelocity": 0.16724918062405764,
-                    "velocityX": 0.4046100104681557,
-                    "velocityY": 0.5098343834730725,
-                    "timestamp": 0.07826263839297289
+                    "x": 1.3816658535274804,
+                    "y": 5.599900984360149,
+                    "heading": 0.013082876743969601,
+                    "angularVelocity": 0.16716651866961724,
+                    "velocityX": 0.4046105912700521,
+                    "velocityY": 0.509835013927756,
+                    "timestamp": 0.07826254233257365
                 },
                 {
-                    "x": 1.444997693090777,
-                    "y": 5.6797028720542775,
-                    "heading": 3.179265627435073,
-                    "angularVelocity": 0.3344669414150786,
-                    "velocityX": 0.8092219665957109,
-                    "velocityY": 1.0196677456683545,
-                    "timestamp": 0.15652527678594577
+                    "x": 1.4449977124682947,
+                    "y": 5.679702872674784,
+                    "heading": 0.039246215534929935,
+                    "angularVelocity": 0.3343021835373132,
+                    "velocityX": 0.8092231232623148,
+                    "velocityY": 1.0196690004717623,
+                    "timestamp": 0.1565250846651473
                 },
                 {
-                    "x": 1.539995766815641,
-                    "y": 5.799405527661274,
-                    "heading": 3.2185261574836668,
-                    "angularVelocity": 0.5016509902395458,
-                    "velocityX": 1.213836840611724,
-                    "velocityY": 1.5294993634886844,
-                    "timestamp": 0.23478791517891867
+                    "x": 1.5399958043003432,
+                    "y": 5.799405527723277,
+                    "heading": 0.07848745324353076,
+                    "angularVelocity": 0.501405098007769,
+                    "velocityX": 1.2138385618545031,
+                    "velocityY": 1.5295012336785618,
+                    "timestamp": 0.23478762699772096
                 },
                 {
-                    "x": 1.6666603529158708,
-                    "y": 5.959008729778703,
-                    "heading": 3.270870260258294,
-                    "angularVelocity": 0.6688261966303347,
-                    "velocityX": 1.6184553536800128,
-                    "velocityY": 2.039328157019548,
-                    "timestamp": 0.31305055357189154
+                    "x": 1.6666604122853452,
+                    "y": 5.959008727776544,
+                    "heading": 0.1308059995871385,
+                    "angularVelocity": 0.6685004701391144,
+                    "velocityX": 1.6184576198246376,
+                    "velocityY": 2.039330633740969,
+                    "timestamp": 0.3130501693302946
                 },
                 {
-                    "x": 1.8249917488860277,
-                    "y": 6.158512141354908,
-                    "heading": 3.336302010504542,
-                    "angularVelocity": 0.8360534680379705,
-                    "velocityX": 2.0230776679817777,
-                    "velocityY": 2.5491526438766083,
-                    "timestamp": 0.3913131919648644
+                    "x": 1.8249918316073899,
+                    "y": 6.158512135429991,
+                    "heading": 0.19620604644996878,
+                    "angularVelocity": 0.8356494040905814,
+                    "velocityX": 2.023080449510844,
+                    "velocityY": 2.5491557226145924,
+                    "timestamp": 0.3913127116628683
                 },
                 {
-                    "x": 1.9833277736824528,
-                    "y": 6.358008431463026,
-                    "heading": 3.401912848694198,
-                    "angularVelocity": 0.8383417622622024,
-                    "velocityX": 2.023136812758436,
-                    "velocityY": 2.549061649396556,
-                    "timestamp": 0.4695758303578373
+                    "x": 1.9833278311922586,
+                    "y": 6.358008426099132,
+                    "heading": 0.26178707056088246,
+                    "angularVelocity": 0.8379618417228061,
+                    "velocityX": 2.023138973840461,
+                    "velocityY": 2.549064785314409,
+                    "timestamp": 0.469575253995442
                 },
                 {
-                    "x": 2.1099966456081427,
-                    "y": 6.517605183327805,
-                    "heading": 3.454415313467435,
-                    "angularVelocity": 0.6708496653232114,
-                    "velocityX": 1.618510115767619,
-                    "velocityY": 2.0392457389873657,
-                    "timestamp": 0.5478384687508102
+                    "x": 2.109996681019138,
+                    "y": 6.517605179473628,
+                    "heading": 0.31426571552794874,
+                    "angularVelocity": 0.6705461310477281,
+                    "velocityX": 1.618511819979031,
+                    "velocityY": 2.039248261272889,
+                    "timestamp": 0.5478377963280157
                 },
                 {
-                    "x": 2.20499831408803,
-                    "y": 6.637302614330095,
-                    "heading": 3.493799088289958,
-                    "angularVelocity": 0.5032257489808813,
-                    "velocityX": 1.2138827725544437,
-                    "velocityY": 1.5294326061595858,
-                    "timestamp": 0.6261011071437831
+                    "x": 2.204998332006287,
+                    "y": 6.637302612224517,
+                    "heading": 0.35363163522076196,
+                    "angularVelocity": 0.5029982226430836,
+                    "velocityX": 1.2138840389754155,
+                    "velocityY": 1.5294345057465644,
+                    "timestamp": 0.6261003386605893
                 },
                 {
-                    "x": 2.268332767215697,
-                    "y": 6.717100865980731,
-                    "heading": 3.520056877037348,
-                    "angularVelocity": 0.3355086064891452,
-                    "velocityX": 0.8092552772071353,
-                    "velocityY": 1.0196212814849352,
-                    "timestamp": 0.704363745536756
+                    "x": 2.268332773183591,
+                    "y": 6.717100865272008,
+                    "heading": 0.3798775217934225,
+                    "angularVelocity": 0.3353569382033373,
+                    "velocityX": 0.8092561178011136,
+                    "velocityY": 1.0196225508288703,
+                    "timestamp": 0.704362880993163
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": 3.5331853071795862,
-                    "angularVelocity": 0.16774837153225197,
-                    "velocityX": 0.4046277180855394,
-                    "velocityY": 0.5098107454405937,
-                    "timestamp": 0.7826263839297288
+                    "heading": 0.393,
+                    "angularVelocity": 0.1676725265429579,
+                    "velocityX": 0.40462813847574725,
+                    "velocityY": 0.509811380244222,
+                    "timestamp": 0.7826254233257367
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": 3.5331853071795862,
-                    "angularVelocity": 3.497795149569243e-31,
-                    "velocityX": 5.486835585676403e-32,
-                    "velocityY": -3.2429311821904986e-32,
-                    "timestamp": 0.8608890223227017
+                    "heading": 0.393,
+                    "angularVelocity": 9.092584352802723e-32,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0.8608879656583104
                 }
             ],
             "constraints": [
@@ -3014,7 +3210,7 @@
                 {
                     "x": 1.35,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -3023,7 +3219,7 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -3034,74 +3230,74 @@
                 {
                     "x": 1.35,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
+                    "velocityX": -1.1658790467474463e-32,
+                    "velocityY": 2.7665844766493316e-30,
                     "timestamp": 0
                 },
                 {
-                    "x": 1.425000004529508,
-                    "y": 5.5600000086998405,
-                    "heading": 3.139999994805454,
-                    "angularVelocity": -5.477109235911092e-8,
-                    "velocityX": 0.7907971475606559,
-                    "velocityY": 9.173079093903646e-8,
-                    "timestamp": 0.09484101550044495
+                    "x": 1.4250000045295077,
+                    "y": 5.560000008699219,
+                    "heading": -5.237332222253062e-9,
+                    "angularVelocity": -5.5222228614505115e-8,
+                    "velocityX": 0.7907971475606554,
+                    "velocityY": 9.172423930862072e-8,
+                    "timestamp": 0.09484101550044491
                 },
                 {
-                    "x": 1.5750000090590133,
-                    "y": 5.560000023679525,
-                    "heading": 3.1399999868961004,
-                    "angularVelocity": -8.339591750663206e-8,
-                    "velocityX": 1.5815942473623312,
-                    "velocityY": 1.5794521377308047e-7,
-                    "timestamp": 0.1896820310008899
+                    "x": 1.575000009059013,
+                    "y": 5.560000023677869,
+                    "heading": -1.3214169082837694e-8,
+                    "angularVelocity": -8.41074599041897e-8,
+                    "velocityX": 1.5815942473623306,
+                    "velocityY": 1.579343082639882e-7,
+                    "timestamp": 0.18968203100088982
                 },
                 {
-                    "x": 1.7999999999999887,
-                    "y": 5.5600000423644245,
-                    "heading": 3.1399999785762156,
-                    "angularVelocity": -8.772454789703223e-8,
-                    "velocityX": 2.372391203887096,
-                    "velocityY": 1.9701287329918457e-7,
-                    "timestamp": 0.28452304650133486
+                    "x": 1.7999999999999883,
+                    "y": 5.560000042361517,
+                    "heading": -2.1609400534628168e-8,
+                    "angularVelocity": -8.851899686654573e-8,
+                    "velocityX": 2.372391203887095,
+                    "velocityY": 1.9699965815991792e-7,
+                    "timestamp": 0.28452304650133475
                 },
                 {
-                    "x": 2.0249999909409757,
-                    "y": 5.560000024808709,
-                    "heading": 3.1399999889274675,
-                    "angularVelocity": 1.0914320422483106e-7,
-                    "velocityX": 2.372391203887217,
-                    "velocityY": -1.851067857722626e-7,
-                    "timestamp": 0.3793640620017798
+                    "x": 2.0249999909409753,
+                    "y": 5.560000024806983,
+                    "heading": -1.1174048484706425e-8,
+                    "angularVelocity": 1.1002994951426502e-7,
+                    "velocityX": 2.372391203887216,
+                    "velocityY": -1.8509433303171823e-7,
+                    "timestamp": 0.37936406200177963
                 },
                 {
-                    "x": 2.174999995470489,
-                    "y": 5.560000009522542,
-                    "heading": 3.1399999962990623,
-                    "angularVelocity": 7.772581212467168e-8,
-                    "velocityX": 1.5815942473624114,
-                    "velocityY": -1.6117675426775267e-7,
-                    "timestamp": 0.47420507750222474
+                    "x": 2.1749999954704884,
+                    "y": 5.560000009521861,
+                    "heading": -3.737396801552092e-9,
+                    "angularVelocity": 7.841176829375941e-8,
+                    "velocityX": 1.581594247362411,
+                    "velocityY": -1.6116572709153717e-7,
+                    "timestamp": 0.4742050775022245
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 3.902254149386954e-8,
-                    "velocityX": 0.7907971475606953,
-                    "velocityY": -1.0040532392087884e-7,
-                    "timestamp": 0.5690460930026697
+                    "heading": 2.347281627650706e-31,
+                    "angularVelocity": 3.940696757721575e-8,
+                    "velocityX": 0.7907971475606952,
+                    "velocityY": -1.0039814487748917e-7,
+                    "timestamp": 0.5690460930026694
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 1.1924381690495433e-31,
+                    "heading": 0,
+                    "angularVelocity": -9.04607706846012e-36,
                     "velocityX": 0,
-                    "velocityY": -3.802893138506909e-33,
-                    "timestamp": 0.6638871085031147
+                    "velocityY": -2.7063013184679635e-30,
+                    "timestamp": 0.6638871085031143
                 }
             ],
             "constraints": [
@@ -3144,7 +3340,7 @@
                 {
                     "x": 1.35,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -3153,7 +3349,7 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -3164,110 +3360,110 @@
                 {
                     "x": 1.35,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 3.0431878595519687e-30,
-                    "velocityX": -9.811045254051387e-32,
-                    "velocityY": 1.5487452756550695e-31,
+                    "heading": -1.4926991504029801e-31,
+                    "angularVelocity": 3.0130798970250824e-30,
+                    "velocityX": -6.359546450907426e-32,
+                    "velocityY": -1.6682795346112646e-37,
                     "timestamp": 0
                 },
                 {
-                    "x": 1.3816658692579147,
-                    "y": 5.519999048484489,
-                    "heading": 3.127016326666322,
-                    "angularVelocity": -0.16577538674563452,
-                    "velocityX": 0.4043094421708381,
-                    "velocityY": -0.5107316733298447,
-                    "timestamp": 0.07832087494146221
+                    "x": 1.381665864471773,
+                    "y": 5.519999051141232,
+                    "heading": -0.0129840210078559,
+                    "angularVelocity": -0.16577982445307443,
+                    "velocityX": 0.40430937766578573,
+                    "velocityY": -0.5107316351191263,
+                    "timestamp": 0.07832087559925201
                 },
                 {
-                    "x": 1.4449977551622273,
-                    "y": 5.439997223821588,
-                    "heading": 3.1010513131375346,
-                    "angularVelocity": -0.33152098400576074,
-                    "velocityX": 0.8086207661960841,
-                    "velocityY": -1.0214623460564694,
-                    "timestamp": 0.15664174988292442
+                    "x": 1.4449977408289922,
+                    "y": 5.439997231153384,
+                    "heading": -0.03894969123495126,
+                    "angularVelocity": -0.3315293659375504,
+                    "velocityX": 0.8086206375075861,
+                    "velocityY": -1.0214622777865288,
+                    "timestamp": 0.15664175119850401
                 },
                 {
-                    "x": 1.5399958776695735,
-                    "y": 5.319994658705983,
-                    "heading": 3.0621074441034875,
-                    "angularVelocity": -0.49723485677543305,
-                    "velocityX": 1.2129348986250355,
-                    "velocityY": -1.5321913245389087,
-                    "timestamp": 0.23496262482438662
+                    "x": 1.5399958493613461,
+                    "y": 5.319994671739442,
+                    "heading": -0.07789445681060216,
+                    "angularVelocity": -0.4972462996317751,
+                    "velocityX": 1.212934710005481,
+                    "velocityY": -1.532191238871801,
+                    "timestamp": 0.23496262679775604
                 },
                 {
-                    "x": 1.6666605105444467,
-                    "y": 5.159991566646187,
-                    "heading": 3.010185320857369,
-                    "angularVelocity": -0.6629410522408352,
-                    "velocityX": 1.6172525264757869,
-                    "velocityY": -2.042917576947189,
-                    "timestamp": 0.31328349976584885
+                    "x": 1.6666604646819347,
+                    "y": 5.1599915852165825,
+                    "heading": -0.12981761618342352,
+                    "angularVelocity": -0.6629542759263352,
+                    "velocityX": 1.6172522887601493,
+                    "velocityY": -2.042917489093896,
+                    "timestamp": 0.31328350239700803
                 },
                 {
-                    "x": 1.8249919396490273,
-                    "y": 4.959988272167279,
-                    "heading": 2.9452809657766084,
-                    "angularVelocity": -0.8286980339439337,
-                    "velocityX": 2.021573804211457,
-                    "velocityY": -2.5536396858231227,
-                    "timestamp": 0.3916043747073111
+                    "x": 1.8249918742420412,
+                    "y": 4.95998829498017,
+                    "heading": -0.19472302000870664,
+                    "angularVelocity": -0.8287114173414627,
+                    "velocityX": 2.0215735376893504,
+                    "velocityY": -2.5536396102078434,
+                    "timestamp": 0.39160437799626
                 },
                 {
-                    "x": 1.9833278878841576,
-                    "y": 4.759991849616898,
-                    "heading": 2.880203589095056,
-                    "angularVelocity": -0.8309071716856798,
-                    "velocityX": 2.021631504416561,
-                    "velocityY": -2.553551945121407,
-                    "timestamp": 0.4699252496487733
+                    "x": 1.9833278418494473,
+                    "y": 4.759991864222915,
+                    "heading": -0.25979913954005024,
+                    "angularVelocity": -0.8308911134284025,
+                    "velocityX": 2.0216317347825643,
+                    "velocityY": -2.5535520284602864,
+                    "timestamp": 0.469925253595512
                 },
                 {
-                    "x": 2.109996705593866,
-                    "y": 4.599994984440838,
-                    "heading": 2.828128420495832,
-                    "angularVelocity": -0.6648951334896099,
-                    "velocityX": 1.6173059583971898,
-                    "velocityY": -2.0428380721696886,
-                    "timestamp": 0.5482461245902355
+                    "x": 2.1099966770232617,
+                    "y": 4.599994992856252,
+                    "heading": -0.31187323961880925,
+                    "angularVelocity": -0.6648814850489074,
+                    "velocityX": 1.6173061677954905,
+                    "velocityY": -2.0428381340541457,
+                    "timestamp": 0.548246129194764
                 },
                 {
-                    "x": 2.2049983397447894,
-                    "y": 4.479997467084651,
-                    "heading": 2.789065384524658,
-                    "angularVelocity": -0.4987563788117872,
-                    "velocityX": 1.212979735248467,
-                    "velocityY": -1.5321268748066883,
-                    "timestamp": 0.6265669995316977
+                    "x": 2.204998325241535,
+                    "y": 4.479997471163016,
+                    "heading": -0.3509354488627182,
+                    "angularVelocity": -0.49874581897910997,
+                    "velocityX": 1.212979904672827,
+                    "velocityY": -1.5321269173142713,
+                    "timestamp": 0.6265670047940161
                 },
                 {
-                    "x": 2.268332774342349,
-                    "y": 4.3999991596507995,
-                    "heading": 2.7630214971574416,
-                    "angularVelocity": -0.3325280442370214,
-                    "velocityX": 0.8086533078811499,
-                    "velocityY": -1.0214174381177892,
-                    "timestamp": 0.7048878744731599
+                    "x": 2.2683327695206965,
+                    "y": 4.399999160989759,
+                    "heading": -0.37697877943637265,
+                    "angularVelocity": -0.33252093231071334,
+                    "velocityX": 0.8086534247041309,
+                    "velocityY": -1.0214174645159662,
+                    "timestamp": 0.7048878803932681
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": -0.1662583208777211,
-                    "velocityX": 0.4043267606665432,
-                    "velocityY": -0.510708794822514,
-                    "timestamp": 0.7832087494146222
+                    "heading": -0.39,
+                    "angularVelocity": -0.16625478793487553,
+                    "velocityX": 0.40432681883354604,
+                    "velocityY": -0.5107088076290647,
+                    "timestamp": 0.7832087559925202
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": 6.867412911994789e-31,
-                    "velocityX": 0,
-                    "velocityY": 9.317443122449736e-32,
-                    "timestamp": 0.8615296243560844
+                    "heading": -0.39,
+                    "angularVelocity": 0,
+                    "velocityX": 7.259744325812885e-33,
+                    "velocityY": -1.1119528318776295e-29,
+                    "timestamp": 0.8615296315917722
                 }
             ],
             "constraints": [
@@ -3310,7 +3506,7 @@
                 {
                     "x": 1.35,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -3319,7 +3515,7 @@
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -3330,218 +3526,218 @@
                 {
                     "x": 1.35,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": -5.5460688640051806e-30,
-                    "velocityX": 1.8487232872526845e-30,
-                    "velocityY": -2.9628257998243394e-30,
+                    "heading": 0,
+                    "angularVelocity": -1.7848043602710886e-30,
+                    "velocityX": 4.033392918765628e-32,
+                    "velocityY": -2.9619524123271102e-31,
                     "timestamp": 0
                 },
                 {
-                    "x": 1.423394920733784,
-                    "y": 5.582111402692338,
-                    "heading": 3.1399999953305415,
-                    "angularVelocity": -4.870437841651369e-8,
-                    "velocityX": 0.7654820737524797,
-                    "velocityY": 0.23061381110796328,
-                    "timestamp": 0.09588065253313424
-                },
-                {
-                    "x": 1.5701847607820971,
-                    "y": 5.626334202094221,
-                    "heading": 3.1399999893931145,
-                    "angularVelocity": -6.193245197617188e-8,
-                    "velocityX": 1.530964132702707,
-                    "velocityY": 0.46122755981751334,
-                    "timestamp": 0.19176130506626848
-                },
-                {
-                    "x": 1.7903695158505508,
-                    "y": 5.692668391072556,
-                    "heading": 3.1399999853590668,
-                    "angularVelocity": -4.208451353086861e-8,
-                    "velocityX": 2.2964461468641186,
-                    "velocityY": 0.6918412341313931,
-                    "timestamp": 0.2876419575994027
-                },
-                {
-                    "x": 2.0839491699386645,
-                    "y": 5.781113958575535,
-                    "heading": 3.139999985983334,
-                    "angularVelocity": 6.496408603804424e-9,
-                    "velocityX": 3.0619279941465973,
-                    "velocityY": 0.922454793178729,
-                    "timestamp": 0.38352261013253697
-                },
-                {
-                    "x": 2.431241902978436,
-                    "y": 5.885741341465996,
-                    "heading": 3.1399999859835575,
-                    "angularVelocity": 2.3334348335212476e-12,
-                    "velocityX": 3.6221356849840607,
-                    "velocityY": 1.0912251859537307,
-                    "timestamp": 0.47940326266567124
-                },
-                {
-                    "x": 2.7785346564747577,
-                    "y": 5.990368656458575,
-                    "heading": 3.139999985983492,
-                    "angularVelocity": -6.768139199697334e-13,
-                    "velocityX": 3.62213589833913,
-                    "velocityY": 1.0912244778026317,
-                    "timestamp": 0.5752839151988055
-                },
-                {
-                    "x": 3.125827431847794,
-                    "y": 6.094995898834488,
-                    "heading": 3.139999985983408,
-                    "angularVelocity": -8.748780481443456e-13,
-                    "velocityX": 3.6221361265054535,
-                    "velocityY": 1.091223720437653,
-                    "timestamp": 0.6711645677319398
-                },
-                {
-                    "x": 3.473120229952072,
-                    "y": 6.199623065757117,
-                    "heading": 3.1399999859833065,
-                    "angularVelocity": -1.0615094861925232e-12,
-                    "velocityX": 3.6221363635841364,
-                    "velocityY": 1.0912229334878236,
-                    "timestamp": 0.767045220265074
-                },
-                {
-                    "x": 3.820413051729017,
-                    "y": 6.304250154101482,
-                    "heading": 3.139999985983188,
-                    "angularVelocity": -1.2288425429953676e-12,
-                    "velocityX": 3.6221366104815065,
-                    "velocityY": 1.0912221139455949,
-                    "timestamp": 0.8629258727982083
-                },
-                {
-                    "x": 4.167705898643341,
-                    "y": 6.408877159005614,
-                    "heading": 3.139999985983049,
-                    "angularVelocity": -1.4178096575705385e-12,
-                    "velocityX": 3.6221368726552323,
-                    "velocityY": 1.0912212436948225,
-                    "timestamp": 0.9588065253313426
-                },
-                {
-                    "x": 4.5149987734968,
-                    "y": 6.5135040711691525,
-                    "heading": 3.139999985982883,
-                    "angularVelocity": -1.676018359549876e-12,
-                    "velocityX": 3.622137164050193,
-                    "velocityY": 1.0912202764447858,
-                    "timestamp": 1.0546871778644769
-                },
-                {
-                    "x": 4.862291683371807,
-                    "y": 6.6181308670796435,
-                    "heading": 3.1399999859826795,
-                    "angularVelocity": -2.066469238283253e-12,
-                    "velocityX": 3.6221375293116886,
-                    "velocityY": 1.0912190639696435,
-                    "timestamp": 1.150567830397611
-                },
-                {
-                    "x": 5.209584643601777,
-                    "y": 6.722757495842404,
-                    "heading": 3.139999985982697,
-                    "angularVelocity": 1.7634954198261724e-13,
-                    "velocityX": 3.6221380544925466,
-                    "velocityY": 1.0912173206895408,
-                    "timestamp": 1.2464484829307452
-                },
-                {
-                    "x": 5.55687764566899,
-                    "y": 6.827383985745101,
-                    "heading": 3.139999985982843,
-                    "angularVelocity": 1.4429880633934061e-12,
-                    "velocityX": 3.6221384908382315,
-                    "velocityY": 1.091215872433919,
-                    "timestamp": 1.3423291354638793
-                },
-                {
-                    "x": 5.904170680203765,
-                    "y": 6.932010367880933,
-                    "heading": 3.1399999859829473,
-                    "angularVelocity": 9.965559645934633e-13,
-                    "velocityX": 3.6221388294623984,
-                    "velocityY": 1.0912147484660204,
-                    "timestamp": 1.4382097879970135
-                },
-                {
-                    "x": 6.251463742058712,
-                    "y": 7.036636659333844,
-                    "heading": 3.1399999859830086,
-                    "angularVelocity": 5.792335690532441e-13,
-                    "velocityX": 3.6221391144013104,
-                    "velocityY": 1.0912138026768703,
-                    "timestamp": 1.5340904405301476
-                },
-                {
-                    "x": 6.598756826003656,
-                    "y": 7.141262877462233,
-                    "heading": 3.139999985983042,
-                    "angularVelocity": 3.176331549030912e-13,
-                    "velocityX": 3.6221393447915826,
-                    "velocityY": 1.0912130379289529,
-                    "timestamp": 1.6299710930632818
-                },
-                {
-                    "x": 6.9460499272931315,
-                    "y": 7.245889038012359,
-                    "heading": 3.139999985982778,
-                    "angularVelocity": -2.760419154137832e-12,
-                    "velocityX": 3.622139525688641,
-                    "velocityY": 1.0912124374075387,
-                    "timestamp": 1.725851745596416
-                },
-                {
-                    "x": 7.239629936933939,
-                    "y": 7.334333425317839,
-                    "heading": 3.139999987228266,
-                    "angularVelocity": 1.3004483336236485e-8,
-                    "velocityX": 3.061931702425989,
-                    "velocityY": 0.9224424841673509,
-                    "timestamp": 1.8217323981295501
-                },
-                {
-                    "x": 7.459814962920636,
-                    "y": 7.4006667150284455,
-                    "heading": 3.13999999169634,
-                    "angularVelocity": 4.661129161255315e-8,
-                    "velocityX": 2.2964489724386126,
-                    "velocityY": 0.6918318551100047,
-                    "timestamp": 1.9176130506626843
-                },
-                {
-                    "x": 7.606604986293984,
-                    "y": 7.444888905913496,
-                    "heading": 3.1399999966584766,
-                    "angularVelocity": 5.176056299229228e-8,
-                    "velocityX": 1.530966044713388,
-                    "velocityY": 0.46122121321737414,
-                    "timestamp": 2.0134937031958184
+                    "x": 1.4233949207458116,
+                    "y": 5.582111402652435,
+                    "heading": -4.931478664026919e-9,
+                    "angularVelocity": -5.143711867197603e-8,
+                    "velocityX": 0.7654820738778157,
+                    "velocityY": 0.2306138106917608,
+                    "timestamp": 0.09588065253314748
+                },
+                {
+                    "x": 1.570184760818058,
+                    "y": 5.626334201974914,
+                    "heading": -1.1368240805751946e-8,
+                    "angularVelocity": -6.714026800936027e-8,
+                    "velocityX": 1.530964132952106,
+                    "velocityY": 0.4612275589893128,
+                    "timestamp": 0.19176130506629496
+                },
+                {
+                    "x": 1.7903695159222153,
+                    "y": 5.69266839083479,
+                    "heading": -1.6115736339739044e-8,
+                    "angularVelocity": -4.952541704566216e-8,
+                    "velocityX": 2.2964461472361695,
+                    "velocityY": 0.6918412328958446,
+                    "timestamp": 0.28764195759944244
+                },
+                {
+                    "x": 2.0839491700576396,
+                    "y": 5.781113958180788,
+                    "heading": -1.639772094621516e-8,
+                    "angularVelocity": -2.9553274583871895e-9,
+                    "velocityX": 3.0619279946395954,
+                    "velocityY": 0.9224547915413995,
+                    "timestamp": 0.3835226101325899
+                },
+                {
+                    "x": 2.431241903149783,
+                    "y": 5.885741340897451,
+                    "heading": -1.6397499367976412e-8,
+                    "angularVelocity": 2.3168753954621105e-12,
+                    "velocityX": 3.6221356855297673,
+                    "velocityY": 1.0912251841409724,
+                    "timestamp": 0.4794032626657374
+                },
+                {
+                    "x": 2.7785346566980107,
+                    "y": 5.9903686557177505,
+                    "heading": -1.6397564221401302e-8,
+                    "angularVelocity": -6.713496881725287e-13,
+                    "velocityX": 3.622135898879975,
+                    "velocityY": 1.091224476005724,
+                    "timestamp": 0.5752839151988849
+                },
+                {
+                    "x": 3.125827432121608,
+                    "y": 6.094995897925851,
+                    "heading": -1.6397647407961563e-8,
+                    "angularVelocity": -8.684642063246534e-13,
+                    "velocityX": 3.6221361270322743,
+                    "velocityY": 1.0912237186873281,
+                    "timestamp": 0.6711645677320324
+                },
+                {
+                    "x": 3.4731202302782154,
+                    "y": 6.1996230646748005,
+                    "heading": -1.6397748264247437e-8,
+                    "angularVelocity": -1.0543921850153982e-12,
+                    "velocityX": 3.6221363641294033,
+                    "velocityY": 1.0912229316763022,
+                    "timestamp": 0.7670452202651799
+                },
+                {
+                    "x": 3.8204130521112987,
+                    "y": 6.304250152832848,
+                    "heading": -1.639786614425739e-8,
+                    "angularVelocity": -1.2203887930777277e-12,
+                    "velocityX": 3.622136611066492,
+                    "velocityY": 1.091222112002269,
+                    "timestamp": 0.8629258727983273
+                },
+                {
+                    "x": 4.167705899087819,
+                    "y": 6.408877157530555,
+                    "heading": -1.6398004207896958e-8,
+                    "angularVelocity": -1.408074947723948e-12,
+                    "velocityX": 3.6221368733034063,
+                    "velocityY": 1.0912212415417761,
+                    "timestamp": 0.9588065253314748
+                },
+                {
+                    "x": 4.514998774010408,
+                    "y": 6.51350406946466,
+                    "heading": -1.6398168919050745e-8,
+                    "angularVelocity": -1.6668913936591836e-12,
+                    "velocityX": 3.6221371647706677,
+                    "velocityY": 1.091220274051783,
+                    "timestamp": 1.0546871778646223
+                },
+                {
+                    "x": 4.862291683961819,
+                    "y": 6.618130865121593,
+                    "heading": -1.6398370762014746e-8,
+                    "angularVelocity": -2.0513887673195245e-12,
+                    "velocityX": 3.6221375301080334,
+                    "velocityY": 1.0912190613250203,
+                    "timestamp": 1.1505678303977698
+                },
+                {
+                    "x": 5.20958464417323,
+                    "y": 6.722757493946074,
+                    "heading": -1.639835180923073e-8,
+                    "angularVelocity": 1.902358989315915e-13,
+                    "velocityX": 3.6221380542985027,
+                    "velocityY": 1.091217321333091,
+                    "timestamp": 1.2464484829309173
+                },
+                {
+                    "x": 5.556877646148189,
+                    "y": 6.8273839841549115,
+                    "heading": -1.6398206125528607e-8,
+                    "angularVelocity": 1.4352640935347951e-12,
+                    "velocityX": 3.62213848987558,
+                    "velocityY": 1.09121587562664,
+                    "timestamp": 1.3423291354640647
+                },
+                {
+                    "x": 5.904170680592406,
+                    "y": 6.932010366591235,
+                    "heading": -1.6398102572596455e-8,
+                    "angularVelocity": 9.899977906647045e-13,
+                    "velocityX": 3.6221388285174303,
+                    "velocityY": 1.091214751599812,
+                    "timestamp": 1.4382097879972122
+                },
+                {
+                    "x": 6.2514637423704675,
+                    "y": 7.036636658299249,
+                    "heading": -1.6398041778119566e-8,
+                    "angularVelocity": 5.750349690087699e-13,
+                    "velocityX": 3.6221391135989474,
+                    "velocityY": 1.0912138053372793,
+                    "timestamp": 1.5340904405303597
+                },
+                {
+                    "x": 6.5987568262419405,
+                    "y": 7.141262876671409,
+                    "heading": -1.639800860171266e-8,
+                    "angularVelocity": 3.175769618903374e-13,
+                    "velocityX": 3.6221393440248217,
+                    "velocityY": 1.0912130404711837,
+                    "timestamp": 1.6299710930635072
+                },
+                {
+                    "x": 6.946049927457118,
+                    "y": 7.2458890374680776,
+                    "heading": -1.6398270316090924e-8,
+                    "angularVelocity": -2.740394345840068e-12,
+                    "velocityX": 3.6221395249132713,
+                    "velocityY": 1.0912124399786907,
+                    "timestamp": 1.7258517455966547
+                },
+                {
+                    "x": 7.239629937032664,
+                    "y": 7.334333424990155,
+                    "heading": -1.4134914023195805e-8,
+                    "angularVelocity": 2.36203418937418e-8,
+                    "velocityX": 3.061931701744924,
+                    "velocityY": 0.9224424864262022,
+                    "timestamp": 1.8217323981298021
+                },
+                {
+                    "x": 7.45981496297016,
+                    "y": 7.4006667148640615,
+                    "heading": -8.953329207671134e-9,
+                    "angularVelocity": 5.4052849582725206e-8,
+                    "velocityX": 2.2964489719251726,
+                    "velocityY": 0.6918318568130147,
+                    "timestamp": 1.9176130506629496
+                },
+                {
+                    "x": 7.606604986310546,
+                    "y": 7.444888905858524,
+                    "heading": -3.5476510780655253e-9,
+                    "angularVelocity": 5.638647309242438e-8,
+                    "velocityX": 1.5309660443693864,
+                    "velocityY": 0.4612212143584205,
+                    "timestamp": 2.0134937031960973
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
-                    "angularVelocity": 3.485452160899378e-8,
-                    "velocityX": 0.7654830434176825,
-                    "velocityY": 0.23061059246584142,
-                    "timestamp": 2.109374355728953
+                    "heading": 1.0344705803481413e-31,
+                    "angularVelocity": 3.7004322975715074e-8,
+                    "velocityX": 0.7654830432448523,
+                    "velocityY": 0.23061059303913045,
+                    "timestamp": 2.109374355729245
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
-                    "angularVelocity": -1.7922203019823127e-29,
+                    "heading": 0,
+                    "angularVelocity": -1.5335768309775317e-33,
                     "velocityX": 0,
-                    "velocityY": -5.07505109843192e-30,
-                    "timestamp": 2.205255008262087
+                    "velocityY": -2.5688294662733772e-30,
+                    "timestamp": 2.2052550082623927
                 }
             ],
             "constraints": [
@@ -3584,7 +3780,7 @@
                 {
                     "x": 1.35,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -3593,7 +3789,7 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -3604,209 +3800,209 @@
                 {
                     "x": 1.35,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "angularVelocity": 0,
-                    "velocityX": -2.32021994810819e-32,
+                    "velocityX": 4.006684802425094e-34,
                     "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 1.430775984981732,
-                    "y": 5.581186654302167,
-                    "heading": 3.138045514038262,
-                    "angularVelocity": -0.019529788750833752,
-                    "velocityX": 0.8071369934195644,
-                    "velocityY": 0.2117031746247163,
-                    "timestamp": 0.10007716860989323
-                },
-                {
-                    "x": 1.592327952146185,
-                    "y": 5.62355996139984,
-                    "heading": 3.134136547157813,
-                    "angularVelocity": -0.0390595271102885,
-                    "velocityX": 1.6142739588706034,
-                    "velocityY": 0.42340633419443324,
-                    "timestamp": 0.20015433721978645
-                },
-                {
-                    "x": 1.8346558957131356,
-                    "y": 5.687119918958483,
-                    "heading": 3.1282731070819607,
-                    "angularVelocity": -0.05858918829621809,
-                    "velocityX": 2.4214108665639773,
-                    "velocityY": 0.6351094704367861,
-                    "timestamp": 0.30023150582967967
-                },
-                {
-                    "x": 2.1577597979432506,
-                    "y": 5.771866521439831,
-                    "heading": 3.1204552085550783,
-                    "angularVelocity": -0.078118702152105,
-                    "velocityX": 3.2285475970008037,
-                    "velocityY": 0.8468125513392032,
-                    "timestamp": 0.4003086744395729
-                },
-                {
-                    "x": 2.523958853996556,
-                    "y": 5.867916494702879,
-                    "heading": 3.120455207736121,
-                    "angularVelocity": -8.183261644434384e-9,
-                    "velocityX": 3.659166832354859,
-                    "velocityY": 0.9597590998747719,
-                    "timestamp": 0.5003858430494661
-                },
-                {
-                    "x": 2.890157912923332,
-                    "y": 5.963966457010659,
-                    "heading": 3.1204552069171707,
-                    "angularVelocity": -8.18318468791684e-9,
-                    "velocityX": 3.6591668610673986,
-                    "velocityY": 0.9597589904065732,
-                    "timestamp": 0.6004630116593593
-                },
-                {
-                    "x": 3.256356975588488,
-                    "y": 6.06001640506554,
-                    "heading": 3.1204552060982262,
-                    "angularVelocity": -8.183132612245063e-9,
-                    "velocityX": 3.659166898422379,
-                    "velocityY": 0.9597588479874845,
-                    "timestamp": 0.7005401802692526
-                },
-                {
-                    "x": 3.6225560432776467,
-                    "y": 6.156066333965972,
-                    "heading": 3.1204552052792867,
-                    "angularVelocity": -8.183078308583848e-9,
-                    "velocityX": 3.6591669486236644,
-                    "velocityY": 0.9597586565906886,
-                    "timestamp": 0.8006173488791458
-                },
-                {
-                    "x": 3.9887551180209555,
-                    "y": 6.252116235971829,
-                    "heading": 3.1204552044603537,
-                    "angularVelocity": -8.183012061558546e-9,
-                    "velocityX": 3.659167019110774,
-                    "velocityY": 0.9597583878523045,
-                    "timestamp": 0.9006945174890391
-                },
-                {
-                    "x": 4.354954203462548,
-                    "y": 6.348166097189504,
-                    "heading": 3.1204552036414284,
-                    "angularVelocity": -8.182937495729244e-9,
-                    "velocityX": 3.659167126011114,
-                    "velocityY": 0.9597579802850231,
-                    "timestamp": 1.0007716860989322
-                },
-                {
-                    "x": 4.721153326841163,
-                    "y": 6.444215813768638,
-                    "heading": 3.120455202822509,
-                    "angularVelocity": -8.182878073364452e-9,
-                    "velocityX": 3.659167505088806,
-                    "velocityY": 0.9597565350148991,
-                    "timestamp": 1.1008488547088253
-                },
-                {
-                    "x": 5.092057106129702,
-                    "y": 6.520095539851432,
-                    "heading": 3.1204551978641732,
-                    "angularVelocity": -4.954512725191857e-8,
-                    "velocityX": 3.7061777870070114,
-                    "velocityY": 0.7582121590446533,
-                    "timestamp": 1.2009260233187184
-                },
-                {
-                    "x": 5.470573222615853,
-                    "y": 6.51282289206487,
-                    "heading": 3.120455142790982,
-                    "angularVelocity": -5.503072477147126e-7,
-                    "velocityX": 3.7822424609316347,
-                    "velocityY": -0.07267039912792687,
-                    "timestamp": 1.3010031919286116
-                },
-                {
-                    "x": 5.849058511568644,
-                    "y": 6.504092056642741,
-                    "heading": 3.1204550877725445,
-                    "angularVelocity": -5.497601318399077e-7,
-                    "velocityX": 3.7819344233063643,
-                    "velocityY": -0.08724103153050607,
-                    "timestamp": 1.4010803605385047
-                },
-                {
-                    "x": 6.221366644651584,
-                    "y": 6.4354334883920234,
-                    "heading": 3.1204550635505535,
-                    "angularVelocity": -2.420331372889176e-7,
-                    "velocityX": 3.720210496104441,
-                    "velocityY": -0.6860562624264548,
-                    "timestamp": 1.5011575291483978
-                },
-                {
-                    "x": 6.569565662964383,
-                    "y": 6.286823971409196,
-                    "heading": 3.1204550644036275,
-                    "angularVelocity": 8.52416239462205e-9,
-                    "velocityX": 3.479305251631334,
-                    "velocityY": -1.4849492551305274,
-                    "timestamp": 1.601234697758291
-                },
-                {
-                    "x": 6.915949214018099,
-                    "y": 6.134030710794496,
-                    "heading": 3.1204550652542946,
-                    "angularVelocity": 8.500112010852639e-9,
-                    "velocityX": 3.4611645779462337,
-                    "velocityY": -1.5267544309761423,
-                    "timestamp": 1.701311866368184
-                },
-                {
-                    "x": 7.22156951411921,
-                    "y": 5.999218440367679,
-                    "heading": 3.1282730490164057,
-                    "angularVelocity": 0.07811955384714984,
-                    "velocityX": 3.0538463902035207,
-                    "velocityY": -1.347083178904912,
-                    "timestamp": 1.8013890349780772
-                },
-                {
-                    "x": 7.4507847539347845,
-                    "y": 5.898109225088713,
-                    "heading": 3.1341365308407623,
-                    "angularVelocity": 0.05858960545933885,
-                    "velocityX": 2.290384939936379,
-                    "velocityY": -1.010312508671161,
-                    "timestamp": 1.9014662035879704
-                },
-                {
-                    "x": 7.603594917403085,
-                    "y": 5.830703076393403,
-                    "heading": 3.1380455124480764,
-                    "angularVelocity": 0.03905967426538248,
-                    "velocityX": 1.526923329175751,
-                    "velocityY": -0.6735417241675008,
-                    "timestamp": 2.0015433721978635
+                    "x": 1.4307759237486215,
+                    "y": 5.581186236587336,
+                    "heading": -0.002104963011526108,
+                    "angularVelocity": -0.021033383553861286,
+                    "velocityX": 0.8071357913745165,
+                    "velocityY": 0.21169884590096585,
+                    "timestamp": 0.1000772417873624
+                },
+                {
+                    "x": 1.592327768448065,
+                    "y": 5.623558708255768,
+                    "heading": -0.006314883423264977,
+                    "angularVelocity": -0.04206671103789844,
+                    "velocityX": 1.6142715547926287,
+                    "velocityY": 0.42339767675114676,
+                    "timestamp": 0.2001544835747248
+                },
+                {
+                    "x": 1.8346555283201378,
+                    "y": 5.6871174126709505,
+                    "heading": -0.012629752552513055,
+                    "angularVelocity": -0.06309995176191506,
+                    "velocityX": 2.4214072604734147,
+                    "velocityY": 0.6350964842758995,
+                    "timestamp": 0.3002317253620872
+                },
+                {
+                    "x": 2.1577591856288034,
+                    "y": 5.77186234429474,
+                    "heading": -0.02104955396970995,
+                    "angularVelocity": -0.08413302831713466,
+                    "velocityX": 3.228542788930726,
+                    "velocityY": 0.8467952364619561,
+                    "timestamp": 0.4003089671494496
+                },
+                {
+                    "x": 2.5239589562380984,
+                    "y": 5.867910684343787,
+                    "heading": -0.021049554851793276,
+                    "angularVelocity": -8.814025154574047e-9,
+                    "velocityX": 3.6591712967806616,
+                    "velocityY": 0.9597420785549242,
+                    "timestamp": 0.500386208936812
+                },
+                {
+                    "x": 2.8901587297210503,
+                    "y": 5.963959013436644,
+                    "heading": -0.021049555733868682,
+                    "angularVelocity": -8.813945991956408e-9,
+                    "velocityX": 3.659171325495056,
+                    "velocityY": 0.9597419690775807,
+                    "timestamp": 0.6004634507241744
+                },
+                {
+                    "x": 3.2563585069426413,
+                    "y": 6.060007328275347,
+                    "heading": -0.02104955661593894,
+                    "angularVelocity": -8.813894597525165e-9,
+                    "velocityX": 3.659171362852588,
+                    "velocityY": 0.9597418266460623,
+                    "timestamp": 0.7005406925115368
+                },
+                {
+                    "x": 3.622558289188601,
+                    "y": 6.15605562395784,
+                    "heading": -0.021049557498003604,
+                    "angularVelocity": -8.813838677794314e-9,
+                    "velocityX": 3.6591714130574977,
+                    "velocityY": 0.9597416352318217,
+                    "timestamp": 0.8006179342988993
+                },
+                {
+                    "x": 3.9887580784892815,
+                    "y": 6.252103892743073,
+                    "heading": -0.02104955838006166,
+                    "angularVelocity": -8.81377261667878e-9,
+                    "velocityX": 3.659171483550255,
+                    "velocityY": 0.9597413664668089,
+                    "timestamp": 0.9006951760862617
+                },
+                {
+                    "x": 4.354957878489299,
+                    "y": 6.3481521207353335,
+                    "heading": -0.021049559262112345,
+                    "angularVelocity": -8.813699036774153e-9,
+                    "velocityX": 3.6591715904610522,
+                    "velocityY": 0.9597409588519333,
+                    "timestamp": 1.0007724178736241
+                },
+                {
+                    "x": 4.721157716449094,
+                    "y": 6.444200203999558,
+                    "heading": -0.021049560144156942,
+                    "angularVelocity": -8.81363814954091e-9,
+                    "velocityX": 3.659171969765827,
+                    "velocityY": 0.9597395126886197,
+                    "timestamp": 1.1008496596609865
+                },
+                {
+                    "x": 5.092065894328832,
+                    "y": 6.520059807846628,
+                    "heading": -0.02104956519242719,
+                    "angularVelocity": -5.0443738888274115e-8,
+                    "velocityX": 3.70621902897584,
+                    "velocityY": 0.7580105375830706,
+                    "timestamp": 1.2009269014483488
+                },
+                {
+                    "x": 5.47058189966855,
+                    "y": 6.512767035208857,
+                    "heading": -0.021049620221549888,
+                    "angularVelocity": -5.498665002090286e-7,
+                    "velocityX": 3.7822385847120277,
+                    "velocityY": -0.07287143917560433,
+                    "timestamp": 1.3010041432357111
+                },
+                {
+                    "x": 5.849069453983326,
+                    "y": 6.50412284194313,
+                    "heading": -0.02104967520004647,
+                    "angularVelocity": -5.493606298291521e-7,
+                    "velocityX": 3.7819542940538056,
+                    "velocityY": -0.08637521489745259,
+                    "timestamp": 1.4010813850230734
+                },
+                {
+                    "x": 6.2213754396920145,
+                    "y": 6.435451104158958,
+                    "heading": -0.021049699334872447,
+                    "angularVelocity": -2.4116198195152956e-7,
+                    "velocityX": 3.720186318681132,
+                    "velocityY": -0.6861873544644725,
+                    "timestamp": 1.5011586268104358
+                },
+                {
+                    "x": 6.5695694546238315,
+                    "y": 6.286829159405189,
+                    "heading": -0.02104969841623417,
+                    "angularVelocity": 9.179292491243133e-9,
+                    "velocityX": 3.4792527123362977,
+                    "velocityY": -1.4850723511100814,
+                    "timestamp": 1.601235868597798
+                },
+                {
+                    "x": 6.915952468622566,
+                    "y": 6.134033995374984,
+                    "heading": -0.021049697500180466,
+                    "angularVelocity": 9.153466759383175e-9,
+                    "velocityX": 3.4611566807037675,
+                    "velocityY": -1.5267723340622583,
+                    "timestamp": 1.7013131103851604
+                },
+                {
+                    "x": 7.221571466882516,
+                    "y": 5.999220411113471,
+                    "heading": -0.012629808861323527,
+                    "angularVelocity": 0.0841338998605394,
+                    "velocityX": 3.053831148837094,
+                    "velocityY": -1.347095322110863,
+                    "timestamp": 1.8013903521725227
+                },
+                {
+                    "x": 7.450785730315982,
+                    "y": 5.898110210460681,
+                    "heading": -0.0063148981970481905,
+                    "angularVelocity": 0.0631003667916113,
+                    "velocityX": 2.290373508899117,
+                    "velocityY": -1.0103216160535444,
+                    "timestamp": 1.901467593959885
+                },
+                {
+                    "x": 7.603595242863294,
+                    "y": 5.830703404850431,
+                    "heading": -0.0021049639599730564,
+                    "angularVelocity": 0.04206684918455414,
+                    "velocityX": 1.5269157084883698,
+                    "velocityY": -0.673547795746317,
+                    "timestamp": 2.0015448357472474
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
-                    "angularVelocity": 0.019529804640431036,
-                    "velocityX": 0.7634616732088615,
-                    "velocityY": -0.33677088252547477,
-                    "timestamp": 2.101620540807757
+                    "heading": 0,
+                    "angularVelocity": 0.02103339303101045,
+                    "velocityX": 0.7634578628680123,
+                    "velocityY": -0.33677391831044023,
+                    "timestamp": 2.1016220775346097
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
+                    "heading": 0,
                     "angularVelocity": 0,
-                    "velocityX": 2.2237121345666e-32,
+                    "velocityX": 1.7450911582816108e-33,
                     "velocityY": 0,
-                    "timestamp": 2.20169770941765
+                    "timestamp": 2.201699319321972
                 }
             ],
             "constraints": [
@@ -3839,7 +4035,7 @@
                 {
                     "x": 1.35,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -3848,222 +4044,14 @@
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 1.35,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 4.242495082651476e-31,
-                    "velocityX": 1.1828951034939025e-32,
-                    "velocityY": -1.8669507935571478e-37,
-                    "timestamp": 0
-                },
-                {
-                    "x": 1.458096319535533,
-                    "y": 5.590985596762502,
-                    "heading": 3.1146996746822455,
-                    "angularVelocity": -0.21764643682588541,
-                    "velocityX": 0.9299002477407399,
-                    "velocityY": 0.2665540716802577,
-                    "timestamp": 0.11624507015474053
-                },
-                {
-                    "x": 1.674289226072187,
-                    "y": 5.652956746779397,
-                    "heading": 3.0641193580719652,
-                    "angularVelocity": -0.4351179498876838,
-                    "velocityX": 1.8598027963583075,
-                    "velocityY": 0.5331077690813153,
-                    "timestamp": 0.23249014030948106
-                },
-                {
-                    "x": 1.9985791294926345,
-                    "y": 5.745913563807996,
-                    "heading": 2.9882935824900567,
-                    "angularVelocity": -0.652292398128992,
-                    "velocityX": 2.789708870997854,
-                    "velocityY": 0.7996624450813951,
-                    "timestamp": 0.3487352104642216
-                },
-                {
-                    "x": 2.421300960273809,
-                    "y": 5.867094149195328,
-                    "heading": 2.9882935750965625,
-                    "angularVelocity": -6.360264782367115e-8,
-                    "velocityX": 3.6364710367369995,
-                    "velocityY": 1.0424578455329059,
-                    "timestamp": 0.4649802806189621
-                },
-                {
-                    "x": 2.844022813063846,
-                    "y": 5.988274657808269,
-                    "heading": 2.988293567703214,
-                    "angularVelocity": -6.360139701488917e-8,
-                    "velocityX": 3.636471226068578,
-                    "velocityY": 1.0424571850800302,
-                    "timestamp": 0.5812253507737026
-                },
-                {
-                    "x": 3.266744690508405,
-                    "y": 6.109455080417214,
-                    "heading": 2.988293560309863,
-                    "angularVelocity": -6.360141400691824e-8,
-                    "velocityX": 3.636471438159484,
-                    "velocityY": 1.0424564452293308,
-                    "timestamp": 0.6974704209284431
-                },
-                {
-                    "x": 3.6894665932724995,
-                    "y": 6.2306354147022835,
-                    "heading": 2.9882935529165104,
-                    "angularVelocity": -6.360143032479277e-8,
-                    "velocityX": 3.6364716559711723,
-                    "velocityY": 1.0424556854218385,
-                    "timestamp": 0.8137154910831836
-                },
-                {
-                    "x": 4.112188522356607,
-                    "y": 6.35181565717337,
-                    "heading": 2.988293545523163,
-                    "angularVelocity": -6.360138139725806e-8,
-                    "velocityX": 3.6364718823894826,
-                    "velocityY": 1.042454895590637,
-                    "timestamp": 0.9299605612379241
-                },
-                {
-                    "x": 4.534910480631917,
-                    "y": 6.472995797814601,
-                    "heading": 2.988293538129828,
-                    "angularVelocity": -6.360128257953913e-8,
-                    "velocityX": 3.636472133507264,
-                    "velocityY": 1.0424540195977474,
-                    "timestamp": 1.0462056313926646
-                },
-                {
-                    "x": 4.95763256393494,
-                    "y": 6.594175502311251,
-                    "heading": 2.9882935307365046,
-                    "angularVelocity": -6.360117492960731e-8,
-                    "velocityX": 3.6364732090600738,
-                    "velocityY": 1.0424502676572964,
-                    "timestamp": 1.1624507015474053
-                },
-                {
-                    "x": 5.395072294009931,
-                    "y": 6.63917509969073,
-                    "heading": 2.988293514020312,
-                    "angularVelocity": -1.4380130211846583e-7,
-                    "velocityX": 3.763081991285213,
-                    "velocityY": 0.3871097270583396,
-                    "timestamp": 1.2786957717021459
-                },
-                {
-                    "x": 5.834408437796079,
-                    "y": 6.6201427711176,
-                    "heading": 2.9882934665190843,
-                    "angularVelocity": -4.0863004174605414e-7,
-                    "velocityX": 3.7793959193394016,
-                    "velocityY": -0.16372589863635184,
-                    "timestamp": 1.3949408418568865
-                },
-                {
-                    "x": 6.258700069994256,
-                    "y": 6.504578567848497,
-                    "heading": 2.988293412737735,
-                    "angularVelocity": -4.6265488113342876e-7,
-                    "velocityX": 3.649975277518237,
-                    "velocityY": -0.9941428321671584,
-                    "timestamp": 1.511185912011627
-                },
-                {
-                    "x": 6.6396986873456,
-                    "y": 6.284991393688008,
-                    "heading": 2.988293385419437,
-                    "angularVelocity": -2.350060774071174e-7,
-                    "velocityX": 3.277546452887644,
-                    "velocityY": -1.8890020356836066,
-                    "timestamp": 1.6274309821663677
-                },
-                {
-                    "x": 6.952392117761244,
-                    "y": 5.9757968201953435,
-                    "heading": 2.988293399279684,
-                    "angularVelocity": 1.1923298633596667e-7,
-                    "velocityX": 2.689950034004883,
-                    "velocityY": -2.6598510636285697,
-                    "timestamp": 1.7436760523211083
-                },
-                {
-                    "x": 7.176252511109016,
-                    "y": 5.597293189983828,
-                    "heading": 2.988293408698133,
-                    "angularVelocity": 8.102235350708823e-8,
-                    "velocityX": 1.9257624693226023,
-                    "velocityY": -3.2560832877271055,
-                    "timestamp": 1.859921122475849
-                },
-                {
-                    "x": 7.318788154943764,
-                    "y": 5.181285899414456,
-                    "heading": 2.98829341726252,
-                    "angularVelocity": 7.367527341392295e-8,
-                    "velocityX": 1.226165063559341,
-                    "velocityY": -3.5787091015180317,
-                    "timestamp": 1.9761661926305896
-                },
-                {
-                    "x": 7.461323091390308,
-                    "y": 4.765278366475121,
-                    "heading": 2.9882934258271017,
-                    "angularVelocity": 7.36769447232769e-8,
-                    "velocityX": 1.2261589782414601,
-                    "velocityY": -3.578711186509359,
-                    "timestamp": 2.09241126278533
-                },
-                {
-                    "x": 7.570661558741158,
-                    "y": 4.44613912293617,
-                    "heading": 3.0641409244839792,
-                    "angularVelocity": 0.6524792712147914,
-                    "velocityX": 0.9405858433850398,
-                    "velocityY": -2.745400240320956,
-                    "timestamp": 2.2086563329400706
-                },
-                {
-                    "x": 7.643553854632525,
-                    "y": 4.2333797165247065,
-                    "heading": 3.1147139591088293,
-                    "angularVelocity": 0.43505530649626106,
-                    "velocityX": 0.6270570940714855,
-                    "velocityY": -1.8302660588379978,
-                    "timestamp": 2.324901403094811
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": 0.21752355482697977,
-                    "velocityX": 0.31352852485666055,
-                    "velocityY": -0.9151331439956911,
-                    "timestamp": 2.4411464732495514
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": -2.3792206080273036e-31,
-                    "velocityX": 0,
-                    "velocityY": -4.1394011967092386e-32,
-                    "timestamp": 2.5573915434042918
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -4104,7 +4092,7 @@
                 {
                     "x": 1.35,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -4113,7 +4101,7 @@
                 {
                     "x": 4.018431663513184,
                     "y": 5.088562488555908,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -4122,7 +4110,7 @@
                 {
                     "x": 6.893649101257324,
                     "y": 3.5118300914764404,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -4131,7 +4119,7 @@
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -4142,1091 +4130,1091 @@
                 {
                     "x": 1.35,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 4.408218114222724e-33,
                     "angularVelocity": 0,
-                    "velocityX": 4.411137018023184e-34,
-                    "velocityY": 2.784782344800776e-32,
+                    "velocityX": 1.8935205228973059e-34,
+                    "velocityY": 9.513896878368867e-32,
                     "timestamp": 0
                 },
                 {
-                    "x": 1.3544641699489606,
-                    "y": 5.55925259826767,
-                    "heading": 3.1400000009005375,
-                    "angularVelocity": 3.865132870171629e-8,
-                    "velocityX": 0.19160342420680077,
-                    "velocityY": -0.032078691629102596,
-                    "timestamp": 0.023299009229303495
-                },
-                {
-                    "x": 1.3633925116373848,
-                    "y": 5.557757806179583,
-                    "heading": 3.1400000027300097,
-                    "angularVelocity": 7.852144487332144e-8,
-                    "velocityX": 0.383206925262499,
-                    "velocityY": -0.06415689497258387,
-                    "timestamp": 0.04659801845860699
-                },
-                {
-                    "x": 1.3767850270669844,
-                    "y": 5.555515636454451,
-                    "heading": 3.140000005520194,
-                    "angularVelocity": 1.1975551924808857e-7,
-                    "velocityX": 0.5748105122322142,
-                    "velocityY": -0.09623455242535102,
-                    "timestamp": 0.06989702768791048
-                },
-                {
-                    "x": 1.3946417184903905,
-                    "y": 5.55252610340551,
-                    "heading": 3.1400000093068963,
-                    "angularVelocity": 1.6252630014020525e-7,
-                    "velocityX": 0.7664141958854979,
-                    "velocityY": -0.12831159554996177,
-                    "timestamp": 0.09319603691721398
-                },
-                {
-                    "x": 1.4169625884614865,
-                    "y": 5.548789223260415,
-                    "heading": 3.14000001413076,
-                    "angularVelocity": 2.070415703537556e-7,
-                    "velocityX": 0.9580179891521703,
-                    "velocityY": -0.16038794217918884,
-                    "timestamp": 0.11649504614651748
-                },
-                {
-                    "x": 1.443747639900165,
-                    "y": 5.544305014572816,
-                    "heading": 3.1400000200383236,
-                    "angularVelocity": 2.5355430687602407e-7,
-                    "velocityX": 1.1496219077414547,
-                    "velocityY": -0.1924634924804626,
-                    "timestamp": 0.13979405537582099
-                },
-                {
-                    "x": 1.474996876177077,
-                    "y": 5.539073498761034,
-                    "heading": 3.1400000270834036,
-                    "angularVelocity": 3.0237681109940727e-7,
-                    "velocityX": 1.3412259710000187,
-                    "velocityY": -0.22453812350142868,
-                    "timestamp": 0.1630930646051245
-                },
-                {
-                    "x": 1.5107103012267313,
-                    "y": 5.5330947008269575,
-                    "heading": 3.1400000353289426,
-                    "angularVelocity": 3.539008296544223e-7,
-                    "velocityX": 1.5328302031287921,
-                    "velocityY": -0.25661168143439866,
-                    "timestamp": 0.186392073834428
-                },
-                {
-                    "x": 1.5508879197018797,
-                    "y": 5.526368650337421,
-                    "heading": 3.140000044849547,
-                    "angularVelocity": 4.086269951598913e-7,
-                    "velocityX": 1.7244346349549924,
-                    "velocityY": -0.2886839703505207,
-                    "timestamp": 0.2096910830637315
-                },
-                {
-                    "x": 1.5955297371899198,
-                    "y": 5.5188953827998875,
-                    "heading": 3.1400000557350607,
-                    "angularVelocity": 4.6720929680542457e-7,
-                    "velocityX": 1.9160393065938472,
-                    "velocityY": -0.3207547352763359,
-                    "timestamp": 0.232990092293035
-                },
-                {
-                    "x": 1.6446357605259214,
-                    "y": 5.510674941652546,
-                    "heading": 3.140000068095764,
-                    "angularVelocity": 5.305248328425052e-7,
-                    "velocityX": 2.107644271595818,
-                    "velocityY": -0.3528236358230067,
-                    "timestamp": 0.25628910152233847
-                },
-                {
-                    "x": 1.6982059982629796,
-                    "y": 5.501707381254886,
-                    "heading": 3.1400000820702374,
-                    "angularVelocity": 5.997883043629641e-7,
-                    "velocityX": 2.299249603699023,
-                    "velocityY": -0.38489020324511874,
-                    "timestamp": 0.27958811075164197
-                },
-                {
-                    "x": 1.7562404614129117,
-                    "y": 5.491992771597749,
-                    "heading": 3.1400000978378597,
-                    "angularVelocity": 6.767507519595272e-7,
-                    "velocityX": 2.490855408432506,
-                    "velocityY": -0.41695376664068867,
-                    "timestamp": 0.3028871199809455
-                },
-                {
-                    "x": 1.8187391646840712,
-                    "y": 5.4815312061757675,
-                    "heading": 3.140000115639928,
-                    "angularVelocity": 7.640697380946438e-7,
-                    "velocityX": 2.6824618444526434,
-                    "velocityY": -0.4490133172205749,
-                    "timestamp": 0.326186129210249
-                },
-                {
-                    "x": 1.8857021287172593,
-                    "y": 5.47032281621074,
-                    "heading": 3.140000135818395,
-                    "angularVelocity": 8.660654300433757e-7,
-                    "velocityX": 2.874069166381749,
-                    "velocityY": -0.48106723572405463,
-                    "timestamp": 0.3494851384395525
-                },
-                {
-                    "x": 1.9571293845792233,
-                    "y": 5.458367799245384,
-                    "heading": 3.14000015889542,
-                    "angularVelocity": 9.904723735244152e-7,
-                    "velocityX": 3.0656778217043392,
-                    "velocityY": -0.5131126756388134,
-                    "timestamp": 0.372784147668856
-                },
-                {
-                    "x": 2.033020984323074,
-                    "y": 5.445666486377279,
-                    "heading": 3.140000185766696,
-                    "angularVelocity": 0.000001153322655289093,
-                    "velocityX": 3.2572887111610362,
-                    "velocityY": -0.545143904752752,
-                    "timestamp": 0.3960831568981595
-                },
-                {
-                    "x": 2.113377033060567,
-                    "y": 5.432219546645115,
-                    "heading": 3.1400002183250084,
-                    "angularVelocity": 0.0000013974118596484493,
-                    "velocityX": 3.448904112043669,
-                    "velocityY": -0.5771464185388335,
-                    "timestamp": 0.419382166127463
-                },
-                {
-                    "x": 2.1981978506136755,
-                    "y": 5.41802904044786,
-                    "heading": 3.1400002631776545,
-                    "angularVelocity": 0.0000019250880740696254,
-                    "velocityX": 3.640533239775037,
-                    "velocityY": -0.6090604994243719,
-                    "timestamp": 0.4426811753567665
-                },
-                {
-                    "x": 2.2851188032420633,
-                    "y": 5.403428173451014,
-                    "heading": 3.140000263177585,
-                    "angularVelocity": -2.9753453619857404e-12,
-                    "velocityX": 3.73067162525748,
-                    "velocityY": -0.6266732998482398,
-                    "timestamp": 0.46598018458607
-                },
-                {
-                    "x": 2.372043080680341,
-                    "y": 5.388847120989839,
-                    "heading": 3.140000263177669,
-                    "angularVelocity": 3.609665835664611e-12,
-                    "velocityX": 3.730814327029464,
-                    "velocityY": -0.6258228544259767,
-                    "timestamp": 0.4892791938153735
-                },
-                {
-                    "x": 2.458969517114403,
-                    "y": 5.374278945126162,
-                    "heading": 3.140000263177672,
-                    "angularVelocity": 1.1048975247425585e-13,
-                    "velocityX": 3.7309069917332867,
-                    "velocityY": -0.6252701872544597,
-                    "timestamp": 0.5125782030446769
-                },
-                {
-                    "x": 2.5458971210754204,
-                    "y": 5.359717737461923,
-                    "heading": 3.1400002631776736,
-                    "angularVelocity": 8.62983817390415e-14,
-                    "velocityX": 3.7309571023169674,
-                    "velocityY": -0.6249711101842599,
-                    "timestamp": 0.5358772122739804
-                },
-                {
-                    "x": 2.632825281306951,
-                    "y": 5.345159851008328,
-                    "heading": 3.140000263177678,
-                    "angularVelocity": 1.7012303031669384e-13,
-                    "velocityX": 3.7309809776027962,
-                    "velocityY": -0.6248285628956001,
-                    "timestamp": 0.5591762215032839
-                },
-                {
-                    "x": 2.7197537163641496,
-                    "y": 5.330603605663881,
-                    "heading": 3.1400002631776815,
-                    "angularVelocity": 1.5893105684431692e-13,
-                    "velocityX": 3.7309927731970562,
-                    "velocityY": -0.6247581260336735,
-                    "timestamp": 0.5824752307325874
-                },
-                {
-                    "x": 2.8066823211500065,
-                    "y": 5.316048373923756,
-                    "heading": 3.140000263177685,
-                    "angularVelocity": 1.4661255756791518e-13,
-                    "velocityX": 3.7310000579983225,
-                    "velocityY": -0.6247146218539804,
-                    "timestamp": 0.605774239961891
-                },
-                {
-                    "x": 2.8936112175364808,
-                    "y": 5.301494883804396,
-                    "heading": 3.140000263177688,
-                    "angularVelocity": 1.4712645864153266e-13,
-                    "velocityX": 3.731012573578327,
-                    "velocityY": -0.6246398710039542,
-                    "timestamp": 0.6290732491911945
-                },
-                {
-                    "x": 2.9805406318812673,
-                    "y": 5.286944487835434,
-                    "heading": 3.1400002631776913,
-                    "angularVelocity": 1.2989281986974027e-13,
-                    "velocityX": 3.7310348044942168,
-                    "velocityY": -0.6245070692018166,
-                    "timestamp": 0.652372258420498
-                },
-                {
-                    "x": 3.067470472751592,
-                    "y": 5.272396640371872,
-                    "heading": 3.140000263177696,
-                    "angularVelocity": 2.2497040560944553e-13,
-                    "velocityX": 3.7310531110902496,
-                    "velocityY": -0.6243976866335479,
-                    "timestamp": 0.6756712676498015
-                },
-                {
-                    "x": 3.1544000489958055,
-                    "y": 5.257847211710117,
-                    "heading": 3.1400002631777,
-                    "angularVelocity": 1.6698909622359586e-13,
-                    "velocityX": 3.73104175326474,
-                    "velocityY": -0.6244655520950864,
-                    "timestamp": 0.698970276879105
-                },
-                {
-                    "x": 3.24132763546162,
-                    "y": 5.243285899348815,
-                    "heading": 3.1400002631776998,
-                    "angularVelocity": -2.5660759099238358e-14,
-                    "velocityX": 3.7309563514197523,
-                    "velocityY": -0.6249756038008819,
-                    "timestamp": 0.7222692861084085
-                },
-                {
-                    "x": 3.3282514237632603,
-                    "y": 5.228701930838363,
-                    "heading": 3.1400002631777038,
-                    "angularVelocity": 1.8102189988940347e-13,
-                    "velocityX": 3.730793333150296,
-                    "velocityY": -0.62594801206375,
-                    "timestamp": 0.745568295337712
-                },
-                {
-                    "x": 3.4151697732878015,
-                    "y": 5.214085582937486,
-                    "heading": 3.140000263177707,
-                    "angularVelocity": 1.4033623034653362e-13,
-                    "velocityX": 3.7305598993143207,
-                    "velocityY": -0.6273377445673929,
-                    "timestamp": 0.7688673045670155
-                },
-                {
-                    "x": 3.502081162717683,
-                    "y": 5.199427905968822,
-                    "heading": 3.1400002631777144,
-                    "angularVelocity": 3.3137702069013046e-13,
-                    "velocityX": 3.7302611701022674,
-                    "velocityY": -0.6291115997313443,
-                    "timestamp": 0.792166313796319
-                },
-                {
-                    "x": 3.588983960416736,
-                    "y": 5.18471937597994,
-                    "heading": 3.140000263177713,
-                    "angularVelocity": -6.829626544199834e-14,
-                    "velocityX": 3.729892410608752,
-                    "velocityY": -0.6312942256021684,
-                    "timestamp": 0.8154653230256225
-                },
-                {
-                    "x": 3.675876953607923,
-                    "y": 5.169953034745646,
-                    "heading": 3.140000263177716,
-                    "angularVelocity": 1.3540589961163151e-13,
-                    "velocityX": 3.7294715983848317,
-                    "velocityY": -0.6337755004484098,
-                    "timestamp": 0.838764332254926
-                },
-                {
-                    "x": 3.7627586434088305,
-                    "y": 5.155120331788361,
-                    "heading": 3.1400002631777193,
-                    "angularVelocity": 1.3533863072281271e-13,
-                    "velocityX": 3.728986453707805,
-                    "velocityY": -0.6366237641880743,
-                    "timestamp": 0.8620633414842295
-                },
-                {
-                    "x": 3.8490562562014863,
-                    "y": 5.137199419063008,
-                    "heading": 3.1400002631777224,
-                    "angularVelocity": 1.3559815009211054e-13,
-                    "velocityX": 3.7039177049679806,
-                    "velocityY": -0.7691705921529395,
-                    "timestamp": 0.885362350713533
-                },
-                {
-                    "x": 3.9343637094417163,
-                    "y": 5.1150391964944655,
-                    "heading": 3.1400002631777255,
-                    "angularVelocity": 1.370480919312873e-13,
-                    "velocityX": 3.6614197797262853,
-                    "velocityY": -0.9511229576576579,
-                    "timestamp": 0.9086613599428365
+                    "x": 1.3544641547566925,
+                    "y": 5.559252486403593,
+                    "heading": 5.298729726015063e-12,
+                    "angularVelocity": 2.274228720676412e-10,
+                    "velocityX": 0.1916026983161006,
+                    "velocityY": -0.0320834805032552,
+                    "timestamp": 0.02329901820759936
+                },
+                {
+                    "x": 1.3633924655059346,
+                    "y": 5.557757467272512,
+                    "heading": 1.605830471024488e-11,
+                    "angularVelocity": 4.618037851350714e-10,
+                    "velocityX": 0.38320544967553505,
+                    "velocityY": -0.06416661499484601,
+                    "timestamp": 0.04659803641519872
+                },
+                {
+                    "x": 1.3767849336296958,
+                    "y": 5.555514951621415,
+                    "heading": 3.246020157611451e-11,
+                    "angularVelocity": 7.039737222967939e-10,
+                    "velocityX": 0.574808260349448,
+                    "velocityY": -0.09624936257493934,
+                    "timestamp": 0.06989705462279808
+                },
+                {
+                    "x": 1.3946415606835925,
+                    "y": 5.552524949597437,
+                    "heading": 5.470894313107332e-11,
+                    "angularVelocity": 9.549218317932479e-10,
+                    "velocityX": 0.7664111377908356,
+                    "velocityY": -0.12833167463728912,
+                    "timestamp": 0.09319607283039744
+                },
+                {
+                    "x": 1.416962348431807,
+                    "y": 5.548787472707871,
+                    "heading": 8.303679095756625e-11,
+                    "angularVelocity": 1.2158386690956822e-9,
+                    "velocityX": 0.9580140909513821,
+                    "velocityY": -0.1604134928036672,
+                    "timestamp": 0.1164950910379968
+                },
+                {
+                    "x": 1.4437472988920377,
+                    "y": 5.544302534113244,
+                    "heading": 1.1770979097345793e-10,
+                    "angularVelocity": 1.4881742667086445e-9,
+                    "velocityX": 1.1496171307120806,
+                    "velocityY": -0.19249474611664116,
+                    "timestamp": 0.13979410924559615
+                },
+                {
+                    "x": 1.4749964143943655,
+                    "y": 5.539070149011117,
+                    "heading": 1.59035725627389e-10,
+                    "angularVelocity": 1.7737199674396097e-9,
+                    "velocityX": 1.3412202704804594,
+                    "velocityY": -0.22457534714647368,
+                    "timestamp": 0.1630931274531955
+                },
+                {
+                    "x": 1.5107096976598688,
+                    "y": 5.533090335148591,
+                    "heading": 2.073747689126765e-10,
+                    "angularVelocity": 2.0747244300773254e-9,
+                    "velocityX": 1.532823527038234,
+                    "velocityY": -0.25665518646593166,
+                    "timestamp": 0.18639214566079487
+                },
+                {
+                    "x": 1.5508871519080267,
+                    "y": 5.526363113522473,
+                    "heading": 2.631540997774015e-10,
+                    "angularVelocity": 2.3940635247907603e-9,
+                    "velocityX": 1.7244269217769022,
+                    "velocityY": -0.28873412459698383,
+                    "timestamp": 0.20969116386839423
+                },
+                {
+                    "x": 1.595528781007412,
+                    "y": 5.518888509361551,
+                    "heading": 3.268885139183836e-10,
+                    "angularVelocity": 2.7354977853334202e-9,
+                    "velocityX": 1.9160304825557395,
+                    "velocityY": -0.3208119799018042,
+                    "timestamp": 0.2329901820759936
+                },
+                {
+                    "x": 1.6446345896939198,
+                    "y": 5.5106665535489805,
+                    "heading": 3.9921044507968937e-10,
+                    "angularVelocity": 3.104076267437521e-9,
+                    "velocityX": 2.1076342466004383,
+                    "velocityY": -0.35288850969337676,
+                    "timestamp": 0.2562892002835929
+                },
+                {
+                    "x": 1.698204583899122,
+                    "y": 5.501697284762255,
+                    "heading": 4.809154717853132e-10,
+                    "angularVelocity": 3.5068011876967e-9,
+                    "velocityX": 2.299238265229739,
+                    "velocityY": -0.38496337943692166,
+                    "timestamp": 0.2795882184911923
+                },
+                {
+                    "x": 1.7562387712682006,
+                    "y": 5.491980752848314,
+                    "heading": 5.730347406124241e-10,
+                    "angularVelocity": 3.953783188523521e-9,
+                    "velocityX": 2.4908426119922917,
+                    "velocityY": -0.41703610973659105,
+                    "timestamp": 0.3028872366987917
+                },
+                {
+                    "x": 1.8187371620272614,
+                    "y": 5.48151702447463,
+                    "heading": 6.769576861119728e-10,
+                    "angularVelocity": 4.460400083403711e-9,
+                    "velocityX": 2.6824473976619876,
+                    "velocityY": -0.449105978650929,
+                    "timestamp": 0.3261862549063911
+                },
+                {
+                    "x": 1.8856997705540457,
+                    "y": 5.4703061933617745,
+                    "heading": 7.946577963057737e-10,
+                    "angularVelocity": 5.051719599980361e-9,
+                    "velocityX": 2.8740528004281045,
+                    "velocityY": -0.4811718250535651,
+                    "timestamp": 0.34948527311399047
+                },
+                {
+                    "x": 1.957126618545275,
+                    "y": 5.4583484009138274,
+                    "heading": 9.291581669995643e-10,
+                    "angularVelocity": 5.772791178735968e-9,
+                    "velocityX": 3.0656591344232145,
+                    "velocityY": -0.5132316023559669,
+                    "timestamp": 0.37278429132158986
+                },
+                {
+                    "x": 2.0330177424946343,
+                    "y": 5.445643884917638,
+                    "heading": 1.0856695619650843e-9,
+                    "angularVelocity": 6.717510100758224e-9,
+                    "velocityX": 3.257267034736916,
+                    "velocityY": -0.5452811737842305,
+                    "timestamp": 0.39608330952918924
+                },
+                {
+                    "x": 2.1133732165489003,
+                    "y": 5.432193128386492,
+                    "heading": 1.2752962310893515e-9,
+                    "angularVelocity": 8.13882633127911e-9,
+                    "velocityX": 3.44887811744974,
+                    "velocityY": -0.5773100141552516,
+                    "timestamp": 0.41938232773678863
+                },
+                {
+                    "x": 2.1981932628587333,
+                    "y": 5.417997633484574,
+                    "heading": 1.5377676317044546e-9,
+                    "angularVelocity": 1.1265341261395982e-8,
+                    "velocityX": 3.640498734927221,
+                    "velocityY": -0.6092743812441802,
+                    "timestamp": 0.442681345944388
+                },
+                {
+                    "x": 2.285115669591906,
+                    "y": 5.403405223128801,
+                    "heading": 1.537767230961928e-9,
+                    "angularVelocity": -1.719997404273206e-14,
+                    "velocityX": 3.730732598201487,
+                    "velocityY": -0.6263100970908951,
+                    "timestamp": 0.4659803641519874
+                },
+                {
+                    "x": 2.3720410537379144,
+                    "y": 5.388830564421114,
+                    "heading": 1.5377672475870007e-9,
+                    "angularVelocity": 7.135525631450618e-16,
+                    "velocityX": 3.7308603895453194,
+                    "velocityY": -0.6255481916751834,
+                    "timestamp": 0.4892793823595868
+                },
+                {
+                    "x": 2.45896811239221,
+                    "y": 5.374265896231001,
+                    "heading": 1.5377672778413637e-9,
+                    "angularVelocity": 1.298525326828493e-15,
+                    "velocityX": 3.730932259882311,
+                    "velocityY": -0.625119396028046,
+                    "timestamp": 0.5125784005671862
+                },
+                {
+                    "x": 2.5458958805121723,
+                    "y": 5.359705463035692,
+                    "heading": 1.5377672869369513e-9,
+                    "angularVelocity": 3.903849318113422e-16,
+                    "velocityX": 3.730962710335439,
+                    "velocityY": -0.6249376289364498,
+                    "timestamp": 0.5358774187747856
+                },
+                {
+                    "x": 2.632823878731898,
+                    "y": 5.345146403658019,
+                    "heading": 1.5377672987189865e-9,
+                    "angularVelocity": 5.056879723770718e-16,
+                    "velocityX": 3.7309725862775656,
+                    "velocityY": -0.6248786643237555,
+                    "timestamp": 0.559176436982385
+                },
+                {
+                    "x": 2.7197519358547084,
+                    "y": 5.330587695987482,
+                    "heading": 1.5377673268477785e-9,
+                    "angularVelocity": 1.2072952571647457e-15,
+                    "velocityX": 3.7309751144130874,
+                    "velocityY": -0.6248635689629851,
+                    "timestamp": 0.5824754551899843
+                },
+                {
+                    "x": 2.806680023647356,
+                    "y": 5.316029171436071,
+                    "heading": 1.5377673567071691e-9,
+                    "angularVelocity": 1.2815730065587283e-15,
+                    "velocityX": 3.7309764307702067,
+                    "velocityY": -0.6248557094441328,
+                    "timestamp": 0.6057744733975837
+                },
+                {
+                    "x": 2.8936082747189524,
+                    "y": 5.301471621825431,
+                    "heading": 1.5377673928791505e-9,
+                    "angularVelocity": 1.5525110632783364e-15,
+                    "velocityX": 3.7309834387453797,
+                    "velocityY": -0.6248138647313558,
+                    "timestamp": 0.6290734916051831
+                },
+                {
+                    "x": 2.980536964716405,
+                    "y": 5.286916693414742,
+                    "heading": 1.5377674058328616e-9,
+                    "angularVelocity": 5.559766105183297e-16,
+                    "velocityX": 3.7310022775585137,
+                    "velocityY": -0.6247013621403136,
+                    "timestamp": 0.6523725098127825
+                },
+                {
+                    "x": 3.0674663067511823,
+                    "y": 5.272365659809418,
+                    "heading": 1.5377674408459054e-9,
+                    "angularVelocity": 1.5027691524839091e-15,
+                    "velocityX": 3.73103026317236,
+                    "velocityY": -0.6245341960676912,
+                    "timestamp": 0.6756715280203819
+                },
+                {
+                    "x": 3.154396100803892,
+                    "y": 5.257817326905748,
+                    "heading": 1.5377674636831172e-9,
+                    "angularVelocity": 9.801792492345066e-16,
+                    "velocityX": 3.7310496639016963,
+                    "velocityY": -0.624418281237505,
+                    "timestamp": 0.6989705462279813
+                },
+                {
+                    "x": 3.2413249978571956,
+                    "y": 5.243263635140824,
+                    "heading": 1.5377674638426843e-9,
+                    "angularVelocity": 6.848580307945622e-18,
+                    "velocityX": 3.731011164452385,
+                    "velocityY": -0.6246482849651879,
+                    "timestamp": 0.7222695644355807
+                },
+                {
+                    "x": 3.3282506523054423,
+                    "y": 5.228690588454777,
+                    "heading": 1.5377674749707248e-9,
+                    "angularVelocity": 4.776183371381179e-16,
+                    "velocityX": 3.7308719909897503,
+                    "velocityY": -0.6254790032748808,
+                    "timestamp": 0.74556858264318
+                },
+                {
+                    "x": 3.415171009638962,
+                    "y": 5.214085980519485,
+                    "heading": 1.5377674915535578e-9,
+                    "angularVelocity": 7.117396517888937e-16,
+                    "velocityX": 3.7306446374302076,
+                    "velocityY": -0.626833620407003,
+                    "timestamp": 0.7688676008507794
+                },
+                {
+                    "x": 3.5020842171218036,
+                    "y": 5.1994388833661285,
+                    "heading": 1.53776751807092e-9,
+                    "angularVelocity": 1.13813226337154e-15,
+                    "velocityX": 3.730337763955042,
+                    "velocityY": -0.6286572688425895,
+                    "timestamp": 0.7921666190583788
+                },
+                {
+                    "x": 3.5889883305639807,
+                    "y": 5.184737925794629,
+                    "heading": 1.5377675551515876e-9,
+                    "angularVelocity": 1.5915121086693097e-15,
+                    "velocityX": 3.729947445333141,
+                    "velocityY": -0.6309689721834142,
+                    "timestamp": 0.8154656372659782
+                },
+                {
+                    "x": 3.675881697311449,
+                    "y": 5.1699735802440445,
+                    "heading": 1.5377675887419511e-9,
+                    "angularVelocity": 1.441707334130238e-15,
+                    "velocityX": 3.7294861943638535,
+                    "velocityY": -0.6336896009521162,
+                    "timestamp": 0.8387646554735776
+                },
+                {
+                    "x": 3.7627625314341118,
+                    "y": 5.155135664387593,
+                    "heading": 1.5377676077743658e-9,
+                    "angularVelocity": 8.168762038570203e-16,
+                    "velocityX": 3.728948290804007,
+                    "velocityY": -0.636847257870781,
+                    "timestamp": 0.862063673681177
+                },
+                {
+                    "x": 3.8490591061764308,
+                    "y": 5.137209586747657,
+                    "heading": 1.5377676390897132e-9,
+                    "angularVelocity": 1.344063051824819e-15,
+                    "velocityX": 3.703871724266177,
+                    "velocityY": -0.7693919752476525,
+                    "timestamp": 0.8853626918887764
+                },
+                {
+                    "x": 3.934365266987087,
+                    "y": 5.115044254457407,
+                    "heading": 1.5377676702466937e-9,
+                    "angularVelocity": 1.337265902944942e-15,
+                    "velocityX": 3.661362897377225,
+                    "velocityY": -0.9513419017440721,
+                    "timestamp": 0.9086617100963758
                 },
                 {
                     "x": 4.018431663513184,
                     "y": 5.088562488555908,
-                    "heading": 3.140000263177729,
-                    "angularVelocity": 1.413301961445022e-13,
-                    "velocityX": 3.6082201283346866,
-                    "velocityY": -1.1363877183834126,
-                    "timestamp": 0.93196036917214
-                },
-                {
-                    "x": 4.095482183944048,
-                    "y": 5.0601896166081985,
-                    "heading": 3.140000263177732,
-                    "angularVelocity": 1.478757127486931e-13,
-                    "velocityX": 3.549906540385796,
-                    "velocityY": -1.3072078310833697,
-                    "timestamp": 0.9536653125429956
-                },
-                {
-                    "x": 4.171099571738343,
-                    "y": 5.028192533992809,
-                    "heading": 3.1400002631777353,
-                    "angularVelocity": 1.4523452590479803e-13,
-                    "velocityX": 3.483878603243662,
-                    "velocityY": -1.4741841095171686,
-                    "timestamp": 0.9753702559138512
-                },
-                {
-                    "x": 4.2451557679063034,
-                    "y": 4.992731534520922,
-                    "heading": 3.1400002631777384,
-                    "angularVelocity": 1.4237189069796376e-13,
-                    "velocityX": 3.4119506742159467,
-                    "velocityY": -1.6337752587458831,
-                    "timestamp": 0.9970751992847068
-                },
-                {
-                    "x": 4.31781550635648,
-                    "y": 4.954490651976443,
-                    "heading": 3.1400002631777415,
-                    "angularVelocity": 1.4182458601020346e-13,
-                    "velocityX": 3.347612440573966,
-                    "velocityY": -1.761851293089538,
-                    "timestamp": 1.0187801426555625
-                },
-                {
-                    "x": 4.390182790341615,
-                    "y": 4.91569917224677,
-                    "heading": 3.140000263177745,
-                    "angularVelocity": 1.421722636666585e-13,
-                    "velocityX": 3.3341383457540337,
-                    "velocityY": -1.787218656449447,
-                    "timestamp": 1.040485086026418
-                },
-                {
-                    "x": 4.462533664586848,
-                    "y": 4.87687709490227,
-                    "heading": 3.1400002631777486,
-                    "angularVelocity": 1.635268036057102e-13,
-                    "velocityX": 3.333382308772387,
-                    "velocityY": -1.7886283636415046,
-                    "timestamp": 1.0621900293972737
-                },
-                {
-                    "x": 4.534860115355159,
-                    "y": 4.838009534998386,
-                    "heading": 3.140000263177751,
-                    "angularVelocity": 1.1808318022164126e-13,
-                    "velocityX": 3.3322570592611274,
-                    "velocityY": -1.7907238567558525,
-                    "timestamp": 1.0838949727681293
-                },
-                {
-                    "x": 4.607177418550424,
-                    "y": 4.799124957707118,
-                    "heading": 3.140000263177754,
-                    "angularVelocity": 1.4385253669729274e-13,
-                    "velocityX": 3.3318356081243334,
-                    "velocityY": -1.7915078895502723,
-                    "timestamp": 1.105599916138985
-                },
-                {
-                    "x": 4.679489672949611,
-                    "y": 4.7602309921876325,
-                    "heading": 3.1400002631777575,
-                    "angularVelocity": 1.4120580598782794e-13,
-                    "velocityX": 3.331602997698768,
-                    "velocityY": -1.7919404282640008,
-                    "timestamp": 1.1273048595098405
-                },
-                {
-                    "x": 4.751800053049455,
-                    "y": 4.721333542151952,
-                    "heading": 3.1400002631777606,
-                    "angularVelocity": 1.4195129118241079e-13,
-                    "velocityX": 3.3315166441264745,
-                    "velocityY": -1.7921009684797435,
-                    "timestamp": 1.149009802880696
-                },
-                {
-                    "x": 4.824110260922509,
-                    "y": 4.682435771933003,
-                    "heading": 3.1400002631777637,
-                    "angularVelocity": 1.420416383867913e-13,
-                    "velocityX": 3.3315087092142113,
-                    "velocityY": -1.7921157201114153,
-                    "timestamp": 1.1707147462515517
-                },
-                {
-                    "x": 4.896420645274053,
-                    "y": 4.643538329777489,
-                    "heading": 3.140000263177767,
-                    "angularVelocity": 1.4171119036284518e-13,
-                    "velocityX": 3.3315168400108037,
-                    "velocityY": -1.7921006054248252,
-                    "timestamp": 1.1924196896224073
-                },
-                {
-                    "x": 4.96873125432361,
-                    "y": 4.6046413053457185,
-                    "heading": 3.14000026317777,
-                    "angularVelocity": 1.4137748376763704e-13,
-                    "velocityX": 3.3315271924000274,
-                    "velocityY": -1.792081359866257,
-                    "timestamp": 1.214124632993263
-                },
-                {
-                    "x": 5.04104400801269,
-                    "y": 4.56574826813338,
-                    "heading": 3.1400002631777735,
-                    "angularVelocity": 1.421351986736705e-13,
-                    "velocityX": 3.331626001206414,
-                    "velocityY": -1.7918976588811946,
-                    "timestamp": 1.2358295763641185
-                },
-                {
-                    "x": 5.11336222581346,
-                    "y": 4.5268653919090065,
-                    "heading": 3.1400002631777766,
-                    "angularVelocity": 1.4239328375117255e-13,
-                    "velocityX": 3.3318777462404188,
-                    "velocityY": -1.7914295172313686,
-                    "timestamp": 1.257534519734974
-                },
-                {
-                    "x": 5.185674775789392,
-                    "y": 4.487971975945349,
-                    "heading": 3.1400002631777797,
-                    "angularVelocity": 1.4189340221530015e-13,
-                    "velocityX": 3.331616615642773,
-                    "velocityY": -1.7919151088820495,
-                    "timestamp": 1.2792394631058297
-                },
-                {
-                    "x": 5.25798679998857,
-                    "y": 4.4490775824474165,
-                    "heading": 3.140000263177783,
-                    "angularVelocity": 1.4182847689207558e-13,
-                    "velocityX": 3.331592391818018,
-                    "velocityY": -1.791960146283848,
-                    "timestamp": 1.3009444064766853
-                },
-                {
-                    "x": 5.330301009987797,
-                    "y": 4.410187253034054,
-                    "heading": 3.1400002631777864,
-                    "angularVelocity": 1.4221031722558925e-13,
-                    "velocityX": 3.331693096990619,
-                    "velocityY": -1.7917729039338035,
-                    "timestamp": 1.322649349847541
-                },
-                {
-                    "x": 5.4026181052176305,
-                    "y": 4.3713022890025295,
-                    "heading": 3.1400002631777895,
-                    "angularVelocity": 1.4161705945188678e-13,
-                    "velocityX": 3.3318260266449937,
-                    "velocityY": -1.791525707625898,
-                    "timestamp": 1.3443542932183965
-                },
-                {
-                    "x": 5.474936056465776,
-                    "y": 4.332418917012381,
-                    "heading": 3.1400002631777926,
-                    "angularVelocity": 1.4174219306640175e-13,
-                    "velocityX": 3.331865465508907,
-                    "velocityY": -1.7914523583735324,
-                    "timestamp": 1.366059236589252
-                },
-                {
-                    "x": 5.54725002868687,
-                    "y": 4.293528145471586,
-                    "heading": 3.1400002631777957,
-                    "angularVelocity": 1.4114123611773667e-13,
-                    "velocityX": 3.331682141968083,
-                    "velocityY": -1.7917932738304505,
-                    "timestamp": 1.3877641799601077
-                },
-                {
-                    "x": 5.619560804995751,
-                    "y": 4.2546314319979865,
-                    "heading": 3.1400002631777992,
-                    "angularVelocity": 1.438802546332647e-13,
-                    "velocityX": 3.331534898449109,
-                    "velocityY": -1.7920670332572342,
-                    "timestamp": 1.4094691233309633
-                },
-                {
-                    "x": 5.691871264235437,
-                    "y": 4.215734129079228,
-                    "heading": 3.1400002631778023,
-                    "angularVelocity": 1.4227513855970737e-13,
-                    "velocityX": 3.331520290292617,
-                    "velocityY": -1.7920941904433432,
-                    "timestamp": 1.431174066701819
-                },
-                {
-                    "x": 5.7641829192535345,
-                    "y": 4.1768390492074925,
-                    "heading": 3.1400002631778055,
-                    "angularVelocity": 1.4198857424426595e-13,
-                    "velocityX": 3.3315753827383325,
-                    "velocityY": -1.79199176921174,
-                    "timestamp": 1.4528790100726745
-                },
-                {
-                    "x": 5.836496897649906,
-                    "y": 4.13794828914952,
-                    "heading": 3.1400002631778086,
-                    "angularVelocity": 1.420236059365936e-13,
-                    "velocityX": 3.331682426477163,
-                    "velocityY": -1.7917927447907471,
-                    "timestamp": 1.47458395344353
-                },
-                {
-                    "x": 5.90881236073979,
-                    "y": 4.099060289876852,
-                    "heading": 3.1400002631778117,
-                    "angularVelocity": 1.4199973949506742e-13,
-                    "velocityX": 3.3317508299509484,
-                    "velocityY": -1.7916655486362405,
-                    "timestamp": 1.4962888968143857
-                },
-                {
-                    "x": 5.981129099244504,
-                    "y": 4.06017466243704,
-                    "heading": 3.140000263177815,
-                    "angularVelocity": 1.4813660843765197e-13,
-                    "velocityX": 3.3318095914418606,
-                    "velocityY": -1.7915562724764487,
-                    "timestamp": 1.5179938401852413
-                },
-                {
-                    "x": 6.053446077627243,
-                    "y": 4.021289481108035,
-                    "heading": 3.140000263177829,
-                    "angularVelocity": 6.425453029046258e-13,
-                    "velocityX": 3.331820643210356,
-                    "velocityY": -1.7915357190582988,
-                    "timestamp": 1.539698783556097
-                },
-                {
-                    "x": 6.125761091363011,
-                    "y": 3.9824006462217088,
-                    "heading": 3.140000263177869,
-                    "angularVelocity": 1.8383501574993634e-12,
-                    "velocityX": 3.331730127102014,
-                    "velocityY": -1.7917040474119204,
-                    "timestamp": 1.5614037269269525
-                },
-                {
-                    "x": 6.198074029937981,
-                    "y": 3.9435079527230394,
-                    "heading": 3.140000263178238,
-                    "angularVelocity": 1.7022488728392152e-11,
-                    "velocityX": 3.331634519350639,
-                    "velocityY": -1.79188182315355,
-                    "timestamp": 1.583108670297808
-                },
-                {
-                    "x": 6.270380104650612,
-                    "y": 3.9046024999630506,
-                    "heading": 3.140000263177847,
-                    "angularVelocity": -1.8008440986287348e-11,
-                    "velocityX": 3.33131828436938,
-                    "velocityY": -1.7924696736324577,
-                    "timestamp": 1.6048136136686637
-                },
-                {
-                    "x": 6.342678298341779,
-                    "y": 3.8656824038223503,
-                    "heading": 3.1400002631782598,
-                    "angularVelocity": 1.900200591260141e-11,
-                    "velocityX": 3.3309551863724884,
-                    "velocityY": -1.7931443301131518,
-                    "timestamp": 1.6265185570395193
-                },
-                {
-                    "x": 6.414967377910046,
-                    "y": 3.8267453820171355,
-                    "heading": 3.1400002631778587,
-                    "angularVelocity": -1.8477715999059415e-11,
-                    "velocityX": 3.3305352763678626,
-                    "velocityY": -1.7939241370065324,
-                    "timestamp": 1.648223500410375
-                },
-                {
-                    "x": 6.487241234316261,
-                    "y": 3.787780110681628,
-                    "heading": 3.1400002631778627,
-                    "angularVelocity": 1.954226890690534e-13,
-                    "velocityX": 3.3298339079393355,
-                    "velocityY": -1.795225662182324,
-                    "timestamp": 1.6699284437812305
-                },
-                {
-                    "x": 6.559496489740858,
-                    "y": 3.7487803574447534,
-                    "heading": 3.1400002631778654,
-                    "angularVelocity": 1.160046399416623e-13,
-                    "velocityX": 3.3289769150756516,
-                    "velocityY": -1.79681432798537,
-                    "timestamp": 1.691633387152086
-                },
-                {
-                    "x": 6.630585131325424,
-                    "y": 3.7076925524537225,
-                    "heading": 3.140000263177869,
-                    "angularVelocity": 1.5938094021424355e-13,
-                    "velocityX": 3.275228152865242,
-                    "velocityY": -1.8930159958948245,
-                    "timestamp": 1.7133383305229417
-                },
-                {
-                    "x": 6.699761336489547,
-                    "y": 3.6634607167510884,
-                    "heading": 3.140000263177872,
-                    "angularVelocity": 1.431049344650049e-13,
-                    "velocityX": 3.1871175142993193,
-                    "velocityY": -2.037869205501672,
-                    "timestamp": 1.7350432738937973
-                },
-                {
-                    "x": 6.76677589557052,
-                    "y": 3.6160175775021286,
-                    "heading": 3.140000263178055,
-                    "angularVelocity": 8.443311888840044e-12,
-                    "velocityX": 3.087525175069758,
-                    "velocityY": -2.1858218396761058,
-                    "timestamp": 1.756748217264653
-                },
-                {
-                    "x": 6.8314564099651545,
-                    "y": 3.5654385941456423,
-                    "heading": 3.140000263178058,
-                    "angularVelocity": 1.5118918552164865e-13,
-                    "velocityX": 2.9799900091650118,
-                    "velocityY": -2.3302978723451035,
-                    "timestamp": 1.7784531606355085
+                    "heading": 1.5377676866838871e-9,
+                    "angularVelocity": 7.054887729794734e-16,
+                    "velocityX": 3.608151887646215,
+                    "velocityY": -1.1366043695780652,
+                    "timestamp": 0.9319607283039751
+                },
+                {
+                    "x": 4.095480434320656,
+                    "y": 5.06018496604887,
+                    "heading": 1.5377676994335126e-9,
+                    "angularVelocity": 5.874067965982521e-16,
+                    "velocityX": 3.549827430213289,
+                    "velocityY": -1.3074226459526814,
+                    "timestamp": 0.9536656625077448
+                },
+                {
+                    "x": 4.171095847536846,
+                    "y": 5.028183306370306,
+                    "heading": 1.5377677188417825e-9,
+                    "angularVelocity": 8.941870233905506e-16,
+                    "velocityX": 3.4837891009642465,
+                    "velocityY": -1.4743956087592989,
+                    "timestamp": 0.9753705967115144
+                },
+                {
+                    "x": 4.24514985906727,
+                    "y": 4.992717825181937,
+                    "heading": 1.5377677281143787e-9,
+                    "angularVelocity": 4.2721152582009774e-16,
+                    "velocityX": 3.4118514636002706,
+                    "velocityY": -1.6339824325334937,
+                    "timestamp": 0.997075530915284
+                },
+                {
+                    "x": 4.317807248978416,
+                    "y": 4.95447255507026,
+                    "heading": 1.5377677290979519e-9,
+                    "angularVelocity": 4.531574211416763e-17,
+                    "velocityX": 3.3475056514378028,
+                    "velocityY": -1.7620541832797834,
+                    "timestamp": 1.0187804651190535
+                },
+                {
+                    "x": 4.3901722659206355,
+                    "y": 4.9156769197600605,
+                    "heading": 1.5377677449553673e-9,
+                    "angularVelocity": 7.305902735413318e-16,
+                    "velocityX": 3.334035305654692,
+                    "velocityY": -1.7874108691538961,
+                    "timestamp": 1.0404853993228231
+                },
+                {
+                    "x": 4.4625185516483485,
+                    "y": 4.876846365558822,
+                    "heading": 1.5377677746336623e-9,
+                    "angularVelocity": 1.3673523815751288e-15,
+                    "velocityX": 3.333172312277143,
+                    "velocityY": -1.7890196688239732,
+                    "timestamp": 1.0621903335265928
+                },
+                {
+                    "x": 4.534844691324942,
+                    "y": 4.837978300028894,
+                    "heading": 1.5377677779199108e-9,
+                    "angularVelocity": 1.5140568225814148e-16,
+                    "velocityX": 3.3322441338764652,
+                    "velocityY": -1.7907479085103777,
+                    "timestamp": 1.0838952677303624
+                },
+                {
+                    "x": 4.6071619879089205,
+                    "y": 4.799093783673277,
+                    "heading": 1.537767801979053e-9,
+                    "angularVelocity": 1.1084641137098069e-15,
+                    "velocityX": 3.331836710728115,
+                    "velocityY": -1.79150583873765,
+                    "timestamp": 1.105600201934132
+                },
+                {
+                    "x": 4.6794740379059565,
+                    "y": 4.7601995113399385,
+                    "heading": 1.5377678257958516e-9,
+                    "angularVelocity": 1.0972987247710634e-15,
+                    "velocityX": 3.331594987489656,
+                    "velocityY": -1.791955320763837,
+                    "timestamp": 1.1273051361379016
+                },
+                {
+                    "x": 4.751784687579244,
+                    "y": 4.721302635647572,
+                    "heading": 1.5377678370559733e-9,
+                    "angularVelocity": 5.187816897475249e-16,
+                    "velocityX": 3.331530471108496,
+                    "velocityY": -1.7920752639579989,
+                    "timestamp": 1.1490100703416712
+                },
+                {
+                    "x": 4.824095678023389,
+                    "y": 4.682406393453542,
+                    "heading": 1.5377678370330874e-9,
+                    "angularVelocity": -1.0543217693671843e-18,
+                    "velocityX": 3.3315461712634193,
+                    "velocityY": -1.792046077125237,
+                    "timestamp": 1.1707150045454409
+                },
+                {
+                    "x": 4.896406945309153,
+                    "y": 4.643510665929771,
+                    "heading": 1.5377678640109226e-9,
+                    "angularVelocity": 1.242935520614447e-15,
+                    "velocityX": 3.331558926041247,
+                    "velocityY": -1.7920223649940337,
+                    "timestamp": 1.1924199387492105
+                },
+                {
+                    "x": 4.968718824670649,
+                    "y": 4.604616076350799,
+                    "heading": 1.5377678830932924e-9,
+                    "angularVelocity": 8.791718726461835e-16,
+                    "velocityX": 3.33158712588479,
+                    "velocityY": -1.7919699370612494,
+                    "timestamp": 1.21412487295298
+                },
+                {
+                    "x": 5.041033201559798,
+                    "y": 4.565726130481367,
+                    "heading": 1.5377679126898002e-9,
+                    "angularVelocity": 1.3635842497565112e-15,
+                    "velocityX": 3.331702193162082,
+                    "velocityY": -1.7917559898752364,
+                    "timestamp": 1.2358298071567497
+                },
+                {
+                    "x": 5.113354228854967,
+                    "y": 4.526848553325412,
+                    "heading": 1.5377679256265375e-9,
+                    "angularVelocity": 5.960275144059911e-16,
+                    "velocityX": 3.332008593816099,
+                    "velocityY": -1.7911861326534588,
+                    "timestamp": 1.2575347413605193
+                },
+                {
+                    "x": 5.1856663573422805,
+                    "y": 4.487954426928371,
+                    "heading": 1.537767949104212e-9,
+                    "angularVelocity": 1.0816744331209405e-15,
+                    "velocityX": 3.3315986037289687,
+                    "velocityY": -1.791948597121158,
+                    "timestamp": 1.279239675564289
+                },
+                {
+                    "x": 5.257977857015597,
+                    "y": 4.449059131458437,
+                    "heading": 1.5377679798803482e-9,
+                    "angularVelocity": 1.4179326501228622e-15,
+                    "velocityX": 3.3315696327123336,
+                    "velocityY": -1.7920024592017922,
+                    "timestamp": 1.3009446097680586
+                },
+                {
+                    "x": 5.330290964048228,
+                    "y": 4.410166824431303,
+                    "heading": 1.5377679920779735e-9,
+                    "angularVelocity": 5.619748235100741e-16,
+                    "velocityX": 3.3316436877316113,
+                    "velocityY": -1.7918647742503244,
+                    "timestamp": 1.3226495439718282
+                },
+                {
+                    "x": 5.402605809232356,
+                    "y": 4.371277749351831,
+                    "heading": 1.5377680157791104e-9,
+                    "angularVelocity": 1.0919698963856968e-15,
+                    "velocityX": 3.3317237686705528,
+                    "velocityY": -1.7917158704270917,
+                    "timestamp": 1.3443544781755978
+                },
+                {
+                    "x": 5.474921974691154,
+                    "y": 4.332391129443428,
+                    "heading": 1.5377680240202577e-9,
+                    "angularVelocity": 3.7969007641948726e-16,
+                    "velocityX": 3.331784596990179,
+                    "velocityY": -1.7916027546232325,
+                    "timestamp": 1.3660594123793675
+                },
+                {
+                    "x": 5.54723622900274,
+                    "y": 4.29350095564713,
+                    "heading": 1.537768038636468e-9,
+                    "angularVelocity": 6.734049902652557e-16,
+                    "velocityX": 3.331696545710963,
+                    "velocityY": -1.7917664910290456,
+                    "timestamp": 1.387764346583137
+                },
+                {
+                    "x": 5.619548385303401,
+                    "y": 4.254606880959325,
+                    "heading": 1.5377680494372407e-9,
+                    "angularVelocity": 4.976183803494664e-16,
+                    "velocityX": 3.331599885158129,
+                    "velocityY": -1.7919462147497014,
+                    "timestamp": 1.4094692807869067
+                },
+                {
+                    "x": 5.6918607364808045,
+                    "y": 4.215713168590034,
+                    "heading": 1.5377680578875737e-9,
+                    "angularVelocity": 3.893277832205449e-16,
+                    "velocityX": 3.3316088636117085,
+                    "velocityY": -1.7919295218396705,
+                    "timestamp": 1.4311742149906763
+                },
+                {
+                    "x": 5.764173499508255,
+                    "y": 4.176820221955942,
+                    "heading": 1.5377680813491443e-9,
+                    "angularVelocity": 1.0809325075471762e-15,
+                    "velocityX": 3.331627838562525,
+                    "velocityY": -1.7918942425240987,
+                    "timestamp": 1.452879149194446
+                },
+                {
+                    "x": 5.8364876254997204,
+                    "y": 4.137929809554028,
+                    "heading": 1.5377680862134134e-9,
+                    "angularVelocity": 2.241089873857745e-16,
+                    "velocityX": 3.331690633686676,
+                    "velocityY": -1.7917774841780256,
+                    "timestamp": 1.4745840833982156
+                },
+                {
+                    "x": 5.90880309835116,
+                    "y": 4.099041901645272,
+                    "heading": 1.537768099475084e-9,
+                    "angularVelocity": 6.109980238433844e-16,
+                    "velocityX": 3.3317526868576484,
+                    "velocityY": -1.7916620959822775,
+                    "timestamp": 1.4962890176019852
+                },
+                {
+                    "x": 5.981120307623631,
+                    "y": 4.060157222932383,
+                    "heading": 1.5377681005691863e-9,
+                    "angularVelocity": 5.040806501517701e-17,
+                    "velocityX": 3.331832688069564,
+                    "velocityY": -1.7915133189414525,
+                    "timestamp": 1.5179939518057548
+                },
+                {
+                    "x": 6.053439067813782,
+                    "y": 4.0212754287411325,
+                    "heading": 1.5377681034134081e-9,
+                    "angularVelocity": 1.3104042715425496e-16,
+                    "velocityX": 3.3319041426820193,
+                    "velocityY": -1.7913804218969325,
+                    "timestamp": 1.5396988860095244
+                },
+                {
+                    "x": 6.1257574594958575,
+                    "y": 3.9823929491454755,
+                    "heading": 1.5377681141371771e-9,
+                    "angularVelocity": 4.940705905043031e-16,
+                    "velocityX": 3.3318871646008015,
+                    "velocityY": -1.7914120001783713,
+                    "timestamp": 1.561403820213294
+                },
+                {
+                    "x": 6.198071250397711,
+                    "y": 3.943501913620472,
+                    "heading": 1.5377681379511677e-9,
+                    "angularVelocity": 1.09716939803524e-15,
+                    "velocityX": 3.3316751952742036,
+                    "velocityY": -1.7918061930096516,
+                    "timestamp": 1.5831087544170637
+                },
+                {
+                    "x": 6.270375940604977,
+                    "y": 3.9045939610161593,
+                    "heading": 1.5377681515073034e-9,
+                    "angularVelocity": 6.245647694184294e-16,
+                    "velocityX": 3.331255903765101,
+                    "velocityY": -1.7925856046957434,
+                    "timestamp": 1.6048136886208333
+                },
+                {
+                    "x": 6.342673755736347,
+                    "y": 3.865673234790299,
+                    "heading": 1.5377699302540744e-9,
+                    "angularVelocity": 8.19513085832153e-14,
+                    "velocityX": 3.3309391520146527,
+                    "velocityY": -1.7931741170331514,
+                    "timestamp": 1.626518622824603
+                },
+                {
+                    "x": 6.414965746925861,
+                    "y": 3.826741692219575,
+                    "heading": 1.5377690472977265e-9,
+                    "angularVelocity": -4.068002902168065e-14,
+                    "velocityX": 3.3306708286121007,
+                    "velocityY": -1.7936724527872456,
+                    "timestamp": 1.6482235570283725
+                },
+                {
+                    "x": 6.487241720022193,
+                    "y": 3.7877804203169507,
+                    "heading": 1.5377684535437603e-9,
+                    "angularVelocity": -2.7355713192172554e-14,
+                    "velocityX": 3.3299328354443367,
+                    "velocityY": -1.7950421566324242,
+                    "timestamp": 1.6699284912321422
+                },
+                {
+                    "x": 6.559496923985796,
+                    "y": 3.748780644748349,
+                    "heading": 1.5377682143039968e-9,
+                    "angularVelocity": -1.1022367481483692e-14,
+                    "velocityX": 3.3289759501347236,
+                    "velocityY": -1.796816115748624,
+                    "timestamp": 1.6916334254359118
+                },
+                {
+                    "x": 6.630585509558441,
+                    "y": 3.7076928121475503,
+                    "heading": 1.5377682448018014e-9,
+                    "angularVelocity": 1.4051092264620289e-15,
+                    "velocityX": 3.2752269555510436,
+                    "velocityY": -1.8930180674612387,
+                    "timestamp": 1.7133383596396814
+                },
+                {
+                    "x": 6.6997616539527085,
+                    "y": 3.6634609457782683,
+                    "heading": 1.5377682789348178e-9,
+                    "angularVelocity": 1.5725923809489335e-15,
+                    "velocityX": 3.1871160605618085,
+                    "velocityY": -2.037871479084932,
+                    "timestamp": 1.735043293843451
+                },
+                {
+                    "x": 6.7667761400423245,
+                    "y": 3.6160177634442383,
+                    "heading": 1.5377682945212807e-9,
+                    "angularVelocity": 7.181069480230158e-16,
+                    "velocityX": 3.0875231161945336,
+                    "velocityY": -2.185824747894625,
+                    "timestamp": 1.7567482280472206
+                },
+                {
+                    "x": 6.831456555555867,
+                    "y": 3.5654387099344507,
+                    "heading": 1.5377683209709385e-9,
+                    "angularVelocity": 1.2186011773240001e-15,
+                    "velocityX": 2.979986712068234,
+                    "velocityY": -2.330302088683456,
+                    "timestamp": 1.7784531622509903
                 },
                 {
                     "x": 6.893649101257324,
                     "y": 3.5118300914764404,
-                    "heading": 3.1400002631780617,
-                    "angularVelocity": 1.5501587028498165e-13,
-                    "velocityX": 2.865369894292817,
-                    "velocityY": -2.469875260820684,
-                    "timestamp": 1.8001581040063641
-                },
-                {
-                    "x": 6.9342196158595195,
-                    "y": 3.4745269537623855,
-                    "heading": 3.140000263178065,
-                    "angularVelocity": 2.3326761756772743e-13,
-                    "velocityX": 2.784724807966257,
-                    "velocityY": -2.5604549024296013,
-                    "timestamp": 1.8147270533280853
-                },
-                {
-                    "x": 6.973574770579654,
-                    "y": 3.435943763171971,
-                    "heading": 3.140000263178068,
-                    "angularVelocity": 2.2697466808256025e-13,
-                    "velocityX": 2.701303563563437,
-                    "velocityY": -2.648316617649584,
-                    "timestamp": 1.8292960026498064
-                },
-                {
-                    "x": 7.011676569760775,
-                    "y": 3.396122365046946,
-                    "heading": 3.140000263178071,
-                    "angularVelocity": 2.224027473304724e-13,
-                    "velocityX": 2.615274330340327,
-                    "velocityY": -2.7333061050358776,
-                    "timestamp": 1.8438649519715276
-                },
-                {
-                    "x": 7.048491012484172,
-                    "y": 3.355107883496788,
-                    "heading": 3.1400002631780746,
-                    "angularVelocity": 2.1692590269440246e-13,
-                    "velocityX": 2.5269113036529633,
-                    "velocityY": -2.815198312825684,
-                    "timestamp": 1.8584339012932487
-                },
-                {
-                    "x": 7.083992093604447,
-                    "y": 3.312951458599195,
-                    "heading": 3.1400002631780777,
-                    "angularVelocity": 2.1487378521690259e-13,
-                    "velocityX": 2.4367633064212786,
-                    "velocityY": -2.893580310196796,
-                    "timestamp": 1.8730028506149698
-                },
-                {
-                    "x": 7.118173429008876,
-                    "y": 3.269718054236438,
-                    "heading": 3.140000263178081,
-                    "angularVelocity": 2.1169602046875272e-13,
-                    "velocityX": 2.3461771092494,
-                    "velocityY": -2.967503243236169,
-                    "timestamp": 1.887571799936691
-                },
-                {
-                    "x": 7.151084190942531,
-                    "y": 3.2255097664946186,
-                    "heading": 3.140000263178084,
-                    "angularVelocity": 2.115299519607656e-13,
-                    "velocityX": 2.258966052177459,
-                    "velocityY": -3.034418389794455,
-                    "timestamp": 1.9021407492584121
-                },
-                {
-                    "x": 7.1829258498520225,
-                    "y": 3.1805253278057903,
-                    "heading": 3.1400002631783006,
-                    "angularVelocity": 1.4861478341138462e-11,
-                    "velocityX": 2.1855837511923784,
-                    "velocityY": -3.0876927152018316,
-                    "timestamp": 1.9167096985801333
-                },
-                {
-                    "x": 7.213982568749707,
-                    "y": 3.1352979487738755,
-                    "heading": 3.140000221251036,
-                    "angularVelocity": -0.000002877850999701821,
-                    "velocityX": 2.1317061520275473,
-                    "velocityY": -3.104367928885527,
-                    "timestamp": 1.9312786479018544
-                },
-                {
-                    "x": 7.244041792187231,
-                    "y": 3.091532444769153,
-                    "heading": 3.1400001920667906,
-                    "angularVelocity": -0.000002003181212693182,
-                    "velocityX": 2.063238931905195,
-                    "velocityY": -3.0040260994983523,
-                    "timestamp": 1.9458475972235756
-                },
-                {
-                    "x": 7.273101015062598,
-                    "y": 3.049227126183516,
-                    "heading": 3.140000168751395,
-                    "angularVelocity": -0.0000016003484573673396,
-                    "velocityX": 1.9945997637621933,
-                    "velocityY": -2.9038002433409447,
-                    "timestamp": 1.9604165465452967
-                },
-                {
-                    "x": 7.301159421127851,
-                    "y": 3.008381441393539,
-                    "heading": 3.1400001491208855,
-                    "angularVelocity": -0.0000013474210949527603,
-                    "velocityX": 1.92590456907015,
-                    "velocityY": -2.8036122501352616,
-                    "timestamp": 1.9749854958670179
-                },
-                {
-                    "x": 7.328216598566981,
-                    "y": 2.9689951117692965,
-                    "heading": 3.1400001321528324,
-                    "angularVelocity": -0.0000011646724122855417,
-                    "velocityX": 1.8571811076855331,
-                    "velocityY": -2.703443381845065,
-                    "timestamp": 1.989554445188739
-                },
-                {
-                    "x": 7.35427229894417,
-                    "y": 2.9310679691270813,
-                    "heading": 3.140000117255664,
-                    "angularVelocity": -0.0000010225286768555236,
-                    "velocityX": 1.7884405938827321,
-                    "velocityY": -2.603286057538151,
-                    "timestamp": 2.00412339451046
-                },
-                {
-                    "x": 7.379326356096679,
-                    "y": 2.8945999009401007,
-                    "heading": 3.1400001040435335,
-                    "angularVelocity": -9.068691499457651e-7,
-                    "velocityX": 1.7196886748142857,
-                    "velocityY": -2.50313645697232,
-                    "timestamp": 2.0186923438321815
-                },
-                {
-                    "x": 7.403378651082293,
-                    "y": 2.8595908266403365,
-                    "heading": 3.1400000922448146,
-                    "angularVelocity": -8.098538018281321e-7,
-                    "velocityX": 1.650928591655569,
-                    "velocityY": -2.4029923865251464,
-                    "timestamp": 2.033261293153903
-                },
-                {
-                    "x": 7.426429094558334,
-                    "y": 2.826040685699055,
-                    "heading": 3.1400000816578917,
-                    "angularVelocity": -7.266771822683861e-7,
-                    "velocityX": 1.5821623760933374,
-                    "velocityY": -2.3028524707174887,
-                    "timestamp": 2.0478302424756243
-                },
-                {
-                    "x": 7.448477616956692,
-                    "y": 2.793949430978311,
-                    "heading": 3.1400000721271697,
-                    "angularVelocity": -6.541804651955507e-7,
-                    "velocityX": 1.513391385436516,
-                    "velocityY": -2.202715790417496,
-                    "timestamp": 2.0623991917973457
-                },
-                {
-                    "x": 7.4695241625734585,
-                    "y": 2.763317024730304,
-                    "heading": 3.1400000635289262,
-                    "angularVelocity": -5.901759591769764e-7,
-                    "velocityX": 1.4446165713119874,
-                    "velocityY": -2.102581701093514,
-                    "timestamp": 2.076968141119067
-                },
-                {
-                    "x": 7.489568685800359,
-                    "y": 2.734143436045934,
-                    "heading": 3.1400000557624246,
-                    "angularVelocity": -5.330859058092693e-7,
-                    "velocityX": 1.3758386266751614,
-                    "velocityY": -2.0024497333432913,
-                    "timestamp": 2.0915370904407884
-                },
-                {
-                    "x": 7.508611148608458,
-                    "y": 2.7064286391508827,
-                    "heading": 3.140000048744052,
-                    "angularVelocity": -4.817349694037634e-7,
-                    "velocityX": 1.3070580717652986,
-                    "velocityY": -1.902319534719876,
-                    "timestamp": 2.1061060397625098
-                },
-                {
-                    "x": 7.526651518803911,
-                    "y": 2.680172612224319,
-                    "heading": 3.1400000424032948,
-                    "angularVelocity": -4.3522407744534453e-7,
-                    "velocityX": 1.2382753070979928,
-                    "velocityY": -1.8021908338592867,
-                    "timestamp": 2.120674989084231
-                },
-                {
-                    "x": 7.543689768780771,
-                    "y": 2.6553753365541346,
-                    "heading": 3.1400000366798815,
-                    "angularVelocity": -3.9285011556875146e-7,
-                    "velocityX": 1.1694906475824893,
-                    "velocityY": -1.7020634173815092,
-                    "timestamp": 2.1352439384059525
-                },
-                {
-                    "x": 7.559725874605572,
-                    "y": 2.6320367959168407,
-                    "heading": 3.1400000315217036,
-                    "angularVelocity": -3.5405282918878005e-7,
-                    "velocityX": 1.100704345294715,
-                    "velocityY": -1.6019371144699315,
-                    "timestamp": 2.149812887727674
-                },
-                {
-                    "x": 7.574759815330251,
-                    "y": 2.6101569761120964,
-                    "heading": 3.140000026883262,
-                    "angularVelocity": -3.1837857149022665e-7,
-                    "velocityX": 1.0319166051501305,
-                    "velocityY": -1.5018117862573288,
-                    "timestamp": 2.1643818370493952
-                },
-                {
-                    "x": 7.588791572466392,
-                    "y": 2.5897358646065065,
-                    "heading": 3.1400000227244846,
-                    "angularVelocity": -2.854548657017254e-7,
-                    "velocityX": 0.9631275959768569,
-                    "velocityY": -1.4016873183261147,
-                    "timestamp": 2.1789507863711166
-                },
-                {
-                    "x": 7.601821129576078,
-                    "y": 2.570773450256394,
-                    "heading": 3.140000019009809,
-                    "angularVelocity": -2.5497212819286766e-7,
-                    "velocityX": 0.8943374585191634,
-                    "velocityY": -1.3015636152871453,
-                    "timestamp": 2.193519735692838
-                },
-                {
-                    "x": 7.61384847194871,
-                    "y": 2.5532697230888193,
-                    "heading": 3.1400000157074612,
-                    "angularVelocity": -2.2667026217565558e-7,
-                    "velocityX": 0.8255463113387961,
-                    "velocityY": -1.201440596782113,
-                    "timestamp": 2.2080886850145593
-                },
-                {
-                    "x": 7.624873586342355,
-                    "y": 2.5372246741263043,
-                    "heading": 3.1400000127888834,
-                    "angularVelocity": -2.0032864319885596e-7,
-                    "velocityX": 0.7567542552436427,
-                    "velocityY": -1.1013181944833246,
-                    "timestamp": 2.2226576343362807
-                },
-                {
-                    "x": 7.634896460774273,
-                    "y": 2.5226382952448834,
-                    "heading": 3.140000010228267,
-                    "angularVelocity": -1.757585156167035e-7,
-                    "velocityX": 0.6879613766637577,
-                    "velocityY": -1.0011963498064516,
-                    "timestamp": 2.237226583658002
-                },
-                {
-                    "x": 7.643917084349478,
-                    "y": 2.509510579057917,
-                    "heading": 3.140000008002173,
-                    "angularVelocity": -1.527971366621834e-7,
-                    "velocityX": 0.6191677502613596,
-                    "velocityY": -0.9010750121421621,
-                    "timestamp": 2.2517955329797235
-                },
-                {
-                    "x": 7.6519354471190555,
-                    "y": 2.4978415188200733,
-                    "heading": 3.140000006089223,
-                    "angularVelocity": -1.3130320584948841e-7,
-                    "velocityX": 0.5503734409743167,
-                    "velocityY": -0.8009541374715159,
-                    "timestamp": 2.266364482301445
-                },
-                {
-                    "x": 7.658951539962077,
-                    "y": 2.487631108347293,
-                    "heading": 3.1400000044698366,
-                    "angularVelocity": -1.1115325658440658e-7,
-                    "velocityX": 0.4815785056345368,
-                    "velocityY": -0.7008336872693404,
-                    "timestamp": 2.280933431623166
-                },
-                {
-                    "x": 7.664965354486367,
-                    "y": 2.478879341949535,
-                    "heading": 3.1400000031260147,
-                    "angularVelocity": -9.223877588817121e-8,
-                    "velocityX": 0.4127829942631362,
-                    "velocityY": -0.600713627626585,
-                    "timestamp": 2.2955023809448876
-                },
-                {
-                    "x": 7.669976882944524,
-                    "y": 2.4715862143738576,
-                    "heading": 3.140000002041154,
-                    "angularVelocity": -7.44638832915239e-8,
-                    "velocityX": 0.34398695111687533,
-                    "velocityY": -0.5005939285411971,
-                    "timestamp": 2.310071330266609
-                },
-                {
-                    "x": 7.673986118162365,
-                    "y": 2.4657517207559176,
-                    "heading": 3.1400000011998928,
-                    "angularVelocity": -5.774344306809202e-8,
-                    "velocityX": 0.27519041554106066,
-                    "velocityY": -0.4004745633401276,
-                    "timestamp": 2.3246402795883303
-                },
-                {
-                    "x": 7.676993053477586,
-                    "y": 2.4613758565783885,
-                    "heading": 3.1400000005879747,
-                    "angularVelocity": -4.2001516695103525e-8,
-                    "velocityX": 0.20639342267033312,
-                    "velocityY": -0.30035550820440443,
-                    "timestamp": 2.3392092289100517
-                },
-                {
-                    "x": 7.6789976826868624,
-                    "y": 2.458458617635108,
-                    "heading": 3.1400000001921353,
-                    "angularVelocity": -2.7170084001525683e-8,
-                    "velocityX": 0.13759600400877475,
-                    "velocityY": -0.2002367417759812,
-                    "timestamp": 2.353778178231773
+                    "heading": 1.5377683531646926e-9,
+                    "angularVelocity": 1.4832457732424108e-15,
+                    "velocityX": 2.8653643967583453,
+                    "velocityY": -2.4698816386502185,
+                    "timestamp": 1.8001580964547599
+                },
+                {
+                    "x": 6.934219508494461,
+                    "y": 3.4745268434152994,
+                    "heading": 1.5377683787984002e-9,
+                    "angularVelocity": 1.7594754613333528e-15,
+                    "velocityX": 2.784717658151537,
+                    "velocityY": -2.56046267850036,
+                    "timestamp": 1.8147270446274342
+                },
+                {
+                    "x": 6.973574535692003,
+                    "y": 3.4359435289599864,
+                    "heading": 1.5377684019331704e-9,
+                    "angularVelocity": 1.5879505990145664e-15,
+                    "velocityX": 2.701295023573416,
+                    "velocityY": -2.648325328500865,
+                    "timestamp": 1.8292959928001085
+                },
+                {
+                    "x": 7.011676190859291,
+                    "y": 3.3961219990562412,
+                    "heading": 1.5377684253031837e-9,
+                    "angularVelocity": 1.6040974952296623e-15,
+                    "velocityX": 2.6152646516208327,
+                    "velocityY": -2.733315365788167,
+                    "timestamp": 1.8438649409727828
+                },
+                {
+                    "x": 7.048490476455465,
+                    "y": 3.3551073823105697,
+                    "heading": 1.5377684383832216e-9,
+                    "angularVelocity": 8.97802495662944e-16,
+                    "velocityX": 2.5269007178724983,
+                    "velocityY": -2.815207814562461,
+                    "timestamp": 1.858433889145457
+                },
+                {
+                    "x": 7.083991390306391,
+                    "y": 3.3129508222340074,
+                    "heading": 1.5377684557448877e-9,
+                    "angularVelocity": 1.191689740876308e-15,
+                    "velocityX": 2.4367520173841823,
+                    "velocityY": -2.893589816979968,
+                    "timestamp": 1.8730028373181313
+                },
+                {
+                    "x": 7.118172550776329,
+                    "y": 3.269717285105602,
+                    "heading": 1.5377684739509254e-9,
+                    "angularVelocity": 1.2496467044902449e-15,
+                    "velocityX": 2.346165286939037,
+                    "velocityY": -2.9675125902013777,
+                    "timestamp": 1.8875717854908056
+                },
+                {
+                    "x": 7.15108313208807,
+                    "y": 3.2255088683196913,
+                    "heading": 1.5377684882032233e-9,
+                    "angularVelocity": 9.782653941580851e-16,
+                    "velocityX": 2.2589538326090746,
+                    "velocityY": -3.0344274865927217,
+                    "timestamp": 1.90214073366348
+                },
+                {
+                    "x": 7.182924606349684,
+                    "y": 3.1805243042550315,
+                    "heading": 1.5377697666399164e-9,
+                    "angularVelocity": 8.775078931151136e-14,
+                    "velocityX": 2.185571249497722,
+                    "velocityY": -3.0877015644156707,
+                    "timestamp": 1.9167096818361542
+                },
+                {
+                    "x": 7.2139811373084415,
+                    "y": 3.1352968072431113,
+                    "heading": 1.2915519070501347e-9,
+                    "angularVelocity": -1.6900181039231953e-8,
+                    "velocityX": 2.131693420188762,
+                    "velocityY": -3.1043762717714163,
+                    "timestamp": 1.9312786300088285
+                },
+                {
+                    "x": 7.244040263059187,
+                    "y": 3.0915312465009275,
+                    "heading": 1.1211375399666565e-9,
+                    "angularVelocity": -1.169709485282711e-8,
+                    "velocityX": 2.063232389495776,
+                    "velocityY": -3.0040302308350935,
+                    "timestamp": 1.9458475781815028
+                },
+                {
+                    "x": 7.2730994599044765,
+                    "y": 3.0492259199621516,
+                    "heading": 9.847245150980228e-10,
+                    "angularVelocity": -9.363272118309568e-9,
+                    "velocityX": 1.9945981343934114,
+                    "velocityY": -2.903801018259091,
+                    "timestamp": 1.960416526354177
+                },
+                {
+                    "x": 7.301157878524098,
+                    "y": 3.0083802533354898,
+                    "heading": 8.698146081553592e-10,
+                    "angularVelocity": -7.887316693614981e-9,
+                    "velocityX": 1.9259055826862927,
+                    "velocityY": -2.803611224540764,
+                    "timestamp": 1.9749854745268514
+                },
+                {
+                    "x": 7.328215092540324,
+                    "y": 2.968993958016925,
+                    "heading": 7.704962597777701e-10,
+                    "angularVelocity": -6.8171255926008325e-9,
+                    "velocityX": 1.857183764780944,
+                    "velocityY": -2.7034412403524777,
+                    "timestamp": 1.9895544226995256
+                },
+                {
+                    "x": 7.354270845617658,
+                    "y": 2.9310668604072108,
+                    "heading": 6.833248169018587e-10,
+                    "angularVelocity": -5.983372499761897e-9,
+                    "velocityX": 1.7884443522287568,
+                    "velocityY": -2.6032831718663054,
+                    "timestamp": 2.0041233708722
+                },
+                {
+                    "x": 7.379324966656107,
+                    "y": 2.894598844592948,
+                    "heading": 6.060446454340774e-10,
+                    "angularVelocity": -5.304444180973637e-9,
+                    "velocityX": 1.719693195521089,
+                    "velocityY": -2.5031330595756027,
+                    "timestamp": 2.0186923190448742
+                },
+                {
+                    "x": 7.403377333338156,
+                    "y": 2.8595898276899736,
+                    "heading": 5.370639718880826e-10,
+                    "angularVelocity": -4.734773818100124e-9,
+                    "velocityX": 1.6509336430450512,
+                    "velocityY": -2.4029886363820543,
+                    "timestamp": 2.0332612672175485
+                },
+                {
+                    "x": 7.426427853868939,
+                    "y": 2.8260397474863512,
+                    "heading": 4.75199340983144e-10,
+                    "angularVelocity": -4.246334802521844e-9,
+                    "velocityX": 1.582167789848791,
+                    "velocityY": -2.3028484833621037,
+                    "timestamp": 2.047830215390223
+                },
+                {
+                    "x": 7.44847645681879,
+                    "y": 2.79394855556605,
+                    "heading": 4.195363288341646e-10,
+                    "angularVelocity": -3.820661051835768e-9,
+                    "velocityX": 1.5133970337819913,
+                    "velocityY": -2.202711653576578,
+                    "timestamp": 2.062399163562897
+                },
+                {
+                    "x": 7.469523085022814,
+                    "y": 2.7633162131780225,
+                    "heading": 3.6934715040819367e-10,
+                    "angularVelocity": -3.4449418365911004e-9,
+                    "velocityX": 1.4446223539663485,
+                    "velocityY": -2.1025774836294286,
+                    "timestamp": 2.0769681117355714
+                },
+                {
+                    "x": 7.489567691695811,
+                    "y": 2.7341426886048597,
+                    "heading": 3.2403879933557077e-10,
+                    "angularVelocity": -3.1099260658641154e-9,
+                    "velocityX": 1.3758444628549362,
+                    "velocityY": -2.002445490737794,
+                    "timestamp": 2.0915370599082457
+                },
+                {
+                    "x": 7.5086102378406,
+                    "y": 2.7064279554071735,
+                    "heading": 2.831187733993615e-10,
+                    "angularVelocity": -2.8087152215039624e-9,
+                    "velocityX": 1.3070638950107798,
+                    "velocityY": -1.9023153126229018,
+                    "timestamp": 2.10610600808092
+                },
+                {
+                    "x": 7.52665069045286,
+                    "y": 2.680171991207375,
+                    "heading": 2.4617151569169827e-10,
+                    "angularVelocity": -2.536027844020545e-9,
+                    "velocityX": 1.2382810617787536,
+                    "velocityY": -1.8021866704863527,
+                    "timestamp": 2.1206749562535943
+                },
+                {
+                    "x": 7.543689021238326,
+                    "y": 2.65537477682047,
+                    "heading": 2.128416865029733e-10,
+                    "angularVelocity": -2.287730676987115e-9,
+                    "velocityX": 1.169496286452695,
+                    "velocityY": -1.7020593451912265,
+                    "timestamp": 2.1352439044262685
+                },
+                {
+                    "x": 7.559725205671708,
+                    "y": 2.6320362956163437,
+                    "heading": 1.8282195651887185e-10,
+                    "angularVelocity": -2.0605283283789865e-9,
+                    "velocityX": 1.1007098277319451,
+                    "velocityY": -1.6019331613726957,
+                    "timestamp": 2.149812852598943
+                },
+                {
+                    "x": 7.5747592222906635,
+                    "y": 2.6101565330412897,
+                    "heading": 1.558438913168689e-10,
+                    "angularVelocity": -1.8517510864510685e-9,
+                    "velocityX": 1.0319218958546759,
+                    "velocityY": -1.5018079765081174,
+                    "timestamp": 2.164381800771617
+                },
+                {
+                    "x": 7.588791052155751,
+                    "y": 2.5897354762519833,
+                    "heading": 1.3167100643601026e-10,
+                    "angularVelocity": -1.6592059334641093e-9,
+                    "velocityX": 0.9631326639903722,
+                    "velocityY": -1.4016836731981617,
+                    "timestamp": 2.1789507489442914
+                },
+                {
+                    "x": 7.601820678430282,
+                    "y": 2.570773113830717,
+                    "heading": 1.100933830131512e-10,
+                    "angularVelocity": -1.4810694297523211e-9,
+                    "velocityX": 0.894342276470416,
+                    "velocityY": -1.3015601535897774,
+                    "timestamp": 2.1935196971169657
+                },
+                {
+                    "x": 7.613848086048565,
+                    "y": 2.5532694355605217,
+                    "heading": 9.092342750630814e-11,
+                    "angularVelocity": -1.315809184573053e-9,
+                    "velocityX": 0.8255508548544661,
+                    "velocityY": -1.201437335265309,
+                    "timestamp": 2.20808864528964
+                },
+                {
+                    "x": 7.624873261450447,
+                    "y": 2.537224432245223,
+                    "heading": 7.399249133993465e-11,
+                    "angularVelocity": -1.1621248318093622e-9,
+                    "velocityX": 0.7567585024813989,
+                    "velocityY": -1.101315148158345,
+                    "timestamp": 2.2226575934623143
+                },
+                {
+                    "x": 7.634896192366388,
+                    "y": 2.5226380955637384,
+                    "heading": 5.914813867727886e-11,
+                    "angularVelocity": -1.0189035430874983e-9,
+                    "velocityX": 0.6879653079374735,
+                    "velocityY": -1.0011935322032168,
+                    "timestamp": 2.2372265416349886
+                },
+                {
+                    "x": 7.643916867641572,
+                    "y": 2.5095104179508487,
+                    "heading": 4.6251914688462445e-11,
+                    "angularVelocity": -8.851856741832805e-10,
+                    "velocityX": 0.6191713477368228,
+                    "velocityY": -0.9010724355182365,
+                    "timestamp": 2.251795489807663
+                },
+                {
+                    "x": 7.651935277090606,
+                    "y": 2.4978413924986826,
+                    "heading": 3.517750680608747e-11,
+                    "angularVelocity": -7.601377906979457e-10,
+                    "velocityX": 0.5503766884196106,
+                    "velocityY": -0.8009518129835539,
+                    "timestamp": 2.266364437980337
+                },
+                {
+                    "x": 7.658951411376412,
+                    "y": 2.487631012874611,
+                    "heading": 2.580921448386634e-11,
+                    "angularVelocity": -6.43031492677376e-10,
+                    "velocityX": 0.48158138821334623,
+                    "velocityY": -0.7008316251150604,
+                    "timestamp": 2.2809333861530114
+                },
+                {
+                    "x": 7.664965261908482,
+                    "y": 2.4788792732522666,
+                    "heading": 1.804066569533592e-11,
+                    "angularVelocity": -5.332264785860361e-10,
+                    "velocityX": 0.41278549836213546,
+                    "velocityY": -0.6007118371633381,
+                    "timestamp": 2.2955023343256857
+                },
+                {
+                    "x": 7.669976820756786,
+                    "y": 2.4715861682531717,
+                    "heading": 1.1773732592627752e-11,
+                    "angularVelocity": -4.3015687513290903e-10,
+                    "velocityX": 0.34398906420045333,
+                    "velocityY": -0.500592418385764,
+                    "timestamp": 2.31007128249836
+                },
+                {
+                    "x": 7.67398608057842,
+                    "y": 2.465751692897001,
+                    "heading": 6.917607828867898e-12,
+                    "angularVelocity": -3.3332020945886676e-10,
+                    "velocityX": 0.27519212602817883,
+                    "velocityY": -0.40047334145331454,
+                    "timestamp": 2.3246402306710343
+                },
+                {
+                    "x": 7.676993034554729,
+                    "y": 2.461375842558954,
+                    "heading": 3.3880167119093778e-12,
+                    "angularVelocity": -2.4226808579143967e-10,
+                    "velocityX": 0.20639471982945568,
+                    "velocityY": -0.30035458196322756,
+                    "timestamp": 2.3392091788437086
+                },
+                {
+                    "x": 7.678997676337105,
+                    "y": 2.4584586129329957,
+                    "heading": 1.1065347655527578e-12,
+                    "angularVelocity": -1.56598950365953e-10,
+                    "velocityX": 0.13759687786767488,
+                    "velocityY": -0.2002361180356331,
+                    "timestamp": 2.353778127016383
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": -1.3187979232574289e-8,
-                    "velocityX": 0.06879818791344262,
-                    "velocityY": -0.10011824483002574,
-                    "timestamp": 2.3683471275534944
+                    "heading": -3.441813048880529e-34,
+                    "angularVelocity": -7.595159066564751e-11,
+                    "velocityX": 0.06879862918134139,
+                    "velocityY": -0.10011792997737116,
+                    "timestamp": 2.368347075189057
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": 2.7611774763020037e-32,
-                    "velocityX": -1.1210620234419235e-31,
-                    "velocityY": 4.358756882663372e-31,
-                    "timestamp": 2.382916076875216
+                    "heading": -3.481182114535467e-34,
+                    "angularVelocity": -2.7023024138211624e-34,
+                    "velocityX": -3.7878173595367446e-32,
+                    "velocityY": -1.5253102288533056e-31,
+                    "timestamp": 2.3829160233617315
                 }
             ],
             "constraints": [
@@ -5253,7 +5241,7 @@
                 {
                     "x": 1.35,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -5262,7 +5250,7 @@
                 {
                     "x": 2.7507288455963135,
                     "y": 3.0107133388519287,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -5271,7 +5259,7 @@
                 {
                     "x": 5.311619758605957,
                     "y": 1.6392407417297363,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -5280,7 +5268,7 @@
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -5291,622 +5279,622 @@
                 {
                     "x": 1.35,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "angularVelocity": 0,
                     "velocityX": 0,
-                    "velocityY": 3.295386080925321e-33,
+                    "velocityY": -4.1566893564125267e-32,
                     "timestamp": 0
                 },
                 {
-                    "x": 1.3546830312136446,
+                    "x": 1.3546830312136449,
                     "y": 5.550921973415328,
-                    "heading": 3.14,
-                    "angularVelocity": -6.105321890596023e-17,
-                    "velocityX": 0.13379730014070298,
-                    "velocityY": -0.2593652256885782,
-                    "timestamp": 0.03500093954603884
-                },
-                {
-                    "x": 1.364049093509165,
-                    "y": 5.532765920501418,
-                    "heading": 3.14,
-                    "angularVelocity": -9.872410216704159e-17,
-                    "velocityX": 0.2675945965166781,
+                    "heading": -1.4050458772244498e-18,
+                    "angularVelocity": -4.014308801297789e-17,
+                    "velocityX": 0.13379730014070312,
+                    "velocityY": -0.25936522568857817,
+                    "timestamp": 0.03500093954603885
+                },
+                {
+                    "x": 1.3640490935091651,
+                    "y": 5.532765920501419,
+                    "heading": -3.603876179363921e-18,
+                    "angularVelocity": -6.28220327965604e-17,
+                    "velocityX": 0.26759459651667833,
                     "velocityY": -0.5187304440792354,
-                    "timestamp": 0.07000187909207768
+                    "timestamp": 0.0700018790920777
                 },
                 {
-                    "x": 1.3780981867284383,
+                    "x": 1.3780981867284385,
                     "y": 5.505531841564791,
-                    "heading": 3.14,
-                    "angularVelocity": -1.396517144912572e-16,
-                    "velocityX": 0.40139188837498685,
+                    "heading": -4.091508575199503e-18,
+                    "angularVelocity": -1.3931950355515604e-17,
+                    "velocityX": 0.4013918883749871,
                     "velocityY": -0.7780956537124009,
-                    "timestamp": 0.10500281863811653
+                    "timestamp": 0.10500281863811654
                 },
                 {
-                    "x": 1.3968303106782043,
-                    "y": 5.46921973698008,
-                    "heading": 3.14,
-                    "angularVelocity": -1.3147784440878345e-16,
-                    "velocityX": 0.5351891747117138,
+                    "x": 1.3968303106782045,
+                    "y": 5.469219736980081,
+                    "heading": -4.985304542720085e-18,
+                    "angularVelocity": -2.5536326104216816e-17,
+                    "velocityX": 0.5351891747117141,
                     "velocityY": -1.0374608526419855,
-                    "timestamp": 0.14000375818415536
+                    "timestamp": 0.1400037581841554
                 },
                 {
-                    "x": 1.4202454651168874,
-                    "y": 5.423829607215582,
-                    "heading": 3.14,
-                    "angularVelocity": -7.833997421538468e-18,
-                    "velocityX": 0.6689864541464796,
+                    "x": 1.4202454651168877,
+                    "y": 5.423829607215581,
+                    "heading": -4.3424315250592746e-18,
+                    "angularVelocity": 1.8367331153854942e-17,
+                    "velocityX": 0.6689864541464801,
                     "velocityY": -1.2968260381921248,
-                    "timestamp": 0.1750046977301942
+                    "timestamp": 0.17500469773019423
                 },
                 {
-                    "x": 1.4483436497338924,
+                    "x": 1.4483436497338926,
                     "y": 5.369361452873384,
-                    "heading": 3.14,
-                    "angularVelocity": -2.0049167233544137e-17,
-                    "velocityX": 0.802783724707322,
+                    "heading": -2.0560193041515625e-18,
+                    "angularVelocity": 6.532433512064817e-17,
+                    "velocityX": 0.8027837247073224,
                     "velocityY": -1.5561912065401715,
-                    "timestamp": 0.21000563727623303
+                    "timestamp": 0.21000563727623306
                 },
                 {
-                    "x": 1.4811248641150931,
+                    "x": 1.4811248641150936,
                     "y": 5.305815274756268,
-                    "heading": 3.14,
-                    "angularVelocity": -8.129804229935506e-17,
-                    "velocityX": 0.9365809834363131,
-                    "velocityY": -1.8155563519521838,
-                    "timestamp": 0.24500657682227187
+                    "heading": 2.1208651292681738e-19,
+                    "angularVelocity": 6.480130833433249e-17,
+                    "velocityX": 0.9365809834363137,
+                    "velocityY": -1.815556351952184,
+                    "timestamp": 0.2450065768222719
                 },
                 {
-                    "x": 1.5185891076807168,
+                    "x": 1.518589107680717,
                     "y": 5.233191073988126,
-                    "heading": 3.14,
-                    "angularVelocity": -1.3088931274135507e-16,
-                    "velocityX": 1.0703782256008025,
-                    "velocityY": -2.0749214652539205,
+                    "heading": 3.0112738874394488e-18,
+                    "angularVelocity": 7.99746695569e-17,
+                    "velocityX": 1.0703782256008032,
+                    "velocityY": -2.074921465253921,
                     "timestamp": 0.2800075163683107
                 },
                 {
-                    "x": 1.56073637956111,
-                    "y": 5.151488852254778,
-                    "heading": 3.14,
-                    "angularVelocity": -1.390272479709275e-16,
-                    "velocityX": 1.204175442918738,
+                    "x": 1.5607363795611104,
+                    "y": 5.151488852254779,
+                    "heading": 5.4707987602321704e-18,
+                    "angularVelocity": 7.027028770373186e-17,
+                    "velocityX": 1.2041754429187383,
                     "velocityY": -2.334286530390624,
                     "timestamp": 0.31500845591434956
                 },
                 {
-                    "x": 1.6075666783068707,
+                    "x": 1.6075666783068712,
                     "y": 5.060708612365892,
-                    "heading": 3.14,
-                    "angularVelocity": -2.002620206188659e-16,
-                    "velocityX": 1.3379726188262815,
-                    "velocityY": -2.593651515253289,
+                    "heading": 7.441304754324509e-18,
+                    "angularVelocity": 5.629869559433992e-17,
+                    "velocityX": 1.3379726188262824,
+                    "velocityY": -2.5936515152532893,
                     "timestamp": 0.3500093954603884
                 },
                 {
-                    "x": 1.6590800010192643,
+                    "x": 1.6590800010192646,
                     "y": 4.9608503599406655,
-                    "heading": 3.14,
-                    "angularVelocity": -2.3956924314672257e-16,
-                    "velocityX": 1.471769711915058,
-                    "velocityY": -2.8530163395717247,
+                    "heading": 8.015191331788108e-18,
+                    "angularVelocity": 1.639633984973558e-17,
+                    "velocityX": 1.4717697119150586,
+                    "velocityY": -2.853016339571725,
                     "timestamp": 0.38501033500642723
                 },
                 {
-                    "x": 1.7152763390026582,
-                    "y": 4.851914111835608,
-                    "heading": 3.14,
-                    "angularVelocity": -2.6938408534488506e-16,
-                    "velocityX": 1.6055665565638675,
-                    "velocityY": -3.112380682288517,
+                    "x": 1.7152763390026586,
+                    "y": 4.851914111835609,
+                    "heading": 8.871725964928404e-18,
+                    "angularVelocity": 2.4471795525975083e-17,
+                    "velocityX": 1.6055665565638684,
+                    "velocityY": -3.112380682288518,
                     "timestamp": 0.42001127455246606
                 },
                 {
-                    "x": 1.7759790338056225,
-                    "y": 4.734242319347403,
-                    "heading": 3.14,
-                    "angularVelocity": 3.279195870571412e-16,
-                    "velocityX": 1.734316152375285,
-                    "velocityY": -3.3619609648879023,
+                    "x": 1.775979033805623,
+                    "y": 4.734242319347404,
+                    "heading": 1.526334137039101e-17,
+                    "angularVelocity": 1.8261268092684144e-16,
+                    "velocityX": 1.7343161523752855,
+                    "velocityY": -3.361960964887902,
                     "timestamp": 0.4550122140985049
                 },
                 {
-                    "x": 1.8366817286107124,
-                    "y": 4.616570526856246,
-                    "heading": 3.14,
-                    "angularVelocity": -3.1071543812317194e-16,
-                    "velocityX": 1.7343161524360031,
-                    "velocityY": -3.36196096497222,
+                    "x": 1.8366817286107127,
+                    "y": 4.616570526856247,
+                    "heading": 2.6738432547349554e-17,
+                    "angularVelocity": 3.2785094702569765e-16,
+                    "velocityX": 1.734316152436003,
+                    "velocityY": -3.3619609649722197,
                     "timestamp": 0.49001315364454373
                 },
                 {
-                    "x": 1.8973844234157842,
+                    "x": 1.8973844234157844,
                     "y": 4.498898734365081,
-                    "heading": 3.14,
-                    "angularVelocity": -2.903673566166758e-16,
-                    "velocityX": 1.7343161524354895,
-                    "velocityY": -3.361960964972484,
+                    "heading": 1.9852147320466475e-17,
+                    "angularVelocity": -1.967457250062152e-16,
+                    "velocityX": 1.7343161524354893,
+                    "velocityY": -3.3619609649724835,
                     "timestamp": 0.5250140931905826
                 },
                 {
-                    "x": 1.958087118220856,
-                    "y": 4.381226941873915,
-                    "heading": 3.14,
-                    "angularVelocity": 3.614460545323391e-16,
+                    "x": 1.9580871182208561,
+                    "y": 4.381226941873916,
+                    "heading": 2.777024505719557e-17,
+                    "angularVelocity": 2.2622529107579697e-16,
                     "velocityX": 1.7343161524354895,
-                    "velocityY": -3.3619609649724835,
+                    "velocityY": -3.361960964972483,
                     "timestamp": 0.5600150327366215
                 },
                 {
-                    "x": 2.0187898130259274,
+                    "x": 2.018789813025928,
                     "y": 4.26355514938275,
-                    "heading": 3.14,
-                    "angularVelocity": -3.3191632817458094e-16,
+                    "heading": 1.7414652311267274e-17,
+                    "angularVelocity": -2.9586613617347484e-16,
                     "velocityX": 1.7343161524354893,
                     "velocityY": -3.361960964972483,
                     "timestamp": 0.5950159722826603
                 },
                 {
-                    "x": 2.079492507830999,
-                    "y": 4.145883356891584,
-                    "heading": 3.14,
-                    "angularVelocity": 3.2426066858348807e-16,
-                    "velocityX": 1.7343161524354893,
+                    "x": 2.0794925078309996,
+                    "y": 4.145883356891583,
+                    "heading": 6.771469247853252e-18,
+                    "angularVelocity": -3.040827818183264e-16,
+                    "velocityX": 1.7343161524354895,
                     "velocityY": -3.361960964972483,
                     "timestamp": 0.6300169118286992
                 },
                 {
                     "x": 2.140195202636071,
-                    "y": 4.028211564400418,
-                    "heading": 3.14,
-                    "angularVelocity": -2.798848785469217e-16,
-                    "velocityX": 1.734316152435488,
-                    "velocityY": -3.3619609649724826,
+                    "y": 4.028211564400419,
+                    "heading": 1.7156471914154247e-17,
+                    "angularVelocity": 2.9670636674740876e-16,
+                    "velocityX": 1.7343161524354882,
+                    "velocityY": -3.361960964972482,
                     "timestamp": 0.6650178513747381
                 },
                 {
-                    "x": 2.200897897441142,
-                    "y": 3.9105397719092525,
-                    "heading": 3.14,
-                    "angularVelocity": -1.795170282800533e-16,
+                    "x": 2.2008978974411426,
+                    "y": 3.910539771909253,
+                    "heading": 9.427453062324153e-18,
+                    "angularVelocity": -2.208231822366884e-16,
                     "velocityX": 1.734316152435471,
                     "velocityY": -3.361960964972468,
                     "timestamp": 0.700018790920777
                 },
                 {
-                    "x": 2.2616005922462064,
-                    "y": 3.792867979418094,
-                    "heading": 3.14,
-                    "angularVelocity": -3.385716636422731e-16,
-                    "velocityX": 1.7343161524352515,
-                    "velocityY": -3.3619609649722846,
+                    "x": 2.261600592246207,
+                    "y": 3.7928679794180944,
+                    "heading": -2.0403441270686145e-18,
+                    "angularVelocity": -3.2764255297515643e-16,
+                    "velocityX": 1.7343161524352513,
+                    "velocityY": -3.3619609649722837,
                     "timestamp": 0.7350197304668159
                 },
                 {
-                    "x": 2.3223032870512124,
+                    "x": 2.322303287051213,
                     "y": 3.6751961869269927,
-                    "heading": 3.14,
-                    "angularVelocity": -3.759822562526031e-16,
-                    "velocityX": 1.7343161524334345,
-                    "velocityY": -3.361960964970739,
+                    "heading": 3.357966916114392e-18,
+                    "angularVelocity": 1.5423331811082687e-16,
+                    "velocityX": 1.7343161524334347,
+                    "velocityY": -3.3619609649707387,
                     "timestamp": 0.7700206700128548
                 },
                 {
-                    "x": 2.383005981879538,
-                    "y": 3.5575243944476096,
-                    "heading": 3.14,
-                    "angularVelocity": -2.6556763706975105e-16,
+                    "x": 2.383005981879539,
+                    "y": 3.55752439444761,
+                    "heading": 1.6083387472754582e-17,
+                    "angularVelocity": 3.6357368464068287e-16,
                     "velocityX": 1.7343161531001825,
-                    "velocityY": -3.3619609646356636,
+                    "velocityY": -3.361960964635663,
                     "timestamp": 0.8050216095588937
                 },
                 {
-                    "x": 2.443708981028518,
-                    "y": 3.4398527589551926,
-                    "heading": 3.14,
-                    "angularVelocity": 5.092773468950765e-17,
+                    "x": 2.4437089810285184,
+                    "y": 3.439852758955193,
+                    "heading": 2.2946423291224054e-17,
+                    "angularVelocity": 1.9608147402560587e-16,
                     "velocityX": 1.7343248477441577,
                     "velocityY": -3.3619564794134043,
                     "timestamp": 0.8400225491049326
                 },
                 {
                     "x": 2.507799181067132,
-                    "y": 3.3239911422386283,
-                    "heading": 3.14,
-                    "angularVelocity": -2.1081778211927598e-16,
+                    "y": 3.323991142238629,
+                    "heading": 9.669545465348722e-18,
+                    "angularVelocity": -3.793291836756151e-16,
                     "velocityX": 1.8310994181462568,
-                    "velocityY": -3.310243045471397,
+                    "velocityY": -3.3102430454713967,
                     "timestamp": 0.8750234886509715
                 },
                 {
                     "x": 2.5806292795037855,
-                    "y": 3.2134142705817266,
-                    "heading": 3.14,
-                    "angularVelocity": -3.875593095285532e-16,
+                    "y": 3.213414270581727,
+                    "heading": 3.38196516760506e-17,
+                    "angularVelocity": 6.899845125281262e-16,
                     "velocityX": 2.0808040978720626,
                     "velocityY": -3.159254382625257,
                     "timestamp": 0.9100244281970103
                 },
                 {
                     "x": 2.661766753531331,
-                    "y": 3.108780729790167,
-                    "heading": 3.14,
-                    "angularVelocity": -7.347130647585027e-16,
-                    "velocityX": 2.318151314794784,
+                    "y": 3.1087807297901673,
+                    "heading": 5.146705636243318e-17,
+                    "angularVelocity": 5.041980276892245e-16,
+                    "velocityX": 2.3181513147947848,
                     "velocityY": -2.9894494876003863,
                     "timestamp": 0.9450253677430492
                 },
                 {
                     "x": 2.7507288455963135,
                     "y": 3.0107133388519287,
-                    "heading": 3.14,
-                    "angularVelocity": -3.52994704864956e-16,
-                    "velocityX": 2.541705829014059,
-                    "velocityY": -2.801850242022933,
+                    "heading": 6.422105835490463e-17,
+                    "angularVelocity": 3.643902751722035e-16,
+                    "velocityX": 2.5417058290140595,
+                    "velocityY": -2.8018502420229328,
                     "timestamp": 0.9800263072890881
                 },
                 {
                     "x": 2.9215337593923705,
                     "y": 2.8658352980219064,
-                    "heading": 3.14,
-                    "angularVelocity": 1.5913396610629706e-16,
-                    "velocityX": 2.884920936848367,
-                    "velocityY": -2.4470121145413266,
+                    "heading": 5.1318206888106e-17,
+                    "angularVelocity": -2.1793112103404205e-16,
+                    "velocityX": 2.884920936848368,
+                    "velocityY": -2.4470121145413257,
                     "timestamp": 1.0392324060123015
                 },
                 {
                     "x": 3.109750180151373,
                     "y": 2.744432888397421,
-                    "heading": 3.14,
-                    "angularVelocity": -6.286818756935585e-16,
-                    "velocityX": 3.179003934013425,
-                    "velocityY": -2.0505051378666,
+                    "heading": 6.648964720130561e-17,
+                    "angularVelocity": 2.5624793121609224e-16,
+                    "velocityX": 3.179003934013426,
+                    "velocityY": -2.0505051378665997,
                     "timestamp": 1.098438504735515
                 },
                 {
-                    "x": 3.3097585780303125,
+                    "x": 3.309758578030313,
                     "y": 2.6436330516151556,
-                    "heading": 3.14,
-                    "angularVelocity": -3.4341251732407706e-17,
+                    "heading": 6.809268296964168e-17,
+                    "angularVelocity": 2.7075517621497836e-17,
                     "velocityX": 3.3781722186064402,
-                    "velocityY": -1.702524553319067,
+                    "velocityY": -1.702524553319066,
                     "timestamp": 1.1576446034587284
                 },
                 {
-                    "x": 3.5097675494852,
-                    "y": 2.5428343529427293,
-                    "heading": 3.14,
-                    "angularVelocity": -2.2516273301091217e-16,
-                    "velocityX": 3.3781819063939547,
-                    "velocityY": -1.7025053304654465,
+                    "x": 3.5097675494851996,
+                    "y": 2.542834352942729,
+                    "heading": 6.982732716570642e-17,
+                    "angularVelocity": 2.929840393934814e-17,
+                    "velocityX": 3.378181906393955,
+                    "velocityY": -1.702505330465446,
                     "timestamp": 1.216850702181942
                 },
                 {
                     "x": 3.709776520948374,
-                    "y": 2.442035654287211,
-                    "heading": 3.14,
-                    "angularVelocity": -3.629334864774022e-16,
-                    "velocityX": 3.378181906533671,
-                    "velocityY": -1.7025053301803685,
+                    "y": 2.4420356542872104,
+                    "heading": 8.871000937094426e-17,
+                    "angularVelocity": 3.189313704575333e-16,
+                    "velocityX": 3.3781819065336705,
+                    "velocityY": -1.702505330180368,
                     "timestamp": 1.2760568009051554
                 },
                 {
-                    "x": 3.909785492411555,
-                    "y": 2.3412369556316897,
-                    "heading": 3.14,
-                    "angularVelocity": -2.39059813931354e-16,
+                    "x": 3.9097854924115545,
+                    "y": 2.341236955631689,
+                    "heading": 7.04137580064196e-17,
+                    "angularVelocity": -3.090264644873572e-16,
                     "velocityX": 3.3781819065337895,
-                    "velocityY": -1.7025053301803958,
+                    "velocityY": -1.7025053301803954,
                     "timestamp": 1.3352628996283689
                 },
                 {
                     "x": 4.109794463874735,
                     "y": 2.240438256976168,
-                    "heading": 3.14,
-                    "angularVelocity": 3.7738636092274995e-16,
-                    "velocityX": 3.378181906533792,
-                    "velocityY": -1.7025053301803967,
+                    "heading": 6.635408667627181e-17,
+                    "angularVelocity": -6.856846534504647e-17,
+                    "velocityX": 3.3781819065337926,
+                    "velocityY": -1.7025053301803963,
                     "timestamp": 1.3944689983515823
                 },
                 {
                     "x": 4.3098034353379155,
-                    "y": 2.139639558320647,
-                    "heading": 3.14,
-                    "angularVelocity": 3.725508803000677e-16,
+                    "y": 2.1396395583206465,
+                    "heading": 8.756379552984881e-17,
+                    "angularVelocity": 3.5823520399025277e-16,
                     "velocityX": 3.3781819065337926,
-                    "velocityY": -1.7025053301803965,
+                    "velocityY": -1.7025053301803963,
                     "timestamp": 1.4536750970747958
                 },
                 {
                     "x": 4.509812406801097,
                     "y": 2.0388408596651253,
-                    "heading": 3.14,
-                    "angularVelocity": -2.2963176234131617e-16,
+                    "heading": 7.076319345079811e-17,
+                    "angularVelocity": -2.837647208878057e-16,
                     "velocityX": 3.3781819065337926,
-                    "velocityY": -1.7025053301803967,
+                    "velocityY": -1.7025053301803963,
                     "timestamp": 1.5128811957980093
                 },
                 {
                     "x": 4.709821378264278,
                     "y": 1.9380421610096037,
-                    "heading": 3.14,
-                    "angularVelocity": -2.0546569648664528e-16,
-                    "velocityX": 3.3781819065338015,
-                    "velocityY": -1.702505330180399,
+                    "heading": 6.652684249010322e-17,
+                    "angularVelocity": -7.15526111686399e-17,
+                    "velocityX": 3.378181906533802,
+                    "velocityY": -1.7025053301803985,
                     "timestamp": 1.5720872945212228
                 },
                 {
                     "x": 4.9098303497278835,
-                    "y": 1.8372434623549267,
-                    "heading": 3.14,
-                    "angularVelocity": 1.0642243284106308e-16,
-                    "velocityX": 3.3781819065409677,
-                    "velocityY": -1.7025053301661326,
+                    "y": 1.837243462354927,
+                    "heading": 7.855425123263822e-17,
+                    "angularVelocity": 2.0314476045367986e-16,
+                    "velocityX": 3.378181906540968,
+                    "velocityY": -1.7025053301661324,
                     "timestamp": 1.6312933932444362
                 },
                 {
                     "x": 5.109839349236117,
-                    "y": 1.736444819347842,
-                    "heading": 3.14,
-                    "angularVelocity": 1.2679224790319894e-16,
+                    "y": 1.7364448193478421,
+                    "heading": 7.31685607290898e-17,
+                    "angularVelocity": -9.096513061477316e-17,
                     "velocityX": 3.378182380218786,
-                    "velocityY": -1.7025043902702677,
+                    "velocityY": -1.7025043902702668,
                     "timestamp": 1.6904994919676497
                 },
                 {
                     "x": 5.311619758605957,
                     "y": 1.6392407417297363,
-                    "heading": 3.14,
-                    "angularVelocity": 3.145701878302232e-16,
-                    "velocityX": 3.408101761838499,
-                    "velocityY": -1.6417916348765778,
+                    "heading": 6.073866217998324e-17,
+                    "angularVelocity": -2.0994287441931112e-16,
+                    "velocityX": 3.4081017618384997,
+                    "velocityY": -1.6417916348765775,
                     "timestamp": 1.7497055906908632
                 },
                 {
                     "x": 5.430192834880249,
                     "y": 1.5926107881863976,
-                    "heading": 3.14,
-                    "angularVelocity": 2.0229336550458753e-17,
+                    "heading": 7.355026472308393e-17,
+                    "angularVelocity": 3.8038299911394555e-16,
                     "velocityX": 3.5204949744296554,
-                    "velocityY": -1.384467049902731,
+                    "velocityY": -1.3844670499027307,
                     "timestamp": 1.78338638760593
                 },
                 {
                     "x": 5.550146685206584,
                     "y": 1.5496573158737061,
-                    "heading": 3.14,
-                    "angularVelocity": -2.618637002687426e-16,
-                    "velocityX": 3.561490858687975,
-                    "velocityY": -1.2753104512640405,
+                    "heading": 8.480161924468705e-17,
+                    "angularVelocity": 3.3405844137167443e-16,
+                    "velocityX": 3.5614908586879754,
+                    "velocityY": -1.2753104512640399,
                     "timestamp": 1.8170671845209967
                 },
                 {
                     "x": 5.670100671705041,
                     "y": 1.506704223843596,
-                    "heading": 3.14,
-                    "angularVelocity": -1.632751475559818e-17,
-                    "velocityX": 3.5614949017075617,
+                    "heading": 8.467696633181045e-17,
+                    "angularVelocity": -3.701008417078351e-18,
+                    "velocityX": 3.5614949017075612,
                     "velocityY": -1.2752991604806918,
                     "timestamp": 1.8507479814360634
                 },
                 {
                     "x": 5.790054658213967,
-                    "y": 1.4637511318444056,
-                    "heading": 3.14,
-                    "angularVelocity": -2.9492764372093784e-16,
+                    "y": 1.4637511318444054,
+                    "heading": 8.225246246818014e-17,
+                    "angularVelocity": -7.198475350046388e-17,
                     "velocityX": 3.561494902018399,
-                    "velocityY": -1.2752991595627796,
+                    "velocityY": -1.275299159562779,
                     "timestamp": 1.8844287783511302
                 },
                 {
                     "x": 5.910008644722922,
-                    "y": 1.4207980398452127,
-                    "heading": 3.14,
-                    "angularVelocity": -2.6299005747305263e-17,
+                    "y": 1.4207980398452125,
+                    "heading": 9.443730259968232e-17,
+                    "angularVelocity": 3.617741041647218e-16,
                     "velocityX": 3.5614949020192235,
-                    "velocityY": -1.2752991595628407,
+                    "velocityY": -1.2752991595628402,
                     "timestamp": 1.918109575266197
                 },
                 {
                     "x": 6.02996263123188,
-                    "y": 1.3778449478460184,
-                    "heading": 3.14,
-                    "angularVelocity": -3.0549772131007975e-16,
+                    "y": 1.3778449478460182,
+                    "heading": 8.582959689044548e-17,
+                    "angularVelocity": -2.555671628240573e-16,
                     "velocityX": 3.5614949020193487,
                     "velocityY": -1.2752991595628858,
                     "timestamp": 1.9517903721812637
                 },
                 {
                     "x": 6.149916617740839,
-                    "y": 1.3348918558468237,
-                    "heading": 3.14,
-                    "angularVelocity": 3.2741509193878177e-16,
+                    "y": 1.3348918558468235,
+                    "heading": 7.701354643967334e-17,
+                    "angularVelocity": -2.6175302422337863e-16,
                     "velocityX": 3.56149490201936,
-                    "velocityY": -1.27529915956289,
+                    "velocityY": -1.2752991595628895,
                     "timestamp": 1.9854711690963305
                 },
                 {
                     "x": 6.269870604249798,
-                    "y": 1.2919387638476292,
-                    "heading": 3.14,
-                    "angularVelocity": 3.4990315690965786e-16,
+                    "y": 1.291938763847629,
+                    "heading": 8.367010625720842e-17,
+                    "angularVelocity": 1.9763664839406868e-16,
                     "velocityX": 3.5614949020193607,
-                    "velocityY": -1.2752991595628904,
+                    "velocityY": -1.27529915956289,
                     "timestamp": 2.019151966011397
                 },
                 {
                     "x": 6.3898245907587565,
-                    "y": 1.2489856718484345,
-                    "heading": 3.14,
-                    "angularVelocity": 2.004607715018742e-16,
-                    "velocityX": 3.5614949020193603,
-                    "velocityY": -1.27529915956289,
+                    "y": 1.2489856718484342,
+                    "heading": 7.968637324735258e-17,
+                    "angularVelocity": -1.1827906031741492e-16,
+                    "velocityX": 3.5614949020193607,
+                    "velocityY": -1.2752991595628902,
                     "timestamp": 2.0528327629264638
                 },
                 {
                     "x": 6.509778577267715,
                     "y": 1.2060325798492397,
-                    "heading": 3.14,
-                    "angularVelocity": 3.130071946862803e-16,
-                    "velocityX": 3.5614949020193603,
-                    "velocityY": -1.2752991595628902,
+                    "heading": 7.914041276645377e-17,
+                    "angularVelocity": -1.6209844508000374e-17,
+                    "velocityX": 3.561494902019361,
+                    "velocityY": -1.27529915956289,
                     "timestamp": 2.0865135598415305
                 },
                 {
                     "x": 6.629732563776674,
                     "y": 1.1630794878500452,
-                    "heading": 3.14,
-                    "angularVelocity": -2.481040891393771e-16,
-                    "velocityX": 3.5614949020193607,
-                    "velocityY": -1.2752991595628902,
+                    "heading": 7.575912442206911e-17,
+                    "angularVelocity": -1.0039217162560839e-16,
+                    "velocityX": 3.561494902019361,
+                    "velocityY": -1.27529915956289,
                     "timestamp": 2.1201943567565973
                 },
                 {
                     "x": 6.749686550285633,
                     "y": 1.1201263958508503,
-                    "heading": 3.14,
-                    "angularVelocity": 5.765266318677121e-16,
-                    "velocityX": 3.561494902019357,
+                    "heading": 6.889954024340196e-17,
+                    "angularVelocity": -2.0366454499176497e-16,
+                    "velocityX": 3.5614949020193576,
                     "velocityY": -1.2752991595628989,
                     "timestamp": 2.153875153671664
                 },
                 {
                     "x": 6.869640536794461,
                     "y": 1.077173303851719,
-                    "heading": 3.14,
-                    "angularVelocity": 5.94200130731494e-16,
+                    "heading": 8.054846416919904e-17,
+                    "angularVelocity": 3.458624792985843e-16,
                     "velocityX": 3.5614949020154905,
-                    "velocityY": -1.2752991595610093,
+                    "velocityY": -1.275299159561009,
                     "timestamp": 2.187555950586731
                 },
                 {
                     "x": 6.985406128017608,
                     "y": 1.0357199913369377,
-                    "heading": 3.14,
-                    "angularVelocity": -5.216843428237177e-16,
-                    "velocityX": 3.437139314579594,
+                    "heading": 6.97905590140722e-17,
+                    "angularVelocity": -3.1940767538205783e-16,
+                    "velocityX": 3.437139314579595,
                     "velocityY": -1.2307699434580792,
                     "timestamp": 2.2212367475017976
                 },
                 {
                     "x": 7.092266700241795,
-                    "y": 0.9974553856875887,
-                    "heading": 3.14,
-                    "angularVelocity": -4.51325473252053e-16,
-                    "velocityX": 3.172744768885923,
+                    "y": 0.9974553856875888,
+                    "heading": 5.947830002893604e-17,
+                    "angularVelocity": -3.0617621445548207e-16,
+                    "velocityX": 3.172744768885924,
                     "velocityY": -1.1360956139440452,
                     "timestamp": 2.2549175444168643
                 },
                 {
-                    "x": 7.190222235939472,
+                    "x": 7.190222235939473,
                     "y": 0.9623794931799317,
-                    "heading": 3.14,
-                    "angularVelocity": -3.932803198593372e-16,
-                    "velocityX": 2.908349702790401,
+                    "heading": 5.044588900647289e-17,
+                    "angularVelocity": -2.6817687744625345e-16,
+                    "velocityX": 2.9083497027904013,
                     "velocityY": -1.0414210980846788,
                     "timestamp": 2.288598341331931
                 },
                 {
                     "x": 7.279272729268112,
-                    "y": 0.9304923159060571,
-                    "heading": 3.14,
-                    "angularVelocity": -3.6674368533996665e-16,
-                    "velocityX": 2.643954463227235,
-                    "velocityY": -0.9467465201100715,
+                    "y": 0.930492315906057,
+                    "heading": 4.27627289839913e-17,
+                    "angularVelocity": -2.281169282784766e-16,
+                    "velocityX": 2.6439544632272356,
+                    "velocityY": -0.9467465201100714,
                     "timestamp": 2.322279138246998
                 },
                 {
                     "x": 7.35941817730645,
-                    "y": 0.9017938549120107,
-                    "heading": 3.14,
-                    "angularVelocity": -3.61350987503525e-16,
-                    "velocityX": 2.3795591369302214,
+                    "y": 0.9017938549120106,
+                    "heading": 3.626946964953948e-17,
+                    "angularVelocity": -1.9278817213060433e-16,
+                    "velocityX": 2.379559136930222,
                     "velocityY": -0.8520719110778341,
                     "timestamp": 2.3559599351620646
                 },
                 {
                     "x": 7.430658578301727,
                     "y": 0.8762841108254199,
-                    "heading": 3.14,
-                    "angularVelocity": -3.11296740830968e-16,
-                    "velocityX": 2.115163758592898,
+                    "heading": 2.9442071838197796e-17,
+                    "angularVelocity": -2.0270891340137855e-16,
+                    "velocityX": 2.1151637585928986,
                     "velocityY": -0.7573972834110186,
                     "timestamp": 2.3896407320771313
                 },
                 {
                     "x": 7.492993931085436,
                     "y": 0.8539630840647031,
-                    "heading": 3.14,
-                    "angularVelocity": -3.0797715285651212e-16,
-                    "velocityX": 1.8507683455620387,
-                    "velocityY": -0.6627226433211519,
+                    "heading": 2.3413209096667504e-17,
+                    "angularVelocity": -1.790000012026981e-16,
+                    "velocityX": 1.850768345562039,
+                    "velocityY": -0.662722643321152,
                     "timestamp": 2.423321528992198
                 },
                 {
                     "x": 7.546424234822929,
                     "y": 0.8348307749287305,
-                    "heading": 3.14,
-                    "angularVelocity": -2.384214973522538e-16,
-                    "velocityX": 1.5863729077500837,
-                    "velocityY": -0.5680479943576783,
+                    "heading": 1.7731266545962487e-17,
+                    "angularVelocity": -1.6869976443484408e-16,
+                    "velocityX": 1.5863729077500839,
+                    "velocityY": -0.5680479943576784,
                     "timestamp": 2.457002325907265
                 },
                 {
                     "x": 7.590949488888224,
                     "y": 0.8188871836416546,
-                    "heading": 3.14,
-                    "angularVelocity": -1.6873896303460275e-16,
-                    "velocityX": 1.3219774513523088,
-                    "velocityY": -0.4733733387389998,
+                    "heading": 1.2363765500283435e-17,
+                    "angularVelocity": -1.593638362048541e-16,
+                    "velocityX": 1.321977451352309,
+                    "velocityY": -0.4733733387389999,
                     "timestamp": 2.4906831228223316
                 },
                 {
                     "x": 7.626569692794441,
                     "y": 0.8061323103778162,
-                    "heading": 3.14,
-                    "angularVelocity": -1.2706181170586285e-16,
-                    "velocityX": 1.0575819804988977,
-                    "velocityY": -0.3786986779440515,
+                    "heading": 7.88828312817125e-18,
+                    "angularVelocity": -1.3287934794211324e-16,
+                    "velocityX": 1.057581980498898,
+                    "velocityY": -0.37869867794405154,
                     "timestamp": 2.5243639197373984
                 },
                 {
                     "x": 7.653284846152079,
                     "y": 0.7965661552766882,
-                    "heading": 3.14,
-                    "angularVelocity": -1.0566459548829111e-16,
-                    "velocityX": 0.7931864980809785,
-                    "velocityY": -0.28402401300808755,
+                    "heading": 4.236479358871306e-18,
+                    "angularVelocity": -1.0842391112822455e-16,
+                    "velocityX": 0.7931864980809786,
+                    "velocityY": -0.28402401300808766,
                     "timestamp": 2.558044716652465
                 },
                 {
                     "x": 7.671094948642455,
                     "y": 0.7901887184523844,
-                    "heading": 3.14,
-                    "angularVelocity": -6.428789472113484e-17,
-                    "velocityX": 0.5287910062011897,
-                    "velocityY": -0.18934934468402026,
+                    "heading": 1.249730071610374e-18,
+                    "angularVelocity": -8.867810508445172e-17,
+                    "velocityX": 0.5287910062011898,
+                    "velocityY": -0.18934934468402032,
                     "timestamp": 2.591725513567532
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
-                    "angularVelocity": -8.910267594003032e-17,
-                    "velocityX": 0.26439550643651,
-                    "velocityY": -0.09467467353653364,
+                    "heading": -2.906959217388984e-34,
+                    "angularVelocity": -3.7105121777105e-17,
+                    "velocityX": 0.26439550643651005,
+                    "velocityY": -0.09467467353653367,
                     "timestamp": 2.6254063104825986
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
-                    "angularVelocity": -3.385083394775927e-32,
-                    "velocityX": 1.13191163707898e-33,
-                    "velocityY": 3.1068609037049197e-32,
+                    "heading": -4.078160951605287e-34,
+                    "angularVelocity": -3.477050502256718e-33,
+                    "velocityX": 7.685636324015334e-33,
+                    "velocityY": -2.3747808746440768e-32,
                     "timestamp": 2.6590871073976654
                 }
             ],
@@ -5934,7 +5922,7 @@
                 {
                     "x": 0.71,
                     "y": 4.36,
-                    "heading": 2.1,
+                    "heading": -1.04,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -5943,7 +5931,7 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": 3.5331853071795862,
+                    "heading": 0.393,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -5954,191 +5942,191 @@
                 {
                     "x": 0.71,
                     "y": 4.36,
-                    "heading": 2.1,
-                    "angularVelocity": 1.7952667234276065e-30,
-                    "velocityX": 8.378323076428447e-32,
-                    "velocityY": 1.4697485073236794e-33,
+                    "heading": -1.04,
+                    "angularVelocity": -9.2124979355083e-31,
+                    "velocityX": -2.0292335982741827e-31,
+                    "velocityY": 8.51805082704341e-32,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7269607278962015,
-                    "y": 4.384959045157939,
-                    "heading": 2.138406592301522,
-                    "angularVelocity": 0.6202480809957288,
-                    "velocityX": 0.2739076366713438,
-                    "velocityY": 0.4030766317709414,
-                    "timestamp": 0.06192133999006512
-                },
-                {
-                    "x": 0.7608491761808356,
-                    "y": 4.435120384688249,
-                    "heading": 2.213462658415359,
-                    "angularVelocity": 1.2121195394976814,
-                    "velocityX": 0.5472822178924311,
-                    "velocityY": 0.8100816219151135,
-                    "timestamp": 0.12384267998013024
-                },
-                {
-                    "x": 0.8116352150421299,
-                    "y": 4.5108343869767795,
-                    "heading": 2.3220976741276163,
-                    "angularVelocity": 1.7544035017602526,
-                    "velocityX": 0.8201702170760925,
-                    "velocityY": 1.2227448937745646,
-                    "timestamp": 0.18576401997019537
-                },
-                {
-                    "x": 0.879352990275961,
-                    "y": 4.612608489350272,
-                    "heading": 2.458161516132951,
-                    "angularVelocity": 2.197365916615584,
-                    "velocityX": 1.093609654518071,
-                    "velocityY": 1.6436030355580518,
-                    "timestamp": 0.24768535996026048
-                },
-                {
-                    "x": 0.9641073070497683,
-                    "y": 4.741095934499406,
-                    "heading": 2.609605752410294,
-                    "angularVelocity": 2.445751921738789,
-                    "velocityX": 1.3687416452454926,
-                    "velocityY": 2.075010734098273,
-                    "timestamp": 0.3096066999503256
-                },
-                {
-                    "x": 1.065422526066935,
-                    "y": 4.896666065794743,
-                    "heading": 2.7474397378967583,
-                    "angularVelocity": 2.2259528864940465,
-                    "velocityX": 1.6361922890141296,
-                    "velocityY": 2.5123831512738084,
-                    "timestamp": 0.3715280399403907
-                },
-                {
-                    "x": 1.1791068919270769,
-                    "y": 5.075184614148285,
-                    "heading": 2.811143860874752,
-                    "angularVelocity": 1.0287910918629104,
-                    "velocityX": 1.8359480895985416,
-                    "velocityY": 2.882989101692323,
-                    "timestamp": 0.4334493799304558
-                },
-                {
-                    "x": 1.309522757298702,
-                    "y": 5.269766971770384,
-                    "heading": 2.8111439317105513,
-                    "angularVelocity": 0.0000011439642533999806,
-                    "velocityX": 2.1061537975849616,
-                    "velocityY": 3.142411931869009,
-                    "timestamp": 0.4953707199205209
-                },
-                {
-                    "x": 1.4399391293197474,
-                    "y": 5.464348989818422,
-                    "heading": 2.811144002543952,
-                    "angularVelocity": 0.0000011439255168645237,
-                    "velocityX": 2.1061619797305715,
-                    "velocityY": 3.142406447910498,
-                    "timestamp": 0.5572920599105861
-                },
-                {
-                    "x": 1.570355539071906,
-                    "y": 5.658930982577611,
-                    "heading": 2.811144073377348,
-                    "angularVelocity": 0.0000011439254352819296,
-                    "velocityX": 2.1061625890699887,
-                    "velocityY": 3.1424060395076645,
-                    "timestamp": 0.6192133999006512
-                },
-                {
-                    "x": 1.7007719870139304,
-                    "y": 5.853512949740463,
-                    "heading": 2.811144144210739,
-                    "angularVelocity": 0.0000011439253582531217,
-                    "velocityX": 2.1061632058180395,
-                    "velocityY": 3.1424056261390936,
-                    "timestamp": 0.6811347398907163
-                },
-                {
-                    "x": 1.8311879242943474,
-                    "y": 6.048095259166571,
-                    "heading": 2.811144215047033,
-                    "angularVelocity": 0.0000011439722415453888,
-                    "velocityX": 2.1061549588775286,
-                    "velocityY": 3.1424111535268193,
-                    "timestamp": 0.7430560798807814
-                },
-                {
-                    "x": 1.9446720292676574,
-                    "y": 6.226098534814738,
-                    "heading": 2.87431577200777,
-                    "angularVelocity": 1.0201904056157727,
-                    "velocityX": 1.8327139721381605,
-                    "velocityY": 2.8746676941540104,
-                    "timestamp": 0.8049774198708465
-                },
-                {
-                    "x": 2.045803800007245,
-                    "y": 6.3800371688336455,
-                    "heading": 3.00267788576032,
-                    "angularVelocity": 2.072986692037753,
-                    "velocityX": 1.6332296871452403,
-                    "velocityY": 2.486035251233359,
-                    "timestamp": 0.8668987598609116
-                },
-                {
-                    "x": 2.1306920593857335,
-                    "y": 6.5074855802636336,
-                    "heading": 3.157852124619465,
-                    "angularVelocity": 2.5059896779372313,
-                    "velocityX": 1.370904754194726,
-                    "velocityY": 2.0582308369042988,
-                    "timestamp": 0.9288200998509767
-                },
-                {
-                    "x": 2.1984321789198162,
-                    "y": 6.6082437599743225,
-                    "heading": 3.2989222381105456,
-                    "angularVelocity": 2.278214804681454,
-                    "velocityX": 1.0939705042712429,
-                    "velocityY": 1.6271963708610766,
-                    "timestamp": 0.9907414398410418
-                },
-                {
-                    "x": 2.249205760326253,
-                    "y": 6.6830338505565665,
-                    "heading": 3.412569338792277,
-                    "angularVelocity": 1.8353462748055114,
-                    "velocityX": 0.8199690351433494,
-                    "velocityY": 1.2078241619810486,
-                    "timestamp": 1.052662779831107
-                },
-                {
-                    "x": 2.2830654942840813,
-                    "y": 6.732454760660193,
-                    "heading": 3.492119269429147,
-                    "angularVelocity": 1.2846933003974643,
-                    "velocityX": 0.5468184952596451,
-                    "velocityY": 0.7981240411069244,
-                    "timestamp": 1.1145841198211721
+                    "x": 0.7269596629981327,
+                    "y": 4.384958058102961,
+                    "heading": -1.0015712045971708,
+                    "angularVelocity": 0.6206072078812727,
+                    "velocityX": 0.27389068508516395,
+                    "velocityY": 0.40306105333388875,
+                    "timestamp": 0.06192128437248337
+                },
+                {
+                    "x": 0.7608459539039729,
+                    "y": 4.435118328610172,
+                    "heading": -0.9264796986002697,
+                    "angularVelocity": 1.212692965882186,
+                    "velocityX": 0.5472478687941834,
+                    "velocityY": 0.8100650853021081,
+                    "timestamp": 0.12384256874496674
+                },
+                {
+                    "x": 0.8116291734017863,
+                    "y": 4.510832058036491,
+                    "heading": -0.8178101616266298,
+                    "angularVelocity": 1.754962579909447,
+                    "velocityX": 0.8201254223399239,
+                    "velocityY": 1.2227415854436723,
+                    "timestamp": 0.1857638531174501
+                },
+                {
+                    "x": 0.8793445718640878,
+                    "y": 4.61260707850531,
+                    "heading": -0.6817312766556918,
+                    "angularVelocity": 2.1976108271973906,
+                    "velocityX": 1.0935722530392615,
+                    "velocityY": 1.6436193386525904,
+                    "timestamp": 0.24768513748993348
+                },
+                {
+                    "x": 0.9640980850122779,
+                    "y": 4.741096013070982,
+                    "heading": -0.5303097082973756,
+                    "angularVelocity": 2.445388042138302,
+                    "velocityX": 1.368729896465987,
+                    "velocityY": 2.0750366512547602,
+                    "timestamp": 0.3096064218624168
+                },
+                {
+                    "x": 1.0654131930810256,
+                    "y": 4.896663967052664,
+                    "heading": -0.3925803189619664,
+                    "angularVelocity": 2.2242657065526474,
+                    "velocityX": 1.6361919668735143,
+                    "velocityY": 2.512350245286791,
+                    "timestamp": 0.37152770623490017
+                },
+                {
+                    "x": 1.1790960248711007,
+                    "y": 5.075186526688609,
+                    "heading": -0.32893475345024337,
+                    "angularVelocity": 1.0278463400220732,
+                    "velocityX": 1.8359249641241828,
+                    "velocityY": 2.8830564715365803,
+                    "timestamp": 0.4334489906073835
+                },
+                {
+                    "x": 1.3095059591702463,
+                    "y": 5.269772606119823,
+                    "heading": -0.32893468266066545,
+                    "angularVelocity": 0.0000011432188240302224,
+                    "velocityX": 2.106059905260899,
+                    "velocityY": 3.1424748598671592,
+                    "timestamp": 0.49537027497986685
+                },
+                {
+                    "x": 1.439916398287199,
+                    "y": 5.464358347226451,
+                    "heading": -0.3289346118734761,
+                    "angularVelocity": 0.0000011431802504043458,
+                    "velocityX": 2.106068057834158,
+                    "velocityY": 3.14246939608213,
+                    "timestamp": 0.5572915593523502
+                },
+                {
+                    "x": 1.5703268751378339,
+                    "y": 5.65894406304414,
+                    "heading": -0.32893454108629183,
+                    "angularVelocity": 0.0000011431801680911917,
+                    "velocityX": 2.1060686672156135,
+                    "velocityY": 3.142468987677516,
+                    "timestamp": 0.6192128437248335
+                },
+                {
+                    "x": 1.700737390180202,
+                    "y": 5.853529753265897,
+                    "heading": -0.3289344702991123,
+                    "angularVelocity": 0.0000011431800901048107,
+                    "velocityX": 2.1060692839943727,
+                    "velocityY": 3.1424685743150773,
+                    "timestamp": 0.6811341280973169
+                },
+                {
+                    "x": 1.8311473976991637,
+                    "y": 6.048115783625596,
+                    "heading": -0.32893439950904224,
+                    "angularVelocity": 0.000001143226773133006,
+                    "velocityX": 2.1060610877269452,
+                    "velocityY": 3.1424740673849674,
+                    "timestamp": 0.7430554124698002
+                },
+                {
+                    "x": 1.9446375646824816,
+                    "y": 6.226114798697167,
+                    "heading": -0.26580646349628556,
+                    "angularVelocity": 1.0194868638869756,
+                    "velocityX": 1.8328135169261883,
+                    "velocityY": 2.8746014698408153,
+                    "timestamp": 0.8049766968422836
+                },
+                {
+                    "x": 2.0457805429701614,
+                    "y": 6.3800476604210195,
+                    "heading": -0.13750031114094655,
+                    "angularVelocity": 2.072084803401717,
+                    "velocityX": 1.6334121508084516,
+                    "velocityY": 2.485944264299801,
+                    "timestamp": 0.8668979812147669
+                },
+                {
+                    "x": 2.130677229255867,
+                    "y": 6.507493779703991,
+                    "heading": 0.017637122988449796,
+                    "angularVelocity": 2.505397549511044,
+                    "velocityX": 1.3710420761787727,
+                    "velocityY": 2.0581956684930476,
+                    "timestamp": 0.9288192655872503
+                },
+                {
+                    "x": 2.198423808533576,
+                    "y": 6.608249828696692,
+                    "heading": 0.1586985563433061,
+                    "angularVelocity": 2.2780766707988582,
+                    "velocityX": 1.0940758087345794,
+                    "velocityY": 1.6271634222993538,
+                    "timestamp": 0.9907405499597336
+                },
+                {
+                    "x": 2.2492018276993044,
+                    "y": 6.683037485807007,
+                    "heading": 0.27235601520130115,
+                    "angularVelocity": 1.8355152030486992,
+                    "velocityX": 0.8200414393906406,
+                    "velocityY": 1.2077859474043515,
+                    "timestamp": 1.052661834332217
+                },
+                {
+                    "x": 2.2830642005186585,
+                    "y": 6.732456177807481,
+                    "heading": 0.3519224237627513,
+                    "angularVelocity": 1.2849605651398268,
+                    "velocityX": 0.5468616027997278,
+                    "velocityY": 0.7980889366441329,
+                    "timestamp": 1.1145831187047004
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": 3.5331853071795862,
-                    "angularVelocity": 0.66319685195811,
-                    "velocityX": 0.2734841610119513,
-                    "velocityY": 0.3963938658908936,
-                    "timestamp": 1.1765054598112372
+                    "heading": 0.393,
+                    "angularVelocity": 0.6633837888463243,
+                    "velocityX": 0.27350530036594517,
+                    "velocityY": 0.39637133566025207,
+                    "timestamp": 1.1765044030771838
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": 3.5331853071795862,
-                    "angularVelocity": -1.1911850535673714e-30,
+                    "heading": 0.393,
+                    "angularVelocity": 4.204601117767945e-31,
                     "velocityX": 0,
-                    "velocityY": -7.498214836328326e-32,
-                    "timestamp": 1.2384267998013023
+                    "velocityY": -1.1377088902550092e-31,
+                    "timestamp": 1.2384256874496673
                 }
             ],
             "constraints": [
@@ -6181,7 +6169,7 @@
                 {
                     "x": 0.71,
                     "y": 4.36,
-                    "heading": 2.1,
+                    "heading": -1.04,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -6190,7 +6178,7 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -6201,146 +6189,146 @@
                 {
                     "x": 0.71,
                     "y": 4.36,
-                    "heading": 2.1,
-                    "angularVelocity": 8.845615831720266e-31,
-                    "velocityX": 5.520878611448458e-31,
-                    "velocityY": 4.542986326160959e-31,
+                    "heading": -1.04,
+                    "angularVelocity": 1.681543952112751e-30,
+                    "velocityX": -3.802997916124736e-32,
+                    "velocityY": 9.397294493952762e-34,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7374422286617038,
-                    "y": 4.3811878915004145,
-                    "heading": 2.1328463142733396,
-                    "angularVelocity": 0.5012247413747498,
-                    "velocityX": 0.41876004258027855,
-                    "velocityY": 0.3233207643693322,
-                    "timestamp": 0.06553210877669326
-                },
-                {
-                    "x": 0.7924456920447251,
-                    "y": 4.423718037322063,
-                    "heading": 2.19563146481745,
-                    "angularVelocity": 0.9580822548845105,
-                    "velocityX": 0.8393360813467762,
-                    "velocityY": 0.6489970583210544,
-                    "timestamp": 0.13106421755338651
-                },
-                {
-                    "x": 0.8751785876629536,
-                    "y": 4.487773457465306,
-                    "heading": 2.284091563401004,
-                    "angularVelocity": 1.3498741339912115,
-                    "velocityX": 1.262478762893885,
-                    "velocityY": 0.9774661816777632,
-                    "timestamp": 0.19659632633007978
-                },
-                {
-                    "x": 0.985868892669873,
-                    "y": 4.573562834319481,
-                    "heading": 2.3916808862774843,
-                    "angularVelocity": 1.641780264436542,
-                    "velocityX": 1.6891003062957237,
-                    "velocityY": 1.309119734671004,
-                    "timestamp": 0.26212843510677303
-                },
-                {
-                    "x": 1.1247521909380378,
-                    "y": 4.681315175779475,
-                    "heading": 2.507240555710109,
-                    "angularVelocity": 1.7634053228228717,
-                    "velocityX": 2.1193167877661256,
-                    "velocityY": 1.6442678783186102,
-                    "timestamp": 0.32766054388346627
-                },
-                {
-                    "x": 1.291492862531941,
-                    "y": 4.810942835643569,
-                    "heading": 2.6039577624805075,
-                    "angularVelocity": 1.4758750874318616,
-                    "velocityX": 2.5444118113470626,
-                    "velocityY": 1.9780785676502115,
-                    "timestamp": 0.3931926526601595
-                },
-                {
-                    "x": 1.4800025601939901,
-                    "y": 4.960730002903915,
-                    "heading": 2.6228917229305413,
-                    "angularVelocity": 0.2889264637363189,
-                    "velocityX": 2.8766005120392,
-                    "velocityY": 2.28570650413168,
-                    "timestamp": 0.45872476143685276
-                },
-                {
-                    "x": 1.6682727616271493,
-                    "y": 5.110599963848934,
-                    "heading": 2.6420440088534156,
-                    "angularVelocity": 0.2922580438871624,
-                    "velocityX": 2.8729458726059685,
-                    "velocityY": 2.2869699105167616,
-                    "timestamp": 0.5242568702135461
-                },
-                {
-                    "x": 1.8350043100801503,
-                    "y": 5.239747952271642,
-                    "heading": 2.7356929069056295,
-                    "angularVelocity": 1.429053631881899,
-                    "velocityX": 2.544272594998539,
-                    "velocityY": 1.9707589277004427,
-                    "timestamp": 0.5897889789902393
-                },
-                {
-                    "x": 1.9739563679504606,
-                    "y": 5.347167856813287,
-                    "heading": 2.8514085217331835,
-                    "angularVelocity": 1.765785002003577,
-                    "velocityX": 2.1203660383309844,
-                    "velocityY": 1.6391949923004236,
-                    "timestamp": 0.6553210877669325
-                },
-                {
-                    "x": 2.0846918002256074,
-                    "y": 5.4326634298654595,
-                    "heading": 2.9588833399365435,
-                    "angularVelocity": 1.640032958035698,
-                    "velocityX": 1.6897889346501833,
-                    "velocityY": 1.304636378229562,
-                    "timestamp": 0.7208531965436258
-                },
-                {
-                    "x": 2.1674771737861436,
-                    "y": 5.496490038501947,
-                    "heading": 3.046413104094783,
-                    "angularVelocity": 1.3356775143083182,
-                    "velocityX": 1.2632795602936622,
-                    "velocityY": 0.9739745878464279,
-                    "timestamp": 0.786385305320319
-                },
-                {
-                    "x": 2.2225294464291743,
-                    "y": 5.538874362150552,
-                    "heading": 3.1080101238304096,
-                    "angularVelocity": 0.9399517409934369,
-                    "velocityX": 0.8400808957732031,
-                    "velocityY": 0.6467718564197722,
-                    "timestamp": 0.8519174140970123
+                    "x": 0.7374413965489649,
+                    "y": 4.381188295193423,
+                    "heading": -1.0071421527847422,
+                    "angularVelocity": 0.5013988353179083,
+                    "velocityX": 0.41874576197914515,
+                    "velocityY": 0.3233257024648049,
+                    "timestamp": 0.06553235648109366
+                },
+                {
+                    "x": 0.7924436098605977,
+                    "y": 4.423719284711624,
+                    "heading": -0.9443396170050365,
+                    "angularVelocity": 0.9583439258410564,
+                    "velocityX": 0.8393138331215114,
+                    "velocityY": 0.6490074796939781,
+                    "timestamp": 0.13106471296218733
+                },
+                {
+                    "x": 0.8751753442261428,
+                    "y": 4.487775925735343,
+                    "heading": -0.8558629917892655,
+                    "angularVelocity": 1.350121222045466,
+                    "velocityX": 1.2624562705815747,
+                    "velocityY": 0.9774811171669098,
+                    "timestamp": 0.19659706944328098
+                },
+                {
+                    "x": 0.9858650513706745,
+                    "y": 4.5735667081068,
+                    "heading": -0.748264583846062,
+                    "angularVelocity": 1.641912693529484,
+                    "velocityX": 1.6890847985371011,
+                    "velocityY": 1.3091362340405182,
+                    "timestamp": 0.26212942592437466
+                },
+                {
+                    "x": 1.1247485645977424,
+                    "y": 4.681320324271238,
+                    "heading": -0.6327125157113742,
+                    "angularVelocity": 1.763282664312989,
+                    "velocityX": 2.1193120571993105,
+                    "velocityY": 1.6442811147120544,
+                    "timestamp": 0.32766178240546834
+                },
+                {
+                    "x": 1.291488910381981,
+                    "y": 4.810947401273882,
+                    "heading": -0.5360470717928266,
+                    "angularVelocity": 1.4750796264503891,
+                    "velocityX": 2.544397222040073,
+                    "velocityY": 1.9780621964975005,
+                    "timestamp": 0.393194138886562
+                },
+                {
+                    "x": 1.4799946113507723,
+                    "y": 4.960741770751019,
+                    "heading": -0.5171262553925533,
+                    "angularVelocity": 0.2887247981954086,
+                    "velocityX": 2.8765286507463124,
+                    "velocityY": 2.2858077676538255,
+                    "timestamp": 0.4587264953676557
+                },
+                {
+                    "x": 1.6682616674549375,
+                    "y": 5.11061398659558,
+                    "heading": -0.49798039258281773,
+                    "angularVelocity": 0.29215892480933153,
+                    "velocityX": 2.8728870166371596,
+                    "velocityY": 2.2869956749959233,
+                    "timestamp": 0.5242588518487493
+                },
+                {
+                    "x": 1.8349961429002288,
+                    "y": 5.239757672124722,
+                    "heading": -0.4043635195675931,
+                    "angularVelocity": 1.4285595397783832,
+                    "velocityX": 2.5443076427962366,
+                    "velocityY": 1.970685817873752,
+                    "timestamp": 0.589791208329843
+                },
+                {
+                    "x": 1.9739512346286359,
+                    "y": 5.34717518731749,
+                    "heading": -0.2886497176580553,
+                    "angularVelocity": 1.7657506630777153,
+                    "velocityX": 2.120404319177778,
+                    "velocityY": 1.6391523357435742,
+                    "timestamp": 0.6553235648109367
+                },
+                {
+                    "x": 2.084688896220359,
+                    "y": 5.432668536845291,
+                    "heading": -0.1811622358281448,
+                    "angularVelocity": 1.6402200012587402,
+                    "velocityX": 1.6898165660145774,
+                    "velocityY": 1.304597516685743,
+                    "timestamp": 0.7208559212920304
+                },
+                {
+                    "x": 2.167475787005346,
+                    "y": 5.496492971314002,
+                    "heading": -0.09361417394502371,
+                    "angularVelocity": 1.3359516822560165,
+                    "velocityX": 1.2632979375443678,
+                    "velocityY": 0.97393772932798,
+                    "timestamp": 0.786388277773124
+                },
+                {
+                    "x": 2.222528989324329,
+                    "y": 5.538875463156123,
+                    "heading": -0.0320000732344696,
+                    "angularVelocity": 0.9402088375736564,
+                    "velocityX": 0.8400919068867495,
+                    "velocityY": 0.6467414589974029,
+                    "timestamp": 0.8519206342542177
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0.4881557570289305,
-                    "velocityX": 0.41919227205757975,
-                    "velocityY": 0.3223707926359362,
-                    "timestamp": 0.9174495228737055
+                    "heading": 0,
+                    "angularVelocity": 0.48830951537200207,
+                    "velocityX": 0.41919766281556964,
+                    "velocityY": 0.32235277316741867,
+                    "timestamp": 0.9174529907353114
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 7.973690679262731e-27,
-                    "velocityX": -1.618419533015103e-29,
-                    "velocityY": -2.1923403816587234e-30,
-                    "timestamp": 0.9829816316503988
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": 1.4283065738497147e-31,
+                    "velocityY": -2.9738366407041548e-31,
+                    "timestamp": 0.9829853472164051
                 }
             ],
             "constraints": [
@@ -6383,7 +6371,7 @@
                 {
                     "x": 0.71,
                     "y": 4.36,
-                    "heading": 2.1,
+                    "heading": -1.04,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -6392,7 +6380,7 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -6403,119 +6391,119 @@
                 {
                     "x": 0.71,
                     "y": 4.36,
-                    "heading": 2.1,
-                    "angularVelocity": -5.8020277899403155e-31,
-                    "velocityX": 1.3777732804976719e-32,
-                    "velocityY": 5.338313673616136e-29,
+                    "heading": -1.04,
+                    "angularVelocity": -7.700576656700109e-31,
+                    "velocityX": 0,
+                    "velocityY": -1.136023856217491e-30,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7541175647990941,
-                    "y": 4.360036481014467,
-                    "heading": 2.123748591233672,
-                    "angularVelocity": 0.32465550054626485,
-                    "velocityX": 0.603109883101075,
-                    "velocityY": 0.0004987142982711039,
-                    "timestamp": 0.07315012742330113
-                },
-                {
-                    "x": 0.8424087590063745,
-                    "y": 4.360099698646948,
-                    "heading": 2.1698601870465493,
-                    "angularVelocity": 0.6303693163234944,
-                    "velocityX": 1.2069861983474726,
-                    "velocityY": 0.0008642176672118523,
-                    "timestamp": 0.14630025484660225
-                },
-                {
-                    "x": 0.9749550737228709,
-                    "y": 4.360177642554779,
-                    "heading": 2.23617090700284,
-                    "angularVelocity": 0.906501769606733,
-                    "velocityX": 1.8119765390084532,
-                    "velocityY": 0.0010655334525686096,
-                    "timestamp": 0.21945038226990338
-                },
-                {
-                    "x": 1.1518842000389728,
-                    "y": 4.360257435104989,
-                    "heading": 2.318743489919177,
-                    "angularVelocity": 1.1288098302053016,
-                    "velocityX": 2.4187124827857356,
-                    "velocityY": 0.0010908053480699021,
-                    "timestamp": 0.2926005096932045
-                },
-                {
-                    "x": 1.3733949578522506,
-                    "y": 4.360320997027151,
-                    "heading": 2.4076375641139776,
-                    "angularVelocity": 1.2152278789650626,
-                    "velocityX": 3.028166397188238,
-                    "velocityY": 0.0008689242848102628,
-                    "timestamp": 0.36575063711650563
-                },
-                {
-                    "x": 1.6366157123115315,
-                    "y": 4.3597910751743765,
-                    "heading": 2.4407985098803153,
-                    "angularVelocity": 0.45332724541279523,
-                    "velocityX": 3.5983635809144485,
-                    "velocityY": -0.007244305258449855,
-                    "timestamp": 0.43890076453980675
-                },
-                {
-                    "x": 1.8581231700320338,
-                    "y": 4.359897336997892,
-                    "heading": 2.530170275911139,
-                    "angularVelocity": 1.2217581729403981,
-                    "velocityX": 3.0281212832164655,
-                    "velocityY": 0.0014526539769375909,
-                    "timestamp": 0.5120508919631079
-                },
-                {
-                    "x": 2.0350502325675994,
-                    "y": 4.359957426055228,
-                    "heading": 2.6131109520937392,
-                    "angularVelocity": 1.1338418551586247,
-                    "velocityX": 2.4186842698404125,
-                    "velocityY": 0.000821448430104839,
-                    "timestamp": 0.585201019386409
-                },
-                {
-                    "x": 2.1675938018574246,
-                    "y": 4.359986783385126,
-                    "heading": 2.6797755559230114,
-                    "angularVelocity": 0.9113395448181003,
-                    "velocityX": 1.8119390076086361,
-                    "velocityY": 0.00040132985313166364,
-                    "timestamp": 0.6583511468097101
-                },
-                {
-                    "x": 2.255882928551814,
-                    "y": 4.359997887713372,
-                    "heading": 2.726140403287784,
-                    "angularVelocity": 0.6338313957609201,
-                    "velocityX": 1.2069579343789958,
-                    "velocityY": 0.00015180189885440488,
-                    "timestamp": 0.7315012742330111
+                    "x": 0.7541176222366596,
+                    "y": 4.360036691224542,
+                    "heading": -1.0162545462543013,
+                    "angularVelocity": 0.3246127495796489,
+                    "velocityX": 0.6031109286246202,
+                    "velocityY": 0.0005015881949855901,
+                    "timestamp": 0.07315009584931663
+                },
+                {
+                    "x": 0.8424088840100864,
+                    "y": 4.360100335716613,
+                    "heading": -0.970147925779941,
+                    "angularVelocity": 0.630301572936589,
+                    "velocityX": 1.2069876429868645,
+                    "velocityY": 0.0008700534337042256,
+                    "timestamp": 0.14630019169863326
+                },
+                {
+                    "x": 0.9749552173528473,
+                    "y": 4.360178948568722,
+                    "heading": -0.9038422068087609,
+                    "angularVelocity": 0.9064337948053328,
+                    "velocityX": 1.8119775757478125,
+                    "velocityY": 0.0010746787299806282,
+                    "timestamp": 0.21945028754794987
+                },
+                {
+                    "x": 1.151884255871623,
+                    "y": 4.36025970565234,
+                    "heading": -0.8212723355896727,
+                    "angularVelocity": 1.1287732471196155,
+                    "velocityX": 2.4187123265462183,
+                    "velocityY": 0.0011039914941527397,
+                    "timestamp": 0.2926003833972665
+                },
+                {
+                    "x": 1.3733948183653495,
+                    "y": 4.360324587725535,
+                    "heading": -0.7323747736104,
+                    "angularVelocity": 1.2152760833319434,
+                    "velocityX": 3.028165034124097,
+                    "velocityY": 0.000886971813693119,
+                    "timestamp": 0.36575047924658316
+                },
+                {
+                    "x": 1.6366160137298507,
+                    "y": 4.3597898231256424,
+                    "heading": -0.6992108014826405,
+                    "angularVelocity": 0.45336881302434434,
+                    "velocityX": 3.598371161490932,
+                    "velocityY": -0.007310511266700298,
+                    "timestamp": 0.4389005750958998
+                },
+                {
+                    "x": 1.858123270171759,
+                    "y": 4.359896131611675,
+                    "heading": -0.6098312657316373,
+                    "angularVelocity": 1.2218649164225759,
+                    "velocityX": 3.02811983866963,
+                    "velocityY": 0.001453292504923468,
+                    "timestamp": 0.5120506709452164
+                },
+                {
+                    "x": 2.0350502388104363,
+                    "y": 4.359956556106789,
+                    "heading": -0.5268889356205209,
+                    "angularVelocity": 1.1338649546283055,
+                    "velocityX": 2.4186840302045134,
+                    "velocityY": 0.000826034394207285,
+                    "timestamp": 0.585200766794533
+                },
+                {
+                    "x": 2.167593784990145,
+                    "y": 4.359986293011168,
+                    "heading": -0.460224121535325,
+                    "angularVelocity": 0.9113428124904321,
+                    "velocityX": 1.8119394737736823,
+                    "velocityY": 0.0004065190076972891,
+                    "timestamp": 0.6583508626438497
+                },
+                {
+                    "x": 2.2558829179021025,
+                    "y": 4.359997710834851,
+                    "heading": -0.41385943895752736,
+                    "angularVelocity": 0.6338294166193973,
+                    "velocityX": 1.206958540339091,
+                    "velocityY": 0.00015608761065604354,
+                    "timestamp": 0.7315009584931663
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": 0.3261730027365948,
-                    "velocityX": 0.6031031387394808,
-                    "velocityY": 0.000028876048490911067,
-                    "timestamp": 0.8046514016563122
+                    "heading": -0.39,
+                    "angularVelocity": 0.32617098693462643,
+                    "velocityX": 0.6031035446457226,
+                    "velocityY": 0.00003129408268678862,
+                    "timestamp": 0.804651054342483
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": 1.0612763050961144e-31,
-                    "velocityX": 9.233550959236252e-32,
-                    "velocityY": 1.483165202160369e-28,
-                    "timestamp": 0.8778015290796133
+                    "heading": -0.39,
+                    "angularVelocity": 3.953163438305433e-30,
+                    "velocityX": 7.397518409454568e-32,
+                    "velocityY": 0,
+                    "timestamp": 0.8778011501917996
                 }
             ],
             "constraints": [
@@ -6558,16 +6546,16 @@
                 {
                     "x": 0.71,
                     "y": 4.36,
-                    "heading": 2.1,
+                    "heading": -1.04,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 25
+                    "controlIntervalCount": 35
                 },
                 {
                     "x": 3.69740891456604,
                     "y": 6.1177659034729,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -6576,7 +6564,7 @@
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -6587,524 +6575,614 @@
                 {
                     "x": 0.71,
                     "y": 4.36,
-                    "heading": 2.1,
+                    "heading": -1.04,
                     "angularVelocity": 0,
                     "velocityX": 0,
-                    "velocityY": 3.1166835341708484e-33,
+                    "velocityY": 5.951430910881942e-24,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7240863087605185,
-                    "y": 4.368337736173442,
-                    "heading": 2.1163001020338092,
-                    "angularVelocity": 0.3614478974621331,
-                    "velocityX": 0.31235796401343735,
-                    "velocityY": 0.1848857880296264,
-                    "timestamp": 0.045096685160595056
-                },
-                {
-                    "x": 0.7523050752299821,
-                    "y": 4.38505134365204,
-                    "heading": 2.148025504039376,
-                    "angularVelocity": 0.7034974276399452,
-                    "velocityX": 0.6257392615216195,
-                    "velocityY": 0.3706172065436647,
-                    "timestamp": 0.09019337032119011
-                },
-                {
-                    "x": 0.7947129681201482,
-                    "y": 4.410184705499472,
-                    "heading": 2.1940580950016866,
-                    "angularVelocity": 1.020753316976243,
-                    "velocityX": 0.9403771638457716,
-                    "velocityY": 0.5573217135124126,
-                    "timestamp": 0.13529005548178516
-                },
-                {
-                    "x": 0.8513807226870724,
-                    "y": 4.44378606946023,
-                    "heading": 2.252941060288208,
-                    "angularVelocity": 1.3057049554048448,
-                    "velocityX": 1.25658359068129,
-                    "velocityY": 0.7450960938946717,
-                    "timestamp": 0.18038674064238022
-                },
-                {
-                    "x": 0.922394851892513,
-                    "y": 4.485907452258627,
-                    "heading": 2.32274249990232,
-                    "angularVelocity": 1.5478175250695256,
-                    "velocityX": 1.5747084060070078,
-                    "velocityY": 0.9340239232306716,
-                    "timestamp": 0.22548342580297528
-                },
-                {
-                    "x": 1.0078549434585182,
-                    "y": 4.536606503403333,
-                    "heading": 2.4007952890388493,
-                    "angularVelocity": 1.7307877254963087,
-                    "velocityX": 1.8950415371256468,
-                    "velocityY": 1.1242301061410762,
-                    "timestamp": 0.2705801109635703
-                },
-                {
-                    "x": 1.1078580204781514,
-                    "y": 4.595947756684704,
-                    "heading": 2.4830211624583747,
-                    "angularVelocity": 1.823324111887792,
-                    "velocityX": 2.2175261144695955,
-                    "velocityY": 1.315867298672826,
-                    "timestamp": 0.3156767961241654
-                },
-                {
-                    "x": 1.2224032158634355,
-                    "y": 4.663956921963176,
-                    "heading": 2.561776708325237,
-                    "angularVelocity": 1.7463710600103812,
-                    "velocityX": 2.5399914645028585,
-                    "velocityY": 1.5080745965314675,
-                    "timestamp": 0.36077348128476044
-                },
-                {
-                    "x": 1.3501227544876644,
-                    "y": 4.740405284602277,
-                    "heading": 2.616849964946217,
-                    "angularVelocity": 1.2212262702869785,
-                    "velocityX": 2.8321269771692363,
-                    "velocityY": 1.695210243654476,
-                    "timestamp": 0.4058701664453555
-                },
-                {
-                    "x": 1.4890564849563437,
-                    "y": 4.825393782343559,
-                    "heading": 2.6364418869926514,
-                    "angularVelocity": 0.43444261982150123,
-                    "velocityX": 3.0807969582225083,
-                    "velocityY": 1.884584142684248,
-                    "timestamp": 0.45096685160595057
-                },
-                {
-                    "x": 1.6359707555642846,
-                    "y": 4.9121110990754175,
-                    "heading": 2.6364419350884667,
-                    "angularVelocity": 0.0000010665044506613552,
-                    "velocityX": 3.257762074634985,
-                    "velocityY": 1.9229199756711164,
-                    "timestamp": 0.49606353676654563
-                },
-                {
-                    "x": 1.7828851010176074,
-                    "y": 4.99882828900635,
-                    "heading": 2.63644198318385,
-                    "angularVelocity": 0.000001066494868779716,
-                    "velocityX": 3.257763734299801,
-                    "velocityY": 1.9229171639139742,
-                    "timestamp": 0.5411602219271406
-                },
-                {
-                    "x": 1.929799446473537,
-                    "y": 5.085545478932866,
-                    "heading": 2.6364420312792327,
-                    "angularVelocity": 0.0000010664948614258307,
-                    "velocityX": 3.257763734357611,
-                    "velocityY": 1.9229171638160338,
-                    "timestamp": 0.5862569070877357
-                },
-                {
-                    "x": 2.076713791929467,
-                    "y": 5.172262668859381,
-                    "heading": 2.636442079374616,
-                    "angularVelocity": 0.000001066494855048045,
-                    "velocityX": 3.257763734357614,
-                    "velocityY": 1.922917163816031,
-                    "timestamp": 0.6313535922483308
-                },
-                {
-                    "x": 2.2236281373853966,
-                    "y": 5.258979858785897,
-                    "heading": 2.6364421274699983,
-                    "angularVelocity": 0.000001066494848970006,
-                    "velocityX": 3.257763734357614,
-                    "velocityY": 1.9229171638160298,
-                    "timestamp": 0.6764502774089258
-                },
-                {
-                    "x": 2.3705424828413264,
-                    "y": 5.345697048712413,
-                    "heading": 2.6364421755653806,
-                    "angularVelocity": 0.0000010664948432023413,
-                    "velocityX": 3.257763734357615,
-                    "velocityY": 1.9229171638160298,
-                    "timestamp": 0.7215469625695209
-                },
-                {
-                    "x": 2.5174568282972563,
-                    "y": 5.432414238638929,
-                    "heading": 2.6364422236607625,
-                    "angularVelocity": 0.000001066494835980498,
-                    "velocityX": 3.2577637343576162,
-                    "velocityY": 1.9229171638160294,
-                    "timestamp": 0.766643647730116
-                },
-                {
-                    "x": 2.6643711737531857,
-                    "y": 5.519131428565444,
-                    "heading": 2.636442271756144,
-                    "angularVelocity": 0.0000010664948304508311,
-                    "velocityX": 3.2577637343576167,
-                    "velocityY": 1.9229171638160292,
-                    "timestamp": 0.811740332890711
-                },
-                {
-                    "x": 2.8112855192091155,
-                    "y": 5.60584861849196,
-                    "heading": 2.6364423198515254,
-                    "angularVelocity": 0.0000010664948245159241,
-                    "velocityX": 3.257763734357617,
-                    "velocityY": 1.922917163816029,
-                    "timestamp": 0.8568370180513061
-                },
-                {
-                    "x": 2.958199864665046,
-                    "y": 5.692565808418475,
-                    "heading": 2.6364423679469065,
-                    "angularVelocity": 0.0000010664948181966642,
-                    "velocityX": 3.2577637343576185,
-                    "velocityY": 1.922917163816028,
-                    "timestamp": 0.9019337032119011
-                },
-                {
-                    "x": 3.1051142101209757,
-                    "y": 5.779282998344991,
-                    "heading": 2.636442416042287,
-                    "angularVelocity": 0.0000010664948111950998,
-                    "velocityX": 3.257763734357619,
-                    "velocityY": 1.9229171638160278,
-                    "timestamp": 0.9470303883724962
-                },
-                {
-                    "x": 3.252028555576906,
-                    "y": 5.8660001882715065,
-                    "heading": 2.6364424641376676,
-                    "angularVelocity": 0.0000010664948058295197,
-                    "velocityX": 3.2577637343576273,
-                    "velocityY": 1.922917163816015,
-                    "timestamp": 0.9921270735330913
-                },
-                {
-                    "x": 3.3989429010423815,
-                    "y": 5.952717378181851,
-                    "heading": 2.636442512233048,
-                    "angularVelocity": 0.00000106649479979568,
-                    "velocityX": 3.257763734569281,
-                    "velocityY": 1.9229171634574358,
-                    "timestamp": 1.0372237586936863
-                },
-                {
-                    "x": 3.5458575205338394,
-                    "y": 6.039434103841755,
-                    "heading": 2.636442560328411,
-                    "angularVelocity": 0.0000010664944172141213,
-                    "velocityX": 3.2577698109800433,
-                    "velocityY": 1.9229068688994477,
-                    "timestamp": 1.0823204438542813
+                    "x": 0.7172662637575132,
+                    "y": 4.364294720467062,
+                    "heading": -1.0315470131742317,
+                    "angularVelocity": 0.26098153509522004,
+                    "velocityX": 0.22434208273715575,
+                    "velocityY": 0.13259724206381326,
+                    "timestamp": 0.03238921413610362
+                },
+                {
+                    "x": 0.7318144654254066,
+                    "y": 4.3728967855708385,
+                    "heading": -1.0149312072454106,
+                    "angularVelocity": 0.5130042939294357,
+                    "velocityX": 0.4491680967238058,
+                    "velocityY": 0.2655842487449578,
+                    "timestamp": 0.06477842827220724
+                },
+                {
+                    "x": 0.7536624687467911,
+                    "y": 4.385820612299006,
+                    "heading": -0.9904958605622504,
+                    "angularVelocity": 0.7544285137786853,
+                    "velocityX": 0.6745456444104029,
+                    "velocityY": 0.39901637235962667,
+                    "timestamp": 0.09716764240831086
+                },
+                {
+                    "x": 0.7828310955502448,
+                    "y": 4.4030821899572965,
+                    "heading": -0.9586530378385134,
+                    "angularVelocity": 0.9831304516969581,
+                    "velocityX": 0.9005660551343861,
+                    "velocityY": 0.5329421574032179,
+                    "timestamp": 0.12955685654441448
+                },
+                {
+                    "x": 0.8193448564336872,
+                    "y": 4.424698840047832,
+                    "heading": -0.919902879162077,
+                    "angularVelocity": 1.1963908266993857,
+                    "velocityX": 1.1273432177146,
+                    "velocityY": 0.6674027347406944,
+                    "timestamp": 0.1619460706805181
+                },
+                {
+                    "x": 0.8632324943492647,
+                    "y": 4.450689040734006,
+                    "heading": -0.8748582540701377,
+                    "angularVelocity": 1.3907291761589549,
+                    "velocityX": 1.3550078038681697,
+                    "velocityY": 0.8024338156819394,
+                    "timestamp": 0.19433528481662174
+                },
+                {
+                    "x": 0.9145271224696989,
+                    "y": 4.481072488182425,
+                    "heading": -0.8242801160685745,
+                    "angularVelocity": 1.5615734852049024,
+                    "velocityX": 1.5836947418633758,
+                    "velocityY": 0.9380730054376618,
+                    "timestamp": 0.22672449895272537
+                },
+                {
+                    "x": 0.9732655893831887,
+                    "y": 4.515870568727518,
+                    "heading": -0.7691375276578237,
+                    "angularVelocity": 1.7024984977725797,
+                    "velocityX": 1.8135193606940647,
+                    "velocityY": 1.0743724870528544,
+                    "timestamp": 0.25911371308882897
+                },
+                {
+                    "x": 1.0394861119792222,
+                    "y": 4.555106942812012,
+                    "heading": -0.7107271690719036,
+                    "angularVelocity": 1.8033891881560444,
+                    "velocityX": 2.044523905944935,
+                    "velocityY": 1.2114024724285466,
+                    "timestamp": 0.2915029272249326
+                },
+                {
+                    "x": 1.1132202582415271,
+                    "y": 4.598805444215395,
+                    "heading": -0.6509416110612904,
+                    "angularVelocity": 1.8458477491731227,
+                    "velocityX": 2.276503096137642,
+                    "velocityY": 1.3491683132463759,
+                    "timestamp": 0.3238921413610362
+                },
+                {
+                    "x": 1.194458181637227,
+                    "y": 4.646971254862031,
+                    "heading": -0.5929406477835183,
+                    "angularVelocity": 1.790749322723445,
+                    "velocityX": 2.508178279791774,
+                    "velocityY": 1.48709414326129,
+                    "timestamp": 0.35628135549713985
+                },
+                {
+                    "x": 1.2829200414273145,
+                    "y": 4.699514481647374,
+                    "heading": -0.5430956567390244,
+                    "angularVelocity": 1.5389379574026985,
+                    "velocityX": 2.7312135273909255,
+                    "velocityY": 1.6222445708176134,
+                    "timestamp": 0.3886705696332435
+                },
+                {
+                    "x": 1.3771379621947135,
+                    "y": 4.756486240363935,
+                    "heading": -0.5116699283377882,
+                    "angularVelocity": 0.9702528832339478,
+                    "velocityX": 2.908928891311882,
+                    "velocityY": 1.758973171660129,
+                    "timestamp": 0.4210597837693471
+                },
+                {
+                    "x": 1.4771406301349097,
+                    "y": 4.8178582459332,
+                    "heading": -0.4985254849792972,
+                    "angularVelocity": 0.40582779511896505,
+                    "velocityX": 3.087529926474048,
+                    "velocityY": 1.894828485537579,
+                    "timestamp": 0.45344899790545073
+                },
+                {
+                    "x": 1.5826858211522454,
+                    "y": 4.880091207833619,
+                    "heading": -0.49852543793275184,
+                    "angularVelocity": 0.000001452537415626245,
+                    "velocityX": 3.2586524197166784,
+                    "velocityY": 1.9214100607352578,
+                    "timestamp": 0.48583821204155436
+                },
+                {
+                    "x": 1.6882312828144117,
+                    "y": 4.9423237107273525,
+                    "heading": -0.4985253908851185,
+                    "angularVelocity": 0.0000014525710071923091,
+                    "velocityX": 3.2586607757338757,
+                    "velocityY": 1.9213958891446032,
+                    "timestamp": 0.5182274261776579
+                },
+                {
+                    "x": 1.7937767445018398,
+                    "y": 5.004556213578243,
+                    "heading": -0.49852534383749064,
+                    "angularVelocity": 0.0000014525708350435827,
+                    "velocityX": 3.2586607765138327,
+                    "velocityY": 1.921395887821837,
+                    "timestamp": 0.5506166403137616
+                },
+                {
+                    "x": 1.8993222061892705,
+                    "y": 5.06678871642913,
+                    "heading": -0.49852529678986307,
+                    "angularVelocity": 0.0000014525708270038968,
+                    "velocityX": 3.2586607765139086,
+                    "velocityY": 1.9213958878217097,
+                    "timestamp": 0.5830058544498652
+                },
+                {
+                    "x": 2.0048676678767015,
+                    "y": 5.129021219280016,
+                    "heading": -0.4985252497422358,
+                    "angularVelocity": 0.0000014525708183379938,
+                    "velocityX": 3.258660776513909,
+                    "velocityY": 1.9213958878217101,
+                    "timestamp": 0.6153950685859688
+                },
+                {
+                    "x": 2.1104131295641317,
+                    "y": 5.191253722130902,
+                    "heading": -0.49852520269460865,
+                    "angularVelocity": 0.000001452570810915054,
+                    "velocityX": 3.2586607765139104,
+                    "velocityY": 1.9213958878217086,
+                    "timestamp": 0.6477842827220724
+                },
+                {
+                    "x": 2.2159585912515625,
+                    "y": 5.253486224981789,
+                    "heading": -0.4985251556469819,
+                    "angularVelocity": 0.0000014525708028649706,
+                    "velocityX": 3.2586607765139086,
+                    "velocityY": 1.9213958878217123,
+                    "timestamp": 0.6801734968581761
+                },
+                {
+                    "x": 2.321504052938993,
+                    "y": 5.315718727832675,
+                    "heading": -0.4985251085993554,
+                    "angularVelocity": 0.000001452570794356237,
+                    "velocityX": 3.2586607765139117,
+                    "velocityY": 1.9213958878217081,
+                    "timestamp": 0.7125627109942797
+                },
+                {
+                    "x": 2.427049514626424,
+                    "y": 5.3779512306835615,
+                    "heading": -0.49852506155172915,
+                    "angularVelocity": 0.0000014525707870212498,
+                    "velocityX": 3.258660776513912,
+                    "velocityY": 1.9213958878217077,
+                    "timestamp": 0.7449519251303833
+                },
+                {
+                    "x": 2.5325949763138547,
+                    "y": 5.440183733534448,
+                    "heading": -0.4985250145041031,
+                    "angularVelocity": 0.000001452570779107594,
+                    "velocityX": 3.258660776513913,
+                    "velocityY": 1.9213958878217068,
+                    "timestamp": 0.777341139266487
+                },
+                {
+                    "x": 2.6381404380012854,
+                    "y": 5.502416236385335,
+                    "heading": -0.49852496745647734,
+                    "angularVelocity": 0.0000014525707705186945,
+                    "velocityX": 3.258660776513915,
+                    "velocityY": 1.9213958878217055,
+                    "timestamp": 0.8097303534025906
+                },
+                {
+                    "x": 2.743685899688716,
+                    "y": 5.564648739236221,
+                    "heading": -0.4985249204088519,
+                    "angularVelocity": 0.00000145257076360687,
+                    "velocityX": 3.2586607765139153,
+                    "velocityY": 1.9213958878217066,
+                    "timestamp": 0.8421195675386942
+                },
+                {
+                    "x": 2.849231361376147,
+                    "y": 5.626881242087108,
+                    "heading": -0.4985248733612266,
+                    "angularVelocity": 0.0000014525707544682094,
+                    "velocityX": 3.2586607765139153,
+                    "velocityY": 1.9213958878217066,
+                    "timestamp": 0.8745087816747978
+                },
+                {
+                    "x": 2.954776823063578,
+                    "y": 5.689113744937994,
+                    "heading": -0.4985248263136016,
+                    "angularVelocity": 0.0000014525707471414822,
+                    "velocityX": 3.2586607765139166,
+                    "velocityY": 1.921395887821705,
+                    "timestamp": 0.9068979958109015
+                },
+                {
+                    "x": 3.0603222847510096,
+                    "y": 5.751346247788881,
+                    "heading": -0.49852477926597694,
+                    "angularVelocity": 0.0000014525707385370494,
+                    "velocityX": 3.258660776513917,
+                    "velocityY": 1.9213958878217057,
+                    "timestamp": 0.9392872099470051
+                },
+                {
+                    "x": 3.1658677464384404,
+                    "y": 5.813578750639766,
+                    "heading": -0.4985247322183525,
+                    "angularVelocity": 0.0000014525707310188908,
+                    "velocityX": 3.2586607765139175,
+                    "velocityY": 1.9213958878217063,
+                    "timestamp": 0.9716764240831087
+                },
+                {
+                    "x": 3.271413208125871,
+                    "y": 5.875811253490653,
+                    "heading": -0.4985246851707283,
+                    "angularVelocity": 0.0000014525707228586851,
+                    "velocityX": 3.258660776513917,
+                    "velocityY": 1.921395887821708,
+                    "timestamp": 1.0040656382192124
+                },
+                {
+                    "x": 3.376958669813326,
+                    "y": 5.938043756341499,
+                    "heading": -0.4985246381231044,
+                    "angularVelocity": 0.000001452570714543944,
+                    "velocityX": 3.258660776514645,
+                    "velocityY": 1.921395887820474,
+                    "timestamp": 1.0364548523553159
+                },
+                {
+                    "x": 3.482504131751534,
+                    "y": 6.000276258767071,
+                    "heading": -0.49852459107548075,
+                    "angularVelocity": 0.0000014525707064344823,
+                    "velocityX": 3.258660784256519,
+                    "velocityY": 1.9213958746903632,
+                    "timestamp": 1.0688440664914194
+                },
+                {
+                    "x": 3.588052280044538,
+                    "y": 6.0625042049472775,
+                    "heading": -0.4985245440280508,
+                    "angularVelocity": 0.0000014525647243024202,
+                    "velocityX": 3.258743724052004,
+                    "velocityY": 1.921255203004168,
+                    "timestamp": 1.101233280627523
                 },
                 {
                     "x": 3.69740891456604,
                     "y": 6.1177659034729,
-                    "heading": 2.6364426084288106,
-                    "angularVelocity": 0.0000010666061013072793,
-                    "velocityX": 3.3605883335439555,
-                    "velocityY": 1.736974665703161,
-                    "timestamp": 1.1274171290148762
-                },
-                {
-                    "x": 3.8471798789241873,
-                    "y": 6.1780975155460816,
-                    "heading": 2.636442653750165,
-                    "angularVelocity": 0.0000010618213127102396,
-                    "velocityX": 3.5089419861830207,
-                    "velocityY": 1.4134924456482445,
-                    "timestamp": 1.1700997851966806
-                },
-                {
-                    "x": 4.000192250358795,
-                    "y": 6.2296577661788755,
-                    "heading": 2.6364426985309986,
-                    "angularVelocity": 0.0000010491576146038598,
-                    "velocityX": 3.584884004942437,
-                    "velocityY": 1.2079906745535305,
-                    "timestamp": 1.212782441378485
-                },
-                {
-                    "x": 4.153204837360183,
-                    "y": 6.281217377082217,
-                    "heading": 2.6364427433118154,
-                    "angularVelocity": 0.0000010491572166464704,
-                    "velocityX": 3.5848890553961605,
-                    "velocityY": 1.2079756865112976,
-                    "timestamp": 1.2554650975602892
-                },
-                {
-                    "x": 4.306217424370213,
-                    "y": 6.3327769879599085,
-                    "heading": 2.636442788092632,
-                    "angularVelocity": 0.0000010491572158103502,
-                    "velocityX": 3.5848890555986537,
-                    "velocityY": 1.2079756859103623,
-                    "timestamp": 1.2981477537420936
-                },
-                {
-                    "x": 4.459230011380243,
-                    "y": 6.384336598837598,
-                    "heading": 2.6364428328734486,
-                    "angularVelocity": 0.0000010491572148390834,
-                    "velocityX": 3.584889055598662,
-                    "velocityY": 1.2079756859103368,
-                    "timestamp": 1.340830409923898
-                },
-                {
-                    "x": 4.6122425983902735,
-                    "y": 6.435896209715288,
-                    "heading": 2.636442877654265,
-                    "angularVelocity": 0.0000010491572125659184,
-                    "velocityX": 3.584889055598663,
-                    "velocityY": 1.2079756859103368,
-                    "timestamp": 1.3835130661057022
-                },
-                {
-                    "x": 4.765255185400304,
-                    "y": 6.487455820592977,
-                    "heading": 2.636442922435082,
-                    "angularVelocity": 0.0000010491572116366236,
-                    "velocityX": 3.584889055598663,
-                    "velocityY": 1.2079756859103352,
-                    "timestamp": 1.4261957222875066
-                },
-                {
-                    "x": 4.918267772410334,
-                    "y": 6.539015431470667,
-                    "heading": 2.636442967215898,
-                    "angularVelocity": 0.0000010491572113687884,
-                    "velocityX": 3.584889055598663,
-                    "velocityY": 1.2079756859103348,
-                    "timestamp": 1.468878378469311
-                },
-                {
-                    "x": 5.071280359420366,
-                    "y": 6.590575042348357,
-                    "heading": 2.6364430119967146,
-                    "angularVelocity": 0.0000010491572106337029,
-                    "velocityX": 3.584889055598664,
-                    "velocityY": 1.2079756859103339,
-                    "timestamp": 1.5115610346511152
-                },
-                {
-                    "x": 5.224292946430397,
-                    "y": 6.642134653226047,
-                    "heading": 2.6364430567775314,
-                    "angularVelocity": 0.0000010491572087267192,
-                    "velocityX": 3.584889055598664,
-                    "velocityY": 1.2079756859103334,
-                    "timestamp": 1.5542436908329196
-                },
-                {
-                    "x": 5.3773055334404285,
-                    "y": 6.693694264103736,
-                    "heading": 2.636443101558348,
-                    "angularVelocity": 0.0000010491572072659829,
-                    "velocityX": 3.5848890555986643,
-                    "velocityY": 1.2079756859103326,
-                    "timestamp": 1.596926347014724
-                },
-                {
-                    "x": 5.53031812045046,
-                    "y": 6.745253874981426,
-                    "heading": 2.6364431463391638,
-                    "angularVelocity": 0.0000010491572066940514,
-                    "velocityX": 3.5848890555986643,
-                    "velocityY": 1.2079756859103314,
-                    "timestamp": 1.6396090031965282
-                },
-                {
-                    "x": 5.683330707460491,
-                    "y": 6.796813485859116,
-                    "heading": 2.63644319111998,
-                    "angularVelocity": 0.000001049157205644206,
-                    "velocityX": 3.5848890555986648,
-                    "velocityY": 1.2079756859103312,
-                    "timestamp": 1.6822916593783326
-                },
-                {
-                    "x": 5.836343294470522,
-                    "y": 6.848373096736806,
-                    "heading": 2.636443235900796,
-                    "angularVelocity": 0.0000010491572052803454,
-                    "velocityX": 3.5848890555986657,
-                    "velocityY": 1.2079756859103297,
-                    "timestamp": 1.724974315560137
-                },
-                {
-                    "x": 5.989355881480554,
-                    "y": 6.899932707614496,
-                    "heading": 2.6364432806816125,
-                    "angularVelocity": 0.0000010491572041084738,
-                    "velocityX": 3.5848890555986657,
-                    "velocityY": 1.2079756859103294,
-                    "timestamp": 1.7676569717419413
-                },
-                {
-                    "x": 6.142368468490585,
-                    "y": 6.951492318492185,
-                    "heading": 2.6364433254624284,
-                    "angularVelocity": 0.0000010491572019949161,
-                    "velocityX": 3.584889055598666,
-                    "velocityY": 1.2079756859103283,
-                    "timestamp": 1.8103396279237456
-                },
-                {
-                    "x": 6.295381055500616,
-                    "y": 7.0030519293698745,
-                    "heading": 2.6364433702432444,
-                    "angularVelocity": 0.0000010491572021090905,
-                    "velocityX": 3.584889055598666,
-                    "velocityY": 1.2079756859103277,
-                    "timestamp": 1.85302228410555
-                },
-                {
-                    "x": 6.448393642510648,
-                    "y": 7.054611540247564,
-                    "heading": 2.6364434150240608,
-                    "angularVelocity": 0.0000010491572009028897,
-                    "velocityX": 3.584889055598666,
-                    "velocityY": 1.2079756859103268,
-                    "timestamp": 1.8957049402873543
-                },
-                {
-                    "x": 6.601406229520499,
-                    "y": 7.106171151125787,
-                    "heading": 2.6364434598048767,
-                    "angularVelocity": 0.0000010491571991066208,
-                    "velocityX": 3.58488905559446,
-                    "velocityY": 1.2079756859228103,
-                    "timestamp": 1.9383875964691586
-                },
-                {
-                    "x": 6.75441881205228,
-                    "y": 7.157730775293412,
-                    "heading": 2.636443504585761,
-                    "angularVelocity": 0.0000010491588061157563,
-                    "velocityX": 3.5848889506789767,
-                    "velocityY": 1.207975997276489,
-                    "timestamp": 1.981070252650963
-                },
-                {
-                    "x": 6.905487720036347,
-                    "y": 7.20899283211694,
-                    "heading": 2.6411872457126164,
-                    "angularVelocity": 0.11113978255358876,
-                    "velocityX": 3.539351143954066,
-                    "velocityY": 1.2010043752942743,
-                    "timestamp": 2.0237529088327673
-                },
-                {
-                    "x": 7.045499588568849,
-                    "y": 7.254368377570732,
-                    "heading": 2.676148381759387,
-                    "angularVelocity": 0.81909466687958,
-                    "velocityX": 3.2802988627542176,
-                    "velocityY": 1.0630909487103528,
-                    "timestamp": 2.0664355650145714
-                },
-                {
-                    "x": 7.172970608020284,
-                    "y": 7.297222729151116,
-                    "heading": 2.7400991643229964,
-                    "angularVelocity": 1.4982849776549703,
-                    "velocityX": 2.9864828212302403,
-                    "velocityY": 1.0040226034164315,
-                    "timestamp": 2.1091182211963755
-                },
-                {
-                    "x": 7.286231301322222,
-                    "y": 7.335261893158222,
-                    "heading": 2.8124985936039937,
-                    "angularVelocity": 1.6962259558687312,
-                    "velocityX": 2.6535530689447047,
-                    "velocityY": 0.8912089220755359,
-                    "timestamp": 2.1518008773781796
-                },
-                {
-                    "x": 7.3851039731372365,
-                    "y": 7.3684303034498875,
-                    "heading": 2.8845195362089706,
-                    "angularVelocity": 1.6873584975173053,
-                    "velocityX": 2.316460142355517,
-                    "velocityY": 0.7770933971491014,
-                    "timestamp": 2.1944835335599837
-                },
-                {
-                    "x": 7.46964567597808,
-                    "y": 7.396754020582906,
-                    "heading": 2.951617256319432,
-                    "angularVelocity": 1.5720136962578515,
-                    "velocityX": 1.9807038831122041,
-                    "velocityY": 0.6635884377105193,
-                    "timestamp": 2.237166189741788
-                },
-                {
-                    "x": 7.539938623640353,
-                    "y": 7.420270719849242,
-                    "heading": 3.0110324789568197,
-                    "angularVelocity": 1.3920226141576573,
-                    "velocityX": 1.6468737878651336,
-                    "velocityY": 0.5509661621378036,
-                    "timestamp": 2.279848845923592
-                },
-                {
-                    "x": 7.596058846559568,
-                    "y": 7.439019303920545,
-                    "heading": 3.060856486484029,
-                    "angularVelocity": 1.1673127210028025,
-                    "velocityX": 1.3148249884022007,
-                    "velocityY": 0.4392553263659099,
-                    "timestamp": 2.322531502105396
-                },
-                {
-                    "x": 7.638072702511279,
-                    "y": 7.453036150115233,
-                    "heading": 3.09965963024843,
-                    "angularVelocity": 0.9091079898851933,
-                    "velocityX": 0.9843308666816342,
-                    "velocityY": 0.32839676460117173,
-                    "timestamp": 2.3652141582872
-                },
-                {
-                    "x": 7.666037142481598,
-                    "y": 7.462353623790155,
-                    "heading": 3.126330643718343,
-                    "angularVelocity": 0.6248677063655441,
-                    "velocityX": 0.6551710336677993,
-                    "velocityY": 0.2182964817192845,
-                    "timestamp": 2.4078968144690043
+                    "heading": -0.4985244969178424,
+                    "angularVelocity": 0.0000014545029788824699,
+                    "velocityX": 3.3763287390046313,
+                    "velocityY": 1.7061759601022248,
+                    "timestamp": 1.1336224947636264
+                },
+                {
+                    "x": 3.8477077097405896,
+                    "y": 6.176720729441604,
+                    "heading": -0.4985244519972541,
+                    "angularVelocity": 0.0000010525498975792195,
+                    "velocityX": 3.5217032391581933,
+                    "velocityY": 1.3813909907718895,
+                    "timestamp": 1.1763003653755966
+                },
+                {
+                    "x": 4.000682189533232,
+                    "y": 6.228336695265556,
+                    "heading": -0.49852440748602983,
+                    "angularVelocity": 0.000001042957945295385,
+                    "velocityX": 3.58439813418752,
+                    "velocityY": 1.2094316113670969,
+                    "timestamp": 1.2189782359875667
+                },
+                {
+                    "x": 4.153656816760498,
+                    "y": 6.279952224134777,
+                    "heading": -0.4985243629748153,
+                    "angularVelocity": 0.00000104295771772215,
+                    "velocityX": 3.5844015887793454,
+                    "velocityY": 1.2094213729291292,
+                    "timestamp": 1.2616561065995369
+                },
+                {
+                    "x": 4.306631443993679,
+                    "y": 6.331567752986466,
+                    "heading": -0.4985243184636008,
+                    "angularVelocity": 0.0000010429577168479619,
+                    "velocityX": 3.584401588917953,
+                    "velocityY": 1.209421372518333,
+                    "timestamp": 1.304333977211507
+                },
+                {
+                    "x": 4.459606071226857,
+                    "y": 6.38318328183816,
+                    "heading": -0.4985242739523863,
+                    "angularVelocity": 0.000001042957716433214,
+                    "velocityX": 3.584401588917909,
+                    "velocityY": 1.2094213725184633,
+                    "timestamp": 1.3470118478234772
+                },
+                {
+                    "x": 4.612580698460036,
+                    "y": 6.434798810689855,
+                    "heading": -0.4985242294411719,
+                    "angularVelocity": 0.0000010429577140470852,
+                    "velocityX": 3.5844015889179057,
+                    "velocityY": 1.209421372518471,
+                    "timestamp": 1.3896897184354473
+                },
+                {
+                    "x": 4.765555325693214,
+                    "y": 6.4864143395415494,
+                    "heading": -0.49852418492995754,
+                    "angularVelocity": 0.0000010429577143791187,
+                    "velocityX": 3.584401588917906,
+                    "velocityY": 1.2094213725184708,
+                    "timestamp": 1.4323675890474175
+                },
+                {
+                    "x": 4.918529952926393,
+                    "y": 6.5380298683932425,
+                    "heading": -0.4985241404187432,
+                    "angularVelocity": 0.0000010429577134079309,
+                    "velocityX": 3.584401588917911,
+                    "velocityY": 1.2094213725184557,
+                    "timestamp": 1.4750454596593876
+                },
+                {
+                    "x": 5.071504580159572,
+                    "y": 6.589645397244938,
+                    "heading": -0.49852409590752894,
+                    "angularVelocity": 0.000001042957711441133,
+                    "velocityX": 3.584401588917906,
+                    "velocityY": 1.20942137251847,
+                    "timestamp": 1.5177233302713578
+                },
+                {
+                    "x": 5.224479207392751,
+                    "y": 6.641260926096632,
+                    "heading": -0.49852405139631467,
+                    "angularVelocity": 0.0000010429577109856096,
+                    "velocityX": 3.5844015889179066,
+                    "velocityY": 1.2094213725184684,
+                    "timestamp": 1.560401200883328
+                },
+                {
+                    "x": 5.377453834625929,
+                    "y": 6.692876454948327,
+                    "heading": -0.4985240068851004,
+                    "angularVelocity": 0.000001042957710628888,
+                    "velocityX": 3.584401588917908,
+                    "velocityY": 1.209421372518467,
+                    "timestamp": 1.603079071495298
+                },
+                {
+                    "x": 5.530428461859109,
+                    "y": 6.744491983800022,
+                    "heading": -0.4985239623738862,
+                    "angularVelocity": 0.0000010429577097054993,
+                    "velocityX": 3.5844015889179075,
+                    "velocityY": 1.2094213725184686,
+                    "timestamp": 1.6457569421072682
+                },
+                {
+                    "x": 5.683403089092287,
+                    "y": 6.796107512651717,
+                    "heading": -0.4985239178626721,
+                    "angularVelocity": 0.0000010429577084041104,
+                    "velocityX": 3.584401588917908,
+                    "velocityY": 1.2094213725184642,
+                    "timestamp": 1.6884348127192383
+                },
+                {
+                    "x": 5.836377716325465,
+                    "y": 6.847723041503412,
+                    "heading": -0.4985238733514579,
+                    "angularVelocity": 0.000001042957708437487,
+                    "velocityX": 3.5844015889179084,
+                    "velocityY": 1.209421372518465,
+                    "timestamp": 1.7311126833312085
+                },
+                {
+                    "x": 5.989352343558645,
+                    "y": 6.899338570355106,
+                    "heading": -0.4985238288402439,
+                    "angularVelocity": 0.0000010429577061050693,
+                    "velocityX": 3.5844015889179084,
+                    "velocityY": 1.2094213725184642,
+                    "timestamp": 1.7737905539431786
+                },
+                {
+                    "x": 6.142326970791823,
+                    "y": 6.9509540992067995,
+                    "heading": -0.49852378432902983,
+                    "angularVelocity": 0.0000010429577060929704,
+                    "velocityX": 3.584401588917909,
+                    "velocityY": 1.2094213725184646,
+                    "timestamp": 1.8164684245551488
+                },
+                {
+                    "x": 6.295301598025002,
+                    "y": 7.002569628058494,
+                    "heading": -0.4985237398178158,
+                    "angularVelocity": 0.0000010429577057751362,
+                    "velocityX": 3.58440158891791,
+                    "velocityY": 1.2094213725184613,
+                    "timestamp": 1.859146295167119
+                },
+                {
+                    "x": 6.44827622525818,
+                    "y": 7.054185156910189,
+                    "heading": -0.49852369530660184,
+                    "angularVelocity": 0.0000010429577050460837,
+                    "velocityX": 3.5844015889179097,
+                    "velocityY": 1.2094213725184615,
+                    "timestamp": 1.901824165779089
+                },
+                {
+                    "x": 6.60125085249139,
+                    "y": 7.105800685761796,
+                    "heading": -0.4985236507953879,
+                    "angularVelocity": 0.0000010429577031279755,
+                    "velocityX": 3.584401588918602,
+                    "velocityY": 1.2094213725164094,
+                    "timestamp": 1.9445020363910592
+                },
+                {
+                    "x": 6.754225480461122,
+                    "y": 7.15741621243045,
+                    "heading": -0.49852360628410947,
+                    "angularVelocity": 0.000001042959214800434,
+                    "velocityX": 3.584401606176385,
+                    "velocityY": 1.209421321366894,
+                    "timestamp": 1.9871799070030294
+                },
+                {
+                    "x": 6.905473027890085,
+                    "y": 7.208390081717835,
+                    "heading": -0.49401401466016337,
+                    "angularVelocity": 0.10566580664128356,
+                    "velocityX": 3.543933782547739,
+                    "velocityY": 1.1943864245440503,
+                    "timestamp": 2.0298577776149997
+                },
+                {
+                    "x": 7.045508663452743,
+                    "y": 7.253974234854833,
+                    "heading": -0.45930885159823864,
+                    "angularVelocity": 0.8131887220303519,
+                    "velocityX": 3.2812235839006783,
+                    "velocityY": 1.0680981146283737,
+                    "timestamp": 2.07253564822697
+                },
+                {
+                    "x": 7.17296615196941,
+                    "y": 7.296903697598188,
+                    "heading": -0.39602280772923965,
+                    "angularVelocity": 1.4828772607799376,
+                    "velocityX": 2.986500654531669,
+                    "velocityY": 1.0058951425592673,
+                    "timestamp": 2.1152135188389405
+                },
+                {
+                    "x": 7.286216905364156,
+                    "y": 7.335009151903716,
+                    "heading": -0.3243524220820172,
+                    "angularVelocity": 1.6793336832302157,
+                    "velocityX": 2.6536177126649325,
+                    "velocityY": 0.8928621264164087,
+                    "timestamp": 2.157891389450911
+                },
+                {
+                    "x": 7.38508554947564,
+                    "y": 7.368236895639727,
+                    "heading": -0.2530399098538711,
+                    "angularVelocity": 1.6709482269282783,
+                    "velocityX": 2.31662551795061,
+                    "velocityY": 0.7785707969856192,
+                    "timestamp": 2.2005692600628812
+                },
+                {
+                    "x": 7.469627579006154,
+                    "y": 7.396613050645165,
+                    "heading": -0.18659143584410423,
+                    "angularVelocity": 1.5569772591027433,
+                    "velocityX": 1.9809336388681473,
+                    "velocityY": 0.6648915374301301,
+                    "timestamp": 2.2432471306748516
+                },
+                {
+                    "x": 7.539923583097788,
+                    "y": 7.420174905803342,
+                    "heading": -0.12774543015301906,
+                    "angularVelocity": 1.3788411850749698,
+                    "velocityX": 1.6471300719468636,
+                    "velocityY": 0.5520860066427349,
+                    "timestamp": 2.285925001286822
+                },
+                {
+                    "x": 7.596048224439389,
+                    "y": 7.4389607760624,
+                    "heading": -0.07839593087005978,
+                    "angularVelocity": 1.1563252471438406,
+                    "velocityX": 1.315075952403783,
+                    "velocityY": 0.4401782467044836,
+                    "timestamp": 2.3286028718987923
+                },
+                {
+                    "x": 7.638066692552635,
+                    "y": 7.453006402993039,
+                    "heading": -0.03996068961729096,
+                    "angularVelocity": 0.9005894788478175,
+                    "velocityX": 0.9845493111706484,
+                    "velocityY": 0.3291079599154316,
+                    "timestamp": 2.3712807425107627
+                },
+                {
+                    "x": 7.666034942577791,
+                    "y": 7.46234355914255,
+                    "heading": -0.013541291231896178,
+                    "angularVelocity": 0.6190420938664342,
+                    "velocityX": 0.655333774251397,
+                    "velocityY": 0.2187821467102802,
+                    "timestamp": 2.413958613122733
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
-                    "angularVelocity": 0.32025552073032726,
-                    "velocityX": 0.3271318790219412,
-                    "velocityY": 0.10885864717634237,
-                    "timestamp": 2.4505794706508084
+                    "heading": 0,
+                    "angularVelocity": 0.31729069510085045,
+                    "velocityX": 0.3272201077973249,
+                    "velocityY": 0.10910668200355174,
+                    "timestamp": 2.4566364837347034
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
+                    "heading": 0,
                     "angularVelocity": 0,
-                    "velocityX": -9.011054390829982e-34,
-                    "velocityY": 6.746182534290948e-34,
-                    "timestamp": 2.4932621268326125
+                    "velocityX": 5.5875375349932e-23,
+                    "velocityY": -3.709612464642882e-23,
+                    "timestamp": 2.499314354346674
                 }
             ],
             "constraints": [
@@ -7131,16 +7209,16 @@
                 {
                     "x": 0.71,
                     "y": 4.36,
-                    "heading": 2.1,
+                    "heading": -1.04,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 30
+                    "controlIntervalCount": 40
                 },
                 {
                     "x": 5.7195634841918945,
                     "y": 6.649363040924072,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -7149,7 +7227,7 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -7160,515 +7238,605 @@
                 {
                     "x": 0.71,
                     "y": 4.36,
-                    "heading": 2.1,
+                    "heading": -1.04,
                     "angularVelocity": 0,
                     "velocityX": 0,
-                    "velocityY": 1.2156956395995823e-32,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7323642464210136,
-                    "y": 4.371312176942183,
-                    "heading": 2.122836377232617,
-                    "angularVelocity": 0.41032613171248355,
-                    "velocityX": 0.4018428417574259,
-                    "velocityY": 0.20325823832090237,
-                    "timestamp": 0.05565421129117423
-                },
-                {
-                    "x": 0.7771770070003539,
-                    "y": 4.393987109637107,
-                    "heading": 2.1669272728918627,
-                    "angularVelocity": 0.7922292785458643,
-                    "velocityX": 0.8051998139886801,
-                    "velocityY": 0.40742528137344314,
-                    "timestamp": 0.11130842258234847
-                },
-                {
-                    "x": 0.8445465122121998,
-                    "y": 4.4280825394621655,
-                    "heading": 2.2301328847833437,
-                    "angularVelocity": 1.135684262252843,
-                    "velocityX": 1.2105014813592279,
-                    "velocityY": 0.612629826818977,
-                    "timestamp": 0.1669626338735227
-                },
-                {
-                    "x": 0.9346100694611125,
-                    "y": 4.4736618275321725,
-                    "heading": 2.3094955096415575,
-                    "angularVelocity": 1.4259949609743032,
-                    "velocityX": 1.6182703008351718,
-                    "velocityY": 0.8189728506175846,
-                    "timestamp": 0.22261684516469693
-                },
-                {
-                    "x": 1.047532617883247,
-                    "y": 4.530798800054343,
-                    "heading": 2.4006762269852384,
-                    "angularVelocity": 1.638343536424179,
-                    "velocityX": 2.0290027619175355,
-                    "velocityY": 1.0266423905144981,
-                    "timestamp": 0.27827105645587114
-                },
-                {
-                    "x": 1.1834754046687317,
-                    "y": 4.5995806121594685,
-                    "heading": 2.4961863960775177,
-                    "angularVelocity": 1.7161355246341592,
-                    "velocityX": 2.4426325273796983,
-                    "velocityY": 1.2358779418375128,
-                    "timestamp": 0.33392526774704534
-                },
-                {
-                    "x": 1.342193634237211,
-                    "y": 4.6799334714255165,
-                    "heading": 2.578155037608781,
-                    "angularVelocity": 1.4728201088398516,
-                    "velocityX": 2.851863783282992,
-                    "velocityY": 1.4437875841174588,
-                    "timestamp": 0.38957947903821954
-                },
-                {
-                    "x": 1.518721607083693,
-                    "y": 4.772275835878884,
-                    "heading": 2.6065642311000707,
-                    "angularVelocity": 0.5104590080821917,
-                    "velocityX": 3.17187089262509,
-                    "velocityY": 1.6592161187991088,
-                    "timestamp": 0.44523369032939375
-                },
-                {
-                    "x": 1.7067559979094145,
-                    "y": 4.866979097734677,
-                    "heading": 2.6065642752111695,
-                    "angularVelocity": 7.925922929779288e-7,
-                    "velocityX": 3.378619271809538,
-                    "velocityY": 1.7016369410091843,
-                    "timestamp": 0.500887901620568
-                },
-                {
-                    "x": 1.894790428298913,
-                    "y": 4.9616822810363175,
-                    "heading": 2.6065643193220347,
-                    "angularVelocity": 7.92588092448351e-7,
-                    "velocityX": 3.378619982695133,
-                    "velocityY": 1.7016355295409376,
-                    "timestamp": 0.5565421129117422
-                },
-                {
-                    "x": 2.082824858689142,
-                    "y": 5.056385464336507,
-                    "heading": 2.6065643634329,
-                    "angularVelocity": 7.925880881331428e-7,
-                    "velocityX": 3.378619982708264,
-                    "velocityY": 1.7016355295148662,
-                    "timestamp": 0.6121963242029164
-                },
-                {
-                    "x": 2.270859289079371,
-                    "y": 5.151088647636696,
-                    "heading": 2.606564407543764,
-                    "angularVelocity": 7.925880850764007e-7,
-                    "velocityX": 3.3786199827082646,
-                    "velocityY": 1.7016355295148653,
-                    "timestamp": 0.6678505354940906
-                },
-                {
-                    "x": 2.4588937194696,
-                    "y": 5.245791830936885,
-                    "heading": 2.606564451654629,
-                    "angularVelocity": 7.92588082043664e-7,
-                    "velocityX": 3.378619982708265,
-                    "velocityY": 1.7016355295148649,
-                    "timestamp": 0.7235047467852648
-                },
-                {
-                    "x": 2.6469281498598294,
-                    "y": 5.340495014237074,
-                    "heading": 2.6065644957654936,
-                    "angularVelocity": 7.92588079438066e-7,
-                    "velocityX": 3.378619982708265,
-                    "velocityY": 1.7016355295148646,
-                    "timestamp": 0.779158958076439
-                },
-                {
-                    "x": 2.8349625802500587,
-                    "y": 5.435198197537264,
-                    "heading": 2.606564539876358,
-                    "angularVelocity": 7.925880759204128e-7,
-                    "velocityX": 3.3786199827082655,
-                    "velocityY": 1.7016355295148642,
-                    "timestamp": 0.8348131693676132
-                },
-                {
-                    "x": 3.022997010640288,
-                    "y": 5.529901380837453,
-                    "heading": 2.606564583987222,
-                    "angularVelocity": 7.925880718883517e-7,
-                    "velocityX": 3.3786199827082664,
-                    "velocityY": 1.7016355295148637,
-                    "timestamp": 0.8904673806587874
-                },
-                {
-                    "x": 3.2110314410305167,
-                    "y": 5.624604564137642,
-                    "heading": 2.6065646280980856,
-                    "angularVelocity": 7.925880686816096e-7,
-                    "velocityX": 3.3786199827082664,
-                    "velocityY": 1.7016355295148633,
-                    "timestamp": 0.9461215919499616
-                },
-                {
-                    "x": 3.399065871420746,
-                    "y": 5.719307747437831,
-                    "heading": 2.6065646722089495,
-                    "angularVelocity": 7.92588065491887e-7,
-                    "velocityX": 3.378619982708267,
-                    "velocityY": 1.7016355295148633,
-                    "timestamp": 1.001775803241136
-                },
-                {
-                    "x": 3.5871003018109753,
-                    "y": 5.8140109307380206,
-                    "heading": 2.606564716319813,
-                    "angularVelocity": 7.92588062190758e-7,
-                    "velocityX": 3.378619982708267,
-                    "velocityY": 1.7016355295148633,
-                    "timestamp": 1.0574300145323101
-                },
-                {
-                    "x": 3.7751347322012045,
-                    "y": 5.90871411403821,
-                    "heading": 2.6065647604306763,
-                    "angularVelocity": 7.92588058976371e-7,
-                    "velocityX": 3.378619982708267,
-                    "velocityY": 1.7016355295148629,
-                    "timestamp": 1.1130842258234843
-                },
-                {
-                    "x": 3.963169162591434,
-                    "y": 6.003417297338399,
-                    "heading": 2.6065648045415397,
-                    "angularVelocity": 7.925880551764629e-7,
-                    "velocityX": 3.3786199827082677,
-                    "velocityY": 1.7016355295148622,
-                    "timestamp": 1.1687384371146585
-                },
-                {
-                    "x": 4.151203592981663,
-                    "y": 6.098120480638587,
-                    "heading": 2.6065648486524027,
-                    "angularVelocity": 7.925880525134244e-7,
-                    "velocityX": 3.378619982708268,
-                    "velocityY": 1.7016355295148615,
-                    "timestamp": 1.2243926484058327
-                },
-                {
-                    "x": 4.339238023371892,
-                    "y": 6.192823663938777,
-                    "heading": 2.606564892763265,
-                    "angularVelocity": 7.925880485176538e-7,
-                    "velocityX": 3.3786199827082686,
-                    "velocityY": 1.701635529514862,
-                    "timestamp": 1.280046859697007
-                },
-                {
-                    "x": 4.527272453762121,
-                    "y": 6.287526847238966,
-                    "heading": 2.6065649368741277,
-                    "angularVelocity": 7.925880449736749e-7,
-                    "velocityX": 3.37861998270827,
-                    "velocityY": 1.7016355295148586,
-                    "timestamp": 1.3357010709881811
-                },
-                {
-                    "x": 4.715306884156566,
-                    "y": 6.382230030530785,
-                    "heading": 2.6065649809849902,
-                    "angularVelocity": 7.925880433141827e-7,
-                    "velocityX": 3.3786199827840138,
-                    "velocityY": 1.7016355293644703,
-                    "timestamp": 1.3913552822793553
-                },
-                {
-                    "x": 4.903341542764552,
-                    "y": 6.476932760700215,
-                    "heading": 2.6065650250958465,
-                    "angularVelocity": 7.925879324126063e-7,
-                    "velocityX": 3.378624083345972,
-                    "velocityY": 1.7016273876195969,
-                    "timestamp": 1.4470094935705295
-                },
-                {
-                    "x": 5.097883205540706,
-                    "y": 6.557426122164244,
-                    "heading": 2.6065650695055442,
-                    "angularVelocity": 7.97957544404275e-7,
-                    "velocityX": 3.495542534208987,
-                    "velocityY": 1.4463121405656068,
-                    "timestamp": 1.5026637048617038
-                },
-                {
-                    "x": 5.300816520591001,
-                    "y": 6.613494907557912,
-                    "heading": 2.6065651167952333,
-                    "angularVelocity": 8.497055004082428e-7,
-                    "velocityX": 3.6463245160115534,
-                    "velocityY": 1.0074491056988386,
-                    "timestamp": 1.558317916152878
-                },
-                {
-                    "x": 5.5090879488774736,
-                    "y": 6.644295063089685,
-                    "heading": 2.606565171305544,
-                    "angularVelocity": 9.79446290291877e-7,
-                    "velocityX": 3.742240226832587,
-                    "velocityY": 0.5534200344809682,
-                    "timestamp": 1.6139721274440522
+                    "x": 0.7226799898554153,
+                    "y": 4.366381680411806,
+                    "heading": -1.026991220057009,
+                    "angularVelocity": 0.3105337423596819,
+                    "velocityX": 0.3026851649532591,
+                    "velocityY": 0.15233766037290783,
+                    "timestamp": 0.04189167928786116
+                },
+                {
+                    "x": 0.7480755502425919,
+                    "y": 4.379165077654378,
+                    "heading": -1.001621000893582,
+                    "angularVelocity": 0.6056147567896238,
+                    "velocityX": 0.6062196793943158,
+                    "velocityY": 0.3051536118838673,
+                    "timestamp": 0.08378335857572232
+                },
+                {
+                    "x": 0.7862289943958503,
+                    "y": 4.398373013543058,
+                    "heading": -0.964694126725153,
+                    "angularVelocity": 0.8814846956762871,
+                    "velocityX": 0.9107642568130221,
+                    "velocityY": 0.4585143449774818,
+                    "timestamp": 0.12567503786358347
+                },
+                {
+                    "x": 0.8371909372554502,
+                    "y": 4.424030289360705,
+                    "heading": -0.9172220550063085,
+                    "angularVelocity": 1.1332100437568369,
+                    "velocityX": 1.2165170679697972,
+                    "velocityY": 0.6124671116987632,
+                    "timestamp": 0.16756671715144464
+                },
+                {
+                    "x": 0.9010212440299602,
+                    "y": 4.456163358075127,
+                    "heading": -0.8604944995891576,
+                    "angularVelocity": 1.354148517832005,
+                    "velocityX": 1.523698926841682,
+                    "velocityY": 0.767051339565964,
+                    "timestamp": 0.2094583964393058
+                },
+                {
+                    "x": 0.9777886193671033,
+                    "y": 4.494801080149917,
+                    "heading": -0.796206518536166,
+                    "angularVelocity": 1.5346241102256366,
+                    "velocityX": 1.8325208404664681,
+                    "velocityY": 0.9223244981249834,
+                    "timestamp": 0.25135007572716694
+                },
+                {
+                    "x": 1.0675667159355893,
+                    "y": 4.539976112871383,
+                    "heading": -0.7267376359019178,
+                    "angularVelocity": 1.658297872398206,
+                    "velocityX": 2.143100923492954,
+                    "velocityY": 1.07837722166836,
+                    "timestamp": 0.2932417550150281
+                },
+                {
+                    "x": 1.1704147696438687,
+                    "y": 4.591719023811467,
+                    "heading": -0.6558650979752687,
+                    "angularVelocity": 1.6918046526529587,
+                    "velocityX": 2.4550950321555027,
+                    "velocityY": 1.23515962643868,
+                    "timestamp": 0.3351334343028892
+                },
+                {
+                    "x": 1.286251945776662,
+                    "y": 4.64999734523705,
+                    "heading": -0.5908433350340152,
+                    "angularVelocity": 1.5521402829056499,
+                    "velocityX": 2.765159528144268,
+                    "velocityY": 1.3911669910657207,
+                    "timestamp": 0.37702511359075036
+                },
+                {
+                    "x": 1.4132089603720126,
+                    "y": 4.714842752574763,
+                    "heading": -0.5508840698501896,
+                    "angularVelocity": 0.9538711711517488,
+                    "velocityX": 3.0306021805179513,
+                    "velocityY": 1.547930482617412,
+                    "timestamp": 0.4189167928786115
+                },
+                {
+                    "x": 1.550306465812279,
+                    "y": 4.786404286835006,
+                    "heading": -0.541265046364201,
+                    "angularVelocity": 0.22961656466170327,
+                    "velocityX": 3.272666738857444,
+                    "velocityY": 1.7082517453765305,
+                    "timestamp": 0.46080847216647264
+                },
+                {
+                    "x": 1.6919561664396436,
+                    "y": 4.857462558344818,
+                    "heading": -0.5412650016128938,
+                    "angularVelocity": 0.000001068262432444083,
+                    "velocityX": 3.3813325948098276,
+                    "velocityY": 1.6962383155263616,
+                    "timestamp": 0.5027001514543338
+                },
+                {
+                    "x": 1.8336059249814771,
+                    "y": 4.928520714406238,
+                    "heading": -0.541264956861776,
+                    "angularVelocity": 0.0000010682579123299043,
+                    "velocityX": 3.3813339772912605,
+                    "velocityY": 1.6962355596475316,
+                    "timestamp": 0.544591830742195
+                },
+                {
+                    "x": 1.9752556835258195,
+                    "y": 4.999578870462656,
+                    "heading": -0.5412649121106583,
+                    "angularVelocity": 0.0000010682579080478101,
+                    "velocityX": 3.3813339773511535,
+                    "velocityY": 1.696235559528139,
+                    "timestamp": 0.5864835100300562
+                },
+                {
+                    "x": 2.1169054420701623,
+                    "y": 5.0706370265190746,
+                    "heading": -0.5412648673595408,
+                    "angularVelocity": 0.0000010682579032833847,
+                    "velocityX": 3.381333977351157,
+                    "velocityY": 1.6962355595281329,
+                    "timestamp": 0.6283751893179174
+                },
+                {
+                    "x": 2.258555200614505,
+                    "y": 5.141695182575493,
+                    "heading": -0.5412648226084235,
+                    "angularVelocity": 0.0000010682578992307337,
+                    "velocityX": 3.381333977351157,
+                    "velocityY": 1.6962355595281327,
+                    "timestamp": 0.6702668686057786
+                },
+                {
+                    "x": 2.400204959158848,
+                    "y": 5.212753338631911,
+                    "heading": -0.5412647778573064,
+                    "angularVelocity": 0.0000010682578937479676,
+                    "velocityX": 3.3813339773511575,
+                    "velocityY": 1.696235559528132,
+                    "timestamp": 0.7121585478936397
+                },
+                {
+                    "x": 2.5418547177031905,
+                    "y": 5.2838114946883294,
+                    "heading": -0.5412647331061895,
+                    "angularVelocity": 0.0000010682578893581349,
+                    "velocityX": 3.3813339773511584,
+                    "velocityY": 1.696235559528132,
+                    "timestamp": 0.7540502271815009
+                },
+                {
+                    "x": 2.6835044762475335,
+                    "y": 5.354869650744748,
+                    "heading": -0.5412646883550728,
+                    "angularVelocity": 0.0000010682578845109594,
+                    "velocityX": 3.3813339773511584,
+                    "velocityY": 1.6962355595281313,
+                    "timestamp": 0.7959419064693621
+                },
+                {
+                    "x": 2.8251542347918766,
+                    "y": 5.425927806801167,
+                    "heading": -0.5412646436039563,
+                    "angularVelocity": 0.0000010682578796288815,
+                    "velocityX": 3.3813339773511593,
+                    "velocityY": 1.6962355595281313,
+                    "timestamp": 0.8378335857572233
+                },
+                {
+                    "x": 2.9668039933362196,
+                    "y": 5.496985962857585,
+                    "heading": -0.5412645988528401,
+                    "angularVelocity": 0.0000010682578747880459,
+                    "velocityX": 3.38133397735116,
+                    "velocityY": 1.6962355595281309,
+                    "timestamp": 0.8797252650450845
+                },
+                {
+                    "x": 3.108453751880562,
+                    "y": 5.5680441189140035,
+                    "heading": -0.541264554101724,
+                    "angularVelocity": 0.0000010682578699288912,
+                    "velocityX": 3.38133397735116,
+                    "velocityY": 1.69623555952813,
+                    "timestamp": 0.9216169443329457
+                },
+                {
+                    "x": 3.250103510424905,
+                    "y": 5.639102274970423,
+                    "heading": -0.5412645093506081,
+                    "angularVelocity": 0.000001068257865104235,
+                    "velocityX": 3.3813339773511606,
+                    "velocityY": 1.69623555952813,
+                    "timestamp": 0.9635086236208069
+                },
+                {
+                    "x": 3.3917532689692482,
+                    "y": 5.71016043102684,
+                    "heading": -0.5412644645994924,
+                    "angularVelocity": 0.0000010682578613220535,
+                    "velocityX": 3.381333977351161,
+                    "velocityY": 1.6962355595281293,
+                    "timestamp": 1.005400302908668
+                },
+                {
+                    "x": 3.5334030275135913,
+                    "y": 5.7812185870832575,
+                    "heading": -0.5412644198483769,
+                    "angularVelocity": 0.0000010682578558700584,
+                    "velocityX": 3.381333977351162,
+                    "velocityY": 1.6962355595281287,
+                    "timestamp": 1.0472919821965292
+                },
+                {
+                    "x": 3.675052786057934,
+                    "y": 5.852276743139676,
+                    "heading": -0.5412643750972616,
+                    "angularVelocity": 0.0000010682578513840427,
+                    "velocityX": 3.381333977351162,
+                    "velocityY": 1.6962355595281287,
+                    "timestamp": 1.0891836614843904
+                },
+                {
+                    "x": 3.816702544602277,
+                    "y": 5.923334899196093,
+                    "heading": -0.5412643303461464,
+                    "angularVelocity": 0.0000010682578474926016,
+                    "velocityX": 3.381333977351163,
+                    "velocityY": 1.6962355595281278,
+                    "timestamp": 1.1310753407722516
+                },
+                {
+                    "x": 3.95835230314662,
+                    "y": 5.994393055252511,
+                    "heading": -0.5412642855950315,
+                    "angularVelocity": 0.000001068257842976485,
+                    "velocityX": 3.3813339773511637,
+                    "velocityY": 1.6962355595281275,
+                    "timestamp": 1.1729670200601128
+                },
+                {
+                    "x": 4.100002061690963,
+                    "y": 6.065451211308929,
+                    "heading": -0.5412642408439168,
+                    "angularVelocity": 0.0000010682578369198447,
+                    "velocityX": 3.3813339773511637,
+                    "velocityY": 1.6962355595281269,
+                    "timestamp": 1.214858699347974
+                },
+                {
+                    "x": 4.2416518202353055,
+                    "y": 6.136509367365346,
+                    "heading": -0.5412641960928023,
+                    "angularVelocity": 0.0000010682578319796932,
+                    "velocityX": 3.3813339773511646,
+                    "velocityY": 1.696235559528127,
+                    "timestamp": 1.2567503786358352
+                },
+                {
+                    "x": 4.383301578779649,
+                    "y": 6.207567523421765,
+                    "heading": -0.5412641513416879,
+                    "angularVelocity": 0.0000010682578277344439,
+                    "velocityX": 3.381333977351165,
+                    "velocityY": 1.6962355595281264,
+                    "timestamp": 1.2986420579236964
+                },
+                {
+                    "x": 4.524951337323992,
+                    "y": 6.278625679478183,
+                    "heading": -0.5412641065905739,
+                    "angularVelocity": 0.0000010682578220378627,
+                    "velocityX": 3.3813339773511655,
+                    "velocityY": 1.6962355595281264,
+                    "timestamp": 1.3405337372115576
+                },
+                {
+                    "x": 4.666601095868337,
+                    "y": 6.349683835534597,
+                    "heading": -0.54126406183946,
+                    "angularVelocity": 0.0000010682578180389232,
+                    "velocityX": 3.381333977351213,
+                    "velocityY": 1.6962355595280314,
+                    "timestamp": 1.3824254164994187
+                },
+                {
+                    "x": 4.8082508544580715,
+                    "y": 6.420741991500528,
+                    "heading": -0.5412640170883463,
+                    "angularVelocity": 0.000001068257814314864,
+                    "velocityX": 3.3813339784347316,
+                    "velocityY": 1.6962355573681103,
+                    "timestamp": 1.42431709578728
+                },
+                {
+                    "x": 4.949901660762853,
+                    "y": 6.491798058877407,
+                    "heading": -0.54126397233727,
+                    "angularVelocity": 0.0000010682569195340074,
+                    "velocityX": 3.3813589885337056,
+                    "velocityY": 1.6961857004731962,
+                    "timestamp": 1.4662087750751411
+                },
+                {
+                    "x": 5.0966757016987385,
+                    "y": 6.551558383908352,
+                    "heading": -0.5412639273749705,
+                    "angularVelocity": 0.000001073299046263079,
+                    "velocityX": 3.503656177813225,
+                    "velocityY": 1.4265440308634512,
+                    "timestamp": 1.5081004543630023
+                },
+                {
+                    "x": 5.2483360143420885,
+                    "y": 6.597526467706988,
+                    "heading": -0.5412638804954443,
+                    "angularVelocity": 0.000001119065339105663,
+                    "velocityX": 3.6202968040790893,
+                    "velocityY": 1.097308214425216,
+                    "timestamp": 1.5499921336508635
+                },
+                {
+                    "x": 5.403589728009299,
+                    "y": 6.629310092724924,
+                    "heading": -0.5412638294474256,
+                    "angularVelocity": 0.0000012185717939310704,
+                    "velocityX": 3.7060751993343666,
+                    "velocityY": 0.7587097380253649,
+                    "timestamp": 1.5918838129387247
+                },
+                {
+                    "x": 5.561113217492597,
+                    "y": 6.646638205385644,
+                    "heading": -0.54126377105557,
+                    "angularVelocity": 0.0000013938771748607603,
+                    "velocityX": 3.760257219598817,
+                    "velocityY": 0.4136409176067717,
+                    "timestamp": 1.633775492226586
                 },
                 {
                     "x": 5.7195634841918945,
                     "y": 6.649363040924072,
-                    "heading": 2.6065652359371767,
-                    "angularVelocity": 0.0000011613071303259608,
-                    "velocityX": 3.781843825137414,
-                    "velocityY": 0.0910618930142201,
-                    "timestamp": 1.6696263387352264
-                },
-                {
-                    "x": 5.838739218913105,
-                    "y": 6.643942809110191,
-                    "heading": 2.606565305952835,
-                    "angularVelocity": 0.0000022201789880280334,
-                    "velocityX": 3.77903269949585,
-                    "velocityY": -0.1718741932778599,
-                    "timestamp": 1.7011623809444163
-                },
-                {
-                    "x": 5.957250522969574,
-                    "y": 6.630256808888294,
-                    "heading": 2.606565367221186,
-                    "angularVelocity": 0.00000194280407444938,
-                    "velocityX": 3.7579637695288737,
-                    "velocityY": -0.4339796392683931,
-                    "timestamp": 1.7326984231536062
-                },
-                {
-                    "x": 6.074524798614125,
-                    "y": 6.608371177242311,
-                    "heading": 2.60656542248837,
-                    "angularVelocity": 0.000001752508552376974,
-                    "velocityX": 3.718737908410616,
-                    "velocityY": -0.693987898063049,
-                    "timestamp": 1.7642344653627962
-                },
-                {
-                    "x": 6.189995430266423,
-                    "y": 6.578391677512442,
-                    "heading": 2.606565473629249,
-                    "angularVelocity": 0.0000016216644704538906,
-                    "velocityX": 3.6615448091532925,
-                    "velocityY": -0.9506424278292124,
-                    "timestamp": 1.795770507571986
-                },
-                {
-                    "x": 6.303104529085719,
-                    "y": 6.5404632008330585,
-                    "heading": 2.606565522006597,
-                    "angularVelocity": 0.000001534033588555687,
-                    "velocityX": 3.586661194483535,
-                    "velocityY": -1.2027024960136288,
-                    "timestamp": 1.827306549781176
-                },
-                {
-                    "x": 6.413305655265644,
-                    "y": 6.4947691134226035,
-                    "heading": 2.6065655686762406,
-                    "angularVelocity": 0.000001479882707645635,
-                    "velocityX": 3.4944501104140286,
-                    "velocityY": -1.4489480673367345,
-                    "timestamp": 1.858842591990366
-                },
-                {
-                    "x": 6.520066647831161,
-                    "y": 6.441530702158678,
-                    "heading": 2.6065656145154117,
-                    "angularVelocity": 0.000001453548632665294,
-                    "velocityX": 3.3853643351100575,
-                    "velocityY": -1.6881766871941624,
-                    "timestamp": 1.8903786341995559
-                },
-                {
-                    "x": 6.6242681306818785,
-                    "y": 6.383442071865126,
-                    "heading": 2.6065656601861753,
-                    "angularVelocity": 0.0000014482084799406948,
-                    "velocityX": 3.3042029231034076,
-                    "velocityY": -1.8419759178475867,
-                    "timestamp": 1.9219146764087458
-                },
-                {
-                    "x": 6.728469078482098,
-                    "y": 6.325352481789039,
-                    "heading": 2.6065657058569283,
-                    "angularVelocity": 0.0000014482081475491236,
-                    "velocityX": 3.3041859567861076,
-                    "velocityY": -1.8420063523113561,
-                    "timestamp": 1.9534507186179357
-                },
-                {
-                    "x": 6.832670029211484,
-                    "y": 6.267262896967356,
-                    "heading": 2.6065657515277794,
-                    "angularVelocity": 0.0000014482112413699588,
-                    "velocityX": 3.3041860496692763,
-                    "velocityY": -1.8420061856955277,
-                    "timestamp": 1.9849867608271257
-                },
-                {
-                    "x": 6.936347380984222,
-                    "y": 6.209534885446302,
-                    "heading": 2.608700420086438,
-                    "angularVelocity": 0.06768980535028488,
-                    "velocityX": 3.2875828578934874,
-                    "velocityY": -1.8305407868915176,
-                    "timestamp": 2.0165228030363154
-                },
-                {
-                    "x": 7.0344207678165205,
-                    "y": 6.154966008244285,
-                    "heading": 2.632443469248245,
-                    "angularVelocity": 0.7528861422848001,
-                    "velocityX": 3.1098825331898827,
-                    "velocityY": -1.730365428865226,
-                    "timestamp": 2.0480588452455053
-                },
-                {
-                    "x": 7.1267043537102115,
-                    "y": 6.103735031776186,
-                    "heading": 2.6761348400547162,
-                    "angularVelocity": 1.3854424254207447,
-                    "velocityX": 2.9262893955285,
-                    "velocityY": -1.6245214325965975,
-                    "timestamp": 2.0795948874546952
-                },
-                {
-                    "x": 7.212236583644832,
-                    "y": 6.056223391759097,
-                    "heading": 2.7313687787461847,
-                    "angularVelocity": 1.7514543621257885,
-                    "velocityX": 2.7122055889973176,
-                    "velocityY": -1.5065822052725362,
-                    "timestamp": 2.111130929663885
-                },
-                {
-                    "x": 7.290555468694817,
-                    "y": 6.012738407550467,
-                    "heading": 2.789179732571192,
-                    "angularVelocity": 1.8331708665763002,
-                    "velocityX": 2.4834722293453213,
-                    "velocityY": -1.3788979580943836,
-                    "timestamp": 2.142666971873075
-                },
-                {
-                    "x": 7.361635905599927,
-                    "y": 5.973292872851013,
-                    "heading": 2.8461736255132477,
-                    "angularVelocity": 1.8072620706173155,
-                    "velocityX": 2.2539428515984796,
-                    "velocityY": -1.2508080258708043,
-                    "timestamp": 2.174203014082265
-                },
-                {
-                    "x": 7.425505527447213,
-                    "y": 5.937869720935748,
-                    "heading": 2.900420841886284,
-                    "angularVelocity": 1.7201656445407778,
-                    "velocityX": 2.0252897121209994,
-                    "velocityY": -1.1232592752220105,
-                    "timestamp": 2.205739056291455
-                },
-                {
-                    "x": 7.482197651528883,
-                    "y": 5.90644694625493,
-                    "heading": 2.9506667136326055,
-                    "angularVelocity": 1.5932840085963447,
-                    "velocityX": 1.7976930556348818,
-                    "velocityY": -0.9964083150440386,
-                    "timestamp": 2.237275098500645
-                },
-                {
-                    "x": 7.531741810313519,
-                    "y": 5.879003134115031,
-                    "heading": 2.9960449007561833,
-                    "angularVelocity": 1.4389309483596011,
-                    "velocityX": 1.5710328663309023,
-                    "velocityY": -0.8702364094344716,
-                    "timestamp": 2.268811140709835
-                },
-                {
-                    "x": 7.5741623718487,
-                    "y": 5.855519184527709,
-                    "heading": 3.0359339398400818,
-                    "angularVelocity": 1.264871438822304,
-                    "velocityX": 1.3451453817124663,
-                    "velocityY": -0.7446701596714312,
-                    "timestamp": 2.3003471829190247
-                },
-                {
-                    "x": 7.609479178893684,
-                    "y": 5.835978699422795,
-                    "heading": 3.0698731008846027,
-                    "angularVelocity": 1.076202296388058,
-                    "velocityX": 1.1198871060203273,
-                    "velocityY": -0.6196238886063927,
-                    "timestamp": 2.3318832251282147
-                },
-                {
-                    "x": 7.637708552445183,
-                    "y": 5.820367770747114,
-                    "heading": 3.0975089102886857,
-                    "angularVelocity": 0.8763245945944945,
-                    "velocityX": 0.895146365046179,
-                    "velocityY": -0.4950186384241787,
-                    "timestamp": 2.3634192673374046
-                },
-                {
-                    "x": 7.658864191629111,
-                    "y": 5.808674501363959,
-                    "heading": 3.118559676862925,
-                    "angularVelocity": 0.6675145357366632,
-                    "velocityX": 0.6708400199236877,
-                    "velocityY": -0.3707906434672271,
-                    "timestamp": 2.3949553095465945
-                },
-                {
-                    "x": 7.672957876154406,
-                    "y": 5.800888435996016,
-                    "heading": 3.1327912648609026,
-                    "angularVelocity": 0.45128009100108973,
-                    "velocityX": 0.4469072064213547,
-                    "velocityY": -0.24689418273527672,
-                    "timestamp": 2.4264913517557845
+                    "heading": -0.5412637028328066,
+                    "angularVelocity": 0.000001628551649307522,
+                    "velocityX": 3.7823804009024578,
+                    "velocityY": 0.06504479134637085,
+                    "timestamp": 1.675667171514447
+                },
+                {
+                    "x": 5.8386728592662305,
+                    "y": 6.64312635201416,
+                    "heading": -0.5412636336326876,
+                    "angularVelocity": 0.000002194803906563273,
+                    "velocityX": 3.777764046962115,
+                    "velocityY": -0.19780759592812253,
+                    "timestamp": 1.7071962383364254
+                },
+                {
+                    "x": 5.95706146956484,
+                    "y": 6.62863229973747,
+                    "heading": -0.5412635727602775,
+                    "angularVelocity": 0.0000019306759164775456,
+                    "velocityX": 3.7549037200201067,
+                    "velocityY": -0.459704448549871,
+                    "timestamp": 1.7387253051584037
+                },
+                {
+                    "x": 6.0741575637122,
+                    "y": 6.605950895283392,
+                    "heading": -0.5412635175985968,
+                    "angularVelocity": 0.000001749550058024173,
+                    "velocityX": 3.713909289117771,
+                    "velocityY": -0.7193807727372066,
+                    "timestamp": 1.770254371980382
+                },
+                {
+                    "x": 6.189395638406918,
+                    "y": 6.575191700510336,
+                    "heading": -0.5412634663439824,
+                    "angularVelocity": 0.0000016256305579593937,
+                    "velocityX": 3.654978922953296,
+                    "velocityY": -0.9755821492190131,
+                    "timestamp": 1.8017834388023604
+                },
+                {
+                    "x": 6.302219177732501,
+                    "y": 6.536503313694644,
+                    "heading": -0.5412634176727114,
+                    "angularVelocity": 0.0000015436952600896132,
+                    "velocityX": 3.578397672300798,
+                    "velocityY": -1.2270704691051513,
+                    "timestamp": 1.8333125056243387
+                },
+                {
+                    "x": 6.412083374588637,
+                    "y": 6.49007271186475,
+                    "heading": -0.5412633705487488,
+                    "angularVelocity": 0.0000014946196396789507,
+                    "velocityX": 3.484536902930231,
+                    "velocityY": -1.4726284825381617,
+                    "timestamp": 1.864841572446317
+                },
+                {
+                    "x": 6.518458035998333,
+                    "y": 6.436124829047777,
+                    "heading": -0.5412633241017745,
+                    "angularVelocity": 0.0000014731477658308475,
+                    "velocityX": 3.3738601275551834,
+                    "velocityY": -1.7110523163142606,
+                    "timestamp": 1.8963706392682953
+                },
+                {
+                    "x": 6.622870515934444,
+                    "y": 6.378470889852871,
+                    "heading": -0.5412632777559726,
+                    "angularVelocity": 0.0000014699389024112294,
+                    "velocityX": 3.3116260790606757,
+                    "velocityY": -1.8285964351699866,
+                    "timestamp": 1.9278997060902736
+                },
+                {
+                    "x": 6.7272827038996414,
+                    "y": 6.320816421895741,
+                    "heading": -0.5412632314101712,
+                    "angularVelocity": 0.0000014699388892769531,
+                    "velocityX": 3.311616818687876,
+                    "velocityY": -1.8286132057971307,
+                    "timestamp": 1.959428772912252
+                },
+                {
+                    "x": 6.831694893880165,
+                    "y": 6.263161957588452,
+                    "heading": -0.5412631850642508,
+                    "angularVelocity": 0.0000014699426583817394,
+                    "velocityX": 3.311616882607505,
+                    "velocityY": -1.828613090035986,
+                    "timestamp": 1.9909578397342302
+                },
+                {
+                    "x": 6.935462774507961,
+                    "y": 6.205910954727142,
+                    "heading": -0.5387292297574777,
+                    "angularVelocity": 0.08036886473933494,
+                    "velocityX": 3.2911814743423053,
+                    "velocityY": -1.815816598206477,
+                    "timestamp": 2.0224869065562086
+                },
+                {
+                    "x": 7.033610221214887,
+                    "y": 6.151825220973909,
+                    "heading": -0.514724354544779,
+                    "angularVelocity": 0.7613569836442236,
+                    "velocityX": 3.11291949302188,
+                    "velocityY": -1.715424502051262,
+                    "timestamp": 2.054015973378187
+                },
+                {
+                    "x": 7.126006777010342,
+                    "y": 6.101041958255997,
+                    "heading": -0.47044950446923295,
+                    "angularVelocity": 1.4042550109565184,
+                    "velocityX": 2.930519838000589,
+                    "velocityY": -1.610680804625417,
+                    "timestamp": 2.0855450402001656
+                },
+                {
+                    "x": 7.2116589199714145,
+                    "y": 6.053937431211348,
+                    "heading": -0.4144026036835486,
+                    "angularVelocity": 1.7776263757548019,
+                    "velocityX": 2.7166088817245364,
+                    "velocityY": -1.4940032101366643,
+                    "timestamp": 2.117074107022144
+                },
+                {
+                    "x": 7.290084520639544,
+                    "y": 6.01082680132011,
+                    "heading": -0.35575031779267857,
+                    "angularVelocity": 1.8602607626174494,
+                    "velocityX": 2.4874063387585092,
+                    "velocityY": -1.3673297130756235,
+                    "timestamp": 2.1486031738441227
+                },
+                {
+                    "x": 7.3612581622075055,
+                    "y": 5.971723406014521,
+                    "heading": -0.2979353118382045,
+                    "angularVelocity": 1.8337049517168786,
+                    "velocityX": 2.2573976568931324,
+                    "velocityY": -1.2402331958119186,
+                    "timestamp": 2.1801322406661012
+                },
+                {
+                    "x": 7.425208576949436,
+                    "y": 5.936609973900717,
+                    "heading": -0.2429131400794928,
+                    "angularVelocity": 1.7451252861171487,
+                    "velocityX": 2.028300269811719,
+                    "velocityY": -1.1136844712868785,
+                    "timestamp": 2.2116613074880798
+                },
+                {
+                    "x": 7.481970224994158,
+                    "y": 5.90546408447042,
+                    "heading": -0.1919554556036592,
+                    "angularVelocity": 1.616212898515343,
+                    "velocityX": 1.8002958465346786,
+                    "velocityY": -0.9878468527519235,
+                    "timestamp": 2.2431903743100583
+                },
+                {
+                    "x": 7.531573575573894,
+                    "y": 5.8782638409384305,
+                    "heading": -0.14593987362659777,
+                    "angularVelocity": 1.4594653954358323,
+                    "velocityX": 1.5732578087328088,
+                    "velocityY": -0.8627037294052898,
+                    "timestamp": 2.274719441132037
+                },
+                {
+                    "x": 7.574043708984396,
+                    "y": 5.854989674792864,
+                    "heading": -0.10549518757833008,
+                    "angularVelocity": 1.2827745989638484,
+                    "velocityX": 1.347015236774987,
+                    "velocityY": -0.7381812559495933,
+                    "timestamp": 2.3062485079540154
+                },
+                {
+                    "x": 7.60940100035129,
+                    "y": 5.835624772967359,
+                    "heading": -0.07108683169331761,
+                    "angularVelocity": 1.091321734299697,
+                    "velocityX": 1.1214188978865451,
+                    "velocityY": -0.614192038566993,
+                    "timestamp": 2.337777574775994
+                },
+                {
+                    "x": 7.637662176953836,
+                    "y": 5.820154874880602,
+                    "heading": -0.043071374084194446,
+                    "angularVelocity": 0.8885596826352652,
+                    "velocityX": 0.8963530941818176,
+                    "velocityY": -0.4906551206892414,
+                    "timestamp": 2.3693066415979724
+                },
+                {
+                    "x": 7.65884126375683,
+                    "y": 5.808567782468078,
+                    "heading": -0.021732750340195603,
+                    "angularVelocity": 0.6767921126395045,
+                    "velocityX": 0.6717321169883477,
+                    "velocityY": -0.3675050859559891,
+                    "timestamp": 2.400835708419951
+                },
+                {
+                    "x": 7.672950320288622,
+                    "y": 5.8008527701995645,
+                    "heading": -0.007307045765720019,
+                    "angularVelocity": 0.45753668054709684,
+                    "velocityX": 0.44749362902029066,
+                    "velocityY": -0.24469523034329732,
+                    "timestamp": 2.4323647752419295
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
-                    "angularVelocity": 0.22858718577555334,
-                    "velocityX": 0.2233039833876712,
-                    "velocityY": -0.12330133154384845,
-                    "timestamp": 2.4580273939649744
+                    "heading": 5.476349034092433e-35,
+                    "angularVelocity": 0.23175585268595406,
+                    "velocityX": 0.22359303404641942,
+                    "velocityY": -0.12219740664444498,
+                    "timestamp": 2.463893842063908
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
-                    "angularVelocity": 3.1728947336241785e-32,
-                    "velocityX": 2.0932493862769896e-33,
-                    "velocityY": -2.0877321220317972e-33,
-                    "timestamp": 2.4895634361741643
+                    "heading": 1.465348958332225e-34,
+                    "angularVelocity": 2.9106988947740147e-33,
+                    "velocityX": 3.969530612508356e-33,
+                    "velocityY": 4.7510565840732025e-33,
+                    "timestamp": 2.4954229088858866
                 }
             ],
             "constraints": [
@@ -7695,7 +7863,7 @@
                 {
                     "x": 0.71,
                     "y": 4.36,
-                    "heading": 2.1,
+                    "heading": -1.04,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -7704,7 +7872,7 @@
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -7715,272 +7883,272 @@
                 {
                     "x": 0.71,
                     "y": 4.36,
-                    "heading": 2.1,
+                    "heading": -1.04,
                     "angularVelocity": 0,
-                    "velocityX": 1.276910697826558e-30,
-                    "velocityY": -1.1746574795316565e-31,
+                    "velocityX": -1.418990828065155e-30,
+                    "velocityY": 1.8798559266210403e-30,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.761216454009274,
-                    "y": 4.38433482667126,
-                    "heading": 2.165113129644946,
-                    "angularVelocity": 0.7716161812421117,
-                    "velocityX": 0.6069351123943075,
-                    "velocityY": 0.28837726169296907,
-                    "timestamp": 0.08438538644967974
-                },
-                {
-                    "x": 0.8642342955307559,
-                    "y": 4.433284699389541,
-                    "heading": 2.2862365234671036,
-                    "angularVelocity": 1.435359828498015,
-                    "velocityX": 1.2208019167265607,
-                    "velocityY": 0.58007523313856,
-                    "timestamp": 0.1687707728993595
-                },
-                {
-                    "x": 1.0198907802831574,
-                    "y": 4.507085180578877,
-                    "heading": 2.44934046785083,
-                    "angularVelocity": 1.9328458545483174,
-                    "velocityX": 1.844590530437629,
-                    "velocityY": 0.8745647119048293,
-                    "timestamp": 0.25315615934903923
-                },
-                {
-                    "x": 1.22914875569361,
-                    "y": 4.606070154859929,
-                    "heading": 2.62757889489394,
-                    "angularVelocity": 2.112195423189735,
-                    "velocityX": 2.4797892646404183,
-                    "velocityY": 1.1730108546707563,
-                    "timestamp": 0.337541545798719
-                },
-                {
-                    "x": 1.4854756632487267,
-                    "y": 4.729647702991699,
-                    "heading": 2.7127404908574917,
-                    "angularVelocity": 1.0091983878551618,
-                    "velocityX": 3.0375746126133176,
-                    "velocityY": 1.4644425217566792,
-                    "timestamp": 0.4219269322483987
-                },
-                {
-                    "x": 1.7744042397686677,
-                    "y": 4.865385720511678,
-                    "heading": 2.7127405410221264,
-                    "angularVelocity": 5.944706378546851e-7,
-                    "velocityX": 3.4239172050510245,
-                    "velocityY": 1.6085488640966181,
-                    "timestamp": 0.5063123186980785
-                },
-                {
-                    "x": 2.0633325652631687,
-                    "y": 5.001124272354809,
-                    "heading": 2.712740591190551,
-                    "angularVelocity": 5.945155495227871e-7,
-                    "velocityX": 3.423914230300861,
-                    "velocityY": 1.608555196036035,
-                    "timestamp": 0.5906977051477582
-                },
-                {
-                    "x": 2.352241240419818,
-                    "y": 5.1369046433555585,
-                    "heading": 2.712740641359129,
-                    "angularVelocity": 5.945173720540621e-7,
-                    "velocityX": 3.4236813660732097,
-                    "velocityY": 1.6090507694920106,
-                    "timestamp": 0.675083091597438
-                },
-                {
-                    "x": 2.647177658207551,
-                    "y": 5.259039982285096,
-                    "heading": 2.712740691973867,
-                    "angularVelocity": 5.998045355791158e-7,
-                    "velocityX": 3.4951124856626223,
-                    "velocityY": 1.4473517758000545,
-                    "timestamp": 0.7594684780471177
-                },
-                {
-                    "x": 2.9596305957770053,
-                    "y": 5.324444044648143,
-                    "heading": 2.712740749087516,
-                    "angularVelocity": 6.768191931683843e-7,
-                    "velocityX": 3.7026901305450077,
-                    "velocityY": 0.7750638483126594,
-                    "timestamp": 0.8438538644967974
-                },
-                {
-                    "x": 3.2787911260313356,
-                    "y": 5.330853358998101,
-                    "heading": 2.712740828995151,
-                    "angularVelocity": 9.469368813287509e-7,
-                    "velocityX": 3.7821777405102064,
-                    "velocityY": 0.0759528944478236,
-                    "timestamp": 0.9282392509464772
-                },
-                {
-                    "x": 3.5936179112160946,
-                    "y": 5.278046121689486,
-                    "heading": 2.7127406682187845,
-                    "angularVelocity": -0.000001905263138772926,
-                    "velocityX": 3.730821157908589,
-                    "velocityY": -0.6257865198035278,
-                    "timestamp": 1.012624637396157
-                },
-                {
-                    "x": 3.893219599027341,
-                    "y": 5.167849261276542,
-                    "heading": 2.7127405436852854,
-                    "angularVelocity": -0.0000014757709178373628,
-                    "velocityX": 3.550397769285573,
-                    "velocityY": -1.3058761125501486,
-                    "timestamp": 1.0970100238458367
-                },
-                {
-                    "x": 4.167231798281599,
-                    "y": 5.0040755200168485,
-                    "heading": 2.7127404397173622,
-                    "angularVelocity": -0.0000012320607563537726,
-                    "velocityX": 3.247152271058871,
-                    "velocityY": -1.940783210815242,
-                    "timestamp": 1.1813954102955164
-                },
-                {
-                    "x": 4.431867133754474,
-                    "y": 4.825546352606267,
-                    "heading": 2.712740340652136,
-                    "angularVelocity": -0.0000011739618702897745,
-                    "velocityX": 3.136032749351509,
-                    "velocityY": -2.115640810829663,
-                    "timestamp": 1.2657807967451962
-                },
-                {
-                    "x": 4.666216273140183,
-                    "y": 4.608786081760519,
-                    "heading": 2.712740321249029,
-                    "angularVelocity": -2.2993444839053286e-7,
-                    "velocityX": 2.7771294206901915,
-                    "velocityY": -2.5686944145832347,
-                    "timestamp": 1.350166183194876
-                },
-                {
-                    "x": 4.936653737211477,
-                    "y": 4.439174492795635,
-                    "heading": 2.7127404800965853,
-                    "angularVelocity": 0.0000018824059850759979,
-                    "velocityX": 3.2047902539685085,
-                    "velocityY": -2.009964000887954,
-                    "timestamp": 1.4345515696445557
-                },
-                {
-                    "x": 5.233678140767672,
-                    "y": 4.322208601201109,
-                    "heading": 2.7127406283303626,
-                    "angularVelocity": 0.0000017566285300639338,
-                    "velocityX": 3.5198559377731047,
-                    "velocityY": -1.3860917928520893,
-                    "timestamp": 1.5189369560942354
-                },
-                {
-                    "x": 5.5320155723758075,
-                    "y": 4.208633545762192,
-                    "heading": 2.7127407751281813,
-                    "angularVelocity": 0.000001739611855911675,
-                    "velocityX": 3.5354158363194896,
-                    "velocityY": -1.3459090515232621,
-                    "timestamp": 1.6033223425439151
-                },
-                {
-                    "x": 5.844921751101867,
-                    "y": 4.1454334406216455,
-                    "heading": 2.712740834966484,
-                    "angularVelocity": 7.091074065603108e-7,
-                    "velocityX": 3.708061216412759,
-                    "velocityY": -0.748946088885059,
-                    "timestamp": 1.687707728993595
-                },
-                {
-                    "x": 6.164108889380454,
-                    "y": 4.140523996019331,
-                    "heading": 2.7127408721494533,
-                    "angularVelocity": 4.406328022097576e-7,
-                    "velocityX": 3.7824930560568397,
-                    "velocityY": -0.05817884836341004,
-                    "timestamp": 1.7720931154432746
-                },
-                {
-                    "x": 6.483326965734431,
-                    "y": 4.138437864876922,
-                    "heading": 2.712740909220737,
-                    "angularVelocity": 4.393092878808321e-7,
-                    "velocityX": 3.7828596844114597,
-                    "velocityY": -0.02472147406577555,
-                    "timestamp": 1.8564785018929544
-                },
-                {
-                    "x": 6.802545327798344,
-                    "y": 4.1363959875067975,
-                    "heading": 2.712740946289213,
-                    "angularVelocity": 4.392760075001551e-7,
-                    "velocityX": 3.782863070186549,
-                    "velocityY": -0.024197049465056585,
-                    "timestamp": 1.9408638883426341
-                },
-                {
-                    "x": 7.094929199701602,
-                    "y": 4.132589002725807,
-                    "heading": 2.784661628236826,
-                    "angularVelocity": 0.8522883519706954,
-                    "velocityX": 3.464863813565768,
-                    "velocityY": -0.04511426611993158,
-                    "timestamp": 2.025249274792314
-                },
-                {
-                    "x": 7.329645788590582,
-                    "y": 4.130189422490391,
-                    "heading": 2.9078456854173678,
-                    "angularVelocity": 1.4597794993094293,
-                    "velocityX": 2.7814838417425016,
-                    "velocityY": -0.02843596902640405,
-                    "timestamp": 2.1096346612419934
-                },
-                {
-                    "x": 7.5051104174827215,
-                    "y": 4.128539949714214,
-                    "heading": 3.0176429032166108,
-                    "angularVelocity": 1.3011401904845583,
-                    "velocityX": 2.0793248247641927,
-                    "velocityY": -0.01954690077910012,
-                    "timestamp": 2.194020047691673
-                },
-                {
-                    "x": 7.621781937801462,
-                    "y": 4.1275005025427935,
-                    "heading": 3.0976594405390174,
-                    "angularVelocity": 0.9482274205158485,
-                    "velocityX": 1.3826033775209736,
-                    "velocityY": -0.01231785757139544,
-                    "timestamp": 2.2784054341413524
+                    "x": 0.7612279700458028,
+                    "y": 4.384337560333447,
+                    "heading": -0.9750502376080931,
+                    "angularVelocity": 0.7696859194569483,
+                    "velocityX": 0.6070760811828038,
+                    "velocityY": 0.28841179417367857,
+                    "timestamp": 0.0843847610434429
+                },
+                {
+                    "x": 0.8642667385584091,
+                    "y": 4.433290258850185,
+                    "heading": -0.8542284973914237,
+                    "angularVelocity": 1.4317957261788106,
+                    "velocityX": 1.2210589594435701,
+                    "velocityY": 0.5801130193585202,
+                    "timestamp": 0.1687695220868858
+                },
+                {
+                    "x": 1.0199495140663002,
+                    "y": 4.507092326164681,
+                    "heading": -0.6915219404263555,
+                    "angularVelocity": 1.9281509475538106,
+                    "velocityX": 1.8449157594667183,
+                    "velocityY": 0.8745899899690757,
+                    "timestamp": 0.25315428313032873
+                },
+                {
+                    "x": 1.2292345273519185,
+                    "y": 4.606077015104397,
+                    "heading": -0.5137189126301165,
+                    "angularVelocity": 2.1070513869757743,
+                    "velocityX": 2.480128055086372,
+                    "velocityY": 1.173016166850306,
+                    "timestamp": 0.3375390441737716
+                },
+                {
+                    "x": 1.485641598676529,
+                    "y": 4.729632973105035,
+                    "heading": -0.42871970592241276,
+                    "angularVelocity": 1.007281476615537,
+                    "velocityX": 3.038547104406527,
+                    "velocityY": 1.4641975218379706,
+                    "timestamp": 0.4219238052172145
+                },
+                {
+                    "x": 1.7745745246623985,
+                    "y": 4.865356167675212,
+                    "heading": -0.42871965586223265,
+                    "angularVelocity": 5.932372081830595e-7,
+                    "velocityX": 3.4239941242128182,
+                    "velocityY": 1.6083851265555307,
+                    "timestamp": 0.5063085662606575
+                },
+                {
+                    "x": 2.063508085893233,
+                    "y": 5.001078009918069,
+                    "heading": -0.4287196058112515,
+                    "angularVelocity": 5.931281972756146e-7,
+                    "velocityX": 3.4240016521710777,
+                    "velocityY": 1.6083691008265977,
+                    "timestamp": 0.5906933273041004
+                },
+                {
+                    "x": 2.3524294773436862,
+                    "y": 5.136825756955775,
+                    "heading": -0.4287195557601797,
+                    "angularVelocity": 5.931292703362374e-7,
+                    "velocityX": 3.423857434421327,
+                    "velocityY": 1.6086760851027126,
+                    "timestamp": 0.6750780883475433
+                },
+                {
+                    "x": 2.647360543237261,
+                    "y": 5.258967835966316,
+                    "heading": -0.4287195052637089,
+                    "angularVelocity": 5.984074633873495e-7,
+                    "velocityX": 3.4950749666965533,
+                    "velocityY": 1.447442375852424,
+                    "timestamp": 0.7594628493909863
+                },
+                {
+                    "x": 2.959809428031792,
+                    "y": 5.324379711821066,
+                    "heading": -0.42871944828896014,
+                    "angularVelocity": 6.751781731100131e-7,
+                    "velocityX": 3.702669545200082,
+                    "velocityY": 0.7751621862272086,
+                    "timestamp": 0.8438476104344292
+                },
+                {
+                    "x": 3.2789674137401184,
+                    "y": 5.330797896945005,
+                    "heading": -0.4287193686010616,
+                    "angularVelocity": 9.443399202542765e-7,
+                    "velocityX": 3.782175617514861,
+                    "velocityY": 0.0760585802995927,
+                    "timestamp": 0.9282323714778722
+                },
+                {
+                    "x": 3.593793413911528,
+                    "y": 5.278000284107237,
+                    "heading": -0.42871953037751775,
+                    "angularVelocity": -0.000001917128805011847,
+                    "velocityX": 3.7308395055989028,
+                    "velocityY": -0.625677103128148,
+                    "timestamp": 1.0126171325213151
+                },
+                {
+                    "x": 3.893396265271842,
+                    "y": 5.167813441772834,
+                    "heading": -0.42871965571467335,
+                    "angularVelocity": -0.0000014853055701792625,
+                    "velocityX": 3.550437871193987,
+                    "velocityY": -1.3057670718262655,
+                    "timestamp": 1.097001893564758
+                },
+                {
+                    "x": 4.167411681517661,
+                    "y": 5.0040496950416875,
+                    "heading": -0.4287197602752934,
+                    "angularVelocity": -0.0000012390936318239587,
+                    "velocityX": 3.247214459785327,
+                    "velocityY": -1.9406791546980178,
+                    "timestamp": 1.181386654608201
+                },
+                {
+                    "x": 4.4320204493985145,
+                    "y": 4.825485382292427,
+                    "heading": -0.42871985986303984,
+                    "angularVelocity": -0.0000011801626881351292,
+                    "velocityX": 3.1357411528921184,
+                    "velocityY": -2.1160729797797915,
+                    "timestamp": 1.265771415651644
+                },
+                {
+                    "x": 4.666313927400726,
+                    "y": 4.608668432226026,
+                    "heading": -0.4287198793836171,
+                    "angularVelocity": -2.3132822841838536e-7,
+                    "velocityX": 2.77649038884734,
+                    "velocityY": -2.5693851281356843,
+                    "timestamp": 1.350156176695087
+                },
+                {
+                    "x": 4.936706954544898,
+                    "y": 4.438990462685779,
+                    "heading": -0.4287197198154701,
+                    "angularVelocity": 0.0000018909592816311697,
+                    "velocityX": 3.204287406881873,
+                    "velocityY": -2.010765539204243,
+                    "timestamp": 1.4345409377385299
+                },
+                {
+                    "x": 5.233776210787073,
+                    "y": 4.322144997944839,
+                    "heading": -0.42871957094251306,
+                    "angularVelocity": 0.0000017642161351814778,
+                    "velocityX": 3.5204135506067917,
+                    "velocityY": -1.384674949557486,
+                    "timestamp": 1.5189256987819728
+                },
+                {
+                    "x": 5.532138329985476,
+                    "y": 4.208641466951728,
+                    "heading": -0.42871942351676046,
+                    "angularVelocity": 0.0000017470660671471722,
+                    "velocityX": 3.5357345983929718,
+                    "velocityY": -1.3450714274656952,
+                    "timestamp": 1.6033104598254158
+                },
+                {
+                    "x": 5.845046534221127,
+                    "y": 4.145463344127181,
+                    "heading": -0.42871936362457447,
+                    "angularVelocity": 7.097512069029325e-7,
+                    "velocityX": 3.708112701469523,
+                    "velocityY": -0.7486911385809435,
+                    "timestamp": 1.6876952208688587
+                },
+                {
+                    "x": 6.164234411105391,
+                    "y": 4.140760073731674,
+                    "heading": -0.4287193263126739,
+                    "angularVelocity": 4.4216396519651743e-7,
+                    "velocityX": 3.7825298423246796,
+                    "velocityY": -0.055736016049057806,
+                    "timestamp": 1.7720799819123016
+                },
+                {
+                    "x": 6.483450051427475,
+                    "y": 4.138663317552119,
+                    "heading": -0.42871928910258295,
+                    "angularVelocity": 4.4095747260310005e-7,
+                    "velocityX": 3.782858852414673,
+                    "velocityY": -0.024847569082867782,
+                    "timestamp": 1.8564647429557446
+                },
+                {
+                    "x": 6.802665691714121,
+                    "y": 4.136566553568004,
+                    "heading": -0.4287192518958351,
+                    "angularVelocity": 4.4091785477775863e-7,
+                    "velocityX": 3.7828588519947024,
+                    "velocityY": -0.024847661569458286,
+                    "timestamp": 1.9408495039991875
+                },
+                {
+                    "x": 7.0950029322992965,
+                    "y": 4.132696093446151,
+                    "heading": -0.35666839777179793,
+                    "angularVelocity": 0.853837271482519,
+                    "velocityX": 3.4643368893901423,
+                    "velocityY": -0.04586681379319509,
+                    "timestamp": 2.0252342650426303
+                },
+                {
+                    "x": 7.329695147990892,
+                    "y": 4.13025195968354,
+                    "heading": -0.23302792235716066,
+                    "angularVelocity": 1.465199093839195,
+                    "velocityX": 2.7812156222231823,
+                    "velocityY": -0.02896416049909081,
+                    "timestamp": 2.109619026086073
+                },
+                {
+                    "x": 7.505137224924894,
+                    "y": 4.1285705338055685,
+                    "heading": -0.12281843303988041,
+                    "angularVelocity": 1.3060354494646,
+                    "velocityX": 2.0790729838492856,
+                    "velocityY": -0.019925705271038107,
+                    "timestamp": 2.194003787129516
+                },
+                {
+                    "x": 7.621791455543556,
+                    "y": 4.127510522704197,
+                    "heading": -0.04250049300917318,
+                    "angularVelocity": 0.9518062152170125,
+                    "velocityX": 1.3824087332380297,
+                    "velocityY": -0.012561641322782502,
+                    "timestamp": 2.278388548172959
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": 0.5017522730222338,
-                    "velocityX": 0.6899069216594048,
-                    "velocityY": -0.00593115187164541,
-                    "timestamp": 2.362790820591032
+                    "heading": -1.5179160305718205e-31,
+                    "angularVelocity": 0.5036512811515541,
+                    "velocityX": 0.6897992449901723,
+                    "velocityY": -0.0060499395609808024,
+                    "timestamp": 2.362773309216402
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": 3.889752365812458e-29,
+                    "heading": 0,
+                    "angularVelocity": 2.3304076579629275e-30,
                     "velocityX": 0,
                     "velocityY": 0,
-                    "timestamp": 2.4471762070407115
+                    "timestamp": 2.447158070259845
                 }
             ],
             "constraints": [
@@ -8023,16 +8191,16 @@
                 {
                     "x": 0.71,
                     "y": 4.36,
-                    "heading": 2.1,
+                    "heading": -1.04,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 22
+                    "controlIntervalCount": 33
                 },
                 {
                     "x": 2.4943342208862305,
                     "y": 2.6289942264556885,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -8041,7 +8209,7 @@
                 {
                     "x": 5.236664772033691,
                     "y": 1.4238771200180054,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -8050,7 +8218,7 @@
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -8061,578 +8229,677 @@
                 {
                     "x": 0.71,
                     "y": 4.36,
-                    "heading": 2.1,
+                    "heading": -1.04,
                     "angularVelocity": 0,
-                    "velocityX": 4.814287914507576e-35,
-                    "velocityY": 2.4063306961726914e-34,
+                    "velocityX": 0,
+                    "velocityY": 2.687708804701003e-35,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7189874993597535,
-                    "y": 4.351109558323372,
-                    "heading": 2.1116856791570084,
-                    "angularVelocity": 0.2954657391089134,
-                    "velocityX": 0.22724380032956057,
-                    "velocityY": -0.22478975211418176,
-                    "timestamp": 0.039550031053517704
-                },
-                {
-                    "x": 0.7369922825493326,
-                    "y": 4.3333146846737405,
-                    "heading": 2.134597544316128,
-                    "angularVelocity": 0.5793134556106004,
-                    "velocityX": 0.45524068401401757,
-                    "velocityY": -0.4499332409006739,
-                    "timestamp": 0.07910006210703541
-                },
-                {
-                    "x": 0.7640484897224145,
-                    "y": 4.306598997837255,
-                    "heading": 2.168179876708578,
-                    "angularVelocity": 0.849110139686295,
-                    "velocityX": 0.6841007820315073,
-                    "velocityY": -0.67549091934555,
-                    "timestamp": 0.11865009316055311
-                },
-                {
-                    "x": 0.800195666694539,
-                    "y": 4.270941917715144,
-                    "heading": 2.2117258914758198,
-                    "angularVelocity": 1.1010361713323769,
-                    "velocityX": 0.9139607734621368,
-                    "velocityY": -0.9015689538615157,
-                    "timestamp": 0.15820012421407081
-                },
-                {
-                    "x": 0.8454794987248587,
-                    "y": 4.226316705338956,
-                    "heading": 2.264304991238856,
-                    "angularVelocity": 1.3294325784950254,
-                    "velocityX": 1.14497589063946,
-                    "velocityY": -1.1283230679592324,
-                    "timestamp": 0.19775015526758852
-                },
-                {
-                    "x": 0.8999520283363762,
-                    "y": 4.172689027996317,
-                    "heading": 2.324662422471035,
-                    "angularVelocity": 1.5261032576815274,
-                    "velocityX": 1.3773068733576257,
-                    "velocityY": -1.3559452651268766,
-                    "timestamp": 0.23730018632110622
-                },
-                {
-                    "x": 0.963671089377297,
-                    "y": 4.110017781664974,
-                    "heading": 2.391063304695866,
-                    "angularVelocity": 1.6789084725364454,
-                    "velocityX": 1.6111001519745567,
-                    "velocityY": -1.584606754076596,
-                    "timestamp": 0.2768502173746239
-                },
-                {
-                    "x": 1.0366970385232381,
-                    "y": 4.038262577868419,
-                    "heading": 2.4609901353872035,
-                    "angularVelocity": 1.7680600704640437,
-                    "velocityX": 1.846419514743869,
-                    "velocityY": -1.8142894426418532,
-                    "timestamp": 0.31640024842814163
-                },
-                {
-                    "x": 1.1190717343496346,
-                    "y": 3.9574142704891733,
-                    "heading": 2.5303614875414775,
-                    "angularVelocity": 1.7540151121601602,
-                    "velocityX": 2.082797247742485,
-                    "velocityY": -2.0442033855762225,
-                    "timestamp": 0.35595027948165936
-                },
-                {
-                    "x": 1.21066072775428,
-                    "y": 3.867689453216591,
-                    "heading": 2.590569114237573,
-                    "angularVelocity": 1.522315535343692,
-                    "velocityX": 2.3157755117994827,
-                    "velocityY": -2.268640880488067,
-                    "timestamp": 0.3955003105351771
-                },
-                {
-                    "x": 1.310358949370952,
-                    "y": 3.7710393719100375,
-                    "heading": 2.620822405939027,
-                    "angularVelocity": 0.764937242666538,
-                    "velocityX": 2.5208127265883533,
-                    "velocityY": -2.4437422356450176,
-                    "timestamp": 0.4350503415886948
-                },
-                {
-                    "x": 1.418053038225142,
-                    "y": 3.6673441481868845,
-                    "heading": 2.6211471035683176,
-                    "angularVelocity": 0.008209794547368329,
-                    "velocityX": 2.722983673728656,
-                    "velocityY": -2.621874647401313,
-                    "timestamp": 0.47460037264221255
-                },
-                {
-                    "x": 1.5245836891775304,
-                    "y": 3.5622918530292758,
-                    "heading": 2.6211471537170588,
-                    "angularVelocity": 0.0000012679823435232559,
-                    "velocityX": 2.6935668092961826,
-                    "velocityY": -2.656187425376634,
-                    "timestamp": 0.5141504036957303
-                },
-                {
-                    "x": 1.6311142725711645,
-                    "y": 3.4572394893622254,
-                    "heading": 2.621147203865785,
-                    "angularVelocity": 0.0000012679819635220034,
-                    "velocityX": 2.6935651011115636,
-                    "velocityY": -2.6561891575988192,
-                    "timestamp": 0.553700434749248
-                },
-                {
-                    "x": 1.7376448559612427,
-                    "y": 3.3521871256915694,
-                    "heading": 2.621147254014511,
-                    "angularVelocity": 0.0000012679819538520794,
-                    "velocityX": 2.6935651010216595,
-                    "velocityY": -2.656189157689989,
-                    "timestamp": 0.5932504658027657
-                },
-                {
-                    "x": 1.8441754393513208,
-                    "y": 3.2471347620209134,
-                    "heading": 2.621147304163236,
-                    "angularVelocity": 0.000001267981944384733,
-                    "velocityX": 2.693565101021656,
-                    "velocityY": -2.656189157689993,
-                    "timestamp": 0.6328004968562835
-                },
-                {
-                    "x": 1.9507060227413988,
-                    "y": 3.142082398350257,
-                    "heading": 2.621147354311961,
-                    "angularVelocity": 0.0000012679819349678633,
-                    "velocityX": 2.6935651010216572,
-                    "velocityY": -2.656189157689994,
-                    "timestamp": 0.6723505279098012
-                },
-                {
-                    "x": 2.057236606131477,
-                    "y": 3.037030034679601,
-                    "heading": 2.6211474044606855,
-                    "angularVelocity": 0.0000012679819252039202,
-                    "velocityX": 2.693565101021662,
-                    "velocityY": -2.6561891576899894,
-                    "timestamp": 0.7119005589633189
-                },
-                {
-                    "x": 2.1637671895248896,
-                    "y": 2.931977671012326,
-                    "heading": 2.6211474546094093,
-                    "angularVelocity": 0.0000012679819154308074,
-                    "velocityX": 2.69356510110597,
-                    "velocityY": -2.656189157604496,
-                    "timestamp": 0.7514505900168367
-                },
-                {
-                    "x": 2.2702978362708315,
-                    "y": 2.8269253715890703,
-                    "heading": 2.6211475047581403,
-                    "angularVelocity": 0.0000012679820770656017,
-                    "velocityX": 2.693566702938569,
-                    "velocityY": -2.6561875332310767,
-                    "timestamp": 0.7910006210703544
-                },
-                {
-                    "x": 2.3779994803856632,
-                    "y": 2.7230739385096507,
-                    "heading": 2.6211475550861425,
-                    "angularVelocity": 0.0000012725148543062649,
-                    "velocityX": 2.723174704189075,
-                    "velocityY": -2.6258243119680853,
-                    "timestamp": 0.8305506521238721
+                    "x": 0.7140616140612069,
+                    "y": 4.355992841184312,
+                    "heading": -1.0347167301054676,
+                    "angularVelocity": 0.1988304134727778,
+                    "velocityX": 0.15285465616518656,
+                    "velocityY": -0.15080528916361516,
+                    "timestamp": 0.026571739213607736
+                },
+                {
+                    "x": 0.7221925258884458,
+                    "y": 4.3479754290630535,
+                    "heading": -1.0242624006876555,
+                    "angularVelocity": 0.39343790535390527,
+                    "velocityX": 0.30599848063671325,
+                    "velocityY": -0.3017270362623706,
+                    "timestamp": 0.05314347842721547
+                },
+                {
+                    "x": 0.7344011981721603,
+                    "y": 4.335944497876898,
+                    "heading": -1.0087613580778494,
+                    "angularVelocity": 0.5833657513042195,
+                    "velocityX": 0.45946078973491633,
+                    "velocityY": -0.4527716868451825,
+                    "timestamp": 0.0797152176408232
+                },
+                {
+                    "x": 0.7506970627866689,
+                    "y": 4.31989642008046,
+                    "heading": -0.9883558514269565,
+                    "angularVelocity": 0.7679401971717026,
+                    "velocityX": 0.6132780576953482,
+                    "velocityY": -0.6039528563572185,
+                    "timestamp": 0.10628695685443094
+                },
+                {
+                    "x": 0.7710907256657638,
+                    "y": 4.29982697901967,
+                    "heading": -0.963213241169254,
+                    "angularVelocity": 0.9462162057057438,
+                    "velocityX": 0.7674944690353961,
+                    "velocityY": -0.7552927152962536,
+                    "timestamp": 0.13285869606803868
+                },
+                {
+                    "x": 0.7955941760833964,
+                    "y": 4.27573110299105,
+                    "heading": -0.9335350310204893,
+                    "angularVelocity": 1.1169088297226009,
+                    "velocityX": 0.9221620843352277,
+                    "velocityY": -0.9068234425649082,
+                    "timestamp": 0.1594304352816464
+                },
+                {
+                    "x": 0.8242209914920048,
+                    "y": 4.247602572989807,
+                    "heading": -0.8995682913456332,
+                    "angularVelocity": 1.2783032153748224,
+                    "velocityX": 1.0773406730541775,
+                    "velocityY": -1.0585882156647894,
+                    "timestamp": 0.18600217449525414
+                },
+                {
+                    "x": 0.856986533327333,
+                    "y": 4.215433742997202,
+                    "heading": -0.8616204330063739,
+                    "angularVelocity": 1.4281285103018635,
+                    "velocityX": 1.2330973735640383,
+                    "velocityY": -1.210640738794065,
+                    "timestamp": 0.21257391370886186
+                },
+                {
+                    "x": 0.893908134971617,
+                    "y": 4.179215360896215,
+                    "heading": -0.8200791656995261,
+                    "angularVelocity": 1.5633627506615773,
+                    "velocityX": 1.3895063980372095,
+                    "velocityY": -1.3630414558050108,
+                    "timestamp": 0.2391456529224696
+                },
+                {
+                    "x": 0.9350052770321623,
+                    "y": 4.138936674970777,
+                    "heading": -0.7754414952973147,
+                    "angularVelocity": 1.6798926876172218,
+                    "velocityX": 1.546648555074586,
+                    "velocityY": -1.5158468025612346,
+                    "timestamp": 0.2657173921360773
+                },
+                {
+                    "x": 0.9802996637151041,
+                    "y": 4.094586228803994,
+                    "heading": -0.7283605055619151,
+                    "angularVelocity": 1.7718444907546276,
+                    "velocityX": 1.7046075275248067,
+                    "velocityY": -1.669083299751435,
+                    "timestamp": 0.29228913134968504
+                },
+                {
+                    "x": 1.0298146765069804,
+                    "y": 4.046154350498487,
+                    "heading": -0.6797316150493184,
+                    "angularVelocity": 1.8300981400454652,
+                    "velocityX": 1.8634464381059026,
+                    "velocityY": -1.8226837888242364,
+                    "timestamp": 0.31886087056329276
+                },
+                {
+                    "x": 1.0835715343165668,
+                    "y": 3.9936405744754793,
+                    "heading": -0.6308802721211105,
+                    "angularVelocity": 1.8384699072761677,
+                    "velocityX": 2.0230839004342207,
+                    "velocityY": -1.9763017994740086,
+                    "timestamp": 0.3454326097769005
+                },
+                {
+                    "x": 1.1415677463510772,
+                    "y": 3.937082152581159,
+                    "heading": -0.5840793547040092,
+                    "angularVelocity": 1.7613042579137594,
+                    "velocityX": 2.1826276243449603,
+                    "velocityY": -2.12851787531304,
+                    "timestamp": 0.3720043489905082
+                },
+                {
+                    "x": 1.203636670620202,
+                    "y": 3.876777756204273,
+                    "heading": -0.5447644235662668,
+                    "angularVelocity": 1.4795768851144226,
+                    "velocityX": 2.33589994882,
+                    "velocityY": -2.2694937614773476,
+                    "timestamp": 0.39857608820411594
+                },
+                {
+                    "x": 1.2693974629594884,
+                    "y": 3.8133907541919347,
+                    "heading": -0.5190150912676422,
+                    "angularVelocity": 0.969049563960718,
+                    "velocityX": 2.4748395959572558,
+                    "velocityY": -2.3855044452595306,
+                    "timestamp": 0.42514782741772367
+                },
+                {
+                    "x": 1.3388335264794184,
+                    "y": 3.7468848835898942,
+                    "heading": -0.5067181615747146,
+                    "angularVelocity": 0.4627822663045784,
+                    "velocityX": 2.613154636275026,
+                    "velocityY": -2.5028798479243823,
+                    "timestamp": 0.4517195666313314
+                },
+                {
+                    "x": 1.410439403600934,
+                    "y": 3.676338986398213,
+                    "heading": -0.5067181155488927,
+                    "angularVelocity": 0.0000017321343363764108,
+                    "velocityX": 2.6948133332892708,
+                    "velocityY": -2.6549220818618244,
+                    "timestamp": 0.4782913058449391
+                },
+                {
+                    "x": 1.482040293717765,
+                    "y": 3.605788027611728,
+                    "heading": -0.5067180695571892,
+                    "angularVelocity": 0.0000017308503249172533,
+                    "velocityX": 2.694625652511417,
+                    "velocityY": -2.655112569761896,
+                    "timestamp": 0.5048630450585468
+                },
+                {
+                    "x": 1.5536411829944505,
+                    "y": 3.5352370679725946,
+                    "heading": -0.5067180235654862,
+                    "angularVelocity": 0.0000017308503100877725,
+                    "velocityX": 2.6946256208934063,
+                    "velocityY": -2.6551126018504427,
+                    "timestamp": 0.5314347842721546
+                },
+                {
+                    "x": 1.6252420722709946,
+                    "y": 3.4646861083333174,
+                    "heading": -0.5067179775737835,
+                    "angularVelocity": 0.0000017308502987237163,
+                    "velocityX": 2.6946256208880808,
+                    "velocityY": -2.6551126018558486,
+                    "timestamp": 0.5580065234857624
+                },
+                {
+                    "x": 1.6968429615475384,
+                    "y": 3.39413514869404,
+                    "heading": -0.506717931582081,
+                    "angularVelocity": 0.0000017308502871423956,
+                    "velocityX": 2.6946256208880808,
+                    "velocityY": -2.65511260185585,
+                    "timestamp": 0.5845782626993702
+                },
+                {
+                    "x": 1.7684438508240825,
+                    "y": 3.323584189054763,
+                    "heading": -0.5067178855903789,
+                    "angularVelocity": 0.0000017308502762001758,
+                    "velocityX": 2.694625620888081,
+                    "velocityY": -2.65511260185585,
+                    "timestamp": 0.611150001912978
+                },
+                {
+                    "x": 1.8400447401006266,
+                    "y": 3.2530332294154856,
+                    "heading": -0.506717839598677,
+                    "angularVelocity": 0.000001730850265567339,
+                    "velocityX": 2.6946256208880826,
+                    "velocityY": -2.6551126018558495,
+                    "timestamp": 0.6377217411265858
+                },
+                {
+                    "x": 1.9116456293771706,
+                    "y": 3.1824822697762087,
+                    "heading": -0.5067177936069754,
+                    "angularVelocity": 0.0000017308502535085818,
+                    "velocityX": 2.694625620888084,
+                    "velocityY": -2.65511260185585,
+                    "timestamp": 0.6642934803401935
+                },
+                {
+                    "x": 1.9832465186537145,
+                    "y": 3.1119313101369315,
+                    "heading": -0.5067177476152741,
+                    "angularVelocity": 0.0000017308502432657092,
+                    "velocityX": 2.6946256208880843,
+                    "velocityY": -2.65511260185585,
+                    "timestamp": 0.6908652195538013
+                },
+                {
+                    "x": 2.0548474079302586,
+                    "y": 3.041380350497654,
+                    "heading": -0.5067177016235731,
+                    "angularVelocity": 0.000001730850231529751,
+                    "velocityX": 2.694625620888085,
+                    "velocityY": -2.6551126018558504,
+                    "timestamp": 0.7174369587674091
+                },
+                {
+                    "x": 2.1264482972068026,
+                    "y": 2.970829390858377,
+                    "heading": -0.5067176556318724,
+                    "angularVelocity": 0.0000017308502204807436,
+                    "velocityX": 2.6946256208880865,
+                    "velocityY": -2.6551126018558495,
+                    "timestamp": 0.7440086979810169
+                },
+                {
+                    "x": 2.1980491864833867,
+                    "y": 2.9002784312191405,
+                    "heading": -0.5067176096401721,
+                    "angularVelocity": 0.0000017308502087562552,
+                    "velocityX": 2.694625620889591,
+                    "velocityY": -2.6551126018543236,
+                    "timestamp": 0.7705804371946247
+                },
+                {
+                    "x": 2.2696500759972245,
+                    "y": 2.829727471820689,
+                    "heading": -0.506717563648472,
+                    "angularVelocity": 0.0000017308501985869009,
+                    "velocityX": 2.6946256298184115,
+                    "velocityY": -2.6551125927926265,
+                    "timestamp": 0.7971521764082324
+                },
+                {
+                    "x": 2.3412523738653572,
+                    "y": 2.7591779417640674,
+                    "heading": -0.5067175176565489,
+                    "angularVelocity": 0.0000017308585893832803,
+                    "velocityX": 2.694678631779742,
+                    "velocityY": -2.6550588009870313,
+                    "timestamp": 0.8237239156218402
+                },
+                {
+                    "x": 2.4158866145337003,
+                    "y": 2.6918440324740667,
+                    "heading": -0.5067174710515008,
+                    "angularVelocity": 0.000001753932919872258,
+                    "velocityX": 2.808782672009746,
+                    "velocityY": -2.5340422299311864,
+                    "timestamp": 0.850295654835448
                 },
                 {
                     "x": 2.4943342208862305,
                     "y": 2.6289942264556885,
-                    "heading": 2.6211476076664186,
-                    "angularVelocity": 0.0000013294623338808287,
-                    "velocityX": 2.941457627255649,
-                    "velocityY": -2.378751913662396,
-                    "timestamp": 0.8701006831773899
-                },
-                {
-                    "x": 2.6932881359026166,
-                    "y": 2.508539731368457,
-                    "heading": 2.6211476558145916,
-                    "angularVelocity": 7.831464659210102e-7,
-                    "velocityX": 3.236053335597206,
-                    "velocityY": -1.9592334766702564,
-                    "timestamp": 0.9315811021889849
-                },
-                {
-                    "x": 2.899938569108162,
-                    "y": 2.401827293887261,
-                    "heading": 2.621147703190774,
-                    "angularVelocity": 7.705897835721374e-7,
-                    "velocityX": 3.361239830954497,
-                    "velocityY": -1.7357142192064432,
-                    "timestamp": 0.99306152120058
-                },
-                {
-                    "x": 3.106589142147435,
-                    "y": 2.295115127196865,
-                    "heading": 2.6211477505669496,
-                    "angularVelocity": 7.705896615935058e-7,
-                    "velocityX": 3.361242105397809,
-                    "velocityY": -1.7357098147016614,
-                    "timestamp": 1.0545419402121752
-                },
-                {
-                    "x": 3.313239715188592,
-                    "y": 2.188402960510117,
-                    "heading": 2.621147797943125,
-                    "angularVelocity": 7.705896625567414e-7,
-                    "velocityX": 3.361242105428448,
-                    "velocityY": -1.735709814642326,
-                    "timestamp": 1.1160223592237704
-                },
-                {
-                    "x": 3.519890288229749,
-                    "y": 2.0816907938233693,
-                    "heading": 2.6211478453193005,
-                    "angularVelocity": 7.705896648559933e-7,
-                    "velocityX": 3.361242105428449,
-                    "velocityY": -1.735709814642324,
-                    "timestamp": 1.1775027782353655
-                },
-                {
-                    "x": 3.726540861270906,
-                    "y": 1.9749786271366212,
-                    "heading": 2.621147892695476,
-                    "angularVelocity": 7.705896659187971e-7,
-                    "velocityX": 3.361242105428449,
-                    "velocityY": -1.7357098146423233,
-                    "timestamp": 1.2389831972469607
-                },
-                {
-                    "x": 3.933191434312063,
-                    "y": 1.868266460449873,
-                    "heading": 2.6211479400716513,
-                    "angularVelocity": 7.705896660607392e-7,
-                    "velocityX": 3.36124210542845,
-                    "velocityY": -1.7357098146423227,
-                    "timestamp": 1.3004636162585559
-                },
-                {
-                    "x": 4.139842007353606,
-                    "y": 1.7615542937638724,
-                    "heading": 2.6211479874478267,
-                    "angularVelocity": 7.705896681379808e-7,
-                    "velocityX": 3.361242105434726,
-                    "velocityY": -1.7357098146301677,
-                    "timestamp": 1.361944035270151
-                },
-                {
-                    "x": 4.3464926090376705,
-                    "y": 1.6548421825447925,
-                    "heading": 2.621148034824002,
-                    "angularVelocity": 7.705896639238046e-7,
-                    "velocityX": 3.361242571315101,
-                    "velocityY": -1.7357089124417728,
-                    "timestamp": 1.4234244542817462
-                },
-                {
-                    "x": 4.555173710801168,
-                    "y": 1.5521575021408582,
-                    "heading": 2.6211480823085673,
-                    "angularVelocity": 7.723526622271063e-7,
-                    "velocityX": 3.3942693481666213,
-                    "velocityY": -1.6702013755724032,
-                    "timestamp": 1.4849048732933414
-                },
-                {
-                    "x": 4.775821132533127,
-                    "y": 1.478627504505032,
-                    "heading": 2.621148131521485,
-                    "angularVelocity": 8.004649014817169e-7,
-                    "velocityX": 3.5889056268524353,
-                    "velocityY": -1.1959905091401257,
-                    "timestamp": 1.5463852923049366
-                },
-                {
-                    "x": 5.004383859827403,
-                    "y": 1.4356039834024816,
-                    "heading": 2.621148186381714,
-                    "angularVelocity": 8.923203458439086e-7,
-                    "velocityX": 3.7176507735116338,
-                    "velocityY": -0.699792255716998,
-                    "timestamp": 1.6078657113165318
+                    "heading": -0.5067174224716422,
+                    "angularVelocity": 0.000001828252870098291,
+                    "velocityX": 2.952294756541798,
+                    "velocityY": -2.365287628074865,
+                    "timestamp": 0.8768673940490558
+                },
+                {
+                    "x": 2.693828949223235,
+                    "y": 2.509453130751911,
+                    "heading": -0.5067173754009504,
+                    "angularVelocity": 7.656472375459629e-7,
+                    "velocityX": 3.244961599231293,
+                    "velocityY": -1.9444436869204162,
+                    "timestamp": 0.9383456957905196
+                },
+                {
+                    "x": 2.900420237054028,
+                    "y": 2.4026436735913763,
+                    "heading": -0.5067173289743672,
+                    "angularVelocity": 7.551702291921459e-7,
+                    "velocityX": 3.3603935368868174,
+                    "velocityY": -1.7373521085488381,
+                    "timestamp": 0.9998239975319835
+                },
+                {
+                    "x": 3.1070116472391316,
+                    "y": 2.295834453089378,
+                    "heading": -0.5067172825477892,
+                    "angularVelocity": 7.55170142432061e-7,
+                    "velocityX": 3.3603955270899863,
+                    "velocityY": -1.7373482590844154,
+                    "timestamp": 1.0613022992734473
+                },
+                {
+                    "x": 3.3136030574258837,
+                    "y": 2.1890252325905677,
+                    "heading": -0.5067172361212112,
+                    "angularVelocity": 7.551701448333524e-7,
+                    "velocityX": 3.3603955271167996,
+                    "velocityY": -1.737348259032553,
+                    "timestamp": 1.1227806010149113
+                },
+                {
+                    "x": 3.5201944676126358,
+                    "y": 2.0822160120917577,
+                    "heading": -0.5067171896946332,
+                    "angularVelocity": 7.551701452492318e-7,
+                    "velocityX": 3.3603955271167996,
+                    "velocityY": -1.737348259032552,
+                    "timestamp": 1.1842589027563752
+                },
+                {
+                    "x": 3.726785877799388,
+                    "y": 1.9754067915929476,
+                    "heading": -0.5067171432680552,
+                    "angularVelocity": 7.5517014486464e-7,
+                    "velocityX": 3.3603955271168005,
+                    "velocityY": -1.7373482590325509,
+                    "timestamp": 1.2457372044978392
+                },
+                {
+                    "x": 3.93337728798614,
+                    "y": 1.8685975710941374,
+                    "heading": -0.506717096841477,
+                    "angularVelocity": 7.551701472516521e-7,
+                    "velocityX": 3.360395527116801,
+                    "velocityY": -1.7373482590325502,
+                    "timestamp": 1.3072155062393032
+                },
+                {
+                    "x": 4.139968698173285,
+                    "y": 1.761788350596088,
+                    "heading": -0.5067170504148988,
+                    "angularVelocity": 7.551701467640916e-7,
+                    "velocityX": 3.360395527123197,
+                    "velocityY": -1.7373482590201774,
+                    "timestamp": 1.3686938079807671
+                },
+                {
+                    "x": 4.346560137549085,
+                    "y": 1.6549791865550314,
+                    "heading": -0.5067170039883208,
+                    "angularVelocity": 7.55170145558408e-7,
+                    "velocityX": 3.36039600190298,
+                    "velocityY": -1.737347340696294,
+                    "timestamp": 1.430172109722231
+                },
+                {
+                    "x": 4.5552180589852185,
+                    "y": 1.552265545137979,
+                    "heading": -0.5067169574392034,
+                    "angularVelocity": 7.571633584219393e-7,
+                    "velocityX": 3.3940091955305984,
+                    "velocityY": -1.6707299731374539,
+                    "timestamp": 1.491650411463695
+                },
+                {
+                    "x": 4.775846094749007,
+                    "y": 1.478702717632789,
+                    "heading": -0.5067169090858323,
+                    "angularVelocity": 7.865111701229533e-7,
+                    "velocityX": 3.5887138960279197,
+                    "velocityY": -1.1965657056459655,
+                    "timestamp": 1.553128713205159
+                },
+                {
+                    "x": 5.004393851392972,
+                    "y": 1.4356429788646896,
+                    "heading": -0.5067168550876733,
+                    "angularVelocity": 8.783287361409535e-7,
+                    "velocityX": 3.717535295706156,
+                    "velocityY": -0.7004054690576753,
+                    "timestamp": 1.614607014946623
                 },
                 {
                     "x": 5.236664772033691,
                     "y": 1.4238771200180054,
-                    "heading": 2.6211482490896194,
-                    "angularVelocity": 0.0000010199654837334,
-                    "velocityX": 3.7781283202133062,
-                    "velocityY": -0.1907414356148787,
-                    "timestamp": 1.669346130328127
-                },
-                {
-                    "x": 5.366075062772529,
-                    "y": 1.4271130159839853,
-                    "heading": 2.621148315155484,
-                    "angularVelocity": 0.0000019306429598932118,
-                    "velocityX": 3.781757302483639,
-                    "velocityY": 0.09456259722124823,
-                    "timestamp": 1.703565748454824
-                },
-                {
-                    "x": 5.494873366116125,
-                    "y": 1.4400934888882522,
-                    "heading": 2.621148371231603,
-                    "angularVelocity": 0.00000163871257714578,
-                    "velocityX": 3.7638731930533114,
-                    "velocityY": 0.3793283974183189,
-                    "timestamp": 1.7377853665815208
-                },
-                {
-                    "x": 5.622326965929087,
-                    "y": 1.4627446781635232,
-                    "heading": 2.6211484208273035,
-                    "angularVelocity": 0.0000014493353027620846,
-                    "velocityX": 3.724576917868275,
-                    "velocityY": 0.6619357700429461,
-                    "timestamp": 1.7720049847082178
-                },
-                {
-                    "x": 5.747710804313215,
-                    "y": 1.4949376920061848,
-                    "heading": 2.6211484662381075,
-                    "angularVelocity": 0.000001327040057798138,
-                    "velocityX": 3.6640922736162436,
-                    "velocityY": 0.9407765371158774,
-                    "timestamp": 1.8062246028349147
-                },
-                {
-                    "x": 5.8703116218973515,
-                    "y": 1.5364893102203447,
-                    "heading": 2.62114850910788,
-                    "angularVelocity": 0.000001252783485820186,
-                    "velocityX": 3.5827640486872516,
-                    "velocityY": 1.2142630598715765,
-                    "timestamp": 1.8404442209616116
-                },
-                {
-                    "x": 5.989432110510927,
-                    "y": 1.5871628422758242,
-                    "heading": 2.621148550731445,
-                    "angularVelocity": 0.00000121636555358505,
-                    "velocityX": 3.481058385062543,
-                    "velocityY": 1.4808327745757486,
-                    "timestamp": 1.8746638390883086
-                },
-                {
-                    "x": 6.104583123843471,
-                    "y": 1.6463045521525654,
-                    "heading": 2.621148592213924,
-                    "angularVelocity": 0.0000012122426079588706,
-                    "velocityX": 3.3650583973847925,
-                    "velocityY": 1.7282983596652888,
-                    "timestamp": 1.9088834572150055
-                },
-                {
-                    "x": 6.2197290464685,
-                    "y": 1.7054561727682278,
-                    "heading": 2.621148633696581,
-                    "angularVelocity": 0.0000012122477903456845,
-                    "velocityX": 3.3649096316243146,
-                    "velocityY": 1.7285879812175449,
-                    "timestamp": 1.9431030753417025
-                },
-                {
-                    "x": 6.334874968704583,
-                    "y": 1.7646077941410194,
-                    "heading": 2.6211486751792368,
-                    "angularVelocity": 0.0000012122477871677539,
-                    "velocityX": 3.364909620258179,
-                    "velocityY": 1.7285880033431293,
-                    "timestamp": 1.9773226934683994
-                },
-                {
-                    "x": 6.450020890940638,
-                    "y": 1.8237594155138686,
-                    "heading": 2.6211487166618928,
-                    "angularVelocity": 0.000001212247782062354,
-                    "velocityX": 3.364909620257311,
-                    "velocityY": 1.7285880033448198,
-                    "timestamp": 2.0115423115950963
-                },
-                {
-                    "x": 6.565166813176692,
-                    "y": 1.882911036886718,
-                    "heading": 2.621148758144549,
-                    "angularVelocity": 0.0000012122477787509118,
-                    "velocityX": 3.3649096202573117,
-                    "velocityY": 1.7285880033448189,
-                    "timestamp": 2.0457619297217935
-                },
-                {
-                    "x": 6.68031273541274,
-                    "y": 1.9420626582595795,
-                    "heading": 2.6211487996272047,
-                    "angularVelocity": 0.0000012122477734983372,
-                    "velocityX": 3.3649096202571287,
-                    "velocityY": 1.7285880033451764,
-                    "timestamp": 2.0799815478484907
-                },
-                {
-                    "x": 6.79545865756654,
-                    "y": 2.001214279792547,
-                    "heading": 2.6211488411098602,
-                    "angularVelocity": 0.0000012122477688304903,
-                    "velocityX": 3.3649096178535927,
-                    "velocityY": 1.7285880080239562,
-                    "timestamp": 2.114201165975188
-                },
-                {
-                    "x": 6.910603503099618,
-                    "y": 2.0603679970417836,
-                    "heading": 2.621148882606884,
-                    "angularVelocity": 0.000001212667636144746,
-                    "velocityX": 3.3648781557631158,
-                    "velocityY": 1.728649251146572,
-                    "timestamp": 2.148420784101885
-                },
-                {
-                    "x": 7.018224220241307,
-                    "y": 2.1177284432893,
-                    "heading": 2.639872440534831,
-                    "angularVelocity": 0.5471585877616616,
-                    "velocityX": 3.1450005299073442,
-                    "velocityY": 1.6762444874498934,
-                    "timestamp": 2.182640402228582
-                },
-                {
-                    "x": 7.119181986421491,
-                    "y": 2.170426181119355,
-                    "heading": 2.6783971426308466,
-                    "angularVelocity": 1.1258074813511751,
-                    "velocityX": 2.9502890945887246,
-                    "velocityY": 1.5399861458109638,
-                    "timestamp": 2.2168600203552793
-                },
-                {
-                    "x": 7.213084203441603,
-                    "y": 2.2185446189369,
-                    "heading": 2.734533820088199,
-                    "angularVelocity": 1.6404822885371884,
-                    "velocityX": 2.7441047609719855,
-                    "velocityY": 1.4061652482324039,
-                    "timestamp": 2.2510796384819765
-                },
-                {
-                    "x": 7.29840370402033,
-                    "y": 2.262230718496702,
-                    "heading": 2.7957315713049304,
-                    "angularVelocity": 1.788382061721109,
-                    "velocityX": 2.493292013453601,
-                    "velocityY": 1.2766390144406516,
-                    "timestamp": 2.2852992566086736
-                },
-                {
-                    "x": 7.375044913403133,
-                    "y": 2.301443411516585,
-                    "heading": 2.8566177755066406,
-                    "angularVelocity": 1.7792777223954108,
-                    "velocityX": 2.239686284605582,
-                    "velocityY": 1.1459126421194965,
-                    "timestamp": 2.3195188747353708
-                },
-                {
-                    "x": 7.443040169771806,
-                    "y": 2.336203371638747,
-                    "heading": 2.9144119075075787,
-                    "angularVelocity": 1.6889180874829506,
-                    "velocityX": 1.9870255745380796,
-                    "velocityY": 1.0157904157043582,
-                    "timestamp": 2.353738492862068
-                },
-                {
-                    "x": 7.502434435273893,
-                    "y": 2.3665382868697598,
-                    "heading": 2.967461608910358,
-                    "angularVelocity": 1.5502715783199053,
-                    "velocityX": 1.7356787934389564,
-                    "velocityY": 0.8864773159857819,
-                    "timestamp": 2.387958110988765
-                },
-                {
-                    "x": 7.553267406161016,
-                    "y": 2.3924752684695187,
-                    "heading": 3.014662356421872,
-                    "angularVelocity": 1.379347581751362,
-                    "velocityX": 1.48549205601638,
-                    "velocityY": 0.7579564886939567,
-                    "timestamp": 2.4221777291154623
-                },
-                {
-                    "x": 7.595572130014844,
-                    "y": 2.41403971266414,
-                    "heading": 3.055206797581657,
-                    "angularVelocity": 1.184830321883501,
-                    "velocityX": 1.2362710681690243,
-                    "velocityY": 0.6301778153917356,
-                    "timestamp": 2.4563973472421594
-                },
-                {
-                    "x": 7.629376218234675,
-                    "y": 2.4312548848965747,
-                    "heading": 3.088468435996466,
-                    "angularVelocity": 0.9720049560944556,
-                    "velocityX": 0.9878569683236498,
-                    "velocityY": 0.5030790281965085,
-                    "timestamp": 2.4906169653688566
-                },
-                {
-                    "x": 7.654702978974042,
-                    "y": 2.4441416202766635,
-                    "heading": 3.113945152945951,
-                    "angularVelocity": 0.7445061734809163,
-                    "velocityX": 0.7401240027166469,
-                    "velocityY": 0.3765891054767559,
-                    "timestamp": 2.5248365834955537
-                },
-                {
-                    "x": 7.671572100944035,
-                    "y": 2.4527181446432627,
-                    "heading": 3.1312307619473794,
-                    "angularVelocity": 0.5051374021015854,
-                    "velocityX": 0.4929664003711423,
-                    "velocityY": 0.250631796498847,
-                    "timestamp": 2.559056201622251
+                    "heading": -0.5067167933216484,
+                    "angularVelocity": 0.000001004680076164895,
+                    "velocityX": 3.7780959145145716,
+                    "velocityY": -0.19138230096471237,
+                    "timestamp": 1.676085316688087
+                },
+                {
+                    "x": 5.366036550697627,
+                    "y": 1.4270871399969536,
+                    "heading": -0.5067167282581706,
+                    "angularVelocity": 0.000001901925327561791,
+                    "velocityX": 3.781775437099809,
+                    "velocityY": 0.09383479793163368,
+                    "timestamp": 1.7102945871261934
+                },
+                {
+                    "x": 5.494798697340742,
+                    "y": 1.4400359689986706,
+                    "heading": -0.5067166730622293,
+                    "angularVelocity": 0.0000016134790500794753,
+                    "velocityX": 3.76395476998197,
+                    "velocityY": 0.37851812786083194,
+                    "timestamp": 1.7445038575643
+                },
+                {
+                    "x": 5.6222191444596294,
+                    "y": 1.4626499710733982,
+                    "heading": -0.5067166242756445,
+                    "angularVelocity": 0.0000014261217576278581,
+                    "velocityX": 3.724734421022641,
+                    "velocityY": 0.6610489433161765,
+                    "timestamp": 1.7787131280024064
+                },
+                {
+                    "x": 5.747573461036382,
+                    "y": 1.4948005438980272,
+                    "heading": -0.5067165796372217,
+                    "angularVelocity": 0.0000013048633380814483,
+                    "velocityX": 3.6643376187618593,
+                    "velocityY": 0.9398204759378852,
+                    "timestamp": 1.812922398440513
+                },
+                {
+                    "x": 5.870148986660447,
+                    "y": 1.5363048197489622,
+                    "heading": -0.5067165375293654,
+                    "angularVelocity": 0.0000012308902156872765,
+                    "velocityX": 3.583108439738183,
+                    "velocityY": 1.2132464480944425,
+                    "timestamp": 1.8471316688786195
+                },
+                {
+                    "x": 5.9892489774115445,
+                    "y": 1.5869265226922225,
+                    "heading": -0.5067164966802475,
+                    "angularVelocity": 0.0000011940949728764683,
+                    "velocityX": 3.481512152285735,
+                    "velocityY": 1.4797656394002383,
+                    "timestamp": 1.881340939316726
+                },
+                {
+                    "x": 6.104355729464833,
+                    "y": 1.6460687267313168,
+                    "heading": -0.5067164560018657,
+                    "angularVelocity": 0.0000011891040447074693,
+                    "velocityX": 3.3647824282469623,
+                    "velocityY": 1.7288355840881788,
+                    "timestamp": 1.9155502097548325
+                },
+                {
+                    "x": 6.219456425834801,
+                    "y": 1.705222715286976,
+                    "heading": -0.5067164153233215,
+                    "angularVelocity": 0.0000011891087921450935,
+                    "velocityX": 3.3646054094668667,
+                    "velocityY": 1.7291800672184492,
+                    "timestamp": 1.949759480192939
+                },
+                {
+                    "x": 6.334557121741728,
+                    "y": 1.764376704743614,
+                    "heading": -0.5067163746447773,
+                    "angularVelocity": 0.0000011891087898511233,
+                    "velocityX": 3.3646053959312843,
+                    "velocityY": 1.7291800935557256,
+                    "timestamp": 1.9839687506310455
+                },
+                {
+                    "x": 6.449657817648618,
+                    "y": 1.8235306942003213,
+                    "heading": -0.5067163339662332,
+                    "angularVelocity": 0.000001189108787218646,
+                    "velocityX": 3.3646053959302495,
+                    "velocityY": 1.7291800935577397,
+                    "timestamp": 2.018178021069152
+                },
+                {
+                    "x": 6.564758513555509,
+                    "y": 1.8826846836570283,
+                    "heading": -0.5067162932876892,
+                    "angularVelocity": 0.0000011891087819705439,
+                    "velocityX": 3.3646053959302504,
+                    "velocityY": 1.7291800935577386,
+                    "timestamp": 2.0523872915072587
+                },
+                {
+                    "x": 6.679859209462396,
+                    "y": 1.9418386731137416,
+                    "heading": -0.5067162526091454,
+                    "angularVelocity": 0.0000011891087784797324,
+                    "velocityX": 3.3646053959301545,
+                    "velocityY": 1.7291800935579253,
+                    "timestamp": 2.0865965619453655
+                },
+                {
+                    "x": 6.7949599053264,
+                    "y": 2.0009926626538963,
+                    "heading": -0.5067162119306018,
+                    "angularVelocity": 0.0000011891087739781964,
+                    "velocityX": 3.3646053946765995,
+                    "velocityY": 1.7291800959970696,
+                    "timestamp": 2.120805832383472
+                },
+                {
+                    "x": 6.910060040297267,
+                    "y": 2.06014774355027,
+                    "heading": -0.5067161712446878,
+                    "angularVelocity": 0.0000011893242211155864,
+                    "velocityX": 3.3645889987368625,
+                    "velocityY": 1.7292119983499907,
+                    "timestamp": 2.155015102821579
+                },
+                {
+                    "x": 7.017868923227757,
+                    "y": 2.1174564154331827,
+                    "heading": -0.4884934235175169,
+                    "angularVelocity": 0.5326844885552473,
+                    "velocityX": 3.151452268634164,
+                    "velocityY": 1.6752380611740627,
+                    "timestamp": 2.1892243732596857
+                },
+                {
+                    "x": 7.119036711180674,
+                    "y": 2.1700871649460596,
+                    "heading": -0.45052101262699384,
+                    "angularVelocity": 1.1100035284068692,
+                    "velocityX": 2.9573208272872957,
+                    "velocityY": 1.5384937719762113,
+                    "timestamp": 2.2234336436977924
+                },
+                {
+                    "x": 7.212945348428852,
+                    "y": 2.2182419084801723,
+                    "heading": -0.3958429177265348,
+                    "angularVelocity": 1.5983414495607555,
+                    "velocityX": 2.745122478366898,
+                    "velocityY": 1.4076518708938115,
+                    "timestamp": 2.257642914135899
+                },
+                {
+                    "x": 7.29827375889861,
+                    "y": 2.2619654936466005,
+                    "heading": -0.3361817714679226,
+                    "angularVelocity": 1.7440052212324946,
+                    "velocityX": 2.494306641941948,
+                    "velocityY": 1.2781209481077944,
+                    "timestamp": 2.291852184574006
+                },
+                {
+                    "x": 7.374929402835313,
+                    "y": 2.3012175879094494,
+                    "heading": -0.2767819040095215,
+                    "angularVelocity": 1.7363675605380382,
+                    "velocityX": 2.2407856979994336,
+                    "velocityY": 1.1474110309913224,
+                    "timestamp": 2.3260614550121126
+                },
+                {
+                    "x": 7.442942536538503,
+                    "y": 2.336017519822898,
+                    "heading": -0.22036844778019443,
+                    "angularVelocity": 1.6490692583284896,
+                    "velocityX": 1.9881492014348112,
+                    "velocityY": 1.0172661231232827,
+                    "timestamp": 2.3602707254502193
+                },
+                {
+                    "x": 7.502356073864926,
+                    "y": 2.3663915967666074,
+                    "heading": -0.16856655782281169,
+                    "angularVelocity": 1.5142646801283255,
+                    "velocityX": 1.7367671559648723,
+                    "velocityY": 0.8878902284298598,
+                    "timestamp": 2.394479995888326
+                },
+                {
+                    "x": 7.553208169092694,
+                    "y": 2.392365594084153,
+                    "heading": -0.12246425110299042,
+                    "angularVelocity": 1.3476553615264124,
+                    "velocityX": 1.4865004303372251,
+                    "velocityY": 0.7592677945160818,
+                    "timestamp": 2.428689266326433
+                },
+                {
+                    "x": 7.595530700913324,
+                    "y": 2.4139635888667987,
+                    "heading": -0.08285639859822795,
+                    "angularVelocity": 1.1578104998299616,
+                    "velocityX": 1.2371655775940182,
+                    "velocityY": 0.6313491783381474,
+                    "timestamp": 2.4628985367645395
+                },
+                {
+                    "x": 7.629350339345417,
+                    "y": 2.431207580669044,
+                    "heading": -0.050358801794747596,
+                    "angularVelocity": 0.9499646261757312,
+                    "velocityX": 0.988610338629752,
+                    "velocityY": 0.5040736496688569,
+                    "timestamp": 2.4971078072026462
+                },
+                {
+                    "x": 7.654689602970606,
+                    "y": 2.444117248020803,
+                    "heading": -0.025464280349729525,
+                    "angularVelocity": 0.7277127260009457,
+                    "velocityX": 0.7407133592934628,
+                    "velocityY": 0.3773733606834889,
+                    "timestamp": 2.531317077640753
+                },
+                {
+                    "x": 7.671567523131761,
+                    "y": 2.4527098140869614,
+                    "heading": -0.008571412442380029,
+                    "angularVelocity": 0.49380965133159116,
+                    "velocityX": 0.4933727011714593,
+                    "velocityY": 0.2511765365386793,
+                    "timestamp": 2.5655263480788597
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": 0.2562634691057309,
-                    "velocityX": 0.24628851861412304,
-                    "velocityY": 0.1251286715381821,
-                    "timestamp": 2.593275819748948
+                    "heading": 5.0379367595035626e-36,
+                    "angularVelocity": 0.2505581771434718,
+                    "velocityX": 0.24649683434481964,
+                    "velocityY": 0.12541003821758284,
+                    "timestamp": 2.5997356185169664
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": 3.214441635047838e-33,
-                    "velocityX": 1.400444892944232e-33,
-                    "velocityY": -3.598995466574735e-33,
-                    "timestamp": 2.627495437875645
+                    "heading": 2.854280403338002e-36,
+                    "angularVelocity": -6.383230494440131e-35,
+                    "velocityX": -1.2082929677962258e-35,
+                    "velocityY": 1.8766665143888344e-34,
+                    "timestamp": 2.633944888955073
                 }
             ],
             "constraints": [
@@ -8659,7 +8926,7 @@
                 {
                     "x": 0.71,
                     "y": 4.36,
-                    "heading": 2.1,
+                    "heading": -1.04,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -8668,7 +8935,7 @@
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -8679,290 +8946,290 @@
                 {
                     "x": 0.71,
                     "y": 4.36,
-                    "heading": 2.1,
+                    "heading": -1.04,
                     "angularVelocity": 0,
-                    "velocityX": 8.526478185532948e-32,
-                    "velocityY": -7.785685234118549e-31,
+                    "velocityX": -3.7161803829578067e-31,
+                    "velocityY": -1.7109886686245282e-31,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.755735101962148,
-                    "y": 4.33059726025757,
-                    "heading": 2.1511666979053023,
-                    "angularVelocity": 0.6232717514717016,
-                    "velocityX": 0.5571083980530026,
-                    "velocityY": -0.35816063665566134,
-                    "timestamp": 0.08209372201529226
-                },
-                {
-                    "x": 0.8475193720501324,
-                    "y": 4.271815657764732,
-                    "heading": 2.250112545454498,
-                    "angularVelocity": 1.2052791019849762,
-                    "velocityX": 1.1180424986808006,
-                    "velocityY": -0.7160304229096619,
-                    "timestamp": 0.16418744403058452
-                },
-                {
-                    "x": 0.9858871774715989,
-                    "y": 4.183669855686051,
-                    "heading": 2.389658659666248,
-                    "angularVelocity": 1.6998390472021188,
-                    "velocityX": 1.6854858328349602,
-                    "velocityY": -1.0737215942317955,
-                    "timestamp": 0.2462811660458768
-                },
-                {
-                    "x": 1.17173213163872,
-                    "y": 4.066157035949835,
-                    "heading": 2.5522635706365153,
-                    "angularVelocity": 1.9807228491842224,
-                    "velocityX": 2.2638144501780824,
-                    "velocityY": -1.4314470930472174,
-                    "timestamp": 0.32837488806116905
-                },
-                {
-                    "x": 1.4047988716632624,
-                    "y": 3.9214804758601747,
-                    "heading": 2.664319566325619,
-                    "angularVelocity": 1.3649764310629042,
-                    "velocityX": 2.839032441252049,
-                    "velocityY": -1.76233403161705,
-                    "timestamp": 0.4104686100764613
-                },
-                {
-                    "x": 1.6657539489096052,
-                    "y": 3.7531140130982656,
-                    "heading": 2.6643196178388515,
-                    "angularVelocity": 6.274929563351661e-7,
-                    "velocityX": 3.178745838783293,
-                    "velocityY": -2.050905460597173,
-                    "timestamp": 0.49256233209175354
-                },
-                {
-                    "x": 1.9267089834441824,
-                    "y": 3.584747484136064,
-                    "heading": 2.664319669351516,
-                    "angularVelocity": 6.274860319306578e-7,
-                    "velocityX": 3.1787453185027585,
-                    "velocityY": -2.05090626699614,
-                    "timestamp": 0.5746560541070458
-                },
-                {
-                    "x": 2.1876640582968827,
-                    "y": 3.416381017663811,
-                    "heading": 2.6643197208641496,
-                    "angularVelocity": 6.274856686006067e-7,
-                    "velocityX": 3.1787458096258594,
-                    "velocityY": -2.0509055057936005,
-                    "timestamp": 0.6567497761223381
-                },
-                {
-                    "x": 2.4486192865071836,
-                    "y": 3.248014788884269,
-                    "heading": 2.6643197723767935,
-                    "angularVelocity": 6.274857907572903e-7,
-                    "velocityX": 3.178747677705374,
-                    "velocityY": -2.050902610411298,
-                    "timestamp": 0.7388434981376304
-                },
-                {
-                    "x": 2.7195077051073344,
-                    "y": 3.0961455009278414,
-                    "heading": 2.664319841917229,
-                    "angularVelocity": 8.470859127420906e-7,
-                    "velocityX": 3.2997458508421667,
-                    "velocityY": -1.849950084223712,
-                    "timestamp": 0.8209372201529227
-                },
-                {
-                    "x": 2.9903964357661867,
-                    "y": 2.9442767695902643,
-                    "heading": 2.664319911457928,
-                    "angularVelocity": 8.470891223849179e-7,
-                    "velocityX": 3.2997496520914464,
-                    "velocityY": -1.8499433039384836,
-                    "timestamp": 0.903030942168215
-                },
-                {
-                    "x": 3.2612853045995296,
-                    "y": 2.79240828471517,
-                    "heading": 2.6643199809985263,
-                    "angularVelocity": 8.470878995035866e-7,
-                    "velocityX": 3.299751335222467,
-                    "velocityY": -1.8499403017298282,
-                    "timestamp": 0.9851246641835073
-                },
-                {
-                    "x": 3.5321743153682585,
-                    "y": 2.6405400530113767,
-                    "heading": 2.664320050538881,
-                    "angularVelocity": 8.47084928778718e-7,
-                    "velocityX": 3.299753064165714,
-                    "velocityY": -1.8499372177997218,
-                    "timestamp": 1.0672183861987996
-                },
-                {
-                    "x": 3.8030633934021476,
-                    "y": 2.4886719412892284,
-                    "heading": 2.6643201200792013,
-                    "angularVelocity": 8.470845077302885e-7,
-                    "velocityX": 3.299753883536049,
-                    "velocityY": -1.8499357562793664,
-                    "timestamp": 1.1493121082140918
-                },
-                {
-                    "x": 4.073952519237506,
-                    "y": 2.3368039148311976,
-                    "heading": 2.664320189619518,
-                    "angularVelocity": 8.470844698893479e-7,
-                    "velocityX": 3.2997544658152695,
-                    "velocityY": -1.8499347176601568,
-                    "timestamp": 1.231405830229384
-                },
-                {
-                    "x": 4.3448416924892905,
-                    "y": 2.1849359729505413,
-                    "heading": 2.6643202591598345,
-                    "angularVelocity": 8.470844641828709e-7,
-                    "velocityX": 3.2997550434042244,
-                    "velocityY": -1.8499336874062895,
-                    "timestamp": 1.3134995522446762
-                },
-                {
-                    "x": 4.615730948551723,
-                    "y": 2.0330681787806033,
-                    "heading": 2.664320328700174,
-                    "angularVelocity": 8.470847405524923e-7,
-                    "velocityX": 3.29975605213723,
-                    "velocityY": -1.8499318881125704,
-                    "timestamp": 1.3955932742599684
-                },
-                {
-                    "x": 4.889033051365783,
-                    "y": 1.8855867361226037,
-                    "heading": 2.6643204031763625,
-                    "angularVelocity": 9.072093065949494e-7,
-                    "velocityX": 3.3291474196181556,
-                    "velocityY": -1.7965008655659056,
-                    "timestamp": 1.4776869962752606
-                },
-                {
-                    "x": 5.16486669963939,
-                    "y": 1.742896114789829,
-                    "heading": 2.6643204810266874,
-                    "angularVelocity": 9.483103351737687e-7,
-                    "velocityX": 3.359984679732577,
-                    "velocityY": -1.738142915559301,
-                    "timestamp": 1.5597807182905528
-                },
-                {
-                    "x": 5.440736180585003,
-                    "y": 1.6002747825074959,
-                    "heading": 2.6643205588558216,
-                    "angularVelocity": 9.480522059758943e-7,
-                    "velocityX": 3.3604211646564863,
-                    "velocityY": -1.7372988918174013,
-                    "timestamp": 1.641874440305845
-                },
-                {
-                    "x": 5.732995703254166,
-                    "y": 1.4952548268359402,
-                    "heading": 2.664320597611399,
-                    "angularVelocity": 4.720894158515159e-7,
-                    "velocityX": 3.5600715315931244,
-                    "velocityY": -1.2792690243961995,
-                    "timestamp": 1.7239681623211371
-                },
-                {
-                    "x": 6.025255587661767,
-                    "y": 1.3902358778499353,
-                    "heading": 2.6643206363668037,
-                    "angularVelocity": 4.720873108891009e-7,
-                    "velocityX": 3.560075938001194,
-                    "velocityY": -1.2792567617587365,
-                    "timestamp": 1.8060618843364293
-                },
-                {
-                    "x": 6.317515503776411,
-                    "y": 1.2852170171023363,
-                    "heading": 2.664320675122227,
-                    "angularVelocity": 4.7208754093652347e-7,
-                    "velocityX": 3.5600763242310083,
-                    "velocityY": -1.2792556869091232,
-                    "timestamp": 1.8881556063517215
-                },
-                {
-                    "x": 6.60977544021274,
-                    "y": 1.1801982129085715,
-                    "heading": 2.6643207138776748,
-                    "angularVelocity": 4.720878420422611e-7,
-                    "velocityX": 3.5600765717735112,
-                    "velocityY": -1.2792549980155836,
-                    "timestamp": 1.9702493283670137
-                },
-                {
-                    "x": 6.902035364394445,
-                    "y": 1.0751793746113998,
-                    "heading": 2.664320752633444,
-                    "angularVelocity": 4.720917543940102e-7,
-                    "velocityX": 3.5600764224974903,
-                    "velocityY": -1.2792554134359897,
-                    "timestamp": 2.052343050382306
-                },
-                {
-                    "x": 7.1614900474496475,
-                    "y": 0.9773985940984262,
-                    "heading": 2.7732250681220454,
-                    "angularVelocity": 1.3265851859940632,
-                    "velocityX": 3.1604692379140946,
-                    "velocityY": -1.1910871880648686,
-                    "timestamp": 2.1344367723975983
-                },
-                {
-                    "x": 7.369273961287434,
-                    "y": 0.9006112993544645,
-                    "heading": 2.9067869208138672,
-                    "angularVelocity": 1.626943564173427,
-                    "velocityX": 2.5310572932614863,
-                    "velocityY": -0.935361351135448,
-                    "timestamp": 2.2165304944128907
-                },
-                {
-                    "x": 7.524785907770447,
-                    "y": 0.8435839044250819,
-                    "heading": 3.0191766997290386,
-                    "angularVelocity": 1.3690423111067538,
-                    "velocityX": 1.8943220341994518,
-                    "velocityY": -0.6946620707337358,
-                    "timestamp": 2.298624216428183
-                },
-                {
-                    "x": 7.62830248590124,
-                    "y": 0.8058044628378582,
-                    "heading": 3.098670404043058,
-                    "angularVelocity": 0.9683286658535433,
-                    "velocityX": 1.2609560827502735,
-                    "velocityY": -0.460198912411186,
-                    "timestamp": 2.3807179384434756
+                    "x": 0.755738680236623,
+                    "y": 4.330590345332975,
+                    "heading": -0.988927755482241,
+                    "angularVelocity": 0.6221231913092291,
+                    "velocityX": 0.5571537727343439,
+                    "velocityY": -0.35824601776392057,
+                    "timestamp": 0.08209345870916634
+                },
+                {
+                    "x": 0.8475293838257123,
+                    "y": 4.27179448486036,
+                    "heading": -0.8901763080939228,
+                    "angularVelocity": 1.2029149330663034,
+                    "velocityX": 1.1181244526957685,
+                    "velocityY": -0.7162063993540811,
+                    "timestamp": 0.1641869174183327
+                },
+                {
+                    "x": 0.9859043696516769,
+                    "y": 4.183626202129818,
+                    "heading": -0.7509222083756354,
+                    "angularVelocity": 1.6962874010659592,
+                    "velocityX": 1.6855787050730526,
+                    "velocityY": -1.0739988802627505,
+                    "timestamp": 0.24628037612749903
+                },
+                {
+                    "x": 1.1717528122715652,
+                    "y": 4.066081969426692,
+                    "heading": -0.5886701024002182,
+                    "angularVelocity": 1.9764316003572233,
+                    "velocityX": 2.263864204799754,
+                    "velocityY": -1.4318343331050452,
+                    "timestamp": 0.3283738348366654
+                },
+                {
+                    "x": 1.404824377392605,
+                    "y": 3.921350877318508,
+                    "heading": -0.4767670307510258,
+                    "angularVelocity": 1.3631180048782332,
+                    "velocityX": 2.8391003228009395,
+                    "velocityY": -1.7630039516415732,
+                    "timestamp": 0.4104672935458317
+                },
+                {
+                    "x": 1.6657642530728491,
+                    "y": 3.752962692607369,
+                    "heading": -0.4767669793872945,
+                    "angularVelocity": 6.256738607298855e-7,
+                    "velocityX": 3.1785708603736573,
+                    "velocityY": -2.051176638904815,
+                    "timestamp": 0.49256075225499807
+                },
+                {
+                    "x": 1.926704086710966,
+                    "y": 3.584574442746028,
+                    "heading": -0.476766928024127,
+                    "angularVelocity": 6.256669936190949e-7,
+                    "velocityX": 3.178570348248477,
+                    "velocityY": -2.051177432514957,
+                    "timestamp": 0.5746542109641644
+                },
+                {
+                    "x": 2.187643960670788,
+                    "y": 3.416186255368488,
+                    "heading": -0.47676687666098916,
+                    "angularVelocity": 6.256666327585045e-7,
+                    "velocityX": 3.178570839416784,
+                    "velocityY": -2.0511766713848885,
+                    "timestamp": 0.6567476696733308
+                },
+                {
+                    "x": 2.448583988107283,
+                    "y": 3.2477983058236712,
+                    "heading": -0.4767668252978414,
+                    "angularVelocity": 6.256667527167073e-7,
+                    "velocityX": 3.1785727089527405,
+                    "velocityY": -2.051173774287752,
+                    "timestamp": 0.7388411283824972
+                },
+                {
+                    "x": 2.7194719503347105,
+                    "y": 3.095930240616485,
+                    "heading": -0.47676675593417933,
+                    "angularVelocity": 8.449353114908744e-7,
+                    "velocityX": 3.2997508752445928,
+                    "velocityY": -1.8499411231437015,
+                    "timestamp": 0.8209345870916636
+                },
+                {
+                    "x": 2.9903602222167747,
+                    "y": 2.9440627277435274,
+                    "heading": -0.4767666865702561,
+                    "angularVelocity": 8.449384925343815e-7,
+                    "velocityX": 3.2997546472215804,
+                    "velocityY": -1.8499343950287315,
+                    "timestamp": 0.90302804580083
+                },
+                {
+                    "x": 3.2612486310597473,
+                    "y": 2.7921954591699243,
+                    "heading": -0.4767666172064318,
+                    "angularVelocity": 8.44937286881618e-7,
+                    "velocityX": 3.299756315575062,
+                    "velocityY": -1.8499314191600278,
+                    "timestamp": 0.9851215045099964
+                },
+                {
+                    "x": 3.5321371803805834,
+                    "y": 2.6403284411694066,
+                    "heading": -0.4767665478428475,
+                    "angularVelocity": 8.449343642301634e-7,
+                    "velocityX": 3.2997580267694278,
+                    "velocityY": -1.84992836686951,
+                    "timestamp": 1.0672149632191628
+                },
+                {
+                    "x": 3.8030257968728702,
+                    "y": 2.488461542984137,
+                    "heading": -0.47676647847929754,
+                    "angularVelocity": 8.449339449505073e-7,
+                    "velocityX": 3.2997588450008877,
+                    "velocityY": -1.849926907371401,
+                    "timestamp": 1.1493084219283292
+                },
+                {
+                    "x": 4.07391446115854,
+                    "y": 2.336594730049097,
+                    "heading": -0.4767664091157508,
+                    "angularVelocity": 8.449339066268147e-7,
+                    "velocityX": 3.2997594271834845,
+                    "velocityY": -1.8499258689180111,
+                    "timestamp": 1.2314018806374956
+                },
+                {
+                    "x": 4.344803172855204,
+                    "y": 2.184728001682272,
+                    "heading": -0.4767663397522045,
+                    "angularVelocity": 8.449339005791607e-7,
+                    "velocityX": 3.2997600047081037,
+                    "velocityY": -1.8499248387724243,
+                    "timestamp": 1.313495339346662
+                },
+                {
+                    "x": 4.6156919675948584,
+                    "y": 2.0328614214415324,
+                    "heading": -0.47676627038863556,
+                    "angularVelocity": 8.449341769958558e-7,
+                    "velocityX": 3.2997610162745823,
+                    "velocityY": -1.8499230344132471,
+                    "timestamp": 1.3955887980558284
+                },
+                {
+                    "x": 4.88903699417146,
+                    "y": 1.8854616483024196,
+                    "heading": -0.4767661960128138,
+                    "angularVelocity": 9.059896238747082e-7,
+                    "velocityX": 3.3296809620969254,
+                    "velocityY": -1.7955117917653842,
+                    "timestamp": 1.4776822567649948
+                },
+                {
+                    "x": 5.164941930672226,
+                    "y": 1.7429110878363532,
+                    "heading": -0.47676611821696785,
+                    "angularVelocity": 9.476497536578291e-7,
+                    "velocityX": 3.3608638354271223,
+                    "velocityY": -1.736442375647519,
+                    "timestamp": 1.5597757154741612
+                },
+                {
+                    "x": 5.4408826630316725,
+                    "y": 1.6004298311654348,
+                    "heading": -0.4767660404421832,
+                    "angularVelocity": 9.473932014254074e-7,
+                    "velocityX": 3.3612998733190906,
+                    "velocityY": -1.7355981695897218,
+                    "timestamp": 1.6418691741833276
+                },
+                {
+                    "x": 5.73313461063715,
+                    "y": 1.4953917422787115,
+                    "heading": -0.4767660015440003,
+                    "angularVelocity": 4.738280431565268e-7,
+                    "velocityX": 3.55999067649034,
+                    "velocityY": -1.2794940125356749,
+                    "timestamp": 1.723962632892494
+                },
+                {
+                    "x": 6.025386917456382,
+                    "y": 1.3903546528530497,
+                    "heading": -0.476765962645991,
+                    "angularVelocity": 4.738259283996548e-7,
+                    "velocityX": 3.5599950521587655,
+                    "velocityY": -1.2794818378621156,
+                    "timestamp": 1.8060560916016604
+                },
+                {
+                    "x": 6.31763925599377,
+                    "y": 1.2853176516791878,
+                    "heading": -0.4767659237479627,
+                    "angularVelocity": 4.7382616041897537e-7,
+                    "velocityX": 3.5599954385251906,
+                    "velocityY": -1.2794807628458942,
+                    "timestamp": 1.8881495503108268
+                },
+                {
+                    "x": 6.609891614856598,
+                    "y": 1.1802807070583716,
+                    "heading": -0.4767658848499098,
+                    "angularVelocity": 4.7382646005084667e-7,
+                    "velocityX": 3.5599956861142137,
+                    "velocityY": -1.279480073959752,
+                    "timestamp": 1.9702430090199932
+                },
+                {
+                    "x": 6.9021439611938415,
+                    "y": 1.0752437275870097,
+                    "heading": -0.4767658459515333,
+                    "angularVelocity": 4.738304011500936e-7,
+                    "velocityX": 3.559995533537077,
+                    "velocityY": -1.2794804984825638,
+                    "timestamp": 2.0523364677291593
+                },
+                {
+                    "x": 7.1615588383732955,
+                    "y": 0.9774297585720934,
+                    "heading": -0.3677133060860067,
+                    "angularVelocity": 1.3283949973635365,
+                    "velocityX": 3.159994489920166,
+                    "velocityY": -1.1914952854093204,
+                    "timestamp": 2.1344299264383255
+                },
+                {
+                    "x": 7.369316955835238,
+                    "y": 0.9006256752646921,
+                    "heading": -0.23381405774557934,
+                    "angularVelocity": 1.6310586792888595,
+                    "velocityX": 2.5307511795546396,
+                    "velocityY": -0.9355688567038716,
+                    "timestamp": 2.2165233851474917
+                },
+                {
+                    "x": 7.5248080438305305,
+                    "y": 0.8435895899389048,
+                    "heading": -0.12113401657570753,
+                    "angularVelocity": 1.3725824559185034,
+                    "velocityX": 1.894074027824225,
+                    "velocityY": -0.6947701585804769,
+                    "timestamp": 2.298616843856658
+                },
+                {
+                    "x": 7.628310039444481,
+                    "y": 0.8058059657857352,
+                    "heading": -0.04143578092949631,
+                    "angularVelocity": 0.9708232166043739,
+                    "velocityX": 1.2607824940185441,
+                    "velocityY": -0.4602513372840855,
+                    "timestamp": 2.380710302565824
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
-                    "angularVelocity": 0.5034440508038269,
-                    "velocityX": 0.6297377293860565,
-                    "velocityY": -0.22906091204337628,
-                    "timestamp": 2.462811660458768
+                    "heading": 6.308726444888931e-32,
+                    "angularVelocity": 0.5047391300236409,
+                    "velocityX": 0.6296477376917574,
+                    "velocityY": -0.22907995449868002,
+                    "timestamp": 2.4628037612749902
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
-                    "angularVelocity": 1.633672716236069e-30,
-                    "velocityX": -5.394468608823283e-31,
-                    "velocityY": 5.5679704387753575e-31,
-                    "timestamp": 2.5449053824740604
+                    "heading": 0,
+                    "angularVelocity": -2.9272372134325455e-30,
+                    "velocityX": -7.795387702917406e-31,
+                    "velocityY": 4.863651986550052e-31,
+                    "timestamp": 2.5448972199841564
                 }
             ],
             "constraints": [
@@ -9005,7 +9272,7 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -9014,7 +9281,7 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": -3.143185307179586,
+                    "heading": -0.003,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -9025,101 +9292,101 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
+                    "heading": 0.39,
+                    "angularVelocity": 1.792103156985809e-33,
+                    "velocityX": -7.27073415334495e-34,
+                    "velocityY": -7.853383039035825e-35,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.2980013304879616,
-                    "y": 6.709118488011682,
-                    "heading": -2.76564785007466,
-                    "angularVelocity": -0.20597421909793953,
-                    "velocityX": -0.026308687136743093,
-                    "velocityY": -0.6302691420205844,
-                    "timestamp": 0.07596994489499284
+                    "x": 2.298001341213542,
+                    "y": 6.709118496985434,
+                    "heading": 0.374359095500822,
+                    "angularVelocity": -0.2058832051525312,
+                    "velocityX": -0.02630859852023658,
+                    "velocityY": -0.6302702831973344,
+                    "timestamp": 0.07596979310473732
                 },
                 {
-                    "x": 2.294004010549845,
-                    "y": 6.6133557271698225,
-                    "heading": -2.796958855018623,
-                    "angularVelocity": -0.41214989674195696,
-                    "velocityX": -0.05261712304308655,
-                    "velocityY": -1.2605348203717124,
-                    "timestamp": 0.15193988978998568
+                    "x": 2.294004039374054,
+                    "y": 6.613355753676287,
+                    "heading": 0.3430620070158405,
+                    "angularVelocity": -0.4119675361210102,
+                    "velocityX": -0.0526169899393711,
+                    "velocityY": -1.2605371081782701,
+                    "timestamp": 0.15193958620947465
                 },
                 {
-                    "x": 2.2880079544710785,
-                    "y": 6.469712199120552,
-                    "heading": -2.8439591282551087,
-                    "angularVelocity": -0.6186693079934498,
-                    "velocityX": -0.07892668721893853,
-                    "velocityY": -1.8907941587665607,
-                    "timestamp": 0.22790983468497852
+                    "x": 2.2880080040813597,
+                    "y": 6.469712250400375,
+                    "heading": 0.296082691458658,
+                    "angularVelocity": -0.6183946755312791,
+                    "velocityX": -0.07892657130746197,
+                    "velocityY": -1.8907976105433808,
+                    "timestamp": 0.22790937931421196
                 },
                 {
-                    "x": 2.2800128266474267,
-                    "y": 6.278188582998367,
-                    "heading": -2.9066839984131234,
-                    "angularVelocity": -0.8256537535299542,
-                    "velocityX": -0.10524066898696272,
-                    "velocityY": -2.5210445576459732,
-                    "timestamp": 0.30387977957997137
+                    "x": 2.280012894319733,
+                    "y": 6.278188663497132,
+                    "heading": 0.23338596481601495,
+                    "angularVelocity": -0.8252849465603376,
+                    "velocityX": -0.10524064150870725,
+                    "velocityY": -2.5210492101669324,
+                    "timestamp": 0.3038791724189493
                 },
                 {
-                    "x": 2.2700178539327394,
-                    "y": 6.038785706839934,
-                    "heading": -2.985174219418862,
-                    "angularVelocity": -1.0331746470822045,
-                    "velocityX": -0.13156482775526857,
-                    "velocityY": -3.1512840569956957,
-                    "timestamp": 0.37984972447496423
+                    "x": 2.270017931664709,
+                    "y": 6.038785816229407,
+                    "heading": 0.1549313176962556,
+                    "angularVelocity": -1.0327084478378958,
+                    "velocityX": -0.13156495820969516,
+                    "velocityY": -3.1512899730774246,
+                    "timestamp": 0.37984896552368663
                 },
                 {
-                    "x": 2.2620112641506833,
-                    "y": 5.847271276792759,
-                    "heading": -3.048371938278793,
-                    "angularVelocity": -0.8318779083923777,
-                    "velocityX": -0.10539154389440776,
-                    "velocityY": -2.520923640419774,
-                    "timestamp": 0.4558196693699571
+                    "x": 2.2620113109689415,
+                    "y": 5.847271344563018,
+                    "heading": 0.09176539516362914,
+                    "angularVelocity": -0.8314610314331311,
+                    "velocityX": -0.10539216139142096,
+                    "velocityY": -2.5209292251507525,
+                    "timestamp": 0.45581875862842397
                 },
                 {
-                    "x": 2.256005772930322,
-                    "y": 5.703635612537123,
-                    "heading": -3.0957774424626403,
-                    "angularVelocity": -0.624003403574559,
-                    "velocityX": -0.0790508829335374,
-                    "velocityY": -1.8906906468626763,
-                    "timestamp": 0.53178961426495
+                    "x": 2.2560057963370115,
+                    "y": 5.703635646814395,
+                    "heading": 0.0443838554377995,
+                    "angularVelocity": -0.6236892031613949,
+                    "velocityX": -0.079051349049357,
+                    "velocityY": -1.8906948653999391,
+                    "timestamp": 0.5317885517331613
                 },
                 {
-                    "x": 2.25200192771716,
-                    "y": 5.607878538669833,
-                    "heading": -3.127382702013982,
-                    "angularVelocity": -0.4160231996354262,
-                    "velocityX": -0.05270301589261462,
-                    "velocityY": -1.260459962155393,
-                    "timestamp": 0.6077595591599428
+                    "x": 2.252001935551058,
+                    "y": 5.607878550007709,
+                    "heading": 0.012794605457377246,
+                    "angularVelocity": -0.4158132948561684,
+                    "velocityX": -0.05270332618167983,
+                    "velocityY": -1.2604627825518706,
+                    "timestamp": 0.6077583448378986
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -0.20801127587293636,
-                    "velocityX": -0.026351575217375103,
-                    "velocityY": -0.6302300039313167,
-                    "timestamp": 0.6837295040549357
+                    "heading": -0.003,
+                    "angularVelocity": -0.20790639031492017,
+                    "velocityX": -0.026351730987313093,
+                    "velocityY": -0.6302314123944018,
+                    "timestamp": 0.6837281379426359
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": 2.276477315177118e-32,
-                    "velocityX": 0,
-                    "velocityY": -9.616776104992252e-34,
-                    "timestamp": 0.7596994489499286
+                    "heading": -0.003,
+                    "angularVelocity": -6.008173298568887e-34,
+                    "velocityX": -7.305982513747354e-33,
+                    "velocityY": -3.8813573012664696e-35,
+                    "timestamp": 0.7596979310473732
                 }
             ],
             "constraints": [
@@ -9146,7 +9413,7 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -9155,159 +9422,14 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": -3.5331853071795862,
+                    "heading": -0.393,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": -2.75,
-                    "angularVelocity": -3.577903969873187e-35,
-                    "velocityX": -4.511566930271669e-37,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.3001399949944257,
-                    "y": 6.713175615322878,
-                    "heading": -2.7771678602975163,
-                    "angularVelocity": -0.3719998319011172,
-                    "velocityX": 0.001916901582349506,
-                    "velocityY": -0.6000716859748281,
-                    "timestamp": 0.07303191552177325
-                },
-                {
-                    "x": 2.3004163850099326,
-                    "y": 6.6254379467440225,
-                    "heading": -2.829600510901429,
-                    "angularVelocity": -0.7179416044247195,
-                    "velocityX": 0.0037845100122603365,
-                    "velocityY": -1.2013606373599366,
-                    "timestamp": 0.1460638310435465
-                },
-                {
-                    "x": 2.3008288453246886,
-                    "y": 6.493659773055691,
-                    "heading": -2.9042529721112227,
-                    "angularVelocity": -1.022189554750722,
-                    "velocityX": 0.005647672141821651,
-                    "velocityY": -1.8043915834173219,
-                    "timestamp": 0.21909574656531977
-                },
-                {
-                    "x": 2.301391538953172,
-                    "y": 6.317653855065593,
-                    "heading": -2.995610403714753,
-                    "angularVelocity": -1.2509247628359643,
-                    "velocityX": 0.007704763382735362,
-                    "velocityY": -2.4099863290265735,
-                    "timestamp": 0.292127662087093
-                },
-                {
-                    "x": 2.3021600106867623,
-                    "y": 6.097194240566218,
-                    "heading": -3.0915567903699093,
-                    "angularVelocity": -1.3137596894408665,
-                    "velocityX": 0.010522409662952134,
-                    "velocityY": -3.0186749577128253,
-                    "timestamp": 0.36515957760886625
-                },
-                {
-                    "x": 2.303349915614448,
-                    "y": 5.834382556129677,
-                    "heading": -3.1370632737304405,
-                    "angularVelocity": -0.6231040639617954,
-                    "velocityX": 0.016292944244779627,
-                    "velocityY": -3.598586762498201,
-                    "timestamp": 0.4381914931306395
-                },
-                {
-                    "x": 2.3036783877036533,
-                    "y": 5.558107366997832,
-                    "heading": -3.137063302917444,
-                    "angularVelocity": -3.99647241237663e-7,
-                    "velocityX": 0.004497651292023743,
-                    "velocityY": -3.7829377356188716,
-                    "timestamp": 0.5112234086524128
-                },
-                {
-                    "x": 2.3040068828134284,
-                    "y": 5.281832177893351,
-                    "heading": -3.1370633321044417,
-                    "angularVelocity": -3.996471578074284e-7,
-                    "velocityX": 0.0044979665044766115,
-                    "velocityY": -3.782937735244173,
-                    "timestamp": 0.584255324174186
-                },
-                {
-                    "x": 2.302092596714716,
-                    "y": 5.019360768744023,
-                    "heading": -3.182327854705923,
-                    "angularVelocity": -0.6197909814920075,
-                    "velocityX": -0.026211637542794358,
-                    "velocityY": -3.5939274942210355,
-                    "timestamp": 0.6572872396959593
-                },
-                {
-                    "x": 2.3012286144088434,
-                    "y": 4.799012511457164,
-                    "heading": -3.280568867162049,
-                    "angularVelocity": -1.3451791830222102,
-                    "velocityX": -0.01183020190145165,
-                    "velocityY": -3.017150183075313,
-                    "timestamp": 0.7303191552177325
-                },
-                {
-                    "x": 2.3006858203530896,
-                    "y": 4.623118549812072,
-                    "heading": -3.3743826060457494,
-                    "angularVelocity": -1.284558103309387,
-                    "velocityX": -0.007432285623012945,
-                    "velocityY": -2.408453350681352,
-                    "timestamp": 0.8033510707395057
-                },
-                {
-                    "x": 2.3003278174317203,
-                    "y": 4.491440647891757,
-                    "heading": -3.4511285642395753,
-                    "angularVelocity": -1.0508550630983442,
-                    "velocityX": -0.00490200645582827,
-                    "velocityY": -1.8030185978218984,
-                    "timestamp": 0.876382986261279
-                },
-                {
-                    "x": 2.3001060473289097,
-                    "y": 4.403780156277999,
-                    "heading": -3.505127434733723,
-                    "angularVelocity": -0.7393872953811381,
-                    "velocityX": -0.0030366190072672575,
-                    "velocityY": -1.2003038806728776,
-                    "timestamp": 0.9494149017830522
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -3.5331853071795862,
-                    "angularVelocity": -0.38418645116186434,
-                    "velocityX": -0.001452068293050597,
-                    "velocityY": -0.5994660822629829,
-                    "timestamp": 1.0224468173048256
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -3.5331853071795862,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 8.704854669398703e-34,
-                    "timestamp": 1.095478732826599
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -9348,7 +9470,7 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -9357,7 +9479,7 @@
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": -2.88,
+                    "heading": 0.26,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -9368,128 +9490,128 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 0.39,
                     "angularVelocity": 0,
-                    "velocityX": -1.6054364331739777e-34,
-                    "velocityY": -2.5654655421376103e-34,
+                    "velocityX": 1.3865554557202343e-36,
+                    "velocityY": 1.1876687369143486e-32,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.373020964607517,
-                    "y": 6.753775569864816,
-                    "heading": -2.7564987511657213,
-                    "angularVelocity": -0.06940035185307017,
-                    "velocityX": 0.7797929951747273,
-                    "velocityY": -0.03443378276852441,
-                    "timestamp": 0.09364147287724231
-                },
-                {
-                    "x": 2.5190628897265572,
-                    "y": 6.7473267051761905,
-                    "heading": -2.769496449901771,
-                    "angularVelocity": -0.13880280111661264,
-                    "velocityX": 1.5595859466082067,
-                    "velocityY": -0.06886761271975776,
-                    "timestamp": 0.18728294575448462
-                },
-                {
-                    "x": 2.738125767578475,
-                    "y": 6.73765340144405,
-                    "heading": -2.7889934253789708,
-                    "angularVelocity": -0.20820876560484325,
-                    "velocityX": 2.339378814973306,
-                    "velocityY": -0.10330149062058414,
-                    "timestamp": 0.2809244186317269
-                },
-                {
-                    "x": 3.030209577950943,
-                    "y": 6.724755654214012,
-                    "heading": -2.814990111841214,
-                    "angularVelocity": -0.2776193673960349,
-                    "velocityX": 3.1191714674903457,
-                    "velocityY": -0.13773541608988796,
-                    "timestamp": 0.37456589150896924
-                },
-                {
-                    "x": 3.3841048307253856,
-                    "y": 6.709128194593673,
-                    "heading": -2.814990114833307,
-                    "angularVelocity": -3.1952637479967674e-8,
-                    "velocityX": 3.779257650489721,
-                    "velocityY": -0.16688609373799812,
-                    "timestamp": 0.4682073643862116
-                },
-                {
-                    "x": 3.7380000807913594,
-                    "y": 6.693500673639671,
-                    "heading": -2.814990117825208,
-                    "angularVelocity": -3.1950597051067127e-8,
-                    "velocityX": 3.779257621565883,
-                    "velocityY": -0.16688674872173656,
-                    "timestamp": 0.5618488372634539
-                },
-                {
-                    "x": 4.091895327848636,
-                    "y": 6.677873084552109,
-                    "heading": -2.8149901208171126,
-                    "angularVelocity": -3.1950635675887034e-8,
-                    "velocityX": 3.779257589435919,
-                    "velocityY": -0.16688747632201045,
-                    "timestamp": 0.6554903101406963
-                },
-                {
-                    "x": 4.445790571527361,
-                    "y": 6.662245418924005,
-                    "heading": -2.814990123809099,
-                    "angularVelocity": -3.195149337938281e-8,
-                    "velocityX": 3.779257553356315,
-                    "velocityY": -0.16688829370019806,
-                    "timestamp": 0.7491317830179386
-                },
-                {
-                    "x": 4.737874328479073,
-                    "y": 6.649347276687054,
-                    "heading": -2.8409937626507493,
-                    "angularVelocity": -0.27769361205743737,
-                    "velocityX": 3.119170897008512,
-                    "velocityY": -0.13773963438078918,
-                    "timestamp": 0.8427732558951809
-                },
-                {
-                    "x": 4.956937160751323,
-                    "y": 6.639673651230963,
-                    "heading": -2.8604968062706018,
-                    "angularVelocity": -0.20827356747619122,
-                    "velocityX": 2.3393783282266916,
-                    "velocityY": -0.10330492632038867,
-                    "timestamp": 0.9364147287724233
-                },
-                {
-                    "x": 5.102979052789527,
-                    "y": 6.633224554793233,
-                    "heading": -2.8734989345094433,
-                    "angularVelocity": -0.1388501039053446,
-                    "velocityX": 1.5595855933369822,
-                    "velocityY": -0.06887008757539573,
-                    "timestamp": 1.0300562016496655
+                    "x": 2.373020964624076,
+                    "y": 6.753775569913306,
+                    "heading": 0.3835012481902214,
+                    "angularVelocity": -0.06940035874833077,
+                    "velocityX": 0.7797929955465333,
+                    "velocityY": -0.034433782259311436,
+                    "timestamp": 0.09364147285382916
+                },
+                {
+                    "x": 2.5190628897747724,
+                    "y": 6.747326705309919,
+                    "heading": 0.37050354798266344,
+                    "angularVelocity": -0.13880281686559404,
+                    "velocityX": 1.5595859473362035,
+                    "velocityY": -0.06886761182670828,
+                    "timestamp": 0.18728294570765833
+                },
+                {
+                    "x": 2.738125767671939,
+                    "y": 6.737653401682201,
+                    "heading": 0.35100657007014807,
+                    "angularVelocity": -0.20820879166370637,
+                    "velocityX": 2.3393788160414357,
+                    "velocityY": -0.10330148953128306,
+                    "timestamp": 0.2809244185614875
+                },
+                {
+                    "x": 3.0302095781027427,
+                    "y": 6.7247556545524185,
+                    "heading": 0.3250098803168658,
+                    "angularVelocity": -0.27761940261055373,
+                    "velocityX": 3.119171468893202,
+                    "velocityY": -0.13773541505367973,
+                    "timestamp": 0.37456589141531665
+                },
+                {
+                    "x": 3.384104830792732,
+                    "y": 6.709128195027275,
+                    "heading": 0.3250098773247812,
+                    "angularVelocity": -3.195254338233196e-8,
+                    "velocityX": 3.77925765053277,
+                    "velocityY": -0.16688609276314717,
+                    "timestamp": 0.46820736426914583
+                },
+                {
+                    "x": 3.738000080774581,
+                    "y": 6.693500674175882,
+                    "heading": 0.32500987433288897,
+                    "angularVelocity": -3.1950503021671023e-8,
+                    "velocityX": 3.7792576216124307,
+                    "velocityY": -0.16688674766770242,
+                    "timestamp": 0.561848837122975
+                },
+                {
+                    "x": 4.091895327748041,
+                    "y": 6.677873085197927,
+                    "heading": 0.32500987134099335,
+                    "angularVelocity": -3.1950540402322325e-8,
+                    "velocityX": 3.7792575894857667,
+                    "velocityY": -0.1668874751932408,
+                    "timestamp": 0.6554903099768041
+                },
+                {
+                    "x": 4.445790571342994,
+                    "y": 6.662245419680397,
+                    "heading": 0.32500986834901585,
+                    "angularVelocity": -3.1951399041908766e-8,
+                    "velocityX": 3.7792575534066395,
+                    "velocityY": -0.166888292561096,
+                    "timestamp": 0.7491317828306333
+                },
+                {
+                    "x": 4.737874328369047,
+                    "y": 6.649347277173169,
+                    "heading": 0.29900623239629953,
+                    "angularVelocity": -0.2776935812758607,
+                    "velocityX": 3.119170898582289,
+                    "velocityY": -0.13773963730152244,
+                    "timestamp": 0.8427732556844625
+                },
+                {
+                    "x": 4.956937160697527,
+                    "y": 6.639673651489352,
+                    "heading": 0.27950319129562423,
+                    "angularVelocity": -0.20827354062589903,
+                    "velocityX": 2.339378329412089,
+                    "velocityY": -0.10330492877809694,
+                    "timestamp": 0.9364147285382917
+                },
+                {
+                    "x": 5.102979052772278,
+                    "y": 6.633224554883952,
+                    "heading": 0.26650106474792545,
+                    "angularVelocity": -0.13885008588030126,
+                    "velocityX": 1.5595855941172054,
+                    "velocityY": -0.06887008938317173,
+                    "timestamp": 1.0300562013921208
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": -2.88,
-                    "angularVelocity": -0.06942506659508625,
-                    "velocityX": 0.7797928093911877,
-                    "velocityY": -0.0344351139955655,
-                    "timestamp": 1.1236976745269078
+                    "heading": 0.26,
+                    "angularVelocity": -0.06942505868186148,
+                    "velocityX": 0.7797928097703705,
+                    "velocityY": -0.03443511497296222,
+                    "timestamp": 1.12369767424595
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": -2.88,
-                    "angularVelocity": 1.756012262603739e-32,
-                    "velocityX": 4.952933956711668e-34,
-                    "velocityY": 0,
-                    "timestamp": 1.2173391474041502
+                    "heading": 0.26,
+                    "angularVelocity": 0,
+                    "velocityX": -1.6658104354370778e-33,
+                    "velocityY": -5.427630465010738e-32,
+                    "timestamp": 1.217339147099779
                 }
             ],
             "constraints": [
@@ -9532,7 +9654,7 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -9541,7 +9663,7 @@
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": -3.143185307179586,
+                    "heading": -0.003,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -9552,128 +9674,128 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 0.39,
                     "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
+                    "velocityX": 1.1894430825117781e-34,
+                    "velocityY": 3.241165026752671e-33,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.347774752133698,
-                    "y": 6.727866832421063,
-                    "heading": -2.7630995428589378,
-                    "angularVelocity": -0.15973393577530476,
-                    "velocityX": 0.5825584351440611,
-                    "velocityY": -0.3552456424698836,
-                    "timestamp": 0.08200851494302656
-                },
-                {
-                    "x": 2.4433243460965994,
-                    "y": 6.669600579952885,
-                    "heading": -2.78929580141764,
-                    "angularVelocity": -0.31943339757952466,
-                    "velocityX": 1.1651179640221794,
-                    "velocityY": -0.7104902766335507,
-                    "timestamp": 0.16401702988605313
-                },
-                {
-                    "x": 2.586648917905952,
-                    "y": 6.582201380168291,
-                    "heading": -2.8285851612262256,
-                    "angularVelocity": -0.4790887853033408,
-                    "velocityX": 1.74767915147499,
-                    "velocityY": -1.065733233254043,
-                    "timestamp": 0.24602554482907968
-                },
-                {
-                    "x": 2.7777486316597817,
-                    "y": 6.465669444023111,
-                    "heading": -2.8809649184638944,
-                    "angularVelocity": -0.6387112030264004,
-                    "velocityX": 2.3302423399154564,
-                    "velocityY": -1.4209736175095709,
-                    "timestamp": 0.32803405977210626
-                },
-                {
-                    "x": 3.016623643228877,
-                    "y": 6.320005072616726,
-                    "heading": -2.9464355230657095,
-                    "angularVelocity": -0.7983391071927027,
-                    "velocityX": 2.9128074290218198,
-                    "velocityY": -1.776210330202685,
-                    "timestamp": 0.41004257471513283
-                },
-                {
-                    "x": 3.2814961744061293,
-                    "y": 6.158489554910389,
-                    "heading": -2.9464355313484747,
-                    "angularVelocity": -1.0099884469874073e-7,
-                    "velocityX": 3.229817432510106,
-                    "velocityY": -1.9694969213690323,
-                    "timestamp": 0.4920510896581594
-                },
-                {
-                    "x": 3.546368691279801,
-                    "y": 5.996974013747309,
-                    "heading": -2.9464355396312305,
-                    "angularVelocity": -1.009987284542081e-7,
-                    "velocityX": 3.229817258094308,
-                    "velocityY": -1.9694972073971664,
-                    "timestamp": 0.574059604601186
-                },
-                {
-                    "x": 3.785245806593771,
-                    "y": 5.851315740974219,
-                    "heading": -3.0120060162505475,
-                    "angularVelocity": -0.7995569321657724,
-                    "velocityX": 2.912833081783328,
-                    "velocityY": -1.7761359643481167,
-                    "timestamp": 0.6560681195442125
-                },
-                {
-                    "x": 3.976347491680443,
-                    "y": 5.7347893459358685,
-                    "heading": -3.0644727513052605,
-                    "angularVelocity": -0.6397717979793073,
-                    "velocityX": 2.3302663780637296,
-                    "velocityY": -1.4209060500523003,
-                    "timestamp": 0.738076634487239
-                },
-                {
-                    "x": 4.119673744072056,
-                    "y": 5.64739465388186,
-                    "heading": -3.1038280635353432,
-                    "angularVelocity": -0.4798929996162412,
-                    "velocityX": 1.7476996442526245,
-                    "velocityY": -1.0656782666376068,
-                    "timestamp": 0.8200851494302656
-                },
-                {
-                    "x": 4.2152245778751,
-                    "y": 5.589131554648123,
-                    "heading": -3.130066448240266,
-                    "angularVelocity": -0.3199470777290782,
-                    "velocityX": 1.1651330824539987,
-                    "velocityY": -0.7104518265476871,
-                    "timestamp": 0.9020936643732921
+                    "x": 2.347774772680024,
+                    "y": 6.727866824995534,
+                    "heading": 0.376906913073687,
+                    "angularVelocity": -0.15965534994810393,
+                    "velocityX": 0.5825591851522491,
+                    "velocityY": -0.35524603759330214,
+                    "timestamp": 0.08200844463131937
+                },
+                {
+                    "x": 2.443324407414016,
+                    "y": 6.669600558032611,
+                    "heading": 0.35072353035940795,
+                    "angularVelocity": -0.3192766651286972,
+                    "velocityX": 1.1651194601183832,
+                    "velocityY": -0.7104910625346852,
+                    "timestamp": 0.16401688926263874
+                },
+                {
+                    "x": 2.5866490395090382,
+                    "y": 6.582201337136711,
+                    "heading": 0.31145340371020835,
+                    "angularVelocity": -0.4788546694885386,
+                    "velocityX": 1.7476813850007613,
+                    "velocityY": -1.0657344044118373,
+                    "timestamp": 0.24602533389395811
+                },
+                {
+                    "x": 2.7777488318834505,
+                    "y": 6.4656693736120285,
+                    "heading": 0.25909914961330927,
+                    "angularVelocity": -0.6384007687533294,
+                    "velocityX": 2.330245296487799,
+                    "velocityY": -1.420975169673899,
+                    "timestamp": 0.3280337785252775
+                },
+                {
+                    "x": 3.016623938716172,
+                    "y": 6.320004968537675,
+                    "heading": 0.19366021259536087,
+                    "angularVelocity": -0.7979536413858164,
+                    "velocityX": 2.9128110880119595,
+                    "velocityY": -1.7762122636175968,
+                    "timestamp": 0.4100422231565969
+                },
+                {
+                    "x": 3.281496231375059,
+                    "y": 6.1584895705745515,
+                    "heading": 0.19366020431656966,
+                    "angularVelocity": -1.0095047213207674e-7,
+                    "velocityX": 3.2298172932026463,
+                    "velocityY": -1.9694971498269327,
+                    "timestamp": 0.4920506677879163
+                },
+                {
+                    "x": 3.54636850972689,
+                    "y": 5.99697414914899,
+                    "heading": 0.1936601960377879,
+                    "angularVelocity": -1.0095035712972839e-7,
+                    "velocityX": 3.2298171187443208,
+                    "velocityY": -1.9694974359247637,
+                    "timestamp": 0.5740591124192357
+                },
+                {
+                    "x": 3.7852456869653537,
+                    "y": 5.851315832693838,
+                    "heading": 0.1281196168762306,
+                    "angularVelocity": -0.7991930520838956,
+                    "velocityX": 2.91283633426253,
+                    "velocityY": -1.7761380198097898,
+                    "timestamp": 0.6560675570505551
+                },
+                {
+                    "x": 3.976347420420938,
+                    "y": 5.73478940159931,
+                    "heading": 0.07567676319198373,
+                    "angularVelocity": -0.63948114026563,
+                    "velocityX": 2.3302689657718703,
+                    "velocityY": -1.420907707960915,
+                    "timestamp": 0.7380760016818745
+                },
+                {
+                    "x": 4.119673708551739,
+                    "y": 5.647394681888429,
+                    "heading": 0.03633934856671714,
+                    "angularVelocity": -0.47967517006466276,
+                    "velocityX": 1.7477015784794376,
+                    "velocityY": -1.0656795175640326,
+                    "timestamp": 0.8200844463131939
+                },
+                {
+                    "x": 4.215224566025358,
+                    "y": 5.589131563991008,
+                    "heading": 0.01011289315165925,
+                    "angularVelocity": -0.31980188787828684,
+                    "velocityX": 1.1651343700417993,
+                    "velocityY": -0.7104526632513426,
+                    "timestamp": 0.9020928909445133
                 },
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -0.15996947327279604,
-                    "velocityX": 0.5825666049201219,
-                    "velocityY": -0.3552259746242433,
-                    "timestamp": 0.9841021793163186
+                    "heading": -0.003,
+                    "angularVelocity": -0.15989686440963644,
+                    "velocityX": 0.5825672488903126,
+                    "velocityY": -0.3552263931107682,
+                    "timestamp": 0.9841013355758327
                 },
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": 0,
-                    "velocityX": -4.515906316961801e-34,
-                    "velocityY": 2.878377535752905e-33,
-                    "timestamp": 1.0661106942593452
+                    "heading": -0.003,
+                    "angularVelocity": -2.9357360091407933e-34,
+                    "velocityX": 2.8612808764100986e-34,
+                    "velocityY": 0,
+                    "timestamp": 1.066109780207152
                 }
             ],
             "constraints": [
@@ -9716,7 +9838,7 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -9725,7 +9847,7 @@
                 {
                     "x": 2.1914827823638916,
                     "y": 2.376894474029541,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -9734,555 +9856,14 @@
                 {
                     "x": 4.263,
                     "y": 3,
-                    "heading": -3.7091853071795864,
+                    "heading": -0.569,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": -2.75,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -7.419385593511551e-32,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.293773644442264,
-                    "y": 6.737033643205664,
-                    "heading": -2.75526766821854,
-                    "angularVelocity": -0.10504784252909477,
-                    "velocityX": -0.1241659859778435,
-                    "velocityY": -0.3981690982414253,
-                    "timestamp": 0.050145420331514534
-                },
-                {
-                    "x": 2.2813193646388377,
-                    "y": 6.697095621507987,
-                    "heading": -2.7655160845701583,
-                    "angularVelocity": -0.20437392455513897,
-                    "velocityX": -0.24836325473174134,
-                    "velocityY": -0.7964440507955471,
-                    "timestamp": 0.10029084066302907
-                },
-                {
-                    "x": 2.2626352666399625,
-                    "y": 6.637179497704236,
-                    "heading": -2.7803732946846287,
-                    "angularVelocity": -0.2962824923243045,
-                    "velocityX": -0.3725982926327797,
-                    "velocityY": -1.1948473740501462,
-                    "timestamp": 0.1504362609945436
-                },
-                {
-                    "x": 2.2377190441074863,
-                    "y": 6.557277385764274,
-                    "heading": -2.799337789044294,
-                    "angularVelocity": -0.3781899570148323,
-                    "velocityX": -0.4968793235305074,
-                    "velocityY": -1.5934079605220988,
-                    "timestamp": 0.20058168132605814
-                },
-                {
-                    "x": 2.2065679165597203,
-                    "y": 6.457379637066711,
-                    "heading": -2.8216967909576067,
-                    "angularVelocity": -0.44588322852808987,
-                    "velocityX": -0.6212158027956237,
-                    "velocityY": -1.9921609598071555,
-                    "timestamp": 0.25072710165757267
-                },
-                {
-                    "x": 2.1691787676944525,
-                    "y": 6.337475004416767,
-                    "heading": -2.846358557659031,
-                    "angularVelocity": -0.49180496520688655,
-                    "velocityX": -0.7456144273611754,
-                    "velocityY": -2.3911382506567254,
-                    "timestamp": 0.3008725219890872
-                },
-                {
-                    "x": 2.1255494213188793,
-                    "y": 6.197554115260485,
-                    "heading": -2.8714458003624506,
-                    "angularVelocity": -0.5002898078741331,
-                    "velocityX": -0.8700564495648131,
-                    "velocityY": -2.7903024489824935,
-                    "timestamp": 0.35101794232060174
-                },
-                {
-                    "x": 2.0756883391523315,
-                    "y": 6.037638592000563,
-                    "heading": -2.8929930081362487,
-                    "angularVelocity": -0.4296944293486671,
-                    "velocityX": -0.9943297281568805,
-                    "velocityY": -3.1890354533417224,
-                    "timestamp": 0.4011633626521163
-                },
-                {
-                    "x": 2.0198170203632526,
-                    "y": 5.858213938792853,
-                    "heading": -2.897462778158027,
-                    "angularVelocity": -0.08913615624774217,
-                    "velocityX": -1.1141858702092737,
-                    "velocityY": -3.578086533556236,
-                    "timestamp": 0.4513087829836308
-                },
-                {
-                    "x": 1.9633339655295539,
-                    "y": 5.677120968466723,
-                    "heading": -2.897462791142581,
-                    "angularVelocity": -2.5893798802100903e-7,
-                    "velocityX": -1.1263851107496088,
-                    "velocityY": -3.611356114454979,
-                    "timestamp": 0.5014542033151453
-                },
-                {
-                    "x": 1.9068509267595308,
-                    "y": 5.496027993130308,
-                    "heading": -2.897462804127121,
-                    "angularVelocity": -2.5893770297899326e-7,
-                    "velocityX": -1.1263847904077713,
-                    "velocityY": -3.611356214370084,
-                    "timestamp": 0.5515996236466598
-                },
-                {
-                    "x": 1.8503679083113405,
-                    "y": 5.314935011455497,
-                    "heading": -2.897462817111663,
-                    "angularVelocity": -2.589377492115138e-7,
-                    "velocityX": -1.1263843851497735,
-                    "velocityY": -3.611356340770398,
-                    "timestamp": 0.6017450439781744
-                },
-                {
-                    "x": 1.7938849123636174,
-                    "y": 5.133842022762774,
-                    "heading": -2.8974628300962064,
-                    "angularVelocity": -2.589377769209582e-7,
-                    "velocityX": -1.1263839364454482,
-                    "velocityY": -3.6113564807215806,
-                    "timestamp": 0.651890464309689
-                },
-                {
-                    "x": 1.737401940953919,
-                    "y": 4.952749026416629,
-                    "heading": -2.89746284308075,
-                    "angularVelocity": -2.5893777760216377e-7,
-                    "velocityX": -1.126383447108141,
-                    "velocityY": -3.611356633346137,
-                    "timestamp": 0.7020358846412036
-                },
-                {
-                    "x": 1.680918995692082,
-                    "y": 4.771656021914957,
-                    "heading": -2.897462856065292,
-                    "angularVelocity": -2.5893774300527816e-7,
-                    "velocityX": -1.1263829256674796,
-                    "velocityY": -3.6113567959836606,
-                    "timestamp": 0.7521813049727182
-                },
-                {
-                    "x": 1.6244360774524895,
-                    "y": 4.590563008985036,
-                    "heading": -2.8974628690498303,
-                    "angularVelocity": -2.589376688920553e-7,
-                    "velocityX": -1.1263823867898586,
-                    "velocityY": -3.611356964059789,
-                    "timestamp": 0.8023267253042328
-                },
-                {
-                    "x": 1.567953186210211,
-                    "y": 4.40946998763471,
-                    "heading": -2.897462882034363,
-                    "angularVelocity": -2.589375607053774e-7,
-                    "velocityX": -1.1263818484094146,
-                    "velocityY": -3.6113571319795557,
-                    "timestamp": 0.8524721456357474
-                },
-                {
-                    "x": 1.5114708900262606,
-                    "y": 4.228376780686611,
-                    "heading": -2.8974628950188928,
-                    "angularVelocity": -2.589374956200649e-7,
-                    "velocityX": -1.1263699817559003,
-                    "velocityY": -3.611360833170398,
-                    "timestamp": 0.902617565967262
-                },
-                {
-                    "x": 1.4675217320832292,
-                    "y": 4.043840955415942,
-                    "heading": -2.8974629081797687,
-                    "angularVelocity": -2.624542007539591e-7,
-                    "velocityX": -0.8764341320200526,
-                    "velocityY": -3.6800135296641385,
-                    "timestamp": 0.9527629862987765
-                },
-                {
-                    "x": 1.4442056001491934,
-                    "y": 3.855582205665926,
-                    "heading": -2.8974629221278954,
-                    "angularVelocity": -2.7815354458428775e-7,
-                    "velocityX": -0.4649703159309848,
-                    "velocityY": -3.75425609169144,
-                    "timestamp": 1.0029084066302911
-                },
-                {
-                    "x": 1.4418077217209881,
-                    "y": 3.665900239807532,
-                    "heading": -2.897462937731955,
-                    "angularVelocity": -3.1117616956346287e-7,
-                    "velocityX": -0.04781849294218802,
-                    "velocityY": -3.782637868112273,
-                    "timestamp": 1.0530538269618057
-                },
-                {
-                    "x": 1.4603574724576953,
-                    "y": 3.47711225483097,
-                    "heading": -2.8974629563063408,
-                    "angularVelocity": -3.7041041196117577e-7,
-                    "velocityX": 0.3699191394562724,
-                    "velocityY": -3.7648101008720767,
-                    "timestamp": 1.1031992472933203
-                },
-                {
-                    "x": 1.4996282585911056,
-                    "y": 3.2915246350140723,
-                    "heading": -2.897463288823342,
-                    "angularVelocity": -0.000006631054220103306,
-                    "velocityX": 0.7831380388037097,
-                    "velocityY": -3.7009884170870753,
-                    "timestamp": 1.153344667624835
-                },
-                {
-                    "x": 1.5575649679963268,
-                    "y": 3.1148801932866275,
-                    "heading": -2.9095242713797083,
-                    "angularVelocity": -0.24052012081323423,
-                    "velocityX": 1.1553738910193128,
-                    "velocityY": -3.5226435546783237,
-                    "timestamp": 1.2034900879563495
-                },
-                {
-                    "x": 1.6317457698728626,
-                    "y": 2.9507691275120527,
-                    "heading": -2.937069238173179,
-                    "angularVelocity": -0.549301743037936,
-                    "velocityX": 1.4793135920712512,
-                    "velocityY": -3.2727029644905956,
-                    "timestamp": 1.253635508287864
-                },
-                {
-                    "x": 1.7203254363653924,
-                    "y": 2.801423061648199,
-                    "heading": -2.9780621265122655,
-                    "angularVelocity": -0.8174802019421967,
-                    "velocityX": 1.7664557582112876,
-                    "velocityY": -2.9782593281004814,
-                    "timestamp": 1.3037809286193787
-                },
-                {
-                    "x": 1.821600608747065,
-                    "y": 2.668597377909185,
-                    "heading": -3.0274133717272194,
-                    "angularVelocity": -0.9841625593860135,
-                    "velocityX": 2.019629543677879,
-                    "velocityY": -2.648809858625076,
-                    "timestamp": 1.3539263489508933
-                },
-                {
-                    "x": 1.9344033083274008,
-                    "y": 2.553226314276784,
-                    "heading": -3.081880110008977,
-                    "angularVelocity": -1.0861757249549924,
-                    "velocityX": 2.2495114974525996,
-                    "velocityY": -2.300729814800146,
-                    "timestamp": 1.4040717692824078
-                },
-                {
-                    "x": 2.057902367887511,
-                    "y": 2.4558701645332204,
-                    "heading": -3.139236203843767,
-                    "angularVelocity": -1.1437952549924524,
-                    "velocityX": 2.4628183140883095,
-                    "velocityY": -1.9414763920600462,
-                    "timestamp": 1.4542171896139224
-                },
-                {
-                    "x": 2.1914827823638916,
-                    "y": 2.376894474029541,
-                    "heading": -3.1978794252899205,
-                    "angularVelocity": -1.1694631545305318,
-                    "velocityX": 2.663860699407278,
-                    "velocityY": -1.5749332637270959,
-                    "timestamp": 1.504362609945437
-                },
-                {
-                    "x": 2.278019971764992,
-                    "y": 2.3350837736524683,
-                    "heading": -3.2342809833466872,
-                    "angularVelocity": -1.1705395923861364,
-                    "velocityX": 2.7827162301626456,
-                    "velocityY": -1.3444776209968852,
-                    "timestamp": 1.535460710593682
-                },
-                {
-                    "x": 2.368131283216315,
-                    "y": 2.300501166585177,
-                    "heading": -3.270450856694399,
-                    "angularVelocity": -1.1630894682876274,
-                    "velocityX": 2.8976467878404213,
-                    "velocityY": -1.112048850135951,
-                    "timestamp": 1.5665588112419269
-                },
-                {
-                    "x": 2.461670879436653,
-                    "y": 2.2732156435859956,
-                    "heading": -3.306075840058796,
-                    "angularVelocity": -1.1455678199567971,
-                    "velocityX": 3.007887757467237,
-                    "velocityY": -0.8774015914287735,
-                    "timestamp": 1.5976569118901718
-                },
-                {
-                    "x": 2.5584617966487637,
-                    "y": 2.2533048266167017,
-                    "heading": -3.3407803732359653,
-                    "angularVelocity": -1.1159695432761514,
-                    "velocityX": 3.112438225952366,
-                    "velocityY": -0.6402582972673794,
-                    "timestamp": 1.6287550125384167
-                },
-                {
-                    "x": 2.658285404214264,
-                    "y": 2.240855747544563,
-                    "heading": -3.37410574490051,
-                    "angularVelocity": -1.0716208054472351,
-                    "velocityX": 3.209958341013189,
-                    "velocityY": -0.40031637986360386,
-                    "timestamp": 1.6598531131866616
-                },
-                {
-                    "x": 2.7608658878900645,
-                    "y": 2.2359648026671843,
-                    "heading": -3.405478910014421,
-                    "angularVelocity": -1.0088450567700136,
-                    "velocityX": 3.2986092892329157,
-                    "velocityY": -0.1572747137422285,
-                    "timestamp": 1.6909512138349065
-                },
-                {
-                    "x": 2.8658468441128373,
-                    "y": 2.238735383081805,
-                    "heading": -3.4341645997548764,
-                    "angularVelocity": -0.9224257797905216,
-                    "velocityX": 3.3757996158745374,
-                    "velocityY": 0.08909162800513673,
-                    "timestamp": 1.7220493144831515
-                },
-                {
-                    "x": 2.972755430801249,
-                    "y": 2.249269494915596,
-                    "heading": -3.459191954885324,
-                    "angularVelocity": -0.8047872573808069,
-                    "velocityX": 3.4377850884743864,
-                    "velocityY": 0.3387381098589673,
-                    "timestamp": 1.7531474151313964
-                },
-                {
-                    "x": 3.0809481205317057,
-                    "y": 2.267644679606423,
-                    "heading": -3.4792519494186864,
-                    "angularVelocity": -0.6450552964717784,
-                    "velocityX": 3.4790770971590295,
-                    "velocityY": 0.5908780378155715,
-                    "timestamp": 1.7842455157796413
-                },
-                {
-                    "x": 3.1896989899339894,
-                    "y": 2.293908495297157,
-                    "heading": -3.4936234234211128,
-                    "angularVelocity": -0.4621334969934767,
-                    "velocityX": 3.4970260927630066,
-                    "velocityY": 0.8445472599052618,
-                    "timestamp": 1.8153436164278862
-                },
-                {
-                    "x": 3.298236220991847,
-                    "y": 2.3281164444820805,
-                    "heading": -3.5030903221852148,
-                    "angularVelocity": -0.30442048121156556,
-                    "velocityX": 3.4901562730643,
-                    "velocityY": 1.1000012371126273,
-                    "timestamp": 1.8464417170761311
-                },
-                {
-                    "x": 3.405452310949792,
-                    "y": 2.3702201329887522,
-                    "heading": -3.5091052020695845,
-                    "angularVelocity": -0.1934163102887616,
-                    "velocityX": 3.447673257305325,
-                    "velocityY": 1.3538990365653494,
-                    "timestamp": 1.877539817724376
-                },
-                {
-                    "x": 3.5097674187642585,
-                    "y": 2.419844212041495,
-                    "heading": -3.514308835252089,
-                    "angularVelocity": -0.1673296141576457,
-                    "velocityX": 3.354388391573783,
-                    "velocityY": 1.5957270064190303,
-                    "timestamp": 1.908637918372621
-                },
-                {
-                    "x": 3.6091747208997034,
-                    "y": 2.4757866931575614,
-                    "heading": -3.523108085575375,
-                    "angularVelocity": -0.2829513745169274,
-                    "velocityX": 3.196571496756511,
-                    "velocityY": 1.7989034683770408,
-                    "timestamp": 1.939736019020866
-                },
-                {
-                    "x": 3.701528766735335,
-                    "y": 2.535299893209444,
-                    "heading": -3.5376794547025563,
-                    "angularVelocity": -0.4685613855325914,
-                    "velocityX": 2.969764838060721,
-                    "velocityY": 1.913724594471066,
-                    "timestamp": 1.9708341196691108
-                },
-                {
-                    "x": 3.7858722644846305,
-                    "y": 2.59510740434591,
-                    "heading": -3.555385401955535,
-                    "angularVelocity": -0.5693578348478764,
-                    "velocityX": 2.7121752129918284,
-                    "velocityY": 1.923188551383177,
-                    "timestamp": 2.001932220317356
-                },
-                {
-                    "x": 3.862373728468494,
-                    "y": 2.653074903297687,
-                    "heading": -3.5744288715420867,
-                    "angularVelocity": -0.6123676105476805,
-                    "velocityX": 2.460004385772028,
-                    "velocityY": 1.8640205589227477,
-                    "timestamp": 2.033030320965601
-                },
-                {
-                    "x": 3.931417292290283,
-                    "y": 2.707976512433947,
-                    "heading": -3.5936932545527167,
-                    "angularVelocity": -0.6194713699243786,
-                    "velocityX": 2.220185875746878,
-                    "velocityY": 1.7654328718419663,
-                    "timestamp": 2.064128421613846
-                },
-                {
-                    "x": 3.9933519727900295,
-                    "y": 2.7590743829972735,
-                    "heading": -3.612463672222599,
-                    "angularVelocity": -0.6035872699166261,
-                    "velocityX": 1.991590457574803,
-                    "velocityY": 1.643118695295981,
-                    "timestamp": 2.0952265222620907
-                },
-                {
-                    "x": 4.04845853802061,
-                    "y": 2.8058913864006936,
-                    "heading": -3.6302579260771157,
-                    "angularVelocity": -0.5721974488337824,
-                    "velocityX": 1.7720235024607334,
-                    "velocityY": 1.5054618265268112,
-                    "timestamp": 2.1263246229103356
-                },
-                {
-                    "x": 4.096959063236219,
-                    "y": 2.8480989028223083,
-                    "heading": -3.64673270512686,
-                    "angularVelocity": -0.5297680149691044,
-                    "velocityX": 1.5595976668866187,
-                    "velocityY": 1.3572377586345543,
-                    "timestamp": 2.1574227235585806
-                },
-                {
-                    "x": 4.139030722594851,
-                    "y": 2.8854588640311274,
-                    "heading": -3.6616322031984536,
-                    "angularVelocity": -0.4791127998494546,
-                    "velocityX": 1.352869097521714,
-                    "velocityY": 1.2013582961674927,
-                    "timestamp": 2.1885208242068255
-                },
-                {
-                    "x": 4.174817002989322,
-                    "y": 2.917791762586731,
-                    "heading": -3.6747587890471625,
-                    "angularVelocity": -0.4221024942061285,
-                    "velocityX": 1.150754536402508,
-                    "velocityY": 1.0397065377505372,
-                    "timestamp": 2.2196189248550704
-                },
-                {
-                    "x": 4.204435949248215,
-                    "y": 2.9449578523068656,
-                    "heading": -3.68595534704571,
-                    "angularVelocity": -0.360039930579135,
-                    "velocityX": 0.9524358607593557,
-                    "velocityY": 0.8735610585165674,
-                    "timestamp": 2.2507170255033153
-                },
-                {
-                    "x": 4.227986113342503,
-                    "y": 2.9668454867866734,
-                    "heading": -3.6950940909562924,
-                    "angularVelocity": -0.2938682337530664,
-                    "velocityX": 0.7572862523234876,
-                    "velocityY": 0.7038254434694367,
-                    "timestamp": 2.2818151261515602
-                },
-                {
-                    "x": 4.245550869753413,
-                    "y": 2.983363547472538,
-                    "heading": -3.702069156081572,
-                    "angularVelocity": -0.22429231946250391,
-                    "velocityX": 0.5648176591100355,
-                    "velocityY": 0.5311597924485464,
-                    "timestamp": 2.312913226799805
-                },
-                {
-                    "x": 4.25720158771329,
-                    "y": 2.99443633378999,
-                    "heading": -3.7067915044128146,
-                    "angularVelocity": -0.15185327183332542,
-                    "velocityX": 0.3746440366779525,
-                    "velocityY": 0.3560598906890862,
-                    "timestamp": 2.34401132744805
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -3.7091853071795864,
-                    "angularVelocity": -0.07697585115715987,
-                    "velocityX": 0.1864555122609252,
-                    "velocityY": 0.17890694589168776,
-                    "timestamp": 2.375109428096295
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -3.7091853071795864,
-                    "angularVelocity": 7.12380386376873e-29,
-                    "velocityX": -3.695914984045945e-30,
-                    "velocityY": 3.729410348522699e-30,
-                    "timestamp": 2.40620752874454
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -10313,7 +9894,7 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -10322,7 +9903,7 @@
                 {
                     "x": 2.170377492904663,
                     "y": 3.170344352722168,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -10331,7 +9912,7 @@
                 {
                     "x": 5.176,
                     "y": 1.62,
-                    "heading": -3.7831853071795862,
+                    "heading": -0.643,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -10342,560 +9923,560 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 0.39,
                     "angularVelocity": 0,
                     "velocityX": 0,
-                    "velocityY": -1.7629358050488447e-33,
+                    "velocityY": 9.203343680058695e-35,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.2980097531788157,
-                    "y": 6.74279910356074,
-                    "heading": -2.762722782141496,
-                    "angularVelocity": -0.3022479537187966,
-                    "velocityX": -0.047281170298139656,
-                    "velocityY": -0.3373626807410066,
-                    "timestamp": 0.04209385699707004
-                },
-                {
-                    "x": 2.2940230158560544,
-                    "y": 6.7143715511474085,
-                    "heading": -2.7877986560948775,
-                    "angularVelocity": -0.5957133829557797,
-                    "velocityX": -0.09471066818701897,
-                    "velocityY": -0.6753373162100508,
-                    "timestamp": 0.08418771399414007
-                },
-                {
-                    "x": 2.2880322840288483,
-                    "y": 6.671686949257674,
-                    "heading": -2.8247754513459067,
-                    "angularVelocity": -0.8784368525222962,
-                    "velocityX": -0.14231843443624606,
-                    "velocityY": -1.0140339929578401,
-                    "timestamp": 0.12628157099121012
-                },
-                {
-                    "x": 2.2800276497015446,
-                    "y": 6.614707345454749,
-                    "heading": -2.873051459668216,
-                    "angularVelocity": -1.1468658794006534,
-                    "velocityX": -0.19016157934542763,
-                    "velocityY": -1.353632284323334,
-                    "timestamp": 0.16837542798828015
-                },
-                {
-                    "x": 2.269996639934327,
-                    "y": 6.543383180902105,
-                    "heading": -2.9317676826482244,
-                    "angularVelocity": -1.3948881658455714,
-                    "velocityX": -0.23830103684524737,
-                    "velocityY": -1.694407916993876,
-                    "timestamp": 0.21046928498535017
-                },
-                {
-                    "x": 2.2579259581538498,
-                    "y": 6.457648474415292,
-                    "heading": -2.9996334069438224,
-                    "angularVelocity": -1.6122477039897383,
-                    "velocityX": -0.28675637353255107,
-                    "velocityY": -2.0367510274190597,
-                    "timestamp": 0.2525631419824202
-                },
-                {
-                    "x": 2.2438060571207714,
-                    "y": 6.357417527376881,
-                    "heading": -3.0746299268265957,
-                    "angularVelocity": -1.7816499896408666,
-                    "velocityX": -0.3354385185957493,
-                    "velocityY": -2.3811300315242536,
-                    "timestamp": 0.2946569989794902
-                },
-                {
-                    "x": 2.227639514948982,
-                    "y": 6.242593213913433,
-                    "heading": -3.1534176716951676,
-                    "angularVelocity": -1.8717159815993158,
-                    "velocityX": -0.3840594168625274,
-                    "velocityY": -2.727816400180199,
-                    "timestamp": 0.33675085597656024
-                },
-                {
-                    "x": 2.2094628615660015,
-                    "y": 6.1131506228921415,
-                    "heading": -3.2295701531339893,
-                    "angularVelocity": -1.8091115158233875,
-                    "velocityX": -0.43181249426122753,
-                    "velocityY": -3.07509456855668,
-                    "timestamp": 0.37884471297363026
-                },
-                {
-                    "x": 2.189573480043473,
-                    "y": 5.97059741661809,
-                    "heading": -3.280208967362098,
-                    "angularVelocity": -1.2029977255739333,
-                    "velocityX": -0.4725008098904457,
-                    "velocityY": -3.386556054580001,
-                    "timestamp": 0.4209385699707003
-                },
-                {
-                    "x": 2.167754438435126,
-                    "y": 5.816653814130607,
-                    "heading": -3.2929329955032975,
-                    "angularVelocity": -0.3022775542304222,
-                    "velocityX": -0.5183426553158416,
-                    "velocityY": -3.657151267896363,
-                    "timestamp": 0.4630324269677703
-                },
-                {
-                    "x": 2.1447655867486377,
-                    "y": 5.659083438978025,
-                    "heading": -3.2929330394086493,
-                    "angularVelocity": -0.0000010430346734701328,
-                    "velocityX": -0.5461331730206714,
-                    "velocityY": -3.74331045890019,
-                    "timestamp": 0.5051262839648404
-                },
-                {
-                    "x": 2.1217767257990308,
-                    "y": 5.50151306517681,
-                    "heading": -3.2929330833137715,
-                    "angularVelocity": -0.0000010430292064009232,
-                    "velocityX": -0.5461333930793576,
-                    "velocityY": -3.7433104267965445,
-                    "timestamp": 0.5472201409619104
-                },
-                {
-                    "x": 2.098787889918124,
-                    "y": 5.3439426877181715,
-                    "heading": -3.292933127218878,
-                    "angularVelocity": -0.0000010430288283584586,
-                    "velocityX": -0.5461327975363938,
-                    "velocityY": -3.7433105136838924,
-                    "timestamp": 0.5893139979589804
-                },
-                {
-                    "x": 2.075799081922294,
-                    "y": 5.186372306191217,
-                    "heading": -3.292933171123966,
-                    "angularVelocity": -0.000001043028392528962,
-                    "velocityX": -0.5461321350863673,
-                    "velocityY": -3.7433106103325895,
-                    "timestamp": 0.6314078549560505
-                },
-                {
-                    "x": 2.052810304885704,
-                    "y": 5.028801920147444,
-                    "heading": -3.292933215029033,
-                    "angularVelocity": -0.0000010430278884550004,
-                    "velocityX": -0.5461313996051773,
-                    "velocityY": -3.7433107176360743,
-                    "timestamp": 0.6735017119531205
-                },
-                {
-                    "x": 2.029821561935122,
-                    "y": 4.8712315291306805,
-                    "heading": -3.2929332589340747,
-                    "angularVelocity": -0.0000010430273082107483,
-                    "velocityX": -0.546130589843121,
-                    "velocityY": -3.7433108357766267,
-                    "timestamp": 0.7155955689501905
-                },
-                {
-                    "x": 2.0068328558598987,
-                    "y": 4.71366113273398,
-                    "heading": -3.29293330283909,
-                    "angularVelocity": -0.0000010430266595669302,
-                    "velocityX": -0.5461297138160472,
-                    "velocityY": -3.7433109635847206,
-                    "timestamp": 0.7576894259472605
-                },
-                {
-                    "x": 1.9838441886370295,
-                    "y": 4.556090730668921,
-                    "heading": -3.2929333467440762,
-                    "angularVelocity": -0.0000010430259761326365,
-                    "velocityX": -0.5461287908225987,
-                    "velocityY": -3.7433110982447326,
-                    "timestamp": 0.7997832829443305
-                },
-                {
-                    "x": 1.960855561483052,
-                    "y": 4.398520322758029,
-                    "heading": -3.2929333906490403,
-                    "angularVelocity": -0.0000010430254501600513,
-                    "velocityX": -0.5461278389285532,
-                    "velocityY": -3.7433112371209067,
-                    "timestamp": 0.8418771399414006
-                },
-                {
-                    "x": 1.9378696268054536,
-                    "y": 4.240949522053656,
-                    "heading": -3.2929334345540515,
-                    "angularVelocity": -0.0000010430265803117889,
-                    "velocityX": -0.5460638752870397,
-                    "velocityY": -3.7433205684938873,
-                    "timestamp": 0.8839709969384706
-                },
-                {
-                    "x": 1.9275923145038192,
-                    "y": 4.0820429860952725,
-                    "heading": -3.292933478993091,
-                    "angularVelocity": -0.0000010557131812143582,
-                    "velocityX": -0.24415230712523545,
-                    "velocityY": -3.7750528769422123,
-                    "timestamp": 0.9260648539355406
-                },
-                {
-                    "x": 1.9320863630886849,
-                    "y": 3.9228678829456647,
-                    "heading": -3.29293352534017,
-                    "angularVelocity": -0.0000011010413859861484,
-                    "velocityX": 0.10676257547932101,
-                    "velocityY": -3.781433076106252,
-                    "timestamp": 0.9681587109326106
-                },
-                {
-                    "x": 1.9513134143780595,
-                    "y": 3.764794388276326,
-                    "heading": -3.2929335753664444,
-                    "angularVelocity": -0.0000011884459622031428,
-                    "velocityX": 0.4567662044063322,
-                    "velocityY": -3.75526278526441,
-                    "timestamp": 1.0102525679296808
-                },
-                {
-                    "x": 1.9851080356036048,
-                    "y": 3.609183233024402,
-                    "heading": -3.292933631555695,
-                    "angularVelocity": -0.000001334856299032327,
-                    "velocityX": 0.8028397404376079,
-                    "velocityY": -3.696766377639273,
-                    "timestamp": 1.052346424926751
-                },
-                {
-                    "x": 2.033179344139022,
-                    "y": 3.457373959395419,
-                    "heading": -3.2929336978163843,
-                    "angularVelocity": -0.000001574117798079096,
-                    "velocityX": 1.142002942110131,
-                    "velocityY": -3.6064472219675583,
-                    "timestamp": 1.094440281923821
-                },
-                {
-                    "x": 2.095113543139509,
-                    "y": 3.310673385447386,
-                    "heading": -3.29293378107099,
-                    "angularVelocity": -0.0000019778326800211228,
-                    "velocityX": 1.4713358057162051,
-                    "velocityY": -3.485082727350082,
-                    "timestamp": 1.1365341389208912
+                    "x": 2.298008161759761,
+                    "y": 6.742798044155305,
+                    "heading": 0.37730350882287184,
+                    "angularVelocity": -0.30162955292479776,
+                    "velocityX": -0.04731994607959398,
+                    "velocityY": -0.3373947598852993,
+                    "timestamp": 0.0420929947149233
+                },
+                {
+                    "x": 2.2940179124306126,
+                    "y": 6.714368498753073,
+                    "heading": 0.3522801711626307,
+                    "angularVelocity": -0.5944774856175671,
+                    "velocityX": -0.09479604281359731,
+                    "velocityY": -0.6753984978919232,
+                    "timestamp": 0.0841859894298466
+                },
+                {
+                    "x": 2.28802134748838,
+                    "y": 6.671681118579196,
+                    "heading": 0.31538217591497186,
+                    "angularVelocity": -0.8765828019021266,
+                    "velocityX": -0.14245992671333493,
+                    "velocityY": -1.014120769096589,
+                    "timestamp": 0.1262789841447699
+                },
+                {
+                    "x": 2.2800080756588827,
+                    "y": 6.61469814393844,
+                    "heading": 0.2672110996394858,
+                    "angularVelocity": -1.1443965106718308,
+                    "velocityX": -0.19037067530517543,
+                    "velocityY": -1.3537400944426796,
+                    "timestamp": 0.1683719788596932
+                },
+                {
+                    "x": 2.2699650128963897,
+                    "y": 6.543370292056319,
+                    "heading": 0.20862519634351256,
+                    "angularVelocity": -1.391820745773707,
+                    "velocityX": -0.23859226055333851,
+                    "velocityY": -1.694530226827328,
+                    "timestamp": 0.21046497357461652
+                },
+                {
+                    "x": 2.2578780077181495,
+                    "y": 6.4576319980717,
+                    "heading": 0.14091301714440063,
+                    "angularVelocity": -1.608632972248606,
+                    "velocityX": -0.2871500414760241,
+                    "velocityY": -2.036877978516017,
+                    "timestamp": 0.2525579682895398
+                },
+                {
+                    "x": 2.2437361567314555,
+                    "y": 6.357398182016401,
+                    "heading": 0.06608849726258184,
+                    "angularVelocity": -1.7776002964049293,
+                    "velocityX": -0.3359668534508049,
+                    "velocityY": -2.3812469683883926,
+                    "timestamp": 0.2946509630044631
+                },
+                {
+                    "x": 2.227539619778089,
+                    "y": 6.242572554004172,
+                    "heading": -0.012517898946656567,
+                    "angularVelocity": -1.8674460380308828,
+                    "velocityX": -0.3847798680768185,
+                    "velocityY": -2.727903509595609,
+                    "timestamp": 0.33674395771938637
+                },
+                {
+                    "x": 2.209319968778144,
+                    "y": 6.1131309245806085,
+                    "heading": -0.08849823738903302,
+                    "angularVelocity": -1.8050589880087344,
+                    "velocityX": -0.4328428310539146,
+                    "velocityY": -3.0751347177888695,
+                    "timestamp": 0.37883695243430965
+                },
+                {
+                    "x": 2.189363625025874,
+                    "y": 5.970570114356374,
+                    "heading": -0.13907359717832485,
+                    "angularVelocity": -1.201514887021361,
+                    "velocityX": -0.4741013056311528,
+                    "velocityY": -3.38680607520881,
+                    "timestamp": 0.42092994714923293
+                },
+                {
+                    "x": 2.1674827409245396,
+                    "y": 5.816618965425972,
+                    "heading": -0.15173248317406665,
+                    "angularVelocity": -0.3007361695568274,
+                    "velocityX": -0.5198224609468644,
+                    "velocityY": -3.657405465518488,
+                    "timestamp": 0.4630229418641562
+                },
+                {
+                    "x": 2.1445157307124236,
+                    "y": 5.659048701686038,
+                    "heading": -0.1517325271793822,
+                    "angularVelocity": -0.0000010454308573871989,
+                    "velocityX": -0.5456254744443967,
+                    "velocityY": -3.7433844944292183,
+                    "timestamp": 0.5051159365790795
+                },
+                {
+                    "x": 2.1215487153797565,
+                    "y": 5.501478438692377,
+                    "heading": -0.1517325711844689,
+                    "angularVelocity": -0.0000010454254199880688,
+                    "velocityX": -0.5456255960929445,
+                    "velocityY": -3.7433844767000393,
+                    "timestamp": 0.5472089312940028
+                },
+                {
+                    "x": 2.0985817251133763,
+                    "y": 5.343908172045118,
+                    "heading": -0.15173261518953962,
+                    "angularVelocity": -0.000001045425040533604,
+                    "velocityX": -0.5456250005951166,
+                    "velocityY": -3.7433845634983034,
+                    "timestamp": 0.5893019260089262
+                },
+                {
+                    "x": 2.075614762728362,
+                    "y": 5.186337901333946,
+                    "heading": -0.15173265919459192,
+                    "angularVelocity": -0.0000010454246033549915,
+                    "velocityX": -0.5456243382196686,
+                    "velocityY": -3.743384660044355,
+                    "timestamp": 0.6313949207238495
+                },
+                {
+                    "x": 2.0526478312972185,
+                    "y": 5.028767626111028,
+                    "heading": -0.15173270319962293,
+                    "angularVelocity": -0.0000010454240970480152,
+                    "velocityX": -0.5456236028509798,
+                    "velocityY": -3.7433847672295513,
+                    "timestamp": 0.6734879154387728
+                },
+                {
+                    "x": 2.0296809339445536,
+                    "y": 4.871197345920939,
+                    "heading": -0.15173274720462945,
+                    "angularVelocity": -0.0000010454235162635837,
+                    "velocityX": -0.5456227932512114,
+                    "velocityY": -3.743384885234266,
+                    "timestamp": 0.7155809101536962
+                },
+                {
+                    "x": 2.0067140734575584,
+                    "y": 4.7136270603574335,
+                    "heading": -0.15173279120960867,
+                    "angularVelocity": -0.0000010454228670132247,
+                    "velocityX": -0.5456219174363699,
+                    "velocityY": -3.7433850128900636,
+                    "timestamp": 0.7576739048686195
+                },
+                {
+                    "x": 1.983747251812091,
+                    "y": 4.55605676913253,
+                    "heading": -0.15173283521455908,
+                    "angularVelocity": -0.000001045422182833939,
+                    "velocityX": -0.545620994681206,
+                    "velocityY": -3.743385147387475,
+                    "timestamp": 0.7997668995835429
+                },
+                {
+                    "x": 1.9607804702256346,
+                    "y": 4.398486472068785,
+                    "heading": -0.15173287921948736,
+                    "angularVelocity": -0.0000010454216570158517,
+                    "velocityX": -0.5456200430024101,
+                    "velocityY": -3.7433852861004024,
+                    "timestamp": 0.8418598942984662
+                },
+                {
+                    "x": 1.9378163633559966,
+                    "y": 4.240915785172817,
+                    "heading": -0.15173292322446258,
+                    "angularVelocity": -0.0000010454227717672262,
+                    "velocityX": -0.5455564999630786,
+                    "velocityY": -3.7433945473141725,
+                    "timestamp": 0.8839528890133895
+                },
+                {
+                    "x": 1.9275487572437193,
+                    "y": 4.082011890541997,
+                    "heading": -0.15173296776197548,
+                    "angularVelocity": -0.000001058074228463605,
+                    "velocityX": -0.2439267194414494,
+                    "velocityY": -3.775067459728259,
+                    "timestamp": 0.9260458837283129
+                },
+                {
+                    "x": 1.9320519221515333,
+                    "y": 3.9228403083205765,
+                    "heading": -0.15173301420750987,
+                    "angularVelocity": -0.0000011034029462076059,
+                    "velocityX": 0.10698133830372927,
+                    "velocityY": -3.781426892988182,
+                    "timestamp": 0.9681388784432362
+                },
+                {
+                    "x": 1.9512874233679367,
+                    "y": 3.764771127717043,
+                    "heading": -0.15173306433513473,
+                    "angularVelocity": -0.0000011908780826817618,
+                    "velocityX": 0.45697630559851177,
+                    "velocityY": -3.7552372235348,
+                    "timestamp": 1.0102318731581594
+                },
+                {
+                    "x": 1.9850897622274482,
+                    "y": 3.609164986787025,
+                    "heading": -0.15173312063283023,
+                    "angularVelocity": -0.0000013374599716310608,
+                    "velocityX": 0.8030395339756486,
+                    "velocityY": -3.6967229816711127,
+                    "timestamp": 1.0523248678730828
+                },
+                {
+                    "x": 2.0331680016062417,
+                    "y": 3.4573613296965586,
+                    "heading": -0.1517331870158403,
+                    "angularVelocity": -0.0000015770560044658274,
+                    "velocityX": 1.142190991741159,
+                    "velocityY": -3.606387668982997,
+                    "timestamp": 1.094417862588006
+                },
+                {
+                    "x": 2.0951083019579966,
+                    "y": 3.3106668726225337,
+                    "heading": -0.15173327041919607,
+                    "angularVelocity": -0.0000019814070334134687,
+                    "velocityX": 1.4715108956074094,
+                    "velocityY": -3.4850088017619067,
+                    "timestamp": 1.1365108573029294
                 },
                 {
                     "x": 2.170377492904663,
                     "y": 3.170344352722168,
-                    "heading": -3.292933888363931,
-                    "angularVelocity": -0.00000254889782501678,
-                    "velocityX": 1.7880031704006887,
-                    "velocityY": -3.3337176190574715,
-                    "timestamp": 1.1786279959179613
-                },
-                {
-                    "x": 2.2421588223512248,
-                    "y": 3.0583154434355615,
-                    "heading": -3.2929339938603395,
-                    "angularVelocity": -0.000002999460293233564,
-                    "velocityX": 2.0408775039380207,
-                    "velocityY": -3.1851915047625172,
-                    "timestamp": 1.2137997930042903
-                },
-                {
-                    "x": 2.3224028083906,
-                    "y": 2.952183724176483,
-                    "heading": -3.2929340733758186,
-                    "angularVelocity": -0.000002260773842517862,
-                    "velocityX": 2.2814866650804433,
-                    "velocityY": -3.0175233582344014,
-                    "timestamp": 1.2489715900906193
-                },
-                {
-                    "x": 2.4106271869517015,
-                    "y": 2.852587036133821,
-                    "heading": -3.292934137641087,
-                    "angularVelocity": -0.0000018271818267107102,
-                    "velocityX": 2.508384156332853,
-                    "velocityY": -2.8317201932617277,
-                    "timestamp": 1.2841433871769483
-                },
-                {
-                    "x": 2.506301732241236,
-                    "y": 2.7601239405733753,
-                    "heading": -3.29293419233114,
-                    "angularVelocity": -0.0000015549405448920558,
-                    "velocityX": 2.720206336193231,
-                    "velocityY": -2.628898811553339,
-                    "timestamp": 1.3193151842632773
-                },
-                {
-                    "x": 2.6088514418801845,
-                    "y": 2.6753501198214895,
-                    "heading": -3.292934240814094,
-                    "angularVelocity": -0.0000013784610795710298,
-                    "velocityX": 2.9156801225493263,
-                    "velocityY": -2.410278341587403,
-                    "timestamp": 1.3544869813496063
-                },
-                {
-                    "x": 2.717659989787404,
-                    "y": 2.5987750311541573,
-                    "heading": -3.29293428528508,
-                    "angularVelocity": -0.0000012643933456255718,
-                    "velocityX": 3.093630605230363,
-                    "velocityY": -2.177173048035584,
-                    "timestamp": 1.3896587784359353
-                },
-                {
-                    "x": 2.8320734217114962,
-                    "y": 2.5308588229326605,
-                    "heading": -3.2929343273033043,
-                    "angularVelocity": -0.0000011946567396881703,
-                    "velocityX": 3.252987944951041,
-                    "velocityY": -1.9309848756034937,
-                    "timestamp": 1.4248305755222643
-                },
-                {
-                    "x": 2.951404044154876,
-                    "y": 2.472009459742262,
-                    "heading": -3.29293436807755,
-                    "angularVelocity": -0.000001159288093356335,
-                    "velocityX": 3.392792871813832,
-                    "velocityY": -1.6731975066827869,
-                    "timestamp": 1.4600023726085933
-                },
-                {
-                    "x": 3.0749338856232122,
-                    "y": 2.4225784637746113,
-                    "heading": -3.2929344086367345,
-                    "angularVelocity": -0.0000011531735020269185,
-                    "velocityX": 3.512184525719075,
-                    "velocityY": -1.4054157041314157,
-                    "timestamp": 1.4951741696949223
-                },
-                {
-                    "x": 3.199149697823942,
-                    "y": 2.374897470692157,
-                    "heading": -3.2929344492220305,
-                    "angularVelocity": -0.000001153915896405561,
-                    "velocityX": 3.531687957139154,
-                    "velocityY": -1.3556598477303206,
-                    "timestamp": 1.5303459667812513
-                },
-                {
-                    "x": 3.3233655700450013,
-                    "y": 2.3272166339715414,
-                    "heading": -3.2929344898073354,
-                    "angularVelocity": -0.0000011539161555742895,
-                    "velocityX": 3.5316896636294053,
-                    "velocityY": -1.3556554020706744,
-                    "timestamp": 1.5655177638675803
-                },
-                {
-                    "x": 3.4475814520083037,
-                    "y": 2.279535822630966,
-                    "heading": -3.2929345303926483,
-                    "angularVelocity": -0.0000011539163817435631,
-                    "velocityX": 3.531689940619614,
-                    "velocityY": -1.3556546804686425,
-                    "timestamp": 1.6006895609539094
-                },
-                {
-                    "x": 3.571797343695941,
-                    "y": 2.231855036623794,
-                    "heading": -3.2929345709779727,
-                    "angularVelocity": -0.0000011539167138358367,
-                    "velocityX": 3.53169021710065,
-                    "velocityY": -1.3556539601925897,
-                    "timestamp": 1.6358613580402384
-                },
-                {
-                    "x": 3.696013245125145,
-                    "y": 2.1841742759949323,
-                    "heading": -3.2929346115633122,
-                    "angularVelocity": -0.0000011539171376230348,
-                    "velocityX": 3.531690494071608,
-                    "velocityY": -1.355653238639736,
-                    "timestamp": 1.6710331551265674
-                },
-                {
-                    "x": 3.8202291563227684,
-                    "y": 2.1364935408143535,
-                    "heading": -3.292934652148666,
-                    "angularVelocity": -0.0000011539175501305985,
-                    "velocityX": 3.531690771806052,
-                    "velocityY": -1.3556525150974317,
-                    "timestamp": 1.7062049522128964
-                },
-                {
-                    "x": 3.9444450773804367,
-                    "y": 2.0888128313207726,
-                    "heading": -3.292934692734033,
-                    "angularVelocity": -0.0000011539179284682577,
-                    "velocityX": 3.5316910521455807,
-                    "velocityY": -1.355651784768035,
-                    "timestamp": 1.7413767492992254
-                },
-                {
-                    "x": 4.068661008518651,
-                    "y": 2.041132148088645,
-                    "heading": -3.292934733319412,
-                    "angularVelocity": -0.0000011539182628370223,
-                    "velocityX": 3.531691338754367,
-                    "velocityY": -1.3556510381057705,
-                    "timestamp": 1.7765485463855544
-                },
-                {
-                    "x": 4.192876950169643,
-                    "y": 1.9934514922440194,
-                    "heading": -3.2929347739048014,
-                    "angularVelocity": -0.0000011539185452860982,
-                    "velocityX": 3.5316916376522904,
-                    "velocityY": -1.3556502594278441,
-                    "timestamp": 1.8117203434718834
-                },
-                {
-                    "x": 4.317092949197956,
-                    "y": 1.945770985877502,
-                    "heading": -3.292934814490671,
-                    "angularVelocity": -0.0000011539322132251563,
-                    "velocityX": 3.5316932689969183,
-                    "velocityY": -1.3556460094855505,
-                    "timestamp": 1.8468921405582124
-                },
-                {
-                    "x": 4.4381547246220405,
-                    "y": 1.900646588966398,
-                    "heading": -3.304224160857315,
-                    "angularVelocity": -0.3209772403421673,
-                    "velocityX": 3.442012790160775,
-                    "velocityY": -1.2829710350126964,
-                    "timestamp": 1.8820639376445414
-                },
-                {
-                    "x": 4.551299156991204,
-                    "y": 1.858106592529644,
-                    "heading": -3.340115682679576,
-                    "angularVelocity": -1.0204631208967,
-                    "velocityX": 3.2169079132195413,
-                    "velocityY": -1.2094916939370357,
-                    "timestamp": 1.9172357347308704
-                },
-                {
-                    "x": 4.6559354752004225,
-                    "y": 1.8184604535185338,
-                    "heading": -3.3958506942974975,
-                    "angularVelocity": -1.5846506643126916,
-                    "velocityX": 2.9750063083893172,
-                    "velocityY": -1.127213912721008,
-                    "timestamp": 1.9524075318171994
-                },
-                {
-                    "x": 4.750941844820644,
-                    "y": 1.7823637508813854,
-                    "heading": -3.4552749718768117,
-                    "angularVelocity": -1.6895433984637673,
-                    "velocityX": 2.7012088517123067,
-                    "velocityY": -1.0262967953712765,
-                    "timestamp": 1.9875793289035284
-                },
-                {
-                    "x": 4.836288167632038,
-                    "y": 1.7498748942497964,
-                    "heading": -3.5138218394398413,
-                    "angularVelocity": -1.664596990006722,
-                    "velocityX": 2.4265556463296942,
-                    "velocityY": -0.9237189829068263,
-                    "timestamp": 2.0227511259898576
-                },
-                {
-                    "x": 4.9120167698263195,
-                    "y": 1.7210024537073532,
-                    "heading": -3.5691368807810755,
-                    "angularVelocity": -1.57271012355337,
-                    "velocityX": 2.153105853772738,
-                    "velocityY": -0.820897506930789,
-                    "timestamp": 2.0579229230761866
-                },
-                {
-                    "x": 4.9781740579406755,
-                    "y": 1.695744953740728,
-                    "heading": -3.6197442140788145,
-                    "angularVelocity": -1.438861175433386,
-                    "velocityX": 1.8809754858977845,
-                    "velocityY": -0.718117982559453,
-                    "timestamp": 2.0930947201625156
-                },
-                {
-                    "x": 5.034800882324283,
-                    "y": 1.6740995356540391,
-                    "heading": -3.6646343712421663,
-                    "angularVelocity": -1.2763111607055304,
-                    "velocityX": 1.6100065698837365,
-                    "velocityY": -0.6154197362608602,
-                    "timestamp": 2.1282665172488446
-                },
-                {
-                    "x": 5.081931555960545,
-                    "y": 1.6560639982794174,
-                    "heading": -3.703081117816373,
-                    "angularVelocity": -1.0931129415946466,
-                    "velocityX": 1.3400132361897832,
-                    "velocityY": -0.5127840732833052,
-                    "timestamp": 2.1634383143351736
-                },
-                {
-                    "x": 5.11959462557562,
-                    "y": 1.64163698634227,
-                    "heading": -3.734541789094988,
-                    "angularVelocity": -0.8944857495167222,
-                    "velocityX": 1.0708315393333863,
-                    "velocityY": -0.41018694329825167,
-                    "timestamp": 2.1986101114215026
-                },
-                {
-                    "x": 5.1478139750030305,
-                    "y": 1.6308177057465072,
-                    "heading": -3.758596048910276,
-                    "angularVelocity": -0.6839076137124058,
-                    "velocityX": 0.8023289045523005,
-                    "velocityY": -0.30761239095081433,
-                    "timestamp": 2.2337819085078316
-                },
-                {
-                    "x": 5.166609893194281,
-                    "y": 1.6236055874501054,
-                    "heading": -3.7749054122717336,
-                    "angularVelocity": -0.4637057162995195,
-                    "velocityX": 0.5344031226245449,
-                    "velocityY": -0.20505401753284183,
-                    "timestamp": 2.2689537055941607
+                    "heading": -0.1517333779044128,
+                    "angularVelocity": -0.000002553517929494838,
+                    "velocityX": 1.7881643122906952,
+                    "velocityY": -3.333631186155945,
+                    "timestamp": 1.1786038520178528
+                },
+                {
+                    "x": 2.2421653916580504,
+                    "y": 3.058317386598826,
+                    "heading": -0.1517334835913584,
+                    "angularVelocity": -0.0000030048345176781924,
+                    "velocityX": 2.041035009880172,
+                    "velocityY": -3.1850905776457417,
+                    "timestamp": 1.213776153524627
+                },
+                {
+                    "x": 2.322415894828655,
+                    "y": 2.9521882028356305,
+                    "heading": -0.15173356325163515,
+                    "angularVelocity": -0.000002264858236060329,
+                    "velocityX": 2.2816392369190814,
+                    "velocityY": -3.0174079948323826,
+                    "timestamp": 1.2489484550314012
+                },
+                {
+                    "x": 2.4106466853387967,
+                    "y": 2.8525946456824203,
+                    "heading": -0.1517336276365959,
+                    "angularVelocity": -0.0000018305586497777263,
+                    "velocityX": 2.5085304836576707,
+                    "velocityY": -2.8315905666289245,
+                    "timestamp": 1.2841207565381754
+                },
+                {
+                    "x": 2.5063274836460994,
+                    "y": 2.7601352747554455,
+                    "heading": -0.15173368243148194,
+                    "angularVelocity": -0.0000015578987915956282,
+                    "velocityX": 2.720345107040385,
+                    "velocityY": -2.628755212654101,
+                    "timestamp": 1.3192930580449496
+                },
+                {
+                    "x": 2.6088832332920924,
+                    "y": 2.67536576592715,
+                    "heading": -0.15173373101035148,
+                    "angularVelocity": -0.000001381168347773627,
+                    "velocityX": 2.915810033819959,
+                    "velocityY": -2.410121180496788,
+                    "timestamp": 1.3544653595517238
+                },
+                {
+                    "x": 2.7176975542058304,
+                    "y": 2.598795565150263,
+                    "heading": -0.1517337755722331,
+                    "angularVelocity": -0.0000012669595030854418,
+                    "velocityX": 3.0937503732242226,
+                    "velocityY": -2.1770028544233733,
+                    "timestamp": 1.389637661058498
+                },
+                {
+                    "x": 2.8321164386506434,
+                    "y": 2.5308848045360426,
+                    "heading": -0.15173381767917685,
+                    "angularVelocity": -0.0000011971620259359605,
+                    "velocityX": 3.2530963156555406,
+                    "velocityY": -1.930802299108592,
+                    "timestamp": 1.4248099625652721
+                },
+                {
+                    "x": 2.951452140465328,
+                    "y": 2.4720414272198683,
+                    "heading": -0.15173385854226368,
+                    "angularVelocity": -0.0000011617973536146762,
+                    "velocityX": 3.3928886283344455,
+                    "velocityY": -1.6730033235055968,
+                    "timestamp": 1.4599822640720463
+                },
+                {
+                    "x": 3.0749866313948626,
+                    "y": 2.4226169148084327,
+                    "heading": -0.151733899192453,
+                    "angularVelocity": -0.000001155744366153098,
+                    "velocityX": 3.512266347021429,
+                    "velocityY": -1.4052112114959712,
+                    "timestamp": 1.4951545655788205
+                },
+                {
+                    "x": 3.1992028583958065,
+                    "y": 2.3749316777596947,
+                    "heading": -0.1517339398689439,
+                    "angularVelocity": -0.0000011564921588237654,
+                    "velocityX": 3.5316491011263524,
+                    "velocityY": -1.3557610678264387,
+                    "timestamp": 1.5303268670855947
+                },
+                {
+                    "x": 3.3234191450837622,
+                    "y": 2.3272465961911335,
+                    "heading": -0.15173398054544393,
+                    "angularVelocity": -0.0000011564924185481092,
+                    "velocityX": 3.5316507981154315,
+                    "velocityY": -1.355756647297499,
+                    "timestamp": 1.565499168592369
+                },
+                {
+                    "x": 3.4476354415156054,
+                    "y": 2.2795615400047207,
+                    "heading": -0.15173402122195195,
+                    "angularVelocity": -0.0000011564926460273957,
+                    "velocityX": 3.5316510751484063,
+                    "velocityY": -1.3557559256458895,
+                    "timestamp": 1.6006714700991431
+                },
+                {
+                    "x": 3.57185174767356,
+                    "y": 2.231876509154167,
+                    "heading": -0.15173406189847174,
+                    "angularVelocity": -0.0000011564929795008858,
+                    "velocityX": 3.531651351675977,
+                    "velocityY": -1.355755205310334,
+                    "timestamp": 1.6358437716059173
+                },
+                {
+                    "x": 3.6960680635749434,
+                    "y": 2.1841915036846027,
+                    "heading": -0.15173410257500647,
+                    "angularVelocity": -0.000001156493404477995,
+                    "velocityX": 3.5316516286959314,
+                    "velocityY": -1.3557544836916666,
+                    "timestamp": 1.6710160731126915
+                },
+                {
+                    "x": 3.820284389246338,
+                    "y": 2.136506523665285,
+                    "heading": -0.15173414325155568,
+                    "angularVelocity": -0.0000011564938164652249,
+                    "velocityX": 3.5316519064716276,
+                    "velocityY": -1.3557537601039011,
+                    "timestamp": 1.7061883746194657
+                },
+                {
+                    "x": 3.9445007247793735,
+                    "y": 2.0888215693349235,
+                    "heading": -0.15173418392811813,
+                    "angularVelocity": -0.0000011564941935950735,
+                    "velocityX": 3.5316521868525257,
+                    "velocityY": -1.3557530297293134,
+                    "timestamp": 1.74136067612624
+                },
+                {
+                    "x": 4.068717070394662,
+                    "y": 2.0411366412682117,
+                    "heading": -0.1517342246046924,
+                    "angularVelocity": -0.0000011564945294151802,
+                    "velocityX": 3.531652473505735,
+                    "velocityY": -1.3557522830153055,
+                    "timestamp": 1.776532977633014
+                },
+                {
+                    "x": 4.192933426524707,
+                    "y": 1.9934517405918115,
+                    "heading": -0.15173426528127656,
+                    "angularVelocity": -0.0000011564948108738022,
+                    "velocityX": 3.5316527724556495,
+                    "velocityY": -1.3557515042686648,
+                    "timestamp": 1.8117052791397883
+                },
+                {
+                    "x": 4.3171498402859925,
+                    "y": 1.9457669900421575,
+                    "heading": -0.1517343059583456,
+                    "angularVelocity": -0.0000011565085966842365,
+                    "velocityX": 3.531654410996122,
+                    "velocityY": -1.3557472359455842,
+                    "timestamp": 1.8468775806465625
+                },
+                {
+                    "x": 4.438199866372425,
+                    "y": 1.9006463371040783,
+                    "heading": -0.16306820061092758,
+                    "angularVelocity": -0.3222392100329015,
+                    "velocityX": 3.441629375976964,
+                    "velocityY": -1.282846188765579,
+                    "timestamp": 1.8820498821533367
+                },
+                {
+                    "x": 4.551333178258229,
+                    "y": 1.8581105785728176,
+                    "heading": -0.19901052601489952,
+                    "angularVelocity": -1.0218929061849813,
+                    "velocityX": 3.2165456066050515,
+                    "velocityY": -1.2093538582645278,
+                    "timestamp": 1.9172221836601109
+                },
+                {
+                    "x": 4.6559660625940325,
+                    "y": 1.8184646011809935,
+                    "heading": -0.2548690023306322,
+                    "angularVelocity": -1.5881382202121255,
+                    "velocityX": 2.9748660125539748,
+                    "velocityY": -1.1271931518097666,
+                    "timestamp": 1.952394485166885
+                },
+                {
+                    "x": 4.750968691238499,
+                    "y": 1.7823676048635404,
+                    "heading": -0.3144227593810797,
+                    "angularVelocity": -1.693200458860429,
+                    "velocityX": 2.7010637511499986,
+                    "velocityY": -1.0262904265875403,
+                    "timestamp": 1.9875667866736593
+                },
+                {
+                    "x": 4.836310937621718,
+                    "y": 1.749878315803868,
+                    "heading": -0.37309459063768413,
+                    "angularVelocity": -1.6681260180060764,
+                    "velocityX": 2.426404947278819,
+                    "velocityY": -0.9237180300360781,
+                    "timestamp": 2.0227390881804337
+                },
+                {
+                    "x": 4.912035358987092,
+                    "y": 1.7210053742684595,
+                    "heading": -0.4285254423369764,
+                    "angularVelocity": -1.5759802266171352,
+                    "velocityX": 2.1529561081121917,
+                    "velocityY": -0.8208999780650579,
+                    "timestamp": 2.057911389687208
+                },
+                {
+                    "x": 4.978188578205751,
+                    "y": 1.6957473341798728,
+                    "heading": -0.4792367761737124,
+                    "angularVelocity": -1.4417974276425698,
+                    "velocityX": 1.8808328253957978,
+                    "velocityY": -0.71812304019178,
+                    "timestamp": 2.0930836911939825
+                },
+                {
+                    "x": 5.034811621143831,
+                    "y": 1.6741013653395114,
+                    "heading": -0.5242175587960242,
+                    "angularVelocity": -1.278869471013956,
+                    "velocityX": 1.6098759680874073,
+                    "velocityY": -0.6154265690060781,
+                    "timestamp": 2.128255992700757
+                },
+                {
+                    "x": 5.081938937252143,
+                    "y": 1.656065298518901,
+                    "heading": -0.5627406592667319,
+                    "angularVelocity": -1.0952681178195878,
+                    "velocityX": 1.3398985590759445,
+                    "velocityY": -0.5127917721601607,
+                    "timestamp": 2.1634282942075314
+                },
+                {
+                    "x": 5.11959917834615,
+                    "y": 1.6416378108353973,
+                    "heading": -0.5942629145071194,
+                    "angularVelocity": -0.8962238434785516,
+                    "velocityX": 1.0707357631047747,
+                    "velocityY": -0.41019458680362747,
+                    "timestamp": 2.1986005957143058
+                },
+                {
+                    "x": 5.147816310703936,
+                    "y": 1.63081813815951,
+                    "heading": -0.6183637008514038,
+                    "angularVelocity": -0.6852206228143011,
+                    "velocityX": 0.8022543634897267,
+                    "velocityY": -0.3076191267666608,
+                    "timestamp": 2.23377289722108
+                },
+                {
+                    "x": 5.1666106911988186,
+                    "y": 1.6236057376407542,
+                    "heading": -0.6347043261398958,
+                    "angularVelocity": -0.4645878884367218,
+                    "velocityX": 0.5343517395716112,
+                    "velocityY": -0.20505910076332604,
+                    "timestamp": 2.2689451987278546
                 },
                 {
                     "x": 5.176,
                     "y": 1.62,
-                    "heading": -3.7831853071795862,
-                    "angularVelocity": -0.2354129044793956,
-                    "velocityX": 0.2669783060180454,
-                    "velocityY": -0.10251359750698616,
-                    "timestamp": 2.3041255026804897
+                    "heading": -0.643,
+                    "angularVelocity": -0.23585814702817923,
+                    "velocityX": 0.26695178873562564,
+                    "velocityY": -0.10251639745723456,
+                    "timestamp": 2.304117500234629
                 },
                 {
                     "x": 5.176,
                     "y": 1.62,
-                    "heading": -3.7831853071795862,
-                    "angularVelocity": -9.348588807340857e-32,
-                    "velocityX": 3.3761299345246814e-33,
-                    "velocityY": 7.633245142283867e-33,
-                    "timestamp": 2.3392972997668187
+                    "heading": -0.643,
+                    "angularVelocity": -2.979902278450704e-33,
+                    "velocityX": -7.596111021745797e-35,
+                    "velocityY": -9.748244176889607e-35,
+                    "timestamp": 2.3392898017414034
                 }
             ],
             "constraints": [
@@ -10933,7 +10514,7 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -10942,7 +10523,7 @@
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": -3.143185307179586,
+                    "heading": -0.003,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -10953,200 +10534,200 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
-                    "angularVelocity": -1.8545515236340358e-32,
-                    "velocityX": 2.346645959584709e-34,
-                    "velocityY": 1.5264694726605652e-33,
+                    "heading": 0.39,
+                    "angularVelocity": 6.154725095706714e-32,
+                    "velocityX": 1.2115062621827067e-33,
+                    "velocityY": 6.540799795634554e-33,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.3666998117674303,
-                    "y": 6.765820939449683,
-                    "heading": -2.7737296132590643,
-                    "angularVelocity": -0.2635310200357621,
-                    "velocityX": 0.7407398190339184,
-                    "velocityY": 0.09796161216242502,
-                    "timestamp": 0.09004485792922705
-                },
-                {
-                    "x": 2.500184310330562,
-                    "y": 6.783480740643308,
-                    "heading": -2.817788411123353,
-                    "angularVelocity": -0.4892983217200214,
-                    "velocityX": 1.4824222241324077,
-                    "velocityY": 0.19612226172321479,
-                    "timestamp": 0.1800897158584541
-                },
-                {
-                    "x": 2.700568444033807,
-                    "y": 6.810009643084086,
-                    "heading": -2.8763096511057067,
-                    "angularVelocity": -0.6499120696969705,
-                    "velocityX": 2.2253811967890735,
-                    "velocityY": 0.294618738380691,
-                    "timestamp": 0.27013457378768113
-                },
-                {
-                    "x": 2.9679652961316036,
-                    "y": 6.845459467588075,
-                    "heading": -2.9368283914330156,
-                    "angularVelocity": -0.6720954612963594,
-                    "velocityX": 2.969596024105716,
-                    "velocityY": 0.3936907150417343,
-                    "timestamp": 0.3601794317169082
-                },
-                {
-                    "x": 3.3011724137612752,
-                    "y": 6.889825844077578,
-                    "heading": -2.9514563142993295,
-                    "angularVelocity": -0.1624515069790128,
-                    "velocityX": 3.7004569199449846,
-                    "velocityY": 0.49271415947342084,
-                    "timestamp": 0.45022428964613526
-                },
-                {
-                    "x": 3.638895175262513,
-                    "y": 6.93426783648319,
-                    "heading": -2.9514563336020103,
-                    "angularVelocity": -2.143673866142303e-7,
-                    "velocityX": 3.750605745490536,
-                    "velocityY": 0.49355391776554697,
-                    "timestamp": 0.5402691475753623
-                },
-                {
-                    "x": 3.976617946407746,
-                    "y": 6.9787097556023205,
-                    "heading": -2.9514563529046813,
-                    "angularVelocity": -2.1436727225160337e-7,
-                    "velocityX": 3.75060585259265,
-                    "velocityY": 0.4935531038769741,
-                    "timestamp": 0.6303140055045893
-                },
-                {
-                    "x": 4.314340727620448,
-                    "y": 7.023151598216748,
-                    "heading": -2.9514563722073515,
-                    "angularVelocity": -2.1436726735785024e-7,
-                    "velocityX": 3.7506059643976966,
-                    "velocityY": 0.49355225424817656,
-                    "timestamp": 0.7203588634338163
-                },
-                {
-                    "x": 4.65206351967383,
-                    "y": 7.06759335845053,
-                    "heading": -2.951456391510026,
-                    "angularVelocity": -2.1436731957684306e-7,
-                    "velocityX": 3.7506060847896903,
-                    "velocityY": 0.49355133936368123,
-                    "timestamp": 0.8104037213630433
-                },
-                {
-                    "x": 4.989786323250196,
-                    "y": 7.11203503111852,
-                    "heading": -2.9514564108127086,
-                    "angularVelocity": -2.143674043301644e-7,
-                    "velocityX": 3.7506062127590543,
-                    "velocityY": 0.4935503668951324,
-                    "timestamp": 0.9004485792922703
-                },
-                {
-                    "x": 5.327509138668733,
-                    "y": 7.15647661379494,
-                    "heading": -2.951456430115396,
-                    "angularVelocity": -2.1436746122645403e-7,
-                    "velocityX": 3.7506063442731943,
-                    "velocityY": 0.4935493674869138,
-                    "timestamp": 0.9904934372214973
-                },
-                {
-                    "x": 5.66523196589638,
-                    "y": 7.200918106730859,
-                    "heading": -2.95145644941808,
-                    "angularVelocity": -2.143674220103917e-7,
-                    "velocityX": 3.7506064754201436,
-                    "velocityY": 0.49354837086698544,
-                    "timestamp": 1.0805382951507243
-                },
-                {
-                    "x": 6.002954804577347,
-                    "y": 7.24535951262982,
-                    "heading": -2.9514564687207536,
-                    "angularVelocity": -2.1436730521441728e-7,
-                    "velocityX": 3.7506066026158704,
-                    "velocityY": 0.4935474042714428,
-                    "timestamp": 1.1705831530799513
-                },
-                {
-                    "x": 6.340677653913631,
-                    "y": 7.289800837555904,
-                    "heading": -2.95145648802342,
-                    "angularVelocity": -2.1436722572774826e-7,
-                    "velocityX": 3.750606720949308,
-                    "velocityY": 0.49354650502102115,
-                    "timestamp": 1.2606280110091783
-                },
-                {
-                    "x": 6.678400512367262,
-                    "y": 7.334242093196452,
-                    "heading": -2.9514565073260908,
-                    "angularVelocity": -2.1436727271528376e-7,
-                    "velocityX": 3.750606822202691,
-                    "velocityY": 0.4935457355652348,
-                    "timestamp": 1.3506728689384053
-                },
-                {
-                    "x": 7.0118090549698895,
-                    "y": 7.378580230758345,
-                    "heading": -2.965622818302132,
-                    "angularVelocity": -0.15732504111646,
-                    "velocityX": 3.7026938602610375,
-                    "velocityY": 0.4924005499208098,
-                    "timestamp": 1.4407177268676323
-                },
-                {
-                    "x": 7.2792751287326976,
-                    "y": 7.414016497420869,
-                    "heading": -3.0230864740546695,
-                    "angularVelocity": -0.6381669878107006,
-                    "velocityX": 2.9703647705572496,
-                    "velocityY": 0.39354014740493215,
-                    "timestamp": 1.5307625847968593
-                },
-                {
-                    "x": 7.479727754094387,
-                    "y": 7.440534879064414,
-                    "heading": -3.078696937244319,
-                    "angularVelocity": -0.617586217231393,
-                    "velocityX": 2.2261418360973004,
-                    "velocityY": 0.2945018988689779,
-                    "timestamp": 1.6208074427260863
-                },
-                {
-                    "x": 7.613268208977269,
-                    "y": 7.458185411749662,
-                    "heading": -3.120596791778004,
-                    "angularVelocity": -0.4653220127974119,
-                    "velocityX": 1.48304365128591,
-                    "velocityY": 0.1960193296003781,
-                    "timestamp": 1.7108523006553134
+                    "x": 2.3666999539229265,
+                    "y": 6.765820977804648,
+                    "heading": 0.36628259128993745,
+                    "angularVelocity": -0.26339586361920003,
+                    "velocityX": 0.7407424724032508,
+                    "velocityY": 0.09796218023747214,
+                    "timestamp": 0.09004472729439511
+                },
+                {
+                    "x": 2.5001846245776895,
+                    "y": 6.783480891306791,
+                    "heading": 0.3222458902940237,
+                    "angularVelocity": -0.4890536327789505,
+                    "velocityX": 1.4824262859761226,
+                    "velocityY": 0.1961237935054848,
+                    "timestamp": 0.18008945458879022
+                },
+                {
+                    "x": 2.70056880915549,
+                    "y": 6.810010034997764,
+                    "heading": 0.26375227569196347,
+                    "angularVelocity": -0.6496062163731077,
+                    "velocityX": 2.225384990313294,
+                    "velocityY": 0.2946218450330504,
+                    "timestamp": 0.27013418188318533
+                },
+                {
+                    "x": 2.967965472322839,
+                    "y": 6.8454603445891635,
+                    "heading": 0.20325713210858826,
+                    "angularVelocity": -0.6718343805472418,
+                    "velocityX": 2.9695982341432847,
+                    "velocityY": 0.3936966733820705,
+                    "timestamp": 0.36017890917758044
+                },
+                {
+                    "x": 3.3011740475809854,
+                    "y": 6.889827568786931,
+                    "heading": 0.1886164627156386,
+                    "angularVelocity": -0.16259330038374148,
+                    "velocityX": 3.700478476310374,
+                    "velocityY": 0.4927242885939573,
+                    "timestamp": 0.45022363647197555
+                },
+                {
+                    "x": 3.6388963576772766,
+                    "y": 6.934269203744266,
+                    "heading": 0.18861644342798295,
+                    "angularVelocity": -2.1420083336262225e-7,
+                    "velocityX": 3.7506061736644574,
+                    "velocityY": 0.4935506641275844,
+                    "timestamp": 0.5402683637663707
+                },
+                {
+                    "x": 3.9766186774172247,
+                    "y": 6.978710765417191,
+                    "heading": 0.1886164241403376,
+                    "angularVelocity": -2.1420071941558326e-7,
+                    "velocityX": 3.750606280762983,
+                    "velocityY": 0.4935498502608198,
+                    "timestamp": 0.6303130910607657
+                },
+                {
+                    "x": 4.314341007224031,
+                    "y": 7.023152250589549,
+                    "heading": 0.18861640485269274,
+                    "angularVelocity": -2.1420071403103001e-7,
+                    "velocityX": 3.750606392561398,
+                    "velocityY": 0.4935490006767261,
+                    "timestamp": 0.7203578183551609
+                },
+                {
+                    "x": 4.652063347871557,
+                    "y": 7.067593653380398,
+                    "heading": 0.1886163855650432,
+                    "angularVelocity": -2.1420076555553473e-7,
+                    "velocityX": 3.750606512954019,
+                    "velocityY": 0.49354808578132814,
+                    "timestamp": 0.810402545649556
+                },
+                {
+                    "x": 4.989785700043158,
+                    "y": 7.112034968596579,
+                    "heading": 0.18861636627738598,
+                    "angularVelocity": -2.1420085095836123e-7,
+                    "velocityX": 3.7506066409356835,
+                    "velocityY": 0.4935471132127838,
+                    "timestamp": 0.9004472729439512
+                },
+                {
+                    "x": 5.327508064058316,
+                    "y": 7.15647619381006,
+                    "heading": 0.18861634698972354,
+                    "angularVelocity": -2.1420090940402864e-7,
+                    "velocityX": 3.7506067724653955,
+                    "velocityY": 0.4935461136795145,
+                    "timestamp": 0.9904920002383464
+                },
+                {
+                    "x": 5.6652304398823485,
+                    "y": 7.200917329284216,
+                    "heading": 0.18861632770206457,
+                    "angularVelocity": -2.1420087020950116e-7,
+                    "velocityX": 3.750606903609938,
+                    "velocityY": 0.4935451170711973,
+                    "timestamp": 1.0805367275327415
+                },
+                {
+                    "x": 6.002952827157885,
+                    "y": 7.245358377734637,
+                    "heading": 0.18861630841441618,
+                    "angularVelocity": -2.1420075320811155e-7,
+                    "velocityX": 3.750607030785673,
+                    "velocityY": 0.49354415062109597,
+                    "timestamp": 1.1705814548271367
+                },
+                {
+                    "x": 6.340675225087303,
+                    "y": 7.289799345222555,
+                    "heading": 0.1886162891267749,
+                    "angularVelocity": -2.1420067405327174e-7,
+                    "velocityX": 3.750607149103328,
+                    "velocityY": 0.4935432514845822,
+                    "timestamp": 1.2606261821215319
+                },
+                {
+                    "x": 6.678397632133336,
+                    "y": 7.334240243430023,
+                    "heading": 0.18861626983912938,
+                    "angularVelocity": -2.1420072105835628e-7,
+                    "velocityX": 3.7506072503487453,
+                    "velocityY": 0.4935424820841753,
+                    "timestamp": 1.350670909415927
+                },
+                {
+                    "x": 7.011808183476814,
+                    "y": 7.378579327724673,
+                    "heading": 0.17443906284640512,
+                    "angularVelocity": -0.15744627607536488,
+                    "velocityX": 3.7027215403008924,
+                    "velocityY": 0.49241177831197624,
+                    "timestamp": 1.4407156367103222
+                },
+                {
+                    "x": 7.279274281127278,
+                    "y": 7.414016049929063,
+                    "heading": 0.11701095642225132,
+                    "angularVelocity": -0.637773117313093,
+                    "velocityX": 2.9703693451811146,
+                    "velocityY": 0.3935457774060549,
+                    "timestamp": 1.5307603640047174
+                },
+                {
+                    "x": 7.479727163444043,
+                    "y": 7.440534681549159,
+                    "heading": 0.061439635089441606,
+                    "angularVelocity": -0.6171524197205129,
+                    "velocityX": 2.226147919371209,
+                    "velocityY": 0.294505102263186,
+                    "timestamp": 1.6208050912991125
+                },
+                {
+                    "x": 7.613267962945412,
+                    "y": 7.458185352165826,
+                    "heading": 0.019571051398075176,
+                    "angularVelocity": -0.46497540666073534,
+                    "velocityX": 1.4830496300440337,
+                    "velocityY": 0.19602114579078314,
+                    "timestamp": 1.7108498185935077
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -0.2508584712225979,
-                    "velocityX": 0.7410949670794122,
-                    "velocityY": 0.09789107843632346,
-                    "timestamp": 1.8008971585845404
+                    "heading": -0.003,
+                    "angularVelocity": -0.2506648870652985,
+                    "velocityX": 0.7410987745724689,
+                    "velocityY": 0.09789188216823574,
+                    "timestamp": 1.8008945458879029
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": 4.6252031231996675e-32,
+                    "heading": -0.003,
+                    "angularVelocity": 5.335131961970383e-32,
                     "velocityX": 0,
-                    "velocityY": -8.558401294468068e-35,
-                    "timestamp": 1.8909420165137674
+                    "velocityY": -3.2330177098423496e-35,
+                    "timestamp": 1.890939273182298
                 }
             ],
             "constraints": [
@@ -11189,7 +10770,7 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -11198,7 +10779,7 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": -3.143185307179586,
+                    "heading": -0.003,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -11209,200 +10790,200 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
-                    "angularVelocity": 1.5124244158252309e-30,
-                    "velocityX": -1.0784945790349375e-32,
-                    "velocityY": 7.7332582255010576e-31,
+                    "heading": 0.39,
+                    "angularVelocity": 5.971193698047002e-30,
+                    "velocityX": 3.860312986446214e-32,
+                    "velocityY": -2.276118500298611e-31,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.368718302927459,
-                    "y": 6.754046521321464,
-                    "heading": -2.805815501863529,
-                    "angularVelocity": -0.6069900647837034,
-                    "velocityX": 0.7473072131063252,
-                    "velocityY": -0.0321188944749646,
-                    "timestamp": 0.09195455593398905
-                },
-                {
-                    "x": 2.506490387717156,
-                    "y": 6.748005444139933,
-                    "heading": -2.9118057011244183,
-                    "angularVelocity": -1.1526367365307737,
-                    "velocityX": 1.498262738483551,
-                    "velocityY": -0.06569633358752333,
-                    "timestamp": 0.1839091118679781
-                },
-                {
-                    "x": 2.7138594941009426,
-                    "y": 6.738708328412372,
-                    "heading": -3.056707583490325,
-                    "angularVelocity": -1.5757988377424839,
-                    "velocityX": 2.255125961704928,
-                    "velocityY": -0.10110554755149347,
-                    "timestamp": 0.27586366780196714
-                },
-                {
-                    "x": 2.99155716708356,
-                    "y": 6.725962728573371,
-                    "heading": -3.2144148322825616,
-                    "angularVelocity": -1.7150563905223961,
-                    "velocityX": 3.019944690744492,
-                    "velocityY": -0.13860759490970812,
-                    "timestamp": 0.3678182237359562
-                },
-                {
-                    "x": 3.331057359197656,
-                    "y": 6.709255401861718,
-                    "heading": -3.240411366891808,
-                    "angularVelocity": -0.28271067534607036,
-                    "velocityX": 3.6920431909628384,
-                    "velocityY": -0.1816911249470588,
-                    "timestamp": 0.45977277966994523
-                },
-                {
-                    "x": 3.6785771644429963,
-                    "y": 6.69390611896786,
-                    "heading": -3.2404114102003327,
-                    "angularVelocity": -4.7097747760964064e-7,
-                    "velocityX": 3.7792559782988104,
-                    "velocityY": -0.1669224840243556,
-                    "timestamp": 0.5517273356039343
-                },
-                {
-                    "x": 4.026096967074563,
-                    "y": 6.678556776895953,
-                    "heading": -3.2404114535088273,
-                    "angularVelocity": -4.70977144522105e-7,
-                    "velocityX": 3.779255949874186,
-                    "velocityY": -0.1669231275818996,
-                    "timestamp": 0.6436818915379233
-                },
-                {
-                    "x": 4.373616766444988,
-                    "y": 6.663207360989526,
-                    "heading": -3.240411496817302,
-                    "angularVelocity": -4.7097693251045375e-7,
-                    "velocityX": 3.779255914409469,
-                    "velocityY": -0.16692393052765972,
-                    "timestamp": 0.7356364474719124
-                },
-                {
-                    "x": 4.721136562002901,
-                    "y": 6.647857858765673,
-                    "heading": -3.240411540125758,
-                    "angularVelocity": -4.709767302327839e-7,
-                    "velocityX": 3.779255872948645,
-                    "velocityY": -0.1669248692242324,
-                    "timestamp": 0.8275910034059014
-                },
-                {
-                    "x": 5.068656352361164,
-                    "y": 6.632508238819668,
-                    "heading": -3.2404115834342044,
-                    "angularVelocity": -4.709766303037568e-7,
-                    "velocityX": 3.7792558164027836,
-                    "velocityY": -0.166926149445212,
-                    "timestamp": 0.9195455593398905
-                },
-                {
-                    "x": 5.4161761153722905,
-                    "y": 6.617157999739619,
-                    "heading": -3.2404116267428487,
-                    "angularVelocity": -4.709787784210011e-7,
-                    "velocityX": 3.7792555190043995,
-                    "velocityY": -0.16693288248891244,
-                    "timestamp": 1.0115001152738796
-                },
-                {
-                    "x": 5.762341737151603,
-                    "y": 6.582880068913748,
-                    "heading": -3.240411680913547,
-                    "angularVelocity": -5.891029283022056e-7,
-                    "velocityX": 3.7645293184582718,
-                    "velocityY": -0.37277033723568115,
-                    "timestamp": 1.1034546712078686
-                },
-                {
-                    "x": 6.108499177819837,
-                    "y": 6.548519619057327,
-                    "heading": -3.2404117350781716,
-                    "angularVelocity": -5.89036882514826e-7,
-                    "velocityX": 3.7644403493909295,
-                    "velocityY": -0.37366772649185126,
-                    "timestamp": 1.1954092271418575
-                },
-                {
-                    "x": 6.443025175700078,
-                    "y": 6.453136314569308,
-                    "heading": -3.2404117521954174,
-                    "angularVelocity": -1.8614896803626685e-7,
-                    "velocityX": 3.637949142186984,
-                    "velocityY": -1.0372874244152857,
-                    "timestamp": 1.2873637830758464
-                },
-                {
-                    "x": 6.751449154461275,
-                    "y": 6.292262864409884,
-                    "heading": -3.240411747251824,
-                    "angularVelocity": 5.3761265862029046e-8,
-                    "velocityX": 3.3540913294454215,
-                    "velocityY": -1.7494886308287982,
-                    "timestamp": 1.3793183390098354
-                },
-                {
-                    "x": 7.05837809298162,
-                    "y": 6.128555062623798,
-                    "heading": -3.240411742317762,
-                    "angularVelocity": 5.3657614685044897e-8,
-                    "velocityX": 3.337832861056693,
-                    "velocityY": -1.780312026123053,
-                    "timestamp": 1.4712728949438243
-                },
-                {
-                    "x": 7.307026842216431,
-                    "y": 5.995933066242413,
-                    "heading": -3.201520178002944,
-                    "angularVelocity": 0.422943310636366,
-                    "velocityX": 2.7040394759048985,
-                    "velocityY": -1.44225585164687,
-                    "timestamp": 1.5632274508778132
-                },
-                {
-                    "x": 7.493513419749655,
-                    "y": 5.8964665430996925,
-                    "heading": -3.172352554188821,
-                    "angularVelocity": 0.3171960705792624,
-                    "velocityX": 2.0280297766550737,
-                    "velocityY": -1.0816921699249264,
-                    "timestamp": 1.6551820068118022
-                },
-                {
-                    "x": 7.617837807165489,
-                    "y": 5.830155516567485,
-                    "heading": -3.1529077720147476,
-                    "angularVelocity": 0.21146078056243686,
-                    "velocityX": 1.3520198771345535,
-                    "velocityY": -0.721128234035621,
-                    "timestamp": 1.747136562745791
+                    "x": 2.3687225561485863,
+                    "y": 6.754047320431167,
+                    "heading": 0.3342984160631402,
+                    "angularVelocity": -0.6057639861694455,
+                    "velocityX": 0.7473692238179469,
+                    "velocityY": -0.03211088121883033,
+                    "timestamp": 0.09195261720507575
+                },
+                {
+                    "x": 2.5065025886783423,
+                    "y": 6.748008642736996,
+                    "heading": 0.22854226308747308,
+                    "angularVelocity": -1.1501157464589211,
+                    "velocityX": 1.498380760848541,
+                    "velocityY": -0.06567162390498478,
+                    "timestamp": 0.1839052344101515
+                },
+                {
+                    "x": 2.713881673600013,
+                    "y": 6.738717029343825,
+                    "heading": 0.08399021423974613,
+                    "angularVelocity": -1.5720275642110566,
+                    "velocityX": 2.2552820270375413,
+                    "velocityY": -0.10104784045947955,
+                    "timestamp": 0.27585785161522725
+                },
+                {
+                    "x": 2.991587720371644,
+                    "y": 6.725981822834066,
+                    "heading": -0.07328714089763647,
+                    "angularVelocity": -1.710417385800096,
+                    "velocityX": 3.0200994296038606,
+                    "velocityY": -0.13849748812866708,
+                    "timestamp": 0.367810468820303
+                },
+                {
+                    "x": 3.331126465665418,
+                    "y": 6.709290658495849,
+                    "heading": -0.09912948961150339,
+                    "angularVelocity": -0.2810398387707929,
+                    "velocityX": 3.692540306237542,
+                    "velocityY": -0.18151918722434224,
+                    "timestamp": 0.45976308602537874
+                },
+                {
+                    "x": 3.678639360618228,
+                    "y": 6.693951135251218,
+                    "heading": -0.09912953281628478,
+                    "angularVelocity": -4.6985918076152147e-7,
+                    "velocityX": 3.7792605095489114,
+                    "velocityY": -0.16681986560992548,
+                    "timestamp": 0.5517157032304545
+                },
+                {
+                    "x": 4.0261522529573766,
+                    "y": 6.678611552794569,
+                    "heading": -0.09912957602103574,
+                    "angularVelocity": -4.698588497287581e-7,
+                    "velocityX": 3.77926048112489,
+                    "velocityY": -0.16682050955045719,
+                    "timestamp": 0.6436683204355302
+                },
+                {
+                    "x": 4.373665142037621,
+                    "y": 6.663271896508641,
+                    "heading": -0.0991296192257673,
+                    "angularVelocity": -4.698586389884745e-7,
+                    "velocityX": 3.779260445683786,
+                    "velocityY": -0.1668213124561534,
+                    "timestamp": 0.735620937640606
+                },
+                {
+                    "x": 4.721178027308032,
+                    "y": 6.647932153912761,
+                    "heading": -0.09912966243048027,
+                    "angularVelocity": -4.6985843677462385e-7,
+                    "velocityX": 3.779260404251215,
+                    "velocityY": -0.1668222510912166,
+                    "timestamp": 0.8275735548456817
+                },
+                {
+                    "x": 5.068690907382649,
+                    "y": 6.632592293609503,
+                    "heading": -0.09912970563518403,
+                    "angularVelocity": -4.698583364707967e-7,
+                    "velocityX": 3.7792603477460833,
+                    "velocityY": -0.16682353117852486,
+                    "timestamp": 0.9195261720507575
+                },
+                {
+                    "x": 5.416203760083376,
+                    "y": 6.617251813184553,
+                    "heading": -0.09912974884008513,
+                    "angularVelocity": -4.698604826805718e-7,
+                    "velocityX": 3.779260050050473,
+                    "velocityY": -0.16683027510502133,
+                    "timestamp": 1.0114787892558332
+                },
+                {
+                    "x": 5.762351413678364,
+                    "y": 6.582867023594091,
+                    "heading": -0.09912980297819443,
+                    "angularVelocity": -5.887609395849737e-7,
+                    "velocityX": 3.7644132827997527,
+                    "velocityY": -0.3739403035563012,
+                    "timestamp": 1.103431406460909
+                },
+                {
+                    "x": 6.108490861918874,
+                    "y": 6.5483997316058735,
+                    "heading": -0.0991298571102857,
+                    "angularVelocity": -5.886954924717416e-7,
+                    "velocityX": 3.7643240482055917,
+                    "velocityY": -0.3748375308486051,
+                    "timestamp": 1.1953840236659847
+                },
+                {
+                    "x": 6.4430213978276925,
+                    "y": 6.453059099313297,
+                    "heading": -0.09912987433352956,
+                    "angularVelocity": -1.8730564053245824e-7,
+                    "velocityX": 3.6380751965192784,
+                    "velocityY": -1.0368452273625233,
+                    "timestamp": 1.2873366408710605
+                },
+                {
+                    "x": 6.751459095644277,
+                    "y": 6.29222781566083,
+                    "heading": -0.09912986944515287,
+                    "angularVelocity": 5.316190922150303e-8,
+                    "velocityX": 3.3543112441127847,
+                    "velocityY": -1.7490669492721016,
+                    "timestamp": 1.3792892580761362
+                },
+                {
+                    "x": 7.058385657911462,
+                    "y": 6.1285311430110365,
+                    "heading": -0.09912986456630993,
+                    "angularVelocity": 5.305822810376802e-8,
+                    "velocityX": 3.337877393774089,
+                    "velocityY": -1.7802285310130133,
+                    "timestamp": 1.471241875281212
+                },
+                {
+                    "x": 7.307031381410103,
+                    "y": 5.995918713976455,
+                    "heading": -0.06067697672181489,
+                    "angularVelocity": 0.4181815484244036,
+                    "velocityX": 2.7040635824873065,
+                    "velocityY": -1.442182213681043,
+                    "timestamp": 1.5631944924862877
+                },
+                {
+                    "x": 7.4935156893705,
+                    "y": 5.896459366852187,
+                    "heading": -0.03183830930275791,
+                    "angularVelocity": 0.3136253028528817,
+                    "velocityX": 2.028047853651562,
+                    "velocityY": -1.0816369359281015,
+                    "timestamp": 1.6551471096913635
+                },
+                {
+                    "x": 7.617838563683624,
+                    "y": 5.830153124497745,
+                    "heading": -0.012612817121248107,
+                    "angularVelocity": 0.2090804238734443,
+                    "velocityX": 1.3520319278771131,
+                    "velocityY": -0.7210914095741754,
+                    "timestamp": 1.7470997268964392
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": 0.10573119228742708,
-                    "velocityX": 0.6760099290690327,
-                    "velocityY": -0.3605641529201271,
-                    "timestamp": 1.83909111867978
+                    "heading": -0.003,
+                    "angularVelocity": 0.10454098440514517,
+                    "velocityX": 0.6760159548013833,
+                    "velocityY": -0.36054574089833413,
+                    "timestamp": 1.839052344101515
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": 3.5257869003902484e-31,
+                    "heading": -0.003,
+                    "angularVelocity": -2.423215560944852e-29,
                     "velocityX": 0,
-                    "velocityY": -2.4831219594454356e-32,
-                    "timestamp": 1.931045674613769
+                    "velocityY": -2.0134519400533763e-31,
+                    "timestamp": 1.9310049613065907
                 }
             ],
             "constraints": [
@@ -11445,7 +11026,7 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -11454,7 +11035,7 @@
                 {
                     "x": 4.748738765716553,
                     "y": 4.2041850090026855,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -11463,537 +11044,14 @@
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": -3.143185307179586,
+                    "heading": -0.003,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": -2.75,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -1.6129974497596655e-31,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.3086679912938655,
-                    "y": 6.746243793862482,
-                    "heading": -2.736085320412874,
-                    "angularVelocity": 0.33564007387910216,
-                    "velocityX": 0.20908316429709453,
-                    "velocityY": -0.25945360797208683,
-                    "timestamp": 0.04145714612176501
-                },
-                {
-                    "x": 2.3260411538964876,
-                    "y": 6.724713219688379,
-                    "heading": -2.708761093447192,
-                    "angularVelocity": 0.6590957053683163,
-                    "velocityX": 0.4190631586553287,
-                    "velocityY": -0.5193453044467995,
-                    "timestamp": 0.08291429224353002
-                },
-                {
-                    "x": 2.352163012722991,
-                    "y": 6.692386750887991,
-                    "heading": -2.6686499617636756,
-                    "angularVelocity": 0.9675323903315748,
-                    "velocityX": 0.6300930302770934,
-                    "velocityY": -0.7797562501152615,
-                    "timestamp": 0.12437143836529502
-                },
-                {
-                    "x": 2.387085245084356,
-                    "y": 6.649236203863692,
-                    "heading": -2.6165769378043913,
-                    "angularVelocity": 1.2560687078251613,
-                    "velocityX": 0.8423694254976876,
-                    "velocityY": -1.0408470206212592,
-                    "timestamp": 0.16582858448706003
-                },
-                {
-                    "x": 2.4308688712635975,
-                    "y": 6.595222928663028,
-                    "heading": -2.553684267520584,
-                    "angularVelocity": 1.5170525751841024,
-                    "velocityX": 1.056117708890124,
-                    "velocityY": -1.3028700779841438,
-                    "timestamp": 0.20728573060882505
-                },
-                {
-                    "x": 2.4835846779423907,
-                    "y": 6.530294843671354,
-                    "heading": -2.481592483057959,
-                    "angularVelocity": 1.7389471106110828,
-                    "velocityX": 1.271573458625441,
-                    "velocityY": -1.5661494112733225,
-                    "timestamp": 0.24874287673059006
-                },
-                {
-                    "x": 2.5453132269093315,
-                    "y": 6.454388220972069,
-                    "heading": -2.4026634058172087,
-                    "angularVelocity": 1.9038714582264231,
-                    "velocityX": 1.4889724629292092,
-                    "velocityY": -1.830965944359449,
-                    "timestamp": 0.2902000228523551
-                },
-                {
-                    "x": 2.6161417178055544,
-                    "y": 6.3674459995028085,
-                    "heading": -2.3205870553837444,
-                    "angularVelocity": 1.9797877594467097,
-                    "velocityX": 1.708474835392443,
-                    "velocityY": -2.0971588640930596,
-                    "timestamp": 0.33165716897412006
-                },
-                {
-                    "x": 2.696110411470073,
-                    "y": 6.269518903939401,
-                    "heading": -2.2423584912738157,
-                    "angularVelocity": 1.8869741752160536,
-                    "velocityX": 1.9289483513804893,
-                    "velocityY": -2.3621282390202,
-                    "timestamp": 0.37311431509588505
-                },
-                {
-                    "x": 2.7844874173560816,
-                    "y": 6.1625113811609245,
-                    "heading": -2.1922364761983766,
-                    "angularVelocity": 1.2090078494121173,
-                    "velocityX": 2.131767720489823,
-                    "velocityY": -2.581159891329275,
-                    "timestamp": 0.41457146121765004
-                },
-                {
-                    "x": 2.8809289045046786,
-                    "y": 6.046946295339885,
-                    "heading": -2.1746156662329965,
-                    "angularVelocity": 0.42503673344096826,
-                    "velocityX": 2.3262934420361696,
-                    "velocityY": -2.7875793833374174,
-                    "timestamp": 0.456028607339415
-                },
-                {
-                    "x": 2.9795919491542358,
-                    "y": 5.925039697257722,
-                    "heading": -2.1746156143142836,
-                    "angularVelocity": 0.000001252346527187571,
-                    "velocityX": 2.3798802831186356,
-                    "velocityY": -2.940544863462319,
-                    "timestamp": 0.49748575346118
-                },
-                {
-                    "x": 3.0782548699283607,
-                    "y": 5.803132998918896,
-                    "heading": -2.174615562396172,
-                    "angularVelocity": 0.0000012523320226734394,
-                    "velocityX": 2.37987729508295,
-                    "velocityY": -2.940547281782763,
-                    "timestamp": 0.5389428995829451
-                },
-                {
-                    "x": 3.1769177990519513,
-                    "y": 5.6812263073375435,
-                    "heading": -2.1746155104780764,
-                    "angularVelocity": 0.0000012523316371956637,
-                    "velocityX": 2.379877496482862,
-                    "velocityY": -2.940547118783714,
-                    "timestamp": 0.58040004570471
-                },
-                {
-                    "x": 3.2755807374460058,
-                    "y": 5.559319623259062,
-                    "heading": -2.174615458559997,
-                    "angularVelocity": 0.0000012523312531723852,
-                    "velocityX": 2.3798777200984556,
-                    "velocityY": -2.9405469378047924,
-                    "timestamp": 0.621857191826475
-                },
-                {
-                    "x": 3.374243686147731,
-                    "y": 5.437412947522895,
-                    "heading": -2.1746154066419328,
-                    "angularVelocity": 0.0000012523308771614933,
-                    "velocityX": 2.379877968732799,
-                    "velocityY": -2.9405467365773754,
-                    "timestamp": 0.66331433794824
-                },
-                {
-                    "x": 3.472906646327713,
-                    "y": 5.315506281076437,
-                    "heading": -2.174615354723884,
-                    "angularVelocity": 0.000001252330513608687,
-                    "velocityX": 2.3798782456032144,
-                    "velocityY": -2.9405465124975687,
-                    "timestamp": 0.704771484070005
-                },
-                {
-                    "x": 3.5715696192957083,
-                    "y": 5.193599624979717,
-                    "heading": -2.1746153028058486,
-                    "angularVelocity": 0.0000012523301754013102,
-                    "velocityX": 2.379878554066626,
-                    "velocityY": -2.940546262848488,
-                    "timestamp": 0.74622863019177
-                },
-                {
-                    "x": 3.6702326064938156,
-                    "y": 5.071692980399876,
-                    "heading": -2.174615250887826,
-                    "angularVelocity": 0.0000012523298755846902,
-                    "velocityX": 2.3798788973153537,
-                    "velocityY": -2.940545985046467,
-                    "timestamp": 0.787685776313535
-                },
-                {
-                    "x": 3.7688956094655266,
-                    "y": 4.949786348586114,
-                    "heading": -2.1746151989698133,
-                    "angularVelocity": 0.0000012523296313000306,
-                    "velocityX": 2.379879277795066,
-                    "velocityY": -2.940545677112118,
-                    "timestamp": 0.8291429224353
-                },
-                {
-                    "x": 3.8675586330203298,
-                    "y": 4.82787973343092,
-                    "heading": -2.1746151470518074,
-                    "angularVelocity": 0.0000012523294719911187,
-                    "velocityX": 2.379879774285891,
-                    "velocityY": -2.940545275285949,
-                    "timestamp": 0.870600068557065
-                },
-                {
-                    "x": 3.9662940139338945,
-                    "y": 4.706031714187132,
-                    "heading": -2.1746150951231753,
-                    "angularVelocity": 0.0000012525857863060412,
-                    "velocityX": 2.381625127392215,
-                    "velocityY": -2.939131866093794,
-                    "timestamp": 0.9120572146788299
-                },
-                {
-                    "x": 4.07569184834889,
-                    "y": 4.593658547130379,
-                    "heading": -2.174615041084707,
-                    "angularVelocity": 0.0000013034777659582994,
-                    "velocityX": 2.638817300488551,
-                    "velocityY": -2.7105861731702134,
-                    "timestamp": 0.9535143608005949
-                },
-                {
-                    "x": 4.1948903308489776,
-                    "y": 4.491740265556163,
-                    "heading": -2.17461498286675,
-                    "angularVelocity": 0.0000014042924363783103,
-                    "velocityX": 2.8752216119746037,
-                    "velocityY": -2.458400809232485,
-                    "timestamp": 0.9949715069223599
-                },
-                {
-                    "x": 4.322894391111494,
-                    "y": 4.401128047699223,
-                    "heading": -2.1746149179117107,
-                    "angularVelocity": 0.00000156679957607106,
-                    "velocityX": 3.087623539897125,
-                    "velocityY": -2.1856839250535947,
-                    "timestamp": 1.036428653044125
-                },
-                {
-                    "x": 4.458635270784365,
-                    "y": 4.322578536823241,
-                    "heading": -2.1746148427021956,
-                    "angularVelocity": 0.0000018141508159566887,
-                    "velocityX": 3.2742456336522188,
-                    "velocityY": -1.8947158264409445,
-                    "timestamp": 1.07788579916589
-                },
-                {
-                    "x": 4.600979576445416,
-                    "y": 4.2567476280747965,
-                    "heading": -2.174614751821203,
-                    "angularVelocity": 0.000002192167120840163,
-                    "velocityX": 3.433528811726906,
-                    "velocityY": -1.5879266883226886,
-                    "timestamp": 1.1193429452876549
-                },
-                {
-                    "x": 4.748738765716553,
-                    "y": 4.2041850090026855,
-                    "heading": -2.174614640016373,
-                    "angularVelocity": 0.000002696877156656919,
-                    "velocityX": 3.564142809954843,
-                    "velocityY": -1.2678783753644691,
-                    "timestamp": 1.1608000914094199
-                },
-                {
-                    "x": 4.880414686886391,
-                    "y": 4.168830160718546,
-                    "heading": -2.174614553815122,
-                    "angularVelocity": 0.0000023917765874926725,
-                    "velocityX": 3.65353612769536,
-                    "velocityY": -0.9809706615128672,
-                    "timestamp": 1.1968407699651018
-                },
-                {
-                    "x": 5.014481469919241,
-                    "y": 4.144038758633067,
-                    "heading": -2.1746144885447847,
-                    "angularVelocity": 0.0000018110185423275369,
-                    "velocityX": 3.7198739980913533,
-                    "velocityY": -0.6878727892755006,
-                    "timestamp": 1.2328814485207837
-                },
-                {
-                    "x": 5.150093083338604,
-                    "y": 4.129967234831094,
-                    "heading": -2.1746144356224972,
-                    "angularVelocity": 0.0000014684043018781928,
-                    "velocityX": 3.7627375192130574,
-                    "velocityY": -0.3904344858611157,
-                    "timestamp": 1.2689221270764657
-                },
-                {
-                    "x": 5.286393751264718,
-                    "y": 4.1267043465211,
-                    "heading": -2.1746143904583044,
-                    "angularVelocity": 0.000001253144907070333,
-                    "velocityX": 3.7818563187020433,
-                    "velocityY": -0.09053348718039304,
-                    "timestamp": 1.3049628056321476
-                },
-                {
-                    "x": 5.422523381587792,
-                    "y": 4.134270240486654,
-                    "heading": -2.1746143488300143,
-                    "angularVelocity": 0.0000011550362392666718,
-                    "velocityX": 3.7771106366034917,
-                    "velocityY": 0.20992651272823276,
-                    "timestamp": 1.3410034841878296
-                },
-                {
-                    "x": 5.558824040120251,
-                    "y": 4.131006987177355,
-                    "heading": -2.1746143024581284,
-                    "angularVelocity": 0.0000012866540842201377,
-                    "velocityX": 3.7818560580616727,
-                    "velocityY": -0.09054361460641977,
-                    "timestamp": 1.3770441627435115
-                },
-                {
-                    "x": 5.69443561806916,
-                    "y": 4.116935078707767,
-                    "heading": -2.1746143409291885,
-                    "angularVelocity": -0.000001067434403137199,
-                    "velocityX": 3.762736535034787,
-                    "velocityY": -0.3904451590124025,
-                    "timestamp": 1.4130848412991934
-                },
-                {
-                    "x": 5.830736232387668,
-                    "y": 4.113670630261612,
-                    "heading": -2.1746144425807508,
-                    "angularVelocity": -0.000002820467492063081,
-                    "velocityX": 3.7818548312825957,
-                    "velocityY": -0.09057677538205054,
-                    "timestamp": 1.4491255198548754
-                },
-                {
-                    "x": 5.967072945403984,
-                    "y": 4.1145733334601475,
-                    "heading": -2.174614542773063,
-                    "angularVelocity": -0.000002779978522071658,
-                    "velocityX": 3.7828564411094536,
-                    "velocityY": 0.02504678698377033,
-                    "timestamp": 1.4851661984105573
-                },
-                {
-                    "x": 6.103409655835918,
-                    "y": 4.115476426894617,
-                    "heading": -2.1746146429653797,
-                    "angularVelocity": -0.0000027799786444267726,
-                    "velocityX": 3.7828563694020714,
-                    "velocityY": 0.025057614636036382,
-                    "timestamp": 1.5212068769662392
-                },
-                {
-                    "x": 6.239746366170269,
-                    "y": 4.1163795350566765,
-                    "heading": -2.1746147431577554,
-                    "angularVelocity": -0.000002779980281350245,
-                    "velocityX": 3.782856366694493,
-                    "velocityY": 0.025058023274013706,
-                    "timestamp": 1.5572475555219212
-                },
-                {
-                    "x": 6.37608307646628,
-                    "y": 4.117282649003582,
-                    "heading": -2.174614843350174,
-                    "angularVelocity": -0.0000027799814898706564,
-                    "velocityX": 3.7828563656306846,
-                    "velocityY": 0.025058183782793973,
-                    "timestamp": 1.5932882340776031
-                },
-                {
-                    "x": 6.512419786749522,
-                    "y": 4.118185764875291,
-                    "heading": -2.174614943542629,
-                    "angularVelocity": -0.00000277998246882532,
-                    "velocityX": 3.782856365276422,
-                    "velocityY": 0.025058237189222135,
-                    "timestamp": 1.629328912633285
-                },
-                {
-                    "x": 6.648756497843916,
-                    "y": 4.11908875827432,
-                    "heading": -2.1746150437352676,
-                    "angularVelocity": -0.000002779987571113984,
-                    "velocityX": 3.782856387782983,
-                    "velocityY": 0.02505483901017122,
-                    "timestamp": 1.665369591188967
-                },
-                {
-                    "x": 6.784343394669118,
-                    "y": 4.118261735488966,
-                    "heading": -2.17648334857758,
-                    "angularVelocity": -0.05183878098815612,
-                    "velocityX": 3.762051722076281,
-                    "velocityY": -0.02294692604294446,
-                    "timestamp": 1.701410269744649
-                },
-                {
-                    "x": 6.911008509970103,
-                    "y": 4.118512712423575,
-                    "heading": -2.2004225516217266,
-                    "angularVelocity": -0.6642273121234696,
-                    "velocityX": 3.5145041763098894,
-                    "velocityY": 0.006963712800855175,
-                    "timestamp": 1.7374509483003309
-                },
-                {
-                    "x": 7.028751474127586,
-                    "y": 4.119997003379487,
-                    "heading": -2.2461001757447607,
-                    "angularVelocity": -1.267390791559704,
-                    "velocityX": 3.2669463749294456,
-                    "velocityY": 0.04118376832493557,
-                    "timestamp": 1.7734916268560128
-                },
-                {
-                    "x": 7.137616008718106,
-                    "y": 4.122892854321661,
-                    "heading": -2.313255173533344,
-                    "angularVelocity": -1.863311138407951,
-                    "velocityX": 3.0206016910121933,
-                    "velocityY": 0.08034951222410198,
-                    "timestamp": 1.8095323054116947
-                },
-                {
-                    "x": 7.237654743603063,
-                    "y": 4.124394361652757,
-                    "heading": -2.4021910734424403,
-                    "angularVelocity": -2.467653314897855,
-                    "velocityX": 2.775717297619687,
-                    "velocityY": 0.04166146119519181,
-                    "timestamp": 1.8455729839673767
-                },
-                {
-                    "x": 7.328556844177467,
-                    "y": 4.1248711523025925,
-                    "heading": -2.5119123449992884,
-                    "angularVelocity": -3.04437307936175,
-                    "velocityX": 2.5222083550386993,
-                    "velocityY": 0.013229236211493491,
-                    "timestamp": 1.8816136625230586
-                },
-                {
-                    "x": 7.408737931676895,
-                    "y": 4.125345359362809,
-                    "heading": -2.6271223888576807,
-                    "angularVelocity": -3.1966668907300306,
-                    "velocityX": 2.224738565217367,
-                    "velocityY": 0.013157550834778516,
-                    "timestamp": 1.9176543410787406
-                },
-                {
-                    "x": 7.478113482970281,
-                    "y": 4.125802607297277,
-                    "heading": -2.739701188642133,
-                    "angularVelocity": -3.123659273243732,
-                    "velocityX": 1.9249235606428023,
-                    "velocityY": 0.012686995716837342,
-                    "timestamp": 1.9536950196344225
-                },
-                {
-                    "x": 7.536867447368059,
-                    "y": 4.126215703233864,
-                    "heading": -2.8446379232047505,
-                    "angularVelocity": -2.911619280433174,
-                    "velocityX": 1.630212491893156,
-                    "velocityY": 0.011461935600054532,
-                    "timestamp": 1.9897356981901044
-                },
-                {
-                    "x": 7.585249192103714,
-                    "y": 4.126543507918265,
-                    "heading": -2.93809966154793,
-                    "angularVelocity": -2.593229153518397,
-                    "velocityX": 1.3424204724921138,
-                    "velocityY": 0.009095408231394075,
-                    "timestamp": 2.025776376745786
-                },
-                {
-                    "x": 7.623522580110136,
-                    "y": 4.126765972225253,
-                    "heading": -3.01693933623456,
-                    "angularVelocity": -2.1875191546359196,
-                    "velocityX": 1.0619497062823264,
-                    "velocityY": 0.006172589304754541,
-                    "timestamp": 2.061817055301468
-                },
-                {
-                    "x": 7.651933909825069,
-                    "y": 4.126896328898384,
-                    "heading": -3.078642707410718,
-                    "angularVelocity": -1.7120479871328622,
-                    "velocityX": 0.7883128413089711,
-                    "velocityY": 0.003616931710359869,
-                    "timestamp": 2.0978577338571496
-                },
-                {
-                    "x": 7.670698144632144,
-                    "y": 4.126965664579503,
-                    "heading": -3.1212443544252966,
-                    "angularVelocity": -1.1820434220948564,
-                    "velocityX": 0.5206404418297583,
-                    "velocityY": 0.0019238173058132363,
-                    "timestamp": 2.1338984124128313
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -0.6087830094650188,
-                    "velocityX": 0.25809323632697007,
-                    "velocityY": 0.0009526851844440906,
-                    "timestamp": 2.169939090968513
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -7.326485384728368e-34,
-                    "velocityX": -1.4740993581424338e-35,
-                    "velocityY": 9.762457034283277e-35,
-                    "timestamp": 2.2059797695241947
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -12029,7 +11087,7 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -12038,7 +11096,7 @@
                 {
                     "x": 4.169802665710449,
                     "y": 4.898907661437988,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -12047,7 +11105,7 @@
                 {
                     "x": 5.703982353210449,
                     "y": 3.987083911895752,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -12056,7 +11114,7 @@
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": -3.143185307179586,
+                    "heading": -0.003,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -12067,578 +11125,578 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 0.39,
                     "angularVelocity": 0,
-                    "velocityX": -2.2987344072087424e-31,
-                    "velocityY": -2.682409549792184e-31,
+                    "velocityX": 1.0547928981710615e-31,
+                    "velocityY": 1.4824740100837611e-32,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.30727109352441,
-                    "y": 6.7499479361039905,
-                    "heading": -2.752438811293409,
-                    "angularVelocity": -0.06989284029038495,
-                    "velocityX": 0.20837913118223148,
-                    "velocityY": -0.20210205559294603,
-                    "timestamp": 0.03489357827320871
-                },
-                {
-                    "x": 2.321814327969628,
-                    "y": 6.735842827712052,
-                    "heading": -2.7572390739653767,
-                    "angularVelocity": -0.1375686561688486,
-                    "velocityX": 0.41678827924577844,
-                    "velocityY": -0.4042322137758234,
-                    "timestamp": 0.06978715654641741
-                },
-                {
-                    "x": 2.3436309003249467,
-                    "y": 6.714683550648929,
-                    "heading": -2.7643090195026687,
-                    "angularVelocity": -0.202614517832932,
-                    "velocityX": 0.6252317313088365,
-                    "velocityY": -0.6063945892120965,
-                    "timestamp": 0.10468073481962611
-                },
-                {
-                    "x": 2.3727221871027773,
-                    "y": 6.6864688072553555,
-                    "heading": -2.7735381226601596,
-                    "angularVelocity": -0.26449288419860933,
-                    "velocityX": 0.8337146322470192,
-                    "velocityY": -0.8085941537052478,
-                    "timestamp": 0.13957431309283483
-                },
-                {
-                    "x": 2.409089780647816,
-                    "y": 6.651197089471134,
-                    "heading": -2.7847907497118745,
-                    "angularVelocity": -0.32248418214977886,
-                    "velocityX": 1.042243167504621,
-                    "velocityY": -1.0108369370447357,
-                    "timestamp": 0.17446789136604354
-                },
-                {
-                    "x": 2.452735530672034,
-                    "y": 6.6088666355169075,
-                    "heading": -2.7978965041034827,
-                    "angularVelocity": -0.3755921587918882,
-                    "velocityX": 1.2508247128592573,
-                    "velocityY": -1.213130210458454,
-                    "timestamp": 0.20936146963925226
-                },
-                {
-                    "x": 2.5036615834922005,
-                    "y": 6.559475386735866,
-                    "heading": -2.8126348139869735,
-                    "angularVelocity": -0.42237886203854363,
-                    "velocityX": 1.459467768579872,
-                    "velocityY": -1.4154824820291676,
-                    "timestamp": 0.24425504791246097
-                },
-                {
-                    "x": 2.5618703896153354,
-                    "y": 6.503020972116864,
-                    "heading": -2.8287087438800844,
-                    "angularVelocity": -0.4606558194535241,
-                    "velocityX": 1.6681810523235565,
-                    "velocityY": -1.6179027033850466,
-                    "timestamp": 0.27914862618566966
-                },
-                {
-                    "x": 2.6273645712551357,
-                    "y": 6.439500824983286,
-                    "heading": -2.8456966886741912,
-                    "angularVelocity": -0.48685017802115704,
-                    "velocityX": 1.8769694849578602,
-                    "velocityY": -1.8203964820182346,
-                    "timestamp": 0.31404220445887837
-                },
-                {
-                    "x": 2.700146215046118,
-                    "y": 6.368912855338095,
-                    "heading": -2.8629525699896186,
-                    "angularVelocity": -0.4945288551468594,
-                    "velocityX": 2.085817717550196,
-                    "velocityY": -2.0229501569744945,
-                    "timestamp": 0.3489357827320871
-                },
-                {
-                    "x": 2.780213462242076,
-                    "y": 6.2912587361998735,
-                    "heading": -2.8793619429160175,
-                    "angularVelocity": -0.47026913657055547,
-                    "velocityX": 2.294612681137229,
-                    "velocityY": -2.225455885613285,
-                    "timestamp": 0.3838293610052958
-                },
-                {
-                    "x": 2.8675389350998404,
-                    "y": 6.20656386499432,
-                    "heading": -2.892555231685043,
-                    "angularVelocity": -0.37810076873529364,
-                    "velocityX": 2.502623037798732,
-                    "velocityY": -2.42723376039027,
-                    "timestamp": 0.4187229392785045
-                },
-                {
-                    "x": 2.9617036478615395,
-                    "y": 6.11507787860357,
-                    "heading": -2.8943452360571387,
-                    "angularVelocity": -0.05129896275126653,
-                    "velocityX": 2.698625862455626,
-                    "velocityY": -2.621857399502999,
-                    "timestamp": 0.4536165175517132
-                },
-                {
-                    "x": 3.05646121242224,
-                    "y": 6.023180735189561,
-                    "heading": -2.894345248240927,
-                    "angularVelocity": -3.4916993473204957e-7,
-                    "velocityX": 2.7156161462940758,
-                    "velocityY": -2.633640571181149,
-                    "timestamp": 0.48851009582492194
-                },
-                {
-                    "x": 3.151218783278769,
-                    "y": 5.931283598267322,
-                    "heading": -2.894345260424686,
-                    "angularVelocity": -3.4916908908792776e-7,
-                    "velocityX": 2.7156163267235134,
-                    "velocityY": -2.633640385136315,
-                    "timestamp": 0.5234036740981306
-                },
-                {
-                    "x": 3.245976354135778,
-                    "y": 5.839386461345578,
-                    "heading": -2.894345272608445,
-                    "angularVelocity": -3.491690897148369e-7,
-                    "velocityX": 2.715616326737254,
-                    "velocityY": -2.633640385122147,
-                    "timestamp": 0.5582972523713393
-                },
-                {
-                    "x": 3.340733924992786,
-                    "y": 5.747489324423833,
-                    "heading": -2.894345284792204,
-                    "angularVelocity": -3.491690888486973e-7,
-                    "velocityX": 2.715616326737255,
-                    "velocityY": -2.633640385122146,
-                    "timestamp": 0.593190830644548
-                },
-                {
-                    "x": 3.435491495849795,
-                    "y": 5.655592187502088,
-                    "heading": -2.894345296975963,
-                    "angularVelocity": -3.4916908897748007e-7,
-                    "velocityX": 2.715616326737255,
-                    "velocityY": -2.633640385122146,
-                    "timestamp": 0.6280844089177567
-                },
-                {
-                    "x": 3.530249066706803,
-                    "y": 5.5636950505803435,
-                    "heading": -2.8943453091597218,
-                    "angularVelocity": -3.491690889850577e-7,
-                    "velocityX": 2.715616326737255,
-                    "velocityY": -2.633640385122146,
-                    "timestamp": 0.6629779871909655
-                },
-                {
-                    "x": 3.625006637563812,
-                    "y": 5.471797913658599,
-                    "heading": -2.8943453213434807,
-                    "angularVelocity": -3.49169089086754e-7,
-                    "velocityX": 2.715616326737255,
-                    "velocityY": -2.633640385122146,
-                    "timestamp": 0.6978715654641742
-                },
-                {
-                    "x": 3.7197642084208202,
-                    "y": 5.379900776736854,
-                    "heading": -2.8943453335272395,
-                    "angularVelocity": -3.4916908901998635e-7,
-                    "velocityX": 2.71561632673725,
-                    "velocityY": -2.6336403851221513,
-                    "timestamp": 0.7327651437373829
-                },
-                {
-                    "x": 3.814521779275631,
-                    "y": 5.288003639812843,
-                    "heading": -2.8943453457109984,
-                    "angularVelocity": -3.4916908849611257e-7,
-                    "velocityX": 2.715616326674264,
-                    "velocityY": -2.633640385187098,
-                    "timestamp": 0.7676587220105916
-                },
-                {
-                    "x": 3.9092793212702794,
-                    "y": 5.196106473130367,
-                    "heading": -2.8943453578947573,
-                    "angularVelocity": -3.4916909136087424e-7,
-                    "velocityX": 2.715615499583321,
-                    "velocityY": -2.6336412380221117,
-                    "timestamp": 0.8025523002838003
-                },
-                {
-                    "x": 4.0036585134090545,
-                    "y": 5.103820780001642,
-                    "heading": -2.8943453700832222,
-                    "angularVelocity": -3.49303962122221e-7,
-                    "velocityX": 2.704772534355369,
-                    "velocityY": -2.644775849761548,
-                    "timestamp": 0.837445878557009
-                },
-                {
-                    "x": 4.09067331484616,
-                    "y": 5.004560993237775,
-                    "heading": -2.894345382446594,
-                    "angularVelocity": -3.5431651835190834e-7,
-                    "velocityX": 2.4937196396368213,
-                    "velocityY": -2.8446433892988883,
-                    "timestamp": 0.8723394568302177
+                    "x": 2.3072711000078927,
+                    "y": 6.749947927475375,
+                    "heading": 0.3875624240531776,
+                    "angularVelocity": -0.06985747897751753,
+                    "velocityX": 0.2083794421285326,
+                    "velocityY": -0.20210242424612682,
+                    "timestamp": 0.034893557318421635
+                },
+                {
+                    "x": 2.321814346754588,
+                    "y": 6.735842802544083,
+                    "heading": 0.3827646159677583,
+                    "angularVelocity": -0.1374983938048157,
+                    "velocityX": 0.41678888208448434,
+                    "velocityY": -0.40423293052572296,
+                    "timestamp": 0.06978711463684327
+                },
+                {
+                    "x": 2.343630936460155,
+                    "y": 6.714683501867812,
+                    "heading": 0.3756983254810237,
+                    "angularVelocity": -0.20250989093060998,
+                    "velocityX": 0.6252326040156703,
+                    "velocityY": -0.6063956300924075,
+                    "timestamp": 0.1046806719552649
+                },
+                {
+                    "x": 2.372722244735657,
+                    "y": 6.6864687287677045,
+                    "heading": 0.36647405587954873,
+                    "angularVelocity": -0.2643545201567945,
+                    "velocityX": 0.8337157490143364,
+                    "velocityY": -0.8085954906412398,
+                    "timestamp": 0.13957422927368654
+                },
+                {
+                    "x": 2.4090898628559576,
+                    "y": 6.651196976344128,
+                    "heading": 0.3552274150516352,
+                    "angularVelocity": -0.3223128191053197,
+                    "velocityX": 1.042244497699901,
+                    "velocityY": -1.0108385368022033,
+                    "timestamp": 0.17446778659210818
+                },
+                {
+                    "x": 2.452735639247045,
+                    "y": 6.608866484215248,
+                    "heading": 0.3421287688368095,
+                    "angularVelocity": -0.3753886740550377,
+                    "velocityX": 1.2508262196599174,
+                    "velocityY": -1.213132033016619,
+                    "timestamp": 0.20936134391052982
+                },
+                {
+                    "x": 2.5036617186589587,
+                    "y": 6.55947519544937,
+                    "heading": 0.3273986519293141,
+                    "angularVelocity": -0.42214431658760004,
+                    "velocityX": 1.459469407122573,
+                    "velocityY": -1.4154844779841917,
+                    "timestamp": 0.24425490122895147
+                },
+                {
+                    "x": 2.5618705496598224,
+                    "y": 6.503020741236739,
+                    "heading": 0.31133395345849213,
+                    "angularVelocity": -0.4603915365872077,
+                    "velocityX": 1.6681827670844436,
+                    "velocityY": -1.6179048096889317,
+                    "timestamp": 0.2791484585473731
+                },
+                {
+                    "x": 2.6273647520175327,
+                    "y": 6.43950055782557,
+                    "heading": 0.294356217256913,
+                    "angularVelocity": -0.486557906568497,
+                    "velocityX": 1.8769712058889019,
+                    "velocityY": -1.8203986148936577,
+                    "timestamp": 0.3140420158657947
+                },
+                {
+                    "x": 2.7001464092257668,
+                    "y": 6.368912559254549,
+                    "heading": 0.2771114307132975,
+                    "angularVelocity": -0.4942111916606252,
+                    "velocityX": 2.0858193546752704,
+                    "velocityY": -2.022952200799379,
+                    "timestamp": 0.34893557318421636
+                },
+                {
+                    "x": 2.7802136587601143,
+                    "y": 6.291258423996795,
+                    "heading": 0.2607138645949654,
+                    "angularVelocity": -0.46993105256354084,
+                    "velocityX": 2.294614126146364,
+                    "velocityY": -2.225457684039485,
+                    "timestamp": 0.383829130502638
+                },
+                {
+                    "x": 2.867539127268515,
+                    "y": 6.206563552711186,
+                    "heading": 0.24753249327015822,
+                    "angularVelocity": -0.37775945870237265,
+                    "velocityX": 2.5026244160637545,
+                    "velocityY": -2.427235220322282,
+                    "timestamp": 0.41872268782105965
+                },
+                {
+                    "x": 2.961704724093414,
+                    "y": 6.1150775036748035,
+                    "heading": 0.24574502539338514,
+                    "angularVelocity": -0.05122630118968667,
+                    "velocityX": 2.6986528190746,
+                    "velocityY": -2.6218607693541762,
+                    "timestamp": 0.4536162451394813
+                },
+                {
+                    "x": 3.0564622015652247,
+                    "y": 6.023180384324785,
+                    "heading": 0.2457450132145974,
+                    "angularVelocity": -3.4902683097432965e-7,
+                    "velocityX": 2.7156152812710346,
+                    "velocityY": -2.633641463133395,
+                    "timestamp": 0.48850980245790293
+                },
+                {
+                    "x": 3.1512196852998877,
+                    "y": 5.931283271432529,
+                    "heading": 0.24574500103583913,
+                    "angularVelocity": -3.490259866463667e-7,
+                    "velocityX": 2.7156154607555005,
+                    "velocityY": -2.6336412780630685,
+                    "timestamp": 0.5234033597763246
+                },
+                {
+                    "x": 3.2459771690350276,
+                    "y": 5.839386158540766,
+                    "heading": 0.2457449888570809,
+                    "angularVelocity": -3.4902598588384144e-7,
+                    "velocityX": 2.7156154607691696,
+                    "velocityY": -2.6336412780489744,
+                    "timestamp": 0.5582969170947462
+                },
+                {
+                    "x": 3.3407346527701676,
+                    "y": 5.747489045649002,
+                    "heading": 0.24574497667832262,
+                    "angularVelocity": -3.4902598639891e-7,
+                    "velocityX": 2.7156154607691705,
+                    "velocityY": -2.6336412780489735,
+                    "timestamp": 0.5931904744131677
+                },
+                {
+                    "x": 3.4354921365053075,
+                    "y": 5.655591932757238,
+                    "heading": 0.24574496449956437,
+                    "angularVelocity": -3.4902598640760607e-7,
+                    "velocityX": 2.71561546076917,
+                    "velocityY": -2.6336412780489735,
+                    "timestamp": 0.6280840317315893
+                },
+                {
+                    "x": 3.5302496202404474,
+                    "y": 5.563694819865473,
+                    "heading": 0.2457449523208061,
+                    "angularVelocity": -3.490259865548224e-7,
+                    "velocityX": 2.7156154607691705,
+                    "velocityY": -2.6336412780489735,
+                    "timestamp": 0.6629775890500109
+                },
+                {
+                    "x": 3.6250071039755873,
+                    "y": 5.471797706973709,
+                    "heading": 0.2457449401420478,
+                    "angularVelocity": -3.4902598644057735e-7,
+                    "velocityX": 2.71561546076917,
+                    "velocityY": -2.633641278048974,
+                    "timestamp": 0.6978711463684325
+                },
+                {
+                    "x": 3.719764587710727,
+                    "y": 5.379900594081945,
+                    "heading": 0.24574492796328956,
+                    "angularVelocity": -3.4902598592532335e-7,
+                    "velocityX": 2.715615460769166,
+                    "velocityY": -2.633641278048979,
+                    "timestamp": 0.7327647036868541
+                },
+                {
+                    "x": 3.8145220714436685,
+                    "y": 5.288003481187913,
+                    "heading": 0.24574491578453134,
+                    "angularVelocity": -3.490259859254125e-7,
+                    "velocityX": 2.7156154607061804,
+                    "velocityY": -2.6336412781139247,
+                    "timestamp": 0.7676582610052757
+                },
+                {
+                    "x": 3.909279526317127,
+                    "y": 5.196106338536137,
+                    "heading": 0.245744903605773,
+                    "angularVelocity": -3.4902598866436484e-7,
+                    "velocityX": 2.7156146336342,
+                    "velocityY": -2.6336421309288234,
+                    "timestamp": 0.8025518183236973
+                },
+                {
+                    "x": 4.003658642045134,
+                    "y": 5.103820680647278,
+                    "heading": 0.24574489142231412,
+                    "angularVelocity": -3.4916069991394096e-7,
+                    "velocityX": 2.7047719688402356,
+                    "velocityY": -2.644776428116158,
+                    "timestamp": 0.8374453756421188
+                },
+                {
+                    "x": 4.090673374879912,
+                    "y": 5.004560939161802,
+                    "heading": 0.24574487906407927,
+                    "angularVelocity": -3.541695320845138e-7,
+                    "velocityX": 2.493719171155964,
+                    "velocityY": -2.8446437999909007,
+                    "timestamp": 0.8723389329605404
                 },
                 {
                     "x": 4.169802665710449,
                     "y": 4.898907661437988,
-                    "heading": -2.8943453954133593,
-                    "angularVelocity": -3.71608932660535e-7,
-                    "velocityX": 2.2677339149549134,
-                    "velocityY": -3.0278732370909647,
-                    "timestamp": 0.9072330351034265
-                },
-                {
-                    "x": 4.267711504415231,
-                    "y": 4.731769329108682,
-                    "heading": -2.8943454067066363,
-                    "angularVelocity": -2.2055157567695873e-7,
-                    "velocityX": 1.9121064876426594,
-                    "velocityY": -3.2641209293092457,
-                    "timestamp": 0.9584377349135761
-                },
-                {
-                    "x": 4.3838302459464416,
-                    "y": 4.576728047661677,
-                    "heading": -2.8943454207852617,
-                    "angularVelocity": -2.749479155903796e-7,
-                    "velocityX": 2.2677360078614184,
-                    "velocityY": -3.027872090293428,
-                    "timestamp": 1.0096424347237256
-                },
-                {
-                    "x": 4.5166798451311,
-                    "y": 4.43575875391577,
-                    "heading": -2.8943454409364353,
-                    "angularVelocity": -3.9354148044055934e-7,
-                    "velocityX": 2.594480578486377,
-                    "velocityY": -2.7530538069469372,
-                    "timestamp": 1.0608471345338752
-                },
-                {
-                    "x": 4.664567965806816,
-                    "y": 4.3106572284696325,
-                    "heading": -2.8943459031991594,
-                    "angularVelocity": -0.000009027740145972362,
-                    "velocityX": 2.888174742241181,
-                    "velocityY": -2.443164903025968,
-                    "timestamp": 1.1120518343440249
-                },
-                {
-                    "x": 4.823636896940825,
-                    "y": 4.2042733124430525,
-                    "heading": -2.9000775312757443,
-                    "angularVelocity": -0.1119355859488654,
-                    "velocityX": 3.1065299029929774,
-                    "velocityY": -2.077620148560901,
-                    "timestamp": 1.1632565341541745
-                },
-                {
-                    "x": 4.99080976041859,
-                    "y": 4.118113444137051,
-                    "heading": -2.9122463824645286,
-                    "angularVelocity": -0.23765106003750752,
-                    "velocityX": 3.2647953038995774,
-                    "velocityY": -1.6826554715769175,
-                    "timestamp": 1.2144612339643241
-                },
-                {
-                    "x": 5.163718201552664,
-                    "y": 4.0530282335458665,
-                    "heading": -2.927811187303136,
-                    "angularVelocity": -0.30397219193387703,
-                    "velocityX": 3.3768080229971553,
-                    "velocityY": -1.2710788430065951,
-                    "timestamp": 1.2656659337744738
-                },
-                {
-                    "x": 5.340853772484394,
-                    "y": 4.009388376225937,
-                    "heading": -2.9448561503180763,
-                    "angularVelocity": -0.3328788778791379,
-                    "velocityX": 3.459361574005724,
-                    "velocityY": -0.852262731384641,
-                    "timestamp": 1.3168706335846234
-                },
-                {
-                    "x": 5.521188530660567,
-                    "y": 3.987375039379581,
-                    "heading": -2.9620889053684105,
-                    "angularVelocity": -0.3365463544211233,
-                    "velocityX": 3.5218399647844,
-                    "velocityY": -0.42990852261559487,
-                    "timestamp": 1.368075333394773
+                    "heading": 0.24574486610274957,
+                    "angularVelocity": -3.7145337638188186e-7,
+                    "velocityX": 2.267733556325072,
+                    "velocityY": -3.0278735056925665,
+                    "timestamp": 0.907232490278962
+                },
+                {
+                    "x": 4.2677114826543,
+                    "y": 4.731769321945997,
+                    "heading": 0.24574485481420902,
+                    "angularVelocity": -2.2045907687430814e-7,
+                    "velocityX": 1.9121061102333996,
+                    "velocityY": -3.264121150398313,
+                    "timestamp": 0.9584371888152268
+                },
+                {
+                    "x": 4.383830202938794,
+                    "y": 4.576728030606808,
+                    "heading": 0.24574484074146902,
+                    "angularVelocity": -2.7483298164960767e-7,
+                    "velocityX": 2.2677356493419163,
+                    "velocityY": -3.02787235881067,
+                    "timestamp": 1.0096418873514916
+                },
+                {
+                    "x": 4.516679781731222,
+                    "y": 4.435758724265,
+                    "heading": 0.24574482059862868,
+                    "angularVelocity": -3.9337875070195686e-7,
+                    "velocityX": 2.59448024478339,
+                    "velocityY": -2.753054121429295,
+                    "timestamp": 1.0608465858877565
+                },
+                {
+                    "x": 4.664567883303712,
+                    "y": 4.3106571835035465,
+                    "heading": 0.24574435877996634,
+                    "angularVelocity": -0.000009019068084382453,
+                    "velocityX": 2.8881744410183567,
+                    "velocityY": -2.4431652629075082,
+                    "timestamp": 1.1120512844240213
+                },
+                {
+                    "x": 4.823636899736968,
+                    "y": 4.204273204304783,
+                    "heading": 0.2400133838174815,
+                    "angularVelocity": -0.1119228337693714,
+                    "velocityX": 3.1065316461261507,
+                    "velocityY": -2.0776214339670136,
+                    "timestamp": 1.1632559829602862
+                },
+                {
+                    "x": 4.9908096959943835,
+                    "y": 4.118113363926556,
+                    "heading": 0.22784637628221624,
+                    "angularVelocity": -0.23761506039622932,
+                    "velocityX": 3.26479407234511,
+                    "velocityY": -1.6826549680241993,
+                    "timestamp": 1.214460681496551
+                },
+                {
+                    "x": 5.163718040144699,
+                    "y": 4.053028192881653,
+                    "heading": 0.21228377101249774,
+                    "angularVelocity": -0.3039292431083551,
+                    "velocityX": 3.3768062129661107,
+                    "velocityY": -1.2710781023114057,
+                    "timestamp": 1.265665380032816
+                },
+                {
+                    "x": 5.340853559631665,
+                    "y": 4.009388363770341,
+                    "heading": 0.19524077553782296,
+                    "angularVelocity": -0.332840461165967,
+                    "velocityX": 3.4593606553803307,
+                    "velocityY": -0.8522622016884831,
+                    "timestamp": 1.3168700785690808
+                },
+                {
+                    "x": 5.521188360604893,
+                    "y": 3.987375039650421,
+                    "heading": 0.17800934218450973,
+                    "angularVelocity": -0.3365205507676074,
+                    "velocityX": 3.521840888204999,
+                    "velocityY": -0.42990828477058174,
+                    "timestamp": 1.3680747771053456
                 },
                 {
                     "x": 5.703982353210449,
                     "y": 3.987083911895752,
-                    "heading": -2.978585149802318,
-                    "angularVelocity": -0.3221627017650738,
-                    "velocityX": 3.569864157540669,
-                    "velocityY": -0.0056855617728103595,
-                    "timestamp": 1.4192800332049227
-                },
-                {
-                    "x": 5.82977348163753,
-                    "y": 3.977187586416487,
-                    "heading": -2.987958123186903,
-                    "angularVelocity": -0.27093528448013743,
-                    "velocityX": 3.636119912761043,
-                    "velocityY": -0.2860633065962091,
-                    "timestamp": 1.4538749122092192
-                },
-                {
-                    "x": 5.956799020313408,
-                    "y": 3.9574239534344695,
-                    "heading": -2.994346009010497,
-                    "angularVelocity": -0.18464830655430406,
-                    "velocityX": 3.6718017906668177,
-                    "velocityY": -0.5712878192047837,
-                    "timestamp": 1.4884697912135156
-                },
-                {
-                    "x": 6.083502386416879,
-                    "y": 3.927762346295825,
-                    "heading": -2.9963234364674123,
-                    "angularVelocity": -0.057159542505398835,
-                    "velocityX": 3.662489066307642,
-                    "velocityY": -0.8573987824891895,
-                    "timestamp": 1.523064670217812
-                },
-                {
-                    "x": 6.208288544878107,
-                    "y": 3.888323197622386,
-                    "heading": -2.9963235773653807,
-                    "angularVelocity": -0.000004072798431777223,
-                    "velocityX": 3.6070702385092757,
-                    "velocityY": -1.1400285189186714,
-                    "timestamp": 1.5576595492221086
-                },
-                {
-                    "x": 6.329706848437896,
-                    "y": 3.8394904257798843,
-                    "heading": -2.9963235979992118,
-                    "angularVelocity": -5.964417674236108e-7,
-                    "velocityX": 3.5097189831104894,
-                    "velocityY": -1.4115607063241151,
-                    "timestamp": 1.592254428226405
-                },
-                {
-                    "x": 6.447051291726402,
-                    "y": 3.7815479813892945,
-                    "heading": -2.996323615206392,
-                    "angularVelocity": -4.973909609914855e-7,
-                    "velocityX": 3.3919599277665977,
-                    "velocityY": -1.6748850135707645,
-                    "timestamp": 1.6268493072307015
-                },
-                {
-                    "x": 6.559639598402797,
-                    "y": 3.714832771082226,
-                    "heading": -2.996323630138998,
-                    "angularVelocity": -4.316421016363579e-7,
-                    "velocityX": 3.2544789840835127,
-                    "velocityY": -1.9284706935608398,
-                    "timestamp": 1.661444186234998
-                },
-                {
-                    "x": 6.666817153320071,
-                    "y": 3.639732715012558,
-                    "heading": -2.996323643537738,
-                    "angularVelocity": -3.8730413434828734e-7,
-                    "velocityX": 3.0980757268717096,
-                    "velocityY": -2.1708431487891944,
-                    "timestamp": 1.6960390652392945
-                },
-                {
-                    "x": 6.767960818192567,
-                    "y": 3.556684498992451,
-                    "heading": -2.996323655916169,
-                    "angularVelocity": -3.578110804784623e-7,
-                    "velocityX": 2.9236600266743116,
-                    "velocityY": -2.400592758534959,
-                    "timestamp": 1.730633944243591
-                },
-                {
-                    "x": 6.862482590576338,
-                    "y": 3.466171063796537,
-                    "heading": -2.99632366765796,
-                    "angularVelocity": -3.394083586636284e-7,
-                    "velocityX": 2.7322475205660415,
-                    "velocityY": -2.6163824762813186,
-                    "timestamp": 1.7652288232478874
-                },
-                {
-                    "x": 6.949833264778536,
-                    "y": 3.3687189870150447,
-                    "heading": -2.9963236790740537,
-                    "angularVelocity": -3.299937460810232e-7,
-                    "velocityX": 2.524959667913537,
-                    "velocityY": -2.8169509356974314,
-                    "timestamp": 1.799823702252184
-                },
-                {
-                    "x": 7.031655596531464,
-                    "y": 3.2665810723307764,
-                    "heading": -2.9963236904142594,
-                    "angularVelocity": -3.2780012680929783e-7,
-                    "velocityX": 2.365157332759168,
-                    "velocityY": -2.9523998240197193,
-                    "timestamp": 1.8344185812564804
-                },
-                {
-                    "x": 7.1134768697956385,
-                    "y": 3.164442309703858,
-                    "heading": -2.9963237017545317,
-                    "angularVelocity": -3.2780205084524347e-7,
-                    "velocityX": 2.3651267360701933,
-                    "velocityY": -2.952424334660453,
-                    "timestamp": 1.8690134602607769
-                },
-                {
-                    "x": 7.194171018365882,
-                    "y": 3.0635712609157633,
-                    "heading": -3.000602693910309,
-                    "angularVelocity": -0.1236886001319993,
-                    "velocityX": 2.3325460557391247,
-                    "velocityY": -2.9157797827698024,
-                    "timestamp": 1.9036083392650733
-                },
-                {
-                    "x": 7.268926895683992,
-                    "y": 2.9702293971794824,
-                    "heading": -3.0147524025112875,
-                    "angularVelocity": -0.4090116516731078,
-                    "velocityX": 2.1608943135435097,
-                    "velocityY": -2.698141066621111,
-                    "timestamp": 1.9382032182693698
-                },
-                {
-                    "x": 7.337464009959094,
-                    "y": 2.8846562411973626,
-                    "heading": -3.031563935832344,
-                    "angularVelocity": -0.4859543899248224,
-                    "velocityX": 1.9811346721747336,
-                    "velocityY": -2.473578704278531,
-                    "timestamp": 1.9727980972736663
-                },
-                {
-                    "x": 7.399764505912731,
-                    "y": 2.806871719273257,
-                    "heading": -3.0489337906921676,
-                    "angularVelocity": -0.5020932392238406,
-                    "velocityX": 1.8008589059062745,
-                    "velocityY": -2.2484403519505154,
-                    "timestamp": 2.0073929762779628
-                },
-                {
-                    "x": 7.455827645980218,
-                    "y": 2.7368758607336336,
-                    "heading": -3.0658680766465114,
-                    "angularVelocity": -0.48950267905953376,
-                    "velocityX": 1.6205618195839024,
-                    "velocityY": -2.023301151911255,
-                    "timestamp": 2.041987855282259
-                },
-                {
-                    "x": 7.505655239118239,
-                    "y": 2.674665896616921,
-                    "heading": -3.081789323798935,
-                    "angularVelocity": -0.4602197669327373,
-                    "velocityX": 1.4403170229857634,
-                    "velocityY": -1.7982419915094008,
-                    "timestamp": 2.0765827342865553
-                },
-                {
-                    "x": 7.549249359478674,
-                    "y": 2.6202389173200062,
-                    "heading": -3.0963205890594514,
-                    "angularVelocity": -0.42004093318875263,
-                    "velocityX": 1.2601321818475304,
-                    "velocityY": -1.5732669361310743,
-                    "timestamp": 2.1111776132908515
-                },
-                {
-                    "x": 7.586611891972334,
-                    "y": 2.573592354245129,
-                    "heading": -3.1091966615790723,
-                    "angularVelocity": -0.37219591136657937,
-                    "velocityX": 1.0800018259644524,
-                    "velocityY": -1.348366128671367,
-                    "timestamp": 2.1457724922951478
-                },
-                {
-                    "x": 7.617744461907291,
-                    "y": 2.5347240241708393,
-                    "heading": -3.1202208856007014,
-                    "angularVelocity": -0.318666355799661,
-                    "velocityX": 0.8999184512566408,
-                    "velocityY": -1.1235284294378556,
-                    "timestamp": 2.180367371299444
-                },
-                {
-                    "x": 7.642648454713618,
-                    "y": 2.5036320816555615,
-                    "heading": -3.1292416557703198,
-                    "angularVelocity": -0.26075449399601563,
-                    "velocityX": 0.7198751238076218,
-                    "velocityY": -0.8987440745613371,
-                    "timestamp": 2.2149622503037403
-                },
-                {
-                    "x": 7.66132505338278,
-                    "y": 2.48031495979902,
-                    "heading": -3.136138533441162,
-                    "angularVelocity": -0.19936123118063845,
-                    "velocityX": 0.5398659919244735,
-                    "velocityY": -0.6740050125235452,
-                    "timestamp": 2.2495571293080365
-                },
-                {
-                    "x": 7.673775274710525,
-                    "y": 2.4647713180548445,
-                    "heading": -3.1408135216098865,
-                    "angularVelocity": -0.1351352657757358,
-                    "velocityX": 0.3598862515518098,
-                    "velocityY": -0.4493047003357149,
-                    "timestamp": 2.284152008312333
+                    "heading": 0.16151347881375258,
+                    "angularVelocity": -0.32215526782320836,
+                    "velocityX": 3.5698675674478513,
+                    "velocityY": -0.005685567203615865,
+                    "timestamp": 1.4192794756416105
+                },
+                {
+                    "x": 5.829773721796989,
+                    "y": 3.97718762682745,
+                    "heading": 0.15213932045666498,
+                    "angularVelocity": -0.2709697131085786,
+                    "velocityX": 3.6361292138465493,
+                    "velocityY": -0.2860623240667741,
+                    "timestamp": 1.453874332201581
+                },
+                {
+                    "x": 5.956799770812804,
+                    "y": 3.9574240312185514,
+                    "heading": 0.1457481223879076,
+                    "angularVelocity": -0.18474417021149397,
+                    "velocityX": 3.671818924747189,
+                    "velocityY": -0.5712871095342833,
+                    "timestamp": 1.4884691887615515
+                },
+                {
+                    "x": 6.083503083107889,
+                    "y": 3.927762559716917,
+                    "heading": 0.14376941436575622,
+                    "angularVelocity": -0.05719659564771788,
+                    "velocityX": 3.6624898870571543,
+                    "velocityY": -0.8573954180216341,
+                    "timestamp": 1.523064045321522
+                },
+                {
+                    "x": 6.208289204844049,
+                    "y": 3.888323576656908,
+                    "heading": 0.14376927335150447,
+                    "angularVelocity": -0.000004076162347950456,
+                    "velocityX": 3.607071517115297,
+                    "velocityY": -1.1400244713152792,
+                    "timestamp": 1.5576589018814926
+                },
+                {
+                    "x": 6.329707486841884,
+                    "y": 3.8394909787467797,
+                    "heading": 0.14376925272536353,
+                    "angularVelocity": -5.962198716671582e-7,
+                    "velocityX": 3.5097206368627396,
+                    "velocityY": -1.4115565944167625,
+                    "timestamp": 1.592253758441463
+                },
+                {
+                    "x": 6.447051924752506,
+                    "y": 3.781548715234953,
+                    "heading": 0.14376923552481305,
+                    "angularVelocity": -4.971996473322837e-7,
+                    "velocityX": 3.3919619729367447,
+                    "velocityY": -1.6748808717094477,
+                    "timestamp": 1.6268486150014336
+                },
+                {
+                    "x": 6.559640243151509,
+                    "y": 3.714833691264027,
+                    "heading": 0.14376922059812475,
+                    "angularVelocity": -4.3147131718664413e-7,
+                    "velocityX": 3.254481434366676,
+                    "velocityY": -1.9284665584687428,
+                    "timestamp": 1.6614434715614042
+                },
+                {
+                    "x": 6.666817827672777,
+                    "y": 3.6397338254007683,
+                    "heading": 0.14376920720482705,
+                    "angularVelocity": -3.8714707984086e-7,
+                    "velocityX": 3.0980785925640646,
+                    "velocityY": -2.1708390590685647,
+                    "timestamp": 1.6960383281213747
+                },
+                {
+                    "x": 6.7679615406661,
+                    "y": 3.5566858017838987,
+                    "heading": 0.14376919483153264,
+                    "angularVelocity": -3.5766283306602865e-7,
+                    "velocityX": 2.923663314457974,
+                    "velocityY": -2.4005887543688678,
+                    "timestamp": 1.7306331846813452
+                },
+                {
+                    "x": 6.862483380166932,
+                    "y": 3.466172559439492,
+                    "heading": 0.1437691830947059,
+                    "angularVelocity": -3.392650779789794e-7,
+                    "velocityX": 2.7322512332715396,
+                    "velocityY": -2.6163785991568234,
+                    "timestamp": 1.7652280412413157
+                },
+                {
+                    "x": 6.949834140776404,
+                    "y": 3.3687206741301803,
+                    "heading": 0.14376917168351727,
+                    "angularVelocity": -3.2985217383486685e-7,
+                    "velocityX": 2.5249638037391158,
+                    "velocityY": -2.8169472285678507,
+                    "timestamp": 1.7998228978012862
+                },
+                {
+                    "x": 7.031656177612435,
+                    "y": 3.2665826319797073,
+                    "heading": 0.14376916034823112,
+                    "angularVelocity": -3.2765813438231676e-7,
+                    "velocityX": 2.3651503423404003,
+                    "velocityY": -2.952405424009072,
+                    "timestamp": 1.8344177543612568
+                },
+                {
+                    "x": 7.113477155727132,
+                    "y": 3.1644437417043827,
+                    "heading": 0.14376914901287846,
+                    "angularVelocity": -3.2766005667768915e-7,
+                    "velocityX": 2.3651197389085663,
+                    "velocityY": -2.952429939932413,
+                    "timestamp": 1.8690126109212273
+                },
+                {
+                    "x": 7.194171820782979,
+                    "y": 3.0635723954548135,
+                    "heading": 0.13949339206360356,
+                    "angularVelocity": -0.12359516339843184,
+                    "velocityX": 2.332562498588806,
+                    "velocityY": -2.91579027288948,
+                    "timestamp": 1.9036074674811978
+                },
+                {
+                    "x": 7.268927567281138,
+                    "y": 2.9702304150182344,
+                    "heading": 0.12535509322542532,
+                    "angularVelocity": -0.4086821060717307,
+                    "velocityX": 2.160891933995128,
+                    "velocityY": -2.6981461904537576,
+                    "timestamp": 1.9382023240411683
+                },
+                {
+                    "x": 7.337464553197734,
+                    "y": 2.884657135542947,
+                    "heading": 0.10855528307456563,
+                    "angularVelocity": -0.48561583487814164,
+                    "velocityX": 1.9811322471531543,
+                    "velocityY": -2.473583878775318,
+                    "timestamp": 1.9727971806011388
+                },
+                {
+                    "x": 7.399764936518732,
+                    "y": 2.806872483431395,
+                    "heading": 0.0911966728700688,
+                    "angularVelocity": -0.5017685266133554,
+                    "velocityX": 1.8008568184984473,
+                    "velocityY": -2.2484455738878646,
+                    "timestamp": 2.0073920371611096
+                },
+                {
+                    "x": 7.455827979936063,
+                    "y": 2.7368764943083312,
+                    "heading": 0.07427285825902008,
+                    "angularVelocity": -0.4892003116622579,
+                    "velocityX": 1.6205600771936914,
+                    "velocityY": -2.023306239230242,
+                    "timestamp": 2.0419868937210803
+                },
+                {
+                    "x": 7.505655491239628,
+                    "y": 2.674666404415114,
+                    "heading": 0.05836114831277334,
+                    "angularVelocity": -0.4599443827339935,
+                    "velocityX": 1.4403155919200792,
+                    "velocityY": -1.7982467938658737,
+                    "timestamp": 2.076581750281051
+                },
+                {
+                    "x": 7.549249543273319,
+                    "y": 2.6202393080696202,
+                    "heading": 0.04383838388032337,
+                    "angularVelocity": -0.4197954805008245,
+                    "velocityX": 1.2601310243365538,
+                    "velocityY": -1.5732713402393688,
+                    "timestamp": 2.111176606841022
+                },
+                {
+                    "x": 7.586612019746033,
+                    "y": 2.5735926396425732,
+                    "heading": 0.030969705386613037,
+                    "angularVelocity": -0.3719824208954968,
+                    "velocityX": 1.0800009073009074,
+                    "velocityY": -1.348370048772548,
+                    "timestamp": 2.1457714634009926
+                },
+                {
+                    "x": 7.6177445449139505,
+                    "y": 2.5347242182058114,
+                    "heading": 0.01995171749042388,
+                    "angularVelocity": -0.31848630090688007,
+                    "velocityX": 0.8999177410650449,
+                    "velocityY": -1.1235317992830327,
+                    "timestamp": 2.1803663199609633
+                },
+                {
+                    "x": 7.642648503297853,
+                    "y": 2.503632200129417,
+                    "heading": 0.010935986949086615,
+                    "angularVelocity": -0.26060898751548167,
+                    "velocityX": 0.7198745958298824,
+                    "velocityY": -0.8987468418172337,
+                    "timestamp": 2.214961176520934
+                },
+                {
+                    "x": 7.661325077102299,
+                    "y": 2.4803150199755253,
+                    "heading": 0.004042922192056018,
+                    "angularVelocity": -0.19925114431624774,
+                    "velocityX": 0.5398656234365572,
+                    "velocityY": -0.6740071349471043,
+                    "timestamp": 2.249556033080905
+                },
+                {
+                    "x": 7.673775282436928,
+                    "y": 2.464771338402636,
+                    "heading": -0.0006295042844945549,
+                    "angularVelocity": -0.13506130509461317,
+                    "velocityX": 0.3598860227400032,
+                    "velocityY": -0.44930614312403183,
+                    "timestamp": 2.2841508896408755
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -0.06855886298677888,
-                    "velocityX": 0.17993198613883732,
-                    "velocityY": -0.22463781572640673,
-                    "timestamp": 2.318746887316629
+                    "heading": -0.003,
+                    "angularVelocity": -0.06852162290068065,
+                    "velocityX": 0.17993187953478723,
+                    "velocityY": -0.22463854964001964,
+                    "timestamp": 2.3187457462008463
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -2.3105673781771525e-33,
+                    "heading": -0.003,
+                    "angularVelocity": -8.413636330536319e-34,
                     "velocityX": 0,
-                    "velocityY": -9.473748472540581e-34,
-                    "timestamp": 2.3533417663209253
+                    "velocityY": 4.110332962762654e-35,
+                    "timestamp": 2.353340602760817
                 }
             ],
             "constraints": [
@@ -12672,7 +11730,7 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -12681,7 +11739,7 @@
                 {
                     "x": 2.382448196411133,
                     "y": 1.9443897008895874,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -12690,627 +11748,14 @@
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": -3.143185307179586,
+                    "heading": -0.003,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": -2.75,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -2.713469488843566e-34,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.2985074122681866,
-                    "y": 6.738728579173207,
-                    "heading": -2.7557122969387127,
-                    "angularVelocity": -0.12159424626473993,
-                    "velocityX": -0.03177182176995015,
-                    "velocityY": -0.38893280014262377,
-                    "timestamp": 0.046978349010656624
-                },
-                {
-                    "x": 2.2955230171267877,
-                    "y": 6.702182881977796,
-                    "heading": -2.7670222648124514,
-                    "angularVelocity": -0.24074851738984976,
-                    "velocityX": -0.06352703328764883,
-                    "velocityY": -0.777926384495132,
-                    "timestamp": 0.09395669802131325
-                },
-                {
-                    "x": 2.2910478333318625,
-                    "y": 6.647359316893119,
-                    "heading": -2.7837824265023454,
-                    "angularVelocity": -0.3567635313470153,
-                    "velocityX": -0.09526055915480924,
-                    "velocityY": -1.1669964193982714,
-                    "timestamp": 0.14093504703196988
-                },
-                {
-                    "x": 2.2850832407725172,
-                    "y": 6.574253215154707,
-                    "heading": -2.805794841014387,
-                    "angularVelocity": -0.46856509382754397,
-                    "velocityX": -0.12696471214840174,
-                    "velocityY": -1.556165835496437,
-                    "timestamp": 0.1879133960426265
-                },
-                {
-                    "x": 2.2776312180066953,
-                    "y": 6.482858263005064,
-                    "heading": -2.832779187520957,
-                    "angularVelocity": -0.5743996346156903,
-                    "velocityX": -0.15862674876316962,
-                    "velocityY": -1.945469648771827,
-                    "timestamp": 0.2348917450532831
-                },
-                {
-                    "x": 2.2686948542910046,
-                    "y": 6.373165502071328,
-                    "heading": -2.864308391554441,
-                    "angularVelocity": -0.6711432968053561,
-                    "velocityX": -0.1902230262213726,
-                    "velocityY": -2.3349641535689947,
-                    "timestamp": 0.28187009406393976
-                },
-                {
-                    "x": 2.2582796451376828,
-                    "y": 6.245161498589565,
-                    "heading": -2.8996563968568023,
-                    "angularVelocity": -0.7524318339570105,
-                    "velocityX": -0.22170232399947937,
-                    "velocityY": -2.7247446148592136,
-                    "timestamp": 0.3288484430745964
-                },
-                {
-                    "x": 2.2463978083089957,
-                    "y": 6.098825641295113,
-                    "heading": -2.9373287044127423,
-                    "angularVelocity": -0.8019078649910463,
-                    "velocityX": -0.2529215495842953,
-                    "velocityY": -3.1149638158049613,
-                    "timestamp": 0.375826792085253
-                },
-                {
-                    "x": 2.23309481763283,
-                    "y": 5.934156164582871,
-                    "heading": -2.9725027104068498,
-                    "angularVelocity": -0.7487280148165356,
-                    "velocityX": -0.2831728010098399,
-                    "velocityY": -3.505220600129828,
-                    "timestamp": 0.4228051410959096
-                },
-                {
-                    "x": 2.2185299400469667,
-                    "y": 5.757037726236599,
-                    "heading": -2.9725027230810714,
-                    "angularVelocity": -2.697885773041044e-7,
-                    "velocityX": -0.31003383244820454,
-                    "velocityY": -3.77021419603515,
-                    "timestamp": 0.4697834901065662
-                },
-                {
-                    "x": 2.2039649895163667,
-                    "y": 5.57991929388838,
-                    "heading": -2.972502735754471,
-                    "angularVelocity": -2.6977108201545366e-7,
-                    "velocityX": -0.3100353851791582,
-                    "velocityY": -3.7702140683582024,
-                    "timestamp": 0.5167618391172228
-                },
-                {
-                    "x": 2.189400055868242,
-                    "y": 5.402800860151866,
-                    "heading": -2.972502748427869,
-                    "angularVelocity": -2.6977103310705994e-7,
-                    "velocityX": -0.31003502581200326,
-                    "velocityY": -3.770214097910002,
-                    "timestamp": 0.5637401881278795
-                },
-                {
-                    "x": 2.174835140412249,
-                    "y": 5.225682424919363,
-                    "heading": -2.9725027611012638,
-                    "angularVelocity": -2.69770971237986e-7,
-                    "velocityX": -0.3100346385669985,
-                    "velocityY": -3.7702141297542338,
-                    "timestamp": 0.6107185371385362
-                },
-                {
-                    "x": 2.1602702444212443,
-                    "y": 5.048563988086202,
-                    "heading": -2.972502773774655,
-                    "angularVelocity": -2.697708934411251e-7,
-                    "velocityX": -0.31003422422742555,
-                    "velocityY": -3.770214163826477,
-                    "timestamp": 0.6576968861491929
-                },
-                {
-                    "x": 2.145705369054558,
-                    "y": 4.87144554955705,
-                    "heading": -2.972502786448042,
-                    "angularVelocity": -2.697707995800925e-7,
-                    "velocityX": -0.31003378520991653,
-                    "velocityY": -3.7702141999279966,
-                    "timestamp": 0.7046752351598495
-                },
-                {
-                    "x": 2.1311405152628695,
-                    "y": 4.694327109253736,
-                    "heading": -2.9725027991214237,
-                    "angularVelocity": -2.697706911872684e-7,
-                    "velocityX": -0.31003332595584043,
-                    "velocityY": -3.7702142376935592,
-                    "timestamp": 0.7516535841705062
-                },
-                {
-                    "x": 2.116575683697044,
-                    "y": 4.5172086671227385,
-                    "heading": -2.9725028117947994,
-                    "angularVelocity": -2.6977056864766263e-7,
-                    "velocityX": -0.3100328528472012,
-                    "velocityY": -3.77021427659835,
-                    "timestamp": 0.7986319331811629
-                },
-                {
-                    "x": 2.102010874647989,
-                    "y": 4.340090223140139,
-                    "heading": -2.9725028244681693,
-                    "angularVelocity": -2.697704390106331e-7,
-                    "velocityX": -0.3100323735461829,
-                    "velocityY": -3.7702143160122867,
-                    "timestamp": 0.8456102821918196
-                },
-                {
-                    "x": 2.087446088035901,
-                    "y": 4.162971777312504,
-                    "heading": -2.9725028371415325,
-                    "angularVelocity": -2.697703054465384e-7,
-                    "velocityX": -0.3100318959438893,
-                    "velocityY": -3.7702143552864653,
-                    "timestamp": 0.8925886312024762
-                },
-                {
-                    "x": 2.0728813234455363,
-                    "y": 3.9858533296739824,
-                    "heading": -2.97250284981489,
-                    "angularVelocity": -2.69770174050259e-7,
-                    "velocityX": -0.31003142718065585,
-                    "velocityY": -3.770214393833725,
-                    "timestamp": 0.9395669802131329
-                },
-                {
-                    "x": 2.0583165802454695,
-                    "y": 3.8087348802765,
-                    "heading": -2.972502862488243,
-                    "angularVelocity": -2.697700772639611e-7,
-                    "velocityX": -0.3100309718581832,
-                    "velocityY": -3.770214431275663,
-                    "timestamp": 0.9865453292237896
-                },
-                {
-                    "x": 2.0437518576774885,
-                    "y": 3.631616429182409,
-                    "heading": -2.972502875161594,
-                    "angularVelocity": -2.6977003935784156e-7,
-                    "velocityX": -0.31003053267531316,
-                    "velocityY": -3.7702144673903546,
-                    "timestamp": 1.0335236782344461
-                },
-                {
-                    "x": 2.02918715486261,
-                    "y": 3.4544979764639927,
-                    "heading": -2.972502887834946,
-                    "angularVelocity": -2.697700540709368e-7,
-                    "velocityX": -0.31003011220283416,
-                    "velocityY": -3.770214501966405,
-                    "timestamp": 1.0805020272451027
-                },
-                {
-                    "x": 2.014622470919118,
-                    "y": 3.2773795221937574,
-                    "heading": -2.972502900508301,
-                    "angularVelocity": -2.6977012019750386e-7,
-                    "velocityX": -0.3100297104989377,
-                    "velocityY": -3.770214534999038,
-                    "timestamp": 1.1274803762557593
-                },
-                {
-                    "x": 2.000060409435934,
-                    "y": 3.1002608522940482,
-                    "heading": -2.972502913181681,
-                    "angularVelocity": -2.697706573776715e-7,
-                    "velocityX": -0.30997388775603324,
-                    "velocityY": -3.77021912497458,
-                    "timestamp": 1.1744587252664158
-                },
-                {
-                    "x": 2.001608653690626,
-                    "y": 2.9225513149097844,
-                    "heading": -2.972502926099156,
-                    "angularVelocity": -2.749665584566826e-7,
-                    "velocityX": 0.03295654886341899,
-                    "velocityY": -3.782796567498611,
-                    "timestamp": 1.2214370742770724
-                },
-                {
-                    "x": 2.0215247790201643,
-                    "y": 2.7459545305120523,
-                    "heading": -2.9725029398381673,
-                    "angularVelocity": -2.9245411249129853e-7,
-                    "velocityX": 0.4239426405772788,
-                    "velocityY": -3.759110060629698,
-                    "timestamp": 1.268415423287729
-                },
-                {
-                    "x": 2.0595955339413297,
-                    "y": 2.572363939972743,
-                    "heading": -2.9725029551587165,
-                    "angularVelocity": -3.2611936426906016e-7,
-                    "velocityX": 0.8103893755936399,
-                    "velocityY": -3.695119011098735,
-                    "timestamp": 1.3153937722983855
-                },
-                {
-                    "x": 2.1154127951527757,
-                    "y": 2.4036407715704846,
-                    "heading": -2.972502973192088,
-                    "angularVelocity": -3.8386557912539916e-7,
-                    "velocityX": 1.1881486341460188,
-                    "velocityY": -3.5915091091001314,
-                    "timestamp": 1.362372121309042
-                },
-                {
-                    "x": 2.1883781192124445,
-                    "y": 2.241594069488506,
-                    "heading": -2.9725029958666975,
-                    "angularVelocity": -4.826608466149902e-7,
-                    "velocityX": 1.5531691853010408,
-                    "velocityY": -3.4493911662416843,
-                    "timestamp": 1.4093504703196986
-                },
-                {
-                    "x": 2.277709186746539,
-                    "y": 2.0879612953868114,
-                    "heading": -2.9725030269682566,
-                    "angularVelocity": -6.620402725580348e-7,
-                    "velocityX": 1.901536972145005,
-                    "velocityY": -3.2702889168549634,
-                    "timestamp": 1.4563288193303552
-                },
-                {
-                    "x": 2.382448196411133,
-                    "y": 1.9443897008895872,
-                    "heading": -2.9725030765065807,
-                    "angularVelocity": -0.0000010544926575738783,
-                    "velocityX": 2.2295166150014207,
-                    "velocityY": -3.0561226079838564,
-                    "timestamp": 1.5033071683410117
-                },
-                {
-                    "x": 2.5069799488821,
-                    "y": 1.8074960632068364,
-                    "heading": -2.9725031217478985,
-                    "angularVelocity": -9.247979619471774e-7,
-                    "velocityX": 2.5456091262787446,
-                    "velocityY": -2.798303938555362,
-                    "timestamp": 1.5522273859759206
-                },
-                {
-                    "x": 2.645527130072252,
-                    "y": 1.6848065781413901,
-                    "heading": -2.9725031505695743,
-                    "angularVelocity": -5.891567373541898e-7,
-                    "velocityX": 2.8321047593068385,
-                    "velocityY": -2.507950516105164,
-                    "timestamp": 1.6011476036108294
-                },
-                {
-                    "x": 2.7964788901009507,
-                    "y": 1.5777477131393498,
-                    "heading": -2.972503171586243,
-                    "angularVelocity": -4.2961111591525744e-7,
-                    "velocityX": 3.085672291060715,
-                    "velocityY": -2.188438036008342,
-                    "timestamp": 1.6500678212457383
-                },
-                {
-                    "x": 2.9580801560836862,
-                    "y": 1.4875641931626062,
-                    "heading": -2.9725031883859017,
-                    "angularVelocity": -3.434093106618729e-7,
-                    "velocityX": 3.3033635947567466,
-                    "velocityY": -1.8434815774897553,
-                    "timestamp": 1.698988038880647
-                },
-                {
-                    "x": 3.1284520354522702,
-                    "y": 1.4153045196097347,
-                    "heading": -2.9725032027821,
-                    "angularVelocity": -2.9427911753262145e-7,
-                    "velocityX": 3.482647616984601,
-                    "velocityY": -1.4770922339746126,
-                    "timestamp": 1.747908256515556
-                },
-                {
-                    "x": 3.305613655866368,
-                    "y": 1.3618087375272205,
-                    "heading": -2.9725032158448177,
-                    "angularVelocity": -2.670208303687875e-7,
-                    "velocityX": 3.6214397437119468,
-                    "velocityY": -1.0935311547825342,
-                    "timestamp": 1.7968284741504648
-                },
-                {
-                    "x": 3.4875051596049977,
-                    "y": 1.327698333409946,
-                    "heading": -2.9725032283101815,
-                    "angularVelocity": -2.548100678766224e-7,
-                    "velocityX": 3.7181253995246735,
-                    "velocityY": -0.6972659928016005,
-                    "timestamp": 1.8457486917853736
-                },
-                {
-                    "x": 3.671044139168993,
-                    "y": 1.3040027251815136,
-                    "heading": -2.972503240670167,
-                    "angularVelocity": -2.5265597445839956e-7,
-                    "velocityX": 3.751802188079885,
-                    "velocityY": -0.48437250229081197,
-                    "timestamp": 1.8946689094202824
-                },
-                {
-                    "x": 3.854583175253026,
-                    "y": 1.2803075547441987,
-                    "heading": -2.972503253030148,
-                    "angularVelocity": -2.5265589039445086e-7,
-                    "velocityX": 3.751803343431197,
-                    "velocityY": -0.48436355320721586,
-                    "timestamp": 1.9435891270551913
-                },
-                {
-                    "x": 4.038122213757311,
-                    "y": 1.256612403053765,
-                    "heading": -2.972503265390126,
-                    "angularVelocity": -2.5265581959003427e-7,
-                    "velocityX": 3.751803392904627,
-                    "velocityY": -0.4843631699938504,
-                    "timestamp": 1.9925093446901
-                },
-                {
-                    "x": 4.221661254747456,
-                    "y": 1.2329172706184417,
-                    "heading": -2.9725032777500995,
-                    "angularVelocity": -2.526557374147453e-7,
-                    "velocityX": 3.7518034437192385,
-                    "velocityY": -0.48436277639154923,
-                    "timestamp": 2.041429562325009
-                },
-                {
-                    "x": 4.405200298259638,
-                    "y": 1.2092221577184428,
-                    "heading": -2.972503290110069,
-                    "angularVelocity": -2.526556474640549e-7,
-                    "velocityX": 3.7518034952732924,
-                    "velocityY": -0.4843623770612628,
-                    "timestamp": 2.090349779959918
-                },
-                {
-                    "x": 4.5887393442969575,
-                    "y": 1.1855270643778184,
-                    "heading": -2.9725033024700336,
-                    "angularVelocity": -2.526555535451317e-7,
-                    "velocityX": 3.7518035468907667,
-                    "velocityY": -0.4843619772393562,
-                    "timestamp": 2.1392699975948273
-                },
-                {
-                    "x": 4.772278392830339,
-                    "y": 1.1618319903713676,
-                    "heading": -2.9725033148299937,
-                    "angularVelocity": -2.5265545524016583e-7,
-                    "velocityX": 3.7518035979138844,
-                    "velocityY": -0.48436158202088675,
-                    "timestamp": 2.1881902152297363
-                },
-                {
-                    "x": 4.955817443801929,
-                    "y": 1.1381369352509825,
-                    "heading": -2.972503327189949,
-                    "angularVelocity": -2.526553571562985e-7,
-                    "velocityX": 3.751803647754394,
-                    "velocityY": -0.4843611959623946,
-                    "timestamp": 2.2371104328646454
-                },
-                {
-                    "x": 5.139356497135768,
-                    "y": 1.114441898428302,
-                    "heading": -2.9725033395499,
-                    "angularVelocity": -2.526552791781282e-7,
-                    "velocityX": 3.751803696042169,
-                    "velocityY": -0.48436082193084695,
-                    "timestamp": 2.2860306504995545
-                },
-                {
-                    "x": 5.322895552747796,
-                    "y": 1.0907468792522317,
-                    "heading": -2.97250335190985,
-                    "angularVelocity": -2.5265524884530903e-7,
-                    "velocityX": 3.7518037426116595,
-                    "velocityY": -0.4843604612086112,
-                    "timestamp": 2.3349508681344635
-                },
-                {
-                    "x": 5.506434610543511,
-                    "y": 1.067051876990783,
-                    "heading": -2.9725033642697998,
-                    "angularVelocity": -2.5265526208757776e-7,
-                    "velocityX": 3.7518037872493903,
-                    "velocityY": -0.4843601154492814,
-                    "timestamp": 2.3838710857693726
-                },
-                {
-                    "x": 5.689973670421635,
-                    "y": 1.043356890859468,
-                    "heading": -2.972503376629753,
-                    "angularVelocity": -2.52655316177873e-7,
-                    "velocityX": 3.751803829816819,
-                    "velocityY": -0.48435978572601984,
-                    "timestamp": 2.4327913034042816
-                },
-                {
-                    "x": 5.873512732277465,
-                    "y": 1.0196619200472952,
-                    "heading": -2.9725033889897103,
-                    "angularVelocity": -2.526554076637863e-7,
-                    "velocityX": 3.751803870244009,
-                    "velocityY": -0.4843594725805982,
-                    "timestamp": 2.4817115210391907
-                },
-                {
-                    "x": 6.057051796000616,
-                    "y": 0.9959669636992331,
-                    "heading": -2.9725034013496736,
-                    "angularVelocity": -2.5265551720355475e-7,
-                    "velocityX": 3.751803908414743,
-                    "velocityY": -0.48435917691325625,
-                    "timestamp": 2.5306317386740997
-                },
-                {
-                    "x": 6.240590861474878,
-                    "y": 0.972272020915138,
-                    "heading": -2.9725034137096418,
-                    "angularVelocity": -2.526556193143771e-7,
-                    "velocityX": 3.7518039442099766,
-                    "velocityY": -0.48435889964615636,
-                    "timestamp": 2.579551956309009
-                },
-                {
-                    "x": 6.424129928585653,
-                    "y": 0.9485770908073501,
-                    "heading": -2.972503426069614,
-                    "angularVelocity": -2.5265571512549996e-7,
-                    "velocityX": 3.7518039776626577,
-                    "velocityY": -0.4843586405241034,
-                    "timestamp": 2.628472173943918
-                },
-                {
-                    "x": 6.607668997222064,
-                    "y": 0.9248821725170546,
-                    "heading": -2.972503438429591,
-                    "angularVelocity": -2.526558029491111e-7,
-                    "velocityX": 3.7518040088489135,
-                    "velocityY": -0.4843583989574683,
-                    "timestamp": 2.677392391578827
-                },
-                {
-                    "x": 6.791208068059123,
-                    "y": 0.9011872712731527,
-                    "heading": -2.972503450789653,
-                    "angularVelocity": -2.5265754822248404e-7,
-                    "velocityX": 3.751804053833296,
-                    "velocityY": -0.48435805050452835,
-                    "timestamp": 2.726312609213736
-                },
-                {
-                    "x": 6.968893183591214,
-                    "y": 0.8784347773708095,
-                    "heading": -2.9889996672823953,
-                    "angularVelocity": -0.3372065229932763,
-                    "velocityX": 3.632140741035789,
-                    "velocityY": -0.4650938814733942,
-                    "timestamp": 2.775232826848645
-                },
-                {
-                    "x": 7.126993065244261,
-                    "y": 0.8581341417430071,
-                    "heading": -3.0160355228350078,
-                    "angularVelocity": -0.5526519884760304,
-                    "velocityX": 3.231790235132359,
-                    "velocityY": -0.4149743523077879,
-                    "timestamp": 2.824153044483554
-                },
-                {
-                    "x": 7.265300475634767,
-                    "y": 0.8403578347966174,
-                    "heading": -3.044055057812944,
-                    "angularVelocity": -0.5727598185896393,
-                    "velocityX": 2.827203497390216,
-                    "velocityY": -0.363373423214384,
-                    "timestamp": 2.873073262118463
-                },
-                {
-                    "x": 7.3838209572244,
-                    "y": 0.8251160468222167,
-                    "heading": -3.0703114433567618,
-                    "angularVelocity": -0.5367184941769645,
-                    "velocityX": 2.4227300555805176,
-                    "velocityY": -0.31156418984375384,
-                    "timestamp": 2.921993479753372
-                },
-                {
-                    "x": 7.4825676028202075,
-                    "y": 0.812412258635403,
-                    "heading": -3.093486935010673,
-                    "angularVelocity": -0.47374056728180086,
-                    "velocityX": 2.018524249682452,
-                    "velocityY": -0.25968380356812476,
-                    "timestamp": 2.9709136973882813
-                },
-                {
-                    "x": 7.561551267898866,
-                    "y": 0.8022480583211733,
-                    "heading": -3.1128054575324193,
-                    "angularVelocity": -0.39489854002532226,
-                    "velocityX": 1.6145403454276093,
-                    "velocityY": -0.20777095453836575,
-                    "timestamp": 3.0198339150231903
-                },
-                {
-                    "x": 7.62078038482921,
-                    "y": 0.7946243025369127,
-                    "heading": -3.1277548992054633,
-                    "angularVelocity": -0.30558820863414116,
-                    "velocityX": 1.210728810987095,
-                    "velocityY": -0.1558405941927034,
-                    "timestamp": 3.0687541326580994
-                },
-                {
-                    "x": 7.6602615463029124,
-                    "y": 0.7895415063368129,
-                    "heading": -3.137971889882548,
-                    "angularVelocity": -0.208850065903912,
-                    "velocityX": 0.8070520406992162,
-                    "velocityY": -0.10389970539445781,
-                    "timestamp": 3.1176743502930084
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -0.10656978952845035,
-                    "velocityX": 0.40348254058055355,
-                    "velocityY": -0.051952065213203134,
-                    "timestamp": 3.1665945679279175
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": 1.1636456429625425e-30,
-                    "velocityX": -1.050271820667376e-32,
-                    "velocityY": -1.190420457247715e-32,
-                    "timestamp": 3.2155147855628265
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -13341,7 +11786,7 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -13350,7 +11795,7 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": 3.5331853071795862,
+                    "heading": 0.393,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -13361,101 +11806,101 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "angularVelocity": 0,
-                    "velocityX": 7.733955963828693e-33,
-                    "velocityY": 1.1033116642945717e-34,
+                    "velocityX": -1.6570559277738772e-34,
+                    "velocityY": -1.2609465104041817e-35,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.2520020198561768,
-                    "y": 5.607878032622388,
-                    "heading": 3.1558282209608954,
-                    "angularVelocity": 0.2083481053076329,
-                    "velocityX": 0.026352743296497748,
-                    "velocityY": 0.6302222724446542,
-                    "timestamp": 0.07597007391799751
+                    "x": 2.25200199902585,
+                    "y": 5.607878029194235,
+                    "heading": 0.015821088838010276,
+                    "angularVelocity": 0.20825462300812525,
+                    "velocityX": 0.026352519517453685,
+                    "velocityY": 0.6302234329322915,
+                    "timestamp": 0.0759699285878182
                 },
                 {
-                    "x": 2.2560055614843093,
-                    "y": 5.703634205053828,
-                    "heading": 3.187479734547443,
-                    "angularVelocity": 0.41663133855460127,
-                    "velocityX": 0.052698930271593884,
-                    "velocityY": 1.2604459557957022,
-                    "timestamp": 0.15194014783599502
+                    "x": 2.2560055011308777,
+                    "y": 5.703634196198671,
+                    "heading": 0.04745824894413579,
+                    "angularVelocity": 0.4164431992265759,
+                    "velocityX": 0.05269851083774254,
+                    "velocityY": 1.2604482955877103,
+                    "timestamp": 0.1519398571756364
                 },
                 {
-                    "x": 2.2620099218556464,
-                    "y": 5.847268779856728,
-                    "heading": 3.234941916913622,
-                    "angularVelocity": 0.6247484031331855,
-                    "velocityX": 0.07903586322448838,
-                    "velocityY": 1.8906730952761694,
-                    "timestamp": 0.22791022175399253
+                    "x": 2.2620098071430843,
+                    "y": 5.847268765721161,
+                    "heading": 0.09489872499810467,
+                    "angularVelocity": 0.6244638758496344,
+                    "velocityX": 0.07903529888495084,
+                    "velocityY": 1.8906766426199095,
+                    "timestamp": 0.2279097857634546
                 },
                 {
-                    "x": 2.270014272624616,
-                    "y": 6.038782285632903,
-                    "heading": 3.2981885303415055,
-                    "angularVelocity": 0.8325200985871338,
-                    "velocityX": 0.10536189260009911,
-                    "velocityY": 2.520907192783506,
-                    "timestamp": 0.30388029567199004
+                    "x": 2.270014094994434,
+                    "y": 6.038782268539179,
+                    "heading": 0.1581161373911332,
+                    "angularVelocity": 0.8321373149634034,
+                    "velocityX": 0.10536126596587263,
+                    "velocityY": 2.5209119763306935,
+                    "timestamp": 0.3038797143512728
                 },
                 {
-                    "x": 2.2800095231252646,
-                    "y": 6.278187238938093,
-                    "heading": 3.376569786131263,
-                    "angularVelocity": 1.0317385747756829,
-                    "velocityX": 0.13156826083172354,
-                    "velocityY": 3.151306046688924,
-                    "timestamp": 0.3798503695899875
+                    "x": 2.28000939681014,
+                    "y": 6.278187229695624,
+                    "heading": 0.23645967758045489,
+                    "angularVelocity": 1.031244094151802,
+                    "velocityX": 0.13156918798668546,
+                    "velocityY": 3.1513121784720814,
+                    "timestamp": 0.379849642939091
                 },
                 {
-                    "x": 2.2880056735413383,
-                    "y": 6.469712014906391,
-                    "heading": 3.4392336964848362,
-                    "angularVelocity": 0.8248499326354866,
-                    "velocityX": 0.10525395071624018,
-                    "velocityY": 2.5210555432002106,
-                    "timestamp": 0.455820443507985
+                    "x": 2.2880055947499205,
+                    "y": 6.469712010307998,
+                    "heading": 0.29909347070783976,
+                    "angularVelocity": 0.824455074417803,
+                    "velocityX": 0.10525477762609038,
+                    "velocityY": 2.521060427100167,
+                    "timestamp": 0.4558195715269092
                 },
                 {
-                    "x": 2.2940028218748654,
-                    "y": 6.613355955414726,
-                    "heading": 3.486212731686583,
-                    "angularVelocity": 0.6183886993772851,
-                    "velocityX": 0.07894093060907224,
-                    "velocityY": 1.8907963767862817,
-                    "timestamp": 0.5317905174259825
+                    "x": 2.29400278172627,
+                    "y": 6.613355953306298,
+                    "heading": 0.3460499566805711,
+                    "angularVelocity": 0.6180930645268612,
+                    "velocityX": 0.07894159028222533,
+                    "velocityY": 1.8908000266480898,
+                    "timestamp": 0.5317895001147275
                 },
                 {
-                    "x": 2.29800094256259,
-                    "y": 6.709118670546653,
-                    "heading": 3.517527025119772,
-                    "angularVelocity": 0.412192483411159,
-                    "velocityX": 0.052627574010790594,
-                    "velocityY": 1.2605320778717877,
-                    "timestamp": 0.60776059134398
+                    "x": 2.298000929157321,
+                    "y": 6.709118669792275,
+                    "heading": 0.37734923040208523,
+                    "angularVelocity": 0.4119955659209739,
+                    "velocityX": 0.052628026712297535,
+                    "velocityY": 1.2605345070882814,
+                    "timestamp": 0.6077594287025456
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": 3.5331853071795862,
-                    "angularVelocity": 0.20611118631681047,
-                    "velocityX": 0.026313748747538647,
-                    "velocityY": 0.6302656688873288,
-                    "timestamp": 0.6837306652619775
+                    "heading": 0.393,
+                    "angularVelocity": 0.20601269329644184,
+                    "velocityX": 0.026313975540574065,
+                    "velocityY": 0.6302668845130636,
+                    "timestamp": 0.6837293572903638
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": 3.5331853071795862,
-                    "angularVelocity": -1.7034359069625615e-32,
-                    "velocityX": 1.3795062534340913e-33,
-                    "velocityY": 0,
-                    "timestamp": 0.7597007391799749
+                    "heading": 0.393,
+                    "angularVelocity": 1.653751635278537e-33,
+                    "velocityX": 2.0986524182646467e-33,
+                    "velocityY": -2.349121999954591e-34,
+                    "timestamp": 0.7596992858781819
                 }
             ],
             "constraints": [
@@ -13498,7 +11943,7 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -13507,7 +11952,7 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -13518,101 +11963,101 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": -6.311192632400173e-33,
                     "angularVelocity": 0,
-                    "velocityX": -1.4697340738263578e-34,
-                    "velocityY": 1.0017538611600725e-35,
+                    "velocityX": 0,
+                    "velocityY": 1.0022615189154172e-34,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.2520018980434946,
-                    "y": 5.512001884856013,
-                    "heading": 3.124302281529173,
-                    "angularVelocity": -0.20638131373671467,
-                    "velocityX": 0.026319388320742283,
-                    "velocityY": -0.6310416433261579,
-                    "timestamp": 0.07606172374138878
+                    "x": 2.2520019144790657,
+                    "y": 5.512001876818988,
+                    "heading": -0.015697245285858377,
+                    "angularVelocity": -0.2063751026737087,
+                    "velocityX": 0.02631960567841837,
+                    "velocityY": -0.6310417795758498,
+                    "timestamp": 0.07606172005484914
                 },
                 {
-                    "x": 2.2560052212232096,
-                    "y": 5.416005549224728,
-                    "heading": 3.092911780798619,
-                    "angularVelocity": -0.4126977300341266,
-                    "velocityX": 0.05263255922684882,
-                    "velocityY": -1.2620846716237228,
-                    "timestamp": 0.15212344748277756
+                    "x": 2.256005269562975,
+                    "y": 5.416005526715618,
+                    "heading": -0.04708689462727081,
+                    "angularVelocity": -0.41268655663817416,
+                    "velocityX": 0.05263298122921632,
+                    "velocityY": -1.2620849230617632,
+                    "timestamp": 0.15212344010969828
                 },
                 {
-                    "x": 2.262009303064208,
-                    "y": 5.272010738006526,
-                    "heading": 3.045840934307484,
-                    "angularVelocity": -0.618850640976494,
-                    "velocityX": 0.07893696784222709,
-                    "velocityY": -1.8931310537713693,
-                    "timestamp": 0.22818517122416634
+                    "x": 2.26200939656581,
+                    "y": 5.272010697248206,
+                    "heading": -0.09415666260883829,
+                    "angularVelocity": -0.6188364915705936,
+                    "velocityX": 0.07893756542062844,
+                    "velocityY": -1.8931313854535041,
+                    "timestamp": 0.22818516016454743
                 },
                 {
-                    "x": 2.2700133593824035,
-                    "y": 5.080016941067703,
-                    "heading": 2.9831153695364567,
-                    "angularVelocity": -0.8246666218648364,
-                    "velocityX": 0.10523106661912582,
-                    "velocityY": -2.524184142757597,
-                    "timestamp": 0.3042468949655551
+                    "x": 2.2700135068749114,
+                    "y": 5.080016881567182,
+                    "heading": -0.15688110795154597,
+                    "angularVelocity": -0.8246519444666291,
+                    "velocityX": 0.10523178154963624,
+                    "velocityY": -2.5241845115068955,
+                    "timestamp": 0.30424688021939655
                 },
                 {
-                    "x": 2.2800088926734445,
-                    "y": 4.840012206333479,
-                    "heading": 2.90536283495199,
-                    "angularVelocity": -1.0222294573395003,
-                    "velocityX": 0.1314134468609495,
-                    "velocityY": -3.1553943682665504,
-                    "timestamp": 0.3803086187069439
+                    "x": 2.280008999433411,
+                    "y": 4.840012166841925,
+                    "heading": -0.23463480916086144,
+                    "angularVelocity": -1.0222448447556298,
+                    "velocityX": 0.13141291771066904,
+                    "velocityY": -3.155394258139129,
+                    "timestamp": 0.3803086002742457
                 },
                 {
-                    "x": 2.288005283531118,
-                    "y": 4.648007638478312,
-                    "heading": 2.8432005737453654,
-                    "angularVelocity": -0.8172607475736078,
-                    "velocityX": 0.10513028714497022,
-                    "velocityY": -2.524325750333843,
-                    "timestamp": 0.45637034244833263
+                    "x": 2.2880053509029836,
+                    "y": 4.648007615057375,
+                    "heading": -0.29679802026954394,
+                    "angularVelocity": -0.8172732757536348,
+                    "velocityX": 0.10512977439645693,
+                    "velocityY": -2.524325661398308,
+                    "timestamp": 0.4563703203290948
                 },
                 {
-                    "x": 2.2940026200964168,
-                    "y": 4.504003869225784,
-                    "heading": 2.7965971516354546,
-                    "angularVelocity": -0.6127053111281423,
-                    "velocityX": 0.07884828623776054,
-                    "velocityY": -1.8932488269940233,
-                    "timestamp": 0.5324320661897214
+                    "x": 2.2940026546256975,
+                    "y": 4.504003857619058,
+                    "heading": -0.34340214900619426,
+                    "angularVelocity": -0.6127146310002387,
+                    "velocityX": 0.07884785827074504,
+                    "velocityY": -1.893248763431504,
+                    "timestamp": 0.532432040383944
                 },
                 {
-                    "x": 2.2980008723577208,
-                    "y": 4.408001271885206,
-                    "heading": 2.7655331089053763,
-                    "angularVelocity": -0.40840571580650337,
-                    "velocityX": 0.052565890761270766,
-                    "velocityY": -1.262166995675615,
-                    "timestamp": 0.6084937899311101
+                    "x": 2.2980008838862798,
+                    "y": 4.408001268016137,
+                    "heading": -0.37446665790253814,
+                    "angularVelocity": -0.4084118643904297,
+                    "velocityX": 0.05256559091352319,
+                    "velocityY": -1.26216695512135,
+                    "timestamp": 0.6084937604387931
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": -0.20421715603224325,
-                    "velocityX": 0.026282965254326718,
-                    "velocityY": -0.6310831456885047,
-                    "timestamp": 0.6845555136724989
+                    "heading": -0.39,
+                    "angularVelocity": -0.20422023175732293,
+                    "velocityX": 0.02628281495972678,
+                    "velocityY": -0.6310831254081802,
+                    "timestamp": 0.6845554804936422
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "angularVelocity": 0,
-                    "velocityX": 2.533367966722586e-34,
-                    "velocityY": 7.359145603780692e-32,
-                    "timestamp": 0.7606172374138876
+                    "velocityX": -1.9690468515346402e-33,
+                    "velocityY": 7.92544572453684e-31,
+                    "timestamp": 0.7606172005484914
                 }
             ],
             "constraints": [
@@ -13655,7 +12100,7 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -13664,7 +12109,7 @@
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": 3.4031853071795863,
+                    "heading": 0.263,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -13675,146 +12120,146 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 3.1925891483075246e-34,
+                    "heading": 0,
+                    "angularVelocity": 4.4337460853781175e-35,
                     "velocityX": 0,
                     "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.3070751616194327,
-                    "y": 5.580871513694386,
-                    "heading": 3.1487661024318196,
-                    "angularVelocity": 0.1026391987797076,
-                    "velocityX": 0.668272918826128,
-                    "velocityY": 0.24437718582154933,
-                    "timestamp": 0.08540696474681245
-                },
-                {
-                    "x": 2.4212254758610405,
-                    "y": 5.622614541188738,
-                    "heading": 3.166299183933703,
-                    "angularVelocity": 0.20528866180714583,
-                    "velocityX": 1.3365457323065477,
-                    "velocityY": 0.48875437287928064,
-                    "timestamp": 0.1708139294936249
-                },
-                {
-                    "x": 2.592450926566031,
-                    "y": 5.685229083532584,
-                    "heading": 3.1926007136716263,
-                    "angularVelocity": 0.307955326780356,
-                    "velocityX": 2.0048183565893676,
-                    "velocityY": 0.7331315722255889,
-                    "timestamp": 0.25622089424043737
-                },
-                {
-                    "x": 2.820751488987388,
-                    "y": 5.768715144253157,
-                    "heading": 3.2276726330952217,
-                    "angularVelocity": 0.4106447235019468,
-                    "velocityX": 2.6730906911181096,
-                    "velocityY": 0.9775088128710195,
-                    "timestamp": 0.3416278589872498
-                },
-                {
-                    "x": 3.106127123163594,
-                    "y": 5.873072729004609,
-                    "heading": 3.271517021699339,
-                    "angularVelocity": 0.5133584682946327,
-                    "velocityX": 3.341362557751549,
-                    "velocityY": 1.2218861197188997,
-                    "timestamp": 0.4270348237340622
-                },
-                {
-                    "x": 3.4095640006306436,
-                    "y": 5.984036075660027,
-                    "heading": 3.2715170269491542,
-                    "angularVelocity": 6.146823646853408e-8,
-                    "velocityX": 3.5528352794948703,
-                    "velocityY": 1.2992306538976923,
-                    "timestamp": 0.5124417884808746
-                },
-                {
-                    "x": 3.7130008780817154,
-                    "y": 6.094999422359182,
-                    "heading": 3.271517032198949,
-                    "angularVelocity": 6.146799253439994e-8,
-                    "velocityX": 3.5528352793077906,
-                    "velocityY": 1.299230654409779,
-                    "timestamp": 0.597848753227687
-                },
-                {
-                    "x": 4.016437755532787,
-                    "y": 6.205962769058336,
-                    "heading": 3.271517037448744,
-                    "angularVelocity": 6.146799251320457e-8,
-                    "velocityX": 3.55283527930779,
-                    "velocityY": 1.2992306544097794,
-                    "timestamp": 0.6832557179744995
-                },
-                {
-                    "x": 4.319874632996578,
-                    "y": 6.3169261157226675,
-                    "heading": 3.2715170426985596,
-                    "angularVelocity": 6.146823670561877e-8,
-                    "velocityX": 3.5528352794567044,
-                    "velocityY": 1.2992306540020593,
-                    "timestamp": 0.7686626827213119
-                },
-                {
-                    "x": 4.605249758095724,
-                    "y": 6.421284043313642,
-                    "heading": 3.315404813019647,
-                    "angularVelocity": 0.5138664095040972,
-                    "velocityX": 3.3413565971479624,
-                    "velocityY": 1.221890133905845,
-                    "timestamp": 0.8540696474681243
-                },
-                {
-                    "x": 4.833549856620357,
-                    "y": 6.504770412469219,
-                    "heading": 3.3505164790016466,
-                    "angularVelocity": 0.41111010192303765,
-                    "velocityX": 2.673085259515143,
-                    "velocityY": 0.9775124242275821,
-                    "timestamp": 0.9394766122149367
-                },
-                {
-                    "x": 5.004774927843149,
-                    "y": 6.567385202446666,
-                    "heading": 3.3768507903488065,
-                    "angularVelocity": 0.3083391550704053,
-                    "velocityX": 2.0048139133662572,
-                    "velocityY": 0.7331344716800138,
-                    "timestamp": 1.0248835769617493
-                },
-                {
-                    "x": 5.118924975241942,
-                    "y": 6.609128400529722,
-                    "heading": 3.3944071451755278,
-                    "angularVelocity": 0.20556116095176463,
-                    "velocityX": 1.3365426079382265,
-                    "velocityY": 0.4887563702422027,
-                    "timestamp": 1.1102905417085618
+                    "x": 2.3070751783284065,
+                    "y": 5.580871519986164,
+                    "heading": 0.00875997642576413,
+                    "angularVelocity": 0.10256751513529143,
+                    "velocityX": 0.6682733985254588,
+                    "velocityY": 0.24437736336610263,
+                    "timestamp": 0.08540692844327274
+                },
+                {
+                    "x": 2.421225525991661,
+                    "y": 5.622614560146346,
+                    "heading": 0.026280802198455293,
+                    "angularVelocity": 0.20514525100066663,
+                    "velocityX": 1.3365466917484683,
+                    "velocityY": 0.48875472893171557,
+                    "timestamp": 0.1708138568865455
+                },
+                {
+                    "x": 2.592451026856299,
+                    "y": 5.685229121649564,
+                    "heading": 0.052563939294491345,
+                    "angularVelocity": 0.30774010463909035,
+                    "velocityX": 2.0048197960703633,
+                    "velocityY": 0.7331321081849562,
+                    "timestamp": 0.25622078532981823
+                },
+                {
+                    "x": 2.8207516562305845,
+                    "y": 5.768715208168554,
+                    "heading": 0.0876113178799828,
+                    "angularVelocity": 0.41035755792072426,
+                    "velocityX": 2.673092611285307,
+                    "velocityY": 0.9775095304409794,
+                    "timestamp": 0.341627713773091
+                },
+                {
+                    "x": 3.106127374237044,
+                    "y": 5.873072825511933,
+                    "heading": 0.13142500230326418,
+                    "angularVelocity": 0.5129991819385286,
+                    "velocityX": 3.341364959588794,
+                    "velocityY": 1.221887020708095,
+                    "timestamp": 0.4270346422163637
+                },
+                {
+                    "x": 3.40956412390744,
+                    "y": 5.984036121763709,
+                    "heading": 0.13142500754936084,
+                    "angularVelocity": 6.142471997127927e-8,
+                    "velocityX": 3.5528352933560723,
+                    "velocityY": 1.299230615997127,
+                    "timestamp": 0.5124415706596365
+                },
+                {
+                    "x": 3.7130008735619784,
+                    "y": 6.094999418058892,
+                    "heading": 0.1314250127954367,
+                    "angularVelocity": 6.142447634082945e-8,
+                    "velocityX": 3.5528352931703995,
+                    "velocityY": 1.2992306165053664,
+                    "timestamp": 0.5978484991029092
+                },
+                {
+                    "x": 4.016437623216516,
+                    "y": 6.205962714354075,
+                    "heading": 0.13142501804151255,
+                    "angularVelocity": 6.142447606684555e-8,
+                    "velocityX": 3.552835293170399,
+                    "velocityY": 1.2992306165053664,
+                    "timestamp": 0.683255427546182
+                },
+                {
+                    "x": 4.3198743728836435,
+                    "y": 6.316926010614789,
+                    "heading": 0.13142502328760913,
+                    "angularVelocity": 6.142471942956278e-8,
+                    "velocityX": 3.5528352933178016,
+                    "velocityY": 1.2992306161017806,
+                    "timestamp": 0.7686623559894548
+                },
+                {
+                    "x": 4.605249584503867,
+                    "y": 6.421283973215489,
+                    "heading": 0.17528171773503587,
+                    "angularVelocity": 0.5135027713419801,
+                    "velocityX": 3.3413590304886087,
+                    "velocityY": 1.2218910632059061,
+                    "timestamp": 0.8540692844327276
+                },
+                {
+                    "x": 4.83354975240058,
+                    "y": 6.5047703704067485,
+                    "heading": 0.21036850612417926,
+                    "angularVelocity": 0.4108189935954435,
+                    "velocityX": 2.6730872080050085,
+                    "velocityY": 0.9775131679944534,
+                    "timestamp": 0.9394762128760004
+                },
+                {
+                    "x": 5.004774875721697,
+                    "y": 6.567385181417454,
+                    "heading": 0.23668415165777779,
+                    "angularVelocity": 0.3081207346202291,
+                    "velocityX": 2.0048153755446823,
+                    "velocityY": 0.7331350295812797,
+                    "timestamp": 1.0248831413192732
+                },
+                {
+                    "x": 5.118924957871047,
+                    "y": 6.6091283935214395,
+                    "heading": 0.25422806054578806,
+                    "angularVelocity": 0.20541552316405948,
+                    "velocityX": 1.3365435829385781,
+                    "velocityY": 0.48875674216187737,
+                    "timestamp": 1.110290069762546
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": 0.10278040005380541,
-                    "velocityX": 0.6682713163646097,
-                    "velocityY": 0.24437819014118428,
-                    "timestamp": 1.1956975064553743
+                    "heading": 0.263,
+                    "angularVelocity": 0.10270758607175808,
+                    "velocityX": 0.6682718038134577,
+                    "velocityY": 0.2443783760754632,
+                    "timestamp": 1.1956969982058188
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": -1.3221581062655587e-34,
-                    "velocityX": 2.469071735487476e-35,
+                    "heading": 0.263,
+                    "angularVelocity": 1.86588452749739e-32,
+                    "velocityX": -1.0770205854918045e-34,
                     "velocityY": 0,
-                    "timestamp": 1.2811044712021868
+                    "timestamp": 1.2811039266490916
                 }
             ],
             "constraints": [
@@ -13841,7 +12286,7 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -13850,7 +12295,7 @@
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -13861,8 +12306,8 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": -1.2603829918079947e-39,
+                    "heading": 0,
+                    "angularVelocity": 0,
                     "velocityX": 0,
                     "velocityY": 0,
                     "timestamp": 0
@@ -13870,91 +12315,91 @@
                 {
                     "x": 2.3319036557469652,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 1.854813108024866e-16,
+                    "heading": -1.815106931159875e-19,
+                    "angularVelocity": -1.831407645203353e-18,
                     "velocityX": 0.8263919813283321,
-                    "velocityY": 1.2609253605375368e-21,
+                    "velocityY": 8.319303153097966e-34,
                     "timestamp": 0.099109935233537
                 },
                 {
                     "x": 2.4957109641637625,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 3.709616393980982e-16,
+                    "heading": -5.438547016393454e-19,
+                    "angularVelocity": -3.655980680963076e-18,
                     "velocityX": 1.6527839316089838,
-                    "velocityY": 2.5218434283644083e-21,
+                    "velocityY": 1.3987786573815764e-33,
                     "timestamp": 0.198219870467074
                 },
                 {
                     "x": 2.741421919096125,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 5.564379296417211e-16,
+                    "heading": -1.0860329369861945e-18,
+                    "angularVelocity": -5.4704731072674304e-18,
                     "velocityX": 2.4791758197942864,
-                    "velocityY": 3.782763291910568e-21,
+                    "velocityY": 2.272018181833636e-33,
                     "timestamp": 0.297329805700611
                 },
                 {
                     "x": 3.0690365020812695,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 7.41869431396362e-16,
+                    "heading": -1.80338172905721e-18,
+                    "angularVelocity": -7.237910009033217e-18,
                     "velocityX": 3.3055675216936846,
-                    "velocityY": 5.0436652946129374e-21,
+                    "velocityY": 3.0574601440031788e-33,
                     "timestamp": 0.396439740934148
                 },
                 {
                     "x": 3.4439634979187304,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 5.10358456113492e-16,
-                    "velocityX": 3.7829405796099507,
-                    "velocityY": -2.5218394750934443e-20,
+                    "heading": 1.7928981709293245e-18,
+                    "angularVelocity": 3.6285765816640505e-17,
+                    "velocityX": 3.7829405796099502,
+                    "velocityY": 2.997011014948752e-33,
                     "timestamp": 0.495549676167685
                 },
                 {
                     "x": 3.771578080903875,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 7.390704390186125e-16,
-                    "velocityX": 3.3055675216936846,
-                    "velocityY": 5.043665294641482e-21,
+                    "heading": 1.0808328664707184e-18,
+                    "angularVelocity": -7.184600643588333e-18,
+                    "velocityX": 3.305567521693685,
+                    "velocityY": -4.3766441461200414e-33,
                     "timestamp": 0.594659611401222
                 },
                 {
                     "x": 4.017289035836238,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 5.549819222123311e-16,
+                    "heading": 5.416591039281022e-19,
+                    "angularVelocity": -5.4401585595028644e-18,
                     "velocityX": 2.479175819794286,
-                    "velocityY": 3.782763291928162e-21,
+                    "velocityY": -5.119323199602909e-33,
                     "timestamp": 0.6937695466347591
                 },
                 {
                     "x": 4.181096344253034,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 3.701778935205264e-16,
+                    "heading": 1.8086099379948042e-19,
+                    "angularVelocity": -3.64038286599245e-18,
                     "velocityX": 1.6527839316089838,
-                    "velocityY": 2.5218434283731754e-21,
+                    "velocityY": -5.9637817525889115e-33,
                     "timestamp": 0.792879481868296
                 },
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 1.8514488061077656e-16,
+                    "heading": 0,
+                    "angularVelocity": -1.8248523050897816e-18,
                     "velocityX": 0.8263919813283321,
-                    "velocityY": 1.260925360546343e-21,
+                    "velocityY": -3.8230331267939286e-33,
                     "timestamp": 0.891989417101833
                 },
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "angularVelocity": 0,
                     "velocityX": 0,
-                    "velocityY": 3.337507404137172e-39,
+                    "velocityY": 2.0684896902092895e-40,
                     "timestamp": 0.9910993523353699
                 }
             ],
@@ -13982,7 +12427,7 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -13991,16 +12436,16 @@
                 {
                     "x": 2.2392399311065674,
                     "y": 2.659268856048584,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
-                    "controlIntervalCount": 23
+                    "controlIntervalCount": 29
                 },
                 {
                     "x": 4.263,
                     "y": 3,
-                    "heading": 2.574,
+                    "heading": -0.566,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -14011,470 +12456,524 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "angularVelocity": 0,
                     "velocityX": 0,
-                    "velocityY": 1.712760733525474e-32,
+                    "velocityY": -4.99624931676796e-32,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.2447404981946972,
-                    "y": 5.548452449977017,
-                    "heading": 3.1382782323729086,
-                    "angularVelocity": -0.0441204598358695,
-                    "velocityX": -0.13477523592974824,
-                    "velocityY": -0.2959070718806661,
-                    "timestamp": 0.03902424484008447
-                },
-                {
-                    "x": 2.2342214959104107,
-                    "y": 5.525357349752352,
-                    "heading": 3.134834682439341,
-                    "angularVelocity": -0.08824129583236773,
-                    "velocityX": -0.269550437872442,
-                    "velocityY": -0.5918141483404872,
-                    "timestamp": 0.07804848968016893
-                },
-                {
-                    "x": 2.218442994608338,
-                    "y": 5.490714699360087,
-                    "heading": 3.129669313928024,
-                    "angularVelocity": -0.13236306128370928,
-                    "velocityX": -0.40432560237181914,
-                    "velocityY": -0.8877212239269984,
-                    "timestamp": 0.11707273452025341
-                },
-                {
-                    "x": 2.1974049958845625,
-                    "y": 5.44452449907313,
-                    "heading": 3.1227820694139163,
-                    "angularVelocity": -0.17648629825721382,
-                    "velocityX": -0.539100725971382,
-                    "velocityY": -1.1836282925201553,
-                    "timestamp": 0.15609697936033787
-                },
-                {
-                    "x": 2.17110750147078,
-                    "y": 5.386786749443186,
-                    "heading": 3.114172871086463,
-                    "angularVelocity": -0.22061152913360044,
-                    "velocityX": -0.6738758051961007,
-                    "velocityY": -1.479535346975897,
-                    "timestamp": 0.19512122420042233
-                },
-                {
-                    "x": 2.139550513239859,
-                    "y": 5.317501451364342,
-                    "heading": 3.10384162215273,
-                    "angularVelocity": -0.2647392403381948,
-                    "velocityX": -0.8086508364284021,
-                    "velocityY": -1.775442378520474,
-                    "timestamp": 0.2341454690405068
-                },
-                {
-                    "x": 2.1027340332257456,
-                    "y": 5.236668606182096,
-                    "heading": 3.091788209481888,
-                    "angularVelocity": -0.30886985053106986,
-                    "velocityX": -0.9434258155405923,
-                    "velocityY": -2.0713493755866326,
-                    "timestamp": 0.27316971388059125
-                },
-                {
-                    "x": 2.0606580636823133,
-                    "y": 5.1442882159029235,
-                    "heading": 3.078012508890752,
-                    "angularVelocity": -0.35300364292993813,
-                    "velocityX": -1.078200736897071,
-                    "velocityY": -2.3672563212366358,
-                    "timestamp": 0.31219395872067573
-                },
-                {
-                    "x": 2.013322607256609,
-                    "y": 5.040360283671343,
-                    "heading": 3.0625143970359345,
-                    "angularVelocity": -0.3971405960148595,
-                    "velocityX": -1.212975590422751,
-                    "velocityY": -2.6631631863080094,
-                    "timestamp": 0.3512182035607602
-                },
-                {
-                    "x": 1.960727667582776,
-                    "y": 4.92488481518758,
-                    "heading": 3.0452937850632114,
-                    "angularVelocity": -0.44127982599779075,
-                    "velocityX": -1.3477503508231259,
-                    "velocityY": -2.9590699053106113,
-                    "timestamp": 0.3902424484008447
-                },
-                {
-                    "x": 1.902873252468991,
-                    "y": 4.797861825841284,
-                    "heading": 3.026350773365474,
-                    "angularVelocity": -0.4854164834032721,
-                    "velocityX": -1.482524911138253,
-                    "velocityY": -3.25497622995194,
-                    "timestamp": 0.4292666932409292
-                },
-                {
-                    "x": 1.8416825412927555,
-                    "y": 4.663514351826141,
-                    "heading": 3.02635076853487,
-                    "angularVelocity": -1.2378468871067517e-7,
-                    "velocityX": -1.5680178162827485,
-                    "velocityY": -3.4426668489212484,
-                    "timestamp": 0.4682909380810137
-                },
-                {
-                    "x": 1.7807776273702332,
-                    "y": 4.529037073556694,
-                    "heading": 3.0263507637038423,
-                    "angularVelocity": -1.237955425988301e-7,
-                    "velocityX": -1.5606942343751464,
-                    "velocityY": -3.445993095328429,
-                    "timestamp": 0.5073151829210981
-                },
-                {
-                    "x": 1.7316376139293153,
-                    "y": 4.389829293964471,
-                    "heading": 3.02635075879599,
-                    "angularVelocity": -1.2576418143070467e-7,
-                    "velocityX": -1.2592175362335267,
-                    "velocityY": -3.5672126433882343,
-                    "timestamp": 0.5463394277611826
-                },
-                {
-                    "x": 1.6946418503710443,
-                    "y": 4.246913724355564,
-                    "heading": 3.026350753656809,
-                    "angularVelocity": -1.3169201974173754e-7,
-                    "velocityX": -0.9480199734770174,
-                    "velocityY": -3.662225116579624,
-                    "timestamp": 0.5853636726012671
-                },
-                {
-                    "x": 1.6700643070030454,
-                    "y": 4.101347620951498,
-                    "heading": 3.02635074809997,
-                    "angularVelocity": -1.423945209571316e-7,
-                    "velocityX": -0.6298018954293696,
-                    "velocityY": -3.7301452981493113,
-                    "timestamp": 0.6243879174413516
-                },
-                {
-                    "x": 1.6580868892212808,
-                    "y": 3.9542079360929074,
-                    "heading": 3.0263507418753948,
-                    "angularVelocity": -1.595053312793056e-7,
-                    "velocityX": -0.3069224742421484,
-                    "velocityY": -3.7704684731645255,
-                    "timestamp": 0.6634121622814361
-                },
-                {
-                    "x": 1.6587982403438444,
-                    "y": 3.806583282470554,
-                    "heading": 3.0263507346134717,
-                    "angularVelocity": -1.8608747530006848e-7,
-                    "velocityX": 0.018228440434743125,
-                    "velocityY": -3.7828958440399596,
-                    "timestamp": 0.7024364071215206
-                },
-                {
-                    "x": 1.6721931105563068,
-                    "y": 3.659565885100826,
-                    "heading": 3.0263506736368497,
-                    "angularVelocity": -0.0000015625317616489644,
-                    "velocityX": 0.34324482811563534,
-                    "velocityY": -3.767334844586533,
-                    "timestamp": 0.741460651961605
-                },
-                {
-                    "x": 1.6979565964079348,
-                    "y": 3.5152981288336416,
-                    "heading": 3.0233678381587312,
-                    "angularVelocity": -0.07643544392291168,
-                    "velocityX": 0.6601917848050372,
-                    "velocityY": -3.6968750288025483,
-                    "timestamp": 0.7804848968016895
-                },
-                {
-                    "x": 1.7349981432705297,
-                    "y": 3.376556575471776,
-                    "heading": 3.013707601859302,
-                    "angularVelocity": -0.2475444775167341,
-                    "velocityX": 0.9491931750224177,
-                    "velocityY": -3.5552655517207494,
-                    "timestamp": 0.819509141641774
-                },
-                {
-                    "x": 1.7820959712007873,
-                    "y": 3.2454658997474337,
-                    "heading": 2.9995738602901865,
-                    "angularVelocity": -0.3621784771756839,
-                    "velocityX": 1.2068863375389995,
-                    "velocityY": -3.3592110817655803,
-                    "timestamp": 0.8585333864818585
-                },
-                {
-                    "x": 1.8383454622663515,
-                    "y": 3.1231345959204413,
-                    "heading": 2.9823282080134397,
-                    "angularVelocity": -0.44192148617905314,
-                    "velocityX": 1.441398579167467,
-                    "velocityY": -3.1347513405650798,
-                    "timestamp": 0.897557631321943
-                },
-                {
-                    "x": 1.9031002792591356,
-                    "y": 3.0102116491438333,
-                    "heading": 2.962874002379933,
-                    "angularVelocity": -0.4985158768152219,
-                    "velocityX": 1.6593483681274654,
-                    "velocityY": -2.893661292853978,
-                    "timestamp": 0.9365818761620275
-                },
-                {
-                    "x": 1.9758873013923517,
-                    "y": 2.9071136453404876,
-                    "heading": 2.9418434761111434,
-                    "angularVelocity": -0.5389092435989264,
-                    "velocityX": 1.8651743917531916,
-                    "velocityY": -2.6418961910942005,
-                    "timestamp": 0.975606121002112
-                },
-                {
-                    "x": 2.0563490115304868,
-                    "y": 2.814127161728483,
-                    "heading": 2.919699686688593,
-                    "angularVelocity": -0.5674367182066591,
-                    "velocityX": 2.0618390046462407,
-                    "velocityY": -2.3827875207591935,
-                    "timestamp": 1.0146303658421965
-                },
-                {
-                    "x": 2.1442072150434264,
-                    "y": 2.731459952378173,
-                    "heading": 2.8967947458212753,
-                    "angularVelocity": -0.586941296652354,
-                    "velocityX": 2.2513748535810034,
-                    "velocityY": -2.11835513253537,
-                    "timestamp": 1.0536546106822808
+                    "x": 2.2447003077480563,
+                    "y": 5.548424158968356,
+                    "heading": -0.001651505925702968,
+                    "angularVelocity": -0.042250731032121805,
+                    "velocityX": -0.13558284496887443,
+                    "velocityY": -0.2961465280181771,
+                    "timestamp": 0.03908822132445813
+                },
+                {
+                    "x": 2.234100924566356,
+                    "y": 5.525272476708423,
+                    "heading": -0.0049545293965937804,
+                    "angularVelocity": -0.08450175932731885,
+                    "velocityX": -0.27116565611205723,
+                    "velocityY": -0.5922930610671583,
+                    "timestamp": 0.07817644264891627
+                },
+                {
+                    "x": 2.2182018518980375,
+                    "y": 5.490544953203296,
+                    "heading": -0.009909100000268336,
+                    "angularVelocity": -0.12675354456654908,
+                    "velocityX": -0.4067484303351874,
+                    "velocityY": -0.8884395945486457,
+                    "timestamp": 0.1172646639733744
+                },
+                {
+                    "x": 2.1970030913105703,
+                    "y": 5.444241588640874,
+                    "heading": -0.016515264878529855,
+                    "angularVelocity": -0.16900653584180803,
+                    "velocityX": -0.5423311644575062,
+                    "velocityY": -1.1845861232230646,
+                    "timestamp": 0.15635288529783253
+                },
+                {
+                    "x": 2.170504644501127,
+                    "y": 5.386362383452732,
+                    "heading": -0.024773088013563174,
+                    "angularVelocity": -0.21126116398317848,
+                    "velocityX": -0.6779138551608025,
+                    "velocityY": -1.480732640856411,
+                    "timestamp": 0.19544110662229067
+                },
+                {
+                    "x": 2.1387065133083243,
+                    "y": 5.31690733837683,
+                    "heading": -0.03468264893636917,
+                    "angularVelocity": -0.2535178267784212,
+                    "velocityX": -0.8134964988264785,
+                    "velocityY": -1.7768791396104282,
+                    "timestamp": 0.2345293279467488
+                },
+                {
+                    "x": 2.1016086997403236,
+                    "y": 5.235876454566401,
+                    "heading": -0.04624404029676127,
+                    "angularVelocity": -0.2957768598481422,
+                    "velocityX": -0.9490790911170489,
+                    "velocityY": -2.0730256088611454,
+                    "timestamp": 0.27361754927120696
+                },
+                {
+                    "x": 2.0592112060450187,
+                    "y": 5.143269733801358,
+                    "heading": -0.05945736298631538,
+                    "angularVelocity": -0.33803847404262444,
+                    "velocityX": -1.0846616258994186,
+                    "velocityY": -2.369172032575987,
+                    "timestamp": 0.31270577059566507
+                },
+                {
+                    "x": 2.011514034901109,
+                    "y": 5.039087178972333,
+                    "heading": -0.07432271508161363,
+                    "angularVelocity": -0.3803025973454465,
+                    "velocityX": -1.2202440921521842,
+                    "velocityY": -2.6653183823394446,
+                    "timestamp": 0.3517939919201232
+                },
+                {
+                    "x": 1.9585171900495353,
+                    "y": 4.923328795522167,
+                    "heading": -0.09084016031123929,
+                    "angularVelocity": -0.4225683510264515,
+                    "velocityX": -1.3558264626999135,
+                    "velocityY": -2.9614645928578263,
+                    "timestamp": 0.3908822132445813
+                },
+                {
+                    "x": 1.9002206796290688,
+                    "y": 4.795994598697166,
+                    "heading": -0.10900958241827326,
+                    "angularVelocity": -0.46483113048874997,
+                    "velocityX": -1.491408625032185,
+                    "velocityY": -3.257610413327559,
+                    "timestamp": 0.4299704345690394
+                },
+                {
+                    "x": 1.8386671955063139,
+                    "y": 4.661546724325594,
+                    "heading": -0.10900958703871873,
+                    "angularVelocity": -1.182055697467884e-7,
+                    "velocityX": -1.5747322860207698,
+                    "velocityY": -3.439600724104782,
+                    "timestamp": 0.4690586558934975
+                },
+                {
+                    "x": 1.7777943297173011,
+                    "y": 4.526789325478927,
+                    "heading": -0.10900959166091434,
+                    "angularVelocity": -1.1825034389504455e-7,
+                    "velocityX": -1.5573199221351706,
+                    "velocityY": -3.447519336526256,
+                    "timestamp": 0.5081468772179556
+                },
+                {
+                    "x": 1.728739704549767,
+                    "y": 4.38729485969642,
+                    "heading": -0.10900959636164997,
+                    "angularVelocity": -1.2025964513922604e-7,
+                    "velocityX": -1.2549720479816096,
+                    "velocityY": -3.5687084511883413,
+                    "timestamp": 0.5472350985424137
+                },
+                {
+                    "x": 1.6918738865194354,
+                    "y": 4.244095790476986,
+                    "heading": -0.10900960128999353,
+                    "angularVelocity": -1.2608257379035844e-7,
+                    "velocityX": -0.9431439134643449,
+                    "velocityY": -3.6634838927766116,
+                    "timestamp": 0.5863233198668718
+                },
+                {
+                    "x": 1.6674707596733624,
+                    "y": 4.098254956364247,
+                    "heading": -0.10900960662650709,
+                    "angularVelocity": -1.3652485001388814e-7,
+                    "velocityX": -0.6243089610931135,
+                    "velocityY": -3.7310685718382324,
+                    "timestamp": 0.6254115411913299
+                },
+                {
+                    "x": 1.6557115284805652,
+                    "y": 3.9508548842430677,
+                    "heading": -0.1090096126146074,
+                    "angularVelocity": -1.531944945007552e-7,
+                    "velocityX": -0.3008382268193907,
+                    "velocityY": -3.7709587985000237,
+                    "timestamp": 0.664499762515788
+                },
+                {
+                    "x": 1.6566835066189385,
+                    "y": 3.8029896922639588,
+                    "heading": -0.10900961961577929,
+                    "angularVelocity": -1.79112061302596e-7,
+                    "velocityX": 0.024866266754091815,
+                    "velocityY": -3.7828580316236344,
+                    "timestamp": 0.7035879838402461
+                },
+                {
+                    "x": 1.6703794917330834,
+                    "y": 3.6557569809887474,
+                    "heading": -0.10900969829200789,
+                    "angularVelocity": -0.000002012786100988185,
+                    "velocityX": 0.3503865013576149,
+                    "velocityY": -3.766677180142832,
+                    "timestamp": 0.7426762051647042
+                },
+                {
+                    "x": 1.6964297342683634,
+                    "y": 3.5114772969924033,
+                    "heading": -0.1124967438020359,
+                    "angularVelocity": -0.0892096235606636,
+                    "velocityX": 0.6664473760284892,
+                    "velocityY": -3.6911294274233937,
+                    "timestamp": 0.7817644264891623
+                },
+                {
+                    "x": 1.7337062034817727,
+                    "y": 3.3729422790966246,
+                    "heading": -0.12243166775651844,
+                    "angularVelocity": -0.2541666931326176,
+                    "velocityX": 0.9536496660724848,
+                    "velocityY": -3.54416274779685,
+                    "timestamp": 0.8208526478136204
+                },
+                {
+                    "x": 1.7809992442628195,
+                    "y": 3.2421845220219927,
+                    "heading": -0.1367067464310455,
+                    "angularVelocity": -0.3652015413039525,
+                    "velocityX": 1.2099051626962352,
+                    "velocityY": -3.3451958836719715,
+                    "timestamp": 0.8599408691380785
+                },
+                {
+                    "x": 1.8374227958909133,
+                    "y": 3.120270451905998,
+                    "heading": -0.154016140064332,
+                    "angularVelocity": -0.4428288892862246,
+                    "velocityX": 1.443492431128254,
+                    "velocityY": -3.118946475052685,
+                    "timestamp": 0.8990290904625367
+                },
+                {
+                    "x": 1.9023445230612375,
+                    "y": 3.007828092475748,
+                    "heading": -0.17349166731783877,
+                    "angularVelocity": -0.4982454200677405,
+                    "velocityX": 1.660902567846671,
+                    "velocityY": -2.8766302384778886,
+                    "timestamp": 0.9381173117869948
+                },
+                {
+                    "x": 1.9753011558084586,
+                    "y": 2.90526251520286,
+                    "heading": -0.19452421295735298,
+                    "angularVelocity": -0.5380788617862533,
+                    "velocityX": 1.866460797529241,
+                    "velocityY": -2.623950995915806,
+                    "timestamp": 0.9772055331114529
+                },
+                {
+                    "x": 2.0559421744907826,
+                    "y": 2.8128534355639654,
+                    "heading": -0.21666661660279088,
+                    "angularVelocity": -0.5664725304751266,
+                    "velocityX": 2.0630516291077963,
+                    "velocityY": -2.3641157491367464,
+                    "timestamp": 1.016293754435911
+                },
+                {
+                    "x": 2.1439944855858766,
+                    "y": 2.7308042438703497,
+                    "heading": -0.2395781801271676,
+                    "angularVelocity": -0.586150066389446,
+                    "velocityX": 2.252655866948419,
+                    "velocityY": -2.099077136627731,
+                    "timestamp": 1.055381975760369
                 },
                 {
                     "x": 2.2392399311065674,
                     "y": 2.659268856048584,
-                    "heading": 2.8734047162081335,
-                    "angularVelocity": -0.5993717420795943,
-                    "velocityX": 2.4352224226909684,
-                    "velocityY": -1.849903736136748,
-                    "timestamp": 1.0926788555223652
-                },
-                {
-                    "x": 2.338491380229597,
-                    "y": 2.598983086121257,
-                    "heading": 2.850361085284195,
-                    "angularVelocity": -0.6059434386623828,
-                    "velocityX": 2.609864937185211,
-                    "velocityY": -1.5852435257599564,
-                    "timestamp": 1.1307081986155205
-                },
-                {
-                    "x": 2.444184622569516,
-                    "y": 2.5488913725009676,
-                    "heading": 2.8272194988809463,
-                    "angularVelocity": -0.608519225445519,
-                    "velocityX": 2.779255010558571,
-                    "velocityY": -1.3171858766423765,
-                    "timestamp": 1.1687375417086758
-                },
-                {
-                    "x": 2.5560703795596873,
-                    "y": 2.509147043381333,
-                    "heading": 2.804166943788384,
-                    "angularVelocity": -0.6061781039996946,
-                    "velocityX": 2.9420901832588977,
-                    "velocityY": -1.0450963883935758,
-                    "timestamp": 1.2067668848018311
-                },
-                {
-                    "x": 2.67382962144658,
-                    "y": 2.4799341019260455,
-                    "heading": 2.7814386686411225,
-                    "angularVelocity": -0.5976510057401518,
-                    "velocityX": 3.096536314035744,
-                    "velocityY": -0.7681684478149349,
-                    "timestamp": 1.2447962278949865
-                },
-                {
-                    "x": 2.7970412331199452,
-                    "y": 2.46147554775192,
-                    "heading": 2.7593387310325537,
-                    "angularVelocity": -0.5811285657613138,
-                    "velocityX": 3.239909018978801,
-                    "velocityY": -0.48537662427964845,
-                    "timestamp": 1.2828255709881418
-                },
-                {
-                    "x": 2.925127627515394,
-                    "y": 2.4540428102885845,
-                    "heading": 2.738273276977094,
-                    "angularVelocity": -0.5539263195754057,
-                    "velocityX": 3.3680937922509844,
-                    "velocityY": -0.19544743239797485,
-                    "timestamp": 1.320854914081297
-                },
-                {
-                    "x": 3.057257978674903,
-                    "y": 2.457961811901398,
-                    "heading": 2.7188066654897645,
-                    "angularVelocity": -0.5118839796850494,
-                    "velocityX": 3.47443159446217,
-                    "velocityY": 0.10305204597435572,
-                    "timestamp": 1.3588842571744524
-                },
-                {
-                    "x": 3.192166604008838,
-                    "y": 2.4735957350107154,
-                    "heading": 2.701757241393799,
-                    "angularVelocity": -0.4483228662195614,
-                    "velocityX": 3.5474876598173757,
-                    "velocityY": 0.41110158203408753,
-                    "timestamp": 1.3969136002676077
-                },
-                {
-                    "x": 3.3278072233432696,
-                    "y": 2.5012232638000396,
-                    "heading": 2.6883398256989803,
-                    "angularVelocity": -0.3528174457801884,
-                    "velocityX": 3.566735796675976,
-                    "velocityY": 0.7264792537080078,
-                    "timestamp": 1.434942943360763
-                },
-                {
-                    "x": 3.4613865978283758,
-                    "y": 2.5407156767305445,
-                    "heading": 2.677154917488752,
-                    "angularVelocity": -0.2941125799315481,
-                    "velocityX": 3.512534364790241,
-                    "velocityY": 1.0384721301591493,
-                    "timestamp": 1.4729722864539183
-                },
-                {
-                    "x": 3.5888930381916024,
-                    "y": 2.590617982563406,
-                    "heading": 2.6645014610802713,
-                    "angularVelocity": -0.3327287662446667,
-                    "velocityX": 3.35284362001453,
-                    "velocityY": 1.3122053071130373,
-                    "timestamp": 1.5110016295470736
-                },
-                {
-                    "x": 3.7058440395321557,
-                    "y": 2.646348954214802,
-                    "heading": 2.6512815676345496,
-                    "angularVelocity": -0.34762350255007635,
-                    "velocityX": 3.0752832373169756,
-                    "velocityY": 1.4654728985163021,
-                    "timestamp": 1.549030972640229
-                },
-                {
-                    "x": 3.810776558765862,
-                    "y": 2.7030629098701224,
-                    "heading": 2.6383327014335145,
-                    "angularVelocity": -0.34049670985152497,
-                    "velocityX": 2.7592514279479117,
-                    "velocityY": 1.491320939107423,
-                    "timestamp": 1.5870603157333842
-                },
-                {
-                    "x": 3.9038282779644375,
-                    "y": 2.757723211117941,
-                    "heading": 2.626141706261417,
-                    "angularVelocity": -0.3205681239939803,
-                    "velocityX": 2.446840035354867,
-                    "velocityY": 1.4373191015665727,
-                    "timestamp": 1.6250896588265396
-                },
-                {
-                    "x": 3.9854403181685942,
-                    "y": 2.8085799309132606,
-                    "heading": 2.6149760707773027,
-                    "angularVelocity": -0.2936057942615943,
-                    "velocityX": 2.1460281342290126,
-                    "velocityY": 1.337302084623056,
-                    "timestamp": 1.6631190019196949
-                },
-                {
-                    "x": 4.056026736802803,
-                    "y": 2.8545641040147816,
-                    "heading": 2.6049941853375604,
-                    "angularVelocity": -0.26247851337555306,
-                    "velocityX": 1.8561040736703007,
-                    "velocityY": 1.209176108797926,
-                    "timestamp": 1.7011483450128502
-                },
-                {
-                    "x": 4.115926253855544,
-                    "y": 2.894973650540783,
-                    "heading": 2.5962978923918545,
-                    "angularVelocity": -0.22867323593790462,
-                    "velocityX": 1.5750868193019991,
-                    "velocityY": 1.0625886023594207,
-                    "timestamp": 1.7391776881060055
-                },
-                {
-                    "x": 4.1654099219914915,
-                    "y": 2.9293183863277776,
-                    "heading": 2.588957208164297,
-                    "angularVelocity": -0.19302684796776065,
-                    "velocityX": 1.301197026063159,
-                    "velocityY": 0.9031114658716605,
-                    "timestamp": 1.7772070311991608
-                },
-                {
-                    "x": 4.20469589755594,
-                    "y": 2.957239109446396,
-                    "heading": 2.5830228338283105,
-                    "angularVelocity": -0.15604724808021303,
-                    "velocityX": 1.0330437596099378,
-                    "velocityY": 0.7341889406352728,
-                    "timestamp": 1.815236374292316
-                },
-                {
-                    "x": 4.23396207855711,
-                    "y": 2.9784623659467853,
-                    "heading": 2.578532998453876,
-                    "angularVelocity": -0.11806239627740928,
-                    "velocityX": 0.769568407465833,
-                    "velocityY": 0.5580758113124009,
-                    "timestamp": 1.8532657173854714
-                },
-                {
-                    "x": 4.253355635131929,
-                    "y": 2.992773543728212,
-                    "heading": 2.575517463166205,
-                    "angularVelocity": -0.0792949612693492,
-                    "velocityX": 0.5099629653689784,
-                    "velocityY": 0.3763193528315992,
-                    "timestamp": 1.8912950604786267
+                    "heading": -0.2629913490343549,
+                    "angularVelocity": -0.5989827143282399,
+                    "velocityX": 2.4366789353272003,
+                    "velocityY": -1.830100869209989,
+                    "timestamp": 1.0944701970848272
+                },
+                {
+                    "x": 2.3166964235934535,
+                    "y": 2.610513248595422,
+                    "heading": -0.2811797949710667,
+                    "angularVelocity": -0.6046182408202432,
+                    "velocityX": 2.5747998696792043,
+                    "velocityY": -1.620728330008343,
+                    "timestamp": 1.124552726371468
+                },
+                {
+                    "x": 2.398213254515867,
+                    "y": 2.5681176642029904,
+                    "heading": -0.29946946527559826,
+                    "angularVelocity": -0.6079831296849904,
+                    "velocityX": 2.7097731758410535,
+                    "velocityY": -1.409309170398157,
+                    "timestamp": 1.154635255658109
+                },
+                {
+                    "x": 2.483678189488588,
+                    "y": 2.5321524360945276,
+                    "heading": -0.31778023201379,
+                    "angularVelocity": -0.6086844149213316,
+                    "velocityX": 2.8410155993982453,
+                    "velocityY": -1.1955520018204469,
+                    "timestamp": 1.1847177849447499
+                },
+                {
+                    "x": 2.5729562458095865,
+                    "y": 2.5026984837196036,
+                    "heading": -0.33601688083426684,
+                    "angularVelocity": -0.6062205955726221,
+                    "velocityX": 2.9677709434035733,
+                    "velocityY": -0.9791049181495252,
+                    "timestamp": 1.2148003142313908
+                },
+                {
+                    "x": 2.665882293188928,
+                    "y": 2.479849558811985,
+                    "heading": -0.35406456410202075,
+                    "angularVelocity": -0.5999390242685017,
+                    "velocityX": 3.08903704518594,
+                    "velocityY": -0.7595413500609144,
+                    "timestamp": 1.2448828435180317
+                },
+                {
+                    "x": 2.7622502226261916,
+                    "y": 2.4637149411085275,
+                    "heading": -0.37178230702948656,
+                    "angularVelocity": -0.5889711851900935,
+                    "velocityX": 3.2034516951353296,
+                    "velocityY": -0.5363451174508257,
+                    "timestamp": 1.2749653728046726
+                },
+                {
+                    "x": 2.8617965704136163,
+                    "y": 2.4544224370162078,
+                    "heading": -0.3889934776908366,
+                    "angularVelocity": -0.57213176782302,
+                    "velocityX": 3.309108314626244,
+                    "velocityY": -0.30890035886875983,
+                    "timestamp": 1.3050479020913135
+                },
+                {
+                    "x": 2.96417486527032,
+                    "y": 2.452120989561553,
+                    "heading": -0.40547141981393114,
+                    "angularVelocity": -0.5477578685651567,
+                    "velocityX": 3.4032475754010525,
+                    "velocityY": -0.07650445322366382,
+                    "timestamp": 1.3351304313779544
+                },
+                {
+                    "x": 3.068913958464018,
+                    "y": 2.456980552046256,
+                    "heading": -0.4209173641629785,
+                    "angularVelocity": -0.5134523165215639,
+                    "velocityX": 3.4817249638713506,
+                    "velocityY": 0.16154102065001244,
+                    "timestamp": 1.3652129606645953
+                },
+                {
+                    "x": 3.175348297900835,
+                    "y": 2.469181868617686,
+                    "heading": -0.43492680962689906,
+                    "angularVelocity": -0.4657003847794821,
+                    "velocityX": 3.5380781457121575,
+                    "velocityY": 0.4055947708108251,
+                    "timestamp": 1.3952954899512362
+                },
+                {
+                    "x": 3.2825012821379658,
+                    "y": 2.488873812540701,
+                    "heading": -0.44694408076253883,
+                    "angularVelocity": -0.3994767534718848,
+                    "velocityX": 3.5619672540204803,
+                    "velocityY": 0.6545973490252659,
+                    "timestamp": 1.425378019237877
+                },
+                {
+                    "x": 3.3889723525651156,
+                    "y": 2.516050236824646,
+                    "heading": -0.4565561347315361,
+                    "angularVelocity": -0.3195227993435668,
+                    "velocityX": 3.539299153094488,
+                    "velocityY": 0.9033955896790181,
+                    "timestamp": 1.455460548524518
+                },
+                {
+                    "x": 3.4932758480756925,
+                    "y": 2.5504542246131905,
+                    "heading": -0.4662703499641935,
+                    "angularVelocity": -0.32291883239253294,
+                    "velocityX": 3.4672448754798637,
+                    "velocityY": 1.1436534295611822,
+                    "timestamp": 1.485543077811159
+                },
+                {
+                    "x": 3.5930108067726363,
+                    "y": 2.5908608883384305,
+                    "heading": -0.4766346317588353,
+                    "angularVelocity": -0.3445282707409342,
+                    "velocityX": 3.315378096922126,
+                    "velocityY": 1.3431936969204072,
+                    "timestamp": 1.5156256070977998
+                },
+                {
+                    "x": 3.686096614652251,
+                    "y": 2.634834387797071,
+                    "heading": -0.48720467819938723,
+                    "angularVelocity": -0.3513682755805826,
+                    "velocityX": 3.094347785475507,
+                    "velocityY": 1.4617620426674243,
+                    "timestamp": 1.5457081363844407
+                },
+                {
+                    "x": 3.771715868998783,
+                    "y": 2.679896340499022,
+                    "heading": -0.4975881116213702,
+                    "angularVelocity": -0.34516490694787005,
+                    "velocityX": 2.8461454663839936,
+                    "velocityY": 1.497944280967274,
+                    "timestamp": 1.5757906656710816
+                },
+                {
+                    "x": 3.8498203373891897,
+                    "y": 2.724287723719235,
+                    "heading": -0.507530883254748,
+                    "angularVelocity": -0.33051647814035945,
+                    "velocityX": 2.596339810598824,
+                    "velocityY": 1.475653286903962,
+                    "timestamp": 1.6058731949577225
+                },
+                {
+                    "x": 3.920599661686896,
+                    "y": 2.7668759161025953,
+                    "heading": -0.5168796397769617,
+                    "angularVelocity": -0.3107702957131982,
+                    "velocityX": 2.35283820796086,
+                    "velocityY": 1.415711823216824,
+                    "timestamp": 1.6359557242443634
+                },
+                {
+                    "x": 3.984278053809844,
+                    "y": 2.806919241781621,
+                    "heading": -0.525538519539268,
+                    "angularVelocity": -0.287837491315874,
+                    "velocityX": 2.11678983227083,
+                    "velocityY": 1.3311156551191636,
+                    "timestamp": 1.6660382535310043
+                },
+                {
+                    "x": 4.041059171114166,
+                    "y": 2.843909698702473,
+                    "heading": -0.5334440710807248,
+                    "angularVelocity": -0.26279544070665484,
+                    "velocityX": 1.8875114111345854,
+                    "velocityY": 1.229632540814369,
+                    "timestamp": 1.6961207828176452
+                },
+                {
+                    "x": 4.091116341717457,
+                    "y": 2.8774832028649056,
+                    "heading": -0.5405519914461838,
+                    "angularVelocity": -0.23628067632639732,
+                    "velocityX": 1.6639947434713853,
+                    "velocityY": 1.11604658778956,
+                    "timestamp": 1.7262033121042861
+                },
+                {
+                    "x": 4.1345948250930045,
+                    "y": 2.9073683770064043,
+                    "heading": -0.5468299348831553,
+                    "angularVelocity": -0.20869067813911088,
+                    "velocityX": 1.445306774615446,
+                    "velocityY": 0.9934395428236782,
+                    "timestamp": 1.756285841390927
+                },
+                {
+                    "x": 4.171616484177909,
+                    "y": 2.933356232054327,
+                    "heading": -0.552253409487953,
+                    "angularVelocity": -0.18028652288901045,
+                    "velocityX": 1.2306697595852323,
+                    "velocityY": 0.863885306993297,
+                    "timestamp": 1.786368370677568
+                },
+                {
+                    "x": 4.202284260430688,
+                    "y": 2.9552814196152344,
+                    "heading": -0.5568033106587852,
+                    "angularVelocity": -0.15124729464987938,
+                    "velocityX": 1.0194547127524736,
+                    "velocityY": 0.7288345787681281,
+                    "timestamp": 1.8164508999642088
+                },
+                {
+                    "x": 4.226685878711894,
+                    "y": 2.973010149818714,
+                    "heading": -0.5604643670760648,
+                    "angularVelocity": -0.12170041895060278,
+                    "velocityX": 0.8111558057069341,
+                    "velocityY": 0.5893364229633967,
+                    "timestamp": 1.8465334292508497
+                },
+                {
+                    "x": 4.244896778926994,
+                    "y": 2.98643211128414,
+                    "heading": -0.5632241207030344,
+                    "angularVelocity": -0.09173941461743668,
+                    "velocityX": 0.6053646633758383,
+                    "velocityY": 0.4461713088529227,
+                    "timestamp": 1.8766159585374906
+                },
+                {
+                    "x": 4.256982406189367,
+                    "y": 2.9954548894256625,
+                    "heading": -0.565072233243941,
+                    "angularVelocity": -0.06143474583855434,
+                    "velocityX": 0.4017490400230914,
+                    "velocityY": 0.2999341596429636,
+                    "timestamp": 1.9066984878241315
                 },
                 {
                     "x": 4.263,
                     "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": -0.03990242909239911,
-                    "velocityX": 0.25360324643122656,
-                    "velocityY": 0.190023168532958,
-                    "timestamp": 1.929324403571782
+                    "heading": -0.566,
+                    "angularVelocity": -0.030840716457660636,
+                    "velocityX": 0.20003616561938206,
+                    "velocityY": 0.15108804618886046,
+                    "timestamp": 1.9367810171107724
                 },
                 {
                     "x": 4.263,
                     "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": -5.006919316497686e-30,
-                    "velocityX": -1.2207915195710053e-31,
-                    "velocityY": 7.406976698260565e-32,
-                    "timestamp": 1.9673537466649373
+                    "heading": -0.566,
+                    "angularVelocity": -4.9732030554004765e-30,
+                    "velocityX": -4.223945033236224e-32,
+                    "velocityY": 9.969983495378875e-32,
+                    "timestamp": 1.9668635463974133
                 }
             ],
             "constraints": [
@@ -14512,7 +13011,7 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -14521,16 +13020,16 @@
                 {
                     "x": 2.008638858795166,
                     "y": 3.158904790878296,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
-                    "controlIntervalCount": 27
+                    "controlIntervalCount": 33
                 },
                 {
                     "x": 5.176,
                     "y": 1.62,
-                    "heading": 2.5,
+                    "heading": -0.64,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -14541,497 +13040,551 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "angularVelocity": 0,
                     "velocityX": 0,
-                    "velocityY": -4.120593510226553e-35,
+                    "velocityY": 6.585905017096791e-33,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.2473912878554576,
-                    "y": 5.551785895834254,
-                    "heading": 3.1358246059620716,
-                    "angularVelocity": -0.12928713365744599,
-                    "velocityX": -0.08077630820983339,
-                    "velocityY": -0.2543419791057163,
-                    "timestamp": 0.03229551092834574
-                },
-                {
-                    "x": 2.242172157117942,
-                    "y": 5.535355099551312,
-                    "heading": 3.127553990435098,
-                    "angularVelocity": -0.25609180004378934,
-                    "velocityX": -0.16160545498398599,
-                    "velocityY": -0.5087640916843489,
-                    "timestamp": 0.06459102185669148
-                },
-                {
-                    "x": 2.2343406441985514,
-                    "y": 5.510704667985898,
-                    "heading": 3.115281361261821,
-                    "angularVelocity": -0.38001037359361073,
-                    "velocityX": -0.24249540243429488,
-                    "velocityY": -0.7632773365965662,
-                    "timestamp": 0.09688653278503723
-                },
-                {
-                    "x": 2.2238944664634275,
-                    "y": 5.477831199999642,
-                    "heading": 3.0991173688763074,
-                    "angularVelocity": -0.5005027609371739,
-                    "velocityX": -0.3234560294866074,
-                    "velocityY": -1.017895894546866,
-                    "timestamp": 0.12918204371338296
-                },
-                {
-                    "x": 2.2108309419755985,
-                    "y": 5.43673070422605,
-                    "heading": 3.0791962484868605,
-                    "angularVelocity": -0.6168386818107923,
-                    "velocityX": -0.40449970018474013,
-                    "velocityY": -1.272638041391628,
-                    "timestamp": 0.1614775546417287
-                },
-                {
-                    "x": 2.19514688330712,
-                    "y": 5.387398428523646,
-                    "heading": 3.0556846451348814,
-                    "angularVelocity": -0.7280145963364562,
-                    "velocityX": -0.4856420665793569,
-                    "velocityY": -1.5275273338098838,
-                    "timestamp": 0.19377306557007443
-                },
-                {
-                    "x": 2.17683845193207,
-                    "y": 5.329828642238818,
-                    "heading": 3.028794799017408,
-                    "angularVelocity": -0.832618693574285,
-                    "velocityX": -0.5669032893045367,
-                    "velocityY": -1.782594070505913,
-                    "timestamp": 0.22606857649842016
-                },
-                {
-                    "x": 2.1559009488672944,
-                    "y": 5.264014369161548,
-                    "heading": 2.9988052787110364,
-                    "angularVelocity": -0.9285971778836174,
-                    "velocityX": -0.6483100116059459,
-                    "velocityY": -2.037876818957682,
-                    "timestamp": 0.2583640874267659
-                },
-                {
-                    "x": 2.132328497947263,
-                    "y": 5.189947104033879,
-                    "heading": 2.966095877804453,
-                    "angularVelocity": -1.012815712349497,
-                    "velocityX": -0.7298986838242586,
-                    "velocityY": -2.293422924690741,
-                    "timestamp": 0.2906595983551117
-                },
-                {
-                    "x": 2.1061135495185,
-                    "y": 5.107616699879427,
-                    "heading": 2.9312120529633434,
-                    "angularVelocity": -1.0801446962243946,
-                    "velocityX": -0.8117211239334865,
-                    "velocityY": -2.549283221966035,
-                    "timestamp": 0.32295510928345744
-                },
-                {
-                    "x": 2.0772461397099717,
-                    "y": 5.017012347335229,
-                    "heading": 2.89500074789377,
-                    "angularVelocity": -1.1212488679902226,
-                    "velocityX": -0.8938520859006283,
-                    "velocityY": -2.805478220958382,
-                    "timestamp": 0.3552506202118032
-                },
-                {
-                    "x": 2.0457134444911373,
-                    "y": 4.918129964629122,
-                    "heading": 2.858963174705018,
-                    "angularVelocity": -1.1158694243515237,
-                    "velocityX": -0.976380131864019,
-                    "velocityY": -3.0617996081715195,
-                    "timestamp": 0.38754613114014896
-                },
-                {
-                    "x": 2.0115103463196724,
-                    "y": 4.811035711883079,
-                    "heading": 2.8266343874462354,
-                    "angularVelocity": -1.0010303701499224,
-                    "velocityX": -1.0590666376931392,
-                    "velocityY": -3.3160724096811367,
-                    "timestamp": 0.4198416420684947
-                },
-                {
-                    "x": 1.9751456955721198,
-                    "y": 4.697500624139868,
-                    "heading": 2.816667462304015,
-                    "angularVelocity": -0.30861642549437257,
-                    "velocityX": -1.1259970721081056,
-                    "velocityY": -3.5155067834385165,
-                    "timestamp": 0.4521371529968405
-                },
-                {
-                    "x": 1.9382198046413848,
-                    "y": 4.581042609670271,
-                    "heading": 2.816667439698893,
-                    "angularVelocity": -6.999462561903041e-7,
-                    "velocityX": -1.1433753444143555,
-                    "velocityY": -3.6060124494695995,
-                    "timestamp": 0.48443266392518625
-                },
-                {
-                    "x": 1.901294488957654,
-                    "y": 4.4645844128058085,
-                    "heading": 2.816667417094136,
-                    "angularVelocity": -6.999349526609492e-7,
-                    "velocityX": -1.1433575324340735,
-                    "velocityY": -3.6060180971544047,
-                    "timestamp": 0.516728174853532
-                },
-                {
-                    "x": 1.8682421277834205,
-                    "y": 4.346968376660799,
-                    "heading": 2.816667394414886,
-                    "angularVelocity": -7.022415636891642e-7,
-                    "velocityX": -1.0234351531879067,
-                    "velocityY": -3.6418694971559726,
-                    "timestamp": 0.5490236857818777
-                },
-                {
-                    "x": 1.8436394280164332,
-                    "y": 4.227299259504863,
-                    "heading": 2.81666737130494,
-                    "angularVelocity": -7.15577656003266e-7,
-                    "velocityX": -0.7617993665303343,
-                    "velocityY": -3.705441211982887,
-                    "timestamp": 0.5813191967102234
-                },
-                {
-                    "x": 1.8276118980147091,
-                    "y": 4.1061831643448095,
-                    "heading": 2.8166673472723573,
-                    "angularVelocity": -7.441462228176694e-7,
-                    "velocityX": -0.4962773320813651,
-                    "velocityY": -3.7502455195328888,
-                    "timestamp": 0.6136147076385691
-                },
-                {
-                    "x": 1.8202409034217624,
-                    "y": 3.984233755462536,
-                    "heading": 2.8166673217380835,
-                    "angularVelocity": -7.906446726868519e-7,
-                    "velocityX": -0.22823588731266858,
-                    "velocityY": -3.776048292062723,
-                    "timestamp": 0.6459102185669148
-                },
-                {
-                    "x": 1.821563847305773,
-                    "y": 3.8620689519300933,
-                    "heading": 2.816667293968721,
-                    "angularVelocity": -8.598520920997921e-7,
-                    "velocityX": 0.04096370814339873,
-                    "velocityY": -3.782717784013104,
-                    "timestamp": 0.6782057294952605
-                },
-                {
-                    "x": 1.831574050917961,
-                    "y": 3.740307773577626,
-                    "heading": 2.816667262971477,
-                    "angularVelocity": -9.5980040013024e-7,
-                    "velocityX": 0.30995650245006834,
-                    "velocityY": -3.7702199114489767,
-                    "timestamp": 0.7105012404236062
-                },
-                {
-                    "x": 1.8502208046317663,
-                    "y": 3.6195671988548175,
-                    "heading": 2.816667227314634,
-                    "angularVelocity": -0.00000110408047182699,
-                    "velocityX": 0.5773791210542256,
-                    "velocityY": -3.738617883788745,
-                    "timestamp": 0.7427967513519519
-                },
-                {
-                    "x": 1.8774096306565946,
-                    "y": 3.5004590367763506,
-                    "heading": 2.8166671847943117,
-                    "angularVelocity": -0.0000013166016324392044,
-                    "velocityX": 0.841876324085786,
-                    "velocityY": -3.688071767705825,
-                    "timestamp": 0.7750922622802976
-                },
-                {
-                    "x": 1.913002764036886,
-                    "y": 3.383586826300032,
-                    "heading": 2.8166671317574883,
-                    "angularVelocity": -0.0000016422351652599994,
-                    "velocityX": 1.1021077653566822,
-                    "velocityY": -3.618837637702142,
-                    "timestamp": 0.8073877732086433
-                },
-                {
-                    "x": 1.9568198515105035,
-                    "y": 3.2695427783414357,
-                    "heading": 2.8166670615606066,
-                    "angularVelocity": -0.0000021735801416730847,
-                    "velocityX": 1.3567547381688734,
-                    "velocityY": -3.53126625590866,
-                    "timestamp": 0.839683284136989
+                    "x": 2.24734688866176,
+                    "y": 5.551784954914585,
+                    "heading": -0.0040460391001905256,
+                    "angularVelocity": -0.1252124844254258,
+                    "velocityX": -0.08210564799104332,
+                    "velocityY": -0.2542304163009863,
+                    "timestamp": 0.03231338407473799
+                },
+                {
+                    "x": 2.242039203719346,
+                    "y": 5.535352462397274,
+                    "heading": -0.012062241013264058,
+                    "angularVelocity": -0.24807683078112674,
+                    "velocityX": -0.16425654862202216,
+                    "velocityY": -0.5085351778477981,
+                    "timestamp": 0.06462676814947597
+                },
+                {
+                    "x": 2.2340752644376702,
+                    "y": 5.510699787230797,
+                    "heading": -0.023960373976211733,
+                    "angularVelocity": -0.3682106750388487,
+                    "velocityX": -0.24645946284226605,
+                    "velocityY": -0.7629245859689895,
+                    "timestamp": 0.09694015222421395
+                },
+                {
+                    "x": 2.223453118891146,
+                    "y": 5.477823766232339,
+                    "heading": -0.039635722986674345,
+                    "angularVelocity": -0.4851039115621088,
+                    "velocityX": -0.32872278316489917,
+                    "velocityY": -1.0174118848839497,
+                    "timestamp": 0.12925353629895195
+                },
+                {
+                    "x": 2.210170475892355,
+                    "y": 5.4367206851300205,
+                    "heading": -0.05896132885919571,
+                    "angularVelocity": -0.5980681511978081,
+                    "velocityX": -0.4110570087017019,
+                    "velocityY": -1.2720141291067089,
+                    "timestamp": 0.16156692037368994
+                },
+                {
+                    "x": 2.194224615527819,
+                    "y": 5.387386118415271,
+                    "heading": -0.08177969695024358,
+                    "angularVelocity": -0.706158415295445,
+                    "velocityX": -0.4934754072075649,
+                    "velocityY": -1.5267533292286215,
+                    "timestamp": 0.19388030444842794
+                },
+                {
+                    "x": 2.1756122677867826,
+                    "y": 5.329814722150547,
+                    "heading": -0.10789038891803496,
+                    "angularVelocity": -0.8080457282778595,
+                    "velocityX": -0.575995002503841,
+                    "velocityY": -1.7816579078058363,
+                    "timestamp": 0.22619368852316593
+                },
+                {
+                    "x": 2.1543294408518685,
+                    "y": 5.263999973874467,
+                    "heading": -0.13703048715481858,
+                    "angularVelocity": -0.901796548742399,
+                    "velocityX": -0.6586381322887348,
+                    "velocityY": -2.0367643365317547,
+                    "timestamp": 0.2585070725979039
+                },
+                {
+                    "x": 2.1303711674850674,
+                    "y": 5.189933880722087,
+                    "heading": -0.16884170077604577,
+                    "angularVelocity": -0.984459366671516,
+                    "velocityX": -0.7414349828352131,
+                    "velocityY": -2.2921181198809735,
+                    "timestamp": 0.2908204566726419
+                },
+                {
+                    "x": 2.103731122650155,
+                    "y": 5.107606803578156,
+                    "heading": -0.20280968351747503,
+                    "angularVelocity": -1.0512047473228812,
+                    "velocityX": -0.8244275738281073,
+                    "velocityY": -2.547770204244666,
+                    "timestamp": 0.3231338407473799
+                },
+                {
+                    "x": 2.0744011006982923,
+                    "y": 5.0170081629947525,
+                    "heading": -0.23813652479783928,
+                    "angularVelocity": -1.0932572459343544,
+                    "velocityX": -0.9076741044523434,
+                    "velocityY": -2.8037496900311267,
+                    "timestamp": 0.3554472248221179
+                },
+                {
+                    "x": 2.0423709680329383,
+                    "y": 4.918132526359071,
+                    "heading": -0.27341203074981035,
+                    "angularVelocity": -1.0916685751756738,
+                    "velocityX": -0.991234238768389,
+                    "velocityY": -3.05989729849995,
+                    "timestamp": 0.3877606088968559
+                },
+                {
+                    "x": 2.007638970172759,
+                    "y": 4.811032750261161,
+                    "heading": -0.30535515788271084,
+                    "angularVelocity": -0.9885416847402974,
+                    "velocityX": -1.0748486688935617,
+                    "velocityY": -3.3144091578337234,
+                    "timestamp": 0.42007399297159387
+                },
+                {
+                    "x": 1.9707337326379286,
+                    "y": 4.697468787945422,
+                    "heading": -0.31502999189110337,
+                    "angularVelocity": -0.2994064003329394,
+                    "velocityX": -1.1421037626226502,
+                    "velocityY": -3.5144558692173984,
+                    "timestamp": 0.45238737704633186
+                },
+                {
+                    "x": 1.9331993823421831,
+                    "y": 4.581134405528862,
+                    "heading": -0.31503001351145116,
+                    "angularVelocity": -6.690833652147531e-7,
+                    "velocityX": -1.1615728705149717,
+                    "velocityY": -3.6001918631452843,
+                    "timestamp": 0.48470076112106986
+                },
+                {
+                    "x": 1.8956658919440634,
+                    "y": 4.464799745676637,
+                    "heading": -0.31503003513146777,
+                    "angularVelocity": -6.690731172580015e-7,
+                    "velocityX": -1.1615462593242614,
+                    "velocityY": -3.600200448927068,
+                    "timestamp": 0.5170141451958078
+                },
+                {
+                    "x": 1.8630741103436357,
+                    "y": 4.346985095772027,
+                    "heading": -0.31503005685343444,
+                    "angularVelocity": -6.722281571161063e-7,
+                    "velocityX": -1.008615548437953,
+                    "velocityY": -3.646001595874795,
+                    "timestamp": 0.5493275292705457
+                },
+                {
+                    "x": 1.8389499213812899,
+                    "y": 4.227149627930832,
+                    "heading": -0.3150300790516974,
+                    "angularVelocity": -6.869680669187691e-7,
+                    "velocityX": -0.746569560976628,
+                    "velocityY": -3.708539704910713,
+                    "timestamp": 0.5816409133452837
+                },
+                {
+                    "x": 1.8234163358184028,
+                    "y": 4.105901026539528,
+                    "heading": -0.3150301022065626,
+                    "angularVelocity": -7.165719666567231e-7,
+                    "velocityX": -0.48071676822704484,
+                    "velocityY": -3.752271848434857,
+                    "timestamp": 0.6139542974200216
+                },
+                {
+                    "x": 1.8165522814590422,
+                    "y": 3.983854314068792,
+                    "heading": -0.3150301268891266,
+                    "angularVelocity": -7.638495563130961e-7,
+                    "velocityX": -0.21242140233547732,
+                    "velocityY": -3.7769709352772667,
+                    "timestamp": 0.6462676814947595
+                },
+                {
+                    "x": 1.818392625605847,
+                    "y": 3.861628588886844,
+                    "heading": -0.3150301538292836,
+                    "angularVelocity": -8.337151223773892e-7,
+                    "velocityX": 0.05695299949234183,
+                    "velocityY": -3.7825108289262035,
+                    "timestamp": 0.6785810655694975
+                },
+                {
+                    "x": 1.828928055424025,
+                    "y": 3.7398438657688415,
+                    "heading": -0.315030184023291,
+                    "angularVelocity": -9.344118022266017e-7,
+                    "velocityX": 0.32603919768385625,
+                    "velocityY": -3.768863169401422,
+                    "timestamp": 0.7108944496442354
+                },
+                {
+                    "x": 1.848105139864153,
+                    "y": 3.6191179259506723,
+                    "heading": -0.31503021891945676,
+                    "angularVelocity": -0.0000010799291597080048,
+                    "velocityX": 0.593471868986944,
+                    "velocityY": -3.7360970778839238,
+                    "timestamp": 0.7432078337189734
+                },
+                {
+                    "x": 1.8758266057917121,
+                    "y": 3.5000631817385077,
+                    "heading": -0.3150302607662258,
+                    "angularVelocity": -0.0000012950289872625492,
+                    "velocityX": 0.8578942355106304,
+                    "velocityY": -3.6843787062599658,
+                    "timestamp": 0.7755212177937113
+                },
+                {
+                    "x": 1.911951833448642,
+                    "y": 3.3832835694780297,
+                    "heading": -0.3150303133273049,
+                    "angularVelocity": -0.000001626603978371328,
+                    "velocityX": 1.1179648523774262,
+                    "velocityY": -3.6139703594763453,
+                    "timestamp": 0.8078346018684492
+                },
+                {
+                    "x": 1.9562975704379837,
+                    "y": 3.2693714862411047,
+                    "heading": -0.315030383530948,
+                    "angularVelocity": -0.000002172587152219365,
+                    "velocityX": 1.3723643703418302,
+                    "velocityY": -3.525229142619559,
+                    "timestamp": 0.8401479859431872
                 },
                 {
                     "x": 2.008638858795166,
                     "y": 3.158904790878296,
-                    "heading": 2.8166669305303302,
-                    "angularVelocity": -0.000004057228785819985,
-                    "velocityX": 1.6045266290919962,
-                    "velocityY": -3.4258008089301617,
-                    "timestamp": 0.8719787950653347
-                },
-                {
-                    "x": 2.095074231369689,
-                    "y": 3.013583476455127,
-                    "heading": 2.8166668472664824,
-                    "angularVelocity": -0.000001862874616466948,
-                    "velocityX": 1.9338316217798797,
-                    "velocityY": -3.2512956765222865,
-                    "timestamp": 0.9166752265441874
-                },
-                {
-                    "x": 2.195389426610098,
-                    "y": 2.8774723973475123,
-                    "heading": 2.8166667934576224,
-                    "angularVelocity": -0.0000012038737383971035,
-                    "velocityX": 2.2443669868336342,
-                    "velocityY": -3.0452336932538677,
-                    "timestamp": 0.9613716580230401
-                },
-                {
-                    "x": 2.308610813706186,
-                    "y": 2.751892601119491,
-                    "heading": 2.8166667540699692,
-                    "angularVelocity": -8.812259002769856e-7,
-                    "velocityX": 2.5331191629841037,
-                    "velocityY": -2.8096157136713913,
-                    "timestamp": 1.006068089501893
-                },
-                {
-                    "x": 2.433639499231453,
-                    "y": 2.638062914068913,
-                    "heading": 2.816666722710724,
-                    "angularVelocity": -7.016051243778922e-7,
-                    "velocityX": 2.7972856308321177,
-                    "velocityY": -2.5467287495744175,
-                    "timestamp": 1.0507645209807457
-                },
-                {
-                    "x": 2.5692619900937426,
-                    "y": 2.537088109244684,
-                    "heading": 2.8166666961232654,
-                    "angularVelocity": -5.94845213173575e-7,
-                    "velocityX": 3.034302434780658,
-                    "velocityY": -2.259124531496504,
-                    "timestamp": 1.0954609524595984
-                },
-                {
-                    "x": 2.7141619661921816,
-                    "y": 2.449948169817928,
-                    "heading": 2.8166666724078224,
-                    "angularVelocity": -5.305891904122224e-7,
-                    "velocityX": 3.241869010661312,
-                    "velocityY": -1.9495950021868031,
-                    "timestamp": 1.1401573839384511
-                },
-                {
-                    "x": 2.866933035141265,
-                    "y": 2.377488711333149,
-                    "heading": 2.8166666503106814,
-                    "angularVelocity": -4.943826596232375e-7,
-                    "velocityX": 3.4179701576705006,
-                    "velocityY": -1.6211463888132076,
-                    "timestamp": 1.1848538154173038
-                },
-                {
-                    "x": 3.0260921573816075,
-                    "y": 2.3204120378316904,
-                    "heading": 2.8166666288878814,
-                    "angularVelocity": -4.79295537569982e-7,
-                    "velocityX": 3.560891037031571,
-                    "velocityY": -1.2769850212418752,
-                    "timestamp": 1.2295502468961566
-                },
-                {
-                    "x": 3.1869067054641667,
-                    "y": 2.2681812614436785,
-                    "heading": 2.8166666075502422,
-                    "angularVelocity": -4.773902106314294e-7,
-                    "velocityX": 3.597928128974802,
-                    "velocityY": -1.1685670345455579,
-                    "timestamp": 1.2742466783750093
-                },
-                {
-                    "x": 3.3477213293158035,
-                    "y": 2.2159507183434,
-                    "heading": 2.8166665862126,
-                    "angularVelocity": -4.773902944556367e-7,
-                    "velocityX": 3.597929824167796,
-                    "velocityY": -1.1685618151639285,
-                    "timestamp": 1.318943109853862
-                },
-                {
-                    "x": 3.508535966619386,
-                    "y": 2.163720216660845,
-                    "heading": 2.8166665648749514,
-                    "angularVelocity": -4.773904335055608e-7,
-                    "velocityX": 3.5979301251302083,
-                    "velocityY": -1.1685608885189713,
-                    "timestamp": 1.3636395413327147
-                },
-                {
-                    "x": 3.669350617545261,
-                    "y": 2.111489756920536,
-                    "heading": 2.816666543537298,
-                    "angularVelocity": -4.773905387074157e-7,
-                    "velocityX": 3.5979304299038075,
-                    "velocityY": -1.1685599501387545,
-                    "timestamp": 1.4083359728115674
-                },
-                {
-                    "x": 3.830165282253442,
-                    "y": 2.0592593396151875,
-                    "heading": 2.816666522199642,
-                    "angularVelocity": -4.773905991181995e-7,
-                    "velocityX": 3.597930738257429,
-                    "velocityY": -1.168559000734998,
-                    "timestamp": 1.4530324042904201
-                },
-                {
-                    "x": 3.990979960989088,
-                    "y": 2.0070289654996674,
-                    "heading": 2.816666500861985,
-                    "angularVelocity": -4.773906042282552e-7,
-                    "velocityX": 3.597931052096018,
-                    "velocityY": -1.1685580344424535,
-                    "timestamp": 1.4977288357692728
-                },
-                {
-                    "x": 4.151794654203474,
-                    "y": 1.9547986359634688,
-                    "heading": 2.8166664795243315,
-                    "angularVelocity": -4.773905411161549e-7,
-                    "velocityX": 3.597931376031047,
-                    "velocityY": -1.1685570370625784,
-                    "timestamp": 1.5424252672481256
-                },
-                {
-                    "x": 4.312609353229998,
-                    "y": 1.902568324322763,
-                    "heading": 2.8166664581865346,
-                    "angularVelocity": -4.773937498526183e-7,
-                    "velocityX": 3.597931506066887,
-                    "velocityY": -1.1685566366840177,
-                    "timestamp": 1.5871216987269783
-                },
-                {
-                    "x": 4.468360661654424,
-                    "y": 1.8512590257414445,
-                    "heading": 2.7996529762164895,
-                    "angularVelocity": -0.38064519710247374,
-                    "velocityX": 3.4846475047592103,
-                    "velocityY": -1.1479506726525697,
-                    "timestamp": 1.631818130205831
-                },
-                {
-                    "x": 4.61007924771562,
-                    "y": 1.8048518324023994,
-                    "heading": 2.753515893557521,
-                    "angularVelocity": -1.0322319060481902,
-                    "velocityX": 3.1706912917253294,
-                    "velocityY": -1.0382751330159734,
-                    "timestamp": 1.6765145616846837
-                },
-                {
-                    "x": 4.736004026753032,
-                    "y": 1.7636749139398393,
-                    "heading": 2.704438737111309,
-                    "angularVelocity": -1.0980106201416129,
-                    "velocityX": 2.8173340660761905,
-                    "velocityY": -0.9212574046776457,
-                    "timestamp": 1.7212109931635364
-                },
-                {
-                    "x": 4.846114017481673,
-                    "y": 1.7276966103545506,
-                    "heading": 2.657457847080447,
-                    "angularVelocity": -1.05111053559365,
-                    "velocityX": 2.4635074229748755,
-                    "velocityY": -0.8049480102748526,
-                    "timestamp": 1.7659074246423891
-                },
-                {
-                    "x": 4.9404369366183385,
-                    "y": 1.6968910625978524,
-                    "heading": 2.6148397238370804,
-                    "angularVelocity": -0.9535016965175586,
-                    "velocityX": 2.110300890157008,
-                    "velocityY": -0.6892171642667541,
-                    "timestamp": 1.8106038561212419
-                },
-                {
-                    "x": 5.018999070193965,
-                    "y": 1.6712409213417598,
-                    "heading": 2.5778779941552346,
-                    "angularVelocity": -0.8269503505964041,
-                    "velocityX": 1.7576824586722803,
-                    "velocityY": -0.5738744773892818,
-                    "timestamp": 1.8553002876000946
-                },
-                {
-                    "x": 5.08182145352524,
-                    "y": 1.6507342817906896,
-                    "heading": 2.547405196833757,
-                    "angularVelocity": -0.6817724886134384,
-                    "velocityX": 1.4055346534990782,
-                    "velocityY": -0.45879813829819405,
-                    "timestamp": 1.8999967190789473
-                },
-                {
-                    "x": 5.128920654811687,
-                    "y": 1.6353626111579855,
-                    "heading": 2.524000017522932,
-                    "angularVelocity": -0.52364760533286,
-                    "velocityX": 1.0537575311516838,
-                    "velocityY": -0.34391270452937267,
-                    "timestamp": 1.9446931505578
-                },
-                {
-                    "x": 5.160309902680238,
-                    "y": 1.6251195157583214,
-                    "heading": 2.5080885464702414,
-                    "angularVelocity": -0.35598974070712264,
-                    "velocityX": 0.702276374869034,
-                    "velocityY": -0.2291703176462827,
-                    "timestamp": 1.9893895820366527
+                    "heading": -0.3150304952316807,
+                    "angularVelocity": -0.00000345679463513026,
+                    "velocityX": 1.6198021301675205,
+                    "velocityY": -3.418604968990847,
+                    "timestamp": 0.8724613700179251
+                },
+                {
+                    "x": 2.0775165593610554,
+                    "y": 3.0394044439018932,
+                    "heading": -0.31503058092883834,
+                    "angularVelocity": -0.0000023503872475309846,
+                    "velocityX": 1.8890856295919138,
+                    "velocityY": -3.277496001603852,
+                    "timestamp": 0.9089222373745671
+                },
+                {
+                    "x": 2.155767716832464,
+                    "y": 2.9258208551273497,
+                    "heading": -0.31503063899444206,
+                    "angularVelocity": -0.0000015925458689218539,
+                    "velocityX": 2.146168293419776,
+                    "velocityY": -3.11521905563918,
+                    "timestamp": 0.9453831047312091
+                },
+                {
+                    "x": 2.242886939864493,
+                    "y": 2.818887609111427,
+                    "heading": -0.31503068232040304,
+                    "angularVelocity": -0.000001188286624986991,
+                    "velocityX": 2.38938975806231,
+                    "velocityY": -2.9328223316783686,
+                    "timestamp": 0.9818439720878511
+                },
+                {
+                    "x": 2.3383115633038396,
+                    "y": 2.719295334161189,
+                    "heading": -0.31503071689328305,
+                    "angularVelocity": -9.482188022909844e-7,
+                    "velocityX": 2.6171791939547426,
+                    "velocityY": -2.7314839763979357,
+                    "timestamp": 1.0183048394444931
+                },
+                {
+                    "x": 2.4414252810789625,
+                    "y": 2.627687242403576,
+                    "heading": -0.3150307459166021,
+                    "angularVelocity": -7.96012851101892e-7,
+                    "velocityX": 2.8280654095942417,
+                    "velocityY": -2.5125044574926996,
+                    "timestamp": 1.0547657068011351
+                },
+                {
+                    "x": 2.5515621254678895,
+                    "y": 2.5446549735283583,
+                    "heading": -0.3150307712925926,
+                    "angularVelocity": -6.959787951410741e-7,
+                    "velocityX": 3.020686351523773,
+                    "velocityY": -2.2772982349278084,
+                    "timestamp": 1.0912265741577771
+                },
+                {
+                    "x": 2.668010765906386,
+                    "y": 2.470734767010798,
+                    "heading": -0.31503079425241676,
+                    "angularVelocity": -6.297114091058936e-7,
+                    "velocityX": 3.1937978682584367,
+                    "velocityY": -2.027384751835701,
+                    "timestamp": 1.1276874415144191
+                },
+                {
+                    "x": 2.790019095895906,
+                    "y": 2.406403976394343,
+                    "heading": -0.31503081565982244,
+                    "angularVelocity": -5.871337458164478e-7,
+                    "velocityX": 3.3462816119017478,
+                    "velocityY": -1.7643790529502215,
+                    "timestamp": 1.1641483088710611
+                },
+                {
+                    "x": 2.916799054801627,
+                    "y": 2.352077873335088,
+                    "heading": -0.31503083617399774,
+                    "angularVelocity": -5.626354173508468e-7,
+                    "velocityX": 3.477151480397404,
+                    "velocityY": -1.4899838374074656,
+                    "timestamp": 1.2006091762277031
+                },
+                {
+                    "x": 3.0475310715630033,
+                    "y": 2.308104889106193,
+                    "heading": -0.31503085634737477,
+                    "angularVelocity": -5.532884563345105e-7,
+                    "velocityX": 3.585543247850392,
+                    "velocityY": -1.2060323140087088,
+                    "timestamp": 1.2370700435843451
+                },
+                {
+                    "x": 3.1788176786437843,
+                    "y": 2.2658164704041117,
+                    "heading": -0.3150308764961414,
+                    "angularVelocity": -5.526134756982608e-7,
+                    "velocityX": 3.600753810835078,
+                    "velocityY": -1.1598302993847416,
+                    "timestamp": 1.2735309109409871
+                },
+                {
+                    "x": 3.3101043321458232,
+                    "y": 2.2235281958195148,
+                    "heading": -0.3150308966449121,
+                    "angularVelocity": -5.526135855693721e-7,
+                    "velocityX": 3.600755084015385,
+                    "velocityY": -1.159826346722746,
+                    "timestamp": 1.3099917782976291
+                },
+                {
+                    "x": 3.441390996396766,
+                    "y": 2.181239954605585,
+                    "heading": -0.31503091679368933,
+                    "angularVelocity": -5.526137662420421e-7,
+                    "velocityX": 3.6007553788219733,
+                    "velocityY": -1.1598254314766572,
+                    "timestamp": 1.3464526456542711
+                },
+                {
+                    "x": 3.5726776715245805,
+                    "y": 2.1389517471596347,
+                    "heading": -0.31503093694247225,
+                    "angularVelocity": -5.526139217103117e-7,
+                    "velocityX": 3.600755677138282,
+                    "velocityY": -1.1598245053335783,
+                    "timestamp": 1.3829135130109131
+                },
+                {
+                    "x": 3.7039643576361168,
+                    "y": 2.0966635738134185,
+                    "heading": -0.3150309570912594,
+                    "angularVelocity": -5.526140383605173e-7,
+                    "velocityX": 3.6007559783851373,
+                    "velocityY": -1.159823570091575,
+                    "timestamp": 1.4193743803675551
+                },
+                {
+                    "x": 3.835251054850112,
+                    "y": 2.0543754349355976,
+                    "heading": -0.31503097724004914,
+                    "angularVelocity": -5.526141098258176e-7,
+                    "velocityX": 3.600756282888565,
+                    "velocityY": -1.1598226247384584,
+                    "timestamp": 1.4558352477241971
+                },
+                {
+                    "x": 3.966537763342614,
+                    "y": 2.012087331072757,
+                    "heading": -0.3150309973888395,
+                    "angularVelocity": -5.526141250500724e-7,
+                    "velocityX": 3.6007565922204043,
+                    "velocityY": -1.1598216643943178,
+                    "timestamp": 1.4922961150808391
+                },
+                {
+                    "x": 4.097824483400825,
+                    "y": 1.9697992631165677,
+                    "heading": -0.315031017537628,
+                    "angularVelocity": -5.526140761706961e-7,
+                    "velocityX": 3.6007569094292164,
+                    "velocityY": -1.1598206795946073,
+                    "timestamp": 1.5287569824374811
+                },
+                {
+                    "x": 4.2291112154854185,
+                    "y": 1.9275112324972625,
+                    "heading": -0.3150310376864126,
+                    "angularVelocity": -5.526139663753558e-7,
+                    "velocityX": 3.600757239272763,
+                    "velocityY": -1.1598196555683948,
+                    "timestamp": 1.5652178497941232
+                },
+                {
+                    "x": 4.360397911943487,
+                    "y": 1.8852230912740195,
+                    "heading": -0.3150310578362786,
+                    "angularVelocity": -5.526436278671969e-7,
+                    "velocityX": 3.600756262156026,
+                    "velocityY": -1.159822689065559,
+                    "timestamp": 1.6016787171507652
+                },
+                {
+                    "x": 4.485161433583451,
+                    "y": 1.8443297112343198,
+                    "heading": -0.3370639022790847,
+                    "angularVelocity": -0.6042874467931063,
+                    "velocityX": 3.4218473307173203,
+                    "velocityY": -1.121569041122948,
+                    "timestamp": 1.6381395845074072
+                },
+                {
+                    "x": 4.600444501590908,
+                    "y": 1.8067758967734413,
+                    "heading": -0.37703885598165165,
+                    "angularVelocity": -1.0963796695112746,
+                    "velocityX": 3.1618301034864644,
+                    "velocityY": -1.029975894252485,
+                    "timestamp": 1.6746004518640492
+                },
+                {
+                    "x": 4.705219738357152,
+                    "y": 1.7727109130585983,
+                    "heading": -0.41891394403468696,
+                    "angularVelocity": -1.1484940180779766,
+                    "velocityX": 2.873635334600916,
+                    "velocityY": -0.9342889016225668,
+                    "timestamp": 1.7110613192206912
+                },
+                {
+                    "x": 4.799468915908326,
+                    "y": 1.742101106125887,
+                    "heading": -0.4594847392760727,
+                    "angularVelocity": -1.112721615877786,
+                    "velocityX": 2.5849406331801226,
+                    "velocityY": -0.8395249249915333,
+                    "timestamp": 1.7475221865773332
+                },
+                {
+                    "x": 4.883206605537126,
+                    "y": 1.7149241083644653,
+                    "heading": -0.4972998071219025,
+                    "angularVelocity": -1.0371412033604122,
+                    "velocityX": 2.2966455737248266,
+                    "velocityY": -0.7453744173332496,
+                    "timestamp": 1.7839830539339752
+                },
+                {
+                    "x": 4.956447647832475,
+                    "y": 1.6911654776329053,
+                    "heading": -0.531527000493052,
+                    "angularVelocity": -0.9387377715499247,
+                    "velocityX": 2.008757542132538,
+                    "velocityY": -0.6516200094519161,
+                    "timestamp": 1.8204439212906172
+                },
+                {
+                    "x": 5.019204190370849,
+                    "y": 1.670815407546972,
+                    "heading": -0.5616288284566417,
+                    "angularVelocity": -0.8255927559032131,
+                    "velocityX": 1.721202678052634,
+                    "velocityY": -0.5581345579873002,
+                    "timestamp": 1.8569047886472592
+                },
+                {
+                    "x": 5.071485904769321,
+                    "y": 1.6538669030565063,
+                    "heading": -0.5872312038868318,
+                    "angularVelocity": -0.7021877779198676,
+                    "velocityX": 1.433913074175601,
+                    "velocityY": -0.46484095742112364,
+                    "timestamp": 1.8933656560039012
+                },
+                {
+                    "x": 5.113300541419085,
+                    "y": 1.6403147551859525,
+                    "heading": -0.6080594891340769,
+                    "angularVelocity": -0.5712504050853902,
+                    "velocityX": 1.14683603768266,
+                    "velocityY": -0.3716902216832496,
+                    "timestamp": 1.9298265233605432
+                },
+                {
+                    "x": 5.144654410764751,
+                    "y": 1.6301549374248205,
+                    "heading": -0.6239033656036709,
+                    "angularVelocity": -0.43454469457947725,
+                    "velocityX": 0.8599320756409368,
+                    "velocityY": -0.2786499197003079,
+                    "timestamp": 1.9662873907171852
+                },
+                {
+                    "x": 5.165552750765901,
+                    "y": 1.6233842339153202,
+                    "heading": -0.634595855168354,
+                    "angularVelocity": -0.29325933089016243,
+                    "velocityX": 0.57317177336273,
+                    "velocityY": -0.18569781797215093,
+                    "timestamp": 2.002748258073827
                 },
                 {
                     "x": 5.176,
                     "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": -0.18096626962419662,
-                    "velocityX": 0.35103691280557286,
-                    "velocityY": -0.1145396978893815,
-                    "timestamp": 2.0340860135155054
+                    "heading": -0.64,
+                    "angularVelocity": -0.14821767070941108,
+                    "velocityX": 0.28653320646240676,
+                    "velocityY": -0.09281825037833577,
+                    "timestamp": 2.039209125430469
                 },
                 {
                     "x": 5.176,
                     "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": 7.15236707545134e-33,
-                    "velocityX": -4.116164438326626e-35,
-                    "velocityY": 1.5643849141244448e-34,
-                    "timestamp": 2.078782444994358
+                    "heading": -0.64,
+                    "angularVelocity": 2.131559152063316e-30,
+                    "velocityX": 2.0336113095175017e-32,
+                    "velocityY": 7.340802046004562e-32,
+                    "timestamp": 2.075669992787111
                 }
             ],
             "constraints": [
@@ -15074,7 +13627,7 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -15083,7 +13636,7 @@
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -15094,191 +13647,191 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 3.909956769253128e-30,
-                    "velocityX": -2.302876892990051e-31,
-                    "velocityY": 1.6352823102584027e-31,
+                    "heading": 0,
+                    "angularVelocity": -9.278807284273418e-29,
+                    "velocityX": -7.385267906050254e-32,
+                    "velocityY": -1.1558173496237395e-34,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.326889522840126,
-                    "y": 5.587767009829938,
-                    "heading": 3.1412161105492817,
-                    "angularVelocity": 0.012281818163829663,
-                    "velocityX": 0.7765273796780119,
-                    "velocityY": 0.2804262868111325,
-                    "timestamp": 0.0990171433131028
-                },
-                {
-                    "x": 2.4806685686648633,
-                    "y": 5.6433010199081535,
-                    "heading": 3.143648327345736,
-                    "angularVelocity": 0.024563592879525555,
-                    "velocityX": 1.5530547608152194,
-                    "velocityY": 0.560852476854545,
-                    "timestamp": 0.1980342866262056
-                },
-                {
-                    "x": 2.711337134772754,
-                    "y": 5.72660201934395,
-                    "heading": 3.147296644122033,
-                    "angularVelocity": 0.03684530429983614,
-                    "velocityX": 2.3295821146697007,
-                    "velocityY": 0.8412785569099963,
-                    "timestamp": 0.2970514299393084
-                },
-                {
-                    "x": 3.0188952067968695,
-                    "y": 5.837669992778753,
-                    "heading": 3.1521610500975985,
-                    "angularVelocity": 0.04912690684429555,
-                    "velocityX": 3.106109323428804,
-                    "velocityY": 1.1217044818551853,
-                    "timestamp": 0.3960685732524112
-                },
-                {
-                    "x": 3.3712021943007997,
-                    "y": 5.964897910508787,
-                    "heading": 3.1521610506223987,
-                    "angularVelocity": 5.300094570798509e-9,
-                    "velocityX": 3.558040312169958,
-                    "velocityY": 1.2849079813151831,
-                    "timestamp": 0.495085716565514
-                },
-                {
-                    "x": 3.7235092123405478,
-                    "y": 6.092125743682096,
-                    "heading": 3.1521610511472007,
-                    "angularVelocity": 5.300112966968611e-9,
-                    "velocityX": 3.558040620559158,
-                    "velocityY": 1.2849071273547275,
-                    "timestamp": 0.5941028598786168
-                },
-                {
-                    "x": 4.075816261929694,
-                    "y": 6.219353489491835,
-                    "heading": 3.1521610516720138,
-                    "angularVelocity": 5.300218907643243e-9,
-                    "velocityX": 3.5580409391847763,
-                    "velocityY": 1.284906245047206,
-                    "timestamp": 0.6931200031917195
-                },
-                {
-                    "x": 4.428123345781717,
-                    "y": 6.346581140424019,
-                    "heading": 3.152161052196838,
-                    "angularVelocity": 5.300337345645622e-9,
-                    "velocityX": 3.558041285214527,
-                    "velocityY": 1.284905286853995,
-                    "timestamp": 0.7921371465048224
-                },
-                {
-                    "x": 4.780430481129625,
-                    "y": 6.473808648758581,
-                    "heading": 3.152161052721672,
-                    "angularVelocity": 5.300431772031768e-9,
-                    "velocityX": 3.558041805284913,
-                    "velocityY": 1.2849038467233598,
-                    "timestamp": 0.8911542898179252
-                },
-                {
-                    "x": 5.134794229593029,
-                    "y": 6.595190238737374,
-                    "heading": 3.1521610521993333,
-                    "angularVelocity": -5.2752356524836585e-9,
-                    "velocityX": 3.5788120784587116,
-                    "velocityY": 1.225864389916352,
-                    "timestamp": 0.9901714331310281
-                },
-                {
-                    "x": 5.489158282910728,
-                    "y": 6.716570938689376,
-                    "heading": 3.152161051676551,
-                    "angularVelocity": -5.279713305479767e-9,
-                    "velocityX": 3.578815157261821,
-                    "velocityY": 1.225855401303425,
-                    "timestamp": 1.089188576444131
-                },
-                {
-                    "x": 5.843522381964551,
-                    "y": 6.837951505117218,
-                    "heading": 3.1521610511537586,
-                    "angularVelocity": -5.279818233241214e-9,
-                    "velocityX": 3.5788156191628895,
-                    "velocityY": 1.2258540528080475,
-                    "timestamp": 1.1882057197572338
-                },
-                {
-                    "x": 6.19788651637458,
-                    "y": 6.959331968324431,
-                    "heading": 3.1521610506309807,
-                    "angularVelocity": -5.279669747527449e-9,
-                    "velocityX": 3.5788159762344476,
-                    "velocityY": 1.2258530103559488,
-                    "timestamp": 1.2872228630703366
-                },
-                {
-                    "x": 6.5522506780354695,
-                    "y": 7.080712351974079,
-                    "heading": 3.1521610501082176,
-                    "angularVelocity": -5.2795212284316414e-9,
-                    "velocityX": 3.5788162514480155,
-                    "velocityY": 1.2258522068833249,
-                    "timestamp": 1.3862400063834395
-                },
-                {
-                    "x": 6.9066148601448845,
-                    "y": 7.202092675925132,
-                    "heading": 3.1521610495854597,
-                    "angularVelocity": -5.279467664817609e-9,
-                    "velocityX": 3.5788164579630073,
-                    "velocityY": 1.2258516039716054,
-                    "timestamp": 1.4852571496965423
-                },
-                {
-                    "x": 7.215968896352917,
-                    "y": 7.308055607481538,
-                    "heading": 3.147296630745593,
-                    "angularVelocity": -0.049127036764239344,
-                    "velocityX": 3.1242472349441757,
-                    "velocityY": 1.0701473301581856,
-                    "timestamp": 1.5842742930096452
-                },
-                {
-                    "x": 7.447984442479348,
-                    "y": 7.3875278059692215,
-                    "heading": 3.1436483149588383,
-                    "angularVelocity": -0.0368452943061847,
-                    "velocityX": 2.3431856177952266,
-                    "velocityY": 0.8026104958045841,
-                    "timestamp": 1.683291436322748
-                },
-                {
-                    "x": 7.6026614794307905,
-                    "y": 7.440509269529631,
-                    "heading": 3.141216104773647,
-                    "angularVelocity": -0.024563526110654314,
-                    "velocityX": 1.5621238078171693,
-                    "velocityY": 0.5350736426810189,
-                    "timestamp": 1.7823085796358509
+                    "x": 2.3268918247049313,
+                    "y": 5.587760380936702,
+                    "heading": 0.001219352926916483,
+                    "angularVelocity": 0.01231456999280599,
+                    "velocityX": 0.776551018417967,
+                    "velocityY": 0.2803594812685564,
+                    "timestamp": 0.09901709337987798
+                },
+                {
+                    "x": 2.480675474257635,
+                    "y": 5.643281133231001,
+                    "heading": 0.0036580544911558616,
+                    "angularVelocity": 0.024629096663863686,
+                    "velocityX": 1.5531020382785252,
+                    "velocityY": 0.5607188657951702,
+                    "timestamp": 0.19803418675975595
+                },
+                {
+                    "x": 2.7113509459548553,
+                    "y": 5.726562245995426,
+                    "heading": 0.007316098440537021,
+                    "angularVelocity": 0.03694356019247884,
+                    "velocityX": 2.3296530308381804,
+                    "velocityY": 0.8410781403663149,
+                    "timestamp": 0.29705128013963394
+                },
+                {
+                    "x": 3.0189182254277274,
+                    "y": 5.837603703876164,
+                    "heading": 0.01219347400929413,
+                    "angularVelocity": 0.04925791499475694,
+                    "velocityX": 3.1062038782828476,
+                    "velocityY": 1.1214372598752256,
+                    "timestamp": 0.3960683735195119
+                },
+                {
+                    "x": 3.3712359540410324,
+                    "y": 5.964801318197582,
+                    "heading": 0.012193474535478832,
+                    "angularVelocity": 5.31407952800269e-9,
+                    "velocityX": 3.5581505837748795,
+                    "velocityY": 1.2846025870848905,
+                    "timestamp": 0.49508546689938987
+                },
+                {
+                    "x": 3.723553713186701,
+                    "y": 6.0919988479491165,
+                    "heading": 0.012193475061665435,
+                    "angularVelocity": 5.314098710197071e-9,
+                    "velocityX": 3.5581508921293548,
+                    "velocityY": 1.2846017329911394,
+                    "timestamp": 0.5941025602792679
+                },
+                {
+                    "x": 4.075871503881281,
+                    "y": 6.219196290314954,
+                    "heading": 0.012193475587862622,
+                    "angularVelocity": 5.314205602691753e-9,
+                    "velocityX": 3.558151210750214,
+                    "velocityY": 1.2846008504596915,
+                    "timestamp": 0.6931196536591459
+                },
+                {
+                    "x": 4.428189328839488,
+                    "y": 6.346393637775665,
+                    "heading": 0.012193476114071436,
+                    "angularVelocity": 5.3143230401144125e-9,
+                    "velocityX": 3.5581515567877036,
+                    "velocityY": 1.2845998919875412,
+                    "timestamp": 0.7921367470390238
+                },
+                {
+                    "x": 4.780507205251625,
+                    "y": 6.473590842716609,
+                    "heading": 0.012193476640289547,
+                    "angularVelocity": 5.314416922842863e-9,
+                    "velocityX": 3.5581520764346486,
+                    "velocityY": 1.2845984526424705,
+                    "timestamp": 0.8911538404189018
+                },
+                {
+                    "x": 5.134861642335281,
+                    "y": 6.594999030315895,
+                    "heading": 0.012193476116517958,
+                    "angularVelocity": -5.289708644023114e-9,
+                    "velocityX": 3.5787198451098035,
+                    "velocityY": 1.2261336245600862,
+                    "timestamp": 0.9901709337987797
+                },
+                {
+                    "x": 5.489216369156675,
+                    "y": 6.716406372237666,
+                    "heading": 0.012193475592327948,
+                    "angularVelocity": -5.293934612313601e-9,
+                    "velocityX": 3.578722771248356,
+                    "velocityY": 1.2261250838377307,
+                    "timestamp": 1.0891880271786578
+                },
+                {
+                    "x": 5.843571141725175,
+                    "y": 6.837813580636051,
+                    "heading": 0.012193475068127738,
+                    "angularVelocity": -5.2940376086876394e-9,
+                    "velocityX": 3.5787232332605625,
+                    "velocityY": 1.2261237353494956,
+                    "timestamp": 1.1882051205585358
+                },
+                {
+                    "x": 6.1979259496904,
+                    "y": 6.959220685720912,
+                    "heading": 0.012193474543942472,
+                    "angularVelocity": -5.293886679384445e-9,
+                    "velocityX": 3.5787235907415074,
+                    "velocityY": 1.2261226919587003,
+                    "timestamp": 1.2872222139384137
+                },
+                {
+                    "x": 6.552280784920285,
+                    "y": 7.080627711227486,
+                    "heading": 0.012193474019771769,
+                    "angularVelocity": -5.293739610148028e-9,
+                    "velocityX": 3.5787238660945766,
+                    "velocityY": 1.2261218882763663,
+                    "timestamp": 1.3862393073182917
+                },
+                {
+                    "x": 6.906635640601096,
+                    "y": 7.202034677043121,
+                    "heading": 0.01219347349560639,
+                    "angularVelocity": -5.293685828527132e-9,
+                    "velocityX": 3.5787240726339373,
+                    "velocityY": 1.2261212854416952,
+                    "timestamp": 1.4852564006981697
+                },
+                {
+                    "x": 7.215981364626797,
+                    "y": 7.3080208081486955,
+                    "heading": 0.0073160851772923075,
+                    "angularVelocity": -0.0492580437559288,
+                    "velocityX": 3.124164863524117,
+                    "velocityY": 1.0703821682480574,
+                    "timestamp": 1.5842734940780476
+                },
+                {
+                    "x": 7.447990676616371,
+                    "y": 7.387510406301363,
+                    "heading": 0.003658042213644346,
+                    "angularVelocity": -0.03694355023729677,
+                    "velocityX": 2.343123839229173,
+                    "velocityY": 0.8027866244033968,
+                    "timestamp": 1.6832905874579256
+                },
+                {
+                    "x": 7.602663557476493,
+                    "y": 7.440503469639918,
+                    "heading": 0.0012193472043836597,
+                    "angularVelocity": -0.024629030463387942,
+                    "velocityX": 1.5620826221056736,
+                    "velocityY": 0.5351910617619017,
+                    "timestamp": 1.7823076808378036
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
-                    "angularVelocity": -0.012281759834182485,
-                    "velocityX": 0.7810619250512634,
-                    "velocityY": 0.2675368081121253,
-                    "timestamp": 1.8813257229489537
+                    "heading": 1.0919443501059542e-30,
+                    "angularVelocity": -0.01231451219942279,
+                    "velocityX": 0.781041332195102,
+                    "velocityY": 0.26759551765904444,
+                    "timestamp": 1.8813247742176815
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
-                    "angularVelocity": 1.8685058153122784e-28,
+                    "heading": 0,
+                    "angularVelocity": -8.461898211343436e-30,
                     "velocityX": 0,
-                    "velocityY": -1.820396114239373e-31,
-                    "timestamp": 1.9803428662620566
+                    "velocityY": -1.0908466908101513e-31,
+                    "timestamp": 1.9803418675975595
                 }
             ],
             "constraints": [
@@ -15321,7 +13874,7 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -15330,7 +13883,7 @@
                 {
                     "x": 5.403600215911865,
                     "y": 6.822902202606201,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -15339,7 +13892,7 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -15350,524 +13903,524 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "angularVelocity": 0,
                     "velocityX": 0,
-                    "velocityY": 4.173822195239149e-34,
+                    "velocityY": -1.3324658216077414e-32,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.260404640394534,
-                    "y": 5.564834920701341,
-                    "heading": 3.1398005529486164,
-                    "angularVelocity": -0.005376730415880204,
-                    "velocityX": 0.2804902157612115,
-                    "velocityY": 0.1303406844718941,
-                    "timestamp": 0.037094486045788126
-                },
-                {
-                    "x": 2.2812139214883365,
-                    "y": 5.574504760754136,
-                    "heading": 3.139401659951885,
-                    "angularVelocity": -0.010753431015026859,
-                    "velocityX": 0.5609804397374861,
-                    "velocityY": 0.26068133255328624,
-                    "timestamp": 0.07418897209157625
-                },
-                {
-                    "x": 2.3124278435365335,
-                    "y": 5.58900951877657,
-                    "heading": 3.1388033223810634,
-                    "angularVelocity": -0.016130094647582943,
-                    "velocityX": 0.8414706705915215,
-                    "velocityY": 0.3910219433834154,
-                    "timestamp": 0.11128345813736437
-                },
-                {
-                    "x": 2.3540464067273468,
-                    "y": 5.608349193345913,
-                    "heading": 3.1380055419662973,
-                    "angularVelocity": -0.02150671163853717,
-                    "velocityX": 1.1219609065196614,
-                    "velocityY": 0.5213625158593963,
-                    "timestamp": 0.1483779441831525
-                },
-                {
-                    "x": 2.4060696111557784,
-                    "y": 5.632523782985854,
-                    "heading": 3.1370083209408497,
-                    "angularVelocity": -0.026883268424770904,
-                    "velocityX": 1.4024511450088724,
-                    "velocityY": 0.651703048536671,
-                    "timestamp": 0.18547243022894064
-                },
-                {
-                    "x": 2.4684974567819364,
-                    "y": 5.661533286147284,
-                    "heading": 3.1358116622714935,
-                    "angularVelocity": -0.032259745232204366,
-                    "velocityX": 1.6829413824226938,
-                    "velocityY": 0.7820435394527967,
-                    "timestamp": 0.22256691627472877
-                },
-                {
-                    "x": 2.541329943361364,
-                    "y": 5.695377701176762,
-                    "heading": 3.1344155700455096,
-                    "angularVelocity": -0.03763611185395848,
-                    "velocityX": 1.9634316132463963,
-                    "velocityY": 0.9123839857951219,
-                    "timestamp": 0.2596614023205169
-                },
-                {
-                    "x": 2.6245670703196087,
-                    "y": 5.734057026259781,
-                    "heading": 3.1328200501674663,
-                    "angularVelocity": -0.04301231929927239,
-                    "velocityX": 2.243921828583893,
-                    "velocityY": 1.0427243832216457,
-                    "timestamp": 0.296755888366305
-                },
-                {
-                    "x": 2.7182088365016934,
-                    "y": 5.777571259306566,
-                    "heading": 3.1310251117440115,
-                    "angularVelocity": -0.04838828124588956,
-                    "velocityX": 2.5244120127853087,
-                    "velocityY": 1.17306472431166,
-                    "timestamp": 0.33385037441209314
-                },
-                {
-                    "x": 2.822255239588457,
-                    "y": 5.825920397683519,
-                    "heading": 3.1290307702747278,
-                    "angularVelocity": -0.053763825351902786,
-                    "velocityX": 2.804902134466352,
-                    "velocityY": 1.3034049944046322,
-                    "timestamp": 0.3709448604578813
-                },
-                {
-                    "x": 2.9367062743473644,
-                    "y": 5.879104437402994,
-                    "heading": 3.126837057015217,
-                    "angularVelocity": -0.05913852686363322,
-                    "velocityX": 3.0853921150877586,
-                    "velocityY": 1.4337451569978974,
-                    "timestamp": 0.4080393465036694
-                },
-                {
-                    "x": 3.0615619238881426,
-                    "y": 5.937123369060452,
-                    "heading": 3.1244440632726094,
-                    "angularVelocity": -0.06451076690090568,
-                    "velocityX": 3.3658816403780625,
-                    "velocityY": 1.5640850660618872,
-                    "timestamp": 0.44513383254945754
-                },
-                {
-                    "x": 3.188819545942614,
-                    "y": 5.996258327229638,
-                    "heading": 3.124444062637546,
-                    "angularVelocity": -1.7120162809754954e-8,
-                    "velocityX": 3.430634458646746,
-                    "velocityY": 1.594171114709378,
-                    "timestamp": 0.4822283185952457
-                },
-                {
-                    "x": 3.3160771824859174,
-                    "y": 6.055393254219169,
-                    "heading": 3.1244440620025067,
-                    "angularVelocity": -1.711950789942117e-8,
-                    "velocityX": 3.4306348492393433,
-                    "velocityY": 1.5941702741624886,
-                    "timestamp": 0.5193228046410338
-                },
-                {
-                    "x": 3.4433348335743874,
-                    "y": 6.114528149907673,
-                    "heading": 3.1244440613674698,
-                    "angularVelocity": -1.7119436749370044e-8,
-                    "velocityX": 3.4306352413506347,
-                    "velocityY": 1.5941694303436482,
-                    "timestamp": 0.5564172906868219
-                },
-                {
-                    "x": 3.570592499306144,
-                    "y": 6.173663014083979,
-                    "heading": 3.124444060732438,
-                    "angularVelocity": -1.7119304165961875e-8,
-                    "velocityX": 3.4306356361070582,
-                    "velocityY": 1.5941685808319566,
-                    "timestamp": 0.59351177673261
-                },
-                {
-                    "x": 3.6978501798346053,
-                    "y": 6.232797846417909,
-                    "heading": 3.1244440600974133,
-                    "angularVelocity": -1.7119116923870704e-8,
-                    "velocityX": 3.430636034999362,
-                    "velocityY": 1.5941677224193291,
-                    "timestamp": 0.6306062627783982
-                },
-                {
-                    "x": 3.825107875386636,
-                    "y": 6.291932646421232,
-                    "heading": 3.1244440594623972,
-                    "angularVelocity": -1.7118879018616895e-8,
-                    "velocityX": 3.430636440007508,
-                    "velocityY": 1.594166850844881,
-                    "timestamp": 0.6677007488241863
-                },
-                {
-                    "x": 3.9523655862834066,
-                    "y": 6.351067413402769,
-                    "heading": 3.1244440588273914,
-                    "angularVelocity": -1.711859691944282e-8,
-                    "velocityX": 3.4306368536738447,
-                    "velocityY": 1.5941659606374654,
-                    "timestamp": 0.7047952348699744
-                },
-                {
-                    "x": 4.079623312940078,
-                    "y": 6.410202146469073,
-                    "heading": 3.1244440581923967,
-                    "angularVelocity": -1.711830453291075e-8,
-                    "velocityX": 3.4306372785321515,
-                    "velocityY": 1.5941650463443597,
-                    "timestamp": 0.7418897209157626
-                },
-                {
-                    "x": 4.206881055880688,
-                    "y": 6.469336844492392,
-                    "heading": 3.124444057557412,
-                    "angularVelocity": -1.711804273623487e-8,
-                    "velocityX": 3.430637717517568,
-                    "velocityY": 1.5941641016490944,
-                    "timestamp": 0.7789842069615507
-                },
-                {
-                    "x": 4.334138815794606,
-                    "y": 6.528471505989175,
-                    "heading": 3.1244440569224348,
-                    "angularVelocity": -1.7117826690263867e-8,
-                    "velocityX": 3.4306381750871577,
-                    "velocityY": 1.59416311696,
-                    "timestamp": 0.8160786930073388
-                },
-                {
-                    "x": 4.461396593547393,
-                    "y": 6.587606129096708,
-                    "heading": 3.1244440562874636,
-                    "angularVelocity": -1.7117677202487012e-8,
-                    "velocityX": 3.430638655990672,
-                    "velocityY": 1.5941620820555098,
-                    "timestamp": 0.853173179053127
-                },
-                {
-                    "x": 4.5886544601407655,
-                    "y": 6.646740561018822,
-                    "heading": 3.124444055652494,
-                    "angularVelocity": -1.711762346285072e-8,
-                    "velocityX": 3.4306410509715453,
-                    "velocityY": 1.5941569280437173,
-                    "timestamp": 0.8902676650989151
-                },
-                {
-                    "x": 4.7169368908362825,
-                    "y": 6.703618180644166,
-                    "heading": 3.1244440550164043,
-                    "angularVelocity": -1.7147829637187305e-8,
-                    "velocityX": 3.458261439103553,
-                    "velocityY": 1.5333173656899308,
-                    "timestamp": 0.9273621511447032
-                },
-                {
-                    "x": 4.8494363294608345,
-                    "y": 6.749827966380956,
-                    "heading": 3.124444054367944,
-                    "angularVelocity": -1.7481320274300667e-8,
-                    "velocityX": 3.5719443170340583,
-                    "velocityY": 1.245731931148746,
-                    "timestamp": 0.9644566371904914
-                },
-                {
-                    "x": 4.985267831359121,
-                    "y": 6.785059321241209,
-                    "heading": 3.124444053688418,
-                    "angularVelocity": -1.8318771799358465e-8,
-                    "velocityX": 3.6617706936448973,
-                    "velocityY": 0.9497733656900947,
-                    "timestamp": 1.0015511232362795
-                },
-                {
-                    "x": 5.12352345692124,
-                    "y": 6.809076540845587,
-                    "heading": 3.1244440529552464,
-                    "angularVelocity": -1.9764980034334986e-8,
-                    "velocityX": 3.7271206667066523,
-                    "velocityY": 0.6474606380779142,
-                    "timestamp": 1.0386456092820675
-                },
-                {
-                    "x": 5.26327900081687,
-                    "y": 6.821719014416903,
-                    "heading": 3.124444052138408,
-                    "angularVelocity": -2.2020481247534803e-8,
-                    "velocityX": 3.767555741926746,
-                    "velocityY": 0.3408181354961927,
-                    "timestamp": 1.0757400953278555
+                    "x": 2.2604047390881647,
+                    "y": 5.564834315872384,
+                    "heading": -0.00020682184904158323,
+                    "angularVelocity": -0.005575579259585174,
+                    "velocityX": 0.2804947723375417,
+                    "velocityY": 0.13032526030129518,
+                    "timestamp": 0.037094235309470626
+                },
+                {
+                    "x": 2.2812142175691745,
+                    "y": 5.574502946267583,
+                    "heading": -0.0006204643713099006,
+                    "angularVelocity": -0.011151126821122416,
+                    "velocityX": 0.5609895528887481,
+                    "velocityY": 0.2606504842204101,
+                    "timestamp": 0.07418847061894125
+                },
+                {
+                    "x": 2.3124284356981284,
+                    "y": 5.589005889804122,
+                    "heading": -0.0012409261115257607,
+                    "angularVelocity": -0.01672663515070949,
+                    "velocityX": 0.8414843403169981,
+                    "velocityY": 0.39097567089719987,
+                    "timestamp": 0.11128270592841188
+                },
+                {
+                    "x": 2.3540473936632536,
+                    "y": 5.608343145059642,
+                    "heading": -0.0020682052367836136,
+                    "angularVelocity": -0.022302094068156527,
+                    "velocityX": 1.121979132819583,
+                    "velocityY": 0.5213008192295232,
+                    "timestamp": 0.1483769412378825
+                },
+                {
+                    "x": 2.4060710915596104,
+                    "y": 5.632514710558234,
+                    "heading": -0.0031022993856005916,
+                    "angularVelocity": -0.027877489323882353,
+                    "velocityX": 1.402473927884819,
+                    "velocityY": 0.6516259277737647,
+                    "timestamp": 0.18547117654735312
+                },
+                {
+                    "x": 2.46849952934744,
+                    "y": 5.661520584751242,
+                    "heading": -0.004343205426988983,
+                    "angularVelocity": -0.03345280017329315,
+                    "velocityX": 1.6829687218782206,
+                    "velocityY": 0.781950994568762,
+                    "timestamp": 0.22256541185682374
+                },
+                {
+                    "x": 2.5413327067825313,
+                    "y": 5.695360765985745,
+                    "heading": -0.005790919055957113,
+                    "angularVelocity": -0.039027994967159495,
+                    "velocityX": 1.9634635092880914,
+                    "velocityY": 0.9122760168036823,
+                    "timestamp": 0.25965964716629436
+                },
+                {
+                    "x": 2.6245706232908623,
+                    "y": 5.734035252447861,
+                    "heading": -0.007445434065373688,
+                    "angularVelocity": -0.044603022426903935,
+                    "velocityX": 2.2439582812232635,
+                    "velocityY": 1.042600990139337,
+                    "timestamp": 0.296753882475765
+                },
+                {
+                    "x": 2.7182132777182155,
+                    "y": 5.777544042048614,
+                    "heading": -0.009306740898831883,
+                    "angularVelocity": -0.05017779226152841,
+                    "velocityX": 2.5244530220426173,
+                    "velocityY": 1.1729259071596103,
+                    "timestamp": 0.33384811778523565
+                },
+                {
+                    "x": 2.8222606677468502,
+                    "y": 5.825887132155531,
+                    "heading": -0.011374823315541767,
+                    "angularVelocity": -0.05575212427098156,
+                    "velocityX": 2.804947700379475,
+                    "velocityY": 1.3032507532126736,
+                    "timestamp": 0.3709423530947063
+                },
+                {
+                    "x": 2.9367127881472834,
+                    "y": 5.879064518782887,
+                    "heading": -0.013649648601341033,
+                    "angularVelocity": -0.0613255743600492,
+                    "velocityX": 3.0854422377380093,
+                    "velocityY": 1.4335754918171768,
+                    "timestamp": 0.40803658840417695
+                },
+                {
+                    "x": 3.061569622038521,
+                    "y": 5.9370761925311655,
+                    "heading": -0.016131121234305167,
+                    "angularVelocity": -0.06689644933404303,
+                    "velocityX": 3.365936320012513,
+                    "velocityY": 1.563899977025988,
+                    "timestamp": 0.4451308237136476
+                },
+                {
+                    "x": 3.188829425192231,
+                    "y": 5.996204205741329,
+                    "heading": -0.01613112189285862,
+                    "angularVelocity": -1.7753525563113908e-8,
+                    "velocityX": 3.430716446693241,
+                    "velocityY": 1.5939946656635118,
+                    "timestamp": 0.48222505902311824
+                },
+                {
+                    "x": 3.3160892428339666,
+                    "y": 6.055332187769375,
+                    "heading": -0.01613112255138683,
+                    "angularVelocity": -1.775284497104555e-8,
+                    "velocityX": 3.4307168372667594,
+                    "velocityY": 1.5939938250445522,
+                    "timestamp": 0.5193192943325888
+                },
+                {
+                    "x": 3.443349075020492,
+                    "y": 6.114460138492994,
+                    "heading": -0.016131123209912376,
+                    "angularVelocity": -1.775277322447583e-8,
+                    "velocityX": 3.43071722937053,
+                    "velocityY": 1.593992981128321,
+                    "timestamp": 0.5564135296420595
+                },
+                {
+                    "x": 3.5706089218502877,
+                    "y": 6.173588057700207,
+                    "heading": -0.01613112386843302,
+                    "angularVelocity": -1.775264097704561e-8,
+                    "velocityX": 3.430717624129179,
+                    "velocityY": 1.5939921314975005,
+                    "timestamp": 0.5935077649515301
+                },
+                {
+                    "x": 3.697868783477107,
+                    "y": 6.2327159450600735,
+                    "heading": -0.016131124526946636,
+                    "angularVelocity": -1.7752451586110053e-8,
+                    "velocityX": 3.430718023032762,
+                    "velocityY": 1.5939912729450545,
+                    "timestamp": 0.6306020002610008
+                },
+                {
+                    "x": 3.8251286601281493,
+                    "y": 6.291843800083575,
+                    "heading": -0.016131125185451407,
+                    "angularVelocity": -1.7752213148650806e-8,
+                    "velocityX": 3.4307184280612866,
+                    "velocityY": 1.5939904012094348,
+                    "timestamp": 0.6676962355704714
+                },
+                {
+                    "x": 3.95238855212494,
+                    "y": 6.350971622078674,
+                    "heading": -0.016131125843945708,
+                    "angularVelocity": -1.775193091205886e-8,
+                    "velocityX": 3.430718841757593,
+                    "velocityY": 1.5939895108176958,
+                    "timestamp": 0.7047904708799421
+                },
+                {
+                    "x": 4.079648459881884,
+                    "y": 6.410099410153433,
+                    "heading": -0.01613112650242917,
+                    "angularVelocity": -1.7751638714463086e-8,
+                    "velocityX": 3.430719266625584,
+                    "velocityY": 1.5939885963806835,
+                    "timestamp": 0.7418847061894127
+                },
+                {
+                    "x": 4.2069083839232695,
+                    "y": 6.469227163179404,
+                    "heading": -0.01613112716090301,
+                    "angularVelocity": -1.7751379248608663e-8,
+                    "velocityX": 3.430719705627546,
+                    "velocityY": 1.5939876515226545,
+                    "timestamp": 0.7789789414988834
+                },
+                {
+                    "x": 4.334168324938812,
+                    "y": 6.528354879672101,
+                    "heading": -0.016131127819368877,
+                    "angularVelocity": -1.7751164242273923e-8,
+                    "velocityX": 3.430720163223074,
+                    "velocityY": 1.5939866666451934,
+                    "timestamp": 0.816073176808354
+                },
+                {
+                    "x": 4.461428283794271,
+                    "y": 6.58748255776812,
+                    "heading": -0.01613112847782922,
+                    "angularVelocity": -1.775101542628313e-8,
+                    "velocityX": 3.430720644158123,
+                    "velocityY": 1.5939856315335348,
+                    "timestamp": 0.8531674121178247
+                },
+                {
+                    "x": 4.58868833054201,
+                    "y": 6.646610046694028,
+                    "heading": -0.016131129136287645,
+                    "angularVelocity": -1.7750963701844846e-8,
+                    "velocityX": 3.430723013590394,
+                    "velocityY": 1.5939805318162525,
+                    "timestamp": 0.8902616474272953
+                },
+                {
+                    "x": 4.716960790002456,
+                    "y": 6.703507810971908,
+                    "heading": -0.016131129795891656,
+                    "angularVelocity": -1.7781846861521853e-8,
+                    "velocityX": 3.4580160068077612,
+                    "velocityY": 1.5338707969903813,
+                    "timestamp": 0.927355882736766
+                },
+                {
+                    "x": 4.849451901724698,
+                    "y": 6.749738586923234,
+                    "heading": -0.016131130468276335,
+                    "angularVelocity": -1.8126392792981438e-8,
+                    "velocityX": 3.5717439816966663,
+                    "velocityY": 1.2463062135027452,
+                    "timestamp": 0.9644501180462366
+                },
+                {
+                    "x": 4.985276799458285,
+                    "y": 6.784991618369029,
+                    "heading": -0.016131131172819703,
+                    "angularVelocity": -1.899333848953808e-8,
+                    "velocityX": 3.661617407676018,
+                    "velocityY": 0.950364150970728,
+                    "timestamp": 1.0015443533557071
+                },
+                {
+                    "x": 5.12352760039525,
+                    "y": 6.80903105836077,
+                    "heading": -0.01613113193292308,
+                    "angularVelocity": -2.0491145599848975e-8,
+                    "velocityX": 3.7270157959468126,
+                    "velocityY": 0.6480640399022476,
+                    "timestamp": 1.0386385886651777
+                },
+                {
+                    "x": 5.263280144021708,
+                    "y": 6.821696149590996,
+                    "heading": -0.01613113277968929,
+                    "angularVelocity": -2.2827434072857776e-8,
+                    "velocityX": 3.767500326143083,
+                    "velocityY": 0.341430174380559,
+                    "timestamp": 1.0757328239746482
                 },
                 {
                     "x": 5.403600215911865,
                     "y": 6.822902202606201,
-                    "heading": 3.124444051205612,
-                    "angularVelocity": -2.5146473765828148e-8,
-                    "velocityX": 3.782805210504538,
-                    "velocityY": 0.03189660554501852,
-                    "timestamp": 1.1128345813736435
-                },
-                {
-                    "x": 5.530809022252107,
-                    "y": 6.8145114887459926,
-                    "heading": 3.1244440503418645,
-                    "angularVelocity": -2.5630462052464063e-8,
-                    "velocityX": 3.774737065106383,
-                    "velocityY": -0.24898227978112425,
-                    "timestamp": 1.1465346256803473
-                },
-                {
-                    "x": 5.657044071820609,
-                    "y": 6.79670145003808,
-                    "heading": 3.1244440495837504,
-                    "angularVelocity": -2.2495928655681543e-8,
-                    "velocityX": 3.745842243399973,
-                    "velocityY": -0.5284871006643926,
-                    "timestamp": 1.180234669987051
-                },
-                {
-                    "x": 5.781608875607717,
-                    "y": 6.769570371843818,
-                    "heading": 3.124444048897901,
-                    "angularVelocity": -2.0351591481890114e-8,
-                    "velocityX": 3.6962801192023655,
-                    "velocityY": -0.8050754458167036,
-                    "timestamp": 1.2139347142937547
-                },
-                {
-                    "x": 5.903816171014302,
-                    "y": 6.733267988087913,
-                    "heading": 3.1244440482605893,
-                    "angularVelocity": -1.891130244410514e-8,
-                    "velocityX": 3.6263244728813384,
-                    "velocityY": -1.0772206536441533,
-                    "timestamp": 1.2476347586004584
-                },
-                {
-                    "x": 6.022991737743217,
-                    "y": 6.687994699591429,
-                    "heading": 3.124444047653726,
-                    "angularVelocity": -1.8007800496265653e-8,
-                    "velocityX": 3.5363623158563073,
-                    "velocityY": -1.3434192573888535,
-                    "timestamp": 1.281334802907162
-                },
-                {
-                    "x": 6.1384782869774295,
-                    "y": 6.6340007793146,
-                    "heading": 3.1244440470624193,
-                    "angularVelocity": -1.754616693571922e-8,
-                    "velocityX": 3.4268960652742955,
-                    "velocityY": -1.6021913735611486,
-                    "timestamp": 1.3150348472138658
-                },
-                {
-                    "x": 6.251164558797792,
-                    "y": 6.57438290738676,
-                    "heading": 3.1244440464747822,
-                    "angularVelocity": -1.7437275134789687e-8,
-                    "velocityX": 3.343801889244,
-                    "velocityY": -1.769073992462997,
-                    "timestamp": 1.3487348915205695
-                },
-                {
-                    "x": 6.363850200907552,
-                    "y": 6.514763845231331,
-                    "heading": 3.124444045887316,
-                    "angularVelocity": -1.7432210000565644e-8,
-                    "velocityX": 3.343783203493965,
-                    "velocityY": -1.769109310742662,
-                    "timestamp": 1.3824349358272732
-                },
-                {
-                    "x": 6.475774212207488,
-                    "y": 6.453726953025987,
-                    "heading": 3.124444046521184,
-                    "angularVelocity": 1.880910439494107e-8,
-                    "velocityX": 3.3211829124412344,
-                    "velocityY": -1.8111813637349317,
-                    "timestamp": 1.4161349801339769
-                },
-                {
-                    "x": 6.587697896511305,
-                    "y": 6.392689461208172,
-                    "heading": 3.1244440471552553,
-                    "angularVelocity": 1.881516364731064e-8,
-                    "velocityX": 3.3211732093049866,
-                    "velocityY": -1.8111991563664802,
-                    "timestamp": 1.4498350244406806
-                },
-                {
-                    "x": 6.699621529072565,
-                    "y": 6.331651874510795,
-                    "heading": 3.124444047789331,
-                    "angularVelocity": 1.881528213317004e-8,
-                    "velocityX": 3.321171673919603,
-                    "velocityY": -1.811201971780052,
-                    "timestamp": 1.4835350687473843
-                },
-                {
-                    "x": 6.811545132450604,
-                    "y": 6.2706142343006634,
-                    "heading": 3.124444048423398,
-                    "angularVelocity": 1.881501023244474e-8,
-                    "velocityX": 3.3211708079497897,
-                    "velocityY": -1.8112035596935343,
-                    "timestamp": 1.517235113054088
-                },
-                {
-                    "x": 6.923468714015748,
-                    "y": 6.209576554092949,
-                    "heading": 3.1244440490575007,
-                    "angularVelocity": 1.8816081178773256e-8,
-                    "velocityX": 3.3211701606836845,
-                    "velocityY": -1.8112047465638819,
-                    "timestamp": 1.5509351573607917
-                },
-                {
-                    "x": 7.031544581287166,
-                    "y": 6.150637092992946,
-                    "heading": 3.126666281190466,
-                    "angularVelocity": 0.06594151962352889,
-                    "velocityX": 3.2069948124643886,
-                    "velocityY": -1.7489431338307682,
-                    "timestamp": 1.5846352016674954
-                },
-                {
-                    "x": 7.131306941921243,
-                    "y": 6.096231417458986,
-                    "heading": 3.1287176070894986,
-                    "angularVelocity": 0.060870124690714414,
-                    "velocityX": 2.960303545186476,
-                    "velocityY": -1.6144096143855096,
-                    "timestamp": 1.618335245974199
-                },
-                {
-                    "x": 7.2227557801672635,
-                    "y": 6.046359537502576,
-                    "heading": 3.1305980016068635,
-                    "angularVelocity": 0.05579798353529268,
-                    "velocityX": 2.7136118105289606,
-                    "velocityY": -1.4798757978632517,
-                    "timestamp": 1.6520352902809028
-                },
-                {
-                    "x": 7.305891091138876,
-                    "y": 6.001021457128046,
-                    "heading": 3.132307456329253,
-                    "angularVelocity": 0.05072559272714715,
-                    "velocityX": 2.466919930876006,
-                    "velocityY": -1.3453418625182645,
-                    "timestamp": 1.6857353345876065
-                },
-                {
-                    "x": 7.380712872646774,
-                    "y": 5.9602171788044025,
-                    "heading": 3.1338459674980714,
-                    "angularVelocity": 0.04565309038812635,
-                    "velocityX": 2.2202279862586183,
-                    "velocityY": -1.2108078539091887,
-                    "timestamp": 1.7194353788943102
-                },
-                {
-                    "x": 7.447221123569205,
-                    "y": 5.923946704366036,
-                    "heading": 3.135213533279876,
-                    "angularVelocity": 0.04058053364434269,
-                    "velocityX": 1.9735360083547677,
-                    "velocityY": -1.0762737908671516,
-                    "timestamp": 1.753135423201014
-                },
-                {
-                    "x": 7.505415843310863,
-                    "y": 5.892210035316634,
-                    "heading": 3.136410152813217,
-                    "angularVelocity": 0.035507951338280094,
-                    "velocityX": 1.7268440127861169,
-                    "velocityY": -0.941739683205353,
-                    "timestamp": 1.7868354675077176
-                },
-                {
-                    "x": 7.555297031572268,
-                    "y": 5.865007172961733,
-                    "heading": 3.1374358257913886,
-                    "angularVelocity": 0.030435359931185808,
-                    "velocityX": 1.4801520083308155,
-                    "velocityY": -0.807205536803692,
-                    "timestamp": 1.8205355118144213
-                },
-                {
-                    "x": 7.596864688235311,
-                    "y": 5.8423381184765635,
-                    "heading": 3.138290552251957,
-                    "angularVelocity": 0.025362769638798365,
-                    "velocityX": 1.2334600003707947,
-                    "velocityY": -0.6726713555287516,
-                    "timestamp": 1.854235556121125
-                },
-                {
-                    "x": 7.6301188133003475,
-                    "y": 5.824202872944982,
-                    "heading": 3.1389743324564656,
-                    "angularVelocity": 0.020290187107322288,
-                    "velocityX": 0.9867679924213419,
-                    "velocityY": -0.5381371420919633,
-                    "timestamp": 1.8879356004278287
-                },
-                {
-                    "x": 7.655059406849112,
-                    "y": 5.8106014373839985,
-                    "heading": 3.1394871668082627,
-                    "angularVelocity": 0.015217616544653215,
-                    "velocityX": 0.7400759868972333,
-                    "velocityY": -0.4036028984768356,
-                    "timestamp": 1.9216356447345324
-                },
-                {
-                    "x": 7.671686469021436,
-                    "y": 5.801533812759989,
-                    "heading": 3.1398290558033333,
-                    "angularVelocity": 0.010145060699586378,
-                    "velocityX": 0.4933839855224485,
-                    "velocityY": -0.2690686261859067,
-                    "timestamp": 1.9553356890412361
+                    "heading": -0.016131133746573465,
+                    "angularVelocity": -2.6065618224331403e-8,
+                    "velocityX": 3.782799961220162,
+                    "velocityY": 0.03251321951088522,
+                    "timestamp": 1.1128270592841187
+                },
+                {
+                    "x": 5.530811600424101,
+                    "y": 6.814532053807006,
+                    "heading": -0.016131134641902804,
+                    "angularVelocity": -2.6567346760379892e-8,
+                    "velocityX": 3.7747774237594087,
+                    "velocityY": -0.24836966315439601,
+                    "timestamp": 1.1465274262810004
+                },
+                {
+                    "x": 5.6570507349000225,
+                    "y": 6.796742242248266,
+                    "heading": -0.016131135427811184,
+                    "angularVelocity": -2.332046941486525e-8,
+                    "velocityX": 3.7459275884919543,
+                    "velocityY": -0.5278818346513846,
+                    "timestamp": 1.180227793277882
+                },
+                {
+                    "x": 5.781621094465109,
+                    "y": 6.769630943607204,
+                    "heading": -0.01613113613886582,
+                    "angularVelocity": -2.1099314246183972e-8,
+                    "velocityX": 3.696409584400467,
+                    "velocityY": -0.8044808130300218,
+                    "timestamp": 1.2139281602747636
+                },
+                {
+                    "x": 5.903835372739644,
+                    "y": 6.733347785640362,
+                    "heading": -0.016131136799649604,
+                    "angularVelocity": -1.9607613962388268e-8,
+                    "velocityX": 3.6264969543460657,
+                    "velocityY": -1.0766398469844052,
+                    "timestamp": 1.2476285272716452
+                },
+                {
+                    "x": 6.023019298132563,
+                    "y": 6.68809306728661,
+                    "heading": -0.01613113742890953,
+                    "angularVelocity": -1.8672198003746715e-8,
+                    "velocityX": 3.536576483097275,
+                    "velocityY": -1.3428553569739705,
+                    "timestamp": 1.2813288942685268
+                },
+                {
+                    "x": 6.13851552436941,
+                    "y": 6.634116966515058,
+                    "heading": -0.016131138042081584,
+                    "angularVelocity": -1.819481831325098e-8,
+                    "velocityX": 3.4271504000990243,
+                    "velocityY": -1.601647269198795,
+                    "timestamp": 1.3150292612654084
+                },
+                {
+                    "x": 6.251225914876305,
+                    "y": 6.574542092363786,
+                    "heading": -0.016131138651474624,
+                    "angularVelocity": -1.808268258780769e-8,
+                    "velocityX": 3.344485551665478,
+                    "velocityY": -1.7677811685785219,
+                    "timestamp": 1.34872962826229
+                },
+                {
+                    "x": 6.363935678841957,
+                    "y": 6.51496603286697,
+                    "heading": -0.016131139260695985,
+                    "angularVelocity": -1.807758837477246e-8,
+                    "velocityX": 3.344466960139662,
+                    "velocityY": -1.7678163416537895,
+                    "timestamp": 1.3824299952591717
+                },
+                {
+                    "x": 6.475851865782255,
+                    "y": 6.4539122462129415,
+                    "heading": -0.016131138603390965,
+                    "angularVelocity": 1.950438761664722e-8,
+                    "velocityX": 3.320918936896271,
+                    "velocityY": -1.8116653346735878,
+                    "timestamp": 1.4161303622560533
+                },
+                {
+                    "x": 6.587767727105175,
+                    "y": 6.3928578626815735,
+                    "heading": -0.016131137945887282,
+                    "angularVelocity": 1.9510282594383425e-8,
+                    "velocityX": 3.320909274764734,
+                    "velocityY": -1.811683045974467,
+                    "timestamp": 1.4498307292529349
+                },
+                {
+                    "x": 6.699683536860704,
+                    "y": 6.331803384624609,
+                    "heading": -0.016131137288379963,
+                    "angularVelocity": 1.951039048402864e-8,
+                    "velocityX": 3.3209077445917723,
+                    "velocityY": -1.811685850857758,
+                    "timestamp": 1.4835310962498165
+                },
+                {
+                    "x": 6.811599317469464,
+                    "y": 6.270748853140253,
+                    "heading": -0.0161311366308819,
+                    "angularVelocity": 1.951011573787136e-8,
+                    "velocityX": 3.320906879711909,
+                    "velocityY": -1.811687436223046,
+                    "timestamp": 1.517231463246698
+                },
+                {
+                    "x": 6.923515076276675,
+                    "y": 6.209694281692974,
+                    "heading": -0.016131135973346068,
+                    "angularVelocity": 1.951123652121546e-8,
+                    "velocityX": 3.3209062327887042,
+                    "velocityY": -1.8116886220535497,
+                    "timestamp": 1.5509318302435797
+                },
+                {
+                    "x": 7.031584320380035,
+                    "y": 6.150738002364414,
+                    "heading": -0.01382673748429001,
+                    "angularVelocity": 0.06837903246775291,
+                    "velocityX": 3.2067675735804393,
+                    "velocityY": -1.7494254390171946,
+                    "timestamp": 1.5846321972404613
+                },
+                {
+                    "x": 7.1313405673144,
+                    "y": 6.0963168023113505,
+                    "heading": -0.011699564081675173,
+                    "angularVelocity": 0.06312018509503639,
+                    "velocityX": 2.9600937860289633,
+                    "velocityY": -1.6148548191804175,
+                    "timestamp": 1.618332564237343
+                },
+                {
+                    "x": 7.222783801332874,
+                    "y": 6.046430691545947,
+                    "heading": -0.009749641962936324,
+                    "angularVelocity": 0.05786056035883372,
+                    "velocityX": 2.7134195312156404,
+                    "velocityY": -1.480283902250094,
+                    "timestamp": 1.6520329312342246
+                },
+                {
+                    "x": 7.305914017550448,
+                    "y": 6.00107967407256,
+                    "heading": -0.007976979919226685,
+                    "angularVelocity": 0.05260067476039131,
+                    "velocityX": 2.4667451314481808,
+                    "velocityY": -1.3457128664973763,
+                    "timestamp": 1.6857332982311062
+                },
+                {
+                    "x": 7.380731213778514,
+                    "y": 5.960263752360125,
+                    "heading": -0.006381581900532674,
+                    "angularVelocity": 0.047340671952945675,
+                    "velocityX": 2.2200706667375205,
+                    "velocityY": -1.2111417574833037,
+                    "timestamp": 1.7194336652279878
+                },
+                {
+                    "x": 7.447235388895749,
+                    "y": 5.923982928242947,
+                    "heading": -0.004963449854323965,
+                    "angularVelocity": 0.042080611357711836,
+                    "velocityX": 1.973396168753552,
+                    "velocityY": -1.076570594039371,
+                    "timestamp": 1.7531340322248694
+                },
+                {
+                    "x": 7.505426542307151,
+                    "y": 5.892237203224628,
+                    "heading": -0.00372258471684395,
+                    "angularVelocity": 0.03682052298107744,
+                    "velocityX": 1.72672165311393,
+                    "velocityY": -0.9419993859786789,
+                    "timestamp": 1.786834399221751
+                },
+                {
+                    "x": 7.5553046737134695,
+                    "y": 5.865026578610625,
+                    "heading": -0.002658986847128457,
+                    "angularVelocity": 0.03156042395067696,
+                    "velocityX": 1.4800471285945758,
+                    "velocityY": -0.8074281391808545,
+                    "timestamp": 1.8205347662186326
+                },
+                {
+                    "x": 7.596869782996782,
+                    "y": 5.842351055576096,
+                    "heading": -0.0017726562460022867,
+                    "angularVelocity": 0.026300324895801946,
+                    "velocityX": 1.233372600576068,
+                    "velocityY": -0.6728568575121819,
+                    "timestamp": 1.8542351332155143
+                },
+                {
+                    "x": 7.630121870157635,
+                    "y": 5.82421063520489,
+                    "heading": -0.0010635926829587166,
+                    "angularVelocity": 0.021040232680814546,
+                    "velocityX": 0.9866980725738072,
+                    "velocityY": -0.5382855436821761,
+                    "timestamp": 1.8879355002123959
+                },
+                {
+                    "x": 7.655060935277959,
+                    "y": 5.8106053185140585,
+                    "heading": -0.0005317957806907868,
+                    "angularVelocity": 0.015780151661757422,
+                    "velocityX": 0.7400235470026807,
+                    "velocityY": -0.40371419967297517,
+                    "timestamp": 1.9216358672092775
+                },
+                {
+                    "x": 7.671686978497787,
+                    "y": 5.801535106470048,
+                    "heading": -0.00017726506577651584,
+                    "angularVelocity": 0.010520084690677335,
+                    "velocityX": 0.4933490255867586,
+                    "velocityY": -0.2691428269861255,
+                    "timestamp": 1.955336234206159
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
-                    "angularVelocity": 0.0050725214219634815,
-                    "velocityX": 0.24669198956838576,
-                    "velocityY": -0.1345343263862727,
-                    "timestamp": 1.9890357333479398
+                    "heading": 3.986792057163996e-31,
+                    "angularVelocity": 0.005260033690221658,
+                    "velocityX": 0.24667450959752207,
+                    "velocityY": -0.13457142678797143,
+                    "timestamp": 1.9890366012030407
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
-                    "angularVelocity": 1.6384700465306213e-30,
-                    "velocityX": 7.518277191788514e-33,
-                    "velocityY": 3.939051930235097e-33,
-                    "timestamp": 2.0227357776546437
+                    "heading": 0,
+                    "angularVelocity": -1.0163368054249497e-29,
+                    "velocityX": -1.0830525743862323e-33,
+                    "velocityY": 5.9205016331337416e-33,
+                    "timestamp": 2.022736968199922
                 }
             ],
             "constraints": [
@@ -15910,7 +14463,7 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -15919,7 +14472,7 @@
                 {
                     "x": 4.148105144500732,
                     "y": 5.054959774017334,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -15928,7 +14481,7 @@
                 {
                     "x": 5.480467319488525,
                     "y": 4.235044002532959,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -15937,7 +14490,7 @@
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -15948,569 +14501,569 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 1.2492921998824363e-31,
-                    "velocityX": 1.8348820495773462e-32,
-                    "velocityY": 2.100679182506497e-31,
+                    "heading": 0,
+                    "angularVelocity": 8.750120685231394e-32,
+                    "velocityX": 3.618098191401073e-32,
+                    "velocityY": 7.301888602720515e-32,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.256418341264554,
-                    "y": 5.5583910221605555,
-                    "heading": 3.140000000000001,
-                    "angularVelocity": 2.3717958634313657e-14,
-                    "velocityX": 0.2278392560297752,
-                    "velocityY": -0.05711574046896039,
-                    "timestamp": 0.028170480260500258
-                },
-                {
-                    "x": 2.269255024758466,
-                    "y": 5.555173071001971,
-                    "heading": 3.1400000000000023,
-                    "angularVelocity": 4.731676431156196e-14,
-                    "velocityX": 0.4556785463083383,
-                    "velocityY": -0.11423132047543481,
-                    "timestamp": 0.056340960521000516
-                },
-                {
-                    "x": 2.2885100515714525,
-                    "y": 5.550346151638775,
-                    "heading": 3.140000000000004,
-                    "angularVelocity": 7.082238243214026e-14,
-                    "velocityX": 0.6835178752697619,
-                    "velocityY": -0.17134671892564213,
-                    "timestamp": 0.08451144078150077
-                },
-                {
-                    "x": 2.314183422941926,
-                    "y": 5.543910269896753,
-                    "heading": 3.140000000000007,
-                    "angularVelocity": 9.417090371236357e-14,
-                    "velocityX": 0.9113572481926052,
-                    "velocityY": -0.22846191057115275,
-                    "timestamp": 0.11268192104200103
-                },
-                {
-                    "x": 2.3462751402864916,
-                    "y": 5.535865432460115,
-                    "heading": 3.1400000000000103,
-                    "angularVelocity": 1.1735309251828409e-13,
-                    "velocityX": 1.1391966714022896,
-                    "velocityY": -0.28557686493967693,
-                    "timestamp": 0.1408524013025013
-                },
-                {
-                    "x": 2.384785205236557,
-                    "y": 5.526211647057785,
-                    "heading": 3.140000000000014,
-                    "angularVelocity": 1.4030928785894893e-13,
-                    "velocityX": 1.367036152523929,
-                    "velocityY": -0.3426915449455824,
-                    "timestamp": 0.16902288156300155
-                },
-                {
-                    "x": 2.4297136196836084,
-                    "y": 5.514948922700363,
-                    "heading": 3.1400000000000188,
-                    "angularVelocity": 1.629004634033803e-13,
-                    "velocityX": 1.5948757007898307,
-                    "velocityY": -0.39980590509187397,
-                    "timestamp": 0.1971933618235018
-                },
-                {
-                    "x": 2.481060385834354,
-                    "y": 5.50207726998147,
-                    "heading": 3.1400000000000237,
-                    "angularVelocity": 1.8496997642686924e-13,
-                    "velocityX": 1.822715327389804,
-                    "velocityY": -0.4569198891841572,
-                    "timestamp": 0.22536384208400206
-                },
-                {
-                    "x": 2.5388255062752263,
-                    "y": 5.487596701457088,
-                    "heading": 3.14000000000003,
-                    "angularVelocity": 2.0646227541738248e-13,
-                    "velocityX": 2.0505550458033337,
-                    "velocityY": -0.5140334275623145,
-                    "timestamp": 0.25353432234450235
-                },
-                {
-                    "x": 2.6030089840400077,
-                    "y": 5.471507232107069,
-                    "heading": 3.140000000000036,
-                    "angularVelocity": 2.2698477398227208e-13,
-                    "velocityX": 2.278394871910569,
-                    "velocityY": -0.5711464341833905,
-                    "timestamp": 0.28170480260500264
-                },
-                {
-                    "x": 2.673610822657147,
-                    "y": 5.453808879843062,
-                    "heading": 3.140000000000043,
-                    "angularVelocity": 2.462514676500777e-13,
-                    "velocityX": 2.506234823271173,
-                    "velocityY": -0.6282588049740611,
-                    "timestamp": 0.3098752828655029
-                },
-                {
-                    "x": 2.7506310261005877,
-                    "y": 5.434501665888006,
-                    "heading": 3.1400000000000508,
-                    "angularVelocity": 2.636502833251031e-13,
-                    "velocityX": 2.7340749156994817,
-                    "velocityY": -0.6853704223895046,
-                    "timestamp": 0.3380457631260032
-                },
-                {
-                    "x": 2.834069598399475,
-                    "y": 5.413585614377554,
-                    "heading": 3.1400000000000587,
-                    "angularVelocity": 2.7876299092334793e-13,
-                    "velocityX": 2.9619151511549577,
-                    "velocityY": -0.7424811830340069,
-                    "timestamp": 0.3662162433865035
-                },
-                {
-                    "x": 2.9239265420507494,
-                    "y": 5.391060748829609,
-                    "heading": 3.1400000000000667,
-                    "angularVelocity": 2.92402388167881e-13,
-                    "velocityX": 3.189755475247231,
-                    "velocityY": -0.7995911088362938,
-                    "timestamp": 0.3943867236470038
-                },
-                {
-                    "x": 3.0202018518398366,
-                    "y": 5.366927076318611,
-                    "heading": 3.1400000000000756,
-                    "angularVelocity": 3.135357172375438e-13,
-                    "velocityX": 3.41759561423172,
-                    "velocityY": -0.8567007835090982,
-                    "timestamp": 0.42255720390750406
-                },
-                {
-                    "x": 3.122895489704816,
-                    "y": 5.341184519185279,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": 4.18703156863577e-13,
-                    "velocityX": 3.645434402088397,
-                    "velocityY": -0.9138132149428461,
-                    "timestamp": 0.45072768416800435
-                },
-                {
-                    "x": 3.2262642847828022,
-                    "y": 5.315271868839329,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": 1.5556336116797473e-16,
-                    "velocityX": 3.669401235694458,
-                    "velocityY": -0.9198512097177582,
-                    "timestamp": 0.47889816442850464
-                },
-                {
-                    "x": 3.3296318094364086,
-                    "y": 5.289354150507015,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": -1.6083670828215077e-16,
-                    "velocityX": 3.669356137976281,
-                    "velocityY": -0.9200311138697771,
-                    "timestamp": 0.5070686446890049
-                },
-                {
-                    "x": 3.4329975109343875,
-                    "y": 5.263429161959932,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": 1.7787924056306163e-16,
-                    "velocityX": 3.669291419320054,
-                    "velocityY": -0.9202891930612488,
-                    "timestamp": 0.5352391249495052
-                },
-                {
-                    "x": 3.536360974744539,
-                    "y": 5.23749525314096,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": 1.5501276329034833e-16,
-                    "velocityX": 3.669211985536661,
-                    "velocityY": -0.9206058462317519,
-                    "timestamp": 0.5634096052100055
-                },
-                {
-                    "x": 3.6397250084453354,
-                    "y": 5.211563615834452,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": 2.0671720135979797e-16,
-                    "velocityX": 3.6692322156015877,
-                    "velocityY": -0.9205252117362434,
-                    "timestamp": 0.5915800854705058
-                },
-                {
-                    "x": 3.743083009106784,
-                    "y": 5.185607942562298,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": 1.5977153188511187e-16,
-                    "velocityX": 3.6690180538517057,
-                    "velocityY": -0.921378444106565,
-                    "timestamp": 0.6197505657310061
-                },
-                {
-                    "x": 3.8464299042889807,
-                    "y": 5.159608086186739,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": -2.0588084435766592e-17,
-                    "velocityX": 3.6686238298573155,
-                    "velocityY": -0.9229468626423776,
-                    "timestamp": 0.6479210459915064
-                },
-                {
-                    "x": 3.949030591223734,
-                    "y": 5.130804053392629,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": 7.696524756540132e-18,
-                    "velocityX": 3.642134815806339,
-                    "velocityY": -1.0224899443584623,
-                    "timestamp": 0.6760915262520066
-                },
-                {
-                    "x": 4.049738686087192,
-                    "y": 5.095955052913767,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": 3.5716950666970086e-16,
-                    "velocityX": 3.5749512941271244,
-                    "velocityY": -1.2370751281698067,
-                    "timestamp": 0.7042620065125069
+                    "x": 2.256418341264449,
+                    "y": 5.558391022160101,
+                    "heading": -2.5296452464584536e-15,
+                    "angularVelocity": -8.979773235851594e-14,
+                    "velocityX": 0.22783925602590605,
+                    "velocityY": -0.05711574048502746,
+                    "timestamp": 0.0281704802605187
+                },
+                {
+                    "x": 2.2692550247581535,
+                    "y": 5.555173071000616,
+                    "heading": -7.593688933611638e-15,
+                    "angularVelocity": -1.7976419432911875e-13,
+                    "velocityX": 0.4556785463006615,
+                    "velocityY": -0.11423132050732354,
+                    "timestamp": 0.0563409605210374
+                },
+                {
+                    "x": 2.2885100515708303,
+                    "y": 5.55034615163608,
+                    "heading": -1.5195474783343342e-14,
+                    "angularVelocity": -2.6984935224723246e-13,
+                    "velocityX": 0.6835178752583485,
+                    "velocityY": -0.1713467189730698,
+                    "timestamp": 0.0845114407815561
+                },
+                {
+                    "x": 2.314183422940896,
+                    "y": 5.54391026989229,
+                    "heading": -2.533887633402318e-14,
+                    "angularVelocity": -3.6007201361891127e-13,
+                    "velocityX": 0.9113572481775378,
+                    "velocityY": -0.2284619106337897,
+                    "timestamp": 0.1126819210420748
+                },
+                {
+                    "x": 2.346275140284958,
+                    "y": 5.535865432453464,
+                    "heading": -3.8024880419905343e-14,
+                    "angularVelocity": -4.5032970578145327e-13,
+                    "velocityX": 1.139196671383666,
+                    "velocityY": -0.2855768650171331,
+                    "timestamp": 0.14085240130259352
+                },
+                {
+                    "x": 2.3847852052344276,
+                    "y": 5.526211647048542,
+                    "heading": -5.3249348657163737e-14,
+                    "angularVelocity": -5.404404929621474e-13,
+                    "velocityX": 1.3670361525018668,
+                    "velocityY": -0.3426915450373883,
+                    "timestamp": 0.16902288156311224
+                },
+                {
+                    "x": 2.4297136196807942,
+                    "y": 5.514948922688138,
+                    "heading": -7.10022499667577e-14,
+                    "angularVelocity": -6.301951953680268e-13,
+                    "velocityX": 1.594875700764475,
+                    "velocityY": -0.3998059051974526,
+                    "timestamp": 0.19719336182363095
+                },
+                {
+                    "x": 2.4810603858307716,
+                    "y": 5.502077269965895,
+                    "heading": -9.12659901579941e-14,
+                    "angularVelocity": -7.193253365088161e-13,
+                    "velocityX": 1.8227153273613368,
+                    "velocityY": -0.45691988930278143,
+                    "timestamp": 0.22536384208414967
+                },
+                {
+                    "x": 2.5388255062707983,
+                    "y": 5.4875967014378215,
+                    "heading": -1.1400857239830863e-13,
+                    "angularVelocity": -8.073196492084524e-13,
+                    "velocityX": 2.050555045771992,
+                    "velocityY": -0.5140334276930385,
+                    "timestamp": 0.2535343223446684
+                },
+                {
+                    "x": 2.603008984034667,
+                    "y": 5.4715072320838045,
+                    "heading": -1.3917722607752625e-13,
+                    "angularVelocity": -8.934407027436765e-13,
+                    "velocityX": 2.2783948718766736,
+                    "velocityY": -0.571146434324935,
+                    "timestamp": 0.2817048026051871
+                },
+                {
+                    "x": 2.673610822650838,
+                    "y": 5.453808879815544,
+                    "heading": -1.6668826583849139e-13,
+                    "angularVelocity": -9.76591080715578e-13,
+                    "velocityX": 2.5062348232351805,
+                    "velocityY": -0.6282588051246059,
+                    "timestamp": 0.3098752828657058
+                },
+                {
+                    "x": 2.7506310260932763,
+                    "y": 5.434501665856061,
+                    "heading": -1.9641690622397207e-13,
+                    "angularVelocity": -1.0553118055097966e-12,
+                    "velocityX": 2.7340749156620854,
+                    "velocityY": -0.6853704225462842,
+                    "timestamp": 0.33804576312622453
+                },
+                {
+                    "x": 2.8340695983911575,
+                    "y": 5.413585614341132,
+                    "heading": -2.282023162882806e-13,
+                    "angularVelocity": -1.128323328886794e-12,
+                    "velocityX": 2.9619151511173074,
+                    "velocityY": -0.7424811831924288,
+                    "timestamp": 0.36621624338674325
+                },
+                {
+                    "x": 2.9239265420414853,
+                    "y": 5.391060748788907,
+                    "heading": -2.61980248572899e-13,
+                    "angularVelocity": -1.1990541861941579e-12,
+                    "velocityX": 3.189755475211519,
+                    "velocityY": -0.7995911089877218,
+                    "timestamp": 0.39438672364726196
+                },
+                {
+                    "x": 3.020201851829833,
+                    "y": 5.366927076274376,
+                    "heading": -2.9867647823192555e-13,
+                    "angularVelocity": -1.302648350982896e-12,
+                    "velocityX": 3.417595614203248,
+                    "velocityY": -0.8567007836339452,
+                    "timestamp": 0.4225572039077807
+                },
+                {
+                    "x": 3.12289548969471,
+                    "y": 5.341184519140017,
+                    "heading": -3.474157859958988e-13,
+                    "angularVelocity": -1.7301553723681501e-12,
+                    "velocityX": 3.6454344020823566,
+                    "velocityY": -0.9138132149787032,
+                    "timestamp": 0.4507276841682994
+                },
+                {
+                    "x": 3.2262642847742486,
+                    "y": 5.315271868799975,
+                    "heading": -3.474349355086831e-13,
+                    "angularVelocity": -6.797723212352284e-16,
+                    "velocityX": 3.6694012357471815,
+                    "velocityY": -0.9198512095073661,
+                    "timestamp": 0.4788981644288181
+                },
+                {
+                    "x": 3.3296318094304937,
+                    "y": 5.289354150477899,
+                    "heading": -3.47428956348503e-13,
+                    "angularVelocity": 2.1224913898406702e-16,
+                    "velocityX": 3.669356138067555,
+                    "velocityY": -0.920031113505745,
+                    "timestamp": 0.5070686446893368
+                },
+                {
+                    "x": 3.4329975109325144,
+                    "y": 5.263429161946644,
+                    "heading": -3.4742781851400116e-13,
+                    "angularVelocity": 4.0391022494337805e-17,
+                    "velocityX": 3.669291419461124,
+                    "velocityY": -0.9202891924987843,
+                    "timestamp": 0.5352391249498555
+                },
+                {
+                    "x": 3.536360974747324,
+                    "y": 5.237495253145949,
+                    "heading": -3.474322284236003e-13,
+                    "angularVelocity": -1.565436427905273e-16,
+                    "velocityX": 3.669211985699591,
+                    "velocityY": -0.9206058455823695,
+                    "timestamp": 0.5634096052103742
+                },
+                {
+                    "x": 3.6397250084501422,
+                    "y": 5.211563615847215,
+                    "heading": -3.474363074560882e-13,
+                    "angularVelocity": -1.4479811668640362e-16,
+                    "velocityX": 3.6692322156709736,
+                    "velocityY": -0.9205252114596718,
+                    "timestamp": 0.5915800854708929
+                },
+                {
+                    "x": 3.7430830091153613,
+                    "y": 5.185607942589789,
+                    "heading": -3.474405753435316e-13,
+                    "angularVelocity": -1.515021186684875e-16,
+                    "velocityX": 3.669018053983158,
+                    "velocityY": -0.9213784435831122,
+                    "timestamp": 0.6197505657314116
+                },
+                {
+                    "x": 3.8464299042958676,
+                    "y": 5.1596080862072276,
+                    "heading": -3.474364187384818e-13,
+                    "angularVelocity": 1.4755179936907513e-16,
+                    "velocityX": 3.668623829794913,
+                    "velocityY": -0.92294686289042,
+                    "timestamp": 0.6479210459919303
+                },
+                {
+                    "x": 3.9490305912287407,
+                    "y": 5.130804053406161,
+                    "heading": -3.4744608105895375e-13,
+                    "angularVelocity": -3.429945241470833e-16,
+                    "velocityX": 3.642134815737201,
+                    "velocityY": -1.0224899446047346,
+                    "timestamp": 0.6760915262524491
+                },
+                {
+                    "x": 4.049738686089906,
+                    "y": 5.09595505292046,
+                    "heading": -3.4745592092862155e-13,
+                    "angularVelocity": -3.4929719255187864e-16,
+                    "velocityX": 3.57495129404341,
+                    "velocityY": -1.237075128411728,
+                    "timestamp": 0.7042620065129678
                 },
                 {
                     "x": 4.148105144500732,
                     "y": 5.054959774017334,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": 3.8010173489100904e-16,
-                    "velocityX": 3.4918275266846264,
-                    "velocityY": -1.455256655809122,
-                    "timestamp": 0.7324324867730072
-                },
-                {
-                    "x": 4.301558125078007,
-                    "y": 4.971822103513501,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": -3.218591761776222e-17,
-                    "velocityX": 3.32615174670464,
-                    "velocityY": -1.8020406441293784,
-                    "timestamp": 0.7785677783240725
-                },
-                {
-                    "x": 4.445898119119771,
-                    "y": 4.873711676762074,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": -4.812512433810467e-17,
-                    "velocityX": 3.128624295828435,
-                    "velocityY": -2.126580833303964,
-                    "timestamp": 0.8247030698751379
-                },
-                {
-                    "x": 4.5875715760336115,
-                    "y": 4.7717885618775915,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": -6.437963156007962e-17,
-                    "velocityX": 3.0708260888967014,
-                    "velocityY": -2.2092222994111217,
-                    "timestamp": 0.8708383614262032
-                },
-                {
-                    "x": 4.729219222551534,
-                    "y": 4.669829579874243,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": -5.401066324195286e-16,
-                    "velocityX": 3.0702666386935125,
-                    "velocityY": -2.2099997329304943,
-                    "timestamp": 0.9169736529772685
-                },
-                {
-                    "x": 4.870877206247071,
-                    "y": 4.567884960570754,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": -5.491807811904997e-16,
-                    "velocityX": 3.0704907009776297,
-                    "velocityY": -2.209688415909936,
-                    "timestamp": 0.9631089445283338
-                },
-                {
-                    "x": 5.012587244990002,
-                    "y": 4.46601271378786,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": 2.3992830315072076e-16,
-                    "velocityX": 3.0716190139795567,
-                    "velocityY": -2.2081197139288955,
-                    "timestamp": 1.0092442360793992
-                },
-                {
-                    "x": 5.160453670190759,
-                    "y": 4.373302358536228,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": 3.2225913830486856e-16,
-                    "velocityX": 3.205061033110239,
-                    "velocityY": -2.009532228659069,
-                    "timestamp": 1.0553795276304645
-                },
-                {
-                    "x": 5.316942928230888,
-                    "y": 4.296031397168284,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": 3.540598605731943e-16,
-                    "velocityX": 3.391964216104365,
-                    "velocityY": -1.674877491153309,
-                    "timestamp": 1.1015148191815298
+                    "heading": -3.474586084587748e-13,
+                    "angularVelocity": -9.540235481877701e-17,
+                    "velocityX": 3.491827526585996,
+                    "velocityY": -1.4552566560457803,
+                    "timestamp": 0.7324324867734865
+                },
+                {
+                    "x": 4.301558125072517,
+                    "y": 4.97182210350292,
+                    "heading": -3.4745624581283064e-13,
+                    "angularVelocity": 5.1211249699831376e-17,
+                    "velocityX": 3.3261517465815813,
+                    "velocityY": -1.8020406443565171,
+                    "timestamp": 0.7785677783246081
+                },
+                {
+                    "x": 4.445898119107738,
+                    "y": 4.873711676741488,
+                    "heading": -3.4747329562610653e-13,
+                    "angularVelocity": -3.6956119081305154e-16,
+                    "velocityX": 3.1286242956827928,
+                    "velocityY": -2.1265808335182323,
+                    "timestamp": 0.8247030698757297
+                },
+                {
+                    "x": 4.587571576014754,
+                    "y": 4.771788561847154,
+                    "heading": -3.4746251546031876e-13,
+                    "angularVelocity": 2.3366419539300746e-16,
+                    "velocityX": 3.0708260887450245,
+                    "velocityY": -2.2092222996219535,
+                    "timestamp": 0.8708383614268513
+                },
+                {
+                    "x": 4.729219222551993,
+                    "y": 4.66982957987028,
+                    "heading": -3.474587781166732e-13,
+                    "angularVelocity": 8.100834549122579e-17,
+                    "velocityX": 3.070266639108493,
+                    "velocityY": -2.209999732353969,
+                    "timestamp": 0.916973652977973
+                },
+                {
+                    "x": 4.870877206231954,
+                    "y": 4.567884960544782,
+                    "heading": -3.47487867941798e-13,
+                    "angularVelocity": -6.305330291624225e-16,
+                    "velocityX": 3.070490700636254,
+                    "velocityY": -2.209688416384298,
+                    "timestamp": 0.9631089445290946
+                },
+                {
+                    "x": 5.012587244979431,
+                    "y": 4.466012713767847,
+                    "heading": -3.4750411523417436e-13,
+                    "angularVelocity": -3.5216624493767476e-16,
+                    "velocityX": 3.0716190140743462,
+                    "velocityY": -2.208119713797033,
+                    "timestamp": 1.009244236080216
+                },
+                {
+                    "x": 5.160453670184351,
+                    "y": 4.373302358522453,
+                    "heading": -3.47491376477911e-13,
+                    "angularVelocity": 2.7611738924244047e-16,
+                    "velocityX": 3.2050610331965634,
+                    "velocityY": -2.009532228521384,
+                    "timestamp": 1.0553795276313376
+                },
+                {
+                    "x": 5.316942928228011,
+                    "y": 4.29603139716118,
+                    "heading": -3.4750002331748596e-13,
+                    "angularVelocity": -1.8742353831509497e-16,
+                    "velocityX": 3.3919642161767567,
+                    "velocityY": -1.6748774910066955,
+                    "timestamp": 1.101514819182459
                 },
                 {
                     "x": 5.480467319488525,
                     "y": 4.235044002532959,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": 3.7214299347620075e-16,
-                    "velocityX": 3.544453405635226,
-                    "velocityY": -1.3219249859178432,
-                    "timestamp": 1.1476501107325952
-                },
-                {
-                    "x": 5.591417678770796,
-                    "y": 4.201991970754989,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": 2.3864185821028e-16,
-                    "velocityX": 3.6254884971019536,
-                    "velocityY": -1.0800304009303623,
-                    "timestamp": 1.1782529824203136
-                },
-                {
-                    "x": 5.704342870000873,
-                    "y": 4.176490587908602,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": 1.1130552624117688e-16,
-                    "velocityX": 3.6900194328910065,
-                    "velocityY": -0.8333003225044783,
-                    "timestamp": 1.208855854108032
-                },
-                {
-                    "x": 5.818728492964508,
-                    "y": 4.158648255815286,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": 1.0429128791162552e-16,
-                    "velocityX": 3.737741481609135,
-                    "velocityY": -0.583028033314901,
-                    "timestamp": 1.2394587257957506
-                },
-                {
-                    "x": 5.934052260793966,
-                    "y": 4.148506842860091,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": 7.090230424723168e-16,
-                    "velocityX": 3.7683969336671863,
-                    "velocityY": -0.3313876246402188,
-                    "timestamp": 1.270061597483469
-                },
-                {
-                    "x": 6.049784939893549,
-                    "y": 4.145614314312393,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": 5.904438564058107e-16,
-                    "velocityX": 3.7817587931139536,
-                    "velocityY": -0.0945182065847883,
-                    "timestamp": 1.3006644691711875
-                },
-                {
-                    "x": 6.1655453259799495,
-                    "y": 4.144216905752427,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": -4.945243197986353e-16,
-                    "velocityX": 3.7826641652342365,
-                    "velocityY": -0.04566266116445697,
-                    "timestamp": 1.331267340858906
-                },
-                {
-                    "x": 6.281306292238703,
-                    "y": 4.142868414104831,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": 1.3561359601509856e-16,
-                    "velocityX": 3.782683123335836,
-                    "velocityY": -0.04406421926917252,
-                    "timestamp": 1.3618702125466244
-                },
-                {
-                    "x": 6.397067393015407,
-                    "y": 4.141531519361414,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": -2.815905629868065e-16,
-                    "velocityX": 3.782687518934728,
-                    "velocityY": -0.04368527104047988,
-                    "timestamp": 1.392473084234343
-                },
-                {
-                    "x": 6.512828617959603,
-                    "y": 4.140205420378013,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": 4.8810263606010396e-17,
-                    "velocityX": 3.782691576315187,
-                    "velocityY": -0.043332501527048845,
-                    "timestamp": 1.4230759559220614
-                },
-                {
-                    "x": 6.628589931635543,
-                    "y": 4.1388870909429585,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": -3.260693519621353e-16,
-                    "velocityX": 3.7826944757734187,
-                    "velocityY": -0.04307861853318547,
-                    "timestamp": 1.4536788276097798
-                },
-                {
-                    "x": 6.744351296534275,
-                    "y": 4.137573267441446,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": -5.381950425280429e-16,
-                    "velocityX": 3.782696149563972,
-                    "velocityY": -0.04293137960615145,
-                    "timestamp": 1.4842816992974983
-                },
-                {
-                    "x": 6.860112695906051,
-                    "y": 4.136261713626517,
-                    "heading": 3.140000000000087,
-                    "angularVelocity": 6.589038089157793e-16,
-                    "velocityX": 3.7826972760284443,
-                    "velocityY": -0.04285721378647441,
-                    "timestamp": 1.5148845709852168
-                },
-                {
-                    "x": 6.969430224754608,
-                    "y": 4.135029589996691,
-                    "heading": 3.1400000000000587,
-                    "angularVelocity": -9.14507649037293e-13,
-                    "velocityX": 3.5721330326141576,
-                    "velocityY": -0.04026169968457299,
-                    "timestamp": 1.5454874426729353
-                },
-                {
-                    "x": 7.070940187047598,
-                    "y": 4.13388425582119,
-                    "heading": 3.140000000000041,
-                    "angularVelocity": -5.92710722161247e-13,
-                    "velocityX": 3.3170077412616483,
-                    "velocityY": -0.037425709147242185,
-                    "timestamp": 1.5760903143606537
-                },
-                {
-                    "x": 7.16464169000894,
-                    "y": 4.132826276442613,
-                    "heading": 3.1400000000000285,
-                    "angularVelocity": -4.0082806789240517e-13,
-                    "velocityX": 3.06185327695782,
-                    "velocityY": -0.034571245100923735,
-                    "timestamp": 1.6066931860483722
-                },
-                {
-                    "x": 7.250534738512908,
-                    "y": 4.131855992104498,
-                    "heading": 3.14000000000002,
-                    "angularVelocity": -2.818757978482064e-13,
-                    "velocityX": 2.8066989719287347,
-                    "velocityY": -0.031705663029917695,
-                    "timestamp": 1.6372960577360907
-                },
-                {
-                    "x": 7.328619329487215,
-                    "y": 4.130973602817362,
-                    "heading": 3.140000000000014,
-                    "angularVelocity": -2.020529664357193e-13,
-                    "velocityX": 2.5515445665075394,
-                    "velocityY": -0.028833545281456593,
-                    "timestamp": 1.6678989294238091
-                },
-                {
-                    "x": 7.398895462248533,
-                    "y": 4.130179235854988,
-                    "heading": 3.1400000000000095,
-                    "angularVelocity": -1.455432238063259e-13,
-                    "velocityX": 2.296390138757389,
-                    "velocityY": -0.02595726864966503,
-                    "timestamp": 1.6985018011115276
-                },
-                {
-                    "x": 7.4613631364100685,
-                    "y": 4.129472977966136,
-                    "heading": 3.1400000000000063,
-                    "angularVelocity": -1.0415995882589759e-13,
-                    "velocityX": 2.0412356983681157,
-                    "velocityY": -0.0230781573571433,
-                    "timestamp": 1.729104672799246
-                },
-                {
-                    "x": 7.5160223517191325,
-                    "y": 4.128854891515294,
-                    "heading": 3.1400000000000037,
-                    "angularVelocity": -7.336162887662554e-14,
-                    "velocityX": 1.7860812497217864,
-                    "velocityY": -0.02019700820070875,
-                    "timestamp": 1.7597075444869645
-                },
-                {
-                    "x": 7.562873107995996,
-                    "y": 4.12832502323739,
-                    "heading": 3.1400000000000023,
-                    "angularVelocity": -5.027990878360554e-14,
-                    "velocityX": 1.5309267952025598,
-                    "velocityY": -0.017314331902335514,
-                    "timestamp": 1.790310416174683
-                },
-                {
-                    "x": 7.6019154051050695,
-                    "y": 4.127883409326492,
-                    "heading": 3.1400000000000015,
-                    "angularVelocity": -3.297949891981295e-14,
-                    "velocityX": 1.2757723362526898,
-                    "velocityY": -0.014430472902237576,
-                    "timestamp": 1.8209132878624015
-                },
-                {
-                    "x": 7.63314924293966,
-                    "y": 4.127530078568267,
-                    "heading": 3.1400000000000006,
-                    "angularVelocity": -2.0267021915169143e-14,
-                    "velocityX": 1.0206178738164648,
-                    "velocityY": -0.011545673282216062,
-                    "timestamp": 1.85151615955012
-                },
-                {
-                    "x": 7.656574621413162,
-                    "y": 4.127265054361888,
-                    "heading": 3.14,
-                    "angularVelocity": -1.117466715568672e-14,
-                    "velocityX": 0.7654634085502764,
-                    "velocityY": -0.008660109055131773,
-                    "timestamp": 1.8821190312378384
-                },
-                {
-                    "x": 7.672191540453593,
-                    "y": 4.127088356077733,
-                    "heading": 3.14,
-                    "angularVelocity": -5.057381822634304e-15,
-                    "velocityX": 0.5103089409318914,
-                    "velocityY": -0.005773911868497247,
-                    "timestamp": 1.9127219029255569
+                    "heading": -3.475163170309309e-13,
+                    "angularVelocity": -3.5317243928660403e-16,
+                    "velocityX": 3.5444534056932553,
+                    "velocityY": -1.321924985762245,
+                    "timestamp": 1.1476501107335806
+                },
+                {
+                    "x": 5.5914176787721175,
+                    "y": 4.201991970759951,
+                    "heading": -3.4752963658489967e-13,
+                    "angularVelocity": -4.352387405096259e-16,
+                    "velocityX": 3.625488497149835,
+                    "velocityY": -1.0800304007696078,
+                    "timestamp": 1.1782529824212593
+                },
+                {
+                    "x": 5.704342870003183,
+                    "y": 4.176490587918632,
+                    "heading": -3.4752519346531394e-13,
+                    "angularVelocity": 1.4518636145588672e-16,
+                    "velocityX": 3.6900194329281493,
+                    "velocityY": -0.8333003223399679,
+                    "timestamp": 1.208855854108938
+                },
+                {
+                    "x": 5.818728492967466,
+                    "y": 4.15864825583045,
+                    "heading": -3.47528320752536e-13,
+                    "angularVelocity": -1.0218933861720526e-16,
+                    "velocityX": 3.737741481635173,
+                    "velocityY": -0.5830280331479286,
+                    "timestamp": 1.2394587257966168
+                },
+                {
+                    "x": 5.934052260797228,
+                    "y": 4.148506842880417,
+                    "heading": -3.475166383708343e-13,
+                    "angularVelocity": 3.817413550372978e-16,
+                    "velocityX": 3.768396933681978,
+                    "velocityY": -0.3313876244719343,
+                    "timestamp": 1.2700615974842955
+                },
+                {
+                    "x": 6.049784939896788,
+                    "y": 4.145614314337883,
+                    "heading": -3.4752553093783573e-13,
+                    "angularVelocity": -2.9057949488904467e-16,
+                    "velocityX": 3.7817587931181613,
+                    "velocityY": -0.0945182064161531,
+                    "timestamp": 1.3006644691719742
+                },
+                {
+                    "x": 6.1655453259831,
+                    "y": 4.144216905783092,
+                    "heading": -3.4753489515012823e-13,
+                    "angularVelocity": -3.0599129336581387e-16,
+                    "velocityX": 3.7826641652362696,
+                    "velocityY": -0.04566266099543126,
+                    "timestamp": 1.331267340859653
+                },
+                {
+                    "x": 6.281306292241875,
+                    "y": 4.142868414150292,
+                    "heading": -3.4754223223995867e-13,
+                    "angularVelocity": -2.397516775868154e-16,
+                    "velocityX": 3.7826831233414593,
+                    "velocityY": -0.04406421878574171,
+                    "timestamp": 1.3618702125473316
+                },
+                {
+                    "x": 6.39706739301833,
+                    "y": 4.141531519398385,
+                    "heading": -3.475633687164305e-13,
+                    "angularVelocity": -6.906697084916283e-16,
+                    "velocityX": 3.7826875189315166,
+                    "velocityY": -0.04368527131792852,
+                    "timestamp": 1.3924730842350104
+                },
+                {
+                    "x": 6.512828617962261,
+                    "y": 4.14020542040497,
+                    "heading": -3.4758398313992887e-13,
+                    "angularVelocity": -6.73610754719036e-16,
+                    "velocityX": 3.7826915763114313,
+                    "velocityY": -0.043332501854336236,
+                    "timestamp": 1.423075955922689
+                },
+                {
+                    "x": 6.628589931637748,
+                    "y": 4.138887090943481,
+                    "heading": -3.4757335612235213e-13,
+                    "angularVelocity": 3.472555676974266e-16,
+                    "velocityX": 3.7826944757635745,
+                    "velocityY": -0.04307861939704374,
+                    "timestamp": 1.4536788276103678
+                },
+                {
+                    "x": 6.744351296535766,
+                    "y": 4.137573267392239,
+                    "heading": -3.475819448429605e-13,
+                    "angularVelocity": -2.806508055564177e-16,
+                    "velocityX": 3.782696149545524,
+                    "velocityY": -0.042931381231176444,
+                    "timestamp": 1.4842816992980465
+                },
+                {
+                    "x": 6.860112695906494,
+                    "y": 4.136261713498386,
+                    "heading": -3.475979530506806e-13,
+                    "angularVelocity": -5.230943272138926e-16,
+                    "velocityX": 3.7826972759991557,
+                    "velocityY": -0.04285721636551622,
+                    "timestamp": 1.5148845709857253
+                },
+                {
+                    "x": 6.9694302247553,
+                    "y": 4.135029589899985,
+                    "heading": -2.392571520797574e-13,
+                    "angularVelocity": 3.5402168172320343e-12,
+                    "velocityX": 3.5721330326269505,
+                    "velocityY": -0.04026169865772549,
+                    "timestamp": 1.545487442673404
+                },
+                {
+                    "x": 7.070940187048315,
+                    "y": 4.133884255746116,
+                    "heading": -1.6833543298116243e-13,
+                    "angularVelocity": 2.317485846705618e-12,
+                    "velocityX": 3.3170077412667367,
+                    "velocityY": -0.03742570844047695,
+                    "timestamp": 1.5760903143610827
+                },
+                {
+                    "x": 7.164641690009617,
+                    "y": 4.132826276383854,
+                    "heading": -1.1981581485168293e-13,
+                    "angularVelocity": 1.5854597325485145e-12,
+                    "velocityX": 3.061853276960511,
+                    "velocityY": -0.034571244567840914,
+                    "timestamp": 1.6066931860487614
+                },
+                {
+                    "x": 7.250534738513507,
+                    "y": 4.131855992058627,
+                    "heading": -8.523066967233492e-14,
+                    "angularVelocity": 1.1301275060622065e-12,
+                    "velocityX": 2.8066989719298543,
+                    "velocityY": -0.03170566260882202,
+                    "timestamp": 1.6372960577364402
+                },
+                {
+                    "x": 7.328619329487729,
+                    "y": 4.1309736027819035,
+                    "heading": -6.003549662940244e-14,
+                    "angularVelocity": 8.232944437274219e-13,
+                    "velocityX": 2.5515445665080714,
+                    "velocityY": -0.028833544941282405,
+                    "timestamp": 1.6678989294241189
+                },
+                {
+                    "x": 7.398895462248961,
+                    "y": 4.130179235828021,
+                    "heading": -4.153185117983844e-14,
+                    "angularVelocity": 6.046375734752488e-13,
+                    "velocityX": 2.2963901387575416,
+                    "velocityY": -0.02595726837221309,
+                    "timestamp": 1.6985018011117976
+                },
+                {
+                    "x": 7.461363136410412,
+                    "y": 4.129472977946104,
+                    "heading": -2.797073854746349e-14,
+                    "angularVelocity": 4.431320366500053e-13,
+                    "velocityX": 2.041235698368024,
+                    "velocityY": -0.0230781571305896,
+                    "timestamp": 1.7291046727994763
+                },
+                {
+                    "x": 7.516022351719397,
+                    "y": 4.1288548915008905,
+                    "heading": -1.8141238812912564e-14,
+                    "angularVelocity": 3.211953369611228e-13,
+                    "velocityX": 1.7860812497215437,
+                    "velocityY": -0.02019700801679021,
+                    "timestamp": 1.759707544487155
+                },
+                {
+                    "x": 7.5628731079961895,
+                    "y": 4.128325023227497,
+                    "heading": -1.1163529343344889e-14,
+                    "angularVelocity": 2.2800831618830555e-13,
+                    "velocityX": 1.5309267952022354,
+                    "velocityY": -0.017314331754962387,
+                    "timestamp": 1.7903104161748338
+                },
+                {
+                    "x": 7.601915405105201,
+                    "y": 4.127883409320133,
+                    "heading": -6.372564950074351e-15,
+                    "angularVelocity": 1.5655276164796086e-13,
+                    "velocityX": 1.2757723362523365,
+                    "velocityY": -0.014430472786738478,
+                    "timestamp": 1.8209132878625125
+                },
+                {
+                    "x": 7.63314924293974,
+                    "y": 4.127530078564582,
+                    "heading": -3.2435183420078772e-15,
+                    "angularVelocity": 1.0224681844586284e-13,
+                    "velocityX": 1.020617873816125,
+                    "velocityY": -0.011545673194894595,
+                    "timestamp": 1.8515161595501912
+                },
+                {
+                    "x": 7.656574621413203,
+                    "y": 4.127265054360105,
+                    "heading": -1.3581702257498648e-15,
+                    "angularVelocity": 6.160689169092944e-14,
+                    "velocityX": 0.7654634085499833,
+                    "velocityY": -0.008660108992989924,
+                    "timestamp": 1.88211903123787
+                },
+                {
+                    "x": 7.672191540453606,
+                    "y": 4.127088356077157,
+                    "heading": -3.7222995775515084e-16,
+                    "angularVelocity": 3.2217238765593925e-14,
+                    "velocityX": 0.5103089409316736,
+                    "velocityY": -0.0057739118290542285,
+                    "timestamp": 1.9127219029255487
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": -1.4274059532547784e-15,
-                    "velocityX": 0.2551544713218168,
-                    "velocityY": -0.002887182568422342,
-                    "timestamp": 1.9433247746132754
+                    "heading": 2.9643987227812266e-34,
+                    "angularVelocity": 1.2163231774130929e-14,
+                    "velocityX": 0.25515447132169783,
+                    "velocityY": -0.0028871825495910644,
+                    "timestamp": 1.9433247746132274
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": 3.6920170793948963e-32,
-                    "velocityX": 1.993299363399013e-33,
-                    "velocityY": 1.7597210552293873e-31,
-                    "timestamp": 1.9739276463009938
+                    "heading": -1.7511816257564745e-34,
+                    "angularVelocity": -1.536305146096975e-32,
+                    "velocityX": 2.290424541792184e-34,
+                    "velocityY": 7.21466542895957e-33,
+                    "timestamp": 1.9739276463009061
                 }
             ],
             "constraints": [
@@ -16537,7 +15090,7 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -16546,7 +15099,7 @@
                 {
                     "x": 4.032804489135742,
                     "y": 5.285560607910156,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -16555,7 +15108,7 @@
                 {
                     "x": 5.390789031982422,
                     "y": 4.09412145614624,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -16564,7 +15117,7 @@
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -16575,587 +15128,587 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "angularVelocity": 0,
                     "velocityX": 0,
                     "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.254338747446347,
-                    "y": 5.559909413116517,
-                    "heading": 3.1115254109417156,
-                    "angularVelocity": -0.9384273980850008,
-                    "velocityX": 0.14299063170637197,
-                    "velocityY": -0.0029854412716235888,
-                    "timestamp": 0.030342879072361703
-                },
-                {
-                    "x": 2.263150716185563,
-                    "y": 5.559590842447939,
-                    "heading": 3.055209315133754,
-                    "angularVelocity": -1.855990516709349,
-                    "velocityX": 0.29041307247742426,
-                    "velocityY": -0.010499025745671282,
-                    "timestamp": 0.060685758144723406
-                },
-                {
-                    "x": 2.2766003091950555,
-                    "y": 5.558840744374995,
-                    "heading": 2.9719321573579647,
-                    "angularVelocity": -2.744537114529888,
-                    "velocityX": 0.44325368655420766,
-                    "velocityY": -0.024720728417179556,
-                    "timestamp": 0.09102863721708511
-                },
-                {
-                    "x": 2.2949251974733853,
-                    "y": 5.557404975825004,
-                    "heading": 2.863139887890429,
-                    "angularVelocity": -3.5854300182948253,
-                    "velocityX": 0.6039271433217751,
-                    "velocityY": -0.04731813835356722,
-                    "timestamp": 0.12137151628944681
-                },
-                {
-                    "x": 2.3185983013070146,
-                    "y": 5.55505416569458,
-                    "heading": 2.7318619262997976,
-                    "angularVelocity": -4.326483366247482,
-                    "velocityX": 0.7801864739721567,
-                    "velocityY": -0.07747485414348045,
-                    "timestamp": 0.15171439536180853
-                },
-                {
-                    "x": 2.348656357948328,
-                    "y": 5.552070493546886,
-                    "heading": 2.5849479304647764,
-                    "angularVelocity": -4.841794856864455,
-                    "velocityX": 0.9906132035009321,
-                    "velocityY": -0.0983318735370822,
-                    "timestamp": 0.18205727443417025
-                },
-                {
-                    "x": 2.38579846451748,
-                    "y": 5.549354804791822,
-                    "heading": 2.427729518710298,
-                    "angularVelocity": -5.181394006137112,
-                    "velocityX": 1.2240798403004254,
-                    "velocityY": -0.08950003553016357,
-                    "timestamp": 0.21240015350653196
-                },
-                {
-                    "x": 2.430432513687245,
-                    "y": 5.547202777187511,
-                    "heading": 2.264898570795434,
-                    "angularVelocity": -5.366364461544502,
-                    "velocityX": 1.470989257918541,
-                    "velocityY": -0.07092364568235572,
-                    "timestamp": 0.24274303257889368
-                },
-                {
-                    "x": 2.4823970400851527,
-                    "y": 5.546086320842798,
-                    "heading": 2.109408164193531,
-                    "angularVelocity": -5.12444472494155,
-                    "velocityX": 1.7125773158830133,
-                    "velocityY": -0.03679467403373135,
-                    "timestamp": 0.2730859116512554
-                },
-                {
-                    "x": 2.541153647389717,
-                    "y": 5.546054192747997,
-                    "heading": 1.9668933595072364,
-                    "angularVelocity": -4.696812202508044,
-                    "velocityX": 1.936421628430232,
-                    "velocityY": -0.0010588347507973917,
-                    "timestamp": 0.3034287907236171
-                },
-                {
-                    "x": 2.60683848805178,
-                    "y": 5.546562966972454,
-                    "heading": 1.8368003827209543,
-                    "angularVelocity": -4.287430222954007,
-                    "velocityX": 2.164753071236843,
-                    "velocityY": 0.016767499987179104,
-                    "timestamp": 0.33377166979597883
-                },
-                {
-                    "x": 2.679458734246464,
-                    "y": 5.546832906614029,
-                    "heading": 1.7192242968736162,
-                    "angularVelocity": -3.874915282987571,
-                    "velocityX": 2.3933208849924683,
-                    "velocityY": 0.008896309441567017,
-                    "timestamp": 0.36411454886834055
-                },
-                {
-                    "x": 2.7588038922481295,
-                    "y": 5.545750285790584,
-                    "heading": 1.6147732040322635,
-                    "angularVelocity": -3.442359328930443,
-                    "velocityX": 2.6149515282463223,
-                    "velocityY": -0.035679568206544436,
-                    "timestamp": 0.39445742794070227
-                },
-                {
-                    "x": 2.844227039552491,
-                    "y": 5.54174873240555,
-                    "heading": 1.5239846755378514,
-                    "angularVelocity": -2.992086818060329,
-                    "velocityX": 2.8152617653929393,
-                    "velocityY": -0.13187784110699777,
-                    "timestamp": 0.424800307013064
-                },
-                {
-                    "x": 2.9352842549562403,
-                    "y": 5.535535053694752,
-                    "heading": 1.4520400757805025,
-                    "angularVelocity": -2.371053833941581,
-                    "velocityX": 3.0009418416293347,
-                    "velocityY": -0.20478210706307357,
-                    "timestamp": 0.4551431860854257
-                },
-                {
-                    "x": 3.0322214391168845,
-                    "y": 5.5275999113119605,
-                    "heading": 1.3988891918450945,
-                    "angularVelocity": -1.7516756998785012,
-                    "velocityX": 3.1947259826421948,
-                    "velocityY": -0.26151580289620013,
-                    "timestamp": 0.4854860651577874
-                },
-                {
-                    "x": 3.1352234766500766,
-                    "y": 5.5186341666131185,
-                    "heading": 1.36445468986127,
-                    "angularVelocity": -1.1348462320172443,
-                    "velocityX": 3.394603303383066,
-                    "velocityY": -0.2954810147534299,
-                    "timestamp": 0.5158289442301491
-                },
-                {
-                    "x": 3.2443643044344928,
-                    "y": 5.509491432490049,
-                    "heading": 1.3485232387156947,
-                    "angularVelocity": -0.525047445484061,
-                    "velocityX": 3.5969173368201925,
-                    "velocityY": -0.3013139953287366,
-                    "timestamp": 0.5461718233025108
-                },
-                {
-                    "x": 3.358782949449409,
-                    "y": 5.500338621504484,
-                    "heading": 1.3485196823103536,
-                    "angularVelocity": -0.00011720724762443023,
-                    "velocityX": 3.7708565736972712,
-                    "velocityY": -0.3016460950767756,
-                    "timestamp": 0.5765147023748725
-                },
-                {
-                    "x": 3.4684760810380606,
-                    "y": 5.486343295947389,
-                    "heading": 1.335475696910559,
-                    "angularVelocity": -0.42988621378634684,
-                    "velocityX": 3.615119426441224,
-                    "velocityY": -0.4612392094935691,
-                    "timestamp": 0.6068575814472341
-                },
-                {
-                    "x": 3.573383189765519,
-                    "y": 5.467459869277216,
-                    "heading": 1.309845724977452,
-                    "angularVelocity": -0.8446783138799892,
-                    "velocityX": 3.457388090209756,
-                    "velocityY": -0.6223347041373206,
-                    "timestamp": 0.6372004605195958
-                },
-                {
-                    "x": 3.6735704123100494,
-                    "y": 5.443371550860819,
-                    "heading": 1.272922802934567,
-                    "angularVelocity": -1.216856250022656,
-                    "velocityX": 3.301836398108563,
-                    "velocityY": -0.7938705605012504,
-                    "timestamp": 0.6675433395919574
-                },
-                {
-                    "x": 3.7692345180845526,
-                    "y": 5.4136136865914954,
-                    "heading": 1.2267696953368106,
-                    "angularVelocity": -1.521052352602741,
-                    "velocityX": 3.152769568977396,
-                    "velocityY": -0.9807198650582027,
-                    "timestamp": 0.6978862186643191
-                },
-                {
-                    "x": 3.860739036058752,
-                    "y": 5.377644621585128,
-                    "heading": 1.1742567314728194,
-                    "angularVelocity": -1.730651983905632,
-                    "velocityX": 3.0156834411124778,
-                    "velocityY": -1.1854203063785596,
-                    "timestamp": 0.7282290977366808
-                },
-                {
-                    "x": 3.9485394410884225,
-                    "y": 5.335005141499111,
-                    "heading": 1.1187025848702756,
-                    "angularVelocity": -1.830879214528657,
-                    "velocityX": 2.893608243973284,
-                    "velocityY": -1.4052549194270891,
-                    "timestamp": 0.7585719768090424
+                    "x": 2.253608767824088,
+                    "y": 5.559959646221142,
+                    "heading": 0.04333845773860565,
+                    "angularVelocity": 1.2657316926377489,
+                    "velocityX": 0.10539673178657698,
+                    "velocityY": -0.0011785619397610312,
+                    "timestamp": 0.03423984560921402
+                },
+                {
+                    "x": 2.260842860117559,
+                    "y": 5.560038303784881,
+                    "heading": 0.12991520296053788,
+                    "angularVelocity": 2.5285378389274697,
+                    "velocityX": 0.211277012637124,
+                    "velocityY": 0.002297252290136546,
+                    "timestamp": 0.06847969121842803
+                },
+                {
+                    "x": 2.271629728646886,
+                    "y": 5.560441749821537,
+                    "heading": 0.2597959154554935,
+                    "angularVelocity": 3.7932622120236568,
+                    "velocityX": 0.3150384687021929,
+                    "velocityY": 0.011782939714764799,
+                    "timestamp": 0.10271953682764205
+                },
+                {
+                    "x": 2.285721166472015,
+                    "y": 5.561272024294721,
+                    "heading": 0.433416913958427,
+                    "angularVelocity": 5.070729596286839,
+                    "velocityX": 0.41155085761651194,
+                    "velocityY": 0.024248779701277118,
+                    "timestamp": 0.13695938243685607
+                },
+                {
+                    "x": 2.302746549134424,
+                    "y": 5.561830584813036,
+                    "heading": 0.6513804515506975,
+                    "angularVelocity": 6.36578622695705,
+                    "velocityX": 0.497238885266109,
+                    "velocityY": 0.01631317280720512,
+                    "timestamp": 0.17119922804607007
+                },
+                {
+                    "x": 2.325552284167229,
+                    "y": 5.559809655953846,
+                    "heading": 0.9008347442106217,
+                    "angularVelocity": 7.285497005652254,
+                    "velocityX": 0.6660583488924382,
+                    "velocityY": -0.05902272113767896,
+                    "timestamp": 0.20543907365528408
+                },
+                {
+                    "x": 2.357593597831257,
+                    "y": 5.5547755319598515,
+                    "heading": 1.1463891087836293,
+                    "angularVelocity": 7.171596723173552,
+                    "velocityX": 0.9357902494573596,
+                    "velocityY": -0.14702531230571617,
+                    "timestamp": 0.23967891926449808
+                },
+                {
+                    "x": 2.398826095012215,
+                    "y": 5.54977146589545,
+                    "heading": 1.3795454342192353,
+                    "angularVelocity": 6.809502825937482,
+                    "velocityX": 1.2042255578939407,
+                    "velocityY": -0.14614744825411982,
+                    "timestamp": 0.2739187648737121
+                },
+                {
+                    "x": 2.4467424620473803,
+                    "y": 5.551292722520974,
+                    "heading": 1.6025344347532522,
+                    "angularVelocity": 6.512558586829906,
+                    "velocityX": 1.399432917485785,
+                    "velocityY": 0.04442942421196648,
+                    "timestamp": 0.3081586104829261
+                },
+                {
+                    "x": 2.499282041125752,
+                    "y": 5.560003512593039,
+                    "heading": 1.8091707816342142,
+                    "angularVelocity": 6.034967249541443,
+                    "velocityX": 1.5344572425359657,
+                    "velocityY": 0.2544050627880495,
+                    "timestamp": 0.3423984560921401
+                },
+                {
+                    "x": 2.558927262082263,
+                    "y": 5.571143049666431,
+                    "heading": 1.9899644077603311,
+                    "angularVelocity": 5.2802114877955235,
+                    "velocityX": 1.7419827658469296,
+                    "velocityY": 0.3253384142127747,
+                    "timestamp": 0.3766383017013541
+                },
+                {
+                    "x": 2.624771333399717,
+                    "y": 5.5772713811756995,
+                    "heading": 2.148730759711285,
+                    "angularVelocity": 4.63688866366352,
+                    "velocityX": 1.9230247726273466,
+                    "velocityY": 0.17898245159199333,
+                    "timestamp": 0.4108781473105681
+                },
+                {
+                    "x": 2.6962684360037885,
+                    "y": 5.576686266483743,
+                    "heading": 2.2908744479705683,
+                    "angularVelocity": 4.151411483614645,
+                    "velocityX": 2.0881257298902987,
+                    "velocityY": -0.0170887070763856,
+                    "timestamp": 0.4451179929197821
+                },
+                {
+                    "x": 2.773687793639176,
+                    "y": 5.569334398339137,
+                    "heading": 2.4184902740666434,
+                    "angularVelocity": 3.727114530613817,
+                    "velocityX": 2.2610895656186547,
+                    "velocityY": -0.2147167434256298,
+                    "timestamp": 0.4793578385289961
+                },
+                {
+                    "x": 2.8582271385194487,
+                    "y": 5.556743437974915,
+                    "heading": 2.5300657438997165,
+                    "angularVelocity": 3.258644069441941,
+                    "velocityX": 2.4690340559690878,
+                    "velocityY": -0.36772830426646747,
+                    "timestamp": 0.5135976841382102
+                },
+                {
+                    "x": 2.9508927177061977,
+                    "y": 5.541492370817128,
+                    "heading": 2.6233336392816216,
+                    "angularVelocity": 2.7239578252305745,
+                    "velocityX": 2.706366735538439,
+                    "velocityY": -0.445418689437171,
+                    "timestamp": 0.5478375297474242
+                },
+                {
+                    "x": 3.0518533392296137,
+                    "y": 5.525810656425357,
+                    "heading": 2.697205719709221,
+                    "angularVelocity": 2.1574887127329996,
+                    "velocityX": 2.948629578406965,
+                    "velocityY": -0.45799606022611716,
+                    "timestamp": 0.5820773753566382
+                },
+                {
+                    "x": 3.1608544462331407,
+                    "y": 5.511189976920693,
+                    "heading": 2.751249379299885,
+                    "angularVelocity": 1.5783850256649679,
+                    "velocityX": 3.183457900119567,
+                    "velocityY": -0.42700775206563796,
+                    "timestamp": 0.6163172209658522
+                },
+                {
+                    "x": 3.2775207255393237,
+                    "y": 5.498694402974223,
+                    "heading": 2.785399702937351,
+                    "angularVelocity": 0.9973854446433661,
+                    "velocityX": 3.40732492306531,
+                    "velocityY": -0.36494247342945346,
+                    "timestamp": 0.6505570665750662
+                },
+                {
+                    "x": 3.401135401206032,
+                    "y": 5.488837187261409,
+                    "heading": 2.800896234569334,
+                    "angularVelocity": 0.4525876608454414,
+                    "velocityX": 3.610257974803583,
+                    "velocityY": -0.28788727102673717,
+                    "timestamp": 0.6847969121842802
+                },
+                {
+                    "x": 3.5179017921083777,
+                    "y": 5.474779629986763,
+                    "heading": 2.835487088185811,
+                    "angularVelocity": 1.0102514483058458,
+                    "velocityX": 3.410248756230483,
+                    "velocityY": -0.4105613510962388,
+                    "timestamp": 0.7190367577934942
+                },
+                {
+                    "x": 3.6287098119916026,
+                    "y": 5.4546983370085105,
+                    "heading": 2.8876881760682602,
+                    "angularVelocity": 1.5245713569573334,
+                    "velocityX": 3.2362301263825226,
+                    "velocityY": -0.5864890048700768,
+                    "timestamp": 0.7532766034027082
+                },
+                {
+                    "x": 3.7356372636451365,
+                    "y": 5.425785607175325,
+                    "heading": 2.9455736884009496,
+                    "angularVelocity": 1.6905891747687094,
+                    "velocityX": 3.1228952628442794,
+                    "velocityY": -0.8444176461299453,
+                    "timestamp": 0.7875164490119222
+                },
+                {
+                    "x": 3.8388362586877394,
+                    "y": 5.387864157402447,
+                    "heading": 3.0009390226743027,
+                    "angularVelocity": 1.6169855117119507,
+                    "velocityX": 3.0140029315678847,
+                    "velocityY": -1.1075239709221774,
+                    "timestamp": 0.8217562946211362
+                },
+                {
+                    "x": 3.9379980862804507,
+                    "y": 5.341070528950818,
+                    "heading": 3.053673275383994,
+                    "angularVelocity": 1.5401428298350917,
+                    "velocityX": 2.896094472050628,
+                    "velocityY": -1.3666425072616961,
+                    "timestamp": 0.8559961402303502
                 },
                 {
                     "x": 4.032804489135742,
                     "y": 5.285560607910156,
-                    "heading": 1.064415461383156,
-                    "angularVelocity": -1.7891223623722634,
-                    "velocityX": 2.7770946799861798,
-                    "velocityY": -1.6295267654410455,
-                    "timestamp": 0.7889148558814041
-                },
-                {
-                    "x": 4.1733297943826075,
-                    "y": 5.174158598481438,
-                    "heading": 0.9768931525481981,
-                    "angularVelocity": -1.5934853179105337,
-                    "velocityX": 2.55849067153879,
-                    "velocityY": -2.0282539248952025,
-                    "timestamp": 0.8438399360507789
-                },
-                {
-                    "x": 4.303490854924698,
-                    "y": 5.0407757547823575,
-                    "heading": 0.9131112872795231,
-                    "angularVelocity": -1.1612521105474607,
-                    "velocityX": 2.369792818521288,
-                    "velocityY": -2.428450596480911,
-                    "timestamp": 0.8987650162201537
-                },
-                {
-                    "x": 4.424699657845323,
-                    "y": 4.885599604328108,
-                    "heading": 0.881895788498386,
-                    "angularVelocity": -0.5683286885495016,
-                    "velocityX": 2.206802476152012,
-                    "velocityY": -2.8252330260734433,
-                    "timestamp": 0.9536900963895285
-                },
-                {
-                    "x": 4.534874604263042,
-                    "y": 4.709437065112464,
-                    "heading": 0.8818953110956026,
-                    "angularVelocity": -0.000008691890515565163,
-                    "velocityX": 2.005913256347883,
-                    "velocityY": -3.2073242073093726,
-                    "timestamp": 1.0086151765589033
-                },
-                {
-                    "x": 4.656392865207935,
-                    "y": 4.550190331352248,
-                    "heading": 0.8602662015486029,
-                    "angularVelocity": -0.3937929536070082,
-                    "velocityX": 2.2124366604502335,
-                    "velocityY": -2.899344584826112,
-                    "timestamp": 1.0635402567282781
-                },
-                {
-                    "x": 4.786084989350276,
-                    "y": 4.412405730848589,
-                    "heading": 0.8009192530896463,
-                    "angularVelocity": -1.0805072705573824,
-                    "velocityX": 2.3612550722257284,
-                    "velocityY": -2.508591704896303,
-                    "timestamp": 1.118465336897653
-                },
-                {
-                    "x": 4.924876209285355,
-                    "y": 4.297327162575611,
-                    "heading": 0.7205421769770328,
-                    "angularVelocity": -1.463394789133692,
-                    "velocityX": 2.5269188412121024,
-                    "velocityY": -2.0951916304556226,
-                    "timestamp": 1.1733904170670277
-                },
-                {
-                    "x": 5.072286801869267,
-                    "y": 4.205735025902594,
-                    "heading": 0.6308477205816693,
-                    "angularVelocity": -1.6330327806307947,
-                    "velocityX": 2.6838484737634416,
-                    "velocityY": -1.6675831221469524,
-                    "timestamp": 1.2283154972364025
-                },
-                {
-                    "x": 5.227765920434763,
-                    "y": 4.1379313687941846,
-                    "heading": 0.5364496515071664,
-                    "angularVelocity": -1.7186696639022372,
-                    "velocityX": 2.830749050998892,
-                    "velocityY": -1.2344753416712253,
-                    "timestamp": 1.2832405774057773
+                    "heading": 3.1036105469083837,
+                    "angularVelocity": 1.4584549268806075,
+                    "velocityX": 2.76889107320562,
+                    "velocityY": -1.62120827512502,
+                    "timestamp": 0.8902359858395642
+                },
+                {
+                    "x": 4.173725003261308,
+                    "y": 5.173637687034127,
+                    "heading": 3.175484729095798,
+                    "angularVelocity": 1.2994086167535415,
+                    "velocityX": 2.547692714675012,
+                    "velocityY": -2.023447131813128,
+                    "timestamp": 0.9455489807364162
+                },
+                {
+                    "x": 4.301872286760783,
+                    "y": 5.039921697201636,
+                    "heading": 3.2346580028937466,
+                    "angularVelocity": 1.0697897285853943,
+                    "velocityX": 2.3167663175433564,
+                    "velocityY": -2.4174425934058767,
+                    "timestamp": 1.0008619756332682
+                },
+                {
+                    "x": 4.416442024233276,
+                    "y": 4.885133369968262,
+                    "heading": 3.2770092965554323,
+                    "angularVelocity": 0.765666255111711,
+                    "velocityX": 2.0712987551323803,
+                    "velocityY": -2.7984079965661373,
+                    "timestamp": 1.0561749705301202
+                },
+                {
+                    "x": 4.517908742039174,
+                    "y": 4.711097539386313,
+                    "heading": 3.282986865137276,
+                    "angularVelocity": 0.10806806959180061,
+                    "velocityX": 1.834410123608644,
+                    "velocityY": -3.146382344808711,
+                    "timestamp": 1.1114879654269723
+                },
+                {
+                    "x": 4.627451956204509,
+                    "y": 4.5560902871099245,
+                    "heading": 3.2338916761611958,
+                    "angularVelocity": -0.8875886953442504,
+                    "velocityX": 1.9804245705663106,
+                    "velocityY": -2.802365928032756,
+                    "timestamp": 1.1668009603238243
+                },
+                {
+                    "x": 4.751713919325069,
+                    "y": 4.421499213962681,
+                    "heading": 3.169974564154514,
+                    "angularVelocity": -1.1555532678328913,
+                    "velocityX": 2.246523865726009,
+                    "velocityY": -2.433263167149585,
+                    "timestamp": 1.2221139552206763
+                },
+                {
+                    "x": 4.891049798654671,
+                    "y": 4.307366204928411,
+                    "heading": 3.098048256171184,
+                    "angularVelocity": -1.3003509955925967,
+                    "velocityX": 2.5190442063286667,
+                    "velocityY": -2.063403170396147,
+                    "timestamp": 1.2774269501175284
+                },
+                {
+                    "x": 5.0446094980944505,
+                    "y": 4.214363163694223,
+                    "heading": 3.021068837736617,
+                    "angularVelocity": -1.3917058473893738,
+                    "velocityX": 2.7761957154216415,
+                    "velocityY": -1.6813958710357464,
+                    "timestamp": 1.3327399450143804
+                },
+                {
+                    "x": 5.211468562298102,
+                    "y": 4.143120621239898,
+                    "heading": 2.9418200950482523,
+                    "angularVelocity": -1.432732811451417,
+                    "velocityX": 3.016634057056783,
+                    "velocityY": -1.2879892435254758,
+                    "timestamp": 1.3880529399112325
                 },
                 {
                     "x": 5.390789031982422,
                     "y": 4.09412145614624,
-                    "heading": 0.44081122980213755,
-                    "angularVelocity": -1.7412522914869581,
-                    "velocityX": 2.968099655839153,
-                    "velocityY": -0.7976303814732078,
-                    "timestamp": 1.3381656575751522
-                },
-                {
-                    "x": 5.527208209694737,
-                    "y": 4.074474240030864,
-                    "heading": 0.3646632920053755,
-                    "angularVelocity": -1.7150127332494942,
-                    "velocityX": 3.0724486257327652,
-                    "velocityY": -0.44249689204593257,
-                    "timestamp": 1.3825664578260528
-                },
-                {
-                    "x": 5.669753286288942,
-                    "y": 4.0699039581909835,
-                    "heading": 0.2967653403322419,
-                    "angularVelocity": -1.529205583896121,
-                    "velocityX": 3.2104168345775173,
-                    "velocityY": -0.10293242045310311,
-                    "timestamp": 1.4269672580769535
-                },
-                {
-                    "x": 5.825688318788907,
-                    "y": 4.069335183916362,
-                    "heading": 0.2632559067133074,
-                    "angularVelocity": -0.7547033708757247,
-                    "velocityX": 3.5119869826400643,
-                    "velocityY": -0.012810000527210512,
-                    "timestamp": 1.4713680583278541
-                },
-                {
-                    "x": 5.986467377363025,
-                    "y": 4.053794613625281,
-                    "heading": 0.24734267435752091,
-                    "angularVelocity": -0.35839967446225596,
-                    "velocityX": 3.6210847026536497,
-                    "velocityY": -0.3500065359917586,
-                    "timestamp": 1.5157688585787548
-                },
-                {
-                    "x": 6.146135354603106,
-                    "y": 4.023716272835588,
-                    "heading": 0.25998224016695326,
-                    "angularVelocity": 0.28466977482406836,
-                    "velocityX": 3.5960607993060463,
-                    "velocityY": -0.6774278981398296,
-                    "timestamp": 1.5601696588296554
-                },
-                {
-                    "x": 6.299693722526358,
-                    "y": 3.978509002442476,
-                    "heading": 0.279745523920822,
-                    "angularVelocity": 0.4451109809325518,
-                    "velocityX": 3.4584594659447885,
-                    "velocityY": -1.0181634145703302,
-                    "timestamp": 1.604570459080556
-                },
-                {
-                    "x": 6.446348354717166,
-                    "y": 3.918416502871273,
-                    "heading": 0.30303477341760615,
-                    "angularVelocity": 0.5245231924915958,
-                    "velocityX": 3.3029727248628453,
-                    "velocityY": -1.3534102816082356,
-                    "timestamp": 1.6489712593314567
-                },
-                {
-                    "x": 6.585501043822193,
-                    "y": 3.8437018977675175,
-                    "heading": 0.32757979886137184,
-                    "angularVelocity": 0.5528059247821282,
-                    "velocityX": 3.134013088023211,
-                    "velocityY": -1.6827310472234156,
-                    "timestamp": 1.6933720595823574
-                },
-                {
-                    "x": 6.716485001612935,
-                    "y": 3.754724015839551,
-                    "heading": 0.3514161297676369,
-                    "angularVelocity": 0.5368446237808859,
-                    "velocityX": 2.950035968959475,
-                    "velocityY": -2.0039702308329863,
-                    "timestamp": 1.737772859833258
-                },
-                {
-                    "x": 6.838464987128356,
-                    "y": 3.6520072183603935,
-                    "heading": 0.37305697715168934,
-                    "angularVelocity": 0.48739768791922733,
-                    "velocityX": 2.747247455589403,
-                    "velocityY": -2.313399688715609,
-                    "timestamp": 1.7821736600841587
-                },
-                {
-                    "x": 6.950225204946805,
-                    "y": 3.536435013339574,
-                    "heading": 0.39217088238591646,
-                    "angularVelocity": 0.430485602201265,
-                    "velocityX": 2.5170766559817053,
-                    "velocityY": -2.60293067619823,
-                    "timestamp": 1.8265744603350593
-                },
-                {
-                    "x": 7.049675913816151,
-                    "y": 3.409971379240331,
-                    "heading": 0.4121286773514249,
-                    "angularVelocity": 0.44949178511942617,
-                    "velocityX": 2.23984046024776,
-                    "velocityY": -2.848228711748902,
-                    "timestamp": 1.87097526058596
-                },
-                {
-                    "x": 7.134078905097545,
-                    "y": 3.2784202019721485,
-                    "heading": 0.44760621801548245,
-                    "angularVelocity": 0.799029307210242,
-                    "velocityX": 1.9009340102981855,
-                    "velocityY": -2.9628109521632653,
-                    "timestamp": 1.9153760608368606
-                },
-                {
-                    "x": 7.206535248844668,
-                    "y": 3.1515334712387078,
-                    "heading": 0.5188452438191252,
-                    "angularVelocity": 1.6044536450037898,
-                    "velocityX": 1.6318702216556016,
-                    "velocityY": -2.857757743473708,
-                    "timestamp": 1.9597768610877613
-                },
-                {
-                    "x": 7.286286308378336,
-                    "y": 3.038022763329368,
-                    "heading": 0.612459613040285,
-                    "angularVelocity": 2.1083937382245868,
-                    "velocityX": 1.796162661100898,
-                    "velocityY": -2.556501397900756,
-                    "timestamp": 2.004177661338662
-                },
-                {
-                    "x": 7.36964311440034,
-                    "y": 2.9389690479632176,
-                    "heading": 0.7307138885874557,
-                    "angularVelocity": 2.6633365812989322,
-                    "velocityX": 1.8773717039100728,
-                    "velocityY": -2.2308993262827714,
-                    "timestamp": 2.0485784615895626
-                },
-                {
-                    "x": 7.443994240643498,
-                    "y": 2.8513361754602453,
-                    "heading": 0.8783950719218284,
-                    "angularVelocity": 3.326092829405186,
-                    "velocityX": 1.674544733946552,
-                    "velocityY": -1.9736777717467908,
-                    "timestamp": 2.0929792618404632
-                },
-                {
-                    "x": 7.506362936213548,
-                    "y": 2.772668193478103,
-                    "heading": 1.052033337356815,
-                    "angularVelocity": 3.9107012588464345,
-                    "velocityX": 1.4046750332790265,
-                    "velocityY": -1.7717694622079865,
-                    "timestamp": 2.137380062091364
-                },
-                {
-                    "x": 7.555798075190431,
-                    "y": 2.7014303099088472,
-                    "heading": 1.2519057769914284,
-                    "angularVelocity": 4.501550388848195,
-                    "velocityX": 1.1133839637469243,
-                    "velocityY": -1.6044279194677442,
-                    "timestamp": 2.1817808623422645
-                },
-                {
-                    "x": 7.592358210662926,
-                    "y": 2.6364422153707756,
-                    "heading": 1.482218675635827,
-                    "angularVelocity": 5.187133955760781,
-                    "velocityX": 0.8234116337070656,
-                    "velocityY": -1.4636694422360859,
-                    "timestamp": 2.226181662593165
-                },
-                {
-                    "x": 7.621254126448735,
-                    "y": 2.5813646495023557,
-                    "heading": 1.7524007538852981,
-                    "angularVelocity": 6.085072267227659,
-                    "velocityX": 0.6507971843417887,
-                    "velocityY": -1.2404633600562773,
-                    "timestamp": 2.270582462844066
-                },
-                {
-                    "x": 7.645052051647252,
-                    "y": 2.5375067096061756,
-                    "heading": 2.067951038154778,
-                    "angularVelocity": 7.10686029275066,
-                    "velocityX": 0.5359796459532341,
-                    "velocityY": -0.9877736357981667,
-                    "timestamp": 2.3149832630949665
-                },
-                {
-                    "x": 7.661427436541854,
-                    "y": 2.5075370043722347,
-                    "heading": 2.400674072151774,
-                    "angularVelocity": 7.493626964307851,
-                    "velocityX": 0.36880832782444795,
-                    "velocityY": -0.6749811954871879,
-                    "timestamp": 2.359384063345867
-                },
-                {
-                    "x": 7.6692908711764884,
-                    "y": 2.4875264930527843,
-                    "heading": 2.6956822674348517,
-                    "angularVelocity": 6.644208969569037,
-                    "velocityX": 0.17710119164969557,
-                    "velocityY": -0.4506790689891867,
-                    "timestamp": 2.403784863596768
-                },
-                {
-                    "x": 7.674629616295112,
-                    "y": 2.472122256604734,
-                    "heading": 2.9172973649743583,
-                    "angularVelocity": 4.991241065188027,
-                    "velocityX": 0.12023984001312674,
-                    "velocityY": -0.3469360092836942,
-                    "timestamp": 2.4481856638476684
-                },
-                {
-                    "x": 7.678206340030146,
-                    "y": 2.4619495353215974,
-                    "heading": 3.0655099943755024,
-                    "angularVelocity": 3.3380621196830216,
-                    "velocityX": 0.0805553889754595,
-                    "velocityY": -0.22911121479010477,
-                    "timestamp": 2.492586464098569
+                    "heading": 2.865640793449113,
+                    "angularVelocity": -1.3772405876991196,
+                    "velocityX": 3.2419229878750393,
+                    "velocityY": -0.8858526858838836,
+                    "timestamp": 1.4433659348080845
+                },
+                {
+                    "x": 5.535592658236132,
+                    "y": 4.049337172847784,
+                    "heading": 2.841642220937184,
+                    "angularVelocity": -0.5722130304709164,
+                    "velocityX": 3.452643767066762,
+                    "velocityY": -1.0678197818199107,
+                    "timestamp": 1.4853058614332775
+                },
+                {
+                    "x": 5.681734895850587,
+                    "y": 4.003264514213434,
+                    "heading": 2.8233979117656802,
+                    "angularVelocity": -0.4350105171749157,
+                    "velocityX": 3.484561118108144,
+                    "velocityY": -1.098539323783022,
+                    "timestamp": 1.5272457880584704
+                },
+                {
+                    "x": 5.827742551539107,
+                    "y": 3.9572785916007636,
+                    "heading": 2.804954082691072,
+                    "angularVelocity": -0.4397677954812897,
+                    "velocityX": 3.481352196758831,
+                    "velocityY": -1.096471222366128,
+                    "timestamp": 1.5691857146836634
+                },
+                {
+                    "x": 5.9679573209760495,
+                    "y": 3.908501280249766,
+                    "heading": 2.7717324952229117,
+                    "angularVelocity": -0.7921231661908814,
+                    "velocityX": 3.3432287731451744,
+                    "velocityY": -1.1630280564605782,
+                    "timestamp": 1.6111256413088564
+                },
+                {
+                    "x": 6.096940014064459,
+                    "y": 3.8543767396001813,
+                    "heading": 2.711734513417447,
+                    "angularVelocity": -1.4305695463335533,
+                    "velocityX": 3.07541532537948,
+                    "velocityY": -1.2905254015650245,
+                    "timestamp": 1.6530655679340494
+                },
+                {
+                    "x": 6.215088187160598,
+                    "y": 3.7918967611554555,
+                    "heading": 2.632459037184743,
+                    "angularVelocity": -1.890214948184595,
+                    "velocityX": 2.8170810633981405,
+                    "velocityY": -1.4897493503766595,
+                    "timestamp": 1.6950054945592423
+                },
+                {
+                    "x": 6.32424952389229,
+                    "y": 3.717983083464812,
+                    "heading": 2.5463998013121922,
+                    "angularVelocity": -2.0519643880553406,
+                    "velocityX": 2.6028022821127137,
+                    "velocityY": -1.7623702194615716,
+                    "timestamp": 1.7369454211844353
+                },
+                {
+                    "x": 6.43630340548619,
+                    "y": 3.6322698094657313,
+                    "heading": 2.4886072976482807,
+                    "angularVelocity": -1.3779829464268867,
+                    "velocityX": 2.6717710451737124,
+                    "velocityY": -2.043715401914737,
+                    "timestamp": 1.7788853478096283
+                },
+                {
+                    "x": 6.552059359988985,
+                    "y": 3.5378523447078845,
+                    "heading": 2.4573864362362134,
+                    "angularVelocity": -0.7444185987992016,
+                    "velocityX": 2.7600418936656252,
+                    "velocityY": -2.251254886581771,
+                    "timestamp": 1.8208252744348212
+                },
+                {
+                    "x": 6.67032629558411,
+                    "y": 3.432112823682954,
+                    "heading": 2.4573457680499984,
+                    "angularVelocity": -0.0009696770950155036,
+                    "velocityX": 2.8199127922194083,
+                    "velocityY": -2.521213782033988,
+                    "timestamp": 1.8627652010600142
+                },
+                {
+                    "x": 6.787202921818474,
+                    "y": 3.3248215481814336,
+                    "heading": 2.4573432508654287,
+                    "angularVelocity": -0.00006001881195361165,
+                    "velocityX": 2.7867627733081557,
+                    "velocityY": -2.558213238195593,
+                    "timestamp": 1.9047051276852072
+                },
+                {
+                    "x": 6.904079764497574,
+                    "y": 3.217530508972575,
+                    "heading": 2.4573407325391345,
+                    "angularVelocity": -0.000060046034804859665,
+                    "velocityX": 2.7867679341358973,
+                    "velocityY": -2.5582076041213115,
+                    "timestamp": 1.9466450543104001
+                },
+                {
+                    "x": 7.017539426879104,
+                    "y": 3.1193532157248125,
+                    "heading": 2.428286840001792,
+                    "angularVelocity": -0.6927501995172902,
+                    "velocityX": 2.7052899590285975,
+                    "velocityY": -2.3409028376503684,
+                    "timestamp": 1.988584980935593
+                },
+                {
+                    "x": 7.118887938132189,
+                    "y": 3.020968055873212,
+                    "heading": 2.3710302150679574,
+                    "angularVelocity": -1.365205653446252,
+                    "velocityX": 2.4165161794108876,
+                    "velocityY": -2.345859131582302,
+                    "timestamp": 2.030524907560786
+                },
+                {
+                    "x": 7.207551237715759,
+                    "y": 2.9223074295194613,
+                    "heading": 2.2882909026899103,
+                    "angularVelocity": -1.9728053679604207,
+                    "velocityX": 2.1140547139229047,
+                    "velocityY": -2.3524272523282552,
+                    "timestamp": 2.0724648341859786
+                },
+                {
+                    "x": 7.28673413242529,
+                    "y": 2.8318719270188626,
+                    "heading": 2.1761461762136682,
+                    "angularVelocity": -2.6739371167349164,
+                    "velocityX": 1.8880074688058086,
+                    "velocityY": -2.1563104606452623,
+                    "timestamp": 2.1144047608111713
+                },
+                {
+                    "x": 7.3579924776067,
+                    "y": 2.7517426977667294,
+                    "heading": 2.0378729016459984,
+                    "angularVelocity": -3.2969364921256217,
+                    "velocityX": 1.6990574594521515,
+                    "velocityY": -1.9105715174046114,
+                    "timestamp": 2.156344687436364
+                },
+                {
+                    "x": 7.4224137082831785,
+                    "y": 2.6828558926645387,
+                    "heading": 1.874977709266495,
+                    "angularVelocity": -3.884012335912244,
+                    "velocityX": 1.5360358460374843,
+                    "velocityY": -1.6425113405136766,
+                    "timestamp": 2.198284614061557
+                },
+                {
+                    "x": 7.480883603746462,
+                    "y": 2.6255140291502124,
+                    "heading": 1.6865689006450588,
+                    "angularVelocity": -4.4923495051672475,
+                    "velocityX": 1.3941344243592637,
+                    "velocityY": -1.3672380504328716,
+                    "timestamp": 2.2402245406867496
+                },
+                {
+                    "x": 7.534437188323511,
+                    "y": 2.579534136686106,
+                    "heading": 1.4695142507662173,
+                    "angularVelocity": -5.175370281846384,
+                    "velocityX": 1.27691173748694,
+                    "velocityY": -1.0963274417482318,
+                    "timestamp": 2.2821644673119423
+                },
+                {
+                    "x": 7.579534895925645,
+                    "y": 2.5413039154996566,
+                    "heading": 1.2172257697546927,
+                    "angularVelocity": -6.015472636997345,
+                    "velocityX": 1.0752929542572058,
+                    "velocityY": -0.9115471643072651,
+                    "timestamp": 2.324104393937135
+                },
+                {
+                    "x": 7.615712715030757,
+                    "y": 2.5097826728946813,
+                    "heading": 0.9253708991340255,
+                    "angularVelocity": -6.958878903840344,
+                    "velocityX": 0.8626104530039771,
+                    "velocityY": -0.7515807761580874,
+                    "timestamp": 2.366044320562328
+                },
+                {
+                    "x": 7.6401514150011085,
+                    "y": 2.4865499215178333,
+                    "heading": 0.6440959436378817,
+                    "angularVelocity": -6.706615345559152,
+                    "velocityX": 0.5827072657697844,
+                    "velocityY": -0.5539530763721487,
+                    "timestamp": 2.4079842471875206
+                },
+                {
+                    "x": 7.656070937434432,
+                    "y": 2.471863188851966,
+                    "heading": 0.3945229348796906,
+                    "angularVelocity": -5.950725927314207,
+                    "velocityX": 0.37957916749813,
+                    "velocityY": -0.3501849871393242,
+                    "timestamp": 2.4499241738127133
+                },
+                {
+                    "x": 7.668072859318959,
+                    "y": 2.4642781587251137,
+                    "heading": 0.19785094472919199,
+                    "angularVelocity": -4.689373729909185,
+                    "velocityX": 0.28616935818189254,
+                    "velocityY": -0.18085463512222474,
+                    "timestamp": 2.491864100437906
+                },
+                {
+                    "x": 7.676136280306529,
+                    "y": 2.459442710773055,
+                    "heading": 0.06623696286278732,
+                    "angularVelocity": -3.1381547956105695,
+                    "velocityX": 0.19226120874341576,
+                    "velocityY": -0.11529462116784107,
+                    "timestamp": 2.533804027063099
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": 1.6776725915652129,
-                    "velocityX": 0.04039701896629419,
-                    "velocityY": -0.11147401158601906,
-                    "timestamp": 2.5369872643494698
+                    "heading": 2.1057025676027217e-33,
+                    "angularVelocity": -1.57932948845455,
+                    "velocityX": 0.09212509425682942,
+                    "velocityY": -0.058243086471875524,
+                    "timestamp": 2.5757439536882916
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": -5.831834401931518e-35,
-                    "velocityX": -1.3540056730700436e-35,
-                    "velocityY": -1.938442767179852e-35,
-                    "timestamp": 2.5813880646003704
+                    "heading": -7.524501141381425e-34,
+                    "angularVelocity": -5.907340858361994e-32,
+                    "velocityX": -7.634877430876997e-33,
+                    "velocityY": -7.90956322394541e-33,
+                    "timestamp": 2.6176838803134843
                 }
             ],
             "constraints": [
@@ -17198,7 +15751,7 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -17207,7 +15760,7 @@
                 {
                     "x": 2.1495614051818848,
                     "y": 3.005171298980713,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -17216,7 +15769,7 @@
                 {
                     "x": 4.929586887359619,
                     "y": 1.5703192949295044,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -17225,7 +15778,7 @@
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -17236,623 +15789,623 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "angularVelocity": 0,
                     "velocityX": 0,
-                    "velocityY": -2.730529759762368e-29,
+                    "velocityY": 6.892560738534845e-27,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.2482118663055717,
-                    "y": 5.55085896880981,
-                    "heading": 3.1399999903330857,
-                    "angularVelocity": -2.8923280730678937e-7,
-                    "velocityX": -0.05350072563170815,
-                    "velocityY": -0.2734984543883791,
-                    "timestamp": 0.03342260639319256
-                },
-                {
-                    "x": 2.24463557685663,
-                    "y": 5.5325769109933445,
-                    "heading": 3.1399999700354155,
-                    "angularVelocity": -6.073036282303753e-7,
-                    "velocityX": -0.10700211129830166,
-                    "velocityY": -0.5469967722250598,
-                    "timestamp": 0.06684521278638512
-                },
-                {
-                    "x": 2.2392711054637595,
-                    "y": 5.505153831967315,
-                    "heading": 3.139999938021822,
-                    "angularVelocity": -9.578425018869592e-7,
-                    "velocityX": -0.16050428054878876,
-                    "velocityY": -0.8204949279944175,
-                    "timestamp": 0.1002678191795777
-                },
-                {
-                    "x": 2.2321184205712044,
-                    "y": 5.468589738256669,
-                    "heading": 3.139999893047122,
-                    "angularVelocity": -0.0000013456371104363374,
-                    "velocityX": -0.2140073939433933,
-                    "velocityY": -1.0939928885381913,
-                    "timestamp": 0.13369042557277025
-                },
-                {
-                    "x": 2.2231774834707245,
-                    "y": 5.422884637863401,
-                    "heading": 3.1399998336497137,
-                    "angularVelocity": -0.000001777162647778402,
-                    "velocityX": -0.2675116654835123,
-                    "velocityY": -1.3674906096633432,
-                    "timestamp": 0.1671130319659628
-                },
-                {
-                    "x": 2.2124482456246803,
-                    "y": 5.368038540819271,
-                    "heading": 3.139999758066302,
-                    "angularVelocity": -0.0000022614457562380146,
-                    "velocityX": -0.3210173892634857,
-                    "velocityY": -1.6409880306395483,
-                    "timestamp": 0.20053563835915536
-                },
-                {
-                    "x": 2.199930644465073,
-                    "y": 5.30405146005317,
-                    "heading": 3.139999664095155,
-                    "angularVelocity": -0.0000028116043956655657,
-                    "velocityX": -0.3745249850698391,
-                    "velocityY": -1.9144850647849765,
-                    "timestamp": 0.23395824475234792
-                },
-                {
-                    "x": 2.1856245964060905,
-                    "y": 5.230923412833771,
-                    "heading": 3.1399995488597034,
-                    "angularVelocity": -0.0000034478296167107145,
-                    "velocityX": -0.4280350817528267,
-                    "velocityY": -2.1879815822539745,
-                    "timestamp": 0.2673808511455405
-                },
-                {
-                    "x": 2.1695299843090114,
-                    "y": 5.148654423357543,
-                    "heading": 3.139999408375343,
-                    "angularVelocity": -0.000004203273662517842,
-                    "velocityX": -0.4815486832110918,
-                    "velocityY": -2.461477375767798,
-                    "timestamp": 0.30080345753873305
-                },
-                {
-                    "x": 2.15164663254415,
-                    "y": 5.057244527897619,
-                    "heading": 3.139999236662074,
-                    "angularVelocity": -0.000005137638535462322,
-                    "velocityX": -0.535067539486715,
-                    "velocityY": -2.7349720839942284,
-                    "timestamp": 0.3342260639319256
-                },
-                {
-                    "x": 2.1319742493327887,
-                    "y": 4.956693786709351,
-                    "heading": 3.1399990236359914,
-                    "angularVelocity": -0.000006373712524248939,
-                    "velocityX": -0.5885951257435309,
-                    "velocityY": -3.008464989388312,
-                    "timestamp": 0.36764867032511817
-                },
-                {
-                    "x": 2.110512257081521,
-                    "y": 4.847002319075526,
-                    "heading": 3.13999874874802,
-                    "angularVelocity": -0.000008224612190539215,
-                    "velocityX": -0.6421399935196433,
-                    "velocityY": -3.28195432585699,
-                    "timestamp": 0.4010712767183107
-                },
-                {
-                    "x": 2.087258999095236,
-                    "y": 4.728170467468909,
-                    "heading": 3.139998349832889,
-                    "angularVelocity": -0.00001193548864433131,
-                    "velocityX": -0.6957344293958355,
-                    "velocityY": -3.555433415585576,
-                    "timestamp": 0.4344938831115033
-                },
-                {
-                    "x": 2.0629644962421034,
-                    "y": 4.604090796237193,
-                    "heading": 3.1399983498325463,
-                    "angularVelocity": -1.0248884056674418e-11,
-                    "velocityX": -0.7268883391614586,
-                    "velocityY": -3.712447490549055,
-                    "timestamp": 0.46791648950469583
-                },
-                {
-                    "x": 2.0387448816492295,
-                    "y": 4.479996482972041,
-                    "heading": 3.139998349833487,
-                    "angularVelocity": 2.8141899837441888e-11,
-                    "velocityX": -0.7246476922026518,
-                    "velocityY": -3.712885578275693,
-                    "timestamp": 0.5013390958978884
-                },
-                {
-                    "x": 2.0146234361555044,
-                    "y": 4.355883050210464,
-                    "heading": 3.139998349833582,
-                    "angularVelocity": 2.8336640394810287e-12,
-                    "velocityX": -0.7217104856241171,
-                    "velocityY": -3.713457631102453,
-                    "timestamp": 0.534761702291081
-                },
-                {
-                    "x": 1.9906204010106838,
-                    "y": 4.231746663125483,
-                    "heading": 3.1399983498337,
-                    "angularVelocity": 3.5336881649516105e-12,
-                    "velocityX": -0.7181676635393196,
-                    "velocityY": -3.714144421443354,
-                    "timestamp": 0.5681843086842736
-                },
-                {
-                    "x": 1.9667619480914416,
-                    "y": 4.107582407046251,
-                    "heading": 3.139998349833824,
-                    "angularVelocity": 3.708930519775865e-12,
-                    "velocityX": -0.7138417823660727,
-                    "velocityY": -3.7149782580858504,
-                    "timestamp": 0.6016069150774662
-                },
-                {
-                    "x": 1.9503914931986956,
-                    "y": 3.982210973196183,
-                    "heading": 3.1399983498339594,
-                    "angularVelocity": 4.046203674508551e-12,
-                    "velocityX": -0.4898018634439243,
-                    "velocityY": -3.751096858670778,
-                    "timestamp": 0.6350295214706588
-                },
-                {
-                    "x": 1.943224502199385,
-                    "y": 3.8559785593201936,
-                    "heading": 3.1399983498340998,
-                    "angularVelocity": 4.203331220184306e-12,
-                    "velocityX": -0.21443543076194166,
-                    "velocityY": -3.7768572681307018,
-                    "timestamp": 0.6684521278638514
-                },
-                {
-                    "x": 1.9453593714612354,
-                    "y": 3.7295608780305693,
-                    "heading": 3.1399983498342436,
-                    "angularVelocity": 4.304427970302576e-12,
-                    "velocityX": 0.06387500833733431,
-                    "velocityY": -3.7824004448488933,
-                    "timestamp": 0.701874734257044
-                },
-                {
-                    "x": 1.9567930300665735,
-                    "y": 3.6036432110345795,
-                    "heading": 3.139998349834389,
-                    "angularVelocity": 4.350609924565741e-12,
-                    "velocityX": 0.3420935659816486,
-                    "velocityY": -3.767440082758668,
-                    "timestamp": 0.7352973406502367
-                },
-                {
-                    "x": 1.9774653925716308,
-                    "y": 3.4789089323576285,
-                    "heading": 3.1399983498345336,
-                    "angularVelocity": 4.327887690330789e-12,
-                    "velocityX": 0.6185143750313882,
-                    "velocityY": -3.732033259450771,
-                    "timestamp": 0.7687199470434293
-                },
-                {
-                    "x": 2.0072643339986427,
-                    "y": 3.3560349759367867,
-                    "heading": 3.1399983498346824,
-                    "angularVelocity": 4.434032141487508e-12,
-                    "velocityX": 0.8915804194501273,
-                    "velocityY": -3.6763726615238776,
-                    "timestamp": 0.8021425534366219
-                },
-                {
-                    "x": 2.0460281463567345,
-                    "y": 3.235688177587346,
-                    "heading": 3.1399983498348414,
-                    "angularVelocity": 4.757708405328401e-12,
-                    "velocityX": 1.1598081819894186,
-                    "velocityY": -3.600760423457161,
-                    "timestamp": 0.8355651598298145
-                },
-                {
-                    "x": 2.093546464867386,
-                    "y": 3.118521659475836,
-                    "heading": 3.139998349835024,
-                    "angularVelocity": 5.453307843013083e-12,
-                    "velocityX": 1.421741857944163,
-                    "velocityY": -3.5056068558230677,
-                    "timestamp": 0.8689877662230071
+                    "x": 2.24821488195175,
+                    "y": 5.550858605907564,
+                    "heading": -1.0860081979744884e-8,
+                    "angularVelocity": -3.249360952601713e-7,
+                    "velocityX": -0.053411135206172675,
+                    "velocityY": -0.27351257600550494,
+                    "timestamp": 0.0334222075852833
+                },
+                {
+                    "x": 2.2446446457975586,
+                    "y": 5.532575817984525,
+                    "heading": -3.253781151640391e-8,
+                    "angularVelocity": -6.486025430910346e-7,
+                    "velocityX": -0.10682227213842765,
+                    "velocityY": -0.5470251441769277,
+                    "timestamp": 0.0668444151705666
+                },
+                {
+                    "x": 2.2392892913368425,
+                    "y": 5.505151636566201,
+                    "heading": -6.492851000422694e-8,
+                    "angularVelocity": -9.691369834959166e-7,
+                    "velocityX": -0.1602334150722015,
+                    "velocityY": -0.8205377023156053,
+                    "timestamp": 0.10026662275584991
+                },
+                {
+                    "x": 2.2321488181807467,
+                    "y": 5.468586062083933,
+                    "heading": -1.0785000725838172e-7,
+                    "angularVelocity": -0.000001284220850944888,
+                    "velocityX": -0.21364456964054193,
+                    "velocityY": -1.0940502475484135,
+                    "timestamp": 0.1336888303411332
+                },
+                {
+                    "x": 2.2232232256851456,
+                    "y": 5.42287909509794,
+                    "heading": -1.6102300741445552e-7,
+                    "angularVelocity": -0.0000015909481231905412,
+                    "velocityX": -0.26705574348127187,
+                    "velocityY": -1.3675627760193463,
+                    "timestamp": 0.16711103792641652
+                },
+                {
+                    "x": 2.212512512848178,
+                    "y": 5.368030736346982,
+                    "heading": -2.2404505707142586e-7,
+                    "angularVelocity": -0.0000018856338989468898,
+                    "velocityX": -0.32046694729784664,
+                    "velocityY": -1.6410752823864576,
+                    "timestamp": 0.20053324551169982
+                },
+                {
+                    "x": 2.200016678146418,
+                    "y": 5.304040986826963,
+                    "heading": -2.963548114247805e-7,
+                    "angularVelocity": -0.000002163524111560841,
+                    "velocityX": -0.3738781966956197,
+                    "velocityY": -1.9145877589558649,
+                    "timestamp": 0.23395545309698312
+                },
+                {
+                    "x": 2.1857357192573095,
+                    "y": 5.230909847922857,
+                    "heading": -3.7718073304542066e-7,
+                    "angularVelocity": -0.0000024183297502514874,
+                    "velocityX": -0.4272895155847271,
+                    "velocityY": -2.1881001940865983,
+                    "timestamp": 0.2673776606822664
+                },
+                {
+                    "x": 2.169669632551276,
+                    "y": 5.148637321647842,
+                    "heading": -4.654625982128869e-7,
+                    "angularVelocity": -0.0000026414132559778984,
+                    "velocityX": -0.48070094307146977,
+                    "velocityY": -2.4616125689825754,
+                    "timestamp": 0.30079986826754973
+                },
+                {
+                    "x": 2.15181841205741,
+                    "y": 5.057223411125392,
+                    "heading": -5.597189955047307e-7,
+                    "angularVelocity": -0.0000028201725975039026,
+                    "velocityX": -0.5341125492089794,
+                    "velocityY": -2.735124850423542,
+                    "timestamp": 0.33422207585283303
+                },
+                {
+                    "x": 2.1321820469941515,
+                    "y": 4.956668121725741,
+                    "heading": -6.57784320236276e-7,
+                    "angularVelocity": -0.0000029341365576412115,
+                    "velocityX": -0.5875244779329194,
+                    "velocityY": -3.008636971185932,
+                    "timestamp": 0.36764428343811634
+                },
+                {
+                    "x": 2.1107605140888954,
+                    "y": 4.846971464532025,
+                    "heading": -7.56114109936986e-7,
+                    "angularVelocity": -0.0000029420493990798148,
+                    "velocityX": -0.6409371029779476,
+                    "velocityY": -3.282148760338516,
+                    "timestamp": 0.40106649102339964
+                },
+                {
+                    "x": 2.087553735835974,
+                    "y": 4.728133474365802,
+                    "heading": -8.465523402036467e-7,
+                    "angularVelocity": -0.0000027059321418483588,
+                    "velocityX": -0.6943520470083785,
+                    "velocityY": -3.5556595076225634,
+                    "timestamp": 0.43448869860868294
+                },
+                {
+                    "x": 2.0633251233427066,
+                    "y": 4.604042454619442,
+                    "heading": -8.465523885072994e-7,
+                    "angularVelocity": -1.4452562228577176e-12,
+                    "velocityX": -0.724925558303899,
+                    "velocityY": -3.712831339152857,
+                    "timestamp": 0.46791090619396625
+                },
+                {
+                    "x": 2.039088654512122,
+                    "y": 4.479952969280304,
+                    "heading": -8.465523654496814e-7,
+                    "angularVelocity": 6.898891300327124e-13,
+                    "velocityX": -0.725160621670614,
+                    "velocityY": -3.7127854293461913,
+                    "timestamp": 0.5013331137792496
+                },
+                {
+                    "x": 2.0148473415026222,
+                    "y": 4.355864430051467,
+                    "heading": -8.465523174606992e-7,
+                    "angularVelocity": 1.4358411666699279e-12,
+                    "velocityX": -0.7253055606758912,
+                    "velocityY": -3.712757121497041,
+                    "timestamp": 0.5347553213645329
+                },
+                {
+                    "x": 1.9907968545860597,
+                    "y": 4.231738764257757,
+                    "heading": -8.465522534990754e-7,
+                    "angularVelocity": 1.9137462023851052e-12,
+                    "velocityX": -0.7195960008785613,
+                    "velocityY": -3.7138679567028667,
+                    "timestamp": 0.5681775289498161
+                },
+                {
+                    "x": 1.9669031646192998,
+                    "y": 4.107582820499775,
+                    "heading": -8.465521766194915e-7,
+                    "angularVelocity": 2.3002544763334585e-12,
+                    "velocityX": -0.7149046006511871,
+                    "velocityY": -3.7147738802470687,
+                    "timestamp": 0.6015997365350993
+                },
+                {
+                    "x": 1.9504985155072192,
+                    "y": 3.982217377752346,
+                    "heading": -8.465520941748615e-7,
+                    "angularVelocity": 2.4667619198655976e-12,
+                    "velocityX": -0.4908308067350749,
+                    "velocityY": -3.7509623631996885,
+                    "timestamp": 0.6350219441203826
+                },
+                {
+                    "x": 1.94330794196602,
+                    "y": 3.855987816020002,
+                    "heading": -8.465520093116816e-7,
+                    "angularVelocity": 2.5391254773053977e-12,
+                    "velocityX": -0.2151435844765948,
+                    "velocityY": -3.7768169984060798,
+                    "timestamp": 0.6684441517056658
+                },
+                {
+                    "x": 1.9454293697222904,
+                    "y": 3.7295714172218672,
+                    "heading": -8.465519250884413e-7,
+                    "angularVelocity": 2.519978339764665e-12,
+                    "velocityX": 0.0634735976376205,
+                    "velocityY": -3.7824072056149025,
+                    "timestamp": 0.7018663592909491
+                },
+                {
+                    "x": 1.9568513812515056,
+                    "y": 3.603654207925799,
+                    "heading": -8.465518451486746e-7,
+                    "angularVelocity": 2.391815832767542e-12,
+                    "velocityX": 0.34174916483985474,
+                    "velocityY": -3.767471342961152,
+                    "timestamp": 0.7352885668762323
+                },
+                {
+                    "x": 1.9775120291721782,
+                    "y": 3.4789195174340257,
+                    "heading": -8.465517739710846e-7,
+                    "angularVelocity": 2.129649524174011e-12,
+                    "velocityX": 0.6181712523959566,
+                    "velocityY": -3.7320901132416737,
+                    "timestamp": 0.7687107744615156
+                },
+                {
+                    "x": 2.0072992100178335,
+                    "y": 3.3560442616931043,
+                    "heading": -8.465517105809759e-7,
+                    "angularVelocity": 1.8966463157464128e-12,
+                    "velocityX": 0.8912391789143821,
+                    "velocityY": -3.6764554055067418,
+                    "timestamp": 0.8021329820467988
+                },
+                {
+                    "x": 2.046051283468891,
+                    "y": 3.235695267642527,
+                    "heading": -8.465516527951994e-7,
+                    "angularVelocity": 1.7289634182989629e-12,
+                    "velocityX": 1.1594707905613406,
+                    "velocityY": -3.600869085126494,
+                    "timestamp": 0.8355551896320821
+                },
+                {
+                    "x": 2.0935579534966013,
+                    "y": 3.1185256537724477,
+                    "heading": -8.46551596321545e-7,
+                    "angularVelocity": 1.6897044173558387e-12,
+                    "velocityX": 1.421410297528723,
+                    "velocityY": -3.5057413120014775,
+                    "timestamp": 0.8689773972173653
                 },
                 {
                     "x": 2.1495614051818848,
                     "y": 3.005171298980713,
-                    "heading": 3.1399983498353623,
-                    "angularVelocity": 1.0122205936259764e-11,
-                    "velocityX": 1.67595966800623,
-                    "velocityY": -3.391427920419207,
-                    "timestamp": 0.9024103726161997
-                },
-                {
-                    "x": 2.2887605263975965,
-                    "y": 2.8026901614733335,
-                    "heading": 3.1399983498355466,
-                    "angularVelocity": 2.834267287592998e-12,
-                    "velocityX": 2.1430749184844986,
-                    "velocityY": -3.117349042649137,
-                    "timestamp": 0.9673633614337364
-                },
-                {
-                    "x": 2.4554470535268806,
-                    "y": 2.6221614552311943,
-                    "heading": 3.1399983498356696,
-                    "angularVelocity": 1.896559244966036e-12,
-                    "velocityX": 2.566264157563543,
-                    "velocityY": -2.7793748914229504,
-                    "timestamp": 1.0323163502512729
-                },
-                {
-                    "x": 2.646204500588454,
-                    "y": 2.4672853384780784,
-                    "heading": 3.139998349835769,
-                    "angularVelocity": 1.528915692972437e-12,
-                    "velocityX": 2.9368540314174165,
-                    "velocityY": -2.384434027946127,
-                    "timestamp": 1.0972693390688095
-                },
-                {
-                    "x": 2.8571229789617445,
-                    "y": 2.341236093184693,
-                    "heading": 3.1399983498358592,
-                    "angularVelocity": 1.3872590523351098e-12,
-                    "velocityX": 3.247248236192457,
-                    "velocityY": -1.940622711722675,
-                    "timestamp": 1.1622223278863462
-                },
-                {
-                    "x": 3.0838578351814623,
-                    "y": 2.2465454263799622,
-                    "heading": 3.139998349835948,
-                    "angularVelocity": 1.37081210529667e-12,
-                    "velocityX": 3.490753240880161,
-                    "velocityY": -1.4578338661343044,
-                    "timestamp": 1.2271753167038828
-                },
-                {
-                    "x": 3.3145391093778356,
-                    "y": 2.1619217463740292,
-                    "heading": 3.1399983498360386,
-                    "angularVelocity": 1.3907396736325547e-12,
-                    "velocityX": 3.551511306806079,
-                    "velocityY": -1.3028450506530576,
-                    "timestamp": 1.2921283055214194
-                },
-                {
-                    "x": 3.5452163544414024,
-                    "y": 2.0772870838321214,
-                    "heading": 3.1399983498361257,
-                    "angularVelocity": 1.3371940783811797e-12,
-                    "velocityX": 3.5514492752906865,
-                    "velocityY": -1.3030141350149058,
-                    "timestamp": 1.357081294338956
-                },
-                {
-                    "x": 3.7758941865765565,
-                    "y": 1.9926540214142978,
-                    "heading": 3.139998349836204,
-                    "angularVelocity": 1.2081341732104707e-12,
-                    "velocityX": 3.5514583136980007,
-                    "velocityY": -1.302989499904737,
-                    "timestamp": 1.4220342831564927
-                },
-                {
-                    "x": 4.006572310105761,
-                    "y": 1.908021753240333,
-                    "heading": 3.1399983498362696,
-                    "angularVelocity": 1.0139841411422692e-12,
-                    "velocityX": 3.5514627999275414,
-                    "velocityY": -1.3029772719259731,
-                    "timestamp": 1.4869872719740294
-                },
-                {
-                    "x": 4.237248920507999,
-                    "y": 1.8233853608613613,
-                    "heading": 3.1399983498363224,
-                    "angularVelocity": 8.135622214219814e-13,
-                    "velocityX": 3.5514395042053684,
-                    "velocityY": -1.3030407671488682,
-                    "timestamp": 1.551940260791566
-                },
-                {
-                    "x": 4.46792193477352,
-                    "y": 1.7387391679278503,
-                    "heading": 3.1399983498363646,
-                    "angularVelocity": 6.458697792259404e-13,
-                    "velocityX": 3.551384138968564,
-                    "velocityY": -1.3031916540771904,
-                    "timestamp": 1.6168932496091026
-                },
-                {
-                    "x": 4.698662422545312,
-                    "y": 1.6542770762611563,
-                    "heading": 3.139998349836401,
-                    "angularVelocity": 5.573297491220617e-13,
-                    "velocityX": 3.5524229442068638,
-                    "velocityY": -1.3003572769873404,
-                    "timestamp": 1.6818462384266393
+                    "heading": -8.465515057383081e-7,
+                    "angularVelocity": 2.7102707524537993e-12,
+                    "velocityX": 1.6756359238847198,
+                    "velocityY": -3.3915878986274275,
+                    "timestamp": 0.9023996048026486
+                },
+                {
+                    "x": 2.288743031901554,
+                    "y": 2.8026748842621148,
+                    "heading": -8.46551464739476e-7,
+                    "angularVelocity": 6.312009526868921e-13,
+                    "velocityX": 2.142782210187666,
+                    "velocityY": -3.117550249366458,
+                    "timestamp": 0.9673533019435205
+                },
+                {
+                    "x": 2.455414708004788,
+                    "y": 2.6221288200825,
+                    "heading": -8.46551435217149e-7,
+                    "angularVelocity": 4.5451345461171864e-13,
+                    "velocityX": 2.566007532127087,
+                    "velocityY": -2.779611817758129,
+                    "timestamp": 1.0323069990843925
+                },
+                {
+                    "x": 2.646160177401748,
+                    "y": 2.4672337011827246,
+                    "heading": -8.465514075797998e-7,
+                    "angularVelocity": 4.254931139136813e-13,
+                    "velocityX": 2.9366376017527633,
+                    "velocityY": -2.384700574689416,
+                    "timestamp": 1.0972606962252645
+                },
+                {
+                    "x": 2.857069726290126,
+                    "y": 2.341164292711998,
+                    "heading": -8.465513787578765e-7,
+                    "angularVelocity": 4.4373032078954734e-13,
+                    "velocityX": 3.2470753501649696,
+                    "velocityY": -1.9409119729950552,
+                    "timestamp": 1.1622143933661364
+                },
+                {
+                    "x": 3.0838173053505575,
+                    "y": 2.2464971410417167,
+                    "heading": -8.465513471236816e-7,
+                    "angularVelocity": 4.870268741320482e-13,
+                    "velocityX": 3.490911049579749,
+                    "velocityY": -1.457455939200772,
+                    "timestamp": 1.2271680905070084
+                },
+                {
+                    "x": 3.3145112489857773,
+                    "y": 2.1619002222864983,
+                    "heading": -8.465513138187862e-7,
+                    "angularVelocity": 5.127482864627404e-13,
+                    "velocityX": 3.5516676307835056,
+                    "velocityY": -1.3024188380298123,
+                    "timestamp": 1.2921217876478803
+                },
+                {
+                    "x": 3.5452013267301767,
+                    "y": 2.0772927620920076,
+                    "heading": -8.465512839726007e-7,
+                    "angularVelocity": 4.594994306997602e-13,
+                    "velocityX": 3.5516081131487938,
+                    "velocityY": -1.302581129610464,
+                    "timestamp": 1.3570754847887523
+                },
+                {
+                    "x": 3.77589133724735,
+                    "y": 1.9926851185956267,
+                    "heading": -8.465512625373994e-7,
+                    "angularVelocity": 3.3000743037279784e-13,
+                    "velocityX": 3.5516070781466733,
+                    "velocityY": -1.302583951649345,
+                    "timestamp": 1.4220291819296242
+                },
+                {
+                    "x": 4.006581361028944,
+                    "y": 1.9080775112659911,
+                    "heading": -8.465512535495126e-7,
+                    "angularVelocity": 1.3837377280671698e-13,
+                    "velocityX": 3.5516072823601217,
+                    "velocityY": -1.302583394841325,
+                    "timestamp": 1.4869828790704962
+                },
+                {
+                    "x": 4.2372714040114055,
+                    "y": 1.823469956289169,
+                    "heading": -8.465512575529088e-7,
+                    "angularVelocity": -6.163459506326445e-14,
+                    "velocityX": 3.551607577968653,
+                    "velocityY": -1.3025825888392473,
+                    "timestamp": 1.5519365762113682
+                },
+                {
+                    "x": 4.467961461705261,
+                    "y": 1.7388624414245573,
+                    "heading": -8.465512722492451e-7,
+                    "angularVelocity": -2.2625863777166434e-13,
+                    "velocityX": 3.5516078044591044,
+                    "velocityY": -1.3025819712885556,
+                    "timestamp": 1.6168902733522401
+                },
+                {
+                    "x": 4.698651553926265,
+                    "y": 1.6542550207002664,
+                    "heading": -8.465512926518018e-7,
+                    "angularVelocity": -3.141092229421582e-13,
+                    "velocityX": 3.551608336024854,
+                    "velocityY": -1.3025805219428788,
+                    "timestamp": 1.681843970493112
                 },
                 {
                     "x": 4.929586887359619,
                     "y": 1.5703192949295044,
-                    "heading": 3.139998349836444,
-                    "angularVelocity": 6.621983947605345e-13,
-                    "velocityX": 3.5552554088546553,
-                    "velocityY": -1.2925930408850796,
-                    "timestamp": 1.746799227244176
-                },
-                {
-                    "x": 5.059491461224377,
-                    "y": 1.5327116535816125,
-                    "heading": 3.1399983498365303,
-                    "angularVelocity": 2.411376289586005e-12,
-                    "velocityX": 3.6337293619618993,
-                    "velocityY": -1.0519721248789429,
-                    "timestamp": 1.7825488830178295
-                },
-                {
-                    "x": 5.189524196366832,
-                    "y": 1.4955495654455135,
-                    "heading": 3.1399983498366377,
-                    "angularVelocity": 3.0099354211020166e-12,
-                    "velocityX": 3.6373143273250914,
-                    "velocityY": -1.0395089779716884,
-                    "timestamp": 1.8182985387914832
-                },
-                {
-                    "x": 5.319583815273274,
-                    "y": 1.4584816742976305,
-                    "heading": 3.1399983498367723,
-                    "angularVelocity": 3.767098328405853e-12,
-                    "velocityX": 3.6380663279640872,
-                    "velocityY": -1.036874071811995,
-                    "timestamp": 1.8540481945651368
-                },
-                {
-                    "x": 5.4496589768626915,
-                    "y": 1.4214683610582035,
-                    "heading": 3.139998349836922,
-                    "angularVelocity": 4.1821295987378e-12,
-                    "velocityX": 3.6385010925132435,
-                    "velocityY": -1.0353474023301,
-                    "timestamp": 1.8897978503387904
-                },
-                {
-                    "x": 5.579738820656374,
-                    "y": 1.3844715066642421,
-                    "heading": 3.139998349837078,
-                    "angularVelocity": 4.36086747938318e-12,
-                    "velocityX": 3.6386320645231356,
-                    "velocityY": -1.0348870106108623,
-                    "timestamp": 1.925547506112444
-                },
-                {
-                    "x": 5.709817887256256,
-                    "y": 1.3474719197526863,
-                    "heading": 3.1399983498372332,
-                    "angularVelocity": 4.346123552895358e-12,
-                    "velocityX": 3.6386103246271904,
-                    "velocityY": -1.0349634454051337,
-                    "timestamp": 1.9612971618860977
-                },
-                {
-                    "x": 5.839896082809478,
-                    "y": 1.3104692706317995,
-                    "heading": 3.139998349837383,
-                    "angularVelocity": 4.183764792386501e-12,
-                    "velocityX": 3.638585959451041,
-                    "velocityY": -1.0350491024349304,
-                    "timestamp": 1.9970468176597513
-                },
-                {
-                    "x": 5.969973859581082,
-                    "y": 1.2734651493862033,
-                    "heading": 3.139998349837523,
-                    "angularVelocity": 3.914068769730236e-12,
-                    "velocityX": 3.6385742451671077,
-                    "velocityY": -1.0350902811450293,
-                    "timestamp": 2.032796473433405
-                },
-                {
-                    "x": 6.100051488327663,
-                    "y": 1.2364605078211643,
-                    "heading": 3.1399983498376502,
-                    "angularVelocity": 3.5614211180657343e-12,
-                    "velocityX": 3.6385701045671635,
-                    "velocityY": -1.0351048356752235,
-                    "timestamp": 2.068546129207059
-                },
-                {
-                    "x": 6.230129108868512,
-                    "y": 1.199455837420786,
-                    "heading": 3.1399983498377644,
-                    "angularVelocity": 3.192303325422517e-12,
-                    "velocityX": 3.6385698750339617,
-                    "velocityY": -1.0351056422659504,
-                    "timestamp": 2.1042957849807125
-                },
-                {
-                    "x": 6.360206785724816,
-                    "y": 1.162451364982862,
-                    "heading": 3.1399983498378674,
-                    "angularVelocity": 2.8800276139093587e-12,
-                    "velocityX": 3.638571450306583,
-                    "velocityY": -1.0351001048013342,
-                    "timestamp": 2.140045440754366
-                },
-                {
-                    "x": 6.490284555094277,
-                    "y": 1.1254472177490595,
-                    "heading": 3.1399983498379616,
-                    "angularVelocity": 2.621534784795507e-12,
-                    "velocityX": 3.638574038112148,
-                    "velocityY": -1.0350910080949738,
-                    "timestamp": 2.1757950965280197
-                },
-                {
-                    "x": 6.6203624349981105,
-                    "y": 1.0884434590721859,
-                    "heading": 3.1399983498380477,
-                    "angularVelocity": 2.4107623793093176e-12,
-                    "velocityX": 3.638577130012462,
-                    "velocityY": -1.0350801392650475,
-                    "timestamp": 2.2115447523016734
-                },
-                {
-                    "x": 6.75044029323431,
-                    "y": 1.0514396242316386,
-                    "heading": 3.1399983498381276,
-                    "angularVelocity": 2.2461604882641728e-12,
-                    "velocityX": 3.6385765239189465,
-                    "velocityY": -1.0350822697380073,
-                    "timestamp": 2.247294408075327
-                },
-                {
-                    "x": 6.880516952547443,
-                    "y": 1.0144315751292232,
-                    "heading": 3.139998349838195,
-                    "angularVelocity": 1.892677955933963e-12,
-                    "velocityX": 3.6385429872864736,
-                    "velocityY": -1.0352001523239789,
-                    "timestamp": 2.2830440638489806
-                },
-                {
-                    "x": 7.003514384307231,
-                    "y": 0.97944241190024,
-                    "heading": 3.1399986104136794,
-                    "angularVelocity": 0.000007288894915666013,
-                    "velocityX": 3.440520729445303,
-                    "velocityY": -0.9787272764389812,
-                    "timestamp": 2.3187937196226343
-                },
-                {
-                    "x": 7.116262004659963,
-                    "y": 0.9473688227429191,
-                    "heading": 3.139998845492451,
-                    "angularVelocity": 0.000006575693298715729,
-                    "velocityX": 3.1538099574045058,
-                    "velocityY": -0.8971719716796405,
-                    "timestamp": 2.354543375396288
-                },
-                {
-                    "x": 7.218759831907218,
-                    "y": 0.9182109395297621,
-                    "heading": 3.1399990575434797,
-                    "angularVelocity": 0.00000593155441577883,
-                    "velocityX": 2.8670996973009437,
-                    "velocityY": -0.8156129781434808,
-                    "timestamp": 2.3902930311699415
-                },
-                {
-                    "x": 7.3110078714654865,
-                    "y": 0.8919688038160335,
-                    "heading": 3.1399992473559544,
-                    "angularVelocity": 0.000005309490974163329,
-                    "velocityX": 2.5803895887090773,
-                    "velocityY": -0.7340528222110998,
-                    "timestamp": 2.426042686943595
-                },
-                {
-                    "x": 7.393006125886103,
-                    "y": 0.8686424358274811,
-                    "heading": 3.139999415401417,
-                    "angularVelocity": 0.000004700617645814799,
-                    "velocityX": 2.293679551483878,
-                    "velocityY": -0.6524921005180893,
-                    "timestamp": 2.461792342717249
-                },
-                {
-                    "x": 7.464754596626899,
-                    "y": 0.8482318474429659,
-                    "heading": 3.1399995619997028,
-                    "angularVelocity": 0.000004100690828648421,
-                    "velocityX": 2.0069695550376028,
-                    "velocityY": -0.5709310465460533,
-                    "timestamp": 2.4975419984909024
-                },
-                {
-                    "x": 7.526253284614184,
-                    "y": 0.8307370464214682,
-                    "heading": 3.1399996873763563,
-                    "angularVelocity": 0.00000350707302580956,
-                    "velocityX": 1.7202595845023039,
-                    "velocityY": -0.4893697755375005,
-                    "timestamp": 2.533291654264556
-                },
-                {
-                    "x": 7.577502190476697,
-                    "y": 0.8161580381894111,
-                    "heading": 3.139999791690666,
-                    "angularVelocity": 0.0000029179108844478315,
-                    "velocityX": 1.433549631554256,
-                    "velocityY": -0.40780835273943755,
-                    "timestamp": 2.5690413100382097
-                },
-                {
-                    "x": 7.618501314660242,
-                    "y": 0.8044948267260937,
-                    "heading": 3.1399998750527174,
-                    "angularVelocity": 0.000002331828092755085,
-                    "velocityX": 1.1468396910764296,
-                    "velocityY": -0.3262468186312345,
-                    "timestamp": 2.6047909658118633
-                },
-                {
-                    "x": 7.649250657490367,
-                    "y": 0.7957474150520292,
-                    "heading": 3.139999937534455,
-                    "angularVelocity": 0.0000017477577406037154,
-                    "velocityX": 0.860129759704899,
-                    "velocityY": -0.24468520003233896,
-                    "timestamp": 2.640540621585517
-                },
-                {
-                    "x": 7.669750219209449,
-                    "y": 0.789915805520138,
-                    "heading": 3.1399999791776474,
-                    "angularVelocity": 0.0000011648557571115021,
-                    "velocityX": 0.5734198351132136,
-                    "velocityY": -0.16312351561685162,
-                    "timestamp": 2.6762902773591706
+                    "heading": -8.465513064459796e-7,
+                    "angularVelocity": -2.1236938120360531e-13,
+                    "velocityX": 3.555383967330626,
+                    "velocityY": -1.2922393869021438,
+                    "timestamp": 1.746797667633984
+                },
+                {
+                    "x": 5.059495672066219,
+                    "y": 1.5327262216225606,
+                    "heading": -8.465512750899748e-7,
+                    "angularVelocity": 8.770995258558909e-13,
+                    "velocityX": 3.633847296905686,
+                    "velocityY": -1.051564666141291,
+                    "timestamp": 1.7825473219513848
+                },
+                {
+                    "x": 5.189548300230332,
+                    "y": 1.4956338313822377,
+                    "heading": -8.465512186056332e-7,
+                    "angularVelocity": 1.579996862146135e-12,
+                    "velocityX": 3.637870929029155,
+                    "velocityY": -1.0375594099736098,
+                    "timestamp": 1.8182969762687855
+                },
+                {
+                    "x": 5.319623170079142,
+                    "y": 1.4586195133360007,
+                    "heading": -8.46551136487956e-7,
+                    "angularVelocity": 2.2970201754243682e-12,
+                    "velocityX": 3.638493080071459,
+                    "velocityY": -1.0353755512600933,
+                    "timestamp": 1.8540466305861862
+                },
+                {
+                    "x": 5.4496989055100356,
+                    "y": 1.42160823723968,
+                    "heading": -8.465510387564672e-7,
+                    "angularVelocity": 2.7337743725450157e-12,
+                    "velocityX": 3.6385172923918394,
+                    "velocityY": -1.0352904609291835,
+                    "timestamp": 1.889796284903587
+                },
+                {
+                    "x": 5.57977446957584,
+                    "y": 1.3845963588884114,
+                    "heading": -8.465509335637302e-7,
+                    "angularVelocity": 2.9424826347181554e-12,
+                    "velocityX": 3.6385124989164392,
+                    "velocityY": -1.03530730738433,
+                    "timestamp": 1.9255459392209877
+                },
+                {
+                    "x": 5.709850057975644,
+                    "y": 1.3475845660573411,
+                    "heading": -8.465508277220688e-7,
+                    "angularVelocity": 2.960634540295608e-12,
+                    "velocityX": 3.6385131795943324,
+                    "velocityY": -1.0353049151878095,
+                    "timestamp": 1.9612955935383884
+                },
+                {
+                    "x": 5.839925662966751,
+                    "y": 1.3105728315354166,
+                    "heading": -8.465507264753305e-7,
+                    "angularVelocity": 2.832103981671134e-12,
+                    "velocityX": 3.6385136436912036,
+                    "velocityY": -1.0353032841469862,
+                    "timestamp": 1.9970452478557892
+                },
+                {
+                    "x": 5.970001275498046,
+                    "y": 1.273561123513075,
+                    "heading": -8.465506337302748e-7,
+                    "angularVelocity": 2.594292378130792e-12,
+                    "velocityX": 3.638513854607602,
+                    "velocityY": -1.0353025428927405,
+                    "timestamp": 2.03279490217319
+                },
+                {
+                    "x": 6.100076894564937,
+                    "y": 1.2365494384597497,
+                    "heading": -8.46550552278632e-7,
+                    "angularVelocity": 2.2783896618382965e-12,
+                    "velocityX": 3.6385140374232323,
+                    "velocityY": -1.0353019003965735,
+                    "timestamp": 2.0685445564905907
+                },
+                {
+                    "x": 6.230152520105601,
+                    "y": 1.1995377761581834,
+                    "heading": -8.465504825990077e-7,
+                    "angularVelocity": 1.9490992420524453e-12,
+                    "velocityX": 3.6385142185095396,
+                    "velocityY": -1.03530126397757,
+                    "timestamp": 2.1042942108079914
+                },
+                {
+                    "x": 6.3602281516940815,
+                    "y": 1.16252613511139,
+                    "heading": -8.465504225140753e-7,
+                    "angularVelocity": 1.680713660601537e-12,
+                    "velocityX": 3.6385143876808734,
+                    "velocityY": -1.0353006694327234,
+                    "timestamp": 2.140043865125392
+                },
+                {
+                    "x": 6.490303788775648,
+                    "y": 1.1255145133698101,
+                    "heading": -8.465503700121188e-7,
+                    "angularVelocity": 1.4686003981542915e-12,
+                    "velocityX": 3.638514541335137,
+                    "velocityY": -1.0353001294215285,
+                    "timestamp": 2.175793519442793
+                },
+                {
+                    "x": 6.62037943067546,
+                    "y": 1.0885029085617548,
+                    "heading": -8.465503232940394e-7,
+                    "angularVelocity": 1.306812060469801e-12,
+                    "velocityX": 3.638514676112538,
+                    "velocityY": -1.0352996557519205,
+                    "timestamp": 2.2115431737601936
+                },
+                {
+                    "x": 6.7504550766965234,
+                    "y": 1.0514913182376744,
+                    "heading": -8.465502808582129e-7,
+                    "angularVelocity": 1.187027603302574e-12,
+                    "velocityX": 3.638514791393393,
+                    "velocityY": -1.0352992506018621,
+                    "timestamp": 2.2472928280775943
+                },
+                {
+                    "x": 6.880530727542945,
+                    "y": 1.0144797448710672,
+                    "heading": -8.465502433332782e-7,
+                    "angularVelocity": 1.0496586699357012e-12,
+                    "velocityX": 3.6385149263697616,
+                    "velocityY": -1.0352987762623522,
+                    "timestamp": 2.283042482394995
+                },
+                {
+                    "x": 7.003525953233288,
+                    "y": 0.9794828656529073,
+                    "heading": -7.145353891597533e-7,
+                    "angularVelocity": 0.000003692758900357643,
+                    "velocityX": 3.440459160761053,
+                    "velocityY": -0.9789431502593731,
+                    "timestamp": 2.318792136712396
+                },
+                {
+                    "x": 7.116271603762692,
+                    "y": 0.9474023877264399,
+                    "heading": -5.944969754078434e-7,
+                    "angularVelocity": 0.0000033577503345576707,
+                    "velocityX": 3.153754985388111,
+                    "velocityY": -0.8973647029323183,
+                    "timestamp": 2.3545417910297965
+                },
+                {
+                    "x": 7.218767661219051,
+                    "y": 0.9182383156679585,
+                    "heading": -4.859087151281335e-7,
+                    "angularVelocity": 0.0000030374632243726353,
+                    "velocityX": 2.867050308972391,
+                    "velocityY": -0.8157861275958178,
+                    "timestamp": 2.3902914453471973
+                },
+                {
+                    "x": 7.311014119553375,
+                    "y": 0.8919906507276494,
+                    "heading": -3.8850367492824467e-7,
+                    "angularVelocity": 0.000002724642854703664,
+                    "velocityX": 2.580345463352478,
+                    "velocityY": -0.7342075172887301,
+                    "timestamp": 2.426041099664598
+                },
+                {
+                    "x": 7.393010975686544,
+                    "y": 0.8686593933386082,
+                    "heading": -3.0210953963079075e-7,
+                    "angularVelocity": 0.0000024166425366030095,
+                    "velocityX": 2.293640531602499,
+                    "velocityY": -0.6526288948669626,
+                    "timestamp": 2.4617907539819988
+                },
+                {
+                    "x": 7.464758227730078,
+                    "y": 0.8482445436165693,
+                    "heading": -2.2661033918382436e-7,
+                    "angularVelocity": 0.000002111886167406066,
+                    "velocityX": 2.006935547027439,
+                    "velocityY": -0.5710502692078399,
+                    "timestamp": 2.4975404082993995
+                },
+                {
+                    "x": 7.526255874392654,
+                    "y": 0.8307461015250325,
+                    "heading": -1.6193015142138781e-7,
+                    "angularVelocity": 0.0000018092535156938384,
+                    "velocityX": 1.7202305263310733,
+                    "velocityY": -0.48947164456971237,
+                    "timestamp": 2.5332900626168002
+                },
+                {
+                    "x": 7.577503914725456,
+                    "y": 0.8161640669449958,
+                    "heading": -1.0802379214331377e-7,
+                    "angularVelocity": 0.0000015078847699136293,
+                    "velocityX": 1.4335254790941394,
+                    "velocityY": -0.4078930232603403,
+                    "timestamp": 2.569039716934201
+                },
+                {
+                    "x": 7.618502347994633,
+                    "y": 0.8044984397090525,
+                    "heading": -6.487055743300418e-8,
+                    "angularVelocity": 0.0000012070951602673883,
+                    "velocityX": 1.1468204113297347,
+                    "velocityY": -0.3263144066337203,
+                    "timestamp": 2.6047893712516017
+                },
+                {
+                    "x": 7.649251173610278,
+                    "y": 0.7957492196197445,
+                    "heading": -3.246962719134405e-8,
+                    "angularVelocity": 9.063284906782187e-7,
+                    "velocityX": 0.8601153270642448,
+                    "velocityY": -0.24473579553046104,
+                    "timestamp": 2.6405390255690024
+                },
+                {
+                    "x": 7.669750391083677,
+                    "y": 0.7899164064601075,
+                    "heading": -1.0836483282929717e-8,
+                    "angularVelocity": 6.051287577519143e-7,
+                    "velocityX": 0.573410229128317,
+                    "velocityY": -0.16315719049619,
+                    "timestamp": 2.676288679886403
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
-                    "angularVelocity": 5.824490440069194e-7,
-                    "velocityX": 0.2867099156267607,
-                    "velocityY": -0.0815617789049158,
-                    "timestamp": 2.712039933132824
+                    "heading": 2.0494644799121527e-30,
+                    "angularVelocity": 3.031213445055282e-7,
+                    "velocityX": 0.28670511958863415,
+                    "velocityY": -0.08157859189950141,
+                    "timestamp": 2.712038334203804
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
-                    "angularVelocity": -6.025226692032396e-31,
-                    "velocityX": 4.372989448335815e-32,
-                    "velocityY": 7.524777583668013e-32,
-                    "timestamp": 2.747789588906478
+                    "heading": -1.7051999701665118e-29,
+                    "angularVelocity": -2.5846596071248955e-28,
+                    "velocityX": 4.524644517512267e-29,
+                    "velocityY": 1.710870028377411e-29,
+                    "timestamp": 2.7477879885212046
                 }
             ],
             "constraints": [
@@ -17890,7 +16443,7 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -17899,7 +16452,7 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": 3.5331853071795862,
+                    "heading": 0.393,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -17910,146 +16463,146 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": 0,
-                    "velocityX": 1.6975195374214185e-34,
-                    "velocityY": -4.870070626129674e-33,
+                    "heading": -0.39,
+                    "angularVelocity": -1.4895643422557431e-33,
+                    "velocityX": -9.369180406325922e-35,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.300139974954616,
-                    "y": 4.403824386953961,
-                    "heading": 2.777167813185812,
-                    "angularVelocity": 0.371999186817317,
-                    "velocityX": 0.0019166271843757909,
-                    "velocityY": 0.6000717171507387,
-                    "timestamp": 0.07303191552177725
-                },
-                {
-                    "x": 2.3004163368424435,
-                    "y": 4.4915620593167,
-                    "heading": 2.8296003827219796,
-                    "angularVelocity": 0.7179404943929278,
-                    "velocityX": 0.003784124869971189,
-                    "velocityY": 1.2013606891712643,
-                    "timestamp": 0.1460638310435545
-                },
-                {
-                    "x": 2.3008287718098006,
-                    "y": 4.623340237169809,
-                    "heading": 2.9042527526949042,
-                    "angularVelocity": 1.022188305476718,
-                    "velocityX": 0.005647325068915905,
-                    "velocityY": 1.8043916404440152,
-                    "timestamp": 0.21909574656533176
-                },
-                {
-                    "x": 2.30139145489797,
-                    "y": 4.799346158732557,
-                    "heading": 2.995610111682823,
-                    "angularVelocity": 1.2509237685362016,
-                    "velocityX": 0.007704619057970502,
-                    "velocityY": 2.409986377945481,
-                    "timestamp": 0.292127662087109
-                },
-                {
-                    "x": 2.3021599471353418,
-                    "y": 5.019805776345521,
-                    "heading": 3.0915564646710374,
-                    "angularVelocity": 1.3137592284513517,
-                    "velocityX": 0.01052269041392973,
-                    "velocityY": 3.0186750003459215,
-                    "timestamp": 0.36515957760888623
-                },
-                {
-                    "x": 2.303349941373319,
-                    "y": 5.282617464741668,
-                    "heading": 3.1370629336033304,
-                    "angularVelocity": 0.6231038664010262,
-                    "velocityX": 0.0162941671387857,
-                    "velocityY": 3.598586816715496,
-                    "timestamp": 0.43819149313066347
-                },
-                {
-                    "x": 2.30367850771832,
-                    "y": 5.558892653761466,
-                    "heading": 3.1370629627902624,
-                    "angularVelocity": 3.996462650884406e-7,
-                    "velocityX": 0.004498941903047958,
-                    "velocityY": 3.7829377340844195,
-                    "timestamp": 0.5112234086524408
-                },
-                {
-                    "x": 2.304007097232836,
-                    "y": 5.835167842753699,
-                    "heading": 3.137062991977204,
-                    "angularVelocity": 3.996463893760175e-7,
-                    "velocityX": 0.004499259154964363,
-                    "velocityY": 3.7829377337070014,
-                    "timestamp": 0.584255324174218
-                },
-                {
-                    "x": 2.3020927945039733,
-                    "y": 6.097639248519623,
-                    "heading": 3.182327539673564,
-                    "angularVelocity": 0.6197913251072079,
-                    "velocityX": -0.026211865253511206,
-                    "velocityY": 3.5939274478930847,
-                    "timestamp": 0.6572872396959952
-                },
-                {
-                    "x": 2.3012287765145363,
-                    "y": 6.317987502538012,
-                    "heading": 3.2805685838685283,
-                    "angularVelocity": 1.3451796176107471,
-                    "velocityX": -0.011830690503794171,
-                    "velocityY": 3.0171501383211403,
-                    "timestamp": 0.7303191552177725
-                },
-                {
-                    "x": 2.3006859363348666,
-                    "y": 6.493881460544365,
-                    "heading": 3.3743823922179033,
-                    "angularVelocity": 1.284559054478048,
-                    "velocityX": -0.007432917181360783,
-                    "velocityY": 2.40845330085727,
-                    "timestamp": 0.8033510707395497
-                },
-                {
-                    "x": 2.3003278852317357,
-                    "y": 6.625559358254098,
-                    "heading": 3.451128438683188,
-                    "angularVelocity": 1.050856271767924,
-                    "velocityX": -0.004902666191527756,
-                    "velocityY": 1.80301854016781,
-                    "timestamp": 0.8763829862613269
-                },
-                {
-                    "x": 2.300106073487947,
-                    "y": 6.713219846040187,
-                    "heading": 3.5051273882405054,
-                    "angularVelocity": 0.739388377964907,
-                    "velocityX": -0.0030371891823418365,
-                    "velocityY": 1.2003038282618885,
-                    "timestamp": 0.9494149017831042
+                    "x": 2.300137763138577,
+                    "y": 4.403828106227067,
+                    "heading": -0.362912658197764,
+                    "angularVelocity": 0.3708979956671865,
+                    "velocityX": 0.001886345007500309,
+                    "velocityY": 0.6001237357356949,
+                    "timestamp": 0.07303178264285357
+                },
+                {
+                    "x": 2.3004080637884394,
+                    "y": 4.491572820860884,
+                    "heading": -0.31063890006100675,
+                    "angularVelocity": 0.7157672487934603,
+                    "velocityX": 0.0037011372320465844,
+                    "velocityY": 1.201459302491822,
+                    "timestamp": 0.14606356528570713
+                },
+                {
+                    "x": 2.3008080200629966,
+                    "y": 4.623360729086378,
+                    "heading": -0.2362185458103229,
+                    "angularVelocity": 1.019013250910508,
+                    "velocityX": 0.005476468738453403,
+                    "velocityY": 1.8045281582400208,
+                    "timestamp": 0.2190953479285607
+                },
+                {
+                    "x": 2.3013470975241233,
+                    "y": 4.799377996239057,
+                    "heading": -0.1451530728621028,
+                    "angularVelocity": 1.2469293457282353,
+                    "velocityX": 0.0073814090471083236,
+                    "velocityY": 2.4101461142398,
+                    "timestamp": 0.29212713057141426
+                },
+                {
+                    "x": 2.30206992781241,
+                    "y": 5.019849097252827,
+                    "heading": -0.0495237523149135,
+                    "angularVelocity": 1.3094205986295608,
+                    "velocityX": 0.009897475621284645,
+                    "velocityY": 3.0188377311277894,
+                    "timestamp": 0.36515891321426786
+                },
+                {
+                    "x": 2.303151073626671,
+                    "y": 5.282690240833408,
+                    "heading": -0.004114697333369339,
+                    "angularVelocity": 0.621771143169372,
+                    "velocityX": 0.014803771387434566,
+                    "velocityY": 3.598996684305928,
+                    "timestamp": 0.43819069585712145
+                },
+                {
+                    "x": 2.3035206151085394,
+                    "y": 5.5589648754006,
+                    "heading": -0.004114668066309664,
+                    "angularVelocity": 4.007441502253877e-7,
+                    "velocityX": 0.005060009060371044,
+                    "velocityY": 3.7829370250792667,
+                    "timestamp": 0.511222478499975
+                },
+                {
+                    "x": 2.3038901811674446,
+                    "y": 5.835239509934909,
+                    "heading": -0.0041146387992405135,
+                    "angularVelocity": 4.0074427997802584e-7,
+                    "velocityX": 0.0050603455856026045,
+                    "velocityY": 3.7829370246290135,
+                    "timestamp": 0.5842542611428286
+                },
+                {
+                    "x": 2.3020054253248463,
+                    "y": 6.097682298903223,
+                    "heading": 0.0412179398919569,
+                    "angularVelocity": 0.6207239786667508,
+                    "velocityX": -0.02580733722214711,
+                    "velocityY": 3.593542146598477,
+                    "timestamp": 0.6572860437856822
+                },
+                {
+                    "x": 2.301170577421719,
+                    "y": 6.318019035246094,
+                    "heading": 0.13972641476186656,
+                    "angularVelocity": 1.3488439047372631,
+                    "velocityX": -0.011431295703265835,
+                    "velocityY": 3.016997920212089,
+                    "timestamp": 0.7303178264285358
+                },
+                {
+                    "x": 2.3006511827054728,
+                    "y": 6.4939017376421555,
+                    "heading": 0.2337896338628955,
+                    "angularVelocity": 1.2879764904688844,
+                    "velocityX": -0.007111899743518039,
+                    "velocityY": 2.4083035636166743,
+                    "timestamp": 0.8033496090713894
+                },
+                {
+                    "x": 2.300310606012852,
+                    "y": 6.62557002983581,
+                    "heading": 0.3107349410237032,
+                    "angularVelocity": 1.0535865944433043,
+                    "velocityX": -0.004663403798945386,
+                    "velocityY": 1.8028902955518677,
+                    "timestamp": 0.876381391714243
+                },
+                {
+                    "x": 2.30010034874232,
+                    "y": 6.713223552423264,
+                    "heading": 0.36487146716810653,
+                    "angularVelocity": 0.741273513877466,
+                    "velocityX": -0.002878983134789396,
+                    "velocityY": 1.2002106400182577,
+                    "timestamp": 0.9494131743570966
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": 3.5331853071795862,
-                    "angularVelocity": 0.3841870877769111,
-                    "velocityX": -0.0014524264794266268,
-                    "velocityY": 0.5994660505208633,
-                    "timestamp": 1.0224468173048815
+                    "heading": 0.393,
+                    "angularVelocity": 0.38515467942840836,
+                    "velocityX": -0.0013740420771443632,
+                    "velocityY": 0.5994163909542732,
+                    "timestamp": 1.02244495699995
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": 3.5331853071795862,
-                    "angularVelocity": -9.351204897899337e-33,
-                    "velocityX": 1.378221818744695e-30,
+                    "heading": 0.393,
+                    "angularVelocity": 8.681406116042979e-33,
+                    "velocityX": 1.0196837521674476e-33,
                     "velocityY": 0,
-                    "timestamp": 1.0954787328266589
+                    "timestamp": 1.0954767396428036
                 }
             ],
             "constraints": [
@@ -18092,7 +16645,7 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -18101,7 +16654,7 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -18112,101 +16665,101 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "angularVelocity": 0,
-                    "velocityX": -7.080178799155261e-34,
-                    "velocityY": 4.018324582530293e-34,
+                    "velocityX": 1.7319348109741582e-34,
+                    "velocityY": -4.404349434317184e-36,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.298001228420339,
-                    "y": 4.408001448490065,
-                    "heading": 2.765522898073599,
-                    "angularVelocity": 0.20408324361211075,
-                    "velocityX": -0.026278326719846846,
-                    "velocityY": 0.6310864929657463,
-                    "timestamp": 0.07606160015322963
+                    "x": 2.298001242299744,
+                    "y": 4.408001442881322,
+                    "heading": -0.3744767722715256,
+                    "angularVelocity": 0.20408758634762672,
+                    "velocityX": -0.02627814536217338,
+                    "velocityY": 0.6310864460799332,
+                    "timestamp": 0.07606159691669512
                 },
                 {
-                    "x": 2.294003714931991,
-                    "y": 4.50400409446738,
-                    "heading": 2.7965834623359793,
-                    "angularVelocity": 0.40836064715713705,
-                    "velocityX": -0.052556263348324604,
-                    "velocityY": 1.2621696859376152,
-                    "timestamp": 0.15212320030645926
+                    "x": 2.2940037533773614,
+                    "y": 4.504004077466311,
+                    "heading": -0.3434155460751892,
+                    "angularVelocity": 0.4083693671374729,
+                    "velocityX": -0.05255594261004741,
+                    "velocityY": 1.26216958986719,
+                    "timestamp": 0.15212319383339024
                 },
                 {
-                    "x": 2.288007388147497,
-                    "y": 4.648007477823444,
-                    "heading": 2.8432069040547985,
-                    "angularVelocity": 0.6129695092516301,
-                    "velocityX": -0.07883513852474495,
-                    "velocityY": 1.8932468297532885,
-                    "timestamp": 0.2281848004596889
+                    "x": 2.2880074570982525,
+                    "y": 4.648007443871442,
+                    "heading": -0.29679113248213845,
+                    "angularVelocity": 0.6129823127972874,
+                    "velocityX": -0.07883474081771351,
+                    "velocityY": 1.8932466874557927,
+                    "timestamp": 0.22818479075008535
                 },
                 {
-                    "x": 2.2800119392615206,
-                    "y": 4.840010948654735,
-                    "heading": 2.9054273803096926,
-                    "angularVelocity": 0.8180274426195046,
-                    "velocityX": -0.10511807363859851,
-                    "velocityY": 2.5243154291323253,
-                    "timestamp": 0.3042464006129185
+                    "x": 2.2800120385493474,
+                    "y": 4.8400108934587145,
+                    "heading": -0.2345694550660327,
+                    "angularVelocity": 0.8180432693814297,
+                    "velocityX": -0.10511767926279252,
+                    "velocityY": 2.524315257245527,
+                    "timestamp": 0.30424638766678047
                 },
                 {
-                    "x": 2.270016643226651,
-                    "y": 5.0800137105269405,
-                    "heading": 2.98328446819424,
-                    "angularVelocity": 1.0236057054768744,
-                    "velocityX": -0.13141054112368558,
-                    "velocityY": 3.155373557599477,
-                    "timestamp": 0.3803080007661481
+                    "x": 2.270016765601229,
+                    "y": 5.0800136328565335,
+                    "heading": -0.15671111441894314,
+                    "angularVelocity": 1.0236222194014972,
+                    "velocityX": -0.1314102431883619,
+                    "velocityY": 3.1553733963892037,
+                    "timestamp": 0.3803079845834756
                 },
                 {
-                    "x": 2.262010502624602,
-                    "y": 5.272008371939251,
-                    "heading": 3.045963904195738,
-                    "angularVelocity": 0.8240614958826565,
-                    "velocityX": -0.10525864018007076,
-                    "velocityY": 2.524199609599701,
-                    "timestamp": 0.4563696009193777
+                    "x": 2.2620105775052464,
+                    "y": 5.272008323301556,
+                    "heading": -0.09403333232435188,
+                    "angularVelocity": 0.8240397866381615,
+                    "velocityX": -0.10525926907308407,
+                    "velocityY": 2.524200098708164,
+                    "timestamp": 0.4563695815001707
                 },
                 {
-                    "x": 2.2560053812396474,
-                    "y": 5.416004211679494,
-                    "heading": 3.092980743508332,
-                    "angularVelocity": 0.6181416012531585,
-                    "velocityX": -0.07895076323476027,
-                    "velocityY": 1.8931476520367212,
-                    "timestamp": 0.5324312010726073
+                    "x": 2.256005419001405,
+                    "y": 5.416004186972581,
+                    "heading": -0.04701785122811225,
+                    "angularVelocity": 0.6181237707608528,
+                    "velocityX": -0.07895125460510179,
+                    "velocityY": 1.8931480472166962,
+                    "timestamp": 0.5324311784168658
                 },
                 {
-                    "x": 2.2520017956431593,
-                    "y": 5.512001402686665,
-                    "heading": 3.124326930482601,
-                    "angularVelocity": 0.412115797079227,
-                    "velocityX": -0.05263609480240691,
-                    "velocityY": 1.2620979681439777,
-                    "timestamp": 0.6084928012258369
+                    "x": 2.252001808264489,
+                    "y": 5.512001394535746,
+                    "heading": -0.015672604394819867,
+                    "angularVelocity": 0.4121034543571656,
+                    "velocityX": -0.05263642756936803,
+                    "velocityY": 1.262098239513749,
+                    "timestamp": 0.6084927753335609
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0.20605758340377522,
-                    "velocityX": -0.026318084803984385,
-                    "velocityY": 0.6310490078651869,
-                    "timestamp": 0.6845544013790665
+                    "heading": -1.7090620600656545e-34,
+                    "angularVelocity": 0.2060514770940842,
+                    "velocityX": -0.026318251859498508,
+                    "velocityY": 0.6310491418793501,
+                    "timestamp": 0.6845543722502561
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": -1.7962813810508635e-31,
-                    "velocityX": -5.242761228396242e-32,
-                    "velocityY": 0,
-                    "timestamp": 0.7606160015322961
+                    "heading": 0,
+                    "angularVelocity": 2.0052459094930362e-33,
+                    "velocityX": -1.9224144442861376e-33,
+                    "velocityY": 1.2850315433355203e-34,
+                    "timestamp": 0.7606159691669512
                 }
             ],
             "constraints": [
@@ -18249,7 +16802,7 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -18258,7 +16811,7 @@
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": 3.4031853071795863,
+                    "heading": 0.263,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -18269,173 +16822,173 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": -1.3064230645361168e-30,
-                    "velocityX": 1.1529790698064783e-31,
-                    "velocityY": -2.8761439718242387e-32,
+                    "heading": -0.39,
+                    "angularVelocity": 8.834598343952546e-30,
+                    "velocityX": -1.1855672444586933e-30,
+                    "velocityY": 4.732277944627028e-32,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.322180658668465,
-                    "y": 4.411221384877813,
-                    "heading": 2.7352853365602194,
-                    "angularVelocity": -0.1796018847298344,
-                    "velocityX": 0.2707291347647005,
-                    "velocityY": 0.6251897843383167,
-                    "timestamp": 0.08192933755631272
-                },
-                {
-                    "x": 2.368773081487876,
-                    "y": 4.512701566380937,
-                    "heading": 2.7089075651304313,
-                    "angularVelocity": -0.32195758218669895,
-                    "velocityX": 0.5686903398600793,
-                    "velocityY": 1.238630562994284,
-                    "timestamp": 0.16385867511262545
-                },
-                {
-                    "x": 2.4433012157913003,
-                    "y": 4.662644128727173,
-                    "heading": 2.675827445796147,
-                    "angularVelocity": -0.4037640278922902,
-                    "velocityX": 0.9096635774968738,
-                    "velocityY": 1.8301449373147467,
-                    "timestamp": 0.24578801266893818
-                },
-                {
-                    "x": 2.5519618748459716,
-                    "y": 4.856937153180571,
-                    "heading": 2.6453653517833016,
-                    "angularVelocity": -0.3718093532967731,
-                    "velocityX": 1.3262728870471487,
-                    "velocityY": 2.371470711817401,
-                    "timestamp": 0.3277173502252509
-                },
-                {
-                    "x": 2.7016003249644123,
-                    "y": 5.089209181124471,
-                    "heading": 2.6267184772011043,
-                    "angularVelocity": -0.22759703835491757,
-                    "velocityX": 1.8264330529413755,
-                    "velocityY": 2.835028756143084,
-                    "timestamp": 0.4096466877815636
-                },
-                {
-                    "x": 2.9069631472288275,
-                    "y": 5.3213406432232695,
-                    "heading": 2.6267188101864813,
-                    "angularVelocity": 0.00000406429963372636,
-                    "velocityX": 2.5065846788186557,
-                    "velocityY": 2.833313060040874,
-                    "timestamp": 0.4915760253378763
-                },
-                {
-                    "x": 3.1507251830977756,
-                    "y": 5.512753871430069,
-                    "heading": 2.626718909792111,
-                    "angularVelocity": 0.0000012157504557439165,
-                    "velocityX": 2.975271656522333,
-                    "velocityY": 2.3363209555458044,
-                    "timestamp": 0.5735053628941891
-                },
-                {
-                    "x": 3.4078762515345984,
-                    "y": 5.685763359739643,
-                    "heading": 2.6267190032534695,
-                    "angularVelocity": 0.0000011407556990966525,
-                    "velocityX": 3.1386933680511517,
-                    "velocityY": 2.1116915316282108,
-                    "timestamp": 0.6554347004505018
-                },
-                {
-                    "x": 3.665027465532842,
-                    "y": 5.858772631695159,
-                    "heading": 2.626719096714767,
-                    "angularVelocity": 0.0000011407549466335633,
-                    "velocityX": 3.1386951447214493,
-                    "velocityY": 2.111688890888465,
-                    "timestamp": 0.7373640380068145
-                },
-                {
-                    "x": 3.922178727707041,
-                    "y": 6.031781832044647,
-                    "heading": 2.6267191901760296,
-                    "angularVelocity": 0.0000011407545266438845,
-                    "velocityX": 3.138695732739822,
-                    "velocityY": 2.1116880168910543,
-                    "timestamp": 0.8192933755631272
-                },
-                {
-                    "x": 4.179330140012749,
-                    "y": 6.204790809246775,
-                    "heading": 2.6267192836372875,
-                    "angularVelocity": 0.0000011407544682834688,
-                    "velocityX": 3.1386975651909763,
-                    "velocityY": 2.111685293234714,
-                    "timestamp": 0.9012227131194399
-                },
-                {
-                    "x": 4.443624087689148,
-                    "y": 6.354175617474429,
-                    "heading": 2.642647782574834,
-                    "angularVelocity": 0.1944175238399595,
-                    "velocityX": 3.225876780643333,
-                    "velocityY": 1.823337191332418,
-                    "timestamp": 0.9831520506757526
-                },
-                {
-                    "x": 4.674751705763445,
-                    "y": 6.469415592948305,
-                    "heading": 2.769232553851288,
-                    "angularVelocity": 1.5450481482222196,
-                    "velocityX": 2.821060501257377,
-                    "velocityY": 1.4065776547341065,
-                    "timestamp": 1.0650813882320653
-                },
-                {
-                    "x": 4.870834347570915,
-                    "y": 6.549065174445882,
-                    "heading": 2.9849459088839683,
-                    "angularVelocity": 2.6329195556404166,
-                    "velocityX": 2.39331413698659,
-                    "velocityY": 0.9721741182495338,
-                    "timestamp": 1.1470107257883781
-                },
-                {
-                    "x": 5.022947063703052,
-                    "y": 6.5945122449585005,
-                    "heading": 3.181337324435562,
-                    "angularVelocity": 2.397082918150139,
-                    "velocityX": 1.856633053179344,
-                    "velocityY": 0.5547105819253249,
-                    "timestamp": 1.228940063344691
-                },
-                {
-                    "x": 5.124965104320653,
-                    "y": 6.619330120195266,
-                    "heading": 3.326379958974982,
-                    "angularVelocity": 1.7703381824578692,
-                    "velocityX": 1.2451954777185934,
-                    "velocityY": 0.3029180508106365,
-                    "timestamp": 1.3108694009010038
+                    "x": 2.3221690172235006,
+                    "y": 4.411229758092553,
+                    "heading": -0.4048175852865869,
+                    "angularVelocity": -0.18084977247894177,
+                    "velocityX": 0.27057456686796094,
+                    "velocityY": 0.6252631529349166,
+                    "timestamp": 0.08193311544440174
+                },
+                {
+                    "x": 2.3687303268224738,
+                    "y": 4.512732101891575,
+                    "heading": -0.43137596230210584,
+                    "angularVelocity": -0.3241470420293335,
+                    "velocityX": 0.5682843786231674,
+                    "velocityY": 1.2388439429951275,
+                    "timestamp": 0.16386623088880348
+                },
+                {
+                    "x": 2.4431953900618826,
+                    "y": 4.662720774571155,
+                    "heading": -0.464675218076173,
+                    "angularVelocity": -0.40641998773593985,
+                    "velocityX": 0.9088518462346009,
+                    "velocityY": 1.8306233305795314,
+                    "timestamp": 0.24579934633320522
+                },
+                {
+                    "x": 2.551739981424586,
+                    "y": 4.8571070042037645,
+                    "heading": -0.49532439278782164,
+                    "angularVelocity": -0.3740755437574734,
+                    "velocityX": 1.3247951182371547,
+                    "velocityY": 2.3724989410968234,
+                    "timestamp": 0.32773246177760695
+                },
+                {
+                    "x": 2.7012931711639805,
+                    "y": 5.089437783878959,
+                    "heading": -0.5138025644966426,
+                    "angularVelocity": -0.22552751239337346,
+                    "velocityX": 1.8253082276711035,
+                    "velocityY": 2.835615104039969,
+                    "timestamp": 0.4096655772220087
+                },
+                {
+                    "x": 2.906580390144238,
+                    "y": 5.321655181575939,
+                    "heading": -0.513802224550241,
+                    "angularVelocity": 0.0000041490720786488115,
+                    "velocityX": 2.5055463577429817,
+                    "velocityY": 2.834231268241715,
+                    "timestamp": 0.49159869266641043
+                },
+                {
+                    "x": 3.150285117410734,
+                    "y": 5.513164499731697,
+                    "heading": -0.5138021244683586,
+                    "angularVelocity": 0.0000012215071026742433,
+                    "velocityX": 2.974435012566654,
+                    "velocityY": 2.3373860143974894,
+                    "timestamp": 0.5735318081108122
+                },
+                {
+                    "x": 3.407534970443545,
+                    "y": 5.686052692352735,
+                    "heading": -0.513802030650944,
+                    "angularVelocity": 0.0000011450487881316094,
+                    "velocityX": 3.1397543183546524,
+                    "velocityY": 2.110113739521574,
+                    "timestamp": 0.6554649235552139
+                },
+                {
+                    "x": 3.664784970537928,
+                    "y": 5.858940666152602,
+                    "heading": -0.5138019368336011,
+                    "angularVelocity": 0.0000011450479143905032,
+                    "velocityX": 3.1397561132524885,
+                    "velocityY": 2.1101110687922637,
+                    "timestamp": 0.7373980389996156
+                },
+                {
+                    "x": 3.922035018762957,
+                    "y": 6.031828568336093,
+                    "heading": -0.5138018430162984,
+                    "angularVelocity": 0.0000011450474228928023,
+                    "velocityX": 3.139756700690747,
+                    "velocityY": 2.110110194708886,
+                    "timestamp": 0.8193311544440174
+                },
+                {
+                    "x": 4.1792852160642955,
+                    "y": 6.204716248700005,
+                    "heading": -0.5138017491990241,
+                    "angularVelocity": 0.0000011450470770765712,
+                    "velocityX": 3.1397585201786073,
+                    "velocityY": 2.1101074873837833,
+                    "timestamp": 0.9012642698884191
+                },
+                {
+                    "x": 4.443584051036782,
+                    "y": 6.354085007666646,
+                    "heading": -0.49783246108316886,
+                    "angularVelocity": 0.19490639443210175,
+                    "velocityX": 3.225787687175579,
+                    "velocityY": 1.8230572358499022,
+                    "timestamp": 0.9831973853328209
+                },
+                {
+                    "x": 4.674708985410259,
+                    "y": 6.469317132198814,
+                    "heading": -0.3712043024716245,
+                    "angularVelocity": 1.545506452729414,
+                    "velocityX": 2.8208976690299576,
+                    "velocityY": 1.4064169769104102,
+                    "timestamp": 1.0651305007772227
+                },
+                {
+                    "x": 4.870816846139177,
+                    "y": 6.548974010532027,
+                    "heading": -0.15522222327575852,
+                    "angularVelocity": 2.636077952417564,
+                    "velocityX": 2.393511581553284,
+                    "velocityY": 0.9722183503111966,
+                    "timestamp": 1.1470636162216246
+                },
+                {
+                    "x": 5.022935939261114,
+                    "y": 6.594453280726005,
+                    "heading": 0.041233837030506995,
+                    "angularVelocity": 2.3977613842791428,
+                    "velocityX": 1.856625276566729,
+                    "velocityY": 0.5550780041415454,
+                    "timestamp": 1.2289967316660264
+                },
+                {
+                    "x": 5.124959991715153,
+                    "y": 6.619307482178149,
+                    "heading": 0.1862409016069847,
+                    "angularVelocity": 1.7698224190545027,
+                    "velocityX": 1.245211437410438,
+                    "velocityY": 0.30334744769969435,
+                    "timestamp": 1.3109298471104283
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": 0.93745843058738,
-                    "velocityX": 0.6229135643171693,
-                    "velocityY": 0.13023271178533075,
-                    "timestamp": 1.3927987384573166
+                    "heading": 0.263,
+                    "angularVelocity": 0.9368507224054672,
+                    "velocityX": 0.6229472418814781,
+                    "velocityY": 0.13050300557783806,
+                    "timestamp": 1.39286296255483
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": -5.432893499766539e-33,
+                    "heading": 0.263,
+                    "angularVelocity": 1.6488266469177426e-31,
                     "velocityX": 0,
-                    "velocityY": 6.79272724985994e-33,
-                    "timestamp": 1.4747280760136294
+                    "velocityY": 1.9617182542723147e-31,
+                    "timestamp": 1.474796077999232
                 }
             ],
             "constraints": [
@@ -18478,7 +17031,7 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -18487,7 +17040,7 @@
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -18498,128 +17051,128 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "angularVelocity": 0,
-                    "velocityX": -1.3306255005182603e-31,
-                    "velocityY": -4.9176166375307755e-31,
+                    "velocityX": -1.8814689011962518e-30,
+                    "velocityY": -8.215910483342913e-30,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.322374290257025,
-                    "y": 4.418269020876138,
-                    "heading": 2.7110352663465282,
-                    "angularVelocity": -0.44706114894751375,
-                    "velocityX": 0.25671100431863964,
-                    "velocityY": 0.6685485303865802,
-                    "timestamp": 0.08715750349857801
-                },
-                {
-                    "x": 2.3748782855305732,
-                    "y": 4.5313296521256,
-                    "heading": 2.6355865457570187,
-                    "angularVelocity": -0.8656594964395732,
-                    "velocityX": 0.6024036160513037,
-                    "velocityY": 1.297199055859919,
-                    "timestamp": 0.17431500699715602
-                },
-                {
-                    "x": 2.469236718208719,
-                    "y": 4.691158857574848,
-                    "heading": 2.529430325019821,
-                    "angularVelocity": -1.2179814298941005,
-                    "velocityX": 1.0826197273959968,
-                    "velocityY": 1.8337974245884079,
-                    "timestamp": 0.26147251049573406
-                },
-                {
-                    "x": 2.6203114191501853,
-                    "y": 4.878748173786945,
-                    "heading": 2.4055559201698435,
-                    "angularVelocity": -1.421270686717166,
-                    "velocityX": 1.7333527794761936,
-                    "velocityY": 2.1523025405971743,
-                    "timestamp": 0.34863001399431204
-                },
-                {
-                    "x": 2.832356831160347,
-                    "y": 5.08316808870822,
-                    "heading": 2.2936588957805637,
-                    "angularVelocity": -1.2838484341294274,
-                    "velocityX": 2.4328991021825375,
-                    "velocityY": 2.345408102752848,
-                    "timestamp": 0.43578751749289
-                },
-                {
-                    "x": 3.098575041354179,
-                    "y": 5.2672072069550175,
-                    "heading": 2.274563036259226,
-                    "angularVelocity": -0.2190959900732964,
-                    "velocityX": 3.054449697474129,
-                    "velocityY": 2.1115694100829816,
-                    "timestamp": 0.522945020991468
-                },
-                {
-                    "x": 3.397459622153223,
-                    "y": 5.3975878258937575,
-                    "heading": 2.2845714271399826,
-                    "angularVelocity": 0.11483108715843302,
-                    "velocityX": 3.429246694794569,
-                    "velocityY": 1.495919613402737,
-                    "timestamp": 0.610102524490046
-                },
-                {
-                    "x": 3.6781811959965687,
-                    "y": 5.472346031669266,
-                    "heading": 2.382872565425927,
-                    "angularVelocity": 1.1278562870671076,
-                    "velocityX": 3.220853771332781,
-                    "velocityY": 0.8577368875271776,
-                    "timestamp": 0.697260027988624
-                },
-                {
-                    "x": 3.9150228109281926,
-                    "y": 5.514569108720165,
-                    "heading": 2.601398031866265,
-                    "angularVelocity": 2.5072478865104606,
-                    "velocityX": 2.717397876540693,
-                    "velocityY": 0.484445691489864,
-                    "timestamp": 0.784417531487202
-                },
-                {
-                    "x": 4.091172675855641,
-                    "y": 5.53970946369802,
-                    "heading": 2.8415186627639684,
-                    "angularVelocity": 2.7550196054160887,
-                    "velocityX": 2.0210522084346136,
-                    "velocityY": 0.28844739659465746,
-                    "timestamp": 0.8715750349857799
-                },
-                {
-                    "x": 4.206403236830247,
-                    "y": 5.5538325492768035,
-                    "heading": 3.0331865406048246,
-                    "angularVelocity": 2.1990978418052483,
-                    "velocityX": 1.322095704318632,
-                    "velocityY": 0.16204096046663827,
-                    "timestamp": 0.9587325384843579
+                    "x": 2.3223907686912884,
+                    "y": 4.418261357303619,
+                    "heading": -0.4292195342409252,
+                    "angularVelocity": -0.4499474209247965,
+                    "velocityX": 0.25687884418209317,
+                    "velocityY": 0.6684053741511637,
+                    "timestamp": 0.08716470506779908
+                },
+                {
+                    "x": 2.37490967527786,
+                    "y": 4.531319644696227,
+                    "heading": -0.5050759172241152,
+                    "angularVelocity": -0.8702648959138656,
+                    "velocityX": 0.6025249158558101,
+                    "velocityY": 1.2970649909807879,
+                    "timestamp": 0.17432941013559816
+                },
+                {
+                    "x": 2.4692607215165214,
+                    "y": 4.691180973762412,
+                    "heading": -0.6116326818995184,
+                    "angularVelocity": -1.222476053725161,
+                    "velocityX": 1.0824455399151758,
+                    "velocityY": 1.8340144550691926,
+                    "timestamp": 0.26149411520339727
+                },
+                {
+                    "x": 2.6203318015659547,
+                    "y": 4.878827771954509,
+                    "heading": -0.7357227336999919,
+                    "angularVelocity": -1.4236272778523056,
+                    "velocityX": 1.7331680286410147,
+                    "velocityY": 2.1527841807775245,
+                    "timestamp": 0.3486588202711963
+                },
+                {
+                    "x": 2.8323811449649123,
+                    "y": 5.083303607446834,
+                    "heading": -0.8475943342447533,
+                    "angularVelocity": -1.2834506863500095,
+                    "velocityX": 2.432743198454233,
+                    "velocityY": 2.3458558751879917,
+                    "timestamp": 0.4358235253389954
+                },
+                {
+                    "x": 3.0986117768734247,
+                    "y": 5.267387942263234,
+                    "heading": -0.8666588377121885,
+                    "angularVelocity": -0.2187181549298817,
+                    "velocityX": 3.054339846632081,
+                    "velocityY": 2.111913700312692,
+                    "timestamp": 0.5229882304067944
+                },
+                {
+                    "x": 3.3974594561484373,
+                    "y": 5.39777666903363,
+                    "heading": -0.8564935513214242,
+                    "angularVelocity": 0.11662158878249514,
+                    "velocityX": 3.428540015623993,
+                    "velocityY": 1.4958890375292995,
+                    "timestamp": 0.6101529354745935
+                },
+                {
+                    "x": 3.678181440932401,
+                    "y": 5.472509264787258,
+                    "heading": -0.7581228056155188,
+                    "angularVelocity": 1.1285616767634317,
+                    "velocityX": 3.2205923781375785,
+                    "velocityY": 0.8573722092617639,
+                    "timestamp": 0.6973176405423925
+                },
+                {
+                    "x": 3.9150331223150032,
+                    "y": 5.514676359956983,
+                    "heading": -0.5395207483783628,
+                    "angularVelocity": 2.507919427560975,
+                    "velocityX": 2.7172888521606593,
+                    "velocityY": 0.483763412460658,
+                    "timestamp": 0.7844823456101916
+                },
+                {
+                    "x": 4.091189365639749,
+                    "y": 5.539762978420037,
+                    "heading": -0.2990438366325736,
+                    "angularVelocity": 2.758879428992955,
+                    "velocityX": 2.0209584049843006,
+                    "velocityY": 0.28780707103340436,
+                    "timestamp": 0.8716470506779906
+                },
+                {
+                    "x": 4.206412444969991,
+                    "y": 5.553849615471576,
+                    "heading": -0.10702573317574342,
+                    "angularVelocity": 2.202934126920672,
+                    "velocityX": 1.3219006390329475,
+                    "velocityY": 0.16160941565260478,
+                    "timestamp": 0.9588117557457897
                 },
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 1.2255222454474992,
-                    "velocityX": 0.649361912605446,
-                    "velocityY": 0.0707621314933191,
-                    "timestamp": 1.045890041982936
+                    "heading": 0,
+                    "angularVelocity": 1.2278563105616622,
+                    "velocityX": 0.6492026214738336,
+                    "velocityY": 0.07056049261727199,
+                    "timestamp": 1.0459764608135889
                 },
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 2.336425691005573e-30,
-                    "velocityX": -1.4618072975561068e-30,
-                    "velocityY": 3.4366383722351896e-30,
-                    "timestamp": 1.133047545481514
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": 1.8862699039176484e-32,
+                    "velocityY": 0,
+                    "timestamp": 1.133141165881388
                 }
             ],
             "constraints": [
@@ -18662,7 +17215,7 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -18671,7 +17224,7 @@
                 {
                     "x": 4.263,
                     "y": 3,
-                    "heading": 2.574,
+                    "heading": -0.566,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -18682,119 +17235,119 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": -2.857623496877536e-30,
-                    "velocityX": -2.5600502945380354e-30,
-                    "velocityY": 7.096305433334873e-32,
+                    "heading": -0.39,
+                    "angularVelocity": 1.6493730429010574e-29,
+                    "velocityX": 1.2833975714085328e-29,
+                    "velocityY": -2.2715148176984883e-30,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.2589562715242293,
-                    "y": 4.250539211803029,
-                    "heading": 2.836221335933998,
-                    "angularVelocity": 0.7208664352087386,
-                    "velocityX": -0.3431522593974162,
-                    "velocityY": -0.9151633679526462,
-                    "timestamp": 0.1196079214161022
-                },
-                {
-                    "x": 2.2003066234183892,
-                    "y": 4.024396154786088,
-                    "heading": 2.9853349315252267,
-                    "angularVelocity": 1.2466866226394515,
-                    "velocityX": -0.49034919603525046,
-                    "velocityY": -1.8907030097966573,
-                    "timestamp": 0.2392158428322044
-                },
-                {
-                    "x": 2.1964987876971427,
-                    "y": 3.692388997223788,
-                    "heading": 3.120971884356915,
-                    "angularVelocity": 1.1340131257679336,
-                    "velocityX": -0.03183598273568803,
-                    "velocityY": -2.7757957301782654,
-                    "timestamp": 0.3588237642483066
-                },
-                {
-                    "x": 2.306992116857372,
-                    "y": 3.3594977499513337,
-                    "heading": 3.1303317506759742,
-                    "angularVelocity": 0.07825456883065958,
-                    "velocityX": 0.9237960818316977,
-                    "velocityY": -2.7831872950485823,
-                    "timestamp": 0.4784316856644088
-                },
-                {
-                    "x": 2.522086295400282,
-                    "y": 3.073270003946497,
-                    "heading": 3.020615571964542,
-                    "angularVelocity": -0.9172985987240034,
-                    "velocityX": 1.7983272010439872,
-                    "velocityY": -2.39305008076411,
-                    "timestamp": 0.598039607080511
-                },
-                {
-                    "x": 2.8292729817758957,
-                    "y": 2.8628580598282483,
-                    "heading": 2.911598621970614,
-                    "angularVelocity": -0.9114525919611488,
-                    "velocityX": 2.568280451149536,
-                    "velocityY": -1.7591806765559896,
-                    "timestamp": 0.7176475284966132
-                },
-                {
-                    "x": 3.212139121131616,
-                    "y": 2.7446490517974147,
-                    "heading": 2.8046662811978735,
-                    "angularVelocity": -0.8940239033224252,
-                    "velocityX": 3.2010098898363166,
-                    "velocityY": -0.9883041744332437,
-                    "timestamp": 0.8372554499127154
-                },
-                {
-                    "x": 3.61962025055875,
-                    "y": 2.7431499033487023,
-                    "heading": 2.702792215888405,
-                    "angularVelocity": -0.8517334312273422,
-                    "velocityX": 3.406807213124051,
-                    "velocityY": -0.012533855876376997,
-                    "timestamp": 0.9568633713288176
-                },
-                {
-                    "x": 3.9493476486905155,
-                    "y": 2.8320906325242436,
-                    "heading": 2.608128750394254,
-                    "angularVelocity": -0.7914481279620891,
-                    "velocityX": 2.7567354588888784,
-                    "velocityY": 0.7436023310372396,
-                    "timestamp": 1.0764712927449198
-                },
-                {
-                    "x": 4.162172137163363,
-                    "y": 2.936272219295665,
-                    "heading": 2.5785368758755864,
-                    "angularVelocity": -0.2474073135651704,
-                    "velocityX": 1.7793511161560736,
-                    "velocityY": 0.8710258111499355,
-                    "timestamp": 1.196079214161022
+                    "x": 2.2589715401081683,
+                    "y": 4.250531667444243,
+                    "heading": -0.30389257282100673,
+                    "angularVelocity": 0.7199270532350494,
+                    "velocityX": -0.3430307837128689,
+                    "velocityY": -0.9152429315492105,
+                    "timestamp": 0.11960576671208706
+                },
+                {
+                    "x": 2.20035382258247,
+                    "y": 4.024377658967842,
+                    "heading": -0.15502297939398071,
+                    "angularVelocity": 1.244669028252838,
+                    "velocityX": -0.49009106447848483,
+                    "velocityY": -1.890828633880168,
+                    "timestamp": 0.23921153342417412
+                },
+                {
+                    "x": 2.196580641751606,
+                    "y": 3.6923673074709873,
+                    "heading": -0.01977955961101222,
+                    "angularVelocity": 1.1307433036110668,
+                    "velocityX": -0.031546813624408795,
+                    "velocityY": -2.7758724401313977,
+                    "timestamp": 0.3588173001362612
+                },
+                {
+                    "x": 2.3070960866935795,
+                    "y": 3.359465621426977,
+                    "heading": -0.010919467382492536,
+                    "angularVelocity": 0.07407746693291484,
+                    "velocityX": 0.9239976297129873,
+                    "velocityY": -2.7833247108007275,
+                    "timestamp": 0.47842306684834823
+                },
+                {
+                    "x": 2.522207133394474,
+                    "y": 3.0732241883923836,
+                    "heading": -0.12115396742434953,
+                    "angularVelocity": -0.9216487053427286,
+                    "velocityX": 1.7985006293108312,
+                    "velocityY": -2.3932076262144095,
+                    "timestamp": 0.5980288335604353
+                },
+                {
+                    "x": 2.8293982195436245,
+                    "y": 2.8628067869682225,
+                    "heading": -0.23050381325401667,
+                    "angularVelocity": -0.9142522876239637,
+                    "velocityX": 2.5683635044839206,
+                    "velocityY": -1.759257995734114,
+                    "timestamp": 0.7176346002725225
+                },
+                {
+                    "x": 3.2122552089453738,
+                    "y": 2.74459738527869,
+                    "heading": -0.337506021977117,
+                    "angularVelocity": -0.8946241612299765,
+                    "velocityX": 3.200991055250252,
+                    "velocityY": -0.9883252700855956,
+                    "timestamp": 0.8372403669846096
+                },
+                {
+                    "x": 3.6197114657100724,
+                    "y": 2.743095325026798,
+                    "heading": -0.4390431608485269,
+                    "angularVelocity": -0.8489318003875522,
+                    "velocityX": 3.406660631552456,
+                    "velocityY": -0.012558426681036643,
+                    "timestamp": 0.9568461336966967
+                },
+                {
+                    "x": 3.949401767099036,
+                    "y": 2.832014285774074,
+                    "heading": -0.5329543188084961,
+                    "angularVelocity": -0.7851724924441524,
+                    "velocityX": 2.7564749631394143,
+                    "velocityY": 0.7434337255770672,
+                    "timestamp": 1.0764519004087838
+                },
+                {
+                    "x": 4.162198776267108,
+                    "y": 2.936236025183353,
+                    "heading": -0.5618569264034092,
+                    "angularVelocity": -0.24164894711549667,
+                    "velocityX": 1.7791534222619447,
+                    "velocityY": 0.8713772109346234,
+                    "timestamp": 1.196057667120871
                 },
                 {
                     "x": 4.263,
                     "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": -0.037931232495828804,
-                    "velocityX": 0.842986498242598,
-                    "velocityY": 0.5328056866955708,
-                    "timestamp": 1.315687135577124
+                    "heading": -0.566,
+                    "angularVelocity": -0.03463941338677448,
+                    "velocityX": 0.8427789604454271,
+                    "velocityY": 0.5331178969835093,
+                    "timestamp": 1.315663433832958
                 },
                 {
                     "x": 4.263,
                     "y": 3,
-                    "heading": 2.574,
+                    "heading": -0.566,
                     "angularVelocity": 0,
-                    "velocityX": 1.3483931579260095e-31,
-                    "velocityY": -3.3355137434500586e-29,
-                    "timestamp": 1.4352950569932261
+                    "velocityX": 1.4642265257925552e-30,
+                    "velocityY": 5.326911647277941e-29,
+                    "timestamp": 1.4352692005450451
                 }
             ],
             "constraints": [
@@ -18837,7 +17390,7 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -18846,183 +17399,20 @@
                 {
                     "x": 5.176,
                     "y": 1.62,
-                    "heading": 2.5,
+                    "heading": -0.64,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": -2.979591758162217e-32,
-                    "velocityX": -5.0524169564327655e-33,
-                    "velocityY": 3.4190943066430706e-33,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.2859321803383907,
-                    "y": 4.28632531036449,
-                    "heading": 2.8392348343623115,
-                    "angularVelocity": 0.9181215337942725,
-                    "velocityX": -0.14474132503475426,
-                    "velocityY": -0.7580259383384993,
-                    "timestamp": 0.09719283458425766
-                },
-                {
-                    "x": 2.269559639424535,
-                    "y": 4.136180116377401,
-                    "heading": 2.9969853519762393,
-                    "angularVelocity": 1.623067361793753,
-                    "velocityX": -0.1684541971009417,
-                    "velocityY": -1.5448175231161345,
-                    "timestamp": 0.19438566916851532
-                },
-                {
-                    "x": 2.2724010855339243,
-                    "y": 3.9101733835281753,
-                    "heading": 3.1867879122348075,
-                    "angularVelocity": 1.952845197595569,
-                    "velocityX": 0.029235139828396976,
-                    "velocityY": -2.325343568957208,
-                    "timestamp": 0.291578503752773
-                },
-                {
-                    "x": 2.3226715061490353,
-                    "y": 3.6216059722814986,
-                    "heading": 3.3538116791494788,
-                    "angularVelocity": 1.7184781947055583,
-                    "velocityX": 0.5172235260978133,
-                    "velocityY": -2.969019398199714,
-                    "timestamp": 0.38877133833703065
-                },
-                {
-                    "x": 2.437426080371791,
-                    "y": 3.296473518840458,
-                    "heading": 3.427293312254084,
-                    "angularVelocity": 0.7560396136085858,
-                    "velocityX": 1.1806896538579041,
-                    "velocityY": -3.3452306935155747,
-                    "timestamp": 0.4859641729212883
-                },
-                {
-                    "x": 2.6276283995685685,
-                    "y": 2.9818185735083116,
-                    "heading": 3.4272931426736677,
-                    "angularVelocity": -0.000001744783111288276,
-                    "velocityX": 1.9569582470803324,
-                    "velocityY": -3.2374294532933714,
-                    "timestamp": 0.583157007505546
-                },
-                {
-                    "x": 2.8804858808256992,
-                    "y": 2.7148958271590775,
-                    "heading": 3.4272929522201836,
-                    "angularVelocity": -0.00000195954243555236,
-                    "velocityX": 2.6016062021313475,
-                    "velocityY": -2.7463212436492483,
-                    "timestamp": 0.6803498420898036
-                },
-                {
-                    "x": 3.1843940400703103,
-                    "y": 2.5079549973523347,
-                    "heading": 3.4272928289705424,
-                    "angularVelocity": -0.0000012680939050558275,
-                    "velocityX": 3.126857659256241,
-                    "velocityY": -2.1291778420902356,
-                    "timestamp": 0.7775426766740613
-                },
-                {
-                    "x": 3.5145691798617946,
-                    "y": 2.3461864714628624,
-                    "heading": 3.4272927145634307,
-                    "angularVelocity": -0.0000011771146716870812,
-                    "velocityX": 3.397114007465763,
-                    "velocityY": -1.66440794304886,
-                    "timestamp": 0.8747355112583189
-                },
-                {
-                    "x": 3.844744525773579,
-                    "y": 2.1844183662726744,
-                    "heading": 3.427292600156351,
-                    "angularVelocity": -0.0000011771143448425205,
-                    "velocityX": 3.397116128201317,
-                    "velocityY": -1.6644036145478303,
-                    "timestamp": 0.9719283458425766
-                },
-                {
-                    "x": 4.174919952487512,
-                    "y": 2.022650426003138,
-                    "heading": 3.42729248574922,
-                    "angularVelocity": -0.0000011771148700868944,
-                    "velocityX": 3.3971169595604263,
-                    "velocityY": -1.6644019177082188,
-                    "timestamp": 1.0691211804268343
-                },
-                {
-                    "x": 4.503263696345804,
-                    "y": 1.8851757396047895,
-                    "heading": 3.3983831121261034,
-                    "angularVelocity": -0.29744346634992885,
-                    "velocityX": 3.3782710964525586,
-                    "velocityY": -1.4144528965164656,
-                    "timestamp": 1.166314015011092
-                },
-                {
-                    "x": 4.772302673813676,
-                    "y": 1.7735532891774939,
-                    "heading": 3.2030632550848326,
-                    "angularVelocity": -2.009611694902728,
-                    "velocityX": 2.7680947738450667,
-                    "velocityY": -1.1484637823843746,
-                    "timestamp": 1.2635068495953496
-                },
-                {
-                    "x": 4.977265937404966,
-                    "y": 1.6951772631841453,
-                    "heading": 2.8890004427677543,
-                    "angularVelocity": -3.2313371007284832,
-                    "velocityX": 2.1088310107223527,
-                    "velocityY": -0.8063971621838382,
-                    "timestamp": 1.3606996841796073
-                },
-                {
-                    "x": 5.110692645903806,
-                    "y": 1.6448171116124384,
-                    "heading": 2.6382240127177106,
-                    "angularVelocity": -2.5801946318649924,
-                    "velocityX": 1.3728039630655127,
-                    "velocityY": -0.5181467521461082,
-                    "timestamp": 1.457892518763865
-                },
+            "trajectory": [],
+            "constraints": [
                 {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": -1.4221625833731872,
-                    "velocityX": 0.6719358929652187,
-                    "velocityY": -0.2553389014590782,
-                    "timestamp": 1.5550853533481226
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": 5.001833480552254e-32,
-                    "velocityX": 0,
-                    "velocityY": -3.22512482003026e-32,
-                    "timestamp": 1.6522781879323802
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
                 },
                 {
                     "scope": [
@@ -19057,16 +17447,16 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 26
+                    "controlIntervalCount": 30
                 },
                 {
                     "x": 4.276216983795166,
                     "y": 6.32326602935791,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -19075,7 +17465,7 @@
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -19086,515 +17476,551 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "angularVelocity": 0,
                     "velocityX": 0,
-                    "velocityY": -5.253317174178206e-37,
+                    "velocityY": 1.095053106710567e-32,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.3042260943612694,
-                    "y": 4.3705069747477046,
-                    "heading": 2.7457396895687083,
-                    "angularVelocity": -0.11528375442815593,
-                    "velocityX": 0.11435786954779384,
-                    "velocityY": 0.2843181303644528,
-                    "timestamp": 0.03695499381005128
-                },
-                {
-                    "x": 2.312934956515594,
-                    "y": 4.391423172843284,
-                    "heading": 2.737520345774514,
-                    "angularVelocity": -0.2224149687709801,
-                    "velocityX": 0.23566130734830049,
-                    "velocityY": 0.5659911134903584,
-                    "timestamp": 0.07390998762010256
-                },
-                {
-                    "x": 2.326431561960596,
-                    "y": 4.422622192917349,
-                    "heading": 2.725704837211404,
-                    "angularVelocity": -0.31972697989997184,
-                    "velocityX": 0.3652173645157235,
-                    "velocityY": 0.8442436828545633,
-                    "timestamp": 0.11086498143015384
-                },
-                {
-                    "x": 2.3450829408251197,
-                    "y": 4.463936524002756,
-                    "heading": 2.7107373323748916,
-                    "angularVelocity": -0.40501981717128094,
-                    "velocityX": 0.5047052357900914,
-                    "velocityY": 1.1179634151141484,
-                    "timestamp": 0.14781997524020513
-                },
-                {
-                    "x": 2.369336863920227,
-                    "y": 4.515137741117879,
-                    "heading": 2.6931719166575077,
-                    "angularVelocity": -0.4753191356943592,
-                    "velocityX": 0.6563097593730617,
-                    "velocityY": 1.3855019805522522,
-                    "timestamp": 0.18477496905025642
-                },
-                {
-                    "x": 2.3997470229979276,
-                    "y": 4.575903866240714,
-                    "heading": 2.6737152145246563,
-                    "angularVelocity": -0.5264972369596066,
-                    "velocityX": 0.8228971498144095,
-                    "velocityY": 1.6443278392948362,
-                    "timestamp": 0.22172996286030772
-                },
-                {
-                    "x": 2.4370055110126323,
-                    "y": 4.645763622209906,
-                    "heading": 2.6532916288606927,
-                    "angularVelocity": -0.5526610495171655,
-                    "velocityX": 1.0082125356646991,
-                    "velocityY": 1.890400965246281,
-                    "timestamp": 0.258684956670359
-                },
-                {
-                    "x": 2.4818450920186503,
-                    "y": 4.724119837508184,
-                    "heading": 2.6331026993833615,
-                    "angularVelocity": -0.5463112666478134,
-                    "velocityX": 1.213356474540179,
-                    "velocityY": 2.1203146644004023,
-                    "timestamp": 0.2956399504804103
-                },
-                {
-                    "x": 2.5345067531707874,
-                    "y": 4.810746963179403,
-                    "heading": 2.6138856788459637,
-                    "angularVelocity": -0.5200114668175445,
-                    "velocityX": 1.425021511918485,
-                    "velocityY": 2.3441250218165925,
-                    "timestamp": 0.3325949442904616
-                },
-                {
-                    "x": 2.595354080808564,
-                    "y": 4.90526717496031,
-                    "heading": 2.5968144984302115,
-                    "angularVelocity": -0.46194515695220434,
-                    "velocityX": 1.6465251746633045,
-                    "velocityY": 2.557711476471661,
-                    "timestamp": 0.3695499381005129
-                },
-                {
-                    "x": 2.6649832778781826,
-                    "y": 5.00693656288579,
-                    "heading": 2.584029518775602,
-                    "angularVelocity": -0.3459608116923114,
-                    "velocityX": 1.8841620547283313,
-                    "velocityY": 2.7511677703982533,
-                    "timestamp": 0.4065049319105642
-                },
-                {
-                    "x": 2.7444162884451204,
-                    "y": 5.113834606219336,
-                    "heading": 2.580154938465408,
-                    "angularVelocity": -0.10484591960993231,
-                    "velocityX": 2.149452682233515,
-                    "velocityY": 2.8926548840192674,
-                    "timestamp": 0.4434599257206155
-                },
-                {
-                    "x": 2.835170549378822,
-                    "y": 5.2201703041601135,
-                    "heading": 2.5801550588658806,
-                    "angularVelocity": 0.0000032580298363436347,
-                    "velocityX": 2.4558050638616957,
-                    "velocityY": 2.877437850141254,
-                    "timestamp": 0.4804149195306668
-                },
-                {
-                    "x": 2.934277899482491,
-                    "y": 5.318767249922712,
-                    "heading": 2.580155115984828,
-                    "angularVelocity": 0.0000015456354283753369,
-                    "velocityX": 2.681839174783285,
-                    "velocityY": 2.6680276627669444,
-                    "timestamp": 0.517369913340718
-                },
-                {
-                    "x": 3.041080419735732,
-                    "y": 5.408971720433865,
-                    "heading": 2.5801551701001046,
-                    "angularVelocity": 0.0000014643562563831181,
-                    "velocityX": 2.8900700349783843,
-                    "velocityY": 2.440927766753384,
-                    "timestamp": 0.5543249071507693
-                },
-                {
-                    "x": 3.1521380355083095,
-                    "y": 5.493882443444743,
-                    "heading": 2.5801552231562916,
-                    "angularVelocity": 0.0000014356973685406667,
-                    "velocityX": 3.005212674189958,
-                    "velocityY": 2.2976792648733673,
-                    "timestamp": 0.5912799009608206
-                },
-                {
-                    "x": 3.263196121755215,
-                    "y": 5.578792551102765,
-                    "heading": 2.580155276212398,
-                    "angularVelocity": 0.0000014356951704765205,
-                    "velocityX": 3.005225405198131,
-                    "velocityY": 2.29766261346058,
-                    "timestamp": 0.6282348947708719
-                },
-                {
-                    "x": 3.374254224320857,
-                    "y": 5.6637026374167005,
-                    "heading": 2.580155329268507,
-                    "angularVelocity": 0.0000014356952512853246,
-                    "velocityX": 3.0052258467821833,
-                    "velocityY": 2.297662035890833,
-                    "timestamp": 0.6651898885809232
-                },
-                {
-                    "x": 3.485312339954463,
-                    "y": 5.748612706638388,
-                    "heading": 2.5801553823246177,
-                    "angularVelocity": 0.0000014356952943350777,
-                    "velocityX": 3.0052262004005508,
-                    "velocityY": 2.2976615733755907,
-                    "timestamp": 0.7021448823909745
-                },
-                {
-                    "x": 3.5963704664420417,
-                    "y": 5.833522761663614,
-                    "heading": 2.58015543538073,
-                    "angularVelocity": 0.0000014356953270064253,
-                    "velocityX": 3.0052264941084177,
-                    "velocityY": 2.297661189220169,
-                    "timestamp": 0.7390998762010258
-                },
-                {
-                    "x": 3.70742860215257,
-                    "y": 5.918432804625673,
-                    "heading": 2.5801554884368434,
-                    "angularVelocity": 0.0000014356953601191765,
-                    "velocityX": 3.0052267436809124,
-                    "velocityY": 2.2976608627915387,
-                    "timestamp": 0.7760548700110771
-                },
-                {
-                    "x": 3.8184867458225393,
-                    "y": 6.00334283717717,
-                    "heading": 2.580155541492958,
-                    "angularVelocity": 0.0000014356953984229,
-                    "velocityX": 3.005226959062899,
-                    "velocityY": 2.2976605810823423,
-                    "timestamp": 0.8130098638211284
-                },
-                {
-                    "x": 3.9295448964619117,
-                    "y": 6.088252860613022,
-                    "heading": 2.580155594549074,
-                    "angularVelocity": 0.0000014356954384844398,
-                    "velocityX": 3.0052271476545696,
-                    "velocityY": 2.2976603344135205,
-                    "timestamp": 0.8499648576311797
-                },
-                {
-                    "x": 4.040603429478696,
-                    "y": 6.173162383915615,
-                    "heading": 2.5801556476051415,
-                    "angularVelocity": 0.0000014356941260173984,
-                    "velocityX": 3.005237494765267,
-                    "velocityY": 2.2976468008363797,
-                    "timestamp": 0.886919851441231
-                },
-                {
-                    "x": 4.155351262344264,
-                    "y": 6.253015747837148,
-                    "heading": 2.580155700374003,
-                    "angularVelocity": 0.0000014279223609881846,
-                    "velocityX": 3.105069735781099,
-                    "velocityY": 2.1608274197522537,
-                    "timestamp": 0.9238748452512823
+                    "x": 2.3031602212725026,
+                    "y": 4.367935645195281,
+                    "heading": -0.3933962364434778,
+                    "angularVelocity": -0.10578752662991632,
+                    "velocityX": 0.0984360180997807,
+                    "velocityY": 0.24718310735800264,
+                    "timestamp": 0.032104318454850914
+                },
+                {
+                    "x": 2.309652031024921,
+                    "y": 4.383743043384139,
+                    "heading": -0.39998627296935113,
+                    "angularVelocity": -0.20526947286363117,
+                    "velocityX": 0.20220986038211264,
+                    "velocityY": 0.4923760711845366,
+                    "timestamp": 0.06420863690970183
+                },
+                {
+                    "x": 2.3196739941431104,
+                    "y": 4.407342390021744,
+                    "heading": -0.40953275493580865,
+                    "angularVelocity": -0.29735818811675285,
+                    "velocityX": 0.31216869257896596,
+                    "velocityY": 0.7350832465356143,
+                    "timestamp": 0.09631295536455274
+                },
+                {
+                    "x": 2.333458307096076,
+                    "y": 4.438632228164309,
+                    "heading": -0.4217541925110665,
+                    "angularVelocity": -0.3806789292987277,
+                    "velocityX": 0.4293600866297902,
+                    "velocityY": 0.9746301945817274,
+                    "timestamp": 0.12841727381940365
+                },
+                {
+                    "x": 2.3512791398330983,
+                    "y": 4.477480960055527,
+                    "heading": -0.4363123371700568,
+                    "angularVelocity": -0.4534637506622021,
+                    "velocityX": 0.5550914517025046,
+                    "velocityY": 1.21007807550416,
+                    "timestamp": 0.16052159227425455
+                },
+                {
+                    "x": 2.3734634122157945,
+                    "y": 4.523713969255995,
+                    "heading": -0.45279460426513807,
+                    "angularVelocity": -0.5133971966500652,
+                    "velocityX": 0.691005866201282,
+                    "velocityY": 1.4400869236793286,
+                    "timestamp": 0.19262591072910545
+                },
+                {
+                    "x": 2.4004044405613056,
+                    "y": 4.577093572392249,
+                    "heading": -0.4706891378053212,
+                    "angularVelocity": -0.5573871180398601,
+                    "velocityX": 0.8391714773013875,
+                    "velocityY": 1.6626923013900174,
+                    "timestamp": 0.22473022918395635
+                },
+                {
+                    "x": 2.432578225951104,
+                    "y": 4.63728707308353,
+                    "heading": -0.48934877664951465,
+                    "angularVelocity": -0.581218968109697,
+                    "velocityX": 1.002163787873114,
+                    "velocityY": 1.8749347000134435,
+                    "timestamp": 0.25683454763880725
+                },
+                {
+                    "x": 2.4705256917588616,
+                    "y": 4.703846202675345,
+                    "heading": -0.5079484621261662,
+                    "angularVelocity": -0.5793515131868818,
+                    "velocityX": 1.1820050271780043,
+                    "velocityY": 2.073214221489191,
+                    "timestamp": 0.28893886609365815
+                },
+                {
+                    "x": 2.5143889510907504,
+                    "y": 4.77663720854218,
+                    "heading": -0.5259955708448517,
+                    "angularVelocity": -0.5621395995079442,
+                    "velocityX": 1.366272870535308,
+                    "velocityY": 2.267327554989265,
+                    "timestamp": 0.32104318454850905
+                },
+                {
+                    "x": 2.564366566619072,
+                    "y": 4.855456871813396,
+                    "heading": -0.5427715655324792,
+                    "angularVelocity": -0.5225463580925951,
+                    "velocityX": 1.5567256348583118,
+                    "velocityY": 2.4551109341275055,
+                    "timestamp": 0.35314750300335995
+                },
+                {
+                    "x": 2.620751841763659,
+                    "y": 4.939963572730219,
+                    "heading": -0.5571329301868887,
+                    "angularVelocity": -0.44733435704626356,
+                    "velocityX": 1.7563143482981505,
+                    "velocityY": 2.6322533847172953,
+                    "timestamp": 0.38525182145821085
+                },
+                {
+                    "x": 2.6840019979529464,
+                    "y": 5.029487269137319,
+                    "heading": -0.5670183536644556,
+                    "angularVelocity": -0.30791569338152147,
+                    "velocityX": 1.9701448039844656,
+                    "velocityY": 2.788525055686805,
+                    "timestamp": 0.41735613991306175
+                },
+                {
+                    "x": 2.7547918742113016,
+                    "y": 5.122370588176559,
+                    "heading": -0.568384562504894,
+                    "angularVelocity": -0.0425552980468726,
+                    "velocityX": 2.2049954543625816,
+                    "velocityY": 2.893172118569162,
+                    "timestamp": 0.44946045836791265
+                },
+                {
+                    "x": 2.8341176789452183,
+                    "y": 5.214333601098647,
+                    "heading": -0.5683844391321649,
+                    "angularVelocity": 0.0000038428702119811485,
+                    "velocityX": 2.4708764599838617,
+                    "velocityY": 2.864506002562167,
+                    "timestamp": 0.48156477682276355
+                },
+                {
+                    "x": 2.9197482153809866,
+                    "y": 5.3004567509455125,
+                    "heading": -0.5683843814173083,
+                    "angularVelocity": 0.0000017977287583592433,
+                    "velocityX": 2.6672591276526463,
+                    "velocityY": 2.6826032755681406,
+                    "timestamp": 0.5136690952776145
+                },
+                {
+                    "x": 3.0112544304014244,
+                    "y": 5.380309105627853,
+                    "heading": -0.5683843265105415,
+                    "angularVelocity": 0.0000017102610943456471,
+                    "velocityX": 2.850277452521701,
+                    "velocityY": 2.487277678691065,
+                    "timestamp": 0.5457734137324655
+                },
+                {
+                    "x": 3.107551803887462,
+                    "y": 5.454313172250514,
+                    "heading": -0.5683842730108181,
+                    "angularVelocity": 0.0000016664338651344868,
+                    "velocityX": 2.999514648518804,
+                    "velocityY": 2.305112526426488,
+                    "timestamp": 0.5778777321873164
+                },
+                {
+                    "x": 3.2038516202489813,
+                    "y": 5.528314059986814,
+                    "heading": -0.5683842195116714,
+                    "angularVelocity": 0.000001666415897857932,
+                    "velocityX": 2.9995907403219917,
+                    "velocityY": 2.305013509019602,
+                    "timestamp": 0.6099820506421674
+                },
+                {
+                    "x": 3.3001514533185667,
+                    "y": 5.602314925980335,
+                    "heading": -0.5683841660125195,
+                    "angularVelocity": 0.000001666416059709123,
+                    "velocityX": 2.9995912607524815,
+                    "velocityY": 2.305012831765639,
+                    "timestamp": 0.6420863690970183
+                },
+                {
+                    "x": 3.3964512995777554,
+                    "y": 5.676315774809768,
+                    "heading": -0.568384112513365,
+                    "angularVelocity": 0.0000016664161452516387,
+                    "velocityX": 2.999591671588281,
+                    "velocityY": 2.30501229713074,
+                    "timestamp": 0.6741906875518693
+                },
+                {
+                    "x": 3.492751156794414,
+                    "y": 5.750316609379858,
+                    "heading": -0.5683840590142086,
+                    "angularVelocity": 0.000001666416198927138,
+                    "velocityX": 2.999592012896559,
+                    "velocityY": 2.3050118529742454,
+                    "timestamp": 0.7062950060067202
+                },
+                {
+                    "x": 3.5890510233460025,
+                    "y": 5.824317431802072,
+                    "heading": -0.5683840055150509,
+                    "angularVelocity": 0.0000016664162437925357,
+                    "velocityX": 2.999592303665221,
+                    "velocityY": 2.305011474586604,
+                    "timestamp": 0.7383993244615712
+                },
+                {
+                    "x": 3.6853508979893648,
+                    "y": 5.898318243694168,
+                    "heading": -0.5683839520158918,
+                    "angularVelocity": 0.0000016664162889267752,
+                    "velocityX": 2.9995925557115117,
+                    "velocityY": 2.3050111465896648,
+                    "timestamp": 0.7705036429164221
+                },
+                {
+                    "x": 3.7816507797340138,
+                    "y": 5.972319046345101,
+                    "heading": -0.5683838985167311,
+                    "angularVelocity": 0.0000016664163350532929,
+                    "velocityX": 2.9995927769056334,
+                    "velocityY": 2.305010858741686,
+                    "timestamp": 0.8026079613712731
+                },
+                {
+                    "x": 3.877950667767949,
+                    "y": 6.046319840811553,
+                    "heading": -0.5683838450175687,
+                    "angularVelocity": 0.0000016664163859759036,
+                    "velocityX": 2.999592972807203,
+                    "velocityY": 2.305010603807763,
+                    "timestamp": 0.8347122798261241
+                },
+                {
+                    "x": 3.9742505615842325,
+                    "y": 6.120320627753221,
+                    "heading": -0.5683837915184047,
+                    "angularVelocity": 0.0000016664164376020863,
+                    "velocityX": 2.999593152918432,
+                    "velocityY": 2.3050103694223405,
+                    "timestamp": 0.866816598280975
+                },
+                {
+                    "x": 4.070552264919673,
+                    "y": 6.194319059842733,
+                    "heading": -0.5683837380195041,
+                    "angularVelocity": 0.0000016664082331173329,
+                    "velocityX": 2.999649516649054,
+                    "velocityY": 2.304937019409932,
+                    "timestamp": 0.898920916735826
+                },
+                {
+                    "x": 4.171102213813095,
+                    "y": 6.262432889296395,
+                    "heading": -0.5683836849080734,
+                    "angularVelocity": 0.0000016543391427080076,
+                    "velocityX": 3.131975812999348,
+                    "velocityY": 2.1216407241116846,
+                    "timestamp": 0.9310252351906769
                 },
                 {
                     "x": 4.276216983795166,
                     "y": 6.32326602935791,
-                    "heading": 2.5801557537673054,
-                    "angularVelocity": 0.0000014448196748631945,
-                    "velocityX": 3.2706194478654775,
-                    "velocityY": 1.9009685641363763,
-                    "timestamp": 0.9608298390613336
-                },
-                {
-                    "x": 4.413687737524023,
-                    "y": 6.387653258704535,
-                    "heading": 2.580155801751457,
-                    "angularVelocity": 0.0000011957738462706154,
-                    "velocityX": 3.425796355950964,
-                    "velocityY": 1.604541545618551,
-                    "timestamp": 1.0009579551094623
-                },
-                {
-                    "x": 4.556309721411339,
-                    "y": 6.439642616989784,
-                    "heading": 2.580155847910912,
-                    "angularVelocity": 0.0000011503020593677978,
-                    "velocityX": 3.55416595476994,
-                    "velocityY": 1.2955843285264883,
-                    "timestamp": 1.041086071157591
-                },
-                {
-                    "x": 4.700491299069936,
-                    "y": 6.48713573606665,
-                    "heading": 2.580155893776208,
-                    "angularVelocity": 0.000001142971587913107,
-                    "velocityX": 3.593031317136076,
-                    "velocityY": 1.1835372241224518,
-                    "timestamp": 1.08121418720572
-                },
-                {
-                    "x": 4.844672963256385,
-                    "y": 6.5346285924578735,
-                    "heading": 2.5801559396414997,
-                    "angularVelocity": 0.0000011429714605909094,
-                    "velocityX": 3.5930334734259763,
-                    "velocityY": 1.1835306779481511,
-                    "timestamp": 1.1213423032538488
-                },
-                {
-                    "x": 4.988854630036052,
-                    "y": 6.58212144097644,
-                    "heading": 2.5801559855067953,
-                    "angularVelocity": 0.0000011429715641749761,
-                    "velocityX": 3.5930335380494514,
-                    "velocityY": 1.18353048176008,
-                    "timestamp": 1.1614704193019776
-                },
-                {
-                    "x": 5.133036299227264,
-                    "y": 6.62961428217388,
-                    "heading": 2.580156031372096,
-                    "angularVelocity": 0.0000011429716937026712,
-                    "velocityX": 3.593033598145609,
-                    "velocityY": 1.1835302993162664,
-                    "timestamp": 1.2015985353501064
-                },
-                {
-                    "x": 5.277217970668299,
-                    "y": 6.677107116541164,
-                    "heading": 2.580156077237403,
-                    "angularVelocity": 0.0000011429718454732932,
-                    "velocityX": 3.593033654211569,
-                    "velocityY": 1.183530129107538,
-                    "timestamp": 1.2417266513982352
-                },
-                {
-                    "x": 5.421399644212107,
-                    "y": 6.724599944524706,
-                    "heading": 2.5801561231027166,
-                    "angularVelocity": 0.0000011429720157589239,
-                    "velocityX": 3.593033706613086,
-                    "velocityY": 1.183529970023511,
-                    "timestamp": 1.281854767446364
-                },
-                {
-                    "x": 5.565581319724973,
-                    "y": 6.772092766530448,
-                    "heading": 2.580156168968038,
-                    "angularVelocity": 0.0000011429722040135848,
-                    "velocityX": 3.593033755682377,
-                    "velocityY": 1.1835298210556366,
-                    "timestamp": 1.3219828834944929
-                },
-                {
-                    "x": 5.709762997085268,
-                    "y": 6.819585582927637,
-                    "heading": 2.5801562148333677,
-                    "angularVelocity": 0.0000011429724102352075,
-                    "velocityX": 3.5930338017206545,
-                    "velocityY": 1.1835296812894733,
-                    "timestamp": 1.3621109995426217
-                },
-                {
-                    "x": 5.853944676177738,
-                    "y": 6.867078394066171,
-                    "heading": 2.580156260698706,
-                    "angularVelocity": 0.0000011429726261958928,
-                    "velocityX": 3.5930338448867536,
-                    "velocityY": 1.183529550242829,
-                    "timestamp": 1.4022391155907505
-                },
-                {
-                    "x": 5.998126356881069,
-                    "y": 6.91457120031434,
-                    "heading": 2.5801563065640525,
-                    "angularVelocity": 0.0000011429728355759127,
-                    "velocityX": 3.593033885029711,
-                    "velocityY": 1.183529428374024,
-                    "timestamp": 1.4423672316388794
-                },
-                {
-                    "x": 6.142308039084736,
-                    "y": 6.962064002007683,
-                    "heading": 2.580156352429407,
-                    "angularVelocity": 0.0000011429730341793388,
-                    "velocityX": 3.5930339224183525,
-                    "velocityY": 1.1835293148669426,
-                    "timestamp": 1.4824953476870082
-                },
-                {
-                    "x": 6.286489722687973,
-                    "y": 7.009556799452112,
-                    "heading": 2.5801563982947693,
-                    "angularVelocity": 0.0000011429732271830896,
-                    "velocityX": 3.5930339572958836,
-                    "velocityY": 1.1835292089832443,
-                    "timestamp": 1.522623463735137
-                },
-                {
-                    "x": 6.430671407598782,
-                    "y": 7.057049592926921,
-                    "heading": 2.580156444160139,
-                    "angularVelocity": 0.000001142973413162616,
-                    "velocityX": 3.5930339898808183,
-                    "velocityY": 1.183529110059557,
-                    "timestamp": 1.5627515797832658
-                },
-                {
-                    "x": 6.574853093733082,
-                    "y": 7.104542382687364,
-                    "heading": 2.580156490025516,
-                    "angularVelocity": 0.0000011429735908015805,
-                    "velocityX": 3.593034020370455,
-                    "velocityY": 1.1835290174969095,
-                    "timestamp": 1.6028796958313947
-                },
-                {
-                    "x": 6.719034781004445,
-                    "y": 7.15203516899583,
-                    "heading": 2.5801565358908998,
-                    "angularVelocity": 0.0000011429737623342271,
-                    "velocityX": 3.593034048706264,
-                    "velocityY": 1.1835289314729884,
-                    "timestamp": 1.6430078118795235
-                },
-                {
-                    "x": 6.863216267597302,
-                    "y": 7.199528564522877,
-                    "heading": 2.580156581764359,
-                    "angularVelocity": 0.0000011431750045992354,
-                    "velocityX": 3.593029047761165,
-                    "velocityY": 1.1835441133115774,
-                    "timestamp": 1.6831359279276523
-                },
-                {
-                    "x": 6.997116821056572,
-                    "y": 7.244893026525659,
-                    "heading": 2.605743326369695,
-                    "angularVelocity": 0.6376263608948962,
-                    "velocityX": 3.3368263114738097,
-                    "velocityY": 1.1304907000461517,
-                    "timestamp": 1.7232640439757811
-                },
-                {
-                    "x": 7.1211832753718545,
-                    "y": 7.2851403173258245,
-                    "heading": 2.65804915297094,
-                    "angularVelocity": 1.3034707769113922,
-                    "velocityX": 3.091758760029488,
-                    "velocityY": 1.0029698566435126,
-                    "timestamp": 1.76339216002391
-                },
-                {
-                    "x": 7.233669414925018,
-                    "y": 7.321896904745842,
-                    "heading": 2.730695495833792,
-                    "angularVelocity": 1.8103601668147513,
-                    "velocityX": 2.8031751956221984,
-                    "velocityY": 0.9159808892082741,
-                    "timestamp": 1.8035202760720388
-                },
-                {
-                    "x": 7.333463334152891,
-                    "y": 7.354467297173912,
-                    "heading": 2.807930546248393,
-                    "angularVelocity": 1.9247115992678774,
-                    "velocityX": 2.486882740973504,
-                    "velocityY": 0.811660143451645,
-                    "timestamp": 1.8436483921201676
-                },
-                {
-                    "x": 7.42052379329864,
-                    "y": 7.382839226266775,
-                    "heading": 2.882629127741458,
-                    "angularVelocity": 1.8615023292763984,
-                    "velocityX": 2.169562584032867,
-                    "velocityY": 0.7070336683345496,
-                    "timestamp": 1.8837765081682964
-                },
-                {
-                    "x": 7.494937832171454,
-                    "y": 7.407046736601957,
-                    "heading": 2.951143873381968,
-                    "angularVelocity": 1.7074000074744335,
-                    "velocityX": 1.8544114750755487,
-                    "velocityY": 0.6032555903234524,
-                    "timestamp": 1.9239046242164253
-                },
-                {
-                    "x": 7.556793519582923,
-                    "y": 7.427131383067914,
-                    "heading": 3.011211531798935,
-                    "angularVelocity": 1.4968970470710155,
-                    "velocityX": 1.5414550570298573,
-                    "velocityY": 0.5005130677420416,
-                    "timestamp": 1.964032740264554
-                },
-                {
-                    "x": 7.606167226555021,
-                    "y": 7.443134327538265,
-                    "heading": 3.0612259719014916,
-                    "angularVelocity": 1.2463690057756673,
-                    "velocityX": 1.2304018188364594,
-                    "velocityY": 0.3987963065885285,
-                    "timestamp": 2.0041608563126827
-                },
-                {
-                    "x": 7.643124291158215,
-                    "y": 7.455092771789357,
-                    "heading": 3.0999636114018587,
-                    "angularVelocity": 0.9653490698119509,
-                    "velocityX": 0.9209768173235189,
-                    "velocityY": 0.2980066205139035,
-                    "timestamp": 2.0442889723608113
-                },
-                {
-                    "x": 7.667720023098554,
-                    "y": 7.46303884043931,
-                    "heading": 3.1264689002845456,
-                    "angularVelocity": 0.6605166524861815,
-                    "velocityX": 0.6129301438133489,
-                    "velocityY": 0.1980174858052883,
-                    "timestamp": 2.08441708840894
+                    "heading": -0.5683836309724583,
+                    "angularVelocity": 0.0000016800112198234746,
+                    "velocityX": 3.274162948822482,
+                    "velocityY": 1.8948584797732355,
+                    "timestamp": 0.9631295536455279
+                },
+                {
+                    "x": 4.413834682670106,
+                    "y": 6.38740625436239,
+                    "heading": -0.5683835821873748,
+                    "angularVelocity": 0.0000012155044166425132,
+                    "velocityX": 3.428812837046335,
+                    "velocityY": 1.5980853383274327,
+                    "timestamp": 1.0032652231659112
+                },
+                {
+                    "x": 4.5565820466376845,
+                    "y": 6.439134291384663,
+                    "heading": -0.5683835352510233,
+                    "angularVelocity": 0.0000011694423477450916,
+                    "velocityX": 3.556620972650851,
+                    "velocityY": 1.2888295533727718,
+                    "timestamp": 1.0434008926862943
+                },
+                {
+                    "x": 4.7007810920363475,
+                    "y": 6.4866657027970485,
+                    "heading": -0.5683834885999294,
+                    "angularVelocity": 0.000001162335011018839,
+                    "velocityX": 3.5927903314389917,
+                    "velocityY": 1.184268556632563,
+                    "timestamp": 1.0835365622066775
+                },
+                {
+                    "x": 4.84498021653845,
+                    "y": 6.534196874227661,
+                    "heading": -0.5683834419488406,
+                    "angularVelocity": 0.0000011623348856187121,
+                    "velocityX": 3.5927923023401758,
+                    "velocityY": 1.1842625773683353,
+                    "timestamp": 1.1236722317270607
+                },
+                {
+                    "x": 4.989179343628637,
+                    "y": 6.581728037806586,
+                    "heading": -0.5683833952977476,
+                    "angularVelocity": 0.0000011623349868626642,
+                    "velocityX": 3.5927923668235673,
+                    "velocityY": 1.1842623817396558,
+                    "timestamp": 1.1638079012474438
+                },
+                {
+                    "x": 5.133378473125504,
+                    "y": 6.629259194084157,
+                    "heading": -0.5683833486466496,
+                    "angularVelocity": 0.0000011623351115999292,
+                    "velocityX": 3.592792426787221,
+                    "velocityY": 1.1842621998228242,
+                    "timestamp": 1.203943570767827
+                },
+                {
+                    "x": 5.277577604867278,
+                    "y": 6.676790343551162,
+                    "heading": -0.5683833019955459,
+                    "angularVelocity": 0.0000011623352597981392,
+                    "velocityX": 3.5927924827201663,
+                    "velocityY": 1.184262030134241,
+                    "timestamp": 1.2440792402882102
+                },
+                {
+                    "x": 5.421776738706852,
+                    "y": 6.7243214866538885,
+                    "heading": -0.5683832553444353,
+                    "angularVelocity": 0.0000011623354265532326,
+                    "velocityX": 3.5927925349879053,
+                    "velocityY": 1.1842618715650601,
+                    "timestamp": 1.2842149098085933
+                },
+                {
+                    "x": 5.565975874510482,
+                    "y": 6.771852623798087,
+                    "heading": -0.5683832086933173,
+                    "angularVelocity": 0.0000011623356113966992,
+                    "velocityX": 3.5927925839233144,
+                    "velocityY": 1.18426172310541,
+                    "timestamp": 1.3243505793289765
+                },
+                {
+                    "x": 5.710175012156514,
+                    "y": 6.819383755352828,
+                    "heading": -0.5683831620421912,
+                    "angularVelocity": 0.0000011623358144790056,
+                    "velocityX": 3.5927926298276605,
+                    "velocityY": 1.1842615838413222,
+                    "timestamp": 1.3644862488493597
+                },
+                {
+                    "x": 5.854374151534237,
+                    "y": 6.866914881653982,
+                    "heading": -0.5683831153910563,
+                    "angularVelocity": 0.000001162336033125057,
+                    "velocityX": 3.592792672973599,
+                    "velocityY": 1.1842614529456208,
+                    "timestamp": 1.4046219183697428
+                },
+                {
+                    "x": 5.998573292523273,
+                    "y": 6.9144460030667485,
+                    "heading": -0.568383068739913,
+                    "angularVelocity": 0.000001162336242705986,
+                    "velocityX": 3.59279271312027,
+                    "velocityY": 1.184261331149038,
+                    "timestamp": 1.444757587890126
+                },
+                {
+                    "x": 6.142772435013044,
+                    "y": 6.9619771199265985,
+                    "heading": -0.5683830220887615,
+                    "angularVelocity": 0.0000011623364467362213,
+                    "velocityX": 3.5927927505118338,
+                    "velocityY": 1.1842612177108516,
+                    "timestamp": 1.4848932574105091
+                },
+                {
+                    "x": 6.286971578902684,
+                    "y": 7.00950823253954,
+                    "heading": -0.5683829754376022,
+                    "angularVelocity": 0.0000011623366413601916,
+                    "velocityX": 3.59279278539024,
+                    "velocityY": 1.1842611118970372,
+                    "timestamp": 1.5250289269308923
+                },
+                {
+                    "x": 6.431170724100105,
+                    "y": 7.057039341184945,
+                    "heading": -0.5683829287864354,
+                    "angularVelocity": 0.0000011623368281432719,
+                    "velocityX": 3.592792817974252,
+                    "velocityY": 1.1842610130439195,
+                    "timestamp": 1.5651645964512755
+                },
+                {
+                    "x": 6.575369870521147,
+                    "y": 7.10457044611814,
+                    "heading": -0.5683828821352613,
+                    "angularVelocity": 0.0000011623370098744323,
+                    "velocityX": 3.5927928484613587,
+                    "velocityY": 1.1842609205523604,
+                    "timestamp": 1.6053002659716586
+                },
+                {
+                    "x": 6.719569018075523,
+                    "y": 7.152101547613029,
+                    "heading": -0.5683828354840802,
+                    "angularVelocity": 0.0000011623371832362076,
+                    "velocityX": 3.5927928766989634,
+                    "velocityY": 1.1842608348853054,
+                    "timestamp": 1.6454359354920418
+                },
+                {
+                    "x": 6.8637678827817385,
+                    "y": 7.1996335071853474,
+                    "heading": -0.5683827888209949,
+                    "angularVelocity": 0.0000011626337817909648,
+                    "velocityX": 3.592785829397527,
+                    "velocityY": 1.1842822143076228,
+                    "timestamp": 1.685571605012425
+                },
+                {
+                    "x": 6.99756534811195,
+                    "y": 7.244965845733408,
+                    "heading": -0.5424482126349314,
+                    "angularVelocity": 0.6461727559544621,
+                    "velocityX": 3.3336298342366253,
+                    "velocityY": 1.1294775716906411,
+                    "timestamp": 1.7257072745328081
+                },
+                {
+                    "x": 7.121516587135396,
+                    "y": 7.285176196921745,
+                    "heading": -0.489772109644447,
+                    "angularVelocity": 1.3124510845329824,
+                    "velocityX": 3.088306249893159,
+                    "velocityY": 1.0018607305881775,
+                    "timestamp": 1.7658429440531913
+                },
+                {
+                    "x": 7.233956124618328,
+                    "y": 7.321947450112657,
+                    "heading": -0.4161805837381191,
+                    "angularVelocity": 1.8335691614401448,
+                    "velocityX": 2.8014865287304875,
+                    "velocityY": 0.9161739079059802,
+                    "timestamp": 1.8059786135735745
+                },
+                {
+                    "x": 7.333707629732149,
+                    "y": 7.35452494917019,
+                    "heading": -0.3377597360881954,
+                    "angularVelocity": 1.953894094381483,
+                    "velocityX": 2.4853579448365832,
+                    "velocityY": 0.8116844554190893,
+                    "timestamp": 1.8461142830939576
+                },
+                {
+                    "x": 7.420722566948463,
+                    "y": 7.382895867936689,
+                    "heading": -0.26184604843223563,
+                    "angularVelocity": 1.8914269666638126,
+                    "velocityX": 2.1680200743162716,
+                    "velocityY": 0.7068754328887092,
+                    "timestamp": 1.8862499526143408
+                },
+                {
+                    "x": 7.495090208476241,
+                    "y": 7.40709623575076,
+                    "heading": -0.19218016370533045,
+                    "angularVelocity": 1.7357598754276362,
+                    "velocityX": 1.8529064649092093,
+                    "velocityY": 0.6029640990984653,
+                    "timestamp": 1.926385622134724
+                },
+                {
+                    "x": 7.556901497509452,
+                    "y": 7.427169942598205,
+                    "heading": -0.13107853540878284,
+                    "angularVelocity": 1.522377202790068,
+                    "velocityX": 1.5400587500308252,
+                    "velocityY": 0.500146305949861,
+                    "timestamp": 1.9665212916551071
+                },
+                {
+                    "x": 7.606235479245265,
+                    "y": 7.443160506733181,
+                    "heading": -0.08018624320935208,
+                    "angularVelocity": 1.2680065589434022,
+                    "velocityX": 1.2291804852229526,
+                    "velocityY": 0.3984127915657456,
+                    "timestamp": 2.0066569611754903
+                },
+                {
+                    "x": 7.643159935464096,
+                    "y": 7.455107253348189,
+                    "heading": -0.040758426269481916,
+                    "angularVelocity": 0.982363503861482,
+                    "velocityX": 0.9199910369024534,
+                    "velocityY": 0.29765908374694494,
+                    "timestamp": 2.0467926306958737
+                },
+                {
+                    "x": 7.667732329164437,
+                    "y": 7.4630440972663505,
+                    "heading": -0.013776014916518935,
+                    "angularVelocity": 0.6722800859036306,
+                    "velocityX": 0.6122333075286244,
+                    "velocityY": 0.1977503804721968,
+                    "timestamp": 2.086928300216257
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
-                    "angularVelocity": 0.3371974826634153,
-                    "velocityX": 0.30601927303833204,
-                    "velocityY": 0.0987128216021636,
-                    "timestamp": 2.1245452044570685
+                    "heading": 2.662292212835229e-35,
+                    "angularVelocity": 0.3432362056280802,
+                    "velocityX": 0.30565506897381545,
+                    "velocityY": 0.09856326756028964,
+                    "timestamp": 2.1270639697366405
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
-                    "angularVelocity": -8.758785880018889e-35,
-                    "velocityX": 1.2314161274227302e-35,
-                    "velocityY": -4.94083755736297e-36,
-                    "timestamp": 2.164673320505197
+                    "heading": -1.5591265591207004e-41,
+                    "angularVelocity": 3.563315968840566e-34,
+                    "velocityX": -7.620676928265867e-35,
+                    "velocityY": -1.1745137620932586e-33,
+                    "timestamp": 2.167199639257024
                 }
             ],
             "constraints": [
@@ -19627,16 +18053,16 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 29
+                    "controlIntervalCount": 33
                 },
                 {
                     "x": 5.493278503417969,
                     "y": 6.720412731170654,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -19645,7 +18071,7 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -19656,515 +18082,551 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "angularVelocity": 0,
                     "velocityX": 0,
-                    "velocityY": 3.853182471863691e-34,
+                    "velocityY": -3.138879459525789e-33,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.30610760298278,
-                    "y": 4.37532703674674,
-                    "heading": 2.7433864075571766,
-                    "angularVelocity": -0.14821621401149343,
-                    "velocityX": 0.13687656120621225,
-                    "velocityY": 0.3434918886001163,
-                    "timestamp": 0.04462124799861899
-                },
-                {
-                    "x": 2.318848696543319,
-                    "y": 4.405778929719923,
-                    "heading": 2.7307174870785778,
-                    "angularVelocity": -0.28392124933375007,
-                    "velocityX": 0.28553870929233327,
-                    "velocityY": 0.6824527403206025,
-                    "timestamp": 0.08924249599723798
-                },
-                {
-                    "x": 2.3388726580644836,
-                    "y": 4.451074836672454,
-                    "heading": 2.7126983162955036,
-                    "angularVelocity": -0.4038248948938342,
-                    "velocityX": 0.44875395510641974,
-                    "velocityY": 1.0151196791701815,
-                    "timestamp": 0.13386374399585696
-                },
-                {
-                    "x": 2.3669952727104597,
-                    "y": 4.510809884172449,
-                    "heading": 2.6902411170995073,
-                    "angularVelocity": -0.5032848744322687,
-                    "velocityX": 0.6302516381174824,
-                    "velocityY": 1.3387130611372482,
-                    "timestamp": 0.17848499199447596
-                },
-                {
-                    "x": 2.404258116570724,
-                    "y": 4.5843745316547775,
-                    "heading": 2.664559931990299,
-                    "angularVelocity": -0.5755371322201343,
-                    "velocityX": 0.8350919243992831,
-                    "velocityY": 1.648646122237679,
-                    "timestamp": 0.22310623999309495
-                },
-                {
-                    "x": 2.452003610186855,
-                    "y": 4.670805411588522,
-                    "heading": 2.637322930497274,
-                    "angularVelocity": -0.6104042964882519,
-                    "velocityX": 1.070016993195909,
-                    "velocityY": 1.9369893001742093,
-                    "timestamp": 0.2677274879917139
-                },
-                {
-                    "x": 2.5115645242184845,
-                    "y": 4.768897077167795,
-                    "heading": 2.6107796946069493,
-                    "angularVelocity": -0.5948564211190585,
-                    "velocityX": 1.3348105824712122,
-                    "velocityY": 2.1983173931466227,
-                    "timestamp": 0.3123487359903329
-                },
-                {
-                    "x": 2.5833793756558516,
-                    "y": 4.878178047966119,
-                    "heading": 2.586043040896354,
-                    "angularVelocity": -0.5543693827514722,
-                    "velocityX": 1.60943170929665,
-                    "velocityY": 2.449079210014641,
-                    "timestamp": 0.3569699839889519
-                },
-                {
-                    "x": 2.6682042395216055,
-                    "y": 4.997714430694696,
-                    "heading": 2.5651891379462763,
-                    "angularVelocity": -0.46735364619839126,
-                    "velocityX": 1.9009971184216836,
-                    "velocityY": 2.678911686474495,
-                    "timestamp": 0.4015912319875709
-                },
-                {
-                    "x": 2.76752425780781,
-                    "y": 5.124957228979585,
-                    "heading": 2.5531720535122697,
-                    "angularVelocity": -0.269313050911911,
-                    "velocityX": 2.2258458187740118,
-                    "velocityY": 2.8516189930148728,
-                    "timestamp": 0.4462124799861899
-                },
-                {
-                    "x": 2.882656696791853,
-                    "y": 5.248398665617137,
-                    "heading": 2.553172127817037,
-                    "angularVelocity": 0.0000016652328226055909,
-                    "velocityX": 2.580215573253489,
-                    "velocityY": 2.76642725549436,
-                    "timestamp": 0.4908337279848089
-                },
-                {
-                    "x": 3.0093581189008844,
-                    "y": 5.359933509101588,
-                    "heading": 2.5531721828704046,
-                    "angularVelocity": 0.000001233792641064896,
-                    "velocityX": 2.8394862939053795,
-                    "velocityY": 2.4995904078680997,
-                    "timestamp": 0.5354549759834278
-                },
-                {
-                    "x": 3.146402398989035,
-                    "y": 5.458483685796272,
-                    "heading": 2.553172234700022,
-                    "angularVelocity": 0.0000011615456718217746,
-                    "velocityX": 3.071278510461917,
-                    "velocityY": 2.20859301599395,
-                    "timestamp": 0.5800762239820468
-                },
-                {
-                    "x": 3.286524028997536,
-                    "y": 5.552607013873938,
-                    "heading": 2.5531722858926313,
-                    "angularVelocity": 0.0000011472697806901915,
-                    "velocityX": 3.140244531323671,
-                    "velocityY": 2.1093835851605776,
-                    "timestamp": 0.6246974719806658
-                },
-                {
-                    "x": 3.426645803515369,
-                    "y": 5.646730126819845,
-                    "heading": 2.5531723370852246,
-                    "angularVelocity": 0.000001147269414679425,
-                    "velocityX": 3.140247769900325,
-                    "velocityY": 2.109378763875461,
-                    "timestamp": 0.6693187199792848
-                },
-                {
-                    "x": 3.5667675932030476,
-                    "y": 5.740853217182279,
-                    "heading": 2.5531723882778183,
-                    "angularVelocity": 0.0000011472694285852317,
-                    "velocityX": 3.1402481098694217,
-                    "velocityY": 2.109378257760682,
-                    "timestamp": 0.7139399679779038
-                },
-                {
-                    "x": 3.70688939501189,
-                    "y": 5.834976289499829,
-                    "heading": 2.553172439470412,
-                    "angularVelocity": 0.0000011472694368681445,
-                    "velocityX": 3.1402483815149935,
-                    "velocityY": 2.109377853359542,
-                    "timestamp": 0.7585612159765228
-                },
-                {
-                    "x": 3.8470112068261555,
-                    "y": 5.929099346922214,
-                    "heading": 2.553172490663007,
-                    "angularVelocity": 0.0000011472694500175128,
-                    "velocityX": 3.140248605744995,
-                    "velocityY": 2.1093775195462685,
-                    "timestamp": 0.8031824639751418
-                },
-                {
-                    "x": 3.9871330270690666,
-                    "y": 6.023222391796794,
-                    "heading": 2.5531725418556026,
-                    "angularVelocity": 0.000001147269470343035,
-                    "velocityX": 3.1402487946380906,
-                    "velocityY": 2.109377238339292,
-                    "timestamp": 0.8478037119737608
-                },
-                {
-                    "x": 4.127254854511192,
-                    "y": 6.117345425953829,
-                    "heading": 2.553172593048199,
-                    "angularVelocity": 0.000001147269495912955,
-                    "velocityX": 3.1402489559785947,
-                    "velocityY": 2.109376998150064,
-                    "timestamp": 0.8924249599723798
-                },
-                {
-                    "x": 4.267376688165873,
-                    "y": 6.21146845086217,
-                    "heading": 2.5531726442407967,
-                    "angularVelocity": 0.0000011472695239009516,
-                    "velocityX": 3.1402490952072233,
-                    "velocityY": 2.1093767908789607,
-                    "timestamp": 0.9370462079709988
-                },
-                {
-                    "x": 4.407498527228544,
-                    "y": 6.305591467719579,
-                    "heading": 2.553172695433396,
-                    "angularVelocity": 0.0000011472695542073644,
-                    "velocityX": 3.140249216404848,
-                    "velocityY": 2.1093766104507625,
-                    "timestamp": 0.9816674559696178
-                },
-                {
-                    "x": 4.547620436764314,
-                    "y": 6.399714379662917,
-                    "heading": 2.5531727466259886,
-                    "angularVelocity": 0.0000011472694062933976,
-                    "velocityX": 3.1402507957667765,
-                    "velocityY": 2.1093742592374776,
-                    "timestamp": 1.0262887039682367
-                },
-                {
-                    "x": 4.689466118908105,
-                    "y": 6.4912188851781,
-                    "heading": 2.553172797709037,
-                    "angularVelocity": 0.0000011448144250292167,
-                    "velocityX": 3.1788820014218735,
-                    "velocityY": 2.050693551153348,
-                    "timestamp": 1.0709099519668557
-                },
-                {
-                    "x": 4.839613537732198,
-                    "y": 6.568348804669705,
-                    "heading": 2.5531728491363963,
-                    "angularVelocity": 0.0000011525307241082326,
-                    "velocityX": 3.364930959096914,
-                    "velocityY": 1.7285468907994308,
-                    "timestamp": 1.1155311999654747
-                },
-                {
-                    "x": 4.996611329828,
-                    "y": 6.630356582585623,
-                    "heading": 2.5531729033694757,
-                    "angularVelocity": 0.0000012154092882901391,
-                    "velocityX": 3.5184536322395523,
-                    "velocityY": 1.3896468767040242,
-                    "timestamp": 1.1601524479640937
-                },
-                {
-                    "x": 5.158940949366324,
-                    "y": 6.676642268737186,
-                    "heading": 2.553172963573569,
-                    "angularVelocity": 0.0000013492247785707482,
-                    "velocityX": 3.637944405843334,
-                    "velocityY": 1.037301470209733,
-                    "timestamp": 1.2047736959627127
-                },
-                {
-                    "x": 5.325032205172685,
-                    "y": 6.706758095236814,
-                    "heading": 2.5531730345823234,
-                    "angularVelocity": 0.0000015913663950616823,
-                    "velocityX": 3.7222458639324056,
-                    "velocityY": 0.6749212057125495,
-                    "timestamp": 1.2493949439613317
+                    "x": 2.304641315068496,
+                    "y": 4.371901353899003,
+                    "heading": -0.39554860460981267,
+                    "angularVelocity": -0.1412451028521288,
+                    "velocityX": 0.11814916908296796,
+                    "velocityY": 0.3029604871416256,
+                    "timestamp": 0.039283518492098594
+                },
+                {
+                    "x": 2.314310378834228,
+                    "y": 4.395560050627161,
+                    "heading": -0.4062436571914542,
+                    "angularVelocity": -0.27225291909092403,
+                    "velocityX": 0.24613538035494495,
+                    "velocityY": 0.6022550330596644,
+                    "timestamp": 0.07856703698419719
+                },
+                {
+                    "x": 2.3294686593461313,
+                    "y": 4.43078385259534,
+                    "heading": -0.42159423390969814,
+                    "angularVelocity": -0.3907637937605655,
+                    "velocityX": 0.385868707635076,
+                    "velocityY": 0.8966559850097278,
+                    "timestamp": 0.11785055547629578
+                },
+                {
+                    "x": 2.3506733572393026,
+                    "y": 4.477309554936099,
+                    "heading": -0.44099148005781696,
+                    "angularVelocity": -0.49377568233887975,
+                    "velocityX": 0.5397861165984255,
+                    "velocityY": 1.1843568022074944,
+                    "timestamp": 0.15713407396839438
+                },
+                {
+                    "x": 2.3786040121360466,
+                    "y": 4.534766426387325,
+                    "heading": -0.4636665715847995,
+                    "angularVelocity": -0.5772164102747018,
+                    "velocityX": 0.7110018646207296,
+                    "velocityY": 1.462620296162777,
+                    "timestamp": 0.19641759246049298
+                },
+                {
+                    "x": 2.4140943920114943,
+                    "y": 4.602616046670193,
+                    "heading": -0.48862953085625704,
+                    "angularVelocity": -0.6354562989682879,
+                    "velocityX": 0.903441983756771,
+                    "velocityY": 1.727177780588029,
+                    "timestamp": 0.23570111095259158
+                },
+                {
+                    "x": 2.458163120855333,
+                    "y": 4.68005234098581,
+                    "heading": -0.5145799632934547,
+                    "angularVelocity": -0.6605933845360225,
+                    "velocityX": 1.1218121628463973,
+                    "velocityY": 1.9712158505149888,
+                    "timestamp": 0.27498462944469015
+                },
+                {
+                    "x": 2.5115097740388146,
+                    "y": 4.766403162212065,
+                    "heading": -0.5400224328341426,
+                    "angularVelocity": -0.6476626971635644,
+                    "velocityX": 1.3579907103842537,
+                    "velocityY": 2.1981437646332993,
+                    "timestamp": 0.31426814793678876
+                },
+                {
+                    "x": 2.5744195202560527,
+                    "y": 4.861351309968927,
+                    "heading": -0.563860333505168,
+                    "angularVelocity": -0.6068168429413633,
+                    "velocityX": 1.6014285031491418,
+                    "velocityY": 2.416997035944537,
+                    "timestamp": 0.35355166642888736
+                },
+                {
+                    "x": 2.6473242968990576,
+                    "y": 4.964346697364103,
+                    "heading": -0.5842892856317753,
+                    "angularVelocity": -0.5200387569839264,
+                    "velocityX": 1.8558616804566808,
+                    "velocityY": 2.6218473127835553,
+                    "timestamp": 0.39283518492098596
+                },
+                {
+                    "x": 2.7309171688764238,
+                    "y": 5.07424676618957,
+                    "heading": -0.5978668700067779,
+                    "angularVelocity": -0.3456305569399124,
+                    "velocityX": 2.127937496081736,
+                    "velocityY": 2.7976126641403014,
+                    "timestamp": 0.43211870341308456
+                },
+                {
+                    "x": 2.826173576921219,
+                    "y": 5.187875652325513,
+                    "heading": -0.5974605263008922,
+                    "angularVelocity": 0.010343872481393205,
+                    "velocityX": 2.4248441000503926,
+                    "velocityY": 2.8925333192550147,
+                    "timestamp": 0.47140222190518316
+                },
+                {
+                    "x": 2.9311441629204515,
+                    "y": 5.293067244130248,
+                    "heading": -0.5974604636085413,
+                    "angularVelocity": 0.000001595894492248037,
+                    "velocityX": 2.672127905761066,
+                    "velocityY": 2.677753822532693,
+                    "timestamp": 0.5106857403972818
+                },
+                {
+                    "x": 3.044820699228354,
+                    "y": 5.388784226639372,
+                    "heading": -0.597460407679312,
+                    "angularVelocity": 0.0000014237326879146737,
+                    "velocityX": 2.8937463005192323,
+                    "velocityY": 2.436568468997834,
+                    "timestamp": 0.5499692588893803
+                },
+                {
+                    "x": 3.1663502143390825,
+                    "y": 5.474310087323753,
+                    "heading": -0.5974603544159754,
+                    "angularVelocity": 0.0000013558698005366095,
+                    "velocityX": 3.0936514796952492,
+                    "velocityY": 2.177143595261183,
+                    "timestamp": 0.5892527773814789
+                },
+                {
+                    "x": 3.289918343405979,
+                    "y": 5.556863181758907,
+                    "heading": -0.5974603016382944,
+                    "angularVelocity": 0.0000013435069733969365,
+                    "velocityX": 3.145546371864123,
+                    "velocityY": 2.1014689519673895,
+                    "timestamp": 0.6285362958735774
+                },
+                {
+                    "x": 3.413486607435696,
+                    "y": 5.639416074177005,
+                    "heading": -0.5974602488606314,
+                    "angularVelocity": 0.0000013435065140042136,
+                    "velocityX": 3.145549807473418,
+                    "velocityY": 2.101463809427605,
+                    "timestamp": 0.667819814365676
+                },
+                {
+                    "x": 3.5370548851900594,
+                    "y": 5.721968946051526,
+                    "heading": -0.5974601960829673,
+                    "angularVelocity": 0.0000013435065431603766,
+                    "velocityX": 3.145550156847558,
+                    "velocityY": 2.1014632864709872,
+                    "timestamp": 0.7071033328577745
+                },
+                {
+                    "x": 3.6606231741521,
+                    "y": 5.804521801149959,
+                    "heading": -0.5974601433053026,
+                    "angularVelocity": 0.0000013435065598734946,
+                    "velocityX": 3.1455504421498475,
+                    "velocityY": 2.1014628594194082,
+                    "timestamp": 0.746386851349873
+                },
+                {
+                    "x": 3.7841914725218415,
+                    "y": 5.887074642166574,
+                    "heading": -0.597460090527637,
+                    "angularVelocity": 0.0000013435065802330538,
+                    "velocityX": 3.1455506816319834,
+                    "velocityY": 2.101462500953099,
+                    "timestamp": 0.7856703698419716
+                },
+                {
+                    "x": 3.9077597789317933,
+                    "y": 5.969627471148282,
+                    "heading": -0.5974600377499704,
+                    "angularVelocity": 0.0000013435066065618314,
+                    "velocityX": 3.14555088630331,
+                    "velocityY": 2.1014621945928877,
+                    "timestamp": 0.8249538883340701
+                },
+                {
+                    "x": 4.031328092300017,
+                    "y": 6.052180289714568,
+                    "heading": -0.5974599849723026,
+                    "angularVelocity": 0.0000013435066364860128,
+                    "velocityX": 3.1455510634328734,
+                    "velocityY": 2.101461929458217,
+                    "timestamp": 0.8642374068261687
+                },
+                {
+                    "x": 4.154896411746533,
+                    "y": 6.134733099182619,
+                    "heading": -0.5974599321946334,
+                    "angularVelocity": 0.0000013435066718879358,
+                    "velocityX": 3.14555121816168,
+                    "velocityY": 2.1014616978538454,
+                    "timestamp": 0.9035209253182672
+                },
+                {
+                    "x": 4.27846473654201,
+                    "y": 6.217285900644126,
+                    "heading": -0.5974598794169629,
+                    "angularVelocity": 0.000001343506708988157,
+                    "velocityX": 3.14555135432466,
+                    "velocityY": 2.1014614940395124,
+                    "timestamp": 0.9428044438103658
+                },
+                {
+                    "x": 4.4020330660739395,
+                    "y": 6.299838695015915,
+                    "heading": -0.5974598266392908,
+                    "angularVelocity": 0.0000013435067467217265,
+                    "velocityX": 3.1455514748956417,
+                    "velocityY": 2.1014613135638847,
+                    "timestamp": 0.9820879623024643
+                },
+                {
+                    "x": 4.525601399950297,
+                    "y": 6.382391482884783,
+                    "heading": -0.5974597738616171,
+                    "angularVelocity": 0.0000013435067857429384,
+                    "velocityX": 3.1455515854872873,
+                    "velocityY": 2.101461148025713,
+                    "timestamp": 1.0213714807945629
+                },
+                {
+                    "x": 4.6491721527053596,
+                    "y": 6.464940649963802,
+                    "heading": -0.5974597210842042,
+                    "angularVelocity": 0.0000013435001464771913,
+                    "velocityX": 3.1456131603868194,
+                    "velocityY": 2.101368977314248,
+                    "timestamp": 1.0606549992866614
+                },
+                {
+                    "x": 4.778837800088318,
+                    "y": 6.537541722502267,
+                    "heading": -0.5974596685786843,
+                    "angularVelocity": 0.0000013365788503576918,
+                    "velocityX": 3.300764604603488,
+                    "velocityY": 1.8481305984081453,
+                    "timestamp": 1.09993851777876
+                },
+                {
+                    "x": 4.914297553492409,
+                    "y": 6.59865432938236,
+                    "heading": -0.5974596146272461,
+                    "angularVelocity": 0.0000013733860988547744,
+                    "velocityX": 3.4482591835906553,
+                    "velocityY": 1.5556805812161723,
+                    "timestamp": 1.1392220362708585
+                },
+                {
+                    "x": 5.054536029054771,
+                    "y": 6.647820017049671,
+                    "heading": -0.5974595570647036,
+                    "angularVelocity": 0.0000014653102566024163,
+                    "velocityX": 3.5699061831898264,
+                    "velocityY": 1.2515601848929776,
+                    "timestamp": 1.178505554762957
+                },
+                {
+                    "x": 5.198501873691458,
+                    "y": 6.684670110841039,
+                    "heading": -0.5974594930097576,
+                    "angularVelocity": 0.0000016305806703071774,
+                    "velocityX": 3.6647899720507366,
+                    "velocityY": 0.9380548180449542,
+                    "timestamp": 1.2177890732550556
+                },
+                {
+                    "x": 5.345115761176417,
+                    "y": 6.7089283098414425,
+                    "heading": -0.5974594180971905,
+                    "angularVelocity": 0.0000019069719319939646,
+                    "velocityX": 3.7321984667555648,
+                    "velocityY": 0.6175159438731035,
+                    "timestamp": 1.2570725917471541
                 },
                 {
                     "x": 5.493278503417969,
                     "y": 6.720412731170654,
-                    "heading": 2.553173120074835,
-                    "angularVelocity": 0.000001915959677785522,
-                    "velocityX": 3.7705421921522753,
-                    "velocityY": 0.30601196842950656,
-                    "timestamp": 1.2940161919599507
-                },
-                {
-                    "x": 5.617518941656642,
-                    "y": 6.721477956029719,
-                    "heading": 2.5531732061618477,
-                    "angularVelocity": 0.0000026211268995160273,
-                    "velocityX": 3.782800031375301,
-                    "velocityY": 0.03243334205370069,
-                    "timestamp": 1.32685970396381
-                },
-                {
-                    "x": 5.741510905915031,
-                    "y": 6.713552321996202,
-                    "heading": 2.5531732789431283,
-                    "angularVelocity": 0.0000022160017651615355,
-                    "velocityX": 3.7752346412837734,
-                    "velocityY": -0.24131505889453328,
-                    "timestamp": 1.3597032159676692
-                },
-                {
-                    "x": 5.8646046105180325,
-                    "y": 6.696677372566761,
-                    "heading": 2.553173343002795,
-                    "angularVelocity": 0.0000019504511878675683,
-                    "velocityX": 3.7478849578735156,
-                    "velocityY": -0.5137985678101988,
-                    "timestamp": 1.3925467279715285
-                },
-                {
-                    "x": 5.986154981292517,
-                    "y": 6.670941556957854,
-                    "heading": 2.5531734012532086,
-                    "angularVelocity": 0.0000017735744392757528,
-                    "velocityX": 3.7008944341945598,
-                    "velocityY": -0.7835890268336883,
-                    "timestamp": 1.4253902399753877
-                },
-                {
-                    "x": 6.105525040034572,
-                    "y": 6.636479774106562,
-                    "heading": 2.553173455691931,
-                    "angularVelocity": 0.000001657518306287931,
-                    "velocityX": 3.634509571571689,
-                    "velocityY": -1.0492721621013206,
-                    "timestamp": 1.458233751979247
-                },
-                {
-                    "x": 6.222089256538776,
-                    "y": 6.593472690949968,
-                    "heading": 2.5531735077894595,
-                    "angularVelocity": 0.0000015862350106976908,
-                    "velocityX": 3.549078931951851,
-                    "velocityY": -1.3094544563790795,
-                    "timestamp": 1.4910772639831062
-                },
-                {
-                    "x": 6.335236901536747,
-                    "y": 6.54214592976541,
-                    "heading": 2.553173558707783,
-                    "angularVelocity": 0.000001550331263346088,
-                    "velocityX": 3.4450531655894774,
-                    "velocityY": -1.5627671358204458,
-                    "timestamp": 1.5239207759869655
-                },
-                {
-                    "x": 6.444377333823394,
-                    "y": 6.482772601626653,
-                    "heading": 2.5531736094378257,
-                    "angularVelocity": 0.0000015445985995778538,
-                    "velocityX": 3.3230439020592755,
-                    "velocityY": -1.8077642893915407,
-                    "timestamp": 1.5567642879908248
-                },
-                {
-                    "x": 6.553087073787246,
-                    "y": 6.422614322949958,
-                    "heading": 2.5531736601729174,
-                    "angularVelocity": 0.0000015447523320156895,
-                    "velocityX": 3.3099304346952643,
-                    "velocityY": -1.8316640032170186,
-                    "timestamp": 1.589607799994684
-                },
-                {
-                    "x": 6.661796772216766,
-                    "y": 6.362455969218214,
-                    "heading": 2.553173710908019,
-                    "angularVelocity": 0.0000015447526253443646,
-                    "velocityX": 3.3099291700822753,
-                    "velocityY": -1.8316662884491148,
-                    "timestamp": 1.6224513119985433
-                },
-                {
-                    "x": 6.770506467400396,
-                    "y": 6.302297609620959,
-                    "heading": 2.5531737616431265,
-                    "angularVelocity": 0.0000015447528238732798,
-                    "velocityX": 3.3099290712533134,
-                    "velocityY": -1.831666467038753,
-                    "timestamp": 1.6552948240024026
-                },
-                {
-                    "x": 6.879216090079623,
-                    "y": 6.242139119005013,
-                    "heading": 2.5531738123794403,
-                    "angularVelocity": 0.0000015447895469442916,
-                    "velocityX": 3.309926863681755,
-                    "velocityY": -1.831670456219097,
-                    "timestamp": 1.6881383360062618
-                },
-                {
-                    "x": 6.983740227457298,
-                    "y": 6.1833650705733145,
-                    "heading": 2.56712091506752,
-                    "angularVelocity": 0.4246532065888785,
-                    "velocityX": 3.18248966083146,
-                    "velocityY": -1.7895177721795115,
-                    "timestamp": 1.720981848010121
-                },
-                {
-                    "x": 7.082197224690116,
-                    "y": 6.128185260935312,
-                    "heading": 2.6040317014746788,
-                    "angularVelocity": 1.1238379867177672,
-                    "velocityX": 2.9977609343741736,
-                    "velocityY": -1.6800824964004346,
-                    "timestamp": 1.7538253600139804
-                },
-                {
-                    "x": 7.174469097455313,
-                    "y": 6.076712532186898,
-                    "heading": 2.662828331449144,
-                    "angularVelocity": 1.7902053217559901,
-                    "velocityX": 2.809439890422065,
-                    "velocityY": -1.567211470635817,
-                    "timestamp": 1.7866688720178396
-                },
-                {
-                    "x": 7.2591302790362535,
-                    "y": 6.029614181571086,
-                    "heading": 2.728764792304138,
-                    "angularVelocity": 2.0075947069012146,
-                    "velocityX": 2.577714026776202,
-                    "velocityY": -1.4340229695982882,
-                    "timestamp": 1.819512384021699
-                },
-                {
-                    "x": 7.335970518862824,
-                    "y": 5.986956845665554,
-                    "heading": 2.7950149925408634,
-                    "angularVelocity": 2.0171472596761757,
-                    "velocityX": 2.339586577024463,
-                    "velocityY": -1.2988055571072747,
-                    "timestamp": 1.8523558960255582
-                },
-                {
-                    "x": 7.405004233411928,
-                    "y": 5.94870169343351,
-                    "heading": 2.858549177078574,
-                    "angularVelocity": 1.9344516058527972,
-                    "velocityX": 2.1018980717102513,
-                    "velocityY": -1.1647704492609365,
-                    "timestamp": 1.8851994080294174
-                },
-                {
-                    "x": 7.466271161714403,
-                    "y": 5.914805217934882,
-                    "heading": 2.9175764455342432,
-                    "angularVelocity": 1.7972276670270115,
-                    "velocityX": 1.8654195171112133,
-                    "velocityY": -1.0320600152214254,
-                    "timestamp": 1.9180429200332767
-                },
-                {
-                    "x": 7.519808154158724,
-                    "y": 5.885228450835136,
-                    "heading": 2.9709348371498114,
-                    "angularVelocity": 1.6246250282034163,
-                    "velocityX": 1.6300629615380646,
-                    "velocityY": -0.9005360661877405,
-                    "timestamp": 1.950886432037136
-                },
-                {
-                    "x": 7.565644888760801,
-                    "y": 5.8599385495006775,
-                    "heading": 3.0178335837303445,
-                    "angularVelocity": 1.427945542943834,
-                    "velocityX": 1.3956100247954721,
-                    "velocityY": -0.770012090408821,
-                    "timestamp": 1.9837299440409952
-                },
-                {
-                    "x": 7.603804643651593,
-                    "y": 5.838908591345399,
-                    "heading": 3.057713507827927,
-                    "angularVelocity": 1.214240550550639,
-                    "velocityX": 1.1618658469383054,
-                    "velocityY": -0.6403078377491017,
-                    "timestamp": 2.0165734560448545
-                },
-                {
-                    "x": 7.6343059239967275,
-                    "y": 5.822116657822096,
-                    "heading": 3.090162525369235,
-                    "angularVelocity": 0.987988663864437,
-                    "velocityX": 0.9286851035160462,
-                    "velocityY": -0.5112709481656601,
-                    "timestamp": 2.0494169680487135
-                },
-                {
-                    "x": 7.657163897668815,
-                    "y": 5.809544651656199,
-                    "heading": 3.1148617613725005,
-                    "angularVelocity": 0.7520278586639367,
-                    "velocityX": 0.6959661825873334,
-                    "velocityY": -0.3827850737892687,
-                    "timestamp": 2.0822604800525726
-                },
-                {
-                    "x": 7.67239146004521,
-                    "y": 5.801177089276775,
-                    "heading": 3.1315500981004414,
-                    "angularVelocity": 0.5081166936708882,
-                    "velocityX": 0.4636398925488508,
-                    "velocityY": -0.2547706341039437,
-                    "timestamp": 2.1151039920564316
+                    "heading": -0.5974593280342978,
+                    "angularVelocity": 0.0000022926381384477543,
+                    "velocityX": 3.7716260642834416,
+                    "velocityY": 0.2923470648746024,
+                    "timestamp": 1.2963561102392527
+                },
+                {
+                    "x": 5.617528419554277,
+                    "y": 6.721027319451783,
+                    "heading": -0.5974592417476945,
+                    "angularVelocity": 0.0000026270679336343826,
+                    "velocityX": 3.7828927995748978,
+                    "velocityY": 0.018711655155499447,
+                    "timestamp": 1.3292013222818118
+                },
+                {
+                    "x": 5.7414972044827435,
+                    "y": 6.712651079745108,
+                    "heading": -0.5974591685782128,
+                    "angularVelocity": 0.0000022277061710620633,
+                    "velocityX": 3.7743335244066216,
+                    "velocityY": -0.25502163589934396,
+                    "timestamp": 1.3620465343243708
+                },
+                {
+                    "x": 5.864535126960113,
+                    "y": 6.695327921921503,
+                    "heading": -0.597459104000633,
+                    "angularVelocity": 0.000001966118527479946,
+                    "velocityX": 3.745992637160019,
+                    "velocityY": -0.5274180541456184,
+                    "timestamp": 1.39489174636693
+                },
+                {
+                    "x": 5.985997338800097,
+                    "y": 6.669148654285341,
+                    "heading": -0.5974590451258569,
+                    "angularVelocity": 0.0000017924918823644274,
+                    "velocityX": 3.6980188065957362,
+                    "velocityY": -0.7970497374814797,
+                    "timestamp": 1.427736958409489
+                },
+                {
+                    "x": 6.105247258928482,
+                    "y": 6.634250515985412,
+                    "heading": -0.5974589899613981,
+                    "angularVelocity": 0.0000016795281661896423,
+                    "velocityX": 3.6306637318675383,
+                    "velocityY": -1.062503059946642,
+                    "timestamp": 1.460582170452048
+                },
+                {
+                    "x": 6.221659925637703,
+                    "y": 6.5908164867561565,
+                    "heading": -0.5974589370314296,
+                    "angularVelocity": 0.0000016114972403778573,
+                    "velocityX": 3.544281174328253,
+                    "velocityY": -1.3223854110871922,
+                    "timestamp": 1.4934273824946072
+                },
+                {
+                    "x": 6.334625365293315,
+                    "y": 6.539074496952364,
+                    "heading": -0.5974588851619594,
+                    "angularVelocity": 0.000001579209477235118,
+                    "velocityX": 3.439327458421827,
+                    "velocityY": -1.5753282316046957,
+                    "timestamp": 1.5262725945371662
+                },
+                {
+                    "x": 6.443562887659525,
+                    "y": 6.4793163144818955,
+                    "heading": -0.5974588333468978,
+                    "angularVelocity": 0.000001577552964572491,
+                    "velocityX": 3.3166941417319027,
+                    "velocityY": -1.819387933752184,
+                    "timestamp": 1.5591178065797253
+                },
+                {
+                    "x": 6.552407332287041,
+                    "y": 6.419388766569548,
+                    "heading": -0.5974587815290724,
+                    "angularVelocity": 0.0000015776371105880298,
+                    "velocityX": 3.313860312014723,
+                    "velocityY": -1.82454440649205,
+                    "timestamp": 1.5919630186222844
+                },
+                {
+                    "x": 6.661251765496281,
+                    "y": 6.359461197918566,
+                    "heading": -0.5974587297112393,
+                    "angularVelocity": 0.0000015776373457032493,
+                    "velocityX": 3.3138599643757076,
+                    "velocityY": -1.8245450378971961,
+                    "timestamp": 1.6248082306648435
+                },
+                {
+                    "x": 6.7700961954772865,
+                    "y": 6.299533623404254,
+                    "heading": -0.5974586778933993,
+                    "angularVelocity": 0.0000015776375522202912,
+                    "velocityX": 3.3138598660894165,
+                    "velocityY": -1.8245452164111537,
+                    "timestamp": 1.6576534427074026
+                },
+                {
+                    "x": 6.878940549868732,
+                    "y": 6.239605911600315,
+                    "heading": -0.5974586260742238,
+                    "angularVelocity": 0.0000015776782163840074,
+                    "velocityX": 3.313857564701912,
+                    "velocityY": -1.8245493963088628,
+                    "timestamp": 1.6904986547499616
+                },
+                {
+                    "x": 6.983465419726436,
+                    "y": 6.181138244955353,
+                    "heading": -0.5831453971579332,
+                    "angularVelocity": 0.4357782466962171,
+                    "velocityX": 3.182347238991671,
+                    "velocityY": -1.7800970981449478,
+                    "timestamp": 1.7233438667925207
+                },
+                {
+                    "x": 7.08190934617205,
+                    "y": 6.126284607678917,
+                    "heading": -0.5460039664027865,
+                    "angularVelocity": 1.1308019782912204,
+                    "velocityX": 2.997207821891462,
+                    "velocityY": -1.67006494600528,
+                    "timestamp": 1.7561890788350798
+                },
+                {
+                    "x": 7.174234078294548,
+                    "y": 6.075101338854014,
+                    "heading": -0.4864629486170815,
+                    "angularVelocity": 1.8127761729340726,
+                    "velocityX": 2.8109038237566843,
+                    "velocityY": -1.5583175032799648,
+                    "timestamp": 1.7890342908776389
+                },
+                {
+                    "x": 7.258956033298187,
+                    "y": 6.028257642494675,
+                    "heading": -0.41936488473987615,
+                    "angularVelocity": 2.0428567728619895,
+                    "velocityX": 2.5794309043853687,
+                    "velocityY": -1.4261955836547433,
+                    "timestamp": 1.821879502920198
+                },
+                {
+                    "x": 7.335844716602702,
+                    "y": 5.985834491701992,
+                    "heading": -0.35186229403264746,
+                    "angularVelocity": 2.0551729311350044,
+                    "velocityX": 2.3409403843979617,
+                    "velocityY": -1.2916083701234404,
+                    "timestamp": 1.854724714962757
+                },
+                {
+                    "x": 7.404915151007572,
+                    "y": 5.947794038432469,
+                    "heading": -0.2870870788105502,
+                    "angularVelocity": 1.9721356993583767,
+                    "velocityX": 2.1029072461275145,
+                    "velocityY": -1.1581734719873862,
+                    "timestamp": 1.8875699270053161
+                },
+                {
+                    "x": 7.466209510665893,
+                    "y": 5.914092129035952,
+                    "heading": -0.22688750133470567,
+                    "angularVelocity": 1.8328265744751362,
+                    "velocityX": 1.866158135283738,
+                    "velocityY": -1.0260828687243908,
+                    "timestamp": 1.9204151390478752
+                },
+                {
+                    "x": 7.519766848297347,
+                    "y": 5.8846886900937605,
+                    "heading": -0.17246123732016092,
+                    "angularVelocity": 1.657053208974533,
+                    "velocityX": 1.6305980172109753,
+                    "velocityY": -0.8952123342686217,
+                    "timestamp": 1.9532603510904343
+                },
+                {
+                    "x": 7.565618503926309,
+                    "y": 5.859549729252428,
+                    "heading": -0.12462132977080041,
+                    "angularVelocity": 1.4565260680124767,
+                    "velocityX": 1.395992072438578,
+                    "velocityY": -0.7653767254946843,
+                    "timestamp": 1.9861055631329934
+                },
+                {
+                    "x": 7.603788954978725,
+                    "y": 5.838647308998552,
+                    "heading": -0.08394075693399232,
+                    "angularVelocity": 1.2385541242393951,
+                    "velocityX": 1.1621313634075436,
+                    "velocityY": -0.6363916977236806,
+                    "timestamp": 2.0189507751755524
+                },
+                {
+                    "x": 7.634297586862898,
+                    "y": 5.821958675739452,
+                    "heading": -0.050840342274680116,
+                    "angularVelocity": 1.0077698574891136,
+                    "velocityX": 0.928860859374263,
+                    "velocityY": -0.5080994221459559,
+                    "timestamp": 2.0517959872181115
+                },
+                {
+                    "x": 7.657160251333572,
+                    "y": 5.809465047264352,
+                    "heading": -0.025644988874275507,
+                    "angularVelocity": 0.7670936442051631,
+                    "velocityX": 0.6960729752956952,
+                    "velocityY": -0.3803789867120345,
+                    "timestamp": 2.0846411992606706
+                },
+                {
+                    "x": 7.672390418071531,
+                    "y": 5.801150340997017,
+                    "heading": -0.00862072816733467,
+                    "angularVelocity": 0.5183178810016766,
+                    "velocityX": 0.46369518693407513,
+                    "velocityY": -0.2531481987862983,
+                    "timestamp": 2.1174864113032297
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
-                    "angularVelocity": 0.257277659544025,
-                    "velocityX": 0.2316603642721254,
-                    "velocityY": -0.1271815656097946,
-                    "timestamp": 2.1479475040602907
+                    "heading": 8.329701949714689e-33,
+                    "angularVelocity": 0.26246529193262896,
+                    "velocityX": 0.2316800975013136,
+                    "velocityY": -0.12636060901771562,
+                    "timestamp": 2.1503316233457888
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
-                    "angularVelocity": -3.3673783800968066e-32,
-                    "velocityX": -2.4380081043450124e-32,
-                    "velocityY": -4.2407952262773785e-32,
-                    "timestamp": 2.1807910160641497
+                    "heading": 5.789840595031671e-40,
+                    "angularVelocity": 1.2947513524749855e-30,
+                    "velocityX": 9.16355000594536e-32,
+                    "velocityY": -1.9486684829089695e-31,
+                    "timestamp": 2.183176835388348
                 }
             ],
             "constraints": [
@@ -20197,7 +18659,7 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -20206,25 +18668,25 @@
                 {
                     "x": 3.558790922164917,
                     "y": 5.477728366851807,
-                    "heading": 3.141592653589793,
+                    "heading": 0.002,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
-                    "controlIntervalCount": 42
+                    "controlIntervalCount": 11
                 },
                 {
                     "x": 5.390789031982422,
                     "y": 4.081310272216797,
-                    "heading": -3.131789038956166,
+                    "heading": 0.008,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
-                    "controlIntervalCount": 42
+                    "controlIntervalCount": 11
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -20235,866 +18697,308 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "angularVelocity": 0,
-                    "velocityX": -5.720774693888146e-34,
-                    "velocityY": 0,
+                    "velocityX": 0,
+                    "velocityY": 1.9823938961383557e-29,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.3121576423431627,
-                    "y": 4.3971322791414,
-                    "heading": 2.7106310928070285,
-                    "angularVelocity": -0.5647989987388474,
-                    "velocityX": 0.1744174454420521,
-                    "velocityY": 0.5327116136893348,
-                    "timestamp": 0.06970427936465769
-                },
-                {
-                    "x": 2.3388751368329244,
-                    "y": 4.470343590887415,
-                    "heading": 2.629622228062893,
-                    "angularVelocity": -1.1621792160039144,
-                    "velocityX": 0.38329776497637447,
-                    "velocityY": 1.0503130139687884,
-                    "timestamp": 0.13940855872931537
-                },
-                {
-                    "x": 2.3840539747737246,
-                    "y": 4.577503337755321,
-                    "heading": 2.5035786127839184,
-                    "angularVelocity": -1.8082622247563562,
-                    "velocityX": 0.6481501329989676,
-                    "velocityY": 1.5373481778256302,
-                    "timestamp": 0.20911283809397307
-                },
-                {
-                    "x": 2.4545399331182174,
-                    "y": 4.713290189810374,
-                    "heading": 2.3271901249556994,
-                    "angularVelocity": -2.5305259510028497,
-                    "velocityX": 1.011214217935542,
-                    "velocityY": 1.948041831760208,
-                    "timestamp": 0.27881711745863075
-                },
-                {
-                    "x": 2.555797664071962,
-                    "y": 4.87530128954077,
-                    "heading": 2.141593101929859,
-                    "angularVelocity": -2.662634557268572,
-                    "velocityX": 1.4526759601661592,
-                    "velocityY": 2.3242633193700435,
-                    "timestamp": 0.3485213968232884
-                },
-                {
-                    "x": 2.6889975632138237,
-                    "y": 5.05237912689222,
-                    "heading": 2.0338949096526036,
-                    "angularVelocity": -1.545072888765306,
-                    "velocityX": 1.910928573624968,
-                    "velocityY": 2.54041558087227,
-                    "timestamp": 0.4182256761879461
-                },
-                {
-                    "x": 2.8587006072828576,
-                    "y": 5.214124992637136,
-                    "heading": 1.956646376370765,
-                    "angularVelocity": -1.108232291990471,
-                    "velocityX": 2.4346144256256212,
-                    "velocityY": 2.3204581873480494,
-                    "timestamp": 0.48792995555260377
-                },
-                {
-                    "x": 3.035891081202655,
-                    "y": 5.338211520127435,
-                    "heading": 1.8312106553523118,
-                    "angularVelocity": -1.7995411782716633,
-                    "velocityX": 2.5420315012917163,
-                    "velocityY": 1.7801852141837777,
-                    "timestamp": 0.5576342349172615
-                },
-                {
-                    "x": 3.2127166120567576,
-                    "y": 5.42406293198425,
-                    "heading": 1.6557539162164756,
-                    "angularVelocity": -2.5171587847273322,
-                    "velocityX": 2.536795910750906,
-                    "velocityY": 1.231651953643812,
-                    "timestamp": 0.6273385142819192
-                },
-                {
-                    "x": 3.3872389413542425,
-                    "y": 5.4708692312967075,
-                    "heading": 1.441472502737752,
-                    "angularVelocity": -3.0741500440412146,
-                    "velocityX": 2.503753440796252,
-                    "velocityY": 0.6714982170261123,
-                    "timestamp": 0.697042793646577
+                    "x": 2.312764146242865,
+                    "y": 4.398513079462153,
+                    "heading": -0.39804543849617763,
+                    "angularVelocity": -0.11524733034669515,
+                    "velocityX": 0.18284072142228638,
+                    "velocityY": 0.5516827447028667,
+                    "timestamp": 0.06981019405093988
+                },
+                {
+                    "x": 2.339895425931373,
+                    "y": 4.474992910905513,
+                    "heading": -0.4126392714825622,
+                    "angularVelocity": -0.20905017072795415,
+                    "velocityX": 0.3886435220157258,
+                    "velocityY": 1.0955395910772,
+                    "timestamp": 0.13962038810187977
+                },
+                {
+                    "x": 2.3838571031189573,
+                    "y": 4.588440455462814,
+                    "heading": -0.43142102712692415,
+                    "angularVelocity": -0.2690403013169746,
+                    "velocityX": 0.6297314852832604,
+                    "velocityY": 1.6250856497386854,
+                    "timestamp": 0.20943058215281965
+                },
+                {
+                    "x": 2.448821264845437,
+                    "y": 4.736675156040894,
+                    "heading": -0.45019318323311186,
+                    "angularVelocity": -0.26890279219946406,
+                    "velocityX": 0.9305827409539079,
+                    "velocityY": 2.1233961972720627,
+                    "timestamp": 0.27924077620375953
+                },
+                {
+                    "x": 2.542623225560298,
+                    "y": 4.913429983875953,
+                    "heading": -0.4600490129319591,
+                    "angularVelocity": -0.14118037962391586,
+                    "velocityX": 1.343671393412285,
+                    "velocityY": 2.5319343433823196,
+                    "timestamp": 0.34905097025469944
+                },
+                {
+                    "x": 2.675927389267578,
+                    "y": 5.095797559489528,
+                    "heading": -0.44245218860567326,
+                    "angularVelocity": 0.25206668688824874,
+                    "velocityX": 1.9095228930350086,
+                    "velocityY": 2.6123344605070336,
+                    "timestamp": 0.41886116430563936
+                },
+                {
+                    "x": 2.83772713712192,
+                    "y": 5.250967901017311,
+                    "heading": -0.38879601501351296,
+                    "angularVelocity": 0.7686008371275667,
+                    "velocityX": 2.3177094699953265,
+                    "velocityY": 2.2227461710524636,
+                    "timestamp": 0.48867135835657927
+                },
+                {
+                    "x": 3.013235940099558,
+                    "y": 5.368218424760011,
+                    "heading": -0.31718431053283597,
+                    "angularVelocity": 1.0258058360734288,
+                    "velocityX": 2.514085591121856,
+                    "velocityY": 1.6795616361728827,
+                    "timestamp": 0.5584815524075192
+                },
+                {
+                    "x": 3.1945050869972476,
+                    "y": 5.445323486359522,
+                    "heading": -0.23653218942173773,
+                    "angularVelocity": 1.1553057859494003,
+                    "velocityX": 2.5965999573717062,
+                    "velocityY": 1.1044957351381008,
+                    "timestamp": 0.6282917464584591
+                },
+                {
+                    "x": 3.377225953863421,
+                    "y": 5.481834951968023,
+                    "heading": -0.15180568644047004,
+                    "angularVelocity": 1.2136694952369833,
+                    "velocityX": 2.6173952006491676,
+                    "velocityY": 0.523010515923758,
+                    "timestamp": 0.698101940509399
                 },
                 {
                     "x": 3.558790922164917,
                     "y": 5.477728366851807,
-                    "heading": 1.205442867585498,
-                    "angularVelocity": -3.386157023695285,
-                    "velocityX": 2.461139866509503,
-                    "velocityY": 0.09840336371911824,
-                    "timestamp": 0.7667470730112347
-                },
-                {
-                    "x": 3.6181705627536003,
-                    "y": 5.4752230037777725,
-                    "heading": 1.1222490393160696,
-                    "angularVelocity": -3.4253809174942864,
-                    "velocityX": 2.44486750990022,
-                    "velocityY": -0.10315456138644599,
-                    "timestamp": 0.7910345406749146
-                },
-                {
-                    "x": 3.6770128805090945,
-                    "y": 5.467829320967843,
-                    "heading": 1.0387507075885118,
-                    "angularVelocity": -3.4379183900025705,
-                    "velocityX": 2.4227440493307753,
-                    "velocityY": -0.3044237839989221,
-                    "timestamp": 0.8153220083385946
-                },
-                {
-                    "x": 3.735181283810321,
-                    "y": 5.455564580406075,
-                    "heading": 0.9556144237478981,
-                    "angularVelocity": -3.4230116120725995,
-                    "velocityX": 2.394996633931211,
-                    "velocityY": -0.5049822703463023,
-                    "timestamp": 0.8396094760022745
-                },
-                {
-                    "x": 3.7925490950380945,
-                    "y": 5.438455755513165,
-                    "heading": 0.8735217241735449,
-                    "angularVelocity": -3.380043597427691,
-                    "velocityX": 2.362033457838118,
-                    "velocityY": -0.7044301666120085,
-                    "timestamp": 0.8638969436659545
-                },
-                {
-                    "x": 3.8490034281899477,
-                    "y": 5.416538531692629,
-                    "heading": 0.7931634356424634,
-                    "angularVelocity": -3.308631827897467,
-                    "velocityX": 2.3244223701541378,
-                    "velocityY": -0.902408769989304,
-                    "timestamp": 0.8881844113296344
-                },
-                {
-                    "x": 3.9044487728906376,
-                    "y": 5.389855903636327,
-                    "heading": 0.7152329727062241,
-                    "angularVelocity": -3.2086697557513846,
-                    "velocityX": 2.282878786231156,
-                    "velocityY": -1.0986171315094786,
-                    "timestamp": 0.9124718789933144
-                },
-                {
-                    "x": 3.958810912211869,
-                    "y": 5.358456406059338,
-                    "heading": 0.6404209450438546,
-                    "angularVelocity": -3.080272867403366,
-                    "velocityX": 2.2382794317633223,
-                    "velocityY": -1.2928271490380632,
-                    "timestamp": 0.9367593466569943
-                },
-                {
-                    "x": 4.012042256408639,
-                    "y": 5.322391998548557,
-                    "heading": 0.5694141014307214,
-                    "angularVelocity": -2.923600129762354,
-                    "velocityX": 2.1917206410273016,
-                    "velocityY": -1.4848978086221774,
-                    "timestamp": 0.9610468143206743
-                },
-                {
-                    "x": 4.064130365340956,
-                    "y": 5.281715631109981,
-                    "heading": 0.5029022798709881,
-                    "angularVelocity": -2.738524348472814,
-                    "velocityX": 2.1446496462128213,
-                    "velocityY": -1.6747883312431175,
-                    "timestamp": 0.9853342819843542
-                },
-                {
-                    "x": 4.1151123432623455,
-                    "y": 5.236478617606072,
-                    "heading": 0.44159733021441666,
-                    "angularVelocity": -2.524139218855187,
-                    "velocityX": 2.09910636330477,
-                    "velocityY": -1.8625660826533157,
-                    "timestamp": 1.0096217496480342
-                },
-                {
-                    "x": 4.165095928896697,
-                    "y": 5.186728422848365,
-                    "heading": 0.38626165615570274,
-                    "angularVelocity": -2.278363262278863,
-                    "velocityX": 2.057999060523636,
-                    "velocityY": -2.0483895417431537,
-                    "timestamp": 1.0339092173117141
-                },
-                {
-                    "x": 4.214269146284126,
-                    "y": 5.132508462649398,
-                    "heading": 0.33770705635311316,
-                    "angularVelocity": -1.9991627153126146,
-                    "velocityX": 2.0246333651722517,
-                    "velocityY": -2.232425419964549,
-                    "timestamp": 1.058196684975394
-                },
-                {
-                    "x": 4.262857394718322,
-                    "y": 5.073859999949952,
-                    "heading": 0.2966997330272638,
-                    "angularVelocity": -1.6884149427884858,
-                    "velocityX": 2.000548147177036,
-                    "velocityY": -2.4147623585784754,
-                    "timestamp": 1.082484152639074
-                },
-                {
-                    "x": 4.311032470035773,
-                    "y": 5.010821065356263,
-                    "heading": 0.26381895203232264,
-                    "angularVelocity": -1.353816768806731,
-                    "velocityX": 1.9835363647027058,
-                    "velocityY": -2.59553344410457,
-                    "timestamp": 1.106771620302754
-                },
-                {
-                    "x": 4.358621953046075,
-                    "y": 4.943414538605821,
-                    "heading": 0.23906687218938943,
-                    "angularVelocity": -1.019129708608845,
-                    "velocityX": 1.9594254810463076,
-                    "velocityY": -2.77536249080602,
-                    "timestamp": 1.131059087966434
-                },
-                {
-                    "x": 4.401785346523604,
-                    "y": 4.873960680394997,
-                    "heading": 0.21204412295947933,
-                    "angularVelocity": -1.1126211099529368,
-                    "velocityX": 1.7771878927533025,
-                    "velocityY": -2.8596582884878865,
-                    "timestamp": 1.155346555630114
-                },
-                {
-                    "x": 4.442527933324705,
-                    "y": 4.807784869243466,
-                    "heading": 0.1747999499890042,
-                    "angularVelocity": -1.5334728793554109,
-                    "velocityX": 1.6775148140301646,
-                    "velocityY": -2.7246896246203156,
-                    "timestamp": 1.1796340232937939
-                },
-                {
-                    "x": 4.4813722294916225,
-                    "y": 4.745319922619003,
-                    "heading": 0.12759770745686402,
-                    "angularVelocity": -1.9434814360134838,
-                    "velocityX": 1.5993555484998627,
-                    "velocityY": -2.571900351631775,
-                    "timestamp": 1.2039214909574738
-                },
-                {
-                    "x": 4.518612325734112,
-                    "y": 4.686765196550011,
-                    "heading": 0.07078885768669944,
-                    "angularVelocity": -2.3390190594105262,
-                    "velocityX": 1.5333050262040608,
-                    "velocityY": -2.410902893616876,
-                    "timestamp": 1.2282089586211538
-                },
-                {
-                    "x": 4.554475342160425,
-                    "y": 4.632242142038935,
-                    "heading": 0.004632855038261056,
-                    "angularVelocity": -2.723874039258911,
-                    "velocityX": 1.47660583321928,
-                    "velocityY": -2.244904873002102,
-                    "timestamp": 1.2524964262848337
-                },
-                {
-                    "x": 4.589156761961767,
-                    "y": 4.581819399142938,
-                    "heading": -0.07076666263842435,
-                    "angularVelocity": -3.1044618863019378,
-                    "velocityX": 1.4279553670061842,
-                    "velocityY": -2.076080701134665,
-                    "timestamp": 1.2767838939485137
-                },
-                {
-                    "x": 4.6228372561227475,
-                    "y": 4.535518314545829,
-                    "heading": -0.1555015238515939,
-                    "angularVelocity": -3.488830634240387,
-                    "velocityX": 1.3867437572072365,
-                    "velocityY": -1.9063776116251314,
-                    "timestamp": 1.3010713616121936
-                },
-                {
-                    "x": 4.654626762222228,
-                    "y": 4.492711334026638,
-                    "heading": -0.25016165034544124,
-                    "angularVelocity": -3.8974885239025583,
-                    "velocityX": 1.3088851641383707,
-                    "velocityY": -1.762513124544663,
-                    "timestamp": 1.3253588292758736
-                },
-                {
-                    "x": 4.685559266058307,
-                    "y": 4.453815281857176,
-                    "heading": -0.35503604485130436,
-                    "angularVelocity": -4.318045666930297,
-                    "velocityX": 1.2735993832052854,
-                    "velocityY": -1.6014865241643912,
-                    "timestamp": 1.3496462969395535
-                },
-                {
-                    "x": 4.716254412162635,
-                    "y": 4.418834840133045,
-                    "heading": -0.4707020428721612,
-                    "angularVelocity": -4.762373732104905,
-                    "velocityX": 1.2638265351241147,
-                    "velocityY": -1.440267145530449,
-                    "timestamp": 1.3739337646032335
-                },
-                {
-                    "x": 4.747074366156251,
-                    "y": 4.387541660715592,
-                    "heading": -0.5981615568154537,
-                    "angularVelocity": -5.247954035730924,
-                    "velocityX": 1.268965312497503,
-                    "velocityY": -1.2884496585146104,
-                    "timestamp": 1.3982212322669134
-                },
-                {
-                    "x": 4.7760218641280225,
-                    "y": 4.359163482704458,
-                    "heading": -0.7400182505648043,
-                    "angularVelocity": -5.840736288925144,
-                    "velocityX": 1.191869748325327,
-                    "velocityY": -1.1684288540946337,
-                    "timestamp": 1.4225086999305934
-                },
-                {
-                    "x": 4.800681197551656,
-                    "y": 4.328735146997818,
-                    "heading": -0.8864907082583147,
-                    "angularVelocity": -6.030783436206011,
-                    "velocityX": 1.015311014104143,
-                    "velocityY": -1.2528410177625973,
-                    "timestamp": 1.4467961675942733
-                },
-                {
-                    "x": 4.821257943768876,
-                    "y": 4.295565486099031,
-                    "heading": -1.0326334280053908,
-                    "angularVelocity": -6.017206971545368,
-                    "velocityX": 0.8472166181404436,
-                    "velocityY": -1.3657109649348727,
-                    "timestamp": 1.4710836352579533
-                },
-                {
-                    "x": 4.842407281192705,
-                    "y": 4.263513715444916,
-                    "heading": -1.1735548575556736,
-                    "angularVelocity": -5.802228190344444,
-                    "velocityX": 0.8707922010105955,
-                    "velocityY": -1.319683513240253,
-                    "timestamp": 1.4953711029216332
-                },
-                {
-                    "x": 4.867138629271623,
-                    "y": 4.234341103924059,
-                    "heading": -1.3080892458596534,
-                    "angularVelocity": -5.539251360699312,
-                    "velocityX": 1.0182761093657138,
-                    "velocityY": -1.2011384605766136,
-                    "timestamp": 1.5196585705853132
-                },
-                {
-                    "x": 4.89576478236693,
-                    "y": 4.207846540583478,
-                    "heading": -1.4377333776779964,
-                    "angularVelocity": -5.337902395325306,
-                    "velocityX": 1.1786388557138734,
-                    "velocityY": -1.0908738493227768,
-                    "timestamp": 1.5439460382489931
-                },
-                {
-                    "x": 4.928534232838641,
-                    "y": 4.183793595288781,
-                    "heading": -1.5636936940682342,
-                    "angularVelocity": -5.1862268283573245,
-                    "velocityX": 1.3492329017369804,
-                    "velocityY": -0.9903438937220402,
-                    "timestamp": 1.568233505912673
-                },
-                {
-                    "x": 4.965642273050373,
-                    "y": 4.1619364014492675,
-                    "heading": -1.6870176099857392,
-                    "angularVelocity": -5.077676998904523,
-                    "velocityX": 1.5278678174928029,
-                    "velocityY": -0.8999371256888635,
-                    "timestamp": 1.592520973576353
-                },
-                {
-                    "x": 5.007242132688007,
-                    "y": 4.142028453113074,
-                    "heading": -1.8086864696957439,
-                    "angularVelocity": -5.009532545542041,
-                    "velocityX": 1.712811735405568,
-                    "velocityY": -0.8196798699587677,
-                    "timestamp": 1.616808441240033
-                },
-                {
-                    "x": 5.0534540304279405,
-                    "y": 4.123819388018015,
-                    "heading": -1.9296932957732547,
-                    "angularVelocity": -4.982274305133403,
-                    "velocityX": 1.902705476744277,
-                    "velocityY": -0.749730904316916,
-                    "timestamp": 1.641095908903713
-                },
-                {
-                    "x": 5.102543724383187,
-                    "y": 4.108450898288225,
-                    "heading": -2.039853637710442,
-                    "angularVelocity": -4.535686612644417,
-                    "velocityX": 2.0211944133087334,
-                    "velocityY": -0.6327744803453944,
-                    "timestamp": 1.6653833765673929
-                },
-                {
-                    "x": 5.15444764406018,
-                    "y": 4.096180097409251,
-                    "heading": -2.1398530735226235,
-                    "angularVelocity": -4.117326565161934,
-                    "velocityX": 2.1370659302868042,
-                    "velocityY": -0.5052317948043931,
-                    "timestamp": 1.6896708442310728
-                },
-                {
-                    "x": 5.20922556245635,
-                    "y": 4.087205112082737,
-                    "heading": -2.231114838342211,
-                    "angularVelocity": -3.757566086482643,
-                    "velocityX": 2.2553985106519345,
-                    "velocityY": -0.36953153991984866,
-                    "timestamp": 1.7139583118947528
-                },
-                {
-                    "x": 5.266890046930524,
-                    "y": 4.081668558801882,
-                    "heading": -2.314693981916533,
-                    "angularVelocity": -3.4412456964094225,
-                    "velocityX": 2.3742485331398995,
-                    "velocityY": -0.22795926514540626,
-                    "timestamp": 1.7382457795584327
-                },
-                {
-                    "x": 5.327424168986788,
-                    "y": 4.079676285408984,
-                    "heading": -2.3913241555270592,
-                    "angularVelocity": -3.1551322958679826,
-                    "velocityX": 2.492401550236051,
-                    "velocityY": -0.08202886445327343,
-                    "timestamp": 1.7625332472221127
+                    "heading": -0.06601521216801902,
+                    "angularVelocity": 1.2289104110775417,
+                    "velocityX": 2.6008374674955865,
+                    "velocityY": -0.05882500648124433,
+                    "timestamp": 0.7679121345603389
+                },
+                {
+                    "x": 3.7207755893743903,
+                    "y": 5.440802283745667,
+                    "heading": 0.01087618717506099,
+                    "angularVelocity": 1.2168377614801031,
+                    "velocityX": 2.5634734381227786,
+                    "velocityY": -0.5843703286511746,
+                    "timestamp": 0.8311016588990232
+                },
+                {
+                    "x": 3.8800031263189014,
+                    "y": 5.370704697701765,
+                    "heading": 0.08523277253593173,
+                    "angularVelocity": 1.1767232961398173,
+                    "velocityX": 2.519840726939224,
+                    "velocityY": -1.1093228945227975,
+                    "timestamp": 0.8942911832377075
+                },
+                {
+                    "x": 4.035880089753818,
+                    "y": 5.267514783368049,
+                    "heading": 0.15438353932229767,
+                    "angularVelocity": 1.0943390935099226,
+                    "velocityX": 2.4668165343191313,
+                    "velocityY": -1.633022489291642,
+                    "timestamp": 0.9574807075763918
+                },
+                {
+                    "x": 4.187424190703861,
+                    "y": 5.131425904894565,
+                    "heading": 0.2138534475851711,
+                    "angularVelocity": 0.9411355581283213,
+                    "velocityX": 2.3982472179488545,
+                    "velocityY": -2.153662017503815,
+                    "timestamp": 1.0206702319150762
+                },
+                {
+                    "x": 4.332733928007212,
+                    "y": 4.96304651892829,
+                    "heading": 0.25476003945729764,
+                    "angularVelocity": 0.6473635034690916,
+                    "velocityX": 2.2995858699928062,
+                    "velocityY": -2.664672471059999,
+                    "timestamp": 1.0838597562537606
+                },
+                {
+                    "x": 4.46716744223408,
+                    "y": 4.765388489141315,
+                    "heading": 0.2547605945579538,
+                    "angularVelocity": 0.000008784690993212257,
+                    "velocityX": 2.12746520302083,
+                    "velocityY": -3.1280189533989655,
+                    "timestamp": 1.147049280592445
+                },
+                {
+                    "x": 4.612743650855858,
+                    "y": 4.575786978724626,
+                    "heading": 0.2547605407972418,
+                    "angularVelocity": -8.507852266556338e-7,
+                    "velocityX": 2.303802887345736,
+                    "velocityY": -3.0005212478052368,
+                    "timestamp": 1.2102388049311295
+                },
+                {
+                    "x": 4.782527461386278,
+                    "y": 4.409030186652972,
+                    "heading": 0.25208197397278675,
+                    "angularVelocity": -0.04238941267495421,
+                    "velocityX": 2.6868980627285057,
+                    "velocityY": -2.638994260793031,
+                    "timestamp": 1.2734283292698139
+                },
+                {
+                    "x": 4.970403968588328,
+                    "y": 4.270066434839271,
+                    "heading": 0.23891743895373094,
+                    "angularVelocity": -0.2083341369570075,
+                    "velocityX": 2.9732223682415455,
+                    "velocityY": -2.1991580609051504,
+                    "timestamp": 1.3366178536084983
+                },
+                {
+                    "x": 5.173968377386639,
+                    "y": 4.1604195633547185,
+                    "heading": 0.21971651393307054,
+                    "angularVelocity": -0.3038624711945638,
+                    "velocityX": 3.2214898106793903,
+                    "velocityY": -1.7352064702582655,
+                    "timestamp": 1.3998073779471827
                 },
                 {
                     "x": 5.390789031982422,
                     "y": 4.081310272216797,
-                    "heading": -2.461469590575517,
-                    "angularVelocity": -2.888132925992722,
-                    "velocityX": 2.608953056493045,
-                    "velocityY": 0.06727695247771216,
-                    "timestamp": 1.7868207148857926
-                },
-                {
-                    "x": 5.47852793656002,
-                    "y": 4.0897392429132315,
-                    "heading": -2.541079365075053,
-                    "angularVelocity": -2.4976756873239485,
-                    "velocityX": 2.752718873700339,
-                    "velocityY": 0.2644503807478311,
-                    "timestamp": 1.8186942583180192
-                },
-                {
-                    "x": 5.570818505990881,
-                    "y": 4.101985641066581,
-                    "heading": -2.595195903422616,
-                    "angularVelocity": -1.6978513375091853,
-                    "velocityX": 2.8955227280299005,
-                    "velocityY": 0.384218283711974,
-                    "timestamp": 1.8505678017502458
-                },
-                {
-                    "x": 5.666172128712358,
-                    "y": 4.1161647975520586,
-                    "heading": -2.6133567142303673,
-                    "angularVelocity": -0.5697769639691046,
-                    "velocityX": 2.9916229089567223,
-                    "velocityY": 0.4448566101734819,
-                    "timestamp": 1.8824413451824724
-                },
-                {
-                    "x": 5.762848661651843,
-                    "y": 4.1299008615103485,
-                    "heading": -2.5899687129898252,
-                    "angularVelocity": 0.7337747461392797,
-                    "velocityX": 3.033127871240633,
-                    "velocityY": 0.43095503289420706,
-                    "timestamp": 1.914314888614699
-                },
-                {
-                    "x": 5.85803719097752,
-                    "y": 4.141633583817035,
-                    "heading": -2.5274756720169904,
-                    "angularVelocity": 1.960655585900415,
-                    "velocityX": 2.9864432716142737,
-                    "velocityY": 0.36810222658906844,
-                    "timestamp": 1.9461884320469256
-                },
-                {
-                    "x": 5.9473700887935115,
-                    "y": 4.148469896281193,
-                    "heading": -2.4508224753320094,
-                    "angularVelocity": 2.404916066140258,
-                    "velocityX": 2.8027287899740645,
-                    "velocityY": 0.21448234893285298,
-                    "timestamp": 1.9780619754791522
-                },
-                {
-                    "x": 6.0307522620060645,
-                    "y": 4.150565536032173,
-                    "heading": -2.3598343948212093,
-                    "angularVelocity": 2.854658463194389,
-                    "velocityX": 2.6160308592563855,
-                    "velocityY": 0.06574856527754983,
-                    "timestamp": 2.009935518911379
-                },
-                {
-                    "x": 6.108380396897077,
-                    "y": 4.1477533333139265,
-                    "heading": -2.2542021077222087,
-                    "angularVelocity": 3.3141055472419727,
-                    "velocityX": 2.4355037605428165,
-                    "velocityY": -0.0882299994108409,
-                    "timestamp": 2.0418090623436056
-                },
-                {
-                    "x": 6.180764914708381,
-                    "y": 4.139596420595998,
-                    "heading": -2.1333489972804904,
-                    "angularVelocity": 3.7916433953661524,
-                    "velocityX": 2.270990609036518,
-                    "velocityY": -0.2559148384387137,
-                    "timestamp": 2.0736826057758324
-                },
-                {
-                    "x": 6.2487782508806635,
-                    "y": 4.1255186451287145,
-                    "heading": -1.9962477740611584,
-                    "angularVelocity": 4.301411404441215,
-                    "velocityX": 2.133849231946881,
-                    "velocityY": -0.4416758838633167,
-                    "timestamp": 2.1055561492080592
-                },
-                {
-                    "x": 6.318999325259612,
-                    "y": 4.108403885933248,
-                    "heading": -1.8570700146176022,
-                    "angularVelocity": 4.366560616000945,
-                    "velocityX": 2.2031147722330044,
-                    "velocityY": -0.5369581587895442,
-                    "timestamp": 2.137429692640286
-                },
-                {
-                    "x": 6.393124517437766,
-                    "y": 4.09868995620756,
-                    "heading": -1.7226225348946564,
-                    "angularVelocity": 4.218152901914545,
-                    "velocityX": 2.3256024964958177,
-                    "velocityY": -0.3047646630925519,
-                    "timestamp": 2.169303236072513
-                },
-                {
-                    "x": 6.468812945370671,
-                    "y": 4.097296283093324,
-                    "heading": -1.5892075925845424,
-                    "angularVelocity": 4.185758090994726,
-                    "velocityX": 2.3746474280101166,
-                    "velocityY": -0.04372507616542974,
-                    "timestamp": 2.2011767795047397
-                },
-                {
-                    "x": 6.543776258333572,
-                    "y": 4.104341851159788,
-                    "heading": -1.4561024343589095,
-                    "angularVelocity": 4.176038930489708,
-                    "velocityX": 2.351897683490888,
-                    "velocityY": 0.22104753057800555,
-                    "timestamp": 2.2330503229369665
-                },
-                {
-                    "x": 6.614153245906366,
-                    "y": 4.118263151934315,
-                    "heading": -1.3162963129525131,
-                    "angularVelocity": 4.386274833347886,
-                    "velocityX": 2.208006390078291,
-                    "velocityY": 0.43676664956088845,
-                    "timestamp": 2.2649238663691933
-                },
-                {
-                    "x": 6.676725935567768,
-                    "y": 4.132111873679476,
-                    "heading": -1.164406226014857,
-                    "angularVelocity": 4.765396958785676,
-                    "velocityX": 1.9631544824770328,
-                    "velocityY": 0.4344895563497056,
-                    "timestamp": 2.29679740980142
-                },
-                {
-                    "x": 6.732923820729643,
-                    "y": 4.140964584087909,
-                    "heading": -1.0033332436559739,
-                    "angularVelocity": 5.053500960800803,
-                    "velocityX": 1.7631514764390552,
-                    "velocityY": 0.27774478313828127,
-                    "timestamp": 2.328670953233647
-                },
-                {
-                    "x": 6.785034236650818,
-                    "y": 4.142631098888223,
-                    "heading": -0.8355937264762496,
-                    "angularVelocity": 5.262656708890626,
-                    "velocityX": 1.6349112872241036,
-                    "velocityY": 0.0522852065023047,
-                    "timestamp": 2.3605444966658737
-                },
-                {
-                    "x": 6.835668705020557,
-                    "y": 4.136059975961784,
-                    "heading": -0.6631765444972689,
-                    "angularVelocity": 5.409413683345085,
-                    "velocityX": 1.5886049342899025,
-                    "velocityY": -0.2061622969661811,
-                    "timestamp": 2.3924180400981006
-                },
-                {
-                    "x": 6.8889189304898775,
-                    "y": 4.121434248751472,
-                    "heading": -0.4913644813841438,
-                    "angularVelocity": 5.390428694520651,
-                    "velocityX": 1.6706716522606933,
-                    "velocityY": -0.45886731236551676,
-                    "timestamp": 2.4242915835303274
-                },
-                {
-                    "x": 6.949941444507073,
-                    "y": 4.105368374325079,
-                    "heading": -0.330980540513716,
-                    "angularVelocity": 5.031882985067382,
-                    "velocityX": 1.9145192986449624,
-                    "velocityY": -0.5040504661979253,
-                    "timestamp": 2.456165126962554
-                },
-                {
-                    "x": 7.016508340627581,
-                    "y": 4.095339734393401,
-                    "heading": -0.17821539872753786,
-                    "angularVelocity": 4.792850914458417,
-                    "velocityX": 2.0884686467963878,
-                    "velocityY": -0.31463837564850267,
-                    "timestamp": 2.488038670394781
-                },
-                {
-                    "x": 7.085360516906702,
-                    "y": 4.093448303803928,
-                    "heading": -0.02746406984555507,
-                    "angularVelocity": 4.7296695832557045,
-                    "velocityX": 2.1601669869408497,
-                    "velocityY": -0.05934171057873482,
-                    "timestamp": 2.519912213827008
-                },
-                {
-                    "x": 7.153846633279139,
-                    "y": 4.100009116269512,
-                    "heading": 0.121772089508303,
-                    "angularVelocity": 4.682132680703696,
-                    "velocityX": 2.1486822297640367,
-                    "velocityY": 0.20583881674575163,
-                    "timestamp": 2.5517857572592346
-                },
-                {
-                    "x": 7.2178667451999,
-                    "y": 4.113588367220929,
-                    "heading": 0.27676556654047163,
-                    "angularVelocity": 4.862762665899784,
-                    "velocityX": 2.008565883391336,
-                    "velocityY": 0.4260351843305824,
-                    "timestamp": 2.5836593006914614
-                },
-                {
-                    "x": 7.274040071387795,
-                    "y": 4.126189441281968,
-                    "heading": 0.44297350868055696,
-                    "angularVelocity": 5.214605100104267,
-                    "velocityX": 1.7623809636144747,
-                    "velocityY": 0.3953458795013916,
-                    "timestamp": 2.6155328441236882
-                },
-                {
-                    "x": 7.324153742240104,
-                    "y": 4.133335840122352,
-                    "heading": 0.617641408977622,
-                    "angularVelocity": 5.480027680902935,
-                    "velocityX": 1.5722654419916315,
-                    "velocityY": 0.22421099353388252,
-                    "timestamp": 2.647406387555915
-                },
-                {
-                    "x": 7.370062418422902,
-                    "y": 4.133468677579737,
-                    "heading": 0.8003841200365157,
-                    "angularVelocity": 5.733366653991959,
-                    "velocityX": 1.440338012007226,
-                    "velocityY": 0.00416764008895702,
-                    "timestamp": 2.679279930988142
-                },
-                {
-                    "x": 7.413378960747577,
-                    "y": 4.125978597308371,
-                    "heading": 0.9928213817572759,
-                    "angularVelocity": 6.037523318671553,
-                    "velocityX": 1.3590124492050755,
-                    "velocityY": -0.2349936488013014,
-                    "timestamp": 2.7111534744203687
-                },
-                {
-                    "x": 7.454276397546973,
-                    "y": 4.11151825545799,
-                    "heading": 1.2015436063625509,
-                    "angularVelocity": 6.548447462362883,
-                    "velocityX": 1.2831154743229476,
-                    "velocityY": -0.4536785149454069,
-                    "timestamp": 2.7430270178525955
-                },
-                {
-                    "x": 7.496754891759898,
-                    "y": 4.105364802951648,
-                    "heading": 1.412047118709339,
-                    "angularVelocity": 6.604333553135948,
-                    "velocityX": 1.3327195422513116,
-                    "velocityY": -0.19305831243477098,
-                    "timestamp": 2.7749005612848223
-                },
-                {
-                    "x": 7.53888503669894,
-                    "y": 4.1076484340015265,
-                    "heading": 1.6249494045277855,
-                    "angularVelocity": 6.679592630519619,
-                    "velocityX": 1.3217904381614993,
-                    "velocityY": 0.07164660103553838,
-                    "timestamp": 2.806774104717049
-                },
-                {
-                    "x": 7.576001000454075,
-                    "y": 4.116500503183424,
-                    "heading": 1.8445622032463762,
-                    "angularVelocity": 6.890128146108359,
-                    "velocityX": 1.1644756044791265,
-                    "velocityY": 0.277724665308084,
-                    "timestamp": 2.838647648149276
-                },
-                {
-                    "x": 7.60515186851165,
-                    "y": 4.124308875688622,
-                    "heading": 2.0739714188941045,
-                    "angularVelocity": 7.1974807612943055,
-                    "velocityX": 0.9145788299176564,
-                    "velocityY": 0.24497974383681875,
-                    "timestamp": 2.8705211915815028
-                },
-                {
-                    "x": 7.62773108442525,
-                    "y": 4.127593662867362,
-                    "heading": 2.3140115714104335,
-                    "angularVelocity": 7.53101559061771,
-                    "velocityX": 0.708399929289633,
-                    "velocityY": 0.10305685609521825,
-                    "timestamp": 2.9023947350137296
-                },
-                {
-                    "x": 7.642808233685486,
-                    "y": 4.127168273515703,
-                    "heading": 2.549384793893863,
-                    "angularVelocity": 7.384595408537156,
-                    "velocityX": 0.473030219946994,
-                    "velocityY": -0.013346158156635379,
-                    "timestamp": 2.9342682784459564
-                },
-                {
-                    "x": 7.655297430086512,
-                    "y": 4.12694395453313,
-                    "heading": 2.7459505912038105,
-                    "angularVelocity": 6.167051922793274,
-                    "velocityX": 0.39183583173240866,
-                    "velocityY": -0.007037779876922506,
-                    "timestamp": 2.966141821878183
-                },
-                {
-                    "x": 7.665239437445753,
-                    "y": 4.126897699920467,
-                    "heading": 2.903400180799407,
-                    "angularVelocity": 4.939820698956328,
-                    "velocityX": 0.31192036682023955,
-                    "velocityY": -0.001451191417126478,
-                    "timestamp": 2.99801536531041
-                },
-                {
-                    "x": 7.672649475189336,
-                    "y": 4.12693483442264,
-                    "heading": 3.0216231319761504,
-                    "angularVelocity": 3.7091248241076054,
-                    "velocityX": 0.23248239591994224,
-                    "velocityY": 0.001165057228464201,
-                    "timestamp": 3.029888908742637
-                },
-                {
-                    "x": 7.677558550585934,
-                    "y": 4.1269805608298435,
-                    "heading": 3.1005189939079423,
-                    "angularVelocity": 2.4752774067793823,
-                    "velocityX": 0.15401724653037796,
-                    "velocityY": 0.0014346195082003044,
-                    "timestamp": 3.0617624521748636
+                    "heading": 0.19899217158132915,
+                    "angularVelocity": -0.3279711719205329,
+                    "velocityX": 3.4312753081359326,
+                    "velocityY": -1.25193680386463,
+                    "timestamp": 1.462996902285867
+                },
+                {
+                    "x": 5.681530534379024,
+                    "y": 4.031634954440038,
+                    "heading": 0.1807325834147537,
+                    "angularVelocity": -0.22892527550150313,
+                    "velocityX": 3.645102941474752,
+                    "velocityY": -0.6227925681552634,
+                    "timestamp": 1.542759122191511
+                },
+                {
+                    "x": 5.983259555492767,
+                    "y": 4.033604453850848,
+                    "heading": 0.1807316659794543,
+                    "angularVelocity": -0.000011502133581259988,
+                    "velocityX": 3.7828563632073102,
+                    "velocityY": 0.024692133859281144,
+                    "timestamp": 1.6225213420971547
+                },
+                {
+                    "x": 6.284539191755923,
+                    "y": 4.050188554583329,
+                    "heading": 0.18073165818528908,
+                    "angularVelocity": -9.771750653524246e-8,
+                    "velocityX": 3.777222306745724,
+                    "velocityY": 0.20791924738433482,
+                    "timestamp": 1.7022835620027985
+                },
+                {
+                    "x": 6.585818825161056,
+                    "y": 4.0667727072366,
+                    "heading": 0.18073165039112304,
+                    "angularVelocity": -9.77175162921636e-8,
+                    "velocityX": 3.777222270913928,
+                    "velocityY": 0.20791989832899174,
+                    "timestamp": 1.7820457819084423
+                },
+                {
+                    "x": 6.88709845856161,
+                    "y": 4.083356859971397,
+                    "heading": 0.180731642596831,
+                    "angularVelocity": -9.771909620772691e-8,
+                    "velocityX": 3.777222270856537,
+                    "velocityY": 0.20791989935110283,
+                    "timestamp": 1.8618080018140861
+                },
+                {
+                    "x": 7.15139892307148,
+                    "y": 4.097904823415219,
+                    "heading": 0.12048670978727856,
+                    "angularVelocity": -0.7553066211144209,
+                    "velocityX": 3.313604671767403,
+                    "velocityY": 0.1823916568650699,
+                    "timestamp": 1.94157022171973
+                },
+                {
+                    "x": 7.3628393316310445,
+                    "y": 4.109542993215659,
+                    "heading": 0.07229126938267637,
+                    "angularVelocity": -0.6042389550038586,
+                    "velocityX": 2.650884200686756,
+                    "velocityY": 0.1459108060709806,
+                    "timestamp": 2.0213324416253737
+                },
+                {
+                    "x": 7.521419659615098,
+                    "y": 4.118271521022388,
+                    "heading": 0.03614542857914898,
+                    "angularVelocity": -0.4531699449471921,
+                    "velocityX": 1.98816342087339,
+                    "velocityY": 0.10943185654883955,
+                    "timestamp": 2.1010946615310178
+                },
+                {
+                    "x": 7.627139886542953,
+                    "y": 4.12409050689369,
+                    "heading": 0.012048510432709781,
+                    "angularVelocity": -0.302109421915929,
+                    "velocityX": 1.3254423842882344,
+                    "velocityY": 0.07295416148382058,
+                    "timestamp": 2.180856881436662
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": 1.238676401825463,
-                    "velocityX": 0.07659799166218657,
-                    "velocityY": 0.0006098841880359634,
-                    "timestamp": 3.0936359956070905
+                    "heading": 2.791295127527068e-32,
+                    "angularVelocity": -0.15105535486544666,
+                    "velocityX": 0.6627211920578459,
+                    "velocityY": 0.03647708288149429,
+                    "timestamp": 2.260619101342306
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": -2.349124968959552e-31,
-                    "velocityX": 6.522838948908763e-32,
-                    "velocityY": -6.176555916296272e-32,
-                    "timestamp": 3.1255095390393173
+                    "heading": 0,
+                    "angularVelocity": -1.938584239537671e-30,
+                    "velocityX": -1.689352589413745e-32,
+                    "velocityY": 0,
+                    "timestamp": 2.34038132124795
                 }
             ],
             "constraints": [
@@ -21137,16 +19041,16 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 23
+                    "controlIntervalCount": 27
                 },
                 {
                     "x": 2.6104869842529297,
                     "y": 2.6966493129730225,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -21155,7 +19059,7 @@
                 {
                     "x": 5.646764755249023,
                     "y": 1.5066063404083252,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -21164,7 +19068,7 @@
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -21175,587 +19079,623 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "angularVelocity": 0,
                     "velocityX": 0,
                     "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.291384520253252,
-                    "y": 4.354032854764245,
-                    "heading": 2.74598551663602,
-                    "angularVelocity": -0.1129108392120503,
-                    "velocityX": -0.24231786763597,
-                    "velocityY": -0.1678311541441644,
-                    "timestamp": 0.03555445510807641
-                },
-                {
-                    "x": 2.274618850931762,
-                    "y": 4.341572242021881,
-                    "heading": 2.7363109696234753,
-                    "angularVelocity": -0.2721050564025352,
-                    "velocityX": -0.47154904415004567,
-                    "velocityY": -0.3504655803186264,
-                    "timestamp": 0.07110891021615282
-                },
-                {
-                    "x": 2.250443983775186,
-                    "y": 4.321957452674385,
-                    "heading": 2.718567267740621,
-                    "angularVelocity": -0.4990570613139176,
-                    "velocityX": -0.6799391829544631,
-                    "velocityY": -0.5516830250350497,
-                    "timestamp": 0.10666336532422922
-                },
-                {
-                    "x": 2.2201183125080237,
-                    "y": 4.294438762451095,
-                    "heading": 2.688674932435733,
-                    "angularVelocity": -0.8407479516708394,
-                    "velocityX": -0.8529359028279262,
-                    "velocityY": -0.7739871174973922,
-                    "timestamp": 0.14221782043230563
-                },
-                {
-                    "x": 2.1856076192251765,
-                    "y": 4.258557396291605,
-                    "heading": 2.63987728987899,
-                    "angularVelocity": -1.3724761751631571,
-                    "velocityX": -0.9706432900727432,
-                    "velocityY": -1.009194657896466,
-                    "timestamp": 0.17777227554038205
-                },
-                {
-                    "x": 2.1489602975624615,
-                    "y": 4.2143768004023965,
-                    "heading": 2.56722210682279,
-                    "angularVelocity": -2.0434902696538804,
-                    "velocityX": -1.0307378231874669,
-                    "velocityY": -1.2426177185084084,
-                    "timestamp": 0.21332673064845847
-                },
-                {
-                    "x": 2.1162123400405237,
-                    "y": 4.163459966768879,
-                    "heading": 2.4666019501957965,
-                    "angularVelocity": -2.8300294947885973,
-                    "velocityX": -0.9210648123390461,
-                    "velocityY": -1.4320802689492316,
-                    "timestamp": 0.2488811857565349
-                },
-                {
-                    "x": 2.0883593879928255,
-                    "y": 4.106985689754014,
-                    "heading": 2.334003256666977,
-                    "angularVelocity": -3.7294536824078297,
-                    "velocityX": -0.7833885222831486,
-                    "velocityY": -1.588388201793505,
-                    "timestamp": 0.2844356408646113
-                },
-                {
-                    "x": 2.0671900205502793,
-                    "y": 4.044210359842601,
-                    "heading": 2.181383778591042,
-                    "angularVelocity": -4.2925556758502195,
-                    "velocityX": -0.5954068872155824,
-                    "velocityY": -1.765610799563443,
-                    "timestamp": 0.31999009597268774
-                },
-                {
-                    "x": 2.0546372297458078,
-                    "y": 3.9756937078562617,
-                    "heading": 2.021719977202011,
-                    "angularVelocity": -4.49068340110105,
-                    "velocityX": -0.35305816855621736,
-                    "velocityY": -1.927090480730638,
-                    "timestamp": 0.35554455108076416
-                },
-                {
-                    "x": 2.0513227865299846,
-                    "y": 3.902144544449752,
-                    "heading": 1.8643846688260233,
-                    "angularVelocity": -4.42519250816047,
-                    "velocityX": -0.0932216006615165,
-                    "velocityY": -2.068634245214532,
-                    "timestamp": 0.3910990061888406
-                },
-                {
-                    "x": 2.056389020104272,
-                    "y": 3.822243076047335,
-                    "heading": 1.7096006131055106,
-                    "angularVelocity": -4.353436306364677,
-                    "velocityX": 0.14249222942348805,
-                    "velocityY": -2.247298352893811,
-                    "timestamp": 0.426653461296917
-                },
-                {
-                    "x": 2.0690751873782185,
-                    "y": 3.7373642087091223,
-                    "heading": 1.5737353715537044,
-                    "angularVelocity": -3.821328189078159,
-                    "velocityX": 0.35680949786416494,
-                    "velocityY": -2.3872920307793386,
-                    "timestamp": 0.4622079164049934
-                },
-                {
-                    "x": 2.089274580938585,
-                    "y": 3.648200269765939,
-                    "heading": 1.460663871168444,
-                    "angularVelocity": -3.1802343768608408,
-                    "velocityX": 0.5681255274188619,
-                    "velocityY": -2.507813399815797,
-                    "timestamp": 0.49776237151306985
-                },
-                {
-                    "x": 2.1169367654773477,
-                    "y": 3.55409496646177,
-                    "heading": 1.3686138295927854,
-                    "angularVelocity": -2.5889875487010094,
-                    "velocityX": 0.7780230200315752,
-                    "velocityY": -2.646793573916784,
-                    "timestamp": 0.5333168266211462
-                },
-                {
-                    "x": 2.1519045434306308,
-                    "y": 3.4545849603291003,
-                    "heading": 1.296733638399589,
-                    "angularVelocity": -2.0216929488779694,
-                    "velocityX": 0.9834991943200934,
-                    "velocityY": -2.798805545751856,
-                    "timestamp": 0.5688712817292226
-                },
-                {
-                    "x": 2.1939849352834964,
-                    "y": 3.3493550759903967,
-                    "heading": 1.244789692250972,
-                    "angularVelocity": -1.4609687025358928,
-                    "velocityX": 1.1835476517626813,
-                    "velocityY": -2.959682099439628,
-                    "timestamp": 0.604425736837299
-                },
-                {
-                    "x": 2.243083202431903,
-                    "y": 3.2382839213654937,
-                    "heading": 1.2128218877185057,
-                    "angularVelocity": -0.8991223303884791,
-                    "velocityX": 1.3809315034968324,
-                    "velocityY": -3.123972911053636,
-                    "timestamp": 0.6399801919453755
-                },
-                {
-                    "x": 2.299610340542871,
-                    "y": 3.1217794185343526,
-                    "heading": 1.2005607333325377,
-                    "angularVelocity": -0.3448556404168553,
-                    "velocityX": 1.5898749661368734,
-                    "velocityY": -3.276790559073325,
-                    "timestamp": 0.6755346470534519
-                },
-                {
-                    "x": 2.3661237313117724,
-                    "y": 3.0048767909387704,
-                    "heading": 1.2005598156629014,
-                    "angularVelocity": -0.000025810257353301716,
-                    "velocityX": 1.8707470151551455,
-                    "velocityY": -3.2879881646400775,
-                    "timestamp": 0.7110891021615283
-                },
-                {
-                    "x": 2.4413914156279892,
-                    "y": 2.893838891515528,
-                    "heading": 1.1996733189380515,
-                    "angularVelocity": -0.02493349208011108,
-                    "velocityX": 2.116969141769213,
-                    "velocityY": -3.123037579558325,
-                    "timestamp": 0.7466435572696047
-                },
-                {
-                    "x": 2.522968201294396,
-                    "y": 2.7909984002833275,
-                    "heading": 1.191481222958301,
-                    "angularVelocity": -0.2304098306344059,
-                    "velocityX": 2.2944181093040097,
-                    "velocityY": -2.89247833835709,
-                    "timestamp": 0.7821980123776812
+                    "x": 2.298402468716654,
+                    "y": 4.3526843009931255,
+                    "heading": -0.37533408415570846,
+                    "angularVelocity": 0.4620245693037669,
+                    "velocityX": -0.050327487964169394,
+                    "velocityY": -0.2304685720750862,
+                    "timestamp": 0.03174271850172795
+                },
+                {
+                    "x": 2.2952744131725273,
+                    "y": 4.338010072410149,
+                    "heading": -0.34621912169362346,
+                    "angularVelocity": 0.9172170449263883,
+                    "velocityX": -0.09854403440450425,
+                    "velocityY": -0.462286447903903,
+                    "timestamp": 0.0634854370034559
+                },
+                {
+                    "x": 2.2907034035575107,
+                    "y": 4.3159283160610205,
+                    "heading": -0.3028943022400045,
+                    "angularVelocity": 1.364874260887912,
+                    "velocityX": -0.14400183194038138,
+                    "velocityY": -0.6956479278208441,
+                    "timestamp": 0.09522815550518385
+                },
+                {
+                    "x": 2.2847999846220546,
+                    "y": 4.286378308280002,
+                    "heading": -0.24568561151458213,
+                    "angularVelocity": 1.8022618548662834,
+                    "velocityX": -0.1859771063759033,
+                    "velocityY": -0.9309224028625668,
+                    "timestamp": 0.1269708740069118
+                },
+                {
+                    "x": 2.2777013130983144,
+                    "y": 4.249280679670162,
+                    "heading": -0.1750916915555501,
+                    "angularVelocity": 2.223940585151498,
+                    "velocityX": -0.22363149278956135,
+                    "velocityY": -1.1686972748670326,
+                    "timestamp": 0.15871359250863973
+                },
+                {
+                    "x": 2.2695785209744463,
+                    "y": 4.204529290238701,
+                    "heading": -0.09189311777806415,
+                    "angularVelocity": 2.6210286233977427,
+                    "velocityX": -0.25589465890975394,
+                    "velocityY": -1.409815905623355,
+                    "timestamp": 0.19045631101036767
+                },
+                {
+                    "x": 2.2606569711878857,
+                    "y": 4.151982062664549,
+                    "heading": 0.002706319559794192,
+                    "angularVelocity": 2.9801933105606198,
+                    "velocityX": -0.2810581515277304,
+                    "velocityY": -1.6554104391308149,
+                    "timestamp": 0.2221990295120956
+                },
+                {
+                    "x": 2.251280347316294,
+                    "y": 4.091451352956859,
+                    "heading": 0.10685098530745366,
+                    "angularVelocity": 3.2808993893194844,
+                    "velocityX": -0.2953944814487593,
+                    "velocityY": -1.90691637530654,
+                    "timestamp": 0.2539417480138236
+                },
+                {
+                    "x": 2.242133674900212,
+                    "y": 4.022709367456616,
+                    "heading": 0.21739538615105536,
+                    "angularVelocity": 3.4825120865934673,
+                    "velocityX": -0.28815025454054716,
+                    "velocityY": -2.165598560706175,
+                    "timestamp": 0.2856844665155515
+                },
+                {
+                    "x": 2.2351422077578347,
+                    "y": 3.945850682832356,
+                    "heading": 0.32707167243976176,
+                    "angularVelocity": 3.455163623831899,
+                    "velocityX": -0.22025420229828702,
+                    "velocityY": -2.4213012700873704,
+                    "timestamp": 0.31742718501727946
+                },
+                {
+                    "x": 2.2344515310594897,
+                    "y": 3.864378163543378,
+                    "heading": 0.4256089749961829,
+                    "angularVelocity": 3.1042490122910227,
+                    "velocityX": -0.021758586880562782,
+                    "velocityY": -2.566652231898267,
+                    "timestamp": 0.3491699035190074
+                },
+                {
+                    "x": 2.2416429003575074,
+                    "y": 3.7806290345563274,
+                    "heading": 0.5177286971400864,
+                    "angularVelocity": 2.9020741288711216,
+                    "velocityX": 0.22655177746121863,
+                    "velocityY": -2.638372922674897,
+                    "timestamp": 0.38091262202073534
+                },
+                {
+                    "x": 2.256915898193716,
+                    "y": 3.695399323053411,
+                    "heading": 0.6036240465401915,
+                    "angularVelocity": 2.705985922265222,
+                    "velocityX": 0.4811496480800017,
+                    "velocityY": -2.6850161399464656,
+                    "timestamp": 0.4126553405224633
+                },
+                {
+                    "x": 2.280071045497703,
+                    "y": 3.6084777900686467,
+                    "heading": 0.6811513337134051,
+                    "angularVelocity": 2.4423644486842004,
+                    "velocityX": 0.729463272111561,
+                    "velocityY": -2.7383140791811433,
+                    "timestamp": 0.4443980590241912
+                },
+                {
+                    "x": 2.3096855944046597,
+                    "y": 3.5244760350530098,
+                    "heading": 0.76088689172136,
+                    "angularVelocity": 2.511932240574098,
+                    "velocityX": 0.932955660534971,
+                    "velocityY": -2.6463314731869354,
+                    "timestamp": 0.47614077752591916
+                },
+                {
+                    "x": 2.3348942368332115,
+                    "y": 3.446420745412462,
+                    "heading": 0.8559069862879931,
+                    "angularVelocity": 2.993445396350149,
+                    "velocityX": 0.7941551202420774,
+                    "velocityY": -2.458998262429792,
+                    "timestamp": 0.5078834960276472
+                },
+                {
+                    "x": 2.355075569752848,
+                    "y": 3.373868279566975,
+                    "heading": 0.9657696902827837,
+                    "angularVelocity": 3.4610363944982643,
+                    "velocityX": 0.6357783413709821,
+                    "velocityY": -2.285641220097014,
+                    "timestamp": 0.5396262145293751
+                },
+                {
+                    "x": 2.370025604730125,
+                    "y": 3.306495756130267,
+                    "heading": 1.091029644988018,
+                    "angularVelocity": 3.9461004166489144,
+                    "velocityX": 0.4709752561508733,
+                    "velocityY": -2.1224560030370347,
+                    "timestamp": 0.571368933031103
+                },
+                {
+                    "x": 2.3800631598495494,
+                    "y": 3.2441375159647747,
+                    "heading": 1.2334110423596254,
+                    "angularVelocity": 4.485482154399005,
+                    "velocityX": 0.31621598883778346,
+                    "velocityY": -1.9644895934825695,
+                    "timestamp": 0.603111651532831
+                },
+                {
+                    "x": 2.3890320547539976,
+                    "y": 3.1882487557001293,
+                    "heading": 1.3963738258608078,
+                    "angularVelocity": 5.133863487221998,
+                    "velocityX": 0.28254967840764567,
+                    "velocityY": -1.760679705539431,
+                    "timestamp": 0.6348543700345589
+                },
+                {
+                    "x": 2.399614951757013,
+                    "y": 3.1248820965808815,
+                    "heading": 1.5465533770663946,
+                    "angularVelocity": 4.7311496397956585,
+                    "velocityX": 0.33339605120587323,
+                    "velocityY": -1.9962581061165,
+                    "timestamp": 0.6665970885362869
+                },
+                {
+                    "x": 2.4156041612276775,
+                    "y": 3.056078363317597,
+                    "heading": 1.6846738705269053,
+                    "angularVelocity": 4.351249671731503,
+                    "velocityX": 0.5037126694046519,
+                    "velocityY": -2.167543818263053,
+                    "timestamp": 0.6983398070380148
+                },
+                {
+                    "x": 2.4386535219180887,
+                    "y": 2.9838461830329104,
+                    "heading": 1.8122491249375776,
+                    "angularVelocity": 4.019039969866676,
+                    "velocityX": 0.7261306459671242,
+                    "velocityY": -2.275551171861821,
+                    "timestamp": 0.7300825255397427
+                },
+                {
+                    "x": 2.4695800740474128,
+                    "y": 2.910121142766613,
+                    "heading": 1.9308538821800434,
+                    "angularVelocity": 3.736439814882526,
+                    "velocityX": 0.9742880757878827,
+                    "velocityY": -2.3225811696715355,
+                    "timestamp": 0.7618252440414707
+                },
+                {
+                    "x": 2.508646228883482,
+                    "y": 2.8366969767650785,
+                    "heading": 2.0420489888957944,
+                    "angularVelocity": 3.503011461028351,
+                    "velocityX": 1.2307123233299608,
+                    "velocityY": -2.313102641084062,
+                    "timestamp": 0.7935679625431986
+                },
+                {
+                    "x": 2.555734299118471,
+                    "y": 2.7651139971749186,
+                    "heading": 2.147169748045839,
+                    "angularVelocity": 3.3116495408016253,
+                    "velocityX": 1.4834290337302587,
+                    "velocityY": -2.255099215470886,
+                    "timestamp": 0.8253106810449266
                 },
                 {
                     "x": 2.6104869842529297,
                     "y": 2.6966493129730225,
-                    "heading": 1.1763516697745318,
-                    "angularVelocity": -0.4255318535406987,
-                    "velocityX": 2.4615419556423808,
-                    "velocityY": -2.653650211302861,
-                    "timestamp": 0.8177524674857576
-                },
-                {
-                    "x": 2.7952393085826728,
-                    "y": 2.5500987298885027,
-                    "heading": 1.131439771707853,
-                    "angularVelocity": -0.6752506092749339,
-                    "velocityX": 2.777752108882206,
-                    "velocityY": -2.203388740562692,
-                    "timestamp": 0.884263918863645
-                },
-                {
-                    "x": 2.9911597283478026,
-                    "y": 2.437065890505019,
-                    "heading": 1.0475109017935547,
-                    "angularVelocity": -1.2618709737283185,
-                    "velocityX": 2.9456644789180824,
-                    "velocityY": -1.6994492984566387,
-                    "timestamp": 0.9507753702415324
-                },
-                {
-                    "x": 3.2086737929320703,
-                    "y": 2.3383672587643702,
-                    "heading": 1.0088878551520015,
-                    "angularVelocity": -0.5806976970343763,
-                    "velocityX": 3.2703250354356728,
-                    "velocityY": -1.4839344157427674,
-                    "timestamp": 1.0172868216194197
-                },
-                {
-                    "x": 3.4256073991681717,
-                    "y": 2.2401993070344908,
-                    "heading": 0.9691349707945731,
-                    "angularVelocity": -0.5976848126733959,
-                    "velocityX": 3.2615978413038147,
-                    "velocityY": -1.4759556391595554,
-                    "timestamp": 1.083798272997307
-                },
-                {
-                    "x": 3.641898030119061,
-                    "y": 2.142612874932996,
-                    "heading": 0.9280602960666862,
-                    "angularVelocity": -0.6175579374222298,
-                    "velocityX": 3.251930704714679,
-                    "velocityY": -1.4672124886743745,
-                    "timestamp": 1.1503097243751943
-                },
-                {
-                    "x": 3.8574674811397935,
-                    "y": 2.045673041019181,
-                    "heading": 0.8854247472051884,
-                    "angularVelocity": -0.6410256877310082,
-                    "velocityX": 3.2410877609025053,
-                    "velocityY": -1.4574908817287378,
-                    "timestamp": 1.2168211757530816
-                },
-                {
-                    "x": 4.072209689171901,
-                    "y": 1.949463725749032,
-                    "heading": 0.8409091268268268,
-                    "angularVelocity": -0.6692925722736718,
-                    "velocityX": 3.2286501584823513,
-                    "velocityY": -1.446507530312815,
-                    "timestamp": 1.2833326271309689
-                },
-                {
-                    "x": 4.28597464141242,
-                    "y": 1.8540976935596791,
-                    "heading": 0.7940685595150717,
-                    "angularVelocity": -0.7042481609013259,
-                    "velocityX": 3.2139571128286692,
-                    "velocityY": -1.4338287650276498,
-                    "timestamp": 1.3498440785088561
-                },
-                {
-                    "x": 4.498537652945423,
-                    "y": 1.7597351759166577,
-                    "heading": 0.7442459258691415,
-                    "angularVelocity": -0.7490835429655718,
-                    "velocityX": 3.195885928354179,
-                    "velocityY": -1.4187409188666942,
-                    "timestamp": 1.4163555298867434
-                },
-                {
-                    "x": 4.709533812148138,
-                    "y": 1.6666229170102642,
-                    "heading": 0.6903878152301356,
-                    "angularVelocity": -0.8097569594897124,
-                    "velocityX": 3.1723282958288084,
-                    "velocityY": -1.399943272585839,
-                    "timestamp": 1.4828669812646307
-                },
-                {
-                    "x": 4.924860666444789,
-                    "y": 1.5757431293365876,
-                    "heading": 0.6452055578690052,
-                    "angularVelocity": -0.679315462602457,
-                    "velocityX": 3.2374403179576423,
-                    "velocityY": -1.3663780565746433,
-                    "timestamp": 1.549378432642518
-                },
-                {
-                    "x": 5.15821860461426,
-                    "y": 1.5166188222705783,
-                    "heading": 0.6185000496396688,
-                    "angularVelocity": -0.4015174481399904,
-                    "velocityX": 3.5085377530500543,
-                    "velocityY": -0.8889342487820968,
-                    "timestamp": 1.6158898840204052
-                },
-                {
-                    "x": 5.39961745545984,
-                    "y": 1.4934726786236598,
-                    "heading": 0.5961273041444063,
-                    "angularVelocity": -0.33637433903149927,
-                    "velocityX": 3.6294329148534703,
-                    "velocityY": -0.3480023840618476,
-                    "timestamp": 1.6824013353982925
+                    "heading": 2.247054267099527,
+                    "angularVelocity": 3.1466907614812087,
+                    "velocityX": 1.7248896036260801,
+                    "velocityY": -2.1568626580665695,
+                    "timestamp": 0.8570533995466545
+                },
+                {
+                    "x": 2.751018773357776,
+                    "y": 2.5737681266542305,
+                    "heading": 2.423101617636397,
+                    "angularVelocity": 2.7414576820608,
+                    "velocityX": 2.188399607493837,
+                    "velocityY": -1.9135395743649004,
+                    "timestamp": 0.9212700940770386
+                },
+                {
+                    "x": 2.920945282929504,
+                    "y": 2.4615612619997704,
+                    "heading": 2.5486951706813996,
+                    "angularVelocity": 1.9557772937936253,
+                    "velocityX": 2.646142265876451,
+                    "velocityY": -1.7473161064272718,
+                    "timestamp": 0.9854867886074228
+                },
+                {
+                    "x": 3.118708267205298,
+                    "y": 2.3583606370011725,
+                    "heading": 2.6122511958810777,
+                    "angularVelocity": 0.9897118757741349,
+                    "velocityX": 3.079619493373657,
+                    "velocityY": -1.6070684695514719,
+                    "timestamp": 1.049703483137807
+                },
+                {
+                    "x": 3.3434611246912245,
+                    "y": 2.26616504003377,
+                    "heading": 2.6122519169792926,
+                    "angularVelocity": 0.000011229139401682067,
+                    "velocityX": 3.499913209945475,
+                    "velocityY": -1.4356951512629466,
+                    "timestamp": 1.113920177668191
+                },
+                {
+                    "x": 3.5722970045795748,
+                    "y": 2.184629282343257,
+                    "heading": 2.6122522165022306,
+                    "angularVelocity": 0.000004664253435080619,
+                    "velocityX": 3.5634951559220656,
+                    "velocityY": -1.269697207039109,
+                    "timestamp": 1.1781368721985752
+                },
+                {
+                    "x": 3.80113295504209,
+                    "y": 2.103093722723815,
+                    "heading": 2.6122525160244625,
+                    "angularVelocity": 0.000004664242440601037,
+                    "velocityX": 3.56349625492233,
+                    "velocityY": -1.2696941226220015,
+                    "timestamp": 1.2423535667289594
+                },
+                {
+                    "x": 4.029968923086795,
+                    "y": 2.0215582124500746,
+                    "heading": 2.612252815546594,
+                    "angularVelocity": 0.00000466424087155042,
+                    "velocityX": 3.563496528717025,
+                    "velocityY": -1.2696933541971844,
+                    "timestamp": 1.3065702612593435
+                },
+                {
+                    "x": 4.258804909490432,
+                    "y": 1.940022753702107,
+                    "heading": 2.6122531150686714,
+                    "angularVelocity": 0.000004664240033500949,
+                    "velocityX": 3.5634968146073587,
+                    "velocityY": -1.2696925518237052,
+                    "timestamp": 1.3707869557897276
+                },
+                {
+                    "x": 4.487640916105967,
+                    "y": 1.8584873516805105,
+                    "heading": 2.6122534145907514,
+                    "angularVelocity": 0.000004664240074560689,
+                    "velocityX": 3.563497129352575,
+                    "velocityY": -1.2696916684650783,
+                    "timestamp": 1.4350036503201118
+                },
+                {
+                    "x": 4.716476945604857,
+                    "y": 1.7769520138830965,
+                    "heading": 2.6122537141129194,
+                    "angularVelocity": 0.000004664241438058459,
+                    "velocityX": 3.5634974856984654,
+                    "velocityY": -1.2696906683484759,
+                    "timestamp": 1.499220344850496
+                },
+                {
+                    "x": 4.945312999982612,
+                    "y": 1.6954167459104819,
+                    "heading": 2.612254013635151,
+                    "angularVelocity": 0.000004664242435524347,
+                    "velocityX": 3.563497873118986,
+                    "velocityY": -1.269689581017522,
+                    "timestamp": 1.56343703938088
+                },
+                {
+                    "x": 5.174149135873993,
+                    "y": 1.613881706713074,
+                    "heading": 2.6122543131567744,
+                    "angularVelocity": 0.0000046642329655920085,
+                    "velocityX": 3.563499142471556,
+                    "velocityY": -1.269686018467187,
+                    "timestamp": 1.6276537339112642
+                },
+                {
+                    "x": 5.406651475687904,
+                    "y": 1.5434785463415874,
+                    "heading": 2.61225458420666,
+                    "angularVelocity": 0.000004220863245612629,
+                    "velocityX": 3.6205902766281897,
+                    "velocityY": -1.096337344772168,
+                    "timestamp": 1.6918704284416484
                 },
                 {
                     "x": 5.646764755249023,
                     "y": 1.5066063404083252,
-                    "heading": 0.585736957474559,
-                    "angularVelocity": -0.15621891350429284,
-                    "velocityX": 3.7158608731150236,
-                    "velocityY": 0.19746466980619687,
-                    "timestamp": 1.7489127867761798
-                },
-                {
-                    "x": 5.78386589002804,
-                    "y": 1.5248142647082106,
-                    "heading": 0.5861133793107512,
-                    "angularVelocity": 0.010285140296820981,
-                    "velocityX": 3.7460749363537476,
-                    "velocityY": 0.4975031678094133,
-                    "timestamp": 1.7855113965499783
-                },
-                {
-                    "x": 5.9191942381460025,
-                    "y": 1.554048616452813,
-                    "heading": 0.586114299955562,
-                    "angularVelocity": 0.00002515518530364309,
-                    "velocityX": 3.69763630242715,
-                    "velocityY": 0.7987831211428086,
-                    "timestamp": 1.8221100063237767
-                },
-                {
-                    "x": 6.0517259817313,
-                    "y": 1.5940956419716419,
-                    "heading": 0.5861150757223451,
-                    "angularVelocity": 0.000021196618886827452,
-                    "velocityX": 3.6212234400274688,
-                    "velocityY": 1.0942225884082484,
-                    "timestamp": 1.8587086160975752
-                },
-                {
-                    "x": 6.180598624537076,
-                    "y": 1.644694580267861,
-                    "heading": 0.5861160659567196,
-                    "angularVelocity": 0.00002705661173256264,
-                    "velocityX": 3.521244211249743,
-                    "velocityY": 1.3825371676397282,
-                    "timestamp": 1.8953072258713737
-                },
-                {
-                    "x": 6.304973627271485,
-                    "y": 1.7055153509385155,
-                    "heading": 0.5861182648046722,
-                    "angularVelocity": 0.00006008009501406976,
-                    "velocityX": 3.3983532025703247,
-                    "velocityY": 1.6618328140485057,
-                    "timestamp": 1.9319058356451722
-                },
-                {
-                    "x": 6.427040241028168,
-                    "y": 1.7708422617715522,
-                    "heading": 0.5861256207319434,
-                    "angularVelocity": 0.0002009892538702712,
-                    "velocityX": 3.335280069683768,
-                    "velocityY": 1.784956074473775,
-                    "timestamp": 1.9685044454189706
-                },
-                {
-                    "x": 6.5491066329086145,
-                    "y": 1.8361695861725622,
-                    "heading": 0.5861329781592151,
-                    "angularVelocity": 0.0002010302390486887,
-                    "velocityX": 3.3352740072612326,
-                    "velocityY": 1.7849673745744064,
-                    "timestamp": 2.005103055192769
-                },
-                {
-                    "x": 6.6711729055545455,
-                    "y": 1.9014971320931282,
-                    "heading": 0.586140337461326,
-                    "angularVelocity": 0.00020108146611328895,
-                    "velocityX": 3.3352707493638243,
-                    "velocityY": 1.7849734272511966,
-                    "timestamp": 2.0417016649665674
-                },
-                {
-                    "x": 6.789839361593639,
-                    "y": 1.9681130226753558,
-                    "heading": 0.59368067703471,
-                    "angularVelocity": 0.20602803275828666,
-                    "velocityX": 3.2423760567006434,
-                    "velocityY": 1.820175438191616,
-                    "timestamp": 2.0783002747403656
-                },
-                {
-                    "x": 6.899503815249364,
-                    "y": 2.0364566539738247,
-                    "heading": 0.622995969771871,
-                    "angularVelocity": 0.8009947076773526,
-                    "velocityX": 2.9964103645881037,
-                    "velocityY": 1.8673832618472042,
-                    "timestamp": 2.114898884514164
-                },
-                {
-                    "x": 7.0002752102658246,
-                    "y": 2.1033967313761632,
-                    "heading": 0.6764261486140803,
-                    "angularVelocity": 1.4598964051487269,
-                    "velocityX": 2.753421390574382,
-                    "velocityY": 1.829033338043951,
-                    "timestamp": 2.151497494287962
-                },
-                {
-                    "x": 7.092625779879376,
-                    "y": 2.166299323632935,
-                    "heading": 0.7535384043827013,
-                    "angularVelocity": 2.1069722660292705,
-                    "velocityX": 2.5233354541151716,
-                    "velocityY": 1.7187153458983286,
-                    "timestamp": 2.1880961040617604
-                },
-                {
-                    "x": 7.177366976931162,
-                    "y": 2.223194562450384,
-                    "heading": 0.8522168962516827,
-                    "angularVelocity": 2.696236072322813,
-                    "velocityX": 2.3154212024865664,
-                    "velocityY": 1.5545737712196184,
-                    "timestamp": 2.2246947138355586
-                },
-                {
-                    "x": 7.2553185898803205,
-                    "y": 2.2728300819136558,
-                    "heading": 0.9705292362405307,
-                    "angularVelocity": 3.2327003872576037,
-                    "velocityX": 2.1299063934654243,
-                    "velocityY": 1.3562132488105336,
-                    "timestamp": 2.261293323609357
-                },
-                {
-                    "x": 7.327198001580843,
-                    "y": 2.3144960913605033,
-                    "heading": 1.1077214572134357,
-                    "angularVelocity": 3.748563724710752,
-                    "velocityX": 1.9639929534148923,
-                    "velocityY": 1.1384588022432733,
-                    "timestamp": 2.297891933383155
-                },
-                {
-                    "x": 7.393635990725555,
-                    "y": 2.347893157970517,
-                    "heading": 1.2645826825431576,
-                    "angularVelocity": 4.285988629055024,
-                    "velocityX": 1.8153145585403265,
-                    "velocityY": 0.9125228203046082,
-                    "timestamp": 2.3344905431569534
-                },
-                {
-                    "x": 7.455288023435831,
-                    "y": 2.373088898730826,
-                    "heading": 1.443531300083093,
-                    "angularVelocity": 4.889492214211044,
-                    "velocityX": 1.684545754369454,
-                    "velocityY": 0.6884343672077653,
-                    "timestamp": 2.3710891529307516
-                },
-                {
-                    "x": 7.509792212794572,
-                    "y": 2.3938656260636906,
-                    "heading": 1.6501307836273436,
-                    "angularVelocity": 5.64500905420071,
-                    "velocityX": 1.4892420694559032,
-                    "velocityY": 0.5676917090916096,
-                    "timestamp": 2.40768776270455
-                },
-                {
-                    "x": 7.556730666272478,
-                    "y": 2.4114332825613682,
-                    "heading": 1.888437501864436,
-                    "angularVelocity": 6.51135984973125,
-                    "velocityX": 1.2825201221580125,
-                    "velocityY": 0.48000884750149375,
-                    "timestamp": 2.444286372478348
-                },
-                {
-                    "x": 7.593462104085248,
-                    "y": 2.4287428163755616,
-                    "heading": 2.1429512418884697,
-                    "angularVelocity": 6.954191473312333,
-                    "velocityX": 1.0036293192499064,
-                    "velocityY": 0.4729560472700227,
-                    "timestamp": 2.4808849822521464
-                },
-                {
-                    "x": 7.620818492305724,
-                    "y": 2.4405113575302453,
-                    "heading": 2.4064355131818194,
-                    "angularVelocity": 7.199297266257987,
-                    "velocityX": 0.7474706932737378,
-                    "velocityY": 0.32155705441874516,
-                    "timestamp": 2.5174835920259446
-                },
-                {
-                    "x": 7.6400541302749785,
-                    "y": 2.446864161128428,
-                    "heading": 2.650140298596015,
-                    "angularVelocity": 6.658853626420207,
-                    "velocityX": 0.5255838428875319,
-                    "velocityY": 0.17358046213904924,
-                    "timestamp": 2.554082201799743
-                },
-                {
-                    "x": 7.6561677382990485,
-                    "y": 2.4508382476782002,
-                    "heading": 2.845393638086096,
-                    "angularVelocity": 5.334993342557683,
-                    "velocityX": 0.4402792380273905,
-                    "velocityY": 0.10858572427572141,
-                    "timestamp": 2.590680811573541
-                },
-                {
-                    "x": 7.668208670546753,
-                    "y": 2.453890313590812,
-                    "heading": 2.992258715743694,
-                    "angularVelocity": 4.01285946557297,
-                    "velocityX": 0.3289997167139778,
-                    "velocityY": 0.08339294665767145,
-                    "timestamp": 2.6272794213473394
-                },
-                {
-                    "x": 7.676124567266394,
-                    "y": 2.4559646616612736,
-                    "heading": 3.0905802273604146,
-                    "angularVelocity": 2.686482143021458,
-                    "velocityX": 0.21628954674959436,
-                    "velocityY": 0.056678329676522185,
-                    "timestamp": 2.6638780311211376
+                    "heading": 2.6122548161665473,
+                    "angularVelocity": 0.0000036121430551488755,
+                    "velocityX": 3.7391099202016678,
+                    "velocityY": -0.5741841152508433,
+                    "timestamp": 1.7560871229720325
+                },
+                {
+                    "x": 5.787436421990979,
+                    "y": 1.496717214982701,
+                    "heading": 2.612254832075579,
+                    "angularVelocity": 4.2677202387530557e-7,
+                    "velocityX": 3.773625708136921,
+                    "velocityY": -0.26528339929021244,
+                    "timestamp": 1.7933647115888978
+                },
+                {
+                    "x": 5.926771623718837,
+                    "y": 1.4982765413849686,
+                    "heading": 2.6080354125638485,
+                    "angularVelocity": -0.11318917527356848,
+                    "velocityX": 3.7377740057150746,
+                    "velocityY": 0.04183013065287323,
+                    "timestamp": 1.830642300205763
+                },
+                {
+                    "x": 6.064421169286406,
+                    "y": 1.511276136415188,
+                    "heading": 2.60121232475526,
+                    "angularVelocity": -0.18303458087688948,
+                    "velocityX": 3.6925549820916683,
+                    "velocityY": 0.34872413996054913,
+                    "timestamp": 1.8679198888226283
+                },
+                {
+                    "x": 6.200023778520788,
+                    "y": 1.5356760041928015,
+                    "heading": 2.5932728220304684,
+                    "angularVelocity": -0.2129832700927332,
+                    "velocityX": 3.6376443398227996,
+                    "velocityY": 0.6545452289951588,
+                    "timestamp": 1.9051974774394935
+                },
+                {
+                    "x": 6.333175231231244,
+                    "y": 1.5714002898334831,
+                    "heading": 2.5855391112934316,
+                    "angularVelocity": -0.20746274166290535,
+                    "velocityX": 3.5718901798871894,
+                    "velocityY": 0.9583314523868992,
+                    "timestamp": 1.9424750660563588
+                },
+                {
+                    "x": 6.463368734714764,
+                    "y": 1.6183219287773098,
+                    "heading": 2.5790588590244545,
+                    "angularVelocity": -0.17383775371256402,
+                    "velocityX": 3.4925409157130343,
+                    "velocityY": 1.258709071181673,
+                    "timestamp": 1.979752654673224
+                },
+                {
+                    "x": 6.589861303526444,
+                    "y": 1.6762129661852994,
+                    "heading": 2.574283804517795,
+                    "angularVelocity": -0.12809451157737914,
+                    "velocityX": 3.393260495247066,
+                    "velocityY": 1.5529716260079764,
+                    "timestamp": 2.017030243290089
+                },
+                {
+                    "x": 6.711340738368746,
+                    "y": 1.7445488061219798,
+                    "heading": 2.570091844488755,
+                    "angularVelocity": -0.11245255352014773,
+                    "velocityX": 3.258779318878531,
+                    "velocityY": 1.8331614911851861,
+                    "timestamp": 2.054307831906954
+                },
+                {
+                    "x": 6.825305386557433,
+                    "y": 1.8215960437325822,
+                    "heading": 2.560973344137785,
+                    "angularVelocity": -0.24461078866147912,
+                    "velocityX": 3.057189384216965,
+                    "velocityY": 2.0668514372666396,
+                    "timestamp": 2.091585420523819
+                },
+                {
+                    "x": 6.929260415482823,
+                    "y": 1.9025407238929235,
+                    "heading": 2.5356026772435865,
+                    "angularVelocity": -0.680587662333947,
+                    "velocityX": 2.788673645010342,
+                    "velocityY": 2.171403332771371,
+                    "timestamp": 2.128863009140684
+                },
+                {
+                    "x": 7.023455735639986,
+                    "y": 1.9838668818491878,
+                    "heading": 2.488274688562586,
+                    "angularVelocity": -1.2696097155701718,
+                    "velocityX": 2.5268619471418052,
+                    "velocityY": 2.1816367681967113,
+                    "timestamp": 2.166140597757549
+                },
+                {
+                    "x": 7.110101814938688,
+                    "y": 2.060020595789651,
+                    "heading": 2.4125158730705962,
+                    "angularVelocity": -2.0322885224854694,
+                    "velocityX": 2.3243477519224953,
+                    "velocityY": 2.0428819772427436,
+                    "timestamp": 2.2034181863744142
+                },
+                {
+                    "x": 7.191835709933807,
+                    "y": 2.1285351921147058,
+                    "heading": 2.3080325133818067,
+                    "angularVelocity": -2.8028465242925935,
+                    "velocityX": 2.1925746280203615,
+                    "velocityY": 1.8379567688575535,
+                    "timestamp": 2.2406957749912793
+                },
+                {
+                    "x": 7.274315415796274,
+                    "y": 2.1867028423762775,
+                    "heading": 2.1850264648931392,
+                    "angularVelocity": -3.2997319046816447,
+                    "velocityX": 2.2125815784434106,
+                    "velocityY": 1.5603919786607532,
+                    "timestamp": 2.2779733636081443
+                },
+                {
+                    "x": 7.351412179448119,
+                    "y": 2.2360068924730196,
+                    "heading": 2.043511695780604,
+                    "angularVelocity": -3.796242578000636,
+                    "velocityX": 2.0681800114335913,
+                    "velocityY": 1.322619083639869,
+                    "timestamp": 2.3152509522250093
+                },
+                {
+                    "x": 7.420172175937219,
+                    "y": 2.2789013315216375,
+                    "heading": 1.8838780266002726,
+                    "angularVelocity": -4.282296014933475,
+                    "velocityX": 1.8445398170951257,
+                    "velocityY": 1.1506763350355667,
+                    "timestamp": 2.3525285408418743
+                },
+                {
+                    "x": 7.479537718839642,
+                    "y": 2.3170603162912466,
+                    "heading": 1.7061327210648876,
+                    "angularVelocity": -4.768154597182533,
+                    "velocityX": 1.592526370537947,
+                    "velocityY": 1.0236441300375574,
+                    "timestamp": 2.3898061294587394
+                },
+                {
+                    "x": 7.529281054923766,
+                    "y": 2.3515994880332096,
+                    "heading": 1.5086439015102746,
+                    "angularVelocity": -5.2977895535137876,
+                    "velocityX": 1.3344032682848619,
+                    "velocityY": 0.9265398601007353,
+                    "timestamp": 2.4270837180756044
+                },
+                {
+                    "x": 7.569719945861703,
+                    "y": 2.383438536522646,
+                    "heading": 1.288404786035775,
+                    "angularVelocity": -5.908083748068905,
+                    "velocityX": 1.0848043674059311,
+                    "velocityY": 0.8541069760888933,
+                    "timestamp": 2.4643613066924694
+                },
+                {
+                    "x": 7.603202598509127,
+                    "y": 2.4091886515491456,
+                    "heading": 1.0408456353949866,
+                    "angularVelocity": -6.640964714353526,
+                    "velocityX": 0.8981979223912477,
+                    "velocityY": 0.6907666504707783,
+                    "timestamp": 2.5016388953093345
+                },
+                {
+                    "x": 7.630132008026638,
+                    "y": 2.427100499568469,
+                    "heading": 0.7720308301798764,
+                    "angularVelocity": -7.211164004677401,
+                    "velocityX": 0.7224021326670985,
+                    "velocityY": 0.48049910640464033,
+                    "timestamp": 2.5389164839261995
+                },
+                {
+                    "x": 7.649623319500903,
+                    "y": 2.438227448776781,
+                    "heading": 0.5254269726030665,
+                    "angularVelocity": -6.615338242807932,
+                    "velocityX": 0.5228694289911082,
+                    "velocityY": 0.29848897477445496,
+                    "timestamp": 2.5761940725430645
+                },
+                {
+                    "x": 7.66290885295536,
+                    "y": 2.4458359453096237,
+                    "heading": 0.3183730749926382,
+                    "angularVelocity": -5.554380132752273,
+                    "velocityX": 0.3563946582222695,
+                    "velocityY": 0.2041037742822349,
+                    "timestamp": 2.6134716611599296
+                },
+                {
+                    "x": 7.671732744074589,
+                    "y": 2.451418069701132,
+                    "heading": 0.15988099310288126,
+                    "angularVelocity": -4.251672057404801,
+                    "velocityX": 0.23670766931625417,
+                    "velocityY": 0.14974478228408286,
+                    "timestamp": 2.6507492497767946
+                },
+                {
+                    "x": 7.677275184086734,
+                    "y": 2.4551558590330185,
+                    "heading": 0.05341064749493927,
+                    "angularVelocity": -2.8561489505727446,
+                    "velocityX": 0.14868021827027583,
+                    "velocityY": 0.10026907508163654,
+                    "timestamp": 2.6880268383936596
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": 1.350318302936367,
-                    "velocityX": 0.10589016242853574,
-                    "velocityY": 0.028289007290860937,
-                    "timestamp": 2.700476640894936
+                    "heading": 1.0247603884557974e-38,
+                    "angularVelocity": -1.4327817189005392,
+                    "velocityX": 0.07309528363731636,
+                    "velocityY": 0.04947050051802475,
+                    "timestamp": 2.7253044270105247
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": -2.3806959148386727e-33,
-                    "velocityX": 4.3916568461332404e-35,
-                    "velocityY": 1.2700164208802373e-34,
-                    "timestamp": 2.737075250668734
+                    "heading": 0,
+                    "angularVelocity": -2.6319896813971663e-37,
+                    "velocityX": 5.227147746061896e-36,
+                    "velocityY": -6.467829854732196e-35,
+                    "timestamp": 2.7625820156273897
                 }
             ],
             "constraints": [
@@ -21793,16 +19733,16 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 23
+                    "controlIntervalCount": 27
                 },
                 {
                     "x": 2.7430825233459473,
                     "y": 2.7833950519561768,
-                    "heading": 0,
+                    "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
@@ -21811,7 +19751,7 @@
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -21822,533 +19762,569 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "angularVelocity": 0,
                     "velocityX": 0,
-                    "velocityY": 2.0996459321900828e-33,
+                    "velocityY": -4.609910300520975e-35,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.2983894677520005,
-                    "y": 4.3533419612939515,
-                    "heading": 2.758567124910339,
-                    "angularVelocity": 0.29099245314259387,
-                    "velocityX": -0.05470361814673814,
-                    "velocityY": -0.226148099446212,
-                    "timestamp": 0.02944105531884974
-                },
-                {
-                    "x": 2.295387593245339,
-                    "y": 4.339939574817211,
-                    "heading": 2.775252952592925,
-                    "angularVelocity": 0.5667537220346529,
-                    "velocityX": -0.10196219103394874,
-                    "velocityY": -0.45522778757728155,
-                    "timestamp": 0.05888211063769948
-                },
-                {
-                    "x": 2.2912544329244677,
-                    "y": 4.31970469118274,
-                    "heading": 2.7995524599349486,
-                    "angularVelocity": 0.8253612881351188,
-                    "velocityX": -0.14038764154710928,
-                    "velocityY": -0.687301573103468,
-                    "timestamp": 0.08832316595654921
-                },
-                {
-                    "x": 2.286299680545296,
-                    "y": 4.292550560538348,
-                    "heading": 2.8308875928825827,
-                    "angularVelocity": 1.064334569813193,
-                    "velocityX": -0.1682939801413744,
-                    "velocityY": -0.922321919180889,
-                    "timestamp": 0.11776422127539896
-                },
-                {
-                    "x": 2.280893923712392,
-                    "y": 4.258397506462172,
-                    "heading": 2.868587408224874,
-                    "angularVelocity": 1.2805184778194243,
-                    "velocityX": -0.18361287577360647,
-                    "velocityY": -1.1600485684461586,
-                    "timestamp": 0.1472052765942487
-                },
-                {
-                    "x": 2.2754825429318224,
-                    "y": 4.217182700090041,
-                    "heading": 2.9118655493454413,
-                    "angularVelocity": 1.4699928603733934,
-                    "velocityX": -0.18380389975713432,
-                    "velocityY": -1.3999092738276546,
-                    "timestamp": 0.17664633191309845
-                },
-                {
-                    "x": 2.2706016921590138,
-                    "y": 4.168876446645306,
-                    "heading": 2.959795755168493,
-                    "angularVelocity": 1.6280056982999451,
-                    "velocityX": -0.16578382534010866,
-                    "velocityY": -1.6407785971519722,
-                    "timestamp": 0.2060873872319482
-                },
-                {
-                    "x": 2.2668942123670868,
-                    "y": 4.113508113408168,
-                    "heading": 3.0112851676807657,
-                    "angularVelocity": 1.748898331076693,
-                    "velocityX": -0.12592890274395677,
-                    "velocityY": -1.880650426334687,
-                    "timestamp": 0.23552844255079794
-                },
-                {
-                    "x": 2.2651203704726766,
-                    "y": 4.051204935167073,
-                    "heading": 3.0650426993920417,
-                    "angularVelocity": 1.8259376618492857,
-                    "velocityX": -0.06025062196987639,
-                    "velocityY": -2.1162005765875382,
-                    "timestamp": 0.2649694978696477
-                },
-                {
-                    "x": 2.2661533431015752,
-                    "y": 3.9822449997835685,
-                    "heading": 3.1195407017354904,
-                    "angularVelocity": 1.8510886159897915,
-                    "velocityX": 0.03508612778011459,
-                    "velocityY": -2.3423051462205073,
-                    "timestamp": 0.2944105531884974
-                },
-                {
-                    "x": 2.2709425124463385,
-                    "y": 3.9071174668882147,
-                    "heading": 3.1729959824105403,
-                    "angularVelocity": 1.8156713506402375,
-                    "velocityX": 0.16266975802653263,
-                    "velocityY": -2.5517948348561,
-                    "timestamp": 0.3238516085073471
-                },
-                {
-                    "x": 2.2797199160454222,
-                    "y": 3.825989687229236,
-                    "heading": 3.2244176609184314,
-                    "angularVelocity": 1.7465976661158726,
-                    "velocityX": 0.2981348156179574,
-                    "velocityY": -2.7556002589022834,
-                    "timestamp": 0.35329266382619684
-                },
-                {
-                    "x": 2.292782229397934,
-                    "y": 3.7391196184482327,
-                    "heading": 3.272453259481913,
-                    "angularVelocity": 1.6315854864321535,
-                    "velocityX": 0.4436768047559834,
-                    "velocityY": -2.9506438488767315,
-                    "timestamp": 0.38273371914504656
-                },
-                {
-                    "x": 2.31050581940653,
-                    "y": 3.6469279173399998,
-                    "heading": 3.3152109067306803,
-                    "angularVelocity": 1.4523136750940884,
-                    "velocityX": 0.6020025374990144,
-                    "velocityY": -3.131399337074948,
-                    "timestamp": 0.4121747744638963
-                },
-                {
-                    "x": 2.3333478257637266,
-                    "y": 3.5501341924969156,
-                    "heading": 3.350152812747203,
-                    "angularVelocity": 1.186842850505799,
-                    "velocityX": 0.7758555564606926,
-                    "velocityY": -3.287712474800853,
-                    "timestamp": 0.441615829782746
-                },
-                {
-                    "x": 2.3619457015355163,
-                    "y": 3.450260447984075,
-                    "heading": 3.374288635448379,
-                    "angularVelocity": 0.8198015471858178,
-                    "velocityX": 0.9713604170119489,
-                    "velocityY": -3.3923289580212845,
-                    "timestamp": 0.4710568851015957
-                },
-                {
-                    "x": 2.396816597338257,
-                    "y": 3.3487121183154565,
-                    "heading": 3.38694435959391,
-                    "angularVelocity": 0.42986652511155254,
-                    "velocityX": 1.184430905247282,
-                    "velocityY": -3.4492082083620734,
-                    "timestamp": 0.5004979404204455
-                },
-                {
-                    "x": 2.4384933115651135,
-                    "y": 3.2467782441206525,
-                    "heading": 3.390761607070638,
-                    "angularVelocity": 0.12965729099677573,
-                    "velocityX": 1.4155985162723614,
-                    "velocityY": -3.462303680722349,
-                    "timestamp": 0.5299389957392953
-                },
-                {
-                    "x": 2.487106839227099,
-                    "y": 3.1465743963564163,
-                    "heading": 3.3907619234819757,
-                    "angularVelocity": 0.000010747282459786006,
-                    "velocityX": 1.6512155266004023,
-                    "velocityY": -3.403541302409769,
-                    "timestamp": 0.559380051058145
-                },
-                {
-                    "x": 2.5421170689241235,
-                    "y": 3.049734421677047,
-                    "heading": 3.39076189132422,
-                    "angularVelocity": -0.0000010922759119129131,
-                    "velocityX": 1.8684870192748764,
-                    "velocityY": -3.2892834047754955,
-                    "timestamp": 0.5888211063769948
-                },
-                {
-                    "x": 2.603292311228317,
-                    "y": 2.9566661889623873,
-                    "heading": 3.3907618600542158,
-                    "angularVelocity": -0.0000010621224075785883,
-                    "velocityX": 2.077888908589026,
-                    "velocityY": -3.161171761905971,
-                    "timestamp": 0.6182621616958446
-                },
-                {
-                    "x": 2.670374957735173,
-                    "y": 2.8677616065314258,
-                    "heading": 3.3907618290531647,
-                    "angularVelocity": -0.0000010529870817317445,
-                    "velocityX": 2.2785408260792437,
-                    "velocityY": -3.019748492984228,
-                    "timestamp": 0.6477032170146944
+                    "x": 2.298808348352727,
+                    "y": 4.35514296819253,
+                    "heading": -0.3837399750205753,
+                    "angularVelocity": 0.24884159467028183,
+                    "velocityX": -0.047369219319970686,
+                    "velocityY": -0.19307136062676852,
+                    "timestamp": 0.02515666638336474
+                },
+                {
+                    "x": 2.2965596540188837,
+                    "y": 4.3453750993410925,
+                    "heading": -0.37149402212882315,
+                    "angularVelocity": 0.48678758564965857,
+                    "velocityX": -0.08938761199815681,
+                    "velocityY": -0.38828152755153583,
+                    "timestamp": 0.05031333276672948
+                },
+                {
+                    "x": 2.2934096466067952,
+                    "y": 4.330641358670927,
+                    "heading": -0.3535645425221108,
+                    "angularVelocity": 0.7127128584321681,
+                    "velocityX": -0.1252156133919033,
+                    "velocityY": -0.5856793760205414,
+                    "timestamp": 0.07546999915009422
+                },
+                {
+                    "x": 2.289538880665139,
+                    "y": 4.310886769515238,
+                    "heading": -0.33028900557963653,
+                    "angularVelocity": 0.9252234214095049,
+                    "velocityX": -0.15386640990779057,
+                    "velocityY": -0.7852625961900879,
+                    "timestamp": 0.10062666553345896
+                },
+                {
+                    "x": 2.2851573432175654,
+                    "y": 4.286058471237428,
+                    "heading": -0.3020481691352866,
+                    "angularVelocity": 1.1225985197714665,
+                    "velocityX": -0.17417003432819553,
+                    "velocityY": -0.9869470739663532,
+                    "timestamp": 0.1257833319168237
+                },
+                {
+                    "x": 2.280510060472114,
+                    "y": 4.25610900707676,
+                    "heading": -0.26927550139535,
+                    "angularVelocity": 1.3027428690475438,
+                    "velocityX": -0.18473364771909595,
+                    "velocityY": -1.1905180004483138,
+                    "timestamp": 0.15093999830018845
+                },
+                {
+                    "x": 2.2758836751536,
+                    "y": 4.2210014881511375,
+                    "heading": -0.23246752663964654,
+                    "angularVelocity": 1.463149933889621,
+                    "velocityX": -0.1839029562984313,
+                    "velocityY": -1.3955552930033008,
+                    "timestamp": 0.1760966646835532
+                },
+                {
+                    "x": 2.271613693612169,
+                    "y": 4.180717494993525,
+                    "heading": -0.19219487185489256,
+                    "angularVelocity": 1.6008740653882818,
+                    "velocityX": -0.16973558723403512,
+                    "velocityY": -1.6013247758554696,
+                    "timestamp": 0.20125333106691795
+                },
+                {
+                    "x": 2.268091592763015,
+                    "y": 4.135268758759812,
+                    "heading": -0.1491141249151469,
+                    "angularVelocity": 1.7124982413501877,
+                    "velocityX": -0.1400066604803655,
+                    "velocityY": -1.8066279347634933,
+                    "timestamp": 0.2264099974502827
+                },
+                {
+                    "x": 2.2657701315686967,
+                    "y": 4.084713672798125,
+                    "heading": -0.10398137252966654,
+                    "angularVelocity": 1.7940672940403972,
+                    "velocityX": -0.09228015981695388,
+                    "velocityY": -2.0096099058307106,
+                    "timestamp": 0.25156666383364745
+                },
+                {
+                    "x": 2.265163909208295,
+                    "y": 4.029179243747037,
+                    "heading": -0.057668550112221675,
+                    "angularVelocity": 1.8409761337881332,
+                    "velocityX": -0.02409788129965802,
+                    "velocityY": -2.2075432493635576,
+                    "timestamp": 0.2767233302170122
+                },
+                {
+                    "x": 2.266840363517046,
+                    "y": 3.9688874958283327,
+                    "heading": -0.011179237419756825,
+                    "angularVelocity": 1.8479917801512262,
+                    "velocityX": 0.06664055893588815,
+                    "velocityY": -2.3966509314037507,
+                    "timestamp": 0.3018799966003769
+                },
+                {
+                    "x": 2.2713096946462064,
+                    "y": 3.9041248350083095,
+                    "heading": 0.03446639703260334,
+                    "angularVelocity": 1.8144548151476838,
+                    "velocityX": 0.1776599117328043,
+                    "velocityY": -2.574373719995273,
+                    "timestamp": 0.3270366629837416
+                },
+                {
+                    "x": 2.2787160490905882,
+                    "y": 3.8349972418338227,
+                    "heading": 0.07865637806724814,
+                    "angularVelocity": 1.7565912892125506,
+                    "velocityX": 0.29440921668697884,
+                    "velocityY": -2.74788368701343,
+                    "timestamp": 0.35219332936710634
+                },
+                {
+                    "x": 2.289237241442295,
+                    "y": 3.7616571782461574,
+                    "heading": 0.12059430680114623,
+                    "angularVelocity": 1.667070195025133,
+                    "velocityX": 0.41822681079334173,
+                    "velocityY": -2.915333155436007,
+                    "timestamp": 0.37734999575047107
+                },
+                {
+                    "x": 2.303092280593543,
+                    "y": 3.684333317171128,
+                    "heading": 0.15922184993862323,
+                    "angularVelocity": 1.5354794052927534,
+                    "velocityX": 0.550750204343837,
+                    "velocityY": -3.073692670431113,
+                    "timestamp": 0.4025066621338358
+                },
+                {
+                    "x": 2.3205444941015814,
+                    "y": 3.6033821039148743,
+                    "heading": 0.19314125064776663,
+                    "angularVelocity": 1.3483265307192482,
+                    "velocityX": 0.6937411039317692,
+                    "velocityY": -3.217883165544707,
+                    "timestamp": 0.4276633285172005
+                },
+                {
+                    "x": 2.341904587688066,
+                    "y": 3.5193842561187316,
+                    "heading": 0.22066827104785883,
+                    "angularVelocity": 1.0942236932591152,
+                    "velocityX": 0.8490828339882706,
+                    "velocityY": -3.3389896147642064,
+                    "timestamp": 0.45281999490056524
+                },
+                {
+                    "x": 2.3675851289681646,
+                    "y": 3.4337423994061407,
+                    "heading": 0.23918679984335672,
+                    "angularVelocity": 0.7361280907928144,
+                    "velocityX": 1.0208244959308244,
+                    "velocityY": -3.40434044032252,
+                    "timestamp": 0.47797666128392996
+                },
+                {
+                    "x": 2.3978885412195137,
+                    "y": 3.3469640370766824,
+                    "heading": 0.24945072521405584,
+                    "angularVelocity": 0.40800021808479553,
+                    "velocityX": 1.2045877537807594,
+                    "velocityY": -3.4495175555868487,
+                    "timestamp": 0.5031333276672947
+                },
+                {
+                    "x": 2.4331457577160354,
+                    "y": 3.2598611919071945,
+                    "heading": 0.25313177220508776,
+                    "angularVelocity": 0.1463249118518414,
+                    "velocityX": 1.4015059054023244,
+                    "velocityY": -3.462416038839161,
+                    "timestamp": 0.5282899940506595
+                },
+                {
+                    "x": 2.473498104501041,
+                    "y": 3.173715186571369,
+                    "heading": 0.2532432496800442,
+                    "angularVelocity": 0.004431329384332257,
+                    "velocityX": 1.604041893710102,
+                    "velocityY": -3.424380799229854,
+                    "timestamp": 0.5534466604340242
+                },
+                {
+                    "x": 2.5185817721335604,
+                    "y": 3.089905547060991,
+                    "heading": 0.25324321969105484,
+                    "angularVelocity": -0.0000011920891614956396,
+                    "velocityX": 1.7921161311870697,
+                    "velocityY": -3.3315081669882356,
+                    "timestamp": 0.578603326817389
+                },
+                {
+                    "x": 2.568241478882672,
+                    "y": 3.0087236123099395,
+                    "heading": 0.2532431890633549,
+                    "angularVelocity": -0.0000012174784783024135,
+                    "velocityX": 1.97401778090718,
+                    "velocityY": -3.2270545514223747,
+                    "timestamp": 0.6037599932007538
+                },
+                {
+                    "x": 2.6223245423144714,
+                    "y": 2.9304189809174512,
+                    "heading": 0.25324315880142845,
+                    "angularVelocity": -0.0000012029386552177117,
+                    "velocityX": 2.1498501672527754,
+                    "velocityY": -3.1126791681853656,
+                    "timestamp": 0.6289166595841186
+                },
+                {
+                    "x": 2.6806646810204167,
+                    "y": 2.855232404738762,
+                    "heading": 0.25324312847978453,
+                    "angularVelocity": -0.0000012053124772832063,
+                    "velocityX": 2.319072718813166,
+                    "velocityY": -2.9887336832676343,
+                    "timestamp": 0.6540733259674834
                 },
                 {
                     "x": 2.7430825233459473,
                     "y": 2.7833950519561768,
-                    "heading": 3.3907617949090247,
-                    "angularVelocity": -0.0000011597457911507082,
-                    "velocityX": 2.4695978056269863,
-                    "velocityY": -2.8656090504076794,
-                    "timestamp": 0.6771442723335441
-                },
-                {
-                    "x": 2.8743019890864545,
-                    "y": 2.6600421338324565,
-                    "heading": 3.3907617697237464,
-                    "angularVelocity": -5.290207073739654e-7,
-                    "velocityX": 2.7562853520145434,
-                    "velocityY": -2.5910472919087915,
-                    "timestamp": 0.7247516311451417
-                },
-                {
-                    "x": 3.0177249974626466,
-                    "y": 2.5511186165163586,
-                    "heading": 3.390761748438384,
-                    "angularVelocity": -4.4710235312140834e-7,
-                    "velocityX": 3.0126226691923277,
-                    "velocityY": -2.287955476529456,
-                    "timestamp": 0.7723589899567394
-                },
-                {
-                    "x": 3.171772290811784,
-                    "y": 2.457823792449377,
-                    "heading": 3.3907617293701717,
-                    "angularVelocity": -4.005307751758197e-7,
-                    "velocityX": 3.235787432753145,
-                    "velocityY": -1.95967233629129,
-                    "timestamp": 0.819966348768337
-                },
-                {
-                    "x": 3.3347475520740515,
-                    "y": 2.3811846546739925,
-                    "heading": 3.3907617114029063,
-                    "angularVelocity": -3.7740521255041523e-7,
-                    "velocityX": 3.4233207917967023,
-                    "velocityY": -1.6098170469543955,
-                    "timestamp": 0.8675737075799346
-                },
-                {
-                    "x": 3.5038153046646756,
-                    "y": 2.3191315453490344,
-                    "heading": 3.3907616937093077,
-                    "angularVelocity": -3.7165679655198125e-7,
-                    "velocityX": 3.551294522758467,
-                    "velocityY": -1.3034352434994065,
-                    "timestamp": 0.9151810663915322
-                },
-                {
-                    "x": 3.6728836971491297,
-                    "y": 2.257080179484392,
-                    "heading": 3.3907616760157264,
-                    "angularVelocity": -3.7165643530673955e-7,
-                    "velocityX": 3.551307963828218,
-                    "velocityY": -1.303398621843431,
-                    "timestamp": 0.9627884252031298
-                },
-                {
-                    "x": 3.8419520974106347,
-                    "y": 2.195028834809519,
-                    "heading": 3.3907616583221465,
-                    "angularVelocity": -3.71656403732769e-7,
-                    "velocityX": 3.5513081271863745,
-                    "velocityY": -1.3033981767490248,
-                    "timestamp": 1.0103957840147275
-                },
-                {
-                    "x": 4.011020504671481,
-                    "y": 2.1329775092054373,
-                    "heading": 3.390761640628565,
-                    "angularVelocity": -3.7165644168556193e-7,
-                    "velocityX": 3.5513082742086395,
-                    "velocityY": -1.3033977761640818,
-                    "timestamp": 1.0580031428263252
-                },
-                {
-                    "x": 4.180088918243229,
-                    "y": 2.070926200796389,
-                    "heading": 3.390761622934979,
-                    "angularVelocity": -3.716565361157419e-7,
-                    "velocityX": 3.55130840677009,
-                    "velocityY": -1.3033974149797176,
-                    "timestamp": 1.105610501637923
-                },
-                {
-                    "x": 4.349157337505363,
-                    "y": 2.008874907891698,
-                    "heading": 3.3907616052413863,
-                    "angularVelocity": -3.716566778801587e-7,
-                    "velocityX": 3.551308526297583,
-                    "velocityY": -1.3033970893082818,
-                    "timestamp": 1.1532178604495207
-                },
-                {
-                    "x": 4.518225761904485,
-                    "y": 1.9468236289835406,
-                    "heading": 3.390761587547785,
-                    "angularVelocity": -3.716568562531517e-7,
-                    "velocityX": 3.551308634200803,
-                    "velocityY": -1.303396795308895,
-                    "timestamp": 1.2008252192611184
-                },
-                {
-                    "x": 4.687294190885785,
-                    "y": 1.8847723625602526,
-                    "heading": 3.3907615698541753,
-                    "angularVelocity": -3.716570319734318e-7,
-                    "velocityX": 3.5513087304501405,
-                    "velocityY": -1.3033965330622725,
-                    "timestamp": 1.2484325780727161
-                },
-                {
-                    "x": 4.8563626239355635,
-                    "y": 1.8227211072221874,
-                    "heading": 3.390761552160558,
-                    "angularVelocity": -3.716571921571891e-7,
-                    "velocityX": 3.5513088159091875,
-                    "velocityY": -1.3033963002154438,
-                    "timestamp": 1.2960399368843138
-                },
-                {
-                    "x": 5.025431060602707,
-                    "y": 1.7606698617402123,
-                    "heading": 3.3907615344669337,
-                    "angularVelocity": -3.716573365191525e-7,
-                    "velocityX": 3.551308891892495,
-                    "velocityY": -1.3033960931867248,
-                    "timestamp": 1.3436472956959116
-                },
-                {
-                    "x": 5.1944995004938015,
-                    "y": 1.6986186250424111,
-                    "heading": 3.3907615167733036,
-                    "angularVelocity": -3.7165746535883894e-7,
-                    "velocityX": 3.551308959612086,
-                    "velocityY": -1.3033959086737847,
-                    "timestamp": 1.3912546545075093
-                },
-                {
-                    "x": 5.363567943266909,
-                    "y": 1.6365673961971205,
-                    "heading": 3.3907614990796673,
-                    "angularVelocity": -3.716575836762919e-7,
-                    "velocityX": 3.551309020149218,
-                    "velocityY": -1.3033957437305734,
-                    "timestamp": 1.438862013319107
-                },
-                {
-                    "x": 5.532636388625086,
-                    "y": 1.574516174395274,
-                    "heading": 3.390761481386026,
-                    "angularVelocity": -3.716576912991928e-7,
-                    "velocityX": 3.5513090744490245,
-                    "velocityY": -1.3033955957819334,
-                    "timestamp": 1.4864693721307047
-                },
-                {
-                    "x": 5.701704836310244,
-                    "y": 1.5124649589336634,
-                    "heading": 3.3907614636923804,
-                    "angularVelocity": -3.7165778948932697e-7,
-                    "velocityX": 3.551309123327601,
-                    "velocityY": -1.3033954626042876,
-                    "timestamp": 1.5340767309423025
-                },
-                {
-                    "x": 5.870773286097621,
-                    "y": 1.4504137491998943,
-                    "heading": 3.3907614459987303,
-                    "angularVelocity": -3.716578803925389e-7,
-                    "velocityX": 3.551309167485058,
-                    "velocityY": -1.3033953422900804,
-                    "timestamp": 1.5816840897539002
-                },
-                {
-                    "x": 6.039841737790961,
-                    "y": 1.388362544659232,
-                    "heading": 3.3907614283050767,
-                    "angularVelocity": -3.716579611998838e-7,
-                    "velocityX": 3.551309207520092,
-                    "velocityY": -1.3033952332080676,
-                    "timestamp": 1.629291448565498
-                },
-                {
-                    "x": 6.208910191218329,
-                    "y": 1.3263113448432136,
-                    "heading": 3.390761410611419,
-                    "angularVelocity": -3.7165803844263857e-7,
-                    "velocityX": 3.551309243943619,
-                    "velocityY": -1.303395133966182,
-                    "timestamp": 1.6768988073770956
-                },
-                {
-                    "x": 6.377978646228594,
-                    "y": 1.2642601493400643,
-                    "heading": 3.3907613929177582,
-                    "angularVelocity": -3.7165810831214996e-7,
-                    "velocityX": 3.5513092771926584,
-                    "velocityY": -1.3033950433736952,
-                    "timestamp": 1.7245061661886933
-                },
-                {
-                    "x": 6.547047102688449,
-                    "y": 1.2022089577865573,
-                    "heading": 3.390761375224094,
-                    "angularVelocity": -3.716581745376039e-7,
-                    "velocityX": 3.551309307641484,
-                    "velocityY": -1.3033949604108395,
-                    "timestamp": 1.772113525000291
-                },
-                {
-                    "x": 6.716115560480058,
-                    "y": 1.1401577698616354,
-                    "heading": 3.3907613575304274,
-                    "angularVelocity": -3.716582357126045e-7,
-                    "velocityX": 3.5513093356151892,
-                    "velocityY": -1.3033948841918472,
-                    "timestamp": 1.8197208838118888
-                },
-                {
-                    "x": 6.885184026346865,
-                    "y": 1.0781066039393965,
-                    "heading": 3.3907613398363963,
-                    "angularVelocity": -3.7166587620925834e-7,
-                    "velocityX": 3.5513095052359795,
-                    "velocityY": -1.3033944220220548,
-                    "timestamp": 1.8673282426234865
-                },
-                {
-                    "x": 7.044084736711506,
-                    "y": 1.0199905851581765,
-                    "heading": 3.36350523152736,
-                    "angularVelocity": -0.5725188077939848,
-                    "velocityX": 3.337734214441043,
-                    "velocityY": -1.220736042325084,
-                    "timestamp": 1.9149356014350842
-                },
-                {
-                    "x": 7.185551132214723,
-                    "y": 0.9681799151008591,
-                    "heading": 3.323163851941908,
-                    "angularVelocity": -0.8473769726461788,
-                    "velocityX": 2.9715237105057524,
-                    "velocityY": -1.0882912085577785,
-                    "timestamp": 1.962542960246682
-                },
-                {
-                    "x": 7.3092683658418975,
-                    "y": 0.9228556276324137,
-                    "heading": 3.282206369227669,
-                    "angularVelocity": -0.8603183149967459,
-                    "velocityX": 2.598699795902896,
-                    "velocityY": -0.952043730210126,
-                    "timestamp": 2.0101503190582797
-                },
-                {
-                    "x": 7.415256126479253,
-                    "y": 0.8840204066687376,
-                    "heading": 3.244221221054594,
-                    "angularVelocity": -0.7978839641870918,
-                    "velocityX": 2.226289449427224,
-                    "velocityY": -0.815739875790283,
-                    "timestamp": 2.057757677869877
-                },
-                {
-                    "x": 7.503541109453659,
-                    "y": 0.8516687069454374,
-                    "heading": 3.210907204397792,
-                    "angularVelocity": -0.6997661178524867,
-                    "velocityX": 1.8544398424576745,
-                    "velocityY": -0.6795525005142472,
-                    "timestamp": 2.1053650366814747
-                },
-                {
-                    "x": 7.574144027018988,
-                    "y": 0.8257950136289538,
-                    "heading": 3.1832598866385293,
-                    "angularVelocity": -0.5807362233362916,
-                    "velocityX": 1.4830252996124917,
-                    "velocityY": -0.5434809651776052,
-                    "timestamp": 2.152972395493072
-                },
-                {
-                    "x": 7.627080539080189,
-                    "y": 0.8063947169930168,
-                    "heading": 3.161935751906697,
-                    "angularVelocity": -0.44791677724069034,
-                    "velocityX": 1.1119396955141558,
-                    "velocityY": -0.407506257860514,
-                    "timestamp": 2.2005797543046697
-                },
-                {
-                    "x": 7.662362703782539,
-                    "y": 0.7934640657363813,
-                    "heading": 3.147400394630243,
-                    "angularVelocity": -0.3053174475395025,
-                    "velocityX": 0.7411073746387762,
-                    "velocityY": -0.27161034721139543,
-                    "timestamp": 2.248187113116267
+                    "heading": 0.2532430937019675,
+                    "angularVelocity": -0.0000013824493495341482,
+                    "velocityX": 2.4811650865953045,
+                    "velocityY": -2.8555990562441647,
+                    "timestamp": 0.6792299923508481
+                },
+                {
+                    "x": 2.8747857816965947,
+                    "y": 2.6605842580771375,
+                    "heading": 0.2532430684120768,
+                    "angularVelocity": -5.312693002073383e-7,
+                    "velocityX": 2.7667141273306664,
+                    "velocityY": -2.579908520631989,
+                    "timestamp": 0.7268327631068262
+                },
+                {
+                    "x": 3.0186320127358077,
+                    "y": 2.5522491183421203,
+                    "heading": 0.2532430469824583,
+                    "angularVelocity": -4.5017586399077444e-7,
+                    "velocityX": 3.021803747025554,
+                    "velocityY": -2.2758158404343,
+                    "timestamp": 0.7744355338628043
+                },
+                {
+                    "x": 3.1730376030001257,
+                    "y": 2.4595822138278454,
+                    "heading": 0.2532430277420137,
+                    "angularVelocity": -4.041874945252969e-7,
+                    "velocityX": 3.2436261127705017,
+                    "velocityY": -1.946670394236201,
+                    "timestamp": 0.8220383046187824
+                },
+                {
+                    "x": 3.336302611834685,
+                    "y": 2.38360340882859,
+                    "heading": 0.25324300957525164,
+                    "angularVelocity": -3.816324501860361e-7,
+                    "velocityX": 3.4297375182526793,
+                    "velocityY": -1.5961004746706646,
+                    "timestamp": 0.8696410753747604
+                },
+                {
+                    "x": 3.505316504811254,
+                    "y": 2.3214540241877932,
+                    "heading": 0.25324299166355524,
+                    "angularVelocity": -3.7627424124720095e-7,
+                    "velocityX": 3.550505365390803,
+                    "velocityY": -1.3055833442844713,
+                    "timestamp": 0.9172438461307385
+                },
+                {
+                    "x": 3.6743308817857088,
+                    "y": 2.2593059557844626,
+                    "heading": 0.25324297375187266,
+                    "angularVelocity": -3.76273948990246e-7,
+                    "velocityX": 3.5505155328218008,
+                    "velocityY": -1.3055556938463668,
+                    "timestamp": 0.9648466168867166
+                },
+                {
+                    "x": 3.843345266545321,
+                    "y": 2.1971579085532174,
+                    "heading": 0.2532429558401917,
+                    "angularVelocity": -3.762739157561073e-7,
+                    "velocityX": 3.5505156963660025,
+                    "velocityY": -1.3055552490805507,
+                    "timestamp": 1.0124493876426945
+                },
+                {
+                    "x": 4.012359658311276,
+                    "y": 2.135009880376043,
+                    "heading": 0.25324293792850894,
+                    "angularVelocity": -3.7627395316214525e-7,
+                    "velocityX": 3.5505158435494915,
+                    "velocityY": -1.3055548488082471,
+                    "timestamp": 1.0600521583986726
+                },
+                {
+                    "x": 4.181374056391421,
+                    "y": 2.07286186937061,
+                    "heading": 0.25324292001682175,
+                    "angularVelocity": -3.7627404706616014e-7,
+                    "velocityX": 3.5505159761928384,
+                    "velocityY": -1.30555448807838,
+                    "timestamp": 1.1076549291546507
+                },
+                {
+                    "x": 4.3503884601627245,
+                    "y": 2.010713873842556,
+                    "heading": 0.25324290210512795,
+                    "angularVelocity": -3.7627418619029544e-7,
+                    "velocityX": 3.5505160957480255,
+                    "velocityY": -1.30555416294226,
+                    "timestamp": 1.1552576999106288
+                },
+                {
+                    "x": 4.51940286907005,
+                    "y": 1.9485658922821711,
+                    "heading": 0.2532428841934256,
+                    "angularVelocity": -3.7627436389492574e-7,
+                    "velocityX": 3.5505162036413855,
+                    "velocityY": -1.3055538695209365,
+                    "timestamp": 1.2028604706666068
+                },
+                {
+                    "x": 4.688417282561973,
+                    "y": 1.8864179231898264,
+                    "heading": 0.25324286628171494,
+                    "angularVelocity": -3.7627454024029507e-7,
+                    "velocityX": 3.550516299950831,
+                    "velocityY": -1.3055536076025602,
+                    "timestamp": 1.250463241422585
+                },
+                {
+                    "x": 4.857431700124039,
+                    "y": 1.824269965166445,
+                    "heading": 0.2532428483699966,
+                    "angularVelocity": -3.7627470129766985e-7,
+                    "velocityX": 3.5505163854530717,
+                    "velocityY": -1.3055533750748494,
+                    "timestamp": 1.298066012178563
+                },
+                {
+                    "x": 5.02644612130461,
+                    "y": 1.7621220169837726,
+                    "heading": 0.2532428304582713,
+                    "angularVelocity": -3.76274846815446e-7,
+                    "velocityX": 3.5505164614676405,
+                    "velocityY": -1.3055531683492934,
+                    "timestamp": 1.345668782934541
+                },
+                {
+                    "x": 5.1954605457099055,
+                    "y": 1.6999740775709098,
+                    "heading": 0.25324281254653974,
+                    "angularVelocity": -3.7627497879654404e-7,
+                    "velocityX": 3.5505165292100247,
+                    "velocityY": -1.3055529841203253,
+                    "timestamp": 1.3932715536905191
+                },
+                {
+                    "x": 5.364474972997734,
+                    "y": 1.6378261459972472,
+                    "heading": 0.25324279463480254,
+                    "angularVelocity": -3.7627509767034604e-7,
+                    "velocityX": 3.550516589763906,
+                    "velocityY": -1.3055528194408317,
+                    "timestamp": 1.4408743244464972
+                },
+                {
+                    "x": 5.533489402870974,
+                    "y": 1.5756782214547507,
+                    "heading": 0.2532427767230601,
+                    "angularVelocity": -3.762752068122207e-7,
+                    "velocityX": 3.5505166440761182,
+                    "velocityY": -1.3055526717358554,
+                    "timestamp": 1.4884770952024753
+                },
+                {
+                    "x": 5.702503835071411,
+                    "y": 1.513530303241192,
+                    "heading": 0.25324275881131303,
+                    "angularVelocity": -3.7627530496428226e-7,
+                    "velocityX": 3.550516692963962,
+                    "velocityY": -1.3055525387827163,
+                    "timestamp": 1.5360798659584534
+                },
+                {
+                    "x": 5.871518269374186,
+                    "y": 1.4513823907450571,
+                    "heading": 0.2532427408995616,
+                    "angularVelocity": -3.762753964251323e-7,
+                    "velocityX": 3.5505167371281625,
+                    "velocityY": -1.3055524186757583,
+                    "timestamp": 1.5836826367144314
+                },
+                {
+                    "x": 6.040532705582966,
+                    "y": 1.3892344834324082,
+                    "heading": 0.25324272298780626,
+                    "angularVelocity": -3.7627547903960503e-7,
+                    "velocityX": 3.550516777167954,
+                    "velocityY": -1.3055523097853368,
+                    "timestamp": 1.6312854074704095
+                },
+                {
+                    "x": 6.20954714352589,
+                    "y": 1.3270865808358587,
+                    "heading": 0.2532427050760471,
+                    "angularVelocity": -3.7627555795965075e-7,
+                    "velocityX": 3.5505168135974197,
+                    "velocityY": -1.3055522107133783,
+                    "timestamp": 1.6788881782263876
+                },
+                {
+                    "x": 6.378561583051887,
+                    "y": 1.2649386825445668,
+                    "heading": 0.25324268716428455,
+                    "angularVelocity": -3.7627562961930894e-7,
+                    "velocityX": 3.5505168468533315,
+                    "velocityY": -1.3055521202720615,
+                    "timestamp": 1.7264909489823657
+                },
+                {
+                    "x": 6.547576024027691,
+                    "y": 1.2027907881961044,
+                    "heading": 0.25324266925251887,
+                    "angularVelocity": -3.7627569544162935e-7,
+                    "velocityX": 3.5505168773096716,
+                    "velocityY": -1.3055520374443248,
+                    "timestamp": 1.7740937197383437
+                },
+                {
+                    "x": 6.716590466335514,
+                    "y": 1.1406428974701475,
+                    "heading": 0.25324265134075025,
+                    "angularVelocity": -3.762757576316506e-7,
+                    "velocityX": 3.550516905291665,
+                    "velocityY": -1.3055519613457025,
+                    "timestamp": 1.8216964904943218
+                },
+                {
+                    "x": 6.88560491686231,
+                    "y": 1.0784950290966298,
+                    "heading": 0.253242633428611,
+                    "angularVelocity": -3.7628354454135297e-7,
+                    "velocityX": 3.550517077949107,
+                    "velocityY": -1.3055514917839766,
+                    "timestamp": 1.8692992612502999
+                },
+                {
+                    "x": 7.0444139114910005,
+                    "y": 1.0203058482073266,
+                    "heading": 0.2259027129119336,
+                    "angularVelocity": -0.574334646544581,
+                    "velocityX": 3.336129223292024,
+                    "velocityY": -1.2223906290579827,
+                    "timestamp": 1.916902032006278
+                },
+                {
+                    "x": 7.185810534595712,
+                    "y": 0.9684234171781619,
+                    "heading": 0.18518533646408028,
+                    "angularVelocity": -0.8553572786042075,
+                    "velocityX": 2.9703443908662663,
+                    "velocityY": -1.089903596055891,
+                    "timestamp": 1.964504802762256
+                },
+                {
+                    "x": 7.3094655243228,
+                    "y": 0.9230369546143523,
+                    "heading": 0.14380134057962407,
+                    "angularVelocity": -0.8693610734677483,
+                    "velocityX": 2.59764269523238,
+                    "velocityY": -0.953441613650398,
+                    "timestamp": 2.012107573518234
+                },
+                {
+                    "x": 7.415398593682274,
+                    "y": 0.8841490900710189,
+                    "heading": 0.10540313701630608,
+                    "angularVelocity": -0.8066379950897271,
+                    "velocityX": 2.2253551143589885,
+                    "velocityY": -0.8169243917057022,
+                    "timestamp": 2.059710344274212
+                },
+                {
+                    "x": 7.503637039429732,
+                    "y": 0.8517540010892736,
+                    "heading": 0.07171800478429124,
+                    "angularVelocity": -0.70762965468317,
+                    "velocityX": 1.8536409613588987,
+                    "velocityY": -0.680529483206126,
+                    "timestamp": 2.1073131150301903
+                },
+                {
+                    "x": 7.574202084965293,
+                    "y": 0.8258459262172375,
+                    "heading": 0.04375776286271558,
+                    "angularVelocity": -0.5873658502969467,
+                    "velocityX": 1.482372652157004,
+                    "velocityY": -0.5442556065663985,
+                    "timestamp": 2.1549158857861683
+                },
+                {
+                    "x": 7.627109788299601,
+                    "y": 0.8064200554899893,
+                    "heading": 0.022189516524924582,
+                    "angularVelocity": -0.4530880449870122,
+                    "velocityX": 1.1114416764840027,
+                    "velocityY": -0.40808277372822227,
+                    "timestamp": 2.2025186565421464
+                },
+                {
+                    "x": 7.66237251850946,
+                    "y": 0.7934724766877013,
+                    "heading": 0.0074863600733853135,
+                    "angularVelocity": -0.3088718622474896,
+                    "velocityX": 0.7407705402406998,
+                    "velocityY": -0.27199212559831976,
+                    "timestamp": 2.2501214272981245
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
-                    "angularVelocity": -0.1554464438896817,
-                    "velocityX": 0.370474158989983,
-                    "velocityY": -0.13577870937899197,
-                    "timestamp": 2.2957944719278647
+                    "heading": -1.0670900456397286e-32,
+                    "angularVelocity": -0.1572673177316084,
+                    "velocityX": 0.37030368633165434,
+                    "velocityY": -0.13596848639085585,
+                    "timestamp": 2.2977241980541026
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
-                    "angularVelocity": 1.845639156363882e-34,
-                    "velocityX": 1.6232927575664752e-34,
-                    "velocityY": 1.3237538522972492e-34,
-                    "timestamp": 2.343401830739462
+                    "heading": 0,
+                    "angularVelocity": 2.230662363627099e-31,
+                    "velocityX": 2.3924515989499496e-33,
+                    "velocityY": 1.2585115119762557e-33,
+                    "timestamp": 2.3453269688100806
                 }
             ],
             "constraints": [
@@ -22381,7 +20357,7 @@
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": -2.88,
+                    "heading": 0.26,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -22390,7 +20366,7 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -22401,128 +20377,128 @@
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": -2.88,
-                    "angularVelocity": -6.0746648103992426e-33,
-                    "velocityX": -3.2850296304780664e-35,
-                    "velocityY": -2.0665663458795258e-33,
+                    "heading": 0.26,
+                    "angularVelocity": 2.3848638783294046e-33,
+                    "velocityX": 3.9311043555760955e-34,
+                    "velocityY": 6.649026992883858e-33,
                     "timestamp": 0
                 },
                 {
-                    "x": 5.102979054151253,
-                    "y": 6.633224554548809,
-                    "heading": -2.8734987043090925,
-                    "angularVelocity": 0.06942752469656709,
-                    "velocityX": -0.7797927924375683,
-                    "velocityY": 0.03443511127885569,
-                    "timestamp": 0.09364147316685267
-                },
-                {
-                    "x": 4.9569371637209425,
-                    "y": 6.639673650012151,
-                    "heading": -2.8604963348705175,
-                    "angularVelocity": 0.1388526792548496,
-                    "velocityX": -1.5595855713428277,
-                    "velocityY": 0.0688700769568818,
-                    "timestamp": 0.18728294633370535
-                },
-                {
-                    "x": 4.737874332155748,
-                    "y": 6.649347273486247,
-                    "heading": -2.8409932661051047,
-                    "angularVelocity": 0.2082738353622199,
-                    "velocityX": -2.3393783134408976,
-                    "velocityY": 0.10330490483508797,
-                    "timestamp": 0.280924419500558
-                },
-                {
-                    "x": 4.445790573832511,
-                    "y": 6.662245412808618,
-                    "heading": -2.8149900541710062,
-                    "angularVelocity": 0.27768905224050994,
-                    "velocityX": -3.119170902008226,
-                    "velocityY": 0.13773960282991304,
-                    "timestamp": 0.3745658926674107
-                },
-                {
-                    "x": 4.091895329085733,
-                    "y": 6.677873079084158,
-                    "heading": -2.814990051178798,
-                    "angularVelocity": 3.195386469208516e-8,
-                    "velocityX": -3.7792575530737516,
-                    "velocityY": 0.16688830009804687,
-                    "timestamp": 0.46820736583426337
-                },
-                {
-                    "x": 3.738000080960507,
-                    "y": 6.693500668821502,
-                    "heading": -2.8149900481866714,
-                    "angularVelocity": 3.195300400483476e-8,
-                    "velocityX": -3.7792575891522513,
-                    "velocityY": 0.16688748274491266,
-                    "timestamp": 0.561848839001116
-                },
-                {
-                    "x": 3.384104829826614,
-                    "y": 6.709128190425974,
-                    "heading": -2.8149900451945484,
-                    "angularVelocity": 3.1952965397811645e-8,
-                    "velocityX": -3.7792576212818934,
-                    "velocityY": 0.1668867551519713,
-                    "timestamp": 0.6554903121679687
-                },
-                {
-                    "x": 3.0302095759842613,
-                    "y": 6.724755650696981,
-                    "heading": -2.8149900422022327,
-                    "angularVelocity": 3.195501290053148e-8,
-                    "velocityX": -3.779257650205636,
-                    "velocityY": 0.16688610017037783,
-                    "timestamp": 0.7491317853348214
-                },
-                {
-                    "x": 2.738125764107778,
-                    "y": 6.7376533993595,
-                    "heading": -2.7889938425825695,
-                    "angularVelocity": 0.2776141675314341,
-                    "velocityX": -3.1191714739049314,
-                    "velocityY": 0.1377354309613996,
-                    "timestamp": 0.8427732585016741
-                },
-                {
-                    "x": 2.519062886855627,
-                    "y": 6.747326704296012,
-                    "heading": -2.7694968873701145,
-                    "angularVelocity": 0.20820854855310947,
-                    "velocityX": -2.339378801333246,
-                    "velocityY": 0.10330150316261463,
-                    "timestamp": 0.9364147316685267
-                },
-                {
-                    "x": 2.3730209632759087,
-                    "y": 6.753775569660814,
-                    "heading": -2.756498972823295,
-                    "angularVelocity": 0.13880510533677407,
-                    "velocityX": -1.559585925346323,
-                    "velocityY": 0.0688676197276826,
-                    "timestamp": 1.0300562048353794
+                    "x": 5.1029790541350515,
+                    "y": 6.633224554642693,
+                    "heading": 0.2665012951102971,
+                    "angularVelocity": 0.06942751851370552,
+                    "velocityX": -0.7797927928070008,
+                    "velocityY": 0.03443511229011806,
+                    "timestamp": 0.09364147314326547
+                },
+                {
+                    "x": 4.956937163671988,
+                    "y": 6.639673650268821,
+                    "heading": 0.2795036636182218,
+                    "angularVelocity": 0.13885266935138055,
+                    "velocityX": -1.5595855720854526,
+                    "velocityY": 0.06887007871263316,
+                    "timestamp": 0.18728294628653094
+                },
+                {
+                    "x": 4.737874332057849,
+                    "y": 6.649347273946163,
+                    "heading": 0.29900673147988355,
+                    "angularVelocity": 0.20827382576349504,
+                    "velocityX": -2.3393783145528313,
+                    "velocityY": 0.10330490703159452,
+                    "timestamp": 0.2809244194297964
+                },
+                {
+                    "x": 4.445790573670186,
+                    "y": 6.662245413482842,
+                    "heading": 0.325009942942769,
+                    "angularVelocity": 0.27768904727835797,
+                    "velocityX": -3.1191709034819257,
+                    "velocityY": 0.13773960515320766,
+                    "timestamp": 0.3745658925730619
+                },
+                {
+                    "x": 4.091895329009691,
+                    "y": 6.677873079689744,
+                    "heading": 0.32500994593496846,
+                    "angularVelocity": 3.195376913904642e-8,
+                    "velocityX": -3.7792575531042867,
+                    "velocityY": 0.16688829940708347,
+                    "timestamp": 0.4682073657163274
+                },
+                {
+                    "x": 3.738000080970794,
+                    "y": 6.693500669359476,
+                    "heading": 0.3250099489270859,
+                    "angularVelocity": 3.1952908210754574e-8,
+                    "velocityX": -3.779257589182282,
+                    "velocityY": 0.1668874820649196,
+                    "timestamp": 0.5618488388595928
+                },
+                {
+                    "x": 3.3841048299235403,
+                    "y": 6.709128190903334,
+                    "heading": 0.3250099519191997,
+                    "angularVelocity": 3.195286925539051e-8,
+                    "velocityX": -3.779257621308623,
+                    "velocityY": 0.16688675454671412,
+                    "timestamp": 0.6554903120028582
+                },
+                {
+                    "x": 3.030209576168151,
+                    "y": 6.724755651121081,
+                    "heading": 0.32500995491150664,
+                    "angularVelocity": 3.195491758844704e-8,
+                    "velocityX": -3.7792576502288973,
+                    "velocityY": 0.16688609964363924,
+                    "timestamp": 0.7491317851461237
+                },
+                {
+                    "x": 2.7381257642201002,
+                    "y": 6.737653399625744,
+                    "heading": 0.3510061559504441,
+                    "angularVelocity": 0.2776141827578367,
+                    "velocityX": -3.119171475454889,
+                    "velocityY": 0.13773542931034466,
+                    "timestamp": 0.8427732582893891
+                },
+                {
+                    "x": 2.519062886912394,
+                    "y": 6.747326704432178,
+                    "heading": 0.37050311197995217,
+                    "angularVelocity": 0.20820855733088625,
+                    "velocityX": -2.339378802515782,
+                    "velocityY": 0.10330150179952581,
+                    "timestamp": 0.9364147314326545
+                },
+                {
+                    "x": 2.373020963294867,
+                    "y": 6.7537755697062725,
+                    "heading": 0.383501026964712,
+                    "angularVelocity": 0.13880511004851873,
+                    "velocityX": -1.5595859261429292,
+                    "velocityY": 0.06886761877636173,
+                    "timestamp": 1.03005620457592
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
-                    "angularVelocity": 0.06940271872607656,
-                    "velocityX": -0.779792978542731,
-                    "velocityY": 0.034433784840577714,
-                    "timestamp": 1.123697678002232
+                    "heading": 0.39,
+                    "angularVelocity": 0.06940272100743458,
+                    "velocityX": -0.7797929789416104,
+                    "velocityY": 0.034433784363791235,
+                    "timestamp": 1.1236976777191856
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 0.39,
                     "angularVelocity": 0,
-                    "velocityX": -2.0910816279231796e-36,
-                    "velocityY": 1.251790530664498e-31,
-                    "timestamp": 1.2173391511690848
+                    "velocityX": 0,
+                    "velocityY": 2.0630865476520398e-33,
+                    "timestamp": 1.2173391508624511
                 }
             ],
             "constraints": [
@@ -22565,7 +20541,7 @@
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": -2.88,
+                    "heading": 0.26,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -22574,7 +20550,7 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": -3.143185307179586,
+                    "heading": -0.003,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -22585,146 +20561,146 @@
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": -2.88,
+                    "heading": 0.26,
                     "angularVelocity": 0,
-                    "velocityX": -8.07520618943781e-35,
-                    "velocityY": -1.6093142036511268e-34,
+                    "velocityX": -1.9284905870694258e-35,
+                    "velocityY": -6.933375493218394e-34,
                     "timestamp": 0
                 },
                 {
-                    "x": 5.118924952955758,
-                    "y": 6.609128496946765,
-                    "heading": -2.888779485010293,
-                    "angularVelocity": -0.10279588776958423,
-                    "velocityX": -0.6682715584712982,
-                    "velocityY": -0.24437705434061835,
-                    "timestamp": 0.08540696715389907
-                },
-                {
-                    "x": 5.004774858765426,
-                    "y": 6.567385488245384,
-                    "heading": -2.906338067516999,
-                    "angularVelocity": -0.20558723827608577,
-                    "velocityX": -1.3365431181350638,
-                    "velocityY": -0.48875413906409965,
-                    "timestamp": 0.17081393430779815
-                },
-                {
-                    "x": 4.833549713525512,
-                    "y": 6.504770974662745,
-                    "heading": -2.9326749312069125,
-                    "angularVelocity": -0.3083690308596943,
-                    "velocityX": -2.004814723503466,
-                    "velocityY": -0.7331312148084055,
-                    "timestamp": 0.25622090146169724
-                },
-                {
-                    "x": 4.6052495100619275,
-                    "y": 6.421284959128349,
-                    "heading": -2.96778862782457,
-                    "angularVelocity": -0.4111338663318194,
-                    "velocityX": -2.673086412870736,
-                    "velocityY": -0.9775082562521819,
-                    "timestamp": 0.3416278686155963
-                },
-                {
-                    "x": 4.319874242645404,
-                    "y": 6.316927450773692,
-                    "heading": -3.011676691839701,
-                    "angularVelocity": -0.513869833781204,
-                    "velocityX": -3.3413581693199683,
-                    "velocityY": -1.2218851907785282,
-                    "timestamp": 0.42703483576949536
-                },
-                {
-                    "x": 4.016437344871394,
-                    "y": 6.205964133135793,
-                    "heading": -3.0116766970908038,
-                    "angularVelocity": -6.148330724628331e-8,
-                    "velocityX": -3.5528354171297467,
-                    "velocityY": -1.2992302775246547,
-                    "timestamp": 0.5124418029233945
-                },
-                {
-                    "x": 3.7130004778936962,
-                    "y": 6.095000731283241,
-                    "heading": -3.011676702341878,
-                    "angularVelocity": -6.148297185895772e-8,
-                    "velocityX": -3.552835056546619,
-                    "velocityY": -1.299231263564264,
-                    "timestamp": 0.5978487700772935
-                },
-                {
-                    "x": 3.4095636397411098,
-                    "y": 5.984037250606526,
-                    "heading": -3.011676707592942,
-                    "angularVelocity": -6.148285239694146e-8,
-                    "velocityX": -3.5528347190435716,
-                    "velocityY": -1.2992321864885386,
-                    "timestamp": 0.6832557372311926
-                },
-                {
-                    "x": 3.106126829490984,
-                    "y": 5.873073693628802,
-                    "heading": -3.0116767128440194,
-                    "angularVelocity": -6.14830072681285e-8,
-                    "velocityX": -3.5528343923435184,
-                    "velocityY": -1.2992330798700915,
-                    "timestamp": 0.7686627043850917
-                },
-                {
-                    "x": 2.8207512525311835,
-                    "y": 5.768715824967853,
-                    "heading": -3.055515171895212,
-                    "angularVelocity": -0.5132890267862834,
-                    "velocityX": -3.3413617936528297,
-                    "velocityY": -1.221889409477577,
-                    "timestamp": 0.8540696715389907
-                },
-                {
-                    "x": 2.5924507605120093,
-                    "y": 5.68522951062146,
-                    "heading": -3.090584080219249,
-                    "angularVelocity": -0.4106094560276795,
-                    "velocityX": -2.6730897914661687,
-                    "velocityY": -0.9775117549362862,
-                    "timestamp": 0.9394766386928898
-                },
-                {
-                    "x": 2.421225380941837,
-                    "y": 5.6226147624914855,
-                    "heading": -3.116884817520286,
-                    "angularVelocity": -0.3079460397375414,
-                    "velocityX": -2.004817467193667,
-                    "velocityY": -0.7331339610402823,
-                    "timestamp": 1.024883605846789
-                },
-                {
-                    "x": 2.307075126087855,
-                    "y": 5.580871589665102,
-                    "heading": -3.1344184120174683,
-                    "angularVelocity": -0.20529466250203854,
-                    "velocityX": -1.3365449992889762,
-                    "velocityY": -0.48875606074577516,
-                    "timestamp": 1.110290573000688
+                    "x": 5.118924935599245,
+                    "y": 6.609128491648739,
+                    "heading": 0.2512266763512042,
+                    "angularVelocity": -0.10272378997697001,
+                    "velocityX": -0.6682720441106688,
+                    "velocityY": -0.24437721964946355,
+                    "timestamp": 0.08540693106010512
+                },
+                {
+                    "x": 5.004774806713452,
+                    "y": 6.567385472222137,
+                    "heading": 0.23368042064504363,
+                    "angularVelocity": -0.2054429949463047,
+                    "velocityX": -1.3365440892081688,
+                    "velocityY": -0.4887544711942054,
+                    "timestamp": 0.17081386212021024
+                },
+                {
+                    "x": 4.833549609463453,
+                    "y": 6.504770942290587,
+                    "heading": 0.2073620554535925,
+                    "angularVelocity": -0.3081525687058067,
+                    "velocityX": -2.0048161797255077,
+                    "velocityY": -0.7331317160604409,
+                    "timestamp": 0.2562207931803154
+                },
+                {
+                    "x": 4.605249336717623,
+                    "y": 6.421284904520194,
+                    "heading": 0.17227303630641458,
+                    "angularVelocity": -0.4108451001767527,
+                    "velocityX": -2.673088353744534,
+                    "velocityY": -0.9775089297101582,
+                    "timestamp": 0.3416277242404205
+                },
+                {
+                    "x": 4.319873982796504,
+                    "y": 6.316927367787122,
+                    "heading": 0.12841583542221255,
+                    "angularVelocity": -0.5135086853002309,
+                    "velocityX": -3.341360594262375,
+                    "velocityY": -1.22188603943198,
+                    "timestamp": 0.4270346553005256
+                },
+                {
+                    "x": 4.016437213590231,
+                    "y": 6.205964096134007,
+                    "heading": 0.12841583017477892,
+                    "angularVelocity": -6.144037201192911e-8,
+                    "velocityX": -3.552835413237479,
+                    "velocityY": -1.2992302881721054,
+                    "timestamp": 0.5124415863606308
+                },
+                {
+                    "x": 3.7130004751736263,
+                    "y": 6.095000740284403,
+                    "heading": 0.12841582492737386,
+                    "angularVelocity": -6.144003768621477e-8,
+                    "velocityX": -3.5528350527319725,
+                    "velocityY": -1.2992312739994434,
+                    "timestamp": 0.5978485174207359
+                },
+                {
+                    "x": 3.409563765577162,
+                    "y": 5.984037305624233,
+                    "heading": 0.12841581967997912,
+                    "angularVelocity": -6.143991704551595e-8,
+                    "velocityX": -3.5528347152869912,
+                    "velocityY": -1.2992321967649234,
+                    "timestamp": 0.683255448480841
+                },
+                {
+                    "x": 3.1061270838779644,
+                    "y": 5.873073794677257,
+                    "heading": 0.12841581443257097,
+                    "angularVelocity": -6.1440073698458e-8,
+                    "velocityX": -3.5528343886476135,
+                    "velocityY": -1.2992330899805498,
+                    "timestamp": 0.7686623795409461
+                },
+                {
+                    "x": 2.8207514220046077,
+                    "y": 5.76871589253347,
+                    "heading": 0.08460828566897327,
+                    "angularVelocity": -0.512927091745846,
+                    "velocityX": -3.3413641999678414,
+                    "velocityY": -1.2218903178987286,
+                    "timestamp": 0.8540693106010512
+                },
+                {
+                    "x": 2.592450862147759,
+                    "y": 5.685229551240001,
+                    "heading": 0.0495641189011112,
+                    "angularVelocity": -0.4103199392939169,
+                    "velocityX": -2.673091715427432,
+                    "velocityY": -0.9775124835561102,
+                    "timestamp": 0.9394762416611563
+                },
+                {
+                    "x": 2.421225431748781,
+                    "y": 5.622614782821321,
+                    "heading": 0.02328193601640482,
+                    "angularVelocity": -0.30772892268205126,
+                    "velocityX": -2.004818909585662,
+                    "velocityY": -0.7331345084231428,
+                    "timestamp": 1.0248831727212615
+                },
+                {
+                    "x": 2.3070751430245986,
+                    "y": 5.580871596442558,
+                    "heading": 0.005760710543037936,
+                    "angularVelocity": -0.2051499246710589,
+                    "velocityX": -1.3365459607001804,
+                    "velocityY": -0.48875642597890145,
+                    "timestamp": 1.1102901037813666
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -0.10264847768590626,
-                    "velocityX": -0.6682724839650215,
-                    "velocityY": -0.24437806844836182,
-                    "timestamp": 1.195697540154587
+                    "heading": -0.003,
+                    "angularVelocity": -0.10257610751605846,
+                    "velocityX": -0.6682729646898584,
+                    "velocityY": -0.24437825107975505,
+                    "timestamp": 1.1956970348414717
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": 0,
-                    "velocityX": -3.560921302709756e-34,
-                    "velocityY": 7.627797313302701e-34,
-                    "timestamp": 1.2811045073084861
+                    "heading": -0.003,
+                    "angularVelocity": -5.146817856452735e-35,
+                    "velocityX": 4.663929716319662e-34,
+                    "velocityY": 0,
+                    "timestamp": 1.2811039659015768
                 }
             ],
             "constraints": [
@@ -22767,7 +20743,7 @@
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": -2.88,
+                    "heading": 0.26,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -22776,7 +20752,7 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": -3.5331853071795862,
+                    "heading": -0.393,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -22824,7 +20800,7 @@
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": -2.88,
+                    "heading": 0.26,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -22833,7 +20809,7 @@
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": -3.143185307179586,
+                    "heading": -0.003,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -22844,128 +20820,128 @@
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": -2.88,
+                    "heading": 0.26,
                     "angularVelocity": 0,
-                    "velocityX": 1.8796456462447601e-29,
-                    "velocityY": -4.3119341309057906e-30,
+                    "velocityX": -1.5159307977916785e-33,
+                    "velocityY": 2.4731304546687864e-31,
                     "timestamp": 0
                 },
                 {
-                    "x": 5.238209828475023,
-                    "y": 6.650800952122015,
-                    "heading": -2.892998872920493,
-                    "angularVelocity": -0.1464435777830739,
-                    "velocityX": 0.7008476743058382,
-                    "velocityY": 0.23434076697564932,
-                    "timestamp": 0.08876369396038813
-                },
-                {
-                    "x": 5.362652804152687,
-                    "y": 6.692404857453675,
-                    "heading": -2.917433036424788,
-                    "angularVelocity": -0.27527204439886777,
-                    "velocityX": 1.401958054303432,
-                    "velocityY": 0.46870407793850294,
-                    "timestamp": 0.17752738792077627
-                },
-                {
-                    "x": 5.5493620218671325,
-                    "y": 6.7548129941209,
-                    "heading": -2.950667481161702,
-                    "angularVelocity": -0.37441484524641305,
-                    "velocityX": 2.1034412763117816,
-                    "velocityY": 0.70308178809801,
-                    "timestamp": 0.2662910818811644
-                },
-                {
-                    "x": 5.798380867010647,
-                    "y": 6.838019600580292,
-                    "heading": -2.987306905639368,
-                    "angularVelocity": -0.4127748952720924,
-                    "velocityX": 2.8054132723935825,
-                    "velocityY": 0.9373945894737359,
-                    "timestamp": 0.35505477584155254
-                },
-                {
-                    "x": 6.109622963136531,
-                    "y": 6.941929172833481,
-                    "heading": -3.009821564076527,
-                    "angularVelocity": -0.25364715496791174,
-                    "velocityX": 3.5064121628854164,
-                    "velocityY": 1.1706314554748125,
-                    "timestamp": 0.4438184698019407
-                },
-                {
-                    "x": 6.428045089115211,
-                    "y": 7.048516124767363,
-                    "heading": -3.0098215751251205,
-                    "angularVelocity": -1.2447253578662016e-7,
-                    "velocityX": 3.5873014266485415,
-                    "velocityY": 1.2007944597359943,
-                    "timestamp": 0.5325821637623288
-                },
-                {
-                    "x": 6.746467236003722,
-                    "y": 7.1551030142348395,
-                    "heading": -3.0098215861735884,
-                    "angularVelocity": -1.2447113137335796e-7,
-                    "velocityX": 3.5873016622160976,
-                    "velocityY": 1.2007937559974857,
-                    "timestamp": 0.6213458577227169
-                },
-                {
-                    "x": 7.057671617899985,
-                    "y": 7.2589999980659305,
-                    "heading": -3.0321320878151,
-                    "angularVelocity": -0.2513471515963816,
-                    "velocityX": 3.5059872793849554,
-                    "velocityY": 1.1704896359626165,
-                    "timestamp": 0.7101095516831051
-                },
-                {
-                    "x": 7.306674739642628,
-                    "y": 7.342201198643625,
-                    "heading": -3.06987813605104,
-                    "angularVelocity": -0.42524197169930134,
-                    "velocityX": 2.8052361346509134,
-                    "velocityY": 0.9373336875121538,
-                    "timestamp": 0.7988732456434933
-                },
-                {
-                    "x": 7.493368024895104,
-                    "y": 7.4046034302940065,
-                    "heading": -3.1043079780358913,
-                    "angularVelocity": -0.3878820320283419,
-                    "velocityX": 2.1032617833134357,
-                    "velocityY": 0.7030152629492495,
-                    "timestamp": 0.8876369396038815
-                },
-                {
-                    "x": 7.617797798391618,
-                    "y": 7.446202168700309,
-                    "heading": -3.129675012430027,
-                    "angularVelocity": -0.28578164406378687,
-                    "velocityX": 1.4018093202853217,
-                    "velocityY": 0.4686458680340631,
-                    "timestamp": 0.9764006335642696
+                    "x": 5.238209862421909,
+                    "y": 6.650800928993829,
+                    "heading": 0.2470076817123004,
+                    "angularVelocity": -0.14636981892397002,
+                    "velocityX": 0.7008484626165542,
+                    "velocityY": 0.23434064212588615,
+                    "timestamp": 0.08876364255629132
+                },
+                {
+                    "x": 5.362652882936083,
+                    "y": 6.6924048077649365,
+                    "heading": 0.22258636188758224,
+                    "angularVelocity": -0.27512750853804663,
+                    "velocityX": 1.4019593713167977,
+                    "velocityY": 0.4687040501427167,
+                    "timestamp": 0.17752728511258264
+                },
+                {
+                    "x": 5.549362119474242,
+                    "y": 6.7548129470224065,
+                    "heading": 0.1893705540696427,
+                    "angularVelocity": -0.37420510089603504,
+                    "velocityX": 2.1034427065056938,
+                    "velocityY": 0.7030822244423112,
+                    "timestamp": 0.26629092766887397
+                },
+                {
+                    "x": 5.798380890690993,
+                    "y": 6.838019653320762,
+                    "heading": 0.15275441561528638,
+                    "angularVelocity": -0.4125127969134817,
+                    "velocityX": 2.80541406419279,
+                    "velocityY": 0.9373962571031511,
+                    "timestamp": 0.3550545702251653
+                },
+                {
+                    "x": 6.109622726468052,
+                    "y": 6.941929683454389,
+                    "heading": 0.13026224178151366,
+                    "angularVelocity": -0.2533939931523947,
+                    "velocityX": 3.5064112604341973,
+                    "velocityY": 1.17063729182453,
+                    "timestamp": 0.4438182127814566
+                },
+                {
+                    "x": 6.428044865529091,
+                    "y": 7.048515983686504,
+                    "heading": 0.13026223073820456,
+                    "angularVelocity": -1.2441305517804787e-7,
+                    "velocityX": 3.587303651482315,
+                    "velocityY": 1.2007878131408318,
+                    "timestamp": 0.5325818553377479
+                },
+                {
+                    "x": 6.746467025502362,
+                    "y": 7.155102221444599,
+                    "heading": 0.13026221969501808,
+                    "angularVelocity": -1.244116833309462e-7,
+                    "velocityX": 3.5873038870770317,
+                    "velocityY": 1.2007871093161688,
+                    "timestamp": 0.6213454978940393
+                },
+                {
+                    "x": 7.057671328205741,
+                    "y": 7.258999778631647,
+                    "heading": 0.10797327486362365,
+                    "angularVelocity": -0.25110444085082756,
+                    "velocityX": 3.505988417567436,
+                    "velocityY": 1.1704967731634406,
+                    "timestamp": 0.7101091404503306
+                },
+                {
+                    "x": 7.306674452232408,
+                    "y": 7.342201135769052,
+                    "heading": 0.07025598564329198,
+                    "angularVelocity": -0.4249182225204012,
+                    "velocityX": 2.8052377849284973,
+                    "velocityY": 0.9373359941155657,
+                    "timestamp": 0.7988727830066219
+                },
+                {
+                    "x": 7.493367837286706,
+                    "y": 7.404603419315824,
+                    "heading": 0.03585067831804618,
+                    "angularVelocity": -0.38760585227578265,
+                    "velocityX": 2.103264125692411,
+                    "velocityY": 0.7030162547322278,
+                    "timestamp": 0.8876364255629132
+                },
+                {
+                    "x": 7.617797724211195,
+                    "y": 7.446202169802599,
+                    "heading": 0.01050114896385133,
+                    "angularVelocity": -0.2855845999919636,
+                    "velocityX": 1.401811409955239,
+                    "velocityY": 0.46864627552974614,
+                    "timestamp": 0.9764000681192045
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": -0.15220518825005472,
-                    "velocityX": 0.7007617510398297,
-                    "velocityY": 0.23430560820061222,
-                    "timestamp": 1.0651643275246578
+                    "heading": -0.003,
+                    "angularVelocity": -0.15210224112604764,
+                    "velocityX": 0.7007629925666751,
+                    "velocityY": 0.2343057314716256,
+                    "timestamp": 1.0651637106754959
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": 0,
-                    "velocityX": 1.9101182509980996e-31,
-                    "velocityY": -4.716397884209056e-30,
-                    "timestamp": 1.153928021485046
+                    "heading": -0.003,
+                    "angularVelocity": 2.057519635914241e-30,
+                    "velocityX": 3.0968464213310355e-31,
+                    "velocityY": -5.4576131966501765e-31,
+                    "timestamp": 1.1539273532317873
                 }
             ],
             "constraints": [
@@ -23008,7 +20984,7 @@
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": -2.88,
+                    "heading": 0.26,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -23017,7 +20993,7 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": -3.143185307179586,
+                    "heading": -0.003,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -23028,128 +21004,128 @@
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": -2.88,
+                    "heading": 0.26,
                     "angularVelocity": 0,
-                    "velocityX": -2.1431055300877995e-28,
-                    "velocityY": 1.1784110457685481e-29,
+                    "velocityX": -4.471050480852603e-35,
+                    "velocityY": 4.2685150703849235e-34,
                     "timestamp": 0
                 },
                 {
-                    "x": 5.242060097443899,
-                    "y": 6.625920772567306,
-                    "heading": -2.9233556616628746,
-                    "angularVelocity": -0.48269065593589344,
-                    "velocityX": 0.7354654627191669,
-                    "velocityY": -0.04541517508160964,
-                    "timestamp": 0.08982080164534181
-                },
-                {
-                    "x": 5.3743817426165945,
-                    "y": 6.616908903759451,
-                    "heading": -3.004544486756046,
-                    "angularVelocity": -0.9038978010210977,
-                    "velocityX": 1.4731737275644603,
-                    "velocityY": -0.1003316452622891,
-                    "timestamp": 0.17964160329068363
-                },
-                {
-                    "x": 5.573213116332803,
-                    "y": 6.601503149315071,
-                    "heading": -3.1137348179578126,
-                    "angularVelocity": -1.215646367006541,
-                    "velocityX": 2.2136450585388348,
-                    "velocityY": -0.17151655476435285,
-                    "timestamp": 0.26946240493602547
-                },
-                {
-                    "x": 5.83860703608144,
-                    "y": 6.576562673459042,
-                    "heading": -3.229878025812627,
-                    "angularVelocity": -1.2930546791757207,
-                    "velocityX": 2.954704421327105,
-                    "velocityY": -0.2776692636802449,
-                    "timestamp": 0.35928320658136725
-                },
-                {
-                    "x": 6.164587325578701,
-                    "y": 6.531858569137778,
-                    "heading": -3.2661345564867124,
-                    "angularVelocity": -0.40365405351480704,
-                    "velocityX": 3.629229349170066,
-                    "velocityY": -0.49770324359594226,
-                    "timestamp": 0.44910400822670904
-                },
-                {
-                    "x": 6.48571912004567,
-                    "y": 6.420820709470225,
-                    "heading": -3.2661345579017715,
-                    "angularVelocity": -1.5754250259377596e-8,
-                    "velocityX": 3.5752497036818096,
-                    "velocityY": -1.2362154159565475,
-                    "timestamp": 0.5389248098720508
-                },
-                {
-                    "x": 6.786864453228811,
-                    "y": 6.263446876676794,
-                    "heading": -3.2661345453374624,
-                    "angularVelocity": 1.3988195638330222e-7,
-                    "velocityX": 3.3527348639373855,
-                    "velocityY": -1.7520867094330927,
-                    "timestamp": 0.6287456115173926
-                },
-                {
-                    "x": 7.084475527096153,
-                    "y": 6.108061043338325,
-                    "heading": -3.255293907795362,
-                    "angularVelocity": 0.12069183689660036,
-                    "velocityX": 3.313386970675918,
-                    "velocityY": -1.7299537578389734,
-                    "timestamp": 0.7185664131627344
-                },
-                {
-                    "x": 7.322762050188167,
-                    "y": 5.983606942670793,
-                    "heading": -3.219406300389853,
-                    "angularVelocity": 0.3995467280198762,
-                    "velocityX": 2.652910224881898,
-                    "velocityY": -1.385582163460734,
-                    "timestamp": 0.8083872148080762
-                },
-                {
-                    "x": 7.5014175228899544,
-                    "y": 5.890287602383411,
-                    "heading": -3.184260886361704,
-                    "angularVelocity": 0.39128368244731204,
-                    "velocityX": 1.9890211335143717,
-                    "velocityY": -1.0389502050521982,
-                    "timestamp": 0.898208016453418
-                },
-                {
-                    "x": 7.620482912490659,
-                    "y": 5.8280911603292145,
-                    "heading": -3.1576094170182762,
-                    "angularVelocity": 0.296718230690803,
-                    "velocityX": 1.325588142386374,
-                    "velocityY": -0.6924503112294588,
-                    "timestamp": 0.9880288180987598
+                    "x": 5.242063425720018,
+                    "y": 6.625917381735586,
+                    "heading": 0.21680591867144897,
+                    "angularVelocity": -0.4809077162576206,
+                    "velocityX": 0.7355269567955554,
+                    "velocityY": -0.04545443647608383,
+                    "timestamp": 0.08981781715769374
+                },
+                {
+                    "x": 5.374390186000276,
+                    "y": 6.616899298560691,
+                    "heading": 0.13592925344567436,
+                    "angularVelocity": -0.9004523577296353,
+                    "velocityX": 1.473279628338445,
+                    "velocityY": -0.10040416768380533,
+                    "timestamp": 0.17963563431538748
+                },
+                {
+                    "x": 5.573225936929258,
+                    "y": 6.6014853361346235,
+                    "heading": 0.027174299165301757,
+                    "angularVelocity": -1.2108394272088667,
+                    "velocityX": 2.2137673484079947,
+                    "velocityY": -0.17161363873946028,
+                    "timestamp": 0.2694534514730812
+                },
+                {
+                    "x": 5.838620178684051,
+                    "y": 6.5765367651917135,
+                    "heading": -0.08847883132007715,
+                    "angularVelocity": -1.287641295961646,
+                    "velocityX": 2.9548061860470294,
+                    "velocityY": -0.27776861799154184,
+                    "timestamp": 0.35927126863077496
+                },
+                {
+                    "x": 6.164628937797069,
+                    "y": 6.53182752325973,
+                    "heading": -0.12460373302334064,
+                    "angularVelocity": -0.4022019555411679,
+                    "velocityX": 3.6296669127534233,
+                    "velocityY": -0.4977769817483746,
+                    "timestamp": 0.44908908578846873
+                },
+                {
+                    "x": 6.48574789195848,
+                    "y": 6.420787077372237,
+                    "heading": -0.12460373465453414,
+                    "angularVelocity": -1.816113510684907e-8,
+                    "velocityX": 3.5752255434756393,
+                    "velocityY": -1.2362852872781145,
+                    "timestamp": 0.5389069029461625
+                },
+                {
+                    "x": 6.786885646715688,
+                    "y": 6.263423119459949,
+                    "heading": -0.12460372224054271,
+                    "angularVelocity": 1.3821301643987816e-7,
+                    "velocityX": 3.3527618938734416,
+                    "velocityY": -1.7520349847291758,
+                    "timestamp": 0.6287247201038563
+                },
+                {
+                    "x": 7.084493676309923,
+                    "y": 6.108042134366637,
+                    "heading": -0.11377963368638179,
+                    "angularVelocity": 0.12051159665968046,
+                    "velocityX": 3.313463174814436,
+                    "velocityY": -1.729957262494017,
+                    "timestamp": 0.71854253726155
+                },
+                {
+                    "x": 7.322771133319765,
+                    "y": 5.983596419357133,
+                    "heading": -0.07830445075984473,
+                    "angularVelocity": 0.3949682150953753,
+                    "velocityX": 2.652897437837947,
+                    "velocityY": -1.3855348409437966,
+                    "timestamp": 0.8083603544192438
+                },
+                {
+                    "x": 7.5014211847430525,
+                    "y": 5.8902827728941185,
+                    "heading": -0.04357887601405372,
+                    "angularVelocity": 0.38662234114221544,
+                    "velocityX": 1.9890268665695783,
+                    "velocityY": -1.0389213344962835,
+                    "timestamp": 0.8981781715769376
+                },
+                {
+                    "x": 7.620483882250166,
+                    "y": 5.828089677839219,
+                    "heading": -0.01724918141894823,
+                    "angularVelocity": 0.293145563189075,
+                    "velocityX": 1.3256022165187353,
+                    "velocityY": -0.6924360558184839,
+                    "timestamp": 0.9879959887346313
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": 0.16058763197915146,
-                    "velocityX": 0.6626203108757032,
-                    "velocityY": -0.3461465468987679,
-                    "timestamp": 1.0778496197441017
+                    "heading": -0.003,
+                    "angularVelocity": 0.1586453764950761,
+                    "velocityX": 0.6626315316185003,
+                    "velocityY": -0.346141543215585,
+                    "timestamp": 1.077813805892325
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": -3.143185307179586,
-                    "angularVelocity": 2.6110530197350993e-29,
-                    "velocityX": -1.9135813261242845e-30,
-                    "velocityY": -4.114109248291899e-30,
-                    "timestamp": 1.1676704213894435
+                    "heading": -0.003,
+                    "angularVelocity": 2.21164904203514e-32,
+                    "velocityX": 6.504499750841485e-34,
+                    "velocityY": 5.343957154186367e-33,
+                    "timestamp": 1.1676316230500188
                 }
             ],
             "constraints": [
@@ -23192,7 +21168,7 @@
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": -2.88,
+                    "heading": 0.26,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -23201,7 +21177,7 @@
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": -3.143185307179586,
+                    "heading": -0.003,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -23249,7 +21225,7 @@
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": -2.88,
+                    "heading": 0.26,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -23258,7 +21234,7 @@
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": -3.143185307179586,
+                    "heading": -0.003,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -23306,16 +21282,16 @@
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": -2.88,
+                    "heading": 0.26,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 23
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -23363,7 +21339,7 @@
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -23372,7 +21348,7 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": 3.5331853071795862,
+                    "heading": 0.393,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -23383,128 +21359,128 @@
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": -7.11323283877368e-33,
-                    "velocityX": -2.26183607094562e-34,
-                    "velocityY": 8.143994372388985e-34,
+                    "heading": -7.967068308454128e-24,
+                    "angularVelocity": 3.748757739861167e-23,
+                    "velocityX": 2.372714537320914e-25,
+                    "velocityY": 2.748536601620217e-25,
                     "timestamp": 0
                 },
                 {
-                    "x": 4.215224482174133,
-                    "y": 5.589131482897013,
-                    "heading": 3.153116118672446,
-                    "angularVelocity": 0.15993604804856262,
-                    "velocityX": -0.5825677325255438,
-                    "velocityY": 0.35522507570249656,
-                    "timestamp": 0.0820085204835332
-                },
-                {
-                    "x": 4.119673447940142,
-                    "y": 5.647394549238196,
-                    "heading": 3.179344081068732,
-                    "angularVelocity": 0.3198199679940867,
-                    "velocityX": -1.1651354477633493,
-                    "velocityY": 0.7104513774624363,
-                    "timestamp": 0.1640170409670664
-                },
-                {
-                    "x": 3.976346906814042,
-                    "y": 5.734789369483009,
-                    "heading": 3.218677503841872,
-                    "angularVelocity": 0.47962605033262895,
-                    "velocityX": -1.7477030469642283,
-                    "velocityY": 1.0656797577803132,
-                    "timestamp": 0.24602556145059962
-                },
-                {
-                    "x": 3.785244904014577,
-                    "y": 5.851316186750812,
-                    "heading": 3.271109373786434,
-                    "angularVelocity": 0.6393466146617031,
-                    "velocityX": -2.3302700947743262,
-                    "velocityY": 1.4209111026603802,
-                    "timestamp": 0.3280340819341328
-                },
-                {
-                    "x": 3.5463675589434582,
-                    "y": 5.996975305481239,
-                    "heading": 3.3366342258406214,
-                    "angularVelocity": 0.7990005388201682,
-                    "velocityX": -2.9128356866172687,
-                    "velocityY": 1.7761461598331643,
-                    "timestamp": 0.410042602417666
-                },
-                {
-                    "x": 3.2814945861155147,
-                    "y": 6.158490139171684,
-                    "heading": 3.3366342341249666,
-                    "angularVelocity": 1.010181030485091e-7,
-                    "velocityX": -3.2298225997276515,
-                    "velocityY": 1.9694884475190098,
-                    "timestamp": 0.4920511229011992
-                },
-                {
-                    "x": 3.0166215989993184,
-                    "y": 6.32000494943038,
-                    "heading": 3.336634242409321,
-                    "angularVelocity": 1.0101821933668142e-7,
-                    "velocityX": -3.2298227739565295,
-                    "velocityY": 1.969488161795665,
-                    "timestamp": 0.5740596433847324
-                },
-                {
-                    "x": 2.7777475013239767,
-                    "y": 6.46566975957478,
-                    "heading": 3.402143020545178,
-                    "angularVelocity": 0.7988045357922341,
-                    "velocityX": -2.9127960883443285,
-                    "velocityY": 1.776215560109378,
-                    "timestamp": 0.6560681638682656
-                },
-                {
-                    "x": 2.5866484088099964,
-                    "y": 6.582201778902395,
-                    "heading": 3.4545564874050534,
-                    "angularVelocity": 0.6391222101171739,
-                    "velocityX": -2.3302346071753846,
-                    "velocityY": 1.4209745358229475,
-                    "timestamp": 0.7380766843517989
-                },
-                {
-                    "x": 2.443324185159995,
-                    "y": 6.669600869933244,
-                    "heading": 3.493870163524806,
-                    "angularVelocity": 0.47938526250630853,
-                    "velocityX": -1.7476747879969323,
-                    "velocityY": 1.0657318351255574,
-                    "timestamp": 0.8200852048353321
-                },
-                {
-                    "x": 2.347774731346008,
-                    "y": 6.727866955511231,
-                    "heading": 3.5200803945158796,
-                    "angularVelocity": 0.3196037538116112,
-                    "velocityX": -1.1651161763511244,
-                    "velocityY": 0.710488193597954,
-                    "timestamp": 0.9020937253188653
+                    "x": 4.215224464845852,
+                    "y": 5.589131496345612,
+                    "heading": 0.013109636548148297,
+                    "angularVelocity": 0.15985714241706056,
+                    "velocityX": -0.5825684410958524,
+                    "velocityY": 0.3552255429080149,
+                    "timestamp": 0.08200845048228003
+                },
+                {
+                    "x": 4.119673396532112,
+                    "y": 5.647394589751744,
+                    "heading": 0.03932466572409392,
+                    "angularVelocity": 0.31966253504094716,
+                    "velocityX": -1.1651368578703514,
+                    "velocityY": 0.7104523139200205,
+                    "timestamp": 0.16401690096456006
+                },
+                {
+                    "x": 3.9763468054990545,
+                    "y": 5.734789450682316,
+                    "heading": 0.07863876132846631,
+                    "angularVelocity": 0.47939078684174374,
+                    "velocityX": -1.7477051473375471,
+                    "velocityY": 1.06568116354614,
+                    "timestamp": 0.2460253514468401
+                },
+                {
+                    "x": 3.785244738141483,
+                    "y": 5.8513163218996445,
+                    "heading": 0.131044995583191,
+                    "angularVelocity": 0.6390345622497571,
+                    "velocityX": -2.330272871072761,
+                    "velocityY": 1.4209129733832384,
+                    "timestamp": 0.32803380192912013
+                },
+                {
+                    "x": 3.5463673151125503,
+                    "y": 5.996975506896697,
+                    "heading": 0.19653801558690542,
+                    "angularVelocity": 0.7986130650995023,
+                    "velocityX": -2.9128391235796927,
+                    "velocityY": 1.776148483972697,
+                    "timestamp": 0.41004225241140013
+                },
+                {
+                    "x": 3.2814945553511796,
+                    "y": 6.158490181857634,
+                    "heading": 0.19653800981726008,
+                    "angularVelocity": -7.035427786522906e-8,
+                    "velocityX": -3.2298227585534365,
+                    "velocityY": 1.9694881931202608,
+                    "timestamp": 0.49205070289368014
+                },
+                {
+                    "x": 3.016621781154659,
+                    "y": 6.320004832717382,
+                    "heading": 0.19653801616374372,
+                    "angularVelocity": 7.738816679079332e-8,
+                    "velocityX": -3.229822934573717,
+                    "velocityY": 1.9694878992335856,
+                    "timestamp": 0.5740591533759601
+                },
+                {
+                    "x": 2.7777476228533557,
+                    "y": 6.465669683581835,
+                    "heading": 0.2620170313059908,
+                    "angularVelocity": 0.7984422917049926,
+                    "velocityX": -2.9127993139306705,
+                    "velocityY": 1.7762175727966987,
+                    "timestamp": 0.6560676038582401
+                },
+                {
+                    "x": 2.5866484818505064,
+                    "y": 6.582201734022544,
+                    "heading": 0.31440674809174973,
+                    "angularVelocity": 0.6388331504578179,
+                    "velocityX": -2.330237187497417,
+                    "velocityY": 1.4209761281355728,
+                    "timestamp": 0.7380760543405201
+                },
+                {
+                    "x": 2.443324221748551,
+                    "y": 6.669600847656536,
+                    "heading": 0.3537026369190334,
+                    "angularVelocity": 0.47916877585407597,
+                    "velocityX": -1.7476767242776265,
+                    "velocityY": 1.0657330204388804,
+                    "timestamp": 0.8200845048228002
+                },
+                {
+                    "x": 2.347774743561573,
+                    "y": 6.727866948074407,
+                    "heading": 0.37990101547487837,
+                    "angularVelocity": 0.31945949962200243,
+                    "velocityX": -1.1651174680787846,
+                    "velocityY": 0.7104889810161812,
+                    "timestamp": 0.9020929553050802
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": 3.5331853071795862,
-                    "angularVelocity": 0.1597994036038963,
-                    "velocityX": -0.5825581423042643,
-                    "velocityY": 0.35524411752579316,
-                    "timestamp": 0.9841022458023986
+                    "heading": 0.393,
+                    "angularVelocity": 0.15972725308292482,
+                    "velocityX": -0.5825587885226076,
+                    "velocityY": 0.355244511440778,
+                    "timestamp": 0.9841014057873602
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": 3.5331853071795862,
-                    "angularVelocity": 0,
-                    "velocityX": -5.266786892384168e-34,
-                    "velocityY": -2.2308589105860002e-32,
-                    "timestamp": 1.0661107662859317
+                    "heading": 0.393,
+                    "angularVelocity": 1.2555409321863598e-23,
+                    "velocityX": -4.0981987257829375e-24,
+                    "velocityY": -7.824281389954736e-24,
+                    "timestamp": 1.0661098562696403
                 }
             ],
             "constraints": [
@@ -23547,7 +21523,7 @@
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -23556,7 +21532,7 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -23567,100 +21543,100 @@
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": 9.713847301484863e-36,
-                    "velocityY": -1.0645735433948216e-32,
+                    "heading": -1.042790690974161e-34,
+                    "angularVelocity": 1.2672485017804847e-33,
+                    "velocityX": 0,
+                    "velocityY": 1.0649309840552648e-32,
                     "timestamp": 0
                 },
                 {
-                    "x": 4.181096344253042,
-                    "y": 5.560000047628424,
-                    "heading": 3.139999989014019,
-                    "angularVelocity": -1.10846415392014e-7,
-                    "velocityX": -0.826391981328299,
-                    "velocityY": 4.805615505681296e-7,
+                    "x": 4.181096344253041,
+                    "y": 5.560000047617906,
+                    "heading": -1.1491589479255403e-8,
+                    "angularVelocity": -1.1594790933974305e-7,
+                    "velocityX": -0.8263919813282988,
+                    "velocityY": 4.80455414778758e-7,
                     "timestamp": 0.09910993523353247
                 },
                 {
-                    "x": 4.017289035836252,
-                    "y": 5.560000133123364,
-                    "heading": 3.139999981407119,
-                    "angularVelocity": -7.675215101799687e-8,
-                    "velocityX": -1.6527839316089914,
-                    "velocityY": 8.626273272073532e-7,
+                    "x": 4.017289035836251,
+                    "y": 5.560000133094268,
+                    "heading": -2.0027929206394758e-8,
+                    "angularVelocity": -8.613001771881656e-8,
+                    "velocityX": -1.6527839316089912,
+                    "velocityY": 8.624398858779209e-7,
                     "timestamp": 0.19821987046706493
                 },
                 {
-                    "x": 3.7715780809038826,
-                    "y": 5.560000246490977,
-                    "heading": 3.1399999920859667,
-                    "angularVelocity": 1.0774748441307597e-7,
-                    "velocityX": -2.4791758197944644,
-                    "velocityY": 0.0000011438571635435649,
+                    "x": 3.771578080903882,
+                    "y": 5.560000246435489,
+                    "heading": -1.061567052726606e-8,
+                    "angularVelocity": 9.49678480632688e-8,
+                    "velocityX": -2.4791758197944636,
+                    "velocityY": 0.0000011435908848510677,
                     "timestamp": 0.2973298057005974
                 },
                 {
-                    "x": 3.4439634979186673,
-                    "y": 5.56000037760455,
-                    "heading": 3.1400000352544852,
-                    "angularVelocity": 4.355619377078705e-7,
-                    "velocityX": -3.305567521694553,
-                    "velocityY": 0.0000013229104502120412,
+                    "x": 3.443963497918667,
+                    "y": 5.560000377513817,
+                    "heading": 3.109895895304223e-8,
+                    "angularVelocity": 4.208924841656569e-7,
+                    "velocityX": -3.30556752169455,
+                    "velocityY": 0.0000013225548252865442,
                     "timestamp": 0.39643974093412987
                 },
                 {
-                    "x": 3.069036502081214,
-                    "y": 5.560000367749523,
-                    "heading": 3.140000035254493,
-                    "angularVelocity": 8.147365661806906e-14,
-                    "velocityX": -3.782940579610041,
-                    "velocityY": -9.943529898465916e-8,
+                    "x": 3.0690365020812136,
+                    "y": 5.5600003676341405,
+                    "heading": 3.1098967052884415e-8,
+                    "angularVelocity": 8.336617080453033e-14,
+                    "velocityX": -3.7829405796100413,
+                    "velocityY": -9.968399941565162e-8,
                     "timestamp": 0.4955496761676623
                 },
                 {
-                    "x": 2.741421919096052,
-                    "y": 5.560000236043854,
-                    "heading": 3.1400000358317235,
-                    "angularVelocity": 5.824165114605996e-9,
-                    "velocityX": -3.305567521694012,
-                    "velocityY": -0.0000013288845794272055,
+                    "x": 2.7414219190960516,
+                    "y": 5.560000235969226,
+                    "heading": 3.331211577627848e-8,
+                    "angularVelocity": 2.233025994415073e-8,
+                    "velocityX": -3.3055675216940097,
+                    "velocityY": -0.0000013284733736467011,
                     "timestamp": 0.5946596114011948
                 },
                 {
-                    "x": 2.4957109641637163,
-                    "y": 5.560000125368319,
-                    "heading": 3.1400000237983328,
-                    "angularVelocity": -1.214145560764502e-7,
-                    "velocityX": -2.479175819794127,
-                    "velocityY": -0.0000011166946380844406,
+                    "x": 2.495710964163716,
+                    "y": 5.560000125328881,
+                    "heading": 2.2528078300409453e-8,
+                    "angularVelocity": -1.088088286759601e-7,
+                    "velocityX": -2.4791758197941256,
+                    "velocityY": -0.0000011163395746408495,
                     "timestamp": 0.6937695466347273
                 },
                 {
                     "x": 2.3319036557469475,
-                    "y": 5.560000044140379,
-                    "heading": 3.140000009519778,
-                    "angularVelocity": -1.4406783354118398e-7,
-                    "velocityX": -1.6527839316087705,
-                    "velocityY": -8.195741267361974e-7,
+                    "y": 5.560000044126654,
+                    "heading": 9.093200341480335e-9,
+                    "angularVelocity": -1.3555529933337065e-7,
+                    "velocityX": -1.6527839316087698,
+                    "velocityY": -8.193146909757129e-7,
                     "timestamp": 0.7928794818682598
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": -9.605270403416775e-8,
-                    "velocityX": -0.8263919813281883,
-                    "velocityY": -4.4536784825580755e-7,
+                    "heading": 7.024905960060336e-33,
+                    "angularVelocity": -9.17486204713572e-8,
+                    "velocityX": -0.826391981328188,
+                    "velocityY": -4.4522936458958866e-7,
                     "timestamp": 0.8919894171017924
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": 4.934081445690025e-38,
-                    "velocityY": -9.331512134237052e-34,
+                    "heading": 0,
+                    "angularVelocity": 2.1129142513587384e-31,
+                    "velocityX": -1.9174161859426788e-38,
+                    "velocityY": -1.530570219467473e-32,
                     "timestamp": 0.9910993523353249
                 }
             ],
@@ -23704,7 +21680,7 @@
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -23713,7 +21689,7 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -23724,128 +21700,128 @@
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 1.4062915610922596e-31,
-                    "velocityX": -1.6151999437241794e-32,
-                    "velocityY": -1.528021229010722e-32,
+                    "heading": 3.6296173977413556e-31,
+                    "angularVelocity": 2.1802637913498067e-31,
+                    "velocityX": -2.5358916422596453e-32,
+                    "velocityY": 1.2879635720357984e-31,
                     "timestamp": 0
                 },
                 {
-                    "x": 4.206513702340257,
-                    "y": 5.554388398990967,
-                    "heading": 3.0326218488137218,
-                    "angularVelocity": -1.231959178646129,
-                    "velocityX": -0.6480723694798501,
-                    "velocityY": -0.0643824026918128,
-                    "timestamp": 0.0871604782426985
-                },
-                {
-                    "x": 4.0911807616230185,
-                    "y": 5.5411300283093805,
-                    "heading": 2.844661289825114,
-                    "angularVelocity": -2.156488385311873,
-                    "velocityX": -1.323225193832629,
-                    "velocityY": -0.15211447836104172,
-                    "timestamp": 0.174320956485397
-                },
-                {
-                    "x": 3.915187082588679,
-                    "y": 5.516242186917599,
-                    "heading": 2.6068500185118766,
-                    "angularVelocity": -2.728430087901214,
-                    "velocityX": -2.0191912961317713,
-                    "velocityY": -0.2855404409608899,
-                    "timestamp": 0.2614814347280955
-                },
-                {
-                    "x": 3.678761658477574,
-                    "y": 5.47299330729491,
-                    "heading": 2.386294579571215,
-                    "angularVelocity": -2.530452372307143,
-                    "velocityX": -2.7125301384049063,
-                    "velocityY": -0.4961982826925549,
-                    "timestamp": 0.348641912970794
-                },
-                {
-                    "x": 3.3968919941686,
-                    "y": 5.398640264151657,
-                    "heading": 2.288226309782543,
-                    "angularVelocity": -1.1251460726913511,
-                    "velocityX": -3.233915990273797,
-                    "velocityY": -0.8530591460984852,
-                    "timestamp": 0.4358023912134925
-                },
-                {
-                    "x": 3.0985878200503763,
-                    "y": 5.268158980231371,
-                    "heading": 2.276780497851342,
-                    "angularVelocity": -0.1313188289230101,
-                    "velocityX": -3.4224705982864747,
-                    "velocityY": -1.4970234967844136,
-                    "timestamp": 0.522962869456191
-                },
-                {
-                    "x": 2.8320780769343714,
-                    "y": 5.083465560010488,
-                    "heading": 2.2937556690853027,
-                    "angularVelocity": 0.19475766512768708,
-                    "velocityX": -3.0576902340290975,
-                    "velocityY": -2.11900420861165,
-                    "timestamp": 0.6101233476988894
-                },
-                {
-                    "x": 2.620143221340131,
-                    "y": 4.878958491159276,
-                    "heading": 2.4039835676400654,
-                    "angularVelocity": 1.264654586311841,
-                    "velocityX": -2.4315476448409066,
-                    "velocityY": -2.34632798000215,
-                    "timestamp": 0.6972838259415879
-                },
-                {
-                    "x": 2.469192165605932,
-                    "y": 4.691124569407488,
-                    "heading": 2.528247411458178,
-                    "angularVelocity": 1.42569024773017,
-                    "velocityX": -1.7318750284260176,
-                    "velocityY": -2.155035464912952,
-                    "timestamp": 0.7844443041842863
-                },
-                {
-                    "x": 2.3748255822885,
-                    "y": 4.531333386407196,
-                    "heading": 2.6349251655798476,
-                    "angularVelocity": 1.2239234601791131,
-                    "velocityX": -1.0826762911358505,
-                    "velocityY": -1.833298603013082,
-                    "timestamp": 0.8716047824269848
-                },
-                {
-                    "x": 2.3223557063303257,
-                    "y": 4.418269595148307,
-                    "heading": 2.710801114054306,
-                    "angularVelocity": 0.8705315758270817,
-                    "velocityX": -0.6019916023415092,
-                    "velocityY": -1.2971910381682688,
-                    "timestamp": 0.9587652606696833
+                    "x": 4.206523470432328,
+                    "y": 5.554409314419878,
+                    "heading": -0.10760113621403576,
+                    "angularVelocity": -1.2344151326641672,
+                    "velocityX": -0.6479065667114609,
+                    "velocityY": -0.06413711903878926,
+                    "timestamp": 0.08716770668697735
+                },
+                {
+                    "x": 4.091196970051195,
+                    "y": 5.54119730250816,
+                    "heading": -0.2959135822275083,
+                    "angularVelocity": -2.1603464536437884,
+                    "velocityX": -1.3230415800116804,
+                    "velocityY": -0.15157002993279564,
+                    "timestamp": 0.1743354133739547
+                },
+                {
+                    "x": 3.9151950635806365,
+                    "y": 5.5163708141067165,
+                    "heading": -0.5340651676870422,
+                    "angularVelocity": -2.732107961894037,
+                    "velocityX": -2.0191182395401066,
+                    "velocityY": -0.28481291231621586,
+                    "timestamp": 0.261503120060932
+                },
+                {
+                    "x": 3.678758755729211,
+                    "y": 5.473177786204715,
+                    "heading": -0.7546652286499241,
+                    "angularVelocity": -2.5307544427555704,
+                    "velocityX": -2.712430059683426,
+                    "velocityY": -0.49551639642315154,
+                    "timestamp": 0.3486708267479094
+                },
+                {
+                    "x": 3.3968844052666176,
+                    "y": 5.398848722834353,
+                    "heading": -0.8527881698077676,
+                    "angularVelocity": -1.1256799666670962,
+                    "velocityX": -3.2337015756857643,
+                    "velocityY": -0.8527133062853276,
+                    "timestamp": 0.43583853343488677
+                },
+                {
+                    "x": 3.0986154730622166,
+                    "y": 5.268356785097819,
+                    "heading": -0.8643849747424638,
+                    "angularVelocity": -0.1330401518573905,
+                    "velocityX": -3.4217824873550513,
+                    "velocityY": -1.4970215770977777,
+                    "timestamp": 0.5230062401218641
+                },
+                {
+                    "x": 2.8320964566491864,
+                    "y": 5.083619646782129,
+                    "heading": -0.847429861393553,
+                    "angularVelocity": 0.19451140787489107,
+                    "velocityX": -3.0575430574319302,
+                    "velocityY": -2.11933002871221,
+                    "timestamp": 0.6101739468088415
+                },
+                {
+                    "x": 2.6201643415258493,
+                    "y": 4.879059787480057,
+                    "heading": -0.7372423358568471,
+                    "angularVelocity": 1.2640865490747921,
+                    "velocityX": -2.4313145679557007,
+                    "velocityY": -2.3467390284415206,
+                    "timestamp": 0.6973416534958189
+                },
+                {
+                    "x": 2.4692268443526784,
+                    "y": 4.691160871442081,
+                    "heading": -0.6128307115212047,
+                    "angularVelocity": 1.4272673799072706,
+                    "velocityX": -1.7315758657640712,
+                    "velocityY": -2.1556023805091904,
+                    "timestamp": 0.7845093601827963
+                },
+                {
+                    "x": 2.3748885078351467,
+                    "y": 4.53130895090854,
+                    "heading": -0.5057978375616147,
+                    "angularVelocity": 1.227895949401916,
+                    "velocityX": -1.0822624582324516,
+                    "velocityY": -1.8338433648091068,
+                    "timestamp": 0.8716770668697736
+                },
+                {
+                    "x": 2.322390449292023,
+                    "y": 4.41825305597818,
+                    "heading": -0.4294901557678491,
+                    "angularVelocity": 0.8754122907900764,
+                    "velocityX": -0.6022649962748851,
+                    "velocityY": -1.2969928799015857,
+                    "timestamp": 0.958844773556751
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": 0.44973234126302364,
-                    "velocityX": -0.2564890278375538,
-                    "velocityY": -0.6685323018312901,
-                    "timestamp": 1.0459257389123817
+                    "heading": -0.39,
+                    "angularVelocity": 0.45303653461548066,
+                    "velocityX": -0.2568663343688406,
+                    "velocityY": -0.6682871236634659,
+                    "timestamp": 1.0460124802437283
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "angularVelocity": 0,
-                    "velocityX": 6.1103463204495e-32,
-                    "velocityY": 1.8635567714760387e-30,
-                    "timestamp": 1.1330862171550802
+                    "velocityX": 3.506966732338881e-31,
+                    "velocityY": 5.081769191491498e-30,
+                    "timestamp": 1.1331801869307057
                 }
             ],
             "constraints": [
@@ -23888,7 +21864,7 @@
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -23897,7 +21873,7 @@
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -23908,146 +21884,146 @@
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": 7.124511967920712e-30,
-                    "velocityY": -7.12047614876129e-30,
+                    "heading": -5.481945073939837e-30,
+                    "angularVelocity": 8.103891066426341e-30,
+                    "velocityX": -1.0115776003849141e-29,
+                    "velocityY": 1.8597851705411875e-30,
                     "timestamp": 0
                 },
                 {
-                    "x": 4.30430205284905,
-                    "y": 5.6386850261363115,
-                    "heading": 3.0976627193075768,
-                    "angularVelocity": -0.4082995079820314,
-                    "velocityX": 0.3983158006634516,
-                    "velocityY": 0.758836111615579,
-                    "timestamp": 0.10369172596279416
-                },
-                {
-                    "x": 4.388754424241646,
-                    "y": 5.795256557860491,
-                    "heading": 3.0179562679096885,
-                    "angularVelocity": -0.7686867072352654,
-                    "velocityX": 0.8144562221183627,
-                    "velocityY": 1.5099713142045914,
-                    "timestamp": 0.20738345192558832
-                },
-                {
-                    "x": 4.5199632474011375,
-                    "y": 6.027943066067755,
-                    "heading": 2.911091818022606,
-                    "angularVelocity": -1.030597657574192,
-                    "velocityX": 1.265374087866657,
-                    "velocityY": 2.2440219414493656,
-                    "timestamp": 0.31107517788838246
-                },
-                {
-                    "x": 4.707776355466448,
-                    "y": 6.330145783951522,
-                    "heading": 2.807461071652614,
-                    "angularVelocity": -0.9994119145743484,
-                    "velocityX": 1.8112641709976698,
-                    "velocityY": 2.914434252856334,
-                    "timestamp": 0.41476690385117665
-                },
-                {
-                    "x": 4.978743687535899,
-                    "y": 6.613772866928766,
-                    "heading": 2.807461069210919,
-                    "angularVelocity": -2.3547634644306893e-8,
-                    "velocityX": 2.613201097324575,
-                    "velocityY": 2.7352913681754196,
-                    "timestamp": 0.5184586298139708
-                },
-                {
-                    "x": 5.307032554871728,
-                    "y": 6.82846808577305,
-                    "heading": 2.80746110336737,
-                    "angularVelocity": 3.2940382214109427e-7,
-                    "velocityX": 3.166008322145415,
-                    "velocityY": 2.0705144682548786,
-                    "timestamp": 0.622150355776765
-                },
-                {
-                    "x": 5.675494511223277,
-                    "y": 6.963016991691847,
-                    "heading": 2.8074611309590716,
-                    "angularVelocity": 2.660935682689419e-7,
-                    "velocityX": 3.5534364283198543,
-                    "velocityY": 1.2975857491953324,
-                    "timestamp": 0.7258420817395592
-                },
-                {
-                    "x": 6.055822077771958,
-                    "y": 7.05903034533374,
-                    "heading": 2.8074611577219364,
-                    "angularVelocity": 2.5810029119060704e-7,
-                    "velocityX": 3.6678680291727868,
-                    "velocityY": 0.9259499998712609,
-                    "timestamp": 0.8295338077023534
-                },
-                {
-                    "x": 6.436149809853972,
-                    "y": 7.155043043263379,
-                    "heading": 2.807461184484807,
-                    "angularVelocity": 2.58100350606906e-7,
-                    "velocityX": 3.6678696255714556,
-                    "velocityY": 0.9259436762012891,
-                    "timestamp": 0.9332255336651476
-                },
-                {
-                    "x": 6.816477543748484,
-                    "y": 7.2510557340128905,
-                    "heading": 2.8074612112477313,
-                    "angularVelocity": 2.581008643735431e-7,
-                    "velocityX": 3.667869643051154,
-                    "velocityY": 0.9259436069563147,
-                    "timestamp": 1.0369172596279417
-                },
-                {
-                    "x": 7.162445200777026,
-                    "y": 7.337344185673276,
-                    "heading": 2.898451236968193,
-                    "angularVelocity": 0.8775051709825819,
-                    "velocityX": 3.3365020575767024,
-                    "velocityY": 0.8321633270078459,
-                    "timestamp": 1.1406089855907358
-                },
-                {
-                    "x": 7.421534649307807,
-                    "y": 7.402207433353085,
-                    "heading": 3.009161772576331,
-                    "angularVelocity": 1.0676891968008626,
-                    "velocityX": 2.498651132721501,
-                    "velocityY": 0.6255392807626672,
-                    "timestamp": 1.2443007115535298
-                },
-                {
-                    "x": 7.593924390045372,
-                    "y": 7.445415282758517,
-                    "heading": 3.094199643650422,
-                    "angularVelocity": 0.8201027640778757,
-                    "velocityX": 1.6625216634876054,
-                    "velocityY": 0.416695247419558,
-                    "timestamp": 1.347992437516324
+                    "x": 4.304265157789358,
+                    "y": 5.638698398929109,
+                    "heading": -0.04261689056565391,
+                    "angularVelocity": -0.41098438981919094,
+                    "velocityX": 0.39794868817859114,
+                    "velocityY": 0.7589435323492035,
+                    "timestamp": 0.10369466972795005
+                },
+                {
+                    "x": 4.388659791773407,
+                    "y": 5.795293123128996,
+                    "heading": -0.12280542510902584,
+                    "angularVelocity": -0.7733139490558338,
+                    "velocityX": 0.8138762986126182,
+                    "velocityY": 1.5101521091752137,
+                    "timestamp": 0.2073893394559001
+                },
+                {
+                    "x": 4.519821289569113,
+                    "y": 6.028001369445331,
+                    "heading": -0.2301897087601478,
+                    "angularVelocity": -1.035581519598341,
+                    "velocityX": 1.2648817739601788,
+                    "velocityY": 2.244167872146774,
+                    "timestamp": 0.31108400918385015
+                },
+                {
+                    "x": 4.707676223581395,
+                    "y": 6.330157885665521,
+                    "heading": -0.33387515355278086,
+                    "angularVelocity": -0.9999110375168158,
+                    "velocityX": 1.811616108187066,
+                    "velocityY": 2.9139059607683615,
+                    "timestamp": 0.4147786789118002
+                },
+                {
+                    "x": 4.978690084088412,
+                    "y": 6.613755913169546,
+                    "heading": -0.33387515567200693,
+                    "angularVelocity": -2.0437175255333274e-8,
+                    "velocityX": 2.613575617898612,
+                    "velocityY": 2.734933514403967,
+                    "timestamp": 0.5184733486397503
+                },
+                {
+                    "x": 5.307019069498364,
+                    "y": 6.828410126616754,
+                    "heading": -0.33387512130937075,
+                    "angularVelocity": 3.313828600417541e-7,
+                    "velocityX": 3.1663053295925763,
+                    "velocityY": 2.0700602452408625,
+                    "timestamp": 0.6221680183677003
+                },
+                {
+                    "x": 5.675511663962328,
+                    "y": 6.962907583051813,
+                    "heading": -0.33387509354517536,
+                    "angularVelocity": 2.6774949424459703e-7,
+                    "velocityX": 3.5536310152752564,
+                    "velocityY": 1.2970527490748707,
+                    "timestamp": 0.7258626880956504
+                },
+                {
+                    "x": 6.0558437987294225,
+                    "y": 7.05894833386441,
+                    "heading": -0.33387506661120775,
+                    "angularVelocity": 2.597430292697957e-7,
+                    "velocityX": 3.6678079573899045,
+                    "velocityY": 0.9261879232999765,
+                    "timestamp": 0.8295573578236004
+                },
+                {
+                    "x": 6.43617610221841,
+                    "y": 7.15498841651687,
+                    "heading": -0.333875039677234,
+                    "angularVelocity": 2.5974308807396863e-7,
+                    "velocityX": 3.667809584492749,
+                    "velocityY": 0.9261814797658935,
+                    "timestamp": 0.9332520275515505
+                },
+                {
+                    "x": 6.8165084241422855,
+                    "y": 7.251028426164244,
+                    "heading": -0.33387501274320475,
+                    "angularVelocity": 2.5974362337794873e-7,
+                    "velocityX": 3.667809762273227,
+                    "velocityY": 0.9261807757269309,
+                    "timestamp": 1.0369466972795005
+                },
+                {
+                    "x": 7.162466794959021,
+                    "y": 7.337325744518129,
+                    "heading": -0.24283225626293373,
+                    "angularVelocity": 0.8779887791641197,
+                    "velocityX": 3.3363177849390095,
+                    "velocityY": 0.8322252106139182,
+                    "timestamp": 1.1406413670074507
+                },
+                {
+                    "x": 7.421550082994963,
+                    "y": 7.402199028754473,
+                    "heading": -0.13158720138626476,
+                    "angularVelocity": 1.0728136284004517,
+                    "velocityX": 2.498520789117373,
+                    "velocityY": 0.6256183119782293,
+                    "timestamp": 1.2443360367354006
+                },
+                {
+                    "x": 7.5939307756893495,
+                    "y": 7.445412774465034,
+                    "heading": -0.046073125428300596,
+                    "angularVelocity": 0.8246718580841027,
+                    "velocityX": 1.6623872099370305,
+                    "velocityY": 0.41674028013136566,
+                    "timestamp": 1.3480307064633505
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
-                    "angularVelocity": 0.4416973092532584,
-                    "velocityX": 0.8301106877660893,
-                    "velocityY": 0.20816238751031255,
-                    "timestamp": 1.451684163479118
+                    "heading": 6.33230436519987e-31,
+                    "angularVelocity": 0.444315272416587,
+                    "velocityX": 0.8300255407192906,
+                    "velocityY": 0.2081806672570287,
+                    "timestamp": 1.4517253761913005
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
+                    "heading": 0,
                     "angularVelocity": 0,
-                    "velocityX": -4.6860665144292015e-31,
-                    "velocityY": 3.7247780913146357e-28,
-                    "timestamp": 1.555375889441912
+                    "velocityX": 2.6078100454164866e-31,
+                    "velocityY": 7.556791844119902e-29,
+                    "timestamp": 1.5554200459192504
                 }
             ],
             "constraints": [
@@ -24090,7 +22066,7 @@
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -24099,7 +22075,7 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -24147,7 +22123,7 @@
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -24156,7 +22132,7 @@
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -24167,146 +22143,146 @@
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": -3.269880825493717e-29,
-                    "velocityX": -1.6106277248271234e-30,
-                    "velocityY": 0,
+                    "heading": 0,
+                    "angularVelocity": -9.522541691773037e-34,
+                    "velocityX": 0,
+                    "velocityY": -4.2308696063129985e-35,
                     "timestamp": 0
                 },
                 {
-                    "x": 4.257789422547788,
-                    "y": 5.465225005289041,
-                    "heading": 3.082306365857339,
-                    "angularVelocity": -0.5369837298016524,
-                    "velocityX": -0.04849747041052485,
-                    "velocityY": -0.882118641130872,
-                    "timestamp": 0.1074401903461163
-                },
-                {
-                    "x": 4.273795168733638,
-                    "y": 5.277874820636679,
-                    "heading": 2.968634839381067,
-                    "angularVelocity": -1.057998185875108,
-                    "velocityX": 0.14897354643815672,
-                    "velocityY": -1.7437625905987038,
-                    "timestamp": 0.2148803806922326
-                },
-                {
-                    "x": 4.354522429535891,
-                    "y": 5.020377016639425,
-                    "heading": 2.810559381522931,
-                    "angularVelocity": -1.471287954245977,
-                    "velocityX": 0.7513693017686587,
-                    "velocityY": -2.396661837322988,
-                    "timestamp": 0.3223205710383489
-                },
-                {
-                    "x": 4.529578110420142,
-                    "y": 4.7442561245273325,
-                    "heading": 2.6367348361471494,
-                    "angularVelocity": -1.6178726491066266,
-                    "velocityX": 1.6293314477600391,
-                    "velocityY": -2.5699963042002807,
-                    "timestamp": 0.4297607613844652
-                },
-                {
-                    "x": 4.793919720832893,
-                    "y": 4.487710124588879,
-                    "heading": 2.5690751936264027,
-                    "angularVelocity": -0.6297423925143462,
-                    "velocityX": 2.4603605928208117,
-                    "velocityY": -2.3878029172509625,
-                    "timestamp": 0.5372009517305815
-                },
-                {
-                    "x": 5.141097516271185,
-                    "y": 4.276379117165763,
-                    "heading": 2.5690757140293337,
-                    "angularVelocity": 0.000004843652357933311,
-                    "velocityX": 3.231358715205786,
-                    "velocityY": -1.9669641941467053,
-                    "timestamp": 0.6446411420766978
-                },
-                {
-                    "x": 5.528234411762885,
-                    "y": 4.152611717586108,
-                    "heading": 2.5690757988249406,
-                    "angularVelocity": 7.892354497964203e-7,
-                    "velocityX": 3.6032781982659046,
-                    "velocityY": -1.1519655650361502,
-                    "timestamp": 0.752081332422814
-                },
-                {
-                    "x": 5.933619437652518,
-                    "y": 4.123348539284839,
-                    "heading": 2.569075852374573,
-                    "angularVelocity": 4.984134150731162e-7,
-                    "velocityX": 3.773122744698165,
-                    "velocityY": -0.2723671487085231,
-                    "timestamp": 0.8595215227689303
-                },
-                {
-                    "x": 6.340054515622032,
-                    "y": 4.125318524745646,
-                    "heading": 2.569075903811562,
-                    "angularVelocity": 4.787499761820812e-7,
-                    "velocityX": 3.782896108618124,
-                    "velocityY": 0.01833564753065316,
-                    "timestamp": 0.9669617131150466
-                },
-                {
-                    "x": 6.746489593269827,
-                    "y": 4.127288577706153,
-                    "heading": 2.5690759552486404,
-                    "angularVelocity": 4.787508167496477e-7,
-                    "velocityX": 3.7828961056237245,
-                    "velocityY": 0.01833627578441685,
-                    "timestamp": 1.074401903461163
-                },
-                {
-                    "x": 7.118708956476355,
-                    "y": 4.125866161972181,
-                    "heading": 2.656486209298221,
-                    "angularVelocity": 0.8135712880626333,
-                    "velocityX": 3.464433207046916,
-                    "velocityY": -0.013239140115027938,
-                    "timestamp": 1.1818420938072793
-                },
-                {
-                    "x": 7.400847427365447,
-                    "y": 4.126314118090732,
-                    "heading": 2.8696985577774003,
-                    "angularVelocity": 1.9844747835268808,
-                    "velocityX": 2.6260049426586924,
-                    "velocityY": 0.004169353359354839,
-                    "timestamp": 1.2892822841533955
-                },
-                {
-                    "x": 7.587357907068713,
-                    "y": 4.126749409200305,
-                    "heading": 3.0436879317659473,
-                    "angularVelocity": 1.6194067920769941,
-                    "velocityX": 1.7359470334371772,
-                    "velocityY": 0.004051473737811161,
-                    "timestamp": 1.3967224744995117
+                    "x": 4.271262994735721,
+                    "y": 5.464085078742271,
+                    "heading": 0.013066573413208545,
+                    "angularVelocity": 0.12156077000957308,
+                    "velocityX": 0.0768721814736735,
+                    "velocityY": -0.8923144052220247,
+                    "timestamp": 0.10749005137249076
+                },
+                {
+                    "x": 4.301424317332477,
+                    "y": 5.27437671657762,
+                    "heading": 0.033784324267673776,
+                    "angularVelocity": 0.192741101059399,
+                    "velocityX": 0.2805964106597774,
+                    "velocityY": -1.7648922829820315,
+                    "timestamp": 0.21498010274498153
+                },
+                {
+                    "x": 4.379013311524821,
+                    "y": 5.000817118183451,
+                    "heading": 0.051063906614390905,
+                    "angularVelocity": 0.16075517804747627,
+                    "velocityX": 0.7218248870629933,
+                    "velocityY": -2.544975975927198,
+                    "timestamp": 0.32247015411747226
+                },
+                {
+                    "x": 4.5426840546799285,
+                    "y": 4.684552935799795,
+                    "heading": 0.043724640930798246,
+                    "angularVelocity": -0.06827855778168279,
+                    "velocityX": 1.5226594560638123,
+                    "velocityY": -2.9422646872470954,
+                    "timestamp": 0.42996020548996305
+                },
+                {
+                    "x": 4.7987371937718075,
+                    "y": 4.3933637164950845,
+                    "heading": -0.0025480008668042047,
+                    "angularVelocity": -0.4304830187237655,
+                    "velocityX": 2.382110119238521,
+                    "velocityY": -2.7089876280330105,
+                    "timestamp": 0.5374502568624538
+                },
+                {
+                    "x": 5.129552682758233,
+                    "y": 4.161939062753506,
+                    "heading": -0.00976861092561694,
+                    "angularVelocity": -0.06717468236935505,
+                    "velocityX": 3.0776382070935524,
+                    "velocityY": -2.152986725623672,
+                    "timestamp": 0.6449403082349446
+                },
+                {
+                    "x": 5.508331065778466,
+                    "y": 4.0140416634822165,
+                    "heading": -0.009768639154040644,
+                    "angularVelocity": -2.6261429169311795e-7,
+                    "velocityX": 3.5238459576843355,
+                    "velocityY": -1.3759170954228361,
+                    "timestamp": 0.7524303596074354
+                },
+                {
+                    "x": 5.911272135069668,
+                    "y": 3.959404581326429,
+                    "heading": -0.009768652411608816,
+                    "angularVelocity": -1.233376298753571e-7,
+                    "velocityX": 3.7486359355701584,
+                    "velocityY": -0.5082989677477323,
+                    "timestamp": 0.8599204109799262
+                },
+                {
+                    "x": 6.316087697151271,
+                    "y": 3.997759236961094,
+                    "heading": -0.009768652034230711,
+                    "angularVelocity": 3.51081891151765e-9,
+                    "velocityX": 3.7660746916826353,
+                    "velocityY": 0.3568205163634492,
+                    "timestamp": 0.967410462352417
+                },
+                {
+                    "x": 6.720902827559142,
+                    "y": 4.036118448432054,
+                    "heading": -0.009768651656829377,
+                    "angularVelocity": 3.5110350109919825e-9,
+                    "velocityX": 3.766070675741356,
+                    "velocityY": 0.35686290015837574,
+                    "timestamp": 1.0749005137249077
+                },
+                {
+                    "x": 7.104541677751467,
+                    "y": 4.072471064803872,
+                    "heading": -0.005861206189315246,
+                    "angularVelocity": 0.03635169411142489,
+                    "velocityX": 3.569063790497984,
+                    "velocityY": 0.3381951716242375,
+                    "timestamp": 1.1823905650973985
+                },
+                {
+                    "x": 7.392270834070994,
+                    "y": 4.099735530737146,
+                    "heading": -0.0029306066828158664,
+                    "angularVelocity": 0.027263913907193366,
+                    "velocityX": 2.6767980166131413,
+                    "velocityY": 0.2536464127158502,
+                    "timestamp": 1.2898806164698893
+                },
+                {
+                    "x": 7.584090276967534,
+                    "y": 4.117911843086064,
+                    "heading": -0.0009768696053991616,
+                    "angularVelocity": 0.018175980497453854,
+                    "velocityX": 1.7845320608492279,
+                    "velocityY": 0.16909762454137037,
+                    "timestamp": 1.39737066784238
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": 0.8964249590751834,
-                    "velocityX": 0.8622666493129096,
-                    "velocityY": 0.002332374867239492,
-                    "timestamp": 1.5041626648456279
+                    "heading": -6.809535330810639e-32,
+                    "angularVelocity": 0.009088000172350516,
+                    "velocityX": 0.8922660451626774,
+                    "velocityY": 0.08454881915017429,
+                    "timestamp": 1.5048607192148709
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": 4.233852791919794e-30,
-                    "velocityY": -1.3542182850561966e-29,
-                    "timestamp": 1.611602855191744
+                    "heading": 0,
+                    "angularVelocity": -9.489213882041181e-31,
+                    "velocityX": 2.198140684032685e-33,
+                    "velocityY": -6.756921472052438e-32,
+                    "timestamp": 1.6123507705873616
                 }
             ],
             "constraints": [
@@ -24349,23 +22325,186 @@
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 17
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 0,
+                    "angularVelocity": 7.441191049306378e-32,
+                    "velocityX": -6.15829104129807e-34,
+                    "velocityY": 2.4252394047543234e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.262142393092874,
+                    "y": 5.471176724349191,
+                    "heading": -0.06580713819401311,
+                    "angularVelocity": -0.6311237133024392,
+                    "velocityX": -0.008224883662067025,
+                    "velocityY": -0.8518601035521746,
+                    "timestamp": 0.10426979181255681
+                },
+                {
+                    "x": 4.27936690849282,
+                    "y": 5.294863623901166,
+                    "heading": -0.18680392528158013,
+                    "angularVelocity": -1.1604203382805245,
+                    "velocityX": 0.16519180771848577,
+                    "velocityY": -1.6909317395107013,
+                    "timestamp": 0.20853958362511363
+                },
+                {
+                    "x": 4.348116425232891,
+                    "y": 5.0445566747334585,
+                    "heading": -0.3412830267774945,
+                    "angularVelocity": -1.4815326549574157,
+                    "velocityX": 0.6593426105967491,
+                    "velocityY": -2.400570144205118,
+                    "timestamp": 0.31280937543767046
+                },
+                {
+                    "x": 4.502267029635194,
+                    "y": 4.763921050939977,
+                    "heading": -0.4878370803946758,
+                    "angularVelocity": -1.4055274405902505,
+                    "velocityX": 1.4783822018117698,
+                    "velocityY": -2.6914374615609895,
+                    "timestamp": 0.41707916725022726
+                },
+                {
+                    "x": 4.740029978369904,
+                    "y": 4.496898841850153,
+                    "heading": -0.5207249369577782,
+                    "angularVelocity": -0.31541116551017917,
+                    "velocityX": 2.280266840487519,
+                    "velocityY": -2.5608779345205184,
+                    "timestamp": 0.5213489590627841
+                },
+                {
+                    "x": 5.0582868126043,
+                    "y": 4.2697391199027495,
+                    "heading": -0.509978725166827,
+                    "angularVelocity": 0.10306160206274768,
+                    "velocityX": 3.0522438829312937,
+                    "velocityY": -2.1785765368723764,
+                    "timestamp": 0.6256187508753409
+                },
+                {
+                    "x": 5.423286105863135,
+                    "y": 4.1201949004052105,
+                    "heading": -0.5099785687835147,
+                    "angularVelocity": 0.0000014997949991595825,
+                    "velocityX": 3.5005276879710774,
+                    "velocityY": -1.4342046425715584,
+                    "timestamp": 0.7298885426878977
+                },
+                {
+                    "x": 5.805099761015269,
+                    "y": 4.021168099470067,
+                    "heading": -0.5099784851647622,
+                    "angularVelocity": 8.019460959727046e-7,
+                    "velocityX": 3.6617859162748747,
+                    "velocityY": -0.9497170677501874,
+                    "timestamp": 0.8341583345004545
+                },
+                {
+                    "x": 6.154222032422552,
+                    "y": 3.8375880828272497,
+                    "heading": -0.5099784796190555,
+                    "angularVelocity": 5.318612994948929e-8,
+                    "velocityX": 3.348259024386382,
+                    "velocityY": -1.7606251384181746,
+                    "timestamp": 0.9384281263130113
+                },
+                {
+                    "x": 6.493379431179238,
+                    "y": 3.636192348012897,
+                    "heading": -0.5099784741544249,
+                    "angularVelocity": 5.240856867670856e-8,
+                    "velocityX": 3.2526908595576858,
+                    "velocityY": -1.9314868795019449,
+                    "timestamp": 1.0426979181255682
+                },
+                {
+                    "x": 6.795285003697281,
+                    "y": 3.3823400148372036,
+                    "heading": -0.5099784394461939,
+                    "angularVelocity": 3.32869476219209e-7,
+                    "velocityX": 2.895427019368865,
+                    "velocityY": -2.4345721686299853,
+                    "timestamp": 1.146967709938125
+                },
+                {
+                    "x": 7.066681771760518,
+                    "y": 3.0961027469536373,
+                    "heading": -0.509978390292543,
+                    "angularVelocity": 4.714083530965781e-7,
+                    "velocityX": 2.6028321659174227,
+                    "velocityY": -2.7451600593787298,
+                    "timestamp": 1.2512375017506818
+                },
+                {
+                    "x": 7.312943361341716,
+                    "y": 2.8410544331524363,
+                    "heading": -0.40533862017886096,
+                    "angularVelocity": 1.0035482788897323,
+                    "velocityX": 2.361773101301441,
+                    "velocityY": -2.446042227260748,
+                    "timestamp": 1.3555072935632386
+                },
+                {
+                    "x": 7.49731350893092,
+                    "y": 2.6484381177603322,
+                    "heading": -0.22178416093118458,
+                    "angularVelocity": 1.7603800300824186,
+                    "velocityX": 1.768202893515326,
+                    "velocityY": -1.8472878102448496,
+                    "timestamp": 1.4597770853757954
+                },
+                {
+                    "x": 7.619307085639959,
+                    "y": 2.520663921373434,
+                    "heading": -0.0776901780416505,
+                    "angularVelocity": 1.3819341190262298,
+                    "velocityX": 1.1699800545142,
+                    "velocityY": -1.225419118670484,
+                    "timestamp": 1.5640468771883522
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 1.429761379305419e-34,
+                    "angularVelocity": 0.7450880709660586,
+                    "velocityX": 0.5820757220763062,
+                    "velocityY": -0.6105691808408042,
+                    "timestamp": 1.668316669000909
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 0,
+                    "angularVelocity": -5.61453765178046e-32,
+                    "velocityX": 0,
+                    "velocityY": 2.318867528557263e-33,
+                    "timestamp": 1.7725864608134658
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -24406,23 +22545,222 @@
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 21
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 0,
+                    "angularVelocity": -1.2613927548149136e-32,
+                    "velocityX": 4.983015637334932e-33,
+                    "velocityY": -3.6905945238681026e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.258006222511338,
+                    "y": 5.473607841813564,
+                    "heading": -0.06530003091818118,
+                    "angularVelocity": -0.6342708718414432,
+                    "velocityX": -0.04850545332028736,
+                    "velocityY": -0.8391424739420397,
+                    "timestamp": 0.10295290831912149
+                },
+                {
+                    "x": 4.271377859079598,
+                    "y": 5.302453682559796,
+                    "heading": -0.1916286288786279,
+                    "angularVelocity": -1.2270522515873779,
+                    "velocityX": 0.12988109599401854,
+                    "velocityY": -1.662450940416803,
+                    "timestamp": 0.20590581663824298
+                },
+                {
+                    "x": 4.340766271943036,
+                    "y": 5.0642424556348775,
+                    "heading": -0.3653686640748466,
+                    "angularVelocity": -1.6875680156375907,
+                    "velocityX": 0.673982056420939,
+                    "velocityY": -2.3137882242873524,
+                    "timestamp": 0.30885872495736444
+                },
+                {
+                    "x": 4.494848034080926,
+                    "y": 4.801255485663876,
+                    "heading": -0.5565822720790902,
+                    "angularVelocity": -1.8572919514963226,
+                    "velocityX": 1.4966236957608356,
+                    "velocityY": -2.5544394448365244,
+                    "timestamp": 0.41181163327648596
+                },
+                {
+                    "x": 4.73233691115851,
+                    "y": 4.546498868370588,
+                    "heading": -0.6490495118957932,
+                    "angularVelocity": -0.8981508276588281,
+                    "velocityX": 2.3067719111095206,
+                    "velocityY": -2.4744965582091383,
+                    "timestamp": 0.5147645415956075
+                },
+                {
+                    "x": 5.049349589860016,
+                    "y": 4.3202563768587865,
+                    "heading": -0.6490493910522949,
+                    "angularVelocity": 0.0000011737744981731646,
+                    "velocityX": 3.079200810130272,
+                    "velocityY": -2.1975337579636096,
+                    "timestamp": 0.617717449914729
+                },
+                {
+                    "x": 5.4092082222704905,
+                    "y": 4.171311637049865,
+                    "heading": -0.6490493274377749,
+                    "angularVelocity": 6.178992034513071e-7,
+                    "velocityX": 3.4953712166637025,
+                    "velocityY": -1.446726879703489,
+                    "timestamp": 0.7206703582338505
+                },
+                {
+                    "x": 5.789554256648214,
+                    "y": 4.087528164867666,
+                    "heading": -0.6490492895940018,
+                    "angularVelocity": 3.675833320529158e-7,
+                    "velocityX": 3.6943690138288297,
+                    "velocityY": -0.8138038405141147,
+                    "timestamp": 0.823623266552972
+                },
+                {
+                    "x": 6.141218018459777,
+                    "y": 3.9201502365347105,
+                    "heading": -0.6490493503243893,
+                    "angularVelocity": -5.898851085689342e-7,
+                    "velocityX": 3.4157729738097102,
+                    "velocityY": -1.6257717345306835,
+                    "timestamp": 0.9265761748720935
+                },
+                {
+                    "x": 6.456138339829479,
+                    "y": 3.69100407104571,
+                    "heading": -0.6490494074378677,
+                    "angularVelocity": -5.547534235510974e-7,
+                    "velocityX": 3.0588773693847333,
+                    "velocityY": -2.2257376622981755,
+                    "timestamp": 1.029529083191215
+                },
+                {
+                    "x": 6.721775326533507,
+                    "y": 3.406188756476768,
+                    "heading": -0.6490494689481024,
+                    "angularVelocity": -5.974599048336077e-7,
+                    "velocityX": 2.580179531020508,
+                    "velocityY": -2.7664620574496457,
+                    "timestamp": 1.1324819915103363
+                },
+                {
+                    "x": 6.916359430113237,
+                    "y": 3.0688168532177102,
+                    "heading": -0.6490494077682868,
+                    "angularVelocity": 5.942504852606566e-7,
+                    "velocityX": 1.8900301774533879,
+                    "velocityY": -3.276953597204961,
+                    "timestamp": 1.2354348998294578
+                },
+                {
+                    "x": 7.040594979350531,
+                    "y": 2.699698573462511,
+                    "heading": -0.6490493528706274,
+                    "angularVelocity": 5.332307776860117e-7,
+                    "velocityX": 1.206722095234099,
+                    "velocityY": -3.585311826364818,
+                    "timestamp": 1.3383878081485792
+                },
+                {
+                    "x": 7.164812386541997,
+                    "y": 2.330574188065911,
+                    "heading": -0.649049297974002,
+                    "angularVelocity": 5.332207349098274e-7,
+                    "velocityX": 1.2065458783003051,
+                    "velocityY": -3.585371131551044,
+                    "timestamp": 1.4413407164677006
+                },
+                {
+                    "x": 7.289029734328084,
+                    "y": 1.9614497826782673,
+                    "heading": -0.6490492430773671,
+                    "angularVelocity": 5.332208261000816e-7,
+                    "velocityX": 1.206545301285243,
+                    "velocityY": -3.5853713257276327,
+                    "timestamp": 1.544293624786822
+                },
+                {
+                    "x": 7.413246520819429,
+                    "y": 1.5923251884092744,
+                    "heading": -0.6490491881699392,
+                    "angularVelocity": 5.333256623024379e-7,
+                    "velocityX": 1.2065398493290926,
+                    "velocityY": -3.585373160365936,
+                    "timestamp": 1.6472465331059434
+                },
+                {
+                    "x": 7.520074458284658,
+                    "y": 1.2704783284902474,
+                    "heading": -0.5256216946651037,
+                    "angularVelocity": 1.1988733054752465,
+                    "velocityX": 1.037638850707321,
+                    "velocityY": -3.1261560763432104,
+                    "timestamp": 1.7501994414250648
+                },
+                {
+                    "x": 7.601104601009551,
+                    "y": 1.0271731610238788,
+                    "heading": -0.2913266412644364,
+                    "angularVelocity": 2.275749730881093,
+                    "velocityX": 0.7870602593733868,
+                    "velocityY": -2.3632665792422265,
+                    "timestamp": 1.8531523497441862
+                },
+                {
+                    "x": 7.654009357663848,
+                    "y": 0.8666565360101991,
+                    "heading": -0.10310334462657153,
+                    "angularVelocity": 1.828246522715339,
+                    "velocityX": 0.5138733574219042,
+                    "velocityY": -1.5591266690216163,
+                    "timestamp": 1.9561052580633076
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": -1.429149737287701e-32,
+                    "angularVelocity": 1.0014612147428026,
+                    "velocityX": 0.2524517545010862,
+                    "velocityY": -0.7737181718391956,
+                    "timestamp": 2.059058166382429
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 0,
+                    "angularVelocity": -8.032741852653566e-32,
+                    "velocityX": 0,
+                    "velocityY": 2.733638748586157e-32,
+                    "timestamp": 2.1620110747015504
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -24463,23 +22801,222 @@
                 {
                     "x": 4.263,
                     "y": 3,
-                    "heading": 2.574,
+                    "heading": -0.566,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 21
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": -0.566,
+                    "angularVelocity": -1.6794679447375943e-30,
+                    "velocityX": 6.742046899995187e-33,
+                    "velocityY": 9.708842875589211e-32,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.290023963257714,
+                    "y": 3.0497874441329906,
+                    "heading": -0.5200869306801483,
+                    "angularVelocity": 0.5503538158132694,
+                    "velocityX": 0.3239326300245928,
+                    "velocityY": 0.5967954280576505,
+                    "timestamp": 0.08342464065958187
+                },
+                {
+                    "x": 4.342730369898429,
+                    "y": 3.150178003649595,
+                    "heading": -0.42997936088427535,
+                    "angularVelocity": 1.0801073769506666,
+                    "velocityX": 0.6317846408926776,
+                    "velocityY": 1.2033681982072046,
+                    "timestamp": 0.16684928131916374
+                },
+                {
+                    "x": 4.418537982231504,
+                    "y": 3.3026029150734924,
+                    "heading": -0.2990998156933407,
+                    "angularVelocity": 1.5688355880967437,
+                    "velocityX": 0.9086957010988046,
+                    "velocityY": 1.8270970089745318,
+                    "timestamp": 0.2502739219787456
+                },
+                {
+                    "x": 4.5104462205151705,
+                    "y": 3.5100667276192694,
+                    "heading": -0.13617594937576347,
+                    "angularVelocity": 1.9529465758491624,
+                    "velocityX": 1.1016917490685052,
+                    "velocityY": 2.4868409489750483,
+                    "timestamp": 0.3336985626383275
+                },
+                {
+                    "x": 4.573472263152817,
+                    "y": 3.7674681923433693,
+                    "heading": 0.004287761033867021,
+                    "angularVelocity": 1.6837196935950753,
+                    "velocityX": 0.7554847361563879,
+                    "velocityY": 3.0854369007645883,
+                    "timestamp": 0.41712320329790936
+                },
+                {
+                    "x": 4.6071871310155705,
+                    "y": 4.066840423815047,
+                    "heading": 0.04705620405427594,
+                    "angularVelocity": 0.5126596013152507,
+                    "velocityX": 0.40413560785150865,
+                    "velocityY": 3.5885348633778387,
+                    "timestamp": 0.5005478439574912
+                },
+                {
+                    "x": 4.587417601167403,
+                    "y": 4.370555626705338,
+                    "heading": 0.08458578048595149,
+                    "angularVelocity": 0.4498620088136386,
+                    "velocityX": -0.23697470785444333,
+                    "velocityY": 3.6405934804036506,
+                    "timestamp": 0.5839724846170731
+                },
+                {
+                    "x": 4.510022832963655,
+                    "y": 4.67116829443229,
+                    "heading": 0.1002321163817716,
+                    "angularVelocity": 0.1875505338964035,
+                    "velocityX": -0.9277207260569706,
+                    "velocityY": 3.603403806719601,
+                    "timestamp": 0.667397125276655
+                },
+                {
+                    "x": 4.376634060333898,
+                    "y": 4.957183607465761,
+                    "heading": 0.10023219176740035,
+                    "angularVelocity": 9.036374403612271e-7,
+                    "velocityX": -1.5989133615097586,
+                    "velocityY": 3.4284272700744434,
+                    "timestamp": 0.7508217659362368
+                },
+                {
+                    "x": 4.19313075849175,
+                    "y": 5.213940045953902,
+                    "heading": 0.10023221692184411,
+                    "angularVelocity": 3.015229499928316e-7,
+                    "velocityX": -2.1996295146291525,
+                    "velocityY": 3.0777050576201734,
+                    "timestamp": 0.8342464065958187
+                },
+                {
+                    "x": 3.9657176811920825,
+                    "y": 5.432756463781841,
+                    "heading": 0.10023223856547658,
+                    "angularVelocity": 2.5943932502769966e-7,
+                    "velocityX": -2.725970115084308,
+                    "velocityY": 2.6229231087830502,
+                    "timestamp": 0.9176710472554006
+                },
+                {
+                    "x": 3.7186426713834653,
+                    "y": 5.629101242954768,
+                    "heading": 0.10023225962798245,
+                    "angularVelocity": 2.524734383899783e-7,
+                    "velocityX": -2.961655068037016,
+                    "velocityY": 2.3535585843770224,
+                    "timestamp": 1.0010956879149824
+                },
+                {
+                    "x": 3.4715770348675625,
+                    "y": 5.825457816649942,
+                    "heading": 0.10023228069061084,
+                    "angularVelocity": 2.5247490699416256e-7,
+                    "velocityX": -2.961542711631996,
+                    "velocityY": 2.353699963736333,
+                    "timestamp": 1.0845203285745642
+                },
+                {
+                    "x": 3.224511384705754,
+                    "y": 6.021814373175139,
+                    "heading": 0.10023230175325984,
+                    "angularVelocity": 2.5247515406639525e-7,
+                    "velocityX": -2.96154287520364,
+                    "velocityY": 2.353699757922098,
+                    "timestamp": 1.167944969234146
+                },
+                {
+                    "x": 2.9774457295747583,
+                    "y": 6.218170923447359,
+                    "heading": 0.10023232281654228,
+                    "angularVelocity": 2.524827469327523e-7,
+                    "velocityX": -2.9615429347686173,
+                    "velocityY": 2.3536996829685104,
+                    "timestamp": 1.2513696098937277
+                },
+                {
+                    "x": 2.751412346836958,
+                    "y": 6.39797212959724,
+                    "heading": 0.1658210049038553,
+                    "angularVelocity": 0.7862027521934517,
+                    "velocityX": -2.7094319010631414,
+                    "velocityY": 2.155252989144637,
+                    "timestamp": 1.3347942505533095
+                },
+                {
+                    "x": 2.5706626829115913,
+                    "y": 6.54172747182469,
+                    "heading": 0.24638520673123038,
+                    "angularVelocity": 0.9657123026291593,
+                    "velocityX": -2.166622025534686,
+                    "velocityY": 1.723176043563091,
+                    "timestamp": 1.4182188912128912
+                },
+                {
+                    "x": 2.435257350873027,
+                    "y": 6.649419264379792,
+                    "heading": 0.31518856404188306,
+                    "angularVelocity": 0.8247366337651717,
+                    "velocityX": -1.6230855891977036,
+                    "velocityY": 1.2908871012647558,
+                    "timestamp": 1.501643531872473
+                },
+                {
+                    "x": 2.3450659538089185,
+                    "y": 6.721154353305962,
+                    "heading": 0.3643262231924431,
+                    "angularVelocity": 0.5890065424563048,
+                    "velocityX": -1.0811122031935256,
+                    "velocityY": 0.8598789081859941,
+                    "timestamp": 1.5850681725320548
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 0.39,
+                    "angularVelocity": 0.3077481257883978,
+                    "velocityX": -0.5401995555822935,
+                    "velocityY": 0.42967696846676384,
+                    "timestamp": 1.6684928131916366
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 0.39,
+                    "angularVelocity": -2.8369790280137563e-29,
+                    "velocityX": 0,
+                    "velocityY": -1.9155196850548007e-31,
+                    "timestamp": 1.7519174538512183
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -24520,23 +23057,177 @@
                 {
                     "x": 4.263,
                     "y": 3,
-                    "heading": 2.574,
+                    "heading": -0.566,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 16
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": -0.566,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 2.1175745999315144e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.331197427717942,
+                    "y": 3.064203342806604,
+                    "heading": -0.5918486780870994,
+                    "angularVelocity": -0.24352382070717674,
+                    "velocityX": 0.642497001367488,
+                    "velocityY": 0.604868198278966,
+                    "timestamp": 0.10614435176007272
+                },
+                {
+                    "x": 4.457228812945604,
+                    "y": 3.20188389088458,
+                    "heading": -0.6500618235234895,
+                    "angularVelocity": -0.5484337552691863,
+                    "velocityX": 1.1873583769444531,
+                    "velocityY": 1.2971066834454588,
+                    "timestamp": 0.21228870352014545
+                },
+                {
+                    "x": 4.610677951553499,
+                    "y": 3.4285671398637003,
+                    "heading": -0.7523465196948876,
+                    "angularVelocity": -0.9636376733695737,
+                    "velocityX": 1.4456646638603075,
+                    "velocityY": 2.1356129197671536,
+                    "timestamp": 0.31843305528021815
+                },
+                {
+                    "x": 4.708912918877337,
+                    "y": 3.730071149255109,
+                    "heading": -0.9027593729452231,
+                    "angularVelocity": -1.4170594172577993,
+                    "velocityX": 0.9254846413861598,
+                    "velocityY": 2.840509215911215,
+                    "timestamp": 0.4245774070402909
+                },
+                {
+                    "x": 4.717930123488916,
+                    "y": 4.059451821332611,
+                    "heading": -1.0196863730682848,
+                    "angularVelocity": -1.1015847587195386,
+                    "velocityX": 0.08495227925044571,
+                    "velocityY": 3.10313894819415,
+                    "timestamp": 0.5307217588003637
+                },
+                {
+                    "x": 4.639893034223715,
+                    "y": 4.389369622514019,
+                    "heading": -1.0106655427651627,
+                    "angularVelocity": 0.08498643737080536,
+                    "velocityX": -0.7351977563685651,
+                    "velocityY": 3.1081993126412395,
+                    "timestamp": 0.6368661105604364
+                },
+                {
+                    "x": 4.471904603111427,
+                    "y": 4.710090421849215,
+                    "heading": -0.9107146957571205,
+                    "angularVelocity": 0.9416501712118399,
+                    "velocityX": -1.5826412647185075,
+                    "velocityY": 3.021553139823678,
+                    "timestamp": 0.7430104623205092
+                },
+                {
+                    "x": 4.218561716713538,
+                    "y": 4.992188206454831,
+                    "heading": -0.8355436069817835,
+                    "angularVelocity": 0.7081967860640641,
+                    "velocityX": -2.3867768957743736,
+                    "velocityY": 2.657680601255776,
+                    "timestamp": 0.8491548140805819
+                },
+                {
+                    "x": 3.8896224481808446,
+                    "y": 5.222473200709689,
+                    "heading": -0.83554222765338,
+                    "angularVelocity": 0.000012994835623464205,
+                    "velocityX": -3.098980426921093,
+                    "velocityY": 2.169545439171289,
+                    "timestamp": 0.9552991658406547
+                },
+                {
+                    "x": 3.516178298161353,
+                    "y": 5.370026719708121,
+                    "heading": -0.8355421446101063,
+                    "angularVelocity": 7.823616840140413e-7,
+                    "velocityX": -3.518266811442025,
+                    "velocityY": 1.390121250464274,
+                    "timestamp": 1.0614435176007273
+                },
+                {
+                    "x": 3.1189612059977283,
+                    "y": 5.42877328608909,
+                    "heading": -0.8355420737846555,
+                    "angularVelocity": 6.672559542292647e-7,
+                    "velocityX": -3.742234848835754,
+                    "velocityY": 0.55345918465599,
+                    "timestamp": 1.1675878693608
+                },
+                {
+                    "x": 2.774416222135212,
+                    "y": 5.484170620762628,
+                    "heading": -0.7035735462776811,
+                    "angularVelocity": 1.243292980914089,
+                    "velocityX": -3.2460039384980304,
+                    "velocityY": 0.5219056290320314,
+                    "timestamp": 1.2737322211208726
+                },
+                {
+                    "x": 2.5095166350816602,
+                    "y": 5.523131311595933,
+                    "heading": -0.3884677902217159,
+                    "angularVelocity": 2.9686530732056857,
+                    "velocityX": -2.4956541036901063,
+                    "velocityY": 0.3670538298766209,
+                    "timestamp": 1.3798765728809452
+                },
+                {
+                    "x": 2.335800273937377,
+                    "y": 5.547984865708467,
+                    "heading": -0.13742924633332027,
+                    "angularVelocity": 2.365067379711733,
+                    "velocityX": -1.6366048523895966,
+                    "velocityY": 0.23414862590816604,
+                    "timestamp": 1.4860209246410179
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 0,
+                    "angularVelocity": 1.2947391364164484,
+                    "velocityX": -0.8083357476365635,
+                    "velocityY": 0.11319617193282665,
+                    "timestamp": 1.5921652764010905
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": -1.986468774957569e-34,
+                    "velocityY": 0,
+                    "timestamp": 1.6983096281611632
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -24577,23 +23268,141 @@
                 {
                     "x": 4.263,
                     "y": 3,
-                    "heading": 2.574,
+                    "heading": -0.566,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 12
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": -0.566,
+                    "angularVelocity": 0,
+                    "velocityX": -3.809622996347125e-33,
+                    "velocityY": 4.7029981856297106e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.1621681366579715,
+                    "y": 2.9362991956112916,
+                    "heading": -0.5618109581410129,
+                    "angularVelocity": 0.03502487973446616,
+                    "velocityX": -0.8430624486077395,
+                    "velocityY": -0.5326069988815002,
+                    "timestamp": 0.11960189130537771
+                },
+                {
+                    "x": 3.949371568032143,
+                    "y": 2.832059073063443,
+                    "heading": -0.5332490097615643,
+                    "angularVelocity": 0.2388085010004113,
+                    "velocityX": -1.7792073879709687,
+                    "velocityY": -0.8715591485229383,
+                    "timestamp": 0.23920378261075542
+                },
+                {
+                    "x": 3.6196334559490815,
+                    "y": 2.7428786025637635,
+                    "heading": -0.4400400521459035,
+                    "angularVelocity": 0.7793267865446122,
+                    "velocityX": -2.756964028613438,
+                    "velocityY": -0.7456443165432319,
+                    "timestamp": 0.35880567391613316
+                },
+                {
+                    "x": 3.2124362068162218,
+                    "y": 2.7443781288173086,
+                    "heading": -0.34020495702705544,
+                    "angularVelocity": 0.8347283979314292,
+                    "velocityX": -3.4046054346513173,
+                    "velocityY": 0.012537646664080927,
+                    "timestamp": 0.47840756522151084
+                },
+                {
+                    "x": 2.829910158877178,
+                    "y": 2.8625567119312745,
+                    "heading": -0.23363822858373923,
+                    "angularVelocity": 0.891012067453128,
+                    "velocityX": -3.1983277502054186,
+                    "velocityY": 0.9880996180254248,
+                    "timestamp": 0.5980094565268885
+                },
+                {
+                    "x": 2.5228974587582895,
+                    "y": 3.073051960500077,
+                    "heading": -0.12255766699698727,
+                    "angularVelocity": 0.9287525504349555,
+                    "velocityX": -2.566955227614215,
+                    "velocityY": 1.7599658857512914,
+                    "timestamp": 0.7176113478322662
+                },
+                {
+                    "x": 2.307806503132395,
+                    "y": 3.359545759981809,
+                    "heading": -0.008191563913849259,
+                    "angularVelocity": 0.9562231987710198,
+                    "velocityX": -1.7983909224036065,
+                    "velocityY": 2.395395226236259,
+                    "timestamp": 0.8372132391376439
+                },
+                {
+                    "x": 2.197079056424326,
+                    "y": 3.692587049160117,
+                    "heading": -0.017161158826022753,
+                    "angularVelocity": -0.07499542703130056,
+                    "velocityX": -0.9258001315827844,
+                    "velocityY": 2.7845821294578066,
+                    "timestamp": 0.9568151304430216
+                },
+                {
+                    "x": 2.2005818293568207,
+                    "y": 4.024472502975606,
+                    "heading": -0.15274865723996348,
+                    "angularVelocity": -1.133656808718376,
+                    "velocityX": 0.029286935969546842,
+                    "velocityY": 2.774918107006309,
+                    "timestamp": 1.0764170217483993
+                },
+                {
+                    "x": 2.2590478504225837,
+                    "y": 4.250579625353832,
+                    "heading": -0.3027008584759083,
+                    "angularVelocity": -1.2537611203242134,
+                    "velocityX": 0.48883859968803833,
+                    "velocityY": 1.8904978835235247,
+                    "timestamp": 1.196018913053777
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": -0.39,
+                    "angularVelocity": -0.729914389908579,
+                    "velocityX": 0.3424038627687911,
+                    "velocityY": 0.914871608232253,
+                    "timestamp": 1.3156208043591548
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": -0.39,
+                    "angularVelocity": 3.1982065543283105e-28,
+                    "velocityX": 4.1170744495691044e-30,
+                    "velocityY": 1.8090480101607624e-30,
+                    "timestamp": 1.4352226956645326
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -24634,7 +23443,7 @@
                 {
                     "x": 4.263,
                     "y": 3,
-                    "heading": 2.574,
+                    "heading": -0.566,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -24643,14 +23452,231 @@
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": -0.566,
+                    "angularVelocity": 3.2688003829398086e-32,
+                    "velocityX": -2.078713781970587e-32,
+                    "velocityY": -4.638967376158192e-34,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.29154268151456,
+                    "y": 3.0623077457400183,
+                    "heading": -0.5127665183876435,
+                    "angularVelocity": 0.580669777278592,
+                    "velocityX": 0.31134301225465,
+                    "velocityY": 0.6796516730776717,
+                    "timestamp": 0.09167599846825866
+                },
+                {
+                    "x": 4.3492977760941525,
+                    "y": 3.186661868715021,
+                    "heading": -0.4070750475472326,
+                    "angularVelocity": 1.1528804987818582,
+                    "velocityX": 0.6299914431757085,
+                    "velocityY": 1.3564523436094174,
+                    "timestamp": 0.18335199693651733
+                },
+                {
+                    "x": 4.438053987492854,
+                    "y": 3.3723523918382017,
+                    "heading": -0.25179906449139605,
+                    "angularVelocity": 1.6937473891773143,
+                    "velocityX": 0.968151019696088,
+                    "velocityY": 2.025508597950785,
+                    "timestamp": 0.27502799540477596
+                },
+                {
+                    "x": 4.5709698511757795,
+                    "y": 3.6117104026755453,
+                    "heading": -0.06452556421817643,
+                    "angularVelocity": 2.042775681773022,
+                    "velocityX": 1.449843643960376,
+                    "velocityY": 2.6109125053076734,
+                    "timestamp": 0.36670399387303465
+                },
+                {
+                    "x": 4.761013106791701,
+                    "y": 3.810602792551275,
+                    "heading": 0.11153690871840848,
+                    "angularVelocity": 1.9204860146414893,
+                    "velocityX": 2.0729881189319217,
+                    "velocityY": 2.1695143025313537,
+                    "timestamp": 0.45837999234129334
+                },
+                {
+                    "x": 5.007328053154023,
+                    "y": 3.968591564774131,
+                    "heading": 0.25752334504560864,
+                    "angularVelocity": 1.5924171949733021,
+                    "velocityX": 2.6867986220799525,
+                    "velocityY": 1.7233384404049508,
+                    "timestamp": 0.550055990809552
+                },
+                {
+                    "x": 5.305296422655045,
+                    "y": 4.081628168088871,
+                    "heading": 0.35020186087766364,
+                    "angularVelocity": 1.0109354398157564,
+                    "velocityX": 3.2502331524012615,
+                    "velocityY": 1.2330010602925316,
+                    "timestamp": 0.6417319892778107
+                },
+                {
+                    "x": 5.6446965706540295,
+                    "y": 4.152909996485894,
+                    "heading": 0.3502021264888923,
+                    "angularVelocity": 0.0000028972820926514052,
+                    "velocityX": 3.702170182705961,
+                    "velocityY": 0.7775407913523397,
+                    "timestamp": 0.7334079877460694
+                },
+                {
+                    "x": 5.981889969698284,
+                    "y": 4.233991234618027,
+                    "heading": 0.3502024148432213,
+                    "angularVelocity": 0.0000031453633856654694,
+                    "velocityX": 3.6780990082262504,
+                    "velocityY": 0.884432561268565,
+                    "timestamp": 0.8250839862143281
+                },
+                {
+                    "x": 6.295899946950055,
+                    "y": 4.381202864314355,
+                    "heading": 0.350202827556894,
+                    "angularVelocity": 0.000004501872678764248,
+                    "velocityX": 3.425214696303447,
+                    "velocityY": 1.6057815803042044,
+                    "timestamp": 0.9167599846825868
+                },
+                {
+                    "x": 6.57390491063703,
+                    "y": 4.588535293884794,
+                    "heading": 0.350203324900352,
+                    "angularVelocity": 0.0000054250127210279085,
+                    "velocityX": 3.0324727118541346,
+                    "velocityY": 2.2615780906081357,
+                    "timestamp": 1.0084359831508454
+                },
+                {
+                    "x": 6.804553755441848,
+                    "y": 4.847523149382792,
+                    "heading": 0.35020327996165895,
+                    "angularVelocity": -4.901903857536901e-7,
+                    "velocityX": 2.5159130923965525,
+                    "velocityY": 2.825034467311176,
+                    "timestamp": 1.100111981619104
+                },
+                {
+                    "x": 6.978428828519116,
+                    "y": 5.147591577808381,
+                    "heading": 0.35020324373344786,
+                    "angularVelocity": -3.9517661894346874e-7,
+                    "velocityX": 1.8966258997164847,
+                    "velocityY": 3.273140554116584,
+                    "timestamp": 1.1917879800873628
+                },
+                {
+                    "x": 7.088431556948234,
+                    "y": 5.476488224562697,
+                    "heading": 0.3502032113343976,
+                    "angularVelocity": -3.5340820734555857e-7,
+                    "velocityX": 1.1999076123202526,
+                    "velocityY": 3.587598196360961,
+                    "timestamp": 1.2834639785556214
+                },
+                {
+                    "x": 7.187536215850895,
+                    "y": 5.808831226900728,
+                    "heading": 0.3502031791295722,
+                    "angularVelocity": -3.5128960565806065e-7,
+                    "velocityX": 1.0810316828670656,
+                    "velocityY": 3.6251909757285055,
+                    "timestamp": 1.3751399770238801
+                },
+                {
+                    "x": 7.286640767886556,
+                    "y": 6.141174261106461,
+                    "heading": 0.3502031469247365,
+                    "angularVelocity": -3.512897194087527e-7,
+                    "velocityX": 1.0810305171639119,
+                    "velocityY": 3.6251913233407715,
+                    "timestamp": 1.4668159754921388
+                },
+                {
+                    "x": 7.3857452620780215,
+                    "y": 6.473517312561292,
+                    "heading": 0.350203114719879,
+                    "angularVelocity": -3.512899557892109e-7,
+                    "velocityX": 1.0810298862005645,
+                    "velocityY": 3.625191511493593,
+                    "timestamp": 1.5584919739603975
+                },
+                {
+                    "x": 7.481976228665169,
+                    "y": 6.80241098377653,
+                    "heading": 0.33998449018149474,
+                    "angularVelocity": -0.11146455679915658,
+                    "velocityX": 1.0496855032395944,
+                    "velocityY": 3.5875657392388547,
+                    "timestamp": 1.6501679724286562
+                },
+                {
+                    "x": 7.561455250393006,
+                    "y": 7.068914029375232,
+                    "heading": 0.2316909043200923,
+                    "angularVelocity": -1.1812643185871456,
+                    "velocityX": 0.8669556160367877,
+                    "velocityY": 2.907010014087551,
+                    "timestamp": 1.741843970896915
+                },
+                {
+                    "x": 7.620860593043084,
+                    "y": 7.268277067392774,
+                    "heading": 0.12481105079738766,
+                    "angularVelocity": -1.1658433538600752,
+                    "velocityX": 0.6479923168837385,
+                    "velocityY": 2.1746481232660675,
+                    "timestamp": 1.8335199693651736
+                },
+                {
+                    "x": 7.66032628893014,
+                    "y": 7.4008489817464715,
+                    "heading": 0.04379045785830094,
+                    "angularVelocity": -0.8837710446877722,
+                    "velocityX": 0.430491039600946,
+                    "velocityY": 1.4460918513977026,
+                    "timestamp": 1.9251959678334323
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 6.52409657127002e-31,
+                    "angularVelocity": -0.4776654586801488,
+                    "velocityX": 0.21460045593800534,
+                    "velocityY": 0.7215740145599008,
+                    "timestamp": 2.0168719663016907
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 0,
+                    "angularVelocity": -6.745238901014272e-30,
+                    "velocityX": 0,
+                    "velocityY": 3.373152313643601e-31,
+                    "timestamp": 2.1085479647699494
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -24691,7 +23717,7 @@
                 {
                     "x": 4.263,
                     "y": 3,
-                    "heading": 2.574,
+                    "heading": -0.566,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -24700,7 +23726,7 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -24711,191 +23737,191 @@
                 {
                     "x": 4.263,
                     "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": 1.5794772183943222e-24,
-                    "velocityX": -1.3909195862275997e-25,
-                    "velocityY": -1.589027573571451e-24,
+                    "heading": -0.566,
+                    "angularVelocity": -2.8196964588027846e-31,
+                    "velocityX": -1.564828148658428e-31,
+                    "velocityY": 3.07478616058615e-32,
                     "timestamp": 0
                 },
                 {
-                    "x": 4.294802050577596,
-                    "y": 3.0477360619772367,
-                    "heading": 2.544164914123716,
-                    "angularVelocity": -0.3578657180774014,
-                    "velocityX": 0.38145905506951994,
-                    "velocityY": 0.5725842442186713,
-                    "timestamp": 0.08336949970108862
-                },
-                {
-                    "x": 4.359925633327058,
-                    "y": 3.142202439962799,
-                    "heading": 2.4853835413264154,
-                    "angularVelocity": -0.7050704755102848,
-                    "velocityX": 0.781143979308431,
-                    "velocityY": 1.13310477241989,
-                    "timestamp": 0.16673899940217723
-                },
-                {
-                    "x": 4.461027910894954,
-                    "y": 3.2814650300922756,
-                    "heading": 2.399327847716391,
-                    "angularVelocity": -1.0322203433937678,
-                    "velocityX": 1.2127010229206814,
-                    "velocityY": 1.6704261226082242,
-                    "timestamp": 0.2501084991032658
-                },
-                {
-                    "x": 4.603798889123446,
-                    "y": 3.46048671868814,
-                    "heading": 2.2901235890618308,
-                    "angularVelocity": -1.309882619496323,
-                    "velocityX": 1.7125085161885507,
-                    "velocityY": 2.147328330357326,
-                    "timestamp": 0.33347799880435447
-                },
-                {
-                    "x": 4.803010081194681,
-                    "y": 3.6526247699506835,
-                    "heading": 2.177140187929377,
-                    "angularVelocity": -1.3552126561577384,
-                    "velocityX": 2.389497271609915,
-                    "velocityY": 2.3046564025384755,
-                    "timestamp": 0.4168474985054431
-                },
-                {
-                    "x": 5.0510307536076,
-                    "y": 3.8467573295012585,
-                    "heading": 2.175769825428097,
-                    "angularVelocity": -0.016437216322438507,
-                    "velocityX": 2.9749569483104428,
-                    "velocityY": 2.328580119187639,
-                    "timestamp": 0.5002169982065318
-                },
-                {
-                    "x": 5.320458962981737,
-                    "y": 4.010690590396415,
-                    "heading": 2.175769541029465,
-                    "angularVelocity": -0.000003411303085546954,
-                    "velocityX": 3.231735950679083,
-                    "velocityY": 1.9663457437422465,
-                    "timestamp": 0.5835864979076204
-                },
-                {
-                    "x": 5.614141443438261,
-                    "y": 4.125652797356739,
-                    "heading": 2.175769455965937,
-                    "angularVelocity": -0.0000010203195273976758,
-                    "velocityX": 3.522660943264475,
-                    "velocityY": 1.3789480250272361,
-                    "timestamp": 0.666955997608709
-                },
-                {
-                    "x": 5.906454716700063,
-                    "y": 4.244053209777629,
-                    "heading": 2.1757695445063954,
-                    "angularVelocity": 0.000001062024591118681,
-                    "velocityX": 3.506237584606561,
-                    "velocityY": 1.4201885922957453,
-                    "timestamp": 0.7503254973097977
-                },
-                {
-                    "x": 6.172161728896083,
-                    "y": 4.41393857041553,
-                    "heading": 2.1757932909481013,
-                    "angularVelocity": 0.00028483368367179416,
-                    "velocityX": 3.187100955969282,
-                    "velocityY": 2.0377399558232225,
-                    "timestamp": 0.8336949970108863
-                },
-                {
-                    "x": 6.426803794432408,
-                    "y": 4.599225132728127,
-                    "heading": 2.1773610490246513,
-                    "angularVelocity": 0.018804935643986023,
-                    "velocityX": 3.0543791968203453,
-                    "velocityY": 2.2224742019194084,
-                    "timestamp": 0.917064496711975
-                },
-                {
-                    "x": 6.681572955411179,
-                    "y": 4.785089288083454,
-                    "heading": 2.177430344851842,
-                    "angularVelocity": 0.0008311891931529928,
-                    "velocityX": 3.0559036805092323,
-                    "velocityY": 2.2294023116574087,
-                    "timestamp": 1.0004339964130635
-                },
-                {
-                    "x": 6.9013556581674305,
-                    "y": 5.011277814262196,
-                    "heading": 2.1774305498100937,
-                    "angularVelocity": 0.000002458432073886186,
-                    "velocityX": 2.636248310764209,
-                    "velocityY": 2.7130848450538085,
-                    "timestamp": 1.083803496114152
-                },
-                {
-                    "x": 7.121935686700776,
-                    "y": 5.230936165800854,
-                    "heading": 2.189684453697612,
-                    "angularVelocity": 0.14698305653090518,
-                    "velocityX": 2.645812069452359,
-                    "velocityY": 2.634756743487918,
-                    "timestamp": 1.1671729958152406
-                },
-                {
-                    "x": 7.311158558493755,
-                    "y": 5.417630564055526,
-                    "heading": 2.349466738853841,
-                    "angularVelocity": 1.9165556435999858,
-                    "velocityX": 2.2696894244467765,
-                    "velocityY": 2.239360904455968,
-                    "timestamp": 1.2505424955163291
-                },
-                {
-                    "x": 7.462052071844248,
-                    "y": 5.571824642162805,
-                    "heading": 2.618524466568701,
-                    "angularVelocity": 3.2272920993832837,
-                    "velocityX": 1.8099366541901185,
-                    "velocityY": 1.8495262495291969,
-                    "timestamp": 1.3339119952174177
-                },
-                {
-                    "x": 7.571973798709058,
-                    "y": 5.685339226077728,
-                    "heading": 2.8696843432959835,
-                    "angularVelocity": 3.0126110583341195,
-                    "velocityX": 1.3184885030967077,
-                    "velocityY": 1.3615840843703815,
-                    "timestamp": 1.4172814949185062
-                },
-                {
-                    "x": 7.644303858666078,
-                    "y": 5.759924476296826,
-                    "heading": 3.0477755113688096,
-                    "angularVelocity": 2.1361669280894184,
-                    "velocityX": 0.8675841910572869,
-                    "velocityY": 0.8946347343634476,
-                    "timestamp": 1.5006509946195947
+                    "x": 4.294807569207289,
+                    "y": 3.0477321348748183,
+                    "heading": -0.5960373953696199,
+                    "angularVelocity": -0.36026847684168845,
+                    "velocityX": 0.381499939303545,
+                    "velocityY": 0.5724991570056517,
+                    "timestamp": 0.08337503084628527
+                },
+                {
+                    "x": 4.359939756522268,
+                    "y": 3.142193816557181,
+                    "heading": -0.6551934033575421,
+                    "angularVelocity": -0.7095170746861289,
+                    "velocityX": 0.7811953609355742,
+                    "velocityY": 1.1329732741750633,
+                    "timestamp": 0.16675006169257053
+                },
+                {
+                    "x": 4.461050205756915,
+                    "y": 3.2814565261988764,
+                    "heading": -0.7417418377173162,
+                    "angularVelocity": -1.038061797174498,
+                    "velocityX": 1.2127185826300881,
+                    "velocityY": 1.6703167390540123,
+                    "timestamp": 0.2501250925388558
+                },
+                {
+                    "x": 4.603825208189836,
+                    "y": 3.4604941356958814,
+                    "heading": -0.8514332359801334,
+                    "angularVelocity": -1.315638472926628,
+                    "velocityX": 1.7124431737380506,
+                    "velocityY": 2.147376830685539,
+                    "timestamp": 0.33350012338514107
+                },
+                {
+                    "x": 4.8030511460470855,
+                    "y": 3.6526461815261504,
+                    "heading": -0.9644207302174959,
+                    "angularVelocity": -1.3551718433024906,
+                    "velocityX": 2.3895156119888465,
+                    "velocityY": 2.3046713611960277,
+                    "timestamp": 0.41687515423142635
+                },
+                {
+                    "x": 5.051097476442208,
+                    "y": 3.8467657037250893,
+                    "heading": -0.9658198239131095,
+                    "angularVelocity": -0.016780727772000164,
+                    "velocityX": 2.975067329839247,
+                    "velocityY": 2.328269269930804,
+                    "timestamp": 0.5002501850777116
+                },
+                {
+                    "x": 5.320526470824812,
+                    "y": 4.010737925204951,
+                    "heading": -0.965820109061322,
+                    "angularVelocity": -0.000003420067250854742,
+                    "velocityX": 3.2315309709370754,
+                    "velocityY": 1.9666825884858703,
+                    "timestamp": 0.5836252159239969
+                },
+                {
+                    "x": 5.614243645591117,
+                    "y": 4.1256688963704224,
+                    "heading": -0.9658201941489747,
+                    "angularVelocity": -0.000001020541184209393,
+                    "velocityX": 3.5228433715103167,
+                    "velocityY": 1.37848190278171,
+                    "timestamp": 0.6670002467702821
+                },
+                {
+                    "x": 5.906557788271019,
+                    "y": 4.244122886725936,
+                    "heading": -0.965820105112684,
+                    "angularVelocity": 0.0000010679011417361832,
+                    "velocityX": 3.506015406684877,
+                    "velocityY": 1.4207369898777267,
+                    "timestamp": 0.7503752776165674
+                },
+                {
+                    "x": 6.172253111887642,
+                    "y": 4.41406398245588,
+                    "heading": -0.9657938677546691,
+                    "angularVelocity": 0.0003146908342779248,
+                    "velocityX": 3.1867493291423483,
+                    "velocityY": 2.0382732576526004,
+                    "timestamp": 0.8337503084628527
+                },
+                {
+                    "x": 6.426785390452526,
+                    "y": 4.5994304003649855,
+                    "heading": -0.9640144900160125,
+                    "angularVelocity": 0.021341854037057975,
+                    "velocityX": 3.0528597828545716,
+                    "velocityY": 2.2232845496735987,
+                    "timestamp": 0.917125339309138
+                },
+                {
+                    "x": 6.681598941757368,
+                    "y": 4.785246665794811,
+                    "heading": -0.9639002296367303,
+                    "angularVelocity": 0.0013704388246982366,
+                    "velocityX": 3.0562333676929114,
+                    "velocityY": 2.2286800201898096,
+                    "timestamp": 1.0005003701554231
+                },
+                {
+                    "x": 6.9014822245071255,
+                    "y": 5.01136660335564,
+                    "heading": -0.9639000238890051,
+                    "angularVelocity": 0.000002467737919950576,
+                    "velocityX": 2.6372797769052228,
+                    "velocityY": 2.7120822057351384,
+                    "timestamp": 1.0838754010017084
+                },
+                {
+                    "x": 7.122012116423131,
+                    "y": 5.23107273201418,
+                    "heading": -0.9515772318649645,
+                    "angularVelocity": 0.1477995498047812,
+                    "velocityX": 2.645035206314798,
+                    "velocityY": 2.635154990990089,
+                    "timestamp": 1.1672504318479937
+                },
+                {
+                    "x": 7.311148990719613,
+                    "y": 5.417840497645029,
+                    "heading": -0.7917020160601519,
+                    "angularVelocity": 1.9175431083146124,
+                    "velocityX": 2.2685073981583956,
+                    "velocityY": 2.2400923122317544,
+                    "timestamp": 1.250625462694279
+                },
+                {
+                    "x": 7.462007390054419,
+                    "y": 5.572027096128993,
+                    "heading": -0.5225549044290918,
+                    "angularVelocity": 3.228150069620655,
+                    "velocityX": 1.8093954245478587,
+                    "velocityY": 1.8493138403538476,
+                    "timestamp": 1.3340004935405643
+                },
+                {
+                    "x": 7.571928087470151,
+                    "y": 5.685474744918936,
+                    "heading": -0.27088873612536774,
+                    "angularVelocity": 3.018483660506339,
+                    "velocityX": 1.318388686636741,
+                    "velocityY": 1.3606909363440005,
+                    "timestamp": 1.4173755243868496
+                },
+                {
+                    "x": 7.6442805622690475,
+                    "y": 5.7599789636443175,
+                    "heading": -0.0924144798498188,
+                    "angularVelocity": 2.1406199729580164,
+                    "velocityX": 0.8677954786282445,
+                    "velocityY": 0.8936034921863096,
+                    "timestamp": 1.5007505552331348
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
-                    "angularVelocity": 1.1062137707656932,
-                    "velocityX": 0.4281678726861332,
-                    "velocityY": 0.44471328046952424,
-                    "timestamp": 1.5840204943206833
+                    "heading": -2.5332548087748344e-31,
+                    "angularVelocity": 1.1084191383412998,
+                    "velocityX": 0.42841888474747514,
+                    "velocityY": 0.4440302567795869,
+                    "timestamp": 1.5841255860794201
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
-                    "angularVelocity": 1.4380080051319566e-21,
+                    "heading": 0,
+                    "angularVelocity": 5.737670210968143e-33,
                     "velocityX": 0,
-                    "velocityY": 1.143279687808587e-23,
-                    "timestamp": 1.6673899940217718
+                    "velocityY": -3.2710687085311585e-31,
+                    "timestamp": 1.6675006169257054
                 }
             ],
             "constraints": [
@@ -24938,7 +23964,7 @@
                 {
                     "x": 4.263,
                     "y": 3,
-                    "heading": 2.574,
+                    "heading": -0.566,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -24947,7 +23973,7 @@
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -24958,164 +23984,164 @@
                 {
                     "x": 4.263,
                     "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": -2.0886910556164006e-30,
-                    "velocityX": -6.72399315077418e-33,
-                    "velocityY": 1.0977042871870097e-32,
+                    "heading": -0.566,
+                    "angularVelocity": -1.0027901855857707e-30,
+                    "velocityX": -5.17158696002297e-32,
+                    "velocityY": 1.9371271312239428e-32,
                     "timestamp": 0
                 },
                 {
-                    "x": 4.300222432415914,
-                    "y": 3.053065889088155,
-                    "heading": 2.5374285401398833,
-                    "angularVelocity": -0.4122912034410164,
-                    "velocityX": 0.41962999329145273,
-                    "velocityY": 0.5982424370672392,
-                    "timestamp": 0.08870298360694638
-                },
-                {
-                    "x": 4.375819084559984,
-                    "y": 3.158500650682638,
-                    "heading": 2.466843265689978,
-                    "angularVelocity": -0.7957485935611535,
-                    "velocityX": 0.8522447506280879,
-                    "velocityY": 1.1886270033675244,
-                    "timestamp": 0.17740596721389276
-                },
-                {
-                    "x": 4.491836015930185,
-                    "y": 3.3149606373409832,
-                    "heading": 2.3668866050755812,
-                    "angularVelocity": -1.1268692049561326,
-                    "velocityX": 1.307925919203419,
-                    "velocityY": 1.7638638554892148,
-                    "timestamp": 0.26610895082083913
-                },
-                {
-                    "x": 4.652875991060298,
-                    "y": 3.5188568125430604,
-                    "heading": 2.248368974552935,
-                    "angularVelocity": -1.3361177460254572,
-                    "velocityX": 1.8154967125311139,
-                    "velocityY": 2.2986394246394894,
-                    "timestamp": 0.3548119344277855
-                },
-                {
-                    "x": 4.870873285433555,
-                    "y": 3.7498811407551353,
-                    "heading": 2.196009009563727,
-                    "angularVelocity": -0.5902841467116942,
-                    "velocityX": 2.4576094907836294,
-                    "velocityY": 2.604470772209555,
-                    "timestamp": 0.4435149180347319
-                },
-                {
-                    "x": 5.1428014530518755,
-                    "y": 3.946488109616269,
-                    "heading": 2.1960088759837526,
-                    "angularVelocity": -0.0000015059242530161288,
-                    "velocityX": 3.0656033941684258,
-                    "velocityY": 2.21646398876866,
-                    "timestamp": 0.5322179016416783
-                },
-                {
-                    "x": 5.447787504833652,
-                    "y": 4.086426434262152,
-                    "heading": 2.1960087423061556,
-                    "angularVelocity": -0.0000015070248085665752,
-                    "velocityX": 3.4382840281135,
-                    "velocityY": 1.577605610945025,
-                    "timestamp": 0.6209208852486247
-                },
-                {
-                    "x": 5.774173248105482,
-                    "y": 4.1643468008186435,
-                    "heading": 2.1960088209928723,
-                    "angularVelocity": 8.870808373208906e-7,
-                    "velocityX": 3.679535118210733,
-                    "velocityY": 0.8784413261876967,
-                    "timestamp": 0.709623868855571
-                },
-                {
-                    "x": 6.1094823510079035,
-                    "y": 4.1772711464289065,
-                    "heading": 2.1960089213757183,
-                    "angularVelocity": 0.0000011316738380089979,
-                    "velocityX": 3.780133308572978,
-                    "velocityY": 0.1457036176768579,
-                    "timestamp": 0.7983268524625174
-                },
-                {
-                    "x": 6.444914319541054,
-                    "y": 4.1680717704970895,
-                    "heading": 2.1960090177915097,
-                    "angularVelocity": 0.000001086950938877831,
-                    "velocityX": 3.78151844383826,
-                    "velocityY": -0.10370988164931202,
-                    "timestamp": 0.8870298360694638
-                },
-                {
-                    "x": 6.780346283198341,
-                    "y": 4.1588722167865155,
-                    "heading": 2.196009114207664,
-                    "angularVelocity": 0.0000010869550292397467,
-                    "velocityX": 3.7815183888698267,
-                    "velocityY": -0.10371188585197635,
-                    "timestamp": 0.9757328196764102
-                },
-                {
-                    "x": 7.07908088896125,
-                    "y": 4.144071317525266,
-                    "heading": 2.2852514937378463,
-                    "angularVelocity": 1.0060809219859659,
-                    "velocityX": 3.3678078641259517,
-                    "velocityY": -0.16685909153668751,
-                    "timestamp": 1.0644358032833565
-                },
-                {
-                    "x": 7.324007491183461,
-                    "y": 4.136590325759102,
-                    "heading": 2.509537504382542,
-                    "angularVelocity": 2.5285058238687177,
-                    "velocityX": 2.7611991419308914,
-                    "velocityY": -0.08433754381152235,
-                    "timestamp": 1.153138786890303
-                },
-                {
-                    "x": 7.505234177737953,
-                    "y": 4.13149769076231,
-                    "heading": 2.790057598116992,
-                    "angularVelocity": 3.162465142970483,
-                    "velocityX": 2.043073177307424,
-                    "velocityY": -0.05741221760204461,
-                    "timestamp": 1.2418417704972493
-                },
-                {
-                    "x": 7.622663550770296,
-                    "y": 4.128429038475573,
-                    "heading": 3.015163183857666,
-                    "angularVelocity": 2.5377453675982737,
-                    "velocityX": 1.3238491903799634,
-                    "velocityY": -0.03459469075285244,
-                    "timestamp": 1.3305447541041957
+                    "x": 4.300239460670815,
+                    "y": 3.0530479185441526,
+                    "heading": -0.6028935811234369,
+                    "angularVelocity": -0.4158951640704737,
+                    "velocityX": 0.4197942063083121,
+                    "velocityY": 0.5980003056006388,
+                    "timestamp": 0.08870884855296232
+                },
+                {
+                    "x": 4.375862012794379,
+                    "y": 3.1584586937478076,
+                    "heading": -0.6740120078870278,
+                    "angularVelocity": -0.8017061197804932,
+                    "velocityX": 0.8524803709791626,
+                    "velocityY": 1.1882780232540273,
+                    "timestamp": 0.17741769710592464
+                },
+                {
+                    "x": 4.491898262382256,
+                    "y": 3.3149103934712074,
+                    "heading": -0.7745169367898204,
+                    "angularVelocity": -1.1329752391362358,
+                    "velocityX": 1.3080572172977813,
+                    "velocityY": 1.763653821185524,
+                    "timestamp": 0.26612654565888694
+                },
+                {
+                    "x": 4.65291407862912,
+                    "y": 3.5188640922932937,
+                    "heading": -0.893163244153065,
+                    "angularVelocity": -1.3374799616794542,
+                    "velocityX": 1.8151043427277864,
+                    "velocityY": 2.299135905256609,
+                    "timestamp": 0.3548353942118493
+                },
+                {
+                    "x": 4.870914712338585,
+                    "y": 3.7498942741357597,
+                    "heading": -0.9455687285938831,
+                    "angularVelocity": -0.5907582534963245,
+                    "velocityX": 2.457484650804715,
+                    "velocityY": 2.6043645657798487,
+                    "timestamp": 0.44354424276481164
+                },
+                {
+                    "x": 5.142859309500119,
+                    "y": 3.9465163862068953,
+                    "heading": -0.9455688623878027,
+                    "angularVelocity": -0.0000015082364580335284,
+                    "velocityX": 3.065585920655626,
+                    "velocityY": 2.216488155110533,
+                    "timestamp": 0.532253091317774
+                },
+                {
+                    "x": 5.447866241414174,
+                    "y": 4.086462405550872,
+                    "heading": -0.9455689961292948,
+                    "angularVelocity": -0.0000015076454528851402,
+                    "velocityX": 3.438292085732067,
+                    "velocityY": 1.5775880492961702,
+                    "timestamp": 0.6209619398707363
+                },
+                {
+                    "x": 5.774274975125892,
+                    "y": 4.16438201701327,
+                    "heading": -0.9455689173593601,
+                    "angularVelocity": 8.879602884106285e-7,
+                    "velocityX": 3.6795510147653454,
+                    "velocityY": 0.8783747363813094,
+                    "timestamp": 0.7096707884236987
+                },
+                {
+                    "x": 6.109606649873876,
+                    "y": 4.1772967927030695,
+                    "heading": -0.9455688167433821,
+                    "angularVelocity": 0.0000011342270768412302,
+                    "velocityX": 3.780137835379285,
+                    "velocityY": 0.14558610443566755,
+                    "timestamp": 0.798379636976661
+                },
+                {
+                    "x": 6.445060320326861,
+                    "y": 4.168079453807152,
+                    "heading": -0.9455687201101868,
+                    "angularVelocity": 0.0000010893298350239914,
+                    "velocityX": 3.781513072539862,
+                    "velocityY": -0.10390551840399499,
+                    "timestamp": 0.8870884855296234
+                },
+                {
+                    "x": 6.780513985890507,
+                    "y": 4.158861936977066,
+                    "heading": -0.9455686234766241,
+                    "angularVelocity": 0.0000010893339756447378,
+                    "velocityX": 3.781513017423149,
+                    "velocityY": -0.10390752422611307,
+                    "timestamp": 0.9757973340825857
+                },
+                {
+                    "x": 7.079210544299498,
+                    "y": 4.144050975293544,
+                    "heading": -0.856180476499644,
+                    "angularVelocity": 1.0076576174203435,
+                    "velocityX": 3.3671563015572095,
+                    "velocityY": -0.16696149172400718,
+                    "timestamp": 1.064506182635548
+                },
+                {
+                    "x": 7.324091725395792,
+                    "y": 4.136558502839443,
+                    "heading": -0.6317167360096363,
+                    "angularVelocity": 2.5303421716267107,
+                    "velocityX": 2.760504561730298,
+                    "velocityY": -0.08446138774564337,
+                    "timestamp": 1.1532150311885103
+                },
+                {
+                    "x": 7.50528856962614,
+                    "y": 4.13147246408345,
+                    "heading": -0.35065072299807204,
+                    "angularVelocity": 3.1684101146207286,
+                    "velocityX": 2.042601692909661,
+                    "velocityY": -0.05733406350050504,
+                    "timestamp": 1.2419238797414727
+                },
+                {
+                    "x": 7.622685008969739,
+                    "y": 4.128418008534293,
+                    "heading": -0.12509027443555182,
+                    "angularVelocity": 2.5427051781407424,
+                    "velocityX": 1.323390408720159,
+                    "velocityY": -0.03443236609404116,
+                    "timestamp": 1.330632728294435
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": 1.4073575776830842,
-                    "velocityX": 0.6463869297087906,
-                    "velocityY": -0.016110376646462816,
-                    "timestamp": 1.419247737711142
+                    "heading": 3.405276849475189e-32,
+                    "angularVelocity": 1.4101217237744719,
+                    "velocityX": 0.6461022994345517,
+                    "velocityY": -0.01598497283444213,
+                    "timestamp": 1.4193415768473974
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": 1.404904480201356e-31,
+                    "heading": 0,
+                    "angularVelocity": -2.3485646679922316e-31,
                     "velocityX": 0,
-                    "velocityY": -1.2280872780983036e-31,
-                    "timestamp": 1.5079507213180885
+                    "velocityY": -2.6996723431981715e-31,
+                    "timestamp": 1.5080504254003597
                 }
             ],
             "constraints": [
@@ -25158,16 +24184,16 @@
                 {
                     "x": 4.263,
                     "y": 3,
-                    "heading": 2.574,
+                    "heading": -0.566,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 17
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -25215,7 +24241,7 @@
                 {
                     "x": 4.263,
                     "y": 3,
-                    "heading": 2.574,
+                    "heading": -0.566,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -25224,7 +24250,7 @@
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -25235,182 +24261,182 @@
                 {
                     "x": 4.263,
                     "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": -5.037969685186626e-28,
-                    "velocityX": 1.6468312353259754e-28,
-                    "velocityY": 5.118071700544948e-29,
+                    "heading": -0.566,
+                    "angularVelocity": -2.2118032332823513e-27,
+                    "velocityX": -2.2398078489200873e-28,
+                    "velocityY": 7.32316842173409e-29,
                     "timestamp": 0
                 },
                 {
-                    "x": 4.2573202524069735,
-                    "y": 2.93910307994174,
-                    "heading": 2.524484159587002,
-                    "angularVelocity": -0.5709623504386324,
-                    "velocityX": -0.06549261829272982,
-                    "velocityY": -0.7021964753285136,
-                    "timestamp": 0.08672347725723033
-                },
-                {
-                    "x": 4.260125495463358,
-                    "y": 2.8166106869531333,
-                    "heading": 2.4459027831938824,
-                    "angularVelocity": -0.9061142251024153,
-                    "velocityX": 0.032346985442753974,
-                    "velocityY": -1.412447896032608,
-                    "timestamp": 0.17344695451446066
-                },
-                {
-                    "x": 4.2940460497089665,
-                    "y": 2.639724962337947,
-                    "heading": 2.375950811095946,
-                    "angularVelocity": -0.8066094016324238,
-                    "velocityX": 0.3911346191181556,
-                    "velocityY": -2.039652124309153,
-                    "timestamp": 0.26017043177169097
-                },
-                {
-                    "x": 4.380083643145815,
-                    "y": 2.4322611848370723,
-                    "heading": 2.367349671900665,
-                    "angularVelocity": -0.09917890134603644,
-                    "velocityX": 0.9920911402301427,
-                    "velocityY": -2.3922446846257825,
-                    "timestamp": 0.3468939090289213
-                },
-                {
-                    "x": 4.5223038363919486,
-                    "y": 2.2062304273482725,
-                    "heading": 2.4344770790327512,
-                    "angularVelocity": 0.774039617126779,
-                    "velocityX": 1.639927246278367,
-                    "velocityY": -2.6063387289968842,
-                    "timestamp": 0.43361738628615165
-                },
-                {
-                    "x": 4.727106314265492,
-                    "y": 1.976782887261895,
-                    "heading": 2.494103363003819,
-                    "angularVelocity": 0.6875448939185098,
-                    "velocityX": 2.361557496894169,
-                    "velocityY": -2.6457373175410677,
-                    "timestamp": 0.5203408635433819
-                },
-                {
-                    "x": 4.985728714890376,
-                    "y": 1.77493098646381,
-                    "heading": 2.4941035609909017,
-                    "angularVelocity": 0.0000022829698365674285,
-                    "velocityX": 2.982149803077939,
-                    "velocityY": -2.327534678981717,
-                    "timestamp": 0.6070643408006122
-                },
-                {
-                    "x": 5.274471193088088,
-                    "y": 1.6191826835790144,
-                    "heading": 2.494103670963996,
-                    "angularVelocity": 0.000001268089077272548,
-                    "velocityX": 3.329461494507118,
-                    "velocityY": -1.795918565659344,
-                    "timestamp": 0.6937878180578425
-                },
-                {
-                    "x": 5.576873114142296,
-                    "y": 1.4919708990806198,
-                    "heading": 2.4941037354476974,
-                    "angularVelocity": 7.435553047089643e-7,
-                    "velocityX": 3.486967204477438,
-                    "velocityY": -1.4668667415291887,
-                    "timestamp": 0.7805112953150728
-                },
-                {
-                    "x": 5.888127945966446,
-                    "y": 1.388287773871042,
-                    "heading": 2.4941037912285227,
-                    "angularVelocity": 6.432032813789173e-7,
-                    "velocityX": 3.589049259417249,
-                    "velocityY": -1.1955600546555811,
-                    "timestamp": 0.8672347725723031
-                },
-                {
-                    "x": 6.199389301961685,
-                    "y": 1.2846242361365592,
-                    "heading": 2.4941038470080463,
-                    "angularVelocity": 6.431882742929459e-7,
-                    "velocityX": 3.58912448900094,
-                    "velocityY": -1.1953341933810824,
-                    "timestamp": 0.9539582498295334
-                },
-                {
-                    "x": 6.510650764865791,
-                    "y": 1.1809610194297586,
-                    "heading": 2.494103902787595,
-                    "angularVelocity": 6.431885596372589e-7,
-                    "velocityX": 3.589125721756674,
-                    "velocityY": -1.1953304916421486,
-                    "timestamp": 1.0406817270867637
-                },
-                {
-                    "x": 6.821912914234967,
-                    "y": 1.0772998639276916,
-                    "heading": 2.4941039585563476,
-                    "angularVelocity": 6.430640741979831e-7,
-                    "velocityX": 3.589133637318779,
-                    "velocityY": -1.1953067240904078,
-                    "timestamp": 1.127405204343994
-                },
-                {
-                    "x": 7.104835642474277,
-                    "y": 0.9795860470438206,
-                    "heading": 2.575393099111341,
-                    "angularVelocity": 0.9373371908725603,
-                    "velocityX": 3.2623545225260457,
-                    "velocityY": -1.126728539655348,
-                    "timestamp": 1.2141286816012242
-                },
-                {
-                    "x": 7.336162807105594,
-                    "y": 0.9014195336331526,
-                    "heading": 2.773123816483908,
-                    "angularVelocity": 2.280013712850493,
-                    "velocityX": 2.6674110857568296,
-                    "velocityY": -0.9013304803129645,
-                    "timestamp": 1.3008521588584545
-                },
-                {
-                    "x": 7.508575525064269,
-                    "y": 0.8437553503848311,
-                    "heading": 2.9477959992299394,
-                    "angularVelocity": 2.0141279878335374,
-                    "velocityX": 1.9880743186447822,
-                    "velocityY": -0.664920100900538,
-                    "timestamp": 1.3875756361156848
-                },
-                {
-                    "x": 7.62299427085809,
-                    "y": 0.8057956185147762,
-                    "heading": 3.0737109392018835,
-                    "angularVelocity": 1.451912953149597,
-                    "velocityX": 1.31935145375275,
-                    "velocityY": -0.4377099843155908,
-                    "timestamp": 1.474299113372915
+                    "x": 4.257275610783411,
+                    "y": 2.9390850995701614,
+                    "heading": -0.6153074756658753,
+                    "angularVelocity": -0.5685220144659363,
+                    "velocityX": -0.06600299944485946,
+                    "velocityY": -0.7023572274929988,
+                    "timestamp": 0.0867292284401611
+                },
+                {
+                    "x": 4.259999029271463,
+                    "y": 2.8165594202827866,
+                    "heading": -0.6936339270516045,
+                    "angularVelocity": -0.9031148183195571,
+                    "velocityX": 0.03140139186100546,
+                    "velocityY": -1.4127380295088343,
+                    "timestamp": 0.1734584568803222
+                },
+                {
+                    "x": 4.293815085802935,
+                    "y": 2.6396142169455077,
+                    "heading": -0.7636289676296801,
+                    "angularVelocity": -0.8070524993355632,
+                    "velocityX": 0.3899038091270867,
+                    "velocityY": -2.0402026689233352,
+                    "timestamp": 0.2601876853204833
+                },
+                {
+                    "x": 4.379794167011224,
+                    "y": 2.432035667571809,
+                    "heading": -0.7730870281638328,
+                    "angularVelocity": -0.10905274616477362,
+                    "velocityX": 0.9913506986587586,
+                    "velocityY": -2.3934093858210397,
+                    "timestamp": 0.3469169137606444
+                },
+                {
+                    "x": 4.522000800858502,
+                    "y": 2.2059673134468256,
+                    "heading": -0.7069342437539652,
+                    "angularVelocity": 0.7627507542686203,
+                    "velocityX": 1.6396621577856372,
+                    "velocityY": -2.6065993920487904,
+                    "timestamp": 0.43364614220080544
+                },
+                {
+                    "x": 4.726803922559788,
+                    "y": 1.9764479224139355,
+                    "heading": -0.6474090008701758,
+                    "angularVelocity": 0.6863342837743838,
+                    "velocityX": 2.3614083208706247,
+                    "velocityY": -2.6463903249323897,
+                    "timestamp": 0.5203753706409665
+                },
+                {
+                    "x": 4.985426543182091,
+                    "y": 1.774560944536545,
+                    "heading": -0.6474088034070749,
+                    "angularVelocity": 0.000002276776866191791,
+                    "velocityX": 2.9819545875556583,
+                    "velocityY": -2.3277847792302735,
+                    "timestamp": 0.6071045990811276
+                },
+                {
+                    "x": 5.274431548584119,
+                    "y": 1.6192543598542732,
+                    "heading": -0.6474086941993331,
+                    "angularVelocity": 0.000001259180368575944,
+                    "velocityX": 3.3322676864516145,
+                    "velocityY": -1.7907064028526984,
+                    "timestamp": 0.6938338275212886
+                },
+                {
+                    "x": 5.576835908979959,
+                    "y": 1.4919922741488194,
+                    "heading": -0.6474086295363626,
+                    "angularVelocity": 7.455729944345581e-7,
+                    "velocityX": 3.4867641028824017,
+                    "velocityY": -1.4673494506325018,
+                    "timestamp": 0.7805630559614497
+                },
+                {
+                    "x": 5.888110144336631,
+                    "y": 1.388298557512795,
+                    "heading": -0.6474085736154427,
+                    "angularVelocity": 6.447759407143774e-7,
+                    "velocityX": 3.589034988031027,
+                    "velocityY": -1.1956028953671916,
+                    "timestamp": 0.8672922844016108
+                },
+                {
+                    "x": 6.199390629137113,
+                    "y": 1.2846236026664921,
+                    "heading": -0.6474085176958322,
+                    "angularVelocity": 6.447608435198244e-7,
+                    "velocityX": 3.5891070449826046,
+                    "velocityY": -1.1953865693365275,
+                    "timestamp": 0.9540215128417718
+                },
+                {
+                    "x": 6.510671944712167,
+                    "y": 1.1809511422971397,
+                    "heading": -0.6474084617763125,
+                    "angularVelocity": 6.447597973785494e-7,
+                    "velocityX": 3.589116623928252,
+                    "velocityY": -1.1953578076724478,
+                    "timestamp": 1.040750741281933
+                },
+                {
+                    "x": 6.8219538378473334,
+                    "y": 1.0772804160640193,
+                    "heading": -0.6474084058668956,
+                    "angularVelocity": 6.446433092317229e-7,
+                    "velocityX": 3.5891232832762197,
+                    "velocityY": -1.1953378128419065,
+                    "timestamp": 1.127479969722094
+                },
+                {
+                    "x": 7.104843996542823,
+                    "y": 0.9795715918943764,
+                    "heading": -0.5659567926753784,
+                    "angularVelocity": 0.9391483662017565,
+                    "velocityX": 3.2617626581409067,
+                    "velocityY": -1.126596257420401,
+                    "timestamp": 1.2142091981622551
+                },
+                {
+                    "x": 7.336171547926519,
+                    "y": 0.9014041069183023,
+                    "heading": -0.36771202193482067,
+                    "angularVelocity": 2.2857896271651064,
+                    "velocityX": 2.6672386638755765,
+                    "velocityY": -0.9012819136285574,
+                    "timestamp": 1.3009384266024162
+                },
+                {
+                    "x": 7.508580909570239,
+                    "y": 0.8437450323938049,
+                    "heading": -0.19262457792893875,
+                    "angularVelocity": 2.018782446873579,
+                    "velocityX": 1.9879037868147627,
+                    "velocityY": -0.6648171044699364,
+                    "timestamp": 1.3876676550425773
+                },
+                {
+                    "x": 7.622996353926532,
+                    "y": 0.8057914476404057,
+                    "heading": -0.06643070405801102,
+                    "angularVelocity": 1.4550328204291798,
+                    "velocityX": 1.3192258989739947,
+                    "velocityY": -0.4376100818144245,
+                    "timestamp": 1.4743968834827383
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
-                    "angularVelocity": 0.7643727269087626,
-                    "velocityX": 0.6573275305004772,
-                    "velocityY": -0.2167304530355382,
-                    "timestamp": 1.5610225906301454
+                    "heading": -1.2156690964354266e-28,
+                    "angularVelocity": 0.7659552062526378,
+                    "velocityX": 0.6572599237729437,
+                    "velocityY": -0.21666799046150395,
+                    "timestamp": 1.5611261119228994
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
-                    "angularVelocity": -1.052133339029093e-28,
+                    "heading": 0,
+                    "angularVelocity": 1.25023472254267e-27,
                     "velocityX": 0,
-                    "velocityY": -1.789975640452255e-29,
-                    "timestamp": 1.6477460678873757
+                    "velocityY": -4.9030509543456705e-29,
+                    "timestamp": 1.6478553403630605
                 }
             ],
             "constraints": [
@@ -25453,23 +24479,267 @@
                 {
                     "x": 5.176,
                     "y": 1.62,
-                    "heading": 2.5,
+                    "heading": -0.64,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 26
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": -2.75,
+                    "heading": 0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": -0.64,
+                    "angularVelocity": 0,
+                    "velocityX": -4.2485019622555225e-35,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.123544064624381,
+                    "y": 1.640479835564366,
+                    "heading": -0.5261105113915723,
+                    "angularVelocity": 1.3044458865146935,
+                    "velocityX": -0.6008098724480858,
+                    "velocityY": 0.23456806756139406,
+                    "timestamp": 0.08730871076049408
+                },
+                {
+                    "x": 5.016418569113309,
+                    "y": 1.6815339921073649,
+                    "heading": -0.31680443246750034,
+                    "angularVelocity": 2.397310384049101,
+                    "velocityX": -1.2269737415426907,
+                    "velocityY": 0.4702183342922002,
+                    "timestamp": 0.17461742152098816
+                },
+                {
+                    "x": 4.8522086939147036,
+                    "y": 1.7459427773647251,
+                    "heading": -0.051891218250848345,
+                    "angularVelocity": 3.0342128741697256,
+                    "velocityX": -1.8807960141464901,
+                    "velocityY": 0.7377131639710823,
+                    "timestamp": 0.26192613228148226
+                },
+                {
+                    "x": 4.632595451769563,
+                    "y": 1.839668047804236,
+                    "heading": 0.17554509517769362,
+                    "angularVelocity": 2.6049670353333583,
+                    "velocityX": -2.515364620920639,
+                    "velocityY": 1.073492777789593,
+                    "timestamp": 0.34923484304197633
+                },
+                {
+                    "x": 4.3704901470738005,
+                    "y": 1.9627849470607086,
+                    "heading": 0.27538033292006053,
+                    "angularVelocity": 1.1434739658020574,
+                    "velocityX": -3.002052171114658,
+                    "velocityY": 1.4101330575617794,
+                    "timestamp": 0.4365435538024704
+                },
+                {
+                    "x": 4.075718453010027,
+                    "y": 2.1117712847634245,
+                    "heading": 0.2753804447713452,
+                    "angularVelocity": 0.0000012811010915301473,
+                    "velocityX": -3.3762002839830365,
+                    "velocityY": 1.7064315393617047,
+                    "timestamp": 0.5238522645629645
+                },
+                {
+                    "x": 3.780946849777969,
+                    "y": 2.260757802178556,
+                    "heading": 0.2753805566219007,
+                    "angularVelocity": 0.0000012810927397041138,
+                    "velocityX": -3.3761992436319233,
+                    "velocityY": 1.7064335977177825,
+                    "timestamp": 0.6111609753234586
+                },
+                {
+                    "x": 3.4861752863833098,
+                    "y": 2.409744398412427,
+                    "heading": 0.27538066847245973,
+                    "angularVelocity": 0.0000012810927810381003,
+                    "velocityX": -3.3761987873498507,
+                    "velocityY": 1.7064345004769594,
+                    "timestamp": 0.6984696860839527
+                },
+                {
+                    "x": 3.191403823869153,
+                    "y": 2.5587311942392557,
+                    "heading": 0.2753807803230405,
+                    "angularVelocity": 0.000001281093028963251,
+                    "velocityX": -3.3761976319038296,
+                    "velocityY": 1.7064367865370367,
+                    "timestamp": 0.7857783968444467
+                },
+                {
+                    "x": 2.907963654667952,
+                    "y": 2.72828634801538,
+                    "heading": 0.27538089480545036,
+                    "angularVelocity": 0.0000013112369766496227,
+                    "velocityX": -3.246413407463273,
+                    "velocityY": 1.9420187550501031,
+                    "timestamp": 0.8730871076049408
+                },
+                {
+                    "x": 2.662249448997808,
+                    "y": 2.9489938499602837,
+                    "heading": 0.27538103946019826,
+                    "angularVelocity": 0.000001656819194794751,
+                    "velocityX": -2.81431490088302,
+                    "velocityY": 2.527897846875211,
+                    "timestamp": 0.9603958183654349
+                },
+                {
+                    "x": 2.463361015566044,
+                    "y": 3.212680537293485,
+                    "heading": 0.27538116270778873,
+                    "angularVelocity": 0.0000014116299433562534,
+                    "velocityX": -2.277990726232991,
+                    "velocityY": 3.0201647125055904,
+                    "timestamp": 1.047704529125929
+                },
+                {
+                    "x": 2.3186638703555946,
+                    "y": 3.509581240155329,
+                    "heading": 0.2753811333053821,
+                    "angularVelocity": -3.3676372418890626e-7,
+                    "velocityX": -1.657304797540575,
+                    "velocityY": 3.4005851223287915,
+                    "timestamp": 1.1350132398864232
+                },
+                {
+                    "x": 2.233516432992889,
+                    "y": 3.8287006658490608,
+                    "heading": 0.2753811088610493,
+                    "angularVelocity": -2.799758765995743e-7,
+                    "velocityX": -0.9752456154836949,
+                    "velocityY": 3.655069728026828,
+                    "timestamp": 1.2223219506469174
+                },
+                {
+                    "x": 2.1898375161467403,
+                    "y": 4.15608338798651,
+                    "heading": 0.27538108549661844,
+                    "angularVelocity": -2.6760709935894875e-7,
+                    "velocityX": -0.5002813174732252,
+                    "velocityY": 3.749714310127982,
+                    "timestamp": 1.3096306614074116
+                },
+                {
+                    "x": 2.1796859113101297,
+                    "y": 4.486211000706444,
+                    "heading": 0.2753810837562719,
+                    "angularVelocity": -1.9933252293602923e-8,
+                    "velocityX": -0.11627253166592438,
+                    "velocityY": 3.78115321878412,
+                    "timestamp": 1.3969393721679058
+                },
+                {
+                    "x": 2.197179274626327,
+                    "y": 4.816031069152981,
+                    "heading": 0.2753810893961792,
+                    "angularVelocity": 6.459730400705482e-8,
+                    "velocityX": 0.20036217650934227,
+                    "velocityY": 3.7776307263464504,
+                    "timestamp": 1.4842480829284
+                },
+                {
+                    "x": 2.214672942788018,
+                    "y": 5.1458511214306375,
+                    "heading": 0.27538109503608554,
+                    "angularVelocity": 6.459729260879602e-8,
+                    "velocityX": 0.20036566809100828,
+                    "velocityY": 3.777630541154376,
+                    "timestamp": 1.5715567936888941
+                },
+                {
+                    "x": 2.2321666974024836,
+                    "y": 5.475671169122818,
+                    "heading": 0.2753811006759881,
+                    "angularVelocity": 6.459724966098765e-8,
+                    "velocityX": 0.20036665828744704,
+                    "velocityY": 3.7776304886341237,
+                    "timestamp": 1.6588655044493883
+                },
+                {
+                    "x": 2.2496605217353887,
+                    "y": 5.805491213117091,
+                    "heading": 0.2753811063158993,
+                    "angularVelocity": 6.459734855412882e-8,
+                    "velocityX": 0.20036745681532722,
+                    "velocityY": 3.7776304462797263,
+                    "timestamp": 1.7461742152098825
+                },
+                {
+                    "x": 2.2664262161021513,
+                    "y": 6.122693971906954,
+                    "heading": 0.30917552472065174,
+                    "angularVelocity": 0.3870681185232194,
+                    "velocityX": 0.19202773950877436,
+                    "velocityY": 3.633116971112041,
+                    "timestamp": 1.8334829259703767
+                },
+                {
+                    "x": 2.279851444819284,
+                    "y": 6.376429629998848,
+                    "heading": 0.34011357351362637,
+                    "angularVelocity": 0.35435237244361656,
+                    "velocityX": 0.15376734578020115,
+                    "velocityY": 2.9061894956614607,
+                    "timestamp": 1.9207916367308708
+                },
+                {
+                    "x": 2.289924296428913,
+                    "y": 6.566719611746641,
+                    "heading": 0.3645867540299274,
+                    "angularVelocity": 0.2803062867740199,
+                    "velocityX": 0.11537052284807026,
+                    "velocityY": 2.179507406423607,
+                    "timestamp": 2.008100347491365
+                },
+                {
+                    "x": 2.296641079194263,
+                    "y": 6.6935744437684805,
+                    "heading": 0.3814115840440414,
+                    "angularVelocity": 0.192705056203017,
+                    "velocityX": 0.07693141619941274,
+                    "velocityY": 1.4529458849739352,
+                    "timestamp": 2.095409058251859
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 0.39,
+                    "angularVelocity": 0.09836837448577633,
+                    "velocityX": 0.03847177190545204,
+                    "velocityY": 0.7264516412973858,
+                    "timestamp": 2.182717769012353
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 0.39,
+                    "angularVelocity": 2.6936232860428826e-33,
+                    "velocityX": 0,
+                    "velocityY": 5.62663175256514e-35,
+                    "timestamp": 2.2700264797728473
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -25510,23 +24780,222 @@
                 {
                     "x": 5.176,
                     "y": 1.62,
-                    "heading": 2.5,
+                    "heading": -0.64,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 21
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": -0.64,
+                    "angularVelocity": -2.256840111784074e-31,
+                    "velocityX": -2.1308644711626517e-33,
+                    "velocityY": -1.0217798620245829e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.1132615869041915,
+                    "y": 1.6556053336408048,
+                    "heading": -0.6461329738139044,
+                    "angularVelocity": -0.06592692141286781,
+                    "velocityX": -0.6744118848768044,
+                    "velocityY": 0.3827425493802767,
+                    "timestamp": 0.09302684976743651
+                },
+                {
+                    "x": 4.987784771636742,
+                    "y": 1.7268160147391884,
+                    "heading": -0.6583988506620269,
+                    "angularVelocity": -0.131853081973501,
+                    "velocityX": -1.348823652323362,
+                    "velocityY": 0.7654852472851404,
+                    "timestamp": 0.18605369953487302
+                },
+                {
+                    "x": 4.799569566809376,
+                    "y": 1.8336320552005005,
+                    "heading": -0.6767974581881981,
+                    "angularVelocity": -0.19777738977689827,
+                    "velocityX": -2.02323528419909,
+                    "velocityY": 1.148228073167565,
+                    "timestamp": 0.2790805493023095
+                },
+                {
+                    "x": 4.54861599428641,
+                    "y": 1.9760534609244509,
+                    "heading": -0.7013284716868837,
+                    "angularVelocity": -0.26369820713065356,
+                    "velocityX": -2.697646681042515,
+                    "velocityY": 1.5309709624694186,
+                    "timestamp": 0.37210739906974605
+                },
+                {
+                    "x": 4.242554383646119,
+                    "y": 2.1497500028424636,
+                    "heading": -0.7013284745367188,
+                    "angularVelocity": -3.063454187141868e-8,
+                    "velocityX": -3.29003520387323,
+                    "velocityY": 1.8671656876724017,
+                    "timestamp": 0.4651342488371826
+                },
+                {
+                    "x": 3.9364928269084327,
+                    "y": 2.3234466397396827,
+                    "heading": -0.7013284773864732,
+                    "angularVelocity": -3.0633677229161635e-8,
+                    "velocityX": -3.290034624442601,
+                    "velocityY": 1.8671667086594281,
+                    "timestamp": 0.558161098604619
+                },
+                {
+                    "x": 3.630431318940711,
+                    "y": 2.497143362571822,
+                    "heading": -0.7013284802362207,
+                    "angularVelocity": -3.063360291364155e-8,
+                    "velocityX": -3.2900341001857365,
+                    "velocityY": 1.867167632424124,
+                    "timestamp": 0.6511879483720555
+                },
+                {
+                    "x": 3.3243698648507793,
+                    "y": 2.6708401803390394,
+                    "heading": -0.7013284830859674,
+                    "angularVelocity": -3.063359264364299e-8,
+                    "velocityX": -3.2900335210218676,
+                    "velocityY": 1.8671686529367946,
+                    "timestamp": 0.744214798139492
+                },
+                {
+                    "x": 3.0183085209148324,
+                    "y": 2.844537192202297,
+                    "heading": -0.7013284859357423,
+                    "angularVelocity": -3.063389607722874e-8,
+                    "velocityX": -3.290032336912287,
+                    "velocityY": 1.8671707393886097,
+                    "timestamp": 0.8372416479069285
+                },
+                {
+                    "x": 2.714360997870643,
+                    "y": 3.021907148464231,
+                    "heading": -0.7013284887932081,
+                    "angularVelocity": -3.071657002046057e-8,
+                    "velocityX": -3.2673096402172703,
+                    "velocityY": 1.9066533662630898,
+                    "timestamp": 0.9302684976743649
+                },
+                {
+                    "x": 2.4529780195268613,
+                    "y": 3.2575406255854364,
+                    "heading": -0.7013284919399162,
+                    "angularVelocity": -3.382580562568125e-8,
+                    "velocityX": -2.8097584621776335,
+                    "velocityY": 2.532962018065539,
+                    "timestamp": 1.0232953474418014
+                },
+                {
+                    "x": 2.2451503238556145,
+                    "y": 3.5415333045009083,
+                    "heading": -0.7013284961513817,
+                    "angularVelocity": -4.527150531202637e-8,
+                    "velocityX": -2.2340614155032408,
+                    "velocityY": 3.0528033532839474,
+                    "timestamp": 1.1163221972092379
+                },
+                {
+                    "x": 2.099615704405981,
+                    "y": 3.861945316168419,
+                    "heading": -0.7013284332934493,
+                    "angularVelocity": 6.756966674200994e-7,
+                    "velocityX": -1.564436717071087,
+                    "velocityY": 3.444296055047848,
+                    "timestamp": 1.2093490469766743
+                },
+                {
+                    "x": 2.0224929199626733,
+                    "y": 4.205305507968674,
+                    "heading": -0.7013282219960775,
+                    "angularVelocity": 0.0000022713589924444385,
+                    "velocityX": -0.8290379028862299,
+                    "velocityY": 3.6909794608614943,
+                    "timestamp": 1.3023758967441108
+                },
+                {
+                    "x": 2.0170244087221176,
+                    "y": 4.5571780060792255,
+                    "heading": -0.7013280555552984,
+                    "angularVelocity": 0.0000017891692504432307,
+                    "velocityX": -0.05878422470745493,
+                    "velocityY": 3.782483218449498,
+                    "timestamp": 1.3954027465115473
+                },
+                {
+                    "x": 2.079587370397571,
+                    "y": 4.887709205686539,
+                    "heading": -0.661535006789308,
+                    "angularVelocity": 0.4277587477752017,
+                    "velocityX": 0.6725258549747536,
+                    "velocityY": 3.5530731227987227,
+                    "timestamp": 1.4884295962789837
+                },
+                {
+                    "x": 2.143441139356313,
+                    "y": 5.160361503306635,
+                    "heading": -0.462273406431507,
+                    "angularVelocity": 2.1419794484704924,
+                    "velocityX": 0.6864014971846709,
+                    "velocityY": 2.9308989641347156,
+                    "timestamp": 1.5814564460464202
+                },
+                {
+                    "x": 2.1958862911911914,
+                    "y": 5.3618445595482385,
+                    "heading": -0.25085794958853325,
+                    "angularVelocity": 2.272628358065485,
+                    "velocityX": 0.5637636012182422,
+                    "velocityY": 2.165859176628076,
+                    "timestamp": 1.6744832958138567
+                },
+                {
+                    "x": 2.231834886718389,
+                    "y": 5.494391410801975,
+                    "heading": -0.08836645960984135,
+                    "angularVelocity": 1.7467160328971074,
+                    "velocityX": 0.38643247209883785,
+                    "velocityY": 1.4248236029178551,
+                    "timestamp": 1.7675101455812932
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": -3.8128513304390615e-33,
+                    "angularVelocity": 0.9499027413134385,
+                    "velocityX": 0.19526742362042082,
+                    "velocityY": 0.7052650859622128,
+                    "timestamp": 1.8605369953487296
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 0,
+                    "angularVelocity": 5.82518331797089e-32,
+                    "velocityX": 0,
+                    "velocityY": -4.1086986258215814e-33,
+                    "timestamp": 1.953563845116166
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -25567,16 +25036,16 @@
                 {
                     "x": 5.176,
                     "y": 1.62,
-                    "heading": 2.5,
+                    "heading": -0.64,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 17
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -25624,16 +25093,16 @@
                 {
                     "x": 5.176,
                     "y": 1.62,
-                    "heading": 2.5,
+                    "heading": -0.64,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 25
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -25681,23 +25150,222 @@
                 {
                     "x": 5.176,
                     "y": 1.62,
-                    "heading": 2.5,
+                    "heading": -0.64,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 21
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": -0.64,
+                    "angularVelocity": -2.491480567253294e-33,
+                    "velocityX": -2.0103104784295923e-34,
+                    "velocityY": 1.605618083193578e-34,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.240158813928264,
+                    "y": 1.6204190327819517,
+                    "heading": -0.5237338108165747,
+                    "angularVelocity": 1.2526389382742513,
+                    "velocityX": 0.6912398963489377,
+                    "velocityY": 0.004514612397400031,
+                    "timestamp": 0.0928170006782674
+                },
+                {
+                    "x": 5.369626464101249,
+                    "y": 1.6247166229091008,
+                    "heading": -0.2974184918320366,
+                    "angularVelocity": 2.4382959730515035,
+                    "velocityX": 1.394870004707003,
+                    "velocityY": 0.046301756097956205,
+                    "timestamp": 0.1856340013565348
+                },
+                {
+                    "x": 5.5674551093283755,
+                    "y": 1.639197763374398,
+                    "heading": 0.0009013327241277373,
+                    "angularVelocity": 3.214064475001015,
+                    "velocityX": 2.1313837312289596,
+                    "velocityY": 0.15601819019657054,
+                    "timestamp": 0.2784510020348022
+                },
+                {
+                    "x": 5.831585704851482,
+                    "y": 1.6774487923340957,
+                    "heading": 0.2480008567342784,
+                    "angularVelocity": 2.6622226769282764,
+                    "velocityX": 2.8457135394696165,
+                    "velocityY": 0.41211231434085943,
+                    "timestamp": 0.3712680027130696
+                },
+                {
+                    "x": 6.141513989428359,
+                    "y": 1.75469627200855,
+                    "heading": 0.3453177753240257,
+                    "angularVelocity": 1.0484816130514494,
+                    "velocityX": 3.339132726893265,
+                    "velocityY": 0.8322557194259916,
+                    "timestamp": 0.46408500339133696
+                },
+                {
+                    "x": 6.452443961209774,
+                    "y": 1.9008832260175597,
+                    "heading": 0.36957097138497697,
+                    "angularVelocity": 0.2613012258930921,
+                    "velocityX": 3.3499247929718696,
+                    "velocityY": 1.5750019171136467,
+                    "timestamp": 0.5569020040696043
+                },
+                {
+                    "x": 6.733109637262523,
+                    "y": 2.111863899724239,
+                    "heading": 0.36957109822659895,
+                    "angularVelocity": 0.0000013665774702958542,
+                    "velocityX": 3.023860650546378,
+                    "velocityY": 2.273082217319261,
+                    "timestamp": 0.6497190047478717
+                },
+                {
+                    "x": 6.964965499466835,
+                    "y": 2.3755472934997988,
+                    "heading": 0.36957101579118784,
+                    "angularVelocity": -8.881499138395027e-7,
+                    "velocityX": 2.4979891669630336,
+                    "velocityY": 2.840895437782658,
+                    "timestamp": 0.7425360054261391
+                },
+                {
+                    "x": 7.138307566970259,
+                    "y": 2.680897277378083,
+                    "heading": 0.36957096673523665,
+                    "angularVelocity": -5.285233395413412e-7,
+                    "velocityX": 1.867568077364202,
+                    "velocityY": 3.289806626446832,
+                    "timestamp": 0.8353530061044064
+                },
+                {
+                    "x": 7.245881008413397,
+                    "y": 3.0151337865477195,
+                    "heading": 0.3695709277774119,
+                    "angularVelocity": -4.197272531605919e-7,
+                    "velocityX": 1.1589842448800958,
+                    "velocityY": 3.6010268240427665,
+                    "timestamp": 0.9281700067826738
+                },
+                {
+                    "x": 7.300266060372569,
+                    "y": 3.36201756600148,
+                    "heading": 0.3695708905369613,
+                    "angularVelocity": -4.012244559973519e-7,
+                    "velocityX": 0.5859384763755527,
+                    "velocityY": 3.7372871017042186,
+                    "timestamp": 1.0209870074609413
+                },
+                {
+                    "x": 7.354650396010165,
+                    "y": 3.708901457760651,
+                    "heading": 0.3695708532965109,
+                    "angularVelocity": -4.0122445392101915e-7,
+                    "velocityX": 0.5859307588068667,
+                    "velocityY": 3.7372883116701696,
+                    "timestamp": 1.1138040081392087
+                },
+                {
+                    "x": 7.409034640641628,
+                    "y": 4.055785363787717,
+                    "heading": 0.36957081605605097,
+                    "angularVelocity": -4.012245569815441e-7,
+                    "velocityX": 0.5859297783169686,
+                    "velocityY": 3.737288465390881,
+                    "timestamp": 1.206621008817476
+                },
+                {
+                    "x": 7.463418795891099,
+                    "y": 4.402669283828022,
+                    "heading": 0.36957077881557854,
+                    "angularVelocity": -4.0122469066112833e-7,
+                    "velocityX": 0.5859288153253637,
+                    "velocityY": 3.7372886163679513,
+                    "timestamp": 1.2994380094957434
+                },
+                {
+                    "x": 7.517802854177964,
+                    "y": 4.7495532190700205,
+                    "heading": 0.3695707415750949,
+                    "angularVelocity": -4.0122481249990114e-7,
+                    "velocityX": 0.5859277706610763,
+                    "velocityY": 3.737288780149305,
+                    "timestamp": 1.3922550101740108
+                },
+                {
+                    "x": 7.56989445358868,
+                    "y": 5.095450165697103,
+                    "heading": 0.36620989603022114,
+                    "angularVelocity": -0.036209374579162214,
+                    "velocityX": 0.5612290747390308,
+                    "velocityY": 3.7266550750338174,
+                    "timestamp": 1.4850720108522781
+                },
+                {
+                    "x": 7.614129631330889,
+                    "y": 5.376850109081837,
+                    "heading": 0.24917441522367106,
+                    "angularVelocity": -1.2609272003114111,
+                    "velocityX": 0.4765848650458117,
+                    "velocityY": 3.0317715647821197,
+                    "timestamp": 1.5778890115305455
+                },
+                {
+                    "x": 7.6471559939174325,
+                    "y": 5.587299345919279,
+                    "heading": 0.1342077337923999,
+                    "angularVelocity": -1.2386381868746383,
+                    "velocityX": 0.35582234229936965,
+                    "velocityY": 2.267356575838128,
+                    "timestamp": 1.6707060122088129
+                },
+                {
+                    "x": 7.669079055841979,
+                    "y": 5.72720580380319,
+                    "heading": 0.04709984861251457,
+                    "angularVelocity": -0.9384906271840042,
+                    "velocityX": 0.23619662092442378,
+                    "velocityY": 1.5073365532341503,
+                    "timestamp": 1.7635230128870802
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": -9.59663427624948e-34,
+                    "angularVelocity": -0.5074485090913172,
+                    "velocityX": 0.11766103276571077,
+                    "velocityY": 0.751954875580805,
+                    "timestamp": 1.8563400135653476
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 0,
+                    "angularVelocity": 1.9250732473517817e-32,
+                    "velocityX": 0,
+                    "velocityY": 2.1529996497623123e-34,
+                    "timestamp": 1.949157014243615
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -25738,16 +25406,16 @@
                 {
                     "x": 5.176,
                     "y": 1.62,
-                    "heading": 2.5,
+                    "heading": -0.64,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 18
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -25795,7 +25463,7 @@
                 {
                     "x": 5.176,
                     "y": 1.62,
-                    "heading": 2.5,
+                    "heading": -0.64,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -25804,159 +25472,14 @@
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": 0,
-                    "velocityX": -5.177514657860589e-36,
-                    "velocityY": 7.381711039090797e-35,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.22399403974827,
-                    "y": 1.6219999714389142,
-                    "heading": 2.5657089279494847,
-                    "angularVelocity": 0.8361720983637341,
-                    "velocityX": 0.6107431391380216,
-                    "velocityY": 0.02545042761966764,
-                    "timestamp": 0.07858301906756712
-                },
-                {
-                    "x": 5.320421642922389,
-                    "y": 1.6267873270311852,
-                    "heading": 2.6934140350122218,
-                    "angularVelocity": 1.6250979992628447,
-                    "velocityX": 1.2270793909204332,
-                    "velocityY": 0.06092099348021541,
-                    "timestamp": 0.15716603813513425
-                },
-                {
-                    "x": 5.466088696265102,
-                    "y": 1.6359772771500425,
-                    "heading": 2.8734313337183015,
-                    "angularVelocity": 2.2907913292475777,
-                    "velocityX": 1.8536708702609945,
-                    "velocityY": 0.11694575021297698,
-                    "timestamp": 0.23574905720270137
-                },
-                {
-                    "x": 5.662207065544692,
-                    "y": 1.6523107767500078,
-                    "heading": 3.0800991109745315,
-                    "angularVelocity": 2.629929210005703,
-                    "velocityX": 2.495683820838744,
-                    "velocityY": 0.2078502428867176,
-                    "timestamp": 0.3143320762702685
-                },
-                {
-                    "x": 5.9065811613932215,
-                    "y": 1.6816988713551873,
-                    "heading": 3.2417008687725852,
-                    "angularVelocity": 2.056446287194764,
-                    "velocityX": 3.1097570282762126,
-                    "velocityY": 0.3739751278824109,
-                    "timestamp": 0.39291509533783564
-                },
-                {
-                    "x": 6.188716953254853,
-                    "y": 1.7271916925648765,
-                    "heading": 3.2799906959816494,
-                    "angularVelocity": 0.48725319621713953,
-                    "velocityX": 3.5902895461301405,
-                    "velocityY": 0.5789141439141413,
-                    "timestamp": 0.4714981144054028
-                },
-                {
-                    "x": 6.470102604778816,
-                    "y": 1.8230794962925625,
-                    "heading": 3.2799906961506404,
-                    "angularVelocity": 2.150475348001427e-9,
-                    "velocityX": 3.580743713626258,
-                    "velocityY": 1.22021022945479,
-                    "timestamp": 0.5500811334729699
-                },
-                {
-                    "x": 6.733423435664353,
-                    "y": 1.9610450600579923,
-                    "heading": 3.279990689877103,
-                    "angularVelocity": -7.983324169828431e-8,
-                    "velocityX": 3.3508617257263804,
-                    "velocityY": 1.755666369178364,
-                    "timestamp": 0.6286641525405371
-                },
-                {
-                    "x": 6.996743409287627,
-                    "y": 2.0990122599800607,
-                    "heading": 3.2799906836034283,
-                    "angularVelocity": -7.983499950790008e-8,
-                    "velocityX": 3.3508508167250133,
-                    "velocityY": 1.7556871899187498,
-                    "timestamp": 0.7072471716081042
-                },
-                {
-                    "x": 7.224495559877957,
-                    "y": 2.2183414399847847,
-                    "heading": 3.23332230759167,
-                    "angularVelocity": -0.5938735437439961,
-                    "velocityX": 2.8982362002979842,
-                    "velocityY": 1.5185110144740412,
-                    "timestamp": 0.7858301906756714
-                },
-                {
-                    "x": 7.406697323512462,
-                    "y": 2.313804835379604,
-                    "heading": 3.1959916534176562,
-                    "angularVelocity": -0.4750473399592355,
-                    "velocityX": 2.31858950949497,
-                    "velocityY": 1.2148094655505426,
-                    "timestamp": 0.8644132097432385
-                },
-                {
-                    "x": 7.543348661054632,
-                    "y": 2.3854024089057084,
-                    "heading": 3.1679955366885673,
-                    "angularVelocity": -0.356261658832645,
-                    "velocityX": 1.7389423206643992,
-                    "velocityY": 0.9111074424939519,
-                    "timestamp": 0.9429962288108057
-                },
-                {
-                    "x": 7.634449555023231,
-                    "y": 2.4331341352600573,
-                    "heading": 3.149331937782722,
-                    "angularVelocity": -0.2375016781907786,
-                    "velocityX": 1.1592949093781795,
-                    "velocityY": 0.607405097446157,
-                    "timestamp": 1.0215792478783727
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": -0.11875259939679916,
-                    "velocityX": 0.5796474291424605,
-                    "velocityY": 0.30370256860992423,
-                    "timestamp": 1.1001622669459399
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": -1.0945332293260443e-31,
-                    "velocityX": 7.198086319769953e-33,
-                    "velocityY": -6.46561486115161e-33,
-                    "timestamp": 1.178745286013507
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -25997,7 +25520,7 @@
                 {
                     "x": 5.176,
                     "y": 1.62,
-                    "heading": 2.5,
+                    "heading": -0.64,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -26006,7 +25529,7 @@
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -26017,146 +25540,146 @@
                 {
                     "x": 5.176,
                     "y": 1.62,
-                    "heading": 2.5,
+                    "heading": -0.64,
                     "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
+                    "velocityX": -3.378617665126574e-35,
+                    "velocityY": 1.1970750708579272e-32,
                     "timestamp": 0
                 },
                 {
-                    "x": 5.222452071774302,
-                    "y": 1.6045080698057588,
-                    "heading": 2.5267186887396895,
-                    "angularVelocity": 0.3466731100007665,
-                    "velocityX": 0.6027123690413302,
-                    "velocityY": -0.20100670630495998,
-                    "timestamp": 0.0770717080988209
-                },
-                {
-                    "x": 5.315452291782514,
-                    "y": 1.5735030960792171,
-                    "heading": 2.577725213280899,
-                    "angularVelocity": 0.6618060738424182,
-                    "velocityX": 1.2066713233988244,
-                    "velocityY": -0.40228735668849247,
-                    "timestamp": 0.1541434161976418
-                },
-                {
-                    "x": 5.4551316607074885,
-                    "y": 1.5269515495858468,
-                    "heading": 2.6491768389859613,
-                    "angularVelocity": 0.9270798256274155,
-                    "velocityX": 1.8123300024164322,
-                    "velocityY": -0.6040030465353413,
-                    "timestamp": 0.23121512429646268
-                },
-                {
-                    "x": 5.641664886749133,
-                    "y": 1.4648000786644693,
-                    "heading": 2.734238612850838,
-                    "angularVelocity": 1.1036705421892394,
-                    "velocityX": 2.420255508058452,
-                    "velocityY": -0.8064109704392095,
-                    "timestamp": 0.3082868323952836
-                },
-                {
-                    "x": 5.8751883475378746,
-                    "y": 1.3870009970649684,
-                    "heading": 2.8177772109144708,
-                    "angularVelocity": 1.0839074431374935,
-                    "velocityX": 3.02995050387775,
-                    "velocityY": -1.0094376200894257,
-                    "timestamp": 0.3853585404941045
-                },
-                {
-                    "x": 6.15120111527276,
-                    "y": 1.295041169269926,
-                    "heading": 2.8199508970154183,
-                    "angularVelocity": 0.028203424506439225,
-                    "velocityX": 3.5812462775702794,
-                    "velocityY": -1.193172307497486,
-                    "timestamp": 0.4624302485929254
-                },
-                {
-                    "x": 6.427838786740925,
-                    "y": 1.202967940667996,
-                    "heading": 2.8199509259126705,
-                    "angularVelocity": 3.7493981751466555e-7,
-                    "velocityX": 3.5893543596239312,
-                    "velocityY": -1.1946436750029477,
-                    "timestamp": 0.5395019566917463
-                },
-                {
-                    "x": 6.7044764770342145,
-                    "y": 1.110894768626946,
-                    "heading": 2.8199509548099355,
-                    "angularVelocity": 3.749399855143739e-7,
-                    "velocityX": 3.5893546038785895,
-                    "velocityY": -1.1946429411295039,
-                    "timestamp": 0.6165736647905672
-                },
-                {
-                    "x": 6.980650518345068,
-                    "y": 1.0194616665370815,
-                    "heading": 2.8221688917599996,
-                    "angularVelocity": 0.02877757616608584,
-                    "velocityX": 3.583338790892565,
-                    "velocityY": -1.1863380784636202,
-                    "timestamp": 0.6936453728893881
-                },
-                {
-                    "x": 7.214215740363564,
-                    "y": 0.9418010792136509,
-                    "heading": 2.905688153737692,
-                    "angularVelocity": 1.083656558780357,
-                    "velocityX": 3.030492352901014,
-                    "velocityY": -1.0076406665835769,
-                    "timestamp": 0.770717080988209
-                },
-                {
-                    "x": 7.400786993851664,
-                    "y": 0.8797765728247763,
-                    "heading": 2.990770900783412,
-                    "angularVelocity": 1.1039426677377822,
-                    "velocityX": 2.420748911505631,
-                    "velocityY": -0.8047636145464304,
-                    "timestamp": 0.8477887890870299
-                },
-                {
-                    "x": 7.540501987432905,
-                    "y": 0.8333423582814848,
-                    "heading": 3.0622172150737166,
-                    "angularVelocity": 0.9270109103939473,
-                    "velocityX": 1.8127922298296504,
-                    "velocityY": -0.6024806727230407,
-                    "timestamp": 0.9248604971858508
-                },
-                {
-                    "x": 7.633531379147319,
-                    "y": 0.802434096261962,
-                    "heading": 3.113237732512233,
-                    "angularVelocity": 0.661987630702283,
-                    "velocityX": 1.2070498242381353,
-                    "velocityY": -0.4010325290817799,
-                    "timestamp": 1.0019322052846718
+                    "x": 5.2224556017723405,
+                    "y": 1.6045081405552968,
+                    "heading": -0.6133597294207822,
+                    "angularVelocity": 0.3456555725750699,
+                    "velocityX": 0.6027580531582283,
+                    "velocityY": -0.20100574919793807,
+                    "timestamp": 0.07707172310503405
+                },
+                {
+                    "x": 5.3154629271275695,
+                    "y": 1.5735046785143454,
+                    "heading": -0.5625081883046615,
+                    "angularVelocity": 0.6597950463209403,
+                    "velocityX": 1.2067632798150456,
+                    "velocityY": -0.4022676643507709,
+                    "timestamp": 0.1541434462100681
+                },
+                {
+                    "x": 5.455153059548346,
+                    "y": 1.5269582787185583,
+                    "heading": -0.4912835813545399,
+                    "angularVelocity": 0.9241340932919854,
+                    "velocityX": 1.812469305122525,
+                    "velocityY": -0.6039361509065161,
+                    "timestamp": 0.23121516931510214
+                },
+                {
+                    "x": 5.641700996428857,
+                    "y": 1.4648196222358638,
+                    "heading": -0.40650985692768693,
+                    "angularVelocity": 1.0999329067980297,
+                    "velocityX": 2.4204459088877828,
+                    "velocityY": -0.8062445470177368,
+                    "timestamp": 0.3082868924201362
+                },
+                {
+                    "x": 5.875244676488674,
+                    "y": 1.3870501062573488,
+                    "heading": -0.3232929818469195,
+                    "angularVelocity": 1.0797328997998226,
+                    "velocityX": 3.0302122575038553,
+                    "velocityY": -1.0090538117660863,
+                    "timestamp": 0.38535861552517026
+                },
+                {
+                    "x": 6.151314773692177,
+                    "y": 1.2951594028189055,
+                    "heading": -0.3212316148216143,
+                    "angularVelocity": 0.026746087180327376,
+                    "velocityX": 3.581989425969788,
+                    "velocityY": -1.1922751916836483,
+                    "timestamp": 0.46243033863020433
+                },
+                {
+                    "x": 6.427941619479976,
+                    "y": 1.2030534747397732,
+                    "heading": -0.3212315858589365,
+                    "angularVelocity": 3.7578863628089786e-7,
+                    "velocityX": 3.5892131983452513,
+                    "velocityY": -1.1950677157381002,
+                    "timestamp": 0.5395020617352384
+                },
+                {
+                    "x": 6.704568483880494,
+                    "y": 1.1109476025613012,
+                    "heading": -0.3212315568962456,
+                    "angularVelocity": 3.75788807477423e-7,
+                    "velocityX": 3.5892134398439306,
+                    "velocityY": -1.195066990431097,
+                    "timestamp": 0.6165737848402725
+                },
+                {
+                    "x": 6.980707555601848,
+                    "y": 1.0195121848796085,
+                    "heading": -0.31890263703438493,
+                    "angularVelocity": 0.03021756576905414,
+                    "velocityX": 3.582884365320709,
+                    "velocityY": -1.186367892113736,
+                    "timestamp": 0.6936455079453065
+                },
+                {
+                    "x": 7.214256878757648,
+                    "y": 0.9418349206236948,
+                    "heading": -0.2350677354758115,
+                    "angularVelocity": 1.0877517483853638,
+                    "velocityX": 3.030285476263674,
+                    "velocityY": -1.0078568523770295,
+                    "timestamp": 0.7707172310503406
+                },
+                {
+                    "x": 7.4008131329346885,
+                    "y": 0.8797967164596704,
+                    "heading": -0.14969694055158067,
+                    "angularVelocity": 1.1076798530621503,
+                    "velocityX": 2.420553825205123,
+                    "velocityY": -0.8049411854913137,
+                    "timestamp": 0.8477889541553747
+                },
+                {
+                    "x": 7.540515620024687,
+                    "y": 0.8333523210527743,
+                    "heading": -0.07802156861665294,
+                    "angularVelocity": 0.9299827361748209,
+                    "velocityX": 1.8126296060568263,
+                    "velocityY": -0.6026126513818976,
+                    "timestamp": 0.9248606772604088
+                },
+                {
+                    "x": 7.633536076900545,
+                    "y": 0.8024373784171116,
+                    "heading": -0.026843164489729465,
+                    "angularVelocity": 0.6640360701054664,
+                    "velocityX": 1.2069336603398442,
+                    "velocityY": -0.40111913150730505,
+                    "timestamp": 1.0019324003654428
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
-                    "angularVelocity": 0.3472385411966328,
-                    "velocityX": 0.6029270921710836,
-                    "velocityY": -0.20025631509518968,
-                    "timestamp": 1.0790039133834926
+                    "heading": 1.4987800218197315e-33,
+                    "angularVelocity": 0.3482881063025846,
+                    "velocityX": 0.6028660217721296,
+                    "velocityY": -0.20029886182865572,
+                    "timestamp": 1.0790041234704768
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
-                    "angularVelocity": 1.7638718006253152e-30,
-                    "velocityX": 1.1888064162036181e-31,
-                    "velocityY": -2.2168268180884255e-31,
-                    "timestamp": 1.1560756214823134
+                    "heading": 0,
+                    "angularVelocity": -8.136191152722898e-32,
+                    "velocityX": 4.4028618628466005e-33,
+                    "velocityY": 0,
+                    "timestamp": 1.1560758465755108
                 }
             ],
             "constraints": [
@@ -26199,7 +25722,7 @@
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -26208,7 +25731,7 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": 3.5331853071795862,
+                    "heading": 0.393,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -26219,200 +25742,200 @@
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
-                    "angularVelocity": -3.261859244163768e-33,
-                    "velocityX": 7.471195721597621e-34,
-                    "velocityY": -1.6825458997868564e-33,
+                    "heading": 0,
+                    "angularVelocity": -1.8065144428954335e-34,
+                    "velocityX": -4.551747158955398e-35,
+                    "velocityY": 1.585845739465702e-34,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.6132701032534955,
-                    "y": 7.458186361668427,
-                    "heading": 3.162690215100173,
-                    "angularVelocity": 0.25198683492477825,
-                    "velocityX": -0.7410707832329344,
-                    "velocityY": -0.09788011341186789,
-                    "timestamp": 0.09004524028783478
-                },
-                {
-                    "x": 7.4797323779606675,
-                    "y": 7.440537712219312,
-                    "heading": 3.2047640757495373,
-                    "angularVelocity": 0.46725246681415944,
-                    "velocityX": -1.4830070403051525,
-                    "velocityY": -0.19599758291165675,
-                    "timestamp": 0.18009048057566956
-                },
-                {
-                    "x": 7.27928179650239,
-                    "y": 7.414023208632297,
-                    "heading": 3.2605597645105227,
-                    "angularVelocity": 0.6196406226762411,
-                    "velocityX": -2.226109684615486,
-                    "velocityY": -0.29445758045912396,
-                    "timestamp": 0.27013572086350435
-                },
-                {
-                    "x": 7.011816476608683,
-                    "y": 7.378594826276994,
-                    "heading": 3.318124935630623,
-                    "angularVelocity": 0.6392916597933375,
-                    "velocityX": -2.9703437853987498,
-                    "velocityY": -0.3934509169174634,
-                    "timestamp": 0.36018096115133913
-                },
-                {
-                    "x": 6.67842119537258,
-                    "y": 7.334255965416845,
-                    "heading": 3.332157072604317,
-                    "angularVelocity": 0.15583429983460975,
-                    "velocityX": -3.70253086304603,
-                    "velocityY": -0.49240649165261274,
-                    "timestamp": 0.4502262014391739
-                },
-                {
-                    "x": 6.34069723545727,
-                    "y": 7.289811993684431,
-                    "heading": 3.332157092024104,
-                    "angularVelocity": 2.1566700178855977e-7,
-                    "velocityX": -3.7506031283358707,
-                    "velocityY": -0.4935738034608694,
-                    "timestamp": 0.5402714417270087
-                },
-                {
-                    "x": 6.002973284663831,
-                    "y": 7.245367952636118,
-                    "heading": 3.3321571114438866,
-                    "angularVelocity": 2.156669548142136e-7,
-                    "velocityX": -3.750603027032689,
-                    "velocityY": -0.49357457325056325,
-                    "timestamp": 0.6303166820148435
-                },
-                {
-                    "x": 5.665249344531261,
-                    "y": 7.200923830577433,
-                    "heading": 3.3321571308636764,
-                    "angularVelocity": 2.1566703381132295e-7,
-                    "velocityX": -3.750602908638108,
-                    "velocityY": -0.4935754729135791,
-                    "timestamp": 0.7203619223026783
-                },
-                {
-                    "x": 5.32752541585805,
-                    "y": 7.156479621440943,
-                    "heading": 3.332157150283477,
-                    "angularVelocity": 2.1566715012677575e-7,
-                    "velocityX": -3.750602781375866,
-                    "velocityY": -0.4935764399586428,
-                    "timestamp": 0.810407162590513
-                },
-                {
-                    "x": 4.9898014989996415,
-                    "y": 7.112035322525874,
-                    "heading": 3.332157169703281,
-                    "angularVelocity": 2.1566719028685347e-7,
-                    "velocityX": -3.750602650166234,
-                    "velocityY": -0.4935774369972257,
-                    "timestamp": 0.9004524028783478
-                },
-                {
-                    "x": 4.652077593988678,
-                    "y": 7.06759093358437,
-                    "heading": 3.33215718912308,
-                    "angularVelocity": 2.1566713427464117e-7,
-                    "velocityX": -3.750602518594089,
-                    "velocityY": -0.49357843678839325,
-                    "timestamp": 0.9904976431661826
-                },
-                {
-                    "x": 4.314353700505922,
-                    "y": 7.023146457042436,
-                    "heading": 3.332157208542871,
-                    "angularVelocity": 2.156670494902004e-7,
-                    "velocityX": -3.7506023905672445,
-                    "velocityY": -0.4935794096374718,
-                    "timestamp": 1.0805428834540174
-                },
-                {
-                    "x": 3.9766298178690414,
-                    "y": 6.978701898085155,
-                    "heading": 3.3321572279626577,
-                    "angularVelocity": 2.1566699709425413e-7,
-                    "velocityX": -3.750602270118079,
-                    "velocityY": -0.4935803249034738,
-                    "timestamp": 1.1705881237418523
-                },
-                {
-                    "x": 3.6389059453045984,
-                    "y": 6.934257262589842,
-                    "heading": 3.332157247382445,
-                    "angularVelocity": 2.1566700145288475e-7,
-                    "velocityX": -3.7506021582583307,
-                    "velocityY": -0.49358117489878034,
-                    "timestamp": 1.2606333640296872
-                },
-                {
-                    "x": 3.301182082389383,
-                    "y": 6.889812553772521,
-                    "heading": 3.332157266802242,
-                    "angularVelocity": 2.156671157814786e-7,
-                    "velocityX": -3.750602051098551,
-                    "velocityY": -0.4935819891784481,
-                    "timestamp": 1.350678604317522
-                },
-                {
-                    "x": 2.9679743508365415,
-                    "y": 6.845442796163545,
-                    "heading": 3.346601240828058,
-                    "angularVelocity": 0.16040796803523288,
-                    "velocityX": -3.7004480246565397,
-                    "velocityY": -0.4927496163833412,
-                    "timestamp": 1.440723844605357
-                },
-                {
-                    "x": 2.7005730136886035,
-                    "y": 6.810002161458836,
-                    "heading": 3.406941333368848,
-                    "angularVelocity": 0.670108629261352,
-                    "velocityX": -2.969633223179528,
-                    "velocityY": -0.393586985735394,
-                    "timestamp": 1.5307690848931919
-                },
-                {
-                    "x": 2.500185866956939,
-                    "y": 6.783477694334395,
-                    "heading": 3.465398232800302,
-                    "angularVelocity": 0.6491947741445663,
-                    "velocityX": -2.2254052084387275,
-                    "velocityY": -0.294568230809919,
-                    "timestamp": 1.6208143251810267
-                },
-                {
-                    "x": 2.366700027158657,
-                    "y": 6.765819972243713,
-                    "heading": 3.5094432939271947,
-                    "angularVelocity": 0.4891436902839078,
-                    "velocityX": -1.4824308244565385,
-                    "velocityY": -0.19609833939293356,
-                    "timestamp": 1.7108595654688616
+                    "x": 7.613270090763082,
+                    "y": 7.458186379338955,
+                    "heading": 0.02268667453207945,
+                    "angularVelocity": 0.251947675366546,
+                    "velocityX": -0.7410713935129049,
+                    "velocityY": -0.09787997945546167,
+                    "timestamp": 0.09004518298918118
+                },
+                {
+                    "x": 7.479732324735008,
+                    "y": 7.440537819966174,
+                    "heading": 0.06475236187583014,
+                    "angularVelocity": 0.4671619952041724,
+                    "velocityX": -1.4830084363770935,
+                    "velocityY": -0.19599670728529434,
+                    "timestamp": 0.18009036597836237
+                },
+                {
+                    "x": 7.279281674321286,
+                    "y": 7.414023565204846,
+                    "heading": 0.12053400112707001,
+                    "angularVelocity": 0.6194849896406114,
+                    "velocityX": -2.226111866948016,
+                    "velocityY": -0.2944550044894082,
+                    "timestamp": 0.27013554896754355
+                },
+                {
+                    "x": 7.011816314313711,
+                    "y": 7.378595756973036,
+                    "heading": 0.17807802747436277,
+                    "angularVelocity": 0.63905724256462,
+                    "velocityX": -2.970346121010285,
+                    "velocityY": -0.39344479133399113,
+                    "timestamp": 0.36018073195672473
+                },
+                {
+                    "x": 6.678420949429204,
+                    "y": 7.334257918470772,
+                    "heading": 0.1920910018934364,
+                    "angularVelocity": 0.15562158856134758,
+                    "velocityX": -3.702534148046146,
+                    "velocityY": -0.4923954511546947,
+                    "timestamp": 0.4502259149459059
+                },
+                {
+                    "x": 6.340697263329751,
+                    "y": 7.289813527368774,
+                    "heading": 0.19209102131154623,
+                    "angularVelocity": 2.156485134469417e-7,
+                    "velocityX": -3.7506024740938,
+                    "velocityY": -0.49357877486170487,
+                    "timestamp": 0.5402710979350871
+                },
+                {
+                    "x": 6.002973586353306,
+                    "y": 7.245369066942952,
+                    "heading": 0.1920910407296517,
+                    "angularVelocity": 2.1564846490105217e-7,
+                    "velocityX": -3.7506023727779296,
+                    "velocityY": -0.49357954473991933,
+                    "timestamp": 0.6303162809242683
+                },
+                {
+                    "x": 5.665249920039304,
+                    "y": 7.200924525495619,
+                    "heading": 0.1920910601477644,
+                    "angularVelocity": 2.156485448418072e-7,
+                    "velocityX": -3.750602254365785,
+                    "velocityY": -0.4935804445271851,
+                    "timestamp": 0.7203614639134495
+                },
+                {
+                    "x": 5.327526265186639,
+                    "y": 7.156479896956337,
+                    "heading": 0.1920910795658876,
+                    "angularVelocity": 2.1564866168111011e-7,
+                    "velocityX": -3.7506021270814847,
+                    "velocityY": -0.4935814117299572,
+                    "timestamp": 0.8104066469026306
+                },
+                {
+                    "x": 4.989802622149171,
+                    "y": 7.1120351786364,
+                    "heading": 0.19209109898401447,
+                    "angularVelocity": 2.1564870237166035e-7,
+                    "velocityX": -3.75060199586739,
+                    "velocityY": -0.4935824087922295,
+                    "timestamp": 0.9004518298918118
+                },
+                {
+                    "x": 4.652078990957916,
+                    "y": 7.067590370300322,
+                    "heading": 0.19209111840213633,
+                    "angularVelocity": 2.156486466006611e-7,
+                    "velocityX": -3.75060186430886,
+                    "velocityY": -0.49358340846970244,
+                    "timestamp": 0.990497012880993
+                },
+                {
+                    "x": 4.314355371293917,
+                    "y": 7.023145474371938,
+                    "heading": 0.19209113782025056,
+                    "angularVelocity": 2.156485622267904e-7,
+                    "velocityX": -3.7506017362924924,
+                    "velocityY": -0.4935843812291933,
+                    "timestamp": 1.0805421958701742
+                },
+                {
+                    "x": 3.97663176247589,
+                    "y": 6.97870049602832,
+                    "heading": 0.19209115723836015,
+                    "angularVelocity": 2.1564851021035735e-7,
+                    "velocityX": -3.7506016158421813,
+                    "velocityY": -0.4935852964945219,
+                    "timestamp": 1.1705873788593553
+                },
+                {
+                    "x": 3.638908163731045,
+                    "y": 6.934255441141791,
+                    "heading": 0.1920911766564701,
+                    "angularVelocity": 2.1564851460850062e-7,
+                    "velocityX": -3.7506015039740865,
+                    "velocityY": -0.49358614654454386,
+                    "timestamp": 1.2606325618485363
+                },
+                {
+                    "x": 3.301184574635452,
+                    "y": 6.88981031293383,
+                    "heading": 0.1920911960745901,
+                    "angularVelocity": 2.156486262427343e-7,
+                    "velocityX": -3.750601396813981,
+                    "velocityY": -0.49358696081834263,
+                    "timestamp": 1.3506777448377174
+                },
+                {
+                    "x": 2.967975728993463,
+                    "y": 6.845441681932465,
+                    "heading": 0.20651305517000687,
+                    "angularVelocity": 0.16016247195754518,
+                    "velocityX": -3.70046275192781,
+                    "velocityY": -0.49273741835468604,
+                    "timestamp": 1.4407229278268985
+                },
+                {
+                    "x": 2.7005739783902083,
+                    "y": 6.810001707269663,
+                    "heading": 0.26681564809465086,
+                    "angularVelocity": 0.6696926023448618,
+                    "velocityX": -2.969639704495717,
+                    "velocityY": -0.3935799060684999,
+                    "timestamp": 1.5307681108160796
+                },
+                {
+                    "x": 2.5001863972917366,
+                    "y": 6.7834775390625595,
+                    "heading": 0.32524314681975514,
+                    "angularVelocity": 0.6488686766523009,
+                    "velocityX": -2.2254114484119376,
+                    "velocityY": -0.2945650986159953,
+                    "timestamp": 1.6208132938052606
+                },
+                {
+                    "x": 2.3667002159439683,
+                    "y": 6.765819938844314,
+                    "heading": 0.3692681668658828,
+                    "angularVelocity": 0.4889214346026397,
+                    "velocityX": -1.482435560865111,
+                    "velocityY": -0.1960971107179338,
+                    "timestamp": 1.7108584767944417
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": 3.5331853071795862,
-                    "angularVelocity": 0.26366760948717133,
-                    "velocityX": -0.7407390656679514,
-                    "velocityY": -0.09795045485490664,
-                    "timestamp": 1.8009048057566965
+                    "heading": 0.393,
+                    "angularVelocity": 0.2635547215998056,
+                    "velocityX": -0.7407416335861364,
+                    "velocityY": -0.09795014626572365,
+                    "timestamp": 1.8009036597836228
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": 3.5331853071795862,
-                    "angularVelocity": 1.378321455008778e-32,
+                    "heading": 0.393,
+                    "angularVelocity": 9.742765950850105e-34,
                     "velocityX": 0,
-                    "velocityY": 4.004611385179353e-34,
-                    "timestamp": 1.8909500460445314
+                    "velocityY": -8.251938001070805e-34,
+                    "timestamp": 1.8909488427728038
                 }
             ],
             "constraints": [
@@ -26455,7 +25978,7 @@
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -26464,14 +25987,204 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 0,
+                    "angularVelocity": 1.1003498306069892e-28,
+                    "velocityX": -2.6796850098783343e-31,
+                    "velocityY": 7.1039345927107565e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.602663557475489,
+                    "y": 7.440503469641943,
+                    "heading": 0.0012193469853657303,
+                    "angularVelocity": 0.01231450998750236,
+                    "velocityX": -0.781041332205243,
+                    "velocityY": -0.26759551763859557,
+                    "timestamp": 0.09901709337987814
+                },
+                {
+                    "x": 7.447990676613656,
+                    "y": 7.387510406307137,
+                    "heading": 0.00365804177605024,
+                    "angularVelocity": 0.024629028255928873,
+                    "velocityX": -1.562082622122956,
+                    "velocityY": -0.5351910617240387,
+                    "timestamp": 0.1980341867597563
+                },
+                {
+                    "x": 7.2159813646219675,
+                    "y": 7.3080208081596165,
+                    "heading": 0.0073160847395553,
+                    "angularVelocity": 0.03694355023585351,
+                    "velocityX": -2.3431238392505196,
+                    "velocityY": -0.8027866243514209,
+                    "timestamp": 0.29705128013963444
+                },
+                {
+                    "x": 6.906635640594062,
+                    "y": 7.202034677060231,
+                    "heading": 0.012193473491592185,
+                    "angularVelocity": 0.049258048136210826,
+                    "velocityX": -3.1241648635463743,
+                    "velocityY": -1.0703821681855634,
+                    "timestamp": 0.3960683735195126
+                },
+                {
+                    "x": 6.5522807849123526,
+                    "y": 7.080627711247213,
+                    "heading": 0.012193474015757445,
+                    "angularVelocity": 5.293684623965923e-9,
+                    "velocityX": -3.5787240726429963,
+                    "velocityY": -1.2261212854152546,
+                    "timestamp": 0.4950854668993907
+                },
+                {
+                    "x": 6.197925949682556,
+                    "y": 6.959220685740379,
+                    "heading": 0.012193474539927869,
+                    "angularVelocity": 5.293736792766535e-9,
+                    "velocityX": -3.578723866093675,
+                    "velocityY": -1.2261218882789995,
+                    "timestamp": 0.5941025602792689
+                },
+                {
+                    "x": 5.843571141718115,
+                    "y": 6.8378135806532265,
+                    "heading": 0.012193475064112974,
+                    "angularVelocity": 5.2938850654703056e-9,
+                    "velocityX": -3.578723590733579,
+                    "velocityY": -1.2261226919818409,
+                    "timestamp": 0.6931196536591471
+                },
+                {
+                    "x": 5.489216369151057,
+                    "y": 6.716406372250631,
+                    "heading": 0.012193475588313017,
+                    "angularVelocity": 5.294035915259789e-9,
+                    "velocityX": -3.578723233245997,
+                    "velocityY": -1.2261237353920091,
+                    "timestamp": 0.7921367470390253
+                },
+                {
+                    "x": 5.134861642331667,
+                    "y": 6.594999030323008,
+                    "heading": 0.012193476112502804,
+                    "angularVelocity": 5.293932348218223e-9,
+                    "velocityX": -3.5787227712281067,
+                    "velocityY": -1.2261250838968347,
+                    "timestamp": 0.8911538404189034
+                },
+                {
+                    "x": 4.780507205250472,
+                    "y": 6.473590842716537,
+                    "heading": 0.012193476636274196,
+                    "angularVelocity": 5.2897066685502164e-9,
+                    "velocityX": -3.578719845084944,
+                    "velocityY": -1.2261336246326482,
+                    "timestamp": 0.9901709337987816
+                },
+                {
+                    "x": 4.428189328838181,
+                    "y": 6.346393637776016,
+                    "heading": 0.012193476110055806,
+                    "angularVelocity": -5.314419742287748e-9,
+                    "velocityX": -3.558152076436196,
+                    "velocityY": -1.2845984526381864,
+                    "timestamp": 1.0891880271786598
+                },
+                {
+                    "x": 4.075871503880035,
+                    "y": 6.219196290315138,
+                    "heading": 0.012193475583846744,
+                    "angularVelocity": -5.314325547246037e-9,
+                    "velocityX": -3.5581515567870925,
+                    "velocityY": -1.2845998919892312,
+                    "timestamp": 1.188205120558538
+                },
+                {
+                    "x": 3.723553713185669,
+                    "y": 6.091998847948706,
+                    "heading": 0.012193475057649396,
+                    "angularVelocity": -5.314207229499686e-9,
+                    "velocityX": -3.5581512107480435,
+                    "velocityY": -1.2846008504657016,
+                    "timestamp": 1.2872222139384162
+                },
+                {
+                    "x": 3.3712359540403063,
+                    "y": 5.96480131819632,
+                    "heading": 0.012193474531462604,
+                    "angularVelocity": -5.314100634839972e-9,
+                    "velocityX": -3.5581508921262577,
+                    "velocityY": -1.284601732999717,
+                    "timestamp": 1.3862393073182944
+                },
+                {
+                    "x": 3.018918225427334,
+                    "y": 5.837603703873982,
+                    "heading": 0.012193474005277722,
+                    "angularVelocity": -5.314081331919232e-9,
+                    "velocityX": -3.558150583771516,
+                    "velocityY": -1.2846025870942077,
+                    "timestamp": 1.4852564006981726
+                },
+                {
+                    "x": 2.7113509459543246,
+                    "y": 5.726562245993832,
+                    "heading": 0.0073160988837450616,
+                    "angularVelocity": -0.04925791047811804,
+                    "velocityX": -3.1062038782842247,
+                    "velocityY": -1.121437259869279,
+                    "timestamp": 1.5842734940780507
+                },
+                {
+                    "x": 2.48067547425723,
+                    "y": 5.6432811332300385,
+                    "heading": 0.003658054936391331,
+                    "angularVelocity": -0.03694356017200323,
+                    "velocityX": -2.329653030836908,
+                    "velocityY": -0.8410781403599302,
+                    "timestamp": 1.683290587457929
+                },
+                {
+                    "x": 2.326891824704753,
+                    "y": 5.5877603809363166,
+                    "heading": 0.0012193531500403563,
+                    "angularVelocity": -0.024629098907027813,
+                    "velocityX": -1.5531020382762346,
+                    "velocityY": -0.560718865789336,
+                    "timestamp": 1.782307680837807
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": -2.184437138856228e-30,
+                    "angularVelocity": -0.012314572246193395,
+                    "velocityX": -0.7765510184161662,
+                    "velocityY": -0.28035948126465987,
+                    "timestamp": 1.8813247742176853
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 0,
+                    "angularVelocity": 2.219047153199805e-29,
+                    "velocityX": 0,
+                    "velocityY": -7.552577135588168e-32,
+                    "timestamp": 1.9803418675975635
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -26512,7 +26225,7 @@
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -26521,7 +26234,7 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -26569,7 +26282,7 @@
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -26578,7 +26291,7 @@
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": 3.4031853071795863,
+                    "heading": 0.263,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -26589,128 +26302,128 @@
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
-                    "angularVelocity": -8.261119414836203e-30,
-                    "velocityX": 2.821294333158614e-31,
-                    "velocityY": -2.62095350113427e-31,
+                    "heading": 1.9214160460800783e-32,
+                    "angularVelocity": -7.67482132813978e-30,
+                    "velocityX": -1.6843046577095525e-31,
+                    "velocityY": 2.6209535651210065e-31,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.617797825979087,
-                    "y": 7.446201794633709,
-                    "heading": 3.1534584721408314,
-                    "angularVelocity": 0.15162222909690662,
-                    "velocityX": -0.7007654495821862,
-                    "velocityY": -0.2343111629647132,
-                    "timestamp": 0.08876318611043306
-                },
-                {
-                    "x": 7.493368421156802,
-                    "y": 7.404602777573554,
-                    "heading": 3.178746831508521,
-                    "angularVelocity": 0.28489693164482327,
-                    "velocityX": -1.4018131871435613,
-                    "velocityY": -0.4686516886383534,
-                    "timestamp": 0.17752637222086612
-                },
-                {
-                    "x": 7.306676117263916,
-                    "y": 7.342201100203473,
-                    "heading": 3.213107467103745,
-                    "angularVelocity": 0.38710457678121596,
-                    "velocityX": -2.103262760992579,
-                    "velocityY": -0.7030130406946671,
-                    "timestamp": 0.2662895583312992
-                },
-                {
-                    "x": 7.057674416387051,
-                    "y": 7.2590025582832345,
-                    "heading": 3.2508668949579036,
-                    "angularVelocity": 0.4253951385364178,
-                    "velocityX": -2.8052361771613255,
-                    "velocityY": -0.9373090981171329,
-                    "timestamp": 0.35505274444173224
-                },
-                {
-                    "x": 6.74646520076521,
-                    "y": 7.155112122369572,
-                    "heading": 3.2734870599198675,
-                    "angularVelocity": 0.25483723550559106,
-                    "velocityX": -3.5060617949738107,
-                    "velocityY": -1.1704225644085642,
-                    "timestamp": 0.4438159305521653
-                },
-                {
-                    "x": 6.4280476263908515,
-                    "y": 7.048517625465743,
-                    "heading": 3.2734870708664827,
-                    "angularVelocity": 1.233247689161178e-7,
-                    "velocityX": -3.5872706729821253,
-                    "velocityY": -1.200886331076399,
-                    "timestamp": 0.5325791166625984
-                },
-                {
-                    "x": 6.109630072909297,
-                    "y": 6.941923066153303,
-                    "heading": 3.273487081813265,
-                    "angularVelocity": 1.2332664336181238e-7,
-                    "velocityX": -3.5872704376046,
-                    "velocityY": -1.2008870341690594,
-                    "timestamp": 0.6213423027730315
-                },
-                {
-                    "x": 5.798381254717505,
-                    "y": 6.838018859230789,
-                    "heading": 3.296331970538473,
-                    "angularVelocity": 0.25736895809035,
-                    "velocityX": -3.5065079548134683,
-                    "velocityY": -1.1705777076875172,
-                    "timestamp": 0.7101054888834646
-                },
-                {
-                    "x": 5.549362929627812,
-                    "y": 6.754814274993791,
-                    "heading": 3.3329076351356512,
-                    "angularVelocity": 0.4120589424672841,
-                    "velocityX": -2.8054234643993468,
-                    "velocityY": -0.9373771704727861,
-                    "timestamp": 0.7988686749938977
-                },
-                {
-                    "x": 5.36265371808689,
-                    "y": 6.692406142204076,
-                    "heading": 3.3659874710813016,
-                    "angularVelocity": 0.37267517533582245,
-                    "velocityX": -2.103453241394186,
-                    "velocityY": -0.7030857670337234,
-                    "timestamp": 0.8876318611043308
-                },
-                {
-                    "x": 5.238210282238424,
-                    "y": 6.650801541740841,
-                    "heading": 3.3902750965399324,
-                    "angularVelocity": 0.27362273171986246,
-                    "velocityX": -1.4019712597217382,
-                    "velocityY": -0.4687145908921929,
-                    "timestamp": 0.9763950472147639
+                    "x": 7.6177977502927385,
+                    "y": 7.446201757051976,
+                    "heading": 0.01344244845353824,
+                    "angularVelocity": 0.15144190849275008,
+                    "velocityX": -0.7007672331072132,
+                    "velocityY": -0.23431189760056984,
+                    "timestamp": 0.08876306820393338
+                },
+                {
+                    "x": 7.493368265532485,
+                    "y": 7.404602703026682,
+                    "heading": 0.038702181990287625,
+                    "angularVelocity": 0.2845748129935982,
+                    "velocityX": -1.4018159497887253,
+                    "velocityY": -0.4686527276087507,
+                    "timestamp": 0.17752613640786677
+                },
+                {
+                    "x": 7.306675971092295,
+                    "y": 7.342201042854806,
+                    "heading": 0.0730274998055895,
+                    "angularVelocity": 0.38670720275422266,
+                    "velocityX": -2.1032654483226527,
+                    "velocityY": -0.7030137807724448,
+                    "timestamp": 0.26628920461180017
+                },
+                {
+                    "x": 7.057674447994484,
+                    "y": 7.2590026396848994,
+                    "heading": 0.11075741429874605,
+                    "angularVelocity": 0.42506320764649574,
+                    "velocityX": -2.8052379005867665,
+                    "velocityY": -0.9373087800171416,
+                    "timestamp": 0.35505227281573354
+                },
+                {
+                    "x": 6.746464686770232,
+                    "y": 7.155112270199717,
+                    "heading": 0.1333971305446714,
+                    "angularVelocity": 0.25505783773756635,
+                    "velocityX": -3.506072598902806,
+                    "velocityY": -1.1704233707338827,
+                    "timestamp": 0.4438153410196669
+                },
+                {
+                    "x": 6.428047616602866,
+                    "y": 7.048517672195703,
+                    "heading": 0.13339714147429826,
+                    "angularVelocity": 1.23133576958653e-7,
+                    "velocityX": -3.5872697576863386,
+                    "velocityY": -1.200889065236652,
+                    "timestamp": 0.5325784092236003
+                },
+                {
+                    "x": 6.109630567326096,
+                    "y": 6.9419230117899575,
+                    "heading": 0.13339715240409608,
+                    "angularVelocity": 1.231354889650669e-7,
+                    "velocityX": -3.5872695223333455,
+                    "velocityY": -1.200889768252866,
+                    "timestamp": 0.6213414774275338
+                },
+                {
+                    "x": 5.798381237872348,
+                    "y": 6.8380188367314085,
+                    "heading": 0.15626327537058046,
+                    "angularVelocity": 0.25760852379277716,
+                    "velocityX": -3.506518372457005,
+                    "velocityY": -1.170578903621919,
+                    "timestamp": 0.7101045456314672
+                },
+                {
+                    "x": 5.549363092262374,
+                    "y": 6.7548143649024714,
+                    "heading": 0.19280777245315245,
+                    "angularVelocity": 0.41170835825755314,
+                    "velocityX": -2.8054251689136636,
+                    "velocityY": -0.9373771492347068,
+                    "timestamp": 0.7988676138354006
+                },
+                {
+                    "x": 5.36265388534366,
+                    "y": 6.692406232212667,
+                    "heading": 0.22585001680889633,
+                    "angularVelocity": 0.3722521655212111,
+                    "velocityX": -2.1034559833970325,
+                    "velocityY": -0.7030866998372853,
+                    "timestamp": 0.887630682039334
+                },
+                {
+                    "x": 5.238210362873152,
+                    "y": 6.650801584329758,
+                    "heading": 0.25010700866879837,
+                    "angularVelocity": 0.27327797868904535,
+                    "velocityX": -1.4019740978791027,
+                    "velocityY": -0.46871574772688473,
+                    "timestamp": 0.9763937502432675
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": 0.14544555244060076,
-                    "velocityX": -0.7008567962045305,
-                    "velocityY": -0.2343487503412182,
-                    "timestamp": 1.065158233325197
+                    "heading": 0.263,
+                    "angularVelocity": 0.1452517538281686,
+                    "velocityX": -0.7008586355989793,
+                    "velocityY": -0.23434954143793293,
+                    "timestamp": 1.065156818447201
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": 3.4031853071795863,
+                    "heading": 0.263,
                     "angularVelocity": 0,
-                    "velocityX": 1.430834166779729e-32,
-                    "velocityY": -6.582148241954714e-30,
-                    "timestamp": 1.15392141943563
+                    "velocityX": -1.1321901910980418e-33,
+                    "velocityY": 0,
+                    "timestamp": 1.1539198866511342
                 }
             ],
             "constraints": [
@@ -26753,7 +26466,7 @@
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -26762,7 +26475,7 @@
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -26773,146 +26486,146 @@
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
-                    "angularVelocity": -3.436405776355784e-29,
-                    "velocityX": 4.0035032875632384e-30,
-                    "velocityY": 0,
+                    "heading": -1.0300263407047986e-28,
+                    "angularVelocity": -5.012733855435028e-29,
+                    "velocityX": 0,
+                    "velocityY": -6.93481134012921e-30,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.593933573152452,
-                    "y": 7.445362365518837,
-                    "heading": 3.094092714332116,
-                    "angularVelocity": -0.44270408674328476,
-                    "velocityX": -0.8299762955364275,
-                    "velocityY": -0.2086611977356942,
-                    "timestamp": 0.10369745173495665
-                },
-                {
-                    "x": 7.421555540437275,
-                    "y": 7.402033887236261,
-                    "heading": 3.009154332069554,
-                    "angularVelocity": -0.8190980669386346,
-                    "velocityX": -1.6623169598782952,
-                    "velocityY": -0.41783551627973,
-                    "timestamp": 0.2073949034699133
-                },
-                {
-                    "x": 7.1624840188793355,
-                    "y": 7.336952212665302,
-                    "heading": 2.898930851860923,
-                    "angularVelocity": -1.0629333543342216,
-                    "velocityX": -2.4983402892108755,
-                    "velocityY": -0.6276111271981535,
-                    "timestamp": 0.31109235520486994
-                },
-                {
-                    "x": 6.816593659112812,
-                    "y": 7.250270000971659,
-                    "heading": 2.8098268582890107,
-                    "angularVelocity": -0.8592688834789594,
-                    "velocityX": -3.335572417445675,
-                    "velocityY": -0.8359145788384311,
-                    "timestamp": 0.4147898069398266
-                },
-                {
-                    "x": 6.436271393844499,
-                    "y": 7.15414721538473,
-                    "heading": 2.8098268314636434,
-                    "angularVelocity": -2.586887820849056e-7,
-                    "velocityX": -3.6676143811169655,
-                    "velocityY": -0.9269541727276405,
-                    "timestamp": 0.5184872586747833
-                },
-                {
-                    "x": 6.055949159763292,
-                    "y": 7.058024306401983,
-                    "heading": 2.8098268046376322,
-                    "angularVelocity": -2.5869499105961153e-7,
-                    "velocityX": -3.6676140803660213,
-                    "velocityY": -0.9269553626876206,
-                    "timestamp": 0.62218471040974
-                },
-                {
-                    "x": 5.675627085616213,
-                    "y": 6.961900764622196,
-                    "heading": 2.809826777811619,
-                    "angularVelocity": -2.5869501157187357e-7,
-                    "velocityX": -3.6676125380511024,
-                    "velocityY": -0.9269614650269816,
-                    "timestamp": 0.7258821621446967
-                },
-                {
-                    "x": 5.306947068096014,
-                    "y": 6.827887110574328,
-                    "heading": 2.8098267501768928,
-                    "angularVelocity": -2.6649378350721345e-7,
-                    "velocityX": -3.555343080778155,
-                    "velocityY": -1.2923524330219798,
-                    "timestamp": 0.8295796138796534
-                },
-                {
-                    "x": 4.978327022444636,
-                    "y": 6.61365948285698,
-                    "heading": 2.809826716038805,
-                    "angularVelocity": -3.2920855048251957e-7,
-                    "velocityX": -3.1690272051362296,
-                    "velocityY": -2.065890956172196,
-                    "timestamp": 0.9332770656146101
-                },
-                {
-                    "x": 4.706934591498499,
-                    "y": 6.330409136170975,
-                    "heading": 2.809826716485968,
-                    "angularVelocity": 4.312190502568527e-9,
-                    "velocityX": -2.617156221348633,
-                    "velocityY": -2.731507302705652,
-                    "timestamp": 1.0369745173495668
-                },
-                {
-                    "x": 4.519067005656564,
-                    "y": 6.028361651029434,
-                    "heading": 2.9086869877879162,
-                    "angularVelocity": 0.9533529479069416,
-                    "velocityX": -1.8116895130858792,
-                    "velocityY": -2.9127763516651948,
-                    "timestamp": 1.1406719690845235
-                },
-                {
-                    "x": 4.388194794926867,
-                    "y": 5.795490351049463,
-                    "heading": 3.0157821623071808,
-                    "angularVelocity": 1.0327657307624416,
-                    "velocityX": -1.2620581175340149,
-                    "velocityY": -2.245680063336305,
-                    "timestamp": 1.2443694208194802
-                },
-                {
-                    "x": 4.304089821797064,
-                    "y": 5.638764448430372,
-                    "heading": 3.096687373599028,
-                    "angularVelocity": 0.7802044306606039,
-                    "velocityX": -0.8110611372086162,
-                    "velocityY": -1.5113766056631632,
-                    "timestamp": 1.348066872554437
+                    "x": 7.593939909156862,
+                    "y": 7.445359818001645,
+                    "heading": -0.04618113523184224,
+                    "angularVelocity": -0.44533187598567886,
+                    "velocityX": -0.829890852839506,
+                    "velocityY": -0.20867964370321365,
+                    "timestamp": 0.10370049332232066
+                },
+                {
+                    "x": 7.42157070597782,
+                    "y": 7.402025181316033,
+                    "heading": -0.13159429090032063,
+                    "angularVelocity": -0.8236523562429084,
+                    "velocityX": -1.6621830586985462,
+                    "velocityY": -0.41788264739409464,
+                    "timestamp": 0.20740098664464132
+                },
+                {
+                    "x": 7.162505106155824,
+                    "y": 7.336932536575702,
+                    "heading": -0.24235044061142916,
+                    "angularVelocity": -1.0680387928995545,
+                    "velocityX": -2.4982099074184148,
+                    "velocityY": -0.6276985061006933,
+                    "timestamp": 0.31110147996696197
+                },
+                {
+                    "x": 6.816624706468089,
+                    "y": 7.25023883508299,
+                    "heading": -0.3315040304347048,
+                    "angularVelocity": -0.8597219450650766,
+                    "velocityX": -3.3353785368472084,
+                    "velocityY": -0.8360008589665135,
+                    "timestamp": 0.41480197328928264
+                },
+                {
+                    "x": 6.436297343977517,
+                    "y": 7.154089263427661,
+                    "heading": -0.33150405743314826,
+                    "angularVelocity": -2.603501934813404e-7,
+                    "velocityX": -3.6675559614595192,
+                    "velocityY": -0.9271852869251594,
+                    "timestamp": 0.5185024666116033
+                },
+                {
+                    "x": 6.0559700009320565,
+                    "y": 7.057939614855378,
+                    "heading": -0.3315040844315366,
+                    "angularVelocity": -2.603496620899773e-7,
+                    "velocityX": -3.6675557739472766,
+                    "velocityY": -0.9271860286473393,
+                    "timestamp": 0.6222029599339239
+                },
+                {
+                    "x": 5.675642815380094,
+                    "y": 6.961789343307514,
+                    "heading": -0.3315041114299189,
+                    "angularVelocity": -2.603496031733668e-7,
+                    "velocityX": -3.667554255212956,
+                    "velocityY": -0.927192036097853,
+                    "timestamp": 0.7259034532562446
+                },
+                {
+                    "x": 5.306931757393967,
+                    "y": 6.827827416139461,
+                    "heading": -0.33150413923830896,
+                    "angularVelocity": -2.681606334045615e-7,
+                    "velocityX": -3.5555381288313455,
+                    "velocityY": -1.2918157173241618,
+                    "timestamp": 0.8296039465785653
+                },
+                {
+                    "x": 4.978271181730368,
+                    "y": 6.6136408985726165,
+                    "heading": -0.3315041735834931,
+                    "angularVelocity": -3.311959575473401e-7,
+                    "velocityX": -3.1693250931994936,
+                    "velocityY": -2.0654339309757477,
+                    "timestamp": 0.933304439900886
+                },
+                {
+                    "x": 4.706831855999529,
+                    "y": 6.330419553539566,
+                    "heading": -0.3315041734698381,
+                    "angularVelocity": 1.0959926731432931e-9,
+                    "velocityX": -2.617531672555858,
+                    "velocityY": -2.7311475187754666,
+                    "timestamp": 1.0370049332232065
+                },
+                {
+                    "x": 4.5189225775062924,
+                    "y": 6.028421356099611,
+                    "heading": -0.23258180854056226,
+                    "angularVelocity": 0.9539237641021904,
+                    "velocityX": -1.8120384240525347,
+                    "velocityY": -2.912215629498277,
+                    "timestamp": 1.1407054265455272
+                },
+                {
+                    "x": 4.388098272679404,
+                    "y": 5.795527872027247,
+                    "heading": -0.12496959809450543,
+                    "angularVelocity": 1.0377212971549954,
+                    "velocityX": -1.2615591366596612,
+                    "velocityY": -2.2458281210725595,
+                    "timestamp": 1.2444059198678479
+                },
+                {
+                    "x": 4.304052104796344,
+                    "y": 5.638778152214578,
+                    "heading": -0.04358897284576924,
+                    "angularVelocity": 0.7847660376675182,
+                    "velocityX": -0.8104702802311671,
+                    "velocityY": -1.5115619491362144,
+                    "timestamp": 1.3481064131901686
                 },
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0.41768264963403856,
-                    "velocityX": -0.39624717010474564,
-                    "velocityY": -0.7595601156303902,
-                    "timestamp": 1.4517643242893936
+                    "heading": 0,
+                    "angularVelocity": 0.42033525057857213,
+                    "velocityX": -0.3958718370677089,
+                    "velocityY": -0.7596699850764065,
+                    "timestamp": 1.4518069065124892
                 },
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 1.3748988870988578e-26,
-                    "velocityX": 7.122460517165917e-28,
-                    "velocityY": 4.07656513453054e-28,
-                    "timestamp": 1.5554617760243503
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": -5.117426772684828e-28,
+                    "velocityY": 3.5837289853658606e-29,
+                    "timestamp": 1.55550739983481
                 }
             ],
             "constraints": [
@@ -26955,16 +26668,16 @@
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 23
                 },
                 {
                     "x": 4.263,
                     "y": 3,
-                    "heading": 2.574,
+                    "heading": -0.566,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -27012,16 +26725,16 @@
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 25
                 },
                 {
                     "x": 5.176,
                     "y": 1.62,
-                    "heading": 2.5,
+                    "heading": -0.64,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -27069,7 +26782,7 @@
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -27078,7 +26791,7 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -27126,7 +26839,7 @@
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -27135,7 +26848,7 @@
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -27183,7 +26896,7 @@
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -27192,186 +26905,14 @@
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 3.14,
-                    "angularVelocity": 1.0688205894738666e-32,
-                    "velocityX": 4.722903296583099e-32,
-                    "velocityY": -7.052966104933725e-38,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.680000100019751,
-                    "y": 7.385155846048263,
-                    "heading": 3.1400000209558985,
-                    "angularVelocity": 2.1151775994912985e-7,
-                    "velocityX": 0.0000010095466114596713,
-                    "velocityY": -0.8260917454272356,
-                    "timestamp": 0.09907392781101554
-                },
-                {
-                    "x": 7.680000295060445,
-                    "y": 7.221467541258139,
-                    "heading": 3.1400000632436256,
-                    "angularVelocity": 4.2682995289468236e-7,
-                    "velocityX": 0.000001968637858711507,
-                    "velocityY": -1.6521834594299558,
-                    "timestamp": 0.19814785562203108
-                },
-                {
-                    "x": 7.680000579860811,
-                    "y": 6.975935091856332,
-                    "heading": 3.140000126701719,
-                    "angularVelocity": 6.405123969521912e-7,
-                    "velocityX": 0.0000028746245916429767,
-                    "velocityY": -2.4782751105836276,
-                    "timestamp": 0.2972217834330466
-                },
-                {
-                    "x": 7.680000948789776,
-                    "y": 6.648558516522933,
-                    "heading": 3.140000210348385,
-                    "angularVelocity": 8.442850564566377e-7,
-                    "velocityX": 0.0000037237742032736077,
-                    "velocityY": -3.3043665731903222,
-                    "timestamp": 0.39629571124406215
-                },
-                {
-                    "x": 7.68000127512063,
-                    "y": 6.273767735073521,
-                    "heading": 3.1400002103483864,
-                    "angularVelocity": 1.8797601612350415e-14,
-                    "velocityX": 0.0000032938116008237123,
-                    "velocityY": -3.782940575085686,
-                    "timestamp": 0.4953696390550777
-                },
-                {
-                    "x": 7.680001534050369,
-                    "y": 5.898976953624012,
-                    "heading": 3.140000210348386,
-                    "angularVelocity": -4.168435621307512e-16,
-                    "velocityX": 0.0000026135000838056266,
-                    "velocityY": -3.782940575086706,
-                    "timestamp": 0.5944435668660932
-                },
-                {
-                    "x": 7.680001718421092,
-                    "y": 5.524186172174471,
-                    "heading": 3.14000021034838,
-                    "angularVelocity": -6.61889670232267e-14,
-                    "velocityX": 0.000001860940720346917,
-                    "velocityY": -3.782940575087075,
-                    "timestamp": 0.6935174946771088
-                },
-                {
-                    "x": 7.680001822997413,
-                    "y": 5.14939539072491,
-                    "heading": 3.1400002103483726,
-                    "angularVelocity": -8.462091393548897e-14,
-                    "velocityX": 0.000001055538133316693,
-                    "velocityY": -3.7829405750872986,
-                    "timestamp": 0.7925914224881243
-                },
-                {
-                    "x": 7.6800018440805555,
-                    "y": 4.774604609275343,
-                    "heading": 3.140000210348366,
-                    "angularVelocity": -6.932568582141466e-14,
-                    "velocityX": 2.128020680091996e-7,
-                    "velocityY": -3.782940575087379,
-                    "timestamp": 0.8916653502991398
-                },
-                {
-                    "x": 7.680001779518267,
-                    "y": 4.39981382782578,
-                    "heading": 3.1400002103483597,
-                    "angularVelocity": -6.86156871514602e-14,
-                    "velocityX": -6.516576918751784e-7,
-                    "velocityY": -3.782940575087321,
-                    "timestamp": 0.9907392781101554
-                },
-                {
-                    "x": 7.680001628799847,
-                    "y": 4.025023046376237,
-                    "heading": 3.140000210348353,
-                    "angularVelocity": -7.135901447410535e-14,
-                    "velocityX": -0.0000015212721994966025,
-                    "velocityY": -3.782940575087123,
-                    "timestamp": 1.089813205921171
-                },
-                {
-                    "x": 7.680001391869683,
-                    "y": 3.6502322649267236,
-                    "heading": 3.140000210348346,
-                    "angularVelocity": -7.415296718915011e-14,
-                    "velocityX": -0.0000023914480342122818,
-                    "velocityY": -3.7829405750867786,
-                    "timestamp": 1.1888871337321865
-                },
-                {
-                    "x": 7.680001068770943,
-                    "y": 3.27544148347729,
-                    "heading": 3.1400002103483433,
-                    "angularVelocity": -3.3132922253880277e-14,
-                    "velocityX": -0.0000032611884784431762,
-                    "velocityY": -3.782940575085905,
-                    "timestamp": 1.287961061543202
-                },
-                {
-                    "x": 7.680000658927351,
-                    "y": 2.9480649081437575,
-                    "heading": 3.140000122293031,
-                    "angularVelocity": -8.887836013747625e-7,
-                    "velocityX": -0.000004136744686858471,
-                    "velocityY": -3.3043665731916545,
-                    "timestamp": 1.3870349893542175
-                },
-                {
-                    "x": 7.680000338206625,
-                    "y": 2.7025324587418944,
-                    "heading": 3.140000057972934,
-                    "angularVelocity": -6.492129951120484e-7,
-                    "velocityX": -0.0000032371856464875894,
-                    "velocityY": -2.4782751105841743,
-                    "timestamp": 1.486108917165233
-                },
-                {
-                    "x": 7.680000115622722,
-                    "y": 2.5388441539517457,
-                    "heading": 3.140000017927834,
-                    "angularVelocity": -4.0419405265238827e-7,
-                    "velocityX": -0.000002246644408273398,
-                    "velocityY": -1.6521834594301972,
-                    "timestamp": 1.5851828449762486
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": -1.8095406608446286e-7,
-                    "velocityX": -0.0000011670347327659162,
-                    "velocityY": -0.8260917454273138,
-                    "timestamp": 1.6842567727872642
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": -2.451701531139976e-31,
-                    "velocityX": 0,
-                    "velocityY": -1.4987552972984166e-37,
-                    "timestamp": 1.7833307005982797
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -27412,7 +26953,7 @@
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -27421,231 +26962,14 @@
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 3.14,
-                    "angularVelocity": 1.8987898957165807e-31,
-                    "velocityX": -1.39380671571913e-32,
-                    "velocityY": -1.373126916943459e-36,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.680000133861116,
-                    "y": 7.389044729512878,
-                    "heading": 3.1400000003198203,
-                    "angularVelocity": 3.307649058744193e-9,
-                    "velocityX": 0.000001384414398977881,
-                    "velocityY": -0.8062267267090539,
-                    "timestamp": 0.0966914986871212
-                },
-                {
-                    "x": 7.6800003968709785,
-                    "y": 7.233134191633238,
-                    "heading": 3.1400000013587985,
-                    "angularVelocity": 1.0745310107378493e-8,
-                    "velocityX": 0.0000027200926202754936,
-                    "velocityY": -1.6124534214132102,
-                    "timestamp": 0.1933829973742424
-                },
-                {
-                    "x": 7.680000784081611,
-                    "y": 6.9992683925502845,
-                    "heading": 3.1400000030373074,
-                    "angularVelocity": 1.735946006644181e-8,
-                    "velocityX": 0.0000040045979798072784,
-                    "velocityY": -2.4186800521075664,
-                    "timestamp": 0.2900744960613636
-                },
-                {
-                    "x": 7.680001290213695,
-                    "y": 6.687447350831601,
-                    "heading": 3.1400000045486194,
-                    "angularVelocity": 1.5630280874799268e-8,
-                    "velocityX": 0.000005234503549587981,
-                    "velocityY": -3.2249064907728675,
-                    "timestamp": 0.3867659947484848
-                },
-                {
-                    "x": 7.680001791921761,
-                    "y": 6.32166915785587,
-                    "heading": 3.140000004548618,
-                    "angularVelocity": -2.2141915496959812e-14,
-                    "velocityX": 0.0000051887503514693284,
-                    "velocityY": -3.782940568118399,
-                    "timestamp": 0.48345749343560596
-                },
-                {
-                    "x": 7.6800022292578305,
-                    "y": 5.955890964879955,
-                    "heading": 3.1400000045486127,
-                    "angularVelocity": -6.389600617895948e-14,
-                    "velocityX": 0.000004523003602297052,
-                    "velocityY": -3.782940568120494,
-                    "timestamp": 0.5801489921227272
-                },
-                {
-                    "x": 7.680002595183966,
-                    "y": 5.590112771903998,
-                    "heading": 3.140000004548604,
-                    "angularVelocity": -1.1343594252599792e-13,
-                    "velocityX": 0.0000037844699136287606,
-                    "velocityY": -3.782940568121032,
-                    "timestamp": 0.6768404908098484
-                },
-                {
-                    "x": 7.680002884490285,
-                    "y": 5.224334578928017,
-                    "heading": 3.140000004548593,
-                    "angularVelocity": -1.3664646403255707e-13,
-                    "velocityX": 0.0000029920547840648683,
-                    "velocityY": -3.7829405681213992,
-                    "timestamp": 0.7735319894969697
-                },
-                {
-                    "x": 7.680003093405051,
-                    "y": 4.858556385952021,
-                    "heading": 3.140000004548582,
-                    "angularVelocity": -1.389367975129708e-13,
-                    "velocityX": 0.0000021606317619867176,
-                    "velocityY": -3.782940568121637,
-                    "timestamp": 0.8702234881840909
-                },
-                {
-                    "x": 7.6800032195393335,
-                    "y": 4.492778192976016,
-                    "heading": 3.1400000045485705,
-                    "angularVelocity": -1.422935768803712e-13,
-                    "velocityX": 0.0000013045020379807613,
-                    "velocityY": -3.782940568121783,
-                    "timestamp": 0.9669149868712121
-                },
-                {
-                    "x": 7.680003262132589,
-                    "y": 4.127000000000006,
-                    "heading": 3.1400000045485568,
-                    "angularVelocity": -1.5798635695024144e-13,
-                    "velocityX": 4.405066330821885e-7,
-                    "velocityY": -3.782940568121863,
-                    "timestamp": 1.0636064855583334
-                },
-                {
-                    "x": 7.680003221112365,
-                    "y": 3.7612218070239956,
-                    "heading": 3.1400000045485426,
-                    "angularVelocity": -1.6204973501388964e-13,
-                    "velocityX": -4.2423808567477835e-7,
-                    "velocityY": -3.782940568121861,
-                    "timestamp": 1.1602979842454546
-                },
-                {
-                    "x": 7.680003096442586,
-                    "y": 3.3954436140479913,
-                    "heading": 3.14000000454853,
-                    "angularVelocity": -1.5384845387063427e-13,
-                    "velocityX": -0.0000012893559013190512,
-                    "velocityY": -3.782940568121779,
-                    "timestamp": 1.2569894829325758
-                },
-                {
-                    "x": 7.680002888734547,
-                    "y": 3.029665421071995,
-                    "heading": 3.140000004548518,
-                    "angularVelocity": -1.4981778939731422e-13,
-                    "velocityX": -0.0000021481515920122195,
-                    "velocityY": -3.7829405681216324,
-                    "timestamp": 1.353680981619697
-                },
-                {
-                    "x": 7.680002600247316,
-                    "y": 2.663887228096014,
-                    "heading": 3.1400000045485066,
-                    "angularVelocity": -1.4657528609032921e-13,
-                    "velocityX": -0.0000029835836472893606,
-                    "velocityY": -3.782940568121397,
-                    "timestamp": 1.4503724803068183
-                },
-                {
-                    "x": 7.680002234659543,
-                    "y": 2.2981090351200577,
-                    "heading": 3.140000004548497,
-                    "angularVelocity": -1.235449081893301e-13,
-                    "velocityX": -0.000003780970508619144,
-                    "velocityY": -3.782940568121035,
-                    "timestamp": 1.5470639789939395
-                },
-                {
-                    "x": 7.680001797111122,
-                    "y": 1.9323308421441414,
-                    "heading": 3.1400000045484906,
-                    "angularVelocity": -7.160301715418214e-14,
-                    "velocityX": -0.0000045251997748623165,
-                    "velocityY": -3.7829405681205035,
-                    "timestamp": 1.6437554776810608
-                },
-                {
-                    "x": 7.6800012945835014,
-                    "y": 1.566552649168411,
-                    "heading": 3.140000004548489,
-                    "angularVelocity": -2.2663051081772477e-14,
-                    "velocityX": -0.000005197226332013039,
-                    "velocityY": -3.7829405681183954,
-                    "timestamp": 1.740446976368182
-                },
-                {
-                    "x": 7.680000786831981,
-                    "y": 1.2547316074497201,
-                    "heading": 3.140000001978014,
-                    "angularVelocity": -2.6584233523758634e-8,
-                    "velocityX": -0.0000052512520234957685,
-                    "velocityY": -3.2249064907729323,
-                    "timestamp": 1.8371384750553033
-                },
-                {
-                    "x": 7.680000398309784,
-                    "y": 1.0208658083667639,
-                    "heading": 3.140000000933386,
-                    "angularVelocity": -1.0803673621572709e-8,
-                    "velocityX": -0.000004018162386759126,
-                    "velocityY": -2.4186800521075957,
-                    "timestamp": 1.9338299737424245
-                },
-                {
-                    "x": 7.680000134361733,
-                    "y": 0.8649552704871222,
-                    "heading": 3.1400000003740773,
-                    "angularVelocity": -5.784435922761806e-9,
-                    "velocityX": -0.0000027297955176854786,
-                    "velocityY": -1.612453421413225,
-                    "timestamp": 2.0305214724295455
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 3.14,
-                    "angularVelocity": -3.8687549359618056e-9,
-                    "velocityX": -0.000001389591862211489,
-                    "velocityY": -0.8062267267090597,
-                    "timestamp": 2.1272129711166667
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 3.14,
-                    "angularVelocity": 5.6019143334006085e-31,
-                    "velocityX": 0,
-                    "velocityY": 9.268974451742519e-38,
-                    "timestamp": 2.223904469803788
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -27686,7 +27010,7 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -27695,7 +27019,7 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": 3.5331853071795862,
+                    "heading": 0.393,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -27706,200 +27030,200 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
-                    "angularVelocity": -3.1814997382550796e-29,
-                    "velocityX": -1.8229862739599686e-32,
-                    "velocityY": -3.244948758863147e-31,
+                    "heading": 0,
+                    "angularVelocity": 5.4178362746118835e-30,
+                    "velocityX": -4.378571579060787e-32,
+                    "velocityY": 1.8329583672175558e-31,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.617836558696546,
-                    "y": 5.830157448001668,
-                    "heading": 3.1302887872729133,
-                    "angularVelocity": -0.10560741421513488,
-                    "velocityX": -0.676014466912156,
-                    "velocityY": 0.36058033572491816,
-                    "timestamp": 0.0919557854839981
-                },
-                {
-                    "x": 7.493509680194616,
-                    "y": 5.896472329123028,
-                    "heading": 3.1108653372490442,
-                    "angularVelocity": -0.21122597041214858,
-                    "velocityX": -1.3520288891835412,
-                    "velocityY": 0.7211605096114337,
-                    "timestamp": 0.1839115709679962
-                },
-                {
-                    "x": 7.307019376427131,
-                    "y": 5.9959446237909395,
-                    "heading": 3.0817278950569205,
-                    "angularVelocity": -0.316863610470641,
-                    "velocityX": -2.028043181686891,
-                    "velocityY": 1.0817404706441378,
-                    "timestamp": 0.27586735645199434
-                },
-                {
-                    "x": 7.058365675689159,
-                    "y": 6.128574306111159,
-                    "heading": 3.0428740808433132,
-                    "angularVelocity": -0.4225271309369522,
-                    "velocityX": -2.704057166487276,
-                    "velocityY": 1.442320150082345,
-                    "timestamp": 0.3678231419359924
-                },
-                {
-                    "x": 6.75143523206277,
-                    "y": 6.292289169474125,
-                    "heading": 3.0428740759198862,
-                    "angularVelocity": -5.354123851149769e-8,
-                    "velocityX": -3.3378045982740363,
-                    "velocityY": 1.7803650145694723,
-                    "timestamp": 0.4597789274199905
-                },
-                {
-                    "x": 6.443010139077041,
-                    "y": 6.453170540992539,
-                    "heading": 3.042874070986951,
-                    "angularVelocity": -5.3644645461672824e-8,
-                    "velocityX": -3.354058598514181,
-                    "velocityY": 1.7495513813691466,
-                    "timestamp": 0.5517347129039886
-                },
-                {
-                    "x": 6.108481192244814,
-                    "y": 6.548560466077805,
-                    "heading": 3.0428740880877947,
-                    "angularVelocity": 1.8596810871163974e-7,
-                    "velocityX": -3.637932568043149,
-                    "velocityY": 1.037345552356419,
-                    "timestamp": 0.6436904983879866
-                },
-                {
-                    "x": 5.76231566928878,
-                    "y": 6.582886564044467,
-                    "heading": 3.0428741422485728,
-                    "angularVelocity": 5.889871735366024e-7,
-                    "velocityX": -3.764477907877501,
-                    "velocityY": 0.3732891605024142,
-                    "timestamp": 0.7356462838719847
-                },
-                {
-                    "x": 5.41614197674267,
-                    "y": 6.617130174737203,
-                    "heading": 3.042874196415433,
-                    "angularVelocity": 5.890533147506573e-7,
-                    "velocityX": -3.764566750466717,
-                    "velocityY": 0.37239212859200393,
-                    "timestamp": 0.8276020693559828
-                },
-                {
-                    "x": 5.068621146465333,
-                    "y": 6.632561444279093,
-                    "heading": 3.0428742397901987,
-                    "angularVelocity": 4.716915377702693e-7,
-                    "velocityX": -3.779216592498277,
-                    "velocityY": 0.16781183979527767,
-                    "timestamp": 0.9195578548399809
-                },
-                {
-                    "x": 4.721100288864093,
-                    "y": 6.647992098459583,
-                    "heading": 3.042874283164768,
-                    "angularVelocity": 4.716894023860314e-7,
-                    "velocityX": -3.779216889640001,
-                    "velocityY": 0.16780514786832676,
-                    "timestamp": 1.011513640323979
-                },
-                {
-                    "x": 4.3735794260416885,
-                    "y": 6.663422635051171,
-                    "heading": 3.042874326539347,
-                    "angularVelocity": 4.716895035423872e-7,
-                    "velocityX": -3.779216946419108,
-                    "velocityY": 0.16780386911352246,
-                    "timestamp": 1.1034694258079771
-                },
-                {
-                    "x": 4.026058559390409,
-                    "y": 6.678853085409858,
-                    "heading": 3.042874369913944,
-                    "angularVelocity": 4.716897058235115e-7,
-                    "velocityX": -3.779216988057318,
-                    "velocityY": 0.1678029313486884,
-                    "timestamp": 1.1954252112919752
-                },
-                {
-                    "x": 3.678537689463977,
-                    "y": 6.69428346200604,
-                    "heading": 3.042874413288561,
-                    "angularVelocity": 4.7168991769457604e-7,
-                    "velocityX": -3.779217023673906,
-                    "velocityY": 0.16780212919682533,
-                    "timestamp": 1.2873809967759733
-                },
-                {
-                    "x": 3.3310168167092282,
-                    "y": 6.7097137749028235,
-                    "heading": 3.042874456663209,
-                    "angularVelocity": 4.7169025361622227e-7,
-                    "velocityX": -3.7792170544312707,
-                    "velocityY": 0.16780143647915202,
-                    "timestamp": 1.3793367822599714
-                },
-                {
-                    "x": 2.9915174484895957,
-                    "y": 6.725380176084037,
-                    "heading": 3.069608362610464,
-                    "angularVelocity": 0.2907256547975106,
-                    "velocityX": -3.6919848646033437,
-                    "velocityY": 0.1703688473624042,
-                    "timestamp": 1.4712925677439694
-                },
-                {
-                    "x": 2.7138500007590802,
-                    "y": 6.7384126132729225,
-                    "heading": 3.2277640151767573,
-                    "angularVelocity": 1.7199097559099776,
-                    "velocityX": -3.019575617445345,
-                    "velocityY": 0.14172503796570737,
-                    "timestamp": 1.5632483532279675
-                },
-                {
-                    "x": 2.5064920875678816,
-                    "y": 6.747862712816966,
-                    "heading": 3.3721576212980104,
-                    "angularVelocity": 1.570250369362352,
-                    "velocityX": -2.2549740845537376,
-                    "velocityY": 0.1027678627756077,
-                    "timestamp": 1.6552041387119656
-                },
-                {
-                    "x": 2.368714185355249,
-                    "y": 6.753998249997193,
-                    "heading": 3.477486882189963,
-                    "angularVelocity": 1.14543375751254,
-                    "velocityX": -1.4983059683243996,
-                    "velocityY": 0.06672268795197085,
-                    "timestamp": 1.7471599241959637
+                    "x": 7.617836111630202,
+                    "y": 5.830159453299192,
+                    "heading": -0.009839435811264896,
+                    "angularVelocity": -0.10699973937901981,
+                    "velocityX": -0.6760062245377694,
+                    "velocityY": 0.3605951529154081,
+                    "timestamp": 0.09195756801248979
+                },
+                {
+                    "x": 7.493508339255607,
+                    "y": 5.896478344435856,
+                    "heading": -0.0295193862966414,
+                    "angularVelocity": -0.21401121093919698,
+                    "velocityX": -1.352012401607965,
+                    "velocityY": 0.721190137691066,
+                    "timestamp": 0.18391513602497958
+                },
+                {
+                    "x": 7.3070166954300815,
+                    "y": 5.995956653040542,
+                    "heading": -0.059041701653073396,
+                    "angularVelocity": -0.32104280261546536,
+                    "velocityX": -2.0280184421601457,
+                    "velocityY": 1.0817849009575307,
+                    "timestamp": 0.2758727040374694
+                },
+                {
+                    "x": 7.0583612095128645,
+                    "y": 6.128594352415652,
+                    "heading": -0.09840889768284368,
+                    "angularVelocity": -0.42810175258683864,
+                    "velocityX": -2.7040241634429143,
+                    "velocityY": 1.4423793738987822,
+                    "timestamp": 0.36783027204995916
+                },
+                {
+                    "x": 6.751428460841339,
+                    "y": 6.292319222162274,
+                    "heading": -0.09840890267081659,
+                    "angularVelocity": -5.4242114104221296e-8,
+                    "velocityX": -3.3377649638345908,
+                    "velocityY": 1.780439318756071,
+                    "timestamp": 0.45978784006244894
+                },
+                {
+                    "x": 6.443018724200086,
+                    "y": 6.45324460704626,
+                    "heading": -0.09840890766830068,
+                    "angularVelocity": -5.4345544379257814e-8,
+                    "velocityX": -3.3538265887954406,
+                    "velocityY": 1.7499960945262094,
+                    "timestamp": 0.5517454080749388
+                },
+                {
+                    "x": 6.108495580653603,
+                    "y": 6.54867946433224,
+                    "heading": -0.0984088906849307,
+                    "angularVelocity": 1.846870285822299e-7,
+                    "velocityX": -3.637798941149115,
+                    "velocityY": 1.037814063036315,
+                    "timestamp": 0.6437029760874285
+                },
+                {
+                    "x": 5.762312734581719,
+                    "y": 6.58289903236131,
+                    "heading": -0.09840883650367563,
+                    "angularVelocity": 5.891984340231216e-7,
+                    "velocityX": -3.7645933179188145,
+                    "velocityY": 0.37212345616210646,
+                    "timestamp": 0.7356605440999183
+                },
+                {
+                    "x": 5.416121743024081,
+                    "y": 6.617036096003367,
+                    "heading": -0.09840878231628362,
+                    "angularVelocity": 5.892651705038846e-7,
+                    "velocityX": -3.7646818966614886,
+                    "velocityY": 0.37122625554234295,
+                    "timestamp": 0.8276181121124081
+                },
+                {
+                    "x": 5.0685946034380285,
+                    "y": 6.632477283287523,
+                    "heading": -0.09840873884834453,
+                    "angularVelocity": 4.7269561430407727e-7,
+                    "velocityX": -3.7792119463060407,
+                    "velocityY": 0.16791643817786392,
+                    "timestamp": 0.9195756801248979
+                },
+                {
+                    "x": 4.72106743655186,
+                    "y": 6.647917856129691,
+                    "heading": -0.09840869538060193,
+                    "angularVelocity": 4.726934774100163e-7,
+                    "velocityX": -3.779212243183364,
+                    "velocityY": 0.1679097563788475,
+                    "timestamp": 1.0115332481373878
+                },
+                {
+                    "x": 4.373540264440576,
+                    "y": 6.66335831136751,
+                    "heading": -0.09840865191285005,
+                    "angularVelocity": 4.7269357834441634e-7,
+                    "velocityX": -3.779212300004299,
+                    "velocityY": 0.16790847748084475,
+                    "timestamp": 1.1034908161498775
+                },
+                {
+                    "x": 4.026013088497698,
+                    "y": 6.678798680365008,
+                    "heading": -0.09840860844507962,
+                    "angularVelocity": 4.726937801388588e-7,
+                    "velocityX": -3.7792123416712937,
+                    "velocityY": 0.16790753965352348,
+                    "timestamp": 1.1954483841623673
+                },
+                {
+                    "x": 3.6784859092773967,
+                    "y": 6.694238975594936,
+                    "heading": -0.09840856497728971,
+                    "angularVelocity": 4.7269399197494456e-7,
+                    "velocityX": -3.7792123773118855,
+                    "velocityY": 0.1679067374621128,
+                    "timestamp": 1.287405952174857
+                },
+                {
+                    "x": 3.3309587272270753,
+                    "y": 6.709679207126903,
+                    "heading": -0.09840852150946863,
+                    "angularVelocity": 4.72694330698846e-7,
+                    "velocityX": -3.779212408087172,
+                    "velocityY": 0.16790604477348844,
+                    "timestamp": 1.3793635201873469
+                },
+                {
+                    "x": 2.991492172408929,
+                    "y": 6.7253546024137885,
+                    "heading": -0.07153351873489558,
+                    "angularVelocity": 0.29225438814261445,
+                    "velocityX": -3.691556466261044,
+                    "velocityY": 0.17046335201859075,
+                    "timestamp": 1.4713210881998366
+                },
+                {
+                    "x": 2.7138316831246163,
+                    "y": 6.738400020288083,
+                    "heading": 0.08699256852088375,
+                    "angularVelocity": 1.723904738696962,
+                    "velocityX": -3.019441415051349,
+                    "velocityY": 0.1418634502439354,
+                    "timestamp": 1.5632786562123264
+                },
+                {
+                    "x": 2.5064819973568118,
+                    "y": 6.747857404358727,
+                    "heading": 0.23167973935786532,
+                    "angularVelocity": 1.5734123244465315,
+                    "velocityX": -2.254840903792083,
+                    "velocityY": 0.10284508687050122,
+                    "timestamp": 1.6552362242248162
+                },
+                {
+                    "x": 2.36871059980762,
+                    "y": 6.753996751301636,
+                    "heading": 0.3372042620417382,
+                    "angularVelocity": 1.1475349442640808,
+                    "velocityX": -1.4982061892989513,
+                    "velocityY": 0.0667628241546722,
+                    "timestamp": 1.747193792237306
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": 3.5331853071795862,
-                    "angularVelocity": 0.6057087620584267,
-                    "velocityX": -0.7472524430472809,
-                    "velocityY": 0.03264340559983963,
-                    "timestamp": 1.8391157096799617
+                    "heading": 0.393,
+                    "angularVelocity": 0.6067552585849546,
+                    "velocityX": -0.7471989667918153,
+                    "velocityY": 0.03265907051778923,
+                    "timestamp": 1.8391513602497958
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": 3.5331853071795862,
-                    "angularVelocity": -1.2870903265044466e-29,
+                    "heading": 0.393,
+                    "angularVelocity": -8.637511489261282e-30,
                     "velocityX": 0,
-                    "velocityY": 1.2655145174510225e-29,
-                    "timestamp": 1.9310714951639598
+                    "velocityY": 5.8539743527830045e-30,
+                    "timestamp": 1.9311089282622855
                 }
             ],
             "constraints": [
@@ -27942,7 +27266,7 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -27951,195 +27275,14 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 3.14,
-                    "angularVelocity": -1.4170612284410768e-31,
-                    "velocityX": -1.067311144931875e-32,
-                    "velocityY": -8.392488596494602e-33,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.598760740640866,
-                    "y": 5.839328600808308,
-                    "heading": 3.1292747695902707,
-                    "angularVelocity": -0.10229725511121834,
-                    "velocityX": -0.7748601122982537,
-                    "velocityY": 0.40373022396426267,
-                    "timestamp": 0.10484377511467942
-                },
-                {
-                    "x": 7.436282224463055,
-                    "y": 5.923985787899839,
-                    "heading": 3.1078233746588477,
-                    "angularVelocity": -0.2046034197830007,
-                    "velocityX": -1.5497202003656476,
-                    "velocityY": 0.8074603093882498,
-                    "timestamp": 0.20968755022935884
-                },
-                {
-                    "x": 7.19256446073334,
-                    "y": 6.050971542878038,
-                    "heading": 3.0756442181443964,
-                    "angularVelocity": -0.30692481722690457,
-                    "velocityX": -2.3245802000465203,
-                    "velocityY": 1.2111902193458808,
-                    "timestamp": 0.31453132534403827
-                },
-                {
-                    "x": 6.8676074741032105,
-                    "y": 6.220285840922999,
-                    "heading": 3.032735095907097,
-                    "angularVelocity": -0.4092672377579469,
-                    "velocityX": -3.099439964601499,
-                    "velocityY": 1.614919892571246,
-                    "timestamp": 0.4193751004587177
-                },
-                {
-                    "x": 6.515870869646968,
-                    "y": 6.403553456428483,
-                    "heading": 3.032735091775592,
-                    "angularVelocity": -3.94062953033438e-8,
-                    "velocityX": -3.3548639780617218,
-                    "velocityY": 1.7480066442192022,
-                    "timestamp": 0.5242188755733971
-                },
-                {
-                    "x": 6.1437635937584405,
-                    "y": 6.5408190150249235,
-                    "heading": 3.032735095078602,
-                    "angularVelocity": 3.1504111682057e-8,
-                    "velocityX": -3.5491594563579234,
-                    "velocityY": 1.3092389934099258,
-                    "timestamp": 0.6290626506880765
-                },
-                {
-                    "x": 5.750083988314003,
-                    "y": 6.589006339833596,
-                    "heading": 3.032735167102088,
-                    "angularVelocity": 6.86960058704524e-7,
-                    "velocityX": -3.754916350673425,
-                    "velocityY": 0.45961073755656895,
-                    "timestamp": 0.733906425802756
-                },
-                {
-                    "x": 5.355289339691467,
-                    "y": 6.626991015951962,
-                    "heading": 3.032735238530673,
-                    "angularVelocity": 6.812858975549085e-7,
-                    "velocityX": -3.7655516332820365,
-                    "velocityY": 0.36229786724885477,
-                    "timestamp": 0.8387502009174355
-                },
-                {
-                    "x": 4.960619519356147,
-                    "y": 6.587730487852754,
-                    "heading": 3.032735268670057,
-                    "angularVelocity": 2.874694691913672e-7,
-                    "velocityX": -3.76436102099172,
-                    "velocityY": -0.3744669443299256,
-                    "timestamp": 0.9435939760321149
-                },
-                {
-                    "x": 4.585499557692294,
-                    "y": 6.45892571115183,
-                    "heading": 3.032735273052689,
-                    "angularVelocity": 4.180154788476782e-8,
-                    "velocityX": -3.577894455379368,
-                    "velocityY": -1.2285400498030161,
-                    "timestamp": 1.0484377511467944
-                },
-                {
-                    "x": 4.215352940634833,
-                    "y": 6.316457686051506,
-                    "heading": 3.0327352773918506,
-                    "angularVelocity": 4.1386927047063915e-8,
-                    "velocityX": -3.530458691062866,
-                    "velocityY": -1.358860122544141,
-                    "timestamp": 1.1532815262614737
-                },
-                {
-                    "x": 3.8452063807699624,
-                    "y": 6.173989512359033,
-                    "heading": 3.0327352817310285,
-                    "angularVelocity": 4.138708047875789e-8,
-                    "velocityX": -3.530458145559901,
-                    "velocityY": -1.3588615398161574,
-                    "timestamp": 1.258125301376153
-                },
-                {
-                    "x": 3.47505985812915,
-                    "y": 6.031521241954782,
-                    "heading": 3.0327352860702175,
-                    "angularVelocity": 4.138718836240866e-8,
-                    "velocityX": -3.530457790516812,
-                    "velocityY": -1.3588624622531746,
-                    "timestamp": 1.3629690764908324
-                },
-                {
-                    "x": 3.1049133713898427,
-                    "y": 5.889052878274973,
-                    "heading": 3.0327352904094194,
-                    "angularVelocity": 4.138730707919649e-8,
-                    "velocityX": -3.53045744808823,
-                    "velocityY": -1.3588633519155102,
-                    "timestamp": 1.4678128516055118
-                },
-                {
-                    "x": 2.762948024328836,
-                    "y": 5.75743175203549,
-                    "heading": 3.07563992564996,
-                    "angularVelocity": 0.4092244407797366,
-                    "velocityX": -3.2616657182265767,
-                    "velocityY": -1.255402393661561,
-                    "timestamp": 1.5726566267201911
-                },
-                {
-                    "x": 2.5064740111386725,
-                    "y": 5.658715887693948,
-                    "heading": 3.107819702204835,
-                    "angularVelocity": 0.30693073117289776,
-                    "velocityX": -2.4462493162768064,
-                    "velocityY": -0.9415519827816698,
-                    "timestamp": 1.6775004018348705
-                },
-                {
-                    "x": 2.3354913370033428,
-                    "y": 5.592905299768155,
-                    "heading": 3.129273261906622,
-                    "angularVelocity": 0.20462406736424307,
-                    "velocityX": -1.6308328648821255,
-                    "velocityY": -0.6277014334309162,
-                    "timestamp": 1.7823441769495498
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0.1023116353989093,
-                    "velocityX": -0.8154164318275585,
-                    "velocityY": -0.3138507720860239,
-                    "timestamp": 1.8871879520642292
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 6.542152862475765e-30,
-                    "velocityX": 0,
-                    "velocityY": -3.777031189999863e-33,
-                    "timestamp": 1.9920317271789085
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -28180,7 +27323,7 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -28189,222 +27332,14 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 3.14,
-                    "angularVelocity": 1.7201358648458989e-31,
-                    "velocityX": -4.9530751339705636e-33,
-                    "velocityY": 8.276147677005227e-33,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.605093588171019,
-                    "y": 5.8310278116994025,
-                    "heading": 3.10842986087817,
-                    "angularVelocity": -0.3169118675748513,
-                    "velocityX": -0.7519362133453025,
-                    "velocityY": 0.3415828265288231,
-                    "timestamp": 0.0996180400671603
-                },
-                {
-                    "x": 7.455282578249206,
-                    "y": 5.899083098798643,
-                    "heading": 3.0452020176570493,
-                    "angularVelocity": -0.6347027423797291,
-                    "velocityX": -1.5038542197860265,
-                    "velocityY": 0.6831622771674665,
-                    "timestamp": 0.1992360801343206
-                },
-                {
-                    "x": 7.230570445252572,
-                    "y": 6.001166640275022,
-                    "heading": 2.9501837123513917,
-                    "angularVelocity": -0.9538262873029698,
-                    "velocityX": -2.2557373428059657,
-                    "velocityY": 1.0247495474469857,
-                    "timestamp": 0.2988541202014809
-                },
-                {
-                    "x": 6.930961848120999,
-                    "y": 6.137281948011585,
-                    "heading": 2.82324695361152,
-                    "angularVelocity": -1.274234653224393,
-                    "velocityX": -3.007573697791919,
-                    "velocityY": 1.3663720712111798,
-                    "timestamp": 0.3984721602686412
-                },
-                {
-                    "x": 6.58788061707114,
-                    "y": 6.293200302520464,
-                    "heading": 2.8232469405560607,
-                    "angularVelocity": -1.3105517366390336e-7,
-                    "velocityX": -3.4439668840961075,
-                    "velocityY": 1.565161836187118,
-                    "timestamp": 0.4980902003358015
-                },
-                {
-                    "x": 6.242826706460482,
-                    "y": 6.444702986624836,
-                    "heading": 2.8232469291979005,
-                    "angularVelocity": -1.1401709907689636e-7,
-                    "velocityX": -3.46376931706376,
-                    "velocityY": 1.5208358245377296,
-                    "timestamp": 0.5977082404029618
-                },
-                {
-                    "x": 5.895869664946667,
-                    "y": 6.591795052657303,
-                    "heading": 2.823246919432399,
-                    "angularVelocity": -9.802945216969962e-8,
-                    "velocityX": -3.482873596789339,
-                    "velocityY": 1.4765605299331361,
-                    "timestamp": 0.6973262804701221
-                },
-                {
-                    "x": 5.529327027819563,
-                    "y": 6.679326525812769,
-                    "heading": 2.8232469090031302,
-                    "angularVelocity": -1.0469256638864389e-7,
-                    "velocityX": -3.679480512565681,
-                    "velocityY": 0.8786709023431315,
-                    "timestamp": 0.7969443205372824
-                },
-                {
-                    "x": 5.152516966350426,
-                    "y": 6.684752095540444,
-                    "heading": 2.823246873859642,
-                    "angularVelocity": -3.5278236840852576e-7,
-                    "velocityX": -3.7825484341500837,
-                    "velocityY": 0.05446372689140277,
-                    "timestamp": 0.8965623606044426
-                },
-                {
-                    "x": 4.783606202844696,
-                    "y": 6.607810054904123,
-                    "heading": 2.823246710781253,
-                    "angularVelocity": -0.0000016370367136474411,
-                    "velocityX": -3.703252576109883,
-                    "velocityY": -0.7723705524064612,
-                    "timestamp": 0.9961804006716029
-                },
-                {
-                    "x": 4.4403804832702125,
-                    "y": 6.452210088653612,
-                    "heading": 2.8232466000560583,
-                    "angularVelocity": -0.0000011114974222563995,
-                    "velocityX": -3.445417309385819,
-                    "velocityY": -1.5619657458188145,
-                    "timestamp": 1.095798440738763
-                },
-                {
-                    "x": 4.124524669473561,
-                    "y": 6.246661186403647,
-                    "heading": 2.823246495691381,
-                    "angularVelocity": -0.0000010476483740088647,
-                    "velocityX": -3.170668822471396,
-                    "velocityY": -2.0633702701979306,
-                    "timestamp": 1.1954164808059233
-                },
-                {
-                    "x": 3.808669145932633,
-                    "y": 6.041111838134177,
-                    "heading": 2.82324639132671,
-                    "angularVelocity": -0.0000010476483076103719,
-                    "velocityX": -3.1706659087850397,
-                    "velocityY": -2.063374747494463,
-                    "timestamp": 1.2950345208730836
-                },
-                {
-                    "x": 3.492813679718565,
-                    "y": 5.835562401773946,
-                    "heading": 2.8232462869620267,
-                    "angularVelocity": -0.0000010476484251301315,
-                    "velocityX": -3.1706653333184023,
-                    "velocityY": -2.063375631779686,
-                    "timestamp": 1.3946525609402438
-                },
-                {
-                    "x": 3.176958352678971,
-                    "y": 5.630012751552842,
-                    "heading": 2.8232461825973036,
-                    "angularVelocity": -0.000001047648830467493,
-                    "velocityX": -3.1706639362373616,
-                    "velocityY": -2.0633777785883685,
-                    "timestamp": 1.494270601007404
-                },
-                {
-                    "x": 2.880984417024366,
-                    "y": 5.396743220702011,
-                    "heading": 2.8232460713596375,
-                    "angularVelocity": -0.0000011166417889330504,
-                    "velocityX": -2.971087721210592,
-                    "velocityY": -2.3416394329136074,
-                    "timestamp": 1.5938886410745643
-                },
-                {
-                    "x": 2.6437210391842343,
-                    "y": 5.10516988637377,
-                    "heading": 2.820727987751454,
-                    "angularVelocity": -0.02527738556676537,
-                    "velocityX": -2.3817310366694042,
-                    "velocityY": -2.926912978127948,
-                    "timestamp": 1.6935066811417245
-                },
-                {
-                    "x": 2.4878211519832623,
-                    "y": 4.818636614610494,
-                    "heading": 2.7673750452255876,
-                    "angularVelocity": -0.5355751075798869,
-                    "velocityX": -1.5649764550262995,
-                    "velocityY": -2.876319104151233,
-                    "timestamp": 1.7931247212088848
-                },
-                {
-                    "x": 2.3880615291520813,
-                    "y": 4.591997027963517,
-                    "heading": 2.751203678024379,
-                    "angularVelocity": -0.1623337217867963,
-                    "velocityX": -1.0014212562696991,
-                    "velocityY": -2.275085782597012,
-                    "timestamp": 1.892742761276045
-                },
-                {
-                    "x": 2.3279705887355506,
-                    "y": 4.437873884538665,
-                    "heading": 2.748674861705227,
-                    "angularVelocity": -0.025385124194849635,
-                    "velocityX": -0.6032134378072334,
-                    "velocityY": -1.547140892562696,
-                    "timestamp": 1.9923608013432053
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": 0.013302191991328724,
-                    "velocityX": -0.28077834814551317,
-                    "velocityY": -0.7817247206044561,
-                    "timestamp": 2.0919788414103655
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": 2.75,
-                    "angularVelocity": -2.062592729534739e-30,
-                    "velocityX": 0,
-                    "velocityY": 2.2969932039584724e-33,
-                    "timestamp": 2.1915968814775257
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -28445,7 +27380,7 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -28454,7 +27389,7 @@
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": 3.4031853071795863,
+                    "heading": 0.263,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -28465,128 +27400,128 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
-                    "angularVelocity": 6.88462418020868e-33,
-                    "velocityX": -2.6678679284923953e-34,
-                    "velocityY": 0,
+                    "heading": -2.09363537243035e-32,
+                    "angularVelocity": -1.4612041776101527e-32,
+                    "velocityX": -1.1521781181914207e-34,
+                    "velocityY": -1.3541198100986985e-32,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.620477294727877,
-                    "y": 5.828094267433712,
-                    "heading": 3.125579827082933,
-                    "angularVelocity": -0.16053625399389732,
-                    "velocityX": -0.6626517023703261,
-                    "velocityY": 0.34616486523434487,
-                    "timestamp": 0.08982502430644557
-                },
-                {
-                    "x": 7.501400723607879,
-                    "y": 5.890296797165295,
-                    "heading": 3.0989291927764526,
-                    "angularVelocity": -0.29669498574873204,
-                    "velocityX": -1.3256503078001867,
-                    "velocityY": 0.6924855318644141,
-                    "timestamp": 0.17965004861289113
-                },
-                {
-                    "x": 7.322728500258778,
-                    "y": 5.983625073051111,
-                    "heading": 3.0637763178084882,
-                    "angularVelocity": -0.3913483490751664,
-                    "velocityX": -1.9891141107799197,
-                    "velocityY": 1.0390008419860823,
-                    "timestamp": 0.2694750729193367
-                },
-                {
-                    "x": 7.084419480032935,
-                    "y": 6.108090729851432,
-                    "heading": 3.027860333619382,
-                    "angularVelocity": -0.3998438571703182,
-                    "velocityX": -2.6530359670466983,
-                    "velocityY": 1.3856456790448122,
-                    "timestamp": 0.35930009722578227
-                },
-                {
-                    "x": 6.786779796078207,
-                    "y": 6.263490017806672,
-                    "heading": 3.0170090626873085,
-                    "angularVelocity": -0.1208045421179424,
-                    "velocityX": -3.3135497179416755,
-                    "velocityY": 1.7300222199225983,
-                    "timestamp": 0.44912512153222783
-                },
-                {
-                    "x": 6.48562142682328,
-                    "y": 6.4208733949571855,
-                    "heading": 3.0170090501527196,
-                    "angularVelocity": -1.3954451104022202e-7,
-                    "velocityX": -3.3527223797624646,
-                    "velocityY": 1.752110599086374,
-                    "timestamp": 0.5389501458386734
-                },
-                {
-                    "x": 6.164454252506572,
-                    "y": 6.531857797451653,
-                    "heading": 3.017009051216684,
-                    "angularVelocity": 1.1844854204542515e-8,
-                    "velocityX": -3.5754755069257693,
-                    "velocityY": 1.2355621760349855,
-                    "timestamp": 0.628775170145119
-                },
-                {
-                    "x": 5.838539791831514,
-                    "y": 6.576460283854247,
-                    "heading": 3.052457229239417,
-                    "angularVelocity": 0.3946358856725586,
-                    "velocityX": -3.6283258834773524,
-                    "velocityY": 0.4965485592347737,
-                    "timestamp": 0.7186001944515645
-                },
-                {
-                    "x": 5.573157198508507,
-                    "y": 6.601492716526358,
-                    "heading": 3.1680842973805725,
-                    "angularVelocity": 1.287247835822277,
-                    "velocityX": -2.9544394267863754,
-                    "velocityY": 0.2786799432050252,
-                    "timestamp": 0.8084252187580101
-                },
-                {
-                    "x": 5.374340211248987,
-                    "y": 6.616920258488131,
-                    "heading": 3.277639232341259,
-                    "angularVelocity": 1.2196482640175015,
-                    "velocityX": -2.2133808345126464,
-                    "velocityY": 0.17175104689246343,
-                    "timestamp": 0.8982502430644557
-                },
-                {
-                    "x": 5.242040057703165,
-                    "y": 6.625927417066914,
-                    "heading": 3.3593576896006887,
-                    "angularVelocity": 0.9097515741341804,
-                    "velocityX": -1.4728652128661812,
-                    "velocityY": 0.10027449085964446,
-                    "timestamp": 0.9880752673709012
+                    "x": 7.620476774011981,
+                    "y": 5.828095362884936,
+                    "heading": -0.014618875325545894,
+                    "angularVelocity": -0.16274311877436243,
+                    "velocityX": -0.6626361618854291,
+                    "velocityY": 0.346165913767109,
+                    "timestamp": 0.08982791675397632
+                },
+                {
+                    "x": 7.50139830253108,
+                    "y": 5.890300516939614,
+                    "heading": -0.04163561640032371,
+                    "angularVelocity": -0.3007610779705845,
+                    "velocityX": -1.3256287775996987,
+                    "velocityY": 0.692492448923733,
+                    "timestamp": 0.17965583350795264
+                },
+                {
+                    "x": 7.322721643819432,
+                    "y": 5.983633497452849,
+                    "heading": -0.07726722942321354,
+                    "angularVelocity": -0.3966652496292318,
+                    "velocityX": -1.9890994377729274,
+                    "velocityY": 1.0390197600692306,
+                    "timestamp": 0.26948375026192894
+                },
+                {
+                    "x": 7.084404587107726,
+                    "y": 6.108106334427675,
+                    "heading": -0.11365614278993857,
+                    "angularVelocity": -0.4050958174437935,
+                    "velocityX": -2.6530400049732523,
+                    "velocityY": 1.3856809939802615,
+                    "timestamp": 0.3593116670159053
+                },
+                {
+                    "x": 6.786764353262062,
+                    "y": 6.2635080892247865,
+                    "heading": -0.12453213530374094,
+                    "angularVelocity": -0.12107586268075081,
+                    "velocityX": -3.313449143664897,
+                    "velocityY": 1.7299939752886722,
+                    "timestamp": 0.4491395837698816
+                },
+                {
+                    "x": 6.485598483517562,
+                    "y": 6.4209007383980845,
+                    "heading": -0.12453214800780214,
+                    "angularVelocity": -1.4142664848458141e-7,
+                    "velocityX": -3.352697920951937,
+                    "velocityY": 1.752157401182633,
+                    "timestamp": 0.5389675005238579
+                },
+                {
+                    "x": 6.164419225797539,
+                    "y": 6.531883674906018,
+                    "heading": -0.12453214718798315,
+                    "angularVelocity": 9.126550019087707e-9,
+                    "velocityX": -3.575494894306401,
+                    "velocityY": 1.2355060711459842,
+                    "timestamp": 0.6287954172778342
+                },
+                {
+                    "x": 5.83853002564033,
+                    "y": 6.576482035264996,
+                    "heading": -0.08897109874736252,
+                    "angularVelocity": 0.3958796967096147,
+                    "velocityX": -3.6279278417395133,
+                    "velocityY": 0.4964866376799699,
+                    "timestamp": 0.7186233340318104
+                },
+                {
+                    "x": 5.573146790095341,
+                    "y": 6.601508859331819,
+                    "heading": 0.02708371250022136,
+                    "angularVelocity": 1.2919681925323805,
+                    "velocityX": -2.9543514436812472,
+                    "velocityY": 0.278608532527451,
+                    "timestamp": 0.8084512507857867
+                },
+                {
+                    "x": 5.3743330314019,
+                    "y": 6.61692927227141,
+                    "heading": 0.137026184669163,
+                    "angularVelocity": 1.2239232094189134,
+                    "velocityX": -2.213273622252188,
+                    "velocityY": 0.1716661534278421,
+                    "timestamp": 0.898279167539763
+                },
+                {
+                    "x": 5.242037120454666,
+                    "y": 6.625930660217185,
+                    "heading": 0.21902488374006282,
+                    "angularVelocity": 0.912842043253442,
+                    "velocityX": -1.4727705565027283,
+                    "velocityY": 0.10020702105814829,
+                    "timestamp": 0.9881070842937393
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": 0.48792213436398174,
-                    "velocityX": -0.7352077910701558,
-                    "velocityY": 0.04533906853386082,
-                    "timestamp": 1.0779002916773468
+                    "heading": 0.263,
+                    "angularVelocity": 0.4895484371565439,
+                    "velocityX": -0.7351514188571259,
+                    "velocityY": 0.04530150458636799,
+                    "timestamp": 1.0779350010477156
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": 3.4031853071795863,
+                    "heading": 0.263,
                     "angularVelocity": 0,
-                    "velocityX": 2.612853322689367e-35,
-                    "velocityY": 8.19047982527468e-33,
-                    "timestamp": 1.1677253159837924
+                    "velocityX": 4.777602762336884e-34,
+                    "velocityY": 2.1640483822800166e-33,
+                    "timestamp": 1.167762917801692
                 }
             ],
             "constraints": [
@@ -28629,7 +27564,7 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -28638,150 +27573,14 @@
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 3.14,
-                    "angularVelocity": 7.320432516399354e-29,
-                    "velocityX": -1.2956627863132083e-30,
-                    "velocityY": -2.1869206977044484e-30,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.583655523801243,
-                    "y": 5.855694285619672,
-                    "heading": 3.1309164320392644,
-                    "angularVelocity": -0.078082149385061,
-                    "velocityX": -0.8281749875703897,
-                    "velocityY": 0.5045347816645432,
-                    "timestamp": 0.1163334773987831
-                },
-                {
-                    "x": 7.390966571410279,
-                    "y": 5.973082843106357,
-                    "heading": 3.112749003492932,
-                    "angularVelocity": -0.15616681416695535,
-                    "velocityX": -1.6563499750844712,
-                    "velocityY": 1.0090694451115323,
-                    "timestamp": 0.2326669547975662
-                },
-                {
-                    "x": 7.1019331530012675,
-                    "y": 6.149165651208255,
-                    "heading": 3.0854972394904423,
-                    "angularVelocity": -0.2342555609256986,
-                    "velocityX": -2.484524875141701,
-                    "velocityY": 1.5136039258785212,
-                    "timestamp": 0.3490004321963493
-                },
-                {
-                    "x": 6.726101496816599,
-                    "y": 6.378126994339933,
-                    "heading": 3.085497236816664,
-                    "angularVelocity": -2.298373345534558e-8,
-                    "velocityX": -3.2306406082605346,
-                    "velocityY": 1.968146644037928,
-                    "timestamp": 0.4653339095951324
-                },
-                {
-                    "x": 6.345661043001768,
-                    "y": 6.599345871138349,
-                    "heading": 3.085497234127535,
-                    "angularVelocity": -2.3115694536288248e-8,
-                    "velocityX": -3.2702577308052767,
-                    "velocityY": 1.9015925745956355,
-                    "timestamp": 0.5816673869939155
-                },
-                {
-                    "x": 5.9214720079184415,
-                    "y": 6.716548218753983,
-                    "heading": 3.08549723097445,
-                    "angularVelocity": -2.7103849507961225e-8,
-                    "velocityX": -3.6463195682635403,
-                    "velocityY": 1.0074687891764202,
-                    "timestamp": 0.6980008643926986
-                },
-                {
-                    "x": 5.481423496557165,
-                    "y": 6.7220267434353165,
-                    "heading": 3.0854971884524733,
-                    "angularVelocity": -3.6551797104544465e-7,
-                    "velocityX": -3.7826472757521072,
-                    "velocityY": 0.047093277050021864,
-                    "timestamp": 0.8143343417914817
-                },
-                {
-                    "x": 5.071345998787579,
-                    "y": 6.619502331936359,
-                    "heading": 3.038730840809692,
-                    "angularVelocity": -0.40200249049953485,
-                    "velocityX": -3.5250171054705812,
-                    "velocityY": -0.8812975747944856,
-                    "timestamp": 0.9306678191902648
-                },
-                {
-                    "x": 4.728494542469685,
-                    "y": 6.427075201621358,
-                    "heading": 2.9510390674106386,
-                    "angularVelocity": -0.7537965455846517,
-                    "velocityX": -2.947143539280811,
-                    "velocityY": -1.6540993583074475,
-                    "timestamp": 1.0470012965890478
-                },
-                {
-                    "x": 4.474342078218714,
-                    "y": 6.168607565995357,
-                    "heading": 2.943927263065388,
-                    "angularVelocity": -0.061132912935043464,
-                    "velocityX": -2.1846889642931684,
-                    "velocityY": -2.221782082039817,
-                    "timestamp": 1.163334773987831
-                },
-                {
-                    "x": 4.328922284450032,
-                    "y": 5.887474207261646,
-                    "heading": 3.0089644652983214,
-                    "angularVelocity": 0.5590583526527744,
-                    "velocityX": -1.2500253325205173,
-                    "velocityY": -2.4166161368150747,
-                    "timestamp": 1.2796682513866142
-                },
-                {
-                    "x": 4.275692020371012,
-                    "y": 5.671261483228584,
-                    "heading": 3.090017233953519,
-                    "angularVelocity": 0.696727807571276,
-                    "velocityX": -0.45756617329120264,
-                    "velocityY": -1.8585597960929203,
-                    "timestamp": 1.3960017287853974
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0.42965075199414443,
-                    "velocityX": -0.10910032653373211,
-                    "velocityY": -0.9564012502367466,
-                    "timestamp": 1.5123352061841806
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": -1.1779881542407281e-29,
-                    "velocityX": 2.2171306210967267e-31,
-                    "velocityY": -6.290632939767308e-32,
-                    "timestamp": 1.6286686835829638
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -28822,7 +27621,7 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -28831,7 +27630,7 @@
                 {
                     "x": 4.263,
                     "y": 3,
-                    "heading": 2.574,
+                    "heading": -0.566,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -28842,191 +27641,191 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
-                    "angularVelocity": 7.712459531346682e-31,
-                    "velocityX": 8.463399539512725e-31,
-                    "velocityY": -2.525463677436265e-31,
+                    "heading": 0,
+                    "angularVelocity": 7.446350164213638e-30,
+                    "velocityX": -3.476389258222187e-32,
+                    "velocityY": 2.4606016306403766e-37,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.643960549352244,
-                    "y": 5.758768623179225,
-                    "heading": 3.0545306432351627,
-                    "angularVelocity": -1.025550451563212,
-                    "velocityX": -0.43243890307482463,
-                    "velocityY": -0.45873991856878515,
-                    "timestamp": 0.0833399825767314
-                },
-                {
-                    "x": 7.572007137321917,
-                    "y": 5.681963480289103,
-                    "heading": 2.882145272333543,
-                    "angularVelocity": -2.0684594065387945,
-                    "velocityX": -0.8633720551126874,
-                    "velocityY": -0.9215881803119774,
-                    "timestamp": 0.1666799651534628
-                },
-                {
-                    "x": 7.462165154490984,
-                    "y": 5.566757202432317,
-                    "heading": 2.631422178575744,
-                    "angularVelocity": -3.008437079128932,
-                    "velocityX": -1.3179986296469572,
-                    "velocityY": -1.382365034102495,
-                    "timestamp": 0.2500199477301942
-                },
-                {
-                    "x": 7.311173429444628,
-                    "y": 5.412128972668698,
-                    "heading": 2.3433148642731845,
-                    "angularVelocity": -3.457011933465413,
-                    "velocityX": -1.8117561388657581,
-                    "velocityY": -1.8553907138299572,
-                    "timestamp": 0.3333599303069256
-                },
-                {
-                    "x": 7.123843168628933,
-                    "y": 5.227542500531133,
-                    "heading": 2.191575313033137,
-                    "angularVelocity": -1.820729337210275,
-                    "velocityX": -2.2477837770510787,
-                    "velocityY": -2.21486093985698,
-                    "timestamp": 0.41669991288365704
-                },
-                {
-                    "x": 6.902601176392792,
-                    "y": 5.010431192670728,
-                    "heading": 2.175785118248912,
-                    "angularVelocity": -0.1894672196467848,
-                    "velocityX": -2.6546920865077652,
-                    "velocityY": -2.6051278287766304,
-                    "timestamp": 0.5000398954603884
-                },
-                {
-                    "x": 6.682324869385203,
-                    "y": 4.7848793693995555,
-                    "heading": 2.175784967765777,
-                    "angularVelocity": -0.0000018056535426780284,
-                    "velocityX": -2.643104788326184,
-                    "velocityY": -2.7064059326328107,
-                    "timestamp": 0.5833798780371198
-                },
-                {
-                    "x": 6.427295318194512,
-                    "y": 4.59952808942922,
-                    "heading": 2.175784213925318,
-                    "angularVelocity": -0.00000904536377316099,
-                    "velocityX": -3.060110445258255,
-                    "velocityY": -2.224037901611934,
-                    "timestamp": 0.6667198606138511
-                },
-                {
-                    "x": 6.1722637689115825,
-                    "y": 4.4141795587676,
-                    "heading": 2.1757834600140606,
-                    "angularVelocity": -0.000009046213285018581,
-                    "velocityX": -3.0601344204519796,
-                    "velocityY": -2.224004912539731,
-                    "timestamp": 0.7500598431905825
-                },
-                {
-                    "x": 5.906721685974762,
-                    "y": 4.244230836092675,
-                    "heading": 2.175782696018566,
-                    "angularVelocity": -0.000009167214470514032,
-                    "velocityX": -3.1862507613597577,
-                    "velocityY": -2.039221960701194,
-                    "timestamp": 0.8333998257673139
-                },
-                {
-                    "x": 5.614573321214709,
-                    "y": 4.1257208216835135,
-                    "heading": 2.175782627659757,
-                    "angularVelocity": -8.202402673559022e-7,
-                    "velocityX": -3.5055006699943965,
-                    "velocityY": -1.4220067096852238,
-                    "timestamp": 0.9167398083440452
-                },
-                {
-                    "x": 5.32091408424666,
-                    "y": 4.0110057741909,
-                    "heading": 2.1757827056250556,
-                    "angularVelocity": 9.355089415031885e-7,
-                    "velocityX": -3.523629689959162,
-                    "velocityY": -1.3764707400435987,
-                    "timestamp": 1.0000797909207766
-                },
-                {
-                    "x": 5.051276753694785,
-                    "y": 3.8476320535360697,
-                    "heading": 2.1757829434200273,
-                    "angularVelocity": 0.0000028533119900283243,
-                    "velocityX": -3.235389811890331,
-                    "velocityY": -1.9603282314633543,
-                    "timestamp": 1.083419773497508
-                },
-                {
-                    "x": 4.804772075990701,
-                    "y": 3.6532031340051674,
-                    "heading": 2.180086334431566,
-                    "angularVelocity": 0.05163657200885012,
-                    "velocityX": -2.957820125257733,
-                    "velocityY": -2.332960885273652,
-                    "timestamp": 1.1667597560742393
-                },
-                {
-                    "x": 4.605798608727452,
-                    "y": 3.4589597133454815,
-                    "heading": 2.2977487638281207,
-                    "angularVelocity": 1.411836501024247,
-                    "velocityX": -2.387491107045202,
-                    "velocityY": -2.330735076418397,
-                    "timestamp": 1.2500997386509707
-                },
-                {
-                    "x": 4.462499334629702,
-                    "y": 3.280406115781069,
-                    "heading": 2.4052460747163282,
-                    "angularVelocity": 1.28986481115753,
-                    "velocityX": -1.7194540923477422,
-                    "velocityY": -2.142472220941705,
-                    "timestamp": 1.333439721227702
-                },
-                {
-                    "x": 4.360731419840454,
-                    "y": 3.1416564242722327,
-                    "heading": 2.488750866660265,
-                    "angularVelocity": 1.0019775546155592,
-                    "velocityX": -1.2211175433778025,
-                    "velocityY": -1.6648634571178227,
-                    "timestamp": 1.4167797038044334
-                },
-                {
-                    "x": 4.295085549107569,
-                    "y": 3.0475525606989393,
-                    "heading": 2.5453815690352877,
-                    "angularVelocity": 0.6795142094358206,
-                    "velocityX": -0.7876875984758661,
-                    "velocityY": -1.129156266461315,
-                    "timestamp": 1.5001196863811648
+                    "x": 7.6436015698643995,
+                    "y": 5.74806653588022,
+                    "heading": -0.0038807165751560974,
+                    "angularVelocity": -0.045372809336175934,
+                    "velocityX": -0.4255654848002544,
+                    "velocityY": -0.572123393880186,
+                    "timestamp": 0.08552956345292874
+                },
+                {
+                    "x": 7.570804707716712,
+                    "y": 5.6501996114987145,
+                    "heading": -0.011642164214821673,
+                    "angularVelocity": -0.09074578807989701,
+                    "velocityX": -0.8511309915401587,
+                    "velocityY": -1.1442467426525054,
+                    "timestamp": 0.17105912690585748
+                },
+                {
+                    "x": 7.461609412342726,
+                    "y": 5.503399231866818,
+                    "heading": -0.02328436718357624,
+                    "angularVelocity": -0.136119050521775,
+                    "velocityX": -1.2766965124764296,
+                    "velocityY": -1.7163700328330225,
+                    "timestamp": 0.25658869035878623
+                },
+                {
+                    "x": 7.3160156842382476,
+                    "y": 5.307665404737857,
+                    "heading": -0.038807352623976644,
+                    "angularVelocity": -0.18149263031072865,
+                    "velocityX": -1.7022620276158245,
+                    "velocityY": -2.288493232362672,
+                    "timestamp": 0.34211825381171496
+                },
+                {
+                    "x": 7.134023531165023,
+                    "y": 5.062998148299065,
+                    "heading": -0.05821111114579378,
+                    "angularVelocity": -0.22686610031040333,
+                    "velocityX": -2.127827452005934,
+                    "velocityY": -2.8606162192496707,
+                    "timestamp": 0.4276478172646437
+                },
+                {
+                    "x": 6.9409164317403365,
+                    "y": 4.803390071813602,
+                    "heading": -0.058211113467083825,
+                    "angularVelocity": -2.714020682501112e-8,
+                    "velocityX": -2.2577818900123794,
+                    "velocityY": -3.0353022511138943,
+                    "timestamp": 0.5131773807175725
+                },
+                {
+                    "x": 6.70968017178257,
+                    "y": 4.577079452070077,
+                    "heading": -0.05821111591888997,
+                    "angularVelocity": -2.866618337367041e-8,
+                    "velocityX": -2.7035828387575864,
+                    "velocityY": -2.645992924634468,
+                    "timestamp": 0.5987069441705012
+                },
+                {
+                    "x": 6.4400791107327935,
+                    "y": 4.398188508585333,
+                    "heading": -0.05821111892081074,
+                    "angularVelocity": -3.509804859460385e-8,
+                    "velocityX": -3.152138864804934,
+                    "velocityY": -2.091568532127445,
+                    "timestamp": 0.6842365076234299
+                },
+                {
+                    "x": 6.141694589328426,
+                    "y": 4.273075039260189,
+                    "heading": -0.058211144902897714,
+                    "angularVelocity": -3.0377902004237926e-7,
+                    "velocityX": -3.488671160686832,
+                    "velocityY": -1.462809632999017,
+                    "timestamp": 0.7697660710763586
+                },
+                {
+                    "x": 5.825131041705898,
+                    "y": 4.206185466847777,
+                    "heading": -0.05821124348692034,
+                    "angularVelocity": -0.000001152630957627144,
+                    "velocityX": -3.7012178578082944,
+                    "velocityY": -0.7820637649954203,
+                    "timestamp": 0.8552956345292874
+                },
+                {
+                    "x": 5.503318785454965,
+                    "y": 4.1726658015863975,
+                    "heading": -0.058211338856237596,
+                    "angularVelocity": -0.0000011150450605169934,
+                    "velocityX": -3.76258504380235,
+                    "velocityY": -0.39190735820634004,
+                    "timestamp": 0.9408251979822161
+                },
+                {
+                    "x": 5.1935160355708865,
+                    "y": 4.079344267897855,
+                    "heading": -0.0582116136415855,
+                    "angularVelocity": -0.0000032127528402629677,
+                    "velocityX": -3.622171532005757,
+                    "velocityY": -1.091102654112135,
+                    "timestamp": 1.026354761435145
+                },
+                {
+                    "x": 4.918641828284195,
+                    "y": 3.9369980738928456,
+                    "heading": -0.09326111746178506,
+                    "angularVelocity": -0.4097940221510551,
+                    "velocityX": -3.2137917719873403,
+                    "velocityY": -1.664292301495839,
+                    "timestamp": 1.1118843248880736
+                },
+                {
+                    "x": 4.689025118769517,
+                    "y": 3.755353610578967,
+                    "heading": -0.16867561276178636,
+                    "angularVelocity": -0.8817360016283226,
+                    "velocityX": -2.6846472756878708,
+                    "velocityY": -2.1237623107224977,
+                    "timestamp": 1.1974138883410024
+                },
+                {
+                    "x": 4.510985991586203,
+                    "y": 3.542062267326439,
+                    "heading": -0.2715725395002121,
+                    "angularVelocity": -1.2030568447254515,
+                    "velocityX": -2.0816092120158958,
+                    "velocityY": -2.493773318156982,
+                    "timestamp": 1.282943451793931
+                },
+                {
+                    "x": 4.39344009542189,
+                    "y": 3.3364721307805043,
+                    "heading": -0.3792171957634167,
+                    "angularVelocity": -1.2585666513128753,
+                    "velocityX": -1.3743305989045989,
+                    "velocityY": -2.4037318588569803,
+                    "timestamp": 1.3684730152468598
+                },
+                {
+                    "x": 4.321460732883776,
+                    "y": 3.1711461674458645,
+                    "heading": -0.4697306425856333,
+                    "angularVelocity": -1.0582708851545903,
+                    "velocityX": -0.8415728975131295,
+                    "velocityY": -1.932968632835679,
+                    "timestamp": 1.4540025786997886
+                },
+                {
+                    "x": 4.280782976343733,
+                    "y": 3.057632956228743,
+                    "heading": -0.5333100471714295,
+                    "angularVelocity": -0.7433617338733072,
+                    "velocityX": -0.475598785938263,
+                    "velocityY": -1.3271809960727023,
+                    "timestamp": 1.5395321421527173
                 },
                 {
                     "x": 4.263,
                     "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": 0.3433937718713025,
-                    "velocityX": -0.3849958701158514,
-                    "velocityY": -0.5705852008687156,
-                    "timestamp": 1.5834596689578961
+                    "heading": -0.566,
+                    "angularVelocity": -0.38220647351440606,
+                    "velocityX": -0.20791613596297567,
+                    "velocityY": -0.6738366700592523,
+                    "timestamp": 1.625061705605646
                 },
                 {
                     "x": 4.263,
                     "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": -1.6359372913753935e-30,
+                    "heading": -0.566,
+                    "angularVelocity": -1.9453350650223036e-32,
                     "velocityX": 0,
-                    "velocityY": -3.7530428699783824e-32,
-                    "timestamp": 1.6667996515346275
+                    "velocityY": 3.675541041669649e-33,
+                    "timestamp": 1.7105912690585747
                 }
             ],
             "constraints": [
@@ -29069,7 +27868,7 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -29078,7 +27877,7 @@
                 {
                     "x": 5.176,
                     "y": 1.62,
-                    "heading": 2.5,
+                    "heading": -0.64,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -29126,7 +27925,7 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -29135,7 +27934,7 @@
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -29146,92 +27945,92 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
+                    "heading": 0,
                     "angularVelocity": 0,
-                    "velocityX": 1.6870437677917242e-24,
-                    "velocityY": 1.6813073596046228e-23,
+                    "velocityX": 0,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.680000027670122,
-                    "y": 5.880500005575848,
-                    "heading": 3.140000018022814,
-                    "angularVelocity": 1.8010003546528393e-7,
-                    "velocityX": 2.7650455327562645e-7,
-                    "velocityY": 0.8344065481228156,
-                    "timestamp": 0.10007112931184436
-                },
-                {
-                    "x": 7.680000074983718,
-                    "y": 6.047500013629854,
-                    "heading": 3.1400000460729856,
-                    "angularVelocity": 2.8030233382034975e-7,
-                    "velocityX": 4.7279965639225817e-7,
-                    "velocityY": 1.6688130652907482,
-                    "timestamp": 0.20014225862368873
-                },
-                {
-                    "x": 7.680000134184045,
-                    "y": 6.298000017966634,
-                    "heading": 3.1400000770424787,
-                    "angularVelocity": 3.0947480383704467e-7,
-                    "velocityX": 5.915824876001831e-7,
-                    "velocityY": 2.5032195205488788,
-                    "timestamp": 0.3002133879355331
-                },
-                {
-                    "x": 7.680000197955803,
-                    "y": 6.632000000000009,
-                    "heading": 3.1400001051570197,
-                    "angularVelocity": 2.8094557567454647e-7,
-                    "velocityX": 6.372642912914395e-7,
-                    "velocityY": 3.3376257900773374,
-                    "timestamp": 0.40028451724737746
-                },
-                {
-                    "x": 7.680000129458281,
-                    "y": 6.96599998203339,
-                    "heading": 3.140000063575334,
-                    "angularVelocity": -4.1552129568712905e-7,
-                    "velocityX": -6.844883427144649e-7,
-                    "velocityY": 3.337625790077381,
-                    "timestamp": 0.5003556465592218
+                    "x": 7.680000027621492,
+                    "y": 5.880500005575844,
+                    "heading": 1.812927824526906e-8,
+                    "angularVelocity": 1.811639213454267e-7,
+                    "velocityX": 2.7601859924059844e-7,
+                    "velocityY": 0.8344065481228081,
+                    "timestamp": 0.10007112931184069
+                },
+                {
+                    "x": 7.6800000748220105,
+                    "y": 6.047500013629843,
+                    "heading": 4.634757751496617e-8,
+                    "angularVelocity": 2.819824195265022e-7,
+                    "velocityX": 4.716696889582397e-7,
+                    "velocityY": 1.6688130652907391,
+                    "timestamp": 0.20014225862368137
+                },
+                {
+                    "x": 7.68000013381266,
+                    "y": 6.298000017966615,
+                    "heading": 7.748634725130675e-8,
+                    "angularVelocity": 3.1116636502244896e-7,
+                    "velocityX": 5.894871979234934e-7,
+                    "velocityY": 2.5032195205488934,
+                    "timestamp": 0.30021338793552205
+                },
+                {
+                    "x": 7.680000197181065,
+                    "y": 6.63200000000002,
+                    "heading": 1.0565412902376676e-7,
+                    "angularVelocity": 2.814776011952798e-7,
+                    "velocityX": 6.332336214323824e-7,
+                    "velocityY": 3.3376257900777544,
+                    "timestamp": 0.40028451724736275
+                },
+                {
+                    "x": 7.680000129088366,
+                    "y": 6.965999982033409,
+                    "heading": 6.403571463499393e-8,
+                    "angularVelocity": -4.1588832167210794e-7,
+                    "velocityX": -6.804429810396128e-7,
+                    "velocityY": 3.337625790077598,
+                    "timestamp": 0.5003556465592034
                 },
                 {
-                    "x": 7.680000069924504,
-                    "y": 7.216499986370157,
-                    "heading": 3.1400000316818817,
-                    "angularVelocity": -3.1870782840594973e-7,
-                    "velocityX": -5.949146101339814e-7,
-                    "velocityY": 2.5032195205487504,
-                    "timestamp": 0.6004267758710662
+                    "x": 7.680000069764372,
+                    "y": 7.216499986370169,
+                    "heading": 3.1970340337388056e-8,
+                    "angularVelocity": -3.2042582447411e-7,
+                    "velocityX": -5.92818279593764e-7,
+                    "velocityY": 2.503219520548763,
+                    "timestamp": 0.6004267758710441
                 },
                 {
-                    "x": 7.6800000249972635,
-                    "y": 7.383499994424155,
-                    "heading": 3.14000001043482,
-                    "angularVelocity": -2.1231960430791816e-7,
-                    "velocityX": -4.4895306909597897e-7,
-                    "velocityY": 1.6688130652906694,
-                    "timestamp": 0.7004979051829106
+                    "x": 7.680000024949458,
+                    "y": 7.3834999944241595,
+                    "heading": 1.0547562280132128e-8,
+                    "angularVelocity": -2.1407550924431962e-7,
+                    "velocityX": -4.47830590750479e-7,
+                    "velocityY": 1.6688130652906517,
+                    "timestamp": 0.7004979051828848
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
-                    "angularVelocity": -1.0427402507810036e-7,
-                    "velocityX": -2.497949654636061e-7,
-                    "velocityY": 0.8344065481227817,
-                    "timestamp": 0.8005690344947549
+                    "heading": -4.103497297298223e-32,
+                    "angularVelocity": -1.0540065169911972e-7,
+                    "velocityX": -2.4931725183429086e-7,
+                    "velocityY": 0.8344065481227665,
+                    "timestamp": 0.8005690344947255
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": 4.128313998274909e-23,
-                    "velocityY": -7.415182378687901e-21,
-                    "timestamp": 0.9006401638065993
+                    "heading": 0,
+                    "angularVelocity": 9.056551022450501e-31,
+                    "velocityX": 0,
+                    "velocityY": 3.73051624872423e-37,
+                    "timestamp": 0.9006401638065662
                 }
             ],
             "constraints": [
@@ -29274,7 +28073,7 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -29283,14 +28082,105 @@
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": -8.386874849918624e-34,
+                    "velocityY": 2.2609899154097436e-36,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.6800000367448975,
+                    "y": 5.713499994424246,
+                    "heading": -2.1127327949152964e-9,
+                    "angularVelocity": -2.1112310428149592e-8,
+                    "velocityX": 3.67187799986145e-7,
+                    "velocityY": -0.8344065481223435,
+                    "timestamp": 0.10007112931179547
+                },
+                {
+                    "x": 7.680000101493379,
+                    "y": 5.546499986370426,
+                    "heading": -1.0421334714876827e-8,
+                    "angularVelocity": -8.302696522536156e-8,
+                    "velocityX": 6.470245866798498e-7,
+                    "velocityY": -1.668813065289814,
+                    "timestamp": 0.20014225862359095
+                },
+                {
+                    "x": 7.680000185334384,
+                    "y": 5.295999982033924,
+                    "heading": -2.6903389225477584e-8,
+                    "angularVelocity": -1.6470340117310356e-7,
+                    "velocityX": 8.378141164132646e-7,
+                    "velocityY": -2.503219520547496,
+                    "timestamp": 0.30021338793538643
+                },
+                {
+                    "x": 7.680000279163236,
+                    "y": 4.961999999999876,
+                    "heading": -5.074281043975999e-8,
+                    "angularVelocity": -2.3822478681802103e-7,
+                    "velocityX": 9.376216058628794e-7,
+                    "velocityY": -3.3376257900847826,
+                    "timestamp": 0.4002845172471819
+                },
+                {
+                    "x": 7.680000186570606,
+                    "y": 4.628000017966085,
+                    "heading": -3.213613801692531e-8,
+                    "angularVelocity": 1.8593453255418192e-7,
+                    "velocityX": -9.252681679909059e-7,
+                    "velocityY": -3.3376257900823116,
+                    "timestamp": 0.5003556465589774
+                },
+                {
+                    "x": 7.68000010282861,
+                    "y": 4.377500013629577,
+                    "heading": -1.6413785806619038e-8,
+                    "angularVelocity": 1.5711175564873292e-7,
+                    "velocityX": -8.368247227344002e-7,
+                    "velocityY": -2.5032195205475483,
+                    "timestamp": 0.6004267758707729
+                },
+                {
+                    "x": 7.6800000374582895,
+                    "y": 4.2105000055757555,
+                    "heading": -5.494810627011176e-9,
+                    "angularVelocity": 1.0911213201504113e-7,
+                    "velocityX": -6.532385577861589e-7,
+                    "velocityY": -1.668813065289845,
+                    "timestamp": 0.7004979051825684
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 0,
+                    "angularVelocity": 5.4909043426679847e-8,
+                    "velocityX": -3.743166510876222e-7,
+                    "velocityY": -0.8344065481223572,
+                    "timestamp": 0.8005690344943639
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": 1.4758845853352304e-34,
+                    "velocityY": 2.7202161180895526e-29,
+                    "timestamp": 0.9006401638061594
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -29331,7 +28221,7 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -29340,7 +28230,7 @@
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -29351,136 +28241,136 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
-                    "angularVelocity": -1.429570475254817e-33,
-                    "velocityX": -4.923067503198675e-33,
-                    "velocityY": 6.357653017323465e-38,
+                    "heading": 0,
+                    "angularVelocity": -3.3479093461707414e-32,
+                    "velocityX": 0,
+                    "velocityY": 6.414063862178313e-32,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.680000074935176,
-                    "y": 5.7205546279944945,
-                    "heading": 3.1400000039815823,
-                    "angularVelocity": 4.158287610282007e-8,
-                    "velocityX": 7.826085092728229e-7,
-                    "velocityY": -0.7983807350943761,
-                    "timestamp": 0.09575052183150115
+                    "x": 7.680000074936813,
+                    "y": 5.720554627994495,
+                    "heading": 3.835783864206662e-9,
+                    "angularVelocity": 4.006018550688398e-8,
+                    "velocityX": 7.826256085285986e-7,
+                    "velocityY": -0.7983807350943762,
+                    "timestamp": 0.09575052183150116
                 },
                 {
-                    "x": 7.68000021698637,
-                    "y": 5.567663887017397,
-                    "heading": 3.1400000079801687,
-                    "angularVelocity": 4.176046399562833e-8,
-                    "velocityX": 0.000001483555234342966,
-                    "velocityY": -1.5967614385031572,
-                    "timestamp": 0.1915010436630023
+                    "x": 7.680000216991337,
+                    "y": 5.567663887017398,
+                    "heading": 7.581387411185645e-9,
+                    "angularVelocity": 3.911835760311887e-8,
+                    "velocityX": 0.0000014835900057043023,
+                    "velocityY": -1.5967614385031577,
+                    "timestamp": 0.19150104366300233
                 },
                 {
-                    "x": 7.680000418191477,
+                    "x": 7.68000041820133,
                     "y": 5.338327783136532,
-                    "heading": 3.140000009761107,
-                    "angularVelocity": 1.8599774209907478e-8,
-                    "velocityX": 0.000002101347243657271,
-                    "velocityY": -2.395142078540755,
+                    "heading": 9.040192056984193e-9,
+                    "angularVelocity": 1.523546877140865e-8,
+                    "velocityX": 0.0000021013982858961807,
+                    "velocityY": -2.3951420785407556,
                     "timestamp": 0.2872515654945035
                 },
                 {
-                    "x": 7.680000670419164,
-                    "y": 5.032546334555315,
-                    "heading": 3.1400000093900653,
-                    "angularVelocity": -3.875097409582088e-9,
-                    "velocityX": 0.000002634217150547891,
-                    "velocityY": -3.193522528465398,
-                    "timestamp": 0.3830020873260046
+                    "x": 7.680000670435067,
+                    "y": 5.032546334555316,
+                    "heading": 8.315274931561148e-9,
+                    "angularVelocity": -7.570901836021169e-9,
+                    "velocityX": 0.000002634280323205928,
+                    "velocityY": -3.193522528465399,
+                    "timestamp": 0.38300208732600466
                 },
                 {
-                    "x": 7.68000083273763,
-                    "y": 4.670327800733128,
-                    "heading": 3.1400000093900697,
-                    "angularVelocity": 4.937002430391119e-14,
-                    "velocityX": 0.000001695223016776456,
-                    "velocityY": -3.7829405719543154,
-                    "timestamp": 0.4787526091575057
+                    "x": 7.680000832750872,
+                    "y": 4.67032780073313,
+                    "heading": 8.315279560400229e-9,
+                    "angularVelocity": 4.9454766224662506e-14,
+                    "velocityX": 0.0000016951952366761453,
+                    "velocityY": -3.782940571954316,
+                    "timestamp": 0.47875260915750584
                 },
                 {
-                    "x": 7.6800009141474845,
-                    "y": 4.308109266911048,
-                    "heading": 3.1400000093900733,
-                    "angularVelocity": 3.662638141198336e-14,
-                    "velocityX": 8.50228817967247e-7,
+                    "x": 7.680000914152323,
+                    "y": 4.308109266911049,
+                    "heading": 8.315282864092028e-9,
+                    "angularVelocity": 3.631070768427552e-14,
+                    "velocityX": 8.501410582319702e-7,
                     "velocityY": -3.782940571953249,
-                    "timestamp": 0.5745031309890068
+                    "timestamp": 0.574503130989007
                 },
                 {
-                    "x": 7.680000914400225,
-                    "y": 3.9458907330889583,
-                    "heading": 3.1400000093900764,
-                    "angularVelocity": 3.286235025790432e-14,
-                    "velocityX": 2.639571649715247e-9,
+                    "x": 7.680000914394957,
+                    "y": 3.9458907330889588,
+                    "heading": 8.315285941661222e-9,
+                    "angularVelocity": 3.388658702074781e-14,
+                    "velocityX": 2.5340105811348506e-9,
                     "velocityY": -3.7829405719533424,
-                    "timestamp": 0.670253652820508
+                    "timestamp": 0.6702536528205081
                 },
                 {
-                    "x": 7.680000833444056,
-                    "y": 3.5836721992668767,
-                    "heading": 3.140000009390079,
-                    "angularVelocity": 2.6535816526961207e-14,
-                    "velocityX": -8.454906188658589e-7,
+                    "x": 7.680000833430653,
+                    "y": 3.583672199266877,
+                    "heading": 8.315288291957849e-9,
+                    "angularVelocity": 2.592873274060122e-14,
+                    "velocityX": -8.455755806508837e-7,
                     "velocityY": -3.78294057195325,
-                    "timestamp": 0.7660041746520091
+                    "timestamp": 0.7660041746520092
                 },
                 {
-                    "x": 7.680000671408071,
-                    "y": 3.221453665444688,
-                    "heading": 3.140000009390085,
-                    "angularVelocity": 5.86589821918151e-14,
-                    "velocityX": -0.0000016922728517221796,
-                    "velocityY": -3.7829405719543456,
-                    "timestamp": 0.8617546964835102
+                    "x": 7.6800006713921105,
+                    "y": 3.2214536654446886,
+                    "heading": 8.315293718879221e-9,
+                    "angularVelocity": 5.742758811630884e-14,
+                    "velocityX": -0.0000016922995574745742,
+                    "velocityY": -3.782940571954345,
+                    "timestamp": 0.8617546964835103
                 },
                 {
-                    "x": 7.680000418877801,
-                    "y": 2.9156722168634683,
-                    "heading": 3.140000001402919,
-                    "angularVelocity": -8.341641379215641e-8,
-                    "velocityX": -0.000002637377255306636,
-                    "velocityY": -3.193522528465426,
-                    "timestamp": 0.9575052183150113
+                    "x": 7.680000418867919,
+                    "y": 2.915672216863469,
+                    "heading": 6.817552183291689e-10,
+                    "angularVelocity": -7.972319645162058e-8,
+                    "velocityX": -0.000002637313779217548,
+                    "velocityY": -3.1935225284654263,
+                    "timestamp": 0.9575052183150115
                 },
                 {
-                    "x": 7.680000217377681,
-                    "y": 2.686336112982602,
-                    "heading": 3.139999997550481,
-                    "angularVelocity": -4.023411698701823e-8,
-                    "velocityX": -0.0000021044283112699676,
-                    "velocityY": -2.395142078540766,
+                    "x": 7.680000217372703,
+                    "y": 2.6863361129826027,
+                    "heading": -2.8485300950885154e-9,
+                    "angularVelocity": -3.6869616762863894e-8,
+                    "velocityX": -0.00000210437708878744,
+                    "velocityY": -2.395142078540767,
                     "timestamp": 1.0532557401465126
                 },
                 {
-                    "x": 7.680000075082089,
-                    "y": 2.5334453720055046,
-                    "heading": 3.139999997845116,
-                    "angularVelocity": 3.0771112395983046e-9,
-                    "velocityX": -0.0000014861076817541395,
-                    "velocityY": -1.5967614385031628,
+                    "x": 7.680000075080448,
+                    "y": 2.533445372005505,
+                    "heading": -2.300788754937178e-9,
+                    "angularVelocity": 5.720505703316253e-9,
+                    "velocityX": -0.0000014860728228146123,
+                    "velocityY": -1.596761438503163,
                     "timestamp": 1.1490062619780137
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": 2.250519944152171e-8,
-                    "velocityX": -7.841428415253779e-7,
-                    "velocityY": -0.7983807350943782,
+                    "heading": 0,
+                    "angularVelocity": 2.4028994457396085e-8,
+                    "velocityX": -7.841257148612366e-7,
+                    "velocityY": -0.7983807350943783,
                     "timestamp": 1.2447567838095148
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
+                    "heading": 0,
                     "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 5.079076909387901e-38,
+                    "velocityX": -1.5050925658054699e-33,
+                    "velocityY": -8.233479718145206e-39,
                     "timestamp": 1.340507305641016
                 }
             ],
@@ -29524,7 +28414,7 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -29533,7 +28423,7 @@
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -29544,173 +28434,173 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
-                    "angularVelocity": -8.977040531488188e-32,
-                    "velocityX": -1.4835352698294527e-33,
-                    "velocityY": -6.682715069575163e-38,
+                    "heading": 0,
+                    "angularVelocity": 1.5719252729433342e-31,
+                    "velocityX": 9.448616775755806e-32,
+                    "velocityY": 5.840312874632397e-38,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.680000115631693,
+                    "x": 7.680000115633079,
                     "y": 5.715155846048254,
-                    "heading": 3.139999983275777,
-                    "angularVelocity": -1.688054668223312e-7,
-                    "velocityX": 0.0000011671252822265972,
-                    "velocityY": -0.8260917454273096,
-                    "timestamp": 0.09907392781101573
+                    "heading": -1.667708661616254e-8,
+                    "angularVelocity": -1.683296946208156e-7,
+                    "velocityX": 0.0000011671392662331341,
+                    "velocityY": -0.8260917454273093,
+                    "timestamp": 0.0990739278110157
                 },
                 {
-                    "x": 7.680000338287477,
+                    "x": 7.68000033829167,
                     "y": 5.551467541258106,
-                    "heading": 3.1399999456936927,
-                    "angularVelocity": -3.793336766253788e-7,
-                    "velocityX": 0.0000022473699405935927,
+                    "heading": -5.4122540412155035e-8,
+                    "angularVelocity": -3.7795460569235657e-7,
+                    "velocityX": 0.0000022473982686017914,
                     "velocityY": -1.6521834594301872,
-                    "timestamp": 0.19814785562203147
+                    "timestamp": 0.1981478556220314
                 },
                 {
-                    "x": 7.680000659193895,
+                    "x": 7.680000659202128,
                     "y": 5.305935091856244,
-                    "heading": 3.1399998851536046,
-                    "angularVelocity": -6.110596046233074e-7,
-                    "velocityX": 0.0000032390599337968516,
+                    "heading": -1.1439481389604968e-7,
+                    "angularVelocity": -6.083564268701089e-7,
+                    "velocityX": 0.000003239100713146307,
                     "velocityY": -2.4782751105841556,
-                    "timestamp": 0.29722178343304717
+                    "timestamp": 0.2972217834330471
                 },
                 {
-                    "x": 7.680001069383311,
-                    "y": 4.978558516522714,
-                    "heading": 3.1399998022537425,
-                    "angularVelocity": -8.367472472020738e-7,
-                    "velocityX": 0.0000041402352562216615,
-                    "velocityY": -3.3043665731916145,
-                    "timestamp": 0.39629571124406293
+                    "x": 7.680001069396323,
+                    "y": 4.978558516522715,
+                    "heading": -1.9685469205745493e-7,
+                    "angularVelocity": -8.323062817894511e-7,
+                    "velocityX": 0.000004140283484089047,
+                    "velocityY": -3.304366573191614,
+                    "timestamp": 0.3962957112440628
                 },
                 {
-                    "x": 7.680001393532974,
+                    "x": 7.680001393540419,
                     "y": 4.603767735073282,
-                    "heading": 3.1399998022537403,
-                    "angularVelocity": -2.4397007486697484e-14,
-                    "velocityX": 0.0000032717959136681545,
+                    "heading": -1.9685469402018675e-7,
+                    "angularVelocity": -2.3695928725495928e-14,
+                    "velocityX": 0.0000032717397250257404,
                     "velocityY": -3.782940575085891,
-                    "timestamp": 0.4953696390550787
+                    "timestamp": 0.4953696390550785
                 },
                 {
-                    "x": 7.680001631666853,
-                    "y": 4.228976953623769,
-                    "heading": 3.1399998022537345,
-                    "angularVelocity": -6.162047103580494e-14,
-                    "velocityX": 0.000002403597704835693,
-                    "velocityY": -3.7829405750867746,
-                    "timestamp": 0.5944435668660945
+                    "x": 7.680001631663456,
+                    "y": 4.228976953623768,
+                    "heading": -1.9685469983118006e-7,
+                    "angularVelocity": -6.145037083886684e-14,
+                    "velocityX": 0.0000024034882723677366,
+                    "velocityY": -3.782940575086774,
+                    "timestamp": 0.5944435668660942
                 },
                 {
-                    "x": 7.68000178361998,
-                    "y": 3.854186172174224,
-                    "heading": 3.139999802253728,
-                    "angularVelocity": -7.021738392770843e-14,
-                    "velocityX": 0.0000015337346806740877,
+                    "x": 7.680001783604786,
+                    "y": 3.854186172174223,
+                    "heading": -1.9685470647072664e-7,
+                    "angularVelocity": -7.008912946454424e-14,
+                    "velocityX": 0.0000015336156103104506,
                     "velocityY": -3.78294057508712,
-                    "timestamp": 0.6935174946771102
+                    "timestamp": 0.6935174946771099
                 },
                 {
-                    "x": 7.680001849334011,
-                    "y": 3.4793953907246618,
-                    "heading": 3.1399998022537208,
-                    "angularVelocity": -7.676689036353258e-14,
-                    "velocityX": 6.632827646851356e-7,
+                    "x": 7.680001849310107,
+                    "y": 3.479395390724661,
+                    "heading": -1.9685471357202238e-7,
+                    "angularVelocity": -7.568241713719523e-14,
+                    "velocityX": 6.631948562175283e-7,
                     "velocityY": -3.782940575087319,
-                    "timestamp": 0.792591422488126
+                    "timestamp": 0.7925914224881256
                 },
                 {
-                    "x": 7.680001829174184,
-                    "y": 3.104604609275094,
-                    "heading": 3.139999802253714,
-                    "angularVelocity": -7.698791271549556e-14,
-                    "velocityX": -2.0348261115822963e-7,
+                    "x": 7.680001829148679,
+                    "y": 3.104604609275093,
+                    "heading": -1.968547206040393e-7,
+                    "angularVelocity": -7.70206577850091e-14,
+                    "velocityX": -2.0349878104492754e-7,
                     "velocityY": -3.7829405750873777,
-                    "timestamp": 0.8916653502991417
+                    "timestamp": 0.8916653502991414
                 },
                 {
-                    "x": 7.680001725145299,
-                    "y": 2.729813827825533,
-                    "heading": 3.1399998022537057,
-                    "angularVelocity": -9.316120157306394e-14,
-                    "velocityX": -0.000001050012614805773,
-                    "velocityY": -3.7829405750872973,
-                    "timestamp": 0.9907392781101575
+                    "x": 7.680001725125185,
+                    "y": 2.729813827825532,
+                    "heading": -1.968547288641082e-7,
+                    "angularVelocity": -9.134372064218233e-14,
+                    "velocityX": -0.0000010499582046587897,
+                    "velocityY": -3.7829405750872978,
+                    "timestamp": 0.9907392781101572
                 },
                 {
-                    "x": 7.680001540841657,
-                    "y": 2.355023046375991,
-                    "heading": 3.1399998022536986,
-                    "angularVelocity": -7.445499842222973e-14,
-                    "velocityX": -0.0000018602636449128091,
+                    "x": 7.680001540827678,
+                    "y": 2.3550230463759907,
+                    "heading": -1.9685473554016492e-7,
+                    "angularVelocity": -7.335754268551387e-14,
+                    "velocityX": -0.0000018602017039855543,
                     "velocityY": -3.7829405750870753,
-                    "timestamp": 1.0898132059211731
+                    "timestamp": 1.089813205921173
                 },
                 {
-                    "x": 7.680001281423707,
-                    "y": 1.9802322649264825,
-                    "heading": 3.139999802253698,
-                    "angularVelocity": -7.093550187375142e-15,
-                    "velocityX": -0.000002618427817479533,
+                    "x": 7.680001281411145,
+                    "y": 1.980232264926482,
+                    "heading": -1.9685473620018866e-7,
+                    "angularVelocity": -6.8770489517299035e-15,
+                    "velocityX": -0.0000026184135239642236,
                     "velocityY": -3.782940575086706,
-                    "timestamp": 1.188887133732189
+                    "timestamp": 1.1888871337321887
                 },
                 {
-                    "x": 7.680000953984531,
-                    "y": 1.6054414834770696,
-                    "heading": 3.139999802253699,
-                    "angularVelocity": 1.1174094242032294e-14,
-                    "velocityX": -0.0000033049984216512085,
-                    "velocityY": -3.782940575085678,
-                    "timestamp": 1.2879610615432047
+                    "x": 7.680000953971803,
+                    "y": 1.6054414834770692,
+                    "heading": -1.968547355090094e-7,
+                    "angularVelocity": 1.0827757762815176e-14,
+                    "velocityX": -0.000003305000098197799,
+                    "velocityY": -3.7829405750856786,
+                    "timestamp": 1.2879610615432044
                 },
                 {
-                    "x": 7.680000583113676,
-                    "y": 1.2780649081436686,
-                    "heading": 3.139999880388909,
-                    "angularVelocity": 7.886553669648788e-7,
-                    "velocityX": -0.0000037433746003660557,
-                    "velocityY": -3.304366573190331,
-                    "timestamp": 1.3870349893542204
+                    "x": 7.6800005831051426,
+                    "y": 1.2780649081436681,
+                    "heading": -1.1900708358832985e-7,
+                    "angularVelocity": 7.857529072428817e-7,
+                    "velocityX": -0.0000037433322544868336,
+                    "velocityY": -3.30436657319033,
+                    "timestamp": 1.3870349893542202
                 },
                 {
-                    "x": 7.6800002967542085,
+                    "x": 7.680000296749478,
                     "y": 1.0325324587418607,
-                    "heading": 3.139999940073013,
-                    "angularVelocity": 6.024197635453253e-7,
-                    "velocityX": -0.0000028903613408431902,
-                    "velocityY": -2.47827511058363,
-                    "timestamp": 1.4861089171652362
+                    "heading": -5.959102520162259e-8,
+                    "angularVelocity": 5.997142524637507e-7,
+                    "velocityX": -0.000002890322961269868,
+                    "velocityY": -2.478275110583629,
+                    "timestamp": 1.486108917165236
                 },
                 {
-                    "x": 7.680000100606573,
+                    "x": 7.680000100604838,
                     "y": 0.8688441539517371,
-                    "heading": 3.1399999800752636,
-                    "angularVelocity": 4.0376157345960537e-7,
-                    "velocityX": -0.000001979810751989867,
-                    "velocityY": -1.6521834594299558,
-                    "timestamp": 1.585182844976252
+                    "heading": -1.9801599569632452e-8,
+                    "angularVelocity": 4.016134309966653e-7,
+                    "velocityX": -0.000001979780510399852,
+                    "velocityY": -1.6521834594299551,
+                    "timestamp": 1.5851828449762517
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
-                    "angularVelocity": 2.0110976517179292e-7,
-                    "velocityX": -0.00000101546967480447,
-                    "velocityY": -0.8260917454272348,
-                    "timestamp": 1.6842567727872677
+                    "heading": -3.727510081119492e-33,
+                    "angularVelocity": 1.998668869584927e-7,
+                    "velocityX": -0.0000010154521677217573,
+                    "velocityY": -0.8260917454272346,
+                    "timestamp": 1.6842567727872675
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
-                    "angularVelocity": -8.944233053536814e-32,
+                    "heading": 0,
+                    "angularVelocity": 1.3621739357478042e-33,
                     "velocityX": 0,
-                    "velocityY": -1.1208522306082827e-37,
-                    "timestamp": 1.7833307005982835
+                    "velocityY": 1.5851260517441878e-39,
+                    "timestamp": 1.7833307005982832
                 }
             ],
             "constraints": [
@@ -29753,7 +28643,7 @@
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -29762,7 +28652,7 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": 3.5331853071795862,
+                    "heading": 0.393,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -29810,7 +28700,7 @@
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -29819,7 +28709,7 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -29830,191 +28720,191 @@
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": 8.742415967937999e-33,
-                    "velocityX": -1.6780440317133427e-34,
-                    "velocityY": 4.640205301974453e-35,
+                    "heading": 0,
+                    "angularVelocity": -8.248091288877123e-33,
+                    "velocityX": -1.7499589770527321e-34,
+                    "velocityY": 1.243057151683689e-35,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.599500768056594,
-                    "y": 4.12633480373168,
-                    "heading": 3.0835764078705523,
-                    "angularVelocity": -0.5690015744722524,
-                    "velocityX": -0.8117914508973499,
-                    "velocityY": -0.006708146534495369,
-                    "timestamp": 0.09916245342867555
-                },
-                {
-                    "x": 7.438156123912552,
-                    "y": 4.125079622260014,
-                    "heading": 2.9779167441714383,
-                    "angularVelocity": -1.065520870508837,
-                    "velocityX": -1.6270739434668355,
-                    "velocityY": -0.012657829937315014,
-                    "timestamp": 0.1983249068573511
-                },
-                {
-                    "x": 7.195408805362699,
-                    "y": 4.123338544846998,
-                    "heading": 2.8371936481283773,
-                    "angularVelocity": -1.4191167238946776,
-                    "velocityX": -2.4479761256053836,
-                    "velocityY": -0.017557829125994403,
-                    "timestamp": 0.2974873602860266
-                },
-                {
-                    "x": 6.870725465009593,
-                    "y": 4.121560782802172,
-                    "heading": 2.707127276825701,
-                    "angularVelocity": -1.311649387499567,
-                    "velocityX": -3.2742568293415686,
-                    "velocityY": -0.017927773903902572,
-                    "timestamp": 0.3966498137147022
-                },
-                {
-                    "x": 6.495621806376681,
-                    "y": 4.117497642893674,
-                    "heading": 2.7071272422206913,
-                    "angularVelocity": -3.489729050319063e-7,
-                    "velocityX": -3.7827186164036664,
-                    "velocityY": -0.04097458027721316,
-                    "timestamp": 0.4958122671433778
-                },
-                {
-                    "x": 6.120518146053048,
-                    "y": 4.113434659065473,
-                    "heading": 2.7071272076158857,
-                    "angularVelocity": -3.4897084992850824e-7,
-                    "velocityX": -3.7827186334536713,
-                    "velocityY": -0.04097300629137556,
-                    "timestamp": 0.5949747205720534
-                },
-                {
-                    "x": 5.746992350808683,
-                    "y": 4.148043089964446,
-                    "heading": 2.7071271713462406,
-                    "angularVelocity": -3.657598578804205e-7,
-                    "velocityX": -3.76680671291609,
-                    "velocityY": 0.3490074085739077,
-                    "timestamp": 0.6941371740007289
-                },
-                {
-                    "x": 5.389907487037217,
-                    "y": 4.262976321781226,
-                    "heading": 2.7071270362524373,
-                    "angularVelocity": -0.000001362348332205964,
-                    "velocityX": -3.6010087631434655,
-                    "velocityY": 1.1590398164104305,
-                    "timestamp": 0.7932996274294045
-                },
-                {
-                    "x": 5.0459552426535295,
-                    "y": 4.412696400521408,
-                    "heading": 2.70712690066141,
-                    "angularVelocity": -0.000001367362571417024,
-                    "velocityX": -3.468573361096618,
-                    "velocityY": 1.509846454614715,
-                    "timestamp": 0.8924620808580801
-                },
-                {
-                    "x": 4.713262422237293,
-                    "y": 4.586001722574957,
-                    "heading": 2.70712676137173,
-                    "angularVelocity": -0.0000014046614974130796,
-                    "velocityX": -3.355028127208762,
-                    "velocityY": 1.747690946132164,
-                    "timestamp": 0.9916245342867557
-                },
-                {
-                    "x": 4.426168065896197,
-                    "y": 4.827447572944867,
-                    "heading": 2.7071267614112156,
-                    "angularVelocity": 3.9818977543377344e-10,
-                    "velocityX": -2.8951921459627346,
-                    "velocityY": 2.4348515191142828,
-                    "timestamp": 1.0907869877154313
-                },
-                {
-                    "x": 4.093475286723252,
-                    "y": 5.000753014560729,
-                    "heading": 2.707126833350956,
-                    "angularVelocity": 7.254735825966592e-7,
-                    "velocityX": -3.355027711292369,
-                    "velocityY": 1.7476921518537738,
-                    "timestamp": 1.1899494411441067
-                },
-                {
-                    "x": 3.752079510529913,
-                    "y": 5.156214217951438,
-                    "heading": 2.707126904178975,
-                    "angularVelocity": 7.142624716180523e-7,
-                    "velocityX": -3.4427927546074075,
-                    "velocityY": 1.5677426083706831,
-                    "timestamp": 1.2891118945727822
-                },
-                {
-                    "x": 3.4085800456948445,
-                    "y": 5.306970245158155,
-                    "heading": 2.7071269730653125,
-                    "angularVelocity": 6.946816574949046e-7,
-                    "velocityX": -3.4640073229141812,
-                    "velocityY": 1.5202934376281008,
-                    "timestamp": 1.3882743480014577
-                },
-                {
-                    "x": 3.041842891660795,
-                    "y": 5.385857046749469,
-                    "heading": 2.7071269977260264,
-                    "angularVelocity": 2.486900331372695e-7,
-                    "velocityX": -3.698346918149141,
-                    "velocityY": 0.7955309581771803,
-                    "timestamp": 1.4874368014301331
-                },
-                {
-                    "x": 2.7246821450713132,
-                    "y": 5.456194623849801,
-                    "heading": 2.863679707498997,
-                    "angularVelocity": 1.5787498630777044,
-                    "velocityX": -3.1983955178923207,
-                    "velocityY": 0.7093166281018212,
-                    "timestamp": 1.5865992548588086
-                },
-                {
-                    "x": 2.4872060579091175,
-                    "y": 5.508276609290408,
-                    "heading": 2.997740485854431,
-                    "angularVelocity": 1.3519308339003542,
-                    "velocityX": -2.394818592634002,
-                    "velocityY": 0.5252188065119572,
-                    "timestamp": 1.685761708287484
-                },
-                {
-                    "x": 2.3290362313275734,
-                    "y": 5.542798533700601,
-                    "heading": 3.0916641126613666,
-                    "angularVelocity": 0.9471692516612843,
-                    "velocityX": -1.5950576161904975,
-                    "velocityY": 0.34813503716932903,
-                    "timestamp": 1.7849241617161595
+                    "x": 7.599512207946833,
+                    "y": 4.126338330595212,
+                    "heading": -0.05672955044148203,
+                    "angularVelocity": -0.5720722032058021,
+                    "velocityX": -0.8116550928518662,
+                    "velocityY": -0.0066724074357217035,
+                    "timestamp": 0.09916501819801522
+                },
+                {
+                    "x": 7.438184902121127,
+                    "y": 4.125091465076226,
+                    "heading": -0.16291927611759752,
+                    "angularVelocity": -1.070838563898341,
+                    "velocityX": -1.6268570183042166,
+                    "velocityY": -0.01257364281924666,
+                    "timestamp": 0.19833003639603045
+                },
+                {
+                    "x": 7.19545269041534,
+                    "y": 4.123364615129754,
+                    "heading": -0.30423484322627714,
+                    "angularVelocity": -1.4250546178138856,
+                    "velocityX": -2.4477604715515007,
+                    "velocityY": -0.017413902380612675,
+                    "timestamp": 0.2974950545940457
+                },
+                {
+                    "x": 6.870785634071806,
+                    "y": 4.1216134281783505,
+                    "heading": -0.4343749203987102,
+                    "angularVelocity": -1.3123587282822462,
+                    "velocityX": -3.274007933878773,
+                    "velocityY": -0.01765932163604032,
+                    "timestamp": 0.3966600727920609
+                },
+                {
+                    "x": 6.495672194218458,
+                    "y": 4.117557524267507,
+                    "heading": -0.4343749552288927,
+                    "angularVelocity": -3.5123456983963544e-7,
+                    "velocityX": -3.782719417287976,
+                    "velocityY": -0.040900551268437134,
+                    "timestamp": 0.4958250909900761
+                },
+                {
+                    "x": 6.120558752684864,
+                    "y": 4.113501775750749,
+                    "heading": -0.43437499005887026,
+                    "angularVelocity": -3.512325030859258e-7,
+                    "velocityX": -3.782719434231919,
+                    "velocityY": -0.04089898424321494,
+                    "timestamp": 0.5949901091880914
+                },
+                {
+                    "x": 5.747019040696225,
+                    "y": 4.148065138700183,
+                    "heading": -0.43437502656757854,
+                    "angularVelocity": -3.681611615448704e-7,
+                    "velocityX": -3.766849628794958,
+                    "velocityY": 0.3485439076955189,
+                    "timestamp": 0.6941551273861066
+                },
+                {
+                    "x": 5.389911454629062,
+                    "y": 4.262959433025878,
+                    "heading": -0.434375162279621,
+                    "angularVelocity": -0.0000013685475476133038,
+                    "velocityX": -3.601144764115114,
+                    "velocityY": 1.1586171859140066,
+                    "timestamp": 0.7933201455841219
+                },
+                {
+                    "x": 5.045913588511523,
+                    "y": 4.412598985545015,
+                    "heading": -0.4343752985502662,
+                    "angularVelocity": -0.0000013741806101348962,
+                    "velocityX": -3.468943709873945,
+                    "velocityY": 1.5089953618556682,
+                    "timestamp": 0.8924851637821372
+                },
+                {
+                    "x": 4.713243052034498,
+                    "y": 4.585968073714987,
+                    "heading": -0.43437543856751193,
+                    "angularVelocity": -0.0000014119620841874337,
+                    "velocityX": -3.354716638207433,
+                    "velocityY": 1.7482887748155678,
+                    "timestamp": 0.9916501819801524
+                },
+                {
+                    "x": 4.426185676495707,
+                    "y": 4.82747296096862,
+                    "heading": -0.4343754384530987,
+                    "angularVelocity": 1.153766536466057e-9,
+                    "velocityX": -2.894744343873237,
+                    "velocityY": 2.4353838847827403,
+                    "timestamp": 1.0908152001781677
+                },
+                {
+                    "x": 4.09351517988242,
+                    "y": 5.000842166204109,
+                    "heading": -0.4343753660253311,
+                    "angularVelocity": 7.303761838407394e-7,
+                    "velocityX": -3.354716236213479,
+                    "velocityY": 1.748289955327798,
+                    "timestamp": 1.189980218376183
+                },
+                {
+                    "x": 3.752063829141965,
+                    "y": 5.156204697333757,
+                    "heading": -0.43437529472941433,
+                    "angularVelocity": 7.189623727917814e-7,
+                    "velocityX": -3.4432641363372296,
+                    "velocityY": 1.5667070298864525,
+                    "timestamp": 1.2891452365741982
+                },
+                {
+                    "x": 3.4085469650969755,
+                    "y": 5.306945220863998,
+                    "heading": -0.43437522535251666,
+                    "angularVelocity": 6.996105981774619e-7,
+                    "velocityX": -3.46409318817495,
+                    "velocityY": 1.5200977750968405,
+                    "timestamp": 1.3883102547722135
+                },
+                {
+                    "x": 3.0417944650952866,
+                    "y": 5.3858068126004595,
+                    "heading": -0.43437520041032157,
+                    "angularVelocity": 2.5152211488411685e-7,
+                    "velocityX": -3.6984060172242224,
+                    "velocityY": 0.7952561615930772,
+                    "timestamp": 1.4874752729702287
+                },
+                {
+                    "x": 2.7246409461958954,
+                    "y": 5.456187357907523,
+                    "heading": -0.2777696543737322,
+                    "angularVelocity": 1.5792418423589194,
+                    "velocityX": -3.1982399102281316,
+                    "velocityY": 0.7097315826285274,
+                    "timestamp": 1.586640291168244
+                },
+                {
+                    "x": 2.4871805723397817,
+                    "y": 5.508279050924135,
+                    "heading": -0.14311381126711797,
+                    "angularVelocity": 1.3578966207390795,
+                    "velocityX": -2.394598197742947,
+                    "velocityY": 0.5253031155865293,
+                    "timestamp": 1.6858053093662593
+                },
+                {
+                    "x": 2.3290265259060328,
+                    "y": 5.5428007138640085,
+                    "heading": -0.04864902301750531,
+                    "angularVelocity": 0.9526019353012469,
+                    "velocityX": -1.594857231992267,
+                    "velocityY": 0.3481233964071807,
+                    "timestamp": 1.7849703275642745
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0.4874414223060736,
-                    "velocityX": -0.7970378766840579,
-                    "velocityY": 0.17346753438054688,
-                    "timestamp": 1.884086615144835
+                    "heading": -2.5681051863102055e-32,
+                    "angularVelocity": 0.490586538494469,
+                    "velocityX": -0.7969193909512572,
+                    "velocityY": 0.1734410626703791,
+                    "timestamp": 1.8841353457622898
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": -6.536266088535149e-32,
+                    "heading": 0,
+                    "angularVelocity": 1.14717044306562e-31,
                     "velocityX": 0,
-                    "velocityY": 2.2958668874154396e-32,
-                    "timestamp": 1.9832490685735105
+                    "velocityY": 5.846493116157662e-32,
+                    "timestamp": 1.983300363960305
                 }
             ],
             "constraints": [
@@ -30057,7 +28947,7 @@
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -30066,14 +28956,213 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 0,
+                    "angularVelocity": 5.58164160972529e-32,
+                    "velocityX": 1.1273756142039836e-32,
+                    "velocityY": 2.4507602066878393e-32,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.588395006557631,
+                    "y": 4.128528104169273,
+                    "heading": -0.06094869084906696,
+                    "angularVelocity": -0.5766457987157873,
+                    "velocityX": -0.8666902254019087,
+                    "velocityY": 0.01445765014696692,
+                    "timestamp": 0.10569519622756653
+                },
+                {
+                    "x": 7.4046836501246664,
+                    "y": 4.131675342578391,
+                    "heading": -0.17161815397097052,
+                    "angularVelocity": -1.0470623743734504,
+                    "velocityX": -1.7381239922903071,
+                    "velocityY": 0.029776551077511915,
+                    "timestamp": 0.21139039245513305
+                },
+                {
+                    "x": 7.128148507793846,
+                    "y": 4.136585421276787,
+                    "heading": -0.30937735862178317,
+                    "angularVelocity": -1.3033629679272352,
+                    "velocityX": -2.6163454177749763,
+                    "velocityY": 0.0464550790730766,
+                    "timestamp": 0.3170855886826996
+                },
+                {
+                    "x": 6.759745872373483,
+                    "y": 4.143898882898015,
+                    "heading": -0.3925495634756188,
+                    "angularVelocity": -0.7869061965196801,
+                    "velocityX": -3.485519196418107,
+                    "velocityY": 0.06919388848555069,
+                    "timestamp": 0.4227807849102661
+                },
+                {
+                    "x": 6.3599483672427946,
+                    "y": 4.149634642956697,
+                    "heading": -0.39254959749689916,
+                    "angularVelocity": -3.2188104690533836e-7,
+                    "velocityX": -3.782551330619671,
+                    "velocityY": 0.05426698907234567,
+                    "timestamp": 0.5284759811378327
+                },
+                {
+                    "x": 5.960150865134662,
+                    "y": 4.155370613693523,
+                    "heading": -0.39254963151811834,
+                    "angularVelocity": -3.2188046698591794e-7,
+                    "velocityX": -3.7825513020227493,
+                    "velocityY": 0.05426898233365955,
+                    "timestamp": 0.6341711773653992
+                },
+                {
+                    "x": 5.56430885680334,
+                    "y": 4.211762521698523,
+                    "heading": -0.3925496691964927,
+                    "angularVelocity": -3.5648142721372326e-7,
+                    "velocityX": -3.745127711188092,
+                    "velocityY": 0.5335333110464675,
+                    "timestamp": 0.7398663735929657
+                },
+                {
+                    "x": 5.196451370697568,
+                    "y": 4.356905683947299,
+                    "heading": -0.407192768864793,
+                    "angularVelocity": -0.13854082485237101,
+                    "velocityX": -3.4803614472105,
+                    "velocityY": 1.3732238306864724,
+                    "timestamp": 0.8455615698205322
+                },
+                {
+                    "x": 4.853119399386178,
+                    "y": 4.529141900436451,
+                    "heading": -0.46036689242459117,
+                    "angularVelocity": -0.5030893120753747,
+                    "velocityX": -3.2483214333807724,
+                    "velocityY": 1.6295557663597022,
+                    "timestamp": 0.9512567660480987
+                },
+                {
+                    "x": 4.514017884763102,
+                    "y": 4.740993233583884,
+                    "heading": -0.4603674462778229,
+                    "angularVelocity": -0.000005240098429403888,
+                    "velocityX": -3.208296372268186,
+                    "velocityY": 2.0043610372916896,
+                    "timestamp": 1.0569519622756653
+                },
+                {
+                    "x": 4.21750261375463,
+                    "y": 5.009227519029098,
+                    "heading": -0.46036738913342323,
+                    "angularVelocity": 5.406527608893175e-7,
+                    "velocityX": -2.8053807702864813,
+                    "velocityY": 2.537809616888297,
+                    "timestamp": 1.1626471585032319
+                },
+                {
+                    "x": 3.866969646487487,
+                    "y": 5.201574981603133,
+                    "heading": -0.46036721071098435,
+                    "angularVelocity": 0.0000016880846552508654,
+                    "velocityX": -3.3164512653198517,
+                    "velocityY": 1.8198316426784653,
+                    "timestamp": 1.2683423547307984
+                },
+                {
+                    "x": 3.5045677865422378,
+                    "y": 5.303672838764994,
+                    "heading": -0.3887465759877037,
+                    "angularVelocity": 0.6776148517580518,
+                    "velocityX": -3.4287448519891264,
+                    "velocityY": 0.9659649710289621,
+                    "timestamp": 1.3740375509583649
+                },
+                {
+                    "x": 3.16557460185915,
+                    "y": 5.315860378892371,
+                    "heading": -0.29320626763004903,
+                    "angularVelocity": 0.9039229006392311,
+                    "velocityX": -3.207271444515043,
+                    "velocityY": 0.1153083636945626,
+                    "timestamp": 1.4797327471859314
+                },
+                {
+                    "x": 2.8681536233597122,
+                    "y": 5.2454821587095966,
+                    "heading": -0.23849853505250618,
+                    "angularVelocity": 0.5175990445181124,
+                    "velocityX": -2.8139498209462337,
+                    "velocityY": -0.6658601591622668,
+                    "timestamp": 1.585427943413498
+                },
+                {
+                    "x": 2.618332286986499,
+                    "y": 5.0970676603891505,
+                    "heading": -0.24744806192055374,
+                    "angularVelocity": -0.08467297651615932,
+                    "velocityX": -2.3636016137889215,
+                    "velocityY": -1.4041744905880433,
+                    "timestamp": 1.6911231396410644
+                },
+                {
+                    "x": 2.4325478975720265,
+                    "y": 4.8813111498736355,
+                    "heading": -0.27927376577641677,
+                    "angularVelocity": -0.3011083283987745,
+                    "velocityX": -1.7577373054350616,
+                    "velocityY": -2.0413085761341616,
+                    "timestamp": 1.796818335868631
+                },
+                {
+                    "x": 2.3348070320385017,
+                    "y": 4.635496280912222,
+                    "heading": -0.32770640318337707,
+                    "angularVelocity": -0.4582293153861289,
+                    "velocityX": -0.9247427416008974,
+                    "velocityY": -2.3256957528340565,
+                    "timestamp": 1.9025135320961974
+                },
+                {
+                    "x": 2.305548232287739,
+                    "y": 4.452786448669473,
+                    "heading": -0.3683698320727586,
+                    "angularVelocity": -0.3847235289845278,
+                    "velocityX": -0.2768224176221525,
+                    "velocityY": -1.72864840374928,
+                    "timestamp": 2.008208728323764
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": -0.39,
+                    "angularVelocity": -0.2046466509288718,
+                    "velocityX": -0.05249275734153,
+                    "velocityY": -0.8778681716972112,
+                    "timestamp": 2.1139039245513307
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": -0.39,
+                    "angularVelocity": 5.518495272475637e-32,
+                    "velocityX": 0,
+                    "velocityY": 3.7393231194786592e-34,
+                    "timestamp": 2.219599120778897
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -30114,7 +29203,7 @@
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -30123,7 +29212,7 @@
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": 3.4031853071795863,
+                    "heading": 0.263,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -30134,155 +29223,155 @@
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": -7.693901589446952e-31,
-                    "velocityX": -6.624376358473947e-32,
+                    "heading": -7.709535672280055e-33,
+                    "angularVelocity": 3.0776034223095e-32,
+                    "velocityX": -2.0787724059290825e-32,
                     "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.653617802747577,
-                    "y": 4.193887846119818,
-                    "heading": 3.0831794581771232,
-                    "angularVelocity": -0.6046893294524585,
-                    "velocityX": -0.2807617219803893,
-                    "velocityY": 0.7118264895254428,
-                    "timestamp": 0.0939665032196413
-                },
-                {
-                    "x": 7.6005882437737835,
-                    "y": 4.328100423051846,
-                    "heading": 2.9787754661598114,
-                    "angularVelocity": -1.1110766969084016,
-                    "velocityX": -0.5643453481485778,
-                    "velocityY": 1.4283023453401595,
-                    "timestamp": 0.1879330064392826
-                },
-                {
-                    "x": 7.520633934300323,
-                    "y": 4.5302283764630555,
-                    "heading": 2.841927884917239,
-                    "angularVelocity": -1.4563442988050657,
-                    "velocityX": -0.8508809707068956,
-                    "velocityY": 2.1510639055999357,
-                    "timestamp": 0.2818995096589239
-                },
-                {
-                    "x": 7.4136169796865685,
-                    "y": 4.800820969540026,
-                    "heading": 2.7078159162812883,
-                    "angularVelocity": -1.4272316627817019,
-                    "velocityX": -1.1388840804643727,
-                    "velocityY": 2.879670774217013,
-                    "timestamp": 0.3758660128785652
-                },
-                {
-                    "x": 7.279364393815875,
-                    "y": 5.128465147908359,
-                    "heading": 2.7044182580824483,
-                    "angularVelocity": -0.03615818491083086,
-                    "velocityX": -1.4287281240729475,
-                    "velocityY": 3.486818889093733,
-                    "timestamp": 0.4698325160982065
-                },
-                {
-                    "x": 7.14774134031491,
-                    "y": 5.45866822544814,
-                    "heading": 2.7044182151788783,
-                    "angularVelocity": -4.565836605912663e-7,
-                    "velocityX": -1.4007444034956136,
-                    "velocityY": 3.514050924806152,
-                    "timestamp": 0.5637990193178478
-                },
-                {
-                    "x": 6.993945263950606,
-                    "y": 5.779145162484634,
-                    "heading": 2.704418171322215,
-                    "angularVelocity": -4.667265617625592e-7,
-                    "velocityX": -1.636711712096111,
-                    "velocityY": 3.410544460587175,
-                    "timestamp": 0.6577655225374891
-                },
-                {
-                    "x": 6.777429472030302,
-                    "y": 6.061066444299655,
-                    "heading": 2.704418118850798,
-                    "angularVelocity": -5.58405553736054e-7,
-                    "velocityX": -2.3041805803309763,
-                    "velocityY": 3.000231701248326,
-                    "timestamp": 0.7517320257571304
-                },
-                {
-                    "x": 6.5074813547317545,
-                    "y": 6.2923383909446455,
-                    "heading": 2.7044180981669834,
-                    "angularVelocity": -2.2011901843785396e-7,
-                    "velocityX": -2.872812204872187,
-                    "velocityY": 2.461216909438525,
-                    "timestamp": 0.8456985289767717
-                },
-                {
-                    "x": 6.195734686238664,
-                    "y": 6.463002989375195,
-                    "heading": 2.7046305201623446,
-                    "angularVelocity": 0.002260614028208905,
-                    "velocityX": -3.317636155560676,
-                    "velocityY": 1.81622804491969,
-                    "timestamp": 0.939665032196413
-                },
-                {
-                    "x": 5.885885567596677,
-                    "y": 6.564828261025872,
-                    "heading": 2.8003077522097084,
-                    "angularVelocity": 1.018205730436987,
-                    "velocityX": -3.297442258947648,
-                    "velocityY": 1.0836337222495915,
-                    "timestamp": 1.0336315354160543
-                },
-                {
-                    "x": 5.602675613806244,
-                    "y": 6.605201169261095,
-                    "heading": 3.005504879522894,
-                    "angularVelocity": 2.183726331004879,
-                    "velocityX": -3.0139458646071584,
-                    "velocityY": 0.4296521297685472,
-                    "timestamp": 1.1275980386356956
-                },
-                {
-                    "x": 5.389000568947429,
-                    "y": 6.621873348579134,
-                    "heading": 3.193372437152187,
-                    "angularVelocity": 1.999303487862736,
-                    "velocityX": -2.273949093959171,
-                    "velocityY": 0.17742683559341804,
-                    "timestamp": 1.221564541855337
-                },
-                {
-                    "x": 5.246855829881342,
-                    "y": 6.628281724607146,
-                    "heading": 3.330497456216328,
-                    "angularVelocity": 1.459296817118111,
-                    "velocityX": -1.5127171299950581,
-                    "velocityY": 0.06819851551815755,
-                    "timestamp": 1.3155310450749782
+                    "x": 7.653625318689537,
+                    "y": 4.193887618013532,
+                    "heading": -0.05699658004084171,
+                    "angularVelocity": -0.6065329552716214,
+                    "velocityX": -0.28066795214939116,
+                    "velocityY": 0.7117891037630026,
+                    "timestamp": 0.0939711182145365
+                },
+                {
+                    "x": 7.600609429037226,
+                    "y": 4.328102182971276,
+                    "heading": -0.1617251039431606,
+                    "angularVelocity": -1.1144756590341216,
+                    "velocityX": -0.5641721697008566,
+                    "velocityY": 1.4282533560080801,
+                    "timestamp": 0.187942236429073
+                },
+                {
+                    "x": 7.520673672603614,
+                    "y": 4.5302373715153195,
+                    "heading": -0.299017155202882,
+                    "angularVelocity": -1.4610026343017766,
+                    "velocityX": -0.8506417498525306,
+                    "velocityY": 2.151035258328725,
+                    "timestamp": 0.2819133546436095
+                },
+                {
+                    "x": 7.413680530437971,
+                    "y": 4.80084724043498,
+                    "heading": -0.43363333911195256,
+                    "angularVelocity": -1.4325272111984575,
+                    "velocityX": -1.1385747472045327,
+                    "velocityY": 2.8797131933862543,
+                    "timestamp": 0.375884472858146
+                },
+                {
+                    "x": 7.279380160390009,
+                    "y": 5.1284214711267,
+                    "heading": -0.4371879316114584,
+                    "angularVelocity": -0.037826436111898676,
+                    "velocityX": -1.4291664566697349,
+                    "velocityY": 3.4859032957750506,
+                    "timestamp": 0.46985559107268254
+                },
+                {
+                    "x": 7.147763767829931,
+                    "y": 5.458645997615663,
+                    "heading": -0.43718797458387054,
+                    "angularVelocity": -4.5729382513446123e-7,
+                    "velocityX": -1.4006047289934083,
+                    "velocityY": 3.514106597466034,
+                    "timestamp": 0.563826709287219
+                },
+                {
+                    "x": 6.993957305230812,
+                    "y": 5.779137314807096,
+                    "heading": -0.43718801851214967,
+                    "angularVelocity": -4.6746574877776706e-7,
+                    "velocityX": -1.6367418577267254,
+                    "velocityY": 3.410529993478958,
+                    "timestamp": 0.6577978275017555
+                },
+                {
+                    "x": 6.777425503833482,
+                    "y": 6.0610683140345785,
+                    "heading": -0.43718807107725144,
+                    "angularVelocity": -5.593750798607861e-7,
+                    "velocityX": -2.3042377861566687,
+                    "velocityY": 3.000187766030755,
+                    "timestamp": 0.751768945716292
+                },
+                {
+                    "x": 6.507457353311083,
+                    "y": 6.292343710685516,
+                    "heading": -0.4371880918313372,
+                    "angularVelocity": -2.2085600557820128e-7,
+                    "velocityX": -2.87288430372895,
+                    "velocityY": 2.4611327506280603,
+                    "timestamp": 0.8457400639308286
+                },
+                {
+                    "x": 6.1957042493791805,
+                    "y": 6.462993619393473,
+                    "heading": -0.4369141825473297,
+                    "angularVelocity": 0.0029148241418374306,
+                    "velocityX": -3.3175417070186306,
+                    "velocityY": 1.8159825268692036,
+                    "timestamp": 0.9397111821453651
+                },
+                {
+                    "x": 5.885878112264826,
+                    "y": 6.564806359719212,
+                    "heading": -0.3410861416211598,
+                    "angularVelocity": 1.0197605684269313,
+                    "velocityX": -3.297035759508787,
+                    "velocityY": 1.083447151211916,
+                    "timestamp": 1.0336823003599016
+                },
+                {
+                    "x": 5.602663899704114,
+                    "y": 6.605182844765102,
+                    "heading": -0.13545254769192583,
+                    "angularVelocity": 2.1882637754695176,
+                    "velocityX": -3.0138431673669435,
+                    "velocityY": 0.4296690920896744,
+                    "timestamp": 1.127653418574438
+                },
+                {
+                    "x": 5.388992479168701,
+                    "y": 6.621862905384601,
+                    "heading": 0.0527880784098822,
+                    "angularVelocity": 2.0031753338515554,
+                    "velocityX": -2.2737988500636965,
+                    "velocityY": 0.17750199142483714,
+                    "timestamp": 1.2216245367889744
+                },
+                {
+                    "x": 5.246852509457665,
+                    "y": 6.628278045699216,
+                    "heading": 0.19017469868317557,
+                    "angularVelocity": 1.4620089968455965,
+                    "velocityX": -1.5125920858633375,
+                    "velocityY": 0.06826714884852915,
+                    "timestamp": 1.3155956550035108
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": 0.7735506640420036,
-                    "velocityX": -0.7540541304992547,
-                    "velocityY": 0.018286041663562246,
-                    "timestamp": 1.4094975482946195
+                    "heading": 0.263,
+                    "angularVelocity": 0.7749753615846516,
+                    "velocityX": -0.7539817638001085,
+                    "velocityY": 0.01832429296896703,
+                    "timestamp": 1.4095667732180472
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
-                    "heading": 3.4031853071795863,
-                    "angularVelocity": 3.878626979187187e-32,
-                    "velocityX": -5.758991747455131e-33,
-                    "velocityY": 3.749392123602624e-30,
-                    "timestamp": 1.5034640515142608
+                    "heading": 0.263,
+                    "angularVelocity": 6.971645129510918e-32,
+                    "velocityX": 3.634932162806737e-32,
+                    "velocityY": 0,
+                    "timestamp": 1.5035378914325837
                 }
             ],
             "constraints": [
@@ -30325,23 +29414,168 @@
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 15
                 },
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 1.358270377512338e-32,
+                    "angularVelocity": -4.2989327725546786e-32,
+                    "velocityX": 9.28706448380456e-34,
+                    "velocityY": 4.1113328098198935e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.585313636391529,
+                    "y": 4.127746947940637,
+                    "heading": -0.08924586799649764,
+                    "angularVelocity": -0.8241960155421162,
+                    "velocityX": -0.8744396280098535,
+                    "velocityY": 0.006898151480975746,
+                    "timestamp": 0.10828233370892486
+                },
+                {
+                    "x": 7.394793136195202,
+                    "y": 4.129317895600586,
+                    "heading": -0.2483761167773939,
+                    "angularVelocity": -1.4695864351119023,
+                    "velocityX": -1.7594790735529262,
+                    "velocityY": 0.014507885138232623,
+                    "timestamp": 0.21656466741784972
+                },
+                {
+                    "x": 7.106938899563738,
+                    "y": 4.131715387581193,
+                    "heading": -0.440143741107867,
+                    "angularVelocity": -1.7709964106053178,
+                    "velocityX": -2.6583674988502586,
+                    "velocityY": 0.022141118486160887,
+                    "timestamp": 0.32484700112677456
+                },
+                {
+                    "x": 6.726884682874397,
+                    "y": 4.136782702937484,
+                    "heading": -0.5152103460374668,
+                    "angularVelocity": -0.6932488648738155,
+                    "velocityX": -3.509845084341914,
+                    "velocityY": 0.04679724921622947,
+                    "timestamp": 0.43312933483569943
+                },
+                {
+                    "x": 6.317264615210477,
+                    "y": 4.138917325587368,
+                    "heading": -0.515210397277194,
+                    "angularVelocity": -4.7320486478177106e-7,
+                    "velocityX": -3.782889171607859,
+                    "velocityY": 0.019713489511795114,
+                    "timestamp": 0.5414116685446243
+                },
+                {
+                    "x": 5.907644548289307,
+                    "y": 4.14105209076814,
+                    "heading": -0.5152104485168384,
+                    "angularVelocity": -4.732041015646636e-7,
+                    "velocityX": -3.7828891647484775,
+                    "velocityY": 0.019714805801191258,
+                    "timestamp": 0.6496940022535491
+                },
+                {
+                    "x": 5.500258277815471,
+                    "y": 4.183825709430579,
+                    "heading": -0.5152105024899405,
+                    "angularVelocity": -4.984479029228616e-7,
+                    "velocityX": -3.762259793632967,
+                    "velocityY": 0.3950193646307994,
+                    "timestamp": 0.7579763359624739
+                },
+                {
+                    "x": 5.114610765355458,
+                    "y": 4.3219164843017905,
+                    "heading": -0.5152109227554116,
+                    "angularVelocity": -0.000003881200715936794,
+                    "velocityX": -3.5614998241234512,
+                    "velocityY": 1.2752844359857862,
+                    "timestamp": 0.8662586696713988
+                },
+                {
+                    "x": 4.790273412388094,
+                    "y": 4.533322369220829,
+                    "heading": -0.5901314422767667,
+                    "angularVelocity": -0.6918997490647923,
+                    "velocityX": -2.9952933397170605,
+                    "velocityY": 1.952358041038544,
+                    "timestamp": 0.9745410033803236
+                },
+                {
+                    "x": 4.547771251602889,
+                    "y": 4.790487049727204,
+                    "heading": -0.7662492650374113,
+                    "angularVelocity": -1.6264686650923976,
+                    "velocityX": -2.239535780943542,
+                    "velocityY": 2.3749458632620883,
+                    "timestamp": 1.0828233370892484
+                },
+                {
+                    "x": 4.394005608234175,
+                    "y": 5.057835455573767,
+                    "heading": -0.8024780004938983,
+                    "angularVelocity": -0.3345766037318139,
+                    "velocityX": -1.4200436775037824,
+                    "velocityY": 2.468993756315091,
+                    "timestamp": 1.1911056707981733
+                },
+                {
+                    "x": 4.299277084870805,
+                    "y": 5.304155354811625,
+                    "heading": -0.5153879845736841,
+                    "angularVelocity": 2.651309831315093,
+                    "velocityX": -0.8748289782709269,
+                    "velocityY": 2.2747930414945974,
+                    "timestamp": 1.2993880045070982
+                },
+                {
+                    "x": 4.268346904630433,
+                    "y": 5.476762087961931,
+                    "heading": -0.20205360473699635,
+                    "angularVelocity": 2.893679597624539,
+                    "velocityX": -0.2856438273995415,
+                    "velocityY": 1.5940433424190128,
+                    "timestamp": 1.4076703382160232
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 2.864290108945775e-30,
+                    "angularVelocity": 1.8659886411401074,
+                    "velocityX": -0.04937928882107947,
+                    "velocityY": 0.7687118404911842,
+                    "timestamp": 1.515952671924948
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 0,
+                    "angularVelocity": -1.339793214188515e-31,
+                    "velocityX": -1.755749143439243e-31,
+                    "velocityY": 0,
+                    "timestamp": 1.624235005633873
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -30382,30 +29616,193 @@
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 17
                 },
                 {
                     "x": 4.263,
                     "y": 3,
-                    "heading": 2.574,
+                    "heading": -0.566,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
-            "constraints": [
+            "trajectory": [
                 {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 0,
+                    "angularVelocity": 4.8594393424661863e-32,
+                    "velocityX": 8.070887902586468e-34,
+                    "velocityY": -1.5484627527783442e-32,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.613090966537703,
+                    "y": 4.130415229492922,
+                    "heading": -0.02261774188189381,
+                    "angularVelocity": -0.25176724339812206,
+                    "velocityX": -0.7447915447554241,
+                    "velocityY": 0.038016302406854975,
+                    "timestamp": 0.08983591977315897
+                },
+                {
+                    "x": 7.479193313628693,
+                    "y": 4.137259099498979,
+                    "heading": -0.06454119971556918,
+                    "angularVelocity": -0.46666698504702897,
+                    "velocityX": -1.4904689933291897,
+                    "velocityY": 0.0761818883097586,
+                    "timestamp": 0.17967183954631794
+                },
+                {
+                    "x": 7.278200146895047,
+                    "y": 4.147551727639681,
+                    "heading": -0.12012980810986089,
+                    "angularVelocity": -0.6187793093216658,
+                    "velocityX": -2.237336326507015,
+                    "velocityY": 0.11457141160316474,
+                    "timestamp": 0.2695077593194769
+                },
+                {
+                    "x": 7.010006404514947,
+                    "y": 4.161329433657577,
+                    "heading": -0.1775477987284744,
+                    "angularVelocity": -0.6391429036434741,
+                    "velocityX": -2.985373145364283,
+                    "velocityY": 0.15336522465649807,
+                    "timestamp": 0.35934367909263587
+                },
+                {
+                    "x": 6.675632733992502,
+                    "y": 4.178685588756415,
+                    "heading": -0.19285230682653456,
+                    "angularVelocity": -0.17036067687364617,
+                    "velocityX": -3.7220487235639528,
+                    "velocityY": 0.19319839038333309,
+                    "timestamp": 0.44917959886579484
+                },
+                {
+                    "x": 6.3362226416697665,
+                    "y": 4.195852203162663,
+                    "heading": -0.19285232652882817,
+                    "angularVelocity": -2.193200640663355e-7,
+                    "velocityX": -3.7781111740035236,
+                    "velocityY": 0.1910885362012055,
+                    "timestamp": 0.5390155186389538
+                },
+                {
+                    "x": 5.996812544590127,
+                    "y": 4.213018723498308,
+                    "heading": -0.19285234623213549,
+                    "angularVelocity": -2.1933156264800868e-7,
+                    "velocityX": -3.7781112269558137,
+                    "velocityY": 0.1910874890356692,
+                    "timestamp": 0.6288514384121128
+                },
+                {
+                    "x": 5.65703115405788,
+                    "y": 4.206499194085455,
+                    "heading": -0.1928524148916534,
+                    "angularVelocity": -7.642946895727943e-7,
+                    "velocityX": -3.782244244720454,
+                    "velocityY": -0.07257152185432766,
+                    "timestamp": 0.7186873581852717
+                },
+                {
+                    "x": 5.325195518902672,
+                    "y": 4.133157682156235,
+                    "heading": -0.192852601151229,
+                    "angularVelocity": -0.0000020733763948999505,
+                    "velocityX": -3.6937968241559735,
+                    "velocityY": -0.8163940672507154,
+                    "timestamp": 0.8085232779584307
+                },
+                {
+                    "x": 5.022921580906399,
+                    "y": 3.999719932132448,
+                    "heading": -0.21619143682617745,
+                    "angularVelocity": -0.2597940304978627,
+                    "velocityX": -3.364733602738329,
+                    "velocityY": -1.485349628067107,
+                    "timestamp": 0.8983591977315897
+                },
+                {
+                    "x": 4.76405537852928,
+                    "y": 3.8156128777716756,
+                    "heading": -0.2707600447886353,
+                    "angularVelocity": -0.6074252715762222,
+                    "velocityX": -2.881544520603651,
+                    "velocityY": -2.0493701720293274,
+                    "timestamp": 0.9881951175047486
+                },
+                {
+                    "x": 4.5584097754694755,
+                    "y": 3.590752248351908,
+                    "heading": -0.3464948618413025,
+                    "angularVelocity": -0.8430349156856024,
+                    "velocityX": -2.2891244791432657,
+                    "velocityY": -2.5030147182477354,
+                    "timestamp": 1.0780310372779076
+                },
+                {
+                    "x": 4.420003796304514,
+                    "y": 3.3682412112408735,
+                    "heading": -0.42679529224450924,
+                    "angularVelocity": -0.8938566066393836,
+                    "velocityX": -1.5406529984228516,
+                    "velocityY": -2.476860454851527,
+                    "timestamp": 1.1678669570510665
+                },
+                {
+                    "x": 4.3340794956684014,
+                    "y": 3.187695881574611,
+                    "heading": -0.4942860320434666,
+                    "angularVelocity": -0.7512667535241839,
+                    "velocityX": -0.9564581834687845,
+                    "velocityY": -2.009723172222817,
+                    "timestamp": 1.2577028768242253
+                },
+                {
+                    "x": 4.284833149556787,
+                    "y": 3.063288277698013,
+                    "heading": -0.5416494132844137,
+                    "angularVelocity": -0.5272209752980105,
+                    "velocityX": -0.5481810197605272,
+                    "velocityY": -1.3848314147664058,
+                    "timestamp": 1.3475387965973842
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": -0.566,
+                    "angularVelocity": -0.27105624093568903,
+                    "velocityX": -0.24303362855860308,
+                    "velocityY": -0.7044874461979944,
+                    "timestamp": 1.437374716370543
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": -0.566,
+                    "angularVelocity": 6.221864662981848e-31,
+                    "velocityX": 0,
+                    "velocityY": 2.6923638006152617e-32,
+                    "timestamp": 1.527210636143702
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
                 {
                     "scope": [
                         "last"
@@ -30439,16 +29836,16 @@
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 18
                 },
                 {
                     "x": 5.176,
                     "y": 1.62,
-                    "heading": 2.5,
+                    "heading": -0.64,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -30496,23 +29893,159 @@
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 14
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 0,
+                    "angularVelocity": 1.212112656390561e-31,
+                    "velocityX": -5.041767818814111e-32,
+                    "velocityY": -1.2487171964671538e-38,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.6800000701104585,
+                    "y": 4.20344537200552,
+                    "heading": 1.2930383103747041e-8,
+                    "angularVelocity": 1.350424233383134e-7,
+                    "velocityX": 7.322200986357225e-7,
+                    "velocityY": 0.7983807350945226,
+                    "timestamp": 0.09575052183150286
+                },
+                {
+                    "x": 7.680000201769031,
+                    "y": 4.356336112982647,
+                    "heading": 3.8758528041627975e-8,
+                    "angularVelocity": 2.6974415299903026e-7,
+                    "velocityX": 0.0000013750167071257275,
+                    "velocityY": 1.596761438503451,
+                    "timestamp": 0.19150104366300572
+                },
+                {
+                    "x": 7.680000386416211,
+                    "y": 4.58567221686356,
+                    "heading": 7.786709578005107e-8,
+                    "angularVelocity": 4.0844231330927475e-7,
+                    "velocityX": 0.000001928419471656301,
+                    "velocityY": 2.395142078541207,
+                    "timestamp": 0.2872515654945086
+                },
+                {
+                    "x": 7.6800006155005685,
+                    "y": 4.891453665444849,
+                    "heading": 1.3155130559553437e-7,
+                    "angularVelocity": 5.606674749182053e-7,
+                    "velocityX": 0.0000023925127342176106,
+                    "velocityY": 3.1935225284661,
+                    "timestamp": 0.38300208732601143
+                },
+                {
+                    "x": 7.68000074582338,
+                    "y": 5.253672199267105,
+                    "heading": 1.3155130364873708e-7,
+                    "angularVelocity": -1.7664134647076396e-14,
+                    "velocityX": 0.000001361066521463048,
+                    "velocityY": 3.7829405719549913,
+                    "timestamp": 0.4787526091575143
+                },
+                {
+                    "x": 7.680000798333519,
+                    "y": 5.615890733089199,
+                    "heading": 1.315513137062799e-7,
+                    "angularVelocity": 1.0799702172100096e-13,
+                    "velocityX": 5.484057877231364e-7,
+                    "velocityY": 3.7829405719533282,
+                    "timestamp": 0.5745031309890172
+                },
+                {
+                    "x": 7.680000776767743,
+                    "y": 5.9781092669112965,
+                    "heading": 1.3155132085570774e-7,
+                    "angularVelocity": 7.667783798806564e-14,
+                    "velocityX": -2.2522878887411694e-7,
+                    "velocityY": 3.782940571953354,
+                    "timestamp": 0.6702536528205201
+                },
+                {
+                    "x": 7.680000686239199,
+                    "y": 6.340327800733382,
+                    "heading": 1.315513220333048e-7,
+                    "angularVelocity": 1.2578458101628908e-14,
+                    "velocityX": -9.454626397270524e-7,
+                    "velocityY": 3.78294057195323,
+                    "timestamp": 0.766004174652023
+                },
+                {
+                    "x": 7.680000533602743,
+                    "y": 6.702546334555452,
+                    "heading": 1.3155133191379775e-7,
+                    "angularVelocity": 1.0229175193513549e-13,
+                    "velocityX": -0.0000015941058350287079,
+                    "velocityY": 3.7829405719530396,
+                    "timestamp": 0.8617546964835259
+                },
+                {
+                    "x": 7.680000329705533,
+                    "y": 7.00832778313656,
+                    "heading": 7.923155128573452e-8,
+                    "angularVelocity": -5.46417642222627e-7,
+                    "velocityX": -0.000002129463080315811,
+                    "velocityY": 3.1935225284642037,
+                    "timestamp": 0.9575052183150288
+                },
+                {
+                    "x": 7.680000169486885,
+                    "y": 7.2376638870173995,
+                    "heading": 3.947148220681983e-8,
+                    "angularVelocity": -4.152464695661009e-7,
+                    "velocityX": -0.0000016732926513675465,
+                    "velocityY": 2.395142078540454,
+                    "timestamp": 1.0532557401465317
+                },
+                {
+                    "x": 7.680000058003377,
+                    "y": 7.390554627994493,
+                    "heading": 1.303400507527686e-8,
+                    "angularVelocity": -2.7610790251013916e-7,
+                    "velocityX": -0.000001164312243995105,
+                    "velocityY": 1.5967614385030973,
+                    "timestamp": 1.1490062619780346
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 0,
+                    "angularVelocity": -1.3612463214689187e-7,
+                    "velocityX": -6.057760896118273e-7,
+                    "velocityY": 0.7983807350943913,
+                    "timestamp": 1.2447567838095375
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": 1.2167404646059922e-31,
+                    "velocityY": -2.3530958630073903e-37,
+                    "timestamp": 1.3405073056410404
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -30553,7 +30086,7 @@
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -30562,7 +30095,7 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -30610,7 +30143,7 @@
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -30619,105 +30152,14 @@
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": 2.0369445518795058e-34,
-                    "velocityX": 0,
-                    "velocityY": 7.181654629664687e-43,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.680000037559162,
-                    "y": 4.043499994424155,
-                    "heading": 3.140000006758021,
-                    "angularVelocity": 6.75321744601227e-8,
-                    "velocityX": 3.753246582974315e-7,
-                    "velocityY": -0.8344065481227707,
-                    "timestamp": 0.10007112931184514
-                },
-                {
-                    "x": 7.6800001031226985,
-                    "y": 3.8764999863701557,
-                    "heading": 3.1400000189870485,
-                    "angularVelocity": 1.2220335610640094e-7,
-                    "velocityX": 6.551693493497348e-7,
-                    "velocityY": -1.6688130652906727,
-                    "timestamp": 0.20014225862369028
-                },
-                {
-                    "x": 7.680000187135912,
-                    "y": 3.625999982033383,
-                    "heading": 3.1400000350867425,
-                    "angularVelocity": 1.6088250383461684e-7,
-                    "velocityX": 8.39534968426326e-7,
-                    "velocityY": -2.503219520548785,
-                    "timestamp": 0.3002133879355354
-                },
-                {
-                    "x": 7.680000280051002,
-                    "y": 3.2919999999999914,
-                    "heading": 3.1400000531130368,
-                    "angularVelocity": 1.801348130208338e-7,
-                    "velocityX": 9.284904765306952e-7,
-                    "velocityY": -3.3376257900774653,
-                    "timestamp": 0.40028451724738057
-                },
-                {
-                    "x": 7.680000185977535,
-                    "y": 2.958000017966607,
-                    "heading": 3.1400000281215186,
-                    "angularVelocity": -2.4973754429186553e-7,
-                    "velocityX": -9.400660019914908e-7,
-                    "velocityY": -3.337625790077398,
-                    "timestamp": 0.5003556465592257
-                },
-                {
-                    "x": 7.680000101870251,
-                    "y": 2.7075000136298395,
-                    "heading": 3.1400000109296125,
-                    "angularVelocity": -1.7179686113327866e-7,
-                    "velocityX": -8.404750236739033e-7,
-                    "velocityY": -2.503219520548736,
-                    "timestamp": 0.6004267758710709
-                },
-                {
-                    "x": 7.6800000368892665,
-                    "y": 2.5405000055758427,
-                    "heading": 3.140000002248845,
-                    "angularVelocity": -8.674597729312873e-8,
-                    "velocityX": -6.493479632352893e-7,
-                    "velocityY": -1.6688130652906423,
-                    "timestamp": 0.700497905182916
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": -2.2472459301603394e-8,
-                    "velocityX": -3.686304669837251e-7,
-                    "velocityY": -0.8344065481227574,
-                    "timestamp": 0.8005690344947611
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": 7.368205253143154e-34,
-                    "velocityX": 0,
-                    "velocityY": -2.1562414433932607e-41,
-                    "timestamp": 0.9006401638066063
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -30758,7 +30200,7 @@
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -30767,7 +30209,7 @@
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -30778,137 +30220,137 @@
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": 9.207800442360542e-32,
-                    "velocityX": -6.095364969086891e-33,
-                    "velocityY": -9.366947302592518e-40,
+                    "heading": 0,
+                    "angularVelocity": 2.405138964716482e-32,
+                    "velocityX": 6.733008165659914e-33,
+                    "velocityY": 1.175464991429931e-38,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.680000070435844,
-                    "y": 4.0505546279944795,
-                    "heading": 3.1399999875764033,
-                    "angularVelocity": -1.2974964830179728e-7,
-                    "velocityX": 7.356183706178639e-7,
-                    "velocityY": -0.7983807350945191,
-                    "timestamp": 0.0957505218315028
+                    "x": 7.6800000704332305,
+                    "y": 4.05055462799448,
+                    "heading": -1.2377486895022226e-8,
+                    "angularVelocity": -1.2926808085167386e-7,
+                    "velocityX": 7.355910665177674e-7,
+                    "velocityY": -0.7983807350945193,
+                    "timestamp": 0.09575052183150283
                 },
                 {
-                    "x": 7.680000202739516,
+                    "x": 7.680000202731891,
                     "y": 3.8976638870173534,
-                    "heading": 3.1399999616102225,
-                    "angularVelocity": -2.711857701387203e-7,
-                    "velocityX": 0.0000013817539912349048,
-                    "velocityY": -1.5967614385034457,
-                    "timestamp": 0.1915010436630056
+                    "heading": -3.8235321936944764e-8,
+                    "angularVelocity": -2.700542284308498e-7,
+                    "velocityX": 0.0000013817016646870076,
+                    "velocityY": -1.5967614385034459,
+                    "timestamp": 0.19150104366300566
                 },
                 {
-                    "x": 7.680000388342391,
+                    "x": 7.6800003883277,
                     "y": 3.6683277831364425,
-                    "heading": 3.139999920718594,
-                    "angularVelocity": -4.2706425730703774e-7,
-                    "velocityX": 0.00000193840056979922,
-                    "velocityY": -2.395142078541202,
-                    "timestamp": 0.2872515654945084
-                },
-                {
-                    "x": 7.680000618679091,
-                    "y": 3.362546334555153,
-                    "heading": 3.139999863216847,
-                    "angularVelocity": -6.005370836912587e-7,
-                    "velocityX": 0.000002405591946532482,
-                    "velocityY": -3.1935225284661044,
-                    "timestamp": 0.3830020873260112
-                },
-                {
-                    "x": 7.680000750151485,
-                    "y": 3.000327800732898,
-                    "heading": 3.139999863216849,
-                    "angularVelocity": 2.234907225706593e-14,
-                    "velocityX": 0.0000013730725434542363,
-                    "velocityY": -3.7829405719549856,
-                    "timestamp": 0.478752609157514
-                },
-                {
-                    "x": 7.6800008034310885,
-                    "y": 2.6381092669108033,
-                    "heading": 3.139999863216839,
-                    "angularVelocity": -1.1397010502093565e-13,
-                    "velocityX": 5.56441925019577e-7,
+                    "heading": -7.893916226933331e-8,
+                    "angularVelocity": -4.251030288838169e-7,
+                    "velocityX": 0.0000019383267807539473,
+                    "velocityY": -2.3951420785412023,
+                    "timestamp": 0.2872515654945085
+                },
+                {
+                    "x": 7.680000618655782,
+                    "y": 3.3625463345551534,
+                    "heading": -1.3615412726424167e-7,
+                    "angularVelocity": -5.975419949298212e-7,
+                    "velocityX": 0.0000024055019376452327,
+                    "velocityY": -3.193522528466104,
+                    "timestamp": 0.3830020873260113
+                },
+                {
+                    "x": 7.680000750128537,
+                    "y": 3.0003278007328986,
+                    "heading": -1.3615412487460524e-7,
+                    "angularVelocity": 2.2064625199669458e-14,
+                    "velocityX": 0.000001373076316791352,
+                    "velocityY": -3.7829405719549847,
+                    "timestamp": 0.47875260915751416
+                },
+                {
+                    "x": 7.68000080341356,
+                    "y": 2.638109266910804,
+                    "heading": -1.3615413544372385e-7,
+                    "angularVelocity": -1.1353706918076568e-13,
+                    "velocityX": 5.564985227619036e-7,
                     "velocityY": -3.7829405719533264,
-                    "timestamp": 0.5745031309890168
+                    "timestamp": 0.574503130989017
                 },
                 {
-                    "x": 7.68000078216065,
-                    "y": 2.275890733088706,
-                    "heading": 3.1399998632168313,
-                    "angularVelocity": -8.126780055977152e-14,
-                    "velocityX": -2.2214435413644554e-7,
-                    "velocityY": -3.7829405719533535,
-                    "timestamp": 0.6702536528205196
+                    "x": 7.680000782147778,
+                    "y": 2.2758907330887066,
+                    "heading": -1.361541430398869e-7,
+                    "angularVelocity": -8.149555996364277e-14,
+                    "velocityX": -2.2209570916932486e-7,
+                    "velocityY": -3.782940571953353,
+                    "timestamp": 0.6702536528205197
                 },
                 {
-                    "x": 7.6800006913830785,
-                    "y": 1.9136721992666206,
-                    "heading": 3.1399998632168296,
-                    "angularVelocity": -1.6383854112995832e-14,
-                    "velocityX": -9.4806343038794e-7,
+                    "x": 7.68000069137085,
+                    "y": 1.9136721992666208,
+                    "heading": -1.3615414447383612e-7,
+                    "angularVelocity": -1.5331626502863808e-14,
+                    "velocityX": -9.48056722512506e-7,
                     "velocityY": -3.7829405719532305,
-                    "timestamp": 0.7660041746520224
+                    "timestamp": 0.7660041746520225
                 },
                 {
-                    "x": 7.680000537903045,
+                    "x": 7.68000053789062,
                     "y": 1.55145366544455,
-                    "heading": 3.13999986321682,
-                    "angularVelocity": -1.0277752214408611e-13,
-                    "velocityX": -0.0000016029159878741863,
-                    "velocityY": -3.7829405719530524,
-                    "timestamp": 0.8617546964835252
+                    "heading": -1.361541542308917e-7,
+                    "angularVelocity": -1.0101247732712604e-13,
+                    "velocityX": -0.000001602918056057891,
+                    "velocityY": -3.782940571953052,
+                    "timestamp": 0.8617546964835253
                 },
                 {
-                    "x": 7.680000332408874,
+                    "x": 7.68000033240059,
                     "y": 1.2456722168634402,
-                    "heading": 3.139999917268044,
-                    "angularVelocity": 5.645005022977776e-7,
-                    "velocityX": -0.0000021461414302094452,
-                    "velocityY": -3.1935225284642264,
-                    "timestamp": 0.957505218315028
+                    "heading": -8.229196402929451e-8,
+                    "angularVelocity": 5.625262643908853e-7,
+                    "velocityX": -0.0000021460981663034263,
+                    "velocityY": -3.193522528464227,
+                    "timestamp": 0.9575052183150281
                 },
                 {
-                    "x": 7.680000170899522,
-                    "y": 1.0163361129826003,
-                    "heading": 3.139999958602803,
-                    "angularVelocity": 4.3169222333319617e-7,
-                    "velocityX": -0.0000016867725067406415,
-                    "velocityY": -2.3951420785404567,
-                    "timestamp": 1.0532557401465308
+                    "x": 7.680000170894954,
+                    "y": 1.0163361129826005,
+                    "heading": -4.114641273305121e-8,
+                    "angularVelocity": 4.2971617502568956e-7,
+                    "velocityX": -0.000001686733705584938,
+                    "velocityY": -2.395142078540457,
+                    "timestamp": 1.053255740146531
                 },
                 {
-                    "x": 7.680000058494392,
+                    "x": 7.680000058492725,
                     "y": 0.8634453720055071,
-                    "heading": 3.1399999862669987,
-                    "angularVelocity": 2.889195077791466e-7,
-                    "velocityX": -0.0000011739374912229912,
-                    "velocityY": -1.5967614385030957,
-                    "timestamp": 1.1490062619780337
+                    "heading": -1.3639289939958608e-8,
+                    "angularVelocity": 2.872790734007971e-7,
+                    "velocityX": -0.0000011739071958177648,
+                    "velocityY": -1.5967614385030962,
+                    "timestamp": 1.149006261978034
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
-                    "angularVelocity": 1.4342481443578566e-7,
-                    "velocityX": -6.10904148268517e-7,
-                    "velocityY": -0.7983807350943894,
-                    "timestamp": 1.2447567838095366
+                    "heading": 0,
+                    "angularVelocity": 1.4244610959089762e-7,
+                    "velocityX": -6.10886739716558e-7,
+                    "velocityY": -0.7983807350943898,
+                    "timestamp": 1.2447567838095368
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
+                    "heading": 0,
                     "angularVelocity": 0,
-                    "velocityX": -3.045628877095783e-32,
-                    "velocityY": -7.047678165048749e-38,
-                    "timestamp": 1.3405073056410395
+                    "velocityX": 6.08432159346481e-32,
+                    "velocityY": 1.351843649746573e-37,
+                    "timestamp": 1.3405073056410397
                 }
             ],
             "constraints": [
@@ -30951,7 +30393,7 @@
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -30960,7 +30402,7 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": 3.5331853071795862,
+                    "heading": 0.393,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -30971,236 +30413,236 @@
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": 1.0740254772261542e-28,
-                    "velocityX": 2.0063238328605694e-29,
-                    "velocityY": 1.599232652510583e-28,
+                    "heading": 0,
+                    "angularVelocity": 2.322866633708497e-28,
+                    "velocityX": -2.1618763976557105e-31,
+                    "velocityY": -1.247476705840714e-31,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.635665892651778,
-                    "y": 2.515360941496967,
-                    "heading": 3.1393295631597837,
-                    "angularVelocity": -0.0071510111194275595,
-                    "velocityX": -0.47287630332947994,
-                    "velocityY": 0.6224892734875569,
-                    "timestamp": 0.09375413197081223
-                },
-                {
-                    "x": 7.546997676041258,
-                    "y": 2.6320828188122176,
-                    "heading": 3.137988695350318,
-                    "angularVelocity": -0.014301959617960866,
-                    "velocityX": -0.9457526270748597,
-                    "velocityY": 1.2449784864051567,
-                    "timestamp": 0.18750826394162445
-                },
-                {
-                    "x": 7.413995349999322,
-                    "y": 2.8071656233620255,
-                    "heading": 3.1359774017076236,
-                    "angularVelocity": -0.02145285333469997,
-                    "velocityX": -1.4186289526241012,
-                    "velocityY": 1.8674676077670485,
-                    "timestamp": 0.2812623959124367
-                },
-                {
-                    "x": 7.236658921990214,
-                    "y": 3.0406093354422112,
-                    "heading": 3.133295687975677,
-                    "angularVelocity": -0.028603685785057682,
-                    "velocityX": -1.8915051985582343,
-                    "velocityY": 2.489956518960288,
-                    "timestamp": 0.3750165278832489
-                },
-                {
-                    "x": 7.022133976710729,
-                    "y": 3.323040275054587,
-                    "heading": 3.133295687627681,
-                    "angularVelocity": -3.7117965182714086e-9,
-                    "velocityX": -2.288165233573672,
-                    "velocityY": 3.0124639167935428,
-                    "timestamp": 0.46877065985406113
-                },
-                {
-                    "x": 6.786563329839658,
-                    "y": 3.588171672312515,
-                    "heading": 3.133295687271953,
-                    "angularVelocity": -3.794263922237942e-9,
-                    "velocityX": -2.512642823512138,
-                    "velocityY": 2.8279435976269225,
-                    "timestamp": 0.5625247918248734
-                },
-                {
-                    "x": 6.501527367007155,
-                    "y": 3.7992230523695487,
-                    "heading": 3.133295686856248,
-                    "angularVelocity": -4.433993924393781e-9,
-                    "velocityX": -3.040249606505238,
-                    "velocityY": 2.251115504143757,
-                    "timestamp": 0.6562789237956856
-                },
-                {
-                    "x": 6.179197632360147,
-                    "y": 3.947181584790984,
-                    "heading": 3.133295705650933,
-                    "angularVelocity": 2.004678114437141e-7,
-                    "velocityX": -3.438032307177196,
-                    "velocityY": 1.5781547896737034,
-                    "timestamp": 0.7500330557664978
-                },
-                {
-                    "x": 5.833338645417163,
-                    "y": 4.025728972736192,
-                    "heading": 3.133296009616361,
-                    "angularVelocity": 0.000003242155002257518,
-                    "velocityX": -3.6889999370977806,
-                    "velocityY": 0.837801879171189,
-                    "timestamp": 0.84378718773731
-                },
-                {
-                    "x": 5.481107394947439,
-                    "y": 4.067217134926503,
-                    "heading": 3.1332963782586343,
-                    "angularVelocity": 0.000003932010945944395,
-                    "velocityX": -3.7569677524120277,
-                    "velocityY": 0.4425208928736066,
-                    "timestamp": 0.9375413197081222
-                },
-                {
-                    "x": 5.149070300488837,
-                    "y": 4.179082889717707,
-                    "heading": 3.1449832895383,
-                    "angularVelocity": 0.12465489289866781,
-                    "velocityX": -3.541572915015461,
-                    "velocityY": 1.1931821290397846,
-                    "timestamp": 1.0312954516789343
-                },
-                {
-                    "x": 4.859012780941325,
-                    "y": 4.350314048380944,
-                    "heading": 3.189517171995987,
-                    "angularVelocity": 0.4750071439148605,
-                    "velocityX": -3.093810517469455,
-                    "velocityY": 1.826385195657799,
-                    "timestamp": 1.1250495836497465
-                },
-                {
-                    "x": 4.618219764951525,
-                    "y": 4.5756090572292365,
-                    "heading": 3.250644921045485,
-                    "angularVelocity": 0.6520005866891553,
-                    "velocityX": -2.5683456390462376,
-                    "velocityY": 2.4030408485721884,
-                    "timestamp": 1.2188037156205587
-                },
-                {
-                    "x": 4.427454911100163,
-                    "y": 4.854246931015481,
-                    "heading": 3.2945713874044515,
-                    "angularVelocity": 0.46852832441237513,
-                    "velocityX": -2.034735428095452,
-                    "velocityY": 2.9720063311233567,
-                    "timestamp": 1.3125578475913708
-                },
-                {
-                    "x": 4.197108920054306,
-                    "y": 5.123929924649606,
-                    "heading": 3.2945713966505874,
-                    "angularVelocity": 9.862110665096438e-8,
-                    "velocityX": -2.4569156175171605,
-                    "velocityY": 2.8764918192414846,
-                    "timestamp": 1.406311979562183
-                },
-                {
-                    "x": 3.9283154974180072,
-                    "y": 5.355313346500023,
-                    "heading": 3.2945714071744505,
-                    "angularVelocity": 1.1224958813512746e-7,
-                    "velocityX": -2.8670034801237065,
-                    "velocityY": 2.4679810583970268,
-                    "timestamp": 1.5000661115329952
-                },
-                {
-                    "x": 3.6595153829213625,
-                    "y": 5.586688994322917,
-                    "heading": 3.2945714176982936,
-                    "angularVelocity": 1.1224938145766723e-7,
-                    "velocityX": -2.867074856821552,
-                    "velocityY": 2.4678981390913592,
-                    "timestamp": 1.5938202435038074
-                },
-                {
-                    "x": 3.3907152487550194,
-                    "y": 5.818064619294552,
-                    "heading": 3.2945714282221594,
-                    "angularVelocity": 1.1224961995364469e-7,
-                    "velocityX": -2.867075066622423,
-                    "velocityY": 2.4678978953553523,
-                    "timestamp": 1.6875743754746195
-                },
-                {
-                    "x": 3.121915101893736,
-                    "y": 6.049440229517853,
-                    "heading": 3.294571438746038,
-                    "angularVelocity": 1.1224975583094217e-7,
-                    "velocityX": -2.8670752020291417,
-                    "velocityY": 2.467897738046713,
-                    "timestamp": 1.7813285074454317
-                },
-                {
-                    "x": 2.853115082553889,
-                    "y": 6.2808159878868715,
-                    "heading": 3.2945714492725595,
-                    "angularVelocity": 1.1227794602420482e-7,
-                    "velocityX": -2.8670738418604556,
-                    "velocityY": 2.467899318198048,
-                    "timestamp": 1.8750826394162439
-                },
-                {
-                    "x": 2.6318681155961685,
-                    "y": 6.471289698509282,
-                    "heading": 3.3899753271617903,
-                    "angularVelocity": 1.0175965142415218,
-                    "velocityX": -2.3598636380805007,
-                    "velocityY": 2.031630037188212,
-                    "timestamp": 1.968836771387056
-                },
-                {
-                    "x": 2.465933868110727,
-                    "y": 6.614144871792939,
-                    "heading": 3.461571184706241,
-                    "angularVelocity": 0.7636554895173272,
-                    "velocityX": -1.7698872998696302,
-                    "velocityY": 1.5237213579891205,
-                    "timestamp": 2.062590903357868
-                },
-                {
-                    "x": 2.3553113103707712,
-                    "y": 6.709381606049684,
-                    "heading": 3.5093149725923682,
-                    "angularVelocity": 0.5092446261566541,
-                    "velocityX": -1.1799219449271308,
-                    "velocityY": 1.0158137274034427,
-                    "timestamp": 2.1563450353286804
+                    "x": 7.635666877682845,
+                    "y": 2.515362381859407,
+                    "heading": -0.0008300252680408195,
+                    "angularVelocity": -0.00885317162168204,
+                    "velocityX": -0.4728636048938783,
+                    "velocityY": 0.6225017511467659,
+                    "timestamp": 0.09375456655646779
+                },
+                {
+                    "x": 7.547000631163484,
+                    "y": 2.632087139923421,
+                    "heading": -0.0024900693866490363,
+                    "angularVelocity": -0.01770627479364842,
+                    "velocityX": -0.9457272298940119,
+                    "velocityY": 1.2450034419786018,
+                    "timestamp": 0.18750913311293557
+                },
+                {
+                    "x": 7.4140012603329,
+                    "y": 2.8071742656560237,
+                    "heading": -0.004980126198969091,
+                    "angularVelocity": -0.026559312295687615,
+                    "velocityX": -1.418590856056911,
+                    "velocityY": 1.867505041764004,
+                    "timestamp": 0.28126369966940334
+                },
+                {
+                    "x": 7.236668772838196,
+                    "y": 3.0406237394955027,
+                    "heading": -0.008300187115435396,
+                    "angularVelocity": -0.035412258180156565,
+                    "velocityX": -1.891454400654691,
+                    "velocityY": 2.4900064329013105,
+                    "timestamp": 0.37501826622587114
+                },
+                {
+                    "x": 7.022136423676778,
+                    "y": 3.3230511197393278,
+                    "heading": -0.008300187546286333,
+                    "angularVelocity": -4.59551947810532e-9,
+                    "velocityX": -2.2882335980104607,
+                    "velocityY": 3.012411988206685,
+                    "timestamp": 0.46877283278233894
+                },
+                {
+                    "x": 6.786560921928144,
+                    "y": 3.5881804025568163,
+                    "heading": -0.008300187986713265,
+                    "angularVelocity": -4.6976584720676315e-9,
+                    "velocityX": -2.51268295936015,
+                    "velocityY": 2.827907936172931,
+                    "timestamp": 0.5625273993388067
+                },
+                {
+                    "x": 6.5015204114211205,
+                    "y": 3.799228403383322,
+                    "heading": -0.008300188501414158,
+                    "angularVelocity": -5.489875460815635e-9,
+                    "velocityX": -3.0402840200359327,
+                    "velocityY": 2.251069026055312,
+                    "timestamp": 0.6562819658952744
+                },
+                {
+                    "x": 6.179186778264032,
+                    "y": 3.9471823835437703,
+                    "heading": -0.008300169798343478,
+                    "angularVelocity": 1.9948970346527163e-7,
+                    "velocityX": -3.4380579527606097,
+                    "velocityY": 1.5780989192812942,
+                    "timestamp": 0.7500365324517422
+                },
+                {
+                    "x": 5.833324835964247,
+                    "y": 4.025724182042927,
+                    "heading": -0.008299866211032016,
+                    "angularVelocity": 0.0000032381069276099122,
+                    "velocityX": -3.689014359545628,
+                    "velocityY": 0.8377383778085268,
+                    "timestamp": 0.8437910990082099
+                },
+                {
+                    "x": 5.481092764022396,
+                    "y": 4.0672194252777,
+                    "heading": -0.008299498284594501,
+                    "angularVelocity": 0.000003924357511610969,
+                    "velocityX": -3.7569590994770814,
+                    "velocityY": 0.44259436909433375,
+                    "timestamp": 0.9375456655646777
+                },
+                {
+                    "x": 5.14904862351455,
+                    "y": 4.179095043376677,
+                    "heading": 0.0033651913425959347,
+                    "angularVelocity": 0.12441729566489874,
+                    "velocityX": -3.5416316527670983,
+                    "velocityY": 1.1932818017093059,
+                    "timestamp": 1.0313002321211455
+                },
+                {
+                    "x": 4.858987953002521,
+                    "y": 4.350333701259038,
+                    "heading": 0.047887100206889445,
+                    "angularVelocity": 0.474877229979814,
+                    "velocityX": -3.093829785211866,
+                    "velocityY": 1.8264567174893411,
+                    "timestamp": 1.1250547986776134
+                },
+                {
+                    "x": 4.6181973815778665,
+                    "y": 4.575631660267378,
+                    "heading": 0.10901822024185409,
+                    "angularVelocity": 0.652033519862158,
+                    "velocityX": -2.5683076597621346,
+                    "velocityY": 2.403061176467013,
+                    "timestamp": 1.2188093652340812
+                },
+                {
+                    "x": 4.427445933507109,
+                    "y": 4.854263962903063,
+                    "heading": 0.15298068479753676,
+                    "angularVelocity": 0.46891011467909954,
+                    "velocityX": -2.0345830083473238,
+                    "velocityY": 2.971933132108998,
+                    "timestamp": 1.312563931790549
+                },
+                {
+                    "x": 4.19709169118396,
+                    "y": 5.123942070734021,
+                    "heading": 0.15298069409509907,
+                    "angularVelocity": 9.916916750047786e-8,
+                    "velocityX": -2.4569922381797515,
+                    "velocityY": 2.8764263729866686,
+                    "timestamp": 1.406318498347017
+                },
+                {
+                    "x": 3.9282946631899107,
+                    "y": 5.355323824233057,
+                    "heading": 0.15298070467968702,
+                    "angularVelocity": 1.1289677246612154e-7,
+                    "velocityX": -2.8670286458223346,
+                    "velocityY": 2.4679518235485323,
+                    "timestamp": 1.5000730649034848
+                },
+                {
+                    "x": 3.6594937496643456,
+                    "y": 5.586701063819283,
+                    "heading": 0.1529807152642756,
+                    "angularVelocity": 1.1289677885329184e-7,
+                    "velocityX": -2.867070089473112,
+                    "velocityY": 2.4679036774904017,
+                    "timestamp": 1.5938276314599527
+                },
+                {
+                    "x": 3.3906928188108107,
+                    "y": 5.818078283274842,
+                    "heading": 0.1529807258488866,
+                    "angularVelocity": 1.1289701863908675e-7,
+                    "velocityX": -2.8670702742957928,
+                    "velocityY": 2.467903462773746,
+                    "timestamp": 1.6875821980164205
+                },
+                {
+                    "x": 3.1218918754104785,
+                    "y": 6.049455488154229,
+                    "heading": 0.15298073643351034,
+                    "angularVelocity": 1.1289715379702627e-7,
+                    "velocityX": -2.867070408121775,
+                    "velocityY": 2.467903307302158,
+                    "timestamp": 1.7813367645728884
+                },
+                {
+                    "x": 2.853090944613356,
+                    "y": 6.280832707674931,
+                    "heading": 0.15298074701856604,
+                    "angularVelocity": 1.1290176153813175e-7,
+                    "velocityX": -2.8670702736940825,
+                    "velocityY": 2.4679034634685753,
+                    "timestamp": 1.8750913311293562
+                },
+                {
+                    "x": 2.631853660621228,
+                    "y": 6.471299719512446,
+                    "heading": 0.24894777935018825,
+                    "angularVelocity": 1.0235984854542721,
+                    "velocityX": -2.3597494193403232,
+                    "velocityY": 2.0315491696374846,
+                    "timestamp": 1.968845897685824
+                },
+                {
+                    "x": 2.465926649664366,
+                    "y": 6.614149879341798,
+                    "heading": 0.32096506390227886,
+                    "angularVelocity": 0.7681469521670183,
+                    "velocityX": -1.7698019099360354,
+                    "velocityY": 1.52366082075921,
+                    "timestamp": 2.062600464242292
+                },
+                {
+                    "x": 2.355308907973467,
+                    "y": 6.709383273555383,
+                    "heading": 0.3689895410149908,
+                    "angularVelocity": 0.5122361382129277,
+                    "velocityX": -1.1798651068828128,
+                    "velocityY": 1.0157733933549364,
+                    "timestamp": 2.1563550307987596
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": 3.5331853071795862,
-                    "angularVelocity": 0.2546056806823578,
-                    "velocityX": -0.5899613084572273,
-                    "velocityY": 0.5079071497898447,
-                    "timestamp": 2.2500991672994926
+                    "heading": 0.393,
+                    "angularVelocity": 0.25609908793666974,
+                    "velocityX": -0.5899329494543092,
+                    "velocityY": 0.5078870095990298,
+                    "timestamp": 2.250109597355227
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": 3.5331853071795862,
+                    "heading": 0.393,
                     "angularVelocity": 0,
-                    "velocityX": 7.663961659075425e-29,
-                    "velocityY": 7.569331315704837e-30,
-                    "timestamp": 2.3438532992703047
+                    "velocityX": -3.330818616870831e-32,
+                    "velocityY": 5.736512740310497e-33,
+                    "timestamp": 2.343864163911695
                 }
             ],
             "constraints": [
@@ -31243,7 +30685,7 @@
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -31252,222 +30694,14 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": 1.048713449145388e-30,
-                    "velocityX": -1.8824383693720305e-31,
-                    "velocityY": -8.291277152276166e-32,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.627273403539296,
-                    "y": 2.514475736726478,
-                    "heading": 3.079415345929856,
-                    "angularVelocity": -0.619115297628915,
-                    "velocityX": -0.5388137138314486,
-                    "velocityY": 0.5873452344657393,
-                    "timestamp": 0.09785681972674802
-                },
-                {
-                    "x": 7.521466473904217,
-                    "y": 2.629566524398062,
-                    "heading": 2.9639191611810785,
-                    "angularVelocity": -1.1802568801161224,
-                    "velocityX": -1.08124226733027,
-                    "velocityY": 1.1761141225819405,
-                    "timestamp": 0.19571363945349604
-                },
-                {
-                    "x": 7.362002347569612,
-                    "y": 2.8026163659937295,
-                    "heading": 2.8066032187897654,
-                    "angularVelocity": -1.6076134788622511,
-                    "velocityX": -1.6295657960261463,
-                    "velocityY": 1.7683983812153936,
-                    "timestamp": 0.29357045918024405
-                },
-                {
-                    "x": 7.14775766144854,
-                    "y": 3.033681724074784,
-                    "heading": 2.6597575656629697,
-                    "angularVelocity": -1.5006174688370466,
-                    "velocityX": -2.18936898541483,
-                    "velocityY": 2.361259631431653,
-                    "timestamp": 0.3914272789069921
-                },
-                {
-                    "x": 6.898594719017662,
-                    "y": 3.307462468622087,
-                    "heading": 2.6597575308458405,
-                    "angularVelocity": -3.5579665549162934e-7,
-                    "velocityX": -2.546199060286565,
-                    "velocityY": 2.7977686717369177,
-                    "timestamp": 0.4892840986337401
-                },
-                {
-                    "x": 6.632320724562211,
-                    "y": 3.5646321159756953,
-                    "heading": 2.6597574950521046,
-                    "angularVelocity": -3.657766164691694e-7,
-                    "velocityX": -2.721057103623286,
-                    "velocityY": 2.628019672739413,
-                    "timestamp": 0.5871409183604881
-                },
-                {
-                    "x": 6.3170953901288,
-                    "y": 3.7587224487937174,
-                    "heading": 2.6597575172185417,
-                    "angularVelocity": 2.265190860696483e-7,
-                    "velocityX": -3.221291426735886,
-                    "velocityY": 1.983411410262395,
-                    "timestamp": 0.6849977380872361
-                },
-                {
-                    "x": 5.989980622324306,
-                    "y": 3.9320256447233213,
-                    "heading": 2.65975753898433,
-                    "angularVelocity": 2.224248489911636e-7,
-                    "velocityX": -3.342789687197243,
-                    "velocityY": 1.770987412155123,
-                    "timestamp": 0.782854557813984
-                },
-                {
-                    "x": 5.662865689540743,
-                    "y": 4.1053285292498645,
-                    "heading": 2.659757560750018,
-                    "angularVelocity": 2.2242382298690162e-7,
-                    "velocityX": -3.3427913731203196,
-                    "velocityY": 1.7709842299235545,
-                    "timestamp": 0.880711377540732
-                },
-                {
-                    "x": 5.335750711483487,
-                    "y": 4.278631328320837,
-                    "heading": 2.659757582515508,
-                    "angularVelocity": 2.2242180190175268e-7,
-                    "velocityX": -3.3427918357727204,
-                    "velocityY": 1.7709833566520703,
-                    "timestamp": 0.97856819726748
-                },
-                {
-                    "x": 5.008636210362509,
-                    "y": 4.451935027622489,
-                    "heading": 2.659757604280525,
-                    "angularVelocity": 2.224169652952219e-7,
-                    "velocityX": -3.342786961955248,
-                    "velocityY": 1.7709925561200404,
-                    "timestamp": 1.076425016994228
-                },
-                {
-                    "x": 4.6815226569833905,
-                    "y": 4.625240515795651,
-                    "heading": 2.6597576260457285,
-                    "angularVelocity": 2.224188696537283e-7,
-                    "velocityX": -3.3427772769699504,
-                    "velocityY": 1.7710108366192054,
-                    "timestamp": 1.174281836720976
-                },
-                {
-                    "x": 4.378427967662734,
-                    "y": 4.837776808087965,
-                    "heading": 2.65975767630399,
-                    "angularVelocity": 5.135897752366777e-7,
-                    "velocityX": -3.097328220628949,
-                    "velocityY": 2.171910888641101,
-                    "timestamp": 1.272138656447724
-                },
-                {
-                    "x": 4.050949875066163,
-                    "y": 5.0103924451271915,
-                    "heading": 2.6597577704243744,
-                    "angularVelocity": 9.61817319833198e-7,
-                    "velocityX": -3.3465025075514396,
-                    "velocityY": 1.7639612397095272,
-                    "timestamp": 1.369995476174472
-                },
-                {
-                    "x": 3.722909272369472,
-                    "y": 5.181936673224133,
-                    "heading": 2.659757864941058,
-                    "angularVelocity": 9.65867108780206e-7,
-                    "velocityX": -3.352250804928058,
-                    "velocityY": 1.7530124990363987,
-                    "timestamp": 1.46785229590122
-                },
-                {
-                    "x": 3.383528958103272,
-                    "y": 5.329784787197771,
-                    "heading": 2.659757940242804,
-                    "angularVelocity": 7.6950943654844e-7,
-                    "velocityX": -3.468131451787155,
-                    "velocityY": 1.5108616281060843,
-                    "timestamp": 1.5657091156279679
-                },
-                {
-                    "x": 3.020395743291134,
-                    "y": 5.401703742215627,
-                    "heading": 2.6597579736142767,
-                    "angularVelocity": 3.4102347755826357e-7,
-                    "velocityX": -3.7108626238430706,
-                    "velocityY": 0.7349406532797214,
-                    "timestamp": 1.6635659353547159
-                },
-                {
-                    "x": 2.7114854008089204,
-                    "y": 5.466141485650731,
-                    "heading": 2.823345730065705,
-                    "angularVelocity": 1.6717052210385108,
-                    "velocityX": -3.156758449179158,
-                    "velocityY": 0.6584900634931665,
-                    "timestamp": 1.7614227550814638
-                },
-                {
-                    "x": 2.4804696774793777,
-                    "y": 5.513370093880283,
-                    "heading": 2.974776278106282,
-                    "angularVelocity": 1.5474705642736624,
-                    "velocityX": -2.3607524133179756,
-                    "velocityY": 0.48262970696813046,
-                    "timestamp": 1.8592795748082118
-                },
-                {
-                    "x": 2.3267564356504185,
-                    "y": 5.544523353838769,
-                    "heading": 3.083396984959406,
-                    "angularVelocity": 1.1099962900534985,
-                    "velocityX": -1.5707974391379447,
-                    "velocityY": 0.3183555325574442,
-                    "timestamp": 1.9571363945349598
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 0.5784268812194232,
-                    "velocityX": -0.7843749251687325,
-                    "velocityY": 0.15815603045805554,
-                    "timestamp": 2.054993214261708
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 3.14,
-                    "angularVelocity": 1.5606110446115462e-29,
-                    "velocityX": 0,
-                    "velocityY": 3.583632692315791e-31,
-                    "timestamp": 2.152850033988456
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -31508,7 +30742,7 @@
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -31517,7 +30751,7 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -31565,7 +30799,7 @@
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -31574,7 +30808,7 @@
                 {
                     "x": 4.263,
                     "y": 3,
-                    "heading": 2.574,
+                    "heading": -0.566,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -31622,7 +30856,7 @@
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -31631,159 +30865,14 @@
                 {
                     "x": 5.176,
                     "y": 1.62,
-                    "heading": 2.5,
+                    "heading": -0.64,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": -1.3041266298020504e-30,
-                    "velocityX": -3.538115329170266e-32,
-                    "velocityY": -5.678450231579595e-32,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.634416744984149,
-                    "y": 2.432986632964242,
-                    "heading": 3.1489798678517915,
-                    "angularVelocity": 0.11416974002165144,
-                    "velocityX": -0.5795439822048055,
-                    "velocityY": -0.3053051466642632,
-                    "timestamp": 0.07865365945555033
-                },
-                {
-                    "x": 7.543250241722733,
-                    "y": 2.384959916525659,
-                    "heading": 3.166940966282971,
-                    "angularVelocity": 0.22835680571645545,
-                    "velocityX": -1.1590878783324414,
-                    "velocityY": -0.6106100691440082,
-                    "timestamp": 0.15730731891110067
-                },
-                {
-                    "x": 7.406500507389113,
-                    "y": 2.312919874583363,
-                    "heading": 3.193885700427137,
-                    "angularVelocity": 0.34257445019952903,
-                    "velocityX": -1.7386315561185093,
-                    "velocityY": -0.9159146877712426,
-                    "timestamp": 0.235960978366651
-                },
-                {
-                    "x": 7.224167573256533,
-                    "y": 2.216866534550912,
-                    "heading": 3.2298175365571806,
-                    "angularVelocity": 0.45683616476039074,
-                    "velocityX": -2.3181748362976387,
-                    "velocityY": -1.2212189578634138,
-                    "timestamp": 0.31461463782220134
-                },
-                {
-                    "x": 6.996251494378262,
-                    "y": 2.0967999260644565,
-                    "heading": 3.2747409040255966,
-                    "angularVelocity": 0.5711541939609761,
-                    "velocityX": -2.8977174165313135,
-                    "velocityY": -1.5265228511625537,
-                    "timestamp": 0.3932682972777517
-                },
-                {
-                    "x": 6.733004702063821,
-                    "y": 1.9581188703597256,
-                    "heading": 3.2747409100209213,
-                    "angularVelocity": 7.622435906772787e-8,
-                    "velocityX": -3.3469109274338362,
-                    "velocityY": -1.7631863115422364,
-                    "timestamp": 0.47192195673330206
-                },
-                {
-                    "x": 6.469756571981624,
-                    "y": 1.819440354058334,
-                    "heading": 3.2747409160161256,
-                    "angularVelocity": 7.622282910230258e-8,
-                    "velocityX": -3.3469279357684036,
-                    "velocityY": -1.7631540256529774,
-                    "timestamp": 0.5505756161888524
-                },
-                {
-                    "x": 6.187670135263903,
-                    "y": 1.7247907391748545,
-                    "heading": 3.2747409182109637,
-                    "angularVelocity": 2.790510194041111e-8,
-                    "velocityX": -3.5864375373041195,
-                    "velocityY": -1.2033720431910602,
-                    "timestamp": 0.6292292756444028
-                },
-                {
-                    "x": 5.905373163009314,
-                    "y": 1.6806498188741994,
-                    "heading": 3.236681586369982,
-                    "angularVelocity": -0.48388507419022697,
-                    "velocityX": -3.5891142790899004,
-                    "velocityY": -0.561206186796692,
-                    "timestamp": 0.7078829350999531
-                },
-                {
-                    "x": 5.661530976081841,
-                    "y": 1.6531145750479088,
-                    "heading": 3.0865007795970714,
-                    "angularVelocity": -1.9093937626358353,
-                    "velocityX": -3.1002014224814998,
-                    "velocityY": -0.3500821705803985,
-                    "timestamp": 0.7865365945555035
-                },
-                {
-                    "x": 5.4654145585609575,
-                    "y": 1.6369706610279315,
-                    "heading": 2.879148606187945,
-                    "angularVelocity": -2.6362686090442846,
-                    "velocityX": -2.4934175838533594,
-                    "velocityY": -0.20525318378988838,
-                    "timestamp": 0.8651902540110539
-                },
-                {
-                    "x": 5.319930503793118,
-                    "y": 1.6274302419537983,
-                    "heading": 2.697897950588003,
-                    "angularVelocity": -2.304414783171942,
-                    "velocityX": -1.8496794144722244,
-                    "velocityY": -0.12129656954518526,
-                    "timestamp": 0.9438439134666042
-                },
-                {
-                    "x": 5.223761655113151,
-                    "y": 1.6221627992964307,
-                    "heading": 2.568061248027121,
-                    "angularVelocity": -1.6507395009924064,
-                    "velocityX": -1.2226875309509793,
-                    "velocityY": -0.0669700900610273,
-                    "timestamp": 1.0224975729221546
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": -0.8653284347892848,
-                    "velocityX": -0.6072400883031053,
-                    "velocityY": -0.027497758036964814,
-                    "timestamp": 1.1011512323777048
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": 0,
-                    "velocityX": -1.7743974032793004e-34,
-                    "velocityY": 1.752346488498292e-33,
-                    "timestamp": 1.179804891833255
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -31824,7 +30913,7 @@
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -31833,186 +30922,14 @@
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": 1.7868231110584375e-31,
-                    "velocityX": 3.500790942409583e-35,
-                    "velocityY": -4.724665555223456e-38,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.6800001156222075,
-                    "y": 2.5388441539517457,
-                    "heading": 3.140000017932032,
-                    "angularVelocity": 1.8099644459911076e-7,
-                    "velocityX": 0.0000011670295392084272,
-                    "velocityY": 0.8260917454273137,
-                    "timestamp": 0.09907392781101554
-                },
-                {
-                    "x": 7.680000338204962,
-                    "y": 2.7025324587418944,
-                    "heading": 3.1400000579849436,
-                    "angularVelocity": 4.042728942464055e-7,
-                    "velocityX": 0.000002246632826435788,
-                    "velocityY": 1.6521834594301972,
-                    "timestamp": 0.19814785562203108
-                },
-                {
-                    "x": 7.680000658923826,
-                    "y": 2.948064908143757,
-                    "heading": 3.1400001223157976,
-                    "angularVelocity": 6.493215714833696e-7,
-                    "velocityX": 0.0000032371668547106284,
-                    "velocityY": 2.4782751105841747,
-                    "timestamp": 0.2972217834330466
-                },
-                {
-                    "x": 7.6800010687648195,
-                    "y": 3.2754414834772896,
-                    "heading": 3.140000210384731,
-                    "angularVelocity": 8.889210892181502e-7,
-                    "velocityX": 0.0000041367184645969174,
-                    "velocityY": 3.3043665731916554,
-                    "timestamp": 0.39629571124406215
-                },
-                {
-                    "x": 7.6800013918601255,
-                    "y": 3.650232264926723,
-                    "heading": 3.140000210384734,
-                    "angularVelocity": 3.2741652225293424e-14,
-                    "velocityX": 0.000003261153797251531,
-                    "velocityY": 3.7829405750859055,
-                    "timestamp": 0.4953696390550777
-                },
-                {
-                    "x": 7.680001628786808,
-                    "y": 4.025023046376236,
-                    "heading": 3.140000210384741,
-                    "angularVelocity": 7.304626800025654e-14,
-                    "velocityX": 0.00000239141289972485,
-                    "velocityY": 3.782940575086779,
-                    "timestamp": 0.5944435668660932
-                },
-                {
-                    "x": 7.6800017795018,
-                    "y": 4.399813827825781,
-                    "heading": 3.1400002103847475,
-                    "angularVelocity": 7.220243449496737e-14,
-                    "velocityX": 0.0000015212375955225791,
-                    "velocityY": 3.782940575087123,
-                    "timestamp": 0.6935174946771088
-                },
-                {
-                    "x": 7.680001844060805,
-                    "y": 4.774604609275342,
-                    "heading": 3.140000210384754,
-                    "angularVelocity": 6.886052815923548e-14,
-                    "velocityX": 6.516245436906888e-7,
-                    "velocityY": 3.782940575087321,
-                    "timestamp": 0.7925914224881243
-                },
-                {
-                    "x": 7.680001822974652,
-                    "y": 5.14939539072491,
-                    "heading": 3.1400002103847604,
-                    "angularVelocity": 6.934745116017498e-14,
-                    "velocityX": -2.1283245913448782e-7,
-                    "velocityY": 3.782940575087379,
-                    "timestamp": 0.8916653502991398
-                },
-                {
-                    "x": 7.680001718396276,
-                    "y": 5.5241861721744705,
-                    "heading": 3.140000210384768,
-                    "angularVelocity": 8.52485669839133e-14,
-                    "velocityX": -0.0000010555588768470353,
-                    "velocityY": 3.7829405750872986,
-                    "timestamp": 0.9907392781101554
-                },
-                {
-                    "x": 7.680001534024895,
-                    "y": 5.898976953624011,
-                    "heading": 3.140000210384774,
-                    "angularVelocity": 6.71024495132872e-14,
-                    "velocityX": -0.000001860947354605527,
-                    "velocityY": 3.7829405750870757,
-                    "timestamp": 1.089813205921171
-                },
-                {
-                    "x": 7.680001275096042,
-                    "y": 6.273767735073519,
-                    "heading": 3.140000210384774,
-                    "angularVelocity": 2.6424584851850765e-16,
-                    "velocityX": -0.000002613491149837329,
-                    "velocityY": 3.7829405750867053,
-                    "timestamp": 1.1888871337321865
-                },
-                {
-                    "x": 7.680000948769078,
-                    "y": 6.648558516522932,
-                    "heading": 3.1400002103847724,
-                    "angularVelocity": -2.22727936711479e-14,
-                    "velocityX": -0.000003293772326272547,
-                    "velocityY": 3.782940575085685,
-                    "timestamp": 1.287961061543202
-                },
-                {
-                    "x": 7.680000579847807,
-                    "y": 6.975935091856332,
-                    "heading": 3.140000126724757,
-                    "angularVelocity": -8.444197945421442e-7,
-                    "velocityX": -0.0000037236965492526806,
-                    "velocityY": 3.3043665731903227,
-                    "timestamp": 1.3870349893542175
-                },
-                {
-                    "x": 7.680000295053659,
-                    "y": 7.221467541258139,
-                    "heading": 3.14000006325574,
-                    "angularVelocity": -6.40622649068188e-7,
-                    "velocityX": -0.000002874561832669146,
-                    "velocityY": 2.4782751105836276,
-                    "timestamp": 1.486108917165233
-                },
-                {
-                    "x": 7.680000100017398,
-                    "y": 7.385155846048263,
-                    "heading": 3.14000002096013,
-                    "angularVelocity": -4.269095158176857e-7,
-                    "velocityX": -0.000001968593124089932,
-                    "velocityY": 1.6521834594299558,
-                    "timestamp": 1.5851828449762486
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 3.14,
-                    "angularVelocity": -2.115604748661088e-7,
-                    "velocityX": -0.0000010095228578568797,
-                    "velocityY": 0.8260917454272356,
-                    "timestamp": 1.6842567727872642
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 3.14,
-                    "angularVelocity": 8.944588154710766e-32,
-                    "velocityX": 0,
-                    "velocityY": -4.821363548294539e-38,
-                    "timestamp": 1.7833307005982797
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -32053,7 +30970,7 @@
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -32062,150 +30979,14 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": -9.54519608761012e-33,
-                    "velocityX": 3.049131565114831e-33,
-                    "velocityY": -1.5169993504833503e-38,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.68000007508226,
-                    "y": 2.533445372005505,
-                    "heading": 3.1399999978468434,
-                    "angularVelocity": -2.248715497169588e-8,
-                    "velocityX": 7.84144633669789e-7,
-                    "velocityY": 0.7983807350943783,
-                    "timestamp": 0.09575052183150116
-                },
-                {
-                    "x": 7.680000217378094,
-                    "y": 2.6863361129826027,
-                    "heading": 3.1399999975552166,
-                    "angularVelocity": -3.0456922527341925e-9,
-                    "velocityX": 0.0000014861101970902313,
-                    "velocityY": 1.5967614385031632,
-                    "timestamp": 0.19150104366300233
-                },
-                {
-                    "x": 7.680000418878438,
-                    "y": 2.915672216863469,
-                    "heading": 3.1400000014113987,
-                    "angularVelocity": 4.027321769334868e-8,
-                    "velocityX": 0.000002104430661113429,
-                    "velocityY": 2.395142078540767,
-                    "timestamp": 0.2872515654945035
-                },
-                {
-                    "x": 7.680000671408876,
-                    "y": 3.2214536654446886,
-                    "heading": 3.14000000940269,
-                    "angularVelocity": 8.345949630994641e-8,
-                    "velocityX": 0.0000026373790047185906,
-                    "velocityY": 3.1935225284654267,
-                    "timestamp": 0.38300208732600466
-                },
-                {
-                    "x": 7.680000833444467,
-                    "y": 3.5836721992668776,
-                    "heading": 3.1400000094026845,
-                    "angularVelocity": -5.801733416228813e-14,
-                    "velocityX": 0.0000016922687336334932,
-                    "velocityY": 3.7829405719543456,
-                    "timestamp": 0.47875260915750584
-                },
-                {
-                    "x": 7.680000914400183,
-                    "y": 3.9458907330889588,
-                    "heading": 3.140000009402682,
-                    "angularVelocity": -2.5236519267762366e-14,
-                    "velocityX": 8.45485885682039e-7,
-                    "velocityY": 3.78294057195325,
-                    "timestamp": 0.574503130989007
-                },
-                {
-                    "x": 7.68000091414698,
-                    "y": 4.308109266911049,
-                    "heading": 3.1400000094026788,
-                    "angularVelocity": -3.379642410236896e-14,
-                    "velocityX": -2.644395591425324e-9,
-                    "velocityY": 3.782940571953342,
-                    "timestamp": 0.6702536528205081
-                },
-                {
-                    "x": 7.680000832736705,
-                    "y": 4.67032780073313,
-                    "heading": 3.140000009402675,
-                    "angularVelocity": -3.7410067203640955e-14,
-                    "velocityX": -8.502332108268853e-7,
-                    "velocityY": 3.782940571953249,
-                    "timestamp": 0.7660041746520092
-                },
-                {
-                    "x": 7.680000670417917,
-                    "y": 5.032546334555316,
-                    "heading": 3.1400000094026708,
-                    "angularVelocity": -4.92147091530942e-14,
-                    "velocityX": -0.0000016952263834753921,
-                    "velocityY": 3.7829405719543154,
-                    "timestamp": 0.8617546964835103
-                },
-                {
-                    "x": 7.680000418190559,
-                    "y": 5.338327783136532,
-                    "heading": 3.1400000097695924,
-                    "angularVelocity": 3.832069362065189e-9,
-                    "velocityX": -0.00000263421371002401,
-                    "velocityY": 3.1935225284653987,
-                    "timestamp": 0.9575052183150115
-                },
-                {
-                    "x": 7.68000021698581,
-                    "y": 5.567663887017397,
-                    "heading": 3.140000007984905,
-                    "angularVelocity": -1.863892342341337e-8,
-                    "velocityX": -0.0000021013435063173737,
-                    "velocityY": 2.3951420785407556,
-                    "timestamp": 1.0532557401465126
-                },
-                {
-                    "x": 7.680000074934953,
-                    "y": 5.7205546279944945,
-                    "heading": 3.140000003983309,
-                    "angularVelocity": -4.1791894261559745e-8,
-                    "velocityX": -0.0000014835517222041492,
-                    "velocityY": 1.5967614385031577,
-                    "timestamp": 1.1490062619780137
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 3.14,
-                    "angularVelocity": -4.160090541448797e-8,
-                    "velocityX": -7.826061846711994e-7,
-                    "velocityY": 0.7983807350943762,
-                    "timestamp": 1.2447567838095148
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": 3.04245898705875e-32,
-                    "velocityY": 2.558785218352301e-38,
-                    "timestamp": 1.340507305641016
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -32246,7 +31027,7 @@
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -32255,105 +31036,14 @@
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": -3.7532975982701904e-32,
-                    "velocityY": 5.0637303451538295e-36,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.680000036889488,
-                    "y": 2.540500005575837,
-                    "heading": 3.1400000022500048,
-                    "angularVelocity": 2.2484053179234407e-8,
-                    "velocityX": 3.6863266642951275e-7,
-                    "velocityY": 0.8344065481227464,
-                    "timestamp": 0.1000711293118395
-                },
-                {
-                    "x": 7.680000101870814,
-                    "y": 2.707500013629824,
-                    "heading": 3.140000010932795,
-                    "angularVelocity": 8.676618158835649e-8,
-                    "velocityX": 6.49351381942043e-7,
-                    "velocityY": 1.6688130652906412,
-                    "timestamp": 0.200142258623679
-                },
-                {
-                    "x": 7.680000185978468,
-                    "y": 2.958000017966584,
-                    "heading": 3.1400000281272393,
-                    "angularVelocity": 1.7182222560804709e-7,
-                    "velocityX": 8.404786987428781e-7,
-                    "velocityY": 2.503219520548802,
-                    "timestamp": 0.3002133879355185
-                },
-                {
-                    "x": 7.68000028005223,
-                    "y": 3.2919999999999923,
-                    "heading": 3.1400000531214163,
-                    "angularVelocity": 2.497641049380253e-7,
-                    "velocityX": 9.400689316139974e-7,
-                    "velocityY": 3.3376257900778246,
-                    "timestamp": 0.400284517247358
-                },
-                {
-                    "x": 7.680000187136683,
-                    "y": 3.6259999820334055,
-                    "heading": 3.140000035091715,
-                    "angularVelocity": -1.801688496007097e-7,
-                    "velocityX": -9.284950198395514e-7,
-                    "velocityY": 3.337625790077875,
-                    "timestamp": 0.5003556465591975
-                },
-                {
-                    "x": 7.680000103123094,
-                    "y": 3.8764999863701703,
-                    "heading": 3.1400000189895447,
-                    "angularVelocity": -1.609072425985215e-7,
-                    "velocityX": -8.395387126580118e-7,
-                    "velocityY": 2.5032195205488503,
-                    "timestamp": 0.600426775871037
-                },
-                {
-                    "x": 7.680000037559297,
-                    "y": 4.043499994424161,
-                    "heading": 3.140000006758861,
-                    "angularVelocity": -1.222199010476452e-7,
-                    "velocityX": -6.55171951368837e-7,
-                    "velocityY": 1.6688130652906763,
-                    "timestamp": 0.7004979051828765
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": -6.754056559007022e-8,
-                    "velocityX": -3.753259946110928e-7,
-                    "velocityY": 0.8344065481227645,
-                    "timestamp": 0.800569034494716
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.14,
-                    "angularVelocity": 3.7078876706224725e-32,
-                    "velocityX": 0,
-                    "velocityY": -1.600749389553648e-35,
-                    "timestamp": 0.9006401638065554
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -32394,7 +31084,7 @@
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -32403,7 +31093,7 @@
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -32451,7 +31141,7 @@
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -32460,276 +31150,14 @@
                 {
                     "x": 2.3,
                     "y": 6.757,
-                    "heading": 3.5331853071795862,
+                    "heading": 0.393,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": -1.8495960722802552e-34,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.66217209907118,
-                    "y": 0.8633707808076446,
-                    "heading": 3.200268562588625,
-                    "angularVelocity": 0.6146717037531945,
-                    "velocityX": -0.18182458262791865,
-                    "velocityY": 0.7788962593386439,
-                    "timestamp": 0.09805000331172546
-                },
-                {
-                    "x": 7.626250868838738,
-                    "y": 1.0164003464766511,
-                    "heading": 3.3145470628036264,
-                    "angularVelocity": 1.1655124564522605,
-                    "velocityX": -0.3663562368094907,
-                    "velocityY": 1.5607298368209865,
-                    "timestamp": 0.19610000662345092
-                },
-                {
-                    "x": 7.571819523432209,
-                    "y": 1.2466141084056221,
-                    "heading": 3.4692056792172115,
-                    "angularVelocity": 1.5773443262606215,
-                    "velocityX": -0.555138639143922,
-                    "velocityY": 2.347922020941335,
-                    "timestamp": 0.29415000993517637
-                },
-                {
-                    "x": 7.4978548812850345,
-                    "y": 1.55456588878344,
-                    "heading": 3.620198528248394,
-                    "angularVelocity": 1.5399576127615076,
-                    "velocityX": -0.7543563452213444,
-                    "velocityY": 3.1407625698773547,
-                    "timestamp": 0.39220001324690185
-                },
-                {
-                    "x": 7.414564781883039,
-                    "y": 1.916010778899352,
-                    "heading": 3.6201985877633485,
-                    "angularVelocity": 6.069857444827113e-7,
-                    "velocityX": -0.8494655439959167,
-                    "velocityY": 3.6863322581110816,
-                    "timestamp": 0.4902500165586273
-                },
-                {
-                    "x": 7.3312746982625,
-                    "y": 2.27745567265221,
-                    "heading": 3.620198647277482,
-                    "angularVelocity": 6.069773758970038e-7,
-                    "velocityX": -0.8494653830427515,
-                    "velocityY": 3.686332295203854,
-                    "timestamp": 0.5883000198703527
-                },
-                {
-                    "x": 7.247984475288188,
-                    "y": 2.6389005342928713,
-                    "heading": 3.6201987067915797,
-                    "angularVelocity": 6.069770050551867e-7,
-                    "velocityX": -0.8494668042948591,
-                    "velocityY": 3.686331967695479,
-                    "timestamp": 0.6863500231820782
-                },
-                {
-                    "x": 7.16025127777078,
-                    "y": 2.999292730685755,
-                    "heading": 3.620198766578442,
-                    "angularVelocity": 6.097589007693709e-7,
-                    "velocityX": -0.8947801586348038,
-                    "velocityY": 3.675595963491274,
-                    "timestamp": 0.7844000264938036
-                },
-                {
-                    "x": 6.997140841431304,
-                    "y": 3.3324212817743484,
-                    "heading": 3.6201988348555445,
-                    "angularVelocity": 6.963498230184926e-7,
-                    "velocityX": -1.6635434046943107,
-                    "velocityY": 3.3975373772247037,
-                    "timestamp": 0.882450029805529
-                },
-                {
-                    "x": 6.766266862430273,
-                    "y": 3.622726019325249,
-                    "heading": 3.620198931073779,
-                    "angularVelocity": 9.813180151910862e-7,
-                    "velocityX": -2.354655494166845,
-                    "velocityY": 2.9607825369260774,
-                    "timestamp": 0.9805000331172544
-                },
-                {
-                    "x": 6.478412432979381,
-                    "y": 3.856647861556034,
-                    "heading": 3.62019912006771,
-                    "angularVelocity": 0.000001927525998631886,
-                    "velocityX": -2.935792144093369,
-                    "velocityY": 2.38574027873399,
-                    "timestamp": 1.07855003642898
-                },
-                {
-                    "x": 6.147022412583985,
-                    "y": 4.023260998419484,
-                    "heading": 3.6202003231426523,
-                    "angularVelocity": 0.000012270014292625344,
-                    "velocityX": -3.3798063151698643,
-                    "velocityY": 1.6992670192345174,
-                    "timestamp": 1.1766000397407055
-                },
-                {
-                    "x": 5.787574046048744,
-                    "y": 4.114783912760558,
-                    "heading": 3.6201999548728017,
-                    "angularVelocity": -0.0000037559391933502495,
-                    "velocityX": -3.6659699581290637,
-                    "velocityY": 0.9334310173361262,
-                    "timestamp": 1.274650043052431
-                },
-                {
-                    "x": 5.417217638191157,
-                    "y": 4.135174501302961,
-                    "heading": 3.620199996859582,
-                    "angularVelocity": 4.2821804063172526e-7,
-                    "velocityX": -3.7772197383831925,
-                    "velocityY": 0.2079611203844201,
-                    "timestamp": 1.3727000463641565
-                },
-                {
-                    "x": 5.066383608065517,
-                    "y": 4.23317852630269,
-                    "heading": 3.636482368230358,
-                    "angularVelocity": 0.16606191556170027,
-                    "velocityX": -3.578113393940955,
-                    "velocityY": 0.9995310728154599,
-                    "timestamp": 1.470750049675882
-                },
-                {
-                    "x": 4.742245396435021,
-                    "y": 4.406733140775732,
-                    "heading": 3.644586480575013,
-                    "angularVelocity": 0.08265285130985205,
-                    "velocityX": -3.305846003900474,
-                    "velocityY": 1.7700623009799354,
-                    "timestamp": 1.5688000529876076
-                },
-                {
-                    "x": 4.460670694136194,
-                    "y": 4.648177372257919,
-                    "heading": 3.644586687046199,
-                    "angularVelocity": 0.000002105774391224538,
-                    "velocityX": -2.87174597438442,
-                    "velocityY": 2.4624602073145714,
-                    "timestamp": 1.6668500562993331
-                },
-                {
-                    "x": 4.237545979665016,
-                    "y": 4.944479621179968,
-                    "heading": 3.644586791013843,
-                    "angularVelocity": 0.0000010603532960842864,
-                    "velocityX": -2.2756216923502683,
-                    "velocityY": 3.0219504223781635,
-                    "timestamp": 1.7649000596110587
-                },
-                {
-                    "x": 3.9950087307179025,
-                    "y": 5.225113475172281,
-                    "heading": 3.6445867953690434,
-                    "angularVelocity": 4.441815614419627e-8,
-                    "velocityX": -2.473607758849603,
-                    "velocityY": 2.862150377497781,
-                    "timestamp": 1.8629500629227842
-                },
-                {
-                    "x": 3.7198234606165768,
-                    "y": 5.473815593691706,
-                    "heading": 3.644586791690717,
-                    "angularVelocity": -3.7514802268686446e-8,
-                    "velocityX": -2.8065809363253424,
-                    "velocityY": 2.536482510140642,
-                    "timestamp": 1.9610000662345097
-                },
-                {
-                    "x": 3.4446380366180587,
-                    "y": 5.722517541926013,
-                    "heading": 3.6445867880123934,
-                    "angularVelocity": -3.751477003592028e-8,
-                    "velocityX": -2.8065825059040024,
-                    "velocityY": 2.5364807734235555,
-                    "timestamp": 2.059050069546235
-                },
-                {
-                    "x": 3.1694525996169167,
-                    "y": 5.97121947577308,
-                    "heading": 3.6445867843340722,
-                    "angularVelocity": -3.75147461397837e-8,
-                    "velocityX": -2.806582638516174,
-                    "velocityY": 2.5364806266898405,
-                    "timestamp": 2.1571000728579603
-                },
-                {
-                    "x": 2.894267154657697,
-                    "y": 6.2199214008146,
-                    "heading": 3.6445867806557373,
-                    "angularVelocity": -3.751489444763293e-8,
-                    "velocityX": -2.806582719679637,
-                    "velocityY": 2.53648053688316,
-                    "timestamp": 2.2551500761696857
-                },
-                {
-                    "x": 2.6565603153930746,
-                    "y": 6.434752825744944,
-                    "heading": 3.60002452914154,
-                    "angularVelocity": -0.45448495674724715,
-                    "velocityX": -2.4243430008757167,
-                    "velocityY": 2.191039445938014,
-                    "timestamp": 2.353200079481411
-                },
-                {
-                    "x": 2.478280163108621,
-                    "y": 6.595876410073326,
-                    "heading": 3.56660458464388,
-                    "angularVelocity": -0.3408459293102701,
-                    "velocityX": -1.8182574835582244,
-                    "velocityY": 1.6432797438683373,
-                    "timestamp": 2.4512500827931363
-                },
-                {
-                    "x": 2.3594267213059505,
-                    "y": 6.703292136714203,
-                    "heading": 3.544325120911587,
-                    "angularVelocity": -0.2272255275857591,
-                    "velocityX": -1.2121717265506466,
-                    "velocityY": 1.0955198675452942,
-                    "timestamp": 2.5493000861048616
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 3.5331853071795862,
-                    "angularVelocity": -0.1136135987327294,
-                    "velocityX": -0.6060858674019433,
-                    "velocityY": 0.5477599334193345,
-                    "timestamp": 2.647350089416587
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 3.5331853071795862,
-                    "angularVelocity": 0,
-                    "velocityX": -1.9773011985788902e-32,
-                    "velocityY": -8.72670829182521e-33,
-                    "timestamp": 2.745400092728312
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -32770,7 +31198,7 @@
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -32779,7 +31207,7 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -32827,7 +31255,7 @@
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -32836,7 +31264,7 @@
                 {
                     "x": 2.3,
                     "y": 4.36,
-                    "heading": 2.75,
+                    "heading": -0.39,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -32884,7 +31312,7 @@
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -32893,195 +31321,14 @@
                 {
                     "x": 4.263,
                     "y": 3,
-                    "heading": 2.574,
+                    "heading": -0.566,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 3.14,
-                    "angularVelocity": 9.969319932712997e-31,
-                    "velocityX": 3.5015968931749707e-32,
-                    "velocityY": -1.3449299836267862e-31,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.622998015019161,
-                    "y": 0.8057692963239994,
-                    "heading": 3.0732063030265673,
-                    "angularVelocity": -0.7701918865802806,
-                    "velocityX": -0.6572845693610088,
-                    "velocityY": 0.21642700435214937,
-                    "timestamp": 0.08672344923029876
-                },
-                {
-                    "x": 7.508611650111109,
-                    "y": 0.8437411084542301,
-                    "heading": 2.9459376863662614,
-                    "angularVelocity": -1.4675225419406142,
-                    "velocityX": -1.318978499163396,
-                    "velocityY": 0.43784942212566424,
-                    "timestamp": 0.17344689846059752
-                },
-                {
-                    "x": 7.336139052754301,
-                    "y": 0.9014260823029367,
-                    "heading": 2.7711998622663616,
-                    "angularVelocity": -2.014885543076985,
-                    "velocityX": -1.9887654248944688,
-                    "velocityY": 0.6651600502595449,
-                    "timestamp": 0.2601703476908963
-                },
-                {
-                    "x": 7.104576647516153,
-                    "y": 0.9793814888777497,
-                    "heading": 2.5768200425700405,
-                    "angularVelocity": -2.241375561298711,
-                    "velocityX": -2.6701244852844987,
-                    "velocityY": 0.8988965183776096,
-                    "timestamp": 0.34689379692119504
-                },
-                {
-                    "x": 6.822163575098835,
-                    "y": 1.078219287033468,
-                    "heading": 2.4981665441447336,
-                    "angularVelocity": -0.9069461503593836,
-                    "velocityX": -3.2564787831184523,
-                    "velocityY": 1.1396894269420643,
-                    "timestamp": 0.4336172461514938
-                },
-                {
-                    "x": 6.510888238397409,
-                    "y": 1.1818405008088049,
-                    "heading": 2.498166488214478,
-                    "angularVelocity": -6.449265521718259e-7,
-                    "velocityX": -3.5892868591379226,
-                    "velocityY": 1.194846546061207,
-                    "timestamp": 0.5203406953817925
-                },
-                {
-                    "x": 6.199612902811833,
-                    "y": 1.2854617179365646,
-                    "heading": 2.498166432284442,
-                    "angularVelocity": -6.449240327985389e-7,
-                    "velocityX": -3.589286846271159,
-                    "velocityY": 1.194846584717682,
-                    "timestamp": 0.6070641446120912
-                },
-                {
-                    "x": 5.8883375891897005,
-                    "y": 1.389083001041933,
-                    "heading": 2.4981663763544435,
-                    "angularVelocity": -6.449235898174935e-7,
-                    "velocityX": -3.5892865930127487,
-                    "velocityY": 1.1948473454993283,
-                    "timestamp": 0.69378759384239
-                },
-                {
-                    "x": 5.577064983818878,
-                    "y": 1.4927124193076864,
-                    "heading": 2.4981663204239246,
-                    "angularVelocity": -6.449295917693127e-7,
-                    "velocityX": -3.589255364419621,
-                    "velocityY": 1.194941151274556,
-                    "timestamp": 0.7805110430726887
-                },
-                {
-                    "x": 5.275080475665308,
-                    "y": 1.6209116670289339,
-                    "heading": 2.4981662550669967,
-                    "angularVelocity": -7.536246362770858e-7,
-                    "velocityX": -3.4821551821772507,
-                    "velocityY": 1.4782535618573864,
-                    "timestamp": 0.8672344923029874
-                },
-                {
-                    "x": 4.987205237443244,
-                    "y": 1.7782569230783876,
-                    "heading": 2.49816614132643,
-                    "angularVelocity": -0.000001311531862273131,
-                    "velocityX": -3.319462507281,
-                    "velocityY": 1.8143334639702267,
-                    "timestamp": 0.9539579415332862
-                },
-                {
-                    "x": 4.728299565339017,
-                    "y": 1.9797451797609154,
-                    "heading": 2.498165933556362,
-                    "angularVelocity": -0.0000023957772582798155,
-                    "velocityX": -2.9854171438302592,
-                    "velocityY": 2.3233422848238554,
-                    "timestamp": 1.040681390763585
-                },
-                {
-                    "x": 4.523329416895473,
-                    "y": 2.207660623196326,
-                    "heading": 2.432888839209935,
-                    "angularVelocity": -0.7527040832183707,
-                    "velocityX": -2.3634916537882953,
-                    "velocityY": 2.6280717090733883,
-                    "timestamp": 1.1274048399938839
-                },
-                {
-                    "x": 4.381013836981984,
-                    "y": 2.433217084695142,
-                    "heading": 2.3632823552491717,
-                    "angularVelocity": -0.8026258708405342,
-                    "velocityX": -1.6410276710231155,
-                    "velocityY": 2.600870508503859,
-                    "timestamp": 1.2141282892241827
-                },
-                {
-                    "x": 4.294783443237854,
-                    "y": 2.6401553266407616,
-                    "heading": 2.3696134751420326,
-                    "angularVelocity": 0.07300355266138053,
-                    "velocityX": -0.9943146232011729,
-                    "velocityY": 2.386185556297287,
-                    "timestamp": 1.3008517384544815
-                },
-                {
-                    "x": 4.26053013180991,
-                    "y": 2.816849773520389,
-                    "heading": 2.4414538337655816,
-                    "angularVelocity": 0.8283844711108445,
-                    "velocityX": -0.39497173753989284,
-                    "velocityY": 2.0374471777570338,
-                    "timestamp": 1.3875751876847804
-                },
-                {
-                    "x": 4.257469858369506,
-                    "y": 2.9392038098412794,
-                    "heading": 2.522566294218638,
-                    "angularVelocity": 0.9353002120298306,
-                    "velocityX": -0.035287727455096035,
-                    "velocityY": 1.4108529746778473,
-                    "timestamp": 1.4742986369150792
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": 0.5930772615463757,
-                    "velocityX": 0.06376754706571511,
-                    "velocityY": 0.7010351951900964,
-                    "timestamp": 1.561022086145378
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": 2.574,
-                    "angularVelocity": -5.347216163770528e-32,
-                    "velocityX": 0,
-                    "velocityY": -5.6953276102603335e-33,
-                    "timestamp": 1.647745535375677
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -33122,7 +31369,7 @@
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -33131,159 +31378,14 @@
                 {
                     "x": 5.176,
                     "y": 1.62,
-                    "heading": 2.5,
+                    "heading": -0.64,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 3.14,
-                    "angularVelocity": 7.117315018424497e-31,
-                    "velocityX": 3.987995408855834e-32,
-                    "velocityY": -2.0629863005203966e-31,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.633523057017953,
-                    "y": 0.8024232011655344,
-                    "heading": 3.1133262335807257,
-                    "angularVelocity": -0.3460902136380606,
-                    "velocityX": -0.6030350147430711,
-                    "velocityY": 0.20011493324412974,
-                    "timestamp": 0.07707171531631223
-                },
-                {
-                    "x": 7.540480034234958,
-                    "y": 0.8333108599665054,
-                    "heading": 3.0624083977372165,
-                    "angularVelocity": -0.6606552823501594,
-                    "velocityX": -1.2072265733432221,
-                    "velocityY": 0.4007651662377571,
-                    "timestamp": 0.15414343063262445
-                },
-                {
-                    "x": 7.40074665357272,
-                    "y": 0.8797155912265175,
-                    "heading": 2.9910865272302494,
-                    "angularVelocity": -0.9253961743845064,
-                    "velocityX": -1.813030631130417,
-                    "velocityY": 0.6020980728086949,
-                    "timestamp": 0.23121514594893666
-                },
-                {
-                    "x": 7.214152290317413,
-                    "y": 0.941701209252225,
-                    "heading": 2.9061891700546396,
-                    "angularVelocity": -1.101537144037597,
-                    "velocityX": -2.4210485323896127,
-                    "velocityY": 0.804258965449392,
-                    "timestamp": 0.3082868612652489
-                },
-                {
-                    "x": 6.98056152298191,
-                    "y": 1.0193157827445936,
-                    "heading": 2.8228339071694943,
-                    "angularVelocity": -1.0815285807905615,
-                    "velocityX": -3.0308235177693503,
-                    "velocityY": 1.007043546050951,
-                    "timestamp": 0.38535857658156114
-                },
-                {
-                    "x": 6.704462217906169,
-                    "y": 1.1110743550526192,
-                    "heading": 2.820724227783145,
-                    "angularVelocity": -0.027372939316202514,
-                    "velocityX": -3.582368758014451,
-                    "velocityY": 1.1905609201953868,
-                    "timestamp": 0.4624302918978734
-                },
-                {
-                    "x": 6.4277914338210325,
-                    "y": 1.2030481222509408,
-                    "heading": 2.8207241988506935,
-                    "angularVelocity": -3.753964944317793e-7,
-                    "velocityX": -3.589783657333227,
-                    "velocityY": 1.1933530585228214,
-                    "timestamp": 0.5395020072141856
-                },
-                {
-                    "x": 6.151120668434435,
-                    "y": 1.2950219456972574,
-                    "heading": 2.8207241699182544,
-                    "angularVelocity": -3.753963293976773e-7,
-                    "velocityX": -3.5897834147210324,
-                    "velocityY": 1.1933537883365417,
-                    "timestamp": 0.6165737225304978
-                },
-                {
-                    "x": 5.875226516165077,
-                    "y": 1.387237765393963,
-                    "heading": 2.8184428738821614,
-                    "angularVelocity": -0.029599653085833715,
-                    "velocityX": -3.5797069150083383,
-                    "velocityY": 1.1964936723964186,
-                    "timestamp": 0.69364543784681
-                },
-                {
-                    "x": 5.641686532929837,
-                    "y": 1.4649564505690207,
-                    "heading": 2.7347434741228605,
-                    "angularVelocity": -1.0859937321465907,
-                    "velocityX": -3.0301645976966087,
-                    "velocityY": 1.0083943876958055,
-                    "timestamp": 0.7707171531631222
-                },
-                {
-                    "x": 5.455142642963306,
-                    "y": 1.5270452896537086,
-                    "heading": 2.6494963329119967,
-                    "angularVelocity": -1.1060755669054276,
-                    "velocityX": -2.4203936450737,
-                    "velocityY": 0.8055982513152498,
-                    "timestamp": 0.8477888684794344
-                },
-                {
-                    "x": 5.315456646631149,
-                    "y": 1.5735510161760224,
-                    "heading": 2.577919230350482,
-                    "angularVelocity": -0.928707792057741,
-                    "velocityX": -1.812415822832897,
-                    "velocityY": 0.6034084791216642,
-                    "timestamp": 0.9248605837957465
-                },
-                {
-                    "x": 5.222452869215715,
-                    "y": 1.6045247487520136,
-                    "heading": 2.5268085106209885,
-                    "angularVelocity": -0.6631579369906109,
-                    "velocityX": -1.2067173675029015,
-                    "velocityY": 0.4018819673192824,
-                    "timestamp": 1.0019322991120587
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": -0.3478385100287815,
-                    "velocityX": -0.6027226593448243,
-                    "velocityY": 0.200790279345336,
-                    "timestamp": 1.079004014428371
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": 2.5,
-                    "angularVelocity": -3.134288783198311e-31,
-                    "velocityX": -2.9694883024269136e-32,
-                    "velocityY": 4.283237987843841e-31,
-                    "timestamp": 1.1560757297446833
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -33324,7 +31426,7 @@
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -33333,231 +31435,14 @@
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 3.14,
-                    "angularVelocity": 3.6158431875986385e-31,
-                    "velocityX": 5.569083161704914e-32,
-                    "velocityY": 2.1158895574058437e-37,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.680000134363673,
-                    "y": 0.8649552704871222,
-                    "heading": 3.140000000380936,
-                    "angularVelocity": 3.939689465162077e-9,
-                    "velocityX": 0.0000013896119285392994,
-                    "velocityY": 0.8062267267090596,
-                    "timestamp": 0.09669149868712118
-                },
-                {
-                    "x": 7.680000398315331,
-                    "y": 1.0208658083667637,
-                    "heading": 3.14000000095338,
-                    "angularVelocity": 5.920286837104207e-9,
-                    "velocityX": 0.0000027298328198052245,
-                    "velocityY": 1.6124534214132247,
-                    "timestamp": 0.19338299737424236
-                },
-                {
-                    "x": 7.680000786842507,
-                    "y": 1.2547316074497201,
-                    "heading": 3.1400000020167984,
-                    "angularVelocity": 1.0998009739093907e-8,
-                    "velocityX": 0.000004018213888590017,
-                    "velocityY": 2.4186800521075957,
-                    "timestamp": 0.29007449606136354
-                },
-                {
-                    "x": 7.680001294600068,
-                    "y": 1.566552649168411,
-                    "heading": 3.140000004611078,
-                    "angularVelocity": 2.6830422686000607e-8,
-                    "velocityX": 0.00000525131448771192,
-                    "velocityY": 3.2249064907729323,
-                    "timestamp": 0.3867659947484847
-                },
-                {
-                    "x": 7.680001797129468,
-                    "y": 1.9323308421441416,
-                    "heading": 3.1400000046110796,
-                    "angularVelocity": 2.2904858507221613e-14,
-                    "velocityX": 0.000005197244743376452,
-                    "velocityY": 3.782940568118395,
-                    "timestamp": 0.4834574934356059
-                },
-                {
-                    "x": 7.680002234677101,
-                    "y": 2.2981090351200577,
-                    "heading": 3.1400000046110854,
-                    "angularVelocity": 7.187274756998544e-14,
-                    "velocityX": 0.00000452519161992871,
-                    "velocityY": 3.782940568120504,
-                    "timestamp": 0.5801489921227271
-                },
-                {
-                    "x": 7.680002600262594,
-                    "y": 2.663887228096014,
-                    "heading": 3.140000004611095,
-                    "angularVelocity": 1.2305801027843294e-13,
-                    "velocityX": 0.000003780946939519626,
-                    "velocityY": 3.782940568121035,
-                    "timestamp": 0.6768404908098482
-                },
-                {
-                    "x": 7.680002888746174,
-                    "y": 3.029665421071995,
-                    "heading": 3.1400000046111063,
-                    "angularVelocity": 1.4680337346800463e-13,
-                    "velocityX": 0.000002983545883280996,
-                    "velocityY": 3.782940568121397,
-                    "timestamp": 0.7735319894969694
-                },
-                {
-                    "x": 7.680003096449986,
-                    "y": 3.395443614047991,
-                    "heading": 3.1400000046111183,
-                    "angularVelocity": 1.4951605959877598e-13,
-                    "velocityX": 0.0000021481078702221435,
-                    "velocityY": 3.782940568121633,
-                    "timestamp": 0.8702234881840907
-                },
-                {
-                    "x": 7.680003221115393,
-                    "y": 3.7612218070239956,
-                    "heading": 3.1400000046111316,
-                    "angularVelocity": 1.5542579572657036e-13,
-                    "velocityX": 0.0000012893106851068564,
-                    "velocityY": 3.7829405681217785,
-                    "timestamp": 0.9669149868712119
-                },
-                {
-                    "x": 7.68000326213119,
-                    "y": 4.127000000000005,
-                    "heading": 3.1400000046111454,
-                    "angularVelocity": 1.6157627281901116e-13,
-                    "velocityX": 4.241922977706735e-7,
-                    "velocityY": 3.782940568121862,
-                    "timestamp": 1.0636064855583331
-                },
-                {
-                    "x": 7.6800032195335275,
-                    "y": 4.492778192976015,
-                    "heading": 3.140000004611159,
-                    "angularVelocity": 1.571537708778215e-13,
-                    "velocityX": -4.4055220975717935e-7,
-                    "velocityY": 3.7829405681218624,
-                    "timestamp": 1.1602979842454544
-                },
-                {
-                    "x": 7.680003093394937,
-                    "y": 4.858556385952021,
-                    "heading": 3.1400000046111707,
-                    "angularVelocity": 1.4279777513481356e-13,
-                    "velocityX": -0.00000130454658943142,
-                    "velocityY": 3.7829405681217825,
-                    "timestamp": 1.2569894829325756
-                },
-                {
-                    "x": 7.680002884476087,
-                    "y": 5.224334578928017,
-                    "heading": 3.1400000046111822,
-                    "angularVelocity": 1.3985874807595513e-13,
-                    "velocityX": -0.0000021606740038572212,
-                    "velocityY": 3.7829405681216364,
-                    "timestamp": 1.3536809816196969
-                },
-                {
-                    "x": 7.680002595166353,
-                    "y": 5.590112771903999,
-                    "heading": 3.140000004611193,
-                    "angularVelocity": 1.3691025375631502e-13,
-                    "velocityX": -0.0000029920901128748315,
-                    "velocityY": 3.7829405681213992,
-                    "timestamp": 1.450372480306818
-                },
-                {
-                    "x": 7.680002229238247,
-                    "y": 5.955890964879955,
-                    "heading": 3.1400000046112018,
-                    "angularVelocity": 1.1312968357611626e-13,
-                    "velocityX": -0.000003784490284850132,
-                    "velocityY": 3.782940568121032,
-                    "timestamp": 1.5470639789939393
-                },
-                {
-                    "x": 7.680001791901768,
-                    "y": 6.321669157855871,
-                    "heading": 3.1400000046112067,
-                    "angularVelocity": 6.287792233292802e-14,
-                    "velocityX": -0.000004523007841510769,
-                    "velocityY": 3.7829405681204946,
-                    "timestamp": 1.6437554776810606
-                },
-                {
-                    "x": 7.680001290195951,
-                    "y": 6.687447350831601,
-                    "heading": 3.1400000046112084,
-                    "angularVelocity": 2.135816498669109e-14,
-                    "velocityX": -0.00000518872709755724,
-                    "velocityY": 3.7829405681183994,
-                    "timestamp": 1.7404469763681818
-                },
-                {
-                    "x": 7.68000078407037,
-                    "y": 6.999268392550285,
-                    "heading": 3.1400000030761217,
-                    "angularVelocity": -1.5876160780832585e-8,
-                    "velocityX": -0.000005234436287123649,
-                    "velocityY": 3.224906490772867,
-                    "timestamp": 1.837138475055303
-                },
-                {
-                    "x": 7.68000039686507,
-                    "y": 7.233134191633239,
-                    "heading": 3.1400000013788225,
-                    "angularVelocity": -1.7553792269593633e-8,
-                    "velocityX": -0.000004004542818722395,
-                    "velocityY": 2.4186800521075664,
-                    "timestamp": 1.9338299737424243
-                },
-                {
-                    "x": 7.680000133859054,
-                    "y": 7.389044729512878,
-                    "heading": 3.1400000003266944,
-                    "angularVelocity": -1.08813147296025e-8,
-                    "velocityX": -0.0000027200528426415456,
-                    "velocityY": 1.6124534214132105,
-                    "timestamp": 2.0305214724295455
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 3.14,
-                    "angularVelocity": -3.378739660348149e-9,
-                    "velocityX": -0.000001384393079062506,
-                    "velocityY": 0.806226726709054,
-                    "timestamp": 2.1272129711166667
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 3.14,
-                    "angularVelocity": 1.4001703039309738e-31,
-                    "velocityX": 0,
-                    "velocityY": 2.3509592524815357e-38,
-                    "timestamp": 2.223904469803788
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -33598,7 +31483,7 @@
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -33607,186 +31492,14 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 3.14,
-                    "angularVelocity": -9.959658814138394e-32,
-                    "velocityX": -4.727301624016019e-32,
-                    "velocityY": 3.595241492576121e-38,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.680000100608845,
-                    "y": 0.8688441539517371,
-                    "heading": 3.13999998007937,
-                    "angularVelocity": -2.0106831809452372e-7,
-                    "velocityX": 0.0000010154926233848571,
-                    "velocityY": 0.826091745427235,
-                    "timestamp": 0.09907392781101573
-                },
-                {
-                    "x": 7.680000296760758,
-                    "y": 1.0325324587418605,
-                    "heading": 3.139999940084766,
-                    "angularVelocity": -4.036843910631592e-7,
-                    "velocityX": 0.0000019798539067481827,
-                    "velocityY": 1.6521834594299558,
-                    "timestamp": 0.19814785562203147
-                },
-                {
-                    "x": 7.680000583126213,
-                    "y": 1.2780649081436684,
-                    "heading": 3.1399998804112537,
-                    "angularVelocity": -6.023128542087543e-7,
-                    "velocityX": 0.0000028904217813218476,
-                    "velocityY": 2.47827511058363,
-                    "timestamp": 0.29722178343304717
-                },
-                {
-                    "x": 7.680000954004463,
-                    "y": 1.6054414834770696,
-                    "heading": 3.13999980228898,
-                    "angularVelocity": -7.885247914488142e-7,
-                    "velocityX": 0.0000037434492396662855,
-                    "velocityY": 3.3043665731903316,
-                    "timestamp": 0.39629571124406293
-                },
-                {
-                    "x": 7.680001281447094,
-                    "y": 1.9802322649264825,
-                    "heading": 3.1399998022889792,
-                    "angularVelocity": -1.0474954246602775e-14,
-                    "velocityX": 0.0000033050332881388687,
-                    "velocityY": 3.7829405750856777,
-                    "timestamp": 0.4953696390550787
-                },
-                {
-                    "x": 7.6800015408657325,
-                    "y": 2.355023046375991,
-                    "heading": 3.13999980228898,
-                    "angularVelocity": 7.833153823506449e-15,
-                    "velocityX": 0.0000026184347759635413,
-                    "velocityY": 3.7829405750867062,
-                    "timestamp": 0.5944435668660945
-                },
-                {
-                    "x": 7.680001725168594,
-                    "y": 2.729813827825533,
-                    "heading": 3.139999802288987,
-                    "angularVelocity": 7.465414071871633e-14,
-                    "velocityX": 0.0000018602557564557753,
-                    "velocityY": 3.7829405750870757,
-                    "timestamp": 0.6935174946771102
-                },
-                {
-                    "x": 7.680001829195369,
-                    "y": 3.104604609275094,
-                    "heading": 3.1399998022889952,
-                    "angularVelocity": 9.203271146236807e-14,
-                    "velocityX": 0.0000010499913044241082,
-                    "velocityY": 3.7829405750872978,
-                    "timestamp": 0.792591422488126
-                },
-                {
-                    "x": 7.680001849352207,
-                    "y": 3.479395390724662,
-                    "heading": 3.1399998022890023,
-                    "angularVelocity": 7.71401741801888e-14,
-                    "velocityX": 2.034524499813149e-7,
-                    "velocityY": 3.7829405750873777,
-                    "timestamp": 0.8916653502991417
-                },
-                {
-                    "x": 7.6800017836349985,
-                    "y": 3.8541861721742245,
-                    "heading": 3.1399998022890094,
-                    "angularVelocity": 7.688638756481334e-14,
-                    "velocityX": -6.633148330797741e-7,
-                    "velocityY": 3.7829405750873186,
-                    "timestamp": 0.9907392781101575
-                },
-                {
-                    "x": 7.68000163167861,
-                    "y": 4.22897695362377,
-                    "heading": 3.1399998022890157,
-                    "angularVelocity": 6.981254883852486e-14,
-                    "velocityX": -0.000001533767606062718,
-                    "velocityY": 3.7829405750871206,
-                    "timestamp": 1.0898132059211731
-                },
-                {
-                    "x": 7.680001393541454,
-                    "y": 4.603767735073283,
-                    "heading": 3.139999802289022,
-                    "angularVelocity": 6.191049012077391e-14,
-                    "velocityX": -0.00000240363076954954,
-                    "velocityY": 3.782940575086774,
-                    "timestamp": 1.188887133732189
-                },
-                {
-                    "x": 7.680001069388574,
-                    "y": 4.978558516522716,
-                    "heading": 3.139999802289024,
-                    "angularVelocity": 2.5336752196891655e-14,
-                    "velocityX": -0.000003271828396587149,
-                    "velocityY": 3.782940575085891,
-                    "timestamp": 1.2879610615432047
-                },
-                {
-                    "x": 7.680000659196884,
-                    "y": 5.305935091856245,
-                    "heading": 3.139999885175708,
-                    "angularVelocity": 8.366142293360243e-7,
-                    "velocityX": -0.000004140258200407122,
-                    "velocityY": 3.304366573191614,
-                    "timestamp": 1.3870349893542204
-                },
-                {
-                    "x": 7.6800003382888615,
-                    "y": 5.5514675412581065,
-                    "heading": 3.1399999457053744,
-                    "angularVelocity": 6.10954418213692e-7,
-                    "velocityX": -0.0000032390761240661895,
-                    "velocityY": 2.478275110584155,
-                    "timestamp": 1.4861089171652362
-                },
-                {
-                    "x": 7.6800001156321125,
-                    "y": 5.715155846048254,
-                    "heading": 3.139999983279866,
-                    "angularVelocity": 3.792570390671995e-7,
-                    "velocityX": -0.000002247379691011521,
-                    "velocityY": 1.6521834594301872,
-                    "timestamp": 1.585182844976252
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 3.14,
-                    "angularVelocity": 1.687641935617777e-7,
-                    "velocityX": -0.0000011671295149488337,
-                    "velocityY": 0.8260917454273095,
-                    "timestamp": 1.6842567727872677
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 3.14,
-                    "angularVelocity": -8.926921159411167e-32,
-                    "velocityX": 0,
-                    "velocityY": 7.102031651059941e-38,
-                    "timestamp": 1.7833307005982835
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -33827,7 +31540,7 @@
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -33836,7 +31549,7 @@
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -33884,7 +31597,7 @@
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
@@ -33893,105 +31606,14 @@
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 3.14,
-                    "angularVelocity": 0,
-                    "velocityX": -4.029789856147219e-33,
-                    "velocityY": -3.379656030870708e-35,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.680000025226674,
-                    "y": 0.87050000557584,
-                    "heading": 3.139999988557334,
-                    "angularVelocity": -1.1434532837257691e-7,
-                    "velocityX": 2.5208743257476917e-7,
-                    "velocityY": 0.8344065481227664,
-                    "timestamp": 0.10007112931184067
-                },
-                {
-                    "x": 7.680000070528317,
-                    "y": 1.03750001362983,
-                    "heading": 3.1399999655365143,
-                    "angularVelocity": -2.300445661244102e-7,
-                    "velocityX": 4.5269443024398455e-7,
-                    "velocityY": 1.6688130652906519,
-                    "timestamp": 0.20014225862368135
-                },
-                {
-                    "x": 7.6800001304795185,
-                    "y": 1.2880000179665891,
-                    "heading": 3.139999931372676,
-                    "angularVelocity": -3.413955478564011e-7,
-                    "velocityX": 5.990858804609274e-7,
-                    "velocityY": 2.5032195205487646,
-                    "timestamp": 0.300213387935522
-                },
-                {
-                    "x": 7.680000199269358,
-                    "y": 1.6219999999999795,
-                    "heading": 3.1399998873718746,
-                    "angularVelocity": -4.396952561787762e-7,
-                    "velocityX": 6.874094371094506e-7,
-                    "velocityY": 3.337625790077605,
-                    "timestamp": 0.4002845172473627
-                },
-                {
-                    "x": 7.680000135199738,
-                    "y": 1.9559999820333853,
-                    "heading": 3.1399999178431197,
-                    "angularVelocity": 3.0449586057484084e-7,
-                    "velocityX": -6.402407947941681e-7,
-                    "velocityY": 3.3376257900777593,
-                    "timestamp": 0.5003556465592034
-                },
-                {
-                    "x": 7.680000075581297,
-                    "y": 2.206499986370157,
-                    "heading": 3.139999951084868,
-                    "angularVelocity": 3.3218120651110665e-7,
-                    "velocityX": -5.957606422672805e-7,
-                    "velocityY": 2.503219520548894,
-                    "timestamp": 0.6004267758710441
-                },
-                {
-                    "x": 7.680000027896081,
-                    "y": 2.373499994424156,
-                    "heading": 3.13999998094686,
-                    "angularVelocity": 2.984076655658666e-7,
-                    "velocityX": -4.765132174434968e-7,
-                    "velocityY": 1.6688130652907383,
-                    "timestamp": 0.7004979051828848
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": 1.903959699368805e-7,
-                    "velocityX": -2.787625301130355e-7,
-                    "velocityY": 0.8344065481228072,
-                    "timestamp": 0.8005690344947255
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 3.14,
-                    "angularVelocity": -2.2348525624284448e-32,
-                    "velocityX": 0,
-                    "velocityY": 2.3896717289528774e-36,
-                    "timestamp": 0.9006401638065662
-                }
-            ],
+            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.1.traj
index d3385742..137c9bbd 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.1.traj	
@@ -3,272 +3,272 @@
         {
             "x": 0.71,
             "y": 6.72,
-            "heading": -2.1,
+            "heading": 1.04,
             "angularVelocity": 0,
-            "velocityX": -2.7669422223227046e-34,
+            "velocityX": -1.8357629106361847e-33,
             "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.7613814086171108,
-            "y": 6.725487327512705,
-            "heading": -2.1469081960592686,
-            "angularVelocity": -0.5864829148967596,
-            "velocityX": 0.642410513062366,
-            "velocityY": 0.06860685562450702,
-            "timestamp": 0.07998220385929865
+            "x": 0.7613816250456615,
+            "y": 6.725489542887092,
+            "heading": 0.9931061774124983,
+            "angularVelocity": -0.5863053943309177,
+            "velocityX": 0.6424156162050443,
+            "velocityY": 0.06863481007775876,
+            "timestamp": 0.07998190540446273
         },
         {
-            "x": 0.8643773558424356,
-            "y": 6.73664563014898,
-            "heading": -2.237067397875382,
-            "angularVelocity": -1.1272407793953514,
-            "velocityX": 1.2877357994099667,
-            "velocityY": 0.13950981715763813,
-            "timestamp": 0.1599644077185973
+            "x": 0.8643774278996095,
+            "y": 6.736651981527731,
+            "heading": 0.9029653992770575,
+            "angularVelocity": -1.1270146376184127,
+            "velocityX": 1.2877387995835512,
+            "velocityY": 0.13956204949346018,
+            "timestamp": 0.15996381080892547
         },
         {
-            "x": 1.019344149471572,
-            "y": 6.7536581703311604,
-            "heading": -2.3645059003180053,
-            "angularVelocity": -1.5933357208661998,
-            "velocityX": 1.9375159241891806,
-            "velocityY": 0.21270406867141206,
-            "timestamp": 0.23994661157789593
+            "x": 1.019343252130834,
+            "y": 6.753670139482374,
+            "heading": 0.7755395431364547,
+            "angularVelocity": -1.5931835519073916,
+            "velocityX": 1.9375110338716452,
+            "velocityY": 0.21277510042531103,
+            "timestamp": 0.2399457162133882
         },
         {
-            "x": 1.2269206708549203,
-            "y": 6.776747585260753,
-            "heading": -2.5146569204728233,
-            "angularVelocity": -1.8773053618147075,
-            "velocityX": 2.59528384274717,
-            "velocityY": 0.28868190441726393,
-            "timestamp": 0.3199288154371946
+            "x": 1.2269177512246652,
+            "y": 6.77676689021641,
+            "heading": 0.6253872743553933,
+            "angularVelocity": -1.8773279783940144,
+            "velocityX": 2.5952682427874385,
+            "velocityY": 0.2887746999429258,
+            "timestamp": 0.31992762161785093
         },
         {
-            "x": 1.4861241127588112,
-            "y": 6.80668700452381,
-            "heading": -2.6232938505110392,
-            "angularVelocity": -1.3582637736430165,
-            "velocityX": 3.240763937436266,
-            "velocityY": 0.37432601026755163,
-            "timestamp": 0.39991101929649325
+            "x": 1.4861215960194483,
+            "y": 6.806717977426302,
+            "heading": 0.5166968690593423,
+            "angularVelocity": -1.358937433990995,
+            "velocityX": 3.240781067717853,
+            "velocityY": 0.37447328940753305,
+            "timestamp": 0.39990952702231364
         },
         {
-            "x": 1.7870157378738414,
-            "y": 6.83849222949431,
-            "heading": -2.623293892215126,
-            "angularVelocity": -5.214170797715324e-7,
-            "velocityX": 3.7619821734888195,
-            "velocityY": 0.39765377091197274,
-            "timestamp": 0.4798932231557919
+            "x": 1.7870123176597594,
+            "y": 6.838521008932437,
+            "heading": 0.5166968273679649,
+            "angularVelocity": -5.212601175914625e-7,
+            "velocityX": 3.7619849154471687,
+            "velocityY": 0.397627830261236,
+            "timestamp": 0.47989143242677634
         },
         {
-            "x": 2.0879074285185357,
-            "y": 6.870296834526242,
-            "heading": -2.623293933917092,
-            "angularVelocity": -5.213905536622117e-7,
-            "velocityX": 3.7619829927918795,
-            "velocityY": 0.3976460199556515,
-            "timestamp": 0.5598754270150905
+            "x": 2.087903104978946,
+            "y": 6.870323419047386,
+            "heading": 0.5166967856787098,
+            "angularVelocity": -5.212335818457529e-7,
+            "velocityX": 3.7619857366188425,
+            "velocityY": 0.3976200611141572,
+            "timestamp": 0.559873337831239
         },
         {
-            "x": 2.3887991234818986,
-            "y": 6.9021013987008635,
-            "heading": -2.623293975619048,
-            "angularVelocity": -5.213904366102477e-7,
-            "velocityX": 3.7619830467872393,
-            "velocityY": 0.3976455091256453,
-            "timestamp": 0.6398576308743892
+            "x": 2.3887938966166886,
+            "y": 6.9021257883033815,
+            "heading": 0.5166967439894641,
+            "angularVelocity": -5.212334661065162e-7,
+            "velocityX": 3.761985790613012,
+            "velocityY": 0.3976195502616874,
+            "timestamp": 0.6398552432357018
         },
         {
-            "x": 2.689690823126622,
-            "y": 6.933905918586809,
-            "heading": -2.623294017320994,
-            "angularVelocity": -5.213903027395266e-7,
-            "velocityX": 3.761983105317272,
-            "velocityY": 0.39764495539400585,
-            "timestamp": 0.7198398347336878
+            "x": 2.689684692935055,
+            "y": 6.933928113274756,
+            "heading": 0.5166967023002289,
+            "angularVelocity": -5.212333328622992e-7,
+            "velocityX": 3.76198584913404,
+            "velocityY": 0.3976189965786884,
+            "timestamp": 0.7198371486401645
         },
         {
-            "x": 2.9905825278407376,
-            "y": 6.965710390512973,
-            "heading": -2.62329405902293,
-            "angularVelocity": -5.213901895916786e-7,
-            "velocityX": 3.7619831686987752,
-            "velocityY": 0.39764435576335205,
-            "timestamp": 0.7998220385929865
+            "x": 2.990575494322058,
+            "y": 6.965730390290324,
+            "heading": 0.5166966606110028,
+            "angularVelocity": -5.212332194068621e-7,
+            "velocityX": 3.761985912506342,
+            "velocityY": 0.3976183969955037,
+            "timestamp": 0.7998190540446272
         },
         {
-            "x": 3.2914742379996778,
-            "y": 6.997514810927433,
-            "heading": -2.623294100724858,
-            "angularVelocity": -5.21390082509775e-7,
-            "velocityX": 3.761983236774224,
-            "velocityY": 0.39764371172379265,
-            "timestamp": 0.8798042424522852
+            "x": 3.291466301153434,
+            "y": 6.997532615795069,
+            "heading": 0.5166966189217854,
+            "angularVelocity": -5.21233112050001e-7,
+            "velocityX": 3.76198598057639,
+            "velocityY": 0.39761775296452984,
+            "timestamp": 0.8798009594490899
         },
         {
-            "x": 3.5923659539723536,
-            "y": 7.029319176339941,
-            "heading": -2.6232941424267775,
-            "angularVelocity": -5.213899786160023e-7,
-            "velocityX": 3.7619833094620927,
-            "velocityY": 0.3976430240464025,
-            "timestamp": 0.9597864463115838
+            "x": 3.592357113798132,
+            "y": 7.029334786298146,
+            "heading": 0.5166965772325762,
+            "angularVelocity": -5.212330086999819e-7,
+            "velocityX": 3.7619860532593528,
+            "velocityY": 0.39761706528816065,
+            "timestamp": 0.9597828648535526
         },
         {
-            "x": 3.8932576761280617,
-            "y": 7.0611234832565914,
-            "heading": -2.6232941841286896,
-            "angularVelocity": -5.213898862684442e-7,
-            "velocityX": 3.761983386767183,
-            "velocityY": 0.39764229268549517,
-            "timestamp": 1.0397686501708825
+            "x": 3.8932479326255467,
+            "y": 7.061136898304478,
+            "heading": 0.5166965355433745,
+            "angularVelocity": -5.212329156376395e-7,
+            "velocityX": 3.7619861305608038,
+            "velocityY": 0.3976163339134084,
+            "timestamp": 1.0397647702580153
         },
         {
-            "x": 4.194149404850362,
-            "y": 7.092927728048495,
-            "heading": -2.623294225830597,
-            "angularVelocity": -5.213898264324783e-7,
-            "velocityX": 3.7619834688678555,
-            "velocityY": 0.3976415159533747,
-            "timestamp": 1.119750854030181
+            "x": 4.194138758019097,
+            "y": 7.092938948186284,
+            "heading": 0.5166964938541775,
+            "angularVelocity": -5.212328561470798e-7,
+            "velocityX": 3.7619862126560695,
+            "velocityY": 0.3976155571811608,
+            "timestamp": 1.119746675662478
         },
         {
-            "x": 4.49504114052058,
-            "y": 7.124731907107895,
-            "heading": -2.623294267532503,
-            "angularVelocity": -5.213898089255833e-7,
-            "velocityX": 3.761983555736148,
-            "velocityY": 0.3976406941142751,
-            "timestamp": 1.1997330578894796
+            "x": 4.495029590361164,
+            "y": 7.1247409323255795,
+            "heading": 0.516696452164982,
+            "angularVelocity": -5.212328389753682e-7,
+            "velocityX": 3.7619862995321793,
+            "velocityY": 0.397614735213847,
+            "timestamp": 1.199728581066941
         },
         {
-            "x": 4.795932883453666,
-            "y": 7.15653601745498,
-            "heading": -2.6232943092344048,
-            "angularVelocity": -5.213897595490136e-7,
-            "velocityX": 3.761983646542196,
-            "velocityY": 0.39763983501921824,
-            "timestamp": 1.2797152617487781
+            "x": 4.79592042996658,
+            "y": 7.15654284774347,
+            "heading": 0.5166964104757901,
+            "angularVelocity": -5.212327914819509e-7,
+            "velocityX": 3.7619863903445805,
+            "velocityY": 0.39761387600196396,
+            "timestamp": 1.2797104864714037
         },
         {
-            "x": 5.0968246339465235,
-            "y": 7.188340056280654,
-            "heading": -2.6232943509362996,
-            "angularVelocity": -5.21389667495749e-7,
-            "velocityX": 3.7619837410603636,
-            "velocityY": 0.3976389408026617,
-            "timestamp": 1.3596974656080767
+            "x": 5.096811277131865,
+            "y": 7.188344691634321,
+            "heading": 0.5166963687866055,
+            "angularVelocity": -5.212326996843497e-7,
+            "velocityX": 3.7619864848642997,
+            "velocityY": 0.39761298171169474,
+            "timestamp": 1.3596923918758665
         },
         {
-            "x": 5.3977163922506195,
-            "y": 7.220144021205662,
-            "heading": -2.6232943926381824,
-            "angularVelocity": -5.213895176915956e-7,
-            "velocityX": 3.7619838387225806,
-            "velocityY": 0.39763801683879874,
-            "timestamp": 1.4396796694673752
+            "x": 5.397702132107731,
+            "y": 7.220146461625841,
+            "heading": 0.5166963270974331,
+            "angularVelocity": -5.212325496997691e-7,
+            "velocityX": 3.761986582518669,
+            "velocityY": 0.3976120577610798,
+            "timestamp": 1.4396742972803294
         },
         {
-            "x": 5.698608158576608,
-            "y": 7.251947910236858,
-            "heading": -2.623294434340057,
-            "angularVelocity": -5.213894134163355e-7,
-            "velocityX": 3.7619839390185463,
-            "velocityY": 0.3976370679550778,
-            "timestamp": 1.5196618733266738
+            "x": 5.698592995103496,
+            "y": 7.251948155737401,
+            "heading": 0.516696285408269,
+            "angularVelocity": -5.212324447299826e-7,
+            "velocityX": 3.7619866827900856,
+            "velocityY": 0.3976111090469843,
+            "timestamp": 1.5196562026847922
         },
         {
-            "x": 5.999499932773201,
-            "y": 7.283751724805354,
-            "heading": -2.62329447604193,
-            "angularVelocity": -5.213893967687742e-7,
-            "velocityX": 3.761984037422991,
-            "velocityY": 0.3976361369642237,
-            "timestamp": 1.5996440771859723
+            "x": 5.99948386596862,
+            "y": 7.283749775393132,
+            "heading": 0.5166962437191063,
+            "angularVelocity": -5.212324282706134e-7,
+            "velocityX": 3.7619867811793206,
+            "velocityY": 0.3976101781385786,
+            "timestamp": 1.599638108089255
         },
         {
-            "x": 6.300391714248171,
-            "y": 7.315555470513951,
-            "heading": -2.6232945177438096,
-            "angularVelocity": -5.213894781911953e-7,
-            "velocityX": 3.7619841284229465,
-            "velocityY": 0.39763527602395277,
-            "timestamp": 1.679626281045271
+            "x": 6.300374744112002,
+            "y": 7.315551326185539,
+            "heading": 0.5166962020299369,
+            "angularVelocity": -5.212325087463018e-7,
+            "velocityX": 3.761986872178135,
+            "velocityY": 0.3976093171522739,
+            "timestamp": 1.6796200134937178
         },
         {
-            "x": 6.601283502174795,
-            "y": 7.347359155184038,
-            "heading": -2.6232945594457013,
-            "angularVelocity": -5.213896319500141e-7,
-            "velocityX": 3.76198420908656,
-            "velocityY": 0.39763451287282225,
-            "timestamp": 1.7596084849045694
+            "x": 6.6012656287075195,
+            "y": 7.347352815930844,
+            "heading": 0.5166961603407556,
+            "angularVelocity": -5.21232660707814e-7,
+            "velocityX": 3.7619869528480785,
+            "velocityY": 0.39760855389086563,
+            "timestamp": 1.7596019188981806
         },
         {
-            "x": 6.902175248741792,
-            "y": 7.379163231145718,
-            "heading": -2.6232946011491864,
-            "angularVelocity": -5.214095570695983e-7,
-            "velocityX": 3.7619836919762037,
-            "velocityY": 0.39763940510601253,
-            "timestamp": 1.839590688763868
+            "x": 6.9021564720244495,
+            "y": 7.37915469622686,
+            "heading": 0.5166961186499832,
+            "angularVelocity": -5.212525529907653e-7,
+            "velocityX": 3.7619864367489835,
+            "velocityY": 0.3976134368792068,
+            "timestamp": 1.8395838243026434
         },
         {
-            "x": 7.162101881336522,
-            "y": 7.409614310010791,
-            "heading": -2.734801003168492,
-            "angularVelocity": -1.3941401541705845,
-            "velocityX": 3.249805832457199,
-            "velocityY": 0.3807231783540282,
-            "timestamp": 1.9195728926231665
+            "x": 7.162090895853125,
+            "y": 7.409612003973996,
+            "heading": 0.405130711689902,
+            "angularVelocity": -1.3948830850665912,
+            "velocityX": 3.2499153716606957,
+            "velocityY": 0.3808024776743751,
+            "timestamp": 1.9195657297071063
         },
         {
-            "x": 7.370045087720824,
-            "y": 7.432975674976622,
-            "heading": -2.8811468250983516,
-            "angularVelocity": -1.8297298007354832,
-            "velocityX": 2.5998684251074975,
-            "velocityY": 0.29208203623556095,
-            "timestamp": 1.999555096482465
+            "x": 7.370036531434433,
+            "y": 7.432975355945854,
+            "heading": 0.2588526808330054,
+            "angularVelocity": -1.8288890483063531,
+            "velocityX": 2.5999084984252714,
+            "velocityY": 0.2921079693427033,
+            "timestamp": 1.999547635111569
         },
         {
-            "x": 7.525332087472029,
-            "y": 7.4501357007114875,
-            "heading": -3.00655027342712,
-            "angularVelocity": -1.5678918844168468,
-            "velocityX": 1.9415193912933517,
-            "velocityY": 0.21454804827649476,
-            "timestamp": 2.0795373003417637
+            "x": 7.52532693851905,
+            "y": 7.450135904743354,
+            "heading": 0.13353023499549588,
+            "angularVelocity": -1.5668849748422837,
+            "velocityX": 1.9415692374334488,
+            "velocityY": 0.21455538863097798,
+            "timestamp": 2.079529540516032
         },
         {
-            "x": 7.62852758064907,
-            "y": 7.461415013154985,
-            "heading": -3.096236840109313,
-            "angularVelocity": -1.1213315256974696,
-            "velocityX": 1.2902306787967257,
-            "velocityY": 0.14102277630833499,
-            "timestamp": 2.159519504201062
+            "x": 7.628525628712187,
+            "y": 7.461415184148821,
+            "heading": 0.04391070884179766,
+            "angularVelocity": -1.120497513787634,
+            "velocityX": 1.2902754650726174,
+            "velocityY": 0.14102288947017258,
+            "timestamp": 2.1595114459204945
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.5869864145386997,
-            "velocityX": 0.6435484003601272,
-            "velocityY": 0.06982786889492548,
-            "timestamp": 2.2395017080603608
+            "heading": -0.003,
+            "angularVelocity": -0.5865165202625967,
+            "velocityX": 0.6435752065109067,
+            "velocityY": 0.06982599155316412,
+            "timestamp": 2.239493351324957
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": -3.143185307179586,
-            "angularVelocity": -1.1375235030887645e-31,
+            "heading": -0.003,
+            "angularVelocity": -3.4580717798993225e-32,
             "velocityX": 0,
             "velocityY": 0,
-            "timestamp": 2.3194839119196593
+            "timestamp": 2.3194752567294197
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.traj
index d3385742..137c9bbd 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.traj	
@@ -3,272 +3,272 @@
         {
             "x": 0.71,
             "y": 6.72,
-            "heading": -2.1,
+            "heading": 1.04,
             "angularVelocity": 0,
-            "velocityX": -2.7669422223227046e-34,
+            "velocityX": -1.8357629106361847e-33,
             "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.7613814086171108,
-            "y": 6.725487327512705,
-            "heading": -2.1469081960592686,
-            "angularVelocity": -0.5864829148967596,
-            "velocityX": 0.642410513062366,
-            "velocityY": 0.06860685562450702,
-            "timestamp": 0.07998220385929865
+            "x": 0.7613816250456615,
+            "y": 6.725489542887092,
+            "heading": 0.9931061774124983,
+            "angularVelocity": -0.5863053943309177,
+            "velocityX": 0.6424156162050443,
+            "velocityY": 0.06863481007775876,
+            "timestamp": 0.07998190540446273
         },
         {
-            "x": 0.8643773558424356,
-            "y": 6.73664563014898,
-            "heading": -2.237067397875382,
-            "angularVelocity": -1.1272407793953514,
-            "velocityX": 1.2877357994099667,
-            "velocityY": 0.13950981715763813,
-            "timestamp": 0.1599644077185973
+            "x": 0.8643774278996095,
+            "y": 6.736651981527731,
+            "heading": 0.9029653992770575,
+            "angularVelocity": -1.1270146376184127,
+            "velocityX": 1.2877387995835512,
+            "velocityY": 0.13956204949346018,
+            "timestamp": 0.15996381080892547
         },
         {
-            "x": 1.019344149471572,
-            "y": 6.7536581703311604,
-            "heading": -2.3645059003180053,
-            "angularVelocity": -1.5933357208661998,
-            "velocityX": 1.9375159241891806,
-            "velocityY": 0.21270406867141206,
-            "timestamp": 0.23994661157789593
+            "x": 1.019343252130834,
+            "y": 6.753670139482374,
+            "heading": 0.7755395431364547,
+            "angularVelocity": -1.5931835519073916,
+            "velocityX": 1.9375110338716452,
+            "velocityY": 0.21277510042531103,
+            "timestamp": 0.2399457162133882
         },
         {
-            "x": 1.2269206708549203,
-            "y": 6.776747585260753,
-            "heading": -2.5146569204728233,
-            "angularVelocity": -1.8773053618147075,
-            "velocityX": 2.59528384274717,
-            "velocityY": 0.28868190441726393,
-            "timestamp": 0.3199288154371946
+            "x": 1.2269177512246652,
+            "y": 6.77676689021641,
+            "heading": 0.6253872743553933,
+            "angularVelocity": -1.8773279783940144,
+            "velocityX": 2.5952682427874385,
+            "velocityY": 0.2887746999429258,
+            "timestamp": 0.31992762161785093
         },
         {
-            "x": 1.4861241127588112,
-            "y": 6.80668700452381,
-            "heading": -2.6232938505110392,
-            "angularVelocity": -1.3582637736430165,
-            "velocityX": 3.240763937436266,
-            "velocityY": 0.37432601026755163,
-            "timestamp": 0.39991101929649325
+            "x": 1.4861215960194483,
+            "y": 6.806717977426302,
+            "heading": 0.5166968690593423,
+            "angularVelocity": -1.358937433990995,
+            "velocityX": 3.240781067717853,
+            "velocityY": 0.37447328940753305,
+            "timestamp": 0.39990952702231364
         },
         {
-            "x": 1.7870157378738414,
-            "y": 6.83849222949431,
-            "heading": -2.623293892215126,
-            "angularVelocity": -5.214170797715324e-7,
-            "velocityX": 3.7619821734888195,
-            "velocityY": 0.39765377091197274,
-            "timestamp": 0.4798932231557919
+            "x": 1.7870123176597594,
+            "y": 6.838521008932437,
+            "heading": 0.5166968273679649,
+            "angularVelocity": -5.212601175914625e-7,
+            "velocityX": 3.7619849154471687,
+            "velocityY": 0.397627830261236,
+            "timestamp": 0.47989143242677634
         },
         {
-            "x": 2.0879074285185357,
-            "y": 6.870296834526242,
-            "heading": -2.623293933917092,
-            "angularVelocity": -5.213905536622117e-7,
-            "velocityX": 3.7619829927918795,
-            "velocityY": 0.3976460199556515,
-            "timestamp": 0.5598754270150905
+            "x": 2.087903104978946,
+            "y": 6.870323419047386,
+            "heading": 0.5166967856787098,
+            "angularVelocity": -5.212335818457529e-7,
+            "velocityX": 3.7619857366188425,
+            "velocityY": 0.3976200611141572,
+            "timestamp": 0.559873337831239
         },
         {
-            "x": 2.3887991234818986,
-            "y": 6.9021013987008635,
-            "heading": -2.623293975619048,
-            "angularVelocity": -5.213904366102477e-7,
-            "velocityX": 3.7619830467872393,
-            "velocityY": 0.3976455091256453,
-            "timestamp": 0.6398576308743892
+            "x": 2.3887938966166886,
+            "y": 6.9021257883033815,
+            "heading": 0.5166967439894641,
+            "angularVelocity": -5.212334661065162e-7,
+            "velocityX": 3.761985790613012,
+            "velocityY": 0.3976195502616874,
+            "timestamp": 0.6398552432357018
         },
         {
-            "x": 2.689690823126622,
-            "y": 6.933905918586809,
-            "heading": -2.623294017320994,
-            "angularVelocity": -5.213903027395266e-7,
-            "velocityX": 3.761983105317272,
-            "velocityY": 0.39764495539400585,
-            "timestamp": 0.7198398347336878
+            "x": 2.689684692935055,
+            "y": 6.933928113274756,
+            "heading": 0.5166967023002289,
+            "angularVelocity": -5.212333328622992e-7,
+            "velocityX": 3.76198584913404,
+            "velocityY": 0.3976189965786884,
+            "timestamp": 0.7198371486401645
         },
         {
-            "x": 2.9905825278407376,
-            "y": 6.965710390512973,
-            "heading": -2.62329405902293,
-            "angularVelocity": -5.213901895916786e-7,
-            "velocityX": 3.7619831686987752,
-            "velocityY": 0.39764435576335205,
-            "timestamp": 0.7998220385929865
+            "x": 2.990575494322058,
+            "y": 6.965730390290324,
+            "heading": 0.5166966606110028,
+            "angularVelocity": -5.212332194068621e-7,
+            "velocityX": 3.761985912506342,
+            "velocityY": 0.3976183969955037,
+            "timestamp": 0.7998190540446272
         },
         {
-            "x": 3.2914742379996778,
-            "y": 6.997514810927433,
-            "heading": -2.623294100724858,
-            "angularVelocity": -5.21390082509775e-7,
-            "velocityX": 3.761983236774224,
-            "velocityY": 0.39764371172379265,
-            "timestamp": 0.8798042424522852
+            "x": 3.291466301153434,
+            "y": 6.997532615795069,
+            "heading": 0.5166966189217854,
+            "angularVelocity": -5.21233112050001e-7,
+            "velocityX": 3.76198598057639,
+            "velocityY": 0.39761775296452984,
+            "timestamp": 0.8798009594490899
         },
         {
-            "x": 3.5923659539723536,
-            "y": 7.029319176339941,
-            "heading": -2.6232941424267775,
-            "angularVelocity": -5.213899786160023e-7,
-            "velocityX": 3.7619833094620927,
-            "velocityY": 0.3976430240464025,
-            "timestamp": 0.9597864463115838
+            "x": 3.592357113798132,
+            "y": 7.029334786298146,
+            "heading": 0.5166965772325762,
+            "angularVelocity": -5.212330086999819e-7,
+            "velocityX": 3.7619860532593528,
+            "velocityY": 0.39761706528816065,
+            "timestamp": 0.9597828648535526
         },
         {
-            "x": 3.8932576761280617,
-            "y": 7.0611234832565914,
-            "heading": -2.6232941841286896,
-            "angularVelocity": -5.213898862684442e-7,
-            "velocityX": 3.761983386767183,
-            "velocityY": 0.39764229268549517,
-            "timestamp": 1.0397686501708825
+            "x": 3.8932479326255467,
+            "y": 7.061136898304478,
+            "heading": 0.5166965355433745,
+            "angularVelocity": -5.212329156376395e-7,
+            "velocityX": 3.7619861305608038,
+            "velocityY": 0.3976163339134084,
+            "timestamp": 1.0397647702580153
         },
         {
-            "x": 4.194149404850362,
-            "y": 7.092927728048495,
-            "heading": -2.623294225830597,
-            "angularVelocity": -5.213898264324783e-7,
-            "velocityX": 3.7619834688678555,
-            "velocityY": 0.3976415159533747,
-            "timestamp": 1.119750854030181
+            "x": 4.194138758019097,
+            "y": 7.092938948186284,
+            "heading": 0.5166964938541775,
+            "angularVelocity": -5.212328561470798e-7,
+            "velocityX": 3.7619862126560695,
+            "velocityY": 0.3976155571811608,
+            "timestamp": 1.119746675662478
         },
         {
-            "x": 4.49504114052058,
-            "y": 7.124731907107895,
-            "heading": -2.623294267532503,
-            "angularVelocity": -5.213898089255833e-7,
-            "velocityX": 3.761983555736148,
-            "velocityY": 0.3976406941142751,
-            "timestamp": 1.1997330578894796
+            "x": 4.495029590361164,
+            "y": 7.1247409323255795,
+            "heading": 0.516696452164982,
+            "angularVelocity": -5.212328389753682e-7,
+            "velocityX": 3.7619862995321793,
+            "velocityY": 0.397614735213847,
+            "timestamp": 1.199728581066941
         },
         {
-            "x": 4.795932883453666,
-            "y": 7.15653601745498,
-            "heading": -2.6232943092344048,
-            "angularVelocity": -5.213897595490136e-7,
-            "velocityX": 3.761983646542196,
-            "velocityY": 0.39763983501921824,
-            "timestamp": 1.2797152617487781
+            "x": 4.79592042996658,
+            "y": 7.15654284774347,
+            "heading": 0.5166964104757901,
+            "angularVelocity": -5.212327914819509e-7,
+            "velocityX": 3.7619863903445805,
+            "velocityY": 0.39761387600196396,
+            "timestamp": 1.2797104864714037
         },
         {
-            "x": 5.0968246339465235,
-            "y": 7.188340056280654,
-            "heading": -2.6232943509362996,
-            "angularVelocity": -5.21389667495749e-7,
-            "velocityX": 3.7619837410603636,
-            "velocityY": 0.3976389408026617,
-            "timestamp": 1.3596974656080767
+            "x": 5.096811277131865,
+            "y": 7.188344691634321,
+            "heading": 0.5166963687866055,
+            "angularVelocity": -5.212326996843497e-7,
+            "velocityX": 3.7619864848642997,
+            "velocityY": 0.39761298171169474,
+            "timestamp": 1.3596923918758665
         },
         {
-            "x": 5.3977163922506195,
-            "y": 7.220144021205662,
-            "heading": -2.6232943926381824,
-            "angularVelocity": -5.213895176915956e-7,
-            "velocityX": 3.7619838387225806,
-            "velocityY": 0.39763801683879874,
-            "timestamp": 1.4396796694673752
+            "x": 5.397702132107731,
+            "y": 7.220146461625841,
+            "heading": 0.5166963270974331,
+            "angularVelocity": -5.212325496997691e-7,
+            "velocityX": 3.761986582518669,
+            "velocityY": 0.3976120577610798,
+            "timestamp": 1.4396742972803294
         },
         {
-            "x": 5.698608158576608,
-            "y": 7.251947910236858,
-            "heading": -2.623294434340057,
-            "angularVelocity": -5.213894134163355e-7,
-            "velocityX": 3.7619839390185463,
-            "velocityY": 0.3976370679550778,
-            "timestamp": 1.5196618733266738
+            "x": 5.698592995103496,
+            "y": 7.251948155737401,
+            "heading": 0.516696285408269,
+            "angularVelocity": -5.212324447299826e-7,
+            "velocityX": 3.7619866827900856,
+            "velocityY": 0.3976111090469843,
+            "timestamp": 1.5196562026847922
         },
         {
-            "x": 5.999499932773201,
-            "y": 7.283751724805354,
-            "heading": -2.62329447604193,
-            "angularVelocity": -5.213893967687742e-7,
-            "velocityX": 3.761984037422991,
-            "velocityY": 0.3976361369642237,
-            "timestamp": 1.5996440771859723
+            "x": 5.99948386596862,
+            "y": 7.283749775393132,
+            "heading": 0.5166962437191063,
+            "angularVelocity": -5.212324282706134e-7,
+            "velocityX": 3.7619867811793206,
+            "velocityY": 0.3976101781385786,
+            "timestamp": 1.599638108089255
         },
         {
-            "x": 6.300391714248171,
-            "y": 7.315555470513951,
-            "heading": -2.6232945177438096,
-            "angularVelocity": -5.213894781911953e-7,
-            "velocityX": 3.7619841284229465,
-            "velocityY": 0.39763527602395277,
-            "timestamp": 1.679626281045271
+            "x": 6.300374744112002,
+            "y": 7.315551326185539,
+            "heading": 0.5166962020299369,
+            "angularVelocity": -5.212325087463018e-7,
+            "velocityX": 3.761986872178135,
+            "velocityY": 0.3976093171522739,
+            "timestamp": 1.6796200134937178
         },
         {
-            "x": 6.601283502174795,
-            "y": 7.347359155184038,
-            "heading": -2.6232945594457013,
-            "angularVelocity": -5.213896319500141e-7,
-            "velocityX": 3.76198420908656,
-            "velocityY": 0.39763451287282225,
-            "timestamp": 1.7596084849045694
+            "x": 6.6012656287075195,
+            "y": 7.347352815930844,
+            "heading": 0.5166961603407556,
+            "angularVelocity": -5.21232660707814e-7,
+            "velocityX": 3.7619869528480785,
+            "velocityY": 0.39760855389086563,
+            "timestamp": 1.7596019188981806
         },
         {
-            "x": 6.902175248741792,
-            "y": 7.379163231145718,
-            "heading": -2.6232946011491864,
-            "angularVelocity": -5.214095570695983e-7,
-            "velocityX": 3.7619836919762037,
-            "velocityY": 0.39763940510601253,
-            "timestamp": 1.839590688763868
+            "x": 6.9021564720244495,
+            "y": 7.37915469622686,
+            "heading": 0.5166961186499832,
+            "angularVelocity": -5.212525529907653e-7,
+            "velocityX": 3.7619864367489835,
+            "velocityY": 0.3976134368792068,
+            "timestamp": 1.8395838243026434
         },
         {
-            "x": 7.162101881336522,
-            "y": 7.409614310010791,
-            "heading": -2.734801003168492,
-            "angularVelocity": -1.3941401541705845,
-            "velocityX": 3.249805832457199,
-            "velocityY": 0.3807231783540282,
-            "timestamp": 1.9195728926231665
+            "x": 7.162090895853125,
+            "y": 7.409612003973996,
+            "heading": 0.405130711689902,
+            "angularVelocity": -1.3948830850665912,
+            "velocityX": 3.2499153716606957,
+            "velocityY": 0.3808024776743751,
+            "timestamp": 1.9195657297071063
         },
         {
-            "x": 7.370045087720824,
-            "y": 7.432975674976622,
-            "heading": -2.8811468250983516,
-            "angularVelocity": -1.8297298007354832,
-            "velocityX": 2.5998684251074975,
-            "velocityY": 0.29208203623556095,
-            "timestamp": 1.999555096482465
+            "x": 7.370036531434433,
+            "y": 7.432975355945854,
+            "heading": 0.2588526808330054,
+            "angularVelocity": -1.8288890483063531,
+            "velocityX": 2.5999084984252714,
+            "velocityY": 0.2921079693427033,
+            "timestamp": 1.999547635111569
         },
         {
-            "x": 7.525332087472029,
-            "y": 7.4501357007114875,
-            "heading": -3.00655027342712,
-            "angularVelocity": -1.5678918844168468,
-            "velocityX": 1.9415193912933517,
-            "velocityY": 0.21454804827649476,
-            "timestamp": 2.0795373003417637
+            "x": 7.52532693851905,
+            "y": 7.450135904743354,
+            "heading": 0.13353023499549588,
+            "angularVelocity": -1.5668849748422837,
+            "velocityX": 1.9415692374334488,
+            "velocityY": 0.21455538863097798,
+            "timestamp": 2.079529540516032
         },
         {
-            "x": 7.62852758064907,
-            "y": 7.461415013154985,
-            "heading": -3.096236840109313,
-            "angularVelocity": -1.1213315256974696,
-            "velocityX": 1.2902306787967257,
-            "velocityY": 0.14102277630833499,
-            "timestamp": 2.159519504201062
+            "x": 7.628525628712187,
+            "y": 7.461415184148821,
+            "heading": 0.04391070884179766,
+            "angularVelocity": -1.120497513787634,
+            "velocityX": 1.2902754650726174,
+            "velocityY": 0.14102288947017258,
+            "timestamp": 2.1595114459204945
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.5869864145386997,
-            "velocityX": 0.6435484003601272,
-            "velocityY": 0.06982786889492548,
-            "timestamp": 2.2395017080603608
+            "heading": -0.003,
+            "angularVelocity": -0.5865165202625967,
+            "velocityX": 0.6435752065109067,
+            "velocityY": 0.06982599155316412,
+            "timestamp": 2.239493351324957
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": -3.143185307179586,
-            "angularVelocity": -1.1375235030887645e-31,
+            "heading": -0.003,
+            "angularVelocity": -3.4580717798993225e-32,
             "velocityX": 0,
             "velocityY": 0,
-            "timestamp": 2.3194839119196593
+            "timestamp": 2.3194752567294197
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.1.traj
index 6b188abb..1088f323 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.1.traj	
@@ -3,272 +3,272 @@
         {
             "x": 0.71,
             "y": 6.72,
-            "heading": -2.1,
+            "heading": 1.04,
             "angularVelocity": 0,
-            "velocityX": -7.879763479482e-33,
+            "velocityX": 8.672453699203877e-33,
             "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.7634290430022197,
-            "y": 6.720160003423765,
-            "heading": -2.158070133372329,
-            "angularVelocity": -0.7094408010373536,
-            "velocityX": 0.6527407612984071,
-            "velocityY": 0.0019547562668324166,
-            "timestamp": 0.08185338831290496
+            "x": 0.7586383226811528,
+            "y": 6.716006460523898,
+            "heading": 0.9415437629009579,
+            "angularVelocity": -1.201643840291405,
+            "velocityX": 0.5936235486342963,
+            "velocityY": -0.048740559804155884,
+            "timestamp": 0.08193462471805792
         },
         {
-            "x": 0.8707119576691547,
-            "y": 6.720819673735276,
-            "heading": -2.2686879013575907,
-            "angularVelocity": -1.351413426679394,
-            "velocityX": 1.3106716396005418,
-            "velocityY": 0.008059169267236224,
-            "timestamp": 0.16370677662580993
+            "x": 0.8580797410236602,
+            "y": 6.713039346170398,
+            "heading": 0.7612656224060018,
+            "angularVelocity": -2.2002680931938623,
+            "velocityX": 1.2136678319415195,
+            "velocityY": -0.03621319269735464,
+            "timestamp": 0.16386924943611583
         },
         {
-            "x": 1.0324112516405533,
-            "y": 6.722281472922437,
-            "heading": -2.4240599050623355,
-            "angularVelocity": -1.898174368919177,
-            "velocityX": 1.9754746541861241,
-            "velocityY": 0.017858749861073935,
-            "timestamp": 0.2455601649387149
+            "x": 1.0092416285400778,
+            "y": 6.712497099188053,
+            "heading": 0.5037500418751092,
+            "angularVelocity": -3.142939647518002,
+            "velocityX": 1.8449085235524658,
+            "velocityY": -0.006618044376361366,
+            "timestamp": 0.24580387415417376
         },
         {
-            "x": 1.249511925601742,
-            "y": 6.724737672202976,
-            "heading": -2.6048330548635152,
-            "angularVelocity": -2.2084992878991065,
-            "velocityX": 2.652311387908187,
-            "velocityY": 0.030007301239015387,
-            "timestamp": 0.32741355325161986
+            "x": 1.2158692368887885,
+            "y": 6.714801206948958,
+            "heading": 0.22114949007633808,
+            "angularVelocity": -3.4490980189537326,
+            "velocityX": 2.5218594588030294,
+            "velocityY": 0.028121295103663166,
+            "timestamp": 0.32773849887223166
         },
         {
-            "x": 1.518512933103673,
-            "y": 6.730171304043105,
-            "heading": -2.7105117995327306,
-            "angularVelocity": -1.2910735504953308,
-            "velocityX": 3.2863759588498382,
-            "velocityY": 0.0663824913314231,
-            "timestamp": 0.4092669415645248
+            "x": 1.4708450332398146,
+            "y": 6.7208733735889465,
+            "heading": 0.04860948021713714,
+            "angularVelocity": -2.105825351039585,
+            "velocityX": 3.1119419565100066,
+            "velocityY": 0.07410989750528489,
+            "timestamp": 0.40967312359028957
         },
         {
-            "x": 1.8281517964730614,
-            "y": 6.7323448579922625,
-            "heading": -2.71051185012585,
-            "angularVelocity": -6.180943842017169e-7,
-            "velocityX": 3.782847231512476,
-            "velocityY": 0.026554233049558205,
-            "timestamp": 0.49112032987742976
+            "x": 1.7704680006957136,
+            "y": 6.724924507610315,
+            "heading": 0.013522063773945665,
+            "angularVelocity": -0.42823673830116915,
+            "velocityX": 3.6568540893049817,
+            "velocityY": 0.049443492727383076,
+            "timestamp": 0.49160774830834747
         },
         {
-            "x": 2.1377906615261124,
-            "y": 6.734518172124384,
-            "heading": -2.710511900718092,
-            "angularVelocity": -6.180836584503726e-7,
-            "velocityX": 3.7828472520817247,
-            "velocityY": 0.026551303213165345,
-            "timestamp": 0.5729737181903347
+            "x": 2.080344251704432,
+            "y": 6.7179919202849705,
+            "heading": 0.013521948546161235,
+            "angularVelocity": -0.000001406338099756076,
+            "velocityX": 3.78199390154067,
+            "velocityY": -0.08461120496004566,
+            "timestamp": 0.5735423730264054
         },
         {
-            "x": 2.4474295268603647,
-            "y": 6.736691446193322,
-            "heading": -2.7105119513103224,
-            "angularVelocity": -6.180835185678023e-7,
-            "velocityX": 3.7828472555171566,
-            "velocityY": 0.026550813762651566,
-            "timestamp": 0.6548271065032397
+            "x": 2.3902205002379273,
+            "y": 6.711059222319389,
+            "heading": 0.013521833318547862,
+            "angularVelocity": -0.000001406336012025862,
+            "velocityX": 3.781993871330935,
+            "velocityY": -0.08461255530781114,
+            "timestamp": 0.6554769977444633
         },
         {
-            "x": 2.7570683925033053,
-            "y": 6.738864676282315,
-            "heading": -2.7105120019025377,
-            "angularVelocity": -6.180833387834062e-7,
-            "velocityX": 3.7828472592883804,
-            "velocityY": 0.026550276461191975,
-            "timestamp": 0.7366804948161447
+            "x": 2.7000967477974154,
+            "y": 6.704126480817366,
+            "heading": 0.013521718090975673,
+            "angularVelocity": -0.000001406335509399091,
+            "velocityX": 3.7819938594433236,
+            "velocityY": -0.08461308666366031,
+            "timestamp": 0.7374116224625212
         },
         {
-            "x": 3.0667072584849335,
-            "y": 6.741037858115924,
-            "heading": -2.7105120524947366,
-            "angularVelocity": -6.180831373401399e-7,
-            "velocityX": 3.7828472634261248,
-            "velocityY": 0.026549686926837145,
-            "timestamp": 0.8185338831290497
+            "x": 3.009972994287386,
+            "y": 6.6971936915101855,
+            "heading": 0.013521602863440677,
+            "angularVelocity": -0.0000014063350554339165,
+            "velocityX": 3.7819938463900247,
+            "velocityY": -0.08461367011854067,
+            "timestamp": 0.8193462471805791
         },
         {
-            "x": 3.376346124838015,
-            "y": 6.7432109870244785,
-            "heading": -2.7105121030869213,
-            "angularVelocity": -6.18082958211415e-7,
-            "velocityX": 3.7828472679641574,
-            "velocityY": 0.02654904034329684,
-            "timestamp": 0.9003872714419547
+            "x": 3.319849239613809,
+            "y": 6.690260850195287,
+            "heading": 0.013521487635941729,
+            "angularVelocity": -0.0000014063346155021318,
+            "velocityX": 3.781993832189087,
+            "velocityY": -0.08461430486506759,
+            "timestamp": 0.901280871898637
         },
         {
-            "x": 3.6859849915958844,
-            "y": 6.745384058256853,
-            "heading": -2.710512153679093,
-            "angularVelocity": -6.180828069034381e-7,
-            "velocityX": 3.782847272909432,
-            "velocityY": 0.026548335715409193,
-            "timestamp": 0.9822406597548597
+            "x": 3.629725483679546,
+            "y": 6.683327952531275,
+            "heading": 0.013521372408489362,
+            "angularVelocity": -0.0000014063340469636637,
+            "velocityX": 3.7819938168026104,
+            "velocityY": -0.0846149925976696,
+            "timestamp": 0.9832154966166949
         },
         {
-            "x": 3.99562385879327,
-            "y": 6.747557066862843,
-            "heading": -2.7105122042712537,
-            "angularVelocity": -6.180826661377271e-7,
-            "velocityX": 3.7828472782789873,
-            "velocityY": 0.026547570611047785,
-            "timestamp": 1.0640940480677648
+            "x": 3.939601726375346,
+            "y": 6.6763949936353555,
+            "heading": 0.013521257181093756,
+            "angularVelocity": -0.0000014063333542144944,
+            "velocityX": 3.7819938000827285,
+            "velocityY": -0.08461573992406812,
+            "timestamp": 1.065150121334753
         },
         {
-            "x": 4.305262726470487,
-            "y": 6.749730007095753,
-            "heading": -2.710512254863404,
-            "angularVelocity": -6.180825475741077e-7,
-            "velocityX": 3.782847284141071,
-            "velocityY": 0.026546735299496697,
-            "timestamp": 1.1459474363806696
+            "x": 4.249477967564444,
+            "y": 6.66946196739526,
+            "heading": 0.013521141953758897,
+            "angularVelocity": -0.0000014063326128127892,
+            "velocityX": 3.781993781693661,
+            "velocityY": -0.0846165618498062,
+            "timestamp": 1.1470847460528109
         },
         {
-            "x": 4.614901594678237,
-            "y": 6.751902871728125,
-            "heading": -2.710512305455549,
-            "angularVelocity": -6.180824717562797e-7,
-            "velocityX": 3.78284729062258,
-            "velocityY": 0.0265458116903545,
-            "timestamp": 1.2278008246935745
+            "x": 4.559354207050928,
+            "y": 6.662528865055332,
+            "heading": 0.013521026726476763,
+            "angularVelocity": -0.0000014063319692972842,
+            "velocityX": 3.7819937609134957,
+            "velocityY": -0.08461749063701171,
+            "timestamp": 1.2290193707708688
         },
         {
-            "x": 4.924540463476948,
-            "y": 6.754075652145792,
-            "heading": -2.7105123560476914,
-            "angularVelocity": -6.180824480187367e-7,
-            "velocityX": 3.782847297842324,
-            "velocityY": 0.02654478284224149,
-            "timestamp": 1.3096542130064794
+            "x": 4.869230444482983,
+            "y": 6.655595670892424,
+            "heading": 0.01352091149922989,
+            "angularVelocity": -0.0000014063315389464776,
+            "velocityX": 3.7819937358394955,
+            "velocityY": -0.08461861132293771,
+            "timestamp": 1.3109539954889267
         },
         {
-            "x": 5.234179332930726,
-            "y": 6.756248339209459,
-            "heading": -2.7105124066398263,
-            "angularVelocity": -6.180823518950674e-7,
-            "velocityX": 3.782847305845261,
-            "velocityY": 0.026543642339650734,
-            "timestamp": 1.3915076013193843
+            "x": 5.1791066790320945,
+            "y": 6.648662347878877,
+            "heading": 0.013520796271957977,
+            "angularVelocity": -0.0000014063318445457383,
+            "velocityX": 3.7819937006535858,
+            "velocityY": -0.08462018392597198,
+            "timestamp": 1.3928886202069846
         },
         {
-            "x": 5.543818204521741,
-            "y": 6.758420721665928,
-            "heading": -2.710512457231931,
-            "angularVelocity": -6.180819838461299e-7,
-            "velocityX": 3.782847331955805,
-            "velocityY": 0.026539920964112583,
-            "timestamp": 1.4733609896322892
+            "x": 5.488982909387702,
+            "y": 6.641728837446599,
+            "heading": 0.013520681044501265,
+            "angularVelocity": -0.000001406334100009416,
+            "velocityX": 3.781993649472511,
+            "velocityY": -0.08462247134390694,
+            "timestamp": 1.4748232449250425
         },
         {
-            "x": 5.852559225022806,
-            "y": 6.734757839446224,
-            "heading": -2.710512508060742,
-            "angularVelocity": -6.209738209329952e-7,
-            "velocityX": 3.771878316396938,
-            "velocityY": -0.2890886096156145,
-            "timestamp": 1.5552143779451941
+            "x": 5.798859128679326,
+            "y": 6.634794832555163,
+            "heading": 0.013520565816988437,
+            "angularVelocity": -0.0000014063347848865157,
+            "velocityX": 3.781993514438203,
+            "velocityY": -0.08462850614495548,
+            "timestamp": 1.5567578696431004
         },
         {
-            "x": 6.152023822280739,
-            "y": 6.6560056480568655,
-            "heading": -2.710512532930908,
-            "angularVelocity": -3.03837945152059e-7,
-            "velocityX": 3.6585485760608294,
-            "velocityY": -0.9621127849748639,
-            "timestamp": 1.637067766258099
+            "x": 6.106285584248806,
+            "y": 6.595293875173847,
+            "heading": 0.013520451149743993,
+            "angularVelocity": -0.000001399496791973984,
+            "velocityX": 3.7520945098284613,
+            "velocityY": -0.48210335395128207,
+            "timestamp": 1.6386924943611583
         },
         {
-            "x": 6.432464531176267,
-            "y": 6.5247274697787905,
-            "heading": -2.710512568697855,
-            "angularVelocity": -4.369635492236228e-7,
-            "velocityX": 3.4261343931600465,
-            "velocityY": -1.603820941122575,
-            "timestamp": 1.718921154571004
+            "x": 6.401594301701064,
+            "y": 6.501144389493677,
+            "heading": 0.013520442621996222,
+            "angularVelocity": -1.0407990272609098e-7,
+            "velocityX": 3.604199304853526,
+            "velocityY": -1.1490805749601392,
+            "timestamp": 1.7206271190792162
         },
         {
-            "x": 6.699924709540006,
-            "y": 6.368695406956045,
-            "heading": -2.710512603773811,
-            "angularVelocity": -4.2852173297406095e-7,
-            "velocityX": 3.267551702823897,
-            "velocityY": -1.9062382882218016,
-            "timestamp": 1.8007745428839088
+            "x": 6.675154779128766,
+            "y": 6.355418517983256,
+            "heading": 0.013520441934185553,
+            "angularVelocity": -8.394627700401504e-9,
+            "velocityX": 3.3387652456948387,
+            "velocityY": -1.7785627506304358,
+            "timestamp": 1.802561743797274
         },
         {
-            "x": 6.967384720466867,
-            "y": 6.212663057124264,
-            "heading": -2.710512638850014,
-            "angularVelocity": -4.2852475398494205e-7,
-            "velocityX": 3.2675496572533906,
-            "velocityY": -1.9062417946012,
-            "timestamp": 1.8826279311968137
+            "x": 6.946083635654423,
+            "y": 6.204856506903887,
+            "heading": 0.013520441248790115,
+            "angularVelocity": -8.365150168101951e-9,
+            "velocityX": 3.3066467010490315,
+            "velocityY": -1.8375871201883405,
+            "timestamp": 1.884496368515332
         },
         {
-            "x": 7.2050885186595135,
-            "y": 6.073759568822601,
-            "heading": -2.7948440651741784,
-            "angularVelocity": -1.0302741042531607,
-            "velocityX": 2.9040190405309123,
-            "velocityY": -1.6969790886440652,
-            "timestamp": 1.9644813195097186
+            "x": 7.190722413199009,
+            "y": 6.068904365786755,
+            "heading": 0.008013663922991063,
+            "angularVelocity": -0.06720940438487652,
+            "velocityX": 2.985780167864364,
+            "velocityY": -1.6592758139181236,
+            "timestamp": 1.9664309932333899
         },
         {
-            "x": 7.39556651196216,
-            "y": 5.9626310397123286,
-            "heading": -2.9179406119000064,
-            "angularVelocity": -1.5038662328217944,
-            "velocityX": 2.3270630236404783,
-            "velocityY": -1.3576533775908837,
-            "timestamp": 2.0463347078226235
+            "x": 7.38643344699072,
+            "y": 5.96014263272892,
+            "heading": 0.0036082164005303732,
+            "angularVelocity": -0.053767836706643944,
+            "velocityX": 2.3886242777722417,
+            "velocityY": -1.3274208972347252,
+            "timestamp": 2.048365617951448
         },
         {
-            "x": 7.537983690499959,
-            "y": 5.879640169020095,
-            "heading": -3.0253464428320265,
-            "angularVelocity": -1.3121733033388265,
-            "velocityX": 1.7399057201318795,
-            "velocityY": -1.0138965826922115,
-            "timestamp": 2.1281880961355286
+            "x": 7.533216723981145,
+            "y": 5.878571322292618,
+            "heading": 0.0003041162966361105,
+            "angularVelocity": -0.04032605403715309,
+            "velocityX": 1.7914682284261954,
+            "velocityY": -0.9955658028213874,
+            "timestamp": 2.130300242669506
         },
         {
-            "x": 7.632713372468284,
-            "y": 5.824499681694904,
-            "heading": -3.1026583555829736,
-            "angularVelocity": -0.9445169509098753,
-            "velocityX": 1.1573092320405494,
-            "velocityY": -0.6736494171066164,
-            "timestamp": 2.2100414844484337
+            "x": 7.631072241649754,
+            "y": 5.82419044260697,
+            "heading": -0.0018986254325586427,
+            "angularVelocity": -0.026884137649675257,
+            "velocityX": 1.1943121483174686,
+            "velocityY": -0.66371060919331,
+            "timestamp": 2.212234867387564
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.49511635904048823,
-            "velocityX": 0.5776990849902374,
-            "velocityY": -0.33596265544659853,
-            "timestamp": 2.291894872761339
+            "heading": -0.003,
+            "angularVelocity": -0.013442114017501823,
+            "velocityX": 0.5971560682508651,
+            "velocityY": -0.3318553383326643,
+            "timestamp": 2.294169492105622
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": -3.143185307179586,
-            "angularVelocity": -7.614661899058019e-31,
+            "heading": -0.003,
+            "angularVelocity": -2.3555736468302963e-30,
             "velocityX": 0,
-            "velocityY": -2.3591575038755638e-32,
-            "timestamp": 2.373748261074244
+            "velocityY": 5.757868192156219e-33,
+            "timestamp": 2.3761041168236803
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.traj
index 6b188abb..1088f323 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.traj	
@@ -3,272 +3,272 @@
         {
             "x": 0.71,
             "y": 6.72,
-            "heading": -2.1,
+            "heading": 1.04,
             "angularVelocity": 0,
-            "velocityX": -7.879763479482e-33,
+            "velocityX": 8.672453699203877e-33,
             "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.7634290430022197,
-            "y": 6.720160003423765,
-            "heading": -2.158070133372329,
-            "angularVelocity": -0.7094408010373536,
-            "velocityX": 0.6527407612984071,
-            "velocityY": 0.0019547562668324166,
-            "timestamp": 0.08185338831290496
+            "x": 0.7586383226811528,
+            "y": 6.716006460523898,
+            "heading": 0.9415437629009579,
+            "angularVelocity": -1.201643840291405,
+            "velocityX": 0.5936235486342963,
+            "velocityY": -0.048740559804155884,
+            "timestamp": 0.08193462471805792
         },
         {
-            "x": 0.8707119576691547,
-            "y": 6.720819673735276,
-            "heading": -2.2686879013575907,
-            "angularVelocity": -1.351413426679394,
-            "velocityX": 1.3106716396005418,
-            "velocityY": 0.008059169267236224,
-            "timestamp": 0.16370677662580993
+            "x": 0.8580797410236602,
+            "y": 6.713039346170398,
+            "heading": 0.7612656224060018,
+            "angularVelocity": -2.2002680931938623,
+            "velocityX": 1.2136678319415195,
+            "velocityY": -0.03621319269735464,
+            "timestamp": 0.16386924943611583
         },
         {
-            "x": 1.0324112516405533,
-            "y": 6.722281472922437,
-            "heading": -2.4240599050623355,
-            "angularVelocity": -1.898174368919177,
-            "velocityX": 1.9754746541861241,
-            "velocityY": 0.017858749861073935,
-            "timestamp": 0.2455601649387149
+            "x": 1.0092416285400778,
+            "y": 6.712497099188053,
+            "heading": 0.5037500418751092,
+            "angularVelocity": -3.142939647518002,
+            "velocityX": 1.8449085235524658,
+            "velocityY": -0.006618044376361366,
+            "timestamp": 0.24580387415417376
         },
         {
-            "x": 1.249511925601742,
-            "y": 6.724737672202976,
-            "heading": -2.6048330548635152,
-            "angularVelocity": -2.2084992878991065,
-            "velocityX": 2.652311387908187,
-            "velocityY": 0.030007301239015387,
-            "timestamp": 0.32741355325161986
+            "x": 1.2158692368887885,
+            "y": 6.714801206948958,
+            "heading": 0.22114949007633808,
+            "angularVelocity": -3.4490980189537326,
+            "velocityX": 2.5218594588030294,
+            "velocityY": 0.028121295103663166,
+            "timestamp": 0.32773849887223166
         },
         {
-            "x": 1.518512933103673,
-            "y": 6.730171304043105,
-            "heading": -2.7105117995327306,
-            "angularVelocity": -1.2910735504953308,
-            "velocityX": 3.2863759588498382,
-            "velocityY": 0.0663824913314231,
-            "timestamp": 0.4092669415645248
+            "x": 1.4708450332398146,
+            "y": 6.7208733735889465,
+            "heading": 0.04860948021713714,
+            "angularVelocity": -2.105825351039585,
+            "velocityX": 3.1119419565100066,
+            "velocityY": 0.07410989750528489,
+            "timestamp": 0.40967312359028957
         },
         {
-            "x": 1.8281517964730614,
-            "y": 6.7323448579922625,
-            "heading": -2.71051185012585,
-            "angularVelocity": -6.180943842017169e-7,
-            "velocityX": 3.782847231512476,
-            "velocityY": 0.026554233049558205,
-            "timestamp": 0.49112032987742976
+            "x": 1.7704680006957136,
+            "y": 6.724924507610315,
+            "heading": 0.013522063773945665,
+            "angularVelocity": -0.42823673830116915,
+            "velocityX": 3.6568540893049817,
+            "velocityY": 0.049443492727383076,
+            "timestamp": 0.49160774830834747
         },
         {
-            "x": 2.1377906615261124,
-            "y": 6.734518172124384,
-            "heading": -2.710511900718092,
-            "angularVelocity": -6.180836584503726e-7,
-            "velocityX": 3.7828472520817247,
-            "velocityY": 0.026551303213165345,
-            "timestamp": 0.5729737181903347
+            "x": 2.080344251704432,
+            "y": 6.7179919202849705,
+            "heading": 0.013521948546161235,
+            "angularVelocity": -0.000001406338099756076,
+            "velocityX": 3.78199390154067,
+            "velocityY": -0.08461120496004566,
+            "timestamp": 0.5735423730264054
         },
         {
-            "x": 2.4474295268603647,
-            "y": 6.736691446193322,
-            "heading": -2.7105119513103224,
-            "angularVelocity": -6.180835185678023e-7,
-            "velocityX": 3.7828472555171566,
-            "velocityY": 0.026550813762651566,
-            "timestamp": 0.6548271065032397
+            "x": 2.3902205002379273,
+            "y": 6.711059222319389,
+            "heading": 0.013521833318547862,
+            "angularVelocity": -0.000001406336012025862,
+            "velocityX": 3.781993871330935,
+            "velocityY": -0.08461255530781114,
+            "timestamp": 0.6554769977444633
         },
         {
-            "x": 2.7570683925033053,
-            "y": 6.738864676282315,
-            "heading": -2.7105120019025377,
-            "angularVelocity": -6.180833387834062e-7,
-            "velocityX": 3.7828472592883804,
-            "velocityY": 0.026550276461191975,
-            "timestamp": 0.7366804948161447
+            "x": 2.7000967477974154,
+            "y": 6.704126480817366,
+            "heading": 0.013521718090975673,
+            "angularVelocity": -0.000001406335509399091,
+            "velocityX": 3.7819938594433236,
+            "velocityY": -0.08461308666366031,
+            "timestamp": 0.7374116224625212
         },
         {
-            "x": 3.0667072584849335,
-            "y": 6.741037858115924,
-            "heading": -2.7105120524947366,
-            "angularVelocity": -6.180831373401399e-7,
-            "velocityX": 3.7828472634261248,
-            "velocityY": 0.026549686926837145,
-            "timestamp": 0.8185338831290497
+            "x": 3.009972994287386,
+            "y": 6.6971936915101855,
+            "heading": 0.013521602863440677,
+            "angularVelocity": -0.0000014063350554339165,
+            "velocityX": 3.7819938463900247,
+            "velocityY": -0.08461367011854067,
+            "timestamp": 0.8193462471805791
         },
         {
-            "x": 3.376346124838015,
-            "y": 6.7432109870244785,
-            "heading": -2.7105121030869213,
-            "angularVelocity": -6.18082958211415e-7,
-            "velocityX": 3.7828472679641574,
-            "velocityY": 0.02654904034329684,
-            "timestamp": 0.9003872714419547
+            "x": 3.319849239613809,
+            "y": 6.690260850195287,
+            "heading": 0.013521487635941729,
+            "angularVelocity": -0.0000014063346155021318,
+            "velocityX": 3.781993832189087,
+            "velocityY": -0.08461430486506759,
+            "timestamp": 0.901280871898637
         },
         {
-            "x": 3.6859849915958844,
-            "y": 6.745384058256853,
-            "heading": -2.710512153679093,
-            "angularVelocity": -6.180828069034381e-7,
-            "velocityX": 3.782847272909432,
-            "velocityY": 0.026548335715409193,
-            "timestamp": 0.9822406597548597
+            "x": 3.629725483679546,
+            "y": 6.683327952531275,
+            "heading": 0.013521372408489362,
+            "angularVelocity": -0.0000014063340469636637,
+            "velocityX": 3.7819938168026104,
+            "velocityY": -0.0846149925976696,
+            "timestamp": 0.9832154966166949
         },
         {
-            "x": 3.99562385879327,
-            "y": 6.747557066862843,
-            "heading": -2.7105122042712537,
-            "angularVelocity": -6.180826661377271e-7,
-            "velocityX": 3.7828472782789873,
-            "velocityY": 0.026547570611047785,
-            "timestamp": 1.0640940480677648
+            "x": 3.939601726375346,
+            "y": 6.6763949936353555,
+            "heading": 0.013521257181093756,
+            "angularVelocity": -0.0000014063333542144944,
+            "velocityX": 3.7819938000827285,
+            "velocityY": -0.08461573992406812,
+            "timestamp": 1.065150121334753
         },
         {
-            "x": 4.305262726470487,
-            "y": 6.749730007095753,
-            "heading": -2.710512254863404,
-            "angularVelocity": -6.180825475741077e-7,
-            "velocityX": 3.782847284141071,
-            "velocityY": 0.026546735299496697,
-            "timestamp": 1.1459474363806696
+            "x": 4.249477967564444,
+            "y": 6.66946196739526,
+            "heading": 0.013521141953758897,
+            "angularVelocity": -0.0000014063326128127892,
+            "velocityX": 3.781993781693661,
+            "velocityY": -0.0846165618498062,
+            "timestamp": 1.1470847460528109
         },
         {
-            "x": 4.614901594678237,
-            "y": 6.751902871728125,
-            "heading": -2.710512305455549,
-            "angularVelocity": -6.180824717562797e-7,
-            "velocityX": 3.78284729062258,
-            "velocityY": 0.0265458116903545,
-            "timestamp": 1.2278008246935745
+            "x": 4.559354207050928,
+            "y": 6.662528865055332,
+            "heading": 0.013521026726476763,
+            "angularVelocity": -0.0000014063319692972842,
+            "velocityX": 3.7819937609134957,
+            "velocityY": -0.08461749063701171,
+            "timestamp": 1.2290193707708688
         },
         {
-            "x": 4.924540463476948,
-            "y": 6.754075652145792,
-            "heading": -2.7105123560476914,
-            "angularVelocity": -6.180824480187367e-7,
-            "velocityX": 3.782847297842324,
-            "velocityY": 0.02654478284224149,
-            "timestamp": 1.3096542130064794
+            "x": 4.869230444482983,
+            "y": 6.655595670892424,
+            "heading": 0.01352091149922989,
+            "angularVelocity": -0.0000014063315389464776,
+            "velocityX": 3.7819937358394955,
+            "velocityY": -0.08461861132293771,
+            "timestamp": 1.3109539954889267
         },
         {
-            "x": 5.234179332930726,
-            "y": 6.756248339209459,
-            "heading": -2.7105124066398263,
-            "angularVelocity": -6.180823518950674e-7,
-            "velocityX": 3.782847305845261,
-            "velocityY": 0.026543642339650734,
-            "timestamp": 1.3915076013193843
+            "x": 5.1791066790320945,
+            "y": 6.648662347878877,
+            "heading": 0.013520796271957977,
+            "angularVelocity": -0.0000014063318445457383,
+            "velocityX": 3.7819937006535858,
+            "velocityY": -0.08462018392597198,
+            "timestamp": 1.3928886202069846
         },
         {
-            "x": 5.543818204521741,
-            "y": 6.758420721665928,
-            "heading": -2.710512457231931,
-            "angularVelocity": -6.180819838461299e-7,
-            "velocityX": 3.782847331955805,
-            "velocityY": 0.026539920964112583,
-            "timestamp": 1.4733609896322892
+            "x": 5.488982909387702,
+            "y": 6.641728837446599,
+            "heading": 0.013520681044501265,
+            "angularVelocity": -0.000001406334100009416,
+            "velocityX": 3.781993649472511,
+            "velocityY": -0.08462247134390694,
+            "timestamp": 1.4748232449250425
         },
         {
-            "x": 5.852559225022806,
-            "y": 6.734757839446224,
-            "heading": -2.710512508060742,
-            "angularVelocity": -6.209738209329952e-7,
-            "velocityX": 3.771878316396938,
-            "velocityY": -0.2890886096156145,
-            "timestamp": 1.5552143779451941
+            "x": 5.798859128679326,
+            "y": 6.634794832555163,
+            "heading": 0.013520565816988437,
+            "angularVelocity": -0.0000014063347848865157,
+            "velocityX": 3.781993514438203,
+            "velocityY": -0.08462850614495548,
+            "timestamp": 1.5567578696431004
         },
         {
-            "x": 6.152023822280739,
-            "y": 6.6560056480568655,
-            "heading": -2.710512532930908,
-            "angularVelocity": -3.03837945152059e-7,
-            "velocityX": 3.6585485760608294,
-            "velocityY": -0.9621127849748639,
-            "timestamp": 1.637067766258099
+            "x": 6.106285584248806,
+            "y": 6.595293875173847,
+            "heading": 0.013520451149743993,
+            "angularVelocity": -0.000001399496791973984,
+            "velocityX": 3.7520945098284613,
+            "velocityY": -0.48210335395128207,
+            "timestamp": 1.6386924943611583
         },
         {
-            "x": 6.432464531176267,
-            "y": 6.5247274697787905,
-            "heading": -2.710512568697855,
-            "angularVelocity": -4.369635492236228e-7,
-            "velocityX": 3.4261343931600465,
-            "velocityY": -1.603820941122575,
-            "timestamp": 1.718921154571004
+            "x": 6.401594301701064,
+            "y": 6.501144389493677,
+            "heading": 0.013520442621996222,
+            "angularVelocity": -1.0407990272609098e-7,
+            "velocityX": 3.604199304853526,
+            "velocityY": -1.1490805749601392,
+            "timestamp": 1.7206271190792162
         },
         {
-            "x": 6.699924709540006,
-            "y": 6.368695406956045,
-            "heading": -2.710512603773811,
-            "angularVelocity": -4.2852173297406095e-7,
-            "velocityX": 3.267551702823897,
-            "velocityY": -1.9062382882218016,
-            "timestamp": 1.8007745428839088
+            "x": 6.675154779128766,
+            "y": 6.355418517983256,
+            "heading": 0.013520441934185553,
+            "angularVelocity": -8.394627700401504e-9,
+            "velocityX": 3.3387652456948387,
+            "velocityY": -1.7785627506304358,
+            "timestamp": 1.802561743797274
         },
         {
-            "x": 6.967384720466867,
-            "y": 6.212663057124264,
-            "heading": -2.710512638850014,
-            "angularVelocity": -4.2852475398494205e-7,
-            "velocityX": 3.2675496572533906,
-            "velocityY": -1.9062417946012,
-            "timestamp": 1.8826279311968137
+            "x": 6.946083635654423,
+            "y": 6.204856506903887,
+            "heading": 0.013520441248790115,
+            "angularVelocity": -8.365150168101951e-9,
+            "velocityX": 3.3066467010490315,
+            "velocityY": -1.8375871201883405,
+            "timestamp": 1.884496368515332
         },
         {
-            "x": 7.2050885186595135,
-            "y": 6.073759568822601,
-            "heading": -2.7948440651741784,
-            "angularVelocity": -1.0302741042531607,
-            "velocityX": 2.9040190405309123,
-            "velocityY": -1.6969790886440652,
-            "timestamp": 1.9644813195097186
+            "x": 7.190722413199009,
+            "y": 6.068904365786755,
+            "heading": 0.008013663922991063,
+            "angularVelocity": -0.06720940438487652,
+            "velocityX": 2.985780167864364,
+            "velocityY": -1.6592758139181236,
+            "timestamp": 1.9664309932333899
         },
         {
-            "x": 7.39556651196216,
-            "y": 5.9626310397123286,
-            "heading": -2.9179406119000064,
-            "angularVelocity": -1.5038662328217944,
-            "velocityX": 2.3270630236404783,
-            "velocityY": -1.3576533775908837,
-            "timestamp": 2.0463347078226235
+            "x": 7.38643344699072,
+            "y": 5.96014263272892,
+            "heading": 0.0036082164005303732,
+            "angularVelocity": -0.053767836706643944,
+            "velocityX": 2.3886242777722417,
+            "velocityY": -1.3274208972347252,
+            "timestamp": 2.048365617951448
         },
         {
-            "x": 7.537983690499959,
-            "y": 5.879640169020095,
-            "heading": -3.0253464428320265,
-            "angularVelocity": -1.3121733033388265,
-            "velocityX": 1.7399057201318795,
-            "velocityY": -1.0138965826922115,
-            "timestamp": 2.1281880961355286
+            "x": 7.533216723981145,
+            "y": 5.878571322292618,
+            "heading": 0.0003041162966361105,
+            "angularVelocity": -0.04032605403715309,
+            "velocityX": 1.7914682284261954,
+            "velocityY": -0.9955658028213874,
+            "timestamp": 2.130300242669506
         },
         {
-            "x": 7.632713372468284,
-            "y": 5.824499681694904,
-            "heading": -3.1026583555829736,
-            "angularVelocity": -0.9445169509098753,
-            "velocityX": 1.1573092320405494,
-            "velocityY": -0.6736494171066164,
-            "timestamp": 2.2100414844484337
+            "x": 7.631072241649754,
+            "y": 5.82419044260697,
+            "heading": -0.0018986254325586427,
+            "angularVelocity": -0.026884137649675257,
+            "velocityX": 1.1943121483174686,
+            "velocityY": -0.66371060919331,
+            "timestamp": 2.212234867387564
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.49511635904048823,
-            "velocityX": 0.5776990849902374,
-            "velocityY": -0.33596265544659853,
-            "timestamp": 2.291894872761339
+            "heading": -0.003,
+            "angularVelocity": -0.013442114017501823,
+            "velocityX": 0.5971560682508651,
+            "velocityY": -0.3318553383326643,
+            "timestamp": 2.294169492105622
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": -3.143185307179586,
-            "angularVelocity": -7.614661899058019e-31,
+            "heading": -0.003,
+            "angularVelocity": -2.3555736468302963e-30,
             "velocityX": 0,
-            "velocityY": -2.3591575038755638e-32,
-            "timestamp": 2.373748261074244
+            "velocityY": 5.757868192156219e-33,
+            "timestamp": 2.3761041168236803
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.1.traj
index 38312c6d..463b42ef 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.1.traj	
@@ -3,524 +3,524 @@
         {
             "x": 0.71,
             "y": 6.72,
-            "heading": -2.1,
+            "heading": 1.04,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": 2.27986083101055e-33,
+            "velocityY": 1.3928609100313025e-33,
             "timestamp": 0
         },
         {
-            "x": 0.7292945467072102,
-            "y": 6.711630943578302,
-            "heading": -2.0466837783195437,
-            "angularVelocity": 0.9750423416679173,
-            "velocityX": 0.3528569619125016,
-            "velocityY": -0.1530525628742106,
-            "timestamp": 0.05468092963968429
-        },
-        {
-            "x": 0.7686895561493499,
-            "y": 6.695016697545084,
-            "heading": -1.9453579324448191,
-            "angularVelocity": 1.853038098335252,
-            "velocityX": 0.720452444787058,
-            "velocityY": -0.30383986049058775,
-            "timestamp": 0.10936185927936858
-        },
-        {
-            "x": 0.8291498926090706,
-            "y": 6.670065411203325,
-            "heading": -1.8043277140562832,
-            "angularVelocity": 2.579148147587168,
-            "velocityX": 1.1056932802371746,
-            "velocityY": -0.4563069155219838,
-            "timestamp": 0.16404278891905286
-        },
-        {
-            "x": 0.9116442238767206,
-            "y": 6.636292399159329,
-            "heading": -1.6353628554936872,
-            "angularVelocity": 3.09001437385898,
-            "velocityX": 1.50864902647486,
-            "velocityY": -0.6176378541210026,
-            "timestamp": 0.21872371855873715
-        },
-        {
-            "x": 1.0169609100629697,
-            "y": 6.593200359597327,
-            "heading": -1.4531033706452814,
-            "angularVelocity": 3.333145322316033,
-            "velocityX": 1.926022232617937,
-            "velocityY": -0.7880634042974968,
-            "timestamp": 0.2734046481984214
-        },
-        {
-            "x": 1.1451022790503944,
-            "y": 6.540887351226898,
-            "heading": -1.2848862020086818,
-            "angularVelocity": 3.0763406866901017,
-            "velocityX": 2.3434380108714725,
-            "velocityY": -0.9566956654750047,
-            "timestamp": 0.3280855778381057
-        },
-        {
-            "x": 1.291419394840941,
-            "y": 6.479714636695398,
-            "heading": -1.166754837939456,
-            "angularVelocity": 2.1603759271768683,
-            "velocityX": 2.6758344591924805,
-            "velocityY": -1.1187211873425027,
-            "timestamp": 0.38276650747779
-        },
-        {
-            "x": 1.45556402161766,
-            "y": 6.408096144298377,
-            "heading": -1.0985152891132095,
-            "angularVelocity": 1.24795882725304,
-            "velocityX": 3.0018624017246447,
-            "velocityY": -1.3097526481159927,
-            "timestamp": 0.4374474371174743
-        },
-        {
-            "x": 1.6376170855458871,
-            "y": 6.326295801964295,
-            "heading": -1.0805867815133028,
-            "angularVelocity": 0.3278749596622653,
-            "velocityX": 3.32937031480356,
-            "velocityY": -1.495957418301032,
-            "timestamp": 0.4921283667571586
-        },
-        {
-            "x": 1.828511449146643,
-            "y": 6.246620230558847,
-            "heading": -1.0805866805891169,
-            "angularVelocity": 0.000001845692576742716,
-            "velocityX": 3.491059220438268,
-            "velocityY": -1.4570997956776681,
-            "timestamp": 0.5468092963968428
-        },
-        {
-            "x": 2.0194058587075467,
-            "y": 6.16694476926885,
-            "heading": -1.0805865796652752,
-            "angularVelocity": 0.0000018456862813728727,
-            "velocityX": 3.491060060953374,
-            "velocityY": -1.4570977818960094,
-            "timestamp": 0.6014902260365271
-        },
-        {
-            "x": 2.2103002634541666,
-            "y": 6.087269296444259,
-            "heading": -1.0805864787414845,
-            "angularVelocity": 0.000001845685351855892,
-            "velocityX": 3.491059972910188,
-            "velocityY": -1.4570979928396652,
-            "timestamp": 0.6561711556762114
-        },
-        {
-            "x": 2.4011946628107936,
-            "y": 6.007593810705732,
-            "heading": -1.0805863778177442,
-            "angularVelocity": 0.000001845684424329729,
-            "velocityX": 3.4910598743384673,
-            "velocityY": -1.457098229008597,
-            "timestamp": 0.7108520853158957
-        },
-        {
-            "x": 2.5920890560965946,
-            "y": 5.927918310422065,
-            "heading": -1.0805862768940542,
-            "angularVelocity": 0.0000018456835100189827,
-            "velocityX": 3.4910597633157407,
-            "velocityY": -1.45709849500883,
-            "timestamp": 0.76553301495558
-        },
-        {
-            "x": 2.7829834425031943,
-            "y": 5.8482427936564765,
-            "heading": -1.0805861759704127,
-            "angularVelocity": 0.0000018456826171919115,
-            "velocityX": 3.4910596375095175,
-            "velocityY": -1.4570987964287467,
-            "timestamp": 0.8202139445952643
-        },
-        {
-            "x": 2.9738778210651113,
-            "y": 5.768567258095778,
-            "heading": -1.080586075046818,
-            "angularVelocity": 0.0000018456817639254375,
-            "velocityX": 3.4910594940466586,
-            "velocityY": -1.4570991401520532,
-            "timestamp": 0.8748948742349486
-        },
-        {
-            "x": 3.164772190605788,
-            "y": 5.688891700921064,
-            "heading": -1.0805859741232662,
-            "angularVelocity": 0.0000018456809789441398,
-            "velocityX": 3.491059329067008,
-            "velocityY": -1.4570995354272234,
-            "timestamp": 0.9295758038746329
-        },
-        {
-            "x": 3.3556665493271205,
-            "y": 5.609216117824277,
-            "heading": -1.0805858731997562,
-            "angularVelocity": 0.000001845680216876217,
-            "velocityX": 3.491059131203806,
-            "velocityY": -1.4571000094878162,
-            "timestamp": 0.9842567335143172
-        },
-        {
-            "x": 3.5465606286609406,
-            "y": 5.529539865347568,
-            "heading": -1.0805857722761907,
-            "angularVelocity": 0.0000018456812299162406,
-            "velocityX": 3.491054021789706,
-            "velocityY": -1.4571122510486052,
-            "timestamp": 1.0389376631540015
-        },
-        {
-            "x": 3.7302882443981766,
-            "y": 5.43449767788976,
-            "heading": -1.0805856670205962,
-            "angularVelocity": 0.0000019249049955796687,
-            "velocityX": 3.3599943700279993,
-            "velocityY": -1.7381231095389325,
-            "timestamp": 1.0936185927936857
-        },
-        {
-            "x": 3.901247591247985,
-            "y": 5.318042735383766,
-            "heading": -1.0805855451412456,
-            "angularVelocity": 0.000002228918769474178,
-            "velocityX": 3.1264893990707807,
-            "velocityY": -2.12971767805275,
-            "timestamp": 1.1482995224333699
-        },
-        {
-            "x": 4.056955156553094,
-            "y": 5.181866377235877,
-            "heading": -1.0805853830111598,
-            "angularVelocity": 0.0000029650206572180867,
-            "velocityX": 2.8475661685185734,
-            "velocityY": -2.490381181249336,
-            "timestamp": 1.202980452073054
-        },
-        {
-            "x": 4.195149083685748,
-            "y": 5.0279466523676755,
-            "heading": -1.080585195813012,
-            "angularVelocity": 0.0000034234631562085443,
-            "velocityX": 2.527278304214503,
-            "velocityY": -2.81487030089728,
-            "timestamp": 1.2576613817127382
-        },
-        {
-            "x": 4.313735776578777,
-            "y": 4.858628961688981,
-            "heading": -1.081017907841774,
-            "angularVelocity": -0.007913399271258946,
-            "velocityX": 2.168702940393416,
-            "velocityY": -3.0964669363597066,
-            "timestamp": 1.3123423113524224
-        },
-        {
-            "x": 4.409971486167556,
-            "y": 4.678531583119963,
-            "heading": -1.0900704798968306,
-            "angularVelocity": -0.16555263626108413,
-            "velocityX": 1.759950136600006,
-            "velocityY": -3.293604914103617,
-            "timestamp": 1.3670232409921066
-        },
-        {
-            "x": 4.510748059482075,
-            "y": 4.519168046099015,
-            "heading": -1.1448684085004224,
-            "angularVelocity": -1.0021396666932787,
-            "velocityX": 1.8429930503848142,
-            "velocityY": -2.9144262555714096,
-            "timestamp": 1.4217041706317908
-        },
-        {
-            "x": 4.62613024170022,
-            "y": 4.379607058549792,
-            "heading": -1.2139690573229496,
-            "angularVelocity": -1.2637065477463625,
-            "velocityX": 2.110099132886858,
-            "velocityY": -2.552278984078161,
-            "timestamp": 1.476385100271475
-        },
-        {
-            "x": 4.756158352198169,
-            "y": 4.26013085900244,
-            "heading": -1.289886392340967,
-            "angularVelocity": -1.3883695013648907,
-            "velocityX": 2.3779425725707126,
-            "velocityY": -2.1849701593340063,
-            "timestamp": 1.5310660299111591
+            "x": 0.729279758485921,
+            "y": 6.711628327586588,
+            "heading": 1.093428142295198,
+            "angularVelocity": 0.9770378814200095,
+            "velocityX": 0.352568020824986,
+            "velocityY": -0.15309237280887605,
+            "timestamp": 0.05468379815278649
+        },
+        {
+            "x": 0.7686493967347447,
+            "y": 6.69501140584797,
+            "heading": 1.1949690599508662,
+            "angularVelocity": 1.8568739020644303,
+            "velocityX": 0.7199506906748694,
+            "velocityY": -0.30387285265355396,
+            "timestamp": 0.10936759630557298
+        },
+        {
+            "x": 0.8290790315170126,
+            "y": 6.670059182269693,
+            "heading": 1.3362954219692313,
+            "angularVelocity": 2.584428419245852,
+            "velocityX": 1.1050738394839994,
+            "velocityY": -0.4563001185206737,
+            "timestamp": 0.16405139445835948
+        },
+        {
+            "x": 0.9115429372168562,
+            "y": 6.636285478537955,
+            "heading": 1.5055888656223098,
+            "angularVelocity": 3.0958611027725738,
+            "velocityX": 1.508013497333151,
+            "velocityY": -0.6176181039468854,
+            "timestamp": 0.21873519261114596
+        },
+        {
+            "x": 1.0168329273736771,
+            "y": 6.593191021214006,
+            "heading": 1.6881423696003794,
+            "angularVelocity": 3.3383471913932383,
+            "velocityX": 1.9254330114861617,
+            "velocityY": -0.7880662788554578,
+            "timestamp": 0.27341899076393245
+        },
+        {
+            "x": 1.1449422871140338,
+            "y": 6.540878714080721,
+            "heading": 1.8564615478628756,
+            "angularVelocity": 3.078044758197163,
+            "velocityX": 2.3427297310698694,
+            "velocityY": -0.9566326572109004,
+            "timestamp": 0.32810278891671896
+        },
+        {
+            "x": 1.2912409780613738,
+            "y": 6.479735868066328,
+            "heading": 1.974688906048384,
+            "angularVelocity": 2.1620180415263293,
+            "velocityX": 2.675357160425865,
+            "velocityY": -1.1181162991560991,
+            "timestamp": 0.38278658706950547
+        },
+        {
+            "x": 1.4553681499836075,
+            "y": 6.408144093279595,
+            "heading": 2.0430197361851268,
+            "angularVelocity": 1.2495626208301445,
+            "velocityX": 3.001385738855639,
+            "velocityY": -1.3091953595963746,
+            "timestamp": 0.437470385222292
+        },
+        {
+            "x": 1.6374055328463037,
+            "y": 6.326368400043195,
+            "heading": 2.0610350683831213,
+            "angularVelocity": 0.3294455178051113,
+            "velocityX": 3.328908909254691,
+            "velocityY": -1.4954281889476506,
+            "timestamp": 0.4921541833750785
+        },
+        {
+            "x": 1.8283093127689483,
+            "y": 6.246687216839916,
+            "heading": 2.061035169590908,
+            "angularVelocity": 0.000001850781943801841,
+            "velocityX": 3.4910482880003957,
+            "velocityY": -1.45712598420193,
+            "timestamp": 0.546837981527865
+        },
+        {
+            "x": 2.01921313853121,
+            "y": 6.167006143460992,
+            "heading": 2.0610352707983473,
+            "angularVelocity": 0.0000018507755948234292,
+            "velocityX": 3.491049126267287,
+            "velocityY": -1.4571239758492094,
+            "timestamp": 0.6015217796806515
+        },
+        {
+            "x": 2.210116959480156,
+            "y": 6.087325058550032,
+            "heading": 2.061035372005735,
+            "angularVelocity": 0.000001850774661522582,
+            "velocityX": 3.4910490382464,
+            "velocityY": -1.4571241867349893,
+            "timestamp": 0.6562055778334379
+        },
+        {
+            "x": 2.40102077504018,
+            "y": 6.007643960727976,
+            "heading": 2.0610354732130722,
+            "angularVelocity": 0.0000018507737318277825,
+            "velocityX": 3.49104893969945,
+            "velocityY": -1.4571244228395928,
+            "timestamp": 0.7108893759862244
+        },
+        {
+            "x": 2.5919245845305627,
+            "y": 5.927962848363926,
+            "heading": 2.0610355744203592,
+            "angularVelocity": 0.0000018507728139380304,
+            "velocityX": 3.491048828704212,
+            "velocityY": -1.457124688768356,
+            "timestamp": 0.7655731741390108
+        },
+        {
+            "x": 2.78282838714305,
+            "y": 5.848281719521431,
+            "heading": 2.0610356756275974,
+            "angularVelocity": 0.0000018507719179726602,
+            "velocityX": 3.4910487029284663,
+            "velocityY": -1.457124990108895,
+            "timestamp": 0.8202569722917973
+        },
+        {
+            "x": 2.973732181912291,
+            "y": 5.768600571887663,
+            "heading": 2.061035776834788,
+            "angularVelocity": 0.0000018507710613284672,
+            "velocityX": 3.491048559499401,
+            "velocityY": -1.4571253337439838,
+            "timestamp": 0.8749407704445837
+        },
+        {
+            "x": 3.16463596766188,
+            "y": 5.688919402644143,
+            "heading": 2.061035878041936,
+            "angularVelocity": 0.000001850770272196717,
+            "velocityX": 3.4910483945574473,
+            "velocityY": -1.4571257289205004,
+            "timestamp": 0.9296245685973702
+        },
+        {
+            "x": 3.3555397425941185,
+            "y": 5.609238207483871,
+            "heading": 2.061035979249042,
+            "angularVelocity": 0.000001850769506382635,
+            "velocityX": 3.4910481967410822,
+            "velocityY": -1.457126202858873,
+            "timestamp": 0.9843083667501566
+        },
+        {
+            "x": 3.546443239175115,
+            "y": 5.529556345440524,
+            "heading": 2.0610360804562036,
+            "angularVelocity": 0.0000018507705174022697,
+            "velocityX": 3.491043106545231,
+            "velocityY": -1.4571383981177848,
+            "timestamp": 1.0389921649029432
+        },
+        {
+            "x": 3.7301891812097523,
+            "y": 5.434525969364101,
+            "heading": 2.0610361860102304,
+            "angularVelocity": 0.0000019302614304565207,
+            "velocityX": 3.360153249071168,
+            "velocityY": -1.7378159397580357,
+            "timestamp": 1.0936759630557298
+        },
+        {
+            "x": 3.9011674048607876,
+            "y": 5.318079465137689,
+            "heading": 2.0610363082632275,
+            "angularVelocity": 0.0000022356347107367405,
+            "velocityX": 3.126670593972313,
+            "velocityY": -2.129451650396711,
+            "timestamp": 1.1483597612085163
+        },
+        {
+            "x": 4.05689386248419,
+            "y": 5.181908226701039,
+            "heading": 2.0610364709885136,
+            "angularVelocity": 0.0000029757495174652245,
+            "velocityX": 2.8477622784778513,
+            "velocityY": -2.490156920998575,
+            "timestamp": 1.203043559361303
+        },
+        {
+            "x": 4.195106185477333,
+            "y": 5.027990436285506,
+            "heading": 2.061036659879246,
+            "angularVelocity": 0.0000034542357869786317,
+            "velocityX": 2.52748213660978,
+            "velocityY": -2.814687267798894,
+            "timestamp": 1.2577273575140895
+        },
+        {
+            "x": 4.313711649853664,
+            "y": 4.858669951905228,
+            "heading": 2.0606108116890636,
+            "angularVelocity": -0.007787465475470693,
+            "velocityX": 2.168932451344132,
+            "velocityY": -3.096355595256869,
+            "timestamp": 1.312411155666876
+        },
+        {
+            "x": 4.40996698448611,
+            "y": 4.67856050613378,
+            "heading": 2.051588761211157,
+            "angularVelocity": -0.16498580535132465,
+            "velocityX": 1.7602166982532683,
+            "velocityY": -3.293652815925874,
+            "timestamp": 1.3670949538196626
+        },
+        {
+            "x": 4.510742037442817,
+            "y": 4.51918682060045,
+            "heading": 1.996778499947209,
+            "angularVelocity": -1.002312624862085,
+            "velocityX": 1.8428685709639585,
+            "velocityY": -2.9144589607334965,
+            "timestamp": 1.4217787519724492
+        },
+        {
+            "x": 4.62612450472694,
+            "y": 4.379617585881877,
+            "heading": 1.9276644376664414,
+            "angularVelocity": -1.263885549567404,
+            "velocityX": 2.1099936577511538,
+            "velocityY": -2.5522959163995464,
+            "timestamp": 1.4764625501252358
+        },
+        {
+            "x": 4.756154634809633,
+            "y": 4.26013511834241,
+            "heading": 1.8517335763640235,
+            "angularVelocity": -1.3885440270675238,
+            "velocityX": 2.3778547664042544,
+            "velocityY": -2.1849701662205963,
+            "timestamp": 1.5311463482780223
         },
         {
             "x": 4.900439739227295,
             "y": 4.161101341247559,
-            "heading": -1.3678659526433012,
-            "angularVelocity": -1.4260832947825601,
-            "velocityX": 2.638605231839619,
-            "velocityY": -1.8110430529880912,
-            "timestamp": 1.5857469595508433
-        },
-        {
-            "x": 5.00826704552834,
-            "y": 4.101917336684186,
-            "heading": -1.4217535551168057,
-            "angularVelocity": -1.4073230082403518,
-            "velocityX": 2.816006690010969,
-            "velocityY": -1.5456432930522144,
-            "timestamp": 1.6240378154225759
-        },
-        {
-            "x": 5.122657163782486,
-            "y": 4.053031768762854,
-            "heading": -1.4735948904430307,
-            "angularVelocity": -1.353882908752006,
-            "velocityX": 2.9874004027836745,
-            "velocityY": -1.2766903953541875,
-            "timestamp": 1.6623286712943084
-        },
-        {
-            "x": 5.24335031560154,
-            "y": 4.014573699061021,
-            "heading": -1.5218759194168912,
-            "angularVelocity": -1.2609023192271587,
-            "velocityX": 3.152009770253094,
-            "velocityY": -1.0043669389543197,
-            "timestamp": 1.700619527166041
-        },
-        {
-            "x": 5.370053209601022,
-            "y": 3.986652038058385,
-            "heading": -1.5647151394478649,
-            "angularVelocity": -1.1187846042009912,
-            "velocityX": 3.308959570502,
-            "velocityY": -0.729199187820931,
-            "timestamp": 1.7389103830377735
-        },
-        {
-            "x": 5.5024723935607405,
-            "y": 3.969300942477628,
-            "heading": -1.599514624372743,
-            "angularVelocity": -0.9088197203387187,
-            "velocityX": 3.458245603161718,
-            "velocityY": -0.4531394032789495,
-            "timestamp": 1.777201238909506
-        },
-        {
-            "x": 5.64052782398134,
-            "y": 3.9622378493580968,
-            "heading": -1.6219128460749601,
-            "angularVelocity": -0.5849496228876974,
-            "velocityX": 3.6054412281370607,
-            "velocityY": -0.1844590035592553,
-            "timestamp": 1.8154920947812385
-        },
-        {
-            "x": 5.78519636080909,
-            "y": 3.9633730625174985,
-            "heading": -1.6224984270976561,
-            "angularVelocity": -0.015292972940008887,
-            "velocityX": 3.778148425628394,
-            "velocityY": 0.02964710852127253,
-            "timestamp": 1.853782950652971
-        },
-        {
-            "x": 5.929431807514691,
-            "y": 3.9767231173381155,
-            "heading": -1.6224986694398071,
-            "angularVelocity": -0.000006328982347971965,
-            "velocityX": 3.7668378891493703,
-            "velocityY": 0.3486486399086302,
-            "timestamp": 1.8920738065247036
-        },
-        {
-            "x": 6.073330473121851,
-            "y": 3.9933141696465464,
-            "heading": -1.6224988987872904,
-            "angularVelocity": -0.000005989614964631434,
-            "velocityX": 3.758042549093057,
-            "velocityY": 0.4332901924158613,
-            "timestamp": 1.9303646623964361
-        },
-        {
-            "x": 6.217229120638352,
-            "y": 4.009905378860974,
-            "heading": -1.6224991281340264,
-            "angularVelocity": -0.000005989595448400927,
-            "velocityX": 3.7580420766392404,
-            "velocityY": 0.4332942901565158,
-            "timestamp": 1.9686555182681686
-        },
-        {
-            "x": 6.361127770496839,
-            "y": 4.0264965677626225,
-            "heading": -1.6224993574808346,
-            "angularVelocity": -0.000005989597330541617,
-            "velocityX": 3.7580421378023496,
-            "velocityY": 0.4332937596700765,
-            "timestamp": 2.0069463741399014
-        },
-        {
-            "x": 6.505026421139362,
-            "y": 4.0430877498636315,
-            "heading": -1.622499586827807,
-            "angularVelocity": -0.000005989601616416828,
-            "velocityX": 3.7580421582781125,
-            "velocityY": 0.43329358206529056,
-            "timestamp": 2.045237230011634
-        },
-        {
-            "x": 6.648925182384827,
-            "y": 4.0596779726347245,
-            "heading": -1.6224998161827902,
-            "angularVelocity": -0.000005989810829077631,
-            "velocityX": 3.7580450467730504,
-            "velocityY": 0.43326852830525053,
-            "timestamp": 2.0835280858833665
-        },
-        {
-            "x": 6.790105794643628,
-            "y": 4.068502844708353,
-            "heading": -1.6329532702813698,
-            "angularVelocity": -0.27300131743194306,
-            "velocityX": 3.6870581512131766,
-            "velocityY": 0.2304694390532754,
-            "timestamp": 2.121818941755099
-        },
-        {
-            "x": 6.921555592389516,
-            "y": 4.075747346171521,
-            "heading": -1.6719954127909697,
-            "angularVelocity": -1.0196205235104698,
-            "velocityX": 3.4329292138629897,
-            "velocityY": 0.18919664494924698,
-            "timestamp": 2.1601097976268315
-        },
-        {
-            "x": 7.042894308610419,
-            "y": 4.08202615388359,
-            "heading": -1.737655401763884,
-            "angularVelocity": -1.714769426749386,
-            "velocityX": 3.168869262869602,
-            "velocityY": 0.16397668762232157,
-            "timestamp": 2.198400653498564
-        },
-        {
-            "x": 7.153820883415079,
-            "y": 4.087877633041295,
-            "heading": -1.8279052524624368,
-            "angularVelocity": -2.3569556920031554,
-            "velocityX": 2.8969468631425612,
-            "velocityY": 0.15281661964688553,
-            "timestamp": 2.2366915093702966
-        },
-        {
-            "x": 7.254173862078464,
-            "y": 4.093777241980607,
-            "heading": -1.9412919882105817,
-            "angularVelocity": -2.96119616986284,
-            "velocityX": 2.6208079286488037,
-            "velocityY": 0.15407357200568295,
-            "timestamp": 2.274982365242029
-        },
-        {
-            "x": 7.343968095562701,
-            "y": 4.100172778216793,
-            "heading": -2.0772958326900723,
-            "angularVelocity": -3.5518622235835817,
-            "velocityX": 2.3450568403336534,
-            "velocityY": 0.16702515758877745,
-            "timestamp": 2.3132732211137617
-        },
-        {
-            "x": 7.423449152791422,
-            "y": 4.107548146969254,
-            "heading": -2.2364797071963527,
-            "angularVelocity": -4.15722947116975,
-            "velocityX": 2.0757190044267455,
-            "velocityY": 0.19261436143311772,
-            "timestamp": 2.351564076985494
-        },
-        {
-            "x": 7.492711517186474,
-            "y": 4.113210958649556,
-            "heading": -2.4189397780167137,
-            "angularVelocity": -4.765108187489175,
-            "velocityX": 1.8088486877145706,
-            "velocityY": 0.147889399476235,
-            "timestamp": 2.3898549328572267
-        },
-        {
-            "x": 7.549783547522053,
-            "y": 4.117992264935679,
-            "heading": -2.6004162007304275,
-            "angularVelocity": -4.739419336084478,
-            "velocityX": 1.490487194299341,
-            "velocityY": 0.12486809650168224,
-            "timestamp": 2.4281457887289593
-        },
-        {
-            "x": 7.595268726062492,
-            "y": 4.121696461876066,
-            "heading": -2.768130415473744,
-            "angularVelocity": -4.380006947484507,
-            "velocityX": 1.1878861807843983,
-            "velocityY": 0.09673842112058428,
-            "timestamp": 2.466436644600692
-        },
-        {
-            "x": 7.630218683798498,
-            "y": 4.124204213311165,
-            "heading": -2.9120023323717077,
-            "angularVelocity": -3.7573439825922996,
-            "velocityX": 0.9127494525868564,
-            "velocityY": 0.06549217503781085,
-            "timestamp": 2.5047275004724243
-        },
-        {
-            "x": 7.655568399114973,
-            "y": 4.125734158703857,
-            "heading": -3.0250833006842397,
-            "angularVelocity": -2.9532107793916214,
-            "velocityX": 0.6620305224148624,
-            "velocityY": 0.03995589437375436,
-            "timestamp": 2.543018356344157
-        },
-        {
-            "x": 7.671992151604379,
-            "y": 4.126594890877869,
-            "heading": -3.103103884407084,
-            "angularVelocity": -2.037577430606375,
-            "velocityX": 0.42892100778377035,
-            "velocityY": 0.022478791722372646,
-            "timestamp": 2.5813092122158894
+            "heading": 1.7737408289368037,
+            "angularVelocity": -1.4262496399629772,
+            "velocityX": 2.638534799915143,
+            "velocityY": -1.8110259426046127,
+            "timestamp": 1.5858301464308089
+        },
+        {
+            "x": 5.008276114908647,
+            "y": 4.1019129343639005,
+            "heading": 1.719841693932476,
+            "angularVelocity": -1.4074809230288232,
+            "velocityX": 2.815956908545699,
+            "velocityY": -1.5456009367595303,
+            "timestamp": 1.6241248999315097
+        },
+        {
+            "x": 5.122676890234505,
+            "y": 4.053024888243358,
+            "heading": 1.6679900254477509,
+            "angularVelocity": -1.354014943163857,
+            "velocityX": 2.987374636678728,
+            "velocityY": -1.276625167978901,
+            "timestamp": 1.6624196534322104
+        },
+        {
+            "x": 5.243382451295884,
+            "y": 4.014566134854959,
+            "heading": 1.6197010617895011,
+            "angularVelocity": -1.260981185252083,
+            "velocityX": 3.1520130051018556,
+            "velocityY": -1.0042825680467018,
+            "timestamp": 1.7007144069329112
+        },
+        {
+            "x": 5.370099790784461,
+            "y": 3.9866453379407707,
+            "heading": 1.576858310766692,
+            "angularVelocity": -1.1187629402556432,
+            "velocityX": 3.3090000040412137,
+            "velocityY": -0.7291024059908604,
+            "timestamp": 1.739009160433612
+        },
+        {
+            "x": 5.502536097274536,
+            "y": 3.9692960416426737,
+            "heading": 1.5420636410872666,
+            "angularVelocity": -0.9086014792806747,
+            "velocityX": 3.4583407486263615,
+            "velocityY": -0.45304629778540073,
+            "timestamp": 1.7773039139343128
+        },
+        {
+            "x": 5.640613600815518,
+            "y": 3.9622331858505495,
+            "heading": 1.5196895147171037,
+            "angularVelocity": -0.5842608797508935,
+            "velocityX": 3.6056506680074274,
+            "velocityY": -0.18443403198808295,
+            "timestamp": 1.8155986674350135
+        },
+        {
+            "x": 5.785301546169388,
+            "y": 3.963377277975263,
+            "heading": 1.519119424890785,
+            "angularVelocity": -0.014886891132695255,
+            "velocityX": 3.778270706226711,
+            "velocityY": 0.029875949578638192,
+            "timestamp": 1.8538934209357143
+        },
+        {
+            "x": 5.929550760809323,
+            "y": 3.9767385598159253,
+            "heading": 1.5191191822149777,
+            "angularVelocity": -0.000006337051042848807,
+            "velocityX": 3.7668140268168893,
+            "velocityY": 0.3489063283934661,
+            "timestamp": 1.892188174436415
+        },
+        {
+            "x": 6.073462686179593,
+            "y": 3.9933433315789473,
+            "heading": 1.5191189525644635,
+            "angularVelocity": -0.0000059969184602345635,
+            "velocityX": 3.7580063119517533,
+            "velocityY": 0.4336043516435729,
+            "timestamp": 1.9304829279371158
+        },
+        {
+            "x": 6.217374593429904,
+            "y": 4.009948260386706,
+            "heading": 1.519118722914699,
+            "angularVelocity": -0.000005996898884562842,
+            "velocityX": 3.7580058387809343,
+            "velocityY": 0.43360845259010783,
+            "timestamp": 1.9687776814378166
+        },
+        {
+            "x": 6.361286503020876,
+            "y": 4.026553168908082,
+            "heading": 1.5191184932648618,
+            "angularVelocity": -0.000005996900776462565,
+            "velocityX": 3.7580058999032113,
+            "velocityY": 0.4336079228469906,
+            "timestamp": 2.0070724349385176
+        },
+        {
+            "x": 6.5051984133944005,
+            "y": 4.04315807064667,
+            "heading": 1.5191182636148604,
+            "angularVelocity": -0.000005996905070522029,
+            "velocityX": 3.7580059203381437,
+            "velocityY": 0.4336077457264339,
+            "timestamp": 2.0453671884392186
+        },
+        {
+            "x": 6.6491104352583505,
+            "y": 4.059762006067292,
+            "heading": 1.5191180339567425,
+            "angularVelocity": -0.000005997117020238007,
+            "velocityX": 3.7580088317141747,
+            "velocityY": 0.4335825120357026,
+            "timestamp": 2.0836619419399196
+        },
+        {
+            "x": 6.790280334982046,
+            "y": 4.068588148423426,
+            "heading": 1.5085875786821552,
+            "angularVelocity": -0.27498428144717973,
+            "velocityX": 3.6864031445224414,
+            "velocityY": 0.23047915312922457,
+            "timestamp": 2.1219566954406206
+        },
+        {
+            "x": 6.921716704437578,
+            "y": 4.075833761978748,
+            "heading": 1.4694677109965053,
+            "angularVelocity": -1.0215464028234083,
+            "velocityX": 3.43222915517987,
+            "velocityY": 0.18920642889606504,
+            "timestamp": 2.1602514489413216
+        },
+        {
+            "x": 7.043039125371844,
+            "y": 4.08211395301362,
+            "heading": 1.4037305732475935,
+            "angularVelocity": -1.716609502335841,
+            "velocityX": 3.1681212135768497,
+            "velocityY": 0.16399612115943957,
+            "timestamp": 2.1985462024420226
+        },
+        {
+            "x": 7.153946697571625,
+            "y": 4.087967374487544,
+            "heading": 1.3134036899027293,
+            "angularVelocity": -2.3587273735346383,
+            "velocityX": 2.896155793188521,
+            "velocityY": 0.1528517861805002,
+            "timestamp": 2.2368409559427236
+        },
+        {
+            "x": 7.254278338500056,
+            "y": 4.0938696715910385,
+            "heading": 1.199938125135736,
+            "angularVelocity": -2.9629532610757465,
+            "velocityX": 2.619983986229206,
+            "velocityY": 0.1541280871121784,
+            "timestamp": 2.2751357094434246
+        },
+        {
+            "x": 7.34404947628579,
+            "y": 4.100268813517142,
+            "heading": 1.0638507127340207,
+            "angularVelocity": -3.5536829450860603,
+            "velocityX": 2.344215057660344,
+            "velocityY": 0.16710231405423262,
+            "timestamp": 2.3134304629441256
+        },
+        {
+            "x": 7.4235066081317465,
+            "y": 4.107648882181887,
+            "heading": 0.9045749783818139,
+            "angularVelocity": -4.159205107542786,
+            "velocityX": 2.0748829691383666,
+            "velocityY": 0.192717487125464,
+            "timestamp": 2.3517252164448266
+        },
+        {
+            "x": 7.492760083799764,
+            "y": 4.113297423743801,
+            "heading": 0.7219171880631509,
+            "angularVelocity": -4.769786292404788,
+            "velocityX": 1.808432470175071,
+            "velocityY": 0.1475017083426549,
+            "timestamp": 2.3900199699455276
+        },
+        {
+            "x": 7.549820937161571,
+            "y": 4.1180620764150975,
+            "heading": 0.5402485968001275,
+            "angularVelocity": -4.7439551023536115,
+            "velocityX": 1.4900436259698675,
+            "velocityY": 0.1244205076606485,
+            "timestamp": 2.4283147234462286
+        },
+        {
+            "x": 7.595294548959434,
+            "y": 4.121747510359626,
+            "heading": 0.3723680212509228,
+            "angularVelocity": -4.383905370904984,
+            "velocityX": 1.1874632329734431,
+            "velocityY": 0.09623861254154589,
+            "timestamp": 2.4666094769469296
+        },
+        {
+            "x": 7.630234770559726,
+            "y": 4.124236647711556,
+            "heading": 0.22836852122657442,
+            "angularVelocity": -3.7602931697083015,
+            "velocityX": 0.912402311184801,
+            "velocityY": 0.06499943528517171,
+            "timestamp": 2.5049042304476306
+        },
+        {
+            "x": 7.655576852148959,
+            "y": 4.125750923706981,
+            "heading": 0.11519495737029227,
+            "angularVelocity": -2.9553281718920323,
+            "velocityX": 0.6617637998053468,
+            "velocityY": 0.03954264897925492,
+            "timestamp": 2.5431989839483315
+        },
+        {
+            "x": 7.671995118301485,
+            "y": 4.1266005721732935,
+            "heading": 0.03711275718074835,
+            "angularVelocity": -2.0389790520029063,
+            "velocityX": 0.4287340863083462,
+            "velocityY": 0.02218707234390751,
+            "timestamp": 2.5814937374490325
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": -3.143185307179586,
-            "angularVelocity": -1.0467622585080842,
-            "velocityX": 0.20913213385579937,
-            "velocityY": 0.010579787599633836,
-            "timestamp": 2.619600068087622
+            "heading": -0.003,
+            "angularVelocity": -1.047473962197309,
+            "velocityX": 0.20903337838088354,
+            "velocityY": 0.010430353774151994,
+            "timestamp": 2.6197884909497335
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": -3.143185307179586,
-            "angularVelocity": 7.101458819208695e-33,
-            "velocityX": -7.030931331746161e-34,
-            "velocityY": 2.5921613146305674e-34,
-            "timestamp": 2.6578909239593544
+            "heading": -0.003,
+            "angularVelocity": 7.250240493925158e-33,
+            "velocityX": 3.887711813813661e-34,
+            "velocityY": -4.900942603355191e-34,
+            "timestamp": 2.6580832444504345
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.traj
index 38312c6d..463b42ef 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.traj	
@@ -3,524 +3,524 @@
         {
             "x": 0.71,
             "y": 6.72,
-            "heading": -2.1,
+            "heading": 1.04,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": 2.27986083101055e-33,
+            "velocityY": 1.3928609100313025e-33,
             "timestamp": 0
         },
         {
-            "x": 0.7292945467072102,
-            "y": 6.711630943578302,
-            "heading": -2.0466837783195437,
-            "angularVelocity": 0.9750423416679173,
-            "velocityX": 0.3528569619125016,
-            "velocityY": -0.1530525628742106,
-            "timestamp": 0.05468092963968429
-        },
-        {
-            "x": 0.7686895561493499,
-            "y": 6.695016697545084,
-            "heading": -1.9453579324448191,
-            "angularVelocity": 1.853038098335252,
-            "velocityX": 0.720452444787058,
-            "velocityY": -0.30383986049058775,
-            "timestamp": 0.10936185927936858
-        },
-        {
-            "x": 0.8291498926090706,
-            "y": 6.670065411203325,
-            "heading": -1.8043277140562832,
-            "angularVelocity": 2.579148147587168,
-            "velocityX": 1.1056932802371746,
-            "velocityY": -0.4563069155219838,
-            "timestamp": 0.16404278891905286
-        },
-        {
-            "x": 0.9116442238767206,
-            "y": 6.636292399159329,
-            "heading": -1.6353628554936872,
-            "angularVelocity": 3.09001437385898,
-            "velocityX": 1.50864902647486,
-            "velocityY": -0.6176378541210026,
-            "timestamp": 0.21872371855873715
-        },
-        {
-            "x": 1.0169609100629697,
-            "y": 6.593200359597327,
-            "heading": -1.4531033706452814,
-            "angularVelocity": 3.333145322316033,
-            "velocityX": 1.926022232617937,
-            "velocityY": -0.7880634042974968,
-            "timestamp": 0.2734046481984214
-        },
-        {
-            "x": 1.1451022790503944,
-            "y": 6.540887351226898,
-            "heading": -1.2848862020086818,
-            "angularVelocity": 3.0763406866901017,
-            "velocityX": 2.3434380108714725,
-            "velocityY": -0.9566956654750047,
-            "timestamp": 0.3280855778381057
-        },
-        {
-            "x": 1.291419394840941,
-            "y": 6.479714636695398,
-            "heading": -1.166754837939456,
-            "angularVelocity": 2.1603759271768683,
-            "velocityX": 2.6758344591924805,
-            "velocityY": -1.1187211873425027,
-            "timestamp": 0.38276650747779
-        },
-        {
-            "x": 1.45556402161766,
-            "y": 6.408096144298377,
-            "heading": -1.0985152891132095,
-            "angularVelocity": 1.24795882725304,
-            "velocityX": 3.0018624017246447,
-            "velocityY": -1.3097526481159927,
-            "timestamp": 0.4374474371174743
-        },
-        {
-            "x": 1.6376170855458871,
-            "y": 6.326295801964295,
-            "heading": -1.0805867815133028,
-            "angularVelocity": 0.3278749596622653,
-            "velocityX": 3.32937031480356,
-            "velocityY": -1.495957418301032,
-            "timestamp": 0.4921283667571586
-        },
-        {
-            "x": 1.828511449146643,
-            "y": 6.246620230558847,
-            "heading": -1.0805866805891169,
-            "angularVelocity": 0.000001845692576742716,
-            "velocityX": 3.491059220438268,
-            "velocityY": -1.4570997956776681,
-            "timestamp": 0.5468092963968428
-        },
-        {
-            "x": 2.0194058587075467,
-            "y": 6.16694476926885,
-            "heading": -1.0805865796652752,
-            "angularVelocity": 0.0000018456862813728727,
-            "velocityX": 3.491060060953374,
-            "velocityY": -1.4570977818960094,
-            "timestamp": 0.6014902260365271
-        },
-        {
-            "x": 2.2103002634541666,
-            "y": 6.087269296444259,
-            "heading": -1.0805864787414845,
-            "angularVelocity": 0.000001845685351855892,
-            "velocityX": 3.491059972910188,
-            "velocityY": -1.4570979928396652,
-            "timestamp": 0.6561711556762114
-        },
-        {
-            "x": 2.4011946628107936,
-            "y": 6.007593810705732,
-            "heading": -1.0805863778177442,
-            "angularVelocity": 0.000001845684424329729,
-            "velocityX": 3.4910598743384673,
-            "velocityY": -1.457098229008597,
-            "timestamp": 0.7108520853158957
-        },
-        {
-            "x": 2.5920890560965946,
-            "y": 5.927918310422065,
-            "heading": -1.0805862768940542,
-            "angularVelocity": 0.0000018456835100189827,
-            "velocityX": 3.4910597633157407,
-            "velocityY": -1.45709849500883,
-            "timestamp": 0.76553301495558
-        },
-        {
-            "x": 2.7829834425031943,
-            "y": 5.8482427936564765,
-            "heading": -1.0805861759704127,
-            "angularVelocity": 0.0000018456826171919115,
-            "velocityX": 3.4910596375095175,
-            "velocityY": -1.4570987964287467,
-            "timestamp": 0.8202139445952643
-        },
-        {
-            "x": 2.9738778210651113,
-            "y": 5.768567258095778,
-            "heading": -1.080586075046818,
-            "angularVelocity": 0.0000018456817639254375,
-            "velocityX": 3.4910594940466586,
-            "velocityY": -1.4570991401520532,
-            "timestamp": 0.8748948742349486
-        },
-        {
-            "x": 3.164772190605788,
-            "y": 5.688891700921064,
-            "heading": -1.0805859741232662,
-            "angularVelocity": 0.0000018456809789441398,
-            "velocityX": 3.491059329067008,
-            "velocityY": -1.4570995354272234,
-            "timestamp": 0.9295758038746329
-        },
-        {
-            "x": 3.3556665493271205,
-            "y": 5.609216117824277,
-            "heading": -1.0805858731997562,
-            "angularVelocity": 0.000001845680216876217,
-            "velocityX": 3.491059131203806,
-            "velocityY": -1.4571000094878162,
-            "timestamp": 0.9842567335143172
-        },
-        {
-            "x": 3.5465606286609406,
-            "y": 5.529539865347568,
-            "heading": -1.0805857722761907,
-            "angularVelocity": 0.0000018456812299162406,
-            "velocityX": 3.491054021789706,
-            "velocityY": -1.4571122510486052,
-            "timestamp": 1.0389376631540015
-        },
-        {
-            "x": 3.7302882443981766,
-            "y": 5.43449767788976,
-            "heading": -1.0805856670205962,
-            "angularVelocity": 0.0000019249049955796687,
-            "velocityX": 3.3599943700279993,
-            "velocityY": -1.7381231095389325,
-            "timestamp": 1.0936185927936857
-        },
-        {
-            "x": 3.901247591247985,
-            "y": 5.318042735383766,
-            "heading": -1.0805855451412456,
-            "angularVelocity": 0.000002228918769474178,
-            "velocityX": 3.1264893990707807,
-            "velocityY": -2.12971767805275,
-            "timestamp": 1.1482995224333699
-        },
-        {
-            "x": 4.056955156553094,
-            "y": 5.181866377235877,
-            "heading": -1.0805853830111598,
-            "angularVelocity": 0.0000029650206572180867,
-            "velocityX": 2.8475661685185734,
-            "velocityY": -2.490381181249336,
-            "timestamp": 1.202980452073054
-        },
-        {
-            "x": 4.195149083685748,
-            "y": 5.0279466523676755,
-            "heading": -1.080585195813012,
-            "angularVelocity": 0.0000034234631562085443,
-            "velocityX": 2.527278304214503,
-            "velocityY": -2.81487030089728,
-            "timestamp": 1.2576613817127382
-        },
-        {
-            "x": 4.313735776578777,
-            "y": 4.858628961688981,
-            "heading": -1.081017907841774,
-            "angularVelocity": -0.007913399271258946,
-            "velocityX": 2.168702940393416,
-            "velocityY": -3.0964669363597066,
-            "timestamp": 1.3123423113524224
-        },
-        {
-            "x": 4.409971486167556,
-            "y": 4.678531583119963,
-            "heading": -1.0900704798968306,
-            "angularVelocity": -0.16555263626108413,
-            "velocityX": 1.759950136600006,
-            "velocityY": -3.293604914103617,
-            "timestamp": 1.3670232409921066
-        },
-        {
-            "x": 4.510748059482075,
-            "y": 4.519168046099015,
-            "heading": -1.1448684085004224,
-            "angularVelocity": -1.0021396666932787,
-            "velocityX": 1.8429930503848142,
-            "velocityY": -2.9144262555714096,
-            "timestamp": 1.4217041706317908
-        },
-        {
-            "x": 4.62613024170022,
-            "y": 4.379607058549792,
-            "heading": -1.2139690573229496,
-            "angularVelocity": -1.2637065477463625,
-            "velocityX": 2.110099132886858,
-            "velocityY": -2.552278984078161,
-            "timestamp": 1.476385100271475
-        },
-        {
-            "x": 4.756158352198169,
-            "y": 4.26013085900244,
-            "heading": -1.289886392340967,
-            "angularVelocity": -1.3883695013648907,
-            "velocityX": 2.3779425725707126,
-            "velocityY": -2.1849701593340063,
-            "timestamp": 1.5310660299111591
+            "x": 0.729279758485921,
+            "y": 6.711628327586588,
+            "heading": 1.093428142295198,
+            "angularVelocity": 0.9770378814200095,
+            "velocityX": 0.352568020824986,
+            "velocityY": -0.15309237280887605,
+            "timestamp": 0.05468379815278649
+        },
+        {
+            "x": 0.7686493967347447,
+            "y": 6.69501140584797,
+            "heading": 1.1949690599508662,
+            "angularVelocity": 1.8568739020644303,
+            "velocityX": 0.7199506906748694,
+            "velocityY": -0.30387285265355396,
+            "timestamp": 0.10936759630557298
+        },
+        {
+            "x": 0.8290790315170126,
+            "y": 6.670059182269693,
+            "heading": 1.3362954219692313,
+            "angularVelocity": 2.584428419245852,
+            "velocityX": 1.1050738394839994,
+            "velocityY": -0.4563001185206737,
+            "timestamp": 0.16405139445835948
+        },
+        {
+            "x": 0.9115429372168562,
+            "y": 6.636285478537955,
+            "heading": 1.5055888656223098,
+            "angularVelocity": 3.0958611027725738,
+            "velocityX": 1.508013497333151,
+            "velocityY": -0.6176181039468854,
+            "timestamp": 0.21873519261114596
+        },
+        {
+            "x": 1.0168329273736771,
+            "y": 6.593191021214006,
+            "heading": 1.6881423696003794,
+            "angularVelocity": 3.3383471913932383,
+            "velocityX": 1.9254330114861617,
+            "velocityY": -0.7880662788554578,
+            "timestamp": 0.27341899076393245
+        },
+        {
+            "x": 1.1449422871140338,
+            "y": 6.540878714080721,
+            "heading": 1.8564615478628756,
+            "angularVelocity": 3.078044758197163,
+            "velocityX": 2.3427297310698694,
+            "velocityY": -0.9566326572109004,
+            "timestamp": 0.32810278891671896
+        },
+        {
+            "x": 1.2912409780613738,
+            "y": 6.479735868066328,
+            "heading": 1.974688906048384,
+            "angularVelocity": 2.1620180415263293,
+            "velocityX": 2.675357160425865,
+            "velocityY": -1.1181162991560991,
+            "timestamp": 0.38278658706950547
+        },
+        {
+            "x": 1.4553681499836075,
+            "y": 6.408144093279595,
+            "heading": 2.0430197361851268,
+            "angularVelocity": 1.2495626208301445,
+            "velocityX": 3.001385738855639,
+            "velocityY": -1.3091953595963746,
+            "timestamp": 0.437470385222292
+        },
+        {
+            "x": 1.6374055328463037,
+            "y": 6.326368400043195,
+            "heading": 2.0610350683831213,
+            "angularVelocity": 0.3294455178051113,
+            "velocityX": 3.328908909254691,
+            "velocityY": -1.4954281889476506,
+            "timestamp": 0.4921541833750785
+        },
+        {
+            "x": 1.8283093127689483,
+            "y": 6.246687216839916,
+            "heading": 2.061035169590908,
+            "angularVelocity": 0.000001850781943801841,
+            "velocityX": 3.4910482880003957,
+            "velocityY": -1.45712598420193,
+            "timestamp": 0.546837981527865
+        },
+        {
+            "x": 2.01921313853121,
+            "y": 6.167006143460992,
+            "heading": 2.0610352707983473,
+            "angularVelocity": 0.0000018507755948234292,
+            "velocityX": 3.491049126267287,
+            "velocityY": -1.4571239758492094,
+            "timestamp": 0.6015217796806515
+        },
+        {
+            "x": 2.210116959480156,
+            "y": 6.087325058550032,
+            "heading": 2.061035372005735,
+            "angularVelocity": 0.000001850774661522582,
+            "velocityX": 3.4910490382464,
+            "velocityY": -1.4571241867349893,
+            "timestamp": 0.6562055778334379
+        },
+        {
+            "x": 2.40102077504018,
+            "y": 6.007643960727976,
+            "heading": 2.0610354732130722,
+            "angularVelocity": 0.0000018507737318277825,
+            "velocityX": 3.49104893969945,
+            "velocityY": -1.4571244228395928,
+            "timestamp": 0.7108893759862244
+        },
+        {
+            "x": 2.5919245845305627,
+            "y": 5.927962848363926,
+            "heading": 2.0610355744203592,
+            "angularVelocity": 0.0000018507728139380304,
+            "velocityX": 3.491048828704212,
+            "velocityY": -1.457124688768356,
+            "timestamp": 0.7655731741390108
+        },
+        {
+            "x": 2.78282838714305,
+            "y": 5.848281719521431,
+            "heading": 2.0610356756275974,
+            "angularVelocity": 0.0000018507719179726602,
+            "velocityX": 3.4910487029284663,
+            "velocityY": -1.457124990108895,
+            "timestamp": 0.8202569722917973
+        },
+        {
+            "x": 2.973732181912291,
+            "y": 5.768600571887663,
+            "heading": 2.061035776834788,
+            "angularVelocity": 0.0000018507710613284672,
+            "velocityX": 3.491048559499401,
+            "velocityY": -1.4571253337439838,
+            "timestamp": 0.8749407704445837
+        },
+        {
+            "x": 3.16463596766188,
+            "y": 5.688919402644143,
+            "heading": 2.061035878041936,
+            "angularVelocity": 0.000001850770272196717,
+            "velocityX": 3.4910483945574473,
+            "velocityY": -1.4571257289205004,
+            "timestamp": 0.9296245685973702
+        },
+        {
+            "x": 3.3555397425941185,
+            "y": 5.609238207483871,
+            "heading": 2.061035979249042,
+            "angularVelocity": 0.000001850769506382635,
+            "velocityX": 3.4910481967410822,
+            "velocityY": -1.457126202858873,
+            "timestamp": 0.9843083667501566
+        },
+        {
+            "x": 3.546443239175115,
+            "y": 5.529556345440524,
+            "heading": 2.0610360804562036,
+            "angularVelocity": 0.0000018507705174022697,
+            "velocityX": 3.491043106545231,
+            "velocityY": -1.4571383981177848,
+            "timestamp": 1.0389921649029432
+        },
+        {
+            "x": 3.7301891812097523,
+            "y": 5.434525969364101,
+            "heading": 2.0610361860102304,
+            "angularVelocity": 0.0000019302614304565207,
+            "velocityX": 3.360153249071168,
+            "velocityY": -1.7378159397580357,
+            "timestamp": 1.0936759630557298
+        },
+        {
+            "x": 3.9011674048607876,
+            "y": 5.318079465137689,
+            "heading": 2.0610363082632275,
+            "angularVelocity": 0.0000022356347107367405,
+            "velocityX": 3.126670593972313,
+            "velocityY": -2.129451650396711,
+            "timestamp": 1.1483597612085163
+        },
+        {
+            "x": 4.05689386248419,
+            "y": 5.181908226701039,
+            "heading": 2.0610364709885136,
+            "angularVelocity": 0.0000029757495174652245,
+            "velocityX": 2.8477622784778513,
+            "velocityY": -2.490156920998575,
+            "timestamp": 1.203043559361303
+        },
+        {
+            "x": 4.195106185477333,
+            "y": 5.027990436285506,
+            "heading": 2.061036659879246,
+            "angularVelocity": 0.0000034542357869786317,
+            "velocityX": 2.52748213660978,
+            "velocityY": -2.814687267798894,
+            "timestamp": 1.2577273575140895
+        },
+        {
+            "x": 4.313711649853664,
+            "y": 4.858669951905228,
+            "heading": 2.0606108116890636,
+            "angularVelocity": -0.007787465475470693,
+            "velocityX": 2.168932451344132,
+            "velocityY": -3.096355595256869,
+            "timestamp": 1.312411155666876
+        },
+        {
+            "x": 4.40996698448611,
+            "y": 4.67856050613378,
+            "heading": 2.051588761211157,
+            "angularVelocity": -0.16498580535132465,
+            "velocityX": 1.7602166982532683,
+            "velocityY": -3.293652815925874,
+            "timestamp": 1.3670949538196626
+        },
+        {
+            "x": 4.510742037442817,
+            "y": 4.51918682060045,
+            "heading": 1.996778499947209,
+            "angularVelocity": -1.002312624862085,
+            "velocityX": 1.8428685709639585,
+            "velocityY": -2.9144589607334965,
+            "timestamp": 1.4217787519724492
+        },
+        {
+            "x": 4.62612450472694,
+            "y": 4.379617585881877,
+            "heading": 1.9276644376664414,
+            "angularVelocity": -1.263885549567404,
+            "velocityX": 2.1099936577511538,
+            "velocityY": -2.5522959163995464,
+            "timestamp": 1.4764625501252358
+        },
+        {
+            "x": 4.756154634809633,
+            "y": 4.26013511834241,
+            "heading": 1.8517335763640235,
+            "angularVelocity": -1.3885440270675238,
+            "velocityX": 2.3778547664042544,
+            "velocityY": -2.1849701662205963,
+            "timestamp": 1.5311463482780223
         },
         {
             "x": 4.900439739227295,
             "y": 4.161101341247559,
-            "heading": -1.3678659526433012,
-            "angularVelocity": -1.4260832947825601,
-            "velocityX": 2.638605231839619,
-            "velocityY": -1.8110430529880912,
-            "timestamp": 1.5857469595508433
-        },
-        {
-            "x": 5.00826704552834,
-            "y": 4.101917336684186,
-            "heading": -1.4217535551168057,
-            "angularVelocity": -1.4073230082403518,
-            "velocityX": 2.816006690010969,
-            "velocityY": -1.5456432930522144,
-            "timestamp": 1.6240378154225759
-        },
-        {
-            "x": 5.122657163782486,
-            "y": 4.053031768762854,
-            "heading": -1.4735948904430307,
-            "angularVelocity": -1.353882908752006,
-            "velocityX": 2.9874004027836745,
-            "velocityY": -1.2766903953541875,
-            "timestamp": 1.6623286712943084
-        },
-        {
-            "x": 5.24335031560154,
-            "y": 4.014573699061021,
-            "heading": -1.5218759194168912,
-            "angularVelocity": -1.2609023192271587,
-            "velocityX": 3.152009770253094,
-            "velocityY": -1.0043669389543197,
-            "timestamp": 1.700619527166041
-        },
-        {
-            "x": 5.370053209601022,
-            "y": 3.986652038058385,
-            "heading": -1.5647151394478649,
-            "angularVelocity": -1.1187846042009912,
-            "velocityX": 3.308959570502,
-            "velocityY": -0.729199187820931,
-            "timestamp": 1.7389103830377735
-        },
-        {
-            "x": 5.5024723935607405,
-            "y": 3.969300942477628,
-            "heading": -1.599514624372743,
-            "angularVelocity": -0.9088197203387187,
-            "velocityX": 3.458245603161718,
-            "velocityY": -0.4531394032789495,
-            "timestamp": 1.777201238909506
-        },
-        {
-            "x": 5.64052782398134,
-            "y": 3.9622378493580968,
-            "heading": -1.6219128460749601,
-            "angularVelocity": -0.5849496228876974,
-            "velocityX": 3.6054412281370607,
-            "velocityY": -0.1844590035592553,
-            "timestamp": 1.8154920947812385
-        },
-        {
-            "x": 5.78519636080909,
-            "y": 3.9633730625174985,
-            "heading": -1.6224984270976561,
-            "angularVelocity": -0.015292972940008887,
-            "velocityX": 3.778148425628394,
-            "velocityY": 0.02964710852127253,
-            "timestamp": 1.853782950652971
-        },
-        {
-            "x": 5.929431807514691,
-            "y": 3.9767231173381155,
-            "heading": -1.6224986694398071,
-            "angularVelocity": -0.000006328982347971965,
-            "velocityX": 3.7668378891493703,
-            "velocityY": 0.3486486399086302,
-            "timestamp": 1.8920738065247036
-        },
-        {
-            "x": 6.073330473121851,
-            "y": 3.9933141696465464,
-            "heading": -1.6224988987872904,
-            "angularVelocity": -0.000005989614964631434,
-            "velocityX": 3.758042549093057,
-            "velocityY": 0.4332901924158613,
-            "timestamp": 1.9303646623964361
-        },
-        {
-            "x": 6.217229120638352,
-            "y": 4.009905378860974,
-            "heading": -1.6224991281340264,
-            "angularVelocity": -0.000005989595448400927,
-            "velocityX": 3.7580420766392404,
-            "velocityY": 0.4332942901565158,
-            "timestamp": 1.9686555182681686
-        },
-        {
-            "x": 6.361127770496839,
-            "y": 4.0264965677626225,
-            "heading": -1.6224993574808346,
-            "angularVelocity": -0.000005989597330541617,
-            "velocityX": 3.7580421378023496,
-            "velocityY": 0.4332937596700765,
-            "timestamp": 2.0069463741399014
-        },
-        {
-            "x": 6.505026421139362,
-            "y": 4.0430877498636315,
-            "heading": -1.622499586827807,
-            "angularVelocity": -0.000005989601616416828,
-            "velocityX": 3.7580421582781125,
-            "velocityY": 0.43329358206529056,
-            "timestamp": 2.045237230011634
-        },
-        {
-            "x": 6.648925182384827,
-            "y": 4.0596779726347245,
-            "heading": -1.6224998161827902,
-            "angularVelocity": -0.000005989810829077631,
-            "velocityX": 3.7580450467730504,
-            "velocityY": 0.43326852830525053,
-            "timestamp": 2.0835280858833665
-        },
-        {
-            "x": 6.790105794643628,
-            "y": 4.068502844708353,
-            "heading": -1.6329532702813698,
-            "angularVelocity": -0.27300131743194306,
-            "velocityX": 3.6870581512131766,
-            "velocityY": 0.2304694390532754,
-            "timestamp": 2.121818941755099
-        },
-        {
-            "x": 6.921555592389516,
-            "y": 4.075747346171521,
-            "heading": -1.6719954127909697,
-            "angularVelocity": -1.0196205235104698,
-            "velocityX": 3.4329292138629897,
-            "velocityY": 0.18919664494924698,
-            "timestamp": 2.1601097976268315
-        },
-        {
-            "x": 7.042894308610419,
-            "y": 4.08202615388359,
-            "heading": -1.737655401763884,
-            "angularVelocity": -1.714769426749386,
-            "velocityX": 3.168869262869602,
-            "velocityY": 0.16397668762232157,
-            "timestamp": 2.198400653498564
-        },
-        {
-            "x": 7.153820883415079,
-            "y": 4.087877633041295,
-            "heading": -1.8279052524624368,
-            "angularVelocity": -2.3569556920031554,
-            "velocityX": 2.8969468631425612,
-            "velocityY": 0.15281661964688553,
-            "timestamp": 2.2366915093702966
-        },
-        {
-            "x": 7.254173862078464,
-            "y": 4.093777241980607,
-            "heading": -1.9412919882105817,
-            "angularVelocity": -2.96119616986284,
-            "velocityX": 2.6208079286488037,
-            "velocityY": 0.15407357200568295,
-            "timestamp": 2.274982365242029
-        },
-        {
-            "x": 7.343968095562701,
-            "y": 4.100172778216793,
-            "heading": -2.0772958326900723,
-            "angularVelocity": -3.5518622235835817,
-            "velocityX": 2.3450568403336534,
-            "velocityY": 0.16702515758877745,
-            "timestamp": 2.3132732211137617
-        },
-        {
-            "x": 7.423449152791422,
-            "y": 4.107548146969254,
-            "heading": -2.2364797071963527,
-            "angularVelocity": -4.15722947116975,
-            "velocityX": 2.0757190044267455,
-            "velocityY": 0.19261436143311772,
-            "timestamp": 2.351564076985494
-        },
-        {
-            "x": 7.492711517186474,
-            "y": 4.113210958649556,
-            "heading": -2.4189397780167137,
-            "angularVelocity": -4.765108187489175,
-            "velocityX": 1.8088486877145706,
-            "velocityY": 0.147889399476235,
-            "timestamp": 2.3898549328572267
-        },
-        {
-            "x": 7.549783547522053,
-            "y": 4.117992264935679,
-            "heading": -2.6004162007304275,
-            "angularVelocity": -4.739419336084478,
-            "velocityX": 1.490487194299341,
-            "velocityY": 0.12486809650168224,
-            "timestamp": 2.4281457887289593
-        },
-        {
-            "x": 7.595268726062492,
-            "y": 4.121696461876066,
-            "heading": -2.768130415473744,
-            "angularVelocity": -4.380006947484507,
-            "velocityX": 1.1878861807843983,
-            "velocityY": 0.09673842112058428,
-            "timestamp": 2.466436644600692
-        },
-        {
-            "x": 7.630218683798498,
-            "y": 4.124204213311165,
-            "heading": -2.9120023323717077,
-            "angularVelocity": -3.7573439825922996,
-            "velocityX": 0.9127494525868564,
-            "velocityY": 0.06549217503781085,
-            "timestamp": 2.5047275004724243
-        },
-        {
-            "x": 7.655568399114973,
-            "y": 4.125734158703857,
-            "heading": -3.0250833006842397,
-            "angularVelocity": -2.9532107793916214,
-            "velocityX": 0.6620305224148624,
-            "velocityY": 0.03995589437375436,
-            "timestamp": 2.543018356344157
-        },
-        {
-            "x": 7.671992151604379,
-            "y": 4.126594890877869,
-            "heading": -3.103103884407084,
-            "angularVelocity": -2.037577430606375,
-            "velocityX": 0.42892100778377035,
-            "velocityY": 0.022478791722372646,
-            "timestamp": 2.5813092122158894
+            "heading": 1.7737408289368037,
+            "angularVelocity": -1.4262496399629772,
+            "velocityX": 2.638534799915143,
+            "velocityY": -1.8110259426046127,
+            "timestamp": 1.5858301464308089
+        },
+        {
+            "x": 5.008276114908647,
+            "y": 4.1019129343639005,
+            "heading": 1.719841693932476,
+            "angularVelocity": -1.4074809230288232,
+            "velocityX": 2.815956908545699,
+            "velocityY": -1.5456009367595303,
+            "timestamp": 1.6241248999315097
+        },
+        {
+            "x": 5.122676890234505,
+            "y": 4.053024888243358,
+            "heading": 1.6679900254477509,
+            "angularVelocity": -1.354014943163857,
+            "velocityX": 2.987374636678728,
+            "velocityY": -1.276625167978901,
+            "timestamp": 1.6624196534322104
+        },
+        {
+            "x": 5.243382451295884,
+            "y": 4.014566134854959,
+            "heading": 1.6197010617895011,
+            "angularVelocity": -1.260981185252083,
+            "velocityX": 3.1520130051018556,
+            "velocityY": -1.0042825680467018,
+            "timestamp": 1.7007144069329112
+        },
+        {
+            "x": 5.370099790784461,
+            "y": 3.9866453379407707,
+            "heading": 1.576858310766692,
+            "angularVelocity": -1.1187629402556432,
+            "velocityX": 3.3090000040412137,
+            "velocityY": -0.7291024059908604,
+            "timestamp": 1.739009160433612
+        },
+        {
+            "x": 5.502536097274536,
+            "y": 3.9692960416426737,
+            "heading": 1.5420636410872666,
+            "angularVelocity": -0.9086014792806747,
+            "velocityX": 3.4583407486263615,
+            "velocityY": -0.45304629778540073,
+            "timestamp": 1.7773039139343128
+        },
+        {
+            "x": 5.640613600815518,
+            "y": 3.9622331858505495,
+            "heading": 1.5196895147171037,
+            "angularVelocity": -0.5842608797508935,
+            "velocityX": 3.6056506680074274,
+            "velocityY": -0.18443403198808295,
+            "timestamp": 1.8155986674350135
+        },
+        {
+            "x": 5.785301546169388,
+            "y": 3.963377277975263,
+            "heading": 1.519119424890785,
+            "angularVelocity": -0.014886891132695255,
+            "velocityX": 3.778270706226711,
+            "velocityY": 0.029875949578638192,
+            "timestamp": 1.8538934209357143
+        },
+        {
+            "x": 5.929550760809323,
+            "y": 3.9767385598159253,
+            "heading": 1.5191191822149777,
+            "angularVelocity": -0.000006337051042848807,
+            "velocityX": 3.7668140268168893,
+            "velocityY": 0.3489063283934661,
+            "timestamp": 1.892188174436415
+        },
+        {
+            "x": 6.073462686179593,
+            "y": 3.9933433315789473,
+            "heading": 1.5191189525644635,
+            "angularVelocity": -0.0000059969184602345635,
+            "velocityX": 3.7580063119517533,
+            "velocityY": 0.4336043516435729,
+            "timestamp": 1.9304829279371158
+        },
+        {
+            "x": 6.217374593429904,
+            "y": 4.009948260386706,
+            "heading": 1.519118722914699,
+            "angularVelocity": -0.000005996898884562842,
+            "velocityX": 3.7580058387809343,
+            "velocityY": 0.43360845259010783,
+            "timestamp": 1.9687776814378166
+        },
+        {
+            "x": 6.361286503020876,
+            "y": 4.026553168908082,
+            "heading": 1.5191184932648618,
+            "angularVelocity": -0.000005996900776462565,
+            "velocityX": 3.7580058999032113,
+            "velocityY": 0.4336079228469906,
+            "timestamp": 2.0070724349385176
+        },
+        {
+            "x": 6.5051984133944005,
+            "y": 4.04315807064667,
+            "heading": 1.5191182636148604,
+            "angularVelocity": -0.000005996905070522029,
+            "velocityX": 3.7580059203381437,
+            "velocityY": 0.4336077457264339,
+            "timestamp": 2.0453671884392186
+        },
+        {
+            "x": 6.6491104352583505,
+            "y": 4.059762006067292,
+            "heading": 1.5191180339567425,
+            "angularVelocity": -0.000005997117020238007,
+            "velocityX": 3.7580088317141747,
+            "velocityY": 0.4335825120357026,
+            "timestamp": 2.0836619419399196
+        },
+        {
+            "x": 6.790280334982046,
+            "y": 4.068588148423426,
+            "heading": 1.5085875786821552,
+            "angularVelocity": -0.27498428144717973,
+            "velocityX": 3.6864031445224414,
+            "velocityY": 0.23047915312922457,
+            "timestamp": 2.1219566954406206
+        },
+        {
+            "x": 6.921716704437578,
+            "y": 4.075833761978748,
+            "heading": 1.4694677109965053,
+            "angularVelocity": -1.0215464028234083,
+            "velocityX": 3.43222915517987,
+            "velocityY": 0.18920642889606504,
+            "timestamp": 2.1602514489413216
+        },
+        {
+            "x": 7.043039125371844,
+            "y": 4.08211395301362,
+            "heading": 1.4037305732475935,
+            "angularVelocity": -1.716609502335841,
+            "velocityX": 3.1681212135768497,
+            "velocityY": 0.16399612115943957,
+            "timestamp": 2.1985462024420226
+        },
+        {
+            "x": 7.153946697571625,
+            "y": 4.087967374487544,
+            "heading": 1.3134036899027293,
+            "angularVelocity": -2.3587273735346383,
+            "velocityX": 2.896155793188521,
+            "velocityY": 0.1528517861805002,
+            "timestamp": 2.2368409559427236
+        },
+        {
+            "x": 7.254278338500056,
+            "y": 4.0938696715910385,
+            "heading": 1.199938125135736,
+            "angularVelocity": -2.9629532610757465,
+            "velocityX": 2.619983986229206,
+            "velocityY": 0.1541280871121784,
+            "timestamp": 2.2751357094434246
+        },
+        {
+            "x": 7.34404947628579,
+            "y": 4.100268813517142,
+            "heading": 1.0638507127340207,
+            "angularVelocity": -3.5536829450860603,
+            "velocityX": 2.344215057660344,
+            "velocityY": 0.16710231405423262,
+            "timestamp": 2.3134304629441256
+        },
+        {
+            "x": 7.4235066081317465,
+            "y": 4.107648882181887,
+            "heading": 0.9045749783818139,
+            "angularVelocity": -4.159205107542786,
+            "velocityX": 2.0748829691383666,
+            "velocityY": 0.192717487125464,
+            "timestamp": 2.3517252164448266
+        },
+        {
+            "x": 7.492760083799764,
+            "y": 4.113297423743801,
+            "heading": 0.7219171880631509,
+            "angularVelocity": -4.769786292404788,
+            "velocityX": 1.808432470175071,
+            "velocityY": 0.1475017083426549,
+            "timestamp": 2.3900199699455276
+        },
+        {
+            "x": 7.549820937161571,
+            "y": 4.1180620764150975,
+            "heading": 0.5402485968001275,
+            "angularVelocity": -4.7439551023536115,
+            "velocityX": 1.4900436259698675,
+            "velocityY": 0.1244205076606485,
+            "timestamp": 2.4283147234462286
+        },
+        {
+            "x": 7.595294548959434,
+            "y": 4.121747510359626,
+            "heading": 0.3723680212509228,
+            "angularVelocity": -4.383905370904984,
+            "velocityX": 1.1874632329734431,
+            "velocityY": 0.09623861254154589,
+            "timestamp": 2.4666094769469296
+        },
+        {
+            "x": 7.630234770559726,
+            "y": 4.124236647711556,
+            "heading": 0.22836852122657442,
+            "angularVelocity": -3.7602931697083015,
+            "velocityX": 0.912402311184801,
+            "velocityY": 0.06499943528517171,
+            "timestamp": 2.5049042304476306
+        },
+        {
+            "x": 7.655576852148959,
+            "y": 4.125750923706981,
+            "heading": 0.11519495737029227,
+            "angularVelocity": -2.9553281718920323,
+            "velocityX": 0.6617637998053468,
+            "velocityY": 0.03954264897925492,
+            "timestamp": 2.5431989839483315
+        },
+        {
+            "x": 7.671995118301485,
+            "y": 4.1266005721732935,
+            "heading": 0.03711275718074835,
+            "angularVelocity": -2.0389790520029063,
+            "velocityX": 0.4287340863083462,
+            "velocityY": 0.02218707234390751,
+            "timestamp": 2.5814937374490325
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": -3.143185307179586,
-            "angularVelocity": -1.0467622585080842,
-            "velocityX": 0.20913213385579937,
-            "velocityY": 0.010579787599633836,
-            "timestamp": 2.619600068087622
+            "heading": -0.003,
+            "angularVelocity": -1.047473962197309,
+            "velocityX": 0.20903337838088354,
+            "velocityY": 0.010430353774151994,
+            "timestamp": 2.6197884909497335
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": -3.143185307179586,
-            "angularVelocity": 7.101458819208695e-33,
-            "velocityX": -7.030931331746161e-34,
-            "velocityY": 2.5921613146305674e-34,
-            "timestamp": 2.6578909239593544
+            "heading": -0.003,
+            "angularVelocity": 7.250240493925158e-33,
+            "velocityX": 3.887711813813661e-34,
+            "velocityY": -4.900942603355191e-34,
+            "timestamp": 2.6580832444504345
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.1.traj
index 00ddf5d5..ca6ee59b 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.1.traj	
@@ -3,191 +3,398 @@
         {
             "x": 0.71,
             "y": 6.72,
-            "heading": -2.1,
+            "heading": 1.04,
             "angularVelocity": 0,
-            "velocityX": -3.0738838344554265e-34,
-            "velocityY": -1.6532575942515022e-34,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.8675349003884936,
-            "y": 6.638058463961624,
-            "heading": -2.192308349350307,
-            "angularVelocity": -0.6292858581626611,
-            "velocityX": 1.0739492763036018,
-            "velocityY": -0.5586130635852894,
-            "timestamp": 0.14668746826731788
-        },
-        {
-            "x": 1.183655074306953,
-            "y": 6.473510793186018,
-            "heading": -2.3371750269963787,
-            "angularVelocity": -0.9875872789764978,
-            "velocityX": 2.155059172078514,
-            "velocityY": -1.12175683934867,
-            "timestamp": 0.29337493653463576
-        },
-        {
-            "x": 1.6571019357484182,
-            "y": 6.226344402054212,
-            "heading": -2.393383986431118,
-            "angularVelocity": -0.3831885579503364,
-            "velocityX": 3.2275890165250725,
-            "velocityY": -1.6849864139817283,
-            "timestamp": 0.44006240480195363
-        },
-        {
-            "x": 2.149819339361714,
-            "y": 5.971090303282803,
-            "heading": -2.3933840219605123,
-            "angularVelocity": -2.422115180983058e-7,
-            "velocityX": 3.3589604444967707,
-            "velocityY": -1.7401220553227135,
-            "timestamp": 0.5867498730692715
-        },
-        {
-            "x": 2.6425367239446045,
-            "y": 5.71583616777695,
-            "heading": -2.3933840574898846,
-            "angularVelocity": -2.422113667505868e-7,
-            "velocityX": 3.358960314762406,
-            "velocityY": -1.740122305749301,
-            "timestamp": 0.7334373413365893
-        },
-        {
-            "x": 3.135254076780422,
-            "y": 5.4605819709896695,
-            "heading": -2.3933840930192347,
-            "angularVelocity": -2.4221121497556687e-7,
-            "velocityX": 3.358960098335784,
-            "velocityY": -1.7401227235179682,
-            "timestamp": 0.8801248096039072
-        },
-        {
-            "x": 3.627971327740651,
-            "y": 5.20532757755194,
-            "heading": -2.393384128548559,
-            "angularVelocity": -2.4221104201937785e-7,
-            "velocityX": 3.358959403828,
-            "velocityY": -1.7401240641263462,
-            "timestamp": 1.026812277871225
-        },
-        {
-            "x": 4.091605856430787,
-            "y": 4.900420846350941,
-            "heading": -2.393384184524006,
-            "angularVelocity": -3.815966513199635e-7,
-            "velocityX": 3.160696235108687,
-            "velocityY": -2.07861472287018,
-            "timestamp": 1.1734997461385428
-        },
-        {
-            "x": 4.555240236344247,
-            "y": 4.595513888923491,
-            "heading": -2.393384240499462,
-            "angularVelocity": -3.815967160070403e-7,
-            "velocityX": 3.1606952208661028,
-            "velocityY": -2.078616265104517,
-            "timestamp": 1.3201872144058606
-        },
-        {
-            "x": 5.018874645233154,
-            "y": 4.29060697555542,
-            "heading": -2.393384296474878,
-            "angularVelocity": -3.8159644312835243e-7,
-            "velocityX": 3.1606954183979536,
-            "velocityY": -2.078615964742264,
-            "timestamp": 1.4668746826731784
-        },
-        {
-            "x": 5.414956983036457,
-            "y": 4.102744630717356,
-            "heading": -2.393384340957434,
-            "angularVelocity": -3.838597081329049e-7,
-            "velocityX": 3.4179702017234304,
-            "velocityY": -1.621147512518649,
-            "timestamp": 1.5827570088571772
-        },
-        {
-            "x": 5.811039262323732,
-            "y": 3.9148821625062062,
-            "heading": -2.3933843854399766,
-            "angularVelocity": -3.838595921618151e-7,
-            "velocityX": 3.4179696967626736,
-            "velocityY": -1.6211485771597347,
-            "timestamp": 1.698639335041176
-        },
-        {
-            "x": 6.175303264444137,
-            "y": 3.6709862524403962,
-            "heading": -2.39338444094527,
-            "angularVelocity": -4.789798009551726e-7,
-            "velocityX": 3.143395667964284,
-            "velocityY": -2.1046860043053552,
-            "timestamp": 1.8145216612251747
-        },
-        {
-            "x": 6.517947229314261,
-            "y": 3.397547803025246,
-            "heading": -2.3933845251610077,
-            "angularVelocity": -7.267349611095117e-7,
-            "velocityX": 2.9568267755177056,
-            "velocityY": -2.3596216819205247,
-            "timestamp": 1.9304039874091734
-        },
-        {
-            "x": 6.860591017465818,
-            "y": 3.124109132165372,
-            "heading": -2.3933846093768096,
-            "angularVelocity": -7.267355129115313e-7,
-            "velocityX": 2.9568252505347843,
-            "velocityY": -2.3596235928652884,
-            "timestamp": 2.046286313593172
-        },
-        {
-            "x": 7.1795790741631205,
-            "y": 2.8553069978284444,
-            "heading": -2.4556792266097736,
-            "angularVelocity": -0.5375678870482116,
-            "velocityX": 2.7526894497337926,
-            "velocityY": -2.3196128623628227,
-            "timestamp": 2.1621686397771707
-        },
-        {
-            "x": 7.431770863942688,
-            "y": 2.653048744111035,
-            "heading": -2.7460349787453167,
-            "angularVelocity": -2.5056085918961837,
-            "velocityX": 2.1762748305478063,
-            "velocityY": -1.7453761965069832,
-            "timestamp": 2.278050965961169
-        },
-        {
-            "x": 7.5976853712302646,
-            "y": 2.521507979737877,
-            "heading": -3.002083319178936,
-            "angularVelocity": -2.2095547169726615,
-            "velocityX": 1.4317498858638353,
-            "velocityY": -1.135123609482061,
-            "timestamp": 2.3939332921451677
+            "x": 0.7271139423991175,
+            "y": 6.709732017673653,
+            "heading": 0.9305052411067223,
+            "angularVelocity": -1.767895408627961,
+            "velocityX": 0.2763206248110281,
+            "velocityY": -0.16578618916650492,
+            "timestamp": 0.06193508867035021
+        },
+        {
+            "x": 0.7615532933104725,
+            "y": 6.686914726598375,
+            "heading": 0.7206788937202419,
+            "angularVelocity": -3.387842851138587,
+            "velocityX": 0.5560555680263672,
+            "velocityY": -0.36840652956392367,
+            "timestamp": 0.12387017734070042
+        },
+        {
+            "x": 0.8202948153873781,
+            "y": 6.650989300229197,
+            "heading": 0.4540271404715134,
+            "angularVelocity": -4.305342237709284,
+            "velocityX": 0.9484368770271357,
+            "velocityY": -0.5800496477915925,
+            "timestamp": 0.18580526601105063
+        },
+        {
+            "x": 0.9068549514543394,
+            "y": 6.604915057765644,
+            "heading": 0.1437481054753499,
+            "angularVelocity": -5.009745552276917,
+            "velocityX": 1.3975944480789861,
+            "velocityY": -0.7439117865606645,
+            "timestamp": 0.24774035468140085
+        },
+        {
+            "x": 1.022793030858839,
+            "y": 6.550181878656492,
+            "heading": -0.13251218552677876,
+            "angularVelocity": -4.460481076769346,
+            "velocityX": 1.871928851536495,
+            "velocityY": -0.8837184265686551,
+            "timestamp": 0.30967544335175107
+        },
+        {
+            "x": 1.1650239086947571,
+            "y": 6.483494014593417,
+            "heading": -0.35594040398018906,
+            "angularVelocity": -3.6074577957352734,
+            "velocityX": 2.296450701684509,
+            "velocityY": -1.0767380090149254,
+            "timestamp": 0.37161053202210126
+        },
+        {
+            "x": 1.3271753983931303,
+            "y": 6.398307258999271,
+            "heading": -0.5136957877872723,
+            "angularVelocity": -2.5471083870846947,
+            "velocityX": 2.61808763302859,
+            "velocityY": -1.3754199343695408,
+            "timestamp": 0.43354562069245145
+        },
+        {
+            "x": 1.510544798478812,
+            "y": 6.2989823057862875,
+            "heading": -0.5925289533905876,
+            "angularVelocity": -1.2728352747326335,
+            "velocityX": 2.96067066379232,
+            "velocityY": -1.6036943733404823,
+            "timestamp": 0.49548070936280164
+        },
+        {
+            "x": 1.7178378905511122,
+            "y": 6.190936000456915,
+            "heading": -0.5942013489806535,
+            "angularVelocity": -0.027002392762643195,
+            "velocityX": 3.3469410720572084,
+            "velocityY": -1.7445087695675872,
+            "timestamp": 0.5574157980331519
+        },
+        {
+            "x": 1.927701821836241,
+            "y": 6.086763021965953,
+            "heading": -0.5942019920614379,
+            "angularVelocity": -0.000010383141417040622,
+            "velocityX": 3.388449678374247,
+            "velocityY": -1.6819702809408126,
+            "timestamp": 0.6193508867035021
+        },
+        {
+            "x": 2.1375657815463693,
+            "y": 5.982590100737573,
+            "heading": -0.594202635139582,
+            "angularVelocity": -0.0000103830987882079,
+            "velocityX": 3.3884501373224802,
+            "velocityY": -1.6819693563827811,
+            "timestamp": 0.6812859753738524
+        },
+        {
+            "x": 2.347429741257157,
+            "y": 5.878417179509917,
+            "heading": -0.5942032782166262,
+            "angularVelocity": -0.000010383081029176535,
+            "velocityX": 3.3884501373331317,
+            "velocityY": -1.6819693563710962,
+            "timestamp": 0.7432210640442026
+        },
+        {
+            "x": 2.5572937009682177,
+            "y": 5.774244258282216,
+            "heading": -0.5942039212925836,
+            "angularVelocity": -0.000010383063479788062,
+            "velocityX": 3.3884501373375415,
+            "velocityY": -1.6819693563718265,
+            "timestamp": 0.8051561527145529
+        },
+        {
+            "x": 2.7671576606795463,
+            "y": 5.670071337054474,
+            "heading": -0.594204564367474,
+            "angularVelocity": -0.00001038304625561267,
+            "velocityX": 3.388450137341862,
+            "velocityY": -1.68196935637249,
+            "timestamp": 0.8670912413849031
+        },
+        {
+            "x": 2.977021620391137,
+            "y": 5.5658984158266955,
+            "heading": -0.5942052074413191,
+            "angularVelocity": -0.000010383029375253952,
+            "velocityX": 3.388450137346089,
+            "velocityY": -1.6819693563730826,
+            "timestamp": 0.9290263300552534
+        },
+        {
+            "x": 3.186885580102983,
+            "y": 5.461725494598884,
+            "heading": -0.5942058505141404,
+            "angularVelocity": -0.000010383012846871076,
+            "velocityX": 3.3884501373502207,
+            "velocityY": -1.6819693563736025,
+            "timestamp": 0.9909614187256036
+        },
+        {
+            "x": 3.3967495398150795,
+            "y": 5.357552573371046,
+            "heading": -0.594206493585961,
+            "angularVelocity": -0.000010382996688205053,
+            "velocityX": 3.3884501373542517,
+            "velocityY": -1.681969356374048,
+            "timestamp": 1.0528965073959538
+        },
+        {
+            "x": 3.6066134995274184,
+            "y": 5.253379652143185,
+            "heading": -0.5942071366568044,
+            "angularVelocity": -0.000010382980910469912,
+            "velocityX": 3.38845013735818,
+            "velocityY": -1.6819693563744145,
+            "timestamp": 1.114831596066304
+        },
+        {
+            "x": 3.8164774592399944,
+            "y": 5.149206730915306,
+            "heading": -0.5942077797266946,
+            "angularVelocity": -0.000010382965523044611,
+            "velocityX": 3.388450137362003,
+            "velocityY": -1.681969356374701,
+            "timestamp": 1.1767666847366542
+        },
+        {
+            "x": 4.026341418952801,
+            "y": 5.045033809687414,
+            "heading": -0.594208422795657,
+            "angularVelocity": -0.00001038295054018878,
+            "velocityX": 3.388450137365716,
+            "velocityY": -1.6819693563749047,
+            "timestamp": 1.2387017734070045
+        },
+        {
+            "x": 4.236205378665829,
+            "y": 4.940860888459515,
+            "heading": -0.594209065863717,
+            "angularVelocity": -0.000010382935971395078,
+            "velocityX": 3.388450137369317,
+            "velocityY": -1.681969356375023,
+            "timestamp": 1.3006368620773547
+        },
+        {
+            "x": 4.446069338379075,
+            "y": 4.836687967231613,
+            "heading": -0.5942097089309012,
+            "angularVelocity": -0.000010382921828372961,
+            "velocityX": 3.3884501373728035,
+            "velocityY": -1.6819693563750544,
+            "timestamp": 1.362571950747705
+        },
+        {
+            "x": 4.6559332980925285,
+            "y": 4.732515046003717,
+            "heading": -0.5942103519972364,
+            "angularVelocity": -0.000010382908121631514,
+            "velocityX": 3.388450137376173,
+            "velocityY": -1.6819693563749973,
+            "timestamp": 1.4245070394180552
+        },
+        {
+            "x": 4.865797257806183,
+            "y": 4.628342124775828,
+            "heading": -0.5942109950627501,
+            "angularVelocity": -0.00001038289485891648,
+            "velocityX": 3.388450137379422,
+            "velocityY": -1.6819693563748486,
+            "timestamp": 1.4864421280884055
+        },
+        {
+            "x": 5.075661217518705,
+            "y": 4.524169203545277,
+            "heading": -0.594211638127464,
+            "angularVelocity": -0.000010382881945026341,
+            "velocityX": 3.3884501373611275,
+            "velocityY": -1.6819693564178315,
+            "timestamp": 1.5483772167587557
+        },
+        {
+            "x": 5.285525078653758,
+            "y": 4.419996083725492,
+            "heading": -0.5942122811951368,
+            "angularVelocity": -0.000010382929719206097,
+            "velocityX": 3.3884485457355824,
+            "velocityY": -1.6819725628269917,
+            "timestamp": 1.610312305429106
+        },
+        {
+            "x": 5.489266395568848,
+            "y": 4.304305553436279,
+            "heading": -0.5942133396987443,
+            "angularVelocity": -0.000017090531880803138,
+            "velocityX": 3.2895943364108793,
+            "velocityY": -1.8679319392755818,
+            "timestamp": 1.6722473940994562
+        },
+        {
+            "x": 5.681024172067428,
+            "y": 4.156179263559471,
+            "heading": -0.5942132880529166,
+            "angularVelocity": 8.063055616728172e-7,
+            "velocityX": 2.9937628707653796,
+            "velocityY": -2.312578894659331,
+            "timestamp": 1.7362998206864544
+        },
+        {
+            "x": 5.864967854996084,
+            "y": 3.998454299605836,
+            "heading": -0.5942132372714615,
+            "angularVelocity": 7.928107921043856e-7,
+            "velocityX": 2.871767593048443,
+            "velocityY": -2.4624354198261003,
+            "timestamp": 1.8003522472734526
+        },
+        {
+            "x": 6.0489114264021975,
+            "y": 3.840729205591288,
+            "heading": -0.5942131864900138,
+            "angularVelocity": 7.928106775338953e-7,
+            "velocityX": 2.871765851934987,
+            "velocityY": -2.4624374503645132,
+            "timestamp": 1.8644046738604507
+        },
+        {
+            "x": 6.232854997807022,
+            "y": 3.683004111575237,
+            "heading": -0.5942131357085663,
+            "angularVelocity": 7.928106743629292e-7,
+            "velocityX": 2.87176585191486,
+            "velocityY": -2.462437450387986,
+            "timestamp": 1.928457100447449
+        },
+        {
+            "x": 6.4167985692118465,
+            "y": 3.5252790175591864,
+            "heading": -0.594213084927119,
+            "angularVelocity": 7.928106702567426e-7,
+            "velocityX": 2.871765851914861,
+            "velocityY": -2.462437450387986,
+            "timestamp": 1.992509527034447
+        },
+        {
+            "x": 6.6007421406166715,
+            "y": 3.367553923543135,
+            "heading": -0.5942130341456721,
+            "angularVelocity": 7.928106666471698e-7,
+            "velocityX": 2.8717658519148612,
+            "velocityY": -2.462437450387985,
+            "timestamp": 2.0565619536214452
+        },
+        {
+            "x": 6.784685712022088,
+            "y": 3.2098288295277735,
+            "heading": -0.5942129833642253,
+            "angularVelocity": 7.928106614555171e-7,
+            "velocityX": 2.8717658519240943,
+            "velocityY": -2.4624374503772177,
+            "timestamp": 2.120614380208443
+        },
+        {
+            "x": 6.968629334581004,
+            "y": 3.052103795169223,
+            "heading": -0.594212932582592,
+            "angularVelocity": 7.928135744203515e-7,
+            "velocityX": 2.871766650543336,
+            "velocityY": -2.462436519002493,
+            "timestamp": 2.184666806795441
+        },
+        {
+            "x": 7.14554518368725,
+            "y": 2.9064126516878526,
+            "heading": -0.554455950225117,
+            "angularVelocity": 0.6206943979472176,
+            "velocityX": 2.7620475684235433,
+            "velocityY": -2.2745608752775093,
+            "timestamp": 2.248719233382439
+        },
+        {
+            "x": 7.299470766691596,
+            "y": 2.7775737815312107,
+            "heading": -0.43989859569345574,
+            "angularVelocity": 1.7884935924490803,
+            "velocityX": 2.4031186827134454,
+            "velocityY": -2.0114596280228136,
+            "timestamp": 2.312771659969437
+        },
+        {
+            "x": 7.4271138269599435,
+            "y": 2.670230084319628,
+            "heading": -0.3103960824325961,
+            "angularVelocity": 2.021820564205559,
+            "velocityX": 1.9927903917110543,
+            "velocityY": -1.6758724521667427,
+            "timestamp": 2.376824086556435
+        },
+        {
+            "x": 7.52865057013146,
+            "y": 2.5846878922538004,
+            "heading": -0.1939655985882249,
+            "angularVelocity": 1.8177372825404448,
+            "velocityX": 1.5852130603296692,
+            "velocityY": -1.3355027533522315,
+            "timestamp": 2.440876513143433
+        },
+        {
+            "x": 7.604480145165485,
+            "y": 2.520737422138354,
+            "heading": -0.10094853997619462,
+            "angularVelocity": 1.4522019471922978,
+            "velocityX": 1.1838673267285131,
+            "velocityY": -0.9984082340516305,
+            "timestamp": 2.504928939730431
+        },
+        {
+            "x": 7.654868393442648,
+            "y": 2.4782123742697575,
+            "heading": -0.03632146129528005,
+            "angularVelocity": 1.008971589751342,
+            "velocityX": 0.7866719648587251,
+            "velocityY": -0.6639100208145539,
+            "timestamp": 2.568981366317429
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": -3.143185307179586,
-            "angularVelocity": -1.2176316496840747,
-            "velocityX": 0.7103294478144603,
-            "velocityY": -0.5566679739881255,
-            "timestamp": 2.509815618329166
+            "heading": -0.003,
+            "angularVelocity": 0.5202216851226045,
+            "velocityX": 0.39235994475896174,
+            "velocityY": -0.3311720632620588,
+            "timestamp": 2.6330337929044267
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": -3.143185307179586,
-            "angularVelocity": 2.6221634517625535e-33,
-            "velocityX": -1.3293034908554191e-34,
+            "heading": -0.003,
+            "angularVelocity": -9.133242422473854e-34,
+            "velocityX": 1.8081868884139647e-34,
             "velocityY": 0,
-            "timestamp": 2.6256979445131647
+            "timestamp": 2.6970862194914247
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.traj
index 00ddf5d5..ca6ee59b 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.traj	
@@ -3,191 +3,398 @@
         {
             "x": 0.71,
             "y": 6.72,
-            "heading": -2.1,
+            "heading": 1.04,
             "angularVelocity": 0,
-            "velocityX": -3.0738838344554265e-34,
-            "velocityY": -1.6532575942515022e-34,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.8675349003884936,
-            "y": 6.638058463961624,
-            "heading": -2.192308349350307,
-            "angularVelocity": -0.6292858581626611,
-            "velocityX": 1.0739492763036018,
-            "velocityY": -0.5586130635852894,
-            "timestamp": 0.14668746826731788
-        },
-        {
-            "x": 1.183655074306953,
-            "y": 6.473510793186018,
-            "heading": -2.3371750269963787,
-            "angularVelocity": -0.9875872789764978,
-            "velocityX": 2.155059172078514,
-            "velocityY": -1.12175683934867,
-            "timestamp": 0.29337493653463576
-        },
-        {
-            "x": 1.6571019357484182,
-            "y": 6.226344402054212,
-            "heading": -2.393383986431118,
-            "angularVelocity": -0.3831885579503364,
-            "velocityX": 3.2275890165250725,
-            "velocityY": -1.6849864139817283,
-            "timestamp": 0.44006240480195363
-        },
-        {
-            "x": 2.149819339361714,
-            "y": 5.971090303282803,
-            "heading": -2.3933840219605123,
-            "angularVelocity": -2.422115180983058e-7,
-            "velocityX": 3.3589604444967707,
-            "velocityY": -1.7401220553227135,
-            "timestamp": 0.5867498730692715
-        },
-        {
-            "x": 2.6425367239446045,
-            "y": 5.71583616777695,
-            "heading": -2.3933840574898846,
-            "angularVelocity": -2.422113667505868e-7,
-            "velocityX": 3.358960314762406,
-            "velocityY": -1.740122305749301,
-            "timestamp": 0.7334373413365893
-        },
-        {
-            "x": 3.135254076780422,
-            "y": 5.4605819709896695,
-            "heading": -2.3933840930192347,
-            "angularVelocity": -2.4221121497556687e-7,
-            "velocityX": 3.358960098335784,
-            "velocityY": -1.7401227235179682,
-            "timestamp": 0.8801248096039072
-        },
-        {
-            "x": 3.627971327740651,
-            "y": 5.20532757755194,
-            "heading": -2.393384128548559,
-            "angularVelocity": -2.4221104201937785e-7,
-            "velocityX": 3.358959403828,
-            "velocityY": -1.7401240641263462,
-            "timestamp": 1.026812277871225
-        },
-        {
-            "x": 4.091605856430787,
-            "y": 4.900420846350941,
-            "heading": -2.393384184524006,
-            "angularVelocity": -3.815966513199635e-7,
-            "velocityX": 3.160696235108687,
-            "velocityY": -2.07861472287018,
-            "timestamp": 1.1734997461385428
-        },
-        {
-            "x": 4.555240236344247,
-            "y": 4.595513888923491,
-            "heading": -2.393384240499462,
-            "angularVelocity": -3.815967160070403e-7,
-            "velocityX": 3.1606952208661028,
-            "velocityY": -2.078616265104517,
-            "timestamp": 1.3201872144058606
-        },
-        {
-            "x": 5.018874645233154,
-            "y": 4.29060697555542,
-            "heading": -2.393384296474878,
-            "angularVelocity": -3.8159644312835243e-7,
-            "velocityX": 3.1606954183979536,
-            "velocityY": -2.078615964742264,
-            "timestamp": 1.4668746826731784
-        },
-        {
-            "x": 5.414956983036457,
-            "y": 4.102744630717356,
-            "heading": -2.393384340957434,
-            "angularVelocity": -3.838597081329049e-7,
-            "velocityX": 3.4179702017234304,
-            "velocityY": -1.621147512518649,
-            "timestamp": 1.5827570088571772
-        },
-        {
-            "x": 5.811039262323732,
-            "y": 3.9148821625062062,
-            "heading": -2.3933843854399766,
-            "angularVelocity": -3.838595921618151e-7,
-            "velocityX": 3.4179696967626736,
-            "velocityY": -1.6211485771597347,
-            "timestamp": 1.698639335041176
-        },
-        {
-            "x": 6.175303264444137,
-            "y": 3.6709862524403962,
-            "heading": -2.39338444094527,
-            "angularVelocity": -4.789798009551726e-7,
-            "velocityX": 3.143395667964284,
-            "velocityY": -2.1046860043053552,
-            "timestamp": 1.8145216612251747
-        },
-        {
-            "x": 6.517947229314261,
-            "y": 3.397547803025246,
-            "heading": -2.3933845251610077,
-            "angularVelocity": -7.267349611095117e-7,
-            "velocityX": 2.9568267755177056,
-            "velocityY": -2.3596216819205247,
-            "timestamp": 1.9304039874091734
-        },
-        {
-            "x": 6.860591017465818,
-            "y": 3.124109132165372,
-            "heading": -2.3933846093768096,
-            "angularVelocity": -7.267355129115313e-7,
-            "velocityX": 2.9568252505347843,
-            "velocityY": -2.3596235928652884,
-            "timestamp": 2.046286313593172
-        },
-        {
-            "x": 7.1795790741631205,
-            "y": 2.8553069978284444,
-            "heading": -2.4556792266097736,
-            "angularVelocity": -0.5375678870482116,
-            "velocityX": 2.7526894497337926,
-            "velocityY": -2.3196128623628227,
-            "timestamp": 2.1621686397771707
-        },
-        {
-            "x": 7.431770863942688,
-            "y": 2.653048744111035,
-            "heading": -2.7460349787453167,
-            "angularVelocity": -2.5056085918961837,
-            "velocityX": 2.1762748305478063,
-            "velocityY": -1.7453761965069832,
-            "timestamp": 2.278050965961169
-        },
-        {
-            "x": 7.5976853712302646,
-            "y": 2.521507979737877,
-            "heading": -3.002083319178936,
-            "angularVelocity": -2.2095547169726615,
-            "velocityX": 1.4317498858638353,
-            "velocityY": -1.135123609482061,
-            "timestamp": 2.3939332921451677
+            "x": 0.7271139423991175,
+            "y": 6.709732017673653,
+            "heading": 0.9305052411067223,
+            "angularVelocity": -1.767895408627961,
+            "velocityX": 0.2763206248110281,
+            "velocityY": -0.16578618916650492,
+            "timestamp": 0.06193508867035021
+        },
+        {
+            "x": 0.7615532933104725,
+            "y": 6.686914726598375,
+            "heading": 0.7206788937202419,
+            "angularVelocity": -3.387842851138587,
+            "velocityX": 0.5560555680263672,
+            "velocityY": -0.36840652956392367,
+            "timestamp": 0.12387017734070042
+        },
+        {
+            "x": 0.8202948153873781,
+            "y": 6.650989300229197,
+            "heading": 0.4540271404715134,
+            "angularVelocity": -4.305342237709284,
+            "velocityX": 0.9484368770271357,
+            "velocityY": -0.5800496477915925,
+            "timestamp": 0.18580526601105063
+        },
+        {
+            "x": 0.9068549514543394,
+            "y": 6.604915057765644,
+            "heading": 0.1437481054753499,
+            "angularVelocity": -5.009745552276917,
+            "velocityX": 1.3975944480789861,
+            "velocityY": -0.7439117865606645,
+            "timestamp": 0.24774035468140085
+        },
+        {
+            "x": 1.022793030858839,
+            "y": 6.550181878656492,
+            "heading": -0.13251218552677876,
+            "angularVelocity": -4.460481076769346,
+            "velocityX": 1.871928851536495,
+            "velocityY": -0.8837184265686551,
+            "timestamp": 0.30967544335175107
+        },
+        {
+            "x": 1.1650239086947571,
+            "y": 6.483494014593417,
+            "heading": -0.35594040398018906,
+            "angularVelocity": -3.6074577957352734,
+            "velocityX": 2.296450701684509,
+            "velocityY": -1.0767380090149254,
+            "timestamp": 0.37161053202210126
+        },
+        {
+            "x": 1.3271753983931303,
+            "y": 6.398307258999271,
+            "heading": -0.5136957877872723,
+            "angularVelocity": -2.5471083870846947,
+            "velocityX": 2.61808763302859,
+            "velocityY": -1.3754199343695408,
+            "timestamp": 0.43354562069245145
+        },
+        {
+            "x": 1.510544798478812,
+            "y": 6.2989823057862875,
+            "heading": -0.5925289533905876,
+            "angularVelocity": -1.2728352747326335,
+            "velocityX": 2.96067066379232,
+            "velocityY": -1.6036943733404823,
+            "timestamp": 0.49548070936280164
+        },
+        {
+            "x": 1.7178378905511122,
+            "y": 6.190936000456915,
+            "heading": -0.5942013489806535,
+            "angularVelocity": -0.027002392762643195,
+            "velocityX": 3.3469410720572084,
+            "velocityY": -1.7445087695675872,
+            "timestamp": 0.5574157980331519
+        },
+        {
+            "x": 1.927701821836241,
+            "y": 6.086763021965953,
+            "heading": -0.5942019920614379,
+            "angularVelocity": -0.000010383141417040622,
+            "velocityX": 3.388449678374247,
+            "velocityY": -1.6819702809408126,
+            "timestamp": 0.6193508867035021
+        },
+        {
+            "x": 2.1375657815463693,
+            "y": 5.982590100737573,
+            "heading": -0.594202635139582,
+            "angularVelocity": -0.0000103830987882079,
+            "velocityX": 3.3884501373224802,
+            "velocityY": -1.6819693563827811,
+            "timestamp": 0.6812859753738524
+        },
+        {
+            "x": 2.347429741257157,
+            "y": 5.878417179509917,
+            "heading": -0.5942032782166262,
+            "angularVelocity": -0.000010383081029176535,
+            "velocityX": 3.3884501373331317,
+            "velocityY": -1.6819693563710962,
+            "timestamp": 0.7432210640442026
+        },
+        {
+            "x": 2.5572937009682177,
+            "y": 5.774244258282216,
+            "heading": -0.5942039212925836,
+            "angularVelocity": -0.000010383063479788062,
+            "velocityX": 3.3884501373375415,
+            "velocityY": -1.6819693563718265,
+            "timestamp": 0.8051561527145529
+        },
+        {
+            "x": 2.7671576606795463,
+            "y": 5.670071337054474,
+            "heading": -0.594204564367474,
+            "angularVelocity": -0.00001038304625561267,
+            "velocityX": 3.388450137341862,
+            "velocityY": -1.68196935637249,
+            "timestamp": 0.8670912413849031
+        },
+        {
+            "x": 2.977021620391137,
+            "y": 5.5658984158266955,
+            "heading": -0.5942052074413191,
+            "angularVelocity": -0.000010383029375253952,
+            "velocityX": 3.388450137346089,
+            "velocityY": -1.6819693563730826,
+            "timestamp": 0.9290263300552534
+        },
+        {
+            "x": 3.186885580102983,
+            "y": 5.461725494598884,
+            "heading": -0.5942058505141404,
+            "angularVelocity": -0.000010383012846871076,
+            "velocityX": 3.3884501373502207,
+            "velocityY": -1.6819693563736025,
+            "timestamp": 0.9909614187256036
+        },
+        {
+            "x": 3.3967495398150795,
+            "y": 5.357552573371046,
+            "heading": -0.594206493585961,
+            "angularVelocity": -0.000010382996688205053,
+            "velocityX": 3.3884501373542517,
+            "velocityY": -1.681969356374048,
+            "timestamp": 1.0528965073959538
+        },
+        {
+            "x": 3.6066134995274184,
+            "y": 5.253379652143185,
+            "heading": -0.5942071366568044,
+            "angularVelocity": -0.000010382980910469912,
+            "velocityX": 3.38845013735818,
+            "velocityY": -1.6819693563744145,
+            "timestamp": 1.114831596066304
+        },
+        {
+            "x": 3.8164774592399944,
+            "y": 5.149206730915306,
+            "heading": -0.5942077797266946,
+            "angularVelocity": -0.000010382965523044611,
+            "velocityX": 3.388450137362003,
+            "velocityY": -1.681969356374701,
+            "timestamp": 1.1767666847366542
+        },
+        {
+            "x": 4.026341418952801,
+            "y": 5.045033809687414,
+            "heading": -0.594208422795657,
+            "angularVelocity": -0.00001038295054018878,
+            "velocityX": 3.388450137365716,
+            "velocityY": -1.6819693563749047,
+            "timestamp": 1.2387017734070045
+        },
+        {
+            "x": 4.236205378665829,
+            "y": 4.940860888459515,
+            "heading": -0.594209065863717,
+            "angularVelocity": -0.000010382935971395078,
+            "velocityX": 3.388450137369317,
+            "velocityY": -1.681969356375023,
+            "timestamp": 1.3006368620773547
+        },
+        {
+            "x": 4.446069338379075,
+            "y": 4.836687967231613,
+            "heading": -0.5942097089309012,
+            "angularVelocity": -0.000010382921828372961,
+            "velocityX": 3.3884501373728035,
+            "velocityY": -1.6819693563750544,
+            "timestamp": 1.362571950747705
+        },
+        {
+            "x": 4.6559332980925285,
+            "y": 4.732515046003717,
+            "heading": -0.5942103519972364,
+            "angularVelocity": -0.000010382908121631514,
+            "velocityX": 3.388450137376173,
+            "velocityY": -1.6819693563749973,
+            "timestamp": 1.4245070394180552
+        },
+        {
+            "x": 4.865797257806183,
+            "y": 4.628342124775828,
+            "heading": -0.5942109950627501,
+            "angularVelocity": -0.00001038289485891648,
+            "velocityX": 3.388450137379422,
+            "velocityY": -1.6819693563748486,
+            "timestamp": 1.4864421280884055
+        },
+        {
+            "x": 5.075661217518705,
+            "y": 4.524169203545277,
+            "heading": -0.594211638127464,
+            "angularVelocity": -0.000010382881945026341,
+            "velocityX": 3.3884501373611275,
+            "velocityY": -1.6819693564178315,
+            "timestamp": 1.5483772167587557
+        },
+        {
+            "x": 5.285525078653758,
+            "y": 4.419996083725492,
+            "heading": -0.5942122811951368,
+            "angularVelocity": -0.000010382929719206097,
+            "velocityX": 3.3884485457355824,
+            "velocityY": -1.6819725628269917,
+            "timestamp": 1.610312305429106
+        },
+        {
+            "x": 5.489266395568848,
+            "y": 4.304305553436279,
+            "heading": -0.5942133396987443,
+            "angularVelocity": -0.000017090531880803138,
+            "velocityX": 3.2895943364108793,
+            "velocityY": -1.8679319392755818,
+            "timestamp": 1.6722473940994562
+        },
+        {
+            "x": 5.681024172067428,
+            "y": 4.156179263559471,
+            "heading": -0.5942132880529166,
+            "angularVelocity": 8.063055616728172e-7,
+            "velocityX": 2.9937628707653796,
+            "velocityY": -2.312578894659331,
+            "timestamp": 1.7362998206864544
+        },
+        {
+            "x": 5.864967854996084,
+            "y": 3.998454299605836,
+            "heading": -0.5942132372714615,
+            "angularVelocity": 7.928107921043856e-7,
+            "velocityX": 2.871767593048443,
+            "velocityY": -2.4624354198261003,
+            "timestamp": 1.8003522472734526
+        },
+        {
+            "x": 6.0489114264021975,
+            "y": 3.840729205591288,
+            "heading": -0.5942131864900138,
+            "angularVelocity": 7.928106775338953e-7,
+            "velocityX": 2.871765851934987,
+            "velocityY": -2.4624374503645132,
+            "timestamp": 1.8644046738604507
+        },
+        {
+            "x": 6.232854997807022,
+            "y": 3.683004111575237,
+            "heading": -0.5942131357085663,
+            "angularVelocity": 7.928106743629292e-7,
+            "velocityX": 2.87176585191486,
+            "velocityY": -2.462437450387986,
+            "timestamp": 1.928457100447449
+        },
+        {
+            "x": 6.4167985692118465,
+            "y": 3.5252790175591864,
+            "heading": -0.594213084927119,
+            "angularVelocity": 7.928106702567426e-7,
+            "velocityX": 2.871765851914861,
+            "velocityY": -2.462437450387986,
+            "timestamp": 1.992509527034447
+        },
+        {
+            "x": 6.6007421406166715,
+            "y": 3.367553923543135,
+            "heading": -0.5942130341456721,
+            "angularVelocity": 7.928106666471698e-7,
+            "velocityX": 2.8717658519148612,
+            "velocityY": -2.462437450387985,
+            "timestamp": 2.0565619536214452
+        },
+        {
+            "x": 6.784685712022088,
+            "y": 3.2098288295277735,
+            "heading": -0.5942129833642253,
+            "angularVelocity": 7.928106614555171e-7,
+            "velocityX": 2.8717658519240943,
+            "velocityY": -2.4624374503772177,
+            "timestamp": 2.120614380208443
+        },
+        {
+            "x": 6.968629334581004,
+            "y": 3.052103795169223,
+            "heading": -0.594212932582592,
+            "angularVelocity": 7.928135744203515e-7,
+            "velocityX": 2.871766650543336,
+            "velocityY": -2.462436519002493,
+            "timestamp": 2.184666806795441
+        },
+        {
+            "x": 7.14554518368725,
+            "y": 2.9064126516878526,
+            "heading": -0.554455950225117,
+            "angularVelocity": 0.6206943979472176,
+            "velocityX": 2.7620475684235433,
+            "velocityY": -2.2745608752775093,
+            "timestamp": 2.248719233382439
+        },
+        {
+            "x": 7.299470766691596,
+            "y": 2.7775737815312107,
+            "heading": -0.43989859569345574,
+            "angularVelocity": 1.7884935924490803,
+            "velocityX": 2.4031186827134454,
+            "velocityY": -2.0114596280228136,
+            "timestamp": 2.312771659969437
+        },
+        {
+            "x": 7.4271138269599435,
+            "y": 2.670230084319628,
+            "heading": -0.3103960824325961,
+            "angularVelocity": 2.021820564205559,
+            "velocityX": 1.9927903917110543,
+            "velocityY": -1.6758724521667427,
+            "timestamp": 2.376824086556435
+        },
+        {
+            "x": 7.52865057013146,
+            "y": 2.5846878922538004,
+            "heading": -0.1939655985882249,
+            "angularVelocity": 1.8177372825404448,
+            "velocityX": 1.5852130603296692,
+            "velocityY": -1.3355027533522315,
+            "timestamp": 2.440876513143433
+        },
+        {
+            "x": 7.604480145165485,
+            "y": 2.520737422138354,
+            "heading": -0.10094853997619462,
+            "angularVelocity": 1.4522019471922978,
+            "velocityX": 1.1838673267285131,
+            "velocityY": -0.9984082340516305,
+            "timestamp": 2.504928939730431
+        },
+        {
+            "x": 7.654868393442648,
+            "y": 2.4782123742697575,
+            "heading": -0.03632146129528005,
+            "angularVelocity": 1.008971589751342,
+            "velocityX": 0.7866719648587251,
+            "velocityY": -0.6639100208145539,
+            "timestamp": 2.568981366317429
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": -3.143185307179586,
-            "angularVelocity": -1.2176316496840747,
-            "velocityX": 0.7103294478144603,
-            "velocityY": -0.5566679739881255,
-            "timestamp": 2.509815618329166
+            "heading": -0.003,
+            "angularVelocity": 0.5202216851226045,
+            "velocityX": 0.39235994475896174,
+            "velocityY": -0.3311720632620588,
+            "timestamp": 2.6330337929044267
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": -3.143185307179586,
-            "angularVelocity": 2.6221634517625535e-33,
-            "velocityX": -1.3293034908554191e-34,
+            "heading": -0.003,
+            "angularVelocity": -9.133242422473854e-34,
+            "velocityX": 1.8081868884139647e-34,
             "velocityY": 0,
-            "timestamp": 2.6256979445131647
+            "timestamp": 2.6970862194914247
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.1.traj
index 19f07e68..17baf1d1 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.1.traj	
@@ -3,659 +3,659 @@
         {
             "x": 0.71,
             "y": 6.72,
-            "heading": -2.1,
+            "heading": 1.04,
             "angularVelocity": 0,
-            "velocityX": 3.044182207072049e-31,
-            "velocityY": 6.458172299259412e-31,
+            "velocityX": -2.70270630963226e-31,
+            "velocityY": -3.181766396851851e-31,
             "timestamp": 0
         },
         {
-            "x": 0.7256414580836799,
-            "y": 6.712155644836975,
-            "heading": -2.1163027262729814,
-            "angularVelocity": -0.3503504178414695,
-            "velocityX": 0.3361395685300024,
-            "velocityY": -0.16857751660924442,
-            "timestamp": 0.04653262974092379
-        },
-        {
-            "x": 0.7569790078652245,
-            "y": 6.696436652979446,
-            "heading": -2.1479259826479233,
-            "angularVelocity": -0.6795931489587892,
-            "velocityX": 0.6734532296158987,
-            "velocityY": -0.3378057923018942,
-            "timestamp": 0.09306525948184757
-        },
-        {
-            "x": 0.8040786472875676,
-            "y": 6.672808979329537,
-            "heading": -2.1936219074673273,
-            "angularVelocity": -0.9820189633351775,
-            "velocityX": 1.0121852060495677,
-            "velocityY": -0.5077657072349027,
-            "timestamp": 0.13959788922277136
-        },
-        {
-            "x": 0.8670199327112733,
-            "y": 6.641236276400096,
-            "heading": -2.2517861808528257,
-            "angularVelocity": -1.2499674681912987,
-            "velocityX": 1.3526268722430308,
-            "velocityY": -0.6785067404360078,
-            "timestamp": 0.18613051896369515
-        },
-        {
-            "x": 0.9458962124147239,
-            "y": 6.601679900322021,
-            "heading": -2.32031104809549,
-            "angularVelocity": -1.4726196998587933,
-            "velocityX": 1.6950746205965188,
-            "velocityY": -0.8500782418336558,
-            "timestamp": 0.23266314870461893
-        },
-        {
-            "x": 1.0408105422873288,
-            "y": 6.554096827806623,
-            "heading": -2.396269750321632,
-            "angularVelocity": -1.6323750161792245,
-            "velocityX": 2.039737070547226,
-            "velocityY": -1.022574326452373,
-            "timestamp": 0.2791957784455427
-        },
-        {
-            "x": 1.1518552587221624,
-            "y": 6.498443176244561,
-            "heading": -2.4750808863817895,
-            "angularVelocity": -1.6936746644023601,
-            "velocityX": 2.3863838569428775,
-            "velocityY": -1.19601346134745,
-            "timestamp": 0.32572840818646653
-        },
-        {
-            "x": 1.2789760591669448,
-            "y": 6.434740942054882,
-            "heading": -2.5479819963693036,
-            "angularVelocity": -1.5666664530556718,
-            "velocityX": 2.7318636654010686,
-            "velocityY": -1.3689798866801088,
-            "timestamp": 0.3722610379273903
-        },
-        {
-            "x": 1.4202169645474811,
-            "y": 6.363052674678261,
-            "heading": -2.59222819945715,
-            "angularVelocity": -0.9508640137943395,
-            "velocityX": 3.0353089040296184,
-            "velocityY": -1.5406021060002604,
-            "timestamp": 0.41879366766831405
-        },
-        {
-            "x": 1.573995110361942,
-            "y": 6.28311594811204,
-            "heading": -2.599064012600599,
-            "angularVelocity": -0.14690364979431952,
-            "velocityX": 3.304737915536797,
-            "velocityY": -1.7178639378700842,
-            "timestamp": 0.4653262974092378
-        },
-        {
-            "x": 1.7314466383055478,
-            "y": 6.204403629832344,
-            "heading": -2.599064059608806,
-            "angularVelocity": -0.0000010102203007383576,
-            "velocityX": 3.3836799858560402,
-            "velocityY": -1.6915510410205123,
-            "timestamp": 0.5118589271501616
-        },
-        {
-            "x": 1.8889032791179137,
-            "y": 6.1257015398581975,
-            "heading": -2.5990641066080484,
-            "angularVelocity": -0.0000010100276450499987,
-            "velocityX": 3.3837898629208136,
-            "velocityY": -1.69133123170322,
-            "timestamp": 0.5583915568910853
-        },
-        {
-            "x": 2.0463595988203984,
-            "y": 6.046998807454145,
-            "heading": -2.5990641536073187,
-            "angularVelocity": -0.0000010100282384452147,
-            "velocityX": 3.3837829621742297,
-            "velocityY": -1.691345037712453,
-            "timestamp": 0.6049241866320091
-        },
-        {
-            "x": 2.2038159116861142,
-            "y": 5.968296061372362,
-            "heading": -2.599064200606589,
-            "angularVelocity": -0.0000010100282469186477,
-            "velocityX": 3.3837828152499863,
-            "velocityY": -1.6913453316510296,
-            "timestamp": 0.6514568163729328
-        },
-        {
-            "x": 2.3612722547062472,
-            "y": 5.889593375618833,
-            "heading": -2.5990642476058565,
-            "angularVelocity": -0.0000010100281851545483,
-            "velocityX": 3.3837834632773647,
-            "velocityY": -1.6913440351789604,
-            "timestamp": 0.6979894461138566
-        },
-        {
-            "x": 2.5187286297414215,
-            "y": 5.8108907539159365,
-            "heading": -2.599064294605121,
-            "angularVelocity": -0.0000010100281210334782,
-            "velocityX": 3.383784151290135,
-            "velocityY": -1.6913426587117795,
-            "timestamp": 0.7445220758547804
-        },
-        {
-            "x": 2.6761850576997652,
-            "y": 5.732188238093708,
-            "heading": -2.599064341604381,
-            "angularVelocity": -0.000001010028016565385,
-            "velocityX": 3.3837852886247464,
-            "velocityY": -1.6913403833048126,
-            "timestamp": 0.7910547055957041
-        },
-        {
-            "x": 2.83364151622281,
-            "y": 5.653485783421201,
-            "heading": -2.5990643886036375,
-            "angularVelocity": -0.0000010100279552603614,
-            "velocityX": 3.3837859454691737,
-            "velocityY": -1.6913390691793195,
-            "timestamp": 0.8375873353366279
-        },
-        {
-            "x": 2.9910977149954676,
-            "y": 5.574782809080547,
-            "heading": -2.5990644356029167,
-            "angularVelocity": -0.000001010028434200447,
-            "velocityX": 3.3837803633561943,
-            "velocityY": -1.691350237002125,
-            "timestamp": 0.8841199650775516
-        },
-        {
-            "x": 3.1485530895378893,
-            "y": 5.496078185774728,
-            "heading": -2.599064482602267,
-            "angularVelocity": -0.0000010100299667197812,
-            "velocityX": 3.3837626504042433,
-            "velocityY": -1.6913856737520705,
-            "timestamp": 0.9306525948184754
-        },
-        {
-            "x": 3.3060077005580313,
-            "y": 5.417372034978031,
-            "heading": -2.5990645296016837,
-            "angularVelocity": -0.0000010100313828443218,
-            "velocityX": 3.383746242085582,
-            "velocityY": -1.6914184999808708,
-            "timestamp": 0.9771852245593992
-        },
-        {
-            "x": 3.463466207634218,
-            "y": 5.3386736786731355,
-            "heading": -2.5990645766007607,
-            "angularVelocity": -0.0000010100240886605546,
-            "velocityX": 3.383829969488029,
-            "velocityY": -1.6912509940353067,
-            "timestamp": 1.023717854300323
-        },
-        {
-            "x": 3.620938146457774,
-            "y": 5.260002202075332,
-            "heading": -2.5990646235986756,
-            "angularVelocity": -0.0000010099991153726974,
-            "velocityX": 3.3841186217193853,
-            "velocityY": -1.690673341164269,
-            "timestamp": 1.0702504840412468
-        },
-        {
-            "x": 3.7783857783864416,
-            "y": 5.181282090499446,
-            "heading": -2.5990646705986817,
-            "angularVelocity": -0.0000010100440573860445,
-            "velocityX": 3.383596259333376,
-            "velocityY": -1.691718521279299,
-            "timestamp": 1.1167831137821707
-        },
-        {
-            "x": 3.9357445242921885,
-            "y": 5.102384448798495,
-            "heading": -2.599064717606569,
-            "angularVelocity": -0.0000010102134222869035,
-            "velocityX": 3.381686072364613,
-            "velocityY": -1.6955336962675904,
-            "timestamp": 1.1633157435230945
+            "x": 0.7256414720586181,
+            "y": 6.712155337793633,
+            "heading": 1.0237015987984905,
+            "angularVelocity": -0.3502584296953155,
+            "velocityX": 0.33614078912617174,
+            "velocityY": -0.16858457660462928,
+            "timestamp": 0.046532502346056014
+        },
+        {
+            "x": 0.756979104040484,
+            "y": 6.696435827024302,
+            "heading": 0.9920873730999193,
+            "angularVelocity": -0.6794009370802031,
+            "velocityX": 0.6734568398839507,
+            "velocityY": -0.33781786873231273,
+            "timestamp": 0.09306500469211203
+        },
+        {
+            "x": 0.8040789517219918,
+            "y": 6.672807532790365,
+            "heading": 0.9464055818688567,
+            "angularVelocity": -0.9817179160357762,
+            "velocityX": 1.0121924527341073,
+            "velocityY": -0.5077804339471543,
+            "timestamp": 0.13959750703816803
+        },
+        {
+            "x": 0.8670206146686451,
+            "y": 6.641234227942618,
+            "heading": 0.8882606987188267,
+            "angularVelocity": -1.2495541872573583,
+            "velocityX": 1.352638688514273,
+            "velocityY": -0.6785215334633973,
+            "timestamp": 0.18613000938422405
+        },
+        {
+            "x": 0.9458974559747272,
+            "y": 6.601677384330224,
+            "heading": 0.8197602775960398,
+            "angularVelocity": -1.4720983757420398,
+            "velocityX": 1.6950913303456163,
+            "velocityY": -0.8500906166232347,
+            "timestamp": 0.23266251173028007
+        },
+        {
+            "x": 1.04081251945038,
+            "y": 6.554094078416389,
+            "heading": 0.743830819371293,
+            "angularVelocity": -1.6317510212566968,
+            "velocityX": 2.039758420249741,
+            "velocityY": -1.0225821418316814,
+            "timestamp": 0.27919501407633607
+        },
+        {
+            "x": 1.15185810993152,
+            "y": 6.498440545281296,
+            "heading": 0.6650543509456113,
+            "angularVelocity": -1.6929342815026889,
+            "velocityX": 2.3864091738569346,
+            "velocityY": -1.1960141907087913,
+            "timestamp": 0.32572751642239206
+        },
+        {
+            "x": 1.2789797685079123,
+            "y": 6.43473905676892,
+            "heading": 0.5921954143923897,
+            "angularVelocity": -1.5657644201343583,
+            "velocityX": 2.7318895861437724,
+            "velocityY": -1.3689676097497174,
+            "timestamp": 0.37226001876844805
+        },
+        {
+            "x": 1.420223914518488,
+            "y": 6.363052522149474,
+            "heading": 0.5479689699773799,
+            "angularVelocity": -0.9504419961367557,
+            "velocityX": 3.035386856270174,
+            "velocityY": -1.5405690862335149,
+            "timestamp": 0.41879252111450405
+        },
+        {
+            "x": 1.5740028534513755,
+            "y": 6.283114192872816,
+            "heading": 0.5411402298477218,
+            "angularVelocity": -0.14675205040326236,
+            "velocityX": 3.304764007515849,
+            "velocityY": -1.717903083787275,
+            "timestamp": 0.46532502346056004
+        },
+        {
+            "x": 1.7314538549626566,
+            "y": 6.20440189931456,
+            "heading": 0.5411401828527235,
+            "angularVelocity": -0.0000010099392058430412,
+            "velocityX": 3.383677936345371,
+            "velocityY": -1.6915551408103213,
+            "timestamp": 0.511857525806616
+        },
+        {
+            "x": 1.888910012901083,
+            "y": 6.125699921176338,
+            "heading": 0.5411401358667378,
+            "angularVelocity": -0.0000010097455172737006,
+            "velocityX": 3.383788749796141,
+            "velocityY": -1.6913334587700644,
+            "timestamp": 0.558390028152672
+        },
+        {
+            "x": 2.0463658388325006,
+            "y": 6.046997278808062,
+            "heading": 0.5411400888807237,
+            "angularVelocity": -0.0000010097461271838877,
+            "velocityX": 3.3837816148475093,
+            "velocityY": -1.691347733310028,
+            "timestamp": 0.604922530498728
+        },
+        {
+            "x": 2.2038216598445532,
+            "y": 5.9682946265981345,
+            "heading": 0.5411400418947097,
+            "angularVelocity": -0.0000010097461301639238,
+            "velocityX": 3.3837815091285663,
+            "velocityY": -1.6913479448107236,
+            "timestamp": 0.651455032844784
+        },
+        {
+            "x": 2.3612775122085363,
+            "y": 5.8895920371121315,
+            "heading": 0.5411399949086985,
+            "angularVelocity": -0.0000010097460671631127,
+            "velocityX": 3.3837821828926944,
+            "velocityY": -1.6913465968512624,
+            "timestamp": 0.69798753519084
+        },
+        {
+            "x": 2.5187333978232895,
+            "y": 5.810889514148883,
+            "heading": 0.5411399479226903,
+            "angularVelocity": -0.0000010097460006294838,
+            "velocityX": 3.383782897463604,
+            "velocityY": -1.6913451672535464,
+            "timestamp": 0.744520037536896
+        },
+        {
+            "x": 2.676189337500118,
+            "y": 5.732187099344657,
+            "heading": 0.541139900936687,
+            "angularVelocity": -0.0000010097458968738495,
+            "velocityX": 3.3837840592768114,
+            "velocityY": -1.6913428428777157,
+            "timestamp": 0.791052539882952
+        },
+        {
+            "x": 2.833645305906935,
+            "y": 5.653484742019454,
+            "heading": 0.5411398539506863,
+            "angularVelocity": -0.0000010097458381135432,
+            "velocityX": 3.3837846766944186,
+            "velocityY": -1.6913416076332204,
+            "timestamp": 0.837585042229008
+        },
+        {
+            "x": 2.991101010217502,
+            "y": 5.57478185633263,
+            "heading": 0.5411398069646632,
+            "angularVelocity": -0.0000010097463224464638,
+            "velocityX": 3.383779001172013,
+            "velocityY": -1.6913529623124643,
+            "timestamp": 0.884117544575064
+        },
+        {
+            "x": 3.14855589978306,
+            "y": 5.496077340659936,
+            "heading": 0.5411397599785701,
+            "angularVelocity": -0.0000010097478275290572,
+            "velocityX": 3.3837614920124643,
+            "velocityY": -1.6913879912875969,
+            "timestamp": 0.93065004692112
+        },
+        {
+            "x": 3.306010013541662,
+            "y": 5.417371272923184,
+            "heading": 0.5411397129924104,
+            "angularVelocity": -0.0000010097492581811278,
+            "velocityX": 3.3837448196468105,
+            "velocityY": -1.6914213456927205,
+            "timestamp": 0.977182549267176
+        },
+        {
+            "x": 3.4634680562159517,
+            "y": 5.338673065406879,
+            "heading": 0.5411396660065909,
+            "angularVelocity": -0.0000010097419454671228,
+            "velocityX": 3.383829253443873,
+            "velocityY": -1.6912524267565827,
+            "timestamp": 1.023715051613232
+        },
+        {
+            "x": 3.6209395624979943,
+            "y": 5.2600018013447745,
+            "heading": 0.5411396190219294,
+            "angularVelocity": -0.0000010097170609305348,
+            "velocityX": 3.3841185911564384,
+            "velocityY": -1.690673402356174,
+            "timestamp": 1.0702475539592882
+        },
+        {
+            "x": 3.778386684395729,
+            "y": 5.181281747313384,
+            "heading": 0.5411395720351826,
+            "angularVelocity": -0.0000010097618775946008,
+            "velocityX": 3.3835945620651766,
+            "velocityY": -1.6917219161486032,
+            "timestamp": 1.1167800563053443
+        },
+        {
+            "x": 3.9357449151310644,
+            "y": 5.10238415335025,
+            "heading": 0.5411395250405994,
+            "angularVelocity": -0.000001009930280464059,
+            "velocityX": 3.3816842594175633,
+            "velocityY": -1.6955373123309678,
+            "timestamp": 1.1633125586514004
         },
         {
             "x": 4.092995643615723,
             "y": 5.023272514343262,
-            "heading": -2.5990647646424785,
-            "angularVelocity": -0.000001010815630420508,
-            "velocityX": 3.379373145230901,
-            "velocityY": -1.700138911036495,
-            "timestamp": 1.2098483732640184
-        },
-        {
-            "x": 4.261229917640389,
-            "y": 4.916068911394467,
-            "heading": -2.599064809945939,
-            "angularVelocity": -8.591015508014648e-7,
-            "velocityX": 3.1902711911232386,
-            "velocityY": -2.0329303767326627,
-            "timestamp": 1.2625819071694577
-        },
-        {
-            "x": 4.429297438166161,
-            "y": 4.8086040706119935,
-            "heading": -2.5990648552466906,
-            "angularVelocity": -8.590501714319945e-7,
-            "velocityX": 3.187109000265223,
-            "velocityY": -2.0378842991085575,
-            "timestamp": 1.315315441074897
-        },
-        {
-            "x": 4.597219271125054,
-            "y": 4.700911724294517,
-            "heading": -2.599064900558894,
-            "angularVelocity": -8.592673362873697e-7,
-            "velocityX": 3.184346288265075,
-            "velocityY": -2.0421985469549115,
-            "timestamp": 1.3680489749803364
-        },
-        {
-            "x": 4.765178923625143,
-            "y": 4.59327837178587,
-            "heading": -2.5990649458681174,
-            "angularVelocity": -8.592108353148986e-7,
-            "velocityX": 3.1850634702628065,
-            "velocityY": -2.0410798317000203,
-            "timestamp": 1.4207825088857757
-        },
-        {
-            "x": 4.93313403793343,
-            "y": 4.485637937706628,
-            "heading": -2.599064991177699,
-            "angularVelocity": -8.592176153657431e-7,
-            "velocityX": 3.184977411326241,
-            "velocityY": -2.041214121404157,
-            "timestamp": 1.473516042791215
-        },
-        {
-            "x": 5.101087827105617,
-            "y": 4.377995436130629,
-            "heading": -2.599065036487385,
-            "angularVelocity": -8.592196078701955e-7,
-            "velocityX": 3.184952282419374,
-            "velocityY": -2.041253327892839,
-            "timestamp": 1.5262495766966544
-        },
-        {
-            "x": 5.269016802174646,
-            "y": 4.270314227125004,
-            "heading": -2.599065081799025,
-            "angularVelocity": -8.592566538542498e-7,
-            "velocityX": 3.1844817259951634,
-            "velocityY": -2.041987347154346,
-            "timestamp": 1.5789831106020937
-        },
-        {
-            "x": 5.437055950944385,
-            "y": 4.162805027649924,
-            "heading": -2.599065127101977,
-            "angularVelocity": -8.590919076920599e-7,
-            "velocityX": 3.1865709791255945,
-            "velocityY": -2.038725484769403,
-            "timestamp": 1.631716644507533
-        },
-        {
-            "x": 5.605213465892237,
-            "y": 4.055481061993006,
-            "heading": -2.599065172406386,
-            "angularVelocity": -8.591195260284143e-7,
-            "velocityX": 3.188815588376687,
-            "velocityY": -2.035212846712725,
-            "timestamp": 1.6844501784129724
+            "heading": 0.5411394780180799,
+            "angularVelocity": -0.0000010105306414990813,
+            "velocityX": 3.379373997880459,
+            "velocityY": -1.7001372163227824,
+            "timestamp": 1.2098450609974565
+        },
+        {
+            "x": 4.2612300154414715,
+            "y": 4.916069066513277,
+            "heading": 0.541139432730096,
+            "angularVelocity": -8.588080638060829e-7,
+            "velocityX": 3.1902730598822937,
+            "velocityY": -2.0329274441782497,
+            "timestamp": 1.2625785946693153
+        },
+        {
+            "x": 4.429297905294805,
+            "y": 4.808604804974877,
+            "heading": 0.5411393874448718,
+            "angularVelocity": -8.58755730533247e-7,
+            "velocityX": 3.1871160180389406,
+            "velocityY": -2.0378733237761697,
+            "timestamp": 1.3153121283411742
+        },
+        {
+            "x": 4.5972191269209945,
+            "y": 4.700911507061467,
+            "heading": 0.5411393421481548,
+            "angularVelocity": -8.589736709635531e-7,
+            "velocityX": 3.1843347095055985,
+            "velocityY": -2.042216601367096,
+            "timestamp": 1.368045662013033
+        },
+        {
+            "x": 4.765178956046694,
+            "y": 4.593278431809082,
+            "heading": 0.5411392968544698,
+            "angularVelocity": -8.589161734970966e-7,
+            "velocityX": 3.185066833769243,
+            "velocityY": -2.0410745830567376,
+            "timestamp": 1.4207791956848919
+        },
+        {
+            "x": 4.9331339491528,
+            "y": 4.485637810244685,
+            "heading": 0.5411392515604048,
+            "angularVelocity": -8.58923380695705e-7,
+            "velocityX": 3.1849751270448214,
+            "velocityY": -2.041217685776372,
+            "timestamp": 1.4735127293567507
+        },
+        {
+            "x": 5.1010877481253685,
+            "y": 4.377995325600118,
+            "heading": 0.5411392062662455,
+            "angularVelocity": -8.589251678490757e-7,
+            "velocityX": 3.1849524823742743,
+            "velocityY": -2.041253015859115,
+            "timestamp": 1.5262462630286096
+        },
+        {
+            "x": 5.269016703536996,
+            "y": 4.270314087571009,
+            "heading": 0.5411391609701367,
+            "angularVelocity": -8.589621362197366e-7,
+            "velocityX": 3.184481367332135,
+            "velocityY": -2.0419879065793034,
+            "timestamp": 1.5789797967004684
+        },
+        {
+            "x": 5.437056059940434,
+            "y": 4.162805214274757,
+            "heading": 0.5411391156827058,
+            "angularVelocity": -8.587975760545075e-7,
+            "velocityX": 3.18657493065327,
+            "velocityY": -2.038719308384206,
+            "timestamp": 1.6317133303723272
+        },
+        {
+            "x": 5.605213513282879,
+            "y": 4.0554811537322735,
+            "heading": 0.5411390703937885,
+            "angularVelocity": -8.588257618591655e-7,
+            "velocityX": 3.188814434261697,
+            "velocityY": -2.03521465506778,
+            "timestamp": 1.684446864044186
         },
         {
             "x": 5.7832255363464355,
             "y": 3.9654417037963863,
-            "heading": -2.599065217938776,
-            "angularVelocity": -8.634427952488677e-7,
-            "velocityX": 3.3756901400427615,
-            "velocityY": -1.707440247757134,
-            "timestamp": 1.7371837123184117
-        },
-        {
-            "x": 5.945200300467497,
-            "y": 3.903126168683373,
-            "heading": -2.599065262330506,
-            "angularVelocity": -9.676334890696444e-7,
-            "velocityX": 3.5306622744884395,
-            "velocityY": -1.3583295529523924,
-            "timestamp": 1.7830603075655527
-        },
-        {
-            "x": 6.112434090508488,
-            "y": 3.856737486793876,
-            "heading": -2.5990653054733555,
-            "angularVelocity": -9.404108896808373e-7,
-            "velocityX": 3.6452964554166534,
-            "velocityY": -1.01116226345067,
-            "timestamp": 1.8289369028126936
-        },
-        {
-            "x": 6.280097327658818,
-            "y": 3.8119257778274935,
-            "heading": -2.599065348543951,
-            "angularVelocity": -9.38835914358075e-7,
-            "velocityX": 3.6546573747925137,
-            "velocityY": -0.9767880272056341,
-            "timestamp": 1.8748134980598345
-        },
-        {
-            "x": 6.444605868501446,
-            "y": 3.756644766145867,
-            "heading": -2.599065392143275,
-            "angularVelocity": -9.503609402316767e-7,
-            "velocityX": 3.5858925440393525,
-            "velocityY": -1.2049937747953314,
-            "timestamp": 1.9206900933069755
-        },
-        {
-            "x": 6.602719051918594,
-            "y": 3.685095684621415,
-            "heading": -2.5990654382603977,
-            "angularVelocity": -0.0000010052429246959388,
-            "velocityX": 3.446489055375297,
-            "velocityY": -1.5595987701135843,
-            "timestamp": 1.9665666885541164
-        },
-        {
-            "x": 6.752801915063028,
-            "y": 3.597951074081566,
-            "heading": -2.599065489714602,
-            "angularVelocity": -0.0000011215785414644313,
-            "velocityX": 3.2714472888827597,
-            "velocityY": -1.8995439846919482,
-            "timestamp": 2.0124432838012574
-        },
-        {
-            "x": 6.893316861258978,
-            "y": 3.49609530674715,
-            "heading": -2.599065550773928,
-            "angularVelocity": -0.0000013309472018502562,
-            "velocityX": 3.06288959411619,
-            "velocityY": -2.2202120010370177,
-            "timestamp": 2.058319879048398
-        },
-        {
-            "x": 7.022827120260116,
-            "y": 3.380569821579126,
-            "heading": -2.5990656290967817,
-            "angularVelocity": -0.0000017072507862805843,
-            "velocityX": 2.8230137459733338,
-            "velocityY": -2.5181791400535034,
-            "timestamp": 2.104196474295539
+            "heading": 0.5411390248741499,
+            "angularVelocity": -8.632009947472993e-7,
+            "velocityX": 3.3756892563138687,
+            "velocityY": -1.7074419949962474,
+            "timestamp": 1.737180397716045
+        },
+        {
+            "x": 5.9452002659111205,
+            "y": 3.9031260890359523,
+            "heading": 0.54113898049095,
+            "angularVelocity": -9.674475759968245e-7,
+            "velocityX": 3.530661595583403,
+            "velocityY": -1.358331317676469,
+            "timestamp": 1.7830569919972126
+        },
+        {
+            "x": 6.112434032055133,
+            "y": 3.8567373346649623,
+            "heading": 0.5411389373525121,
+            "angularVelocity": -9.403147416321739e-7,
+            "velocityX": 3.6452960112747332,
+            "velocityY": -1.0111638646647567,
+            "timestamp": 1.8289335862783802
+        },
+        {
+            "x": 6.280097272848707,
+            "y": 3.8119256534794217,
+            "heading": 0.5411388942859083,
+            "angularVelocity": -9.387489265836176e-7,
+            "velocityX": 3.65465753115864,
+            "velocityY": -0.9767874422169035,
+            "timestamp": 1.8748101805595478
+        },
+        {
+            "x": 6.4446058180798245,
+            "y": 3.756644666326502,
+            "heading": 0.5411388506931323,
+            "angularVelocity": -9.502182266145116e-7,
+            "velocityX": 3.5858927152021427,
+            "velocityY": -1.204993265500441,
+            "timestamp": 1.9206867748407155
+        },
+        {
+            "x": 6.602719007178768,
+            "y": 3.68509560621824,
+            "heading": 0.5411388045872843,
+            "angularVelocity": -0.0000010049971826881646,
+            "velocityX": 3.4464892517938135,
+            "velocityY": -1.5595983361306547,
+            "timestamp": 1.9665633691218831
+        },
+        {
+            "x": 6.75280187814694,
+            "y": 3.5979510164257396,
+            "heading": 0.5411387531507533,
+            "angularVelocity": -0.000001121193317368561,
+            "velocityX": 3.271447528304838,
+            "velocityY": -1.899543572446005,
+            "timestamp": 2.0124399634030508
+        },
+        {
+            "x": 6.893316834439751,
+            "y": 3.4960952692408354,
+            "heading": 0.5411386921189019,
+            "angularVelocity": -0.0000013303483490867941,
+            "velocityX": 3.0628898786954792,
+            "velocityY": -2.2202116085744548,
+            "timestamp": 2.0583165576842184
+        },
+        {
+            "x": 7.022827105769335,
+            "y": 3.380569803374169,
+            "heading": 0.5411386138405815,
+            "angularVelocity": -0.0000017062801116380342,
+            "velocityX": 2.8230140741451093,
+            "velocityY": -2.5181787723526305,
+            "timestamp": 2.104193151965386
         },
         {
             "x": 7.140008449554443,
             "y": 3.2525558471679688,
-            "heading": -2.5990657325552275,
-            "angularVelocity": -0.000002255146554794306,
-            "velocityX": 2.5542725797993215,
-            "velocityY": -2.790398322315311,
-            "timestamp": 2.1500730695426795
-        },
-        {
-            "x": 7.220411639481594,
-            "y": 3.1500923106303724,
-            "heading": -2.599065847631647,
-            "angularVelocity": -0.000003342401256012092,
-            "velocityX": 2.3353152959116907,
-            "velocityY": -2.9760593375246422,
-            "timestamp": 2.1845023348619734
-        },
-        {
-            "x": 7.292813286750944,
-            "y": 3.0418266490558983,
-            "heading": -2.5990659368308053,
-            "angularVelocity": -0.0000025907947068596104,
-            "velocityX": 2.1029100272080186,
-            "velocityY": -3.1445823943795888,
-            "timestamp": 2.2189316001812673
-        },
-        {
-            "x": 7.356796447286593,
-            "y": 2.9283823501608603,
-            "heading": -2.599066009494182,
-            "angularVelocity": -0.0000021105119622632264,
-            "velocityX": 1.8583945937350983,
-            "velocityY": -3.294996214498541,
-            "timestamp": 2.2533608655005612
-        },
-        {
-            "x": 7.411992663456803,
-            "y": 2.8104127239306367,
-            "heading": -2.5990660711538616,
-            "angularVelocity": -0.0000017909089651975303,
-            "velocityX": 1.603177287064451,
-            "velocityY": -3.426434608353826,
-            "timestamp": 2.287790130819855
-        },
-        {
-            "x": 7.458084080553435,
-            "y": 2.688597137512427,
-            "heading": -2.599066125344508,
-            "angularVelocity": -0.0000015739704607850277,
-            "velocityX": 1.3387278720350797,
-            "velocityY": -3.538140744175163,
-            "timestamp": 2.322219396139149
-        },
-        {
-            "x": 7.494809849686971,
-            "y": 2.5636384472618623,
-            "heading": -2.599066174481706,
-            "angularVelocity": -0.0000014271927566927072,
-            "velocityX": 1.0667020859416116,
-            "velocityY": -3.6294323765468546,
-            "timestamp": 2.356648661458443
-        },
-        {
-            "x": 7.5219737035667364,
-            "y": 2.4362587715435033,
-            "heading": -2.599066220329768,
-            "angularVelocity": -0.0000013316596016825942,
-            "velocityX": 0.7889757050532209,
-            "velocityY": -3.699750039306788,
-            "timestamp": 2.391077926777737
-        },
-        {
-            "x": 7.539529775464085,
-            "y": 2.307203584640024,
-            "heading": -2.5990662642857343,
-            "angularVelocity": -0.0000012767035893272692,
-            "velocityX": 0.5099171223818962,
-            "velocityY": -3.748415358464242,
-            "timestamp": 2.425507192097031
-        },
-        {
-            "x": 7.55111544342237,
-            "y": 2.1774760653156817,
-            "heading": -2.599066307607684,
-            "angularVelocity": -0.0000012582885205698109,
-            "velocityX": 0.3365063950923211,
-            "velocityY": -3.7679432924653384,
-            "timestamp": 2.4599364574163247
-        },
-        {
-            "x": 7.56271571323303,
-            "y": 2.047749850875278,
-            "heading": -2.5990663509314866,
-            "angularVelocity": -0.0000012583423688964546,
-            "velocityX": 0.3369305067394331,
-            "velocityY": -3.767905392035856,
-            "timestamp": 2.4943657227356186
-        },
-        {
-            "x": 7.5743766744470316,
-            "y": 1.9180290778400821,
-            "heading": -2.599066394258993,
-            "angularVelocity": -0.0000012584499309396356,
-            "velocityX": 0.33869329205287857,
-            "velocityY": -3.7677473461074933,
-            "timestamp": 2.5287949880549125
-        },
-        {
-            "x": 7.586111158859003,
-            "y": 1.7883149350094998,
-            "heading": -2.5990664375910546,
-            "angularVelocity": -0.0000012585822270319525,
-            "velocityX": 0.34082877758442714,
-            "velocityY": -3.7675547714315485,
-            "timestamp": 2.5632242533742065
-        },
-        {
-            "x": 7.597929633339714,
-            "y": 1.6586084182080805,
-            "heading": -2.59906648228782,
-            "angularVelocity": -0.0000012982201369267655,
-            "velocityX": 0.3432682739856076,
-            "velocityY": -3.7673332729737243,
-            "timestamp": 2.5976535186935004
-        },
-        {
-            "x": 7.610690036465753,
-            "y": 1.5360359058677395,
-            "heading": -2.6173573406008535,
-            "angularVelocity": -0.5312590362708309,
-            "velocityX": 0.37062664589906397,
-            "velocityY": -3.5601257013070047,
-            "timestamp": 2.6320827840127943
-        },
-        {
-            "x": 7.621968158889347,
-            "y": 1.4215047209508034,
-            "heading": -2.657213150288673,
-            "angularVelocity": -1.1576142946476244,
-            "velocityX": 0.3275737172727147,
-            "velocityY": -3.3265648817881672,
-            "timestamp": 2.666512049332088
-        },
-        {
-            "x": 7.631829723236215,
-            "y": 1.3151734834169941,
-            "heading": -2.7178665355180023,
-            "angularVelocity": -1.76168107762032,
-            "velocityX": 0.28642970610680407,
-            "velocityY": -3.08839693637669,
-            "timestamp": 2.700941314651382
-        },
-        {
-            "x": 7.640728077188491,
-            "y": 1.2186118736781408,
-            "heading": -2.7826314980185702,
-            "angularVelocity": -1.8811020769678346,
-            "velocityX": 0.2584532045561674,
-            "velocityY": -2.8046375327311788,
-            "timestamp": 2.735370579970676
-        },
-        {
-            "x": 7.648685947205252,
-            "y": 1.1318864826516366,
-            "heading": -2.8465688471146957,
-            "angularVelocity": -1.8570639978286094,
-            "velocityX": 0.23113679432217057,
-            "velocityY": -2.518943992043375,
-            "timestamp": 2.76979984528997
-        },
-        {
-            "x": 7.655720752960135,
-            "y": 1.0549563243440254,
-            "heading": -2.9071249866023474,
-            "angularVelocity": -1.7588565694352556,
-            "velocityX": 0.20432633951521018,
-            "velocityY": -2.2344408919030423,
-            "timestamp": 2.804229110609264
-        },
-        {
-            "x": 7.661844928847109,
-            "y": 0.9877692823605362,
-            "heading": -2.962675956286548,
-            "angularVelocity": -1.6134811233710036,
-            "velocityX": 0.17787704239939275,
-            "velocityY": -1.9514515154593546,
-            "timestamp": 2.8386583759285577
-        },
-        {
-            "x": 7.667068746842824,
-            "y": 0.930277282093812,
-            "heading": -3.0120810880108007,
-            "angularVelocity": -1.4349749048108316,
-            "velocityX": 0.1517260954387245,
-            "velocityY": -1.6698584687633535,
-            "timestamp": 2.8730876412478517
-        },
-        {
-            "x": 7.6714018244195765,
-            "y": 0.8824389383291659,
-            "heading": -3.0544961336196454,
-            "angularVelocity": -1.231947449807498,
-            "velocityX": 0.12585448851644476,
-            "velocityY": -1.3894674580185522,
-            "timestamp": 2.9075169065671456
-        },
-        {
-            "x": 7.674853405168496,
-            "y": 0.8442193530437802,
-            "heading": -3.0892755187362573,
-            "angularVelocity": -1.0101692497377945,
-            "velocityX": 0.10025136223247667,
-            "velocityY": -1.1100900623623653,
-            "timestamp": 2.9419461718864395
-        },
-        {
-            "x": 7.677432109437281,
-            "y": 0.8155891364108041,
-            "heading": -3.1159120874216906,
-            "angularVelocity": -0.773660676125647,
-            "velocityX": 0.07489861444537417,
-            "velocityY": -0.8315662959247566,
-            "timestamp": 2.9763754372057334
-        },
-        {
-            "x": 7.679145555938743,
-            "y": 0.7965232635297996,
-            "heading": -3.1339959816499734,
-            "angularVelocity": -0.525247752473809,
-            "velocityX": 0.04976715261198055,
-            "velocityY": -0.5537693791659258,
-            "timestamp": 3.0108047025250273
+            "heading": 0.541138510449912,
+            "angularVelocity": -0.0000022536692406151377,
+            "velocityX": 2.5542729494461978,
+            "velocityY": -2.790397984245173,
+            "timestamp": 2.1500697462465537
+        },
+        {
+            "x": 7.220411568965462,
+            "y": 3.1500924469852043,
+            "heading": 0.5411383954402629,
+            "angularVelocity": -0.0000033404657713126234,
+            "velocityX": 2.3353159519018676,
+            "velocityY": -2.97605882316215,
+            "timestamp": 2.1844989716991257
+        },
+        {
+            "x": 7.292813165778826,
+            "y": 3.0418269330928753,
+            "heading": 0.5411383062644725,
+            "angularVelocity": -0.000002590118983726798,
+            "velocityX": 2.102910996737314,
+            "velocityY": -3.1445817461525896,
+            "timestamp": 2.2189281971516976
+        },
+        {
+            "x": 7.3567962971772936,
+            "y": 2.928382790909777,
+            "heading": 0.5411382336043034,
+            "angularVelocity": -0.000002110421251450144,
+            "velocityX": 1.8583958993386591,
+            "velocityY": -3.2949954781694024,
+            "timestamp": 2.2533574226042696
+        },
+        {
+            "x": 7.411992506551136,
+            "y": 2.810413328004449,
+            "heading": 0.5411381719390687,
+            "angularVelocity": -0.00000179107237982498,
+            "velocityX": 1.603178946034542,
+            "velocityY": -3.426433832147572,
+            "timestamp": 2.2877866480568416
+        },
+        {
+            "x": 7.458083940034777,
+            "y": 2.688597909034967,
+            "heading": 0.5411381177395469,
+            "angularVelocity": -0.0000015742300651441758,
+            "velocityX": 1.3387298981538813,
+            "velocityY": -3.5381399775400135,
+            "timestamp": 2.3222158735094136
+        },
+        {
+            "x": 7.4948097486817264,
+            "y": 2.563639387590104,
+            "heading": 0.5411380685927316,
+            "angularVelocity": -0.0000014274737405214791,
+            "velocityX": 1.0667044687814087,
+            "velocityY": -3.6294316762078314,
+            "timestamp": 2.3566450989619856
+        },
+        {
+            "x": 7.521973664613143,
+            "y": 2.436259879309271,
+            "heading": 0.5411380227355477,
+            "angularVelocity": -0.0000013319260973088958,
+            "velocityX": 0.7889784209297904,
+            "velocityY": -3.6997494601296452,
+            "timestamp": 2.3910743244145576
+        },
+        {
+            "x": 7.539529818670366,
+            "y": 2.3072048557852676,
+            "heading": 0.5411379787715688,
+            "angularVelocity": -0.0000012769377868531526,
+            "velocityX": 0.5099200991725963,
+            "velocityY": -3.748414953504679,
+            "timestamp": 2.4255035498671296
+        },
+        {
+            "x": 7.551115587366525,
+            "y": 2.177477496871536,
+            "heading": 0.5411379354425048,
+            "angularVelocity": -0.0000012584966258013745,
+            "velocityX": 0.336509710684848,
+            "velocityY": -3.7679429963488027,
+            "timestamp": 2.4599327753197016
+        },
+        {
+            "x": 7.56271584344406,
+            "y": 2.0477514326184285,
+            "heading": 0.5411378921115911,
+            "angularVelocity": -0.0000012585503448264627,
+            "velocityX": 0.3369304980023622,
+            "velocityY": -3.76790539281253,
+            "timestamp": 2.4943620007722735
+        },
+        {
+            "x": 7.574376716473137,
+            "y": 1.9180308030778739,
+            "heading": 0.5411378487769694,
+            "angularVelocity": -0.0000012586580504181092,
+            "velocityX": 0.3386911228976962,
+            "velocityY": -3.767747541089928,
+            "timestamp": 2.5287912262248455
+        },
+        {
+            "x": 7.58611108718596,
+            "y": 1.788316801391074,
+            "heading": 0.541137805437783,
+            "angularVelocity": -0.0000012587906282413006,
+            "velocityX": 0.34082586983899804,
+            "velocityY": -3.7675550344719086,
+            "timestamp": 2.5632204516774175
+        },
+        {
+            "x": 7.597929211011114,
+            "y": 1.6586104040760536,
+            "heading": 0.541137760738121,
+            "angularVelocity": -0.0000012983057658513822,
+            "velocityX": 0.3432584866444477,
+            "velocityY": -3.7673341647982834,
+            "timestamp": 2.5976496771299895
+        },
+        {
+            "x": 7.610687440254598,
+            "y": 1.5360368797463901,
+            "heading": 0.5228402741207003,
+            "angularVelocity": -0.5314521711397164,
+            "velocityX": 0.37056393444221536,
+            "velocityY": -3.560159217014913,
+            "timestamp": 2.6320789025825615
+        },
+        {
+            "x": 7.6219652115761445,
+            "y": 1.4215043504075173,
+            "heading": 0.4829676933785713,
+            "angularVelocity": -1.1581027518921772,
+            "velocityX": 0.3275638987899306,
+            "velocityY": -3.3266077825840545,
+            "timestamp": 2.6665081280351335
+        },
+        {
+            "x": 7.631828440236478,
+            "y": 1.3151733520485158,
+            "heading": 0.42230189460591644,
+            "angularVelocity": -1.7620436700286786,
+            "velocityX": 0.2864783779098223,
+            "velocityY": -3.0883935656779764,
+            "timestamp": 2.7009373534877055
+        },
+        {
+            "x": 7.640727843462309,
+            "y": 1.2186121100176637,
+            "heading": 0.3575309788065375,
+            "angularVelocity": -1.8812771692652814,
+            "velocityX": 0.2584839800738238,
+            "velocityY": -2.804630100199854,
+            "timestamp": 2.7353665789402775
+        },
+        {
+            "x": 7.648686345492233,
+            "y": 1.1318869979940107,
+            "heading": 0.2935915741251458,
+            "angularVelocity": -1.8571258528454382,
+            "velocityX": 0.23115541884308086,
+            "velocityY": -2.518938805147378,
+            "timestamp": 2.7697958043928494
+        },
+        {
+            "x": 7.655721476158885,
+            "y": 1.054956995812666,
+            "heading": 0.2330360616068616,
+            "angularVelocity": -1.7588403956899903,
+            "velocityX": 0.20433601320340974,
+            "velocityY": -2.234438944533275,
+            "timestamp": 2.8042250298454214
+        },
+        {
+            "x": 7.661845751852373,
+            "y": 0.9877699914439897,
+            "heading": 0.17748768247912494,
+            "angularVelocity": -1.6134077487241032,
+            "velocityX": 0.17788014725831017,
+            "velocityY": -1.9514526825829648,
+            "timestamp": 2.8386542552979934
+        },
+        {
+            "x": 7.667069516180101,
+            "y": 0.9302779311076093,
+            "heading": 0.1280864585154153,
+            "angularVelocity": -1.4348630651528973,
+            "velocityX": 0.15172471233573487,
+            "velocityY": -1.6698621470755346,
+            "timestamp": 2.8730834807505654
+        },
+        {
+            "x": 7.6714024480246685,
+            "y": 0.88243945985842,
+            "heading": 0.08567598072175592,
+            "angularVelocity": -1.2318162037100373,
+            "velocityX": 0.1258504014428196,
+            "velocityY": -1.3894727697283988,
+            "timestamp": 2.9075127062031374
+        },
+        {
+            "x": 7.674853841393155,
+            "y": 0.8442197139951643,
+            "heading": 0.050901182135293016,
+            "angularVelocity": -1.0100372032583045,
+            "velocityX": 0.10024603583499626,
+            "velocityY": -1.110096011770723,
+            "timestamp": 2.9419419316557094
+        },
+        {
+            "x": 7.6774323573426795,
+            "y": 0.8155893376478138,
+            "heading": 0.02426863930506587,
+            "angularVelocity": -0.7735446406401408,
+            "velocityX": 0.07489323142279661,
+            "velocityY": -0.8315718977410657,
+            "timestamp": 2.9763711571082814
+        },
+        {
+            "x": 7.6791456483845035,
+            "y": 0.7965233362757584,
+            "heading": 0.006187731070844568,
+            "angularVelocity": -0.5251616322048499,
+            "velocityX": 0.04976269489958636,
+            "velocityY": -0.5537737524278505,
+            "timestamp": 3.0108003825608534
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.2669044908275344,
-            "velocityX": 0.024817377116055982,
-            "velocityY": -0.2766037393328496,
-            "timestamp": 3.045233967844321
+            "heading": -0.003,
+            "angularVelocity": -0.26685848868430057,
+            "velocityX": 0.024814720757314154,
+            "velocityY": -0.2766061725343536,
+            "timestamp": 3.0452296080134253
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": -3.143185307179586,
-            "angularVelocity": -4.433819854347959e-31,
+            "heading": -0.003,
+            "angularVelocity": -2.388488097950269e-30,
             "velocityX": 0,
-            "velocityY": 2.2179802932100695e-31,
-            "timestamp": 3.079663233163615
+            "velocityY": 9.563090319474696e-31,
+            "timestamp": 3.0796588334659973
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.traj
index 19f07e68..17baf1d1 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.traj	
@@ -3,659 +3,659 @@
         {
             "x": 0.71,
             "y": 6.72,
-            "heading": -2.1,
+            "heading": 1.04,
             "angularVelocity": 0,
-            "velocityX": 3.044182207072049e-31,
-            "velocityY": 6.458172299259412e-31,
+            "velocityX": -2.70270630963226e-31,
+            "velocityY": -3.181766396851851e-31,
             "timestamp": 0
         },
         {
-            "x": 0.7256414580836799,
-            "y": 6.712155644836975,
-            "heading": -2.1163027262729814,
-            "angularVelocity": -0.3503504178414695,
-            "velocityX": 0.3361395685300024,
-            "velocityY": -0.16857751660924442,
-            "timestamp": 0.04653262974092379
-        },
-        {
-            "x": 0.7569790078652245,
-            "y": 6.696436652979446,
-            "heading": -2.1479259826479233,
-            "angularVelocity": -0.6795931489587892,
-            "velocityX": 0.6734532296158987,
-            "velocityY": -0.3378057923018942,
-            "timestamp": 0.09306525948184757
-        },
-        {
-            "x": 0.8040786472875676,
-            "y": 6.672808979329537,
-            "heading": -2.1936219074673273,
-            "angularVelocity": -0.9820189633351775,
-            "velocityX": 1.0121852060495677,
-            "velocityY": -0.5077657072349027,
-            "timestamp": 0.13959788922277136
-        },
-        {
-            "x": 0.8670199327112733,
-            "y": 6.641236276400096,
-            "heading": -2.2517861808528257,
-            "angularVelocity": -1.2499674681912987,
-            "velocityX": 1.3526268722430308,
-            "velocityY": -0.6785067404360078,
-            "timestamp": 0.18613051896369515
-        },
-        {
-            "x": 0.9458962124147239,
-            "y": 6.601679900322021,
-            "heading": -2.32031104809549,
-            "angularVelocity": -1.4726196998587933,
-            "velocityX": 1.6950746205965188,
-            "velocityY": -0.8500782418336558,
-            "timestamp": 0.23266314870461893
-        },
-        {
-            "x": 1.0408105422873288,
-            "y": 6.554096827806623,
-            "heading": -2.396269750321632,
-            "angularVelocity": -1.6323750161792245,
-            "velocityX": 2.039737070547226,
-            "velocityY": -1.022574326452373,
-            "timestamp": 0.2791957784455427
-        },
-        {
-            "x": 1.1518552587221624,
-            "y": 6.498443176244561,
-            "heading": -2.4750808863817895,
-            "angularVelocity": -1.6936746644023601,
-            "velocityX": 2.3863838569428775,
-            "velocityY": -1.19601346134745,
-            "timestamp": 0.32572840818646653
-        },
-        {
-            "x": 1.2789760591669448,
-            "y": 6.434740942054882,
-            "heading": -2.5479819963693036,
-            "angularVelocity": -1.5666664530556718,
-            "velocityX": 2.7318636654010686,
-            "velocityY": -1.3689798866801088,
-            "timestamp": 0.3722610379273903
-        },
-        {
-            "x": 1.4202169645474811,
-            "y": 6.363052674678261,
-            "heading": -2.59222819945715,
-            "angularVelocity": -0.9508640137943395,
-            "velocityX": 3.0353089040296184,
-            "velocityY": -1.5406021060002604,
-            "timestamp": 0.41879366766831405
-        },
-        {
-            "x": 1.573995110361942,
-            "y": 6.28311594811204,
-            "heading": -2.599064012600599,
-            "angularVelocity": -0.14690364979431952,
-            "velocityX": 3.304737915536797,
-            "velocityY": -1.7178639378700842,
-            "timestamp": 0.4653262974092378
-        },
-        {
-            "x": 1.7314466383055478,
-            "y": 6.204403629832344,
-            "heading": -2.599064059608806,
-            "angularVelocity": -0.0000010102203007383576,
-            "velocityX": 3.3836799858560402,
-            "velocityY": -1.6915510410205123,
-            "timestamp": 0.5118589271501616
-        },
-        {
-            "x": 1.8889032791179137,
-            "y": 6.1257015398581975,
-            "heading": -2.5990641066080484,
-            "angularVelocity": -0.0000010100276450499987,
-            "velocityX": 3.3837898629208136,
-            "velocityY": -1.69133123170322,
-            "timestamp": 0.5583915568910853
-        },
-        {
-            "x": 2.0463595988203984,
-            "y": 6.046998807454145,
-            "heading": -2.5990641536073187,
-            "angularVelocity": -0.0000010100282384452147,
-            "velocityX": 3.3837829621742297,
-            "velocityY": -1.691345037712453,
-            "timestamp": 0.6049241866320091
-        },
-        {
-            "x": 2.2038159116861142,
-            "y": 5.968296061372362,
-            "heading": -2.599064200606589,
-            "angularVelocity": -0.0000010100282469186477,
-            "velocityX": 3.3837828152499863,
-            "velocityY": -1.6913453316510296,
-            "timestamp": 0.6514568163729328
-        },
-        {
-            "x": 2.3612722547062472,
-            "y": 5.889593375618833,
-            "heading": -2.5990642476058565,
-            "angularVelocity": -0.0000010100281851545483,
-            "velocityX": 3.3837834632773647,
-            "velocityY": -1.6913440351789604,
-            "timestamp": 0.6979894461138566
-        },
-        {
-            "x": 2.5187286297414215,
-            "y": 5.8108907539159365,
-            "heading": -2.599064294605121,
-            "angularVelocity": -0.0000010100281210334782,
-            "velocityX": 3.383784151290135,
-            "velocityY": -1.6913426587117795,
-            "timestamp": 0.7445220758547804
-        },
-        {
-            "x": 2.6761850576997652,
-            "y": 5.732188238093708,
-            "heading": -2.599064341604381,
-            "angularVelocity": -0.000001010028016565385,
-            "velocityX": 3.3837852886247464,
-            "velocityY": -1.6913403833048126,
-            "timestamp": 0.7910547055957041
-        },
-        {
-            "x": 2.83364151622281,
-            "y": 5.653485783421201,
-            "heading": -2.5990643886036375,
-            "angularVelocity": -0.0000010100279552603614,
-            "velocityX": 3.3837859454691737,
-            "velocityY": -1.6913390691793195,
-            "timestamp": 0.8375873353366279
-        },
-        {
-            "x": 2.9910977149954676,
-            "y": 5.574782809080547,
-            "heading": -2.5990644356029167,
-            "angularVelocity": -0.000001010028434200447,
-            "velocityX": 3.3837803633561943,
-            "velocityY": -1.691350237002125,
-            "timestamp": 0.8841199650775516
-        },
-        {
-            "x": 3.1485530895378893,
-            "y": 5.496078185774728,
-            "heading": -2.599064482602267,
-            "angularVelocity": -0.0000010100299667197812,
-            "velocityX": 3.3837626504042433,
-            "velocityY": -1.6913856737520705,
-            "timestamp": 0.9306525948184754
-        },
-        {
-            "x": 3.3060077005580313,
-            "y": 5.417372034978031,
-            "heading": -2.5990645296016837,
-            "angularVelocity": -0.0000010100313828443218,
-            "velocityX": 3.383746242085582,
-            "velocityY": -1.6914184999808708,
-            "timestamp": 0.9771852245593992
-        },
-        {
-            "x": 3.463466207634218,
-            "y": 5.3386736786731355,
-            "heading": -2.5990645766007607,
-            "angularVelocity": -0.0000010100240886605546,
-            "velocityX": 3.383829969488029,
-            "velocityY": -1.6912509940353067,
-            "timestamp": 1.023717854300323
-        },
-        {
-            "x": 3.620938146457774,
-            "y": 5.260002202075332,
-            "heading": -2.5990646235986756,
-            "angularVelocity": -0.0000010099991153726974,
-            "velocityX": 3.3841186217193853,
-            "velocityY": -1.690673341164269,
-            "timestamp": 1.0702504840412468
-        },
-        {
-            "x": 3.7783857783864416,
-            "y": 5.181282090499446,
-            "heading": -2.5990646705986817,
-            "angularVelocity": -0.0000010100440573860445,
-            "velocityX": 3.383596259333376,
-            "velocityY": -1.691718521279299,
-            "timestamp": 1.1167831137821707
-        },
-        {
-            "x": 3.9357445242921885,
-            "y": 5.102384448798495,
-            "heading": -2.599064717606569,
-            "angularVelocity": -0.0000010102134222869035,
-            "velocityX": 3.381686072364613,
-            "velocityY": -1.6955336962675904,
-            "timestamp": 1.1633157435230945
+            "x": 0.7256414720586181,
+            "y": 6.712155337793633,
+            "heading": 1.0237015987984905,
+            "angularVelocity": -0.3502584296953155,
+            "velocityX": 0.33614078912617174,
+            "velocityY": -0.16858457660462928,
+            "timestamp": 0.046532502346056014
+        },
+        {
+            "x": 0.756979104040484,
+            "y": 6.696435827024302,
+            "heading": 0.9920873730999193,
+            "angularVelocity": -0.6794009370802031,
+            "velocityX": 0.6734568398839507,
+            "velocityY": -0.33781786873231273,
+            "timestamp": 0.09306500469211203
+        },
+        {
+            "x": 0.8040789517219918,
+            "y": 6.672807532790365,
+            "heading": 0.9464055818688567,
+            "angularVelocity": -0.9817179160357762,
+            "velocityX": 1.0121924527341073,
+            "velocityY": -0.5077804339471543,
+            "timestamp": 0.13959750703816803
+        },
+        {
+            "x": 0.8670206146686451,
+            "y": 6.641234227942618,
+            "heading": 0.8882606987188267,
+            "angularVelocity": -1.2495541872573583,
+            "velocityX": 1.352638688514273,
+            "velocityY": -0.6785215334633973,
+            "timestamp": 0.18613000938422405
+        },
+        {
+            "x": 0.9458974559747272,
+            "y": 6.601677384330224,
+            "heading": 0.8197602775960398,
+            "angularVelocity": -1.4720983757420398,
+            "velocityX": 1.6950913303456163,
+            "velocityY": -0.8500906166232347,
+            "timestamp": 0.23266251173028007
+        },
+        {
+            "x": 1.04081251945038,
+            "y": 6.554094078416389,
+            "heading": 0.743830819371293,
+            "angularVelocity": -1.6317510212566968,
+            "velocityX": 2.039758420249741,
+            "velocityY": -1.0225821418316814,
+            "timestamp": 0.27919501407633607
+        },
+        {
+            "x": 1.15185810993152,
+            "y": 6.498440545281296,
+            "heading": 0.6650543509456113,
+            "angularVelocity": -1.6929342815026889,
+            "velocityX": 2.3864091738569346,
+            "velocityY": -1.1960141907087913,
+            "timestamp": 0.32572751642239206
+        },
+        {
+            "x": 1.2789797685079123,
+            "y": 6.43473905676892,
+            "heading": 0.5921954143923897,
+            "angularVelocity": -1.5657644201343583,
+            "velocityX": 2.7318895861437724,
+            "velocityY": -1.3689676097497174,
+            "timestamp": 0.37226001876844805
+        },
+        {
+            "x": 1.420223914518488,
+            "y": 6.363052522149474,
+            "heading": 0.5479689699773799,
+            "angularVelocity": -0.9504419961367557,
+            "velocityX": 3.035386856270174,
+            "velocityY": -1.5405690862335149,
+            "timestamp": 0.41879252111450405
+        },
+        {
+            "x": 1.5740028534513755,
+            "y": 6.283114192872816,
+            "heading": 0.5411402298477218,
+            "angularVelocity": -0.14675205040326236,
+            "velocityX": 3.304764007515849,
+            "velocityY": -1.717903083787275,
+            "timestamp": 0.46532502346056004
+        },
+        {
+            "x": 1.7314538549626566,
+            "y": 6.20440189931456,
+            "heading": 0.5411401828527235,
+            "angularVelocity": -0.0000010099392058430412,
+            "velocityX": 3.383677936345371,
+            "velocityY": -1.6915551408103213,
+            "timestamp": 0.511857525806616
+        },
+        {
+            "x": 1.888910012901083,
+            "y": 6.125699921176338,
+            "heading": 0.5411401358667378,
+            "angularVelocity": -0.0000010097455172737006,
+            "velocityX": 3.383788749796141,
+            "velocityY": -1.6913334587700644,
+            "timestamp": 0.558390028152672
+        },
+        {
+            "x": 2.0463658388325006,
+            "y": 6.046997278808062,
+            "heading": 0.5411400888807237,
+            "angularVelocity": -0.0000010097461271838877,
+            "velocityX": 3.3837816148475093,
+            "velocityY": -1.691347733310028,
+            "timestamp": 0.604922530498728
+        },
+        {
+            "x": 2.2038216598445532,
+            "y": 5.9682946265981345,
+            "heading": 0.5411400418947097,
+            "angularVelocity": -0.0000010097461301639238,
+            "velocityX": 3.3837815091285663,
+            "velocityY": -1.6913479448107236,
+            "timestamp": 0.651455032844784
+        },
+        {
+            "x": 2.3612775122085363,
+            "y": 5.8895920371121315,
+            "heading": 0.5411399949086985,
+            "angularVelocity": -0.0000010097460671631127,
+            "velocityX": 3.3837821828926944,
+            "velocityY": -1.6913465968512624,
+            "timestamp": 0.69798753519084
+        },
+        {
+            "x": 2.5187333978232895,
+            "y": 5.810889514148883,
+            "heading": 0.5411399479226903,
+            "angularVelocity": -0.0000010097460006294838,
+            "velocityX": 3.383782897463604,
+            "velocityY": -1.6913451672535464,
+            "timestamp": 0.744520037536896
+        },
+        {
+            "x": 2.676189337500118,
+            "y": 5.732187099344657,
+            "heading": 0.541139900936687,
+            "angularVelocity": -0.0000010097458968738495,
+            "velocityX": 3.3837840592768114,
+            "velocityY": -1.6913428428777157,
+            "timestamp": 0.791052539882952
+        },
+        {
+            "x": 2.833645305906935,
+            "y": 5.653484742019454,
+            "heading": 0.5411398539506863,
+            "angularVelocity": -0.0000010097458381135432,
+            "velocityX": 3.3837846766944186,
+            "velocityY": -1.6913416076332204,
+            "timestamp": 0.837585042229008
+        },
+        {
+            "x": 2.991101010217502,
+            "y": 5.57478185633263,
+            "heading": 0.5411398069646632,
+            "angularVelocity": -0.0000010097463224464638,
+            "velocityX": 3.383779001172013,
+            "velocityY": -1.6913529623124643,
+            "timestamp": 0.884117544575064
+        },
+        {
+            "x": 3.14855589978306,
+            "y": 5.496077340659936,
+            "heading": 0.5411397599785701,
+            "angularVelocity": -0.0000010097478275290572,
+            "velocityX": 3.3837614920124643,
+            "velocityY": -1.6913879912875969,
+            "timestamp": 0.93065004692112
+        },
+        {
+            "x": 3.306010013541662,
+            "y": 5.417371272923184,
+            "heading": 0.5411397129924104,
+            "angularVelocity": -0.0000010097492581811278,
+            "velocityX": 3.3837448196468105,
+            "velocityY": -1.6914213456927205,
+            "timestamp": 0.977182549267176
+        },
+        {
+            "x": 3.4634680562159517,
+            "y": 5.338673065406879,
+            "heading": 0.5411396660065909,
+            "angularVelocity": -0.0000010097419454671228,
+            "velocityX": 3.383829253443873,
+            "velocityY": -1.6912524267565827,
+            "timestamp": 1.023715051613232
+        },
+        {
+            "x": 3.6209395624979943,
+            "y": 5.2600018013447745,
+            "heading": 0.5411396190219294,
+            "angularVelocity": -0.0000010097170609305348,
+            "velocityX": 3.3841185911564384,
+            "velocityY": -1.690673402356174,
+            "timestamp": 1.0702475539592882
+        },
+        {
+            "x": 3.778386684395729,
+            "y": 5.181281747313384,
+            "heading": 0.5411395720351826,
+            "angularVelocity": -0.0000010097618775946008,
+            "velocityX": 3.3835945620651766,
+            "velocityY": -1.6917219161486032,
+            "timestamp": 1.1167800563053443
+        },
+        {
+            "x": 3.9357449151310644,
+            "y": 5.10238415335025,
+            "heading": 0.5411395250405994,
+            "angularVelocity": -0.000001009930280464059,
+            "velocityX": 3.3816842594175633,
+            "velocityY": -1.6955373123309678,
+            "timestamp": 1.1633125586514004
         },
         {
             "x": 4.092995643615723,
             "y": 5.023272514343262,
-            "heading": -2.5990647646424785,
-            "angularVelocity": -0.000001010815630420508,
-            "velocityX": 3.379373145230901,
-            "velocityY": -1.700138911036495,
-            "timestamp": 1.2098483732640184
-        },
-        {
-            "x": 4.261229917640389,
-            "y": 4.916068911394467,
-            "heading": -2.599064809945939,
-            "angularVelocity": -8.591015508014648e-7,
-            "velocityX": 3.1902711911232386,
-            "velocityY": -2.0329303767326627,
-            "timestamp": 1.2625819071694577
-        },
-        {
-            "x": 4.429297438166161,
-            "y": 4.8086040706119935,
-            "heading": -2.5990648552466906,
-            "angularVelocity": -8.590501714319945e-7,
-            "velocityX": 3.187109000265223,
-            "velocityY": -2.0378842991085575,
-            "timestamp": 1.315315441074897
-        },
-        {
-            "x": 4.597219271125054,
-            "y": 4.700911724294517,
-            "heading": -2.599064900558894,
-            "angularVelocity": -8.592673362873697e-7,
-            "velocityX": 3.184346288265075,
-            "velocityY": -2.0421985469549115,
-            "timestamp": 1.3680489749803364
-        },
-        {
-            "x": 4.765178923625143,
-            "y": 4.59327837178587,
-            "heading": -2.5990649458681174,
-            "angularVelocity": -8.592108353148986e-7,
-            "velocityX": 3.1850634702628065,
-            "velocityY": -2.0410798317000203,
-            "timestamp": 1.4207825088857757
-        },
-        {
-            "x": 4.93313403793343,
-            "y": 4.485637937706628,
-            "heading": -2.599064991177699,
-            "angularVelocity": -8.592176153657431e-7,
-            "velocityX": 3.184977411326241,
-            "velocityY": -2.041214121404157,
-            "timestamp": 1.473516042791215
-        },
-        {
-            "x": 5.101087827105617,
-            "y": 4.377995436130629,
-            "heading": -2.599065036487385,
-            "angularVelocity": -8.592196078701955e-7,
-            "velocityX": 3.184952282419374,
-            "velocityY": -2.041253327892839,
-            "timestamp": 1.5262495766966544
-        },
-        {
-            "x": 5.269016802174646,
-            "y": 4.270314227125004,
-            "heading": -2.599065081799025,
-            "angularVelocity": -8.592566538542498e-7,
-            "velocityX": 3.1844817259951634,
-            "velocityY": -2.041987347154346,
-            "timestamp": 1.5789831106020937
-        },
-        {
-            "x": 5.437055950944385,
-            "y": 4.162805027649924,
-            "heading": -2.599065127101977,
-            "angularVelocity": -8.590919076920599e-7,
-            "velocityX": 3.1865709791255945,
-            "velocityY": -2.038725484769403,
-            "timestamp": 1.631716644507533
-        },
-        {
-            "x": 5.605213465892237,
-            "y": 4.055481061993006,
-            "heading": -2.599065172406386,
-            "angularVelocity": -8.591195260284143e-7,
-            "velocityX": 3.188815588376687,
-            "velocityY": -2.035212846712725,
-            "timestamp": 1.6844501784129724
+            "heading": 0.5411394780180799,
+            "angularVelocity": -0.0000010105306414990813,
+            "velocityX": 3.379373997880459,
+            "velocityY": -1.7001372163227824,
+            "timestamp": 1.2098450609974565
+        },
+        {
+            "x": 4.2612300154414715,
+            "y": 4.916069066513277,
+            "heading": 0.541139432730096,
+            "angularVelocity": -8.588080638060829e-7,
+            "velocityX": 3.1902730598822937,
+            "velocityY": -2.0329274441782497,
+            "timestamp": 1.2625785946693153
+        },
+        {
+            "x": 4.429297905294805,
+            "y": 4.808604804974877,
+            "heading": 0.5411393874448718,
+            "angularVelocity": -8.58755730533247e-7,
+            "velocityX": 3.1871160180389406,
+            "velocityY": -2.0378733237761697,
+            "timestamp": 1.3153121283411742
+        },
+        {
+            "x": 4.5972191269209945,
+            "y": 4.700911507061467,
+            "heading": 0.5411393421481548,
+            "angularVelocity": -8.589736709635531e-7,
+            "velocityX": 3.1843347095055985,
+            "velocityY": -2.042216601367096,
+            "timestamp": 1.368045662013033
+        },
+        {
+            "x": 4.765178956046694,
+            "y": 4.593278431809082,
+            "heading": 0.5411392968544698,
+            "angularVelocity": -8.589161734970966e-7,
+            "velocityX": 3.185066833769243,
+            "velocityY": -2.0410745830567376,
+            "timestamp": 1.4207791956848919
+        },
+        {
+            "x": 4.9331339491528,
+            "y": 4.485637810244685,
+            "heading": 0.5411392515604048,
+            "angularVelocity": -8.58923380695705e-7,
+            "velocityX": 3.1849751270448214,
+            "velocityY": -2.041217685776372,
+            "timestamp": 1.4735127293567507
+        },
+        {
+            "x": 5.1010877481253685,
+            "y": 4.377995325600118,
+            "heading": 0.5411392062662455,
+            "angularVelocity": -8.589251678490757e-7,
+            "velocityX": 3.1849524823742743,
+            "velocityY": -2.041253015859115,
+            "timestamp": 1.5262462630286096
+        },
+        {
+            "x": 5.269016703536996,
+            "y": 4.270314087571009,
+            "heading": 0.5411391609701367,
+            "angularVelocity": -8.589621362197366e-7,
+            "velocityX": 3.184481367332135,
+            "velocityY": -2.0419879065793034,
+            "timestamp": 1.5789797967004684
+        },
+        {
+            "x": 5.437056059940434,
+            "y": 4.162805214274757,
+            "heading": 0.5411391156827058,
+            "angularVelocity": -8.587975760545075e-7,
+            "velocityX": 3.18657493065327,
+            "velocityY": -2.038719308384206,
+            "timestamp": 1.6317133303723272
+        },
+        {
+            "x": 5.605213513282879,
+            "y": 4.0554811537322735,
+            "heading": 0.5411390703937885,
+            "angularVelocity": -8.588257618591655e-7,
+            "velocityX": 3.188814434261697,
+            "velocityY": -2.03521465506778,
+            "timestamp": 1.684446864044186
         },
         {
             "x": 5.7832255363464355,
             "y": 3.9654417037963863,
-            "heading": -2.599065217938776,
-            "angularVelocity": -8.634427952488677e-7,
-            "velocityX": 3.3756901400427615,
-            "velocityY": -1.707440247757134,
-            "timestamp": 1.7371837123184117
-        },
-        {
-            "x": 5.945200300467497,
-            "y": 3.903126168683373,
-            "heading": -2.599065262330506,
-            "angularVelocity": -9.676334890696444e-7,
-            "velocityX": 3.5306622744884395,
-            "velocityY": -1.3583295529523924,
-            "timestamp": 1.7830603075655527
-        },
-        {
-            "x": 6.112434090508488,
-            "y": 3.856737486793876,
-            "heading": -2.5990653054733555,
-            "angularVelocity": -9.404108896808373e-7,
-            "velocityX": 3.6452964554166534,
-            "velocityY": -1.01116226345067,
-            "timestamp": 1.8289369028126936
-        },
-        {
-            "x": 6.280097327658818,
-            "y": 3.8119257778274935,
-            "heading": -2.599065348543951,
-            "angularVelocity": -9.38835914358075e-7,
-            "velocityX": 3.6546573747925137,
-            "velocityY": -0.9767880272056341,
-            "timestamp": 1.8748134980598345
-        },
-        {
-            "x": 6.444605868501446,
-            "y": 3.756644766145867,
-            "heading": -2.599065392143275,
-            "angularVelocity": -9.503609402316767e-7,
-            "velocityX": 3.5858925440393525,
-            "velocityY": -1.2049937747953314,
-            "timestamp": 1.9206900933069755
-        },
-        {
-            "x": 6.602719051918594,
-            "y": 3.685095684621415,
-            "heading": -2.5990654382603977,
-            "angularVelocity": -0.0000010052429246959388,
-            "velocityX": 3.446489055375297,
-            "velocityY": -1.5595987701135843,
-            "timestamp": 1.9665666885541164
-        },
-        {
-            "x": 6.752801915063028,
-            "y": 3.597951074081566,
-            "heading": -2.599065489714602,
-            "angularVelocity": -0.0000011215785414644313,
-            "velocityX": 3.2714472888827597,
-            "velocityY": -1.8995439846919482,
-            "timestamp": 2.0124432838012574
-        },
-        {
-            "x": 6.893316861258978,
-            "y": 3.49609530674715,
-            "heading": -2.599065550773928,
-            "angularVelocity": -0.0000013309472018502562,
-            "velocityX": 3.06288959411619,
-            "velocityY": -2.2202120010370177,
-            "timestamp": 2.058319879048398
-        },
-        {
-            "x": 7.022827120260116,
-            "y": 3.380569821579126,
-            "heading": -2.5990656290967817,
-            "angularVelocity": -0.0000017072507862805843,
-            "velocityX": 2.8230137459733338,
-            "velocityY": -2.5181791400535034,
-            "timestamp": 2.104196474295539
+            "heading": 0.5411390248741499,
+            "angularVelocity": -8.632009947472993e-7,
+            "velocityX": 3.3756892563138687,
+            "velocityY": -1.7074419949962474,
+            "timestamp": 1.737180397716045
+        },
+        {
+            "x": 5.9452002659111205,
+            "y": 3.9031260890359523,
+            "heading": 0.54113898049095,
+            "angularVelocity": -9.674475759968245e-7,
+            "velocityX": 3.530661595583403,
+            "velocityY": -1.358331317676469,
+            "timestamp": 1.7830569919972126
+        },
+        {
+            "x": 6.112434032055133,
+            "y": 3.8567373346649623,
+            "heading": 0.5411389373525121,
+            "angularVelocity": -9.403147416321739e-7,
+            "velocityX": 3.6452960112747332,
+            "velocityY": -1.0111638646647567,
+            "timestamp": 1.8289335862783802
+        },
+        {
+            "x": 6.280097272848707,
+            "y": 3.8119256534794217,
+            "heading": 0.5411388942859083,
+            "angularVelocity": -9.387489265836176e-7,
+            "velocityX": 3.65465753115864,
+            "velocityY": -0.9767874422169035,
+            "timestamp": 1.8748101805595478
+        },
+        {
+            "x": 6.4446058180798245,
+            "y": 3.756644666326502,
+            "heading": 0.5411388506931323,
+            "angularVelocity": -9.502182266145116e-7,
+            "velocityX": 3.5858927152021427,
+            "velocityY": -1.204993265500441,
+            "timestamp": 1.9206867748407155
+        },
+        {
+            "x": 6.602719007178768,
+            "y": 3.68509560621824,
+            "heading": 0.5411388045872843,
+            "angularVelocity": -0.0000010049971826881646,
+            "velocityX": 3.4464892517938135,
+            "velocityY": -1.5595983361306547,
+            "timestamp": 1.9665633691218831
+        },
+        {
+            "x": 6.75280187814694,
+            "y": 3.5979510164257396,
+            "heading": 0.5411387531507533,
+            "angularVelocity": -0.000001121193317368561,
+            "velocityX": 3.271447528304838,
+            "velocityY": -1.899543572446005,
+            "timestamp": 2.0124399634030508
+        },
+        {
+            "x": 6.893316834439751,
+            "y": 3.4960952692408354,
+            "heading": 0.5411386921189019,
+            "angularVelocity": -0.0000013303483490867941,
+            "velocityX": 3.0628898786954792,
+            "velocityY": -2.2202116085744548,
+            "timestamp": 2.0583165576842184
+        },
+        {
+            "x": 7.022827105769335,
+            "y": 3.380569803374169,
+            "heading": 0.5411386138405815,
+            "angularVelocity": -0.0000017062801116380342,
+            "velocityX": 2.8230140741451093,
+            "velocityY": -2.5181787723526305,
+            "timestamp": 2.104193151965386
         },
         {
             "x": 7.140008449554443,
             "y": 3.2525558471679688,
-            "heading": -2.5990657325552275,
-            "angularVelocity": -0.000002255146554794306,
-            "velocityX": 2.5542725797993215,
-            "velocityY": -2.790398322315311,
-            "timestamp": 2.1500730695426795
-        },
-        {
-            "x": 7.220411639481594,
-            "y": 3.1500923106303724,
-            "heading": -2.599065847631647,
-            "angularVelocity": -0.000003342401256012092,
-            "velocityX": 2.3353152959116907,
-            "velocityY": -2.9760593375246422,
-            "timestamp": 2.1845023348619734
-        },
-        {
-            "x": 7.292813286750944,
-            "y": 3.0418266490558983,
-            "heading": -2.5990659368308053,
-            "angularVelocity": -0.0000025907947068596104,
-            "velocityX": 2.1029100272080186,
-            "velocityY": -3.1445823943795888,
-            "timestamp": 2.2189316001812673
-        },
-        {
-            "x": 7.356796447286593,
-            "y": 2.9283823501608603,
-            "heading": -2.599066009494182,
-            "angularVelocity": -0.0000021105119622632264,
-            "velocityX": 1.8583945937350983,
-            "velocityY": -3.294996214498541,
-            "timestamp": 2.2533608655005612
-        },
-        {
-            "x": 7.411992663456803,
-            "y": 2.8104127239306367,
-            "heading": -2.5990660711538616,
-            "angularVelocity": -0.0000017909089651975303,
-            "velocityX": 1.603177287064451,
-            "velocityY": -3.426434608353826,
-            "timestamp": 2.287790130819855
-        },
-        {
-            "x": 7.458084080553435,
-            "y": 2.688597137512427,
-            "heading": -2.599066125344508,
-            "angularVelocity": -0.0000015739704607850277,
-            "velocityX": 1.3387278720350797,
-            "velocityY": -3.538140744175163,
-            "timestamp": 2.322219396139149
-        },
-        {
-            "x": 7.494809849686971,
-            "y": 2.5636384472618623,
-            "heading": -2.599066174481706,
-            "angularVelocity": -0.0000014271927566927072,
-            "velocityX": 1.0667020859416116,
-            "velocityY": -3.6294323765468546,
-            "timestamp": 2.356648661458443
-        },
-        {
-            "x": 7.5219737035667364,
-            "y": 2.4362587715435033,
-            "heading": -2.599066220329768,
-            "angularVelocity": -0.0000013316596016825942,
-            "velocityX": 0.7889757050532209,
-            "velocityY": -3.699750039306788,
-            "timestamp": 2.391077926777737
-        },
-        {
-            "x": 7.539529775464085,
-            "y": 2.307203584640024,
-            "heading": -2.5990662642857343,
-            "angularVelocity": -0.0000012767035893272692,
-            "velocityX": 0.5099171223818962,
-            "velocityY": -3.748415358464242,
-            "timestamp": 2.425507192097031
-        },
-        {
-            "x": 7.55111544342237,
-            "y": 2.1774760653156817,
-            "heading": -2.599066307607684,
-            "angularVelocity": -0.0000012582885205698109,
-            "velocityX": 0.3365063950923211,
-            "velocityY": -3.7679432924653384,
-            "timestamp": 2.4599364574163247
-        },
-        {
-            "x": 7.56271571323303,
-            "y": 2.047749850875278,
-            "heading": -2.5990663509314866,
-            "angularVelocity": -0.0000012583423688964546,
-            "velocityX": 0.3369305067394331,
-            "velocityY": -3.767905392035856,
-            "timestamp": 2.4943657227356186
-        },
-        {
-            "x": 7.5743766744470316,
-            "y": 1.9180290778400821,
-            "heading": -2.599066394258993,
-            "angularVelocity": -0.0000012584499309396356,
-            "velocityX": 0.33869329205287857,
-            "velocityY": -3.7677473461074933,
-            "timestamp": 2.5287949880549125
-        },
-        {
-            "x": 7.586111158859003,
-            "y": 1.7883149350094998,
-            "heading": -2.5990664375910546,
-            "angularVelocity": -0.0000012585822270319525,
-            "velocityX": 0.34082877758442714,
-            "velocityY": -3.7675547714315485,
-            "timestamp": 2.5632242533742065
-        },
-        {
-            "x": 7.597929633339714,
-            "y": 1.6586084182080805,
-            "heading": -2.59906648228782,
-            "angularVelocity": -0.0000012982201369267655,
-            "velocityX": 0.3432682739856076,
-            "velocityY": -3.7673332729737243,
-            "timestamp": 2.5976535186935004
-        },
-        {
-            "x": 7.610690036465753,
-            "y": 1.5360359058677395,
-            "heading": -2.6173573406008535,
-            "angularVelocity": -0.5312590362708309,
-            "velocityX": 0.37062664589906397,
-            "velocityY": -3.5601257013070047,
-            "timestamp": 2.6320827840127943
-        },
-        {
-            "x": 7.621968158889347,
-            "y": 1.4215047209508034,
-            "heading": -2.657213150288673,
-            "angularVelocity": -1.1576142946476244,
-            "velocityX": 0.3275737172727147,
-            "velocityY": -3.3265648817881672,
-            "timestamp": 2.666512049332088
-        },
-        {
-            "x": 7.631829723236215,
-            "y": 1.3151734834169941,
-            "heading": -2.7178665355180023,
-            "angularVelocity": -1.76168107762032,
-            "velocityX": 0.28642970610680407,
-            "velocityY": -3.08839693637669,
-            "timestamp": 2.700941314651382
-        },
-        {
-            "x": 7.640728077188491,
-            "y": 1.2186118736781408,
-            "heading": -2.7826314980185702,
-            "angularVelocity": -1.8811020769678346,
-            "velocityX": 0.2584532045561674,
-            "velocityY": -2.8046375327311788,
-            "timestamp": 2.735370579970676
-        },
-        {
-            "x": 7.648685947205252,
-            "y": 1.1318864826516366,
-            "heading": -2.8465688471146957,
-            "angularVelocity": -1.8570639978286094,
-            "velocityX": 0.23113679432217057,
-            "velocityY": -2.518943992043375,
-            "timestamp": 2.76979984528997
-        },
-        {
-            "x": 7.655720752960135,
-            "y": 1.0549563243440254,
-            "heading": -2.9071249866023474,
-            "angularVelocity": -1.7588565694352556,
-            "velocityX": 0.20432633951521018,
-            "velocityY": -2.2344408919030423,
-            "timestamp": 2.804229110609264
-        },
-        {
-            "x": 7.661844928847109,
-            "y": 0.9877692823605362,
-            "heading": -2.962675956286548,
-            "angularVelocity": -1.6134811233710036,
-            "velocityX": 0.17787704239939275,
-            "velocityY": -1.9514515154593546,
-            "timestamp": 2.8386583759285577
-        },
-        {
-            "x": 7.667068746842824,
-            "y": 0.930277282093812,
-            "heading": -3.0120810880108007,
-            "angularVelocity": -1.4349749048108316,
-            "velocityX": 0.1517260954387245,
-            "velocityY": -1.6698584687633535,
-            "timestamp": 2.8730876412478517
-        },
-        {
-            "x": 7.6714018244195765,
-            "y": 0.8824389383291659,
-            "heading": -3.0544961336196454,
-            "angularVelocity": -1.231947449807498,
-            "velocityX": 0.12585448851644476,
-            "velocityY": -1.3894674580185522,
-            "timestamp": 2.9075169065671456
-        },
-        {
-            "x": 7.674853405168496,
-            "y": 0.8442193530437802,
-            "heading": -3.0892755187362573,
-            "angularVelocity": -1.0101692497377945,
-            "velocityX": 0.10025136223247667,
-            "velocityY": -1.1100900623623653,
-            "timestamp": 2.9419461718864395
-        },
-        {
-            "x": 7.677432109437281,
-            "y": 0.8155891364108041,
-            "heading": -3.1159120874216906,
-            "angularVelocity": -0.773660676125647,
-            "velocityX": 0.07489861444537417,
-            "velocityY": -0.8315662959247566,
-            "timestamp": 2.9763754372057334
-        },
-        {
-            "x": 7.679145555938743,
-            "y": 0.7965232635297996,
-            "heading": -3.1339959816499734,
-            "angularVelocity": -0.525247752473809,
-            "velocityX": 0.04976715261198055,
-            "velocityY": -0.5537693791659258,
-            "timestamp": 3.0108047025250273
+            "heading": 0.541138510449912,
+            "angularVelocity": -0.0000022536692406151377,
+            "velocityX": 2.5542729494461978,
+            "velocityY": -2.790397984245173,
+            "timestamp": 2.1500697462465537
+        },
+        {
+            "x": 7.220411568965462,
+            "y": 3.1500924469852043,
+            "heading": 0.5411383954402629,
+            "angularVelocity": -0.0000033404657713126234,
+            "velocityX": 2.3353159519018676,
+            "velocityY": -2.97605882316215,
+            "timestamp": 2.1844989716991257
+        },
+        {
+            "x": 7.292813165778826,
+            "y": 3.0418269330928753,
+            "heading": 0.5411383062644725,
+            "angularVelocity": -0.000002590118983726798,
+            "velocityX": 2.102910996737314,
+            "velocityY": -3.1445817461525896,
+            "timestamp": 2.2189281971516976
+        },
+        {
+            "x": 7.3567962971772936,
+            "y": 2.928382790909777,
+            "heading": 0.5411382336043034,
+            "angularVelocity": -0.000002110421251450144,
+            "velocityX": 1.8583958993386591,
+            "velocityY": -3.2949954781694024,
+            "timestamp": 2.2533574226042696
+        },
+        {
+            "x": 7.411992506551136,
+            "y": 2.810413328004449,
+            "heading": 0.5411381719390687,
+            "angularVelocity": -0.00000179107237982498,
+            "velocityX": 1.603178946034542,
+            "velocityY": -3.426433832147572,
+            "timestamp": 2.2877866480568416
+        },
+        {
+            "x": 7.458083940034777,
+            "y": 2.688597909034967,
+            "heading": 0.5411381177395469,
+            "angularVelocity": -0.0000015742300651441758,
+            "velocityX": 1.3387298981538813,
+            "velocityY": -3.5381399775400135,
+            "timestamp": 2.3222158735094136
+        },
+        {
+            "x": 7.4948097486817264,
+            "y": 2.563639387590104,
+            "heading": 0.5411380685927316,
+            "angularVelocity": -0.0000014274737405214791,
+            "velocityX": 1.0667044687814087,
+            "velocityY": -3.6294316762078314,
+            "timestamp": 2.3566450989619856
+        },
+        {
+            "x": 7.521973664613143,
+            "y": 2.436259879309271,
+            "heading": 0.5411380227355477,
+            "angularVelocity": -0.0000013319260973088958,
+            "velocityX": 0.7889784209297904,
+            "velocityY": -3.6997494601296452,
+            "timestamp": 2.3910743244145576
+        },
+        {
+            "x": 7.539529818670366,
+            "y": 2.3072048557852676,
+            "heading": 0.5411379787715688,
+            "angularVelocity": -0.0000012769377868531526,
+            "velocityX": 0.5099200991725963,
+            "velocityY": -3.748414953504679,
+            "timestamp": 2.4255035498671296
+        },
+        {
+            "x": 7.551115587366525,
+            "y": 2.177477496871536,
+            "heading": 0.5411379354425048,
+            "angularVelocity": -0.0000012584966258013745,
+            "velocityX": 0.336509710684848,
+            "velocityY": -3.7679429963488027,
+            "timestamp": 2.4599327753197016
+        },
+        {
+            "x": 7.56271584344406,
+            "y": 2.0477514326184285,
+            "heading": 0.5411378921115911,
+            "angularVelocity": -0.0000012585503448264627,
+            "velocityX": 0.3369304980023622,
+            "velocityY": -3.76790539281253,
+            "timestamp": 2.4943620007722735
+        },
+        {
+            "x": 7.574376716473137,
+            "y": 1.9180308030778739,
+            "heading": 0.5411378487769694,
+            "angularVelocity": -0.0000012586580504181092,
+            "velocityX": 0.3386911228976962,
+            "velocityY": -3.767747541089928,
+            "timestamp": 2.5287912262248455
+        },
+        {
+            "x": 7.58611108718596,
+            "y": 1.788316801391074,
+            "heading": 0.541137805437783,
+            "angularVelocity": -0.0000012587906282413006,
+            "velocityX": 0.34082586983899804,
+            "velocityY": -3.7675550344719086,
+            "timestamp": 2.5632204516774175
+        },
+        {
+            "x": 7.597929211011114,
+            "y": 1.6586104040760536,
+            "heading": 0.541137760738121,
+            "angularVelocity": -0.0000012983057658513822,
+            "velocityX": 0.3432584866444477,
+            "velocityY": -3.7673341647982834,
+            "timestamp": 2.5976496771299895
+        },
+        {
+            "x": 7.610687440254598,
+            "y": 1.5360368797463901,
+            "heading": 0.5228402741207003,
+            "angularVelocity": -0.5314521711397164,
+            "velocityX": 0.37056393444221536,
+            "velocityY": -3.560159217014913,
+            "timestamp": 2.6320789025825615
+        },
+        {
+            "x": 7.6219652115761445,
+            "y": 1.4215043504075173,
+            "heading": 0.4829676933785713,
+            "angularVelocity": -1.1581027518921772,
+            "velocityX": 0.3275638987899306,
+            "velocityY": -3.3266077825840545,
+            "timestamp": 2.6665081280351335
+        },
+        {
+            "x": 7.631828440236478,
+            "y": 1.3151733520485158,
+            "heading": 0.42230189460591644,
+            "angularVelocity": -1.7620436700286786,
+            "velocityX": 0.2864783779098223,
+            "velocityY": -3.0883935656779764,
+            "timestamp": 2.7009373534877055
+        },
+        {
+            "x": 7.640727843462309,
+            "y": 1.2186121100176637,
+            "heading": 0.3575309788065375,
+            "angularVelocity": -1.8812771692652814,
+            "velocityX": 0.2584839800738238,
+            "velocityY": -2.804630100199854,
+            "timestamp": 2.7353665789402775
+        },
+        {
+            "x": 7.648686345492233,
+            "y": 1.1318869979940107,
+            "heading": 0.2935915741251458,
+            "angularVelocity": -1.8571258528454382,
+            "velocityX": 0.23115541884308086,
+            "velocityY": -2.518938805147378,
+            "timestamp": 2.7697958043928494
+        },
+        {
+            "x": 7.655721476158885,
+            "y": 1.054956995812666,
+            "heading": 0.2330360616068616,
+            "angularVelocity": -1.7588403956899903,
+            "velocityX": 0.20433601320340974,
+            "velocityY": -2.234438944533275,
+            "timestamp": 2.8042250298454214
+        },
+        {
+            "x": 7.661845751852373,
+            "y": 0.9877699914439897,
+            "heading": 0.17748768247912494,
+            "angularVelocity": -1.6134077487241032,
+            "velocityX": 0.17788014725831017,
+            "velocityY": -1.9514526825829648,
+            "timestamp": 2.8386542552979934
+        },
+        {
+            "x": 7.667069516180101,
+            "y": 0.9302779311076093,
+            "heading": 0.1280864585154153,
+            "angularVelocity": -1.4348630651528973,
+            "velocityX": 0.15172471233573487,
+            "velocityY": -1.6698621470755346,
+            "timestamp": 2.8730834807505654
+        },
+        {
+            "x": 7.6714024480246685,
+            "y": 0.88243945985842,
+            "heading": 0.08567598072175592,
+            "angularVelocity": -1.2318162037100373,
+            "velocityX": 0.1258504014428196,
+            "velocityY": -1.3894727697283988,
+            "timestamp": 2.9075127062031374
+        },
+        {
+            "x": 7.674853841393155,
+            "y": 0.8442197139951643,
+            "heading": 0.050901182135293016,
+            "angularVelocity": -1.0100372032583045,
+            "velocityX": 0.10024603583499626,
+            "velocityY": -1.110096011770723,
+            "timestamp": 2.9419419316557094
+        },
+        {
+            "x": 7.6774323573426795,
+            "y": 0.8155893376478138,
+            "heading": 0.02426863930506587,
+            "angularVelocity": -0.7735446406401408,
+            "velocityX": 0.07489323142279661,
+            "velocityY": -0.8315718977410657,
+            "timestamp": 2.9763711571082814
+        },
+        {
+            "x": 7.6791456483845035,
+            "y": 0.7965233362757584,
+            "heading": 0.006187731070844568,
+            "angularVelocity": -0.5251616322048499,
+            "velocityX": 0.04976269489958636,
+            "velocityY": -0.5537737524278505,
+            "timestamp": 3.0108003825608534
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.2669044908275344,
-            "velocityX": 0.024817377116055982,
-            "velocityY": -0.2766037393328496,
-            "timestamp": 3.045233967844321
+            "heading": -0.003,
+            "angularVelocity": -0.26685848868430057,
+            "velocityX": 0.024814720757314154,
+            "velocityY": -0.2766061725343536,
+            "timestamp": 3.0452296080134253
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": -3.143185307179586,
-            "angularVelocity": -4.433819854347959e-31,
+            "heading": -0.003,
+            "angularVelocity": -2.388488097950269e-30,
             "velocityX": 0,
-            "velocityY": 2.2179802932100695e-31,
-            "timestamp": 3.079663233163615
+            "velocityY": 9.563090319474696e-31,
+            "timestamp": 3.0796588334659973
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj
index 9e6c02b6..7714b3ff 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj	
@@ -3,119 +3,119 @@
         {
             "x": 0.71,
             "y": 6.72,
-            "heading": -2.1,
-            "angularVelocity": -8.4685465984255e-32,
-            "velocityX": -1.4242674774019307e-33,
-            "velocityY": -4.454347432487034e-33,
+            "heading": 1.04,
+            "angularVelocity": 6.421381184701714e-32,
+            "velocityX": 9.791473054297313e-34,
+            "velocityY": 6.656933252086786e-33,
             "timestamp": 0
         },
         {
-            "x": 0.7541194456754691,
-            "y": 6.720987374332373,
-            "heading": -2.1237071075059206,
-            "angularVelocity": -0.324046498361879,
-            "velocityX": 0.6030576221600736,
-            "velocityY": 0.013496171766179704,
-            "timestamp": 0.07315958550932472
+            "x": 0.7541195074128881,
+            "y": 6.720987207910964,
+            "heading": 1.0162961446571532,
+            "angularVelocity": -0.3240022105828226,
+            "velocityX": 0.6030587735562275,
+            "velocityY": 0.01349390387478939,
+            "timestamp": 0.0731595482024364
         },
         {
-            "x": 0.8424134938705199,
-            "y": 6.722973032705717,
-            "heading": -2.1697540257675185,
-            "angularVelocity": -0.6294037608468737,
-            "velocityX": 1.2068691693694522,
-            "velocityY": 0.027141465599130124,
-            "timestamp": 0.14631917101864944
+            "x": 0.842413628953284,
+            "y": 6.722972522190638,
+            "heading": 0.9702543638919431,
+            "angularVelocity": -0.6293338586206134,
+            "velocityX": 1.2068707873384146,
+            "velocityY": 0.027136776107259192,
+            "timestamp": 0.1463190964048728
         },
         {
-            "x": 0.9749625148381232,
-            "y": 6.725970459573204,
-            "heading": -2.2360020755513283,
-            "angularVelocity": -0.9055279540276729,
-            "velocityX": 1.811779277381365,
-            "velocityY": 0.040971075035846904,
-            "timestamp": 0.21947875652797416
+            "x": 0.9749626727340511,
+            "y": 6.725969397195605,
+            "heading": 0.9040114408742106,
+            "angularVelocity": -0.9054583392778368,
+            "velocityX": 1.811780513105384,
+            "velocityY": 0.04096355265456321,
+            "timestamp": 0.2194786446073092
         },
         {
-            "x": 1.1518930933890432,
-            "y": 6.729994686272588,
-            "heading": -2.318545216019314,
-            "angularVelocity": -1.1282614560115307,
-            "velocityX": 2.418419641379311,
-            "velocityY": 0.055006144053120966,
-            "timestamp": 0.2926383420372989
+            "x": 1.1518931623979327,
+            "y": 6.729992809371002,
+            "heading": 0.821470963167313,
+            "angularVelocity": -1.1282256347246662,
+            "velocityX": 2.418419659649969,
+            "velocityY": 0.05499503857340178,
+            "timestamp": 0.2926381928097456
         },
         {
-            "x": 1.3734035763726509,
-            "y": 6.735067561069991,
-            "heading": -2.4074942006161044,
-            "angularVelocity": -1.215821330554877,
-            "velocityX": 3.0277711586456673,
-            "velocityY": 0.0693398515327658,
-            "timestamp": 0.3657979275466236
+            "x": 1.3734034405206448,
+            "y": 6.735064554387548,
+            "heading": 0.7325177503671355,
+            "angularVelocity": -1.215879744828927,
+            "velocityX": 3.027769902429938,
+            "velocityY": 0.06932444419316391,
+            "timestamp": 0.36579774101218204
         },
         {
-            "x": 1.6366126950009532,
-            "y": 6.741789773246868,
-            "heading": -2.44063463913809,
-            "angularVelocity": -0.45298833080218254,
-            "velocityX": 3.59773933649109,
-            "velocityY": 0.09188422993442831,
-            "timestamp": 0.4389575130559483
+            "x": 1.6366130242279704,
+            "y": 6.741790766846534,
+            "heading": 0.6993734477778637,
+            "angularVelocity": -0.45304137879066375,
+            "velocityX": 3.5977475281693874,
+            "velocityY": 0.09193895566910278,
+            "timestamp": 0.43895728921461846
         },
         {
-            "x": 1.858120407547834,
-            "y": 6.746837523733308,
-            "heading": -2.530120705195818,
-            "angularVelocity": -1.2231625621549276,
-            "velocityX": 3.027733290241823,
-            "velocityY": 0.06899643363609939,
-            "timestamp": 0.512117098565273
+            "x": 1.8581205064725355,
+            "y": 6.746838576232807,
+            "heading": 0.6098795976632514,
+            "angularVelocity": -1.223269584265702,
+            "velocityX": 3.0277316862542305,
+            "velocityY": 0.06899727390750988,
+            "timestamp": 0.5121168374170548
         },
         {
-            "x": 2.0350481983019497,
-            "y": 6.750889776022306,
-            "heading": -2.613093703885261,
-            "angularVelocity": -1.134137080080631,
-            "velocityX": 2.418381535684925,
-            "velocityY": 0.05538921879868258,
-            "timestamp": 0.5852766840745978
+            "x": 2.0350481932432154,
+            "y": 6.750890564225749,
+            "heading": 0.5269052400796844,
+            "angularVelocity": -1.134156232813029,
+            "velocityX": 2.4183813475871814,
+            "velocityY": 0.05538563444554272,
+            "timestamp": 0.5852763856194912
         },
         {
-            "x": 2.1675926697007295,
-            "y": 6.753939244710426,
-            "heading": -2.6797699231631307,
-            "angularVelocity": -0.9113804953059623,
-            "velocityX": 1.8117170904677296,
-            "velocityY": 0.041682421611463606,
-            "timestamp": 0.6584362695839225
+            "x": 2.167592642728435,
+            "y": 6.753939698452488,
+            "heading": 0.4602292040948866,
+            "angularVelocity": -0.9113784546659414,
+            "velocityX": 1.8117177148013692,
+            "velocityY": 0.04167787119598535,
+            "timestamp": 0.6584359338219277
         },
         {
-            "x": 2.255882529656213,
-            "y": 6.7559780839356005,
-            "heading": -2.7261390096327216,
-            "angularVelocity": -0.6338073971683945,
-            "velocityX": 1.2068119213746804,
-            "velocityY": 0.027868381306137554,
-            "timestamp": 0.7315958550932473
+            "x": 2.255882514523324,
+            "y": 6.755978249842539,
+            "heading": 0.41386063175705684,
+            "angularVelocity": -0.633800692829844,
+            "velocityX": 1.2068126986047898,
+            "velocityY": 0.02786446116929368,
+            "timestamp": 0.7315954820243641
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": -0.32614988454563837,
-            "velocityX": 0.603030621847418,
-            "velocityY": 0.013968314025900756,
-            "timestamp": 0.8047554406025721
+            "heading": 0.39,
+            "angularVelocity": -0.3261451491065467,
+            "velocityX": 0.6030311362038534,
+            "velocityY": 0.013966053407445108,
+            "timestamp": 0.8047550302268005
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": -2.091519469848374e-32,
-            "velocityX": -8.675585045811285e-34,
-            "velocityY": 3.70068025566541e-32,
-            "timestamp": 0.8779150261118969
+            "heading": 0.39,
+            "angularVelocity": -1.0168243354874858e-31,
+            "velocityX": -4.086489815881427e-33,
+            "velocityY": 0,
+            "timestamp": 0.8779145784292369
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.traj
index 9e6c02b6..7714b3ff 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.traj	
@@ -3,119 +3,119 @@
         {
             "x": 0.71,
             "y": 6.72,
-            "heading": -2.1,
-            "angularVelocity": -8.4685465984255e-32,
-            "velocityX": -1.4242674774019307e-33,
-            "velocityY": -4.454347432487034e-33,
+            "heading": 1.04,
+            "angularVelocity": 6.421381184701714e-32,
+            "velocityX": 9.791473054297313e-34,
+            "velocityY": 6.656933252086786e-33,
             "timestamp": 0
         },
         {
-            "x": 0.7541194456754691,
-            "y": 6.720987374332373,
-            "heading": -2.1237071075059206,
-            "angularVelocity": -0.324046498361879,
-            "velocityX": 0.6030576221600736,
-            "velocityY": 0.013496171766179704,
-            "timestamp": 0.07315958550932472
+            "x": 0.7541195074128881,
+            "y": 6.720987207910964,
+            "heading": 1.0162961446571532,
+            "angularVelocity": -0.3240022105828226,
+            "velocityX": 0.6030587735562275,
+            "velocityY": 0.01349390387478939,
+            "timestamp": 0.0731595482024364
         },
         {
-            "x": 0.8424134938705199,
-            "y": 6.722973032705717,
-            "heading": -2.1697540257675185,
-            "angularVelocity": -0.6294037608468737,
-            "velocityX": 1.2068691693694522,
-            "velocityY": 0.027141465599130124,
-            "timestamp": 0.14631917101864944
+            "x": 0.842413628953284,
+            "y": 6.722972522190638,
+            "heading": 0.9702543638919431,
+            "angularVelocity": -0.6293338586206134,
+            "velocityX": 1.2068707873384146,
+            "velocityY": 0.027136776107259192,
+            "timestamp": 0.1463190964048728
         },
         {
-            "x": 0.9749625148381232,
-            "y": 6.725970459573204,
-            "heading": -2.2360020755513283,
-            "angularVelocity": -0.9055279540276729,
-            "velocityX": 1.811779277381365,
-            "velocityY": 0.040971075035846904,
-            "timestamp": 0.21947875652797416
+            "x": 0.9749626727340511,
+            "y": 6.725969397195605,
+            "heading": 0.9040114408742106,
+            "angularVelocity": -0.9054583392778368,
+            "velocityX": 1.811780513105384,
+            "velocityY": 0.04096355265456321,
+            "timestamp": 0.2194786446073092
         },
         {
-            "x": 1.1518930933890432,
-            "y": 6.729994686272588,
-            "heading": -2.318545216019314,
-            "angularVelocity": -1.1282614560115307,
-            "velocityX": 2.418419641379311,
-            "velocityY": 0.055006144053120966,
-            "timestamp": 0.2926383420372989
+            "x": 1.1518931623979327,
+            "y": 6.729992809371002,
+            "heading": 0.821470963167313,
+            "angularVelocity": -1.1282256347246662,
+            "velocityX": 2.418419659649969,
+            "velocityY": 0.05499503857340178,
+            "timestamp": 0.2926381928097456
         },
         {
-            "x": 1.3734035763726509,
-            "y": 6.735067561069991,
-            "heading": -2.4074942006161044,
-            "angularVelocity": -1.215821330554877,
-            "velocityX": 3.0277711586456673,
-            "velocityY": 0.0693398515327658,
-            "timestamp": 0.3657979275466236
+            "x": 1.3734034405206448,
+            "y": 6.735064554387548,
+            "heading": 0.7325177503671355,
+            "angularVelocity": -1.215879744828927,
+            "velocityX": 3.027769902429938,
+            "velocityY": 0.06932444419316391,
+            "timestamp": 0.36579774101218204
         },
         {
-            "x": 1.6366126950009532,
-            "y": 6.741789773246868,
-            "heading": -2.44063463913809,
-            "angularVelocity": -0.45298833080218254,
-            "velocityX": 3.59773933649109,
-            "velocityY": 0.09188422993442831,
-            "timestamp": 0.4389575130559483
+            "x": 1.6366130242279704,
+            "y": 6.741790766846534,
+            "heading": 0.6993734477778637,
+            "angularVelocity": -0.45304137879066375,
+            "velocityX": 3.5977475281693874,
+            "velocityY": 0.09193895566910278,
+            "timestamp": 0.43895728921461846
         },
         {
-            "x": 1.858120407547834,
-            "y": 6.746837523733308,
-            "heading": -2.530120705195818,
-            "angularVelocity": -1.2231625621549276,
-            "velocityX": 3.027733290241823,
-            "velocityY": 0.06899643363609939,
-            "timestamp": 0.512117098565273
+            "x": 1.8581205064725355,
+            "y": 6.746838576232807,
+            "heading": 0.6098795976632514,
+            "angularVelocity": -1.223269584265702,
+            "velocityX": 3.0277316862542305,
+            "velocityY": 0.06899727390750988,
+            "timestamp": 0.5121168374170548
         },
         {
-            "x": 2.0350481983019497,
-            "y": 6.750889776022306,
-            "heading": -2.613093703885261,
-            "angularVelocity": -1.134137080080631,
-            "velocityX": 2.418381535684925,
-            "velocityY": 0.05538921879868258,
-            "timestamp": 0.5852766840745978
+            "x": 2.0350481932432154,
+            "y": 6.750890564225749,
+            "heading": 0.5269052400796844,
+            "angularVelocity": -1.134156232813029,
+            "velocityX": 2.4183813475871814,
+            "velocityY": 0.05538563444554272,
+            "timestamp": 0.5852763856194912
         },
         {
-            "x": 2.1675926697007295,
-            "y": 6.753939244710426,
-            "heading": -2.6797699231631307,
-            "angularVelocity": -0.9113804953059623,
-            "velocityX": 1.8117170904677296,
-            "velocityY": 0.041682421611463606,
-            "timestamp": 0.6584362695839225
+            "x": 2.167592642728435,
+            "y": 6.753939698452488,
+            "heading": 0.4602292040948866,
+            "angularVelocity": -0.9113784546659414,
+            "velocityX": 1.8117177148013692,
+            "velocityY": 0.04167787119598535,
+            "timestamp": 0.6584359338219277
         },
         {
-            "x": 2.255882529656213,
-            "y": 6.7559780839356005,
-            "heading": -2.7261390096327216,
-            "angularVelocity": -0.6338073971683945,
-            "velocityX": 1.2068119213746804,
-            "velocityY": 0.027868381306137554,
-            "timestamp": 0.7315958550932473
+            "x": 2.255882514523324,
+            "y": 6.755978249842539,
+            "heading": 0.41386063175705684,
+            "angularVelocity": -0.633800692829844,
+            "velocityX": 1.2068126986047898,
+            "velocityY": 0.02786446116929368,
+            "timestamp": 0.7315954820243641
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": -0.32614988454563837,
-            "velocityX": 0.603030621847418,
-            "velocityY": 0.013968314025900756,
-            "timestamp": 0.8047554406025721
+            "heading": 0.39,
+            "angularVelocity": -0.3261451491065467,
+            "velocityX": 0.6030311362038534,
+            "velocityY": 0.013966053407445108,
+            "timestamp": 0.8047550302268005
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": -2.091519469848374e-32,
-            "velocityX": -8.675585045811285e-34,
-            "velocityY": 3.70068025566541e-32,
-            "timestamp": 0.8779150261118969
+            "heading": 0.39,
+            "angularVelocity": -1.0168243354874858e-31,
+            "velocityX": -4.086489815881427e-33,
+            "velocityY": 0,
+            "timestamp": 0.8779145784292369
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.1.traj
index 66f9741c..886a091a 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.1.traj	
@@ -3,146 +3,146 @@
         {
             "x": 0.71,
             "y": 6.72,
-            "heading": -2.1,
-            "angularVelocity": -1.5217589356437866e-30,
-            "velocityX": -1.7239506448843928e-31,
-            "velocityY": 1.9880283347693254e-31,
+            "heading": 1.04,
+            "angularVelocity": 1.094777100362113e-30,
+            "velocityX": 3.4815734426203924e-31,
+            "velocityY": -2.2714927632651006e-31,
             "timestamp": 0
         },
         {
-            "x": 0.737435952483546,
-            "y": 6.69951466757637,
-            "heading": -2.132336617905336,
-            "angularVelocity": -0.4965864735710579,
-            "velocityX": 0.4213280106395778,
-            "velocityY": -0.3145888360360173,
-            "timestamp": 0.06511779846276568
+            "x": 0.7374353381803705,
+            "y": 6.699514089758397,
+            "heading": 1.0076617651006685,
+            "angularVelocity": -0.49661202140938077,
+            "velocityX": 0.42131918437160226,
+            "velocityY": -0.3145981630464246,
+            "timestamp": 0.06511770457661528
         },
         {
-            "x": 0.7924253707255817,
-            "y": 6.658399914346492,
-            "heading": -2.1942155291625705,
-            "angularVelocity": -0.9502611070707117,
-            "velocityX": 0.8444606473217692,
-            "velocityY": -0.6313904063170618,
-            "timestamp": 0.13023559692553136
+            "x": 0.7924238459364756,
+            "y": 6.658398285396173,
+            "heading": 0.9457835104036431,
+            "angularVelocity": -0.9502523944808582,
+            "velocityX": 0.8444478827015074,
+            "velocityY": -0.6314074586865741,
+            "timestamp": 0.13023540915323056
         },
         {
-            "x": 0.8751335704497958,
-            "y": 6.596485085823342,
-            "heading": -2.281554213555253,
-            "angularVelocity": -1.3412413572707518,
-            "velocityX": 1.2701320019519164,
-            "velocityY": -0.950812680784239,
-            "timestamp": 0.19535339538829705
+            "x": 0.8751312050328514,
+            "y": 6.596482140589978,
+            "heading": 0.858451871788505,
+            "angularVelocity": -1.3411350904174961,
+            "velocityX": 1.270120923858195,
+            "velocityY": -0.9508342655621027,
+            "timestamp": 0.19535311372984585
         },
         {
-            "x": 0.9857847196454437,
-            "y": 6.513575479492505,
-            "heading": -2.388116786100485,
-            "angularVelocity": -1.6364584654403693,
-            "velocityX": 1.6992458560913724,
-            "velocityY": -1.2732249598125673,
-            "timestamp": 0.2604711938510627
+            "x": 0.9857818852205986,
+            "y": 6.513571276882533,
+            "heading": 0.7519056244975073,
+            "angularVelocity": -1.6362101210990878,
+            "velocityX": 1.6992411035858814,
+            "velocityY": -1.2732461048269026,
+            "timestamp": 0.2604708183064611
         },
         {
-            "x": 1.1246179227996527,
-            "y": 6.409453213658469,
-            "heading": -2.503336368742031,
-            "angularVelocity": -1.7694023041554334,
-            "velocityX": 2.132031586319575,
-            "velocityY": -1.5989832010916685,
-            "timestamp": 0.3255889923138284
+            "x": 1.1246151400642461,
+            "y": 6.409448224375173,
+            "heading": 0.63671471411989,
+            "angularVelocity": -1.7689645408506078,
+            "velocityX": 2.1320354540492286,
+            "velocityY": -1.5989975872821531,
+            "timestamp": 0.3255885228830764
         },
         {
-            "x": 1.2913924480076884,
-            "y": 6.28415287204147,
-            "heading": -2.6022121669144127,
-            "angularVelocity": -1.518414327672898,
-            "velocityX": 2.5611204485575056,
-            "velocityY": -1.9242103476309298,
-            "timestamp": 0.39070679077659404
+            "x": 1.2913898302664586,
+            "y": 6.284148790385846,
+            "heading": 0.5378900355869111,
+            "angularVelocity": -1.5176314824903545,
+            "velocityX": 2.5611266749427672,
+            "velocityY": -1.9241991836782961,
+            "timestamp": 0.39070622745969164
         },
         {
-            "x": 1.4799082257130864,
-            "y": 6.139182928400833,
-            "heading": -2.6246781987943484,
-            "angularVelocity": -0.3450060108033547,
-            "velocityX": 2.8949961785515774,
-            "velocityY": -2.2262721876804705,
-            "timestamp": 0.4558245892393597
+            "x": 1.4799020803988825,
+            "y": 6.139172930172668,
+            "heading": 0.5154397477236047,
+            "angularVelocity": -0.3447647304104786,
+            "velocityX": 2.8949461802761682,
+            "velocityY": -2.2263662571613483,
+            "timestamp": 0.4558239320363069
         },
         {
-            "x": 1.6682193277242308,
-            "y": 5.99414498496963,
-            "heading": -2.64727359752809,
-            "angularVelocity": -0.34699266970246656,
-            "velocityX": 2.8918530180165907,
-            "velocityY": -2.2273164458122023,
-            "timestamp": 0.5209423877021254
+            "x": 1.668209892858901,
+            "y": 5.994133297119725,
+            "heading": 0.4928531841519364,
+            "angularVelocity": -0.3468574901176696,
+            "velocityX": 2.891806670464904,
+            "velocityY": -2.2273456043324518,
+            "timestamp": 0.5209416366129221
         },
         {
-            "x": 1.8350272177323106,
-            "y": 5.869345925023006,
-            "heading": -2.74292245957107,
-            "angularVelocity": -1.4688589648446377,
-            "velocityX": 2.5616328246026367,
-            "velocityY": -1.9165122730305941,
-            "timestamp": 0.5860601861648911
+            "x": 1.8350205833080036,
+            "y": 5.869338176931447,
+            "heading": 0.3972353467498499,
+            "angularVelocity": -1.4683846432207315,
+            "velocityX": 2.5616795237743473,
+            "velocityY": -1.9164545341343717,
+            "timestamp": 0.5860593411895374
         },
         {
-            "x": 1.9739579805969827,
-            "y": 5.765588033107393,
-            "heading": -2.8581454024211346,
-            "angularVelocity": -1.7694539061536165,
-            "velocityX": 2.133529789771879,
-            "velocityY": -1.5933875893384517,
-            "timestamp": 0.6511779846276567
+            "x": 1.9739536541090807,
+            "y": 5.765582598648054,
+            "heading": 0.2820330218741829,
+            "angularVelocity": -1.7691398310903903,
+            "velocityX": 2.1335683084100903,
+            "velocityY": -1.5933543566683568,
+            "timestamp": 0.6511770457661527
         },
         {
-            "x": 2.084683421379683,
-            "y": 5.683004349697254,
-            "heading": -2.964498621197243,
-            "angularVelocity": -1.6332434647175749,
-            "velocityX": 1.7003867359860632,
-            "velocityY": -1.2682198317463163,
-            "timestamp": 0.7162957830904224
+            "x": 2.084680900392731,
+            "y": 5.683000749842365,
+            "heading": 0.17568947614699124,
+            "angularVelocity": -1.6330972723719295,
+            "velocityX": 1.7004169143181675,
+            "velocityY": -1.2681934865889841,
+            "timestamp": 0.7162947503427679
         },
         {
-            "x": 2.1674690836499857,
-            "y": 5.621349763098201,
-            "heading": -3.050914836384586,
-            "angularVelocity": -1.327075196449642,
-            "velocityX": 1.2713215775812727,
-            "velocityY": -0.9468162016304505,
-            "timestamp": 0.7814135815531881
+            "x": 2.1674678561101657,
+            "y": 5.621347760780931,
+            "heading": 0.08927495882603552,
+            "angularVelocity": -1.3270510360094632,
+            "velocityX": 1.271343273779417,
+            "velocityY": -0.9467930336656067,
+            "timestamp": 0.7814124549193832
         },
         {
-            "x": 2.22252551206485,
-            "y": 5.580407151857498,
-            "heading": -3.1116606802742157,
-            "angularVelocity": -0.9328608356494681,
-            "velocityX": 0.8454897081071593,
-            "velocityY": -0.6287468588808828,
-            "timestamp": 0.8465313800159537
+            "x": 2.222525102580128,
+            "y": 5.580406412641355,
+            "heading": 0.02852690376362466,
+            "angularVelocity": -0.9328961371931729,
+            "velocityX": 0.8455034898409743,
+            "velocityY": -0.6287283682029188,
+            "timestamp": 0.8465301594959984
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.48411690274504937,
-            "velocityX": 0.42191979126659057,
-            "velocityY": -0.3133882339275879,
-            "timestamp": 0.9116491784787194
+            "heading": -0.003,
+            "angularVelocity": -0.4841525660126885,
+            "velocityX": 0.42192668796465593,
+            "velocityY": -0.3133773337686722,
+            "timestamp": 0.9116478640726137
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": -1.0063297310782738e-31,
-            "velocityX": -3.625667697405941e-32,
-            "velocityY": 0,
-            "timestamp": 0.9767669769414851
+            "heading": -0.003,
+            "angularVelocity": 5.587549482799738e-30,
+            "velocityX": -2.981373275826657e-31,
+            "velocityY": -1.0757932056005479e-30,
+            "timestamp": 0.9767655686492289
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.traj
index 66f9741c..886a091a 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.traj	
@@ -3,146 +3,146 @@
         {
             "x": 0.71,
             "y": 6.72,
-            "heading": -2.1,
-            "angularVelocity": -1.5217589356437866e-30,
-            "velocityX": -1.7239506448843928e-31,
-            "velocityY": 1.9880283347693254e-31,
+            "heading": 1.04,
+            "angularVelocity": 1.094777100362113e-30,
+            "velocityX": 3.4815734426203924e-31,
+            "velocityY": -2.2714927632651006e-31,
             "timestamp": 0
         },
         {
-            "x": 0.737435952483546,
-            "y": 6.69951466757637,
-            "heading": -2.132336617905336,
-            "angularVelocity": -0.4965864735710579,
-            "velocityX": 0.4213280106395778,
-            "velocityY": -0.3145888360360173,
-            "timestamp": 0.06511779846276568
+            "x": 0.7374353381803705,
+            "y": 6.699514089758397,
+            "heading": 1.0076617651006685,
+            "angularVelocity": -0.49661202140938077,
+            "velocityX": 0.42131918437160226,
+            "velocityY": -0.3145981630464246,
+            "timestamp": 0.06511770457661528
         },
         {
-            "x": 0.7924253707255817,
-            "y": 6.658399914346492,
-            "heading": -2.1942155291625705,
-            "angularVelocity": -0.9502611070707117,
-            "velocityX": 0.8444606473217692,
-            "velocityY": -0.6313904063170618,
-            "timestamp": 0.13023559692553136
+            "x": 0.7924238459364756,
+            "y": 6.658398285396173,
+            "heading": 0.9457835104036431,
+            "angularVelocity": -0.9502523944808582,
+            "velocityX": 0.8444478827015074,
+            "velocityY": -0.6314074586865741,
+            "timestamp": 0.13023540915323056
         },
         {
-            "x": 0.8751335704497958,
-            "y": 6.596485085823342,
-            "heading": -2.281554213555253,
-            "angularVelocity": -1.3412413572707518,
-            "velocityX": 1.2701320019519164,
-            "velocityY": -0.950812680784239,
-            "timestamp": 0.19535339538829705
+            "x": 0.8751312050328514,
+            "y": 6.596482140589978,
+            "heading": 0.858451871788505,
+            "angularVelocity": -1.3411350904174961,
+            "velocityX": 1.270120923858195,
+            "velocityY": -0.9508342655621027,
+            "timestamp": 0.19535311372984585
         },
         {
-            "x": 0.9857847196454437,
-            "y": 6.513575479492505,
-            "heading": -2.388116786100485,
-            "angularVelocity": -1.6364584654403693,
-            "velocityX": 1.6992458560913724,
-            "velocityY": -1.2732249598125673,
-            "timestamp": 0.2604711938510627
+            "x": 0.9857818852205986,
+            "y": 6.513571276882533,
+            "heading": 0.7519056244975073,
+            "angularVelocity": -1.6362101210990878,
+            "velocityX": 1.6992411035858814,
+            "velocityY": -1.2732461048269026,
+            "timestamp": 0.2604708183064611
         },
         {
-            "x": 1.1246179227996527,
-            "y": 6.409453213658469,
-            "heading": -2.503336368742031,
-            "angularVelocity": -1.7694023041554334,
-            "velocityX": 2.132031586319575,
-            "velocityY": -1.5989832010916685,
-            "timestamp": 0.3255889923138284
+            "x": 1.1246151400642461,
+            "y": 6.409448224375173,
+            "heading": 0.63671471411989,
+            "angularVelocity": -1.7689645408506078,
+            "velocityX": 2.1320354540492286,
+            "velocityY": -1.5989975872821531,
+            "timestamp": 0.3255885228830764
         },
         {
-            "x": 1.2913924480076884,
-            "y": 6.28415287204147,
-            "heading": -2.6022121669144127,
-            "angularVelocity": -1.518414327672898,
-            "velocityX": 2.5611204485575056,
-            "velocityY": -1.9242103476309298,
-            "timestamp": 0.39070679077659404
+            "x": 1.2913898302664586,
+            "y": 6.284148790385846,
+            "heading": 0.5378900355869111,
+            "angularVelocity": -1.5176314824903545,
+            "velocityX": 2.5611266749427672,
+            "velocityY": -1.9241991836782961,
+            "timestamp": 0.39070622745969164
         },
         {
-            "x": 1.4799082257130864,
-            "y": 6.139182928400833,
-            "heading": -2.6246781987943484,
-            "angularVelocity": -0.3450060108033547,
-            "velocityX": 2.8949961785515774,
-            "velocityY": -2.2262721876804705,
-            "timestamp": 0.4558245892393597
+            "x": 1.4799020803988825,
+            "y": 6.139172930172668,
+            "heading": 0.5154397477236047,
+            "angularVelocity": -0.3447647304104786,
+            "velocityX": 2.8949461802761682,
+            "velocityY": -2.2263662571613483,
+            "timestamp": 0.4558239320363069
         },
         {
-            "x": 1.6682193277242308,
-            "y": 5.99414498496963,
-            "heading": -2.64727359752809,
-            "angularVelocity": -0.34699266970246656,
-            "velocityX": 2.8918530180165907,
-            "velocityY": -2.2273164458122023,
-            "timestamp": 0.5209423877021254
+            "x": 1.668209892858901,
+            "y": 5.994133297119725,
+            "heading": 0.4928531841519364,
+            "angularVelocity": -0.3468574901176696,
+            "velocityX": 2.891806670464904,
+            "velocityY": -2.2273456043324518,
+            "timestamp": 0.5209416366129221
         },
         {
-            "x": 1.8350272177323106,
-            "y": 5.869345925023006,
-            "heading": -2.74292245957107,
-            "angularVelocity": -1.4688589648446377,
-            "velocityX": 2.5616328246026367,
-            "velocityY": -1.9165122730305941,
-            "timestamp": 0.5860601861648911
+            "x": 1.8350205833080036,
+            "y": 5.869338176931447,
+            "heading": 0.3972353467498499,
+            "angularVelocity": -1.4683846432207315,
+            "velocityX": 2.5616795237743473,
+            "velocityY": -1.9164545341343717,
+            "timestamp": 0.5860593411895374
         },
         {
-            "x": 1.9739579805969827,
-            "y": 5.765588033107393,
-            "heading": -2.8581454024211346,
-            "angularVelocity": -1.7694539061536165,
-            "velocityX": 2.133529789771879,
-            "velocityY": -1.5933875893384517,
-            "timestamp": 0.6511779846276567
+            "x": 1.9739536541090807,
+            "y": 5.765582598648054,
+            "heading": 0.2820330218741829,
+            "angularVelocity": -1.7691398310903903,
+            "velocityX": 2.1335683084100903,
+            "velocityY": -1.5933543566683568,
+            "timestamp": 0.6511770457661527
         },
         {
-            "x": 2.084683421379683,
-            "y": 5.683004349697254,
-            "heading": -2.964498621197243,
-            "angularVelocity": -1.6332434647175749,
-            "velocityX": 1.7003867359860632,
-            "velocityY": -1.2682198317463163,
-            "timestamp": 0.7162957830904224
+            "x": 2.084680900392731,
+            "y": 5.683000749842365,
+            "heading": 0.17568947614699124,
+            "angularVelocity": -1.6330972723719295,
+            "velocityX": 1.7004169143181675,
+            "velocityY": -1.2681934865889841,
+            "timestamp": 0.7162947503427679
         },
         {
-            "x": 2.1674690836499857,
-            "y": 5.621349763098201,
-            "heading": -3.050914836384586,
-            "angularVelocity": -1.327075196449642,
-            "velocityX": 1.2713215775812727,
-            "velocityY": -0.9468162016304505,
-            "timestamp": 0.7814135815531881
+            "x": 2.1674678561101657,
+            "y": 5.621347760780931,
+            "heading": 0.08927495882603552,
+            "angularVelocity": -1.3270510360094632,
+            "velocityX": 1.271343273779417,
+            "velocityY": -0.9467930336656067,
+            "timestamp": 0.7814124549193832
         },
         {
-            "x": 2.22252551206485,
-            "y": 5.580407151857498,
-            "heading": -3.1116606802742157,
-            "angularVelocity": -0.9328608356494681,
-            "velocityX": 0.8454897081071593,
-            "velocityY": -0.6287468588808828,
-            "timestamp": 0.8465313800159537
+            "x": 2.222525102580128,
+            "y": 5.580406412641355,
+            "heading": 0.02852690376362466,
+            "angularVelocity": -0.9328961371931729,
+            "velocityX": 0.8455034898409743,
+            "velocityY": -0.6287283682029188,
+            "timestamp": 0.8465301594959984
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.48411690274504937,
-            "velocityX": 0.42191979126659057,
-            "velocityY": -0.3133882339275879,
-            "timestamp": 0.9116491784787194
+            "heading": -0.003,
+            "angularVelocity": -0.4841525660126885,
+            "velocityX": 0.42192668796465593,
+            "velocityY": -0.3133773337686722,
+            "timestamp": 0.9116478640726137
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": -1.0063297310782738e-31,
-            "velocityX": -3.625667697405941e-32,
-            "velocityY": 0,
-            "timestamp": 0.9767669769414851
+            "heading": -0.003,
+            "angularVelocity": 5.587549482799738e-30,
+            "velocityX": -2.981373275826657e-31,
+            "velocityY": -1.0757932056005479e-30,
+            "timestamp": 0.9767655686492289
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.1.traj
index d2e16138..f63af9ed 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.1.traj	
@@ -3,191 +3,191 @@
         {
             "x": 0.71,
             "y": 6.72,
-            "heading": -2.1,
-            "angularVelocity": 2.001911519996272e-31,
-            "velocityX": 6.781624829737293e-33,
-            "velocityY": -4.0943813533671796e-32,
+            "heading": 1.04,
+            "angularVelocity": 1.9442039286246259e-31,
+            "velocityX": 1.5017390020853034e-32,
+            "velocityY": 2.127944688535994e-32,
             "timestamp": 0
         },
         {
-            "x": 0.7269029739475109,
-            "y": 6.6955056066592755,
-            "heading": -2.1380447151842032,
-            "angularVelocity": -0.6185663850269976,
-            "velocityX": 0.27482428085724375,
-            "velocityY": -0.3982526421565261,
-            "timestamp": 0.061504659976863815
-        },
-        {
-            "x": 0.7606767634590716,
-            "y": 6.646279647618601,
-            "heading": -2.2124384537514556,
-            "angularVelocity": -1.2095626346887691,
-            "velocityX": 0.5491257007886129,
-            "velocityY": -0.8003614532490962,
-            "timestamp": 0.12300931995372763
-        },
-        {
-            "x": 0.8112924785066278,
-            "y": 6.571981628339174,
-            "heading": -2.320229893429466,
-            "angularVelocity": -1.752573540257899,
-            "velocityX": 0.8229573997579402,
-            "velocityY": -1.208006341428056,
-            "timestamp": 0.18451397993059143
-        },
-        {
-            "x": 0.8787853408643597,
-            "y": 6.472118688114863,
-            "heading": -2.4555147279599576,
-            "angularVelocity": -2.199586739954034,
-            "velocityX": 1.0973617671103395,
-            "velocityY": -1.6236646176383343,
-            "timestamp": 0.24601863990745526
-        },
-        {
-            "x": 0.9632675519056847,
-            "y": 6.346052032301826,
-            "heading": -2.6067469489099335,
-            "angularVelocity": -2.4588741894819854,
-            "velocityX": 1.37359040880975,
-            "velocityY": -2.049709011649839,
-            "timestamp": 0.3075232998843191
-        },
-        {
-            "x": 1.0643460613698084,
-            "y": 6.193349880124978,
-            "heading": -2.74665413649709,
-            "angularVelocity": -2.274741257650809,
-            "velocityX": 1.643428473584707,
-            "velocityY": -2.482773699330916,
-            "timestamp": 0.3690279598611829
-        },
-        {
-            "x": 1.1777092396516182,
-            "y": 6.018032409257481,
-            "heading": -2.813593357981312,
-            "angularVelocity": -1.0883601585538758,
-            "velocityX": 1.8431640517068764,
-            "velocityY": -2.850474597102823,
-            "timestamp": 0.43053261983804675
-        },
-        {
-            "x": 1.3085884905177934,
-            "y": 5.825664975179578,
-            "heading": -2.813593427712779,
-            "angularVelocity": -0.0000011337590858279882,
-            "velocityX": 2.1279566607702214,
-            "velocityY": -3.1276887661888684,
-            "timestamp": 0.4920372798149106
-        },
-        {
-            "x": 1.4394691966966826,
-            "y": 5.633298531244646,
-            "heading": -2.8135934974374672,
-            "angularVelocity": -0.000001133648866416165,
-            "velocityX": 2.127980322598685,
-            "velocityY": -3.127672667522972,
-            "timestamp": 0.5535419397917744
-        },
-        {
-            "x": 1.5703499198247641,
-            "y": 5.440932098841455,
-            "heading": -2.813593567162125,
-            "angularVelocity": -0.0000011336483749077044,
-            "velocityX": 2.1279805981744278,
-            "velocityY": -3.1276724800292155,
-            "timestamp": 0.6150465997686382
-        },
-        {
-            "x": 1.7012306619349875,
-            "y": 5.248565679353167,
-            "heading": -2.81359363688675,
-            "angularVelocity": -0.0000011336478448323905,
-            "velocityX": 2.1279809068037574,
-            "velocityY": -3.1276722700467015,
-            "timestamp": 0.676551259745502
-        },
-        {
-            "x": 1.8321095906543323,
-            "y": 5.056198026100399,
-            "heading": -2.8135937066202716,
-            "angularVelocity": -0.0000011337924880961462,
-            "velocityX": 2.1279514230072554,
-            "velocityY": -3.127692329737781,
-            "timestamp": 0.7380559197223658
-        },
-        {
-            "x": 1.945432077761735,
-            "y": 4.881276454017442,
-            "heading": -2.879360901618447,
-            "angularVelocity": -1.0693042612204542,
-            "velocityX": 1.8425024567249244,
-            "velocityY": -2.8440377062283995,
-            "timestamp": 0.7995605796992297
-        },
-        {
-            "x": 2.046464590922967,
-            "y": 4.730069111144926,
-            "heading": -3.0091287979589167,
-            "angularVelocity": -2.109887224631182,
-            "velocityX": 1.6426806228867414,
-            "velocityY": -2.458469698546365,
-            "timestamp": 0.8610652396760935
-        },
-        {
-            "x": 2.131137367212944,
-            "y": 4.6049715874663555,
-            "heading": -3.1629399624477488,
-            "angularVelocity": -2.500805053579533,
-            "velocityX": 1.3766887959681082,
-            "velocityY": -2.0339519595039928,
-            "timestamp": 0.9225698996529573
-        },
-        {
-            "x": 2.1987047903626262,
-            "y": 4.5060589736155245,
-            "heading": -3.302229643809037,
-            "angularVelocity": -2.2647012667606727,
-            "velocityX": 1.0985740458544027,
-            "velocityY": -1.6082133270558499,
-            "timestamp": 0.9840745596298212
-        },
-        {
-            "x": 2.2493460059898798,
-            "y": 4.432630316522509,
-            "heading": -3.4143130823982606,
-            "angularVelocity": -1.8223568528203613,
-            "velocityX": 0.8233720119142732,
-            "velocityY": -1.1938714419466272,
-            "timestamp": 1.0455792196066849
-        },
-        {
-            "x": 2.2831132091553847,
-            "y": 4.384103436110321,
-            "heading": -3.4927231006079147,
-            "angularVelocity": -1.2748630467862023,
-            "velocityX": 0.5490186138449845,
-            "velocityY": -0.7889951823234699,
-            "timestamp": 1.1070838795835487
+            "x": 0.7269018921676205,
+            "y": 6.6955064720122275,
+            "heading": 1.0019343004430092,
+            "angularVelocity": -0.6189082678684702,
+            "velocityX": 0.2748070028110285,
+            "velocityY": -0.3982390224617833,
+            "timestamp": 0.06150459047524045
+        },
+        {
+            "x": 0.7606735160812093,
+            "y": 6.646281374376347,
+            "heading": 0.9275074281520226,
+            "angularVelocity": -1.2101027210472746,
+            "velocityX": 0.5490911109664911,
+            "velocityY": -0.8003483521396102,
+            "timestamp": 0.1230091809504809
+        },
+        {
+            "x": 0.8112864226090604,
+            "y": 6.571983368952451,
+            "heading": 0.8196845896852679,
+            "angularVelocity": -1.7530860320118746,
+            "velocityX": 0.8229126661403596,
+            "velocityY": -1.2080074812270452,
+            "timestamp": 0.18451377142572134
+        },
+        {
+            "x": 0.8787769249457482,
+            "y": 6.472119220055144,
+            "heading": 0.6843882308491542,
+            "angularVelocity": -2.199776598635823,
+            "velocityX": 1.0973246356929591,
+            "velocityY": -1.6236861041698774,
+            "timestamp": 0.2460183619009618
+        },
+        {
+            "x": 0.9632583450726888,
+            "y": 6.346050730942796,
+            "heading": 0.5331821377651369,
+            "angularVelocity": -2.458452156427049,
+            "velocityX": 1.373579101562342,
+            "velocityY": -2.0497411353888846,
+            "timestamp": 0.30752295237620225
+        },
+        {
+            "x": 1.064336906516991,
+            "y": 6.193349961542198,
+            "heading": 0.3933800445651471,
+            "angularVelocity": -2.273035103880077,
+            "velocityX": 1.6434311758403277,
+            "velocityY": -2.4827540224346163,
+            "timestamp": 0.3690275428514427
+        },
+        {
+            "x": 1.1776989215840556,
+            "y": 6.018028084645807,
+            "heading": 0.32650139843607295,
+            "angularVelocity": -1.0873764968160713,
+            "velocityX": 1.8431472218760627,
+            "velocityY": -2.8505494556047704,
+            "timestamp": 0.4305321333266831
+        },
+        {
+            "x": 1.3085726451154573,
+            "y": 5.825657208090679,
+            "heading": 0.3265013287499931,
+            "angularVelocity": -0.0000011330224187482564,
+            "velocityX": 2.1278691967567314,
+            "velocityY": -3.1277482716118437,
+            "timestamp": 0.49203672380192354
+        },
+        {
+            "x": 1.4394478126055321,
+            "y": 5.633287313894505,
+            "heading": 0.3265012590706379,
+            "angularVelocity": -0.000001132913083706052,
+            "velocityX": 2.1278926740071626,
+            "velocityY": -3.127732299487714,
+            "timestamp": 0.553541314277164
+        },
+        {
+            "x": 1.570322997049186,
+            "y": 5.440917431232361,
+            "heading": 0.3265011893913128,
+            "angularVelocity": -0.0000011329125928412157,
+            "velocityX": 2.1278929496545396,
+            "velocityY": -3.1277321119565196,
+            "timestamp": 0.6150459047524045
+        },
+        {
+            "x": 1.7011982004807327,
+            "y": 5.248547561488253,
+            "heading": 0.32650111971202045,
+            "angularVelocity": -0.0000011329120610367875,
+            "velocityX": 2.1278932583777177,
+            "velocityY": -3.12773190192281,
+            "timestamp": 0.676550495227645
+        },
+        {
+            "x": 1.8320716066458687,
+            "y": 5.056176469023738,
+            "heading": 0.3265010500238972,
+            "angularVelocity": -0.0000011330556420647947,
+            "velocityX": 2.1278640367148025,
+            "velocityY": -3.127751782071573,
+            "timestamp": 0.7380550857028855
+        },
+        {
+            "x": 1.9453996822937256,
+            "y": 4.881259566133339,
+            "heading": 0.2607759086629828,
+            "angularVelocity": -1.0686217216156084,
+            "velocityX": 1.8425954025899711,
+            "velocityY": -2.843965003893719,
+            "timestamp": 0.799559676178126
+        },
+        {
+            "x": 2.0464428806975716,
+            "y": 4.730058134054239,
+            "heading": 0.1310608851796909,
+            "angularVelocity": -2.109029951764503,
+            "velocityX": 1.6428562099689532,
+            "velocityY": -2.458376373385783,
+            "timestamp": 0.8610642666533664
+        },
+        {
+            "x": 2.1311236772079067,
+            "y": 4.60496312733231,
+            "heading": -0.022718365147026284,
+            "angularVelocity": -2.500288988813334,
+            "velocityX": 1.3768207520124016,
+            "velocityY": -2.0339133348475604,
+            "timestamp": 0.9225688571286069
+        },
+        {
+            "x": 2.1986971663058994,
+            "y": 4.506052833762169,
+            "heading": -0.16200277924985632,
+            "angularVelocity": -2.264618185839331,
+            "velocityX": 1.0986739132129535,
+            "velocityY": -1.6081774190490494,
+            "timestamp": 0.9840734476038474
+        },
+        {
+            "x": 2.2493424732278595,
+            "y": 4.432626695040595,
+            "heading": -0.27409854656488936,
+            "angularVelocity": -1.822559364250361,
+            "velocityX": 0.823439462495856,
+            "velocityY": -1.1938318449763972,
+            "timestamp": 1.0455780380790878
+        },
+        {
+            "x": 2.2831120594312195,
+            "y": 4.384102039370989,
+            "heading": -0.35252594149925776,
+            "angularVelocity": -1.2751470146921873,
+            "velocityX": 0.5490579799398,
+            "velocityY": -0.7889599019302513,
+            "timestamp": 1.1070826285543283
         },
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": -3.5331853071795862,
-            "angularVelocity": -0.6578722097950352,
-            "velocityX": 0.2745611609098736,
-            "velocityY": -0.39189609566798916,
-            "timestamp": 1.1685885395604125
+            "heading": -0.393,
+            "angularVelocity": -0.6580656531163417,
+            "velocityX": 0.2745801644769678,
+            "velocityY": -0.3918738290061591,
+            "timestamp": 1.1685872190295687
         },
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": -3.5331853071795862,
-            "angularVelocity": 8.150679561734844e-31,
+            "heading": -0.393,
+            "angularVelocity": -1.5278905371426354e-30,
             "velocityX": 0,
-            "velocityY": -5.044788235947052e-32,
-            "timestamp": 1.2300931995372764
+            "velocityY": -1.0222462264365078e-31,
+            "timestamp": 1.2300918095048092
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.traj
index d2e16138..f63af9ed 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.traj	
@@ -3,191 +3,191 @@
         {
             "x": 0.71,
             "y": 6.72,
-            "heading": -2.1,
-            "angularVelocity": 2.001911519996272e-31,
-            "velocityX": 6.781624829737293e-33,
-            "velocityY": -4.0943813533671796e-32,
+            "heading": 1.04,
+            "angularVelocity": 1.9442039286246259e-31,
+            "velocityX": 1.5017390020853034e-32,
+            "velocityY": 2.127944688535994e-32,
             "timestamp": 0
         },
         {
-            "x": 0.7269029739475109,
-            "y": 6.6955056066592755,
-            "heading": -2.1380447151842032,
-            "angularVelocity": -0.6185663850269976,
-            "velocityX": 0.27482428085724375,
-            "velocityY": -0.3982526421565261,
-            "timestamp": 0.061504659976863815
-        },
-        {
-            "x": 0.7606767634590716,
-            "y": 6.646279647618601,
-            "heading": -2.2124384537514556,
-            "angularVelocity": -1.2095626346887691,
-            "velocityX": 0.5491257007886129,
-            "velocityY": -0.8003614532490962,
-            "timestamp": 0.12300931995372763
-        },
-        {
-            "x": 0.8112924785066278,
-            "y": 6.571981628339174,
-            "heading": -2.320229893429466,
-            "angularVelocity": -1.752573540257899,
-            "velocityX": 0.8229573997579402,
-            "velocityY": -1.208006341428056,
-            "timestamp": 0.18451397993059143
-        },
-        {
-            "x": 0.8787853408643597,
-            "y": 6.472118688114863,
-            "heading": -2.4555147279599576,
-            "angularVelocity": -2.199586739954034,
-            "velocityX": 1.0973617671103395,
-            "velocityY": -1.6236646176383343,
-            "timestamp": 0.24601863990745526
-        },
-        {
-            "x": 0.9632675519056847,
-            "y": 6.346052032301826,
-            "heading": -2.6067469489099335,
-            "angularVelocity": -2.4588741894819854,
-            "velocityX": 1.37359040880975,
-            "velocityY": -2.049709011649839,
-            "timestamp": 0.3075232998843191
-        },
-        {
-            "x": 1.0643460613698084,
-            "y": 6.193349880124978,
-            "heading": -2.74665413649709,
-            "angularVelocity": -2.274741257650809,
-            "velocityX": 1.643428473584707,
-            "velocityY": -2.482773699330916,
-            "timestamp": 0.3690279598611829
-        },
-        {
-            "x": 1.1777092396516182,
-            "y": 6.018032409257481,
-            "heading": -2.813593357981312,
-            "angularVelocity": -1.0883601585538758,
-            "velocityX": 1.8431640517068764,
-            "velocityY": -2.850474597102823,
-            "timestamp": 0.43053261983804675
-        },
-        {
-            "x": 1.3085884905177934,
-            "y": 5.825664975179578,
-            "heading": -2.813593427712779,
-            "angularVelocity": -0.0000011337590858279882,
-            "velocityX": 2.1279566607702214,
-            "velocityY": -3.1276887661888684,
-            "timestamp": 0.4920372798149106
-        },
-        {
-            "x": 1.4394691966966826,
-            "y": 5.633298531244646,
-            "heading": -2.8135934974374672,
-            "angularVelocity": -0.000001133648866416165,
-            "velocityX": 2.127980322598685,
-            "velocityY": -3.127672667522972,
-            "timestamp": 0.5535419397917744
-        },
-        {
-            "x": 1.5703499198247641,
-            "y": 5.440932098841455,
-            "heading": -2.813593567162125,
-            "angularVelocity": -0.0000011336483749077044,
-            "velocityX": 2.1279805981744278,
-            "velocityY": -3.1276724800292155,
-            "timestamp": 0.6150465997686382
-        },
-        {
-            "x": 1.7012306619349875,
-            "y": 5.248565679353167,
-            "heading": -2.81359363688675,
-            "angularVelocity": -0.0000011336478448323905,
-            "velocityX": 2.1279809068037574,
-            "velocityY": -3.1276722700467015,
-            "timestamp": 0.676551259745502
-        },
-        {
-            "x": 1.8321095906543323,
-            "y": 5.056198026100399,
-            "heading": -2.8135937066202716,
-            "angularVelocity": -0.0000011337924880961462,
-            "velocityX": 2.1279514230072554,
-            "velocityY": -3.127692329737781,
-            "timestamp": 0.7380559197223658
-        },
-        {
-            "x": 1.945432077761735,
-            "y": 4.881276454017442,
-            "heading": -2.879360901618447,
-            "angularVelocity": -1.0693042612204542,
-            "velocityX": 1.8425024567249244,
-            "velocityY": -2.8440377062283995,
-            "timestamp": 0.7995605796992297
-        },
-        {
-            "x": 2.046464590922967,
-            "y": 4.730069111144926,
-            "heading": -3.0091287979589167,
-            "angularVelocity": -2.109887224631182,
-            "velocityX": 1.6426806228867414,
-            "velocityY": -2.458469698546365,
-            "timestamp": 0.8610652396760935
-        },
-        {
-            "x": 2.131137367212944,
-            "y": 4.6049715874663555,
-            "heading": -3.1629399624477488,
-            "angularVelocity": -2.500805053579533,
-            "velocityX": 1.3766887959681082,
-            "velocityY": -2.0339519595039928,
-            "timestamp": 0.9225698996529573
-        },
-        {
-            "x": 2.1987047903626262,
-            "y": 4.5060589736155245,
-            "heading": -3.302229643809037,
-            "angularVelocity": -2.2647012667606727,
-            "velocityX": 1.0985740458544027,
-            "velocityY": -1.6082133270558499,
-            "timestamp": 0.9840745596298212
-        },
-        {
-            "x": 2.2493460059898798,
-            "y": 4.432630316522509,
-            "heading": -3.4143130823982606,
-            "angularVelocity": -1.8223568528203613,
-            "velocityX": 0.8233720119142732,
-            "velocityY": -1.1938714419466272,
-            "timestamp": 1.0455792196066849
-        },
-        {
-            "x": 2.2831132091553847,
-            "y": 4.384103436110321,
-            "heading": -3.4927231006079147,
-            "angularVelocity": -1.2748630467862023,
-            "velocityX": 0.5490186138449845,
-            "velocityY": -0.7889951823234699,
-            "timestamp": 1.1070838795835487
+            "x": 0.7269018921676205,
+            "y": 6.6955064720122275,
+            "heading": 1.0019343004430092,
+            "angularVelocity": -0.6189082678684702,
+            "velocityX": 0.2748070028110285,
+            "velocityY": -0.3982390224617833,
+            "timestamp": 0.06150459047524045
+        },
+        {
+            "x": 0.7606735160812093,
+            "y": 6.646281374376347,
+            "heading": 0.9275074281520226,
+            "angularVelocity": -1.2101027210472746,
+            "velocityX": 0.5490911109664911,
+            "velocityY": -0.8003483521396102,
+            "timestamp": 0.1230091809504809
+        },
+        {
+            "x": 0.8112864226090604,
+            "y": 6.571983368952451,
+            "heading": 0.8196845896852679,
+            "angularVelocity": -1.7530860320118746,
+            "velocityX": 0.8229126661403596,
+            "velocityY": -1.2080074812270452,
+            "timestamp": 0.18451377142572134
+        },
+        {
+            "x": 0.8787769249457482,
+            "y": 6.472119220055144,
+            "heading": 0.6843882308491542,
+            "angularVelocity": -2.199776598635823,
+            "velocityX": 1.0973246356929591,
+            "velocityY": -1.6236861041698774,
+            "timestamp": 0.2460183619009618
+        },
+        {
+            "x": 0.9632583450726888,
+            "y": 6.346050730942796,
+            "heading": 0.5331821377651369,
+            "angularVelocity": -2.458452156427049,
+            "velocityX": 1.373579101562342,
+            "velocityY": -2.0497411353888846,
+            "timestamp": 0.30752295237620225
+        },
+        {
+            "x": 1.064336906516991,
+            "y": 6.193349961542198,
+            "heading": 0.3933800445651471,
+            "angularVelocity": -2.273035103880077,
+            "velocityX": 1.6434311758403277,
+            "velocityY": -2.4827540224346163,
+            "timestamp": 0.3690275428514427
+        },
+        {
+            "x": 1.1776989215840556,
+            "y": 6.018028084645807,
+            "heading": 0.32650139843607295,
+            "angularVelocity": -1.0873764968160713,
+            "velocityX": 1.8431472218760627,
+            "velocityY": -2.8505494556047704,
+            "timestamp": 0.4305321333266831
+        },
+        {
+            "x": 1.3085726451154573,
+            "y": 5.825657208090679,
+            "heading": 0.3265013287499931,
+            "angularVelocity": -0.0000011330224187482564,
+            "velocityX": 2.1278691967567314,
+            "velocityY": -3.1277482716118437,
+            "timestamp": 0.49203672380192354
+        },
+        {
+            "x": 1.4394478126055321,
+            "y": 5.633287313894505,
+            "heading": 0.3265012590706379,
+            "angularVelocity": -0.000001132913083706052,
+            "velocityX": 2.1278926740071626,
+            "velocityY": -3.127732299487714,
+            "timestamp": 0.553541314277164
+        },
+        {
+            "x": 1.570322997049186,
+            "y": 5.440917431232361,
+            "heading": 0.3265011893913128,
+            "angularVelocity": -0.0000011329125928412157,
+            "velocityX": 2.1278929496545396,
+            "velocityY": -3.1277321119565196,
+            "timestamp": 0.6150459047524045
+        },
+        {
+            "x": 1.7011982004807327,
+            "y": 5.248547561488253,
+            "heading": 0.32650111971202045,
+            "angularVelocity": -0.0000011329120610367875,
+            "velocityX": 2.1278932583777177,
+            "velocityY": -3.12773190192281,
+            "timestamp": 0.676550495227645
+        },
+        {
+            "x": 1.8320716066458687,
+            "y": 5.056176469023738,
+            "heading": 0.3265010500238972,
+            "angularVelocity": -0.0000011330556420647947,
+            "velocityX": 2.1278640367148025,
+            "velocityY": -3.127751782071573,
+            "timestamp": 0.7380550857028855
+        },
+        {
+            "x": 1.9453996822937256,
+            "y": 4.881259566133339,
+            "heading": 0.2607759086629828,
+            "angularVelocity": -1.0686217216156084,
+            "velocityX": 1.8425954025899711,
+            "velocityY": -2.843965003893719,
+            "timestamp": 0.799559676178126
+        },
+        {
+            "x": 2.0464428806975716,
+            "y": 4.730058134054239,
+            "heading": 0.1310608851796909,
+            "angularVelocity": -2.109029951764503,
+            "velocityX": 1.6428562099689532,
+            "velocityY": -2.458376373385783,
+            "timestamp": 0.8610642666533664
+        },
+        {
+            "x": 2.1311236772079067,
+            "y": 4.60496312733231,
+            "heading": -0.022718365147026284,
+            "angularVelocity": -2.500288988813334,
+            "velocityX": 1.3768207520124016,
+            "velocityY": -2.0339133348475604,
+            "timestamp": 0.9225688571286069
+        },
+        {
+            "x": 2.1986971663058994,
+            "y": 4.506052833762169,
+            "heading": -0.16200277924985632,
+            "angularVelocity": -2.264618185839331,
+            "velocityX": 1.0986739132129535,
+            "velocityY": -1.6081774190490494,
+            "timestamp": 0.9840734476038474
+        },
+        {
+            "x": 2.2493424732278595,
+            "y": 4.432626695040595,
+            "heading": -0.27409854656488936,
+            "angularVelocity": -1.822559364250361,
+            "velocityX": 0.823439462495856,
+            "velocityY": -1.1938318449763972,
+            "timestamp": 1.0455780380790878
+        },
+        {
+            "x": 2.2831120594312195,
+            "y": 4.384102039370989,
+            "heading": -0.35252594149925776,
+            "angularVelocity": -1.2751470146921873,
+            "velocityX": 0.5490579799398,
+            "velocityY": -0.7889599019302513,
+            "timestamp": 1.1070826285543283
         },
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": -3.5331853071795862,
-            "angularVelocity": -0.6578722097950352,
-            "velocityX": 0.2745611609098736,
-            "velocityY": -0.39189609566798916,
-            "timestamp": 1.1685885395604125
+            "heading": -0.393,
+            "angularVelocity": -0.6580656531163417,
+            "velocityX": 0.2745801644769678,
+            "velocityY": -0.3918738290061591,
+            "timestamp": 1.1685872190295687
         },
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": -3.5331853071795862,
-            "angularVelocity": 8.150679561734844e-31,
+            "heading": -0.393,
+            "angularVelocity": -1.5278905371426354e-30,
             "velocityX": 0,
-            "velocityY": -5.044788235947052e-32,
-            "timestamp": 1.2300931995372764
+            "velocityY": -1.0222462264365078e-31,
+            "timestamp": 1.2300918095048092
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.1.traj
deleted file mode 100644
index c222ae0b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.1.traj	
+++ /dev/null
@@ -1,175 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 1.0688205894738666e-32,
-            "velocityX": 4.722903296583099e-32,
-            "velocityY": -7.052966104933725e-38,
-            "timestamp": 0
-        },
-        {
-            "x": 7.680000100019751,
-            "y": 7.385155846048263,
-            "heading": 3.1400000209558985,
-            "angularVelocity": 2.1151775994912985e-7,
-            "velocityX": 0.0000010095466114596713,
-            "velocityY": -0.8260917454272356,
-            "timestamp": 0.09907392781101554
-        },
-        {
-            "x": 7.680000295060445,
-            "y": 7.221467541258139,
-            "heading": 3.1400000632436256,
-            "angularVelocity": 4.2682995289468236e-7,
-            "velocityX": 0.000001968637858711507,
-            "velocityY": -1.6521834594299558,
-            "timestamp": 0.19814785562203108
-        },
-        {
-            "x": 7.680000579860811,
-            "y": 6.975935091856332,
-            "heading": 3.140000126701719,
-            "angularVelocity": 6.405123969521912e-7,
-            "velocityX": 0.0000028746245916429767,
-            "velocityY": -2.4782751105836276,
-            "timestamp": 0.2972217834330466
-        },
-        {
-            "x": 7.680000948789776,
-            "y": 6.648558516522933,
-            "heading": 3.140000210348385,
-            "angularVelocity": 8.442850564566377e-7,
-            "velocityX": 0.0000037237742032736077,
-            "velocityY": -3.3043665731903222,
-            "timestamp": 0.39629571124406215
-        },
-        {
-            "x": 7.68000127512063,
-            "y": 6.273767735073521,
-            "heading": 3.1400002103483864,
-            "angularVelocity": 1.8797601612350415e-14,
-            "velocityX": 0.0000032938116008237123,
-            "velocityY": -3.782940575085686,
-            "timestamp": 0.4953696390550777
-        },
-        {
-            "x": 7.680001534050369,
-            "y": 5.898976953624012,
-            "heading": 3.140000210348386,
-            "angularVelocity": -4.168435621307512e-16,
-            "velocityX": 0.0000026135000838056266,
-            "velocityY": -3.782940575086706,
-            "timestamp": 0.5944435668660932
-        },
-        {
-            "x": 7.680001718421092,
-            "y": 5.524186172174471,
-            "heading": 3.14000021034838,
-            "angularVelocity": -6.61889670232267e-14,
-            "velocityX": 0.000001860940720346917,
-            "velocityY": -3.782940575087075,
-            "timestamp": 0.6935174946771088
-        },
-        {
-            "x": 7.680001822997413,
-            "y": 5.14939539072491,
-            "heading": 3.1400002103483726,
-            "angularVelocity": -8.462091393548897e-14,
-            "velocityX": 0.000001055538133316693,
-            "velocityY": -3.7829405750872986,
-            "timestamp": 0.7925914224881243
-        },
-        {
-            "x": 7.6800018440805555,
-            "y": 4.774604609275343,
-            "heading": 3.140000210348366,
-            "angularVelocity": -6.932568582141466e-14,
-            "velocityX": 2.128020680091996e-7,
-            "velocityY": -3.782940575087379,
-            "timestamp": 0.8916653502991398
-        },
-        {
-            "x": 7.680001779518267,
-            "y": 4.39981382782578,
-            "heading": 3.1400002103483597,
-            "angularVelocity": -6.86156871514602e-14,
-            "velocityX": -6.516576918751784e-7,
-            "velocityY": -3.782940575087321,
-            "timestamp": 0.9907392781101554
-        },
-        {
-            "x": 7.680001628799847,
-            "y": 4.025023046376237,
-            "heading": 3.140000210348353,
-            "angularVelocity": -7.135901447410535e-14,
-            "velocityX": -0.0000015212721994966025,
-            "velocityY": -3.782940575087123,
-            "timestamp": 1.089813205921171
-        },
-        {
-            "x": 7.680001391869683,
-            "y": 3.6502322649267236,
-            "heading": 3.140000210348346,
-            "angularVelocity": -7.415296718915011e-14,
-            "velocityX": -0.0000023914480342122818,
-            "velocityY": -3.7829405750867786,
-            "timestamp": 1.1888871337321865
-        },
-        {
-            "x": 7.680001068770943,
-            "y": 3.27544148347729,
-            "heading": 3.1400002103483433,
-            "angularVelocity": -3.3132922253880277e-14,
-            "velocityX": -0.0000032611884784431762,
-            "velocityY": -3.782940575085905,
-            "timestamp": 1.287961061543202
-        },
-        {
-            "x": 7.680000658927351,
-            "y": 2.9480649081437575,
-            "heading": 3.140000122293031,
-            "angularVelocity": -8.887836013747625e-7,
-            "velocityX": -0.000004136744686858471,
-            "velocityY": -3.3043665731916545,
-            "timestamp": 1.3870349893542175
-        },
-        {
-            "x": 7.680000338206625,
-            "y": 2.7025324587418944,
-            "heading": 3.140000057972934,
-            "angularVelocity": -6.492129951120484e-7,
-            "velocityX": -0.0000032371856464875894,
-            "velocityY": -2.4782751105841743,
-            "timestamp": 1.486108917165233
-        },
-        {
-            "x": 7.680000115622722,
-            "y": 2.5388441539517457,
-            "heading": 3.140000017927834,
-            "angularVelocity": -4.0419405265238827e-7,
-            "velocityX": -0.000002246644408273398,
-            "velocityY": -1.6521834594301972,
-            "timestamp": 1.5851828449762486
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -1.8095406608446286e-7,
-            "velocityX": -0.0000011670347327659162,
-            "velocityY": -0.8260917454273138,
-            "timestamp": 1.6842567727872642
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -2.451701531139976e-31,
-            "velocityX": 0,
-            "velocityY": -1.4987552972984166e-37,
-            "timestamp": 1.7833307005982797
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.traj
deleted file mode 100644
index c222ae0b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.traj	
+++ /dev/null
@@ -1,175 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 1.0688205894738666e-32,
-            "velocityX": 4.722903296583099e-32,
-            "velocityY": -7.052966104933725e-38,
-            "timestamp": 0
-        },
-        {
-            "x": 7.680000100019751,
-            "y": 7.385155846048263,
-            "heading": 3.1400000209558985,
-            "angularVelocity": 2.1151775994912985e-7,
-            "velocityX": 0.0000010095466114596713,
-            "velocityY": -0.8260917454272356,
-            "timestamp": 0.09907392781101554
-        },
-        {
-            "x": 7.680000295060445,
-            "y": 7.221467541258139,
-            "heading": 3.1400000632436256,
-            "angularVelocity": 4.2682995289468236e-7,
-            "velocityX": 0.000001968637858711507,
-            "velocityY": -1.6521834594299558,
-            "timestamp": 0.19814785562203108
-        },
-        {
-            "x": 7.680000579860811,
-            "y": 6.975935091856332,
-            "heading": 3.140000126701719,
-            "angularVelocity": 6.405123969521912e-7,
-            "velocityX": 0.0000028746245916429767,
-            "velocityY": -2.4782751105836276,
-            "timestamp": 0.2972217834330466
-        },
-        {
-            "x": 7.680000948789776,
-            "y": 6.648558516522933,
-            "heading": 3.140000210348385,
-            "angularVelocity": 8.442850564566377e-7,
-            "velocityX": 0.0000037237742032736077,
-            "velocityY": -3.3043665731903222,
-            "timestamp": 0.39629571124406215
-        },
-        {
-            "x": 7.68000127512063,
-            "y": 6.273767735073521,
-            "heading": 3.1400002103483864,
-            "angularVelocity": 1.8797601612350415e-14,
-            "velocityX": 0.0000032938116008237123,
-            "velocityY": -3.782940575085686,
-            "timestamp": 0.4953696390550777
-        },
-        {
-            "x": 7.680001534050369,
-            "y": 5.898976953624012,
-            "heading": 3.140000210348386,
-            "angularVelocity": -4.168435621307512e-16,
-            "velocityX": 0.0000026135000838056266,
-            "velocityY": -3.782940575086706,
-            "timestamp": 0.5944435668660932
-        },
-        {
-            "x": 7.680001718421092,
-            "y": 5.524186172174471,
-            "heading": 3.14000021034838,
-            "angularVelocity": -6.61889670232267e-14,
-            "velocityX": 0.000001860940720346917,
-            "velocityY": -3.782940575087075,
-            "timestamp": 0.6935174946771088
-        },
-        {
-            "x": 7.680001822997413,
-            "y": 5.14939539072491,
-            "heading": 3.1400002103483726,
-            "angularVelocity": -8.462091393548897e-14,
-            "velocityX": 0.000001055538133316693,
-            "velocityY": -3.7829405750872986,
-            "timestamp": 0.7925914224881243
-        },
-        {
-            "x": 7.6800018440805555,
-            "y": 4.774604609275343,
-            "heading": 3.140000210348366,
-            "angularVelocity": -6.932568582141466e-14,
-            "velocityX": 2.128020680091996e-7,
-            "velocityY": -3.782940575087379,
-            "timestamp": 0.8916653502991398
-        },
-        {
-            "x": 7.680001779518267,
-            "y": 4.39981382782578,
-            "heading": 3.1400002103483597,
-            "angularVelocity": -6.86156871514602e-14,
-            "velocityX": -6.516576918751784e-7,
-            "velocityY": -3.782940575087321,
-            "timestamp": 0.9907392781101554
-        },
-        {
-            "x": 7.680001628799847,
-            "y": 4.025023046376237,
-            "heading": 3.140000210348353,
-            "angularVelocity": -7.135901447410535e-14,
-            "velocityX": -0.0000015212721994966025,
-            "velocityY": -3.782940575087123,
-            "timestamp": 1.089813205921171
-        },
-        {
-            "x": 7.680001391869683,
-            "y": 3.6502322649267236,
-            "heading": 3.140000210348346,
-            "angularVelocity": -7.415296718915011e-14,
-            "velocityX": -0.0000023914480342122818,
-            "velocityY": -3.7829405750867786,
-            "timestamp": 1.1888871337321865
-        },
-        {
-            "x": 7.680001068770943,
-            "y": 3.27544148347729,
-            "heading": 3.1400002103483433,
-            "angularVelocity": -3.3132922253880277e-14,
-            "velocityX": -0.0000032611884784431762,
-            "velocityY": -3.782940575085905,
-            "timestamp": 1.287961061543202
-        },
-        {
-            "x": 7.680000658927351,
-            "y": 2.9480649081437575,
-            "heading": 3.140000122293031,
-            "angularVelocity": -8.887836013747625e-7,
-            "velocityX": -0.000004136744686858471,
-            "velocityY": -3.3043665731916545,
-            "timestamp": 1.3870349893542175
-        },
-        {
-            "x": 7.680000338206625,
-            "y": 2.7025324587418944,
-            "heading": 3.140000057972934,
-            "angularVelocity": -6.492129951120484e-7,
-            "velocityX": -0.0000032371856464875894,
-            "velocityY": -2.4782751105841743,
-            "timestamp": 1.486108917165233
-        },
-        {
-            "x": 7.680000115622722,
-            "y": 2.5388441539517457,
-            "heading": 3.140000017927834,
-            "angularVelocity": -4.0419405265238827e-7,
-            "velocityX": -0.000002246644408273398,
-            "velocityY": -1.6521834594301972,
-            "timestamp": 1.5851828449762486
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -1.8095406608446286e-7,
-            "velocityX": -0.0000011670347327659162,
-            "velocityY": -0.8260917454273138,
-            "timestamp": 1.6842567727872642
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -2.451701531139976e-31,
-            "velocityX": 0,
-            "velocityY": -1.4987552972984166e-37,
-            "timestamp": 1.7833307005982797
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.1.traj
deleted file mode 100644
index eb7ef7de..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.1.traj	
+++ /dev/null
@@ -1,220 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 1.8987898957165807e-31,
-            "velocityX": -1.39380671571913e-32,
-            "velocityY": -1.373126916943459e-36,
-            "timestamp": 0
-        },
-        {
-            "x": 7.680000133861116,
-            "y": 7.389044729512878,
-            "heading": 3.1400000003198203,
-            "angularVelocity": 3.307649058744193e-9,
-            "velocityX": 0.000001384414398977881,
-            "velocityY": -0.8062267267090539,
-            "timestamp": 0.0966914986871212
-        },
-        {
-            "x": 7.6800003968709785,
-            "y": 7.233134191633238,
-            "heading": 3.1400000013587985,
-            "angularVelocity": 1.0745310107378493e-8,
-            "velocityX": 0.0000027200926202754936,
-            "velocityY": -1.6124534214132102,
-            "timestamp": 0.1933829973742424
-        },
-        {
-            "x": 7.680000784081611,
-            "y": 6.9992683925502845,
-            "heading": 3.1400000030373074,
-            "angularVelocity": 1.735946006644181e-8,
-            "velocityX": 0.0000040045979798072784,
-            "velocityY": -2.4186800521075664,
-            "timestamp": 0.2900744960613636
-        },
-        {
-            "x": 7.680001290213695,
-            "y": 6.687447350831601,
-            "heading": 3.1400000045486194,
-            "angularVelocity": 1.5630280874799268e-8,
-            "velocityX": 0.000005234503549587981,
-            "velocityY": -3.2249064907728675,
-            "timestamp": 0.3867659947484848
-        },
-        {
-            "x": 7.680001791921761,
-            "y": 6.32166915785587,
-            "heading": 3.140000004548618,
-            "angularVelocity": -2.2141915496959812e-14,
-            "velocityX": 0.0000051887503514693284,
-            "velocityY": -3.782940568118399,
-            "timestamp": 0.48345749343560596
-        },
-        {
-            "x": 7.6800022292578305,
-            "y": 5.955890964879955,
-            "heading": 3.1400000045486127,
-            "angularVelocity": -6.389600617895948e-14,
-            "velocityX": 0.000004523003602297052,
-            "velocityY": -3.782940568120494,
-            "timestamp": 0.5801489921227272
-        },
-        {
-            "x": 7.680002595183966,
-            "y": 5.590112771903998,
-            "heading": 3.140000004548604,
-            "angularVelocity": -1.1343594252599792e-13,
-            "velocityX": 0.0000037844699136287606,
-            "velocityY": -3.782940568121032,
-            "timestamp": 0.6768404908098484
-        },
-        {
-            "x": 7.680002884490285,
-            "y": 5.224334578928017,
-            "heading": 3.140000004548593,
-            "angularVelocity": -1.3664646403255707e-13,
-            "velocityX": 0.0000029920547840648683,
-            "velocityY": -3.7829405681213992,
-            "timestamp": 0.7735319894969697
-        },
-        {
-            "x": 7.680003093405051,
-            "y": 4.858556385952021,
-            "heading": 3.140000004548582,
-            "angularVelocity": -1.389367975129708e-13,
-            "velocityX": 0.0000021606317619867176,
-            "velocityY": -3.782940568121637,
-            "timestamp": 0.8702234881840909
-        },
-        {
-            "x": 7.6800032195393335,
-            "y": 4.492778192976016,
-            "heading": 3.1400000045485705,
-            "angularVelocity": -1.422935768803712e-13,
-            "velocityX": 0.0000013045020379807613,
-            "velocityY": -3.782940568121783,
-            "timestamp": 0.9669149868712121
-        },
-        {
-            "x": 7.680003262132589,
-            "y": 4.127000000000006,
-            "heading": 3.1400000045485568,
-            "angularVelocity": -1.5798635695024144e-13,
-            "velocityX": 4.405066330821885e-7,
-            "velocityY": -3.782940568121863,
-            "timestamp": 1.0636064855583334
-        },
-        {
-            "x": 7.680003221112365,
-            "y": 3.7612218070239956,
-            "heading": 3.1400000045485426,
-            "angularVelocity": -1.6204973501388964e-13,
-            "velocityX": -4.2423808567477835e-7,
-            "velocityY": -3.782940568121861,
-            "timestamp": 1.1602979842454546
-        },
-        {
-            "x": 7.680003096442586,
-            "y": 3.3954436140479913,
-            "heading": 3.14000000454853,
-            "angularVelocity": -1.5384845387063427e-13,
-            "velocityX": -0.0000012893559013190512,
-            "velocityY": -3.782940568121779,
-            "timestamp": 1.2569894829325758
-        },
-        {
-            "x": 7.680002888734547,
-            "y": 3.029665421071995,
-            "heading": 3.140000004548518,
-            "angularVelocity": -1.4981778939731422e-13,
-            "velocityX": -0.0000021481515920122195,
-            "velocityY": -3.7829405681216324,
-            "timestamp": 1.353680981619697
-        },
-        {
-            "x": 7.680002600247316,
-            "y": 2.663887228096014,
-            "heading": 3.1400000045485066,
-            "angularVelocity": -1.4657528609032921e-13,
-            "velocityX": -0.0000029835836472893606,
-            "velocityY": -3.782940568121397,
-            "timestamp": 1.4503724803068183
-        },
-        {
-            "x": 7.680002234659543,
-            "y": 2.2981090351200577,
-            "heading": 3.140000004548497,
-            "angularVelocity": -1.235449081893301e-13,
-            "velocityX": -0.000003780970508619144,
-            "velocityY": -3.782940568121035,
-            "timestamp": 1.5470639789939395
-        },
-        {
-            "x": 7.680001797111122,
-            "y": 1.9323308421441414,
-            "heading": 3.1400000045484906,
-            "angularVelocity": -7.160301715418214e-14,
-            "velocityX": -0.0000045251997748623165,
-            "velocityY": -3.7829405681205035,
-            "timestamp": 1.6437554776810608
-        },
-        {
-            "x": 7.6800012945835014,
-            "y": 1.566552649168411,
-            "heading": 3.140000004548489,
-            "angularVelocity": -2.2663051081772477e-14,
-            "velocityX": -0.000005197226332013039,
-            "velocityY": -3.7829405681183954,
-            "timestamp": 1.740446976368182
-        },
-        {
-            "x": 7.680000786831981,
-            "y": 1.2547316074497201,
-            "heading": 3.140000001978014,
-            "angularVelocity": -2.6584233523758634e-8,
-            "velocityX": -0.0000052512520234957685,
-            "velocityY": -3.2249064907729323,
-            "timestamp": 1.8371384750553033
-        },
-        {
-            "x": 7.680000398309784,
-            "y": 1.0208658083667639,
-            "heading": 3.140000000933386,
-            "angularVelocity": -1.0803673621572709e-8,
-            "velocityX": -0.000004018162386759126,
-            "velocityY": -2.4186800521075957,
-            "timestamp": 1.9338299737424245
-        },
-        {
-            "x": 7.680000134361733,
-            "y": 0.8649552704871222,
-            "heading": 3.1400000003740773,
-            "angularVelocity": -5.784435922761806e-9,
-            "velocityX": -0.0000027297955176854786,
-            "velocityY": -1.612453421413225,
-            "timestamp": 2.0305214724295455
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": -3.8687549359618056e-9,
-            "velocityX": -0.000001389591862211489,
-            "velocityY": -0.8062267267090597,
-            "timestamp": 2.1272129711166667
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 5.6019143334006085e-31,
-            "velocityX": 0,
-            "velocityY": 9.268974451742519e-38,
-            "timestamp": 2.223904469803788
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.traj
deleted file mode 100644
index eb7ef7de..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.traj	
+++ /dev/null
@@ -1,220 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 1.8987898957165807e-31,
-            "velocityX": -1.39380671571913e-32,
-            "velocityY": -1.373126916943459e-36,
-            "timestamp": 0
-        },
-        {
-            "x": 7.680000133861116,
-            "y": 7.389044729512878,
-            "heading": 3.1400000003198203,
-            "angularVelocity": 3.307649058744193e-9,
-            "velocityX": 0.000001384414398977881,
-            "velocityY": -0.8062267267090539,
-            "timestamp": 0.0966914986871212
-        },
-        {
-            "x": 7.6800003968709785,
-            "y": 7.233134191633238,
-            "heading": 3.1400000013587985,
-            "angularVelocity": 1.0745310107378493e-8,
-            "velocityX": 0.0000027200926202754936,
-            "velocityY": -1.6124534214132102,
-            "timestamp": 0.1933829973742424
-        },
-        {
-            "x": 7.680000784081611,
-            "y": 6.9992683925502845,
-            "heading": 3.1400000030373074,
-            "angularVelocity": 1.735946006644181e-8,
-            "velocityX": 0.0000040045979798072784,
-            "velocityY": -2.4186800521075664,
-            "timestamp": 0.2900744960613636
-        },
-        {
-            "x": 7.680001290213695,
-            "y": 6.687447350831601,
-            "heading": 3.1400000045486194,
-            "angularVelocity": 1.5630280874799268e-8,
-            "velocityX": 0.000005234503549587981,
-            "velocityY": -3.2249064907728675,
-            "timestamp": 0.3867659947484848
-        },
-        {
-            "x": 7.680001791921761,
-            "y": 6.32166915785587,
-            "heading": 3.140000004548618,
-            "angularVelocity": -2.2141915496959812e-14,
-            "velocityX": 0.0000051887503514693284,
-            "velocityY": -3.782940568118399,
-            "timestamp": 0.48345749343560596
-        },
-        {
-            "x": 7.6800022292578305,
-            "y": 5.955890964879955,
-            "heading": 3.1400000045486127,
-            "angularVelocity": -6.389600617895948e-14,
-            "velocityX": 0.000004523003602297052,
-            "velocityY": -3.782940568120494,
-            "timestamp": 0.5801489921227272
-        },
-        {
-            "x": 7.680002595183966,
-            "y": 5.590112771903998,
-            "heading": 3.140000004548604,
-            "angularVelocity": -1.1343594252599792e-13,
-            "velocityX": 0.0000037844699136287606,
-            "velocityY": -3.782940568121032,
-            "timestamp": 0.6768404908098484
-        },
-        {
-            "x": 7.680002884490285,
-            "y": 5.224334578928017,
-            "heading": 3.140000004548593,
-            "angularVelocity": -1.3664646403255707e-13,
-            "velocityX": 0.0000029920547840648683,
-            "velocityY": -3.7829405681213992,
-            "timestamp": 0.7735319894969697
-        },
-        {
-            "x": 7.680003093405051,
-            "y": 4.858556385952021,
-            "heading": 3.140000004548582,
-            "angularVelocity": -1.389367975129708e-13,
-            "velocityX": 0.0000021606317619867176,
-            "velocityY": -3.782940568121637,
-            "timestamp": 0.8702234881840909
-        },
-        {
-            "x": 7.6800032195393335,
-            "y": 4.492778192976016,
-            "heading": 3.1400000045485705,
-            "angularVelocity": -1.422935768803712e-13,
-            "velocityX": 0.0000013045020379807613,
-            "velocityY": -3.782940568121783,
-            "timestamp": 0.9669149868712121
-        },
-        {
-            "x": 7.680003262132589,
-            "y": 4.127000000000006,
-            "heading": 3.1400000045485568,
-            "angularVelocity": -1.5798635695024144e-13,
-            "velocityX": 4.405066330821885e-7,
-            "velocityY": -3.782940568121863,
-            "timestamp": 1.0636064855583334
-        },
-        {
-            "x": 7.680003221112365,
-            "y": 3.7612218070239956,
-            "heading": 3.1400000045485426,
-            "angularVelocity": -1.6204973501388964e-13,
-            "velocityX": -4.2423808567477835e-7,
-            "velocityY": -3.782940568121861,
-            "timestamp": 1.1602979842454546
-        },
-        {
-            "x": 7.680003096442586,
-            "y": 3.3954436140479913,
-            "heading": 3.14000000454853,
-            "angularVelocity": -1.5384845387063427e-13,
-            "velocityX": -0.0000012893559013190512,
-            "velocityY": -3.782940568121779,
-            "timestamp": 1.2569894829325758
-        },
-        {
-            "x": 7.680002888734547,
-            "y": 3.029665421071995,
-            "heading": 3.140000004548518,
-            "angularVelocity": -1.4981778939731422e-13,
-            "velocityX": -0.0000021481515920122195,
-            "velocityY": -3.7829405681216324,
-            "timestamp": 1.353680981619697
-        },
-        {
-            "x": 7.680002600247316,
-            "y": 2.663887228096014,
-            "heading": 3.1400000045485066,
-            "angularVelocity": -1.4657528609032921e-13,
-            "velocityX": -0.0000029835836472893606,
-            "velocityY": -3.782940568121397,
-            "timestamp": 1.4503724803068183
-        },
-        {
-            "x": 7.680002234659543,
-            "y": 2.2981090351200577,
-            "heading": 3.140000004548497,
-            "angularVelocity": -1.235449081893301e-13,
-            "velocityX": -0.000003780970508619144,
-            "velocityY": -3.782940568121035,
-            "timestamp": 1.5470639789939395
-        },
-        {
-            "x": 7.680001797111122,
-            "y": 1.9323308421441414,
-            "heading": 3.1400000045484906,
-            "angularVelocity": -7.160301715418214e-14,
-            "velocityX": -0.0000045251997748623165,
-            "velocityY": -3.7829405681205035,
-            "timestamp": 1.6437554776810608
-        },
-        {
-            "x": 7.6800012945835014,
-            "y": 1.566552649168411,
-            "heading": 3.140000004548489,
-            "angularVelocity": -2.2663051081772477e-14,
-            "velocityX": -0.000005197226332013039,
-            "velocityY": -3.7829405681183954,
-            "timestamp": 1.740446976368182
-        },
-        {
-            "x": 7.680000786831981,
-            "y": 1.2547316074497201,
-            "heading": 3.140000001978014,
-            "angularVelocity": -2.6584233523758634e-8,
-            "velocityX": -0.0000052512520234957685,
-            "velocityY": -3.2249064907729323,
-            "timestamp": 1.8371384750553033
-        },
-        {
-            "x": 7.680000398309784,
-            "y": 1.0208658083667639,
-            "heading": 3.140000000933386,
-            "angularVelocity": -1.0803673621572709e-8,
-            "velocityX": -0.000004018162386759126,
-            "velocityY": -2.4186800521075957,
-            "timestamp": 1.9338299737424245
-        },
-        {
-            "x": 7.680000134361733,
-            "y": 0.8649552704871222,
-            "heading": 3.1400000003740773,
-            "angularVelocity": -5.784435922761806e-9,
-            "velocityX": -0.0000027297955176854786,
-            "velocityY": -1.612453421413225,
-            "timestamp": 2.0305214724295455
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": -3.8687549359618056e-9,
-            "velocityX": -0.000001389591862211489,
-            "velocityY": -0.8062267267090597,
-            "timestamp": 2.1272129711166667
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 5.6019143334006085e-31,
-            "velocityX": 0,
-            "velocityY": 9.268974451742519e-38,
-            "timestamp": 2.223904469803788
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.1.traj
index b2a982fb..9c633634 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.1.traj	
@@ -3,128 +3,128 @@
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": -8.261119414836203e-30,
-            "velocityX": 2.821294333158614e-31,
-            "velocityY": -2.62095350113427e-31,
+            "heading": 1.9214160460800783e-32,
+            "angularVelocity": -7.67482132813978e-30,
+            "velocityX": -1.6843046577095525e-31,
+            "velocityY": 2.6209535651210065e-31,
             "timestamp": 0
         },
         {
-            "x": 7.617797825979087,
-            "y": 7.446201794633709,
-            "heading": 3.1534584721408314,
-            "angularVelocity": 0.15162222909690662,
-            "velocityX": -0.7007654495821862,
-            "velocityY": -0.2343111629647132,
-            "timestamp": 0.08876318611043306
+            "x": 7.6177977502927385,
+            "y": 7.446201757051976,
+            "heading": 0.01344244845353824,
+            "angularVelocity": 0.15144190849275008,
+            "velocityX": -0.7007672331072132,
+            "velocityY": -0.23431189760056984,
+            "timestamp": 0.08876306820393338
         },
         {
-            "x": 7.493368421156802,
-            "y": 7.404602777573554,
-            "heading": 3.178746831508521,
-            "angularVelocity": 0.28489693164482327,
-            "velocityX": -1.4018131871435613,
-            "velocityY": -0.4686516886383534,
-            "timestamp": 0.17752637222086612
+            "x": 7.493368265532485,
+            "y": 7.404602703026682,
+            "heading": 0.038702181990287625,
+            "angularVelocity": 0.2845748129935982,
+            "velocityX": -1.4018159497887253,
+            "velocityY": -0.4686527276087507,
+            "timestamp": 0.17752613640786677
         },
         {
-            "x": 7.306676117263916,
-            "y": 7.342201100203473,
-            "heading": 3.213107467103745,
-            "angularVelocity": 0.38710457678121596,
-            "velocityX": -2.103262760992579,
-            "velocityY": -0.7030130406946671,
-            "timestamp": 0.2662895583312992
+            "x": 7.306675971092295,
+            "y": 7.342201042854806,
+            "heading": 0.0730274998055895,
+            "angularVelocity": 0.38670720275422266,
+            "velocityX": -2.1032654483226527,
+            "velocityY": -0.7030137807724448,
+            "timestamp": 0.26628920461180017
         },
         {
-            "x": 7.057674416387051,
-            "y": 7.2590025582832345,
-            "heading": 3.2508668949579036,
-            "angularVelocity": 0.4253951385364178,
-            "velocityX": -2.8052361771613255,
-            "velocityY": -0.9373090981171329,
-            "timestamp": 0.35505274444173224
+            "x": 7.057674447994484,
+            "y": 7.2590026396848994,
+            "heading": 0.11075741429874605,
+            "angularVelocity": 0.42506320764649574,
+            "velocityX": -2.8052379005867665,
+            "velocityY": -0.9373087800171416,
+            "timestamp": 0.35505227281573354
         },
         {
-            "x": 6.74646520076521,
-            "y": 7.155112122369572,
-            "heading": 3.2734870599198675,
-            "angularVelocity": 0.25483723550559106,
-            "velocityX": -3.5060617949738107,
-            "velocityY": -1.1704225644085642,
-            "timestamp": 0.4438159305521653
+            "x": 6.746464686770232,
+            "y": 7.155112270199717,
+            "heading": 0.1333971305446714,
+            "angularVelocity": 0.25505783773756635,
+            "velocityX": -3.506072598902806,
+            "velocityY": -1.1704233707338827,
+            "timestamp": 0.4438153410196669
         },
         {
-            "x": 6.4280476263908515,
-            "y": 7.048517625465743,
-            "heading": 3.2734870708664827,
-            "angularVelocity": 1.233247689161178e-7,
-            "velocityX": -3.5872706729821253,
-            "velocityY": -1.200886331076399,
-            "timestamp": 0.5325791166625984
+            "x": 6.428047616602866,
+            "y": 7.048517672195703,
+            "heading": 0.13339714147429826,
+            "angularVelocity": 1.23133576958653e-7,
+            "velocityX": -3.5872697576863386,
+            "velocityY": -1.200889065236652,
+            "timestamp": 0.5325784092236003
         },
         {
-            "x": 6.109630072909297,
-            "y": 6.941923066153303,
-            "heading": 3.273487081813265,
-            "angularVelocity": 1.2332664336181238e-7,
-            "velocityX": -3.5872704376046,
-            "velocityY": -1.2008870341690594,
-            "timestamp": 0.6213423027730315
+            "x": 6.109630567326096,
+            "y": 6.9419230117899575,
+            "heading": 0.13339715240409608,
+            "angularVelocity": 1.231354889650669e-7,
+            "velocityX": -3.5872695223333455,
+            "velocityY": -1.200889768252866,
+            "timestamp": 0.6213414774275338
         },
         {
-            "x": 5.798381254717505,
-            "y": 6.838018859230789,
-            "heading": 3.296331970538473,
-            "angularVelocity": 0.25736895809035,
-            "velocityX": -3.5065079548134683,
-            "velocityY": -1.1705777076875172,
-            "timestamp": 0.7101054888834646
+            "x": 5.798381237872348,
+            "y": 6.8380188367314085,
+            "heading": 0.15626327537058046,
+            "angularVelocity": 0.25760852379277716,
+            "velocityX": -3.506518372457005,
+            "velocityY": -1.170578903621919,
+            "timestamp": 0.7101045456314672
         },
         {
-            "x": 5.549362929627812,
-            "y": 6.754814274993791,
-            "heading": 3.3329076351356512,
-            "angularVelocity": 0.4120589424672841,
-            "velocityX": -2.8054234643993468,
-            "velocityY": -0.9373771704727861,
-            "timestamp": 0.7988686749938977
+            "x": 5.549363092262374,
+            "y": 6.7548143649024714,
+            "heading": 0.19280777245315245,
+            "angularVelocity": 0.41170835825755314,
+            "velocityX": -2.8054251689136636,
+            "velocityY": -0.9373771492347068,
+            "timestamp": 0.7988676138354006
         },
         {
-            "x": 5.36265371808689,
-            "y": 6.692406142204076,
-            "heading": 3.3659874710813016,
-            "angularVelocity": 0.37267517533582245,
-            "velocityX": -2.103453241394186,
-            "velocityY": -0.7030857670337234,
-            "timestamp": 0.8876318611043308
+            "x": 5.36265388534366,
+            "y": 6.692406232212667,
+            "heading": 0.22585001680889633,
+            "angularVelocity": 0.3722521655212111,
+            "velocityX": -2.1034559833970325,
+            "velocityY": -0.7030866998372853,
+            "timestamp": 0.887630682039334
         },
         {
-            "x": 5.238210282238424,
-            "y": 6.650801541740841,
-            "heading": 3.3902750965399324,
-            "angularVelocity": 0.27362273171986246,
-            "velocityX": -1.4019712597217382,
-            "velocityY": -0.4687145908921929,
-            "timestamp": 0.9763950472147639
+            "x": 5.238210362873152,
+            "y": 6.650801584329758,
+            "heading": 0.25010700866879837,
+            "angularVelocity": 0.27327797868904535,
+            "velocityX": -1.4019740978791027,
+            "velocityY": -0.46871574772688473,
+            "timestamp": 0.9763937502432675
         },
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.14544555244060076,
-            "velocityX": -0.7008567962045305,
-            "velocityY": -0.2343487503412182,
-            "timestamp": 1.065158233325197
+            "heading": 0.263,
+            "angularVelocity": 0.1452517538281686,
+            "velocityX": -0.7008586355989793,
+            "velocityY": -0.23434954143793293,
+            "timestamp": 1.065156818447201
         },
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": 3.4031853071795863,
+            "heading": 0.263,
             "angularVelocity": 0,
-            "velocityX": 1.430834166779729e-32,
-            "velocityY": -6.582148241954714e-30,
-            "timestamp": 1.15392141943563
+            "velocityX": -1.1321901910980418e-33,
+            "velocityY": 0,
+            "timestamp": 1.1539198866511342
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.traj
index b2a982fb..9c633634 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.traj	
@@ -3,128 +3,128 @@
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": -8.261119414836203e-30,
-            "velocityX": 2.821294333158614e-31,
-            "velocityY": -2.62095350113427e-31,
+            "heading": 1.9214160460800783e-32,
+            "angularVelocity": -7.67482132813978e-30,
+            "velocityX": -1.6843046577095525e-31,
+            "velocityY": 2.6209535651210065e-31,
             "timestamp": 0
         },
         {
-            "x": 7.617797825979087,
-            "y": 7.446201794633709,
-            "heading": 3.1534584721408314,
-            "angularVelocity": 0.15162222909690662,
-            "velocityX": -0.7007654495821862,
-            "velocityY": -0.2343111629647132,
-            "timestamp": 0.08876318611043306
+            "x": 7.6177977502927385,
+            "y": 7.446201757051976,
+            "heading": 0.01344244845353824,
+            "angularVelocity": 0.15144190849275008,
+            "velocityX": -0.7007672331072132,
+            "velocityY": -0.23431189760056984,
+            "timestamp": 0.08876306820393338
         },
         {
-            "x": 7.493368421156802,
-            "y": 7.404602777573554,
-            "heading": 3.178746831508521,
-            "angularVelocity": 0.28489693164482327,
-            "velocityX": -1.4018131871435613,
-            "velocityY": -0.4686516886383534,
-            "timestamp": 0.17752637222086612
+            "x": 7.493368265532485,
+            "y": 7.404602703026682,
+            "heading": 0.038702181990287625,
+            "angularVelocity": 0.2845748129935982,
+            "velocityX": -1.4018159497887253,
+            "velocityY": -0.4686527276087507,
+            "timestamp": 0.17752613640786677
         },
         {
-            "x": 7.306676117263916,
-            "y": 7.342201100203473,
-            "heading": 3.213107467103745,
-            "angularVelocity": 0.38710457678121596,
-            "velocityX": -2.103262760992579,
-            "velocityY": -0.7030130406946671,
-            "timestamp": 0.2662895583312992
+            "x": 7.306675971092295,
+            "y": 7.342201042854806,
+            "heading": 0.0730274998055895,
+            "angularVelocity": 0.38670720275422266,
+            "velocityX": -2.1032654483226527,
+            "velocityY": -0.7030137807724448,
+            "timestamp": 0.26628920461180017
         },
         {
-            "x": 7.057674416387051,
-            "y": 7.2590025582832345,
-            "heading": 3.2508668949579036,
-            "angularVelocity": 0.4253951385364178,
-            "velocityX": -2.8052361771613255,
-            "velocityY": -0.9373090981171329,
-            "timestamp": 0.35505274444173224
+            "x": 7.057674447994484,
+            "y": 7.2590026396848994,
+            "heading": 0.11075741429874605,
+            "angularVelocity": 0.42506320764649574,
+            "velocityX": -2.8052379005867665,
+            "velocityY": -0.9373087800171416,
+            "timestamp": 0.35505227281573354
         },
         {
-            "x": 6.74646520076521,
-            "y": 7.155112122369572,
-            "heading": 3.2734870599198675,
-            "angularVelocity": 0.25483723550559106,
-            "velocityX": -3.5060617949738107,
-            "velocityY": -1.1704225644085642,
-            "timestamp": 0.4438159305521653
+            "x": 6.746464686770232,
+            "y": 7.155112270199717,
+            "heading": 0.1333971305446714,
+            "angularVelocity": 0.25505783773756635,
+            "velocityX": -3.506072598902806,
+            "velocityY": -1.1704233707338827,
+            "timestamp": 0.4438153410196669
         },
         {
-            "x": 6.4280476263908515,
-            "y": 7.048517625465743,
-            "heading": 3.2734870708664827,
-            "angularVelocity": 1.233247689161178e-7,
-            "velocityX": -3.5872706729821253,
-            "velocityY": -1.200886331076399,
-            "timestamp": 0.5325791166625984
+            "x": 6.428047616602866,
+            "y": 7.048517672195703,
+            "heading": 0.13339714147429826,
+            "angularVelocity": 1.23133576958653e-7,
+            "velocityX": -3.5872697576863386,
+            "velocityY": -1.200889065236652,
+            "timestamp": 0.5325784092236003
         },
         {
-            "x": 6.109630072909297,
-            "y": 6.941923066153303,
-            "heading": 3.273487081813265,
-            "angularVelocity": 1.2332664336181238e-7,
-            "velocityX": -3.5872704376046,
-            "velocityY": -1.2008870341690594,
-            "timestamp": 0.6213423027730315
+            "x": 6.109630567326096,
+            "y": 6.9419230117899575,
+            "heading": 0.13339715240409608,
+            "angularVelocity": 1.231354889650669e-7,
+            "velocityX": -3.5872695223333455,
+            "velocityY": -1.200889768252866,
+            "timestamp": 0.6213414774275338
         },
         {
-            "x": 5.798381254717505,
-            "y": 6.838018859230789,
-            "heading": 3.296331970538473,
-            "angularVelocity": 0.25736895809035,
-            "velocityX": -3.5065079548134683,
-            "velocityY": -1.1705777076875172,
-            "timestamp": 0.7101054888834646
+            "x": 5.798381237872348,
+            "y": 6.8380188367314085,
+            "heading": 0.15626327537058046,
+            "angularVelocity": 0.25760852379277716,
+            "velocityX": -3.506518372457005,
+            "velocityY": -1.170578903621919,
+            "timestamp": 0.7101045456314672
         },
         {
-            "x": 5.549362929627812,
-            "y": 6.754814274993791,
-            "heading": 3.3329076351356512,
-            "angularVelocity": 0.4120589424672841,
-            "velocityX": -2.8054234643993468,
-            "velocityY": -0.9373771704727861,
-            "timestamp": 0.7988686749938977
+            "x": 5.549363092262374,
+            "y": 6.7548143649024714,
+            "heading": 0.19280777245315245,
+            "angularVelocity": 0.41170835825755314,
+            "velocityX": -2.8054251689136636,
+            "velocityY": -0.9373771492347068,
+            "timestamp": 0.7988676138354006
         },
         {
-            "x": 5.36265371808689,
-            "y": 6.692406142204076,
-            "heading": 3.3659874710813016,
-            "angularVelocity": 0.37267517533582245,
-            "velocityX": -2.103453241394186,
-            "velocityY": -0.7030857670337234,
-            "timestamp": 0.8876318611043308
+            "x": 5.36265388534366,
+            "y": 6.692406232212667,
+            "heading": 0.22585001680889633,
+            "angularVelocity": 0.3722521655212111,
+            "velocityX": -2.1034559833970325,
+            "velocityY": -0.7030866998372853,
+            "timestamp": 0.887630682039334
         },
         {
-            "x": 5.238210282238424,
-            "y": 6.650801541740841,
-            "heading": 3.3902750965399324,
-            "angularVelocity": 0.27362273171986246,
-            "velocityX": -1.4019712597217382,
-            "velocityY": -0.4687145908921929,
-            "timestamp": 0.9763950472147639
+            "x": 5.238210362873152,
+            "y": 6.650801584329758,
+            "heading": 0.25010700866879837,
+            "angularVelocity": 0.27327797868904535,
+            "velocityX": -1.4019740978791027,
+            "velocityY": -0.46871574772688473,
+            "timestamp": 0.9763937502432675
         },
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.14544555244060076,
-            "velocityX": -0.7008567962045305,
-            "velocityY": -0.2343487503412182,
-            "timestamp": 1.065158233325197
+            "heading": 0.263,
+            "angularVelocity": 0.1452517538281686,
+            "velocityX": -0.7008586355989793,
+            "velocityY": -0.23434954143793293,
+            "timestamp": 1.065156818447201
         },
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": 3.4031853071795863,
+            "heading": 0.263,
             "angularVelocity": 0,
-            "velocityX": 1.430834166779729e-32,
-            "velocityY": -6.582148241954714e-30,
-            "timestamp": 1.15392141943563
+            "velocityX": -1.1321901910980418e-33,
+            "velocityY": 0,
+            "timestamp": 1.1539198866511342
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.1.traj
index c20ce3b6..dc1b491d 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.1.traj	
@@ -3,146 +3,146 @@
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": -3.436405776355784e-29,
-            "velocityX": 4.0035032875632384e-30,
-            "velocityY": 0,
+            "heading": -1.0300263407047986e-28,
+            "angularVelocity": -5.012733855435028e-29,
+            "velocityX": 0,
+            "velocityY": -6.93481134012921e-30,
             "timestamp": 0
         },
         {
-            "x": 7.593933573152452,
-            "y": 7.445362365518837,
-            "heading": 3.094092714332116,
-            "angularVelocity": -0.44270408674328476,
-            "velocityX": -0.8299762955364275,
-            "velocityY": -0.2086611977356942,
-            "timestamp": 0.10369745173495665
+            "x": 7.593939909156862,
+            "y": 7.445359818001645,
+            "heading": -0.04618113523184224,
+            "angularVelocity": -0.44533187598567886,
+            "velocityX": -0.829890852839506,
+            "velocityY": -0.20867964370321365,
+            "timestamp": 0.10370049332232066
         },
         {
-            "x": 7.421555540437275,
-            "y": 7.402033887236261,
-            "heading": 3.009154332069554,
-            "angularVelocity": -0.8190980669386346,
-            "velocityX": -1.6623169598782952,
-            "velocityY": -0.41783551627973,
-            "timestamp": 0.2073949034699133
+            "x": 7.42157070597782,
+            "y": 7.402025181316033,
+            "heading": -0.13159429090032063,
+            "angularVelocity": -0.8236523562429084,
+            "velocityX": -1.6621830586985462,
+            "velocityY": -0.41788264739409464,
+            "timestamp": 0.20740098664464132
         },
         {
-            "x": 7.1624840188793355,
-            "y": 7.336952212665302,
-            "heading": 2.898930851860923,
-            "angularVelocity": -1.0629333543342216,
-            "velocityX": -2.4983402892108755,
-            "velocityY": -0.6276111271981535,
-            "timestamp": 0.31109235520486994
+            "x": 7.162505106155824,
+            "y": 7.336932536575702,
+            "heading": -0.24235044061142916,
+            "angularVelocity": -1.0680387928995545,
+            "velocityX": -2.4982099074184148,
+            "velocityY": -0.6276985061006933,
+            "timestamp": 0.31110147996696197
         },
         {
-            "x": 6.816593659112812,
-            "y": 7.250270000971659,
-            "heading": 2.8098268582890107,
-            "angularVelocity": -0.8592688834789594,
-            "velocityX": -3.335572417445675,
-            "velocityY": -0.8359145788384311,
-            "timestamp": 0.4147898069398266
+            "x": 6.816624706468089,
+            "y": 7.25023883508299,
+            "heading": -0.3315040304347048,
+            "angularVelocity": -0.8597219450650766,
+            "velocityX": -3.3353785368472084,
+            "velocityY": -0.8360008589665135,
+            "timestamp": 0.41480197328928264
         },
         {
-            "x": 6.436271393844499,
-            "y": 7.15414721538473,
-            "heading": 2.8098268314636434,
-            "angularVelocity": -2.586887820849056e-7,
-            "velocityX": -3.6676143811169655,
-            "velocityY": -0.9269541727276405,
-            "timestamp": 0.5184872586747833
+            "x": 6.436297343977517,
+            "y": 7.154089263427661,
+            "heading": -0.33150405743314826,
+            "angularVelocity": -2.603501934813404e-7,
+            "velocityX": -3.6675559614595192,
+            "velocityY": -0.9271852869251594,
+            "timestamp": 0.5185024666116033
         },
         {
-            "x": 6.055949159763292,
-            "y": 7.058024306401983,
-            "heading": 2.8098268046376322,
-            "angularVelocity": -2.5869499105961153e-7,
-            "velocityX": -3.6676140803660213,
-            "velocityY": -0.9269553626876206,
-            "timestamp": 0.62218471040974
+            "x": 6.0559700009320565,
+            "y": 7.057939614855378,
+            "heading": -0.3315040844315366,
+            "angularVelocity": -2.603496620899773e-7,
+            "velocityX": -3.6675557739472766,
+            "velocityY": -0.9271860286473393,
+            "timestamp": 0.6222029599339239
         },
         {
-            "x": 5.675627085616213,
-            "y": 6.961900764622196,
-            "heading": 2.809826777811619,
-            "angularVelocity": -2.5869501157187357e-7,
-            "velocityX": -3.6676125380511024,
-            "velocityY": -0.9269614650269816,
-            "timestamp": 0.7258821621446967
+            "x": 5.675642815380094,
+            "y": 6.961789343307514,
+            "heading": -0.3315041114299189,
+            "angularVelocity": -2.603496031733668e-7,
+            "velocityX": -3.667554255212956,
+            "velocityY": -0.927192036097853,
+            "timestamp": 0.7259034532562446
         },
         {
-            "x": 5.306947068096014,
-            "y": 6.827887110574328,
-            "heading": 2.8098267501768928,
-            "angularVelocity": -2.6649378350721345e-7,
-            "velocityX": -3.555343080778155,
-            "velocityY": -1.2923524330219798,
-            "timestamp": 0.8295796138796534
+            "x": 5.306931757393967,
+            "y": 6.827827416139461,
+            "heading": -0.33150413923830896,
+            "angularVelocity": -2.681606334045615e-7,
+            "velocityX": -3.5555381288313455,
+            "velocityY": -1.2918157173241618,
+            "timestamp": 0.8296039465785653
         },
         {
-            "x": 4.978327022444636,
-            "y": 6.61365948285698,
-            "heading": 2.809826716038805,
-            "angularVelocity": -3.2920855048251957e-7,
-            "velocityX": -3.1690272051362296,
-            "velocityY": -2.065890956172196,
-            "timestamp": 0.9332770656146101
+            "x": 4.978271181730368,
+            "y": 6.6136408985726165,
+            "heading": -0.3315041735834931,
+            "angularVelocity": -3.311959575473401e-7,
+            "velocityX": -3.1693250931994936,
+            "velocityY": -2.0654339309757477,
+            "timestamp": 0.933304439900886
         },
         {
-            "x": 4.706934591498499,
-            "y": 6.330409136170975,
-            "heading": 2.809826716485968,
-            "angularVelocity": 4.312190502568527e-9,
-            "velocityX": -2.617156221348633,
-            "velocityY": -2.731507302705652,
-            "timestamp": 1.0369745173495668
+            "x": 4.706831855999529,
+            "y": 6.330419553539566,
+            "heading": -0.3315041734698381,
+            "angularVelocity": 1.0959926731432931e-9,
+            "velocityX": -2.617531672555858,
+            "velocityY": -2.7311475187754666,
+            "timestamp": 1.0370049332232065
         },
         {
-            "x": 4.519067005656564,
-            "y": 6.028361651029434,
-            "heading": 2.9086869877879162,
-            "angularVelocity": 0.9533529479069416,
-            "velocityX": -1.8116895130858792,
-            "velocityY": -2.9127763516651948,
-            "timestamp": 1.1406719690845235
+            "x": 4.5189225775062924,
+            "y": 6.028421356099611,
+            "heading": -0.23258180854056226,
+            "angularVelocity": 0.9539237641021904,
+            "velocityX": -1.8120384240525347,
+            "velocityY": -2.912215629498277,
+            "timestamp": 1.1407054265455272
         },
         {
-            "x": 4.388194794926867,
-            "y": 5.795490351049463,
-            "heading": 3.0157821623071808,
-            "angularVelocity": 1.0327657307624416,
-            "velocityX": -1.2620581175340149,
-            "velocityY": -2.245680063336305,
-            "timestamp": 1.2443694208194802
+            "x": 4.388098272679404,
+            "y": 5.795527872027247,
+            "heading": -0.12496959809450543,
+            "angularVelocity": 1.0377212971549954,
+            "velocityX": -1.2615591366596612,
+            "velocityY": -2.2458281210725595,
+            "timestamp": 1.2444059198678479
         },
         {
-            "x": 4.304089821797064,
-            "y": 5.638764448430372,
-            "heading": 3.096687373599028,
-            "angularVelocity": 0.7802044306606039,
-            "velocityX": -0.8110611372086162,
-            "velocityY": -1.5113766056631632,
-            "timestamp": 1.348066872554437
+            "x": 4.304052104796344,
+            "y": 5.638778152214578,
+            "heading": -0.04358897284576924,
+            "angularVelocity": 0.7847660376675182,
+            "velocityX": -0.8104702802311671,
+            "velocityY": -1.5115619491362144,
+            "timestamp": 1.3481064131901686
         },
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0.41768264963403856,
-            "velocityX": -0.39624717010474564,
-            "velocityY": -0.7595601156303902,
-            "timestamp": 1.4517643242893936
+            "heading": 0,
+            "angularVelocity": 0.42033525057857213,
+            "velocityX": -0.3958718370677089,
+            "velocityY": -0.7596699850764065,
+            "timestamp": 1.4518069065124892
         },
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 1.3748988870988578e-26,
-            "velocityX": 7.122460517165917e-28,
-            "velocityY": 4.07656513453054e-28,
-            "timestamp": 1.5554617760243503
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -5.117426772684828e-28,
+            "velocityY": 3.5837289853658606e-29,
+            "timestamp": 1.55550739983481
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.traj
index c20ce3b6..dc1b491d 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.traj	
@@ -3,146 +3,146 @@
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": -3.436405776355784e-29,
-            "velocityX": 4.0035032875632384e-30,
-            "velocityY": 0,
+            "heading": -1.0300263407047986e-28,
+            "angularVelocity": -5.012733855435028e-29,
+            "velocityX": 0,
+            "velocityY": -6.93481134012921e-30,
             "timestamp": 0
         },
         {
-            "x": 7.593933573152452,
-            "y": 7.445362365518837,
-            "heading": 3.094092714332116,
-            "angularVelocity": -0.44270408674328476,
-            "velocityX": -0.8299762955364275,
-            "velocityY": -0.2086611977356942,
-            "timestamp": 0.10369745173495665
+            "x": 7.593939909156862,
+            "y": 7.445359818001645,
+            "heading": -0.04618113523184224,
+            "angularVelocity": -0.44533187598567886,
+            "velocityX": -0.829890852839506,
+            "velocityY": -0.20867964370321365,
+            "timestamp": 0.10370049332232066
         },
         {
-            "x": 7.421555540437275,
-            "y": 7.402033887236261,
-            "heading": 3.009154332069554,
-            "angularVelocity": -0.8190980669386346,
-            "velocityX": -1.6623169598782952,
-            "velocityY": -0.41783551627973,
-            "timestamp": 0.2073949034699133
+            "x": 7.42157070597782,
+            "y": 7.402025181316033,
+            "heading": -0.13159429090032063,
+            "angularVelocity": -0.8236523562429084,
+            "velocityX": -1.6621830586985462,
+            "velocityY": -0.41788264739409464,
+            "timestamp": 0.20740098664464132
         },
         {
-            "x": 7.1624840188793355,
-            "y": 7.336952212665302,
-            "heading": 2.898930851860923,
-            "angularVelocity": -1.0629333543342216,
-            "velocityX": -2.4983402892108755,
-            "velocityY": -0.6276111271981535,
-            "timestamp": 0.31109235520486994
+            "x": 7.162505106155824,
+            "y": 7.336932536575702,
+            "heading": -0.24235044061142916,
+            "angularVelocity": -1.0680387928995545,
+            "velocityX": -2.4982099074184148,
+            "velocityY": -0.6276985061006933,
+            "timestamp": 0.31110147996696197
         },
         {
-            "x": 6.816593659112812,
-            "y": 7.250270000971659,
-            "heading": 2.8098268582890107,
-            "angularVelocity": -0.8592688834789594,
-            "velocityX": -3.335572417445675,
-            "velocityY": -0.8359145788384311,
-            "timestamp": 0.4147898069398266
+            "x": 6.816624706468089,
+            "y": 7.25023883508299,
+            "heading": -0.3315040304347048,
+            "angularVelocity": -0.8597219450650766,
+            "velocityX": -3.3353785368472084,
+            "velocityY": -0.8360008589665135,
+            "timestamp": 0.41480197328928264
         },
         {
-            "x": 6.436271393844499,
-            "y": 7.15414721538473,
-            "heading": 2.8098268314636434,
-            "angularVelocity": -2.586887820849056e-7,
-            "velocityX": -3.6676143811169655,
-            "velocityY": -0.9269541727276405,
-            "timestamp": 0.5184872586747833
+            "x": 6.436297343977517,
+            "y": 7.154089263427661,
+            "heading": -0.33150405743314826,
+            "angularVelocity": -2.603501934813404e-7,
+            "velocityX": -3.6675559614595192,
+            "velocityY": -0.9271852869251594,
+            "timestamp": 0.5185024666116033
         },
         {
-            "x": 6.055949159763292,
-            "y": 7.058024306401983,
-            "heading": 2.8098268046376322,
-            "angularVelocity": -2.5869499105961153e-7,
-            "velocityX": -3.6676140803660213,
-            "velocityY": -0.9269553626876206,
-            "timestamp": 0.62218471040974
+            "x": 6.0559700009320565,
+            "y": 7.057939614855378,
+            "heading": -0.3315040844315366,
+            "angularVelocity": -2.603496620899773e-7,
+            "velocityX": -3.6675557739472766,
+            "velocityY": -0.9271860286473393,
+            "timestamp": 0.6222029599339239
         },
         {
-            "x": 5.675627085616213,
-            "y": 6.961900764622196,
-            "heading": 2.809826777811619,
-            "angularVelocity": -2.5869501157187357e-7,
-            "velocityX": -3.6676125380511024,
-            "velocityY": -0.9269614650269816,
-            "timestamp": 0.7258821621446967
+            "x": 5.675642815380094,
+            "y": 6.961789343307514,
+            "heading": -0.3315041114299189,
+            "angularVelocity": -2.603496031733668e-7,
+            "velocityX": -3.667554255212956,
+            "velocityY": -0.927192036097853,
+            "timestamp": 0.7259034532562446
         },
         {
-            "x": 5.306947068096014,
-            "y": 6.827887110574328,
-            "heading": 2.8098267501768928,
-            "angularVelocity": -2.6649378350721345e-7,
-            "velocityX": -3.555343080778155,
-            "velocityY": -1.2923524330219798,
-            "timestamp": 0.8295796138796534
+            "x": 5.306931757393967,
+            "y": 6.827827416139461,
+            "heading": -0.33150413923830896,
+            "angularVelocity": -2.681606334045615e-7,
+            "velocityX": -3.5555381288313455,
+            "velocityY": -1.2918157173241618,
+            "timestamp": 0.8296039465785653
         },
         {
-            "x": 4.978327022444636,
-            "y": 6.61365948285698,
-            "heading": 2.809826716038805,
-            "angularVelocity": -3.2920855048251957e-7,
-            "velocityX": -3.1690272051362296,
-            "velocityY": -2.065890956172196,
-            "timestamp": 0.9332770656146101
+            "x": 4.978271181730368,
+            "y": 6.6136408985726165,
+            "heading": -0.3315041735834931,
+            "angularVelocity": -3.311959575473401e-7,
+            "velocityX": -3.1693250931994936,
+            "velocityY": -2.0654339309757477,
+            "timestamp": 0.933304439900886
         },
         {
-            "x": 4.706934591498499,
-            "y": 6.330409136170975,
-            "heading": 2.809826716485968,
-            "angularVelocity": 4.312190502568527e-9,
-            "velocityX": -2.617156221348633,
-            "velocityY": -2.731507302705652,
-            "timestamp": 1.0369745173495668
+            "x": 4.706831855999529,
+            "y": 6.330419553539566,
+            "heading": -0.3315041734698381,
+            "angularVelocity": 1.0959926731432931e-9,
+            "velocityX": -2.617531672555858,
+            "velocityY": -2.7311475187754666,
+            "timestamp": 1.0370049332232065
         },
         {
-            "x": 4.519067005656564,
-            "y": 6.028361651029434,
-            "heading": 2.9086869877879162,
-            "angularVelocity": 0.9533529479069416,
-            "velocityX": -1.8116895130858792,
-            "velocityY": -2.9127763516651948,
-            "timestamp": 1.1406719690845235
+            "x": 4.5189225775062924,
+            "y": 6.028421356099611,
+            "heading": -0.23258180854056226,
+            "angularVelocity": 0.9539237641021904,
+            "velocityX": -1.8120384240525347,
+            "velocityY": -2.912215629498277,
+            "timestamp": 1.1407054265455272
         },
         {
-            "x": 4.388194794926867,
-            "y": 5.795490351049463,
-            "heading": 3.0157821623071808,
-            "angularVelocity": 1.0327657307624416,
-            "velocityX": -1.2620581175340149,
-            "velocityY": -2.245680063336305,
-            "timestamp": 1.2443694208194802
+            "x": 4.388098272679404,
+            "y": 5.795527872027247,
+            "heading": -0.12496959809450543,
+            "angularVelocity": 1.0377212971549954,
+            "velocityX": -1.2615591366596612,
+            "velocityY": -2.2458281210725595,
+            "timestamp": 1.2444059198678479
         },
         {
-            "x": 4.304089821797064,
-            "y": 5.638764448430372,
-            "heading": 3.096687373599028,
-            "angularVelocity": 0.7802044306606039,
-            "velocityX": -0.8110611372086162,
-            "velocityY": -1.5113766056631632,
-            "timestamp": 1.348066872554437
+            "x": 4.304052104796344,
+            "y": 5.638778152214578,
+            "heading": -0.04358897284576924,
+            "angularVelocity": 0.7847660376675182,
+            "velocityX": -0.8104702802311671,
+            "velocityY": -1.5115619491362144,
+            "timestamp": 1.3481064131901686
         },
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0.41768264963403856,
-            "velocityX": -0.39624717010474564,
-            "velocityY": -0.7595601156303902,
-            "timestamp": 1.4517643242893936
+            "heading": 0,
+            "angularVelocity": 0.42033525057857213,
+            "velocityX": -0.3958718370677089,
+            "velocityY": -0.7596699850764065,
+            "timestamp": 1.4518069065124892
         },
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 1.3748988870988578e-26,
-            "velocityX": 7.122460517165917e-28,
-            "velocityY": 4.07656513453054e-28,
-            "timestamp": 1.5554617760243503
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -5.117426772684828e-28,
+            "velocityY": 3.5837289853658606e-29,
+            "timestamp": 1.55550739983481
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.1.traj
index 60c01c11..c4e0d8b5 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.1.traj	
@@ -3,200 +3,200 @@
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": -3.261859244163768e-33,
-            "velocityX": 7.471195721597621e-34,
-            "velocityY": -1.6825458997868564e-33,
+            "heading": 0,
+            "angularVelocity": -1.8065144428954335e-34,
+            "velocityX": -4.551747158955398e-35,
+            "velocityY": 1.585845739465702e-34,
             "timestamp": 0
         },
         {
-            "x": 7.6132701032534955,
-            "y": 7.458186361668427,
-            "heading": 3.162690215100173,
-            "angularVelocity": 0.25198683492477825,
-            "velocityX": -0.7410707832329344,
-            "velocityY": -0.09788011341186789,
-            "timestamp": 0.09004524028783478
-        },
-        {
-            "x": 7.4797323779606675,
-            "y": 7.440537712219312,
-            "heading": 3.2047640757495373,
-            "angularVelocity": 0.46725246681415944,
-            "velocityX": -1.4830070403051525,
-            "velocityY": -0.19599758291165675,
-            "timestamp": 0.18009048057566956
-        },
-        {
-            "x": 7.27928179650239,
-            "y": 7.414023208632297,
-            "heading": 3.2605597645105227,
-            "angularVelocity": 0.6196406226762411,
-            "velocityX": -2.226109684615486,
-            "velocityY": -0.29445758045912396,
-            "timestamp": 0.27013572086350435
-        },
-        {
-            "x": 7.011816476608683,
-            "y": 7.378594826276994,
-            "heading": 3.318124935630623,
-            "angularVelocity": 0.6392916597933375,
-            "velocityX": -2.9703437853987498,
-            "velocityY": -0.3934509169174634,
-            "timestamp": 0.36018096115133913
-        },
-        {
-            "x": 6.67842119537258,
-            "y": 7.334255965416845,
-            "heading": 3.332157072604317,
-            "angularVelocity": 0.15583429983460975,
-            "velocityX": -3.70253086304603,
-            "velocityY": -0.49240649165261274,
-            "timestamp": 0.4502262014391739
-        },
-        {
-            "x": 6.34069723545727,
-            "y": 7.289811993684431,
-            "heading": 3.332157092024104,
-            "angularVelocity": 2.1566700178855977e-7,
-            "velocityX": -3.7506031283358707,
-            "velocityY": -0.4935738034608694,
-            "timestamp": 0.5402714417270087
-        },
-        {
-            "x": 6.002973284663831,
-            "y": 7.245367952636118,
-            "heading": 3.3321571114438866,
-            "angularVelocity": 2.156669548142136e-7,
-            "velocityX": -3.750603027032689,
-            "velocityY": -0.49357457325056325,
-            "timestamp": 0.6303166820148435
-        },
-        {
-            "x": 5.665249344531261,
-            "y": 7.200923830577433,
-            "heading": 3.3321571308636764,
-            "angularVelocity": 2.1566703381132295e-7,
-            "velocityX": -3.750602908638108,
-            "velocityY": -0.4935754729135791,
-            "timestamp": 0.7203619223026783
-        },
-        {
-            "x": 5.32752541585805,
-            "y": 7.156479621440943,
-            "heading": 3.332157150283477,
-            "angularVelocity": 2.1566715012677575e-7,
-            "velocityX": -3.750602781375866,
-            "velocityY": -0.4935764399586428,
-            "timestamp": 0.810407162590513
-        },
-        {
-            "x": 4.9898014989996415,
-            "y": 7.112035322525874,
-            "heading": 3.332157169703281,
-            "angularVelocity": 2.1566719028685347e-7,
-            "velocityX": -3.750602650166234,
-            "velocityY": -0.4935774369972257,
-            "timestamp": 0.9004524028783478
-        },
-        {
-            "x": 4.652077593988678,
-            "y": 7.06759093358437,
-            "heading": 3.33215718912308,
-            "angularVelocity": 2.1566713427464117e-7,
-            "velocityX": -3.750602518594089,
-            "velocityY": -0.49357843678839325,
-            "timestamp": 0.9904976431661826
-        },
-        {
-            "x": 4.314353700505922,
-            "y": 7.023146457042436,
-            "heading": 3.332157208542871,
-            "angularVelocity": 2.156670494902004e-7,
-            "velocityX": -3.7506023905672445,
-            "velocityY": -0.4935794096374718,
-            "timestamp": 1.0805428834540174
-        },
-        {
-            "x": 3.9766298178690414,
-            "y": 6.978701898085155,
-            "heading": 3.3321572279626577,
-            "angularVelocity": 2.1566699709425413e-7,
-            "velocityX": -3.750602270118079,
-            "velocityY": -0.4935803249034738,
-            "timestamp": 1.1705881237418523
-        },
-        {
-            "x": 3.6389059453045984,
-            "y": 6.934257262589842,
-            "heading": 3.332157247382445,
-            "angularVelocity": 2.1566700145288475e-7,
-            "velocityX": -3.7506021582583307,
-            "velocityY": -0.49358117489878034,
-            "timestamp": 1.2606333640296872
-        },
-        {
-            "x": 3.301182082389383,
-            "y": 6.889812553772521,
-            "heading": 3.332157266802242,
-            "angularVelocity": 2.156671157814786e-7,
-            "velocityX": -3.750602051098551,
-            "velocityY": -0.4935819891784481,
-            "timestamp": 1.350678604317522
-        },
-        {
-            "x": 2.9679743508365415,
-            "y": 6.845442796163545,
-            "heading": 3.346601240828058,
-            "angularVelocity": 0.16040796803523288,
-            "velocityX": -3.7004480246565397,
-            "velocityY": -0.4927496163833412,
-            "timestamp": 1.440723844605357
-        },
-        {
-            "x": 2.7005730136886035,
-            "y": 6.810002161458836,
-            "heading": 3.406941333368848,
-            "angularVelocity": 0.670108629261352,
-            "velocityX": -2.969633223179528,
-            "velocityY": -0.393586985735394,
-            "timestamp": 1.5307690848931919
-        },
-        {
-            "x": 2.500185866956939,
-            "y": 6.783477694334395,
-            "heading": 3.465398232800302,
-            "angularVelocity": 0.6491947741445663,
-            "velocityX": -2.2254052084387275,
-            "velocityY": -0.294568230809919,
-            "timestamp": 1.6208143251810267
-        },
-        {
-            "x": 2.366700027158657,
-            "y": 6.765819972243713,
-            "heading": 3.5094432939271947,
-            "angularVelocity": 0.4891436902839078,
-            "velocityX": -1.4824308244565385,
-            "velocityY": -0.19609833939293356,
-            "timestamp": 1.7108595654688616
+            "x": 7.613270090763082,
+            "y": 7.458186379338955,
+            "heading": 0.02268667453207945,
+            "angularVelocity": 0.251947675366546,
+            "velocityX": -0.7410713935129049,
+            "velocityY": -0.09787997945546167,
+            "timestamp": 0.09004518298918118
+        },
+        {
+            "x": 7.479732324735008,
+            "y": 7.440537819966174,
+            "heading": 0.06475236187583014,
+            "angularVelocity": 0.4671619952041724,
+            "velocityX": -1.4830084363770935,
+            "velocityY": -0.19599670728529434,
+            "timestamp": 0.18009036597836237
+        },
+        {
+            "x": 7.279281674321286,
+            "y": 7.414023565204846,
+            "heading": 0.12053400112707001,
+            "angularVelocity": 0.6194849896406114,
+            "velocityX": -2.226111866948016,
+            "velocityY": -0.2944550044894082,
+            "timestamp": 0.27013554896754355
+        },
+        {
+            "x": 7.011816314313711,
+            "y": 7.378595756973036,
+            "heading": 0.17807802747436277,
+            "angularVelocity": 0.63905724256462,
+            "velocityX": -2.970346121010285,
+            "velocityY": -0.39344479133399113,
+            "timestamp": 0.36018073195672473
+        },
+        {
+            "x": 6.678420949429204,
+            "y": 7.334257918470772,
+            "heading": 0.1920910018934364,
+            "angularVelocity": 0.15562158856134758,
+            "velocityX": -3.702534148046146,
+            "velocityY": -0.4923954511546947,
+            "timestamp": 0.4502259149459059
+        },
+        {
+            "x": 6.340697263329751,
+            "y": 7.289813527368774,
+            "heading": 0.19209102131154623,
+            "angularVelocity": 2.156485134469417e-7,
+            "velocityX": -3.7506024740938,
+            "velocityY": -0.49357877486170487,
+            "timestamp": 0.5402710979350871
+        },
+        {
+            "x": 6.002973586353306,
+            "y": 7.245369066942952,
+            "heading": 0.1920910407296517,
+            "angularVelocity": 2.1564846490105217e-7,
+            "velocityX": -3.7506023727779296,
+            "velocityY": -0.49357954473991933,
+            "timestamp": 0.6303162809242683
+        },
+        {
+            "x": 5.665249920039304,
+            "y": 7.200924525495619,
+            "heading": 0.1920910601477644,
+            "angularVelocity": 2.156485448418072e-7,
+            "velocityX": -3.750602254365785,
+            "velocityY": -0.4935804445271851,
+            "timestamp": 0.7203614639134495
+        },
+        {
+            "x": 5.327526265186639,
+            "y": 7.156479896956337,
+            "heading": 0.1920910795658876,
+            "angularVelocity": 2.1564866168111011e-7,
+            "velocityX": -3.7506021270814847,
+            "velocityY": -0.4935814117299572,
+            "timestamp": 0.8104066469026306
+        },
+        {
+            "x": 4.989802622149171,
+            "y": 7.1120351786364,
+            "heading": 0.19209109898401447,
+            "angularVelocity": 2.1564870237166035e-7,
+            "velocityX": -3.75060199586739,
+            "velocityY": -0.4935824087922295,
+            "timestamp": 0.9004518298918118
+        },
+        {
+            "x": 4.652078990957916,
+            "y": 7.067590370300322,
+            "heading": 0.19209111840213633,
+            "angularVelocity": 2.156486466006611e-7,
+            "velocityX": -3.75060186430886,
+            "velocityY": -0.49358340846970244,
+            "timestamp": 0.990497012880993
+        },
+        {
+            "x": 4.314355371293917,
+            "y": 7.023145474371938,
+            "heading": 0.19209113782025056,
+            "angularVelocity": 2.156485622267904e-7,
+            "velocityX": -3.7506017362924924,
+            "velocityY": -0.4935843812291933,
+            "timestamp": 1.0805421958701742
+        },
+        {
+            "x": 3.97663176247589,
+            "y": 6.97870049602832,
+            "heading": 0.19209115723836015,
+            "angularVelocity": 2.1564851021035735e-7,
+            "velocityX": -3.7506016158421813,
+            "velocityY": -0.4935852964945219,
+            "timestamp": 1.1705873788593553
+        },
+        {
+            "x": 3.638908163731045,
+            "y": 6.934255441141791,
+            "heading": 0.1920911766564701,
+            "angularVelocity": 2.1564851460850062e-7,
+            "velocityX": -3.7506015039740865,
+            "velocityY": -0.49358614654454386,
+            "timestamp": 1.2606325618485363
+        },
+        {
+            "x": 3.301184574635452,
+            "y": 6.88981031293383,
+            "heading": 0.1920911960745901,
+            "angularVelocity": 2.156486262427343e-7,
+            "velocityX": -3.750601396813981,
+            "velocityY": -0.49358696081834263,
+            "timestamp": 1.3506777448377174
+        },
+        {
+            "x": 2.967975728993463,
+            "y": 6.845441681932465,
+            "heading": 0.20651305517000687,
+            "angularVelocity": 0.16016247195754518,
+            "velocityX": -3.70046275192781,
+            "velocityY": -0.49273741835468604,
+            "timestamp": 1.4407229278268985
+        },
+        {
+            "x": 2.7005739783902083,
+            "y": 6.810001707269663,
+            "heading": 0.26681564809465086,
+            "angularVelocity": 0.6696926023448618,
+            "velocityX": -2.969639704495717,
+            "velocityY": -0.3935799060684999,
+            "timestamp": 1.5307681108160796
+        },
+        {
+            "x": 2.5001863972917366,
+            "y": 6.7834775390625595,
+            "heading": 0.32524314681975514,
+            "angularVelocity": 0.6488686766523009,
+            "velocityX": -2.2254114484119376,
+            "velocityY": -0.2945650986159953,
+            "timestamp": 1.6208132938052606
+        },
+        {
+            "x": 2.3667002159439683,
+            "y": 6.765819938844314,
+            "heading": 0.3692681668658828,
+            "angularVelocity": 0.4889214346026397,
+            "velocityX": -1.482435560865111,
+            "velocityY": -0.1960971107179338,
+            "timestamp": 1.7108584767944417
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 0.26366760948717133,
-            "velocityX": -0.7407390656679514,
-            "velocityY": -0.09795045485490664,
-            "timestamp": 1.8009048057566965
+            "heading": 0.393,
+            "angularVelocity": 0.2635547215998056,
+            "velocityX": -0.7407416335861364,
+            "velocityY": -0.09795014626572365,
+            "timestamp": 1.8009036597836228
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 1.378321455008778e-32,
+            "heading": 0.393,
+            "angularVelocity": 9.742765950850105e-34,
             "velocityX": 0,
-            "velocityY": 4.004611385179353e-34,
-            "timestamp": 1.8909500460445314
+            "velocityY": -8.251938001070805e-34,
+            "timestamp": 1.8909488427728038
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.traj
index 60c01c11..c4e0d8b5 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.traj	
@@ -3,200 +3,200 @@
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": -3.261859244163768e-33,
-            "velocityX": 7.471195721597621e-34,
-            "velocityY": -1.6825458997868564e-33,
+            "heading": 0,
+            "angularVelocity": -1.8065144428954335e-34,
+            "velocityX": -4.551747158955398e-35,
+            "velocityY": 1.585845739465702e-34,
             "timestamp": 0
         },
         {
-            "x": 7.6132701032534955,
-            "y": 7.458186361668427,
-            "heading": 3.162690215100173,
-            "angularVelocity": 0.25198683492477825,
-            "velocityX": -0.7410707832329344,
-            "velocityY": -0.09788011341186789,
-            "timestamp": 0.09004524028783478
-        },
-        {
-            "x": 7.4797323779606675,
-            "y": 7.440537712219312,
-            "heading": 3.2047640757495373,
-            "angularVelocity": 0.46725246681415944,
-            "velocityX": -1.4830070403051525,
-            "velocityY": -0.19599758291165675,
-            "timestamp": 0.18009048057566956
-        },
-        {
-            "x": 7.27928179650239,
-            "y": 7.414023208632297,
-            "heading": 3.2605597645105227,
-            "angularVelocity": 0.6196406226762411,
-            "velocityX": -2.226109684615486,
-            "velocityY": -0.29445758045912396,
-            "timestamp": 0.27013572086350435
-        },
-        {
-            "x": 7.011816476608683,
-            "y": 7.378594826276994,
-            "heading": 3.318124935630623,
-            "angularVelocity": 0.6392916597933375,
-            "velocityX": -2.9703437853987498,
-            "velocityY": -0.3934509169174634,
-            "timestamp": 0.36018096115133913
-        },
-        {
-            "x": 6.67842119537258,
-            "y": 7.334255965416845,
-            "heading": 3.332157072604317,
-            "angularVelocity": 0.15583429983460975,
-            "velocityX": -3.70253086304603,
-            "velocityY": -0.49240649165261274,
-            "timestamp": 0.4502262014391739
-        },
-        {
-            "x": 6.34069723545727,
-            "y": 7.289811993684431,
-            "heading": 3.332157092024104,
-            "angularVelocity": 2.1566700178855977e-7,
-            "velocityX": -3.7506031283358707,
-            "velocityY": -0.4935738034608694,
-            "timestamp": 0.5402714417270087
-        },
-        {
-            "x": 6.002973284663831,
-            "y": 7.245367952636118,
-            "heading": 3.3321571114438866,
-            "angularVelocity": 2.156669548142136e-7,
-            "velocityX": -3.750603027032689,
-            "velocityY": -0.49357457325056325,
-            "timestamp": 0.6303166820148435
-        },
-        {
-            "x": 5.665249344531261,
-            "y": 7.200923830577433,
-            "heading": 3.3321571308636764,
-            "angularVelocity": 2.1566703381132295e-7,
-            "velocityX": -3.750602908638108,
-            "velocityY": -0.4935754729135791,
-            "timestamp": 0.7203619223026783
-        },
-        {
-            "x": 5.32752541585805,
-            "y": 7.156479621440943,
-            "heading": 3.332157150283477,
-            "angularVelocity": 2.1566715012677575e-7,
-            "velocityX": -3.750602781375866,
-            "velocityY": -0.4935764399586428,
-            "timestamp": 0.810407162590513
-        },
-        {
-            "x": 4.9898014989996415,
-            "y": 7.112035322525874,
-            "heading": 3.332157169703281,
-            "angularVelocity": 2.1566719028685347e-7,
-            "velocityX": -3.750602650166234,
-            "velocityY": -0.4935774369972257,
-            "timestamp": 0.9004524028783478
-        },
-        {
-            "x": 4.652077593988678,
-            "y": 7.06759093358437,
-            "heading": 3.33215718912308,
-            "angularVelocity": 2.1566713427464117e-7,
-            "velocityX": -3.750602518594089,
-            "velocityY": -0.49357843678839325,
-            "timestamp": 0.9904976431661826
-        },
-        {
-            "x": 4.314353700505922,
-            "y": 7.023146457042436,
-            "heading": 3.332157208542871,
-            "angularVelocity": 2.156670494902004e-7,
-            "velocityX": -3.7506023905672445,
-            "velocityY": -0.4935794096374718,
-            "timestamp": 1.0805428834540174
-        },
-        {
-            "x": 3.9766298178690414,
-            "y": 6.978701898085155,
-            "heading": 3.3321572279626577,
-            "angularVelocity": 2.1566699709425413e-7,
-            "velocityX": -3.750602270118079,
-            "velocityY": -0.4935803249034738,
-            "timestamp": 1.1705881237418523
-        },
-        {
-            "x": 3.6389059453045984,
-            "y": 6.934257262589842,
-            "heading": 3.332157247382445,
-            "angularVelocity": 2.1566700145288475e-7,
-            "velocityX": -3.7506021582583307,
-            "velocityY": -0.49358117489878034,
-            "timestamp": 1.2606333640296872
-        },
-        {
-            "x": 3.301182082389383,
-            "y": 6.889812553772521,
-            "heading": 3.332157266802242,
-            "angularVelocity": 2.156671157814786e-7,
-            "velocityX": -3.750602051098551,
-            "velocityY": -0.4935819891784481,
-            "timestamp": 1.350678604317522
-        },
-        {
-            "x": 2.9679743508365415,
-            "y": 6.845442796163545,
-            "heading": 3.346601240828058,
-            "angularVelocity": 0.16040796803523288,
-            "velocityX": -3.7004480246565397,
-            "velocityY": -0.4927496163833412,
-            "timestamp": 1.440723844605357
-        },
-        {
-            "x": 2.7005730136886035,
-            "y": 6.810002161458836,
-            "heading": 3.406941333368848,
-            "angularVelocity": 0.670108629261352,
-            "velocityX": -2.969633223179528,
-            "velocityY": -0.393586985735394,
-            "timestamp": 1.5307690848931919
-        },
-        {
-            "x": 2.500185866956939,
-            "y": 6.783477694334395,
-            "heading": 3.465398232800302,
-            "angularVelocity": 0.6491947741445663,
-            "velocityX": -2.2254052084387275,
-            "velocityY": -0.294568230809919,
-            "timestamp": 1.6208143251810267
-        },
-        {
-            "x": 2.366700027158657,
-            "y": 6.765819972243713,
-            "heading": 3.5094432939271947,
-            "angularVelocity": 0.4891436902839078,
-            "velocityX": -1.4824308244565385,
-            "velocityY": -0.19609833939293356,
-            "timestamp": 1.7108595654688616
+            "x": 7.613270090763082,
+            "y": 7.458186379338955,
+            "heading": 0.02268667453207945,
+            "angularVelocity": 0.251947675366546,
+            "velocityX": -0.7410713935129049,
+            "velocityY": -0.09787997945546167,
+            "timestamp": 0.09004518298918118
+        },
+        {
+            "x": 7.479732324735008,
+            "y": 7.440537819966174,
+            "heading": 0.06475236187583014,
+            "angularVelocity": 0.4671619952041724,
+            "velocityX": -1.4830084363770935,
+            "velocityY": -0.19599670728529434,
+            "timestamp": 0.18009036597836237
+        },
+        {
+            "x": 7.279281674321286,
+            "y": 7.414023565204846,
+            "heading": 0.12053400112707001,
+            "angularVelocity": 0.6194849896406114,
+            "velocityX": -2.226111866948016,
+            "velocityY": -0.2944550044894082,
+            "timestamp": 0.27013554896754355
+        },
+        {
+            "x": 7.011816314313711,
+            "y": 7.378595756973036,
+            "heading": 0.17807802747436277,
+            "angularVelocity": 0.63905724256462,
+            "velocityX": -2.970346121010285,
+            "velocityY": -0.39344479133399113,
+            "timestamp": 0.36018073195672473
+        },
+        {
+            "x": 6.678420949429204,
+            "y": 7.334257918470772,
+            "heading": 0.1920910018934364,
+            "angularVelocity": 0.15562158856134758,
+            "velocityX": -3.702534148046146,
+            "velocityY": -0.4923954511546947,
+            "timestamp": 0.4502259149459059
+        },
+        {
+            "x": 6.340697263329751,
+            "y": 7.289813527368774,
+            "heading": 0.19209102131154623,
+            "angularVelocity": 2.156485134469417e-7,
+            "velocityX": -3.7506024740938,
+            "velocityY": -0.49357877486170487,
+            "timestamp": 0.5402710979350871
+        },
+        {
+            "x": 6.002973586353306,
+            "y": 7.245369066942952,
+            "heading": 0.1920910407296517,
+            "angularVelocity": 2.1564846490105217e-7,
+            "velocityX": -3.7506023727779296,
+            "velocityY": -0.49357954473991933,
+            "timestamp": 0.6303162809242683
+        },
+        {
+            "x": 5.665249920039304,
+            "y": 7.200924525495619,
+            "heading": 0.1920910601477644,
+            "angularVelocity": 2.156485448418072e-7,
+            "velocityX": -3.750602254365785,
+            "velocityY": -0.4935804445271851,
+            "timestamp": 0.7203614639134495
+        },
+        {
+            "x": 5.327526265186639,
+            "y": 7.156479896956337,
+            "heading": 0.1920910795658876,
+            "angularVelocity": 2.1564866168111011e-7,
+            "velocityX": -3.7506021270814847,
+            "velocityY": -0.4935814117299572,
+            "timestamp": 0.8104066469026306
+        },
+        {
+            "x": 4.989802622149171,
+            "y": 7.1120351786364,
+            "heading": 0.19209109898401447,
+            "angularVelocity": 2.1564870237166035e-7,
+            "velocityX": -3.75060199586739,
+            "velocityY": -0.4935824087922295,
+            "timestamp": 0.9004518298918118
+        },
+        {
+            "x": 4.652078990957916,
+            "y": 7.067590370300322,
+            "heading": 0.19209111840213633,
+            "angularVelocity": 2.156486466006611e-7,
+            "velocityX": -3.75060186430886,
+            "velocityY": -0.49358340846970244,
+            "timestamp": 0.990497012880993
+        },
+        {
+            "x": 4.314355371293917,
+            "y": 7.023145474371938,
+            "heading": 0.19209113782025056,
+            "angularVelocity": 2.156485622267904e-7,
+            "velocityX": -3.7506017362924924,
+            "velocityY": -0.4935843812291933,
+            "timestamp": 1.0805421958701742
+        },
+        {
+            "x": 3.97663176247589,
+            "y": 6.97870049602832,
+            "heading": 0.19209115723836015,
+            "angularVelocity": 2.1564851021035735e-7,
+            "velocityX": -3.7506016158421813,
+            "velocityY": -0.4935852964945219,
+            "timestamp": 1.1705873788593553
+        },
+        {
+            "x": 3.638908163731045,
+            "y": 6.934255441141791,
+            "heading": 0.1920911766564701,
+            "angularVelocity": 2.1564851460850062e-7,
+            "velocityX": -3.7506015039740865,
+            "velocityY": -0.49358614654454386,
+            "timestamp": 1.2606325618485363
+        },
+        {
+            "x": 3.301184574635452,
+            "y": 6.88981031293383,
+            "heading": 0.1920911960745901,
+            "angularVelocity": 2.156486262427343e-7,
+            "velocityX": -3.750601396813981,
+            "velocityY": -0.49358696081834263,
+            "timestamp": 1.3506777448377174
+        },
+        {
+            "x": 2.967975728993463,
+            "y": 6.845441681932465,
+            "heading": 0.20651305517000687,
+            "angularVelocity": 0.16016247195754518,
+            "velocityX": -3.70046275192781,
+            "velocityY": -0.49273741835468604,
+            "timestamp": 1.4407229278268985
+        },
+        {
+            "x": 2.7005739783902083,
+            "y": 6.810001707269663,
+            "heading": 0.26681564809465086,
+            "angularVelocity": 0.6696926023448618,
+            "velocityX": -2.969639704495717,
+            "velocityY": -0.3935799060684999,
+            "timestamp": 1.5307681108160796
+        },
+        {
+            "x": 2.5001863972917366,
+            "y": 6.7834775390625595,
+            "heading": 0.32524314681975514,
+            "angularVelocity": 0.6488686766523009,
+            "velocityX": -2.2254114484119376,
+            "velocityY": -0.2945650986159953,
+            "timestamp": 1.6208132938052606
+        },
+        {
+            "x": 2.3667002159439683,
+            "y": 6.765819938844314,
+            "heading": 0.3692681668658828,
+            "angularVelocity": 0.4889214346026397,
+            "velocityX": -1.482435560865111,
+            "velocityY": -0.1960971107179338,
+            "timestamp": 1.7108584767944417
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 0.26366760948717133,
-            "velocityX": -0.7407390656679514,
-            "velocityY": -0.09795045485490664,
-            "timestamp": 1.8009048057566965
+            "heading": 0.393,
+            "angularVelocity": 0.2635547215998056,
+            "velocityX": -0.7407416335861364,
+            "velocityY": -0.09795014626572365,
+            "timestamp": 1.8009036597836228
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 1.378321455008778e-32,
+            "heading": 0.393,
+            "angularVelocity": 9.742765950850105e-34,
             "velocityX": 0,
-            "velocityY": 4.004611385179353e-34,
-            "timestamp": 1.8909500460445314
+            "velocityY": -8.251938001070805e-34,
+            "timestamp": 1.8909488427728038
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.1.traj
new file mode 100644
index 00000000..82d2bcc6
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.1.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 1.1003498306069892e-28,
+            "velocityX": -2.6796850098783343e-31,
+            "velocityY": 7.1039345927107565e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 7.602663557475489,
+            "y": 7.440503469641943,
+            "heading": 0.0012193469853657303,
+            "angularVelocity": 0.01231450998750236,
+            "velocityX": -0.781041332205243,
+            "velocityY": -0.26759551763859557,
+            "timestamp": 0.09901709337987814
+        },
+        {
+            "x": 7.447990676613656,
+            "y": 7.387510406307137,
+            "heading": 0.00365804177605024,
+            "angularVelocity": 0.024629028255928873,
+            "velocityX": -1.562082622122956,
+            "velocityY": -0.5351910617240387,
+            "timestamp": 0.1980341867597563
+        },
+        {
+            "x": 7.2159813646219675,
+            "y": 7.3080208081596165,
+            "heading": 0.0073160847395553,
+            "angularVelocity": 0.03694355023585351,
+            "velocityX": -2.3431238392505196,
+            "velocityY": -0.8027866243514209,
+            "timestamp": 0.29705128013963444
+        },
+        {
+            "x": 6.906635640594062,
+            "y": 7.202034677060231,
+            "heading": 0.012193473491592185,
+            "angularVelocity": 0.049258048136210826,
+            "velocityX": -3.1241648635463743,
+            "velocityY": -1.0703821681855634,
+            "timestamp": 0.3960683735195126
+        },
+        {
+            "x": 6.5522807849123526,
+            "y": 7.080627711247213,
+            "heading": 0.012193474015757445,
+            "angularVelocity": 5.293684623965923e-9,
+            "velocityX": -3.5787240726429963,
+            "velocityY": -1.2261212854152546,
+            "timestamp": 0.4950854668993907
+        },
+        {
+            "x": 6.197925949682556,
+            "y": 6.959220685740379,
+            "heading": 0.012193474539927869,
+            "angularVelocity": 5.293736792766535e-9,
+            "velocityX": -3.578723866093675,
+            "velocityY": -1.2261218882789995,
+            "timestamp": 0.5941025602792689
+        },
+        {
+            "x": 5.843571141718115,
+            "y": 6.8378135806532265,
+            "heading": 0.012193475064112974,
+            "angularVelocity": 5.2938850654703056e-9,
+            "velocityX": -3.578723590733579,
+            "velocityY": -1.2261226919818409,
+            "timestamp": 0.6931196536591471
+        },
+        {
+            "x": 5.489216369151057,
+            "y": 6.716406372250631,
+            "heading": 0.012193475588313017,
+            "angularVelocity": 5.294035915259789e-9,
+            "velocityX": -3.578723233245997,
+            "velocityY": -1.2261237353920091,
+            "timestamp": 0.7921367470390253
+        },
+        {
+            "x": 5.134861642331667,
+            "y": 6.594999030323008,
+            "heading": 0.012193476112502804,
+            "angularVelocity": 5.293932348218223e-9,
+            "velocityX": -3.5787227712281067,
+            "velocityY": -1.2261250838968347,
+            "timestamp": 0.8911538404189034
+        },
+        {
+            "x": 4.780507205250472,
+            "y": 6.473590842716537,
+            "heading": 0.012193476636274196,
+            "angularVelocity": 5.2897066685502164e-9,
+            "velocityX": -3.578719845084944,
+            "velocityY": -1.2261336246326482,
+            "timestamp": 0.9901709337987816
+        },
+        {
+            "x": 4.428189328838181,
+            "y": 6.346393637776016,
+            "heading": 0.012193476110055806,
+            "angularVelocity": -5.314419742287748e-9,
+            "velocityX": -3.558152076436196,
+            "velocityY": -1.2845984526381864,
+            "timestamp": 1.0891880271786598
+        },
+        {
+            "x": 4.075871503880035,
+            "y": 6.219196290315138,
+            "heading": 0.012193475583846744,
+            "angularVelocity": -5.314325547246037e-9,
+            "velocityX": -3.5581515567870925,
+            "velocityY": -1.2845998919892312,
+            "timestamp": 1.188205120558538
+        },
+        {
+            "x": 3.723553713185669,
+            "y": 6.091998847948706,
+            "heading": 0.012193475057649396,
+            "angularVelocity": -5.314207229499686e-9,
+            "velocityX": -3.5581512107480435,
+            "velocityY": -1.2846008504657016,
+            "timestamp": 1.2872222139384162
+        },
+        {
+            "x": 3.3712359540403063,
+            "y": 5.96480131819632,
+            "heading": 0.012193474531462604,
+            "angularVelocity": -5.314100634839972e-9,
+            "velocityX": -3.5581508921262577,
+            "velocityY": -1.284601732999717,
+            "timestamp": 1.3862393073182944
+        },
+        {
+            "x": 3.018918225427334,
+            "y": 5.837603703873982,
+            "heading": 0.012193474005277722,
+            "angularVelocity": -5.314081331919232e-9,
+            "velocityX": -3.558150583771516,
+            "velocityY": -1.2846025870942077,
+            "timestamp": 1.4852564006981726
+        },
+        {
+            "x": 2.7113509459543246,
+            "y": 5.726562245993832,
+            "heading": 0.0073160988837450616,
+            "angularVelocity": -0.04925791047811804,
+            "velocityX": -3.1062038782842247,
+            "velocityY": -1.121437259869279,
+            "timestamp": 1.5842734940780507
+        },
+        {
+            "x": 2.48067547425723,
+            "y": 5.6432811332300385,
+            "heading": 0.003658054936391331,
+            "angularVelocity": -0.03694356017200323,
+            "velocityX": -2.329653030836908,
+            "velocityY": -0.8410781403599302,
+            "timestamp": 1.683290587457929
+        },
+        {
+            "x": 2.326891824704753,
+            "y": 5.5877603809363166,
+            "heading": 0.0012193531500403563,
+            "angularVelocity": -0.024629098907027813,
+            "velocityX": -1.5531020382762346,
+            "velocityY": -0.560718865789336,
+            "timestamp": 1.782307680837807
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -2.184437138856228e-30,
+            "angularVelocity": -0.012314572246193395,
+            "velocityX": -0.7765510184161662,
+            "velocityY": -0.28035948126465987,
+            "timestamp": 1.8813247742176853
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 2.219047153199805e-29,
+            "velocityX": 0,
+            "velocityY": -7.552577135588168e-32,
+            "timestamp": 1.9803418675975635
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.traj
new file mode 100644
index 00000000..82d2bcc6
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 1.1003498306069892e-28,
+            "velocityX": -2.6796850098783343e-31,
+            "velocityY": 7.1039345927107565e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 7.602663557475489,
+            "y": 7.440503469641943,
+            "heading": 0.0012193469853657303,
+            "angularVelocity": 0.01231450998750236,
+            "velocityX": -0.781041332205243,
+            "velocityY": -0.26759551763859557,
+            "timestamp": 0.09901709337987814
+        },
+        {
+            "x": 7.447990676613656,
+            "y": 7.387510406307137,
+            "heading": 0.00365804177605024,
+            "angularVelocity": 0.024629028255928873,
+            "velocityX": -1.562082622122956,
+            "velocityY": -0.5351910617240387,
+            "timestamp": 0.1980341867597563
+        },
+        {
+            "x": 7.2159813646219675,
+            "y": 7.3080208081596165,
+            "heading": 0.0073160847395553,
+            "angularVelocity": 0.03694355023585351,
+            "velocityX": -2.3431238392505196,
+            "velocityY": -0.8027866243514209,
+            "timestamp": 0.29705128013963444
+        },
+        {
+            "x": 6.906635640594062,
+            "y": 7.202034677060231,
+            "heading": 0.012193473491592185,
+            "angularVelocity": 0.049258048136210826,
+            "velocityX": -3.1241648635463743,
+            "velocityY": -1.0703821681855634,
+            "timestamp": 0.3960683735195126
+        },
+        {
+            "x": 6.5522807849123526,
+            "y": 7.080627711247213,
+            "heading": 0.012193474015757445,
+            "angularVelocity": 5.293684623965923e-9,
+            "velocityX": -3.5787240726429963,
+            "velocityY": -1.2261212854152546,
+            "timestamp": 0.4950854668993907
+        },
+        {
+            "x": 6.197925949682556,
+            "y": 6.959220685740379,
+            "heading": 0.012193474539927869,
+            "angularVelocity": 5.293736792766535e-9,
+            "velocityX": -3.578723866093675,
+            "velocityY": -1.2261218882789995,
+            "timestamp": 0.5941025602792689
+        },
+        {
+            "x": 5.843571141718115,
+            "y": 6.8378135806532265,
+            "heading": 0.012193475064112974,
+            "angularVelocity": 5.2938850654703056e-9,
+            "velocityX": -3.578723590733579,
+            "velocityY": -1.2261226919818409,
+            "timestamp": 0.6931196536591471
+        },
+        {
+            "x": 5.489216369151057,
+            "y": 6.716406372250631,
+            "heading": 0.012193475588313017,
+            "angularVelocity": 5.294035915259789e-9,
+            "velocityX": -3.578723233245997,
+            "velocityY": -1.2261237353920091,
+            "timestamp": 0.7921367470390253
+        },
+        {
+            "x": 5.134861642331667,
+            "y": 6.594999030323008,
+            "heading": 0.012193476112502804,
+            "angularVelocity": 5.293932348218223e-9,
+            "velocityX": -3.5787227712281067,
+            "velocityY": -1.2261250838968347,
+            "timestamp": 0.8911538404189034
+        },
+        {
+            "x": 4.780507205250472,
+            "y": 6.473590842716537,
+            "heading": 0.012193476636274196,
+            "angularVelocity": 5.2897066685502164e-9,
+            "velocityX": -3.578719845084944,
+            "velocityY": -1.2261336246326482,
+            "timestamp": 0.9901709337987816
+        },
+        {
+            "x": 4.428189328838181,
+            "y": 6.346393637776016,
+            "heading": 0.012193476110055806,
+            "angularVelocity": -5.314419742287748e-9,
+            "velocityX": -3.558152076436196,
+            "velocityY": -1.2845984526381864,
+            "timestamp": 1.0891880271786598
+        },
+        {
+            "x": 4.075871503880035,
+            "y": 6.219196290315138,
+            "heading": 0.012193475583846744,
+            "angularVelocity": -5.314325547246037e-9,
+            "velocityX": -3.5581515567870925,
+            "velocityY": -1.2845998919892312,
+            "timestamp": 1.188205120558538
+        },
+        {
+            "x": 3.723553713185669,
+            "y": 6.091998847948706,
+            "heading": 0.012193475057649396,
+            "angularVelocity": -5.314207229499686e-9,
+            "velocityX": -3.5581512107480435,
+            "velocityY": -1.2846008504657016,
+            "timestamp": 1.2872222139384162
+        },
+        {
+            "x": 3.3712359540403063,
+            "y": 5.96480131819632,
+            "heading": 0.012193474531462604,
+            "angularVelocity": -5.314100634839972e-9,
+            "velocityX": -3.5581508921262577,
+            "velocityY": -1.284601732999717,
+            "timestamp": 1.3862393073182944
+        },
+        {
+            "x": 3.018918225427334,
+            "y": 5.837603703873982,
+            "heading": 0.012193474005277722,
+            "angularVelocity": -5.314081331919232e-9,
+            "velocityX": -3.558150583771516,
+            "velocityY": -1.2846025870942077,
+            "timestamp": 1.4852564006981726
+        },
+        {
+            "x": 2.7113509459543246,
+            "y": 5.726562245993832,
+            "heading": 0.0073160988837450616,
+            "angularVelocity": -0.04925791047811804,
+            "velocityX": -3.1062038782842247,
+            "velocityY": -1.121437259869279,
+            "timestamp": 1.5842734940780507
+        },
+        {
+            "x": 2.48067547425723,
+            "y": 5.6432811332300385,
+            "heading": 0.003658054936391331,
+            "angularVelocity": -0.03694356017200323,
+            "velocityX": -2.329653030836908,
+            "velocityY": -0.8410781403599302,
+            "timestamp": 1.683290587457929
+        },
+        {
+            "x": 2.326891824704753,
+            "y": 5.5877603809363166,
+            "heading": 0.0012193531500403563,
+            "angularVelocity": -0.024629098907027813,
+            "velocityX": -1.5531020382762346,
+            "velocityY": -0.560718865789336,
+            "timestamp": 1.782307680837807
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -2.184437138856228e-30,
+            "angularVelocity": -0.012314572246193395,
+            "velocityX": -0.7765510184161662,
+            "velocityY": -0.28035948126465987,
+            "timestamp": 1.8813247742176853
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 2.219047153199805e-29,
+            "velocityX": 0,
+            "velocityY": -7.552577135588168e-32,
+            "timestamp": 1.9803418675975635
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.1.traj
index 88fa5a42..e08f778f 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.1.traj	
@@ -3,92 +3,92 @@
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
-            "velocityX": 1.6870437677917242e-24,
-            "velocityY": 1.6813073596046228e-23,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.680000027670122,
-            "y": 5.880500005575848,
-            "heading": 3.140000018022814,
-            "angularVelocity": 1.8010003546528393e-7,
-            "velocityX": 2.7650455327562645e-7,
-            "velocityY": 0.8344065481228156,
-            "timestamp": 0.10007112931184436
+            "x": 7.680000027621492,
+            "y": 5.880500005575844,
+            "heading": 1.812927824526906e-8,
+            "angularVelocity": 1.811639213454267e-7,
+            "velocityX": 2.7601859924059844e-7,
+            "velocityY": 0.8344065481228081,
+            "timestamp": 0.10007112931184069
         },
         {
-            "x": 7.680000074983718,
-            "y": 6.047500013629854,
-            "heading": 3.1400000460729856,
-            "angularVelocity": 2.8030233382034975e-7,
-            "velocityX": 4.7279965639225817e-7,
-            "velocityY": 1.6688130652907482,
-            "timestamp": 0.20014225862368873
+            "x": 7.6800000748220105,
+            "y": 6.047500013629843,
+            "heading": 4.634757751496617e-8,
+            "angularVelocity": 2.819824195265022e-7,
+            "velocityX": 4.716696889582397e-7,
+            "velocityY": 1.6688130652907391,
+            "timestamp": 0.20014225862368137
         },
         {
-            "x": 7.680000134184045,
-            "y": 6.298000017966634,
-            "heading": 3.1400000770424787,
-            "angularVelocity": 3.0947480383704467e-7,
-            "velocityX": 5.915824876001831e-7,
-            "velocityY": 2.5032195205488788,
-            "timestamp": 0.3002133879355331
+            "x": 7.68000013381266,
+            "y": 6.298000017966615,
+            "heading": 7.748634725130675e-8,
+            "angularVelocity": 3.1116636502244896e-7,
+            "velocityX": 5.894871979234934e-7,
+            "velocityY": 2.5032195205488934,
+            "timestamp": 0.30021338793552205
         },
         {
-            "x": 7.680000197955803,
-            "y": 6.632000000000009,
-            "heading": 3.1400001051570197,
-            "angularVelocity": 2.8094557567454647e-7,
-            "velocityX": 6.372642912914395e-7,
-            "velocityY": 3.3376257900773374,
-            "timestamp": 0.40028451724737746
+            "x": 7.680000197181065,
+            "y": 6.63200000000002,
+            "heading": 1.0565412902376676e-7,
+            "angularVelocity": 2.814776011952798e-7,
+            "velocityX": 6.332336214323824e-7,
+            "velocityY": 3.3376257900777544,
+            "timestamp": 0.40028451724736275
         },
         {
-            "x": 7.680000129458281,
-            "y": 6.96599998203339,
-            "heading": 3.140000063575334,
-            "angularVelocity": -4.1552129568712905e-7,
-            "velocityX": -6.844883427144649e-7,
-            "velocityY": 3.337625790077381,
-            "timestamp": 0.5003556465592218
+            "x": 7.680000129088366,
+            "y": 6.965999982033409,
+            "heading": 6.403571463499393e-8,
+            "angularVelocity": -4.1588832167210794e-7,
+            "velocityX": -6.804429810396128e-7,
+            "velocityY": 3.337625790077598,
+            "timestamp": 0.5003556465592034
         },
         {
-            "x": 7.680000069924504,
-            "y": 7.216499986370157,
-            "heading": 3.1400000316818817,
-            "angularVelocity": -3.1870782840594973e-7,
-            "velocityX": -5.949146101339814e-7,
-            "velocityY": 2.5032195205487504,
-            "timestamp": 0.6004267758710662
+            "x": 7.680000069764372,
+            "y": 7.216499986370169,
+            "heading": 3.1970340337388056e-8,
+            "angularVelocity": -3.2042582447411e-7,
+            "velocityX": -5.92818279593764e-7,
+            "velocityY": 2.503219520548763,
+            "timestamp": 0.6004267758710441
         },
         {
-            "x": 7.6800000249972635,
-            "y": 7.383499994424155,
-            "heading": 3.14000001043482,
-            "angularVelocity": -2.1231960430791816e-7,
-            "velocityX": -4.4895306909597897e-7,
-            "velocityY": 1.6688130652906694,
-            "timestamp": 0.7004979051829106
+            "x": 7.680000024949458,
+            "y": 7.3834999944241595,
+            "heading": 1.0547562280132128e-8,
+            "angularVelocity": -2.1407550924431962e-7,
+            "velocityX": -4.47830590750479e-7,
+            "velocityY": 1.6688130652906517,
+            "timestamp": 0.7004979051828848
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": -1.0427402507810036e-7,
-            "velocityX": -2.497949654636061e-7,
-            "velocityY": 0.8344065481227817,
-            "timestamp": 0.8005690344947549
+            "heading": -4.103497297298223e-32,
+            "angularVelocity": -1.0540065169911972e-7,
+            "velocityX": -2.4931725183429086e-7,
+            "velocityY": 0.8344065481227665,
+            "timestamp": 0.8005690344947255
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 4.128313998274909e-23,
-            "velocityY": -7.415182378687901e-21,
-            "timestamp": 0.9006401638065993
+            "heading": 0,
+            "angularVelocity": 9.056551022450501e-31,
+            "velocityX": 0,
+            "velocityY": 3.73051624872423e-37,
+            "timestamp": 0.9006401638065662
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.traj
index 88fa5a42..e08f778f 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.traj	
@@ -3,92 +3,92 @@
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
-            "velocityX": 1.6870437677917242e-24,
-            "velocityY": 1.6813073596046228e-23,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.680000027670122,
-            "y": 5.880500005575848,
-            "heading": 3.140000018022814,
-            "angularVelocity": 1.8010003546528393e-7,
-            "velocityX": 2.7650455327562645e-7,
-            "velocityY": 0.8344065481228156,
-            "timestamp": 0.10007112931184436
+            "x": 7.680000027621492,
+            "y": 5.880500005575844,
+            "heading": 1.812927824526906e-8,
+            "angularVelocity": 1.811639213454267e-7,
+            "velocityX": 2.7601859924059844e-7,
+            "velocityY": 0.8344065481228081,
+            "timestamp": 0.10007112931184069
         },
         {
-            "x": 7.680000074983718,
-            "y": 6.047500013629854,
-            "heading": 3.1400000460729856,
-            "angularVelocity": 2.8030233382034975e-7,
-            "velocityX": 4.7279965639225817e-7,
-            "velocityY": 1.6688130652907482,
-            "timestamp": 0.20014225862368873
+            "x": 7.6800000748220105,
+            "y": 6.047500013629843,
+            "heading": 4.634757751496617e-8,
+            "angularVelocity": 2.819824195265022e-7,
+            "velocityX": 4.716696889582397e-7,
+            "velocityY": 1.6688130652907391,
+            "timestamp": 0.20014225862368137
         },
         {
-            "x": 7.680000134184045,
-            "y": 6.298000017966634,
-            "heading": 3.1400000770424787,
-            "angularVelocity": 3.0947480383704467e-7,
-            "velocityX": 5.915824876001831e-7,
-            "velocityY": 2.5032195205488788,
-            "timestamp": 0.3002133879355331
+            "x": 7.68000013381266,
+            "y": 6.298000017966615,
+            "heading": 7.748634725130675e-8,
+            "angularVelocity": 3.1116636502244896e-7,
+            "velocityX": 5.894871979234934e-7,
+            "velocityY": 2.5032195205488934,
+            "timestamp": 0.30021338793552205
         },
         {
-            "x": 7.680000197955803,
-            "y": 6.632000000000009,
-            "heading": 3.1400001051570197,
-            "angularVelocity": 2.8094557567454647e-7,
-            "velocityX": 6.372642912914395e-7,
-            "velocityY": 3.3376257900773374,
-            "timestamp": 0.40028451724737746
+            "x": 7.680000197181065,
+            "y": 6.63200000000002,
+            "heading": 1.0565412902376676e-7,
+            "angularVelocity": 2.814776011952798e-7,
+            "velocityX": 6.332336214323824e-7,
+            "velocityY": 3.3376257900777544,
+            "timestamp": 0.40028451724736275
         },
         {
-            "x": 7.680000129458281,
-            "y": 6.96599998203339,
-            "heading": 3.140000063575334,
-            "angularVelocity": -4.1552129568712905e-7,
-            "velocityX": -6.844883427144649e-7,
-            "velocityY": 3.337625790077381,
-            "timestamp": 0.5003556465592218
+            "x": 7.680000129088366,
+            "y": 6.965999982033409,
+            "heading": 6.403571463499393e-8,
+            "angularVelocity": -4.1588832167210794e-7,
+            "velocityX": -6.804429810396128e-7,
+            "velocityY": 3.337625790077598,
+            "timestamp": 0.5003556465592034
         },
         {
-            "x": 7.680000069924504,
-            "y": 7.216499986370157,
-            "heading": 3.1400000316818817,
-            "angularVelocity": -3.1870782840594973e-7,
-            "velocityX": -5.949146101339814e-7,
-            "velocityY": 2.5032195205487504,
-            "timestamp": 0.6004267758710662
+            "x": 7.680000069764372,
+            "y": 7.216499986370169,
+            "heading": 3.1970340337388056e-8,
+            "angularVelocity": -3.2042582447411e-7,
+            "velocityX": -5.92818279593764e-7,
+            "velocityY": 2.503219520548763,
+            "timestamp": 0.6004267758710441
         },
         {
-            "x": 7.6800000249972635,
-            "y": 7.383499994424155,
-            "heading": 3.14000001043482,
-            "angularVelocity": -2.1231960430791816e-7,
-            "velocityX": -4.4895306909597897e-7,
-            "velocityY": 1.6688130652906694,
-            "timestamp": 0.7004979051829106
+            "x": 7.680000024949458,
+            "y": 7.3834999944241595,
+            "heading": 1.0547562280132128e-8,
+            "angularVelocity": -2.1407550924431962e-7,
+            "velocityX": -4.47830590750479e-7,
+            "velocityY": 1.6688130652906517,
+            "timestamp": 0.7004979051828848
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": -1.0427402507810036e-7,
-            "velocityX": -2.497949654636061e-7,
-            "velocityY": 0.8344065481227817,
-            "timestamp": 0.8005690344947549
+            "heading": -4.103497297298223e-32,
+            "angularVelocity": -1.0540065169911972e-7,
+            "velocityX": -2.4931725183429086e-7,
+            "velocityY": 0.8344065481227665,
+            "timestamp": 0.8005690344947255
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 4.128313998274909e-23,
-            "velocityY": -7.415182378687901e-21,
-            "timestamp": 0.9006401638065993
+            "heading": 0,
+            "angularVelocity": 9.056551022450501e-31,
+            "velocityX": 0,
+            "velocityY": 3.73051624872423e-37,
+            "timestamp": 0.9006401638065662
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.1.traj
new file mode 100644
index 00000000..d4dafa95
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.1.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -8.386874849918624e-34,
+            "velocityY": 2.2609899154097436e-36,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6800000367448975,
+            "y": 5.713499994424246,
+            "heading": -2.1127327949152964e-9,
+            "angularVelocity": -2.1112310428149592e-8,
+            "velocityX": 3.67187799986145e-7,
+            "velocityY": -0.8344065481223435,
+            "timestamp": 0.10007112931179547
+        },
+        {
+            "x": 7.680000101493379,
+            "y": 5.546499986370426,
+            "heading": -1.0421334714876827e-8,
+            "angularVelocity": -8.302696522536156e-8,
+            "velocityX": 6.470245866798498e-7,
+            "velocityY": -1.668813065289814,
+            "timestamp": 0.20014225862359095
+        },
+        {
+            "x": 7.680000185334384,
+            "y": 5.295999982033924,
+            "heading": -2.6903389225477584e-8,
+            "angularVelocity": -1.6470340117310356e-7,
+            "velocityX": 8.378141164132646e-7,
+            "velocityY": -2.503219520547496,
+            "timestamp": 0.30021338793538643
+        },
+        {
+            "x": 7.680000279163236,
+            "y": 4.961999999999876,
+            "heading": -5.074281043975999e-8,
+            "angularVelocity": -2.3822478681802103e-7,
+            "velocityX": 9.376216058628794e-7,
+            "velocityY": -3.3376257900847826,
+            "timestamp": 0.4002845172471819
+        },
+        {
+            "x": 7.680000186570606,
+            "y": 4.628000017966085,
+            "heading": -3.213613801692531e-8,
+            "angularVelocity": 1.8593453255418192e-7,
+            "velocityX": -9.252681679909059e-7,
+            "velocityY": -3.3376257900823116,
+            "timestamp": 0.5003556465589774
+        },
+        {
+            "x": 7.68000010282861,
+            "y": 4.377500013629577,
+            "heading": -1.6413785806619038e-8,
+            "angularVelocity": 1.5711175564873292e-7,
+            "velocityX": -8.368247227344002e-7,
+            "velocityY": -2.5032195205475483,
+            "timestamp": 0.6004267758707729
+        },
+        {
+            "x": 7.6800000374582895,
+            "y": 4.2105000055757555,
+            "heading": -5.494810627011176e-9,
+            "angularVelocity": 1.0911213201504113e-7,
+            "velocityX": -6.532385577861589e-7,
+            "velocityY": -1.668813065289845,
+            "timestamp": 0.7004979051825684
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 5.4909043426679847e-8,
+            "velocityX": -3.743166510876222e-7,
+            "velocityY": -0.8344065481223572,
+            "timestamp": 0.8005690344943639
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 1.4758845853352304e-34,
+            "velocityY": 2.7202161180895526e-29,
+            "timestamp": 0.9006401638061594
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.traj
new file mode 100644
index 00000000..d4dafa95
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -8.386874849918624e-34,
+            "velocityY": 2.2609899154097436e-36,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6800000367448975,
+            "y": 5.713499994424246,
+            "heading": -2.1127327949152964e-9,
+            "angularVelocity": -2.1112310428149592e-8,
+            "velocityX": 3.67187799986145e-7,
+            "velocityY": -0.8344065481223435,
+            "timestamp": 0.10007112931179547
+        },
+        {
+            "x": 7.680000101493379,
+            "y": 5.546499986370426,
+            "heading": -1.0421334714876827e-8,
+            "angularVelocity": -8.302696522536156e-8,
+            "velocityX": 6.470245866798498e-7,
+            "velocityY": -1.668813065289814,
+            "timestamp": 0.20014225862359095
+        },
+        {
+            "x": 7.680000185334384,
+            "y": 5.295999982033924,
+            "heading": -2.6903389225477584e-8,
+            "angularVelocity": -1.6470340117310356e-7,
+            "velocityX": 8.378141164132646e-7,
+            "velocityY": -2.503219520547496,
+            "timestamp": 0.30021338793538643
+        },
+        {
+            "x": 7.680000279163236,
+            "y": 4.961999999999876,
+            "heading": -5.074281043975999e-8,
+            "angularVelocity": -2.3822478681802103e-7,
+            "velocityX": 9.376216058628794e-7,
+            "velocityY": -3.3376257900847826,
+            "timestamp": 0.4002845172471819
+        },
+        {
+            "x": 7.680000186570606,
+            "y": 4.628000017966085,
+            "heading": -3.213613801692531e-8,
+            "angularVelocity": 1.8593453255418192e-7,
+            "velocityX": -9.252681679909059e-7,
+            "velocityY": -3.3376257900823116,
+            "timestamp": 0.5003556465589774
+        },
+        {
+            "x": 7.68000010282861,
+            "y": 4.377500013629577,
+            "heading": -1.6413785806619038e-8,
+            "angularVelocity": 1.5711175564873292e-7,
+            "velocityX": -8.368247227344002e-7,
+            "velocityY": -2.5032195205475483,
+            "timestamp": 0.6004267758707729
+        },
+        {
+            "x": 7.6800000374582895,
+            "y": 4.2105000055757555,
+            "heading": -5.494810627011176e-9,
+            "angularVelocity": 1.0911213201504113e-7,
+            "velocityX": -6.532385577861589e-7,
+            "velocityY": -1.668813065289845,
+            "timestamp": 0.7004979051825684
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 5.4909043426679847e-8,
+            "velocityX": -3.743166510876222e-7,
+            "velocityY": -0.8344065481223572,
+            "timestamp": 0.8005690344943639
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 1.4758845853352304e-34,
+            "velocityY": 2.7202161180895526e-29,
+            "timestamp": 0.9006401638061594
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.1.traj
index 46b2cfe7..be761719 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.1.traj	
@@ -3,136 +3,136 @@
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": -1.429570475254817e-33,
-            "velocityX": -4.923067503198675e-33,
-            "velocityY": 6.357653017323465e-38,
+            "heading": 0,
+            "angularVelocity": -3.3479093461707414e-32,
+            "velocityX": 0,
+            "velocityY": 6.414063862178313e-32,
             "timestamp": 0
         },
         {
-            "x": 7.680000074935176,
-            "y": 5.7205546279944945,
-            "heading": 3.1400000039815823,
-            "angularVelocity": 4.158287610282007e-8,
-            "velocityX": 7.826085092728229e-7,
-            "velocityY": -0.7983807350943761,
-            "timestamp": 0.09575052183150115
+            "x": 7.680000074936813,
+            "y": 5.720554627994495,
+            "heading": 3.835783864206662e-9,
+            "angularVelocity": 4.006018550688398e-8,
+            "velocityX": 7.826256085285986e-7,
+            "velocityY": -0.7983807350943762,
+            "timestamp": 0.09575052183150116
         },
         {
-            "x": 7.68000021698637,
-            "y": 5.567663887017397,
-            "heading": 3.1400000079801687,
-            "angularVelocity": 4.176046399562833e-8,
-            "velocityX": 0.000001483555234342966,
-            "velocityY": -1.5967614385031572,
-            "timestamp": 0.1915010436630023
+            "x": 7.680000216991337,
+            "y": 5.567663887017398,
+            "heading": 7.581387411185645e-9,
+            "angularVelocity": 3.911835760311887e-8,
+            "velocityX": 0.0000014835900057043023,
+            "velocityY": -1.5967614385031577,
+            "timestamp": 0.19150104366300233
         },
         {
-            "x": 7.680000418191477,
+            "x": 7.68000041820133,
             "y": 5.338327783136532,
-            "heading": 3.140000009761107,
-            "angularVelocity": 1.8599774209907478e-8,
-            "velocityX": 0.000002101347243657271,
-            "velocityY": -2.395142078540755,
+            "heading": 9.040192056984193e-9,
+            "angularVelocity": 1.523546877140865e-8,
+            "velocityX": 0.0000021013982858961807,
+            "velocityY": -2.3951420785407556,
             "timestamp": 0.2872515654945035
         },
         {
-            "x": 7.680000670419164,
-            "y": 5.032546334555315,
-            "heading": 3.1400000093900653,
-            "angularVelocity": -3.875097409582088e-9,
-            "velocityX": 0.000002634217150547891,
-            "velocityY": -3.193522528465398,
-            "timestamp": 0.3830020873260046
+            "x": 7.680000670435067,
+            "y": 5.032546334555316,
+            "heading": 8.315274931561148e-9,
+            "angularVelocity": -7.570901836021169e-9,
+            "velocityX": 0.000002634280323205928,
+            "velocityY": -3.193522528465399,
+            "timestamp": 0.38300208732600466
         },
         {
-            "x": 7.68000083273763,
-            "y": 4.670327800733128,
-            "heading": 3.1400000093900697,
-            "angularVelocity": 4.937002430391119e-14,
-            "velocityX": 0.000001695223016776456,
-            "velocityY": -3.7829405719543154,
-            "timestamp": 0.4787526091575057
+            "x": 7.680000832750872,
+            "y": 4.67032780073313,
+            "heading": 8.315279560400229e-9,
+            "angularVelocity": 4.9454766224662506e-14,
+            "velocityX": 0.0000016951952366761453,
+            "velocityY": -3.782940571954316,
+            "timestamp": 0.47875260915750584
         },
         {
-            "x": 7.6800009141474845,
-            "y": 4.308109266911048,
-            "heading": 3.1400000093900733,
-            "angularVelocity": 3.662638141198336e-14,
-            "velocityX": 8.50228817967247e-7,
+            "x": 7.680000914152323,
+            "y": 4.308109266911049,
+            "heading": 8.315282864092028e-9,
+            "angularVelocity": 3.631070768427552e-14,
+            "velocityX": 8.501410582319702e-7,
             "velocityY": -3.782940571953249,
-            "timestamp": 0.5745031309890068
+            "timestamp": 0.574503130989007
         },
         {
-            "x": 7.680000914400225,
-            "y": 3.9458907330889583,
-            "heading": 3.1400000093900764,
-            "angularVelocity": 3.286235025790432e-14,
-            "velocityX": 2.639571649715247e-9,
+            "x": 7.680000914394957,
+            "y": 3.9458907330889588,
+            "heading": 8.315285941661222e-9,
+            "angularVelocity": 3.388658702074781e-14,
+            "velocityX": 2.5340105811348506e-9,
             "velocityY": -3.7829405719533424,
-            "timestamp": 0.670253652820508
+            "timestamp": 0.6702536528205081
         },
         {
-            "x": 7.680000833444056,
-            "y": 3.5836721992668767,
-            "heading": 3.140000009390079,
-            "angularVelocity": 2.6535816526961207e-14,
-            "velocityX": -8.454906188658589e-7,
+            "x": 7.680000833430653,
+            "y": 3.583672199266877,
+            "heading": 8.315288291957849e-9,
+            "angularVelocity": 2.592873274060122e-14,
+            "velocityX": -8.455755806508837e-7,
             "velocityY": -3.78294057195325,
-            "timestamp": 0.7660041746520091
+            "timestamp": 0.7660041746520092
         },
         {
-            "x": 7.680000671408071,
-            "y": 3.221453665444688,
-            "heading": 3.140000009390085,
-            "angularVelocity": 5.86589821918151e-14,
-            "velocityX": -0.0000016922728517221796,
-            "velocityY": -3.7829405719543456,
-            "timestamp": 0.8617546964835102
+            "x": 7.6800006713921105,
+            "y": 3.2214536654446886,
+            "heading": 8.315293718879221e-9,
+            "angularVelocity": 5.742758811630884e-14,
+            "velocityX": -0.0000016922995574745742,
+            "velocityY": -3.782940571954345,
+            "timestamp": 0.8617546964835103
         },
         {
-            "x": 7.680000418877801,
-            "y": 2.9156722168634683,
-            "heading": 3.140000001402919,
-            "angularVelocity": -8.341641379215641e-8,
-            "velocityX": -0.000002637377255306636,
-            "velocityY": -3.193522528465426,
-            "timestamp": 0.9575052183150113
+            "x": 7.680000418867919,
+            "y": 2.915672216863469,
+            "heading": 6.817552183291689e-10,
+            "angularVelocity": -7.972319645162058e-8,
+            "velocityX": -0.000002637313779217548,
+            "velocityY": -3.1935225284654263,
+            "timestamp": 0.9575052183150115
         },
         {
-            "x": 7.680000217377681,
-            "y": 2.686336112982602,
-            "heading": 3.139999997550481,
-            "angularVelocity": -4.023411698701823e-8,
-            "velocityX": -0.0000021044283112699676,
-            "velocityY": -2.395142078540766,
+            "x": 7.680000217372703,
+            "y": 2.6863361129826027,
+            "heading": -2.8485300950885154e-9,
+            "angularVelocity": -3.6869616762863894e-8,
+            "velocityX": -0.00000210437708878744,
+            "velocityY": -2.395142078540767,
             "timestamp": 1.0532557401465126
         },
         {
-            "x": 7.680000075082089,
-            "y": 2.5334453720055046,
-            "heading": 3.139999997845116,
-            "angularVelocity": 3.0771112395983046e-9,
-            "velocityX": -0.0000014861076817541395,
-            "velocityY": -1.5967614385031628,
+            "x": 7.680000075080448,
+            "y": 2.533445372005505,
+            "heading": -2.300788754937178e-9,
+            "angularVelocity": 5.720505703316253e-9,
+            "velocityX": -0.0000014860728228146123,
+            "velocityY": -1.596761438503163,
             "timestamp": 1.1490062619780137
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 2.250519944152171e-8,
-            "velocityX": -7.841428415253779e-7,
-            "velocityY": -0.7983807350943782,
+            "heading": 0,
+            "angularVelocity": 2.4028994457396085e-8,
+            "velocityX": -7.841257148612366e-7,
+            "velocityY": -0.7983807350943783,
             "timestamp": 1.2447567838095148
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 5.079076909387901e-38,
+            "velocityX": -1.5050925658054699e-33,
+            "velocityY": -8.233479718145206e-39,
             "timestamp": 1.340507305641016
         }
     ]
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.traj
index 46b2cfe7..be761719 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.traj	
@@ -3,136 +3,136 @@
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": -1.429570475254817e-33,
-            "velocityX": -4.923067503198675e-33,
-            "velocityY": 6.357653017323465e-38,
+            "heading": 0,
+            "angularVelocity": -3.3479093461707414e-32,
+            "velocityX": 0,
+            "velocityY": 6.414063862178313e-32,
             "timestamp": 0
         },
         {
-            "x": 7.680000074935176,
-            "y": 5.7205546279944945,
-            "heading": 3.1400000039815823,
-            "angularVelocity": 4.158287610282007e-8,
-            "velocityX": 7.826085092728229e-7,
-            "velocityY": -0.7983807350943761,
-            "timestamp": 0.09575052183150115
+            "x": 7.680000074936813,
+            "y": 5.720554627994495,
+            "heading": 3.835783864206662e-9,
+            "angularVelocity": 4.006018550688398e-8,
+            "velocityX": 7.826256085285986e-7,
+            "velocityY": -0.7983807350943762,
+            "timestamp": 0.09575052183150116
         },
         {
-            "x": 7.68000021698637,
-            "y": 5.567663887017397,
-            "heading": 3.1400000079801687,
-            "angularVelocity": 4.176046399562833e-8,
-            "velocityX": 0.000001483555234342966,
-            "velocityY": -1.5967614385031572,
-            "timestamp": 0.1915010436630023
+            "x": 7.680000216991337,
+            "y": 5.567663887017398,
+            "heading": 7.581387411185645e-9,
+            "angularVelocity": 3.911835760311887e-8,
+            "velocityX": 0.0000014835900057043023,
+            "velocityY": -1.5967614385031577,
+            "timestamp": 0.19150104366300233
         },
         {
-            "x": 7.680000418191477,
+            "x": 7.68000041820133,
             "y": 5.338327783136532,
-            "heading": 3.140000009761107,
-            "angularVelocity": 1.8599774209907478e-8,
-            "velocityX": 0.000002101347243657271,
-            "velocityY": -2.395142078540755,
+            "heading": 9.040192056984193e-9,
+            "angularVelocity": 1.523546877140865e-8,
+            "velocityX": 0.0000021013982858961807,
+            "velocityY": -2.3951420785407556,
             "timestamp": 0.2872515654945035
         },
         {
-            "x": 7.680000670419164,
-            "y": 5.032546334555315,
-            "heading": 3.1400000093900653,
-            "angularVelocity": -3.875097409582088e-9,
-            "velocityX": 0.000002634217150547891,
-            "velocityY": -3.193522528465398,
-            "timestamp": 0.3830020873260046
+            "x": 7.680000670435067,
+            "y": 5.032546334555316,
+            "heading": 8.315274931561148e-9,
+            "angularVelocity": -7.570901836021169e-9,
+            "velocityX": 0.000002634280323205928,
+            "velocityY": -3.193522528465399,
+            "timestamp": 0.38300208732600466
         },
         {
-            "x": 7.68000083273763,
-            "y": 4.670327800733128,
-            "heading": 3.1400000093900697,
-            "angularVelocity": 4.937002430391119e-14,
-            "velocityX": 0.000001695223016776456,
-            "velocityY": -3.7829405719543154,
-            "timestamp": 0.4787526091575057
+            "x": 7.680000832750872,
+            "y": 4.67032780073313,
+            "heading": 8.315279560400229e-9,
+            "angularVelocity": 4.9454766224662506e-14,
+            "velocityX": 0.0000016951952366761453,
+            "velocityY": -3.782940571954316,
+            "timestamp": 0.47875260915750584
         },
         {
-            "x": 7.6800009141474845,
-            "y": 4.308109266911048,
-            "heading": 3.1400000093900733,
-            "angularVelocity": 3.662638141198336e-14,
-            "velocityX": 8.50228817967247e-7,
+            "x": 7.680000914152323,
+            "y": 4.308109266911049,
+            "heading": 8.315282864092028e-9,
+            "angularVelocity": 3.631070768427552e-14,
+            "velocityX": 8.501410582319702e-7,
             "velocityY": -3.782940571953249,
-            "timestamp": 0.5745031309890068
+            "timestamp": 0.574503130989007
         },
         {
-            "x": 7.680000914400225,
-            "y": 3.9458907330889583,
-            "heading": 3.1400000093900764,
-            "angularVelocity": 3.286235025790432e-14,
-            "velocityX": 2.639571649715247e-9,
+            "x": 7.680000914394957,
+            "y": 3.9458907330889588,
+            "heading": 8.315285941661222e-9,
+            "angularVelocity": 3.388658702074781e-14,
+            "velocityX": 2.5340105811348506e-9,
             "velocityY": -3.7829405719533424,
-            "timestamp": 0.670253652820508
+            "timestamp": 0.6702536528205081
         },
         {
-            "x": 7.680000833444056,
-            "y": 3.5836721992668767,
-            "heading": 3.140000009390079,
-            "angularVelocity": 2.6535816526961207e-14,
-            "velocityX": -8.454906188658589e-7,
+            "x": 7.680000833430653,
+            "y": 3.583672199266877,
+            "heading": 8.315288291957849e-9,
+            "angularVelocity": 2.592873274060122e-14,
+            "velocityX": -8.455755806508837e-7,
             "velocityY": -3.78294057195325,
-            "timestamp": 0.7660041746520091
+            "timestamp": 0.7660041746520092
         },
         {
-            "x": 7.680000671408071,
-            "y": 3.221453665444688,
-            "heading": 3.140000009390085,
-            "angularVelocity": 5.86589821918151e-14,
-            "velocityX": -0.0000016922728517221796,
-            "velocityY": -3.7829405719543456,
-            "timestamp": 0.8617546964835102
+            "x": 7.6800006713921105,
+            "y": 3.2214536654446886,
+            "heading": 8.315293718879221e-9,
+            "angularVelocity": 5.742758811630884e-14,
+            "velocityX": -0.0000016922995574745742,
+            "velocityY": -3.782940571954345,
+            "timestamp": 0.8617546964835103
         },
         {
-            "x": 7.680000418877801,
-            "y": 2.9156722168634683,
-            "heading": 3.140000001402919,
-            "angularVelocity": -8.341641379215641e-8,
-            "velocityX": -0.000002637377255306636,
-            "velocityY": -3.193522528465426,
-            "timestamp": 0.9575052183150113
+            "x": 7.680000418867919,
+            "y": 2.915672216863469,
+            "heading": 6.817552183291689e-10,
+            "angularVelocity": -7.972319645162058e-8,
+            "velocityX": -0.000002637313779217548,
+            "velocityY": -3.1935225284654263,
+            "timestamp": 0.9575052183150115
         },
         {
-            "x": 7.680000217377681,
-            "y": 2.686336112982602,
-            "heading": 3.139999997550481,
-            "angularVelocity": -4.023411698701823e-8,
-            "velocityX": -0.0000021044283112699676,
-            "velocityY": -2.395142078540766,
+            "x": 7.680000217372703,
+            "y": 2.6863361129826027,
+            "heading": -2.8485300950885154e-9,
+            "angularVelocity": -3.6869616762863894e-8,
+            "velocityX": -0.00000210437708878744,
+            "velocityY": -2.395142078540767,
             "timestamp": 1.0532557401465126
         },
         {
-            "x": 7.680000075082089,
-            "y": 2.5334453720055046,
-            "heading": 3.139999997845116,
-            "angularVelocity": 3.0771112395983046e-9,
-            "velocityX": -0.0000014861076817541395,
-            "velocityY": -1.5967614385031628,
+            "x": 7.680000075080448,
+            "y": 2.533445372005505,
+            "heading": -2.300788754937178e-9,
+            "angularVelocity": 5.720505703316253e-9,
+            "velocityX": -0.0000014860728228146123,
+            "velocityY": -1.596761438503163,
             "timestamp": 1.1490062619780137
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 2.250519944152171e-8,
-            "velocityX": -7.841428415253779e-7,
-            "velocityY": -0.7983807350943782,
+            "heading": 0,
+            "angularVelocity": 2.4028994457396085e-8,
+            "velocityX": -7.841257148612366e-7,
+            "velocityY": -0.7983807350943783,
             "timestamp": 1.2447567838095148
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 5.079076909387901e-38,
+            "velocityX": -1.5050925658054699e-33,
+            "velocityY": -8.233479718145206e-39,
             "timestamp": 1.340507305641016
         }
     ]
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.1.traj
index 8470d032..df939479 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.1.traj	
@@ -3,173 +3,173 @@
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": -8.977040531488188e-32,
-            "velocityX": -1.4835352698294527e-33,
-            "velocityY": -6.682715069575163e-38,
+            "heading": 0,
+            "angularVelocity": 1.5719252729433342e-31,
+            "velocityX": 9.448616775755806e-32,
+            "velocityY": 5.840312874632397e-38,
             "timestamp": 0
         },
         {
-            "x": 7.680000115631693,
+            "x": 7.680000115633079,
             "y": 5.715155846048254,
-            "heading": 3.139999983275777,
-            "angularVelocity": -1.688054668223312e-7,
-            "velocityX": 0.0000011671252822265972,
-            "velocityY": -0.8260917454273096,
-            "timestamp": 0.09907392781101573
+            "heading": -1.667708661616254e-8,
+            "angularVelocity": -1.683296946208156e-7,
+            "velocityX": 0.0000011671392662331341,
+            "velocityY": -0.8260917454273093,
+            "timestamp": 0.0990739278110157
         },
         {
-            "x": 7.680000338287477,
+            "x": 7.68000033829167,
             "y": 5.551467541258106,
-            "heading": 3.1399999456936927,
-            "angularVelocity": -3.793336766253788e-7,
-            "velocityX": 0.0000022473699405935927,
+            "heading": -5.4122540412155035e-8,
+            "angularVelocity": -3.7795460569235657e-7,
+            "velocityX": 0.0000022473982686017914,
             "velocityY": -1.6521834594301872,
-            "timestamp": 0.19814785562203147
+            "timestamp": 0.1981478556220314
         },
         {
-            "x": 7.680000659193895,
+            "x": 7.680000659202128,
             "y": 5.305935091856244,
-            "heading": 3.1399998851536046,
-            "angularVelocity": -6.110596046233074e-7,
-            "velocityX": 0.0000032390599337968516,
+            "heading": -1.1439481389604968e-7,
+            "angularVelocity": -6.083564268701089e-7,
+            "velocityX": 0.000003239100713146307,
             "velocityY": -2.4782751105841556,
-            "timestamp": 0.29722178343304717
+            "timestamp": 0.2972217834330471
         },
         {
-            "x": 7.680001069383311,
-            "y": 4.978558516522714,
-            "heading": 3.1399998022537425,
-            "angularVelocity": -8.367472472020738e-7,
-            "velocityX": 0.0000041402352562216615,
-            "velocityY": -3.3043665731916145,
-            "timestamp": 0.39629571124406293
+            "x": 7.680001069396323,
+            "y": 4.978558516522715,
+            "heading": -1.9685469205745493e-7,
+            "angularVelocity": -8.323062817894511e-7,
+            "velocityX": 0.000004140283484089047,
+            "velocityY": -3.304366573191614,
+            "timestamp": 0.3962957112440628
         },
         {
-            "x": 7.680001393532974,
+            "x": 7.680001393540419,
             "y": 4.603767735073282,
-            "heading": 3.1399998022537403,
-            "angularVelocity": -2.4397007486697484e-14,
-            "velocityX": 0.0000032717959136681545,
+            "heading": -1.9685469402018675e-7,
+            "angularVelocity": -2.3695928725495928e-14,
+            "velocityX": 0.0000032717397250257404,
             "velocityY": -3.782940575085891,
-            "timestamp": 0.4953696390550787
+            "timestamp": 0.4953696390550785
         },
         {
-            "x": 7.680001631666853,
-            "y": 4.228976953623769,
-            "heading": 3.1399998022537345,
-            "angularVelocity": -6.162047103580494e-14,
-            "velocityX": 0.000002403597704835693,
-            "velocityY": -3.7829405750867746,
-            "timestamp": 0.5944435668660945
+            "x": 7.680001631663456,
+            "y": 4.228976953623768,
+            "heading": -1.9685469983118006e-7,
+            "angularVelocity": -6.145037083886684e-14,
+            "velocityX": 0.0000024034882723677366,
+            "velocityY": -3.782940575086774,
+            "timestamp": 0.5944435668660942
         },
         {
-            "x": 7.68000178361998,
-            "y": 3.854186172174224,
-            "heading": 3.139999802253728,
-            "angularVelocity": -7.021738392770843e-14,
-            "velocityX": 0.0000015337346806740877,
+            "x": 7.680001783604786,
+            "y": 3.854186172174223,
+            "heading": -1.9685470647072664e-7,
+            "angularVelocity": -7.008912946454424e-14,
+            "velocityX": 0.0000015336156103104506,
             "velocityY": -3.78294057508712,
-            "timestamp": 0.6935174946771102
+            "timestamp": 0.6935174946771099
         },
         {
-            "x": 7.680001849334011,
-            "y": 3.4793953907246618,
-            "heading": 3.1399998022537208,
-            "angularVelocity": -7.676689036353258e-14,
-            "velocityX": 6.632827646851356e-7,
+            "x": 7.680001849310107,
+            "y": 3.479395390724661,
+            "heading": -1.9685471357202238e-7,
+            "angularVelocity": -7.568241713719523e-14,
+            "velocityX": 6.631948562175283e-7,
             "velocityY": -3.782940575087319,
-            "timestamp": 0.792591422488126
+            "timestamp": 0.7925914224881256
         },
         {
-            "x": 7.680001829174184,
-            "y": 3.104604609275094,
-            "heading": 3.139999802253714,
-            "angularVelocity": -7.698791271549556e-14,
-            "velocityX": -2.0348261115822963e-7,
+            "x": 7.680001829148679,
+            "y": 3.104604609275093,
+            "heading": -1.968547206040393e-7,
+            "angularVelocity": -7.70206577850091e-14,
+            "velocityX": -2.0349878104492754e-7,
             "velocityY": -3.7829405750873777,
-            "timestamp": 0.8916653502991417
+            "timestamp": 0.8916653502991414
         },
         {
-            "x": 7.680001725145299,
-            "y": 2.729813827825533,
-            "heading": 3.1399998022537057,
-            "angularVelocity": -9.316120157306394e-14,
-            "velocityX": -0.000001050012614805773,
-            "velocityY": -3.7829405750872973,
-            "timestamp": 0.9907392781101575
+            "x": 7.680001725125185,
+            "y": 2.729813827825532,
+            "heading": -1.968547288641082e-7,
+            "angularVelocity": -9.134372064218233e-14,
+            "velocityX": -0.0000010499582046587897,
+            "velocityY": -3.7829405750872978,
+            "timestamp": 0.9907392781101572
         },
         {
-            "x": 7.680001540841657,
-            "y": 2.355023046375991,
-            "heading": 3.1399998022536986,
-            "angularVelocity": -7.445499842222973e-14,
-            "velocityX": -0.0000018602636449128091,
+            "x": 7.680001540827678,
+            "y": 2.3550230463759907,
+            "heading": -1.9685473554016492e-7,
+            "angularVelocity": -7.335754268551387e-14,
+            "velocityX": -0.0000018602017039855543,
             "velocityY": -3.7829405750870753,
-            "timestamp": 1.0898132059211731
+            "timestamp": 1.089813205921173
         },
         {
-            "x": 7.680001281423707,
-            "y": 1.9802322649264825,
-            "heading": 3.139999802253698,
-            "angularVelocity": -7.093550187375142e-15,
-            "velocityX": -0.000002618427817479533,
+            "x": 7.680001281411145,
+            "y": 1.980232264926482,
+            "heading": -1.9685473620018866e-7,
+            "angularVelocity": -6.8770489517299035e-15,
+            "velocityX": -0.0000026184135239642236,
             "velocityY": -3.782940575086706,
-            "timestamp": 1.188887133732189
+            "timestamp": 1.1888871337321887
         },
         {
-            "x": 7.680000953984531,
-            "y": 1.6054414834770696,
-            "heading": 3.139999802253699,
-            "angularVelocity": 1.1174094242032294e-14,
-            "velocityX": -0.0000033049984216512085,
-            "velocityY": -3.782940575085678,
-            "timestamp": 1.2879610615432047
+            "x": 7.680000953971803,
+            "y": 1.6054414834770692,
+            "heading": -1.968547355090094e-7,
+            "angularVelocity": 1.0827757762815176e-14,
+            "velocityX": -0.000003305000098197799,
+            "velocityY": -3.7829405750856786,
+            "timestamp": 1.2879610615432044
         },
         {
-            "x": 7.680000583113676,
-            "y": 1.2780649081436686,
-            "heading": 3.139999880388909,
-            "angularVelocity": 7.886553669648788e-7,
-            "velocityX": -0.0000037433746003660557,
-            "velocityY": -3.304366573190331,
-            "timestamp": 1.3870349893542204
+            "x": 7.6800005831051426,
+            "y": 1.2780649081436681,
+            "heading": -1.1900708358832985e-7,
+            "angularVelocity": 7.857529072428817e-7,
+            "velocityX": -0.0000037433322544868336,
+            "velocityY": -3.30436657319033,
+            "timestamp": 1.3870349893542202
         },
         {
-            "x": 7.6800002967542085,
+            "x": 7.680000296749478,
             "y": 1.0325324587418607,
-            "heading": 3.139999940073013,
-            "angularVelocity": 6.024197635453253e-7,
-            "velocityX": -0.0000028903613408431902,
-            "velocityY": -2.47827511058363,
-            "timestamp": 1.4861089171652362
+            "heading": -5.959102520162259e-8,
+            "angularVelocity": 5.997142524637507e-7,
+            "velocityX": -0.000002890322961269868,
+            "velocityY": -2.478275110583629,
+            "timestamp": 1.486108917165236
         },
         {
-            "x": 7.680000100606573,
+            "x": 7.680000100604838,
             "y": 0.8688441539517371,
-            "heading": 3.1399999800752636,
-            "angularVelocity": 4.0376157345960537e-7,
-            "velocityX": -0.000001979810751989867,
-            "velocityY": -1.6521834594299558,
-            "timestamp": 1.585182844976252
+            "heading": -1.9801599569632452e-8,
+            "angularVelocity": 4.016134309966653e-7,
+            "velocityX": -0.000001979780510399852,
+            "velocityY": -1.6521834594299551,
+            "timestamp": 1.5851828449762517
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 2.0110976517179292e-7,
-            "velocityX": -0.00000101546967480447,
-            "velocityY": -0.8260917454272348,
-            "timestamp": 1.6842567727872677
+            "heading": -3.727510081119492e-33,
+            "angularVelocity": 1.998668869584927e-7,
+            "velocityX": -0.0000010154521677217573,
+            "velocityY": -0.8260917454272346,
+            "timestamp": 1.6842567727872675
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": -8.944233053536814e-32,
+            "heading": 0,
+            "angularVelocity": 1.3621739357478042e-33,
             "velocityX": 0,
-            "velocityY": -1.1208522306082827e-37,
-            "timestamp": 1.7833307005982835
+            "velocityY": 1.5851260517441878e-39,
+            "timestamp": 1.7833307005982832
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.traj
index 8470d032..df939479 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.traj	
@@ -3,173 +3,173 @@
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": -8.977040531488188e-32,
-            "velocityX": -1.4835352698294527e-33,
-            "velocityY": -6.682715069575163e-38,
+            "heading": 0,
+            "angularVelocity": 1.5719252729433342e-31,
+            "velocityX": 9.448616775755806e-32,
+            "velocityY": 5.840312874632397e-38,
             "timestamp": 0
         },
         {
-            "x": 7.680000115631693,
+            "x": 7.680000115633079,
             "y": 5.715155846048254,
-            "heading": 3.139999983275777,
-            "angularVelocity": -1.688054668223312e-7,
-            "velocityX": 0.0000011671252822265972,
-            "velocityY": -0.8260917454273096,
-            "timestamp": 0.09907392781101573
+            "heading": -1.667708661616254e-8,
+            "angularVelocity": -1.683296946208156e-7,
+            "velocityX": 0.0000011671392662331341,
+            "velocityY": -0.8260917454273093,
+            "timestamp": 0.0990739278110157
         },
         {
-            "x": 7.680000338287477,
+            "x": 7.68000033829167,
             "y": 5.551467541258106,
-            "heading": 3.1399999456936927,
-            "angularVelocity": -3.793336766253788e-7,
-            "velocityX": 0.0000022473699405935927,
+            "heading": -5.4122540412155035e-8,
+            "angularVelocity": -3.7795460569235657e-7,
+            "velocityX": 0.0000022473982686017914,
             "velocityY": -1.6521834594301872,
-            "timestamp": 0.19814785562203147
+            "timestamp": 0.1981478556220314
         },
         {
-            "x": 7.680000659193895,
+            "x": 7.680000659202128,
             "y": 5.305935091856244,
-            "heading": 3.1399998851536046,
-            "angularVelocity": -6.110596046233074e-7,
-            "velocityX": 0.0000032390599337968516,
+            "heading": -1.1439481389604968e-7,
+            "angularVelocity": -6.083564268701089e-7,
+            "velocityX": 0.000003239100713146307,
             "velocityY": -2.4782751105841556,
-            "timestamp": 0.29722178343304717
+            "timestamp": 0.2972217834330471
         },
         {
-            "x": 7.680001069383311,
-            "y": 4.978558516522714,
-            "heading": 3.1399998022537425,
-            "angularVelocity": -8.367472472020738e-7,
-            "velocityX": 0.0000041402352562216615,
-            "velocityY": -3.3043665731916145,
-            "timestamp": 0.39629571124406293
+            "x": 7.680001069396323,
+            "y": 4.978558516522715,
+            "heading": -1.9685469205745493e-7,
+            "angularVelocity": -8.323062817894511e-7,
+            "velocityX": 0.000004140283484089047,
+            "velocityY": -3.304366573191614,
+            "timestamp": 0.3962957112440628
         },
         {
-            "x": 7.680001393532974,
+            "x": 7.680001393540419,
             "y": 4.603767735073282,
-            "heading": 3.1399998022537403,
-            "angularVelocity": -2.4397007486697484e-14,
-            "velocityX": 0.0000032717959136681545,
+            "heading": -1.9685469402018675e-7,
+            "angularVelocity": -2.3695928725495928e-14,
+            "velocityX": 0.0000032717397250257404,
             "velocityY": -3.782940575085891,
-            "timestamp": 0.4953696390550787
+            "timestamp": 0.4953696390550785
         },
         {
-            "x": 7.680001631666853,
-            "y": 4.228976953623769,
-            "heading": 3.1399998022537345,
-            "angularVelocity": -6.162047103580494e-14,
-            "velocityX": 0.000002403597704835693,
-            "velocityY": -3.7829405750867746,
-            "timestamp": 0.5944435668660945
+            "x": 7.680001631663456,
+            "y": 4.228976953623768,
+            "heading": -1.9685469983118006e-7,
+            "angularVelocity": -6.145037083886684e-14,
+            "velocityX": 0.0000024034882723677366,
+            "velocityY": -3.782940575086774,
+            "timestamp": 0.5944435668660942
         },
         {
-            "x": 7.68000178361998,
-            "y": 3.854186172174224,
-            "heading": 3.139999802253728,
-            "angularVelocity": -7.021738392770843e-14,
-            "velocityX": 0.0000015337346806740877,
+            "x": 7.680001783604786,
+            "y": 3.854186172174223,
+            "heading": -1.9685470647072664e-7,
+            "angularVelocity": -7.008912946454424e-14,
+            "velocityX": 0.0000015336156103104506,
             "velocityY": -3.78294057508712,
-            "timestamp": 0.6935174946771102
+            "timestamp": 0.6935174946771099
         },
         {
-            "x": 7.680001849334011,
-            "y": 3.4793953907246618,
-            "heading": 3.1399998022537208,
-            "angularVelocity": -7.676689036353258e-14,
-            "velocityX": 6.632827646851356e-7,
+            "x": 7.680001849310107,
+            "y": 3.479395390724661,
+            "heading": -1.9685471357202238e-7,
+            "angularVelocity": -7.568241713719523e-14,
+            "velocityX": 6.631948562175283e-7,
             "velocityY": -3.782940575087319,
-            "timestamp": 0.792591422488126
+            "timestamp": 0.7925914224881256
         },
         {
-            "x": 7.680001829174184,
-            "y": 3.104604609275094,
-            "heading": 3.139999802253714,
-            "angularVelocity": -7.698791271549556e-14,
-            "velocityX": -2.0348261115822963e-7,
+            "x": 7.680001829148679,
+            "y": 3.104604609275093,
+            "heading": -1.968547206040393e-7,
+            "angularVelocity": -7.70206577850091e-14,
+            "velocityX": -2.0349878104492754e-7,
             "velocityY": -3.7829405750873777,
-            "timestamp": 0.8916653502991417
+            "timestamp": 0.8916653502991414
         },
         {
-            "x": 7.680001725145299,
-            "y": 2.729813827825533,
-            "heading": 3.1399998022537057,
-            "angularVelocity": -9.316120157306394e-14,
-            "velocityX": -0.000001050012614805773,
-            "velocityY": -3.7829405750872973,
-            "timestamp": 0.9907392781101575
+            "x": 7.680001725125185,
+            "y": 2.729813827825532,
+            "heading": -1.968547288641082e-7,
+            "angularVelocity": -9.134372064218233e-14,
+            "velocityX": -0.0000010499582046587897,
+            "velocityY": -3.7829405750872978,
+            "timestamp": 0.9907392781101572
         },
         {
-            "x": 7.680001540841657,
-            "y": 2.355023046375991,
-            "heading": 3.1399998022536986,
-            "angularVelocity": -7.445499842222973e-14,
-            "velocityX": -0.0000018602636449128091,
+            "x": 7.680001540827678,
+            "y": 2.3550230463759907,
+            "heading": -1.9685473554016492e-7,
+            "angularVelocity": -7.335754268551387e-14,
+            "velocityX": -0.0000018602017039855543,
             "velocityY": -3.7829405750870753,
-            "timestamp": 1.0898132059211731
+            "timestamp": 1.089813205921173
         },
         {
-            "x": 7.680001281423707,
-            "y": 1.9802322649264825,
-            "heading": 3.139999802253698,
-            "angularVelocity": -7.093550187375142e-15,
-            "velocityX": -0.000002618427817479533,
+            "x": 7.680001281411145,
+            "y": 1.980232264926482,
+            "heading": -1.9685473620018866e-7,
+            "angularVelocity": -6.8770489517299035e-15,
+            "velocityX": -0.0000026184135239642236,
             "velocityY": -3.782940575086706,
-            "timestamp": 1.188887133732189
+            "timestamp": 1.1888871337321887
         },
         {
-            "x": 7.680000953984531,
-            "y": 1.6054414834770696,
-            "heading": 3.139999802253699,
-            "angularVelocity": 1.1174094242032294e-14,
-            "velocityX": -0.0000033049984216512085,
-            "velocityY": -3.782940575085678,
-            "timestamp": 1.2879610615432047
+            "x": 7.680000953971803,
+            "y": 1.6054414834770692,
+            "heading": -1.968547355090094e-7,
+            "angularVelocity": 1.0827757762815176e-14,
+            "velocityX": -0.000003305000098197799,
+            "velocityY": -3.7829405750856786,
+            "timestamp": 1.2879610615432044
         },
         {
-            "x": 7.680000583113676,
-            "y": 1.2780649081436686,
-            "heading": 3.139999880388909,
-            "angularVelocity": 7.886553669648788e-7,
-            "velocityX": -0.0000037433746003660557,
-            "velocityY": -3.304366573190331,
-            "timestamp": 1.3870349893542204
+            "x": 7.6800005831051426,
+            "y": 1.2780649081436681,
+            "heading": -1.1900708358832985e-7,
+            "angularVelocity": 7.857529072428817e-7,
+            "velocityX": -0.0000037433322544868336,
+            "velocityY": -3.30436657319033,
+            "timestamp": 1.3870349893542202
         },
         {
-            "x": 7.6800002967542085,
+            "x": 7.680000296749478,
             "y": 1.0325324587418607,
-            "heading": 3.139999940073013,
-            "angularVelocity": 6.024197635453253e-7,
-            "velocityX": -0.0000028903613408431902,
-            "velocityY": -2.47827511058363,
-            "timestamp": 1.4861089171652362
+            "heading": -5.959102520162259e-8,
+            "angularVelocity": 5.997142524637507e-7,
+            "velocityX": -0.000002890322961269868,
+            "velocityY": -2.478275110583629,
+            "timestamp": 1.486108917165236
         },
         {
-            "x": 7.680000100606573,
+            "x": 7.680000100604838,
             "y": 0.8688441539517371,
-            "heading": 3.1399999800752636,
-            "angularVelocity": 4.0376157345960537e-7,
-            "velocityX": -0.000001979810751989867,
-            "velocityY": -1.6521834594299558,
-            "timestamp": 1.585182844976252
+            "heading": -1.9801599569632452e-8,
+            "angularVelocity": 4.016134309966653e-7,
+            "velocityX": -0.000001979780510399852,
+            "velocityY": -1.6521834594299551,
+            "timestamp": 1.5851828449762517
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 2.0110976517179292e-7,
-            "velocityX": -0.00000101546967480447,
-            "velocityY": -0.8260917454272348,
-            "timestamp": 1.6842567727872677
+            "heading": -3.727510081119492e-33,
+            "angularVelocity": 1.998668869584927e-7,
+            "velocityX": -0.0000010154521677217573,
+            "velocityY": -0.8260917454272346,
+            "timestamp": 1.6842567727872675
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": -8.944233053536814e-32,
+            "heading": 0,
+            "angularVelocity": 1.3621739357478042e-33,
             "velocityX": 0,
-            "velocityY": -1.1208522306082827e-37,
-            "timestamp": 1.7833307005982835
+            "velocityY": 1.5851260517441878e-39,
+            "timestamp": 1.7833307005982832
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.1.traj
index 1d87c7a2..4ab726d7 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.1.traj	
@@ -3,128 +3,128 @@
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 6.88462418020868e-33,
-            "velocityX": -2.6678679284923953e-34,
-            "velocityY": 0,
+            "heading": -2.09363537243035e-32,
+            "angularVelocity": -1.4612041776101527e-32,
+            "velocityX": -1.1521781181914207e-34,
+            "velocityY": -1.3541198100986985e-32,
             "timestamp": 0
         },
         {
-            "x": 7.620477294727877,
-            "y": 5.828094267433712,
-            "heading": 3.125579827082933,
-            "angularVelocity": -0.16053625399389732,
-            "velocityX": -0.6626517023703261,
-            "velocityY": 0.34616486523434487,
-            "timestamp": 0.08982502430644557
+            "x": 7.620476774011981,
+            "y": 5.828095362884936,
+            "heading": -0.014618875325545894,
+            "angularVelocity": -0.16274311877436243,
+            "velocityX": -0.6626361618854291,
+            "velocityY": 0.346165913767109,
+            "timestamp": 0.08982791675397632
         },
         {
-            "x": 7.501400723607879,
-            "y": 5.890296797165295,
-            "heading": 3.0989291927764526,
-            "angularVelocity": -0.29669498574873204,
-            "velocityX": -1.3256503078001867,
-            "velocityY": 0.6924855318644141,
-            "timestamp": 0.17965004861289113
+            "x": 7.50139830253108,
+            "y": 5.890300516939614,
+            "heading": -0.04163561640032371,
+            "angularVelocity": -0.3007610779705845,
+            "velocityX": -1.3256287775996987,
+            "velocityY": 0.692492448923733,
+            "timestamp": 0.17965583350795264
         },
         {
-            "x": 7.322728500258778,
-            "y": 5.983625073051111,
-            "heading": 3.0637763178084882,
-            "angularVelocity": -0.3913483490751664,
-            "velocityX": -1.9891141107799197,
-            "velocityY": 1.0390008419860823,
-            "timestamp": 0.2694750729193367
+            "x": 7.322721643819432,
+            "y": 5.983633497452849,
+            "heading": -0.07726722942321354,
+            "angularVelocity": -0.3966652496292318,
+            "velocityX": -1.9890994377729274,
+            "velocityY": 1.0390197600692306,
+            "timestamp": 0.26948375026192894
         },
         {
-            "x": 7.084419480032935,
-            "y": 6.108090729851432,
-            "heading": 3.027860333619382,
-            "angularVelocity": -0.3998438571703182,
-            "velocityX": -2.6530359670466983,
-            "velocityY": 1.3856456790448122,
-            "timestamp": 0.35930009722578227
+            "x": 7.084404587107726,
+            "y": 6.108106334427675,
+            "heading": -0.11365614278993857,
+            "angularVelocity": -0.4050958174437935,
+            "velocityX": -2.6530400049732523,
+            "velocityY": 1.3856809939802615,
+            "timestamp": 0.3593116670159053
         },
         {
-            "x": 6.786779796078207,
-            "y": 6.263490017806672,
-            "heading": 3.0170090626873085,
-            "angularVelocity": -0.1208045421179424,
-            "velocityX": -3.3135497179416755,
-            "velocityY": 1.7300222199225983,
-            "timestamp": 0.44912512153222783
+            "x": 6.786764353262062,
+            "y": 6.2635080892247865,
+            "heading": -0.12453213530374094,
+            "angularVelocity": -0.12107586268075081,
+            "velocityX": -3.313449143664897,
+            "velocityY": 1.7299939752886722,
+            "timestamp": 0.4491395837698816
         },
         {
-            "x": 6.48562142682328,
-            "y": 6.4208733949571855,
-            "heading": 3.0170090501527196,
-            "angularVelocity": -1.3954451104022202e-7,
-            "velocityX": -3.3527223797624646,
-            "velocityY": 1.752110599086374,
-            "timestamp": 0.5389501458386734
+            "x": 6.485598483517562,
+            "y": 6.4209007383980845,
+            "heading": -0.12453214800780214,
+            "angularVelocity": -1.4142664848458141e-7,
+            "velocityX": -3.352697920951937,
+            "velocityY": 1.752157401182633,
+            "timestamp": 0.5389675005238579
         },
         {
-            "x": 6.164454252506572,
-            "y": 6.531857797451653,
-            "heading": 3.017009051216684,
-            "angularVelocity": 1.1844854204542515e-8,
-            "velocityX": -3.5754755069257693,
-            "velocityY": 1.2355621760349855,
-            "timestamp": 0.628775170145119
+            "x": 6.164419225797539,
+            "y": 6.531883674906018,
+            "heading": -0.12453214718798315,
+            "angularVelocity": 9.126550019087707e-9,
+            "velocityX": -3.575494894306401,
+            "velocityY": 1.2355060711459842,
+            "timestamp": 0.6287954172778342
         },
         {
-            "x": 5.838539791831514,
-            "y": 6.576460283854247,
-            "heading": 3.052457229239417,
-            "angularVelocity": 0.3946358856725586,
-            "velocityX": -3.6283258834773524,
-            "velocityY": 0.4965485592347737,
-            "timestamp": 0.7186001944515645
+            "x": 5.83853002564033,
+            "y": 6.576482035264996,
+            "heading": -0.08897109874736252,
+            "angularVelocity": 0.3958796967096147,
+            "velocityX": -3.6279278417395133,
+            "velocityY": 0.4964866376799699,
+            "timestamp": 0.7186233340318104
         },
         {
-            "x": 5.573157198508507,
-            "y": 6.601492716526358,
-            "heading": 3.1680842973805725,
-            "angularVelocity": 1.287247835822277,
-            "velocityX": -2.9544394267863754,
-            "velocityY": 0.2786799432050252,
-            "timestamp": 0.8084252187580101
+            "x": 5.573146790095341,
+            "y": 6.601508859331819,
+            "heading": 0.02708371250022136,
+            "angularVelocity": 1.2919681925323805,
+            "velocityX": -2.9543514436812472,
+            "velocityY": 0.278608532527451,
+            "timestamp": 0.8084512507857867
         },
         {
-            "x": 5.374340211248987,
-            "y": 6.616920258488131,
-            "heading": 3.277639232341259,
-            "angularVelocity": 1.2196482640175015,
-            "velocityX": -2.2133808345126464,
-            "velocityY": 0.17175104689246343,
-            "timestamp": 0.8982502430644557
+            "x": 5.3743330314019,
+            "y": 6.61692927227141,
+            "heading": 0.137026184669163,
+            "angularVelocity": 1.2239232094189134,
+            "velocityX": -2.213273622252188,
+            "velocityY": 0.1716661534278421,
+            "timestamp": 0.898279167539763
         },
         {
-            "x": 5.242040057703165,
-            "y": 6.625927417066914,
-            "heading": 3.3593576896006887,
-            "angularVelocity": 0.9097515741341804,
-            "velocityX": -1.4728652128661812,
-            "velocityY": 0.10027449085964446,
-            "timestamp": 0.9880752673709012
+            "x": 5.242037120454666,
+            "y": 6.625930660217185,
+            "heading": 0.21902488374006282,
+            "angularVelocity": 0.912842043253442,
+            "velocityX": -1.4727705565027283,
+            "velocityY": 0.10020702105814829,
+            "timestamp": 0.9881070842937393
         },
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.48792213436398174,
-            "velocityX": -0.7352077910701558,
-            "velocityY": 0.04533906853386082,
-            "timestamp": 1.0779002916773468
+            "heading": 0.263,
+            "angularVelocity": 0.4895484371565439,
+            "velocityX": -0.7351514188571259,
+            "velocityY": 0.04530150458636799,
+            "timestamp": 1.0779350010477156
         },
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": 3.4031853071795863,
+            "heading": 0.263,
             "angularVelocity": 0,
-            "velocityX": 2.612853322689367e-35,
-            "velocityY": 8.19047982527468e-33,
-            "timestamp": 1.1677253159837924
+            "velocityX": 4.777602762336884e-34,
+            "velocityY": 2.1640483822800166e-33,
+            "timestamp": 1.167762917801692
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.traj
index 1d87c7a2..4ab726d7 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.traj	
@@ -3,128 +3,128 @@
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 6.88462418020868e-33,
-            "velocityX": -2.6678679284923953e-34,
-            "velocityY": 0,
+            "heading": -2.09363537243035e-32,
+            "angularVelocity": -1.4612041776101527e-32,
+            "velocityX": -1.1521781181914207e-34,
+            "velocityY": -1.3541198100986985e-32,
             "timestamp": 0
         },
         {
-            "x": 7.620477294727877,
-            "y": 5.828094267433712,
-            "heading": 3.125579827082933,
-            "angularVelocity": -0.16053625399389732,
-            "velocityX": -0.6626517023703261,
-            "velocityY": 0.34616486523434487,
-            "timestamp": 0.08982502430644557
+            "x": 7.620476774011981,
+            "y": 5.828095362884936,
+            "heading": -0.014618875325545894,
+            "angularVelocity": -0.16274311877436243,
+            "velocityX": -0.6626361618854291,
+            "velocityY": 0.346165913767109,
+            "timestamp": 0.08982791675397632
         },
         {
-            "x": 7.501400723607879,
-            "y": 5.890296797165295,
-            "heading": 3.0989291927764526,
-            "angularVelocity": -0.29669498574873204,
-            "velocityX": -1.3256503078001867,
-            "velocityY": 0.6924855318644141,
-            "timestamp": 0.17965004861289113
+            "x": 7.50139830253108,
+            "y": 5.890300516939614,
+            "heading": -0.04163561640032371,
+            "angularVelocity": -0.3007610779705845,
+            "velocityX": -1.3256287775996987,
+            "velocityY": 0.692492448923733,
+            "timestamp": 0.17965583350795264
         },
         {
-            "x": 7.322728500258778,
-            "y": 5.983625073051111,
-            "heading": 3.0637763178084882,
-            "angularVelocity": -0.3913483490751664,
-            "velocityX": -1.9891141107799197,
-            "velocityY": 1.0390008419860823,
-            "timestamp": 0.2694750729193367
+            "x": 7.322721643819432,
+            "y": 5.983633497452849,
+            "heading": -0.07726722942321354,
+            "angularVelocity": -0.3966652496292318,
+            "velocityX": -1.9890994377729274,
+            "velocityY": 1.0390197600692306,
+            "timestamp": 0.26948375026192894
         },
         {
-            "x": 7.084419480032935,
-            "y": 6.108090729851432,
-            "heading": 3.027860333619382,
-            "angularVelocity": -0.3998438571703182,
-            "velocityX": -2.6530359670466983,
-            "velocityY": 1.3856456790448122,
-            "timestamp": 0.35930009722578227
+            "x": 7.084404587107726,
+            "y": 6.108106334427675,
+            "heading": -0.11365614278993857,
+            "angularVelocity": -0.4050958174437935,
+            "velocityX": -2.6530400049732523,
+            "velocityY": 1.3856809939802615,
+            "timestamp": 0.3593116670159053
         },
         {
-            "x": 6.786779796078207,
-            "y": 6.263490017806672,
-            "heading": 3.0170090626873085,
-            "angularVelocity": -0.1208045421179424,
-            "velocityX": -3.3135497179416755,
-            "velocityY": 1.7300222199225983,
-            "timestamp": 0.44912512153222783
+            "x": 6.786764353262062,
+            "y": 6.2635080892247865,
+            "heading": -0.12453213530374094,
+            "angularVelocity": -0.12107586268075081,
+            "velocityX": -3.313449143664897,
+            "velocityY": 1.7299939752886722,
+            "timestamp": 0.4491395837698816
         },
         {
-            "x": 6.48562142682328,
-            "y": 6.4208733949571855,
-            "heading": 3.0170090501527196,
-            "angularVelocity": -1.3954451104022202e-7,
-            "velocityX": -3.3527223797624646,
-            "velocityY": 1.752110599086374,
-            "timestamp": 0.5389501458386734
+            "x": 6.485598483517562,
+            "y": 6.4209007383980845,
+            "heading": -0.12453214800780214,
+            "angularVelocity": -1.4142664848458141e-7,
+            "velocityX": -3.352697920951937,
+            "velocityY": 1.752157401182633,
+            "timestamp": 0.5389675005238579
         },
         {
-            "x": 6.164454252506572,
-            "y": 6.531857797451653,
-            "heading": 3.017009051216684,
-            "angularVelocity": 1.1844854204542515e-8,
-            "velocityX": -3.5754755069257693,
-            "velocityY": 1.2355621760349855,
-            "timestamp": 0.628775170145119
+            "x": 6.164419225797539,
+            "y": 6.531883674906018,
+            "heading": -0.12453214718798315,
+            "angularVelocity": 9.126550019087707e-9,
+            "velocityX": -3.575494894306401,
+            "velocityY": 1.2355060711459842,
+            "timestamp": 0.6287954172778342
         },
         {
-            "x": 5.838539791831514,
-            "y": 6.576460283854247,
-            "heading": 3.052457229239417,
-            "angularVelocity": 0.3946358856725586,
-            "velocityX": -3.6283258834773524,
-            "velocityY": 0.4965485592347737,
-            "timestamp": 0.7186001944515645
+            "x": 5.83853002564033,
+            "y": 6.576482035264996,
+            "heading": -0.08897109874736252,
+            "angularVelocity": 0.3958796967096147,
+            "velocityX": -3.6279278417395133,
+            "velocityY": 0.4964866376799699,
+            "timestamp": 0.7186233340318104
         },
         {
-            "x": 5.573157198508507,
-            "y": 6.601492716526358,
-            "heading": 3.1680842973805725,
-            "angularVelocity": 1.287247835822277,
-            "velocityX": -2.9544394267863754,
-            "velocityY": 0.2786799432050252,
-            "timestamp": 0.8084252187580101
+            "x": 5.573146790095341,
+            "y": 6.601508859331819,
+            "heading": 0.02708371250022136,
+            "angularVelocity": 1.2919681925323805,
+            "velocityX": -2.9543514436812472,
+            "velocityY": 0.278608532527451,
+            "timestamp": 0.8084512507857867
         },
         {
-            "x": 5.374340211248987,
-            "y": 6.616920258488131,
-            "heading": 3.277639232341259,
-            "angularVelocity": 1.2196482640175015,
-            "velocityX": -2.2133808345126464,
-            "velocityY": 0.17175104689246343,
-            "timestamp": 0.8982502430644557
+            "x": 5.3743330314019,
+            "y": 6.61692927227141,
+            "heading": 0.137026184669163,
+            "angularVelocity": 1.2239232094189134,
+            "velocityX": -2.213273622252188,
+            "velocityY": 0.1716661534278421,
+            "timestamp": 0.898279167539763
         },
         {
-            "x": 5.242040057703165,
-            "y": 6.625927417066914,
-            "heading": 3.3593576896006887,
-            "angularVelocity": 0.9097515741341804,
-            "velocityX": -1.4728652128661812,
-            "velocityY": 0.10027449085964446,
-            "timestamp": 0.9880752673709012
+            "x": 5.242037120454666,
+            "y": 6.625930660217185,
+            "heading": 0.21902488374006282,
+            "angularVelocity": 0.912842043253442,
+            "velocityX": -1.4727705565027283,
+            "velocityY": 0.10020702105814829,
+            "timestamp": 0.9881070842937393
         },
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.48792213436398174,
-            "velocityX": -0.7352077910701558,
-            "velocityY": 0.04533906853386082,
-            "timestamp": 1.0779002916773468
+            "heading": 0.263,
+            "angularVelocity": 0.4895484371565439,
+            "velocityX": -0.7351514188571259,
+            "velocityY": 0.04530150458636799,
+            "timestamp": 1.0779350010477156
         },
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": 3.4031853071795863,
+            "heading": 0.263,
             "angularVelocity": 0,
-            "velocityX": 2.612853322689367e-35,
-            "velocityY": 8.19047982527468e-33,
-            "timestamp": 1.1677253159837924
+            "velocityX": 4.777602762336884e-34,
+            "velocityY": 2.1640483822800166e-33,
+            "timestamp": 1.167762917801692
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.1.traj
deleted file mode 100644
index 7d7772cd..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.1.traj	
+++ /dev/null
@@ -1,139 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 7.320432516399354e-29,
-            "velocityX": -1.2956627863132083e-30,
-            "velocityY": -2.1869206977044484e-30,
-            "timestamp": 0
-        },
-        {
-            "x": 7.583655523801243,
-            "y": 5.855694285619672,
-            "heading": 3.1309164320392644,
-            "angularVelocity": -0.078082149385061,
-            "velocityX": -0.8281749875703897,
-            "velocityY": 0.5045347816645432,
-            "timestamp": 0.1163334773987831
-        },
-        {
-            "x": 7.390966571410279,
-            "y": 5.973082843106357,
-            "heading": 3.112749003492932,
-            "angularVelocity": -0.15616681416695535,
-            "velocityX": -1.6563499750844712,
-            "velocityY": 1.0090694451115323,
-            "timestamp": 0.2326669547975662
-        },
-        {
-            "x": 7.1019331530012675,
-            "y": 6.149165651208255,
-            "heading": 3.0854972394904423,
-            "angularVelocity": -0.2342555609256986,
-            "velocityX": -2.484524875141701,
-            "velocityY": 1.5136039258785212,
-            "timestamp": 0.3490004321963493
-        },
-        {
-            "x": 6.726101496816599,
-            "y": 6.378126994339933,
-            "heading": 3.085497236816664,
-            "angularVelocity": -2.298373345534558e-8,
-            "velocityX": -3.2306406082605346,
-            "velocityY": 1.968146644037928,
-            "timestamp": 0.4653339095951324
-        },
-        {
-            "x": 6.345661043001768,
-            "y": 6.599345871138349,
-            "heading": 3.085497234127535,
-            "angularVelocity": -2.3115694536288248e-8,
-            "velocityX": -3.2702577308052767,
-            "velocityY": 1.9015925745956355,
-            "timestamp": 0.5816673869939155
-        },
-        {
-            "x": 5.9214720079184415,
-            "y": 6.716548218753983,
-            "heading": 3.08549723097445,
-            "angularVelocity": -2.7103849507961225e-8,
-            "velocityX": -3.6463195682635403,
-            "velocityY": 1.0074687891764202,
-            "timestamp": 0.6980008643926986
-        },
-        {
-            "x": 5.481423496557165,
-            "y": 6.7220267434353165,
-            "heading": 3.0854971884524733,
-            "angularVelocity": -3.6551797104544465e-7,
-            "velocityX": -3.7826472757521072,
-            "velocityY": 0.047093277050021864,
-            "timestamp": 0.8143343417914817
-        },
-        {
-            "x": 5.071345998787579,
-            "y": 6.619502331936359,
-            "heading": 3.038730840809692,
-            "angularVelocity": -0.40200249049953485,
-            "velocityX": -3.5250171054705812,
-            "velocityY": -0.8812975747944856,
-            "timestamp": 0.9306678191902648
-        },
-        {
-            "x": 4.728494542469685,
-            "y": 6.427075201621358,
-            "heading": 2.9510390674106386,
-            "angularVelocity": -0.7537965455846517,
-            "velocityX": -2.947143539280811,
-            "velocityY": -1.6540993583074475,
-            "timestamp": 1.0470012965890478
-        },
-        {
-            "x": 4.474342078218714,
-            "y": 6.168607565995357,
-            "heading": 2.943927263065388,
-            "angularVelocity": -0.061132912935043464,
-            "velocityX": -2.1846889642931684,
-            "velocityY": -2.221782082039817,
-            "timestamp": 1.163334773987831
-        },
-        {
-            "x": 4.328922284450032,
-            "y": 5.887474207261646,
-            "heading": 3.0089644652983214,
-            "angularVelocity": 0.5590583526527744,
-            "velocityX": -1.2500253325205173,
-            "velocityY": -2.4166161368150747,
-            "timestamp": 1.2796682513866142
-        },
-        {
-            "x": 4.275692020371012,
-            "y": 5.671261483228584,
-            "heading": 3.090017233953519,
-            "angularVelocity": 0.696727807571276,
-            "velocityX": -0.45756617329120264,
-            "velocityY": -1.8585597960929203,
-            "timestamp": 1.3960017287853974
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0.42965075199414443,
-            "velocityX": -0.10910032653373211,
-            "velocityY": -0.9564012502367466,
-            "timestamp": 1.5123352061841806
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -1.1779881542407281e-29,
-            "velocityX": 2.2171306210967267e-31,
-            "velocityY": -6.290632939767308e-32,
-            "timestamp": 1.6286686835829638
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.traj
deleted file mode 100644
index 7d7772cd..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.traj	
+++ /dev/null
@@ -1,139 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 7.320432516399354e-29,
-            "velocityX": -1.2956627863132083e-30,
-            "velocityY": -2.1869206977044484e-30,
-            "timestamp": 0
-        },
-        {
-            "x": 7.583655523801243,
-            "y": 5.855694285619672,
-            "heading": 3.1309164320392644,
-            "angularVelocity": -0.078082149385061,
-            "velocityX": -0.8281749875703897,
-            "velocityY": 0.5045347816645432,
-            "timestamp": 0.1163334773987831
-        },
-        {
-            "x": 7.390966571410279,
-            "y": 5.973082843106357,
-            "heading": 3.112749003492932,
-            "angularVelocity": -0.15616681416695535,
-            "velocityX": -1.6563499750844712,
-            "velocityY": 1.0090694451115323,
-            "timestamp": 0.2326669547975662
-        },
-        {
-            "x": 7.1019331530012675,
-            "y": 6.149165651208255,
-            "heading": 3.0854972394904423,
-            "angularVelocity": -0.2342555609256986,
-            "velocityX": -2.484524875141701,
-            "velocityY": 1.5136039258785212,
-            "timestamp": 0.3490004321963493
-        },
-        {
-            "x": 6.726101496816599,
-            "y": 6.378126994339933,
-            "heading": 3.085497236816664,
-            "angularVelocity": -2.298373345534558e-8,
-            "velocityX": -3.2306406082605346,
-            "velocityY": 1.968146644037928,
-            "timestamp": 0.4653339095951324
-        },
-        {
-            "x": 6.345661043001768,
-            "y": 6.599345871138349,
-            "heading": 3.085497234127535,
-            "angularVelocity": -2.3115694536288248e-8,
-            "velocityX": -3.2702577308052767,
-            "velocityY": 1.9015925745956355,
-            "timestamp": 0.5816673869939155
-        },
-        {
-            "x": 5.9214720079184415,
-            "y": 6.716548218753983,
-            "heading": 3.08549723097445,
-            "angularVelocity": -2.7103849507961225e-8,
-            "velocityX": -3.6463195682635403,
-            "velocityY": 1.0074687891764202,
-            "timestamp": 0.6980008643926986
-        },
-        {
-            "x": 5.481423496557165,
-            "y": 6.7220267434353165,
-            "heading": 3.0854971884524733,
-            "angularVelocity": -3.6551797104544465e-7,
-            "velocityX": -3.7826472757521072,
-            "velocityY": 0.047093277050021864,
-            "timestamp": 0.8143343417914817
-        },
-        {
-            "x": 5.071345998787579,
-            "y": 6.619502331936359,
-            "heading": 3.038730840809692,
-            "angularVelocity": -0.40200249049953485,
-            "velocityX": -3.5250171054705812,
-            "velocityY": -0.8812975747944856,
-            "timestamp": 0.9306678191902648
-        },
-        {
-            "x": 4.728494542469685,
-            "y": 6.427075201621358,
-            "heading": 2.9510390674106386,
-            "angularVelocity": -0.7537965455846517,
-            "velocityX": -2.947143539280811,
-            "velocityY": -1.6540993583074475,
-            "timestamp": 1.0470012965890478
-        },
-        {
-            "x": 4.474342078218714,
-            "y": 6.168607565995357,
-            "heading": 2.943927263065388,
-            "angularVelocity": -0.061132912935043464,
-            "velocityX": -2.1846889642931684,
-            "velocityY": -2.221782082039817,
-            "timestamp": 1.163334773987831
-        },
-        {
-            "x": 4.328922284450032,
-            "y": 5.887474207261646,
-            "heading": 3.0089644652983214,
-            "angularVelocity": 0.5590583526527744,
-            "velocityX": -1.2500253325205173,
-            "velocityY": -2.4166161368150747,
-            "timestamp": 1.2796682513866142
-        },
-        {
-            "x": 4.275692020371012,
-            "y": 5.671261483228584,
-            "heading": 3.090017233953519,
-            "angularVelocity": 0.696727807571276,
-            "velocityX": -0.45756617329120264,
-            "velocityY": -1.8585597960929203,
-            "timestamp": 1.3960017287853974
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0.42965075199414443,
-            "velocityX": -0.10910032653373211,
-            "velocityY": -0.9564012502367466,
-            "timestamp": 1.5123352061841806
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -1.1779881542407281e-29,
-            "velocityX": 2.2171306210967267e-31,
-            "velocityY": -6.290632939767308e-32,
-            "timestamp": 1.6286686835829638
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.1.traj
index 9d9edbcb..90749c58 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.1.traj	
@@ -3,191 +3,191 @@
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 7.712459531346682e-31,
-            "velocityX": 8.463399539512725e-31,
-            "velocityY": -2.525463677436265e-31,
+            "heading": 0,
+            "angularVelocity": 7.446350164213638e-30,
+            "velocityX": -3.476389258222187e-32,
+            "velocityY": 2.4606016306403766e-37,
             "timestamp": 0
         },
         {
-            "x": 7.643960549352244,
-            "y": 5.758768623179225,
-            "heading": 3.0545306432351627,
-            "angularVelocity": -1.025550451563212,
-            "velocityX": -0.43243890307482463,
-            "velocityY": -0.45873991856878515,
-            "timestamp": 0.0833399825767314
-        },
-        {
-            "x": 7.572007137321917,
-            "y": 5.681963480289103,
-            "heading": 2.882145272333543,
-            "angularVelocity": -2.0684594065387945,
-            "velocityX": -0.8633720551126874,
-            "velocityY": -0.9215881803119774,
-            "timestamp": 0.1666799651534628
-        },
-        {
-            "x": 7.462165154490984,
-            "y": 5.566757202432317,
-            "heading": 2.631422178575744,
-            "angularVelocity": -3.008437079128932,
-            "velocityX": -1.3179986296469572,
-            "velocityY": -1.382365034102495,
-            "timestamp": 0.2500199477301942
-        },
-        {
-            "x": 7.311173429444628,
-            "y": 5.412128972668698,
-            "heading": 2.3433148642731845,
-            "angularVelocity": -3.457011933465413,
-            "velocityX": -1.8117561388657581,
-            "velocityY": -1.8553907138299572,
-            "timestamp": 0.3333599303069256
-        },
-        {
-            "x": 7.123843168628933,
-            "y": 5.227542500531133,
-            "heading": 2.191575313033137,
-            "angularVelocity": -1.820729337210275,
-            "velocityX": -2.2477837770510787,
-            "velocityY": -2.21486093985698,
-            "timestamp": 0.41669991288365704
-        },
-        {
-            "x": 6.902601176392792,
-            "y": 5.010431192670728,
-            "heading": 2.175785118248912,
-            "angularVelocity": -0.1894672196467848,
-            "velocityX": -2.6546920865077652,
-            "velocityY": -2.6051278287766304,
-            "timestamp": 0.5000398954603884
-        },
-        {
-            "x": 6.682324869385203,
-            "y": 4.7848793693995555,
-            "heading": 2.175784967765777,
-            "angularVelocity": -0.0000018056535426780284,
-            "velocityX": -2.643104788326184,
-            "velocityY": -2.7064059326328107,
-            "timestamp": 0.5833798780371198
-        },
-        {
-            "x": 6.427295318194512,
-            "y": 4.59952808942922,
-            "heading": 2.175784213925318,
-            "angularVelocity": -0.00000904536377316099,
-            "velocityX": -3.060110445258255,
-            "velocityY": -2.224037901611934,
-            "timestamp": 0.6667198606138511
-        },
-        {
-            "x": 6.1722637689115825,
-            "y": 4.4141795587676,
-            "heading": 2.1757834600140606,
-            "angularVelocity": -0.000009046213285018581,
-            "velocityX": -3.0601344204519796,
-            "velocityY": -2.224004912539731,
-            "timestamp": 0.7500598431905825
-        },
-        {
-            "x": 5.906721685974762,
-            "y": 4.244230836092675,
-            "heading": 2.175782696018566,
-            "angularVelocity": -0.000009167214470514032,
-            "velocityX": -3.1862507613597577,
-            "velocityY": -2.039221960701194,
-            "timestamp": 0.8333998257673139
-        },
-        {
-            "x": 5.614573321214709,
-            "y": 4.1257208216835135,
-            "heading": 2.175782627659757,
-            "angularVelocity": -8.202402673559022e-7,
-            "velocityX": -3.5055006699943965,
-            "velocityY": -1.4220067096852238,
-            "timestamp": 0.9167398083440452
-        },
-        {
-            "x": 5.32091408424666,
-            "y": 4.0110057741909,
-            "heading": 2.1757827056250556,
-            "angularVelocity": 9.355089415031885e-7,
-            "velocityX": -3.523629689959162,
-            "velocityY": -1.3764707400435987,
-            "timestamp": 1.0000797909207766
-        },
-        {
-            "x": 5.051276753694785,
-            "y": 3.8476320535360697,
-            "heading": 2.1757829434200273,
-            "angularVelocity": 0.0000028533119900283243,
-            "velocityX": -3.235389811890331,
-            "velocityY": -1.9603282314633543,
-            "timestamp": 1.083419773497508
-        },
-        {
-            "x": 4.804772075990701,
-            "y": 3.6532031340051674,
-            "heading": 2.180086334431566,
-            "angularVelocity": 0.05163657200885012,
-            "velocityX": -2.957820125257733,
-            "velocityY": -2.332960885273652,
-            "timestamp": 1.1667597560742393
-        },
-        {
-            "x": 4.605798608727452,
-            "y": 3.4589597133454815,
-            "heading": 2.2977487638281207,
-            "angularVelocity": 1.411836501024247,
-            "velocityX": -2.387491107045202,
-            "velocityY": -2.330735076418397,
-            "timestamp": 1.2500997386509707
-        },
-        {
-            "x": 4.462499334629702,
-            "y": 3.280406115781069,
-            "heading": 2.4052460747163282,
-            "angularVelocity": 1.28986481115753,
-            "velocityX": -1.7194540923477422,
-            "velocityY": -2.142472220941705,
-            "timestamp": 1.333439721227702
-        },
-        {
-            "x": 4.360731419840454,
-            "y": 3.1416564242722327,
-            "heading": 2.488750866660265,
-            "angularVelocity": 1.0019775546155592,
-            "velocityX": -1.2211175433778025,
-            "velocityY": -1.6648634571178227,
-            "timestamp": 1.4167797038044334
-        },
-        {
-            "x": 4.295085549107569,
-            "y": 3.0475525606989393,
-            "heading": 2.5453815690352877,
-            "angularVelocity": 0.6795142094358206,
-            "velocityX": -0.7876875984758661,
-            "velocityY": -1.129156266461315,
-            "timestamp": 1.5001196863811648
+            "x": 7.6436015698643995,
+            "y": 5.74806653588022,
+            "heading": -0.0038807165751560974,
+            "angularVelocity": -0.045372809336175934,
+            "velocityX": -0.4255654848002544,
+            "velocityY": -0.572123393880186,
+            "timestamp": 0.08552956345292874
+        },
+        {
+            "x": 7.570804707716712,
+            "y": 5.6501996114987145,
+            "heading": -0.011642164214821673,
+            "angularVelocity": -0.09074578807989701,
+            "velocityX": -0.8511309915401587,
+            "velocityY": -1.1442467426525054,
+            "timestamp": 0.17105912690585748
+        },
+        {
+            "x": 7.461609412342726,
+            "y": 5.503399231866818,
+            "heading": -0.02328436718357624,
+            "angularVelocity": -0.136119050521775,
+            "velocityX": -1.2766965124764296,
+            "velocityY": -1.7163700328330225,
+            "timestamp": 0.25658869035878623
+        },
+        {
+            "x": 7.3160156842382476,
+            "y": 5.307665404737857,
+            "heading": -0.038807352623976644,
+            "angularVelocity": -0.18149263031072865,
+            "velocityX": -1.7022620276158245,
+            "velocityY": -2.288493232362672,
+            "timestamp": 0.34211825381171496
+        },
+        {
+            "x": 7.134023531165023,
+            "y": 5.062998148299065,
+            "heading": -0.05821111114579378,
+            "angularVelocity": -0.22686610031040333,
+            "velocityX": -2.127827452005934,
+            "velocityY": -2.8606162192496707,
+            "timestamp": 0.4276478172646437
+        },
+        {
+            "x": 6.9409164317403365,
+            "y": 4.803390071813602,
+            "heading": -0.058211113467083825,
+            "angularVelocity": -2.714020682501112e-8,
+            "velocityX": -2.2577818900123794,
+            "velocityY": -3.0353022511138943,
+            "timestamp": 0.5131773807175725
+        },
+        {
+            "x": 6.70968017178257,
+            "y": 4.577079452070077,
+            "heading": -0.05821111591888997,
+            "angularVelocity": -2.866618337367041e-8,
+            "velocityX": -2.7035828387575864,
+            "velocityY": -2.645992924634468,
+            "timestamp": 0.5987069441705012
+        },
+        {
+            "x": 6.4400791107327935,
+            "y": 4.398188508585333,
+            "heading": -0.05821111892081074,
+            "angularVelocity": -3.509804859460385e-8,
+            "velocityX": -3.152138864804934,
+            "velocityY": -2.091568532127445,
+            "timestamp": 0.6842365076234299
+        },
+        {
+            "x": 6.141694589328426,
+            "y": 4.273075039260189,
+            "heading": -0.058211144902897714,
+            "angularVelocity": -3.0377902004237926e-7,
+            "velocityX": -3.488671160686832,
+            "velocityY": -1.462809632999017,
+            "timestamp": 0.7697660710763586
+        },
+        {
+            "x": 5.825131041705898,
+            "y": 4.206185466847777,
+            "heading": -0.05821124348692034,
+            "angularVelocity": -0.000001152630957627144,
+            "velocityX": -3.7012178578082944,
+            "velocityY": -0.7820637649954203,
+            "timestamp": 0.8552956345292874
+        },
+        {
+            "x": 5.503318785454965,
+            "y": 4.1726658015863975,
+            "heading": -0.058211338856237596,
+            "angularVelocity": -0.0000011150450605169934,
+            "velocityX": -3.76258504380235,
+            "velocityY": -0.39190735820634004,
+            "timestamp": 0.9408251979822161
+        },
+        {
+            "x": 5.1935160355708865,
+            "y": 4.079344267897855,
+            "heading": -0.0582116136415855,
+            "angularVelocity": -0.0000032127528402629677,
+            "velocityX": -3.622171532005757,
+            "velocityY": -1.091102654112135,
+            "timestamp": 1.026354761435145
+        },
+        {
+            "x": 4.918641828284195,
+            "y": 3.9369980738928456,
+            "heading": -0.09326111746178506,
+            "angularVelocity": -0.4097940221510551,
+            "velocityX": -3.2137917719873403,
+            "velocityY": -1.664292301495839,
+            "timestamp": 1.1118843248880736
+        },
+        {
+            "x": 4.689025118769517,
+            "y": 3.755353610578967,
+            "heading": -0.16867561276178636,
+            "angularVelocity": -0.8817360016283226,
+            "velocityX": -2.6846472756878708,
+            "velocityY": -2.1237623107224977,
+            "timestamp": 1.1974138883410024
+        },
+        {
+            "x": 4.510985991586203,
+            "y": 3.542062267326439,
+            "heading": -0.2715725395002121,
+            "angularVelocity": -1.2030568447254515,
+            "velocityX": -2.0816092120158958,
+            "velocityY": -2.493773318156982,
+            "timestamp": 1.282943451793931
+        },
+        {
+            "x": 4.39344009542189,
+            "y": 3.3364721307805043,
+            "heading": -0.3792171957634167,
+            "angularVelocity": -1.2585666513128753,
+            "velocityX": -1.3743305989045989,
+            "velocityY": -2.4037318588569803,
+            "timestamp": 1.3684730152468598
+        },
+        {
+            "x": 4.321460732883776,
+            "y": 3.1711461674458645,
+            "heading": -0.4697306425856333,
+            "angularVelocity": -1.0582708851545903,
+            "velocityX": -0.8415728975131295,
+            "velocityY": -1.932968632835679,
+            "timestamp": 1.4540025786997886
+        },
+        {
+            "x": 4.280782976343733,
+            "y": 3.057632956228743,
+            "heading": -0.5333100471714295,
+            "angularVelocity": -0.7433617338733072,
+            "velocityX": -0.475598785938263,
+            "velocityY": -1.3271809960727023,
+            "timestamp": 1.5395321421527173
         },
         {
             "x": 4.263,
             "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0.3433937718713025,
-            "velocityX": -0.3849958701158514,
-            "velocityY": -0.5705852008687156,
-            "timestamp": 1.5834596689578961
+            "heading": -0.566,
+            "angularVelocity": -0.38220647351440606,
+            "velocityX": -0.20791613596297567,
+            "velocityY": -0.6738366700592523,
+            "timestamp": 1.625061705605646
         },
         {
             "x": 4.263,
             "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -1.6359372913753935e-30,
+            "heading": -0.566,
+            "angularVelocity": -1.9453350650223036e-32,
             "velocityX": 0,
-            "velocityY": -3.7530428699783824e-32,
-            "timestamp": 1.6667996515346275
+            "velocityY": 3.675541041669649e-33,
+            "timestamp": 1.7105912690585747
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.traj
index 9d9edbcb..90749c58 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.traj	
@@ -3,191 +3,191 @@
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 7.712459531346682e-31,
-            "velocityX": 8.463399539512725e-31,
-            "velocityY": -2.525463677436265e-31,
+            "heading": 0,
+            "angularVelocity": 7.446350164213638e-30,
+            "velocityX": -3.476389258222187e-32,
+            "velocityY": 2.4606016306403766e-37,
             "timestamp": 0
         },
         {
-            "x": 7.643960549352244,
-            "y": 5.758768623179225,
-            "heading": 3.0545306432351627,
-            "angularVelocity": -1.025550451563212,
-            "velocityX": -0.43243890307482463,
-            "velocityY": -0.45873991856878515,
-            "timestamp": 0.0833399825767314
-        },
-        {
-            "x": 7.572007137321917,
-            "y": 5.681963480289103,
-            "heading": 2.882145272333543,
-            "angularVelocity": -2.0684594065387945,
-            "velocityX": -0.8633720551126874,
-            "velocityY": -0.9215881803119774,
-            "timestamp": 0.1666799651534628
-        },
-        {
-            "x": 7.462165154490984,
-            "y": 5.566757202432317,
-            "heading": 2.631422178575744,
-            "angularVelocity": -3.008437079128932,
-            "velocityX": -1.3179986296469572,
-            "velocityY": -1.382365034102495,
-            "timestamp": 0.2500199477301942
-        },
-        {
-            "x": 7.311173429444628,
-            "y": 5.412128972668698,
-            "heading": 2.3433148642731845,
-            "angularVelocity": -3.457011933465413,
-            "velocityX": -1.8117561388657581,
-            "velocityY": -1.8553907138299572,
-            "timestamp": 0.3333599303069256
-        },
-        {
-            "x": 7.123843168628933,
-            "y": 5.227542500531133,
-            "heading": 2.191575313033137,
-            "angularVelocity": -1.820729337210275,
-            "velocityX": -2.2477837770510787,
-            "velocityY": -2.21486093985698,
-            "timestamp": 0.41669991288365704
-        },
-        {
-            "x": 6.902601176392792,
-            "y": 5.010431192670728,
-            "heading": 2.175785118248912,
-            "angularVelocity": -0.1894672196467848,
-            "velocityX": -2.6546920865077652,
-            "velocityY": -2.6051278287766304,
-            "timestamp": 0.5000398954603884
-        },
-        {
-            "x": 6.682324869385203,
-            "y": 4.7848793693995555,
-            "heading": 2.175784967765777,
-            "angularVelocity": -0.0000018056535426780284,
-            "velocityX": -2.643104788326184,
-            "velocityY": -2.7064059326328107,
-            "timestamp": 0.5833798780371198
-        },
-        {
-            "x": 6.427295318194512,
-            "y": 4.59952808942922,
-            "heading": 2.175784213925318,
-            "angularVelocity": -0.00000904536377316099,
-            "velocityX": -3.060110445258255,
-            "velocityY": -2.224037901611934,
-            "timestamp": 0.6667198606138511
-        },
-        {
-            "x": 6.1722637689115825,
-            "y": 4.4141795587676,
-            "heading": 2.1757834600140606,
-            "angularVelocity": -0.000009046213285018581,
-            "velocityX": -3.0601344204519796,
-            "velocityY": -2.224004912539731,
-            "timestamp": 0.7500598431905825
-        },
-        {
-            "x": 5.906721685974762,
-            "y": 4.244230836092675,
-            "heading": 2.175782696018566,
-            "angularVelocity": -0.000009167214470514032,
-            "velocityX": -3.1862507613597577,
-            "velocityY": -2.039221960701194,
-            "timestamp": 0.8333998257673139
-        },
-        {
-            "x": 5.614573321214709,
-            "y": 4.1257208216835135,
-            "heading": 2.175782627659757,
-            "angularVelocity": -8.202402673559022e-7,
-            "velocityX": -3.5055006699943965,
-            "velocityY": -1.4220067096852238,
-            "timestamp": 0.9167398083440452
-        },
-        {
-            "x": 5.32091408424666,
-            "y": 4.0110057741909,
-            "heading": 2.1757827056250556,
-            "angularVelocity": 9.355089415031885e-7,
-            "velocityX": -3.523629689959162,
-            "velocityY": -1.3764707400435987,
-            "timestamp": 1.0000797909207766
-        },
-        {
-            "x": 5.051276753694785,
-            "y": 3.8476320535360697,
-            "heading": 2.1757829434200273,
-            "angularVelocity": 0.0000028533119900283243,
-            "velocityX": -3.235389811890331,
-            "velocityY": -1.9603282314633543,
-            "timestamp": 1.083419773497508
-        },
-        {
-            "x": 4.804772075990701,
-            "y": 3.6532031340051674,
-            "heading": 2.180086334431566,
-            "angularVelocity": 0.05163657200885012,
-            "velocityX": -2.957820125257733,
-            "velocityY": -2.332960885273652,
-            "timestamp": 1.1667597560742393
-        },
-        {
-            "x": 4.605798608727452,
-            "y": 3.4589597133454815,
-            "heading": 2.2977487638281207,
-            "angularVelocity": 1.411836501024247,
-            "velocityX": -2.387491107045202,
-            "velocityY": -2.330735076418397,
-            "timestamp": 1.2500997386509707
-        },
-        {
-            "x": 4.462499334629702,
-            "y": 3.280406115781069,
-            "heading": 2.4052460747163282,
-            "angularVelocity": 1.28986481115753,
-            "velocityX": -1.7194540923477422,
-            "velocityY": -2.142472220941705,
-            "timestamp": 1.333439721227702
-        },
-        {
-            "x": 4.360731419840454,
-            "y": 3.1416564242722327,
-            "heading": 2.488750866660265,
-            "angularVelocity": 1.0019775546155592,
-            "velocityX": -1.2211175433778025,
-            "velocityY": -1.6648634571178227,
-            "timestamp": 1.4167797038044334
-        },
-        {
-            "x": 4.295085549107569,
-            "y": 3.0475525606989393,
-            "heading": 2.5453815690352877,
-            "angularVelocity": 0.6795142094358206,
-            "velocityX": -0.7876875984758661,
-            "velocityY": -1.129156266461315,
-            "timestamp": 1.5001196863811648
+            "x": 7.6436015698643995,
+            "y": 5.74806653588022,
+            "heading": -0.0038807165751560974,
+            "angularVelocity": -0.045372809336175934,
+            "velocityX": -0.4255654848002544,
+            "velocityY": -0.572123393880186,
+            "timestamp": 0.08552956345292874
+        },
+        {
+            "x": 7.570804707716712,
+            "y": 5.6501996114987145,
+            "heading": -0.011642164214821673,
+            "angularVelocity": -0.09074578807989701,
+            "velocityX": -0.8511309915401587,
+            "velocityY": -1.1442467426525054,
+            "timestamp": 0.17105912690585748
+        },
+        {
+            "x": 7.461609412342726,
+            "y": 5.503399231866818,
+            "heading": -0.02328436718357624,
+            "angularVelocity": -0.136119050521775,
+            "velocityX": -1.2766965124764296,
+            "velocityY": -1.7163700328330225,
+            "timestamp": 0.25658869035878623
+        },
+        {
+            "x": 7.3160156842382476,
+            "y": 5.307665404737857,
+            "heading": -0.038807352623976644,
+            "angularVelocity": -0.18149263031072865,
+            "velocityX": -1.7022620276158245,
+            "velocityY": -2.288493232362672,
+            "timestamp": 0.34211825381171496
+        },
+        {
+            "x": 7.134023531165023,
+            "y": 5.062998148299065,
+            "heading": -0.05821111114579378,
+            "angularVelocity": -0.22686610031040333,
+            "velocityX": -2.127827452005934,
+            "velocityY": -2.8606162192496707,
+            "timestamp": 0.4276478172646437
+        },
+        {
+            "x": 6.9409164317403365,
+            "y": 4.803390071813602,
+            "heading": -0.058211113467083825,
+            "angularVelocity": -2.714020682501112e-8,
+            "velocityX": -2.2577818900123794,
+            "velocityY": -3.0353022511138943,
+            "timestamp": 0.5131773807175725
+        },
+        {
+            "x": 6.70968017178257,
+            "y": 4.577079452070077,
+            "heading": -0.05821111591888997,
+            "angularVelocity": -2.866618337367041e-8,
+            "velocityX": -2.7035828387575864,
+            "velocityY": -2.645992924634468,
+            "timestamp": 0.5987069441705012
+        },
+        {
+            "x": 6.4400791107327935,
+            "y": 4.398188508585333,
+            "heading": -0.05821111892081074,
+            "angularVelocity": -3.509804859460385e-8,
+            "velocityX": -3.152138864804934,
+            "velocityY": -2.091568532127445,
+            "timestamp": 0.6842365076234299
+        },
+        {
+            "x": 6.141694589328426,
+            "y": 4.273075039260189,
+            "heading": -0.058211144902897714,
+            "angularVelocity": -3.0377902004237926e-7,
+            "velocityX": -3.488671160686832,
+            "velocityY": -1.462809632999017,
+            "timestamp": 0.7697660710763586
+        },
+        {
+            "x": 5.825131041705898,
+            "y": 4.206185466847777,
+            "heading": -0.05821124348692034,
+            "angularVelocity": -0.000001152630957627144,
+            "velocityX": -3.7012178578082944,
+            "velocityY": -0.7820637649954203,
+            "timestamp": 0.8552956345292874
+        },
+        {
+            "x": 5.503318785454965,
+            "y": 4.1726658015863975,
+            "heading": -0.058211338856237596,
+            "angularVelocity": -0.0000011150450605169934,
+            "velocityX": -3.76258504380235,
+            "velocityY": -0.39190735820634004,
+            "timestamp": 0.9408251979822161
+        },
+        {
+            "x": 5.1935160355708865,
+            "y": 4.079344267897855,
+            "heading": -0.0582116136415855,
+            "angularVelocity": -0.0000032127528402629677,
+            "velocityX": -3.622171532005757,
+            "velocityY": -1.091102654112135,
+            "timestamp": 1.026354761435145
+        },
+        {
+            "x": 4.918641828284195,
+            "y": 3.9369980738928456,
+            "heading": -0.09326111746178506,
+            "angularVelocity": -0.4097940221510551,
+            "velocityX": -3.2137917719873403,
+            "velocityY": -1.664292301495839,
+            "timestamp": 1.1118843248880736
+        },
+        {
+            "x": 4.689025118769517,
+            "y": 3.755353610578967,
+            "heading": -0.16867561276178636,
+            "angularVelocity": -0.8817360016283226,
+            "velocityX": -2.6846472756878708,
+            "velocityY": -2.1237623107224977,
+            "timestamp": 1.1974138883410024
+        },
+        {
+            "x": 4.510985991586203,
+            "y": 3.542062267326439,
+            "heading": -0.2715725395002121,
+            "angularVelocity": -1.2030568447254515,
+            "velocityX": -2.0816092120158958,
+            "velocityY": -2.493773318156982,
+            "timestamp": 1.282943451793931
+        },
+        {
+            "x": 4.39344009542189,
+            "y": 3.3364721307805043,
+            "heading": -0.3792171957634167,
+            "angularVelocity": -1.2585666513128753,
+            "velocityX": -1.3743305989045989,
+            "velocityY": -2.4037318588569803,
+            "timestamp": 1.3684730152468598
+        },
+        {
+            "x": 4.321460732883776,
+            "y": 3.1711461674458645,
+            "heading": -0.4697306425856333,
+            "angularVelocity": -1.0582708851545903,
+            "velocityX": -0.8415728975131295,
+            "velocityY": -1.932968632835679,
+            "timestamp": 1.4540025786997886
+        },
+        {
+            "x": 4.280782976343733,
+            "y": 3.057632956228743,
+            "heading": -0.5333100471714295,
+            "angularVelocity": -0.7433617338733072,
+            "velocityX": -0.475598785938263,
+            "velocityY": -1.3271809960727023,
+            "timestamp": 1.5395321421527173
         },
         {
             "x": 4.263,
             "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0.3433937718713025,
-            "velocityX": -0.3849958701158514,
-            "velocityY": -0.5705852008687156,
-            "timestamp": 1.5834596689578961
+            "heading": -0.566,
+            "angularVelocity": -0.38220647351440606,
+            "velocityX": -0.20791613596297567,
+            "velocityY": -0.6738366700592523,
+            "timestamp": 1.625061705605646
         },
         {
             "x": 4.263,
             "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -1.6359372913753935e-30,
+            "heading": -0.566,
+            "angularVelocity": -1.9453350650223036e-32,
             "velocityX": 0,
-            "velocityY": -3.7530428699783824e-32,
-            "timestamp": 1.6667996515346275
+            "velocityY": 3.675541041669649e-33,
+            "timestamp": 1.7105912690585747
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.1.traj
index 16009ccb..a8bed878 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.1.traj	
@@ -3,200 +3,200 @@
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": -3.1814997382550796e-29,
-            "velocityX": -1.8229862739599686e-32,
-            "velocityY": -3.244948758863147e-31,
+            "heading": 0,
+            "angularVelocity": 5.4178362746118835e-30,
+            "velocityX": -4.378571579060787e-32,
+            "velocityY": 1.8329583672175558e-31,
             "timestamp": 0
         },
         {
-            "x": 7.617836558696546,
-            "y": 5.830157448001668,
-            "heading": 3.1302887872729133,
-            "angularVelocity": -0.10560741421513488,
-            "velocityX": -0.676014466912156,
-            "velocityY": 0.36058033572491816,
-            "timestamp": 0.0919557854839981
-        },
-        {
-            "x": 7.493509680194616,
-            "y": 5.896472329123028,
-            "heading": 3.1108653372490442,
-            "angularVelocity": -0.21122597041214858,
-            "velocityX": -1.3520288891835412,
-            "velocityY": 0.7211605096114337,
-            "timestamp": 0.1839115709679962
-        },
-        {
-            "x": 7.307019376427131,
-            "y": 5.9959446237909395,
-            "heading": 3.0817278950569205,
-            "angularVelocity": -0.316863610470641,
-            "velocityX": -2.028043181686891,
-            "velocityY": 1.0817404706441378,
-            "timestamp": 0.27586735645199434
-        },
-        {
-            "x": 7.058365675689159,
-            "y": 6.128574306111159,
-            "heading": 3.0428740808433132,
-            "angularVelocity": -0.4225271309369522,
-            "velocityX": -2.704057166487276,
-            "velocityY": 1.442320150082345,
-            "timestamp": 0.3678231419359924
-        },
-        {
-            "x": 6.75143523206277,
-            "y": 6.292289169474125,
-            "heading": 3.0428740759198862,
-            "angularVelocity": -5.354123851149769e-8,
-            "velocityX": -3.3378045982740363,
-            "velocityY": 1.7803650145694723,
-            "timestamp": 0.4597789274199905
-        },
-        {
-            "x": 6.443010139077041,
-            "y": 6.453170540992539,
-            "heading": 3.042874070986951,
-            "angularVelocity": -5.3644645461672824e-8,
-            "velocityX": -3.354058598514181,
-            "velocityY": 1.7495513813691466,
-            "timestamp": 0.5517347129039886
-        },
-        {
-            "x": 6.108481192244814,
-            "y": 6.548560466077805,
-            "heading": 3.0428740880877947,
-            "angularVelocity": 1.8596810871163974e-7,
-            "velocityX": -3.637932568043149,
-            "velocityY": 1.037345552356419,
-            "timestamp": 0.6436904983879866
-        },
-        {
-            "x": 5.76231566928878,
-            "y": 6.582886564044467,
-            "heading": 3.0428741422485728,
-            "angularVelocity": 5.889871735366024e-7,
-            "velocityX": -3.764477907877501,
-            "velocityY": 0.3732891605024142,
-            "timestamp": 0.7356462838719847
-        },
-        {
-            "x": 5.41614197674267,
-            "y": 6.617130174737203,
-            "heading": 3.042874196415433,
-            "angularVelocity": 5.890533147506573e-7,
-            "velocityX": -3.764566750466717,
-            "velocityY": 0.37239212859200393,
-            "timestamp": 0.8276020693559828
-        },
-        {
-            "x": 5.068621146465333,
-            "y": 6.632561444279093,
-            "heading": 3.0428742397901987,
-            "angularVelocity": 4.716915377702693e-7,
-            "velocityX": -3.779216592498277,
-            "velocityY": 0.16781183979527767,
-            "timestamp": 0.9195578548399809
-        },
-        {
-            "x": 4.721100288864093,
-            "y": 6.647992098459583,
-            "heading": 3.042874283164768,
-            "angularVelocity": 4.716894023860314e-7,
-            "velocityX": -3.779216889640001,
-            "velocityY": 0.16780514786832676,
-            "timestamp": 1.011513640323979
-        },
-        {
-            "x": 4.3735794260416885,
-            "y": 6.663422635051171,
-            "heading": 3.042874326539347,
-            "angularVelocity": 4.716895035423872e-7,
-            "velocityX": -3.779216946419108,
-            "velocityY": 0.16780386911352246,
-            "timestamp": 1.1034694258079771
-        },
-        {
-            "x": 4.026058559390409,
-            "y": 6.678853085409858,
-            "heading": 3.042874369913944,
-            "angularVelocity": 4.716897058235115e-7,
-            "velocityX": -3.779216988057318,
-            "velocityY": 0.1678029313486884,
-            "timestamp": 1.1954252112919752
-        },
-        {
-            "x": 3.678537689463977,
-            "y": 6.69428346200604,
-            "heading": 3.042874413288561,
-            "angularVelocity": 4.7168991769457604e-7,
-            "velocityX": -3.779217023673906,
-            "velocityY": 0.16780212919682533,
-            "timestamp": 1.2873809967759733
-        },
-        {
-            "x": 3.3310168167092282,
-            "y": 6.7097137749028235,
-            "heading": 3.042874456663209,
-            "angularVelocity": 4.7169025361622227e-7,
-            "velocityX": -3.7792170544312707,
-            "velocityY": 0.16780143647915202,
-            "timestamp": 1.3793367822599714
-        },
-        {
-            "x": 2.9915174484895957,
-            "y": 6.725380176084037,
-            "heading": 3.069608362610464,
-            "angularVelocity": 0.2907256547975106,
-            "velocityX": -3.6919848646033437,
-            "velocityY": 0.1703688473624042,
-            "timestamp": 1.4712925677439694
-        },
-        {
-            "x": 2.7138500007590802,
-            "y": 6.7384126132729225,
-            "heading": 3.2277640151767573,
-            "angularVelocity": 1.7199097559099776,
-            "velocityX": -3.019575617445345,
-            "velocityY": 0.14172503796570737,
-            "timestamp": 1.5632483532279675
-        },
-        {
-            "x": 2.5064920875678816,
-            "y": 6.747862712816966,
-            "heading": 3.3721576212980104,
-            "angularVelocity": 1.570250369362352,
-            "velocityX": -2.2549740845537376,
-            "velocityY": 0.1027678627756077,
-            "timestamp": 1.6552041387119656
-        },
-        {
-            "x": 2.368714185355249,
-            "y": 6.753998249997193,
-            "heading": 3.477486882189963,
-            "angularVelocity": 1.14543375751254,
-            "velocityX": -1.4983059683243996,
-            "velocityY": 0.06672268795197085,
-            "timestamp": 1.7471599241959637
+            "x": 7.617836111630202,
+            "y": 5.830159453299192,
+            "heading": -0.009839435811264896,
+            "angularVelocity": -0.10699973937901981,
+            "velocityX": -0.6760062245377694,
+            "velocityY": 0.3605951529154081,
+            "timestamp": 0.09195756801248979
+        },
+        {
+            "x": 7.493508339255607,
+            "y": 5.896478344435856,
+            "heading": -0.0295193862966414,
+            "angularVelocity": -0.21401121093919698,
+            "velocityX": -1.352012401607965,
+            "velocityY": 0.721190137691066,
+            "timestamp": 0.18391513602497958
+        },
+        {
+            "x": 7.3070166954300815,
+            "y": 5.995956653040542,
+            "heading": -0.059041701653073396,
+            "angularVelocity": -0.32104280261546536,
+            "velocityX": -2.0280184421601457,
+            "velocityY": 1.0817849009575307,
+            "timestamp": 0.2758727040374694
+        },
+        {
+            "x": 7.0583612095128645,
+            "y": 6.128594352415652,
+            "heading": -0.09840889768284368,
+            "angularVelocity": -0.42810175258683864,
+            "velocityX": -2.7040241634429143,
+            "velocityY": 1.4423793738987822,
+            "timestamp": 0.36783027204995916
+        },
+        {
+            "x": 6.751428460841339,
+            "y": 6.292319222162274,
+            "heading": -0.09840890267081659,
+            "angularVelocity": -5.4242114104221296e-8,
+            "velocityX": -3.3377649638345908,
+            "velocityY": 1.780439318756071,
+            "timestamp": 0.45978784006244894
+        },
+        {
+            "x": 6.443018724200086,
+            "y": 6.45324460704626,
+            "heading": -0.09840890766830068,
+            "angularVelocity": -5.4345544379257814e-8,
+            "velocityX": -3.3538265887954406,
+            "velocityY": 1.7499960945262094,
+            "timestamp": 0.5517454080749388
+        },
+        {
+            "x": 6.108495580653603,
+            "y": 6.54867946433224,
+            "heading": -0.0984088906849307,
+            "angularVelocity": 1.846870285822299e-7,
+            "velocityX": -3.637798941149115,
+            "velocityY": 1.037814063036315,
+            "timestamp": 0.6437029760874285
+        },
+        {
+            "x": 5.762312734581719,
+            "y": 6.58289903236131,
+            "heading": -0.09840883650367563,
+            "angularVelocity": 5.891984340231216e-7,
+            "velocityX": -3.7645933179188145,
+            "velocityY": 0.37212345616210646,
+            "timestamp": 0.7356605440999183
+        },
+        {
+            "x": 5.416121743024081,
+            "y": 6.617036096003367,
+            "heading": -0.09840878231628362,
+            "angularVelocity": 5.892651705038846e-7,
+            "velocityX": -3.7646818966614886,
+            "velocityY": 0.37122625554234295,
+            "timestamp": 0.8276181121124081
+        },
+        {
+            "x": 5.0685946034380285,
+            "y": 6.632477283287523,
+            "heading": -0.09840873884834453,
+            "angularVelocity": 4.7269561430407727e-7,
+            "velocityX": -3.7792119463060407,
+            "velocityY": 0.16791643817786392,
+            "timestamp": 0.9195756801248979
+        },
+        {
+            "x": 4.72106743655186,
+            "y": 6.647917856129691,
+            "heading": -0.09840869538060193,
+            "angularVelocity": 4.726934774100163e-7,
+            "velocityX": -3.779212243183364,
+            "velocityY": 0.1679097563788475,
+            "timestamp": 1.0115332481373878
+        },
+        {
+            "x": 4.373540264440576,
+            "y": 6.66335831136751,
+            "heading": -0.09840865191285005,
+            "angularVelocity": 4.7269357834441634e-7,
+            "velocityX": -3.779212300004299,
+            "velocityY": 0.16790847748084475,
+            "timestamp": 1.1034908161498775
+        },
+        {
+            "x": 4.026013088497698,
+            "y": 6.678798680365008,
+            "heading": -0.09840860844507962,
+            "angularVelocity": 4.726937801388588e-7,
+            "velocityX": -3.7792123416712937,
+            "velocityY": 0.16790753965352348,
+            "timestamp": 1.1954483841623673
+        },
+        {
+            "x": 3.6784859092773967,
+            "y": 6.694238975594936,
+            "heading": -0.09840856497728971,
+            "angularVelocity": 4.7269399197494456e-7,
+            "velocityX": -3.7792123773118855,
+            "velocityY": 0.1679067374621128,
+            "timestamp": 1.287405952174857
+        },
+        {
+            "x": 3.3309587272270753,
+            "y": 6.709679207126903,
+            "heading": -0.09840852150946863,
+            "angularVelocity": 4.72694330698846e-7,
+            "velocityX": -3.779212408087172,
+            "velocityY": 0.16790604477348844,
+            "timestamp": 1.3793635201873469
+        },
+        {
+            "x": 2.991492172408929,
+            "y": 6.7253546024137885,
+            "heading": -0.07153351873489558,
+            "angularVelocity": 0.29225438814261445,
+            "velocityX": -3.691556466261044,
+            "velocityY": 0.17046335201859075,
+            "timestamp": 1.4713210881998366
+        },
+        {
+            "x": 2.7138316831246163,
+            "y": 6.738400020288083,
+            "heading": 0.08699256852088375,
+            "angularVelocity": 1.723904738696962,
+            "velocityX": -3.019441415051349,
+            "velocityY": 0.1418634502439354,
+            "timestamp": 1.5632786562123264
+        },
+        {
+            "x": 2.5064819973568118,
+            "y": 6.747857404358727,
+            "heading": 0.23167973935786532,
+            "angularVelocity": 1.5734123244465315,
+            "velocityX": -2.254840903792083,
+            "velocityY": 0.10284508687050122,
+            "timestamp": 1.6552362242248162
+        },
+        {
+            "x": 2.36871059980762,
+            "y": 6.753996751301636,
+            "heading": 0.3372042620417382,
+            "angularVelocity": 1.1475349442640808,
+            "velocityX": -1.4982061892989513,
+            "velocityY": 0.0667628241546722,
+            "timestamp": 1.747193792237306
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 0.6057087620584267,
-            "velocityX": -0.7472524430472809,
-            "velocityY": 0.03264340559983963,
-            "timestamp": 1.8391157096799617
+            "heading": 0.393,
+            "angularVelocity": 0.6067552585849546,
+            "velocityX": -0.7471989667918153,
+            "velocityY": 0.03265907051778923,
+            "timestamp": 1.8391513602497958
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": -1.2870903265044466e-29,
+            "heading": 0.393,
+            "angularVelocity": -8.637511489261282e-30,
             "velocityX": 0,
-            "velocityY": 1.2655145174510225e-29,
-            "timestamp": 1.9310714951639598
+            "velocityY": 5.8539743527830045e-30,
+            "timestamp": 1.9311089282622855
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.traj
index 16009ccb..a8bed878 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.traj	
@@ -3,200 +3,200 @@
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": -3.1814997382550796e-29,
-            "velocityX": -1.8229862739599686e-32,
-            "velocityY": -3.244948758863147e-31,
+            "heading": 0,
+            "angularVelocity": 5.4178362746118835e-30,
+            "velocityX": -4.378571579060787e-32,
+            "velocityY": 1.8329583672175558e-31,
             "timestamp": 0
         },
         {
-            "x": 7.617836558696546,
-            "y": 5.830157448001668,
-            "heading": 3.1302887872729133,
-            "angularVelocity": -0.10560741421513488,
-            "velocityX": -0.676014466912156,
-            "velocityY": 0.36058033572491816,
-            "timestamp": 0.0919557854839981
-        },
-        {
-            "x": 7.493509680194616,
-            "y": 5.896472329123028,
-            "heading": 3.1108653372490442,
-            "angularVelocity": -0.21122597041214858,
-            "velocityX": -1.3520288891835412,
-            "velocityY": 0.7211605096114337,
-            "timestamp": 0.1839115709679962
-        },
-        {
-            "x": 7.307019376427131,
-            "y": 5.9959446237909395,
-            "heading": 3.0817278950569205,
-            "angularVelocity": -0.316863610470641,
-            "velocityX": -2.028043181686891,
-            "velocityY": 1.0817404706441378,
-            "timestamp": 0.27586735645199434
-        },
-        {
-            "x": 7.058365675689159,
-            "y": 6.128574306111159,
-            "heading": 3.0428740808433132,
-            "angularVelocity": -0.4225271309369522,
-            "velocityX": -2.704057166487276,
-            "velocityY": 1.442320150082345,
-            "timestamp": 0.3678231419359924
-        },
-        {
-            "x": 6.75143523206277,
-            "y": 6.292289169474125,
-            "heading": 3.0428740759198862,
-            "angularVelocity": -5.354123851149769e-8,
-            "velocityX": -3.3378045982740363,
-            "velocityY": 1.7803650145694723,
-            "timestamp": 0.4597789274199905
-        },
-        {
-            "x": 6.443010139077041,
-            "y": 6.453170540992539,
-            "heading": 3.042874070986951,
-            "angularVelocity": -5.3644645461672824e-8,
-            "velocityX": -3.354058598514181,
-            "velocityY": 1.7495513813691466,
-            "timestamp": 0.5517347129039886
-        },
-        {
-            "x": 6.108481192244814,
-            "y": 6.548560466077805,
-            "heading": 3.0428740880877947,
-            "angularVelocity": 1.8596810871163974e-7,
-            "velocityX": -3.637932568043149,
-            "velocityY": 1.037345552356419,
-            "timestamp": 0.6436904983879866
-        },
-        {
-            "x": 5.76231566928878,
-            "y": 6.582886564044467,
-            "heading": 3.0428741422485728,
-            "angularVelocity": 5.889871735366024e-7,
-            "velocityX": -3.764477907877501,
-            "velocityY": 0.3732891605024142,
-            "timestamp": 0.7356462838719847
-        },
-        {
-            "x": 5.41614197674267,
-            "y": 6.617130174737203,
-            "heading": 3.042874196415433,
-            "angularVelocity": 5.890533147506573e-7,
-            "velocityX": -3.764566750466717,
-            "velocityY": 0.37239212859200393,
-            "timestamp": 0.8276020693559828
-        },
-        {
-            "x": 5.068621146465333,
-            "y": 6.632561444279093,
-            "heading": 3.0428742397901987,
-            "angularVelocity": 4.716915377702693e-7,
-            "velocityX": -3.779216592498277,
-            "velocityY": 0.16781183979527767,
-            "timestamp": 0.9195578548399809
-        },
-        {
-            "x": 4.721100288864093,
-            "y": 6.647992098459583,
-            "heading": 3.042874283164768,
-            "angularVelocity": 4.716894023860314e-7,
-            "velocityX": -3.779216889640001,
-            "velocityY": 0.16780514786832676,
-            "timestamp": 1.011513640323979
-        },
-        {
-            "x": 4.3735794260416885,
-            "y": 6.663422635051171,
-            "heading": 3.042874326539347,
-            "angularVelocity": 4.716895035423872e-7,
-            "velocityX": -3.779216946419108,
-            "velocityY": 0.16780386911352246,
-            "timestamp": 1.1034694258079771
-        },
-        {
-            "x": 4.026058559390409,
-            "y": 6.678853085409858,
-            "heading": 3.042874369913944,
-            "angularVelocity": 4.716897058235115e-7,
-            "velocityX": -3.779216988057318,
-            "velocityY": 0.1678029313486884,
-            "timestamp": 1.1954252112919752
-        },
-        {
-            "x": 3.678537689463977,
-            "y": 6.69428346200604,
-            "heading": 3.042874413288561,
-            "angularVelocity": 4.7168991769457604e-7,
-            "velocityX": -3.779217023673906,
-            "velocityY": 0.16780212919682533,
-            "timestamp": 1.2873809967759733
-        },
-        {
-            "x": 3.3310168167092282,
-            "y": 6.7097137749028235,
-            "heading": 3.042874456663209,
-            "angularVelocity": 4.7169025361622227e-7,
-            "velocityX": -3.7792170544312707,
-            "velocityY": 0.16780143647915202,
-            "timestamp": 1.3793367822599714
-        },
-        {
-            "x": 2.9915174484895957,
-            "y": 6.725380176084037,
-            "heading": 3.069608362610464,
-            "angularVelocity": 0.2907256547975106,
-            "velocityX": -3.6919848646033437,
-            "velocityY": 0.1703688473624042,
-            "timestamp": 1.4712925677439694
-        },
-        {
-            "x": 2.7138500007590802,
-            "y": 6.7384126132729225,
-            "heading": 3.2277640151767573,
-            "angularVelocity": 1.7199097559099776,
-            "velocityX": -3.019575617445345,
-            "velocityY": 0.14172503796570737,
-            "timestamp": 1.5632483532279675
-        },
-        {
-            "x": 2.5064920875678816,
-            "y": 6.747862712816966,
-            "heading": 3.3721576212980104,
-            "angularVelocity": 1.570250369362352,
-            "velocityX": -2.2549740845537376,
-            "velocityY": 0.1027678627756077,
-            "timestamp": 1.6552041387119656
-        },
-        {
-            "x": 2.368714185355249,
-            "y": 6.753998249997193,
-            "heading": 3.477486882189963,
-            "angularVelocity": 1.14543375751254,
-            "velocityX": -1.4983059683243996,
-            "velocityY": 0.06672268795197085,
-            "timestamp": 1.7471599241959637
+            "x": 7.617836111630202,
+            "y": 5.830159453299192,
+            "heading": -0.009839435811264896,
+            "angularVelocity": -0.10699973937901981,
+            "velocityX": -0.6760062245377694,
+            "velocityY": 0.3605951529154081,
+            "timestamp": 0.09195756801248979
+        },
+        {
+            "x": 7.493508339255607,
+            "y": 5.896478344435856,
+            "heading": -0.0295193862966414,
+            "angularVelocity": -0.21401121093919698,
+            "velocityX": -1.352012401607965,
+            "velocityY": 0.721190137691066,
+            "timestamp": 0.18391513602497958
+        },
+        {
+            "x": 7.3070166954300815,
+            "y": 5.995956653040542,
+            "heading": -0.059041701653073396,
+            "angularVelocity": -0.32104280261546536,
+            "velocityX": -2.0280184421601457,
+            "velocityY": 1.0817849009575307,
+            "timestamp": 0.2758727040374694
+        },
+        {
+            "x": 7.0583612095128645,
+            "y": 6.128594352415652,
+            "heading": -0.09840889768284368,
+            "angularVelocity": -0.42810175258683864,
+            "velocityX": -2.7040241634429143,
+            "velocityY": 1.4423793738987822,
+            "timestamp": 0.36783027204995916
+        },
+        {
+            "x": 6.751428460841339,
+            "y": 6.292319222162274,
+            "heading": -0.09840890267081659,
+            "angularVelocity": -5.4242114104221296e-8,
+            "velocityX": -3.3377649638345908,
+            "velocityY": 1.780439318756071,
+            "timestamp": 0.45978784006244894
+        },
+        {
+            "x": 6.443018724200086,
+            "y": 6.45324460704626,
+            "heading": -0.09840890766830068,
+            "angularVelocity": -5.4345544379257814e-8,
+            "velocityX": -3.3538265887954406,
+            "velocityY": 1.7499960945262094,
+            "timestamp": 0.5517454080749388
+        },
+        {
+            "x": 6.108495580653603,
+            "y": 6.54867946433224,
+            "heading": -0.0984088906849307,
+            "angularVelocity": 1.846870285822299e-7,
+            "velocityX": -3.637798941149115,
+            "velocityY": 1.037814063036315,
+            "timestamp": 0.6437029760874285
+        },
+        {
+            "x": 5.762312734581719,
+            "y": 6.58289903236131,
+            "heading": -0.09840883650367563,
+            "angularVelocity": 5.891984340231216e-7,
+            "velocityX": -3.7645933179188145,
+            "velocityY": 0.37212345616210646,
+            "timestamp": 0.7356605440999183
+        },
+        {
+            "x": 5.416121743024081,
+            "y": 6.617036096003367,
+            "heading": -0.09840878231628362,
+            "angularVelocity": 5.892651705038846e-7,
+            "velocityX": -3.7646818966614886,
+            "velocityY": 0.37122625554234295,
+            "timestamp": 0.8276181121124081
+        },
+        {
+            "x": 5.0685946034380285,
+            "y": 6.632477283287523,
+            "heading": -0.09840873884834453,
+            "angularVelocity": 4.7269561430407727e-7,
+            "velocityX": -3.7792119463060407,
+            "velocityY": 0.16791643817786392,
+            "timestamp": 0.9195756801248979
+        },
+        {
+            "x": 4.72106743655186,
+            "y": 6.647917856129691,
+            "heading": -0.09840869538060193,
+            "angularVelocity": 4.726934774100163e-7,
+            "velocityX": -3.779212243183364,
+            "velocityY": 0.1679097563788475,
+            "timestamp": 1.0115332481373878
+        },
+        {
+            "x": 4.373540264440576,
+            "y": 6.66335831136751,
+            "heading": -0.09840865191285005,
+            "angularVelocity": 4.7269357834441634e-7,
+            "velocityX": -3.779212300004299,
+            "velocityY": 0.16790847748084475,
+            "timestamp": 1.1034908161498775
+        },
+        {
+            "x": 4.026013088497698,
+            "y": 6.678798680365008,
+            "heading": -0.09840860844507962,
+            "angularVelocity": 4.726937801388588e-7,
+            "velocityX": -3.7792123416712937,
+            "velocityY": 0.16790753965352348,
+            "timestamp": 1.1954483841623673
+        },
+        {
+            "x": 3.6784859092773967,
+            "y": 6.694238975594936,
+            "heading": -0.09840856497728971,
+            "angularVelocity": 4.7269399197494456e-7,
+            "velocityX": -3.7792123773118855,
+            "velocityY": 0.1679067374621128,
+            "timestamp": 1.287405952174857
+        },
+        {
+            "x": 3.3309587272270753,
+            "y": 6.709679207126903,
+            "heading": -0.09840852150946863,
+            "angularVelocity": 4.72694330698846e-7,
+            "velocityX": -3.779212408087172,
+            "velocityY": 0.16790604477348844,
+            "timestamp": 1.3793635201873469
+        },
+        {
+            "x": 2.991492172408929,
+            "y": 6.7253546024137885,
+            "heading": -0.07153351873489558,
+            "angularVelocity": 0.29225438814261445,
+            "velocityX": -3.691556466261044,
+            "velocityY": 0.17046335201859075,
+            "timestamp": 1.4713210881998366
+        },
+        {
+            "x": 2.7138316831246163,
+            "y": 6.738400020288083,
+            "heading": 0.08699256852088375,
+            "angularVelocity": 1.723904738696962,
+            "velocityX": -3.019441415051349,
+            "velocityY": 0.1418634502439354,
+            "timestamp": 1.5632786562123264
+        },
+        {
+            "x": 2.5064819973568118,
+            "y": 6.747857404358727,
+            "heading": 0.23167973935786532,
+            "angularVelocity": 1.5734123244465315,
+            "velocityX": -2.254840903792083,
+            "velocityY": 0.10284508687050122,
+            "timestamp": 1.6552362242248162
+        },
+        {
+            "x": 2.36871059980762,
+            "y": 6.753996751301636,
+            "heading": 0.3372042620417382,
+            "angularVelocity": 1.1475349442640808,
+            "velocityX": -1.4982061892989513,
+            "velocityY": 0.0667628241546722,
+            "timestamp": 1.747193792237306
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 0.6057087620584267,
-            "velocityX": -0.7472524430472809,
-            "velocityY": 0.03264340559983963,
-            "timestamp": 1.8391157096799617
+            "heading": 0.393,
+            "angularVelocity": 0.6067552585849546,
+            "velocityX": -0.7471989667918153,
+            "velocityY": 0.03265907051778923,
+            "timestamp": 1.8391513602497958
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": -1.2870903265044466e-29,
+            "heading": 0.393,
+            "angularVelocity": -8.637511489261282e-30,
             "velocityX": 0,
-            "velocityY": 1.2655145174510225e-29,
-            "timestamp": 1.9310714951639598
+            "velocityY": 5.8539743527830045e-30,
+            "timestamp": 1.9311089282622855
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.1.traj
deleted file mode 100644
index 66484d18..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.1.traj	
+++ /dev/null
@@ -1,184 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": -1.4170612284410768e-31,
-            "velocityX": -1.067311144931875e-32,
-            "velocityY": -8.392488596494602e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 7.598760740640866,
-            "y": 5.839328600808308,
-            "heading": 3.1292747695902707,
-            "angularVelocity": -0.10229725511121834,
-            "velocityX": -0.7748601122982537,
-            "velocityY": 0.40373022396426267,
-            "timestamp": 0.10484377511467942
-        },
-        {
-            "x": 7.436282224463055,
-            "y": 5.923985787899839,
-            "heading": 3.1078233746588477,
-            "angularVelocity": -0.2046034197830007,
-            "velocityX": -1.5497202003656476,
-            "velocityY": 0.8074603093882498,
-            "timestamp": 0.20968755022935884
-        },
-        {
-            "x": 7.19256446073334,
-            "y": 6.050971542878038,
-            "heading": 3.0756442181443964,
-            "angularVelocity": -0.30692481722690457,
-            "velocityX": -2.3245802000465203,
-            "velocityY": 1.2111902193458808,
-            "timestamp": 0.31453132534403827
-        },
-        {
-            "x": 6.8676074741032105,
-            "y": 6.220285840922999,
-            "heading": 3.032735095907097,
-            "angularVelocity": -0.4092672377579469,
-            "velocityX": -3.099439964601499,
-            "velocityY": 1.614919892571246,
-            "timestamp": 0.4193751004587177
-        },
-        {
-            "x": 6.515870869646968,
-            "y": 6.403553456428483,
-            "heading": 3.032735091775592,
-            "angularVelocity": -3.94062953033438e-8,
-            "velocityX": -3.3548639780617218,
-            "velocityY": 1.7480066442192022,
-            "timestamp": 0.5242188755733971
-        },
-        {
-            "x": 6.1437635937584405,
-            "y": 6.5408190150249235,
-            "heading": 3.032735095078602,
-            "angularVelocity": 3.1504111682057e-8,
-            "velocityX": -3.5491594563579234,
-            "velocityY": 1.3092389934099258,
-            "timestamp": 0.6290626506880765
-        },
-        {
-            "x": 5.750083988314003,
-            "y": 6.589006339833596,
-            "heading": 3.032735167102088,
-            "angularVelocity": 6.86960058704524e-7,
-            "velocityX": -3.754916350673425,
-            "velocityY": 0.45961073755656895,
-            "timestamp": 0.733906425802756
-        },
-        {
-            "x": 5.355289339691467,
-            "y": 6.626991015951962,
-            "heading": 3.032735238530673,
-            "angularVelocity": 6.812858975549085e-7,
-            "velocityX": -3.7655516332820365,
-            "velocityY": 0.36229786724885477,
-            "timestamp": 0.8387502009174355
-        },
-        {
-            "x": 4.960619519356147,
-            "y": 6.587730487852754,
-            "heading": 3.032735268670057,
-            "angularVelocity": 2.874694691913672e-7,
-            "velocityX": -3.76436102099172,
-            "velocityY": -0.3744669443299256,
-            "timestamp": 0.9435939760321149
-        },
-        {
-            "x": 4.585499557692294,
-            "y": 6.45892571115183,
-            "heading": 3.032735273052689,
-            "angularVelocity": 4.180154788476782e-8,
-            "velocityX": -3.577894455379368,
-            "velocityY": -1.2285400498030161,
-            "timestamp": 1.0484377511467944
-        },
-        {
-            "x": 4.215352940634833,
-            "y": 6.316457686051506,
-            "heading": 3.0327352773918506,
-            "angularVelocity": 4.1386927047063915e-8,
-            "velocityX": -3.530458691062866,
-            "velocityY": -1.358860122544141,
-            "timestamp": 1.1532815262614737
-        },
-        {
-            "x": 3.8452063807699624,
-            "y": 6.173989512359033,
-            "heading": 3.0327352817310285,
-            "angularVelocity": 4.138708047875789e-8,
-            "velocityX": -3.530458145559901,
-            "velocityY": -1.3588615398161574,
-            "timestamp": 1.258125301376153
-        },
-        {
-            "x": 3.47505985812915,
-            "y": 6.031521241954782,
-            "heading": 3.0327352860702175,
-            "angularVelocity": 4.138718836240866e-8,
-            "velocityX": -3.530457790516812,
-            "velocityY": -1.3588624622531746,
-            "timestamp": 1.3629690764908324
-        },
-        {
-            "x": 3.1049133713898427,
-            "y": 5.889052878274973,
-            "heading": 3.0327352904094194,
-            "angularVelocity": 4.138730707919649e-8,
-            "velocityX": -3.53045744808823,
-            "velocityY": -1.3588633519155102,
-            "timestamp": 1.4678128516055118
-        },
-        {
-            "x": 2.762948024328836,
-            "y": 5.75743175203549,
-            "heading": 3.07563992564996,
-            "angularVelocity": 0.4092244407797366,
-            "velocityX": -3.2616657182265767,
-            "velocityY": -1.255402393661561,
-            "timestamp": 1.5726566267201911
-        },
-        {
-            "x": 2.5064740111386725,
-            "y": 5.658715887693948,
-            "heading": 3.107819702204835,
-            "angularVelocity": 0.30693073117289776,
-            "velocityX": -2.4462493162768064,
-            "velocityY": -0.9415519827816698,
-            "timestamp": 1.6775004018348705
-        },
-        {
-            "x": 2.3354913370033428,
-            "y": 5.592905299768155,
-            "heading": 3.129273261906622,
-            "angularVelocity": 0.20462406736424307,
-            "velocityX": -1.6308328648821255,
-            "velocityY": -0.6277014334309162,
-            "timestamp": 1.7823441769495498
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0.1023116353989093,
-            "velocityX": -0.8154164318275585,
-            "velocityY": -0.3138507720860239,
-            "timestamp": 1.8871879520642292
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 6.542152862475765e-30,
-            "velocityX": 0,
-            "velocityY": -3.777031189999863e-33,
-            "timestamp": 1.9920317271789085
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.traj
deleted file mode 100644
index 66484d18..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.traj	
+++ /dev/null
@@ -1,184 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": -1.4170612284410768e-31,
-            "velocityX": -1.067311144931875e-32,
-            "velocityY": -8.392488596494602e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 7.598760740640866,
-            "y": 5.839328600808308,
-            "heading": 3.1292747695902707,
-            "angularVelocity": -0.10229725511121834,
-            "velocityX": -0.7748601122982537,
-            "velocityY": 0.40373022396426267,
-            "timestamp": 0.10484377511467942
-        },
-        {
-            "x": 7.436282224463055,
-            "y": 5.923985787899839,
-            "heading": 3.1078233746588477,
-            "angularVelocity": -0.2046034197830007,
-            "velocityX": -1.5497202003656476,
-            "velocityY": 0.8074603093882498,
-            "timestamp": 0.20968755022935884
-        },
-        {
-            "x": 7.19256446073334,
-            "y": 6.050971542878038,
-            "heading": 3.0756442181443964,
-            "angularVelocity": -0.30692481722690457,
-            "velocityX": -2.3245802000465203,
-            "velocityY": 1.2111902193458808,
-            "timestamp": 0.31453132534403827
-        },
-        {
-            "x": 6.8676074741032105,
-            "y": 6.220285840922999,
-            "heading": 3.032735095907097,
-            "angularVelocity": -0.4092672377579469,
-            "velocityX": -3.099439964601499,
-            "velocityY": 1.614919892571246,
-            "timestamp": 0.4193751004587177
-        },
-        {
-            "x": 6.515870869646968,
-            "y": 6.403553456428483,
-            "heading": 3.032735091775592,
-            "angularVelocity": -3.94062953033438e-8,
-            "velocityX": -3.3548639780617218,
-            "velocityY": 1.7480066442192022,
-            "timestamp": 0.5242188755733971
-        },
-        {
-            "x": 6.1437635937584405,
-            "y": 6.5408190150249235,
-            "heading": 3.032735095078602,
-            "angularVelocity": 3.1504111682057e-8,
-            "velocityX": -3.5491594563579234,
-            "velocityY": 1.3092389934099258,
-            "timestamp": 0.6290626506880765
-        },
-        {
-            "x": 5.750083988314003,
-            "y": 6.589006339833596,
-            "heading": 3.032735167102088,
-            "angularVelocity": 6.86960058704524e-7,
-            "velocityX": -3.754916350673425,
-            "velocityY": 0.45961073755656895,
-            "timestamp": 0.733906425802756
-        },
-        {
-            "x": 5.355289339691467,
-            "y": 6.626991015951962,
-            "heading": 3.032735238530673,
-            "angularVelocity": 6.812858975549085e-7,
-            "velocityX": -3.7655516332820365,
-            "velocityY": 0.36229786724885477,
-            "timestamp": 0.8387502009174355
-        },
-        {
-            "x": 4.960619519356147,
-            "y": 6.587730487852754,
-            "heading": 3.032735268670057,
-            "angularVelocity": 2.874694691913672e-7,
-            "velocityX": -3.76436102099172,
-            "velocityY": -0.3744669443299256,
-            "timestamp": 0.9435939760321149
-        },
-        {
-            "x": 4.585499557692294,
-            "y": 6.45892571115183,
-            "heading": 3.032735273052689,
-            "angularVelocity": 4.180154788476782e-8,
-            "velocityX": -3.577894455379368,
-            "velocityY": -1.2285400498030161,
-            "timestamp": 1.0484377511467944
-        },
-        {
-            "x": 4.215352940634833,
-            "y": 6.316457686051506,
-            "heading": 3.0327352773918506,
-            "angularVelocity": 4.1386927047063915e-8,
-            "velocityX": -3.530458691062866,
-            "velocityY": -1.358860122544141,
-            "timestamp": 1.1532815262614737
-        },
-        {
-            "x": 3.8452063807699624,
-            "y": 6.173989512359033,
-            "heading": 3.0327352817310285,
-            "angularVelocity": 4.138708047875789e-8,
-            "velocityX": -3.530458145559901,
-            "velocityY": -1.3588615398161574,
-            "timestamp": 1.258125301376153
-        },
-        {
-            "x": 3.47505985812915,
-            "y": 6.031521241954782,
-            "heading": 3.0327352860702175,
-            "angularVelocity": 4.138718836240866e-8,
-            "velocityX": -3.530457790516812,
-            "velocityY": -1.3588624622531746,
-            "timestamp": 1.3629690764908324
-        },
-        {
-            "x": 3.1049133713898427,
-            "y": 5.889052878274973,
-            "heading": 3.0327352904094194,
-            "angularVelocity": 4.138730707919649e-8,
-            "velocityX": -3.53045744808823,
-            "velocityY": -1.3588633519155102,
-            "timestamp": 1.4678128516055118
-        },
-        {
-            "x": 2.762948024328836,
-            "y": 5.75743175203549,
-            "heading": 3.07563992564996,
-            "angularVelocity": 0.4092244407797366,
-            "velocityX": -3.2616657182265767,
-            "velocityY": -1.255402393661561,
-            "timestamp": 1.5726566267201911
-        },
-        {
-            "x": 2.5064740111386725,
-            "y": 5.658715887693948,
-            "heading": 3.107819702204835,
-            "angularVelocity": 0.30693073117289776,
-            "velocityX": -2.4462493162768064,
-            "velocityY": -0.9415519827816698,
-            "timestamp": 1.6775004018348705
-        },
-        {
-            "x": 2.3354913370033428,
-            "y": 5.592905299768155,
-            "heading": 3.129273261906622,
-            "angularVelocity": 0.20462406736424307,
-            "velocityX": -1.6308328648821255,
-            "velocityY": -0.6277014334309162,
-            "timestamp": 1.7823441769495498
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0.1023116353989093,
-            "velocityX": -0.8154164318275585,
-            "velocityY": -0.3138507720860239,
-            "timestamp": 1.8871879520642292
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 6.542152862475765e-30,
-            "velocityX": 0,
-            "velocityY": -3.777031189999863e-33,
-            "timestamp": 1.9920317271789085
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.1.traj
deleted file mode 100644
index 4813b980..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.1.traj	
+++ /dev/null
@@ -1,211 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 1.7201358648458989e-31,
-            "velocityX": -4.9530751339705636e-33,
-            "velocityY": 8.276147677005227e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 7.605093588171019,
-            "y": 5.8310278116994025,
-            "heading": 3.10842986087817,
-            "angularVelocity": -0.3169118675748513,
-            "velocityX": -0.7519362133453025,
-            "velocityY": 0.3415828265288231,
-            "timestamp": 0.0996180400671603
-        },
-        {
-            "x": 7.455282578249206,
-            "y": 5.899083098798643,
-            "heading": 3.0452020176570493,
-            "angularVelocity": -0.6347027423797291,
-            "velocityX": -1.5038542197860265,
-            "velocityY": 0.6831622771674665,
-            "timestamp": 0.1992360801343206
-        },
-        {
-            "x": 7.230570445252572,
-            "y": 6.001166640275022,
-            "heading": 2.9501837123513917,
-            "angularVelocity": -0.9538262873029698,
-            "velocityX": -2.2557373428059657,
-            "velocityY": 1.0247495474469857,
-            "timestamp": 0.2988541202014809
-        },
-        {
-            "x": 6.930961848120999,
-            "y": 6.137281948011585,
-            "heading": 2.82324695361152,
-            "angularVelocity": -1.274234653224393,
-            "velocityX": -3.007573697791919,
-            "velocityY": 1.3663720712111798,
-            "timestamp": 0.3984721602686412
-        },
-        {
-            "x": 6.58788061707114,
-            "y": 6.293200302520464,
-            "heading": 2.8232469405560607,
-            "angularVelocity": -1.3105517366390336e-7,
-            "velocityX": -3.4439668840961075,
-            "velocityY": 1.565161836187118,
-            "timestamp": 0.4980902003358015
-        },
-        {
-            "x": 6.242826706460482,
-            "y": 6.444702986624836,
-            "heading": 2.8232469291979005,
-            "angularVelocity": -1.1401709907689636e-7,
-            "velocityX": -3.46376931706376,
-            "velocityY": 1.5208358245377296,
-            "timestamp": 0.5977082404029618
-        },
-        {
-            "x": 5.895869664946667,
-            "y": 6.591795052657303,
-            "heading": 2.823246919432399,
-            "angularVelocity": -9.802945216969962e-8,
-            "velocityX": -3.482873596789339,
-            "velocityY": 1.4765605299331361,
-            "timestamp": 0.6973262804701221
-        },
-        {
-            "x": 5.529327027819563,
-            "y": 6.679326525812769,
-            "heading": 2.8232469090031302,
-            "angularVelocity": -1.0469256638864389e-7,
-            "velocityX": -3.679480512565681,
-            "velocityY": 0.8786709023431315,
-            "timestamp": 0.7969443205372824
-        },
-        {
-            "x": 5.152516966350426,
-            "y": 6.684752095540444,
-            "heading": 2.823246873859642,
-            "angularVelocity": -3.5278236840852576e-7,
-            "velocityX": -3.7825484341500837,
-            "velocityY": 0.05446372689140277,
-            "timestamp": 0.8965623606044426
-        },
-        {
-            "x": 4.783606202844696,
-            "y": 6.607810054904123,
-            "heading": 2.823246710781253,
-            "angularVelocity": -0.0000016370367136474411,
-            "velocityX": -3.703252576109883,
-            "velocityY": -0.7723705524064612,
-            "timestamp": 0.9961804006716029
-        },
-        {
-            "x": 4.4403804832702125,
-            "y": 6.452210088653612,
-            "heading": 2.8232466000560583,
-            "angularVelocity": -0.0000011114974222563995,
-            "velocityX": -3.445417309385819,
-            "velocityY": -1.5619657458188145,
-            "timestamp": 1.095798440738763
-        },
-        {
-            "x": 4.124524669473561,
-            "y": 6.246661186403647,
-            "heading": 2.823246495691381,
-            "angularVelocity": -0.0000010476483740088647,
-            "velocityX": -3.170668822471396,
-            "velocityY": -2.0633702701979306,
-            "timestamp": 1.1954164808059233
-        },
-        {
-            "x": 3.808669145932633,
-            "y": 6.041111838134177,
-            "heading": 2.82324639132671,
-            "angularVelocity": -0.0000010476483076103719,
-            "velocityX": -3.1706659087850397,
-            "velocityY": -2.063374747494463,
-            "timestamp": 1.2950345208730836
-        },
-        {
-            "x": 3.492813679718565,
-            "y": 5.835562401773946,
-            "heading": 2.8232462869620267,
-            "angularVelocity": -0.0000010476484251301315,
-            "velocityX": -3.1706653333184023,
-            "velocityY": -2.063375631779686,
-            "timestamp": 1.3946525609402438
-        },
-        {
-            "x": 3.176958352678971,
-            "y": 5.630012751552842,
-            "heading": 2.8232461825973036,
-            "angularVelocity": -0.000001047648830467493,
-            "velocityX": -3.1706639362373616,
-            "velocityY": -2.0633777785883685,
-            "timestamp": 1.494270601007404
-        },
-        {
-            "x": 2.880984417024366,
-            "y": 5.396743220702011,
-            "heading": 2.8232460713596375,
-            "angularVelocity": -0.0000011166417889330504,
-            "velocityX": -2.971087721210592,
-            "velocityY": -2.3416394329136074,
-            "timestamp": 1.5938886410745643
-        },
-        {
-            "x": 2.6437210391842343,
-            "y": 5.10516988637377,
-            "heading": 2.820727987751454,
-            "angularVelocity": -0.02527738556676537,
-            "velocityX": -2.3817310366694042,
-            "velocityY": -2.926912978127948,
-            "timestamp": 1.6935066811417245
-        },
-        {
-            "x": 2.4878211519832623,
-            "y": 4.818636614610494,
-            "heading": 2.7673750452255876,
-            "angularVelocity": -0.5355751075798869,
-            "velocityX": -1.5649764550262995,
-            "velocityY": -2.876319104151233,
-            "timestamp": 1.7931247212088848
-        },
-        {
-            "x": 2.3880615291520813,
-            "y": 4.591997027963517,
-            "heading": 2.751203678024379,
-            "angularVelocity": -0.1623337217867963,
-            "velocityX": -1.0014212562696991,
-            "velocityY": -2.275085782597012,
-            "timestamp": 1.892742761276045
-        },
-        {
-            "x": 2.3279705887355506,
-            "y": 4.437873884538665,
-            "heading": 2.748674861705227,
-            "angularVelocity": -0.025385124194849635,
-            "velocityX": -0.6032134378072334,
-            "velocityY": -1.547140892562696,
-            "timestamp": 1.9923608013432053
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0.013302191991328724,
-            "velocityX": -0.28077834814551317,
-            "velocityY": -0.7817247206044561,
-            "timestamp": 2.0919788414103655
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": -2.062592729534739e-30,
-            "velocityX": 0,
-            "velocityY": 2.2969932039584724e-33,
-            "timestamp": 2.1915968814775257
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.traj
deleted file mode 100644
index 4813b980..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.traj	
+++ /dev/null
@@ -1,211 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 1.7201358648458989e-31,
-            "velocityX": -4.9530751339705636e-33,
-            "velocityY": 8.276147677005227e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 7.605093588171019,
-            "y": 5.8310278116994025,
-            "heading": 3.10842986087817,
-            "angularVelocity": -0.3169118675748513,
-            "velocityX": -0.7519362133453025,
-            "velocityY": 0.3415828265288231,
-            "timestamp": 0.0996180400671603
-        },
-        {
-            "x": 7.455282578249206,
-            "y": 5.899083098798643,
-            "heading": 3.0452020176570493,
-            "angularVelocity": -0.6347027423797291,
-            "velocityX": -1.5038542197860265,
-            "velocityY": 0.6831622771674665,
-            "timestamp": 0.1992360801343206
-        },
-        {
-            "x": 7.230570445252572,
-            "y": 6.001166640275022,
-            "heading": 2.9501837123513917,
-            "angularVelocity": -0.9538262873029698,
-            "velocityX": -2.2557373428059657,
-            "velocityY": 1.0247495474469857,
-            "timestamp": 0.2988541202014809
-        },
-        {
-            "x": 6.930961848120999,
-            "y": 6.137281948011585,
-            "heading": 2.82324695361152,
-            "angularVelocity": -1.274234653224393,
-            "velocityX": -3.007573697791919,
-            "velocityY": 1.3663720712111798,
-            "timestamp": 0.3984721602686412
-        },
-        {
-            "x": 6.58788061707114,
-            "y": 6.293200302520464,
-            "heading": 2.8232469405560607,
-            "angularVelocity": -1.3105517366390336e-7,
-            "velocityX": -3.4439668840961075,
-            "velocityY": 1.565161836187118,
-            "timestamp": 0.4980902003358015
-        },
-        {
-            "x": 6.242826706460482,
-            "y": 6.444702986624836,
-            "heading": 2.8232469291979005,
-            "angularVelocity": -1.1401709907689636e-7,
-            "velocityX": -3.46376931706376,
-            "velocityY": 1.5208358245377296,
-            "timestamp": 0.5977082404029618
-        },
-        {
-            "x": 5.895869664946667,
-            "y": 6.591795052657303,
-            "heading": 2.823246919432399,
-            "angularVelocity": -9.802945216969962e-8,
-            "velocityX": -3.482873596789339,
-            "velocityY": 1.4765605299331361,
-            "timestamp": 0.6973262804701221
-        },
-        {
-            "x": 5.529327027819563,
-            "y": 6.679326525812769,
-            "heading": 2.8232469090031302,
-            "angularVelocity": -1.0469256638864389e-7,
-            "velocityX": -3.679480512565681,
-            "velocityY": 0.8786709023431315,
-            "timestamp": 0.7969443205372824
-        },
-        {
-            "x": 5.152516966350426,
-            "y": 6.684752095540444,
-            "heading": 2.823246873859642,
-            "angularVelocity": -3.5278236840852576e-7,
-            "velocityX": -3.7825484341500837,
-            "velocityY": 0.05446372689140277,
-            "timestamp": 0.8965623606044426
-        },
-        {
-            "x": 4.783606202844696,
-            "y": 6.607810054904123,
-            "heading": 2.823246710781253,
-            "angularVelocity": -0.0000016370367136474411,
-            "velocityX": -3.703252576109883,
-            "velocityY": -0.7723705524064612,
-            "timestamp": 0.9961804006716029
-        },
-        {
-            "x": 4.4403804832702125,
-            "y": 6.452210088653612,
-            "heading": 2.8232466000560583,
-            "angularVelocity": -0.0000011114974222563995,
-            "velocityX": -3.445417309385819,
-            "velocityY": -1.5619657458188145,
-            "timestamp": 1.095798440738763
-        },
-        {
-            "x": 4.124524669473561,
-            "y": 6.246661186403647,
-            "heading": 2.823246495691381,
-            "angularVelocity": -0.0000010476483740088647,
-            "velocityX": -3.170668822471396,
-            "velocityY": -2.0633702701979306,
-            "timestamp": 1.1954164808059233
-        },
-        {
-            "x": 3.808669145932633,
-            "y": 6.041111838134177,
-            "heading": 2.82324639132671,
-            "angularVelocity": -0.0000010476483076103719,
-            "velocityX": -3.1706659087850397,
-            "velocityY": -2.063374747494463,
-            "timestamp": 1.2950345208730836
-        },
-        {
-            "x": 3.492813679718565,
-            "y": 5.835562401773946,
-            "heading": 2.8232462869620267,
-            "angularVelocity": -0.0000010476484251301315,
-            "velocityX": -3.1706653333184023,
-            "velocityY": -2.063375631779686,
-            "timestamp": 1.3946525609402438
-        },
-        {
-            "x": 3.176958352678971,
-            "y": 5.630012751552842,
-            "heading": 2.8232461825973036,
-            "angularVelocity": -0.000001047648830467493,
-            "velocityX": -3.1706639362373616,
-            "velocityY": -2.0633777785883685,
-            "timestamp": 1.494270601007404
-        },
-        {
-            "x": 2.880984417024366,
-            "y": 5.396743220702011,
-            "heading": 2.8232460713596375,
-            "angularVelocity": -0.0000011166417889330504,
-            "velocityX": -2.971087721210592,
-            "velocityY": -2.3416394329136074,
-            "timestamp": 1.5938886410745643
-        },
-        {
-            "x": 2.6437210391842343,
-            "y": 5.10516988637377,
-            "heading": 2.820727987751454,
-            "angularVelocity": -0.02527738556676537,
-            "velocityX": -2.3817310366694042,
-            "velocityY": -2.926912978127948,
-            "timestamp": 1.6935066811417245
-        },
-        {
-            "x": 2.4878211519832623,
-            "y": 4.818636614610494,
-            "heading": 2.7673750452255876,
-            "angularVelocity": -0.5355751075798869,
-            "velocityX": -1.5649764550262995,
-            "velocityY": -2.876319104151233,
-            "timestamp": 1.7931247212088848
-        },
-        {
-            "x": 2.3880615291520813,
-            "y": 4.591997027963517,
-            "heading": 2.751203678024379,
-            "angularVelocity": -0.1623337217867963,
-            "velocityX": -1.0014212562696991,
-            "velocityY": -2.275085782597012,
-            "timestamp": 1.892742761276045
-        },
-        {
-            "x": 2.3279705887355506,
-            "y": 4.437873884538665,
-            "heading": 2.748674861705227,
-            "angularVelocity": -0.025385124194849635,
-            "velocityX": -0.6032134378072334,
-            "velocityY": -1.547140892562696,
-            "timestamp": 1.9923608013432053
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0.013302191991328724,
-            "velocityX": -0.28077834814551317,
-            "velocityY": -0.7817247206044561,
-            "timestamp": 2.0919788414103655
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": -2.062592729534739e-30,
-            "velocityX": 0,
-            "velocityY": 2.2969932039584724e-33,
-            "timestamp": 2.1915968814775257
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.1.traj
new file mode 100644
index 00000000..69543755
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.1.traj	
@@ -0,0 +1,139 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 1.212112656390561e-31,
+            "velocityX": -5.041767818814111e-32,
+            "velocityY": -1.2487171964671538e-38,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6800000701104585,
+            "y": 4.20344537200552,
+            "heading": 1.2930383103747041e-8,
+            "angularVelocity": 1.350424233383134e-7,
+            "velocityX": 7.322200986357225e-7,
+            "velocityY": 0.7983807350945226,
+            "timestamp": 0.09575052183150286
+        },
+        {
+            "x": 7.680000201769031,
+            "y": 4.356336112982647,
+            "heading": 3.8758528041627975e-8,
+            "angularVelocity": 2.6974415299903026e-7,
+            "velocityX": 0.0000013750167071257275,
+            "velocityY": 1.596761438503451,
+            "timestamp": 0.19150104366300572
+        },
+        {
+            "x": 7.680000386416211,
+            "y": 4.58567221686356,
+            "heading": 7.786709578005107e-8,
+            "angularVelocity": 4.0844231330927475e-7,
+            "velocityX": 0.000001928419471656301,
+            "velocityY": 2.395142078541207,
+            "timestamp": 0.2872515654945086
+        },
+        {
+            "x": 7.6800006155005685,
+            "y": 4.891453665444849,
+            "heading": 1.3155130559553437e-7,
+            "angularVelocity": 5.606674749182053e-7,
+            "velocityX": 0.0000023925127342176106,
+            "velocityY": 3.1935225284661,
+            "timestamp": 0.38300208732601143
+        },
+        {
+            "x": 7.68000074582338,
+            "y": 5.253672199267105,
+            "heading": 1.3155130364873708e-7,
+            "angularVelocity": -1.7664134647076396e-14,
+            "velocityX": 0.000001361066521463048,
+            "velocityY": 3.7829405719549913,
+            "timestamp": 0.4787526091575143
+        },
+        {
+            "x": 7.680000798333519,
+            "y": 5.615890733089199,
+            "heading": 1.315513137062799e-7,
+            "angularVelocity": 1.0799702172100096e-13,
+            "velocityX": 5.484057877231364e-7,
+            "velocityY": 3.7829405719533282,
+            "timestamp": 0.5745031309890172
+        },
+        {
+            "x": 7.680000776767743,
+            "y": 5.9781092669112965,
+            "heading": 1.3155132085570774e-7,
+            "angularVelocity": 7.667783798806564e-14,
+            "velocityX": -2.2522878887411694e-7,
+            "velocityY": 3.782940571953354,
+            "timestamp": 0.6702536528205201
+        },
+        {
+            "x": 7.680000686239199,
+            "y": 6.340327800733382,
+            "heading": 1.315513220333048e-7,
+            "angularVelocity": 1.2578458101628908e-14,
+            "velocityX": -9.454626397270524e-7,
+            "velocityY": 3.78294057195323,
+            "timestamp": 0.766004174652023
+        },
+        {
+            "x": 7.680000533602743,
+            "y": 6.702546334555452,
+            "heading": 1.3155133191379775e-7,
+            "angularVelocity": 1.0229175193513549e-13,
+            "velocityX": -0.0000015941058350287079,
+            "velocityY": 3.7829405719530396,
+            "timestamp": 0.8617546964835259
+        },
+        {
+            "x": 7.680000329705533,
+            "y": 7.00832778313656,
+            "heading": 7.923155128573452e-8,
+            "angularVelocity": -5.46417642222627e-7,
+            "velocityX": -0.000002129463080315811,
+            "velocityY": 3.1935225284642037,
+            "timestamp": 0.9575052183150288
+        },
+        {
+            "x": 7.680000169486885,
+            "y": 7.2376638870173995,
+            "heading": 3.947148220681983e-8,
+            "angularVelocity": -4.152464695661009e-7,
+            "velocityX": -0.0000016732926513675465,
+            "velocityY": 2.395142078540454,
+            "timestamp": 1.0532557401465317
+        },
+        {
+            "x": 7.680000058003377,
+            "y": 7.390554627994493,
+            "heading": 1.303400507527686e-8,
+            "angularVelocity": -2.7610790251013916e-7,
+            "velocityX": -0.000001164312243995105,
+            "velocityY": 1.5967614385030973,
+            "timestamp": 1.1490062619780346
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": -1.3612463214689187e-7,
+            "velocityX": -6.057760896118273e-7,
+            "velocityY": 0.7983807350943913,
+            "timestamp": 1.2447567838095375
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 1.2167404646059922e-31,
+            "velocityY": -2.3530958630073903e-37,
+            "timestamp": 1.3405073056410404
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.traj
new file mode 100644
index 00000000..69543755
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.traj	
@@ -0,0 +1,139 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 1.212112656390561e-31,
+            "velocityX": -5.041767818814111e-32,
+            "velocityY": -1.2487171964671538e-38,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6800000701104585,
+            "y": 4.20344537200552,
+            "heading": 1.2930383103747041e-8,
+            "angularVelocity": 1.350424233383134e-7,
+            "velocityX": 7.322200986357225e-7,
+            "velocityY": 0.7983807350945226,
+            "timestamp": 0.09575052183150286
+        },
+        {
+            "x": 7.680000201769031,
+            "y": 4.356336112982647,
+            "heading": 3.8758528041627975e-8,
+            "angularVelocity": 2.6974415299903026e-7,
+            "velocityX": 0.0000013750167071257275,
+            "velocityY": 1.596761438503451,
+            "timestamp": 0.19150104366300572
+        },
+        {
+            "x": 7.680000386416211,
+            "y": 4.58567221686356,
+            "heading": 7.786709578005107e-8,
+            "angularVelocity": 4.0844231330927475e-7,
+            "velocityX": 0.000001928419471656301,
+            "velocityY": 2.395142078541207,
+            "timestamp": 0.2872515654945086
+        },
+        {
+            "x": 7.6800006155005685,
+            "y": 4.891453665444849,
+            "heading": 1.3155130559553437e-7,
+            "angularVelocity": 5.606674749182053e-7,
+            "velocityX": 0.0000023925127342176106,
+            "velocityY": 3.1935225284661,
+            "timestamp": 0.38300208732601143
+        },
+        {
+            "x": 7.68000074582338,
+            "y": 5.253672199267105,
+            "heading": 1.3155130364873708e-7,
+            "angularVelocity": -1.7664134647076396e-14,
+            "velocityX": 0.000001361066521463048,
+            "velocityY": 3.7829405719549913,
+            "timestamp": 0.4787526091575143
+        },
+        {
+            "x": 7.680000798333519,
+            "y": 5.615890733089199,
+            "heading": 1.315513137062799e-7,
+            "angularVelocity": 1.0799702172100096e-13,
+            "velocityX": 5.484057877231364e-7,
+            "velocityY": 3.7829405719533282,
+            "timestamp": 0.5745031309890172
+        },
+        {
+            "x": 7.680000776767743,
+            "y": 5.9781092669112965,
+            "heading": 1.3155132085570774e-7,
+            "angularVelocity": 7.667783798806564e-14,
+            "velocityX": -2.2522878887411694e-7,
+            "velocityY": 3.782940571953354,
+            "timestamp": 0.6702536528205201
+        },
+        {
+            "x": 7.680000686239199,
+            "y": 6.340327800733382,
+            "heading": 1.315513220333048e-7,
+            "angularVelocity": 1.2578458101628908e-14,
+            "velocityX": -9.454626397270524e-7,
+            "velocityY": 3.78294057195323,
+            "timestamp": 0.766004174652023
+        },
+        {
+            "x": 7.680000533602743,
+            "y": 6.702546334555452,
+            "heading": 1.3155133191379775e-7,
+            "angularVelocity": 1.0229175193513549e-13,
+            "velocityX": -0.0000015941058350287079,
+            "velocityY": 3.7829405719530396,
+            "timestamp": 0.8617546964835259
+        },
+        {
+            "x": 7.680000329705533,
+            "y": 7.00832778313656,
+            "heading": 7.923155128573452e-8,
+            "angularVelocity": -5.46417642222627e-7,
+            "velocityX": -0.000002129463080315811,
+            "velocityY": 3.1935225284642037,
+            "timestamp": 0.9575052183150288
+        },
+        {
+            "x": 7.680000169486885,
+            "y": 7.2376638870173995,
+            "heading": 3.947148220681983e-8,
+            "angularVelocity": -4.152464695661009e-7,
+            "velocityX": -0.0000016732926513675465,
+            "velocityY": 2.395142078540454,
+            "timestamp": 1.0532557401465317
+        },
+        {
+            "x": 7.680000058003377,
+            "y": 7.390554627994493,
+            "heading": 1.303400507527686e-8,
+            "angularVelocity": -2.7610790251013916e-7,
+            "velocityX": -0.000001164312243995105,
+            "velocityY": 1.5967614385030973,
+            "timestamp": 1.1490062619780346
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": -1.3612463214689187e-7,
+            "velocityX": -6.057760896118273e-7,
+            "velocityY": 0.7983807350943913,
+            "timestamp": 1.2447567838095375
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 1.2167404646059922e-31,
+            "velocityY": -2.3530958630073903e-37,
+            "timestamp": 1.3405073056410404
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.1.traj
deleted file mode 100644
index e53f5f73..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.1.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 2.0369445518795058e-34,
-            "velocityX": 0,
-            "velocityY": 7.181654629664687e-43,
-            "timestamp": 0
-        },
-        {
-            "x": 7.680000037559162,
-            "y": 4.043499994424155,
-            "heading": 3.140000006758021,
-            "angularVelocity": 6.75321744601227e-8,
-            "velocityX": 3.753246582974315e-7,
-            "velocityY": -0.8344065481227707,
-            "timestamp": 0.10007112931184514
-        },
-        {
-            "x": 7.6800001031226985,
-            "y": 3.8764999863701557,
-            "heading": 3.1400000189870485,
-            "angularVelocity": 1.2220335610640094e-7,
-            "velocityX": 6.551693493497348e-7,
-            "velocityY": -1.6688130652906727,
-            "timestamp": 0.20014225862369028
-        },
-        {
-            "x": 7.680000187135912,
-            "y": 3.625999982033383,
-            "heading": 3.1400000350867425,
-            "angularVelocity": 1.6088250383461684e-7,
-            "velocityX": 8.39534968426326e-7,
-            "velocityY": -2.503219520548785,
-            "timestamp": 0.3002133879355354
-        },
-        {
-            "x": 7.680000280051002,
-            "y": 3.2919999999999914,
-            "heading": 3.1400000531130368,
-            "angularVelocity": 1.801348130208338e-7,
-            "velocityX": 9.284904765306952e-7,
-            "velocityY": -3.3376257900774653,
-            "timestamp": 0.40028451724738057
-        },
-        {
-            "x": 7.680000185977535,
-            "y": 2.958000017966607,
-            "heading": 3.1400000281215186,
-            "angularVelocity": -2.4973754429186553e-7,
-            "velocityX": -9.400660019914908e-7,
-            "velocityY": -3.337625790077398,
-            "timestamp": 0.5003556465592257
-        },
-        {
-            "x": 7.680000101870251,
-            "y": 2.7075000136298395,
-            "heading": 3.1400000109296125,
-            "angularVelocity": -1.7179686113327866e-7,
-            "velocityX": -8.404750236739033e-7,
-            "velocityY": -2.503219520548736,
-            "timestamp": 0.6004267758710709
-        },
-        {
-            "x": 7.6800000368892665,
-            "y": 2.5405000055758427,
-            "heading": 3.140000002248845,
-            "angularVelocity": -8.674597729312873e-8,
-            "velocityX": -6.493479632352893e-7,
-            "velocityY": -1.6688130652906423,
-            "timestamp": 0.700497905182916
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -2.2472459301603394e-8,
-            "velocityX": -3.686304669837251e-7,
-            "velocityY": -0.8344065481227574,
-            "timestamp": 0.8005690344947611
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 7.368205253143154e-34,
-            "velocityX": 0,
-            "velocityY": -2.1562414433932607e-41,
-            "timestamp": 0.9006401638066063
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.traj
deleted file mode 100644
index e53f5f73..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 2.0369445518795058e-34,
-            "velocityX": 0,
-            "velocityY": 7.181654629664687e-43,
-            "timestamp": 0
-        },
-        {
-            "x": 7.680000037559162,
-            "y": 4.043499994424155,
-            "heading": 3.140000006758021,
-            "angularVelocity": 6.75321744601227e-8,
-            "velocityX": 3.753246582974315e-7,
-            "velocityY": -0.8344065481227707,
-            "timestamp": 0.10007112931184514
-        },
-        {
-            "x": 7.6800001031226985,
-            "y": 3.8764999863701557,
-            "heading": 3.1400000189870485,
-            "angularVelocity": 1.2220335610640094e-7,
-            "velocityX": 6.551693493497348e-7,
-            "velocityY": -1.6688130652906727,
-            "timestamp": 0.20014225862369028
-        },
-        {
-            "x": 7.680000187135912,
-            "y": 3.625999982033383,
-            "heading": 3.1400000350867425,
-            "angularVelocity": 1.6088250383461684e-7,
-            "velocityX": 8.39534968426326e-7,
-            "velocityY": -2.503219520548785,
-            "timestamp": 0.3002133879355354
-        },
-        {
-            "x": 7.680000280051002,
-            "y": 3.2919999999999914,
-            "heading": 3.1400000531130368,
-            "angularVelocity": 1.801348130208338e-7,
-            "velocityX": 9.284904765306952e-7,
-            "velocityY": -3.3376257900774653,
-            "timestamp": 0.40028451724738057
-        },
-        {
-            "x": 7.680000185977535,
-            "y": 2.958000017966607,
-            "heading": 3.1400000281215186,
-            "angularVelocity": -2.4973754429186553e-7,
-            "velocityX": -9.400660019914908e-7,
-            "velocityY": -3.337625790077398,
-            "timestamp": 0.5003556465592257
-        },
-        {
-            "x": 7.680000101870251,
-            "y": 2.7075000136298395,
-            "heading": 3.1400000109296125,
-            "angularVelocity": -1.7179686113327866e-7,
-            "velocityX": -8.404750236739033e-7,
-            "velocityY": -2.503219520548736,
-            "timestamp": 0.6004267758710709
-        },
-        {
-            "x": 7.6800000368892665,
-            "y": 2.5405000055758427,
-            "heading": 3.140000002248845,
-            "angularVelocity": -8.674597729312873e-8,
-            "velocityX": -6.493479632352893e-7,
-            "velocityY": -1.6688130652906423,
-            "timestamp": 0.700497905182916
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -2.2472459301603394e-8,
-            "velocityX": -3.686304669837251e-7,
-            "velocityY": -0.8344065481227574,
-            "timestamp": 0.8005690344947611
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 7.368205253143154e-34,
-            "velocityX": 0,
-            "velocityY": -2.1562414433932607e-41,
-            "timestamp": 0.9006401638066063
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.1.traj
index 7ea9ac6c..c126c683 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.1.traj	
@@ -3,137 +3,137 @@
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 9.207800442360542e-32,
-            "velocityX": -6.095364969086891e-33,
-            "velocityY": -9.366947302592518e-40,
+            "heading": 0,
+            "angularVelocity": 2.405138964716482e-32,
+            "velocityX": 6.733008165659914e-33,
+            "velocityY": 1.175464991429931e-38,
             "timestamp": 0
         },
         {
-            "x": 7.680000070435844,
-            "y": 4.0505546279944795,
-            "heading": 3.1399999875764033,
-            "angularVelocity": -1.2974964830179728e-7,
-            "velocityX": 7.356183706178639e-7,
-            "velocityY": -0.7983807350945191,
-            "timestamp": 0.0957505218315028
+            "x": 7.6800000704332305,
+            "y": 4.05055462799448,
+            "heading": -1.2377486895022226e-8,
+            "angularVelocity": -1.2926808085167386e-7,
+            "velocityX": 7.355910665177674e-7,
+            "velocityY": -0.7983807350945193,
+            "timestamp": 0.09575052183150283
         },
         {
-            "x": 7.680000202739516,
+            "x": 7.680000202731891,
             "y": 3.8976638870173534,
-            "heading": 3.1399999616102225,
-            "angularVelocity": -2.711857701387203e-7,
-            "velocityX": 0.0000013817539912349048,
-            "velocityY": -1.5967614385034457,
-            "timestamp": 0.1915010436630056
+            "heading": -3.8235321936944764e-8,
+            "angularVelocity": -2.700542284308498e-7,
+            "velocityX": 0.0000013817016646870076,
+            "velocityY": -1.5967614385034459,
+            "timestamp": 0.19150104366300566
         },
         {
-            "x": 7.680000388342391,
+            "x": 7.6800003883277,
             "y": 3.6683277831364425,
-            "heading": 3.139999920718594,
-            "angularVelocity": -4.2706425730703774e-7,
-            "velocityX": 0.00000193840056979922,
-            "velocityY": -2.395142078541202,
-            "timestamp": 0.2872515654945084
+            "heading": -7.893916226933331e-8,
+            "angularVelocity": -4.251030288838169e-7,
+            "velocityX": 0.0000019383267807539473,
+            "velocityY": -2.3951420785412023,
+            "timestamp": 0.2872515654945085
         },
         {
-            "x": 7.680000618679091,
-            "y": 3.362546334555153,
-            "heading": 3.139999863216847,
-            "angularVelocity": -6.005370836912587e-7,
-            "velocityX": 0.000002405591946532482,
-            "velocityY": -3.1935225284661044,
-            "timestamp": 0.3830020873260112
+            "x": 7.680000618655782,
+            "y": 3.3625463345551534,
+            "heading": -1.3615412726424167e-7,
+            "angularVelocity": -5.975419949298212e-7,
+            "velocityX": 0.0000024055019376452327,
+            "velocityY": -3.193522528466104,
+            "timestamp": 0.3830020873260113
         },
         {
-            "x": 7.680000750151485,
-            "y": 3.000327800732898,
-            "heading": 3.139999863216849,
-            "angularVelocity": 2.234907225706593e-14,
-            "velocityX": 0.0000013730725434542363,
-            "velocityY": -3.7829405719549856,
-            "timestamp": 0.478752609157514
+            "x": 7.680000750128537,
+            "y": 3.0003278007328986,
+            "heading": -1.3615412487460524e-7,
+            "angularVelocity": 2.2064625199669458e-14,
+            "velocityX": 0.000001373076316791352,
+            "velocityY": -3.7829405719549847,
+            "timestamp": 0.47875260915751416
         },
         {
-            "x": 7.6800008034310885,
-            "y": 2.6381092669108033,
-            "heading": 3.139999863216839,
-            "angularVelocity": -1.1397010502093565e-13,
-            "velocityX": 5.56441925019577e-7,
+            "x": 7.68000080341356,
+            "y": 2.638109266910804,
+            "heading": -1.3615413544372385e-7,
+            "angularVelocity": -1.1353706918076568e-13,
+            "velocityX": 5.564985227619036e-7,
             "velocityY": -3.7829405719533264,
-            "timestamp": 0.5745031309890168
+            "timestamp": 0.574503130989017
         },
         {
-            "x": 7.68000078216065,
-            "y": 2.275890733088706,
-            "heading": 3.1399998632168313,
-            "angularVelocity": -8.126780055977152e-14,
-            "velocityX": -2.2214435413644554e-7,
-            "velocityY": -3.7829405719533535,
-            "timestamp": 0.6702536528205196
+            "x": 7.680000782147778,
+            "y": 2.2758907330887066,
+            "heading": -1.361541430398869e-7,
+            "angularVelocity": -8.149555996364277e-14,
+            "velocityX": -2.2209570916932486e-7,
+            "velocityY": -3.782940571953353,
+            "timestamp": 0.6702536528205197
         },
         {
-            "x": 7.6800006913830785,
-            "y": 1.9136721992666206,
-            "heading": 3.1399998632168296,
-            "angularVelocity": -1.6383854112995832e-14,
-            "velocityX": -9.4806343038794e-7,
+            "x": 7.68000069137085,
+            "y": 1.9136721992666208,
+            "heading": -1.3615414447383612e-7,
+            "angularVelocity": -1.5331626502863808e-14,
+            "velocityX": -9.48056722512506e-7,
             "velocityY": -3.7829405719532305,
-            "timestamp": 0.7660041746520224
+            "timestamp": 0.7660041746520225
         },
         {
-            "x": 7.680000537903045,
+            "x": 7.68000053789062,
             "y": 1.55145366544455,
-            "heading": 3.13999986321682,
-            "angularVelocity": -1.0277752214408611e-13,
-            "velocityX": -0.0000016029159878741863,
-            "velocityY": -3.7829405719530524,
-            "timestamp": 0.8617546964835252
+            "heading": -1.361541542308917e-7,
+            "angularVelocity": -1.0101247732712604e-13,
+            "velocityX": -0.000001602918056057891,
+            "velocityY": -3.782940571953052,
+            "timestamp": 0.8617546964835253
         },
         {
-            "x": 7.680000332408874,
+            "x": 7.68000033240059,
             "y": 1.2456722168634402,
-            "heading": 3.139999917268044,
-            "angularVelocity": 5.645005022977776e-7,
-            "velocityX": -0.0000021461414302094452,
-            "velocityY": -3.1935225284642264,
-            "timestamp": 0.957505218315028
+            "heading": -8.229196402929451e-8,
+            "angularVelocity": 5.625262643908853e-7,
+            "velocityX": -0.0000021460981663034263,
+            "velocityY": -3.193522528464227,
+            "timestamp": 0.9575052183150281
         },
         {
-            "x": 7.680000170899522,
-            "y": 1.0163361129826003,
-            "heading": 3.139999958602803,
-            "angularVelocity": 4.3169222333319617e-7,
-            "velocityX": -0.0000016867725067406415,
-            "velocityY": -2.3951420785404567,
-            "timestamp": 1.0532557401465308
+            "x": 7.680000170894954,
+            "y": 1.0163361129826005,
+            "heading": -4.114641273305121e-8,
+            "angularVelocity": 4.2971617502568956e-7,
+            "velocityX": -0.000001686733705584938,
+            "velocityY": -2.395142078540457,
+            "timestamp": 1.053255740146531
         },
         {
-            "x": 7.680000058494392,
+            "x": 7.680000058492725,
             "y": 0.8634453720055071,
-            "heading": 3.1399999862669987,
-            "angularVelocity": 2.889195077791466e-7,
-            "velocityX": -0.0000011739374912229912,
-            "velocityY": -1.5967614385030957,
-            "timestamp": 1.1490062619780337
+            "heading": -1.3639289939958608e-8,
+            "angularVelocity": 2.872790734007971e-7,
+            "velocityX": -0.0000011739071958177648,
+            "velocityY": -1.5967614385030962,
+            "timestamp": 1.149006261978034
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 1.4342481443578566e-7,
-            "velocityX": -6.10904148268517e-7,
-            "velocityY": -0.7983807350943894,
-            "timestamp": 1.2447567838095366
+            "heading": 0,
+            "angularVelocity": 1.4244610959089762e-7,
+            "velocityX": -6.10886739716558e-7,
+            "velocityY": -0.7983807350943898,
+            "timestamp": 1.2447567838095368
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
-            "velocityX": -3.045628877095783e-32,
-            "velocityY": -7.047678165048749e-38,
-            "timestamp": 1.3405073056410395
+            "velocityX": 6.08432159346481e-32,
+            "velocityY": 1.351843649746573e-37,
+            "timestamp": 1.3405073056410397
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.traj
index 7ea9ac6c..c126c683 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.traj	
@@ -3,137 +3,137 @@
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 9.207800442360542e-32,
-            "velocityX": -6.095364969086891e-33,
-            "velocityY": -9.366947302592518e-40,
+            "heading": 0,
+            "angularVelocity": 2.405138964716482e-32,
+            "velocityX": 6.733008165659914e-33,
+            "velocityY": 1.175464991429931e-38,
             "timestamp": 0
         },
         {
-            "x": 7.680000070435844,
-            "y": 4.0505546279944795,
-            "heading": 3.1399999875764033,
-            "angularVelocity": -1.2974964830179728e-7,
-            "velocityX": 7.356183706178639e-7,
-            "velocityY": -0.7983807350945191,
-            "timestamp": 0.0957505218315028
+            "x": 7.6800000704332305,
+            "y": 4.05055462799448,
+            "heading": -1.2377486895022226e-8,
+            "angularVelocity": -1.2926808085167386e-7,
+            "velocityX": 7.355910665177674e-7,
+            "velocityY": -0.7983807350945193,
+            "timestamp": 0.09575052183150283
         },
         {
-            "x": 7.680000202739516,
+            "x": 7.680000202731891,
             "y": 3.8976638870173534,
-            "heading": 3.1399999616102225,
-            "angularVelocity": -2.711857701387203e-7,
-            "velocityX": 0.0000013817539912349048,
-            "velocityY": -1.5967614385034457,
-            "timestamp": 0.1915010436630056
+            "heading": -3.8235321936944764e-8,
+            "angularVelocity": -2.700542284308498e-7,
+            "velocityX": 0.0000013817016646870076,
+            "velocityY": -1.5967614385034459,
+            "timestamp": 0.19150104366300566
         },
         {
-            "x": 7.680000388342391,
+            "x": 7.6800003883277,
             "y": 3.6683277831364425,
-            "heading": 3.139999920718594,
-            "angularVelocity": -4.2706425730703774e-7,
-            "velocityX": 0.00000193840056979922,
-            "velocityY": -2.395142078541202,
-            "timestamp": 0.2872515654945084
+            "heading": -7.893916226933331e-8,
+            "angularVelocity": -4.251030288838169e-7,
+            "velocityX": 0.0000019383267807539473,
+            "velocityY": -2.3951420785412023,
+            "timestamp": 0.2872515654945085
         },
         {
-            "x": 7.680000618679091,
-            "y": 3.362546334555153,
-            "heading": 3.139999863216847,
-            "angularVelocity": -6.005370836912587e-7,
-            "velocityX": 0.000002405591946532482,
-            "velocityY": -3.1935225284661044,
-            "timestamp": 0.3830020873260112
+            "x": 7.680000618655782,
+            "y": 3.3625463345551534,
+            "heading": -1.3615412726424167e-7,
+            "angularVelocity": -5.975419949298212e-7,
+            "velocityX": 0.0000024055019376452327,
+            "velocityY": -3.193522528466104,
+            "timestamp": 0.3830020873260113
         },
         {
-            "x": 7.680000750151485,
-            "y": 3.000327800732898,
-            "heading": 3.139999863216849,
-            "angularVelocity": 2.234907225706593e-14,
-            "velocityX": 0.0000013730725434542363,
-            "velocityY": -3.7829405719549856,
-            "timestamp": 0.478752609157514
+            "x": 7.680000750128537,
+            "y": 3.0003278007328986,
+            "heading": -1.3615412487460524e-7,
+            "angularVelocity": 2.2064625199669458e-14,
+            "velocityX": 0.000001373076316791352,
+            "velocityY": -3.7829405719549847,
+            "timestamp": 0.47875260915751416
         },
         {
-            "x": 7.6800008034310885,
-            "y": 2.6381092669108033,
-            "heading": 3.139999863216839,
-            "angularVelocity": -1.1397010502093565e-13,
-            "velocityX": 5.56441925019577e-7,
+            "x": 7.68000080341356,
+            "y": 2.638109266910804,
+            "heading": -1.3615413544372385e-7,
+            "angularVelocity": -1.1353706918076568e-13,
+            "velocityX": 5.564985227619036e-7,
             "velocityY": -3.7829405719533264,
-            "timestamp": 0.5745031309890168
+            "timestamp": 0.574503130989017
         },
         {
-            "x": 7.68000078216065,
-            "y": 2.275890733088706,
-            "heading": 3.1399998632168313,
-            "angularVelocity": -8.126780055977152e-14,
-            "velocityX": -2.2214435413644554e-7,
-            "velocityY": -3.7829405719533535,
-            "timestamp": 0.6702536528205196
+            "x": 7.680000782147778,
+            "y": 2.2758907330887066,
+            "heading": -1.361541430398869e-7,
+            "angularVelocity": -8.149555996364277e-14,
+            "velocityX": -2.2209570916932486e-7,
+            "velocityY": -3.782940571953353,
+            "timestamp": 0.6702536528205197
         },
         {
-            "x": 7.6800006913830785,
-            "y": 1.9136721992666206,
-            "heading": 3.1399998632168296,
-            "angularVelocity": -1.6383854112995832e-14,
-            "velocityX": -9.4806343038794e-7,
+            "x": 7.68000069137085,
+            "y": 1.9136721992666208,
+            "heading": -1.3615414447383612e-7,
+            "angularVelocity": -1.5331626502863808e-14,
+            "velocityX": -9.48056722512506e-7,
             "velocityY": -3.7829405719532305,
-            "timestamp": 0.7660041746520224
+            "timestamp": 0.7660041746520225
         },
         {
-            "x": 7.680000537903045,
+            "x": 7.68000053789062,
             "y": 1.55145366544455,
-            "heading": 3.13999986321682,
-            "angularVelocity": -1.0277752214408611e-13,
-            "velocityX": -0.0000016029159878741863,
-            "velocityY": -3.7829405719530524,
-            "timestamp": 0.8617546964835252
+            "heading": -1.361541542308917e-7,
+            "angularVelocity": -1.0101247732712604e-13,
+            "velocityX": -0.000001602918056057891,
+            "velocityY": -3.782940571953052,
+            "timestamp": 0.8617546964835253
         },
         {
-            "x": 7.680000332408874,
+            "x": 7.68000033240059,
             "y": 1.2456722168634402,
-            "heading": 3.139999917268044,
-            "angularVelocity": 5.645005022977776e-7,
-            "velocityX": -0.0000021461414302094452,
-            "velocityY": -3.1935225284642264,
-            "timestamp": 0.957505218315028
+            "heading": -8.229196402929451e-8,
+            "angularVelocity": 5.625262643908853e-7,
+            "velocityX": -0.0000021460981663034263,
+            "velocityY": -3.193522528464227,
+            "timestamp": 0.9575052183150281
         },
         {
-            "x": 7.680000170899522,
-            "y": 1.0163361129826003,
-            "heading": 3.139999958602803,
-            "angularVelocity": 4.3169222333319617e-7,
-            "velocityX": -0.0000016867725067406415,
-            "velocityY": -2.3951420785404567,
-            "timestamp": 1.0532557401465308
+            "x": 7.680000170894954,
+            "y": 1.0163361129826005,
+            "heading": -4.114641273305121e-8,
+            "angularVelocity": 4.2971617502568956e-7,
+            "velocityX": -0.000001686733705584938,
+            "velocityY": -2.395142078540457,
+            "timestamp": 1.053255740146531
         },
         {
-            "x": 7.680000058494392,
+            "x": 7.680000058492725,
             "y": 0.8634453720055071,
-            "heading": 3.1399999862669987,
-            "angularVelocity": 2.889195077791466e-7,
-            "velocityX": -0.0000011739374912229912,
-            "velocityY": -1.5967614385030957,
-            "timestamp": 1.1490062619780337
+            "heading": -1.3639289939958608e-8,
+            "angularVelocity": 2.872790734007971e-7,
+            "velocityX": -0.0000011739071958177648,
+            "velocityY": -1.5967614385030962,
+            "timestamp": 1.149006261978034
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 1.4342481443578566e-7,
-            "velocityX": -6.10904148268517e-7,
-            "velocityY": -0.7983807350943894,
-            "timestamp": 1.2447567838095366
+            "heading": 0,
+            "angularVelocity": 1.4244610959089762e-7,
+            "velocityX": -6.10886739716558e-7,
+            "velocityY": -0.7983807350943898,
+            "timestamp": 1.2447567838095368
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
-            "velocityX": -3.045628877095783e-32,
-            "velocityY": -7.047678165048749e-38,
-            "timestamp": 1.3405073056410395
+            "velocityX": 6.08432159346481e-32,
+            "velocityY": 1.351843649746573e-37,
+            "timestamp": 1.3405073056410397
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.1.traj
index 626b4f65..e393ae58 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.1.traj	
@@ -3,155 +3,155 @@
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": -7.693901589446952e-31,
-            "velocityX": -6.624376358473947e-32,
+            "heading": -7.709535672280055e-33,
+            "angularVelocity": 3.0776034223095e-32,
+            "velocityX": -2.0787724059290825e-32,
             "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.653617802747577,
-            "y": 4.193887846119818,
-            "heading": 3.0831794581771232,
-            "angularVelocity": -0.6046893294524585,
-            "velocityX": -0.2807617219803893,
-            "velocityY": 0.7118264895254428,
-            "timestamp": 0.0939665032196413
-        },
-        {
-            "x": 7.6005882437737835,
-            "y": 4.328100423051846,
-            "heading": 2.9787754661598114,
-            "angularVelocity": -1.1110766969084016,
-            "velocityX": -0.5643453481485778,
-            "velocityY": 1.4283023453401595,
-            "timestamp": 0.1879330064392826
-        },
-        {
-            "x": 7.520633934300323,
-            "y": 4.5302283764630555,
-            "heading": 2.841927884917239,
-            "angularVelocity": -1.4563442988050657,
-            "velocityX": -0.8508809707068956,
-            "velocityY": 2.1510639055999357,
-            "timestamp": 0.2818995096589239
-        },
-        {
-            "x": 7.4136169796865685,
-            "y": 4.800820969540026,
-            "heading": 2.7078159162812883,
-            "angularVelocity": -1.4272316627817019,
-            "velocityX": -1.1388840804643727,
-            "velocityY": 2.879670774217013,
-            "timestamp": 0.3758660128785652
-        },
-        {
-            "x": 7.279364393815875,
-            "y": 5.128465147908359,
-            "heading": 2.7044182580824483,
-            "angularVelocity": -0.03615818491083086,
-            "velocityX": -1.4287281240729475,
-            "velocityY": 3.486818889093733,
-            "timestamp": 0.4698325160982065
-        },
-        {
-            "x": 7.14774134031491,
-            "y": 5.45866822544814,
-            "heading": 2.7044182151788783,
-            "angularVelocity": -4.565836605912663e-7,
-            "velocityX": -1.4007444034956136,
-            "velocityY": 3.514050924806152,
-            "timestamp": 0.5637990193178478
-        },
-        {
-            "x": 6.993945263950606,
-            "y": 5.779145162484634,
-            "heading": 2.704418171322215,
-            "angularVelocity": -4.667265617625592e-7,
-            "velocityX": -1.636711712096111,
-            "velocityY": 3.410544460587175,
-            "timestamp": 0.6577655225374891
-        },
-        {
-            "x": 6.777429472030302,
-            "y": 6.061066444299655,
-            "heading": 2.704418118850798,
-            "angularVelocity": -5.58405553736054e-7,
-            "velocityX": -2.3041805803309763,
-            "velocityY": 3.000231701248326,
-            "timestamp": 0.7517320257571304
-        },
-        {
-            "x": 6.5074813547317545,
-            "y": 6.2923383909446455,
-            "heading": 2.7044180981669834,
-            "angularVelocity": -2.2011901843785396e-7,
-            "velocityX": -2.872812204872187,
-            "velocityY": 2.461216909438525,
-            "timestamp": 0.8456985289767717
-        },
-        {
-            "x": 6.195734686238664,
-            "y": 6.463002989375195,
-            "heading": 2.7046305201623446,
-            "angularVelocity": 0.002260614028208905,
-            "velocityX": -3.317636155560676,
-            "velocityY": 1.81622804491969,
-            "timestamp": 0.939665032196413
-        },
-        {
-            "x": 5.885885567596677,
-            "y": 6.564828261025872,
-            "heading": 2.8003077522097084,
-            "angularVelocity": 1.018205730436987,
-            "velocityX": -3.297442258947648,
-            "velocityY": 1.0836337222495915,
-            "timestamp": 1.0336315354160543
-        },
-        {
-            "x": 5.602675613806244,
-            "y": 6.605201169261095,
-            "heading": 3.005504879522894,
-            "angularVelocity": 2.183726331004879,
-            "velocityX": -3.0139458646071584,
-            "velocityY": 0.4296521297685472,
-            "timestamp": 1.1275980386356956
-        },
-        {
-            "x": 5.389000568947429,
-            "y": 6.621873348579134,
-            "heading": 3.193372437152187,
-            "angularVelocity": 1.999303487862736,
-            "velocityX": -2.273949093959171,
-            "velocityY": 0.17742683559341804,
-            "timestamp": 1.221564541855337
-        },
-        {
-            "x": 5.246855829881342,
-            "y": 6.628281724607146,
-            "heading": 3.330497456216328,
-            "angularVelocity": 1.459296817118111,
-            "velocityX": -1.5127171299950581,
-            "velocityY": 0.06819851551815755,
-            "timestamp": 1.3155310450749782
+            "x": 7.653625318689537,
+            "y": 4.193887618013532,
+            "heading": -0.05699658004084171,
+            "angularVelocity": -0.6065329552716214,
+            "velocityX": -0.28066795214939116,
+            "velocityY": 0.7117891037630026,
+            "timestamp": 0.0939711182145365
+        },
+        {
+            "x": 7.600609429037226,
+            "y": 4.328102182971276,
+            "heading": -0.1617251039431606,
+            "angularVelocity": -1.1144756590341216,
+            "velocityX": -0.5641721697008566,
+            "velocityY": 1.4282533560080801,
+            "timestamp": 0.187942236429073
+        },
+        {
+            "x": 7.520673672603614,
+            "y": 4.5302373715153195,
+            "heading": -0.299017155202882,
+            "angularVelocity": -1.4610026343017766,
+            "velocityX": -0.8506417498525306,
+            "velocityY": 2.151035258328725,
+            "timestamp": 0.2819133546436095
+        },
+        {
+            "x": 7.413680530437971,
+            "y": 4.80084724043498,
+            "heading": -0.43363333911195256,
+            "angularVelocity": -1.4325272111984575,
+            "velocityX": -1.1385747472045327,
+            "velocityY": 2.8797131933862543,
+            "timestamp": 0.375884472858146
+        },
+        {
+            "x": 7.279380160390009,
+            "y": 5.1284214711267,
+            "heading": -0.4371879316114584,
+            "angularVelocity": -0.037826436111898676,
+            "velocityX": -1.4291664566697349,
+            "velocityY": 3.4859032957750506,
+            "timestamp": 0.46985559107268254
+        },
+        {
+            "x": 7.147763767829931,
+            "y": 5.458645997615663,
+            "heading": -0.43718797458387054,
+            "angularVelocity": -4.5729382513446123e-7,
+            "velocityX": -1.4006047289934083,
+            "velocityY": 3.514106597466034,
+            "timestamp": 0.563826709287219
+        },
+        {
+            "x": 6.993957305230812,
+            "y": 5.779137314807096,
+            "heading": -0.43718801851214967,
+            "angularVelocity": -4.6746574877776706e-7,
+            "velocityX": -1.6367418577267254,
+            "velocityY": 3.410529993478958,
+            "timestamp": 0.6577978275017555
+        },
+        {
+            "x": 6.777425503833482,
+            "y": 6.0610683140345785,
+            "heading": -0.43718807107725144,
+            "angularVelocity": -5.593750798607861e-7,
+            "velocityX": -2.3042377861566687,
+            "velocityY": 3.000187766030755,
+            "timestamp": 0.751768945716292
+        },
+        {
+            "x": 6.507457353311083,
+            "y": 6.292343710685516,
+            "heading": -0.4371880918313372,
+            "angularVelocity": -2.2085600557820128e-7,
+            "velocityX": -2.87288430372895,
+            "velocityY": 2.4611327506280603,
+            "timestamp": 0.8457400639308286
+        },
+        {
+            "x": 6.1957042493791805,
+            "y": 6.462993619393473,
+            "heading": -0.4369141825473297,
+            "angularVelocity": 0.0029148241418374306,
+            "velocityX": -3.3175417070186306,
+            "velocityY": 1.8159825268692036,
+            "timestamp": 0.9397111821453651
+        },
+        {
+            "x": 5.885878112264826,
+            "y": 6.564806359719212,
+            "heading": -0.3410861416211598,
+            "angularVelocity": 1.0197605684269313,
+            "velocityX": -3.297035759508787,
+            "velocityY": 1.083447151211916,
+            "timestamp": 1.0336823003599016
+        },
+        {
+            "x": 5.602663899704114,
+            "y": 6.605182844765102,
+            "heading": -0.13545254769192583,
+            "angularVelocity": 2.1882637754695176,
+            "velocityX": -3.0138431673669435,
+            "velocityY": 0.4296690920896744,
+            "timestamp": 1.127653418574438
+        },
+        {
+            "x": 5.388992479168701,
+            "y": 6.621862905384601,
+            "heading": 0.0527880784098822,
+            "angularVelocity": 2.0031753338515554,
+            "velocityX": -2.2737988500636965,
+            "velocityY": 0.17750199142483714,
+            "timestamp": 1.2216245367889744
+        },
+        {
+            "x": 5.246852509457665,
+            "y": 6.628278045699216,
+            "heading": 0.19017469868317557,
+            "angularVelocity": 1.4620089968455965,
+            "velocityX": -1.5125920858633375,
+            "velocityY": 0.06826714884852915,
+            "timestamp": 1.3155956550035108
         },
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.7735506640420036,
-            "velocityX": -0.7540541304992547,
-            "velocityY": 0.018286041663562246,
-            "timestamp": 1.4094975482946195
+            "heading": 0.263,
+            "angularVelocity": 0.7749753615846516,
+            "velocityX": -0.7539817638001085,
+            "velocityY": 0.01832429296896703,
+            "timestamp": 1.4095667732180472
         },
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 3.878626979187187e-32,
-            "velocityX": -5.758991747455131e-33,
-            "velocityY": 3.749392123602624e-30,
-            "timestamp": 1.5034640515142608
+            "heading": 0.263,
+            "angularVelocity": 6.971645129510918e-32,
+            "velocityX": 3.634932162806737e-32,
+            "velocityY": 0,
+            "timestamp": 1.5035378914325837
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.traj
index 626b4f65..e393ae58 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.traj	
@@ -3,155 +3,155 @@
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": -7.693901589446952e-31,
-            "velocityX": -6.624376358473947e-32,
+            "heading": -7.709535672280055e-33,
+            "angularVelocity": 3.0776034223095e-32,
+            "velocityX": -2.0787724059290825e-32,
             "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.653617802747577,
-            "y": 4.193887846119818,
-            "heading": 3.0831794581771232,
-            "angularVelocity": -0.6046893294524585,
-            "velocityX": -0.2807617219803893,
-            "velocityY": 0.7118264895254428,
-            "timestamp": 0.0939665032196413
-        },
-        {
-            "x": 7.6005882437737835,
-            "y": 4.328100423051846,
-            "heading": 2.9787754661598114,
-            "angularVelocity": -1.1110766969084016,
-            "velocityX": -0.5643453481485778,
-            "velocityY": 1.4283023453401595,
-            "timestamp": 0.1879330064392826
-        },
-        {
-            "x": 7.520633934300323,
-            "y": 4.5302283764630555,
-            "heading": 2.841927884917239,
-            "angularVelocity": -1.4563442988050657,
-            "velocityX": -0.8508809707068956,
-            "velocityY": 2.1510639055999357,
-            "timestamp": 0.2818995096589239
-        },
-        {
-            "x": 7.4136169796865685,
-            "y": 4.800820969540026,
-            "heading": 2.7078159162812883,
-            "angularVelocity": -1.4272316627817019,
-            "velocityX": -1.1388840804643727,
-            "velocityY": 2.879670774217013,
-            "timestamp": 0.3758660128785652
-        },
-        {
-            "x": 7.279364393815875,
-            "y": 5.128465147908359,
-            "heading": 2.7044182580824483,
-            "angularVelocity": -0.03615818491083086,
-            "velocityX": -1.4287281240729475,
-            "velocityY": 3.486818889093733,
-            "timestamp": 0.4698325160982065
-        },
-        {
-            "x": 7.14774134031491,
-            "y": 5.45866822544814,
-            "heading": 2.7044182151788783,
-            "angularVelocity": -4.565836605912663e-7,
-            "velocityX": -1.4007444034956136,
-            "velocityY": 3.514050924806152,
-            "timestamp": 0.5637990193178478
-        },
-        {
-            "x": 6.993945263950606,
-            "y": 5.779145162484634,
-            "heading": 2.704418171322215,
-            "angularVelocity": -4.667265617625592e-7,
-            "velocityX": -1.636711712096111,
-            "velocityY": 3.410544460587175,
-            "timestamp": 0.6577655225374891
-        },
-        {
-            "x": 6.777429472030302,
-            "y": 6.061066444299655,
-            "heading": 2.704418118850798,
-            "angularVelocity": -5.58405553736054e-7,
-            "velocityX": -2.3041805803309763,
-            "velocityY": 3.000231701248326,
-            "timestamp": 0.7517320257571304
-        },
-        {
-            "x": 6.5074813547317545,
-            "y": 6.2923383909446455,
-            "heading": 2.7044180981669834,
-            "angularVelocity": -2.2011901843785396e-7,
-            "velocityX": -2.872812204872187,
-            "velocityY": 2.461216909438525,
-            "timestamp": 0.8456985289767717
-        },
-        {
-            "x": 6.195734686238664,
-            "y": 6.463002989375195,
-            "heading": 2.7046305201623446,
-            "angularVelocity": 0.002260614028208905,
-            "velocityX": -3.317636155560676,
-            "velocityY": 1.81622804491969,
-            "timestamp": 0.939665032196413
-        },
-        {
-            "x": 5.885885567596677,
-            "y": 6.564828261025872,
-            "heading": 2.8003077522097084,
-            "angularVelocity": 1.018205730436987,
-            "velocityX": -3.297442258947648,
-            "velocityY": 1.0836337222495915,
-            "timestamp": 1.0336315354160543
-        },
-        {
-            "x": 5.602675613806244,
-            "y": 6.605201169261095,
-            "heading": 3.005504879522894,
-            "angularVelocity": 2.183726331004879,
-            "velocityX": -3.0139458646071584,
-            "velocityY": 0.4296521297685472,
-            "timestamp": 1.1275980386356956
-        },
-        {
-            "x": 5.389000568947429,
-            "y": 6.621873348579134,
-            "heading": 3.193372437152187,
-            "angularVelocity": 1.999303487862736,
-            "velocityX": -2.273949093959171,
-            "velocityY": 0.17742683559341804,
-            "timestamp": 1.221564541855337
-        },
-        {
-            "x": 5.246855829881342,
-            "y": 6.628281724607146,
-            "heading": 3.330497456216328,
-            "angularVelocity": 1.459296817118111,
-            "velocityX": -1.5127171299950581,
-            "velocityY": 0.06819851551815755,
-            "timestamp": 1.3155310450749782
+            "x": 7.653625318689537,
+            "y": 4.193887618013532,
+            "heading": -0.05699658004084171,
+            "angularVelocity": -0.6065329552716214,
+            "velocityX": -0.28066795214939116,
+            "velocityY": 0.7117891037630026,
+            "timestamp": 0.0939711182145365
+        },
+        {
+            "x": 7.600609429037226,
+            "y": 4.328102182971276,
+            "heading": -0.1617251039431606,
+            "angularVelocity": -1.1144756590341216,
+            "velocityX": -0.5641721697008566,
+            "velocityY": 1.4282533560080801,
+            "timestamp": 0.187942236429073
+        },
+        {
+            "x": 7.520673672603614,
+            "y": 4.5302373715153195,
+            "heading": -0.299017155202882,
+            "angularVelocity": -1.4610026343017766,
+            "velocityX": -0.8506417498525306,
+            "velocityY": 2.151035258328725,
+            "timestamp": 0.2819133546436095
+        },
+        {
+            "x": 7.413680530437971,
+            "y": 4.80084724043498,
+            "heading": -0.43363333911195256,
+            "angularVelocity": -1.4325272111984575,
+            "velocityX": -1.1385747472045327,
+            "velocityY": 2.8797131933862543,
+            "timestamp": 0.375884472858146
+        },
+        {
+            "x": 7.279380160390009,
+            "y": 5.1284214711267,
+            "heading": -0.4371879316114584,
+            "angularVelocity": -0.037826436111898676,
+            "velocityX": -1.4291664566697349,
+            "velocityY": 3.4859032957750506,
+            "timestamp": 0.46985559107268254
+        },
+        {
+            "x": 7.147763767829931,
+            "y": 5.458645997615663,
+            "heading": -0.43718797458387054,
+            "angularVelocity": -4.5729382513446123e-7,
+            "velocityX": -1.4006047289934083,
+            "velocityY": 3.514106597466034,
+            "timestamp": 0.563826709287219
+        },
+        {
+            "x": 6.993957305230812,
+            "y": 5.779137314807096,
+            "heading": -0.43718801851214967,
+            "angularVelocity": -4.6746574877776706e-7,
+            "velocityX": -1.6367418577267254,
+            "velocityY": 3.410529993478958,
+            "timestamp": 0.6577978275017555
+        },
+        {
+            "x": 6.777425503833482,
+            "y": 6.0610683140345785,
+            "heading": -0.43718807107725144,
+            "angularVelocity": -5.593750798607861e-7,
+            "velocityX": -2.3042377861566687,
+            "velocityY": 3.000187766030755,
+            "timestamp": 0.751768945716292
+        },
+        {
+            "x": 6.507457353311083,
+            "y": 6.292343710685516,
+            "heading": -0.4371880918313372,
+            "angularVelocity": -2.2085600557820128e-7,
+            "velocityX": -2.87288430372895,
+            "velocityY": 2.4611327506280603,
+            "timestamp": 0.8457400639308286
+        },
+        {
+            "x": 6.1957042493791805,
+            "y": 6.462993619393473,
+            "heading": -0.4369141825473297,
+            "angularVelocity": 0.0029148241418374306,
+            "velocityX": -3.3175417070186306,
+            "velocityY": 1.8159825268692036,
+            "timestamp": 0.9397111821453651
+        },
+        {
+            "x": 5.885878112264826,
+            "y": 6.564806359719212,
+            "heading": -0.3410861416211598,
+            "angularVelocity": 1.0197605684269313,
+            "velocityX": -3.297035759508787,
+            "velocityY": 1.083447151211916,
+            "timestamp": 1.0336823003599016
+        },
+        {
+            "x": 5.602663899704114,
+            "y": 6.605182844765102,
+            "heading": -0.13545254769192583,
+            "angularVelocity": 2.1882637754695176,
+            "velocityX": -3.0138431673669435,
+            "velocityY": 0.4296690920896744,
+            "timestamp": 1.127653418574438
+        },
+        {
+            "x": 5.388992479168701,
+            "y": 6.621862905384601,
+            "heading": 0.0527880784098822,
+            "angularVelocity": 2.0031753338515554,
+            "velocityX": -2.2737988500636965,
+            "velocityY": 0.17750199142483714,
+            "timestamp": 1.2216245367889744
+        },
+        {
+            "x": 5.246852509457665,
+            "y": 6.628278045699216,
+            "heading": 0.19017469868317557,
+            "angularVelocity": 1.4620089968455965,
+            "velocityX": -1.5125920858633375,
+            "velocityY": 0.06826714884852915,
+            "timestamp": 1.3155956550035108
         },
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.7735506640420036,
-            "velocityX": -0.7540541304992547,
-            "velocityY": 0.018286041663562246,
-            "timestamp": 1.4094975482946195
+            "heading": 0.263,
+            "angularVelocity": 0.7749753615846516,
+            "velocityX": -0.7539817638001085,
+            "velocityY": 0.01832429296896703,
+            "timestamp": 1.4095667732180472
         },
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 3.878626979187187e-32,
-            "velocityX": -5.758991747455131e-33,
-            "velocityY": 3.749392123602624e-30,
-            "timestamp": 1.5034640515142608
+            "heading": 0.263,
+            "angularVelocity": 6.971645129510918e-32,
+            "velocityX": 3.634932162806737e-32,
+            "velocityY": 0,
+            "timestamp": 1.5035378914325837
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S2.1.traj
new file mode 100644
index 00000000..67cbd06a
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S2.1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 1.358270377512338e-32,
+            "angularVelocity": -4.2989327725546786e-32,
+            "velocityX": 9.28706448380456e-34,
+            "velocityY": 4.1113328098198935e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 7.585313636391529,
+            "y": 4.127746947940637,
+            "heading": -0.08924586799649764,
+            "angularVelocity": -0.8241960155421162,
+            "velocityX": -0.8744396280098535,
+            "velocityY": 0.006898151480975746,
+            "timestamp": 0.10828233370892486
+        },
+        {
+            "x": 7.394793136195202,
+            "y": 4.129317895600586,
+            "heading": -0.2483761167773939,
+            "angularVelocity": -1.4695864351119023,
+            "velocityX": -1.7594790735529262,
+            "velocityY": 0.014507885138232623,
+            "timestamp": 0.21656466741784972
+        },
+        {
+            "x": 7.106938899563738,
+            "y": 4.131715387581193,
+            "heading": -0.440143741107867,
+            "angularVelocity": -1.7709964106053178,
+            "velocityX": -2.6583674988502586,
+            "velocityY": 0.022141118486160887,
+            "timestamp": 0.32484700112677456
+        },
+        {
+            "x": 6.726884682874397,
+            "y": 4.136782702937484,
+            "heading": -0.5152103460374668,
+            "angularVelocity": -0.6932488648738155,
+            "velocityX": -3.509845084341914,
+            "velocityY": 0.04679724921622947,
+            "timestamp": 0.43312933483569943
+        },
+        {
+            "x": 6.317264615210477,
+            "y": 4.138917325587368,
+            "heading": -0.515210397277194,
+            "angularVelocity": -4.7320486478177106e-7,
+            "velocityX": -3.782889171607859,
+            "velocityY": 0.019713489511795114,
+            "timestamp": 0.5414116685446243
+        },
+        {
+            "x": 5.907644548289307,
+            "y": 4.14105209076814,
+            "heading": -0.5152104485168384,
+            "angularVelocity": -4.732041015646636e-7,
+            "velocityX": -3.7828891647484775,
+            "velocityY": 0.019714805801191258,
+            "timestamp": 0.6496940022535491
+        },
+        {
+            "x": 5.500258277815471,
+            "y": 4.183825709430579,
+            "heading": -0.5152105024899405,
+            "angularVelocity": -4.984479029228616e-7,
+            "velocityX": -3.762259793632967,
+            "velocityY": 0.3950193646307994,
+            "timestamp": 0.7579763359624739
+        },
+        {
+            "x": 5.114610765355458,
+            "y": 4.3219164843017905,
+            "heading": -0.5152109227554116,
+            "angularVelocity": -0.000003881200715936794,
+            "velocityX": -3.5614998241234512,
+            "velocityY": 1.2752844359857862,
+            "timestamp": 0.8662586696713988
+        },
+        {
+            "x": 4.790273412388094,
+            "y": 4.533322369220829,
+            "heading": -0.5901314422767667,
+            "angularVelocity": -0.6918997490647923,
+            "velocityX": -2.9952933397170605,
+            "velocityY": 1.952358041038544,
+            "timestamp": 0.9745410033803236
+        },
+        {
+            "x": 4.547771251602889,
+            "y": 4.790487049727204,
+            "heading": -0.7662492650374113,
+            "angularVelocity": -1.6264686650923976,
+            "velocityX": -2.239535780943542,
+            "velocityY": 2.3749458632620883,
+            "timestamp": 1.0828233370892484
+        },
+        {
+            "x": 4.394005608234175,
+            "y": 5.057835455573767,
+            "heading": -0.8024780004938983,
+            "angularVelocity": -0.3345766037318139,
+            "velocityX": -1.4200436775037824,
+            "velocityY": 2.468993756315091,
+            "timestamp": 1.1911056707981733
+        },
+        {
+            "x": 4.299277084870805,
+            "y": 5.304155354811625,
+            "heading": -0.5153879845736841,
+            "angularVelocity": 2.651309831315093,
+            "velocityX": -0.8748289782709269,
+            "velocityY": 2.2747930414945974,
+            "timestamp": 1.2993880045070982
+        },
+        {
+            "x": 4.268346904630433,
+            "y": 5.476762087961931,
+            "heading": -0.20205360473699635,
+            "angularVelocity": 2.893679597624539,
+            "velocityX": -0.2856438273995415,
+            "velocityY": 1.5940433424190128,
+            "timestamp": 1.4076703382160232
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 2.864290108945775e-30,
+            "angularVelocity": 1.8659886411401074,
+            "velocityX": -0.04937928882107947,
+            "velocityY": 0.7687118404911842,
+            "timestamp": 1.515952671924948
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": -1.339793214188515e-31,
+            "velocityX": -1.755749143439243e-31,
+            "velocityY": 0,
+            "timestamp": 1.624235005633873
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S2.traj
new file mode 100644
index 00000000..67cbd06a
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S2.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 1.358270377512338e-32,
+            "angularVelocity": -4.2989327725546786e-32,
+            "velocityX": 9.28706448380456e-34,
+            "velocityY": 4.1113328098198935e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 7.585313636391529,
+            "y": 4.127746947940637,
+            "heading": -0.08924586799649764,
+            "angularVelocity": -0.8241960155421162,
+            "velocityX": -0.8744396280098535,
+            "velocityY": 0.006898151480975746,
+            "timestamp": 0.10828233370892486
+        },
+        {
+            "x": 7.394793136195202,
+            "y": 4.129317895600586,
+            "heading": -0.2483761167773939,
+            "angularVelocity": -1.4695864351119023,
+            "velocityX": -1.7594790735529262,
+            "velocityY": 0.014507885138232623,
+            "timestamp": 0.21656466741784972
+        },
+        {
+            "x": 7.106938899563738,
+            "y": 4.131715387581193,
+            "heading": -0.440143741107867,
+            "angularVelocity": -1.7709964106053178,
+            "velocityX": -2.6583674988502586,
+            "velocityY": 0.022141118486160887,
+            "timestamp": 0.32484700112677456
+        },
+        {
+            "x": 6.726884682874397,
+            "y": 4.136782702937484,
+            "heading": -0.5152103460374668,
+            "angularVelocity": -0.6932488648738155,
+            "velocityX": -3.509845084341914,
+            "velocityY": 0.04679724921622947,
+            "timestamp": 0.43312933483569943
+        },
+        {
+            "x": 6.317264615210477,
+            "y": 4.138917325587368,
+            "heading": -0.515210397277194,
+            "angularVelocity": -4.7320486478177106e-7,
+            "velocityX": -3.782889171607859,
+            "velocityY": 0.019713489511795114,
+            "timestamp": 0.5414116685446243
+        },
+        {
+            "x": 5.907644548289307,
+            "y": 4.14105209076814,
+            "heading": -0.5152104485168384,
+            "angularVelocity": -4.732041015646636e-7,
+            "velocityX": -3.7828891647484775,
+            "velocityY": 0.019714805801191258,
+            "timestamp": 0.6496940022535491
+        },
+        {
+            "x": 5.500258277815471,
+            "y": 4.183825709430579,
+            "heading": -0.5152105024899405,
+            "angularVelocity": -4.984479029228616e-7,
+            "velocityX": -3.762259793632967,
+            "velocityY": 0.3950193646307994,
+            "timestamp": 0.7579763359624739
+        },
+        {
+            "x": 5.114610765355458,
+            "y": 4.3219164843017905,
+            "heading": -0.5152109227554116,
+            "angularVelocity": -0.000003881200715936794,
+            "velocityX": -3.5614998241234512,
+            "velocityY": 1.2752844359857862,
+            "timestamp": 0.8662586696713988
+        },
+        {
+            "x": 4.790273412388094,
+            "y": 4.533322369220829,
+            "heading": -0.5901314422767667,
+            "angularVelocity": -0.6918997490647923,
+            "velocityX": -2.9952933397170605,
+            "velocityY": 1.952358041038544,
+            "timestamp": 0.9745410033803236
+        },
+        {
+            "x": 4.547771251602889,
+            "y": 4.790487049727204,
+            "heading": -0.7662492650374113,
+            "angularVelocity": -1.6264686650923976,
+            "velocityX": -2.239535780943542,
+            "velocityY": 2.3749458632620883,
+            "timestamp": 1.0828233370892484
+        },
+        {
+            "x": 4.394005608234175,
+            "y": 5.057835455573767,
+            "heading": -0.8024780004938983,
+            "angularVelocity": -0.3345766037318139,
+            "velocityX": -1.4200436775037824,
+            "velocityY": 2.468993756315091,
+            "timestamp": 1.1911056707981733
+        },
+        {
+            "x": 4.299277084870805,
+            "y": 5.304155354811625,
+            "heading": -0.5153879845736841,
+            "angularVelocity": 2.651309831315093,
+            "velocityX": -0.8748289782709269,
+            "velocityY": 2.2747930414945974,
+            "timestamp": 1.2993880045070982
+        },
+        {
+            "x": 4.268346904630433,
+            "y": 5.476762087961931,
+            "heading": -0.20205360473699635,
+            "angularVelocity": 2.893679597624539,
+            "velocityX": -0.2856438273995415,
+            "velocityY": 1.5940433424190128,
+            "timestamp": 1.4076703382160232
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 2.864290108945775e-30,
+            "angularVelocity": 1.8659886411401074,
+            "velocityX": -0.04937928882107947,
+            "velocityY": 0.7687118404911842,
+            "timestamp": 1.515952671924948
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": -1.339793214188515e-31,
+            "velocityX": -1.755749143439243e-31,
+            "velocityY": 0,
+            "timestamp": 1.624235005633873
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S3.1.traj
new file mode 100644
index 00000000..c2db4e34
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S3.1.traj	
@@ -0,0 +1,166 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 4.8594393424661863e-32,
+            "velocityX": 8.070887902586468e-34,
+            "velocityY": -1.5484627527783442e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.613090966537703,
+            "y": 4.130415229492922,
+            "heading": -0.02261774188189381,
+            "angularVelocity": -0.25176724339812206,
+            "velocityX": -0.7447915447554241,
+            "velocityY": 0.038016302406854975,
+            "timestamp": 0.08983591977315897
+        },
+        {
+            "x": 7.479193313628693,
+            "y": 4.137259099498979,
+            "heading": -0.06454119971556918,
+            "angularVelocity": -0.46666698504702897,
+            "velocityX": -1.4904689933291897,
+            "velocityY": 0.0761818883097586,
+            "timestamp": 0.17967183954631794
+        },
+        {
+            "x": 7.278200146895047,
+            "y": 4.147551727639681,
+            "heading": -0.12012980810986089,
+            "angularVelocity": -0.6187793093216658,
+            "velocityX": -2.237336326507015,
+            "velocityY": 0.11457141160316474,
+            "timestamp": 0.2695077593194769
+        },
+        {
+            "x": 7.010006404514947,
+            "y": 4.161329433657577,
+            "heading": -0.1775477987284744,
+            "angularVelocity": -0.6391429036434741,
+            "velocityX": -2.985373145364283,
+            "velocityY": 0.15336522465649807,
+            "timestamp": 0.35934367909263587
+        },
+        {
+            "x": 6.675632733992502,
+            "y": 4.178685588756415,
+            "heading": -0.19285230682653456,
+            "angularVelocity": -0.17036067687364617,
+            "velocityX": -3.7220487235639528,
+            "velocityY": 0.19319839038333309,
+            "timestamp": 0.44917959886579484
+        },
+        {
+            "x": 6.3362226416697665,
+            "y": 4.195852203162663,
+            "heading": -0.19285232652882817,
+            "angularVelocity": -2.193200640663355e-7,
+            "velocityX": -3.7781111740035236,
+            "velocityY": 0.1910885362012055,
+            "timestamp": 0.5390155186389538
+        },
+        {
+            "x": 5.996812544590127,
+            "y": 4.213018723498308,
+            "heading": -0.19285234623213549,
+            "angularVelocity": -2.1933156264800868e-7,
+            "velocityX": -3.7781112269558137,
+            "velocityY": 0.1910874890356692,
+            "timestamp": 0.6288514384121128
+        },
+        {
+            "x": 5.65703115405788,
+            "y": 4.206499194085455,
+            "heading": -0.1928524148916534,
+            "angularVelocity": -7.642946895727943e-7,
+            "velocityX": -3.782244244720454,
+            "velocityY": -0.07257152185432766,
+            "timestamp": 0.7186873581852717
+        },
+        {
+            "x": 5.325195518902672,
+            "y": 4.133157682156235,
+            "heading": -0.192852601151229,
+            "angularVelocity": -0.0000020733763948999505,
+            "velocityX": -3.6937968241559735,
+            "velocityY": -0.8163940672507154,
+            "timestamp": 0.8085232779584307
+        },
+        {
+            "x": 5.022921580906399,
+            "y": 3.999719932132448,
+            "heading": -0.21619143682617745,
+            "angularVelocity": -0.2597940304978627,
+            "velocityX": -3.364733602738329,
+            "velocityY": -1.485349628067107,
+            "timestamp": 0.8983591977315897
+        },
+        {
+            "x": 4.76405537852928,
+            "y": 3.8156128777716756,
+            "heading": -0.2707600447886353,
+            "angularVelocity": -0.6074252715762222,
+            "velocityX": -2.881544520603651,
+            "velocityY": -2.0493701720293274,
+            "timestamp": 0.9881951175047486
+        },
+        {
+            "x": 4.5584097754694755,
+            "y": 3.590752248351908,
+            "heading": -0.3464948618413025,
+            "angularVelocity": -0.8430349156856024,
+            "velocityX": -2.2891244791432657,
+            "velocityY": -2.5030147182477354,
+            "timestamp": 1.0780310372779076
+        },
+        {
+            "x": 4.420003796304514,
+            "y": 3.3682412112408735,
+            "heading": -0.42679529224450924,
+            "angularVelocity": -0.8938566066393836,
+            "velocityX": -1.5406529984228516,
+            "velocityY": -2.476860454851527,
+            "timestamp": 1.1678669570510665
+        },
+        {
+            "x": 4.3340794956684014,
+            "y": 3.187695881574611,
+            "heading": -0.4942860320434666,
+            "angularVelocity": -0.7512667535241839,
+            "velocityX": -0.9564581834687845,
+            "velocityY": -2.009723172222817,
+            "timestamp": 1.2577028768242253
+        },
+        {
+            "x": 4.284833149556787,
+            "y": 3.063288277698013,
+            "heading": -0.5416494132844137,
+            "angularVelocity": -0.5272209752980105,
+            "velocityX": -0.5481810197605272,
+            "velocityY": -1.3848314147664058,
+            "timestamp": 1.3475387965973842
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -0.27105624093568903,
+            "velocityX": -0.24303362855860308,
+            "velocityY": -0.7044874461979944,
+            "timestamp": 1.437374716370543
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 6.221864662981848e-31,
+            "velocityX": 0,
+            "velocityY": 2.6923638006152617e-32,
+            "timestamp": 1.527210636143702
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S3.traj
new file mode 100644
index 00000000..c2db4e34
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S3.traj	
@@ -0,0 +1,166 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 4.8594393424661863e-32,
+            "velocityX": 8.070887902586468e-34,
+            "velocityY": -1.5484627527783442e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.613090966537703,
+            "y": 4.130415229492922,
+            "heading": -0.02261774188189381,
+            "angularVelocity": -0.25176724339812206,
+            "velocityX": -0.7447915447554241,
+            "velocityY": 0.038016302406854975,
+            "timestamp": 0.08983591977315897
+        },
+        {
+            "x": 7.479193313628693,
+            "y": 4.137259099498979,
+            "heading": -0.06454119971556918,
+            "angularVelocity": -0.46666698504702897,
+            "velocityX": -1.4904689933291897,
+            "velocityY": 0.0761818883097586,
+            "timestamp": 0.17967183954631794
+        },
+        {
+            "x": 7.278200146895047,
+            "y": 4.147551727639681,
+            "heading": -0.12012980810986089,
+            "angularVelocity": -0.6187793093216658,
+            "velocityX": -2.237336326507015,
+            "velocityY": 0.11457141160316474,
+            "timestamp": 0.2695077593194769
+        },
+        {
+            "x": 7.010006404514947,
+            "y": 4.161329433657577,
+            "heading": -0.1775477987284744,
+            "angularVelocity": -0.6391429036434741,
+            "velocityX": -2.985373145364283,
+            "velocityY": 0.15336522465649807,
+            "timestamp": 0.35934367909263587
+        },
+        {
+            "x": 6.675632733992502,
+            "y": 4.178685588756415,
+            "heading": -0.19285230682653456,
+            "angularVelocity": -0.17036067687364617,
+            "velocityX": -3.7220487235639528,
+            "velocityY": 0.19319839038333309,
+            "timestamp": 0.44917959886579484
+        },
+        {
+            "x": 6.3362226416697665,
+            "y": 4.195852203162663,
+            "heading": -0.19285232652882817,
+            "angularVelocity": -2.193200640663355e-7,
+            "velocityX": -3.7781111740035236,
+            "velocityY": 0.1910885362012055,
+            "timestamp": 0.5390155186389538
+        },
+        {
+            "x": 5.996812544590127,
+            "y": 4.213018723498308,
+            "heading": -0.19285234623213549,
+            "angularVelocity": -2.1933156264800868e-7,
+            "velocityX": -3.7781112269558137,
+            "velocityY": 0.1910874890356692,
+            "timestamp": 0.6288514384121128
+        },
+        {
+            "x": 5.65703115405788,
+            "y": 4.206499194085455,
+            "heading": -0.1928524148916534,
+            "angularVelocity": -7.642946895727943e-7,
+            "velocityX": -3.782244244720454,
+            "velocityY": -0.07257152185432766,
+            "timestamp": 0.7186873581852717
+        },
+        {
+            "x": 5.325195518902672,
+            "y": 4.133157682156235,
+            "heading": -0.192852601151229,
+            "angularVelocity": -0.0000020733763948999505,
+            "velocityX": -3.6937968241559735,
+            "velocityY": -0.8163940672507154,
+            "timestamp": 0.8085232779584307
+        },
+        {
+            "x": 5.022921580906399,
+            "y": 3.999719932132448,
+            "heading": -0.21619143682617745,
+            "angularVelocity": -0.2597940304978627,
+            "velocityX": -3.364733602738329,
+            "velocityY": -1.485349628067107,
+            "timestamp": 0.8983591977315897
+        },
+        {
+            "x": 4.76405537852928,
+            "y": 3.8156128777716756,
+            "heading": -0.2707600447886353,
+            "angularVelocity": -0.6074252715762222,
+            "velocityX": -2.881544520603651,
+            "velocityY": -2.0493701720293274,
+            "timestamp": 0.9881951175047486
+        },
+        {
+            "x": 4.5584097754694755,
+            "y": 3.590752248351908,
+            "heading": -0.3464948618413025,
+            "angularVelocity": -0.8430349156856024,
+            "velocityX": -2.2891244791432657,
+            "velocityY": -2.5030147182477354,
+            "timestamp": 1.0780310372779076
+        },
+        {
+            "x": 4.420003796304514,
+            "y": 3.3682412112408735,
+            "heading": -0.42679529224450924,
+            "angularVelocity": -0.8938566066393836,
+            "velocityX": -1.5406529984228516,
+            "velocityY": -2.476860454851527,
+            "timestamp": 1.1678669570510665
+        },
+        {
+            "x": 4.3340794956684014,
+            "y": 3.187695881574611,
+            "heading": -0.4942860320434666,
+            "angularVelocity": -0.7512667535241839,
+            "velocityX": -0.9564581834687845,
+            "velocityY": -2.009723172222817,
+            "timestamp": 1.2577028768242253
+        },
+        {
+            "x": 4.284833149556787,
+            "y": 3.063288277698013,
+            "heading": -0.5416494132844137,
+            "angularVelocity": -0.5272209752980105,
+            "velocityX": -0.5481810197605272,
+            "velocityY": -1.3848314147664058,
+            "timestamp": 1.3475387965973842
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -0.27105624093568903,
+            "velocityX": -0.24303362855860308,
+            "velocityY": -0.7044874461979944,
+            "timestamp": 1.437374716370543
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 6.221864662981848e-31,
+            "velocityX": 0,
+            "velocityY": 2.6923638006152617e-32,
+            "timestamp": 1.527210636143702
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.1.traj
index 8079765c..2f31d71f 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.1.traj	
@@ -3,191 +3,191 @@
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 8.742415967937999e-33,
-            "velocityX": -1.6780440317133427e-34,
-            "velocityY": 4.640205301974453e-35,
+            "heading": 0,
+            "angularVelocity": -8.248091288877123e-33,
+            "velocityX": -1.7499589770527321e-34,
+            "velocityY": 1.243057151683689e-35,
             "timestamp": 0
         },
         {
-            "x": 7.599500768056594,
-            "y": 4.12633480373168,
-            "heading": 3.0835764078705523,
-            "angularVelocity": -0.5690015744722524,
-            "velocityX": -0.8117914508973499,
-            "velocityY": -0.006708146534495369,
-            "timestamp": 0.09916245342867555
-        },
-        {
-            "x": 7.438156123912552,
-            "y": 4.125079622260014,
-            "heading": 2.9779167441714383,
-            "angularVelocity": -1.065520870508837,
-            "velocityX": -1.6270739434668355,
-            "velocityY": -0.012657829937315014,
-            "timestamp": 0.1983249068573511
-        },
-        {
-            "x": 7.195408805362699,
-            "y": 4.123338544846998,
-            "heading": 2.8371936481283773,
-            "angularVelocity": -1.4191167238946776,
-            "velocityX": -2.4479761256053836,
-            "velocityY": -0.017557829125994403,
-            "timestamp": 0.2974873602860266
-        },
-        {
-            "x": 6.870725465009593,
-            "y": 4.121560782802172,
-            "heading": 2.707127276825701,
-            "angularVelocity": -1.311649387499567,
-            "velocityX": -3.2742568293415686,
-            "velocityY": -0.017927773903902572,
-            "timestamp": 0.3966498137147022
-        },
-        {
-            "x": 6.495621806376681,
-            "y": 4.117497642893674,
-            "heading": 2.7071272422206913,
-            "angularVelocity": -3.489729050319063e-7,
-            "velocityX": -3.7827186164036664,
-            "velocityY": -0.04097458027721316,
-            "timestamp": 0.4958122671433778
-        },
-        {
-            "x": 6.120518146053048,
-            "y": 4.113434659065473,
-            "heading": 2.7071272076158857,
-            "angularVelocity": -3.4897084992850824e-7,
-            "velocityX": -3.7827186334536713,
-            "velocityY": -0.04097300629137556,
-            "timestamp": 0.5949747205720534
-        },
-        {
-            "x": 5.746992350808683,
-            "y": 4.148043089964446,
-            "heading": 2.7071271713462406,
-            "angularVelocity": -3.657598578804205e-7,
-            "velocityX": -3.76680671291609,
-            "velocityY": 0.3490074085739077,
-            "timestamp": 0.6941371740007289
-        },
-        {
-            "x": 5.389907487037217,
-            "y": 4.262976321781226,
-            "heading": 2.7071270362524373,
-            "angularVelocity": -0.000001362348332205964,
-            "velocityX": -3.6010087631434655,
-            "velocityY": 1.1590398164104305,
-            "timestamp": 0.7932996274294045
-        },
-        {
-            "x": 5.0459552426535295,
-            "y": 4.412696400521408,
-            "heading": 2.70712690066141,
-            "angularVelocity": -0.000001367362571417024,
-            "velocityX": -3.468573361096618,
-            "velocityY": 1.509846454614715,
-            "timestamp": 0.8924620808580801
-        },
-        {
-            "x": 4.713262422237293,
-            "y": 4.586001722574957,
-            "heading": 2.70712676137173,
-            "angularVelocity": -0.0000014046614974130796,
-            "velocityX": -3.355028127208762,
-            "velocityY": 1.747690946132164,
-            "timestamp": 0.9916245342867557
-        },
-        {
-            "x": 4.426168065896197,
-            "y": 4.827447572944867,
-            "heading": 2.7071267614112156,
-            "angularVelocity": 3.9818977543377344e-10,
-            "velocityX": -2.8951921459627346,
-            "velocityY": 2.4348515191142828,
-            "timestamp": 1.0907869877154313
-        },
-        {
-            "x": 4.093475286723252,
-            "y": 5.000753014560729,
-            "heading": 2.707126833350956,
-            "angularVelocity": 7.254735825966592e-7,
-            "velocityX": -3.355027711292369,
-            "velocityY": 1.7476921518537738,
-            "timestamp": 1.1899494411441067
-        },
-        {
-            "x": 3.752079510529913,
-            "y": 5.156214217951438,
-            "heading": 2.707126904178975,
-            "angularVelocity": 7.142624716180523e-7,
-            "velocityX": -3.4427927546074075,
-            "velocityY": 1.5677426083706831,
-            "timestamp": 1.2891118945727822
-        },
-        {
-            "x": 3.4085800456948445,
-            "y": 5.306970245158155,
-            "heading": 2.7071269730653125,
-            "angularVelocity": 6.946816574949046e-7,
-            "velocityX": -3.4640073229141812,
-            "velocityY": 1.5202934376281008,
-            "timestamp": 1.3882743480014577
-        },
-        {
-            "x": 3.041842891660795,
-            "y": 5.385857046749469,
-            "heading": 2.7071269977260264,
-            "angularVelocity": 2.486900331372695e-7,
-            "velocityX": -3.698346918149141,
-            "velocityY": 0.7955309581771803,
-            "timestamp": 1.4874368014301331
-        },
-        {
-            "x": 2.7246821450713132,
-            "y": 5.456194623849801,
-            "heading": 2.863679707498997,
-            "angularVelocity": 1.5787498630777044,
-            "velocityX": -3.1983955178923207,
-            "velocityY": 0.7093166281018212,
-            "timestamp": 1.5865992548588086
-        },
-        {
-            "x": 2.4872060579091175,
-            "y": 5.508276609290408,
-            "heading": 2.997740485854431,
-            "angularVelocity": 1.3519308339003542,
-            "velocityX": -2.394818592634002,
-            "velocityY": 0.5252188065119572,
-            "timestamp": 1.685761708287484
-        },
-        {
-            "x": 2.3290362313275734,
-            "y": 5.542798533700601,
-            "heading": 3.0916641126613666,
-            "angularVelocity": 0.9471692516612843,
-            "velocityX": -1.5950576161904975,
-            "velocityY": 0.34813503716932903,
-            "timestamp": 1.7849241617161595
+            "x": 7.599512207946833,
+            "y": 4.126338330595212,
+            "heading": -0.05672955044148203,
+            "angularVelocity": -0.5720722032058021,
+            "velocityX": -0.8116550928518662,
+            "velocityY": -0.0066724074357217035,
+            "timestamp": 0.09916501819801522
+        },
+        {
+            "x": 7.438184902121127,
+            "y": 4.125091465076226,
+            "heading": -0.16291927611759752,
+            "angularVelocity": -1.070838563898341,
+            "velocityX": -1.6268570183042166,
+            "velocityY": -0.01257364281924666,
+            "timestamp": 0.19833003639603045
+        },
+        {
+            "x": 7.19545269041534,
+            "y": 4.123364615129754,
+            "heading": -0.30423484322627714,
+            "angularVelocity": -1.4250546178138856,
+            "velocityX": -2.4477604715515007,
+            "velocityY": -0.017413902380612675,
+            "timestamp": 0.2974950545940457
+        },
+        {
+            "x": 6.870785634071806,
+            "y": 4.1216134281783505,
+            "heading": -0.4343749203987102,
+            "angularVelocity": -1.3123587282822462,
+            "velocityX": -3.274007933878773,
+            "velocityY": -0.01765932163604032,
+            "timestamp": 0.3966600727920609
+        },
+        {
+            "x": 6.495672194218458,
+            "y": 4.117557524267507,
+            "heading": -0.4343749552288927,
+            "angularVelocity": -3.5123456983963544e-7,
+            "velocityX": -3.782719417287976,
+            "velocityY": -0.040900551268437134,
+            "timestamp": 0.4958250909900761
+        },
+        {
+            "x": 6.120558752684864,
+            "y": 4.113501775750749,
+            "heading": -0.43437499005887026,
+            "angularVelocity": -3.512325030859258e-7,
+            "velocityX": -3.782719434231919,
+            "velocityY": -0.04089898424321494,
+            "timestamp": 0.5949901091880914
+        },
+        {
+            "x": 5.747019040696225,
+            "y": 4.148065138700183,
+            "heading": -0.43437502656757854,
+            "angularVelocity": -3.681611615448704e-7,
+            "velocityX": -3.766849628794958,
+            "velocityY": 0.3485439076955189,
+            "timestamp": 0.6941551273861066
+        },
+        {
+            "x": 5.389911454629062,
+            "y": 4.262959433025878,
+            "heading": -0.434375162279621,
+            "angularVelocity": -0.0000013685475476133038,
+            "velocityX": -3.601144764115114,
+            "velocityY": 1.1586171859140066,
+            "timestamp": 0.7933201455841219
+        },
+        {
+            "x": 5.045913588511523,
+            "y": 4.412598985545015,
+            "heading": -0.4343752985502662,
+            "angularVelocity": -0.0000013741806101348962,
+            "velocityX": -3.468943709873945,
+            "velocityY": 1.5089953618556682,
+            "timestamp": 0.8924851637821372
+        },
+        {
+            "x": 4.713243052034498,
+            "y": 4.585968073714987,
+            "heading": -0.43437543856751193,
+            "angularVelocity": -0.0000014119620841874337,
+            "velocityX": -3.354716638207433,
+            "velocityY": 1.7482887748155678,
+            "timestamp": 0.9916501819801524
+        },
+        {
+            "x": 4.426185676495707,
+            "y": 4.82747296096862,
+            "heading": -0.4343754384530987,
+            "angularVelocity": 1.153766536466057e-9,
+            "velocityX": -2.894744343873237,
+            "velocityY": 2.4353838847827403,
+            "timestamp": 1.0908152001781677
+        },
+        {
+            "x": 4.09351517988242,
+            "y": 5.000842166204109,
+            "heading": -0.4343753660253311,
+            "angularVelocity": 7.303761838407394e-7,
+            "velocityX": -3.354716236213479,
+            "velocityY": 1.748289955327798,
+            "timestamp": 1.189980218376183
+        },
+        {
+            "x": 3.752063829141965,
+            "y": 5.156204697333757,
+            "heading": -0.43437529472941433,
+            "angularVelocity": 7.189623727917814e-7,
+            "velocityX": -3.4432641363372296,
+            "velocityY": 1.5667070298864525,
+            "timestamp": 1.2891452365741982
+        },
+        {
+            "x": 3.4085469650969755,
+            "y": 5.306945220863998,
+            "heading": -0.43437522535251666,
+            "angularVelocity": 6.996105981774619e-7,
+            "velocityX": -3.46409318817495,
+            "velocityY": 1.5200977750968405,
+            "timestamp": 1.3883102547722135
+        },
+        {
+            "x": 3.0417944650952866,
+            "y": 5.3858068126004595,
+            "heading": -0.43437520041032157,
+            "angularVelocity": 2.5152211488411685e-7,
+            "velocityX": -3.6984060172242224,
+            "velocityY": 0.7952561615930772,
+            "timestamp": 1.4874752729702287
+        },
+        {
+            "x": 2.7246409461958954,
+            "y": 5.456187357907523,
+            "heading": -0.2777696543737322,
+            "angularVelocity": 1.5792418423589194,
+            "velocityX": -3.1982399102281316,
+            "velocityY": 0.7097315826285274,
+            "timestamp": 1.586640291168244
+        },
+        {
+            "x": 2.4871805723397817,
+            "y": 5.508279050924135,
+            "heading": -0.14311381126711797,
+            "angularVelocity": 1.3578966207390795,
+            "velocityX": -2.394598197742947,
+            "velocityY": 0.5253031155865293,
+            "timestamp": 1.6858053093662593
+        },
+        {
+            "x": 2.3290265259060328,
+            "y": 5.5428007138640085,
+            "heading": -0.04864902301750531,
+            "angularVelocity": 0.9526019353012469,
+            "velocityX": -1.594857231992267,
+            "velocityY": 0.3481233964071807,
+            "timestamp": 1.7849703275642745
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0.4874414223060736,
-            "velocityX": -0.7970378766840579,
-            "velocityY": 0.17346753438054688,
-            "timestamp": 1.884086615144835
+            "heading": -2.5681051863102055e-32,
+            "angularVelocity": 0.490586538494469,
+            "velocityX": -0.7969193909512572,
+            "velocityY": 0.1734410626703791,
+            "timestamp": 1.8841353457622898
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -6.536266088535149e-32,
+            "heading": 0,
+            "angularVelocity": 1.14717044306562e-31,
             "velocityX": 0,
-            "velocityY": 2.2958668874154396e-32,
-            "timestamp": 1.9832490685735105
+            "velocityY": 5.846493116157662e-32,
+            "timestamp": 1.983300363960305
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.traj
index 8079765c..2f31d71f 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.traj	
@@ -3,191 +3,191 @@
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 8.742415967937999e-33,
-            "velocityX": -1.6780440317133427e-34,
-            "velocityY": 4.640205301974453e-35,
+            "heading": 0,
+            "angularVelocity": -8.248091288877123e-33,
+            "velocityX": -1.7499589770527321e-34,
+            "velocityY": 1.243057151683689e-35,
             "timestamp": 0
         },
         {
-            "x": 7.599500768056594,
-            "y": 4.12633480373168,
-            "heading": 3.0835764078705523,
-            "angularVelocity": -0.5690015744722524,
-            "velocityX": -0.8117914508973499,
-            "velocityY": -0.006708146534495369,
-            "timestamp": 0.09916245342867555
-        },
-        {
-            "x": 7.438156123912552,
-            "y": 4.125079622260014,
-            "heading": 2.9779167441714383,
-            "angularVelocity": -1.065520870508837,
-            "velocityX": -1.6270739434668355,
-            "velocityY": -0.012657829937315014,
-            "timestamp": 0.1983249068573511
-        },
-        {
-            "x": 7.195408805362699,
-            "y": 4.123338544846998,
-            "heading": 2.8371936481283773,
-            "angularVelocity": -1.4191167238946776,
-            "velocityX": -2.4479761256053836,
-            "velocityY": -0.017557829125994403,
-            "timestamp": 0.2974873602860266
-        },
-        {
-            "x": 6.870725465009593,
-            "y": 4.121560782802172,
-            "heading": 2.707127276825701,
-            "angularVelocity": -1.311649387499567,
-            "velocityX": -3.2742568293415686,
-            "velocityY": -0.017927773903902572,
-            "timestamp": 0.3966498137147022
-        },
-        {
-            "x": 6.495621806376681,
-            "y": 4.117497642893674,
-            "heading": 2.7071272422206913,
-            "angularVelocity": -3.489729050319063e-7,
-            "velocityX": -3.7827186164036664,
-            "velocityY": -0.04097458027721316,
-            "timestamp": 0.4958122671433778
-        },
-        {
-            "x": 6.120518146053048,
-            "y": 4.113434659065473,
-            "heading": 2.7071272076158857,
-            "angularVelocity": -3.4897084992850824e-7,
-            "velocityX": -3.7827186334536713,
-            "velocityY": -0.04097300629137556,
-            "timestamp": 0.5949747205720534
-        },
-        {
-            "x": 5.746992350808683,
-            "y": 4.148043089964446,
-            "heading": 2.7071271713462406,
-            "angularVelocity": -3.657598578804205e-7,
-            "velocityX": -3.76680671291609,
-            "velocityY": 0.3490074085739077,
-            "timestamp": 0.6941371740007289
-        },
-        {
-            "x": 5.389907487037217,
-            "y": 4.262976321781226,
-            "heading": 2.7071270362524373,
-            "angularVelocity": -0.000001362348332205964,
-            "velocityX": -3.6010087631434655,
-            "velocityY": 1.1590398164104305,
-            "timestamp": 0.7932996274294045
-        },
-        {
-            "x": 5.0459552426535295,
-            "y": 4.412696400521408,
-            "heading": 2.70712690066141,
-            "angularVelocity": -0.000001367362571417024,
-            "velocityX": -3.468573361096618,
-            "velocityY": 1.509846454614715,
-            "timestamp": 0.8924620808580801
-        },
-        {
-            "x": 4.713262422237293,
-            "y": 4.586001722574957,
-            "heading": 2.70712676137173,
-            "angularVelocity": -0.0000014046614974130796,
-            "velocityX": -3.355028127208762,
-            "velocityY": 1.747690946132164,
-            "timestamp": 0.9916245342867557
-        },
-        {
-            "x": 4.426168065896197,
-            "y": 4.827447572944867,
-            "heading": 2.7071267614112156,
-            "angularVelocity": 3.9818977543377344e-10,
-            "velocityX": -2.8951921459627346,
-            "velocityY": 2.4348515191142828,
-            "timestamp": 1.0907869877154313
-        },
-        {
-            "x": 4.093475286723252,
-            "y": 5.000753014560729,
-            "heading": 2.707126833350956,
-            "angularVelocity": 7.254735825966592e-7,
-            "velocityX": -3.355027711292369,
-            "velocityY": 1.7476921518537738,
-            "timestamp": 1.1899494411441067
-        },
-        {
-            "x": 3.752079510529913,
-            "y": 5.156214217951438,
-            "heading": 2.707126904178975,
-            "angularVelocity": 7.142624716180523e-7,
-            "velocityX": -3.4427927546074075,
-            "velocityY": 1.5677426083706831,
-            "timestamp": 1.2891118945727822
-        },
-        {
-            "x": 3.4085800456948445,
-            "y": 5.306970245158155,
-            "heading": 2.7071269730653125,
-            "angularVelocity": 6.946816574949046e-7,
-            "velocityX": -3.4640073229141812,
-            "velocityY": 1.5202934376281008,
-            "timestamp": 1.3882743480014577
-        },
-        {
-            "x": 3.041842891660795,
-            "y": 5.385857046749469,
-            "heading": 2.7071269977260264,
-            "angularVelocity": 2.486900331372695e-7,
-            "velocityX": -3.698346918149141,
-            "velocityY": 0.7955309581771803,
-            "timestamp": 1.4874368014301331
-        },
-        {
-            "x": 2.7246821450713132,
-            "y": 5.456194623849801,
-            "heading": 2.863679707498997,
-            "angularVelocity": 1.5787498630777044,
-            "velocityX": -3.1983955178923207,
-            "velocityY": 0.7093166281018212,
-            "timestamp": 1.5865992548588086
-        },
-        {
-            "x": 2.4872060579091175,
-            "y": 5.508276609290408,
-            "heading": 2.997740485854431,
-            "angularVelocity": 1.3519308339003542,
-            "velocityX": -2.394818592634002,
-            "velocityY": 0.5252188065119572,
-            "timestamp": 1.685761708287484
-        },
-        {
-            "x": 2.3290362313275734,
-            "y": 5.542798533700601,
-            "heading": 3.0916641126613666,
-            "angularVelocity": 0.9471692516612843,
-            "velocityX": -1.5950576161904975,
-            "velocityY": 0.34813503716932903,
-            "timestamp": 1.7849241617161595
+            "x": 7.599512207946833,
+            "y": 4.126338330595212,
+            "heading": -0.05672955044148203,
+            "angularVelocity": -0.5720722032058021,
+            "velocityX": -0.8116550928518662,
+            "velocityY": -0.0066724074357217035,
+            "timestamp": 0.09916501819801522
+        },
+        {
+            "x": 7.438184902121127,
+            "y": 4.125091465076226,
+            "heading": -0.16291927611759752,
+            "angularVelocity": -1.070838563898341,
+            "velocityX": -1.6268570183042166,
+            "velocityY": -0.01257364281924666,
+            "timestamp": 0.19833003639603045
+        },
+        {
+            "x": 7.19545269041534,
+            "y": 4.123364615129754,
+            "heading": -0.30423484322627714,
+            "angularVelocity": -1.4250546178138856,
+            "velocityX": -2.4477604715515007,
+            "velocityY": -0.017413902380612675,
+            "timestamp": 0.2974950545940457
+        },
+        {
+            "x": 6.870785634071806,
+            "y": 4.1216134281783505,
+            "heading": -0.4343749203987102,
+            "angularVelocity": -1.3123587282822462,
+            "velocityX": -3.274007933878773,
+            "velocityY": -0.01765932163604032,
+            "timestamp": 0.3966600727920609
+        },
+        {
+            "x": 6.495672194218458,
+            "y": 4.117557524267507,
+            "heading": -0.4343749552288927,
+            "angularVelocity": -3.5123456983963544e-7,
+            "velocityX": -3.782719417287976,
+            "velocityY": -0.040900551268437134,
+            "timestamp": 0.4958250909900761
+        },
+        {
+            "x": 6.120558752684864,
+            "y": 4.113501775750749,
+            "heading": -0.43437499005887026,
+            "angularVelocity": -3.512325030859258e-7,
+            "velocityX": -3.782719434231919,
+            "velocityY": -0.04089898424321494,
+            "timestamp": 0.5949901091880914
+        },
+        {
+            "x": 5.747019040696225,
+            "y": 4.148065138700183,
+            "heading": -0.43437502656757854,
+            "angularVelocity": -3.681611615448704e-7,
+            "velocityX": -3.766849628794958,
+            "velocityY": 0.3485439076955189,
+            "timestamp": 0.6941551273861066
+        },
+        {
+            "x": 5.389911454629062,
+            "y": 4.262959433025878,
+            "heading": -0.434375162279621,
+            "angularVelocity": -0.0000013685475476133038,
+            "velocityX": -3.601144764115114,
+            "velocityY": 1.1586171859140066,
+            "timestamp": 0.7933201455841219
+        },
+        {
+            "x": 5.045913588511523,
+            "y": 4.412598985545015,
+            "heading": -0.4343752985502662,
+            "angularVelocity": -0.0000013741806101348962,
+            "velocityX": -3.468943709873945,
+            "velocityY": 1.5089953618556682,
+            "timestamp": 0.8924851637821372
+        },
+        {
+            "x": 4.713243052034498,
+            "y": 4.585968073714987,
+            "heading": -0.43437543856751193,
+            "angularVelocity": -0.0000014119620841874337,
+            "velocityX": -3.354716638207433,
+            "velocityY": 1.7482887748155678,
+            "timestamp": 0.9916501819801524
+        },
+        {
+            "x": 4.426185676495707,
+            "y": 4.82747296096862,
+            "heading": -0.4343754384530987,
+            "angularVelocity": 1.153766536466057e-9,
+            "velocityX": -2.894744343873237,
+            "velocityY": 2.4353838847827403,
+            "timestamp": 1.0908152001781677
+        },
+        {
+            "x": 4.09351517988242,
+            "y": 5.000842166204109,
+            "heading": -0.4343753660253311,
+            "angularVelocity": 7.303761838407394e-7,
+            "velocityX": -3.354716236213479,
+            "velocityY": 1.748289955327798,
+            "timestamp": 1.189980218376183
+        },
+        {
+            "x": 3.752063829141965,
+            "y": 5.156204697333757,
+            "heading": -0.43437529472941433,
+            "angularVelocity": 7.189623727917814e-7,
+            "velocityX": -3.4432641363372296,
+            "velocityY": 1.5667070298864525,
+            "timestamp": 1.2891452365741982
+        },
+        {
+            "x": 3.4085469650969755,
+            "y": 5.306945220863998,
+            "heading": -0.43437522535251666,
+            "angularVelocity": 6.996105981774619e-7,
+            "velocityX": -3.46409318817495,
+            "velocityY": 1.5200977750968405,
+            "timestamp": 1.3883102547722135
+        },
+        {
+            "x": 3.0417944650952866,
+            "y": 5.3858068126004595,
+            "heading": -0.43437520041032157,
+            "angularVelocity": 2.5152211488411685e-7,
+            "velocityX": -3.6984060172242224,
+            "velocityY": 0.7952561615930772,
+            "timestamp": 1.4874752729702287
+        },
+        {
+            "x": 2.7246409461958954,
+            "y": 5.456187357907523,
+            "heading": -0.2777696543737322,
+            "angularVelocity": 1.5792418423589194,
+            "velocityX": -3.1982399102281316,
+            "velocityY": 0.7097315826285274,
+            "timestamp": 1.586640291168244
+        },
+        {
+            "x": 2.4871805723397817,
+            "y": 5.508279050924135,
+            "heading": -0.14311381126711797,
+            "angularVelocity": 1.3578966207390795,
+            "velocityX": -2.394598197742947,
+            "velocityY": 0.5253031155865293,
+            "timestamp": 1.6858053093662593
+        },
+        {
+            "x": 2.3290265259060328,
+            "y": 5.5428007138640085,
+            "heading": -0.04864902301750531,
+            "angularVelocity": 0.9526019353012469,
+            "velocityX": -1.594857231992267,
+            "velocityY": 0.3481233964071807,
+            "timestamp": 1.7849703275642745
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0.4874414223060736,
-            "velocityX": -0.7970378766840579,
-            "velocityY": 0.17346753438054688,
-            "timestamp": 1.884086615144835
+            "heading": -2.5681051863102055e-32,
+            "angularVelocity": 0.490586538494469,
+            "velocityX": -0.7969193909512572,
+            "velocityY": 0.1734410626703791,
+            "timestamp": 1.8841353457622898
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -6.536266088535149e-32,
+            "heading": 0,
+            "angularVelocity": 1.14717044306562e-31,
             "velocityX": 0,
-            "velocityY": 2.2958668874154396e-32,
-            "timestamp": 1.9832490685735105
+            "velocityY": 5.846493116157662e-32,
+            "timestamp": 1.983300363960305
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W3.1.traj
new file mode 100644
index 00000000..3874f8b2
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W3.1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 5.58164160972529e-32,
+            "velocityX": 1.1273756142039836e-32,
+            "velocityY": 2.4507602066878393e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.588395006557631,
+            "y": 4.128528104169273,
+            "heading": -0.06094869084906696,
+            "angularVelocity": -0.5766457987157873,
+            "velocityX": -0.8666902254019087,
+            "velocityY": 0.01445765014696692,
+            "timestamp": 0.10569519622756653
+        },
+        {
+            "x": 7.4046836501246664,
+            "y": 4.131675342578391,
+            "heading": -0.17161815397097052,
+            "angularVelocity": -1.0470623743734504,
+            "velocityX": -1.7381239922903071,
+            "velocityY": 0.029776551077511915,
+            "timestamp": 0.21139039245513305
+        },
+        {
+            "x": 7.128148507793846,
+            "y": 4.136585421276787,
+            "heading": -0.30937735862178317,
+            "angularVelocity": -1.3033629679272352,
+            "velocityX": -2.6163454177749763,
+            "velocityY": 0.0464550790730766,
+            "timestamp": 0.3170855886826996
+        },
+        {
+            "x": 6.759745872373483,
+            "y": 4.143898882898015,
+            "heading": -0.3925495634756188,
+            "angularVelocity": -0.7869061965196801,
+            "velocityX": -3.485519196418107,
+            "velocityY": 0.06919388848555069,
+            "timestamp": 0.4227807849102661
+        },
+        {
+            "x": 6.3599483672427946,
+            "y": 4.149634642956697,
+            "heading": -0.39254959749689916,
+            "angularVelocity": -3.2188104690533836e-7,
+            "velocityX": -3.782551330619671,
+            "velocityY": 0.05426698907234567,
+            "timestamp": 0.5284759811378327
+        },
+        {
+            "x": 5.960150865134662,
+            "y": 4.155370613693523,
+            "heading": -0.39254963151811834,
+            "angularVelocity": -3.2188046698591794e-7,
+            "velocityX": -3.7825513020227493,
+            "velocityY": 0.05426898233365955,
+            "timestamp": 0.6341711773653992
+        },
+        {
+            "x": 5.56430885680334,
+            "y": 4.211762521698523,
+            "heading": -0.3925496691964927,
+            "angularVelocity": -3.5648142721372326e-7,
+            "velocityX": -3.745127711188092,
+            "velocityY": 0.5335333110464675,
+            "timestamp": 0.7398663735929657
+        },
+        {
+            "x": 5.196451370697568,
+            "y": 4.356905683947299,
+            "heading": -0.407192768864793,
+            "angularVelocity": -0.13854082485237101,
+            "velocityX": -3.4803614472105,
+            "velocityY": 1.3732238306864724,
+            "timestamp": 0.8455615698205322
+        },
+        {
+            "x": 4.853119399386178,
+            "y": 4.529141900436451,
+            "heading": -0.46036689242459117,
+            "angularVelocity": -0.5030893120753747,
+            "velocityX": -3.2483214333807724,
+            "velocityY": 1.6295557663597022,
+            "timestamp": 0.9512567660480987
+        },
+        {
+            "x": 4.514017884763102,
+            "y": 4.740993233583884,
+            "heading": -0.4603674462778229,
+            "angularVelocity": -0.000005240098429403888,
+            "velocityX": -3.208296372268186,
+            "velocityY": 2.0043610372916896,
+            "timestamp": 1.0569519622756653
+        },
+        {
+            "x": 4.21750261375463,
+            "y": 5.009227519029098,
+            "heading": -0.46036738913342323,
+            "angularVelocity": 5.406527608893175e-7,
+            "velocityX": -2.8053807702864813,
+            "velocityY": 2.537809616888297,
+            "timestamp": 1.1626471585032319
+        },
+        {
+            "x": 3.866969646487487,
+            "y": 5.201574981603133,
+            "heading": -0.46036721071098435,
+            "angularVelocity": 0.0000016880846552508654,
+            "velocityX": -3.3164512653198517,
+            "velocityY": 1.8198316426784653,
+            "timestamp": 1.2683423547307984
+        },
+        {
+            "x": 3.5045677865422378,
+            "y": 5.303672838764994,
+            "heading": -0.3887465759877037,
+            "angularVelocity": 0.6776148517580518,
+            "velocityX": -3.4287448519891264,
+            "velocityY": 0.9659649710289621,
+            "timestamp": 1.3740375509583649
+        },
+        {
+            "x": 3.16557460185915,
+            "y": 5.315860378892371,
+            "heading": -0.29320626763004903,
+            "angularVelocity": 0.9039229006392311,
+            "velocityX": -3.207271444515043,
+            "velocityY": 0.1153083636945626,
+            "timestamp": 1.4797327471859314
+        },
+        {
+            "x": 2.8681536233597122,
+            "y": 5.2454821587095966,
+            "heading": -0.23849853505250618,
+            "angularVelocity": 0.5175990445181124,
+            "velocityX": -2.8139498209462337,
+            "velocityY": -0.6658601591622668,
+            "timestamp": 1.585427943413498
+        },
+        {
+            "x": 2.618332286986499,
+            "y": 5.0970676603891505,
+            "heading": -0.24744806192055374,
+            "angularVelocity": -0.08467297651615932,
+            "velocityX": -2.3636016137889215,
+            "velocityY": -1.4041744905880433,
+            "timestamp": 1.6911231396410644
+        },
+        {
+            "x": 2.4325478975720265,
+            "y": 4.8813111498736355,
+            "heading": -0.27927376577641677,
+            "angularVelocity": -0.3011083283987745,
+            "velocityX": -1.7577373054350616,
+            "velocityY": -2.0413085761341616,
+            "timestamp": 1.796818335868631
+        },
+        {
+            "x": 2.3348070320385017,
+            "y": 4.635496280912222,
+            "heading": -0.32770640318337707,
+            "angularVelocity": -0.4582293153861289,
+            "velocityX": -0.9247427416008974,
+            "velocityY": -2.3256957528340565,
+            "timestamp": 1.9025135320961974
+        },
+        {
+            "x": 2.305548232287739,
+            "y": 4.452786448669473,
+            "heading": -0.3683698320727586,
+            "angularVelocity": -0.3847235289845278,
+            "velocityX": -0.2768224176221525,
+            "velocityY": -1.72864840374928,
+            "timestamp": 2.008208728323764
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.2046466509288718,
+            "velocityX": -0.05249275734153,
+            "velocityY": -0.8778681716972112,
+            "timestamp": 2.1139039245513307
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 5.518495272475637e-32,
+            "velocityX": 0,
+            "velocityY": 3.7393231194786592e-34,
+            "timestamp": 2.219599120778897
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W3.traj
new file mode 100644
index 00000000..3874f8b2
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W3.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 5.58164160972529e-32,
+            "velocityX": 1.1273756142039836e-32,
+            "velocityY": 2.4507602066878393e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.588395006557631,
+            "y": 4.128528104169273,
+            "heading": -0.06094869084906696,
+            "angularVelocity": -0.5766457987157873,
+            "velocityX": -0.8666902254019087,
+            "velocityY": 0.01445765014696692,
+            "timestamp": 0.10569519622756653
+        },
+        {
+            "x": 7.4046836501246664,
+            "y": 4.131675342578391,
+            "heading": -0.17161815397097052,
+            "angularVelocity": -1.0470623743734504,
+            "velocityX": -1.7381239922903071,
+            "velocityY": 0.029776551077511915,
+            "timestamp": 0.21139039245513305
+        },
+        {
+            "x": 7.128148507793846,
+            "y": 4.136585421276787,
+            "heading": -0.30937735862178317,
+            "angularVelocity": -1.3033629679272352,
+            "velocityX": -2.6163454177749763,
+            "velocityY": 0.0464550790730766,
+            "timestamp": 0.3170855886826996
+        },
+        {
+            "x": 6.759745872373483,
+            "y": 4.143898882898015,
+            "heading": -0.3925495634756188,
+            "angularVelocity": -0.7869061965196801,
+            "velocityX": -3.485519196418107,
+            "velocityY": 0.06919388848555069,
+            "timestamp": 0.4227807849102661
+        },
+        {
+            "x": 6.3599483672427946,
+            "y": 4.149634642956697,
+            "heading": -0.39254959749689916,
+            "angularVelocity": -3.2188104690533836e-7,
+            "velocityX": -3.782551330619671,
+            "velocityY": 0.05426698907234567,
+            "timestamp": 0.5284759811378327
+        },
+        {
+            "x": 5.960150865134662,
+            "y": 4.155370613693523,
+            "heading": -0.39254963151811834,
+            "angularVelocity": -3.2188046698591794e-7,
+            "velocityX": -3.7825513020227493,
+            "velocityY": 0.05426898233365955,
+            "timestamp": 0.6341711773653992
+        },
+        {
+            "x": 5.56430885680334,
+            "y": 4.211762521698523,
+            "heading": -0.3925496691964927,
+            "angularVelocity": -3.5648142721372326e-7,
+            "velocityX": -3.745127711188092,
+            "velocityY": 0.5335333110464675,
+            "timestamp": 0.7398663735929657
+        },
+        {
+            "x": 5.196451370697568,
+            "y": 4.356905683947299,
+            "heading": -0.407192768864793,
+            "angularVelocity": -0.13854082485237101,
+            "velocityX": -3.4803614472105,
+            "velocityY": 1.3732238306864724,
+            "timestamp": 0.8455615698205322
+        },
+        {
+            "x": 4.853119399386178,
+            "y": 4.529141900436451,
+            "heading": -0.46036689242459117,
+            "angularVelocity": -0.5030893120753747,
+            "velocityX": -3.2483214333807724,
+            "velocityY": 1.6295557663597022,
+            "timestamp": 0.9512567660480987
+        },
+        {
+            "x": 4.514017884763102,
+            "y": 4.740993233583884,
+            "heading": -0.4603674462778229,
+            "angularVelocity": -0.000005240098429403888,
+            "velocityX": -3.208296372268186,
+            "velocityY": 2.0043610372916896,
+            "timestamp": 1.0569519622756653
+        },
+        {
+            "x": 4.21750261375463,
+            "y": 5.009227519029098,
+            "heading": -0.46036738913342323,
+            "angularVelocity": 5.406527608893175e-7,
+            "velocityX": -2.8053807702864813,
+            "velocityY": 2.537809616888297,
+            "timestamp": 1.1626471585032319
+        },
+        {
+            "x": 3.866969646487487,
+            "y": 5.201574981603133,
+            "heading": -0.46036721071098435,
+            "angularVelocity": 0.0000016880846552508654,
+            "velocityX": -3.3164512653198517,
+            "velocityY": 1.8198316426784653,
+            "timestamp": 1.2683423547307984
+        },
+        {
+            "x": 3.5045677865422378,
+            "y": 5.303672838764994,
+            "heading": -0.3887465759877037,
+            "angularVelocity": 0.6776148517580518,
+            "velocityX": -3.4287448519891264,
+            "velocityY": 0.9659649710289621,
+            "timestamp": 1.3740375509583649
+        },
+        {
+            "x": 3.16557460185915,
+            "y": 5.315860378892371,
+            "heading": -0.29320626763004903,
+            "angularVelocity": 0.9039229006392311,
+            "velocityX": -3.207271444515043,
+            "velocityY": 0.1153083636945626,
+            "timestamp": 1.4797327471859314
+        },
+        {
+            "x": 2.8681536233597122,
+            "y": 5.2454821587095966,
+            "heading": -0.23849853505250618,
+            "angularVelocity": 0.5175990445181124,
+            "velocityX": -2.8139498209462337,
+            "velocityY": -0.6658601591622668,
+            "timestamp": 1.585427943413498
+        },
+        {
+            "x": 2.618332286986499,
+            "y": 5.0970676603891505,
+            "heading": -0.24744806192055374,
+            "angularVelocity": -0.08467297651615932,
+            "velocityX": -2.3636016137889215,
+            "velocityY": -1.4041744905880433,
+            "timestamp": 1.6911231396410644
+        },
+        {
+            "x": 2.4325478975720265,
+            "y": 4.8813111498736355,
+            "heading": -0.27927376577641677,
+            "angularVelocity": -0.3011083283987745,
+            "velocityX": -1.7577373054350616,
+            "velocityY": -2.0413085761341616,
+            "timestamp": 1.796818335868631
+        },
+        {
+            "x": 2.3348070320385017,
+            "y": 4.635496280912222,
+            "heading": -0.32770640318337707,
+            "angularVelocity": -0.4582293153861289,
+            "velocityX": -0.9247427416008974,
+            "velocityY": -2.3256957528340565,
+            "timestamp": 1.9025135320961974
+        },
+        {
+            "x": 2.305548232287739,
+            "y": 4.452786448669473,
+            "heading": -0.3683698320727586,
+            "angularVelocity": -0.3847235289845278,
+            "velocityX": -0.2768224176221525,
+            "velocityY": -1.72864840374928,
+            "timestamp": 2.008208728323764
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.2046466509288718,
+            "velocityX": -0.05249275734153,
+            "velocityY": -0.8778681716972112,
+            "timestamp": 2.1139039245513307
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 5.518495272475637e-32,
+            "velocityX": 0,
+            "velocityY": 3.7393231194786592e-34,
+            "timestamp": 2.219599120778897
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.1.traj
index 5b28f5d1..f850aede 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.1.traj	
@@ -3,236 +3,236 @@
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 1.0740254772261542e-28,
-            "velocityX": 2.0063238328605694e-29,
-            "velocityY": 1.599232652510583e-28,
+            "heading": 0,
+            "angularVelocity": 2.322866633708497e-28,
+            "velocityX": -2.1618763976557105e-31,
+            "velocityY": -1.247476705840714e-31,
             "timestamp": 0
         },
         {
-            "x": 7.635665892651778,
-            "y": 2.515360941496967,
-            "heading": 3.1393295631597837,
-            "angularVelocity": -0.0071510111194275595,
-            "velocityX": -0.47287630332947994,
-            "velocityY": 0.6224892734875569,
-            "timestamp": 0.09375413197081223
-        },
-        {
-            "x": 7.546997676041258,
-            "y": 2.6320828188122176,
-            "heading": 3.137988695350318,
-            "angularVelocity": -0.014301959617960866,
-            "velocityX": -0.9457526270748597,
-            "velocityY": 1.2449784864051567,
-            "timestamp": 0.18750826394162445
-        },
-        {
-            "x": 7.413995349999322,
-            "y": 2.8071656233620255,
-            "heading": 3.1359774017076236,
-            "angularVelocity": -0.02145285333469997,
-            "velocityX": -1.4186289526241012,
-            "velocityY": 1.8674676077670485,
-            "timestamp": 0.2812623959124367
-        },
-        {
-            "x": 7.236658921990214,
-            "y": 3.0406093354422112,
-            "heading": 3.133295687975677,
-            "angularVelocity": -0.028603685785057682,
-            "velocityX": -1.8915051985582343,
-            "velocityY": 2.489956518960288,
-            "timestamp": 0.3750165278832489
-        },
-        {
-            "x": 7.022133976710729,
-            "y": 3.323040275054587,
-            "heading": 3.133295687627681,
-            "angularVelocity": -3.7117965182714086e-9,
-            "velocityX": -2.288165233573672,
-            "velocityY": 3.0124639167935428,
-            "timestamp": 0.46877065985406113
-        },
-        {
-            "x": 6.786563329839658,
-            "y": 3.588171672312515,
-            "heading": 3.133295687271953,
-            "angularVelocity": -3.794263922237942e-9,
-            "velocityX": -2.512642823512138,
-            "velocityY": 2.8279435976269225,
-            "timestamp": 0.5625247918248734
-        },
-        {
-            "x": 6.501527367007155,
-            "y": 3.7992230523695487,
-            "heading": 3.133295686856248,
-            "angularVelocity": -4.433993924393781e-9,
-            "velocityX": -3.040249606505238,
-            "velocityY": 2.251115504143757,
-            "timestamp": 0.6562789237956856
-        },
-        {
-            "x": 6.179197632360147,
-            "y": 3.947181584790984,
-            "heading": 3.133295705650933,
-            "angularVelocity": 2.004678114437141e-7,
-            "velocityX": -3.438032307177196,
-            "velocityY": 1.5781547896737034,
-            "timestamp": 0.7500330557664978
-        },
-        {
-            "x": 5.833338645417163,
-            "y": 4.025728972736192,
-            "heading": 3.133296009616361,
-            "angularVelocity": 0.000003242155002257518,
-            "velocityX": -3.6889999370977806,
-            "velocityY": 0.837801879171189,
-            "timestamp": 0.84378718773731
-        },
-        {
-            "x": 5.481107394947439,
-            "y": 4.067217134926503,
-            "heading": 3.1332963782586343,
-            "angularVelocity": 0.000003932010945944395,
-            "velocityX": -3.7569677524120277,
-            "velocityY": 0.4425208928736066,
-            "timestamp": 0.9375413197081222
-        },
-        {
-            "x": 5.149070300488837,
-            "y": 4.179082889717707,
-            "heading": 3.1449832895383,
-            "angularVelocity": 0.12465489289866781,
-            "velocityX": -3.541572915015461,
-            "velocityY": 1.1931821290397846,
-            "timestamp": 1.0312954516789343
-        },
-        {
-            "x": 4.859012780941325,
-            "y": 4.350314048380944,
-            "heading": 3.189517171995987,
-            "angularVelocity": 0.4750071439148605,
-            "velocityX": -3.093810517469455,
-            "velocityY": 1.826385195657799,
-            "timestamp": 1.1250495836497465
-        },
-        {
-            "x": 4.618219764951525,
-            "y": 4.5756090572292365,
-            "heading": 3.250644921045485,
-            "angularVelocity": 0.6520005866891553,
-            "velocityX": -2.5683456390462376,
-            "velocityY": 2.4030408485721884,
-            "timestamp": 1.2188037156205587
-        },
-        {
-            "x": 4.427454911100163,
-            "y": 4.854246931015481,
-            "heading": 3.2945713874044515,
-            "angularVelocity": 0.46852832441237513,
-            "velocityX": -2.034735428095452,
-            "velocityY": 2.9720063311233567,
-            "timestamp": 1.3125578475913708
-        },
-        {
-            "x": 4.197108920054306,
-            "y": 5.123929924649606,
-            "heading": 3.2945713966505874,
-            "angularVelocity": 9.862110665096438e-8,
-            "velocityX": -2.4569156175171605,
-            "velocityY": 2.8764918192414846,
-            "timestamp": 1.406311979562183
-        },
-        {
-            "x": 3.9283154974180072,
-            "y": 5.355313346500023,
-            "heading": 3.2945714071744505,
-            "angularVelocity": 1.1224958813512746e-7,
-            "velocityX": -2.8670034801237065,
-            "velocityY": 2.4679810583970268,
-            "timestamp": 1.5000661115329952
-        },
-        {
-            "x": 3.6595153829213625,
-            "y": 5.586688994322917,
-            "heading": 3.2945714176982936,
-            "angularVelocity": 1.1224938145766723e-7,
-            "velocityX": -2.867074856821552,
-            "velocityY": 2.4678981390913592,
-            "timestamp": 1.5938202435038074
-        },
-        {
-            "x": 3.3907152487550194,
-            "y": 5.818064619294552,
-            "heading": 3.2945714282221594,
-            "angularVelocity": 1.1224961995364469e-7,
-            "velocityX": -2.867075066622423,
-            "velocityY": 2.4678978953553523,
-            "timestamp": 1.6875743754746195
-        },
-        {
-            "x": 3.121915101893736,
-            "y": 6.049440229517853,
-            "heading": 3.294571438746038,
-            "angularVelocity": 1.1224975583094217e-7,
-            "velocityX": -2.8670752020291417,
-            "velocityY": 2.467897738046713,
-            "timestamp": 1.7813285074454317
-        },
-        {
-            "x": 2.853115082553889,
-            "y": 6.2808159878868715,
-            "heading": 3.2945714492725595,
-            "angularVelocity": 1.1227794602420482e-7,
-            "velocityX": -2.8670738418604556,
-            "velocityY": 2.467899318198048,
-            "timestamp": 1.8750826394162439
-        },
-        {
-            "x": 2.6318681155961685,
-            "y": 6.471289698509282,
-            "heading": 3.3899753271617903,
-            "angularVelocity": 1.0175965142415218,
-            "velocityX": -2.3598636380805007,
-            "velocityY": 2.031630037188212,
-            "timestamp": 1.968836771387056
-        },
-        {
-            "x": 2.465933868110727,
-            "y": 6.614144871792939,
-            "heading": 3.461571184706241,
-            "angularVelocity": 0.7636554895173272,
-            "velocityX": -1.7698872998696302,
-            "velocityY": 1.5237213579891205,
-            "timestamp": 2.062590903357868
-        },
-        {
-            "x": 2.3553113103707712,
-            "y": 6.709381606049684,
-            "heading": 3.5093149725923682,
-            "angularVelocity": 0.5092446261566541,
-            "velocityX": -1.1799219449271308,
-            "velocityY": 1.0158137274034427,
-            "timestamp": 2.1563450353286804
+            "x": 7.635666877682845,
+            "y": 2.515362381859407,
+            "heading": -0.0008300252680408195,
+            "angularVelocity": -0.00885317162168204,
+            "velocityX": -0.4728636048938783,
+            "velocityY": 0.6225017511467659,
+            "timestamp": 0.09375456655646779
+        },
+        {
+            "x": 7.547000631163484,
+            "y": 2.632087139923421,
+            "heading": -0.0024900693866490363,
+            "angularVelocity": -0.01770627479364842,
+            "velocityX": -0.9457272298940119,
+            "velocityY": 1.2450034419786018,
+            "timestamp": 0.18750913311293557
+        },
+        {
+            "x": 7.4140012603329,
+            "y": 2.8071742656560237,
+            "heading": -0.004980126198969091,
+            "angularVelocity": -0.026559312295687615,
+            "velocityX": -1.418590856056911,
+            "velocityY": 1.867505041764004,
+            "timestamp": 0.28126369966940334
+        },
+        {
+            "x": 7.236668772838196,
+            "y": 3.0406237394955027,
+            "heading": -0.008300187115435396,
+            "angularVelocity": -0.035412258180156565,
+            "velocityX": -1.891454400654691,
+            "velocityY": 2.4900064329013105,
+            "timestamp": 0.37501826622587114
+        },
+        {
+            "x": 7.022136423676778,
+            "y": 3.3230511197393278,
+            "heading": -0.008300187546286333,
+            "angularVelocity": -4.59551947810532e-9,
+            "velocityX": -2.2882335980104607,
+            "velocityY": 3.012411988206685,
+            "timestamp": 0.46877283278233894
+        },
+        {
+            "x": 6.786560921928144,
+            "y": 3.5881804025568163,
+            "heading": -0.008300187986713265,
+            "angularVelocity": -4.6976584720676315e-9,
+            "velocityX": -2.51268295936015,
+            "velocityY": 2.827907936172931,
+            "timestamp": 0.5625273993388067
+        },
+        {
+            "x": 6.5015204114211205,
+            "y": 3.799228403383322,
+            "heading": -0.008300188501414158,
+            "angularVelocity": -5.489875460815635e-9,
+            "velocityX": -3.0402840200359327,
+            "velocityY": 2.251069026055312,
+            "timestamp": 0.6562819658952744
+        },
+        {
+            "x": 6.179186778264032,
+            "y": 3.9471823835437703,
+            "heading": -0.008300169798343478,
+            "angularVelocity": 1.9948970346527163e-7,
+            "velocityX": -3.4380579527606097,
+            "velocityY": 1.5780989192812942,
+            "timestamp": 0.7500365324517422
+        },
+        {
+            "x": 5.833324835964247,
+            "y": 4.025724182042927,
+            "heading": -0.008299866211032016,
+            "angularVelocity": 0.0000032381069276099122,
+            "velocityX": -3.689014359545628,
+            "velocityY": 0.8377383778085268,
+            "timestamp": 0.8437910990082099
+        },
+        {
+            "x": 5.481092764022396,
+            "y": 4.0672194252777,
+            "heading": -0.008299498284594501,
+            "angularVelocity": 0.000003924357511610969,
+            "velocityX": -3.7569590994770814,
+            "velocityY": 0.44259436909433375,
+            "timestamp": 0.9375456655646777
+        },
+        {
+            "x": 5.14904862351455,
+            "y": 4.179095043376677,
+            "heading": 0.0033651913425959347,
+            "angularVelocity": 0.12441729566489874,
+            "velocityX": -3.5416316527670983,
+            "velocityY": 1.1932818017093059,
+            "timestamp": 1.0313002321211455
+        },
+        {
+            "x": 4.858987953002521,
+            "y": 4.350333701259038,
+            "heading": 0.047887100206889445,
+            "angularVelocity": 0.474877229979814,
+            "velocityX": -3.093829785211866,
+            "velocityY": 1.8264567174893411,
+            "timestamp": 1.1250547986776134
+        },
+        {
+            "x": 4.6181973815778665,
+            "y": 4.575631660267378,
+            "heading": 0.10901822024185409,
+            "angularVelocity": 0.652033519862158,
+            "velocityX": -2.5683076597621346,
+            "velocityY": 2.403061176467013,
+            "timestamp": 1.2188093652340812
+        },
+        {
+            "x": 4.427445933507109,
+            "y": 4.854263962903063,
+            "heading": 0.15298068479753676,
+            "angularVelocity": 0.46891011467909954,
+            "velocityX": -2.0345830083473238,
+            "velocityY": 2.971933132108998,
+            "timestamp": 1.312563931790549
+        },
+        {
+            "x": 4.19709169118396,
+            "y": 5.123942070734021,
+            "heading": 0.15298069409509907,
+            "angularVelocity": 9.916916750047786e-8,
+            "velocityX": -2.4569922381797515,
+            "velocityY": 2.8764263729866686,
+            "timestamp": 1.406318498347017
+        },
+        {
+            "x": 3.9282946631899107,
+            "y": 5.355323824233057,
+            "heading": 0.15298070467968702,
+            "angularVelocity": 1.1289677246612154e-7,
+            "velocityX": -2.8670286458223346,
+            "velocityY": 2.4679518235485323,
+            "timestamp": 1.5000730649034848
+        },
+        {
+            "x": 3.6594937496643456,
+            "y": 5.586701063819283,
+            "heading": 0.1529807152642756,
+            "angularVelocity": 1.1289677885329184e-7,
+            "velocityX": -2.867070089473112,
+            "velocityY": 2.4679036774904017,
+            "timestamp": 1.5938276314599527
+        },
+        {
+            "x": 3.3906928188108107,
+            "y": 5.818078283274842,
+            "heading": 0.1529807258488866,
+            "angularVelocity": 1.1289701863908675e-7,
+            "velocityX": -2.8670702742957928,
+            "velocityY": 2.467903462773746,
+            "timestamp": 1.6875821980164205
+        },
+        {
+            "x": 3.1218918754104785,
+            "y": 6.049455488154229,
+            "heading": 0.15298073643351034,
+            "angularVelocity": 1.1289715379702627e-7,
+            "velocityX": -2.867070408121775,
+            "velocityY": 2.467903307302158,
+            "timestamp": 1.7813367645728884
+        },
+        {
+            "x": 2.853090944613356,
+            "y": 6.280832707674931,
+            "heading": 0.15298074701856604,
+            "angularVelocity": 1.1290176153813175e-7,
+            "velocityX": -2.8670702736940825,
+            "velocityY": 2.4679034634685753,
+            "timestamp": 1.8750913311293562
+        },
+        {
+            "x": 2.631853660621228,
+            "y": 6.471299719512446,
+            "heading": 0.24894777935018825,
+            "angularVelocity": 1.0235984854542721,
+            "velocityX": -2.3597494193403232,
+            "velocityY": 2.0315491696374846,
+            "timestamp": 1.968845897685824
+        },
+        {
+            "x": 2.465926649664366,
+            "y": 6.614149879341798,
+            "heading": 0.32096506390227886,
+            "angularVelocity": 0.7681469521670183,
+            "velocityX": -1.7698019099360354,
+            "velocityY": 1.52366082075921,
+            "timestamp": 2.062600464242292
+        },
+        {
+            "x": 2.355308907973467,
+            "y": 6.709383273555383,
+            "heading": 0.3689895410149908,
+            "angularVelocity": 0.5122361382129277,
+            "velocityX": -1.1798651068828128,
+            "velocityY": 1.0157733933549364,
+            "timestamp": 2.1563550307987596
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 0.2546056806823578,
-            "velocityX": -0.5899613084572273,
-            "velocityY": 0.5079071497898447,
-            "timestamp": 2.2500991672994926
+            "heading": 0.393,
+            "angularVelocity": 0.25609908793666974,
+            "velocityX": -0.5899329494543092,
+            "velocityY": 0.5078870095990298,
+            "timestamp": 2.250109597355227
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
+            "heading": 0.393,
             "angularVelocity": 0,
-            "velocityX": 7.663961659075425e-29,
-            "velocityY": 7.569331315704837e-30,
-            "timestamp": 2.3438532992703047
+            "velocityX": -3.330818616870831e-32,
+            "velocityY": 5.736512740310497e-33,
+            "timestamp": 2.343864163911695
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.traj
index 5b28f5d1..f850aede 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.traj	
@@ -3,236 +3,236 @@
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 1.0740254772261542e-28,
-            "velocityX": 2.0063238328605694e-29,
-            "velocityY": 1.599232652510583e-28,
+            "heading": 0,
+            "angularVelocity": 2.322866633708497e-28,
+            "velocityX": -2.1618763976557105e-31,
+            "velocityY": -1.247476705840714e-31,
             "timestamp": 0
         },
         {
-            "x": 7.635665892651778,
-            "y": 2.515360941496967,
-            "heading": 3.1393295631597837,
-            "angularVelocity": -0.0071510111194275595,
-            "velocityX": -0.47287630332947994,
-            "velocityY": 0.6224892734875569,
-            "timestamp": 0.09375413197081223
-        },
-        {
-            "x": 7.546997676041258,
-            "y": 2.6320828188122176,
-            "heading": 3.137988695350318,
-            "angularVelocity": -0.014301959617960866,
-            "velocityX": -0.9457526270748597,
-            "velocityY": 1.2449784864051567,
-            "timestamp": 0.18750826394162445
-        },
-        {
-            "x": 7.413995349999322,
-            "y": 2.8071656233620255,
-            "heading": 3.1359774017076236,
-            "angularVelocity": -0.02145285333469997,
-            "velocityX": -1.4186289526241012,
-            "velocityY": 1.8674676077670485,
-            "timestamp": 0.2812623959124367
-        },
-        {
-            "x": 7.236658921990214,
-            "y": 3.0406093354422112,
-            "heading": 3.133295687975677,
-            "angularVelocity": -0.028603685785057682,
-            "velocityX": -1.8915051985582343,
-            "velocityY": 2.489956518960288,
-            "timestamp": 0.3750165278832489
-        },
-        {
-            "x": 7.022133976710729,
-            "y": 3.323040275054587,
-            "heading": 3.133295687627681,
-            "angularVelocity": -3.7117965182714086e-9,
-            "velocityX": -2.288165233573672,
-            "velocityY": 3.0124639167935428,
-            "timestamp": 0.46877065985406113
-        },
-        {
-            "x": 6.786563329839658,
-            "y": 3.588171672312515,
-            "heading": 3.133295687271953,
-            "angularVelocity": -3.794263922237942e-9,
-            "velocityX": -2.512642823512138,
-            "velocityY": 2.8279435976269225,
-            "timestamp": 0.5625247918248734
-        },
-        {
-            "x": 6.501527367007155,
-            "y": 3.7992230523695487,
-            "heading": 3.133295686856248,
-            "angularVelocity": -4.433993924393781e-9,
-            "velocityX": -3.040249606505238,
-            "velocityY": 2.251115504143757,
-            "timestamp": 0.6562789237956856
-        },
-        {
-            "x": 6.179197632360147,
-            "y": 3.947181584790984,
-            "heading": 3.133295705650933,
-            "angularVelocity": 2.004678114437141e-7,
-            "velocityX": -3.438032307177196,
-            "velocityY": 1.5781547896737034,
-            "timestamp": 0.7500330557664978
-        },
-        {
-            "x": 5.833338645417163,
-            "y": 4.025728972736192,
-            "heading": 3.133296009616361,
-            "angularVelocity": 0.000003242155002257518,
-            "velocityX": -3.6889999370977806,
-            "velocityY": 0.837801879171189,
-            "timestamp": 0.84378718773731
-        },
-        {
-            "x": 5.481107394947439,
-            "y": 4.067217134926503,
-            "heading": 3.1332963782586343,
-            "angularVelocity": 0.000003932010945944395,
-            "velocityX": -3.7569677524120277,
-            "velocityY": 0.4425208928736066,
-            "timestamp": 0.9375413197081222
-        },
-        {
-            "x": 5.149070300488837,
-            "y": 4.179082889717707,
-            "heading": 3.1449832895383,
-            "angularVelocity": 0.12465489289866781,
-            "velocityX": -3.541572915015461,
-            "velocityY": 1.1931821290397846,
-            "timestamp": 1.0312954516789343
-        },
-        {
-            "x": 4.859012780941325,
-            "y": 4.350314048380944,
-            "heading": 3.189517171995987,
-            "angularVelocity": 0.4750071439148605,
-            "velocityX": -3.093810517469455,
-            "velocityY": 1.826385195657799,
-            "timestamp": 1.1250495836497465
-        },
-        {
-            "x": 4.618219764951525,
-            "y": 4.5756090572292365,
-            "heading": 3.250644921045485,
-            "angularVelocity": 0.6520005866891553,
-            "velocityX": -2.5683456390462376,
-            "velocityY": 2.4030408485721884,
-            "timestamp": 1.2188037156205587
-        },
-        {
-            "x": 4.427454911100163,
-            "y": 4.854246931015481,
-            "heading": 3.2945713874044515,
-            "angularVelocity": 0.46852832441237513,
-            "velocityX": -2.034735428095452,
-            "velocityY": 2.9720063311233567,
-            "timestamp": 1.3125578475913708
-        },
-        {
-            "x": 4.197108920054306,
-            "y": 5.123929924649606,
-            "heading": 3.2945713966505874,
-            "angularVelocity": 9.862110665096438e-8,
-            "velocityX": -2.4569156175171605,
-            "velocityY": 2.8764918192414846,
-            "timestamp": 1.406311979562183
-        },
-        {
-            "x": 3.9283154974180072,
-            "y": 5.355313346500023,
-            "heading": 3.2945714071744505,
-            "angularVelocity": 1.1224958813512746e-7,
-            "velocityX": -2.8670034801237065,
-            "velocityY": 2.4679810583970268,
-            "timestamp": 1.5000661115329952
-        },
-        {
-            "x": 3.6595153829213625,
-            "y": 5.586688994322917,
-            "heading": 3.2945714176982936,
-            "angularVelocity": 1.1224938145766723e-7,
-            "velocityX": -2.867074856821552,
-            "velocityY": 2.4678981390913592,
-            "timestamp": 1.5938202435038074
-        },
-        {
-            "x": 3.3907152487550194,
-            "y": 5.818064619294552,
-            "heading": 3.2945714282221594,
-            "angularVelocity": 1.1224961995364469e-7,
-            "velocityX": -2.867075066622423,
-            "velocityY": 2.4678978953553523,
-            "timestamp": 1.6875743754746195
-        },
-        {
-            "x": 3.121915101893736,
-            "y": 6.049440229517853,
-            "heading": 3.294571438746038,
-            "angularVelocity": 1.1224975583094217e-7,
-            "velocityX": -2.8670752020291417,
-            "velocityY": 2.467897738046713,
-            "timestamp": 1.7813285074454317
-        },
-        {
-            "x": 2.853115082553889,
-            "y": 6.2808159878868715,
-            "heading": 3.2945714492725595,
-            "angularVelocity": 1.1227794602420482e-7,
-            "velocityX": -2.8670738418604556,
-            "velocityY": 2.467899318198048,
-            "timestamp": 1.8750826394162439
-        },
-        {
-            "x": 2.6318681155961685,
-            "y": 6.471289698509282,
-            "heading": 3.3899753271617903,
-            "angularVelocity": 1.0175965142415218,
-            "velocityX": -2.3598636380805007,
-            "velocityY": 2.031630037188212,
-            "timestamp": 1.968836771387056
-        },
-        {
-            "x": 2.465933868110727,
-            "y": 6.614144871792939,
-            "heading": 3.461571184706241,
-            "angularVelocity": 0.7636554895173272,
-            "velocityX": -1.7698872998696302,
-            "velocityY": 1.5237213579891205,
-            "timestamp": 2.062590903357868
-        },
-        {
-            "x": 2.3553113103707712,
-            "y": 6.709381606049684,
-            "heading": 3.5093149725923682,
-            "angularVelocity": 0.5092446261566541,
-            "velocityX": -1.1799219449271308,
-            "velocityY": 1.0158137274034427,
-            "timestamp": 2.1563450353286804
+            "x": 7.635666877682845,
+            "y": 2.515362381859407,
+            "heading": -0.0008300252680408195,
+            "angularVelocity": -0.00885317162168204,
+            "velocityX": -0.4728636048938783,
+            "velocityY": 0.6225017511467659,
+            "timestamp": 0.09375456655646779
+        },
+        {
+            "x": 7.547000631163484,
+            "y": 2.632087139923421,
+            "heading": -0.0024900693866490363,
+            "angularVelocity": -0.01770627479364842,
+            "velocityX": -0.9457272298940119,
+            "velocityY": 1.2450034419786018,
+            "timestamp": 0.18750913311293557
+        },
+        {
+            "x": 7.4140012603329,
+            "y": 2.8071742656560237,
+            "heading": -0.004980126198969091,
+            "angularVelocity": -0.026559312295687615,
+            "velocityX": -1.418590856056911,
+            "velocityY": 1.867505041764004,
+            "timestamp": 0.28126369966940334
+        },
+        {
+            "x": 7.236668772838196,
+            "y": 3.0406237394955027,
+            "heading": -0.008300187115435396,
+            "angularVelocity": -0.035412258180156565,
+            "velocityX": -1.891454400654691,
+            "velocityY": 2.4900064329013105,
+            "timestamp": 0.37501826622587114
+        },
+        {
+            "x": 7.022136423676778,
+            "y": 3.3230511197393278,
+            "heading": -0.008300187546286333,
+            "angularVelocity": -4.59551947810532e-9,
+            "velocityX": -2.2882335980104607,
+            "velocityY": 3.012411988206685,
+            "timestamp": 0.46877283278233894
+        },
+        {
+            "x": 6.786560921928144,
+            "y": 3.5881804025568163,
+            "heading": -0.008300187986713265,
+            "angularVelocity": -4.6976584720676315e-9,
+            "velocityX": -2.51268295936015,
+            "velocityY": 2.827907936172931,
+            "timestamp": 0.5625273993388067
+        },
+        {
+            "x": 6.5015204114211205,
+            "y": 3.799228403383322,
+            "heading": -0.008300188501414158,
+            "angularVelocity": -5.489875460815635e-9,
+            "velocityX": -3.0402840200359327,
+            "velocityY": 2.251069026055312,
+            "timestamp": 0.6562819658952744
+        },
+        {
+            "x": 6.179186778264032,
+            "y": 3.9471823835437703,
+            "heading": -0.008300169798343478,
+            "angularVelocity": 1.9948970346527163e-7,
+            "velocityX": -3.4380579527606097,
+            "velocityY": 1.5780989192812942,
+            "timestamp": 0.7500365324517422
+        },
+        {
+            "x": 5.833324835964247,
+            "y": 4.025724182042927,
+            "heading": -0.008299866211032016,
+            "angularVelocity": 0.0000032381069276099122,
+            "velocityX": -3.689014359545628,
+            "velocityY": 0.8377383778085268,
+            "timestamp": 0.8437910990082099
+        },
+        {
+            "x": 5.481092764022396,
+            "y": 4.0672194252777,
+            "heading": -0.008299498284594501,
+            "angularVelocity": 0.000003924357511610969,
+            "velocityX": -3.7569590994770814,
+            "velocityY": 0.44259436909433375,
+            "timestamp": 0.9375456655646777
+        },
+        {
+            "x": 5.14904862351455,
+            "y": 4.179095043376677,
+            "heading": 0.0033651913425959347,
+            "angularVelocity": 0.12441729566489874,
+            "velocityX": -3.5416316527670983,
+            "velocityY": 1.1932818017093059,
+            "timestamp": 1.0313002321211455
+        },
+        {
+            "x": 4.858987953002521,
+            "y": 4.350333701259038,
+            "heading": 0.047887100206889445,
+            "angularVelocity": 0.474877229979814,
+            "velocityX": -3.093829785211866,
+            "velocityY": 1.8264567174893411,
+            "timestamp": 1.1250547986776134
+        },
+        {
+            "x": 4.6181973815778665,
+            "y": 4.575631660267378,
+            "heading": 0.10901822024185409,
+            "angularVelocity": 0.652033519862158,
+            "velocityX": -2.5683076597621346,
+            "velocityY": 2.403061176467013,
+            "timestamp": 1.2188093652340812
+        },
+        {
+            "x": 4.427445933507109,
+            "y": 4.854263962903063,
+            "heading": 0.15298068479753676,
+            "angularVelocity": 0.46891011467909954,
+            "velocityX": -2.0345830083473238,
+            "velocityY": 2.971933132108998,
+            "timestamp": 1.312563931790549
+        },
+        {
+            "x": 4.19709169118396,
+            "y": 5.123942070734021,
+            "heading": 0.15298069409509907,
+            "angularVelocity": 9.916916750047786e-8,
+            "velocityX": -2.4569922381797515,
+            "velocityY": 2.8764263729866686,
+            "timestamp": 1.406318498347017
+        },
+        {
+            "x": 3.9282946631899107,
+            "y": 5.355323824233057,
+            "heading": 0.15298070467968702,
+            "angularVelocity": 1.1289677246612154e-7,
+            "velocityX": -2.8670286458223346,
+            "velocityY": 2.4679518235485323,
+            "timestamp": 1.5000730649034848
+        },
+        {
+            "x": 3.6594937496643456,
+            "y": 5.586701063819283,
+            "heading": 0.1529807152642756,
+            "angularVelocity": 1.1289677885329184e-7,
+            "velocityX": -2.867070089473112,
+            "velocityY": 2.4679036774904017,
+            "timestamp": 1.5938276314599527
+        },
+        {
+            "x": 3.3906928188108107,
+            "y": 5.818078283274842,
+            "heading": 0.1529807258488866,
+            "angularVelocity": 1.1289701863908675e-7,
+            "velocityX": -2.8670702742957928,
+            "velocityY": 2.467903462773746,
+            "timestamp": 1.6875821980164205
+        },
+        {
+            "x": 3.1218918754104785,
+            "y": 6.049455488154229,
+            "heading": 0.15298073643351034,
+            "angularVelocity": 1.1289715379702627e-7,
+            "velocityX": -2.867070408121775,
+            "velocityY": 2.467903307302158,
+            "timestamp": 1.7813367645728884
+        },
+        {
+            "x": 2.853090944613356,
+            "y": 6.280832707674931,
+            "heading": 0.15298074701856604,
+            "angularVelocity": 1.1290176153813175e-7,
+            "velocityX": -2.8670702736940825,
+            "velocityY": 2.4679034634685753,
+            "timestamp": 1.8750913311293562
+        },
+        {
+            "x": 2.631853660621228,
+            "y": 6.471299719512446,
+            "heading": 0.24894777935018825,
+            "angularVelocity": 1.0235984854542721,
+            "velocityX": -2.3597494193403232,
+            "velocityY": 2.0315491696374846,
+            "timestamp": 1.968845897685824
+        },
+        {
+            "x": 2.465926649664366,
+            "y": 6.614149879341798,
+            "heading": 0.32096506390227886,
+            "angularVelocity": 0.7681469521670183,
+            "velocityX": -1.7698019099360354,
+            "velocityY": 1.52366082075921,
+            "timestamp": 2.062600464242292
+        },
+        {
+            "x": 2.355308907973467,
+            "y": 6.709383273555383,
+            "heading": 0.3689895410149908,
+            "angularVelocity": 0.5122361382129277,
+            "velocityX": -1.1798651068828128,
+            "velocityY": 1.0157733933549364,
+            "timestamp": 2.1563550307987596
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 0.2546056806823578,
-            "velocityX": -0.5899613084572273,
-            "velocityY": 0.5079071497898447,
-            "timestamp": 2.2500991672994926
+            "heading": 0.393,
+            "angularVelocity": 0.25609908793666974,
+            "velocityX": -0.5899329494543092,
+            "velocityY": 0.5078870095990298,
+            "timestamp": 2.250109597355227
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
+            "heading": 0.393,
             "angularVelocity": 0,
-            "velocityX": 7.663961659075425e-29,
-            "velocityY": 7.569331315704837e-30,
-            "timestamp": 2.3438532992703047
+            "velocityX": -3.330818616870831e-32,
+            "velocityY": 5.736512740310497e-33,
+            "timestamp": 2.343864163911695
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.1.traj
deleted file mode 100644
index 03ea08d7..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.1.traj	
+++ /dev/null
@@ -1,211 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 1.048713449145388e-30,
-            "velocityX": -1.8824383693720305e-31,
-            "velocityY": -8.291277152276166e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.627273403539296,
-            "y": 2.514475736726478,
-            "heading": 3.079415345929856,
-            "angularVelocity": -0.619115297628915,
-            "velocityX": -0.5388137138314486,
-            "velocityY": 0.5873452344657393,
-            "timestamp": 0.09785681972674802
-        },
-        {
-            "x": 7.521466473904217,
-            "y": 2.629566524398062,
-            "heading": 2.9639191611810785,
-            "angularVelocity": -1.1802568801161224,
-            "velocityX": -1.08124226733027,
-            "velocityY": 1.1761141225819405,
-            "timestamp": 0.19571363945349604
-        },
-        {
-            "x": 7.362002347569612,
-            "y": 2.8026163659937295,
-            "heading": 2.8066032187897654,
-            "angularVelocity": -1.6076134788622511,
-            "velocityX": -1.6295657960261463,
-            "velocityY": 1.7683983812153936,
-            "timestamp": 0.29357045918024405
-        },
-        {
-            "x": 7.14775766144854,
-            "y": 3.033681724074784,
-            "heading": 2.6597575656629697,
-            "angularVelocity": -1.5006174688370466,
-            "velocityX": -2.18936898541483,
-            "velocityY": 2.361259631431653,
-            "timestamp": 0.3914272789069921
-        },
-        {
-            "x": 6.898594719017662,
-            "y": 3.307462468622087,
-            "heading": 2.6597575308458405,
-            "angularVelocity": -3.5579665549162934e-7,
-            "velocityX": -2.546199060286565,
-            "velocityY": 2.7977686717369177,
-            "timestamp": 0.4892840986337401
-        },
-        {
-            "x": 6.632320724562211,
-            "y": 3.5646321159756953,
-            "heading": 2.6597574950521046,
-            "angularVelocity": -3.657766164691694e-7,
-            "velocityX": -2.721057103623286,
-            "velocityY": 2.628019672739413,
-            "timestamp": 0.5871409183604881
-        },
-        {
-            "x": 6.3170953901288,
-            "y": 3.7587224487937174,
-            "heading": 2.6597575172185417,
-            "angularVelocity": 2.265190860696483e-7,
-            "velocityX": -3.221291426735886,
-            "velocityY": 1.983411410262395,
-            "timestamp": 0.6849977380872361
-        },
-        {
-            "x": 5.989980622324306,
-            "y": 3.9320256447233213,
-            "heading": 2.65975753898433,
-            "angularVelocity": 2.224248489911636e-7,
-            "velocityX": -3.342789687197243,
-            "velocityY": 1.770987412155123,
-            "timestamp": 0.782854557813984
-        },
-        {
-            "x": 5.662865689540743,
-            "y": 4.1053285292498645,
-            "heading": 2.659757560750018,
-            "angularVelocity": 2.2242382298690162e-7,
-            "velocityX": -3.3427913731203196,
-            "velocityY": 1.7709842299235545,
-            "timestamp": 0.880711377540732
-        },
-        {
-            "x": 5.335750711483487,
-            "y": 4.278631328320837,
-            "heading": 2.659757582515508,
-            "angularVelocity": 2.2242180190175268e-7,
-            "velocityX": -3.3427918357727204,
-            "velocityY": 1.7709833566520703,
-            "timestamp": 0.97856819726748
-        },
-        {
-            "x": 5.008636210362509,
-            "y": 4.451935027622489,
-            "heading": 2.659757604280525,
-            "angularVelocity": 2.224169652952219e-7,
-            "velocityX": -3.342786961955248,
-            "velocityY": 1.7709925561200404,
-            "timestamp": 1.076425016994228
-        },
-        {
-            "x": 4.6815226569833905,
-            "y": 4.625240515795651,
-            "heading": 2.6597576260457285,
-            "angularVelocity": 2.224188696537283e-7,
-            "velocityX": -3.3427772769699504,
-            "velocityY": 1.7710108366192054,
-            "timestamp": 1.174281836720976
-        },
-        {
-            "x": 4.378427967662734,
-            "y": 4.837776808087965,
-            "heading": 2.65975767630399,
-            "angularVelocity": 5.135897752366777e-7,
-            "velocityX": -3.097328220628949,
-            "velocityY": 2.171910888641101,
-            "timestamp": 1.272138656447724
-        },
-        {
-            "x": 4.050949875066163,
-            "y": 5.0103924451271915,
-            "heading": 2.6597577704243744,
-            "angularVelocity": 9.61817319833198e-7,
-            "velocityX": -3.3465025075514396,
-            "velocityY": 1.7639612397095272,
-            "timestamp": 1.369995476174472
-        },
-        {
-            "x": 3.722909272369472,
-            "y": 5.181936673224133,
-            "heading": 2.659757864941058,
-            "angularVelocity": 9.65867108780206e-7,
-            "velocityX": -3.352250804928058,
-            "velocityY": 1.7530124990363987,
-            "timestamp": 1.46785229590122
-        },
-        {
-            "x": 3.383528958103272,
-            "y": 5.329784787197771,
-            "heading": 2.659757940242804,
-            "angularVelocity": 7.6950943654844e-7,
-            "velocityX": -3.468131451787155,
-            "velocityY": 1.5108616281060843,
-            "timestamp": 1.5657091156279679
-        },
-        {
-            "x": 3.020395743291134,
-            "y": 5.401703742215627,
-            "heading": 2.6597579736142767,
-            "angularVelocity": 3.4102347755826357e-7,
-            "velocityX": -3.7108626238430706,
-            "velocityY": 0.7349406532797214,
-            "timestamp": 1.6635659353547159
-        },
-        {
-            "x": 2.7114854008089204,
-            "y": 5.466141485650731,
-            "heading": 2.823345730065705,
-            "angularVelocity": 1.6717052210385108,
-            "velocityX": -3.156758449179158,
-            "velocityY": 0.6584900634931665,
-            "timestamp": 1.7614227550814638
-        },
-        {
-            "x": 2.4804696774793777,
-            "y": 5.513370093880283,
-            "heading": 2.974776278106282,
-            "angularVelocity": 1.5474705642736624,
-            "velocityX": -2.3607524133179756,
-            "velocityY": 0.48262970696813046,
-            "timestamp": 1.8592795748082118
-        },
-        {
-            "x": 2.3267564356504185,
-            "y": 5.544523353838769,
-            "heading": 3.083396984959406,
-            "angularVelocity": 1.1099962900534985,
-            "velocityX": -1.5707974391379447,
-            "velocityY": 0.3183555325574442,
-            "timestamp": 1.9571363945349598
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0.5784268812194232,
-            "velocityX": -0.7843749251687325,
-            "velocityY": 0.15815603045805554,
-            "timestamp": 2.054993214261708
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 1.5606110446115462e-29,
-            "velocityX": 0,
-            "velocityY": 3.583632692315791e-31,
-            "timestamp": 2.152850033988456
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.traj
deleted file mode 100644
index 03ea08d7..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.traj	
+++ /dev/null
@@ -1,211 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 1.048713449145388e-30,
-            "velocityX": -1.8824383693720305e-31,
-            "velocityY": -8.291277152276166e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.627273403539296,
-            "y": 2.514475736726478,
-            "heading": 3.079415345929856,
-            "angularVelocity": -0.619115297628915,
-            "velocityX": -0.5388137138314486,
-            "velocityY": 0.5873452344657393,
-            "timestamp": 0.09785681972674802
-        },
-        {
-            "x": 7.521466473904217,
-            "y": 2.629566524398062,
-            "heading": 2.9639191611810785,
-            "angularVelocity": -1.1802568801161224,
-            "velocityX": -1.08124226733027,
-            "velocityY": 1.1761141225819405,
-            "timestamp": 0.19571363945349604
-        },
-        {
-            "x": 7.362002347569612,
-            "y": 2.8026163659937295,
-            "heading": 2.8066032187897654,
-            "angularVelocity": -1.6076134788622511,
-            "velocityX": -1.6295657960261463,
-            "velocityY": 1.7683983812153936,
-            "timestamp": 0.29357045918024405
-        },
-        {
-            "x": 7.14775766144854,
-            "y": 3.033681724074784,
-            "heading": 2.6597575656629697,
-            "angularVelocity": -1.5006174688370466,
-            "velocityX": -2.18936898541483,
-            "velocityY": 2.361259631431653,
-            "timestamp": 0.3914272789069921
-        },
-        {
-            "x": 6.898594719017662,
-            "y": 3.307462468622087,
-            "heading": 2.6597575308458405,
-            "angularVelocity": -3.5579665549162934e-7,
-            "velocityX": -2.546199060286565,
-            "velocityY": 2.7977686717369177,
-            "timestamp": 0.4892840986337401
-        },
-        {
-            "x": 6.632320724562211,
-            "y": 3.5646321159756953,
-            "heading": 2.6597574950521046,
-            "angularVelocity": -3.657766164691694e-7,
-            "velocityX": -2.721057103623286,
-            "velocityY": 2.628019672739413,
-            "timestamp": 0.5871409183604881
-        },
-        {
-            "x": 6.3170953901288,
-            "y": 3.7587224487937174,
-            "heading": 2.6597575172185417,
-            "angularVelocity": 2.265190860696483e-7,
-            "velocityX": -3.221291426735886,
-            "velocityY": 1.983411410262395,
-            "timestamp": 0.6849977380872361
-        },
-        {
-            "x": 5.989980622324306,
-            "y": 3.9320256447233213,
-            "heading": 2.65975753898433,
-            "angularVelocity": 2.224248489911636e-7,
-            "velocityX": -3.342789687197243,
-            "velocityY": 1.770987412155123,
-            "timestamp": 0.782854557813984
-        },
-        {
-            "x": 5.662865689540743,
-            "y": 4.1053285292498645,
-            "heading": 2.659757560750018,
-            "angularVelocity": 2.2242382298690162e-7,
-            "velocityX": -3.3427913731203196,
-            "velocityY": 1.7709842299235545,
-            "timestamp": 0.880711377540732
-        },
-        {
-            "x": 5.335750711483487,
-            "y": 4.278631328320837,
-            "heading": 2.659757582515508,
-            "angularVelocity": 2.2242180190175268e-7,
-            "velocityX": -3.3427918357727204,
-            "velocityY": 1.7709833566520703,
-            "timestamp": 0.97856819726748
-        },
-        {
-            "x": 5.008636210362509,
-            "y": 4.451935027622489,
-            "heading": 2.659757604280525,
-            "angularVelocity": 2.224169652952219e-7,
-            "velocityX": -3.342786961955248,
-            "velocityY": 1.7709925561200404,
-            "timestamp": 1.076425016994228
-        },
-        {
-            "x": 4.6815226569833905,
-            "y": 4.625240515795651,
-            "heading": 2.6597576260457285,
-            "angularVelocity": 2.224188696537283e-7,
-            "velocityX": -3.3427772769699504,
-            "velocityY": 1.7710108366192054,
-            "timestamp": 1.174281836720976
-        },
-        {
-            "x": 4.378427967662734,
-            "y": 4.837776808087965,
-            "heading": 2.65975767630399,
-            "angularVelocity": 5.135897752366777e-7,
-            "velocityX": -3.097328220628949,
-            "velocityY": 2.171910888641101,
-            "timestamp": 1.272138656447724
-        },
-        {
-            "x": 4.050949875066163,
-            "y": 5.0103924451271915,
-            "heading": 2.6597577704243744,
-            "angularVelocity": 9.61817319833198e-7,
-            "velocityX": -3.3465025075514396,
-            "velocityY": 1.7639612397095272,
-            "timestamp": 1.369995476174472
-        },
-        {
-            "x": 3.722909272369472,
-            "y": 5.181936673224133,
-            "heading": 2.659757864941058,
-            "angularVelocity": 9.65867108780206e-7,
-            "velocityX": -3.352250804928058,
-            "velocityY": 1.7530124990363987,
-            "timestamp": 1.46785229590122
-        },
-        {
-            "x": 3.383528958103272,
-            "y": 5.329784787197771,
-            "heading": 2.659757940242804,
-            "angularVelocity": 7.6950943654844e-7,
-            "velocityX": -3.468131451787155,
-            "velocityY": 1.5108616281060843,
-            "timestamp": 1.5657091156279679
-        },
-        {
-            "x": 3.020395743291134,
-            "y": 5.401703742215627,
-            "heading": 2.6597579736142767,
-            "angularVelocity": 3.4102347755826357e-7,
-            "velocityX": -3.7108626238430706,
-            "velocityY": 0.7349406532797214,
-            "timestamp": 1.6635659353547159
-        },
-        {
-            "x": 2.7114854008089204,
-            "y": 5.466141485650731,
-            "heading": 2.823345730065705,
-            "angularVelocity": 1.6717052210385108,
-            "velocityX": -3.156758449179158,
-            "velocityY": 0.6584900634931665,
-            "timestamp": 1.7614227550814638
-        },
-        {
-            "x": 2.4804696774793777,
-            "y": 5.513370093880283,
-            "heading": 2.974776278106282,
-            "angularVelocity": 1.5474705642736624,
-            "velocityX": -2.3607524133179756,
-            "velocityY": 0.48262970696813046,
-            "timestamp": 1.8592795748082118
-        },
-        {
-            "x": 2.3267564356504185,
-            "y": 5.544523353838769,
-            "heading": 3.083396984959406,
-            "angularVelocity": 1.1099962900534985,
-            "velocityX": -1.5707974391379447,
-            "velocityY": 0.3183555325574442,
-            "timestamp": 1.9571363945349598
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0.5784268812194232,
-            "velocityX": -0.7843749251687325,
-            "velocityY": 0.15815603045805554,
-            "timestamp": 2.054993214261708
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 1.5606110446115462e-29,
-            "velocityX": 0,
-            "velocityY": 3.583632692315791e-31,
-            "timestamp": 2.152850033988456
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.1.traj
index db86197d..98bf1cf8 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.1.traj	
@@ -3,218 +3,218 @@
         {
             "x": 1.35,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -5.5460688640051806e-30,
-            "velocityX": 1.8487232872526845e-30,
-            "velocityY": -2.9628257998243394e-30,
+            "heading": 0,
+            "angularVelocity": -1.7848043602710886e-30,
+            "velocityX": 4.033392918765628e-32,
+            "velocityY": -2.9619524123271102e-31,
             "timestamp": 0
         },
         {
-            "x": 1.423394920733784,
-            "y": 5.582111402692338,
-            "heading": 3.1399999953305415,
-            "angularVelocity": -4.870437841651369e-8,
-            "velocityX": 0.7654820737524797,
-            "velocityY": 0.23061381110796328,
-            "timestamp": 0.09588065253313424
-        },
-        {
-            "x": 1.5701847607820971,
-            "y": 5.626334202094221,
-            "heading": 3.1399999893931145,
-            "angularVelocity": -6.193245197617188e-8,
-            "velocityX": 1.530964132702707,
-            "velocityY": 0.46122755981751334,
-            "timestamp": 0.19176130506626848
-        },
-        {
-            "x": 1.7903695158505508,
-            "y": 5.692668391072556,
-            "heading": 3.1399999853590668,
-            "angularVelocity": -4.208451353086861e-8,
-            "velocityX": 2.2964461468641186,
-            "velocityY": 0.6918412341313931,
-            "timestamp": 0.2876419575994027
-        },
-        {
-            "x": 2.0839491699386645,
-            "y": 5.781113958575535,
-            "heading": 3.139999985983334,
-            "angularVelocity": 6.496408603804424e-9,
-            "velocityX": 3.0619279941465973,
-            "velocityY": 0.922454793178729,
-            "timestamp": 0.38352261013253697
-        },
-        {
-            "x": 2.431241902978436,
-            "y": 5.885741341465996,
-            "heading": 3.1399999859835575,
-            "angularVelocity": 2.3334348335212476e-12,
-            "velocityX": 3.6221356849840607,
-            "velocityY": 1.0912251859537307,
-            "timestamp": 0.47940326266567124
-        },
-        {
-            "x": 2.7785346564747577,
-            "y": 5.990368656458575,
-            "heading": 3.139999985983492,
-            "angularVelocity": -6.768139199697334e-13,
-            "velocityX": 3.62213589833913,
-            "velocityY": 1.0912244778026317,
-            "timestamp": 0.5752839151988055
-        },
-        {
-            "x": 3.125827431847794,
-            "y": 6.094995898834488,
-            "heading": 3.139999985983408,
-            "angularVelocity": -8.748780481443456e-13,
-            "velocityX": 3.6221361265054535,
-            "velocityY": 1.091223720437653,
-            "timestamp": 0.6711645677319398
-        },
-        {
-            "x": 3.473120229952072,
-            "y": 6.199623065757117,
-            "heading": 3.1399999859833065,
-            "angularVelocity": -1.0615094861925232e-12,
-            "velocityX": 3.6221363635841364,
-            "velocityY": 1.0912229334878236,
-            "timestamp": 0.767045220265074
-        },
-        {
-            "x": 3.820413051729017,
-            "y": 6.304250154101482,
-            "heading": 3.139999985983188,
-            "angularVelocity": -1.2288425429953676e-12,
-            "velocityX": 3.6221366104815065,
-            "velocityY": 1.0912221139455949,
-            "timestamp": 0.8629258727982083
-        },
-        {
-            "x": 4.167705898643341,
-            "y": 6.408877159005614,
-            "heading": 3.139999985983049,
-            "angularVelocity": -1.4178096575705385e-12,
-            "velocityX": 3.6221368726552323,
-            "velocityY": 1.0912212436948225,
-            "timestamp": 0.9588065253313426
-        },
-        {
-            "x": 4.5149987734968,
-            "y": 6.5135040711691525,
-            "heading": 3.139999985982883,
-            "angularVelocity": -1.676018359549876e-12,
-            "velocityX": 3.622137164050193,
-            "velocityY": 1.0912202764447858,
-            "timestamp": 1.0546871778644769
-        },
-        {
-            "x": 4.862291683371807,
-            "y": 6.6181308670796435,
-            "heading": 3.1399999859826795,
-            "angularVelocity": -2.066469238283253e-12,
-            "velocityX": 3.6221375293116886,
-            "velocityY": 1.0912190639696435,
-            "timestamp": 1.150567830397611
-        },
-        {
-            "x": 5.209584643601777,
-            "y": 6.722757495842404,
-            "heading": 3.139999985982697,
-            "angularVelocity": 1.7634954198261724e-13,
-            "velocityX": 3.6221380544925466,
-            "velocityY": 1.0912173206895408,
-            "timestamp": 1.2464484829307452
-        },
-        {
-            "x": 5.55687764566899,
-            "y": 6.827383985745101,
-            "heading": 3.139999985982843,
-            "angularVelocity": 1.4429880633934061e-12,
-            "velocityX": 3.6221384908382315,
-            "velocityY": 1.091215872433919,
-            "timestamp": 1.3423291354638793
-        },
-        {
-            "x": 5.904170680203765,
-            "y": 6.932010367880933,
-            "heading": 3.1399999859829473,
-            "angularVelocity": 9.965559645934633e-13,
-            "velocityX": 3.6221388294623984,
-            "velocityY": 1.0912147484660204,
-            "timestamp": 1.4382097879970135
-        },
-        {
-            "x": 6.251463742058712,
-            "y": 7.036636659333844,
-            "heading": 3.1399999859830086,
-            "angularVelocity": 5.792335690532441e-13,
-            "velocityX": 3.6221391144013104,
-            "velocityY": 1.0912138026768703,
-            "timestamp": 1.5340904405301476
-        },
-        {
-            "x": 6.598756826003656,
-            "y": 7.141262877462233,
-            "heading": 3.139999985983042,
-            "angularVelocity": 3.176331549030912e-13,
-            "velocityX": 3.6221393447915826,
-            "velocityY": 1.0912130379289529,
-            "timestamp": 1.6299710930632818
-        },
-        {
-            "x": 6.9460499272931315,
-            "y": 7.245889038012359,
-            "heading": 3.139999985982778,
-            "angularVelocity": -2.760419154137832e-12,
-            "velocityX": 3.622139525688641,
-            "velocityY": 1.0912124374075387,
-            "timestamp": 1.725851745596416
-        },
-        {
-            "x": 7.239629936933939,
-            "y": 7.334333425317839,
-            "heading": 3.139999987228266,
-            "angularVelocity": 1.3004483336236485e-8,
-            "velocityX": 3.061931702425989,
-            "velocityY": 0.9224424841673509,
-            "timestamp": 1.8217323981295501
-        },
-        {
-            "x": 7.459814962920636,
-            "y": 7.4006667150284455,
-            "heading": 3.13999999169634,
-            "angularVelocity": 4.661129161255315e-8,
-            "velocityX": 2.2964489724386126,
-            "velocityY": 0.6918318551100047,
-            "timestamp": 1.9176130506626843
-        },
-        {
-            "x": 7.606604986293984,
-            "y": 7.444888905913496,
-            "heading": 3.1399999966584766,
-            "angularVelocity": 5.176056299229228e-8,
-            "velocityX": 1.530966044713388,
-            "velocityY": 0.46122121321737414,
-            "timestamp": 2.0134937031958184
+            "x": 1.4233949207458116,
+            "y": 5.582111402652435,
+            "heading": -4.931478664026919e-9,
+            "angularVelocity": -5.143711867197603e-8,
+            "velocityX": 0.7654820738778157,
+            "velocityY": 0.2306138106917608,
+            "timestamp": 0.09588065253314748
+        },
+        {
+            "x": 1.570184760818058,
+            "y": 5.626334201974914,
+            "heading": -1.1368240805751946e-8,
+            "angularVelocity": -6.714026800936027e-8,
+            "velocityX": 1.530964132952106,
+            "velocityY": 0.4612275589893128,
+            "timestamp": 0.19176130506629496
+        },
+        {
+            "x": 1.7903695159222153,
+            "y": 5.69266839083479,
+            "heading": -1.6115736339739044e-8,
+            "angularVelocity": -4.952541704566216e-8,
+            "velocityX": 2.2964461472361695,
+            "velocityY": 0.6918412328958446,
+            "timestamp": 0.28764195759944244
+        },
+        {
+            "x": 2.0839491700576396,
+            "y": 5.781113958180788,
+            "heading": -1.639772094621516e-8,
+            "angularVelocity": -2.9553274583871895e-9,
+            "velocityX": 3.0619279946395954,
+            "velocityY": 0.9224547915413995,
+            "timestamp": 0.3835226101325899
+        },
+        {
+            "x": 2.431241903149783,
+            "y": 5.885741340897451,
+            "heading": -1.6397499367976412e-8,
+            "angularVelocity": 2.3168753954621105e-12,
+            "velocityX": 3.6221356855297673,
+            "velocityY": 1.0912251841409724,
+            "timestamp": 0.4794032626657374
+        },
+        {
+            "x": 2.7785346566980107,
+            "y": 5.9903686557177505,
+            "heading": -1.6397564221401302e-8,
+            "angularVelocity": -6.713496881725287e-13,
+            "velocityX": 3.622135898879975,
+            "velocityY": 1.091224476005724,
+            "timestamp": 0.5752839151988849
+        },
+        {
+            "x": 3.125827432121608,
+            "y": 6.094995897925851,
+            "heading": -1.6397647407961563e-8,
+            "angularVelocity": -8.684642063246534e-13,
+            "velocityX": 3.6221361270322743,
+            "velocityY": 1.0912237186873281,
+            "timestamp": 0.6711645677320324
+        },
+        {
+            "x": 3.4731202302782154,
+            "y": 6.1996230646748005,
+            "heading": -1.6397748264247437e-8,
+            "angularVelocity": -1.0543921850153982e-12,
+            "velocityX": 3.6221363641294033,
+            "velocityY": 1.0912229316763022,
+            "timestamp": 0.7670452202651799
+        },
+        {
+            "x": 3.8204130521112987,
+            "y": 6.304250152832848,
+            "heading": -1.639786614425739e-8,
+            "angularVelocity": -1.2203887930777277e-12,
+            "velocityX": 3.622136611066492,
+            "velocityY": 1.091222112002269,
+            "timestamp": 0.8629258727983273
+        },
+        {
+            "x": 4.167705899087819,
+            "y": 6.408877157530555,
+            "heading": -1.6398004207896958e-8,
+            "angularVelocity": -1.408074947723948e-12,
+            "velocityX": 3.6221368733034063,
+            "velocityY": 1.0912212415417761,
+            "timestamp": 0.9588065253314748
+        },
+        {
+            "x": 4.514998774010408,
+            "y": 6.51350406946466,
+            "heading": -1.6398168919050745e-8,
+            "angularVelocity": -1.6668913936591836e-12,
+            "velocityX": 3.6221371647706677,
+            "velocityY": 1.091220274051783,
+            "timestamp": 1.0546871778646223
+        },
+        {
+            "x": 4.862291683961819,
+            "y": 6.618130865121593,
+            "heading": -1.6398370762014746e-8,
+            "angularVelocity": -2.0513887673195245e-12,
+            "velocityX": 3.6221375301080334,
+            "velocityY": 1.0912190613250203,
+            "timestamp": 1.1505678303977698
+        },
+        {
+            "x": 5.20958464417323,
+            "y": 6.722757493946074,
+            "heading": -1.639835180923073e-8,
+            "angularVelocity": 1.902358989315915e-13,
+            "velocityX": 3.6221380542985027,
+            "velocityY": 1.091217321333091,
+            "timestamp": 1.2464484829309173
+        },
+        {
+            "x": 5.556877646148189,
+            "y": 6.8273839841549115,
+            "heading": -1.6398206125528607e-8,
+            "angularVelocity": 1.4352640935347951e-12,
+            "velocityX": 3.62213848987558,
+            "velocityY": 1.09121587562664,
+            "timestamp": 1.3423291354640647
+        },
+        {
+            "x": 5.904170680592406,
+            "y": 6.932010366591235,
+            "heading": -1.6398102572596455e-8,
+            "angularVelocity": 9.899977906647045e-13,
+            "velocityX": 3.6221388285174303,
+            "velocityY": 1.091214751599812,
+            "timestamp": 1.4382097879972122
+        },
+        {
+            "x": 6.2514637423704675,
+            "y": 7.036636658299249,
+            "heading": -1.6398041778119566e-8,
+            "angularVelocity": 5.750349690087699e-13,
+            "velocityX": 3.6221391135989474,
+            "velocityY": 1.0912138053372793,
+            "timestamp": 1.5340904405303597
+        },
+        {
+            "x": 6.5987568262419405,
+            "y": 7.141262876671409,
+            "heading": -1.639800860171266e-8,
+            "angularVelocity": 3.175769618903374e-13,
+            "velocityX": 3.6221393440248217,
+            "velocityY": 1.0912130404711837,
+            "timestamp": 1.6299710930635072
+        },
+        {
+            "x": 6.946049927457118,
+            "y": 7.2458890374680776,
+            "heading": -1.6398270316090924e-8,
+            "angularVelocity": -2.740394345840068e-12,
+            "velocityX": 3.6221395249132713,
+            "velocityY": 1.0912124399786907,
+            "timestamp": 1.7258517455966547
+        },
+        {
+            "x": 7.239629937032664,
+            "y": 7.334333424990155,
+            "heading": -1.4134914023195805e-8,
+            "angularVelocity": 2.36203418937418e-8,
+            "velocityX": 3.061931701744924,
+            "velocityY": 0.9224424864262022,
+            "timestamp": 1.8217323981298021
+        },
+        {
+            "x": 7.45981496297016,
+            "y": 7.4006667148640615,
+            "heading": -8.953329207671134e-9,
+            "angularVelocity": 5.4052849582725206e-8,
+            "velocityX": 2.2964489719251726,
+            "velocityY": 0.6918318568130147,
+            "timestamp": 1.9176130506629496
+        },
+        {
+            "x": 7.606604986310546,
+            "y": 7.444888905858524,
+            "heading": -3.5476510780655253e-9,
+            "angularVelocity": 5.638647309242438e-8,
+            "velocityX": 1.5309660443693864,
+            "velocityY": 0.4612212143584205,
+            "timestamp": 2.0134937031960973
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 3.485452160899378e-8,
-            "velocityX": 0.7654830434176825,
-            "velocityY": 0.23061059246584142,
-            "timestamp": 2.109374355728953
+            "heading": 1.0344705803481413e-31,
+            "angularVelocity": 3.7004322975715074e-8,
+            "velocityX": 0.7654830432448523,
+            "velocityY": 0.23061059303913045,
+            "timestamp": 2.109374355729245
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": -1.7922203019823127e-29,
+            "heading": 0,
+            "angularVelocity": -1.5335768309775317e-33,
             "velocityX": 0,
-            "velocityY": -5.07505109843192e-30,
-            "timestamp": 2.205255008262087
+            "velocityY": -2.5688294662733772e-30,
+            "timestamp": 2.2052550082623927
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.traj
index db86197d..98bf1cf8 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.traj	
@@ -3,218 +3,218 @@
         {
             "x": 1.35,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -5.5460688640051806e-30,
-            "velocityX": 1.8487232872526845e-30,
-            "velocityY": -2.9628257998243394e-30,
+            "heading": 0,
+            "angularVelocity": -1.7848043602710886e-30,
+            "velocityX": 4.033392918765628e-32,
+            "velocityY": -2.9619524123271102e-31,
             "timestamp": 0
         },
         {
-            "x": 1.423394920733784,
-            "y": 5.582111402692338,
-            "heading": 3.1399999953305415,
-            "angularVelocity": -4.870437841651369e-8,
-            "velocityX": 0.7654820737524797,
-            "velocityY": 0.23061381110796328,
-            "timestamp": 0.09588065253313424
-        },
-        {
-            "x": 1.5701847607820971,
-            "y": 5.626334202094221,
-            "heading": 3.1399999893931145,
-            "angularVelocity": -6.193245197617188e-8,
-            "velocityX": 1.530964132702707,
-            "velocityY": 0.46122755981751334,
-            "timestamp": 0.19176130506626848
-        },
-        {
-            "x": 1.7903695158505508,
-            "y": 5.692668391072556,
-            "heading": 3.1399999853590668,
-            "angularVelocity": -4.208451353086861e-8,
-            "velocityX": 2.2964461468641186,
-            "velocityY": 0.6918412341313931,
-            "timestamp": 0.2876419575994027
-        },
-        {
-            "x": 2.0839491699386645,
-            "y": 5.781113958575535,
-            "heading": 3.139999985983334,
-            "angularVelocity": 6.496408603804424e-9,
-            "velocityX": 3.0619279941465973,
-            "velocityY": 0.922454793178729,
-            "timestamp": 0.38352261013253697
-        },
-        {
-            "x": 2.431241902978436,
-            "y": 5.885741341465996,
-            "heading": 3.1399999859835575,
-            "angularVelocity": 2.3334348335212476e-12,
-            "velocityX": 3.6221356849840607,
-            "velocityY": 1.0912251859537307,
-            "timestamp": 0.47940326266567124
-        },
-        {
-            "x": 2.7785346564747577,
-            "y": 5.990368656458575,
-            "heading": 3.139999985983492,
-            "angularVelocity": -6.768139199697334e-13,
-            "velocityX": 3.62213589833913,
-            "velocityY": 1.0912244778026317,
-            "timestamp": 0.5752839151988055
-        },
-        {
-            "x": 3.125827431847794,
-            "y": 6.094995898834488,
-            "heading": 3.139999985983408,
-            "angularVelocity": -8.748780481443456e-13,
-            "velocityX": 3.6221361265054535,
-            "velocityY": 1.091223720437653,
-            "timestamp": 0.6711645677319398
-        },
-        {
-            "x": 3.473120229952072,
-            "y": 6.199623065757117,
-            "heading": 3.1399999859833065,
-            "angularVelocity": -1.0615094861925232e-12,
-            "velocityX": 3.6221363635841364,
-            "velocityY": 1.0912229334878236,
-            "timestamp": 0.767045220265074
-        },
-        {
-            "x": 3.820413051729017,
-            "y": 6.304250154101482,
-            "heading": 3.139999985983188,
-            "angularVelocity": -1.2288425429953676e-12,
-            "velocityX": 3.6221366104815065,
-            "velocityY": 1.0912221139455949,
-            "timestamp": 0.8629258727982083
-        },
-        {
-            "x": 4.167705898643341,
-            "y": 6.408877159005614,
-            "heading": 3.139999985983049,
-            "angularVelocity": -1.4178096575705385e-12,
-            "velocityX": 3.6221368726552323,
-            "velocityY": 1.0912212436948225,
-            "timestamp": 0.9588065253313426
-        },
-        {
-            "x": 4.5149987734968,
-            "y": 6.5135040711691525,
-            "heading": 3.139999985982883,
-            "angularVelocity": -1.676018359549876e-12,
-            "velocityX": 3.622137164050193,
-            "velocityY": 1.0912202764447858,
-            "timestamp": 1.0546871778644769
-        },
-        {
-            "x": 4.862291683371807,
-            "y": 6.6181308670796435,
-            "heading": 3.1399999859826795,
-            "angularVelocity": -2.066469238283253e-12,
-            "velocityX": 3.6221375293116886,
-            "velocityY": 1.0912190639696435,
-            "timestamp": 1.150567830397611
-        },
-        {
-            "x": 5.209584643601777,
-            "y": 6.722757495842404,
-            "heading": 3.139999985982697,
-            "angularVelocity": 1.7634954198261724e-13,
-            "velocityX": 3.6221380544925466,
-            "velocityY": 1.0912173206895408,
-            "timestamp": 1.2464484829307452
-        },
-        {
-            "x": 5.55687764566899,
-            "y": 6.827383985745101,
-            "heading": 3.139999985982843,
-            "angularVelocity": 1.4429880633934061e-12,
-            "velocityX": 3.6221384908382315,
-            "velocityY": 1.091215872433919,
-            "timestamp": 1.3423291354638793
-        },
-        {
-            "x": 5.904170680203765,
-            "y": 6.932010367880933,
-            "heading": 3.1399999859829473,
-            "angularVelocity": 9.965559645934633e-13,
-            "velocityX": 3.6221388294623984,
-            "velocityY": 1.0912147484660204,
-            "timestamp": 1.4382097879970135
-        },
-        {
-            "x": 6.251463742058712,
-            "y": 7.036636659333844,
-            "heading": 3.1399999859830086,
-            "angularVelocity": 5.792335690532441e-13,
-            "velocityX": 3.6221391144013104,
-            "velocityY": 1.0912138026768703,
-            "timestamp": 1.5340904405301476
-        },
-        {
-            "x": 6.598756826003656,
-            "y": 7.141262877462233,
-            "heading": 3.139999985983042,
-            "angularVelocity": 3.176331549030912e-13,
-            "velocityX": 3.6221393447915826,
-            "velocityY": 1.0912130379289529,
-            "timestamp": 1.6299710930632818
-        },
-        {
-            "x": 6.9460499272931315,
-            "y": 7.245889038012359,
-            "heading": 3.139999985982778,
-            "angularVelocity": -2.760419154137832e-12,
-            "velocityX": 3.622139525688641,
-            "velocityY": 1.0912124374075387,
-            "timestamp": 1.725851745596416
-        },
-        {
-            "x": 7.239629936933939,
-            "y": 7.334333425317839,
-            "heading": 3.139999987228266,
-            "angularVelocity": 1.3004483336236485e-8,
-            "velocityX": 3.061931702425989,
-            "velocityY": 0.9224424841673509,
-            "timestamp": 1.8217323981295501
-        },
-        {
-            "x": 7.459814962920636,
-            "y": 7.4006667150284455,
-            "heading": 3.13999999169634,
-            "angularVelocity": 4.661129161255315e-8,
-            "velocityX": 2.2964489724386126,
-            "velocityY": 0.6918318551100047,
-            "timestamp": 1.9176130506626843
-        },
-        {
-            "x": 7.606604986293984,
-            "y": 7.444888905913496,
-            "heading": 3.1399999966584766,
-            "angularVelocity": 5.176056299229228e-8,
-            "velocityX": 1.530966044713388,
-            "velocityY": 0.46122121321737414,
-            "timestamp": 2.0134937031958184
+            "x": 1.4233949207458116,
+            "y": 5.582111402652435,
+            "heading": -4.931478664026919e-9,
+            "angularVelocity": -5.143711867197603e-8,
+            "velocityX": 0.7654820738778157,
+            "velocityY": 0.2306138106917608,
+            "timestamp": 0.09588065253314748
+        },
+        {
+            "x": 1.570184760818058,
+            "y": 5.626334201974914,
+            "heading": -1.1368240805751946e-8,
+            "angularVelocity": -6.714026800936027e-8,
+            "velocityX": 1.530964132952106,
+            "velocityY": 0.4612275589893128,
+            "timestamp": 0.19176130506629496
+        },
+        {
+            "x": 1.7903695159222153,
+            "y": 5.69266839083479,
+            "heading": -1.6115736339739044e-8,
+            "angularVelocity": -4.952541704566216e-8,
+            "velocityX": 2.2964461472361695,
+            "velocityY": 0.6918412328958446,
+            "timestamp": 0.28764195759944244
+        },
+        {
+            "x": 2.0839491700576396,
+            "y": 5.781113958180788,
+            "heading": -1.639772094621516e-8,
+            "angularVelocity": -2.9553274583871895e-9,
+            "velocityX": 3.0619279946395954,
+            "velocityY": 0.9224547915413995,
+            "timestamp": 0.3835226101325899
+        },
+        {
+            "x": 2.431241903149783,
+            "y": 5.885741340897451,
+            "heading": -1.6397499367976412e-8,
+            "angularVelocity": 2.3168753954621105e-12,
+            "velocityX": 3.6221356855297673,
+            "velocityY": 1.0912251841409724,
+            "timestamp": 0.4794032626657374
+        },
+        {
+            "x": 2.7785346566980107,
+            "y": 5.9903686557177505,
+            "heading": -1.6397564221401302e-8,
+            "angularVelocity": -6.713496881725287e-13,
+            "velocityX": 3.622135898879975,
+            "velocityY": 1.091224476005724,
+            "timestamp": 0.5752839151988849
+        },
+        {
+            "x": 3.125827432121608,
+            "y": 6.094995897925851,
+            "heading": -1.6397647407961563e-8,
+            "angularVelocity": -8.684642063246534e-13,
+            "velocityX": 3.6221361270322743,
+            "velocityY": 1.0912237186873281,
+            "timestamp": 0.6711645677320324
+        },
+        {
+            "x": 3.4731202302782154,
+            "y": 6.1996230646748005,
+            "heading": -1.6397748264247437e-8,
+            "angularVelocity": -1.0543921850153982e-12,
+            "velocityX": 3.6221363641294033,
+            "velocityY": 1.0912229316763022,
+            "timestamp": 0.7670452202651799
+        },
+        {
+            "x": 3.8204130521112987,
+            "y": 6.304250152832848,
+            "heading": -1.639786614425739e-8,
+            "angularVelocity": -1.2203887930777277e-12,
+            "velocityX": 3.622136611066492,
+            "velocityY": 1.091222112002269,
+            "timestamp": 0.8629258727983273
+        },
+        {
+            "x": 4.167705899087819,
+            "y": 6.408877157530555,
+            "heading": -1.6398004207896958e-8,
+            "angularVelocity": -1.408074947723948e-12,
+            "velocityX": 3.6221368733034063,
+            "velocityY": 1.0912212415417761,
+            "timestamp": 0.9588065253314748
+        },
+        {
+            "x": 4.514998774010408,
+            "y": 6.51350406946466,
+            "heading": -1.6398168919050745e-8,
+            "angularVelocity": -1.6668913936591836e-12,
+            "velocityX": 3.6221371647706677,
+            "velocityY": 1.091220274051783,
+            "timestamp": 1.0546871778646223
+        },
+        {
+            "x": 4.862291683961819,
+            "y": 6.618130865121593,
+            "heading": -1.6398370762014746e-8,
+            "angularVelocity": -2.0513887673195245e-12,
+            "velocityX": 3.6221375301080334,
+            "velocityY": 1.0912190613250203,
+            "timestamp": 1.1505678303977698
+        },
+        {
+            "x": 5.20958464417323,
+            "y": 6.722757493946074,
+            "heading": -1.639835180923073e-8,
+            "angularVelocity": 1.902358989315915e-13,
+            "velocityX": 3.6221380542985027,
+            "velocityY": 1.091217321333091,
+            "timestamp": 1.2464484829309173
+        },
+        {
+            "x": 5.556877646148189,
+            "y": 6.8273839841549115,
+            "heading": -1.6398206125528607e-8,
+            "angularVelocity": 1.4352640935347951e-12,
+            "velocityX": 3.62213848987558,
+            "velocityY": 1.09121587562664,
+            "timestamp": 1.3423291354640647
+        },
+        {
+            "x": 5.904170680592406,
+            "y": 6.932010366591235,
+            "heading": -1.6398102572596455e-8,
+            "angularVelocity": 9.899977906647045e-13,
+            "velocityX": 3.6221388285174303,
+            "velocityY": 1.091214751599812,
+            "timestamp": 1.4382097879972122
+        },
+        {
+            "x": 6.2514637423704675,
+            "y": 7.036636658299249,
+            "heading": -1.6398041778119566e-8,
+            "angularVelocity": 5.750349690087699e-13,
+            "velocityX": 3.6221391135989474,
+            "velocityY": 1.0912138053372793,
+            "timestamp": 1.5340904405303597
+        },
+        {
+            "x": 6.5987568262419405,
+            "y": 7.141262876671409,
+            "heading": -1.639800860171266e-8,
+            "angularVelocity": 3.175769618903374e-13,
+            "velocityX": 3.6221393440248217,
+            "velocityY": 1.0912130404711837,
+            "timestamp": 1.6299710930635072
+        },
+        {
+            "x": 6.946049927457118,
+            "y": 7.2458890374680776,
+            "heading": -1.6398270316090924e-8,
+            "angularVelocity": -2.740394345840068e-12,
+            "velocityX": 3.6221395249132713,
+            "velocityY": 1.0912124399786907,
+            "timestamp": 1.7258517455966547
+        },
+        {
+            "x": 7.239629937032664,
+            "y": 7.334333424990155,
+            "heading": -1.4134914023195805e-8,
+            "angularVelocity": 2.36203418937418e-8,
+            "velocityX": 3.061931701744924,
+            "velocityY": 0.9224424864262022,
+            "timestamp": 1.8217323981298021
+        },
+        {
+            "x": 7.45981496297016,
+            "y": 7.4006667148640615,
+            "heading": -8.953329207671134e-9,
+            "angularVelocity": 5.4052849582725206e-8,
+            "velocityX": 2.2964489719251726,
+            "velocityY": 0.6918318568130147,
+            "timestamp": 1.9176130506629496
+        },
+        {
+            "x": 7.606604986310546,
+            "y": 7.444888905858524,
+            "heading": -3.5476510780655253e-9,
+            "angularVelocity": 5.638647309242438e-8,
+            "velocityX": 1.5309660443693864,
+            "velocityY": 0.4612212143584205,
+            "timestamp": 2.0134937031960973
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 3.485452160899378e-8,
-            "velocityX": 0.7654830434176825,
-            "velocityY": 0.23061059246584142,
-            "timestamp": 2.109374355728953
+            "heading": 1.0344705803481413e-31,
+            "angularVelocity": 3.7004322975715074e-8,
+            "velocityX": 0.7654830432448523,
+            "velocityY": 0.23061059303913045,
+            "timestamp": 2.109374355729245
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": -1.7922203019823127e-29,
+            "heading": 0,
+            "angularVelocity": -1.5335768309775317e-33,
             "velocityX": 0,
-            "velocityY": -5.07505109843192e-30,
-            "timestamp": 2.205255008262087
+            "velocityY": -2.5688294662733772e-30,
+            "timestamp": 2.2052550082623927
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.1.traj
index c5a01bdd..a76ce3f5 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.1.traj	
@@ -3,209 +3,209 @@
         {
             "x": 1.35,
             "y": 5.56,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
-            "velocityX": -2.32021994810819e-32,
+            "velocityX": 4.006684802425094e-34,
             "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 1.430775984981732,
-            "y": 5.581186654302167,
-            "heading": 3.138045514038262,
-            "angularVelocity": -0.019529788750833752,
-            "velocityX": 0.8071369934195644,
-            "velocityY": 0.2117031746247163,
-            "timestamp": 0.10007716860989323
-        },
-        {
-            "x": 1.592327952146185,
-            "y": 5.62355996139984,
-            "heading": 3.134136547157813,
-            "angularVelocity": -0.0390595271102885,
-            "velocityX": 1.6142739588706034,
-            "velocityY": 0.42340633419443324,
-            "timestamp": 0.20015433721978645
-        },
-        {
-            "x": 1.8346558957131356,
-            "y": 5.687119918958483,
-            "heading": 3.1282731070819607,
-            "angularVelocity": -0.05858918829621809,
-            "velocityX": 2.4214108665639773,
-            "velocityY": 0.6351094704367861,
-            "timestamp": 0.30023150582967967
-        },
-        {
-            "x": 2.1577597979432506,
-            "y": 5.771866521439831,
-            "heading": 3.1204552085550783,
-            "angularVelocity": -0.078118702152105,
-            "velocityX": 3.2285475970008037,
-            "velocityY": 0.8468125513392032,
-            "timestamp": 0.4003086744395729
-        },
-        {
-            "x": 2.523958853996556,
-            "y": 5.867916494702879,
-            "heading": 3.120455207736121,
-            "angularVelocity": -8.183261644434384e-9,
-            "velocityX": 3.659166832354859,
-            "velocityY": 0.9597590998747719,
-            "timestamp": 0.5003858430494661
-        },
-        {
-            "x": 2.890157912923332,
-            "y": 5.963966457010659,
-            "heading": 3.1204552069171707,
-            "angularVelocity": -8.18318468791684e-9,
-            "velocityX": 3.6591668610673986,
-            "velocityY": 0.9597589904065732,
-            "timestamp": 0.6004630116593593
-        },
-        {
-            "x": 3.256356975588488,
-            "y": 6.06001640506554,
-            "heading": 3.1204552060982262,
-            "angularVelocity": -8.183132612245063e-9,
-            "velocityX": 3.659166898422379,
-            "velocityY": 0.9597588479874845,
-            "timestamp": 0.7005401802692526
-        },
-        {
-            "x": 3.6225560432776467,
-            "y": 6.156066333965972,
-            "heading": 3.1204552052792867,
-            "angularVelocity": -8.183078308583848e-9,
-            "velocityX": 3.6591669486236644,
-            "velocityY": 0.9597586565906886,
-            "timestamp": 0.8006173488791458
-        },
-        {
-            "x": 3.9887551180209555,
-            "y": 6.252116235971829,
-            "heading": 3.1204552044603537,
-            "angularVelocity": -8.183012061558546e-9,
-            "velocityX": 3.659167019110774,
-            "velocityY": 0.9597583878523045,
-            "timestamp": 0.9006945174890391
-        },
-        {
-            "x": 4.354954203462548,
-            "y": 6.348166097189504,
-            "heading": 3.1204552036414284,
-            "angularVelocity": -8.182937495729244e-9,
-            "velocityX": 3.659167126011114,
-            "velocityY": 0.9597579802850231,
-            "timestamp": 1.0007716860989322
-        },
-        {
-            "x": 4.721153326841163,
-            "y": 6.444215813768638,
-            "heading": 3.120455202822509,
-            "angularVelocity": -8.182878073364452e-9,
-            "velocityX": 3.659167505088806,
-            "velocityY": 0.9597565350148991,
-            "timestamp": 1.1008488547088253
-        },
-        {
-            "x": 5.092057106129702,
-            "y": 6.520095539851432,
-            "heading": 3.1204551978641732,
-            "angularVelocity": -4.954512725191857e-8,
-            "velocityX": 3.7061777870070114,
-            "velocityY": 0.7582121590446533,
-            "timestamp": 1.2009260233187184
-        },
-        {
-            "x": 5.470573222615853,
-            "y": 6.51282289206487,
-            "heading": 3.120455142790982,
-            "angularVelocity": -5.503072477147126e-7,
-            "velocityX": 3.7822424609316347,
-            "velocityY": -0.07267039912792687,
-            "timestamp": 1.3010031919286116
-        },
-        {
-            "x": 5.849058511568644,
-            "y": 6.504092056642741,
-            "heading": 3.1204550877725445,
-            "angularVelocity": -5.497601318399077e-7,
-            "velocityX": 3.7819344233063643,
-            "velocityY": -0.08724103153050607,
-            "timestamp": 1.4010803605385047
-        },
-        {
-            "x": 6.221366644651584,
-            "y": 6.4354334883920234,
-            "heading": 3.1204550635505535,
-            "angularVelocity": -2.420331372889176e-7,
-            "velocityX": 3.720210496104441,
-            "velocityY": -0.6860562624264548,
-            "timestamp": 1.5011575291483978
-        },
-        {
-            "x": 6.569565662964383,
-            "y": 6.286823971409196,
-            "heading": 3.1204550644036275,
-            "angularVelocity": 8.52416239462205e-9,
-            "velocityX": 3.479305251631334,
-            "velocityY": -1.4849492551305274,
-            "timestamp": 1.601234697758291
-        },
-        {
-            "x": 6.915949214018099,
-            "y": 6.134030710794496,
-            "heading": 3.1204550652542946,
-            "angularVelocity": 8.500112010852639e-9,
-            "velocityX": 3.4611645779462337,
-            "velocityY": -1.5267544309761423,
-            "timestamp": 1.701311866368184
-        },
-        {
-            "x": 7.22156951411921,
-            "y": 5.999218440367679,
-            "heading": 3.1282730490164057,
-            "angularVelocity": 0.07811955384714984,
-            "velocityX": 3.0538463902035207,
-            "velocityY": -1.347083178904912,
-            "timestamp": 1.8013890349780772
-        },
-        {
-            "x": 7.4507847539347845,
-            "y": 5.898109225088713,
-            "heading": 3.1341365308407623,
-            "angularVelocity": 0.05858960545933885,
-            "velocityX": 2.290384939936379,
-            "velocityY": -1.010312508671161,
-            "timestamp": 1.9014662035879704
-        },
-        {
-            "x": 7.603594917403085,
-            "y": 5.830703076393403,
-            "heading": 3.1380455124480764,
-            "angularVelocity": 0.03905967426538248,
-            "velocityX": 1.526923329175751,
-            "velocityY": -0.6735417241675008,
-            "timestamp": 2.0015433721978635
+            "x": 1.4307759237486215,
+            "y": 5.581186236587336,
+            "heading": -0.002104963011526108,
+            "angularVelocity": -0.021033383553861286,
+            "velocityX": 0.8071357913745165,
+            "velocityY": 0.21169884590096585,
+            "timestamp": 0.1000772417873624
+        },
+        {
+            "x": 1.592327768448065,
+            "y": 5.623558708255768,
+            "heading": -0.006314883423264977,
+            "angularVelocity": -0.04206671103789844,
+            "velocityX": 1.6142715547926287,
+            "velocityY": 0.42339767675114676,
+            "timestamp": 0.2001544835747248
+        },
+        {
+            "x": 1.8346555283201378,
+            "y": 5.6871174126709505,
+            "heading": -0.012629752552513055,
+            "angularVelocity": -0.06309995176191506,
+            "velocityX": 2.4214072604734147,
+            "velocityY": 0.6350964842758995,
+            "timestamp": 0.3002317253620872
+        },
+        {
+            "x": 2.1577591856288034,
+            "y": 5.77186234429474,
+            "heading": -0.02104955396970995,
+            "angularVelocity": -0.08413302831713466,
+            "velocityX": 3.228542788930726,
+            "velocityY": 0.8467952364619561,
+            "timestamp": 0.4003089671494496
+        },
+        {
+            "x": 2.5239589562380984,
+            "y": 5.867910684343787,
+            "heading": -0.021049554851793276,
+            "angularVelocity": -8.814025154574047e-9,
+            "velocityX": 3.6591712967806616,
+            "velocityY": 0.9597420785549242,
+            "timestamp": 0.500386208936812
+        },
+        {
+            "x": 2.8901587297210503,
+            "y": 5.963959013436644,
+            "heading": -0.021049555733868682,
+            "angularVelocity": -8.813945991956408e-9,
+            "velocityX": 3.659171325495056,
+            "velocityY": 0.9597419690775807,
+            "timestamp": 0.6004634507241744
+        },
+        {
+            "x": 3.2563585069426413,
+            "y": 6.060007328275347,
+            "heading": -0.02104955661593894,
+            "angularVelocity": -8.813894597525165e-9,
+            "velocityX": 3.659171362852588,
+            "velocityY": 0.9597418266460623,
+            "timestamp": 0.7005406925115368
+        },
+        {
+            "x": 3.622558289188601,
+            "y": 6.15605562395784,
+            "heading": -0.021049557498003604,
+            "angularVelocity": -8.813838677794314e-9,
+            "velocityX": 3.6591714130574977,
+            "velocityY": 0.9597416352318217,
+            "timestamp": 0.8006179342988993
+        },
+        {
+            "x": 3.9887580784892815,
+            "y": 6.252103892743073,
+            "heading": -0.02104955838006166,
+            "angularVelocity": -8.81377261667878e-9,
+            "velocityX": 3.659171483550255,
+            "velocityY": 0.9597413664668089,
+            "timestamp": 0.9006951760862617
+        },
+        {
+            "x": 4.354957878489299,
+            "y": 6.3481521207353335,
+            "heading": -0.021049559262112345,
+            "angularVelocity": -8.813699036774153e-9,
+            "velocityX": 3.6591715904610522,
+            "velocityY": 0.9597409588519333,
+            "timestamp": 1.0007724178736241
+        },
+        {
+            "x": 4.721157716449094,
+            "y": 6.444200203999558,
+            "heading": -0.021049560144156942,
+            "angularVelocity": -8.81363814954091e-9,
+            "velocityX": 3.659171969765827,
+            "velocityY": 0.9597395126886197,
+            "timestamp": 1.1008496596609865
+        },
+        {
+            "x": 5.092065894328832,
+            "y": 6.520059807846628,
+            "heading": -0.02104956519242719,
+            "angularVelocity": -5.0443738888274115e-8,
+            "velocityX": 3.70621902897584,
+            "velocityY": 0.7580105375830706,
+            "timestamp": 1.2009269014483488
+        },
+        {
+            "x": 5.47058189966855,
+            "y": 6.512767035208857,
+            "heading": -0.021049620221549888,
+            "angularVelocity": -5.498665002090286e-7,
+            "velocityX": 3.7822385847120277,
+            "velocityY": -0.07287143917560433,
+            "timestamp": 1.3010041432357111
+        },
+        {
+            "x": 5.849069453983326,
+            "y": 6.50412284194313,
+            "heading": -0.02104967520004647,
+            "angularVelocity": -5.493606298291521e-7,
+            "velocityX": 3.7819542940538056,
+            "velocityY": -0.08637521489745259,
+            "timestamp": 1.4010813850230734
+        },
+        {
+            "x": 6.2213754396920145,
+            "y": 6.435451104158958,
+            "heading": -0.021049699334872447,
+            "angularVelocity": -2.4116198195152956e-7,
+            "velocityX": 3.720186318681132,
+            "velocityY": -0.6861873544644725,
+            "timestamp": 1.5011586268104358
+        },
+        {
+            "x": 6.5695694546238315,
+            "y": 6.286829159405189,
+            "heading": -0.02104969841623417,
+            "angularVelocity": 9.179292491243133e-9,
+            "velocityX": 3.4792527123362977,
+            "velocityY": -1.4850723511100814,
+            "timestamp": 1.601235868597798
+        },
+        {
+            "x": 6.915952468622566,
+            "y": 6.134033995374984,
+            "heading": -0.021049697500180466,
+            "angularVelocity": 9.153466759383175e-9,
+            "velocityX": 3.4611566807037675,
+            "velocityY": -1.5267723340622583,
+            "timestamp": 1.7013131103851604
+        },
+        {
+            "x": 7.221571466882516,
+            "y": 5.999220411113471,
+            "heading": -0.012629808861323527,
+            "angularVelocity": 0.0841338998605394,
+            "velocityX": 3.053831148837094,
+            "velocityY": -1.347095322110863,
+            "timestamp": 1.8013903521725227
+        },
+        {
+            "x": 7.450785730315982,
+            "y": 5.898110210460681,
+            "heading": -0.0063148981970481905,
+            "angularVelocity": 0.0631003667916113,
+            "velocityX": 2.290373508899117,
+            "velocityY": -1.0103216160535444,
+            "timestamp": 1.901467593959885
+        },
+        {
+            "x": 7.603595242863294,
+            "y": 5.830703404850431,
+            "heading": -0.0021049639599730564,
+            "angularVelocity": 0.04206684918455414,
+            "velocityX": 1.5269157084883698,
+            "velocityY": -0.673547795746317,
+            "timestamp": 2.0015448357472474
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 0.019529804640431036,
-            "velocityX": 0.7634616732088615,
-            "velocityY": -0.33677088252547477,
-            "timestamp": 2.101620540807757
+            "heading": 0,
+            "angularVelocity": 0.02103339303101045,
+            "velocityX": 0.7634578628680123,
+            "velocityY": -0.33677391831044023,
+            "timestamp": 2.1016220775346097
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
-            "velocityX": 2.2237121345666e-32,
+            "velocityX": 1.7450911582816108e-33,
             "velocityY": 0,
-            "timestamp": 2.20169770941765
+            "timestamp": 2.201699319321972
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.traj
index c5a01bdd..a76ce3f5 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.traj	
@@ -3,209 +3,209 @@
         {
             "x": 1.35,
             "y": 5.56,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
-            "velocityX": -2.32021994810819e-32,
+            "velocityX": 4.006684802425094e-34,
             "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 1.430775984981732,
-            "y": 5.581186654302167,
-            "heading": 3.138045514038262,
-            "angularVelocity": -0.019529788750833752,
-            "velocityX": 0.8071369934195644,
-            "velocityY": 0.2117031746247163,
-            "timestamp": 0.10007716860989323
-        },
-        {
-            "x": 1.592327952146185,
-            "y": 5.62355996139984,
-            "heading": 3.134136547157813,
-            "angularVelocity": -0.0390595271102885,
-            "velocityX": 1.6142739588706034,
-            "velocityY": 0.42340633419443324,
-            "timestamp": 0.20015433721978645
-        },
-        {
-            "x": 1.8346558957131356,
-            "y": 5.687119918958483,
-            "heading": 3.1282731070819607,
-            "angularVelocity": -0.05858918829621809,
-            "velocityX": 2.4214108665639773,
-            "velocityY": 0.6351094704367861,
-            "timestamp": 0.30023150582967967
-        },
-        {
-            "x": 2.1577597979432506,
-            "y": 5.771866521439831,
-            "heading": 3.1204552085550783,
-            "angularVelocity": -0.078118702152105,
-            "velocityX": 3.2285475970008037,
-            "velocityY": 0.8468125513392032,
-            "timestamp": 0.4003086744395729
-        },
-        {
-            "x": 2.523958853996556,
-            "y": 5.867916494702879,
-            "heading": 3.120455207736121,
-            "angularVelocity": -8.183261644434384e-9,
-            "velocityX": 3.659166832354859,
-            "velocityY": 0.9597590998747719,
-            "timestamp": 0.5003858430494661
-        },
-        {
-            "x": 2.890157912923332,
-            "y": 5.963966457010659,
-            "heading": 3.1204552069171707,
-            "angularVelocity": -8.18318468791684e-9,
-            "velocityX": 3.6591668610673986,
-            "velocityY": 0.9597589904065732,
-            "timestamp": 0.6004630116593593
-        },
-        {
-            "x": 3.256356975588488,
-            "y": 6.06001640506554,
-            "heading": 3.1204552060982262,
-            "angularVelocity": -8.183132612245063e-9,
-            "velocityX": 3.659166898422379,
-            "velocityY": 0.9597588479874845,
-            "timestamp": 0.7005401802692526
-        },
-        {
-            "x": 3.6225560432776467,
-            "y": 6.156066333965972,
-            "heading": 3.1204552052792867,
-            "angularVelocity": -8.183078308583848e-9,
-            "velocityX": 3.6591669486236644,
-            "velocityY": 0.9597586565906886,
-            "timestamp": 0.8006173488791458
-        },
-        {
-            "x": 3.9887551180209555,
-            "y": 6.252116235971829,
-            "heading": 3.1204552044603537,
-            "angularVelocity": -8.183012061558546e-9,
-            "velocityX": 3.659167019110774,
-            "velocityY": 0.9597583878523045,
-            "timestamp": 0.9006945174890391
-        },
-        {
-            "x": 4.354954203462548,
-            "y": 6.348166097189504,
-            "heading": 3.1204552036414284,
-            "angularVelocity": -8.182937495729244e-9,
-            "velocityX": 3.659167126011114,
-            "velocityY": 0.9597579802850231,
-            "timestamp": 1.0007716860989322
-        },
-        {
-            "x": 4.721153326841163,
-            "y": 6.444215813768638,
-            "heading": 3.120455202822509,
-            "angularVelocity": -8.182878073364452e-9,
-            "velocityX": 3.659167505088806,
-            "velocityY": 0.9597565350148991,
-            "timestamp": 1.1008488547088253
-        },
-        {
-            "x": 5.092057106129702,
-            "y": 6.520095539851432,
-            "heading": 3.1204551978641732,
-            "angularVelocity": -4.954512725191857e-8,
-            "velocityX": 3.7061777870070114,
-            "velocityY": 0.7582121590446533,
-            "timestamp": 1.2009260233187184
-        },
-        {
-            "x": 5.470573222615853,
-            "y": 6.51282289206487,
-            "heading": 3.120455142790982,
-            "angularVelocity": -5.503072477147126e-7,
-            "velocityX": 3.7822424609316347,
-            "velocityY": -0.07267039912792687,
-            "timestamp": 1.3010031919286116
-        },
-        {
-            "x": 5.849058511568644,
-            "y": 6.504092056642741,
-            "heading": 3.1204550877725445,
-            "angularVelocity": -5.497601318399077e-7,
-            "velocityX": 3.7819344233063643,
-            "velocityY": -0.08724103153050607,
-            "timestamp": 1.4010803605385047
-        },
-        {
-            "x": 6.221366644651584,
-            "y": 6.4354334883920234,
-            "heading": 3.1204550635505535,
-            "angularVelocity": -2.420331372889176e-7,
-            "velocityX": 3.720210496104441,
-            "velocityY": -0.6860562624264548,
-            "timestamp": 1.5011575291483978
-        },
-        {
-            "x": 6.569565662964383,
-            "y": 6.286823971409196,
-            "heading": 3.1204550644036275,
-            "angularVelocity": 8.52416239462205e-9,
-            "velocityX": 3.479305251631334,
-            "velocityY": -1.4849492551305274,
-            "timestamp": 1.601234697758291
-        },
-        {
-            "x": 6.915949214018099,
-            "y": 6.134030710794496,
-            "heading": 3.1204550652542946,
-            "angularVelocity": 8.500112010852639e-9,
-            "velocityX": 3.4611645779462337,
-            "velocityY": -1.5267544309761423,
-            "timestamp": 1.701311866368184
-        },
-        {
-            "x": 7.22156951411921,
-            "y": 5.999218440367679,
-            "heading": 3.1282730490164057,
-            "angularVelocity": 0.07811955384714984,
-            "velocityX": 3.0538463902035207,
-            "velocityY": -1.347083178904912,
-            "timestamp": 1.8013890349780772
-        },
-        {
-            "x": 7.4507847539347845,
-            "y": 5.898109225088713,
-            "heading": 3.1341365308407623,
-            "angularVelocity": 0.05858960545933885,
-            "velocityX": 2.290384939936379,
-            "velocityY": -1.010312508671161,
-            "timestamp": 1.9014662035879704
-        },
-        {
-            "x": 7.603594917403085,
-            "y": 5.830703076393403,
-            "heading": 3.1380455124480764,
-            "angularVelocity": 0.03905967426538248,
-            "velocityX": 1.526923329175751,
-            "velocityY": -0.6735417241675008,
-            "timestamp": 2.0015433721978635
+            "x": 1.4307759237486215,
+            "y": 5.581186236587336,
+            "heading": -0.002104963011526108,
+            "angularVelocity": -0.021033383553861286,
+            "velocityX": 0.8071357913745165,
+            "velocityY": 0.21169884590096585,
+            "timestamp": 0.1000772417873624
+        },
+        {
+            "x": 1.592327768448065,
+            "y": 5.623558708255768,
+            "heading": -0.006314883423264977,
+            "angularVelocity": -0.04206671103789844,
+            "velocityX": 1.6142715547926287,
+            "velocityY": 0.42339767675114676,
+            "timestamp": 0.2001544835747248
+        },
+        {
+            "x": 1.8346555283201378,
+            "y": 5.6871174126709505,
+            "heading": -0.012629752552513055,
+            "angularVelocity": -0.06309995176191506,
+            "velocityX": 2.4214072604734147,
+            "velocityY": 0.6350964842758995,
+            "timestamp": 0.3002317253620872
+        },
+        {
+            "x": 2.1577591856288034,
+            "y": 5.77186234429474,
+            "heading": -0.02104955396970995,
+            "angularVelocity": -0.08413302831713466,
+            "velocityX": 3.228542788930726,
+            "velocityY": 0.8467952364619561,
+            "timestamp": 0.4003089671494496
+        },
+        {
+            "x": 2.5239589562380984,
+            "y": 5.867910684343787,
+            "heading": -0.021049554851793276,
+            "angularVelocity": -8.814025154574047e-9,
+            "velocityX": 3.6591712967806616,
+            "velocityY": 0.9597420785549242,
+            "timestamp": 0.500386208936812
+        },
+        {
+            "x": 2.8901587297210503,
+            "y": 5.963959013436644,
+            "heading": -0.021049555733868682,
+            "angularVelocity": -8.813945991956408e-9,
+            "velocityX": 3.659171325495056,
+            "velocityY": 0.9597419690775807,
+            "timestamp": 0.6004634507241744
+        },
+        {
+            "x": 3.2563585069426413,
+            "y": 6.060007328275347,
+            "heading": -0.02104955661593894,
+            "angularVelocity": -8.813894597525165e-9,
+            "velocityX": 3.659171362852588,
+            "velocityY": 0.9597418266460623,
+            "timestamp": 0.7005406925115368
+        },
+        {
+            "x": 3.622558289188601,
+            "y": 6.15605562395784,
+            "heading": -0.021049557498003604,
+            "angularVelocity": -8.813838677794314e-9,
+            "velocityX": 3.6591714130574977,
+            "velocityY": 0.9597416352318217,
+            "timestamp": 0.8006179342988993
+        },
+        {
+            "x": 3.9887580784892815,
+            "y": 6.252103892743073,
+            "heading": -0.02104955838006166,
+            "angularVelocity": -8.81377261667878e-9,
+            "velocityX": 3.659171483550255,
+            "velocityY": 0.9597413664668089,
+            "timestamp": 0.9006951760862617
+        },
+        {
+            "x": 4.354957878489299,
+            "y": 6.3481521207353335,
+            "heading": -0.021049559262112345,
+            "angularVelocity": -8.813699036774153e-9,
+            "velocityX": 3.6591715904610522,
+            "velocityY": 0.9597409588519333,
+            "timestamp": 1.0007724178736241
+        },
+        {
+            "x": 4.721157716449094,
+            "y": 6.444200203999558,
+            "heading": -0.021049560144156942,
+            "angularVelocity": -8.81363814954091e-9,
+            "velocityX": 3.659171969765827,
+            "velocityY": 0.9597395126886197,
+            "timestamp": 1.1008496596609865
+        },
+        {
+            "x": 5.092065894328832,
+            "y": 6.520059807846628,
+            "heading": -0.02104956519242719,
+            "angularVelocity": -5.0443738888274115e-8,
+            "velocityX": 3.70621902897584,
+            "velocityY": 0.7580105375830706,
+            "timestamp": 1.2009269014483488
+        },
+        {
+            "x": 5.47058189966855,
+            "y": 6.512767035208857,
+            "heading": -0.021049620221549888,
+            "angularVelocity": -5.498665002090286e-7,
+            "velocityX": 3.7822385847120277,
+            "velocityY": -0.07287143917560433,
+            "timestamp": 1.3010041432357111
+        },
+        {
+            "x": 5.849069453983326,
+            "y": 6.50412284194313,
+            "heading": -0.02104967520004647,
+            "angularVelocity": -5.493606298291521e-7,
+            "velocityX": 3.7819542940538056,
+            "velocityY": -0.08637521489745259,
+            "timestamp": 1.4010813850230734
+        },
+        {
+            "x": 6.2213754396920145,
+            "y": 6.435451104158958,
+            "heading": -0.021049699334872447,
+            "angularVelocity": -2.4116198195152956e-7,
+            "velocityX": 3.720186318681132,
+            "velocityY": -0.6861873544644725,
+            "timestamp": 1.5011586268104358
+        },
+        {
+            "x": 6.5695694546238315,
+            "y": 6.286829159405189,
+            "heading": -0.02104969841623417,
+            "angularVelocity": 9.179292491243133e-9,
+            "velocityX": 3.4792527123362977,
+            "velocityY": -1.4850723511100814,
+            "timestamp": 1.601235868597798
+        },
+        {
+            "x": 6.915952468622566,
+            "y": 6.134033995374984,
+            "heading": -0.021049697500180466,
+            "angularVelocity": 9.153466759383175e-9,
+            "velocityX": 3.4611566807037675,
+            "velocityY": -1.5267723340622583,
+            "timestamp": 1.7013131103851604
+        },
+        {
+            "x": 7.221571466882516,
+            "y": 5.999220411113471,
+            "heading": -0.012629808861323527,
+            "angularVelocity": 0.0841338998605394,
+            "velocityX": 3.053831148837094,
+            "velocityY": -1.347095322110863,
+            "timestamp": 1.8013903521725227
+        },
+        {
+            "x": 7.450785730315982,
+            "y": 5.898110210460681,
+            "heading": -0.0063148981970481905,
+            "angularVelocity": 0.0631003667916113,
+            "velocityX": 2.290373508899117,
+            "velocityY": -1.0103216160535444,
+            "timestamp": 1.901467593959885
+        },
+        {
+            "x": 7.603595242863294,
+            "y": 5.830703404850431,
+            "heading": -0.0021049639599730564,
+            "angularVelocity": 0.04206684918455414,
+            "velocityX": 1.5269157084883698,
+            "velocityY": -0.673547795746317,
+            "timestamp": 2.0015448357472474
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 0.019529804640431036,
-            "velocityX": 0.7634616732088615,
-            "velocityY": -0.33677088252547477,
-            "timestamp": 2.101620540807757
+            "heading": 0,
+            "angularVelocity": 0.02103339303101045,
+            "velocityX": 0.7634578628680123,
+            "velocityY": -0.33677391831044023,
+            "timestamp": 2.1016220775346097
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
-            "velocityX": 2.2237121345666e-32,
+            "velocityX": 1.7450911582816108e-33,
             "velocityY": 0,
-            "timestamp": 2.20169770941765
+            "timestamp": 2.201699319321972
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.1.traj
index 92207f04..f95c4186 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.1.traj	
@@ -3,1091 +3,1091 @@
         {
             "x": 1.35,
             "y": 5.56,
-            "heading": 3.14,
+            "heading": 4.408218114222724e-33,
             "angularVelocity": 0,
-            "velocityX": 4.411137018023184e-34,
-            "velocityY": 2.784782344800776e-32,
+            "velocityX": 1.8935205228973059e-34,
+            "velocityY": 9.513896878368867e-32,
             "timestamp": 0
         },
         {
-            "x": 1.3544641699489606,
-            "y": 5.55925259826767,
-            "heading": 3.1400000009005375,
-            "angularVelocity": 3.865132870171629e-8,
-            "velocityX": 0.19160342420680077,
-            "velocityY": -0.032078691629102596,
-            "timestamp": 0.023299009229303495
-        },
-        {
-            "x": 1.3633925116373848,
-            "y": 5.557757806179583,
-            "heading": 3.1400000027300097,
-            "angularVelocity": 7.852144487332144e-8,
-            "velocityX": 0.383206925262499,
-            "velocityY": -0.06415689497258387,
-            "timestamp": 0.04659801845860699
-        },
-        {
-            "x": 1.3767850270669844,
-            "y": 5.555515636454451,
-            "heading": 3.140000005520194,
-            "angularVelocity": 1.1975551924808857e-7,
-            "velocityX": 0.5748105122322142,
-            "velocityY": -0.09623455242535102,
-            "timestamp": 0.06989702768791048
-        },
-        {
-            "x": 1.3946417184903905,
-            "y": 5.55252610340551,
-            "heading": 3.1400000093068963,
-            "angularVelocity": 1.6252630014020525e-7,
-            "velocityX": 0.7664141958854979,
-            "velocityY": -0.12831159554996177,
-            "timestamp": 0.09319603691721398
-        },
-        {
-            "x": 1.4169625884614865,
-            "y": 5.548789223260415,
-            "heading": 3.14000001413076,
-            "angularVelocity": 2.070415703537556e-7,
-            "velocityX": 0.9580179891521703,
-            "velocityY": -0.16038794217918884,
-            "timestamp": 0.11649504614651748
-        },
-        {
-            "x": 1.443747639900165,
-            "y": 5.544305014572816,
-            "heading": 3.1400000200383236,
-            "angularVelocity": 2.5355430687602407e-7,
-            "velocityX": 1.1496219077414547,
-            "velocityY": -0.1924634924804626,
-            "timestamp": 0.13979405537582099
-        },
-        {
-            "x": 1.474996876177077,
-            "y": 5.539073498761034,
-            "heading": 3.1400000270834036,
-            "angularVelocity": 3.0237681109940727e-7,
-            "velocityX": 1.3412259710000187,
-            "velocityY": -0.22453812350142868,
-            "timestamp": 0.1630930646051245
-        },
-        {
-            "x": 1.5107103012267313,
-            "y": 5.5330947008269575,
-            "heading": 3.1400000353289426,
-            "angularVelocity": 3.539008296544223e-7,
-            "velocityX": 1.5328302031287921,
-            "velocityY": -0.25661168143439866,
-            "timestamp": 0.186392073834428
-        },
-        {
-            "x": 1.5508879197018797,
-            "y": 5.526368650337421,
-            "heading": 3.140000044849547,
-            "angularVelocity": 4.086269951598913e-7,
-            "velocityX": 1.7244346349549924,
-            "velocityY": -0.2886839703505207,
-            "timestamp": 0.2096910830637315
-        },
-        {
-            "x": 1.5955297371899198,
-            "y": 5.5188953827998875,
-            "heading": 3.1400000557350607,
-            "angularVelocity": 4.6720929680542457e-7,
-            "velocityX": 1.9160393065938472,
-            "velocityY": -0.3207547352763359,
-            "timestamp": 0.232990092293035
-        },
-        {
-            "x": 1.6446357605259214,
-            "y": 5.510674941652546,
-            "heading": 3.140000068095764,
-            "angularVelocity": 5.305248328425052e-7,
-            "velocityX": 2.107644271595818,
-            "velocityY": -0.3528236358230067,
-            "timestamp": 0.25628910152233847
-        },
-        {
-            "x": 1.6982059982629796,
-            "y": 5.501707381254886,
-            "heading": 3.1400000820702374,
-            "angularVelocity": 5.997883043629641e-7,
-            "velocityX": 2.299249603699023,
-            "velocityY": -0.38489020324511874,
-            "timestamp": 0.27958811075164197
-        },
-        {
-            "x": 1.7562404614129117,
-            "y": 5.491992771597749,
-            "heading": 3.1400000978378597,
-            "angularVelocity": 6.767507519595272e-7,
-            "velocityX": 2.490855408432506,
-            "velocityY": -0.41695376664068867,
-            "timestamp": 0.3028871199809455
-        },
-        {
-            "x": 1.8187391646840712,
-            "y": 5.4815312061757675,
-            "heading": 3.140000115639928,
-            "angularVelocity": 7.640697380946438e-7,
-            "velocityX": 2.6824618444526434,
-            "velocityY": -0.4490133172205749,
-            "timestamp": 0.326186129210249
-        },
-        {
-            "x": 1.8857021287172593,
-            "y": 5.47032281621074,
-            "heading": 3.140000135818395,
-            "angularVelocity": 8.660654300433757e-7,
-            "velocityX": 2.874069166381749,
-            "velocityY": -0.48106723572405463,
-            "timestamp": 0.3494851384395525
-        },
-        {
-            "x": 1.9571293845792233,
-            "y": 5.458367799245384,
-            "heading": 3.14000015889542,
-            "angularVelocity": 9.904723735244152e-7,
-            "velocityX": 3.0656778217043392,
-            "velocityY": -0.5131126756388134,
-            "timestamp": 0.372784147668856
-        },
-        {
-            "x": 2.033020984323074,
-            "y": 5.445666486377279,
-            "heading": 3.140000185766696,
-            "angularVelocity": 0.000001153322655289093,
-            "velocityX": 3.2572887111610362,
-            "velocityY": -0.545143904752752,
-            "timestamp": 0.3960831568981595
-        },
-        {
-            "x": 2.113377033060567,
-            "y": 5.432219546645115,
-            "heading": 3.1400002183250084,
-            "angularVelocity": 0.0000013974118596484493,
-            "velocityX": 3.448904112043669,
-            "velocityY": -0.5771464185388335,
-            "timestamp": 0.419382166127463
-        },
-        {
-            "x": 2.1981978506136755,
-            "y": 5.41802904044786,
-            "heading": 3.1400002631776545,
-            "angularVelocity": 0.0000019250880740696254,
-            "velocityX": 3.640533239775037,
-            "velocityY": -0.6090604994243719,
-            "timestamp": 0.4426811753567665
-        },
-        {
-            "x": 2.2851188032420633,
-            "y": 5.403428173451014,
-            "heading": 3.140000263177585,
-            "angularVelocity": -2.9753453619857404e-12,
-            "velocityX": 3.73067162525748,
-            "velocityY": -0.6266732998482398,
-            "timestamp": 0.46598018458607
-        },
-        {
-            "x": 2.372043080680341,
-            "y": 5.388847120989839,
-            "heading": 3.140000263177669,
-            "angularVelocity": 3.609665835664611e-12,
-            "velocityX": 3.730814327029464,
-            "velocityY": -0.6258228544259767,
-            "timestamp": 0.4892791938153735
-        },
-        {
-            "x": 2.458969517114403,
-            "y": 5.374278945126162,
-            "heading": 3.140000263177672,
-            "angularVelocity": 1.1048975247425585e-13,
-            "velocityX": 3.7309069917332867,
-            "velocityY": -0.6252701872544597,
-            "timestamp": 0.5125782030446769
-        },
-        {
-            "x": 2.5458971210754204,
-            "y": 5.359717737461923,
-            "heading": 3.1400002631776736,
-            "angularVelocity": 8.62983817390415e-14,
-            "velocityX": 3.7309571023169674,
-            "velocityY": -0.6249711101842599,
-            "timestamp": 0.5358772122739804
-        },
-        {
-            "x": 2.632825281306951,
-            "y": 5.345159851008328,
-            "heading": 3.140000263177678,
-            "angularVelocity": 1.7012303031669384e-13,
-            "velocityX": 3.7309809776027962,
-            "velocityY": -0.6248285628956001,
-            "timestamp": 0.5591762215032839
-        },
-        {
-            "x": 2.7197537163641496,
-            "y": 5.330603605663881,
-            "heading": 3.1400002631776815,
-            "angularVelocity": 1.5893105684431692e-13,
-            "velocityX": 3.7309927731970562,
-            "velocityY": -0.6247581260336735,
-            "timestamp": 0.5824752307325874
-        },
-        {
-            "x": 2.8066823211500065,
-            "y": 5.316048373923756,
-            "heading": 3.140000263177685,
-            "angularVelocity": 1.4661255756791518e-13,
-            "velocityX": 3.7310000579983225,
-            "velocityY": -0.6247146218539804,
-            "timestamp": 0.605774239961891
-        },
-        {
-            "x": 2.8936112175364808,
-            "y": 5.301494883804396,
-            "heading": 3.140000263177688,
-            "angularVelocity": 1.4712645864153266e-13,
-            "velocityX": 3.731012573578327,
-            "velocityY": -0.6246398710039542,
-            "timestamp": 0.6290732491911945
-        },
-        {
-            "x": 2.9805406318812673,
-            "y": 5.286944487835434,
-            "heading": 3.1400002631776913,
-            "angularVelocity": 1.2989281986974027e-13,
-            "velocityX": 3.7310348044942168,
-            "velocityY": -0.6245070692018166,
-            "timestamp": 0.652372258420498
-        },
-        {
-            "x": 3.067470472751592,
-            "y": 5.272396640371872,
-            "heading": 3.140000263177696,
-            "angularVelocity": 2.2497040560944553e-13,
-            "velocityX": 3.7310531110902496,
-            "velocityY": -0.6243976866335479,
-            "timestamp": 0.6756712676498015
-        },
-        {
-            "x": 3.1544000489958055,
-            "y": 5.257847211710117,
-            "heading": 3.1400002631777,
-            "angularVelocity": 1.6698909622359586e-13,
-            "velocityX": 3.73104175326474,
-            "velocityY": -0.6244655520950864,
-            "timestamp": 0.698970276879105
-        },
-        {
-            "x": 3.24132763546162,
-            "y": 5.243285899348815,
-            "heading": 3.1400002631776998,
-            "angularVelocity": -2.5660759099238358e-14,
-            "velocityX": 3.7309563514197523,
-            "velocityY": -0.6249756038008819,
-            "timestamp": 0.7222692861084085
-        },
-        {
-            "x": 3.3282514237632603,
-            "y": 5.228701930838363,
-            "heading": 3.1400002631777038,
-            "angularVelocity": 1.8102189988940347e-13,
-            "velocityX": 3.730793333150296,
-            "velocityY": -0.62594801206375,
-            "timestamp": 0.745568295337712
-        },
-        {
-            "x": 3.4151697732878015,
-            "y": 5.214085582937486,
-            "heading": 3.140000263177707,
-            "angularVelocity": 1.4033623034653362e-13,
-            "velocityX": 3.7305598993143207,
-            "velocityY": -0.6273377445673929,
-            "timestamp": 0.7688673045670155
-        },
-        {
-            "x": 3.502081162717683,
-            "y": 5.199427905968822,
-            "heading": 3.1400002631777144,
-            "angularVelocity": 3.3137702069013046e-13,
-            "velocityX": 3.7302611701022674,
-            "velocityY": -0.6291115997313443,
-            "timestamp": 0.792166313796319
-        },
-        {
-            "x": 3.588983960416736,
-            "y": 5.18471937597994,
-            "heading": 3.140000263177713,
-            "angularVelocity": -6.829626544199834e-14,
-            "velocityX": 3.729892410608752,
-            "velocityY": -0.6312942256021684,
-            "timestamp": 0.8154653230256225
-        },
-        {
-            "x": 3.675876953607923,
-            "y": 5.169953034745646,
-            "heading": 3.140000263177716,
-            "angularVelocity": 1.3540589961163151e-13,
-            "velocityX": 3.7294715983848317,
-            "velocityY": -0.6337755004484098,
-            "timestamp": 0.838764332254926
-        },
-        {
-            "x": 3.7627586434088305,
-            "y": 5.155120331788361,
-            "heading": 3.1400002631777193,
-            "angularVelocity": 1.3533863072281271e-13,
-            "velocityX": 3.728986453707805,
-            "velocityY": -0.6366237641880743,
-            "timestamp": 0.8620633414842295
-        },
-        {
-            "x": 3.8490562562014863,
-            "y": 5.137199419063008,
-            "heading": 3.1400002631777224,
-            "angularVelocity": 1.3559815009211054e-13,
-            "velocityX": 3.7039177049679806,
-            "velocityY": -0.7691705921529395,
-            "timestamp": 0.885362350713533
-        },
-        {
-            "x": 3.9343637094417163,
-            "y": 5.1150391964944655,
-            "heading": 3.1400002631777255,
-            "angularVelocity": 1.370480919312873e-13,
-            "velocityX": 3.6614197797262853,
-            "velocityY": -0.9511229576576579,
-            "timestamp": 0.9086613599428365
+            "x": 1.3544641547566925,
+            "y": 5.559252486403593,
+            "heading": 5.298729726015063e-12,
+            "angularVelocity": 2.274228720676412e-10,
+            "velocityX": 0.1916026983161006,
+            "velocityY": -0.0320834805032552,
+            "timestamp": 0.02329901820759936
+        },
+        {
+            "x": 1.3633924655059346,
+            "y": 5.557757467272512,
+            "heading": 1.605830471024488e-11,
+            "angularVelocity": 4.618037851350714e-10,
+            "velocityX": 0.38320544967553505,
+            "velocityY": -0.06416661499484601,
+            "timestamp": 0.04659803641519872
+        },
+        {
+            "x": 1.3767849336296958,
+            "y": 5.555514951621415,
+            "heading": 3.246020157611451e-11,
+            "angularVelocity": 7.039737222967939e-10,
+            "velocityX": 0.574808260349448,
+            "velocityY": -0.09624936257493934,
+            "timestamp": 0.06989705462279808
+        },
+        {
+            "x": 1.3946415606835925,
+            "y": 5.552524949597437,
+            "heading": 5.470894313107332e-11,
+            "angularVelocity": 9.549218317932479e-10,
+            "velocityX": 0.7664111377908356,
+            "velocityY": -0.12833167463728912,
+            "timestamp": 0.09319607283039744
+        },
+        {
+            "x": 1.416962348431807,
+            "y": 5.548787472707871,
+            "heading": 8.303679095756625e-11,
+            "angularVelocity": 1.2158386690956822e-9,
+            "velocityX": 0.9580140909513821,
+            "velocityY": -0.1604134928036672,
+            "timestamp": 0.1164950910379968
+        },
+        {
+            "x": 1.4437472988920377,
+            "y": 5.544302534113244,
+            "heading": 1.1770979097345793e-10,
+            "angularVelocity": 1.4881742667086445e-9,
+            "velocityX": 1.1496171307120806,
+            "velocityY": -0.19249474611664116,
+            "timestamp": 0.13979410924559615
+        },
+        {
+            "x": 1.4749964143943655,
+            "y": 5.539070149011117,
+            "heading": 1.59035725627389e-10,
+            "angularVelocity": 1.7737199674396097e-9,
+            "velocityX": 1.3412202704804594,
+            "velocityY": -0.22457534714647368,
+            "timestamp": 0.1630931274531955
+        },
+        {
+            "x": 1.5107096976598688,
+            "y": 5.533090335148591,
+            "heading": 2.073747689126765e-10,
+            "angularVelocity": 2.0747244300773254e-9,
+            "velocityX": 1.532823527038234,
+            "velocityY": -0.25665518646593166,
+            "timestamp": 0.18639214566079487
+        },
+        {
+            "x": 1.5508871519080267,
+            "y": 5.526363113522473,
+            "heading": 2.631540997774015e-10,
+            "angularVelocity": 2.3940635247907603e-9,
+            "velocityX": 1.7244269217769022,
+            "velocityY": -0.28873412459698383,
+            "timestamp": 0.20969116386839423
+        },
+        {
+            "x": 1.595528781007412,
+            "y": 5.518888509361551,
+            "heading": 3.268885139183836e-10,
+            "angularVelocity": 2.7354977853334202e-9,
+            "velocityX": 1.9160304825557395,
+            "velocityY": -0.3208119799018042,
+            "timestamp": 0.2329901820759936
+        },
+        {
+            "x": 1.6446345896939198,
+            "y": 5.5106665535489805,
+            "heading": 3.9921044507968937e-10,
+            "angularVelocity": 3.104076267437521e-9,
+            "velocityX": 2.1076342466004383,
+            "velocityY": -0.35288850969337676,
+            "timestamp": 0.2562892002835929
+        },
+        {
+            "x": 1.698204583899122,
+            "y": 5.501697284762255,
+            "heading": 4.809154717853132e-10,
+            "angularVelocity": 3.5068011876967e-9,
+            "velocityX": 2.299238265229739,
+            "velocityY": -0.38496337943692166,
+            "timestamp": 0.2795882184911923
+        },
+        {
+            "x": 1.7562387712682006,
+            "y": 5.491980752848314,
+            "heading": 5.730347406124241e-10,
+            "angularVelocity": 3.953783188523521e-9,
+            "velocityX": 2.4908426119922917,
+            "velocityY": -0.41703610973659105,
+            "timestamp": 0.3028872366987917
+        },
+        {
+            "x": 1.8187371620272614,
+            "y": 5.48151702447463,
+            "heading": 6.769576861119728e-10,
+            "angularVelocity": 4.460400083403711e-9,
+            "velocityX": 2.6824473976619876,
+            "velocityY": -0.449105978650929,
+            "timestamp": 0.3261862549063911
+        },
+        {
+            "x": 1.8856997705540457,
+            "y": 5.4703061933617745,
+            "heading": 7.946577963057737e-10,
+            "angularVelocity": 5.051719599980361e-9,
+            "velocityX": 2.8740528004281045,
+            "velocityY": -0.4811718250535651,
+            "timestamp": 0.34948527311399047
+        },
+        {
+            "x": 1.957126618545275,
+            "y": 5.4583484009138274,
+            "heading": 9.291581669995643e-10,
+            "angularVelocity": 5.772791178735968e-9,
+            "velocityX": 3.0656591344232145,
+            "velocityY": -0.5132316023559669,
+            "timestamp": 0.37278429132158986
+        },
+        {
+            "x": 2.0330177424946343,
+            "y": 5.445643884917638,
+            "heading": 1.0856695619650843e-9,
+            "angularVelocity": 6.717510100758224e-9,
+            "velocityX": 3.257267034736916,
+            "velocityY": -0.5452811737842305,
+            "timestamp": 0.39608330952918924
+        },
+        {
+            "x": 2.1133732165489003,
+            "y": 5.432193128386492,
+            "heading": 1.2752962310893515e-9,
+            "angularVelocity": 8.13882633127911e-9,
+            "velocityX": 3.44887811744974,
+            "velocityY": -0.5773100141552516,
+            "timestamp": 0.41938232773678863
+        },
+        {
+            "x": 2.1981932628587333,
+            "y": 5.417997633484574,
+            "heading": 1.5377676317044546e-9,
+            "angularVelocity": 1.1265341261395982e-8,
+            "velocityX": 3.640498734927221,
+            "velocityY": -0.6092743812441802,
+            "timestamp": 0.442681345944388
+        },
+        {
+            "x": 2.285115669591906,
+            "y": 5.403405223128801,
+            "heading": 1.537767230961928e-9,
+            "angularVelocity": -1.719997404273206e-14,
+            "velocityX": 3.730732598201487,
+            "velocityY": -0.6263100970908951,
+            "timestamp": 0.4659803641519874
+        },
+        {
+            "x": 2.3720410537379144,
+            "y": 5.388830564421114,
+            "heading": 1.5377672475870007e-9,
+            "angularVelocity": 7.135525631450618e-16,
+            "velocityX": 3.7308603895453194,
+            "velocityY": -0.6255481916751834,
+            "timestamp": 0.4892793823595868
+        },
+        {
+            "x": 2.45896811239221,
+            "y": 5.374265896231001,
+            "heading": 1.5377672778413637e-9,
+            "angularVelocity": 1.298525326828493e-15,
+            "velocityX": 3.730932259882311,
+            "velocityY": -0.625119396028046,
+            "timestamp": 0.5125784005671862
+        },
+        {
+            "x": 2.5458958805121723,
+            "y": 5.359705463035692,
+            "heading": 1.5377672869369513e-9,
+            "angularVelocity": 3.903849318113422e-16,
+            "velocityX": 3.730962710335439,
+            "velocityY": -0.6249376289364498,
+            "timestamp": 0.5358774187747856
+        },
+        {
+            "x": 2.632823878731898,
+            "y": 5.345146403658019,
+            "heading": 1.5377672987189865e-9,
+            "angularVelocity": 5.056879723770718e-16,
+            "velocityX": 3.7309725862775656,
+            "velocityY": -0.6248786643237555,
+            "timestamp": 0.559176436982385
+        },
+        {
+            "x": 2.7197519358547084,
+            "y": 5.330587695987482,
+            "heading": 1.5377673268477785e-9,
+            "angularVelocity": 1.2072952571647457e-15,
+            "velocityX": 3.7309751144130874,
+            "velocityY": -0.6248635689629851,
+            "timestamp": 0.5824754551899843
+        },
+        {
+            "x": 2.806680023647356,
+            "y": 5.316029171436071,
+            "heading": 1.5377673567071691e-9,
+            "angularVelocity": 1.2815730065587283e-15,
+            "velocityX": 3.7309764307702067,
+            "velocityY": -0.6248557094441328,
+            "timestamp": 0.6057744733975837
+        },
+        {
+            "x": 2.8936082747189524,
+            "y": 5.301471621825431,
+            "heading": 1.5377673928791505e-9,
+            "angularVelocity": 1.5525110632783364e-15,
+            "velocityX": 3.7309834387453797,
+            "velocityY": -0.6248138647313558,
+            "timestamp": 0.6290734916051831
+        },
+        {
+            "x": 2.980536964716405,
+            "y": 5.286916693414742,
+            "heading": 1.5377674058328616e-9,
+            "angularVelocity": 5.559766105183297e-16,
+            "velocityX": 3.7310022775585137,
+            "velocityY": -0.6247013621403136,
+            "timestamp": 0.6523725098127825
+        },
+        {
+            "x": 3.0674663067511823,
+            "y": 5.272365659809418,
+            "heading": 1.5377674408459054e-9,
+            "angularVelocity": 1.5027691524839091e-15,
+            "velocityX": 3.73103026317236,
+            "velocityY": -0.6245341960676912,
+            "timestamp": 0.6756715280203819
+        },
+        {
+            "x": 3.154396100803892,
+            "y": 5.257817326905748,
+            "heading": 1.5377674636831172e-9,
+            "angularVelocity": 9.801792492345066e-16,
+            "velocityX": 3.7310496639016963,
+            "velocityY": -0.624418281237505,
+            "timestamp": 0.6989705462279813
+        },
+        {
+            "x": 3.2413249978571956,
+            "y": 5.243263635140824,
+            "heading": 1.5377674638426843e-9,
+            "angularVelocity": 6.848580307945622e-18,
+            "velocityX": 3.731011164452385,
+            "velocityY": -0.6246482849651879,
+            "timestamp": 0.7222695644355807
+        },
+        {
+            "x": 3.3282506523054423,
+            "y": 5.228690588454777,
+            "heading": 1.5377674749707248e-9,
+            "angularVelocity": 4.776183371381179e-16,
+            "velocityX": 3.7308719909897503,
+            "velocityY": -0.6254790032748808,
+            "timestamp": 0.74556858264318
+        },
+        {
+            "x": 3.415171009638962,
+            "y": 5.214085980519485,
+            "heading": 1.5377674915535578e-9,
+            "angularVelocity": 7.117396517888937e-16,
+            "velocityX": 3.7306446374302076,
+            "velocityY": -0.626833620407003,
+            "timestamp": 0.7688676008507794
+        },
+        {
+            "x": 3.5020842171218036,
+            "y": 5.1994388833661285,
+            "heading": 1.53776751807092e-9,
+            "angularVelocity": 1.13813226337154e-15,
+            "velocityX": 3.730337763955042,
+            "velocityY": -0.6286572688425895,
+            "timestamp": 0.7921666190583788
+        },
+        {
+            "x": 3.5889883305639807,
+            "y": 5.184737925794629,
+            "heading": 1.5377675551515876e-9,
+            "angularVelocity": 1.5915121086693097e-15,
+            "velocityX": 3.729947445333141,
+            "velocityY": -0.6309689721834142,
+            "timestamp": 0.8154656372659782
+        },
+        {
+            "x": 3.675881697311449,
+            "y": 5.1699735802440445,
+            "heading": 1.5377675887419511e-9,
+            "angularVelocity": 1.441707334130238e-15,
+            "velocityX": 3.7294861943638535,
+            "velocityY": -0.6336896009521162,
+            "timestamp": 0.8387646554735776
+        },
+        {
+            "x": 3.7627625314341118,
+            "y": 5.155135664387593,
+            "heading": 1.5377676077743658e-9,
+            "angularVelocity": 8.168762038570203e-16,
+            "velocityX": 3.728948290804007,
+            "velocityY": -0.636847257870781,
+            "timestamp": 0.862063673681177
+        },
+        {
+            "x": 3.8490591061764308,
+            "y": 5.137209586747657,
+            "heading": 1.5377676390897132e-9,
+            "angularVelocity": 1.344063051824819e-15,
+            "velocityX": 3.703871724266177,
+            "velocityY": -0.7693919752476525,
+            "timestamp": 0.8853626918887764
+        },
+        {
+            "x": 3.934365266987087,
+            "y": 5.115044254457407,
+            "heading": 1.5377676702466937e-9,
+            "angularVelocity": 1.337265902944942e-15,
+            "velocityX": 3.661362897377225,
+            "velocityY": -0.9513419017440721,
+            "timestamp": 0.9086617100963758
         },
         {
             "x": 4.018431663513184,
             "y": 5.088562488555908,
-            "heading": 3.140000263177729,
-            "angularVelocity": 1.413301961445022e-13,
-            "velocityX": 3.6082201283346866,
-            "velocityY": -1.1363877183834126,
-            "timestamp": 0.93196036917214
-        },
-        {
-            "x": 4.095482183944048,
-            "y": 5.0601896166081985,
-            "heading": 3.140000263177732,
-            "angularVelocity": 1.478757127486931e-13,
-            "velocityX": 3.549906540385796,
-            "velocityY": -1.3072078310833697,
-            "timestamp": 0.9536653125429956
-        },
-        {
-            "x": 4.171099571738343,
-            "y": 5.028192533992809,
-            "heading": 3.1400002631777353,
-            "angularVelocity": 1.4523452590479803e-13,
-            "velocityX": 3.483878603243662,
-            "velocityY": -1.4741841095171686,
-            "timestamp": 0.9753702559138512
-        },
-        {
-            "x": 4.2451557679063034,
-            "y": 4.992731534520922,
-            "heading": 3.1400002631777384,
-            "angularVelocity": 1.4237189069796376e-13,
-            "velocityX": 3.4119506742159467,
-            "velocityY": -1.6337752587458831,
-            "timestamp": 0.9970751992847068
-        },
-        {
-            "x": 4.31781550635648,
-            "y": 4.954490651976443,
-            "heading": 3.1400002631777415,
-            "angularVelocity": 1.4182458601020346e-13,
-            "velocityX": 3.347612440573966,
-            "velocityY": -1.761851293089538,
-            "timestamp": 1.0187801426555625
-        },
-        {
-            "x": 4.390182790341615,
-            "y": 4.91569917224677,
-            "heading": 3.140000263177745,
-            "angularVelocity": 1.421722636666585e-13,
-            "velocityX": 3.3341383457540337,
-            "velocityY": -1.787218656449447,
-            "timestamp": 1.040485086026418
-        },
-        {
-            "x": 4.462533664586848,
-            "y": 4.87687709490227,
-            "heading": 3.1400002631777486,
-            "angularVelocity": 1.635268036057102e-13,
-            "velocityX": 3.333382308772387,
-            "velocityY": -1.7886283636415046,
-            "timestamp": 1.0621900293972737
-        },
-        {
-            "x": 4.534860115355159,
-            "y": 4.838009534998386,
-            "heading": 3.140000263177751,
-            "angularVelocity": 1.1808318022164126e-13,
-            "velocityX": 3.3322570592611274,
-            "velocityY": -1.7907238567558525,
-            "timestamp": 1.0838949727681293
-        },
-        {
-            "x": 4.607177418550424,
-            "y": 4.799124957707118,
-            "heading": 3.140000263177754,
-            "angularVelocity": 1.4385253669729274e-13,
-            "velocityX": 3.3318356081243334,
-            "velocityY": -1.7915078895502723,
-            "timestamp": 1.105599916138985
-        },
-        {
-            "x": 4.679489672949611,
-            "y": 4.7602309921876325,
-            "heading": 3.1400002631777575,
-            "angularVelocity": 1.4120580598782794e-13,
-            "velocityX": 3.331602997698768,
-            "velocityY": -1.7919404282640008,
-            "timestamp": 1.1273048595098405
-        },
-        {
-            "x": 4.751800053049455,
-            "y": 4.721333542151952,
-            "heading": 3.1400002631777606,
-            "angularVelocity": 1.4195129118241079e-13,
-            "velocityX": 3.3315166441264745,
-            "velocityY": -1.7921009684797435,
-            "timestamp": 1.149009802880696
-        },
-        {
-            "x": 4.824110260922509,
-            "y": 4.682435771933003,
-            "heading": 3.1400002631777637,
-            "angularVelocity": 1.420416383867913e-13,
-            "velocityX": 3.3315087092142113,
-            "velocityY": -1.7921157201114153,
-            "timestamp": 1.1707147462515517
-        },
-        {
-            "x": 4.896420645274053,
-            "y": 4.643538329777489,
-            "heading": 3.140000263177767,
-            "angularVelocity": 1.4171119036284518e-13,
-            "velocityX": 3.3315168400108037,
-            "velocityY": -1.7921006054248252,
-            "timestamp": 1.1924196896224073
-        },
-        {
-            "x": 4.96873125432361,
-            "y": 4.6046413053457185,
-            "heading": 3.14000026317777,
-            "angularVelocity": 1.4137748376763704e-13,
-            "velocityX": 3.3315271924000274,
-            "velocityY": -1.792081359866257,
-            "timestamp": 1.214124632993263
-        },
-        {
-            "x": 5.04104400801269,
-            "y": 4.56574826813338,
-            "heading": 3.1400002631777735,
-            "angularVelocity": 1.421351986736705e-13,
-            "velocityX": 3.331626001206414,
-            "velocityY": -1.7918976588811946,
-            "timestamp": 1.2358295763641185
-        },
-        {
-            "x": 5.11336222581346,
-            "y": 4.5268653919090065,
-            "heading": 3.1400002631777766,
-            "angularVelocity": 1.4239328375117255e-13,
-            "velocityX": 3.3318777462404188,
-            "velocityY": -1.7914295172313686,
-            "timestamp": 1.257534519734974
-        },
-        {
-            "x": 5.185674775789392,
-            "y": 4.487971975945349,
-            "heading": 3.1400002631777797,
-            "angularVelocity": 1.4189340221530015e-13,
-            "velocityX": 3.331616615642773,
-            "velocityY": -1.7919151088820495,
-            "timestamp": 1.2792394631058297
-        },
-        {
-            "x": 5.25798679998857,
-            "y": 4.4490775824474165,
-            "heading": 3.140000263177783,
-            "angularVelocity": 1.4182847689207558e-13,
-            "velocityX": 3.331592391818018,
-            "velocityY": -1.791960146283848,
-            "timestamp": 1.3009444064766853
-        },
-        {
-            "x": 5.330301009987797,
-            "y": 4.410187253034054,
-            "heading": 3.1400002631777864,
-            "angularVelocity": 1.4221031722558925e-13,
-            "velocityX": 3.331693096990619,
-            "velocityY": -1.7917729039338035,
-            "timestamp": 1.322649349847541
-        },
-        {
-            "x": 5.4026181052176305,
-            "y": 4.3713022890025295,
-            "heading": 3.1400002631777895,
-            "angularVelocity": 1.4161705945188678e-13,
-            "velocityX": 3.3318260266449937,
-            "velocityY": -1.791525707625898,
-            "timestamp": 1.3443542932183965
-        },
-        {
-            "x": 5.474936056465776,
-            "y": 4.332418917012381,
-            "heading": 3.1400002631777926,
-            "angularVelocity": 1.4174219306640175e-13,
-            "velocityX": 3.331865465508907,
-            "velocityY": -1.7914523583735324,
-            "timestamp": 1.366059236589252
-        },
-        {
-            "x": 5.54725002868687,
-            "y": 4.293528145471586,
-            "heading": 3.1400002631777957,
-            "angularVelocity": 1.4114123611773667e-13,
-            "velocityX": 3.331682141968083,
-            "velocityY": -1.7917932738304505,
-            "timestamp": 1.3877641799601077
-        },
-        {
-            "x": 5.619560804995751,
-            "y": 4.2546314319979865,
-            "heading": 3.1400002631777992,
-            "angularVelocity": 1.438802546332647e-13,
-            "velocityX": 3.331534898449109,
-            "velocityY": -1.7920670332572342,
-            "timestamp": 1.4094691233309633
-        },
-        {
-            "x": 5.691871264235437,
-            "y": 4.215734129079228,
-            "heading": 3.1400002631778023,
-            "angularVelocity": 1.4227513855970737e-13,
-            "velocityX": 3.331520290292617,
-            "velocityY": -1.7920941904433432,
-            "timestamp": 1.431174066701819
-        },
-        {
-            "x": 5.7641829192535345,
-            "y": 4.1768390492074925,
-            "heading": 3.1400002631778055,
-            "angularVelocity": 1.4198857424426595e-13,
-            "velocityX": 3.3315753827383325,
-            "velocityY": -1.79199176921174,
-            "timestamp": 1.4528790100726745
-        },
-        {
-            "x": 5.836496897649906,
-            "y": 4.13794828914952,
-            "heading": 3.1400002631778086,
-            "angularVelocity": 1.420236059365936e-13,
-            "velocityX": 3.331682426477163,
-            "velocityY": -1.7917927447907471,
-            "timestamp": 1.47458395344353
-        },
-        {
-            "x": 5.90881236073979,
-            "y": 4.099060289876852,
-            "heading": 3.1400002631778117,
-            "angularVelocity": 1.4199973949506742e-13,
-            "velocityX": 3.3317508299509484,
-            "velocityY": -1.7916655486362405,
-            "timestamp": 1.4962888968143857
-        },
-        {
-            "x": 5.981129099244504,
-            "y": 4.06017466243704,
-            "heading": 3.140000263177815,
-            "angularVelocity": 1.4813660843765197e-13,
-            "velocityX": 3.3318095914418606,
-            "velocityY": -1.7915562724764487,
-            "timestamp": 1.5179938401852413
-        },
-        {
-            "x": 6.053446077627243,
-            "y": 4.021289481108035,
-            "heading": 3.140000263177829,
-            "angularVelocity": 6.425453029046258e-13,
-            "velocityX": 3.331820643210356,
-            "velocityY": -1.7915357190582988,
-            "timestamp": 1.539698783556097
-        },
-        {
-            "x": 6.125761091363011,
-            "y": 3.9824006462217088,
-            "heading": 3.140000263177869,
-            "angularVelocity": 1.8383501574993634e-12,
-            "velocityX": 3.331730127102014,
-            "velocityY": -1.7917040474119204,
-            "timestamp": 1.5614037269269525
-        },
-        {
-            "x": 6.198074029937981,
-            "y": 3.9435079527230394,
-            "heading": 3.140000263178238,
-            "angularVelocity": 1.7022488728392152e-11,
-            "velocityX": 3.331634519350639,
-            "velocityY": -1.79188182315355,
-            "timestamp": 1.583108670297808
-        },
-        {
-            "x": 6.270380104650612,
-            "y": 3.9046024999630506,
-            "heading": 3.140000263177847,
-            "angularVelocity": -1.8008440986287348e-11,
-            "velocityX": 3.33131828436938,
-            "velocityY": -1.7924696736324577,
-            "timestamp": 1.6048136136686637
-        },
-        {
-            "x": 6.342678298341779,
-            "y": 3.8656824038223503,
-            "heading": 3.1400002631782598,
-            "angularVelocity": 1.900200591260141e-11,
-            "velocityX": 3.3309551863724884,
-            "velocityY": -1.7931443301131518,
-            "timestamp": 1.6265185570395193
-        },
-        {
-            "x": 6.414967377910046,
-            "y": 3.8267453820171355,
-            "heading": 3.1400002631778587,
-            "angularVelocity": -1.8477715999059415e-11,
-            "velocityX": 3.3305352763678626,
-            "velocityY": -1.7939241370065324,
-            "timestamp": 1.648223500410375
-        },
-        {
-            "x": 6.487241234316261,
-            "y": 3.787780110681628,
-            "heading": 3.1400002631778627,
-            "angularVelocity": 1.954226890690534e-13,
-            "velocityX": 3.3298339079393355,
-            "velocityY": -1.795225662182324,
-            "timestamp": 1.6699284437812305
-        },
-        {
-            "x": 6.559496489740858,
-            "y": 3.7487803574447534,
-            "heading": 3.1400002631778654,
-            "angularVelocity": 1.160046399416623e-13,
-            "velocityX": 3.3289769150756516,
-            "velocityY": -1.79681432798537,
-            "timestamp": 1.691633387152086
-        },
-        {
-            "x": 6.630585131325424,
-            "y": 3.7076925524537225,
-            "heading": 3.140000263177869,
-            "angularVelocity": 1.5938094021424355e-13,
-            "velocityX": 3.275228152865242,
-            "velocityY": -1.8930159958948245,
-            "timestamp": 1.7133383305229417
-        },
-        {
-            "x": 6.699761336489547,
-            "y": 3.6634607167510884,
-            "heading": 3.140000263177872,
-            "angularVelocity": 1.431049344650049e-13,
-            "velocityX": 3.1871175142993193,
-            "velocityY": -2.037869205501672,
-            "timestamp": 1.7350432738937973
-        },
-        {
-            "x": 6.76677589557052,
-            "y": 3.6160175775021286,
-            "heading": 3.140000263178055,
-            "angularVelocity": 8.443311888840044e-12,
-            "velocityX": 3.087525175069758,
-            "velocityY": -2.1858218396761058,
-            "timestamp": 1.756748217264653
-        },
-        {
-            "x": 6.8314564099651545,
-            "y": 3.5654385941456423,
-            "heading": 3.140000263178058,
-            "angularVelocity": 1.5118918552164865e-13,
-            "velocityX": 2.9799900091650118,
-            "velocityY": -2.3302978723451035,
-            "timestamp": 1.7784531606355085
+            "heading": 1.5377676866838871e-9,
+            "angularVelocity": 7.054887729794734e-16,
+            "velocityX": 3.608151887646215,
+            "velocityY": -1.1366043695780652,
+            "timestamp": 0.9319607283039751
+        },
+        {
+            "x": 4.095480434320656,
+            "y": 5.06018496604887,
+            "heading": 1.5377676994335126e-9,
+            "angularVelocity": 5.874067965982521e-16,
+            "velocityX": 3.549827430213289,
+            "velocityY": -1.3074226459526814,
+            "timestamp": 0.9536656625077448
+        },
+        {
+            "x": 4.171095847536846,
+            "y": 5.028183306370306,
+            "heading": 1.5377677188417825e-9,
+            "angularVelocity": 8.941870233905506e-16,
+            "velocityX": 3.4837891009642465,
+            "velocityY": -1.4743956087592989,
+            "timestamp": 0.9753705967115144
+        },
+        {
+            "x": 4.24514985906727,
+            "y": 4.992717825181937,
+            "heading": 1.5377677281143787e-9,
+            "angularVelocity": 4.2721152582009774e-16,
+            "velocityX": 3.4118514636002706,
+            "velocityY": -1.6339824325334937,
+            "timestamp": 0.997075530915284
+        },
+        {
+            "x": 4.317807248978416,
+            "y": 4.95447255507026,
+            "heading": 1.5377677290979519e-9,
+            "angularVelocity": 4.531574211416763e-17,
+            "velocityX": 3.3475056514378028,
+            "velocityY": -1.7620541832797834,
+            "timestamp": 1.0187804651190535
+        },
+        {
+            "x": 4.3901722659206355,
+            "y": 4.9156769197600605,
+            "heading": 1.5377677449553673e-9,
+            "angularVelocity": 7.305902735413318e-16,
+            "velocityX": 3.334035305654692,
+            "velocityY": -1.7874108691538961,
+            "timestamp": 1.0404853993228231
+        },
+        {
+            "x": 4.4625185516483485,
+            "y": 4.876846365558822,
+            "heading": 1.5377677746336623e-9,
+            "angularVelocity": 1.3673523815751288e-15,
+            "velocityX": 3.333172312277143,
+            "velocityY": -1.7890196688239732,
+            "timestamp": 1.0621903335265928
+        },
+        {
+            "x": 4.534844691324942,
+            "y": 4.837978300028894,
+            "heading": 1.5377677779199108e-9,
+            "angularVelocity": 1.5140568225814148e-16,
+            "velocityX": 3.3322441338764652,
+            "velocityY": -1.7907479085103777,
+            "timestamp": 1.0838952677303624
+        },
+        {
+            "x": 4.6071619879089205,
+            "y": 4.799093783673277,
+            "heading": 1.537767801979053e-9,
+            "angularVelocity": 1.1084641137098069e-15,
+            "velocityX": 3.331836710728115,
+            "velocityY": -1.79150583873765,
+            "timestamp": 1.105600201934132
+        },
+        {
+            "x": 4.6794740379059565,
+            "y": 4.7601995113399385,
+            "heading": 1.5377678257958516e-9,
+            "angularVelocity": 1.0972987247710634e-15,
+            "velocityX": 3.331594987489656,
+            "velocityY": -1.791955320763837,
+            "timestamp": 1.1273051361379016
+        },
+        {
+            "x": 4.751784687579244,
+            "y": 4.721302635647572,
+            "heading": 1.5377678370559733e-9,
+            "angularVelocity": 5.187816897475249e-16,
+            "velocityX": 3.331530471108496,
+            "velocityY": -1.7920752639579989,
+            "timestamp": 1.1490100703416712
+        },
+        {
+            "x": 4.824095678023389,
+            "y": 4.682406393453542,
+            "heading": 1.5377678370330874e-9,
+            "angularVelocity": -1.0543217693671843e-18,
+            "velocityX": 3.3315461712634193,
+            "velocityY": -1.792046077125237,
+            "timestamp": 1.1707150045454409
+        },
+        {
+            "x": 4.896406945309153,
+            "y": 4.643510665929771,
+            "heading": 1.5377678640109226e-9,
+            "angularVelocity": 1.242935520614447e-15,
+            "velocityX": 3.331558926041247,
+            "velocityY": -1.7920223649940337,
+            "timestamp": 1.1924199387492105
+        },
+        {
+            "x": 4.968718824670649,
+            "y": 4.604616076350799,
+            "heading": 1.5377678830932924e-9,
+            "angularVelocity": 8.791718726461835e-16,
+            "velocityX": 3.33158712588479,
+            "velocityY": -1.7919699370612494,
+            "timestamp": 1.21412487295298
+        },
+        {
+            "x": 5.041033201559798,
+            "y": 4.565726130481367,
+            "heading": 1.5377679126898002e-9,
+            "angularVelocity": 1.3635842497565112e-15,
+            "velocityX": 3.331702193162082,
+            "velocityY": -1.7917559898752364,
+            "timestamp": 1.2358298071567497
+        },
+        {
+            "x": 5.113354228854967,
+            "y": 4.526848553325412,
+            "heading": 1.5377679256265375e-9,
+            "angularVelocity": 5.960275144059911e-16,
+            "velocityX": 3.332008593816099,
+            "velocityY": -1.7911861326534588,
+            "timestamp": 1.2575347413605193
+        },
+        {
+            "x": 5.1856663573422805,
+            "y": 4.487954426928371,
+            "heading": 1.537767949104212e-9,
+            "angularVelocity": 1.0816744331209405e-15,
+            "velocityX": 3.3315986037289687,
+            "velocityY": -1.791948597121158,
+            "timestamp": 1.279239675564289
+        },
+        {
+            "x": 5.257977857015597,
+            "y": 4.449059131458437,
+            "heading": 1.5377679798803482e-9,
+            "angularVelocity": 1.4179326501228622e-15,
+            "velocityX": 3.3315696327123336,
+            "velocityY": -1.7920024592017922,
+            "timestamp": 1.3009446097680586
+        },
+        {
+            "x": 5.330290964048228,
+            "y": 4.410166824431303,
+            "heading": 1.5377679920779735e-9,
+            "angularVelocity": 5.619748235100741e-16,
+            "velocityX": 3.3316436877316113,
+            "velocityY": -1.7918647742503244,
+            "timestamp": 1.3226495439718282
+        },
+        {
+            "x": 5.402605809232356,
+            "y": 4.371277749351831,
+            "heading": 1.5377680157791104e-9,
+            "angularVelocity": 1.0919698963856968e-15,
+            "velocityX": 3.3317237686705528,
+            "velocityY": -1.7917158704270917,
+            "timestamp": 1.3443544781755978
+        },
+        {
+            "x": 5.474921974691154,
+            "y": 4.332391129443428,
+            "heading": 1.5377680240202577e-9,
+            "angularVelocity": 3.7969007641948726e-16,
+            "velocityX": 3.331784596990179,
+            "velocityY": -1.7916027546232325,
+            "timestamp": 1.3660594123793675
+        },
+        {
+            "x": 5.54723622900274,
+            "y": 4.29350095564713,
+            "heading": 1.537768038636468e-9,
+            "angularVelocity": 6.734049902652557e-16,
+            "velocityX": 3.331696545710963,
+            "velocityY": -1.7917664910290456,
+            "timestamp": 1.387764346583137
+        },
+        {
+            "x": 5.619548385303401,
+            "y": 4.254606880959325,
+            "heading": 1.5377680494372407e-9,
+            "angularVelocity": 4.976183803494664e-16,
+            "velocityX": 3.331599885158129,
+            "velocityY": -1.7919462147497014,
+            "timestamp": 1.4094692807869067
+        },
+        {
+            "x": 5.6918607364808045,
+            "y": 4.215713168590034,
+            "heading": 1.5377680578875737e-9,
+            "angularVelocity": 3.893277832205449e-16,
+            "velocityX": 3.3316088636117085,
+            "velocityY": -1.7919295218396705,
+            "timestamp": 1.4311742149906763
+        },
+        {
+            "x": 5.764173499508255,
+            "y": 4.176820221955942,
+            "heading": 1.5377680813491443e-9,
+            "angularVelocity": 1.0809325075471762e-15,
+            "velocityX": 3.331627838562525,
+            "velocityY": -1.7918942425240987,
+            "timestamp": 1.452879149194446
+        },
+        {
+            "x": 5.8364876254997204,
+            "y": 4.137929809554028,
+            "heading": 1.5377680862134134e-9,
+            "angularVelocity": 2.241089873857745e-16,
+            "velocityX": 3.331690633686676,
+            "velocityY": -1.7917774841780256,
+            "timestamp": 1.4745840833982156
+        },
+        {
+            "x": 5.90880309835116,
+            "y": 4.099041901645272,
+            "heading": 1.537768099475084e-9,
+            "angularVelocity": 6.109980238433844e-16,
+            "velocityX": 3.3317526868576484,
+            "velocityY": -1.7916620959822775,
+            "timestamp": 1.4962890176019852
+        },
+        {
+            "x": 5.981120307623631,
+            "y": 4.060157222932383,
+            "heading": 1.5377681005691863e-9,
+            "angularVelocity": 5.040806501517701e-17,
+            "velocityX": 3.331832688069564,
+            "velocityY": -1.7915133189414525,
+            "timestamp": 1.5179939518057548
+        },
+        {
+            "x": 6.053439067813782,
+            "y": 4.0212754287411325,
+            "heading": 1.5377681034134081e-9,
+            "angularVelocity": 1.3104042715425496e-16,
+            "velocityX": 3.3319041426820193,
+            "velocityY": -1.7913804218969325,
+            "timestamp": 1.5396988860095244
+        },
+        {
+            "x": 6.1257574594958575,
+            "y": 3.9823929491454755,
+            "heading": 1.5377681141371771e-9,
+            "angularVelocity": 4.940705905043031e-16,
+            "velocityX": 3.3318871646008015,
+            "velocityY": -1.7914120001783713,
+            "timestamp": 1.561403820213294
+        },
+        {
+            "x": 6.198071250397711,
+            "y": 3.943501913620472,
+            "heading": 1.5377681379511677e-9,
+            "angularVelocity": 1.09716939803524e-15,
+            "velocityX": 3.3316751952742036,
+            "velocityY": -1.7918061930096516,
+            "timestamp": 1.5831087544170637
+        },
+        {
+            "x": 6.270375940604977,
+            "y": 3.9045939610161593,
+            "heading": 1.5377681515073034e-9,
+            "angularVelocity": 6.245647694184294e-16,
+            "velocityX": 3.331255903765101,
+            "velocityY": -1.7925856046957434,
+            "timestamp": 1.6048136886208333
+        },
+        {
+            "x": 6.342673755736347,
+            "y": 3.865673234790299,
+            "heading": 1.5377699302540744e-9,
+            "angularVelocity": 8.19513085832153e-14,
+            "velocityX": 3.3309391520146527,
+            "velocityY": -1.7931741170331514,
+            "timestamp": 1.626518622824603
+        },
+        {
+            "x": 6.414965746925861,
+            "y": 3.826741692219575,
+            "heading": 1.5377690472977265e-9,
+            "angularVelocity": -4.068002902168065e-14,
+            "velocityX": 3.3306708286121007,
+            "velocityY": -1.7936724527872456,
+            "timestamp": 1.6482235570283725
+        },
+        {
+            "x": 6.487241720022193,
+            "y": 3.7877804203169507,
+            "heading": 1.5377684535437603e-9,
+            "angularVelocity": -2.7355713192172554e-14,
+            "velocityX": 3.3299328354443367,
+            "velocityY": -1.7950421566324242,
+            "timestamp": 1.6699284912321422
+        },
+        {
+            "x": 6.559496923985796,
+            "y": 3.748780644748349,
+            "heading": 1.5377682143039968e-9,
+            "angularVelocity": -1.1022367481483692e-14,
+            "velocityX": 3.3289759501347236,
+            "velocityY": -1.796816115748624,
+            "timestamp": 1.6916334254359118
+        },
+        {
+            "x": 6.630585509558441,
+            "y": 3.7076928121475503,
+            "heading": 1.5377682448018014e-9,
+            "angularVelocity": 1.4051092264620289e-15,
+            "velocityX": 3.2752269555510436,
+            "velocityY": -1.8930180674612387,
+            "timestamp": 1.7133383596396814
+        },
+        {
+            "x": 6.6997616539527085,
+            "y": 3.6634609457782683,
+            "heading": 1.5377682789348178e-9,
+            "angularVelocity": 1.5725923809489335e-15,
+            "velocityX": 3.1871160605618085,
+            "velocityY": -2.037871479084932,
+            "timestamp": 1.735043293843451
+        },
+        {
+            "x": 6.7667761400423245,
+            "y": 3.6160177634442383,
+            "heading": 1.5377682945212807e-9,
+            "angularVelocity": 7.181069480230158e-16,
+            "velocityX": 3.0875231161945336,
+            "velocityY": -2.185824747894625,
+            "timestamp": 1.7567482280472206
+        },
+        {
+            "x": 6.831456555555867,
+            "y": 3.5654387099344507,
+            "heading": 1.5377683209709385e-9,
+            "angularVelocity": 1.2186011773240001e-15,
+            "velocityX": 2.979986712068234,
+            "velocityY": -2.330302088683456,
+            "timestamp": 1.7784531622509903
         },
         {
             "x": 6.893649101257324,
             "y": 3.5118300914764404,
-            "heading": 3.1400002631780617,
-            "angularVelocity": 1.5501587028498165e-13,
-            "velocityX": 2.865369894292817,
-            "velocityY": -2.469875260820684,
-            "timestamp": 1.8001581040063641
-        },
-        {
-            "x": 6.9342196158595195,
-            "y": 3.4745269537623855,
-            "heading": 3.140000263178065,
-            "angularVelocity": 2.3326761756772743e-13,
-            "velocityX": 2.784724807966257,
-            "velocityY": -2.5604549024296013,
-            "timestamp": 1.8147270533280853
-        },
-        {
-            "x": 6.973574770579654,
-            "y": 3.435943763171971,
-            "heading": 3.140000263178068,
-            "angularVelocity": 2.2697466808256025e-13,
-            "velocityX": 2.701303563563437,
-            "velocityY": -2.648316617649584,
-            "timestamp": 1.8292960026498064
-        },
-        {
-            "x": 7.011676569760775,
-            "y": 3.396122365046946,
-            "heading": 3.140000263178071,
-            "angularVelocity": 2.224027473304724e-13,
-            "velocityX": 2.615274330340327,
-            "velocityY": -2.7333061050358776,
-            "timestamp": 1.8438649519715276
-        },
-        {
-            "x": 7.048491012484172,
-            "y": 3.355107883496788,
-            "heading": 3.1400002631780746,
-            "angularVelocity": 2.1692590269440246e-13,
-            "velocityX": 2.5269113036529633,
-            "velocityY": -2.815198312825684,
-            "timestamp": 1.8584339012932487
-        },
-        {
-            "x": 7.083992093604447,
-            "y": 3.312951458599195,
-            "heading": 3.1400002631780777,
-            "angularVelocity": 2.1487378521690259e-13,
-            "velocityX": 2.4367633064212786,
-            "velocityY": -2.893580310196796,
-            "timestamp": 1.8730028506149698
-        },
-        {
-            "x": 7.118173429008876,
-            "y": 3.269718054236438,
-            "heading": 3.140000263178081,
-            "angularVelocity": 2.1169602046875272e-13,
-            "velocityX": 2.3461771092494,
-            "velocityY": -2.967503243236169,
-            "timestamp": 1.887571799936691
-        },
-        {
-            "x": 7.151084190942531,
-            "y": 3.2255097664946186,
-            "heading": 3.140000263178084,
-            "angularVelocity": 2.115299519607656e-13,
-            "velocityX": 2.258966052177459,
-            "velocityY": -3.034418389794455,
-            "timestamp": 1.9021407492584121
-        },
-        {
-            "x": 7.1829258498520225,
-            "y": 3.1805253278057903,
-            "heading": 3.1400002631783006,
-            "angularVelocity": 1.4861478341138462e-11,
-            "velocityX": 2.1855837511923784,
-            "velocityY": -3.0876927152018316,
-            "timestamp": 1.9167096985801333
-        },
-        {
-            "x": 7.213982568749707,
-            "y": 3.1352979487738755,
-            "heading": 3.140000221251036,
-            "angularVelocity": -0.000002877850999701821,
-            "velocityX": 2.1317061520275473,
-            "velocityY": -3.104367928885527,
-            "timestamp": 1.9312786479018544
-        },
-        {
-            "x": 7.244041792187231,
-            "y": 3.091532444769153,
-            "heading": 3.1400001920667906,
-            "angularVelocity": -0.000002003181212693182,
-            "velocityX": 2.063238931905195,
-            "velocityY": -3.0040260994983523,
-            "timestamp": 1.9458475972235756
-        },
-        {
-            "x": 7.273101015062598,
-            "y": 3.049227126183516,
-            "heading": 3.140000168751395,
-            "angularVelocity": -0.0000016003484573673396,
-            "velocityX": 1.9945997637621933,
-            "velocityY": -2.9038002433409447,
-            "timestamp": 1.9604165465452967
-        },
-        {
-            "x": 7.301159421127851,
-            "y": 3.008381441393539,
-            "heading": 3.1400001491208855,
-            "angularVelocity": -0.0000013474210949527603,
-            "velocityX": 1.92590456907015,
-            "velocityY": -2.8036122501352616,
-            "timestamp": 1.9749854958670179
-        },
-        {
-            "x": 7.328216598566981,
-            "y": 2.9689951117692965,
-            "heading": 3.1400001321528324,
-            "angularVelocity": -0.0000011646724122855417,
-            "velocityX": 1.8571811076855331,
-            "velocityY": -2.703443381845065,
-            "timestamp": 1.989554445188739
-        },
-        {
-            "x": 7.35427229894417,
-            "y": 2.9310679691270813,
-            "heading": 3.140000117255664,
-            "angularVelocity": -0.0000010225286768555236,
-            "velocityX": 1.7884405938827321,
-            "velocityY": -2.603286057538151,
-            "timestamp": 2.00412339451046
-        },
-        {
-            "x": 7.379326356096679,
-            "y": 2.8945999009401007,
-            "heading": 3.1400001040435335,
-            "angularVelocity": -9.068691499457651e-7,
-            "velocityX": 1.7196886748142857,
-            "velocityY": -2.50313645697232,
-            "timestamp": 2.0186923438321815
-        },
-        {
-            "x": 7.403378651082293,
-            "y": 2.8595908266403365,
-            "heading": 3.1400000922448146,
-            "angularVelocity": -8.098538018281321e-7,
-            "velocityX": 1.650928591655569,
-            "velocityY": -2.4029923865251464,
-            "timestamp": 2.033261293153903
-        },
-        {
-            "x": 7.426429094558334,
-            "y": 2.826040685699055,
-            "heading": 3.1400000816578917,
-            "angularVelocity": -7.266771822683861e-7,
-            "velocityX": 1.5821623760933374,
-            "velocityY": -2.3028524707174887,
-            "timestamp": 2.0478302424756243
-        },
-        {
-            "x": 7.448477616956692,
-            "y": 2.793949430978311,
-            "heading": 3.1400000721271697,
-            "angularVelocity": -6.541804651955507e-7,
-            "velocityX": 1.513391385436516,
-            "velocityY": -2.202715790417496,
-            "timestamp": 2.0623991917973457
-        },
-        {
-            "x": 7.4695241625734585,
-            "y": 2.763317024730304,
-            "heading": 3.1400000635289262,
-            "angularVelocity": -5.901759591769764e-7,
-            "velocityX": 1.4446165713119874,
-            "velocityY": -2.102581701093514,
-            "timestamp": 2.076968141119067
-        },
-        {
-            "x": 7.489568685800359,
-            "y": 2.734143436045934,
-            "heading": 3.1400000557624246,
-            "angularVelocity": -5.330859058092693e-7,
-            "velocityX": 1.3758386266751614,
-            "velocityY": -2.0024497333432913,
-            "timestamp": 2.0915370904407884
-        },
-        {
-            "x": 7.508611148608458,
-            "y": 2.7064286391508827,
-            "heading": 3.140000048744052,
-            "angularVelocity": -4.817349694037634e-7,
-            "velocityX": 1.3070580717652986,
-            "velocityY": -1.902319534719876,
-            "timestamp": 2.1061060397625098
-        },
-        {
-            "x": 7.526651518803911,
-            "y": 2.680172612224319,
-            "heading": 3.1400000424032948,
-            "angularVelocity": -4.3522407744534453e-7,
-            "velocityX": 1.2382753070979928,
-            "velocityY": -1.8021908338592867,
-            "timestamp": 2.120674989084231
-        },
-        {
-            "x": 7.543689768780771,
-            "y": 2.6553753365541346,
-            "heading": 3.1400000366798815,
-            "angularVelocity": -3.9285011556875146e-7,
-            "velocityX": 1.1694906475824893,
-            "velocityY": -1.7020634173815092,
-            "timestamp": 2.1352439384059525
-        },
-        {
-            "x": 7.559725874605572,
-            "y": 2.6320367959168407,
-            "heading": 3.1400000315217036,
-            "angularVelocity": -3.5405282918878005e-7,
-            "velocityX": 1.100704345294715,
-            "velocityY": -1.6019371144699315,
-            "timestamp": 2.149812887727674
-        },
-        {
-            "x": 7.574759815330251,
-            "y": 2.6101569761120964,
-            "heading": 3.140000026883262,
-            "angularVelocity": -3.1837857149022665e-7,
-            "velocityX": 1.0319166051501305,
-            "velocityY": -1.5018117862573288,
-            "timestamp": 2.1643818370493952
-        },
-        {
-            "x": 7.588791572466392,
-            "y": 2.5897358646065065,
-            "heading": 3.1400000227244846,
-            "angularVelocity": -2.854548657017254e-7,
-            "velocityX": 0.9631275959768569,
-            "velocityY": -1.4016873183261147,
-            "timestamp": 2.1789507863711166
-        },
-        {
-            "x": 7.601821129576078,
-            "y": 2.570773450256394,
-            "heading": 3.140000019009809,
-            "angularVelocity": -2.5497212819286766e-7,
-            "velocityX": 0.8943374585191634,
-            "velocityY": -1.3015636152871453,
-            "timestamp": 2.193519735692838
-        },
-        {
-            "x": 7.61384847194871,
-            "y": 2.5532697230888193,
-            "heading": 3.1400000157074612,
-            "angularVelocity": -2.2667026217565558e-7,
-            "velocityX": 0.8255463113387961,
-            "velocityY": -1.201440596782113,
-            "timestamp": 2.2080886850145593
-        },
-        {
-            "x": 7.624873586342355,
-            "y": 2.5372246741263043,
-            "heading": 3.1400000127888834,
-            "angularVelocity": -2.0032864319885596e-7,
-            "velocityX": 0.7567542552436427,
-            "velocityY": -1.1013181944833246,
-            "timestamp": 2.2226576343362807
-        },
-        {
-            "x": 7.634896460774273,
-            "y": 2.5226382952448834,
-            "heading": 3.140000010228267,
-            "angularVelocity": -1.757585156167035e-7,
-            "velocityX": 0.6879613766637577,
-            "velocityY": -1.0011963498064516,
-            "timestamp": 2.237226583658002
-        },
-        {
-            "x": 7.643917084349478,
-            "y": 2.509510579057917,
-            "heading": 3.140000008002173,
-            "angularVelocity": -1.527971366621834e-7,
-            "velocityX": 0.6191677502613596,
-            "velocityY": -0.9010750121421621,
-            "timestamp": 2.2517955329797235
-        },
-        {
-            "x": 7.6519354471190555,
-            "y": 2.4978415188200733,
-            "heading": 3.140000006089223,
-            "angularVelocity": -1.3130320584948841e-7,
-            "velocityX": 0.5503734409743167,
-            "velocityY": -0.8009541374715159,
-            "timestamp": 2.266364482301445
-        },
-        {
-            "x": 7.658951539962077,
-            "y": 2.487631108347293,
-            "heading": 3.1400000044698366,
-            "angularVelocity": -1.1115325658440658e-7,
-            "velocityX": 0.4815785056345368,
-            "velocityY": -0.7008336872693404,
-            "timestamp": 2.280933431623166
-        },
-        {
-            "x": 7.664965354486367,
-            "y": 2.478879341949535,
-            "heading": 3.1400000031260147,
-            "angularVelocity": -9.223877588817121e-8,
-            "velocityX": 0.4127829942631362,
-            "velocityY": -0.600713627626585,
-            "timestamp": 2.2955023809448876
-        },
-        {
-            "x": 7.669976882944524,
-            "y": 2.4715862143738576,
-            "heading": 3.140000002041154,
-            "angularVelocity": -7.44638832915239e-8,
-            "velocityX": 0.34398695111687533,
-            "velocityY": -0.5005939285411971,
-            "timestamp": 2.310071330266609
-        },
-        {
-            "x": 7.673986118162365,
-            "y": 2.4657517207559176,
-            "heading": 3.1400000011998928,
-            "angularVelocity": -5.774344306809202e-8,
-            "velocityX": 0.27519041554106066,
-            "velocityY": -0.4004745633401276,
-            "timestamp": 2.3246402795883303
-        },
-        {
-            "x": 7.676993053477586,
-            "y": 2.4613758565783885,
-            "heading": 3.1400000005879747,
-            "angularVelocity": -4.2001516695103525e-8,
-            "velocityX": 0.20639342267033312,
-            "velocityY": -0.30035550820440443,
-            "timestamp": 2.3392092289100517
-        },
-        {
-            "x": 7.6789976826868624,
-            "y": 2.458458617635108,
-            "heading": 3.1400000001921353,
-            "angularVelocity": -2.7170084001525683e-8,
-            "velocityX": 0.13759600400877475,
-            "velocityY": -0.2002367417759812,
-            "timestamp": 2.353778178231773
+            "heading": 1.5377683531646926e-9,
+            "angularVelocity": 1.4832457732424108e-15,
+            "velocityX": 2.8653643967583453,
+            "velocityY": -2.4698816386502185,
+            "timestamp": 1.8001580964547599
+        },
+        {
+            "x": 6.934219508494461,
+            "y": 3.4745268434152994,
+            "heading": 1.5377683787984002e-9,
+            "angularVelocity": 1.7594754613333528e-15,
+            "velocityX": 2.784717658151537,
+            "velocityY": -2.56046267850036,
+            "timestamp": 1.8147270446274342
+        },
+        {
+            "x": 6.973574535692003,
+            "y": 3.4359435289599864,
+            "heading": 1.5377684019331704e-9,
+            "angularVelocity": 1.5879505990145664e-15,
+            "velocityX": 2.701295023573416,
+            "velocityY": -2.648325328500865,
+            "timestamp": 1.8292959928001085
+        },
+        {
+            "x": 7.011676190859291,
+            "y": 3.3961219990562412,
+            "heading": 1.5377684253031837e-9,
+            "angularVelocity": 1.6040974952296623e-15,
+            "velocityX": 2.6152646516208327,
+            "velocityY": -2.733315365788167,
+            "timestamp": 1.8438649409727828
+        },
+        {
+            "x": 7.048490476455465,
+            "y": 3.3551073823105697,
+            "heading": 1.5377684383832216e-9,
+            "angularVelocity": 8.97802495662944e-16,
+            "velocityX": 2.5269007178724983,
+            "velocityY": -2.815207814562461,
+            "timestamp": 1.858433889145457
+        },
+        {
+            "x": 7.083991390306391,
+            "y": 3.3129508222340074,
+            "heading": 1.5377684557448877e-9,
+            "angularVelocity": 1.191689740876308e-15,
+            "velocityX": 2.4367520173841823,
+            "velocityY": -2.893589816979968,
+            "timestamp": 1.8730028373181313
+        },
+        {
+            "x": 7.118172550776329,
+            "y": 3.269717285105602,
+            "heading": 1.5377684739509254e-9,
+            "angularVelocity": 1.2496467044902449e-15,
+            "velocityX": 2.346165286939037,
+            "velocityY": -2.9675125902013777,
+            "timestamp": 1.8875717854908056
+        },
+        {
+            "x": 7.15108313208807,
+            "y": 3.2255088683196913,
+            "heading": 1.5377684882032233e-9,
+            "angularVelocity": 9.782653941580851e-16,
+            "velocityX": 2.2589538326090746,
+            "velocityY": -3.0344274865927217,
+            "timestamp": 1.90214073366348
+        },
+        {
+            "x": 7.182924606349684,
+            "y": 3.1805243042550315,
+            "heading": 1.5377697666399164e-9,
+            "angularVelocity": 8.775078931151136e-14,
+            "velocityX": 2.185571249497722,
+            "velocityY": -3.0877015644156707,
+            "timestamp": 1.9167096818361542
+        },
+        {
+            "x": 7.2139811373084415,
+            "y": 3.1352968072431113,
+            "heading": 1.2915519070501347e-9,
+            "angularVelocity": -1.6900181039231953e-8,
+            "velocityX": 2.131693420188762,
+            "velocityY": -3.1043762717714163,
+            "timestamp": 1.9312786300088285
+        },
+        {
+            "x": 7.244040263059187,
+            "y": 3.0915312465009275,
+            "heading": 1.1211375399666565e-9,
+            "angularVelocity": -1.169709485282711e-8,
+            "velocityX": 2.063232389495776,
+            "velocityY": -3.0040302308350935,
+            "timestamp": 1.9458475781815028
+        },
+        {
+            "x": 7.2730994599044765,
+            "y": 3.0492259199621516,
+            "heading": 9.847245150980228e-10,
+            "angularVelocity": -9.363272118309568e-9,
+            "velocityX": 1.9945981343934114,
+            "velocityY": -2.903801018259091,
+            "timestamp": 1.960416526354177
+        },
+        {
+            "x": 7.301157878524098,
+            "y": 3.0083802533354898,
+            "heading": 8.698146081553592e-10,
+            "angularVelocity": -7.887316693614981e-9,
+            "velocityX": 1.9259055826862927,
+            "velocityY": -2.803611224540764,
+            "timestamp": 1.9749854745268514
+        },
+        {
+            "x": 7.328215092540324,
+            "y": 2.968993958016925,
+            "heading": 7.704962597777701e-10,
+            "angularVelocity": -6.8171255926008325e-9,
+            "velocityX": 1.857183764780944,
+            "velocityY": -2.7034412403524777,
+            "timestamp": 1.9895544226995256
+        },
+        {
+            "x": 7.354270845617658,
+            "y": 2.9310668604072108,
+            "heading": 6.833248169018587e-10,
+            "angularVelocity": -5.983372499761897e-9,
+            "velocityX": 1.7884443522287568,
+            "velocityY": -2.6032831718663054,
+            "timestamp": 2.0041233708722
+        },
+        {
+            "x": 7.379324966656107,
+            "y": 2.894598844592948,
+            "heading": 6.060446454340774e-10,
+            "angularVelocity": -5.304444180973637e-9,
+            "velocityX": 1.719693195521089,
+            "velocityY": -2.5031330595756027,
+            "timestamp": 2.0186923190448742
+        },
+        {
+            "x": 7.403377333338156,
+            "y": 2.8595898276899736,
+            "heading": 5.370639718880826e-10,
+            "angularVelocity": -4.734773818100124e-9,
+            "velocityX": 1.6509336430450512,
+            "velocityY": -2.4029886363820543,
+            "timestamp": 2.0332612672175485
+        },
+        {
+            "x": 7.426427853868939,
+            "y": 2.8260397474863512,
+            "heading": 4.75199340983144e-10,
+            "angularVelocity": -4.246334802521844e-9,
+            "velocityX": 1.582167789848791,
+            "velocityY": -2.3028484833621037,
+            "timestamp": 2.047830215390223
+        },
+        {
+            "x": 7.44847645681879,
+            "y": 2.79394855556605,
+            "heading": 4.195363288341646e-10,
+            "angularVelocity": -3.820661051835768e-9,
+            "velocityX": 1.5133970337819913,
+            "velocityY": -2.202711653576578,
+            "timestamp": 2.062399163562897
+        },
+        {
+            "x": 7.469523085022814,
+            "y": 2.7633162131780225,
+            "heading": 3.6934715040819367e-10,
+            "angularVelocity": -3.4449418365911004e-9,
+            "velocityX": 1.4446223539663485,
+            "velocityY": -2.1025774836294286,
+            "timestamp": 2.0769681117355714
+        },
+        {
+            "x": 7.489567691695811,
+            "y": 2.7341426886048597,
+            "heading": 3.2403879933557077e-10,
+            "angularVelocity": -3.1099260658641154e-9,
+            "velocityX": 1.3758444628549362,
+            "velocityY": -2.002445490737794,
+            "timestamp": 2.0915370599082457
+        },
+        {
+            "x": 7.5086102378406,
+            "y": 2.7064279554071735,
+            "heading": 2.831187733993615e-10,
+            "angularVelocity": -2.8087152215039624e-9,
+            "velocityX": 1.3070638950107798,
+            "velocityY": -1.9023153126229018,
+            "timestamp": 2.10610600808092
+        },
+        {
+            "x": 7.52665069045286,
+            "y": 2.680171991207375,
+            "heading": 2.4617151569169827e-10,
+            "angularVelocity": -2.536027844020545e-9,
+            "velocityX": 1.2382810617787536,
+            "velocityY": -1.8021866704863527,
+            "timestamp": 2.1206749562535943
+        },
+        {
+            "x": 7.543689021238326,
+            "y": 2.65537477682047,
+            "heading": 2.128416865029733e-10,
+            "angularVelocity": -2.287730676987115e-9,
+            "velocityX": 1.169496286452695,
+            "velocityY": -1.7020593451912265,
+            "timestamp": 2.1352439044262685
+        },
+        {
+            "x": 7.559725205671708,
+            "y": 2.6320362956163437,
+            "heading": 1.8282195651887185e-10,
+            "angularVelocity": -2.0605283283789865e-9,
+            "velocityX": 1.1007098277319451,
+            "velocityY": -1.6019331613726957,
+            "timestamp": 2.149812852598943
+        },
+        {
+            "x": 7.5747592222906635,
+            "y": 2.6101565330412897,
+            "heading": 1.558438913168689e-10,
+            "angularVelocity": -1.8517510864510685e-9,
+            "velocityX": 1.0319218958546759,
+            "velocityY": -1.5018079765081174,
+            "timestamp": 2.164381800771617
+        },
+        {
+            "x": 7.588791052155751,
+            "y": 2.5897354762519833,
+            "heading": 1.3167100643601026e-10,
+            "angularVelocity": -1.6592059334641093e-9,
+            "velocityX": 0.9631326639903722,
+            "velocityY": -1.4016836731981617,
+            "timestamp": 2.1789507489442914
+        },
+        {
+            "x": 7.601820678430282,
+            "y": 2.570773113830717,
+            "heading": 1.100933830131512e-10,
+            "angularVelocity": -1.4810694297523211e-9,
+            "velocityX": 0.894342276470416,
+            "velocityY": -1.3015601535897774,
+            "timestamp": 2.1935196971169657
+        },
+        {
+            "x": 7.613848086048565,
+            "y": 2.5532694355605217,
+            "heading": 9.092342750630814e-11,
+            "angularVelocity": -1.315809184573053e-9,
+            "velocityX": 0.8255508548544661,
+            "velocityY": -1.201437335265309,
+            "timestamp": 2.20808864528964
+        },
+        {
+            "x": 7.624873261450447,
+            "y": 2.537224432245223,
+            "heading": 7.399249133993465e-11,
+            "angularVelocity": -1.1621248318093622e-9,
+            "velocityX": 0.7567585024813989,
+            "velocityY": -1.101315148158345,
+            "timestamp": 2.2226575934623143
+        },
+        {
+            "x": 7.634896192366388,
+            "y": 2.5226380955637384,
+            "heading": 5.914813867727886e-11,
+            "angularVelocity": -1.0189035430874983e-9,
+            "velocityX": 0.6879653079374735,
+            "velocityY": -1.0011935322032168,
+            "timestamp": 2.2372265416349886
+        },
+        {
+            "x": 7.643916867641572,
+            "y": 2.5095104179508487,
+            "heading": 4.6251914688462445e-11,
+            "angularVelocity": -8.851856741832805e-10,
+            "velocityX": 0.6191713477368228,
+            "velocityY": -0.9010724355182365,
+            "timestamp": 2.251795489807663
+        },
+        {
+            "x": 7.651935277090606,
+            "y": 2.4978413924986826,
+            "heading": 3.517750680608747e-11,
+            "angularVelocity": -7.601377906979457e-10,
+            "velocityX": 0.5503766884196106,
+            "velocityY": -0.8009518129835539,
+            "timestamp": 2.266364437980337
+        },
+        {
+            "x": 7.658951411376412,
+            "y": 2.487631012874611,
+            "heading": 2.580921448386634e-11,
+            "angularVelocity": -6.43031492677376e-10,
+            "velocityX": 0.48158138821334623,
+            "velocityY": -0.7008316251150604,
+            "timestamp": 2.2809333861530114
+        },
+        {
+            "x": 7.664965261908482,
+            "y": 2.4788792732522666,
+            "heading": 1.804066569533592e-11,
+            "angularVelocity": -5.332264785860361e-10,
+            "velocityX": 0.41278549836213546,
+            "velocityY": -0.6007118371633381,
+            "timestamp": 2.2955023343256857
+        },
+        {
+            "x": 7.669976820756786,
+            "y": 2.4715861682531717,
+            "heading": 1.1773732592627752e-11,
+            "angularVelocity": -4.3015687513290903e-10,
+            "velocityX": 0.34398906420045333,
+            "velocityY": -0.500592418385764,
+            "timestamp": 2.31007128249836
+        },
+        {
+            "x": 7.67398608057842,
+            "y": 2.465751692897001,
+            "heading": 6.917607828867898e-12,
+            "angularVelocity": -3.3332020945886676e-10,
+            "velocityX": 0.27519212602817883,
+            "velocityY": -0.40047334145331454,
+            "timestamp": 2.3246402306710343
+        },
+        {
+            "x": 7.676993034554729,
+            "y": 2.461375842558954,
+            "heading": 3.3880167119093778e-12,
+            "angularVelocity": -2.4226808579143967e-10,
+            "velocityX": 0.20639471982945568,
+            "velocityY": -0.30035458196322756,
+            "timestamp": 2.3392091788437086
+        },
+        {
+            "x": 7.678997676337105,
+            "y": 2.4584586129329957,
+            "heading": 1.1065347655527578e-12,
+            "angularVelocity": -1.56598950365953e-10,
+            "velocityX": 0.13759687786767488,
+            "velocityY": -0.2002361180356331,
+            "timestamp": 2.353778127016383
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -1.3187979232574289e-8,
-            "velocityX": 0.06879818791344262,
-            "velocityY": -0.10011824483002574,
-            "timestamp": 2.3683471275534944
+            "heading": -3.441813048880529e-34,
+            "angularVelocity": -7.595159066564751e-11,
+            "velocityX": 0.06879862918134139,
+            "velocityY": -0.10011792997737116,
+            "timestamp": 2.368347075189057
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 2.7611774763020037e-32,
-            "velocityX": -1.1210620234419235e-31,
-            "velocityY": 4.358756882663372e-31,
-            "timestamp": 2.382916076875216
+            "heading": -3.481182114535467e-34,
+            "angularVelocity": -2.7023024138211624e-34,
+            "velocityX": -3.7878173595367446e-32,
+            "velocityY": -1.5253102288533056e-31,
+            "timestamp": 2.3829160233617315
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.traj
index 92207f04..f95c4186 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.traj	
@@ -3,1091 +3,1091 @@
         {
             "x": 1.35,
             "y": 5.56,
-            "heading": 3.14,
+            "heading": 4.408218114222724e-33,
             "angularVelocity": 0,
-            "velocityX": 4.411137018023184e-34,
-            "velocityY": 2.784782344800776e-32,
+            "velocityX": 1.8935205228973059e-34,
+            "velocityY": 9.513896878368867e-32,
             "timestamp": 0
         },
         {
-            "x": 1.3544641699489606,
-            "y": 5.55925259826767,
-            "heading": 3.1400000009005375,
-            "angularVelocity": 3.865132870171629e-8,
-            "velocityX": 0.19160342420680077,
-            "velocityY": -0.032078691629102596,
-            "timestamp": 0.023299009229303495
-        },
-        {
-            "x": 1.3633925116373848,
-            "y": 5.557757806179583,
-            "heading": 3.1400000027300097,
-            "angularVelocity": 7.852144487332144e-8,
-            "velocityX": 0.383206925262499,
-            "velocityY": -0.06415689497258387,
-            "timestamp": 0.04659801845860699
-        },
-        {
-            "x": 1.3767850270669844,
-            "y": 5.555515636454451,
-            "heading": 3.140000005520194,
-            "angularVelocity": 1.1975551924808857e-7,
-            "velocityX": 0.5748105122322142,
-            "velocityY": -0.09623455242535102,
-            "timestamp": 0.06989702768791048
-        },
-        {
-            "x": 1.3946417184903905,
-            "y": 5.55252610340551,
-            "heading": 3.1400000093068963,
-            "angularVelocity": 1.6252630014020525e-7,
-            "velocityX": 0.7664141958854979,
-            "velocityY": -0.12831159554996177,
-            "timestamp": 0.09319603691721398
-        },
-        {
-            "x": 1.4169625884614865,
-            "y": 5.548789223260415,
-            "heading": 3.14000001413076,
-            "angularVelocity": 2.070415703537556e-7,
-            "velocityX": 0.9580179891521703,
-            "velocityY": -0.16038794217918884,
-            "timestamp": 0.11649504614651748
-        },
-        {
-            "x": 1.443747639900165,
-            "y": 5.544305014572816,
-            "heading": 3.1400000200383236,
-            "angularVelocity": 2.5355430687602407e-7,
-            "velocityX": 1.1496219077414547,
-            "velocityY": -0.1924634924804626,
-            "timestamp": 0.13979405537582099
-        },
-        {
-            "x": 1.474996876177077,
-            "y": 5.539073498761034,
-            "heading": 3.1400000270834036,
-            "angularVelocity": 3.0237681109940727e-7,
-            "velocityX": 1.3412259710000187,
-            "velocityY": -0.22453812350142868,
-            "timestamp": 0.1630930646051245
-        },
-        {
-            "x": 1.5107103012267313,
-            "y": 5.5330947008269575,
-            "heading": 3.1400000353289426,
-            "angularVelocity": 3.539008296544223e-7,
-            "velocityX": 1.5328302031287921,
-            "velocityY": -0.25661168143439866,
-            "timestamp": 0.186392073834428
-        },
-        {
-            "x": 1.5508879197018797,
-            "y": 5.526368650337421,
-            "heading": 3.140000044849547,
-            "angularVelocity": 4.086269951598913e-7,
-            "velocityX": 1.7244346349549924,
-            "velocityY": -0.2886839703505207,
-            "timestamp": 0.2096910830637315
-        },
-        {
-            "x": 1.5955297371899198,
-            "y": 5.5188953827998875,
-            "heading": 3.1400000557350607,
-            "angularVelocity": 4.6720929680542457e-7,
-            "velocityX": 1.9160393065938472,
-            "velocityY": -0.3207547352763359,
-            "timestamp": 0.232990092293035
-        },
-        {
-            "x": 1.6446357605259214,
-            "y": 5.510674941652546,
-            "heading": 3.140000068095764,
-            "angularVelocity": 5.305248328425052e-7,
-            "velocityX": 2.107644271595818,
-            "velocityY": -0.3528236358230067,
-            "timestamp": 0.25628910152233847
-        },
-        {
-            "x": 1.6982059982629796,
-            "y": 5.501707381254886,
-            "heading": 3.1400000820702374,
-            "angularVelocity": 5.997883043629641e-7,
-            "velocityX": 2.299249603699023,
-            "velocityY": -0.38489020324511874,
-            "timestamp": 0.27958811075164197
-        },
-        {
-            "x": 1.7562404614129117,
-            "y": 5.491992771597749,
-            "heading": 3.1400000978378597,
-            "angularVelocity": 6.767507519595272e-7,
-            "velocityX": 2.490855408432506,
-            "velocityY": -0.41695376664068867,
-            "timestamp": 0.3028871199809455
-        },
-        {
-            "x": 1.8187391646840712,
-            "y": 5.4815312061757675,
-            "heading": 3.140000115639928,
-            "angularVelocity": 7.640697380946438e-7,
-            "velocityX": 2.6824618444526434,
-            "velocityY": -0.4490133172205749,
-            "timestamp": 0.326186129210249
-        },
-        {
-            "x": 1.8857021287172593,
-            "y": 5.47032281621074,
-            "heading": 3.140000135818395,
-            "angularVelocity": 8.660654300433757e-7,
-            "velocityX": 2.874069166381749,
-            "velocityY": -0.48106723572405463,
-            "timestamp": 0.3494851384395525
-        },
-        {
-            "x": 1.9571293845792233,
-            "y": 5.458367799245384,
-            "heading": 3.14000015889542,
-            "angularVelocity": 9.904723735244152e-7,
-            "velocityX": 3.0656778217043392,
-            "velocityY": -0.5131126756388134,
-            "timestamp": 0.372784147668856
-        },
-        {
-            "x": 2.033020984323074,
-            "y": 5.445666486377279,
-            "heading": 3.140000185766696,
-            "angularVelocity": 0.000001153322655289093,
-            "velocityX": 3.2572887111610362,
-            "velocityY": -0.545143904752752,
-            "timestamp": 0.3960831568981595
-        },
-        {
-            "x": 2.113377033060567,
-            "y": 5.432219546645115,
-            "heading": 3.1400002183250084,
-            "angularVelocity": 0.0000013974118596484493,
-            "velocityX": 3.448904112043669,
-            "velocityY": -0.5771464185388335,
-            "timestamp": 0.419382166127463
-        },
-        {
-            "x": 2.1981978506136755,
-            "y": 5.41802904044786,
-            "heading": 3.1400002631776545,
-            "angularVelocity": 0.0000019250880740696254,
-            "velocityX": 3.640533239775037,
-            "velocityY": -0.6090604994243719,
-            "timestamp": 0.4426811753567665
-        },
-        {
-            "x": 2.2851188032420633,
-            "y": 5.403428173451014,
-            "heading": 3.140000263177585,
-            "angularVelocity": -2.9753453619857404e-12,
-            "velocityX": 3.73067162525748,
-            "velocityY": -0.6266732998482398,
-            "timestamp": 0.46598018458607
-        },
-        {
-            "x": 2.372043080680341,
-            "y": 5.388847120989839,
-            "heading": 3.140000263177669,
-            "angularVelocity": 3.609665835664611e-12,
-            "velocityX": 3.730814327029464,
-            "velocityY": -0.6258228544259767,
-            "timestamp": 0.4892791938153735
-        },
-        {
-            "x": 2.458969517114403,
-            "y": 5.374278945126162,
-            "heading": 3.140000263177672,
-            "angularVelocity": 1.1048975247425585e-13,
-            "velocityX": 3.7309069917332867,
-            "velocityY": -0.6252701872544597,
-            "timestamp": 0.5125782030446769
-        },
-        {
-            "x": 2.5458971210754204,
-            "y": 5.359717737461923,
-            "heading": 3.1400002631776736,
-            "angularVelocity": 8.62983817390415e-14,
-            "velocityX": 3.7309571023169674,
-            "velocityY": -0.6249711101842599,
-            "timestamp": 0.5358772122739804
-        },
-        {
-            "x": 2.632825281306951,
-            "y": 5.345159851008328,
-            "heading": 3.140000263177678,
-            "angularVelocity": 1.7012303031669384e-13,
-            "velocityX": 3.7309809776027962,
-            "velocityY": -0.6248285628956001,
-            "timestamp": 0.5591762215032839
-        },
-        {
-            "x": 2.7197537163641496,
-            "y": 5.330603605663881,
-            "heading": 3.1400002631776815,
-            "angularVelocity": 1.5893105684431692e-13,
-            "velocityX": 3.7309927731970562,
-            "velocityY": -0.6247581260336735,
-            "timestamp": 0.5824752307325874
-        },
-        {
-            "x": 2.8066823211500065,
-            "y": 5.316048373923756,
-            "heading": 3.140000263177685,
-            "angularVelocity": 1.4661255756791518e-13,
-            "velocityX": 3.7310000579983225,
-            "velocityY": -0.6247146218539804,
-            "timestamp": 0.605774239961891
-        },
-        {
-            "x": 2.8936112175364808,
-            "y": 5.301494883804396,
-            "heading": 3.140000263177688,
-            "angularVelocity": 1.4712645864153266e-13,
-            "velocityX": 3.731012573578327,
-            "velocityY": -0.6246398710039542,
-            "timestamp": 0.6290732491911945
-        },
-        {
-            "x": 2.9805406318812673,
-            "y": 5.286944487835434,
-            "heading": 3.1400002631776913,
-            "angularVelocity": 1.2989281986974027e-13,
-            "velocityX": 3.7310348044942168,
-            "velocityY": -0.6245070692018166,
-            "timestamp": 0.652372258420498
-        },
-        {
-            "x": 3.067470472751592,
-            "y": 5.272396640371872,
-            "heading": 3.140000263177696,
-            "angularVelocity": 2.2497040560944553e-13,
-            "velocityX": 3.7310531110902496,
-            "velocityY": -0.6243976866335479,
-            "timestamp": 0.6756712676498015
-        },
-        {
-            "x": 3.1544000489958055,
-            "y": 5.257847211710117,
-            "heading": 3.1400002631777,
-            "angularVelocity": 1.6698909622359586e-13,
-            "velocityX": 3.73104175326474,
-            "velocityY": -0.6244655520950864,
-            "timestamp": 0.698970276879105
-        },
-        {
-            "x": 3.24132763546162,
-            "y": 5.243285899348815,
-            "heading": 3.1400002631776998,
-            "angularVelocity": -2.5660759099238358e-14,
-            "velocityX": 3.7309563514197523,
-            "velocityY": -0.6249756038008819,
-            "timestamp": 0.7222692861084085
-        },
-        {
-            "x": 3.3282514237632603,
-            "y": 5.228701930838363,
-            "heading": 3.1400002631777038,
-            "angularVelocity": 1.8102189988940347e-13,
-            "velocityX": 3.730793333150296,
-            "velocityY": -0.62594801206375,
-            "timestamp": 0.745568295337712
-        },
-        {
-            "x": 3.4151697732878015,
-            "y": 5.214085582937486,
-            "heading": 3.140000263177707,
-            "angularVelocity": 1.4033623034653362e-13,
-            "velocityX": 3.7305598993143207,
-            "velocityY": -0.6273377445673929,
-            "timestamp": 0.7688673045670155
-        },
-        {
-            "x": 3.502081162717683,
-            "y": 5.199427905968822,
-            "heading": 3.1400002631777144,
-            "angularVelocity": 3.3137702069013046e-13,
-            "velocityX": 3.7302611701022674,
-            "velocityY": -0.6291115997313443,
-            "timestamp": 0.792166313796319
-        },
-        {
-            "x": 3.588983960416736,
-            "y": 5.18471937597994,
-            "heading": 3.140000263177713,
-            "angularVelocity": -6.829626544199834e-14,
-            "velocityX": 3.729892410608752,
-            "velocityY": -0.6312942256021684,
-            "timestamp": 0.8154653230256225
-        },
-        {
-            "x": 3.675876953607923,
-            "y": 5.169953034745646,
-            "heading": 3.140000263177716,
-            "angularVelocity": 1.3540589961163151e-13,
-            "velocityX": 3.7294715983848317,
-            "velocityY": -0.6337755004484098,
-            "timestamp": 0.838764332254926
-        },
-        {
-            "x": 3.7627586434088305,
-            "y": 5.155120331788361,
-            "heading": 3.1400002631777193,
-            "angularVelocity": 1.3533863072281271e-13,
-            "velocityX": 3.728986453707805,
-            "velocityY": -0.6366237641880743,
-            "timestamp": 0.8620633414842295
-        },
-        {
-            "x": 3.8490562562014863,
-            "y": 5.137199419063008,
-            "heading": 3.1400002631777224,
-            "angularVelocity": 1.3559815009211054e-13,
-            "velocityX": 3.7039177049679806,
-            "velocityY": -0.7691705921529395,
-            "timestamp": 0.885362350713533
-        },
-        {
-            "x": 3.9343637094417163,
-            "y": 5.1150391964944655,
-            "heading": 3.1400002631777255,
-            "angularVelocity": 1.370480919312873e-13,
-            "velocityX": 3.6614197797262853,
-            "velocityY": -0.9511229576576579,
-            "timestamp": 0.9086613599428365
+            "x": 1.3544641547566925,
+            "y": 5.559252486403593,
+            "heading": 5.298729726015063e-12,
+            "angularVelocity": 2.274228720676412e-10,
+            "velocityX": 0.1916026983161006,
+            "velocityY": -0.0320834805032552,
+            "timestamp": 0.02329901820759936
+        },
+        {
+            "x": 1.3633924655059346,
+            "y": 5.557757467272512,
+            "heading": 1.605830471024488e-11,
+            "angularVelocity": 4.618037851350714e-10,
+            "velocityX": 0.38320544967553505,
+            "velocityY": -0.06416661499484601,
+            "timestamp": 0.04659803641519872
+        },
+        {
+            "x": 1.3767849336296958,
+            "y": 5.555514951621415,
+            "heading": 3.246020157611451e-11,
+            "angularVelocity": 7.039737222967939e-10,
+            "velocityX": 0.574808260349448,
+            "velocityY": -0.09624936257493934,
+            "timestamp": 0.06989705462279808
+        },
+        {
+            "x": 1.3946415606835925,
+            "y": 5.552524949597437,
+            "heading": 5.470894313107332e-11,
+            "angularVelocity": 9.549218317932479e-10,
+            "velocityX": 0.7664111377908356,
+            "velocityY": -0.12833167463728912,
+            "timestamp": 0.09319607283039744
+        },
+        {
+            "x": 1.416962348431807,
+            "y": 5.548787472707871,
+            "heading": 8.303679095756625e-11,
+            "angularVelocity": 1.2158386690956822e-9,
+            "velocityX": 0.9580140909513821,
+            "velocityY": -0.1604134928036672,
+            "timestamp": 0.1164950910379968
+        },
+        {
+            "x": 1.4437472988920377,
+            "y": 5.544302534113244,
+            "heading": 1.1770979097345793e-10,
+            "angularVelocity": 1.4881742667086445e-9,
+            "velocityX": 1.1496171307120806,
+            "velocityY": -0.19249474611664116,
+            "timestamp": 0.13979410924559615
+        },
+        {
+            "x": 1.4749964143943655,
+            "y": 5.539070149011117,
+            "heading": 1.59035725627389e-10,
+            "angularVelocity": 1.7737199674396097e-9,
+            "velocityX": 1.3412202704804594,
+            "velocityY": -0.22457534714647368,
+            "timestamp": 0.1630931274531955
+        },
+        {
+            "x": 1.5107096976598688,
+            "y": 5.533090335148591,
+            "heading": 2.073747689126765e-10,
+            "angularVelocity": 2.0747244300773254e-9,
+            "velocityX": 1.532823527038234,
+            "velocityY": -0.25665518646593166,
+            "timestamp": 0.18639214566079487
+        },
+        {
+            "x": 1.5508871519080267,
+            "y": 5.526363113522473,
+            "heading": 2.631540997774015e-10,
+            "angularVelocity": 2.3940635247907603e-9,
+            "velocityX": 1.7244269217769022,
+            "velocityY": -0.28873412459698383,
+            "timestamp": 0.20969116386839423
+        },
+        {
+            "x": 1.595528781007412,
+            "y": 5.518888509361551,
+            "heading": 3.268885139183836e-10,
+            "angularVelocity": 2.7354977853334202e-9,
+            "velocityX": 1.9160304825557395,
+            "velocityY": -0.3208119799018042,
+            "timestamp": 0.2329901820759936
+        },
+        {
+            "x": 1.6446345896939198,
+            "y": 5.5106665535489805,
+            "heading": 3.9921044507968937e-10,
+            "angularVelocity": 3.104076267437521e-9,
+            "velocityX": 2.1076342466004383,
+            "velocityY": -0.35288850969337676,
+            "timestamp": 0.2562892002835929
+        },
+        {
+            "x": 1.698204583899122,
+            "y": 5.501697284762255,
+            "heading": 4.809154717853132e-10,
+            "angularVelocity": 3.5068011876967e-9,
+            "velocityX": 2.299238265229739,
+            "velocityY": -0.38496337943692166,
+            "timestamp": 0.2795882184911923
+        },
+        {
+            "x": 1.7562387712682006,
+            "y": 5.491980752848314,
+            "heading": 5.730347406124241e-10,
+            "angularVelocity": 3.953783188523521e-9,
+            "velocityX": 2.4908426119922917,
+            "velocityY": -0.41703610973659105,
+            "timestamp": 0.3028872366987917
+        },
+        {
+            "x": 1.8187371620272614,
+            "y": 5.48151702447463,
+            "heading": 6.769576861119728e-10,
+            "angularVelocity": 4.460400083403711e-9,
+            "velocityX": 2.6824473976619876,
+            "velocityY": -0.449105978650929,
+            "timestamp": 0.3261862549063911
+        },
+        {
+            "x": 1.8856997705540457,
+            "y": 5.4703061933617745,
+            "heading": 7.946577963057737e-10,
+            "angularVelocity": 5.051719599980361e-9,
+            "velocityX": 2.8740528004281045,
+            "velocityY": -0.4811718250535651,
+            "timestamp": 0.34948527311399047
+        },
+        {
+            "x": 1.957126618545275,
+            "y": 5.4583484009138274,
+            "heading": 9.291581669995643e-10,
+            "angularVelocity": 5.772791178735968e-9,
+            "velocityX": 3.0656591344232145,
+            "velocityY": -0.5132316023559669,
+            "timestamp": 0.37278429132158986
+        },
+        {
+            "x": 2.0330177424946343,
+            "y": 5.445643884917638,
+            "heading": 1.0856695619650843e-9,
+            "angularVelocity": 6.717510100758224e-9,
+            "velocityX": 3.257267034736916,
+            "velocityY": -0.5452811737842305,
+            "timestamp": 0.39608330952918924
+        },
+        {
+            "x": 2.1133732165489003,
+            "y": 5.432193128386492,
+            "heading": 1.2752962310893515e-9,
+            "angularVelocity": 8.13882633127911e-9,
+            "velocityX": 3.44887811744974,
+            "velocityY": -0.5773100141552516,
+            "timestamp": 0.41938232773678863
+        },
+        {
+            "x": 2.1981932628587333,
+            "y": 5.417997633484574,
+            "heading": 1.5377676317044546e-9,
+            "angularVelocity": 1.1265341261395982e-8,
+            "velocityX": 3.640498734927221,
+            "velocityY": -0.6092743812441802,
+            "timestamp": 0.442681345944388
+        },
+        {
+            "x": 2.285115669591906,
+            "y": 5.403405223128801,
+            "heading": 1.537767230961928e-9,
+            "angularVelocity": -1.719997404273206e-14,
+            "velocityX": 3.730732598201487,
+            "velocityY": -0.6263100970908951,
+            "timestamp": 0.4659803641519874
+        },
+        {
+            "x": 2.3720410537379144,
+            "y": 5.388830564421114,
+            "heading": 1.5377672475870007e-9,
+            "angularVelocity": 7.135525631450618e-16,
+            "velocityX": 3.7308603895453194,
+            "velocityY": -0.6255481916751834,
+            "timestamp": 0.4892793823595868
+        },
+        {
+            "x": 2.45896811239221,
+            "y": 5.374265896231001,
+            "heading": 1.5377672778413637e-9,
+            "angularVelocity": 1.298525326828493e-15,
+            "velocityX": 3.730932259882311,
+            "velocityY": -0.625119396028046,
+            "timestamp": 0.5125784005671862
+        },
+        {
+            "x": 2.5458958805121723,
+            "y": 5.359705463035692,
+            "heading": 1.5377672869369513e-9,
+            "angularVelocity": 3.903849318113422e-16,
+            "velocityX": 3.730962710335439,
+            "velocityY": -0.6249376289364498,
+            "timestamp": 0.5358774187747856
+        },
+        {
+            "x": 2.632823878731898,
+            "y": 5.345146403658019,
+            "heading": 1.5377672987189865e-9,
+            "angularVelocity": 5.056879723770718e-16,
+            "velocityX": 3.7309725862775656,
+            "velocityY": -0.6248786643237555,
+            "timestamp": 0.559176436982385
+        },
+        {
+            "x": 2.7197519358547084,
+            "y": 5.330587695987482,
+            "heading": 1.5377673268477785e-9,
+            "angularVelocity": 1.2072952571647457e-15,
+            "velocityX": 3.7309751144130874,
+            "velocityY": -0.6248635689629851,
+            "timestamp": 0.5824754551899843
+        },
+        {
+            "x": 2.806680023647356,
+            "y": 5.316029171436071,
+            "heading": 1.5377673567071691e-9,
+            "angularVelocity": 1.2815730065587283e-15,
+            "velocityX": 3.7309764307702067,
+            "velocityY": -0.6248557094441328,
+            "timestamp": 0.6057744733975837
+        },
+        {
+            "x": 2.8936082747189524,
+            "y": 5.301471621825431,
+            "heading": 1.5377673928791505e-9,
+            "angularVelocity": 1.5525110632783364e-15,
+            "velocityX": 3.7309834387453797,
+            "velocityY": -0.6248138647313558,
+            "timestamp": 0.6290734916051831
+        },
+        {
+            "x": 2.980536964716405,
+            "y": 5.286916693414742,
+            "heading": 1.5377674058328616e-9,
+            "angularVelocity": 5.559766105183297e-16,
+            "velocityX": 3.7310022775585137,
+            "velocityY": -0.6247013621403136,
+            "timestamp": 0.6523725098127825
+        },
+        {
+            "x": 3.0674663067511823,
+            "y": 5.272365659809418,
+            "heading": 1.5377674408459054e-9,
+            "angularVelocity": 1.5027691524839091e-15,
+            "velocityX": 3.73103026317236,
+            "velocityY": -0.6245341960676912,
+            "timestamp": 0.6756715280203819
+        },
+        {
+            "x": 3.154396100803892,
+            "y": 5.257817326905748,
+            "heading": 1.5377674636831172e-9,
+            "angularVelocity": 9.801792492345066e-16,
+            "velocityX": 3.7310496639016963,
+            "velocityY": -0.624418281237505,
+            "timestamp": 0.6989705462279813
+        },
+        {
+            "x": 3.2413249978571956,
+            "y": 5.243263635140824,
+            "heading": 1.5377674638426843e-9,
+            "angularVelocity": 6.848580307945622e-18,
+            "velocityX": 3.731011164452385,
+            "velocityY": -0.6246482849651879,
+            "timestamp": 0.7222695644355807
+        },
+        {
+            "x": 3.3282506523054423,
+            "y": 5.228690588454777,
+            "heading": 1.5377674749707248e-9,
+            "angularVelocity": 4.776183371381179e-16,
+            "velocityX": 3.7308719909897503,
+            "velocityY": -0.6254790032748808,
+            "timestamp": 0.74556858264318
+        },
+        {
+            "x": 3.415171009638962,
+            "y": 5.214085980519485,
+            "heading": 1.5377674915535578e-9,
+            "angularVelocity": 7.117396517888937e-16,
+            "velocityX": 3.7306446374302076,
+            "velocityY": -0.626833620407003,
+            "timestamp": 0.7688676008507794
+        },
+        {
+            "x": 3.5020842171218036,
+            "y": 5.1994388833661285,
+            "heading": 1.53776751807092e-9,
+            "angularVelocity": 1.13813226337154e-15,
+            "velocityX": 3.730337763955042,
+            "velocityY": -0.6286572688425895,
+            "timestamp": 0.7921666190583788
+        },
+        {
+            "x": 3.5889883305639807,
+            "y": 5.184737925794629,
+            "heading": 1.5377675551515876e-9,
+            "angularVelocity": 1.5915121086693097e-15,
+            "velocityX": 3.729947445333141,
+            "velocityY": -0.6309689721834142,
+            "timestamp": 0.8154656372659782
+        },
+        {
+            "x": 3.675881697311449,
+            "y": 5.1699735802440445,
+            "heading": 1.5377675887419511e-9,
+            "angularVelocity": 1.441707334130238e-15,
+            "velocityX": 3.7294861943638535,
+            "velocityY": -0.6336896009521162,
+            "timestamp": 0.8387646554735776
+        },
+        {
+            "x": 3.7627625314341118,
+            "y": 5.155135664387593,
+            "heading": 1.5377676077743658e-9,
+            "angularVelocity": 8.168762038570203e-16,
+            "velocityX": 3.728948290804007,
+            "velocityY": -0.636847257870781,
+            "timestamp": 0.862063673681177
+        },
+        {
+            "x": 3.8490591061764308,
+            "y": 5.137209586747657,
+            "heading": 1.5377676390897132e-9,
+            "angularVelocity": 1.344063051824819e-15,
+            "velocityX": 3.703871724266177,
+            "velocityY": -0.7693919752476525,
+            "timestamp": 0.8853626918887764
+        },
+        {
+            "x": 3.934365266987087,
+            "y": 5.115044254457407,
+            "heading": 1.5377676702466937e-9,
+            "angularVelocity": 1.337265902944942e-15,
+            "velocityX": 3.661362897377225,
+            "velocityY": -0.9513419017440721,
+            "timestamp": 0.9086617100963758
         },
         {
             "x": 4.018431663513184,
             "y": 5.088562488555908,
-            "heading": 3.140000263177729,
-            "angularVelocity": 1.413301961445022e-13,
-            "velocityX": 3.6082201283346866,
-            "velocityY": -1.1363877183834126,
-            "timestamp": 0.93196036917214
-        },
-        {
-            "x": 4.095482183944048,
-            "y": 5.0601896166081985,
-            "heading": 3.140000263177732,
-            "angularVelocity": 1.478757127486931e-13,
-            "velocityX": 3.549906540385796,
-            "velocityY": -1.3072078310833697,
-            "timestamp": 0.9536653125429956
-        },
-        {
-            "x": 4.171099571738343,
-            "y": 5.028192533992809,
-            "heading": 3.1400002631777353,
-            "angularVelocity": 1.4523452590479803e-13,
-            "velocityX": 3.483878603243662,
-            "velocityY": -1.4741841095171686,
-            "timestamp": 0.9753702559138512
-        },
-        {
-            "x": 4.2451557679063034,
-            "y": 4.992731534520922,
-            "heading": 3.1400002631777384,
-            "angularVelocity": 1.4237189069796376e-13,
-            "velocityX": 3.4119506742159467,
-            "velocityY": -1.6337752587458831,
-            "timestamp": 0.9970751992847068
-        },
-        {
-            "x": 4.31781550635648,
-            "y": 4.954490651976443,
-            "heading": 3.1400002631777415,
-            "angularVelocity": 1.4182458601020346e-13,
-            "velocityX": 3.347612440573966,
-            "velocityY": -1.761851293089538,
-            "timestamp": 1.0187801426555625
-        },
-        {
-            "x": 4.390182790341615,
-            "y": 4.91569917224677,
-            "heading": 3.140000263177745,
-            "angularVelocity": 1.421722636666585e-13,
-            "velocityX": 3.3341383457540337,
-            "velocityY": -1.787218656449447,
-            "timestamp": 1.040485086026418
-        },
-        {
-            "x": 4.462533664586848,
-            "y": 4.87687709490227,
-            "heading": 3.1400002631777486,
-            "angularVelocity": 1.635268036057102e-13,
-            "velocityX": 3.333382308772387,
-            "velocityY": -1.7886283636415046,
-            "timestamp": 1.0621900293972737
-        },
-        {
-            "x": 4.534860115355159,
-            "y": 4.838009534998386,
-            "heading": 3.140000263177751,
-            "angularVelocity": 1.1808318022164126e-13,
-            "velocityX": 3.3322570592611274,
-            "velocityY": -1.7907238567558525,
-            "timestamp": 1.0838949727681293
-        },
-        {
-            "x": 4.607177418550424,
-            "y": 4.799124957707118,
-            "heading": 3.140000263177754,
-            "angularVelocity": 1.4385253669729274e-13,
-            "velocityX": 3.3318356081243334,
-            "velocityY": -1.7915078895502723,
-            "timestamp": 1.105599916138985
-        },
-        {
-            "x": 4.679489672949611,
-            "y": 4.7602309921876325,
-            "heading": 3.1400002631777575,
-            "angularVelocity": 1.4120580598782794e-13,
-            "velocityX": 3.331602997698768,
-            "velocityY": -1.7919404282640008,
-            "timestamp": 1.1273048595098405
-        },
-        {
-            "x": 4.751800053049455,
-            "y": 4.721333542151952,
-            "heading": 3.1400002631777606,
-            "angularVelocity": 1.4195129118241079e-13,
-            "velocityX": 3.3315166441264745,
-            "velocityY": -1.7921009684797435,
-            "timestamp": 1.149009802880696
-        },
-        {
-            "x": 4.824110260922509,
-            "y": 4.682435771933003,
-            "heading": 3.1400002631777637,
-            "angularVelocity": 1.420416383867913e-13,
-            "velocityX": 3.3315087092142113,
-            "velocityY": -1.7921157201114153,
-            "timestamp": 1.1707147462515517
-        },
-        {
-            "x": 4.896420645274053,
-            "y": 4.643538329777489,
-            "heading": 3.140000263177767,
-            "angularVelocity": 1.4171119036284518e-13,
-            "velocityX": 3.3315168400108037,
-            "velocityY": -1.7921006054248252,
-            "timestamp": 1.1924196896224073
-        },
-        {
-            "x": 4.96873125432361,
-            "y": 4.6046413053457185,
-            "heading": 3.14000026317777,
-            "angularVelocity": 1.4137748376763704e-13,
-            "velocityX": 3.3315271924000274,
-            "velocityY": -1.792081359866257,
-            "timestamp": 1.214124632993263
-        },
-        {
-            "x": 5.04104400801269,
-            "y": 4.56574826813338,
-            "heading": 3.1400002631777735,
-            "angularVelocity": 1.421351986736705e-13,
-            "velocityX": 3.331626001206414,
-            "velocityY": -1.7918976588811946,
-            "timestamp": 1.2358295763641185
-        },
-        {
-            "x": 5.11336222581346,
-            "y": 4.5268653919090065,
-            "heading": 3.1400002631777766,
-            "angularVelocity": 1.4239328375117255e-13,
-            "velocityX": 3.3318777462404188,
-            "velocityY": -1.7914295172313686,
-            "timestamp": 1.257534519734974
-        },
-        {
-            "x": 5.185674775789392,
-            "y": 4.487971975945349,
-            "heading": 3.1400002631777797,
-            "angularVelocity": 1.4189340221530015e-13,
-            "velocityX": 3.331616615642773,
-            "velocityY": -1.7919151088820495,
-            "timestamp": 1.2792394631058297
-        },
-        {
-            "x": 5.25798679998857,
-            "y": 4.4490775824474165,
-            "heading": 3.140000263177783,
-            "angularVelocity": 1.4182847689207558e-13,
-            "velocityX": 3.331592391818018,
-            "velocityY": -1.791960146283848,
-            "timestamp": 1.3009444064766853
-        },
-        {
-            "x": 5.330301009987797,
-            "y": 4.410187253034054,
-            "heading": 3.1400002631777864,
-            "angularVelocity": 1.4221031722558925e-13,
-            "velocityX": 3.331693096990619,
-            "velocityY": -1.7917729039338035,
-            "timestamp": 1.322649349847541
-        },
-        {
-            "x": 5.4026181052176305,
-            "y": 4.3713022890025295,
-            "heading": 3.1400002631777895,
-            "angularVelocity": 1.4161705945188678e-13,
-            "velocityX": 3.3318260266449937,
-            "velocityY": -1.791525707625898,
-            "timestamp": 1.3443542932183965
-        },
-        {
-            "x": 5.474936056465776,
-            "y": 4.332418917012381,
-            "heading": 3.1400002631777926,
-            "angularVelocity": 1.4174219306640175e-13,
-            "velocityX": 3.331865465508907,
-            "velocityY": -1.7914523583735324,
-            "timestamp": 1.366059236589252
-        },
-        {
-            "x": 5.54725002868687,
-            "y": 4.293528145471586,
-            "heading": 3.1400002631777957,
-            "angularVelocity": 1.4114123611773667e-13,
-            "velocityX": 3.331682141968083,
-            "velocityY": -1.7917932738304505,
-            "timestamp": 1.3877641799601077
-        },
-        {
-            "x": 5.619560804995751,
-            "y": 4.2546314319979865,
-            "heading": 3.1400002631777992,
-            "angularVelocity": 1.438802546332647e-13,
-            "velocityX": 3.331534898449109,
-            "velocityY": -1.7920670332572342,
-            "timestamp": 1.4094691233309633
-        },
-        {
-            "x": 5.691871264235437,
-            "y": 4.215734129079228,
-            "heading": 3.1400002631778023,
-            "angularVelocity": 1.4227513855970737e-13,
-            "velocityX": 3.331520290292617,
-            "velocityY": -1.7920941904433432,
-            "timestamp": 1.431174066701819
-        },
-        {
-            "x": 5.7641829192535345,
-            "y": 4.1768390492074925,
-            "heading": 3.1400002631778055,
-            "angularVelocity": 1.4198857424426595e-13,
-            "velocityX": 3.3315753827383325,
-            "velocityY": -1.79199176921174,
-            "timestamp": 1.4528790100726745
-        },
-        {
-            "x": 5.836496897649906,
-            "y": 4.13794828914952,
-            "heading": 3.1400002631778086,
-            "angularVelocity": 1.420236059365936e-13,
-            "velocityX": 3.331682426477163,
-            "velocityY": -1.7917927447907471,
-            "timestamp": 1.47458395344353
-        },
-        {
-            "x": 5.90881236073979,
-            "y": 4.099060289876852,
-            "heading": 3.1400002631778117,
-            "angularVelocity": 1.4199973949506742e-13,
-            "velocityX": 3.3317508299509484,
-            "velocityY": -1.7916655486362405,
-            "timestamp": 1.4962888968143857
-        },
-        {
-            "x": 5.981129099244504,
-            "y": 4.06017466243704,
-            "heading": 3.140000263177815,
-            "angularVelocity": 1.4813660843765197e-13,
-            "velocityX": 3.3318095914418606,
-            "velocityY": -1.7915562724764487,
-            "timestamp": 1.5179938401852413
-        },
-        {
-            "x": 6.053446077627243,
-            "y": 4.021289481108035,
-            "heading": 3.140000263177829,
-            "angularVelocity": 6.425453029046258e-13,
-            "velocityX": 3.331820643210356,
-            "velocityY": -1.7915357190582988,
-            "timestamp": 1.539698783556097
-        },
-        {
-            "x": 6.125761091363011,
-            "y": 3.9824006462217088,
-            "heading": 3.140000263177869,
-            "angularVelocity": 1.8383501574993634e-12,
-            "velocityX": 3.331730127102014,
-            "velocityY": -1.7917040474119204,
-            "timestamp": 1.5614037269269525
-        },
-        {
-            "x": 6.198074029937981,
-            "y": 3.9435079527230394,
-            "heading": 3.140000263178238,
-            "angularVelocity": 1.7022488728392152e-11,
-            "velocityX": 3.331634519350639,
-            "velocityY": -1.79188182315355,
-            "timestamp": 1.583108670297808
-        },
-        {
-            "x": 6.270380104650612,
-            "y": 3.9046024999630506,
-            "heading": 3.140000263177847,
-            "angularVelocity": -1.8008440986287348e-11,
-            "velocityX": 3.33131828436938,
-            "velocityY": -1.7924696736324577,
-            "timestamp": 1.6048136136686637
-        },
-        {
-            "x": 6.342678298341779,
-            "y": 3.8656824038223503,
-            "heading": 3.1400002631782598,
-            "angularVelocity": 1.900200591260141e-11,
-            "velocityX": 3.3309551863724884,
-            "velocityY": -1.7931443301131518,
-            "timestamp": 1.6265185570395193
-        },
-        {
-            "x": 6.414967377910046,
-            "y": 3.8267453820171355,
-            "heading": 3.1400002631778587,
-            "angularVelocity": -1.8477715999059415e-11,
-            "velocityX": 3.3305352763678626,
-            "velocityY": -1.7939241370065324,
-            "timestamp": 1.648223500410375
-        },
-        {
-            "x": 6.487241234316261,
-            "y": 3.787780110681628,
-            "heading": 3.1400002631778627,
-            "angularVelocity": 1.954226890690534e-13,
-            "velocityX": 3.3298339079393355,
-            "velocityY": -1.795225662182324,
-            "timestamp": 1.6699284437812305
-        },
-        {
-            "x": 6.559496489740858,
-            "y": 3.7487803574447534,
-            "heading": 3.1400002631778654,
-            "angularVelocity": 1.160046399416623e-13,
-            "velocityX": 3.3289769150756516,
-            "velocityY": -1.79681432798537,
-            "timestamp": 1.691633387152086
-        },
-        {
-            "x": 6.630585131325424,
-            "y": 3.7076925524537225,
-            "heading": 3.140000263177869,
-            "angularVelocity": 1.5938094021424355e-13,
-            "velocityX": 3.275228152865242,
-            "velocityY": -1.8930159958948245,
-            "timestamp": 1.7133383305229417
-        },
-        {
-            "x": 6.699761336489547,
-            "y": 3.6634607167510884,
-            "heading": 3.140000263177872,
-            "angularVelocity": 1.431049344650049e-13,
-            "velocityX": 3.1871175142993193,
-            "velocityY": -2.037869205501672,
-            "timestamp": 1.7350432738937973
-        },
-        {
-            "x": 6.76677589557052,
-            "y": 3.6160175775021286,
-            "heading": 3.140000263178055,
-            "angularVelocity": 8.443311888840044e-12,
-            "velocityX": 3.087525175069758,
-            "velocityY": -2.1858218396761058,
-            "timestamp": 1.756748217264653
-        },
-        {
-            "x": 6.8314564099651545,
-            "y": 3.5654385941456423,
-            "heading": 3.140000263178058,
-            "angularVelocity": 1.5118918552164865e-13,
-            "velocityX": 2.9799900091650118,
-            "velocityY": -2.3302978723451035,
-            "timestamp": 1.7784531606355085
+            "heading": 1.5377676866838871e-9,
+            "angularVelocity": 7.054887729794734e-16,
+            "velocityX": 3.608151887646215,
+            "velocityY": -1.1366043695780652,
+            "timestamp": 0.9319607283039751
+        },
+        {
+            "x": 4.095480434320656,
+            "y": 5.06018496604887,
+            "heading": 1.5377676994335126e-9,
+            "angularVelocity": 5.874067965982521e-16,
+            "velocityX": 3.549827430213289,
+            "velocityY": -1.3074226459526814,
+            "timestamp": 0.9536656625077448
+        },
+        {
+            "x": 4.171095847536846,
+            "y": 5.028183306370306,
+            "heading": 1.5377677188417825e-9,
+            "angularVelocity": 8.941870233905506e-16,
+            "velocityX": 3.4837891009642465,
+            "velocityY": -1.4743956087592989,
+            "timestamp": 0.9753705967115144
+        },
+        {
+            "x": 4.24514985906727,
+            "y": 4.992717825181937,
+            "heading": 1.5377677281143787e-9,
+            "angularVelocity": 4.2721152582009774e-16,
+            "velocityX": 3.4118514636002706,
+            "velocityY": -1.6339824325334937,
+            "timestamp": 0.997075530915284
+        },
+        {
+            "x": 4.317807248978416,
+            "y": 4.95447255507026,
+            "heading": 1.5377677290979519e-9,
+            "angularVelocity": 4.531574211416763e-17,
+            "velocityX": 3.3475056514378028,
+            "velocityY": -1.7620541832797834,
+            "timestamp": 1.0187804651190535
+        },
+        {
+            "x": 4.3901722659206355,
+            "y": 4.9156769197600605,
+            "heading": 1.5377677449553673e-9,
+            "angularVelocity": 7.305902735413318e-16,
+            "velocityX": 3.334035305654692,
+            "velocityY": -1.7874108691538961,
+            "timestamp": 1.0404853993228231
+        },
+        {
+            "x": 4.4625185516483485,
+            "y": 4.876846365558822,
+            "heading": 1.5377677746336623e-9,
+            "angularVelocity": 1.3673523815751288e-15,
+            "velocityX": 3.333172312277143,
+            "velocityY": -1.7890196688239732,
+            "timestamp": 1.0621903335265928
+        },
+        {
+            "x": 4.534844691324942,
+            "y": 4.837978300028894,
+            "heading": 1.5377677779199108e-9,
+            "angularVelocity": 1.5140568225814148e-16,
+            "velocityX": 3.3322441338764652,
+            "velocityY": -1.7907479085103777,
+            "timestamp": 1.0838952677303624
+        },
+        {
+            "x": 4.6071619879089205,
+            "y": 4.799093783673277,
+            "heading": 1.537767801979053e-9,
+            "angularVelocity": 1.1084641137098069e-15,
+            "velocityX": 3.331836710728115,
+            "velocityY": -1.79150583873765,
+            "timestamp": 1.105600201934132
+        },
+        {
+            "x": 4.6794740379059565,
+            "y": 4.7601995113399385,
+            "heading": 1.5377678257958516e-9,
+            "angularVelocity": 1.0972987247710634e-15,
+            "velocityX": 3.331594987489656,
+            "velocityY": -1.791955320763837,
+            "timestamp": 1.1273051361379016
+        },
+        {
+            "x": 4.751784687579244,
+            "y": 4.721302635647572,
+            "heading": 1.5377678370559733e-9,
+            "angularVelocity": 5.187816897475249e-16,
+            "velocityX": 3.331530471108496,
+            "velocityY": -1.7920752639579989,
+            "timestamp": 1.1490100703416712
+        },
+        {
+            "x": 4.824095678023389,
+            "y": 4.682406393453542,
+            "heading": 1.5377678370330874e-9,
+            "angularVelocity": -1.0543217693671843e-18,
+            "velocityX": 3.3315461712634193,
+            "velocityY": -1.792046077125237,
+            "timestamp": 1.1707150045454409
+        },
+        {
+            "x": 4.896406945309153,
+            "y": 4.643510665929771,
+            "heading": 1.5377678640109226e-9,
+            "angularVelocity": 1.242935520614447e-15,
+            "velocityX": 3.331558926041247,
+            "velocityY": -1.7920223649940337,
+            "timestamp": 1.1924199387492105
+        },
+        {
+            "x": 4.968718824670649,
+            "y": 4.604616076350799,
+            "heading": 1.5377678830932924e-9,
+            "angularVelocity": 8.791718726461835e-16,
+            "velocityX": 3.33158712588479,
+            "velocityY": -1.7919699370612494,
+            "timestamp": 1.21412487295298
+        },
+        {
+            "x": 5.041033201559798,
+            "y": 4.565726130481367,
+            "heading": 1.5377679126898002e-9,
+            "angularVelocity": 1.3635842497565112e-15,
+            "velocityX": 3.331702193162082,
+            "velocityY": -1.7917559898752364,
+            "timestamp": 1.2358298071567497
+        },
+        {
+            "x": 5.113354228854967,
+            "y": 4.526848553325412,
+            "heading": 1.5377679256265375e-9,
+            "angularVelocity": 5.960275144059911e-16,
+            "velocityX": 3.332008593816099,
+            "velocityY": -1.7911861326534588,
+            "timestamp": 1.2575347413605193
+        },
+        {
+            "x": 5.1856663573422805,
+            "y": 4.487954426928371,
+            "heading": 1.537767949104212e-9,
+            "angularVelocity": 1.0816744331209405e-15,
+            "velocityX": 3.3315986037289687,
+            "velocityY": -1.791948597121158,
+            "timestamp": 1.279239675564289
+        },
+        {
+            "x": 5.257977857015597,
+            "y": 4.449059131458437,
+            "heading": 1.5377679798803482e-9,
+            "angularVelocity": 1.4179326501228622e-15,
+            "velocityX": 3.3315696327123336,
+            "velocityY": -1.7920024592017922,
+            "timestamp": 1.3009446097680586
+        },
+        {
+            "x": 5.330290964048228,
+            "y": 4.410166824431303,
+            "heading": 1.5377679920779735e-9,
+            "angularVelocity": 5.619748235100741e-16,
+            "velocityX": 3.3316436877316113,
+            "velocityY": -1.7918647742503244,
+            "timestamp": 1.3226495439718282
+        },
+        {
+            "x": 5.402605809232356,
+            "y": 4.371277749351831,
+            "heading": 1.5377680157791104e-9,
+            "angularVelocity": 1.0919698963856968e-15,
+            "velocityX": 3.3317237686705528,
+            "velocityY": -1.7917158704270917,
+            "timestamp": 1.3443544781755978
+        },
+        {
+            "x": 5.474921974691154,
+            "y": 4.332391129443428,
+            "heading": 1.5377680240202577e-9,
+            "angularVelocity": 3.7969007641948726e-16,
+            "velocityX": 3.331784596990179,
+            "velocityY": -1.7916027546232325,
+            "timestamp": 1.3660594123793675
+        },
+        {
+            "x": 5.54723622900274,
+            "y": 4.29350095564713,
+            "heading": 1.537768038636468e-9,
+            "angularVelocity": 6.734049902652557e-16,
+            "velocityX": 3.331696545710963,
+            "velocityY": -1.7917664910290456,
+            "timestamp": 1.387764346583137
+        },
+        {
+            "x": 5.619548385303401,
+            "y": 4.254606880959325,
+            "heading": 1.5377680494372407e-9,
+            "angularVelocity": 4.976183803494664e-16,
+            "velocityX": 3.331599885158129,
+            "velocityY": -1.7919462147497014,
+            "timestamp": 1.4094692807869067
+        },
+        {
+            "x": 5.6918607364808045,
+            "y": 4.215713168590034,
+            "heading": 1.5377680578875737e-9,
+            "angularVelocity": 3.893277832205449e-16,
+            "velocityX": 3.3316088636117085,
+            "velocityY": -1.7919295218396705,
+            "timestamp": 1.4311742149906763
+        },
+        {
+            "x": 5.764173499508255,
+            "y": 4.176820221955942,
+            "heading": 1.5377680813491443e-9,
+            "angularVelocity": 1.0809325075471762e-15,
+            "velocityX": 3.331627838562525,
+            "velocityY": -1.7918942425240987,
+            "timestamp": 1.452879149194446
+        },
+        {
+            "x": 5.8364876254997204,
+            "y": 4.137929809554028,
+            "heading": 1.5377680862134134e-9,
+            "angularVelocity": 2.241089873857745e-16,
+            "velocityX": 3.331690633686676,
+            "velocityY": -1.7917774841780256,
+            "timestamp": 1.4745840833982156
+        },
+        {
+            "x": 5.90880309835116,
+            "y": 4.099041901645272,
+            "heading": 1.537768099475084e-9,
+            "angularVelocity": 6.109980238433844e-16,
+            "velocityX": 3.3317526868576484,
+            "velocityY": -1.7916620959822775,
+            "timestamp": 1.4962890176019852
+        },
+        {
+            "x": 5.981120307623631,
+            "y": 4.060157222932383,
+            "heading": 1.5377681005691863e-9,
+            "angularVelocity": 5.040806501517701e-17,
+            "velocityX": 3.331832688069564,
+            "velocityY": -1.7915133189414525,
+            "timestamp": 1.5179939518057548
+        },
+        {
+            "x": 6.053439067813782,
+            "y": 4.0212754287411325,
+            "heading": 1.5377681034134081e-9,
+            "angularVelocity": 1.3104042715425496e-16,
+            "velocityX": 3.3319041426820193,
+            "velocityY": -1.7913804218969325,
+            "timestamp": 1.5396988860095244
+        },
+        {
+            "x": 6.1257574594958575,
+            "y": 3.9823929491454755,
+            "heading": 1.5377681141371771e-9,
+            "angularVelocity": 4.940705905043031e-16,
+            "velocityX": 3.3318871646008015,
+            "velocityY": -1.7914120001783713,
+            "timestamp": 1.561403820213294
+        },
+        {
+            "x": 6.198071250397711,
+            "y": 3.943501913620472,
+            "heading": 1.5377681379511677e-9,
+            "angularVelocity": 1.09716939803524e-15,
+            "velocityX": 3.3316751952742036,
+            "velocityY": -1.7918061930096516,
+            "timestamp": 1.5831087544170637
+        },
+        {
+            "x": 6.270375940604977,
+            "y": 3.9045939610161593,
+            "heading": 1.5377681515073034e-9,
+            "angularVelocity": 6.245647694184294e-16,
+            "velocityX": 3.331255903765101,
+            "velocityY": -1.7925856046957434,
+            "timestamp": 1.6048136886208333
+        },
+        {
+            "x": 6.342673755736347,
+            "y": 3.865673234790299,
+            "heading": 1.5377699302540744e-9,
+            "angularVelocity": 8.19513085832153e-14,
+            "velocityX": 3.3309391520146527,
+            "velocityY": -1.7931741170331514,
+            "timestamp": 1.626518622824603
+        },
+        {
+            "x": 6.414965746925861,
+            "y": 3.826741692219575,
+            "heading": 1.5377690472977265e-9,
+            "angularVelocity": -4.068002902168065e-14,
+            "velocityX": 3.3306708286121007,
+            "velocityY": -1.7936724527872456,
+            "timestamp": 1.6482235570283725
+        },
+        {
+            "x": 6.487241720022193,
+            "y": 3.7877804203169507,
+            "heading": 1.5377684535437603e-9,
+            "angularVelocity": -2.7355713192172554e-14,
+            "velocityX": 3.3299328354443367,
+            "velocityY": -1.7950421566324242,
+            "timestamp": 1.6699284912321422
+        },
+        {
+            "x": 6.559496923985796,
+            "y": 3.748780644748349,
+            "heading": 1.5377682143039968e-9,
+            "angularVelocity": -1.1022367481483692e-14,
+            "velocityX": 3.3289759501347236,
+            "velocityY": -1.796816115748624,
+            "timestamp": 1.6916334254359118
+        },
+        {
+            "x": 6.630585509558441,
+            "y": 3.7076928121475503,
+            "heading": 1.5377682448018014e-9,
+            "angularVelocity": 1.4051092264620289e-15,
+            "velocityX": 3.2752269555510436,
+            "velocityY": -1.8930180674612387,
+            "timestamp": 1.7133383596396814
+        },
+        {
+            "x": 6.6997616539527085,
+            "y": 3.6634609457782683,
+            "heading": 1.5377682789348178e-9,
+            "angularVelocity": 1.5725923809489335e-15,
+            "velocityX": 3.1871160605618085,
+            "velocityY": -2.037871479084932,
+            "timestamp": 1.735043293843451
+        },
+        {
+            "x": 6.7667761400423245,
+            "y": 3.6160177634442383,
+            "heading": 1.5377682945212807e-9,
+            "angularVelocity": 7.181069480230158e-16,
+            "velocityX": 3.0875231161945336,
+            "velocityY": -2.185824747894625,
+            "timestamp": 1.7567482280472206
+        },
+        {
+            "x": 6.831456555555867,
+            "y": 3.5654387099344507,
+            "heading": 1.5377683209709385e-9,
+            "angularVelocity": 1.2186011773240001e-15,
+            "velocityX": 2.979986712068234,
+            "velocityY": -2.330302088683456,
+            "timestamp": 1.7784531622509903
         },
         {
             "x": 6.893649101257324,
             "y": 3.5118300914764404,
-            "heading": 3.1400002631780617,
-            "angularVelocity": 1.5501587028498165e-13,
-            "velocityX": 2.865369894292817,
-            "velocityY": -2.469875260820684,
-            "timestamp": 1.8001581040063641
-        },
-        {
-            "x": 6.9342196158595195,
-            "y": 3.4745269537623855,
-            "heading": 3.140000263178065,
-            "angularVelocity": 2.3326761756772743e-13,
-            "velocityX": 2.784724807966257,
-            "velocityY": -2.5604549024296013,
-            "timestamp": 1.8147270533280853
-        },
-        {
-            "x": 6.973574770579654,
-            "y": 3.435943763171971,
-            "heading": 3.140000263178068,
-            "angularVelocity": 2.2697466808256025e-13,
-            "velocityX": 2.701303563563437,
-            "velocityY": -2.648316617649584,
-            "timestamp": 1.8292960026498064
-        },
-        {
-            "x": 7.011676569760775,
-            "y": 3.396122365046946,
-            "heading": 3.140000263178071,
-            "angularVelocity": 2.224027473304724e-13,
-            "velocityX": 2.615274330340327,
-            "velocityY": -2.7333061050358776,
-            "timestamp": 1.8438649519715276
-        },
-        {
-            "x": 7.048491012484172,
-            "y": 3.355107883496788,
-            "heading": 3.1400002631780746,
-            "angularVelocity": 2.1692590269440246e-13,
-            "velocityX": 2.5269113036529633,
-            "velocityY": -2.815198312825684,
-            "timestamp": 1.8584339012932487
-        },
-        {
-            "x": 7.083992093604447,
-            "y": 3.312951458599195,
-            "heading": 3.1400002631780777,
-            "angularVelocity": 2.1487378521690259e-13,
-            "velocityX": 2.4367633064212786,
-            "velocityY": -2.893580310196796,
-            "timestamp": 1.8730028506149698
-        },
-        {
-            "x": 7.118173429008876,
-            "y": 3.269718054236438,
-            "heading": 3.140000263178081,
-            "angularVelocity": 2.1169602046875272e-13,
-            "velocityX": 2.3461771092494,
-            "velocityY": -2.967503243236169,
-            "timestamp": 1.887571799936691
-        },
-        {
-            "x": 7.151084190942531,
-            "y": 3.2255097664946186,
-            "heading": 3.140000263178084,
-            "angularVelocity": 2.115299519607656e-13,
-            "velocityX": 2.258966052177459,
-            "velocityY": -3.034418389794455,
-            "timestamp": 1.9021407492584121
-        },
-        {
-            "x": 7.1829258498520225,
-            "y": 3.1805253278057903,
-            "heading": 3.1400002631783006,
-            "angularVelocity": 1.4861478341138462e-11,
-            "velocityX": 2.1855837511923784,
-            "velocityY": -3.0876927152018316,
-            "timestamp": 1.9167096985801333
-        },
-        {
-            "x": 7.213982568749707,
-            "y": 3.1352979487738755,
-            "heading": 3.140000221251036,
-            "angularVelocity": -0.000002877850999701821,
-            "velocityX": 2.1317061520275473,
-            "velocityY": -3.104367928885527,
-            "timestamp": 1.9312786479018544
-        },
-        {
-            "x": 7.244041792187231,
-            "y": 3.091532444769153,
-            "heading": 3.1400001920667906,
-            "angularVelocity": -0.000002003181212693182,
-            "velocityX": 2.063238931905195,
-            "velocityY": -3.0040260994983523,
-            "timestamp": 1.9458475972235756
-        },
-        {
-            "x": 7.273101015062598,
-            "y": 3.049227126183516,
-            "heading": 3.140000168751395,
-            "angularVelocity": -0.0000016003484573673396,
-            "velocityX": 1.9945997637621933,
-            "velocityY": -2.9038002433409447,
-            "timestamp": 1.9604165465452967
-        },
-        {
-            "x": 7.301159421127851,
-            "y": 3.008381441393539,
-            "heading": 3.1400001491208855,
-            "angularVelocity": -0.0000013474210949527603,
-            "velocityX": 1.92590456907015,
-            "velocityY": -2.8036122501352616,
-            "timestamp": 1.9749854958670179
-        },
-        {
-            "x": 7.328216598566981,
-            "y": 2.9689951117692965,
-            "heading": 3.1400001321528324,
-            "angularVelocity": -0.0000011646724122855417,
-            "velocityX": 1.8571811076855331,
-            "velocityY": -2.703443381845065,
-            "timestamp": 1.989554445188739
-        },
-        {
-            "x": 7.35427229894417,
-            "y": 2.9310679691270813,
-            "heading": 3.140000117255664,
-            "angularVelocity": -0.0000010225286768555236,
-            "velocityX": 1.7884405938827321,
-            "velocityY": -2.603286057538151,
-            "timestamp": 2.00412339451046
-        },
-        {
-            "x": 7.379326356096679,
-            "y": 2.8945999009401007,
-            "heading": 3.1400001040435335,
-            "angularVelocity": -9.068691499457651e-7,
-            "velocityX": 1.7196886748142857,
-            "velocityY": -2.50313645697232,
-            "timestamp": 2.0186923438321815
-        },
-        {
-            "x": 7.403378651082293,
-            "y": 2.8595908266403365,
-            "heading": 3.1400000922448146,
-            "angularVelocity": -8.098538018281321e-7,
-            "velocityX": 1.650928591655569,
-            "velocityY": -2.4029923865251464,
-            "timestamp": 2.033261293153903
-        },
-        {
-            "x": 7.426429094558334,
-            "y": 2.826040685699055,
-            "heading": 3.1400000816578917,
-            "angularVelocity": -7.266771822683861e-7,
-            "velocityX": 1.5821623760933374,
-            "velocityY": -2.3028524707174887,
-            "timestamp": 2.0478302424756243
-        },
-        {
-            "x": 7.448477616956692,
-            "y": 2.793949430978311,
-            "heading": 3.1400000721271697,
-            "angularVelocity": -6.541804651955507e-7,
-            "velocityX": 1.513391385436516,
-            "velocityY": -2.202715790417496,
-            "timestamp": 2.0623991917973457
-        },
-        {
-            "x": 7.4695241625734585,
-            "y": 2.763317024730304,
-            "heading": 3.1400000635289262,
-            "angularVelocity": -5.901759591769764e-7,
-            "velocityX": 1.4446165713119874,
-            "velocityY": -2.102581701093514,
-            "timestamp": 2.076968141119067
-        },
-        {
-            "x": 7.489568685800359,
-            "y": 2.734143436045934,
-            "heading": 3.1400000557624246,
-            "angularVelocity": -5.330859058092693e-7,
-            "velocityX": 1.3758386266751614,
-            "velocityY": -2.0024497333432913,
-            "timestamp": 2.0915370904407884
-        },
-        {
-            "x": 7.508611148608458,
-            "y": 2.7064286391508827,
-            "heading": 3.140000048744052,
-            "angularVelocity": -4.817349694037634e-7,
-            "velocityX": 1.3070580717652986,
-            "velocityY": -1.902319534719876,
-            "timestamp": 2.1061060397625098
-        },
-        {
-            "x": 7.526651518803911,
-            "y": 2.680172612224319,
-            "heading": 3.1400000424032948,
-            "angularVelocity": -4.3522407744534453e-7,
-            "velocityX": 1.2382753070979928,
-            "velocityY": -1.8021908338592867,
-            "timestamp": 2.120674989084231
-        },
-        {
-            "x": 7.543689768780771,
-            "y": 2.6553753365541346,
-            "heading": 3.1400000366798815,
-            "angularVelocity": -3.9285011556875146e-7,
-            "velocityX": 1.1694906475824893,
-            "velocityY": -1.7020634173815092,
-            "timestamp": 2.1352439384059525
-        },
-        {
-            "x": 7.559725874605572,
-            "y": 2.6320367959168407,
-            "heading": 3.1400000315217036,
-            "angularVelocity": -3.5405282918878005e-7,
-            "velocityX": 1.100704345294715,
-            "velocityY": -1.6019371144699315,
-            "timestamp": 2.149812887727674
-        },
-        {
-            "x": 7.574759815330251,
-            "y": 2.6101569761120964,
-            "heading": 3.140000026883262,
-            "angularVelocity": -3.1837857149022665e-7,
-            "velocityX": 1.0319166051501305,
-            "velocityY": -1.5018117862573288,
-            "timestamp": 2.1643818370493952
-        },
-        {
-            "x": 7.588791572466392,
-            "y": 2.5897358646065065,
-            "heading": 3.1400000227244846,
-            "angularVelocity": -2.854548657017254e-7,
-            "velocityX": 0.9631275959768569,
-            "velocityY": -1.4016873183261147,
-            "timestamp": 2.1789507863711166
-        },
-        {
-            "x": 7.601821129576078,
-            "y": 2.570773450256394,
-            "heading": 3.140000019009809,
-            "angularVelocity": -2.5497212819286766e-7,
-            "velocityX": 0.8943374585191634,
-            "velocityY": -1.3015636152871453,
-            "timestamp": 2.193519735692838
-        },
-        {
-            "x": 7.61384847194871,
-            "y": 2.5532697230888193,
-            "heading": 3.1400000157074612,
-            "angularVelocity": -2.2667026217565558e-7,
-            "velocityX": 0.8255463113387961,
-            "velocityY": -1.201440596782113,
-            "timestamp": 2.2080886850145593
-        },
-        {
-            "x": 7.624873586342355,
-            "y": 2.5372246741263043,
-            "heading": 3.1400000127888834,
-            "angularVelocity": -2.0032864319885596e-7,
-            "velocityX": 0.7567542552436427,
-            "velocityY": -1.1013181944833246,
-            "timestamp": 2.2226576343362807
-        },
-        {
-            "x": 7.634896460774273,
-            "y": 2.5226382952448834,
-            "heading": 3.140000010228267,
-            "angularVelocity": -1.757585156167035e-7,
-            "velocityX": 0.6879613766637577,
-            "velocityY": -1.0011963498064516,
-            "timestamp": 2.237226583658002
-        },
-        {
-            "x": 7.643917084349478,
-            "y": 2.509510579057917,
-            "heading": 3.140000008002173,
-            "angularVelocity": -1.527971366621834e-7,
-            "velocityX": 0.6191677502613596,
-            "velocityY": -0.9010750121421621,
-            "timestamp": 2.2517955329797235
-        },
-        {
-            "x": 7.6519354471190555,
-            "y": 2.4978415188200733,
-            "heading": 3.140000006089223,
-            "angularVelocity": -1.3130320584948841e-7,
-            "velocityX": 0.5503734409743167,
-            "velocityY": -0.8009541374715159,
-            "timestamp": 2.266364482301445
-        },
-        {
-            "x": 7.658951539962077,
-            "y": 2.487631108347293,
-            "heading": 3.1400000044698366,
-            "angularVelocity": -1.1115325658440658e-7,
-            "velocityX": 0.4815785056345368,
-            "velocityY": -0.7008336872693404,
-            "timestamp": 2.280933431623166
-        },
-        {
-            "x": 7.664965354486367,
-            "y": 2.478879341949535,
-            "heading": 3.1400000031260147,
-            "angularVelocity": -9.223877588817121e-8,
-            "velocityX": 0.4127829942631362,
-            "velocityY": -0.600713627626585,
-            "timestamp": 2.2955023809448876
-        },
-        {
-            "x": 7.669976882944524,
-            "y": 2.4715862143738576,
-            "heading": 3.140000002041154,
-            "angularVelocity": -7.44638832915239e-8,
-            "velocityX": 0.34398695111687533,
-            "velocityY": -0.5005939285411971,
-            "timestamp": 2.310071330266609
-        },
-        {
-            "x": 7.673986118162365,
-            "y": 2.4657517207559176,
-            "heading": 3.1400000011998928,
-            "angularVelocity": -5.774344306809202e-8,
-            "velocityX": 0.27519041554106066,
-            "velocityY": -0.4004745633401276,
-            "timestamp": 2.3246402795883303
-        },
-        {
-            "x": 7.676993053477586,
-            "y": 2.4613758565783885,
-            "heading": 3.1400000005879747,
-            "angularVelocity": -4.2001516695103525e-8,
-            "velocityX": 0.20639342267033312,
-            "velocityY": -0.30035550820440443,
-            "timestamp": 2.3392092289100517
-        },
-        {
-            "x": 7.6789976826868624,
-            "y": 2.458458617635108,
-            "heading": 3.1400000001921353,
-            "angularVelocity": -2.7170084001525683e-8,
-            "velocityX": 0.13759600400877475,
-            "velocityY": -0.2002367417759812,
-            "timestamp": 2.353778178231773
+            "heading": 1.5377683531646926e-9,
+            "angularVelocity": 1.4832457732424108e-15,
+            "velocityX": 2.8653643967583453,
+            "velocityY": -2.4698816386502185,
+            "timestamp": 1.8001580964547599
+        },
+        {
+            "x": 6.934219508494461,
+            "y": 3.4745268434152994,
+            "heading": 1.5377683787984002e-9,
+            "angularVelocity": 1.7594754613333528e-15,
+            "velocityX": 2.784717658151537,
+            "velocityY": -2.56046267850036,
+            "timestamp": 1.8147270446274342
+        },
+        {
+            "x": 6.973574535692003,
+            "y": 3.4359435289599864,
+            "heading": 1.5377684019331704e-9,
+            "angularVelocity": 1.5879505990145664e-15,
+            "velocityX": 2.701295023573416,
+            "velocityY": -2.648325328500865,
+            "timestamp": 1.8292959928001085
+        },
+        {
+            "x": 7.011676190859291,
+            "y": 3.3961219990562412,
+            "heading": 1.5377684253031837e-9,
+            "angularVelocity": 1.6040974952296623e-15,
+            "velocityX": 2.6152646516208327,
+            "velocityY": -2.733315365788167,
+            "timestamp": 1.8438649409727828
+        },
+        {
+            "x": 7.048490476455465,
+            "y": 3.3551073823105697,
+            "heading": 1.5377684383832216e-9,
+            "angularVelocity": 8.97802495662944e-16,
+            "velocityX": 2.5269007178724983,
+            "velocityY": -2.815207814562461,
+            "timestamp": 1.858433889145457
+        },
+        {
+            "x": 7.083991390306391,
+            "y": 3.3129508222340074,
+            "heading": 1.5377684557448877e-9,
+            "angularVelocity": 1.191689740876308e-15,
+            "velocityX": 2.4367520173841823,
+            "velocityY": -2.893589816979968,
+            "timestamp": 1.8730028373181313
+        },
+        {
+            "x": 7.118172550776329,
+            "y": 3.269717285105602,
+            "heading": 1.5377684739509254e-9,
+            "angularVelocity": 1.2496467044902449e-15,
+            "velocityX": 2.346165286939037,
+            "velocityY": -2.9675125902013777,
+            "timestamp": 1.8875717854908056
+        },
+        {
+            "x": 7.15108313208807,
+            "y": 3.2255088683196913,
+            "heading": 1.5377684882032233e-9,
+            "angularVelocity": 9.782653941580851e-16,
+            "velocityX": 2.2589538326090746,
+            "velocityY": -3.0344274865927217,
+            "timestamp": 1.90214073366348
+        },
+        {
+            "x": 7.182924606349684,
+            "y": 3.1805243042550315,
+            "heading": 1.5377697666399164e-9,
+            "angularVelocity": 8.775078931151136e-14,
+            "velocityX": 2.185571249497722,
+            "velocityY": -3.0877015644156707,
+            "timestamp": 1.9167096818361542
+        },
+        {
+            "x": 7.2139811373084415,
+            "y": 3.1352968072431113,
+            "heading": 1.2915519070501347e-9,
+            "angularVelocity": -1.6900181039231953e-8,
+            "velocityX": 2.131693420188762,
+            "velocityY": -3.1043762717714163,
+            "timestamp": 1.9312786300088285
+        },
+        {
+            "x": 7.244040263059187,
+            "y": 3.0915312465009275,
+            "heading": 1.1211375399666565e-9,
+            "angularVelocity": -1.169709485282711e-8,
+            "velocityX": 2.063232389495776,
+            "velocityY": -3.0040302308350935,
+            "timestamp": 1.9458475781815028
+        },
+        {
+            "x": 7.2730994599044765,
+            "y": 3.0492259199621516,
+            "heading": 9.847245150980228e-10,
+            "angularVelocity": -9.363272118309568e-9,
+            "velocityX": 1.9945981343934114,
+            "velocityY": -2.903801018259091,
+            "timestamp": 1.960416526354177
+        },
+        {
+            "x": 7.301157878524098,
+            "y": 3.0083802533354898,
+            "heading": 8.698146081553592e-10,
+            "angularVelocity": -7.887316693614981e-9,
+            "velocityX": 1.9259055826862927,
+            "velocityY": -2.803611224540764,
+            "timestamp": 1.9749854745268514
+        },
+        {
+            "x": 7.328215092540324,
+            "y": 2.968993958016925,
+            "heading": 7.704962597777701e-10,
+            "angularVelocity": -6.8171255926008325e-9,
+            "velocityX": 1.857183764780944,
+            "velocityY": -2.7034412403524777,
+            "timestamp": 1.9895544226995256
+        },
+        {
+            "x": 7.354270845617658,
+            "y": 2.9310668604072108,
+            "heading": 6.833248169018587e-10,
+            "angularVelocity": -5.983372499761897e-9,
+            "velocityX": 1.7884443522287568,
+            "velocityY": -2.6032831718663054,
+            "timestamp": 2.0041233708722
+        },
+        {
+            "x": 7.379324966656107,
+            "y": 2.894598844592948,
+            "heading": 6.060446454340774e-10,
+            "angularVelocity": -5.304444180973637e-9,
+            "velocityX": 1.719693195521089,
+            "velocityY": -2.5031330595756027,
+            "timestamp": 2.0186923190448742
+        },
+        {
+            "x": 7.403377333338156,
+            "y": 2.8595898276899736,
+            "heading": 5.370639718880826e-10,
+            "angularVelocity": -4.734773818100124e-9,
+            "velocityX": 1.6509336430450512,
+            "velocityY": -2.4029886363820543,
+            "timestamp": 2.0332612672175485
+        },
+        {
+            "x": 7.426427853868939,
+            "y": 2.8260397474863512,
+            "heading": 4.75199340983144e-10,
+            "angularVelocity": -4.246334802521844e-9,
+            "velocityX": 1.582167789848791,
+            "velocityY": -2.3028484833621037,
+            "timestamp": 2.047830215390223
+        },
+        {
+            "x": 7.44847645681879,
+            "y": 2.79394855556605,
+            "heading": 4.195363288341646e-10,
+            "angularVelocity": -3.820661051835768e-9,
+            "velocityX": 1.5133970337819913,
+            "velocityY": -2.202711653576578,
+            "timestamp": 2.062399163562897
+        },
+        {
+            "x": 7.469523085022814,
+            "y": 2.7633162131780225,
+            "heading": 3.6934715040819367e-10,
+            "angularVelocity": -3.4449418365911004e-9,
+            "velocityX": 1.4446223539663485,
+            "velocityY": -2.1025774836294286,
+            "timestamp": 2.0769681117355714
+        },
+        {
+            "x": 7.489567691695811,
+            "y": 2.7341426886048597,
+            "heading": 3.2403879933557077e-10,
+            "angularVelocity": -3.1099260658641154e-9,
+            "velocityX": 1.3758444628549362,
+            "velocityY": -2.002445490737794,
+            "timestamp": 2.0915370599082457
+        },
+        {
+            "x": 7.5086102378406,
+            "y": 2.7064279554071735,
+            "heading": 2.831187733993615e-10,
+            "angularVelocity": -2.8087152215039624e-9,
+            "velocityX": 1.3070638950107798,
+            "velocityY": -1.9023153126229018,
+            "timestamp": 2.10610600808092
+        },
+        {
+            "x": 7.52665069045286,
+            "y": 2.680171991207375,
+            "heading": 2.4617151569169827e-10,
+            "angularVelocity": -2.536027844020545e-9,
+            "velocityX": 1.2382810617787536,
+            "velocityY": -1.8021866704863527,
+            "timestamp": 2.1206749562535943
+        },
+        {
+            "x": 7.543689021238326,
+            "y": 2.65537477682047,
+            "heading": 2.128416865029733e-10,
+            "angularVelocity": -2.287730676987115e-9,
+            "velocityX": 1.169496286452695,
+            "velocityY": -1.7020593451912265,
+            "timestamp": 2.1352439044262685
+        },
+        {
+            "x": 7.559725205671708,
+            "y": 2.6320362956163437,
+            "heading": 1.8282195651887185e-10,
+            "angularVelocity": -2.0605283283789865e-9,
+            "velocityX": 1.1007098277319451,
+            "velocityY": -1.6019331613726957,
+            "timestamp": 2.149812852598943
+        },
+        {
+            "x": 7.5747592222906635,
+            "y": 2.6101565330412897,
+            "heading": 1.558438913168689e-10,
+            "angularVelocity": -1.8517510864510685e-9,
+            "velocityX": 1.0319218958546759,
+            "velocityY": -1.5018079765081174,
+            "timestamp": 2.164381800771617
+        },
+        {
+            "x": 7.588791052155751,
+            "y": 2.5897354762519833,
+            "heading": 1.3167100643601026e-10,
+            "angularVelocity": -1.6592059334641093e-9,
+            "velocityX": 0.9631326639903722,
+            "velocityY": -1.4016836731981617,
+            "timestamp": 2.1789507489442914
+        },
+        {
+            "x": 7.601820678430282,
+            "y": 2.570773113830717,
+            "heading": 1.100933830131512e-10,
+            "angularVelocity": -1.4810694297523211e-9,
+            "velocityX": 0.894342276470416,
+            "velocityY": -1.3015601535897774,
+            "timestamp": 2.1935196971169657
+        },
+        {
+            "x": 7.613848086048565,
+            "y": 2.5532694355605217,
+            "heading": 9.092342750630814e-11,
+            "angularVelocity": -1.315809184573053e-9,
+            "velocityX": 0.8255508548544661,
+            "velocityY": -1.201437335265309,
+            "timestamp": 2.20808864528964
+        },
+        {
+            "x": 7.624873261450447,
+            "y": 2.537224432245223,
+            "heading": 7.399249133993465e-11,
+            "angularVelocity": -1.1621248318093622e-9,
+            "velocityX": 0.7567585024813989,
+            "velocityY": -1.101315148158345,
+            "timestamp": 2.2226575934623143
+        },
+        {
+            "x": 7.634896192366388,
+            "y": 2.5226380955637384,
+            "heading": 5.914813867727886e-11,
+            "angularVelocity": -1.0189035430874983e-9,
+            "velocityX": 0.6879653079374735,
+            "velocityY": -1.0011935322032168,
+            "timestamp": 2.2372265416349886
+        },
+        {
+            "x": 7.643916867641572,
+            "y": 2.5095104179508487,
+            "heading": 4.6251914688462445e-11,
+            "angularVelocity": -8.851856741832805e-10,
+            "velocityX": 0.6191713477368228,
+            "velocityY": -0.9010724355182365,
+            "timestamp": 2.251795489807663
+        },
+        {
+            "x": 7.651935277090606,
+            "y": 2.4978413924986826,
+            "heading": 3.517750680608747e-11,
+            "angularVelocity": -7.601377906979457e-10,
+            "velocityX": 0.5503766884196106,
+            "velocityY": -0.8009518129835539,
+            "timestamp": 2.266364437980337
+        },
+        {
+            "x": 7.658951411376412,
+            "y": 2.487631012874611,
+            "heading": 2.580921448386634e-11,
+            "angularVelocity": -6.43031492677376e-10,
+            "velocityX": 0.48158138821334623,
+            "velocityY": -0.7008316251150604,
+            "timestamp": 2.2809333861530114
+        },
+        {
+            "x": 7.664965261908482,
+            "y": 2.4788792732522666,
+            "heading": 1.804066569533592e-11,
+            "angularVelocity": -5.332264785860361e-10,
+            "velocityX": 0.41278549836213546,
+            "velocityY": -0.6007118371633381,
+            "timestamp": 2.2955023343256857
+        },
+        {
+            "x": 7.669976820756786,
+            "y": 2.4715861682531717,
+            "heading": 1.1773732592627752e-11,
+            "angularVelocity": -4.3015687513290903e-10,
+            "velocityX": 0.34398906420045333,
+            "velocityY": -0.500592418385764,
+            "timestamp": 2.31007128249836
+        },
+        {
+            "x": 7.67398608057842,
+            "y": 2.465751692897001,
+            "heading": 6.917607828867898e-12,
+            "angularVelocity": -3.3332020945886676e-10,
+            "velocityX": 0.27519212602817883,
+            "velocityY": -0.40047334145331454,
+            "timestamp": 2.3246402306710343
+        },
+        {
+            "x": 7.676993034554729,
+            "y": 2.461375842558954,
+            "heading": 3.3880167119093778e-12,
+            "angularVelocity": -2.4226808579143967e-10,
+            "velocityX": 0.20639471982945568,
+            "velocityY": -0.30035458196322756,
+            "timestamp": 2.3392091788437086
+        },
+        {
+            "x": 7.678997676337105,
+            "y": 2.4584586129329957,
+            "heading": 1.1065347655527578e-12,
+            "angularVelocity": -1.56598950365953e-10,
+            "velocityX": 0.13759687786767488,
+            "velocityY": -0.2002361180356331,
+            "timestamp": 2.353778127016383
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -1.3187979232574289e-8,
-            "velocityX": 0.06879818791344262,
-            "velocityY": -0.10011824483002574,
-            "timestamp": 2.3683471275534944
+            "heading": -3.441813048880529e-34,
+            "angularVelocity": -7.595159066564751e-11,
+            "velocityX": 0.06879862918134139,
+            "velocityY": -0.10011792997737116,
+            "timestamp": 2.368347075189057
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 2.7611774763020037e-32,
-            "velocityX": -1.1210620234419235e-31,
-            "velocityY": 4.358756882663372e-31,
-            "timestamp": 2.382916076875216
+            "heading": -3.481182114535467e-34,
+            "angularVelocity": -2.7023024138211624e-34,
+            "velocityX": -3.7878173595367446e-32,
+            "velocityY": -1.5253102288533056e-31,
+            "timestamp": 2.3829160233617315
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.1.traj
index 24664a13..39d8ef61 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.1.traj	
@@ -3,622 +3,622 @@
         {
             "x": 1.35,
             "y": 5.56,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": 3.295386080925321e-33,
+            "velocityY": -4.1566893564125267e-32,
             "timestamp": 0
         },
         {
-            "x": 1.3546830312136446,
+            "x": 1.3546830312136449,
             "y": 5.550921973415328,
-            "heading": 3.14,
-            "angularVelocity": -6.105321890596023e-17,
-            "velocityX": 0.13379730014070298,
-            "velocityY": -0.2593652256885782,
-            "timestamp": 0.03500093954603884
-        },
-        {
-            "x": 1.364049093509165,
-            "y": 5.532765920501418,
-            "heading": 3.14,
-            "angularVelocity": -9.872410216704159e-17,
-            "velocityX": 0.2675945965166781,
+            "heading": -1.4050458772244498e-18,
+            "angularVelocity": -4.014308801297789e-17,
+            "velocityX": 0.13379730014070312,
+            "velocityY": -0.25936522568857817,
+            "timestamp": 0.03500093954603885
+        },
+        {
+            "x": 1.3640490935091651,
+            "y": 5.532765920501419,
+            "heading": -3.603876179363921e-18,
+            "angularVelocity": -6.28220327965604e-17,
+            "velocityX": 0.26759459651667833,
             "velocityY": -0.5187304440792354,
-            "timestamp": 0.07000187909207768
+            "timestamp": 0.0700018790920777
         },
         {
-            "x": 1.3780981867284383,
+            "x": 1.3780981867284385,
             "y": 5.505531841564791,
-            "heading": 3.14,
-            "angularVelocity": -1.396517144912572e-16,
-            "velocityX": 0.40139188837498685,
+            "heading": -4.091508575199503e-18,
+            "angularVelocity": -1.3931950355515604e-17,
+            "velocityX": 0.4013918883749871,
             "velocityY": -0.7780956537124009,
-            "timestamp": 0.10500281863811653
+            "timestamp": 0.10500281863811654
         },
         {
-            "x": 1.3968303106782043,
-            "y": 5.46921973698008,
-            "heading": 3.14,
-            "angularVelocity": -1.3147784440878345e-16,
-            "velocityX": 0.5351891747117138,
+            "x": 1.3968303106782045,
+            "y": 5.469219736980081,
+            "heading": -4.985304542720085e-18,
+            "angularVelocity": -2.5536326104216816e-17,
+            "velocityX": 0.5351891747117141,
             "velocityY": -1.0374608526419855,
-            "timestamp": 0.14000375818415536
+            "timestamp": 0.1400037581841554
         },
         {
-            "x": 1.4202454651168874,
-            "y": 5.423829607215582,
-            "heading": 3.14,
-            "angularVelocity": -7.833997421538468e-18,
-            "velocityX": 0.6689864541464796,
+            "x": 1.4202454651168877,
+            "y": 5.423829607215581,
+            "heading": -4.3424315250592746e-18,
+            "angularVelocity": 1.8367331153854942e-17,
+            "velocityX": 0.6689864541464801,
             "velocityY": -1.2968260381921248,
-            "timestamp": 0.1750046977301942
+            "timestamp": 0.17500469773019423
         },
         {
-            "x": 1.4483436497338924,
+            "x": 1.4483436497338926,
             "y": 5.369361452873384,
-            "heading": 3.14,
-            "angularVelocity": -2.0049167233544137e-17,
-            "velocityX": 0.802783724707322,
+            "heading": -2.0560193041515625e-18,
+            "angularVelocity": 6.532433512064817e-17,
+            "velocityX": 0.8027837247073224,
             "velocityY": -1.5561912065401715,
-            "timestamp": 0.21000563727623303
+            "timestamp": 0.21000563727623306
         },
         {
-            "x": 1.4811248641150931,
+            "x": 1.4811248641150936,
             "y": 5.305815274756268,
-            "heading": 3.14,
-            "angularVelocity": -8.129804229935506e-17,
-            "velocityX": 0.9365809834363131,
-            "velocityY": -1.8155563519521838,
-            "timestamp": 0.24500657682227187
+            "heading": 2.1208651292681738e-19,
+            "angularVelocity": 6.480130833433249e-17,
+            "velocityX": 0.9365809834363137,
+            "velocityY": -1.815556351952184,
+            "timestamp": 0.2450065768222719
         },
         {
-            "x": 1.5185891076807168,
+            "x": 1.518589107680717,
             "y": 5.233191073988126,
-            "heading": 3.14,
-            "angularVelocity": -1.3088931274135507e-16,
-            "velocityX": 1.0703782256008025,
-            "velocityY": -2.0749214652539205,
+            "heading": 3.0112738874394488e-18,
+            "angularVelocity": 7.99746695569e-17,
+            "velocityX": 1.0703782256008032,
+            "velocityY": -2.074921465253921,
             "timestamp": 0.2800075163683107
         },
         {
-            "x": 1.56073637956111,
-            "y": 5.151488852254778,
-            "heading": 3.14,
-            "angularVelocity": -1.390272479709275e-16,
-            "velocityX": 1.204175442918738,
+            "x": 1.5607363795611104,
+            "y": 5.151488852254779,
+            "heading": 5.4707987602321704e-18,
+            "angularVelocity": 7.027028770373186e-17,
+            "velocityX": 1.2041754429187383,
             "velocityY": -2.334286530390624,
             "timestamp": 0.31500845591434956
         },
         {
-            "x": 1.6075666783068707,
+            "x": 1.6075666783068712,
             "y": 5.060708612365892,
-            "heading": 3.14,
-            "angularVelocity": -2.002620206188659e-16,
-            "velocityX": 1.3379726188262815,
-            "velocityY": -2.593651515253289,
+            "heading": 7.441304754324509e-18,
+            "angularVelocity": 5.629869559433992e-17,
+            "velocityX": 1.3379726188262824,
+            "velocityY": -2.5936515152532893,
             "timestamp": 0.3500093954603884
         },
         {
-            "x": 1.6590800010192643,
+            "x": 1.6590800010192646,
             "y": 4.9608503599406655,
-            "heading": 3.14,
-            "angularVelocity": -2.3956924314672257e-16,
-            "velocityX": 1.471769711915058,
-            "velocityY": -2.8530163395717247,
+            "heading": 8.015191331788108e-18,
+            "angularVelocity": 1.639633984973558e-17,
+            "velocityX": 1.4717697119150586,
+            "velocityY": -2.853016339571725,
             "timestamp": 0.38501033500642723
         },
         {
-            "x": 1.7152763390026582,
-            "y": 4.851914111835608,
-            "heading": 3.14,
-            "angularVelocity": -2.6938408534488506e-16,
-            "velocityX": 1.6055665565638675,
-            "velocityY": -3.112380682288517,
+            "x": 1.7152763390026586,
+            "y": 4.851914111835609,
+            "heading": 8.871725964928404e-18,
+            "angularVelocity": 2.4471795525975083e-17,
+            "velocityX": 1.6055665565638684,
+            "velocityY": -3.112380682288518,
             "timestamp": 0.42001127455246606
         },
         {
-            "x": 1.7759790338056225,
-            "y": 4.734242319347403,
-            "heading": 3.14,
-            "angularVelocity": 3.279195870571412e-16,
-            "velocityX": 1.734316152375285,
-            "velocityY": -3.3619609648879023,
+            "x": 1.775979033805623,
+            "y": 4.734242319347404,
+            "heading": 1.526334137039101e-17,
+            "angularVelocity": 1.8261268092684144e-16,
+            "velocityX": 1.7343161523752855,
+            "velocityY": -3.361960964887902,
             "timestamp": 0.4550122140985049
         },
         {
-            "x": 1.8366817286107124,
-            "y": 4.616570526856246,
-            "heading": 3.14,
-            "angularVelocity": -3.1071543812317194e-16,
-            "velocityX": 1.7343161524360031,
-            "velocityY": -3.36196096497222,
+            "x": 1.8366817286107127,
+            "y": 4.616570526856247,
+            "heading": 2.6738432547349554e-17,
+            "angularVelocity": 3.2785094702569765e-16,
+            "velocityX": 1.734316152436003,
+            "velocityY": -3.3619609649722197,
             "timestamp": 0.49001315364454373
         },
         {
-            "x": 1.8973844234157842,
+            "x": 1.8973844234157844,
             "y": 4.498898734365081,
-            "heading": 3.14,
-            "angularVelocity": -2.903673566166758e-16,
-            "velocityX": 1.7343161524354895,
-            "velocityY": -3.361960964972484,
+            "heading": 1.9852147320466475e-17,
+            "angularVelocity": -1.967457250062152e-16,
+            "velocityX": 1.7343161524354893,
+            "velocityY": -3.3619609649724835,
             "timestamp": 0.5250140931905826
         },
         {
-            "x": 1.958087118220856,
-            "y": 4.381226941873915,
-            "heading": 3.14,
-            "angularVelocity": 3.614460545323391e-16,
+            "x": 1.9580871182208561,
+            "y": 4.381226941873916,
+            "heading": 2.777024505719557e-17,
+            "angularVelocity": 2.2622529107579697e-16,
             "velocityX": 1.7343161524354895,
-            "velocityY": -3.3619609649724835,
+            "velocityY": -3.361960964972483,
             "timestamp": 0.5600150327366215
         },
         {
-            "x": 2.0187898130259274,
+            "x": 2.018789813025928,
             "y": 4.26355514938275,
-            "heading": 3.14,
-            "angularVelocity": -3.3191632817458094e-16,
+            "heading": 1.7414652311267274e-17,
+            "angularVelocity": -2.9586613617347484e-16,
             "velocityX": 1.7343161524354893,
             "velocityY": -3.361960964972483,
             "timestamp": 0.5950159722826603
         },
         {
-            "x": 2.079492507830999,
-            "y": 4.145883356891584,
-            "heading": 3.14,
-            "angularVelocity": 3.2426066858348807e-16,
-            "velocityX": 1.7343161524354893,
+            "x": 2.0794925078309996,
+            "y": 4.145883356891583,
+            "heading": 6.771469247853252e-18,
+            "angularVelocity": -3.040827818183264e-16,
+            "velocityX": 1.7343161524354895,
             "velocityY": -3.361960964972483,
             "timestamp": 0.6300169118286992
         },
         {
             "x": 2.140195202636071,
-            "y": 4.028211564400418,
-            "heading": 3.14,
-            "angularVelocity": -2.798848785469217e-16,
-            "velocityX": 1.734316152435488,
-            "velocityY": -3.3619609649724826,
+            "y": 4.028211564400419,
+            "heading": 1.7156471914154247e-17,
+            "angularVelocity": 2.9670636674740876e-16,
+            "velocityX": 1.7343161524354882,
+            "velocityY": -3.361960964972482,
             "timestamp": 0.6650178513747381
         },
         {
-            "x": 2.200897897441142,
-            "y": 3.9105397719092525,
-            "heading": 3.14,
-            "angularVelocity": -1.795170282800533e-16,
+            "x": 2.2008978974411426,
+            "y": 3.910539771909253,
+            "heading": 9.427453062324153e-18,
+            "angularVelocity": -2.208231822366884e-16,
             "velocityX": 1.734316152435471,
             "velocityY": -3.361960964972468,
             "timestamp": 0.700018790920777
         },
         {
-            "x": 2.2616005922462064,
-            "y": 3.792867979418094,
-            "heading": 3.14,
-            "angularVelocity": -3.385716636422731e-16,
-            "velocityX": 1.7343161524352515,
-            "velocityY": -3.3619609649722846,
+            "x": 2.261600592246207,
+            "y": 3.7928679794180944,
+            "heading": -2.0403441270686145e-18,
+            "angularVelocity": -3.2764255297515643e-16,
+            "velocityX": 1.7343161524352513,
+            "velocityY": -3.3619609649722837,
             "timestamp": 0.7350197304668159
         },
         {
-            "x": 2.3223032870512124,
+            "x": 2.322303287051213,
             "y": 3.6751961869269927,
-            "heading": 3.14,
-            "angularVelocity": -3.759822562526031e-16,
-            "velocityX": 1.7343161524334345,
-            "velocityY": -3.361960964970739,
+            "heading": 3.357966916114392e-18,
+            "angularVelocity": 1.5423331811082687e-16,
+            "velocityX": 1.7343161524334347,
+            "velocityY": -3.3619609649707387,
             "timestamp": 0.7700206700128548
         },
         {
-            "x": 2.383005981879538,
-            "y": 3.5575243944476096,
-            "heading": 3.14,
-            "angularVelocity": -2.6556763706975105e-16,
+            "x": 2.383005981879539,
+            "y": 3.55752439444761,
+            "heading": 1.6083387472754582e-17,
+            "angularVelocity": 3.6357368464068287e-16,
             "velocityX": 1.7343161531001825,
-            "velocityY": -3.3619609646356636,
+            "velocityY": -3.361960964635663,
             "timestamp": 0.8050216095588937
         },
         {
-            "x": 2.443708981028518,
-            "y": 3.4398527589551926,
-            "heading": 3.14,
-            "angularVelocity": 5.092773468950765e-17,
+            "x": 2.4437089810285184,
+            "y": 3.439852758955193,
+            "heading": 2.2946423291224054e-17,
+            "angularVelocity": 1.9608147402560587e-16,
             "velocityX": 1.7343248477441577,
             "velocityY": -3.3619564794134043,
             "timestamp": 0.8400225491049326
         },
         {
             "x": 2.507799181067132,
-            "y": 3.3239911422386283,
-            "heading": 3.14,
-            "angularVelocity": -2.1081778211927598e-16,
+            "y": 3.323991142238629,
+            "heading": 9.669545465348722e-18,
+            "angularVelocity": -3.793291836756151e-16,
             "velocityX": 1.8310994181462568,
-            "velocityY": -3.310243045471397,
+            "velocityY": -3.3102430454713967,
             "timestamp": 0.8750234886509715
         },
         {
             "x": 2.5806292795037855,
-            "y": 3.2134142705817266,
-            "heading": 3.14,
-            "angularVelocity": -3.875593095285532e-16,
+            "y": 3.213414270581727,
+            "heading": 3.38196516760506e-17,
+            "angularVelocity": 6.899845125281262e-16,
             "velocityX": 2.0808040978720626,
             "velocityY": -3.159254382625257,
             "timestamp": 0.9100244281970103
         },
         {
             "x": 2.661766753531331,
-            "y": 3.108780729790167,
-            "heading": 3.14,
-            "angularVelocity": -7.347130647585027e-16,
-            "velocityX": 2.318151314794784,
+            "y": 3.1087807297901673,
+            "heading": 5.146705636243318e-17,
+            "angularVelocity": 5.041980276892245e-16,
+            "velocityX": 2.3181513147947848,
             "velocityY": -2.9894494876003863,
             "timestamp": 0.9450253677430492
         },
         {
             "x": 2.7507288455963135,
             "y": 3.0107133388519287,
-            "heading": 3.14,
-            "angularVelocity": -3.52994704864956e-16,
-            "velocityX": 2.541705829014059,
-            "velocityY": -2.801850242022933,
+            "heading": 6.422105835490463e-17,
+            "angularVelocity": 3.643902751722035e-16,
+            "velocityX": 2.5417058290140595,
+            "velocityY": -2.8018502420229328,
             "timestamp": 0.9800263072890881
         },
         {
             "x": 2.9215337593923705,
             "y": 2.8658352980219064,
-            "heading": 3.14,
-            "angularVelocity": 1.5913396610629706e-16,
-            "velocityX": 2.884920936848367,
-            "velocityY": -2.4470121145413266,
+            "heading": 5.1318206888106e-17,
+            "angularVelocity": -2.1793112103404205e-16,
+            "velocityX": 2.884920936848368,
+            "velocityY": -2.4470121145413257,
             "timestamp": 1.0392324060123015
         },
         {
             "x": 3.109750180151373,
             "y": 2.744432888397421,
-            "heading": 3.14,
-            "angularVelocity": -6.286818756935585e-16,
-            "velocityX": 3.179003934013425,
-            "velocityY": -2.0505051378666,
+            "heading": 6.648964720130561e-17,
+            "angularVelocity": 2.5624793121609224e-16,
+            "velocityX": 3.179003934013426,
+            "velocityY": -2.0505051378665997,
             "timestamp": 1.098438504735515
         },
         {
-            "x": 3.3097585780303125,
+            "x": 3.309758578030313,
             "y": 2.6436330516151556,
-            "heading": 3.14,
-            "angularVelocity": -3.4341251732407706e-17,
+            "heading": 6.809268296964168e-17,
+            "angularVelocity": 2.7075517621497836e-17,
             "velocityX": 3.3781722186064402,
-            "velocityY": -1.702524553319067,
+            "velocityY": -1.702524553319066,
             "timestamp": 1.1576446034587284
         },
         {
-            "x": 3.5097675494852,
-            "y": 2.5428343529427293,
-            "heading": 3.14,
-            "angularVelocity": -2.2516273301091217e-16,
-            "velocityX": 3.3781819063939547,
-            "velocityY": -1.7025053304654465,
+            "x": 3.5097675494851996,
+            "y": 2.542834352942729,
+            "heading": 6.982732716570642e-17,
+            "angularVelocity": 2.929840393934814e-17,
+            "velocityX": 3.378181906393955,
+            "velocityY": -1.702505330465446,
             "timestamp": 1.216850702181942
         },
         {
             "x": 3.709776520948374,
-            "y": 2.442035654287211,
-            "heading": 3.14,
-            "angularVelocity": -3.629334864774022e-16,
-            "velocityX": 3.378181906533671,
-            "velocityY": -1.7025053301803685,
+            "y": 2.4420356542872104,
+            "heading": 8.871000937094426e-17,
+            "angularVelocity": 3.189313704575333e-16,
+            "velocityX": 3.3781819065336705,
+            "velocityY": -1.702505330180368,
             "timestamp": 1.2760568009051554
         },
         {
-            "x": 3.909785492411555,
-            "y": 2.3412369556316897,
-            "heading": 3.14,
-            "angularVelocity": -2.39059813931354e-16,
+            "x": 3.9097854924115545,
+            "y": 2.341236955631689,
+            "heading": 7.04137580064196e-17,
+            "angularVelocity": -3.090264644873572e-16,
             "velocityX": 3.3781819065337895,
-            "velocityY": -1.7025053301803958,
+            "velocityY": -1.7025053301803954,
             "timestamp": 1.3352628996283689
         },
         {
             "x": 4.109794463874735,
             "y": 2.240438256976168,
-            "heading": 3.14,
-            "angularVelocity": 3.7738636092274995e-16,
-            "velocityX": 3.378181906533792,
-            "velocityY": -1.7025053301803967,
+            "heading": 6.635408667627181e-17,
+            "angularVelocity": -6.856846534504647e-17,
+            "velocityX": 3.3781819065337926,
+            "velocityY": -1.7025053301803963,
             "timestamp": 1.3944689983515823
         },
         {
             "x": 4.3098034353379155,
-            "y": 2.139639558320647,
-            "heading": 3.14,
-            "angularVelocity": 3.725508803000677e-16,
+            "y": 2.1396395583206465,
+            "heading": 8.756379552984881e-17,
+            "angularVelocity": 3.5823520399025277e-16,
             "velocityX": 3.3781819065337926,
-            "velocityY": -1.7025053301803965,
+            "velocityY": -1.7025053301803963,
             "timestamp": 1.4536750970747958
         },
         {
             "x": 4.509812406801097,
             "y": 2.0388408596651253,
-            "heading": 3.14,
-            "angularVelocity": -2.2963176234131617e-16,
+            "heading": 7.076319345079811e-17,
+            "angularVelocity": -2.837647208878057e-16,
             "velocityX": 3.3781819065337926,
-            "velocityY": -1.7025053301803967,
+            "velocityY": -1.7025053301803963,
             "timestamp": 1.5128811957980093
         },
         {
             "x": 4.709821378264278,
             "y": 1.9380421610096037,
-            "heading": 3.14,
-            "angularVelocity": -2.0546569648664528e-16,
-            "velocityX": 3.3781819065338015,
-            "velocityY": -1.702505330180399,
+            "heading": 6.652684249010322e-17,
+            "angularVelocity": -7.15526111686399e-17,
+            "velocityX": 3.378181906533802,
+            "velocityY": -1.7025053301803985,
             "timestamp": 1.5720872945212228
         },
         {
             "x": 4.9098303497278835,
-            "y": 1.8372434623549267,
-            "heading": 3.14,
-            "angularVelocity": 1.0642243284106308e-16,
-            "velocityX": 3.3781819065409677,
-            "velocityY": -1.7025053301661326,
+            "y": 1.837243462354927,
+            "heading": 7.855425123263822e-17,
+            "angularVelocity": 2.0314476045367986e-16,
+            "velocityX": 3.378181906540968,
+            "velocityY": -1.7025053301661324,
             "timestamp": 1.6312933932444362
         },
         {
             "x": 5.109839349236117,
-            "y": 1.736444819347842,
-            "heading": 3.14,
-            "angularVelocity": 1.2679224790319894e-16,
+            "y": 1.7364448193478421,
+            "heading": 7.31685607290898e-17,
+            "angularVelocity": -9.096513061477316e-17,
             "velocityX": 3.378182380218786,
-            "velocityY": -1.7025043902702677,
+            "velocityY": -1.7025043902702668,
             "timestamp": 1.6904994919676497
         },
         {
             "x": 5.311619758605957,
             "y": 1.6392407417297363,
-            "heading": 3.14,
-            "angularVelocity": 3.145701878302232e-16,
-            "velocityX": 3.408101761838499,
-            "velocityY": -1.6417916348765778,
+            "heading": 6.073866217998324e-17,
+            "angularVelocity": -2.0994287441931112e-16,
+            "velocityX": 3.4081017618384997,
+            "velocityY": -1.6417916348765775,
             "timestamp": 1.7497055906908632
         },
         {
             "x": 5.430192834880249,
             "y": 1.5926107881863976,
-            "heading": 3.14,
-            "angularVelocity": 2.0229336550458753e-17,
+            "heading": 7.355026472308393e-17,
+            "angularVelocity": 3.8038299911394555e-16,
             "velocityX": 3.5204949744296554,
-            "velocityY": -1.384467049902731,
+            "velocityY": -1.3844670499027307,
             "timestamp": 1.78338638760593
         },
         {
             "x": 5.550146685206584,
             "y": 1.5496573158737061,
-            "heading": 3.14,
-            "angularVelocity": -2.618637002687426e-16,
-            "velocityX": 3.561490858687975,
-            "velocityY": -1.2753104512640405,
+            "heading": 8.480161924468705e-17,
+            "angularVelocity": 3.3405844137167443e-16,
+            "velocityX": 3.5614908586879754,
+            "velocityY": -1.2753104512640399,
             "timestamp": 1.8170671845209967
         },
         {
             "x": 5.670100671705041,
             "y": 1.506704223843596,
-            "heading": 3.14,
-            "angularVelocity": -1.632751475559818e-17,
-            "velocityX": 3.5614949017075617,
+            "heading": 8.467696633181045e-17,
+            "angularVelocity": -3.701008417078351e-18,
+            "velocityX": 3.5614949017075612,
             "velocityY": -1.2752991604806918,
             "timestamp": 1.8507479814360634
         },
         {
             "x": 5.790054658213967,
-            "y": 1.4637511318444056,
-            "heading": 3.14,
-            "angularVelocity": -2.9492764372093784e-16,
+            "y": 1.4637511318444054,
+            "heading": 8.225246246818014e-17,
+            "angularVelocity": -7.198475350046388e-17,
             "velocityX": 3.561494902018399,
-            "velocityY": -1.2752991595627796,
+            "velocityY": -1.275299159562779,
             "timestamp": 1.8844287783511302
         },
         {
             "x": 5.910008644722922,
-            "y": 1.4207980398452127,
-            "heading": 3.14,
-            "angularVelocity": -2.6299005747305263e-17,
+            "y": 1.4207980398452125,
+            "heading": 9.443730259968232e-17,
+            "angularVelocity": 3.617741041647218e-16,
             "velocityX": 3.5614949020192235,
-            "velocityY": -1.2752991595628407,
+            "velocityY": -1.2752991595628402,
             "timestamp": 1.918109575266197
         },
         {
             "x": 6.02996263123188,
-            "y": 1.3778449478460184,
-            "heading": 3.14,
-            "angularVelocity": -3.0549772131007975e-16,
+            "y": 1.3778449478460182,
+            "heading": 8.582959689044548e-17,
+            "angularVelocity": -2.555671628240573e-16,
             "velocityX": 3.5614949020193487,
             "velocityY": -1.2752991595628858,
             "timestamp": 1.9517903721812637
         },
         {
             "x": 6.149916617740839,
-            "y": 1.3348918558468237,
-            "heading": 3.14,
-            "angularVelocity": 3.2741509193878177e-16,
+            "y": 1.3348918558468235,
+            "heading": 7.701354643967334e-17,
+            "angularVelocity": -2.6175302422337863e-16,
             "velocityX": 3.56149490201936,
-            "velocityY": -1.27529915956289,
+            "velocityY": -1.2752991595628895,
             "timestamp": 1.9854711690963305
         },
         {
             "x": 6.269870604249798,
-            "y": 1.2919387638476292,
-            "heading": 3.14,
-            "angularVelocity": 3.4990315690965786e-16,
+            "y": 1.291938763847629,
+            "heading": 8.367010625720842e-17,
+            "angularVelocity": 1.9763664839406868e-16,
             "velocityX": 3.5614949020193607,
-            "velocityY": -1.2752991595628904,
+            "velocityY": -1.27529915956289,
             "timestamp": 2.019151966011397
         },
         {
             "x": 6.3898245907587565,
-            "y": 1.2489856718484345,
-            "heading": 3.14,
-            "angularVelocity": 2.004607715018742e-16,
-            "velocityX": 3.5614949020193603,
-            "velocityY": -1.27529915956289,
+            "y": 1.2489856718484342,
+            "heading": 7.968637324735258e-17,
+            "angularVelocity": -1.1827906031741492e-16,
+            "velocityX": 3.5614949020193607,
+            "velocityY": -1.2752991595628902,
             "timestamp": 2.0528327629264638
         },
         {
             "x": 6.509778577267715,
             "y": 1.2060325798492397,
-            "heading": 3.14,
-            "angularVelocity": 3.130071946862803e-16,
-            "velocityX": 3.5614949020193603,
-            "velocityY": -1.2752991595628902,
+            "heading": 7.914041276645377e-17,
+            "angularVelocity": -1.6209844508000374e-17,
+            "velocityX": 3.561494902019361,
+            "velocityY": -1.27529915956289,
             "timestamp": 2.0865135598415305
         },
         {
             "x": 6.629732563776674,
             "y": 1.1630794878500452,
-            "heading": 3.14,
-            "angularVelocity": -2.481040891393771e-16,
-            "velocityX": 3.5614949020193607,
-            "velocityY": -1.2752991595628902,
+            "heading": 7.575912442206911e-17,
+            "angularVelocity": -1.0039217162560839e-16,
+            "velocityX": 3.561494902019361,
+            "velocityY": -1.27529915956289,
             "timestamp": 2.1201943567565973
         },
         {
             "x": 6.749686550285633,
             "y": 1.1201263958508503,
-            "heading": 3.14,
-            "angularVelocity": 5.765266318677121e-16,
-            "velocityX": 3.561494902019357,
+            "heading": 6.889954024340196e-17,
+            "angularVelocity": -2.0366454499176497e-16,
+            "velocityX": 3.5614949020193576,
             "velocityY": -1.2752991595628989,
             "timestamp": 2.153875153671664
         },
         {
             "x": 6.869640536794461,
             "y": 1.077173303851719,
-            "heading": 3.14,
-            "angularVelocity": 5.94200130731494e-16,
+            "heading": 8.054846416919904e-17,
+            "angularVelocity": 3.458624792985843e-16,
             "velocityX": 3.5614949020154905,
-            "velocityY": -1.2752991595610093,
+            "velocityY": -1.275299159561009,
             "timestamp": 2.187555950586731
         },
         {
             "x": 6.985406128017608,
             "y": 1.0357199913369377,
-            "heading": 3.14,
-            "angularVelocity": -5.216843428237177e-16,
-            "velocityX": 3.437139314579594,
+            "heading": 6.97905590140722e-17,
+            "angularVelocity": -3.1940767538205783e-16,
+            "velocityX": 3.437139314579595,
             "velocityY": -1.2307699434580792,
             "timestamp": 2.2212367475017976
         },
         {
             "x": 7.092266700241795,
-            "y": 0.9974553856875887,
-            "heading": 3.14,
-            "angularVelocity": -4.51325473252053e-16,
-            "velocityX": 3.172744768885923,
+            "y": 0.9974553856875888,
+            "heading": 5.947830002893604e-17,
+            "angularVelocity": -3.0617621445548207e-16,
+            "velocityX": 3.172744768885924,
             "velocityY": -1.1360956139440452,
             "timestamp": 2.2549175444168643
         },
         {
-            "x": 7.190222235939472,
+            "x": 7.190222235939473,
             "y": 0.9623794931799317,
-            "heading": 3.14,
-            "angularVelocity": -3.932803198593372e-16,
-            "velocityX": 2.908349702790401,
+            "heading": 5.044588900647289e-17,
+            "angularVelocity": -2.6817687744625345e-16,
+            "velocityX": 2.9083497027904013,
             "velocityY": -1.0414210980846788,
             "timestamp": 2.288598341331931
         },
         {
             "x": 7.279272729268112,
-            "y": 0.9304923159060571,
-            "heading": 3.14,
-            "angularVelocity": -3.6674368533996665e-16,
-            "velocityX": 2.643954463227235,
-            "velocityY": -0.9467465201100715,
+            "y": 0.930492315906057,
+            "heading": 4.27627289839913e-17,
+            "angularVelocity": -2.281169282784766e-16,
+            "velocityX": 2.6439544632272356,
+            "velocityY": -0.9467465201100714,
             "timestamp": 2.322279138246998
         },
         {
             "x": 7.35941817730645,
-            "y": 0.9017938549120107,
-            "heading": 3.14,
-            "angularVelocity": -3.61350987503525e-16,
-            "velocityX": 2.3795591369302214,
+            "y": 0.9017938549120106,
+            "heading": 3.626946964953948e-17,
+            "angularVelocity": -1.9278817213060433e-16,
+            "velocityX": 2.379559136930222,
             "velocityY": -0.8520719110778341,
             "timestamp": 2.3559599351620646
         },
         {
             "x": 7.430658578301727,
             "y": 0.8762841108254199,
-            "heading": 3.14,
-            "angularVelocity": -3.11296740830968e-16,
-            "velocityX": 2.115163758592898,
+            "heading": 2.9442071838197796e-17,
+            "angularVelocity": -2.0270891340137855e-16,
+            "velocityX": 2.1151637585928986,
             "velocityY": -0.7573972834110186,
             "timestamp": 2.3896407320771313
         },
         {
             "x": 7.492993931085436,
             "y": 0.8539630840647031,
-            "heading": 3.14,
-            "angularVelocity": -3.0797715285651212e-16,
-            "velocityX": 1.8507683455620387,
-            "velocityY": -0.6627226433211519,
+            "heading": 2.3413209096667504e-17,
+            "angularVelocity": -1.790000012026981e-16,
+            "velocityX": 1.850768345562039,
+            "velocityY": -0.662722643321152,
             "timestamp": 2.423321528992198
         },
         {
             "x": 7.546424234822929,
             "y": 0.8348307749287305,
-            "heading": 3.14,
-            "angularVelocity": -2.384214973522538e-16,
-            "velocityX": 1.5863729077500837,
-            "velocityY": -0.5680479943576783,
+            "heading": 1.7731266545962487e-17,
+            "angularVelocity": -1.6869976443484408e-16,
+            "velocityX": 1.5863729077500839,
+            "velocityY": -0.5680479943576784,
             "timestamp": 2.457002325907265
         },
         {
             "x": 7.590949488888224,
             "y": 0.8188871836416546,
-            "heading": 3.14,
-            "angularVelocity": -1.6873896303460275e-16,
-            "velocityX": 1.3219774513523088,
-            "velocityY": -0.4733733387389998,
+            "heading": 1.2363765500283435e-17,
+            "angularVelocity": -1.593638362048541e-16,
+            "velocityX": 1.321977451352309,
+            "velocityY": -0.4733733387389999,
             "timestamp": 2.4906831228223316
         },
         {
             "x": 7.626569692794441,
             "y": 0.8061323103778162,
-            "heading": 3.14,
-            "angularVelocity": -1.2706181170586285e-16,
-            "velocityX": 1.0575819804988977,
-            "velocityY": -0.3786986779440515,
+            "heading": 7.88828312817125e-18,
+            "angularVelocity": -1.3287934794211324e-16,
+            "velocityX": 1.057581980498898,
+            "velocityY": -0.37869867794405154,
             "timestamp": 2.5243639197373984
         },
         {
             "x": 7.653284846152079,
             "y": 0.7965661552766882,
-            "heading": 3.14,
-            "angularVelocity": -1.0566459548829111e-16,
-            "velocityX": 0.7931864980809785,
-            "velocityY": -0.28402401300808755,
+            "heading": 4.236479358871306e-18,
+            "angularVelocity": -1.0842391112822455e-16,
+            "velocityX": 0.7931864980809786,
+            "velocityY": -0.28402401300808766,
             "timestamp": 2.558044716652465
         },
         {
             "x": 7.671094948642455,
             "y": 0.7901887184523844,
-            "heading": 3.14,
-            "angularVelocity": -6.428789472113484e-17,
-            "velocityX": 0.5287910062011897,
-            "velocityY": -0.18934934468402026,
+            "heading": 1.249730071610374e-18,
+            "angularVelocity": -8.867810508445172e-17,
+            "velocityX": 0.5287910062011898,
+            "velocityY": -0.18934934468402032,
             "timestamp": 2.591725513567532
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": -8.910267594003032e-17,
-            "velocityX": 0.26439550643651,
-            "velocityY": -0.09467467353653364,
+            "heading": -2.906959217388984e-34,
+            "angularVelocity": -3.7105121777105e-17,
+            "velocityX": 0.26439550643651005,
+            "velocityY": -0.09467467353653367,
             "timestamp": 2.6254063104825986
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": -3.385083394775927e-32,
-            "velocityX": 1.13191163707898e-33,
-            "velocityY": 3.1068609037049197e-32,
+            "heading": -4.078160951605287e-34,
+            "angularVelocity": -3.477050502256718e-33,
+            "velocityX": 7.685636324015334e-33,
+            "velocityY": -2.3747808746440768e-32,
             "timestamp": 2.6590871073976654
         }
     ]
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.traj
index 24664a13..39d8ef61 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.traj	
@@ -3,622 +3,622 @@
         {
             "x": 1.35,
             "y": 5.56,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": 3.295386080925321e-33,
+            "velocityY": -4.1566893564125267e-32,
             "timestamp": 0
         },
         {
-            "x": 1.3546830312136446,
+            "x": 1.3546830312136449,
             "y": 5.550921973415328,
-            "heading": 3.14,
-            "angularVelocity": -6.105321890596023e-17,
-            "velocityX": 0.13379730014070298,
-            "velocityY": -0.2593652256885782,
-            "timestamp": 0.03500093954603884
-        },
-        {
-            "x": 1.364049093509165,
-            "y": 5.532765920501418,
-            "heading": 3.14,
-            "angularVelocity": -9.872410216704159e-17,
-            "velocityX": 0.2675945965166781,
+            "heading": -1.4050458772244498e-18,
+            "angularVelocity": -4.014308801297789e-17,
+            "velocityX": 0.13379730014070312,
+            "velocityY": -0.25936522568857817,
+            "timestamp": 0.03500093954603885
+        },
+        {
+            "x": 1.3640490935091651,
+            "y": 5.532765920501419,
+            "heading": -3.603876179363921e-18,
+            "angularVelocity": -6.28220327965604e-17,
+            "velocityX": 0.26759459651667833,
             "velocityY": -0.5187304440792354,
-            "timestamp": 0.07000187909207768
+            "timestamp": 0.0700018790920777
         },
         {
-            "x": 1.3780981867284383,
+            "x": 1.3780981867284385,
             "y": 5.505531841564791,
-            "heading": 3.14,
-            "angularVelocity": -1.396517144912572e-16,
-            "velocityX": 0.40139188837498685,
+            "heading": -4.091508575199503e-18,
+            "angularVelocity": -1.3931950355515604e-17,
+            "velocityX": 0.4013918883749871,
             "velocityY": -0.7780956537124009,
-            "timestamp": 0.10500281863811653
+            "timestamp": 0.10500281863811654
         },
         {
-            "x": 1.3968303106782043,
-            "y": 5.46921973698008,
-            "heading": 3.14,
-            "angularVelocity": -1.3147784440878345e-16,
-            "velocityX": 0.5351891747117138,
+            "x": 1.3968303106782045,
+            "y": 5.469219736980081,
+            "heading": -4.985304542720085e-18,
+            "angularVelocity": -2.5536326104216816e-17,
+            "velocityX": 0.5351891747117141,
             "velocityY": -1.0374608526419855,
-            "timestamp": 0.14000375818415536
+            "timestamp": 0.1400037581841554
         },
         {
-            "x": 1.4202454651168874,
-            "y": 5.423829607215582,
-            "heading": 3.14,
-            "angularVelocity": -7.833997421538468e-18,
-            "velocityX": 0.6689864541464796,
+            "x": 1.4202454651168877,
+            "y": 5.423829607215581,
+            "heading": -4.3424315250592746e-18,
+            "angularVelocity": 1.8367331153854942e-17,
+            "velocityX": 0.6689864541464801,
             "velocityY": -1.2968260381921248,
-            "timestamp": 0.1750046977301942
+            "timestamp": 0.17500469773019423
         },
         {
-            "x": 1.4483436497338924,
+            "x": 1.4483436497338926,
             "y": 5.369361452873384,
-            "heading": 3.14,
-            "angularVelocity": -2.0049167233544137e-17,
-            "velocityX": 0.802783724707322,
+            "heading": -2.0560193041515625e-18,
+            "angularVelocity": 6.532433512064817e-17,
+            "velocityX": 0.8027837247073224,
             "velocityY": -1.5561912065401715,
-            "timestamp": 0.21000563727623303
+            "timestamp": 0.21000563727623306
         },
         {
-            "x": 1.4811248641150931,
+            "x": 1.4811248641150936,
             "y": 5.305815274756268,
-            "heading": 3.14,
-            "angularVelocity": -8.129804229935506e-17,
-            "velocityX": 0.9365809834363131,
-            "velocityY": -1.8155563519521838,
-            "timestamp": 0.24500657682227187
+            "heading": 2.1208651292681738e-19,
+            "angularVelocity": 6.480130833433249e-17,
+            "velocityX": 0.9365809834363137,
+            "velocityY": -1.815556351952184,
+            "timestamp": 0.2450065768222719
         },
         {
-            "x": 1.5185891076807168,
+            "x": 1.518589107680717,
             "y": 5.233191073988126,
-            "heading": 3.14,
-            "angularVelocity": -1.3088931274135507e-16,
-            "velocityX": 1.0703782256008025,
-            "velocityY": -2.0749214652539205,
+            "heading": 3.0112738874394488e-18,
+            "angularVelocity": 7.99746695569e-17,
+            "velocityX": 1.0703782256008032,
+            "velocityY": -2.074921465253921,
             "timestamp": 0.2800075163683107
         },
         {
-            "x": 1.56073637956111,
-            "y": 5.151488852254778,
-            "heading": 3.14,
-            "angularVelocity": -1.390272479709275e-16,
-            "velocityX": 1.204175442918738,
+            "x": 1.5607363795611104,
+            "y": 5.151488852254779,
+            "heading": 5.4707987602321704e-18,
+            "angularVelocity": 7.027028770373186e-17,
+            "velocityX": 1.2041754429187383,
             "velocityY": -2.334286530390624,
             "timestamp": 0.31500845591434956
         },
         {
-            "x": 1.6075666783068707,
+            "x": 1.6075666783068712,
             "y": 5.060708612365892,
-            "heading": 3.14,
-            "angularVelocity": -2.002620206188659e-16,
-            "velocityX": 1.3379726188262815,
-            "velocityY": -2.593651515253289,
+            "heading": 7.441304754324509e-18,
+            "angularVelocity": 5.629869559433992e-17,
+            "velocityX": 1.3379726188262824,
+            "velocityY": -2.5936515152532893,
             "timestamp": 0.3500093954603884
         },
         {
-            "x": 1.6590800010192643,
+            "x": 1.6590800010192646,
             "y": 4.9608503599406655,
-            "heading": 3.14,
-            "angularVelocity": -2.3956924314672257e-16,
-            "velocityX": 1.471769711915058,
-            "velocityY": -2.8530163395717247,
+            "heading": 8.015191331788108e-18,
+            "angularVelocity": 1.639633984973558e-17,
+            "velocityX": 1.4717697119150586,
+            "velocityY": -2.853016339571725,
             "timestamp": 0.38501033500642723
         },
         {
-            "x": 1.7152763390026582,
-            "y": 4.851914111835608,
-            "heading": 3.14,
-            "angularVelocity": -2.6938408534488506e-16,
-            "velocityX": 1.6055665565638675,
-            "velocityY": -3.112380682288517,
+            "x": 1.7152763390026586,
+            "y": 4.851914111835609,
+            "heading": 8.871725964928404e-18,
+            "angularVelocity": 2.4471795525975083e-17,
+            "velocityX": 1.6055665565638684,
+            "velocityY": -3.112380682288518,
             "timestamp": 0.42001127455246606
         },
         {
-            "x": 1.7759790338056225,
-            "y": 4.734242319347403,
-            "heading": 3.14,
-            "angularVelocity": 3.279195870571412e-16,
-            "velocityX": 1.734316152375285,
-            "velocityY": -3.3619609648879023,
+            "x": 1.775979033805623,
+            "y": 4.734242319347404,
+            "heading": 1.526334137039101e-17,
+            "angularVelocity": 1.8261268092684144e-16,
+            "velocityX": 1.7343161523752855,
+            "velocityY": -3.361960964887902,
             "timestamp": 0.4550122140985049
         },
         {
-            "x": 1.8366817286107124,
-            "y": 4.616570526856246,
-            "heading": 3.14,
-            "angularVelocity": -3.1071543812317194e-16,
-            "velocityX": 1.7343161524360031,
-            "velocityY": -3.36196096497222,
+            "x": 1.8366817286107127,
+            "y": 4.616570526856247,
+            "heading": 2.6738432547349554e-17,
+            "angularVelocity": 3.2785094702569765e-16,
+            "velocityX": 1.734316152436003,
+            "velocityY": -3.3619609649722197,
             "timestamp": 0.49001315364454373
         },
         {
-            "x": 1.8973844234157842,
+            "x": 1.8973844234157844,
             "y": 4.498898734365081,
-            "heading": 3.14,
-            "angularVelocity": -2.903673566166758e-16,
-            "velocityX": 1.7343161524354895,
-            "velocityY": -3.361960964972484,
+            "heading": 1.9852147320466475e-17,
+            "angularVelocity": -1.967457250062152e-16,
+            "velocityX": 1.7343161524354893,
+            "velocityY": -3.3619609649724835,
             "timestamp": 0.5250140931905826
         },
         {
-            "x": 1.958087118220856,
-            "y": 4.381226941873915,
-            "heading": 3.14,
-            "angularVelocity": 3.614460545323391e-16,
+            "x": 1.9580871182208561,
+            "y": 4.381226941873916,
+            "heading": 2.777024505719557e-17,
+            "angularVelocity": 2.2622529107579697e-16,
             "velocityX": 1.7343161524354895,
-            "velocityY": -3.3619609649724835,
+            "velocityY": -3.361960964972483,
             "timestamp": 0.5600150327366215
         },
         {
-            "x": 2.0187898130259274,
+            "x": 2.018789813025928,
             "y": 4.26355514938275,
-            "heading": 3.14,
-            "angularVelocity": -3.3191632817458094e-16,
+            "heading": 1.7414652311267274e-17,
+            "angularVelocity": -2.9586613617347484e-16,
             "velocityX": 1.7343161524354893,
             "velocityY": -3.361960964972483,
             "timestamp": 0.5950159722826603
         },
         {
-            "x": 2.079492507830999,
-            "y": 4.145883356891584,
-            "heading": 3.14,
-            "angularVelocity": 3.2426066858348807e-16,
-            "velocityX": 1.7343161524354893,
+            "x": 2.0794925078309996,
+            "y": 4.145883356891583,
+            "heading": 6.771469247853252e-18,
+            "angularVelocity": -3.040827818183264e-16,
+            "velocityX": 1.7343161524354895,
             "velocityY": -3.361960964972483,
             "timestamp": 0.6300169118286992
         },
         {
             "x": 2.140195202636071,
-            "y": 4.028211564400418,
-            "heading": 3.14,
-            "angularVelocity": -2.798848785469217e-16,
-            "velocityX": 1.734316152435488,
-            "velocityY": -3.3619609649724826,
+            "y": 4.028211564400419,
+            "heading": 1.7156471914154247e-17,
+            "angularVelocity": 2.9670636674740876e-16,
+            "velocityX": 1.7343161524354882,
+            "velocityY": -3.361960964972482,
             "timestamp": 0.6650178513747381
         },
         {
-            "x": 2.200897897441142,
-            "y": 3.9105397719092525,
-            "heading": 3.14,
-            "angularVelocity": -1.795170282800533e-16,
+            "x": 2.2008978974411426,
+            "y": 3.910539771909253,
+            "heading": 9.427453062324153e-18,
+            "angularVelocity": -2.208231822366884e-16,
             "velocityX": 1.734316152435471,
             "velocityY": -3.361960964972468,
             "timestamp": 0.700018790920777
         },
         {
-            "x": 2.2616005922462064,
-            "y": 3.792867979418094,
-            "heading": 3.14,
-            "angularVelocity": -3.385716636422731e-16,
-            "velocityX": 1.7343161524352515,
-            "velocityY": -3.3619609649722846,
+            "x": 2.261600592246207,
+            "y": 3.7928679794180944,
+            "heading": -2.0403441270686145e-18,
+            "angularVelocity": -3.2764255297515643e-16,
+            "velocityX": 1.7343161524352513,
+            "velocityY": -3.3619609649722837,
             "timestamp": 0.7350197304668159
         },
         {
-            "x": 2.3223032870512124,
+            "x": 2.322303287051213,
             "y": 3.6751961869269927,
-            "heading": 3.14,
-            "angularVelocity": -3.759822562526031e-16,
-            "velocityX": 1.7343161524334345,
-            "velocityY": -3.361960964970739,
+            "heading": 3.357966916114392e-18,
+            "angularVelocity": 1.5423331811082687e-16,
+            "velocityX": 1.7343161524334347,
+            "velocityY": -3.3619609649707387,
             "timestamp": 0.7700206700128548
         },
         {
-            "x": 2.383005981879538,
-            "y": 3.5575243944476096,
-            "heading": 3.14,
-            "angularVelocity": -2.6556763706975105e-16,
+            "x": 2.383005981879539,
+            "y": 3.55752439444761,
+            "heading": 1.6083387472754582e-17,
+            "angularVelocity": 3.6357368464068287e-16,
             "velocityX": 1.7343161531001825,
-            "velocityY": -3.3619609646356636,
+            "velocityY": -3.361960964635663,
             "timestamp": 0.8050216095588937
         },
         {
-            "x": 2.443708981028518,
-            "y": 3.4398527589551926,
-            "heading": 3.14,
-            "angularVelocity": 5.092773468950765e-17,
+            "x": 2.4437089810285184,
+            "y": 3.439852758955193,
+            "heading": 2.2946423291224054e-17,
+            "angularVelocity": 1.9608147402560587e-16,
             "velocityX": 1.7343248477441577,
             "velocityY": -3.3619564794134043,
             "timestamp": 0.8400225491049326
         },
         {
             "x": 2.507799181067132,
-            "y": 3.3239911422386283,
-            "heading": 3.14,
-            "angularVelocity": -2.1081778211927598e-16,
+            "y": 3.323991142238629,
+            "heading": 9.669545465348722e-18,
+            "angularVelocity": -3.793291836756151e-16,
             "velocityX": 1.8310994181462568,
-            "velocityY": -3.310243045471397,
+            "velocityY": -3.3102430454713967,
             "timestamp": 0.8750234886509715
         },
         {
             "x": 2.5806292795037855,
-            "y": 3.2134142705817266,
-            "heading": 3.14,
-            "angularVelocity": -3.875593095285532e-16,
+            "y": 3.213414270581727,
+            "heading": 3.38196516760506e-17,
+            "angularVelocity": 6.899845125281262e-16,
             "velocityX": 2.0808040978720626,
             "velocityY": -3.159254382625257,
             "timestamp": 0.9100244281970103
         },
         {
             "x": 2.661766753531331,
-            "y": 3.108780729790167,
-            "heading": 3.14,
-            "angularVelocity": -7.347130647585027e-16,
-            "velocityX": 2.318151314794784,
+            "y": 3.1087807297901673,
+            "heading": 5.146705636243318e-17,
+            "angularVelocity": 5.041980276892245e-16,
+            "velocityX": 2.3181513147947848,
             "velocityY": -2.9894494876003863,
             "timestamp": 0.9450253677430492
         },
         {
             "x": 2.7507288455963135,
             "y": 3.0107133388519287,
-            "heading": 3.14,
-            "angularVelocity": -3.52994704864956e-16,
-            "velocityX": 2.541705829014059,
-            "velocityY": -2.801850242022933,
+            "heading": 6.422105835490463e-17,
+            "angularVelocity": 3.643902751722035e-16,
+            "velocityX": 2.5417058290140595,
+            "velocityY": -2.8018502420229328,
             "timestamp": 0.9800263072890881
         },
         {
             "x": 2.9215337593923705,
             "y": 2.8658352980219064,
-            "heading": 3.14,
-            "angularVelocity": 1.5913396610629706e-16,
-            "velocityX": 2.884920936848367,
-            "velocityY": -2.4470121145413266,
+            "heading": 5.1318206888106e-17,
+            "angularVelocity": -2.1793112103404205e-16,
+            "velocityX": 2.884920936848368,
+            "velocityY": -2.4470121145413257,
             "timestamp": 1.0392324060123015
         },
         {
             "x": 3.109750180151373,
             "y": 2.744432888397421,
-            "heading": 3.14,
-            "angularVelocity": -6.286818756935585e-16,
-            "velocityX": 3.179003934013425,
-            "velocityY": -2.0505051378666,
+            "heading": 6.648964720130561e-17,
+            "angularVelocity": 2.5624793121609224e-16,
+            "velocityX": 3.179003934013426,
+            "velocityY": -2.0505051378665997,
             "timestamp": 1.098438504735515
         },
         {
-            "x": 3.3097585780303125,
+            "x": 3.309758578030313,
             "y": 2.6436330516151556,
-            "heading": 3.14,
-            "angularVelocity": -3.4341251732407706e-17,
+            "heading": 6.809268296964168e-17,
+            "angularVelocity": 2.7075517621497836e-17,
             "velocityX": 3.3781722186064402,
-            "velocityY": -1.702524553319067,
+            "velocityY": -1.702524553319066,
             "timestamp": 1.1576446034587284
         },
         {
-            "x": 3.5097675494852,
-            "y": 2.5428343529427293,
-            "heading": 3.14,
-            "angularVelocity": -2.2516273301091217e-16,
-            "velocityX": 3.3781819063939547,
-            "velocityY": -1.7025053304654465,
+            "x": 3.5097675494851996,
+            "y": 2.542834352942729,
+            "heading": 6.982732716570642e-17,
+            "angularVelocity": 2.929840393934814e-17,
+            "velocityX": 3.378181906393955,
+            "velocityY": -1.702505330465446,
             "timestamp": 1.216850702181942
         },
         {
             "x": 3.709776520948374,
-            "y": 2.442035654287211,
-            "heading": 3.14,
-            "angularVelocity": -3.629334864774022e-16,
-            "velocityX": 3.378181906533671,
-            "velocityY": -1.7025053301803685,
+            "y": 2.4420356542872104,
+            "heading": 8.871000937094426e-17,
+            "angularVelocity": 3.189313704575333e-16,
+            "velocityX": 3.3781819065336705,
+            "velocityY": -1.702505330180368,
             "timestamp": 1.2760568009051554
         },
         {
-            "x": 3.909785492411555,
-            "y": 2.3412369556316897,
-            "heading": 3.14,
-            "angularVelocity": -2.39059813931354e-16,
+            "x": 3.9097854924115545,
+            "y": 2.341236955631689,
+            "heading": 7.04137580064196e-17,
+            "angularVelocity": -3.090264644873572e-16,
             "velocityX": 3.3781819065337895,
-            "velocityY": -1.7025053301803958,
+            "velocityY": -1.7025053301803954,
             "timestamp": 1.3352628996283689
         },
         {
             "x": 4.109794463874735,
             "y": 2.240438256976168,
-            "heading": 3.14,
-            "angularVelocity": 3.7738636092274995e-16,
-            "velocityX": 3.378181906533792,
-            "velocityY": -1.7025053301803967,
+            "heading": 6.635408667627181e-17,
+            "angularVelocity": -6.856846534504647e-17,
+            "velocityX": 3.3781819065337926,
+            "velocityY": -1.7025053301803963,
             "timestamp": 1.3944689983515823
         },
         {
             "x": 4.3098034353379155,
-            "y": 2.139639558320647,
-            "heading": 3.14,
-            "angularVelocity": 3.725508803000677e-16,
+            "y": 2.1396395583206465,
+            "heading": 8.756379552984881e-17,
+            "angularVelocity": 3.5823520399025277e-16,
             "velocityX": 3.3781819065337926,
-            "velocityY": -1.7025053301803965,
+            "velocityY": -1.7025053301803963,
             "timestamp": 1.4536750970747958
         },
         {
             "x": 4.509812406801097,
             "y": 2.0388408596651253,
-            "heading": 3.14,
-            "angularVelocity": -2.2963176234131617e-16,
+            "heading": 7.076319345079811e-17,
+            "angularVelocity": -2.837647208878057e-16,
             "velocityX": 3.3781819065337926,
-            "velocityY": -1.7025053301803967,
+            "velocityY": -1.7025053301803963,
             "timestamp": 1.5128811957980093
         },
         {
             "x": 4.709821378264278,
             "y": 1.9380421610096037,
-            "heading": 3.14,
-            "angularVelocity": -2.0546569648664528e-16,
-            "velocityX": 3.3781819065338015,
-            "velocityY": -1.702505330180399,
+            "heading": 6.652684249010322e-17,
+            "angularVelocity": -7.15526111686399e-17,
+            "velocityX": 3.378181906533802,
+            "velocityY": -1.7025053301803985,
             "timestamp": 1.5720872945212228
         },
         {
             "x": 4.9098303497278835,
-            "y": 1.8372434623549267,
-            "heading": 3.14,
-            "angularVelocity": 1.0642243284106308e-16,
-            "velocityX": 3.3781819065409677,
-            "velocityY": -1.7025053301661326,
+            "y": 1.837243462354927,
+            "heading": 7.855425123263822e-17,
+            "angularVelocity": 2.0314476045367986e-16,
+            "velocityX": 3.378181906540968,
+            "velocityY": -1.7025053301661324,
             "timestamp": 1.6312933932444362
         },
         {
             "x": 5.109839349236117,
-            "y": 1.736444819347842,
-            "heading": 3.14,
-            "angularVelocity": 1.2679224790319894e-16,
+            "y": 1.7364448193478421,
+            "heading": 7.31685607290898e-17,
+            "angularVelocity": -9.096513061477316e-17,
             "velocityX": 3.378182380218786,
-            "velocityY": -1.7025043902702677,
+            "velocityY": -1.7025043902702668,
             "timestamp": 1.6904994919676497
         },
         {
             "x": 5.311619758605957,
             "y": 1.6392407417297363,
-            "heading": 3.14,
-            "angularVelocity": 3.145701878302232e-16,
-            "velocityX": 3.408101761838499,
-            "velocityY": -1.6417916348765778,
+            "heading": 6.073866217998324e-17,
+            "angularVelocity": -2.0994287441931112e-16,
+            "velocityX": 3.4081017618384997,
+            "velocityY": -1.6417916348765775,
             "timestamp": 1.7497055906908632
         },
         {
             "x": 5.430192834880249,
             "y": 1.5926107881863976,
-            "heading": 3.14,
-            "angularVelocity": 2.0229336550458753e-17,
+            "heading": 7.355026472308393e-17,
+            "angularVelocity": 3.8038299911394555e-16,
             "velocityX": 3.5204949744296554,
-            "velocityY": -1.384467049902731,
+            "velocityY": -1.3844670499027307,
             "timestamp": 1.78338638760593
         },
         {
             "x": 5.550146685206584,
             "y": 1.5496573158737061,
-            "heading": 3.14,
-            "angularVelocity": -2.618637002687426e-16,
-            "velocityX": 3.561490858687975,
-            "velocityY": -1.2753104512640405,
+            "heading": 8.480161924468705e-17,
+            "angularVelocity": 3.3405844137167443e-16,
+            "velocityX": 3.5614908586879754,
+            "velocityY": -1.2753104512640399,
             "timestamp": 1.8170671845209967
         },
         {
             "x": 5.670100671705041,
             "y": 1.506704223843596,
-            "heading": 3.14,
-            "angularVelocity": -1.632751475559818e-17,
-            "velocityX": 3.5614949017075617,
+            "heading": 8.467696633181045e-17,
+            "angularVelocity": -3.701008417078351e-18,
+            "velocityX": 3.5614949017075612,
             "velocityY": -1.2752991604806918,
             "timestamp": 1.8507479814360634
         },
         {
             "x": 5.790054658213967,
-            "y": 1.4637511318444056,
-            "heading": 3.14,
-            "angularVelocity": -2.9492764372093784e-16,
+            "y": 1.4637511318444054,
+            "heading": 8.225246246818014e-17,
+            "angularVelocity": -7.198475350046388e-17,
             "velocityX": 3.561494902018399,
-            "velocityY": -1.2752991595627796,
+            "velocityY": -1.275299159562779,
             "timestamp": 1.8844287783511302
         },
         {
             "x": 5.910008644722922,
-            "y": 1.4207980398452127,
-            "heading": 3.14,
-            "angularVelocity": -2.6299005747305263e-17,
+            "y": 1.4207980398452125,
+            "heading": 9.443730259968232e-17,
+            "angularVelocity": 3.617741041647218e-16,
             "velocityX": 3.5614949020192235,
-            "velocityY": -1.2752991595628407,
+            "velocityY": -1.2752991595628402,
             "timestamp": 1.918109575266197
         },
         {
             "x": 6.02996263123188,
-            "y": 1.3778449478460184,
-            "heading": 3.14,
-            "angularVelocity": -3.0549772131007975e-16,
+            "y": 1.3778449478460182,
+            "heading": 8.582959689044548e-17,
+            "angularVelocity": -2.555671628240573e-16,
             "velocityX": 3.5614949020193487,
             "velocityY": -1.2752991595628858,
             "timestamp": 1.9517903721812637
         },
         {
             "x": 6.149916617740839,
-            "y": 1.3348918558468237,
-            "heading": 3.14,
-            "angularVelocity": 3.2741509193878177e-16,
+            "y": 1.3348918558468235,
+            "heading": 7.701354643967334e-17,
+            "angularVelocity": -2.6175302422337863e-16,
             "velocityX": 3.56149490201936,
-            "velocityY": -1.27529915956289,
+            "velocityY": -1.2752991595628895,
             "timestamp": 1.9854711690963305
         },
         {
             "x": 6.269870604249798,
-            "y": 1.2919387638476292,
-            "heading": 3.14,
-            "angularVelocity": 3.4990315690965786e-16,
+            "y": 1.291938763847629,
+            "heading": 8.367010625720842e-17,
+            "angularVelocity": 1.9763664839406868e-16,
             "velocityX": 3.5614949020193607,
-            "velocityY": -1.2752991595628904,
+            "velocityY": -1.27529915956289,
             "timestamp": 2.019151966011397
         },
         {
             "x": 6.3898245907587565,
-            "y": 1.2489856718484345,
-            "heading": 3.14,
-            "angularVelocity": 2.004607715018742e-16,
-            "velocityX": 3.5614949020193603,
-            "velocityY": -1.27529915956289,
+            "y": 1.2489856718484342,
+            "heading": 7.968637324735258e-17,
+            "angularVelocity": -1.1827906031741492e-16,
+            "velocityX": 3.5614949020193607,
+            "velocityY": -1.2752991595628902,
             "timestamp": 2.0528327629264638
         },
         {
             "x": 6.509778577267715,
             "y": 1.2060325798492397,
-            "heading": 3.14,
-            "angularVelocity": 3.130071946862803e-16,
-            "velocityX": 3.5614949020193603,
-            "velocityY": -1.2752991595628902,
+            "heading": 7.914041276645377e-17,
+            "angularVelocity": -1.6209844508000374e-17,
+            "velocityX": 3.561494902019361,
+            "velocityY": -1.27529915956289,
             "timestamp": 2.0865135598415305
         },
         {
             "x": 6.629732563776674,
             "y": 1.1630794878500452,
-            "heading": 3.14,
-            "angularVelocity": -2.481040891393771e-16,
-            "velocityX": 3.5614949020193607,
-            "velocityY": -1.2752991595628902,
+            "heading": 7.575912442206911e-17,
+            "angularVelocity": -1.0039217162560839e-16,
+            "velocityX": 3.561494902019361,
+            "velocityY": -1.27529915956289,
             "timestamp": 2.1201943567565973
         },
         {
             "x": 6.749686550285633,
             "y": 1.1201263958508503,
-            "heading": 3.14,
-            "angularVelocity": 5.765266318677121e-16,
-            "velocityX": 3.561494902019357,
+            "heading": 6.889954024340196e-17,
+            "angularVelocity": -2.0366454499176497e-16,
+            "velocityX": 3.5614949020193576,
             "velocityY": -1.2752991595628989,
             "timestamp": 2.153875153671664
         },
         {
             "x": 6.869640536794461,
             "y": 1.077173303851719,
-            "heading": 3.14,
-            "angularVelocity": 5.94200130731494e-16,
+            "heading": 8.054846416919904e-17,
+            "angularVelocity": 3.458624792985843e-16,
             "velocityX": 3.5614949020154905,
-            "velocityY": -1.2752991595610093,
+            "velocityY": -1.275299159561009,
             "timestamp": 2.187555950586731
         },
         {
             "x": 6.985406128017608,
             "y": 1.0357199913369377,
-            "heading": 3.14,
-            "angularVelocity": -5.216843428237177e-16,
-            "velocityX": 3.437139314579594,
+            "heading": 6.97905590140722e-17,
+            "angularVelocity": -3.1940767538205783e-16,
+            "velocityX": 3.437139314579595,
             "velocityY": -1.2307699434580792,
             "timestamp": 2.2212367475017976
         },
         {
             "x": 7.092266700241795,
-            "y": 0.9974553856875887,
-            "heading": 3.14,
-            "angularVelocity": -4.51325473252053e-16,
-            "velocityX": 3.172744768885923,
+            "y": 0.9974553856875888,
+            "heading": 5.947830002893604e-17,
+            "angularVelocity": -3.0617621445548207e-16,
+            "velocityX": 3.172744768885924,
             "velocityY": -1.1360956139440452,
             "timestamp": 2.2549175444168643
         },
         {
-            "x": 7.190222235939472,
+            "x": 7.190222235939473,
             "y": 0.9623794931799317,
-            "heading": 3.14,
-            "angularVelocity": -3.932803198593372e-16,
-            "velocityX": 2.908349702790401,
+            "heading": 5.044588900647289e-17,
+            "angularVelocity": -2.6817687744625345e-16,
+            "velocityX": 2.9083497027904013,
             "velocityY": -1.0414210980846788,
             "timestamp": 2.288598341331931
         },
         {
             "x": 7.279272729268112,
-            "y": 0.9304923159060571,
-            "heading": 3.14,
-            "angularVelocity": -3.6674368533996665e-16,
-            "velocityX": 2.643954463227235,
-            "velocityY": -0.9467465201100715,
+            "y": 0.930492315906057,
+            "heading": 4.27627289839913e-17,
+            "angularVelocity": -2.281169282784766e-16,
+            "velocityX": 2.6439544632272356,
+            "velocityY": -0.9467465201100714,
             "timestamp": 2.322279138246998
         },
         {
             "x": 7.35941817730645,
-            "y": 0.9017938549120107,
-            "heading": 3.14,
-            "angularVelocity": -3.61350987503525e-16,
-            "velocityX": 2.3795591369302214,
+            "y": 0.9017938549120106,
+            "heading": 3.626946964953948e-17,
+            "angularVelocity": -1.9278817213060433e-16,
+            "velocityX": 2.379559136930222,
             "velocityY": -0.8520719110778341,
             "timestamp": 2.3559599351620646
         },
         {
             "x": 7.430658578301727,
             "y": 0.8762841108254199,
-            "heading": 3.14,
-            "angularVelocity": -3.11296740830968e-16,
-            "velocityX": 2.115163758592898,
+            "heading": 2.9442071838197796e-17,
+            "angularVelocity": -2.0270891340137855e-16,
+            "velocityX": 2.1151637585928986,
             "velocityY": -0.7573972834110186,
             "timestamp": 2.3896407320771313
         },
         {
             "x": 7.492993931085436,
             "y": 0.8539630840647031,
-            "heading": 3.14,
-            "angularVelocity": -3.0797715285651212e-16,
-            "velocityX": 1.8507683455620387,
-            "velocityY": -0.6627226433211519,
+            "heading": 2.3413209096667504e-17,
+            "angularVelocity": -1.790000012026981e-16,
+            "velocityX": 1.850768345562039,
+            "velocityY": -0.662722643321152,
             "timestamp": 2.423321528992198
         },
         {
             "x": 7.546424234822929,
             "y": 0.8348307749287305,
-            "heading": 3.14,
-            "angularVelocity": -2.384214973522538e-16,
-            "velocityX": 1.5863729077500837,
-            "velocityY": -0.5680479943576783,
+            "heading": 1.7731266545962487e-17,
+            "angularVelocity": -1.6869976443484408e-16,
+            "velocityX": 1.5863729077500839,
+            "velocityY": -0.5680479943576784,
             "timestamp": 2.457002325907265
         },
         {
             "x": 7.590949488888224,
             "y": 0.8188871836416546,
-            "heading": 3.14,
-            "angularVelocity": -1.6873896303460275e-16,
-            "velocityX": 1.3219774513523088,
-            "velocityY": -0.4733733387389998,
+            "heading": 1.2363765500283435e-17,
+            "angularVelocity": -1.593638362048541e-16,
+            "velocityX": 1.321977451352309,
+            "velocityY": -0.4733733387389999,
             "timestamp": 2.4906831228223316
         },
         {
             "x": 7.626569692794441,
             "y": 0.8061323103778162,
-            "heading": 3.14,
-            "angularVelocity": -1.2706181170586285e-16,
-            "velocityX": 1.0575819804988977,
-            "velocityY": -0.3786986779440515,
+            "heading": 7.88828312817125e-18,
+            "angularVelocity": -1.3287934794211324e-16,
+            "velocityX": 1.057581980498898,
+            "velocityY": -0.37869867794405154,
             "timestamp": 2.5243639197373984
         },
         {
             "x": 7.653284846152079,
             "y": 0.7965661552766882,
-            "heading": 3.14,
-            "angularVelocity": -1.0566459548829111e-16,
-            "velocityX": 0.7931864980809785,
-            "velocityY": -0.28402401300808755,
+            "heading": 4.236479358871306e-18,
+            "angularVelocity": -1.0842391112822455e-16,
+            "velocityX": 0.7931864980809786,
+            "velocityY": -0.28402401300808766,
             "timestamp": 2.558044716652465
         },
         {
             "x": 7.671094948642455,
             "y": 0.7901887184523844,
-            "heading": 3.14,
-            "angularVelocity": -6.428789472113484e-17,
-            "velocityX": 0.5287910062011897,
-            "velocityY": -0.18934934468402026,
+            "heading": 1.249730071610374e-18,
+            "angularVelocity": -8.867810508445172e-17,
+            "velocityX": 0.5287910062011898,
+            "velocityY": -0.18934934468402032,
             "timestamp": 2.591725513567532
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": -8.910267594003032e-17,
-            "velocityX": 0.26439550643651,
-            "velocityY": -0.09467467353653364,
+            "heading": -2.906959217388984e-34,
+            "angularVelocity": -3.7105121777105e-17,
+            "velocityX": 0.26439550643651005,
+            "velocityY": -0.09467467353653367,
             "timestamp": 2.6254063104825986
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": -3.385083394775927e-32,
-            "velocityX": 1.13191163707898e-33,
-            "velocityY": 3.1068609037049197e-32,
+            "heading": -4.078160951605287e-34,
+            "angularVelocity": -3.477050502256718e-33,
+            "velocityX": 7.685636324015334e-33,
+            "velocityY": -2.3747808746440768e-32,
             "timestamp": 2.6590871073976654
         }
     ]
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.1.traj
index e00feed7..641af2c0 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.1.traj	
@@ -3,110 +3,110 @@
         {
             "x": 1.35,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -7.821039013033948e-33,
-            "velocityX": 0,
-            "velocityY": 0,
+            "heading": 0,
+            "angularVelocity": -1.3982338019676807e-32,
+            "velocityX": 1.9225496754980614e-32,
+            "velocityY": -8.567103847838605e-38,
             "timestamp": 0
         },
         {
-            "x": 1.3816658469394465,
-            "y": 5.599900983994057,
-            "heading": 3.1530893621447014,
-            "angularVelocity": 0.16724918062405764,
-            "velocityX": 0.4046100104681557,
-            "velocityY": 0.5098343834730725,
-            "timestamp": 0.07826263839297289
+            "x": 1.3816658535274804,
+            "y": 5.599900984360149,
+            "heading": 0.013082876743969601,
+            "angularVelocity": 0.16716651866961724,
+            "velocityX": 0.4046105912700521,
+            "velocityY": 0.509835013927756,
+            "timestamp": 0.07826254233257365
         },
         {
-            "x": 1.444997693090777,
-            "y": 5.6797028720542775,
-            "heading": 3.179265627435073,
-            "angularVelocity": 0.3344669414150786,
-            "velocityX": 0.8092219665957109,
-            "velocityY": 1.0196677456683545,
-            "timestamp": 0.15652527678594577
+            "x": 1.4449977124682947,
+            "y": 5.679702872674784,
+            "heading": 0.039246215534929935,
+            "angularVelocity": 0.3343021835373132,
+            "velocityX": 0.8092231232623148,
+            "velocityY": 1.0196690004717623,
+            "timestamp": 0.1565250846651473
         },
         {
-            "x": 1.539995766815641,
-            "y": 5.799405527661274,
-            "heading": 3.2185261574836668,
-            "angularVelocity": 0.5016509902395458,
-            "velocityX": 1.213836840611724,
-            "velocityY": 1.5294993634886844,
-            "timestamp": 0.23478791517891867
+            "x": 1.5399958043003432,
+            "y": 5.799405527723277,
+            "heading": 0.07848745324353076,
+            "angularVelocity": 0.501405098007769,
+            "velocityX": 1.2138385618545031,
+            "velocityY": 1.5295012336785618,
+            "timestamp": 0.23478762699772096
         },
         {
-            "x": 1.6666603529158708,
-            "y": 5.959008729778703,
-            "heading": 3.270870260258294,
-            "angularVelocity": 0.6688261966303347,
-            "velocityX": 1.6184553536800128,
-            "velocityY": 2.039328157019548,
-            "timestamp": 0.31305055357189154
+            "x": 1.6666604122853452,
+            "y": 5.959008727776544,
+            "heading": 0.1308059995871385,
+            "angularVelocity": 0.6685004701391144,
+            "velocityX": 1.6184576198246376,
+            "velocityY": 2.039330633740969,
+            "timestamp": 0.3130501693302946
         },
         {
-            "x": 1.8249917488860277,
-            "y": 6.158512141354908,
-            "heading": 3.336302010504542,
-            "angularVelocity": 0.8360534680379705,
-            "velocityX": 2.0230776679817777,
-            "velocityY": 2.5491526438766083,
-            "timestamp": 0.3913131919648644
+            "x": 1.8249918316073899,
+            "y": 6.158512135429991,
+            "heading": 0.19620604644996878,
+            "angularVelocity": 0.8356494040905814,
+            "velocityX": 2.023080449510844,
+            "velocityY": 2.5491557226145924,
+            "timestamp": 0.3913127116628683
         },
         {
-            "x": 1.9833277736824528,
-            "y": 6.358008431463026,
-            "heading": 3.401912848694198,
-            "angularVelocity": 0.8383417622622024,
-            "velocityX": 2.023136812758436,
-            "velocityY": 2.549061649396556,
-            "timestamp": 0.4695758303578373
+            "x": 1.9833278311922586,
+            "y": 6.358008426099132,
+            "heading": 0.26178707056088246,
+            "angularVelocity": 0.8379618417228061,
+            "velocityX": 2.023138973840461,
+            "velocityY": 2.549064785314409,
+            "timestamp": 0.469575253995442
         },
         {
-            "x": 2.1099966456081427,
-            "y": 6.517605183327805,
-            "heading": 3.454415313467435,
-            "angularVelocity": 0.6708496653232114,
-            "velocityX": 1.618510115767619,
-            "velocityY": 2.0392457389873657,
-            "timestamp": 0.5478384687508102
+            "x": 2.109996681019138,
+            "y": 6.517605179473628,
+            "heading": 0.31426571552794874,
+            "angularVelocity": 0.6705461310477281,
+            "velocityX": 1.618511819979031,
+            "velocityY": 2.039248261272889,
+            "timestamp": 0.5478377963280157
         },
         {
-            "x": 2.20499831408803,
-            "y": 6.637302614330095,
-            "heading": 3.493799088289958,
-            "angularVelocity": 0.5032257489808813,
-            "velocityX": 1.2138827725544437,
-            "velocityY": 1.5294326061595858,
-            "timestamp": 0.6261011071437831
+            "x": 2.204998332006287,
+            "y": 6.637302612224517,
+            "heading": 0.35363163522076196,
+            "angularVelocity": 0.5029982226430836,
+            "velocityX": 1.2138840389754155,
+            "velocityY": 1.5294345057465644,
+            "timestamp": 0.6261003386605893
         },
         {
-            "x": 2.268332767215697,
-            "y": 6.717100865980731,
-            "heading": 3.520056877037348,
-            "angularVelocity": 0.3355086064891452,
-            "velocityX": 0.8092552772071353,
-            "velocityY": 1.0196212814849352,
-            "timestamp": 0.704363745536756
+            "x": 2.268332773183591,
+            "y": 6.717100865272008,
+            "heading": 0.3798775217934225,
+            "angularVelocity": 0.3353569382033373,
+            "velocityX": 0.8092561178011136,
+            "velocityY": 1.0196225508288703,
+            "timestamp": 0.704362880993163
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 0.16774837153225197,
-            "velocityX": 0.4046277180855394,
-            "velocityY": 0.5098107454405937,
-            "timestamp": 0.7826263839297288
+            "heading": 0.393,
+            "angularVelocity": 0.1676725265429579,
+            "velocityX": 0.40462813847574725,
+            "velocityY": 0.509811380244222,
+            "timestamp": 0.7826254233257367
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 3.497795149569243e-31,
-            "velocityX": 5.486835585676403e-32,
-            "velocityY": -3.2429311821904986e-32,
-            "timestamp": 0.8608890223227017
+            "heading": 0.393,
+            "angularVelocity": 9.092584352802723e-32,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0.8608879656583104
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.traj
index e00feed7..641af2c0 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.traj	
@@ -3,110 +3,110 @@
         {
             "x": 1.35,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -7.821039013033948e-33,
-            "velocityX": 0,
-            "velocityY": 0,
+            "heading": 0,
+            "angularVelocity": -1.3982338019676807e-32,
+            "velocityX": 1.9225496754980614e-32,
+            "velocityY": -8.567103847838605e-38,
             "timestamp": 0
         },
         {
-            "x": 1.3816658469394465,
-            "y": 5.599900983994057,
-            "heading": 3.1530893621447014,
-            "angularVelocity": 0.16724918062405764,
-            "velocityX": 0.4046100104681557,
-            "velocityY": 0.5098343834730725,
-            "timestamp": 0.07826263839297289
+            "x": 1.3816658535274804,
+            "y": 5.599900984360149,
+            "heading": 0.013082876743969601,
+            "angularVelocity": 0.16716651866961724,
+            "velocityX": 0.4046105912700521,
+            "velocityY": 0.509835013927756,
+            "timestamp": 0.07826254233257365
         },
         {
-            "x": 1.444997693090777,
-            "y": 5.6797028720542775,
-            "heading": 3.179265627435073,
-            "angularVelocity": 0.3344669414150786,
-            "velocityX": 0.8092219665957109,
-            "velocityY": 1.0196677456683545,
-            "timestamp": 0.15652527678594577
+            "x": 1.4449977124682947,
+            "y": 5.679702872674784,
+            "heading": 0.039246215534929935,
+            "angularVelocity": 0.3343021835373132,
+            "velocityX": 0.8092231232623148,
+            "velocityY": 1.0196690004717623,
+            "timestamp": 0.1565250846651473
         },
         {
-            "x": 1.539995766815641,
-            "y": 5.799405527661274,
-            "heading": 3.2185261574836668,
-            "angularVelocity": 0.5016509902395458,
-            "velocityX": 1.213836840611724,
-            "velocityY": 1.5294993634886844,
-            "timestamp": 0.23478791517891867
+            "x": 1.5399958043003432,
+            "y": 5.799405527723277,
+            "heading": 0.07848745324353076,
+            "angularVelocity": 0.501405098007769,
+            "velocityX": 1.2138385618545031,
+            "velocityY": 1.5295012336785618,
+            "timestamp": 0.23478762699772096
         },
         {
-            "x": 1.6666603529158708,
-            "y": 5.959008729778703,
-            "heading": 3.270870260258294,
-            "angularVelocity": 0.6688261966303347,
-            "velocityX": 1.6184553536800128,
-            "velocityY": 2.039328157019548,
-            "timestamp": 0.31305055357189154
+            "x": 1.6666604122853452,
+            "y": 5.959008727776544,
+            "heading": 0.1308059995871385,
+            "angularVelocity": 0.6685004701391144,
+            "velocityX": 1.6184576198246376,
+            "velocityY": 2.039330633740969,
+            "timestamp": 0.3130501693302946
         },
         {
-            "x": 1.8249917488860277,
-            "y": 6.158512141354908,
-            "heading": 3.336302010504542,
-            "angularVelocity": 0.8360534680379705,
-            "velocityX": 2.0230776679817777,
-            "velocityY": 2.5491526438766083,
-            "timestamp": 0.3913131919648644
+            "x": 1.8249918316073899,
+            "y": 6.158512135429991,
+            "heading": 0.19620604644996878,
+            "angularVelocity": 0.8356494040905814,
+            "velocityX": 2.023080449510844,
+            "velocityY": 2.5491557226145924,
+            "timestamp": 0.3913127116628683
         },
         {
-            "x": 1.9833277736824528,
-            "y": 6.358008431463026,
-            "heading": 3.401912848694198,
-            "angularVelocity": 0.8383417622622024,
-            "velocityX": 2.023136812758436,
-            "velocityY": 2.549061649396556,
-            "timestamp": 0.4695758303578373
+            "x": 1.9833278311922586,
+            "y": 6.358008426099132,
+            "heading": 0.26178707056088246,
+            "angularVelocity": 0.8379618417228061,
+            "velocityX": 2.023138973840461,
+            "velocityY": 2.549064785314409,
+            "timestamp": 0.469575253995442
         },
         {
-            "x": 2.1099966456081427,
-            "y": 6.517605183327805,
-            "heading": 3.454415313467435,
-            "angularVelocity": 0.6708496653232114,
-            "velocityX": 1.618510115767619,
-            "velocityY": 2.0392457389873657,
-            "timestamp": 0.5478384687508102
+            "x": 2.109996681019138,
+            "y": 6.517605179473628,
+            "heading": 0.31426571552794874,
+            "angularVelocity": 0.6705461310477281,
+            "velocityX": 1.618511819979031,
+            "velocityY": 2.039248261272889,
+            "timestamp": 0.5478377963280157
         },
         {
-            "x": 2.20499831408803,
-            "y": 6.637302614330095,
-            "heading": 3.493799088289958,
-            "angularVelocity": 0.5032257489808813,
-            "velocityX": 1.2138827725544437,
-            "velocityY": 1.5294326061595858,
-            "timestamp": 0.6261011071437831
+            "x": 2.204998332006287,
+            "y": 6.637302612224517,
+            "heading": 0.35363163522076196,
+            "angularVelocity": 0.5029982226430836,
+            "velocityX": 1.2138840389754155,
+            "velocityY": 1.5294345057465644,
+            "timestamp": 0.6261003386605893
         },
         {
-            "x": 2.268332767215697,
-            "y": 6.717100865980731,
-            "heading": 3.520056877037348,
-            "angularVelocity": 0.3355086064891452,
-            "velocityX": 0.8092552772071353,
-            "velocityY": 1.0196212814849352,
-            "timestamp": 0.704363745536756
+            "x": 2.268332773183591,
+            "y": 6.717100865272008,
+            "heading": 0.3798775217934225,
+            "angularVelocity": 0.3353569382033373,
+            "velocityX": 0.8092561178011136,
+            "velocityY": 1.0196225508288703,
+            "timestamp": 0.704362880993163
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 0.16774837153225197,
-            "velocityX": 0.4046277180855394,
-            "velocityY": 0.5098107454405937,
-            "timestamp": 0.7826263839297288
+            "heading": 0.393,
+            "angularVelocity": 0.1676725265429579,
+            "velocityX": 0.40462813847574725,
+            "velocityY": 0.509811380244222,
+            "timestamp": 0.7826254233257367
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 3.497795149569243e-31,
-            "velocityX": 5.486835585676403e-32,
-            "velocityY": -3.2429311821904986e-32,
-            "timestamp": 0.8608890223227017
+            "heading": 0.393,
+            "angularVelocity": 9.092584352802723e-32,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0.8608879656583104
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.1.traj
index e5085a74..63887ddf 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.1.traj	
@@ -3,74 +3,74 @@
         {
             "x": 1.35,
             "y": 5.56,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
+            "velocityX": -1.1658790467474463e-32,
+            "velocityY": 2.7665844766493316e-30,
             "timestamp": 0
         },
         {
-            "x": 1.425000004529508,
-            "y": 5.5600000086998405,
-            "heading": 3.139999994805454,
-            "angularVelocity": -5.477109235911092e-8,
-            "velocityX": 0.7907971475606559,
-            "velocityY": 9.173079093903646e-8,
-            "timestamp": 0.09484101550044495
+            "x": 1.4250000045295077,
+            "y": 5.560000008699219,
+            "heading": -5.237332222253062e-9,
+            "angularVelocity": -5.5222228614505115e-8,
+            "velocityX": 0.7907971475606554,
+            "velocityY": 9.172423930862072e-8,
+            "timestamp": 0.09484101550044491
         },
         {
-            "x": 1.5750000090590133,
-            "y": 5.560000023679525,
-            "heading": 3.1399999868961004,
-            "angularVelocity": -8.339591750663206e-8,
-            "velocityX": 1.5815942473623312,
-            "velocityY": 1.5794521377308047e-7,
-            "timestamp": 0.1896820310008899
+            "x": 1.575000009059013,
+            "y": 5.560000023677869,
+            "heading": -1.3214169082837694e-8,
+            "angularVelocity": -8.41074599041897e-8,
+            "velocityX": 1.5815942473623306,
+            "velocityY": 1.579343082639882e-7,
+            "timestamp": 0.18968203100088982
         },
         {
-            "x": 1.7999999999999887,
-            "y": 5.5600000423644245,
-            "heading": 3.1399999785762156,
-            "angularVelocity": -8.772454789703223e-8,
-            "velocityX": 2.372391203887096,
-            "velocityY": 1.9701287329918457e-7,
-            "timestamp": 0.28452304650133486
+            "x": 1.7999999999999883,
+            "y": 5.560000042361517,
+            "heading": -2.1609400534628168e-8,
+            "angularVelocity": -8.851899686654573e-8,
+            "velocityX": 2.372391203887095,
+            "velocityY": 1.9699965815991792e-7,
+            "timestamp": 0.28452304650133475
         },
         {
-            "x": 2.0249999909409757,
-            "y": 5.560000024808709,
-            "heading": 3.1399999889274675,
-            "angularVelocity": 1.0914320422483106e-7,
-            "velocityX": 2.372391203887217,
-            "velocityY": -1.851067857722626e-7,
-            "timestamp": 0.3793640620017798
+            "x": 2.0249999909409753,
+            "y": 5.560000024806983,
+            "heading": -1.1174048484706425e-8,
+            "angularVelocity": 1.1002994951426502e-7,
+            "velocityX": 2.372391203887216,
+            "velocityY": -1.8509433303171823e-7,
+            "timestamp": 0.37936406200177963
         },
         {
-            "x": 2.174999995470489,
-            "y": 5.560000009522542,
-            "heading": 3.1399999962990623,
-            "angularVelocity": 7.772581212467168e-8,
-            "velocityX": 1.5815942473624114,
-            "velocityY": -1.6117675426775267e-7,
-            "timestamp": 0.47420507750222474
+            "x": 2.1749999954704884,
+            "y": 5.560000009521861,
+            "heading": -3.737396801552092e-9,
+            "angularVelocity": 7.841176829375941e-8,
+            "velocityX": 1.581594247362411,
+            "velocityY": -1.6116572709153717e-7,
+            "timestamp": 0.4742050775022245
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 3.902254149386954e-8,
-            "velocityX": 0.7907971475606953,
-            "velocityY": -1.0040532392087884e-7,
-            "timestamp": 0.5690460930026697
+            "heading": 2.347281627650706e-31,
+            "angularVelocity": 3.940696757721575e-8,
+            "velocityX": 0.7907971475606952,
+            "velocityY": -1.0039814487748917e-7,
+            "timestamp": 0.5690460930026694
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 1.1924381690495433e-31,
+            "heading": 0,
+            "angularVelocity": -9.04607706846012e-36,
             "velocityX": 0,
-            "velocityY": -3.802893138506909e-33,
-            "timestamp": 0.6638871085031147
+            "velocityY": -2.7063013184679635e-30,
+            "timestamp": 0.6638871085031143
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.traj
index e5085a74..63887ddf 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.traj	
@@ -3,74 +3,74 @@
         {
             "x": 1.35,
             "y": 5.56,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
+            "velocityX": -1.1658790467474463e-32,
+            "velocityY": 2.7665844766493316e-30,
             "timestamp": 0
         },
         {
-            "x": 1.425000004529508,
-            "y": 5.5600000086998405,
-            "heading": 3.139999994805454,
-            "angularVelocity": -5.477109235911092e-8,
-            "velocityX": 0.7907971475606559,
-            "velocityY": 9.173079093903646e-8,
-            "timestamp": 0.09484101550044495
+            "x": 1.4250000045295077,
+            "y": 5.560000008699219,
+            "heading": -5.237332222253062e-9,
+            "angularVelocity": -5.5222228614505115e-8,
+            "velocityX": 0.7907971475606554,
+            "velocityY": 9.172423930862072e-8,
+            "timestamp": 0.09484101550044491
         },
         {
-            "x": 1.5750000090590133,
-            "y": 5.560000023679525,
-            "heading": 3.1399999868961004,
-            "angularVelocity": -8.339591750663206e-8,
-            "velocityX": 1.5815942473623312,
-            "velocityY": 1.5794521377308047e-7,
-            "timestamp": 0.1896820310008899
+            "x": 1.575000009059013,
+            "y": 5.560000023677869,
+            "heading": -1.3214169082837694e-8,
+            "angularVelocity": -8.41074599041897e-8,
+            "velocityX": 1.5815942473623306,
+            "velocityY": 1.579343082639882e-7,
+            "timestamp": 0.18968203100088982
         },
         {
-            "x": 1.7999999999999887,
-            "y": 5.5600000423644245,
-            "heading": 3.1399999785762156,
-            "angularVelocity": -8.772454789703223e-8,
-            "velocityX": 2.372391203887096,
-            "velocityY": 1.9701287329918457e-7,
-            "timestamp": 0.28452304650133486
+            "x": 1.7999999999999883,
+            "y": 5.560000042361517,
+            "heading": -2.1609400534628168e-8,
+            "angularVelocity": -8.851899686654573e-8,
+            "velocityX": 2.372391203887095,
+            "velocityY": 1.9699965815991792e-7,
+            "timestamp": 0.28452304650133475
         },
         {
-            "x": 2.0249999909409757,
-            "y": 5.560000024808709,
-            "heading": 3.1399999889274675,
-            "angularVelocity": 1.0914320422483106e-7,
-            "velocityX": 2.372391203887217,
-            "velocityY": -1.851067857722626e-7,
-            "timestamp": 0.3793640620017798
+            "x": 2.0249999909409753,
+            "y": 5.560000024806983,
+            "heading": -1.1174048484706425e-8,
+            "angularVelocity": 1.1002994951426502e-7,
+            "velocityX": 2.372391203887216,
+            "velocityY": -1.8509433303171823e-7,
+            "timestamp": 0.37936406200177963
         },
         {
-            "x": 2.174999995470489,
-            "y": 5.560000009522542,
-            "heading": 3.1399999962990623,
-            "angularVelocity": 7.772581212467168e-8,
-            "velocityX": 1.5815942473624114,
-            "velocityY": -1.6117675426775267e-7,
-            "timestamp": 0.47420507750222474
+            "x": 2.1749999954704884,
+            "y": 5.560000009521861,
+            "heading": -3.737396801552092e-9,
+            "angularVelocity": 7.841176829375941e-8,
+            "velocityX": 1.581594247362411,
+            "velocityY": -1.6116572709153717e-7,
+            "timestamp": 0.4742050775022245
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 3.902254149386954e-8,
-            "velocityX": 0.7907971475606953,
-            "velocityY": -1.0040532392087884e-7,
-            "timestamp": 0.5690460930026697
+            "heading": 2.347281627650706e-31,
+            "angularVelocity": 3.940696757721575e-8,
+            "velocityX": 0.7907971475606952,
+            "velocityY": -1.0039814487748917e-7,
+            "timestamp": 0.5690460930026694
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 1.1924381690495433e-31,
+            "heading": 0,
+            "angularVelocity": -9.04607706846012e-36,
             "velocityX": 0,
-            "velocityY": -3.802893138506909e-33,
-            "timestamp": 0.6638871085031147
+            "velocityY": -2.7063013184679635e-30,
+            "timestamp": 0.6638871085031143
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.1.traj
index 93f4af7e..fcfc6794 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.1.traj	
@@ -3,110 +3,110 @@
         {
             "x": 1.35,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 3.0431878595519687e-30,
-            "velocityX": -9.811045254051387e-32,
-            "velocityY": 1.5487452756550695e-31,
+            "heading": -1.4926991504029801e-31,
+            "angularVelocity": 3.0130798970250824e-30,
+            "velocityX": -6.359546450907426e-32,
+            "velocityY": -1.6682795346112646e-37,
             "timestamp": 0
         },
         {
-            "x": 1.3816658692579147,
-            "y": 5.519999048484489,
-            "heading": 3.127016326666322,
-            "angularVelocity": -0.16577538674563452,
-            "velocityX": 0.4043094421708381,
-            "velocityY": -0.5107316733298447,
-            "timestamp": 0.07832087494146221
+            "x": 1.381665864471773,
+            "y": 5.519999051141232,
+            "heading": -0.0129840210078559,
+            "angularVelocity": -0.16577982445307443,
+            "velocityX": 0.40430937766578573,
+            "velocityY": -0.5107316351191263,
+            "timestamp": 0.07832087559925201
         },
         {
-            "x": 1.4449977551622273,
-            "y": 5.439997223821588,
-            "heading": 3.1010513131375346,
-            "angularVelocity": -0.33152098400576074,
-            "velocityX": 0.8086207661960841,
-            "velocityY": -1.0214623460564694,
-            "timestamp": 0.15664174988292442
+            "x": 1.4449977408289922,
+            "y": 5.439997231153384,
+            "heading": -0.03894969123495126,
+            "angularVelocity": -0.3315293659375504,
+            "velocityX": 0.8086206375075861,
+            "velocityY": -1.0214622777865288,
+            "timestamp": 0.15664175119850401
         },
         {
-            "x": 1.5399958776695735,
-            "y": 5.319994658705983,
-            "heading": 3.0621074441034875,
-            "angularVelocity": -0.49723485677543305,
-            "velocityX": 1.2129348986250355,
-            "velocityY": -1.5321913245389087,
-            "timestamp": 0.23496262482438662
+            "x": 1.5399958493613461,
+            "y": 5.319994671739442,
+            "heading": -0.07789445681060216,
+            "angularVelocity": -0.4972462996317751,
+            "velocityX": 1.212934710005481,
+            "velocityY": -1.532191238871801,
+            "timestamp": 0.23496262679775604
         },
         {
-            "x": 1.6666605105444467,
-            "y": 5.159991566646187,
-            "heading": 3.010185320857369,
-            "angularVelocity": -0.6629410522408352,
-            "velocityX": 1.6172525264757869,
-            "velocityY": -2.042917576947189,
-            "timestamp": 0.31328349976584885
+            "x": 1.6666604646819347,
+            "y": 5.1599915852165825,
+            "heading": -0.12981761618342352,
+            "angularVelocity": -0.6629542759263352,
+            "velocityX": 1.6172522887601493,
+            "velocityY": -2.042917489093896,
+            "timestamp": 0.31328350239700803
         },
         {
-            "x": 1.8249919396490273,
-            "y": 4.959988272167279,
-            "heading": 2.9452809657766084,
-            "angularVelocity": -0.8286980339439337,
-            "velocityX": 2.021573804211457,
-            "velocityY": -2.5536396858231227,
-            "timestamp": 0.3916043747073111
+            "x": 1.8249918742420412,
+            "y": 4.95998829498017,
+            "heading": -0.19472302000870664,
+            "angularVelocity": -0.8287114173414627,
+            "velocityX": 2.0215735376893504,
+            "velocityY": -2.5536396102078434,
+            "timestamp": 0.39160437799626
         },
         {
-            "x": 1.9833278878841576,
-            "y": 4.759991849616898,
-            "heading": 2.880203589095056,
-            "angularVelocity": -0.8309071716856798,
-            "velocityX": 2.021631504416561,
-            "velocityY": -2.553551945121407,
-            "timestamp": 0.4699252496487733
+            "x": 1.9833278418494473,
+            "y": 4.759991864222915,
+            "heading": -0.25979913954005024,
+            "angularVelocity": -0.8308911134284025,
+            "velocityX": 2.0216317347825643,
+            "velocityY": -2.5535520284602864,
+            "timestamp": 0.469925253595512
         },
         {
-            "x": 2.109996705593866,
-            "y": 4.599994984440838,
-            "heading": 2.828128420495832,
-            "angularVelocity": -0.6648951334896099,
-            "velocityX": 1.6173059583971898,
-            "velocityY": -2.0428380721696886,
-            "timestamp": 0.5482461245902355
+            "x": 2.1099966770232617,
+            "y": 4.599994992856252,
+            "heading": -0.31187323961880925,
+            "angularVelocity": -0.6648814850489074,
+            "velocityX": 1.6173061677954905,
+            "velocityY": -2.0428381340541457,
+            "timestamp": 0.548246129194764
         },
         {
-            "x": 2.2049983397447894,
-            "y": 4.479997467084651,
-            "heading": 2.789065384524658,
-            "angularVelocity": -0.4987563788117872,
-            "velocityX": 1.212979735248467,
-            "velocityY": -1.5321268748066883,
-            "timestamp": 0.6265669995316977
+            "x": 2.204998325241535,
+            "y": 4.479997471163016,
+            "heading": -0.3509354488627182,
+            "angularVelocity": -0.49874581897910997,
+            "velocityX": 1.212979904672827,
+            "velocityY": -1.5321269173142713,
+            "timestamp": 0.6265670047940161
         },
         {
-            "x": 2.268332774342349,
-            "y": 4.3999991596507995,
-            "heading": 2.7630214971574416,
-            "angularVelocity": -0.3325280442370214,
-            "velocityX": 0.8086533078811499,
-            "velocityY": -1.0214174381177892,
-            "timestamp": 0.7048878744731599
+            "x": 2.2683327695206965,
+            "y": 4.399999160989759,
+            "heading": -0.37697877943637265,
+            "angularVelocity": -0.33252093231071334,
+            "velocityX": 0.8086534247041309,
+            "velocityY": -1.0214174645159662,
+            "timestamp": 0.7048878803932681
         },
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": -0.1662583208777211,
-            "velocityX": 0.4043267606665432,
-            "velocityY": -0.510708794822514,
-            "timestamp": 0.7832087494146222
+            "heading": -0.39,
+            "angularVelocity": -0.16625478793487553,
+            "velocityX": 0.40432681883354604,
+            "velocityY": -0.5107088076290647,
+            "timestamp": 0.7832087559925202
         },
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 6.867412911994789e-31,
-            "velocityX": 0,
-            "velocityY": 9.317443122449736e-32,
-            "timestamp": 0.8615296243560844
+            "heading": -0.39,
+            "angularVelocity": 0,
+            "velocityX": 7.259744325812885e-33,
+            "velocityY": -1.1119528318776295e-29,
+            "timestamp": 0.8615296315917722
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.traj
index 93f4af7e..fcfc6794 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.traj	
@@ -3,110 +3,110 @@
         {
             "x": 1.35,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 3.0431878595519687e-30,
-            "velocityX": -9.811045254051387e-32,
-            "velocityY": 1.5487452756550695e-31,
+            "heading": -1.4926991504029801e-31,
+            "angularVelocity": 3.0130798970250824e-30,
+            "velocityX": -6.359546450907426e-32,
+            "velocityY": -1.6682795346112646e-37,
             "timestamp": 0
         },
         {
-            "x": 1.3816658692579147,
-            "y": 5.519999048484489,
-            "heading": 3.127016326666322,
-            "angularVelocity": -0.16577538674563452,
-            "velocityX": 0.4043094421708381,
-            "velocityY": -0.5107316733298447,
-            "timestamp": 0.07832087494146221
+            "x": 1.381665864471773,
+            "y": 5.519999051141232,
+            "heading": -0.0129840210078559,
+            "angularVelocity": -0.16577982445307443,
+            "velocityX": 0.40430937766578573,
+            "velocityY": -0.5107316351191263,
+            "timestamp": 0.07832087559925201
         },
         {
-            "x": 1.4449977551622273,
-            "y": 5.439997223821588,
-            "heading": 3.1010513131375346,
-            "angularVelocity": -0.33152098400576074,
-            "velocityX": 0.8086207661960841,
-            "velocityY": -1.0214623460564694,
-            "timestamp": 0.15664174988292442
+            "x": 1.4449977408289922,
+            "y": 5.439997231153384,
+            "heading": -0.03894969123495126,
+            "angularVelocity": -0.3315293659375504,
+            "velocityX": 0.8086206375075861,
+            "velocityY": -1.0214622777865288,
+            "timestamp": 0.15664175119850401
         },
         {
-            "x": 1.5399958776695735,
-            "y": 5.319994658705983,
-            "heading": 3.0621074441034875,
-            "angularVelocity": -0.49723485677543305,
-            "velocityX": 1.2129348986250355,
-            "velocityY": -1.5321913245389087,
-            "timestamp": 0.23496262482438662
+            "x": 1.5399958493613461,
+            "y": 5.319994671739442,
+            "heading": -0.07789445681060216,
+            "angularVelocity": -0.4972462996317751,
+            "velocityX": 1.212934710005481,
+            "velocityY": -1.532191238871801,
+            "timestamp": 0.23496262679775604
         },
         {
-            "x": 1.6666605105444467,
-            "y": 5.159991566646187,
-            "heading": 3.010185320857369,
-            "angularVelocity": -0.6629410522408352,
-            "velocityX": 1.6172525264757869,
-            "velocityY": -2.042917576947189,
-            "timestamp": 0.31328349976584885
+            "x": 1.6666604646819347,
+            "y": 5.1599915852165825,
+            "heading": -0.12981761618342352,
+            "angularVelocity": -0.6629542759263352,
+            "velocityX": 1.6172522887601493,
+            "velocityY": -2.042917489093896,
+            "timestamp": 0.31328350239700803
         },
         {
-            "x": 1.8249919396490273,
-            "y": 4.959988272167279,
-            "heading": 2.9452809657766084,
-            "angularVelocity": -0.8286980339439337,
-            "velocityX": 2.021573804211457,
-            "velocityY": -2.5536396858231227,
-            "timestamp": 0.3916043747073111
+            "x": 1.8249918742420412,
+            "y": 4.95998829498017,
+            "heading": -0.19472302000870664,
+            "angularVelocity": -0.8287114173414627,
+            "velocityX": 2.0215735376893504,
+            "velocityY": -2.5536396102078434,
+            "timestamp": 0.39160437799626
         },
         {
-            "x": 1.9833278878841576,
-            "y": 4.759991849616898,
-            "heading": 2.880203589095056,
-            "angularVelocity": -0.8309071716856798,
-            "velocityX": 2.021631504416561,
-            "velocityY": -2.553551945121407,
-            "timestamp": 0.4699252496487733
+            "x": 1.9833278418494473,
+            "y": 4.759991864222915,
+            "heading": -0.25979913954005024,
+            "angularVelocity": -0.8308911134284025,
+            "velocityX": 2.0216317347825643,
+            "velocityY": -2.5535520284602864,
+            "timestamp": 0.469925253595512
         },
         {
-            "x": 2.109996705593866,
-            "y": 4.599994984440838,
-            "heading": 2.828128420495832,
-            "angularVelocity": -0.6648951334896099,
-            "velocityX": 1.6173059583971898,
-            "velocityY": -2.0428380721696886,
-            "timestamp": 0.5482461245902355
+            "x": 2.1099966770232617,
+            "y": 4.599994992856252,
+            "heading": -0.31187323961880925,
+            "angularVelocity": -0.6648814850489074,
+            "velocityX": 1.6173061677954905,
+            "velocityY": -2.0428381340541457,
+            "timestamp": 0.548246129194764
         },
         {
-            "x": 2.2049983397447894,
-            "y": 4.479997467084651,
-            "heading": 2.789065384524658,
-            "angularVelocity": -0.4987563788117872,
-            "velocityX": 1.212979735248467,
-            "velocityY": -1.5321268748066883,
-            "timestamp": 0.6265669995316977
+            "x": 2.204998325241535,
+            "y": 4.479997471163016,
+            "heading": -0.3509354488627182,
+            "angularVelocity": -0.49874581897910997,
+            "velocityX": 1.212979904672827,
+            "velocityY": -1.5321269173142713,
+            "timestamp": 0.6265670047940161
         },
         {
-            "x": 2.268332774342349,
-            "y": 4.3999991596507995,
-            "heading": 2.7630214971574416,
-            "angularVelocity": -0.3325280442370214,
-            "velocityX": 0.8086533078811499,
-            "velocityY": -1.0214174381177892,
-            "timestamp": 0.7048878744731599
+            "x": 2.2683327695206965,
+            "y": 4.399999160989759,
+            "heading": -0.37697877943637265,
+            "angularVelocity": -0.33252093231071334,
+            "velocityX": 0.8086534247041309,
+            "velocityY": -1.0214174645159662,
+            "timestamp": 0.7048878803932681
         },
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": -0.1662583208777211,
-            "velocityX": 0.4043267606665432,
-            "velocityY": -0.510708794822514,
-            "timestamp": 0.7832087494146222
+            "heading": -0.39,
+            "angularVelocity": -0.16625478793487553,
+            "velocityX": 0.40432681883354604,
+            "velocityY": -0.5107088076290647,
+            "timestamp": 0.7832087559925202
         },
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 6.867412911994789e-31,
-            "velocityX": 0,
-            "velocityY": 9.317443122449736e-32,
-            "timestamp": 0.8615296243560844
+            "heading": -0.39,
+            "angularVelocity": 0,
+            "velocityX": 7.259744325812885e-33,
+            "velocityY": -1.1119528318776295e-29,
+            "timestamp": 0.8615296315917722
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.1.traj
index cfc8d5b6..0d03ba96 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.1.traj	
@@ -3,128 +3,128 @@
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": -2.88,
+            "heading": 0.26,
             "angularVelocity": 0,
-            "velocityX": 1.8796456462447601e-29,
-            "velocityY": -4.3119341309057906e-30,
+            "velocityX": -1.5159307977916785e-33,
+            "velocityY": 2.4731304546687864e-31,
             "timestamp": 0
         },
         {
-            "x": 5.238209828475023,
-            "y": 6.650800952122015,
-            "heading": -2.892998872920493,
-            "angularVelocity": -0.1464435777830739,
-            "velocityX": 0.7008476743058382,
-            "velocityY": 0.23434076697564932,
-            "timestamp": 0.08876369396038813
+            "x": 5.238209862421909,
+            "y": 6.650800928993829,
+            "heading": 0.2470076817123004,
+            "angularVelocity": -0.14636981892397002,
+            "velocityX": 0.7008484626165542,
+            "velocityY": 0.23434064212588615,
+            "timestamp": 0.08876364255629132
         },
         {
-            "x": 5.362652804152687,
-            "y": 6.692404857453675,
-            "heading": -2.917433036424788,
-            "angularVelocity": -0.27527204439886777,
-            "velocityX": 1.401958054303432,
-            "velocityY": 0.46870407793850294,
-            "timestamp": 0.17752738792077627
+            "x": 5.362652882936083,
+            "y": 6.6924048077649365,
+            "heading": 0.22258636188758224,
+            "angularVelocity": -0.27512750853804663,
+            "velocityX": 1.4019593713167977,
+            "velocityY": 0.4687040501427167,
+            "timestamp": 0.17752728511258264
         },
         {
-            "x": 5.5493620218671325,
-            "y": 6.7548129941209,
-            "heading": -2.950667481161702,
-            "angularVelocity": -0.37441484524641305,
-            "velocityX": 2.1034412763117816,
-            "velocityY": 0.70308178809801,
-            "timestamp": 0.2662910818811644
+            "x": 5.549362119474242,
+            "y": 6.7548129470224065,
+            "heading": 0.1893705540696427,
+            "angularVelocity": -0.37420510089603504,
+            "velocityX": 2.1034427065056938,
+            "velocityY": 0.7030822244423112,
+            "timestamp": 0.26629092766887397
         },
         {
-            "x": 5.798380867010647,
-            "y": 6.838019600580292,
-            "heading": -2.987306905639368,
-            "angularVelocity": -0.4127748952720924,
-            "velocityX": 2.8054132723935825,
-            "velocityY": 0.9373945894737359,
-            "timestamp": 0.35505477584155254
+            "x": 5.798380890690993,
+            "y": 6.838019653320762,
+            "heading": 0.15275441561528638,
+            "angularVelocity": -0.4125127969134817,
+            "velocityX": 2.80541406419279,
+            "velocityY": 0.9373962571031511,
+            "timestamp": 0.3550545702251653
         },
         {
-            "x": 6.109622963136531,
-            "y": 6.941929172833481,
-            "heading": -3.009821564076527,
-            "angularVelocity": -0.25364715496791174,
-            "velocityX": 3.5064121628854164,
-            "velocityY": 1.1706314554748125,
-            "timestamp": 0.4438184698019407
+            "x": 6.109622726468052,
+            "y": 6.941929683454389,
+            "heading": 0.13026224178151366,
+            "angularVelocity": -0.2533939931523947,
+            "velocityX": 3.5064112604341973,
+            "velocityY": 1.17063729182453,
+            "timestamp": 0.4438182127814566
         },
         {
-            "x": 6.428045089115211,
-            "y": 7.048516124767363,
-            "heading": -3.0098215751251205,
-            "angularVelocity": -1.2447253578662016e-7,
-            "velocityX": 3.5873014266485415,
-            "velocityY": 1.2007944597359943,
-            "timestamp": 0.5325821637623288
+            "x": 6.428044865529091,
+            "y": 7.048515983686504,
+            "heading": 0.13026223073820456,
+            "angularVelocity": -1.2441305517804787e-7,
+            "velocityX": 3.587303651482315,
+            "velocityY": 1.2007878131408318,
+            "timestamp": 0.5325818553377479
         },
         {
-            "x": 6.746467236003722,
-            "y": 7.1551030142348395,
-            "heading": -3.0098215861735884,
-            "angularVelocity": -1.2447113137335796e-7,
-            "velocityX": 3.5873016622160976,
-            "velocityY": 1.2007937559974857,
-            "timestamp": 0.6213458577227169
+            "x": 6.746467025502362,
+            "y": 7.155102221444599,
+            "heading": 0.13026221969501808,
+            "angularVelocity": -1.244116833309462e-7,
+            "velocityX": 3.5873038870770317,
+            "velocityY": 1.2007871093161688,
+            "timestamp": 0.6213454978940393
         },
         {
-            "x": 7.057671617899985,
-            "y": 7.2589999980659305,
-            "heading": -3.0321320878151,
-            "angularVelocity": -0.2513471515963816,
-            "velocityX": 3.5059872793849554,
-            "velocityY": 1.1704896359626165,
-            "timestamp": 0.7101095516831051
+            "x": 7.057671328205741,
+            "y": 7.258999778631647,
+            "heading": 0.10797327486362365,
+            "angularVelocity": -0.25110444085082756,
+            "velocityX": 3.505988417567436,
+            "velocityY": 1.1704967731634406,
+            "timestamp": 0.7101091404503306
         },
         {
-            "x": 7.306674739642628,
-            "y": 7.342201198643625,
-            "heading": -3.06987813605104,
-            "angularVelocity": -0.42524197169930134,
-            "velocityX": 2.8052361346509134,
-            "velocityY": 0.9373336875121538,
-            "timestamp": 0.7988732456434933
+            "x": 7.306674452232408,
+            "y": 7.342201135769052,
+            "heading": 0.07025598564329198,
+            "angularVelocity": -0.4249182225204012,
+            "velocityX": 2.8052377849284973,
+            "velocityY": 0.9373359941155657,
+            "timestamp": 0.7988727830066219
         },
         {
-            "x": 7.493368024895104,
-            "y": 7.4046034302940065,
-            "heading": -3.1043079780358913,
-            "angularVelocity": -0.3878820320283419,
-            "velocityX": 2.1032617833134357,
-            "velocityY": 0.7030152629492495,
-            "timestamp": 0.8876369396038815
+            "x": 7.493367837286706,
+            "y": 7.404603419315824,
+            "heading": 0.03585067831804618,
+            "angularVelocity": -0.38760585227578265,
+            "velocityX": 2.103264125692411,
+            "velocityY": 0.7030162547322278,
+            "timestamp": 0.8876364255629132
         },
         {
-            "x": 7.617797798391618,
-            "y": 7.446202168700309,
-            "heading": -3.129675012430027,
-            "angularVelocity": -0.28578164406378687,
-            "velocityX": 1.4018093202853217,
-            "velocityY": 0.4686458680340631,
-            "timestamp": 0.9764006335642696
+            "x": 7.617797724211195,
+            "y": 7.446202169802599,
+            "heading": 0.01050114896385133,
+            "angularVelocity": -0.2855845999919636,
+            "velocityX": 1.401811409955239,
+            "velocityY": 0.46864627552974614,
+            "timestamp": 0.9764000681192045
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.15220518825005472,
-            "velocityX": 0.7007617510398297,
-            "velocityY": 0.23430560820061222,
-            "timestamp": 1.0651643275246578
+            "heading": -0.003,
+            "angularVelocity": -0.15210224112604764,
+            "velocityX": 0.7007629925666751,
+            "velocityY": 0.2343057314716256,
+            "timestamp": 1.0651637106754959
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0,
-            "velocityX": 1.9101182509980996e-31,
-            "velocityY": -4.716397884209056e-30,
-            "timestamp": 1.153928021485046
+            "heading": -0.003,
+            "angularVelocity": 2.057519635914241e-30,
+            "velocityX": 3.0968464213310355e-31,
+            "velocityY": -5.4576131966501765e-31,
+            "timestamp": 1.1539273532317873
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.traj
index cfc8d5b6..0d03ba96 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.traj	
@@ -3,128 +3,128 @@
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": -2.88,
+            "heading": 0.26,
             "angularVelocity": 0,
-            "velocityX": 1.8796456462447601e-29,
-            "velocityY": -4.3119341309057906e-30,
+            "velocityX": -1.5159307977916785e-33,
+            "velocityY": 2.4731304546687864e-31,
             "timestamp": 0
         },
         {
-            "x": 5.238209828475023,
-            "y": 6.650800952122015,
-            "heading": -2.892998872920493,
-            "angularVelocity": -0.1464435777830739,
-            "velocityX": 0.7008476743058382,
-            "velocityY": 0.23434076697564932,
-            "timestamp": 0.08876369396038813
+            "x": 5.238209862421909,
+            "y": 6.650800928993829,
+            "heading": 0.2470076817123004,
+            "angularVelocity": -0.14636981892397002,
+            "velocityX": 0.7008484626165542,
+            "velocityY": 0.23434064212588615,
+            "timestamp": 0.08876364255629132
         },
         {
-            "x": 5.362652804152687,
-            "y": 6.692404857453675,
-            "heading": -2.917433036424788,
-            "angularVelocity": -0.27527204439886777,
-            "velocityX": 1.401958054303432,
-            "velocityY": 0.46870407793850294,
-            "timestamp": 0.17752738792077627
+            "x": 5.362652882936083,
+            "y": 6.6924048077649365,
+            "heading": 0.22258636188758224,
+            "angularVelocity": -0.27512750853804663,
+            "velocityX": 1.4019593713167977,
+            "velocityY": 0.4687040501427167,
+            "timestamp": 0.17752728511258264
         },
         {
-            "x": 5.5493620218671325,
-            "y": 6.7548129941209,
-            "heading": -2.950667481161702,
-            "angularVelocity": -0.37441484524641305,
-            "velocityX": 2.1034412763117816,
-            "velocityY": 0.70308178809801,
-            "timestamp": 0.2662910818811644
+            "x": 5.549362119474242,
+            "y": 6.7548129470224065,
+            "heading": 0.1893705540696427,
+            "angularVelocity": -0.37420510089603504,
+            "velocityX": 2.1034427065056938,
+            "velocityY": 0.7030822244423112,
+            "timestamp": 0.26629092766887397
         },
         {
-            "x": 5.798380867010647,
-            "y": 6.838019600580292,
-            "heading": -2.987306905639368,
-            "angularVelocity": -0.4127748952720924,
-            "velocityX": 2.8054132723935825,
-            "velocityY": 0.9373945894737359,
-            "timestamp": 0.35505477584155254
+            "x": 5.798380890690993,
+            "y": 6.838019653320762,
+            "heading": 0.15275441561528638,
+            "angularVelocity": -0.4125127969134817,
+            "velocityX": 2.80541406419279,
+            "velocityY": 0.9373962571031511,
+            "timestamp": 0.3550545702251653
         },
         {
-            "x": 6.109622963136531,
-            "y": 6.941929172833481,
-            "heading": -3.009821564076527,
-            "angularVelocity": -0.25364715496791174,
-            "velocityX": 3.5064121628854164,
-            "velocityY": 1.1706314554748125,
-            "timestamp": 0.4438184698019407
+            "x": 6.109622726468052,
+            "y": 6.941929683454389,
+            "heading": 0.13026224178151366,
+            "angularVelocity": -0.2533939931523947,
+            "velocityX": 3.5064112604341973,
+            "velocityY": 1.17063729182453,
+            "timestamp": 0.4438182127814566
         },
         {
-            "x": 6.428045089115211,
-            "y": 7.048516124767363,
-            "heading": -3.0098215751251205,
-            "angularVelocity": -1.2447253578662016e-7,
-            "velocityX": 3.5873014266485415,
-            "velocityY": 1.2007944597359943,
-            "timestamp": 0.5325821637623288
+            "x": 6.428044865529091,
+            "y": 7.048515983686504,
+            "heading": 0.13026223073820456,
+            "angularVelocity": -1.2441305517804787e-7,
+            "velocityX": 3.587303651482315,
+            "velocityY": 1.2007878131408318,
+            "timestamp": 0.5325818553377479
         },
         {
-            "x": 6.746467236003722,
-            "y": 7.1551030142348395,
-            "heading": -3.0098215861735884,
-            "angularVelocity": -1.2447113137335796e-7,
-            "velocityX": 3.5873016622160976,
-            "velocityY": 1.2007937559974857,
-            "timestamp": 0.6213458577227169
+            "x": 6.746467025502362,
+            "y": 7.155102221444599,
+            "heading": 0.13026221969501808,
+            "angularVelocity": -1.244116833309462e-7,
+            "velocityX": 3.5873038870770317,
+            "velocityY": 1.2007871093161688,
+            "timestamp": 0.6213454978940393
         },
         {
-            "x": 7.057671617899985,
-            "y": 7.2589999980659305,
-            "heading": -3.0321320878151,
-            "angularVelocity": -0.2513471515963816,
-            "velocityX": 3.5059872793849554,
-            "velocityY": 1.1704896359626165,
-            "timestamp": 0.7101095516831051
+            "x": 7.057671328205741,
+            "y": 7.258999778631647,
+            "heading": 0.10797327486362365,
+            "angularVelocity": -0.25110444085082756,
+            "velocityX": 3.505988417567436,
+            "velocityY": 1.1704967731634406,
+            "timestamp": 0.7101091404503306
         },
         {
-            "x": 7.306674739642628,
-            "y": 7.342201198643625,
-            "heading": -3.06987813605104,
-            "angularVelocity": -0.42524197169930134,
-            "velocityX": 2.8052361346509134,
-            "velocityY": 0.9373336875121538,
-            "timestamp": 0.7988732456434933
+            "x": 7.306674452232408,
+            "y": 7.342201135769052,
+            "heading": 0.07025598564329198,
+            "angularVelocity": -0.4249182225204012,
+            "velocityX": 2.8052377849284973,
+            "velocityY": 0.9373359941155657,
+            "timestamp": 0.7988727830066219
         },
         {
-            "x": 7.493368024895104,
-            "y": 7.4046034302940065,
-            "heading": -3.1043079780358913,
-            "angularVelocity": -0.3878820320283419,
-            "velocityX": 2.1032617833134357,
-            "velocityY": 0.7030152629492495,
-            "timestamp": 0.8876369396038815
+            "x": 7.493367837286706,
+            "y": 7.404603419315824,
+            "heading": 0.03585067831804618,
+            "angularVelocity": -0.38760585227578265,
+            "velocityX": 2.103264125692411,
+            "velocityY": 0.7030162547322278,
+            "timestamp": 0.8876364255629132
         },
         {
-            "x": 7.617797798391618,
-            "y": 7.446202168700309,
-            "heading": -3.129675012430027,
-            "angularVelocity": -0.28578164406378687,
-            "velocityX": 1.4018093202853217,
-            "velocityY": 0.4686458680340631,
-            "timestamp": 0.9764006335642696
+            "x": 7.617797724211195,
+            "y": 7.446202169802599,
+            "heading": 0.01050114896385133,
+            "angularVelocity": -0.2855845999919636,
+            "velocityX": 1.401811409955239,
+            "velocityY": 0.46864627552974614,
+            "timestamp": 0.9764000681192045
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.15220518825005472,
-            "velocityX": 0.7007617510398297,
-            "velocityY": 0.23430560820061222,
-            "timestamp": 1.0651643275246578
+            "heading": -0.003,
+            "angularVelocity": -0.15210224112604764,
+            "velocityX": 0.7007629925666751,
+            "velocityY": 0.2343057314716256,
+            "timestamp": 1.0651637106754959
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0,
-            "velocityX": 1.9101182509980996e-31,
-            "velocityY": -4.716397884209056e-30,
-            "timestamp": 1.153928021485046
+            "heading": -0.003,
+            "angularVelocity": 2.057519635914241e-30,
+            "velocityX": 3.0968464213310355e-31,
+            "velocityY": -5.4576131966501765e-31,
+            "timestamp": 1.1539273532317873
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.1.traj
index 4c23c37c..6b7f0e9c 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.1.traj	
@@ -3,128 +3,128 @@
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": -2.88,
+            "heading": 0.26,
             "angularVelocity": 0,
-            "velocityX": -2.1431055300877995e-28,
-            "velocityY": 1.1784110457685481e-29,
+            "velocityX": -4.471050480852603e-35,
+            "velocityY": 4.2685150703849235e-34,
             "timestamp": 0
         },
         {
-            "x": 5.242060097443899,
-            "y": 6.625920772567306,
-            "heading": -2.9233556616628746,
-            "angularVelocity": -0.48269065593589344,
-            "velocityX": 0.7354654627191669,
-            "velocityY": -0.04541517508160964,
-            "timestamp": 0.08982080164534181
+            "x": 5.242063425720018,
+            "y": 6.625917381735586,
+            "heading": 0.21680591867144897,
+            "angularVelocity": -0.4809077162576206,
+            "velocityX": 0.7355269567955554,
+            "velocityY": -0.04545443647608383,
+            "timestamp": 0.08981781715769374
         },
         {
-            "x": 5.3743817426165945,
-            "y": 6.616908903759451,
-            "heading": -3.004544486756046,
-            "angularVelocity": -0.9038978010210977,
-            "velocityX": 1.4731737275644603,
-            "velocityY": -0.1003316452622891,
-            "timestamp": 0.17964160329068363
+            "x": 5.374390186000276,
+            "y": 6.616899298560691,
+            "heading": 0.13592925344567436,
+            "angularVelocity": -0.9004523577296353,
+            "velocityX": 1.473279628338445,
+            "velocityY": -0.10040416768380533,
+            "timestamp": 0.17963563431538748
         },
         {
-            "x": 5.573213116332803,
-            "y": 6.601503149315071,
-            "heading": -3.1137348179578126,
-            "angularVelocity": -1.215646367006541,
-            "velocityX": 2.2136450585388348,
-            "velocityY": -0.17151655476435285,
-            "timestamp": 0.26946240493602547
+            "x": 5.573225936929258,
+            "y": 6.6014853361346235,
+            "heading": 0.027174299165301757,
+            "angularVelocity": -1.2108394272088667,
+            "velocityX": 2.2137673484079947,
+            "velocityY": -0.17161363873946028,
+            "timestamp": 0.2694534514730812
         },
         {
-            "x": 5.83860703608144,
-            "y": 6.576562673459042,
-            "heading": -3.229878025812627,
-            "angularVelocity": -1.2930546791757207,
-            "velocityX": 2.954704421327105,
-            "velocityY": -0.2776692636802449,
-            "timestamp": 0.35928320658136725
+            "x": 5.838620178684051,
+            "y": 6.5765367651917135,
+            "heading": -0.08847883132007715,
+            "angularVelocity": -1.287641295961646,
+            "velocityX": 2.9548061860470294,
+            "velocityY": -0.27776861799154184,
+            "timestamp": 0.35927126863077496
         },
         {
-            "x": 6.164587325578701,
-            "y": 6.531858569137778,
-            "heading": -3.2661345564867124,
-            "angularVelocity": -0.40365405351480704,
-            "velocityX": 3.629229349170066,
-            "velocityY": -0.49770324359594226,
-            "timestamp": 0.44910400822670904
+            "x": 6.164628937797069,
+            "y": 6.53182752325973,
+            "heading": -0.12460373302334064,
+            "angularVelocity": -0.4022019555411679,
+            "velocityX": 3.6296669127534233,
+            "velocityY": -0.4977769817483746,
+            "timestamp": 0.44908908578846873
         },
         {
-            "x": 6.48571912004567,
-            "y": 6.420820709470225,
-            "heading": -3.2661345579017715,
-            "angularVelocity": -1.5754250259377596e-8,
-            "velocityX": 3.5752497036818096,
-            "velocityY": -1.2362154159565475,
-            "timestamp": 0.5389248098720508
+            "x": 6.48574789195848,
+            "y": 6.420787077372237,
+            "heading": -0.12460373465453414,
+            "angularVelocity": -1.816113510684907e-8,
+            "velocityX": 3.5752255434756393,
+            "velocityY": -1.2362852872781145,
+            "timestamp": 0.5389069029461625
         },
         {
-            "x": 6.786864453228811,
-            "y": 6.263446876676794,
-            "heading": -3.2661345453374624,
-            "angularVelocity": 1.3988195638330222e-7,
-            "velocityX": 3.3527348639373855,
-            "velocityY": -1.7520867094330927,
-            "timestamp": 0.6287456115173926
+            "x": 6.786885646715688,
+            "y": 6.263423119459949,
+            "heading": -0.12460372224054271,
+            "angularVelocity": 1.3821301643987816e-7,
+            "velocityX": 3.3527618938734416,
+            "velocityY": -1.7520349847291758,
+            "timestamp": 0.6287247201038563
         },
         {
-            "x": 7.084475527096153,
-            "y": 6.108061043338325,
-            "heading": -3.255293907795362,
-            "angularVelocity": 0.12069183689660036,
-            "velocityX": 3.313386970675918,
-            "velocityY": -1.7299537578389734,
-            "timestamp": 0.7185664131627344
+            "x": 7.084493676309923,
+            "y": 6.108042134366637,
+            "heading": -0.11377963368638179,
+            "angularVelocity": 0.12051159665968046,
+            "velocityX": 3.313463174814436,
+            "velocityY": -1.729957262494017,
+            "timestamp": 0.71854253726155
         },
         {
-            "x": 7.322762050188167,
-            "y": 5.983606942670793,
-            "heading": -3.219406300389853,
-            "angularVelocity": 0.3995467280198762,
-            "velocityX": 2.652910224881898,
-            "velocityY": -1.385582163460734,
-            "timestamp": 0.8083872148080762
+            "x": 7.322771133319765,
+            "y": 5.983596419357133,
+            "heading": -0.07830445075984473,
+            "angularVelocity": 0.3949682150953753,
+            "velocityX": 2.652897437837947,
+            "velocityY": -1.3855348409437966,
+            "timestamp": 0.8083603544192438
         },
         {
-            "x": 7.5014175228899544,
-            "y": 5.890287602383411,
-            "heading": -3.184260886361704,
-            "angularVelocity": 0.39128368244731204,
-            "velocityX": 1.9890211335143717,
-            "velocityY": -1.0389502050521982,
-            "timestamp": 0.898208016453418
+            "x": 7.5014211847430525,
+            "y": 5.8902827728941185,
+            "heading": -0.04357887601405372,
+            "angularVelocity": 0.38662234114221544,
+            "velocityX": 1.9890268665695783,
+            "velocityY": -1.0389213344962835,
+            "timestamp": 0.8981781715769376
         },
         {
-            "x": 7.620482912490659,
-            "y": 5.8280911603292145,
-            "heading": -3.1576094170182762,
-            "angularVelocity": 0.296718230690803,
-            "velocityX": 1.325588142386374,
-            "velocityY": -0.6924503112294588,
-            "timestamp": 0.9880288180987598
+            "x": 7.620483882250166,
+            "y": 5.828089677839219,
+            "heading": -0.01724918141894823,
+            "angularVelocity": 0.293145563189075,
+            "velocityX": 1.3256022165187353,
+            "velocityY": -0.6924360558184839,
+            "timestamp": 0.9879959887346313
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0.16058763197915146,
-            "velocityX": 0.6626203108757032,
-            "velocityY": -0.3461465468987679,
-            "timestamp": 1.0778496197441017
+            "heading": -0.003,
+            "angularVelocity": 0.1586453764950761,
+            "velocityX": 0.6626315316185003,
+            "velocityY": -0.346141543215585,
+            "timestamp": 1.077813805892325
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": -3.143185307179586,
-            "angularVelocity": 2.6110530197350993e-29,
-            "velocityX": -1.9135813261242845e-30,
-            "velocityY": -4.114109248291899e-30,
-            "timestamp": 1.1676704213894435
+            "heading": -0.003,
+            "angularVelocity": 2.21164904203514e-32,
+            "velocityX": 6.504499750841485e-34,
+            "velocityY": 5.343957154186367e-33,
+            "timestamp": 1.1676316230500188
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.traj
index 4c23c37c..6b7f0e9c 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.traj	
@@ -3,128 +3,128 @@
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": -2.88,
+            "heading": 0.26,
             "angularVelocity": 0,
-            "velocityX": -2.1431055300877995e-28,
-            "velocityY": 1.1784110457685481e-29,
+            "velocityX": -4.471050480852603e-35,
+            "velocityY": 4.2685150703849235e-34,
             "timestamp": 0
         },
         {
-            "x": 5.242060097443899,
-            "y": 6.625920772567306,
-            "heading": -2.9233556616628746,
-            "angularVelocity": -0.48269065593589344,
-            "velocityX": 0.7354654627191669,
-            "velocityY": -0.04541517508160964,
-            "timestamp": 0.08982080164534181
+            "x": 5.242063425720018,
+            "y": 6.625917381735586,
+            "heading": 0.21680591867144897,
+            "angularVelocity": -0.4809077162576206,
+            "velocityX": 0.7355269567955554,
+            "velocityY": -0.04545443647608383,
+            "timestamp": 0.08981781715769374
         },
         {
-            "x": 5.3743817426165945,
-            "y": 6.616908903759451,
-            "heading": -3.004544486756046,
-            "angularVelocity": -0.9038978010210977,
-            "velocityX": 1.4731737275644603,
-            "velocityY": -0.1003316452622891,
-            "timestamp": 0.17964160329068363
+            "x": 5.374390186000276,
+            "y": 6.616899298560691,
+            "heading": 0.13592925344567436,
+            "angularVelocity": -0.9004523577296353,
+            "velocityX": 1.473279628338445,
+            "velocityY": -0.10040416768380533,
+            "timestamp": 0.17963563431538748
         },
         {
-            "x": 5.573213116332803,
-            "y": 6.601503149315071,
-            "heading": -3.1137348179578126,
-            "angularVelocity": -1.215646367006541,
-            "velocityX": 2.2136450585388348,
-            "velocityY": -0.17151655476435285,
-            "timestamp": 0.26946240493602547
+            "x": 5.573225936929258,
+            "y": 6.6014853361346235,
+            "heading": 0.027174299165301757,
+            "angularVelocity": -1.2108394272088667,
+            "velocityX": 2.2137673484079947,
+            "velocityY": -0.17161363873946028,
+            "timestamp": 0.2694534514730812
         },
         {
-            "x": 5.83860703608144,
-            "y": 6.576562673459042,
-            "heading": -3.229878025812627,
-            "angularVelocity": -1.2930546791757207,
-            "velocityX": 2.954704421327105,
-            "velocityY": -0.2776692636802449,
-            "timestamp": 0.35928320658136725
+            "x": 5.838620178684051,
+            "y": 6.5765367651917135,
+            "heading": -0.08847883132007715,
+            "angularVelocity": -1.287641295961646,
+            "velocityX": 2.9548061860470294,
+            "velocityY": -0.27776861799154184,
+            "timestamp": 0.35927126863077496
         },
         {
-            "x": 6.164587325578701,
-            "y": 6.531858569137778,
-            "heading": -3.2661345564867124,
-            "angularVelocity": -0.40365405351480704,
-            "velocityX": 3.629229349170066,
-            "velocityY": -0.49770324359594226,
-            "timestamp": 0.44910400822670904
+            "x": 6.164628937797069,
+            "y": 6.53182752325973,
+            "heading": -0.12460373302334064,
+            "angularVelocity": -0.4022019555411679,
+            "velocityX": 3.6296669127534233,
+            "velocityY": -0.4977769817483746,
+            "timestamp": 0.44908908578846873
         },
         {
-            "x": 6.48571912004567,
-            "y": 6.420820709470225,
-            "heading": -3.2661345579017715,
-            "angularVelocity": -1.5754250259377596e-8,
-            "velocityX": 3.5752497036818096,
-            "velocityY": -1.2362154159565475,
-            "timestamp": 0.5389248098720508
+            "x": 6.48574789195848,
+            "y": 6.420787077372237,
+            "heading": -0.12460373465453414,
+            "angularVelocity": -1.816113510684907e-8,
+            "velocityX": 3.5752255434756393,
+            "velocityY": -1.2362852872781145,
+            "timestamp": 0.5389069029461625
         },
         {
-            "x": 6.786864453228811,
-            "y": 6.263446876676794,
-            "heading": -3.2661345453374624,
-            "angularVelocity": 1.3988195638330222e-7,
-            "velocityX": 3.3527348639373855,
-            "velocityY": -1.7520867094330927,
-            "timestamp": 0.6287456115173926
+            "x": 6.786885646715688,
+            "y": 6.263423119459949,
+            "heading": -0.12460372224054271,
+            "angularVelocity": 1.3821301643987816e-7,
+            "velocityX": 3.3527618938734416,
+            "velocityY": -1.7520349847291758,
+            "timestamp": 0.6287247201038563
         },
         {
-            "x": 7.084475527096153,
-            "y": 6.108061043338325,
-            "heading": -3.255293907795362,
-            "angularVelocity": 0.12069183689660036,
-            "velocityX": 3.313386970675918,
-            "velocityY": -1.7299537578389734,
-            "timestamp": 0.7185664131627344
+            "x": 7.084493676309923,
+            "y": 6.108042134366637,
+            "heading": -0.11377963368638179,
+            "angularVelocity": 0.12051159665968046,
+            "velocityX": 3.313463174814436,
+            "velocityY": -1.729957262494017,
+            "timestamp": 0.71854253726155
         },
         {
-            "x": 7.322762050188167,
-            "y": 5.983606942670793,
-            "heading": -3.219406300389853,
-            "angularVelocity": 0.3995467280198762,
-            "velocityX": 2.652910224881898,
-            "velocityY": -1.385582163460734,
-            "timestamp": 0.8083872148080762
+            "x": 7.322771133319765,
+            "y": 5.983596419357133,
+            "heading": -0.07830445075984473,
+            "angularVelocity": 0.3949682150953753,
+            "velocityX": 2.652897437837947,
+            "velocityY": -1.3855348409437966,
+            "timestamp": 0.8083603544192438
         },
         {
-            "x": 7.5014175228899544,
-            "y": 5.890287602383411,
-            "heading": -3.184260886361704,
-            "angularVelocity": 0.39128368244731204,
-            "velocityX": 1.9890211335143717,
-            "velocityY": -1.0389502050521982,
-            "timestamp": 0.898208016453418
+            "x": 7.5014211847430525,
+            "y": 5.8902827728941185,
+            "heading": -0.04357887601405372,
+            "angularVelocity": 0.38662234114221544,
+            "velocityX": 1.9890268665695783,
+            "velocityY": -1.0389213344962835,
+            "timestamp": 0.8981781715769376
         },
         {
-            "x": 7.620482912490659,
-            "y": 5.8280911603292145,
-            "heading": -3.1576094170182762,
-            "angularVelocity": 0.296718230690803,
-            "velocityX": 1.325588142386374,
-            "velocityY": -0.6924503112294588,
-            "timestamp": 0.9880288180987598
+            "x": 7.620483882250166,
+            "y": 5.828089677839219,
+            "heading": -0.01724918141894823,
+            "angularVelocity": 0.293145563189075,
+            "velocityX": 1.3256022165187353,
+            "velocityY": -0.6924360558184839,
+            "timestamp": 0.9879959887346313
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0.16058763197915146,
-            "velocityX": 0.6626203108757032,
-            "velocityY": -0.3461465468987679,
-            "timestamp": 1.0778496197441017
+            "heading": -0.003,
+            "angularVelocity": 0.1586453764950761,
+            "velocityX": 0.6626315316185003,
+            "velocityY": -0.346141543215585,
+            "timestamp": 1.077813805892325
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": -3.143185307179586,
-            "angularVelocity": 2.6110530197350993e-29,
-            "velocityX": -1.9135813261242845e-30,
-            "velocityY": -4.114109248291899e-30,
-            "timestamp": 1.1676704213894435
+            "heading": -0.003,
+            "angularVelocity": 2.21164904203514e-32,
+            "velocityX": 6.504499750841485e-34,
+            "velocityY": 5.343957154186367e-33,
+            "timestamp": 1.1676316230500188
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj
index 877ee8fe..cf856573 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj	
@@ -3,128 +3,128 @@
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": -6.0746648103992426e-33,
-            "velocityX": -3.2850296304780664e-35,
-            "velocityY": -2.0665663458795258e-33,
+            "heading": 0.26,
+            "angularVelocity": 2.3848638783294046e-33,
+            "velocityX": 3.9311043555760955e-34,
+            "velocityY": 6.649026992883858e-33,
             "timestamp": 0
         },
         {
-            "x": 5.102979054151253,
-            "y": 6.633224554548809,
-            "heading": -2.8734987043090925,
-            "angularVelocity": 0.06942752469656709,
-            "velocityX": -0.7797927924375683,
-            "velocityY": 0.03443511127885569,
-            "timestamp": 0.09364147316685267
+            "x": 5.1029790541350515,
+            "y": 6.633224554642693,
+            "heading": 0.2665012951102971,
+            "angularVelocity": 0.06942751851370552,
+            "velocityX": -0.7797927928070008,
+            "velocityY": 0.03443511229011806,
+            "timestamp": 0.09364147314326547
         },
         {
-            "x": 4.9569371637209425,
-            "y": 6.639673650012151,
-            "heading": -2.8604963348705175,
-            "angularVelocity": 0.1388526792548496,
-            "velocityX": -1.5595855713428277,
-            "velocityY": 0.0688700769568818,
-            "timestamp": 0.18728294633370535
+            "x": 4.956937163671988,
+            "y": 6.639673650268821,
+            "heading": 0.2795036636182218,
+            "angularVelocity": 0.13885266935138055,
+            "velocityX": -1.5595855720854526,
+            "velocityY": 0.06887007871263316,
+            "timestamp": 0.18728294628653094
         },
         {
-            "x": 4.737874332155748,
-            "y": 6.649347273486247,
-            "heading": -2.8409932661051047,
-            "angularVelocity": 0.2082738353622199,
-            "velocityX": -2.3393783134408976,
-            "velocityY": 0.10330490483508797,
-            "timestamp": 0.280924419500558
+            "x": 4.737874332057849,
+            "y": 6.649347273946163,
+            "heading": 0.29900673147988355,
+            "angularVelocity": 0.20827382576349504,
+            "velocityX": -2.3393783145528313,
+            "velocityY": 0.10330490703159452,
+            "timestamp": 0.2809244194297964
         },
         {
-            "x": 4.445790573832511,
-            "y": 6.662245412808618,
-            "heading": -2.8149900541710062,
-            "angularVelocity": 0.27768905224050994,
-            "velocityX": -3.119170902008226,
-            "velocityY": 0.13773960282991304,
-            "timestamp": 0.3745658926674107
+            "x": 4.445790573670186,
+            "y": 6.662245413482842,
+            "heading": 0.325009942942769,
+            "angularVelocity": 0.27768904727835797,
+            "velocityX": -3.1191709034819257,
+            "velocityY": 0.13773960515320766,
+            "timestamp": 0.3745658925730619
         },
         {
-            "x": 4.091895329085733,
-            "y": 6.677873079084158,
-            "heading": -2.814990051178798,
-            "angularVelocity": 3.195386469208516e-8,
-            "velocityX": -3.7792575530737516,
-            "velocityY": 0.16688830009804687,
-            "timestamp": 0.46820736583426337
+            "x": 4.091895329009691,
+            "y": 6.677873079689744,
+            "heading": 0.32500994593496846,
+            "angularVelocity": 3.195376913904642e-8,
+            "velocityX": -3.7792575531042867,
+            "velocityY": 0.16688829940708347,
+            "timestamp": 0.4682073657163274
         },
         {
-            "x": 3.738000080960507,
-            "y": 6.693500668821502,
-            "heading": -2.8149900481866714,
-            "angularVelocity": 3.195300400483476e-8,
-            "velocityX": -3.7792575891522513,
-            "velocityY": 0.16688748274491266,
-            "timestamp": 0.561848839001116
+            "x": 3.738000080970794,
+            "y": 6.693500669359476,
+            "heading": 0.3250099489270859,
+            "angularVelocity": 3.1952908210754574e-8,
+            "velocityX": -3.779257589182282,
+            "velocityY": 0.1668874820649196,
+            "timestamp": 0.5618488388595928
         },
         {
-            "x": 3.384104829826614,
-            "y": 6.709128190425974,
-            "heading": -2.8149900451945484,
-            "angularVelocity": 3.1952965397811645e-8,
-            "velocityX": -3.7792576212818934,
-            "velocityY": 0.1668867551519713,
-            "timestamp": 0.6554903121679687
+            "x": 3.3841048299235403,
+            "y": 6.709128190903334,
+            "heading": 0.3250099519191997,
+            "angularVelocity": 3.195286925539051e-8,
+            "velocityX": -3.779257621308623,
+            "velocityY": 0.16688675454671412,
+            "timestamp": 0.6554903120028582
         },
         {
-            "x": 3.0302095759842613,
-            "y": 6.724755650696981,
-            "heading": -2.8149900422022327,
-            "angularVelocity": 3.195501290053148e-8,
-            "velocityX": -3.779257650205636,
-            "velocityY": 0.16688610017037783,
-            "timestamp": 0.7491317853348214
+            "x": 3.030209576168151,
+            "y": 6.724755651121081,
+            "heading": 0.32500995491150664,
+            "angularVelocity": 3.195491758844704e-8,
+            "velocityX": -3.7792576502288973,
+            "velocityY": 0.16688609964363924,
+            "timestamp": 0.7491317851461237
         },
         {
-            "x": 2.738125764107778,
-            "y": 6.7376533993595,
-            "heading": -2.7889938425825695,
-            "angularVelocity": 0.2776141675314341,
-            "velocityX": -3.1191714739049314,
-            "velocityY": 0.1377354309613996,
-            "timestamp": 0.8427732585016741
+            "x": 2.7381257642201002,
+            "y": 6.737653399625744,
+            "heading": 0.3510061559504441,
+            "angularVelocity": 0.2776141827578367,
+            "velocityX": -3.119171475454889,
+            "velocityY": 0.13773542931034466,
+            "timestamp": 0.8427732582893891
         },
         {
-            "x": 2.519062886855627,
-            "y": 6.747326704296012,
-            "heading": -2.7694968873701145,
-            "angularVelocity": 0.20820854855310947,
-            "velocityX": -2.339378801333246,
-            "velocityY": 0.10330150316261463,
-            "timestamp": 0.9364147316685267
+            "x": 2.519062886912394,
+            "y": 6.747326704432178,
+            "heading": 0.37050311197995217,
+            "angularVelocity": 0.20820855733088625,
+            "velocityX": -2.339378802515782,
+            "velocityY": 0.10330150179952581,
+            "timestamp": 0.9364147314326545
         },
         {
-            "x": 2.3730209632759087,
-            "y": 6.753775569660814,
-            "heading": -2.756498972823295,
-            "angularVelocity": 0.13880510533677407,
-            "velocityX": -1.559585925346323,
-            "velocityY": 0.0688676197276826,
-            "timestamp": 1.0300562048353794
+            "x": 2.373020963294867,
+            "y": 6.7537755697062725,
+            "heading": 0.383501026964712,
+            "angularVelocity": 0.13880511004851873,
+            "velocityX": -1.5595859261429292,
+            "velocityY": 0.06886761877636173,
+            "timestamp": 1.03005620457592
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0.06940271872607656,
-            "velocityX": -0.779792978542731,
-            "velocityY": 0.034433784840577714,
-            "timestamp": 1.123697678002232
+            "heading": 0.39,
+            "angularVelocity": 0.06940272100743458,
+            "velocityX": -0.7797929789416104,
+            "velocityY": 0.034433784363791235,
+            "timestamp": 1.1236976777191856
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": -2.75,
+            "heading": 0.39,
             "angularVelocity": 0,
-            "velocityX": -2.0910816279231796e-36,
-            "velocityY": 1.251790530664498e-31,
-            "timestamp": 1.2173391511690848
+            "velocityX": 0,
+            "velocityY": 2.0630865476520398e-33,
+            "timestamp": 1.2173391508624511
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.traj
index 877ee8fe..cf856573 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.traj	
@@ -3,128 +3,128 @@
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": -6.0746648103992426e-33,
-            "velocityX": -3.2850296304780664e-35,
-            "velocityY": -2.0665663458795258e-33,
+            "heading": 0.26,
+            "angularVelocity": 2.3848638783294046e-33,
+            "velocityX": 3.9311043555760955e-34,
+            "velocityY": 6.649026992883858e-33,
             "timestamp": 0
         },
         {
-            "x": 5.102979054151253,
-            "y": 6.633224554548809,
-            "heading": -2.8734987043090925,
-            "angularVelocity": 0.06942752469656709,
-            "velocityX": -0.7797927924375683,
-            "velocityY": 0.03443511127885569,
-            "timestamp": 0.09364147316685267
+            "x": 5.1029790541350515,
+            "y": 6.633224554642693,
+            "heading": 0.2665012951102971,
+            "angularVelocity": 0.06942751851370552,
+            "velocityX": -0.7797927928070008,
+            "velocityY": 0.03443511229011806,
+            "timestamp": 0.09364147314326547
         },
         {
-            "x": 4.9569371637209425,
-            "y": 6.639673650012151,
-            "heading": -2.8604963348705175,
-            "angularVelocity": 0.1388526792548496,
-            "velocityX": -1.5595855713428277,
-            "velocityY": 0.0688700769568818,
-            "timestamp": 0.18728294633370535
+            "x": 4.956937163671988,
+            "y": 6.639673650268821,
+            "heading": 0.2795036636182218,
+            "angularVelocity": 0.13885266935138055,
+            "velocityX": -1.5595855720854526,
+            "velocityY": 0.06887007871263316,
+            "timestamp": 0.18728294628653094
         },
         {
-            "x": 4.737874332155748,
-            "y": 6.649347273486247,
-            "heading": -2.8409932661051047,
-            "angularVelocity": 0.2082738353622199,
-            "velocityX": -2.3393783134408976,
-            "velocityY": 0.10330490483508797,
-            "timestamp": 0.280924419500558
+            "x": 4.737874332057849,
+            "y": 6.649347273946163,
+            "heading": 0.29900673147988355,
+            "angularVelocity": 0.20827382576349504,
+            "velocityX": -2.3393783145528313,
+            "velocityY": 0.10330490703159452,
+            "timestamp": 0.2809244194297964
         },
         {
-            "x": 4.445790573832511,
-            "y": 6.662245412808618,
-            "heading": -2.8149900541710062,
-            "angularVelocity": 0.27768905224050994,
-            "velocityX": -3.119170902008226,
-            "velocityY": 0.13773960282991304,
-            "timestamp": 0.3745658926674107
+            "x": 4.445790573670186,
+            "y": 6.662245413482842,
+            "heading": 0.325009942942769,
+            "angularVelocity": 0.27768904727835797,
+            "velocityX": -3.1191709034819257,
+            "velocityY": 0.13773960515320766,
+            "timestamp": 0.3745658925730619
         },
         {
-            "x": 4.091895329085733,
-            "y": 6.677873079084158,
-            "heading": -2.814990051178798,
-            "angularVelocity": 3.195386469208516e-8,
-            "velocityX": -3.7792575530737516,
-            "velocityY": 0.16688830009804687,
-            "timestamp": 0.46820736583426337
+            "x": 4.091895329009691,
+            "y": 6.677873079689744,
+            "heading": 0.32500994593496846,
+            "angularVelocity": 3.195376913904642e-8,
+            "velocityX": -3.7792575531042867,
+            "velocityY": 0.16688829940708347,
+            "timestamp": 0.4682073657163274
         },
         {
-            "x": 3.738000080960507,
-            "y": 6.693500668821502,
-            "heading": -2.8149900481866714,
-            "angularVelocity": 3.195300400483476e-8,
-            "velocityX": -3.7792575891522513,
-            "velocityY": 0.16688748274491266,
-            "timestamp": 0.561848839001116
+            "x": 3.738000080970794,
+            "y": 6.693500669359476,
+            "heading": 0.3250099489270859,
+            "angularVelocity": 3.1952908210754574e-8,
+            "velocityX": -3.779257589182282,
+            "velocityY": 0.1668874820649196,
+            "timestamp": 0.5618488388595928
         },
         {
-            "x": 3.384104829826614,
-            "y": 6.709128190425974,
-            "heading": -2.8149900451945484,
-            "angularVelocity": 3.1952965397811645e-8,
-            "velocityX": -3.7792576212818934,
-            "velocityY": 0.1668867551519713,
-            "timestamp": 0.6554903121679687
+            "x": 3.3841048299235403,
+            "y": 6.709128190903334,
+            "heading": 0.3250099519191997,
+            "angularVelocity": 3.195286925539051e-8,
+            "velocityX": -3.779257621308623,
+            "velocityY": 0.16688675454671412,
+            "timestamp": 0.6554903120028582
         },
         {
-            "x": 3.0302095759842613,
-            "y": 6.724755650696981,
-            "heading": -2.8149900422022327,
-            "angularVelocity": 3.195501290053148e-8,
-            "velocityX": -3.779257650205636,
-            "velocityY": 0.16688610017037783,
-            "timestamp": 0.7491317853348214
+            "x": 3.030209576168151,
+            "y": 6.724755651121081,
+            "heading": 0.32500995491150664,
+            "angularVelocity": 3.195491758844704e-8,
+            "velocityX": -3.7792576502288973,
+            "velocityY": 0.16688609964363924,
+            "timestamp": 0.7491317851461237
         },
         {
-            "x": 2.738125764107778,
-            "y": 6.7376533993595,
-            "heading": -2.7889938425825695,
-            "angularVelocity": 0.2776141675314341,
-            "velocityX": -3.1191714739049314,
-            "velocityY": 0.1377354309613996,
-            "timestamp": 0.8427732585016741
+            "x": 2.7381257642201002,
+            "y": 6.737653399625744,
+            "heading": 0.3510061559504441,
+            "angularVelocity": 0.2776141827578367,
+            "velocityX": -3.119171475454889,
+            "velocityY": 0.13773542931034466,
+            "timestamp": 0.8427732582893891
         },
         {
-            "x": 2.519062886855627,
-            "y": 6.747326704296012,
-            "heading": -2.7694968873701145,
-            "angularVelocity": 0.20820854855310947,
-            "velocityX": -2.339378801333246,
-            "velocityY": 0.10330150316261463,
-            "timestamp": 0.9364147316685267
+            "x": 2.519062886912394,
+            "y": 6.747326704432178,
+            "heading": 0.37050311197995217,
+            "angularVelocity": 0.20820855733088625,
+            "velocityX": -2.339378802515782,
+            "velocityY": 0.10330150179952581,
+            "timestamp": 0.9364147314326545
         },
         {
-            "x": 2.3730209632759087,
-            "y": 6.753775569660814,
-            "heading": -2.756498972823295,
-            "angularVelocity": 0.13880510533677407,
-            "velocityX": -1.559585925346323,
-            "velocityY": 0.0688676197276826,
-            "timestamp": 1.0300562048353794
+            "x": 2.373020963294867,
+            "y": 6.7537755697062725,
+            "heading": 0.383501026964712,
+            "angularVelocity": 0.13880511004851873,
+            "velocityX": -1.5595859261429292,
+            "velocityY": 0.06886761877636173,
+            "timestamp": 1.03005620457592
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0.06940271872607656,
-            "velocityX": -0.779792978542731,
-            "velocityY": 0.034433784840577714,
-            "timestamp": 1.123697678002232
+            "heading": 0.39,
+            "angularVelocity": 0.06940272100743458,
+            "velocityX": -0.7797929789416104,
+            "velocityY": 0.034433784363791235,
+            "timestamp": 1.1236976777191856
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": -2.75,
+            "heading": 0.39,
             "angularVelocity": 0,
-            "velocityX": -2.0910816279231796e-36,
-            "velocityY": 1.251790530664498e-31,
-            "timestamp": 1.2173391511690848
+            "velocityX": 0,
+            "velocityY": 2.0630865476520398e-33,
+            "timestamp": 1.2173391508624511
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj
index 75b837e7..6bf8a7f8 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj	
@@ -3,146 +3,146 @@
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": -2.88,
+            "heading": 0.26,
             "angularVelocity": 0,
-            "velocityX": -8.07520618943781e-35,
-            "velocityY": -1.6093142036511268e-34,
+            "velocityX": -1.9284905870694258e-35,
+            "velocityY": -6.933375493218394e-34,
             "timestamp": 0
         },
         {
-            "x": 5.118924952955758,
-            "y": 6.609128496946765,
-            "heading": -2.888779485010293,
-            "angularVelocity": -0.10279588776958423,
-            "velocityX": -0.6682715584712982,
-            "velocityY": -0.24437705434061835,
-            "timestamp": 0.08540696715389907
+            "x": 5.118924935599245,
+            "y": 6.609128491648739,
+            "heading": 0.2512266763512042,
+            "angularVelocity": -0.10272378997697001,
+            "velocityX": -0.6682720441106688,
+            "velocityY": -0.24437721964946355,
+            "timestamp": 0.08540693106010512
         },
         {
-            "x": 5.004774858765426,
-            "y": 6.567385488245384,
-            "heading": -2.906338067516999,
-            "angularVelocity": -0.20558723827608577,
-            "velocityX": -1.3365431181350638,
-            "velocityY": -0.48875413906409965,
-            "timestamp": 0.17081393430779815
+            "x": 5.004774806713452,
+            "y": 6.567385472222137,
+            "heading": 0.23368042064504363,
+            "angularVelocity": -0.2054429949463047,
+            "velocityX": -1.3365440892081688,
+            "velocityY": -0.4887544711942054,
+            "timestamp": 0.17081386212021024
         },
         {
-            "x": 4.833549713525512,
-            "y": 6.504770974662745,
-            "heading": -2.9326749312069125,
-            "angularVelocity": -0.3083690308596943,
-            "velocityX": -2.004814723503466,
-            "velocityY": -0.7331312148084055,
-            "timestamp": 0.25622090146169724
+            "x": 4.833549609463453,
+            "y": 6.504770942290587,
+            "heading": 0.2073620554535925,
+            "angularVelocity": -0.3081525687058067,
+            "velocityX": -2.0048161797255077,
+            "velocityY": -0.7331317160604409,
+            "timestamp": 0.2562207931803154
         },
         {
-            "x": 4.6052495100619275,
-            "y": 6.421284959128349,
-            "heading": -2.96778862782457,
-            "angularVelocity": -0.4111338663318194,
-            "velocityX": -2.673086412870736,
-            "velocityY": -0.9775082562521819,
-            "timestamp": 0.3416278686155963
+            "x": 4.605249336717623,
+            "y": 6.421284904520194,
+            "heading": 0.17227303630641458,
+            "angularVelocity": -0.4108451001767527,
+            "velocityX": -2.673088353744534,
+            "velocityY": -0.9775089297101582,
+            "timestamp": 0.3416277242404205
         },
         {
-            "x": 4.319874242645404,
-            "y": 6.316927450773692,
-            "heading": -3.011676691839701,
-            "angularVelocity": -0.513869833781204,
-            "velocityX": -3.3413581693199683,
-            "velocityY": -1.2218851907785282,
-            "timestamp": 0.42703483576949536
+            "x": 4.319873982796504,
+            "y": 6.316927367787122,
+            "heading": 0.12841583542221255,
+            "angularVelocity": -0.5135086853002309,
+            "velocityX": -3.341360594262375,
+            "velocityY": -1.22188603943198,
+            "timestamp": 0.4270346553005256
         },
         {
-            "x": 4.016437344871394,
-            "y": 6.205964133135793,
-            "heading": -3.0116766970908038,
-            "angularVelocity": -6.148330724628331e-8,
-            "velocityX": -3.5528354171297467,
-            "velocityY": -1.2992302775246547,
-            "timestamp": 0.5124418029233945
+            "x": 4.016437213590231,
+            "y": 6.205964096134007,
+            "heading": 0.12841583017477892,
+            "angularVelocity": -6.144037201192911e-8,
+            "velocityX": -3.552835413237479,
+            "velocityY": -1.2992302881721054,
+            "timestamp": 0.5124415863606308
         },
         {
-            "x": 3.7130004778936962,
-            "y": 6.095000731283241,
-            "heading": -3.011676702341878,
-            "angularVelocity": -6.148297185895772e-8,
-            "velocityX": -3.552835056546619,
-            "velocityY": -1.299231263564264,
-            "timestamp": 0.5978487700772935
+            "x": 3.7130004751736263,
+            "y": 6.095000740284403,
+            "heading": 0.12841582492737386,
+            "angularVelocity": -6.144003768621477e-8,
+            "velocityX": -3.5528350527319725,
+            "velocityY": -1.2992312739994434,
+            "timestamp": 0.5978485174207359
         },
         {
-            "x": 3.4095636397411098,
-            "y": 5.984037250606526,
-            "heading": -3.011676707592942,
-            "angularVelocity": -6.148285239694146e-8,
-            "velocityX": -3.5528347190435716,
-            "velocityY": -1.2992321864885386,
-            "timestamp": 0.6832557372311926
+            "x": 3.409563765577162,
+            "y": 5.984037305624233,
+            "heading": 0.12841581967997912,
+            "angularVelocity": -6.143991704551595e-8,
+            "velocityX": -3.5528347152869912,
+            "velocityY": -1.2992321967649234,
+            "timestamp": 0.683255448480841
         },
         {
-            "x": 3.106126829490984,
-            "y": 5.873073693628802,
-            "heading": -3.0116767128440194,
-            "angularVelocity": -6.14830072681285e-8,
-            "velocityX": -3.5528343923435184,
-            "velocityY": -1.2992330798700915,
-            "timestamp": 0.7686627043850917
+            "x": 3.1061270838779644,
+            "y": 5.873073794677257,
+            "heading": 0.12841581443257097,
+            "angularVelocity": -6.1440073698458e-8,
+            "velocityX": -3.5528343886476135,
+            "velocityY": -1.2992330899805498,
+            "timestamp": 0.7686623795409461
         },
         {
-            "x": 2.8207512525311835,
-            "y": 5.768715824967853,
-            "heading": -3.055515171895212,
-            "angularVelocity": -0.5132890267862834,
-            "velocityX": -3.3413617936528297,
-            "velocityY": -1.221889409477577,
-            "timestamp": 0.8540696715389907
+            "x": 2.8207514220046077,
+            "y": 5.76871589253347,
+            "heading": 0.08460828566897327,
+            "angularVelocity": -0.512927091745846,
+            "velocityX": -3.3413641999678414,
+            "velocityY": -1.2218903178987286,
+            "timestamp": 0.8540693106010512
         },
         {
-            "x": 2.5924507605120093,
-            "y": 5.68522951062146,
-            "heading": -3.090584080219249,
-            "angularVelocity": -0.4106094560276795,
-            "velocityX": -2.6730897914661687,
-            "velocityY": -0.9775117549362862,
-            "timestamp": 0.9394766386928898
+            "x": 2.592450862147759,
+            "y": 5.685229551240001,
+            "heading": 0.0495641189011112,
+            "angularVelocity": -0.4103199392939169,
+            "velocityX": -2.673091715427432,
+            "velocityY": -0.9775124835561102,
+            "timestamp": 0.9394762416611563
         },
         {
-            "x": 2.421225380941837,
-            "y": 5.6226147624914855,
-            "heading": -3.116884817520286,
-            "angularVelocity": -0.3079460397375414,
-            "velocityX": -2.004817467193667,
-            "velocityY": -0.7331339610402823,
-            "timestamp": 1.024883605846789
+            "x": 2.421225431748781,
+            "y": 5.622614782821321,
+            "heading": 0.02328193601640482,
+            "angularVelocity": -0.30772892268205126,
+            "velocityX": -2.004818909585662,
+            "velocityY": -0.7331345084231428,
+            "timestamp": 1.0248831727212615
         },
         {
-            "x": 2.307075126087855,
-            "y": 5.580871589665102,
-            "heading": -3.1344184120174683,
-            "angularVelocity": -0.20529466250203854,
-            "velocityX": -1.3365449992889762,
-            "velocityY": -0.48875606074577516,
-            "timestamp": 1.110290573000688
+            "x": 2.3070751430245986,
+            "y": 5.580871596442558,
+            "heading": 0.005760710543037936,
+            "angularVelocity": -0.2051499246710589,
+            "velocityX": -1.3365459607001804,
+            "velocityY": -0.48875642597890145,
+            "timestamp": 1.1102901037813666
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.10264847768590626,
-            "velocityX": -0.6682724839650215,
-            "velocityY": -0.24437806844836182,
-            "timestamp": 1.195697540154587
+            "heading": -0.003,
+            "angularVelocity": -0.10257610751605846,
+            "velocityX": -0.6682729646898584,
+            "velocityY": -0.24437825107975505,
+            "timestamp": 1.1956970348414717
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0,
-            "velocityX": -3.560921302709756e-34,
-            "velocityY": 7.627797313302701e-34,
-            "timestamp": 1.2811045073084861
+            "heading": -0.003,
+            "angularVelocity": -5.146817856452735e-35,
+            "velocityX": 4.663929716319662e-34,
+            "velocityY": 0,
+            "timestamp": 1.2811039659015768
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.traj
index 75b837e7..6bf8a7f8 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.traj	
@@ -3,146 +3,146 @@
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": -2.88,
+            "heading": 0.26,
             "angularVelocity": 0,
-            "velocityX": -8.07520618943781e-35,
-            "velocityY": -1.6093142036511268e-34,
+            "velocityX": -1.9284905870694258e-35,
+            "velocityY": -6.933375493218394e-34,
             "timestamp": 0
         },
         {
-            "x": 5.118924952955758,
-            "y": 6.609128496946765,
-            "heading": -2.888779485010293,
-            "angularVelocity": -0.10279588776958423,
-            "velocityX": -0.6682715584712982,
-            "velocityY": -0.24437705434061835,
-            "timestamp": 0.08540696715389907
+            "x": 5.118924935599245,
+            "y": 6.609128491648739,
+            "heading": 0.2512266763512042,
+            "angularVelocity": -0.10272378997697001,
+            "velocityX": -0.6682720441106688,
+            "velocityY": -0.24437721964946355,
+            "timestamp": 0.08540693106010512
         },
         {
-            "x": 5.004774858765426,
-            "y": 6.567385488245384,
-            "heading": -2.906338067516999,
-            "angularVelocity": -0.20558723827608577,
-            "velocityX": -1.3365431181350638,
-            "velocityY": -0.48875413906409965,
-            "timestamp": 0.17081393430779815
+            "x": 5.004774806713452,
+            "y": 6.567385472222137,
+            "heading": 0.23368042064504363,
+            "angularVelocity": -0.2054429949463047,
+            "velocityX": -1.3365440892081688,
+            "velocityY": -0.4887544711942054,
+            "timestamp": 0.17081386212021024
         },
         {
-            "x": 4.833549713525512,
-            "y": 6.504770974662745,
-            "heading": -2.9326749312069125,
-            "angularVelocity": -0.3083690308596943,
-            "velocityX": -2.004814723503466,
-            "velocityY": -0.7331312148084055,
-            "timestamp": 0.25622090146169724
+            "x": 4.833549609463453,
+            "y": 6.504770942290587,
+            "heading": 0.2073620554535925,
+            "angularVelocity": -0.3081525687058067,
+            "velocityX": -2.0048161797255077,
+            "velocityY": -0.7331317160604409,
+            "timestamp": 0.2562207931803154
         },
         {
-            "x": 4.6052495100619275,
-            "y": 6.421284959128349,
-            "heading": -2.96778862782457,
-            "angularVelocity": -0.4111338663318194,
-            "velocityX": -2.673086412870736,
-            "velocityY": -0.9775082562521819,
-            "timestamp": 0.3416278686155963
+            "x": 4.605249336717623,
+            "y": 6.421284904520194,
+            "heading": 0.17227303630641458,
+            "angularVelocity": -0.4108451001767527,
+            "velocityX": -2.673088353744534,
+            "velocityY": -0.9775089297101582,
+            "timestamp": 0.3416277242404205
         },
         {
-            "x": 4.319874242645404,
-            "y": 6.316927450773692,
-            "heading": -3.011676691839701,
-            "angularVelocity": -0.513869833781204,
-            "velocityX": -3.3413581693199683,
-            "velocityY": -1.2218851907785282,
-            "timestamp": 0.42703483576949536
+            "x": 4.319873982796504,
+            "y": 6.316927367787122,
+            "heading": 0.12841583542221255,
+            "angularVelocity": -0.5135086853002309,
+            "velocityX": -3.341360594262375,
+            "velocityY": -1.22188603943198,
+            "timestamp": 0.4270346553005256
         },
         {
-            "x": 4.016437344871394,
-            "y": 6.205964133135793,
-            "heading": -3.0116766970908038,
-            "angularVelocity": -6.148330724628331e-8,
-            "velocityX": -3.5528354171297467,
-            "velocityY": -1.2992302775246547,
-            "timestamp": 0.5124418029233945
+            "x": 4.016437213590231,
+            "y": 6.205964096134007,
+            "heading": 0.12841583017477892,
+            "angularVelocity": -6.144037201192911e-8,
+            "velocityX": -3.552835413237479,
+            "velocityY": -1.2992302881721054,
+            "timestamp": 0.5124415863606308
         },
         {
-            "x": 3.7130004778936962,
-            "y": 6.095000731283241,
-            "heading": -3.011676702341878,
-            "angularVelocity": -6.148297185895772e-8,
-            "velocityX": -3.552835056546619,
-            "velocityY": -1.299231263564264,
-            "timestamp": 0.5978487700772935
+            "x": 3.7130004751736263,
+            "y": 6.095000740284403,
+            "heading": 0.12841582492737386,
+            "angularVelocity": -6.144003768621477e-8,
+            "velocityX": -3.5528350527319725,
+            "velocityY": -1.2992312739994434,
+            "timestamp": 0.5978485174207359
         },
         {
-            "x": 3.4095636397411098,
-            "y": 5.984037250606526,
-            "heading": -3.011676707592942,
-            "angularVelocity": -6.148285239694146e-8,
-            "velocityX": -3.5528347190435716,
-            "velocityY": -1.2992321864885386,
-            "timestamp": 0.6832557372311926
+            "x": 3.409563765577162,
+            "y": 5.984037305624233,
+            "heading": 0.12841581967997912,
+            "angularVelocity": -6.143991704551595e-8,
+            "velocityX": -3.5528347152869912,
+            "velocityY": -1.2992321967649234,
+            "timestamp": 0.683255448480841
         },
         {
-            "x": 3.106126829490984,
-            "y": 5.873073693628802,
-            "heading": -3.0116767128440194,
-            "angularVelocity": -6.14830072681285e-8,
-            "velocityX": -3.5528343923435184,
-            "velocityY": -1.2992330798700915,
-            "timestamp": 0.7686627043850917
+            "x": 3.1061270838779644,
+            "y": 5.873073794677257,
+            "heading": 0.12841581443257097,
+            "angularVelocity": -6.1440073698458e-8,
+            "velocityX": -3.5528343886476135,
+            "velocityY": -1.2992330899805498,
+            "timestamp": 0.7686623795409461
         },
         {
-            "x": 2.8207512525311835,
-            "y": 5.768715824967853,
-            "heading": -3.055515171895212,
-            "angularVelocity": -0.5132890267862834,
-            "velocityX": -3.3413617936528297,
-            "velocityY": -1.221889409477577,
-            "timestamp": 0.8540696715389907
+            "x": 2.8207514220046077,
+            "y": 5.76871589253347,
+            "heading": 0.08460828566897327,
+            "angularVelocity": -0.512927091745846,
+            "velocityX": -3.3413641999678414,
+            "velocityY": -1.2218903178987286,
+            "timestamp": 0.8540693106010512
         },
         {
-            "x": 2.5924507605120093,
-            "y": 5.68522951062146,
-            "heading": -3.090584080219249,
-            "angularVelocity": -0.4106094560276795,
-            "velocityX": -2.6730897914661687,
-            "velocityY": -0.9775117549362862,
-            "timestamp": 0.9394766386928898
+            "x": 2.592450862147759,
+            "y": 5.685229551240001,
+            "heading": 0.0495641189011112,
+            "angularVelocity": -0.4103199392939169,
+            "velocityX": -2.673091715427432,
+            "velocityY": -0.9775124835561102,
+            "timestamp": 0.9394762416611563
         },
         {
-            "x": 2.421225380941837,
-            "y": 5.6226147624914855,
-            "heading": -3.116884817520286,
-            "angularVelocity": -0.3079460397375414,
-            "velocityX": -2.004817467193667,
-            "velocityY": -0.7331339610402823,
-            "timestamp": 1.024883605846789
+            "x": 2.421225431748781,
+            "y": 5.622614782821321,
+            "heading": 0.02328193601640482,
+            "angularVelocity": -0.30772892268205126,
+            "velocityX": -2.004818909585662,
+            "velocityY": -0.7331345084231428,
+            "timestamp": 1.0248831727212615
         },
         {
-            "x": 2.307075126087855,
-            "y": 5.580871589665102,
-            "heading": -3.1344184120174683,
-            "angularVelocity": -0.20529466250203854,
-            "velocityX": -1.3365449992889762,
-            "velocityY": -0.48875606074577516,
-            "timestamp": 1.110290573000688
+            "x": 2.3070751430245986,
+            "y": 5.580871596442558,
+            "heading": 0.005760710543037936,
+            "angularVelocity": -0.2051499246710589,
+            "velocityX": -1.3365459607001804,
+            "velocityY": -0.48875642597890145,
+            "timestamp": 1.1102901037813666
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.10264847768590626,
-            "velocityX": -0.6682724839650215,
-            "velocityY": -0.24437806844836182,
-            "timestamp": 1.195697540154587
+            "heading": -0.003,
+            "angularVelocity": -0.10257610751605846,
+            "velocityX": -0.6682729646898584,
+            "velocityY": -0.24437825107975505,
+            "timestamp": 1.1956970348414717
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0,
-            "velocityX": -3.560921302709756e-34,
-            "velocityY": 7.627797313302701e-34,
-            "timestamp": 1.2811045073084861
+            "heading": -0.003,
+            "angularVelocity": -5.146817856452735e-35,
+            "velocityX": 4.663929716319662e-34,
+            "velocityY": 0,
+            "timestamp": 1.2811039659015768
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.1.traj
index a50593ec..60b69798 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.1.traj	
@@ -3,146 +3,146 @@
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 7.124511967920712e-30,
-            "velocityY": -7.12047614876129e-30,
+            "heading": -5.481945073939837e-30,
+            "angularVelocity": 8.103891066426341e-30,
+            "velocityX": -1.0115776003849141e-29,
+            "velocityY": 1.8597851705411875e-30,
             "timestamp": 0
         },
         {
-            "x": 4.30430205284905,
-            "y": 5.6386850261363115,
-            "heading": 3.0976627193075768,
-            "angularVelocity": -0.4082995079820314,
-            "velocityX": 0.3983158006634516,
-            "velocityY": 0.758836111615579,
-            "timestamp": 0.10369172596279416
+            "x": 4.304265157789358,
+            "y": 5.638698398929109,
+            "heading": -0.04261689056565391,
+            "angularVelocity": -0.41098438981919094,
+            "velocityX": 0.39794868817859114,
+            "velocityY": 0.7589435323492035,
+            "timestamp": 0.10369466972795005
         },
         {
-            "x": 4.388754424241646,
-            "y": 5.795256557860491,
-            "heading": 3.0179562679096885,
-            "angularVelocity": -0.7686867072352654,
-            "velocityX": 0.8144562221183627,
-            "velocityY": 1.5099713142045914,
-            "timestamp": 0.20738345192558832
+            "x": 4.388659791773407,
+            "y": 5.795293123128996,
+            "heading": -0.12280542510902584,
+            "angularVelocity": -0.7733139490558338,
+            "velocityX": 0.8138762986126182,
+            "velocityY": 1.5101521091752137,
+            "timestamp": 0.2073893394559001
         },
         {
-            "x": 4.5199632474011375,
-            "y": 6.027943066067755,
-            "heading": 2.911091818022606,
-            "angularVelocity": -1.030597657574192,
-            "velocityX": 1.265374087866657,
-            "velocityY": 2.2440219414493656,
-            "timestamp": 0.31107517788838246
+            "x": 4.519821289569113,
+            "y": 6.028001369445331,
+            "heading": -0.2301897087601478,
+            "angularVelocity": -1.035581519598341,
+            "velocityX": 1.2648817739601788,
+            "velocityY": 2.244167872146774,
+            "timestamp": 0.31108400918385015
         },
         {
-            "x": 4.707776355466448,
-            "y": 6.330145783951522,
-            "heading": 2.807461071652614,
-            "angularVelocity": -0.9994119145743484,
-            "velocityX": 1.8112641709976698,
-            "velocityY": 2.914434252856334,
-            "timestamp": 0.41476690385117665
+            "x": 4.707676223581395,
+            "y": 6.330157885665521,
+            "heading": -0.33387515355278086,
+            "angularVelocity": -0.9999110375168158,
+            "velocityX": 1.811616108187066,
+            "velocityY": 2.9139059607683615,
+            "timestamp": 0.4147786789118002
         },
         {
-            "x": 4.978743687535899,
-            "y": 6.613772866928766,
-            "heading": 2.807461069210919,
-            "angularVelocity": -2.3547634644306893e-8,
-            "velocityX": 2.613201097324575,
-            "velocityY": 2.7352913681754196,
-            "timestamp": 0.5184586298139708
+            "x": 4.978690084088412,
+            "y": 6.613755913169546,
+            "heading": -0.33387515567200693,
+            "angularVelocity": -2.0437175255333274e-8,
+            "velocityX": 2.613575617898612,
+            "velocityY": 2.734933514403967,
+            "timestamp": 0.5184733486397503
         },
         {
-            "x": 5.307032554871728,
-            "y": 6.82846808577305,
-            "heading": 2.80746110336737,
-            "angularVelocity": 3.2940382214109427e-7,
-            "velocityX": 3.166008322145415,
-            "velocityY": 2.0705144682548786,
-            "timestamp": 0.622150355776765
+            "x": 5.307019069498364,
+            "y": 6.828410126616754,
+            "heading": -0.33387512130937075,
+            "angularVelocity": 3.313828600417541e-7,
+            "velocityX": 3.1663053295925763,
+            "velocityY": 2.0700602452408625,
+            "timestamp": 0.6221680183677003
         },
         {
-            "x": 5.675494511223277,
-            "y": 6.963016991691847,
-            "heading": 2.8074611309590716,
-            "angularVelocity": 2.660935682689419e-7,
-            "velocityX": 3.5534364283198543,
-            "velocityY": 1.2975857491953324,
-            "timestamp": 0.7258420817395592
+            "x": 5.675511663962328,
+            "y": 6.962907583051813,
+            "heading": -0.33387509354517536,
+            "angularVelocity": 2.6774949424459703e-7,
+            "velocityX": 3.5536310152752564,
+            "velocityY": 1.2970527490748707,
+            "timestamp": 0.7258626880956504
         },
         {
-            "x": 6.055822077771958,
-            "y": 7.05903034533374,
-            "heading": 2.8074611577219364,
-            "angularVelocity": 2.5810029119060704e-7,
-            "velocityX": 3.6678680291727868,
-            "velocityY": 0.9259499998712609,
-            "timestamp": 0.8295338077023534
+            "x": 6.0558437987294225,
+            "y": 7.05894833386441,
+            "heading": -0.33387506661120775,
+            "angularVelocity": 2.597430292697957e-7,
+            "velocityX": 3.6678079573899045,
+            "velocityY": 0.9261879232999765,
+            "timestamp": 0.8295573578236004
         },
         {
-            "x": 6.436149809853972,
-            "y": 7.155043043263379,
-            "heading": 2.807461184484807,
-            "angularVelocity": 2.58100350606906e-7,
-            "velocityX": 3.6678696255714556,
-            "velocityY": 0.9259436762012891,
-            "timestamp": 0.9332255336651476
+            "x": 6.43617610221841,
+            "y": 7.15498841651687,
+            "heading": -0.333875039677234,
+            "angularVelocity": 2.5974308807396863e-7,
+            "velocityX": 3.667809584492749,
+            "velocityY": 0.9261814797658935,
+            "timestamp": 0.9332520275515505
         },
         {
-            "x": 6.816477543748484,
-            "y": 7.2510557340128905,
-            "heading": 2.8074612112477313,
-            "angularVelocity": 2.581008643735431e-7,
-            "velocityX": 3.667869643051154,
-            "velocityY": 0.9259436069563147,
-            "timestamp": 1.0369172596279417
+            "x": 6.8165084241422855,
+            "y": 7.251028426164244,
+            "heading": -0.33387501274320475,
+            "angularVelocity": 2.5974362337794873e-7,
+            "velocityX": 3.667809762273227,
+            "velocityY": 0.9261807757269309,
+            "timestamp": 1.0369466972795005
         },
         {
-            "x": 7.162445200777026,
-            "y": 7.337344185673276,
-            "heading": 2.898451236968193,
-            "angularVelocity": 0.8775051709825819,
-            "velocityX": 3.3365020575767024,
-            "velocityY": 0.8321633270078459,
-            "timestamp": 1.1406089855907358
+            "x": 7.162466794959021,
+            "y": 7.337325744518129,
+            "heading": -0.24283225626293373,
+            "angularVelocity": 0.8779887791641197,
+            "velocityX": 3.3363177849390095,
+            "velocityY": 0.8322252106139182,
+            "timestamp": 1.1406413670074507
         },
         {
-            "x": 7.421534649307807,
-            "y": 7.402207433353085,
-            "heading": 3.009161772576331,
-            "angularVelocity": 1.0676891968008626,
-            "velocityX": 2.498651132721501,
-            "velocityY": 0.6255392807626672,
-            "timestamp": 1.2443007115535298
+            "x": 7.421550082994963,
+            "y": 7.402199028754473,
+            "heading": -0.13158720138626476,
+            "angularVelocity": 1.0728136284004517,
+            "velocityX": 2.498520789117373,
+            "velocityY": 0.6256183119782293,
+            "timestamp": 1.2443360367354006
         },
         {
-            "x": 7.593924390045372,
-            "y": 7.445415282758517,
-            "heading": 3.094199643650422,
-            "angularVelocity": 0.8201027640778757,
-            "velocityX": 1.6625216634876054,
-            "velocityY": 0.416695247419558,
-            "timestamp": 1.347992437516324
+            "x": 7.5939307756893495,
+            "y": 7.445412774465034,
+            "heading": -0.046073125428300596,
+            "angularVelocity": 0.8246718580841027,
+            "velocityX": 1.6623872099370305,
+            "velocityY": 0.41674028013136566,
+            "timestamp": 1.3480307064633505
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 0.4416973092532584,
-            "velocityX": 0.8301106877660893,
-            "velocityY": 0.20816238751031255,
-            "timestamp": 1.451684163479118
+            "heading": 6.33230436519987e-31,
+            "angularVelocity": 0.444315272416587,
+            "velocityX": 0.8300255407192906,
+            "velocityY": 0.2081806672570287,
+            "timestamp": 1.4517253761913005
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
-            "velocityX": -4.6860665144292015e-31,
-            "velocityY": 3.7247780913146357e-28,
-            "timestamp": 1.555375889441912
+            "velocityX": 2.6078100454164866e-31,
+            "velocityY": 7.556791844119902e-29,
+            "timestamp": 1.5554200459192504
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.traj
index a50593ec..60b69798 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.traj	
@@ -3,146 +3,146 @@
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 7.124511967920712e-30,
-            "velocityY": -7.12047614876129e-30,
+            "heading": -5.481945073939837e-30,
+            "angularVelocity": 8.103891066426341e-30,
+            "velocityX": -1.0115776003849141e-29,
+            "velocityY": 1.8597851705411875e-30,
             "timestamp": 0
         },
         {
-            "x": 4.30430205284905,
-            "y": 5.6386850261363115,
-            "heading": 3.0976627193075768,
-            "angularVelocity": -0.4082995079820314,
-            "velocityX": 0.3983158006634516,
-            "velocityY": 0.758836111615579,
-            "timestamp": 0.10369172596279416
+            "x": 4.304265157789358,
+            "y": 5.638698398929109,
+            "heading": -0.04261689056565391,
+            "angularVelocity": -0.41098438981919094,
+            "velocityX": 0.39794868817859114,
+            "velocityY": 0.7589435323492035,
+            "timestamp": 0.10369466972795005
         },
         {
-            "x": 4.388754424241646,
-            "y": 5.795256557860491,
-            "heading": 3.0179562679096885,
-            "angularVelocity": -0.7686867072352654,
-            "velocityX": 0.8144562221183627,
-            "velocityY": 1.5099713142045914,
-            "timestamp": 0.20738345192558832
+            "x": 4.388659791773407,
+            "y": 5.795293123128996,
+            "heading": -0.12280542510902584,
+            "angularVelocity": -0.7733139490558338,
+            "velocityX": 0.8138762986126182,
+            "velocityY": 1.5101521091752137,
+            "timestamp": 0.2073893394559001
         },
         {
-            "x": 4.5199632474011375,
-            "y": 6.027943066067755,
-            "heading": 2.911091818022606,
-            "angularVelocity": -1.030597657574192,
-            "velocityX": 1.265374087866657,
-            "velocityY": 2.2440219414493656,
-            "timestamp": 0.31107517788838246
+            "x": 4.519821289569113,
+            "y": 6.028001369445331,
+            "heading": -0.2301897087601478,
+            "angularVelocity": -1.035581519598341,
+            "velocityX": 1.2648817739601788,
+            "velocityY": 2.244167872146774,
+            "timestamp": 0.31108400918385015
         },
         {
-            "x": 4.707776355466448,
-            "y": 6.330145783951522,
-            "heading": 2.807461071652614,
-            "angularVelocity": -0.9994119145743484,
-            "velocityX": 1.8112641709976698,
-            "velocityY": 2.914434252856334,
-            "timestamp": 0.41476690385117665
+            "x": 4.707676223581395,
+            "y": 6.330157885665521,
+            "heading": -0.33387515355278086,
+            "angularVelocity": -0.9999110375168158,
+            "velocityX": 1.811616108187066,
+            "velocityY": 2.9139059607683615,
+            "timestamp": 0.4147786789118002
         },
         {
-            "x": 4.978743687535899,
-            "y": 6.613772866928766,
-            "heading": 2.807461069210919,
-            "angularVelocity": -2.3547634644306893e-8,
-            "velocityX": 2.613201097324575,
-            "velocityY": 2.7352913681754196,
-            "timestamp": 0.5184586298139708
+            "x": 4.978690084088412,
+            "y": 6.613755913169546,
+            "heading": -0.33387515567200693,
+            "angularVelocity": -2.0437175255333274e-8,
+            "velocityX": 2.613575617898612,
+            "velocityY": 2.734933514403967,
+            "timestamp": 0.5184733486397503
         },
         {
-            "x": 5.307032554871728,
-            "y": 6.82846808577305,
-            "heading": 2.80746110336737,
-            "angularVelocity": 3.2940382214109427e-7,
-            "velocityX": 3.166008322145415,
-            "velocityY": 2.0705144682548786,
-            "timestamp": 0.622150355776765
+            "x": 5.307019069498364,
+            "y": 6.828410126616754,
+            "heading": -0.33387512130937075,
+            "angularVelocity": 3.313828600417541e-7,
+            "velocityX": 3.1663053295925763,
+            "velocityY": 2.0700602452408625,
+            "timestamp": 0.6221680183677003
         },
         {
-            "x": 5.675494511223277,
-            "y": 6.963016991691847,
-            "heading": 2.8074611309590716,
-            "angularVelocity": 2.660935682689419e-7,
-            "velocityX": 3.5534364283198543,
-            "velocityY": 1.2975857491953324,
-            "timestamp": 0.7258420817395592
+            "x": 5.675511663962328,
+            "y": 6.962907583051813,
+            "heading": -0.33387509354517536,
+            "angularVelocity": 2.6774949424459703e-7,
+            "velocityX": 3.5536310152752564,
+            "velocityY": 1.2970527490748707,
+            "timestamp": 0.7258626880956504
         },
         {
-            "x": 6.055822077771958,
-            "y": 7.05903034533374,
-            "heading": 2.8074611577219364,
-            "angularVelocity": 2.5810029119060704e-7,
-            "velocityX": 3.6678680291727868,
-            "velocityY": 0.9259499998712609,
-            "timestamp": 0.8295338077023534
+            "x": 6.0558437987294225,
+            "y": 7.05894833386441,
+            "heading": -0.33387506661120775,
+            "angularVelocity": 2.597430292697957e-7,
+            "velocityX": 3.6678079573899045,
+            "velocityY": 0.9261879232999765,
+            "timestamp": 0.8295573578236004
         },
         {
-            "x": 6.436149809853972,
-            "y": 7.155043043263379,
-            "heading": 2.807461184484807,
-            "angularVelocity": 2.58100350606906e-7,
-            "velocityX": 3.6678696255714556,
-            "velocityY": 0.9259436762012891,
-            "timestamp": 0.9332255336651476
+            "x": 6.43617610221841,
+            "y": 7.15498841651687,
+            "heading": -0.333875039677234,
+            "angularVelocity": 2.5974308807396863e-7,
+            "velocityX": 3.667809584492749,
+            "velocityY": 0.9261814797658935,
+            "timestamp": 0.9332520275515505
         },
         {
-            "x": 6.816477543748484,
-            "y": 7.2510557340128905,
-            "heading": 2.8074612112477313,
-            "angularVelocity": 2.581008643735431e-7,
-            "velocityX": 3.667869643051154,
-            "velocityY": 0.9259436069563147,
-            "timestamp": 1.0369172596279417
+            "x": 6.8165084241422855,
+            "y": 7.251028426164244,
+            "heading": -0.33387501274320475,
+            "angularVelocity": 2.5974362337794873e-7,
+            "velocityX": 3.667809762273227,
+            "velocityY": 0.9261807757269309,
+            "timestamp": 1.0369466972795005
         },
         {
-            "x": 7.162445200777026,
-            "y": 7.337344185673276,
-            "heading": 2.898451236968193,
-            "angularVelocity": 0.8775051709825819,
-            "velocityX": 3.3365020575767024,
-            "velocityY": 0.8321633270078459,
-            "timestamp": 1.1406089855907358
+            "x": 7.162466794959021,
+            "y": 7.337325744518129,
+            "heading": -0.24283225626293373,
+            "angularVelocity": 0.8779887791641197,
+            "velocityX": 3.3363177849390095,
+            "velocityY": 0.8322252106139182,
+            "timestamp": 1.1406413670074507
         },
         {
-            "x": 7.421534649307807,
-            "y": 7.402207433353085,
-            "heading": 3.009161772576331,
-            "angularVelocity": 1.0676891968008626,
-            "velocityX": 2.498651132721501,
-            "velocityY": 0.6255392807626672,
-            "timestamp": 1.2443007115535298
+            "x": 7.421550082994963,
+            "y": 7.402199028754473,
+            "heading": -0.13158720138626476,
+            "angularVelocity": 1.0728136284004517,
+            "velocityX": 2.498520789117373,
+            "velocityY": 0.6256183119782293,
+            "timestamp": 1.2443360367354006
         },
         {
-            "x": 7.593924390045372,
-            "y": 7.445415282758517,
-            "heading": 3.094199643650422,
-            "angularVelocity": 0.8201027640778757,
-            "velocityX": 1.6625216634876054,
-            "velocityY": 0.416695247419558,
-            "timestamp": 1.347992437516324
+            "x": 7.5939307756893495,
+            "y": 7.445412774465034,
+            "heading": -0.046073125428300596,
+            "angularVelocity": 0.8246718580841027,
+            "velocityX": 1.6623872099370305,
+            "velocityY": 0.41674028013136566,
+            "timestamp": 1.3480307064633505
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 0.4416973092532584,
-            "velocityX": 0.8301106877660893,
-            "velocityY": 0.20816238751031255,
-            "timestamp": 1.451684163479118
+            "heading": 6.33230436519987e-31,
+            "angularVelocity": 0.444315272416587,
+            "velocityX": 0.8300255407192906,
+            "velocityY": 0.2081806672570287,
+            "timestamp": 1.4517253761913005
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
-            "velocityX": -4.6860665144292015e-31,
-            "velocityY": 3.7247780913146357e-28,
-            "timestamp": 1.555375889441912
+            "velocityX": 2.6078100454164866e-31,
+            "velocityY": 7.556791844119902e-29,
+            "timestamp": 1.5554200459192504
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.1.traj
index d6c47c76..2b663e69 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.1.traj	
@@ -3,146 +3,146 @@
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -3.269880825493717e-29,
-            "velocityX": -1.6106277248271234e-30,
-            "velocityY": 0,
+            "heading": 0,
+            "angularVelocity": -9.522541691773037e-34,
+            "velocityX": 0,
+            "velocityY": -4.2308696063129985e-35,
             "timestamp": 0
         },
         {
-            "x": 4.257789422547788,
-            "y": 5.465225005289041,
-            "heading": 3.082306365857339,
-            "angularVelocity": -0.5369837298016524,
-            "velocityX": -0.04849747041052485,
-            "velocityY": -0.882118641130872,
-            "timestamp": 0.1074401903461163
+            "x": 4.271262994735721,
+            "y": 5.464085078742271,
+            "heading": 0.013066573413208545,
+            "angularVelocity": 0.12156077000957308,
+            "velocityX": 0.0768721814736735,
+            "velocityY": -0.8923144052220247,
+            "timestamp": 0.10749005137249076
         },
         {
-            "x": 4.273795168733638,
-            "y": 5.277874820636679,
-            "heading": 2.968634839381067,
-            "angularVelocity": -1.057998185875108,
-            "velocityX": 0.14897354643815672,
-            "velocityY": -1.7437625905987038,
-            "timestamp": 0.2148803806922326
+            "x": 4.301424317332477,
+            "y": 5.27437671657762,
+            "heading": 0.033784324267673776,
+            "angularVelocity": 0.192741101059399,
+            "velocityX": 0.2805964106597774,
+            "velocityY": -1.7648922829820315,
+            "timestamp": 0.21498010274498153
         },
         {
-            "x": 4.354522429535891,
-            "y": 5.020377016639425,
-            "heading": 2.810559381522931,
-            "angularVelocity": -1.471287954245977,
-            "velocityX": 0.7513693017686587,
-            "velocityY": -2.396661837322988,
-            "timestamp": 0.3223205710383489
+            "x": 4.379013311524821,
+            "y": 5.000817118183451,
+            "heading": 0.051063906614390905,
+            "angularVelocity": 0.16075517804747627,
+            "velocityX": 0.7218248870629933,
+            "velocityY": -2.544975975927198,
+            "timestamp": 0.32247015411747226
         },
         {
-            "x": 4.529578110420142,
-            "y": 4.7442561245273325,
-            "heading": 2.6367348361471494,
-            "angularVelocity": -1.6178726491066266,
-            "velocityX": 1.6293314477600391,
-            "velocityY": -2.5699963042002807,
-            "timestamp": 0.4297607613844652
+            "x": 4.5426840546799285,
+            "y": 4.684552935799795,
+            "heading": 0.043724640930798246,
+            "angularVelocity": -0.06827855778168279,
+            "velocityX": 1.5226594560638123,
+            "velocityY": -2.9422646872470954,
+            "timestamp": 0.42996020548996305
         },
         {
-            "x": 4.793919720832893,
-            "y": 4.487710124588879,
-            "heading": 2.5690751936264027,
-            "angularVelocity": -0.6297423925143462,
-            "velocityX": 2.4603605928208117,
-            "velocityY": -2.3878029172509625,
-            "timestamp": 0.5372009517305815
+            "x": 4.7987371937718075,
+            "y": 4.3933637164950845,
+            "heading": -0.0025480008668042047,
+            "angularVelocity": -0.4304830187237655,
+            "velocityX": 2.382110119238521,
+            "velocityY": -2.7089876280330105,
+            "timestamp": 0.5374502568624538
         },
         {
-            "x": 5.141097516271185,
-            "y": 4.276379117165763,
-            "heading": 2.5690757140293337,
-            "angularVelocity": 0.000004843652357933311,
-            "velocityX": 3.231358715205786,
-            "velocityY": -1.9669641941467053,
-            "timestamp": 0.6446411420766978
+            "x": 5.129552682758233,
+            "y": 4.161939062753506,
+            "heading": -0.00976861092561694,
+            "angularVelocity": -0.06717468236935505,
+            "velocityX": 3.0776382070935524,
+            "velocityY": -2.152986725623672,
+            "timestamp": 0.6449403082349446
         },
         {
-            "x": 5.528234411762885,
-            "y": 4.152611717586108,
-            "heading": 2.5690757988249406,
-            "angularVelocity": 7.892354497964203e-7,
-            "velocityX": 3.6032781982659046,
-            "velocityY": -1.1519655650361502,
-            "timestamp": 0.752081332422814
+            "x": 5.508331065778466,
+            "y": 4.0140416634822165,
+            "heading": -0.009768639154040644,
+            "angularVelocity": -2.6261429169311795e-7,
+            "velocityX": 3.5238459576843355,
+            "velocityY": -1.3759170954228361,
+            "timestamp": 0.7524303596074354
         },
         {
-            "x": 5.933619437652518,
-            "y": 4.123348539284839,
-            "heading": 2.569075852374573,
-            "angularVelocity": 4.984134150731162e-7,
-            "velocityX": 3.773122744698165,
-            "velocityY": -0.2723671487085231,
-            "timestamp": 0.8595215227689303
+            "x": 5.911272135069668,
+            "y": 3.959404581326429,
+            "heading": -0.009768652411608816,
+            "angularVelocity": -1.233376298753571e-7,
+            "velocityX": 3.7486359355701584,
+            "velocityY": -0.5082989677477323,
+            "timestamp": 0.8599204109799262
         },
         {
-            "x": 6.340054515622032,
-            "y": 4.125318524745646,
-            "heading": 2.569075903811562,
-            "angularVelocity": 4.787499761820812e-7,
-            "velocityX": 3.782896108618124,
-            "velocityY": 0.01833564753065316,
-            "timestamp": 0.9669617131150466
+            "x": 6.316087697151271,
+            "y": 3.997759236961094,
+            "heading": -0.009768652034230711,
+            "angularVelocity": 3.51081891151765e-9,
+            "velocityX": 3.7660746916826353,
+            "velocityY": 0.3568205163634492,
+            "timestamp": 0.967410462352417
         },
         {
-            "x": 6.746489593269827,
-            "y": 4.127288577706153,
-            "heading": 2.5690759552486404,
-            "angularVelocity": 4.787508167496477e-7,
-            "velocityX": 3.7828961056237245,
-            "velocityY": 0.01833627578441685,
-            "timestamp": 1.074401903461163
+            "x": 6.720902827559142,
+            "y": 4.036118448432054,
+            "heading": -0.009768651656829377,
+            "angularVelocity": 3.5110350109919825e-9,
+            "velocityX": 3.766070675741356,
+            "velocityY": 0.35686290015837574,
+            "timestamp": 1.0749005137249077
         },
         {
-            "x": 7.118708956476355,
-            "y": 4.125866161972181,
-            "heading": 2.656486209298221,
-            "angularVelocity": 0.8135712880626333,
-            "velocityX": 3.464433207046916,
-            "velocityY": -0.013239140115027938,
-            "timestamp": 1.1818420938072793
+            "x": 7.104541677751467,
+            "y": 4.072471064803872,
+            "heading": -0.005861206189315246,
+            "angularVelocity": 0.03635169411142489,
+            "velocityX": 3.569063790497984,
+            "velocityY": 0.3381951716242375,
+            "timestamp": 1.1823905650973985
         },
         {
-            "x": 7.400847427365447,
-            "y": 4.126314118090732,
-            "heading": 2.8696985577774003,
-            "angularVelocity": 1.9844747835268808,
-            "velocityX": 2.6260049426586924,
-            "velocityY": 0.004169353359354839,
-            "timestamp": 1.2892822841533955
+            "x": 7.392270834070994,
+            "y": 4.099735530737146,
+            "heading": -0.0029306066828158664,
+            "angularVelocity": 0.027263913907193366,
+            "velocityX": 2.6767980166131413,
+            "velocityY": 0.2536464127158502,
+            "timestamp": 1.2898806164698893
         },
         {
-            "x": 7.587357907068713,
-            "y": 4.126749409200305,
-            "heading": 3.0436879317659473,
-            "angularVelocity": 1.6194067920769941,
-            "velocityX": 1.7359470334371772,
-            "velocityY": 0.004051473737811161,
-            "timestamp": 1.3967224744995117
+            "x": 7.584090276967534,
+            "y": 4.117911843086064,
+            "heading": -0.0009768696053991616,
+            "angularVelocity": 0.018175980497453854,
+            "velocityX": 1.7845320608492279,
+            "velocityY": 0.16909762454137037,
+            "timestamp": 1.39737066784238
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.8964249590751834,
-            "velocityX": 0.8622666493129096,
-            "velocityY": 0.002332374867239492,
-            "timestamp": 1.5041626648456279
+            "heading": -6.809535330810639e-32,
+            "angularVelocity": 0.009088000172350516,
+            "velocityX": 0.8922660451626774,
+            "velocityY": 0.08454881915017429,
+            "timestamp": 1.5048607192148709
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 4.233852791919794e-30,
-            "velocityY": -1.3542182850561966e-29,
-            "timestamp": 1.611602855191744
+            "heading": 0,
+            "angularVelocity": -9.489213882041181e-31,
+            "velocityX": 2.198140684032685e-33,
+            "velocityY": -6.756921472052438e-32,
+            "timestamp": 1.6123507705873616
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.traj
index d6c47c76..2b663e69 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.traj	
@@ -3,146 +3,146 @@
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -3.269880825493717e-29,
-            "velocityX": -1.6106277248271234e-30,
-            "velocityY": 0,
+            "heading": 0,
+            "angularVelocity": -9.522541691773037e-34,
+            "velocityX": 0,
+            "velocityY": -4.2308696063129985e-35,
             "timestamp": 0
         },
         {
-            "x": 4.257789422547788,
-            "y": 5.465225005289041,
-            "heading": 3.082306365857339,
-            "angularVelocity": -0.5369837298016524,
-            "velocityX": -0.04849747041052485,
-            "velocityY": -0.882118641130872,
-            "timestamp": 0.1074401903461163
+            "x": 4.271262994735721,
+            "y": 5.464085078742271,
+            "heading": 0.013066573413208545,
+            "angularVelocity": 0.12156077000957308,
+            "velocityX": 0.0768721814736735,
+            "velocityY": -0.8923144052220247,
+            "timestamp": 0.10749005137249076
         },
         {
-            "x": 4.273795168733638,
-            "y": 5.277874820636679,
-            "heading": 2.968634839381067,
-            "angularVelocity": -1.057998185875108,
-            "velocityX": 0.14897354643815672,
-            "velocityY": -1.7437625905987038,
-            "timestamp": 0.2148803806922326
+            "x": 4.301424317332477,
+            "y": 5.27437671657762,
+            "heading": 0.033784324267673776,
+            "angularVelocity": 0.192741101059399,
+            "velocityX": 0.2805964106597774,
+            "velocityY": -1.7648922829820315,
+            "timestamp": 0.21498010274498153
         },
         {
-            "x": 4.354522429535891,
-            "y": 5.020377016639425,
-            "heading": 2.810559381522931,
-            "angularVelocity": -1.471287954245977,
-            "velocityX": 0.7513693017686587,
-            "velocityY": -2.396661837322988,
-            "timestamp": 0.3223205710383489
+            "x": 4.379013311524821,
+            "y": 5.000817118183451,
+            "heading": 0.051063906614390905,
+            "angularVelocity": 0.16075517804747627,
+            "velocityX": 0.7218248870629933,
+            "velocityY": -2.544975975927198,
+            "timestamp": 0.32247015411747226
         },
         {
-            "x": 4.529578110420142,
-            "y": 4.7442561245273325,
-            "heading": 2.6367348361471494,
-            "angularVelocity": -1.6178726491066266,
-            "velocityX": 1.6293314477600391,
-            "velocityY": -2.5699963042002807,
-            "timestamp": 0.4297607613844652
+            "x": 4.5426840546799285,
+            "y": 4.684552935799795,
+            "heading": 0.043724640930798246,
+            "angularVelocity": -0.06827855778168279,
+            "velocityX": 1.5226594560638123,
+            "velocityY": -2.9422646872470954,
+            "timestamp": 0.42996020548996305
         },
         {
-            "x": 4.793919720832893,
-            "y": 4.487710124588879,
-            "heading": 2.5690751936264027,
-            "angularVelocity": -0.6297423925143462,
-            "velocityX": 2.4603605928208117,
-            "velocityY": -2.3878029172509625,
-            "timestamp": 0.5372009517305815
+            "x": 4.7987371937718075,
+            "y": 4.3933637164950845,
+            "heading": -0.0025480008668042047,
+            "angularVelocity": -0.4304830187237655,
+            "velocityX": 2.382110119238521,
+            "velocityY": -2.7089876280330105,
+            "timestamp": 0.5374502568624538
         },
         {
-            "x": 5.141097516271185,
-            "y": 4.276379117165763,
-            "heading": 2.5690757140293337,
-            "angularVelocity": 0.000004843652357933311,
-            "velocityX": 3.231358715205786,
-            "velocityY": -1.9669641941467053,
-            "timestamp": 0.6446411420766978
+            "x": 5.129552682758233,
+            "y": 4.161939062753506,
+            "heading": -0.00976861092561694,
+            "angularVelocity": -0.06717468236935505,
+            "velocityX": 3.0776382070935524,
+            "velocityY": -2.152986725623672,
+            "timestamp": 0.6449403082349446
         },
         {
-            "x": 5.528234411762885,
-            "y": 4.152611717586108,
-            "heading": 2.5690757988249406,
-            "angularVelocity": 7.892354497964203e-7,
-            "velocityX": 3.6032781982659046,
-            "velocityY": -1.1519655650361502,
-            "timestamp": 0.752081332422814
+            "x": 5.508331065778466,
+            "y": 4.0140416634822165,
+            "heading": -0.009768639154040644,
+            "angularVelocity": -2.6261429169311795e-7,
+            "velocityX": 3.5238459576843355,
+            "velocityY": -1.3759170954228361,
+            "timestamp": 0.7524303596074354
         },
         {
-            "x": 5.933619437652518,
-            "y": 4.123348539284839,
-            "heading": 2.569075852374573,
-            "angularVelocity": 4.984134150731162e-7,
-            "velocityX": 3.773122744698165,
-            "velocityY": -0.2723671487085231,
-            "timestamp": 0.8595215227689303
+            "x": 5.911272135069668,
+            "y": 3.959404581326429,
+            "heading": -0.009768652411608816,
+            "angularVelocity": -1.233376298753571e-7,
+            "velocityX": 3.7486359355701584,
+            "velocityY": -0.5082989677477323,
+            "timestamp": 0.8599204109799262
         },
         {
-            "x": 6.340054515622032,
-            "y": 4.125318524745646,
-            "heading": 2.569075903811562,
-            "angularVelocity": 4.787499761820812e-7,
-            "velocityX": 3.782896108618124,
-            "velocityY": 0.01833564753065316,
-            "timestamp": 0.9669617131150466
+            "x": 6.316087697151271,
+            "y": 3.997759236961094,
+            "heading": -0.009768652034230711,
+            "angularVelocity": 3.51081891151765e-9,
+            "velocityX": 3.7660746916826353,
+            "velocityY": 0.3568205163634492,
+            "timestamp": 0.967410462352417
         },
         {
-            "x": 6.746489593269827,
-            "y": 4.127288577706153,
-            "heading": 2.5690759552486404,
-            "angularVelocity": 4.787508167496477e-7,
-            "velocityX": 3.7828961056237245,
-            "velocityY": 0.01833627578441685,
-            "timestamp": 1.074401903461163
+            "x": 6.720902827559142,
+            "y": 4.036118448432054,
+            "heading": -0.009768651656829377,
+            "angularVelocity": 3.5110350109919825e-9,
+            "velocityX": 3.766070675741356,
+            "velocityY": 0.35686290015837574,
+            "timestamp": 1.0749005137249077
         },
         {
-            "x": 7.118708956476355,
-            "y": 4.125866161972181,
-            "heading": 2.656486209298221,
-            "angularVelocity": 0.8135712880626333,
-            "velocityX": 3.464433207046916,
-            "velocityY": -0.013239140115027938,
-            "timestamp": 1.1818420938072793
+            "x": 7.104541677751467,
+            "y": 4.072471064803872,
+            "heading": -0.005861206189315246,
+            "angularVelocity": 0.03635169411142489,
+            "velocityX": 3.569063790497984,
+            "velocityY": 0.3381951716242375,
+            "timestamp": 1.1823905650973985
         },
         {
-            "x": 7.400847427365447,
-            "y": 4.126314118090732,
-            "heading": 2.8696985577774003,
-            "angularVelocity": 1.9844747835268808,
-            "velocityX": 2.6260049426586924,
-            "velocityY": 0.004169353359354839,
-            "timestamp": 1.2892822841533955
+            "x": 7.392270834070994,
+            "y": 4.099735530737146,
+            "heading": -0.0029306066828158664,
+            "angularVelocity": 0.027263913907193366,
+            "velocityX": 2.6767980166131413,
+            "velocityY": 0.2536464127158502,
+            "timestamp": 1.2898806164698893
         },
         {
-            "x": 7.587357907068713,
-            "y": 4.126749409200305,
-            "heading": 3.0436879317659473,
-            "angularVelocity": 1.6194067920769941,
-            "velocityX": 1.7359470334371772,
-            "velocityY": 0.004051473737811161,
-            "timestamp": 1.3967224744995117
+            "x": 7.584090276967534,
+            "y": 4.117911843086064,
+            "heading": -0.0009768696053991616,
+            "angularVelocity": 0.018175980497453854,
+            "velocityX": 1.7845320608492279,
+            "velocityY": 0.16909762454137037,
+            "timestamp": 1.39737066784238
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.8964249590751834,
-            "velocityX": 0.8622666493129096,
-            "velocityY": 0.002332374867239492,
-            "timestamp": 1.5041626648456279
+            "heading": -6.809535330810639e-32,
+            "angularVelocity": 0.009088000172350516,
+            "velocityX": 0.8922660451626774,
+            "velocityY": 0.08454881915017429,
+            "timestamp": 1.5048607192148709
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 4.233852791919794e-30,
-            "velocityY": -1.3542182850561966e-29,
-            "timestamp": 1.611602855191744
+            "heading": 0,
+            "angularVelocity": -9.489213882041181e-31,
+            "velocityX": 2.198140684032685e-33,
+            "velocityY": -6.756921472052438e-32,
+            "timestamp": 1.6123507705873616
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C4.1.traj
new file mode 100644
index 00000000..c13783bc
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C4.1.traj	
@@ -0,0 +1,166 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 7.441191049306378e-32,
+            "velocityX": -6.15829104129807e-34,
+            "velocityY": 2.4252394047543234e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 4.262142393092874,
+            "y": 5.471176724349191,
+            "heading": -0.06580713819401311,
+            "angularVelocity": -0.6311237133024392,
+            "velocityX": -0.008224883662067025,
+            "velocityY": -0.8518601035521746,
+            "timestamp": 0.10426979181255681
+        },
+        {
+            "x": 4.27936690849282,
+            "y": 5.294863623901166,
+            "heading": -0.18680392528158013,
+            "angularVelocity": -1.1604203382805245,
+            "velocityX": 0.16519180771848577,
+            "velocityY": -1.6909317395107013,
+            "timestamp": 0.20853958362511363
+        },
+        {
+            "x": 4.348116425232891,
+            "y": 5.0445566747334585,
+            "heading": -0.3412830267774945,
+            "angularVelocity": -1.4815326549574157,
+            "velocityX": 0.6593426105967491,
+            "velocityY": -2.400570144205118,
+            "timestamp": 0.31280937543767046
+        },
+        {
+            "x": 4.502267029635194,
+            "y": 4.763921050939977,
+            "heading": -0.4878370803946758,
+            "angularVelocity": -1.4055274405902505,
+            "velocityX": 1.4783822018117698,
+            "velocityY": -2.6914374615609895,
+            "timestamp": 0.41707916725022726
+        },
+        {
+            "x": 4.740029978369904,
+            "y": 4.496898841850153,
+            "heading": -0.5207249369577782,
+            "angularVelocity": -0.31541116551017917,
+            "velocityX": 2.280266840487519,
+            "velocityY": -2.5608779345205184,
+            "timestamp": 0.5213489590627841
+        },
+        {
+            "x": 5.0582868126043,
+            "y": 4.2697391199027495,
+            "heading": -0.509978725166827,
+            "angularVelocity": 0.10306160206274768,
+            "velocityX": 3.0522438829312937,
+            "velocityY": -2.1785765368723764,
+            "timestamp": 0.6256187508753409
+        },
+        {
+            "x": 5.423286105863135,
+            "y": 4.1201949004052105,
+            "heading": -0.5099785687835147,
+            "angularVelocity": 0.0000014997949991595825,
+            "velocityX": 3.5005276879710774,
+            "velocityY": -1.4342046425715584,
+            "timestamp": 0.7298885426878977
+        },
+        {
+            "x": 5.805099761015269,
+            "y": 4.021168099470067,
+            "heading": -0.5099784851647622,
+            "angularVelocity": 8.019460959727046e-7,
+            "velocityX": 3.6617859162748747,
+            "velocityY": -0.9497170677501874,
+            "timestamp": 0.8341583345004545
+        },
+        {
+            "x": 6.154222032422552,
+            "y": 3.8375880828272497,
+            "heading": -0.5099784796190555,
+            "angularVelocity": 5.318612994948929e-8,
+            "velocityX": 3.348259024386382,
+            "velocityY": -1.7606251384181746,
+            "timestamp": 0.9384281263130113
+        },
+        {
+            "x": 6.493379431179238,
+            "y": 3.636192348012897,
+            "heading": -0.5099784741544249,
+            "angularVelocity": 5.240856867670856e-8,
+            "velocityX": 3.2526908595576858,
+            "velocityY": -1.9314868795019449,
+            "timestamp": 1.0426979181255682
+        },
+        {
+            "x": 6.795285003697281,
+            "y": 3.3823400148372036,
+            "heading": -0.5099784394461939,
+            "angularVelocity": 3.32869476219209e-7,
+            "velocityX": 2.895427019368865,
+            "velocityY": -2.4345721686299853,
+            "timestamp": 1.146967709938125
+        },
+        {
+            "x": 7.066681771760518,
+            "y": 3.0961027469536373,
+            "heading": -0.509978390292543,
+            "angularVelocity": 4.714083530965781e-7,
+            "velocityX": 2.6028321659174227,
+            "velocityY": -2.7451600593787298,
+            "timestamp": 1.2512375017506818
+        },
+        {
+            "x": 7.312943361341716,
+            "y": 2.8410544331524363,
+            "heading": -0.40533862017886096,
+            "angularVelocity": 1.0035482788897323,
+            "velocityX": 2.361773101301441,
+            "velocityY": -2.446042227260748,
+            "timestamp": 1.3555072935632386
+        },
+        {
+            "x": 7.49731350893092,
+            "y": 2.6484381177603322,
+            "heading": -0.22178416093118458,
+            "angularVelocity": 1.7603800300824186,
+            "velocityX": 1.768202893515326,
+            "velocityY": -1.8472878102448496,
+            "timestamp": 1.4597770853757954
+        },
+        {
+            "x": 7.619307085639959,
+            "y": 2.520663921373434,
+            "heading": -0.0776901780416505,
+            "angularVelocity": 1.3819341190262298,
+            "velocityX": 1.1699800545142,
+            "velocityY": -1.225419118670484,
+            "timestamp": 1.5640468771883522
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 1.429761379305419e-34,
+            "angularVelocity": 0.7450880709660586,
+            "velocityX": 0.5820757220763062,
+            "velocityY": -0.6105691808408042,
+            "timestamp": 1.668316669000909
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": -5.61453765178046e-32,
+            "velocityX": 0,
+            "velocityY": 2.318867528557263e-33,
+            "timestamp": 1.7725864608134658
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C4.traj
new file mode 100644
index 00000000..c13783bc
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C4.traj	
@@ -0,0 +1,166 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 7.441191049306378e-32,
+            "velocityX": -6.15829104129807e-34,
+            "velocityY": 2.4252394047543234e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 4.262142393092874,
+            "y": 5.471176724349191,
+            "heading": -0.06580713819401311,
+            "angularVelocity": -0.6311237133024392,
+            "velocityX": -0.008224883662067025,
+            "velocityY": -0.8518601035521746,
+            "timestamp": 0.10426979181255681
+        },
+        {
+            "x": 4.27936690849282,
+            "y": 5.294863623901166,
+            "heading": -0.18680392528158013,
+            "angularVelocity": -1.1604203382805245,
+            "velocityX": 0.16519180771848577,
+            "velocityY": -1.6909317395107013,
+            "timestamp": 0.20853958362511363
+        },
+        {
+            "x": 4.348116425232891,
+            "y": 5.0445566747334585,
+            "heading": -0.3412830267774945,
+            "angularVelocity": -1.4815326549574157,
+            "velocityX": 0.6593426105967491,
+            "velocityY": -2.400570144205118,
+            "timestamp": 0.31280937543767046
+        },
+        {
+            "x": 4.502267029635194,
+            "y": 4.763921050939977,
+            "heading": -0.4878370803946758,
+            "angularVelocity": -1.4055274405902505,
+            "velocityX": 1.4783822018117698,
+            "velocityY": -2.6914374615609895,
+            "timestamp": 0.41707916725022726
+        },
+        {
+            "x": 4.740029978369904,
+            "y": 4.496898841850153,
+            "heading": -0.5207249369577782,
+            "angularVelocity": -0.31541116551017917,
+            "velocityX": 2.280266840487519,
+            "velocityY": -2.5608779345205184,
+            "timestamp": 0.5213489590627841
+        },
+        {
+            "x": 5.0582868126043,
+            "y": 4.2697391199027495,
+            "heading": -0.509978725166827,
+            "angularVelocity": 0.10306160206274768,
+            "velocityX": 3.0522438829312937,
+            "velocityY": -2.1785765368723764,
+            "timestamp": 0.6256187508753409
+        },
+        {
+            "x": 5.423286105863135,
+            "y": 4.1201949004052105,
+            "heading": -0.5099785687835147,
+            "angularVelocity": 0.0000014997949991595825,
+            "velocityX": 3.5005276879710774,
+            "velocityY": -1.4342046425715584,
+            "timestamp": 0.7298885426878977
+        },
+        {
+            "x": 5.805099761015269,
+            "y": 4.021168099470067,
+            "heading": -0.5099784851647622,
+            "angularVelocity": 8.019460959727046e-7,
+            "velocityX": 3.6617859162748747,
+            "velocityY": -0.9497170677501874,
+            "timestamp": 0.8341583345004545
+        },
+        {
+            "x": 6.154222032422552,
+            "y": 3.8375880828272497,
+            "heading": -0.5099784796190555,
+            "angularVelocity": 5.318612994948929e-8,
+            "velocityX": 3.348259024386382,
+            "velocityY": -1.7606251384181746,
+            "timestamp": 0.9384281263130113
+        },
+        {
+            "x": 6.493379431179238,
+            "y": 3.636192348012897,
+            "heading": -0.5099784741544249,
+            "angularVelocity": 5.240856867670856e-8,
+            "velocityX": 3.2526908595576858,
+            "velocityY": -1.9314868795019449,
+            "timestamp": 1.0426979181255682
+        },
+        {
+            "x": 6.795285003697281,
+            "y": 3.3823400148372036,
+            "heading": -0.5099784394461939,
+            "angularVelocity": 3.32869476219209e-7,
+            "velocityX": 2.895427019368865,
+            "velocityY": -2.4345721686299853,
+            "timestamp": 1.146967709938125
+        },
+        {
+            "x": 7.066681771760518,
+            "y": 3.0961027469536373,
+            "heading": -0.509978390292543,
+            "angularVelocity": 4.714083530965781e-7,
+            "velocityX": 2.6028321659174227,
+            "velocityY": -2.7451600593787298,
+            "timestamp": 1.2512375017506818
+        },
+        {
+            "x": 7.312943361341716,
+            "y": 2.8410544331524363,
+            "heading": -0.40533862017886096,
+            "angularVelocity": 1.0035482788897323,
+            "velocityX": 2.361773101301441,
+            "velocityY": -2.446042227260748,
+            "timestamp": 1.3555072935632386
+        },
+        {
+            "x": 7.49731350893092,
+            "y": 2.6484381177603322,
+            "heading": -0.22178416093118458,
+            "angularVelocity": 1.7603800300824186,
+            "velocityX": 1.768202893515326,
+            "velocityY": -1.8472878102448496,
+            "timestamp": 1.4597770853757954
+        },
+        {
+            "x": 7.619307085639959,
+            "y": 2.520663921373434,
+            "heading": -0.0776901780416505,
+            "angularVelocity": 1.3819341190262298,
+            "velocityX": 1.1699800545142,
+            "velocityY": -1.225419118670484,
+            "timestamp": 1.5640468771883522
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 1.429761379305419e-34,
+            "angularVelocity": 0.7450880709660586,
+            "velocityX": 0.5820757220763062,
+            "velocityY": -0.6105691808408042,
+            "timestamp": 1.668316669000909
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": -5.61453765178046e-32,
+            "velocityX": 0,
+            "velocityY": 2.318867528557263e-33,
+            "timestamp": 1.7725864608134658
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C5.1.traj
new file mode 100644
index 00000000..004abebc
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C5.1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": -1.2613927548149136e-32,
+            "velocityX": 4.983015637334932e-33,
+            "velocityY": -3.6905945238681026e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 4.258006222511338,
+            "y": 5.473607841813564,
+            "heading": -0.06530003091818118,
+            "angularVelocity": -0.6342708718414432,
+            "velocityX": -0.04850545332028736,
+            "velocityY": -0.8391424739420397,
+            "timestamp": 0.10295290831912149
+        },
+        {
+            "x": 4.271377859079598,
+            "y": 5.302453682559796,
+            "heading": -0.1916286288786279,
+            "angularVelocity": -1.2270522515873779,
+            "velocityX": 0.12988109599401854,
+            "velocityY": -1.662450940416803,
+            "timestamp": 0.20590581663824298
+        },
+        {
+            "x": 4.340766271943036,
+            "y": 5.0642424556348775,
+            "heading": -0.3653686640748466,
+            "angularVelocity": -1.6875680156375907,
+            "velocityX": 0.673982056420939,
+            "velocityY": -2.3137882242873524,
+            "timestamp": 0.30885872495736444
+        },
+        {
+            "x": 4.494848034080926,
+            "y": 4.801255485663876,
+            "heading": -0.5565822720790902,
+            "angularVelocity": -1.8572919514963226,
+            "velocityX": 1.4966236957608356,
+            "velocityY": -2.5544394448365244,
+            "timestamp": 0.41181163327648596
+        },
+        {
+            "x": 4.73233691115851,
+            "y": 4.546498868370588,
+            "heading": -0.6490495118957932,
+            "angularVelocity": -0.8981508276588281,
+            "velocityX": 2.3067719111095206,
+            "velocityY": -2.4744965582091383,
+            "timestamp": 0.5147645415956075
+        },
+        {
+            "x": 5.049349589860016,
+            "y": 4.3202563768587865,
+            "heading": -0.6490493910522949,
+            "angularVelocity": 0.0000011737744981731646,
+            "velocityX": 3.079200810130272,
+            "velocityY": -2.1975337579636096,
+            "timestamp": 0.617717449914729
+        },
+        {
+            "x": 5.4092082222704905,
+            "y": 4.171311637049865,
+            "heading": -0.6490493274377749,
+            "angularVelocity": 6.178992034513071e-7,
+            "velocityX": 3.4953712166637025,
+            "velocityY": -1.446726879703489,
+            "timestamp": 0.7206703582338505
+        },
+        {
+            "x": 5.789554256648214,
+            "y": 4.087528164867666,
+            "heading": -0.6490492895940018,
+            "angularVelocity": 3.675833320529158e-7,
+            "velocityX": 3.6943690138288297,
+            "velocityY": -0.8138038405141147,
+            "timestamp": 0.823623266552972
+        },
+        {
+            "x": 6.141218018459777,
+            "y": 3.9201502365347105,
+            "heading": -0.6490493503243893,
+            "angularVelocity": -5.898851085689342e-7,
+            "velocityX": 3.4157729738097102,
+            "velocityY": -1.6257717345306835,
+            "timestamp": 0.9265761748720935
+        },
+        {
+            "x": 6.456138339829479,
+            "y": 3.69100407104571,
+            "heading": -0.6490494074378677,
+            "angularVelocity": -5.547534235510974e-7,
+            "velocityX": 3.0588773693847333,
+            "velocityY": -2.2257376622981755,
+            "timestamp": 1.029529083191215
+        },
+        {
+            "x": 6.721775326533507,
+            "y": 3.406188756476768,
+            "heading": -0.6490494689481024,
+            "angularVelocity": -5.974599048336077e-7,
+            "velocityX": 2.580179531020508,
+            "velocityY": -2.7664620574496457,
+            "timestamp": 1.1324819915103363
+        },
+        {
+            "x": 6.916359430113237,
+            "y": 3.0688168532177102,
+            "heading": -0.6490494077682868,
+            "angularVelocity": 5.942504852606566e-7,
+            "velocityX": 1.8900301774533879,
+            "velocityY": -3.276953597204961,
+            "timestamp": 1.2354348998294578
+        },
+        {
+            "x": 7.040594979350531,
+            "y": 2.699698573462511,
+            "heading": -0.6490493528706274,
+            "angularVelocity": 5.332307776860117e-7,
+            "velocityX": 1.206722095234099,
+            "velocityY": -3.585311826364818,
+            "timestamp": 1.3383878081485792
+        },
+        {
+            "x": 7.164812386541997,
+            "y": 2.330574188065911,
+            "heading": -0.649049297974002,
+            "angularVelocity": 5.332207349098274e-7,
+            "velocityX": 1.2065458783003051,
+            "velocityY": -3.585371131551044,
+            "timestamp": 1.4413407164677006
+        },
+        {
+            "x": 7.289029734328084,
+            "y": 1.9614497826782673,
+            "heading": -0.6490492430773671,
+            "angularVelocity": 5.332208261000816e-7,
+            "velocityX": 1.206545301285243,
+            "velocityY": -3.5853713257276327,
+            "timestamp": 1.544293624786822
+        },
+        {
+            "x": 7.413246520819429,
+            "y": 1.5923251884092744,
+            "heading": -0.6490491881699392,
+            "angularVelocity": 5.333256623024379e-7,
+            "velocityX": 1.2065398493290926,
+            "velocityY": -3.585373160365936,
+            "timestamp": 1.6472465331059434
+        },
+        {
+            "x": 7.520074458284658,
+            "y": 1.2704783284902474,
+            "heading": -0.5256216946651037,
+            "angularVelocity": 1.1988733054752465,
+            "velocityX": 1.037638850707321,
+            "velocityY": -3.1261560763432104,
+            "timestamp": 1.7501994414250648
+        },
+        {
+            "x": 7.601104601009551,
+            "y": 1.0271731610238788,
+            "heading": -0.2913266412644364,
+            "angularVelocity": 2.275749730881093,
+            "velocityX": 0.7870602593733868,
+            "velocityY": -2.3632665792422265,
+            "timestamp": 1.8531523497441862
+        },
+        {
+            "x": 7.654009357663848,
+            "y": 0.8666565360101991,
+            "heading": -0.10310334462657153,
+            "angularVelocity": 1.828246522715339,
+            "velocityX": 0.5138733574219042,
+            "velocityY": -1.5591266690216163,
+            "timestamp": 1.9561052580633076
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -1.429149737287701e-32,
+            "angularVelocity": 1.0014612147428026,
+            "velocityX": 0.2524517545010862,
+            "velocityY": -0.7737181718391956,
+            "timestamp": 2.059058166382429
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": -8.032741852653566e-32,
+            "velocityX": 0,
+            "velocityY": 2.733638748586157e-32,
+            "timestamp": 2.1620110747015504
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C5.traj
new file mode 100644
index 00000000..004abebc
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C5.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": -1.2613927548149136e-32,
+            "velocityX": 4.983015637334932e-33,
+            "velocityY": -3.6905945238681026e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 4.258006222511338,
+            "y": 5.473607841813564,
+            "heading": -0.06530003091818118,
+            "angularVelocity": -0.6342708718414432,
+            "velocityX": -0.04850545332028736,
+            "velocityY": -0.8391424739420397,
+            "timestamp": 0.10295290831912149
+        },
+        {
+            "x": 4.271377859079598,
+            "y": 5.302453682559796,
+            "heading": -0.1916286288786279,
+            "angularVelocity": -1.2270522515873779,
+            "velocityX": 0.12988109599401854,
+            "velocityY": -1.662450940416803,
+            "timestamp": 0.20590581663824298
+        },
+        {
+            "x": 4.340766271943036,
+            "y": 5.0642424556348775,
+            "heading": -0.3653686640748466,
+            "angularVelocity": -1.6875680156375907,
+            "velocityX": 0.673982056420939,
+            "velocityY": -2.3137882242873524,
+            "timestamp": 0.30885872495736444
+        },
+        {
+            "x": 4.494848034080926,
+            "y": 4.801255485663876,
+            "heading": -0.5565822720790902,
+            "angularVelocity": -1.8572919514963226,
+            "velocityX": 1.4966236957608356,
+            "velocityY": -2.5544394448365244,
+            "timestamp": 0.41181163327648596
+        },
+        {
+            "x": 4.73233691115851,
+            "y": 4.546498868370588,
+            "heading": -0.6490495118957932,
+            "angularVelocity": -0.8981508276588281,
+            "velocityX": 2.3067719111095206,
+            "velocityY": -2.4744965582091383,
+            "timestamp": 0.5147645415956075
+        },
+        {
+            "x": 5.049349589860016,
+            "y": 4.3202563768587865,
+            "heading": -0.6490493910522949,
+            "angularVelocity": 0.0000011737744981731646,
+            "velocityX": 3.079200810130272,
+            "velocityY": -2.1975337579636096,
+            "timestamp": 0.617717449914729
+        },
+        {
+            "x": 5.4092082222704905,
+            "y": 4.171311637049865,
+            "heading": -0.6490493274377749,
+            "angularVelocity": 6.178992034513071e-7,
+            "velocityX": 3.4953712166637025,
+            "velocityY": -1.446726879703489,
+            "timestamp": 0.7206703582338505
+        },
+        {
+            "x": 5.789554256648214,
+            "y": 4.087528164867666,
+            "heading": -0.6490492895940018,
+            "angularVelocity": 3.675833320529158e-7,
+            "velocityX": 3.6943690138288297,
+            "velocityY": -0.8138038405141147,
+            "timestamp": 0.823623266552972
+        },
+        {
+            "x": 6.141218018459777,
+            "y": 3.9201502365347105,
+            "heading": -0.6490493503243893,
+            "angularVelocity": -5.898851085689342e-7,
+            "velocityX": 3.4157729738097102,
+            "velocityY": -1.6257717345306835,
+            "timestamp": 0.9265761748720935
+        },
+        {
+            "x": 6.456138339829479,
+            "y": 3.69100407104571,
+            "heading": -0.6490494074378677,
+            "angularVelocity": -5.547534235510974e-7,
+            "velocityX": 3.0588773693847333,
+            "velocityY": -2.2257376622981755,
+            "timestamp": 1.029529083191215
+        },
+        {
+            "x": 6.721775326533507,
+            "y": 3.406188756476768,
+            "heading": -0.6490494689481024,
+            "angularVelocity": -5.974599048336077e-7,
+            "velocityX": 2.580179531020508,
+            "velocityY": -2.7664620574496457,
+            "timestamp": 1.1324819915103363
+        },
+        {
+            "x": 6.916359430113237,
+            "y": 3.0688168532177102,
+            "heading": -0.6490494077682868,
+            "angularVelocity": 5.942504852606566e-7,
+            "velocityX": 1.8900301774533879,
+            "velocityY": -3.276953597204961,
+            "timestamp": 1.2354348998294578
+        },
+        {
+            "x": 7.040594979350531,
+            "y": 2.699698573462511,
+            "heading": -0.6490493528706274,
+            "angularVelocity": 5.332307776860117e-7,
+            "velocityX": 1.206722095234099,
+            "velocityY": -3.585311826364818,
+            "timestamp": 1.3383878081485792
+        },
+        {
+            "x": 7.164812386541997,
+            "y": 2.330574188065911,
+            "heading": -0.649049297974002,
+            "angularVelocity": 5.332207349098274e-7,
+            "velocityX": 1.2065458783003051,
+            "velocityY": -3.585371131551044,
+            "timestamp": 1.4413407164677006
+        },
+        {
+            "x": 7.289029734328084,
+            "y": 1.9614497826782673,
+            "heading": -0.6490492430773671,
+            "angularVelocity": 5.332208261000816e-7,
+            "velocityX": 1.206545301285243,
+            "velocityY": -3.5853713257276327,
+            "timestamp": 1.544293624786822
+        },
+        {
+            "x": 7.413246520819429,
+            "y": 1.5923251884092744,
+            "heading": -0.6490491881699392,
+            "angularVelocity": 5.333256623024379e-7,
+            "velocityX": 1.2065398493290926,
+            "velocityY": -3.585373160365936,
+            "timestamp": 1.6472465331059434
+        },
+        {
+            "x": 7.520074458284658,
+            "y": 1.2704783284902474,
+            "heading": -0.5256216946651037,
+            "angularVelocity": 1.1988733054752465,
+            "velocityX": 1.037638850707321,
+            "velocityY": -3.1261560763432104,
+            "timestamp": 1.7501994414250648
+        },
+        {
+            "x": 7.601104601009551,
+            "y": 1.0271731610238788,
+            "heading": -0.2913266412644364,
+            "angularVelocity": 2.275749730881093,
+            "velocityX": 0.7870602593733868,
+            "velocityY": -2.3632665792422265,
+            "timestamp": 1.8531523497441862
+        },
+        {
+            "x": 7.654009357663848,
+            "y": 0.8666565360101991,
+            "heading": -0.10310334462657153,
+            "angularVelocity": 1.828246522715339,
+            "velocityX": 0.5138733574219042,
+            "velocityY": -1.5591266690216163,
+            "timestamp": 1.9561052580633076
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -1.429149737287701e-32,
+            "angularVelocity": 1.0014612147428026,
+            "velocityX": 0.2524517545010862,
+            "velocityY": -0.7737181718391956,
+            "timestamp": 2.059058166382429
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": -8.032741852653566e-32,
+            "velocityX": 0,
+            "velocityY": 2.733638748586157e-32,
+            "timestamp": 2.1620110747015504
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj
index 84804d4a..c9864950 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj	
@@ -3,128 +3,128 @@
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -7.11323283877368e-33,
-            "velocityX": -2.26183607094562e-34,
-            "velocityY": 8.143994372388985e-34,
+            "heading": -7.967068308454128e-24,
+            "angularVelocity": 3.748757739861167e-23,
+            "velocityX": 2.372714537320914e-25,
+            "velocityY": 2.748536601620217e-25,
             "timestamp": 0
         },
         {
-            "x": 4.215224482174133,
-            "y": 5.589131482897013,
-            "heading": 3.153116118672446,
-            "angularVelocity": 0.15993604804856262,
-            "velocityX": -0.5825677325255438,
-            "velocityY": 0.35522507570249656,
-            "timestamp": 0.0820085204835332
+            "x": 4.215224464845852,
+            "y": 5.589131496345612,
+            "heading": 0.013109636548148297,
+            "angularVelocity": 0.15985714241706056,
+            "velocityX": -0.5825684410958524,
+            "velocityY": 0.3552255429080149,
+            "timestamp": 0.08200845048228003
         },
         {
-            "x": 4.119673447940142,
-            "y": 5.647394549238196,
-            "heading": 3.179344081068732,
-            "angularVelocity": 0.3198199679940867,
-            "velocityX": -1.1651354477633493,
-            "velocityY": 0.7104513774624363,
-            "timestamp": 0.1640170409670664
+            "x": 4.119673396532112,
+            "y": 5.647394589751744,
+            "heading": 0.03932466572409392,
+            "angularVelocity": 0.31966253504094716,
+            "velocityX": -1.1651368578703514,
+            "velocityY": 0.7104523139200205,
+            "timestamp": 0.16401690096456006
         },
         {
-            "x": 3.976346906814042,
-            "y": 5.734789369483009,
-            "heading": 3.218677503841872,
-            "angularVelocity": 0.47962605033262895,
-            "velocityX": -1.7477030469642283,
-            "velocityY": 1.0656797577803132,
-            "timestamp": 0.24602556145059962
+            "x": 3.9763468054990545,
+            "y": 5.734789450682316,
+            "heading": 0.07863876132846631,
+            "angularVelocity": 0.47939078684174374,
+            "velocityX": -1.7477051473375471,
+            "velocityY": 1.06568116354614,
+            "timestamp": 0.2460253514468401
         },
         {
-            "x": 3.785244904014577,
-            "y": 5.851316186750812,
-            "heading": 3.271109373786434,
-            "angularVelocity": 0.6393466146617031,
-            "velocityX": -2.3302700947743262,
-            "velocityY": 1.4209111026603802,
-            "timestamp": 0.3280340819341328
+            "x": 3.785244738141483,
+            "y": 5.8513163218996445,
+            "heading": 0.131044995583191,
+            "angularVelocity": 0.6390345622497571,
+            "velocityX": -2.330272871072761,
+            "velocityY": 1.4209129733832384,
+            "timestamp": 0.32803380192912013
         },
         {
-            "x": 3.5463675589434582,
-            "y": 5.996975305481239,
-            "heading": 3.3366342258406214,
-            "angularVelocity": 0.7990005388201682,
-            "velocityX": -2.9128356866172687,
-            "velocityY": 1.7761461598331643,
-            "timestamp": 0.410042602417666
+            "x": 3.5463673151125503,
+            "y": 5.996975506896697,
+            "heading": 0.19653801558690542,
+            "angularVelocity": 0.7986130650995023,
+            "velocityX": -2.9128391235796927,
+            "velocityY": 1.776148483972697,
+            "timestamp": 0.41004225241140013
         },
         {
-            "x": 3.2814945861155147,
-            "y": 6.158490139171684,
-            "heading": 3.3366342341249666,
-            "angularVelocity": 1.010181030485091e-7,
-            "velocityX": -3.2298225997276515,
-            "velocityY": 1.9694884475190098,
-            "timestamp": 0.4920511229011992
+            "x": 3.2814945553511796,
+            "y": 6.158490181857634,
+            "heading": 0.19653800981726008,
+            "angularVelocity": -7.035427786522906e-8,
+            "velocityX": -3.2298227585534365,
+            "velocityY": 1.9694881931202608,
+            "timestamp": 0.49205070289368014
         },
         {
-            "x": 3.0166215989993184,
-            "y": 6.32000494943038,
-            "heading": 3.336634242409321,
-            "angularVelocity": 1.0101821933668142e-7,
-            "velocityX": -3.2298227739565295,
-            "velocityY": 1.969488161795665,
-            "timestamp": 0.5740596433847324
+            "x": 3.016621781154659,
+            "y": 6.320004832717382,
+            "heading": 0.19653801616374372,
+            "angularVelocity": 7.738816679079332e-8,
+            "velocityX": -3.229822934573717,
+            "velocityY": 1.9694878992335856,
+            "timestamp": 0.5740591533759601
         },
         {
-            "x": 2.7777475013239767,
-            "y": 6.46566975957478,
-            "heading": 3.402143020545178,
-            "angularVelocity": 0.7988045357922341,
-            "velocityX": -2.9127960883443285,
-            "velocityY": 1.776215560109378,
-            "timestamp": 0.6560681638682656
+            "x": 2.7777476228533557,
+            "y": 6.465669683581835,
+            "heading": 0.2620170313059908,
+            "angularVelocity": 0.7984422917049926,
+            "velocityX": -2.9127993139306705,
+            "velocityY": 1.7762175727966987,
+            "timestamp": 0.6560676038582401
         },
         {
-            "x": 2.5866484088099964,
-            "y": 6.582201778902395,
-            "heading": 3.4545564874050534,
-            "angularVelocity": 0.6391222101171739,
-            "velocityX": -2.3302346071753846,
-            "velocityY": 1.4209745358229475,
-            "timestamp": 0.7380766843517989
+            "x": 2.5866484818505064,
+            "y": 6.582201734022544,
+            "heading": 0.31440674809174973,
+            "angularVelocity": 0.6388331504578179,
+            "velocityX": -2.330237187497417,
+            "velocityY": 1.4209761281355728,
+            "timestamp": 0.7380760543405201
         },
         {
-            "x": 2.443324185159995,
-            "y": 6.669600869933244,
-            "heading": 3.493870163524806,
-            "angularVelocity": 0.47938526250630853,
-            "velocityX": -1.7476747879969323,
-            "velocityY": 1.0657318351255574,
-            "timestamp": 0.8200852048353321
+            "x": 2.443324221748551,
+            "y": 6.669600847656536,
+            "heading": 0.3537026369190334,
+            "angularVelocity": 0.47916877585407597,
+            "velocityX": -1.7476767242776265,
+            "velocityY": 1.0657330204388804,
+            "timestamp": 0.8200845048228002
         },
         {
-            "x": 2.347774731346008,
-            "y": 6.727866955511231,
-            "heading": 3.5200803945158796,
-            "angularVelocity": 0.3196037538116112,
-            "velocityX": -1.1651161763511244,
-            "velocityY": 0.710488193597954,
-            "timestamp": 0.9020937253188653
+            "x": 2.347774743561573,
+            "y": 6.727866948074407,
+            "heading": 0.37990101547487837,
+            "angularVelocity": 0.31945949962200243,
+            "velocityX": -1.1651174680787846,
+            "velocityY": 0.7104889810161812,
+            "timestamp": 0.9020929553050802
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 0.1597994036038963,
-            "velocityX": -0.5825581423042643,
-            "velocityY": 0.35524411752579316,
-            "timestamp": 0.9841022458023986
+            "heading": 0.393,
+            "angularVelocity": 0.15972725308292482,
+            "velocityX": -0.5825587885226076,
+            "velocityY": 0.355244511440778,
+            "timestamp": 0.9841014057873602
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 0,
-            "velocityX": -5.266786892384168e-34,
-            "velocityY": -2.2308589105860002e-32,
-            "timestamp": 1.0661107662859317
+            "heading": 0.393,
+            "angularVelocity": 1.2555409321863598e-23,
+            "velocityX": -4.0981987257829375e-24,
+            "velocityY": -7.824281389954736e-24,
+            "timestamp": 1.0661098562696403
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.traj
index 84804d4a..c9864950 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.traj	
@@ -3,128 +3,128 @@
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -7.11323283877368e-33,
-            "velocityX": -2.26183607094562e-34,
-            "velocityY": 8.143994372388985e-34,
+            "heading": -7.967068308454128e-24,
+            "angularVelocity": 3.748757739861167e-23,
+            "velocityX": 2.372714537320914e-25,
+            "velocityY": 2.748536601620217e-25,
             "timestamp": 0
         },
         {
-            "x": 4.215224482174133,
-            "y": 5.589131482897013,
-            "heading": 3.153116118672446,
-            "angularVelocity": 0.15993604804856262,
-            "velocityX": -0.5825677325255438,
-            "velocityY": 0.35522507570249656,
-            "timestamp": 0.0820085204835332
+            "x": 4.215224464845852,
+            "y": 5.589131496345612,
+            "heading": 0.013109636548148297,
+            "angularVelocity": 0.15985714241706056,
+            "velocityX": -0.5825684410958524,
+            "velocityY": 0.3552255429080149,
+            "timestamp": 0.08200845048228003
         },
         {
-            "x": 4.119673447940142,
-            "y": 5.647394549238196,
-            "heading": 3.179344081068732,
-            "angularVelocity": 0.3198199679940867,
-            "velocityX": -1.1651354477633493,
-            "velocityY": 0.7104513774624363,
-            "timestamp": 0.1640170409670664
+            "x": 4.119673396532112,
+            "y": 5.647394589751744,
+            "heading": 0.03932466572409392,
+            "angularVelocity": 0.31966253504094716,
+            "velocityX": -1.1651368578703514,
+            "velocityY": 0.7104523139200205,
+            "timestamp": 0.16401690096456006
         },
         {
-            "x": 3.976346906814042,
-            "y": 5.734789369483009,
-            "heading": 3.218677503841872,
-            "angularVelocity": 0.47962605033262895,
-            "velocityX": -1.7477030469642283,
-            "velocityY": 1.0656797577803132,
-            "timestamp": 0.24602556145059962
+            "x": 3.9763468054990545,
+            "y": 5.734789450682316,
+            "heading": 0.07863876132846631,
+            "angularVelocity": 0.47939078684174374,
+            "velocityX": -1.7477051473375471,
+            "velocityY": 1.06568116354614,
+            "timestamp": 0.2460253514468401
         },
         {
-            "x": 3.785244904014577,
-            "y": 5.851316186750812,
-            "heading": 3.271109373786434,
-            "angularVelocity": 0.6393466146617031,
-            "velocityX": -2.3302700947743262,
-            "velocityY": 1.4209111026603802,
-            "timestamp": 0.3280340819341328
+            "x": 3.785244738141483,
+            "y": 5.8513163218996445,
+            "heading": 0.131044995583191,
+            "angularVelocity": 0.6390345622497571,
+            "velocityX": -2.330272871072761,
+            "velocityY": 1.4209129733832384,
+            "timestamp": 0.32803380192912013
         },
         {
-            "x": 3.5463675589434582,
-            "y": 5.996975305481239,
-            "heading": 3.3366342258406214,
-            "angularVelocity": 0.7990005388201682,
-            "velocityX": -2.9128356866172687,
-            "velocityY": 1.7761461598331643,
-            "timestamp": 0.410042602417666
+            "x": 3.5463673151125503,
+            "y": 5.996975506896697,
+            "heading": 0.19653801558690542,
+            "angularVelocity": 0.7986130650995023,
+            "velocityX": -2.9128391235796927,
+            "velocityY": 1.776148483972697,
+            "timestamp": 0.41004225241140013
         },
         {
-            "x": 3.2814945861155147,
-            "y": 6.158490139171684,
-            "heading": 3.3366342341249666,
-            "angularVelocity": 1.010181030485091e-7,
-            "velocityX": -3.2298225997276515,
-            "velocityY": 1.9694884475190098,
-            "timestamp": 0.4920511229011992
+            "x": 3.2814945553511796,
+            "y": 6.158490181857634,
+            "heading": 0.19653800981726008,
+            "angularVelocity": -7.035427786522906e-8,
+            "velocityX": -3.2298227585534365,
+            "velocityY": 1.9694881931202608,
+            "timestamp": 0.49205070289368014
         },
         {
-            "x": 3.0166215989993184,
-            "y": 6.32000494943038,
-            "heading": 3.336634242409321,
-            "angularVelocity": 1.0101821933668142e-7,
-            "velocityX": -3.2298227739565295,
-            "velocityY": 1.969488161795665,
-            "timestamp": 0.5740596433847324
+            "x": 3.016621781154659,
+            "y": 6.320004832717382,
+            "heading": 0.19653801616374372,
+            "angularVelocity": 7.738816679079332e-8,
+            "velocityX": -3.229822934573717,
+            "velocityY": 1.9694878992335856,
+            "timestamp": 0.5740591533759601
         },
         {
-            "x": 2.7777475013239767,
-            "y": 6.46566975957478,
-            "heading": 3.402143020545178,
-            "angularVelocity": 0.7988045357922341,
-            "velocityX": -2.9127960883443285,
-            "velocityY": 1.776215560109378,
-            "timestamp": 0.6560681638682656
+            "x": 2.7777476228533557,
+            "y": 6.465669683581835,
+            "heading": 0.2620170313059908,
+            "angularVelocity": 0.7984422917049926,
+            "velocityX": -2.9127993139306705,
+            "velocityY": 1.7762175727966987,
+            "timestamp": 0.6560676038582401
         },
         {
-            "x": 2.5866484088099964,
-            "y": 6.582201778902395,
-            "heading": 3.4545564874050534,
-            "angularVelocity": 0.6391222101171739,
-            "velocityX": -2.3302346071753846,
-            "velocityY": 1.4209745358229475,
-            "timestamp": 0.7380766843517989
+            "x": 2.5866484818505064,
+            "y": 6.582201734022544,
+            "heading": 0.31440674809174973,
+            "angularVelocity": 0.6388331504578179,
+            "velocityX": -2.330237187497417,
+            "velocityY": 1.4209761281355728,
+            "timestamp": 0.7380760543405201
         },
         {
-            "x": 2.443324185159995,
-            "y": 6.669600869933244,
-            "heading": 3.493870163524806,
-            "angularVelocity": 0.47938526250630853,
-            "velocityX": -1.7476747879969323,
-            "velocityY": 1.0657318351255574,
-            "timestamp": 0.8200852048353321
+            "x": 2.443324221748551,
+            "y": 6.669600847656536,
+            "heading": 0.3537026369190334,
+            "angularVelocity": 0.47916877585407597,
+            "velocityX": -1.7476767242776265,
+            "velocityY": 1.0657330204388804,
+            "timestamp": 0.8200845048228002
         },
         {
-            "x": 2.347774731346008,
-            "y": 6.727866955511231,
-            "heading": 3.5200803945158796,
-            "angularVelocity": 0.3196037538116112,
-            "velocityX": -1.1651161763511244,
-            "velocityY": 0.710488193597954,
-            "timestamp": 0.9020937253188653
+            "x": 2.347774743561573,
+            "y": 6.727866948074407,
+            "heading": 0.37990101547487837,
+            "angularVelocity": 0.31945949962200243,
+            "velocityX": -1.1651174680787846,
+            "velocityY": 0.7104889810161812,
+            "timestamp": 0.9020929553050802
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 0.1597994036038963,
-            "velocityX": -0.5825581423042643,
-            "velocityY": 0.35524411752579316,
-            "timestamp": 0.9841022458023986
+            "heading": 0.393,
+            "angularVelocity": 0.15972725308292482,
+            "velocityX": -0.5825587885226076,
+            "velocityY": 0.355244511440778,
+            "timestamp": 0.9841014057873602
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 0,
-            "velocityX": -5.266786892384168e-34,
-            "velocityY": -2.2308589105860002e-32,
-            "timestamp": 1.0661107662859317
+            "heading": 0.393,
+            "angularVelocity": 1.2555409321863598e-23,
+            "velocityX": -4.0981987257829375e-24,
+            "velocityY": -7.824281389954736e-24,
+            "timestamp": 1.0661098562696403
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj
index b9cb5044..5e114a48 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj	
@@ -3,100 +3,100 @@
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 9.713847301484863e-36,
-            "velocityY": -1.0645735433948216e-32,
+            "heading": -1.042790690974161e-34,
+            "angularVelocity": 1.2672485017804847e-33,
+            "velocityX": 0,
+            "velocityY": 1.0649309840552648e-32,
             "timestamp": 0
         },
         {
-            "x": 4.181096344253042,
-            "y": 5.560000047628424,
-            "heading": 3.139999989014019,
-            "angularVelocity": -1.10846415392014e-7,
-            "velocityX": -0.826391981328299,
-            "velocityY": 4.805615505681296e-7,
+            "x": 4.181096344253041,
+            "y": 5.560000047617906,
+            "heading": -1.1491589479255403e-8,
+            "angularVelocity": -1.1594790933974305e-7,
+            "velocityX": -0.8263919813282988,
+            "velocityY": 4.80455414778758e-7,
             "timestamp": 0.09910993523353247
         },
         {
-            "x": 4.017289035836252,
-            "y": 5.560000133123364,
-            "heading": 3.139999981407119,
-            "angularVelocity": -7.675215101799687e-8,
-            "velocityX": -1.6527839316089914,
-            "velocityY": 8.626273272073532e-7,
+            "x": 4.017289035836251,
+            "y": 5.560000133094268,
+            "heading": -2.0027929206394758e-8,
+            "angularVelocity": -8.613001771881656e-8,
+            "velocityX": -1.6527839316089912,
+            "velocityY": 8.624398858779209e-7,
             "timestamp": 0.19821987046706493
         },
         {
-            "x": 3.7715780809038826,
-            "y": 5.560000246490977,
-            "heading": 3.1399999920859667,
-            "angularVelocity": 1.0774748441307597e-7,
-            "velocityX": -2.4791758197944644,
-            "velocityY": 0.0000011438571635435649,
+            "x": 3.771578080903882,
+            "y": 5.560000246435489,
+            "heading": -1.061567052726606e-8,
+            "angularVelocity": 9.49678480632688e-8,
+            "velocityX": -2.4791758197944636,
+            "velocityY": 0.0000011435908848510677,
             "timestamp": 0.2973298057005974
         },
         {
-            "x": 3.4439634979186673,
-            "y": 5.56000037760455,
-            "heading": 3.1400000352544852,
-            "angularVelocity": 4.355619377078705e-7,
-            "velocityX": -3.305567521694553,
-            "velocityY": 0.0000013229104502120412,
+            "x": 3.443963497918667,
+            "y": 5.560000377513817,
+            "heading": 3.109895895304223e-8,
+            "angularVelocity": 4.208924841656569e-7,
+            "velocityX": -3.30556752169455,
+            "velocityY": 0.0000013225548252865442,
             "timestamp": 0.39643974093412987
         },
         {
-            "x": 3.069036502081214,
-            "y": 5.560000367749523,
-            "heading": 3.140000035254493,
-            "angularVelocity": 8.147365661806906e-14,
-            "velocityX": -3.782940579610041,
-            "velocityY": -9.943529898465916e-8,
+            "x": 3.0690365020812136,
+            "y": 5.5600003676341405,
+            "heading": 3.1098967052884415e-8,
+            "angularVelocity": 8.336617080453033e-14,
+            "velocityX": -3.7829405796100413,
+            "velocityY": -9.968399941565162e-8,
             "timestamp": 0.4955496761676623
         },
         {
-            "x": 2.741421919096052,
-            "y": 5.560000236043854,
-            "heading": 3.1400000358317235,
-            "angularVelocity": 5.824165114605996e-9,
-            "velocityX": -3.305567521694012,
-            "velocityY": -0.0000013288845794272055,
+            "x": 2.7414219190960516,
+            "y": 5.560000235969226,
+            "heading": 3.331211577627848e-8,
+            "angularVelocity": 2.233025994415073e-8,
+            "velocityX": -3.3055675216940097,
+            "velocityY": -0.0000013284733736467011,
             "timestamp": 0.5946596114011948
         },
         {
-            "x": 2.4957109641637163,
-            "y": 5.560000125368319,
-            "heading": 3.1400000237983328,
-            "angularVelocity": -1.214145560764502e-7,
-            "velocityX": -2.479175819794127,
-            "velocityY": -0.0000011166946380844406,
+            "x": 2.495710964163716,
+            "y": 5.560000125328881,
+            "heading": 2.2528078300409453e-8,
+            "angularVelocity": -1.088088286759601e-7,
+            "velocityX": -2.4791758197941256,
+            "velocityY": -0.0000011163395746408495,
             "timestamp": 0.6937695466347273
         },
         {
             "x": 2.3319036557469475,
-            "y": 5.560000044140379,
-            "heading": 3.140000009519778,
-            "angularVelocity": -1.4406783354118398e-7,
-            "velocityX": -1.6527839316087705,
-            "velocityY": -8.195741267361974e-7,
+            "y": 5.560000044126654,
+            "heading": 9.093200341480335e-9,
+            "angularVelocity": -1.3555529933337065e-7,
+            "velocityX": -1.6527839316087698,
+            "velocityY": -8.193146909757129e-7,
             "timestamp": 0.7928794818682598
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -9.605270403416775e-8,
-            "velocityX": -0.8263919813281883,
-            "velocityY": -4.4536784825580755e-7,
+            "heading": 7.024905960060336e-33,
+            "angularVelocity": -9.17486204713572e-8,
+            "velocityX": -0.826391981328188,
+            "velocityY": -4.4522936458958866e-7,
             "timestamp": 0.8919894171017924
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 4.934081445690025e-38,
-            "velocityY": -9.331512134237052e-34,
+            "heading": 0,
+            "angularVelocity": 2.1129142513587384e-31,
+            "velocityX": -1.9174161859426788e-38,
+            "velocityY": -1.530570219467473e-32,
             "timestamp": 0.9910993523353249
         }
     ]
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.traj
index b9cb5044..5e114a48 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.traj	
@@ -3,100 +3,100 @@
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 9.713847301484863e-36,
-            "velocityY": -1.0645735433948216e-32,
+            "heading": -1.042790690974161e-34,
+            "angularVelocity": 1.2672485017804847e-33,
+            "velocityX": 0,
+            "velocityY": 1.0649309840552648e-32,
             "timestamp": 0
         },
         {
-            "x": 4.181096344253042,
-            "y": 5.560000047628424,
-            "heading": 3.139999989014019,
-            "angularVelocity": -1.10846415392014e-7,
-            "velocityX": -0.826391981328299,
-            "velocityY": 4.805615505681296e-7,
+            "x": 4.181096344253041,
+            "y": 5.560000047617906,
+            "heading": -1.1491589479255403e-8,
+            "angularVelocity": -1.1594790933974305e-7,
+            "velocityX": -0.8263919813282988,
+            "velocityY": 4.80455414778758e-7,
             "timestamp": 0.09910993523353247
         },
         {
-            "x": 4.017289035836252,
-            "y": 5.560000133123364,
-            "heading": 3.139999981407119,
-            "angularVelocity": -7.675215101799687e-8,
-            "velocityX": -1.6527839316089914,
-            "velocityY": 8.626273272073532e-7,
+            "x": 4.017289035836251,
+            "y": 5.560000133094268,
+            "heading": -2.0027929206394758e-8,
+            "angularVelocity": -8.613001771881656e-8,
+            "velocityX": -1.6527839316089912,
+            "velocityY": 8.624398858779209e-7,
             "timestamp": 0.19821987046706493
         },
         {
-            "x": 3.7715780809038826,
-            "y": 5.560000246490977,
-            "heading": 3.1399999920859667,
-            "angularVelocity": 1.0774748441307597e-7,
-            "velocityX": -2.4791758197944644,
-            "velocityY": 0.0000011438571635435649,
+            "x": 3.771578080903882,
+            "y": 5.560000246435489,
+            "heading": -1.061567052726606e-8,
+            "angularVelocity": 9.49678480632688e-8,
+            "velocityX": -2.4791758197944636,
+            "velocityY": 0.0000011435908848510677,
             "timestamp": 0.2973298057005974
         },
         {
-            "x": 3.4439634979186673,
-            "y": 5.56000037760455,
-            "heading": 3.1400000352544852,
-            "angularVelocity": 4.355619377078705e-7,
-            "velocityX": -3.305567521694553,
-            "velocityY": 0.0000013229104502120412,
+            "x": 3.443963497918667,
+            "y": 5.560000377513817,
+            "heading": 3.109895895304223e-8,
+            "angularVelocity": 4.208924841656569e-7,
+            "velocityX": -3.30556752169455,
+            "velocityY": 0.0000013225548252865442,
             "timestamp": 0.39643974093412987
         },
         {
-            "x": 3.069036502081214,
-            "y": 5.560000367749523,
-            "heading": 3.140000035254493,
-            "angularVelocity": 8.147365661806906e-14,
-            "velocityX": -3.782940579610041,
-            "velocityY": -9.943529898465916e-8,
+            "x": 3.0690365020812136,
+            "y": 5.5600003676341405,
+            "heading": 3.1098967052884415e-8,
+            "angularVelocity": 8.336617080453033e-14,
+            "velocityX": -3.7829405796100413,
+            "velocityY": -9.968399941565162e-8,
             "timestamp": 0.4955496761676623
         },
         {
-            "x": 2.741421919096052,
-            "y": 5.560000236043854,
-            "heading": 3.1400000358317235,
-            "angularVelocity": 5.824165114605996e-9,
-            "velocityX": -3.305567521694012,
-            "velocityY": -0.0000013288845794272055,
+            "x": 2.7414219190960516,
+            "y": 5.560000235969226,
+            "heading": 3.331211577627848e-8,
+            "angularVelocity": 2.233025994415073e-8,
+            "velocityX": -3.3055675216940097,
+            "velocityY": -0.0000013284733736467011,
             "timestamp": 0.5946596114011948
         },
         {
-            "x": 2.4957109641637163,
-            "y": 5.560000125368319,
-            "heading": 3.1400000237983328,
-            "angularVelocity": -1.214145560764502e-7,
-            "velocityX": -2.479175819794127,
-            "velocityY": -0.0000011166946380844406,
+            "x": 2.495710964163716,
+            "y": 5.560000125328881,
+            "heading": 2.2528078300409453e-8,
+            "angularVelocity": -1.088088286759601e-7,
+            "velocityX": -2.4791758197941256,
+            "velocityY": -0.0000011163395746408495,
             "timestamp": 0.6937695466347273
         },
         {
             "x": 2.3319036557469475,
-            "y": 5.560000044140379,
-            "heading": 3.140000009519778,
-            "angularVelocity": -1.4406783354118398e-7,
-            "velocityX": -1.6527839316087705,
-            "velocityY": -8.195741267361974e-7,
+            "y": 5.560000044126654,
+            "heading": 9.093200341480335e-9,
+            "angularVelocity": -1.3555529933337065e-7,
+            "velocityX": -1.6527839316087698,
+            "velocityY": -8.193146909757129e-7,
             "timestamp": 0.7928794818682598
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -9.605270403416775e-8,
-            "velocityX": -0.8263919813281883,
-            "velocityY": -4.4536784825580755e-7,
+            "heading": 7.024905960060336e-33,
+            "angularVelocity": -9.17486204713572e-8,
+            "velocityX": -0.826391981328188,
+            "velocityY": -4.4522936458958866e-7,
             "timestamp": 0.8919894171017924
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": 4.934081445690025e-38,
-            "velocityY": -9.331512134237052e-34,
+            "heading": 0,
+            "angularVelocity": 2.1129142513587384e-31,
+            "velocityX": -1.9174161859426788e-38,
+            "velocityY": -1.530570219467473e-32,
             "timestamp": 0.9910993523353249
         }
     ]
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj
index ee5c9ea2..c4008c9d 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj	
@@ -3,128 +3,128 @@
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 1.4062915610922596e-31,
-            "velocityX": -1.6151999437241794e-32,
-            "velocityY": -1.528021229010722e-32,
+            "heading": 3.6296173977413556e-31,
+            "angularVelocity": 2.1802637913498067e-31,
+            "velocityX": -2.5358916422596453e-32,
+            "velocityY": 1.2879635720357984e-31,
             "timestamp": 0
         },
         {
-            "x": 4.206513702340257,
-            "y": 5.554388398990967,
-            "heading": 3.0326218488137218,
-            "angularVelocity": -1.231959178646129,
-            "velocityX": -0.6480723694798501,
-            "velocityY": -0.0643824026918128,
-            "timestamp": 0.0871604782426985
+            "x": 4.206523470432328,
+            "y": 5.554409314419878,
+            "heading": -0.10760113621403576,
+            "angularVelocity": -1.2344151326641672,
+            "velocityX": -0.6479065667114609,
+            "velocityY": -0.06413711903878926,
+            "timestamp": 0.08716770668697735
         },
         {
-            "x": 4.0911807616230185,
-            "y": 5.5411300283093805,
-            "heading": 2.844661289825114,
-            "angularVelocity": -2.156488385311873,
-            "velocityX": -1.323225193832629,
-            "velocityY": -0.15211447836104172,
-            "timestamp": 0.174320956485397
+            "x": 4.091196970051195,
+            "y": 5.54119730250816,
+            "heading": -0.2959135822275083,
+            "angularVelocity": -2.1603464536437884,
+            "velocityX": -1.3230415800116804,
+            "velocityY": -0.15157002993279564,
+            "timestamp": 0.1743354133739547
         },
         {
-            "x": 3.915187082588679,
-            "y": 5.516242186917599,
-            "heading": 2.6068500185118766,
-            "angularVelocity": -2.728430087901214,
-            "velocityX": -2.0191912961317713,
-            "velocityY": -0.2855404409608899,
-            "timestamp": 0.2614814347280955
+            "x": 3.9151950635806365,
+            "y": 5.5163708141067165,
+            "heading": -0.5340651676870422,
+            "angularVelocity": -2.732107961894037,
+            "velocityX": -2.0191182395401066,
+            "velocityY": -0.28481291231621586,
+            "timestamp": 0.261503120060932
         },
         {
-            "x": 3.678761658477574,
-            "y": 5.47299330729491,
-            "heading": 2.386294579571215,
-            "angularVelocity": -2.530452372307143,
-            "velocityX": -2.7125301384049063,
-            "velocityY": -0.4961982826925549,
-            "timestamp": 0.348641912970794
+            "x": 3.678758755729211,
+            "y": 5.473177786204715,
+            "heading": -0.7546652286499241,
+            "angularVelocity": -2.5307544427555704,
+            "velocityX": -2.712430059683426,
+            "velocityY": -0.49551639642315154,
+            "timestamp": 0.3486708267479094
         },
         {
-            "x": 3.3968919941686,
-            "y": 5.398640264151657,
-            "heading": 2.288226309782543,
-            "angularVelocity": -1.1251460726913511,
-            "velocityX": -3.233915990273797,
-            "velocityY": -0.8530591460984852,
-            "timestamp": 0.4358023912134925
+            "x": 3.3968844052666176,
+            "y": 5.398848722834353,
+            "heading": -0.8527881698077676,
+            "angularVelocity": -1.1256799666670962,
+            "velocityX": -3.2337015756857643,
+            "velocityY": -0.8527133062853276,
+            "timestamp": 0.43583853343488677
         },
         {
-            "x": 3.0985878200503763,
-            "y": 5.268158980231371,
-            "heading": 2.276780497851342,
-            "angularVelocity": -0.1313188289230101,
-            "velocityX": -3.4224705982864747,
-            "velocityY": -1.4970234967844136,
-            "timestamp": 0.522962869456191
+            "x": 3.0986154730622166,
+            "y": 5.268356785097819,
+            "heading": -0.8643849747424638,
+            "angularVelocity": -0.1330401518573905,
+            "velocityX": -3.4217824873550513,
+            "velocityY": -1.4970215770977777,
+            "timestamp": 0.5230062401218641
         },
         {
-            "x": 2.8320780769343714,
-            "y": 5.083465560010488,
-            "heading": 2.2937556690853027,
-            "angularVelocity": 0.19475766512768708,
-            "velocityX": -3.0576902340290975,
-            "velocityY": -2.11900420861165,
-            "timestamp": 0.6101233476988894
+            "x": 2.8320964566491864,
+            "y": 5.083619646782129,
+            "heading": -0.847429861393553,
+            "angularVelocity": 0.19451140787489107,
+            "velocityX": -3.0575430574319302,
+            "velocityY": -2.11933002871221,
+            "timestamp": 0.6101739468088415
         },
         {
-            "x": 2.620143221340131,
-            "y": 4.878958491159276,
-            "heading": 2.4039835676400654,
-            "angularVelocity": 1.264654586311841,
-            "velocityX": -2.4315476448409066,
-            "velocityY": -2.34632798000215,
-            "timestamp": 0.6972838259415879
+            "x": 2.6201643415258493,
+            "y": 4.879059787480057,
+            "heading": -0.7372423358568471,
+            "angularVelocity": 1.2640865490747921,
+            "velocityX": -2.4313145679557007,
+            "velocityY": -2.3467390284415206,
+            "timestamp": 0.6973416534958189
         },
         {
-            "x": 2.469192165605932,
-            "y": 4.691124569407488,
-            "heading": 2.528247411458178,
-            "angularVelocity": 1.42569024773017,
-            "velocityX": -1.7318750284260176,
-            "velocityY": -2.155035464912952,
-            "timestamp": 0.7844443041842863
+            "x": 2.4692268443526784,
+            "y": 4.691160871442081,
+            "heading": -0.6128307115212047,
+            "angularVelocity": 1.4272673799072706,
+            "velocityX": -1.7315758657640712,
+            "velocityY": -2.1556023805091904,
+            "timestamp": 0.7845093601827963
         },
         {
-            "x": 2.3748255822885,
-            "y": 4.531333386407196,
-            "heading": 2.6349251655798476,
-            "angularVelocity": 1.2239234601791131,
-            "velocityX": -1.0826762911358505,
-            "velocityY": -1.833298603013082,
-            "timestamp": 0.8716047824269848
+            "x": 2.3748885078351467,
+            "y": 4.53130895090854,
+            "heading": -0.5057978375616147,
+            "angularVelocity": 1.227895949401916,
+            "velocityX": -1.0822624582324516,
+            "velocityY": -1.8338433648091068,
+            "timestamp": 0.8716770668697736
         },
         {
-            "x": 2.3223557063303257,
-            "y": 4.418269595148307,
-            "heading": 2.710801114054306,
-            "angularVelocity": 0.8705315758270817,
-            "velocityX": -0.6019916023415092,
-            "velocityY": -1.2971910381682688,
-            "timestamp": 0.9587652606696833
+            "x": 2.322390449292023,
+            "y": 4.41825305597818,
+            "heading": -0.4294901557678491,
+            "angularVelocity": 0.8754122907900764,
+            "velocityX": -0.6022649962748851,
+            "velocityY": -1.2969928799015857,
+            "timestamp": 0.958844773556751
         },
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0.44973234126302364,
-            "velocityX": -0.2564890278375538,
-            "velocityY": -0.6685323018312901,
-            "timestamp": 1.0459257389123817
+            "heading": -0.39,
+            "angularVelocity": 0.45303653461548066,
+            "velocityX": -0.2568663343688406,
+            "velocityY": -0.6682871236634659,
+            "timestamp": 1.0460124802437283
         },
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
+            "heading": -0.39,
             "angularVelocity": 0,
-            "velocityX": 6.1103463204495e-32,
-            "velocityY": 1.8635567714760387e-30,
-            "timestamp": 1.1330862171550802
+            "velocityX": 3.506966732338881e-31,
+            "velocityY": 5.081769191491498e-30,
+            "timestamp": 1.1331801869307057
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.traj
index ee5c9ea2..c4008c9d 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.traj	
@@ -3,128 +3,128 @@
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 1.4062915610922596e-31,
-            "velocityX": -1.6151999437241794e-32,
-            "velocityY": -1.528021229010722e-32,
+            "heading": 3.6296173977413556e-31,
+            "angularVelocity": 2.1802637913498067e-31,
+            "velocityX": -2.5358916422596453e-32,
+            "velocityY": 1.2879635720357984e-31,
             "timestamp": 0
         },
         {
-            "x": 4.206513702340257,
-            "y": 5.554388398990967,
-            "heading": 3.0326218488137218,
-            "angularVelocity": -1.231959178646129,
-            "velocityX": -0.6480723694798501,
-            "velocityY": -0.0643824026918128,
-            "timestamp": 0.0871604782426985
+            "x": 4.206523470432328,
+            "y": 5.554409314419878,
+            "heading": -0.10760113621403576,
+            "angularVelocity": -1.2344151326641672,
+            "velocityX": -0.6479065667114609,
+            "velocityY": -0.06413711903878926,
+            "timestamp": 0.08716770668697735
         },
         {
-            "x": 4.0911807616230185,
-            "y": 5.5411300283093805,
-            "heading": 2.844661289825114,
-            "angularVelocity": -2.156488385311873,
-            "velocityX": -1.323225193832629,
-            "velocityY": -0.15211447836104172,
-            "timestamp": 0.174320956485397
+            "x": 4.091196970051195,
+            "y": 5.54119730250816,
+            "heading": -0.2959135822275083,
+            "angularVelocity": -2.1603464536437884,
+            "velocityX": -1.3230415800116804,
+            "velocityY": -0.15157002993279564,
+            "timestamp": 0.1743354133739547
         },
         {
-            "x": 3.915187082588679,
-            "y": 5.516242186917599,
-            "heading": 2.6068500185118766,
-            "angularVelocity": -2.728430087901214,
-            "velocityX": -2.0191912961317713,
-            "velocityY": -0.2855404409608899,
-            "timestamp": 0.2614814347280955
+            "x": 3.9151950635806365,
+            "y": 5.5163708141067165,
+            "heading": -0.5340651676870422,
+            "angularVelocity": -2.732107961894037,
+            "velocityX": -2.0191182395401066,
+            "velocityY": -0.28481291231621586,
+            "timestamp": 0.261503120060932
         },
         {
-            "x": 3.678761658477574,
-            "y": 5.47299330729491,
-            "heading": 2.386294579571215,
-            "angularVelocity": -2.530452372307143,
-            "velocityX": -2.7125301384049063,
-            "velocityY": -0.4961982826925549,
-            "timestamp": 0.348641912970794
+            "x": 3.678758755729211,
+            "y": 5.473177786204715,
+            "heading": -0.7546652286499241,
+            "angularVelocity": -2.5307544427555704,
+            "velocityX": -2.712430059683426,
+            "velocityY": -0.49551639642315154,
+            "timestamp": 0.3486708267479094
         },
         {
-            "x": 3.3968919941686,
-            "y": 5.398640264151657,
-            "heading": 2.288226309782543,
-            "angularVelocity": -1.1251460726913511,
-            "velocityX": -3.233915990273797,
-            "velocityY": -0.8530591460984852,
-            "timestamp": 0.4358023912134925
+            "x": 3.3968844052666176,
+            "y": 5.398848722834353,
+            "heading": -0.8527881698077676,
+            "angularVelocity": -1.1256799666670962,
+            "velocityX": -3.2337015756857643,
+            "velocityY": -0.8527133062853276,
+            "timestamp": 0.43583853343488677
         },
         {
-            "x": 3.0985878200503763,
-            "y": 5.268158980231371,
-            "heading": 2.276780497851342,
-            "angularVelocity": -0.1313188289230101,
-            "velocityX": -3.4224705982864747,
-            "velocityY": -1.4970234967844136,
-            "timestamp": 0.522962869456191
+            "x": 3.0986154730622166,
+            "y": 5.268356785097819,
+            "heading": -0.8643849747424638,
+            "angularVelocity": -0.1330401518573905,
+            "velocityX": -3.4217824873550513,
+            "velocityY": -1.4970215770977777,
+            "timestamp": 0.5230062401218641
         },
         {
-            "x": 2.8320780769343714,
-            "y": 5.083465560010488,
-            "heading": 2.2937556690853027,
-            "angularVelocity": 0.19475766512768708,
-            "velocityX": -3.0576902340290975,
-            "velocityY": -2.11900420861165,
-            "timestamp": 0.6101233476988894
+            "x": 2.8320964566491864,
+            "y": 5.083619646782129,
+            "heading": -0.847429861393553,
+            "angularVelocity": 0.19451140787489107,
+            "velocityX": -3.0575430574319302,
+            "velocityY": -2.11933002871221,
+            "timestamp": 0.6101739468088415
         },
         {
-            "x": 2.620143221340131,
-            "y": 4.878958491159276,
-            "heading": 2.4039835676400654,
-            "angularVelocity": 1.264654586311841,
-            "velocityX": -2.4315476448409066,
-            "velocityY": -2.34632798000215,
-            "timestamp": 0.6972838259415879
+            "x": 2.6201643415258493,
+            "y": 4.879059787480057,
+            "heading": -0.7372423358568471,
+            "angularVelocity": 1.2640865490747921,
+            "velocityX": -2.4313145679557007,
+            "velocityY": -2.3467390284415206,
+            "timestamp": 0.6973416534958189
         },
         {
-            "x": 2.469192165605932,
-            "y": 4.691124569407488,
-            "heading": 2.528247411458178,
-            "angularVelocity": 1.42569024773017,
-            "velocityX": -1.7318750284260176,
-            "velocityY": -2.155035464912952,
-            "timestamp": 0.7844443041842863
+            "x": 2.4692268443526784,
+            "y": 4.691160871442081,
+            "heading": -0.6128307115212047,
+            "angularVelocity": 1.4272673799072706,
+            "velocityX": -1.7315758657640712,
+            "velocityY": -2.1556023805091904,
+            "timestamp": 0.7845093601827963
         },
         {
-            "x": 2.3748255822885,
-            "y": 4.531333386407196,
-            "heading": 2.6349251655798476,
-            "angularVelocity": 1.2239234601791131,
-            "velocityX": -1.0826762911358505,
-            "velocityY": -1.833298603013082,
-            "timestamp": 0.8716047824269848
+            "x": 2.3748885078351467,
+            "y": 4.53130895090854,
+            "heading": -0.5057978375616147,
+            "angularVelocity": 1.227895949401916,
+            "velocityX": -1.0822624582324516,
+            "velocityY": -1.8338433648091068,
+            "timestamp": 0.8716770668697736
         },
         {
-            "x": 2.3223557063303257,
-            "y": 4.418269595148307,
-            "heading": 2.710801114054306,
-            "angularVelocity": 0.8705315758270817,
-            "velocityX": -0.6019916023415092,
-            "velocityY": -1.2971910381682688,
-            "timestamp": 0.9587652606696833
+            "x": 2.322390449292023,
+            "y": 4.41825305597818,
+            "heading": -0.4294901557678491,
+            "angularVelocity": 0.8754122907900764,
+            "velocityX": -0.6022649962748851,
+            "velocityY": -1.2969928799015857,
+            "timestamp": 0.958844773556751
         },
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0.44973234126302364,
-            "velocityX": -0.2564890278375538,
-            "velocityY": -0.6685323018312901,
-            "timestamp": 1.0459257389123817
+            "heading": -0.39,
+            "angularVelocity": 0.45303653461548066,
+            "velocityX": -0.2568663343688406,
+            "velocityY": -0.6682871236634659,
+            "timestamp": 1.0460124802437283
         },
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
+            "heading": -0.39,
             "angularVelocity": 0,
-            "velocityX": 6.1103463204495e-32,
-            "velocityY": 1.8635567714760387e-30,
-            "timestamp": 1.1330862171550802
+            "velocityX": 3.506966732338881e-31,
+            "velocityY": 5.081769191491498e-30,
+            "timestamp": 1.1331801869307057
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C1.1.traj
new file mode 100644
index 00000000..97fb7eaf
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C1.1.traj	
@@ -0,0 +1,220 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 3.2688003829398086e-32,
+            "velocityX": -2.078713781970587e-32,
+            "velocityY": -4.638967376158192e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 4.29154268151456,
+            "y": 3.0623077457400183,
+            "heading": -0.5127665183876435,
+            "angularVelocity": 0.580669777278592,
+            "velocityX": 0.31134301225465,
+            "velocityY": 0.6796516730776717,
+            "timestamp": 0.09167599846825866
+        },
+        {
+            "x": 4.3492977760941525,
+            "y": 3.186661868715021,
+            "heading": -0.4070750475472326,
+            "angularVelocity": 1.1528804987818582,
+            "velocityX": 0.6299914431757085,
+            "velocityY": 1.3564523436094174,
+            "timestamp": 0.18335199693651733
+        },
+        {
+            "x": 4.438053987492854,
+            "y": 3.3723523918382017,
+            "heading": -0.25179906449139605,
+            "angularVelocity": 1.6937473891773143,
+            "velocityX": 0.968151019696088,
+            "velocityY": 2.025508597950785,
+            "timestamp": 0.27502799540477596
+        },
+        {
+            "x": 4.5709698511757795,
+            "y": 3.6117104026755453,
+            "heading": -0.06452556421817643,
+            "angularVelocity": 2.042775681773022,
+            "velocityX": 1.449843643960376,
+            "velocityY": 2.6109125053076734,
+            "timestamp": 0.36670399387303465
+        },
+        {
+            "x": 4.761013106791701,
+            "y": 3.810602792551275,
+            "heading": 0.11153690871840848,
+            "angularVelocity": 1.9204860146414893,
+            "velocityX": 2.0729881189319217,
+            "velocityY": 2.1695143025313537,
+            "timestamp": 0.45837999234129334
+        },
+        {
+            "x": 5.007328053154023,
+            "y": 3.968591564774131,
+            "heading": 0.25752334504560864,
+            "angularVelocity": 1.5924171949733021,
+            "velocityX": 2.6867986220799525,
+            "velocityY": 1.7233384404049508,
+            "timestamp": 0.550055990809552
+        },
+        {
+            "x": 5.305296422655045,
+            "y": 4.081628168088871,
+            "heading": 0.35020186087766364,
+            "angularVelocity": 1.0109354398157564,
+            "velocityX": 3.2502331524012615,
+            "velocityY": 1.2330010602925316,
+            "timestamp": 0.6417319892778107
+        },
+        {
+            "x": 5.6446965706540295,
+            "y": 4.152909996485894,
+            "heading": 0.3502021264888923,
+            "angularVelocity": 0.0000028972820926514052,
+            "velocityX": 3.702170182705961,
+            "velocityY": 0.7775407913523397,
+            "timestamp": 0.7334079877460694
+        },
+        {
+            "x": 5.981889969698284,
+            "y": 4.233991234618027,
+            "heading": 0.3502024148432213,
+            "angularVelocity": 0.0000031453633856654694,
+            "velocityX": 3.6780990082262504,
+            "velocityY": 0.884432561268565,
+            "timestamp": 0.8250839862143281
+        },
+        {
+            "x": 6.295899946950055,
+            "y": 4.381202864314355,
+            "heading": 0.350202827556894,
+            "angularVelocity": 0.000004501872678764248,
+            "velocityX": 3.425214696303447,
+            "velocityY": 1.6057815803042044,
+            "timestamp": 0.9167599846825868
+        },
+        {
+            "x": 6.57390491063703,
+            "y": 4.588535293884794,
+            "heading": 0.350203324900352,
+            "angularVelocity": 0.0000054250127210279085,
+            "velocityX": 3.0324727118541346,
+            "velocityY": 2.2615780906081357,
+            "timestamp": 1.0084359831508454
+        },
+        {
+            "x": 6.804553755441848,
+            "y": 4.847523149382792,
+            "heading": 0.35020327996165895,
+            "angularVelocity": -4.901903857536901e-7,
+            "velocityX": 2.5159130923965525,
+            "velocityY": 2.825034467311176,
+            "timestamp": 1.100111981619104
+        },
+        {
+            "x": 6.978428828519116,
+            "y": 5.147591577808381,
+            "heading": 0.35020324373344786,
+            "angularVelocity": -3.9517661894346874e-7,
+            "velocityX": 1.8966258997164847,
+            "velocityY": 3.273140554116584,
+            "timestamp": 1.1917879800873628
+        },
+        {
+            "x": 7.088431556948234,
+            "y": 5.476488224562697,
+            "heading": 0.3502032113343976,
+            "angularVelocity": -3.5340820734555857e-7,
+            "velocityX": 1.1999076123202526,
+            "velocityY": 3.587598196360961,
+            "timestamp": 1.2834639785556214
+        },
+        {
+            "x": 7.187536215850895,
+            "y": 5.808831226900728,
+            "heading": 0.3502031791295722,
+            "angularVelocity": -3.5128960565806065e-7,
+            "velocityX": 1.0810316828670656,
+            "velocityY": 3.6251909757285055,
+            "timestamp": 1.3751399770238801
+        },
+        {
+            "x": 7.286640767886556,
+            "y": 6.141174261106461,
+            "heading": 0.3502031469247365,
+            "angularVelocity": -3.512897194087527e-7,
+            "velocityX": 1.0810305171639119,
+            "velocityY": 3.6251913233407715,
+            "timestamp": 1.4668159754921388
+        },
+        {
+            "x": 7.3857452620780215,
+            "y": 6.473517312561292,
+            "heading": 0.350203114719879,
+            "angularVelocity": -3.512899557892109e-7,
+            "velocityX": 1.0810298862005645,
+            "velocityY": 3.625191511493593,
+            "timestamp": 1.5584919739603975
+        },
+        {
+            "x": 7.481976228665169,
+            "y": 6.80241098377653,
+            "heading": 0.33998449018149474,
+            "angularVelocity": -0.11146455679915658,
+            "velocityX": 1.0496855032395944,
+            "velocityY": 3.5875657392388547,
+            "timestamp": 1.6501679724286562
+        },
+        {
+            "x": 7.561455250393006,
+            "y": 7.068914029375232,
+            "heading": 0.2316909043200923,
+            "angularVelocity": -1.1812643185871456,
+            "velocityX": 0.8669556160367877,
+            "velocityY": 2.907010014087551,
+            "timestamp": 1.741843970896915
+        },
+        {
+            "x": 7.620860593043084,
+            "y": 7.268277067392774,
+            "heading": 0.12481105079738766,
+            "angularVelocity": -1.1658433538600752,
+            "velocityX": 0.6479923168837385,
+            "velocityY": 2.1746481232660675,
+            "timestamp": 1.8335199693651736
+        },
+        {
+            "x": 7.66032628893014,
+            "y": 7.4008489817464715,
+            "heading": 0.04379045785830094,
+            "angularVelocity": -0.8837710446877722,
+            "velocityX": 0.430491039600946,
+            "velocityY": 1.4460918513977026,
+            "timestamp": 1.9251959678334323
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 6.52409657127002e-31,
+            "angularVelocity": -0.4776654586801488,
+            "velocityX": 0.21460045593800534,
+            "velocityY": 0.7215740145599008,
+            "timestamp": 2.0168719663016907
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": -6.745238901014272e-30,
+            "velocityX": 0,
+            "velocityY": 3.373152313643601e-31,
+            "timestamp": 2.1085479647699494
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C1.traj
new file mode 100644
index 00000000..97fb7eaf
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C1.traj	
@@ -0,0 +1,220 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 3.2688003829398086e-32,
+            "velocityX": -2.078713781970587e-32,
+            "velocityY": -4.638967376158192e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 4.29154268151456,
+            "y": 3.0623077457400183,
+            "heading": -0.5127665183876435,
+            "angularVelocity": 0.580669777278592,
+            "velocityX": 0.31134301225465,
+            "velocityY": 0.6796516730776717,
+            "timestamp": 0.09167599846825866
+        },
+        {
+            "x": 4.3492977760941525,
+            "y": 3.186661868715021,
+            "heading": -0.4070750475472326,
+            "angularVelocity": 1.1528804987818582,
+            "velocityX": 0.6299914431757085,
+            "velocityY": 1.3564523436094174,
+            "timestamp": 0.18335199693651733
+        },
+        {
+            "x": 4.438053987492854,
+            "y": 3.3723523918382017,
+            "heading": -0.25179906449139605,
+            "angularVelocity": 1.6937473891773143,
+            "velocityX": 0.968151019696088,
+            "velocityY": 2.025508597950785,
+            "timestamp": 0.27502799540477596
+        },
+        {
+            "x": 4.5709698511757795,
+            "y": 3.6117104026755453,
+            "heading": -0.06452556421817643,
+            "angularVelocity": 2.042775681773022,
+            "velocityX": 1.449843643960376,
+            "velocityY": 2.6109125053076734,
+            "timestamp": 0.36670399387303465
+        },
+        {
+            "x": 4.761013106791701,
+            "y": 3.810602792551275,
+            "heading": 0.11153690871840848,
+            "angularVelocity": 1.9204860146414893,
+            "velocityX": 2.0729881189319217,
+            "velocityY": 2.1695143025313537,
+            "timestamp": 0.45837999234129334
+        },
+        {
+            "x": 5.007328053154023,
+            "y": 3.968591564774131,
+            "heading": 0.25752334504560864,
+            "angularVelocity": 1.5924171949733021,
+            "velocityX": 2.6867986220799525,
+            "velocityY": 1.7233384404049508,
+            "timestamp": 0.550055990809552
+        },
+        {
+            "x": 5.305296422655045,
+            "y": 4.081628168088871,
+            "heading": 0.35020186087766364,
+            "angularVelocity": 1.0109354398157564,
+            "velocityX": 3.2502331524012615,
+            "velocityY": 1.2330010602925316,
+            "timestamp": 0.6417319892778107
+        },
+        {
+            "x": 5.6446965706540295,
+            "y": 4.152909996485894,
+            "heading": 0.3502021264888923,
+            "angularVelocity": 0.0000028972820926514052,
+            "velocityX": 3.702170182705961,
+            "velocityY": 0.7775407913523397,
+            "timestamp": 0.7334079877460694
+        },
+        {
+            "x": 5.981889969698284,
+            "y": 4.233991234618027,
+            "heading": 0.3502024148432213,
+            "angularVelocity": 0.0000031453633856654694,
+            "velocityX": 3.6780990082262504,
+            "velocityY": 0.884432561268565,
+            "timestamp": 0.8250839862143281
+        },
+        {
+            "x": 6.295899946950055,
+            "y": 4.381202864314355,
+            "heading": 0.350202827556894,
+            "angularVelocity": 0.000004501872678764248,
+            "velocityX": 3.425214696303447,
+            "velocityY": 1.6057815803042044,
+            "timestamp": 0.9167599846825868
+        },
+        {
+            "x": 6.57390491063703,
+            "y": 4.588535293884794,
+            "heading": 0.350203324900352,
+            "angularVelocity": 0.0000054250127210279085,
+            "velocityX": 3.0324727118541346,
+            "velocityY": 2.2615780906081357,
+            "timestamp": 1.0084359831508454
+        },
+        {
+            "x": 6.804553755441848,
+            "y": 4.847523149382792,
+            "heading": 0.35020327996165895,
+            "angularVelocity": -4.901903857536901e-7,
+            "velocityX": 2.5159130923965525,
+            "velocityY": 2.825034467311176,
+            "timestamp": 1.100111981619104
+        },
+        {
+            "x": 6.978428828519116,
+            "y": 5.147591577808381,
+            "heading": 0.35020324373344786,
+            "angularVelocity": -3.9517661894346874e-7,
+            "velocityX": 1.8966258997164847,
+            "velocityY": 3.273140554116584,
+            "timestamp": 1.1917879800873628
+        },
+        {
+            "x": 7.088431556948234,
+            "y": 5.476488224562697,
+            "heading": 0.3502032113343976,
+            "angularVelocity": -3.5340820734555857e-7,
+            "velocityX": 1.1999076123202526,
+            "velocityY": 3.587598196360961,
+            "timestamp": 1.2834639785556214
+        },
+        {
+            "x": 7.187536215850895,
+            "y": 5.808831226900728,
+            "heading": 0.3502031791295722,
+            "angularVelocity": -3.5128960565806065e-7,
+            "velocityX": 1.0810316828670656,
+            "velocityY": 3.6251909757285055,
+            "timestamp": 1.3751399770238801
+        },
+        {
+            "x": 7.286640767886556,
+            "y": 6.141174261106461,
+            "heading": 0.3502031469247365,
+            "angularVelocity": -3.512897194087527e-7,
+            "velocityX": 1.0810305171639119,
+            "velocityY": 3.6251913233407715,
+            "timestamp": 1.4668159754921388
+        },
+        {
+            "x": 7.3857452620780215,
+            "y": 6.473517312561292,
+            "heading": 0.350203114719879,
+            "angularVelocity": -3.512899557892109e-7,
+            "velocityX": 1.0810298862005645,
+            "velocityY": 3.625191511493593,
+            "timestamp": 1.5584919739603975
+        },
+        {
+            "x": 7.481976228665169,
+            "y": 6.80241098377653,
+            "heading": 0.33998449018149474,
+            "angularVelocity": -0.11146455679915658,
+            "velocityX": 1.0496855032395944,
+            "velocityY": 3.5875657392388547,
+            "timestamp": 1.6501679724286562
+        },
+        {
+            "x": 7.561455250393006,
+            "y": 7.068914029375232,
+            "heading": 0.2316909043200923,
+            "angularVelocity": -1.1812643185871456,
+            "velocityX": 0.8669556160367877,
+            "velocityY": 2.907010014087551,
+            "timestamp": 1.741843970896915
+        },
+        {
+            "x": 7.620860593043084,
+            "y": 7.268277067392774,
+            "heading": 0.12481105079738766,
+            "angularVelocity": -1.1658433538600752,
+            "velocityX": 0.6479923168837385,
+            "velocityY": 2.1746481232660675,
+            "timestamp": 1.8335199693651736
+        },
+        {
+            "x": 7.66032628893014,
+            "y": 7.4008489817464715,
+            "heading": 0.04379045785830094,
+            "angularVelocity": -0.8837710446877722,
+            "velocityX": 0.430491039600946,
+            "velocityY": 1.4460918513977026,
+            "timestamp": 1.9251959678334323
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 6.52409657127002e-31,
+            "angularVelocity": -0.4776654586801488,
+            "velocityX": 0.21460045593800534,
+            "velocityY": 0.7215740145599008,
+            "timestamp": 2.0168719663016907
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": -6.745238901014272e-30,
+            "velocityX": 0,
+            "velocityY": 3.373152313643601e-31,
+            "timestamp": 2.1085479647699494
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.1.traj
index 8d9b9558..69777c27 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.1.traj	
@@ -3,191 +3,191 @@
         {
             "x": 4.263,
             "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 1.5794772183943222e-24,
-            "velocityX": -1.3909195862275997e-25,
-            "velocityY": -1.589027573571451e-24,
+            "heading": -0.566,
+            "angularVelocity": -2.8196964588027846e-31,
+            "velocityX": -1.564828148658428e-31,
+            "velocityY": 3.07478616058615e-32,
             "timestamp": 0
         },
         {
-            "x": 4.294802050577596,
-            "y": 3.0477360619772367,
-            "heading": 2.544164914123716,
-            "angularVelocity": -0.3578657180774014,
-            "velocityX": 0.38145905506951994,
-            "velocityY": 0.5725842442186713,
-            "timestamp": 0.08336949970108862
-        },
-        {
-            "x": 4.359925633327058,
-            "y": 3.142202439962799,
-            "heading": 2.4853835413264154,
-            "angularVelocity": -0.7050704755102848,
-            "velocityX": 0.781143979308431,
-            "velocityY": 1.13310477241989,
-            "timestamp": 0.16673899940217723
-        },
-        {
-            "x": 4.461027910894954,
-            "y": 3.2814650300922756,
-            "heading": 2.399327847716391,
-            "angularVelocity": -1.0322203433937678,
-            "velocityX": 1.2127010229206814,
-            "velocityY": 1.6704261226082242,
-            "timestamp": 0.2501084991032658
-        },
-        {
-            "x": 4.603798889123446,
-            "y": 3.46048671868814,
-            "heading": 2.2901235890618308,
-            "angularVelocity": -1.309882619496323,
-            "velocityX": 1.7125085161885507,
-            "velocityY": 2.147328330357326,
-            "timestamp": 0.33347799880435447
-        },
-        {
-            "x": 4.803010081194681,
-            "y": 3.6526247699506835,
-            "heading": 2.177140187929377,
-            "angularVelocity": -1.3552126561577384,
-            "velocityX": 2.389497271609915,
-            "velocityY": 2.3046564025384755,
-            "timestamp": 0.4168474985054431
-        },
-        {
-            "x": 5.0510307536076,
-            "y": 3.8467573295012585,
-            "heading": 2.175769825428097,
-            "angularVelocity": -0.016437216322438507,
-            "velocityX": 2.9749569483104428,
-            "velocityY": 2.328580119187639,
-            "timestamp": 0.5002169982065318
-        },
-        {
-            "x": 5.320458962981737,
-            "y": 4.010690590396415,
-            "heading": 2.175769541029465,
-            "angularVelocity": -0.000003411303085546954,
-            "velocityX": 3.231735950679083,
-            "velocityY": 1.9663457437422465,
-            "timestamp": 0.5835864979076204
-        },
-        {
-            "x": 5.614141443438261,
-            "y": 4.125652797356739,
-            "heading": 2.175769455965937,
-            "angularVelocity": -0.0000010203195273976758,
-            "velocityX": 3.522660943264475,
-            "velocityY": 1.3789480250272361,
-            "timestamp": 0.666955997608709
-        },
-        {
-            "x": 5.906454716700063,
-            "y": 4.244053209777629,
-            "heading": 2.1757695445063954,
-            "angularVelocity": 0.000001062024591118681,
-            "velocityX": 3.506237584606561,
-            "velocityY": 1.4201885922957453,
-            "timestamp": 0.7503254973097977
-        },
-        {
-            "x": 6.172161728896083,
-            "y": 4.41393857041553,
-            "heading": 2.1757932909481013,
-            "angularVelocity": 0.00028483368367179416,
-            "velocityX": 3.187100955969282,
-            "velocityY": 2.0377399558232225,
-            "timestamp": 0.8336949970108863
-        },
-        {
-            "x": 6.426803794432408,
-            "y": 4.599225132728127,
-            "heading": 2.1773610490246513,
-            "angularVelocity": 0.018804935643986023,
-            "velocityX": 3.0543791968203453,
-            "velocityY": 2.2224742019194084,
-            "timestamp": 0.917064496711975
-        },
-        {
-            "x": 6.681572955411179,
-            "y": 4.785089288083454,
-            "heading": 2.177430344851842,
-            "angularVelocity": 0.0008311891931529928,
-            "velocityX": 3.0559036805092323,
-            "velocityY": 2.2294023116574087,
-            "timestamp": 1.0004339964130635
-        },
-        {
-            "x": 6.9013556581674305,
-            "y": 5.011277814262196,
-            "heading": 2.1774305498100937,
-            "angularVelocity": 0.000002458432073886186,
-            "velocityX": 2.636248310764209,
-            "velocityY": 2.7130848450538085,
-            "timestamp": 1.083803496114152
-        },
-        {
-            "x": 7.121935686700776,
-            "y": 5.230936165800854,
-            "heading": 2.189684453697612,
-            "angularVelocity": 0.14698305653090518,
-            "velocityX": 2.645812069452359,
-            "velocityY": 2.634756743487918,
-            "timestamp": 1.1671729958152406
-        },
-        {
-            "x": 7.311158558493755,
-            "y": 5.417630564055526,
-            "heading": 2.349466738853841,
-            "angularVelocity": 1.9165556435999858,
-            "velocityX": 2.2696894244467765,
-            "velocityY": 2.239360904455968,
-            "timestamp": 1.2505424955163291
-        },
-        {
-            "x": 7.462052071844248,
-            "y": 5.571824642162805,
-            "heading": 2.618524466568701,
-            "angularVelocity": 3.2272920993832837,
-            "velocityX": 1.8099366541901185,
-            "velocityY": 1.8495262495291969,
-            "timestamp": 1.3339119952174177
-        },
-        {
-            "x": 7.571973798709058,
-            "y": 5.685339226077728,
-            "heading": 2.8696843432959835,
-            "angularVelocity": 3.0126110583341195,
-            "velocityX": 1.3184885030967077,
-            "velocityY": 1.3615840843703815,
-            "timestamp": 1.4172814949185062
-        },
-        {
-            "x": 7.644303858666078,
-            "y": 5.759924476296826,
-            "heading": 3.0477755113688096,
-            "angularVelocity": 2.1361669280894184,
-            "velocityX": 0.8675841910572869,
-            "velocityY": 0.8946347343634476,
-            "timestamp": 1.5006509946195947
+            "x": 4.294807569207289,
+            "y": 3.0477321348748183,
+            "heading": -0.5960373953696199,
+            "angularVelocity": -0.36026847684168845,
+            "velocityX": 0.381499939303545,
+            "velocityY": 0.5724991570056517,
+            "timestamp": 0.08337503084628527
+        },
+        {
+            "x": 4.359939756522268,
+            "y": 3.142193816557181,
+            "heading": -0.6551934033575421,
+            "angularVelocity": -0.7095170746861289,
+            "velocityX": 0.7811953609355742,
+            "velocityY": 1.1329732741750633,
+            "timestamp": 0.16675006169257053
+        },
+        {
+            "x": 4.461050205756915,
+            "y": 3.2814565261988764,
+            "heading": -0.7417418377173162,
+            "angularVelocity": -1.038061797174498,
+            "velocityX": 1.2127185826300881,
+            "velocityY": 1.6703167390540123,
+            "timestamp": 0.2501250925388558
+        },
+        {
+            "x": 4.603825208189836,
+            "y": 3.4604941356958814,
+            "heading": -0.8514332359801334,
+            "angularVelocity": -1.315638472926628,
+            "velocityX": 1.7124431737380506,
+            "velocityY": 2.147376830685539,
+            "timestamp": 0.33350012338514107
+        },
+        {
+            "x": 4.8030511460470855,
+            "y": 3.6526461815261504,
+            "heading": -0.9644207302174959,
+            "angularVelocity": -1.3551718433024906,
+            "velocityX": 2.3895156119888465,
+            "velocityY": 2.3046713611960277,
+            "timestamp": 0.41687515423142635
+        },
+        {
+            "x": 5.051097476442208,
+            "y": 3.8467657037250893,
+            "heading": -0.9658198239131095,
+            "angularVelocity": -0.016780727772000164,
+            "velocityX": 2.975067329839247,
+            "velocityY": 2.328269269930804,
+            "timestamp": 0.5002501850777116
+        },
+        {
+            "x": 5.320526470824812,
+            "y": 4.010737925204951,
+            "heading": -0.965820109061322,
+            "angularVelocity": -0.000003420067250854742,
+            "velocityX": 3.2315309709370754,
+            "velocityY": 1.9666825884858703,
+            "timestamp": 0.5836252159239969
+        },
+        {
+            "x": 5.614243645591117,
+            "y": 4.1256688963704224,
+            "heading": -0.9658201941489747,
+            "angularVelocity": -0.000001020541184209393,
+            "velocityX": 3.5228433715103167,
+            "velocityY": 1.37848190278171,
+            "timestamp": 0.6670002467702821
+        },
+        {
+            "x": 5.906557788271019,
+            "y": 4.244122886725936,
+            "heading": -0.965820105112684,
+            "angularVelocity": 0.0000010679011417361832,
+            "velocityX": 3.506015406684877,
+            "velocityY": 1.4207369898777267,
+            "timestamp": 0.7503752776165674
+        },
+        {
+            "x": 6.172253111887642,
+            "y": 4.41406398245588,
+            "heading": -0.9657938677546691,
+            "angularVelocity": 0.0003146908342779248,
+            "velocityX": 3.1867493291423483,
+            "velocityY": 2.0382732576526004,
+            "timestamp": 0.8337503084628527
+        },
+        {
+            "x": 6.426785390452526,
+            "y": 4.5994304003649855,
+            "heading": -0.9640144900160125,
+            "angularVelocity": 0.021341854037057975,
+            "velocityX": 3.0528597828545716,
+            "velocityY": 2.2232845496735987,
+            "timestamp": 0.917125339309138
+        },
+        {
+            "x": 6.681598941757368,
+            "y": 4.785246665794811,
+            "heading": -0.9639002296367303,
+            "angularVelocity": 0.0013704388246982366,
+            "velocityX": 3.0562333676929114,
+            "velocityY": 2.2286800201898096,
+            "timestamp": 1.0005003701554231
+        },
+        {
+            "x": 6.9014822245071255,
+            "y": 5.01136660335564,
+            "heading": -0.9639000238890051,
+            "angularVelocity": 0.000002467737919950576,
+            "velocityX": 2.6372797769052228,
+            "velocityY": 2.7120822057351384,
+            "timestamp": 1.0838754010017084
+        },
+        {
+            "x": 7.122012116423131,
+            "y": 5.23107273201418,
+            "heading": -0.9515772318649645,
+            "angularVelocity": 0.1477995498047812,
+            "velocityX": 2.645035206314798,
+            "velocityY": 2.635154990990089,
+            "timestamp": 1.1672504318479937
+        },
+        {
+            "x": 7.311148990719613,
+            "y": 5.417840497645029,
+            "heading": -0.7917020160601519,
+            "angularVelocity": 1.9175431083146124,
+            "velocityX": 2.2685073981583956,
+            "velocityY": 2.2400923122317544,
+            "timestamp": 1.250625462694279
+        },
+        {
+            "x": 7.462007390054419,
+            "y": 5.572027096128993,
+            "heading": -0.5225549044290918,
+            "angularVelocity": 3.228150069620655,
+            "velocityX": 1.8093954245478587,
+            "velocityY": 1.8493138403538476,
+            "timestamp": 1.3340004935405643
+        },
+        {
+            "x": 7.571928087470151,
+            "y": 5.685474744918936,
+            "heading": -0.27088873612536774,
+            "angularVelocity": 3.018483660506339,
+            "velocityX": 1.318388686636741,
+            "velocityY": 1.3606909363440005,
+            "timestamp": 1.4173755243868496
+        },
+        {
+            "x": 7.6442805622690475,
+            "y": 5.7599789636443175,
+            "heading": -0.0924144798498188,
+            "angularVelocity": 2.1406199729580164,
+            "velocityX": 0.8677954786282445,
+            "velocityY": 0.8936034921863096,
+            "timestamp": 1.5007505552331348
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 1.1062137707656932,
-            "velocityX": 0.4281678726861332,
-            "velocityY": 0.44471328046952424,
-            "timestamp": 1.5840204943206833
+            "heading": -2.5332548087748344e-31,
+            "angularVelocity": 1.1084191383412998,
+            "velocityX": 0.42841888474747514,
+            "velocityY": 0.4440302567795869,
+            "timestamp": 1.5841255860794201
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 1.4380080051319566e-21,
+            "heading": 0,
+            "angularVelocity": 5.737670210968143e-33,
             "velocityX": 0,
-            "velocityY": 1.143279687808587e-23,
-            "timestamp": 1.6673899940217718
+            "velocityY": -3.2710687085311585e-31,
+            "timestamp": 1.6675006169257054
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.traj
index 8d9b9558..69777c27 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.traj	
@@ -3,191 +3,191 @@
         {
             "x": 4.263,
             "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 1.5794772183943222e-24,
-            "velocityX": -1.3909195862275997e-25,
-            "velocityY": -1.589027573571451e-24,
+            "heading": -0.566,
+            "angularVelocity": -2.8196964588027846e-31,
+            "velocityX": -1.564828148658428e-31,
+            "velocityY": 3.07478616058615e-32,
             "timestamp": 0
         },
         {
-            "x": 4.294802050577596,
-            "y": 3.0477360619772367,
-            "heading": 2.544164914123716,
-            "angularVelocity": -0.3578657180774014,
-            "velocityX": 0.38145905506951994,
-            "velocityY": 0.5725842442186713,
-            "timestamp": 0.08336949970108862
-        },
-        {
-            "x": 4.359925633327058,
-            "y": 3.142202439962799,
-            "heading": 2.4853835413264154,
-            "angularVelocity": -0.7050704755102848,
-            "velocityX": 0.781143979308431,
-            "velocityY": 1.13310477241989,
-            "timestamp": 0.16673899940217723
-        },
-        {
-            "x": 4.461027910894954,
-            "y": 3.2814650300922756,
-            "heading": 2.399327847716391,
-            "angularVelocity": -1.0322203433937678,
-            "velocityX": 1.2127010229206814,
-            "velocityY": 1.6704261226082242,
-            "timestamp": 0.2501084991032658
-        },
-        {
-            "x": 4.603798889123446,
-            "y": 3.46048671868814,
-            "heading": 2.2901235890618308,
-            "angularVelocity": -1.309882619496323,
-            "velocityX": 1.7125085161885507,
-            "velocityY": 2.147328330357326,
-            "timestamp": 0.33347799880435447
-        },
-        {
-            "x": 4.803010081194681,
-            "y": 3.6526247699506835,
-            "heading": 2.177140187929377,
-            "angularVelocity": -1.3552126561577384,
-            "velocityX": 2.389497271609915,
-            "velocityY": 2.3046564025384755,
-            "timestamp": 0.4168474985054431
-        },
-        {
-            "x": 5.0510307536076,
-            "y": 3.8467573295012585,
-            "heading": 2.175769825428097,
-            "angularVelocity": -0.016437216322438507,
-            "velocityX": 2.9749569483104428,
-            "velocityY": 2.328580119187639,
-            "timestamp": 0.5002169982065318
-        },
-        {
-            "x": 5.320458962981737,
-            "y": 4.010690590396415,
-            "heading": 2.175769541029465,
-            "angularVelocity": -0.000003411303085546954,
-            "velocityX": 3.231735950679083,
-            "velocityY": 1.9663457437422465,
-            "timestamp": 0.5835864979076204
-        },
-        {
-            "x": 5.614141443438261,
-            "y": 4.125652797356739,
-            "heading": 2.175769455965937,
-            "angularVelocity": -0.0000010203195273976758,
-            "velocityX": 3.522660943264475,
-            "velocityY": 1.3789480250272361,
-            "timestamp": 0.666955997608709
-        },
-        {
-            "x": 5.906454716700063,
-            "y": 4.244053209777629,
-            "heading": 2.1757695445063954,
-            "angularVelocity": 0.000001062024591118681,
-            "velocityX": 3.506237584606561,
-            "velocityY": 1.4201885922957453,
-            "timestamp": 0.7503254973097977
-        },
-        {
-            "x": 6.172161728896083,
-            "y": 4.41393857041553,
-            "heading": 2.1757932909481013,
-            "angularVelocity": 0.00028483368367179416,
-            "velocityX": 3.187100955969282,
-            "velocityY": 2.0377399558232225,
-            "timestamp": 0.8336949970108863
-        },
-        {
-            "x": 6.426803794432408,
-            "y": 4.599225132728127,
-            "heading": 2.1773610490246513,
-            "angularVelocity": 0.018804935643986023,
-            "velocityX": 3.0543791968203453,
-            "velocityY": 2.2224742019194084,
-            "timestamp": 0.917064496711975
-        },
-        {
-            "x": 6.681572955411179,
-            "y": 4.785089288083454,
-            "heading": 2.177430344851842,
-            "angularVelocity": 0.0008311891931529928,
-            "velocityX": 3.0559036805092323,
-            "velocityY": 2.2294023116574087,
-            "timestamp": 1.0004339964130635
-        },
-        {
-            "x": 6.9013556581674305,
-            "y": 5.011277814262196,
-            "heading": 2.1774305498100937,
-            "angularVelocity": 0.000002458432073886186,
-            "velocityX": 2.636248310764209,
-            "velocityY": 2.7130848450538085,
-            "timestamp": 1.083803496114152
-        },
-        {
-            "x": 7.121935686700776,
-            "y": 5.230936165800854,
-            "heading": 2.189684453697612,
-            "angularVelocity": 0.14698305653090518,
-            "velocityX": 2.645812069452359,
-            "velocityY": 2.634756743487918,
-            "timestamp": 1.1671729958152406
-        },
-        {
-            "x": 7.311158558493755,
-            "y": 5.417630564055526,
-            "heading": 2.349466738853841,
-            "angularVelocity": 1.9165556435999858,
-            "velocityX": 2.2696894244467765,
-            "velocityY": 2.239360904455968,
-            "timestamp": 1.2505424955163291
-        },
-        {
-            "x": 7.462052071844248,
-            "y": 5.571824642162805,
-            "heading": 2.618524466568701,
-            "angularVelocity": 3.2272920993832837,
-            "velocityX": 1.8099366541901185,
-            "velocityY": 1.8495262495291969,
-            "timestamp": 1.3339119952174177
-        },
-        {
-            "x": 7.571973798709058,
-            "y": 5.685339226077728,
-            "heading": 2.8696843432959835,
-            "angularVelocity": 3.0126110583341195,
-            "velocityX": 1.3184885030967077,
-            "velocityY": 1.3615840843703815,
-            "timestamp": 1.4172814949185062
-        },
-        {
-            "x": 7.644303858666078,
-            "y": 5.759924476296826,
-            "heading": 3.0477755113688096,
-            "angularVelocity": 2.1361669280894184,
-            "velocityX": 0.8675841910572869,
-            "velocityY": 0.8946347343634476,
-            "timestamp": 1.5006509946195947
+            "x": 4.294807569207289,
+            "y": 3.0477321348748183,
+            "heading": -0.5960373953696199,
+            "angularVelocity": -0.36026847684168845,
+            "velocityX": 0.381499939303545,
+            "velocityY": 0.5724991570056517,
+            "timestamp": 0.08337503084628527
+        },
+        {
+            "x": 4.359939756522268,
+            "y": 3.142193816557181,
+            "heading": -0.6551934033575421,
+            "angularVelocity": -0.7095170746861289,
+            "velocityX": 0.7811953609355742,
+            "velocityY": 1.1329732741750633,
+            "timestamp": 0.16675006169257053
+        },
+        {
+            "x": 4.461050205756915,
+            "y": 3.2814565261988764,
+            "heading": -0.7417418377173162,
+            "angularVelocity": -1.038061797174498,
+            "velocityX": 1.2127185826300881,
+            "velocityY": 1.6703167390540123,
+            "timestamp": 0.2501250925388558
+        },
+        {
+            "x": 4.603825208189836,
+            "y": 3.4604941356958814,
+            "heading": -0.8514332359801334,
+            "angularVelocity": -1.315638472926628,
+            "velocityX": 1.7124431737380506,
+            "velocityY": 2.147376830685539,
+            "timestamp": 0.33350012338514107
+        },
+        {
+            "x": 4.8030511460470855,
+            "y": 3.6526461815261504,
+            "heading": -0.9644207302174959,
+            "angularVelocity": -1.3551718433024906,
+            "velocityX": 2.3895156119888465,
+            "velocityY": 2.3046713611960277,
+            "timestamp": 0.41687515423142635
+        },
+        {
+            "x": 5.051097476442208,
+            "y": 3.8467657037250893,
+            "heading": -0.9658198239131095,
+            "angularVelocity": -0.016780727772000164,
+            "velocityX": 2.975067329839247,
+            "velocityY": 2.328269269930804,
+            "timestamp": 0.5002501850777116
+        },
+        {
+            "x": 5.320526470824812,
+            "y": 4.010737925204951,
+            "heading": -0.965820109061322,
+            "angularVelocity": -0.000003420067250854742,
+            "velocityX": 3.2315309709370754,
+            "velocityY": 1.9666825884858703,
+            "timestamp": 0.5836252159239969
+        },
+        {
+            "x": 5.614243645591117,
+            "y": 4.1256688963704224,
+            "heading": -0.9658201941489747,
+            "angularVelocity": -0.000001020541184209393,
+            "velocityX": 3.5228433715103167,
+            "velocityY": 1.37848190278171,
+            "timestamp": 0.6670002467702821
+        },
+        {
+            "x": 5.906557788271019,
+            "y": 4.244122886725936,
+            "heading": -0.965820105112684,
+            "angularVelocity": 0.0000010679011417361832,
+            "velocityX": 3.506015406684877,
+            "velocityY": 1.4207369898777267,
+            "timestamp": 0.7503752776165674
+        },
+        {
+            "x": 6.172253111887642,
+            "y": 4.41406398245588,
+            "heading": -0.9657938677546691,
+            "angularVelocity": 0.0003146908342779248,
+            "velocityX": 3.1867493291423483,
+            "velocityY": 2.0382732576526004,
+            "timestamp": 0.8337503084628527
+        },
+        {
+            "x": 6.426785390452526,
+            "y": 4.5994304003649855,
+            "heading": -0.9640144900160125,
+            "angularVelocity": 0.021341854037057975,
+            "velocityX": 3.0528597828545716,
+            "velocityY": 2.2232845496735987,
+            "timestamp": 0.917125339309138
+        },
+        {
+            "x": 6.681598941757368,
+            "y": 4.785246665794811,
+            "heading": -0.9639002296367303,
+            "angularVelocity": 0.0013704388246982366,
+            "velocityX": 3.0562333676929114,
+            "velocityY": 2.2286800201898096,
+            "timestamp": 1.0005003701554231
+        },
+        {
+            "x": 6.9014822245071255,
+            "y": 5.01136660335564,
+            "heading": -0.9639000238890051,
+            "angularVelocity": 0.000002467737919950576,
+            "velocityX": 2.6372797769052228,
+            "velocityY": 2.7120822057351384,
+            "timestamp": 1.0838754010017084
+        },
+        {
+            "x": 7.122012116423131,
+            "y": 5.23107273201418,
+            "heading": -0.9515772318649645,
+            "angularVelocity": 0.1477995498047812,
+            "velocityX": 2.645035206314798,
+            "velocityY": 2.635154990990089,
+            "timestamp": 1.1672504318479937
+        },
+        {
+            "x": 7.311148990719613,
+            "y": 5.417840497645029,
+            "heading": -0.7917020160601519,
+            "angularVelocity": 1.9175431083146124,
+            "velocityX": 2.2685073981583956,
+            "velocityY": 2.2400923122317544,
+            "timestamp": 1.250625462694279
+        },
+        {
+            "x": 7.462007390054419,
+            "y": 5.572027096128993,
+            "heading": -0.5225549044290918,
+            "angularVelocity": 3.228150069620655,
+            "velocityX": 1.8093954245478587,
+            "velocityY": 1.8493138403538476,
+            "timestamp": 1.3340004935405643
+        },
+        {
+            "x": 7.571928087470151,
+            "y": 5.685474744918936,
+            "heading": -0.27088873612536774,
+            "angularVelocity": 3.018483660506339,
+            "velocityX": 1.318388686636741,
+            "velocityY": 1.3606909363440005,
+            "timestamp": 1.4173755243868496
+        },
+        {
+            "x": 7.6442805622690475,
+            "y": 5.7599789636443175,
+            "heading": -0.0924144798498188,
+            "angularVelocity": 2.1406199729580164,
+            "velocityX": 0.8677954786282445,
+            "velocityY": 0.8936034921863096,
+            "timestamp": 1.5007505552331348
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 1.1062137707656932,
-            "velocityX": 0.4281678726861332,
-            "velocityY": 0.44471328046952424,
-            "timestamp": 1.5840204943206833
+            "heading": -2.5332548087748344e-31,
+            "angularVelocity": 1.1084191383412998,
+            "velocityX": 0.42841888474747514,
+            "velocityY": 0.4440302567795869,
+            "timestamp": 1.5841255860794201
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 1.4380080051319566e-21,
+            "heading": 0,
+            "angularVelocity": 5.737670210968143e-33,
             "velocityX": 0,
-            "velocityY": 1.143279687808587e-23,
-            "timestamp": 1.6673899940217718
+            "velocityY": -3.2710687085311585e-31,
+            "timestamp": 1.6675006169257054
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.1.traj
index 0e209e38..9d039264 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.1.traj	
@@ -3,164 +3,164 @@
         {
             "x": 4.263,
             "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -2.0886910556164006e-30,
-            "velocityX": -6.72399315077418e-33,
-            "velocityY": 1.0977042871870097e-32,
+            "heading": -0.566,
+            "angularVelocity": -1.0027901855857707e-30,
+            "velocityX": -5.17158696002297e-32,
+            "velocityY": 1.9371271312239428e-32,
             "timestamp": 0
         },
         {
-            "x": 4.300222432415914,
-            "y": 3.053065889088155,
-            "heading": 2.5374285401398833,
-            "angularVelocity": -0.4122912034410164,
-            "velocityX": 0.41962999329145273,
-            "velocityY": 0.5982424370672392,
-            "timestamp": 0.08870298360694638
-        },
-        {
-            "x": 4.375819084559984,
-            "y": 3.158500650682638,
-            "heading": 2.466843265689978,
-            "angularVelocity": -0.7957485935611535,
-            "velocityX": 0.8522447506280879,
-            "velocityY": 1.1886270033675244,
-            "timestamp": 0.17740596721389276
-        },
-        {
-            "x": 4.491836015930185,
-            "y": 3.3149606373409832,
-            "heading": 2.3668866050755812,
-            "angularVelocity": -1.1268692049561326,
-            "velocityX": 1.307925919203419,
-            "velocityY": 1.7638638554892148,
-            "timestamp": 0.26610895082083913
-        },
-        {
-            "x": 4.652875991060298,
-            "y": 3.5188568125430604,
-            "heading": 2.248368974552935,
-            "angularVelocity": -1.3361177460254572,
-            "velocityX": 1.8154967125311139,
-            "velocityY": 2.2986394246394894,
-            "timestamp": 0.3548119344277855
-        },
-        {
-            "x": 4.870873285433555,
-            "y": 3.7498811407551353,
-            "heading": 2.196009009563727,
-            "angularVelocity": -0.5902841467116942,
-            "velocityX": 2.4576094907836294,
-            "velocityY": 2.604470772209555,
-            "timestamp": 0.4435149180347319
-        },
-        {
-            "x": 5.1428014530518755,
-            "y": 3.946488109616269,
-            "heading": 2.1960088759837526,
-            "angularVelocity": -0.0000015059242530161288,
-            "velocityX": 3.0656033941684258,
-            "velocityY": 2.21646398876866,
-            "timestamp": 0.5322179016416783
-        },
-        {
-            "x": 5.447787504833652,
-            "y": 4.086426434262152,
-            "heading": 2.1960087423061556,
-            "angularVelocity": -0.0000015070248085665752,
-            "velocityX": 3.4382840281135,
-            "velocityY": 1.577605610945025,
-            "timestamp": 0.6209208852486247
-        },
-        {
-            "x": 5.774173248105482,
-            "y": 4.1643468008186435,
-            "heading": 2.1960088209928723,
-            "angularVelocity": 8.870808373208906e-7,
-            "velocityX": 3.679535118210733,
-            "velocityY": 0.8784413261876967,
-            "timestamp": 0.709623868855571
-        },
-        {
-            "x": 6.1094823510079035,
-            "y": 4.1772711464289065,
-            "heading": 2.1960089213757183,
-            "angularVelocity": 0.0000011316738380089979,
-            "velocityX": 3.780133308572978,
-            "velocityY": 0.1457036176768579,
-            "timestamp": 0.7983268524625174
-        },
-        {
-            "x": 6.444914319541054,
-            "y": 4.1680717704970895,
-            "heading": 2.1960090177915097,
-            "angularVelocity": 0.000001086950938877831,
-            "velocityX": 3.78151844383826,
-            "velocityY": -0.10370988164931202,
-            "timestamp": 0.8870298360694638
-        },
-        {
-            "x": 6.780346283198341,
-            "y": 4.1588722167865155,
-            "heading": 2.196009114207664,
-            "angularVelocity": 0.0000010869550292397467,
-            "velocityX": 3.7815183888698267,
-            "velocityY": -0.10371188585197635,
-            "timestamp": 0.9757328196764102
-        },
-        {
-            "x": 7.07908088896125,
-            "y": 4.144071317525266,
-            "heading": 2.2852514937378463,
-            "angularVelocity": 1.0060809219859659,
-            "velocityX": 3.3678078641259517,
-            "velocityY": -0.16685909153668751,
-            "timestamp": 1.0644358032833565
-        },
-        {
-            "x": 7.324007491183461,
-            "y": 4.136590325759102,
-            "heading": 2.509537504382542,
-            "angularVelocity": 2.5285058238687177,
-            "velocityX": 2.7611991419308914,
-            "velocityY": -0.08433754381152235,
-            "timestamp": 1.153138786890303
-        },
-        {
-            "x": 7.505234177737953,
-            "y": 4.13149769076231,
-            "heading": 2.790057598116992,
-            "angularVelocity": 3.162465142970483,
-            "velocityX": 2.043073177307424,
-            "velocityY": -0.05741221760204461,
-            "timestamp": 1.2418417704972493
-        },
-        {
-            "x": 7.622663550770296,
-            "y": 4.128429038475573,
-            "heading": 3.015163183857666,
-            "angularVelocity": 2.5377453675982737,
-            "velocityX": 1.3238491903799634,
-            "velocityY": -0.03459469075285244,
-            "timestamp": 1.3305447541041957
+            "x": 4.300239460670815,
+            "y": 3.0530479185441526,
+            "heading": -0.6028935811234369,
+            "angularVelocity": -0.4158951640704737,
+            "velocityX": 0.4197942063083121,
+            "velocityY": 0.5980003056006388,
+            "timestamp": 0.08870884855296232
+        },
+        {
+            "x": 4.375862012794379,
+            "y": 3.1584586937478076,
+            "heading": -0.6740120078870278,
+            "angularVelocity": -0.8017061197804932,
+            "velocityX": 0.8524803709791626,
+            "velocityY": 1.1882780232540273,
+            "timestamp": 0.17741769710592464
+        },
+        {
+            "x": 4.491898262382256,
+            "y": 3.3149103934712074,
+            "heading": -0.7745169367898204,
+            "angularVelocity": -1.1329752391362358,
+            "velocityX": 1.3080572172977813,
+            "velocityY": 1.763653821185524,
+            "timestamp": 0.26612654565888694
+        },
+        {
+            "x": 4.65291407862912,
+            "y": 3.5188640922932937,
+            "heading": -0.893163244153065,
+            "angularVelocity": -1.3374799616794542,
+            "velocityX": 1.8151043427277864,
+            "velocityY": 2.299135905256609,
+            "timestamp": 0.3548353942118493
+        },
+        {
+            "x": 4.870914712338585,
+            "y": 3.7498942741357597,
+            "heading": -0.9455687285938831,
+            "angularVelocity": -0.5907582534963245,
+            "velocityX": 2.457484650804715,
+            "velocityY": 2.6043645657798487,
+            "timestamp": 0.44354424276481164
+        },
+        {
+            "x": 5.142859309500119,
+            "y": 3.9465163862068953,
+            "heading": -0.9455688623878027,
+            "angularVelocity": -0.0000015082364580335284,
+            "velocityX": 3.065585920655626,
+            "velocityY": 2.216488155110533,
+            "timestamp": 0.532253091317774
+        },
+        {
+            "x": 5.447866241414174,
+            "y": 4.086462405550872,
+            "heading": -0.9455689961292948,
+            "angularVelocity": -0.0000015076454528851402,
+            "velocityX": 3.438292085732067,
+            "velocityY": 1.5775880492961702,
+            "timestamp": 0.6209619398707363
+        },
+        {
+            "x": 5.774274975125892,
+            "y": 4.16438201701327,
+            "heading": -0.9455689173593601,
+            "angularVelocity": 8.879602884106285e-7,
+            "velocityX": 3.6795510147653454,
+            "velocityY": 0.8783747363813094,
+            "timestamp": 0.7096707884236987
+        },
+        {
+            "x": 6.109606649873876,
+            "y": 4.1772967927030695,
+            "heading": -0.9455688167433821,
+            "angularVelocity": 0.0000011342270768412302,
+            "velocityX": 3.780137835379285,
+            "velocityY": 0.14558610443566755,
+            "timestamp": 0.798379636976661
+        },
+        {
+            "x": 6.445060320326861,
+            "y": 4.168079453807152,
+            "heading": -0.9455687201101868,
+            "angularVelocity": 0.0000010893298350239914,
+            "velocityX": 3.781513072539862,
+            "velocityY": -0.10390551840399499,
+            "timestamp": 0.8870884855296234
+        },
+        {
+            "x": 6.780513985890507,
+            "y": 4.158861936977066,
+            "heading": -0.9455686234766241,
+            "angularVelocity": 0.0000010893339756447378,
+            "velocityX": 3.781513017423149,
+            "velocityY": -0.10390752422611307,
+            "timestamp": 0.9757973340825857
+        },
+        {
+            "x": 7.079210544299498,
+            "y": 4.144050975293544,
+            "heading": -0.856180476499644,
+            "angularVelocity": 1.0076576174203435,
+            "velocityX": 3.3671563015572095,
+            "velocityY": -0.16696149172400718,
+            "timestamp": 1.064506182635548
+        },
+        {
+            "x": 7.324091725395792,
+            "y": 4.136558502839443,
+            "heading": -0.6317167360096363,
+            "angularVelocity": 2.5303421716267107,
+            "velocityX": 2.760504561730298,
+            "velocityY": -0.08446138774564337,
+            "timestamp": 1.1532150311885103
+        },
+        {
+            "x": 7.50528856962614,
+            "y": 4.13147246408345,
+            "heading": -0.35065072299807204,
+            "angularVelocity": 3.1684101146207286,
+            "velocityX": 2.042601692909661,
+            "velocityY": -0.05733406350050504,
+            "timestamp": 1.2419238797414727
+        },
+        {
+            "x": 7.622685008969739,
+            "y": 4.128418008534293,
+            "heading": -0.12509027443555182,
+            "angularVelocity": 2.5427051781407424,
+            "velocityX": 1.323390408720159,
+            "velocityY": -0.03443236609404116,
+            "timestamp": 1.330632728294435
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 1.4073575776830842,
-            "velocityX": 0.6463869297087906,
-            "velocityY": -0.016110376646462816,
-            "timestamp": 1.419247737711142
+            "heading": 3.405276849475189e-32,
+            "angularVelocity": 1.4101217237744719,
+            "velocityX": 0.6461022994345517,
+            "velocityY": -0.01598497283444213,
+            "timestamp": 1.4193415768473974
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 1.404904480201356e-31,
+            "heading": 0,
+            "angularVelocity": -2.3485646679922316e-31,
             "velocityX": 0,
-            "velocityY": -1.2280872780983036e-31,
-            "timestamp": 1.5079507213180885
+            "velocityY": -2.6996723431981715e-31,
+            "timestamp": 1.5080504254003597
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.traj
index 0e209e38..9d039264 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.traj	
@@ -3,164 +3,164 @@
         {
             "x": 4.263,
             "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -2.0886910556164006e-30,
-            "velocityX": -6.72399315077418e-33,
-            "velocityY": 1.0977042871870097e-32,
+            "heading": -0.566,
+            "angularVelocity": -1.0027901855857707e-30,
+            "velocityX": -5.17158696002297e-32,
+            "velocityY": 1.9371271312239428e-32,
             "timestamp": 0
         },
         {
-            "x": 4.300222432415914,
-            "y": 3.053065889088155,
-            "heading": 2.5374285401398833,
-            "angularVelocity": -0.4122912034410164,
-            "velocityX": 0.41962999329145273,
-            "velocityY": 0.5982424370672392,
-            "timestamp": 0.08870298360694638
-        },
-        {
-            "x": 4.375819084559984,
-            "y": 3.158500650682638,
-            "heading": 2.466843265689978,
-            "angularVelocity": -0.7957485935611535,
-            "velocityX": 0.8522447506280879,
-            "velocityY": 1.1886270033675244,
-            "timestamp": 0.17740596721389276
-        },
-        {
-            "x": 4.491836015930185,
-            "y": 3.3149606373409832,
-            "heading": 2.3668866050755812,
-            "angularVelocity": -1.1268692049561326,
-            "velocityX": 1.307925919203419,
-            "velocityY": 1.7638638554892148,
-            "timestamp": 0.26610895082083913
-        },
-        {
-            "x": 4.652875991060298,
-            "y": 3.5188568125430604,
-            "heading": 2.248368974552935,
-            "angularVelocity": -1.3361177460254572,
-            "velocityX": 1.8154967125311139,
-            "velocityY": 2.2986394246394894,
-            "timestamp": 0.3548119344277855
-        },
-        {
-            "x": 4.870873285433555,
-            "y": 3.7498811407551353,
-            "heading": 2.196009009563727,
-            "angularVelocity": -0.5902841467116942,
-            "velocityX": 2.4576094907836294,
-            "velocityY": 2.604470772209555,
-            "timestamp": 0.4435149180347319
-        },
-        {
-            "x": 5.1428014530518755,
-            "y": 3.946488109616269,
-            "heading": 2.1960088759837526,
-            "angularVelocity": -0.0000015059242530161288,
-            "velocityX": 3.0656033941684258,
-            "velocityY": 2.21646398876866,
-            "timestamp": 0.5322179016416783
-        },
-        {
-            "x": 5.447787504833652,
-            "y": 4.086426434262152,
-            "heading": 2.1960087423061556,
-            "angularVelocity": -0.0000015070248085665752,
-            "velocityX": 3.4382840281135,
-            "velocityY": 1.577605610945025,
-            "timestamp": 0.6209208852486247
-        },
-        {
-            "x": 5.774173248105482,
-            "y": 4.1643468008186435,
-            "heading": 2.1960088209928723,
-            "angularVelocity": 8.870808373208906e-7,
-            "velocityX": 3.679535118210733,
-            "velocityY": 0.8784413261876967,
-            "timestamp": 0.709623868855571
-        },
-        {
-            "x": 6.1094823510079035,
-            "y": 4.1772711464289065,
-            "heading": 2.1960089213757183,
-            "angularVelocity": 0.0000011316738380089979,
-            "velocityX": 3.780133308572978,
-            "velocityY": 0.1457036176768579,
-            "timestamp": 0.7983268524625174
-        },
-        {
-            "x": 6.444914319541054,
-            "y": 4.1680717704970895,
-            "heading": 2.1960090177915097,
-            "angularVelocity": 0.000001086950938877831,
-            "velocityX": 3.78151844383826,
-            "velocityY": -0.10370988164931202,
-            "timestamp": 0.8870298360694638
-        },
-        {
-            "x": 6.780346283198341,
-            "y": 4.1588722167865155,
-            "heading": 2.196009114207664,
-            "angularVelocity": 0.0000010869550292397467,
-            "velocityX": 3.7815183888698267,
-            "velocityY": -0.10371188585197635,
-            "timestamp": 0.9757328196764102
-        },
-        {
-            "x": 7.07908088896125,
-            "y": 4.144071317525266,
-            "heading": 2.2852514937378463,
-            "angularVelocity": 1.0060809219859659,
-            "velocityX": 3.3678078641259517,
-            "velocityY": -0.16685909153668751,
-            "timestamp": 1.0644358032833565
-        },
-        {
-            "x": 7.324007491183461,
-            "y": 4.136590325759102,
-            "heading": 2.509537504382542,
-            "angularVelocity": 2.5285058238687177,
-            "velocityX": 2.7611991419308914,
-            "velocityY": -0.08433754381152235,
-            "timestamp": 1.153138786890303
-        },
-        {
-            "x": 7.505234177737953,
-            "y": 4.13149769076231,
-            "heading": 2.790057598116992,
-            "angularVelocity": 3.162465142970483,
-            "velocityX": 2.043073177307424,
-            "velocityY": -0.05741221760204461,
-            "timestamp": 1.2418417704972493
-        },
-        {
-            "x": 7.622663550770296,
-            "y": 4.128429038475573,
-            "heading": 3.015163183857666,
-            "angularVelocity": 2.5377453675982737,
-            "velocityX": 1.3238491903799634,
-            "velocityY": -0.03459469075285244,
-            "timestamp": 1.3305447541041957
+            "x": 4.300239460670815,
+            "y": 3.0530479185441526,
+            "heading": -0.6028935811234369,
+            "angularVelocity": -0.4158951640704737,
+            "velocityX": 0.4197942063083121,
+            "velocityY": 0.5980003056006388,
+            "timestamp": 0.08870884855296232
+        },
+        {
+            "x": 4.375862012794379,
+            "y": 3.1584586937478076,
+            "heading": -0.6740120078870278,
+            "angularVelocity": -0.8017061197804932,
+            "velocityX": 0.8524803709791626,
+            "velocityY": 1.1882780232540273,
+            "timestamp": 0.17741769710592464
+        },
+        {
+            "x": 4.491898262382256,
+            "y": 3.3149103934712074,
+            "heading": -0.7745169367898204,
+            "angularVelocity": -1.1329752391362358,
+            "velocityX": 1.3080572172977813,
+            "velocityY": 1.763653821185524,
+            "timestamp": 0.26612654565888694
+        },
+        {
+            "x": 4.65291407862912,
+            "y": 3.5188640922932937,
+            "heading": -0.893163244153065,
+            "angularVelocity": -1.3374799616794542,
+            "velocityX": 1.8151043427277864,
+            "velocityY": 2.299135905256609,
+            "timestamp": 0.3548353942118493
+        },
+        {
+            "x": 4.870914712338585,
+            "y": 3.7498942741357597,
+            "heading": -0.9455687285938831,
+            "angularVelocity": -0.5907582534963245,
+            "velocityX": 2.457484650804715,
+            "velocityY": 2.6043645657798487,
+            "timestamp": 0.44354424276481164
+        },
+        {
+            "x": 5.142859309500119,
+            "y": 3.9465163862068953,
+            "heading": -0.9455688623878027,
+            "angularVelocity": -0.0000015082364580335284,
+            "velocityX": 3.065585920655626,
+            "velocityY": 2.216488155110533,
+            "timestamp": 0.532253091317774
+        },
+        {
+            "x": 5.447866241414174,
+            "y": 4.086462405550872,
+            "heading": -0.9455689961292948,
+            "angularVelocity": -0.0000015076454528851402,
+            "velocityX": 3.438292085732067,
+            "velocityY": 1.5775880492961702,
+            "timestamp": 0.6209619398707363
+        },
+        {
+            "x": 5.774274975125892,
+            "y": 4.16438201701327,
+            "heading": -0.9455689173593601,
+            "angularVelocity": 8.879602884106285e-7,
+            "velocityX": 3.6795510147653454,
+            "velocityY": 0.8783747363813094,
+            "timestamp": 0.7096707884236987
+        },
+        {
+            "x": 6.109606649873876,
+            "y": 4.1772967927030695,
+            "heading": -0.9455688167433821,
+            "angularVelocity": 0.0000011342270768412302,
+            "velocityX": 3.780137835379285,
+            "velocityY": 0.14558610443566755,
+            "timestamp": 0.798379636976661
+        },
+        {
+            "x": 6.445060320326861,
+            "y": 4.168079453807152,
+            "heading": -0.9455687201101868,
+            "angularVelocity": 0.0000010893298350239914,
+            "velocityX": 3.781513072539862,
+            "velocityY": -0.10390551840399499,
+            "timestamp": 0.8870884855296234
+        },
+        {
+            "x": 6.780513985890507,
+            "y": 4.158861936977066,
+            "heading": -0.9455686234766241,
+            "angularVelocity": 0.0000010893339756447378,
+            "velocityX": 3.781513017423149,
+            "velocityY": -0.10390752422611307,
+            "timestamp": 0.9757973340825857
+        },
+        {
+            "x": 7.079210544299498,
+            "y": 4.144050975293544,
+            "heading": -0.856180476499644,
+            "angularVelocity": 1.0076576174203435,
+            "velocityX": 3.3671563015572095,
+            "velocityY": -0.16696149172400718,
+            "timestamp": 1.064506182635548
+        },
+        {
+            "x": 7.324091725395792,
+            "y": 4.136558502839443,
+            "heading": -0.6317167360096363,
+            "angularVelocity": 2.5303421716267107,
+            "velocityX": 2.760504561730298,
+            "velocityY": -0.08446138774564337,
+            "timestamp": 1.1532150311885103
+        },
+        {
+            "x": 7.50528856962614,
+            "y": 4.13147246408345,
+            "heading": -0.35065072299807204,
+            "angularVelocity": 3.1684101146207286,
+            "velocityX": 2.042601692909661,
+            "velocityY": -0.05733406350050504,
+            "timestamp": 1.2419238797414727
+        },
+        {
+            "x": 7.622685008969739,
+            "y": 4.128418008534293,
+            "heading": -0.12509027443555182,
+            "angularVelocity": 2.5427051781407424,
+            "velocityX": 1.323390408720159,
+            "velocityY": -0.03443236609404116,
+            "timestamp": 1.330632728294435
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 1.4073575776830842,
-            "velocityX": 0.6463869297087906,
-            "velocityY": -0.016110376646462816,
-            "timestamp": 1.419247737711142
+            "heading": 3.405276849475189e-32,
+            "angularVelocity": 1.4101217237744719,
+            "velocityX": 0.6461022994345517,
+            "velocityY": -0.01598497283444213,
+            "timestamp": 1.4193415768473974
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 1.404904480201356e-31,
+            "heading": 0,
+            "angularVelocity": -2.3485646679922316e-31,
             "velocityX": 0,
-            "velocityY": -1.2280872780983036e-31,
-            "timestamp": 1.5079507213180885
+            "velocityY": -2.6996723431981715e-31,
+            "timestamp": 1.5080504254003597
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.1.traj
index c6e6fe7d..d933d22a 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.1.traj	
@@ -3,182 +3,182 @@
         {
             "x": 4.263,
             "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -5.037969685186626e-28,
-            "velocityX": 1.6468312353259754e-28,
-            "velocityY": 5.118071700544948e-29,
+            "heading": -0.566,
+            "angularVelocity": -2.2118032332823513e-27,
+            "velocityX": -2.2398078489200873e-28,
+            "velocityY": 7.32316842173409e-29,
             "timestamp": 0
         },
         {
-            "x": 4.2573202524069735,
-            "y": 2.93910307994174,
-            "heading": 2.524484159587002,
-            "angularVelocity": -0.5709623504386324,
-            "velocityX": -0.06549261829272982,
-            "velocityY": -0.7021964753285136,
-            "timestamp": 0.08672347725723033
-        },
-        {
-            "x": 4.260125495463358,
-            "y": 2.8166106869531333,
-            "heading": 2.4459027831938824,
-            "angularVelocity": -0.9061142251024153,
-            "velocityX": 0.032346985442753974,
-            "velocityY": -1.412447896032608,
-            "timestamp": 0.17344695451446066
-        },
-        {
-            "x": 4.2940460497089665,
-            "y": 2.639724962337947,
-            "heading": 2.375950811095946,
-            "angularVelocity": -0.8066094016324238,
-            "velocityX": 0.3911346191181556,
-            "velocityY": -2.039652124309153,
-            "timestamp": 0.26017043177169097
-        },
-        {
-            "x": 4.380083643145815,
-            "y": 2.4322611848370723,
-            "heading": 2.367349671900665,
-            "angularVelocity": -0.09917890134603644,
-            "velocityX": 0.9920911402301427,
-            "velocityY": -2.3922446846257825,
-            "timestamp": 0.3468939090289213
-        },
-        {
-            "x": 4.5223038363919486,
-            "y": 2.2062304273482725,
-            "heading": 2.4344770790327512,
-            "angularVelocity": 0.774039617126779,
-            "velocityX": 1.639927246278367,
-            "velocityY": -2.6063387289968842,
-            "timestamp": 0.43361738628615165
-        },
-        {
-            "x": 4.727106314265492,
-            "y": 1.976782887261895,
-            "heading": 2.494103363003819,
-            "angularVelocity": 0.6875448939185098,
-            "velocityX": 2.361557496894169,
-            "velocityY": -2.6457373175410677,
-            "timestamp": 0.5203408635433819
-        },
-        {
-            "x": 4.985728714890376,
-            "y": 1.77493098646381,
-            "heading": 2.4941035609909017,
-            "angularVelocity": 0.0000022829698365674285,
-            "velocityX": 2.982149803077939,
-            "velocityY": -2.327534678981717,
-            "timestamp": 0.6070643408006122
-        },
-        {
-            "x": 5.274471193088088,
-            "y": 1.6191826835790144,
-            "heading": 2.494103670963996,
-            "angularVelocity": 0.000001268089077272548,
-            "velocityX": 3.329461494507118,
-            "velocityY": -1.795918565659344,
-            "timestamp": 0.6937878180578425
-        },
-        {
-            "x": 5.576873114142296,
-            "y": 1.4919708990806198,
-            "heading": 2.4941037354476974,
-            "angularVelocity": 7.435553047089643e-7,
-            "velocityX": 3.486967204477438,
-            "velocityY": -1.4668667415291887,
-            "timestamp": 0.7805112953150728
-        },
-        {
-            "x": 5.888127945966446,
-            "y": 1.388287773871042,
-            "heading": 2.4941037912285227,
-            "angularVelocity": 6.432032813789173e-7,
-            "velocityX": 3.589049259417249,
-            "velocityY": -1.1955600546555811,
-            "timestamp": 0.8672347725723031
-        },
-        {
-            "x": 6.199389301961685,
-            "y": 1.2846242361365592,
-            "heading": 2.4941038470080463,
-            "angularVelocity": 6.431882742929459e-7,
-            "velocityX": 3.58912448900094,
-            "velocityY": -1.1953341933810824,
-            "timestamp": 0.9539582498295334
-        },
-        {
-            "x": 6.510650764865791,
-            "y": 1.1809610194297586,
-            "heading": 2.494103902787595,
-            "angularVelocity": 6.431885596372589e-7,
-            "velocityX": 3.589125721756674,
-            "velocityY": -1.1953304916421486,
-            "timestamp": 1.0406817270867637
-        },
-        {
-            "x": 6.821912914234967,
-            "y": 1.0772998639276916,
-            "heading": 2.4941039585563476,
-            "angularVelocity": 6.430640741979831e-7,
-            "velocityX": 3.589133637318779,
-            "velocityY": -1.1953067240904078,
-            "timestamp": 1.127405204343994
-        },
-        {
-            "x": 7.104835642474277,
-            "y": 0.9795860470438206,
-            "heading": 2.575393099111341,
-            "angularVelocity": 0.9373371908725603,
-            "velocityX": 3.2623545225260457,
-            "velocityY": -1.126728539655348,
-            "timestamp": 1.2141286816012242
-        },
-        {
-            "x": 7.336162807105594,
-            "y": 0.9014195336331526,
-            "heading": 2.773123816483908,
-            "angularVelocity": 2.280013712850493,
-            "velocityX": 2.6674110857568296,
-            "velocityY": -0.9013304803129645,
-            "timestamp": 1.3008521588584545
-        },
-        {
-            "x": 7.508575525064269,
-            "y": 0.8437553503848311,
-            "heading": 2.9477959992299394,
-            "angularVelocity": 2.0141279878335374,
-            "velocityX": 1.9880743186447822,
-            "velocityY": -0.664920100900538,
-            "timestamp": 1.3875756361156848
-        },
-        {
-            "x": 7.62299427085809,
-            "y": 0.8057956185147762,
-            "heading": 3.0737109392018835,
-            "angularVelocity": 1.451912953149597,
-            "velocityX": 1.31935145375275,
-            "velocityY": -0.4377099843155908,
-            "timestamp": 1.474299113372915
+            "x": 4.257275610783411,
+            "y": 2.9390850995701614,
+            "heading": -0.6153074756658753,
+            "angularVelocity": -0.5685220144659363,
+            "velocityX": -0.06600299944485946,
+            "velocityY": -0.7023572274929988,
+            "timestamp": 0.0867292284401611
+        },
+        {
+            "x": 4.259999029271463,
+            "y": 2.8165594202827866,
+            "heading": -0.6936339270516045,
+            "angularVelocity": -0.9031148183195571,
+            "velocityX": 0.03140139186100546,
+            "velocityY": -1.4127380295088343,
+            "timestamp": 0.1734584568803222
+        },
+        {
+            "x": 4.293815085802935,
+            "y": 2.6396142169455077,
+            "heading": -0.7636289676296801,
+            "angularVelocity": -0.8070524993355632,
+            "velocityX": 0.3899038091270867,
+            "velocityY": -2.0402026689233352,
+            "timestamp": 0.2601876853204833
+        },
+        {
+            "x": 4.379794167011224,
+            "y": 2.432035667571809,
+            "heading": -0.7730870281638328,
+            "angularVelocity": -0.10905274616477362,
+            "velocityX": 0.9913506986587586,
+            "velocityY": -2.3934093858210397,
+            "timestamp": 0.3469169137606444
+        },
+        {
+            "x": 4.522000800858502,
+            "y": 2.2059673134468256,
+            "heading": -0.7069342437539652,
+            "angularVelocity": 0.7627507542686203,
+            "velocityX": 1.6396621577856372,
+            "velocityY": -2.6065993920487904,
+            "timestamp": 0.43364614220080544
+        },
+        {
+            "x": 4.726803922559788,
+            "y": 1.9764479224139355,
+            "heading": -0.6474090008701758,
+            "angularVelocity": 0.6863342837743838,
+            "velocityX": 2.3614083208706247,
+            "velocityY": -2.6463903249323897,
+            "timestamp": 0.5203753706409665
+        },
+        {
+            "x": 4.985426543182091,
+            "y": 1.774560944536545,
+            "heading": -0.6474088034070749,
+            "angularVelocity": 0.000002276776866191791,
+            "velocityX": 2.9819545875556583,
+            "velocityY": -2.3277847792302735,
+            "timestamp": 0.6071045990811276
+        },
+        {
+            "x": 5.274431548584119,
+            "y": 1.6192543598542732,
+            "heading": -0.6474086941993331,
+            "angularVelocity": 0.000001259180368575944,
+            "velocityX": 3.3322676864516145,
+            "velocityY": -1.7907064028526984,
+            "timestamp": 0.6938338275212886
+        },
+        {
+            "x": 5.576835908979959,
+            "y": 1.4919922741488194,
+            "heading": -0.6474086295363626,
+            "angularVelocity": 7.455729944345581e-7,
+            "velocityX": 3.4867641028824017,
+            "velocityY": -1.4673494506325018,
+            "timestamp": 0.7805630559614497
+        },
+        {
+            "x": 5.888110144336631,
+            "y": 1.388298557512795,
+            "heading": -0.6474085736154427,
+            "angularVelocity": 6.447759407143774e-7,
+            "velocityX": 3.589034988031027,
+            "velocityY": -1.1956028953671916,
+            "timestamp": 0.8672922844016108
+        },
+        {
+            "x": 6.199390629137113,
+            "y": 1.2846236026664921,
+            "heading": -0.6474085176958322,
+            "angularVelocity": 6.447608435198244e-7,
+            "velocityX": 3.5891070449826046,
+            "velocityY": -1.1953865693365275,
+            "timestamp": 0.9540215128417718
+        },
+        {
+            "x": 6.510671944712167,
+            "y": 1.1809511422971397,
+            "heading": -0.6474084617763125,
+            "angularVelocity": 6.447597973785494e-7,
+            "velocityX": 3.589116623928252,
+            "velocityY": -1.1953578076724478,
+            "timestamp": 1.040750741281933
+        },
+        {
+            "x": 6.8219538378473334,
+            "y": 1.0772804160640193,
+            "heading": -0.6474084058668956,
+            "angularVelocity": 6.446433092317229e-7,
+            "velocityX": 3.5891232832762197,
+            "velocityY": -1.1953378128419065,
+            "timestamp": 1.127479969722094
+        },
+        {
+            "x": 7.104843996542823,
+            "y": 0.9795715918943764,
+            "heading": -0.5659567926753784,
+            "angularVelocity": 0.9391483662017565,
+            "velocityX": 3.2617626581409067,
+            "velocityY": -1.126596257420401,
+            "timestamp": 1.2142091981622551
+        },
+        {
+            "x": 7.336171547926519,
+            "y": 0.9014041069183023,
+            "heading": -0.36771202193482067,
+            "angularVelocity": 2.2857896271651064,
+            "velocityX": 2.6672386638755765,
+            "velocityY": -0.9012819136285574,
+            "timestamp": 1.3009384266024162
+        },
+        {
+            "x": 7.508580909570239,
+            "y": 0.8437450323938049,
+            "heading": -0.19262457792893875,
+            "angularVelocity": 2.018782446873579,
+            "velocityX": 1.9879037868147627,
+            "velocityY": -0.6648171044699364,
+            "timestamp": 1.3876676550425773
+        },
+        {
+            "x": 7.622996353926532,
+            "y": 0.8057914476404057,
+            "heading": -0.06643070405801102,
+            "angularVelocity": 1.4550328204291798,
+            "velocityX": 1.3192258989739947,
+            "velocityY": -0.4376100818144245,
+            "timestamp": 1.4743968834827383
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 0.7643727269087626,
-            "velocityX": 0.6573275305004772,
-            "velocityY": -0.2167304530355382,
-            "timestamp": 1.5610225906301454
+            "heading": -1.2156690964354266e-28,
+            "angularVelocity": 0.7659552062526378,
+            "velocityX": 0.6572599237729437,
+            "velocityY": -0.21666799046150395,
+            "timestamp": 1.5611261119228994
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": -1.052133339029093e-28,
+            "heading": 0,
+            "angularVelocity": 1.25023472254267e-27,
             "velocityX": 0,
-            "velocityY": -1.789975640452255e-29,
-            "timestamp": 1.6477460678873757
+            "velocityY": -4.9030509543456705e-29,
+            "timestamp": 1.6478553403630605
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.traj
index c6e6fe7d..d933d22a 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.traj	
@@ -3,182 +3,182 @@
         {
             "x": 4.263,
             "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -5.037969685186626e-28,
-            "velocityX": 1.6468312353259754e-28,
-            "velocityY": 5.118071700544948e-29,
+            "heading": -0.566,
+            "angularVelocity": -2.2118032332823513e-27,
+            "velocityX": -2.2398078489200873e-28,
+            "velocityY": 7.32316842173409e-29,
             "timestamp": 0
         },
         {
-            "x": 4.2573202524069735,
-            "y": 2.93910307994174,
-            "heading": 2.524484159587002,
-            "angularVelocity": -0.5709623504386324,
-            "velocityX": -0.06549261829272982,
-            "velocityY": -0.7021964753285136,
-            "timestamp": 0.08672347725723033
-        },
-        {
-            "x": 4.260125495463358,
-            "y": 2.8166106869531333,
-            "heading": 2.4459027831938824,
-            "angularVelocity": -0.9061142251024153,
-            "velocityX": 0.032346985442753974,
-            "velocityY": -1.412447896032608,
-            "timestamp": 0.17344695451446066
-        },
-        {
-            "x": 4.2940460497089665,
-            "y": 2.639724962337947,
-            "heading": 2.375950811095946,
-            "angularVelocity": -0.8066094016324238,
-            "velocityX": 0.3911346191181556,
-            "velocityY": -2.039652124309153,
-            "timestamp": 0.26017043177169097
-        },
-        {
-            "x": 4.380083643145815,
-            "y": 2.4322611848370723,
-            "heading": 2.367349671900665,
-            "angularVelocity": -0.09917890134603644,
-            "velocityX": 0.9920911402301427,
-            "velocityY": -2.3922446846257825,
-            "timestamp": 0.3468939090289213
-        },
-        {
-            "x": 4.5223038363919486,
-            "y": 2.2062304273482725,
-            "heading": 2.4344770790327512,
-            "angularVelocity": 0.774039617126779,
-            "velocityX": 1.639927246278367,
-            "velocityY": -2.6063387289968842,
-            "timestamp": 0.43361738628615165
-        },
-        {
-            "x": 4.727106314265492,
-            "y": 1.976782887261895,
-            "heading": 2.494103363003819,
-            "angularVelocity": 0.6875448939185098,
-            "velocityX": 2.361557496894169,
-            "velocityY": -2.6457373175410677,
-            "timestamp": 0.5203408635433819
-        },
-        {
-            "x": 4.985728714890376,
-            "y": 1.77493098646381,
-            "heading": 2.4941035609909017,
-            "angularVelocity": 0.0000022829698365674285,
-            "velocityX": 2.982149803077939,
-            "velocityY": -2.327534678981717,
-            "timestamp": 0.6070643408006122
-        },
-        {
-            "x": 5.274471193088088,
-            "y": 1.6191826835790144,
-            "heading": 2.494103670963996,
-            "angularVelocity": 0.000001268089077272548,
-            "velocityX": 3.329461494507118,
-            "velocityY": -1.795918565659344,
-            "timestamp": 0.6937878180578425
-        },
-        {
-            "x": 5.576873114142296,
-            "y": 1.4919708990806198,
-            "heading": 2.4941037354476974,
-            "angularVelocity": 7.435553047089643e-7,
-            "velocityX": 3.486967204477438,
-            "velocityY": -1.4668667415291887,
-            "timestamp": 0.7805112953150728
-        },
-        {
-            "x": 5.888127945966446,
-            "y": 1.388287773871042,
-            "heading": 2.4941037912285227,
-            "angularVelocity": 6.432032813789173e-7,
-            "velocityX": 3.589049259417249,
-            "velocityY": -1.1955600546555811,
-            "timestamp": 0.8672347725723031
-        },
-        {
-            "x": 6.199389301961685,
-            "y": 1.2846242361365592,
-            "heading": 2.4941038470080463,
-            "angularVelocity": 6.431882742929459e-7,
-            "velocityX": 3.58912448900094,
-            "velocityY": -1.1953341933810824,
-            "timestamp": 0.9539582498295334
-        },
-        {
-            "x": 6.510650764865791,
-            "y": 1.1809610194297586,
-            "heading": 2.494103902787595,
-            "angularVelocity": 6.431885596372589e-7,
-            "velocityX": 3.589125721756674,
-            "velocityY": -1.1953304916421486,
-            "timestamp": 1.0406817270867637
-        },
-        {
-            "x": 6.821912914234967,
-            "y": 1.0772998639276916,
-            "heading": 2.4941039585563476,
-            "angularVelocity": 6.430640741979831e-7,
-            "velocityX": 3.589133637318779,
-            "velocityY": -1.1953067240904078,
-            "timestamp": 1.127405204343994
-        },
-        {
-            "x": 7.104835642474277,
-            "y": 0.9795860470438206,
-            "heading": 2.575393099111341,
-            "angularVelocity": 0.9373371908725603,
-            "velocityX": 3.2623545225260457,
-            "velocityY": -1.126728539655348,
-            "timestamp": 1.2141286816012242
-        },
-        {
-            "x": 7.336162807105594,
-            "y": 0.9014195336331526,
-            "heading": 2.773123816483908,
-            "angularVelocity": 2.280013712850493,
-            "velocityX": 2.6674110857568296,
-            "velocityY": -0.9013304803129645,
-            "timestamp": 1.3008521588584545
-        },
-        {
-            "x": 7.508575525064269,
-            "y": 0.8437553503848311,
-            "heading": 2.9477959992299394,
-            "angularVelocity": 2.0141279878335374,
-            "velocityX": 1.9880743186447822,
-            "velocityY": -0.664920100900538,
-            "timestamp": 1.3875756361156848
-        },
-        {
-            "x": 7.62299427085809,
-            "y": 0.8057956185147762,
-            "heading": 3.0737109392018835,
-            "angularVelocity": 1.451912953149597,
-            "velocityX": 1.31935145375275,
-            "velocityY": -0.4377099843155908,
-            "timestamp": 1.474299113372915
+            "x": 4.257275610783411,
+            "y": 2.9390850995701614,
+            "heading": -0.6153074756658753,
+            "angularVelocity": -0.5685220144659363,
+            "velocityX": -0.06600299944485946,
+            "velocityY": -0.7023572274929988,
+            "timestamp": 0.0867292284401611
+        },
+        {
+            "x": 4.259999029271463,
+            "y": 2.8165594202827866,
+            "heading": -0.6936339270516045,
+            "angularVelocity": -0.9031148183195571,
+            "velocityX": 0.03140139186100546,
+            "velocityY": -1.4127380295088343,
+            "timestamp": 0.1734584568803222
+        },
+        {
+            "x": 4.293815085802935,
+            "y": 2.6396142169455077,
+            "heading": -0.7636289676296801,
+            "angularVelocity": -0.8070524993355632,
+            "velocityX": 0.3899038091270867,
+            "velocityY": -2.0402026689233352,
+            "timestamp": 0.2601876853204833
+        },
+        {
+            "x": 4.379794167011224,
+            "y": 2.432035667571809,
+            "heading": -0.7730870281638328,
+            "angularVelocity": -0.10905274616477362,
+            "velocityX": 0.9913506986587586,
+            "velocityY": -2.3934093858210397,
+            "timestamp": 0.3469169137606444
+        },
+        {
+            "x": 4.522000800858502,
+            "y": 2.2059673134468256,
+            "heading": -0.7069342437539652,
+            "angularVelocity": 0.7627507542686203,
+            "velocityX": 1.6396621577856372,
+            "velocityY": -2.6065993920487904,
+            "timestamp": 0.43364614220080544
+        },
+        {
+            "x": 4.726803922559788,
+            "y": 1.9764479224139355,
+            "heading": -0.6474090008701758,
+            "angularVelocity": 0.6863342837743838,
+            "velocityX": 2.3614083208706247,
+            "velocityY": -2.6463903249323897,
+            "timestamp": 0.5203753706409665
+        },
+        {
+            "x": 4.985426543182091,
+            "y": 1.774560944536545,
+            "heading": -0.6474088034070749,
+            "angularVelocity": 0.000002276776866191791,
+            "velocityX": 2.9819545875556583,
+            "velocityY": -2.3277847792302735,
+            "timestamp": 0.6071045990811276
+        },
+        {
+            "x": 5.274431548584119,
+            "y": 1.6192543598542732,
+            "heading": -0.6474086941993331,
+            "angularVelocity": 0.000001259180368575944,
+            "velocityX": 3.3322676864516145,
+            "velocityY": -1.7907064028526984,
+            "timestamp": 0.6938338275212886
+        },
+        {
+            "x": 5.576835908979959,
+            "y": 1.4919922741488194,
+            "heading": -0.6474086295363626,
+            "angularVelocity": 7.455729944345581e-7,
+            "velocityX": 3.4867641028824017,
+            "velocityY": -1.4673494506325018,
+            "timestamp": 0.7805630559614497
+        },
+        {
+            "x": 5.888110144336631,
+            "y": 1.388298557512795,
+            "heading": -0.6474085736154427,
+            "angularVelocity": 6.447759407143774e-7,
+            "velocityX": 3.589034988031027,
+            "velocityY": -1.1956028953671916,
+            "timestamp": 0.8672922844016108
+        },
+        {
+            "x": 6.199390629137113,
+            "y": 1.2846236026664921,
+            "heading": -0.6474085176958322,
+            "angularVelocity": 6.447608435198244e-7,
+            "velocityX": 3.5891070449826046,
+            "velocityY": -1.1953865693365275,
+            "timestamp": 0.9540215128417718
+        },
+        {
+            "x": 6.510671944712167,
+            "y": 1.1809511422971397,
+            "heading": -0.6474084617763125,
+            "angularVelocity": 6.447597973785494e-7,
+            "velocityX": 3.589116623928252,
+            "velocityY": -1.1953578076724478,
+            "timestamp": 1.040750741281933
+        },
+        {
+            "x": 6.8219538378473334,
+            "y": 1.0772804160640193,
+            "heading": -0.6474084058668956,
+            "angularVelocity": 6.446433092317229e-7,
+            "velocityX": 3.5891232832762197,
+            "velocityY": -1.1953378128419065,
+            "timestamp": 1.127479969722094
+        },
+        {
+            "x": 7.104843996542823,
+            "y": 0.9795715918943764,
+            "heading": -0.5659567926753784,
+            "angularVelocity": 0.9391483662017565,
+            "velocityX": 3.2617626581409067,
+            "velocityY": -1.126596257420401,
+            "timestamp": 1.2142091981622551
+        },
+        {
+            "x": 7.336171547926519,
+            "y": 0.9014041069183023,
+            "heading": -0.36771202193482067,
+            "angularVelocity": 2.2857896271651064,
+            "velocityX": 2.6672386638755765,
+            "velocityY": -0.9012819136285574,
+            "timestamp": 1.3009384266024162
+        },
+        {
+            "x": 7.508580909570239,
+            "y": 0.8437450323938049,
+            "heading": -0.19262457792893875,
+            "angularVelocity": 2.018782446873579,
+            "velocityX": 1.9879037868147627,
+            "velocityY": -0.6648171044699364,
+            "timestamp": 1.3876676550425773
+        },
+        {
+            "x": 7.622996353926532,
+            "y": 0.8057914476404057,
+            "heading": -0.06643070405801102,
+            "angularVelocity": 1.4550328204291798,
+            "velocityX": 1.3192258989739947,
+            "velocityY": -0.4376100818144245,
+            "timestamp": 1.4743968834827383
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 0.7643727269087626,
-            "velocityX": 0.6573275305004772,
-            "velocityY": -0.2167304530355382,
-            "timestamp": 1.5610225906301454
+            "heading": -1.2156690964354266e-28,
+            "angularVelocity": 0.7659552062526378,
+            "velocityX": 0.6572599237729437,
+            "velocityY": -0.21666799046150395,
+            "timestamp": 1.5611261119228994
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": -1.052133339029093e-28,
+            "heading": 0,
+            "angularVelocity": 1.25023472254267e-27,
             "velocityX": 0,
-            "velocityY": -1.789975640452255e-29,
-            "timestamp": 1.6477460678873757
+            "velocityY": -4.9030509543456705e-29,
+            "timestamp": 1.6478553403630605
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W1.1.traj
new file mode 100644
index 00000000..44ee68f9
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W1.1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -1.6794679447375943e-30,
+            "velocityX": 6.742046899995187e-33,
+            "velocityY": 9.708842875589211e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 4.290023963257714,
+            "y": 3.0497874441329906,
+            "heading": -0.5200869306801483,
+            "angularVelocity": 0.5503538158132694,
+            "velocityX": 0.3239326300245928,
+            "velocityY": 0.5967954280576505,
+            "timestamp": 0.08342464065958187
+        },
+        {
+            "x": 4.342730369898429,
+            "y": 3.150178003649595,
+            "heading": -0.42997936088427535,
+            "angularVelocity": 1.0801073769506666,
+            "velocityX": 0.6317846408926776,
+            "velocityY": 1.2033681982072046,
+            "timestamp": 0.16684928131916374
+        },
+        {
+            "x": 4.418537982231504,
+            "y": 3.3026029150734924,
+            "heading": -0.2990998156933407,
+            "angularVelocity": 1.5688355880967437,
+            "velocityX": 0.9086957010988046,
+            "velocityY": 1.8270970089745318,
+            "timestamp": 0.2502739219787456
+        },
+        {
+            "x": 4.5104462205151705,
+            "y": 3.5100667276192694,
+            "heading": -0.13617594937576347,
+            "angularVelocity": 1.9529465758491624,
+            "velocityX": 1.1016917490685052,
+            "velocityY": 2.4868409489750483,
+            "timestamp": 0.3336985626383275
+        },
+        {
+            "x": 4.573472263152817,
+            "y": 3.7674681923433693,
+            "heading": 0.004287761033867021,
+            "angularVelocity": 1.6837196935950753,
+            "velocityX": 0.7554847361563879,
+            "velocityY": 3.0854369007645883,
+            "timestamp": 0.41712320329790936
+        },
+        {
+            "x": 4.6071871310155705,
+            "y": 4.066840423815047,
+            "heading": 0.04705620405427594,
+            "angularVelocity": 0.5126596013152507,
+            "velocityX": 0.40413560785150865,
+            "velocityY": 3.5885348633778387,
+            "timestamp": 0.5005478439574912
+        },
+        {
+            "x": 4.587417601167403,
+            "y": 4.370555626705338,
+            "heading": 0.08458578048595149,
+            "angularVelocity": 0.4498620088136386,
+            "velocityX": -0.23697470785444333,
+            "velocityY": 3.6405934804036506,
+            "timestamp": 0.5839724846170731
+        },
+        {
+            "x": 4.510022832963655,
+            "y": 4.67116829443229,
+            "heading": 0.1002321163817716,
+            "angularVelocity": 0.1875505338964035,
+            "velocityX": -0.9277207260569706,
+            "velocityY": 3.603403806719601,
+            "timestamp": 0.667397125276655
+        },
+        {
+            "x": 4.376634060333898,
+            "y": 4.957183607465761,
+            "heading": 0.10023219176740035,
+            "angularVelocity": 9.036374403612271e-7,
+            "velocityX": -1.5989133615097586,
+            "velocityY": 3.4284272700744434,
+            "timestamp": 0.7508217659362368
+        },
+        {
+            "x": 4.19313075849175,
+            "y": 5.213940045953902,
+            "heading": 0.10023221692184411,
+            "angularVelocity": 3.015229499928316e-7,
+            "velocityX": -2.1996295146291525,
+            "velocityY": 3.0777050576201734,
+            "timestamp": 0.8342464065958187
+        },
+        {
+            "x": 3.9657176811920825,
+            "y": 5.432756463781841,
+            "heading": 0.10023223856547658,
+            "angularVelocity": 2.5943932502769966e-7,
+            "velocityX": -2.725970115084308,
+            "velocityY": 2.6229231087830502,
+            "timestamp": 0.9176710472554006
+        },
+        {
+            "x": 3.7186426713834653,
+            "y": 5.629101242954768,
+            "heading": 0.10023225962798245,
+            "angularVelocity": 2.524734383899783e-7,
+            "velocityX": -2.961655068037016,
+            "velocityY": 2.3535585843770224,
+            "timestamp": 1.0010956879149824
+        },
+        {
+            "x": 3.4715770348675625,
+            "y": 5.825457816649942,
+            "heading": 0.10023228069061084,
+            "angularVelocity": 2.5247490699416256e-7,
+            "velocityX": -2.961542711631996,
+            "velocityY": 2.353699963736333,
+            "timestamp": 1.0845203285745642
+        },
+        {
+            "x": 3.224511384705754,
+            "y": 6.021814373175139,
+            "heading": 0.10023230175325984,
+            "angularVelocity": 2.5247515406639525e-7,
+            "velocityX": -2.96154287520364,
+            "velocityY": 2.353699757922098,
+            "timestamp": 1.167944969234146
+        },
+        {
+            "x": 2.9774457295747583,
+            "y": 6.218170923447359,
+            "heading": 0.10023232281654228,
+            "angularVelocity": 2.524827469327523e-7,
+            "velocityX": -2.9615429347686173,
+            "velocityY": 2.3536996829685104,
+            "timestamp": 1.2513696098937277
+        },
+        {
+            "x": 2.751412346836958,
+            "y": 6.39797212959724,
+            "heading": 0.1658210049038553,
+            "angularVelocity": 0.7862027521934517,
+            "velocityX": -2.7094319010631414,
+            "velocityY": 2.155252989144637,
+            "timestamp": 1.3347942505533095
+        },
+        {
+            "x": 2.5706626829115913,
+            "y": 6.54172747182469,
+            "heading": 0.24638520673123038,
+            "angularVelocity": 0.9657123026291593,
+            "velocityX": -2.166622025534686,
+            "velocityY": 1.723176043563091,
+            "timestamp": 1.4182188912128912
+        },
+        {
+            "x": 2.435257350873027,
+            "y": 6.649419264379792,
+            "heading": 0.31518856404188306,
+            "angularVelocity": 0.8247366337651717,
+            "velocityX": -1.6230855891977036,
+            "velocityY": 1.2908871012647558,
+            "timestamp": 1.501643531872473
+        },
+        {
+            "x": 2.3450659538089185,
+            "y": 6.721154353305962,
+            "heading": 0.3643262231924431,
+            "angularVelocity": 0.5890065424563048,
+            "velocityX": -1.0811122031935256,
+            "velocityY": 0.8598789081859941,
+            "timestamp": 1.5850681725320548
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 0.3077481257883978,
+            "velocityX": -0.5401995555822935,
+            "velocityY": 0.42967696846676384,
+            "timestamp": 1.6684928131916366
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": -2.8369790280137563e-29,
+            "velocityX": 0,
+            "velocityY": -1.9155196850548007e-31,
+            "timestamp": 1.7519174538512183
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W1.traj
new file mode 100644
index 00000000..44ee68f9
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -1.6794679447375943e-30,
+            "velocityX": 6.742046899995187e-33,
+            "velocityY": 9.708842875589211e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 4.290023963257714,
+            "y": 3.0497874441329906,
+            "heading": -0.5200869306801483,
+            "angularVelocity": 0.5503538158132694,
+            "velocityX": 0.3239326300245928,
+            "velocityY": 0.5967954280576505,
+            "timestamp": 0.08342464065958187
+        },
+        {
+            "x": 4.342730369898429,
+            "y": 3.150178003649595,
+            "heading": -0.42997936088427535,
+            "angularVelocity": 1.0801073769506666,
+            "velocityX": 0.6317846408926776,
+            "velocityY": 1.2033681982072046,
+            "timestamp": 0.16684928131916374
+        },
+        {
+            "x": 4.418537982231504,
+            "y": 3.3026029150734924,
+            "heading": -0.2990998156933407,
+            "angularVelocity": 1.5688355880967437,
+            "velocityX": 0.9086957010988046,
+            "velocityY": 1.8270970089745318,
+            "timestamp": 0.2502739219787456
+        },
+        {
+            "x": 4.5104462205151705,
+            "y": 3.5100667276192694,
+            "heading": -0.13617594937576347,
+            "angularVelocity": 1.9529465758491624,
+            "velocityX": 1.1016917490685052,
+            "velocityY": 2.4868409489750483,
+            "timestamp": 0.3336985626383275
+        },
+        {
+            "x": 4.573472263152817,
+            "y": 3.7674681923433693,
+            "heading": 0.004287761033867021,
+            "angularVelocity": 1.6837196935950753,
+            "velocityX": 0.7554847361563879,
+            "velocityY": 3.0854369007645883,
+            "timestamp": 0.41712320329790936
+        },
+        {
+            "x": 4.6071871310155705,
+            "y": 4.066840423815047,
+            "heading": 0.04705620405427594,
+            "angularVelocity": 0.5126596013152507,
+            "velocityX": 0.40413560785150865,
+            "velocityY": 3.5885348633778387,
+            "timestamp": 0.5005478439574912
+        },
+        {
+            "x": 4.587417601167403,
+            "y": 4.370555626705338,
+            "heading": 0.08458578048595149,
+            "angularVelocity": 0.4498620088136386,
+            "velocityX": -0.23697470785444333,
+            "velocityY": 3.6405934804036506,
+            "timestamp": 0.5839724846170731
+        },
+        {
+            "x": 4.510022832963655,
+            "y": 4.67116829443229,
+            "heading": 0.1002321163817716,
+            "angularVelocity": 0.1875505338964035,
+            "velocityX": -0.9277207260569706,
+            "velocityY": 3.603403806719601,
+            "timestamp": 0.667397125276655
+        },
+        {
+            "x": 4.376634060333898,
+            "y": 4.957183607465761,
+            "heading": 0.10023219176740035,
+            "angularVelocity": 9.036374403612271e-7,
+            "velocityX": -1.5989133615097586,
+            "velocityY": 3.4284272700744434,
+            "timestamp": 0.7508217659362368
+        },
+        {
+            "x": 4.19313075849175,
+            "y": 5.213940045953902,
+            "heading": 0.10023221692184411,
+            "angularVelocity": 3.015229499928316e-7,
+            "velocityX": -2.1996295146291525,
+            "velocityY": 3.0777050576201734,
+            "timestamp": 0.8342464065958187
+        },
+        {
+            "x": 3.9657176811920825,
+            "y": 5.432756463781841,
+            "heading": 0.10023223856547658,
+            "angularVelocity": 2.5943932502769966e-7,
+            "velocityX": -2.725970115084308,
+            "velocityY": 2.6229231087830502,
+            "timestamp": 0.9176710472554006
+        },
+        {
+            "x": 3.7186426713834653,
+            "y": 5.629101242954768,
+            "heading": 0.10023225962798245,
+            "angularVelocity": 2.524734383899783e-7,
+            "velocityX": -2.961655068037016,
+            "velocityY": 2.3535585843770224,
+            "timestamp": 1.0010956879149824
+        },
+        {
+            "x": 3.4715770348675625,
+            "y": 5.825457816649942,
+            "heading": 0.10023228069061084,
+            "angularVelocity": 2.5247490699416256e-7,
+            "velocityX": -2.961542711631996,
+            "velocityY": 2.353699963736333,
+            "timestamp": 1.0845203285745642
+        },
+        {
+            "x": 3.224511384705754,
+            "y": 6.021814373175139,
+            "heading": 0.10023230175325984,
+            "angularVelocity": 2.5247515406639525e-7,
+            "velocityX": -2.96154287520364,
+            "velocityY": 2.353699757922098,
+            "timestamp": 1.167944969234146
+        },
+        {
+            "x": 2.9774457295747583,
+            "y": 6.218170923447359,
+            "heading": 0.10023232281654228,
+            "angularVelocity": 2.524827469327523e-7,
+            "velocityX": -2.9615429347686173,
+            "velocityY": 2.3536996829685104,
+            "timestamp": 1.2513696098937277
+        },
+        {
+            "x": 2.751412346836958,
+            "y": 6.39797212959724,
+            "heading": 0.1658210049038553,
+            "angularVelocity": 0.7862027521934517,
+            "velocityX": -2.7094319010631414,
+            "velocityY": 2.155252989144637,
+            "timestamp": 1.3347942505533095
+        },
+        {
+            "x": 2.5706626829115913,
+            "y": 6.54172747182469,
+            "heading": 0.24638520673123038,
+            "angularVelocity": 0.9657123026291593,
+            "velocityX": -2.166622025534686,
+            "velocityY": 1.723176043563091,
+            "timestamp": 1.4182188912128912
+        },
+        {
+            "x": 2.435257350873027,
+            "y": 6.649419264379792,
+            "heading": 0.31518856404188306,
+            "angularVelocity": 0.8247366337651717,
+            "velocityX": -1.6230855891977036,
+            "velocityY": 1.2908871012647558,
+            "timestamp": 1.501643531872473
+        },
+        {
+            "x": 2.3450659538089185,
+            "y": 6.721154353305962,
+            "heading": 0.3643262231924431,
+            "angularVelocity": 0.5890065424563048,
+            "velocityX": -1.0811122031935256,
+            "velocityY": 0.8598789081859941,
+            "timestamp": 1.5850681725320548
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 0.3077481257883978,
+            "velocityX": -0.5401995555822935,
+            "velocityY": 0.42967696846676384,
+            "timestamp": 1.6684928131916366
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": -2.8369790280137563e-29,
+            "velocityX": 0,
+            "velocityY": -1.9155196850548007e-31,
+            "timestamp": 1.7519174538512183
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.1.traj
new file mode 100644
index 00000000..01699583
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.1.traj	
@@ -0,0 +1,157 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.1175745999315144e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 4.331197427717942,
+            "y": 3.064203342806604,
+            "heading": -0.5918486780870994,
+            "angularVelocity": -0.24352382070717674,
+            "velocityX": 0.642497001367488,
+            "velocityY": 0.604868198278966,
+            "timestamp": 0.10614435176007272
+        },
+        {
+            "x": 4.457228812945604,
+            "y": 3.20188389088458,
+            "heading": -0.6500618235234895,
+            "angularVelocity": -0.5484337552691863,
+            "velocityX": 1.1873583769444531,
+            "velocityY": 1.2971066834454588,
+            "timestamp": 0.21228870352014545
+        },
+        {
+            "x": 4.610677951553499,
+            "y": 3.4285671398637003,
+            "heading": -0.7523465196948876,
+            "angularVelocity": -0.9636376733695737,
+            "velocityX": 1.4456646638603075,
+            "velocityY": 2.1356129197671536,
+            "timestamp": 0.31843305528021815
+        },
+        {
+            "x": 4.708912918877337,
+            "y": 3.730071149255109,
+            "heading": -0.9027593729452231,
+            "angularVelocity": -1.4170594172577993,
+            "velocityX": 0.9254846413861598,
+            "velocityY": 2.840509215911215,
+            "timestamp": 0.4245774070402909
+        },
+        {
+            "x": 4.717930123488916,
+            "y": 4.059451821332611,
+            "heading": -1.0196863730682848,
+            "angularVelocity": -1.1015847587195386,
+            "velocityX": 0.08495227925044571,
+            "velocityY": 3.10313894819415,
+            "timestamp": 0.5307217588003637
+        },
+        {
+            "x": 4.639893034223715,
+            "y": 4.389369622514019,
+            "heading": -1.0106655427651627,
+            "angularVelocity": 0.08498643737080536,
+            "velocityX": -0.7351977563685651,
+            "velocityY": 3.1081993126412395,
+            "timestamp": 0.6368661105604364
+        },
+        {
+            "x": 4.471904603111427,
+            "y": 4.710090421849215,
+            "heading": -0.9107146957571205,
+            "angularVelocity": 0.9416501712118399,
+            "velocityX": -1.5826412647185075,
+            "velocityY": 3.021553139823678,
+            "timestamp": 0.7430104623205092
+        },
+        {
+            "x": 4.218561716713538,
+            "y": 4.992188206454831,
+            "heading": -0.8355436069817835,
+            "angularVelocity": 0.7081967860640641,
+            "velocityX": -2.3867768957743736,
+            "velocityY": 2.657680601255776,
+            "timestamp": 0.8491548140805819
+        },
+        {
+            "x": 3.8896224481808446,
+            "y": 5.222473200709689,
+            "heading": -0.83554222765338,
+            "angularVelocity": 0.000012994835623464205,
+            "velocityX": -3.098980426921093,
+            "velocityY": 2.169545439171289,
+            "timestamp": 0.9552991658406547
+        },
+        {
+            "x": 3.516178298161353,
+            "y": 5.370026719708121,
+            "heading": -0.8355421446101063,
+            "angularVelocity": 7.823616840140413e-7,
+            "velocityX": -3.518266811442025,
+            "velocityY": 1.390121250464274,
+            "timestamp": 1.0614435176007273
+        },
+        {
+            "x": 3.1189612059977283,
+            "y": 5.42877328608909,
+            "heading": -0.8355420737846555,
+            "angularVelocity": 6.672559542292647e-7,
+            "velocityX": -3.742234848835754,
+            "velocityY": 0.55345918465599,
+            "timestamp": 1.1675878693608
+        },
+        {
+            "x": 2.774416222135212,
+            "y": 5.484170620762628,
+            "heading": -0.7035735462776811,
+            "angularVelocity": 1.243292980914089,
+            "velocityX": -3.2460039384980304,
+            "velocityY": 0.5219056290320314,
+            "timestamp": 1.2737322211208726
+        },
+        {
+            "x": 2.5095166350816602,
+            "y": 5.523131311595933,
+            "heading": -0.3884677902217159,
+            "angularVelocity": 2.9686530732056857,
+            "velocityX": -2.4956541036901063,
+            "velocityY": 0.3670538298766209,
+            "timestamp": 1.3798765728809452
+        },
+        {
+            "x": 2.335800273937377,
+            "y": 5.547984865708467,
+            "heading": -0.13742924633332027,
+            "angularVelocity": 2.365067379711733,
+            "velocityX": -1.6366048523895966,
+            "velocityY": 0.23414862590816604,
+            "timestamp": 1.4860209246410179
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 1.2947391364164484,
+            "velocityX": -0.8083357476365635,
+            "velocityY": 0.11319617193282665,
+            "timestamp": 1.5921652764010905
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -1.986468774957569e-34,
+            "velocityY": 0,
+            "timestamp": 1.6983096281611632
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.traj
new file mode 100644
index 00000000..01699583
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.traj	
@@ -0,0 +1,157 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.1175745999315144e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 4.331197427717942,
+            "y": 3.064203342806604,
+            "heading": -0.5918486780870994,
+            "angularVelocity": -0.24352382070717674,
+            "velocityX": 0.642497001367488,
+            "velocityY": 0.604868198278966,
+            "timestamp": 0.10614435176007272
+        },
+        {
+            "x": 4.457228812945604,
+            "y": 3.20188389088458,
+            "heading": -0.6500618235234895,
+            "angularVelocity": -0.5484337552691863,
+            "velocityX": 1.1873583769444531,
+            "velocityY": 1.2971066834454588,
+            "timestamp": 0.21228870352014545
+        },
+        {
+            "x": 4.610677951553499,
+            "y": 3.4285671398637003,
+            "heading": -0.7523465196948876,
+            "angularVelocity": -0.9636376733695737,
+            "velocityX": 1.4456646638603075,
+            "velocityY": 2.1356129197671536,
+            "timestamp": 0.31843305528021815
+        },
+        {
+            "x": 4.708912918877337,
+            "y": 3.730071149255109,
+            "heading": -0.9027593729452231,
+            "angularVelocity": -1.4170594172577993,
+            "velocityX": 0.9254846413861598,
+            "velocityY": 2.840509215911215,
+            "timestamp": 0.4245774070402909
+        },
+        {
+            "x": 4.717930123488916,
+            "y": 4.059451821332611,
+            "heading": -1.0196863730682848,
+            "angularVelocity": -1.1015847587195386,
+            "velocityX": 0.08495227925044571,
+            "velocityY": 3.10313894819415,
+            "timestamp": 0.5307217588003637
+        },
+        {
+            "x": 4.639893034223715,
+            "y": 4.389369622514019,
+            "heading": -1.0106655427651627,
+            "angularVelocity": 0.08498643737080536,
+            "velocityX": -0.7351977563685651,
+            "velocityY": 3.1081993126412395,
+            "timestamp": 0.6368661105604364
+        },
+        {
+            "x": 4.471904603111427,
+            "y": 4.710090421849215,
+            "heading": -0.9107146957571205,
+            "angularVelocity": 0.9416501712118399,
+            "velocityX": -1.5826412647185075,
+            "velocityY": 3.021553139823678,
+            "timestamp": 0.7430104623205092
+        },
+        {
+            "x": 4.218561716713538,
+            "y": 4.992188206454831,
+            "heading": -0.8355436069817835,
+            "angularVelocity": 0.7081967860640641,
+            "velocityX": -2.3867768957743736,
+            "velocityY": 2.657680601255776,
+            "timestamp": 0.8491548140805819
+        },
+        {
+            "x": 3.8896224481808446,
+            "y": 5.222473200709689,
+            "heading": -0.83554222765338,
+            "angularVelocity": 0.000012994835623464205,
+            "velocityX": -3.098980426921093,
+            "velocityY": 2.169545439171289,
+            "timestamp": 0.9552991658406547
+        },
+        {
+            "x": 3.516178298161353,
+            "y": 5.370026719708121,
+            "heading": -0.8355421446101063,
+            "angularVelocity": 7.823616840140413e-7,
+            "velocityX": -3.518266811442025,
+            "velocityY": 1.390121250464274,
+            "timestamp": 1.0614435176007273
+        },
+        {
+            "x": 3.1189612059977283,
+            "y": 5.42877328608909,
+            "heading": -0.8355420737846555,
+            "angularVelocity": 6.672559542292647e-7,
+            "velocityX": -3.742234848835754,
+            "velocityY": 0.55345918465599,
+            "timestamp": 1.1675878693608
+        },
+        {
+            "x": 2.774416222135212,
+            "y": 5.484170620762628,
+            "heading": -0.7035735462776811,
+            "angularVelocity": 1.243292980914089,
+            "velocityX": -3.2460039384980304,
+            "velocityY": 0.5219056290320314,
+            "timestamp": 1.2737322211208726
+        },
+        {
+            "x": 2.5095166350816602,
+            "y": 5.523131311595933,
+            "heading": -0.3884677902217159,
+            "angularVelocity": 2.9686530732056857,
+            "velocityX": -2.4956541036901063,
+            "velocityY": 0.3670538298766209,
+            "timestamp": 1.3798765728809452
+        },
+        {
+            "x": 2.335800273937377,
+            "y": 5.547984865708467,
+            "heading": -0.13742924633332027,
+            "angularVelocity": 2.365067379711733,
+            "velocityX": -1.6366048523895966,
+            "velocityY": 0.23414862590816604,
+            "timestamp": 1.4860209246410179
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 1.2947391364164484,
+            "velocityX": -0.8083357476365635,
+            "velocityY": 0.11319617193282665,
+            "timestamp": 1.5921652764010905
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -1.986468774957569e-34,
+            "velocityY": 0,
+            "timestamp": 1.6983096281611632
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.1.traj
new file mode 100644
index 00000000..26f24eb2
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.1.traj	
@@ -0,0 +1,121 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 0,
+            "velocityX": -3.809622996347125e-33,
+            "velocityY": 4.7029981856297106e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 4.1621681366579715,
+            "y": 2.9362991956112916,
+            "heading": -0.5618109581410129,
+            "angularVelocity": 0.03502487973446616,
+            "velocityX": -0.8430624486077395,
+            "velocityY": -0.5326069988815002,
+            "timestamp": 0.11960189130537771
+        },
+        {
+            "x": 3.949371568032143,
+            "y": 2.832059073063443,
+            "heading": -0.5332490097615643,
+            "angularVelocity": 0.2388085010004113,
+            "velocityX": -1.7792073879709687,
+            "velocityY": -0.8715591485229383,
+            "timestamp": 0.23920378261075542
+        },
+        {
+            "x": 3.6196334559490815,
+            "y": 2.7428786025637635,
+            "heading": -0.4400400521459035,
+            "angularVelocity": 0.7793267865446122,
+            "velocityX": -2.756964028613438,
+            "velocityY": -0.7456443165432319,
+            "timestamp": 0.35880567391613316
+        },
+        {
+            "x": 3.2124362068162218,
+            "y": 2.7443781288173086,
+            "heading": -0.34020495702705544,
+            "angularVelocity": 0.8347283979314292,
+            "velocityX": -3.4046054346513173,
+            "velocityY": 0.012537646664080927,
+            "timestamp": 0.47840756522151084
+        },
+        {
+            "x": 2.829910158877178,
+            "y": 2.8625567119312745,
+            "heading": -0.23363822858373923,
+            "angularVelocity": 0.891012067453128,
+            "velocityX": -3.1983277502054186,
+            "velocityY": 0.9880996180254248,
+            "timestamp": 0.5980094565268885
+        },
+        {
+            "x": 2.5228974587582895,
+            "y": 3.073051960500077,
+            "heading": -0.12255766699698727,
+            "angularVelocity": 0.9287525504349555,
+            "velocityX": -2.566955227614215,
+            "velocityY": 1.7599658857512914,
+            "timestamp": 0.7176113478322662
+        },
+        {
+            "x": 2.307806503132395,
+            "y": 3.359545759981809,
+            "heading": -0.008191563913849259,
+            "angularVelocity": 0.9562231987710198,
+            "velocityX": -1.7983909224036065,
+            "velocityY": 2.395395226236259,
+            "timestamp": 0.8372132391376439
+        },
+        {
+            "x": 2.197079056424326,
+            "y": 3.692587049160117,
+            "heading": -0.017161158826022753,
+            "angularVelocity": -0.07499542703130056,
+            "velocityX": -0.9258001315827844,
+            "velocityY": 2.7845821294578066,
+            "timestamp": 0.9568151304430216
+        },
+        {
+            "x": 2.2005818293568207,
+            "y": 4.024472502975606,
+            "heading": -0.15274865723996348,
+            "angularVelocity": -1.133656808718376,
+            "velocityX": 0.029286935969546842,
+            "velocityY": 2.774918107006309,
+            "timestamp": 1.0764170217483993
+        },
+        {
+            "x": 2.2590478504225837,
+            "y": 4.250579625353832,
+            "heading": -0.3027008584759083,
+            "angularVelocity": -1.2537611203242134,
+            "velocityX": 0.48883859968803833,
+            "velocityY": 1.8904978835235247,
+            "timestamp": 1.196018913053777
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.729914389908579,
+            "velocityX": 0.3424038627687911,
+            "velocityY": 0.914871608232253,
+            "timestamp": 1.3156208043591548
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 3.1982065543283105e-28,
+            "velocityX": 4.1170744495691044e-30,
+            "velocityY": 1.8090480101607624e-30,
+            "timestamp": 1.4352226956645326
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.traj
new file mode 100644
index 00000000..26f24eb2
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.traj	
@@ -0,0 +1,121 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 0,
+            "velocityX": -3.809622996347125e-33,
+            "velocityY": 4.7029981856297106e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 4.1621681366579715,
+            "y": 2.9362991956112916,
+            "heading": -0.5618109581410129,
+            "angularVelocity": 0.03502487973446616,
+            "velocityX": -0.8430624486077395,
+            "velocityY": -0.5326069988815002,
+            "timestamp": 0.11960189130537771
+        },
+        {
+            "x": 3.949371568032143,
+            "y": 2.832059073063443,
+            "heading": -0.5332490097615643,
+            "angularVelocity": 0.2388085010004113,
+            "velocityX": -1.7792073879709687,
+            "velocityY": -0.8715591485229383,
+            "timestamp": 0.23920378261075542
+        },
+        {
+            "x": 3.6196334559490815,
+            "y": 2.7428786025637635,
+            "heading": -0.4400400521459035,
+            "angularVelocity": 0.7793267865446122,
+            "velocityX": -2.756964028613438,
+            "velocityY": -0.7456443165432319,
+            "timestamp": 0.35880567391613316
+        },
+        {
+            "x": 3.2124362068162218,
+            "y": 2.7443781288173086,
+            "heading": -0.34020495702705544,
+            "angularVelocity": 0.8347283979314292,
+            "velocityX": -3.4046054346513173,
+            "velocityY": 0.012537646664080927,
+            "timestamp": 0.47840756522151084
+        },
+        {
+            "x": 2.829910158877178,
+            "y": 2.8625567119312745,
+            "heading": -0.23363822858373923,
+            "angularVelocity": 0.891012067453128,
+            "velocityX": -3.1983277502054186,
+            "velocityY": 0.9880996180254248,
+            "timestamp": 0.5980094565268885
+        },
+        {
+            "x": 2.5228974587582895,
+            "y": 3.073051960500077,
+            "heading": -0.12255766699698727,
+            "angularVelocity": 0.9287525504349555,
+            "velocityX": -2.566955227614215,
+            "velocityY": 1.7599658857512914,
+            "timestamp": 0.7176113478322662
+        },
+        {
+            "x": 2.307806503132395,
+            "y": 3.359545759981809,
+            "heading": -0.008191563913849259,
+            "angularVelocity": 0.9562231987710198,
+            "velocityX": -1.7983909224036065,
+            "velocityY": 2.395395226236259,
+            "timestamp": 0.8372132391376439
+        },
+        {
+            "x": 2.197079056424326,
+            "y": 3.692587049160117,
+            "heading": -0.017161158826022753,
+            "angularVelocity": -0.07499542703130056,
+            "velocityX": -0.9258001315827844,
+            "velocityY": 2.7845821294578066,
+            "timestamp": 0.9568151304430216
+        },
+        {
+            "x": 2.2005818293568207,
+            "y": 4.024472502975606,
+            "heading": -0.15274865723996348,
+            "angularVelocity": -1.133656808718376,
+            "velocityX": 0.029286935969546842,
+            "velocityY": 2.774918107006309,
+            "timestamp": 1.0764170217483993
+        },
+        {
+            "x": 2.2590478504225837,
+            "y": 4.250579625353832,
+            "heading": -0.3027008584759083,
+            "angularVelocity": -1.2537611203242134,
+            "velocityX": 0.48883859968803833,
+            "velocityY": 1.8904978835235247,
+            "timestamp": 1.196018913053777
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.729914389908579,
+            "velocityX": 0.3424038627687911,
+            "velocityY": 0.914871608232253,
+            "timestamp": 1.3156208043591548
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 3.1982065543283105e-28,
+            "velocityX": 4.1170744495691044e-30,
+            "velocityY": 1.8090480101607624e-30,
+            "timestamp": 1.4352226956645326
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C2.1.traj
new file mode 100644
index 00000000..0e0e8d38
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C2.1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -2.491480567253294e-33,
+            "velocityX": -2.0103104784295923e-34,
+            "velocityY": 1.605618083193578e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 5.240158813928264,
+            "y": 1.6204190327819517,
+            "heading": -0.5237338108165747,
+            "angularVelocity": 1.2526389382742513,
+            "velocityX": 0.6912398963489377,
+            "velocityY": 0.004514612397400031,
+            "timestamp": 0.0928170006782674
+        },
+        {
+            "x": 5.369626464101249,
+            "y": 1.6247166229091008,
+            "heading": -0.2974184918320366,
+            "angularVelocity": 2.4382959730515035,
+            "velocityX": 1.394870004707003,
+            "velocityY": 0.046301756097956205,
+            "timestamp": 0.1856340013565348
+        },
+        {
+            "x": 5.5674551093283755,
+            "y": 1.639197763374398,
+            "heading": 0.0009013327241277373,
+            "angularVelocity": 3.214064475001015,
+            "velocityX": 2.1313837312289596,
+            "velocityY": 0.15601819019657054,
+            "timestamp": 0.2784510020348022
+        },
+        {
+            "x": 5.831585704851482,
+            "y": 1.6774487923340957,
+            "heading": 0.2480008567342784,
+            "angularVelocity": 2.6622226769282764,
+            "velocityX": 2.8457135394696165,
+            "velocityY": 0.41211231434085943,
+            "timestamp": 0.3712680027130696
+        },
+        {
+            "x": 6.141513989428359,
+            "y": 1.75469627200855,
+            "heading": 0.3453177753240257,
+            "angularVelocity": 1.0484816130514494,
+            "velocityX": 3.339132726893265,
+            "velocityY": 0.8322557194259916,
+            "timestamp": 0.46408500339133696
+        },
+        {
+            "x": 6.452443961209774,
+            "y": 1.9008832260175597,
+            "heading": 0.36957097138497697,
+            "angularVelocity": 0.2613012258930921,
+            "velocityX": 3.3499247929718696,
+            "velocityY": 1.5750019171136467,
+            "timestamp": 0.5569020040696043
+        },
+        {
+            "x": 6.733109637262523,
+            "y": 2.111863899724239,
+            "heading": 0.36957109822659895,
+            "angularVelocity": 0.0000013665774702958542,
+            "velocityX": 3.023860650546378,
+            "velocityY": 2.273082217319261,
+            "timestamp": 0.6497190047478717
+        },
+        {
+            "x": 6.964965499466835,
+            "y": 2.3755472934997988,
+            "heading": 0.36957101579118784,
+            "angularVelocity": -8.881499138395027e-7,
+            "velocityX": 2.4979891669630336,
+            "velocityY": 2.840895437782658,
+            "timestamp": 0.7425360054261391
+        },
+        {
+            "x": 7.138307566970259,
+            "y": 2.680897277378083,
+            "heading": 0.36957096673523665,
+            "angularVelocity": -5.285233395413412e-7,
+            "velocityX": 1.867568077364202,
+            "velocityY": 3.289806626446832,
+            "timestamp": 0.8353530061044064
+        },
+        {
+            "x": 7.245881008413397,
+            "y": 3.0151337865477195,
+            "heading": 0.3695709277774119,
+            "angularVelocity": -4.197272531605919e-7,
+            "velocityX": 1.1589842448800958,
+            "velocityY": 3.6010268240427665,
+            "timestamp": 0.9281700067826738
+        },
+        {
+            "x": 7.300266060372569,
+            "y": 3.36201756600148,
+            "heading": 0.3695708905369613,
+            "angularVelocity": -4.012244559973519e-7,
+            "velocityX": 0.5859384763755527,
+            "velocityY": 3.7372871017042186,
+            "timestamp": 1.0209870074609413
+        },
+        {
+            "x": 7.354650396010165,
+            "y": 3.708901457760651,
+            "heading": 0.3695708532965109,
+            "angularVelocity": -4.0122445392101915e-7,
+            "velocityX": 0.5859307588068667,
+            "velocityY": 3.7372883116701696,
+            "timestamp": 1.1138040081392087
+        },
+        {
+            "x": 7.409034640641628,
+            "y": 4.055785363787717,
+            "heading": 0.36957081605605097,
+            "angularVelocity": -4.012245569815441e-7,
+            "velocityX": 0.5859297783169686,
+            "velocityY": 3.737288465390881,
+            "timestamp": 1.206621008817476
+        },
+        {
+            "x": 7.463418795891099,
+            "y": 4.402669283828022,
+            "heading": 0.36957077881557854,
+            "angularVelocity": -4.0122469066112833e-7,
+            "velocityX": 0.5859288153253637,
+            "velocityY": 3.7372886163679513,
+            "timestamp": 1.2994380094957434
+        },
+        {
+            "x": 7.517802854177964,
+            "y": 4.7495532190700205,
+            "heading": 0.3695707415750949,
+            "angularVelocity": -4.0122481249990114e-7,
+            "velocityX": 0.5859277706610763,
+            "velocityY": 3.737288780149305,
+            "timestamp": 1.3922550101740108
+        },
+        {
+            "x": 7.56989445358868,
+            "y": 5.095450165697103,
+            "heading": 0.36620989603022114,
+            "angularVelocity": -0.036209374579162214,
+            "velocityX": 0.5612290747390308,
+            "velocityY": 3.7266550750338174,
+            "timestamp": 1.4850720108522781
+        },
+        {
+            "x": 7.614129631330889,
+            "y": 5.376850109081837,
+            "heading": 0.24917441522367106,
+            "angularVelocity": -1.2609272003114111,
+            "velocityX": 0.4765848650458117,
+            "velocityY": 3.0317715647821197,
+            "timestamp": 1.5778890115305455
+        },
+        {
+            "x": 7.6471559939174325,
+            "y": 5.587299345919279,
+            "heading": 0.1342077337923999,
+            "angularVelocity": -1.2386381868746383,
+            "velocityX": 0.35582234229936965,
+            "velocityY": 2.267356575838128,
+            "timestamp": 1.6707060122088129
+        },
+        {
+            "x": 7.669079055841979,
+            "y": 5.72720580380319,
+            "heading": 0.04709984861251457,
+            "angularVelocity": -0.9384906271840042,
+            "velocityX": 0.23619662092442378,
+            "velocityY": 1.5073365532341503,
+            "timestamp": 1.7635230128870802
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -9.59663427624948e-34,
+            "angularVelocity": -0.5074485090913172,
+            "velocityX": 0.11766103276571077,
+            "velocityY": 0.751954875580805,
+            "timestamp": 1.8563400135653476
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 1.9250732473517817e-32,
+            "velocityX": 0,
+            "velocityY": 2.1529996497623123e-34,
+            "timestamp": 1.949157014243615
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C2.traj
new file mode 100644
index 00000000..0e0e8d38
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C2.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -2.491480567253294e-33,
+            "velocityX": -2.0103104784295923e-34,
+            "velocityY": 1.605618083193578e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 5.240158813928264,
+            "y": 1.6204190327819517,
+            "heading": -0.5237338108165747,
+            "angularVelocity": 1.2526389382742513,
+            "velocityX": 0.6912398963489377,
+            "velocityY": 0.004514612397400031,
+            "timestamp": 0.0928170006782674
+        },
+        {
+            "x": 5.369626464101249,
+            "y": 1.6247166229091008,
+            "heading": -0.2974184918320366,
+            "angularVelocity": 2.4382959730515035,
+            "velocityX": 1.394870004707003,
+            "velocityY": 0.046301756097956205,
+            "timestamp": 0.1856340013565348
+        },
+        {
+            "x": 5.5674551093283755,
+            "y": 1.639197763374398,
+            "heading": 0.0009013327241277373,
+            "angularVelocity": 3.214064475001015,
+            "velocityX": 2.1313837312289596,
+            "velocityY": 0.15601819019657054,
+            "timestamp": 0.2784510020348022
+        },
+        {
+            "x": 5.831585704851482,
+            "y": 1.6774487923340957,
+            "heading": 0.2480008567342784,
+            "angularVelocity": 2.6622226769282764,
+            "velocityX": 2.8457135394696165,
+            "velocityY": 0.41211231434085943,
+            "timestamp": 0.3712680027130696
+        },
+        {
+            "x": 6.141513989428359,
+            "y": 1.75469627200855,
+            "heading": 0.3453177753240257,
+            "angularVelocity": 1.0484816130514494,
+            "velocityX": 3.339132726893265,
+            "velocityY": 0.8322557194259916,
+            "timestamp": 0.46408500339133696
+        },
+        {
+            "x": 6.452443961209774,
+            "y": 1.9008832260175597,
+            "heading": 0.36957097138497697,
+            "angularVelocity": 0.2613012258930921,
+            "velocityX": 3.3499247929718696,
+            "velocityY": 1.5750019171136467,
+            "timestamp": 0.5569020040696043
+        },
+        {
+            "x": 6.733109637262523,
+            "y": 2.111863899724239,
+            "heading": 0.36957109822659895,
+            "angularVelocity": 0.0000013665774702958542,
+            "velocityX": 3.023860650546378,
+            "velocityY": 2.273082217319261,
+            "timestamp": 0.6497190047478717
+        },
+        {
+            "x": 6.964965499466835,
+            "y": 2.3755472934997988,
+            "heading": 0.36957101579118784,
+            "angularVelocity": -8.881499138395027e-7,
+            "velocityX": 2.4979891669630336,
+            "velocityY": 2.840895437782658,
+            "timestamp": 0.7425360054261391
+        },
+        {
+            "x": 7.138307566970259,
+            "y": 2.680897277378083,
+            "heading": 0.36957096673523665,
+            "angularVelocity": -5.285233395413412e-7,
+            "velocityX": 1.867568077364202,
+            "velocityY": 3.289806626446832,
+            "timestamp": 0.8353530061044064
+        },
+        {
+            "x": 7.245881008413397,
+            "y": 3.0151337865477195,
+            "heading": 0.3695709277774119,
+            "angularVelocity": -4.197272531605919e-7,
+            "velocityX": 1.1589842448800958,
+            "velocityY": 3.6010268240427665,
+            "timestamp": 0.9281700067826738
+        },
+        {
+            "x": 7.300266060372569,
+            "y": 3.36201756600148,
+            "heading": 0.3695708905369613,
+            "angularVelocity": -4.012244559973519e-7,
+            "velocityX": 0.5859384763755527,
+            "velocityY": 3.7372871017042186,
+            "timestamp": 1.0209870074609413
+        },
+        {
+            "x": 7.354650396010165,
+            "y": 3.708901457760651,
+            "heading": 0.3695708532965109,
+            "angularVelocity": -4.0122445392101915e-7,
+            "velocityX": 0.5859307588068667,
+            "velocityY": 3.7372883116701696,
+            "timestamp": 1.1138040081392087
+        },
+        {
+            "x": 7.409034640641628,
+            "y": 4.055785363787717,
+            "heading": 0.36957081605605097,
+            "angularVelocity": -4.012245569815441e-7,
+            "velocityX": 0.5859297783169686,
+            "velocityY": 3.737288465390881,
+            "timestamp": 1.206621008817476
+        },
+        {
+            "x": 7.463418795891099,
+            "y": 4.402669283828022,
+            "heading": 0.36957077881557854,
+            "angularVelocity": -4.0122469066112833e-7,
+            "velocityX": 0.5859288153253637,
+            "velocityY": 3.7372886163679513,
+            "timestamp": 1.2994380094957434
+        },
+        {
+            "x": 7.517802854177964,
+            "y": 4.7495532190700205,
+            "heading": 0.3695707415750949,
+            "angularVelocity": -4.0122481249990114e-7,
+            "velocityX": 0.5859277706610763,
+            "velocityY": 3.737288780149305,
+            "timestamp": 1.3922550101740108
+        },
+        {
+            "x": 7.56989445358868,
+            "y": 5.095450165697103,
+            "heading": 0.36620989603022114,
+            "angularVelocity": -0.036209374579162214,
+            "velocityX": 0.5612290747390308,
+            "velocityY": 3.7266550750338174,
+            "timestamp": 1.4850720108522781
+        },
+        {
+            "x": 7.614129631330889,
+            "y": 5.376850109081837,
+            "heading": 0.24917441522367106,
+            "angularVelocity": -1.2609272003114111,
+            "velocityX": 0.4765848650458117,
+            "velocityY": 3.0317715647821197,
+            "timestamp": 1.5778890115305455
+        },
+        {
+            "x": 7.6471559939174325,
+            "y": 5.587299345919279,
+            "heading": 0.1342077337923999,
+            "angularVelocity": -1.2386381868746383,
+            "velocityX": 0.35582234229936965,
+            "velocityY": 2.267356575838128,
+            "timestamp": 1.6707060122088129
+        },
+        {
+            "x": 7.669079055841979,
+            "y": 5.72720580380319,
+            "heading": 0.04709984861251457,
+            "angularVelocity": -0.9384906271840042,
+            "velocityX": 0.23619662092442378,
+            "velocityY": 1.5073365532341503,
+            "timestamp": 1.7635230128870802
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -9.59663427624948e-34,
+            "angularVelocity": -0.5074485090913172,
+            "velocityX": 0.11766103276571077,
+            "velocityY": 0.751954875580805,
+            "timestamp": 1.8563400135653476
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 1.9250732473517817e-32,
+            "velocityX": 0,
+            "velocityY": 2.1529996497623123e-34,
+            "timestamp": 1.949157014243615
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.1.traj
deleted file mode 100644
index 2987bdd4..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": -5.177514657860589e-36,
-            "velocityY": 7.381711039090797e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 5.22399403974827,
-            "y": 1.6219999714389142,
-            "heading": 2.5657089279494847,
-            "angularVelocity": 0.8361720983637341,
-            "velocityX": 0.6107431391380216,
-            "velocityY": 0.02545042761966764,
-            "timestamp": 0.07858301906756712
-        },
-        {
-            "x": 5.320421642922389,
-            "y": 1.6267873270311852,
-            "heading": 2.6934140350122218,
-            "angularVelocity": 1.6250979992628447,
-            "velocityX": 1.2270793909204332,
-            "velocityY": 0.06092099348021541,
-            "timestamp": 0.15716603813513425
-        },
-        {
-            "x": 5.466088696265102,
-            "y": 1.6359772771500425,
-            "heading": 2.8734313337183015,
-            "angularVelocity": 2.2907913292475777,
-            "velocityX": 1.8536708702609945,
-            "velocityY": 0.11694575021297698,
-            "timestamp": 0.23574905720270137
-        },
-        {
-            "x": 5.662207065544692,
-            "y": 1.6523107767500078,
-            "heading": 3.0800991109745315,
-            "angularVelocity": 2.629929210005703,
-            "velocityX": 2.495683820838744,
-            "velocityY": 0.2078502428867176,
-            "timestamp": 0.3143320762702685
-        },
-        {
-            "x": 5.9065811613932215,
-            "y": 1.6816988713551873,
-            "heading": 3.2417008687725852,
-            "angularVelocity": 2.056446287194764,
-            "velocityX": 3.1097570282762126,
-            "velocityY": 0.3739751278824109,
-            "timestamp": 0.39291509533783564
-        },
-        {
-            "x": 6.188716953254853,
-            "y": 1.7271916925648765,
-            "heading": 3.2799906959816494,
-            "angularVelocity": 0.48725319621713953,
-            "velocityX": 3.5902895461301405,
-            "velocityY": 0.5789141439141413,
-            "timestamp": 0.4714981144054028
-        },
-        {
-            "x": 6.470102604778816,
-            "y": 1.8230794962925625,
-            "heading": 3.2799906961506404,
-            "angularVelocity": 2.150475348001427e-9,
-            "velocityX": 3.580743713626258,
-            "velocityY": 1.22021022945479,
-            "timestamp": 0.5500811334729699
-        },
-        {
-            "x": 6.733423435664353,
-            "y": 1.9610450600579923,
-            "heading": 3.279990689877103,
-            "angularVelocity": -7.983324169828431e-8,
-            "velocityX": 3.3508617257263804,
-            "velocityY": 1.755666369178364,
-            "timestamp": 0.6286641525405371
-        },
-        {
-            "x": 6.996743409287627,
-            "y": 2.0990122599800607,
-            "heading": 3.2799906836034283,
-            "angularVelocity": -7.983499950790008e-8,
-            "velocityX": 3.3508508167250133,
-            "velocityY": 1.7556871899187498,
-            "timestamp": 0.7072471716081042
-        },
-        {
-            "x": 7.224495559877957,
-            "y": 2.2183414399847847,
-            "heading": 3.23332230759167,
-            "angularVelocity": -0.5938735437439961,
-            "velocityX": 2.8982362002979842,
-            "velocityY": 1.5185110144740412,
-            "timestamp": 0.7858301906756714
-        },
-        {
-            "x": 7.406697323512462,
-            "y": 2.313804835379604,
-            "heading": 3.1959916534176562,
-            "angularVelocity": -0.4750473399592355,
-            "velocityX": 2.31858950949497,
-            "velocityY": 1.2148094655505426,
-            "timestamp": 0.8644132097432385
-        },
-        {
-            "x": 7.543348661054632,
-            "y": 2.3854024089057084,
-            "heading": 3.1679955366885673,
-            "angularVelocity": -0.356261658832645,
-            "velocityX": 1.7389423206643992,
-            "velocityY": 0.9111074424939519,
-            "timestamp": 0.9429962288108057
-        },
-        {
-            "x": 7.634449555023231,
-            "y": 2.4331341352600573,
-            "heading": 3.149331937782722,
-            "angularVelocity": -0.2375016781907786,
-            "velocityX": 1.1592949093781795,
-            "velocityY": 0.607405097446157,
-            "timestamp": 1.0215792478783727
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -0.11875259939679916,
-            "velocityX": 0.5796474291424605,
-            "velocityY": 0.30370256860992423,
-            "timestamp": 1.1001622669459399
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -1.0945332293260443e-31,
-            "velocityX": 7.198086319769953e-33,
-            "velocityY": -6.46561486115161e-33,
-            "timestamp": 1.178745286013507
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.traj
deleted file mode 100644
index 2987bdd4..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 0,
-            "velocityX": -5.177514657860589e-36,
-            "velocityY": 7.381711039090797e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 5.22399403974827,
-            "y": 1.6219999714389142,
-            "heading": 2.5657089279494847,
-            "angularVelocity": 0.8361720983637341,
-            "velocityX": 0.6107431391380216,
-            "velocityY": 0.02545042761966764,
-            "timestamp": 0.07858301906756712
-        },
-        {
-            "x": 5.320421642922389,
-            "y": 1.6267873270311852,
-            "heading": 2.6934140350122218,
-            "angularVelocity": 1.6250979992628447,
-            "velocityX": 1.2270793909204332,
-            "velocityY": 0.06092099348021541,
-            "timestamp": 0.15716603813513425
-        },
-        {
-            "x": 5.466088696265102,
-            "y": 1.6359772771500425,
-            "heading": 2.8734313337183015,
-            "angularVelocity": 2.2907913292475777,
-            "velocityX": 1.8536708702609945,
-            "velocityY": 0.11694575021297698,
-            "timestamp": 0.23574905720270137
-        },
-        {
-            "x": 5.662207065544692,
-            "y": 1.6523107767500078,
-            "heading": 3.0800991109745315,
-            "angularVelocity": 2.629929210005703,
-            "velocityX": 2.495683820838744,
-            "velocityY": 0.2078502428867176,
-            "timestamp": 0.3143320762702685
-        },
-        {
-            "x": 5.9065811613932215,
-            "y": 1.6816988713551873,
-            "heading": 3.2417008687725852,
-            "angularVelocity": 2.056446287194764,
-            "velocityX": 3.1097570282762126,
-            "velocityY": 0.3739751278824109,
-            "timestamp": 0.39291509533783564
-        },
-        {
-            "x": 6.188716953254853,
-            "y": 1.7271916925648765,
-            "heading": 3.2799906959816494,
-            "angularVelocity": 0.48725319621713953,
-            "velocityX": 3.5902895461301405,
-            "velocityY": 0.5789141439141413,
-            "timestamp": 0.4714981144054028
-        },
-        {
-            "x": 6.470102604778816,
-            "y": 1.8230794962925625,
-            "heading": 3.2799906961506404,
-            "angularVelocity": 2.150475348001427e-9,
-            "velocityX": 3.580743713626258,
-            "velocityY": 1.22021022945479,
-            "timestamp": 0.5500811334729699
-        },
-        {
-            "x": 6.733423435664353,
-            "y": 1.9610450600579923,
-            "heading": 3.279990689877103,
-            "angularVelocity": -7.983324169828431e-8,
-            "velocityX": 3.3508617257263804,
-            "velocityY": 1.755666369178364,
-            "timestamp": 0.6286641525405371
-        },
-        {
-            "x": 6.996743409287627,
-            "y": 2.0990122599800607,
-            "heading": 3.2799906836034283,
-            "angularVelocity": -7.983499950790008e-8,
-            "velocityX": 3.3508508167250133,
-            "velocityY": 1.7556871899187498,
-            "timestamp": 0.7072471716081042
-        },
-        {
-            "x": 7.224495559877957,
-            "y": 2.2183414399847847,
-            "heading": 3.23332230759167,
-            "angularVelocity": -0.5938735437439961,
-            "velocityX": 2.8982362002979842,
-            "velocityY": 1.5185110144740412,
-            "timestamp": 0.7858301906756714
-        },
-        {
-            "x": 7.406697323512462,
-            "y": 2.313804835379604,
-            "heading": 3.1959916534176562,
-            "angularVelocity": -0.4750473399592355,
-            "velocityX": 2.31858950949497,
-            "velocityY": 1.2148094655505426,
-            "timestamp": 0.8644132097432385
-        },
-        {
-            "x": 7.543348661054632,
-            "y": 2.3854024089057084,
-            "heading": 3.1679955366885673,
-            "angularVelocity": -0.356261658832645,
-            "velocityX": 1.7389423206643992,
-            "velocityY": 0.9111074424939519,
-            "timestamp": 0.9429962288108057
-        },
-        {
-            "x": 7.634449555023231,
-            "y": 2.4331341352600573,
-            "heading": 3.149331937782722,
-            "angularVelocity": -0.2375016781907786,
-            "velocityX": 1.1592949093781795,
-            "velocityY": 0.607405097446157,
-            "timestamp": 1.0215792478783727
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -0.11875259939679916,
-            "velocityX": 0.5796474291424605,
-            "velocityY": 0.30370256860992423,
-            "timestamp": 1.1001622669459399
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -1.0945332293260443e-31,
-            "velocityX": 7.198086319769953e-33,
-            "velocityY": -6.46561486115161e-33,
-            "timestamp": 1.178745286013507
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.1.traj
index 89c7b3cf..10168382 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.1.traj	
@@ -3,146 +3,146 @@
         {
             "x": 5.176,
             "y": 1.62,
-            "heading": 2.5,
+            "heading": -0.64,
             "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
+            "velocityX": -3.378617665126574e-35,
+            "velocityY": 1.1970750708579272e-32,
             "timestamp": 0
         },
         {
-            "x": 5.222452071774302,
-            "y": 1.6045080698057588,
-            "heading": 2.5267186887396895,
-            "angularVelocity": 0.3466731100007665,
-            "velocityX": 0.6027123690413302,
-            "velocityY": -0.20100670630495998,
-            "timestamp": 0.0770717080988209
+            "x": 5.2224556017723405,
+            "y": 1.6045081405552968,
+            "heading": -0.6133597294207822,
+            "angularVelocity": 0.3456555725750699,
+            "velocityX": 0.6027580531582283,
+            "velocityY": -0.20100574919793807,
+            "timestamp": 0.07707172310503405
         },
         {
-            "x": 5.315452291782514,
-            "y": 1.5735030960792171,
-            "heading": 2.577725213280899,
-            "angularVelocity": 0.6618060738424182,
-            "velocityX": 1.2066713233988244,
-            "velocityY": -0.40228735668849247,
-            "timestamp": 0.1541434161976418
+            "x": 5.3154629271275695,
+            "y": 1.5735046785143454,
+            "heading": -0.5625081883046615,
+            "angularVelocity": 0.6597950463209403,
+            "velocityX": 1.2067632798150456,
+            "velocityY": -0.4022676643507709,
+            "timestamp": 0.1541434462100681
         },
         {
-            "x": 5.4551316607074885,
-            "y": 1.5269515495858468,
-            "heading": 2.6491768389859613,
-            "angularVelocity": 0.9270798256274155,
-            "velocityX": 1.8123300024164322,
-            "velocityY": -0.6040030465353413,
-            "timestamp": 0.23121512429646268
+            "x": 5.455153059548346,
+            "y": 1.5269582787185583,
+            "heading": -0.4912835813545399,
+            "angularVelocity": 0.9241340932919854,
+            "velocityX": 1.812469305122525,
+            "velocityY": -0.6039361509065161,
+            "timestamp": 0.23121516931510214
         },
         {
-            "x": 5.641664886749133,
-            "y": 1.4648000786644693,
-            "heading": 2.734238612850838,
-            "angularVelocity": 1.1036705421892394,
-            "velocityX": 2.420255508058452,
-            "velocityY": -0.8064109704392095,
-            "timestamp": 0.3082868323952836
+            "x": 5.641700996428857,
+            "y": 1.4648196222358638,
+            "heading": -0.40650985692768693,
+            "angularVelocity": 1.0999329067980297,
+            "velocityX": 2.4204459088877828,
+            "velocityY": -0.8062445470177368,
+            "timestamp": 0.3082868924201362
         },
         {
-            "x": 5.8751883475378746,
-            "y": 1.3870009970649684,
-            "heading": 2.8177772109144708,
-            "angularVelocity": 1.0839074431374935,
-            "velocityX": 3.02995050387775,
-            "velocityY": -1.0094376200894257,
-            "timestamp": 0.3853585404941045
+            "x": 5.875244676488674,
+            "y": 1.3870501062573488,
+            "heading": -0.3232929818469195,
+            "angularVelocity": 1.0797328997998226,
+            "velocityX": 3.0302122575038553,
+            "velocityY": -1.0090538117660863,
+            "timestamp": 0.38535861552517026
         },
         {
-            "x": 6.15120111527276,
-            "y": 1.295041169269926,
-            "heading": 2.8199508970154183,
-            "angularVelocity": 0.028203424506439225,
-            "velocityX": 3.5812462775702794,
-            "velocityY": -1.193172307497486,
-            "timestamp": 0.4624302485929254
+            "x": 6.151314773692177,
+            "y": 1.2951594028189055,
+            "heading": -0.3212316148216143,
+            "angularVelocity": 0.026746087180327376,
+            "velocityX": 3.581989425969788,
+            "velocityY": -1.1922751916836483,
+            "timestamp": 0.46243033863020433
         },
         {
-            "x": 6.427838786740925,
-            "y": 1.202967940667996,
-            "heading": 2.8199509259126705,
-            "angularVelocity": 3.7493981751466555e-7,
-            "velocityX": 3.5893543596239312,
-            "velocityY": -1.1946436750029477,
-            "timestamp": 0.5395019566917463
+            "x": 6.427941619479976,
+            "y": 1.2030534747397732,
+            "heading": -0.3212315858589365,
+            "angularVelocity": 3.7578863628089786e-7,
+            "velocityX": 3.5892131983452513,
+            "velocityY": -1.1950677157381002,
+            "timestamp": 0.5395020617352384
         },
         {
-            "x": 6.7044764770342145,
-            "y": 1.110894768626946,
-            "heading": 2.8199509548099355,
-            "angularVelocity": 3.749399855143739e-7,
-            "velocityX": 3.5893546038785895,
-            "velocityY": -1.1946429411295039,
-            "timestamp": 0.6165736647905672
+            "x": 6.704568483880494,
+            "y": 1.1109476025613012,
+            "heading": -0.3212315568962456,
+            "angularVelocity": 3.75788807477423e-7,
+            "velocityX": 3.5892134398439306,
+            "velocityY": -1.195066990431097,
+            "timestamp": 0.6165737848402725
         },
         {
-            "x": 6.980650518345068,
-            "y": 1.0194616665370815,
-            "heading": 2.8221688917599996,
-            "angularVelocity": 0.02877757616608584,
-            "velocityX": 3.583338790892565,
-            "velocityY": -1.1863380784636202,
-            "timestamp": 0.6936453728893881
+            "x": 6.980707555601848,
+            "y": 1.0195121848796085,
+            "heading": -0.31890263703438493,
+            "angularVelocity": 0.03021756576905414,
+            "velocityX": 3.582884365320709,
+            "velocityY": -1.186367892113736,
+            "timestamp": 0.6936455079453065
         },
         {
-            "x": 7.214215740363564,
-            "y": 0.9418010792136509,
-            "heading": 2.905688153737692,
-            "angularVelocity": 1.083656558780357,
-            "velocityX": 3.030492352901014,
-            "velocityY": -1.0076406665835769,
-            "timestamp": 0.770717080988209
+            "x": 7.214256878757648,
+            "y": 0.9418349206236948,
+            "heading": -0.2350677354758115,
+            "angularVelocity": 1.0877517483853638,
+            "velocityX": 3.030285476263674,
+            "velocityY": -1.0078568523770295,
+            "timestamp": 0.7707172310503406
         },
         {
-            "x": 7.400786993851664,
-            "y": 0.8797765728247763,
-            "heading": 2.990770900783412,
-            "angularVelocity": 1.1039426677377822,
-            "velocityX": 2.420748911505631,
-            "velocityY": -0.8047636145464304,
-            "timestamp": 0.8477887890870299
+            "x": 7.4008131329346885,
+            "y": 0.8797967164596704,
+            "heading": -0.14969694055158067,
+            "angularVelocity": 1.1076798530621503,
+            "velocityX": 2.420553825205123,
+            "velocityY": -0.8049411854913137,
+            "timestamp": 0.8477889541553747
         },
         {
-            "x": 7.540501987432905,
-            "y": 0.8333423582814848,
-            "heading": 3.0622172150737166,
-            "angularVelocity": 0.9270109103939473,
-            "velocityX": 1.8127922298296504,
-            "velocityY": -0.6024806727230407,
-            "timestamp": 0.9248604971858508
+            "x": 7.540515620024687,
+            "y": 0.8333523210527743,
+            "heading": -0.07802156861665294,
+            "angularVelocity": 0.9299827361748209,
+            "velocityX": 1.8126296060568263,
+            "velocityY": -0.6026126513818976,
+            "timestamp": 0.9248606772604088
         },
         {
-            "x": 7.633531379147319,
-            "y": 0.802434096261962,
-            "heading": 3.113237732512233,
-            "angularVelocity": 0.661987630702283,
-            "velocityX": 1.2070498242381353,
-            "velocityY": -0.4010325290817799,
-            "timestamp": 1.0019322052846718
+            "x": 7.633536076900545,
+            "y": 0.8024373784171116,
+            "heading": -0.026843164489729465,
+            "angularVelocity": 0.6640360701054664,
+            "velocityX": 1.2069336603398442,
+            "velocityY": -0.40111913150730505,
+            "timestamp": 1.0019324003654428
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 0.3472385411966328,
-            "velocityX": 0.6029270921710836,
-            "velocityY": -0.20025631509518968,
-            "timestamp": 1.0790039133834926
+            "heading": 1.4987800218197315e-33,
+            "angularVelocity": 0.3482881063025846,
+            "velocityX": 0.6028660217721296,
+            "velocityY": -0.20029886182865572,
+            "timestamp": 1.0790041234704768
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 1.7638718006253152e-30,
-            "velocityX": 1.1888064162036181e-31,
-            "velocityY": -2.2168268180884255e-31,
-            "timestamp": 1.1560756214823134
+            "heading": 0,
+            "angularVelocity": -8.136191152722898e-32,
+            "velocityX": 4.4028618628466005e-33,
+            "velocityY": 0,
+            "timestamp": 1.1560758465755108
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.traj
index 89c7b3cf..10168382 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.traj	
@@ -3,146 +3,146 @@
         {
             "x": 5.176,
             "y": 1.62,
-            "heading": 2.5,
+            "heading": -0.64,
             "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
+            "velocityX": -3.378617665126574e-35,
+            "velocityY": 1.1970750708579272e-32,
             "timestamp": 0
         },
         {
-            "x": 5.222452071774302,
-            "y": 1.6045080698057588,
-            "heading": 2.5267186887396895,
-            "angularVelocity": 0.3466731100007665,
-            "velocityX": 0.6027123690413302,
-            "velocityY": -0.20100670630495998,
-            "timestamp": 0.0770717080988209
+            "x": 5.2224556017723405,
+            "y": 1.6045081405552968,
+            "heading": -0.6133597294207822,
+            "angularVelocity": 0.3456555725750699,
+            "velocityX": 0.6027580531582283,
+            "velocityY": -0.20100574919793807,
+            "timestamp": 0.07707172310503405
         },
         {
-            "x": 5.315452291782514,
-            "y": 1.5735030960792171,
-            "heading": 2.577725213280899,
-            "angularVelocity": 0.6618060738424182,
-            "velocityX": 1.2066713233988244,
-            "velocityY": -0.40228735668849247,
-            "timestamp": 0.1541434161976418
+            "x": 5.3154629271275695,
+            "y": 1.5735046785143454,
+            "heading": -0.5625081883046615,
+            "angularVelocity": 0.6597950463209403,
+            "velocityX": 1.2067632798150456,
+            "velocityY": -0.4022676643507709,
+            "timestamp": 0.1541434462100681
         },
         {
-            "x": 5.4551316607074885,
-            "y": 1.5269515495858468,
-            "heading": 2.6491768389859613,
-            "angularVelocity": 0.9270798256274155,
-            "velocityX": 1.8123300024164322,
-            "velocityY": -0.6040030465353413,
-            "timestamp": 0.23121512429646268
+            "x": 5.455153059548346,
+            "y": 1.5269582787185583,
+            "heading": -0.4912835813545399,
+            "angularVelocity": 0.9241340932919854,
+            "velocityX": 1.812469305122525,
+            "velocityY": -0.6039361509065161,
+            "timestamp": 0.23121516931510214
         },
         {
-            "x": 5.641664886749133,
-            "y": 1.4648000786644693,
-            "heading": 2.734238612850838,
-            "angularVelocity": 1.1036705421892394,
-            "velocityX": 2.420255508058452,
-            "velocityY": -0.8064109704392095,
-            "timestamp": 0.3082868323952836
+            "x": 5.641700996428857,
+            "y": 1.4648196222358638,
+            "heading": -0.40650985692768693,
+            "angularVelocity": 1.0999329067980297,
+            "velocityX": 2.4204459088877828,
+            "velocityY": -0.8062445470177368,
+            "timestamp": 0.3082868924201362
         },
         {
-            "x": 5.8751883475378746,
-            "y": 1.3870009970649684,
-            "heading": 2.8177772109144708,
-            "angularVelocity": 1.0839074431374935,
-            "velocityX": 3.02995050387775,
-            "velocityY": -1.0094376200894257,
-            "timestamp": 0.3853585404941045
+            "x": 5.875244676488674,
+            "y": 1.3870501062573488,
+            "heading": -0.3232929818469195,
+            "angularVelocity": 1.0797328997998226,
+            "velocityX": 3.0302122575038553,
+            "velocityY": -1.0090538117660863,
+            "timestamp": 0.38535861552517026
         },
         {
-            "x": 6.15120111527276,
-            "y": 1.295041169269926,
-            "heading": 2.8199508970154183,
-            "angularVelocity": 0.028203424506439225,
-            "velocityX": 3.5812462775702794,
-            "velocityY": -1.193172307497486,
-            "timestamp": 0.4624302485929254
+            "x": 6.151314773692177,
+            "y": 1.2951594028189055,
+            "heading": -0.3212316148216143,
+            "angularVelocity": 0.026746087180327376,
+            "velocityX": 3.581989425969788,
+            "velocityY": -1.1922751916836483,
+            "timestamp": 0.46243033863020433
         },
         {
-            "x": 6.427838786740925,
-            "y": 1.202967940667996,
-            "heading": 2.8199509259126705,
-            "angularVelocity": 3.7493981751466555e-7,
-            "velocityX": 3.5893543596239312,
-            "velocityY": -1.1946436750029477,
-            "timestamp": 0.5395019566917463
+            "x": 6.427941619479976,
+            "y": 1.2030534747397732,
+            "heading": -0.3212315858589365,
+            "angularVelocity": 3.7578863628089786e-7,
+            "velocityX": 3.5892131983452513,
+            "velocityY": -1.1950677157381002,
+            "timestamp": 0.5395020617352384
         },
         {
-            "x": 6.7044764770342145,
-            "y": 1.110894768626946,
-            "heading": 2.8199509548099355,
-            "angularVelocity": 3.749399855143739e-7,
-            "velocityX": 3.5893546038785895,
-            "velocityY": -1.1946429411295039,
-            "timestamp": 0.6165736647905672
+            "x": 6.704568483880494,
+            "y": 1.1109476025613012,
+            "heading": -0.3212315568962456,
+            "angularVelocity": 3.75788807477423e-7,
+            "velocityX": 3.5892134398439306,
+            "velocityY": -1.195066990431097,
+            "timestamp": 0.6165737848402725
         },
         {
-            "x": 6.980650518345068,
-            "y": 1.0194616665370815,
-            "heading": 2.8221688917599996,
-            "angularVelocity": 0.02877757616608584,
-            "velocityX": 3.583338790892565,
-            "velocityY": -1.1863380784636202,
-            "timestamp": 0.6936453728893881
+            "x": 6.980707555601848,
+            "y": 1.0195121848796085,
+            "heading": -0.31890263703438493,
+            "angularVelocity": 0.03021756576905414,
+            "velocityX": 3.582884365320709,
+            "velocityY": -1.186367892113736,
+            "timestamp": 0.6936455079453065
         },
         {
-            "x": 7.214215740363564,
-            "y": 0.9418010792136509,
-            "heading": 2.905688153737692,
-            "angularVelocity": 1.083656558780357,
-            "velocityX": 3.030492352901014,
-            "velocityY": -1.0076406665835769,
-            "timestamp": 0.770717080988209
+            "x": 7.214256878757648,
+            "y": 0.9418349206236948,
+            "heading": -0.2350677354758115,
+            "angularVelocity": 1.0877517483853638,
+            "velocityX": 3.030285476263674,
+            "velocityY": -1.0078568523770295,
+            "timestamp": 0.7707172310503406
         },
         {
-            "x": 7.400786993851664,
-            "y": 0.8797765728247763,
-            "heading": 2.990770900783412,
-            "angularVelocity": 1.1039426677377822,
-            "velocityX": 2.420748911505631,
-            "velocityY": -0.8047636145464304,
-            "timestamp": 0.8477887890870299
+            "x": 7.4008131329346885,
+            "y": 0.8797967164596704,
+            "heading": -0.14969694055158067,
+            "angularVelocity": 1.1076798530621503,
+            "velocityX": 2.420553825205123,
+            "velocityY": -0.8049411854913137,
+            "timestamp": 0.8477889541553747
         },
         {
-            "x": 7.540501987432905,
-            "y": 0.8333423582814848,
-            "heading": 3.0622172150737166,
-            "angularVelocity": 0.9270109103939473,
-            "velocityX": 1.8127922298296504,
-            "velocityY": -0.6024806727230407,
-            "timestamp": 0.9248604971858508
+            "x": 7.540515620024687,
+            "y": 0.8333523210527743,
+            "heading": -0.07802156861665294,
+            "angularVelocity": 0.9299827361748209,
+            "velocityX": 1.8126296060568263,
+            "velocityY": -0.6026126513818976,
+            "timestamp": 0.9248606772604088
         },
         {
-            "x": 7.633531379147319,
-            "y": 0.802434096261962,
-            "heading": 3.113237732512233,
-            "angularVelocity": 0.661987630702283,
-            "velocityX": 1.2070498242381353,
-            "velocityY": -0.4010325290817799,
-            "timestamp": 1.0019322052846718
+            "x": 7.633536076900545,
+            "y": 0.8024373784171116,
+            "heading": -0.026843164489729465,
+            "angularVelocity": 0.6640360701054664,
+            "velocityX": 1.2069336603398442,
+            "velocityY": -0.40111913150730505,
+            "timestamp": 1.0019324003654428
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 0.3472385411966328,
-            "velocityX": 0.6029270921710836,
-            "velocityY": -0.20025631509518968,
-            "timestamp": 1.0790039133834926
+            "heading": 1.4987800218197315e-33,
+            "angularVelocity": 0.3482881063025846,
+            "velocityX": 0.6028660217721296,
+            "velocityY": -0.20029886182865572,
+            "timestamp": 1.0790041234704768
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 1.7638718006253152e-30,
-            "velocityX": 1.1888064162036181e-31,
-            "velocityY": -2.2168268180884255e-31,
-            "timestamp": 1.1560756214823134
+            "heading": 0,
+            "angularVelocity": -8.136191152722898e-32,
+            "velocityX": 4.4028618628466005e-33,
+            "velocityY": 0,
+            "timestamp": 1.1560758465755108
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W1.1.traj
new file mode 100644
index 00000000..90e7d73f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W1.1.traj	
@@ -0,0 +1,247 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 0,
+            "velocityX": -4.2485019622555225e-35,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.123544064624381,
+            "y": 1.640479835564366,
+            "heading": -0.5261105113915723,
+            "angularVelocity": 1.3044458865146935,
+            "velocityX": -0.6008098724480858,
+            "velocityY": 0.23456806756139406,
+            "timestamp": 0.08730871076049408
+        },
+        {
+            "x": 5.016418569113309,
+            "y": 1.6815339921073649,
+            "heading": -0.31680443246750034,
+            "angularVelocity": 2.397310384049101,
+            "velocityX": -1.2269737415426907,
+            "velocityY": 0.4702183342922002,
+            "timestamp": 0.17461742152098816
+        },
+        {
+            "x": 4.8522086939147036,
+            "y": 1.7459427773647251,
+            "heading": -0.051891218250848345,
+            "angularVelocity": 3.0342128741697256,
+            "velocityX": -1.8807960141464901,
+            "velocityY": 0.7377131639710823,
+            "timestamp": 0.26192613228148226
+        },
+        {
+            "x": 4.632595451769563,
+            "y": 1.839668047804236,
+            "heading": 0.17554509517769362,
+            "angularVelocity": 2.6049670353333583,
+            "velocityX": -2.515364620920639,
+            "velocityY": 1.073492777789593,
+            "timestamp": 0.34923484304197633
+        },
+        {
+            "x": 4.3704901470738005,
+            "y": 1.9627849470607086,
+            "heading": 0.27538033292006053,
+            "angularVelocity": 1.1434739658020574,
+            "velocityX": -3.002052171114658,
+            "velocityY": 1.4101330575617794,
+            "timestamp": 0.4365435538024704
+        },
+        {
+            "x": 4.075718453010027,
+            "y": 2.1117712847634245,
+            "heading": 0.2753804447713452,
+            "angularVelocity": 0.0000012811010915301473,
+            "velocityX": -3.3762002839830365,
+            "velocityY": 1.7064315393617047,
+            "timestamp": 0.5238522645629645
+        },
+        {
+            "x": 3.780946849777969,
+            "y": 2.260757802178556,
+            "heading": 0.2753805566219007,
+            "angularVelocity": 0.0000012810927397041138,
+            "velocityX": -3.3761992436319233,
+            "velocityY": 1.7064335977177825,
+            "timestamp": 0.6111609753234586
+        },
+        {
+            "x": 3.4861752863833098,
+            "y": 2.409744398412427,
+            "heading": 0.27538066847245973,
+            "angularVelocity": 0.0000012810927810381003,
+            "velocityX": -3.3761987873498507,
+            "velocityY": 1.7064345004769594,
+            "timestamp": 0.6984696860839527
+        },
+        {
+            "x": 3.191403823869153,
+            "y": 2.5587311942392557,
+            "heading": 0.2753807803230405,
+            "angularVelocity": 0.000001281093028963251,
+            "velocityX": -3.3761976319038296,
+            "velocityY": 1.7064367865370367,
+            "timestamp": 0.7857783968444467
+        },
+        {
+            "x": 2.907963654667952,
+            "y": 2.72828634801538,
+            "heading": 0.27538089480545036,
+            "angularVelocity": 0.0000013112369766496227,
+            "velocityX": -3.246413407463273,
+            "velocityY": 1.9420187550501031,
+            "timestamp": 0.8730871076049408
+        },
+        {
+            "x": 2.662249448997808,
+            "y": 2.9489938499602837,
+            "heading": 0.27538103946019826,
+            "angularVelocity": 0.000001656819194794751,
+            "velocityX": -2.81431490088302,
+            "velocityY": 2.527897846875211,
+            "timestamp": 0.9603958183654349
+        },
+        {
+            "x": 2.463361015566044,
+            "y": 3.212680537293485,
+            "heading": 0.27538116270778873,
+            "angularVelocity": 0.0000014116299433562534,
+            "velocityX": -2.277990726232991,
+            "velocityY": 3.0201647125055904,
+            "timestamp": 1.047704529125929
+        },
+        {
+            "x": 2.3186638703555946,
+            "y": 3.509581240155329,
+            "heading": 0.2753811333053821,
+            "angularVelocity": -3.3676372418890626e-7,
+            "velocityX": -1.657304797540575,
+            "velocityY": 3.4005851223287915,
+            "timestamp": 1.1350132398864232
+        },
+        {
+            "x": 2.233516432992889,
+            "y": 3.8287006658490608,
+            "heading": 0.2753811088610493,
+            "angularVelocity": -2.799758765995743e-7,
+            "velocityX": -0.9752456154836949,
+            "velocityY": 3.655069728026828,
+            "timestamp": 1.2223219506469174
+        },
+        {
+            "x": 2.1898375161467403,
+            "y": 4.15608338798651,
+            "heading": 0.27538108549661844,
+            "angularVelocity": -2.6760709935894875e-7,
+            "velocityX": -0.5002813174732252,
+            "velocityY": 3.749714310127982,
+            "timestamp": 1.3096306614074116
+        },
+        {
+            "x": 2.1796859113101297,
+            "y": 4.486211000706444,
+            "heading": 0.2753810837562719,
+            "angularVelocity": -1.9933252293602923e-8,
+            "velocityX": -0.11627253166592438,
+            "velocityY": 3.78115321878412,
+            "timestamp": 1.3969393721679058
+        },
+        {
+            "x": 2.197179274626327,
+            "y": 4.816031069152981,
+            "heading": 0.2753810893961792,
+            "angularVelocity": 6.459730400705482e-8,
+            "velocityX": 0.20036217650934227,
+            "velocityY": 3.7776307263464504,
+            "timestamp": 1.4842480829284
+        },
+        {
+            "x": 2.214672942788018,
+            "y": 5.1458511214306375,
+            "heading": 0.27538109503608554,
+            "angularVelocity": 6.459729260879602e-8,
+            "velocityX": 0.20036566809100828,
+            "velocityY": 3.777630541154376,
+            "timestamp": 1.5715567936888941
+        },
+        {
+            "x": 2.2321666974024836,
+            "y": 5.475671169122818,
+            "heading": 0.2753811006759881,
+            "angularVelocity": 6.459724966098765e-8,
+            "velocityX": 0.20036665828744704,
+            "velocityY": 3.7776304886341237,
+            "timestamp": 1.6588655044493883
+        },
+        {
+            "x": 2.2496605217353887,
+            "y": 5.805491213117091,
+            "heading": 0.2753811063158993,
+            "angularVelocity": 6.459734855412882e-8,
+            "velocityX": 0.20036745681532722,
+            "velocityY": 3.7776304462797263,
+            "timestamp": 1.7461742152098825
+        },
+        {
+            "x": 2.2664262161021513,
+            "y": 6.122693971906954,
+            "heading": 0.30917552472065174,
+            "angularVelocity": 0.3870681185232194,
+            "velocityX": 0.19202773950877436,
+            "velocityY": 3.633116971112041,
+            "timestamp": 1.8334829259703767
+        },
+        {
+            "x": 2.279851444819284,
+            "y": 6.376429629998848,
+            "heading": 0.34011357351362637,
+            "angularVelocity": 0.35435237244361656,
+            "velocityX": 0.15376734578020115,
+            "velocityY": 2.9061894956614607,
+            "timestamp": 1.9207916367308708
+        },
+        {
+            "x": 2.289924296428913,
+            "y": 6.566719611746641,
+            "heading": 0.3645867540299274,
+            "angularVelocity": 0.2803062867740199,
+            "velocityX": 0.11537052284807026,
+            "velocityY": 2.179507406423607,
+            "timestamp": 2.008100347491365
+        },
+        {
+            "x": 2.296641079194263,
+            "y": 6.6935744437684805,
+            "heading": 0.3814115840440414,
+            "angularVelocity": 0.192705056203017,
+            "velocityX": 0.07693141619941274,
+            "velocityY": 1.4529458849739352,
+            "timestamp": 2.095409058251859
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 0.09836837448577633,
+            "velocityX": 0.03847177190545204,
+            "velocityY": 0.7264516412973858,
+            "timestamp": 2.182717769012353
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 2.6936232860428826e-33,
+            "velocityX": 0,
+            "velocityY": 5.62663175256514e-35,
+            "timestamp": 2.2700264797728473
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W1.traj
new file mode 100644
index 00000000..90e7d73f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W1.traj	
@@ -0,0 +1,247 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 0,
+            "velocityX": -4.2485019622555225e-35,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.123544064624381,
+            "y": 1.640479835564366,
+            "heading": -0.5261105113915723,
+            "angularVelocity": 1.3044458865146935,
+            "velocityX": -0.6008098724480858,
+            "velocityY": 0.23456806756139406,
+            "timestamp": 0.08730871076049408
+        },
+        {
+            "x": 5.016418569113309,
+            "y": 1.6815339921073649,
+            "heading": -0.31680443246750034,
+            "angularVelocity": 2.397310384049101,
+            "velocityX": -1.2269737415426907,
+            "velocityY": 0.4702183342922002,
+            "timestamp": 0.17461742152098816
+        },
+        {
+            "x": 4.8522086939147036,
+            "y": 1.7459427773647251,
+            "heading": -0.051891218250848345,
+            "angularVelocity": 3.0342128741697256,
+            "velocityX": -1.8807960141464901,
+            "velocityY": 0.7377131639710823,
+            "timestamp": 0.26192613228148226
+        },
+        {
+            "x": 4.632595451769563,
+            "y": 1.839668047804236,
+            "heading": 0.17554509517769362,
+            "angularVelocity": 2.6049670353333583,
+            "velocityX": -2.515364620920639,
+            "velocityY": 1.073492777789593,
+            "timestamp": 0.34923484304197633
+        },
+        {
+            "x": 4.3704901470738005,
+            "y": 1.9627849470607086,
+            "heading": 0.27538033292006053,
+            "angularVelocity": 1.1434739658020574,
+            "velocityX": -3.002052171114658,
+            "velocityY": 1.4101330575617794,
+            "timestamp": 0.4365435538024704
+        },
+        {
+            "x": 4.075718453010027,
+            "y": 2.1117712847634245,
+            "heading": 0.2753804447713452,
+            "angularVelocity": 0.0000012811010915301473,
+            "velocityX": -3.3762002839830365,
+            "velocityY": 1.7064315393617047,
+            "timestamp": 0.5238522645629645
+        },
+        {
+            "x": 3.780946849777969,
+            "y": 2.260757802178556,
+            "heading": 0.2753805566219007,
+            "angularVelocity": 0.0000012810927397041138,
+            "velocityX": -3.3761992436319233,
+            "velocityY": 1.7064335977177825,
+            "timestamp": 0.6111609753234586
+        },
+        {
+            "x": 3.4861752863833098,
+            "y": 2.409744398412427,
+            "heading": 0.27538066847245973,
+            "angularVelocity": 0.0000012810927810381003,
+            "velocityX": -3.3761987873498507,
+            "velocityY": 1.7064345004769594,
+            "timestamp": 0.6984696860839527
+        },
+        {
+            "x": 3.191403823869153,
+            "y": 2.5587311942392557,
+            "heading": 0.2753807803230405,
+            "angularVelocity": 0.000001281093028963251,
+            "velocityX": -3.3761976319038296,
+            "velocityY": 1.7064367865370367,
+            "timestamp": 0.7857783968444467
+        },
+        {
+            "x": 2.907963654667952,
+            "y": 2.72828634801538,
+            "heading": 0.27538089480545036,
+            "angularVelocity": 0.0000013112369766496227,
+            "velocityX": -3.246413407463273,
+            "velocityY": 1.9420187550501031,
+            "timestamp": 0.8730871076049408
+        },
+        {
+            "x": 2.662249448997808,
+            "y": 2.9489938499602837,
+            "heading": 0.27538103946019826,
+            "angularVelocity": 0.000001656819194794751,
+            "velocityX": -2.81431490088302,
+            "velocityY": 2.527897846875211,
+            "timestamp": 0.9603958183654349
+        },
+        {
+            "x": 2.463361015566044,
+            "y": 3.212680537293485,
+            "heading": 0.27538116270778873,
+            "angularVelocity": 0.0000014116299433562534,
+            "velocityX": -2.277990726232991,
+            "velocityY": 3.0201647125055904,
+            "timestamp": 1.047704529125929
+        },
+        {
+            "x": 2.3186638703555946,
+            "y": 3.509581240155329,
+            "heading": 0.2753811333053821,
+            "angularVelocity": -3.3676372418890626e-7,
+            "velocityX": -1.657304797540575,
+            "velocityY": 3.4005851223287915,
+            "timestamp": 1.1350132398864232
+        },
+        {
+            "x": 2.233516432992889,
+            "y": 3.8287006658490608,
+            "heading": 0.2753811088610493,
+            "angularVelocity": -2.799758765995743e-7,
+            "velocityX": -0.9752456154836949,
+            "velocityY": 3.655069728026828,
+            "timestamp": 1.2223219506469174
+        },
+        {
+            "x": 2.1898375161467403,
+            "y": 4.15608338798651,
+            "heading": 0.27538108549661844,
+            "angularVelocity": -2.6760709935894875e-7,
+            "velocityX": -0.5002813174732252,
+            "velocityY": 3.749714310127982,
+            "timestamp": 1.3096306614074116
+        },
+        {
+            "x": 2.1796859113101297,
+            "y": 4.486211000706444,
+            "heading": 0.2753810837562719,
+            "angularVelocity": -1.9933252293602923e-8,
+            "velocityX": -0.11627253166592438,
+            "velocityY": 3.78115321878412,
+            "timestamp": 1.3969393721679058
+        },
+        {
+            "x": 2.197179274626327,
+            "y": 4.816031069152981,
+            "heading": 0.2753810893961792,
+            "angularVelocity": 6.459730400705482e-8,
+            "velocityX": 0.20036217650934227,
+            "velocityY": 3.7776307263464504,
+            "timestamp": 1.4842480829284
+        },
+        {
+            "x": 2.214672942788018,
+            "y": 5.1458511214306375,
+            "heading": 0.27538109503608554,
+            "angularVelocity": 6.459729260879602e-8,
+            "velocityX": 0.20036566809100828,
+            "velocityY": 3.777630541154376,
+            "timestamp": 1.5715567936888941
+        },
+        {
+            "x": 2.2321666974024836,
+            "y": 5.475671169122818,
+            "heading": 0.2753811006759881,
+            "angularVelocity": 6.459724966098765e-8,
+            "velocityX": 0.20036665828744704,
+            "velocityY": 3.7776304886341237,
+            "timestamp": 1.6588655044493883
+        },
+        {
+            "x": 2.2496605217353887,
+            "y": 5.805491213117091,
+            "heading": 0.2753811063158993,
+            "angularVelocity": 6.459734855412882e-8,
+            "velocityX": 0.20036745681532722,
+            "velocityY": 3.7776304462797263,
+            "timestamp": 1.7461742152098825
+        },
+        {
+            "x": 2.2664262161021513,
+            "y": 6.122693971906954,
+            "heading": 0.30917552472065174,
+            "angularVelocity": 0.3870681185232194,
+            "velocityX": 0.19202773950877436,
+            "velocityY": 3.633116971112041,
+            "timestamp": 1.8334829259703767
+        },
+        {
+            "x": 2.279851444819284,
+            "y": 6.376429629998848,
+            "heading": 0.34011357351362637,
+            "angularVelocity": 0.35435237244361656,
+            "velocityX": 0.15376734578020115,
+            "velocityY": 2.9061894956614607,
+            "timestamp": 1.9207916367308708
+        },
+        {
+            "x": 2.289924296428913,
+            "y": 6.566719611746641,
+            "heading": 0.3645867540299274,
+            "angularVelocity": 0.2803062867740199,
+            "velocityX": 0.11537052284807026,
+            "velocityY": 2.179507406423607,
+            "timestamp": 2.008100347491365
+        },
+        {
+            "x": 2.296641079194263,
+            "y": 6.6935744437684805,
+            "heading": 0.3814115840440414,
+            "angularVelocity": 0.192705056203017,
+            "velocityX": 0.07693141619941274,
+            "velocityY": 1.4529458849739352,
+            "timestamp": 2.095409058251859
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 0.09836837448577633,
+            "velocityX": 0.03847177190545204,
+            "velocityY": 0.7264516412973858,
+            "timestamp": 2.182717769012353
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 2.6936232860428826e-33,
+            "velocityX": 0,
+            "velocityY": 5.62663175256514e-35,
+            "timestamp": 2.2700264797728473
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W2.1.traj
new file mode 100644
index 00000000..97118e90
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W2.1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -2.256840111784074e-31,
+            "velocityX": -2.1308644711626517e-33,
+            "velocityY": -1.0217798620245829e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 5.1132615869041915,
+            "y": 1.6556053336408048,
+            "heading": -0.6461329738139044,
+            "angularVelocity": -0.06592692141286781,
+            "velocityX": -0.6744118848768044,
+            "velocityY": 0.3827425493802767,
+            "timestamp": 0.09302684976743651
+        },
+        {
+            "x": 4.987784771636742,
+            "y": 1.7268160147391884,
+            "heading": -0.6583988506620269,
+            "angularVelocity": -0.131853081973501,
+            "velocityX": -1.348823652323362,
+            "velocityY": 0.7654852472851404,
+            "timestamp": 0.18605369953487302
+        },
+        {
+            "x": 4.799569566809376,
+            "y": 1.8336320552005005,
+            "heading": -0.6767974581881981,
+            "angularVelocity": -0.19777738977689827,
+            "velocityX": -2.02323528419909,
+            "velocityY": 1.148228073167565,
+            "timestamp": 0.2790805493023095
+        },
+        {
+            "x": 4.54861599428641,
+            "y": 1.9760534609244509,
+            "heading": -0.7013284716868837,
+            "angularVelocity": -0.26369820713065356,
+            "velocityX": -2.697646681042515,
+            "velocityY": 1.5309709624694186,
+            "timestamp": 0.37210739906974605
+        },
+        {
+            "x": 4.242554383646119,
+            "y": 2.1497500028424636,
+            "heading": -0.7013284745367188,
+            "angularVelocity": -3.063454187141868e-8,
+            "velocityX": -3.29003520387323,
+            "velocityY": 1.8671656876724017,
+            "timestamp": 0.4651342488371826
+        },
+        {
+            "x": 3.9364928269084327,
+            "y": 2.3234466397396827,
+            "heading": -0.7013284773864732,
+            "angularVelocity": -3.0633677229161635e-8,
+            "velocityX": -3.290034624442601,
+            "velocityY": 1.8671667086594281,
+            "timestamp": 0.558161098604619
+        },
+        {
+            "x": 3.630431318940711,
+            "y": 2.497143362571822,
+            "heading": -0.7013284802362207,
+            "angularVelocity": -3.063360291364155e-8,
+            "velocityX": -3.2900341001857365,
+            "velocityY": 1.867167632424124,
+            "timestamp": 0.6511879483720555
+        },
+        {
+            "x": 3.3243698648507793,
+            "y": 2.6708401803390394,
+            "heading": -0.7013284830859674,
+            "angularVelocity": -3.063359264364299e-8,
+            "velocityX": -3.2900335210218676,
+            "velocityY": 1.8671686529367946,
+            "timestamp": 0.744214798139492
+        },
+        {
+            "x": 3.0183085209148324,
+            "y": 2.844537192202297,
+            "heading": -0.7013284859357423,
+            "angularVelocity": -3.063389607722874e-8,
+            "velocityX": -3.290032336912287,
+            "velocityY": 1.8671707393886097,
+            "timestamp": 0.8372416479069285
+        },
+        {
+            "x": 2.714360997870643,
+            "y": 3.021907148464231,
+            "heading": -0.7013284887932081,
+            "angularVelocity": -3.071657002046057e-8,
+            "velocityX": -3.2673096402172703,
+            "velocityY": 1.9066533662630898,
+            "timestamp": 0.9302684976743649
+        },
+        {
+            "x": 2.4529780195268613,
+            "y": 3.2575406255854364,
+            "heading": -0.7013284919399162,
+            "angularVelocity": -3.382580562568125e-8,
+            "velocityX": -2.8097584621776335,
+            "velocityY": 2.532962018065539,
+            "timestamp": 1.0232953474418014
+        },
+        {
+            "x": 2.2451503238556145,
+            "y": 3.5415333045009083,
+            "heading": -0.7013284961513817,
+            "angularVelocity": -4.527150531202637e-8,
+            "velocityX": -2.2340614155032408,
+            "velocityY": 3.0528033532839474,
+            "timestamp": 1.1163221972092379
+        },
+        {
+            "x": 2.099615704405981,
+            "y": 3.861945316168419,
+            "heading": -0.7013284332934493,
+            "angularVelocity": 6.756966674200994e-7,
+            "velocityX": -1.564436717071087,
+            "velocityY": 3.444296055047848,
+            "timestamp": 1.2093490469766743
+        },
+        {
+            "x": 2.0224929199626733,
+            "y": 4.205305507968674,
+            "heading": -0.7013282219960775,
+            "angularVelocity": 0.0000022713589924444385,
+            "velocityX": -0.8290379028862299,
+            "velocityY": 3.6909794608614943,
+            "timestamp": 1.3023758967441108
+        },
+        {
+            "x": 2.0170244087221176,
+            "y": 4.5571780060792255,
+            "heading": -0.7013280555552984,
+            "angularVelocity": 0.0000017891692504432307,
+            "velocityX": -0.05878422470745493,
+            "velocityY": 3.782483218449498,
+            "timestamp": 1.3954027465115473
+        },
+        {
+            "x": 2.079587370397571,
+            "y": 4.887709205686539,
+            "heading": -0.661535006789308,
+            "angularVelocity": 0.4277587477752017,
+            "velocityX": 0.6725258549747536,
+            "velocityY": 3.5530731227987227,
+            "timestamp": 1.4884295962789837
+        },
+        {
+            "x": 2.143441139356313,
+            "y": 5.160361503306635,
+            "heading": -0.462273406431507,
+            "angularVelocity": 2.1419794484704924,
+            "velocityX": 0.6864014971846709,
+            "velocityY": 2.9308989641347156,
+            "timestamp": 1.5814564460464202
+        },
+        {
+            "x": 2.1958862911911914,
+            "y": 5.3618445595482385,
+            "heading": -0.25085794958853325,
+            "angularVelocity": 2.272628358065485,
+            "velocityX": 0.5637636012182422,
+            "velocityY": 2.165859176628076,
+            "timestamp": 1.6744832958138567
+        },
+        {
+            "x": 2.231834886718389,
+            "y": 5.494391410801975,
+            "heading": -0.08836645960984135,
+            "angularVelocity": 1.7467160328971074,
+            "velocityX": 0.38643247209883785,
+            "velocityY": 1.4248236029178551,
+            "timestamp": 1.7675101455812932
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -3.8128513304390615e-33,
+            "angularVelocity": 0.9499027413134385,
+            "velocityX": 0.19526742362042082,
+            "velocityY": 0.7052650859622128,
+            "timestamp": 1.8605369953487296
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 5.82518331797089e-32,
+            "velocityX": 0,
+            "velocityY": -4.1086986258215814e-33,
+            "timestamp": 1.953563845116166
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W2.traj
new file mode 100644
index 00000000..97118e90
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W2.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -2.256840111784074e-31,
+            "velocityX": -2.1308644711626517e-33,
+            "velocityY": -1.0217798620245829e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 5.1132615869041915,
+            "y": 1.6556053336408048,
+            "heading": -0.6461329738139044,
+            "angularVelocity": -0.06592692141286781,
+            "velocityX": -0.6744118848768044,
+            "velocityY": 0.3827425493802767,
+            "timestamp": 0.09302684976743651
+        },
+        {
+            "x": 4.987784771636742,
+            "y": 1.7268160147391884,
+            "heading": -0.6583988506620269,
+            "angularVelocity": -0.131853081973501,
+            "velocityX": -1.348823652323362,
+            "velocityY": 0.7654852472851404,
+            "timestamp": 0.18605369953487302
+        },
+        {
+            "x": 4.799569566809376,
+            "y": 1.8336320552005005,
+            "heading": -0.6767974581881981,
+            "angularVelocity": -0.19777738977689827,
+            "velocityX": -2.02323528419909,
+            "velocityY": 1.148228073167565,
+            "timestamp": 0.2790805493023095
+        },
+        {
+            "x": 4.54861599428641,
+            "y": 1.9760534609244509,
+            "heading": -0.7013284716868837,
+            "angularVelocity": -0.26369820713065356,
+            "velocityX": -2.697646681042515,
+            "velocityY": 1.5309709624694186,
+            "timestamp": 0.37210739906974605
+        },
+        {
+            "x": 4.242554383646119,
+            "y": 2.1497500028424636,
+            "heading": -0.7013284745367188,
+            "angularVelocity": -3.063454187141868e-8,
+            "velocityX": -3.29003520387323,
+            "velocityY": 1.8671656876724017,
+            "timestamp": 0.4651342488371826
+        },
+        {
+            "x": 3.9364928269084327,
+            "y": 2.3234466397396827,
+            "heading": -0.7013284773864732,
+            "angularVelocity": -3.0633677229161635e-8,
+            "velocityX": -3.290034624442601,
+            "velocityY": 1.8671667086594281,
+            "timestamp": 0.558161098604619
+        },
+        {
+            "x": 3.630431318940711,
+            "y": 2.497143362571822,
+            "heading": -0.7013284802362207,
+            "angularVelocity": -3.063360291364155e-8,
+            "velocityX": -3.2900341001857365,
+            "velocityY": 1.867167632424124,
+            "timestamp": 0.6511879483720555
+        },
+        {
+            "x": 3.3243698648507793,
+            "y": 2.6708401803390394,
+            "heading": -0.7013284830859674,
+            "angularVelocity": -3.063359264364299e-8,
+            "velocityX": -3.2900335210218676,
+            "velocityY": 1.8671686529367946,
+            "timestamp": 0.744214798139492
+        },
+        {
+            "x": 3.0183085209148324,
+            "y": 2.844537192202297,
+            "heading": -0.7013284859357423,
+            "angularVelocity": -3.063389607722874e-8,
+            "velocityX": -3.290032336912287,
+            "velocityY": 1.8671707393886097,
+            "timestamp": 0.8372416479069285
+        },
+        {
+            "x": 2.714360997870643,
+            "y": 3.021907148464231,
+            "heading": -0.7013284887932081,
+            "angularVelocity": -3.071657002046057e-8,
+            "velocityX": -3.2673096402172703,
+            "velocityY": 1.9066533662630898,
+            "timestamp": 0.9302684976743649
+        },
+        {
+            "x": 2.4529780195268613,
+            "y": 3.2575406255854364,
+            "heading": -0.7013284919399162,
+            "angularVelocity": -3.382580562568125e-8,
+            "velocityX": -2.8097584621776335,
+            "velocityY": 2.532962018065539,
+            "timestamp": 1.0232953474418014
+        },
+        {
+            "x": 2.2451503238556145,
+            "y": 3.5415333045009083,
+            "heading": -0.7013284961513817,
+            "angularVelocity": -4.527150531202637e-8,
+            "velocityX": -2.2340614155032408,
+            "velocityY": 3.0528033532839474,
+            "timestamp": 1.1163221972092379
+        },
+        {
+            "x": 2.099615704405981,
+            "y": 3.861945316168419,
+            "heading": -0.7013284332934493,
+            "angularVelocity": 6.756966674200994e-7,
+            "velocityX": -1.564436717071087,
+            "velocityY": 3.444296055047848,
+            "timestamp": 1.2093490469766743
+        },
+        {
+            "x": 2.0224929199626733,
+            "y": 4.205305507968674,
+            "heading": -0.7013282219960775,
+            "angularVelocity": 0.0000022713589924444385,
+            "velocityX": -0.8290379028862299,
+            "velocityY": 3.6909794608614943,
+            "timestamp": 1.3023758967441108
+        },
+        {
+            "x": 2.0170244087221176,
+            "y": 4.5571780060792255,
+            "heading": -0.7013280555552984,
+            "angularVelocity": 0.0000017891692504432307,
+            "velocityX": -0.05878422470745493,
+            "velocityY": 3.782483218449498,
+            "timestamp": 1.3954027465115473
+        },
+        {
+            "x": 2.079587370397571,
+            "y": 4.887709205686539,
+            "heading": -0.661535006789308,
+            "angularVelocity": 0.4277587477752017,
+            "velocityX": 0.6725258549747536,
+            "velocityY": 3.5530731227987227,
+            "timestamp": 1.4884295962789837
+        },
+        {
+            "x": 2.143441139356313,
+            "y": 5.160361503306635,
+            "heading": -0.462273406431507,
+            "angularVelocity": 2.1419794484704924,
+            "velocityX": 0.6864014971846709,
+            "velocityY": 2.9308989641347156,
+            "timestamp": 1.5814564460464202
+        },
+        {
+            "x": 2.1958862911911914,
+            "y": 5.3618445595482385,
+            "heading": -0.25085794958853325,
+            "angularVelocity": 2.272628358065485,
+            "velocityX": 0.5637636012182422,
+            "velocityY": 2.165859176628076,
+            "timestamp": 1.6744832958138567
+        },
+        {
+            "x": 2.231834886718389,
+            "y": 5.494391410801975,
+            "heading": -0.08836645960984135,
+            "angularVelocity": 1.7467160328971074,
+            "velocityX": 0.38643247209883785,
+            "velocityY": 1.4248236029178551,
+            "timestamp": 1.7675101455812932
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -3.8128513304390615e-33,
+            "angularVelocity": 0.9499027413134385,
+            "velocityX": 0.19526742362042082,
+            "velocityY": 0.7052650859622128,
+            "timestamp": 1.8605369953487296
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 5.82518331797089e-32,
+            "velocityX": 0,
+            "velocityY": -4.1086986258215814e-33,
+            "timestamp": 1.953563845116166
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.1.traj
index 5004d1ca..a684ff7a 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.1.traj	
@@ -3,524 +3,614 @@
         {
             "x": 0.71,
             "y": 4.36,
-            "heading": 2.1,
+            "heading": -1.04,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": 3.1166835341708484e-33,
+            "velocityY": 5.951430910881942e-24,
             "timestamp": 0
         },
         {
-            "x": 0.7240863087605185,
-            "y": 4.368337736173442,
-            "heading": 2.1163001020338092,
-            "angularVelocity": 0.3614478974621331,
-            "velocityX": 0.31235796401343735,
-            "velocityY": 0.1848857880296264,
-            "timestamp": 0.045096685160595056
-        },
-        {
-            "x": 0.7523050752299821,
-            "y": 4.38505134365204,
-            "heading": 2.148025504039376,
-            "angularVelocity": 0.7034974276399452,
-            "velocityX": 0.6257392615216195,
-            "velocityY": 0.3706172065436647,
-            "timestamp": 0.09019337032119011
-        },
-        {
-            "x": 0.7947129681201482,
-            "y": 4.410184705499472,
-            "heading": 2.1940580950016866,
-            "angularVelocity": 1.020753316976243,
-            "velocityX": 0.9403771638457716,
-            "velocityY": 0.5573217135124126,
-            "timestamp": 0.13529005548178516
-        },
-        {
-            "x": 0.8513807226870724,
-            "y": 4.44378606946023,
-            "heading": 2.252941060288208,
-            "angularVelocity": 1.3057049554048448,
-            "velocityX": 1.25658359068129,
-            "velocityY": 0.7450960938946717,
-            "timestamp": 0.18038674064238022
-        },
-        {
-            "x": 0.922394851892513,
-            "y": 4.485907452258627,
-            "heading": 2.32274249990232,
-            "angularVelocity": 1.5478175250695256,
-            "velocityX": 1.5747084060070078,
-            "velocityY": 0.9340239232306716,
-            "timestamp": 0.22548342580297528
-        },
-        {
-            "x": 1.0078549434585182,
-            "y": 4.536606503403333,
-            "heading": 2.4007952890388493,
-            "angularVelocity": 1.7307877254963087,
-            "velocityX": 1.8950415371256468,
-            "velocityY": 1.1242301061410762,
-            "timestamp": 0.2705801109635703
-        },
-        {
-            "x": 1.1078580204781514,
-            "y": 4.595947756684704,
-            "heading": 2.4830211624583747,
-            "angularVelocity": 1.823324111887792,
-            "velocityX": 2.2175261144695955,
-            "velocityY": 1.315867298672826,
-            "timestamp": 0.3156767961241654
-        },
-        {
-            "x": 1.2224032158634355,
-            "y": 4.663956921963176,
-            "heading": 2.561776708325237,
-            "angularVelocity": 1.7463710600103812,
-            "velocityX": 2.5399914645028585,
-            "velocityY": 1.5080745965314675,
-            "timestamp": 0.36077348128476044
-        },
-        {
-            "x": 1.3501227544876644,
-            "y": 4.740405284602277,
-            "heading": 2.616849964946217,
-            "angularVelocity": 1.2212262702869785,
-            "velocityX": 2.8321269771692363,
-            "velocityY": 1.695210243654476,
-            "timestamp": 0.4058701664453555
-        },
-        {
-            "x": 1.4890564849563437,
-            "y": 4.825393782343559,
-            "heading": 2.6364418869926514,
-            "angularVelocity": 0.43444261982150123,
-            "velocityX": 3.0807969582225083,
-            "velocityY": 1.884584142684248,
-            "timestamp": 0.45096685160595057
-        },
-        {
-            "x": 1.6359707555642846,
-            "y": 4.9121110990754175,
-            "heading": 2.6364419350884667,
-            "angularVelocity": 0.0000010665044506613552,
-            "velocityX": 3.257762074634985,
-            "velocityY": 1.9229199756711164,
-            "timestamp": 0.49606353676654563
-        },
-        {
-            "x": 1.7828851010176074,
-            "y": 4.99882828900635,
-            "heading": 2.63644198318385,
-            "angularVelocity": 0.000001066494868779716,
-            "velocityX": 3.257763734299801,
-            "velocityY": 1.9229171639139742,
-            "timestamp": 0.5411602219271406
-        },
-        {
-            "x": 1.929799446473537,
-            "y": 5.085545478932866,
-            "heading": 2.6364420312792327,
-            "angularVelocity": 0.0000010664948614258307,
-            "velocityX": 3.257763734357611,
-            "velocityY": 1.9229171638160338,
-            "timestamp": 0.5862569070877357
-        },
-        {
-            "x": 2.076713791929467,
-            "y": 5.172262668859381,
-            "heading": 2.636442079374616,
-            "angularVelocity": 0.000001066494855048045,
-            "velocityX": 3.257763734357614,
-            "velocityY": 1.922917163816031,
-            "timestamp": 0.6313535922483308
-        },
-        {
-            "x": 2.2236281373853966,
-            "y": 5.258979858785897,
-            "heading": 2.6364421274699983,
-            "angularVelocity": 0.000001066494848970006,
-            "velocityX": 3.257763734357614,
-            "velocityY": 1.9229171638160298,
-            "timestamp": 0.6764502774089258
-        },
-        {
-            "x": 2.3705424828413264,
-            "y": 5.345697048712413,
-            "heading": 2.6364421755653806,
-            "angularVelocity": 0.0000010664948432023413,
-            "velocityX": 3.257763734357615,
-            "velocityY": 1.9229171638160298,
-            "timestamp": 0.7215469625695209
-        },
-        {
-            "x": 2.5174568282972563,
-            "y": 5.432414238638929,
-            "heading": 2.6364422236607625,
-            "angularVelocity": 0.000001066494835980498,
-            "velocityX": 3.2577637343576162,
-            "velocityY": 1.9229171638160294,
-            "timestamp": 0.766643647730116
-        },
-        {
-            "x": 2.6643711737531857,
-            "y": 5.519131428565444,
-            "heading": 2.636442271756144,
-            "angularVelocity": 0.0000010664948304508311,
-            "velocityX": 3.2577637343576167,
-            "velocityY": 1.9229171638160292,
-            "timestamp": 0.811740332890711
-        },
-        {
-            "x": 2.8112855192091155,
-            "y": 5.60584861849196,
-            "heading": 2.6364423198515254,
-            "angularVelocity": 0.0000010664948245159241,
-            "velocityX": 3.257763734357617,
-            "velocityY": 1.922917163816029,
-            "timestamp": 0.8568370180513061
-        },
-        {
-            "x": 2.958199864665046,
-            "y": 5.692565808418475,
-            "heading": 2.6364423679469065,
-            "angularVelocity": 0.0000010664948181966642,
-            "velocityX": 3.2577637343576185,
-            "velocityY": 1.922917163816028,
-            "timestamp": 0.9019337032119011
-        },
-        {
-            "x": 3.1051142101209757,
-            "y": 5.779282998344991,
-            "heading": 2.636442416042287,
-            "angularVelocity": 0.0000010664948111950998,
-            "velocityX": 3.257763734357619,
-            "velocityY": 1.9229171638160278,
-            "timestamp": 0.9470303883724962
-        },
-        {
-            "x": 3.252028555576906,
-            "y": 5.8660001882715065,
-            "heading": 2.6364424641376676,
-            "angularVelocity": 0.0000010664948058295197,
-            "velocityX": 3.2577637343576273,
-            "velocityY": 1.922917163816015,
-            "timestamp": 0.9921270735330913
-        },
-        {
-            "x": 3.3989429010423815,
-            "y": 5.952717378181851,
-            "heading": 2.636442512233048,
-            "angularVelocity": 0.00000106649479979568,
-            "velocityX": 3.257763734569281,
-            "velocityY": 1.9229171634574358,
-            "timestamp": 1.0372237586936863
-        },
-        {
-            "x": 3.5458575205338394,
-            "y": 6.039434103841755,
-            "heading": 2.636442560328411,
-            "angularVelocity": 0.0000010664944172141213,
-            "velocityX": 3.2577698109800433,
-            "velocityY": 1.9229068688994477,
-            "timestamp": 1.0823204438542813
+            "x": 0.7172662637575132,
+            "y": 4.364294720467062,
+            "heading": -1.0315470131742317,
+            "angularVelocity": 0.26098153509522004,
+            "velocityX": 0.22434208273715575,
+            "velocityY": 0.13259724206381326,
+            "timestamp": 0.03238921413610362
+        },
+        {
+            "x": 0.7318144654254066,
+            "y": 4.3728967855708385,
+            "heading": -1.0149312072454106,
+            "angularVelocity": 0.5130042939294357,
+            "velocityX": 0.4491680967238058,
+            "velocityY": 0.2655842487449578,
+            "timestamp": 0.06477842827220724
+        },
+        {
+            "x": 0.7536624687467911,
+            "y": 4.385820612299006,
+            "heading": -0.9904958605622504,
+            "angularVelocity": 0.7544285137786853,
+            "velocityX": 0.6745456444104029,
+            "velocityY": 0.39901637235962667,
+            "timestamp": 0.09716764240831086
+        },
+        {
+            "x": 0.7828310955502448,
+            "y": 4.4030821899572965,
+            "heading": -0.9586530378385134,
+            "angularVelocity": 0.9831304516969581,
+            "velocityX": 0.9005660551343861,
+            "velocityY": 0.5329421574032179,
+            "timestamp": 0.12955685654441448
+        },
+        {
+            "x": 0.8193448564336872,
+            "y": 4.424698840047832,
+            "heading": -0.919902879162077,
+            "angularVelocity": 1.1963908266993857,
+            "velocityX": 1.1273432177146,
+            "velocityY": 0.6674027347406944,
+            "timestamp": 0.1619460706805181
+        },
+        {
+            "x": 0.8632324943492647,
+            "y": 4.450689040734006,
+            "heading": -0.8748582540701377,
+            "angularVelocity": 1.3907291761589549,
+            "velocityX": 1.3550078038681697,
+            "velocityY": 0.8024338156819394,
+            "timestamp": 0.19433528481662174
+        },
+        {
+            "x": 0.9145271224696989,
+            "y": 4.481072488182425,
+            "heading": -0.8242801160685745,
+            "angularVelocity": 1.5615734852049024,
+            "velocityX": 1.5836947418633758,
+            "velocityY": 0.9380730054376618,
+            "timestamp": 0.22672449895272537
+        },
+        {
+            "x": 0.9732655893831887,
+            "y": 4.515870568727518,
+            "heading": -0.7691375276578237,
+            "angularVelocity": 1.7024984977725797,
+            "velocityX": 1.8135193606940647,
+            "velocityY": 1.0743724870528544,
+            "timestamp": 0.25911371308882897
+        },
+        {
+            "x": 1.0394861119792222,
+            "y": 4.555106942812012,
+            "heading": -0.7107271690719036,
+            "angularVelocity": 1.8033891881560444,
+            "velocityX": 2.044523905944935,
+            "velocityY": 1.2114024724285466,
+            "timestamp": 0.2915029272249326
+        },
+        {
+            "x": 1.1132202582415271,
+            "y": 4.598805444215395,
+            "heading": -0.6509416110612904,
+            "angularVelocity": 1.8458477491731227,
+            "velocityX": 2.276503096137642,
+            "velocityY": 1.3491683132463759,
+            "timestamp": 0.3238921413610362
+        },
+        {
+            "x": 1.194458181637227,
+            "y": 4.646971254862031,
+            "heading": -0.5929406477835183,
+            "angularVelocity": 1.790749322723445,
+            "velocityX": 2.508178279791774,
+            "velocityY": 1.48709414326129,
+            "timestamp": 0.35628135549713985
+        },
+        {
+            "x": 1.2829200414273145,
+            "y": 4.699514481647374,
+            "heading": -0.5430956567390244,
+            "angularVelocity": 1.5389379574026985,
+            "velocityX": 2.7312135273909255,
+            "velocityY": 1.6222445708176134,
+            "timestamp": 0.3886705696332435
+        },
+        {
+            "x": 1.3771379621947135,
+            "y": 4.756486240363935,
+            "heading": -0.5116699283377882,
+            "angularVelocity": 0.9702528832339478,
+            "velocityX": 2.908928891311882,
+            "velocityY": 1.758973171660129,
+            "timestamp": 0.4210597837693471
+        },
+        {
+            "x": 1.4771406301349097,
+            "y": 4.8178582459332,
+            "heading": -0.4985254849792972,
+            "angularVelocity": 0.40582779511896505,
+            "velocityX": 3.087529926474048,
+            "velocityY": 1.894828485537579,
+            "timestamp": 0.45344899790545073
+        },
+        {
+            "x": 1.5826858211522454,
+            "y": 4.880091207833619,
+            "heading": -0.49852543793275184,
+            "angularVelocity": 0.000001452537415626245,
+            "velocityX": 3.2586524197166784,
+            "velocityY": 1.9214100607352578,
+            "timestamp": 0.48583821204155436
+        },
+        {
+            "x": 1.6882312828144117,
+            "y": 4.9423237107273525,
+            "heading": -0.4985253908851185,
+            "angularVelocity": 0.0000014525710071923091,
+            "velocityX": 3.2586607757338757,
+            "velocityY": 1.9213958891446032,
+            "timestamp": 0.5182274261776579
+        },
+        {
+            "x": 1.7937767445018398,
+            "y": 5.004556213578243,
+            "heading": -0.49852534383749064,
+            "angularVelocity": 0.0000014525708350435827,
+            "velocityX": 3.2586607765138327,
+            "velocityY": 1.921395887821837,
+            "timestamp": 0.5506166403137616
+        },
+        {
+            "x": 1.8993222061892705,
+            "y": 5.06678871642913,
+            "heading": -0.49852529678986307,
+            "angularVelocity": 0.0000014525708270038968,
+            "velocityX": 3.2586607765139086,
+            "velocityY": 1.9213958878217097,
+            "timestamp": 0.5830058544498652
+        },
+        {
+            "x": 2.0048676678767015,
+            "y": 5.129021219280016,
+            "heading": -0.4985252497422358,
+            "angularVelocity": 0.0000014525708183379938,
+            "velocityX": 3.258660776513909,
+            "velocityY": 1.9213958878217101,
+            "timestamp": 0.6153950685859688
+        },
+        {
+            "x": 2.1104131295641317,
+            "y": 5.191253722130902,
+            "heading": -0.49852520269460865,
+            "angularVelocity": 0.000001452570810915054,
+            "velocityX": 3.2586607765139104,
+            "velocityY": 1.9213958878217086,
+            "timestamp": 0.6477842827220724
+        },
+        {
+            "x": 2.2159585912515625,
+            "y": 5.253486224981789,
+            "heading": -0.4985251556469819,
+            "angularVelocity": 0.0000014525708028649706,
+            "velocityX": 3.2586607765139086,
+            "velocityY": 1.9213958878217123,
+            "timestamp": 0.6801734968581761
+        },
+        {
+            "x": 2.321504052938993,
+            "y": 5.315718727832675,
+            "heading": -0.4985251085993554,
+            "angularVelocity": 0.000001452570794356237,
+            "velocityX": 3.2586607765139117,
+            "velocityY": 1.9213958878217081,
+            "timestamp": 0.7125627109942797
+        },
+        {
+            "x": 2.427049514626424,
+            "y": 5.3779512306835615,
+            "heading": -0.49852506155172915,
+            "angularVelocity": 0.0000014525707870212498,
+            "velocityX": 3.258660776513912,
+            "velocityY": 1.9213958878217077,
+            "timestamp": 0.7449519251303833
+        },
+        {
+            "x": 2.5325949763138547,
+            "y": 5.440183733534448,
+            "heading": -0.4985250145041031,
+            "angularVelocity": 0.000001452570779107594,
+            "velocityX": 3.258660776513913,
+            "velocityY": 1.9213958878217068,
+            "timestamp": 0.777341139266487
+        },
+        {
+            "x": 2.6381404380012854,
+            "y": 5.502416236385335,
+            "heading": -0.49852496745647734,
+            "angularVelocity": 0.0000014525707705186945,
+            "velocityX": 3.258660776513915,
+            "velocityY": 1.9213958878217055,
+            "timestamp": 0.8097303534025906
+        },
+        {
+            "x": 2.743685899688716,
+            "y": 5.564648739236221,
+            "heading": -0.4985249204088519,
+            "angularVelocity": 0.00000145257076360687,
+            "velocityX": 3.2586607765139153,
+            "velocityY": 1.9213958878217066,
+            "timestamp": 0.8421195675386942
+        },
+        {
+            "x": 2.849231361376147,
+            "y": 5.626881242087108,
+            "heading": -0.4985248733612266,
+            "angularVelocity": 0.0000014525707544682094,
+            "velocityX": 3.2586607765139153,
+            "velocityY": 1.9213958878217066,
+            "timestamp": 0.8745087816747978
+        },
+        {
+            "x": 2.954776823063578,
+            "y": 5.689113744937994,
+            "heading": -0.4985248263136016,
+            "angularVelocity": 0.0000014525707471414822,
+            "velocityX": 3.2586607765139166,
+            "velocityY": 1.921395887821705,
+            "timestamp": 0.9068979958109015
+        },
+        {
+            "x": 3.0603222847510096,
+            "y": 5.751346247788881,
+            "heading": -0.49852477926597694,
+            "angularVelocity": 0.0000014525707385370494,
+            "velocityX": 3.258660776513917,
+            "velocityY": 1.9213958878217057,
+            "timestamp": 0.9392872099470051
+        },
+        {
+            "x": 3.1658677464384404,
+            "y": 5.813578750639766,
+            "heading": -0.4985247322183525,
+            "angularVelocity": 0.0000014525707310188908,
+            "velocityX": 3.2586607765139175,
+            "velocityY": 1.9213958878217063,
+            "timestamp": 0.9716764240831087
+        },
+        {
+            "x": 3.271413208125871,
+            "y": 5.875811253490653,
+            "heading": -0.4985246851707283,
+            "angularVelocity": 0.0000014525707228586851,
+            "velocityX": 3.258660776513917,
+            "velocityY": 1.921395887821708,
+            "timestamp": 1.0040656382192124
+        },
+        {
+            "x": 3.376958669813326,
+            "y": 5.938043756341499,
+            "heading": -0.4985246381231044,
+            "angularVelocity": 0.000001452570714543944,
+            "velocityX": 3.258660776514645,
+            "velocityY": 1.921395887820474,
+            "timestamp": 1.0364548523553159
+        },
+        {
+            "x": 3.482504131751534,
+            "y": 6.000276258767071,
+            "heading": -0.49852459107548075,
+            "angularVelocity": 0.0000014525707064344823,
+            "velocityX": 3.258660784256519,
+            "velocityY": 1.9213958746903632,
+            "timestamp": 1.0688440664914194
+        },
+        {
+            "x": 3.588052280044538,
+            "y": 6.0625042049472775,
+            "heading": -0.4985245440280508,
+            "angularVelocity": 0.0000014525647243024202,
+            "velocityX": 3.258743724052004,
+            "velocityY": 1.921255203004168,
+            "timestamp": 1.101233280627523
         },
         {
             "x": 3.69740891456604,
             "y": 6.1177659034729,
-            "heading": 2.6364426084288106,
-            "angularVelocity": 0.0000010666061013072793,
-            "velocityX": 3.3605883335439555,
-            "velocityY": 1.736974665703161,
-            "timestamp": 1.1274171290148762
-        },
-        {
-            "x": 3.8471798789241873,
-            "y": 6.1780975155460816,
-            "heading": 2.636442653750165,
-            "angularVelocity": 0.0000010618213127102396,
-            "velocityX": 3.5089419861830207,
-            "velocityY": 1.4134924456482445,
-            "timestamp": 1.1700997851966806
-        },
-        {
-            "x": 4.000192250358795,
-            "y": 6.2296577661788755,
-            "heading": 2.6364426985309986,
-            "angularVelocity": 0.0000010491576146038598,
-            "velocityX": 3.584884004942437,
-            "velocityY": 1.2079906745535305,
-            "timestamp": 1.212782441378485
-        },
-        {
-            "x": 4.153204837360183,
-            "y": 6.281217377082217,
-            "heading": 2.6364427433118154,
-            "angularVelocity": 0.0000010491572166464704,
-            "velocityX": 3.5848890553961605,
-            "velocityY": 1.2079756865112976,
-            "timestamp": 1.2554650975602892
-        },
-        {
-            "x": 4.306217424370213,
-            "y": 6.3327769879599085,
-            "heading": 2.636442788092632,
-            "angularVelocity": 0.0000010491572158103502,
-            "velocityX": 3.5848890555986537,
-            "velocityY": 1.2079756859103623,
-            "timestamp": 1.2981477537420936
-        },
-        {
-            "x": 4.459230011380243,
-            "y": 6.384336598837598,
-            "heading": 2.6364428328734486,
-            "angularVelocity": 0.0000010491572148390834,
-            "velocityX": 3.584889055598662,
-            "velocityY": 1.2079756859103368,
-            "timestamp": 1.340830409923898
-        },
-        {
-            "x": 4.6122425983902735,
-            "y": 6.435896209715288,
-            "heading": 2.636442877654265,
-            "angularVelocity": 0.0000010491572125659184,
-            "velocityX": 3.584889055598663,
-            "velocityY": 1.2079756859103368,
-            "timestamp": 1.3835130661057022
-        },
-        {
-            "x": 4.765255185400304,
-            "y": 6.487455820592977,
-            "heading": 2.636442922435082,
-            "angularVelocity": 0.0000010491572116366236,
-            "velocityX": 3.584889055598663,
-            "velocityY": 1.2079756859103352,
-            "timestamp": 1.4261957222875066
-        },
-        {
-            "x": 4.918267772410334,
-            "y": 6.539015431470667,
-            "heading": 2.636442967215898,
-            "angularVelocity": 0.0000010491572113687884,
-            "velocityX": 3.584889055598663,
-            "velocityY": 1.2079756859103348,
-            "timestamp": 1.468878378469311
-        },
-        {
-            "x": 5.071280359420366,
-            "y": 6.590575042348357,
-            "heading": 2.6364430119967146,
-            "angularVelocity": 0.0000010491572106337029,
-            "velocityX": 3.584889055598664,
-            "velocityY": 1.2079756859103339,
-            "timestamp": 1.5115610346511152
-        },
-        {
-            "x": 5.224292946430397,
-            "y": 6.642134653226047,
-            "heading": 2.6364430567775314,
-            "angularVelocity": 0.0000010491572087267192,
-            "velocityX": 3.584889055598664,
-            "velocityY": 1.2079756859103334,
-            "timestamp": 1.5542436908329196
-        },
-        {
-            "x": 5.3773055334404285,
-            "y": 6.693694264103736,
-            "heading": 2.636443101558348,
-            "angularVelocity": 0.0000010491572072659829,
-            "velocityX": 3.5848890555986643,
-            "velocityY": 1.2079756859103326,
-            "timestamp": 1.596926347014724
-        },
-        {
-            "x": 5.53031812045046,
-            "y": 6.745253874981426,
-            "heading": 2.6364431463391638,
-            "angularVelocity": 0.0000010491572066940514,
-            "velocityX": 3.5848890555986643,
-            "velocityY": 1.2079756859103314,
-            "timestamp": 1.6396090031965282
-        },
-        {
-            "x": 5.683330707460491,
-            "y": 6.796813485859116,
-            "heading": 2.63644319111998,
-            "angularVelocity": 0.000001049157205644206,
-            "velocityX": 3.5848890555986648,
-            "velocityY": 1.2079756859103312,
-            "timestamp": 1.6822916593783326
-        },
-        {
-            "x": 5.836343294470522,
-            "y": 6.848373096736806,
-            "heading": 2.636443235900796,
-            "angularVelocity": 0.0000010491572052803454,
-            "velocityX": 3.5848890555986657,
-            "velocityY": 1.2079756859103297,
-            "timestamp": 1.724974315560137
-        },
-        {
-            "x": 5.989355881480554,
-            "y": 6.899932707614496,
-            "heading": 2.6364432806816125,
-            "angularVelocity": 0.0000010491572041084738,
-            "velocityX": 3.5848890555986657,
-            "velocityY": 1.2079756859103294,
-            "timestamp": 1.7676569717419413
-        },
-        {
-            "x": 6.142368468490585,
-            "y": 6.951492318492185,
-            "heading": 2.6364433254624284,
-            "angularVelocity": 0.0000010491572019949161,
-            "velocityX": 3.584889055598666,
-            "velocityY": 1.2079756859103283,
-            "timestamp": 1.8103396279237456
-        },
-        {
-            "x": 6.295381055500616,
-            "y": 7.0030519293698745,
-            "heading": 2.6364433702432444,
-            "angularVelocity": 0.0000010491572021090905,
-            "velocityX": 3.584889055598666,
-            "velocityY": 1.2079756859103277,
-            "timestamp": 1.85302228410555
-        },
-        {
-            "x": 6.448393642510648,
-            "y": 7.054611540247564,
-            "heading": 2.6364434150240608,
-            "angularVelocity": 0.0000010491572009028897,
-            "velocityX": 3.584889055598666,
-            "velocityY": 1.2079756859103268,
-            "timestamp": 1.8957049402873543
-        },
-        {
-            "x": 6.601406229520499,
-            "y": 7.106171151125787,
-            "heading": 2.6364434598048767,
-            "angularVelocity": 0.0000010491571991066208,
-            "velocityX": 3.58488905559446,
-            "velocityY": 1.2079756859228103,
-            "timestamp": 1.9383875964691586
-        },
-        {
-            "x": 6.75441881205228,
-            "y": 7.157730775293412,
-            "heading": 2.636443504585761,
-            "angularVelocity": 0.0000010491588061157563,
-            "velocityX": 3.5848889506789767,
-            "velocityY": 1.207975997276489,
-            "timestamp": 1.981070252650963
-        },
-        {
-            "x": 6.905487720036347,
-            "y": 7.20899283211694,
-            "heading": 2.6411872457126164,
-            "angularVelocity": 0.11113978255358876,
-            "velocityX": 3.539351143954066,
-            "velocityY": 1.2010043752942743,
-            "timestamp": 2.0237529088327673
-        },
-        {
-            "x": 7.045499588568849,
-            "y": 7.254368377570732,
-            "heading": 2.676148381759387,
-            "angularVelocity": 0.81909466687958,
-            "velocityX": 3.2802988627542176,
-            "velocityY": 1.0630909487103528,
-            "timestamp": 2.0664355650145714
-        },
-        {
-            "x": 7.172970608020284,
-            "y": 7.297222729151116,
-            "heading": 2.7400991643229964,
-            "angularVelocity": 1.4982849776549703,
-            "velocityX": 2.9864828212302403,
-            "velocityY": 1.0040226034164315,
-            "timestamp": 2.1091182211963755
-        },
-        {
-            "x": 7.286231301322222,
-            "y": 7.335261893158222,
-            "heading": 2.8124985936039937,
-            "angularVelocity": 1.6962259558687312,
-            "velocityX": 2.6535530689447047,
-            "velocityY": 0.8912089220755359,
-            "timestamp": 2.1518008773781796
-        },
-        {
-            "x": 7.3851039731372365,
-            "y": 7.3684303034498875,
-            "heading": 2.8845195362089706,
-            "angularVelocity": 1.6873584975173053,
-            "velocityX": 2.316460142355517,
-            "velocityY": 0.7770933971491014,
-            "timestamp": 2.1944835335599837
-        },
-        {
-            "x": 7.46964567597808,
-            "y": 7.396754020582906,
-            "heading": 2.951617256319432,
-            "angularVelocity": 1.5720136962578515,
-            "velocityX": 1.9807038831122041,
-            "velocityY": 0.6635884377105193,
-            "timestamp": 2.237166189741788
-        },
-        {
-            "x": 7.539938623640353,
-            "y": 7.420270719849242,
-            "heading": 3.0110324789568197,
-            "angularVelocity": 1.3920226141576573,
-            "velocityX": 1.6468737878651336,
-            "velocityY": 0.5509661621378036,
-            "timestamp": 2.279848845923592
-        },
-        {
-            "x": 7.596058846559568,
-            "y": 7.439019303920545,
-            "heading": 3.060856486484029,
-            "angularVelocity": 1.1673127210028025,
-            "velocityX": 1.3148249884022007,
-            "velocityY": 0.4392553263659099,
-            "timestamp": 2.322531502105396
-        },
-        {
-            "x": 7.638072702511279,
-            "y": 7.453036150115233,
-            "heading": 3.09965963024843,
-            "angularVelocity": 0.9091079898851933,
-            "velocityX": 0.9843308666816342,
-            "velocityY": 0.32839676460117173,
-            "timestamp": 2.3652141582872
-        },
-        {
-            "x": 7.666037142481598,
-            "y": 7.462353623790155,
-            "heading": 3.126330643718343,
-            "angularVelocity": 0.6248677063655441,
-            "velocityX": 0.6551710336677993,
-            "velocityY": 0.2182964817192845,
-            "timestamp": 2.4078968144690043
+            "heading": -0.4985244969178424,
+            "angularVelocity": 0.0000014545029788824699,
+            "velocityX": 3.3763287390046313,
+            "velocityY": 1.7061759601022248,
+            "timestamp": 1.1336224947636264
+        },
+        {
+            "x": 3.8477077097405896,
+            "y": 6.176720729441604,
+            "heading": -0.4985244519972541,
+            "angularVelocity": 0.0000010525498975792195,
+            "velocityX": 3.5217032391581933,
+            "velocityY": 1.3813909907718895,
+            "timestamp": 1.1763003653755966
+        },
+        {
+            "x": 4.000682189533232,
+            "y": 6.228336695265556,
+            "heading": -0.49852440748602983,
+            "angularVelocity": 0.000001042957945295385,
+            "velocityX": 3.58439813418752,
+            "velocityY": 1.2094316113670969,
+            "timestamp": 1.2189782359875667
+        },
+        {
+            "x": 4.153656816760498,
+            "y": 6.279952224134777,
+            "heading": -0.4985243629748153,
+            "angularVelocity": 0.00000104295771772215,
+            "velocityX": 3.5844015887793454,
+            "velocityY": 1.2094213729291292,
+            "timestamp": 1.2616561065995369
+        },
+        {
+            "x": 4.306631443993679,
+            "y": 6.331567752986466,
+            "heading": -0.4985243184636008,
+            "angularVelocity": 0.0000010429577168479619,
+            "velocityX": 3.584401588917953,
+            "velocityY": 1.209421372518333,
+            "timestamp": 1.304333977211507
+        },
+        {
+            "x": 4.459606071226857,
+            "y": 6.38318328183816,
+            "heading": -0.4985242739523863,
+            "angularVelocity": 0.000001042957716433214,
+            "velocityX": 3.584401588917909,
+            "velocityY": 1.2094213725184633,
+            "timestamp": 1.3470118478234772
+        },
+        {
+            "x": 4.612580698460036,
+            "y": 6.434798810689855,
+            "heading": -0.4985242294411719,
+            "angularVelocity": 0.0000010429577140470852,
+            "velocityX": 3.5844015889179057,
+            "velocityY": 1.209421372518471,
+            "timestamp": 1.3896897184354473
+        },
+        {
+            "x": 4.765555325693214,
+            "y": 6.4864143395415494,
+            "heading": -0.49852418492995754,
+            "angularVelocity": 0.0000010429577143791187,
+            "velocityX": 3.584401588917906,
+            "velocityY": 1.2094213725184708,
+            "timestamp": 1.4323675890474175
+        },
+        {
+            "x": 4.918529952926393,
+            "y": 6.5380298683932425,
+            "heading": -0.4985241404187432,
+            "angularVelocity": 0.0000010429577134079309,
+            "velocityX": 3.584401588917911,
+            "velocityY": 1.2094213725184557,
+            "timestamp": 1.4750454596593876
+        },
+        {
+            "x": 5.071504580159572,
+            "y": 6.589645397244938,
+            "heading": -0.49852409590752894,
+            "angularVelocity": 0.000001042957711441133,
+            "velocityX": 3.584401588917906,
+            "velocityY": 1.20942137251847,
+            "timestamp": 1.5177233302713578
+        },
+        {
+            "x": 5.224479207392751,
+            "y": 6.641260926096632,
+            "heading": -0.49852405139631467,
+            "angularVelocity": 0.0000010429577109856096,
+            "velocityX": 3.5844015889179066,
+            "velocityY": 1.2094213725184684,
+            "timestamp": 1.560401200883328
+        },
+        {
+            "x": 5.377453834625929,
+            "y": 6.692876454948327,
+            "heading": -0.4985240068851004,
+            "angularVelocity": 0.000001042957710628888,
+            "velocityX": 3.584401588917908,
+            "velocityY": 1.209421372518467,
+            "timestamp": 1.603079071495298
+        },
+        {
+            "x": 5.530428461859109,
+            "y": 6.744491983800022,
+            "heading": -0.4985239623738862,
+            "angularVelocity": 0.0000010429577097054993,
+            "velocityX": 3.5844015889179075,
+            "velocityY": 1.2094213725184686,
+            "timestamp": 1.6457569421072682
+        },
+        {
+            "x": 5.683403089092287,
+            "y": 6.796107512651717,
+            "heading": -0.4985239178626721,
+            "angularVelocity": 0.0000010429577084041104,
+            "velocityX": 3.584401588917908,
+            "velocityY": 1.2094213725184642,
+            "timestamp": 1.6884348127192383
+        },
+        {
+            "x": 5.836377716325465,
+            "y": 6.847723041503412,
+            "heading": -0.4985238733514579,
+            "angularVelocity": 0.000001042957708437487,
+            "velocityX": 3.5844015889179084,
+            "velocityY": 1.209421372518465,
+            "timestamp": 1.7311126833312085
+        },
+        {
+            "x": 5.989352343558645,
+            "y": 6.899338570355106,
+            "heading": -0.4985238288402439,
+            "angularVelocity": 0.0000010429577061050693,
+            "velocityX": 3.5844015889179084,
+            "velocityY": 1.2094213725184642,
+            "timestamp": 1.7737905539431786
+        },
+        {
+            "x": 6.142326970791823,
+            "y": 6.9509540992067995,
+            "heading": -0.49852378432902983,
+            "angularVelocity": 0.0000010429577060929704,
+            "velocityX": 3.584401588917909,
+            "velocityY": 1.2094213725184646,
+            "timestamp": 1.8164684245551488
+        },
+        {
+            "x": 6.295301598025002,
+            "y": 7.002569628058494,
+            "heading": -0.4985237398178158,
+            "angularVelocity": 0.0000010429577057751362,
+            "velocityX": 3.58440158891791,
+            "velocityY": 1.2094213725184613,
+            "timestamp": 1.859146295167119
+        },
+        {
+            "x": 6.44827622525818,
+            "y": 7.054185156910189,
+            "heading": -0.49852369530660184,
+            "angularVelocity": 0.0000010429577050460837,
+            "velocityX": 3.5844015889179097,
+            "velocityY": 1.2094213725184615,
+            "timestamp": 1.901824165779089
+        },
+        {
+            "x": 6.60125085249139,
+            "y": 7.105800685761796,
+            "heading": -0.4985236507953879,
+            "angularVelocity": 0.0000010429577031279755,
+            "velocityX": 3.584401588918602,
+            "velocityY": 1.2094213725164094,
+            "timestamp": 1.9445020363910592
+        },
+        {
+            "x": 6.754225480461122,
+            "y": 7.15741621243045,
+            "heading": -0.49852360628410947,
+            "angularVelocity": 0.000001042959214800434,
+            "velocityX": 3.584401606176385,
+            "velocityY": 1.209421321366894,
+            "timestamp": 1.9871799070030294
+        },
+        {
+            "x": 6.905473027890085,
+            "y": 7.208390081717835,
+            "heading": -0.49401401466016337,
+            "angularVelocity": 0.10566580664128356,
+            "velocityX": 3.543933782547739,
+            "velocityY": 1.1943864245440503,
+            "timestamp": 2.0298577776149997
+        },
+        {
+            "x": 7.045508663452743,
+            "y": 7.253974234854833,
+            "heading": -0.45930885159823864,
+            "angularVelocity": 0.8131887220303519,
+            "velocityX": 3.2812235839006783,
+            "velocityY": 1.0680981146283737,
+            "timestamp": 2.07253564822697
+        },
+        {
+            "x": 7.17296615196941,
+            "y": 7.296903697598188,
+            "heading": -0.39602280772923965,
+            "angularVelocity": 1.4828772607799376,
+            "velocityX": 2.986500654531669,
+            "velocityY": 1.0058951425592673,
+            "timestamp": 2.1152135188389405
+        },
+        {
+            "x": 7.286216905364156,
+            "y": 7.335009151903716,
+            "heading": -0.3243524220820172,
+            "angularVelocity": 1.6793336832302157,
+            "velocityX": 2.6536177126649325,
+            "velocityY": 0.8928621264164087,
+            "timestamp": 2.157891389450911
+        },
+        {
+            "x": 7.38508554947564,
+            "y": 7.368236895639727,
+            "heading": -0.2530399098538711,
+            "angularVelocity": 1.6709482269282783,
+            "velocityX": 2.31662551795061,
+            "velocityY": 0.7785707969856192,
+            "timestamp": 2.2005692600628812
+        },
+        {
+            "x": 7.469627579006154,
+            "y": 7.396613050645165,
+            "heading": -0.18659143584410423,
+            "angularVelocity": 1.5569772591027433,
+            "velocityX": 1.9809336388681473,
+            "velocityY": 0.6648915374301301,
+            "timestamp": 2.2432471306748516
+        },
+        {
+            "x": 7.539923583097788,
+            "y": 7.420174905803342,
+            "heading": -0.12774543015301906,
+            "angularVelocity": 1.3788411850749698,
+            "velocityX": 1.6471300719468636,
+            "velocityY": 0.5520860066427349,
+            "timestamp": 2.285925001286822
+        },
+        {
+            "x": 7.596048224439389,
+            "y": 7.4389607760624,
+            "heading": -0.07839593087005978,
+            "angularVelocity": 1.1563252471438406,
+            "velocityX": 1.315075952403783,
+            "velocityY": 0.4401782467044836,
+            "timestamp": 2.3286028718987923
+        },
+        {
+            "x": 7.638066692552635,
+            "y": 7.453006402993039,
+            "heading": -0.03996068961729096,
+            "angularVelocity": 0.9005894788478175,
+            "velocityX": 0.9845493111706484,
+            "velocityY": 0.3291079599154316,
+            "timestamp": 2.3712807425107627
+        },
+        {
+            "x": 7.666034942577791,
+            "y": 7.46234355914255,
+            "heading": -0.013541291231896178,
+            "angularVelocity": 0.6190420938664342,
+            "velocityX": 0.655333774251397,
+            "velocityY": 0.2187821467102802,
+            "timestamp": 2.413958613122733
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 0.32025552073032726,
-            "velocityX": 0.3271318790219412,
-            "velocityY": 0.10885864717634237,
-            "timestamp": 2.4505794706508084
+            "heading": 0,
+            "angularVelocity": 0.31729069510085045,
+            "velocityX": 0.3272201077973249,
+            "velocityY": 0.10910668200355174,
+            "timestamp": 2.4566364837347034
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
-            "velocityX": -9.011054390829982e-34,
-            "velocityY": 6.746182534290948e-34,
-            "timestamp": 2.4932621268326125
+            "velocityX": 5.5875375349932e-23,
+            "velocityY": -3.709612464642882e-23,
+            "timestamp": 2.499314354346674
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.traj
index 5004d1ca..a684ff7a 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.traj	
@@ -3,524 +3,614 @@
         {
             "x": 0.71,
             "y": 4.36,
-            "heading": 2.1,
+            "heading": -1.04,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": 3.1166835341708484e-33,
+            "velocityY": 5.951430910881942e-24,
             "timestamp": 0
         },
         {
-            "x": 0.7240863087605185,
-            "y": 4.368337736173442,
-            "heading": 2.1163001020338092,
-            "angularVelocity": 0.3614478974621331,
-            "velocityX": 0.31235796401343735,
-            "velocityY": 0.1848857880296264,
-            "timestamp": 0.045096685160595056
-        },
-        {
-            "x": 0.7523050752299821,
-            "y": 4.38505134365204,
-            "heading": 2.148025504039376,
-            "angularVelocity": 0.7034974276399452,
-            "velocityX": 0.6257392615216195,
-            "velocityY": 0.3706172065436647,
-            "timestamp": 0.09019337032119011
-        },
-        {
-            "x": 0.7947129681201482,
-            "y": 4.410184705499472,
-            "heading": 2.1940580950016866,
-            "angularVelocity": 1.020753316976243,
-            "velocityX": 0.9403771638457716,
-            "velocityY": 0.5573217135124126,
-            "timestamp": 0.13529005548178516
-        },
-        {
-            "x": 0.8513807226870724,
-            "y": 4.44378606946023,
-            "heading": 2.252941060288208,
-            "angularVelocity": 1.3057049554048448,
-            "velocityX": 1.25658359068129,
-            "velocityY": 0.7450960938946717,
-            "timestamp": 0.18038674064238022
-        },
-        {
-            "x": 0.922394851892513,
-            "y": 4.485907452258627,
-            "heading": 2.32274249990232,
-            "angularVelocity": 1.5478175250695256,
-            "velocityX": 1.5747084060070078,
-            "velocityY": 0.9340239232306716,
-            "timestamp": 0.22548342580297528
-        },
-        {
-            "x": 1.0078549434585182,
-            "y": 4.536606503403333,
-            "heading": 2.4007952890388493,
-            "angularVelocity": 1.7307877254963087,
-            "velocityX": 1.8950415371256468,
-            "velocityY": 1.1242301061410762,
-            "timestamp": 0.2705801109635703
-        },
-        {
-            "x": 1.1078580204781514,
-            "y": 4.595947756684704,
-            "heading": 2.4830211624583747,
-            "angularVelocity": 1.823324111887792,
-            "velocityX": 2.2175261144695955,
-            "velocityY": 1.315867298672826,
-            "timestamp": 0.3156767961241654
-        },
-        {
-            "x": 1.2224032158634355,
-            "y": 4.663956921963176,
-            "heading": 2.561776708325237,
-            "angularVelocity": 1.7463710600103812,
-            "velocityX": 2.5399914645028585,
-            "velocityY": 1.5080745965314675,
-            "timestamp": 0.36077348128476044
-        },
-        {
-            "x": 1.3501227544876644,
-            "y": 4.740405284602277,
-            "heading": 2.616849964946217,
-            "angularVelocity": 1.2212262702869785,
-            "velocityX": 2.8321269771692363,
-            "velocityY": 1.695210243654476,
-            "timestamp": 0.4058701664453555
-        },
-        {
-            "x": 1.4890564849563437,
-            "y": 4.825393782343559,
-            "heading": 2.6364418869926514,
-            "angularVelocity": 0.43444261982150123,
-            "velocityX": 3.0807969582225083,
-            "velocityY": 1.884584142684248,
-            "timestamp": 0.45096685160595057
-        },
-        {
-            "x": 1.6359707555642846,
-            "y": 4.9121110990754175,
-            "heading": 2.6364419350884667,
-            "angularVelocity": 0.0000010665044506613552,
-            "velocityX": 3.257762074634985,
-            "velocityY": 1.9229199756711164,
-            "timestamp": 0.49606353676654563
-        },
-        {
-            "x": 1.7828851010176074,
-            "y": 4.99882828900635,
-            "heading": 2.63644198318385,
-            "angularVelocity": 0.000001066494868779716,
-            "velocityX": 3.257763734299801,
-            "velocityY": 1.9229171639139742,
-            "timestamp": 0.5411602219271406
-        },
-        {
-            "x": 1.929799446473537,
-            "y": 5.085545478932866,
-            "heading": 2.6364420312792327,
-            "angularVelocity": 0.0000010664948614258307,
-            "velocityX": 3.257763734357611,
-            "velocityY": 1.9229171638160338,
-            "timestamp": 0.5862569070877357
-        },
-        {
-            "x": 2.076713791929467,
-            "y": 5.172262668859381,
-            "heading": 2.636442079374616,
-            "angularVelocity": 0.000001066494855048045,
-            "velocityX": 3.257763734357614,
-            "velocityY": 1.922917163816031,
-            "timestamp": 0.6313535922483308
-        },
-        {
-            "x": 2.2236281373853966,
-            "y": 5.258979858785897,
-            "heading": 2.6364421274699983,
-            "angularVelocity": 0.000001066494848970006,
-            "velocityX": 3.257763734357614,
-            "velocityY": 1.9229171638160298,
-            "timestamp": 0.6764502774089258
-        },
-        {
-            "x": 2.3705424828413264,
-            "y": 5.345697048712413,
-            "heading": 2.6364421755653806,
-            "angularVelocity": 0.0000010664948432023413,
-            "velocityX": 3.257763734357615,
-            "velocityY": 1.9229171638160298,
-            "timestamp": 0.7215469625695209
-        },
-        {
-            "x": 2.5174568282972563,
-            "y": 5.432414238638929,
-            "heading": 2.6364422236607625,
-            "angularVelocity": 0.000001066494835980498,
-            "velocityX": 3.2577637343576162,
-            "velocityY": 1.9229171638160294,
-            "timestamp": 0.766643647730116
-        },
-        {
-            "x": 2.6643711737531857,
-            "y": 5.519131428565444,
-            "heading": 2.636442271756144,
-            "angularVelocity": 0.0000010664948304508311,
-            "velocityX": 3.2577637343576167,
-            "velocityY": 1.9229171638160292,
-            "timestamp": 0.811740332890711
-        },
-        {
-            "x": 2.8112855192091155,
-            "y": 5.60584861849196,
-            "heading": 2.6364423198515254,
-            "angularVelocity": 0.0000010664948245159241,
-            "velocityX": 3.257763734357617,
-            "velocityY": 1.922917163816029,
-            "timestamp": 0.8568370180513061
-        },
-        {
-            "x": 2.958199864665046,
-            "y": 5.692565808418475,
-            "heading": 2.6364423679469065,
-            "angularVelocity": 0.0000010664948181966642,
-            "velocityX": 3.2577637343576185,
-            "velocityY": 1.922917163816028,
-            "timestamp": 0.9019337032119011
-        },
-        {
-            "x": 3.1051142101209757,
-            "y": 5.779282998344991,
-            "heading": 2.636442416042287,
-            "angularVelocity": 0.0000010664948111950998,
-            "velocityX": 3.257763734357619,
-            "velocityY": 1.9229171638160278,
-            "timestamp": 0.9470303883724962
-        },
-        {
-            "x": 3.252028555576906,
-            "y": 5.8660001882715065,
-            "heading": 2.6364424641376676,
-            "angularVelocity": 0.0000010664948058295197,
-            "velocityX": 3.2577637343576273,
-            "velocityY": 1.922917163816015,
-            "timestamp": 0.9921270735330913
-        },
-        {
-            "x": 3.3989429010423815,
-            "y": 5.952717378181851,
-            "heading": 2.636442512233048,
-            "angularVelocity": 0.00000106649479979568,
-            "velocityX": 3.257763734569281,
-            "velocityY": 1.9229171634574358,
-            "timestamp": 1.0372237586936863
-        },
-        {
-            "x": 3.5458575205338394,
-            "y": 6.039434103841755,
-            "heading": 2.636442560328411,
-            "angularVelocity": 0.0000010664944172141213,
-            "velocityX": 3.2577698109800433,
-            "velocityY": 1.9229068688994477,
-            "timestamp": 1.0823204438542813
+            "x": 0.7172662637575132,
+            "y": 4.364294720467062,
+            "heading": -1.0315470131742317,
+            "angularVelocity": 0.26098153509522004,
+            "velocityX": 0.22434208273715575,
+            "velocityY": 0.13259724206381326,
+            "timestamp": 0.03238921413610362
+        },
+        {
+            "x": 0.7318144654254066,
+            "y": 4.3728967855708385,
+            "heading": -1.0149312072454106,
+            "angularVelocity": 0.5130042939294357,
+            "velocityX": 0.4491680967238058,
+            "velocityY": 0.2655842487449578,
+            "timestamp": 0.06477842827220724
+        },
+        {
+            "x": 0.7536624687467911,
+            "y": 4.385820612299006,
+            "heading": -0.9904958605622504,
+            "angularVelocity": 0.7544285137786853,
+            "velocityX": 0.6745456444104029,
+            "velocityY": 0.39901637235962667,
+            "timestamp": 0.09716764240831086
+        },
+        {
+            "x": 0.7828310955502448,
+            "y": 4.4030821899572965,
+            "heading": -0.9586530378385134,
+            "angularVelocity": 0.9831304516969581,
+            "velocityX": 0.9005660551343861,
+            "velocityY": 0.5329421574032179,
+            "timestamp": 0.12955685654441448
+        },
+        {
+            "x": 0.8193448564336872,
+            "y": 4.424698840047832,
+            "heading": -0.919902879162077,
+            "angularVelocity": 1.1963908266993857,
+            "velocityX": 1.1273432177146,
+            "velocityY": 0.6674027347406944,
+            "timestamp": 0.1619460706805181
+        },
+        {
+            "x": 0.8632324943492647,
+            "y": 4.450689040734006,
+            "heading": -0.8748582540701377,
+            "angularVelocity": 1.3907291761589549,
+            "velocityX": 1.3550078038681697,
+            "velocityY": 0.8024338156819394,
+            "timestamp": 0.19433528481662174
+        },
+        {
+            "x": 0.9145271224696989,
+            "y": 4.481072488182425,
+            "heading": -0.8242801160685745,
+            "angularVelocity": 1.5615734852049024,
+            "velocityX": 1.5836947418633758,
+            "velocityY": 0.9380730054376618,
+            "timestamp": 0.22672449895272537
+        },
+        {
+            "x": 0.9732655893831887,
+            "y": 4.515870568727518,
+            "heading": -0.7691375276578237,
+            "angularVelocity": 1.7024984977725797,
+            "velocityX": 1.8135193606940647,
+            "velocityY": 1.0743724870528544,
+            "timestamp": 0.25911371308882897
+        },
+        {
+            "x": 1.0394861119792222,
+            "y": 4.555106942812012,
+            "heading": -0.7107271690719036,
+            "angularVelocity": 1.8033891881560444,
+            "velocityX": 2.044523905944935,
+            "velocityY": 1.2114024724285466,
+            "timestamp": 0.2915029272249326
+        },
+        {
+            "x": 1.1132202582415271,
+            "y": 4.598805444215395,
+            "heading": -0.6509416110612904,
+            "angularVelocity": 1.8458477491731227,
+            "velocityX": 2.276503096137642,
+            "velocityY": 1.3491683132463759,
+            "timestamp": 0.3238921413610362
+        },
+        {
+            "x": 1.194458181637227,
+            "y": 4.646971254862031,
+            "heading": -0.5929406477835183,
+            "angularVelocity": 1.790749322723445,
+            "velocityX": 2.508178279791774,
+            "velocityY": 1.48709414326129,
+            "timestamp": 0.35628135549713985
+        },
+        {
+            "x": 1.2829200414273145,
+            "y": 4.699514481647374,
+            "heading": -0.5430956567390244,
+            "angularVelocity": 1.5389379574026985,
+            "velocityX": 2.7312135273909255,
+            "velocityY": 1.6222445708176134,
+            "timestamp": 0.3886705696332435
+        },
+        {
+            "x": 1.3771379621947135,
+            "y": 4.756486240363935,
+            "heading": -0.5116699283377882,
+            "angularVelocity": 0.9702528832339478,
+            "velocityX": 2.908928891311882,
+            "velocityY": 1.758973171660129,
+            "timestamp": 0.4210597837693471
+        },
+        {
+            "x": 1.4771406301349097,
+            "y": 4.8178582459332,
+            "heading": -0.4985254849792972,
+            "angularVelocity": 0.40582779511896505,
+            "velocityX": 3.087529926474048,
+            "velocityY": 1.894828485537579,
+            "timestamp": 0.45344899790545073
+        },
+        {
+            "x": 1.5826858211522454,
+            "y": 4.880091207833619,
+            "heading": -0.49852543793275184,
+            "angularVelocity": 0.000001452537415626245,
+            "velocityX": 3.2586524197166784,
+            "velocityY": 1.9214100607352578,
+            "timestamp": 0.48583821204155436
+        },
+        {
+            "x": 1.6882312828144117,
+            "y": 4.9423237107273525,
+            "heading": -0.4985253908851185,
+            "angularVelocity": 0.0000014525710071923091,
+            "velocityX": 3.2586607757338757,
+            "velocityY": 1.9213958891446032,
+            "timestamp": 0.5182274261776579
+        },
+        {
+            "x": 1.7937767445018398,
+            "y": 5.004556213578243,
+            "heading": -0.49852534383749064,
+            "angularVelocity": 0.0000014525708350435827,
+            "velocityX": 3.2586607765138327,
+            "velocityY": 1.921395887821837,
+            "timestamp": 0.5506166403137616
+        },
+        {
+            "x": 1.8993222061892705,
+            "y": 5.06678871642913,
+            "heading": -0.49852529678986307,
+            "angularVelocity": 0.0000014525708270038968,
+            "velocityX": 3.2586607765139086,
+            "velocityY": 1.9213958878217097,
+            "timestamp": 0.5830058544498652
+        },
+        {
+            "x": 2.0048676678767015,
+            "y": 5.129021219280016,
+            "heading": -0.4985252497422358,
+            "angularVelocity": 0.0000014525708183379938,
+            "velocityX": 3.258660776513909,
+            "velocityY": 1.9213958878217101,
+            "timestamp": 0.6153950685859688
+        },
+        {
+            "x": 2.1104131295641317,
+            "y": 5.191253722130902,
+            "heading": -0.49852520269460865,
+            "angularVelocity": 0.000001452570810915054,
+            "velocityX": 3.2586607765139104,
+            "velocityY": 1.9213958878217086,
+            "timestamp": 0.6477842827220724
+        },
+        {
+            "x": 2.2159585912515625,
+            "y": 5.253486224981789,
+            "heading": -0.4985251556469819,
+            "angularVelocity": 0.0000014525708028649706,
+            "velocityX": 3.2586607765139086,
+            "velocityY": 1.9213958878217123,
+            "timestamp": 0.6801734968581761
+        },
+        {
+            "x": 2.321504052938993,
+            "y": 5.315718727832675,
+            "heading": -0.4985251085993554,
+            "angularVelocity": 0.000001452570794356237,
+            "velocityX": 3.2586607765139117,
+            "velocityY": 1.9213958878217081,
+            "timestamp": 0.7125627109942797
+        },
+        {
+            "x": 2.427049514626424,
+            "y": 5.3779512306835615,
+            "heading": -0.49852506155172915,
+            "angularVelocity": 0.0000014525707870212498,
+            "velocityX": 3.258660776513912,
+            "velocityY": 1.9213958878217077,
+            "timestamp": 0.7449519251303833
+        },
+        {
+            "x": 2.5325949763138547,
+            "y": 5.440183733534448,
+            "heading": -0.4985250145041031,
+            "angularVelocity": 0.000001452570779107594,
+            "velocityX": 3.258660776513913,
+            "velocityY": 1.9213958878217068,
+            "timestamp": 0.777341139266487
+        },
+        {
+            "x": 2.6381404380012854,
+            "y": 5.502416236385335,
+            "heading": -0.49852496745647734,
+            "angularVelocity": 0.0000014525707705186945,
+            "velocityX": 3.258660776513915,
+            "velocityY": 1.9213958878217055,
+            "timestamp": 0.8097303534025906
+        },
+        {
+            "x": 2.743685899688716,
+            "y": 5.564648739236221,
+            "heading": -0.4985249204088519,
+            "angularVelocity": 0.00000145257076360687,
+            "velocityX": 3.2586607765139153,
+            "velocityY": 1.9213958878217066,
+            "timestamp": 0.8421195675386942
+        },
+        {
+            "x": 2.849231361376147,
+            "y": 5.626881242087108,
+            "heading": -0.4985248733612266,
+            "angularVelocity": 0.0000014525707544682094,
+            "velocityX": 3.2586607765139153,
+            "velocityY": 1.9213958878217066,
+            "timestamp": 0.8745087816747978
+        },
+        {
+            "x": 2.954776823063578,
+            "y": 5.689113744937994,
+            "heading": -0.4985248263136016,
+            "angularVelocity": 0.0000014525707471414822,
+            "velocityX": 3.2586607765139166,
+            "velocityY": 1.921395887821705,
+            "timestamp": 0.9068979958109015
+        },
+        {
+            "x": 3.0603222847510096,
+            "y": 5.751346247788881,
+            "heading": -0.49852477926597694,
+            "angularVelocity": 0.0000014525707385370494,
+            "velocityX": 3.258660776513917,
+            "velocityY": 1.9213958878217057,
+            "timestamp": 0.9392872099470051
+        },
+        {
+            "x": 3.1658677464384404,
+            "y": 5.813578750639766,
+            "heading": -0.4985247322183525,
+            "angularVelocity": 0.0000014525707310188908,
+            "velocityX": 3.2586607765139175,
+            "velocityY": 1.9213958878217063,
+            "timestamp": 0.9716764240831087
+        },
+        {
+            "x": 3.271413208125871,
+            "y": 5.875811253490653,
+            "heading": -0.4985246851707283,
+            "angularVelocity": 0.0000014525707228586851,
+            "velocityX": 3.258660776513917,
+            "velocityY": 1.921395887821708,
+            "timestamp": 1.0040656382192124
+        },
+        {
+            "x": 3.376958669813326,
+            "y": 5.938043756341499,
+            "heading": -0.4985246381231044,
+            "angularVelocity": 0.000001452570714543944,
+            "velocityX": 3.258660776514645,
+            "velocityY": 1.921395887820474,
+            "timestamp": 1.0364548523553159
+        },
+        {
+            "x": 3.482504131751534,
+            "y": 6.000276258767071,
+            "heading": -0.49852459107548075,
+            "angularVelocity": 0.0000014525707064344823,
+            "velocityX": 3.258660784256519,
+            "velocityY": 1.9213958746903632,
+            "timestamp": 1.0688440664914194
+        },
+        {
+            "x": 3.588052280044538,
+            "y": 6.0625042049472775,
+            "heading": -0.4985245440280508,
+            "angularVelocity": 0.0000014525647243024202,
+            "velocityX": 3.258743724052004,
+            "velocityY": 1.921255203004168,
+            "timestamp": 1.101233280627523
         },
         {
             "x": 3.69740891456604,
             "y": 6.1177659034729,
-            "heading": 2.6364426084288106,
-            "angularVelocity": 0.0000010666061013072793,
-            "velocityX": 3.3605883335439555,
-            "velocityY": 1.736974665703161,
-            "timestamp": 1.1274171290148762
-        },
-        {
-            "x": 3.8471798789241873,
-            "y": 6.1780975155460816,
-            "heading": 2.636442653750165,
-            "angularVelocity": 0.0000010618213127102396,
-            "velocityX": 3.5089419861830207,
-            "velocityY": 1.4134924456482445,
-            "timestamp": 1.1700997851966806
-        },
-        {
-            "x": 4.000192250358795,
-            "y": 6.2296577661788755,
-            "heading": 2.6364426985309986,
-            "angularVelocity": 0.0000010491576146038598,
-            "velocityX": 3.584884004942437,
-            "velocityY": 1.2079906745535305,
-            "timestamp": 1.212782441378485
-        },
-        {
-            "x": 4.153204837360183,
-            "y": 6.281217377082217,
-            "heading": 2.6364427433118154,
-            "angularVelocity": 0.0000010491572166464704,
-            "velocityX": 3.5848890553961605,
-            "velocityY": 1.2079756865112976,
-            "timestamp": 1.2554650975602892
-        },
-        {
-            "x": 4.306217424370213,
-            "y": 6.3327769879599085,
-            "heading": 2.636442788092632,
-            "angularVelocity": 0.0000010491572158103502,
-            "velocityX": 3.5848890555986537,
-            "velocityY": 1.2079756859103623,
-            "timestamp": 1.2981477537420936
-        },
-        {
-            "x": 4.459230011380243,
-            "y": 6.384336598837598,
-            "heading": 2.6364428328734486,
-            "angularVelocity": 0.0000010491572148390834,
-            "velocityX": 3.584889055598662,
-            "velocityY": 1.2079756859103368,
-            "timestamp": 1.340830409923898
-        },
-        {
-            "x": 4.6122425983902735,
-            "y": 6.435896209715288,
-            "heading": 2.636442877654265,
-            "angularVelocity": 0.0000010491572125659184,
-            "velocityX": 3.584889055598663,
-            "velocityY": 1.2079756859103368,
-            "timestamp": 1.3835130661057022
-        },
-        {
-            "x": 4.765255185400304,
-            "y": 6.487455820592977,
-            "heading": 2.636442922435082,
-            "angularVelocity": 0.0000010491572116366236,
-            "velocityX": 3.584889055598663,
-            "velocityY": 1.2079756859103352,
-            "timestamp": 1.4261957222875066
-        },
-        {
-            "x": 4.918267772410334,
-            "y": 6.539015431470667,
-            "heading": 2.636442967215898,
-            "angularVelocity": 0.0000010491572113687884,
-            "velocityX": 3.584889055598663,
-            "velocityY": 1.2079756859103348,
-            "timestamp": 1.468878378469311
-        },
-        {
-            "x": 5.071280359420366,
-            "y": 6.590575042348357,
-            "heading": 2.6364430119967146,
-            "angularVelocity": 0.0000010491572106337029,
-            "velocityX": 3.584889055598664,
-            "velocityY": 1.2079756859103339,
-            "timestamp": 1.5115610346511152
-        },
-        {
-            "x": 5.224292946430397,
-            "y": 6.642134653226047,
-            "heading": 2.6364430567775314,
-            "angularVelocity": 0.0000010491572087267192,
-            "velocityX": 3.584889055598664,
-            "velocityY": 1.2079756859103334,
-            "timestamp": 1.5542436908329196
-        },
-        {
-            "x": 5.3773055334404285,
-            "y": 6.693694264103736,
-            "heading": 2.636443101558348,
-            "angularVelocity": 0.0000010491572072659829,
-            "velocityX": 3.5848890555986643,
-            "velocityY": 1.2079756859103326,
-            "timestamp": 1.596926347014724
-        },
-        {
-            "x": 5.53031812045046,
-            "y": 6.745253874981426,
-            "heading": 2.6364431463391638,
-            "angularVelocity": 0.0000010491572066940514,
-            "velocityX": 3.5848890555986643,
-            "velocityY": 1.2079756859103314,
-            "timestamp": 1.6396090031965282
-        },
-        {
-            "x": 5.683330707460491,
-            "y": 6.796813485859116,
-            "heading": 2.63644319111998,
-            "angularVelocity": 0.000001049157205644206,
-            "velocityX": 3.5848890555986648,
-            "velocityY": 1.2079756859103312,
-            "timestamp": 1.6822916593783326
-        },
-        {
-            "x": 5.836343294470522,
-            "y": 6.848373096736806,
-            "heading": 2.636443235900796,
-            "angularVelocity": 0.0000010491572052803454,
-            "velocityX": 3.5848890555986657,
-            "velocityY": 1.2079756859103297,
-            "timestamp": 1.724974315560137
-        },
-        {
-            "x": 5.989355881480554,
-            "y": 6.899932707614496,
-            "heading": 2.6364432806816125,
-            "angularVelocity": 0.0000010491572041084738,
-            "velocityX": 3.5848890555986657,
-            "velocityY": 1.2079756859103294,
-            "timestamp": 1.7676569717419413
-        },
-        {
-            "x": 6.142368468490585,
-            "y": 6.951492318492185,
-            "heading": 2.6364433254624284,
-            "angularVelocity": 0.0000010491572019949161,
-            "velocityX": 3.584889055598666,
-            "velocityY": 1.2079756859103283,
-            "timestamp": 1.8103396279237456
-        },
-        {
-            "x": 6.295381055500616,
-            "y": 7.0030519293698745,
-            "heading": 2.6364433702432444,
-            "angularVelocity": 0.0000010491572021090905,
-            "velocityX": 3.584889055598666,
-            "velocityY": 1.2079756859103277,
-            "timestamp": 1.85302228410555
-        },
-        {
-            "x": 6.448393642510648,
-            "y": 7.054611540247564,
-            "heading": 2.6364434150240608,
-            "angularVelocity": 0.0000010491572009028897,
-            "velocityX": 3.584889055598666,
-            "velocityY": 1.2079756859103268,
-            "timestamp": 1.8957049402873543
-        },
-        {
-            "x": 6.601406229520499,
-            "y": 7.106171151125787,
-            "heading": 2.6364434598048767,
-            "angularVelocity": 0.0000010491571991066208,
-            "velocityX": 3.58488905559446,
-            "velocityY": 1.2079756859228103,
-            "timestamp": 1.9383875964691586
-        },
-        {
-            "x": 6.75441881205228,
-            "y": 7.157730775293412,
-            "heading": 2.636443504585761,
-            "angularVelocity": 0.0000010491588061157563,
-            "velocityX": 3.5848889506789767,
-            "velocityY": 1.207975997276489,
-            "timestamp": 1.981070252650963
-        },
-        {
-            "x": 6.905487720036347,
-            "y": 7.20899283211694,
-            "heading": 2.6411872457126164,
-            "angularVelocity": 0.11113978255358876,
-            "velocityX": 3.539351143954066,
-            "velocityY": 1.2010043752942743,
-            "timestamp": 2.0237529088327673
-        },
-        {
-            "x": 7.045499588568849,
-            "y": 7.254368377570732,
-            "heading": 2.676148381759387,
-            "angularVelocity": 0.81909466687958,
-            "velocityX": 3.2802988627542176,
-            "velocityY": 1.0630909487103528,
-            "timestamp": 2.0664355650145714
-        },
-        {
-            "x": 7.172970608020284,
-            "y": 7.297222729151116,
-            "heading": 2.7400991643229964,
-            "angularVelocity": 1.4982849776549703,
-            "velocityX": 2.9864828212302403,
-            "velocityY": 1.0040226034164315,
-            "timestamp": 2.1091182211963755
-        },
-        {
-            "x": 7.286231301322222,
-            "y": 7.335261893158222,
-            "heading": 2.8124985936039937,
-            "angularVelocity": 1.6962259558687312,
-            "velocityX": 2.6535530689447047,
-            "velocityY": 0.8912089220755359,
-            "timestamp": 2.1518008773781796
-        },
-        {
-            "x": 7.3851039731372365,
-            "y": 7.3684303034498875,
-            "heading": 2.8845195362089706,
-            "angularVelocity": 1.6873584975173053,
-            "velocityX": 2.316460142355517,
-            "velocityY": 0.7770933971491014,
-            "timestamp": 2.1944835335599837
-        },
-        {
-            "x": 7.46964567597808,
-            "y": 7.396754020582906,
-            "heading": 2.951617256319432,
-            "angularVelocity": 1.5720136962578515,
-            "velocityX": 1.9807038831122041,
-            "velocityY": 0.6635884377105193,
-            "timestamp": 2.237166189741788
-        },
-        {
-            "x": 7.539938623640353,
-            "y": 7.420270719849242,
-            "heading": 3.0110324789568197,
-            "angularVelocity": 1.3920226141576573,
-            "velocityX": 1.6468737878651336,
-            "velocityY": 0.5509661621378036,
-            "timestamp": 2.279848845923592
-        },
-        {
-            "x": 7.596058846559568,
-            "y": 7.439019303920545,
-            "heading": 3.060856486484029,
-            "angularVelocity": 1.1673127210028025,
-            "velocityX": 1.3148249884022007,
-            "velocityY": 0.4392553263659099,
-            "timestamp": 2.322531502105396
-        },
-        {
-            "x": 7.638072702511279,
-            "y": 7.453036150115233,
-            "heading": 3.09965963024843,
-            "angularVelocity": 0.9091079898851933,
-            "velocityX": 0.9843308666816342,
-            "velocityY": 0.32839676460117173,
-            "timestamp": 2.3652141582872
-        },
-        {
-            "x": 7.666037142481598,
-            "y": 7.462353623790155,
-            "heading": 3.126330643718343,
-            "angularVelocity": 0.6248677063655441,
-            "velocityX": 0.6551710336677993,
-            "velocityY": 0.2182964817192845,
-            "timestamp": 2.4078968144690043
+            "heading": -0.4985244969178424,
+            "angularVelocity": 0.0000014545029788824699,
+            "velocityX": 3.3763287390046313,
+            "velocityY": 1.7061759601022248,
+            "timestamp": 1.1336224947636264
+        },
+        {
+            "x": 3.8477077097405896,
+            "y": 6.176720729441604,
+            "heading": -0.4985244519972541,
+            "angularVelocity": 0.0000010525498975792195,
+            "velocityX": 3.5217032391581933,
+            "velocityY": 1.3813909907718895,
+            "timestamp": 1.1763003653755966
+        },
+        {
+            "x": 4.000682189533232,
+            "y": 6.228336695265556,
+            "heading": -0.49852440748602983,
+            "angularVelocity": 0.000001042957945295385,
+            "velocityX": 3.58439813418752,
+            "velocityY": 1.2094316113670969,
+            "timestamp": 1.2189782359875667
+        },
+        {
+            "x": 4.153656816760498,
+            "y": 6.279952224134777,
+            "heading": -0.4985243629748153,
+            "angularVelocity": 0.00000104295771772215,
+            "velocityX": 3.5844015887793454,
+            "velocityY": 1.2094213729291292,
+            "timestamp": 1.2616561065995369
+        },
+        {
+            "x": 4.306631443993679,
+            "y": 6.331567752986466,
+            "heading": -0.4985243184636008,
+            "angularVelocity": 0.0000010429577168479619,
+            "velocityX": 3.584401588917953,
+            "velocityY": 1.209421372518333,
+            "timestamp": 1.304333977211507
+        },
+        {
+            "x": 4.459606071226857,
+            "y": 6.38318328183816,
+            "heading": -0.4985242739523863,
+            "angularVelocity": 0.000001042957716433214,
+            "velocityX": 3.584401588917909,
+            "velocityY": 1.2094213725184633,
+            "timestamp": 1.3470118478234772
+        },
+        {
+            "x": 4.612580698460036,
+            "y": 6.434798810689855,
+            "heading": -0.4985242294411719,
+            "angularVelocity": 0.0000010429577140470852,
+            "velocityX": 3.5844015889179057,
+            "velocityY": 1.209421372518471,
+            "timestamp": 1.3896897184354473
+        },
+        {
+            "x": 4.765555325693214,
+            "y": 6.4864143395415494,
+            "heading": -0.49852418492995754,
+            "angularVelocity": 0.0000010429577143791187,
+            "velocityX": 3.584401588917906,
+            "velocityY": 1.2094213725184708,
+            "timestamp": 1.4323675890474175
+        },
+        {
+            "x": 4.918529952926393,
+            "y": 6.5380298683932425,
+            "heading": -0.4985241404187432,
+            "angularVelocity": 0.0000010429577134079309,
+            "velocityX": 3.584401588917911,
+            "velocityY": 1.2094213725184557,
+            "timestamp": 1.4750454596593876
+        },
+        {
+            "x": 5.071504580159572,
+            "y": 6.589645397244938,
+            "heading": -0.49852409590752894,
+            "angularVelocity": 0.000001042957711441133,
+            "velocityX": 3.584401588917906,
+            "velocityY": 1.20942137251847,
+            "timestamp": 1.5177233302713578
+        },
+        {
+            "x": 5.224479207392751,
+            "y": 6.641260926096632,
+            "heading": -0.49852405139631467,
+            "angularVelocity": 0.0000010429577109856096,
+            "velocityX": 3.5844015889179066,
+            "velocityY": 1.2094213725184684,
+            "timestamp": 1.560401200883328
+        },
+        {
+            "x": 5.377453834625929,
+            "y": 6.692876454948327,
+            "heading": -0.4985240068851004,
+            "angularVelocity": 0.000001042957710628888,
+            "velocityX": 3.584401588917908,
+            "velocityY": 1.209421372518467,
+            "timestamp": 1.603079071495298
+        },
+        {
+            "x": 5.530428461859109,
+            "y": 6.744491983800022,
+            "heading": -0.4985239623738862,
+            "angularVelocity": 0.0000010429577097054993,
+            "velocityX": 3.5844015889179075,
+            "velocityY": 1.2094213725184686,
+            "timestamp": 1.6457569421072682
+        },
+        {
+            "x": 5.683403089092287,
+            "y": 6.796107512651717,
+            "heading": -0.4985239178626721,
+            "angularVelocity": 0.0000010429577084041104,
+            "velocityX": 3.584401588917908,
+            "velocityY": 1.2094213725184642,
+            "timestamp": 1.6884348127192383
+        },
+        {
+            "x": 5.836377716325465,
+            "y": 6.847723041503412,
+            "heading": -0.4985238733514579,
+            "angularVelocity": 0.000001042957708437487,
+            "velocityX": 3.5844015889179084,
+            "velocityY": 1.209421372518465,
+            "timestamp": 1.7311126833312085
+        },
+        {
+            "x": 5.989352343558645,
+            "y": 6.899338570355106,
+            "heading": -0.4985238288402439,
+            "angularVelocity": 0.0000010429577061050693,
+            "velocityX": 3.5844015889179084,
+            "velocityY": 1.2094213725184642,
+            "timestamp": 1.7737905539431786
+        },
+        {
+            "x": 6.142326970791823,
+            "y": 6.9509540992067995,
+            "heading": -0.49852378432902983,
+            "angularVelocity": 0.0000010429577060929704,
+            "velocityX": 3.584401588917909,
+            "velocityY": 1.2094213725184646,
+            "timestamp": 1.8164684245551488
+        },
+        {
+            "x": 6.295301598025002,
+            "y": 7.002569628058494,
+            "heading": -0.4985237398178158,
+            "angularVelocity": 0.0000010429577057751362,
+            "velocityX": 3.58440158891791,
+            "velocityY": 1.2094213725184613,
+            "timestamp": 1.859146295167119
+        },
+        {
+            "x": 6.44827622525818,
+            "y": 7.054185156910189,
+            "heading": -0.49852369530660184,
+            "angularVelocity": 0.0000010429577050460837,
+            "velocityX": 3.5844015889179097,
+            "velocityY": 1.2094213725184615,
+            "timestamp": 1.901824165779089
+        },
+        {
+            "x": 6.60125085249139,
+            "y": 7.105800685761796,
+            "heading": -0.4985236507953879,
+            "angularVelocity": 0.0000010429577031279755,
+            "velocityX": 3.584401588918602,
+            "velocityY": 1.2094213725164094,
+            "timestamp": 1.9445020363910592
+        },
+        {
+            "x": 6.754225480461122,
+            "y": 7.15741621243045,
+            "heading": -0.49852360628410947,
+            "angularVelocity": 0.000001042959214800434,
+            "velocityX": 3.584401606176385,
+            "velocityY": 1.209421321366894,
+            "timestamp": 1.9871799070030294
+        },
+        {
+            "x": 6.905473027890085,
+            "y": 7.208390081717835,
+            "heading": -0.49401401466016337,
+            "angularVelocity": 0.10566580664128356,
+            "velocityX": 3.543933782547739,
+            "velocityY": 1.1943864245440503,
+            "timestamp": 2.0298577776149997
+        },
+        {
+            "x": 7.045508663452743,
+            "y": 7.253974234854833,
+            "heading": -0.45930885159823864,
+            "angularVelocity": 0.8131887220303519,
+            "velocityX": 3.2812235839006783,
+            "velocityY": 1.0680981146283737,
+            "timestamp": 2.07253564822697
+        },
+        {
+            "x": 7.17296615196941,
+            "y": 7.296903697598188,
+            "heading": -0.39602280772923965,
+            "angularVelocity": 1.4828772607799376,
+            "velocityX": 2.986500654531669,
+            "velocityY": 1.0058951425592673,
+            "timestamp": 2.1152135188389405
+        },
+        {
+            "x": 7.286216905364156,
+            "y": 7.335009151903716,
+            "heading": -0.3243524220820172,
+            "angularVelocity": 1.6793336832302157,
+            "velocityX": 2.6536177126649325,
+            "velocityY": 0.8928621264164087,
+            "timestamp": 2.157891389450911
+        },
+        {
+            "x": 7.38508554947564,
+            "y": 7.368236895639727,
+            "heading": -0.2530399098538711,
+            "angularVelocity": 1.6709482269282783,
+            "velocityX": 2.31662551795061,
+            "velocityY": 0.7785707969856192,
+            "timestamp": 2.2005692600628812
+        },
+        {
+            "x": 7.469627579006154,
+            "y": 7.396613050645165,
+            "heading": -0.18659143584410423,
+            "angularVelocity": 1.5569772591027433,
+            "velocityX": 1.9809336388681473,
+            "velocityY": 0.6648915374301301,
+            "timestamp": 2.2432471306748516
+        },
+        {
+            "x": 7.539923583097788,
+            "y": 7.420174905803342,
+            "heading": -0.12774543015301906,
+            "angularVelocity": 1.3788411850749698,
+            "velocityX": 1.6471300719468636,
+            "velocityY": 0.5520860066427349,
+            "timestamp": 2.285925001286822
+        },
+        {
+            "x": 7.596048224439389,
+            "y": 7.4389607760624,
+            "heading": -0.07839593087005978,
+            "angularVelocity": 1.1563252471438406,
+            "velocityX": 1.315075952403783,
+            "velocityY": 0.4401782467044836,
+            "timestamp": 2.3286028718987923
+        },
+        {
+            "x": 7.638066692552635,
+            "y": 7.453006402993039,
+            "heading": -0.03996068961729096,
+            "angularVelocity": 0.9005894788478175,
+            "velocityX": 0.9845493111706484,
+            "velocityY": 0.3291079599154316,
+            "timestamp": 2.3712807425107627
+        },
+        {
+            "x": 7.666034942577791,
+            "y": 7.46234355914255,
+            "heading": -0.013541291231896178,
+            "angularVelocity": 0.6190420938664342,
+            "velocityX": 0.655333774251397,
+            "velocityY": 0.2187821467102802,
+            "timestamp": 2.413958613122733
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 0.32025552073032726,
-            "velocityX": 0.3271318790219412,
-            "velocityY": 0.10885864717634237,
-            "timestamp": 2.4505794706508084
+            "heading": 0,
+            "angularVelocity": 0.31729069510085045,
+            "velocityX": 0.3272201077973249,
+            "velocityY": 0.10910668200355174,
+            "timestamp": 2.4566364837347034
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
-            "velocityX": -9.011054390829982e-34,
-            "velocityY": 6.746182534290948e-34,
-            "timestamp": 2.4932621268326125
+            "velocityX": 5.5875375349932e-23,
+            "velocityY": -3.709612464642882e-23,
+            "timestamp": 2.499314354346674
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.1.traj
index 8d55d74b..a6fd22bb 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.1.traj	
@@ -3,515 +3,605 @@
         {
             "x": 0.71,
             "y": 4.36,
-            "heading": 2.1,
+            "heading": -1.04,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": 1.2156956395995823e-32,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.7323642464210136,
-            "y": 4.371312176942183,
-            "heading": 2.122836377232617,
-            "angularVelocity": 0.41032613171248355,
-            "velocityX": 0.4018428417574259,
-            "velocityY": 0.20325823832090237,
-            "timestamp": 0.05565421129117423
-        },
-        {
-            "x": 0.7771770070003539,
-            "y": 4.393987109637107,
-            "heading": 2.1669272728918627,
-            "angularVelocity": 0.7922292785458643,
-            "velocityX": 0.8051998139886801,
-            "velocityY": 0.40742528137344314,
-            "timestamp": 0.11130842258234847
-        },
-        {
-            "x": 0.8445465122121998,
-            "y": 4.4280825394621655,
-            "heading": 2.2301328847833437,
-            "angularVelocity": 1.135684262252843,
-            "velocityX": 1.2105014813592279,
-            "velocityY": 0.612629826818977,
-            "timestamp": 0.1669626338735227
-        },
-        {
-            "x": 0.9346100694611125,
-            "y": 4.4736618275321725,
-            "heading": 2.3094955096415575,
-            "angularVelocity": 1.4259949609743032,
-            "velocityX": 1.6182703008351718,
-            "velocityY": 0.8189728506175846,
-            "timestamp": 0.22261684516469693
-        },
-        {
-            "x": 1.047532617883247,
-            "y": 4.530798800054343,
-            "heading": 2.4006762269852384,
-            "angularVelocity": 1.638343536424179,
-            "velocityX": 2.0290027619175355,
-            "velocityY": 1.0266423905144981,
-            "timestamp": 0.27827105645587114
-        },
-        {
-            "x": 1.1834754046687317,
-            "y": 4.5995806121594685,
-            "heading": 2.4961863960775177,
-            "angularVelocity": 1.7161355246341592,
-            "velocityX": 2.4426325273796983,
-            "velocityY": 1.2358779418375128,
-            "timestamp": 0.33392526774704534
-        },
-        {
-            "x": 1.342193634237211,
-            "y": 4.6799334714255165,
-            "heading": 2.578155037608781,
-            "angularVelocity": 1.4728201088398516,
-            "velocityX": 2.851863783282992,
-            "velocityY": 1.4437875841174588,
-            "timestamp": 0.38957947903821954
-        },
-        {
-            "x": 1.518721607083693,
-            "y": 4.772275835878884,
-            "heading": 2.6065642311000707,
-            "angularVelocity": 0.5104590080821917,
-            "velocityX": 3.17187089262509,
-            "velocityY": 1.6592161187991088,
-            "timestamp": 0.44523369032939375
-        },
-        {
-            "x": 1.7067559979094145,
-            "y": 4.866979097734677,
-            "heading": 2.6065642752111695,
-            "angularVelocity": 7.925922929779288e-7,
-            "velocityX": 3.378619271809538,
-            "velocityY": 1.7016369410091843,
-            "timestamp": 0.500887901620568
-        },
-        {
-            "x": 1.894790428298913,
-            "y": 4.9616822810363175,
-            "heading": 2.6065643193220347,
-            "angularVelocity": 7.92588092448351e-7,
-            "velocityX": 3.378619982695133,
-            "velocityY": 1.7016355295409376,
-            "timestamp": 0.5565421129117422
-        },
-        {
-            "x": 2.082824858689142,
-            "y": 5.056385464336507,
-            "heading": 2.6065643634329,
-            "angularVelocity": 7.925880881331428e-7,
-            "velocityX": 3.378619982708264,
-            "velocityY": 1.7016355295148662,
-            "timestamp": 0.6121963242029164
-        },
-        {
-            "x": 2.270859289079371,
-            "y": 5.151088647636696,
-            "heading": 2.606564407543764,
-            "angularVelocity": 7.925880850764007e-7,
-            "velocityX": 3.3786199827082646,
-            "velocityY": 1.7016355295148653,
-            "timestamp": 0.6678505354940906
-        },
-        {
-            "x": 2.4588937194696,
-            "y": 5.245791830936885,
-            "heading": 2.606564451654629,
-            "angularVelocity": 7.92588082043664e-7,
-            "velocityX": 3.378619982708265,
-            "velocityY": 1.7016355295148649,
-            "timestamp": 0.7235047467852648
-        },
-        {
-            "x": 2.6469281498598294,
-            "y": 5.340495014237074,
-            "heading": 2.6065644957654936,
-            "angularVelocity": 7.92588079438066e-7,
-            "velocityX": 3.378619982708265,
-            "velocityY": 1.7016355295148646,
-            "timestamp": 0.779158958076439
-        },
-        {
-            "x": 2.8349625802500587,
-            "y": 5.435198197537264,
-            "heading": 2.606564539876358,
-            "angularVelocity": 7.925880759204128e-7,
-            "velocityX": 3.3786199827082655,
-            "velocityY": 1.7016355295148642,
-            "timestamp": 0.8348131693676132
-        },
-        {
-            "x": 3.022997010640288,
-            "y": 5.529901380837453,
-            "heading": 2.606564583987222,
-            "angularVelocity": 7.925880718883517e-7,
-            "velocityX": 3.3786199827082664,
-            "velocityY": 1.7016355295148637,
-            "timestamp": 0.8904673806587874
-        },
-        {
-            "x": 3.2110314410305167,
-            "y": 5.624604564137642,
-            "heading": 2.6065646280980856,
-            "angularVelocity": 7.925880686816096e-7,
-            "velocityX": 3.3786199827082664,
-            "velocityY": 1.7016355295148633,
-            "timestamp": 0.9461215919499616
-        },
-        {
-            "x": 3.399065871420746,
-            "y": 5.719307747437831,
-            "heading": 2.6065646722089495,
-            "angularVelocity": 7.92588065491887e-7,
-            "velocityX": 3.378619982708267,
-            "velocityY": 1.7016355295148633,
-            "timestamp": 1.001775803241136
-        },
-        {
-            "x": 3.5871003018109753,
-            "y": 5.8140109307380206,
-            "heading": 2.606564716319813,
-            "angularVelocity": 7.92588062190758e-7,
-            "velocityX": 3.378619982708267,
-            "velocityY": 1.7016355295148633,
-            "timestamp": 1.0574300145323101
-        },
-        {
-            "x": 3.7751347322012045,
-            "y": 5.90871411403821,
-            "heading": 2.6065647604306763,
-            "angularVelocity": 7.92588058976371e-7,
-            "velocityX": 3.378619982708267,
-            "velocityY": 1.7016355295148629,
-            "timestamp": 1.1130842258234843
-        },
-        {
-            "x": 3.963169162591434,
-            "y": 6.003417297338399,
-            "heading": 2.6065648045415397,
-            "angularVelocity": 7.925880551764629e-7,
-            "velocityX": 3.3786199827082677,
-            "velocityY": 1.7016355295148622,
-            "timestamp": 1.1687384371146585
-        },
-        {
-            "x": 4.151203592981663,
-            "y": 6.098120480638587,
-            "heading": 2.6065648486524027,
-            "angularVelocity": 7.925880525134244e-7,
-            "velocityX": 3.378619982708268,
-            "velocityY": 1.7016355295148615,
-            "timestamp": 1.2243926484058327
-        },
-        {
-            "x": 4.339238023371892,
-            "y": 6.192823663938777,
-            "heading": 2.606564892763265,
-            "angularVelocity": 7.925880485176538e-7,
-            "velocityX": 3.3786199827082686,
-            "velocityY": 1.701635529514862,
-            "timestamp": 1.280046859697007
-        },
-        {
-            "x": 4.527272453762121,
-            "y": 6.287526847238966,
-            "heading": 2.6065649368741277,
-            "angularVelocity": 7.925880449736749e-7,
-            "velocityX": 3.37861998270827,
-            "velocityY": 1.7016355295148586,
-            "timestamp": 1.3357010709881811
-        },
-        {
-            "x": 4.715306884156566,
-            "y": 6.382230030530785,
-            "heading": 2.6065649809849902,
-            "angularVelocity": 7.925880433141827e-7,
-            "velocityX": 3.3786199827840138,
-            "velocityY": 1.7016355293644703,
-            "timestamp": 1.3913552822793553
-        },
-        {
-            "x": 4.903341542764552,
-            "y": 6.476932760700215,
-            "heading": 2.6065650250958465,
-            "angularVelocity": 7.925879324126063e-7,
-            "velocityX": 3.378624083345972,
-            "velocityY": 1.7016273876195969,
-            "timestamp": 1.4470094935705295
-        },
-        {
-            "x": 5.097883205540706,
-            "y": 6.557426122164244,
-            "heading": 2.6065650695055442,
-            "angularVelocity": 7.97957544404275e-7,
-            "velocityX": 3.495542534208987,
-            "velocityY": 1.4463121405656068,
-            "timestamp": 1.5026637048617038
-        },
-        {
-            "x": 5.300816520591001,
-            "y": 6.613494907557912,
-            "heading": 2.6065651167952333,
-            "angularVelocity": 8.497055004082428e-7,
-            "velocityX": 3.6463245160115534,
-            "velocityY": 1.0074491056988386,
-            "timestamp": 1.558317916152878
-        },
-        {
-            "x": 5.5090879488774736,
-            "y": 6.644295063089685,
-            "heading": 2.606565171305544,
-            "angularVelocity": 9.79446290291877e-7,
-            "velocityX": 3.742240226832587,
-            "velocityY": 0.5534200344809682,
-            "timestamp": 1.6139721274440522
+            "x": 0.7226799898554153,
+            "y": 4.366381680411806,
+            "heading": -1.026991220057009,
+            "angularVelocity": 0.3105337423596819,
+            "velocityX": 0.3026851649532591,
+            "velocityY": 0.15233766037290783,
+            "timestamp": 0.04189167928786116
+        },
+        {
+            "x": 0.7480755502425919,
+            "y": 4.379165077654378,
+            "heading": -1.001621000893582,
+            "angularVelocity": 0.6056147567896238,
+            "velocityX": 0.6062196793943158,
+            "velocityY": 0.3051536118838673,
+            "timestamp": 0.08378335857572232
+        },
+        {
+            "x": 0.7862289943958503,
+            "y": 4.398373013543058,
+            "heading": -0.964694126725153,
+            "angularVelocity": 0.8814846956762871,
+            "velocityX": 0.9107642568130221,
+            "velocityY": 0.4585143449774818,
+            "timestamp": 0.12567503786358347
+        },
+        {
+            "x": 0.8371909372554502,
+            "y": 4.424030289360705,
+            "heading": -0.9172220550063085,
+            "angularVelocity": 1.1332100437568369,
+            "velocityX": 1.2165170679697972,
+            "velocityY": 0.6124671116987632,
+            "timestamp": 0.16756671715144464
+        },
+        {
+            "x": 0.9010212440299602,
+            "y": 4.456163358075127,
+            "heading": -0.8604944995891576,
+            "angularVelocity": 1.354148517832005,
+            "velocityX": 1.523698926841682,
+            "velocityY": 0.767051339565964,
+            "timestamp": 0.2094583964393058
+        },
+        {
+            "x": 0.9777886193671033,
+            "y": 4.494801080149917,
+            "heading": -0.796206518536166,
+            "angularVelocity": 1.5346241102256366,
+            "velocityX": 1.8325208404664681,
+            "velocityY": 0.9223244981249834,
+            "timestamp": 0.25135007572716694
+        },
+        {
+            "x": 1.0675667159355893,
+            "y": 4.539976112871383,
+            "heading": -0.7267376359019178,
+            "angularVelocity": 1.658297872398206,
+            "velocityX": 2.143100923492954,
+            "velocityY": 1.07837722166836,
+            "timestamp": 0.2932417550150281
+        },
+        {
+            "x": 1.1704147696438687,
+            "y": 4.591719023811467,
+            "heading": -0.6558650979752687,
+            "angularVelocity": 1.6918046526529587,
+            "velocityX": 2.4550950321555027,
+            "velocityY": 1.23515962643868,
+            "timestamp": 0.3351334343028892
+        },
+        {
+            "x": 1.286251945776662,
+            "y": 4.64999734523705,
+            "heading": -0.5908433350340152,
+            "angularVelocity": 1.5521402829056499,
+            "velocityX": 2.765159528144268,
+            "velocityY": 1.3911669910657207,
+            "timestamp": 0.37702511359075036
+        },
+        {
+            "x": 1.4132089603720126,
+            "y": 4.714842752574763,
+            "heading": -0.5508840698501896,
+            "angularVelocity": 0.9538711711517488,
+            "velocityX": 3.0306021805179513,
+            "velocityY": 1.547930482617412,
+            "timestamp": 0.4189167928786115
+        },
+        {
+            "x": 1.550306465812279,
+            "y": 4.786404286835006,
+            "heading": -0.541265046364201,
+            "angularVelocity": 0.22961656466170327,
+            "velocityX": 3.272666738857444,
+            "velocityY": 1.7082517453765305,
+            "timestamp": 0.46080847216647264
+        },
+        {
+            "x": 1.6919561664396436,
+            "y": 4.857462558344818,
+            "heading": -0.5412650016128938,
+            "angularVelocity": 0.000001068262432444083,
+            "velocityX": 3.3813325948098276,
+            "velocityY": 1.6962383155263616,
+            "timestamp": 0.5027001514543338
+        },
+        {
+            "x": 1.8336059249814771,
+            "y": 4.928520714406238,
+            "heading": -0.541264956861776,
+            "angularVelocity": 0.0000010682579123299043,
+            "velocityX": 3.3813339772912605,
+            "velocityY": 1.6962355596475316,
+            "timestamp": 0.544591830742195
+        },
+        {
+            "x": 1.9752556835258195,
+            "y": 4.999578870462656,
+            "heading": -0.5412649121106583,
+            "angularVelocity": 0.0000010682579080478101,
+            "velocityX": 3.3813339773511535,
+            "velocityY": 1.696235559528139,
+            "timestamp": 0.5864835100300562
+        },
+        {
+            "x": 2.1169054420701623,
+            "y": 5.0706370265190746,
+            "heading": -0.5412648673595408,
+            "angularVelocity": 0.0000010682579032833847,
+            "velocityX": 3.381333977351157,
+            "velocityY": 1.6962355595281329,
+            "timestamp": 0.6283751893179174
+        },
+        {
+            "x": 2.258555200614505,
+            "y": 5.141695182575493,
+            "heading": -0.5412648226084235,
+            "angularVelocity": 0.0000010682578992307337,
+            "velocityX": 3.381333977351157,
+            "velocityY": 1.6962355595281327,
+            "timestamp": 0.6702668686057786
+        },
+        {
+            "x": 2.400204959158848,
+            "y": 5.212753338631911,
+            "heading": -0.5412647778573064,
+            "angularVelocity": 0.0000010682578937479676,
+            "velocityX": 3.3813339773511575,
+            "velocityY": 1.696235559528132,
+            "timestamp": 0.7121585478936397
+        },
+        {
+            "x": 2.5418547177031905,
+            "y": 5.2838114946883294,
+            "heading": -0.5412647331061895,
+            "angularVelocity": 0.0000010682578893581349,
+            "velocityX": 3.3813339773511584,
+            "velocityY": 1.696235559528132,
+            "timestamp": 0.7540502271815009
+        },
+        {
+            "x": 2.6835044762475335,
+            "y": 5.354869650744748,
+            "heading": -0.5412646883550728,
+            "angularVelocity": 0.0000010682578845109594,
+            "velocityX": 3.3813339773511584,
+            "velocityY": 1.6962355595281313,
+            "timestamp": 0.7959419064693621
+        },
+        {
+            "x": 2.8251542347918766,
+            "y": 5.425927806801167,
+            "heading": -0.5412646436039563,
+            "angularVelocity": 0.0000010682578796288815,
+            "velocityX": 3.3813339773511593,
+            "velocityY": 1.6962355595281313,
+            "timestamp": 0.8378335857572233
+        },
+        {
+            "x": 2.9668039933362196,
+            "y": 5.496985962857585,
+            "heading": -0.5412645988528401,
+            "angularVelocity": 0.0000010682578747880459,
+            "velocityX": 3.38133397735116,
+            "velocityY": 1.6962355595281309,
+            "timestamp": 0.8797252650450845
+        },
+        {
+            "x": 3.108453751880562,
+            "y": 5.5680441189140035,
+            "heading": -0.541264554101724,
+            "angularVelocity": 0.0000010682578699288912,
+            "velocityX": 3.38133397735116,
+            "velocityY": 1.69623555952813,
+            "timestamp": 0.9216169443329457
+        },
+        {
+            "x": 3.250103510424905,
+            "y": 5.639102274970423,
+            "heading": -0.5412645093506081,
+            "angularVelocity": 0.000001068257865104235,
+            "velocityX": 3.3813339773511606,
+            "velocityY": 1.69623555952813,
+            "timestamp": 0.9635086236208069
+        },
+        {
+            "x": 3.3917532689692482,
+            "y": 5.71016043102684,
+            "heading": -0.5412644645994924,
+            "angularVelocity": 0.0000010682578613220535,
+            "velocityX": 3.381333977351161,
+            "velocityY": 1.6962355595281293,
+            "timestamp": 1.005400302908668
+        },
+        {
+            "x": 3.5334030275135913,
+            "y": 5.7812185870832575,
+            "heading": -0.5412644198483769,
+            "angularVelocity": 0.0000010682578558700584,
+            "velocityX": 3.381333977351162,
+            "velocityY": 1.6962355595281287,
+            "timestamp": 1.0472919821965292
+        },
+        {
+            "x": 3.675052786057934,
+            "y": 5.852276743139676,
+            "heading": -0.5412643750972616,
+            "angularVelocity": 0.0000010682578513840427,
+            "velocityX": 3.381333977351162,
+            "velocityY": 1.6962355595281287,
+            "timestamp": 1.0891836614843904
+        },
+        {
+            "x": 3.816702544602277,
+            "y": 5.923334899196093,
+            "heading": -0.5412643303461464,
+            "angularVelocity": 0.0000010682578474926016,
+            "velocityX": 3.381333977351163,
+            "velocityY": 1.6962355595281278,
+            "timestamp": 1.1310753407722516
+        },
+        {
+            "x": 3.95835230314662,
+            "y": 5.994393055252511,
+            "heading": -0.5412642855950315,
+            "angularVelocity": 0.000001068257842976485,
+            "velocityX": 3.3813339773511637,
+            "velocityY": 1.6962355595281275,
+            "timestamp": 1.1729670200601128
+        },
+        {
+            "x": 4.100002061690963,
+            "y": 6.065451211308929,
+            "heading": -0.5412642408439168,
+            "angularVelocity": 0.0000010682578369198447,
+            "velocityX": 3.3813339773511637,
+            "velocityY": 1.6962355595281269,
+            "timestamp": 1.214858699347974
+        },
+        {
+            "x": 4.2416518202353055,
+            "y": 6.136509367365346,
+            "heading": -0.5412641960928023,
+            "angularVelocity": 0.0000010682578319796932,
+            "velocityX": 3.3813339773511646,
+            "velocityY": 1.696235559528127,
+            "timestamp": 1.2567503786358352
+        },
+        {
+            "x": 4.383301578779649,
+            "y": 6.207567523421765,
+            "heading": -0.5412641513416879,
+            "angularVelocity": 0.0000010682578277344439,
+            "velocityX": 3.381333977351165,
+            "velocityY": 1.6962355595281264,
+            "timestamp": 1.2986420579236964
+        },
+        {
+            "x": 4.524951337323992,
+            "y": 6.278625679478183,
+            "heading": -0.5412641065905739,
+            "angularVelocity": 0.0000010682578220378627,
+            "velocityX": 3.3813339773511655,
+            "velocityY": 1.6962355595281264,
+            "timestamp": 1.3405337372115576
+        },
+        {
+            "x": 4.666601095868337,
+            "y": 6.349683835534597,
+            "heading": -0.54126406183946,
+            "angularVelocity": 0.0000010682578180389232,
+            "velocityX": 3.381333977351213,
+            "velocityY": 1.6962355595280314,
+            "timestamp": 1.3824254164994187
+        },
+        {
+            "x": 4.8082508544580715,
+            "y": 6.420741991500528,
+            "heading": -0.5412640170883463,
+            "angularVelocity": 0.000001068257814314864,
+            "velocityX": 3.3813339784347316,
+            "velocityY": 1.6962355573681103,
+            "timestamp": 1.42431709578728
+        },
+        {
+            "x": 4.949901660762853,
+            "y": 6.491798058877407,
+            "heading": -0.54126397233727,
+            "angularVelocity": 0.0000010682569195340074,
+            "velocityX": 3.3813589885337056,
+            "velocityY": 1.6961857004731962,
+            "timestamp": 1.4662087750751411
+        },
+        {
+            "x": 5.0966757016987385,
+            "y": 6.551558383908352,
+            "heading": -0.5412639273749705,
+            "angularVelocity": 0.000001073299046263079,
+            "velocityX": 3.503656177813225,
+            "velocityY": 1.4265440308634512,
+            "timestamp": 1.5081004543630023
+        },
+        {
+            "x": 5.2483360143420885,
+            "y": 6.597526467706988,
+            "heading": -0.5412638804954443,
+            "angularVelocity": 0.000001119065339105663,
+            "velocityX": 3.6202968040790893,
+            "velocityY": 1.097308214425216,
+            "timestamp": 1.5499921336508635
+        },
+        {
+            "x": 5.403589728009299,
+            "y": 6.629310092724924,
+            "heading": -0.5412638294474256,
+            "angularVelocity": 0.0000012185717939310704,
+            "velocityX": 3.7060751993343666,
+            "velocityY": 0.7587097380253649,
+            "timestamp": 1.5918838129387247
+        },
+        {
+            "x": 5.561113217492597,
+            "y": 6.646638205385644,
+            "heading": -0.54126377105557,
+            "angularVelocity": 0.0000013938771748607603,
+            "velocityX": 3.760257219598817,
+            "velocityY": 0.4136409176067717,
+            "timestamp": 1.633775492226586
         },
         {
             "x": 5.7195634841918945,
             "y": 6.649363040924072,
-            "heading": 2.6065652359371767,
-            "angularVelocity": 0.0000011613071303259608,
-            "velocityX": 3.781843825137414,
-            "velocityY": 0.0910618930142201,
-            "timestamp": 1.6696263387352264
-        },
-        {
-            "x": 5.838739218913105,
-            "y": 6.643942809110191,
-            "heading": 2.606565305952835,
-            "angularVelocity": 0.0000022201789880280334,
-            "velocityX": 3.77903269949585,
-            "velocityY": -0.1718741932778599,
-            "timestamp": 1.7011623809444163
-        },
-        {
-            "x": 5.957250522969574,
-            "y": 6.630256808888294,
-            "heading": 2.606565367221186,
-            "angularVelocity": 0.00000194280407444938,
-            "velocityX": 3.7579637695288737,
-            "velocityY": -0.4339796392683931,
-            "timestamp": 1.7326984231536062
-        },
-        {
-            "x": 6.074524798614125,
-            "y": 6.608371177242311,
-            "heading": 2.60656542248837,
-            "angularVelocity": 0.000001752508552376974,
-            "velocityX": 3.718737908410616,
-            "velocityY": -0.693987898063049,
-            "timestamp": 1.7642344653627962
-        },
-        {
-            "x": 6.189995430266423,
-            "y": 6.578391677512442,
-            "heading": 2.606565473629249,
-            "angularVelocity": 0.0000016216644704538906,
-            "velocityX": 3.6615448091532925,
-            "velocityY": -0.9506424278292124,
-            "timestamp": 1.795770507571986
-        },
-        {
-            "x": 6.303104529085719,
-            "y": 6.5404632008330585,
-            "heading": 2.606565522006597,
-            "angularVelocity": 0.000001534033588555687,
-            "velocityX": 3.586661194483535,
-            "velocityY": -1.2027024960136288,
-            "timestamp": 1.827306549781176
-        },
-        {
-            "x": 6.413305655265644,
-            "y": 6.4947691134226035,
-            "heading": 2.6065655686762406,
-            "angularVelocity": 0.000001479882707645635,
-            "velocityX": 3.4944501104140286,
-            "velocityY": -1.4489480673367345,
-            "timestamp": 1.858842591990366
-        },
-        {
-            "x": 6.520066647831161,
-            "y": 6.441530702158678,
-            "heading": 2.6065656145154117,
-            "angularVelocity": 0.000001453548632665294,
-            "velocityX": 3.3853643351100575,
-            "velocityY": -1.6881766871941624,
-            "timestamp": 1.8903786341995559
-        },
-        {
-            "x": 6.6242681306818785,
-            "y": 6.383442071865126,
-            "heading": 2.6065656601861753,
-            "angularVelocity": 0.0000014482084799406948,
-            "velocityX": 3.3042029231034076,
-            "velocityY": -1.8419759178475867,
-            "timestamp": 1.9219146764087458
-        },
-        {
-            "x": 6.728469078482098,
-            "y": 6.325352481789039,
-            "heading": 2.6065657058569283,
-            "angularVelocity": 0.0000014482081475491236,
-            "velocityX": 3.3041859567861076,
-            "velocityY": -1.8420063523113561,
-            "timestamp": 1.9534507186179357
-        },
-        {
-            "x": 6.832670029211484,
-            "y": 6.267262896967356,
-            "heading": 2.6065657515277794,
-            "angularVelocity": 0.0000014482112413699588,
-            "velocityX": 3.3041860496692763,
-            "velocityY": -1.8420061856955277,
-            "timestamp": 1.9849867608271257
-        },
-        {
-            "x": 6.936347380984222,
-            "y": 6.209534885446302,
-            "heading": 2.608700420086438,
-            "angularVelocity": 0.06768980535028488,
-            "velocityX": 3.2875828578934874,
-            "velocityY": -1.8305407868915176,
-            "timestamp": 2.0165228030363154
-        },
-        {
-            "x": 7.0344207678165205,
-            "y": 6.154966008244285,
-            "heading": 2.632443469248245,
-            "angularVelocity": 0.7528861422848001,
-            "velocityX": 3.1098825331898827,
-            "velocityY": -1.730365428865226,
-            "timestamp": 2.0480588452455053
-        },
-        {
-            "x": 7.1267043537102115,
-            "y": 6.103735031776186,
-            "heading": 2.6761348400547162,
-            "angularVelocity": 1.3854424254207447,
-            "velocityX": 2.9262893955285,
-            "velocityY": -1.6245214325965975,
-            "timestamp": 2.0795948874546952
-        },
-        {
-            "x": 7.212236583644832,
-            "y": 6.056223391759097,
-            "heading": 2.7313687787461847,
-            "angularVelocity": 1.7514543621257885,
-            "velocityX": 2.7122055889973176,
-            "velocityY": -1.5065822052725362,
-            "timestamp": 2.111130929663885
-        },
-        {
-            "x": 7.290555468694817,
-            "y": 6.012738407550467,
-            "heading": 2.789179732571192,
-            "angularVelocity": 1.8331708665763002,
-            "velocityX": 2.4834722293453213,
-            "velocityY": -1.3788979580943836,
-            "timestamp": 2.142666971873075
-        },
-        {
-            "x": 7.361635905599927,
-            "y": 5.973292872851013,
-            "heading": 2.8461736255132477,
-            "angularVelocity": 1.8072620706173155,
-            "velocityX": 2.2539428515984796,
-            "velocityY": -1.2508080258708043,
-            "timestamp": 2.174203014082265
-        },
-        {
-            "x": 7.425505527447213,
-            "y": 5.937869720935748,
-            "heading": 2.900420841886284,
-            "angularVelocity": 1.7201656445407778,
-            "velocityX": 2.0252897121209994,
-            "velocityY": -1.1232592752220105,
-            "timestamp": 2.205739056291455
-        },
-        {
-            "x": 7.482197651528883,
-            "y": 5.90644694625493,
-            "heading": 2.9506667136326055,
-            "angularVelocity": 1.5932840085963447,
-            "velocityX": 1.7976930556348818,
-            "velocityY": -0.9964083150440386,
-            "timestamp": 2.237275098500645
-        },
-        {
-            "x": 7.531741810313519,
-            "y": 5.879003134115031,
-            "heading": 2.9960449007561833,
-            "angularVelocity": 1.4389309483596011,
-            "velocityX": 1.5710328663309023,
-            "velocityY": -0.8702364094344716,
-            "timestamp": 2.268811140709835
-        },
-        {
-            "x": 7.5741623718487,
-            "y": 5.855519184527709,
-            "heading": 3.0359339398400818,
-            "angularVelocity": 1.264871438822304,
-            "velocityX": 1.3451453817124663,
-            "velocityY": -0.7446701596714312,
-            "timestamp": 2.3003471829190247
-        },
-        {
-            "x": 7.609479178893684,
-            "y": 5.835978699422795,
-            "heading": 3.0698731008846027,
-            "angularVelocity": 1.076202296388058,
-            "velocityX": 1.1198871060203273,
-            "velocityY": -0.6196238886063927,
-            "timestamp": 2.3318832251282147
-        },
-        {
-            "x": 7.637708552445183,
-            "y": 5.820367770747114,
-            "heading": 3.0975089102886857,
-            "angularVelocity": 0.8763245945944945,
-            "velocityX": 0.895146365046179,
-            "velocityY": -0.4950186384241787,
-            "timestamp": 2.3634192673374046
-        },
-        {
-            "x": 7.658864191629111,
-            "y": 5.808674501363959,
-            "heading": 3.118559676862925,
-            "angularVelocity": 0.6675145357366632,
-            "velocityX": 0.6708400199236877,
-            "velocityY": -0.3707906434672271,
-            "timestamp": 2.3949553095465945
-        },
-        {
-            "x": 7.672957876154406,
-            "y": 5.800888435996016,
-            "heading": 3.1327912648609026,
-            "angularVelocity": 0.45128009100108973,
-            "velocityX": 0.4469072064213547,
-            "velocityY": -0.24689418273527672,
-            "timestamp": 2.4264913517557845
+            "heading": -0.5412637028328066,
+            "angularVelocity": 0.000001628551649307522,
+            "velocityX": 3.7823804009024578,
+            "velocityY": 0.06504479134637085,
+            "timestamp": 1.675667171514447
+        },
+        {
+            "x": 5.8386728592662305,
+            "y": 6.64312635201416,
+            "heading": -0.5412636336326876,
+            "angularVelocity": 0.000002194803906563273,
+            "velocityX": 3.777764046962115,
+            "velocityY": -0.19780759592812253,
+            "timestamp": 1.7071962383364254
+        },
+        {
+            "x": 5.95706146956484,
+            "y": 6.62863229973747,
+            "heading": -0.5412635727602775,
+            "angularVelocity": 0.0000019306759164775456,
+            "velocityX": 3.7549037200201067,
+            "velocityY": -0.459704448549871,
+            "timestamp": 1.7387253051584037
+        },
+        {
+            "x": 6.0741575637122,
+            "y": 6.605950895283392,
+            "heading": -0.5412635175985968,
+            "angularVelocity": 0.000001749550058024173,
+            "velocityX": 3.713909289117771,
+            "velocityY": -0.7193807727372066,
+            "timestamp": 1.770254371980382
+        },
+        {
+            "x": 6.189395638406918,
+            "y": 6.575191700510336,
+            "heading": -0.5412634663439824,
+            "angularVelocity": 0.0000016256305579593937,
+            "velocityX": 3.654978922953296,
+            "velocityY": -0.9755821492190131,
+            "timestamp": 1.8017834388023604
+        },
+        {
+            "x": 6.302219177732501,
+            "y": 6.536503313694644,
+            "heading": -0.5412634176727114,
+            "angularVelocity": 0.0000015436952600896132,
+            "velocityX": 3.578397672300798,
+            "velocityY": -1.2270704691051513,
+            "timestamp": 1.8333125056243387
+        },
+        {
+            "x": 6.412083374588637,
+            "y": 6.49007271186475,
+            "heading": -0.5412633705487488,
+            "angularVelocity": 0.0000014946196396789507,
+            "velocityX": 3.484536902930231,
+            "velocityY": -1.4726284825381617,
+            "timestamp": 1.864841572446317
+        },
+        {
+            "x": 6.518458035998333,
+            "y": 6.436124829047777,
+            "heading": -0.5412633241017745,
+            "angularVelocity": 0.0000014731477658308475,
+            "velocityX": 3.3738601275551834,
+            "velocityY": -1.7110523163142606,
+            "timestamp": 1.8963706392682953
+        },
+        {
+            "x": 6.622870515934444,
+            "y": 6.378470889852871,
+            "heading": -0.5412632777559726,
+            "angularVelocity": 0.0000014699389024112294,
+            "velocityX": 3.3116260790606757,
+            "velocityY": -1.8285964351699866,
+            "timestamp": 1.9278997060902736
+        },
+        {
+            "x": 6.7272827038996414,
+            "y": 6.320816421895741,
+            "heading": -0.5412632314101712,
+            "angularVelocity": 0.0000014699388892769531,
+            "velocityX": 3.311616818687876,
+            "velocityY": -1.8286132057971307,
+            "timestamp": 1.959428772912252
+        },
+        {
+            "x": 6.831694893880165,
+            "y": 6.263161957588452,
+            "heading": -0.5412631850642508,
+            "angularVelocity": 0.0000014699426583817394,
+            "velocityX": 3.311616882607505,
+            "velocityY": -1.828613090035986,
+            "timestamp": 1.9909578397342302
+        },
+        {
+            "x": 6.935462774507961,
+            "y": 6.205910954727142,
+            "heading": -0.5387292297574777,
+            "angularVelocity": 0.08036886473933494,
+            "velocityX": 3.2911814743423053,
+            "velocityY": -1.815816598206477,
+            "timestamp": 2.0224869065562086
+        },
+        {
+            "x": 7.033610221214887,
+            "y": 6.151825220973909,
+            "heading": -0.514724354544779,
+            "angularVelocity": 0.7613569836442236,
+            "velocityX": 3.11291949302188,
+            "velocityY": -1.715424502051262,
+            "timestamp": 2.054015973378187
+        },
+        {
+            "x": 7.126006777010342,
+            "y": 6.101041958255997,
+            "heading": -0.47044950446923295,
+            "angularVelocity": 1.4042550109565184,
+            "velocityX": 2.930519838000589,
+            "velocityY": -1.610680804625417,
+            "timestamp": 2.0855450402001656
+        },
+        {
+            "x": 7.2116589199714145,
+            "y": 6.053937431211348,
+            "heading": -0.4144026036835486,
+            "angularVelocity": 1.7776263757548019,
+            "velocityX": 2.7166088817245364,
+            "velocityY": -1.4940032101366643,
+            "timestamp": 2.117074107022144
+        },
+        {
+            "x": 7.290084520639544,
+            "y": 6.01082680132011,
+            "heading": -0.35575031779267857,
+            "angularVelocity": 1.8602607626174494,
+            "velocityX": 2.4874063387585092,
+            "velocityY": -1.3673297130756235,
+            "timestamp": 2.1486031738441227
+        },
+        {
+            "x": 7.3612581622075055,
+            "y": 5.971723406014521,
+            "heading": -0.2979353118382045,
+            "angularVelocity": 1.8337049517168786,
+            "velocityX": 2.2573976568931324,
+            "velocityY": -1.2402331958119186,
+            "timestamp": 2.1801322406661012
+        },
+        {
+            "x": 7.425208576949436,
+            "y": 5.936609973900717,
+            "heading": -0.2429131400794928,
+            "angularVelocity": 1.7451252861171487,
+            "velocityX": 2.028300269811719,
+            "velocityY": -1.1136844712868785,
+            "timestamp": 2.2116613074880798
+        },
+        {
+            "x": 7.481970224994158,
+            "y": 5.90546408447042,
+            "heading": -0.1919554556036592,
+            "angularVelocity": 1.616212898515343,
+            "velocityX": 1.8002958465346786,
+            "velocityY": -0.9878468527519235,
+            "timestamp": 2.2431903743100583
+        },
+        {
+            "x": 7.531573575573894,
+            "y": 5.8782638409384305,
+            "heading": -0.14593987362659777,
+            "angularVelocity": 1.4594653954358323,
+            "velocityX": 1.5732578087328088,
+            "velocityY": -0.8627037294052898,
+            "timestamp": 2.274719441132037
+        },
+        {
+            "x": 7.574043708984396,
+            "y": 5.854989674792864,
+            "heading": -0.10549518757833008,
+            "angularVelocity": 1.2827745989638484,
+            "velocityX": 1.347015236774987,
+            "velocityY": -0.7381812559495933,
+            "timestamp": 2.3062485079540154
+        },
+        {
+            "x": 7.60940100035129,
+            "y": 5.835624772967359,
+            "heading": -0.07108683169331761,
+            "angularVelocity": 1.091321734299697,
+            "velocityX": 1.1214188978865451,
+            "velocityY": -0.614192038566993,
+            "timestamp": 2.337777574775994
+        },
+        {
+            "x": 7.637662176953836,
+            "y": 5.820154874880602,
+            "heading": -0.043071374084194446,
+            "angularVelocity": 0.8885596826352652,
+            "velocityX": 0.8963530941818176,
+            "velocityY": -0.4906551206892414,
+            "timestamp": 2.3693066415979724
+        },
+        {
+            "x": 7.65884126375683,
+            "y": 5.808567782468078,
+            "heading": -0.021732750340195603,
+            "angularVelocity": 0.6767921126395045,
+            "velocityX": 0.6717321169883477,
+            "velocityY": -0.3675050859559891,
+            "timestamp": 2.400835708419951
+        },
+        {
+            "x": 7.672950320288622,
+            "y": 5.8008527701995645,
+            "heading": -0.007307045765720019,
+            "angularVelocity": 0.45753668054709684,
+            "velocityX": 0.44749362902029066,
+            "velocityY": -0.24469523034329732,
+            "timestamp": 2.4323647752419295
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 0.22858718577555334,
-            "velocityX": 0.2233039833876712,
-            "velocityY": -0.12330133154384845,
-            "timestamp": 2.4580273939649744
+            "heading": 5.476349034092433e-35,
+            "angularVelocity": 0.23175585268595406,
+            "velocityX": 0.22359303404641942,
+            "velocityY": -0.12219740664444498,
+            "timestamp": 2.463893842063908
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 3.1728947336241785e-32,
-            "velocityX": 2.0932493862769896e-33,
-            "velocityY": -2.0877321220317972e-33,
-            "timestamp": 2.4895634361741643
+            "heading": 1.465348958332225e-34,
+            "angularVelocity": 2.9106988947740147e-33,
+            "velocityX": 3.969530612508356e-33,
+            "velocityY": 4.7510565840732025e-33,
+            "timestamp": 2.4954229088858866
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.traj
index 8d55d74b..a6fd22bb 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.traj	
@@ -3,515 +3,605 @@
         {
             "x": 0.71,
             "y": 4.36,
-            "heading": 2.1,
+            "heading": -1.04,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": 1.2156956395995823e-32,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.7323642464210136,
-            "y": 4.371312176942183,
-            "heading": 2.122836377232617,
-            "angularVelocity": 0.41032613171248355,
-            "velocityX": 0.4018428417574259,
-            "velocityY": 0.20325823832090237,
-            "timestamp": 0.05565421129117423
-        },
-        {
-            "x": 0.7771770070003539,
-            "y": 4.393987109637107,
-            "heading": 2.1669272728918627,
-            "angularVelocity": 0.7922292785458643,
-            "velocityX": 0.8051998139886801,
-            "velocityY": 0.40742528137344314,
-            "timestamp": 0.11130842258234847
-        },
-        {
-            "x": 0.8445465122121998,
-            "y": 4.4280825394621655,
-            "heading": 2.2301328847833437,
-            "angularVelocity": 1.135684262252843,
-            "velocityX": 1.2105014813592279,
-            "velocityY": 0.612629826818977,
-            "timestamp": 0.1669626338735227
-        },
-        {
-            "x": 0.9346100694611125,
-            "y": 4.4736618275321725,
-            "heading": 2.3094955096415575,
-            "angularVelocity": 1.4259949609743032,
-            "velocityX": 1.6182703008351718,
-            "velocityY": 0.8189728506175846,
-            "timestamp": 0.22261684516469693
-        },
-        {
-            "x": 1.047532617883247,
-            "y": 4.530798800054343,
-            "heading": 2.4006762269852384,
-            "angularVelocity": 1.638343536424179,
-            "velocityX": 2.0290027619175355,
-            "velocityY": 1.0266423905144981,
-            "timestamp": 0.27827105645587114
-        },
-        {
-            "x": 1.1834754046687317,
-            "y": 4.5995806121594685,
-            "heading": 2.4961863960775177,
-            "angularVelocity": 1.7161355246341592,
-            "velocityX": 2.4426325273796983,
-            "velocityY": 1.2358779418375128,
-            "timestamp": 0.33392526774704534
-        },
-        {
-            "x": 1.342193634237211,
-            "y": 4.6799334714255165,
-            "heading": 2.578155037608781,
-            "angularVelocity": 1.4728201088398516,
-            "velocityX": 2.851863783282992,
-            "velocityY": 1.4437875841174588,
-            "timestamp": 0.38957947903821954
-        },
-        {
-            "x": 1.518721607083693,
-            "y": 4.772275835878884,
-            "heading": 2.6065642311000707,
-            "angularVelocity": 0.5104590080821917,
-            "velocityX": 3.17187089262509,
-            "velocityY": 1.6592161187991088,
-            "timestamp": 0.44523369032939375
-        },
-        {
-            "x": 1.7067559979094145,
-            "y": 4.866979097734677,
-            "heading": 2.6065642752111695,
-            "angularVelocity": 7.925922929779288e-7,
-            "velocityX": 3.378619271809538,
-            "velocityY": 1.7016369410091843,
-            "timestamp": 0.500887901620568
-        },
-        {
-            "x": 1.894790428298913,
-            "y": 4.9616822810363175,
-            "heading": 2.6065643193220347,
-            "angularVelocity": 7.92588092448351e-7,
-            "velocityX": 3.378619982695133,
-            "velocityY": 1.7016355295409376,
-            "timestamp": 0.5565421129117422
-        },
-        {
-            "x": 2.082824858689142,
-            "y": 5.056385464336507,
-            "heading": 2.6065643634329,
-            "angularVelocity": 7.925880881331428e-7,
-            "velocityX": 3.378619982708264,
-            "velocityY": 1.7016355295148662,
-            "timestamp": 0.6121963242029164
-        },
-        {
-            "x": 2.270859289079371,
-            "y": 5.151088647636696,
-            "heading": 2.606564407543764,
-            "angularVelocity": 7.925880850764007e-7,
-            "velocityX": 3.3786199827082646,
-            "velocityY": 1.7016355295148653,
-            "timestamp": 0.6678505354940906
-        },
-        {
-            "x": 2.4588937194696,
-            "y": 5.245791830936885,
-            "heading": 2.606564451654629,
-            "angularVelocity": 7.92588082043664e-7,
-            "velocityX": 3.378619982708265,
-            "velocityY": 1.7016355295148649,
-            "timestamp": 0.7235047467852648
-        },
-        {
-            "x": 2.6469281498598294,
-            "y": 5.340495014237074,
-            "heading": 2.6065644957654936,
-            "angularVelocity": 7.92588079438066e-7,
-            "velocityX": 3.378619982708265,
-            "velocityY": 1.7016355295148646,
-            "timestamp": 0.779158958076439
-        },
-        {
-            "x": 2.8349625802500587,
-            "y": 5.435198197537264,
-            "heading": 2.606564539876358,
-            "angularVelocity": 7.925880759204128e-7,
-            "velocityX": 3.3786199827082655,
-            "velocityY": 1.7016355295148642,
-            "timestamp": 0.8348131693676132
-        },
-        {
-            "x": 3.022997010640288,
-            "y": 5.529901380837453,
-            "heading": 2.606564583987222,
-            "angularVelocity": 7.925880718883517e-7,
-            "velocityX": 3.3786199827082664,
-            "velocityY": 1.7016355295148637,
-            "timestamp": 0.8904673806587874
-        },
-        {
-            "x": 3.2110314410305167,
-            "y": 5.624604564137642,
-            "heading": 2.6065646280980856,
-            "angularVelocity": 7.925880686816096e-7,
-            "velocityX": 3.3786199827082664,
-            "velocityY": 1.7016355295148633,
-            "timestamp": 0.9461215919499616
-        },
-        {
-            "x": 3.399065871420746,
-            "y": 5.719307747437831,
-            "heading": 2.6065646722089495,
-            "angularVelocity": 7.92588065491887e-7,
-            "velocityX": 3.378619982708267,
-            "velocityY": 1.7016355295148633,
-            "timestamp": 1.001775803241136
-        },
-        {
-            "x": 3.5871003018109753,
-            "y": 5.8140109307380206,
-            "heading": 2.606564716319813,
-            "angularVelocity": 7.92588062190758e-7,
-            "velocityX": 3.378619982708267,
-            "velocityY": 1.7016355295148633,
-            "timestamp": 1.0574300145323101
-        },
-        {
-            "x": 3.7751347322012045,
-            "y": 5.90871411403821,
-            "heading": 2.6065647604306763,
-            "angularVelocity": 7.92588058976371e-7,
-            "velocityX": 3.378619982708267,
-            "velocityY": 1.7016355295148629,
-            "timestamp": 1.1130842258234843
-        },
-        {
-            "x": 3.963169162591434,
-            "y": 6.003417297338399,
-            "heading": 2.6065648045415397,
-            "angularVelocity": 7.925880551764629e-7,
-            "velocityX": 3.3786199827082677,
-            "velocityY": 1.7016355295148622,
-            "timestamp": 1.1687384371146585
-        },
-        {
-            "x": 4.151203592981663,
-            "y": 6.098120480638587,
-            "heading": 2.6065648486524027,
-            "angularVelocity": 7.925880525134244e-7,
-            "velocityX": 3.378619982708268,
-            "velocityY": 1.7016355295148615,
-            "timestamp": 1.2243926484058327
-        },
-        {
-            "x": 4.339238023371892,
-            "y": 6.192823663938777,
-            "heading": 2.606564892763265,
-            "angularVelocity": 7.925880485176538e-7,
-            "velocityX": 3.3786199827082686,
-            "velocityY": 1.701635529514862,
-            "timestamp": 1.280046859697007
-        },
-        {
-            "x": 4.527272453762121,
-            "y": 6.287526847238966,
-            "heading": 2.6065649368741277,
-            "angularVelocity": 7.925880449736749e-7,
-            "velocityX": 3.37861998270827,
-            "velocityY": 1.7016355295148586,
-            "timestamp": 1.3357010709881811
-        },
-        {
-            "x": 4.715306884156566,
-            "y": 6.382230030530785,
-            "heading": 2.6065649809849902,
-            "angularVelocity": 7.925880433141827e-7,
-            "velocityX": 3.3786199827840138,
-            "velocityY": 1.7016355293644703,
-            "timestamp": 1.3913552822793553
-        },
-        {
-            "x": 4.903341542764552,
-            "y": 6.476932760700215,
-            "heading": 2.6065650250958465,
-            "angularVelocity": 7.925879324126063e-7,
-            "velocityX": 3.378624083345972,
-            "velocityY": 1.7016273876195969,
-            "timestamp": 1.4470094935705295
-        },
-        {
-            "x": 5.097883205540706,
-            "y": 6.557426122164244,
-            "heading": 2.6065650695055442,
-            "angularVelocity": 7.97957544404275e-7,
-            "velocityX": 3.495542534208987,
-            "velocityY": 1.4463121405656068,
-            "timestamp": 1.5026637048617038
-        },
-        {
-            "x": 5.300816520591001,
-            "y": 6.613494907557912,
-            "heading": 2.6065651167952333,
-            "angularVelocity": 8.497055004082428e-7,
-            "velocityX": 3.6463245160115534,
-            "velocityY": 1.0074491056988386,
-            "timestamp": 1.558317916152878
-        },
-        {
-            "x": 5.5090879488774736,
-            "y": 6.644295063089685,
-            "heading": 2.606565171305544,
-            "angularVelocity": 9.79446290291877e-7,
-            "velocityX": 3.742240226832587,
-            "velocityY": 0.5534200344809682,
-            "timestamp": 1.6139721274440522
+            "x": 0.7226799898554153,
+            "y": 4.366381680411806,
+            "heading": -1.026991220057009,
+            "angularVelocity": 0.3105337423596819,
+            "velocityX": 0.3026851649532591,
+            "velocityY": 0.15233766037290783,
+            "timestamp": 0.04189167928786116
+        },
+        {
+            "x": 0.7480755502425919,
+            "y": 4.379165077654378,
+            "heading": -1.001621000893582,
+            "angularVelocity": 0.6056147567896238,
+            "velocityX": 0.6062196793943158,
+            "velocityY": 0.3051536118838673,
+            "timestamp": 0.08378335857572232
+        },
+        {
+            "x": 0.7862289943958503,
+            "y": 4.398373013543058,
+            "heading": -0.964694126725153,
+            "angularVelocity": 0.8814846956762871,
+            "velocityX": 0.9107642568130221,
+            "velocityY": 0.4585143449774818,
+            "timestamp": 0.12567503786358347
+        },
+        {
+            "x": 0.8371909372554502,
+            "y": 4.424030289360705,
+            "heading": -0.9172220550063085,
+            "angularVelocity": 1.1332100437568369,
+            "velocityX": 1.2165170679697972,
+            "velocityY": 0.6124671116987632,
+            "timestamp": 0.16756671715144464
+        },
+        {
+            "x": 0.9010212440299602,
+            "y": 4.456163358075127,
+            "heading": -0.8604944995891576,
+            "angularVelocity": 1.354148517832005,
+            "velocityX": 1.523698926841682,
+            "velocityY": 0.767051339565964,
+            "timestamp": 0.2094583964393058
+        },
+        {
+            "x": 0.9777886193671033,
+            "y": 4.494801080149917,
+            "heading": -0.796206518536166,
+            "angularVelocity": 1.5346241102256366,
+            "velocityX": 1.8325208404664681,
+            "velocityY": 0.9223244981249834,
+            "timestamp": 0.25135007572716694
+        },
+        {
+            "x": 1.0675667159355893,
+            "y": 4.539976112871383,
+            "heading": -0.7267376359019178,
+            "angularVelocity": 1.658297872398206,
+            "velocityX": 2.143100923492954,
+            "velocityY": 1.07837722166836,
+            "timestamp": 0.2932417550150281
+        },
+        {
+            "x": 1.1704147696438687,
+            "y": 4.591719023811467,
+            "heading": -0.6558650979752687,
+            "angularVelocity": 1.6918046526529587,
+            "velocityX": 2.4550950321555027,
+            "velocityY": 1.23515962643868,
+            "timestamp": 0.3351334343028892
+        },
+        {
+            "x": 1.286251945776662,
+            "y": 4.64999734523705,
+            "heading": -0.5908433350340152,
+            "angularVelocity": 1.5521402829056499,
+            "velocityX": 2.765159528144268,
+            "velocityY": 1.3911669910657207,
+            "timestamp": 0.37702511359075036
+        },
+        {
+            "x": 1.4132089603720126,
+            "y": 4.714842752574763,
+            "heading": -0.5508840698501896,
+            "angularVelocity": 0.9538711711517488,
+            "velocityX": 3.0306021805179513,
+            "velocityY": 1.547930482617412,
+            "timestamp": 0.4189167928786115
+        },
+        {
+            "x": 1.550306465812279,
+            "y": 4.786404286835006,
+            "heading": -0.541265046364201,
+            "angularVelocity": 0.22961656466170327,
+            "velocityX": 3.272666738857444,
+            "velocityY": 1.7082517453765305,
+            "timestamp": 0.46080847216647264
+        },
+        {
+            "x": 1.6919561664396436,
+            "y": 4.857462558344818,
+            "heading": -0.5412650016128938,
+            "angularVelocity": 0.000001068262432444083,
+            "velocityX": 3.3813325948098276,
+            "velocityY": 1.6962383155263616,
+            "timestamp": 0.5027001514543338
+        },
+        {
+            "x": 1.8336059249814771,
+            "y": 4.928520714406238,
+            "heading": -0.541264956861776,
+            "angularVelocity": 0.0000010682579123299043,
+            "velocityX": 3.3813339772912605,
+            "velocityY": 1.6962355596475316,
+            "timestamp": 0.544591830742195
+        },
+        {
+            "x": 1.9752556835258195,
+            "y": 4.999578870462656,
+            "heading": -0.5412649121106583,
+            "angularVelocity": 0.0000010682579080478101,
+            "velocityX": 3.3813339773511535,
+            "velocityY": 1.696235559528139,
+            "timestamp": 0.5864835100300562
+        },
+        {
+            "x": 2.1169054420701623,
+            "y": 5.0706370265190746,
+            "heading": -0.5412648673595408,
+            "angularVelocity": 0.0000010682579032833847,
+            "velocityX": 3.381333977351157,
+            "velocityY": 1.6962355595281329,
+            "timestamp": 0.6283751893179174
+        },
+        {
+            "x": 2.258555200614505,
+            "y": 5.141695182575493,
+            "heading": -0.5412648226084235,
+            "angularVelocity": 0.0000010682578992307337,
+            "velocityX": 3.381333977351157,
+            "velocityY": 1.6962355595281327,
+            "timestamp": 0.6702668686057786
+        },
+        {
+            "x": 2.400204959158848,
+            "y": 5.212753338631911,
+            "heading": -0.5412647778573064,
+            "angularVelocity": 0.0000010682578937479676,
+            "velocityX": 3.3813339773511575,
+            "velocityY": 1.696235559528132,
+            "timestamp": 0.7121585478936397
+        },
+        {
+            "x": 2.5418547177031905,
+            "y": 5.2838114946883294,
+            "heading": -0.5412647331061895,
+            "angularVelocity": 0.0000010682578893581349,
+            "velocityX": 3.3813339773511584,
+            "velocityY": 1.696235559528132,
+            "timestamp": 0.7540502271815009
+        },
+        {
+            "x": 2.6835044762475335,
+            "y": 5.354869650744748,
+            "heading": -0.5412646883550728,
+            "angularVelocity": 0.0000010682578845109594,
+            "velocityX": 3.3813339773511584,
+            "velocityY": 1.6962355595281313,
+            "timestamp": 0.7959419064693621
+        },
+        {
+            "x": 2.8251542347918766,
+            "y": 5.425927806801167,
+            "heading": -0.5412646436039563,
+            "angularVelocity": 0.0000010682578796288815,
+            "velocityX": 3.3813339773511593,
+            "velocityY": 1.6962355595281313,
+            "timestamp": 0.8378335857572233
+        },
+        {
+            "x": 2.9668039933362196,
+            "y": 5.496985962857585,
+            "heading": -0.5412645988528401,
+            "angularVelocity": 0.0000010682578747880459,
+            "velocityX": 3.38133397735116,
+            "velocityY": 1.6962355595281309,
+            "timestamp": 0.8797252650450845
+        },
+        {
+            "x": 3.108453751880562,
+            "y": 5.5680441189140035,
+            "heading": -0.541264554101724,
+            "angularVelocity": 0.0000010682578699288912,
+            "velocityX": 3.38133397735116,
+            "velocityY": 1.69623555952813,
+            "timestamp": 0.9216169443329457
+        },
+        {
+            "x": 3.250103510424905,
+            "y": 5.639102274970423,
+            "heading": -0.5412645093506081,
+            "angularVelocity": 0.000001068257865104235,
+            "velocityX": 3.3813339773511606,
+            "velocityY": 1.69623555952813,
+            "timestamp": 0.9635086236208069
+        },
+        {
+            "x": 3.3917532689692482,
+            "y": 5.71016043102684,
+            "heading": -0.5412644645994924,
+            "angularVelocity": 0.0000010682578613220535,
+            "velocityX": 3.381333977351161,
+            "velocityY": 1.6962355595281293,
+            "timestamp": 1.005400302908668
+        },
+        {
+            "x": 3.5334030275135913,
+            "y": 5.7812185870832575,
+            "heading": -0.5412644198483769,
+            "angularVelocity": 0.0000010682578558700584,
+            "velocityX": 3.381333977351162,
+            "velocityY": 1.6962355595281287,
+            "timestamp": 1.0472919821965292
+        },
+        {
+            "x": 3.675052786057934,
+            "y": 5.852276743139676,
+            "heading": -0.5412643750972616,
+            "angularVelocity": 0.0000010682578513840427,
+            "velocityX": 3.381333977351162,
+            "velocityY": 1.6962355595281287,
+            "timestamp": 1.0891836614843904
+        },
+        {
+            "x": 3.816702544602277,
+            "y": 5.923334899196093,
+            "heading": -0.5412643303461464,
+            "angularVelocity": 0.0000010682578474926016,
+            "velocityX": 3.381333977351163,
+            "velocityY": 1.6962355595281278,
+            "timestamp": 1.1310753407722516
+        },
+        {
+            "x": 3.95835230314662,
+            "y": 5.994393055252511,
+            "heading": -0.5412642855950315,
+            "angularVelocity": 0.000001068257842976485,
+            "velocityX": 3.3813339773511637,
+            "velocityY": 1.6962355595281275,
+            "timestamp": 1.1729670200601128
+        },
+        {
+            "x": 4.100002061690963,
+            "y": 6.065451211308929,
+            "heading": -0.5412642408439168,
+            "angularVelocity": 0.0000010682578369198447,
+            "velocityX": 3.3813339773511637,
+            "velocityY": 1.6962355595281269,
+            "timestamp": 1.214858699347974
+        },
+        {
+            "x": 4.2416518202353055,
+            "y": 6.136509367365346,
+            "heading": -0.5412641960928023,
+            "angularVelocity": 0.0000010682578319796932,
+            "velocityX": 3.3813339773511646,
+            "velocityY": 1.696235559528127,
+            "timestamp": 1.2567503786358352
+        },
+        {
+            "x": 4.383301578779649,
+            "y": 6.207567523421765,
+            "heading": -0.5412641513416879,
+            "angularVelocity": 0.0000010682578277344439,
+            "velocityX": 3.381333977351165,
+            "velocityY": 1.6962355595281264,
+            "timestamp": 1.2986420579236964
+        },
+        {
+            "x": 4.524951337323992,
+            "y": 6.278625679478183,
+            "heading": -0.5412641065905739,
+            "angularVelocity": 0.0000010682578220378627,
+            "velocityX": 3.3813339773511655,
+            "velocityY": 1.6962355595281264,
+            "timestamp": 1.3405337372115576
+        },
+        {
+            "x": 4.666601095868337,
+            "y": 6.349683835534597,
+            "heading": -0.54126406183946,
+            "angularVelocity": 0.0000010682578180389232,
+            "velocityX": 3.381333977351213,
+            "velocityY": 1.6962355595280314,
+            "timestamp": 1.3824254164994187
+        },
+        {
+            "x": 4.8082508544580715,
+            "y": 6.420741991500528,
+            "heading": -0.5412640170883463,
+            "angularVelocity": 0.000001068257814314864,
+            "velocityX": 3.3813339784347316,
+            "velocityY": 1.6962355573681103,
+            "timestamp": 1.42431709578728
+        },
+        {
+            "x": 4.949901660762853,
+            "y": 6.491798058877407,
+            "heading": -0.54126397233727,
+            "angularVelocity": 0.0000010682569195340074,
+            "velocityX": 3.3813589885337056,
+            "velocityY": 1.6961857004731962,
+            "timestamp": 1.4662087750751411
+        },
+        {
+            "x": 5.0966757016987385,
+            "y": 6.551558383908352,
+            "heading": -0.5412639273749705,
+            "angularVelocity": 0.000001073299046263079,
+            "velocityX": 3.503656177813225,
+            "velocityY": 1.4265440308634512,
+            "timestamp": 1.5081004543630023
+        },
+        {
+            "x": 5.2483360143420885,
+            "y": 6.597526467706988,
+            "heading": -0.5412638804954443,
+            "angularVelocity": 0.000001119065339105663,
+            "velocityX": 3.6202968040790893,
+            "velocityY": 1.097308214425216,
+            "timestamp": 1.5499921336508635
+        },
+        {
+            "x": 5.403589728009299,
+            "y": 6.629310092724924,
+            "heading": -0.5412638294474256,
+            "angularVelocity": 0.0000012185717939310704,
+            "velocityX": 3.7060751993343666,
+            "velocityY": 0.7587097380253649,
+            "timestamp": 1.5918838129387247
+        },
+        {
+            "x": 5.561113217492597,
+            "y": 6.646638205385644,
+            "heading": -0.54126377105557,
+            "angularVelocity": 0.0000013938771748607603,
+            "velocityX": 3.760257219598817,
+            "velocityY": 0.4136409176067717,
+            "timestamp": 1.633775492226586
         },
         {
             "x": 5.7195634841918945,
             "y": 6.649363040924072,
-            "heading": 2.6065652359371767,
-            "angularVelocity": 0.0000011613071303259608,
-            "velocityX": 3.781843825137414,
-            "velocityY": 0.0910618930142201,
-            "timestamp": 1.6696263387352264
-        },
-        {
-            "x": 5.838739218913105,
-            "y": 6.643942809110191,
-            "heading": 2.606565305952835,
-            "angularVelocity": 0.0000022201789880280334,
-            "velocityX": 3.77903269949585,
-            "velocityY": -0.1718741932778599,
-            "timestamp": 1.7011623809444163
-        },
-        {
-            "x": 5.957250522969574,
-            "y": 6.630256808888294,
-            "heading": 2.606565367221186,
-            "angularVelocity": 0.00000194280407444938,
-            "velocityX": 3.7579637695288737,
-            "velocityY": -0.4339796392683931,
-            "timestamp": 1.7326984231536062
-        },
-        {
-            "x": 6.074524798614125,
-            "y": 6.608371177242311,
-            "heading": 2.60656542248837,
-            "angularVelocity": 0.000001752508552376974,
-            "velocityX": 3.718737908410616,
-            "velocityY": -0.693987898063049,
-            "timestamp": 1.7642344653627962
-        },
-        {
-            "x": 6.189995430266423,
-            "y": 6.578391677512442,
-            "heading": 2.606565473629249,
-            "angularVelocity": 0.0000016216644704538906,
-            "velocityX": 3.6615448091532925,
-            "velocityY": -0.9506424278292124,
-            "timestamp": 1.795770507571986
-        },
-        {
-            "x": 6.303104529085719,
-            "y": 6.5404632008330585,
-            "heading": 2.606565522006597,
-            "angularVelocity": 0.000001534033588555687,
-            "velocityX": 3.586661194483535,
-            "velocityY": -1.2027024960136288,
-            "timestamp": 1.827306549781176
-        },
-        {
-            "x": 6.413305655265644,
-            "y": 6.4947691134226035,
-            "heading": 2.6065655686762406,
-            "angularVelocity": 0.000001479882707645635,
-            "velocityX": 3.4944501104140286,
-            "velocityY": -1.4489480673367345,
-            "timestamp": 1.858842591990366
-        },
-        {
-            "x": 6.520066647831161,
-            "y": 6.441530702158678,
-            "heading": 2.6065656145154117,
-            "angularVelocity": 0.000001453548632665294,
-            "velocityX": 3.3853643351100575,
-            "velocityY": -1.6881766871941624,
-            "timestamp": 1.8903786341995559
-        },
-        {
-            "x": 6.6242681306818785,
-            "y": 6.383442071865126,
-            "heading": 2.6065656601861753,
-            "angularVelocity": 0.0000014482084799406948,
-            "velocityX": 3.3042029231034076,
-            "velocityY": -1.8419759178475867,
-            "timestamp": 1.9219146764087458
-        },
-        {
-            "x": 6.728469078482098,
-            "y": 6.325352481789039,
-            "heading": 2.6065657058569283,
-            "angularVelocity": 0.0000014482081475491236,
-            "velocityX": 3.3041859567861076,
-            "velocityY": -1.8420063523113561,
-            "timestamp": 1.9534507186179357
-        },
-        {
-            "x": 6.832670029211484,
-            "y": 6.267262896967356,
-            "heading": 2.6065657515277794,
-            "angularVelocity": 0.0000014482112413699588,
-            "velocityX": 3.3041860496692763,
-            "velocityY": -1.8420061856955277,
-            "timestamp": 1.9849867608271257
-        },
-        {
-            "x": 6.936347380984222,
-            "y": 6.209534885446302,
-            "heading": 2.608700420086438,
-            "angularVelocity": 0.06768980535028488,
-            "velocityX": 3.2875828578934874,
-            "velocityY": -1.8305407868915176,
-            "timestamp": 2.0165228030363154
-        },
-        {
-            "x": 7.0344207678165205,
-            "y": 6.154966008244285,
-            "heading": 2.632443469248245,
-            "angularVelocity": 0.7528861422848001,
-            "velocityX": 3.1098825331898827,
-            "velocityY": -1.730365428865226,
-            "timestamp": 2.0480588452455053
-        },
-        {
-            "x": 7.1267043537102115,
-            "y": 6.103735031776186,
-            "heading": 2.6761348400547162,
-            "angularVelocity": 1.3854424254207447,
-            "velocityX": 2.9262893955285,
-            "velocityY": -1.6245214325965975,
-            "timestamp": 2.0795948874546952
-        },
-        {
-            "x": 7.212236583644832,
-            "y": 6.056223391759097,
-            "heading": 2.7313687787461847,
-            "angularVelocity": 1.7514543621257885,
-            "velocityX": 2.7122055889973176,
-            "velocityY": -1.5065822052725362,
-            "timestamp": 2.111130929663885
-        },
-        {
-            "x": 7.290555468694817,
-            "y": 6.012738407550467,
-            "heading": 2.789179732571192,
-            "angularVelocity": 1.8331708665763002,
-            "velocityX": 2.4834722293453213,
-            "velocityY": -1.3788979580943836,
-            "timestamp": 2.142666971873075
-        },
-        {
-            "x": 7.361635905599927,
-            "y": 5.973292872851013,
-            "heading": 2.8461736255132477,
-            "angularVelocity": 1.8072620706173155,
-            "velocityX": 2.2539428515984796,
-            "velocityY": -1.2508080258708043,
-            "timestamp": 2.174203014082265
-        },
-        {
-            "x": 7.425505527447213,
-            "y": 5.937869720935748,
-            "heading": 2.900420841886284,
-            "angularVelocity": 1.7201656445407778,
-            "velocityX": 2.0252897121209994,
-            "velocityY": -1.1232592752220105,
-            "timestamp": 2.205739056291455
-        },
-        {
-            "x": 7.482197651528883,
-            "y": 5.90644694625493,
-            "heading": 2.9506667136326055,
-            "angularVelocity": 1.5932840085963447,
-            "velocityX": 1.7976930556348818,
-            "velocityY": -0.9964083150440386,
-            "timestamp": 2.237275098500645
-        },
-        {
-            "x": 7.531741810313519,
-            "y": 5.879003134115031,
-            "heading": 2.9960449007561833,
-            "angularVelocity": 1.4389309483596011,
-            "velocityX": 1.5710328663309023,
-            "velocityY": -0.8702364094344716,
-            "timestamp": 2.268811140709835
-        },
-        {
-            "x": 7.5741623718487,
-            "y": 5.855519184527709,
-            "heading": 3.0359339398400818,
-            "angularVelocity": 1.264871438822304,
-            "velocityX": 1.3451453817124663,
-            "velocityY": -0.7446701596714312,
-            "timestamp": 2.3003471829190247
-        },
-        {
-            "x": 7.609479178893684,
-            "y": 5.835978699422795,
-            "heading": 3.0698731008846027,
-            "angularVelocity": 1.076202296388058,
-            "velocityX": 1.1198871060203273,
-            "velocityY": -0.6196238886063927,
-            "timestamp": 2.3318832251282147
-        },
-        {
-            "x": 7.637708552445183,
-            "y": 5.820367770747114,
-            "heading": 3.0975089102886857,
-            "angularVelocity": 0.8763245945944945,
-            "velocityX": 0.895146365046179,
-            "velocityY": -0.4950186384241787,
-            "timestamp": 2.3634192673374046
-        },
-        {
-            "x": 7.658864191629111,
-            "y": 5.808674501363959,
-            "heading": 3.118559676862925,
-            "angularVelocity": 0.6675145357366632,
-            "velocityX": 0.6708400199236877,
-            "velocityY": -0.3707906434672271,
-            "timestamp": 2.3949553095465945
-        },
-        {
-            "x": 7.672957876154406,
-            "y": 5.800888435996016,
-            "heading": 3.1327912648609026,
-            "angularVelocity": 0.45128009100108973,
-            "velocityX": 0.4469072064213547,
-            "velocityY": -0.24689418273527672,
-            "timestamp": 2.4264913517557845
+            "heading": -0.5412637028328066,
+            "angularVelocity": 0.000001628551649307522,
+            "velocityX": 3.7823804009024578,
+            "velocityY": 0.06504479134637085,
+            "timestamp": 1.675667171514447
+        },
+        {
+            "x": 5.8386728592662305,
+            "y": 6.64312635201416,
+            "heading": -0.5412636336326876,
+            "angularVelocity": 0.000002194803906563273,
+            "velocityX": 3.777764046962115,
+            "velocityY": -0.19780759592812253,
+            "timestamp": 1.7071962383364254
+        },
+        {
+            "x": 5.95706146956484,
+            "y": 6.62863229973747,
+            "heading": -0.5412635727602775,
+            "angularVelocity": 0.0000019306759164775456,
+            "velocityX": 3.7549037200201067,
+            "velocityY": -0.459704448549871,
+            "timestamp": 1.7387253051584037
+        },
+        {
+            "x": 6.0741575637122,
+            "y": 6.605950895283392,
+            "heading": -0.5412635175985968,
+            "angularVelocity": 0.000001749550058024173,
+            "velocityX": 3.713909289117771,
+            "velocityY": -0.7193807727372066,
+            "timestamp": 1.770254371980382
+        },
+        {
+            "x": 6.189395638406918,
+            "y": 6.575191700510336,
+            "heading": -0.5412634663439824,
+            "angularVelocity": 0.0000016256305579593937,
+            "velocityX": 3.654978922953296,
+            "velocityY": -0.9755821492190131,
+            "timestamp": 1.8017834388023604
+        },
+        {
+            "x": 6.302219177732501,
+            "y": 6.536503313694644,
+            "heading": -0.5412634176727114,
+            "angularVelocity": 0.0000015436952600896132,
+            "velocityX": 3.578397672300798,
+            "velocityY": -1.2270704691051513,
+            "timestamp": 1.8333125056243387
+        },
+        {
+            "x": 6.412083374588637,
+            "y": 6.49007271186475,
+            "heading": -0.5412633705487488,
+            "angularVelocity": 0.0000014946196396789507,
+            "velocityX": 3.484536902930231,
+            "velocityY": -1.4726284825381617,
+            "timestamp": 1.864841572446317
+        },
+        {
+            "x": 6.518458035998333,
+            "y": 6.436124829047777,
+            "heading": -0.5412633241017745,
+            "angularVelocity": 0.0000014731477658308475,
+            "velocityX": 3.3738601275551834,
+            "velocityY": -1.7110523163142606,
+            "timestamp": 1.8963706392682953
+        },
+        {
+            "x": 6.622870515934444,
+            "y": 6.378470889852871,
+            "heading": -0.5412632777559726,
+            "angularVelocity": 0.0000014699389024112294,
+            "velocityX": 3.3116260790606757,
+            "velocityY": -1.8285964351699866,
+            "timestamp": 1.9278997060902736
+        },
+        {
+            "x": 6.7272827038996414,
+            "y": 6.320816421895741,
+            "heading": -0.5412632314101712,
+            "angularVelocity": 0.0000014699388892769531,
+            "velocityX": 3.311616818687876,
+            "velocityY": -1.8286132057971307,
+            "timestamp": 1.959428772912252
+        },
+        {
+            "x": 6.831694893880165,
+            "y": 6.263161957588452,
+            "heading": -0.5412631850642508,
+            "angularVelocity": 0.0000014699426583817394,
+            "velocityX": 3.311616882607505,
+            "velocityY": -1.828613090035986,
+            "timestamp": 1.9909578397342302
+        },
+        {
+            "x": 6.935462774507961,
+            "y": 6.205910954727142,
+            "heading": -0.5387292297574777,
+            "angularVelocity": 0.08036886473933494,
+            "velocityX": 3.2911814743423053,
+            "velocityY": -1.815816598206477,
+            "timestamp": 2.0224869065562086
+        },
+        {
+            "x": 7.033610221214887,
+            "y": 6.151825220973909,
+            "heading": -0.514724354544779,
+            "angularVelocity": 0.7613569836442236,
+            "velocityX": 3.11291949302188,
+            "velocityY": -1.715424502051262,
+            "timestamp": 2.054015973378187
+        },
+        {
+            "x": 7.126006777010342,
+            "y": 6.101041958255997,
+            "heading": -0.47044950446923295,
+            "angularVelocity": 1.4042550109565184,
+            "velocityX": 2.930519838000589,
+            "velocityY": -1.610680804625417,
+            "timestamp": 2.0855450402001656
+        },
+        {
+            "x": 7.2116589199714145,
+            "y": 6.053937431211348,
+            "heading": -0.4144026036835486,
+            "angularVelocity": 1.7776263757548019,
+            "velocityX": 2.7166088817245364,
+            "velocityY": -1.4940032101366643,
+            "timestamp": 2.117074107022144
+        },
+        {
+            "x": 7.290084520639544,
+            "y": 6.01082680132011,
+            "heading": -0.35575031779267857,
+            "angularVelocity": 1.8602607626174494,
+            "velocityX": 2.4874063387585092,
+            "velocityY": -1.3673297130756235,
+            "timestamp": 2.1486031738441227
+        },
+        {
+            "x": 7.3612581622075055,
+            "y": 5.971723406014521,
+            "heading": -0.2979353118382045,
+            "angularVelocity": 1.8337049517168786,
+            "velocityX": 2.2573976568931324,
+            "velocityY": -1.2402331958119186,
+            "timestamp": 2.1801322406661012
+        },
+        {
+            "x": 7.425208576949436,
+            "y": 5.936609973900717,
+            "heading": -0.2429131400794928,
+            "angularVelocity": 1.7451252861171487,
+            "velocityX": 2.028300269811719,
+            "velocityY": -1.1136844712868785,
+            "timestamp": 2.2116613074880798
+        },
+        {
+            "x": 7.481970224994158,
+            "y": 5.90546408447042,
+            "heading": -0.1919554556036592,
+            "angularVelocity": 1.616212898515343,
+            "velocityX": 1.8002958465346786,
+            "velocityY": -0.9878468527519235,
+            "timestamp": 2.2431903743100583
+        },
+        {
+            "x": 7.531573575573894,
+            "y": 5.8782638409384305,
+            "heading": -0.14593987362659777,
+            "angularVelocity": 1.4594653954358323,
+            "velocityX": 1.5732578087328088,
+            "velocityY": -0.8627037294052898,
+            "timestamp": 2.274719441132037
+        },
+        {
+            "x": 7.574043708984396,
+            "y": 5.854989674792864,
+            "heading": -0.10549518757833008,
+            "angularVelocity": 1.2827745989638484,
+            "velocityX": 1.347015236774987,
+            "velocityY": -0.7381812559495933,
+            "timestamp": 2.3062485079540154
+        },
+        {
+            "x": 7.60940100035129,
+            "y": 5.835624772967359,
+            "heading": -0.07108683169331761,
+            "angularVelocity": 1.091321734299697,
+            "velocityX": 1.1214188978865451,
+            "velocityY": -0.614192038566993,
+            "timestamp": 2.337777574775994
+        },
+        {
+            "x": 7.637662176953836,
+            "y": 5.820154874880602,
+            "heading": -0.043071374084194446,
+            "angularVelocity": 0.8885596826352652,
+            "velocityX": 0.8963530941818176,
+            "velocityY": -0.4906551206892414,
+            "timestamp": 2.3693066415979724
+        },
+        {
+            "x": 7.65884126375683,
+            "y": 5.808567782468078,
+            "heading": -0.021732750340195603,
+            "angularVelocity": 0.6767921126395045,
+            "velocityX": 0.6717321169883477,
+            "velocityY": -0.3675050859559891,
+            "timestamp": 2.400835708419951
+        },
+        {
+            "x": 7.672950320288622,
+            "y": 5.8008527701995645,
+            "heading": -0.007307045765720019,
+            "angularVelocity": 0.45753668054709684,
+            "velocityX": 0.44749362902029066,
+            "velocityY": -0.24469523034329732,
+            "timestamp": 2.4323647752419295
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 0.22858718577555334,
-            "velocityX": 0.2233039833876712,
-            "velocityY": -0.12330133154384845,
-            "timestamp": 2.4580273939649744
+            "heading": 5.476349034092433e-35,
+            "angularVelocity": 0.23175585268595406,
+            "velocityX": 0.22359303404641942,
+            "velocityY": -0.12219740664444498,
+            "timestamp": 2.463893842063908
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 3.1728947336241785e-32,
-            "velocityX": 2.0932493862769896e-33,
-            "velocityY": -2.0877321220317972e-33,
-            "timestamp": 2.4895634361741643
+            "heading": 1.465348958332225e-34,
+            "angularVelocity": 2.9106988947740147e-33,
+            "velocityX": 3.969530612508356e-33,
+            "velocityY": 4.7510565840732025e-33,
+            "timestamp": 2.4954229088858866
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.1.traj
index c3926d1c..49c665b8 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.1.traj	
@@ -3,272 +3,272 @@
         {
             "x": 0.71,
             "y": 4.36,
-            "heading": 2.1,
+            "heading": -1.04,
             "angularVelocity": 0,
-            "velocityX": 1.276910697826558e-30,
-            "velocityY": -1.1746574795316565e-31,
+            "velocityX": -1.418990828065155e-30,
+            "velocityY": 1.8798559266210403e-30,
             "timestamp": 0
         },
         {
-            "x": 0.761216454009274,
-            "y": 4.38433482667126,
-            "heading": 2.165113129644946,
-            "angularVelocity": 0.7716161812421117,
-            "velocityX": 0.6069351123943075,
-            "velocityY": 0.28837726169296907,
-            "timestamp": 0.08438538644967974
+            "x": 0.7612279700458028,
+            "y": 4.384337560333447,
+            "heading": -0.9750502376080931,
+            "angularVelocity": 0.7696859194569483,
+            "velocityX": 0.6070760811828038,
+            "velocityY": 0.28841179417367857,
+            "timestamp": 0.0843847610434429
         },
         {
-            "x": 0.8642342955307559,
-            "y": 4.433284699389541,
-            "heading": 2.2862365234671036,
-            "angularVelocity": 1.435359828498015,
-            "velocityX": 1.2208019167265607,
-            "velocityY": 0.58007523313856,
-            "timestamp": 0.1687707728993595
+            "x": 0.8642667385584091,
+            "y": 4.433290258850185,
+            "heading": -0.8542284973914237,
+            "angularVelocity": 1.4317957261788106,
+            "velocityX": 1.2210589594435701,
+            "velocityY": 0.5801130193585202,
+            "timestamp": 0.1687695220868858
         },
         {
-            "x": 1.0198907802831574,
-            "y": 4.507085180578877,
-            "heading": 2.44934046785083,
-            "angularVelocity": 1.9328458545483174,
-            "velocityX": 1.844590530437629,
-            "velocityY": 0.8745647119048293,
-            "timestamp": 0.25315615934903923
+            "x": 1.0199495140663002,
+            "y": 4.507092326164681,
+            "heading": -0.6915219404263555,
+            "angularVelocity": 1.9281509475538106,
+            "velocityX": 1.8449157594667183,
+            "velocityY": 0.8745899899690757,
+            "timestamp": 0.25315428313032873
         },
         {
-            "x": 1.22914875569361,
-            "y": 4.606070154859929,
-            "heading": 2.62757889489394,
-            "angularVelocity": 2.112195423189735,
-            "velocityX": 2.4797892646404183,
-            "velocityY": 1.1730108546707563,
-            "timestamp": 0.337541545798719
+            "x": 1.2292345273519185,
+            "y": 4.606077015104397,
+            "heading": -0.5137189126301165,
+            "angularVelocity": 2.1070513869757743,
+            "velocityX": 2.480128055086372,
+            "velocityY": 1.173016166850306,
+            "timestamp": 0.3375390441737716
         },
         {
-            "x": 1.4854756632487267,
-            "y": 4.729647702991699,
-            "heading": 2.7127404908574917,
-            "angularVelocity": 1.0091983878551618,
-            "velocityX": 3.0375746126133176,
-            "velocityY": 1.4644425217566792,
-            "timestamp": 0.4219269322483987
+            "x": 1.485641598676529,
+            "y": 4.729632973105035,
+            "heading": -0.42871970592241276,
+            "angularVelocity": 1.007281476615537,
+            "velocityX": 3.038547104406527,
+            "velocityY": 1.4641975218379706,
+            "timestamp": 0.4219238052172145
         },
         {
-            "x": 1.7744042397686677,
-            "y": 4.865385720511678,
-            "heading": 2.7127405410221264,
-            "angularVelocity": 5.944706378546851e-7,
-            "velocityX": 3.4239172050510245,
-            "velocityY": 1.6085488640966181,
-            "timestamp": 0.5063123186980785
+            "x": 1.7745745246623985,
+            "y": 4.865356167675212,
+            "heading": -0.42871965586223265,
+            "angularVelocity": 5.932372081830595e-7,
+            "velocityX": 3.4239941242128182,
+            "velocityY": 1.6083851265555307,
+            "timestamp": 0.5063085662606575
         },
         {
-            "x": 2.0633325652631687,
-            "y": 5.001124272354809,
-            "heading": 2.712740591190551,
-            "angularVelocity": 5.945155495227871e-7,
-            "velocityX": 3.423914230300861,
-            "velocityY": 1.608555196036035,
-            "timestamp": 0.5906977051477582
+            "x": 2.063508085893233,
+            "y": 5.001078009918069,
+            "heading": -0.4287196058112515,
+            "angularVelocity": 5.931281972756146e-7,
+            "velocityX": 3.4240016521710777,
+            "velocityY": 1.6083691008265977,
+            "timestamp": 0.5906933273041004
         },
         {
-            "x": 2.352241240419818,
-            "y": 5.1369046433555585,
-            "heading": 2.712740641359129,
-            "angularVelocity": 5.945173720540621e-7,
-            "velocityX": 3.4236813660732097,
-            "velocityY": 1.6090507694920106,
-            "timestamp": 0.675083091597438
+            "x": 2.3524294773436862,
+            "y": 5.136825756955775,
+            "heading": -0.4287195557601797,
+            "angularVelocity": 5.931292703362374e-7,
+            "velocityX": 3.423857434421327,
+            "velocityY": 1.6086760851027126,
+            "timestamp": 0.6750780883475433
         },
         {
-            "x": 2.647177658207551,
-            "y": 5.259039982285096,
-            "heading": 2.712740691973867,
-            "angularVelocity": 5.998045355791158e-7,
-            "velocityX": 3.4951124856626223,
-            "velocityY": 1.4473517758000545,
-            "timestamp": 0.7594684780471177
+            "x": 2.647360543237261,
+            "y": 5.258967835966316,
+            "heading": -0.4287195052637089,
+            "angularVelocity": 5.984074633873495e-7,
+            "velocityX": 3.4950749666965533,
+            "velocityY": 1.447442375852424,
+            "timestamp": 0.7594628493909863
         },
         {
-            "x": 2.9596305957770053,
-            "y": 5.324444044648143,
-            "heading": 2.712740749087516,
-            "angularVelocity": 6.768191931683843e-7,
-            "velocityX": 3.7026901305450077,
-            "velocityY": 0.7750638483126594,
-            "timestamp": 0.8438538644967974
+            "x": 2.959809428031792,
+            "y": 5.324379711821066,
+            "heading": -0.42871944828896014,
+            "angularVelocity": 6.751781731100131e-7,
+            "velocityX": 3.702669545200082,
+            "velocityY": 0.7751621862272086,
+            "timestamp": 0.8438476104344292
         },
         {
-            "x": 3.2787911260313356,
-            "y": 5.330853358998101,
-            "heading": 2.712740828995151,
-            "angularVelocity": 9.469368813287509e-7,
-            "velocityX": 3.7821777405102064,
-            "velocityY": 0.0759528944478236,
-            "timestamp": 0.9282392509464772
+            "x": 3.2789674137401184,
+            "y": 5.330797896945005,
+            "heading": -0.4287193686010616,
+            "angularVelocity": 9.443399202542765e-7,
+            "velocityX": 3.782175617514861,
+            "velocityY": 0.0760585802995927,
+            "timestamp": 0.9282323714778722
         },
         {
-            "x": 3.5936179112160946,
-            "y": 5.278046121689486,
-            "heading": 2.7127406682187845,
-            "angularVelocity": -0.000001905263138772926,
-            "velocityX": 3.730821157908589,
-            "velocityY": -0.6257865198035278,
-            "timestamp": 1.012624637396157
+            "x": 3.593793413911528,
+            "y": 5.278000284107237,
+            "heading": -0.42871953037751775,
+            "angularVelocity": -0.000001917128805011847,
+            "velocityX": 3.7308395055989028,
+            "velocityY": -0.625677103128148,
+            "timestamp": 1.0126171325213151
         },
         {
-            "x": 3.893219599027341,
-            "y": 5.167849261276542,
-            "heading": 2.7127405436852854,
-            "angularVelocity": -0.0000014757709178373628,
-            "velocityX": 3.550397769285573,
-            "velocityY": -1.3058761125501486,
-            "timestamp": 1.0970100238458367
+            "x": 3.893396265271842,
+            "y": 5.167813441772834,
+            "heading": -0.42871965571467335,
+            "angularVelocity": -0.0000014853055701792625,
+            "velocityX": 3.550437871193987,
+            "velocityY": -1.3057670718262655,
+            "timestamp": 1.097001893564758
         },
         {
-            "x": 4.167231798281599,
-            "y": 5.0040755200168485,
-            "heading": 2.7127404397173622,
-            "angularVelocity": -0.0000012320607563537726,
-            "velocityX": 3.247152271058871,
-            "velocityY": -1.940783210815242,
-            "timestamp": 1.1813954102955164
+            "x": 4.167411681517661,
+            "y": 5.0040496950416875,
+            "heading": -0.4287197602752934,
+            "angularVelocity": -0.0000012390936318239587,
+            "velocityX": 3.247214459785327,
+            "velocityY": -1.9406791546980178,
+            "timestamp": 1.181386654608201
         },
         {
-            "x": 4.431867133754474,
-            "y": 4.825546352606267,
-            "heading": 2.712740340652136,
-            "angularVelocity": -0.0000011739618702897745,
-            "velocityX": 3.136032749351509,
-            "velocityY": -2.115640810829663,
-            "timestamp": 1.2657807967451962
+            "x": 4.4320204493985145,
+            "y": 4.825485382292427,
+            "heading": -0.42871985986303984,
+            "angularVelocity": -0.0000011801626881351292,
+            "velocityX": 3.1357411528921184,
+            "velocityY": -2.1160729797797915,
+            "timestamp": 1.265771415651644
         },
         {
-            "x": 4.666216273140183,
-            "y": 4.608786081760519,
-            "heading": 2.712740321249029,
-            "angularVelocity": -2.2993444839053286e-7,
-            "velocityX": 2.7771294206901915,
-            "velocityY": -2.5686944145832347,
-            "timestamp": 1.350166183194876
+            "x": 4.666313927400726,
+            "y": 4.608668432226026,
+            "heading": -0.4287198793836171,
+            "angularVelocity": -2.3132822841838536e-7,
+            "velocityX": 2.77649038884734,
+            "velocityY": -2.5693851281356843,
+            "timestamp": 1.350156176695087
         },
         {
-            "x": 4.936653737211477,
-            "y": 4.439174492795635,
-            "heading": 2.7127404800965853,
-            "angularVelocity": 0.0000018824059850759979,
-            "velocityX": 3.2047902539685085,
-            "velocityY": -2.009964000887954,
-            "timestamp": 1.4345515696445557
+            "x": 4.936706954544898,
+            "y": 4.438990462685779,
+            "heading": -0.4287197198154701,
+            "angularVelocity": 0.0000018909592816311697,
+            "velocityX": 3.204287406881873,
+            "velocityY": -2.010765539204243,
+            "timestamp": 1.4345409377385299
         },
         {
-            "x": 5.233678140767672,
-            "y": 4.322208601201109,
-            "heading": 2.7127406283303626,
-            "angularVelocity": 0.0000017566285300639338,
-            "velocityX": 3.5198559377731047,
-            "velocityY": -1.3860917928520893,
-            "timestamp": 1.5189369560942354
+            "x": 5.233776210787073,
+            "y": 4.322144997944839,
+            "heading": -0.42871957094251306,
+            "angularVelocity": 0.0000017642161351814778,
+            "velocityX": 3.5204135506067917,
+            "velocityY": -1.384674949557486,
+            "timestamp": 1.5189256987819728
         },
         {
-            "x": 5.5320155723758075,
-            "y": 4.208633545762192,
-            "heading": 2.7127407751281813,
-            "angularVelocity": 0.000001739611855911675,
-            "velocityX": 3.5354158363194896,
-            "velocityY": -1.3459090515232621,
-            "timestamp": 1.6033223425439151
+            "x": 5.532138329985476,
+            "y": 4.208641466951728,
+            "heading": -0.42871942351676046,
+            "angularVelocity": 0.0000017470660671471722,
+            "velocityX": 3.5357345983929718,
+            "velocityY": -1.3450714274656952,
+            "timestamp": 1.6033104598254158
         },
         {
-            "x": 5.844921751101867,
-            "y": 4.1454334406216455,
-            "heading": 2.712740834966484,
-            "angularVelocity": 7.091074065603108e-7,
-            "velocityX": 3.708061216412759,
-            "velocityY": -0.748946088885059,
-            "timestamp": 1.687707728993595
+            "x": 5.845046534221127,
+            "y": 4.145463344127181,
+            "heading": -0.42871936362457447,
+            "angularVelocity": 7.097512069029325e-7,
+            "velocityX": 3.708112701469523,
+            "velocityY": -0.7486911385809435,
+            "timestamp": 1.6876952208688587
         },
         {
-            "x": 6.164108889380454,
-            "y": 4.140523996019331,
-            "heading": 2.7127408721494533,
-            "angularVelocity": 4.406328022097576e-7,
-            "velocityX": 3.7824930560568397,
-            "velocityY": -0.05817884836341004,
-            "timestamp": 1.7720931154432746
+            "x": 6.164234411105391,
+            "y": 4.140760073731674,
+            "heading": -0.4287193263126739,
+            "angularVelocity": 4.4216396519651743e-7,
+            "velocityX": 3.7825298423246796,
+            "velocityY": -0.055736016049057806,
+            "timestamp": 1.7720799819123016
         },
         {
-            "x": 6.483326965734431,
-            "y": 4.138437864876922,
-            "heading": 2.712740909220737,
-            "angularVelocity": 4.393092878808321e-7,
-            "velocityX": 3.7828596844114597,
-            "velocityY": -0.02472147406577555,
-            "timestamp": 1.8564785018929544
+            "x": 6.483450051427475,
+            "y": 4.138663317552119,
+            "heading": -0.42871928910258295,
+            "angularVelocity": 4.4095747260310005e-7,
+            "velocityX": 3.782858852414673,
+            "velocityY": -0.024847569082867782,
+            "timestamp": 1.8564647429557446
         },
         {
-            "x": 6.802545327798344,
-            "y": 4.1363959875067975,
-            "heading": 2.712740946289213,
-            "angularVelocity": 4.392760075001551e-7,
-            "velocityX": 3.782863070186549,
-            "velocityY": -0.024197049465056585,
-            "timestamp": 1.9408638883426341
+            "x": 6.802665691714121,
+            "y": 4.136566553568004,
+            "heading": -0.4287192518958351,
+            "angularVelocity": 4.4091785477775863e-7,
+            "velocityX": 3.7828588519947024,
+            "velocityY": -0.024847661569458286,
+            "timestamp": 1.9408495039991875
         },
         {
-            "x": 7.094929199701602,
-            "y": 4.132589002725807,
-            "heading": 2.784661628236826,
-            "angularVelocity": 0.8522883519706954,
-            "velocityX": 3.464863813565768,
-            "velocityY": -0.04511426611993158,
-            "timestamp": 2.025249274792314
+            "x": 7.0950029322992965,
+            "y": 4.132696093446151,
+            "heading": -0.35666839777179793,
+            "angularVelocity": 0.853837271482519,
+            "velocityX": 3.4643368893901423,
+            "velocityY": -0.04586681379319509,
+            "timestamp": 2.0252342650426303
         },
         {
-            "x": 7.329645788590582,
-            "y": 4.130189422490391,
-            "heading": 2.9078456854173678,
-            "angularVelocity": 1.4597794993094293,
-            "velocityX": 2.7814838417425016,
-            "velocityY": -0.02843596902640405,
-            "timestamp": 2.1096346612419934
+            "x": 7.329695147990892,
+            "y": 4.13025195968354,
+            "heading": -0.23302792235716066,
+            "angularVelocity": 1.465199093839195,
+            "velocityX": 2.7812156222231823,
+            "velocityY": -0.02896416049909081,
+            "timestamp": 2.109619026086073
         },
         {
-            "x": 7.5051104174827215,
-            "y": 4.128539949714214,
-            "heading": 3.0176429032166108,
-            "angularVelocity": 1.3011401904845583,
-            "velocityX": 2.0793248247641927,
-            "velocityY": -0.01954690077910012,
-            "timestamp": 2.194020047691673
+            "x": 7.505137224924894,
+            "y": 4.1285705338055685,
+            "heading": -0.12281843303988041,
+            "angularVelocity": 1.3060354494646,
+            "velocityX": 2.0790729838492856,
+            "velocityY": -0.019925705271038107,
+            "timestamp": 2.194003787129516
         },
         {
-            "x": 7.621781937801462,
-            "y": 4.1275005025427935,
-            "heading": 3.0976594405390174,
-            "angularVelocity": 0.9482274205158485,
-            "velocityX": 1.3826033775209736,
-            "velocityY": -0.01231785757139544,
-            "timestamp": 2.2784054341413524
+            "x": 7.621791455543556,
+            "y": 4.127510522704197,
+            "heading": -0.04250049300917318,
+            "angularVelocity": 0.9518062152170125,
+            "velocityX": 1.3824087332380297,
+            "velocityY": -0.012561641322782502,
+            "timestamp": 2.278388548172959
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.5017522730222338,
-            "velocityX": 0.6899069216594048,
-            "velocityY": -0.00593115187164541,
-            "timestamp": 2.362790820591032
+            "heading": -1.5179160305718205e-31,
+            "angularVelocity": 0.5036512811515541,
+            "velocityX": 0.6897992449901723,
+            "velocityY": -0.0060499395609808024,
+            "timestamp": 2.362773309216402
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 3.889752365812458e-29,
+            "heading": 0,
+            "angularVelocity": 2.3304076579629275e-30,
             "velocityX": 0,
             "velocityY": 0,
-            "timestamp": 2.4471762070407115
+            "timestamp": 2.447158070259845
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.traj
index c3926d1c..49c665b8 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.traj	
@@ -3,272 +3,272 @@
         {
             "x": 0.71,
             "y": 4.36,
-            "heading": 2.1,
+            "heading": -1.04,
             "angularVelocity": 0,
-            "velocityX": 1.276910697826558e-30,
-            "velocityY": -1.1746574795316565e-31,
+            "velocityX": -1.418990828065155e-30,
+            "velocityY": 1.8798559266210403e-30,
             "timestamp": 0
         },
         {
-            "x": 0.761216454009274,
-            "y": 4.38433482667126,
-            "heading": 2.165113129644946,
-            "angularVelocity": 0.7716161812421117,
-            "velocityX": 0.6069351123943075,
-            "velocityY": 0.28837726169296907,
-            "timestamp": 0.08438538644967974
+            "x": 0.7612279700458028,
+            "y": 4.384337560333447,
+            "heading": -0.9750502376080931,
+            "angularVelocity": 0.7696859194569483,
+            "velocityX": 0.6070760811828038,
+            "velocityY": 0.28841179417367857,
+            "timestamp": 0.0843847610434429
         },
         {
-            "x": 0.8642342955307559,
-            "y": 4.433284699389541,
-            "heading": 2.2862365234671036,
-            "angularVelocity": 1.435359828498015,
-            "velocityX": 1.2208019167265607,
-            "velocityY": 0.58007523313856,
-            "timestamp": 0.1687707728993595
+            "x": 0.8642667385584091,
+            "y": 4.433290258850185,
+            "heading": -0.8542284973914237,
+            "angularVelocity": 1.4317957261788106,
+            "velocityX": 1.2210589594435701,
+            "velocityY": 0.5801130193585202,
+            "timestamp": 0.1687695220868858
         },
         {
-            "x": 1.0198907802831574,
-            "y": 4.507085180578877,
-            "heading": 2.44934046785083,
-            "angularVelocity": 1.9328458545483174,
-            "velocityX": 1.844590530437629,
-            "velocityY": 0.8745647119048293,
-            "timestamp": 0.25315615934903923
+            "x": 1.0199495140663002,
+            "y": 4.507092326164681,
+            "heading": -0.6915219404263555,
+            "angularVelocity": 1.9281509475538106,
+            "velocityX": 1.8449157594667183,
+            "velocityY": 0.8745899899690757,
+            "timestamp": 0.25315428313032873
         },
         {
-            "x": 1.22914875569361,
-            "y": 4.606070154859929,
-            "heading": 2.62757889489394,
-            "angularVelocity": 2.112195423189735,
-            "velocityX": 2.4797892646404183,
-            "velocityY": 1.1730108546707563,
-            "timestamp": 0.337541545798719
+            "x": 1.2292345273519185,
+            "y": 4.606077015104397,
+            "heading": -0.5137189126301165,
+            "angularVelocity": 2.1070513869757743,
+            "velocityX": 2.480128055086372,
+            "velocityY": 1.173016166850306,
+            "timestamp": 0.3375390441737716
         },
         {
-            "x": 1.4854756632487267,
-            "y": 4.729647702991699,
-            "heading": 2.7127404908574917,
-            "angularVelocity": 1.0091983878551618,
-            "velocityX": 3.0375746126133176,
-            "velocityY": 1.4644425217566792,
-            "timestamp": 0.4219269322483987
+            "x": 1.485641598676529,
+            "y": 4.729632973105035,
+            "heading": -0.42871970592241276,
+            "angularVelocity": 1.007281476615537,
+            "velocityX": 3.038547104406527,
+            "velocityY": 1.4641975218379706,
+            "timestamp": 0.4219238052172145
         },
         {
-            "x": 1.7744042397686677,
-            "y": 4.865385720511678,
-            "heading": 2.7127405410221264,
-            "angularVelocity": 5.944706378546851e-7,
-            "velocityX": 3.4239172050510245,
-            "velocityY": 1.6085488640966181,
-            "timestamp": 0.5063123186980785
+            "x": 1.7745745246623985,
+            "y": 4.865356167675212,
+            "heading": -0.42871965586223265,
+            "angularVelocity": 5.932372081830595e-7,
+            "velocityX": 3.4239941242128182,
+            "velocityY": 1.6083851265555307,
+            "timestamp": 0.5063085662606575
         },
         {
-            "x": 2.0633325652631687,
-            "y": 5.001124272354809,
-            "heading": 2.712740591190551,
-            "angularVelocity": 5.945155495227871e-7,
-            "velocityX": 3.423914230300861,
-            "velocityY": 1.608555196036035,
-            "timestamp": 0.5906977051477582
+            "x": 2.063508085893233,
+            "y": 5.001078009918069,
+            "heading": -0.4287196058112515,
+            "angularVelocity": 5.931281972756146e-7,
+            "velocityX": 3.4240016521710777,
+            "velocityY": 1.6083691008265977,
+            "timestamp": 0.5906933273041004
         },
         {
-            "x": 2.352241240419818,
-            "y": 5.1369046433555585,
-            "heading": 2.712740641359129,
-            "angularVelocity": 5.945173720540621e-7,
-            "velocityX": 3.4236813660732097,
-            "velocityY": 1.6090507694920106,
-            "timestamp": 0.675083091597438
+            "x": 2.3524294773436862,
+            "y": 5.136825756955775,
+            "heading": -0.4287195557601797,
+            "angularVelocity": 5.931292703362374e-7,
+            "velocityX": 3.423857434421327,
+            "velocityY": 1.6086760851027126,
+            "timestamp": 0.6750780883475433
         },
         {
-            "x": 2.647177658207551,
-            "y": 5.259039982285096,
-            "heading": 2.712740691973867,
-            "angularVelocity": 5.998045355791158e-7,
-            "velocityX": 3.4951124856626223,
-            "velocityY": 1.4473517758000545,
-            "timestamp": 0.7594684780471177
+            "x": 2.647360543237261,
+            "y": 5.258967835966316,
+            "heading": -0.4287195052637089,
+            "angularVelocity": 5.984074633873495e-7,
+            "velocityX": 3.4950749666965533,
+            "velocityY": 1.447442375852424,
+            "timestamp": 0.7594628493909863
         },
         {
-            "x": 2.9596305957770053,
-            "y": 5.324444044648143,
-            "heading": 2.712740749087516,
-            "angularVelocity": 6.768191931683843e-7,
-            "velocityX": 3.7026901305450077,
-            "velocityY": 0.7750638483126594,
-            "timestamp": 0.8438538644967974
+            "x": 2.959809428031792,
+            "y": 5.324379711821066,
+            "heading": -0.42871944828896014,
+            "angularVelocity": 6.751781731100131e-7,
+            "velocityX": 3.702669545200082,
+            "velocityY": 0.7751621862272086,
+            "timestamp": 0.8438476104344292
         },
         {
-            "x": 3.2787911260313356,
-            "y": 5.330853358998101,
-            "heading": 2.712740828995151,
-            "angularVelocity": 9.469368813287509e-7,
-            "velocityX": 3.7821777405102064,
-            "velocityY": 0.0759528944478236,
-            "timestamp": 0.9282392509464772
+            "x": 3.2789674137401184,
+            "y": 5.330797896945005,
+            "heading": -0.4287193686010616,
+            "angularVelocity": 9.443399202542765e-7,
+            "velocityX": 3.782175617514861,
+            "velocityY": 0.0760585802995927,
+            "timestamp": 0.9282323714778722
         },
         {
-            "x": 3.5936179112160946,
-            "y": 5.278046121689486,
-            "heading": 2.7127406682187845,
-            "angularVelocity": -0.000001905263138772926,
-            "velocityX": 3.730821157908589,
-            "velocityY": -0.6257865198035278,
-            "timestamp": 1.012624637396157
+            "x": 3.593793413911528,
+            "y": 5.278000284107237,
+            "heading": -0.42871953037751775,
+            "angularVelocity": -0.000001917128805011847,
+            "velocityX": 3.7308395055989028,
+            "velocityY": -0.625677103128148,
+            "timestamp": 1.0126171325213151
         },
         {
-            "x": 3.893219599027341,
-            "y": 5.167849261276542,
-            "heading": 2.7127405436852854,
-            "angularVelocity": -0.0000014757709178373628,
-            "velocityX": 3.550397769285573,
-            "velocityY": -1.3058761125501486,
-            "timestamp": 1.0970100238458367
+            "x": 3.893396265271842,
+            "y": 5.167813441772834,
+            "heading": -0.42871965571467335,
+            "angularVelocity": -0.0000014853055701792625,
+            "velocityX": 3.550437871193987,
+            "velocityY": -1.3057670718262655,
+            "timestamp": 1.097001893564758
         },
         {
-            "x": 4.167231798281599,
-            "y": 5.0040755200168485,
-            "heading": 2.7127404397173622,
-            "angularVelocity": -0.0000012320607563537726,
-            "velocityX": 3.247152271058871,
-            "velocityY": -1.940783210815242,
-            "timestamp": 1.1813954102955164
+            "x": 4.167411681517661,
+            "y": 5.0040496950416875,
+            "heading": -0.4287197602752934,
+            "angularVelocity": -0.0000012390936318239587,
+            "velocityX": 3.247214459785327,
+            "velocityY": -1.9406791546980178,
+            "timestamp": 1.181386654608201
         },
         {
-            "x": 4.431867133754474,
-            "y": 4.825546352606267,
-            "heading": 2.712740340652136,
-            "angularVelocity": -0.0000011739618702897745,
-            "velocityX": 3.136032749351509,
-            "velocityY": -2.115640810829663,
-            "timestamp": 1.2657807967451962
+            "x": 4.4320204493985145,
+            "y": 4.825485382292427,
+            "heading": -0.42871985986303984,
+            "angularVelocity": -0.0000011801626881351292,
+            "velocityX": 3.1357411528921184,
+            "velocityY": -2.1160729797797915,
+            "timestamp": 1.265771415651644
         },
         {
-            "x": 4.666216273140183,
-            "y": 4.608786081760519,
-            "heading": 2.712740321249029,
-            "angularVelocity": -2.2993444839053286e-7,
-            "velocityX": 2.7771294206901915,
-            "velocityY": -2.5686944145832347,
-            "timestamp": 1.350166183194876
+            "x": 4.666313927400726,
+            "y": 4.608668432226026,
+            "heading": -0.4287198793836171,
+            "angularVelocity": -2.3132822841838536e-7,
+            "velocityX": 2.77649038884734,
+            "velocityY": -2.5693851281356843,
+            "timestamp": 1.350156176695087
         },
         {
-            "x": 4.936653737211477,
-            "y": 4.439174492795635,
-            "heading": 2.7127404800965853,
-            "angularVelocity": 0.0000018824059850759979,
-            "velocityX": 3.2047902539685085,
-            "velocityY": -2.009964000887954,
-            "timestamp": 1.4345515696445557
+            "x": 4.936706954544898,
+            "y": 4.438990462685779,
+            "heading": -0.4287197198154701,
+            "angularVelocity": 0.0000018909592816311697,
+            "velocityX": 3.204287406881873,
+            "velocityY": -2.010765539204243,
+            "timestamp": 1.4345409377385299
         },
         {
-            "x": 5.233678140767672,
-            "y": 4.322208601201109,
-            "heading": 2.7127406283303626,
-            "angularVelocity": 0.0000017566285300639338,
-            "velocityX": 3.5198559377731047,
-            "velocityY": -1.3860917928520893,
-            "timestamp": 1.5189369560942354
+            "x": 5.233776210787073,
+            "y": 4.322144997944839,
+            "heading": -0.42871957094251306,
+            "angularVelocity": 0.0000017642161351814778,
+            "velocityX": 3.5204135506067917,
+            "velocityY": -1.384674949557486,
+            "timestamp": 1.5189256987819728
         },
         {
-            "x": 5.5320155723758075,
-            "y": 4.208633545762192,
-            "heading": 2.7127407751281813,
-            "angularVelocity": 0.000001739611855911675,
-            "velocityX": 3.5354158363194896,
-            "velocityY": -1.3459090515232621,
-            "timestamp": 1.6033223425439151
+            "x": 5.532138329985476,
+            "y": 4.208641466951728,
+            "heading": -0.42871942351676046,
+            "angularVelocity": 0.0000017470660671471722,
+            "velocityX": 3.5357345983929718,
+            "velocityY": -1.3450714274656952,
+            "timestamp": 1.6033104598254158
         },
         {
-            "x": 5.844921751101867,
-            "y": 4.1454334406216455,
-            "heading": 2.712740834966484,
-            "angularVelocity": 7.091074065603108e-7,
-            "velocityX": 3.708061216412759,
-            "velocityY": -0.748946088885059,
-            "timestamp": 1.687707728993595
+            "x": 5.845046534221127,
+            "y": 4.145463344127181,
+            "heading": -0.42871936362457447,
+            "angularVelocity": 7.097512069029325e-7,
+            "velocityX": 3.708112701469523,
+            "velocityY": -0.7486911385809435,
+            "timestamp": 1.6876952208688587
         },
         {
-            "x": 6.164108889380454,
-            "y": 4.140523996019331,
-            "heading": 2.7127408721494533,
-            "angularVelocity": 4.406328022097576e-7,
-            "velocityX": 3.7824930560568397,
-            "velocityY": -0.05817884836341004,
-            "timestamp": 1.7720931154432746
+            "x": 6.164234411105391,
+            "y": 4.140760073731674,
+            "heading": -0.4287193263126739,
+            "angularVelocity": 4.4216396519651743e-7,
+            "velocityX": 3.7825298423246796,
+            "velocityY": -0.055736016049057806,
+            "timestamp": 1.7720799819123016
         },
         {
-            "x": 6.483326965734431,
-            "y": 4.138437864876922,
-            "heading": 2.712740909220737,
-            "angularVelocity": 4.393092878808321e-7,
-            "velocityX": 3.7828596844114597,
-            "velocityY": -0.02472147406577555,
-            "timestamp": 1.8564785018929544
+            "x": 6.483450051427475,
+            "y": 4.138663317552119,
+            "heading": -0.42871928910258295,
+            "angularVelocity": 4.4095747260310005e-7,
+            "velocityX": 3.782858852414673,
+            "velocityY": -0.024847569082867782,
+            "timestamp": 1.8564647429557446
         },
         {
-            "x": 6.802545327798344,
-            "y": 4.1363959875067975,
-            "heading": 2.712740946289213,
-            "angularVelocity": 4.392760075001551e-7,
-            "velocityX": 3.782863070186549,
-            "velocityY": -0.024197049465056585,
-            "timestamp": 1.9408638883426341
+            "x": 6.802665691714121,
+            "y": 4.136566553568004,
+            "heading": -0.4287192518958351,
+            "angularVelocity": 4.4091785477775863e-7,
+            "velocityX": 3.7828588519947024,
+            "velocityY": -0.024847661569458286,
+            "timestamp": 1.9408495039991875
         },
         {
-            "x": 7.094929199701602,
-            "y": 4.132589002725807,
-            "heading": 2.784661628236826,
-            "angularVelocity": 0.8522883519706954,
-            "velocityX": 3.464863813565768,
-            "velocityY": -0.04511426611993158,
-            "timestamp": 2.025249274792314
+            "x": 7.0950029322992965,
+            "y": 4.132696093446151,
+            "heading": -0.35666839777179793,
+            "angularVelocity": 0.853837271482519,
+            "velocityX": 3.4643368893901423,
+            "velocityY": -0.04586681379319509,
+            "timestamp": 2.0252342650426303
         },
         {
-            "x": 7.329645788590582,
-            "y": 4.130189422490391,
-            "heading": 2.9078456854173678,
-            "angularVelocity": 1.4597794993094293,
-            "velocityX": 2.7814838417425016,
-            "velocityY": -0.02843596902640405,
-            "timestamp": 2.1096346612419934
+            "x": 7.329695147990892,
+            "y": 4.13025195968354,
+            "heading": -0.23302792235716066,
+            "angularVelocity": 1.465199093839195,
+            "velocityX": 2.7812156222231823,
+            "velocityY": -0.02896416049909081,
+            "timestamp": 2.109619026086073
         },
         {
-            "x": 7.5051104174827215,
-            "y": 4.128539949714214,
-            "heading": 3.0176429032166108,
-            "angularVelocity": 1.3011401904845583,
-            "velocityX": 2.0793248247641927,
-            "velocityY": -0.01954690077910012,
-            "timestamp": 2.194020047691673
+            "x": 7.505137224924894,
+            "y": 4.1285705338055685,
+            "heading": -0.12281843303988041,
+            "angularVelocity": 1.3060354494646,
+            "velocityX": 2.0790729838492856,
+            "velocityY": -0.019925705271038107,
+            "timestamp": 2.194003787129516
         },
         {
-            "x": 7.621781937801462,
-            "y": 4.1275005025427935,
-            "heading": 3.0976594405390174,
-            "angularVelocity": 0.9482274205158485,
-            "velocityX": 1.3826033775209736,
-            "velocityY": -0.01231785757139544,
-            "timestamp": 2.2784054341413524
+            "x": 7.621791455543556,
+            "y": 4.127510522704197,
+            "heading": -0.04250049300917318,
+            "angularVelocity": 0.9518062152170125,
+            "velocityX": 1.3824087332380297,
+            "velocityY": -0.012561641322782502,
+            "timestamp": 2.278388548172959
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.5017522730222338,
-            "velocityX": 0.6899069216594048,
-            "velocityY": -0.00593115187164541,
-            "timestamp": 2.362790820591032
+            "heading": -1.5179160305718205e-31,
+            "angularVelocity": 0.5036512811515541,
+            "velocityX": 0.6897992449901723,
+            "velocityY": -0.0060499395609808024,
+            "timestamp": 2.362773309216402
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 3.889752365812458e-29,
+            "heading": 0,
+            "angularVelocity": 2.3304076579629275e-30,
             "velocityX": 0,
             "velocityY": 0,
-            "timestamp": 2.4471762070407115
+            "timestamp": 2.447158070259845
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.1.traj
index 0e00ab8a..00bde21f 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.1.traj	
@@ -3,578 +3,677 @@
         {
             "x": 0.71,
             "y": 4.36,
-            "heading": 2.1,
+            "heading": -1.04,
             "angularVelocity": 0,
-            "velocityX": 4.814287914507576e-35,
-            "velocityY": 2.4063306961726914e-34,
+            "velocityX": 0,
+            "velocityY": 2.687708804701003e-35,
             "timestamp": 0
         },
         {
-            "x": 0.7189874993597535,
-            "y": 4.351109558323372,
-            "heading": 2.1116856791570084,
-            "angularVelocity": 0.2954657391089134,
-            "velocityX": 0.22724380032956057,
-            "velocityY": -0.22478975211418176,
-            "timestamp": 0.039550031053517704
-        },
-        {
-            "x": 0.7369922825493326,
-            "y": 4.3333146846737405,
-            "heading": 2.134597544316128,
-            "angularVelocity": 0.5793134556106004,
-            "velocityX": 0.45524068401401757,
-            "velocityY": -0.4499332409006739,
-            "timestamp": 0.07910006210703541
-        },
-        {
-            "x": 0.7640484897224145,
-            "y": 4.306598997837255,
-            "heading": 2.168179876708578,
-            "angularVelocity": 0.849110139686295,
-            "velocityX": 0.6841007820315073,
-            "velocityY": -0.67549091934555,
-            "timestamp": 0.11865009316055311
-        },
-        {
-            "x": 0.800195666694539,
-            "y": 4.270941917715144,
-            "heading": 2.2117258914758198,
-            "angularVelocity": 1.1010361713323769,
-            "velocityX": 0.9139607734621368,
-            "velocityY": -0.9015689538615157,
-            "timestamp": 0.15820012421407081
-        },
-        {
-            "x": 0.8454794987248587,
-            "y": 4.226316705338956,
-            "heading": 2.264304991238856,
-            "angularVelocity": 1.3294325784950254,
-            "velocityX": 1.14497589063946,
-            "velocityY": -1.1283230679592324,
-            "timestamp": 0.19775015526758852
-        },
-        {
-            "x": 0.8999520283363762,
-            "y": 4.172689027996317,
-            "heading": 2.324662422471035,
-            "angularVelocity": 1.5261032576815274,
-            "velocityX": 1.3773068733576257,
-            "velocityY": -1.3559452651268766,
-            "timestamp": 0.23730018632110622
-        },
-        {
-            "x": 0.963671089377297,
-            "y": 4.110017781664974,
-            "heading": 2.391063304695866,
-            "angularVelocity": 1.6789084725364454,
-            "velocityX": 1.6111001519745567,
-            "velocityY": -1.584606754076596,
-            "timestamp": 0.2768502173746239
-        },
-        {
-            "x": 1.0366970385232381,
-            "y": 4.038262577868419,
-            "heading": 2.4609901353872035,
-            "angularVelocity": 1.7680600704640437,
-            "velocityX": 1.846419514743869,
-            "velocityY": -1.8142894426418532,
-            "timestamp": 0.31640024842814163
-        },
-        {
-            "x": 1.1190717343496346,
-            "y": 3.9574142704891733,
-            "heading": 2.5303614875414775,
-            "angularVelocity": 1.7540151121601602,
-            "velocityX": 2.082797247742485,
-            "velocityY": -2.0442033855762225,
-            "timestamp": 0.35595027948165936
-        },
-        {
-            "x": 1.21066072775428,
-            "y": 3.867689453216591,
-            "heading": 2.590569114237573,
-            "angularVelocity": 1.522315535343692,
-            "velocityX": 2.3157755117994827,
-            "velocityY": -2.268640880488067,
-            "timestamp": 0.3955003105351771
-        },
-        {
-            "x": 1.310358949370952,
-            "y": 3.7710393719100375,
-            "heading": 2.620822405939027,
-            "angularVelocity": 0.764937242666538,
-            "velocityX": 2.5208127265883533,
-            "velocityY": -2.4437422356450176,
-            "timestamp": 0.4350503415886948
-        },
-        {
-            "x": 1.418053038225142,
-            "y": 3.6673441481868845,
-            "heading": 2.6211471035683176,
-            "angularVelocity": 0.008209794547368329,
-            "velocityX": 2.722983673728656,
-            "velocityY": -2.621874647401313,
-            "timestamp": 0.47460037264221255
-        },
-        {
-            "x": 1.5245836891775304,
-            "y": 3.5622918530292758,
-            "heading": 2.6211471537170588,
-            "angularVelocity": 0.0000012679823435232559,
-            "velocityX": 2.6935668092961826,
-            "velocityY": -2.656187425376634,
-            "timestamp": 0.5141504036957303
-        },
-        {
-            "x": 1.6311142725711645,
-            "y": 3.4572394893622254,
-            "heading": 2.621147203865785,
-            "angularVelocity": 0.0000012679819635220034,
-            "velocityX": 2.6935651011115636,
-            "velocityY": -2.6561891575988192,
-            "timestamp": 0.553700434749248
-        },
-        {
-            "x": 1.7376448559612427,
-            "y": 3.3521871256915694,
-            "heading": 2.621147254014511,
-            "angularVelocity": 0.0000012679819538520794,
-            "velocityX": 2.6935651010216595,
-            "velocityY": -2.656189157689989,
-            "timestamp": 0.5932504658027657
-        },
-        {
-            "x": 1.8441754393513208,
-            "y": 3.2471347620209134,
-            "heading": 2.621147304163236,
-            "angularVelocity": 0.000001267981944384733,
-            "velocityX": 2.693565101021656,
-            "velocityY": -2.656189157689993,
-            "timestamp": 0.6328004968562835
-        },
-        {
-            "x": 1.9507060227413988,
-            "y": 3.142082398350257,
-            "heading": 2.621147354311961,
-            "angularVelocity": 0.0000012679819349678633,
-            "velocityX": 2.6935651010216572,
-            "velocityY": -2.656189157689994,
-            "timestamp": 0.6723505279098012
-        },
-        {
-            "x": 2.057236606131477,
-            "y": 3.037030034679601,
-            "heading": 2.6211474044606855,
-            "angularVelocity": 0.0000012679819252039202,
-            "velocityX": 2.693565101021662,
-            "velocityY": -2.6561891576899894,
-            "timestamp": 0.7119005589633189
-        },
-        {
-            "x": 2.1637671895248896,
-            "y": 2.931977671012326,
-            "heading": 2.6211474546094093,
-            "angularVelocity": 0.0000012679819154308074,
-            "velocityX": 2.69356510110597,
-            "velocityY": -2.656189157604496,
-            "timestamp": 0.7514505900168367
-        },
-        {
-            "x": 2.2702978362708315,
-            "y": 2.8269253715890703,
-            "heading": 2.6211475047581403,
-            "angularVelocity": 0.0000012679820770656017,
-            "velocityX": 2.693566702938569,
-            "velocityY": -2.6561875332310767,
-            "timestamp": 0.7910006210703544
-        },
-        {
-            "x": 2.3779994803856632,
-            "y": 2.7230739385096507,
-            "heading": 2.6211475550861425,
-            "angularVelocity": 0.0000012725148543062649,
-            "velocityX": 2.723174704189075,
-            "velocityY": -2.6258243119680853,
-            "timestamp": 0.8305506521238721
+            "x": 0.7140616140612069,
+            "y": 4.355992841184312,
+            "heading": -1.0347167301054676,
+            "angularVelocity": 0.1988304134727778,
+            "velocityX": 0.15285465616518656,
+            "velocityY": -0.15080528916361516,
+            "timestamp": 0.026571739213607736
+        },
+        {
+            "x": 0.7221925258884458,
+            "y": 4.3479754290630535,
+            "heading": -1.0242624006876555,
+            "angularVelocity": 0.39343790535390527,
+            "velocityX": 0.30599848063671325,
+            "velocityY": -0.3017270362623706,
+            "timestamp": 0.05314347842721547
+        },
+        {
+            "x": 0.7344011981721603,
+            "y": 4.335944497876898,
+            "heading": -1.0087613580778494,
+            "angularVelocity": 0.5833657513042195,
+            "velocityX": 0.45946078973491633,
+            "velocityY": -0.4527716868451825,
+            "timestamp": 0.0797152176408232
+        },
+        {
+            "x": 0.7506970627866689,
+            "y": 4.31989642008046,
+            "heading": -0.9883558514269565,
+            "angularVelocity": 0.7679401971717026,
+            "velocityX": 0.6132780576953482,
+            "velocityY": -0.6039528563572185,
+            "timestamp": 0.10628695685443094
+        },
+        {
+            "x": 0.7710907256657638,
+            "y": 4.29982697901967,
+            "heading": -0.963213241169254,
+            "angularVelocity": 0.9462162057057438,
+            "velocityX": 0.7674944690353961,
+            "velocityY": -0.7552927152962536,
+            "timestamp": 0.13285869606803868
+        },
+        {
+            "x": 0.7955941760833964,
+            "y": 4.27573110299105,
+            "heading": -0.9335350310204893,
+            "angularVelocity": 1.1169088297226009,
+            "velocityX": 0.9221620843352277,
+            "velocityY": -0.9068234425649082,
+            "timestamp": 0.1594304352816464
+        },
+        {
+            "x": 0.8242209914920048,
+            "y": 4.247602572989807,
+            "heading": -0.8995682913456332,
+            "angularVelocity": 1.2783032153748224,
+            "velocityX": 1.0773406730541775,
+            "velocityY": -1.0585882156647894,
+            "timestamp": 0.18600217449525414
+        },
+        {
+            "x": 0.856986533327333,
+            "y": 4.215433742997202,
+            "heading": -0.8616204330063739,
+            "angularVelocity": 1.4281285103018635,
+            "velocityX": 1.2330973735640383,
+            "velocityY": -1.210640738794065,
+            "timestamp": 0.21257391370886186
+        },
+        {
+            "x": 0.893908134971617,
+            "y": 4.179215360896215,
+            "heading": -0.8200791656995261,
+            "angularVelocity": 1.5633627506615773,
+            "velocityX": 1.3895063980372095,
+            "velocityY": -1.3630414558050108,
+            "timestamp": 0.2391456529224696
+        },
+        {
+            "x": 0.9350052770321623,
+            "y": 4.138936674970777,
+            "heading": -0.7754414952973147,
+            "angularVelocity": 1.6798926876172218,
+            "velocityX": 1.546648555074586,
+            "velocityY": -1.5158468025612346,
+            "timestamp": 0.2657173921360773
+        },
+        {
+            "x": 0.9802996637151041,
+            "y": 4.094586228803994,
+            "heading": -0.7283605055619151,
+            "angularVelocity": 1.7718444907546276,
+            "velocityX": 1.7046075275248067,
+            "velocityY": -1.669083299751435,
+            "timestamp": 0.29228913134968504
+        },
+        {
+            "x": 1.0298146765069804,
+            "y": 4.046154350498487,
+            "heading": -0.6797316150493184,
+            "angularVelocity": 1.8300981400454652,
+            "velocityX": 1.8634464381059026,
+            "velocityY": -1.8226837888242364,
+            "timestamp": 0.31886087056329276
+        },
+        {
+            "x": 1.0835715343165668,
+            "y": 3.9936405744754793,
+            "heading": -0.6308802721211105,
+            "angularVelocity": 1.8384699072761677,
+            "velocityX": 2.0230839004342207,
+            "velocityY": -1.9763017994740086,
+            "timestamp": 0.3454326097769005
+        },
+        {
+            "x": 1.1415677463510772,
+            "y": 3.937082152581159,
+            "heading": -0.5840793547040092,
+            "angularVelocity": 1.7613042579137594,
+            "velocityX": 2.1826276243449603,
+            "velocityY": -2.12851787531304,
+            "timestamp": 0.3720043489905082
+        },
+        {
+            "x": 1.203636670620202,
+            "y": 3.876777756204273,
+            "heading": -0.5447644235662668,
+            "angularVelocity": 1.4795768851144226,
+            "velocityX": 2.33589994882,
+            "velocityY": -2.2694937614773476,
+            "timestamp": 0.39857608820411594
+        },
+        {
+            "x": 1.2693974629594884,
+            "y": 3.8133907541919347,
+            "heading": -0.5190150912676422,
+            "angularVelocity": 0.969049563960718,
+            "velocityX": 2.4748395959572558,
+            "velocityY": -2.3855044452595306,
+            "timestamp": 0.42514782741772367
+        },
+        {
+            "x": 1.3388335264794184,
+            "y": 3.7468848835898942,
+            "heading": -0.5067181615747146,
+            "angularVelocity": 0.4627822663045784,
+            "velocityX": 2.613154636275026,
+            "velocityY": -2.5028798479243823,
+            "timestamp": 0.4517195666313314
+        },
+        {
+            "x": 1.410439403600934,
+            "y": 3.676338986398213,
+            "heading": -0.5067181155488927,
+            "angularVelocity": 0.0000017321343363764108,
+            "velocityX": 2.6948133332892708,
+            "velocityY": -2.6549220818618244,
+            "timestamp": 0.4782913058449391
+        },
+        {
+            "x": 1.482040293717765,
+            "y": 3.605788027611728,
+            "heading": -0.5067180695571892,
+            "angularVelocity": 0.0000017308503249172533,
+            "velocityX": 2.694625652511417,
+            "velocityY": -2.655112569761896,
+            "timestamp": 0.5048630450585468
+        },
+        {
+            "x": 1.5536411829944505,
+            "y": 3.5352370679725946,
+            "heading": -0.5067180235654862,
+            "angularVelocity": 0.0000017308503100877725,
+            "velocityX": 2.6946256208934063,
+            "velocityY": -2.6551126018504427,
+            "timestamp": 0.5314347842721546
+        },
+        {
+            "x": 1.6252420722709946,
+            "y": 3.4646861083333174,
+            "heading": -0.5067179775737835,
+            "angularVelocity": 0.0000017308502987237163,
+            "velocityX": 2.6946256208880808,
+            "velocityY": -2.6551126018558486,
+            "timestamp": 0.5580065234857624
+        },
+        {
+            "x": 1.6968429615475384,
+            "y": 3.39413514869404,
+            "heading": -0.506717931582081,
+            "angularVelocity": 0.0000017308502871423956,
+            "velocityX": 2.6946256208880808,
+            "velocityY": -2.65511260185585,
+            "timestamp": 0.5845782626993702
+        },
+        {
+            "x": 1.7684438508240825,
+            "y": 3.323584189054763,
+            "heading": -0.5067178855903789,
+            "angularVelocity": 0.0000017308502762001758,
+            "velocityX": 2.694625620888081,
+            "velocityY": -2.65511260185585,
+            "timestamp": 0.611150001912978
+        },
+        {
+            "x": 1.8400447401006266,
+            "y": 3.2530332294154856,
+            "heading": -0.506717839598677,
+            "angularVelocity": 0.000001730850265567339,
+            "velocityX": 2.6946256208880826,
+            "velocityY": -2.6551126018558495,
+            "timestamp": 0.6377217411265858
+        },
+        {
+            "x": 1.9116456293771706,
+            "y": 3.1824822697762087,
+            "heading": -0.5067177936069754,
+            "angularVelocity": 0.0000017308502535085818,
+            "velocityX": 2.694625620888084,
+            "velocityY": -2.65511260185585,
+            "timestamp": 0.6642934803401935
+        },
+        {
+            "x": 1.9832465186537145,
+            "y": 3.1119313101369315,
+            "heading": -0.5067177476152741,
+            "angularVelocity": 0.0000017308502432657092,
+            "velocityX": 2.6946256208880843,
+            "velocityY": -2.65511260185585,
+            "timestamp": 0.6908652195538013
+        },
+        {
+            "x": 2.0548474079302586,
+            "y": 3.041380350497654,
+            "heading": -0.5067177016235731,
+            "angularVelocity": 0.000001730850231529751,
+            "velocityX": 2.694625620888085,
+            "velocityY": -2.6551126018558504,
+            "timestamp": 0.7174369587674091
+        },
+        {
+            "x": 2.1264482972068026,
+            "y": 2.970829390858377,
+            "heading": -0.5067176556318724,
+            "angularVelocity": 0.0000017308502204807436,
+            "velocityX": 2.6946256208880865,
+            "velocityY": -2.6551126018558495,
+            "timestamp": 0.7440086979810169
+        },
+        {
+            "x": 2.1980491864833867,
+            "y": 2.9002784312191405,
+            "heading": -0.5067176096401721,
+            "angularVelocity": 0.0000017308502087562552,
+            "velocityX": 2.694625620889591,
+            "velocityY": -2.6551126018543236,
+            "timestamp": 0.7705804371946247
+        },
+        {
+            "x": 2.2696500759972245,
+            "y": 2.829727471820689,
+            "heading": -0.506717563648472,
+            "angularVelocity": 0.0000017308501985869009,
+            "velocityX": 2.6946256298184115,
+            "velocityY": -2.6551125927926265,
+            "timestamp": 0.7971521764082324
+        },
+        {
+            "x": 2.3412523738653572,
+            "y": 2.7591779417640674,
+            "heading": -0.5067175176565489,
+            "angularVelocity": 0.0000017308585893832803,
+            "velocityX": 2.694678631779742,
+            "velocityY": -2.6550588009870313,
+            "timestamp": 0.8237239156218402
+        },
+        {
+            "x": 2.4158866145337003,
+            "y": 2.6918440324740667,
+            "heading": -0.5067174710515008,
+            "angularVelocity": 0.000001753932919872258,
+            "velocityX": 2.808782672009746,
+            "velocityY": -2.5340422299311864,
+            "timestamp": 0.850295654835448
         },
         {
             "x": 2.4943342208862305,
             "y": 2.6289942264556885,
-            "heading": 2.6211476076664186,
-            "angularVelocity": 0.0000013294623338808287,
-            "velocityX": 2.941457627255649,
-            "velocityY": -2.378751913662396,
-            "timestamp": 0.8701006831773899
-        },
-        {
-            "x": 2.6932881359026166,
-            "y": 2.508539731368457,
-            "heading": 2.6211476558145916,
-            "angularVelocity": 7.831464659210102e-7,
-            "velocityX": 3.236053335597206,
-            "velocityY": -1.9592334766702564,
-            "timestamp": 0.9315811021889849
-        },
-        {
-            "x": 2.899938569108162,
-            "y": 2.401827293887261,
-            "heading": 2.621147703190774,
-            "angularVelocity": 7.705897835721374e-7,
-            "velocityX": 3.361239830954497,
-            "velocityY": -1.7357142192064432,
-            "timestamp": 0.99306152120058
-        },
-        {
-            "x": 3.106589142147435,
-            "y": 2.295115127196865,
-            "heading": 2.6211477505669496,
-            "angularVelocity": 7.705896615935058e-7,
-            "velocityX": 3.361242105397809,
-            "velocityY": -1.7357098147016614,
-            "timestamp": 1.0545419402121752
-        },
-        {
-            "x": 3.313239715188592,
-            "y": 2.188402960510117,
-            "heading": 2.621147797943125,
-            "angularVelocity": 7.705896625567414e-7,
-            "velocityX": 3.361242105428448,
-            "velocityY": -1.735709814642326,
-            "timestamp": 1.1160223592237704
-        },
-        {
-            "x": 3.519890288229749,
-            "y": 2.0816907938233693,
-            "heading": 2.6211478453193005,
-            "angularVelocity": 7.705896648559933e-7,
-            "velocityX": 3.361242105428449,
-            "velocityY": -1.735709814642324,
-            "timestamp": 1.1775027782353655
-        },
-        {
-            "x": 3.726540861270906,
-            "y": 1.9749786271366212,
-            "heading": 2.621147892695476,
-            "angularVelocity": 7.705896659187971e-7,
-            "velocityX": 3.361242105428449,
-            "velocityY": -1.7357098146423233,
-            "timestamp": 1.2389831972469607
-        },
-        {
-            "x": 3.933191434312063,
-            "y": 1.868266460449873,
-            "heading": 2.6211479400716513,
-            "angularVelocity": 7.705896660607392e-7,
-            "velocityX": 3.36124210542845,
-            "velocityY": -1.7357098146423227,
-            "timestamp": 1.3004636162585559
-        },
-        {
-            "x": 4.139842007353606,
-            "y": 1.7615542937638724,
-            "heading": 2.6211479874478267,
-            "angularVelocity": 7.705896681379808e-7,
-            "velocityX": 3.361242105434726,
-            "velocityY": -1.7357098146301677,
-            "timestamp": 1.361944035270151
-        },
-        {
-            "x": 4.3464926090376705,
-            "y": 1.6548421825447925,
-            "heading": 2.621148034824002,
-            "angularVelocity": 7.705896639238046e-7,
-            "velocityX": 3.361242571315101,
-            "velocityY": -1.7357089124417728,
-            "timestamp": 1.4234244542817462
-        },
-        {
-            "x": 4.555173710801168,
-            "y": 1.5521575021408582,
-            "heading": 2.6211480823085673,
-            "angularVelocity": 7.723526622271063e-7,
-            "velocityX": 3.3942693481666213,
-            "velocityY": -1.6702013755724032,
-            "timestamp": 1.4849048732933414
-        },
-        {
-            "x": 4.775821132533127,
-            "y": 1.478627504505032,
-            "heading": 2.621148131521485,
-            "angularVelocity": 8.004649014817169e-7,
-            "velocityX": 3.5889056268524353,
-            "velocityY": -1.1959905091401257,
-            "timestamp": 1.5463852923049366
-        },
-        {
-            "x": 5.004383859827403,
-            "y": 1.4356039834024816,
-            "heading": 2.621148186381714,
-            "angularVelocity": 8.923203458439086e-7,
-            "velocityX": 3.7176507735116338,
-            "velocityY": -0.699792255716998,
-            "timestamp": 1.6078657113165318
+            "heading": -0.5067174224716422,
+            "angularVelocity": 0.000001828252870098291,
+            "velocityX": 2.952294756541798,
+            "velocityY": -2.365287628074865,
+            "timestamp": 0.8768673940490558
+        },
+        {
+            "x": 2.693828949223235,
+            "y": 2.509453130751911,
+            "heading": -0.5067173754009504,
+            "angularVelocity": 7.656472375459629e-7,
+            "velocityX": 3.244961599231293,
+            "velocityY": -1.9444436869204162,
+            "timestamp": 0.9383456957905196
+        },
+        {
+            "x": 2.900420237054028,
+            "y": 2.4026436735913763,
+            "heading": -0.5067173289743672,
+            "angularVelocity": 7.551702291921459e-7,
+            "velocityX": 3.3603935368868174,
+            "velocityY": -1.7373521085488381,
+            "timestamp": 0.9998239975319835
+        },
+        {
+            "x": 3.1070116472391316,
+            "y": 2.295834453089378,
+            "heading": -0.5067172825477892,
+            "angularVelocity": 7.55170142432061e-7,
+            "velocityX": 3.3603955270899863,
+            "velocityY": -1.7373482590844154,
+            "timestamp": 1.0613022992734473
+        },
+        {
+            "x": 3.3136030574258837,
+            "y": 2.1890252325905677,
+            "heading": -0.5067172361212112,
+            "angularVelocity": 7.551701448333524e-7,
+            "velocityX": 3.3603955271167996,
+            "velocityY": -1.737348259032553,
+            "timestamp": 1.1227806010149113
+        },
+        {
+            "x": 3.5201944676126358,
+            "y": 2.0822160120917577,
+            "heading": -0.5067171896946332,
+            "angularVelocity": 7.551701452492318e-7,
+            "velocityX": 3.3603955271167996,
+            "velocityY": -1.737348259032552,
+            "timestamp": 1.1842589027563752
+        },
+        {
+            "x": 3.726785877799388,
+            "y": 1.9754067915929476,
+            "heading": -0.5067171432680552,
+            "angularVelocity": 7.5517014486464e-7,
+            "velocityX": 3.3603955271168005,
+            "velocityY": -1.7373482590325509,
+            "timestamp": 1.2457372044978392
+        },
+        {
+            "x": 3.93337728798614,
+            "y": 1.8685975710941374,
+            "heading": -0.506717096841477,
+            "angularVelocity": 7.551701472516521e-7,
+            "velocityX": 3.360395527116801,
+            "velocityY": -1.7373482590325502,
+            "timestamp": 1.3072155062393032
+        },
+        {
+            "x": 4.139968698173285,
+            "y": 1.761788350596088,
+            "heading": -0.5067170504148988,
+            "angularVelocity": 7.551701467640916e-7,
+            "velocityX": 3.360395527123197,
+            "velocityY": -1.7373482590201774,
+            "timestamp": 1.3686938079807671
+        },
+        {
+            "x": 4.346560137549085,
+            "y": 1.6549791865550314,
+            "heading": -0.5067170039883208,
+            "angularVelocity": 7.55170145558408e-7,
+            "velocityX": 3.36039600190298,
+            "velocityY": -1.737347340696294,
+            "timestamp": 1.430172109722231
+        },
+        {
+            "x": 4.5552180589852185,
+            "y": 1.552265545137979,
+            "heading": -0.5067169574392034,
+            "angularVelocity": 7.571633584219393e-7,
+            "velocityX": 3.3940091955305984,
+            "velocityY": -1.6707299731374539,
+            "timestamp": 1.491650411463695
+        },
+        {
+            "x": 4.775846094749007,
+            "y": 1.478702717632789,
+            "heading": -0.5067169090858323,
+            "angularVelocity": 7.865111701229533e-7,
+            "velocityX": 3.5887138960279197,
+            "velocityY": -1.1965657056459655,
+            "timestamp": 1.553128713205159
+        },
+        {
+            "x": 5.004393851392972,
+            "y": 1.4356429788646896,
+            "heading": -0.5067168550876733,
+            "angularVelocity": 8.783287361409535e-7,
+            "velocityX": 3.717535295706156,
+            "velocityY": -0.7004054690576753,
+            "timestamp": 1.614607014946623
         },
         {
             "x": 5.236664772033691,
             "y": 1.4238771200180054,
-            "heading": 2.6211482490896194,
-            "angularVelocity": 0.0000010199654837334,
-            "velocityX": 3.7781283202133062,
-            "velocityY": -0.1907414356148787,
-            "timestamp": 1.669346130328127
-        },
-        {
-            "x": 5.366075062772529,
-            "y": 1.4271130159839853,
-            "heading": 2.621148315155484,
-            "angularVelocity": 0.0000019306429598932118,
-            "velocityX": 3.781757302483639,
-            "velocityY": 0.09456259722124823,
-            "timestamp": 1.703565748454824
-        },
-        {
-            "x": 5.494873366116125,
-            "y": 1.4400934888882522,
-            "heading": 2.621148371231603,
-            "angularVelocity": 0.00000163871257714578,
-            "velocityX": 3.7638731930533114,
-            "velocityY": 0.3793283974183189,
-            "timestamp": 1.7377853665815208
-        },
-        {
-            "x": 5.622326965929087,
-            "y": 1.4627446781635232,
-            "heading": 2.6211484208273035,
-            "angularVelocity": 0.0000014493353027620846,
-            "velocityX": 3.724576917868275,
-            "velocityY": 0.6619357700429461,
-            "timestamp": 1.7720049847082178
-        },
-        {
-            "x": 5.747710804313215,
-            "y": 1.4949376920061848,
-            "heading": 2.6211484662381075,
-            "angularVelocity": 0.000001327040057798138,
-            "velocityX": 3.6640922736162436,
-            "velocityY": 0.9407765371158774,
-            "timestamp": 1.8062246028349147
-        },
-        {
-            "x": 5.8703116218973515,
-            "y": 1.5364893102203447,
-            "heading": 2.62114850910788,
-            "angularVelocity": 0.000001252783485820186,
-            "velocityX": 3.5827640486872516,
-            "velocityY": 1.2142630598715765,
-            "timestamp": 1.8404442209616116
-        },
-        {
-            "x": 5.989432110510927,
-            "y": 1.5871628422758242,
-            "heading": 2.621148550731445,
-            "angularVelocity": 0.00000121636555358505,
-            "velocityX": 3.481058385062543,
-            "velocityY": 1.4808327745757486,
-            "timestamp": 1.8746638390883086
-        },
-        {
-            "x": 6.104583123843471,
-            "y": 1.6463045521525654,
-            "heading": 2.621148592213924,
-            "angularVelocity": 0.0000012122426079588706,
-            "velocityX": 3.3650583973847925,
-            "velocityY": 1.7282983596652888,
-            "timestamp": 1.9088834572150055
-        },
-        {
-            "x": 6.2197290464685,
-            "y": 1.7054561727682278,
-            "heading": 2.621148633696581,
-            "angularVelocity": 0.0000012122477903456845,
-            "velocityX": 3.3649096316243146,
-            "velocityY": 1.7285879812175449,
-            "timestamp": 1.9431030753417025
-        },
-        {
-            "x": 6.334874968704583,
-            "y": 1.7646077941410194,
-            "heading": 2.6211486751792368,
-            "angularVelocity": 0.0000012122477871677539,
-            "velocityX": 3.364909620258179,
-            "velocityY": 1.7285880033431293,
-            "timestamp": 1.9773226934683994
-        },
-        {
-            "x": 6.450020890940638,
-            "y": 1.8237594155138686,
-            "heading": 2.6211487166618928,
-            "angularVelocity": 0.000001212247782062354,
-            "velocityX": 3.364909620257311,
-            "velocityY": 1.7285880033448198,
-            "timestamp": 2.0115423115950963
-        },
-        {
-            "x": 6.565166813176692,
-            "y": 1.882911036886718,
-            "heading": 2.621148758144549,
-            "angularVelocity": 0.0000012122477787509118,
-            "velocityX": 3.3649096202573117,
-            "velocityY": 1.7285880033448189,
-            "timestamp": 2.0457619297217935
-        },
-        {
-            "x": 6.68031273541274,
-            "y": 1.9420626582595795,
-            "heading": 2.6211487996272047,
-            "angularVelocity": 0.0000012122477734983372,
-            "velocityX": 3.3649096202571287,
-            "velocityY": 1.7285880033451764,
-            "timestamp": 2.0799815478484907
-        },
-        {
-            "x": 6.79545865756654,
-            "y": 2.001214279792547,
-            "heading": 2.6211488411098602,
-            "angularVelocity": 0.0000012122477688304903,
-            "velocityX": 3.3649096178535927,
-            "velocityY": 1.7285880080239562,
-            "timestamp": 2.114201165975188
-        },
-        {
-            "x": 6.910603503099618,
-            "y": 2.0603679970417836,
-            "heading": 2.621148882606884,
-            "angularVelocity": 0.000001212667636144746,
-            "velocityX": 3.3648781557631158,
-            "velocityY": 1.728649251146572,
-            "timestamp": 2.148420784101885
-        },
-        {
-            "x": 7.018224220241307,
-            "y": 2.1177284432893,
-            "heading": 2.639872440534831,
-            "angularVelocity": 0.5471585877616616,
-            "velocityX": 3.1450005299073442,
-            "velocityY": 1.6762444874498934,
-            "timestamp": 2.182640402228582
-        },
-        {
-            "x": 7.119181986421491,
-            "y": 2.170426181119355,
-            "heading": 2.6783971426308466,
-            "angularVelocity": 1.1258074813511751,
-            "velocityX": 2.9502890945887246,
-            "velocityY": 1.5399861458109638,
-            "timestamp": 2.2168600203552793
-        },
-        {
-            "x": 7.213084203441603,
-            "y": 2.2185446189369,
-            "heading": 2.734533820088199,
-            "angularVelocity": 1.6404822885371884,
-            "velocityX": 2.7441047609719855,
-            "velocityY": 1.4061652482324039,
-            "timestamp": 2.2510796384819765
-        },
-        {
-            "x": 7.29840370402033,
-            "y": 2.262230718496702,
-            "heading": 2.7957315713049304,
-            "angularVelocity": 1.788382061721109,
-            "velocityX": 2.493292013453601,
-            "velocityY": 1.2766390144406516,
-            "timestamp": 2.2852992566086736
-        },
-        {
-            "x": 7.375044913403133,
-            "y": 2.301443411516585,
-            "heading": 2.8566177755066406,
-            "angularVelocity": 1.7792777223954108,
-            "velocityX": 2.239686284605582,
-            "velocityY": 1.1459126421194965,
-            "timestamp": 2.3195188747353708
-        },
-        {
-            "x": 7.443040169771806,
-            "y": 2.336203371638747,
-            "heading": 2.9144119075075787,
-            "angularVelocity": 1.6889180874829506,
-            "velocityX": 1.9870255745380796,
-            "velocityY": 1.0157904157043582,
-            "timestamp": 2.353738492862068
-        },
-        {
-            "x": 7.502434435273893,
-            "y": 2.3665382868697598,
-            "heading": 2.967461608910358,
-            "angularVelocity": 1.5502715783199053,
-            "velocityX": 1.7356787934389564,
-            "velocityY": 0.8864773159857819,
-            "timestamp": 2.387958110988765
-        },
-        {
-            "x": 7.553267406161016,
-            "y": 2.3924752684695187,
-            "heading": 3.014662356421872,
-            "angularVelocity": 1.379347581751362,
-            "velocityX": 1.48549205601638,
-            "velocityY": 0.7579564886939567,
-            "timestamp": 2.4221777291154623
-        },
-        {
-            "x": 7.595572130014844,
-            "y": 2.41403971266414,
-            "heading": 3.055206797581657,
-            "angularVelocity": 1.184830321883501,
-            "velocityX": 1.2362710681690243,
-            "velocityY": 0.6301778153917356,
-            "timestamp": 2.4563973472421594
-        },
-        {
-            "x": 7.629376218234675,
-            "y": 2.4312548848965747,
-            "heading": 3.088468435996466,
-            "angularVelocity": 0.9720049560944556,
-            "velocityX": 0.9878569683236498,
-            "velocityY": 0.5030790281965085,
-            "timestamp": 2.4906169653688566
-        },
-        {
-            "x": 7.654702978974042,
-            "y": 2.4441416202766635,
-            "heading": 3.113945152945951,
-            "angularVelocity": 0.7445061734809163,
-            "velocityX": 0.7401240027166469,
-            "velocityY": 0.3765891054767559,
-            "timestamp": 2.5248365834955537
-        },
-        {
-            "x": 7.671572100944035,
-            "y": 2.4527181446432627,
-            "heading": 3.1312307619473794,
-            "angularVelocity": 0.5051374021015854,
-            "velocityX": 0.4929664003711423,
-            "velocityY": 0.250631796498847,
-            "timestamp": 2.559056201622251
+            "heading": -0.5067167933216484,
+            "angularVelocity": 0.000001004680076164895,
+            "velocityX": 3.7780959145145716,
+            "velocityY": -0.19138230096471237,
+            "timestamp": 1.676085316688087
+        },
+        {
+            "x": 5.366036550697627,
+            "y": 1.4270871399969536,
+            "heading": -0.5067167282581706,
+            "angularVelocity": 0.000001901925327561791,
+            "velocityX": 3.781775437099809,
+            "velocityY": 0.09383479793163368,
+            "timestamp": 1.7102945871261934
+        },
+        {
+            "x": 5.494798697340742,
+            "y": 1.4400359689986706,
+            "heading": -0.5067166730622293,
+            "angularVelocity": 0.0000016134790500794753,
+            "velocityX": 3.76395476998197,
+            "velocityY": 0.37851812786083194,
+            "timestamp": 1.7445038575643
+        },
+        {
+            "x": 5.6222191444596294,
+            "y": 1.4626499710733982,
+            "heading": -0.5067166242756445,
+            "angularVelocity": 0.0000014261217576278581,
+            "velocityX": 3.724734421022641,
+            "velocityY": 0.6610489433161765,
+            "timestamp": 1.7787131280024064
+        },
+        {
+            "x": 5.747573461036382,
+            "y": 1.4948005438980272,
+            "heading": -0.5067165796372217,
+            "angularVelocity": 0.0000013048633380814483,
+            "velocityX": 3.6643376187618593,
+            "velocityY": 0.9398204759378852,
+            "timestamp": 1.812922398440513
+        },
+        {
+            "x": 5.870148986660447,
+            "y": 1.5363048197489622,
+            "heading": -0.5067165375293654,
+            "angularVelocity": 0.0000012308902156872765,
+            "velocityX": 3.583108439738183,
+            "velocityY": 1.2132464480944425,
+            "timestamp": 1.8471316688786195
+        },
+        {
+            "x": 5.9892489774115445,
+            "y": 1.5869265226922225,
+            "heading": -0.5067164966802475,
+            "angularVelocity": 0.0000011940949728764683,
+            "velocityX": 3.481512152285735,
+            "velocityY": 1.4797656394002383,
+            "timestamp": 1.881340939316726
+        },
+        {
+            "x": 6.104355729464833,
+            "y": 1.6460687267313168,
+            "heading": -0.5067164560018657,
+            "angularVelocity": 0.0000011891040447074693,
+            "velocityX": 3.3647824282469623,
+            "velocityY": 1.7288355840881788,
+            "timestamp": 1.9155502097548325
+        },
+        {
+            "x": 6.219456425834801,
+            "y": 1.705222715286976,
+            "heading": -0.5067164153233215,
+            "angularVelocity": 0.0000011891087921450935,
+            "velocityX": 3.3646054094668667,
+            "velocityY": 1.7291800672184492,
+            "timestamp": 1.949759480192939
+        },
+        {
+            "x": 6.334557121741728,
+            "y": 1.764376704743614,
+            "heading": -0.5067163746447773,
+            "angularVelocity": 0.0000011891087898511233,
+            "velocityX": 3.3646053959312843,
+            "velocityY": 1.7291800935557256,
+            "timestamp": 1.9839687506310455
+        },
+        {
+            "x": 6.449657817648618,
+            "y": 1.8235306942003213,
+            "heading": -0.5067163339662332,
+            "angularVelocity": 0.000001189108787218646,
+            "velocityX": 3.3646053959302495,
+            "velocityY": 1.7291800935577397,
+            "timestamp": 2.018178021069152
+        },
+        {
+            "x": 6.564758513555509,
+            "y": 1.8826846836570283,
+            "heading": -0.5067162932876892,
+            "angularVelocity": 0.0000011891087819705439,
+            "velocityX": 3.3646053959302504,
+            "velocityY": 1.7291800935577386,
+            "timestamp": 2.0523872915072587
+        },
+        {
+            "x": 6.679859209462396,
+            "y": 1.9418386731137416,
+            "heading": -0.5067162526091454,
+            "angularVelocity": 0.0000011891087784797324,
+            "velocityX": 3.3646053959301545,
+            "velocityY": 1.7291800935579253,
+            "timestamp": 2.0865965619453655
+        },
+        {
+            "x": 6.7949599053264,
+            "y": 2.0009926626538963,
+            "heading": -0.5067162119306018,
+            "angularVelocity": 0.0000011891087739781964,
+            "velocityX": 3.3646053946765995,
+            "velocityY": 1.7291800959970696,
+            "timestamp": 2.120805832383472
+        },
+        {
+            "x": 6.910060040297267,
+            "y": 2.06014774355027,
+            "heading": -0.5067161712446878,
+            "angularVelocity": 0.0000011893242211155864,
+            "velocityX": 3.3645889987368625,
+            "velocityY": 1.7292119983499907,
+            "timestamp": 2.155015102821579
+        },
+        {
+            "x": 7.017868923227757,
+            "y": 2.1174564154331827,
+            "heading": -0.4884934235175169,
+            "angularVelocity": 0.5326844885552473,
+            "velocityX": 3.151452268634164,
+            "velocityY": 1.6752380611740627,
+            "timestamp": 2.1892243732596857
+        },
+        {
+            "x": 7.119036711180674,
+            "y": 2.1700871649460596,
+            "heading": -0.45052101262699384,
+            "angularVelocity": 1.1100035284068692,
+            "velocityX": 2.9573208272872957,
+            "velocityY": 1.5384937719762113,
+            "timestamp": 2.2234336436977924
+        },
+        {
+            "x": 7.212945348428852,
+            "y": 2.2182419084801723,
+            "heading": -0.3958429177265348,
+            "angularVelocity": 1.5983414495607555,
+            "velocityX": 2.745122478366898,
+            "velocityY": 1.4076518708938115,
+            "timestamp": 2.257642914135899
+        },
+        {
+            "x": 7.29827375889861,
+            "y": 2.2619654936466005,
+            "heading": -0.3361817714679226,
+            "angularVelocity": 1.7440052212324946,
+            "velocityX": 2.494306641941948,
+            "velocityY": 1.2781209481077944,
+            "timestamp": 2.291852184574006
+        },
+        {
+            "x": 7.374929402835313,
+            "y": 2.3012175879094494,
+            "heading": -0.2767819040095215,
+            "angularVelocity": 1.7363675605380382,
+            "velocityX": 2.2407856979994336,
+            "velocityY": 1.1474110309913224,
+            "timestamp": 2.3260614550121126
+        },
+        {
+            "x": 7.442942536538503,
+            "y": 2.336017519822898,
+            "heading": -0.22036844778019443,
+            "angularVelocity": 1.6490692583284896,
+            "velocityX": 1.9881492014348112,
+            "velocityY": 1.0172661231232827,
+            "timestamp": 2.3602707254502193
+        },
+        {
+            "x": 7.502356073864926,
+            "y": 2.3663915967666074,
+            "heading": -0.16856655782281169,
+            "angularVelocity": 1.5142646801283255,
+            "velocityX": 1.7367671559648723,
+            "velocityY": 0.8878902284298598,
+            "timestamp": 2.394479995888326
+        },
+        {
+            "x": 7.553208169092694,
+            "y": 2.392365594084153,
+            "heading": -0.12246425110299042,
+            "angularVelocity": 1.3476553615264124,
+            "velocityX": 1.4865004303372251,
+            "velocityY": 0.7592677945160818,
+            "timestamp": 2.428689266326433
+        },
+        {
+            "x": 7.595530700913324,
+            "y": 2.4139635888667987,
+            "heading": -0.08285639859822795,
+            "angularVelocity": 1.1578104998299616,
+            "velocityX": 1.2371655775940182,
+            "velocityY": 0.6313491783381474,
+            "timestamp": 2.4628985367645395
+        },
+        {
+            "x": 7.629350339345417,
+            "y": 2.431207580669044,
+            "heading": -0.050358801794747596,
+            "angularVelocity": 0.9499646261757312,
+            "velocityX": 0.988610338629752,
+            "velocityY": 0.5040736496688569,
+            "timestamp": 2.4971078072026462
+        },
+        {
+            "x": 7.654689602970606,
+            "y": 2.444117248020803,
+            "heading": -0.025464280349729525,
+            "angularVelocity": 0.7277127260009457,
+            "velocityX": 0.7407133592934628,
+            "velocityY": 0.3773733606834889,
+            "timestamp": 2.531317077640753
+        },
+        {
+            "x": 7.671567523131761,
+            "y": 2.4527098140869614,
+            "heading": -0.008571412442380029,
+            "angularVelocity": 0.49380965133159116,
+            "velocityX": 0.4933727011714593,
+            "velocityY": 0.2511765365386793,
+            "timestamp": 2.5655263480788597
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 0.2562634691057309,
-            "velocityX": 0.24628851861412304,
-            "velocityY": 0.1251286715381821,
-            "timestamp": 2.593275819748948
+            "heading": 5.0379367595035626e-36,
+            "angularVelocity": 0.2505581771434718,
+            "velocityX": 0.24649683434481964,
+            "velocityY": 0.12541003821758284,
+            "timestamp": 2.5997356185169664
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 3.214441635047838e-33,
-            "velocityX": 1.400444892944232e-33,
-            "velocityY": -3.598995466574735e-33,
-            "timestamp": 2.627495437875645
+            "heading": 2.854280403338002e-36,
+            "angularVelocity": -6.383230494440131e-35,
+            "velocityX": -1.2082929677962258e-35,
+            "velocityY": 1.8766665143888344e-34,
+            "timestamp": 2.633944888955073
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.traj
index 0e00ab8a..00bde21f 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.traj	
@@ -3,578 +3,677 @@
         {
             "x": 0.71,
             "y": 4.36,
-            "heading": 2.1,
+            "heading": -1.04,
             "angularVelocity": 0,
-            "velocityX": 4.814287914507576e-35,
-            "velocityY": 2.4063306961726914e-34,
+            "velocityX": 0,
+            "velocityY": 2.687708804701003e-35,
             "timestamp": 0
         },
         {
-            "x": 0.7189874993597535,
-            "y": 4.351109558323372,
-            "heading": 2.1116856791570084,
-            "angularVelocity": 0.2954657391089134,
-            "velocityX": 0.22724380032956057,
-            "velocityY": -0.22478975211418176,
-            "timestamp": 0.039550031053517704
-        },
-        {
-            "x": 0.7369922825493326,
-            "y": 4.3333146846737405,
-            "heading": 2.134597544316128,
-            "angularVelocity": 0.5793134556106004,
-            "velocityX": 0.45524068401401757,
-            "velocityY": -0.4499332409006739,
-            "timestamp": 0.07910006210703541
-        },
-        {
-            "x": 0.7640484897224145,
-            "y": 4.306598997837255,
-            "heading": 2.168179876708578,
-            "angularVelocity": 0.849110139686295,
-            "velocityX": 0.6841007820315073,
-            "velocityY": -0.67549091934555,
-            "timestamp": 0.11865009316055311
-        },
-        {
-            "x": 0.800195666694539,
-            "y": 4.270941917715144,
-            "heading": 2.2117258914758198,
-            "angularVelocity": 1.1010361713323769,
-            "velocityX": 0.9139607734621368,
-            "velocityY": -0.9015689538615157,
-            "timestamp": 0.15820012421407081
-        },
-        {
-            "x": 0.8454794987248587,
-            "y": 4.226316705338956,
-            "heading": 2.264304991238856,
-            "angularVelocity": 1.3294325784950254,
-            "velocityX": 1.14497589063946,
-            "velocityY": -1.1283230679592324,
-            "timestamp": 0.19775015526758852
-        },
-        {
-            "x": 0.8999520283363762,
-            "y": 4.172689027996317,
-            "heading": 2.324662422471035,
-            "angularVelocity": 1.5261032576815274,
-            "velocityX": 1.3773068733576257,
-            "velocityY": -1.3559452651268766,
-            "timestamp": 0.23730018632110622
-        },
-        {
-            "x": 0.963671089377297,
-            "y": 4.110017781664974,
-            "heading": 2.391063304695866,
-            "angularVelocity": 1.6789084725364454,
-            "velocityX": 1.6111001519745567,
-            "velocityY": -1.584606754076596,
-            "timestamp": 0.2768502173746239
-        },
-        {
-            "x": 1.0366970385232381,
-            "y": 4.038262577868419,
-            "heading": 2.4609901353872035,
-            "angularVelocity": 1.7680600704640437,
-            "velocityX": 1.846419514743869,
-            "velocityY": -1.8142894426418532,
-            "timestamp": 0.31640024842814163
-        },
-        {
-            "x": 1.1190717343496346,
-            "y": 3.9574142704891733,
-            "heading": 2.5303614875414775,
-            "angularVelocity": 1.7540151121601602,
-            "velocityX": 2.082797247742485,
-            "velocityY": -2.0442033855762225,
-            "timestamp": 0.35595027948165936
-        },
-        {
-            "x": 1.21066072775428,
-            "y": 3.867689453216591,
-            "heading": 2.590569114237573,
-            "angularVelocity": 1.522315535343692,
-            "velocityX": 2.3157755117994827,
-            "velocityY": -2.268640880488067,
-            "timestamp": 0.3955003105351771
-        },
-        {
-            "x": 1.310358949370952,
-            "y": 3.7710393719100375,
-            "heading": 2.620822405939027,
-            "angularVelocity": 0.764937242666538,
-            "velocityX": 2.5208127265883533,
-            "velocityY": -2.4437422356450176,
-            "timestamp": 0.4350503415886948
-        },
-        {
-            "x": 1.418053038225142,
-            "y": 3.6673441481868845,
-            "heading": 2.6211471035683176,
-            "angularVelocity": 0.008209794547368329,
-            "velocityX": 2.722983673728656,
-            "velocityY": -2.621874647401313,
-            "timestamp": 0.47460037264221255
-        },
-        {
-            "x": 1.5245836891775304,
-            "y": 3.5622918530292758,
-            "heading": 2.6211471537170588,
-            "angularVelocity": 0.0000012679823435232559,
-            "velocityX": 2.6935668092961826,
-            "velocityY": -2.656187425376634,
-            "timestamp": 0.5141504036957303
-        },
-        {
-            "x": 1.6311142725711645,
-            "y": 3.4572394893622254,
-            "heading": 2.621147203865785,
-            "angularVelocity": 0.0000012679819635220034,
-            "velocityX": 2.6935651011115636,
-            "velocityY": -2.6561891575988192,
-            "timestamp": 0.553700434749248
-        },
-        {
-            "x": 1.7376448559612427,
-            "y": 3.3521871256915694,
-            "heading": 2.621147254014511,
-            "angularVelocity": 0.0000012679819538520794,
-            "velocityX": 2.6935651010216595,
-            "velocityY": -2.656189157689989,
-            "timestamp": 0.5932504658027657
-        },
-        {
-            "x": 1.8441754393513208,
-            "y": 3.2471347620209134,
-            "heading": 2.621147304163236,
-            "angularVelocity": 0.000001267981944384733,
-            "velocityX": 2.693565101021656,
-            "velocityY": -2.656189157689993,
-            "timestamp": 0.6328004968562835
-        },
-        {
-            "x": 1.9507060227413988,
-            "y": 3.142082398350257,
-            "heading": 2.621147354311961,
-            "angularVelocity": 0.0000012679819349678633,
-            "velocityX": 2.6935651010216572,
-            "velocityY": -2.656189157689994,
-            "timestamp": 0.6723505279098012
-        },
-        {
-            "x": 2.057236606131477,
-            "y": 3.037030034679601,
-            "heading": 2.6211474044606855,
-            "angularVelocity": 0.0000012679819252039202,
-            "velocityX": 2.693565101021662,
-            "velocityY": -2.6561891576899894,
-            "timestamp": 0.7119005589633189
-        },
-        {
-            "x": 2.1637671895248896,
-            "y": 2.931977671012326,
-            "heading": 2.6211474546094093,
-            "angularVelocity": 0.0000012679819154308074,
-            "velocityX": 2.69356510110597,
-            "velocityY": -2.656189157604496,
-            "timestamp": 0.7514505900168367
-        },
-        {
-            "x": 2.2702978362708315,
-            "y": 2.8269253715890703,
-            "heading": 2.6211475047581403,
-            "angularVelocity": 0.0000012679820770656017,
-            "velocityX": 2.693566702938569,
-            "velocityY": -2.6561875332310767,
-            "timestamp": 0.7910006210703544
-        },
-        {
-            "x": 2.3779994803856632,
-            "y": 2.7230739385096507,
-            "heading": 2.6211475550861425,
-            "angularVelocity": 0.0000012725148543062649,
-            "velocityX": 2.723174704189075,
-            "velocityY": -2.6258243119680853,
-            "timestamp": 0.8305506521238721
+            "x": 0.7140616140612069,
+            "y": 4.355992841184312,
+            "heading": -1.0347167301054676,
+            "angularVelocity": 0.1988304134727778,
+            "velocityX": 0.15285465616518656,
+            "velocityY": -0.15080528916361516,
+            "timestamp": 0.026571739213607736
+        },
+        {
+            "x": 0.7221925258884458,
+            "y": 4.3479754290630535,
+            "heading": -1.0242624006876555,
+            "angularVelocity": 0.39343790535390527,
+            "velocityX": 0.30599848063671325,
+            "velocityY": -0.3017270362623706,
+            "timestamp": 0.05314347842721547
+        },
+        {
+            "x": 0.7344011981721603,
+            "y": 4.335944497876898,
+            "heading": -1.0087613580778494,
+            "angularVelocity": 0.5833657513042195,
+            "velocityX": 0.45946078973491633,
+            "velocityY": -0.4527716868451825,
+            "timestamp": 0.0797152176408232
+        },
+        {
+            "x": 0.7506970627866689,
+            "y": 4.31989642008046,
+            "heading": -0.9883558514269565,
+            "angularVelocity": 0.7679401971717026,
+            "velocityX": 0.6132780576953482,
+            "velocityY": -0.6039528563572185,
+            "timestamp": 0.10628695685443094
+        },
+        {
+            "x": 0.7710907256657638,
+            "y": 4.29982697901967,
+            "heading": -0.963213241169254,
+            "angularVelocity": 0.9462162057057438,
+            "velocityX": 0.7674944690353961,
+            "velocityY": -0.7552927152962536,
+            "timestamp": 0.13285869606803868
+        },
+        {
+            "x": 0.7955941760833964,
+            "y": 4.27573110299105,
+            "heading": -0.9335350310204893,
+            "angularVelocity": 1.1169088297226009,
+            "velocityX": 0.9221620843352277,
+            "velocityY": -0.9068234425649082,
+            "timestamp": 0.1594304352816464
+        },
+        {
+            "x": 0.8242209914920048,
+            "y": 4.247602572989807,
+            "heading": -0.8995682913456332,
+            "angularVelocity": 1.2783032153748224,
+            "velocityX": 1.0773406730541775,
+            "velocityY": -1.0585882156647894,
+            "timestamp": 0.18600217449525414
+        },
+        {
+            "x": 0.856986533327333,
+            "y": 4.215433742997202,
+            "heading": -0.8616204330063739,
+            "angularVelocity": 1.4281285103018635,
+            "velocityX": 1.2330973735640383,
+            "velocityY": -1.210640738794065,
+            "timestamp": 0.21257391370886186
+        },
+        {
+            "x": 0.893908134971617,
+            "y": 4.179215360896215,
+            "heading": -0.8200791656995261,
+            "angularVelocity": 1.5633627506615773,
+            "velocityX": 1.3895063980372095,
+            "velocityY": -1.3630414558050108,
+            "timestamp": 0.2391456529224696
+        },
+        {
+            "x": 0.9350052770321623,
+            "y": 4.138936674970777,
+            "heading": -0.7754414952973147,
+            "angularVelocity": 1.6798926876172218,
+            "velocityX": 1.546648555074586,
+            "velocityY": -1.5158468025612346,
+            "timestamp": 0.2657173921360773
+        },
+        {
+            "x": 0.9802996637151041,
+            "y": 4.094586228803994,
+            "heading": -0.7283605055619151,
+            "angularVelocity": 1.7718444907546276,
+            "velocityX": 1.7046075275248067,
+            "velocityY": -1.669083299751435,
+            "timestamp": 0.29228913134968504
+        },
+        {
+            "x": 1.0298146765069804,
+            "y": 4.046154350498487,
+            "heading": -0.6797316150493184,
+            "angularVelocity": 1.8300981400454652,
+            "velocityX": 1.8634464381059026,
+            "velocityY": -1.8226837888242364,
+            "timestamp": 0.31886087056329276
+        },
+        {
+            "x": 1.0835715343165668,
+            "y": 3.9936405744754793,
+            "heading": -0.6308802721211105,
+            "angularVelocity": 1.8384699072761677,
+            "velocityX": 2.0230839004342207,
+            "velocityY": -1.9763017994740086,
+            "timestamp": 0.3454326097769005
+        },
+        {
+            "x": 1.1415677463510772,
+            "y": 3.937082152581159,
+            "heading": -0.5840793547040092,
+            "angularVelocity": 1.7613042579137594,
+            "velocityX": 2.1826276243449603,
+            "velocityY": -2.12851787531304,
+            "timestamp": 0.3720043489905082
+        },
+        {
+            "x": 1.203636670620202,
+            "y": 3.876777756204273,
+            "heading": -0.5447644235662668,
+            "angularVelocity": 1.4795768851144226,
+            "velocityX": 2.33589994882,
+            "velocityY": -2.2694937614773476,
+            "timestamp": 0.39857608820411594
+        },
+        {
+            "x": 1.2693974629594884,
+            "y": 3.8133907541919347,
+            "heading": -0.5190150912676422,
+            "angularVelocity": 0.969049563960718,
+            "velocityX": 2.4748395959572558,
+            "velocityY": -2.3855044452595306,
+            "timestamp": 0.42514782741772367
+        },
+        {
+            "x": 1.3388335264794184,
+            "y": 3.7468848835898942,
+            "heading": -0.5067181615747146,
+            "angularVelocity": 0.4627822663045784,
+            "velocityX": 2.613154636275026,
+            "velocityY": -2.5028798479243823,
+            "timestamp": 0.4517195666313314
+        },
+        {
+            "x": 1.410439403600934,
+            "y": 3.676338986398213,
+            "heading": -0.5067181155488927,
+            "angularVelocity": 0.0000017321343363764108,
+            "velocityX": 2.6948133332892708,
+            "velocityY": -2.6549220818618244,
+            "timestamp": 0.4782913058449391
+        },
+        {
+            "x": 1.482040293717765,
+            "y": 3.605788027611728,
+            "heading": -0.5067180695571892,
+            "angularVelocity": 0.0000017308503249172533,
+            "velocityX": 2.694625652511417,
+            "velocityY": -2.655112569761896,
+            "timestamp": 0.5048630450585468
+        },
+        {
+            "x": 1.5536411829944505,
+            "y": 3.5352370679725946,
+            "heading": -0.5067180235654862,
+            "angularVelocity": 0.0000017308503100877725,
+            "velocityX": 2.6946256208934063,
+            "velocityY": -2.6551126018504427,
+            "timestamp": 0.5314347842721546
+        },
+        {
+            "x": 1.6252420722709946,
+            "y": 3.4646861083333174,
+            "heading": -0.5067179775737835,
+            "angularVelocity": 0.0000017308502987237163,
+            "velocityX": 2.6946256208880808,
+            "velocityY": -2.6551126018558486,
+            "timestamp": 0.5580065234857624
+        },
+        {
+            "x": 1.6968429615475384,
+            "y": 3.39413514869404,
+            "heading": -0.506717931582081,
+            "angularVelocity": 0.0000017308502871423956,
+            "velocityX": 2.6946256208880808,
+            "velocityY": -2.65511260185585,
+            "timestamp": 0.5845782626993702
+        },
+        {
+            "x": 1.7684438508240825,
+            "y": 3.323584189054763,
+            "heading": -0.5067178855903789,
+            "angularVelocity": 0.0000017308502762001758,
+            "velocityX": 2.694625620888081,
+            "velocityY": -2.65511260185585,
+            "timestamp": 0.611150001912978
+        },
+        {
+            "x": 1.8400447401006266,
+            "y": 3.2530332294154856,
+            "heading": -0.506717839598677,
+            "angularVelocity": 0.000001730850265567339,
+            "velocityX": 2.6946256208880826,
+            "velocityY": -2.6551126018558495,
+            "timestamp": 0.6377217411265858
+        },
+        {
+            "x": 1.9116456293771706,
+            "y": 3.1824822697762087,
+            "heading": -0.5067177936069754,
+            "angularVelocity": 0.0000017308502535085818,
+            "velocityX": 2.694625620888084,
+            "velocityY": -2.65511260185585,
+            "timestamp": 0.6642934803401935
+        },
+        {
+            "x": 1.9832465186537145,
+            "y": 3.1119313101369315,
+            "heading": -0.5067177476152741,
+            "angularVelocity": 0.0000017308502432657092,
+            "velocityX": 2.6946256208880843,
+            "velocityY": -2.65511260185585,
+            "timestamp": 0.6908652195538013
+        },
+        {
+            "x": 2.0548474079302586,
+            "y": 3.041380350497654,
+            "heading": -0.5067177016235731,
+            "angularVelocity": 0.000001730850231529751,
+            "velocityX": 2.694625620888085,
+            "velocityY": -2.6551126018558504,
+            "timestamp": 0.7174369587674091
+        },
+        {
+            "x": 2.1264482972068026,
+            "y": 2.970829390858377,
+            "heading": -0.5067176556318724,
+            "angularVelocity": 0.0000017308502204807436,
+            "velocityX": 2.6946256208880865,
+            "velocityY": -2.6551126018558495,
+            "timestamp": 0.7440086979810169
+        },
+        {
+            "x": 2.1980491864833867,
+            "y": 2.9002784312191405,
+            "heading": -0.5067176096401721,
+            "angularVelocity": 0.0000017308502087562552,
+            "velocityX": 2.694625620889591,
+            "velocityY": -2.6551126018543236,
+            "timestamp": 0.7705804371946247
+        },
+        {
+            "x": 2.2696500759972245,
+            "y": 2.829727471820689,
+            "heading": -0.506717563648472,
+            "angularVelocity": 0.0000017308501985869009,
+            "velocityX": 2.6946256298184115,
+            "velocityY": -2.6551125927926265,
+            "timestamp": 0.7971521764082324
+        },
+        {
+            "x": 2.3412523738653572,
+            "y": 2.7591779417640674,
+            "heading": -0.5067175176565489,
+            "angularVelocity": 0.0000017308585893832803,
+            "velocityX": 2.694678631779742,
+            "velocityY": -2.6550588009870313,
+            "timestamp": 0.8237239156218402
+        },
+        {
+            "x": 2.4158866145337003,
+            "y": 2.6918440324740667,
+            "heading": -0.5067174710515008,
+            "angularVelocity": 0.000001753932919872258,
+            "velocityX": 2.808782672009746,
+            "velocityY": -2.5340422299311864,
+            "timestamp": 0.850295654835448
         },
         {
             "x": 2.4943342208862305,
             "y": 2.6289942264556885,
-            "heading": 2.6211476076664186,
-            "angularVelocity": 0.0000013294623338808287,
-            "velocityX": 2.941457627255649,
-            "velocityY": -2.378751913662396,
-            "timestamp": 0.8701006831773899
-        },
-        {
-            "x": 2.6932881359026166,
-            "y": 2.508539731368457,
-            "heading": 2.6211476558145916,
-            "angularVelocity": 7.831464659210102e-7,
-            "velocityX": 3.236053335597206,
-            "velocityY": -1.9592334766702564,
-            "timestamp": 0.9315811021889849
-        },
-        {
-            "x": 2.899938569108162,
-            "y": 2.401827293887261,
-            "heading": 2.621147703190774,
-            "angularVelocity": 7.705897835721374e-7,
-            "velocityX": 3.361239830954497,
-            "velocityY": -1.7357142192064432,
-            "timestamp": 0.99306152120058
-        },
-        {
-            "x": 3.106589142147435,
-            "y": 2.295115127196865,
-            "heading": 2.6211477505669496,
-            "angularVelocity": 7.705896615935058e-7,
-            "velocityX": 3.361242105397809,
-            "velocityY": -1.7357098147016614,
-            "timestamp": 1.0545419402121752
-        },
-        {
-            "x": 3.313239715188592,
-            "y": 2.188402960510117,
-            "heading": 2.621147797943125,
-            "angularVelocity": 7.705896625567414e-7,
-            "velocityX": 3.361242105428448,
-            "velocityY": -1.735709814642326,
-            "timestamp": 1.1160223592237704
-        },
-        {
-            "x": 3.519890288229749,
-            "y": 2.0816907938233693,
-            "heading": 2.6211478453193005,
-            "angularVelocity": 7.705896648559933e-7,
-            "velocityX": 3.361242105428449,
-            "velocityY": -1.735709814642324,
-            "timestamp": 1.1775027782353655
-        },
-        {
-            "x": 3.726540861270906,
-            "y": 1.9749786271366212,
-            "heading": 2.621147892695476,
-            "angularVelocity": 7.705896659187971e-7,
-            "velocityX": 3.361242105428449,
-            "velocityY": -1.7357098146423233,
-            "timestamp": 1.2389831972469607
-        },
-        {
-            "x": 3.933191434312063,
-            "y": 1.868266460449873,
-            "heading": 2.6211479400716513,
-            "angularVelocity": 7.705896660607392e-7,
-            "velocityX": 3.36124210542845,
-            "velocityY": -1.7357098146423227,
-            "timestamp": 1.3004636162585559
-        },
-        {
-            "x": 4.139842007353606,
-            "y": 1.7615542937638724,
-            "heading": 2.6211479874478267,
-            "angularVelocity": 7.705896681379808e-7,
-            "velocityX": 3.361242105434726,
-            "velocityY": -1.7357098146301677,
-            "timestamp": 1.361944035270151
-        },
-        {
-            "x": 4.3464926090376705,
-            "y": 1.6548421825447925,
-            "heading": 2.621148034824002,
-            "angularVelocity": 7.705896639238046e-7,
-            "velocityX": 3.361242571315101,
-            "velocityY": -1.7357089124417728,
-            "timestamp": 1.4234244542817462
-        },
-        {
-            "x": 4.555173710801168,
-            "y": 1.5521575021408582,
-            "heading": 2.6211480823085673,
-            "angularVelocity": 7.723526622271063e-7,
-            "velocityX": 3.3942693481666213,
-            "velocityY": -1.6702013755724032,
-            "timestamp": 1.4849048732933414
-        },
-        {
-            "x": 4.775821132533127,
-            "y": 1.478627504505032,
-            "heading": 2.621148131521485,
-            "angularVelocity": 8.004649014817169e-7,
-            "velocityX": 3.5889056268524353,
-            "velocityY": -1.1959905091401257,
-            "timestamp": 1.5463852923049366
-        },
-        {
-            "x": 5.004383859827403,
-            "y": 1.4356039834024816,
-            "heading": 2.621148186381714,
-            "angularVelocity": 8.923203458439086e-7,
-            "velocityX": 3.7176507735116338,
-            "velocityY": -0.699792255716998,
-            "timestamp": 1.6078657113165318
+            "heading": -0.5067174224716422,
+            "angularVelocity": 0.000001828252870098291,
+            "velocityX": 2.952294756541798,
+            "velocityY": -2.365287628074865,
+            "timestamp": 0.8768673940490558
+        },
+        {
+            "x": 2.693828949223235,
+            "y": 2.509453130751911,
+            "heading": -0.5067173754009504,
+            "angularVelocity": 7.656472375459629e-7,
+            "velocityX": 3.244961599231293,
+            "velocityY": -1.9444436869204162,
+            "timestamp": 0.9383456957905196
+        },
+        {
+            "x": 2.900420237054028,
+            "y": 2.4026436735913763,
+            "heading": -0.5067173289743672,
+            "angularVelocity": 7.551702291921459e-7,
+            "velocityX": 3.3603935368868174,
+            "velocityY": -1.7373521085488381,
+            "timestamp": 0.9998239975319835
+        },
+        {
+            "x": 3.1070116472391316,
+            "y": 2.295834453089378,
+            "heading": -0.5067172825477892,
+            "angularVelocity": 7.55170142432061e-7,
+            "velocityX": 3.3603955270899863,
+            "velocityY": -1.7373482590844154,
+            "timestamp": 1.0613022992734473
+        },
+        {
+            "x": 3.3136030574258837,
+            "y": 2.1890252325905677,
+            "heading": -0.5067172361212112,
+            "angularVelocity": 7.551701448333524e-7,
+            "velocityX": 3.3603955271167996,
+            "velocityY": -1.737348259032553,
+            "timestamp": 1.1227806010149113
+        },
+        {
+            "x": 3.5201944676126358,
+            "y": 2.0822160120917577,
+            "heading": -0.5067171896946332,
+            "angularVelocity": 7.551701452492318e-7,
+            "velocityX": 3.3603955271167996,
+            "velocityY": -1.737348259032552,
+            "timestamp": 1.1842589027563752
+        },
+        {
+            "x": 3.726785877799388,
+            "y": 1.9754067915929476,
+            "heading": -0.5067171432680552,
+            "angularVelocity": 7.5517014486464e-7,
+            "velocityX": 3.3603955271168005,
+            "velocityY": -1.7373482590325509,
+            "timestamp": 1.2457372044978392
+        },
+        {
+            "x": 3.93337728798614,
+            "y": 1.8685975710941374,
+            "heading": -0.506717096841477,
+            "angularVelocity": 7.551701472516521e-7,
+            "velocityX": 3.360395527116801,
+            "velocityY": -1.7373482590325502,
+            "timestamp": 1.3072155062393032
+        },
+        {
+            "x": 4.139968698173285,
+            "y": 1.761788350596088,
+            "heading": -0.5067170504148988,
+            "angularVelocity": 7.551701467640916e-7,
+            "velocityX": 3.360395527123197,
+            "velocityY": -1.7373482590201774,
+            "timestamp": 1.3686938079807671
+        },
+        {
+            "x": 4.346560137549085,
+            "y": 1.6549791865550314,
+            "heading": -0.5067170039883208,
+            "angularVelocity": 7.55170145558408e-7,
+            "velocityX": 3.36039600190298,
+            "velocityY": -1.737347340696294,
+            "timestamp": 1.430172109722231
+        },
+        {
+            "x": 4.5552180589852185,
+            "y": 1.552265545137979,
+            "heading": -0.5067169574392034,
+            "angularVelocity": 7.571633584219393e-7,
+            "velocityX": 3.3940091955305984,
+            "velocityY": -1.6707299731374539,
+            "timestamp": 1.491650411463695
+        },
+        {
+            "x": 4.775846094749007,
+            "y": 1.478702717632789,
+            "heading": -0.5067169090858323,
+            "angularVelocity": 7.865111701229533e-7,
+            "velocityX": 3.5887138960279197,
+            "velocityY": -1.1965657056459655,
+            "timestamp": 1.553128713205159
+        },
+        {
+            "x": 5.004393851392972,
+            "y": 1.4356429788646896,
+            "heading": -0.5067168550876733,
+            "angularVelocity": 8.783287361409535e-7,
+            "velocityX": 3.717535295706156,
+            "velocityY": -0.7004054690576753,
+            "timestamp": 1.614607014946623
         },
         {
             "x": 5.236664772033691,
             "y": 1.4238771200180054,
-            "heading": 2.6211482490896194,
-            "angularVelocity": 0.0000010199654837334,
-            "velocityX": 3.7781283202133062,
-            "velocityY": -0.1907414356148787,
-            "timestamp": 1.669346130328127
-        },
-        {
-            "x": 5.366075062772529,
-            "y": 1.4271130159839853,
-            "heading": 2.621148315155484,
-            "angularVelocity": 0.0000019306429598932118,
-            "velocityX": 3.781757302483639,
-            "velocityY": 0.09456259722124823,
-            "timestamp": 1.703565748454824
-        },
-        {
-            "x": 5.494873366116125,
-            "y": 1.4400934888882522,
-            "heading": 2.621148371231603,
-            "angularVelocity": 0.00000163871257714578,
-            "velocityX": 3.7638731930533114,
-            "velocityY": 0.3793283974183189,
-            "timestamp": 1.7377853665815208
-        },
-        {
-            "x": 5.622326965929087,
-            "y": 1.4627446781635232,
-            "heading": 2.6211484208273035,
-            "angularVelocity": 0.0000014493353027620846,
-            "velocityX": 3.724576917868275,
-            "velocityY": 0.6619357700429461,
-            "timestamp": 1.7720049847082178
-        },
-        {
-            "x": 5.747710804313215,
-            "y": 1.4949376920061848,
-            "heading": 2.6211484662381075,
-            "angularVelocity": 0.000001327040057798138,
-            "velocityX": 3.6640922736162436,
-            "velocityY": 0.9407765371158774,
-            "timestamp": 1.8062246028349147
-        },
-        {
-            "x": 5.8703116218973515,
-            "y": 1.5364893102203447,
-            "heading": 2.62114850910788,
-            "angularVelocity": 0.000001252783485820186,
-            "velocityX": 3.5827640486872516,
-            "velocityY": 1.2142630598715765,
-            "timestamp": 1.8404442209616116
-        },
-        {
-            "x": 5.989432110510927,
-            "y": 1.5871628422758242,
-            "heading": 2.621148550731445,
-            "angularVelocity": 0.00000121636555358505,
-            "velocityX": 3.481058385062543,
-            "velocityY": 1.4808327745757486,
-            "timestamp": 1.8746638390883086
-        },
-        {
-            "x": 6.104583123843471,
-            "y": 1.6463045521525654,
-            "heading": 2.621148592213924,
-            "angularVelocity": 0.0000012122426079588706,
-            "velocityX": 3.3650583973847925,
-            "velocityY": 1.7282983596652888,
-            "timestamp": 1.9088834572150055
-        },
-        {
-            "x": 6.2197290464685,
-            "y": 1.7054561727682278,
-            "heading": 2.621148633696581,
-            "angularVelocity": 0.0000012122477903456845,
-            "velocityX": 3.3649096316243146,
-            "velocityY": 1.7285879812175449,
-            "timestamp": 1.9431030753417025
-        },
-        {
-            "x": 6.334874968704583,
-            "y": 1.7646077941410194,
-            "heading": 2.6211486751792368,
-            "angularVelocity": 0.0000012122477871677539,
-            "velocityX": 3.364909620258179,
-            "velocityY": 1.7285880033431293,
-            "timestamp": 1.9773226934683994
-        },
-        {
-            "x": 6.450020890940638,
-            "y": 1.8237594155138686,
-            "heading": 2.6211487166618928,
-            "angularVelocity": 0.000001212247782062354,
-            "velocityX": 3.364909620257311,
-            "velocityY": 1.7285880033448198,
-            "timestamp": 2.0115423115950963
-        },
-        {
-            "x": 6.565166813176692,
-            "y": 1.882911036886718,
-            "heading": 2.621148758144549,
-            "angularVelocity": 0.0000012122477787509118,
-            "velocityX": 3.3649096202573117,
-            "velocityY": 1.7285880033448189,
-            "timestamp": 2.0457619297217935
-        },
-        {
-            "x": 6.68031273541274,
-            "y": 1.9420626582595795,
-            "heading": 2.6211487996272047,
-            "angularVelocity": 0.0000012122477734983372,
-            "velocityX": 3.3649096202571287,
-            "velocityY": 1.7285880033451764,
-            "timestamp": 2.0799815478484907
-        },
-        {
-            "x": 6.79545865756654,
-            "y": 2.001214279792547,
-            "heading": 2.6211488411098602,
-            "angularVelocity": 0.0000012122477688304903,
-            "velocityX": 3.3649096178535927,
-            "velocityY": 1.7285880080239562,
-            "timestamp": 2.114201165975188
-        },
-        {
-            "x": 6.910603503099618,
-            "y": 2.0603679970417836,
-            "heading": 2.621148882606884,
-            "angularVelocity": 0.000001212667636144746,
-            "velocityX": 3.3648781557631158,
-            "velocityY": 1.728649251146572,
-            "timestamp": 2.148420784101885
-        },
-        {
-            "x": 7.018224220241307,
-            "y": 2.1177284432893,
-            "heading": 2.639872440534831,
-            "angularVelocity": 0.5471585877616616,
-            "velocityX": 3.1450005299073442,
-            "velocityY": 1.6762444874498934,
-            "timestamp": 2.182640402228582
-        },
-        {
-            "x": 7.119181986421491,
-            "y": 2.170426181119355,
-            "heading": 2.6783971426308466,
-            "angularVelocity": 1.1258074813511751,
-            "velocityX": 2.9502890945887246,
-            "velocityY": 1.5399861458109638,
-            "timestamp": 2.2168600203552793
-        },
-        {
-            "x": 7.213084203441603,
-            "y": 2.2185446189369,
-            "heading": 2.734533820088199,
-            "angularVelocity": 1.6404822885371884,
-            "velocityX": 2.7441047609719855,
-            "velocityY": 1.4061652482324039,
-            "timestamp": 2.2510796384819765
-        },
-        {
-            "x": 7.29840370402033,
-            "y": 2.262230718496702,
-            "heading": 2.7957315713049304,
-            "angularVelocity": 1.788382061721109,
-            "velocityX": 2.493292013453601,
-            "velocityY": 1.2766390144406516,
-            "timestamp": 2.2852992566086736
-        },
-        {
-            "x": 7.375044913403133,
-            "y": 2.301443411516585,
-            "heading": 2.8566177755066406,
-            "angularVelocity": 1.7792777223954108,
-            "velocityX": 2.239686284605582,
-            "velocityY": 1.1459126421194965,
-            "timestamp": 2.3195188747353708
-        },
-        {
-            "x": 7.443040169771806,
-            "y": 2.336203371638747,
-            "heading": 2.9144119075075787,
-            "angularVelocity": 1.6889180874829506,
-            "velocityX": 1.9870255745380796,
-            "velocityY": 1.0157904157043582,
-            "timestamp": 2.353738492862068
-        },
-        {
-            "x": 7.502434435273893,
-            "y": 2.3665382868697598,
-            "heading": 2.967461608910358,
-            "angularVelocity": 1.5502715783199053,
-            "velocityX": 1.7356787934389564,
-            "velocityY": 0.8864773159857819,
-            "timestamp": 2.387958110988765
-        },
-        {
-            "x": 7.553267406161016,
-            "y": 2.3924752684695187,
-            "heading": 3.014662356421872,
-            "angularVelocity": 1.379347581751362,
-            "velocityX": 1.48549205601638,
-            "velocityY": 0.7579564886939567,
-            "timestamp": 2.4221777291154623
-        },
-        {
-            "x": 7.595572130014844,
-            "y": 2.41403971266414,
-            "heading": 3.055206797581657,
-            "angularVelocity": 1.184830321883501,
-            "velocityX": 1.2362710681690243,
-            "velocityY": 0.6301778153917356,
-            "timestamp": 2.4563973472421594
-        },
-        {
-            "x": 7.629376218234675,
-            "y": 2.4312548848965747,
-            "heading": 3.088468435996466,
-            "angularVelocity": 0.9720049560944556,
-            "velocityX": 0.9878569683236498,
-            "velocityY": 0.5030790281965085,
-            "timestamp": 2.4906169653688566
-        },
-        {
-            "x": 7.654702978974042,
-            "y": 2.4441416202766635,
-            "heading": 3.113945152945951,
-            "angularVelocity": 0.7445061734809163,
-            "velocityX": 0.7401240027166469,
-            "velocityY": 0.3765891054767559,
-            "timestamp": 2.5248365834955537
-        },
-        {
-            "x": 7.671572100944035,
-            "y": 2.4527181446432627,
-            "heading": 3.1312307619473794,
-            "angularVelocity": 0.5051374021015854,
-            "velocityX": 0.4929664003711423,
-            "velocityY": 0.250631796498847,
-            "timestamp": 2.559056201622251
+            "heading": -0.5067167933216484,
+            "angularVelocity": 0.000001004680076164895,
+            "velocityX": 3.7780959145145716,
+            "velocityY": -0.19138230096471237,
+            "timestamp": 1.676085316688087
+        },
+        {
+            "x": 5.366036550697627,
+            "y": 1.4270871399969536,
+            "heading": -0.5067167282581706,
+            "angularVelocity": 0.000001901925327561791,
+            "velocityX": 3.781775437099809,
+            "velocityY": 0.09383479793163368,
+            "timestamp": 1.7102945871261934
+        },
+        {
+            "x": 5.494798697340742,
+            "y": 1.4400359689986706,
+            "heading": -0.5067166730622293,
+            "angularVelocity": 0.0000016134790500794753,
+            "velocityX": 3.76395476998197,
+            "velocityY": 0.37851812786083194,
+            "timestamp": 1.7445038575643
+        },
+        {
+            "x": 5.6222191444596294,
+            "y": 1.4626499710733982,
+            "heading": -0.5067166242756445,
+            "angularVelocity": 0.0000014261217576278581,
+            "velocityX": 3.724734421022641,
+            "velocityY": 0.6610489433161765,
+            "timestamp": 1.7787131280024064
+        },
+        {
+            "x": 5.747573461036382,
+            "y": 1.4948005438980272,
+            "heading": -0.5067165796372217,
+            "angularVelocity": 0.0000013048633380814483,
+            "velocityX": 3.6643376187618593,
+            "velocityY": 0.9398204759378852,
+            "timestamp": 1.812922398440513
+        },
+        {
+            "x": 5.870148986660447,
+            "y": 1.5363048197489622,
+            "heading": -0.5067165375293654,
+            "angularVelocity": 0.0000012308902156872765,
+            "velocityX": 3.583108439738183,
+            "velocityY": 1.2132464480944425,
+            "timestamp": 1.8471316688786195
+        },
+        {
+            "x": 5.9892489774115445,
+            "y": 1.5869265226922225,
+            "heading": -0.5067164966802475,
+            "angularVelocity": 0.0000011940949728764683,
+            "velocityX": 3.481512152285735,
+            "velocityY": 1.4797656394002383,
+            "timestamp": 1.881340939316726
+        },
+        {
+            "x": 6.104355729464833,
+            "y": 1.6460687267313168,
+            "heading": -0.5067164560018657,
+            "angularVelocity": 0.0000011891040447074693,
+            "velocityX": 3.3647824282469623,
+            "velocityY": 1.7288355840881788,
+            "timestamp": 1.9155502097548325
+        },
+        {
+            "x": 6.219456425834801,
+            "y": 1.705222715286976,
+            "heading": -0.5067164153233215,
+            "angularVelocity": 0.0000011891087921450935,
+            "velocityX": 3.3646054094668667,
+            "velocityY": 1.7291800672184492,
+            "timestamp": 1.949759480192939
+        },
+        {
+            "x": 6.334557121741728,
+            "y": 1.764376704743614,
+            "heading": -0.5067163746447773,
+            "angularVelocity": 0.0000011891087898511233,
+            "velocityX": 3.3646053959312843,
+            "velocityY": 1.7291800935557256,
+            "timestamp": 1.9839687506310455
+        },
+        {
+            "x": 6.449657817648618,
+            "y": 1.8235306942003213,
+            "heading": -0.5067163339662332,
+            "angularVelocity": 0.000001189108787218646,
+            "velocityX": 3.3646053959302495,
+            "velocityY": 1.7291800935577397,
+            "timestamp": 2.018178021069152
+        },
+        {
+            "x": 6.564758513555509,
+            "y": 1.8826846836570283,
+            "heading": -0.5067162932876892,
+            "angularVelocity": 0.0000011891087819705439,
+            "velocityX": 3.3646053959302504,
+            "velocityY": 1.7291800935577386,
+            "timestamp": 2.0523872915072587
+        },
+        {
+            "x": 6.679859209462396,
+            "y": 1.9418386731137416,
+            "heading": -0.5067162526091454,
+            "angularVelocity": 0.0000011891087784797324,
+            "velocityX": 3.3646053959301545,
+            "velocityY": 1.7291800935579253,
+            "timestamp": 2.0865965619453655
+        },
+        {
+            "x": 6.7949599053264,
+            "y": 2.0009926626538963,
+            "heading": -0.5067162119306018,
+            "angularVelocity": 0.0000011891087739781964,
+            "velocityX": 3.3646053946765995,
+            "velocityY": 1.7291800959970696,
+            "timestamp": 2.120805832383472
+        },
+        {
+            "x": 6.910060040297267,
+            "y": 2.06014774355027,
+            "heading": -0.5067161712446878,
+            "angularVelocity": 0.0000011893242211155864,
+            "velocityX": 3.3645889987368625,
+            "velocityY": 1.7292119983499907,
+            "timestamp": 2.155015102821579
+        },
+        {
+            "x": 7.017868923227757,
+            "y": 2.1174564154331827,
+            "heading": -0.4884934235175169,
+            "angularVelocity": 0.5326844885552473,
+            "velocityX": 3.151452268634164,
+            "velocityY": 1.6752380611740627,
+            "timestamp": 2.1892243732596857
+        },
+        {
+            "x": 7.119036711180674,
+            "y": 2.1700871649460596,
+            "heading": -0.45052101262699384,
+            "angularVelocity": 1.1100035284068692,
+            "velocityX": 2.9573208272872957,
+            "velocityY": 1.5384937719762113,
+            "timestamp": 2.2234336436977924
+        },
+        {
+            "x": 7.212945348428852,
+            "y": 2.2182419084801723,
+            "heading": -0.3958429177265348,
+            "angularVelocity": 1.5983414495607555,
+            "velocityX": 2.745122478366898,
+            "velocityY": 1.4076518708938115,
+            "timestamp": 2.257642914135899
+        },
+        {
+            "x": 7.29827375889861,
+            "y": 2.2619654936466005,
+            "heading": -0.3361817714679226,
+            "angularVelocity": 1.7440052212324946,
+            "velocityX": 2.494306641941948,
+            "velocityY": 1.2781209481077944,
+            "timestamp": 2.291852184574006
+        },
+        {
+            "x": 7.374929402835313,
+            "y": 2.3012175879094494,
+            "heading": -0.2767819040095215,
+            "angularVelocity": 1.7363675605380382,
+            "velocityX": 2.2407856979994336,
+            "velocityY": 1.1474110309913224,
+            "timestamp": 2.3260614550121126
+        },
+        {
+            "x": 7.442942536538503,
+            "y": 2.336017519822898,
+            "heading": -0.22036844778019443,
+            "angularVelocity": 1.6490692583284896,
+            "velocityX": 1.9881492014348112,
+            "velocityY": 1.0172661231232827,
+            "timestamp": 2.3602707254502193
+        },
+        {
+            "x": 7.502356073864926,
+            "y": 2.3663915967666074,
+            "heading": -0.16856655782281169,
+            "angularVelocity": 1.5142646801283255,
+            "velocityX": 1.7367671559648723,
+            "velocityY": 0.8878902284298598,
+            "timestamp": 2.394479995888326
+        },
+        {
+            "x": 7.553208169092694,
+            "y": 2.392365594084153,
+            "heading": -0.12246425110299042,
+            "angularVelocity": 1.3476553615264124,
+            "velocityX": 1.4865004303372251,
+            "velocityY": 0.7592677945160818,
+            "timestamp": 2.428689266326433
+        },
+        {
+            "x": 7.595530700913324,
+            "y": 2.4139635888667987,
+            "heading": -0.08285639859822795,
+            "angularVelocity": 1.1578104998299616,
+            "velocityX": 1.2371655775940182,
+            "velocityY": 0.6313491783381474,
+            "timestamp": 2.4628985367645395
+        },
+        {
+            "x": 7.629350339345417,
+            "y": 2.431207580669044,
+            "heading": -0.050358801794747596,
+            "angularVelocity": 0.9499646261757312,
+            "velocityX": 0.988610338629752,
+            "velocityY": 0.5040736496688569,
+            "timestamp": 2.4971078072026462
+        },
+        {
+            "x": 7.654689602970606,
+            "y": 2.444117248020803,
+            "heading": -0.025464280349729525,
+            "angularVelocity": 0.7277127260009457,
+            "velocityX": 0.7407133592934628,
+            "velocityY": 0.3773733606834889,
+            "timestamp": 2.531317077640753
+        },
+        {
+            "x": 7.671567523131761,
+            "y": 2.4527098140869614,
+            "heading": -0.008571412442380029,
+            "angularVelocity": 0.49380965133159116,
+            "velocityX": 0.4933727011714593,
+            "velocityY": 0.2511765365386793,
+            "timestamp": 2.5655263480788597
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 0.2562634691057309,
-            "velocityX": 0.24628851861412304,
-            "velocityY": 0.1251286715381821,
-            "timestamp": 2.593275819748948
+            "heading": 5.0379367595035626e-36,
+            "angularVelocity": 0.2505581771434718,
+            "velocityX": 0.24649683434481964,
+            "velocityY": 0.12541003821758284,
+            "timestamp": 2.5997356185169664
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 3.214441635047838e-33,
-            "velocityX": 1.400444892944232e-33,
-            "velocityY": -3.598995466574735e-33,
-            "timestamp": 2.627495437875645
+            "heading": 2.854280403338002e-36,
+            "angularVelocity": -6.383230494440131e-35,
+            "velocityX": -1.2082929677962258e-35,
+            "velocityY": 1.8766665143888344e-34,
+            "timestamp": 2.633944888955073
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.1.traj
index d8bd6114..f3a7c6d5 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.1.traj	
@@ -3,290 +3,290 @@
         {
             "x": 0.71,
             "y": 4.36,
-            "heading": 2.1,
+            "heading": -1.04,
             "angularVelocity": 0,
-            "velocityX": 8.526478185532948e-32,
-            "velocityY": -7.785685234118549e-31,
+            "velocityX": -3.7161803829578067e-31,
+            "velocityY": -1.7109886686245282e-31,
             "timestamp": 0
         },
         {
-            "x": 0.755735101962148,
-            "y": 4.33059726025757,
-            "heading": 2.1511666979053023,
-            "angularVelocity": 0.6232717514717016,
-            "velocityX": 0.5571083980530026,
-            "velocityY": -0.35816063665566134,
-            "timestamp": 0.08209372201529226
+            "x": 0.755738680236623,
+            "y": 4.330590345332975,
+            "heading": -0.988927755482241,
+            "angularVelocity": 0.6221231913092291,
+            "velocityX": 0.5571537727343439,
+            "velocityY": -0.35824601776392057,
+            "timestamp": 0.08209345870916634
         },
         {
-            "x": 0.8475193720501324,
-            "y": 4.271815657764732,
-            "heading": 2.250112545454498,
-            "angularVelocity": 1.2052791019849762,
-            "velocityX": 1.1180424986808006,
-            "velocityY": -0.7160304229096619,
-            "timestamp": 0.16418744403058452
+            "x": 0.8475293838257123,
+            "y": 4.27179448486036,
+            "heading": -0.8901763080939228,
+            "angularVelocity": 1.2029149330663034,
+            "velocityX": 1.1181244526957685,
+            "velocityY": -0.7162063993540811,
+            "timestamp": 0.1641869174183327
         },
         {
-            "x": 0.9858871774715989,
-            "y": 4.183669855686051,
-            "heading": 2.389658659666248,
-            "angularVelocity": 1.6998390472021188,
-            "velocityX": 1.6854858328349602,
-            "velocityY": -1.0737215942317955,
-            "timestamp": 0.2462811660458768
+            "x": 0.9859043696516769,
+            "y": 4.183626202129818,
+            "heading": -0.7509222083756354,
+            "angularVelocity": 1.6962874010659592,
+            "velocityX": 1.6855787050730526,
+            "velocityY": -1.0739988802627505,
+            "timestamp": 0.24628037612749903
         },
         {
-            "x": 1.17173213163872,
-            "y": 4.066157035949835,
-            "heading": 2.5522635706365153,
-            "angularVelocity": 1.9807228491842224,
-            "velocityX": 2.2638144501780824,
-            "velocityY": -1.4314470930472174,
-            "timestamp": 0.32837488806116905
+            "x": 1.1717528122715652,
+            "y": 4.066081969426692,
+            "heading": -0.5886701024002182,
+            "angularVelocity": 1.9764316003572233,
+            "velocityX": 2.263864204799754,
+            "velocityY": -1.4318343331050452,
+            "timestamp": 0.3283738348366654
         },
         {
-            "x": 1.4047988716632624,
-            "y": 3.9214804758601747,
-            "heading": 2.664319566325619,
-            "angularVelocity": 1.3649764310629042,
-            "velocityX": 2.839032441252049,
-            "velocityY": -1.76233403161705,
-            "timestamp": 0.4104686100764613
+            "x": 1.404824377392605,
+            "y": 3.921350877318508,
+            "heading": -0.4767670307510258,
+            "angularVelocity": 1.3631180048782332,
+            "velocityX": 2.8391003228009395,
+            "velocityY": -1.7630039516415732,
+            "timestamp": 0.4104672935458317
         },
         {
-            "x": 1.6657539489096052,
-            "y": 3.7531140130982656,
-            "heading": 2.6643196178388515,
-            "angularVelocity": 6.274929563351661e-7,
-            "velocityX": 3.178745838783293,
-            "velocityY": -2.050905460597173,
-            "timestamp": 0.49256233209175354
+            "x": 1.6657642530728491,
+            "y": 3.752962692607369,
+            "heading": -0.4767669793872945,
+            "angularVelocity": 6.256738607298855e-7,
+            "velocityX": 3.1785708603736573,
+            "velocityY": -2.051176638904815,
+            "timestamp": 0.49256075225499807
         },
         {
-            "x": 1.9267089834441824,
-            "y": 3.584747484136064,
-            "heading": 2.664319669351516,
-            "angularVelocity": 6.274860319306578e-7,
-            "velocityX": 3.1787453185027585,
-            "velocityY": -2.05090626699614,
-            "timestamp": 0.5746560541070458
+            "x": 1.926704086710966,
+            "y": 3.584574442746028,
+            "heading": -0.476766928024127,
+            "angularVelocity": 6.256669936190949e-7,
+            "velocityX": 3.178570348248477,
+            "velocityY": -2.051177432514957,
+            "timestamp": 0.5746542109641644
         },
         {
-            "x": 2.1876640582968827,
-            "y": 3.416381017663811,
-            "heading": 2.6643197208641496,
-            "angularVelocity": 6.274856686006067e-7,
-            "velocityX": 3.1787458096258594,
-            "velocityY": -2.0509055057936005,
-            "timestamp": 0.6567497761223381
+            "x": 2.187643960670788,
+            "y": 3.416186255368488,
+            "heading": -0.47676687666098916,
+            "angularVelocity": 6.256666327585045e-7,
+            "velocityX": 3.178570839416784,
+            "velocityY": -2.0511766713848885,
+            "timestamp": 0.6567476696733308
         },
         {
-            "x": 2.4486192865071836,
-            "y": 3.248014788884269,
-            "heading": 2.6643197723767935,
-            "angularVelocity": 6.274857907572903e-7,
-            "velocityX": 3.178747677705374,
-            "velocityY": -2.050902610411298,
-            "timestamp": 0.7388434981376304
+            "x": 2.448583988107283,
+            "y": 3.2477983058236712,
+            "heading": -0.4767668252978414,
+            "angularVelocity": 6.256667527167073e-7,
+            "velocityX": 3.1785727089527405,
+            "velocityY": -2.051173774287752,
+            "timestamp": 0.7388411283824972
         },
         {
-            "x": 2.7195077051073344,
-            "y": 3.0961455009278414,
-            "heading": 2.664319841917229,
-            "angularVelocity": 8.470859127420906e-7,
-            "velocityX": 3.2997458508421667,
-            "velocityY": -1.849950084223712,
-            "timestamp": 0.8209372201529227
+            "x": 2.7194719503347105,
+            "y": 3.095930240616485,
+            "heading": -0.47676675593417933,
+            "angularVelocity": 8.449353114908744e-7,
+            "velocityX": 3.2997508752445928,
+            "velocityY": -1.8499411231437015,
+            "timestamp": 0.8209345870916636
         },
         {
-            "x": 2.9903964357661867,
-            "y": 2.9442767695902643,
-            "heading": 2.664319911457928,
-            "angularVelocity": 8.470891223849179e-7,
-            "velocityX": 3.2997496520914464,
-            "velocityY": -1.8499433039384836,
-            "timestamp": 0.903030942168215
+            "x": 2.9903602222167747,
+            "y": 2.9440627277435274,
+            "heading": -0.4767666865702561,
+            "angularVelocity": 8.449384925343815e-7,
+            "velocityX": 3.2997546472215804,
+            "velocityY": -1.8499343950287315,
+            "timestamp": 0.90302804580083
         },
         {
-            "x": 3.2612853045995296,
-            "y": 2.79240828471517,
-            "heading": 2.6643199809985263,
-            "angularVelocity": 8.470878995035866e-7,
-            "velocityX": 3.299751335222467,
-            "velocityY": -1.8499403017298282,
-            "timestamp": 0.9851246641835073
+            "x": 3.2612486310597473,
+            "y": 2.7921954591699243,
+            "heading": -0.4767666172064318,
+            "angularVelocity": 8.44937286881618e-7,
+            "velocityX": 3.299756315575062,
+            "velocityY": -1.8499314191600278,
+            "timestamp": 0.9851215045099964
         },
         {
-            "x": 3.5321743153682585,
-            "y": 2.6405400530113767,
-            "heading": 2.664320050538881,
-            "angularVelocity": 8.47084928778718e-7,
-            "velocityX": 3.299753064165714,
-            "velocityY": -1.8499372177997218,
-            "timestamp": 1.0672183861987996
+            "x": 3.5321371803805834,
+            "y": 2.6403284411694066,
+            "heading": -0.4767665478428475,
+            "angularVelocity": 8.449343642301634e-7,
+            "velocityX": 3.2997580267694278,
+            "velocityY": -1.84992836686951,
+            "timestamp": 1.0672149632191628
         },
         {
-            "x": 3.8030633934021476,
-            "y": 2.4886719412892284,
-            "heading": 2.6643201200792013,
-            "angularVelocity": 8.470845077302885e-7,
-            "velocityX": 3.299753883536049,
-            "velocityY": -1.8499357562793664,
-            "timestamp": 1.1493121082140918
+            "x": 3.8030257968728702,
+            "y": 2.488461542984137,
+            "heading": -0.47676647847929754,
+            "angularVelocity": 8.449339449505073e-7,
+            "velocityX": 3.2997588450008877,
+            "velocityY": -1.849926907371401,
+            "timestamp": 1.1493084219283292
         },
         {
-            "x": 4.073952519237506,
-            "y": 2.3368039148311976,
-            "heading": 2.664320189619518,
-            "angularVelocity": 8.470844698893479e-7,
-            "velocityX": 3.2997544658152695,
-            "velocityY": -1.8499347176601568,
-            "timestamp": 1.231405830229384
+            "x": 4.07391446115854,
+            "y": 2.336594730049097,
+            "heading": -0.4767664091157508,
+            "angularVelocity": 8.449339066268147e-7,
+            "velocityX": 3.2997594271834845,
+            "velocityY": -1.8499258689180111,
+            "timestamp": 1.2314018806374956
         },
         {
-            "x": 4.3448416924892905,
-            "y": 2.1849359729505413,
-            "heading": 2.6643202591598345,
-            "angularVelocity": 8.470844641828709e-7,
-            "velocityX": 3.2997550434042244,
-            "velocityY": -1.8499336874062895,
-            "timestamp": 1.3134995522446762
+            "x": 4.344803172855204,
+            "y": 2.184728001682272,
+            "heading": -0.4767663397522045,
+            "angularVelocity": 8.449339005791607e-7,
+            "velocityX": 3.2997600047081037,
+            "velocityY": -1.8499248387724243,
+            "timestamp": 1.313495339346662
         },
         {
-            "x": 4.615730948551723,
-            "y": 2.0330681787806033,
-            "heading": 2.664320328700174,
-            "angularVelocity": 8.470847405524923e-7,
-            "velocityX": 3.29975605213723,
-            "velocityY": -1.8499318881125704,
-            "timestamp": 1.3955932742599684
+            "x": 4.6156919675948584,
+            "y": 2.0328614214415324,
+            "heading": -0.47676627038863556,
+            "angularVelocity": 8.449341769958558e-7,
+            "velocityX": 3.2997610162745823,
+            "velocityY": -1.8499230344132471,
+            "timestamp": 1.3955887980558284
         },
         {
-            "x": 4.889033051365783,
-            "y": 1.8855867361226037,
-            "heading": 2.6643204031763625,
-            "angularVelocity": 9.072093065949494e-7,
-            "velocityX": 3.3291474196181556,
-            "velocityY": -1.7965008655659056,
-            "timestamp": 1.4776869962752606
+            "x": 4.88903699417146,
+            "y": 1.8854616483024196,
+            "heading": -0.4767661960128138,
+            "angularVelocity": 9.059896238747082e-7,
+            "velocityX": 3.3296809620969254,
+            "velocityY": -1.7955117917653842,
+            "timestamp": 1.4776822567649948
         },
         {
-            "x": 5.16486669963939,
-            "y": 1.742896114789829,
-            "heading": 2.6643204810266874,
-            "angularVelocity": 9.483103351737687e-7,
-            "velocityX": 3.359984679732577,
-            "velocityY": -1.738142915559301,
-            "timestamp": 1.5597807182905528
+            "x": 5.164941930672226,
+            "y": 1.7429110878363532,
+            "heading": -0.47676611821696785,
+            "angularVelocity": 9.476497536578291e-7,
+            "velocityX": 3.3608638354271223,
+            "velocityY": -1.736442375647519,
+            "timestamp": 1.5597757154741612
         },
         {
-            "x": 5.440736180585003,
-            "y": 1.6002747825074959,
-            "heading": 2.6643205588558216,
-            "angularVelocity": 9.480522059758943e-7,
-            "velocityX": 3.3604211646564863,
-            "velocityY": -1.7372988918174013,
-            "timestamp": 1.641874440305845
+            "x": 5.4408826630316725,
+            "y": 1.6004298311654348,
+            "heading": -0.4767660404421832,
+            "angularVelocity": 9.473932014254074e-7,
+            "velocityX": 3.3612998733190906,
+            "velocityY": -1.7355981695897218,
+            "timestamp": 1.6418691741833276
         },
         {
-            "x": 5.732995703254166,
-            "y": 1.4952548268359402,
-            "heading": 2.664320597611399,
-            "angularVelocity": 4.720894158515159e-7,
-            "velocityX": 3.5600715315931244,
-            "velocityY": -1.2792690243961995,
-            "timestamp": 1.7239681623211371
+            "x": 5.73313461063715,
+            "y": 1.4953917422787115,
+            "heading": -0.4767660015440003,
+            "angularVelocity": 4.738280431565268e-7,
+            "velocityX": 3.55999067649034,
+            "velocityY": -1.2794940125356749,
+            "timestamp": 1.723962632892494
         },
         {
-            "x": 6.025255587661767,
-            "y": 1.3902358778499353,
-            "heading": 2.6643206363668037,
-            "angularVelocity": 4.720873108891009e-7,
-            "velocityX": 3.560075938001194,
-            "velocityY": -1.2792567617587365,
-            "timestamp": 1.8060618843364293
+            "x": 6.025386917456382,
+            "y": 1.3903546528530497,
+            "heading": -0.476765962645991,
+            "angularVelocity": 4.738259283996548e-7,
+            "velocityX": 3.5599950521587655,
+            "velocityY": -1.2794818378621156,
+            "timestamp": 1.8060560916016604
         },
         {
-            "x": 6.317515503776411,
-            "y": 1.2852170171023363,
-            "heading": 2.664320675122227,
-            "angularVelocity": 4.7208754093652347e-7,
-            "velocityX": 3.5600763242310083,
-            "velocityY": -1.2792556869091232,
-            "timestamp": 1.8881556063517215
+            "x": 6.31763925599377,
+            "y": 1.2853176516791878,
+            "heading": -0.4767659237479627,
+            "angularVelocity": 4.7382616041897537e-7,
+            "velocityX": 3.5599954385251906,
+            "velocityY": -1.2794807628458942,
+            "timestamp": 1.8881495503108268
         },
         {
-            "x": 6.60977544021274,
-            "y": 1.1801982129085715,
-            "heading": 2.6643207138776748,
-            "angularVelocity": 4.720878420422611e-7,
-            "velocityX": 3.5600765717735112,
-            "velocityY": -1.2792549980155836,
-            "timestamp": 1.9702493283670137
+            "x": 6.609891614856598,
+            "y": 1.1802807070583716,
+            "heading": -0.4767658848499098,
+            "angularVelocity": 4.7382646005084667e-7,
+            "velocityX": 3.5599956861142137,
+            "velocityY": -1.279480073959752,
+            "timestamp": 1.9702430090199932
         },
         {
-            "x": 6.902035364394445,
-            "y": 1.0751793746113998,
-            "heading": 2.664320752633444,
-            "angularVelocity": 4.720917543940102e-7,
-            "velocityX": 3.5600764224974903,
-            "velocityY": -1.2792554134359897,
-            "timestamp": 2.052343050382306
+            "x": 6.9021439611938415,
+            "y": 1.0752437275870097,
+            "heading": -0.4767658459515333,
+            "angularVelocity": 4.738304011500936e-7,
+            "velocityX": 3.559995533537077,
+            "velocityY": -1.2794804984825638,
+            "timestamp": 2.0523364677291593
         },
         {
-            "x": 7.1614900474496475,
-            "y": 0.9773985940984262,
-            "heading": 2.7732250681220454,
-            "angularVelocity": 1.3265851859940632,
-            "velocityX": 3.1604692379140946,
-            "velocityY": -1.1910871880648686,
-            "timestamp": 2.1344367723975983
+            "x": 7.1615588383732955,
+            "y": 0.9774297585720934,
+            "heading": -0.3677133060860067,
+            "angularVelocity": 1.3283949973635365,
+            "velocityX": 3.159994489920166,
+            "velocityY": -1.1914952854093204,
+            "timestamp": 2.1344299264383255
         },
         {
-            "x": 7.369273961287434,
-            "y": 0.9006112993544645,
-            "heading": 2.9067869208138672,
-            "angularVelocity": 1.626943564173427,
-            "velocityX": 2.5310572932614863,
-            "velocityY": -0.935361351135448,
-            "timestamp": 2.2165304944128907
+            "x": 7.369316955835238,
+            "y": 0.9006256752646921,
+            "heading": -0.23381405774557934,
+            "angularVelocity": 1.6310586792888595,
+            "velocityX": 2.5307511795546396,
+            "velocityY": -0.9355688567038716,
+            "timestamp": 2.2165233851474917
         },
         {
-            "x": 7.524785907770447,
-            "y": 0.8435839044250819,
-            "heading": 3.0191766997290386,
-            "angularVelocity": 1.3690423111067538,
-            "velocityX": 1.8943220341994518,
-            "velocityY": -0.6946620707337358,
-            "timestamp": 2.298624216428183
+            "x": 7.5248080438305305,
+            "y": 0.8435895899389048,
+            "heading": -0.12113401657570753,
+            "angularVelocity": 1.3725824559185034,
+            "velocityX": 1.894074027824225,
+            "velocityY": -0.6947701585804769,
+            "timestamp": 2.298616843856658
         },
         {
-            "x": 7.62830248590124,
-            "y": 0.8058044628378582,
-            "heading": 3.098670404043058,
-            "angularVelocity": 0.9683286658535433,
-            "velocityX": 1.2609560827502735,
-            "velocityY": -0.460198912411186,
-            "timestamp": 2.3807179384434756
+            "x": 7.628310039444481,
+            "y": 0.8058059657857352,
+            "heading": -0.04143578092949631,
+            "angularVelocity": 0.9708232166043739,
+            "velocityX": 1.2607824940185441,
+            "velocityY": -0.4602513372840855,
+            "timestamp": 2.380710302565824
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 0.5034440508038269,
-            "velocityX": 0.6297377293860565,
-            "velocityY": -0.22906091204337628,
-            "timestamp": 2.462811660458768
+            "heading": 6.308726444888931e-32,
+            "angularVelocity": 0.5047391300236409,
+            "velocityX": 0.6296477376917574,
+            "velocityY": -0.22907995449868002,
+            "timestamp": 2.4628037612749902
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 1.633672716236069e-30,
-            "velocityX": -5.394468608823283e-31,
-            "velocityY": 5.5679704387753575e-31,
-            "timestamp": 2.5449053824740604
+            "heading": 0,
+            "angularVelocity": -2.9272372134325455e-30,
+            "velocityX": -7.795387702917406e-31,
+            "velocityY": 4.863651986550052e-31,
+            "timestamp": 2.5448972199841564
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.traj
index d8bd6114..f3a7c6d5 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.traj	
@@ -3,290 +3,290 @@
         {
             "x": 0.71,
             "y": 4.36,
-            "heading": 2.1,
+            "heading": -1.04,
             "angularVelocity": 0,
-            "velocityX": 8.526478185532948e-32,
-            "velocityY": -7.785685234118549e-31,
+            "velocityX": -3.7161803829578067e-31,
+            "velocityY": -1.7109886686245282e-31,
             "timestamp": 0
         },
         {
-            "x": 0.755735101962148,
-            "y": 4.33059726025757,
-            "heading": 2.1511666979053023,
-            "angularVelocity": 0.6232717514717016,
-            "velocityX": 0.5571083980530026,
-            "velocityY": -0.35816063665566134,
-            "timestamp": 0.08209372201529226
+            "x": 0.755738680236623,
+            "y": 4.330590345332975,
+            "heading": -0.988927755482241,
+            "angularVelocity": 0.6221231913092291,
+            "velocityX": 0.5571537727343439,
+            "velocityY": -0.35824601776392057,
+            "timestamp": 0.08209345870916634
         },
         {
-            "x": 0.8475193720501324,
-            "y": 4.271815657764732,
-            "heading": 2.250112545454498,
-            "angularVelocity": 1.2052791019849762,
-            "velocityX": 1.1180424986808006,
-            "velocityY": -0.7160304229096619,
-            "timestamp": 0.16418744403058452
+            "x": 0.8475293838257123,
+            "y": 4.27179448486036,
+            "heading": -0.8901763080939228,
+            "angularVelocity": 1.2029149330663034,
+            "velocityX": 1.1181244526957685,
+            "velocityY": -0.7162063993540811,
+            "timestamp": 0.1641869174183327
         },
         {
-            "x": 0.9858871774715989,
-            "y": 4.183669855686051,
-            "heading": 2.389658659666248,
-            "angularVelocity": 1.6998390472021188,
-            "velocityX": 1.6854858328349602,
-            "velocityY": -1.0737215942317955,
-            "timestamp": 0.2462811660458768
+            "x": 0.9859043696516769,
+            "y": 4.183626202129818,
+            "heading": -0.7509222083756354,
+            "angularVelocity": 1.6962874010659592,
+            "velocityX": 1.6855787050730526,
+            "velocityY": -1.0739988802627505,
+            "timestamp": 0.24628037612749903
         },
         {
-            "x": 1.17173213163872,
-            "y": 4.066157035949835,
-            "heading": 2.5522635706365153,
-            "angularVelocity": 1.9807228491842224,
-            "velocityX": 2.2638144501780824,
-            "velocityY": -1.4314470930472174,
-            "timestamp": 0.32837488806116905
+            "x": 1.1717528122715652,
+            "y": 4.066081969426692,
+            "heading": -0.5886701024002182,
+            "angularVelocity": 1.9764316003572233,
+            "velocityX": 2.263864204799754,
+            "velocityY": -1.4318343331050452,
+            "timestamp": 0.3283738348366654
         },
         {
-            "x": 1.4047988716632624,
-            "y": 3.9214804758601747,
-            "heading": 2.664319566325619,
-            "angularVelocity": 1.3649764310629042,
-            "velocityX": 2.839032441252049,
-            "velocityY": -1.76233403161705,
-            "timestamp": 0.4104686100764613
+            "x": 1.404824377392605,
+            "y": 3.921350877318508,
+            "heading": -0.4767670307510258,
+            "angularVelocity": 1.3631180048782332,
+            "velocityX": 2.8391003228009395,
+            "velocityY": -1.7630039516415732,
+            "timestamp": 0.4104672935458317
         },
         {
-            "x": 1.6657539489096052,
-            "y": 3.7531140130982656,
-            "heading": 2.6643196178388515,
-            "angularVelocity": 6.274929563351661e-7,
-            "velocityX": 3.178745838783293,
-            "velocityY": -2.050905460597173,
-            "timestamp": 0.49256233209175354
+            "x": 1.6657642530728491,
+            "y": 3.752962692607369,
+            "heading": -0.4767669793872945,
+            "angularVelocity": 6.256738607298855e-7,
+            "velocityX": 3.1785708603736573,
+            "velocityY": -2.051176638904815,
+            "timestamp": 0.49256075225499807
         },
         {
-            "x": 1.9267089834441824,
-            "y": 3.584747484136064,
-            "heading": 2.664319669351516,
-            "angularVelocity": 6.274860319306578e-7,
-            "velocityX": 3.1787453185027585,
-            "velocityY": -2.05090626699614,
-            "timestamp": 0.5746560541070458
+            "x": 1.926704086710966,
+            "y": 3.584574442746028,
+            "heading": -0.476766928024127,
+            "angularVelocity": 6.256669936190949e-7,
+            "velocityX": 3.178570348248477,
+            "velocityY": -2.051177432514957,
+            "timestamp": 0.5746542109641644
         },
         {
-            "x": 2.1876640582968827,
-            "y": 3.416381017663811,
-            "heading": 2.6643197208641496,
-            "angularVelocity": 6.274856686006067e-7,
-            "velocityX": 3.1787458096258594,
-            "velocityY": -2.0509055057936005,
-            "timestamp": 0.6567497761223381
+            "x": 2.187643960670788,
+            "y": 3.416186255368488,
+            "heading": -0.47676687666098916,
+            "angularVelocity": 6.256666327585045e-7,
+            "velocityX": 3.178570839416784,
+            "velocityY": -2.0511766713848885,
+            "timestamp": 0.6567476696733308
         },
         {
-            "x": 2.4486192865071836,
-            "y": 3.248014788884269,
-            "heading": 2.6643197723767935,
-            "angularVelocity": 6.274857907572903e-7,
-            "velocityX": 3.178747677705374,
-            "velocityY": -2.050902610411298,
-            "timestamp": 0.7388434981376304
+            "x": 2.448583988107283,
+            "y": 3.2477983058236712,
+            "heading": -0.4767668252978414,
+            "angularVelocity": 6.256667527167073e-7,
+            "velocityX": 3.1785727089527405,
+            "velocityY": -2.051173774287752,
+            "timestamp": 0.7388411283824972
         },
         {
-            "x": 2.7195077051073344,
-            "y": 3.0961455009278414,
-            "heading": 2.664319841917229,
-            "angularVelocity": 8.470859127420906e-7,
-            "velocityX": 3.2997458508421667,
-            "velocityY": -1.849950084223712,
-            "timestamp": 0.8209372201529227
+            "x": 2.7194719503347105,
+            "y": 3.095930240616485,
+            "heading": -0.47676675593417933,
+            "angularVelocity": 8.449353114908744e-7,
+            "velocityX": 3.2997508752445928,
+            "velocityY": -1.8499411231437015,
+            "timestamp": 0.8209345870916636
         },
         {
-            "x": 2.9903964357661867,
-            "y": 2.9442767695902643,
-            "heading": 2.664319911457928,
-            "angularVelocity": 8.470891223849179e-7,
-            "velocityX": 3.2997496520914464,
-            "velocityY": -1.8499433039384836,
-            "timestamp": 0.903030942168215
+            "x": 2.9903602222167747,
+            "y": 2.9440627277435274,
+            "heading": -0.4767666865702561,
+            "angularVelocity": 8.449384925343815e-7,
+            "velocityX": 3.2997546472215804,
+            "velocityY": -1.8499343950287315,
+            "timestamp": 0.90302804580083
         },
         {
-            "x": 3.2612853045995296,
-            "y": 2.79240828471517,
-            "heading": 2.6643199809985263,
-            "angularVelocity": 8.470878995035866e-7,
-            "velocityX": 3.299751335222467,
-            "velocityY": -1.8499403017298282,
-            "timestamp": 0.9851246641835073
+            "x": 3.2612486310597473,
+            "y": 2.7921954591699243,
+            "heading": -0.4767666172064318,
+            "angularVelocity": 8.44937286881618e-7,
+            "velocityX": 3.299756315575062,
+            "velocityY": -1.8499314191600278,
+            "timestamp": 0.9851215045099964
         },
         {
-            "x": 3.5321743153682585,
-            "y": 2.6405400530113767,
-            "heading": 2.664320050538881,
-            "angularVelocity": 8.47084928778718e-7,
-            "velocityX": 3.299753064165714,
-            "velocityY": -1.8499372177997218,
-            "timestamp": 1.0672183861987996
+            "x": 3.5321371803805834,
+            "y": 2.6403284411694066,
+            "heading": -0.4767665478428475,
+            "angularVelocity": 8.449343642301634e-7,
+            "velocityX": 3.2997580267694278,
+            "velocityY": -1.84992836686951,
+            "timestamp": 1.0672149632191628
         },
         {
-            "x": 3.8030633934021476,
-            "y": 2.4886719412892284,
-            "heading": 2.6643201200792013,
-            "angularVelocity": 8.470845077302885e-7,
-            "velocityX": 3.299753883536049,
-            "velocityY": -1.8499357562793664,
-            "timestamp": 1.1493121082140918
+            "x": 3.8030257968728702,
+            "y": 2.488461542984137,
+            "heading": -0.47676647847929754,
+            "angularVelocity": 8.449339449505073e-7,
+            "velocityX": 3.2997588450008877,
+            "velocityY": -1.849926907371401,
+            "timestamp": 1.1493084219283292
         },
         {
-            "x": 4.073952519237506,
-            "y": 2.3368039148311976,
-            "heading": 2.664320189619518,
-            "angularVelocity": 8.470844698893479e-7,
-            "velocityX": 3.2997544658152695,
-            "velocityY": -1.8499347176601568,
-            "timestamp": 1.231405830229384
+            "x": 4.07391446115854,
+            "y": 2.336594730049097,
+            "heading": -0.4767664091157508,
+            "angularVelocity": 8.449339066268147e-7,
+            "velocityX": 3.2997594271834845,
+            "velocityY": -1.8499258689180111,
+            "timestamp": 1.2314018806374956
         },
         {
-            "x": 4.3448416924892905,
-            "y": 2.1849359729505413,
-            "heading": 2.6643202591598345,
-            "angularVelocity": 8.470844641828709e-7,
-            "velocityX": 3.2997550434042244,
-            "velocityY": -1.8499336874062895,
-            "timestamp": 1.3134995522446762
+            "x": 4.344803172855204,
+            "y": 2.184728001682272,
+            "heading": -0.4767663397522045,
+            "angularVelocity": 8.449339005791607e-7,
+            "velocityX": 3.2997600047081037,
+            "velocityY": -1.8499248387724243,
+            "timestamp": 1.313495339346662
         },
         {
-            "x": 4.615730948551723,
-            "y": 2.0330681787806033,
-            "heading": 2.664320328700174,
-            "angularVelocity": 8.470847405524923e-7,
-            "velocityX": 3.29975605213723,
-            "velocityY": -1.8499318881125704,
-            "timestamp": 1.3955932742599684
+            "x": 4.6156919675948584,
+            "y": 2.0328614214415324,
+            "heading": -0.47676627038863556,
+            "angularVelocity": 8.449341769958558e-7,
+            "velocityX": 3.2997610162745823,
+            "velocityY": -1.8499230344132471,
+            "timestamp": 1.3955887980558284
         },
         {
-            "x": 4.889033051365783,
-            "y": 1.8855867361226037,
-            "heading": 2.6643204031763625,
-            "angularVelocity": 9.072093065949494e-7,
-            "velocityX": 3.3291474196181556,
-            "velocityY": -1.7965008655659056,
-            "timestamp": 1.4776869962752606
+            "x": 4.88903699417146,
+            "y": 1.8854616483024196,
+            "heading": -0.4767661960128138,
+            "angularVelocity": 9.059896238747082e-7,
+            "velocityX": 3.3296809620969254,
+            "velocityY": -1.7955117917653842,
+            "timestamp": 1.4776822567649948
         },
         {
-            "x": 5.16486669963939,
-            "y": 1.742896114789829,
-            "heading": 2.6643204810266874,
-            "angularVelocity": 9.483103351737687e-7,
-            "velocityX": 3.359984679732577,
-            "velocityY": -1.738142915559301,
-            "timestamp": 1.5597807182905528
+            "x": 5.164941930672226,
+            "y": 1.7429110878363532,
+            "heading": -0.47676611821696785,
+            "angularVelocity": 9.476497536578291e-7,
+            "velocityX": 3.3608638354271223,
+            "velocityY": -1.736442375647519,
+            "timestamp": 1.5597757154741612
         },
         {
-            "x": 5.440736180585003,
-            "y": 1.6002747825074959,
-            "heading": 2.6643205588558216,
-            "angularVelocity": 9.480522059758943e-7,
-            "velocityX": 3.3604211646564863,
-            "velocityY": -1.7372988918174013,
-            "timestamp": 1.641874440305845
+            "x": 5.4408826630316725,
+            "y": 1.6004298311654348,
+            "heading": -0.4767660404421832,
+            "angularVelocity": 9.473932014254074e-7,
+            "velocityX": 3.3612998733190906,
+            "velocityY": -1.7355981695897218,
+            "timestamp": 1.6418691741833276
         },
         {
-            "x": 5.732995703254166,
-            "y": 1.4952548268359402,
-            "heading": 2.664320597611399,
-            "angularVelocity": 4.720894158515159e-7,
-            "velocityX": 3.5600715315931244,
-            "velocityY": -1.2792690243961995,
-            "timestamp": 1.7239681623211371
+            "x": 5.73313461063715,
+            "y": 1.4953917422787115,
+            "heading": -0.4767660015440003,
+            "angularVelocity": 4.738280431565268e-7,
+            "velocityX": 3.55999067649034,
+            "velocityY": -1.2794940125356749,
+            "timestamp": 1.723962632892494
         },
         {
-            "x": 6.025255587661767,
-            "y": 1.3902358778499353,
-            "heading": 2.6643206363668037,
-            "angularVelocity": 4.720873108891009e-7,
-            "velocityX": 3.560075938001194,
-            "velocityY": -1.2792567617587365,
-            "timestamp": 1.8060618843364293
+            "x": 6.025386917456382,
+            "y": 1.3903546528530497,
+            "heading": -0.476765962645991,
+            "angularVelocity": 4.738259283996548e-7,
+            "velocityX": 3.5599950521587655,
+            "velocityY": -1.2794818378621156,
+            "timestamp": 1.8060560916016604
         },
         {
-            "x": 6.317515503776411,
-            "y": 1.2852170171023363,
-            "heading": 2.664320675122227,
-            "angularVelocity": 4.7208754093652347e-7,
-            "velocityX": 3.5600763242310083,
-            "velocityY": -1.2792556869091232,
-            "timestamp": 1.8881556063517215
+            "x": 6.31763925599377,
+            "y": 1.2853176516791878,
+            "heading": -0.4767659237479627,
+            "angularVelocity": 4.7382616041897537e-7,
+            "velocityX": 3.5599954385251906,
+            "velocityY": -1.2794807628458942,
+            "timestamp": 1.8881495503108268
         },
         {
-            "x": 6.60977544021274,
-            "y": 1.1801982129085715,
-            "heading": 2.6643207138776748,
-            "angularVelocity": 4.720878420422611e-7,
-            "velocityX": 3.5600765717735112,
-            "velocityY": -1.2792549980155836,
-            "timestamp": 1.9702493283670137
+            "x": 6.609891614856598,
+            "y": 1.1802807070583716,
+            "heading": -0.4767658848499098,
+            "angularVelocity": 4.7382646005084667e-7,
+            "velocityX": 3.5599956861142137,
+            "velocityY": -1.279480073959752,
+            "timestamp": 1.9702430090199932
         },
         {
-            "x": 6.902035364394445,
-            "y": 1.0751793746113998,
-            "heading": 2.664320752633444,
-            "angularVelocity": 4.720917543940102e-7,
-            "velocityX": 3.5600764224974903,
-            "velocityY": -1.2792554134359897,
-            "timestamp": 2.052343050382306
+            "x": 6.9021439611938415,
+            "y": 1.0752437275870097,
+            "heading": -0.4767658459515333,
+            "angularVelocity": 4.738304011500936e-7,
+            "velocityX": 3.559995533537077,
+            "velocityY": -1.2794804984825638,
+            "timestamp": 2.0523364677291593
         },
         {
-            "x": 7.1614900474496475,
-            "y": 0.9773985940984262,
-            "heading": 2.7732250681220454,
-            "angularVelocity": 1.3265851859940632,
-            "velocityX": 3.1604692379140946,
-            "velocityY": -1.1910871880648686,
-            "timestamp": 2.1344367723975983
+            "x": 7.1615588383732955,
+            "y": 0.9774297585720934,
+            "heading": -0.3677133060860067,
+            "angularVelocity": 1.3283949973635365,
+            "velocityX": 3.159994489920166,
+            "velocityY": -1.1914952854093204,
+            "timestamp": 2.1344299264383255
         },
         {
-            "x": 7.369273961287434,
-            "y": 0.9006112993544645,
-            "heading": 2.9067869208138672,
-            "angularVelocity": 1.626943564173427,
-            "velocityX": 2.5310572932614863,
-            "velocityY": -0.935361351135448,
-            "timestamp": 2.2165304944128907
+            "x": 7.369316955835238,
+            "y": 0.9006256752646921,
+            "heading": -0.23381405774557934,
+            "angularVelocity": 1.6310586792888595,
+            "velocityX": 2.5307511795546396,
+            "velocityY": -0.9355688567038716,
+            "timestamp": 2.2165233851474917
         },
         {
-            "x": 7.524785907770447,
-            "y": 0.8435839044250819,
-            "heading": 3.0191766997290386,
-            "angularVelocity": 1.3690423111067538,
-            "velocityX": 1.8943220341994518,
-            "velocityY": -0.6946620707337358,
-            "timestamp": 2.298624216428183
+            "x": 7.5248080438305305,
+            "y": 0.8435895899389048,
+            "heading": -0.12113401657570753,
+            "angularVelocity": 1.3725824559185034,
+            "velocityX": 1.894074027824225,
+            "velocityY": -0.6947701585804769,
+            "timestamp": 2.298616843856658
         },
         {
-            "x": 7.62830248590124,
-            "y": 0.8058044628378582,
-            "heading": 3.098670404043058,
-            "angularVelocity": 0.9683286658535433,
-            "velocityX": 1.2609560827502735,
-            "velocityY": -0.460198912411186,
-            "timestamp": 2.3807179384434756
+            "x": 7.628310039444481,
+            "y": 0.8058059657857352,
+            "heading": -0.04143578092949631,
+            "angularVelocity": 0.9708232166043739,
+            "velocityX": 1.2607824940185441,
+            "velocityY": -0.4602513372840855,
+            "timestamp": 2.380710302565824
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 0.5034440508038269,
-            "velocityX": 0.6297377293860565,
-            "velocityY": -0.22906091204337628,
-            "timestamp": 2.462811660458768
+            "heading": 6.308726444888931e-32,
+            "angularVelocity": 0.5047391300236409,
+            "velocityX": 0.6296477376917574,
+            "velocityY": -0.22907995449868002,
+            "timestamp": 2.4628037612749902
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 1.633672716236069e-30,
-            "velocityX": -5.394468608823283e-31,
-            "velocityY": 5.5679704387753575e-31,
-            "timestamp": 2.5449053824740604
+            "heading": 0,
+            "angularVelocity": -2.9272372134325455e-30,
+            "velocityX": -7.795387702917406e-31,
+            "velocityY": 4.863651986550052e-31,
+            "timestamp": 2.5448972199841564
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.1.traj
index 5a8bacb1..83dd207f 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.1.traj	
@@ -3,191 +3,191 @@
         {
             "x": 0.71,
             "y": 4.36,
-            "heading": 2.1,
-            "angularVelocity": 1.7952667234276065e-30,
-            "velocityX": 8.378323076428447e-32,
-            "velocityY": 1.4697485073236794e-33,
+            "heading": -1.04,
+            "angularVelocity": -9.2124979355083e-31,
+            "velocityX": -2.0292335982741827e-31,
+            "velocityY": 8.51805082704341e-32,
             "timestamp": 0
         },
         {
-            "x": 0.7269607278962015,
-            "y": 4.384959045157939,
-            "heading": 2.138406592301522,
-            "angularVelocity": 0.6202480809957288,
-            "velocityX": 0.2739076366713438,
-            "velocityY": 0.4030766317709414,
-            "timestamp": 0.06192133999006512
-        },
-        {
-            "x": 0.7608491761808356,
-            "y": 4.435120384688249,
-            "heading": 2.213462658415359,
-            "angularVelocity": 1.2121195394976814,
-            "velocityX": 0.5472822178924311,
-            "velocityY": 0.8100816219151135,
-            "timestamp": 0.12384267998013024
-        },
-        {
-            "x": 0.8116352150421299,
-            "y": 4.5108343869767795,
-            "heading": 2.3220976741276163,
-            "angularVelocity": 1.7544035017602526,
-            "velocityX": 0.8201702170760925,
-            "velocityY": 1.2227448937745646,
-            "timestamp": 0.18576401997019537
-        },
-        {
-            "x": 0.879352990275961,
-            "y": 4.612608489350272,
-            "heading": 2.458161516132951,
-            "angularVelocity": 2.197365916615584,
-            "velocityX": 1.093609654518071,
-            "velocityY": 1.6436030355580518,
-            "timestamp": 0.24768535996026048
-        },
-        {
-            "x": 0.9641073070497683,
-            "y": 4.741095934499406,
-            "heading": 2.609605752410294,
-            "angularVelocity": 2.445751921738789,
-            "velocityX": 1.3687416452454926,
-            "velocityY": 2.075010734098273,
-            "timestamp": 0.3096066999503256
-        },
-        {
-            "x": 1.065422526066935,
-            "y": 4.896666065794743,
-            "heading": 2.7474397378967583,
-            "angularVelocity": 2.2259528864940465,
-            "velocityX": 1.6361922890141296,
-            "velocityY": 2.5123831512738084,
-            "timestamp": 0.3715280399403907
-        },
-        {
-            "x": 1.1791068919270769,
-            "y": 5.075184614148285,
-            "heading": 2.811143860874752,
-            "angularVelocity": 1.0287910918629104,
-            "velocityX": 1.8359480895985416,
-            "velocityY": 2.882989101692323,
-            "timestamp": 0.4334493799304558
-        },
-        {
-            "x": 1.309522757298702,
-            "y": 5.269766971770384,
-            "heading": 2.8111439317105513,
-            "angularVelocity": 0.0000011439642533999806,
-            "velocityX": 2.1061537975849616,
-            "velocityY": 3.142411931869009,
-            "timestamp": 0.4953707199205209
-        },
-        {
-            "x": 1.4399391293197474,
-            "y": 5.464348989818422,
-            "heading": 2.811144002543952,
-            "angularVelocity": 0.0000011439255168645237,
-            "velocityX": 2.1061619797305715,
-            "velocityY": 3.142406447910498,
-            "timestamp": 0.5572920599105861
-        },
-        {
-            "x": 1.570355539071906,
-            "y": 5.658930982577611,
-            "heading": 2.811144073377348,
-            "angularVelocity": 0.0000011439254352819296,
-            "velocityX": 2.1061625890699887,
-            "velocityY": 3.1424060395076645,
-            "timestamp": 0.6192133999006512
-        },
-        {
-            "x": 1.7007719870139304,
-            "y": 5.853512949740463,
-            "heading": 2.811144144210739,
-            "angularVelocity": 0.0000011439253582531217,
-            "velocityX": 2.1061632058180395,
-            "velocityY": 3.1424056261390936,
-            "timestamp": 0.6811347398907163
-        },
-        {
-            "x": 1.8311879242943474,
-            "y": 6.048095259166571,
-            "heading": 2.811144215047033,
-            "angularVelocity": 0.0000011439722415453888,
-            "velocityX": 2.1061549588775286,
-            "velocityY": 3.1424111535268193,
-            "timestamp": 0.7430560798807814
-        },
-        {
-            "x": 1.9446720292676574,
-            "y": 6.226098534814738,
-            "heading": 2.87431577200777,
-            "angularVelocity": 1.0201904056157727,
-            "velocityX": 1.8327139721381605,
-            "velocityY": 2.8746676941540104,
-            "timestamp": 0.8049774198708465
-        },
-        {
-            "x": 2.045803800007245,
-            "y": 6.3800371688336455,
-            "heading": 3.00267788576032,
-            "angularVelocity": 2.072986692037753,
-            "velocityX": 1.6332296871452403,
-            "velocityY": 2.486035251233359,
-            "timestamp": 0.8668987598609116
-        },
-        {
-            "x": 2.1306920593857335,
-            "y": 6.5074855802636336,
-            "heading": 3.157852124619465,
-            "angularVelocity": 2.5059896779372313,
-            "velocityX": 1.370904754194726,
-            "velocityY": 2.0582308369042988,
-            "timestamp": 0.9288200998509767
-        },
-        {
-            "x": 2.1984321789198162,
-            "y": 6.6082437599743225,
-            "heading": 3.2989222381105456,
-            "angularVelocity": 2.278214804681454,
-            "velocityX": 1.0939705042712429,
-            "velocityY": 1.6271963708610766,
-            "timestamp": 0.9907414398410418
-        },
-        {
-            "x": 2.249205760326253,
-            "y": 6.6830338505565665,
-            "heading": 3.412569338792277,
-            "angularVelocity": 1.8353462748055114,
-            "velocityX": 0.8199690351433494,
-            "velocityY": 1.2078241619810486,
-            "timestamp": 1.052662779831107
-        },
-        {
-            "x": 2.2830654942840813,
-            "y": 6.732454760660193,
-            "heading": 3.492119269429147,
-            "angularVelocity": 1.2846933003974643,
-            "velocityX": 0.5468184952596451,
-            "velocityY": 0.7981240411069244,
-            "timestamp": 1.1145841198211721
+            "x": 0.7269596629981327,
+            "y": 4.384958058102961,
+            "heading": -1.0015712045971708,
+            "angularVelocity": 0.6206072078812727,
+            "velocityX": 0.27389068508516395,
+            "velocityY": 0.40306105333388875,
+            "timestamp": 0.06192128437248337
+        },
+        {
+            "x": 0.7608459539039729,
+            "y": 4.435118328610172,
+            "heading": -0.9264796986002697,
+            "angularVelocity": 1.212692965882186,
+            "velocityX": 0.5472478687941834,
+            "velocityY": 0.8100650853021081,
+            "timestamp": 0.12384256874496674
+        },
+        {
+            "x": 0.8116291734017863,
+            "y": 4.510832058036491,
+            "heading": -0.8178101616266298,
+            "angularVelocity": 1.754962579909447,
+            "velocityX": 0.8201254223399239,
+            "velocityY": 1.2227415854436723,
+            "timestamp": 0.1857638531174501
+        },
+        {
+            "x": 0.8793445718640878,
+            "y": 4.61260707850531,
+            "heading": -0.6817312766556918,
+            "angularVelocity": 2.1976108271973906,
+            "velocityX": 1.0935722530392615,
+            "velocityY": 1.6436193386525904,
+            "timestamp": 0.24768513748993348
+        },
+        {
+            "x": 0.9640980850122779,
+            "y": 4.741096013070982,
+            "heading": -0.5303097082973756,
+            "angularVelocity": 2.445388042138302,
+            "velocityX": 1.368729896465987,
+            "velocityY": 2.0750366512547602,
+            "timestamp": 0.3096064218624168
+        },
+        {
+            "x": 1.0654131930810256,
+            "y": 4.896663967052664,
+            "heading": -0.3925803189619664,
+            "angularVelocity": 2.2242657065526474,
+            "velocityX": 1.6361919668735143,
+            "velocityY": 2.512350245286791,
+            "timestamp": 0.37152770623490017
+        },
+        {
+            "x": 1.1790960248711007,
+            "y": 5.075186526688609,
+            "heading": -0.32893475345024337,
+            "angularVelocity": 1.0278463400220732,
+            "velocityX": 1.8359249641241828,
+            "velocityY": 2.8830564715365803,
+            "timestamp": 0.4334489906073835
+        },
+        {
+            "x": 1.3095059591702463,
+            "y": 5.269772606119823,
+            "heading": -0.32893468266066545,
+            "angularVelocity": 0.0000011432188240302224,
+            "velocityX": 2.106059905260899,
+            "velocityY": 3.1424748598671592,
+            "timestamp": 0.49537027497986685
+        },
+        {
+            "x": 1.439916398287199,
+            "y": 5.464358347226451,
+            "heading": -0.3289346118734761,
+            "angularVelocity": 0.0000011431802504043458,
+            "velocityX": 2.106068057834158,
+            "velocityY": 3.14246939608213,
+            "timestamp": 0.5572915593523502
+        },
+        {
+            "x": 1.5703268751378339,
+            "y": 5.65894406304414,
+            "heading": -0.32893454108629183,
+            "angularVelocity": 0.0000011431801680911917,
+            "velocityX": 2.1060686672156135,
+            "velocityY": 3.142468987677516,
+            "timestamp": 0.6192128437248335
+        },
+        {
+            "x": 1.700737390180202,
+            "y": 5.853529753265897,
+            "heading": -0.3289344702991123,
+            "angularVelocity": 0.0000011431800901048107,
+            "velocityX": 2.1060692839943727,
+            "velocityY": 3.1424685743150773,
+            "timestamp": 0.6811341280973169
+        },
+        {
+            "x": 1.8311473976991637,
+            "y": 6.048115783625596,
+            "heading": -0.32893439950904224,
+            "angularVelocity": 0.000001143226773133006,
+            "velocityX": 2.1060610877269452,
+            "velocityY": 3.1424740673849674,
+            "timestamp": 0.7430554124698002
+        },
+        {
+            "x": 1.9446375646824816,
+            "y": 6.226114798697167,
+            "heading": -0.26580646349628556,
+            "angularVelocity": 1.0194868638869756,
+            "velocityX": 1.8328135169261883,
+            "velocityY": 2.8746014698408153,
+            "timestamp": 0.8049766968422836
+        },
+        {
+            "x": 2.0457805429701614,
+            "y": 6.3800476604210195,
+            "heading": -0.13750031114094655,
+            "angularVelocity": 2.072084803401717,
+            "velocityX": 1.6334121508084516,
+            "velocityY": 2.485944264299801,
+            "timestamp": 0.8668979812147669
+        },
+        {
+            "x": 2.130677229255867,
+            "y": 6.507493779703991,
+            "heading": 0.017637122988449796,
+            "angularVelocity": 2.505397549511044,
+            "velocityX": 1.3710420761787727,
+            "velocityY": 2.0581956684930476,
+            "timestamp": 0.9288192655872503
+        },
+        {
+            "x": 2.198423808533576,
+            "y": 6.608249828696692,
+            "heading": 0.1586985563433061,
+            "angularVelocity": 2.2780766707988582,
+            "velocityX": 1.0940758087345794,
+            "velocityY": 1.6271634222993538,
+            "timestamp": 0.9907405499597336
+        },
+        {
+            "x": 2.2492018276993044,
+            "y": 6.683037485807007,
+            "heading": 0.27235601520130115,
+            "angularVelocity": 1.8355152030486992,
+            "velocityX": 0.8200414393906406,
+            "velocityY": 1.2077859474043515,
+            "timestamp": 1.052661834332217
+        },
+        {
+            "x": 2.2830642005186585,
+            "y": 6.732456177807481,
+            "heading": 0.3519224237627513,
+            "angularVelocity": 1.2849605651398268,
+            "velocityX": 0.5468616027997278,
+            "velocityY": 0.7980889366441329,
+            "timestamp": 1.1145831187047004
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 0.66319685195811,
-            "velocityX": 0.2734841610119513,
-            "velocityY": 0.3963938658908936,
-            "timestamp": 1.1765054598112372
+            "heading": 0.393,
+            "angularVelocity": 0.6633837888463243,
+            "velocityX": 0.27350530036594517,
+            "velocityY": 0.39637133566025207,
+            "timestamp": 1.1765044030771838
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": -1.1911850535673714e-30,
+            "heading": 0.393,
+            "angularVelocity": 4.204601117767945e-31,
             "velocityX": 0,
-            "velocityY": -7.498214836328326e-32,
-            "timestamp": 1.2384267998013023
+            "velocityY": -1.1377088902550092e-31,
+            "timestamp": 1.2384256874496673
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.traj
index 5a8bacb1..83dd207f 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.traj	
@@ -3,191 +3,191 @@
         {
             "x": 0.71,
             "y": 4.36,
-            "heading": 2.1,
-            "angularVelocity": 1.7952667234276065e-30,
-            "velocityX": 8.378323076428447e-32,
-            "velocityY": 1.4697485073236794e-33,
+            "heading": -1.04,
+            "angularVelocity": -9.2124979355083e-31,
+            "velocityX": -2.0292335982741827e-31,
+            "velocityY": 8.51805082704341e-32,
             "timestamp": 0
         },
         {
-            "x": 0.7269607278962015,
-            "y": 4.384959045157939,
-            "heading": 2.138406592301522,
-            "angularVelocity": 0.6202480809957288,
-            "velocityX": 0.2739076366713438,
-            "velocityY": 0.4030766317709414,
-            "timestamp": 0.06192133999006512
-        },
-        {
-            "x": 0.7608491761808356,
-            "y": 4.435120384688249,
-            "heading": 2.213462658415359,
-            "angularVelocity": 1.2121195394976814,
-            "velocityX": 0.5472822178924311,
-            "velocityY": 0.8100816219151135,
-            "timestamp": 0.12384267998013024
-        },
-        {
-            "x": 0.8116352150421299,
-            "y": 4.5108343869767795,
-            "heading": 2.3220976741276163,
-            "angularVelocity": 1.7544035017602526,
-            "velocityX": 0.8201702170760925,
-            "velocityY": 1.2227448937745646,
-            "timestamp": 0.18576401997019537
-        },
-        {
-            "x": 0.879352990275961,
-            "y": 4.612608489350272,
-            "heading": 2.458161516132951,
-            "angularVelocity": 2.197365916615584,
-            "velocityX": 1.093609654518071,
-            "velocityY": 1.6436030355580518,
-            "timestamp": 0.24768535996026048
-        },
-        {
-            "x": 0.9641073070497683,
-            "y": 4.741095934499406,
-            "heading": 2.609605752410294,
-            "angularVelocity": 2.445751921738789,
-            "velocityX": 1.3687416452454926,
-            "velocityY": 2.075010734098273,
-            "timestamp": 0.3096066999503256
-        },
-        {
-            "x": 1.065422526066935,
-            "y": 4.896666065794743,
-            "heading": 2.7474397378967583,
-            "angularVelocity": 2.2259528864940465,
-            "velocityX": 1.6361922890141296,
-            "velocityY": 2.5123831512738084,
-            "timestamp": 0.3715280399403907
-        },
-        {
-            "x": 1.1791068919270769,
-            "y": 5.075184614148285,
-            "heading": 2.811143860874752,
-            "angularVelocity": 1.0287910918629104,
-            "velocityX": 1.8359480895985416,
-            "velocityY": 2.882989101692323,
-            "timestamp": 0.4334493799304558
-        },
-        {
-            "x": 1.309522757298702,
-            "y": 5.269766971770384,
-            "heading": 2.8111439317105513,
-            "angularVelocity": 0.0000011439642533999806,
-            "velocityX": 2.1061537975849616,
-            "velocityY": 3.142411931869009,
-            "timestamp": 0.4953707199205209
-        },
-        {
-            "x": 1.4399391293197474,
-            "y": 5.464348989818422,
-            "heading": 2.811144002543952,
-            "angularVelocity": 0.0000011439255168645237,
-            "velocityX": 2.1061619797305715,
-            "velocityY": 3.142406447910498,
-            "timestamp": 0.5572920599105861
-        },
-        {
-            "x": 1.570355539071906,
-            "y": 5.658930982577611,
-            "heading": 2.811144073377348,
-            "angularVelocity": 0.0000011439254352819296,
-            "velocityX": 2.1061625890699887,
-            "velocityY": 3.1424060395076645,
-            "timestamp": 0.6192133999006512
-        },
-        {
-            "x": 1.7007719870139304,
-            "y": 5.853512949740463,
-            "heading": 2.811144144210739,
-            "angularVelocity": 0.0000011439253582531217,
-            "velocityX": 2.1061632058180395,
-            "velocityY": 3.1424056261390936,
-            "timestamp": 0.6811347398907163
-        },
-        {
-            "x": 1.8311879242943474,
-            "y": 6.048095259166571,
-            "heading": 2.811144215047033,
-            "angularVelocity": 0.0000011439722415453888,
-            "velocityX": 2.1061549588775286,
-            "velocityY": 3.1424111535268193,
-            "timestamp": 0.7430560798807814
-        },
-        {
-            "x": 1.9446720292676574,
-            "y": 6.226098534814738,
-            "heading": 2.87431577200777,
-            "angularVelocity": 1.0201904056157727,
-            "velocityX": 1.8327139721381605,
-            "velocityY": 2.8746676941540104,
-            "timestamp": 0.8049774198708465
-        },
-        {
-            "x": 2.045803800007245,
-            "y": 6.3800371688336455,
-            "heading": 3.00267788576032,
-            "angularVelocity": 2.072986692037753,
-            "velocityX": 1.6332296871452403,
-            "velocityY": 2.486035251233359,
-            "timestamp": 0.8668987598609116
-        },
-        {
-            "x": 2.1306920593857335,
-            "y": 6.5074855802636336,
-            "heading": 3.157852124619465,
-            "angularVelocity": 2.5059896779372313,
-            "velocityX": 1.370904754194726,
-            "velocityY": 2.0582308369042988,
-            "timestamp": 0.9288200998509767
-        },
-        {
-            "x": 2.1984321789198162,
-            "y": 6.6082437599743225,
-            "heading": 3.2989222381105456,
-            "angularVelocity": 2.278214804681454,
-            "velocityX": 1.0939705042712429,
-            "velocityY": 1.6271963708610766,
-            "timestamp": 0.9907414398410418
-        },
-        {
-            "x": 2.249205760326253,
-            "y": 6.6830338505565665,
-            "heading": 3.412569338792277,
-            "angularVelocity": 1.8353462748055114,
-            "velocityX": 0.8199690351433494,
-            "velocityY": 1.2078241619810486,
-            "timestamp": 1.052662779831107
-        },
-        {
-            "x": 2.2830654942840813,
-            "y": 6.732454760660193,
-            "heading": 3.492119269429147,
-            "angularVelocity": 1.2846933003974643,
-            "velocityX": 0.5468184952596451,
-            "velocityY": 0.7981240411069244,
-            "timestamp": 1.1145841198211721
+            "x": 0.7269596629981327,
+            "y": 4.384958058102961,
+            "heading": -1.0015712045971708,
+            "angularVelocity": 0.6206072078812727,
+            "velocityX": 0.27389068508516395,
+            "velocityY": 0.40306105333388875,
+            "timestamp": 0.06192128437248337
+        },
+        {
+            "x": 0.7608459539039729,
+            "y": 4.435118328610172,
+            "heading": -0.9264796986002697,
+            "angularVelocity": 1.212692965882186,
+            "velocityX": 0.5472478687941834,
+            "velocityY": 0.8100650853021081,
+            "timestamp": 0.12384256874496674
+        },
+        {
+            "x": 0.8116291734017863,
+            "y": 4.510832058036491,
+            "heading": -0.8178101616266298,
+            "angularVelocity": 1.754962579909447,
+            "velocityX": 0.8201254223399239,
+            "velocityY": 1.2227415854436723,
+            "timestamp": 0.1857638531174501
+        },
+        {
+            "x": 0.8793445718640878,
+            "y": 4.61260707850531,
+            "heading": -0.6817312766556918,
+            "angularVelocity": 2.1976108271973906,
+            "velocityX": 1.0935722530392615,
+            "velocityY": 1.6436193386525904,
+            "timestamp": 0.24768513748993348
+        },
+        {
+            "x": 0.9640980850122779,
+            "y": 4.741096013070982,
+            "heading": -0.5303097082973756,
+            "angularVelocity": 2.445388042138302,
+            "velocityX": 1.368729896465987,
+            "velocityY": 2.0750366512547602,
+            "timestamp": 0.3096064218624168
+        },
+        {
+            "x": 1.0654131930810256,
+            "y": 4.896663967052664,
+            "heading": -0.3925803189619664,
+            "angularVelocity": 2.2242657065526474,
+            "velocityX": 1.6361919668735143,
+            "velocityY": 2.512350245286791,
+            "timestamp": 0.37152770623490017
+        },
+        {
+            "x": 1.1790960248711007,
+            "y": 5.075186526688609,
+            "heading": -0.32893475345024337,
+            "angularVelocity": 1.0278463400220732,
+            "velocityX": 1.8359249641241828,
+            "velocityY": 2.8830564715365803,
+            "timestamp": 0.4334489906073835
+        },
+        {
+            "x": 1.3095059591702463,
+            "y": 5.269772606119823,
+            "heading": -0.32893468266066545,
+            "angularVelocity": 0.0000011432188240302224,
+            "velocityX": 2.106059905260899,
+            "velocityY": 3.1424748598671592,
+            "timestamp": 0.49537027497986685
+        },
+        {
+            "x": 1.439916398287199,
+            "y": 5.464358347226451,
+            "heading": -0.3289346118734761,
+            "angularVelocity": 0.0000011431802504043458,
+            "velocityX": 2.106068057834158,
+            "velocityY": 3.14246939608213,
+            "timestamp": 0.5572915593523502
+        },
+        {
+            "x": 1.5703268751378339,
+            "y": 5.65894406304414,
+            "heading": -0.32893454108629183,
+            "angularVelocity": 0.0000011431801680911917,
+            "velocityX": 2.1060686672156135,
+            "velocityY": 3.142468987677516,
+            "timestamp": 0.6192128437248335
+        },
+        {
+            "x": 1.700737390180202,
+            "y": 5.853529753265897,
+            "heading": -0.3289344702991123,
+            "angularVelocity": 0.0000011431800901048107,
+            "velocityX": 2.1060692839943727,
+            "velocityY": 3.1424685743150773,
+            "timestamp": 0.6811341280973169
+        },
+        {
+            "x": 1.8311473976991637,
+            "y": 6.048115783625596,
+            "heading": -0.32893439950904224,
+            "angularVelocity": 0.000001143226773133006,
+            "velocityX": 2.1060610877269452,
+            "velocityY": 3.1424740673849674,
+            "timestamp": 0.7430554124698002
+        },
+        {
+            "x": 1.9446375646824816,
+            "y": 6.226114798697167,
+            "heading": -0.26580646349628556,
+            "angularVelocity": 1.0194868638869756,
+            "velocityX": 1.8328135169261883,
+            "velocityY": 2.8746014698408153,
+            "timestamp": 0.8049766968422836
+        },
+        {
+            "x": 2.0457805429701614,
+            "y": 6.3800476604210195,
+            "heading": -0.13750031114094655,
+            "angularVelocity": 2.072084803401717,
+            "velocityX": 1.6334121508084516,
+            "velocityY": 2.485944264299801,
+            "timestamp": 0.8668979812147669
+        },
+        {
+            "x": 2.130677229255867,
+            "y": 6.507493779703991,
+            "heading": 0.017637122988449796,
+            "angularVelocity": 2.505397549511044,
+            "velocityX": 1.3710420761787727,
+            "velocityY": 2.0581956684930476,
+            "timestamp": 0.9288192655872503
+        },
+        {
+            "x": 2.198423808533576,
+            "y": 6.608249828696692,
+            "heading": 0.1586985563433061,
+            "angularVelocity": 2.2780766707988582,
+            "velocityX": 1.0940758087345794,
+            "velocityY": 1.6271634222993538,
+            "timestamp": 0.9907405499597336
+        },
+        {
+            "x": 2.2492018276993044,
+            "y": 6.683037485807007,
+            "heading": 0.27235601520130115,
+            "angularVelocity": 1.8355152030486992,
+            "velocityX": 0.8200414393906406,
+            "velocityY": 1.2077859474043515,
+            "timestamp": 1.052661834332217
+        },
+        {
+            "x": 2.2830642005186585,
+            "y": 6.732456177807481,
+            "heading": 0.3519224237627513,
+            "angularVelocity": 1.2849605651398268,
+            "velocityX": 0.5468616027997278,
+            "velocityY": 0.7980889366441329,
+            "timestamp": 1.1145831187047004
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 0.66319685195811,
-            "velocityX": 0.2734841610119513,
-            "velocityY": 0.3963938658908936,
-            "timestamp": 1.1765054598112372
+            "heading": 0.393,
+            "angularVelocity": 0.6633837888463243,
+            "velocityX": 0.27350530036594517,
+            "velocityY": 0.39637133566025207,
+            "timestamp": 1.1765044030771838
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": -1.1911850535673714e-30,
+            "heading": 0.393,
+            "angularVelocity": 4.204601117767945e-31,
             "velocityX": 0,
-            "velocityY": -7.498214836328326e-32,
-            "timestamp": 1.2384267998013023
+            "velocityY": -1.1377088902550092e-31,
+            "timestamp": 1.2384256874496673
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.1.traj
index 67ac392d..856b6994 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.1.traj	
@@ -3,146 +3,146 @@
         {
             "x": 0.71,
             "y": 4.36,
-            "heading": 2.1,
-            "angularVelocity": 8.845615831720266e-31,
-            "velocityX": 5.520878611448458e-31,
-            "velocityY": 4.542986326160959e-31,
+            "heading": -1.04,
+            "angularVelocity": 1.681543952112751e-30,
+            "velocityX": -3.802997916124736e-32,
+            "velocityY": 9.397294493952762e-34,
             "timestamp": 0
         },
         {
-            "x": 0.7374422286617038,
-            "y": 4.3811878915004145,
-            "heading": 2.1328463142733396,
-            "angularVelocity": 0.5012247413747498,
-            "velocityX": 0.41876004258027855,
-            "velocityY": 0.3233207643693322,
-            "timestamp": 0.06553210877669326
+            "x": 0.7374413965489649,
+            "y": 4.381188295193423,
+            "heading": -1.0071421527847422,
+            "angularVelocity": 0.5013988353179083,
+            "velocityX": 0.41874576197914515,
+            "velocityY": 0.3233257024648049,
+            "timestamp": 0.06553235648109366
         },
         {
-            "x": 0.7924456920447251,
-            "y": 4.423718037322063,
-            "heading": 2.19563146481745,
-            "angularVelocity": 0.9580822548845105,
-            "velocityX": 0.8393360813467762,
-            "velocityY": 0.6489970583210544,
-            "timestamp": 0.13106421755338651
+            "x": 0.7924436098605977,
+            "y": 4.423719284711624,
+            "heading": -0.9443396170050365,
+            "angularVelocity": 0.9583439258410564,
+            "velocityX": 0.8393138331215114,
+            "velocityY": 0.6490074796939781,
+            "timestamp": 0.13106471296218733
         },
         {
-            "x": 0.8751785876629536,
-            "y": 4.487773457465306,
-            "heading": 2.284091563401004,
-            "angularVelocity": 1.3498741339912115,
-            "velocityX": 1.262478762893885,
-            "velocityY": 0.9774661816777632,
-            "timestamp": 0.19659632633007978
+            "x": 0.8751753442261428,
+            "y": 4.487775925735343,
+            "heading": -0.8558629917892655,
+            "angularVelocity": 1.350121222045466,
+            "velocityX": 1.2624562705815747,
+            "velocityY": 0.9774811171669098,
+            "timestamp": 0.19659706944328098
         },
         {
-            "x": 0.985868892669873,
-            "y": 4.573562834319481,
-            "heading": 2.3916808862774843,
-            "angularVelocity": 1.641780264436542,
-            "velocityX": 1.6891003062957237,
-            "velocityY": 1.309119734671004,
-            "timestamp": 0.26212843510677303
+            "x": 0.9858650513706745,
+            "y": 4.5735667081068,
+            "heading": -0.748264583846062,
+            "angularVelocity": 1.641912693529484,
+            "velocityX": 1.6890847985371011,
+            "velocityY": 1.3091362340405182,
+            "timestamp": 0.26212942592437466
         },
         {
-            "x": 1.1247521909380378,
-            "y": 4.681315175779475,
-            "heading": 2.507240555710109,
-            "angularVelocity": 1.7634053228228717,
-            "velocityX": 2.1193167877661256,
-            "velocityY": 1.6442678783186102,
-            "timestamp": 0.32766054388346627
+            "x": 1.1247485645977424,
+            "y": 4.681320324271238,
+            "heading": -0.6327125157113742,
+            "angularVelocity": 1.763282664312989,
+            "velocityX": 2.1193120571993105,
+            "velocityY": 1.6442811147120544,
+            "timestamp": 0.32766178240546834
         },
         {
-            "x": 1.291492862531941,
-            "y": 4.810942835643569,
-            "heading": 2.6039577624805075,
-            "angularVelocity": 1.4758750874318616,
-            "velocityX": 2.5444118113470626,
-            "velocityY": 1.9780785676502115,
-            "timestamp": 0.3931926526601595
+            "x": 1.291488910381981,
+            "y": 4.810947401273882,
+            "heading": -0.5360470717928266,
+            "angularVelocity": 1.4750796264503891,
+            "velocityX": 2.544397222040073,
+            "velocityY": 1.9780621964975005,
+            "timestamp": 0.393194138886562
         },
         {
-            "x": 1.4800025601939901,
-            "y": 4.960730002903915,
-            "heading": 2.6228917229305413,
-            "angularVelocity": 0.2889264637363189,
-            "velocityX": 2.8766005120392,
-            "velocityY": 2.28570650413168,
-            "timestamp": 0.45872476143685276
+            "x": 1.4799946113507723,
+            "y": 4.960741770751019,
+            "heading": -0.5171262553925533,
+            "angularVelocity": 0.2887247981954086,
+            "velocityX": 2.8765286507463124,
+            "velocityY": 2.2858077676538255,
+            "timestamp": 0.4587264953676557
         },
         {
-            "x": 1.6682727616271493,
-            "y": 5.110599963848934,
-            "heading": 2.6420440088534156,
-            "angularVelocity": 0.2922580438871624,
-            "velocityX": 2.8729458726059685,
-            "velocityY": 2.2869699105167616,
-            "timestamp": 0.5242568702135461
+            "x": 1.6682616674549375,
+            "y": 5.11061398659558,
+            "heading": -0.49798039258281773,
+            "angularVelocity": 0.29215892480933153,
+            "velocityX": 2.8728870166371596,
+            "velocityY": 2.2869956749959233,
+            "timestamp": 0.5242588518487493
         },
         {
-            "x": 1.8350043100801503,
-            "y": 5.239747952271642,
-            "heading": 2.7356929069056295,
-            "angularVelocity": 1.429053631881899,
-            "velocityX": 2.544272594998539,
-            "velocityY": 1.9707589277004427,
-            "timestamp": 0.5897889789902393
+            "x": 1.8349961429002288,
+            "y": 5.239757672124722,
+            "heading": -0.4043635195675931,
+            "angularVelocity": 1.4285595397783832,
+            "velocityX": 2.5443076427962366,
+            "velocityY": 1.970685817873752,
+            "timestamp": 0.589791208329843
         },
         {
-            "x": 1.9739563679504606,
-            "y": 5.347167856813287,
-            "heading": 2.8514085217331835,
-            "angularVelocity": 1.765785002003577,
-            "velocityX": 2.1203660383309844,
-            "velocityY": 1.6391949923004236,
-            "timestamp": 0.6553210877669325
+            "x": 1.9739512346286359,
+            "y": 5.34717518731749,
+            "heading": -0.2886497176580553,
+            "angularVelocity": 1.7657506630777153,
+            "velocityX": 2.120404319177778,
+            "velocityY": 1.6391523357435742,
+            "timestamp": 0.6553235648109367
         },
         {
-            "x": 2.0846918002256074,
-            "y": 5.4326634298654595,
-            "heading": 2.9588833399365435,
-            "angularVelocity": 1.640032958035698,
-            "velocityX": 1.6897889346501833,
-            "velocityY": 1.304636378229562,
-            "timestamp": 0.7208531965436258
+            "x": 2.084688896220359,
+            "y": 5.432668536845291,
+            "heading": -0.1811622358281448,
+            "angularVelocity": 1.6402200012587402,
+            "velocityX": 1.6898165660145774,
+            "velocityY": 1.304597516685743,
+            "timestamp": 0.7208559212920304
         },
         {
-            "x": 2.1674771737861436,
-            "y": 5.496490038501947,
-            "heading": 3.046413104094783,
-            "angularVelocity": 1.3356775143083182,
-            "velocityX": 1.2632795602936622,
-            "velocityY": 0.9739745878464279,
-            "timestamp": 0.786385305320319
+            "x": 2.167475787005346,
+            "y": 5.496492971314002,
+            "heading": -0.09361417394502371,
+            "angularVelocity": 1.3359516822560165,
+            "velocityX": 1.2632979375443678,
+            "velocityY": 0.97393772932798,
+            "timestamp": 0.786388277773124
         },
         {
-            "x": 2.2225294464291743,
-            "y": 5.538874362150552,
-            "heading": 3.1080101238304096,
-            "angularVelocity": 0.9399517409934369,
-            "velocityX": 0.8400808957732031,
-            "velocityY": 0.6467718564197722,
-            "timestamp": 0.8519174140970123
+            "x": 2.222528989324329,
+            "y": 5.538875463156123,
+            "heading": -0.0320000732344696,
+            "angularVelocity": 0.9402088375736564,
+            "velocityX": 0.8400919068867495,
+            "velocityY": 0.6467414589974029,
+            "timestamp": 0.8519206342542177
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0.4881557570289305,
-            "velocityX": 0.41919227205757975,
-            "velocityY": 0.3223707926359362,
-            "timestamp": 0.9174495228737055
+            "heading": 0,
+            "angularVelocity": 0.48830951537200207,
+            "velocityX": 0.41919766281556964,
+            "velocityY": 0.32235277316741867,
+            "timestamp": 0.9174529907353114
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 7.973690679262731e-27,
-            "velocityX": -1.618419533015103e-29,
-            "velocityY": -2.1923403816587234e-30,
-            "timestamp": 0.9829816316503988
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 1.4283065738497147e-31,
+            "velocityY": -2.9738366407041548e-31,
+            "timestamp": 0.9829853472164051
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.traj
index 67ac392d..856b6994 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.traj	
@@ -3,146 +3,146 @@
         {
             "x": 0.71,
             "y": 4.36,
-            "heading": 2.1,
-            "angularVelocity": 8.845615831720266e-31,
-            "velocityX": 5.520878611448458e-31,
-            "velocityY": 4.542986326160959e-31,
+            "heading": -1.04,
+            "angularVelocity": 1.681543952112751e-30,
+            "velocityX": -3.802997916124736e-32,
+            "velocityY": 9.397294493952762e-34,
             "timestamp": 0
         },
         {
-            "x": 0.7374422286617038,
-            "y": 4.3811878915004145,
-            "heading": 2.1328463142733396,
-            "angularVelocity": 0.5012247413747498,
-            "velocityX": 0.41876004258027855,
-            "velocityY": 0.3233207643693322,
-            "timestamp": 0.06553210877669326
+            "x": 0.7374413965489649,
+            "y": 4.381188295193423,
+            "heading": -1.0071421527847422,
+            "angularVelocity": 0.5013988353179083,
+            "velocityX": 0.41874576197914515,
+            "velocityY": 0.3233257024648049,
+            "timestamp": 0.06553235648109366
         },
         {
-            "x": 0.7924456920447251,
-            "y": 4.423718037322063,
-            "heading": 2.19563146481745,
-            "angularVelocity": 0.9580822548845105,
-            "velocityX": 0.8393360813467762,
-            "velocityY": 0.6489970583210544,
-            "timestamp": 0.13106421755338651
+            "x": 0.7924436098605977,
+            "y": 4.423719284711624,
+            "heading": -0.9443396170050365,
+            "angularVelocity": 0.9583439258410564,
+            "velocityX": 0.8393138331215114,
+            "velocityY": 0.6490074796939781,
+            "timestamp": 0.13106471296218733
         },
         {
-            "x": 0.8751785876629536,
-            "y": 4.487773457465306,
-            "heading": 2.284091563401004,
-            "angularVelocity": 1.3498741339912115,
-            "velocityX": 1.262478762893885,
-            "velocityY": 0.9774661816777632,
-            "timestamp": 0.19659632633007978
+            "x": 0.8751753442261428,
+            "y": 4.487775925735343,
+            "heading": -0.8558629917892655,
+            "angularVelocity": 1.350121222045466,
+            "velocityX": 1.2624562705815747,
+            "velocityY": 0.9774811171669098,
+            "timestamp": 0.19659706944328098
         },
         {
-            "x": 0.985868892669873,
-            "y": 4.573562834319481,
-            "heading": 2.3916808862774843,
-            "angularVelocity": 1.641780264436542,
-            "velocityX": 1.6891003062957237,
-            "velocityY": 1.309119734671004,
-            "timestamp": 0.26212843510677303
+            "x": 0.9858650513706745,
+            "y": 4.5735667081068,
+            "heading": -0.748264583846062,
+            "angularVelocity": 1.641912693529484,
+            "velocityX": 1.6890847985371011,
+            "velocityY": 1.3091362340405182,
+            "timestamp": 0.26212942592437466
         },
         {
-            "x": 1.1247521909380378,
-            "y": 4.681315175779475,
-            "heading": 2.507240555710109,
-            "angularVelocity": 1.7634053228228717,
-            "velocityX": 2.1193167877661256,
-            "velocityY": 1.6442678783186102,
-            "timestamp": 0.32766054388346627
+            "x": 1.1247485645977424,
+            "y": 4.681320324271238,
+            "heading": -0.6327125157113742,
+            "angularVelocity": 1.763282664312989,
+            "velocityX": 2.1193120571993105,
+            "velocityY": 1.6442811147120544,
+            "timestamp": 0.32766178240546834
         },
         {
-            "x": 1.291492862531941,
-            "y": 4.810942835643569,
-            "heading": 2.6039577624805075,
-            "angularVelocity": 1.4758750874318616,
-            "velocityX": 2.5444118113470626,
-            "velocityY": 1.9780785676502115,
-            "timestamp": 0.3931926526601595
+            "x": 1.291488910381981,
+            "y": 4.810947401273882,
+            "heading": -0.5360470717928266,
+            "angularVelocity": 1.4750796264503891,
+            "velocityX": 2.544397222040073,
+            "velocityY": 1.9780621964975005,
+            "timestamp": 0.393194138886562
         },
         {
-            "x": 1.4800025601939901,
-            "y": 4.960730002903915,
-            "heading": 2.6228917229305413,
-            "angularVelocity": 0.2889264637363189,
-            "velocityX": 2.8766005120392,
-            "velocityY": 2.28570650413168,
-            "timestamp": 0.45872476143685276
+            "x": 1.4799946113507723,
+            "y": 4.960741770751019,
+            "heading": -0.5171262553925533,
+            "angularVelocity": 0.2887247981954086,
+            "velocityX": 2.8765286507463124,
+            "velocityY": 2.2858077676538255,
+            "timestamp": 0.4587264953676557
         },
         {
-            "x": 1.6682727616271493,
-            "y": 5.110599963848934,
-            "heading": 2.6420440088534156,
-            "angularVelocity": 0.2922580438871624,
-            "velocityX": 2.8729458726059685,
-            "velocityY": 2.2869699105167616,
-            "timestamp": 0.5242568702135461
+            "x": 1.6682616674549375,
+            "y": 5.11061398659558,
+            "heading": -0.49798039258281773,
+            "angularVelocity": 0.29215892480933153,
+            "velocityX": 2.8728870166371596,
+            "velocityY": 2.2869956749959233,
+            "timestamp": 0.5242588518487493
         },
         {
-            "x": 1.8350043100801503,
-            "y": 5.239747952271642,
-            "heading": 2.7356929069056295,
-            "angularVelocity": 1.429053631881899,
-            "velocityX": 2.544272594998539,
-            "velocityY": 1.9707589277004427,
-            "timestamp": 0.5897889789902393
+            "x": 1.8349961429002288,
+            "y": 5.239757672124722,
+            "heading": -0.4043635195675931,
+            "angularVelocity": 1.4285595397783832,
+            "velocityX": 2.5443076427962366,
+            "velocityY": 1.970685817873752,
+            "timestamp": 0.589791208329843
         },
         {
-            "x": 1.9739563679504606,
-            "y": 5.347167856813287,
-            "heading": 2.8514085217331835,
-            "angularVelocity": 1.765785002003577,
-            "velocityX": 2.1203660383309844,
-            "velocityY": 1.6391949923004236,
-            "timestamp": 0.6553210877669325
+            "x": 1.9739512346286359,
+            "y": 5.34717518731749,
+            "heading": -0.2886497176580553,
+            "angularVelocity": 1.7657506630777153,
+            "velocityX": 2.120404319177778,
+            "velocityY": 1.6391523357435742,
+            "timestamp": 0.6553235648109367
         },
         {
-            "x": 2.0846918002256074,
-            "y": 5.4326634298654595,
-            "heading": 2.9588833399365435,
-            "angularVelocity": 1.640032958035698,
-            "velocityX": 1.6897889346501833,
-            "velocityY": 1.304636378229562,
-            "timestamp": 0.7208531965436258
+            "x": 2.084688896220359,
+            "y": 5.432668536845291,
+            "heading": -0.1811622358281448,
+            "angularVelocity": 1.6402200012587402,
+            "velocityX": 1.6898165660145774,
+            "velocityY": 1.304597516685743,
+            "timestamp": 0.7208559212920304
         },
         {
-            "x": 2.1674771737861436,
-            "y": 5.496490038501947,
-            "heading": 3.046413104094783,
-            "angularVelocity": 1.3356775143083182,
-            "velocityX": 1.2632795602936622,
-            "velocityY": 0.9739745878464279,
-            "timestamp": 0.786385305320319
+            "x": 2.167475787005346,
+            "y": 5.496492971314002,
+            "heading": -0.09361417394502371,
+            "angularVelocity": 1.3359516822560165,
+            "velocityX": 1.2632979375443678,
+            "velocityY": 0.97393772932798,
+            "timestamp": 0.786388277773124
         },
         {
-            "x": 2.2225294464291743,
-            "y": 5.538874362150552,
-            "heading": 3.1080101238304096,
-            "angularVelocity": 0.9399517409934369,
-            "velocityX": 0.8400808957732031,
-            "velocityY": 0.6467718564197722,
-            "timestamp": 0.8519174140970123
+            "x": 2.222528989324329,
+            "y": 5.538875463156123,
+            "heading": -0.0320000732344696,
+            "angularVelocity": 0.9402088375736564,
+            "velocityX": 0.8400919068867495,
+            "velocityY": 0.6467414589974029,
+            "timestamp": 0.8519206342542177
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0.4881557570289305,
-            "velocityX": 0.41919227205757975,
-            "velocityY": 0.3223707926359362,
-            "timestamp": 0.9174495228737055
+            "heading": 0,
+            "angularVelocity": 0.48830951537200207,
+            "velocityX": 0.41919766281556964,
+            "velocityY": 0.32235277316741867,
+            "timestamp": 0.9174529907353114
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 7.973690679262731e-27,
-            "velocityX": -1.618419533015103e-29,
-            "velocityY": -2.1923403816587234e-30,
-            "timestamp": 0.9829816316503988
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 1.4283065738497147e-31,
+            "velocityY": -2.9738366407041548e-31,
+            "timestamp": 0.9829853472164051
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.1.traj
index d9a920a4..0f3d2b08 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.1.traj	
@@ -3,119 +3,119 @@
         {
             "x": 0.71,
             "y": 4.36,
-            "heading": 2.1,
-            "angularVelocity": -5.8020277899403155e-31,
-            "velocityX": 1.3777732804976719e-32,
-            "velocityY": 5.338313673616136e-29,
+            "heading": -1.04,
+            "angularVelocity": -7.700576656700109e-31,
+            "velocityX": 0,
+            "velocityY": -1.136023856217491e-30,
             "timestamp": 0
         },
         {
-            "x": 0.7541175647990941,
-            "y": 4.360036481014467,
-            "heading": 2.123748591233672,
-            "angularVelocity": 0.32465550054626485,
-            "velocityX": 0.603109883101075,
-            "velocityY": 0.0004987142982711039,
-            "timestamp": 0.07315012742330113
+            "x": 0.7541176222366596,
+            "y": 4.360036691224542,
+            "heading": -1.0162545462543013,
+            "angularVelocity": 0.3246127495796489,
+            "velocityX": 0.6031109286246202,
+            "velocityY": 0.0005015881949855901,
+            "timestamp": 0.07315009584931663
         },
         {
-            "x": 0.8424087590063745,
-            "y": 4.360099698646948,
-            "heading": 2.1698601870465493,
-            "angularVelocity": 0.6303693163234944,
-            "velocityX": 1.2069861983474726,
-            "velocityY": 0.0008642176672118523,
-            "timestamp": 0.14630025484660225
+            "x": 0.8424088840100864,
+            "y": 4.360100335716613,
+            "heading": -0.970147925779941,
+            "angularVelocity": 0.630301572936589,
+            "velocityX": 1.2069876429868645,
+            "velocityY": 0.0008700534337042256,
+            "timestamp": 0.14630019169863326
         },
         {
-            "x": 0.9749550737228709,
-            "y": 4.360177642554779,
-            "heading": 2.23617090700284,
-            "angularVelocity": 0.906501769606733,
-            "velocityX": 1.8119765390084532,
-            "velocityY": 0.0010655334525686096,
-            "timestamp": 0.21945038226990338
+            "x": 0.9749552173528473,
+            "y": 4.360178948568722,
+            "heading": -0.9038422068087609,
+            "angularVelocity": 0.9064337948053328,
+            "velocityX": 1.8119775757478125,
+            "velocityY": 0.0010746787299806282,
+            "timestamp": 0.21945028754794987
         },
         {
-            "x": 1.1518842000389728,
-            "y": 4.360257435104989,
-            "heading": 2.318743489919177,
-            "angularVelocity": 1.1288098302053016,
-            "velocityX": 2.4187124827857356,
-            "velocityY": 0.0010908053480699021,
-            "timestamp": 0.2926005096932045
+            "x": 1.151884255871623,
+            "y": 4.36025970565234,
+            "heading": -0.8212723355896727,
+            "angularVelocity": 1.1287732471196155,
+            "velocityX": 2.4187123265462183,
+            "velocityY": 0.0011039914941527397,
+            "timestamp": 0.2926003833972665
         },
         {
-            "x": 1.3733949578522506,
-            "y": 4.360320997027151,
-            "heading": 2.4076375641139776,
-            "angularVelocity": 1.2152278789650626,
-            "velocityX": 3.028166397188238,
-            "velocityY": 0.0008689242848102628,
-            "timestamp": 0.36575063711650563
+            "x": 1.3733948183653495,
+            "y": 4.360324587725535,
+            "heading": -0.7323747736104,
+            "angularVelocity": 1.2152760833319434,
+            "velocityX": 3.028165034124097,
+            "velocityY": 0.000886971813693119,
+            "timestamp": 0.36575047924658316
         },
         {
-            "x": 1.6366157123115315,
-            "y": 4.3597910751743765,
-            "heading": 2.4407985098803153,
-            "angularVelocity": 0.45332724541279523,
-            "velocityX": 3.5983635809144485,
-            "velocityY": -0.007244305258449855,
-            "timestamp": 0.43890076453980675
+            "x": 1.6366160137298507,
+            "y": 4.3597898231256424,
+            "heading": -0.6992108014826405,
+            "angularVelocity": 0.45336881302434434,
+            "velocityX": 3.598371161490932,
+            "velocityY": -0.007310511266700298,
+            "timestamp": 0.4389005750958998
         },
         {
-            "x": 1.8581231700320338,
-            "y": 4.359897336997892,
-            "heading": 2.530170275911139,
-            "angularVelocity": 1.2217581729403981,
-            "velocityX": 3.0281212832164655,
-            "velocityY": 0.0014526539769375909,
-            "timestamp": 0.5120508919631079
+            "x": 1.858123270171759,
+            "y": 4.359896131611675,
+            "heading": -0.6098312657316373,
+            "angularVelocity": 1.2218649164225759,
+            "velocityX": 3.02811983866963,
+            "velocityY": 0.001453292504923468,
+            "timestamp": 0.5120506709452164
         },
         {
-            "x": 2.0350502325675994,
-            "y": 4.359957426055228,
-            "heading": 2.6131109520937392,
-            "angularVelocity": 1.1338418551586247,
-            "velocityX": 2.4186842698404125,
-            "velocityY": 0.000821448430104839,
-            "timestamp": 0.585201019386409
+            "x": 2.0350502388104363,
+            "y": 4.359956556106789,
+            "heading": -0.5268889356205209,
+            "angularVelocity": 1.1338649546283055,
+            "velocityX": 2.4186840302045134,
+            "velocityY": 0.000826034394207285,
+            "timestamp": 0.585200766794533
         },
         {
-            "x": 2.1675938018574246,
-            "y": 4.359986783385126,
-            "heading": 2.6797755559230114,
-            "angularVelocity": 0.9113395448181003,
-            "velocityX": 1.8119390076086361,
-            "velocityY": 0.00040132985313166364,
-            "timestamp": 0.6583511468097101
+            "x": 2.167593784990145,
+            "y": 4.359986293011168,
+            "heading": -0.460224121535325,
+            "angularVelocity": 0.9113428124904321,
+            "velocityX": 1.8119394737736823,
+            "velocityY": 0.0004065190076972891,
+            "timestamp": 0.6583508626438497
         },
         {
-            "x": 2.255882928551814,
-            "y": 4.359997887713372,
-            "heading": 2.726140403287784,
-            "angularVelocity": 0.6338313957609201,
-            "velocityX": 1.2069579343789958,
-            "velocityY": 0.00015180189885440488,
-            "timestamp": 0.7315012742330111
+            "x": 2.2558829179021025,
+            "y": 4.359997710834851,
+            "heading": -0.41385943895752736,
+            "angularVelocity": 0.6338294166193973,
+            "velocityX": 1.206958540339091,
+            "velocityY": 0.00015608761065604354,
+            "timestamp": 0.7315009584931663
         },
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0.3261730027365948,
-            "velocityX": 0.6031031387394808,
-            "velocityY": 0.000028876048490911067,
-            "timestamp": 0.8046514016563122
+            "heading": -0.39,
+            "angularVelocity": 0.32617098693462643,
+            "velocityX": 0.6031035446457226,
+            "velocityY": 0.00003129408268678862,
+            "timestamp": 0.804651054342483
         },
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 1.0612763050961144e-31,
-            "velocityX": 9.233550959236252e-32,
-            "velocityY": 1.483165202160369e-28,
-            "timestamp": 0.8778015290796133
+            "heading": -0.39,
+            "angularVelocity": 3.953163438305433e-30,
+            "velocityX": 7.397518409454568e-32,
+            "velocityY": 0,
+            "timestamp": 0.8778011501917996
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.traj
index d9a920a4..0f3d2b08 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.traj	
@@ -3,119 +3,119 @@
         {
             "x": 0.71,
             "y": 4.36,
-            "heading": 2.1,
-            "angularVelocity": -5.8020277899403155e-31,
-            "velocityX": 1.3777732804976719e-32,
-            "velocityY": 5.338313673616136e-29,
+            "heading": -1.04,
+            "angularVelocity": -7.700576656700109e-31,
+            "velocityX": 0,
+            "velocityY": -1.136023856217491e-30,
             "timestamp": 0
         },
         {
-            "x": 0.7541175647990941,
-            "y": 4.360036481014467,
-            "heading": 2.123748591233672,
-            "angularVelocity": 0.32465550054626485,
-            "velocityX": 0.603109883101075,
-            "velocityY": 0.0004987142982711039,
-            "timestamp": 0.07315012742330113
+            "x": 0.7541176222366596,
+            "y": 4.360036691224542,
+            "heading": -1.0162545462543013,
+            "angularVelocity": 0.3246127495796489,
+            "velocityX": 0.6031109286246202,
+            "velocityY": 0.0005015881949855901,
+            "timestamp": 0.07315009584931663
         },
         {
-            "x": 0.8424087590063745,
-            "y": 4.360099698646948,
-            "heading": 2.1698601870465493,
-            "angularVelocity": 0.6303693163234944,
-            "velocityX": 1.2069861983474726,
-            "velocityY": 0.0008642176672118523,
-            "timestamp": 0.14630025484660225
+            "x": 0.8424088840100864,
+            "y": 4.360100335716613,
+            "heading": -0.970147925779941,
+            "angularVelocity": 0.630301572936589,
+            "velocityX": 1.2069876429868645,
+            "velocityY": 0.0008700534337042256,
+            "timestamp": 0.14630019169863326
         },
         {
-            "x": 0.9749550737228709,
-            "y": 4.360177642554779,
-            "heading": 2.23617090700284,
-            "angularVelocity": 0.906501769606733,
-            "velocityX": 1.8119765390084532,
-            "velocityY": 0.0010655334525686096,
-            "timestamp": 0.21945038226990338
+            "x": 0.9749552173528473,
+            "y": 4.360178948568722,
+            "heading": -0.9038422068087609,
+            "angularVelocity": 0.9064337948053328,
+            "velocityX": 1.8119775757478125,
+            "velocityY": 0.0010746787299806282,
+            "timestamp": 0.21945028754794987
         },
         {
-            "x": 1.1518842000389728,
-            "y": 4.360257435104989,
-            "heading": 2.318743489919177,
-            "angularVelocity": 1.1288098302053016,
-            "velocityX": 2.4187124827857356,
-            "velocityY": 0.0010908053480699021,
-            "timestamp": 0.2926005096932045
+            "x": 1.151884255871623,
+            "y": 4.36025970565234,
+            "heading": -0.8212723355896727,
+            "angularVelocity": 1.1287732471196155,
+            "velocityX": 2.4187123265462183,
+            "velocityY": 0.0011039914941527397,
+            "timestamp": 0.2926003833972665
         },
         {
-            "x": 1.3733949578522506,
-            "y": 4.360320997027151,
-            "heading": 2.4076375641139776,
-            "angularVelocity": 1.2152278789650626,
-            "velocityX": 3.028166397188238,
-            "velocityY": 0.0008689242848102628,
-            "timestamp": 0.36575063711650563
+            "x": 1.3733948183653495,
+            "y": 4.360324587725535,
+            "heading": -0.7323747736104,
+            "angularVelocity": 1.2152760833319434,
+            "velocityX": 3.028165034124097,
+            "velocityY": 0.000886971813693119,
+            "timestamp": 0.36575047924658316
         },
         {
-            "x": 1.6366157123115315,
-            "y": 4.3597910751743765,
-            "heading": 2.4407985098803153,
-            "angularVelocity": 0.45332724541279523,
-            "velocityX": 3.5983635809144485,
-            "velocityY": -0.007244305258449855,
-            "timestamp": 0.43890076453980675
+            "x": 1.6366160137298507,
+            "y": 4.3597898231256424,
+            "heading": -0.6992108014826405,
+            "angularVelocity": 0.45336881302434434,
+            "velocityX": 3.598371161490932,
+            "velocityY": -0.007310511266700298,
+            "timestamp": 0.4389005750958998
         },
         {
-            "x": 1.8581231700320338,
-            "y": 4.359897336997892,
-            "heading": 2.530170275911139,
-            "angularVelocity": 1.2217581729403981,
-            "velocityX": 3.0281212832164655,
-            "velocityY": 0.0014526539769375909,
-            "timestamp": 0.5120508919631079
+            "x": 1.858123270171759,
+            "y": 4.359896131611675,
+            "heading": -0.6098312657316373,
+            "angularVelocity": 1.2218649164225759,
+            "velocityX": 3.02811983866963,
+            "velocityY": 0.001453292504923468,
+            "timestamp": 0.5120506709452164
         },
         {
-            "x": 2.0350502325675994,
-            "y": 4.359957426055228,
-            "heading": 2.6131109520937392,
-            "angularVelocity": 1.1338418551586247,
-            "velocityX": 2.4186842698404125,
-            "velocityY": 0.000821448430104839,
-            "timestamp": 0.585201019386409
+            "x": 2.0350502388104363,
+            "y": 4.359956556106789,
+            "heading": -0.5268889356205209,
+            "angularVelocity": 1.1338649546283055,
+            "velocityX": 2.4186840302045134,
+            "velocityY": 0.000826034394207285,
+            "timestamp": 0.585200766794533
         },
         {
-            "x": 2.1675938018574246,
-            "y": 4.359986783385126,
-            "heading": 2.6797755559230114,
-            "angularVelocity": 0.9113395448181003,
-            "velocityX": 1.8119390076086361,
-            "velocityY": 0.00040132985313166364,
-            "timestamp": 0.6583511468097101
+            "x": 2.167593784990145,
+            "y": 4.359986293011168,
+            "heading": -0.460224121535325,
+            "angularVelocity": 0.9113428124904321,
+            "velocityX": 1.8119394737736823,
+            "velocityY": 0.0004065190076972891,
+            "timestamp": 0.6583508626438497
         },
         {
-            "x": 2.255882928551814,
-            "y": 4.359997887713372,
-            "heading": 2.726140403287784,
-            "angularVelocity": 0.6338313957609201,
-            "velocityX": 1.2069579343789958,
-            "velocityY": 0.00015180189885440488,
-            "timestamp": 0.7315012742330111
+            "x": 2.2558829179021025,
+            "y": 4.359997710834851,
+            "heading": -0.41385943895752736,
+            "angularVelocity": 0.6338294166193973,
+            "velocityX": 1.206958540339091,
+            "velocityY": 0.00015608761065604354,
+            "timestamp": 0.7315009584931663
         },
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0.3261730027365948,
-            "velocityX": 0.6031031387394808,
-            "velocityY": 0.000028876048490911067,
-            "timestamp": 0.8046514016563122
+            "heading": -0.39,
+            "angularVelocity": 0.32617098693462643,
+            "velocityX": 0.6031035446457226,
+            "velocityY": 0.00003129408268678862,
+            "timestamp": 0.804651054342483
         },
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 1.0612763050961144e-31,
-            "velocityX": 9.233550959236252e-32,
-            "velocityY": 1.483165202160369e-28,
-            "timestamp": 0.8778015290796133
+            "heading": -0.39,
+            "angularVelocity": 3.953163438305433e-30,
+            "velocityX": 7.397518409454568e-32,
+            "velocityY": 0,
+            "timestamp": 0.8778011501917996
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.1.traj
index 0c6c619f..b6c78aaa 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.1.traj	
@@ -3,200 +3,200 @@
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": -1.8545515236340358e-32,
-            "velocityX": 2.346645959584709e-34,
-            "velocityY": 1.5264694726605652e-33,
+            "heading": 0.39,
+            "angularVelocity": 6.154725095706714e-32,
+            "velocityX": 1.2115062621827067e-33,
+            "velocityY": 6.540799795634554e-33,
             "timestamp": 0
         },
         {
-            "x": 2.3666998117674303,
-            "y": 6.765820939449683,
-            "heading": -2.7737296132590643,
-            "angularVelocity": -0.2635310200357621,
-            "velocityX": 0.7407398190339184,
-            "velocityY": 0.09796161216242502,
-            "timestamp": 0.09004485792922705
-        },
-        {
-            "x": 2.500184310330562,
-            "y": 6.783480740643308,
-            "heading": -2.817788411123353,
-            "angularVelocity": -0.4892983217200214,
-            "velocityX": 1.4824222241324077,
-            "velocityY": 0.19612226172321479,
-            "timestamp": 0.1800897158584541
-        },
-        {
-            "x": 2.700568444033807,
-            "y": 6.810009643084086,
-            "heading": -2.8763096511057067,
-            "angularVelocity": -0.6499120696969705,
-            "velocityX": 2.2253811967890735,
-            "velocityY": 0.294618738380691,
-            "timestamp": 0.27013457378768113
-        },
-        {
-            "x": 2.9679652961316036,
-            "y": 6.845459467588075,
-            "heading": -2.9368283914330156,
-            "angularVelocity": -0.6720954612963594,
-            "velocityX": 2.969596024105716,
-            "velocityY": 0.3936907150417343,
-            "timestamp": 0.3601794317169082
-        },
-        {
-            "x": 3.3011724137612752,
-            "y": 6.889825844077578,
-            "heading": -2.9514563142993295,
-            "angularVelocity": -0.1624515069790128,
-            "velocityX": 3.7004569199449846,
-            "velocityY": 0.49271415947342084,
-            "timestamp": 0.45022428964613526
-        },
-        {
-            "x": 3.638895175262513,
-            "y": 6.93426783648319,
-            "heading": -2.9514563336020103,
-            "angularVelocity": -2.143673866142303e-7,
-            "velocityX": 3.750605745490536,
-            "velocityY": 0.49355391776554697,
-            "timestamp": 0.5402691475753623
-        },
-        {
-            "x": 3.976617946407746,
-            "y": 6.9787097556023205,
-            "heading": -2.9514563529046813,
-            "angularVelocity": -2.1436727225160337e-7,
-            "velocityX": 3.75060585259265,
-            "velocityY": 0.4935531038769741,
-            "timestamp": 0.6303140055045893
-        },
-        {
-            "x": 4.314340727620448,
-            "y": 7.023151598216748,
-            "heading": -2.9514563722073515,
-            "angularVelocity": -2.1436726735785024e-7,
-            "velocityX": 3.7506059643976966,
-            "velocityY": 0.49355225424817656,
-            "timestamp": 0.7203588634338163
-        },
-        {
-            "x": 4.65206351967383,
-            "y": 7.06759335845053,
-            "heading": -2.951456391510026,
-            "angularVelocity": -2.1436731957684306e-7,
-            "velocityX": 3.7506060847896903,
-            "velocityY": 0.49355133936368123,
-            "timestamp": 0.8104037213630433
-        },
-        {
-            "x": 4.989786323250196,
-            "y": 7.11203503111852,
-            "heading": -2.9514564108127086,
-            "angularVelocity": -2.143674043301644e-7,
-            "velocityX": 3.7506062127590543,
-            "velocityY": 0.4935503668951324,
-            "timestamp": 0.9004485792922703
-        },
-        {
-            "x": 5.327509138668733,
-            "y": 7.15647661379494,
-            "heading": -2.951456430115396,
-            "angularVelocity": -2.1436746122645403e-7,
-            "velocityX": 3.7506063442731943,
-            "velocityY": 0.4935493674869138,
-            "timestamp": 0.9904934372214973
-        },
-        {
-            "x": 5.66523196589638,
-            "y": 7.200918106730859,
-            "heading": -2.95145644941808,
-            "angularVelocity": -2.143674220103917e-7,
-            "velocityX": 3.7506064754201436,
-            "velocityY": 0.49354837086698544,
-            "timestamp": 1.0805382951507243
-        },
-        {
-            "x": 6.002954804577347,
-            "y": 7.24535951262982,
-            "heading": -2.9514564687207536,
-            "angularVelocity": -2.1436730521441728e-7,
-            "velocityX": 3.7506066026158704,
-            "velocityY": 0.4935474042714428,
-            "timestamp": 1.1705831530799513
-        },
-        {
-            "x": 6.340677653913631,
-            "y": 7.289800837555904,
-            "heading": -2.95145648802342,
-            "angularVelocity": -2.1436722572774826e-7,
-            "velocityX": 3.750606720949308,
-            "velocityY": 0.49354650502102115,
-            "timestamp": 1.2606280110091783
-        },
-        {
-            "x": 6.678400512367262,
-            "y": 7.334242093196452,
-            "heading": -2.9514565073260908,
-            "angularVelocity": -2.1436727271528376e-7,
-            "velocityX": 3.750606822202691,
-            "velocityY": 0.4935457355652348,
-            "timestamp": 1.3506728689384053
-        },
-        {
-            "x": 7.0118090549698895,
-            "y": 7.378580230758345,
-            "heading": -2.965622818302132,
-            "angularVelocity": -0.15732504111646,
-            "velocityX": 3.7026938602610375,
-            "velocityY": 0.4924005499208098,
-            "timestamp": 1.4407177268676323
-        },
-        {
-            "x": 7.2792751287326976,
-            "y": 7.414016497420869,
-            "heading": -3.0230864740546695,
-            "angularVelocity": -0.6381669878107006,
-            "velocityX": 2.9703647705572496,
-            "velocityY": 0.39354014740493215,
-            "timestamp": 1.5307625847968593
-        },
-        {
-            "x": 7.479727754094387,
-            "y": 7.440534879064414,
-            "heading": -3.078696937244319,
-            "angularVelocity": -0.617586217231393,
-            "velocityX": 2.2261418360973004,
-            "velocityY": 0.2945018988689779,
-            "timestamp": 1.6208074427260863
-        },
-        {
-            "x": 7.613268208977269,
-            "y": 7.458185411749662,
-            "heading": -3.120596791778004,
-            "angularVelocity": -0.4653220127974119,
-            "velocityX": 1.48304365128591,
-            "velocityY": 0.1960193296003781,
-            "timestamp": 1.7108523006553134
+            "x": 2.3666999539229265,
+            "y": 6.765820977804648,
+            "heading": 0.36628259128993745,
+            "angularVelocity": -0.26339586361920003,
+            "velocityX": 0.7407424724032508,
+            "velocityY": 0.09796218023747214,
+            "timestamp": 0.09004472729439511
+        },
+        {
+            "x": 2.5001846245776895,
+            "y": 6.783480891306791,
+            "heading": 0.3222458902940237,
+            "angularVelocity": -0.4890536327789505,
+            "velocityX": 1.4824262859761226,
+            "velocityY": 0.1961237935054848,
+            "timestamp": 0.18008945458879022
+        },
+        {
+            "x": 2.70056880915549,
+            "y": 6.810010034997764,
+            "heading": 0.26375227569196347,
+            "angularVelocity": -0.6496062163731077,
+            "velocityX": 2.225384990313294,
+            "velocityY": 0.2946218450330504,
+            "timestamp": 0.27013418188318533
+        },
+        {
+            "x": 2.967965472322839,
+            "y": 6.8454603445891635,
+            "heading": 0.20325713210858826,
+            "angularVelocity": -0.6718343805472418,
+            "velocityX": 2.9695982341432847,
+            "velocityY": 0.3936966733820705,
+            "timestamp": 0.36017890917758044
+        },
+        {
+            "x": 3.3011740475809854,
+            "y": 6.889827568786931,
+            "heading": 0.1886164627156386,
+            "angularVelocity": -0.16259330038374148,
+            "velocityX": 3.700478476310374,
+            "velocityY": 0.4927242885939573,
+            "timestamp": 0.45022363647197555
+        },
+        {
+            "x": 3.6388963576772766,
+            "y": 6.934269203744266,
+            "heading": 0.18861644342798295,
+            "angularVelocity": -2.1420083336262225e-7,
+            "velocityX": 3.7506061736644574,
+            "velocityY": 0.4935506641275844,
+            "timestamp": 0.5402683637663707
+        },
+        {
+            "x": 3.9766186774172247,
+            "y": 6.978710765417191,
+            "heading": 0.1886164241403376,
+            "angularVelocity": -2.1420071941558326e-7,
+            "velocityX": 3.750606280762983,
+            "velocityY": 0.4935498502608198,
+            "timestamp": 0.6303130910607657
+        },
+        {
+            "x": 4.314341007224031,
+            "y": 7.023152250589549,
+            "heading": 0.18861640485269274,
+            "angularVelocity": -2.1420071403103001e-7,
+            "velocityX": 3.750606392561398,
+            "velocityY": 0.4935490006767261,
+            "timestamp": 0.7203578183551609
+        },
+        {
+            "x": 4.652063347871557,
+            "y": 7.067593653380398,
+            "heading": 0.1886163855650432,
+            "angularVelocity": -2.1420076555553473e-7,
+            "velocityX": 3.750606512954019,
+            "velocityY": 0.49354808578132814,
+            "timestamp": 0.810402545649556
+        },
+        {
+            "x": 4.989785700043158,
+            "y": 7.112034968596579,
+            "heading": 0.18861636627738598,
+            "angularVelocity": -2.1420085095836123e-7,
+            "velocityX": 3.7506066409356835,
+            "velocityY": 0.4935471132127838,
+            "timestamp": 0.9004472729439512
+        },
+        {
+            "x": 5.327508064058316,
+            "y": 7.15647619381006,
+            "heading": 0.18861634698972354,
+            "angularVelocity": -2.1420090940402864e-7,
+            "velocityX": 3.7506067724653955,
+            "velocityY": 0.4935461136795145,
+            "timestamp": 0.9904920002383464
+        },
+        {
+            "x": 5.6652304398823485,
+            "y": 7.200917329284216,
+            "heading": 0.18861632770206457,
+            "angularVelocity": -2.1420087020950116e-7,
+            "velocityX": 3.750606903609938,
+            "velocityY": 0.4935451170711973,
+            "timestamp": 1.0805367275327415
+        },
+        {
+            "x": 6.002952827157885,
+            "y": 7.245358377734637,
+            "heading": 0.18861630841441618,
+            "angularVelocity": -2.1420075320811155e-7,
+            "velocityX": 3.750607030785673,
+            "velocityY": 0.49354415062109597,
+            "timestamp": 1.1705814548271367
+        },
+        {
+            "x": 6.340675225087303,
+            "y": 7.289799345222555,
+            "heading": 0.1886162891267749,
+            "angularVelocity": -2.1420067405327174e-7,
+            "velocityX": 3.750607149103328,
+            "velocityY": 0.4935432514845822,
+            "timestamp": 1.2606261821215319
+        },
+        {
+            "x": 6.678397632133336,
+            "y": 7.334240243430023,
+            "heading": 0.18861626983912938,
+            "angularVelocity": -2.1420072105835628e-7,
+            "velocityX": 3.7506072503487453,
+            "velocityY": 0.4935424820841753,
+            "timestamp": 1.350670909415927
+        },
+        {
+            "x": 7.011808183476814,
+            "y": 7.378579327724673,
+            "heading": 0.17443906284640512,
+            "angularVelocity": -0.15744627607536488,
+            "velocityX": 3.7027215403008924,
+            "velocityY": 0.49241177831197624,
+            "timestamp": 1.4407156367103222
+        },
+        {
+            "x": 7.279274281127278,
+            "y": 7.414016049929063,
+            "heading": 0.11701095642225132,
+            "angularVelocity": -0.637773117313093,
+            "velocityX": 2.9703693451811146,
+            "velocityY": 0.3935457774060549,
+            "timestamp": 1.5307603640047174
+        },
+        {
+            "x": 7.479727163444043,
+            "y": 7.440534681549159,
+            "heading": 0.061439635089441606,
+            "angularVelocity": -0.6171524197205129,
+            "velocityX": 2.226147919371209,
+            "velocityY": 0.294505102263186,
+            "timestamp": 1.6208050912991125
+        },
+        {
+            "x": 7.613267962945412,
+            "y": 7.458185352165826,
+            "heading": 0.019571051398075176,
+            "angularVelocity": -0.46497540666073534,
+            "velocityX": 1.4830496300440337,
+            "velocityY": 0.19602114579078314,
+            "timestamp": 1.7108498185935077
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.2508584712225979,
-            "velocityX": 0.7410949670794122,
-            "velocityY": 0.09789107843632346,
-            "timestamp": 1.8008971585845404
+            "heading": -0.003,
+            "angularVelocity": -0.2506648870652985,
+            "velocityX": 0.7410987745724689,
+            "velocityY": 0.09789188216823574,
+            "timestamp": 1.8008945458879029
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": -3.143185307179586,
-            "angularVelocity": 4.6252031231996675e-32,
+            "heading": -0.003,
+            "angularVelocity": 5.335131961970383e-32,
             "velocityX": 0,
-            "velocityY": -8.558401294468068e-35,
-            "timestamp": 1.8909420165137674
+            "velocityY": -3.2330177098423496e-35,
+            "timestamp": 1.890939273182298
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.traj
index 0c6c619f..b6c78aaa 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.traj	
@@ -3,200 +3,200 @@
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": -1.8545515236340358e-32,
-            "velocityX": 2.346645959584709e-34,
-            "velocityY": 1.5264694726605652e-33,
+            "heading": 0.39,
+            "angularVelocity": 6.154725095706714e-32,
+            "velocityX": 1.2115062621827067e-33,
+            "velocityY": 6.540799795634554e-33,
             "timestamp": 0
         },
         {
-            "x": 2.3666998117674303,
-            "y": 6.765820939449683,
-            "heading": -2.7737296132590643,
-            "angularVelocity": -0.2635310200357621,
-            "velocityX": 0.7407398190339184,
-            "velocityY": 0.09796161216242502,
-            "timestamp": 0.09004485792922705
-        },
-        {
-            "x": 2.500184310330562,
-            "y": 6.783480740643308,
-            "heading": -2.817788411123353,
-            "angularVelocity": -0.4892983217200214,
-            "velocityX": 1.4824222241324077,
-            "velocityY": 0.19612226172321479,
-            "timestamp": 0.1800897158584541
-        },
-        {
-            "x": 2.700568444033807,
-            "y": 6.810009643084086,
-            "heading": -2.8763096511057067,
-            "angularVelocity": -0.6499120696969705,
-            "velocityX": 2.2253811967890735,
-            "velocityY": 0.294618738380691,
-            "timestamp": 0.27013457378768113
-        },
-        {
-            "x": 2.9679652961316036,
-            "y": 6.845459467588075,
-            "heading": -2.9368283914330156,
-            "angularVelocity": -0.6720954612963594,
-            "velocityX": 2.969596024105716,
-            "velocityY": 0.3936907150417343,
-            "timestamp": 0.3601794317169082
-        },
-        {
-            "x": 3.3011724137612752,
-            "y": 6.889825844077578,
-            "heading": -2.9514563142993295,
-            "angularVelocity": -0.1624515069790128,
-            "velocityX": 3.7004569199449846,
-            "velocityY": 0.49271415947342084,
-            "timestamp": 0.45022428964613526
-        },
-        {
-            "x": 3.638895175262513,
-            "y": 6.93426783648319,
-            "heading": -2.9514563336020103,
-            "angularVelocity": -2.143673866142303e-7,
-            "velocityX": 3.750605745490536,
-            "velocityY": 0.49355391776554697,
-            "timestamp": 0.5402691475753623
-        },
-        {
-            "x": 3.976617946407746,
-            "y": 6.9787097556023205,
-            "heading": -2.9514563529046813,
-            "angularVelocity": -2.1436727225160337e-7,
-            "velocityX": 3.75060585259265,
-            "velocityY": 0.4935531038769741,
-            "timestamp": 0.6303140055045893
-        },
-        {
-            "x": 4.314340727620448,
-            "y": 7.023151598216748,
-            "heading": -2.9514563722073515,
-            "angularVelocity": -2.1436726735785024e-7,
-            "velocityX": 3.7506059643976966,
-            "velocityY": 0.49355225424817656,
-            "timestamp": 0.7203588634338163
-        },
-        {
-            "x": 4.65206351967383,
-            "y": 7.06759335845053,
-            "heading": -2.951456391510026,
-            "angularVelocity": -2.1436731957684306e-7,
-            "velocityX": 3.7506060847896903,
-            "velocityY": 0.49355133936368123,
-            "timestamp": 0.8104037213630433
-        },
-        {
-            "x": 4.989786323250196,
-            "y": 7.11203503111852,
-            "heading": -2.9514564108127086,
-            "angularVelocity": -2.143674043301644e-7,
-            "velocityX": 3.7506062127590543,
-            "velocityY": 0.4935503668951324,
-            "timestamp": 0.9004485792922703
-        },
-        {
-            "x": 5.327509138668733,
-            "y": 7.15647661379494,
-            "heading": -2.951456430115396,
-            "angularVelocity": -2.1436746122645403e-7,
-            "velocityX": 3.7506063442731943,
-            "velocityY": 0.4935493674869138,
-            "timestamp": 0.9904934372214973
-        },
-        {
-            "x": 5.66523196589638,
-            "y": 7.200918106730859,
-            "heading": -2.95145644941808,
-            "angularVelocity": -2.143674220103917e-7,
-            "velocityX": 3.7506064754201436,
-            "velocityY": 0.49354837086698544,
-            "timestamp": 1.0805382951507243
-        },
-        {
-            "x": 6.002954804577347,
-            "y": 7.24535951262982,
-            "heading": -2.9514564687207536,
-            "angularVelocity": -2.1436730521441728e-7,
-            "velocityX": 3.7506066026158704,
-            "velocityY": 0.4935474042714428,
-            "timestamp": 1.1705831530799513
-        },
-        {
-            "x": 6.340677653913631,
-            "y": 7.289800837555904,
-            "heading": -2.95145648802342,
-            "angularVelocity": -2.1436722572774826e-7,
-            "velocityX": 3.750606720949308,
-            "velocityY": 0.49354650502102115,
-            "timestamp": 1.2606280110091783
-        },
-        {
-            "x": 6.678400512367262,
-            "y": 7.334242093196452,
-            "heading": -2.9514565073260908,
-            "angularVelocity": -2.1436727271528376e-7,
-            "velocityX": 3.750606822202691,
-            "velocityY": 0.4935457355652348,
-            "timestamp": 1.3506728689384053
-        },
-        {
-            "x": 7.0118090549698895,
-            "y": 7.378580230758345,
-            "heading": -2.965622818302132,
-            "angularVelocity": -0.15732504111646,
-            "velocityX": 3.7026938602610375,
-            "velocityY": 0.4924005499208098,
-            "timestamp": 1.4407177268676323
-        },
-        {
-            "x": 7.2792751287326976,
-            "y": 7.414016497420869,
-            "heading": -3.0230864740546695,
-            "angularVelocity": -0.6381669878107006,
-            "velocityX": 2.9703647705572496,
-            "velocityY": 0.39354014740493215,
-            "timestamp": 1.5307625847968593
-        },
-        {
-            "x": 7.479727754094387,
-            "y": 7.440534879064414,
-            "heading": -3.078696937244319,
-            "angularVelocity": -0.617586217231393,
-            "velocityX": 2.2261418360973004,
-            "velocityY": 0.2945018988689779,
-            "timestamp": 1.6208074427260863
-        },
-        {
-            "x": 7.613268208977269,
-            "y": 7.458185411749662,
-            "heading": -3.120596791778004,
-            "angularVelocity": -0.4653220127974119,
-            "velocityX": 1.48304365128591,
-            "velocityY": 0.1960193296003781,
-            "timestamp": 1.7108523006553134
+            "x": 2.3666999539229265,
+            "y": 6.765820977804648,
+            "heading": 0.36628259128993745,
+            "angularVelocity": -0.26339586361920003,
+            "velocityX": 0.7407424724032508,
+            "velocityY": 0.09796218023747214,
+            "timestamp": 0.09004472729439511
+        },
+        {
+            "x": 2.5001846245776895,
+            "y": 6.783480891306791,
+            "heading": 0.3222458902940237,
+            "angularVelocity": -0.4890536327789505,
+            "velocityX": 1.4824262859761226,
+            "velocityY": 0.1961237935054848,
+            "timestamp": 0.18008945458879022
+        },
+        {
+            "x": 2.70056880915549,
+            "y": 6.810010034997764,
+            "heading": 0.26375227569196347,
+            "angularVelocity": -0.6496062163731077,
+            "velocityX": 2.225384990313294,
+            "velocityY": 0.2946218450330504,
+            "timestamp": 0.27013418188318533
+        },
+        {
+            "x": 2.967965472322839,
+            "y": 6.8454603445891635,
+            "heading": 0.20325713210858826,
+            "angularVelocity": -0.6718343805472418,
+            "velocityX": 2.9695982341432847,
+            "velocityY": 0.3936966733820705,
+            "timestamp": 0.36017890917758044
+        },
+        {
+            "x": 3.3011740475809854,
+            "y": 6.889827568786931,
+            "heading": 0.1886164627156386,
+            "angularVelocity": -0.16259330038374148,
+            "velocityX": 3.700478476310374,
+            "velocityY": 0.4927242885939573,
+            "timestamp": 0.45022363647197555
+        },
+        {
+            "x": 3.6388963576772766,
+            "y": 6.934269203744266,
+            "heading": 0.18861644342798295,
+            "angularVelocity": -2.1420083336262225e-7,
+            "velocityX": 3.7506061736644574,
+            "velocityY": 0.4935506641275844,
+            "timestamp": 0.5402683637663707
+        },
+        {
+            "x": 3.9766186774172247,
+            "y": 6.978710765417191,
+            "heading": 0.1886164241403376,
+            "angularVelocity": -2.1420071941558326e-7,
+            "velocityX": 3.750606280762983,
+            "velocityY": 0.4935498502608198,
+            "timestamp": 0.6303130910607657
+        },
+        {
+            "x": 4.314341007224031,
+            "y": 7.023152250589549,
+            "heading": 0.18861640485269274,
+            "angularVelocity": -2.1420071403103001e-7,
+            "velocityX": 3.750606392561398,
+            "velocityY": 0.4935490006767261,
+            "timestamp": 0.7203578183551609
+        },
+        {
+            "x": 4.652063347871557,
+            "y": 7.067593653380398,
+            "heading": 0.1886163855650432,
+            "angularVelocity": -2.1420076555553473e-7,
+            "velocityX": 3.750606512954019,
+            "velocityY": 0.49354808578132814,
+            "timestamp": 0.810402545649556
+        },
+        {
+            "x": 4.989785700043158,
+            "y": 7.112034968596579,
+            "heading": 0.18861636627738598,
+            "angularVelocity": -2.1420085095836123e-7,
+            "velocityX": 3.7506066409356835,
+            "velocityY": 0.4935471132127838,
+            "timestamp": 0.9004472729439512
+        },
+        {
+            "x": 5.327508064058316,
+            "y": 7.15647619381006,
+            "heading": 0.18861634698972354,
+            "angularVelocity": -2.1420090940402864e-7,
+            "velocityX": 3.7506067724653955,
+            "velocityY": 0.4935461136795145,
+            "timestamp": 0.9904920002383464
+        },
+        {
+            "x": 5.6652304398823485,
+            "y": 7.200917329284216,
+            "heading": 0.18861632770206457,
+            "angularVelocity": -2.1420087020950116e-7,
+            "velocityX": 3.750606903609938,
+            "velocityY": 0.4935451170711973,
+            "timestamp": 1.0805367275327415
+        },
+        {
+            "x": 6.002952827157885,
+            "y": 7.245358377734637,
+            "heading": 0.18861630841441618,
+            "angularVelocity": -2.1420075320811155e-7,
+            "velocityX": 3.750607030785673,
+            "velocityY": 0.49354415062109597,
+            "timestamp": 1.1705814548271367
+        },
+        {
+            "x": 6.340675225087303,
+            "y": 7.289799345222555,
+            "heading": 0.1886162891267749,
+            "angularVelocity": -2.1420067405327174e-7,
+            "velocityX": 3.750607149103328,
+            "velocityY": 0.4935432514845822,
+            "timestamp": 1.2606261821215319
+        },
+        {
+            "x": 6.678397632133336,
+            "y": 7.334240243430023,
+            "heading": 0.18861626983912938,
+            "angularVelocity": -2.1420072105835628e-7,
+            "velocityX": 3.7506072503487453,
+            "velocityY": 0.4935424820841753,
+            "timestamp": 1.350670909415927
+        },
+        {
+            "x": 7.011808183476814,
+            "y": 7.378579327724673,
+            "heading": 0.17443906284640512,
+            "angularVelocity": -0.15744627607536488,
+            "velocityX": 3.7027215403008924,
+            "velocityY": 0.49241177831197624,
+            "timestamp": 1.4407156367103222
+        },
+        {
+            "x": 7.279274281127278,
+            "y": 7.414016049929063,
+            "heading": 0.11701095642225132,
+            "angularVelocity": -0.637773117313093,
+            "velocityX": 2.9703693451811146,
+            "velocityY": 0.3935457774060549,
+            "timestamp": 1.5307603640047174
+        },
+        {
+            "x": 7.479727163444043,
+            "y": 7.440534681549159,
+            "heading": 0.061439635089441606,
+            "angularVelocity": -0.6171524197205129,
+            "velocityX": 2.226147919371209,
+            "velocityY": 0.294505102263186,
+            "timestamp": 1.6208050912991125
+        },
+        {
+            "x": 7.613267962945412,
+            "y": 7.458185352165826,
+            "heading": 0.019571051398075176,
+            "angularVelocity": -0.46497540666073534,
+            "velocityX": 1.4830496300440337,
+            "velocityY": 0.19602114579078314,
+            "timestamp": 1.7108498185935077
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.2508584712225979,
-            "velocityX": 0.7410949670794122,
-            "velocityY": 0.09789107843632346,
-            "timestamp": 1.8008971585845404
+            "heading": -0.003,
+            "angularVelocity": -0.2506648870652985,
+            "velocityX": 0.7410987745724689,
+            "velocityY": 0.09789188216823574,
+            "timestamp": 1.8008945458879029
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": -3.143185307179586,
-            "angularVelocity": 4.6252031231996675e-32,
+            "heading": -0.003,
+            "angularVelocity": 5.335131961970383e-32,
             "velocityX": 0,
-            "velocityY": -8.558401294468068e-35,
-            "timestamp": 1.8909420165137674
+            "velocityY": -3.2330177098423496e-35,
+            "timestamp": 1.890939273182298
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.1.traj
index c369c20d..5089d19a 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.1.traj	
@@ -3,200 +3,200 @@
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 1.5124244158252309e-30,
-            "velocityX": -1.0784945790349375e-32,
-            "velocityY": 7.7332582255010576e-31,
+            "heading": 0.39,
+            "angularVelocity": 5.971193698047002e-30,
+            "velocityX": 3.860312986446214e-32,
+            "velocityY": -2.276118500298611e-31,
             "timestamp": 0
         },
         {
-            "x": 2.368718302927459,
-            "y": 6.754046521321464,
-            "heading": -2.805815501863529,
-            "angularVelocity": -0.6069900647837034,
-            "velocityX": 0.7473072131063252,
-            "velocityY": -0.0321188944749646,
-            "timestamp": 0.09195455593398905
-        },
-        {
-            "x": 2.506490387717156,
-            "y": 6.748005444139933,
-            "heading": -2.9118057011244183,
-            "angularVelocity": -1.1526367365307737,
-            "velocityX": 1.498262738483551,
-            "velocityY": -0.06569633358752333,
-            "timestamp": 0.1839091118679781
-        },
-        {
-            "x": 2.7138594941009426,
-            "y": 6.738708328412372,
-            "heading": -3.056707583490325,
-            "angularVelocity": -1.5757988377424839,
-            "velocityX": 2.255125961704928,
-            "velocityY": -0.10110554755149347,
-            "timestamp": 0.27586366780196714
-        },
-        {
-            "x": 2.99155716708356,
-            "y": 6.725962728573371,
-            "heading": -3.2144148322825616,
-            "angularVelocity": -1.7150563905223961,
-            "velocityX": 3.019944690744492,
-            "velocityY": -0.13860759490970812,
-            "timestamp": 0.3678182237359562
-        },
-        {
-            "x": 3.331057359197656,
-            "y": 6.709255401861718,
-            "heading": -3.240411366891808,
-            "angularVelocity": -0.28271067534607036,
-            "velocityX": 3.6920431909628384,
-            "velocityY": -0.1816911249470588,
-            "timestamp": 0.45977277966994523
-        },
-        {
-            "x": 3.6785771644429963,
-            "y": 6.69390611896786,
-            "heading": -3.2404114102003327,
-            "angularVelocity": -4.7097747760964064e-7,
-            "velocityX": 3.7792559782988104,
-            "velocityY": -0.1669224840243556,
-            "timestamp": 0.5517273356039343
-        },
-        {
-            "x": 4.026096967074563,
-            "y": 6.678556776895953,
-            "heading": -3.2404114535088273,
-            "angularVelocity": -4.70977144522105e-7,
-            "velocityX": 3.779255949874186,
-            "velocityY": -0.1669231275818996,
-            "timestamp": 0.6436818915379233
-        },
-        {
-            "x": 4.373616766444988,
-            "y": 6.663207360989526,
-            "heading": -3.240411496817302,
-            "angularVelocity": -4.7097693251045375e-7,
-            "velocityX": 3.779255914409469,
-            "velocityY": -0.16692393052765972,
-            "timestamp": 0.7356364474719124
-        },
-        {
-            "x": 4.721136562002901,
-            "y": 6.647857858765673,
-            "heading": -3.240411540125758,
-            "angularVelocity": -4.709767302327839e-7,
-            "velocityX": 3.779255872948645,
-            "velocityY": -0.1669248692242324,
-            "timestamp": 0.8275910034059014
-        },
-        {
-            "x": 5.068656352361164,
-            "y": 6.632508238819668,
-            "heading": -3.2404115834342044,
-            "angularVelocity": -4.709766303037568e-7,
-            "velocityX": 3.7792558164027836,
-            "velocityY": -0.166926149445212,
-            "timestamp": 0.9195455593398905
-        },
-        {
-            "x": 5.4161761153722905,
-            "y": 6.617157999739619,
-            "heading": -3.2404116267428487,
-            "angularVelocity": -4.709787784210011e-7,
-            "velocityX": 3.7792555190043995,
-            "velocityY": -0.16693288248891244,
-            "timestamp": 1.0115001152738796
-        },
-        {
-            "x": 5.762341737151603,
-            "y": 6.582880068913748,
-            "heading": -3.240411680913547,
-            "angularVelocity": -5.891029283022056e-7,
-            "velocityX": 3.7645293184582718,
-            "velocityY": -0.37277033723568115,
-            "timestamp": 1.1034546712078686
-        },
-        {
-            "x": 6.108499177819837,
-            "y": 6.548519619057327,
-            "heading": -3.2404117350781716,
-            "angularVelocity": -5.89036882514826e-7,
-            "velocityX": 3.7644403493909295,
-            "velocityY": -0.37366772649185126,
-            "timestamp": 1.1954092271418575
-        },
-        {
-            "x": 6.443025175700078,
-            "y": 6.453136314569308,
-            "heading": -3.2404117521954174,
-            "angularVelocity": -1.8614896803626685e-7,
-            "velocityX": 3.637949142186984,
-            "velocityY": -1.0372874244152857,
-            "timestamp": 1.2873637830758464
-        },
-        {
-            "x": 6.751449154461275,
-            "y": 6.292262864409884,
-            "heading": -3.240411747251824,
-            "angularVelocity": 5.3761265862029046e-8,
-            "velocityX": 3.3540913294454215,
-            "velocityY": -1.7494886308287982,
-            "timestamp": 1.3793183390098354
-        },
-        {
-            "x": 7.05837809298162,
-            "y": 6.128555062623798,
-            "heading": -3.240411742317762,
-            "angularVelocity": 5.3657614685044897e-8,
-            "velocityX": 3.337832861056693,
-            "velocityY": -1.780312026123053,
-            "timestamp": 1.4712728949438243
-        },
-        {
-            "x": 7.307026842216431,
-            "y": 5.995933066242413,
-            "heading": -3.201520178002944,
-            "angularVelocity": 0.422943310636366,
-            "velocityX": 2.7040394759048985,
-            "velocityY": -1.44225585164687,
-            "timestamp": 1.5632274508778132
-        },
-        {
-            "x": 7.493513419749655,
-            "y": 5.8964665430996925,
-            "heading": -3.172352554188821,
-            "angularVelocity": 0.3171960705792624,
-            "velocityX": 2.0280297766550737,
-            "velocityY": -1.0816921699249264,
-            "timestamp": 1.6551820068118022
-        },
-        {
-            "x": 7.617837807165489,
-            "y": 5.830155516567485,
-            "heading": -3.1529077720147476,
-            "angularVelocity": 0.21146078056243686,
-            "velocityX": 1.3520198771345535,
-            "velocityY": -0.721128234035621,
-            "timestamp": 1.747136562745791
+            "x": 2.3687225561485863,
+            "y": 6.754047320431167,
+            "heading": 0.3342984160631402,
+            "angularVelocity": -0.6057639861694455,
+            "velocityX": 0.7473692238179469,
+            "velocityY": -0.03211088121883033,
+            "timestamp": 0.09195261720507575
+        },
+        {
+            "x": 2.5065025886783423,
+            "y": 6.748008642736996,
+            "heading": 0.22854226308747308,
+            "angularVelocity": -1.1501157464589211,
+            "velocityX": 1.498380760848541,
+            "velocityY": -0.06567162390498478,
+            "timestamp": 0.1839052344101515
+        },
+        {
+            "x": 2.713881673600013,
+            "y": 6.738717029343825,
+            "heading": 0.08399021423974613,
+            "angularVelocity": -1.5720275642110566,
+            "velocityX": 2.2552820270375413,
+            "velocityY": -0.10104784045947955,
+            "timestamp": 0.27585785161522725
+        },
+        {
+            "x": 2.991587720371644,
+            "y": 6.725981822834066,
+            "heading": -0.07328714089763647,
+            "angularVelocity": -1.710417385800096,
+            "velocityX": 3.0200994296038606,
+            "velocityY": -0.13849748812866708,
+            "timestamp": 0.367810468820303
+        },
+        {
+            "x": 3.331126465665418,
+            "y": 6.709290658495849,
+            "heading": -0.09912948961150339,
+            "angularVelocity": -0.2810398387707929,
+            "velocityX": 3.692540306237542,
+            "velocityY": -0.18151918722434224,
+            "timestamp": 0.45976308602537874
+        },
+        {
+            "x": 3.678639360618228,
+            "y": 6.693951135251218,
+            "heading": -0.09912953281628478,
+            "angularVelocity": -4.6985918076152147e-7,
+            "velocityX": 3.7792605095489114,
+            "velocityY": -0.16681986560992548,
+            "timestamp": 0.5517157032304545
+        },
+        {
+            "x": 4.0261522529573766,
+            "y": 6.678611552794569,
+            "heading": -0.09912957602103574,
+            "angularVelocity": -4.698588497287581e-7,
+            "velocityX": 3.77926048112489,
+            "velocityY": -0.16682050955045719,
+            "timestamp": 0.6436683204355302
+        },
+        {
+            "x": 4.373665142037621,
+            "y": 6.663271896508641,
+            "heading": -0.0991296192257673,
+            "angularVelocity": -4.698586389884745e-7,
+            "velocityX": 3.779260445683786,
+            "velocityY": -0.1668213124561534,
+            "timestamp": 0.735620937640606
+        },
+        {
+            "x": 4.721178027308032,
+            "y": 6.647932153912761,
+            "heading": -0.09912966243048027,
+            "angularVelocity": -4.6985843677462385e-7,
+            "velocityX": 3.779260404251215,
+            "velocityY": -0.1668222510912166,
+            "timestamp": 0.8275735548456817
+        },
+        {
+            "x": 5.068690907382649,
+            "y": 6.632592293609503,
+            "heading": -0.09912970563518403,
+            "angularVelocity": -4.698583364707967e-7,
+            "velocityX": 3.7792603477460833,
+            "velocityY": -0.16682353117852486,
+            "timestamp": 0.9195261720507575
+        },
+        {
+            "x": 5.416203760083376,
+            "y": 6.617251813184553,
+            "heading": -0.09912974884008513,
+            "angularVelocity": -4.698604826805718e-7,
+            "velocityX": 3.779260050050473,
+            "velocityY": -0.16683027510502133,
+            "timestamp": 1.0114787892558332
+        },
+        {
+            "x": 5.762351413678364,
+            "y": 6.582867023594091,
+            "heading": -0.09912980297819443,
+            "angularVelocity": -5.887609395849737e-7,
+            "velocityX": 3.7644132827997527,
+            "velocityY": -0.3739403035563012,
+            "timestamp": 1.103431406460909
+        },
+        {
+            "x": 6.108490861918874,
+            "y": 6.5483997316058735,
+            "heading": -0.0991298571102857,
+            "angularVelocity": -5.886954924717416e-7,
+            "velocityX": 3.7643240482055917,
+            "velocityY": -0.3748375308486051,
+            "timestamp": 1.1953840236659847
+        },
+        {
+            "x": 6.4430213978276925,
+            "y": 6.453059099313297,
+            "heading": -0.09912987433352956,
+            "angularVelocity": -1.8730564053245824e-7,
+            "velocityX": 3.6380751965192784,
+            "velocityY": -1.0368452273625233,
+            "timestamp": 1.2873366408710605
+        },
+        {
+            "x": 6.751459095644277,
+            "y": 6.29222781566083,
+            "heading": -0.09912986944515287,
+            "angularVelocity": 5.316190922150303e-8,
+            "velocityX": 3.3543112441127847,
+            "velocityY": -1.7490669492721016,
+            "timestamp": 1.3792892580761362
+        },
+        {
+            "x": 7.058385657911462,
+            "y": 6.1285311430110365,
+            "heading": -0.09912986456630993,
+            "angularVelocity": 5.305822810376802e-8,
+            "velocityX": 3.337877393774089,
+            "velocityY": -1.7802285310130133,
+            "timestamp": 1.471241875281212
+        },
+        {
+            "x": 7.307031381410103,
+            "y": 5.995918713976455,
+            "heading": -0.06067697672181489,
+            "angularVelocity": 0.4181815484244036,
+            "velocityX": 2.7040635824873065,
+            "velocityY": -1.442182213681043,
+            "timestamp": 1.5631944924862877
+        },
+        {
+            "x": 7.4935156893705,
+            "y": 5.896459366852187,
+            "heading": -0.03183830930275791,
+            "angularVelocity": 0.3136253028528817,
+            "velocityX": 2.028047853651562,
+            "velocityY": -1.0816369359281015,
+            "timestamp": 1.6551471096913635
+        },
+        {
+            "x": 7.617838563683624,
+            "y": 5.830153124497745,
+            "heading": -0.012612817121248107,
+            "angularVelocity": 0.2090804238734443,
+            "velocityX": 1.3520319278771131,
+            "velocityY": -0.7210914095741754,
+            "timestamp": 1.7470997268964392
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0.10573119228742708,
-            "velocityX": 0.6760099290690327,
-            "velocityY": -0.3605641529201271,
-            "timestamp": 1.83909111867978
+            "heading": -0.003,
+            "angularVelocity": 0.10454098440514517,
+            "velocityX": 0.6760159548013833,
+            "velocityY": -0.36054574089833413,
+            "timestamp": 1.839052344101515
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": -3.143185307179586,
-            "angularVelocity": 3.5257869003902484e-31,
+            "heading": -0.003,
+            "angularVelocity": -2.423215560944852e-29,
             "velocityX": 0,
-            "velocityY": -2.4831219594454356e-32,
-            "timestamp": 1.931045674613769
+            "velocityY": -2.0134519400533763e-31,
+            "timestamp": 1.9310049613065907
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.traj
index c369c20d..5089d19a 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.traj	
@@ -3,200 +3,200 @@
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 1.5124244158252309e-30,
-            "velocityX": -1.0784945790349375e-32,
-            "velocityY": 7.7332582255010576e-31,
+            "heading": 0.39,
+            "angularVelocity": 5.971193698047002e-30,
+            "velocityX": 3.860312986446214e-32,
+            "velocityY": -2.276118500298611e-31,
             "timestamp": 0
         },
         {
-            "x": 2.368718302927459,
-            "y": 6.754046521321464,
-            "heading": -2.805815501863529,
-            "angularVelocity": -0.6069900647837034,
-            "velocityX": 0.7473072131063252,
-            "velocityY": -0.0321188944749646,
-            "timestamp": 0.09195455593398905
-        },
-        {
-            "x": 2.506490387717156,
-            "y": 6.748005444139933,
-            "heading": -2.9118057011244183,
-            "angularVelocity": -1.1526367365307737,
-            "velocityX": 1.498262738483551,
-            "velocityY": -0.06569633358752333,
-            "timestamp": 0.1839091118679781
-        },
-        {
-            "x": 2.7138594941009426,
-            "y": 6.738708328412372,
-            "heading": -3.056707583490325,
-            "angularVelocity": -1.5757988377424839,
-            "velocityX": 2.255125961704928,
-            "velocityY": -0.10110554755149347,
-            "timestamp": 0.27586366780196714
-        },
-        {
-            "x": 2.99155716708356,
-            "y": 6.725962728573371,
-            "heading": -3.2144148322825616,
-            "angularVelocity": -1.7150563905223961,
-            "velocityX": 3.019944690744492,
-            "velocityY": -0.13860759490970812,
-            "timestamp": 0.3678182237359562
-        },
-        {
-            "x": 3.331057359197656,
-            "y": 6.709255401861718,
-            "heading": -3.240411366891808,
-            "angularVelocity": -0.28271067534607036,
-            "velocityX": 3.6920431909628384,
-            "velocityY": -0.1816911249470588,
-            "timestamp": 0.45977277966994523
-        },
-        {
-            "x": 3.6785771644429963,
-            "y": 6.69390611896786,
-            "heading": -3.2404114102003327,
-            "angularVelocity": -4.7097747760964064e-7,
-            "velocityX": 3.7792559782988104,
-            "velocityY": -0.1669224840243556,
-            "timestamp": 0.5517273356039343
-        },
-        {
-            "x": 4.026096967074563,
-            "y": 6.678556776895953,
-            "heading": -3.2404114535088273,
-            "angularVelocity": -4.70977144522105e-7,
-            "velocityX": 3.779255949874186,
-            "velocityY": -0.1669231275818996,
-            "timestamp": 0.6436818915379233
-        },
-        {
-            "x": 4.373616766444988,
-            "y": 6.663207360989526,
-            "heading": -3.240411496817302,
-            "angularVelocity": -4.7097693251045375e-7,
-            "velocityX": 3.779255914409469,
-            "velocityY": -0.16692393052765972,
-            "timestamp": 0.7356364474719124
-        },
-        {
-            "x": 4.721136562002901,
-            "y": 6.647857858765673,
-            "heading": -3.240411540125758,
-            "angularVelocity": -4.709767302327839e-7,
-            "velocityX": 3.779255872948645,
-            "velocityY": -0.1669248692242324,
-            "timestamp": 0.8275910034059014
-        },
-        {
-            "x": 5.068656352361164,
-            "y": 6.632508238819668,
-            "heading": -3.2404115834342044,
-            "angularVelocity": -4.709766303037568e-7,
-            "velocityX": 3.7792558164027836,
-            "velocityY": -0.166926149445212,
-            "timestamp": 0.9195455593398905
-        },
-        {
-            "x": 5.4161761153722905,
-            "y": 6.617157999739619,
-            "heading": -3.2404116267428487,
-            "angularVelocity": -4.709787784210011e-7,
-            "velocityX": 3.7792555190043995,
-            "velocityY": -0.16693288248891244,
-            "timestamp": 1.0115001152738796
-        },
-        {
-            "x": 5.762341737151603,
-            "y": 6.582880068913748,
-            "heading": -3.240411680913547,
-            "angularVelocity": -5.891029283022056e-7,
-            "velocityX": 3.7645293184582718,
-            "velocityY": -0.37277033723568115,
-            "timestamp": 1.1034546712078686
-        },
-        {
-            "x": 6.108499177819837,
-            "y": 6.548519619057327,
-            "heading": -3.2404117350781716,
-            "angularVelocity": -5.89036882514826e-7,
-            "velocityX": 3.7644403493909295,
-            "velocityY": -0.37366772649185126,
-            "timestamp": 1.1954092271418575
-        },
-        {
-            "x": 6.443025175700078,
-            "y": 6.453136314569308,
-            "heading": -3.2404117521954174,
-            "angularVelocity": -1.8614896803626685e-7,
-            "velocityX": 3.637949142186984,
-            "velocityY": -1.0372874244152857,
-            "timestamp": 1.2873637830758464
-        },
-        {
-            "x": 6.751449154461275,
-            "y": 6.292262864409884,
-            "heading": -3.240411747251824,
-            "angularVelocity": 5.3761265862029046e-8,
-            "velocityX": 3.3540913294454215,
-            "velocityY": -1.7494886308287982,
-            "timestamp": 1.3793183390098354
-        },
-        {
-            "x": 7.05837809298162,
-            "y": 6.128555062623798,
-            "heading": -3.240411742317762,
-            "angularVelocity": 5.3657614685044897e-8,
-            "velocityX": 3.337832861056693,
-            "velocityY": -1.780312026123053,
-            "timestamp": 1.4712728949438243
-        },
-        {
-            "x": 7.307026842216431,
-            "y": 5.995933066242413,
-            "heading": -3.201520178002944,
-            "angularVelocity": 0.422943310636366,
-            "velocityX": 2.7040394759048985,
-            "velocityY": -1.44225585164687,
-            "timestamp": 1.5632274508778132
-        },
-        {
-            "x": 7.493513419749655,
-            "y": 5.8964665430996925,
-            "heading": -3.172352554188821,
-            "angularVelocity": 0.3171960705792624,
-            "velocityX": 2.0280297766550737,
-            "velocityY": -1.0816921699249264,
-            "timestamp": 1.6551820068118022
-        },
-        {
-            "x": 7.617837807165489,
-            "y": 5.830155516567485,
-            "heading": -3.1529077720147476,
-            "angularVelocity": 0.21146078056243686,
-            "velocityX": 1.3520198771345535,
-            "velocityY": -0.721128234035621,
-            "timestamp": 1.747136562745791
+            "x": 2.3687225561485863,
+            "y": 6.754047320431167,
+            "heading": 0.3342984160631402,
+            "angularVelocity": -0.6057639861694455,
+            "velocityX": 0.7473692238179469,
+            "velocityY": -0.03211088121883033,
+            "timestamp": 0.09195261720507575
+        },
+        {
+            "x": 2.5065025886783423,
+            "y": 6.748008642736996,
+            "heading": 0.22854226308747308,
+            "angularVelocity": -1.1501157464589211,
+            "velocityX": 1.498380760848541,
+            "velocityY": -0.06567162390498478,
+            "timestamp": 0.1839052344101515
+        },
+        {
+            "x": 2.713881673600013,
+            "y": 6.738717029343825,
+            "heading": 0.08399021423974613,
+            "angularVelocity": -1.5720275642110566,
+            "velocityX": 2.2552820270375413,
+            "velocityY": -0.10104784045947955,
+            "timestamp": 0.27585785161522725
+        },
+        {
+            "x": 2.991587720371644,
+            "y": 6.725981822834066,
+            "heading": -0.07328714089763647,
+            "angularVelocity": -1.710417385800096,
+            "velocityX": 3.0200994296038606,
+            "velocityY": -0.13849748812866708,
+            "timestamp": 0.367810468820303
+        },
+        {
+            "x": 3.331126465665418,
+            "y": 6.709290658495849,
+            "heading": -0.09912948961150339,
+            "angularVelocity": -0.2810398387707929,
+            "velocityX": 3.692540306237542,
+            "velocityY": -0.18151918722434224,
+            "timestamp": 0.45976308602537874
+        },
+        {
+            "x": 3.678639360618228,
+            "y": 6.693951135251218,
+            "heading": -0.09912953281628478,
+            "angularVelocity": -4.6985918076152147e-7,
+            "velocityX": 3.7792605095489114,
+            "velocityY": -0.16681986560992548,
+            "timestamp": 0.5517157032304545
+        },
+        {
+            "x": 4.0261522529573766,
+            "y": 6.678611552794569,
+            "heading": -0.09912957602103574,
+            "angularVelocity": -4.698588497287581e-7,
+            "velocityX": 3.77926048112489,
+            "velocityY": -0.16682050955045719,
+            "timestamp": 0.6436683204355302
+        },
+        {
+            "x": 4.373665142037621,
+            "y": 6.663271896508641,
+            "heading": -0.0991296192257673,
+            "angularVelocity": -4.698586389884745e-7,
+            "velocityX": 3.779260445683786,
+            "velocityY": -0.1668213124561534,
+            "timestamp": 0.735620937640606
+        },
+        {
+            "x": 4.721178027308032,
+            "y": 6.647932153912761,
+            "heading": -0.09912966243048027,
+            "angularVelocity": -4.6985843677462385e-7,
+            "velocityX": 3.779260404251215,
+            "velocityY": -0.1668222510912166,
+            "timestamp": 0.8275735548456817
+        },
+        {
+            "x": 5.068690907382649,
+            "y": 6.632592293609503,
+            "heading": -0.09912970563518403,
+            "angularVelocity": -4.698583364707967e-7,
+            "velocityX": 3.7792603477460833,
+            "velocityY": -0.16682353117852486,
+            "timestamp": 0.9195261720507575
+        },
+        {
+            "x": 5.416203760083376,
+            "y": 6.617251813184553,
+            "heading": -0.09912974884008513,
+            "angularVelocity": -4.698604826805718e-7,
+            "velocityX": 3.779260050050473,
+            "velocityY": -0.16683027510502133,
+            "timestamp": 1.0114787892558332
+        },
+        {
+            "x": 5.762351413678364,
+            "y": 6.582867023594091,
+            "heading": -0.09912980297819443,
+            "angularVelocity": -5.887609395849737e-7,
+            "velocityX": 3.7644132827997527,
+            "velocityY": -0.3739403035563012,
+            "timestamp": 1.103431406460909
+        },
+        {
+            "x": 6.108490861918874,
+            "y": 6.5483997316058735,
+            "heading": -0.0991298571102857,
+            "angularVelocity": -5.886954924717416e-7,
+            "velocityX": 3.7643240482055917,
+            "velocityY": -0.3748375308486051,
+            "timestamp": 1.1953840236659847
+        },
+        {
+            "x": 6.4430213978276925,
+            "y": 6.453059099313297,
+            "heading": -0.09912987433352956,
+            "angularVelocity": -1.8730564053245824e-7,
+            "velocityX": 3.6380751965192784,
+            "velocityY": -1.0368452273625233,
+            "timestamp": 1.2873366408710605
+        },
+        {
+            "x": 6.751459095644277,
+            "y": 6.29222781566083,
+            "heading": -0.09912986944515287,
+            "angularVelocity": 5.316190922150303e-8,
+            "velocityX": 3.3543112441127847,
+            "velocityY": -1.7490669492721016,
+            "timestamp": 1.3792892580761362
+        },
+        {
+            "x": 7.058385657911462,
+            "y": 6.1285311430110365,
+            "heading": -0.09912986456630993,
+            "angularVelocity": 5.305822810376802e-8,
+            "velocityX": 3.337877393774089,
+            "velocityY": -1.7802285310130133,
+            "timestamp": 1.471241875281212
+        },
+        {
+            "x": 7.307031381410103,
+            "y": 5.995918713976455,
+            "heading": -0.06067697672181489,
+            "angularVelocity": 0.4181815484244036,
+            "velocityX": 2.7040635824873065,
+            "velocityY": -1.442182213681043,
+            "timestamp": 1.5631944924862877
+        },
+        {
+            "x": 7.4935156893705,
+            "y": 5.896459366852187,
+            "heading": -0.03183830930275791,
+            "angularVelocity": 0.3136253028528817,
+            "velocityX": 2.028047853651562,
+            "velocityY": -1.0816369359281015,
+            "timestamp": 1.6551471096913635
+        },
+        {
+            "x": 7.617838563683624,
+            "y": 5.830153124497745,
+            "heading": -0.012612817121248107,
+            "angularVelocity": 0.2090804238734443,
+            "velocityX": 1.3520319278771131,
+            "velocityY": -0.7210914095741754,
+            "timestamp": 1.7470997268964392
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0.10573119228742708,
-            "velocityX": 0.6760099290690327,
-            "velocityY": -0.3605641529201271,
-            "timestamp": 1.83909111867978
+            "heading": -0.003,
+            "angularVelocity": 0.10454098440514517,
+            "velocityX": 0.6760159548013833,
+            "velocityY": -0.36054574089833413,
+            "timestamp": 1.839052344101515
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": -3.143185307179586,
-            "angularVelocity": 3.5257869003902484e-31,
+            "heading": -0.003,
+            "angularVelocity": -2.423215560944852e-29,
             "velocityX": 0,
-            "velocityY": -2.4831219594454356e-32,
-            "timestamp": 1.931045674613769
+            "velocityY": -2.0134519400533763e-31,
+            "timestamp": 1.9310049613065907
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.1.traj
deleted file mode 100644
index 241d7413..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.1.traj	
+++ /dev/null
@@ -1,526 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -1.6129974497596655e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3086679912938655,
-            "y": 6.746243793862482,
-            "heading": -2.736085320412874,
-            "angularVelocity": 0.33564007387910216,
-            "velocityX": 0.20908316429709453,
-            "velocityY": -0.25945360797208683,
-            "timestamp": 0.04145714612176501
-        },
-        {
-            "x": 2.3260411538964876,
-            "y": 6.724713219688379,
-            "heading": -2.708761093447192,
-            "angularVelocity": 0.6590957053683163,
-            "velocityX": 0.4190631586553287,
-            "velocityY": -0.5193453044467995,
-            "timestamp": 0.08291429224353002
-        },
-        {
-            "x": 2.352163012722991,
-            "y": 6.692386750887991,
-            "heading": -2.6686499617636756,
-            "angularVelocity": 0.9675323903315748,
-            "velocityX": 0.6300930302770934,
-            "velocityY": -0.7797562501152615,
-            "timestamp": 0.12437143836529502
-        },
-        {
-            "x": 2.387085245084356,
-            "y": 6.649236203863692,
-            "heading": -2.6165769378043913,
-            "angularVelocity": 1.2560687078251613,
-            "velocityX": 0.8423694254976876,
-            "velocityY": -1.0408470206212592,
-            "timestamp": 0.16582858448706003
-        },
-        {
-            "x": 2.4308688712635975,
-            "y": 6.595222928663028,
-            "heading": -2.553684267520584,
-            "angularVelocity": 1.5170525751841024,
-            "velocityX": 1.056117708890124,
-            "velocityY": -1.3028700779841438,
-            "timestamp": 0.20728573060882505
-        },
-        {
-            "x": 2.4835846779423907,
-            "y": 6.530294843671354,
-            "heading": -2.481592483057959,
-            "angularVelocity": 1.7389471106110828,
-            "velocityX": 1.271573458625441,
-            "velocityY": -1.5661494112733225,
-            "timestamp": 0.24874287673059006
-        },
-        {
-            "x": 2.5453132269093315,
-            "y": 6.454388220972069,
-            "heading": -2.4026634058172087,
-            "angularVelocity": 1.9038714582264231,
-            "velocityX": 1.4889724629292092,
-            "velocityY": -1.830965944359449,
-            "timestamp": 0.2902000228523551
-        },
-        {
-            "x": 2.6161417178055544,
-            "y": 6.3674459995028085,
-            "heading": -2.3205870553837444,
-            "angularVelocity": 1.9797877594467097,
-            "velocityX": 1.708474835392443,
-            "velocityY": -2.0971588640930596,
-            "timestamp": 0.33165716897412006
-        },
-        {
-            "x": 2.696110411470073,
-            "y": 6.269518903939401,
-            "heading": -2.2423584912738157,
-            "angularVelocity": 1.8869741752160536,
-            "velocityX": 1.9289483513804893,
-            "velocityY": -2.3621282390202,
-            "timestamp": 0.37311431509588505
-        },
-        {
-            "x": 2.7844874173560816,
-            "y": 6.1625113811609245,
-            "heading": -2.1922364761983766,
-            "angularVelocity": 1.2090078494121173,
-            "velocityX": 2.131767720489823,
-            "velocityY": -2.581159891329275,
-            "timestamp": 0.41457146121765004
-        },
-        {
-            "x": 2.8809289045046786,
-            "y": 6.046946295339885,
-            "heading": -2.1746156662329965,
-            "angularVelocity": 0.42503673344096826,
-            "velocityX": 2.3262934420361696,
-            "velocityY": -2.7875793833374174,
-            "timestamp": 0.456028607339415
-        },
-        {
-            "x": 2.9795919491542358,
-            "y": 5.925039697257722,
-            "heading": -2.1746156143142836,
-            "angularVelocity": 0.000001252346527187571,
-            "velocityX": 2.3798802831186356,
-            "velocityY": -2.940544863462319,
-            "timestamp": 0.49748575346118
-        },
-        {
-            "x": 3.0782548699283607,
-            "y": 5.803132998918896,
-            "heading": -2.174615562396172,
-            "angularVelocity": 0.0000012523320226734394,
-            "velocityX": 2.37987729508295,
-            "velocityY": -2.940547281782763,
-            "timestamp": 0.5389428995829451
-        },
-        {
-            "x": 3.1769177990519513,
-            "y": 5.6812263073375435,
-            "heading": -2.1746155104780764,
-            "angularVelocity": 0.0000012523316371956637,
-            "velocityX": 2.379877496482862,
-            "velocityY": -2.940547118783714,
-            "timestamp": 0.58040004570471
-        },
-        {
-            "x": 3.2755807374460058,
-            "y": 5.559319623259062,
-            "heading": -2.174615458559997,
-            "angularVelocity": 0.0000012523312531723852,
-            "velocityX": 2.3798777200984556,
-            "velocityY": -2.9405469378047924,
-            "timestamp": 0.621857191826475
-        },
-        {
-            "x": 3.374243686147731,
-            "y": 5.437412947522895,
-            "heading": -2.1746154066419328,
-            "angularVelocity": 0.0000012523308771614933,
-            "velocityX": 2.379877968732799,
-            "velocityY": -2.9405467365773754,
-            "timestamp": 0.66331433794824
-        },
-        {
-            "x": 3.472906646327713,
-            "y": 5.315506281076437,
-            "heading": -2.174615354723884,
-            "angularVelocity": 0.000001252330513608687,
-            "velocityX": 2.3798782456032144,
-            "velocityY": -2.9405465124975687,
-            "timestamp": 0.704771484070005
-        },
-        {
-            "x": 3.5715696192957083,
-            "y": 5.193599624979717,
-            "heading": -2.1746153028058486,
-            "angularVelocity": 0.0000012523301754013102,
-            "velocityX": 2.379878554066626,
-            "velocityY": -2.940546262848488,
-            "timestamp": 0.74622863019177
-        },
-        {
-            "x": 3.6702326064938156,
-            "y": 5.071692980399876,
-            "heading": -2.174615250887826,
-            "angularVelocity": 0.0000012523298755846902,
-            "velocityX": 2.3798788973153537,
-            "velocityY": -2.940545985046467,
-            "timestamp": 0.787685776313535
-        },
-        {
-            "x": 3.7688956094655266,
-            "y": 4.949786348586114,
-            "heading": -2.1746151989698133,
-            "angularVelocity": 0.0000012523296313000306,
-            "velocityX": 2.379879277795066,
-            "velocityY": -2.940545677112118,
-            "timestamp": 0.8291429224353
-        },
-        {
-            "x": 3.8675586330203298,
-            "y": 4.82787973343092,
-            "heading": -2.1746151470518074,
-            "angularVelocity": 0.0000012523294719911187,
-            "velocityX": 2.379879774285891,
-            "velocityY": -2.940545275285949,
-            "timestamp": 0.870600068557065
-        },
-        {
-            "x": 3.9662940139338945,
-            "y": 4.706031714187132,
-            "heading": -2.1746150951231753,
-            "angularVelocity": 0.0000012525857863060412,
-            "velocityX": 2.381625127392215,
-            "velocityY": -2.939131866093794,
-            "timestamp": 0.9120572146788299
-        },
-        {
-            "x": 4.07569184834889,
-            "y": 4.593658547130379,
-            "heading": -2.174615041084707,
-            "angularVelocity": 0.0000013034777659582994,
-            "velocityX": 2.638817300488551,
-            "velocityY": -2.7105861731702134,
-            "timestamp": 0.9535143608005949
-        },
-        {
-            "x": 4.1948903308489776,
-            "y": 4.491740265556163,
-            "heading": -2.17461498286675,
-            "angularVelocity": 0.0000014042924363783103,
-            "velocityX": 2.8752216119746037,
-            "velocityY": -2.458400809232485,
-            "timestamp": 0.9949715069223599
-        },
-        {
-            "x": 4.322894391111494,
-            "y": 4.401128047699223,
-            "heading": -2.1746149179117107,
-            "angularVelocity": 0.00000156679957607106,
-            "velocityX": 3.087623539897125,
-            "velocityY": -2.1856839250535947,
-            "timestamp": 1.036428653044125
-        },
-        {
-            "x": 4.458635270784365,
-            "y": 4.322578536823241,
-            "heading": -2.1746148427021956,
-            "angularVelocity": 0.0000018141508159566887,
-            "velocityX": 3.2742456336522188,
-            "velocityY": -1.8947158264409445,
-            "timestamp": 1.07788579916589
-        },
-        {
-            "x": 4.600979576445416,
-            "y": 4.2567476280747965,
-            "heading": -2.174614751821203,
-            "angularVelocity": 0.000002192167120840163,
-            "velocityX": 3.433528811726906,
-            "velocityY": -1.5879266883226886,
-            "timestamp": 1.1193429452876549
-        },
-        {
-            "x": 4.748738765716553,
-            "y": 4.2041850090026855,
-            "heading": -2.174614640016373,
-            "angularVelocity": 0.000002696877156656919,
-            "velocityX": 3.564142809954843,
-            "velocityY": -1.2678783753644691,
-            "timestamp": 1.1608000914094199
-        },
-        {
-            "x": 4.880414686886391,
-            "y": 4.168830160718546,
-            "heading": -2.174614553815122,
-            "angularVelocity": 0.0000023917765874926725,
-            "velocityX": 3.65353612769536,
-            "velocityY": -0.9809706615128672,
-            "timestamp": 1.1968407699651018
-        },
-        {
-            "x": 5.014481469919241,
-            "y": 4.144038758633067,
-            "heading": -2.1746144885447847,
-            "angularVelocity": 0.0000018110185423275369,
-            "velocityX": 3.7198739980913533,
-            "velocityY": -0.6878727892755006,
-            "timestamp": 1.2328814485207837
-        },
-        {
-            "x": 5.150093083338604,
-            "y": 4.129967234831094,
-            "heading": -2.1746144356224972,
-            "angularVelocity": 0.0000014684043018781928,
-            "velocityX": 3.7627375192130574,
-            "velocityY": -0.3904344858611157,
-            "timestamp": 1.2689221270764657
-        },
-        {
-            "x": 5.286393751264718,
-            "y": 4.1267043465211,
-            "heading": -2.1746143904583044,
-            "angularVelocity": 0.000001253144907070333,
-            "velocityX": 3.7818563187020433,
-            "velocityY": -0.09053348718039304,
-            "timestamp": 1.3049628056321476
-        },
-        {
-            "x": 5.422523381587792,
-            "y": 4.134270240486654,
-            "heading": -2.1746143488300143,
-            "angularVelocity": 0.0000011550362392666718,
-            "velocityX": 3.7771106366034917,
-            "velocityY": 0.20992651272823276,
-            "timestamp": 1.3410034841878296
-        },
-        {
-            "x": 5.558824040120251,
-            "y": 4.131006987177355,
-            "heading": -2.1746143024581284,
-            "angularVelocity": 0.0000012866540842201377,
-            "velocityX": 3.7818560580616727,
-            "velocityY": -0.09054361460641977,
-            "timestamp": 1.3770441627435115
-        },
-        {
-            "x": 5.69443561806916,
-            "y": 4.116935078707767,
-            "heading": -2.1746143409291885,
-            "angularVelocity": -0.000001067434403137199,
-            "velocityX": 3.762736535034787,
-            "velocityY": -0.3904451590124025,
-            "timestamp": 1.4130848412991934
-        },
-        {
-            "x": 5.830736232387668,
-            "y": 4.113670630261612,
-            "heading": -2.1746144425807508,
-            "angularVelocity": -0.000002820467492063081,
-            "velocityX": 3.7818548312825957,
-            "velocityY": -0.09057677538205054,
-            "timestamp": 1.4491255198548754
-        },
-        {
-            "x": 5.967072945403984,
-            "y": 4.1145733334601475,
-            "heading": -2.174614542773063,
-            "angularVelocity": -0.000002779978522071658,
-            "velocityX": 3.7828564411094536,
-            "velocityY": 0.02504678698377033,
-            "timestamp": 1.4851661984105573
-        },
-        {
-            "x": 6.103409655835918,
-            "y": 4.115476426894617,
-            "heading": -2.1746146429653797,
-            "angularVelocity": -0.0000027799786444267726,
-            "velocityX": 3.7828563694020714,
-            "velocityY": 0.025057614636036382,
-            "timestamp": 1.5212068769662392
-        },
-        {
-            "x": 6.239746366170269,
-            "y": 4.1163795350566765,
-            "heading": -2.1746147431577554,
-            "angularVelocity": -0.000002779980281350245,
-            "velocityX": 3.782856366694493,
-            "velocityY": 0.025058023274013706,
-            "timestamp": 1.5572475555219212
-        },
-        {
-            "x": 6.37608307646628,
-            "y": 4.117282649003582,
-            "heading": -2.174614843350174,
-            "angularVelocity": -0.0000027799814898706564,
-            "velocityX": 3.7828563656306846,
-            "velocityY": 0.025058183782793973,
-            "timestamp": 1.5932882340776031
-        },
-        {
-            "x": 6.512419786749522,
-            "y": 4.118185764875291,
-            "heading": -2.174614943542629,
-            "angularVelocity": -0.00000277998246882532,
-            "velocityX": 3.782856365276422,
-            "velocityY": 0.025058237189222135,
-            "timestamp": 1.629328912633285
-        },
-        {
-            "x": 6.648756497843916,
-            "y": 4.11908875827432,
-            "heading": -2.1746150437352676,
-            "angularVelocity": -0.000002779987571113984,
-            "velocityX": 3.782856387782983,
-            "velocityY": 0.02505483901017122,
-            "timestamp": 1.665369591188967
-        },
-        {
-            "x": 6.784343394669118,
-            "y": 4.118261735488966,
-            "heading": -2.17648334857758,
-            "angularVelocity": -0.05183878098815612,
-            "velocityX": 3.762051722076281,
-            "velocityY": -0.02294692604294446,
-            "timestamp": 1.701410269744649
-        },
-        {
-            "x": 6.911008509970103,
-            "y": 4.118512712423575,
-            "heading": -2.2004225516217266,
-            "angularVelocity": -0.6642273121234696,
-            "velocityX": 3.5145041763098894,
-            "velocityY": 0.006963712800855175,
-            "timestamp": 1.7374509483003309
-        },
-        {
-            "x": 7.028751474127586,
-            "y": 4.119997003379487,
-            "heading": -2.2461001757447607,
-            "angularVelocity": -1.267390791559704,
-            "velocityX": 3.2669463749294456,
-            "velocityY": 0.04118376832493557,
-            "timestamp": 1.7734916268560128
-        },
-        {
-            "x": 7.137616008718106,
-            "y": 4.122892854321661,
-            "heading": -2.313255173533344,
-            "angularVelocity": -1.863311138407951,
-            "velocityX": 3.0206016910121933,
-            "velocityY": 0.08034951222410198,
-            "timestamp": 1.8095323054116947
-        },
-        {
-            "x": 7.237654743603063,
-            "y": 4.124394361652757,
-            "heading": -2.4021910734424403,
-            "angularVelocity": -2.467653314897855,
-            "velocityX": 2.775717297619687,
-            "velocityY": 0.04166146119519181,
-            "timestamp": 1.8455729839673767
-        },
-        {
-            "x": 7.328556844177467,
-            "y": 4.1248711523025925,
-            "heading": -2.5119123449992884,
-            "angularVelocity": -3.04437307936175,
-            "velocityX": 2.5222083550386993,
-            "velocityY": 0.013229236211493491,
-            "timestamp": 1.8816136625230586
-        },
-        {
-            "x": 7.408737931676895,
-            "y": 4.125345359362809,
-            "heading": -2.6271223888576807,
-            "angularVelocity": -3.1966668907300306,
-            "velocityX": 2.224738565217367,
-            "velocityY": 0.013157550834778516,
-            "timestamp": 1.9176543410787406
-        },
-        {
-            "x": 7.478113482970281,
-            "y": 4.125802607297277,
-            "heading": -2.739701188642133,
-            "angularVelocity": -3.123659273243732,
-            "velocityX": 1.9249235606428023,
-            "velocityY": 0.012686995716837342,
-            "timestamp": 1.9536950196344225
-        },
-        {
-            "x": 7.536867447368059,
-            "y": 4.126215703233864,
-            "heading": -2.8446379232047505,
-            "angularVelocity": -2.911619280433174,
-            "velocityX": 1.630212491893156,
-            "velocityY": 0.011461935600054532,
-            "timestamp": 1.9897356981901044
-        },
-        {
-            "x": 7.585249192103714,
-            "y": 4.126543507918265,
-            "heading": -2.93809966154793,
-            "angularVelocity": -2.593229153518397,
-            "velocityX": 1.3424204724921138,
-            "velocityY": 0.009095408231394075,
-            "timestamp": 2.025776376745786
-        },
-        {
-            "x": 7.623522580110136,
-            "y": 4.126765972225253,
-            "heading": -3.01693933623456,
-            "angularVelocity": -2.1875191546359196,
-            "velocityX": 1.0619497062823264,
-            "velocityY": 0.006172589304754541,
-            "timestamp": 2.061817055301468
-        },
-        {
-            "x": 7.651933909825069,
-            "y": 4.126896328898384,
-            "heading": -3.078642707410718,
-            "angularVelocity": -1.7120479871328622,
-            "velocityX": 0.7883128413089711,
-            "velocityY": 0.003616931710359869,
-            "timestamp": 2.0978577338571496
-        },
-        {
-            "x": 7.670698144632144,
-            "y": 4.126965664579503,
-            "heading": -3.1212443544252966,
-            "angularVelocity": -1.1820434220948564,
-            "velocityX": 0.5206404418297583,
-            "velocityY": 0.0019238173058132363,
-            "timestamp": 2.1338984124128313
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.6087830094650188,
-            "velocityX": 0.25809323632697007,
-            "velocityY": 0.0009526851844440906,
-            "timestamp": 2.169939090968513
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -3.143185307179586,
-            "angularVelocity": -7.326485384728368e-34,
-            "velocityX": -1.4740993581424338e-35,
-            "velocityY": 9.762457034283277e-35,
-            "timestamp": 2.2059797695241947
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.traj
deleted file mode 100644
index 241d7413..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.traj	
+++ /dev/null
@@ -1,526 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -1.6129974497596655e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3086679912938655,
-            "y": 6.746243793862482,
-            "heading": -2.736085320412874,
-            "angularVelocity": 0.33564007387910216,
-            "velocityX": 0.20908316429709453,
-            "velocityY": -0.25945360797208683,
-            "timestamp": 0.04145714612176501
-        },
-        {
-            "x": 2.3260411538964876,
-            "y": 6.724713219688379,
-            "heading": -2.708761093447192,
-            "angularVelocity": 0.6590957053683163,
-            "velocityX": 0.4190631586553287,
-            "velocityY": -0.5193453044467995,
-            "timestamp": 0.08291429224353002
-        },
-        {
-            "x": 2.352163012722991,
-            "y": 6.692386750887991,
-            "heading": -2.6686499617636756,
-            "angularVelocity": 0.9675323903315748,
-            "velocityX": 0.6300930302770934,
-            "velocityY": -0.7797562501152615,
-            "timestamp": 0.12437143836529502
-        },
-        {
-            "x": 2.387085245084356,
-            "y": 6.649236203863692,
-            "heading": -2.6165769378043913,
-            "angularVelocity": 1.2560687078251613,
-            "velocityX": 0.8423694254976876,
-            "velocityY": -1.0408470206212592,
-            "timestamp": 0.16582858448706003
-        },
-        {
-            "x": 2.4308688712635975,
-            "y": 6.595222928663028,
-            "heading": -2.553684267520584,
-            "angularVelocity": 1.5170525751841024,
-            "velocityX": 1.056117708890124,
-            "velocityY": -1.3028700779841438,
-            "timestamp": 0.20728573060882505
-        },
-        {
-            "x": 2.4835846779423907,
-            "y": 6.530294843671354,
-            "heading": -2.481592483057959,
-            "angularVelocity": 1.7389471106110828,
-            "velocityX": 1.271573458625441,
-            "velocityY": -1.5661494112733225,
-            "timestamp": 0.24874287673059006
-        },
-        {
-            "x": 2.5453132269093315,
-            "y": 6.454388220972069,
-            "heading": -2.4026634058172087,
-            "angularVelocity": 1.9038714582264231,
-            "velocityX": 1.4889724629292092,
-            "velocityY": -1.830965944359449,
-            "timestamp": 0.2902000228523551
-        },
-        {
-            "x": 2.6161417178055544,
-            "y": 6.3674459995028085,
-            "heading": -2.3205870553837444,
-            "angularVelocity": 1.9797877594467097,
-            "velocityX": 1.708474835392443,
-            "velocityY": -2.0971588640930596,
-            "timestamp": 0.33165716897412006
-        },
-        {
-            "x": 2.696110411470073,
-            "y": 6.269518903939401,
-            "heading": -2.2423584912738157,
-            "angularVelocity": 1.8869741752160536,
-            "velocityX": 1.9289483513804893,
-            "velocityY": -2.3621282390202,
-            "timestamp": 0.37311431509588505
-        },
-        {
-            "x": 2.7844874173560816,
-            "y": 6.1625113811609245,
-            "heading": -2.1922364761983766,
-            "angularVelocity": 1.2090078494121173,
-            "velocityX": 2.131767720489823,
-            "velocityY": -2.581159891329275,
-            "timestamp": 0.41457146121765004
-        },
-        {
-            "x": 2.8809289045046786,
-            "y": 6.046946295339885,
-            "heading": -2.1746156662329965,
-            "angularVelocity": 0.42503673344096826,
-            "velocityX": 2.3262934420361696,
-            "velocityY": -2.7875793833374174,
-            "timestamp": 0.456028607339415
-        },
-        {
-            "x": 2.9795919491542358,
-            "y": 5.925039697257722,
-            "heading": -2.1746156143142836,
-            "angularVelocity": 0.000001252346527187571,
-            "velocityX": 2.3798802831186356,
-            "velocityY": -2.940544863462319,
-            "timestamp": 0.49748575346118
-        },
-        {
-            "x": 3.0782548699283607,
-            "y": 5.803132998918896,
-            "heading": -2.174615562396172,
-            "angularVelocity": 0.0000012523320226734394,
-            "velocityX": 2.37987729508295,
-            "velocityY": -2.940547281782763,
-            "timestamp": 0.5389428995829451
-        },
-        {
-            "x": 3.1769177990519513,
-            "y": 5.6812263073375435,
-            "heading": -2.1746155104780764,
-            "angularVelocity": 0.0000012523316371956637,
-            "velocityX": 2.379877496482862,
-            "velocityY": -2.940547118783714,
-            "timestamp": 0.58040004570471
-        },
-        {
-            "x": 3.2755807374460058,
-            "y": 5.559319623259062,
-            "heading": -2.174615458559997,
-            "angularVelocity": 0.0000012523312531723852,
-            "velocityX": 2.3798777200984556,
-            "velocityY": -2.9405469378047924,
-            "timestamp": 0.621857191826475
-        },
-        {
-            "x": 3.374243686147731,
-            "y": 5.437412947522895,
-            "heading": -2.1746154066419328,
-            "angularVelocity": 0.0000012523308771614933,
-            "velocityX": 2.379877968732799,
-            "velocityY": -2.9405467365773754,
-            "timestamp": 0.66331433794824
-        },
-        {
-            "x": 3.472906646327713,
-            "y": 5.315506281076437,
-            "heading": -2.174615354723884,
-            "angularVelocity": 0.000001252330513608687,
-            "velocityX": 2.3798782456032144,
-            "velocityY": -2.9405465124975687,
-            "timestamp": 0.704771484070005
-        },
-        {
-            "x": 3.5715696192957083,
-            "y": 5.193599624979717,
-            "heading": -2.1746153028058486,
-            "angularVelocity": 0.0000012523301754013102,
-            "velocityX": 2.379878554066626,
-            "velocityY": -2.940546262848488,
-            "timestamp": 0.74622863019177
-        },
-        {
-            "x": 3.6702326064938156,
-            "y": 5.071692980399876,
-            "heading": -2.174615250887826,
-            "angularVelocity": 0.0000012523298755846902,
-            "velocityX": 2.3798788973153537,
-            "velocityY": -2.940545985046467,
-            "timestamp": 0.787685776313535
-        },
-        {
-            "x": 3.7688956094655266,
-            "y": 4.949786348586114,
-            "heading": -2.1746151989698133,
-            "angularVelocity": 0.0000012523296313000306,
-            "velocityX": 2.379879277795066,
-            "velocityY": -2.940545677112118,
-            "timestamp": 0.8291429224353
-        },
-        {
-            "x": 3.8675586330203298,
-            "y": 4.82787973343092,
-            "heading": -2.1746151470518074,
-            "angularVelocity": 0.0000012523294719911187,
-            "velocityX": 2.379879774285891,
-            "velocityY": -2.940545275285949,
-            "timestamp": 0.870600068557065
-        },
-        {
-            "x": 3.9662940139338945,
-            "y": 4.706031714187132,
-            "heading": -2.1746150951231753,
-            "angularVelocity": 0.0000012525857863060412,
-            "velocityX": 2.381625127392215,
-            "velocityY": -2.939131866093794,
-            "timestamp": 0.9120572146788299
-        },
-        {
-            "x": 4.07569184834889,
-            "y": 4.593658547130379,
-            "heading": -2.174615041084707,
-            "angularVelocity": 0.0000013034777659582994,
-            "velocityX": 2.638817300488551,
-            "velocityY": -2.7105861731702134,
-            "timestamp": 0.9535143608005949
-        },
-        {
-            "x": 4.1948903308489776,
-            "y": 4.491740265556163,
-            "heading": -2.17461498286675,
-            "angularVelocity": 0.0000014042924363783103,
-            "velocityX": 2.8752216119746037,
-            "velocityY": -2.458400809232485,
-            "timestamp": 0.9949715069223599
-        },
-        {
-            "x": 4.322894391111494,
-            "y": 4.401128047699223,
-            "heading": -2.1746149179117107,
-            "angularVelocity": 0.00000156679957607106,
-            "velocityX": 3.087623539897125,
-            "velocityY": -2.1856839250535947,
-            "timestamp": 1.036428653044125
-        },
-        {
-            "x": 4.458635270784365,
-            "y": 4.322578536823241,
-            "heading": -2.1746148427021956,
-            "angularVelocity": 0.0000018141508159566887,
-            "velocityX": 3.2742456336522188,
-            "velocityY": -1.8947158264409445,
-            "timestamp": 1.07788579916589
-        },
-        {
-            "x": 4.600979576445416,
-            "y": 4.2567476280747965,
-            "heading": -2.174614751821203,
-            "angularVelocity": 0.000002192167120840163,
-            "velocityX": 3.433528811726906,
-            "velocityY": -1.5879266883226886,
-            "timestamp": 1.1193429452876549
-        },
-        {
-            "x": 4.748738765716553,
-            "y": 4.2041850090026855,
-            "heading": -2.174614640016373,
-            "angularVelocity": 0.000002696877156656919,
-            "velocityX": 3.564142809954843,
-            "velocityY": -1.2678783753644691,
-            "timestamp": 1.1608000914094199
-        },
-        {
-            "x": 4.880414686886391,
-            "y": 4.168830160718546,
-            "heading": -2.174614553815122,
-            "angularVelocity": 0.0000023917765874926725,
-            "velocityX": 3.65353612769536,
-            "velocityY": -0.9809706615128672,
-            "timestamp": 1.1968407699651018
-        },
-        {
-            "x": 5.014481469919241,
-            "y": 4.144038758633067,
-            "heading": -2.1746144885447847,
-            "angularVelocity": 0.0000018110185423275369,
-            "velocityX": 3.7198739980913533,
-            "velocityY": -0.6878727892755006,
-            "timestamp": 1.2328814485207837
-        },
-        {
-            "x": 5.150093083338604,
-            "y": 4.129967234831094,
-            "heading": -2.1746144356224972,
-            "angularVelocity": 0.0000014684043018781928,
-            "velocityX": 3.7627375192130574,
-            "velocityY": -0.3904344858611157,
-            "timestamp": 1.2689221270764657
-        },
-        {
-            "x": 5.286393751264718,
-            "y": 4.1267043465211,
-            "heading": -2.1746143904583044,
-            "angularVelocity": 0.000001253144907070333,
-            "velocityX": 3.7818563187020433,
-            "velocityY": -0.09053348718039304,
-            "timestamp": 1.3049628056321476
-        },
-        {
-            "x": 5.422523381587792,
-            "y": 4.134270240486654,
-            "heading": -2.1746143488300143,
-            "angularVelocity": 0.0000011550362392666718,
-            "velocityX": 3.7771106366034917,
-            "velocityY": 0.20992651272823276,
-            "timestamp": 1.3410034841878296
-        },
-        {
-            "x": 5.558824040120251,
-            "y": 4.131006987177355,
-            "heading": -2.1746143024581284,
-            "angularVelocity": 0.0000012866540842201377,
-            "velocityX": 3.7818560580616727,
-            "velocityY": -0.09054361460641977,
-            "timestamp": 1.3770441627435115
-        },
-        {
-            "x": 5.69443561806916,
-            "y": 4.116935078707767,
-            "heading": -2.1746143409291885,
-            "angularVelocity": -0.000001067434403137199,
-            "velocityX": 3.762736535034787,
-            "velocityY": -0.3904451590124025,
-            "timestamp": 1.4130848412991934
-        },
-        {
-            "x": 5.830736232387668,
-            "y": 4.113670630261612,
-            "heading": -2.1746144425807508,
-            "angularVelocity": -0.000002820467492063081,
-            "velocityX": 3.7818548312825957,
-            "velocityY": -0.09057677538205054,
-            "timestamp": 1.4491255198548754
-        },
-        {
-            "x": 5.967072945403984,
-            "y": 4.1145733334601475,
-            "heading": -2.174614542773063,
-            "angularVelocity": -0.000002779978522071658,
-            "velocityX": 3.7828564411094536,
-            "velocityY": 0.02504678698377033,
-            "timestamp": 1.4851661984105573
-        },
-        {
-            "x": 6.103409655835918,
-            "y": 4.115476426894617,
-            "heading": -2.1746146429653797,
-            "angularVelocity": -0.0000027799786444267726,
-            "velocityX": 3.7828563694020714,
-            "velocityY": 0.025057614636036382,
-            "timestamp": 1.5212068769662392
-        },
-        {
-            "x": 6.239746366170269,
-            "y": 4.1163795350566765,
-            "heading": -2.1746147431577554,
-            "angularVelocity": -0.000002779980281350245,
-            "velocityX": 3.782856366694493,
-            "velocityY": 0.025058023274013706,
-            "timestamp": 1.5572475555219212
-        },
-        {
-            "x": 6.37608307646628,
-            "y": 4.117282649003582,
-            "heading": -2.174614843350174,
-            "angularVelocity": -0.0000027799814898706564,
-            "velocityX": 3.7828563656306846,
-            "velocityY": 0.025058183782793973,
-            "timestamp": 1.5932882340776031
-        },
-        {
-            "x": 6.512419786749522,
-            "y": 4.118185764875291,
-            "heading": -2.174614943542629,
-            "angularVelocity": -0.00000277998246882532,
-            "velocityX": 3.782856365276422,
-            "velocityY": 0.025058237189222135,
-            "timestamp": 1.629328912633285
-        },
-        {
-            "x": 6.648756497843916,
-            "y": 4.11908875827432,
-            "heading": -2.1746150437352676,
-            "angularVelocity": -0.000002779987571113984,
-            "velocityX": 3.782856387782983,
-            "velocityY": 0.02505483901017122,
-            "timestamp": 1.665369591188967
-        },
-        {
-            "x": 6.784343394669118,
-            "y": 4.118261735488966,
-            "heading": -2.17648334857758,
-            "angularVelocity": -0.05183878098815612,
-            "velocityX": 3.762051722076281,
-            "velocityY": -0.02294692604294446,
-            "timestamp": 1.701410269744649
-        },
-        {
-            "x": 6.911008509970103,
-            "y": 4.118512712423575,
-            "heading": -2.2004225516217266,
-            "angularVelocity": -0.6642273121234696,
-            "velocityX": 3.5145041763098894,
-            "velocityY": 0.006963712800855175,
-            "timestamp": 1.7374509483003309
-        },
-        {
-            "x": 7.028751474127586,
-            "y": 4.119997003379487,
-            "heading": -2.2461001757447607,
-            "angularVelocity": -1.267390791559704,
-            "velocityX": 3.2669463749294456,
-            "velocityY": 0.04118376832493557,
-            "timestamp": 1.7734916268560128
-        },
-        {
-            "x": 7.137616008718106,
-            "y": 4.122892854321661,
-            "heading": -2.313255173533344,
-            "angularVelocity": -1.863311138407951,
-            "velocityX": 3.0206016910121933,
-            "velocityY": 0.08034951222410198,
-            "timestamp": 1.8095323054116947
-        },
-        {
-            "x": 7.237654743603063,
-            "y": 4.124394361652757,
-            "heading": -2.4021910734424403,
-            "angularVelocity": -2.467653314897855,
-            "velocityX": 2.775717297619687,
-            "velocityY": 0.04166146119519181,
-            "timestamp": 1.8455729839673767
-        },
-        {
-            "x": 7.328556844177467,
-            "y": 4.1248711523025925,
-            "heading": -2.5119123449992884,
-            "angularVelocity": -3.04437307936175,
-            "velocityX": 2.5222083550386993,
-            "velocityY": 0.013229236211493491,
-            "timestamp": 1.8816136625230586
-        },
-        {
-            "x": 7.408737931676895,
-            "y": 4.125345359362809,
-            "heading": -2.6271223888576807,
-            "angularVelocity": -3.1966668907300306,
-            "velocityX": 2.224738565217367,
-            "velocityY": 0.013157550834778516,
-            "timestamp": 1.9176543410787406
-        },
-        {
-            "x": 7.478113482970281,
-            "y": 4.125802607297277,
-            "heading": -2.739701188642133,
-            "angularVelocity": -3.123659273243732,
-            "velocityX": 1.9249235606428023,
-            "velocityY": 0.012686995716837342,
-            "timestamp": 1.9536950196344225
-        },
-        {
-            "x": 7.536867447368059,
-            "y": 4.126215703233864,
-            "heading": -2.8446379232047505,
-            "angularVelocity": -2.911619280433174,
-            "velocityX": 1.630212491893156,
-            "velocityY": 0.011461935600054532,
-            "timestamp": 1.9897356981901044
-        },
-        {
-            "x": 7.585249192103714,
-            "y": 4.126543507918265,
-            "heading": -2.93809966154793,
-            "angularVelocity": -2.593229153518397,
-            "velocityX": 1.3424204724921138,
-            "velocityY": 0.009095408231394075,
-            "timestamp": 2.025776376745786
-        },
-        {
-            "x": 7.623522580110136,
-            "y": 4.126765972225253,
-            "heading": -3.01693933623456,
-            "angularVelocity": -2.1875191546359196,
-            "velocityX": 1.0619497062823264,
-            "velocityY": 0.006172589304754541,
-            "timestamp": 2.061817055301468
-        },
-        {
-            "x": 7.651933909825069,
-            "y": 4.126896328898384,
-            "heading": -3.078642707410718,
-            "angularVelocity": -1.7120479871328622,
-            "velocityX": 0.7883128413089711,
-            "velocityY": 0.003616931710359869,
-            "timestamp": 2.0978577338571496
-        },
-        {
-            "x": 7.670698144632144,
-            "y": 4.126965664579503,
-            "heading": -3.1212443544252966,
-            "angularVelocity": -1.1820434220948564,
-            "velocityX": 0.5206404418297583,
-            "velocityY": 0.0019238173058132363,
-            "timestamp": 2.1338984124128313
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.6087830094650188,
-            "velocityX": 0.25809323632697007,
-            "velocityY": 0.0009526851844440906,
-            "timestamp": 2.169939090968513
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -3.143185307179586,
-            "angularVelocity": -7.326485384728368e-34,
-            "velocityX": -1.4740993581424338e-35,
-            "velocityY": 9.762457034283277e-35,
-            "timestamp": 2.2059797695241947
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.1.traj
index 5ed5f209..1a6d423b 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.1.traj	
@@ -3,578 +3,578 @@
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": -2.75,
+            "heading": 0.39,
             "angularVelocity": 0,
-            "velocityX": -2.2987344072087424e-31,
-            "velocityY": -2.682409549792184e-31,
+            "velocityX": 1.0547928981710615e-31,
+            "velocityY": 1.4824740100837611e-32,
             "timestamp": 0
         },
         {
-            "x": 2.30727109352441,
-            "y": 6.7499479361039905,
-            "heading": -2.752438811293409,
-            "angularVelocity": -0.06989284029038495,
-            "velocityX": 0.20837913118223148,
-            "velocityY": -0.20210205559294603,
-            "timestamp": 0.03489357827320871
-        },
-        {
-            "x": 2.321814327969628,
-            "y": 6.735842827712052,
-            "heading": -2.7572390739653767,
-            "angularVelocity": -0.1375686561688486,
-            "velocityX": 0.41678827924577844,
-            "velocityY": -0.4042322137758234,
-            "timestamp": 0.06978715654641741
-        },
-        {
-            "x": 2.3436309003249467,
-            "y": 6.714683550648929,
-            "heading": -2.7643090195026687,
-            "angularVelocity": -0.202614517832932,
-            "velocityX": 0.6252317313088365,
-            "velocityY": -0.6063945892120965,
-            "timestamp": 0.10468073481962611
-        },
-        {
-            "x": 2.3727221871027773,
-            "y": 6.6864688072553555,
-            "heading": -2.7735381226601596,
-            "angularVelocity": -0.26449288419860933,
-            "velocityX": 0.8337146322470192,
-            "velocityY": -0.8085941537052478,
-            "timestamp": 0.13957431309283483
-        },
-        {
-            "x": 2.409089780647816,
-            "y": 6.651197089471134,
-            "heading": -2.7847907497118745,
-            "angularVelocity": -0.32248418214977886,
-            "velocityX": 1.042243167504621,
-            "velocityY": -1.0108369370447357,
-            "timestamp": 0.17446789136604354
-        },
-        {
-            "x": 2.452735530672034,
-            "y": 6.6088666355169075,
-            "heading": -2.7978965041034827,
-            "angularVelocity": -0.3755921587918882,
-            "velocityX": 1.2508247128592573,
-            "velocityY": -1.213130210458454,
-            "timestamp": 0.20936146963925226
-        },
-        {
-            "x": 2.5036615834922005,
-            "y": 6.559475386735866,
-            "heading": -2.8126348139869735,
-            "angularVelocity": -0.42237886203854363,
-            "velocityX": 1.459467768579872,
-            "velocityY": -1.4154824820291676,
-            "timestamp": 0.24425504791246097
-        },
-        {
-            "x": 2.5618703896153354,
-            "y": 6.503020972116864,
-            "heading": -2.8287087438800844,
-            "angularVelocity": -0.4606558194535241,
-            "velocityX": 1.6681810523235565,
-            "velocityY": -1.6179027033850466,
-            "timestamp": 0.27914862618566966
-        },
-        {
-            "x": 2.6273645712551357,
-            "y": 6.439500824983286,
-            "heading": -2.8456966886741912,
-            "angularVelocity": -0.48685017802115704,
-            "velocityX": 1.8769694849578602,
-            "velocityY": -1.8203964820182346,
-            "timestamp": 0.31404220445887837
-        },
-        {
-            "x": 2.700146215046118,
-            "y": 6.368912855338095,
-            "heading": -2.8629525699896186,
-            "angularVelocity": -0.4945288551468594,
-            "velocityX": 2.085817717550196,
-            "velocityY": -2.0229501569744945,
-            "timestamp": 0.3489357827320871
-        },
-        {
-            "x": 2.780213462242076,
-            "y": 6.2912587361998735,
-            "heading": -2.8793619429160175,
-            "angularVelocity": -0.47026913657055547,
-            "velocityX": 2.294612681137229,
-            "velocityY": -2.225455885613285,
-            "timestamp": 0.3838293610052958
-        },
-        {
-            "x": 2.8675389350998404,
-            "y": 6.20656386499432,
-            "heading": -2.892555231685043,
-            "angularVelocity": -0.37810076873529364,
-            "velocityX": 2.502623037798732,
-            "velocityY": -2.42723376039027,
-            "timestamp": 0.4187229392785045
-        },
-        {
-            "x": 2.9617036478615395,
-            "y": 6.11507787860357,
-            "heading": -2.8943452360571387,
-            "angularVelocity": -0.05129896275126653,
-            "velocityX": 2.698625862455626,
-            "velocityY": -2.621857399502999,
-            "timestamp": 0.4536165175517132
-        },
-        {
-            "x": 3.05646121242224,
-            "y": 6.023180735189561,
-            "heading": -2.894345248240927,
-            "angularVelocity": -3.4916993473204957e-7,
-            "velocityX": 2.7156161462940758,
-            "velocityY": -2.633640571181149,
-            "timestamp": 0.48851009582492194
-        },
-        {
-            "x": 3.151218783278769,
-            "y": 5.931283598267322,
-            "heading": -2.894345260424686,
-            "angularVelocity": -3.4916908908792776e-7,
-            "velocityX": 2.7156163267235134,
-            "velocityY": -2.633640385136315,
-            "timestamp": 0.5234036740981306
-        },
-        {
-            "x": 3.245976354135778,
-            "y": 5.839386461345578,
-            "heading": -2.894345272608445,
-            "angularVelocity": -3.491690897148369e-7,
-            "velocityX": 2.715616326737254,
-            "velocityY": -2.633640385122147,
-            "timestamp": 0.5582972523713393
-        },
-        {
-            "x": 3.340733924992786,
-            "y": 5.747489324423833,
-            "heading": -2.894345284792204,
-            "angularVelocity": -3.491690888486973e-7,
-            "velocityX": 2.715616326737255,
-            "velocityY": -2.633640385122146,
-            "timestamp": 0.593190830644548
-        },
-        {
-            "x": 3.435491495849795,
-            "y": 5.655592187502088,
-            "heading": -2.894345296975963,
-            "angularVelocity": -3.4916908897748007e-7,
-            "velocityX": 2.715616326737255,
-            "velocityY": -2.633640385122146,
-            "timestamp": 0.6280844089177567
-        },
-        {
-            "x": 3.530249066706803,
-            "y": 5.5636950505803435,
-            "heading": -2.8943453091597218,
-            "angularVelocity": -3.491690889850577e-7,
-            "velocityX": 2.715616326737255,
-            "velocityY": -2.633640385122146,
-            "timestamp": 0.6629779871909655
-        },
-        {
-            "x": 3.625006637563812,
-            "y": 5.471797913658599,
-            "heading": -2.8943453213434807,
-            "angularVelocity": -3.49169089086754e-7,
-            "velocityX": 2.715616326737255,
-            "velocityY": -2.633640385122146,
-            "timestamp": 0.6978715654641742
-        },
-        {
-            "x": 3.7197642084208202,
-            "y": 5.379900776736854,
-            "heading": -2.8943453335272395,
-            "angularVelocity": -3.4916908901998635e-7,
-            "velocityX": 2.71561632673725,
-            "velocityY": -2.6336403851221513,
-            "timestamp": 0.7327651437373829
-        },
-        {
-            "x": 3.814521779275631,
-            "y": 5.288003639812843,
-            "heading": -2.8943453457109984,
-            "angularVelocity": -3.4916908849611257e-7,
-            "velocityX": 2.715616326674264,
-            "velocityY": -2.633640385187098,
-            "timestamp": 0.7676587220105916
-        },
-        {
-            "x": 3.9092793212702794,
-            "y": 5.196106473130367,
-            "heading": -2.8943453578947573,
-            "angularVelocity": -3.4916909136087424e-7,
-            "velocityX": 2.715615499583321,
-            "velocityY": -2.6336412380221117,
-            "timestamp": 0.8025523002838003
-        },
-        {
-            "x": 4.0036585134090545,
-            "y": 5.103820780001642,
-            "heading": -2.8943453700832222,
-            "angularVelocity": -3.49303962122221e-7,
-            "velocityX": 2.704772534355369,
-            "velocityY": -2.644775849761548,
-            "timestamp": 0.837445878557009
-        },
-        {
-            "x": 4.09067331484616,
-            "y": 5.004560993237775,
-            "heading": -2.894345382446594,
-            "angularVelocity": -3.5431651835190834e-7,
-            "velocityX": 2.4937196396368213,
-            "velocityY": -2.8446433892988883,
-            "timestamp": 0.8723394568302177
+            "x": 2.3072711000078927,
+            "y": 6.749947927475375,
+            "heading": 0.3875624240531776,
+            "angularVelocity": -0.06985747897751753,
+            "velocityX": 0.2083794421285326,
+            "velocityY": -0.20210242424612682,
+            "timestamp": 0.034893557318421635
+        },
+        {
+            "x": 2.321814346754588,
+            "y": 6.735842802544083,
+            "heading": 0.3827646159677583,
+            "angularVelocity": -0.1374983938048157,
+            "velocityX": 0.41678888208448434,
+            "velocityY": -0.40423293052572296,
+            "timestamp": 0.06978711463684327
+        },
+        {
+            "x": 2.343630936460155,
+            "y": 6.714683501867812,
+            "heading": 0.3756983254810237,
+            "angularVelocity": -0.20250989093060998,
+            "velocityX": 0.6252326040156703,
+            "velocityY": -0.6063956300924075,
+            "timestamp": 0.1046806719552649
+        },
+        {
+            "x": 2.372722244735657,
+            "y": 6.6864687287677045,
+            "heading": 0.36647405587954873,
+            "angularVelocity": -0.2643545201567945,
+            "velocityX": 0.8337157490143364,
+            "velocityY": -0.8085954906412398,
+            "timestamp": 0.13957422927368654
+        },
+        {
+            "x": 2.4090898628559576,
+            "y": 6.651196976344128,
+            "heading": 0.3552274150516352,
+            "angularVelocity": -0.3223128191053197,
+            "velocityX": 1.042244497699901,
+            "velocityY": -1.0108385368022033,
+            "timestamp": 0.17446778659210818
+        },
+        {
+            "x": 2.452735639247045,
+            "y": 6.608866484215248,
+            "heading": 0.3421287688368095,
+            "angularVelocity": -0.3753886740550377,
+            "velocityX": 1.2508262196599174,
+            "velocityY": -1.213132033016619,
+            "timestamp": 0.20936134391052982
+        },
+        {
+            "x": 2.5036617186589587,
+            "y": 6.55947519544937,
+            "heading": 0.3273986519293141,
+            "angularVelocity": -0.42214431658760004,
+            "velocityX": 1.459469407122573,
+            "velocityY": -1.4154844779841917,
+            "timestamp": 0.24425490122895147
+        },
+        {
+            "x": 2.5618705496598224,
+            "y": 6.503020741236739,
+            "heading": 0.31133395345849213,
+            "angularVelocity": -0.4603915365872077,
+            "velocityX": 1.6681827670844436,
+            "velocityY": -1.6179048096889317,
+            "timestamp": 0.2791484585473731
+        },
+        {
+            "x": 2.6273647520175327,
+            "y": 6.43950055782557,
+            "heading": 0.294356217256913,
+            "angularVelocity": -0.486557906568497,
+            "velocityX": 1.8769712058889019,
+            "velocityY": -1.8203986148936577,
+            "timestamp": 0.3140420158657947
+        },
+        {
+            "x": 2.7001464092257668,
+            "y": 6.368912559254549,
+            "heading": 0.2771114307132975,
+            "angularVelocity": -0.4942111916606252,
+            "velocityX": 2.0858193546752704,
+            "velocityY": -2.022952200799379,
+            "timestamp": 0.34893557318421636
+        },
+        {
+            "x": 2.7802136587601143,
+            "y": 6.291258423996795,
+            "heading": 0.2607138645949654,
+            "angularVelocity": -0.46993105256354084,
+            "velocityX": 2.294614126146364,
+            "velocityY": -2.225457684039485,
+            "timestamp": 0.383829130502638
+        },
+        {
+            "x": 2.867539127268515,
+            "y": 6.206563552711186,
+            "heading": 0.24753249327015822,
+            "angularVelocity": -0.37775945870237265,
+            "velocityX": 2.5026244160637545,
+            "velocityY": -2.427235220322282,
+            "timestamp": 0.41872268782105965
+        },
+        {
+            "x": 2.961704724093414,
+            "y": 6.1150775036748035,
+            "heading": 0.24574502539338514,
+            "angularVelocity": -0.05122630118968667,
+            "velocityX": 2.6986528190746,
+            "velocityY": -2.6218607693541762,
+            "timestamp": 0.4536162451394813
+        },
+        {
+            "x": 3.0564622015652247,
+            "y": 6.023180384324785,
+            "heading": 0.2457450132145974,
+            "angularVelocity": -3.4902683097432965e-7,
+            "velocityX": 2.7156152812710346,
+            "velocityY": -2.633641463133395,
+            "timestamp": 0.48850980245790293
+        },
+        {
+            "x": 3.1512196852998877,
+            "y": 5.931283271432529,
+            "heading": 0.24574500103583913,
+            "angularVelocity": -3.490259866463667e-7,
+            "velocityX": 2.7156154607555005,
+            "velocityY": -2.6336412780630685,
+            "timestamp": 0.5234033597763246
+        },
+        {
+            "x": 3.2459771690350276,
+            "y": 5.839386158540766,
+            "heading": 0.2457449888570809,
+            "angularVelocity": -3.4902598588384144e-7,
+            "velocityX": 2.7156154607691696,
+            "velocityY": -2.6336412780489744,
+            "timestamp": 0.5582969170947462
+        },
+        {
+            "x": 3.3407346527701676,
+            "y": 5.747489045649002,
+            "heading": 0.24574497667832262,
+            "angularVelocity": -3.4902598639891e-7,
+            "velocityX": 2.7156154607691705,
+            "velocityY": -2.6336412780489735,
+            "timestamp": 0.5931904744131677
+        },
+        {
+            "x": 3.4354921365053075,
+            "y": 5.655591932757238,
+            "heading": 0.24574496449956437,
+            "angularVelocity": -3.4902598640760607e-7,
+            "velocityX": 2.71561546076917,
+            "velocityY": -2.6336412780489735,
+            "timestamp": 0.6280840317315893
+        },
+        {
+            "x": 3.5302496202404474,
+            "y": 5.563694819865473,
+            "heading": 0.2457449523208061,
+            "angularVelocity": -3.490259865548224e-7,
+            "velocityX": 2.7156154607691705,
+            "velocityY": -2.6336412780489735,
+            "timestamp": 0.6629775890500109
+        },
+        {
+            "x": 3.6250071039755873,
+            "y": 5.471797706973709,
+            "heading": 0.2457449401420478,
+            "angularVelocity": -3.4902598644057735e-7,
+            "velocityX": 2.71561546076917,
+            "velocityY": -2.633641278048974,
+            "timestamp": 0.6978711463684325
+        },
+        {
+            "x": 3.719764587710727,
+            "y": 5.379900594081945,
+            "heading": 0.24574492796328956,
+            "angularVelocity": -3.4902598592532335e-7,
+            "velocityX": 2.715615460769166,
+            "velocityY": -2.633641278048979,
+            "timestamp": 0.7327647036868541
+        },
+        {
+            "x": 3.8145220714436685,
+            "y": 5.288003481187913,
+            "heading": 0.24574491578453134,
+            "angularVelocity": -3.490259859254125e-7,
+            "velocityX": 2.7156154607061804,
+            "velocityY": -2.6336412781139247,
+            "timestamp": 0.7676582610052757
+        },
+        {
+            "x": 3.909279526317127,
+            "y": 5.196106338536137,
+            "heading": 0.245744903605773,
+            "angularVelocity": -3.4902598866436484e-7,
+            "velocityX": 2.7156146336342,
+            "velocityY": -2.6336421309288234,
+            "timestamp": 0.8025518183236973
+        },
+        {
+            "x": 4.003658642045134,
+            "y": 5.103820680647278,
+            "heading": 0.24574489142231412,
+            "angularVelocity": -3.4916069991394096e-7,
+            "velocityX": 2.7047719688402356,
+            "velocityY": -2.644776428116158,
+            "timestamp": 0.8374453756421188
+        },
+        {
+            "x": 4.090673374879912,
+            "y": 5.004560939161802,
+            "heading": 0.24574487906407927,
+            "angularVelocity": -3.541695320845138e-7,
+            "velocityX": 2.493719171155964,
+            "velocityY": -2.8446437999909007,
+            "timestamp": 0.8723389329605404
         },
         {
             "x": 4.169802665710449,
             "y": 4.898907661437988,
-            "heading": -2.8943453954133593,
-            "angularVelocity": -3.71608932660535e-7,
-            "velocityX": 2.2677339149549134,
-            "velocityY": -3.0278732370909647,
-            "timestamp": 0.9072330351034265
-        },
-        {
-            "x": 4.267711504415231,
-            "y": 4.731769329108682,
-            "heading": -2.8943454067066363,
-            "angularVelocity": -2.2055157567695873e-7,
-            "velocityX": 1.9121064876426594,
-            "velocityY": -3.2641209293092457,
-            "timestamp": 0.9584377349135761
-        },
-        {
-            "x": 4.3838302459464416,
-            "y": 4.576728047661677,
-            "heading": -2.8943454207852617,
-            "angularVelocity": -2.749479155903796e-7,
-            "velocityX": 2.2677360078614184,
-            "velocityY": -3.027872090293428,
-            "timestamp": 1.0096424347237256
-        },
-        {
-            "x": 4.5166798451311,
-            "y": 4.43575875391577,
-            "heading": -2.8943454409364353,
-            "angularVelocity": -3.9354148044055934e-7,
-            "velocityX": 2.594480578486377,
-            "velocityY": -2.7530538069469372,
-            "timestamp": 1.0608471345338752
-        },
-        {
-            "x": 4.664567965806816,
-            "y": 4.3106572284696325,
-            "heading": -2.8943459031991594,
-            "angularVelocity": -0.000009027740145972362,
-            "velocityX": 2.888174742241181,
-            "velocityY": -2.443164903025968,
-            "timestamp": 1.1120518343440249
-        },
-        {
-            "x": 4.823636896940825,
-            "y": 4.2042733124430525,
-            "heading": -2.9000775312757443,
-            "angularVelocity": -0.1119355859488654,
-            "velocityX": 3.1065299029929774,
-            "velocityY": -2.077620148560901,
-            "timestamp": 1.1632565341541745
-        },
-        {
-            "x": 4.99080976041859,
-            "y": 4.118113444137051,
-            "heading": -2.9122463824645286,
-            "angularVelocity": -0.23765106003750752,
-            "velocityX": 3.2647953038995774,
-            "velocityY": -1.6826554715769175,
-            "timestamp": 1.2144612339643241
-        },
-        {
-            "x": 5.163718201552664,
-            "y": 4.0530282335458665,
-            "heading": -2.927811187303136,
-            "angularVelocity": -0.30397219193387703,
-            "velocityX": 3.3768080229971553,
-            "velocityY": -1.2710788430065951,
-            "timestamp": 1.2656659337744738
-        },
-        {
-            "x": 5.340853772484394,
-            "y": 4.009388376225937,
-            "heading": -2.9448561503180763,
-            "angularVelocity": -0.3328788778791379,
-            "velocityX": 3.459361574005724,
-            "velocityY": -0.852262731384641,
-            "timestamp": 1.3168706335846234
-        },
-        {
-            "x": 5.521188530660567,
-            "y": 3.987375039379581,
-            "heading": -2.9620889053684105,
-            "angularVelocity": -0.3365463544211233,
-            "velocityX": 3.5218399647844,
-            "velocityY": -0.42990852261559487,
-            "timestamp": 1.368075333394773
+            "heading": 0.24574486610274957,
+            "angularVelocity": -3.7145337638188186e-7,
+            "velocityX": 2.267733556325072,
+            "velocityY": -3.0278735056925665,
+            "timestamp": 0.907232490278962
+        },
+        {
+            "x": 4.2677114826543,
+            "y": 4.731769321945997,
+            "heading": 0.24574485481420902,
+            "angularVelocity": -2.2045907687430814e-7,
+            "velocityX": 1.9121061102333996,
+            "velocityY": -3.264121150398313,
+            "timestamp": 0.9584371888152268
+        },
+        {
+            "x": 4.383830202938794,
+            "y": 4.576728030606808,
+            "heading": 0.24574484074146902,
+            "angularVelocity": -2.7483298164960767e-7,
+            "velocityX": 2.2677356493419163,
+            "velocityY": -3.02787235881067,
+            "timestamp": 1.0096418873514916
+        },
+        {
+            "x": 4.516679781731222,
+            "y": 4.435758724265,
+            "heading": 0.24574482059862868,
+            "angularVelocity": -3.9337875070195686e-7,
+            "velocityX": 2.59448024478339,
+            "velocityY": -2.753054121429295,
+            "timestamp": 1.0608465858877565
+        },
+        {
+            "x": 4.664567883303712,
+            "y": 4.3106571835035465,
+            "heading": 0.24574435877996634,
+            "angularVelocity": -0.000009019068084382453,
+            "velocityX": 2.8881744410183567,
+            "velocityY": -2.4431652629075082,
+            "timestamp": 1.1120512844240213
+        },
+        {
+            "x": 4.823636899736968,
+            "y": 4.204273204304783,
+            "heading": 0.2400133838174815,
+            "angularVelocity": -0.1119228337693714,
+            "velocityX": 3.1065316461261507,
+            "velocityY": -2.0776214339670136,
+            "timestamp": 1.1632559829602862
+        },
+        {
+            "x": 4.9908096959943835,
+            "y": 4.118113363926556,
+            "heading": 0.22784637628221624,
+            "angularVelocity": -0.23761506039622932,
+            "velocityX": 3.26479407234511,
+            "velocityY": -1.6826549680241993,
+            "timestamp": 1.214460681496551
+        },
+        {
+            "x": 5.163718040144699,
+            "y": 4.053028192881653,
+            "heading": 0.21228377101249774,
+            "angularVelocity": -0.3039292431083551,
+            "velocityX": 3.3768062129661107,
+            "velocityY": -1.2710781023114057,
+            "timestamp": 1.265665380032816
+        },
+        {
+            "x": 5.340853559631665,
+            "y": 4.009388363770341,
+            "heading": 0.19524077553782296,
+            "angularVelocity": -0.332840461165967,
+            "velocityX": 3.4593606553803307,
+            "velocityY": -0.8522622016884831,
+            "timestamp": 1.3168700785690808
+        },
+        {
+            "x": 5.521188360604893,
+            "y": 3.987375039650421,
+            "heading": 0.17800934218450973,
+            "angularVelocity": -0.3365205507676074,
+            "velocityX": 3.521840888204999,
+            "velocityY": -0.42990828477058174,
+            "timestamp": 1.3680747771053456
         },
         {
             "x": 5.703982353210449,
             "y": 3.987083911895752,
-            "heading": -2.978585149802318,
-            "angularVelocity": -0.3221627017650738,
-            "velocityX": 3.569864157540669,
-            "velocityY": -0.0056855617728103595,
-            "timestamp": 1.4192800332049227
-        },
-        {
-            "x": 5.82977348163753,
-            "y": 3.977187586416487,
-            "heading": -2.987958123186903,
-            "angularVelocity": -0.27093528448013743,
-            "velocityX": 3.636119912761043,
-            "velocityY": -0.2860633065962091,
-            "timestamp": 1.4538749122092192
-        },
-        {
-            "x": 5.956799020313408,
-            "y": 3.9574239534344695,
-            "heading": -2.994346009010497,
-            "angularVelocity": -0.18464830655430406,
-            "velocityX": 3.6718017906668177,
-            "velocityY": -0.5712878192047837,
-            "timestamp": 1.4884697912135156
-        },
-        {
-            "x": 6.083502386416879,
-            "y": 3.927762346295825,
-            "heading": -2.9963234364674123,
-            "angularVelocity": -0.057159542505398835,
-            "velocityX": 3.662489066307642,
-            "velocityY": -0.8573987824891895,
-            "timestamp": 1.523064670217812
-        },
-        {
-            "x": 6.208288544878107,
-            "y": 3.888323197622386,
-            "heading": -2.9963235773653807,
-            "angularVelocity": -0.000004072798431777223,
-            "velocityX": 3.6070702385092757,
-            "velocityY": -1.1400285189186714,
-            "timestamp": 1.5576595492221086
-        },
-        {
-            "x": 6.329706848437896,
-            "y": 3.8394904257798843,
-            "heading": -2.9963235979992118,
-            "angularVelocity": -5.964417674236108e-7,
-            "velocityX": 3.5097189831104894,
-            "velocityY": -1.4115607063241151,
-            "timestamp": 1.592254428226405
-        },
-        {
-            "x": 6.447051291726402,
-            "y": 3.7815479813892945,
-            "heading": -2.996323615206392,
-            "angularVelocity": -4.973909609914855e-7,
-            "velocityX": 3.3919599277665977,
-            "velocityY": -1.6748850135707645,
-            "timestamp": 1.6268493072307015
-        },
-        {
-            "x": 6.559639598402797,
-            "y": 3.714832771082226,
-            "heading": -2.996323630138998,
-            "angularVelocity": -4.316421016363579e-7,
-            "velocityX": 3.2544789840835127,
-            "velocityY": -1.9284706935608398,
-            "timestamp": 1.661444186234998
-        },
-        {
-            "x": 6.666817153320071,
-            "y": 3.639732715012558,
-            "heading": -2.996323643537738,
-            "angularVelocity": -3.8730413434828734e-7,
-            "velocityX": 3.0980757268717096,
-            "velocityY": -2.1708431487891944,
-            "timestamp": 1.6960390652392945
-        },
-        {
-            "x": 6.767960818192567,
-            "y": 3.556684498992451,
-            "heading": -2.996323655916169,
-            "angularVelocity": -3.578110804784623e-7,
-            "velocityX": 2.9236600266743116,
-            "velocityY": -2.400592758534959,
-            "timestamp": 1.730633944243591
-        },
-        {
-            "x": 6.862482590576338,
-            "y": 3.466171063796537,
-            "heading": -2.99632366765796,
-            "angularVelocity": -3.394083586636284e-7,
-            "velocityX": 2.7322475205660415,
-            "velocityY": -2.6163824762813186,
-            "timestamp": 1.7652288232478874
-        },
-        {
-            "x": 6.949833264778536,
-            "y": 3.3687189870150447,
-            "heading": -2.9963236790740537,
-            "angularVelocity": -3.299937460810232e-7,
-            "velocityX": 2.524959667913537,
-            "velocityY": -2.8169509356974314,
-            "timestamp": 1.799823702252184
-        },
-        {
-            "x": 7.031655596531464,
-            "y": 3.2665810723307764,
-            "heading": -2.9963236904142594,
-            "angularVelocity": -3.2780012680929783e-7,
-            "velocityX": 2.365157332759168,
-            "velocityY": -2.9523998240197193,
-            "timestamp": 1.8344185812564804
-        },
-        {
-            "x": 7.1134768697956385,
-            "y": 3.164442309703858,
-            "heading": -2.9963237017545317,
-            "angularVelocity": -3.2780205084524347e-7,
-            "velocityX": 2.3651267360701933,
-            "velocityY": -2.952424334660453,
-            "timestamp": 1.8690134602607769
-        },
-        {
-            "x": 7.194171018365882,
-            "y": 3.0635712609157633,
-            "heading": -3.000602693910309,
-            "angularVelocity": -0.1236886001319993,
-            "velocityX": 2.3325460557391247,
-            "velocityY": -2.9157797827698024,
-            "timestamp": 1.9036083392650733
-        },
-        {
-            "x": 7.268926895683992,
-            "y": 2.9702293971794824,
-            "heading": -3.0147524025112875,
-            "angularVelocity": -0.4090116516731078,
-            "velocityX": 2.1608943135435097,
-            "velocityY": -2.698141066621111,
-            "timestamp": 1.9382032182693698
-        },
-        {
-            "x": 7.337464009959094,
-            "y": 2.8846562411973626,
-            "heading": -3.031563935832344,
-            "angularVelocity": -0.4859543899248224,
-            "velocityX": 1.9811346721747336,
-            "velocityY": -2.473578704278531,
-            "timestamp": 1.9727980972736663
-        },
-        {
-            "x": 7.399764505912731,
-            "y": 2.806871719273257,
-            "heading": -3.0489337906921676,
-            "angularVelocity": -0.5020932392238406,
-            "velocityX": 1.8008589059062745,
-            "velocityY": -2.2484403519505154,
-            "timestamp": 2.0073929762779628
-        },
-        {
-            "x": 7.455827645980218,
-            "y": 2.7368758607336336,
-            "heading": -3.0658680766465114,
-            "angularVelocity": -0.48950267905953376,
-            "velocityX": 1.6205618195839024,
-            "velocityY": -2.023301151911255,
-            "timestamp": 2.041987855282259
-        },
-        {
-            "x": 7.505655239118239,
-            "y": 2.674665896616921,
-            "heading": -3.081789323798935,
-            "angularVelocity": -0.4602197669327373,
-            "velocityX": 1.4403170229857634,
-            "velocityY": -1.7982419915094008,
-            "timestamp": 2.0765827342865553
-        },
-        {
-            "x": 7.549249359478674,
-            "y": 2.6202389173200062,
-            "heading": -3.0963205890594514,
-            "angularVelocity": -0.42004093318875263,
-            "velocityX": 1.2601321818475304,
-            "velocityY": -1.5732669361310743,
-            "timestamp": 2.1111776132908515
-        },
-        {
-            "x": 7.586611891972334,
-            "y": 2.573592354245129,
-            "heading": -3.1091966615790723,
-            "angularVelocity": -0.37219591136657937,
-            "velocityX": 1.0800018259644524,
-            "velocityY": -1.348366128671367,
-            "timestamp": 2.1457724922951478
-        },
-        {
-            "x": 7.617744461907291,
-            "y": 2.5347240241708393,
-            "heading": -3.1202208856007014,
-            "angularVelocity": -0.318666355799661,
-            "velocityX": 0.8999184512566408,
-            "velocityY": -1.1235284294378556,
-            "timestamp": 2.180367371299444
-        },
-        {
-            "x": 7.642648454713618,
-            "y": 2.5036320816555615,
-            "heading": -3.1292416557703198,
-            "angularVelocity": -0.26075449399601563,
-            "velocityX": 0.7198751238076218,
-            "velocityY": -0.8987440745613371,
-            "timestamp": 2.2149622503037403
-        },
-        {
-            "x": 7.66132505338278,
-            "y": 2.48031495979902,
-            "heading": -3.136138533441162,
-            "angularVelocity": -0.19936123118063845,
-            "velocityX": 0.5398659919244735,
-            "velocityY": -0.6740050125235452,
-            "timestamp": 2.2495571293080365
-        },
-        {
-            "x": 7.673775274710525,
-            "y": 2.4647713180548445,
-            "heading": -3.1408135216098865,
-            "angularVelocity": -0.1351352657757358,
-            "velocityX": 0.3598862515518098,
-            "velocityY": -0.4493047003357149,
-            "timestamp": 2.284152008312333
+            "heading": 0.16151347881375258,
+            "angularVelocity": -0.32215526782320836,
+            "velocityX": 3.5698675674478513,
+            "velocityY": -0.005685567203615865,
+            "timestamp": 1.4192794756416105
+        },
+        {
+            "x": 5.829773721796989,
+            "y": 3.97718762682745,
+            "heading": 0.15213932045666498,
+            "angularVelocity": -0.2709697131085786,
+            "velocityX": 3.6361292138465493,
+            "velocityY": -0.2860623240667741,
+            "timestamp": 1.453874332201581
+        },
+        {
+            "x": 5.956799770812804,
+            "y": 3.9574240312185514,
+            "heading": 0.1457481223879076,
+            "angularVelocity": -0.18474417021149397,
+            "velocityX": 3.671818924747189,
+            "velocityY": -0.5712871095342833,
+            "timestamp": 1.4884691887615515
+        },
+        {
+            "x": 6.083503083107889,
+            "y": 3.927762559716917,
+            "heading": 0.14376941436575622,
+            "angularVelocity": -0.05719659564771788,
+            "velocityX": 3.6624898870571543,
+            "velocityY": -0.8573954180216341,
+            "timestamp": 1.523064045321522
+        },
+        {
+            "x": 6.208289204844049,
+            "y": 3.888323576656908,
+            "heading": 0.14376927335150447,
+            "angularVelocity": -0.000004076162347950456,
+            "velocityX": 3.607071517115297,
+            "velocityY": -1.1400244713152792,
+            "timestamp": 1.5576589018814926
+        },
+        {
+            "x": 6.329707486841884,
+            "y": 3.8394909787467797,
+            "heading": 0.14376925272536353,
+            "angularVelocity": -5.962198716671582e-7,
+            "velocityX": 3.5097206368627396,
+            "velocityY": -1.4115565944167625,
+            "timestamp": 1.592253758441463
+        },
+        {
+            "x": 6.447051924752506,
+            "y": 3.781548715234953,
+            "heading": 0.14376923552481305,
+            "angularVelocity": -4.971996473322837e-7,
+            "velocityX": 3.3919619729367447,
+            "velocityY": -1.6748808717094477,
+            "timestamp": 1.6268486150014336
+        },
+        {
+            "x": 6.559640243151509,
+            "y": 3.714833691264027,
+            "heading": 0.14376922059812475,
+            "angularVelocity": -4.3147131718664413e-7,
+            "velocityX": 3.254481434366676,
+            "velocityY": -1.9284665584687428,
+            "timestamp": 1.6614434715614042
+        },
+        {
+            "x": 6.666817827672777,
+            "y": 3.6397338254007683,
+            "heading": 0.14376920720482705,
+            "angularVelocity": -3.8714707984086e-7,
+            "velocityX": 3.0980785925640646,
+            "velocityY": -2.1708390590685647,
+            "timestamp": 1.6960383281213747
+        },
+        {
+            "x": 6.7679615406661,
+            "y": 3.5566858017838987,
+            "heading": 0.14376919483153264,
+            "angularVelocity": -3.5766283306602865e-7,
+            "velocityX": 2.923663314457974,
+            "velocityY": -2.4005887543688678,
+            "timestamp": 1.7306331846813452
+        },
+        {
+            "x": 6.862483380166932,
+            "y": 3.466172559439492,
+            "heading": 0.1437691830947059,
+            "angularVelocity": -3.392650779789794e-7,
+            "velocityX": 2.7322512332715396,
+            "velocityY": -2.6163785991568234,
+            "timestamp": 1.7652280412413157
+        },
+        {
+            "x": 6.949834140776404,
+            "y": 3.3687206741301803,
+            "heading": 0.14376917168351727,
+            "angularVelocity": -3.2985217383486685e-7,
+            "velocityX": 2.5249638037391158,
+            "velocityY": -2.8169472285678507,
+            "timestamp": 1.7998228978012862
+        },
+        {
+            "x": 7.031656177612435,
+            "y": 3.2665826319797073,
+            "heading": 0.14376916034823112,
+            "angularVelocity": -3.2765813438231676e-7,
+            "velocityX": 2.3651503423404003,
+            "velocityY": -2.952405424009072,
+            "timestamp": 1.8344177543612568
+        },
+        {
+            "x": 7.113477155727132,
+            "y": 3.1644437417043827,
+            "heading": 0.14376914901287846,
+            "angularVelocity": -3.2766005667768915e-7,
+            "velocityX": 2.3651197389085663,
+            "velocityY": -2.952429939932413,
+            "timestamp": 1.8690126109212273
+        },
+        {
+            "x": 7.194171820782979,
+            "y": 3.0635723954548135,
+            "heading": 0.13949339206360356,
+            "angularVelocity": -0.12359516339843184,
+            "velocityX": 2.332562498588806,
+            "velocityY": -2.91579027288948,
+            "timestamp": 1.9036074674811978
+        },
+        {
+            "x": 7.268927567281138,
+            "y": 2.9702304150182344,
+            "heading": 0.12535509322542532,
+            "angularVelocity": -0.4086821060717307,
+            "velocityX": 2.160891933995128,
+            "velocityY": -2.6981461904537576,
+            "timestamp": 1.9382023240411683
+        },
+        {
+            "x": 7.337464553197734,
+            "y": 2.884657135542947,
+            "heading": 0.10855528307456563,
+            "angularVelocity": -0.48561583487814164,
+            "velocityX": 1.9811322471531543,
+            "velocityY": -2.473583878775318,
+            "timestamp": 1.9727971806011388
+        },
+        {
+            "x": 7.399764936518732,
+            "y": 2.806872483431395,
+            "heading": 0.0911966728700688,
+            "angularVelocity": -0.5017685266133554,
+            "velocityX": 1.8008568184984473,
+            "velocityY": -2.2484455738878646,
+            "timestamp": 2.0073920371611096
+        },
+        {
+            "x": 7.455827979936063,
+            "y": 2.7368764943083312,
+            "heading": 0.07427285825902008,
+            "angularVelocity": -0.4892003116622579,
+            "velocityX": 1.6205600771936914,
+            "velocityY": -2.023306239230242,
+            "timestamp": 2.0419868937210803
+        },
+        {
+            "x": 7.505655491239628,
+            "y": 2.674666404415114,
+            "heading": 0.05836114831277334,
+            "angularVelocity": -0.4599443827339935,
+            "velocityX": 1.4403155919200792,
+            "velocityY": -1.7982467938658737,
+            "timestamp": 2.076581750281051
+        },
+        {
+            "x": 7.549249543273319,
+            "y": 2.6202393080696202,
+            "heading": 0.04383838388032337,
+            "angularVelocity": -0.4197954805008245,
+            "velocityX": 1.2601310243365538,
+            "velocityY": -1.5732713402393688,
+            "timestamp": 2.111176606841022
+        },
+        {
+            "x": 7.586612019746033,
+            "y": 2.5735926396425732,
+            "heading": 0.030969705386613037,
+            "angularVelocity": -0.3719824208954968,
+            "velocityX": 1.0800009073009074,
+            "velocityY": -1.348370048772548,
+            "timestamp": 2.1457714634009926
+        },
+        {
+            "x": 7.6177445449139505,
+            "y": 2.5347242182058114,
+            "heading": 0.01995171749042388,
+            "angularVelocity": -0.31848630090688007,
+            "velocityX": 0.8999177410650449,
+            "velocityY": -1.1235317992830327,
+            "timestamp": 2.1803663199609633
+        },
+        {
+            "x": 7.642648503297853,
+            "y": 2.503632200129417,
+            "heading": 0.010935986949086615,
+            "angularVelocity": -0.26060898751548167,
+            "velocityX": 0.7198745958298824,
+            "velocityY": -0.8987468418172337,
+            "timestamp": 2.214961176520934
+        },
+        {
+            "x": 7.661325077102299,
+            "y": 2.4803150199755253,
+            "heading": 0.004042922192056018,
+            "angularVelocity": -0.19925114431624774,
+            "velocityX": 0.5398656234365572,
+            "velocityY": -0.6740071349471043,
+            "timestamp": 2.249556033080905
+        },
+        {
+            "x": 7.673775282436928,
+            "y": 2.464771338402636,
+            "heading": -0.0006295042844945549,
+            "angularVelocity": -0.13506130509461317,
+            "velocityX": 0.3598860227400032,
+            "velocityY": -0.44930614312403183,
+            "timestamp": 2.2841508896408755
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.06855886298677888,
-            "velocityX": 0.17993198613883732,
-            "velocityY": -0.22463781572640673,
-            "timestamp": 2.318746887316629
+            "heading": -0.003,
+            "angularVelocity": -0.06852162290068065,
+            "velocityX": 0.17993187953478723,
+            "velocityY": -0.22463854964001964,
+            "timestamp": 2.3187457462008463
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": -3.143185307179586,
-            "angularVelocity": -2.3105673781771525e-33,
+            "heading": -0.003,
+            "angularVelocity": -8.413636330536319e-34,
             "velocityX": 0,
-            "velocityY": -9.473748472540581e-34,
-            "timestamp": 2.3533417663209253
+            "velocityY": 4.110332962762654e-35,
+            "timestamp": 2.353340602760817
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.traj
index 5ed5f209..1a6d423b 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.traj	
@@ -3,578 +3,578 @@
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": -2.75,
+            "heading": 0.39,
             "angularVelocity": 0,
-            "velocityX": -2.2987344072087424e-31,
-            "velocityY": -2.682409549792184e-31,
+            "velocityX": 1.0547928981710615e-31,
+            "velocityY": 1.4824740100837611e-32,
             "timestamp": 0
         },
         {
-            "x": 2.30727109352441,
-            "y": 6.7499479361039905,
-            "heading": -2.752438811293409,
-            "angularVelocity": -0.06989284029038495,
-            "velocityX": 0.20837913118223148,
-            "velocityY": -0.20210205559294603,
-            "timestamp": 0.03489357827320871
-        },
-        {
-            "x": 2.321814327969628,
-            "y": 6.735842827712052,
-            "heading": -2.7572390739653767,
-            "angularVelocity": -0.1375686561688486,
-            "velocityX": 0.41678827924577844,
-            "velocityY": -0.4042322137758234,
-            "timestamp": 0.06978715654641741
-        },
-        {
-            "x": 2.3436309003249467,
-            "y": 6.714683550648929,
-            "heading": -2.7643090195026687,
-            "angularVelocity": -0.202614517832932,
-            "velocityX": 0.6252317313088365,
-            "velocityY": -0.6063945892120965,
-            "timestamp": 0.10468073481962611
-        },
-        {
-            "x": 2.3727221871027773,
-            "y": 6.6864688072553555,
-            "heading": -2.7735381226601596,
-            "angularVelocity": -0.26449288419860933,
-            "velocityX": 0.8337146322470192,
-            "velocityY": -0.8085941537052478,
-            "timestamp": 0.13957431309283483
-        },
-        {
-            "x": 2.409089780647816,
-            "y": 6.651197089471134,
-            "heading": -2.7847907497118745,
-            "angularVelocity": -0.32248418214977886,
-            "velocityX": 1.042243167504621,
-            "velocityY": -1.0108369370447357,
-            "timestamp": 0.17446789136604354
-        },
-        {
-            "x": 2.452735530672034,
-            "y": 6.6088666355169075,
-            "heading": -2.7978965041034827,
-            "angularVelocity": -0.3755921587918882,
-            "velocityX": 1.2508247128592573,
-            "velocityY": -1.213130210458454,
-            "timestamp": 0.20936146963925226
-        },
-        {
-            "x": 2.5036615834922005,
-            "y": 6.559475386735866,
-            "heading": -2.8126348139869735,
-            "angularVelocity": -0.42237886203854363,
-            "velocityX": 1.459467768579872,
-            "velocityY": -1.4154824820291676,
-            "timestamp": 0.24425504791246097
-        },
-        {
-            "x": 2.5618703896153354,
-            "y": 6.503020972116864,
-            "heading": -2.8287087438800844,
-            "angularVelocity": -0.4606558194535241,
-            "velocityX": 1.6681810523235565,
-            "velocityY": -1.6179027033850466,
-            "timestamp": 0.27914862618566966
-        },
-        {
-            "x": 2.6273645712551357,
-            "y": 6.439500824983286,
-            "heading": -2.8456966886741912,
-            "angularVelocity": -0.48685017802115704,
-            "velocityX": 1.8769694849578602,
-            "velocityY": -1.8203964820182346,
-            "timestamp": 0.31404220445887837
-        },
-        {
-            "x": 2.700146215046118,
-            "y": 6.368912855338095,
-            "heading": -2.8629525699896186,
-            "angularVelocity": -0.4945288551468594,
-            "velocityX": 2.085817717550196,
-            "velocityY": -2.0229501569744945,
-            "timestamp": 0.3489357827320871
-        },
-        {
-            "x": 2.780213462242076,
-            "y": 6.2912587361998735,
-            "heading": -2.8793619429160175,
-            "angularVelocity": -0.47026913657055547,
-            "velocityX": 2.294612681137229,
-            "velocityY": -2.225455885613285,
-            "timestamp": 0.3838293610052958
-        },
-        {
-            "x": 2.8675389350998404,
-            "y": 6.20656386499432,
-            "heading": -2.892555231685043,
-            "angularVelocity": -0.37810076873529364,
-            "velocityX": 2.502623037798732,
-            "velocityY": -2.42723376039027,
-            "timestamp": 0.4187229392785045
-        },
-        {
-            "x": 2.9617036478615395,
-            "y": 6.11507787860357,
-            "heading": -2.8943452360571387,
-            "angularVelocity": -0.05129896275126653,
-            "velocityX": 2.698625862455626,
-            "velocityY": -2.621857399502999,
-            "timestamp": 0.4536165175517132
-        },
-        {
-            "x": 3.05646121242224,
-            "y": 6.023180735189561,
-            "heading": -2.894345248240927,
-            "angularVelocity": -3.4916993473204957e-7,
-            "velocityX": 2.7156161462940758,
-            "velocityY": -2.633640571181149,
-            "timestamp": 0.48851009582492194
-        },
-        {
-            "x": 3.151218783278769,
-            "y": 5.931283598267322,
-            "heading": -2.894345260424686,
-            "angularVelocity": -3.4916908908792776e-7,
-            "velocityX": 2.7156163267235134,
-            "velocityY": -2.633640385136315,
-            "timestamp": 0.5234036740981306
-        },
-        {
-            "x": 3.245976354135778,
-            "y": 5.839386461345578,
-            "heading": -2.894345272608445,
-            "angularVelocity": -3.491690897148369e-7,
-            "velocityX": 2.715616326737254,
-            "velocityY": -2.633640385122147,
-            "timestamp": 0.5582972523713393
-        },
-        {
-            "x": 3.340733924992786,
-            "y": 5.747489324423833,
-            "heading": -2.894345284792204,
-            "angularVelocity": -3.491690888486973e-7,
-            "velocityX": 2.715616326737255,
-            "velocityY": -2.633640385122146,
-            "timestamp": 0.593190830644548
-        },
-        {
-            "x": 3.435491495849795,
-            "y": 5.655592187502088,
-            "heading": -2.894345296975963,
-            "angularVelocity": -3.4916908897748007e-7,
-            "velocityX": 2.715616326737255,
-            "velocityY": -2.633640385122146,
-            "timestamp": 0.6280844089177567
-        },
-        {
-            "x": 3.530249066706803,
-            "y": 5.5636950505803435,
-            "heading": -2.8943453091597218,
-            "angularVelocity": -3.491690889850577e-7,
-            "velocityX": 2.715616326737255,
-            "velocityY": -2.633640385122146,
-            "timestamp": 0.6629779871909655
-        },
-        {
-            "x": 3.625006637563812,
-            "y": 5.471797913658599,
-            "heading": -2.8943453213434807,
-            "angularVelocity": -3.49169089086754e-7,
-            "velocityX": 2.715616326737255,
-            "velocityY": -2.633640385122146,
-            "timestamp": 0.6978715654641742
-        },
-        {
-            "x": 3.7197642084208202,
-            "y": 5.379900776736854,
-            "heading": -2.8943453335272395,
-            "angularVelocity": -3.4916908901998635e-7,
-            "velocityX": 2.71561632673725,
-            "velocityY": -2.6336403851221513,
-            "timestamp": 0.7327651437373829
-        },
-        {
-            "x": 3.814521779275631,
-            "y": 5.288003639812843,
-            "heading": -2.8943453457109984,
-            "angularVelocity": -3.4916908849611257e-7,
-            "velocityX": 2.715616326674264,
-            "velocityY": -2.633640385187098,
-            "timestamp": 0.7676587220105916
-        },
-        {
-            "x": 3.9092793212702794,
-            "y": 5.196106473130367,
-            "heading": -2.8943453578947573,
-            "angularVelocity": -3.4916909136087424e-7,
-            "velocityX": 2.715615499583321,
-            "velocityY": -2.6336412380221117,
-            "timestamp": 0.8025523002838003
-        },
-        {
-            "x": 4.0036585134090545,
-            "y": 5.103820780001642,
-            "heading": -2.8943453700832222,
-            "angularVelocity": -3.49303962122221e-7,
-            "velocityX": 2.704772534355369,
-            "velocityY": -2.644775849761548,
-            "timestamp": 0.837445878557009
-        },
-        {
-            "x": 4.09067331484616,
-            "y": 5.004560993237775,
-            "heading": -2.894345382446594,
-            "angularVelocity": -3.5431651835190834e-7,
-            "velocityX": 2.4937196396368213,
-            "velocityY": -2.8446433892988883,
-            "timestamp": 0.8723394568302177
+            "x": 2.3072711000078927,
+            "y": 6.749947927475375,
+            "heading": 0.3875624240531776,
+            "angularVelocity": -0.06985747897751753,
+            "velocityX": 0.2083794421285326,
+            "velocityY": -0.20210242424612682,
+            "timestamp": 0.034893557318421635
+        },
+        {
+            "x": 2.321814346754588,
+            "y": 6.735842802544083,
+            "heading": 0.3827646159677583,
+            "angularVelocity": -0.1374983938048157,
+            "velocityX": 0.41678888208448434,
+            "velocityY": -0.40423293052572296,
+            "timestamp": 0.06978711463684327
+        },
+        {
+            "x": 2.343630936460155,
+            "y": 6.714683501867812,
+            "heading": 0.3756983254810237,
+            "angularVelocity": -0.20250989093060998,
+            "velocityX": 0.6252326040156703,
+            "velocityY": -0.6063956300924075,
+            "timestamp": 0.1046806719552649
+        },
+        {
+            "x": 2.372722244735657,
+            "y": 6.6864687287677045,
+            "heading": 0.36647405587954873,
+            "angularVelocity": -0.2643545201567945,
+            "velocityX": 0.8337157490143364,
+            "velocityY": -0.8085954906412398,
+            "timestamp": 0.13957422927368654
+        },
+        {
+            "x": 2.4090898628559576,
+            "y": 6.651196976344128,
+            "heading": 0.3552274150516352,
+            "angularVelocity": -0.3223128191053197,
+            "velocityX": 1.042244497699901,
+            "velocityY": -1.0108385368022033,
+            "timestamp": 0.17446778659210818
+        },
+        {
+            "x": 2.452735639247045,
+            "y": 6.608866484215248,
+            "heading": 0.3421287688368095,
+            "angularVelocity": -0.3753886740550377,
+            "velocityX": 1.2508262196599174,
+            "velocityY": -1.213132033016619,
+            "timestamp": 0.20936134391052982
+        },
+        {
+            "x": 2.5036617186589587,
+            "y": 6.55947519544937,
+            "heading": 0.3273986519293141,
+            "angularVelocity": -0.42214431658760004,
+            "velocityX": 1.459469407122573,
+            "velocityY": -1.4154844779841917,
+            "timestamp": 0.24425490122895147
+        },
+        {
+            "x": 2.5618705496598224,
+            "y": 6.503020741236739,
+            "heading": 0.31133395345849213,
+            "angularVelocity": -0.4603915365872077,
+            "velocityX": 1.6681827670844436,
+            "velocityY": -1.6179048096889317,
+            "timestamp": 0.2791484585473731
+        },
+        {
+            "x": 2.6273647520175327,
+            "y": 6.43950055782557,
+            "heading": 0.294356217256913,
+            "angularVelocity": -0.486557906568497,
+            "velocityX": 1.8769712058889019,
+            "velocityY": -1.8203986148936577,
+            "timestamp": 0.3140420158657947
+        },
+        {
+            "x": 2.7001464092257668,
+            "y": 6.368912559254549,
+            "heading": 0.2771114307132975,
+            "angularVelocity": -0.4942111916606252,
+            "velocityX": 2.0858193546752704,
+            "velocityY": -2.022952200799379,
+            "timestamp": 0.34893557318421636
+        },
+        {
+            "x": 2.7802136587601143,
+            "y": 6.291258423996795,
+            "heading": 0.2607138645949654,
+            "angularVelocity": -0.46993105256354084,
+            "velocityX": 2.294614126146364,
+            "velocityY": -2.225457684039485,
+            "timestamp": 0.383829130502638
+        },
+        {
+            "x": 2.867539127268515,
+            "y": 6.206563552711186,
+            "heading": 0.24753249327015822,
+            "angularVelocity": -0.37775945870237265,
+            "velocityX": 2.5026244160637545,
+            "velocityY": -2.427235220322282,
+            "timestamp": 0.41872268782105965
+        },
+        {
+            "x": 2.961704724093414,
+            "y": 6.1150775036748035,
+            "heading": 0.24574502539338514,
+            "angularVelocity": -0.05122630118968667,
+            "velocityX": 2.6986528190746,
+            "velocityY": -2.6218607693541762,
+            "timestamp": 0.4536162451394813
+        },
+        {
+            "x": 3.0564622015652247,
+            "y": 6.023180384324785,
+            "heading": 0.2457450132145974,
+            "angularVelocity": -3.4902683097432965e-7,
+            "velocityX": 2.7156152812710346,
+            "velocityY": -2.633641463133395,
+            "timestamp": 0.48850980245790293
+        },
+        {
+            "x": 3.1512196852998877,
+            "y": 5.931283271432529,
+            "heading": 0.24574500103583913,
+            "angularVelocity": -3.490259866463667e-7,
+            "velocityX": 2.7156154607555005,
+            "velocityY": -2.6336412780630685,
+            "timestamp": 0.5234033597763246
+        },
+        {
+            "x": 3.2459771690350276,
+            "y": 5.839386158540766,
+            "heading": 0.2457449888570809,
+            "angularVelocity": -3.4902598588384144e-7,
+            "velocityX": 2.7156154607691696,
+            "velocityY": -2.6336412780489744,
+            "timestamp": 0.5582969170947462
+        },
+        {
+            "x": 3.3407346527701676,
+            "y": 5.747489045649002,
+            "heading": 0.24574497667832262,
+            "angularVelocity": -3.4902598639891e-7,
+            "velocityX": 2.7156154607691705,
+            "velocityY": -2.6336412780489735,
+            "timestamp": 0.5931904744131677
+        },
+        {
+            "x": 3.4354921365053075,
+            "y": 5.655591932757238,
+            "heading": 0.24574496449956437,
+            "angularVelocity": -3.4902598640760607e-7,
+            "velocityX": 2.71561546076917,
+            "velocityY": -2.6336412780489735,
+            "timestamp": 0.6280840317315893
+        },
+        {
+            "x": 3.5302496202404474,
+            "y": 5.563694819865473,
+            "heading": 0.2457449523208061,
+            "angularVelocity": -3.490259865548224e-7,
+            "velocityX": 2.7156154607691705,
+            "velocityY": -2.6336412780489735,
+            "timestamp": 0.6629775890500109
+        },
+        {
+            "x": 3.6250071039755873,
+            "y": 5.471797706973709,
+            "heading": 0.2457449401420478,
+            "angularVelocity": -3.4902598644057735e-7,
+            "velocityX": 2.71561546076917,
+            "velocityY": -2.633641278048974,
+            "timestamp": 0.6978711463684325
+        },
+        {
+            "x": 3.719764587710727,
+            "y": 5.379900594081945,
+            "heading": 0.24574492796328956,
+            "angularVelocity": -3.4902598592532335e-7,
+            "velocityX": 2.715615460769166,
+            "velocityY": -2.633641278048979,
+            "timestamp": 0.7327647036868541
+        },
+        {
+            "x": 3.8145220714436685,
+            "y": 5.288003481187913,
+            "heading": 0.24574491578453134,
+            "angularVelocity": -3.490259859254125e-7,
+            "velocityX": 2.7156154607061804,
+            "velocityY": -2.6336412781139247,
+            "timestamp": 0.7676582610052757
+        },
+        {
+            "x": 3.909279526317127,
+            "y": 5.196106338536137,
+            "heading": 0.245744903605773,
+            "angularVelocity": -3.4902598866436484e-7,
+            "velocityX": 2.7156146336342,
+            "velocityY": -2.6336421309288234,
+            "timestamp": 0.8025518183236973
+        },
+        {
+            "x": 4.003658642045134,
+            "y": 5.103820680647278,
+            "heading": 0.24574489142231412,
+            "angularVelocity": -3.4916069991394096e-7,
+            "velocityX": 2.7047719688402356,
+            "velocityY": -2.644776428116158,
+            "timestamp": 0.8374453756421188
+        },
+        {
+            "x": 4.090673374879912,
+            "y": 5.004560939161802,
+            "heading": 0.24574487906407927,
+            "angularVelocity": -3.541695320845138e-7,
+            "velocityX": 2.493719171155964,
+            "velocityY": -2.8446437999909007,
+            "timestamp": 0.8723389329605404
         },
         {
             "x": 4.169802665710449,
             "y": 4.898907661437988,
-            "heading": -2.8943453954133593,
-            "angularVelocity": -3.71608932660535e-7,
-            "velocityX": 2.2677339149549134,
-            "velocityY": -3.0278732370909647,
-            "timestamp": 0.9072330351034265
-        },
-        {
-            "x": 4.267711504415231,
-            "y": 4.731769329108682,
-            "heading": -2.8943454067066363,
-            "angularVelocity": -2.2055157567695873e-7,
-            "velocityX": 1.9121064876426594,
-            "velocityY": -3.2641209293092457,
-            "timestamp": 0.9584377349135761
-        },
-        {
-            "x": 4.3838302459464416,
-            "y": 4.576728047661677,
-            "heading": -2.8943454207852617,
-            "angularVelocity": -2.749479155903796e-7,
-            "velocityX": 2.2677360078614184,
-            "velocityY": -3.027872090293428,
-            "timestamp": 1.0096424347237256
-        },
-        {
-            "x": 4.5166798451311,
-            "y": 4.43575875391577,
-            "heading": -2.8943454409364353,
-            "angularVelocity": -3.9354148044055934e-7,
-            "velocityX": 2.594480578486377,
-            "velocityY": -2.7530538069469372,
-            "timestamp": 1.0608471345338752
-        },
-        {
-            "x": 4.664567965806816,
-            "y": 4.3106572284696325,
-            "heading": -2.8943459031991594,
-            "angularVelocity": -0.000009027740145972362,
-            "velocityX": 2.888174742241181,
-            "velocityY": -2.443164903025968,
-            "timestamp": 1.1120518343440249
-        },
-        {
-            "x": 4.823636896940825,
-            "y": 4.2042733124430525,
-            "heading": -2.9000775312757443,
-            "angularVelocity": -0.1119355859488654,
-            "velocityX": 3.1065299029929774,
-            "velocityY": -2.077620148560901,
-            "timestamp": 1.1632565341541745
-        },
-        {
-            "x": 4.99080976041859,
-            "y": 4.118113444137051,
-            "heading": -2.9122463824645286,
-            "angularVelocity": -0.23765106003750752,
-            "velocityX": 3.2647953038995774,
-            "velocityY": -1.6826554715769175,
-            "timestamp": 1.2144612339643241
-        },
-        {
-            "x": 5.163718201552664,
-            "y": 4.0530282335458665,
-            "heading": -2.927811187303136,
-            "angularVelocity": -0.30397219193387703,
-            "velocityX": 3.3768080229971553,
-            "velocityY": -1.2710788430065951,
-            "timestamp": 1.2656659337744738
-        },
-        {
-            "x": 5.340853772484394,
-            "y": 4.009388376225937,
-            "heading": -2.9448561503180763,
-            "angularVelocity": -0.3328788778791379,
-            "velocityX": 3.459361574005724,
-            "velocityY": -0.852262731384641,
-            "timestamp": 1.3168706335846234
-        },
-        {
-            "x": 5.521188530660567,
-            "y": 3.987375039379581,
-            "heading": -2.9620889053684105,
-            "angularVelocity": -0.3365463544211233,
-            "velocityX": 3.5218399647844,
-            "velocityY": -0.42990852261559487,
-            "timestamp": 1.368075333394773
+            "heading": 0.24574486610274957,
+            "angularVelocity": -3.7145337638188186e-7,
+            "velocityX": 2.267733556325072,
+            "velocityY": -3.0278735056925665,
+            "timestamp": 0.907232490278962
+        },
+        {
+            "x": 4.2677114826543,
+            "y": 4.731769321945997,
+            "heading": 0.24574485481420902,
+            "angularVelocity": -2.2045907687430814e-7,
+            "velocityX": 1.9121061102333996,
+            "velocityY": -3.264121150398313,
+            "timestamp": 0.9584371888152268
+        },
+        {
+            "x": 4.383830202938794,
+            "y": 4.576728030606808,
+            "heading": 0.24574484074146902,
+            "angularVelocity": -2.7483298164960767e-7,
+            "velocityX": 2.2677356493419163,
+            "velocityY": -3.02787235881067,
+            "timestamp": 1.0096418873514916
+        },
+        {
+            "x": 4.516679781731222,
+            "y": 4.435758724265,
+            "heading": 0.24574482059862868,
+            "angularVelocity": -3.9337875070195686e-7,
+            "velocityX": 2.59448024478339,
+            "velocityY": -2.753054121429295,
+            "timestamp": 1.0608465858877565
+        },
+        {
+            "x": 4.664567883303712,
+            "y": 4.3106571835035465,
+            "heading": 0.24574435877996634,
+            "angularVelocity": -0.000009019068084382453,
+            "velocityX": 2.8881744410183567,
+            "velocityY": -2.4431652629075082,
+            "timestamp": 1.1120512844240213
+        },
+        {
+            "x": 4.823636899736968,
+            "y": 4.204273204304783,
+            "heading": 0.2400133838174815,
+            "angularVelocity": -0.1119228337693714,
+            "velocityX": 3.1065316461261507,
+            "velocityY": -2.0776214339670136,
+            "timestamp": 1.1632559829602862
+        },
+        {
+            "x": 4.9908096959943835,
+            "y": 4.118113363926556,
+            "heading": 0.22784637628221624,
+            "angularVelocity": -0.23761506039622932,
+            "velocityX": 3.26479407234511,
+            "velocityY": -1.6826549680241993,
+            "timestamp": 1.214460681496551
+        },
+        {
+            "x": 5.163718040144699,
+            "y": 4.053028192881653,
+            "heading": 0.21228377101249774,
+            "angularVelocity": -0.3039292431083551,
+            "velocityX": 3.3768062129661107,
+            "velocityY": -1.2710781023114057,
+            "timestamp": 1.265665380032816
+        },
+        {
+            "x": 5.340853559631665,
+            "y": 4.009388363770341,
+            "heading": 0.19524077553782296,
+            "angularVelocity": -0.332840461165967,
+            "velocityX": 3.4593606553803307,
+            "velocityY": -0.8522622016884831,
+            "timestamp": 1.3168700785690808
+        },
+        {
+            "x": 5.521188360604893,
+            "y": 3.987375039650421,
+            "heading": 0.17800934218450973,
+            "angularVelocity": -0.3365205507676074,
+            "velocityX": 3.521840888204999,
+            "velocityY": -0.42990828477058174,
+            "timestamp": 1.3680747771053456
         },
         {
             "x": 5.703982353210449,
             "y": 3.987083911895752,
-            "heading": -2.978585149802318,
-            "angularVelocity": -0.3221627017650738,
-            "velocityX": 3.569864157540669,
-            "velocityY": -0.0056855617728103595,
-            "timestamp": 1.4192800332049227
-        },
-        {
-            "x": 5.82977348163753,
-            "y": 3.977187586416487,
-            "heading": -2.987958123186903,
-            "angularVelocity": -0.27093528448013743,
-            "velocityX": 3.636119912761043,
-            "velocityY": -0.2860633065962091,
-            "timestamp": 1.4538749122092192
-        },
-        {
-            "x": 5.956799020313408,
-            "y": 3.9574239534344695,
-            "heading": -2.994346009010497,
-            "angularVelocity": -0.18464830655430406,
-            "velocityX": 3.6718017906668177,
-            "velocityY": -0.5712878192047837,
-            "timestamp": 1.4884697912135156
-        },
-        {
-            "x": 6.083502386416879,
-            "y": 3.927762346295825,
-            "heading": -2.9963234364674123,
-            "angularVelocity": -0.057159542505398835,
-            "velocityX": 3.662489066307642,
-            "velocityY": -0.8573987824891895,
-            "timestamp": 1.523064670217812
-        },
-        {
-            "x": 6.208288544878107,
-            "y": 3.888323197622386,
-            "heading": -2.9963235773653807,
-            "angularVelocity": -0.000004072798431777223,
-            "velocityX": 3.6070702385092757,
-            "velocityY": -1.1400285189186714,
-            "timestamp": 1.5576595492221086
-        },
-        {
-            "x": 6.329706848437896,
-            "y": 3.8394904257798843,
-            "heading": -2.9963235979992118,
-            "angularVelocity": -5.964417674236108e-7,
-            "velocityX": 3.5097189831104894,
-            "velocityY": -1.4115607063241151,
-            "timestamp": 1.592254428226405
-        },
-        {
-            "x": 6.447051291726402,
-            "y": 3.7815479813892945,
-            "heading": -2.996323615206392,
-            "angularVelocity": -4.973909609914855e-7,
-            "velocityX": 3.3919599277665977,
-            "velocityY": -1.6748850135707645,
-            "timestamp": 1.6268493072307015
-        },
-        {
-            "x": 6.559639598402797,
-            "y": 3.714832771082226,
-            "heading": -2.996323630138998,
-            "angularVelocity": -4.316421016363579e-7,
-            "velocityX": 3.2544789840835127,
-            "velocityY": -1.9284706935608398,
-            "timestamp": 1.661444186234998
-        },
-        {
-            "x": 6.666817153320071,
-            "y": 3.639732715012558,
-            "heading": -2.996323643537738,
-            "angularVelocity": -3.8730413434828734e-7,
-            "velocityX": 3.0980757268717096,
-            "velocityY": -2.1708431487891944,
-            "timestamp": 1.6960390652392945
-        },
-        {
-            "x": 6.767960818192567,
-            "y": 3.556684498992451,
-            "heading": -2.996323655916169,
-            "angularVelocity": -3.578110804784623e-7,
-            "velocityX": 2.9236600266743116,
-            "velocityY": -2.400592758534959,
-            "timestamp": 1.730633944243591
-        },
-        {
-            "x": 6.862482590576338,
-            "y": 3.466171063796537,
-            "heading": -2.99632366765796,
-            "angularVelocity": -3.394083586636284e-7,
-            "velocityX": 2.7322475205660415,
-            "velocityY": -2.6163824762813186,
-            "timestamp": 1.7652288232478874
-        },
-        {
-            "x": 6.949833264778536,
-            "y": 3.3687189870150447,
-            "heading": -2.9963236790740537,
-            "angularVelocity": -3.299937460810232e-7,
-            "velocityX": 2.524959667913537,
-            "velocityY": -2.8169509356974314,
-            "timestamp": 1.799823702252184
-        },
-        {
-            "x": 7.031655596531464,
-            "y": 3.2665810723307764,
-            "heading": -2.9963236904142594,
-            "angularVelocity": -3.2780012680929783e-7,
-            "velocityX": 2.365157332759168,
-            "velocityY": -2.9523998240197193,
-            "timestamp": 1.8344185812564804
-        },
-        {
-            "x": 7.1134768697956385,
-            "y": 3.164442309703858,
-            "heading": -2.9963237017545317,
-            "angularVelocity": -3.2780205084524347e-7,
-            "velocityX": 2.3651267360701933,
-            "velocityY": -2.952424334660453,
-            "timestamp": 1.8690134602607769
-        },
-        {
-            "x": 7.194171018365882,
-            "y": 3.0635712609157633,
-            "heading": -3.000602693910309,
-            "angularVelocity": -0.1236886001319993,
-            "velocityX": 2.3325460557391247,
-            "velocityY": -2.9157797827698024,
-            "timestamp": 1.9036083392650733
-        },
-        {
-            "x": 7.268926895683992,
-            "y": 2.9702293971794824,
-            "heading": -3.0147524025112875,
-            "angularVelocity": -0.4090116516731078,
-            "velocityX": 2.1608943135435097,
-            "velocityY": -2.698141066621111,
-            "timestamp": 1.9382032182693698
-        },
-        {
-            "x": 7.337464009959094,
-            "y": 2.8846562411973626,
-            "heading": -3.031563935832344,
-            "angularVelocity": -0.4859543899248224,
-            "velocityX": 1.9811346721747336,
-            "velocityY": -2.473578704278531,
-            "timestamp": 1.9727980972736663
-        },
-        {
-            "x": 7.399764505912731,
-            "y": 2.806871719273257,
-            "heading": -3.0489337906921676,
-            "angularVelocity": -0.5020932392238406,
-            "velocityX": 1.8008589059062745,
-            "velocityY": -2.2484403519505154,
-            "timestamp": 2.0073929762779628
-        },
-        {
-            "x": 7.455827645980218,
-            "y": 2.7368758607336336,
-            "heading": -3.0658680766465114,
-            "angularVelocity": -0.48950267905953376,
-            "velocityX": 1.6205618195839024,
-            "velocityY": -2.023301151911255,
-            "timestamp": 2.041987855282259
-        },
-        {
-            "x": 7.505655239118239,
-            "y": 2.674665896616921,
-            "heading": -3.081789323798935,
-            "angularVelocity": -0.4602197669327373,
-            "velocityX": 1.4403170229857634,
-            "velocityY": -1.7982419915094008,
-            "timestamp": 2.0765827342865553
-        },
-        {
-            "x": 7.549249359478674,
-            "y": 2.6202389173200062,
-            "heading": -3.0963205890594514,
-            "angularVelocity": -0.42004093318875263,
-            "velocityX": 1.2601321818475304,
-            "velocityY": -1.5732669361310743,
-            "timestamp": 2.1111776132908515
-        },
-        {
-            "x": 7.586611891972334,
-            "y": 2.573592354245129,
-            "heading": -3.1091966615790723,
-            "angularVelocity": -0.37219591136657937,
-            "velocityX": 1.0800018259644524,
-            "velocityY": -1.348366128671367,
-            "timestamp": 2.1457724922951478
-        },
-        {
-            "x": 7.617744461907291,
-            "y": 2.5347240241708393,
-            "heading": -3.1202208856007014,
-            "angularVelocity": -0.318666355799661,
-            "velocityX": 0.8999184512566408,
-            "velocityY": -1.1235284294378556,
-            "timestamp": 2.180367371299444
-        },
-        {
-            "x": 7.642648454713618,
-            "y": 2.5036320816555615,
-            "heading": -3.1292416557703198,
-            "angularVelocity": -0.26075449399601563,
-            "velocityX": 0.7198751238076218,
-            "velocityY": -0.8987440745613371,
-            "timestamp": 2.2149622503037403
-        },
-        {
-            "x": 7.66132505338278,
-            "y": 2.48031495979902,
-            "heading": -3.136138533441162,
-            "angularVelocity": -0.19936123118063845,
-            "velocityX": 0.5398659919244735,
-            "velocityY": -0.6740050125235452,
-            "timestamp": 2.2495571293080365
-        },
-        {
-            "x": 7.673775274710525,
-            "y": 2.4647713180548445,
-            "heading": -3.1408135216098865,
-            "angularVelocity": -0.1351352657757358,
-            "velocityX": 0.3598862515518098,
-            "velocityY": -0.4493047003357149,
-            "timestamp": 2.284152008312333
+            "heading": 0.16151347881375258,
+            "angularVelocity": -0.32215526782320836,
+            "velocityX": 3.5698675674478513,
+            "velocityY": -0.005685567203615865,
+            "timestamp": 1.4192794756416105
+        },
+        {
+            "x": 5.829773721796989,
+            "y": 3.97718762682745,
+            "heading": 0.15213932045666498,
+            "angularVelocity": -0.2709697131085786,
+            "velocityX": 3.6361292138465493,
+            "velocityY": -0.2860623240667741,
+            "timestamp": 1.453874332201581
+        },
+        {
+            "x": 5.956799770812804,
+            "y": 3.9574240312185514,
+            "heading": 0.1457481223879076,
+            "angularVelocity": -0.18474417021149397,
+            "velocityX": 3.671818924747189,
+            "velocityY": -0.5712871095342833,
+            "timestamp": 1.4884691887615515
+        },
+        {
+            "x": 6.083503083107889,
+            "y": 3.927762559716917,
+            "heading": 0.14376941436575622,
+            "angularVelocity": -0.05719659564771788,
+            "velocityX": 3.6624898870571543,
+            "velocityY": -0.8573954180216341,
+            "timestamp": 1.523064045321522
+        },
+        {
+            "x": 6.208289204844049,
+            "y": 3.888323576656908,
+            "heading": 0.14376927335150447,
+            "angularVelocity": -0.000004076162347950456,
+            "velocityX": 3.607071517115297,
+            "velocityY": -1.1400244713152792,
+            "timestamp": 1.5576589018814926
+        },
+        {
+            "x": 6.329707486841884,
+            "y": 3.8394909787467797,
+            "heading": 0.14376925272536353,
+            "angularVelocity": -5.962198716671582e-7,
+            "velocityX": 3.5097206368627396,
+            "velocityY": -1.4115565944167625,
+            "timestamp": 1.592253758441463
+        },
+        {
+            "x": 6.447051924752506,
+            "y": 3.781548715234953,
+            "heading": 0.14376923552481305,
+            "angularVelocity": -4.971996473322837e-7,
+            "velocityX": 3.3919619729367447,
+            "velocityY": -1.6748808717094477,
+            "timestamp": 1.6268486150014336
+        },
+        {
+            "x": 6.559640243151509,
+            "y": 3.714833691264027,
+            "heading": 0.14376922059812475,
+            "angularVelocity": -4.3147131718664413e-7,
+            "velocityX": 3.254481434366676,
+            "velocityY": -1.9284665584687428,
+            "timestamp": 1.6614434715614042
+        },
+        {
+            "x": 6.666817827672777,
+            "y": 3.6397338254007683,
+            "heading": 0.14376920720482705,
+            "angularVelocity": -3.8714707984086e-7,
+            "velocityX": 3.0980785925640646,
+            "velocityY": -2.1708390590685647,
+            "timestamp": 1.6960383281213747
+        },
+        {
+            "x": 6.7679615406661,
+            "y": 3.5566858017838987,
+            "heading": 0.14376919483153264,
+            "angularVelocity": -3.5766283306602865e-7,
+            "velocityX": 2.923663314457974,
+            "velocityY": -2.4005887543688678,
+            "timestamp": 1.7306331846813452
+        },
+        {
+            "x": 6.862483380166932,
+            "y": 3.466172559439492,
+            "heading": 0.1437691830947059,
+            "angularVelocity": -3.392650779789794e-7,
+            "velocityX": 2.7322512332715396,
+            "velocityY": -2.6163785991568234,
+            "timestamp": 1.7652280412413157
+        },
+        {
+            "x": 6.949834140776404,
+            "y": 3.3687206741301803,
+            "heading": 0.14376917168351727,
+            "angularVelocity": -3.2985217383486685e-7,
+            "velocityX": 2.5249638037391158,
+            "velocityY": -2.8169472285678507,
+            "timestamp": 1.7998228978012862
+        },
+        {
+            "x": 7.031656177612435,
+            "y": 3.2665826319797073,
+            "heading": 0.14376916034823112,
+            "angularVelocity": -3.2765813438231676e-7,
+            "velocityX": 2.3651503423404003,
+            "velocityY": -2.952405424009072,
+            "timestamp": 1.8344177543612568
+        },
+        {
+            "x": 7.113477155727132,
+            "y": 3.1644437417043827,
+            "heading": 0.14376914901287846,
+            "angularVelocity": -3.2766005667768915e-7,
+            "velocityX": 2.3651197389085663,
+            "velocityY": -2.952429939932413,
+            "timestamp": 1.8690126109212273
+        },
+        {
+            "x": 7.194171820782979,
+            "y": 3.0635723954548135,
+            "heading": 0.13949339206360356,
+            "angularVelocity": -0.12359516339843184,
+            "velocityX": 2.332562498588806,
+            "velocityY": -2.91579027288948,
+            "timestamp": 1.9036074674811978
+        },
+        {
+            "x": 7.268927567281138,
+            "y": 2.9702304150182344,
+            "heading": 0.12535509322542532,
+            "angularVelocity": -0.4086821060717307,
+            "velocityX": 2.160891933995128,
+            "velocityY": -2.6981461904537576,
+            "timestamp": 1.9382023240411683
+        },
+        {
+            "x": 7.337464553197734,
+            "y": 2.884657135542947,
+            "heading": 0.10855528307456563,
+            "angularVelocity": -0.48561583487814164,
+            "velocityX": 1.9811322471531543,
+            "velocityY": -2.473583878775318,
+            "timestamp": 1.9727971806011388
+        },
+        {
+            "x": 7.399764936518732,
+            "y": 2.806872483431395,
+            "heading": 0.0911966728700688,
+            "angularVelocity": -0.5017685266133554,
+            "velocityX": 1.8008568184984473,
+            "velocityY": -2.2484455738878646,
+            "timestamp": 2.0073920371611096
+        },
+        {
+            "x": 7.455827979936063,
+            "y": 2.7368764943083312,
+            "heading": 0.07427285825902008,
+            "angularVelocity": -0.4892003116622579,
+            "velocityX": 1.6205600771936914,
+            "velocityY": -2.023306239230242,
+            "timestamp": 2.0419868937210803
+        },
+        {
+            "x": 7.505655491239628,
+            "y": 2.674666404415114,
+            "heading": 0.05836114831277334,
+            "angularVelocity": -0.4599443827339935,
+            "velocityX": 1.4403155919200792,
+            "velocityY": -1.7982467938658737,
+            "timestamp": 2.076581750281051
+        },
+        {
+            "x": 7.549249543273319,
+            "y": 2.6202393080696202,
+            "heading": 0.04383838388032337,
+            "angularVelocity": -0.4197954805008245,
+            "velocityX": 1.2601310243365538,
+            "velocityY": -1.5732713402393688,
+            "timestamp": 2.111176606841022
+        },
+        {
+            "x": 7.586612019746033,
+            "y": 2.5735926396425732,
+            "heading": 0.030969705386613037,
+            "angularVelocity": -0.3719824208954968,
+            "velocityX": 1.0800009073009074,
+            "velocityY": -1.348370048772548,
+            "timestamp": 2.1457714634009926
+        },
+        {
+            "x": 7.6177445449139505,
+            "y": 2.5347242182058114,
+            "heading": 0.01995171749042388,
+            "angularVelocity": -0.31848630090688007,
+            "velocityX": 0.8999177410650449,
+            "velocityY": -1.1235317992830327,
+            "timestamp": 2.1803663199609633
+        },
+        {
+            "x": 7.642648503297853,
+            "y": 2.503632200129417,
+            "heading": 0.010935986949086615,
+            "angularVelocity": -0.26060898751548167,
+            "velocityX": 0.7198745958298824,
+            "velocityY": -0.8987468418172337,
+            "timestamp": 2.214961176520934
+        },
+        {
+            "x": 7.661325077102299,
+            "y": 2.4803150199755253,
+            "heading": 0.004042922192056018,
+            "angularVelocity": -0.19925114431624774,
+            "velocityX": 0.5398656234365572,
+            "velocityY": -0.6740071349471043,
+            "timestamp": 2.249556033080905
+        },
+        {
+            "x": 7.673775282436928,
+            "y": 2.464771338402636,
+            "heading": -0.0006295042844945549,
+            "angularVelocity": -0.13506130509461317,
+            "velocityX": 0.3598860227400032,
+            "velocityY": -0.44930614312403183,
+            "timestamp": 2.2841508896408755
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.06855886298677888,
-            "velocityX": 0.17993198613883732,
-            "velocityY": -0.22463781572640673,
-            "timestamp": 2.318746887316629
+            "heading": -0.003,
+            "angularVelocity": -0.06852162290068065,
+            "velocityX": 0.17993187953478723,
+            "velocityY": -0.22463854964001964,
+            "timestamp": 2.3187457462008463
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": -3.143185307179586,
-            "angularVelocity": -2.3105673781771525e-33,
+            "heading": -0.003,
+            "angularVelocity": -8.413636330536319e-34,
             "velocityX": 0,
-            "velocityY": -9.473748472540581e-34,
-            "timestamp": 2.3533417663209253
+            "velocityY": 4.110332962762654e-35,
+            "timestamp": 2.353340602760817
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.1.traj
deleted file mode 100644
index 998ccf57..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.1.traj	
+++ /dev/null
@@ -1,616 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -2.713469488843566e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2985074122681866,
-            "y": 6.738728579173207,
-            "heading": -2.7557122969387127,
-            "angularVelocity": -0.12159424626473993,
-            "velocityX": -0.03177182176995015,
-            "velocityY": -0.38893280014262377,
-            "timestamp": 0.046978349010656624
-        },
-        {
-            "x": 2.2955230171267877,
-            "y": 6.702182881977796,
-            "heading": -2.7670222648124514,
-            "angularVelocity": -0.24074851738984976,
-            "velocityX": -0.06352703328764883,
-            "velocityY": -0.777926384495132,
-            "timestamp": 0.09395669802131325
-        },
-        {
-            "x": 2.2910478333318625,
-            "y": 6.647359316893119,
-            "heading": -2.7837824265023454,
-            "angularVelocity": -0.3567635313470153,
-            "velocityX": -0.09526055915480924,
-            "velocityY": -1.1669964193982714,
-            "timestamp": 0.14093504703196988
-        },
-        {
-            "x": 2.2850832407725172,
-            "y": 6.574253215154707,
-            "heading": -2.805794841014387,
-            "angularVelocity": -0.46856509382754397,
-            "velocityX": -0.12696471214840174,
-            "velocityY": -1.556165835496437,
-            "timestamp": 0.1879133960426265
-        },
-        {
-            "x": 2.2776312180066953,
-            "y": 6.482858263005064,
-            "heading": -2.832779187520957,
-            "angularVelocity": -0.5743996346156903,
-            "velocityX": -0.15862674876316962,
-            "velocityY": -1.945469648771827,
-            "timestamp": 0.2348917450532831
-        },
-        {
-            "x": 2.2686948542910046,
-            "y": 6.373165502071328,
-            "heading": -2.864308391554441,
-            "angularVelocity": -0.6711432968053561,
-            "velocityX": -0.1902230262213726,
-            "velocityY": -2.3349641535689947,
-            "timestamp": 0.28187009406393976
-        },
-        {
-            "x": 2.2582796451376828,
-            "y": 6.245161498589565,
-            "heading": -2.8996563968568023,
-            "angularVelocity": -0.7524318339570105,
-            "velocityX": -0.22170232399947937,
-            "velocityY": -2.7247446148592136,
-            "timestamp": 0.3288484430745964
-        },
-        {
-            "x": 2.2463978083089957,
-            "y": 6.098825641295113,
-            "heading": -2.9373287044127423,
-            "angularVelocity": -0.8019078649910463,
-            "velocityX": -0.2529215495842953,
-            "velocityY": -3.1149638158049613,
-            "timestamp": 0.375826792085253
-        },
-        {
-            "x": 2.23309481763283,
-            "y": 5.934156164582871,
-            "heading": -2.9725027104068498,
-            "angularVelocity": -0.7487280148165356,
-            "velocityX": -0.2831728010098399,
-            "velocityY": -3.505220600129828,
-            "timestamp": 0.4228051410959096
-        },
-        {
-            "x": 2.2185299400469667,
-            "y": 5.757037726236599,
-            "heading": -2.9725027230810714,
-            "angularVelocity": -2.697885773041044e-7,
-            "velocityX": -0.31003383244820454,
-            "velocityY": -3.77021419603515,
-            "timestamp": 0.4697834901065662
-        },
-        {
-            "x": 2.2039649895163667,
-            "y": 5.57991929388838,
-            "heading": -2.972502735754471,
-            "angularVelocity": -2.6977108201545366e-7,
-            "velocityX": -0.3100353851791582,
-            "velocityY": -3.7702140683582024,
-            "timestamp": 0.5167618391172228
-        },
-        {
-            "x": 2.189400055868242,
-            "y": 5.402800860151866,
-            "heading": -2.972502748427869,
-            "angularVelocity": -2.6977103310705994e-7,
-            "velocityX": -0.31003502581200326,
-            "velocityY": -3.770214097910002,
-            "timestamp": 0.5637401881278795
-        },
-        {
-            "x": 2.174835140412249,
-            "y": 5.225682424919363,
-            "heading": -2.9725027611012638,
-            "angularVelocity": -2.69770971237986e-7,
-            "velocityX": -0.3100346385669985,
-            "velocityY": -3.7702141297542338,
-            "timestamp": 0.6107185371385362
-        },
-        {
-            "x": 2.1602702444212443,
-            "y": 5.048563988086202,
-            "heading": -2.972502773774655,
-            "angularVelocity": -2.697708934411251e-7,
-            "velocityX": -0.31003422422742555,
-            "velocityY": -3.770214163826477,
-            "timestamp": 0.6576968861491929
-        },
-        {
-            "x": 2.145705369054558,
-            "y": 4.87144554955705,
-            "heading": -2.972502786448042,
-            "angularVelocity": -2.697707995800925e-7,
-            "velocityX": -0.31003378520991653,
-            "velocityY": -3.7702141999279966,
-            "timestamp": 0.7046752351598495
-        },
-        {
-            "x": 2.1311405152628695,
-            "y": 4.694327109253736,
-            "heading": -2.9725027991214237,
-            "angularVelocity": -2.697706911872684e-7,
-            "velocityX": -0.31003332595584043,
-            "velocityY": -3.7702142376935592,
-            "timestamp": 0.7516535841705062
-        },
-        {
-            "x": 2.116575683697044,
-            "y": 4.5172086671227385,
-            "heading": -2.9725028117947994,
-            "angularVelocity": -2.6977056864766263e-7,
-            "velocityX": -0.3100328528472012,
-            "velocityY": -3.77021427659835,
-            "timestamp": 0.7986319331811629
-        },
-        {
-            "x": 2.102010874647989,
-            "y": 4.340090223140139,
-            "heading": -2.9725028244681693,
-            "angularVelocity": -2.697704390106331e-7,
-            "velocityX": -0.3100323735461829,
-            "velocityY": -3.7702143160122867,
-            "timestamp": 0.8456102821918196
-        },
-        {
-            "x": 2.087446088035901,
-            "y": 4.162971777312504,
-            "heading": -2.9725028371415325,
-            "angularVelocity": -2.697703054465384e-7,
-            "velocityX": -0.3100318959438893,
-            "velocityY": -3.7702143552864653,
-            "timestamp": 0.8925886312024762
-        },
-        {
-            "x": 2.0728813234455363,
-            "y": 3.9858533296739824,
-            "heading": -2.97250284981489,
-            "angularVelocity": -2.69770174050259e-7,
-            "velocityX": -0.31003142718065585,
-            "velocityY": -3.770214393833725,
-            "timestamp": 0.9395669802131329
-        },
-        {
-            "x": 2.0583165802454695,
-            "y": 3.8087348802765,
-            "heading": -2.972502862488243,
-            "angularVelocity": -2.697700772639611e-7,
-            "velocityX": -0.3100309718581832,
-            "velocityY": -3.770214431275663,
-            "timestamp": 0.9865453292237896
-        },
-        {
-            "x": 2.0437518576774885,
-            "y": 3.631616429182409,
-            "heading": -2.972502875161594,
-            "angularVelocity": -2.6977003935784156e-7,
-            "velocityX": -0.31003053267531316,
-            "velocityY": -3.7702144673903546,
-            "timestamp": 1.0335236782344461
-        },
-        {
-            "x": 2.02918715486261,
-            "y": 3.4544979764639927,
-            "heading": -2.972502887834946,
-            "angularVelocity": -2.697700540709368e-7,
-            "velocityX": -0.31003011220283416,
-            "velocityY": -3.770214501966405,
-            "timestamp": 1.0805020272451027
-        },
-        {
-            "x": 2.014622470919118,
-            "y": 3.2773795221937574,
-            "heading": -2.972502900508301,
-            "angularVelocity": -2.6977012019750386e-7,
-            "velocityX": -0.3100297104989377,
-            "velocityY": -3.770214534999038,
-            "timestamp": 1.1274803762557593
-        },
-        {
-            "x": 2.000060409435934,
-            "y": 3.1002608522940482,
-            "heading": -2.972502913181681,
-            "angularVelocity": -2.697706573776715e-7,
-            "velocityX": -0.30997388775603324,
-            "velocityY": -3.77021912497458,
-            "timestamp": 1.1744587252664158
-        },
-        {
-            "x": 2.001608653690626,
-            "y": 2.9225513149097844,
-            "heading": -2.972502926099156,
-            "angularVelocity": -2.749665584566826e-7,
-            "velocityX": 0.03295654886341899,
-            "velocityY": -3.782796567498611,
-            "timestamp": 1.2214370742770724
-        },
-        {
-            "x": 2.0215247790201643,
-            "y": 2.7459545305120523,
-            "heading": -2.9725029398381673,
-            "angularVelocity": -2.9245411249129853e-7,
-            "velocityX": 0.4239426405772788,
-            "velocityY": -3.759110060629698,
-            "timestamp": 1.268415423287729
-        },
-        {
-            "x": 2.0595955339413297,
-            "y": 2.572363939972743,
-            "heading": -2.9725029551587165,
-            "angularVelocity": -3.2611936426906016e-7,
-            "velocityX": 0.8103893755936399,
-            "velocityY": -3.695119011098735,
-            "timestamp": 1.3153937722983855
-        },
-        {
-            "x": 2.1154127951527757,
-            "y": 2.4036407715704846,
-            "heading": -2.972502973192088,
-            "angularVelocity": -3.8386557912539916e-7,
-            "velocityX": 1.1881486341460188,
-            "velocityY": -3.5915091091001314,
-            "timestamp": 1.362372121309042
-        },
-        {
-            "x": 2.1883781192124445,
-            "y": 2.241594069488506,
-            "heading": -2.9725029958666975,
-            "angularVelocity": -4.826608466149902e-7,
-            "velocityX": 1.5531691853010408,
-            "velocityY": -3.4493911662416843,
-            "timestamp": 1.4093504703196986
-        },
-        {
-            "x": 2.277709186746539,
-            "y": 2.0879612953868114,
-            "heading": -2.9725030269682566,
-            "angularVelocity": -6.620402725580348e-7,
-            "velocityX": 1.901536972145005,
-            "velocityY": -3.2702889168549634,
-            "timestamp": 1.4563288193303552
-        },
-        {
-            "x": 2.382448196411133,
-            "y": 1.9443897008895872,
-            "heading": -2.9725030765065807,
-            "angularVelocity": -0.0000010544926575738783,
-            "velocityX": 2.2295166150014207,
-            "velocityY": -3.0561226079838564,
-            "timestamp": 1.5033071683410117
-        },
-        {
-            "x": 2.5069799488821,
-            "y": 1.8074960632068364,
-            "heading": -2.9725031217478985,
-            "angularVelocity": -9.247979619471774e-7,
-            "velocityX": 2.5456091262787446,
-            "velocityY": -2.798303938555362,
-            "timestamp": 1.5522273859759206
-        },
-        {
-            "x": 2.645527130072252,
-            "y": 1.6848065781413901,
-            "heading": -2.9725031505695743,
-            "angularVelocity": -5.891567373541898e-7,
-            "velocityX": 2.8321047593068385,
-            "velocityY": -2.507950516105164,
-            "timestamp": 1.6011476036108294
-        },
-        {
-            "x": 2.7964788901009507,
-            "y": 1.5777477131393498,
-            "heading": -2.972503171586243,
-            "angularVelocity": -4.2961111591525744e-7,
-            "velocityX": 3.085672291060715,
-            "velocityY": -2.188438036008342,
-            "timestamp": 1.6500678212457383
-        },
-        {
-            "x": 2.9580801560836862,
-            "y": 1.4875641931626062,
-            "heading": -2.9725031883859017,
-            "angularVelocity": -3.434093106618729e-7,
-            "velocityX": 3.3033635947567466,
-            "velocityY": -1.8434815774897553,
-            "timestamp": 1.698988038880647
-        },
-        {
-            "x": 3.1284520354522702,
-            "y": 1.4153045196097347,
-            "heading": -2.9725032027821,
-            "angularVelocity": -2.9427911753262145e-7,
-            "velocityX": 3.482647616984601,
-            "velocityY": -1.4770922339746126,
-            "timestamp": 1.747908256515556
-        },
-        {
-            "x": 3.305613655866368,
-            "y": 1.3618087375272205,
-            "heading": -2.9725032158448177,
-            "angularVelocity": -2.670208303687875e-7,
-            "velocityX": 3.6214397437119468,
-            "velocityY": -1.0935311547825342,
-            "timestamp": 1.7968284741504648
-        },
-        {
-            "x": 3.4875051596049977,
-            "y": 1.327698333409946,
-            "heading": -2.9725032283101815,
-            "angularVelocity": -2.548100678766224e-7,
-            "velocityX": 3.7181253995246735,
-            "velocityY": -0.6972659928016005,
-            "timestamp": 1.8457486917853736
-        },
-        {
-            "x": 3.671044139168993,
-            "y": 1.3040027251815136,
-            "heading": -2.972503240670167,
-            "angularVelocity": -2.5265597445839956e-7,
-            "velocityX": 3.751802188079885,
-            "velocityY": -0.48437250229081197,
-            "timestamp": 1.8946689094202824
-        },
-        {
-            "x": 3.854583175253026,
-            "y": 1.2803075547441987,
-            "heading": -2.972503253030148,
-            "angularVelocity": -2.5265589039445086e-7,
-            "velocityX": 3.751803343431197,
-            "velocityY": -0.48436355320721586,
-            "timestamp": 1.9435891270551913
-        },
-        {
-            "x": 4.038122213757311,
-            "y": 1.256612403053765,
-            "heading": -2.972503265390126,
-            "angularVelocity": -2.5265581959003427e-7,
-            "velocityX": 3.751803392904627,
-            "velocityY": -0.4843631699938504,
-            "timestamp": 1.9925093446901
-        },
-        {
-            "x": 4.221661254747456,
-            "y": 1.2329172706184417,
-            "heading": -2.9725032777500995,
-            "angularVelocity": -2.526557374147453e-7,
-            "velocityX": 3.7518034437192385,
-            "velocityY": -0.48436277639154923,
-            "timestamp": 2.041429562325009
-        },
-        {
-            "x": 4.405200298259638,
-            "y": 1.2092221577184428,
-            "heading": -2.972503290110069,
-            "angularVelocity": -2.526556474640549e-7,
-            "velocityX": 3.7518034952732924,
-            "velocityY": -0.4843623770612628,
-            "timestamp": 2.090349779959918
-        },
-        {
-            "x": 4.5887393442969575,
-            "y": 1.1855270643778184,
-            "heading": -2.9725033024700336,
-            "angularVelocity": -2.526555535451317e-7,
-            "velocityX": 3.7518035468907667,
-            "velocityY": -0.4843619772393562,
-            "timestamp": 2.1392699975948273
-        },
-        {
-            "x": 4.772278392830339,
-            "y": 1.1618319903713676,
-            "heading": -2.9725033148299937,
-            "angularVelocity": -2.5265545524016583e-7,
-            "velocityX": 3.7518035979138844,
-            "velocityY": -0.48436158202088675,
-            "timestamp": 2.1881902152297363
-        },
-        {
-            "x": 4.955817443801929,
-            "y": 1.1381369352509825,
-            "heading": -2.972503327189949,
-            "angularVelocity": -2.526553571562985e-7,
-            "velocityX": 3.751803647754394,
-            "velocityY": -0.4843611959623946,
-            "timestamp": 2.2371104328646454
-        },
-        {
-            "x": 5.139356497135768,
-            "y": 1.114441898428302,
-            "heading": -2.9725033395499,
-            "angularVelocity": -2.526552791781282e-7,
-            "velocityX": 3.751803696042169,
-            "velocityY": -0.48436082193084695,
-            "timestamp": 2.2860306504995545
-        },
-        {
-            "x": 5.322895552747796,
-            "y": 1.0907468792522317,
-            "heading": -2.97250335190985,
-            "angularVelocity": -2.5265524884530903e-7,
-            "velocityX": 3.7518037426116595,
-            "velocityY": -0.4843604612086112,
-            "timestamp": 2.3349508681344635
-        },
-        {
-            "x": 5.506434610543511,
-            "y": 1.067051876990783,
-            "heading": -2.9725033642697998,
-            "angularVelocity": -2.5265526208757776e-7,
-            "velocityX": 3.7518037872493903,
-            "velocityY": -0.4843601154492814,
-            "timestamp": 2.3838710857693726
-        },
-        {
-            "x": 5.689973670421635,
-            "y": 1.043356890859468,
-            "heading": -2.972503376629753,
-            "angularVelocity": -2.52655316177873e-7,
-            "velocityX": 3.751803829816819,
-            "velocityY": -0.48435978572601984,
-            "timestamp": 2.4327913034042816
-        },
-        {
-            "x": 5.873512732277465,
-            "y": 1.0196619200472952,
-            "heading": -2.9725033889897103,
-            "angularVelocity": -2.526554076637863e-7,
-            "velocityX": 3.751803870244009,
-            "velocityY": -0.4843594725805982,
-            "timestamp": 2.4817115210391907
-        },
-        {
-            "x": 6.057051796000616,
-            "y": 0.9959669636992331,
-            "heading": -2.9725034013496736,
-            "angularVelocity": -2.5265551720355475e-7,
-            "velocityX": 3.751803908414743,
-            "velocityY": -0.48435917691325625,
-            "timestamp": 2.5306317386740997
-        },
-        {
-            "x": 6.240590861474878,
-            "y": 0.972272020915138,
-            "heading": -2.9725034137096418,
-            "angularVelocity": -2.526556193143771e-7,
-            "velocityX": 3.7518039442099766,
-            "velocityY": -0.48435889964615636,
-            "timestamp": 2.579551956309009
-        },
-        {
-            "x": 6.424129928585653,
-            "y": 0.9485770908073501,
-            "heading": -2.972503426069614,
-            "angularVelocity": -2.5265571512549996e-7,
-            "velocityX": 3.7518039776626577,
-            "velocityY": -0.4843586405241034,
-            "timestamp": 2.628472173943918
-        },
-        {
-            "x": 6.607668997222064,
-            "y": 0.9248821725170546,
-            "heading": -2.972503438429591,
-            "angularVelocity": -2.526558029491111e-7,
-            "velocityX": 3.7518040088489135,
-            "velocityY": -0.4843583989574683,
-            "timestamp": 2.677392391578827
-        },
-        {
-            "x": 6.791208068059123,
-            "y": 0.9011872712731527,
-            "heading": -2.972503450789653,
-            "angularVelocity": -2.5265754822248404e-7,
-            "velocityX": 3.751804053833296,
-            "velocityY": -0.48435805050452835,
-            "timestamp": 2.726312609213736
-        },
-        {
-            "x": 6.968893183591214,
-            "y": 0.8784347773708095,
-            "heading": -2.9889996672823953,
-            "angularVelocity": -0.3372065229932763,
-            "velocityX": 3.632140741035789,
-            "velocityY": -0.4650938814733942,
-            "timestamp": 2.775232826848645
-        },
-        {
-            "x": 7.126993065244261,
-            "y": 0.8581341417430071,
-            "heading": -3.0160355228350078,
-            "angularVelocity": -0.5526519884760304,
-            "velocityX": 3.231790235132359,
-            "velocityY": -0.4149743523077879,
-            "timestamp": 2.824153044483554
-        },
-        {
-            "x": 7.265300475634767,
-            "y": 0.8403578347966174,
-            "heading": -3.044055057812944,
-            "angularVelocity": -0.5727598185896393,
-            "velocityX": 2.827203497390216,
-            "velocityY": -0.363373423214384,
-            "timestamp": 2.873073262118463
-        },
-        {
-            "x": 7.3838209572244,
-            "y": 0.8251160468222167,
-            "heading": -3.0703114433567618,
-            "angularVelocity": -0.5367184941769645,
-            "velocityX": 2.4227300555805176,
-            "velocityY": -0.31156418984375384,
-            "timestamp": 2.921993479753372
-        },
-        {
-            "x": 7.4825676028202075,
-            "y": 0.812412258635403,
-            "heading": -3.093486935010673,
-            "angularVelocity": -0.47374056728180086,
-            "velocityX": 2.018524249682452,
-            "velocityY": -0.25968380356812476,
-            "timestamp": 2.9709136973882813
-        },
-        {
-            "x": 7.561551267898866,
-            "y": 0.8022480583211733,
-            "heading": -3.1128054575324193,
-            "angularVelocity": -0.39489854002532226,
-            "velocityX": 1.6145403454276093,
-            "velocityY": -0.20777095453836575,
-            "timestamp": 3.0198339150231903
-        },
-        {
-            "x": 7.62078038482921,
-            "y": 0.7946243025369127,
-            "heading": -3.1277548992054633,
-            "angularVelocity": -0.30558820863414116,
-            "velocityX": 1.210728810987095,
-            "velocityY": -0.1558405941927034,
-            "timestamp": 3.0687541326580994
-        },
-        {
-            "x": 7.6602615463029124,
-            "y": 0.7895415063368129,
-            "heading": -3.137971889882548,
-            "angularVelocity": -0.208850065903912,
-            "velocityX": 0.8070520406992162,
-            "velocityY": -0.10389970539445781,
-            "timestamp": 3.1176743502930084
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.10656978952845035,
-            "velocityX": 0.40348254058055355,
-            "velocityY": -0.051952065213203134,
-            "timestamp": 3.1665945679279175
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -3.143185307179586,
-            "angularVelocity": 1.1636456429625425e-30,
-            "velocityX": -1.050271820667376e-32,
-            "velocityY": -1.190420457247715e-32,
-            "timestamp": 3.2155147855628265
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.traj
deleted file mode 100644
index 998ccf57..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.traj	
+++ /dev/null
@@ -1,616 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -2.713469488843566e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2985074122681866,
-            "y": 6.738728579173207,
-            "heading": -2.7557122969387127,
-            "angularVelocity": -0.12159424626473993,
-            "velocityX": -0.03177182176995015,
-            "velocityY": -0.38893280014262377,
-            "timestamp": 0.046978349010656624
-        },
-        {
-            "x": 2.2955230171267877,
-            "y": 6.702182881977796,
-            "heading": -2.7670222648124514,
-            "angularVelocity": -0.24074851738984976,
-            "velocityX": -0.06352703328764883,
-            "velocityY": -0.777926384495132,
-            "timestamp": 0.09395669802131325
-        },
-        {
-            "x": 2.2910478333318625,
-            "y": 6.647359316893119,
-            "heading": -2.7837824265023454,
-            "angularVelocity": -0.3567635313470153,
-            "velocityX": -0.09526055915480924,
-            "velocityY": -1.1669964193982714,
-            "timestamp": 0.14093504703196988
-        },
-        {
-            "x": 2.2850832407725172,
-            "y": 6.574253215154707,
-            "heading": -2.805794841014387,
-            "angularVelocity": -0.46856509382754397,
-            "velocityX": -0.12696471214840174,
-            "velocityY": -1.556165835496437,
-            "timestamp": 0.1879133960426265
-        },
-        {
-            "x": 2.2776312180066953,
-            "y": 6.482858263005064,
-            "heading": -2.832779187520957,
-            "angularVelocity": -0.5743996346156903,
-            "velocityX": -0.15862674876316962,
-            "velocityY": -1.945469648771827,
-            "timestamp": 0.2348917450532831
-        },
-        {
-            "x": 2.2686948542910046,
-            "y": 6.373165502071328,
-            "heading": -2.864308391554441,
-            "angularVelocity": -0.6711432968053561,
-            "velocityX": -0.1902230262213726,
-            "velocityY": -2.3349641535689947,
-            "timestamp": 0.28187009406393976
-        },
-        {
-            "x": 2.2582796451376828,
-            "y": 6.245161498589565,
-            "heading": -2.8996563968568023,
-            "angularVelocity": -0.7524318339570105,
-            "velocityX": -0.22170232399947937,
-            "velocityY": -2.7247446148592136,
-            "timestamp": 0.3288484430745964
-        },
-        {
-            "x": 2.2463978083089957,
-            "y": 6.098825641295113,
-            "heading": -2.9373287044127423,
-            "angularVelocity": -0.8019078649910463,
-            "velocityX": -0.2529215495842953,
-            "velocityY": -3.1149638158049613,
-            "timestamp": 0.375826792085253
-        },
-        {
-            "x": 2.23309481763283,
-            "y": 5.934156164582871,
-            "heading": -2.9725027104068498,
-            "angularVelocity": -0.7487280148165356,
-            "velocityX": -0.2831728010098399,
-            "velocityY": -3.505220600129828,
-            "timestamp": 0.4228051410959096
-        },
-        {
-            "x": 2.2185299400469667,
-            "y": 5.757037726236599,
-            "heading": -2.9725027230810714,
-            "angularVelocity": -2.697885773041044e-7,
-            "velocityX": -0.31003383244820454,
-            "velocityY": -3.77021419603515,
-            "timestamp": 0.4697834901065662
-        },
-        {
-            "x": 2.2039649895163667,
-            "y": 5.57991929388838,
-            "heading": -2.972502735754471,
-            "angularVelocity": -2.6977108201545366e-7,
-            "velocityX": -0.3100353851791582,
-            "velocityY": -3.7702140683582024,
-            "timestamp": 0.5167618391172228
-        },
-        {
-            "x": 2.189400055868242,
-            "y": 5.402800860151866,
-            "heading": -2.972502748427869,
-            "angularVelocity": -2.6977103310705994e-7,
-            "velocityX": -0.31003502581200326,
-            "velocityY": -3.770214097910002,
-            "timestamp": 0.5637401881278795
-        },
-        {
-            "x": 2.174835140412249,
-            "y": 5.225682424919363,
-            "heading": -2.9725027611012638,
-            "angularVelocity": -2.69770971237986e-7,
-            "velocityX": -0.3100346385669985,
-            "velocityY": -3.7702141297542338,
-            "timestamp": 0.6107185371385362
-        },
-        {
-            "x": 2.1602702444212443,
-            "y": 5.048563988086202,
-            "heading": -2.972502773774655,
-            "angularVelocity": -2.697708934411251e-7,
-            "velocityX": -0.31003422422742555,
-            "velocityY": -3.770214163826477,
-            "timestamp": 0.6576968861491929
-        },
-        {
-            "x": 2.145705369054558,
-            "y": 4.87144554955705,
-            "heading": -2.972502786448042,
-            "angularVelocity": -2.697707995800925e-7,
-            "velocityX": -0.31003378520991653,
-            "velocityY": -3.7702141999279966,
-            "timestamp": 0.7046752351598495
-        },
-        {
-            "x": 2.1311405152628695,
-            "y": 4.694327109253736,
-            "heading": -2.9725027991214237,
-            "angularVelocity": -2.697706911872684e-7,
-            "velocityX": -0.31003332595584043,
-            "velocityY": -3.7702142376935592,
-            "timestamp": 0.7516535841705062
-        },
-        {
-            "x": 2.116575683697044,
-            "y": 4.5172086671227385,
-            "heading": -2.9725028117947994,
-            "angularVelocity": -2.6977056864766263e-7,
-            "velocityX": -0.3100328528472012,
-            "velocityY": -3.77021427659835,
-            "timestamp": 0.7986319331811629
-        },
-        {
-            "x": 2.102010874647989,
-            "y": 4.340090223140139,
-            "heading": -2.9725028244681693,
-            "angularVelocity": -2.697704390106331e-7,
-            "velocityX": -0.3100323735461829,
-            "velocityY": -3.7702143160122867,
-            "timestamp": 0.8456102821918196
-        },
-        {
-            "x": 2.087446088035901,
-            "y": 4.162971777312504,
-            "heading": -2.9725028371415325,
-            "angularVelocity": -2.697703054465384e-7,
-            "velocityX": -0.3100318959438893,
-            "velocityY": -3.7702143552864653,
-            "timestamp": 0.8925886312024762
-        },
-        {
-            "x": 2.0728813234455363,
-            "y": 3.9858533296739824,
-            "heading": -2.97250284981489,
-            "angularVelocity": -2.69770174050259e-7,
-            "velocityX": -0.31003142718065585,
-            "velocityY": -3.770214393833725,
-            "timestamp": 0.9395669802131329
-        },
-        {
-            "x": 2.0583165802454695,
-            "y": 3.8087348802765,
-            "heading": -2.972502862488243,
-            "angularVelocity": -2.697700772639611e-7,
-            "velocityX": -0.3100309718581832,
-            "velocityY": -3.770214431275663,
-            "timestamp": 0.9865453292237896
-        },
-        {
-            "x": 2.0437518576774885,
-            "y": 3.631616429182409,
-            "heading": -2.972502875161594,
-            "angularVelocity": -2.6977003935784156e-7,
-            "velocityX": -0.31003053267531316,
-            "velocityY": -3.7702144673903546,
-            "timestamp": 1.0335236782344461
-        },
-        {
-            "x": 2.02918715486261,
-            "y": 3.4544979764639927,
-            "heading": -2.972502887834946,
-            "angularVelocity": -2.697700540709368e-7,
-            "velocityX": -0.31003011220283416,
-            "velocityY": -3.770214501966405,
-            "timestamp": 1.0805020272451027
-        },
-        {
-            "x": 2.014622470919118,
-            "y": 3.2773795221937574,
-            "heading": -2.972502900508301,
-            "angularVelocity": -2.6977012019750386e-7,
-            "velocityX": -0.3100297104989377,
-            "velocityY": -3.770214534999038,
-            "timestamp": 1.1274803762557593
-        },
-        {
-            "x": 2.000060409435934,
-            "y": 3.1002608522940482,
-            "heading": -2.972502913181681,
-            "angularVelocity": -2.697706573776715e-7,
-            "velocityX": -0.30997388775603324,
-            "velocityY": -3.77021912497458,
-            "timestamp": 1.1744587252664158
-        },
-        {
-            "x": 2.001608653690626,
-            "y": 2.9225513149097844,
-            "heading": -2.972502926099156,
-            "angularVelocity": -2.749665584566826e-7,
-            "velocityX": 0.03295654886341899,
-            "velocityY": -3.782796567498611,
-            "timestamp": 1.2214370742770724
-        },
-        {
-            "x": 2.0215247790201643,
-            "y": 2.7459545305120523,
-            "heading": -2.9725029398381673,
-            "angularVelocity": -2.9245411249129853e-7,
-            "velocityX": 0.4239426405772788,
-            "velocityY": -3.759110060629698,
-            "timestamp": 1.268415423287729
-        },
-        {
-            "x": 2.0595955339413297,
-            "y": 2.572363939972743,
-            "heading": -2.9725029551587165,
-            "angularVelocity": -3.2611936426906016e-7,
-            "velocityX": 0.8103893755936399,
-            "velocityY": -3.695119011098735,
-            "timestamp": 1.3153937722983855
-        },
-        {
-            "x": 2.1154127951527757,
-            "y": 2.4036407715704846,
-            "heading": -2.972502973192088,
-            "angularVelocity": -3.8386557912539916e-7,
-            "velocityX": 1.1881486341460188,
-            "velocityY": -3.5915091091001314,
-            "timestamp": 1.362372121309042
-        },
-        {
-            "x": 2.1883781192124445,
-            "y": 2.241594069488506,
-            "heading": -2.9725029958666975,
-            "angularVelocity": -4.826608466149902e-7,
-            "velocityX": 1.5531691853010408,
-            "velocityY": -3.4493911662416843,
-            "timestamp": 1.4093504703196986
-        },
-        {
-            "x": 2.277709186746539,
-            "y": 2.0879612953868114,
-            "heading": -2.9725030269682566,
-            "angularVelocity": -6.620402725580348e-7,
-            "velocityX": 1.901536972145005,
-            "velocityY": -3.2702889168549634,
-            "timestamp": 1.4563288193303552
-        },
-        {
-            "x": 2.382448196411133,
-            "y": 1.9443897008895872,
-            "heading": -2.9725030765065807,
-            "angularVelocity": -0.0000010544926575738783,
-            "velocityX": 2.2295166150014207,
-            "velocityY": -3.0561226079838564,
-            "timestamp": 1.5033071683410117
-        },
-        {
-            "x": 2.5069799488821,
-            "y": 1.8074960632068364,
-            "heading": -2.9725031217478985,
-            "angularVelocity": -9.247979619471774e-7,
-            "velocityX": 2.5456091262787446,
-            "velocityY": -2.798303938555362,
-            "timestamp": 1.5522273859759206
-        },
-        {
-            "x": 2.645527130072252,
-            "y": 1.6848065781413901,
-            "heading": -2.9725031505695743,
-            "angularVelocity": -5.891567373541898e-7,
-            "velocityX": 2.8321047593068385,
-            "velocityY": -2.507950516105164,
-            "timestamp": 1.6011476036108294
-        },
-        {
-            "x": 2.7964788901009507,
-            "y": 1.5777477131393498,
-            "heading": -2.972503171586243,
-            "angularVelocity": -4.2961111591525744e-7,
-            "velocityX": 3.085672291060715,
-            "velocityY": -2.188438036008342,
-            "timestamp": 1.6500678212457383
-        },
-        {
-            "x": 2.9580801560836862,
-            "y": 1.4875641931626062,
-            "heading": -2.9725031883859017,
-            "angularVelocity": -3.434093106618729e-7,
-            "velocityX": 3.3033635947567466,
-            "velocityY": -1.8434815774897553,
-            "timestamp": 1.698988038880647
-        },
-        {
-            "x": 3.1284520354522702,
-            "y": 1.4153045196097347,
-            "heading": -2.9725032027821,
-            "angularVelocity": -2.9427911753262145e-7,
-            "velocityX": 3.482647616984601,
-            "velocityY": -1.4770922339746126,
-            "timestamp": 1.747908256515556
-        },
-        {
-            "x": 3.305613655866368,
-            "y": 1.3618087375272205,
-            "heading": -2.9725032158448177,
-            "angularVelocity": -2.670208303687875e-7,
-            "velocityX": 3.6214397437119468,
-            "velocityY": -1.0935311547825342,
-            "timestamp": 1.7968284741504648
-        },
-        {
-            "x": 3.4875051596049977,
-            "y": 1.327698333409946,
-            "heading": -2.9725032283101815,
-            "angularVelocity": -2.548100678766224e-7,
-            "velocityX": 3.7181253995246735,
-            "velocityY": -0.6972659928016005,
-            "timestamp": 1.8457486917853736
-        },
-        {
-            "x": 3.671044139168993,
-            "y": 1.3040027251815136,
-            "heading": -2.972503240670167,
-            "angularVelocity": -2.5265597445839956e-7,
-            "velocityX": 3.751802188079885,
-            "velocityY": -0.48437250229081197,
-            "timestamp": 1.8946689094202824
-        },
-        {
-            "x": 3.854583175253026,
-            "y": 1.2803075547441987,
-            "heading": -2.972503253030148,
-            "angularVelocity": -2.5265589039445086e-7,
-            "velocityX": 3.751803343431197,
-            "velocityY": -0.48436355320721586,
-            "timestamp": 1.9435891270551913
-        },
-        {
-            "x": 4.038122213757311,
-            "y": 1.256612403053765,
-            "heading": -2.972503265390126,
-            "angularVelocity": -2.5265581959003427e-7,
-            "velocityX": 3.751803392904627,
-            "velocityY": -0.4843631699938504,
-            "timestamp": 1.9925093446901
-        },
-        {
-            "x": 4.221661254747456,
-            "y": 1.2329172706184417,
-            "heading": -2.9725032777500995,
-            "angularVelocity": -2.526557374147453e-7,
-            "velocityX": 3.7518034437192385,
-            "velocityY": -0.48436277639154923,
-            "timestamp": 2.041429562325009
-        },
-        {
-            "x": 4.405200298259638,
-            "y": 1.2092221577184428,
-            "heading": -2.972503290110069,
-            "angularVelocity": -2.526556474640549e-7,
-            "velocityX": 3.7518034952732924,
-            "velocityY": -0.4843623770612628,
-            "timestamp": 2.090349779959918
-        },
-        {
-            "x": 4.5887393442969575,
-            "y": 1.1855270643778184,
-            "heading": -2.9725033024700336,
-            "angularVelocity": -2.526555535451317e-7,
-            "velocityX": 3.7518035468907667,
-            "velocityY": -0.4843619772393562,
-            "timestamp": 2.1392699975948273
-        },
-        {
-            "x": 4.772278392830339,
-            "y": 1.1618319903713676,
-            "heading": -2.9725033148299937,
-            "angularVelocity": -2.5265545524016583e-7,
-            "velocityX": 3.7518035979138844,
-            "velocityY": -0.48436158202088675,
-            "timestamp": 2.1881902152297363
-        },
-        {
-            "x": 4.955817443801929,
-            "y": 1.1381369352509825,
-            "heading": -2.972503327189949,
-            "angularVelocity": -2.526553571562985e-7,
-            "velocityX": 3.751803647754394,
-            "velocityY": -0.4843611959623946,
-            "timestamp": 2.2371104328646454
-        },
-        {
-            "x": 5.139356497135768,
-            "y": 1.114441898428302,
-            "heading": -2.9725033395499,
-            "angularVelocity": -2.526552791781282e-7,
-            "velocityX": 3.751803696042169,
-            "velocityY": -0.48436082193084695,
-            "timestamp": 2.2860306504995545
-        },
-        {
-            "x": 5.322895552747796,
-            "y": 1.0907468792522317,
-            "heading": -2.97250335190985,
-            "angularVelocity": -2.5265524884530903e-7,
-            "velocityX": 3.7518037426116595,
-            "velocityY": -0.4843604612086112,
-            "timestamp": 2.3349508681344635
-        },
-        {
-            "x": 5.506434610543511,
-            "y": 1.067051876990783,
-            "heading": -2.9725033642697998,
-            "angularVelocity": -2.5265526208757776e-7,
-            "velocityX": 3.7518037872493903,
-            "velocityY": -0.4843601154492814,
-            "timestamp": 2.3838710857693726
-        },
-        {
-            "x": 5.689973670421635,
-            "y": 1.043356890859468,
-            "heading": -2.972503376629753,
-            "angularVelocity": -2.52655316177873e-7,
-            "velocityX": 3.751803829816819,
-            "velocityY": -0.48435978572601984,
-            "timestamp": 2.4327913034042816
-        },
-        {
-            "x": 5.873512732277465,
-            "y": 1.0196619200472952,
-            "heading": -2.9725033889897103,
-            "angularVelocity": -2.526554076637863e-7,
-            "velocityX": 3.751803870244009,
-            "velocityY": -0.4843594725805982,
-            "timestamp": 2.4817115210391907
-        },
-        {
-            "x": 6.057051796000616,
-            "y": 0.9959669636992331,
-            "heading": -2.9725034013496736,
-            "angularVelocity": -2.5265551720355475e-7,
-            "velocityX": 3.751803908414743,
-            "velocityY": -0.48435917691325625,
-            "timestamp": 2.5306317386740997
-        },
-        {
-            "x": 6.240590861474878,
-            "y": 0.972272020915138,
-            "heading": -2.9725034137096418,
-            "angularVelocity": -2.526556193143771e-7,
-            "velocityX": 3.7518039442099766,
-            "velocityY": -0.48435889964615636,
-            "timestamp": 2.579551956309009
-        },
-        {
-            "x": 6.424129928585653,
-            "y": 0.9485770908073501,
-            "heading": -2.972503426069614,
-            "angularVelocity": -2.5265571512549996e-7,
-            "velocityX": 3.7518039776626577,
-            "velocityY": -0.4843586405241034,
-            "timestamp": 2.628472173943918
-        },
-        {
-            "x": 6.607668997222064,
-            "y": 0.9248821725170546,
-            "heading": -2.972503438429591,
-            "angularVelocity": -2.526558029491111e-7,
-            "velocityX": 3.7518040088489135,
-            "velocityY": -0.4843583989574683,
-            "timestamp": 2.677392391578827
-        },
-        {
-            "x": 6.791208068059123,
-            "y": 0.9011872712731527,
-            "heading": -2.972503450789653,
-            "angularVelocity": -2.5265754822248404e-7,
-            "velocityX": 3.751804053833296,
-            "velocityY": -0.48435805050452835,
-            "timestamp": 2.726312609213736
-        },
-        {
-            "x": 6.968893183591214,
-            "y": 0.8784347773708095,
-            "heading": -2.9889996672823953,
-            "angularVelocity": -0.3372065229932763,
-            "velocityX": 3.632140741035789,
-            "velocityY": -0.4650938814733942,
-            "timestamp": 2.775232826848645
-        },
-        {
-            "x": 7.126993065244261,
-            "y": 0.8581341417430071,
-            "heading": -3.0160355228350078,
-            "angularVelocity": -0.5526519884760304,
-            "velocityX": 3.231790235132359,
-            "velocityY": -0.4149743523077879,
-            "timestamp": 2.824153044483554
-        },
-        {
-            "x": 7.265300475634767,
-            "y": 0.8403578347966174,
-            "heading": -3.044055057812944,
-            "angularVelocity": -0.5727598185896393,
-            "velocityX": 2.827203497390216,
-            "velocityY": -0.363373423214384,
-            "timestamp": 2.873073262118463
-        },
-        {
-            "x": 7.3838209572244,
-            "y": 0.8251160468222167,
-            "heading": -3.0703114433567618,
-            "angularVelocity": -0.5367184941769645,
-            "velocityX": 2.4227300555805176,
-            "velocityY": -0.31156418984375384,
-            "timestamp": 2.921993479753372
-        },
-        {
-            "x": 7.4825676028202075,
-            "y": 0.812412258635403,
-            "heading": -3.093486935010673,
-            "angularVelocity": -0.47374056728180086,
-            "velocityX": 2.018524249682452,
-            "velocityY": -0.25968380356812476,
-            "timestamp": 2.9709136973882813
-        },
-        {
-            "x": 7.561551267898866,
-            "y": 0.8022480583211733,
-            "heading": -3.1128054575324193,
-            "angularVelocity": -0.39489854002532226,
-            "velocityX": 1.6145403454276093,
-            "velocityY": -0.20777095453836575,
-            "timestamp": 3.0198339150231903
-        },
-        {
-            "x": 7.62078038482921,
-            "y": 0.7946243025369127,
-            "heading": -3.1277548992054633,
-            "angularVelocity": -0.30558820863414116,
-            "velocityX": 1.210728810987095,
-            "velocityY": -0.1558405941927034,
-            "timestamp": 3.0687541326580994
-        },
-        {
-            "x": 7.6602615463029124,
-            "y": 0.7895415063368129,
-            "heading": -3.137971889882548,
-            "angularVelocity": -0.208850065903912,
-            "velocityX": 0.8070520406992162,
-            "velocityY": -0.10389970539445781,
-            "timestamp": 3.1176743502930084
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.10656978952845035,
-            "velocityX": 0.40348254058055355,
-            "velocityY": -0.051952065213203134,
-            "timestamp": 3.1665945679279175
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -3.143185307179586,
-            "angularVelocity": 1.1636456429625425e-30,
-            "velocityX": -1.050271820667376e-32,
-            "velocityY": -1.190420457247715e-32,
-            "timestamp": 3.2155147855628265
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.1.traj
index f5c62fb3..98cdd92f 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.1.traj	
@@ -3,128 +3,128 @@
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": -2.75,
+            "heading": 0.39,
             "angularVelocity": 0,
-            "velocityX": -1.6054364331739777e-34,
-            "velocityY": -2.5654655421376103e-34,
+            "velocityX": 1.3865554557202343e-36,
+            "velocityY": 1.1876687369143486e-32,
             "timestamp": 0
         },
         {
-            "x": 2.373020964607517,
-            "y": 6.753775569864816,
-            "heading": -2.7564987511657213,
-            "angularVelocity": -0.06940035185307017,
-            "velocityX": 0.7797929951747273,
-            "velocityY": -0.03443378276852441,
-            "timestamp": 0.09364147287724231
+            "x": 2.373020964624076,
+            "y": 6.753775569913306,
+            "heading": 0.3835012481902214,
+            "angularVelocity": -0.06940035874833077,
+            "velocityX": 0.7797929955465333,
+            "velocityY": -0.034433782259311436,
+            "timestamp": 0.09364147285382916
         },
         {
-            "x": 2.5190628897265572,
-            "y": 6.7473267051761905,
-            "heading": -2.769496449901771,
-            "angularVelocity": -0.13880280111661264,
-            "velocityX": 1.5595859466082067,
-            "velocityY": -0.06886761271975776,
-            "timestamp": 0.18728294575448462
+            "x": 2.5190628897747724,
+            "y": 6.747326705309919,
+            "heading": 0.37050354798266344,
+            "angularVelocity": -0.13880281686559404,
+            "velocityX": 1.5595859473362035,
+            "velocityY": -0.06886761182670828,
+            "timestamp": 0.18728294570765833
         },
         {
-            "x": 2.738125767578475,
-            "y": 6.73765340144405,
-            "heading": -2.7889934253789708,
-            "angularVelocity": -0.20820876560484325,
-            "velocityX": 2.339378814973306,
-            "velocityY": -0.10330149062058414,
-            "timestamp": 0.2809244186317269
+            "x": 2.738125767671939,
+            "y": 6.737653401682201,
+            "heading": 0.35100657007014807,
+            "angularVelocity": -0.20820879166370637,
+            "velocityX": 2.3393788160414357,
+            "velocityY": -0.10330148953128306,
+            "timestamp": 0.2809244185614875
         },
         {
-            "x": 3.030209577950943,
-            "y": 6.724755654214012,
-            "heading": -2.814990111841214,
-            "angularVelocity": -0.2776193673960349,
-            "velocityX": 3.1191714674903457,
-            "velocityY": -0.13773541608988796,
-            "timestamp": 0.37456589150896924
+            "x": 3.0302095781027427,
+            "y": 6.7247556545524185,
+            "heading": 0.3250098803168658,
+            "angularVelocity": -0.27761940261055373,
+            "velocityX": 3.119171468893202,
+            "velocityY": -0.13773541505367973,
+            "timestamp": 0.37456589141531665
         },
         {
-            "x": 3.3841048307253856,
-            "y": 6.709128194593673,
-            "heading": -2.814990114833307,
-            "angularVelocity": -3.1952637479967674e-8,
-            "velocityX": 3.779257650489721,
-            "velocityY": -0.16688609373799812,
-            "timestamp": 0.4682073643862116
+            "x": 3.384104830792732,
+            "y": 6.709128195027275,
+            "heading": 0.3250098773247812,
+            "angularVelocity": -3.195254338233196e-8,
+            "velocityX": 3.77925765053277,
+            "velocityY": -0.16688609276314717,
+            "timestamp": 0.46820736426914583
         },
         {
-            "x": 3.7380000807913594,
-            "y": 6.693500673639671,
-            "heading": -2.814990117825208,
-            "angularVelocity": -3.1950597051067127e-8,
-            "velocityX": 3.779257621565883,
-            "velocityY": -0.16688674872173656,
-            "timestamp": 0.5618488372634539
+            "x": 3.738000080774581,
+            "y": 6.693500674175882,
+            "heading": 0.32500987433288897,
+            "angularVelocity": -3.1950503021671023e-8,
+            "velocityX": 3.7792576216124307,
+            "velocityY": -0.16688674766770242,
+            "timestamp": 0.561848837122975
         },
         {
-            "x": 4.091895327848636,
-            "y": 6.677873084552109,
-            "heading": -2.8149901208171126,
-            "angularVelocity": -3.1950635675887034e-8,
-            "velocityX": 3.779257589435919,
-            "velocityY": -0.16688747632201045,
-            "timestamp": 0.6554903101406963
+            "x": 4.091895327748041,
+            "y": 6.677873085197927,
+            "heading": 0.32500987134099335,
+            "angularVelocity": -3.1950540402322325e-8,
+            "velocityX": 3.7792575894857667,
+            "velocityY": -0.1668874751932408,
+            "timestamp": 0.6554903099768041
         },
         {
-            "x": 4.445790571527361,
-            "y": 6.662245418924005,
-            "heading": -2.814990123809099,
-            "angularVelocity": -3.195149337938281e-8,
-            "velocityX": 3.779257553356315,
-            "velocityY": -0.16688829370019806,
-            "timestamp": 0.7491317830179386
+            "x": 4.445790571342994,
+            "y": 6.662245419680397,
+            "heading": 0.32500986834901585,
+            "angularVelocity": -3.1951399041908766e-8,
+            "velocityX": 3.7792575534066395,
+            "velocityY": -0.166888292561096,
+            "timestamp": 0.7491317828306333
         },
         {
-            "x": 4.737874328479073,
-            "y": 6.649347276687054,
-            "heading": -2.8409937626507493,
-            "angularVelocity": -0.27769361205743737,
-            "velocityX": 3.119170897008512,
-            "velocityY": -0.13773963438078918,
-            "timestamp": 0.8427732558951809
+            "x": 4.737874328369047,
+            "y": 6.649347277173169,
+            "heading": 0.29900623239629953,
+            "angularVelocity": -0.2776935812758607,
+            "velocityX": 3.119170898582289,
+            "velocityY": -0.13773963730152244,
+            "timestamp": 0.8427732556844625
         },
         {
-            "x": 4.956937160751323,
-            "y": 6.639673651230963,
-            "heading": -2.8604968062706018,
-            "angularVelocity": -0.20827356747619122,
-            "velocityX": 2.3393783282266916,
-            "velocityY": -0.10330492632038867,
-            "timestamp": 0.9364147287724233
+            "x": 4.956937160697527,
+            "y": 6.639673651489352,
+            "heading": 0.27950319129562423,
+            "angularVelocity": -0.20827354062589903,
+            "velocityX": 2.339378329412089,
+            "velocityY": -0.10330492877809694,
+            "timestamp": 0.9364147285382917
         },
         {
-            "x": 5.102979052789527,
-            "y": 6.633224554793233,
-            "heading": -2.8734989345094433,
-            "angularVelocity": -0.1388501039053446,
-            "velocityX": 1.5595855933369822,
-            "velocityY": -0.06887008757539573,
-            "timestamp": 1.0300562016496655
+            "x": 5.102979052772278,
+            "y": 6.633224554883952,
+            "heading": 0.26650106474792545,
+            "angularVelocity": -0.13885008588030126,
+            "velocityX": 1.5595855941172054,
+            "velocityY": -0.06887008938317173,
+            "timestamp": 1.0300562013921208
         },
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": -0.06942506659508625,
-            "velocityX": 0.7797928093911877,
-            "velocityY": -0.0344351139955655,
-            "timestamp": 1.1236976745269078
+            "heading": 0.26,
+            "angularVelocity": -0.06942505868186148,
+            "velocityX": 0.7797928097703705,
+            "velocityY": -0.03443511497296222,
+            "timestamp": 1.12369767424595
         },
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 1.756012262603739e-32,
-            "velocityX": 4.952933956711668e-34,
-            "velocityY": 0,
-            "timestamp": 1.2173391474041502
+            "heading": 0.26,
+            "angularVelocity": 0,
+            "velocityX": -1.6658104354370778e-33,
+            "velocityY": -5.427630465010738e-32,
+            "timestamp": 1.217339147099779
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.traj
index f5c62fb3..98cdd92f 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.traj	
@@ -3,128 +3,128 @@
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": -2.75,
+            "heading": 0.39,
             "angularVelocity": 0,
-            "velocityX": -1.6054364331739777e-34,
-            "velocityY": -2.5654655421376103e-34,
+            "velocityX": 1.3865554557202343e-36,
+            "velocityY": 1.1876687369143486e-32,
             "timestamp": 0
         },
         {
-            "x": 2.373020964607517,
-            "y": 6.753775569864816,
-            "heading": -2.7564987511657213,
-            "angularVelocity": -0.06940035185307017,
-            "velocityX": 0.7797929951747273,
-            "velocityY": -0.03443378276852441,
-            "timestamp": 0.09364147287724231
+            "x": 2.373020964624076,
+            "y": 6.753775569913306,
+            "heading": 0.3835012481902214,
+            "angularVelocity": -0.06940035874833077,
+            "velocityX": 0.7797929955465333,
+            "velocityY": -0.034433782259311436,
+            "timestamp": 0.09364147285382916
         },
         {
-            "x": 2.5190628897265572,
-            "y": 6.7473267051761905,
-            "heading": -2.769496449901771,
-            "angularVelocity": -0.13880280111661264,
-            "velocityX": 1.5595859466082067,
-            "velocityY": -0.06886761271975776,
-            "timestamp": 0.18728294575448462
+            "x": 2.5190628897747724,
+            "y": 6.747326705309919,
+            "heading": 0.37050354798266344,
+            "angularVelocity": -0.13880281686559404,
+            "velocityX": 1.5595859473362035,
+            "velocityY": -0.06886761182670828,
+            "timestamp": 0.18728294570765833
         },
         {
-            "x": 2.738125767578475,
-            "y": 6.73765340144405,
-            "heading": -2.7889934253789708,
-            "angularVelocity": -0.20820876560484325,
-            "velocityX": 2.339378814973306,
-            "velocityY": -0.10330149062058414,
-            "timestamp": 0.2809244186317269
+            "x": 2.738125767671939,
+            "y": 6.737653401682201,
+            "heading": 0.35100657007014807,
+            "angularVelocity": -0.20820879166370637,
+            "velocityX": 2.3393788160414357,
+            "velocityY": -0.10330148953128306,
+            "timestamp": 0.2809244185614875
         },
         {
-            "x": 3.030209577950943,
-            "y": 6.724755654214012,
-            "heading": -2.814990111841214,
-            "angularVelocity": -0.2776193673960349,
-            "velocityX": 3.1191714674903457,
-            "velocityY": -0.13773541608988796,
-            "timestamp": 0.37456589150896924
+            "x": 3.0302095781027427,
+            "y": 6.7247556545524185,
+            "heading": 0.3250098803168658,
+            "angularVelocity": -0.27761940261055373,
+            "velocityX": 3.119171468893202,
+            "velocityY": -0.13773541505367973,
+            "timestamp": 0.37456589141531665
         },
         {
-            "x": 3.3841048307253856,
-            "y": 6.709128194593673,
-            "heading": -2.814990114833307,
-            "angularVelocity": -3.1952637479967674e-8,
-            "velocityX": 3.779257650489721,
-            "velocityY": -0.16688609373799812,
-            "timestamp": 0.4682073643862116
+            "x": 3.384104830792732,
+            "y": 6.709128195027275,
+            "heading": 0.3250098773247812,
+            "angularVelocity": -3.195254338233196e-8,
+            "velocityX": 3.77925765053277,
+            "velocityY": -0.16688609276314717,
+            "timestamp": 0.46820736426914583
         },
         {
-            "x": 3.7380000807913594,
-            "y": 6.693500673639671,
-            "heading": -2.814990117825208,
-            "angularVelocity": -3.1950597051067127e-8,
-            "velocityX": 3.779257621565883,
-            "velocityY": -0.16688674872173656,
-            "timestamp": 0.5618488372634539
+            "x": 3.738000080774581,
+            "y": 6.693500674175882,
+            "heading": 0.32500987433288897,
+            "angularVelocity": -3.1950503021671023e-8,
+            "velocityX": 3.7792576216124307,
+            "velocityY": -0.16688674766770242,
+            "timestamp": 0.561848837122975
         },
         {
-            "x": 4.091895327848636,
-            "y": 6.677873084552109,
-            "heading": -2.8149901208171126,
-            "angularVelocity": -3.1950635675887034e-8,
-            "velocityX": 3.779257589435919,
-            "velocityY": -0.16688747632201045,
-            "timestamp": 0.6554903101406963
+            "x": 4.091895327748041,
+            "y": 6.677873085197927,
+            "heading": 0.32500987134099335,
+            "angularVelocity": -3.1950540402322325e-8,
+            "velocityX": 3.7792575894857667,
+            "velocityY": -0.1668874751932408,
+            "timestamp": 0.6554903099768041
         },
         {
-            "x": 4.445790571527361,
-            "y": 6.662245418924005,
-            "heading": -2.814990123809099,
-            "angularVelocity": -3.195149337938281e-8,
-            "velocityX": 3.779257553356315,
-            "velocityY": -0.16688829370019806,
-            "timestamp": 0.7491317830179386
+            "x": 4.445790571342994,
+            "y": 6.662245419680397,
+            "heading": 0.32500986834901585,
+            "angularVelocity": -3.1951399041908766e-8,
+            "velocityX": 3.7792575534066395,
+            "velocityY": -0.166888292561096,
+            "timestamp": 0.7491317828306333
         },
         {
-            "x": 4.737874328479073,
-            "y": 6.649347276687054,
-            "heading": -2.8409937626507493,
-            "angularVelocity": -0.27769361205743737,
-            "velocityX": 3.119170897008512,
-            "velocityY": -0.13773963438078918,
-            "timestamp": 0.8427732558951809
+            "x": 4.737874328369047,
+            "y": 6.649347277173169,
+            "heading": 0.29900623239629953,
+            "angularVelocity": -0.2776935812758607,
+            "velocityX": 3.119170898582289,
+            "velocityY": -0.13773963730152244,
+            "timestamp": 0.8427732556844625
         },
         {
-            "x": 4.956937160751323,
-            "y": 6.639673651230963,
-            "heading": -2.8604968062706018,
-            "angularVelocity": -0.20827356747619122,
-            "velocityX": 2.3393783282266916,
-            "velocityY": -0.10330492632038867,
-            "timestamp": 0.9364147287724233
+            "x": 4.956937160697527,
+            "y": 6.639673651489352,
+            "heading": 0.27950319129562423,
+            "angularVelocity": -0.20827354062589903,
+            "velocityX": 2.339378329412089,
+            "velocityY": -0.10330492877809694,
+            "timestamp": 0.9364147285382917
         },
         {
-            "x": 5.102979052789527,
-            "y": 6.633224554793233,
-            "heading": -2.8734989345094433,
-            "angularVelocity": -0.1388501039053446,
-            "velocityX": 1.5595855933369822,
-            "velocityY": -0.06887008757539573,
-            "timestamp": 1.0300562016496655
+            "x": 5.102979052772278,
+            "y": 6.633224554883952,
+            "heading": 0.26650106474792545,
+            "angularVelocity": -0.13885008588030126,
+            "velocityX": 1.5595855941172054,
+            "velocityY": -0.06887008938317173,
+            "timestamp": 1.0300562013921208
         },
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": -0.06942506659508625,
-            "velocityX": 0.7797928093911877,
-            "velocityY": -0.0344351139955655,
-            "timestamp": 1.1236976745269078
+            "heading": 0.26,
+            "angularVelocity": -0.06942505868186148,
+            "velocityX": 0.7797928097703705,
+            "velocityY": -0.03443511497296222,
+            "timestamp": 1.12369767424595
         },
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": -2.88,
-            "angularVelocity": 1.756012262603739e-32,
-            "velocityX": 4.952933956711668e-34,
-            "velocityY": 0,
-            "timestamp": 1.2173391474041502
+            "heading": 0.26,
+            "angularVelocity": 0,
+            "velocityX": -1.6658104354370778e-33,
+            "velocityY": -5.427630465010738e-32,
+            "timestamp": 1.217339147099779
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.1.traj
index 02347e71..e89d75b6 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.1.traj	
@@ -3,128 +3,128 @@
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": -2.75,
+            "heading": 0.39,
             "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
+            "velocityX": 1.1894430825117781e-34,
+            "velocityY": 3.241165026752671e-33,
             "timestamp": 0
         },
         {
-            "x": 2.347774752133698,
-            "y": 6.727866832421063,
-            "heading": -2.7630995428589378,
-            "angularVelocity": -0.15973393577530476,
-            "velocityX": 0.5825584351440611,
-            "velocityY": -0.3552456424698836,
-            "timestamp": 0.08200851494302656
+            "x": 2.347774772680024,
+            "y": 6.727866824995534,
+            "heading": 0.376906913073687,
+            "angularVelocity": -0.15965534994810393,
+            "velocityX": 0.5825591851522491,
+            "velocityY": -0.35524603759330214,
+            "timestamp": 0.08200844463131937
         },
         {
-            "x": 2.4433243460965994,
-            "y": 6.669600579952885,
-            "heading": -2.78929580141764,
-            "angularVelocity": -0.31943339757952466,
-            "velocityX": 1.1651179640221794,
-            "velocityY": -0.7104902766335507,
-            "timestamp": 0.16401702988605313
+            "x": 2.443324407414016,
+            "y": 6.669600558032611,
+            "heading": 0.35072353035940795,
+            "angularVelocity": -0.3192766651286972,
+            "velocityX": 1.1651194601183832,
+            "velocityY": -0.7104910625346852,
+            "timestamp": 0.16401688926263874
         },
         {
-            "x": 2.586648917905952,
-            "y": 6.582201380168291,
-            "heading": -2.8285851612262256,
-            "angularVelocity": -0.4790887853033408,
-            "velocityX": 1.74767915147499,
-            "velocityY": -1.065733233254043,
-            "timestamp": 0.24602554482907968
+            "x": 2.5866490395090382,
+            "y": 6.582201337136711,
+            "heading": 0.31145340371020835,
+            "angularVelocity": -0.4788546694885386,
+            "velocityX": 1.7476813850007613,
+            "velocityY": -1.0657344044118373,
+            "timestamp": 0.24602533389395811
         },
         {
-            "x": 2.7777486316597817,
-            "y": 6.465669444023111,
-            "heading": -2.8809649184638944,
-            "angularVelocity": -0.6387112030264004,
-            "velocityX": 2.3302423399154564,
-            "velocityY": -1.4209736175095709,
-            "timestamp": 0.32803405977210626
+            "x": 2.7777488318834505,
+            "y": 6.4656693736120285,
+            "heading": 0.25909914961330927,
+            "angularVelocity": -0.6384007687533294,
+            "velocityX": 2.330245296487799,
+            "velocityY": -1.420975169673899,
+            "timestamp": 0.3280337785252775
         },
         {
-            "x": 3.016623643228877,
-            "y": 6.320005072616726,
-            "heading": -2.9464355230657095,
-            "angularVelocity": -0.7983391071927027,
-            "velocityX": 2.9128074290218198,
-            "velocityY": -1.776210330202685,
-            "timestamp": 0.41004257471513283
+            "x": 3.016623938716172,
+            "y": 6.320004968537675,
+            "heading": 0.19366021259536087,
+            "angularVelocity": -0.7979536413858164,
+            "velocityX": 2.9128110880119595,
+            "velocityY": -1.7762122636175968,
+            "timestamp": 0.4100422231565969
         },
         {
-            "x": 3.2814961744061293,
-            "y": 6.158489554910389,
-            "heading": -2.9464355313484747,
-            "angularVelocity": -1.0099884469874073e-7,
-            "velocityX": 3.229817432510106,
-            "velocityY": -1.9694969213690323,
-            "timestamp": 0.4920510896581594
+            "x": 3.281496231375059,
+            "y": 6.1584895705745515,
+            "heading": 0.19366020431656966,
+            "angularVelocity": -1.0095047213207674e-7,
+            "velocityX": 3.2298172932026463,
+            "velocityY": -1.9694971498269327,
+            "timestamp": 0.4920506677879163
         },
         {
-            "x": 3.546368691279801,
-            "y": 5.996974013747309,
-            "heading": -2.9464355396312305,
-            "angularVelocity": -1.009987284542081e-7,
-            "velocityX": 3.229817258094308,
-            "velocityY": -1.9694972073971664,
-            "timestamp": 0.574059604601186
+            "x": 3.54636850972689,
+            "y": 5.99697414914899,
+            "heading": 0.1936601960377879,
+            "angularVelocity": -1.0095035712972839e-7,
+            "velocityX": 3.2298171187443208,
+            "velocityY": -1.9694974359247637,
+            "timestamp": 0.5740591124192357
         },
         {
-            "x": 3.785245806593771,
-            "y": 5.851315740974219,
-            "heading": -3.0120060162505475,
-            "angularVelocity": -0.7995569321657724,
-            "velocityX": 2.912833081783328,
-            "velocityY": -1.7761359643481167,
-            "timestamp": 0.6560681195442125
+            "x": 3.7852456869653537,
+            "y": 5.851315832693838,
+            "heading": 0.1281196168762306,
+            "angularVelocity": -0.7991930520838956,
+            "velocityX": 2.91283633426253,
+            "velocityY": -1.7761380198097898,
+            "timestamp": 0.6560675570505551
         },
         {
-            "x": 3.976347491680443,
-            "y": 5.7347893459358685,
-            "heading": -3.0644727513052605,
-            "angularVelocity": -0.6397717979793073,
-            "velocityX": 2.3302663780637296,
-            "velocityY": -1.4209060500523003,
-            "timestamp": 0.738076634487239
+            "x": 3.976347420420938,
+            "y": 5.73478940159931,
+            "heading": 0.07567676319198373,
+            "angularVelocity": -0.63948114026563,
+            "velocityX": 2.3302689657718703,
+            "velocityY": -1.420907707960915,
+            "timestamp": 0.7380760016818745
         },
         {
-            "x": 4.119673744072056,
-            "y": 5.64739465388186,
-            "heading": -3.1038280635353432,
-            "angularVelocity": -0.4798929996162412,
-            "velocityX": 1.7476996442526245,
-            "velocityY": -1.0656782666376068,
-            "timestamp": 0.8200851494302656
+            "x": 4.119673708551739,
+            "y": 5.647394681888429,
+            "heading": 0.03633934856671714,
+            "angularVelocity": -0.47967517006466276,
+            "velocityX": 1.7477015784794376,
+            "velocityY": -1.0656795175640326,
+            "timestamp": 0.8200844463131939
         },
         {
-            "x": 4.2152245778751,
-            "y": 5.589131554648123,
-            "heading": -3.130066448240266,
-            "angularVelocity": -0.3199470777290782,
-            "velocityX": 1.1651330824539987,
-            "velocityY": -0.7104518265476871,
-            "timestamp": 0.9020936643732921
+            "x": 4.215224566025358,
+            "y": 5.589131563991008,
+            "heading": 0.01011289315165925,
+            "angularVelocity": -0.31980188787828684,
+            "velocityX": 1.1651343700417993,
+            "velocityY": -0.7104526632513426,
+            "timestamp": 0.9020928909445133
         },
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.15996947327279604,
-            "velocityX": 0.5825666049201219,
-            "velocityY": -0.3552259746242433,
-            "timestamp": 0.9841021793163186
+            "heading": -0.003,
+            "angularVelocity": -0.15989686440963644,
+            "velocityX": 0.5825672488903126,
+            "velocityY": -0.3552263931107682,
+            "timestamp": 0.9841013355758327
         },
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0,
-            "velocityX": -4.515906316961801e-34,
-            "velocityY": 2.878377535752905e-33,
-            "timestamp": 1.0661106942593452
+            "heading": -0.003,
+            "angularVelocity": -2.9357360091407933e-34,
+            "velocityX": 2.8612808764100986e-34,
+            "velocityY": 0,
+            "timestamp": 1.066109780207152
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.traj
index 02347e71..e89d75b6 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.traj	
@@ -3,128 +3,128 @@
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": -2.75,
+            "heading": 0.39,
             "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
+            "velocityX": 1.1894430825117781e-34,
+            "velocityY": 3.241165026752671e-33,
             "timestamp": 0
         },
         {
-            "x": 2.347774752133698,
-            "y": 6.727866832421063,
-            "heading": -2.7630995428589378,
-            "angularVelocity": -0.15973393577530476,
-            "velocityX": 0.5825584351440611,
-            "velocityY": -0.3552456424698836,
-            "timestamp": 0.08200851494302656
+            "x": 2.347774772680024,
+            "y": 6.727866824995534,
+            "heading": 0.376906913073687,
+            "angularVelocity": -0.15965534994810393,
+            "velocityX": 0.5825591851522491,
+            "velocityY": -0.35524603759330214,
+            "timestamp": 0.08200844463131937
         },
         {
-            "x": 2.4433243460965994,
-            "y": 6.669600579952885,
-            "heading": -2.78929580141764,
-            "angularVelocity": -0.31943339757952466,
-            "velocityX": 1.1651179640221794,
-            "velocityY": -0.7104902766335507,
-            "timestamp": 0.16401702988605313
+            "x": 2.443324407414016,
+            "y": 6.669600558032611,
+            "heading": 0.35072353035940795,
+            "angularVelocity": -0.3192766651286972,
+            "velocityX": 1.1651194601183832,
+            "velocityY": -0.7104910625346852,
+            "timestamp": 0.16401688926263874
         },
         {
-            "x": 2.586648917905952,
-            "y": 6.582201380168291,
-            "heading": -2.8285851612262256,
-            "angularVelocity": -0.4790887853033408,
-            "velocityX": 1.74767915147499,
-            "velocityY": -1.065733233254043,
-            "timestamp": 0.24602554482907968
+            "x": 2.5866490395090382,
+            "y": 6.582201337136711,
+            "heading": 0.31145340371020835,
+            "angularVelocity": -0.4788546694885386,
+            "velocityX": 1.7476813850007613,
+            "velocityY": -1.0657344044118373,
+            "timestamp": 0.24602533389395811
         },
         {
-            "x": 2.7777486316597817,
-            "y": 6.465669444023111,
-            "heading": -2.8809649184638944,
-            "angularVelocity": -0.6387112030264004,
-            "velocityX": 2.3302423399154564,
-            "velocityY": -1.4209736175095709,
-            "timestamp": 0.32803405977210626
+            "x": 2.7777488318834505,
+            "y": 6.4656693736120285,
+            "heading": 0.25909914961330927,
+            "angularVelocity": -0.6384007687533294,
+            "velocityX": 2.330245296487799,
+            "velocityY": -1.420975169673899,
+            "timestamp": 0.3280337785252775
         },
         {
-            "x": 3.016623643228877,
-            "y": 6.320005072616726,
-            "heading": -2.9464355230657095,
-            "angularVelocity": -0.7983391071927027,
-            "velocityX": 2.9128074290218198,
-            "velocityY": -1.776210330202685,
-            "timestamp": 0.41004257471513283
+            "x": 3.016623938716172,
+            "y": 6.320004968537675,
+            "heading": 0.19366021259536087,
+            "angularVelocity": -0.7979536413858164,
+            "velocityX": 2.9128110880119595,
+            "velocityY": -1.7762122636175968,
+            "timestamp": 0.4100422231565969
         },
         {
-            "x": 3.2814961744061293,
-            "y": 6.158489554910389,
-            "heading": -2.9464355313484747,
-            "angularVelocity": -1.0099884469874073e-7,
-            "velocityX": 3.229817432510106,
-            "velocityY": -1.9694969213690323,
-            "timestamp": 0.4920510896581594
+            "x": 3.281496231375059,
+            "y": 6.1584895705745515,
+            "heading": 0.19366020431656966,
+            "angularVelocity": -1.0095047213207674e-7,
+            "velocityX": 3.2298172932026463,
+            "velocityY": -1.9694971498269327,
+            "timestamp": 0.4920506677879163
         },
         {
-            "x": 3.546368691279801,
-            "y": 5.996974013747309,
-            "heading": -2.9464355396312305,
-            "angularVelocity": -1.009987284542081e-7,
-            "velocityX": 3.229817258094308,
-            "velocityY": -1.9694972073971664,
-            "timestamp": 0.574059604601186
+            "x": 3.54636850972689,
+            "y": 5.99697414914899,
+            "heading": 0.1936601960377879,
+            "angularVelocity": -1.0095035712972839e-7,
+            "velocityX": 3.2298171187443208,
+            "velocityY": -1.9694974359247637,
+            "timestamp": 0.5740591124192357
         },
         {
-            "x": 3.785245806593771,
-            "y": 5.851315740974219,
-            "heading": -3.0120060162505475,
-            "angularVelocity": -0.7995569321657724,
-            "velocityX": 2.912833081783328,
-            "velocityY": -1.7761359643481167,
-            "timestamp": 0.6560681195442125
+            "x": 3.7852456869653537,
+            "y": 5.851315832693838,
+            "heading": 0.1281196168762306,
+            "angularVelocity": -0.7991930520838956,
+            "velocityX": 2.91283633426253,
+            "velocityY": -1.7761380198097898,
+            "timestamp": 0.6560675570505551
         },
         {
-            "x": 3.976347491680443,
-            "y": 5.7347893459358685,
-            "heading": -3.0644727513052605,
-            "angularVelocity": -0.6397717979793073,
-            "velocityX": 2.3302663780637296,
-            "velocityY": -1.4209060500523003,
-            "timestamp": 0.738076634487239
+            "x": 3.976347420420938,
+            "y": 5.73478940159931,
+            "heading": 0.07567676319198373,
+            "angularVelocity": -0.63948114026563,
+            "velocityX": 2.3302689657718703,
+            "velocityY": -1.420907707960915,
+            "timestamp": 0.7380760016818745
         },
         {
-            "x": 4.119673744072056,
-            "y": 5.64739465388186,
-            "heading": -3.1038280635353432,
-            "angularVelocity": -0.4798929996162412,
-            "velocityX": 1.7476996442526245,
-            "velocityY": -1.0656782666376068,
-            "timestamp": 0.8200851494302656
+            "x": 4.119673708551739,
+            "y": 5.647394681888429,
+            "heading": 0.03633934856671714,
+            "angularVelocity": -0.47967517006466276,
+            "velocityX": 1.7477015784794376,
+            "velocityY": -1.0656795175640326,
+            "timestamp": 0.8200844463131939
         },
         {
-            "x": 4.2152245778751,
-            "y": 5.589131554648123,
-            "heading": -3.130066448240266,
-            "angularVelocity": -0.3199470777290782,
-            "velocityX": 1.1651330824539987,
-            "velocityY": -0.7104518265476871,
-            "timestamp": 0.9020936643732921
+            "x": 4.215224566025358,
+            "y": 5.589131563991008,
+            "heading": 0.01011289315165925,
+            "angularVelocity": -0.31980188787828684,
+            "velocityX": 1.1651343700417993,
+            "velocityY": -0.7104526632513426,
+            "timestamp": 0.9020928909445133
         },
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.15996947327279604,
-            "velocityX": 0.5825666049201219,
-            "velocityY": -0.3552259746242433,
-            "timestamp": 0.9841021793163186
+            "heading": -0.003,
+            "angularVelocity": -0.15989686440963644,
+            "velocityX": 0.5825672488903126,
+            "velocityY": -0.3552263931107682,
+            "timestamp": 0.9841013355758327
         },
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0,
-            "velocityX": -4.515906316961801e-34,
-            "velocityY": 2.878377535752905e-33,
-            "timestamp": 1.0661106942593452
+            "heading": -0.003,
+            "angularVelocity": -2.9357360091407933e-34,
+            "velocityX": 2.8612808764100986e-34,
+            "velocityY": 0,
+            "timestamp": 1.066109780207152
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.1.traj
deleted file mode 100644
index 96fad438..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.1.traj	
+++ /dev/null
@@ -1,544 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -7.419385593511551e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 2.293773644442264,
-            "y": 6.737033643205664,
-            "heading": -2.75526766821854,
-            "angularVelocity": -0.10504784252909477,
-            "velocityX": -0.1241659859778435,
-            "velocityY": -0.3981690982414253,
-            "timestamp": 0.050145420331514534
-        },
-        {
-            "x": 2.2813193646388377,
-            "y": 6.697095621507987,
-            "heading": -2.7655160845701583,
-            "angularVelocity": -0.20437392455513897,
-            "velocityX": -0.24836325473174134,
-            "velocityY": -0.7964440507955471,
-            "timestamp": 0.10029084066302907
-        },
-        {
-            "x": 2.2626352666399625,
-            "y": 6.637179497704236,
-            "heading": -2.7803732946846287,
-            "angularVelocity": -0.2962824923243045,
-            "velocityX": -0.3725982926327797,
-            "velocityY": -1.1948473740501462,
-            "timestamp": 0.1504362609945436
-        },
-        {
-            "x": 2.2377190441074863,
-            "y": 6.557277385764274,
-            "heading": -2.799337789044294,
-            "angularVelocity": -0.3781899570148323,
-            "velocityX": -0.4968793235305074,
-            "velocityY": -1.5934079605220988,
-            "timestamp": 0.20058168132605814
-        },
-        {
-            "x": 2.2065679165597203,
-            "y": 6.457379637066711,
-            "heading": -2.8216967909576067,
-            "angularVelocity": -0.44588322852808987,
-            "velocityX": -0.6212158027956237,
-            "velocityY": -1.9921609598071555,
-            "timestamp": 0.25072710165757267
-        },
-        {
-            "x": 2.1691787676944525,
-            "y": 6.337475004416767,
-            "heading": -2.846358557659031,
-            "angularVelocity": -0.49180496520688655,
-            "velocityX": -0.7456144273611754,
-            "velocityY": -2.3911382506567254,
-            "timestamp": 0.3008725219890872
-        },
-        {
-            "x": 2.1255494213188793,
-            "y": 6.197554115260485,
-            "heading": -2.8714458003624506,
-            "angularVelocity": -0.5002898078741331,
-            "velocityX": -0.8700564495648131,
-            "velocityY": -2.7903024489824935,
-            "timestamp": 0.35101794232060174
-        },
-        {
-            "x": 2.0756883391523315,
-            "y": 6.037638592000563,
-            "heading": -2.8929930081362487,
-            "angularVelocity": -0.4296944293486671,
-            "velocityX": -0.9943297281568805,
-            "velocityY": -3.1890354533417224,
-            "timestamp": 0.4011633626521163
-        },
-        {
-            "x": 2.0198170203632526,
-            "y": 5.858213938792853,
-            "heading": -2.897462778158027,
-            "angularVelocity": -0.08913615624774217,
-            "velocityX": -1.1141858702092737,
-            "velocityY": -3.578086533556236,
-            "timestamp": 0.4513087829836308
-        },
-        {
-            "x": 1.9633339655295539,
-            "y": 5.677120968466723,
-            "heading": -2.897462791142581,
-            "angularVelocity": -2.5893798802100903e-7,
-            "velocityX": -1.1263851107496088,
-            "velocityY": -3.611356114454979,
-            "timestamp": 0.5014542033151453
-        },
-        {
-            "x": 1.9068509267595308,
-            "y": 5.496027993130308,
-            "heading": -2.897462804127121,
-            "angularVelocity": -2.5893770297899326e-7,
-            "velocityX": -1.1263847904077713,
-            "velocityY": -3.611356214370084,
-            "timestamp": 0.5515996236466598
-        },
-        {
-            "x": 1.8503679083113405,
-            "y": 5.314935011455497,
-            "heading": -2.897462817111663,
-            "angularVelocity": -2.589377492115138e-7,
-            "velocityX": -1.1263843851497735,
-            "velocityY": -3.611356340770398,
-            "timestamp": 0.6017450439781744
-        },
-        {
-            "x": 1.7938849123636174,
-            "y": 5.133842022762774,
-            "heading": -2.8974628300962064,
-            "angularVelocity": -2.589377769209582e-7,
-            "velocityX": -1.1263839364454482,
-            "velocityY": -3.6113564807215806,
-            "timestamp": 0.651890464309689
-        },
-        {
-            "x": 1.737401940953919,
-            "y": 4.952749026416629,
-            "heading": -2.89746284308075,
-            "angularVelocity": -2.5893777760216377e-7,
-            "velocityX": -1.126383447108141,
-            "velocityY": -3.611356633346137,
-            "timestamp": 0.7020358846412036
-        },
-        {
-            "x": 1.680918995692082,
-            "y": 4.771656021914957,
-            "heading": -2.897462856065292,
-            "angularVelocity": -2.5893774300527816e-7,
-            "velocityX": -1.1263829256674796,
-            "velocityY": -3.6113567959836606,
-            "timestamp": 0.7521813049727182
-        },
-        {
-            "x": 1.6244360774524895,
-            "y": 4.590563008985036,
-            "heading": -2.8974628690498303,
-            "angularVelocity": -2.589376688920553e-7,
-            "velocityX": -1.1263823867898586,
-            "velocityY": -3.611356964059789,
-            "timestamp": 0.8023267253042328
-        },
-        {
-            "x": 1.567953186210211,
-            "y": 4.40946998763471,
-            "heading": -2.897462882034363,
-            "angularVelocity": -2.589375607053774e-7,
-            "velocityX": -1.1263818484094146,
-            "velocityY": -3.6113571319795557,
-            "timestamp": 0.8524721456357474
-        },
-        {
-            "x": 1.5114708900262606,
-            "y": 4.228376780686611,
-            "heading": -2.8974628950188928,
-            "angularVelocity": -2.589374956200649e-7,
-            "velocityX": -1.1263699817559003,
-            "velocityY": -3.611360833170398,
-            "timestamp": 0.902617565967262
-        },
-        {
-            "x": 1.4675217320832292,
-            "y": 4.043840955415942,
-            "heading": -2.8974629081797687,
-            "angularVelocity": -2.624542007539591e-7,
-            "velocityX": -0.8764341320200526,
-            "velocityY": -3.6800135296641385,
-            "timestamp": 0.9527629862987765
-        },
-        {
-            "x": 1.4442056001491934,
-            "y": 3.855582205665926,
-            "heading": -2.8974629221278954,
-            "angularVelocity": -2.7815354458428775e-7,
-            "velocityX": -0.4649703159309848,
-            "velocityY": -3.75425609169144,
-            "timestamp": 1.0029084066302911
-        },
-        {
-            "x": 1.4418077217209881,
-            "y": 3.665900239807532,
-            "heading": -2.897462937731955,
-            "angularVelocity": -3.1117616956346287e-7,
-            "velocityX": -0.04781849294218802,
-            "velocityY": -3.782637868112273,
-            "timestamp": 1.0530538269618057
-        },
-        {
-            "x": 1.4603574724576953,
-            "y": 3.47711225483097,
-            "heading": -2.8974629563063408,
-            "angularVelocity": -3.7041041196117577e-7,
-            "velocityX": 0.3699191394562724,
-            "velocityY": -3.7648101008720767,
-            "timestamp": 1.1031992472933203
-        },
-        {
-            "x": 1.4996282585911056,
-            "y": 3.2915246350140723,
-            "heading": -2.897463288823342,
-            "angularVelocity": -0.000006631054220103306,
-            "velocityX": 0.7831380388037097,
-            "velocityY": -3.7009884170870753,
-            "timestamp": 1.153344667624835
-        },
-        {
-            "x": 1.5575649679963268,
-            "y": 3.1148801932866275,
-            "heading": -2.9095242713797083,
-            "angularVelocity": -0.24052012081323423,
-            "velocityX": 1.1553738910193128,
-            "velocityY": -3.5226435546783237,
-            "timestamp": 1.2034900879563495
-        },
-        {
-            "x": 1.6317457698728626,
-            "y": 2.9507691275120527,
-            "heading": -2.937069238173179,
-            "angularVelocity": -0.549301743037936,
-            "velocityX": 1.4793135920712512,
-            "velocityY": -3.2727029644905956,
-            "timestamp": 1.253635508287864
-        },
-        {
-            "x": 1.7203254363653924,
-            "y": 2.801423061648199,
-            "heading": -2.9780621265122655,
-            "angularVelocity": -0.8174802019421967,
-            "velocityX": 1.7664557582112876,
-            "velocityY": -2.9782593281004814,
-            "timestamp": 1.3037809286193787
-        },
-        {
-            "x": 1.821600608747065,
-            "y": 2.668597377909185,
-            "heading": -3.0274133717272194,
-            "angularVelocity": -0.9841625593860135,
-            "velocityX": 2.019629543677879,
-            "velocityY": -2.648809858625076,
-            "timestamp": 1.3539263489508933
-        },
-        {
-            "x": 1.9344033083274008,
-            "y": 2.553226314276784,
-            "heading": -3.081880110008977,
-            "angularVelocity": -1.0861757249549924,
-            "velocityX": 2.2495114974525996,
-            "velocityY": -2.300729814800146,
-            "timestamp": 1.4040717692824078
-        },
-        {
-            "x": 2.057902367887511,
-            "y": 2.4558701645332204,
-            "heading": -3.139236203843767,
-            "angularVelocity": -1.1437952549924524,
-            "velocityX": 2.4628183140883095,
-            "velocityY": -1.9414763920600462,
-            "timestamp": 1.4542171896139224
-        },
-        {
-            "x": 2.1914827823638916,
-            "y": 2.376894474029541,
-            "heading": -3.1978794252899205,
-            "angularVelocity": -1.1694631545305318,
-            "velocityX": 2.663860699407278,
-            "velocityY": -1.5749332637270959,
-            "timestamp": 1.504362609945437
-        },
-        {
-            "x": 2.278019971764992,
-            "y": 2.3350837736524683,
-            "heading": -3.2342809833466872,
-            "angularVelocity": -1.1705395923861364,
-            "velocityX": 2.7827162301626456,
-            "velocityY": -1.3444776209968852,
-            "timestamp": 1.535460710593682
-        },
-        {
-            "x": 2.368131283216315,
-            "y": 2.300501166585177,
-            "heading": -3.270450856694399,
-            "angularVelocity": -1.1630894682876274,
-            "velocityX": 2.8976467878404213,
-            "velocityY": -1.112048850135951,
-            "timestamp": 1.5665588112419269
-        },
-        {
-            "x": 2.461670879436653,
-            "y": 2.2732156435859956,
-            "heading": -3.306075840058796,
-            "angularVelocity": -1.1455678199567971,
-            "velocityX": 3.007887757467237,
-            "velocityY": -0.8774015914287735,
-            "timestamp": 1.5976569118901718
-        },
-        {
-            "x": 2.5584617966487637,
-            "y": 2.2533048266167017,
-            "heading": -3.3407803732359653,
-            "angularVelocity": -1.1159695432761514,
-            "velocityX": 3.112438225952366,
-            "velocityY": -0.6402582972673794,
-            "timestamp": 1.6287550125384167
-        },
-        {
-            "x": 2.658285404214264,
-            "y": 2.240855747544563,
-            "heading": -3.37410574490051,
-            "angularVelocity": -1.0716208054472351,
-            "velocityX": 3.209958341013189,
-            "velocityY": -0.40031637986360386,
-            "timestamp": 1.6598531131866616
-        },
-        {
-            "x": 2.7608658878900645,
-            "y": 2.2359648026671843,
-            "heading": -3.405478910014421,
-            "angularVelocity": -1.0088450567700136,
-            "velocityX": 3.2986092892329157,
-            "velocityY": -0.1572747137422285,
-            "timestamp": 1.6909512138349065
-        },
-        {
-            "x": 2.8658468441128373,
-            "y": 2.238735383081805,
-            "heading": -3.4341645997548764,
-            "angularVelocity": -0.9224257797905216,
-            "velocityX": 3.3757996158745374,
-            "velocityY": 0.08909162800513673,
-            "timestamp": 1.7220493144831515
-        },
-        {
-            "x": 2.972755430801249,
-            "y": 2.249269494915596,
-            "heading": -3.459191954885324,
-            "angularVelocity": -0.8047872573808069,
-            "velocityX": 3.4377850884743864,
-            "velocityY": 0.3387381098589673,
-            "timestamp": 1.7531474151313964
-        },
-        {
-            "x": 3.0809481205317057,
-            "y": 2.267644679606423,
-            "heading": -3.4792519494186864,
-            "angularVelocity": -0.6450552964717784,
-            "velocityX": 3.4790770971590295,
-            "velocityY": 0.5908780378155715,
-            "timestamp": 1.7842455157796413
-        },
-        {
-            "x": 3.1896989899339894,
-            "y": 2.293908495297157,
-            "heading": -3.4936234234211128,
-            "angularVelocity": -0.4621334969934767,
-            "velocityX": 3.4970260927630066,
-            "velocityY": 0.8445472599052618,
-            "timestamp": 1.8153436164278862
-        },
-        {
-            "x": 3.298236220991847,
-            "y": 2.3281164444820805,
-            "heading": -3.5030903221852148,
-            "angularVelocity": -0.30442048121156556,
-            "velocityX": 3.4901562730643,
-            "velocityY": 1.1000012371126273,
-            "timestamp": 1.8464417170761311
-        },
-        {
-            "x": 3.405452310949792,
-            "y": 2.3702201329887522,
-            "heading": -3.5091052020695845,
-            "angularVelocity": -0.1934163102887616,
-            "velocityX": 3.447673257305325,
-            "velocityY": 1.3538990365653494,
-            "timestamp": 1.877539817724376
-        },
-        {
-            "x": 3.5097674187642585,
-            "y": 2.419844212041495,
-            "heading": -3.514308835252089,
-            "angularVelocity": -0.1673296141576457,
-            "velocityX": 3.354388391573783,
-            "velocityY": 1.5957270064190303,
-            "timestamp": 1.908637918372621
-        },
-        {
-            "x": 3.6091747208997034,
-            "y": 2.4757866931575614,
-            "heading": -3.523108085575375,
-            "angularVelocity": -0.2829513745169274,
-            "velocityX": 3.196571496756511,
-            "velocityY": 1.7989034683770408,
-            "timestamp": 1.939736019020866
-        },
-        {
-            "x": 3.701528766735335,
-            "y": 2.535299893209444,
-            "heading": -3.5376794547025563,
-            "angularVelocity": -0.4685613855325914,
-            "velocityX": 2.969764838060721,
-            "velocityY": 1.913724594471066,
-            "timestamp": 1.9708341196691108
-        },
-        {
-            "x": 3.7858722644846305,
-            "y": 2.59510740434591,
-            "heading": -3.555385401955535,
-            "angularVelocity": -0.5693578348478764,
-            "velocityX": 2.7121752129918284,
-            "velocityY": 1.923188551383177,
-            "timestamp": 2.001932220317356
-        },
-        {
-            "x": 3.862373728468494,
-            "y": 2.653074903297687,
-            "heading": -3.5744288715420867,
-            "angularVelocity": -0.6123676105476805,
-            "velocityX": 2.460004385772028,
-            "velocityY": 1.8640205589227477,
-            "timestamp": 2.033030320965601
-        },
-        {
-            "x": 3.931417292290283,
-            "y": 2.707976512433947,
-            "heading": -3.5936932545527167,
-            "angularVelocity": -0.6194713699243786,
-            "velocityX": 2.220185875746878,
-            "velocityY": 1.7654328718419663,
-            "timestamp": 2.064128421613846
-        },
-        {
-            "x": 3.9933519727900295,
-            "y": 2.7590743829972735,
-            "heading": -3.612463672222599,
-            "angularVelocity": -0.6035872699166261,
-            "velocityX": 1.991590457574803,
-            "velocityY": 1.643118695295981,
-            "timestamp": 2.0952265222620907
-        },
-        {
-            "x": 4.04845853802061,
-            "y": 2.8058913864006936,
-            "heading": -3.6302579260771157,
-            "angularVelocity": -0.5721974488337824,
-            "velocityX": 1.7720235024607334,
-            "velocityY": 1.5054618265268112,
-            "timestamp": 2.1263246229103356
-        },
-        {
-            "x": 4.096959063236219,
-            "y": 2.8480989028223083,
-            "heading": -3.64673270512686,
-            "angularVelocity": -0.5297680149691044,
-            "velocityX": 1.5595976668866187,
-            "velocityY": 1.3572377586345543,
-            "timestamp": 2.1574227235585806
-        },
-        {
-            "x": 4.139030722594851,
-            "y": 2.8854588640311274,
-            "heading": -3.6616322031984536,
-            "angularVelocity": -0.4791127998494546,
-            "velocityX": 1.352869097521714,
-            "velocityY": 1.2013582961674927,
-            "timestamp": 2.1885208242068255
-        },
-        {
-            "x": 4.174817002989322,
-            "y": 2.917791762586731,
-            "heading": -3.6747587890471625,
-            "angularVelocity": -0.4221024942061285,
-            "velocityX": 1.150754536402508,
-            "velocityY": 1.0397065377505372,
-            "timestamp": 2.2196189248550704
-        },
-        {
-            "x": 4.204435949248215,
-            "y": 2.9449578523068656,
-            "heading": -3.68595534704571,
-            "angularVelocity": -0.360039930579135,
-            "velocityX": 0.9524358607593557,
-            "velocityY": 0.8735610585165674,
-            "timestamp": 2.2507170255033153
-        },
-        {
-            "x": 4.227986113342503,
-            "y": 2.9668454867866734,
-            "heading": -3.6950940909562924,
-            "angularVelocity": -0.2938682337530664,
-            "velocityX": 0.7572862523234876,
-            "velocityY": 0.7038254434694367,
-            "timestamp": 2.2818151261515602
-        },
-        {
-            "x": 4.245550869753413,
-            "y": 2.983363547472538,
-            "heading": -3.702069156081572,
-            "angularVelocity": -0.22429231946250391,
-            "velocityX": 0.5648176591100355,
-            "velocityY": 0.5311597924485464,
-            "timestamp": 2.312913226799805
-        },
-        {
-            "x": 4.25720158771329,
-            "y": 2.99443633378999,
-            "heading": -3.7067915044128146,
-            "angularVelocity": -0.15185327183332542,
-            "velocityX": 0.3746440366779525,
-            "velocityY": 0.3560598906890862,
-            "timestamp": 2.34401132744805
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -3.7091853071795864,
-            "angularVelocity": -0.07697585115715987,
-            "velocityX": 0.1864555122609252,
-            "velocityY": 0.17890694589168776,
-            "timestamp": 2.375109428096295
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -3.7091853071795864,
-            "angularVelocity": 7.12380386376873e-29,
-            "velocityX": -3.695914984045945e-30,
-            "velocityY": 3.729410348522699e-30,
-            "timestamp": 2.40620752874454
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.traj
deleted file mode 100644
index 96fad438..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.traj	
+++ /dev/null
@@ -1,544 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -7.419385593511551e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 2.293773644442264,
-            "y": 6.737033643205664,
-            "heading": -2.75526766821854,
-            "angularVelocity": -0.10504784252909477,
-            "velocityX": -0.1241659859778435,
-            "velocityY": -0.3981690982414253,
-            "timestamp": 0.050145420331514534
-        },
-        {
-            "x": 2.2813193646388377,
-            "y": 6.697095621507987,
-            "heading": -2.7655160845701583,
-            "angularVelocity": -0.20437392455513897,
-            "velocityX": -0.24836325473174134,
-            "velocityY": -0.7964440507955471,
-            "timestamp": 0.10029084066302907
-        },
-        {
-            "x": 2.2626352666399625,
-            "y": 6.637179497704236,
-            "heading": -2.7803732946846287,
-            "angularVelocity": -0.2962824923243045,
-            "velocityX": -0.3725982926327797,
-            "velocityY": -1.1948473740501462,
-            "timestamp": 0.1504362609945436
-        },
-        {
-            "x": 2.2377190441074863,
-            "y": 6.557277385764274,
-            "heading": -2.799337789044294,
-            "angularVelocity": -0.3781899570148323,
-            "velocityX": -0.4968793235305074,
-            "velocityY": -1.5934079605220988,
-            "timestamp": 0.20058168132605814
-        },
-        {
-            "x": 2.2065679165597203,
-            "y": 6.457379637066711,
-            "heading": -2.8216967909576067,
-            "angularVelocity": -0.44588322852808987,
-            "velocityX": -0.6212158027956237,
-            "velocityY": -1.9921609598071555,
-            "timestamp": 0.25072710165757267
-        },
-        {
-            "x": 2.1691787676944525,
-            "y": 6.337475004416767,
-            "heading": -2.846358557659031,
-            "angularVelocity": -0.49180496520688655,
-            "velocityX": -0.7456144273611754,
-            "velocityY": -2.3911382506567254,
-            "timestamp": 0.3008725219890872
-        },
-        {
-            "x": 2.1255494213188793,
-            "y": 6.197554115260485,
-            "heading": -2.8714458003624506,
-            "angularVelocity": -0.5002898078741331,
-            "velocityX": -0.8700564495648131,
-            "velocityY": -2.7903024489824935,
-            "timestamp": 0.35101794232060174
-        },
-        {
-            "x": 2.0756883391523315,
-            "y": 6.037638592000563,
-            "heading": -2.8929930081362487,
-            "angularVelocity": -0.4296944293486671,
-            "velocityX": -0.9943297281568805,
-            "velocityY": -3.1890354533417224,
-            "timestamp": 0.4011633626521163
-        },
-        {
-            "x": 2.0198170203632526,
-            "y": 5.858213938792853,
-            "heading": -2.897462778158027,
-            "angularVelocity": -0.08913615624774217,
-            "velocityX": -1.1141858702092737,
-            "velocityY": -3.578086533556236,
-            "timestamp": 0.4513087829836308
-        },
-        {
-            "x": 1.9633339655295539,
-            "y": 5.677120968466723,
-            "heading": -2.897462791142581,
-            "angularVelocity": -2.5893798802100903e-7,
-            "velocityX": -1.1263851107496088,
-            "velocityY": -3.611356114454979,
-            "timestamp": 0.5014542033151453
-        },
-        {
-            "x": 1.9068509267595308,
-            "y": 5.496027993130308,
-            "heading": -2.897462804127121,
-            "angularVelocity": -2.5893770297899326e-7,
-            "velocityX": -1.1263847904077713,
-            "velocityY": -3.611356214370084,
-            "timestamp": 0.5515996236466598
-        },
-        {
-            "x": 1.8503679083113405,
-            "y": 5.314935011455497,
-            "heading": -2.897462817111663,
-            "angularVelocity": -2.589377492115138e-7,
-            "velocityX": -1.1263843851497735,
-            "velocityY": -3.611356340770398,
-            "timestamp": 0.6017450439781744
-        },
-        {
-            "x": 1.7938849123636174,
-            "y": 5.133842022762774,
-            "heading": -2.8974628300962064,
-            "angularVelocity": -2.589377769209582e-7,
-            "velocityX": -1.1263839364454482,
-            "velocityY": -3.6113564807215806,
-            "timestamp": 0.651890464309689
-        },
-        {
-            "x": 1.737401940953919,
-            "y": 4.952749026416629,
-            "heading": -2.89746284308075,
-            "angularVelocity": -2.5893777760216377e-7,
-            "velocityX": -1.126383447108141,
-            "velocityY": -3.611356633346137,
-            "timestamp": 0.7020358846412036
-        },
-        {
-            "x": 1.680918995692082,
-            "y": 4.771656021914957,
-            "heading": -2.897462856065292,
-            "angularVelocity": -2.5893774300527816e-7,
-            "velocityX": -1.1263829256674796,
-            "velocityY": -3.6113567959836606,
-            "timestamp": 0.7521813049727182
-        },
-        {
-            "x": 1.6244360774524895,
-            "y": 4.590563008985036,
-            "heading": -2.8974628690498303,
-            "angularVelocity": -2.589376688920553e-7,
-            "velocityX": -1.1263823867898586,
-            "velocityY": -3.611356964059789,
-            "timestamp": 0.8023267253042328
-        },
-        {
-            "x": 1.567953186210211,
-            "y": 4.40946998763471,
-            "heading": -2.897462882034363,
-            "angularVelocity": -2.589375607053774e-7,
-            "velocityX": -1.1263818484094146,
-            "velocityY": -3.6113571319795557,
-            "timestamp": 0.8524721456357474
-        },
-        {
-            "x": 1.5114708900262606,
-            "y": 4.228376780686611,
-            "heading": -2.8974628950188928,
-            "angularVelocity": -2.589374956200649e-7,
-            "velocityX": -1.1263699817559003,
-            "velocityY": -3.611360833170398,
-            "timestamp": 0.902617565967262
-        },
-        {
-            "x": 1.4675217320832292,
-            "y": 4.043840955415942,
-            "heading": -2.8974629081797687,
-            "angularVelocity": -2.624542007539591e-7,
-            "velocityX": -0.8764341320200526,
-            "velocityY": -3.6800135296641385,
-            "timestamp": 0.9527629862987765
-        },
-        {
-            "x": 1.4442056001491934,
-            "y": 3.855582205665926,
-            "heading": -2.8974629221278954,
-            "angularVelocity": -2.7815354458428775e-7,
-            "velocityX": -0.4649703159309848,
-            "velocityY": -3.75425609169144,
-            "timestamp": 1.0029084066302911
-        },
-        {
-            "x": 1.4418077217209881,
-            "y": 3.665900239807532,
-            "heading": -2.897462937731955,
-            "angularVelocity": -3.1117616956346287e-7,
-            "velocityX": -0.04781849294218802,
-            "velocityY": -3.782637868112273,
-            "timestamp": 1.0530538269618057
-        },
-        {
-            "x": 1.4603574724576953,
-            "y": 3.47711225483097,
-            "heading": -2.8974629563063408,
-            "angularVelocity": -3.7041041196117577e-7,
-            "velocityX": 0.3699191394562724,
-            "velocityY": -3.7648101008720767,
-            "timestamp": 1.1031992472933203
-        },
-        {
-            "x": 1.4996282585911056,
-            "y": 3.2915246350140723,
-            "heading": -2.897463288823342,
-            "angularVelocity": -0.000006631054220103306,
-            "velocityX": 0.7831380388037097,
-            "velocityY": -3.7009884170870753,
-            "timestamp": 1.153344667624835
-        },
-        {
-            "x": 1.5575649679963268,
-            "y": 3.1148801932866275,
-            "heading": -2.9095242713797083,
-            "angularVelocity": -0.24052012081323423,
-            "velocityX": 1.1553738910193128,
-            "velocityY": -3.5226435546783237,
-            "timestamp": 1.2034900879563495
-        },
-        {
-            "x": 1.6317457698728626,
-            "y": 2.9507691275120527,
-            "heading": -2.937069238173179,
-            "angularVelocity": -0.549301743037936,
-            "velocityX": 1.4793135920712512,
-            "velocityY": -3.2727029644905956,
-            "timestamp": 1.253635508287864
-        },
-        {
-            "x": 1.7203254363653924,
-            "y": 2.801423061648199,
-            "heading": -2.9780621265122655,
-            "angularVelocity": -0.8174802019421967,
-            "velocityX": 1.7664557582112876,
-            "velocityY": -2.9782593281004814,
-            "timestamp": 1.3037809286193787
-        },
-        {
-            "x": 1.821600608747065,
-            "y": 2.668597377909185,
-            "heading": -3.0274133717272194,
-            "angularVelocity": -0.9841625593860135,
-            "velocityX": 2.019629543677879,
-            "velocityY": -2.648809858625076,
-            "timestamp": 1.3539263489508933
-        },
-        {
-            "x": 1.9344033083274008,
-            "y": 2.553226314276784,
-            "heading": -3.081880110008977,
-            "angularVelocity": -1.0861757249549924,
-            "velocityX": 2.2495114974525996,
-            "velocityY": -2.300729814800146,
-            "timestamp": 1.4040717692824078
-        },
-        {
-            "x": 2.057902367887511,
-            "y": 2.4558701645332204,
-            "heading": -3.139236203843767,
-            "angularVelocity": -1.1437952549924524,
-            "velocityX": 2.4628183140883095,
-            "velocityY": -1.9414763920600462,
-            "timestamp": 1.4542171896139224
-        },
-        {
-            "x": 2.1914827823638916,
-            "y": 2.376894474029541,
-            "heading": -3.1978794252899205,
-            "angularVelocity": -1.1694631545305318,
-            "velocityX": 2.663860699407278,
-            "velocityY": -1.5749332637270959,
-            "timestamp": 1.504362609945437
-        },
-        {
-            "x": 2.278019971764992,
-            "y": 2.3350837736524683,
-            "heading": -3.2342809833466872,
-            "angularVelocity": -1.1705395923861364,
-            "velocityX": 2.7827162301626456,
-            "velocityY": -1.3444776209968852,
-            "timestamp": 1.535460710593682
-        },
-        {
-            "x": 2.368131283216315,
-            "y": 2.300501166585177,
-            "heading": -3.270450856694399,
-            "angularVelocity": -1.1630894682876274,
-            "velocityX": 2.8976467878404213,
-            "velocityY": -1.112048850135951,
-            "timestamp": 1.5665588112419269
-        },
-        {
-            "x": 2.461670879436653,
-            "y": 2.2732156435859956,
-            "heading": -3.306075840058796,
-            "angularVelocity": -1.1455678199567971,
-            "velocityX": 3.007887757467237,
-            "velocityY": -0.8774015914287735,
-            "timestamp": 1.5976569118901718
-        },
-        {
-            "x": 2.5584617966487637,
-            "y": 2.2533048266167017,
-            "heading": -3.3407803732359653,
-            "angularVelocity": -1.1159695432761514,
-            "velocityX": 3.112438225952366,
-            "velocityY": -0.6402582972673794,
-            "timestamp": 1.6287550125384167
-        },
-        {
-            "x": 2.658285404214264,
-            "y": 2.240855747544563,
-            "heading": -3.37410574490051,
-            "angularVelocity": -1.0716208054472351,
-            "velocityX": 3.209958341013189,
-            "velocityY": -0.40031637986360386,
-            "timestamp": 1.6598531131866616
-        },
-        {
-            "x": 2.7608658878900645,
-            "y": 2.2359648026671843,
-            "heading": -3.405478910014421,
-            "angularVelocity": -1.0088450567700136,
-            "velocityX": 3.2986092892329157,
-            "velocityY": -0.1572747137422285,
-            "timestamp": 1.6909512138349065
-        },
-        {
-            "x": 2.8658468441128373,
-            "y": 2.238735383081805,
-            "heading": -3.4341645997548764,
-            "angularVelocity": -0.9224257797905216,
-            "velocityX": 3.3757996158745374,
-            "velocityY": 0.08909162800513673,
-            "timestamp": 1.7220493144831515
-        },
-        {
-            "x": 2.972755430801249,
-            "y": 2.249269494915596,
-            "heading": -3.459191954885324,
-            "angularVelocity": -0.8047872573808069,
-            "velocityX": 3.4377850884743864,
-            "velocityY": 0.3387381098589673,
-            "timestamp": 1.7531474151313964
-        },
-        {
-            "x": 3.0809481205317057,
-            "y": 2.267644679606423,
-            "heading": -3.4792519494186864,
-            "angularVelocity": -0.6450552964717784,
-            "velocityX": 3.4790770971590295,
-            "velocityY": 0.5908780378155715,
-            "timestamp": 1.7842455157796413
-        },
-        {
-            "x": 3.1896989899339894,
-            "y": 2.293908495297157,
-            "heading": -3.4936234234211128,
-            "angularVelocity": -0.4621334969934767,
-            "velocityX": 3.4970260927630066,
-            "velocityY": 0.8445472599052618,
-            "timestamp": 1.8153436164278862
-        },
-        {
-            "x": 3.298236220991847,
-            "y": 2.3281164444820805,
-            "heading": -3.5030903221852148,
-            "angularVelocity": -0.30442048121156556,
-            "velocityX": 3.4901562730643,
-            "velocityY": 1.1000012371126273,
-            "timestamp": 1.8464417170761311
-        },
-        {
-            "x": 3.405452310949792,
-            "y": 2.3702201329887522,
-            "heading": -3.5091052020695845,
-            "angularVelocity": -0.1934163102887616,
-            "velocityX": 3.447673257305325,
-            "velocityY": 1.3538990365653494,
-            "timestamp": 1.877539817724376
-        },
-        {
-            "x": 3.5097674187642585,
-            "y": 2.419844212041495,
-            "heading": -3.514308835252089,
-            "angularVelocity": -0.1673296141576457,
-            "velocityX": 3.354388391573783,
-            "velocityY": 1.5957270064190303,
-            "timestamp": 1.908637918372621
-        },
-        {
-            "x": 3.6091747208997034,
-            "y": 2.4757866931575614,
-            "heading": -3.523108085575375,
-            "angularVelocity": -0.2829513745169274,
-            "velocityX": 3.196571496756511,
-            "velocityY": 1.7989034683770408,
-            "timestamp": 1.939736019020866
-        },
-        {
-            "x": 3.701528766735335,
-            "y": 2.535299893209444,
-            "heading": -3.5376794547025563,
-            "angularVelocity": -0.4685613855325914,
-            "velocityX": 2.969764838060721,
-            "velocityY": 1.913724594471066,
-            "timestamp": 1.9708341196691108
-        },
-        {
-            "x": 3.7858722644846305,
-            "y": 2.59510740434591,
-            "heading": -3.555385401955535,
-            "angularVelocity": -0.5693578348478764,
-            "velocityX": 2.7121752129918284,
-            "velocityY": 1.923188551383177,
-            "timestamp": 2.001932220317356
-        },
-        {
-            "x": 3.862373728468494,
-            "y": 2.653074903297687,
-            "heading": -3.5744288715420867,
-            "angularVelocity": -0.6123676105476805,
-            "velocityX": 2.460004385772028,
-            "velocityY": 1.8640205589227477,
-            "timestamp": 2.033030320965601
-        },
-        {
-            "x": 3.931417292290283,
-            "y": 2.707976512433947,
-            "heading": -3.5936932545527167,
-            "angularVelocity": -0.6194713699243786,
-            "velocityX": 2.220185875746878,
-            "velocityY": 1.7654328718419663,
-            "timestamp": 2.064128421613846
-        },
-        {
-            "x": 3.9933519727900295,
-            "y": 2.7590743829972735,
-            "heading": -3.612463672222599,
-            "angularVelocity": -0.6035872699166261,
-            "velocityX": 1.991590457574803,
-            "velocityY": 1.643118695295981,
-            "timestamp": 2.0952265222620907
-        },
-        {
-            "x": 4.04845853802061,
-            "y": 2.8058913864006936,
-            "heading": -3.6302579260771157,
-            "angularVelocity": -0.5721974488337824,
-            "velocityX": 1.7720235024607334,
-            "velocityY": 1.5054618265268112,
-            "timestamp": 2.1263246229103356
-        },
-        {
-            "x": 4.096959063236219,
-            "y": 2.8480989028223083,
-            "heading": -3.64673270512686,
-            "angularVelocity": -0.5297680149691044,
-            "velocityX": 1.5595976668866187,
-            "velocityY": 1.3572377586345543,
-            "timestamp": 2.1574227235585806
-        },
-        {
-            "x": 4.139030722594851,
-            "y": 2.8854588640311274,
-            "heading": -3.6616322031984536,
-            "angularVelocity": -0.4791127998494546,
-            "velocityX": 1.352869097521714,
-            "velocityY": 1.2013582961674927,
-            "timestamp": 2.1885208242068255
-        },
-        {
-            "x": 4.174817002989322,
-            "y": 2.917791762586731,
-            "heading": -3.6747587890471625,
-            "angularVelocity": -0.4221024942061285,
-            "velocityX": 1.150754536402508,
-            "velocityY": 1.0397065377505372,
-            "timestamp": 2.2196189248550704
-        },
-        {
-            "x": 4.204435949248215,
-            "y": 2.9449578523068656,
-            "heading": -3.68595534704571,
-            "angularVelocity": -0.360039930579135,
-            "velocityX": 0.9524358607593557,
-            "velocityY": 0.8735610585165674,
-            "timestamp": 2.2507170255033153
-        },
-        {
-            "x": 4.227986113342503,
-            "y": 2.9668454867866734,
-            "heading": -3.6950940909562924,
-            "angularVelocity": -0.2938682337530664,
-            "velocityX": 0.7572862523234876,
-            "velocityY": 0.7038254434694367,
-            "timestamp": 2.2818151261515602
-        },
-        {
-            "x": 4.245550869753413,
-            "y": 2.983363547472538,
-            "heading": -3.702069156081572,
-            "angularVelocity": -0.22429231946250391,
-            "velocityX": 0.5648176591100355,
-            "velocityY": 0.5311597924485464,
-            "timestamp": 2.312913226799805
-        },
-        {
-            "x": 4.25720158771329,
-            "y": 2.99443633378999,
-            "heading": -3.7067915044128146,
-            "angularVelocity": -0.15185327183332542,
-            "velocityX": 0.3746440366779525,
-            "velocityY": 0.3560598906890862,
-            "timestamp": 2.34401132744805
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -3.7091853071795864,
-            "angularVelocity": -0.07697585115715987,
-            "velocityX": 0.1864555122609252,
-            "velocityY": 0.17890694589168776,
-            "timestamp": 2.375109428096295
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -3.7091853071795864,
-            "angularVelocity": 7.12380386376873e-29,
-            "velocityX": -3.695914984045945e-30,
-            "velocityY": 3.729410348522699e-30,
-            "timestamp": 2.40620752874454
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.1.traj
index 40c9a79c..1ef15c59 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.1.traj	
@@ -3,560 +3,560 @@
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": -2.75,
+            "heading": 0.39,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": -1.7629358050488447e-33,
+            "velocityY": 9.203343680058695e-35,
             "timestamp": 0
         },
         {
-            "x": 2.2980097531788157,
-            "y": 6.74279910356074,
-            "heading": -2.762722782141496,
-            "angularVelocity": -0.3022479537187966,
-            "velocityX": -0.047281170298139656,
-            "velocityY": -0.3373626807410066,
-            "timestamp": 0.04209385699707004
-        },
-        {
-            "x": 2.2940230158560544,
-            "y": 6.7143715511474085,
-            "heading": -2.7877986560948775,
-            "angularVelocity": -0.5957133829557797,
-            "velocityX": -0.09471066818701897,
-            "velocityY": -0.6753373162100508,
-            "timestamp": 0.08418771399414007
-        },
-        {
-            "x": 2.2880322840288483,
-            "y": 6.671686949257674,
-            "heading": -2.8247754513459067,
-            "angularVelocity": -0.8784368525222962,
-            "velocityX": -0.14231843443624606,
-            "velocityY": -1.0140339929578401,
-            "timestamp": 0.12628157099121012
-        },
-        {
-            "x": 2.2800276497015446,
-            "y": 6.614707345454749,
-            "heading": -2.873051459668216,
-            "angularVelocity": -1.1468658794006534,
-            "velocityX": -0.19016157934542763,
-            "velocityY": -1.353632284323334,
-            "timestamp": 0.16837542798828015
-        },
-        {
-            "x": 2.269996639934327,
-            "y": 6.543383180902105,
-            "heading": -2.9317676826482244,
-            "angularVelocity": -1.3948881658455714,
-            "velocityX": -0.23830103684524737,
-            "velocityY": -1.694407916993876,
-            "timestamp": 0.21046928498535017
-        },
-        {
-            "x": 2.2579259581538498,
-            "y": 6.457648474415292,
-            "heading": -2.9996334069438224,
-            "angularVelocity": -1.6122477039897383,
-            "velocityX": -0.28675637353255107,
-            "velocityY": -2.0367510274190597,
-            "timestamp": 0.2525631419824202
-        },
-        {
-            "x": 2.2438060571207714,
-            "y": 6.357417527376881,
-            "heading": -3.0746299268265957,
-            "angularVelocity": -1.7816499896408666,
-            "velocityX": -0.3354385185957493,
-            "velocityY": -2.3811300315242536,
-            "timestamp": 0.2946569989794902
-        },
-        {
-            "x": 2.227639514948982,
-            "y": 6.242593213913433,
-            "heading": -3.1534176716951676,
-            "angularVelocity": -1.8717159815993158,
-            "velocityX": -0.3840594168625274,
-            "velocityY": -2.727816400180199,
-            "timestamp": 0.33675085597656024
-        },
-        {
-            "x": 2.2094628615660015,
-            "y": 6.1131506228921415,
-            "heading": -3.2295701531339893,
-            "angularVelocity": -1.8091115158233875,
-            "velocityX": -0.43181249426122753,
-            "velocityY": -3.07509456855668,
-            "timestamp": 0.37884471297363026
-        },
-        {
-            "x": 2.189573480043473,
-            "y": 5.97059741661809,
-            "heading": -3.280208967362098,
-            "angularVelocity": -1.2029977255739333,
-            "velocityX": -0.4725008098904457,
-            "velocityY": -3.386556054580001,
-            "timestamp": 0.4209385699707003
-        },
-        {
-            "x": 2.167754438435126,
-            "y": 5.816653814130607,
-            "heading": -3.2929329955032975,
-            "angularVelocity": -0.3022775542304222,
-            "velocityX": -0.5183426553158416,
-            "velocityY": -3.657151267896363,
-            "timestamp": 0.4630324269677703
-        },
-        {
-            "x": 2.1447655867486377,
-            "y": 5.659083438978025,
-            "heading": -3.2929330394086493,
-            "angularVelocity": -0.0000010430346734701328,
-            "velocityX": -0.5461331730206714,
-            "velocityY": -3.74331045890019,
-            "timestamp": 0.5051262839648404
-        },
-        {
-            "x": 2.1217767257990308,
-            "y": 5.50151306517681,
-            "heading": -3.2929330833137715,
-            "angularVelocity": -0.0000010430292064009232,
-            "velocityX": -0.5461333930793576,
-            "velocityY": -3.7433104267965445,
-            "timestamp": 0.5472201409619104
-        },
-        {
-            "x": 2.098787889918124,
-            "y": 5.3439426877181715,
-            "heading": -3.292933127218878,
-            "angularVelocity": -0.0000010430288283584586,
-            "velocityX": -0.5461327975363938,
-            "velocityY": -3.7433105136838924,
-            "timestamp": 0.5893139979589804
-        },
-        {
-            "x": 2.075799081922294,
-            "y": 5.186372306191217,
-            "heading": -3.292933171123966,
-            "angularVelocity": -0.000001043028392528962,
-            "velocityX": -0.5461321350863673,
-            "velocityY": -3.7433106103325895,
-            "timestamp": 0.6314078549560505
-        },
-        {
-            "x": 2.052810304885704,
-            "y": 5.028801920147444,
-            "heading": -3.292933215029033,
-            "angularVelocity": -0.0000010430278884550004,
-            "velocityX": -0.5461313996051773,
-            "velocityY": -3.7433107176360743,
-            "timestamp": 0.6735017119531205
-        },
-        {
-            "x": 2.029821561935122,
-            "y": 4.8712315291306805,
-            "heading": -3.2929332589340747,
-            "angularVelocity": -0.0000010430273082107483,
-            "velocityX": -0.546130589843121,
-            "velocityY": -3.7433108357766267,
-            "timestamp": 0.7155955689501905
-        },
-        {
-            "x": 2.0068328558598987,
-            "y": 4.71366113273398,
-            "heading": -3.29293330283909,
-            "angularVelocity": -0.0000010430266595669302,
-            "velocityX": -0.5461297138160472,
-            "velocityY": -3.7433109635847206,
-            "timestamp": 0.7576894259472605
-        },
-        {
-            "x": 1.9838441886370295,
-            "y": 4.556090730668921,
-            "heading": -3.2929333467440762,
-            "angularVelocity": -0.0000010430259761326365,
-            "velocityX": -0.5461287908225987,
-            "velocityY": -3.7433110982447326,
-            "timestamp": 0.7997832829443305
-        },
-        {
-            "x": 1.960855561483052,
-            "y": 4.398520322758029,
-            "heading": -3.2929333906490403,
-            "angularVelocity": -0.0000010430254501600513,
-            "velocityX": -0.5461278389285532,
-            "velocityY": -3.7433112371209067,
-            "timestamp": 0.8418771399414006
-        },
-        {
-            "x": 1.9378696268054536,
-            "y": 4.240949522053656,
-            "heading": -3.2929334345540515,
-            "angularVelocity": -0.0000010430265803117889,
-            "velocityX": -0.5460638752870397,
-            "velocityY": -3.7433205684938873,
-            "timestamp": 0.8839709969384706
-        },
-        {
-            "x": 1.9275923145038192,
-            "y": 4.0820429860952725,
-            "heading": -3.292933478993091,
-            "angularVelocity": -0.0000010557131812143582,
-            "velocityX": -0.24415230712523545,
-            "velocityY": -3.7750528769422123,
-            "timestamp": 0.9260648539355406
-        },
-        {
-            "x": 1.9320863630886849,
-            "y": 3.9228678829456647,
-            "heading": -3.29293352534017,
-            "angularVelocity": -0.0000011010413859861484,
-            "velocityX": 0.10676257547932101,
-            "velocityY": -3.781433076106252,
-            "timestamp": 0.9681587109326106
-        },
-        {
-            "x": 1.9513134143780595,
-            "y": 3.764794388276326,
-            "heading": -3.2929335753664444,
-            "angularVelocity": -0.0000011884459622031428,
-            "velocityX": 0.4567662044063322,
-            "velocityY": -3.75526278526441,
-            "timestamp": 1.0102525679296808
-        },
-        {
-            "x": 1.9851080356036048,
-            "y": 3.609183233024402,
-            "heading": -3.292933631555695,
-            "angularVelocity": -0.000001334856299032327,
-            "velocityX": 0.8028397404376079,
-            "velocityY": -3.696766377639273,
-            "timestamp": 1.052346424926751
-        },
-        {
-            "x": 2.033179344139022,
-            "y": 3.457373959395419,
-            "heading": -3.2929336978163843,
-            "angularVelocity": -0.000001574117798079096,
-            "velocityX": 1.142002942110131,
-            "velocityY": -3.6064472219675583,
-            "timestamp": 1.094440281923821
-        },
-        {
-            "x": 2.095113543139509,
-            "y": 3.310673385447386,
-            "heading": -3.29293378107099,
-            "angularVelocity": -0.0000019778326800211228,
-            "velocityX": 1.4713358057162051,
-            "velocityY": -3.485082727350082,
-            "timestamp": 1.1365341389208912
+            "x": 2.298008161759761,
+            "y": 6.742798044155305,
+            "heading": 0.37730350882287184,
+            "angularVelocity": -0.30162955292479776,
+            "velocityX": -0.04731994607959398,
+            "velocityY": -0.3373947598852993,
+            "timestamp": 0.0420929947149233
+        },
+        {
+            "x": 2.2940179124306126,
+            "y": 6.714368498753073,
+            "heading": 0.3522801711626307,
+            "angularVelocity": -0.5944774856175671,
+            "velocityX": -0.09479604281359731,
+            "velocityY": -0.6753984978919232,
+            "timestamp": 0.0841859894298466
+        },
+        {
+            "x": 2.28802134748838,
+            "y": 6.671681118579196,
+            "heading": 0.31538217591497186,
+            "angularVelocity": -0.8765828019021266,
+            "velocityX": -0.14245992671333493,
+            "velocityY": -1.014120769096589,
+            "timestamp": 0.1262789841447699
+        },
+        {
+            "x": 2.2800080756588827,
+            "y": 6.61469814393844,
+            "heading": 0.2672110996394858,
+            "angularVelocity": -1.1443965106718308,
+            "velocityX": -0.19037067530517543,
+            "velocityY": -1.3537400944426796,
+            "timestamp": 0.1683719788596932
+        },
+        {
+            "x": 2.2699650128963897,
+            "y": 6.543370292056319,
+            "heading": 0.20862519634351256,
+            "angularVelocity": -1.391820745773707,
+            "velocityX": -0.23859226055333851,
+            "velocityY": -1.694530226827328,
+            "timestamp": 0.21046497357461652
+        },
+        {
+            "x": 2.2578780077181495,
+            "y": 6.4576319980717,
+            "heading": 0.14091301714440063,
+            "angularVelocity": -1.608632972248606,
+            "velocityX": -0.2871500414760241,
+            "velocityY": -2.036877978516017,
+            "timestamp": 0.2525579682895398
+        },
+        {
+            "x": 2.2437361567314555,
+            "y": 6.357398182016401,
+            "heading": 0.06608849726258184,
+            "angularVelocity": -1.7776002964049293,
+            "velocityX": -0.3359668534508049,
+            "velocityY": -2.3812469683883926,
+            "timestamp": 0.2946509630044631
+        },
+        {
+            "x": 2.227539619778089,
+            "y": 6.242572554004172,
+            "heading": -0.012517898946656567,
+            "angularVelocity": -1.8674460380308828,
+            "velocityX": -0.3847798680768185,
+            "velocityY": -2.727903509595609,
+            "timestamp": 0.33674395771938637
+        },
+        {
+            "x": 2.209319968778144,
+            "y": 6.1131309245806085,
+            "heading": -0.08849823738903302,
+            "angularVelocity": -1.8050589880087344,
+            "velocityX": -0.4328428310539146,
+            "velocityY": -3.0751347177888695,
+            "timestamp": 0.37883695243430965
+        },
+        {
+            "x": 2.189363625025874,
+            "y": 5.970570114356374,
+            "heading": -0.13907359717832485,
+            "angularVelocity": -1.201514887021361,
+            "velocityX": -0.4741013056311528,
+            "velocityY": -3.38680607520881,
+            "timestamp": 0.42092994714923293
+        },
+        {
+            "x": 2.1674827409245396,
+            "y": 5.816618965425972,
+            "heading": -0.15173248317406665,
+            "angularVelocity": -0.3007361695568274,
+            "velocityX": -0.5198224609468644,
+            "velocityY": -3.657405465518488,
+            "timestamp": 0.4630229418641562
+        },
+        {
+            "x": 2.1445157307124236,
+            "y": 5.659048701686038,
+            "heading": -0.1517325271793822,
+            "angularVelocity": -0.0000010454308573871989,
+            "velocityX": -0.5456254744443967,
+            "velocityY": -3.7433844944292183,
+            "timestamp": 0.5051159365790795
+        },
+        {
+            "x": 2.1215487153797565,
+            "y": 5.501478438692377,
+            "heading": -0.1517325711844689,
+            "angularVelocity": -0.0000010454254199880688,
+            "velocityX": -0.5456255960929445,
+            "velocityY": -3.7433844767000393,
+            "timestamp": 0.5472089312940028
+        },
+        {
+            "x": 2.0985817251133763,
+            "y": 5.343908172045118,
+            "heading": -0.15173261518953962,
+            "angularVelocity": -0.000001045425040533604,
+            "velocityX": -0.5456250005951166,
+            "velocityY": -3.7433845634983034,
+            "timestamp": 0.5893019260089262
+        },
+        {
+            "x": 2.075614762728362,
+            "y": 5.186337901333946,
+            "heading": -0.15173265919459192,
+            "angularVelocity": -0.0000010454246033549915,
+            "velocityX": -0.5456243382196686,
+            "velocityY": -3.743384660044355,
+            "timestamp": 0.6313949207238495
+        },
+        {
+            "x": 2.0526478312972185,
+            "y": 5.028767626111028,
+            "heading": -0.15173270319962293,
+            "angularVelocity": -0.0000010454240970480152,
+            "velocityX": -0.5456236028509798,
+            "velocityY": -3.7433847672295513,
+            "timestamp": 0.6734879154387728
+        },
+        {
+            "x": 2.0296809339445536,
+            "y": 4.871197345920939,
+            "heading": -0.15173274720462945,
+            "angularVelocity": -0.0000010454235162635837,
+            "velocityX": -0.5456227932512114,
+            "velocityY": -3.743384885234266,
+            "timestamp": 0.7155809101536962
+        },
+        {
+            "x": 2.0067140734575584,
+            "y": 4.7136270603574335,
+            "heading": -0.15173279120960867,
+            "angularVelocity": -0.0000010454228670132247,
+            "velocityX": -0.5456219174363699,
+            "velocityY": -3.7433850128900636,
+            "timestamp": 0.7576739048686195
+        },
+        {
+            "x": 1.983747251812091,
+            "y": 4.55605676913253,
+            "heading": -0.15173283521455908,
+            "angularVelocity": -0.000001045422182833939,
+            "velocityX": -0.545620994681206,
+            "velocityY": -3.743385147387475,
+            "timestamp": 0.7997668995835429
+        },
+        {
+            "x": 1.9607804702256346,
+            "y": 4.398486472068785,
+            "heading": -0.15173287921948736,
+            "angularVelocity": -0.0000010454216570158517,
+            "velocityX": -0.5456200430024101,
+            "velocityY": -3.7433852861004024,
+            "timestamp": 0.8418598942984662
+        },
+        {
+            "x": 1.9378163633559966,
+            "y": 4.240915785172817,
+            "heading": -0.15173292322446258,
+            "angularVelocity": -0.0000010454227717672262,
+            "velocityX": -0.5455564999630786,
+            "velocityY": -3.7433945473141725,
+            "timestamp": 0.8839528890133895
+        },
+        {
+            "x": 1.9275487572437193,
+            "y": 4.082011890541997,
+            "heading": -0.15173296776197548,
+            "angularVelocity": -0.000001058074228463605,
+            "velocityX": -0.2439267194414494,
+            "velocityY": -3.775067459728259,
+            "timestamp": 0.9260458837283129
+        },
+        {
+            "x": 1.9320519221515333,
+            "y": 3.9228403083205765,
+            "heading": -0.15173301420750987,
+            "angularVelocity": -0.0000011034029462076059,
+            "velocityX": 0.10698133830372927,
+            "velocityY": -3.781426892988182,
+            "timestamp": 0.9681388784432362
+        },
+        {
+            "x": 1.9512874233679367,
+            "y": 3.764771127717043,
+            "heading": -0.15173306433513473,
+            "angularVelocity": -0.0000011908780826817618,
+            "velocityX": 0.45697630559851177,
+            "velocityY": -3.7552372235348,
+            "timestamp": 1.0102318731581594
+        },
+        {
+            "x": 1.9850897622274482,
+            "y": 3.609164986787025,
+            "heading": -0.15173312063283023,
+            "angularVelocity": -0.0000013374599716310608,
+            "velocityX": 0.8030395339756486,
+            "velocityY": -3.6967229816711127,
+            "timestamp": 1.0523248678730828
+        },
+        {
+            "x": 2.0331680016062417,
+            "y": 3.4573613296965586,
+            "heading": -0.1517331870158403,
+            "angularVelocity": -0.0000015770560044658274,
+            "velocityX": 1.142190991741159,
+            "velocityY": -3.606387668982997,
+            "timestamp": 1.094417862588006
+        },
+        {
+            "x": 2.0951083019579966,
+            "y": 3.3106668726225337,
+            "heading": -0.15173327041919607,
+            "angularVelocity": -0.0000019814070334134687,
+            "velocityX": 1.4715108956074094,
+            "velocityY": -3.4850088017619067,
+            "timestamp": 1.1365108573029294
         },
         {
             "x": 2.170377492904663,
             "y": 3.170344352722168,
-            "heading": -3.292933888363931,
-            "angularVelocity": -0.00000254889782501678,
-            "velocityX": 1.7880031704006887,
-            "velocityY": -3.3337176190574715,
-            "timestamp": 1.1786279959179613
-        },
-        {
-            "x": 2.2421588223512248,
-            "y": 3.0583154434355615,
-            "heading": -3.2929339938603395,
-            "angularVelocity": -0.000002999460293233564,
-            "velocityX": 2.0408775039380207,
-            "velocityY": -3.1851915047625172,
-            "timestamp": 1.2137997930042903
-        },
-        {
-            "x": 2.3224028083906,
-            "y": 2.952183724176483,
-            "heading": -3.2929340733758186,
-            "angularVelocity": -0.000002260773842517862,
-            "velocityX": 2.2814866650804433,
-            "velocityY": -3.0175233582344014,
-            "timestamp": 1.2489715900906193
-        },
-        {
-            "x": 2.4106271869517015,
-            "y": 2.852587036133821,
-            "heading": -3.292934137641087,
-            "angularVelocity": -0.0000018271818267107102,
-            "velocityX": 2.508384156332853,
-            "velocityY": -2.8317201932617277,
-            "timestamp": 1.2841433871769483
-        },
-        {
-            "x": 2.506301732241236,
-            "y": 2.7601239405733753,
-            "heading": -3.29293419233114,
-            "angularVelocity": -0.0000015549405448920558,
-            "velocityX": 2.720206336193231,
-            "velocityY": -2.628898811553339,
-            "timestamp": 1.3193151842632773
-        },
-        {
-            "x": 2.6088514418801845,
-            "y": 2.6753501198214895,
-            "heading": -3.292934240814094,
-            "angularVelocity": -0.0000013784610795710298,
-            "velocityX": 2.9156801225493263,
-            "velocityY": -2.410278341587403,
-            "timestamp": 1.3544869813496063
-        },
-        {
-            "x": 2.717659989787404,
-            "y": 2.5987750311541573,
-            "heading": -3.29293428528508,
-            "angularVelocity": -0.0000012643933456255718,
-            "velocityX": 3.093630605230363,
-            "velocityY": -2.177173048035584,
-            "timestamp": 1.3896587784359353
-        },
-        {
-            "x": 2.8320734217114962,
-            "y": 2.5308588229326605,
-            "heading": -3.2929343273033043,
-            "angularVelocity": -0.0000011946567396881703,
-            "velocityX": 3.252987944951041,
-            "velocityY": -1.9309848756034937,
-            "timestamp": 1.4248305755222643
-        },
-        {
-            "x": 2.951404044154876,
-            "y": 2.472009459742262,
-            "heading": -3.29293436807755,
-            "angularVelocity": -0.000001159288093356335,
-            "velocityX": 3.392792871813832,
-            "velocityY": -1.6731975066827869,
-            "timestamp": 1.4600023726085933
-        },
-        {
-            "x": 3.0749338856232122,
-            "y": 2.4225784637746113,
-            "heading": -3.2929344086367345,
-            "angularVelocity": -0.0000011531735020269185,
-            "velocityX": 3.512184525719075,
-            "velocityY": -1.4054157041314157,
-            "timestamp": 1.4951741696949223
-        },
-        {
-            "x": 3.199149697823942,
-            "y": 2.374897470692157,
-            "heading": -3.2929344492220305,
-            "angularVelocity": -0.000001153915896405561,
-            "velocityX": 3.531687957139154,
-            "velocityY": -1.3556598477303206,
-            "timestamp": 1.5303459667812513
-        },
-        {
-            "x": 3.3233655700450013,
-            "y": 2.3272166339715414,
-            "heading": -3.2929344898073354,
-            "angularVelocity": -0.0000011539161555742895,
-            "velocityX": 3.5316896636294053,
-            "velocityY": -1.3556554020706744,
-            "timestamp": 1.5655177638675803
-        },
-        {
-            "x": 3.4475814520083037,
-            "y": 2.279535822630966,
-            "heading": -3.2929345303926483,
-            "angularVelocity": -0.0000011539163817435631,
-            "velocityX": 3.531689940619614,
-            "velocityY": -1.3556546804686425,
-            "timestamp": 1.6006895609539094
-        },
-        {
-            "x": 3.571797343695941,
-            "y": 2.231855036623794,
-            "heading": -3.2929345709779727,
-            "angularVelocity": -0.0000011539167138358367,
-            "velocityX": 3.53169021710065,
-            "velocityY": -1.3556539601925897,
-            "timestamp": 1.6358613580402384
-        },
-        {
-            "x": 3.696013245125145,
-            "y": 2.1841742759949323,
-            "heading": -3.2929346115633122,
-            "angularVelocity": -0.0000011539171376230348,
-            "velocityX": 3.531690494071608,
-            "velocityY": -1.355653238639736,
-            "timestamp": 1.6710331551265674
-        },
-        {
-            "x": 3.8202291563227684,
-            "y": 2.1364935408143535,
-            "heading": -3.292934652148666,
-            "angularVelocity": -0.0000011539175501305985,
-            "velocityX": 3.531690771806052,
-            "velocityY": -1.3556525150974317,
-            "timestamp": 1.7062049522128964
-        },
-        {
-            "x": 3.9444450773804367,
-            "y": 2.0888128313207726,
-            "heading": -3.292934692734033,
-            "angularVelocity": -0.0000011539179284682577,
-            "velocityX": 3.5316910521455807,
-            "velocityY": -1.355651784768035,
-            "timestamp": 1.7413767492992254
-        },
-        {
-            "x": 4.068661008518651,
-            "y": 2.041132148088645,
-            "heading": -3.292934733319412,
-            "angularVelocity": -0.0000011539182628370223,
-            "velocityX": 3.531691338754367,
-            "velocityY": -1.3556510381057705,
-            "timestamp": 1.7765485463855544
-        },
-        {
-            "x": 4.192876950169643,
-            "y": 1.9934514922440194,
-            "heading": -3.2929347739048014,
-            "angularVelocity": -0.0000011539185452860982,
-            "velocityX": 3.5316916376522904,
-            "velocityY": -1.3556502594278441,
-            "timestamp": 1.8117203434718834
-        },
-        {
-            "x": 4.317092949197956,
-            "y": 1.945770985877502,
-            "heading": -3.292934814490671,
-            "angularVelocity": -0.0000011539322132251563,
-            "velocityX": 3.5316932689969183,
-            "velocityY": -1.3556460094855505,
-            "timestamp": 1.8468921405582124
-        },
-        {
-            "x": 4.4381547246220405,
-            "y": 1.900646588966398,
-            "heading": -3.304224160857315,
-            "angularVelocity": -0.3209772403421673,
-            "velocityX": 3.442012790160775,
-            "velocityY": -1.2829710350126964,
-            "timestamp": 1.8820639376445414
-        },
-        {
-            "x": 4.551299156991204,
-            "y": 1.858106592529644,
-            "heading": -3.340115682679576,
-            "angularVelocity": -1.0204631208967,
-            "velocityX": 3.2169079132195413,
-            "velocityY": -1.2094916939370357,
-            "timestamp": 1.9172357347308704
-        },
-        {
-            "x": 4.6559354752004225,
-            "y": 1.8184604535185338,
-            "heading": -3.3958506942974975,
-            "angularVelocity": -1.5846506643126916,
-            "velocityX": 2.9750063083893172,
-            "velocityY": -1.127213912721008,
-            "timestamp": 1.9524075318171994
-        },
-        {
-            "x": 4.750941844820644,
-            "y": 1.7823637508813854,
-            "heading": -3.4552749718768117,
-            "angularVelocity": -1.6895433984637673,
-            "velocityX": 2.7012088517123067,
-            "velocityY": -1.0262967953712765,
-            "timestamp": 1.9875793289035284
-        },
-        {
-            "x": 4.836288167632038,
-            "y": 1.7498748942497964,
-            "heading": -3.5138218394398413,
-            "angularVelocity": -1.664596990006722,
-            "velocityX": 2.4265556463296942,
-            "velocityY": -0.9237189829068263,
-            "timestamp": 2.0227511259898576
-        },
-        {
-            "x": 4.9120167698263195,
-            "y": 1.7210024537073532,
-            "heading": -3.5691368807810755,
-            "angularVelocity": -1.57271012355337,
-            "velocityX": 2.153105853772738,
-            "velocityY": -0.820897506930789,
-            "timestamp": 2.0579229230761866
-        },
-        {
-            "x": 4.9781740579406755,
-            "y": 1.695744953740728,
-            "heading": -3.6197442140788145,
-            "angularVelocity": -1.438861175433386,
-            "velocityX": 1.8809754858977845,
-            "velocityY": -0.718117982559453,
-            "timestamp": 2.0930947201625156
-        },
-        {
-            "x": 5.034800882324283,
-            "y": 1.6740995356540391,
-            "heading": -3.6646343712421663,
-            "angularVelocity": -1.2763111607055304,
-            "velocityX": 1.6100065698837365,
-            "velocityY": -0.6154197362608602,
-            "timestamp": 2.1282665172488446
-        },
-        {
-            "x": 5.081931555960545,
-            "y": 1.6560639982794174,
-            "heading": -3.703081117816373,
-            "angularVelocity": -1.0931129415946466,
-            "velocityX": 1.3400132361897832,
-            "velocityY": -0.5127840732833052,
-            "timestamp": 2.1634383143351736
-        },
-        {
-            "x": 5.11959462557562,
-            "y": 1.64163698634227,
-            "heading": -3.734541789094988,
-            "angularVelocity": -0.8944857495167222,
-            "velocityX": 1.0708315393333863,
-            "velocityY": -0.41018694329825167,
-            "timestamp": 2.1986101114215026
-        },
-        {
-            "x": 5.1478139750030305,
-            "y": 1.6308177057465072,
-            "heading": -3.758596048910276,
-            "angularVelocity": -0.6839076137124058,
-            "velocityX": 0.8023289045523005,
-            "velocityY": -0.30761239095081433,
-            "timestamp": 2.2337819085078316
-        },
-        {
-            "x": 5.166609893194281,
-            "y": 1.6236055874501054,
-            "heading": -3.7749054122717336,
-            "angularVelocity": -0.4637057162995195,
-            "velocityX": 0.5344031226245449,
-            "velocityY": -0.20505401753284183,
-            "timestamp": 2.2689537055941607
+            "heading": -0.1517333779044128,
+            "angularVelocity": -0.000002553517929494838,
+            "velocityX": 1.7881643122906952,
+            "velocityY": -3.333631186155945,
+            "timestamp": 1.1786038520178528
+        },
+        {
+            "x": 2.2421653916580504,
+            "y": 3.058317386598826,
+            "heading": -0.1517334835913584,
+            "angularVelocity": -0.0000030048345176781924,
+            "velocityX": 2.041035009880172,
+            "velocityY": -3.1850905776457417,
+            "timestamp": 1.213776153524627
+        },
+        {
+            "x": 2.322415894828655,
+            "y": 2.9521882028356305,
+            "heading": -0.15173356325163515,
+            "angularVelocity": -0.000002264858236060329,
+            "velocityX": 2.2816392369190814,
+            "velocityY": -3.0174079948323826,
+            "timestamp": 1.2489484550314012
+        },
+        {
+            "x": 2.4106466853387967,
+            "y": 2.8525946456824203,
+            "heading": -0.1517336276365959,
+            "angularVelocity": -0.0000018305586497777263,
+            "velocityX": 2.5085304836576707,
+            "velocityY": -2.8315905666289245,
+            "timestamp": 1.2841207565381754
+        },
+        {
+            "x": 2.5063274836460994,
+            "y": 2.7601352747554455,
+            "heading": -0.15173368243148194,
+            "angularVelocity": -0.0000015578987915956282,
+            "velocityX": 2.720345107040385,
+            "velocityY": -2.628755212654101,
+            "timestamp": 1.3192930580449496
+        },
+        {
+            "x": 2.6088832332920924,
+            "y": 2.67536576592715,
+            "heading": -0.15173373101035148,
+            "angularVelocity": -0.000001381168347773627,
+            "velocityX": 2.915810033819959,
+            "velocityY": -2.410121180496788,
+            "timestamp": 1.3544653595517238
+        },
+        {
+            "x": 2.7176975542058304,
+            "y": 2.598795565150263,
+            "heading": -0.1517337755722331,
+            "angularVelocity": -0.0000012669595030854418,
+            "velocityX": 3.0937503732242226,
+            "velocityY": -2.1770028544233733,
+            "timestamp": 1.389637661058498
+        },
+        {
+            "x": 2.8321164386506434,
+            "y": 2.5308848045360426,
+            "heading": -0.15173381767917685,
+            "angularVelocity": -0.0000011971620259359605,
+            "velocityX": 3.2530963156555406,
+            "velocityY": -1.930802299108592,
+            "timestamp": 1.4248099625652721
+        },
+        {
+            "x": 2.951452140465328,
+            "y": 2.4720414272198683,
+            "heading": -0.15173385854226368,
+            "angularVelocity": -0.0000011617973536146762,
+            "velocityX": 3.3928886283344455,
+            "velocityY": -1.6730033235055968,
+            "timestamp": 1.4599822640720463
+        },
+        {
+            "x": 3.0749866313948626,
+            "y": 2.4226169148084327,
+            "heading": -0.151733899192453,
+            "angularVelocity": -0.000001155744366153098,
+            "velocityX": 3.512266347021429,
+            "velocityY": -1.4052112114959712,
+            "timestamp": 1.4951545655788205
+        },
+        {
+            "x": 3.1992028583958065,
+            "y": 2.3749316777596947,
+            "heading": -0.1517339398689439,
+            "angularVelocity": -0.0000011564921588237654,
+            "velocityX": 3.5316491011263524,
+            "velocityY": -1.3557610678264387,
+            "timestamp": 1.5303268670855947
+        },
+        {
+            "x": 3.3234191450837622,
+            "y": 2.3272465961911335,
+            "heading": -0.15173398054544393,
+            "angularVelocity": -0.0000011564924185481092,
+            "velocityX": 3.5316507981154315,
+            "velocityY": -1.355756647297499,
+            "timestamp": 1.565499168592369
+        },
+        {
+            "x": 3.4476354415156054,
+            "y": 2.2795615400047207,
+            "heading": -0.15173402122195195,
+            "angularVelocity": -0.0000011564926460273957,
+            "velocityX": 3.5316510751484063,
+            "velocityY": -1.3557559256458895,
+            "timestamp": 1.6006714700991431
+        },
+        {
+            "x": 3.57185174767356,
+            "y": 2.231876509154167,
+            "heading": -0.15173406189847174,
+            "angularVelocity": -0.0000011564929795008858,
+            "velocityX": 3.531651351675977,
+            "velocityY": -1.355755205310334,
+            "timestamp": 1.6358437716059173
+        },
+        {
+            "x": 3.6960680635749434,
+            "y": 2.1841915036846027,
+            "heading": -0.15173410257500647,
+            "angularVelocity": -0.000001156493404477995,
+            "velocityX": 3.5316516286959314,
+            "velocityY": -1.3557544836916666,
+            "timestamp": 1.6710160731126915
+        },
+        {
+            "x": 3.820284389246338,
+            "y": 2.136506523665285,
+            "heading": -0.15173414325155568,
+            "angularVelocity": -0.0000011564938164652249,
+            "velocityX": 3.5316519064716276,
+            "velocityY": -1.3557537601039011,
+            "timestamp": 1.7061883746194657
+        },
+        {
+            "x": 3.9445007247793735,
+            "y": 2.0888215693349235,
+            "heading": -0.15173418392811813,
+            "angularVelocity": -0.0000011564941935950735,
+            "velocityX": 3.5316521868525257,
+            "velocityY": -1.3557530297293134,
+            "timestamp": 1.74136067612624
+        },
+        {
+            "x": 4.068717070394662,
+            "y": 2.0411366412682117,
+            "heading": -0.1517342246046924,
+            "angularVelocity": -0.0000011564945294151802,
+            "velocityX": 3.531652473505735,
+            "velocityY": -1.3557522830153055,
+            "timestamp": 1.776532977633014
+        },
+        {
+            "x": 4.192933426524707,
+            "y": 1.9934517405918115,
+            "heading": -0.15173426528127656,
+            "angularVelocity": -0.0000011564948108738022,
+            "velocityX": 3.5316527724556495,
+            "velocityY": -1.3557515042686648,
+            "timestamp": 1.8117052791397883
+        },
+        {
+            "x": 4.3171498402859925,
+            "y": 1.9457669900421575,
+            "heading": -0.1517343059583456,
+            "angularVelocity": -0.0000011565085966842365,
+            "velocityX": 3.531654410996122,
+            "velocityY": -1.3557472359455842,
+            "timestamp": 1.8468775806465625
+        },
+        {
+            "x": 4.438199866372425,
+            "y": 1.9006463371040783,
+            "heading": -0.16306820061092758,
+            "angularVelocity": -0.3222392100329015,
+            "velocityX": 3.441629375976964,
+            "velocityY": -1.282846188765579,
+            "timestamp": 1.8820498821533367
+        },
+        {
+            "x": 4.551333178258229,
+            "y": 1.8581105785728176,
+            "heading": -0.19901052601489952,
+            "angularVelocity": -1.0218929061849813,
+            "velocityX": 3.2165456066050515,
+            "velocityY": -1.2093538582645278,
+            "timestamp": 1.9172221836601109
+        },
+        {
+            "x": 4.6559660625940325,
+            "y": 1.8184646011809935,
+            "heading": -0.2548690023306322,
+            "angularVelocity": -1.5881382202121255,
+            "velocityX": 2.9748660125539748,
+            "velocityY": -1.1271931518097666,
+            "timestamp": 1.952394485166885
+        },
+        {
+            "x": 4.750968691238499,
+            "y": 1.7823676048635404,
+            "heading": -0.3144227593810797,
+            "angularVelocity": -1.693200458860429,
+            "velocityX": 2.7010637511499986,
+            "velocityY": -1.0262904265875403,
+            "timestamp": 1.9875667866736593
+        },
+        {
+            "x": 4.836310937621718,
+            "y": 1.749878315803868,
+            "heading": -0.37309459063768413,
+            "angularVelocity": -1.6681260180060764,
+            "velocityX": 2.426404947278819,
+            "velocityY": -0.9237180300360781,
+            "timestamp": 2.0227390881804337
+        },
+        {
+            "x": 4.912035358987092,
+            "y": 1.7210053742684595,
+            "heading": -0.4285254423369764,
+            "angularVelocity": -1.5759802266171352,
+            "velocityX": 2.1529561081121917,
+            "velocityY": -0.8208999780650579,
+            "timestamp": 2.057911389687208
+        },
+        {
+            "x": 4.978188578205751,
+            "y": 1.6957473341798728,
+            "heading": -0.4792367761737124,
+            "angularVelocity": -1.4417974276425698,
+            "velocityX": 1.8808328253957978,
+            "velocityY": -0.71812304019178,
+            "timestamp": 2.0930836911939825
+        },
+        {
+            "x": 5.034811621143831,
+            "y": 1.6741013653395114,
+            "heading": -0.5242175587960242,
+            "angularVelocity": -1.278869471013956,
+            "velocityX": 1.6098759680874073,
+            "velocityY": -0.6154265690060781,
+            "timestamp": 2.128255992700757
+        },
+        {
+            "x": 5.081938937252143,
+            "y": 1.656065298518901,
+            "heading": -0.5627406592667319,
+            "angularVelocity": -1.0952681178195878,
+            "velocityX": 1.3398985590759445,
+            "velocityY": -0.5127917721601607,
+            "timestamp": 2.1634282942075314
+        },
+        {
+            "x": 5.11959917834615,
+            "y": 1.6416378108353973,
+            "heading": -0.5942629145071194,
+            "angularVelocity": -0.8962238434785516,
+            "velocityX": 1.0707357631047747,
+            "velocityY": -0.41019458680362747,
+            "timestamp": 2.1986005957143058
+        },
+        {
+            "x": 5.147816310703936,
+            "y": 1.63081813815951,
+            "heading": -0.6183637008514038,
+            "angularVelocity": -0.6852206228143011,
+            "velocityX": 0.8022543634897267,
+            "velocityY": -0.3076191267666608,
+            "timestamp": 2.23377289722108
+        },
+        {
+            "x": 5.1666106911988186,
+            "y": 1.6236057376407542,
+            "heading": -0.6347043261398958,
+            "angularVelocity": -0.4645878884367218,
+            "velocityX": 0.5343517395716112,
+            "velocityY": -0.20505910076332604,
+            "timestamp": 2.2689451987278546
         },
         {
             "x": 5.176,
             "y": 1.62,
-            "heading": -3.7831853071795862,
-            "angularVelocity": -0.2354129044793956,
-            "velocityX": 0.2669783060180454,
-            "velocityY": -0.10251359750698616,
-            "timestamp": 2.3041255026804897
+            "heading": -0.643,
+            "angularVelocity": -0.23585814702817923,
+            "velocityX": 0.26695178873562564,
+            "velocityY": -0.10251639745723456,
+            "timestamp": 2.304117500234629
         },
         {
             "x": 5.176,
             "y": 1.62,
-            "heading": -3.7831853071795862,
-            "angularVelocity": -9.348588807340857e-32,
-            "velocityX": 3.3761299345246814e-33,
-            "velocityY": 7.633245142283867e-33,
-            "timestamp": 2.3392972997668187
+            "heading": -0.643,
+            "angularVelocity": -2.979902278450704e-33,
+            "velocityX": -7.596111021745797e-35,
+            "velocityY": -9.748244176889607e-35,
+            "timestamp": 2.3392898017414034
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.traj
index 40c9a79c..1ef15c59 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.traj	
@@ -3,560 +3,560 @@
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": -2.75,
+            "heading": 0.39,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": -1.7629358050488447e-33,
+            "velocityY": 9.203343680058695e-35,
             "timestamp": 0
         },
         {
-            "x": 2.2980097531788157,
-            "y": 6.74279910356074,
-            "heading": -2.762722782141496,
-            "angularVelocity": -0.3022479537187966,
-            "velocityX": -0.047281170298139656,
-            "velocityY": -0.3373626807410066,
-            "timestamp": 0.04209385699707004
-        },
-        {
-            "x": 2.2940230158560544,
-            "y": 6.7143715511474085,
-            "heading": -2.7877986560948775,
-            "angularVelocity": -0.5957133829557797,
-            "velocityX": -0.09471066818701897,
-            "velocityY": -0.6753373162100508,
-            "timestamp": 0.08418771399414007
-        },
-        {
-            "x": 2.2880322840288483,
-            "y": 6.671686949257674,
-            "heading": -2.8247754513459067,
-            "angularVelocity": -0.8784368525222962,
-            "velocityX": -0.14231843443624606,
-            "velocityY": -1.0140339929578401,
-            "timestamp": 0.12628157099121012
-        },
-        {
-            "x": 2.2800276497015446,
-            "y": 6.614707345454749,
-            "heading": -2.873051459668216,
-            "angularVelocity": -1.1468658794006534,
-            "velocityX": -0.19016157934542763,
-            "velocityY": -1.353632284323334,
-            "timestamp": 0.16837542798828015
-        },
-        {
-            "x": 2.269996639934327,
-            "y": 6.543383180902105,
-            "heading": -2.9317676826482244,
-            "angularVelocity": -1.3948881658455714,
-            "velocityX": -0.23830103684524737,
-            "velocityY": -1.694407916993876,
-            "timestamp": 0.21046928498535017
-        },
-        {
-            "x": 2.2579259581538498,
-            "y": 6.457648474415292,
-            "heading": -2.9996334069438224,
-            "angularVelocity": -1.6122477039897383,
-            "velocityX": -0.28675637353255107,
-            "velocityY": -2.0367510274190597,
-            "timestamp": 0.2525631419824202
-        },
-        {
-            "x": 2.2438060571207714,
-            "y": 6.357417527376881,
-            "heading": -3.0746299268265957,
-            "angularVelocity": -1.7816499896408666,
-            "velocityX": -0.3354385185957493,
-            "velocityY": -2.3811300315242536,
-            "timestamp": 0.2946569989794902
-        },
-        {
-            "x": 2.227639514948982,
-            "y": 6.242593213913433,
-            "heading": -3.1534176716951676,
-            "angularVelocity": -1.8717159815993158,
-            "velocityX": -0.3840594168625274,
-            "velocityY": -2.727816400180199,
-            "timestamp": 0.33675085597656024
-        },
-        {
-            "x": 2.2094628615660015,
-            "y": 6.1131506228921415,
-            "heading": -3.2295701531339893,
-            "angularVelocity": -1.8091115158233875,
-            "velocityX": -0.43181249426122753,
-            "velocityY": -3.07509456855668,
-            "timestamp": 0.37884471297363026
-        },
-        {
-            "x": 2.189573480043473,
-            "y": 5.97059741661809,
-            "heading": -3.280208967362098,
-            "angularVelocity": -1.2029977255739333,
-            "velocityX": -0.4725008098904457,
-            "velocityY": -3.386556054580001,
-            "timestamp": 0.4209385699707003
-        },
-        {
-            "x": 2.167754438435126,
-            "y": 5.816653814130607,
-            "heading": -3.2929329955032975,
-            "angularVelocity": -0.3022775542304222,
-            "velocityX": -0.5183426553158416,
-            "velocityY": -3.657151267896363,
-            "timestamp": 0.4630324269677703
-        },
-        {
-            "x": 2.1447655867486377,
-            "y": 5.659083438978025,
-            "heading": -3.2929330394086493,
-            "angularVelocity": -0.0000010430346734701328,
-            "velocityX": -0.5461331730206714,
-            "velocityY": -3.74331045890019,
-            "timestamp": 0.5051262839648404
-        },
-        {
-            "x": 2.1217767257990308,
-            "y": 5.50151306517681,
-            "heading": -3.2929330833137715,
-            "angularVelocity": -0.0000010430292064009232,
-            "velocityX": -0.5461333930793576,
-            "velocityY": -3.7433104267965445,
-            "timestamp": 0.5472201409619104
-        },
-        {
-            "x": 2.098787889918124,
-            "y": 5.3439426877181715,
-            "heading": -3.292933127218878,
-            "angularVelocity": -0.0000010430288283584586,
-            "velocityX": -0.5461327975363938,
-            "velocityY": -3.7433105136838924,
-            "timestamp": 0.5893139979589804
-        },
-        {
-            "x": 2.075799081922294,
-            "y": 5.186372306191217,
-            "heading": -3.292933171123966,
-            "angularVelocity": -0.000001043028392528962,
-            "velocityX": -0.5461321350863673,
-            "velocityY": -3.7433106103325895,
-            "timestamp": 0.6314078549560505
-        },
-        {
-            "x": 2.052810304885704,
-            "y": 5.028801920147444,
-            "heading": -3.292933215029033,
-            "angularVelocity": -0.0000010430278884550004,
-            "velocityX": -0.5461313996051773,
-            "velocityY": -3.7433107176360743,
-            "timestamp": 0.6735017119531205
-        },
-        {
-            "x": 2.029821561935122,
-            "y": 4.8712315291306805,
-            "heading": -3.2929332589340747,
-            "angularVelocity": -0.0000010430273082107483,
-            "velocityX": -0.546130589843121,
-            "velocityY": -3.7433108357766267,
-            "timestamp": 0.7155955689501905
-        },
-        {
-            "x": 2.0068328558598987,
-            "y": 4.71366113273398,
-            "heading": -3.29293330283909,
-            "angularVelocity": -0.0000010430266595669302,
-            "velocityX": -0.5461297138160472,
-            "velocityY": -3.7433109635847206,
-            "timestamp": 0.7576894259472605
-        },
-        {
-            "x": 1.9838441886370295,
-            "y": 4.556090730668921,
-            "heading": -3.2929333467440762,
-            "angularVelocity": -0.0000010430259761326365,
-            "velocityX": -0.5461287908225987,
-            "velocityY": -3.7433110982447326,
-            "timestamp": 0.7997832829443305
-        },
-        {
-            "x": 1.960855561483052,
-            "y": 4.398520322758029,
-            "heading": -3.2929333906490403,
-            "angularVelocity": -0.0000010430254501600513,
-            "velocityX": -0.5461278389285532,
-            "velocityY": -3.7433112371209067,
-            "timestamp": 0.8418771399414006
-        },
-        {
-            "x": 1.9378696268054536,
-            "y": 4.240949522053656,
-            "heading": -3.2929334345540515,
-            "angularVelocity": -0.0000010430265803117889,
-            "velocityX": -0.5460638752870397,
-            "velocityY": -3.7433205684938873,
-            "timestamp": 0.8839709969384706
-        },
-        {
-            "x": 1.9275923145038192,
-            "y": 4.0820429860952725,
-            "heading": -3.292933478993091,
-            "angularVelocity": -0.0000010557131812143582,
-            "velocityX": -0.24415230712523545,
-            "velocityY": -3.7750528769422123,
-            "timestamp": 0.9260648539355406
-        },
-        {
-            "x": 1.9320863630886849,
-            "y": 3.9228678829456647,
-            "heading": -3.29293352534017,
-            "angularVelocity": -0.0000011010413859861484,
-            "velocityX": 0.10676257547932101,
-            "velocityY": -3.781433076106252,
-            "timestamp": 0.9681587109326106
-        },
-        {
-            "x": 1.9513134143780595,
-            "y": 3.764794388276326,
-            "heading": -3.2929335753664444,
-            "angularVelocity": -0.0000011884459622031428,
-            "velocityX": 0.4567662044063322,
-            "velocityY": -3.75526278526441,
-            "timestamp": 1.0102525679296808
-        },
-        {
-            "x": 1.9851080356036048,
-            "y": 3.609183233024402,
-            "heading": -3.292933631555695,
-            "angularVelocity": -0.000001334856299032327,
-            "velocityX": 0.8028397404376079,
-            "velocityY": -3.696766377639273,
-            "timestamp": 1.052346424926751
-        },
-        {
-            "x": 2.033179344139022,
-            "y": 3.457373959395419,
-            "heading": -3.2929336978163843,
-            "angularVelocity": -0.000001574117798079096,
-            "velocityX": 1.142002942110131,
-            "velocityY": -3.6064472219675583,
-            "timestamp": 1.094440281923821
-        },
-        {
-            "x": 2.095113543139509,
-            "y": 3.310673385447386,
-            "heading": -3.29293378107099,
-            "angularVelocity": -0.0000019778326800211228,
-            "velocityX": 1.4713358057162051,
-            "velocityY": -3.485082727350082,
-            "timestamp": 1.1365341389208912
+            "x": 2.298008161759761,
+            "y": 6.742798044155305,
+            "heading": 0.37730350882287184,
+            "angularVelocity": -0.30162955292479776,
+            "velocityX": -0.04731994607959398,
+            "velocityY": -0.3373947598852993,
+            "timestamp": 0.0420929947149233
+        },
+        {
+            "x": 2.2940179124306126,
+            "y": 6.714368498753073,
+            "heading": 0.3522801711626307,
+            "angularVelocity": -0.5944774856175671,
+            "velocityX": -0.09479604281359731,
+            "velocityY": -0.6753984978919232,
+            "timestamp": 0.0841859894298466
+        },
+        {
+            "x": 2.28802134748838,
+            "y": 6.671681118579196,
+            "heading": 0.31538217591497186,
+            "angularVelocity": -0.8765828019021266,
+            "velocityX": -0.14245992671333493,
+            "velocityY": -1.014120769096589,
+            "timestamp": 0.1262789841447699
+        },
+        {
+            "x": 2.2800080756588827,
+            "y": 6.61469814393844,
+            "heading": 0.2672110996394858,
+            "angularVelocity": -1.1443965106718308,
+            "velocityX": -0.19037067530517543,
+            "velocityY": -1.3537400944426796,
+            "timestamp": 0.1683719788596932
+        },
+        {
+            "x": 2.2699650128963897,
+            "y": 6.543370292056319,
+            "heading": 0.20862519634351256,
+            "angularVelocity": -1.391820745773707,
+            "velocityX": -0.23859226055333851,
+            "velocityY": -1.694530226827328,
+            "timestamp": 0.21046497357461652
+        },
+        {
+            "x": 2.2578780077181495,
+            "y": 6.4576319980717,
+            "heading": 0.14091301714440063,
+            "angularVelocity": -1.608632972248606,
+            "velocityX": -0.2871500414760241,
+            "velocityY": -2.036877978516017,
+            "timestamp": 0.2525579682895398
+        },
+        {
+            "x": 2.2437361567314555,
+            "y": 6.357398182016401,
+            "heading": 0.06608849726258184,
+            "angularVelocity": -1.7776002964049293,
+            "velocityX": -0.3359668534508049,
+            "velocityY": -2.3812469683883926,
+            "timestamp": 0.2946509630044631
+        },
+        {
+            "x": 2.227539619778089,
+            "y": 6.242572554004172,
+            "heading": -0.012517898946656567,
+            "angularVelocity": -1.8674460380308828,
+            "velocityX": -0.3847798680768185,
+            "velocityY": -2.727903509595609,
+            "timestamp": 0.33674395771938637
+        },
+        {
+            "x": 2.209319968778144,
+            "y": 6.1131309245806085,
+            "heading": -0.08849823738903302,
+            "angularVelocity": -1.8050589880087344,
+            "velocityX": -0.4328428310539146,
+            "velocityY": -3.0751347177888695,
+            "timestamp": 0.37883695243430965
+        },
+        {
+            "x": 2.189363625025874,
+            "y": 5.970570114356374,
+            "heading": -0.13907359717832485,
+            "angularVelocity": -1.201514887021361,
+            "velocityX": -0.4741013056311528,
+            "velocityY": -3.38680607520881,
+            "timestamp": 0.42092994714923293
+        },
+        {
+            "x": 2.1674827409245396,
+            "y": 5.816618965425972,
+            "heading": -0.15173248317406665,
+            "angularVelocity": -0.3007361695568274,
+            "velocityX": -0.5198224609468644,
+            "velocityY": -3.657405465518488,
+            "timestamp": 0.4630229418641562
+        },
+        {
+            "x": 2.1445157307124236,
+            "y": 5.659048701686038,
+            "heading": -0.1517325271793822,
+            "angularVelocity": -0.0000010454308573871989,
+            "velocityX": -0.5456254744443967,
+            "velocityY": -3.7433844944292183,
+            "timestamp": 0.5051159365790795
+        },
+        {
+            "x": 2.1215487153797565,
+            "y": 5.501478438692377,
+            "heading": -0.1517325711844689,
+            "angularVelocity": -0.0000010454254199880688,
+            "velocityX": -0.5456255960929445,
+            "velocityY": -3.7433844767000393,
+            "timestamp": 0.5472089312940028
+        },
+        {
+            "x": 2.0985817251133763,
+            "y": 5.343908172045118,
+            "heading": -0.15173261518953962,
+            "angularVelocity": -0.000001045425040533604,
+            "velocityX": -0.5456250005951166,
+            "velocityY": -3.7433845634983034,
+            "timestamp": 0.5893019260089262
+        },
+        {
+            "x": 2.075614762728362,
+            "y": 5.186337901333946,
+            "heading": -0.15173265919459192,
+            "angularVelocity": -0.0000010454246033549915,
+            "velocityX": -0.5456243382196686,
+            "velocityY": -3.743384660044355,
+            "timestamp": 0.6313949207238495
+        },
+        {
+            "x": 2.0526478312972185,
+            "y": 5.028767626111028,
+            "heading": -0.15173270319962293,
+            "angularVelocity": -0.0000010454240970480152,
+            "velocityX": -0.5456236028509798,
+            "velocityY": -3.7433847672295513,
+            "timestamp": 0.6734879154387728
+        },
+        {
+            "x": 2.0296809339445536,
+            "y": 4.871197345920939,
+            "heading": -0.15173274720462945,
+            "angularVelocity": -0.0000010454235162635837,
+            "velocityX": -0.5456227932512114,
+            "velocityY": -3.743384885234266,
+            "timestamp": 0.7155809101536962
+        },
+        {
+            "x": 2.0067140734575584,
+            "y": 4.7136270603574335,
+            "heading": -0.15173279120960867,
+            "angularVelocity": -0.0000010454228670132247,
+            "velocityX": -0.5456219174363699,
+            "velocityY": -3.7433850128900636,
+            "timestamp": 0.7576739048686195
+        },
+        {
+            "x": 1.983747251812091,
+            "y": 4.55605676913253,
+            "heading": -0.15173283521455908,
+            "angularVelocity": -0.000001045422182833939,
+            "velocityX": -0.545620994681206,
+            "velocityY": -3.743385147387475,
+            "timestamp": 0.7997668995835429
+        },
+        {
+            "x": 1.9607804702256346,
+            "y": 4.398486472068785,
+            "heading": -0.15173287921948736,
+            "angularVelocity": -0.0000010454216570158517,
+            "velocityX": -0.5456200430024101,
+            "velocityY": -3.7433852861004024,
+            "timestamp": 0.8418598942984662
+        },
+        {
+            "x": 1.9378163633559966,
+            "y": 4.240915785172817,
+            "heading": -0.15173292322446258,
+            "angularVelocity": -0.0000010454227717672262,
+            "velocityX": -0.5455564999630786,
+            "velocityY": -3.7433945473141725,
+            "timestamp": 0.8839528890133895
+        },
+        {
+            "x": 1.9275487572437193,
+            "y": 4.082011890541997,
+            "heading": -0.15173296776197548,
+            "angularVelocity": -0.000001058074228463605,
+            "velocityX": -0.2439267194414494,
+            "velocityY": -3.775067459728259,
+            "timestamp": 0.9260458837283129
+        },
+        {
+            "x": 1.9320519221515333,
+            "y": 3.9228403083205765,
+            "heading": -0.15173301420750987,
+            "angularVelocity": -0.0000011034029462076059,
+            "velocityX": 0.10698133830372927,
+            "velocityY": -3.781426892988182,
+            "timestamp": 0.9681388784432362
+        },
+        {
+            "x": 1.9512874233679367,
+            "y": 3.764771127717043,
+            "heading": -0.15173306433513473,
+            "angularVelocity": -0.0000011908780826817618,
+            "velocityX": 0.45697630559851177,
+            "velocityY": -3.7552372235348,
+            "timestamp": 1.0102318731581594
+        },
+        {
+            "x": 1.9850897622274482,
+            "y": 3.609164986787025,
+            "heading": -0.15173312063283023,
+            "angularVelocity": -0.0000013374599716310608,
+            "velocityX": 0.8030395339756486,
+            "velocityY": -3.6967229816711127,
+            "timestamp": 1.0523248678730828
+        },
+        {
+            "x": 2.0331680016062417,
+            "y": 3.4573613296965586,
+            "heading": -0.1517331870158403,
+            "angularVelocity": -0.0000015770560044658274,
+            "velocityX": 1.142190991741159,
+            "velocityY": -3.606387668982997,
+            "timestamp": 1.094417862588006
+        },
+        {
+            "x": 2.0951083019579966,
+            "y": 3.3106668726225337,
+            "heading": -0.15173327041919607,
+            "angularVelocity": -0.0000019814070334134687,
+            "velocityX": 1.4715108956074094,
+            "velocityY": -3.4850088017619067,
+            "timestamp": 1.1365108573029294
         },
         {
             "x": 2.170377492904663,
             "y": 3.170344352722168,
-            "heading": -3.292933888363931,
-            "angularVelocity": -0.00000254889782501678,
-            "velocityX": 1.7880031704006887,
-            "velocityY": -3.3337176190574715,
-            "timestamp": 1.1786279959179613
-        },
-        {
-            "x": 2.2421588223512248,
-            "y": 3.0583154434355615,
-            "heading": -3.2929339938603395,
-            "angularVelocity": -0.000002999460293233564,
-            "velocityX": 2.0408775039380207,
-            "velocityY": -3.1851915047625172,
-            "timestamp": 1.2137997930042903
-        },
-        {
-            "x": 2.3224028083906,
-            "y": 2.952183724176483,
-            "heading": -3.2929340733758186,
-            "angularVelocity": -0.000002260773842517862,
-            "velocityX": 2.2814866650804433,
-            "velocityY": -3.0175233582344014,
-            "timestamp": 1.2489715900906193
-        },
-        {
-            "x": 2.4106271869517015,
-            "y": 2.852587036133821,
-            "heading": -3.292934137641087,
-            "angularVelocity": -0.0000018271818267107102,
-            "velocityX": 2.508384156332853,
-            "velocityY": -2.8317201932617277,
-            "timestamp": 1.2841433871769483
-        },
-        {
-            "x": 2.506301732241236,
-            "y": 2.7601239405733753,
-            "heading": -3.29293419233114,
-            "angularVelocity": -0.0000015549405448920558,
-            "velocityX": 2.720206336193231,
-            "velocityY": -2.628898811553339,
-            "timestamp": 1.3193151842632773
-        },
-        {
-            "x": 2.6088514418801845,
-            "y": 2.6753501198214895,
-            "heading": -3.292934240814094,
-            "angularVelocity": -0.0000013784610795710298,
-            "velocityX": 2.9156801225493263,
-            "velocityY": -2.410278341587403,
-            "timestamp": 1.3544869813496063
-        },
-        {
-            "x": 2.717659989787404,
-            "y": 2.5987750311541573,
-            "heading": -3.29293428528508,
-            "angularVelocity": -0.0000012643933456255718,
-            "velocityX": 3.093630605230363,
-            "velocityY": -2.177173048035584,
-            "timestamp": 1.3896587784359353
-        },
-        {
-            "x": 2.8320734217114962,
-            "y": 2.5308588229326605,
-            "heading": -3.2929343273033043,
-            "angularVelocity": -0.0000011946567396881703,
-            "velocityX": 3.252987944951041,
-            "velocityY": -1.9309848756034937,
-            "timestamp": 1.4248305755222643
-        },
-        {
-            "x": 2.951404044154876,
-            "y": 2.472009459742262,
-            "heading": -3.29293436807755,
-            "angularVelocity": -0.000001159288093356335,
-            "velocityX": 3.392792871813832,
-            "velocityY": -1.6731975066827869,
-            "timestamp": 1.4600023726085933
-        },
-        {
-            "x": 3.0749338856232122,
-            "y": 2.4225784637746113,
-            "heading": -3.2929344086367345,
-            "angularVelocity": -0.0000011531735020269185,
-            "velocityX": 3.512184525719075,
-            "velocityY": -1.4054157041314157,
-            "timestamp": 1.4951741696949223
-        },
-        {
-            "x": 3.199149697823942,
-            "y": 2.374897470692157,
-            "heading": -3.2929344492220305,
-            "angularVelocity": -0.000001153915896405561,
-            "velocityX": 3.531687957139154,
-            "velocityY": -1.3556598477303206,
-            "timestamp": 1.5303459667812513
-        },
-        {
-            "x": 3.3233655700450013,
-            "y": 2.3272166339715414,
-            "heading": -3.2929344898073354,
-            "angularVelocity": -0.0000011539161555742895,
-            "velocityX": 3.5316896636294053,
-            "velocityY": -1.3556554020706744,
-            "timestamp": 1.5655177638675803
-        },
-        {
-            "x": 3.4475814520083037,
-            "y": 2.279535822630966,
-            "heading": -3.2929345303926483,
-            "angularVelocity": -0.0000011539163817435631,
-            "velocityX": 3.531689940619614,
-            "velocityY": -1.3556546804686425,
-            "timestamp": 1.6006895609539094
-        },
-        {
-            "x": 3.571797343695941,
-            "y": 2.231855036623794,
-            "heading": -3.2929345709779727,
-            "angularVelocity": -0.0000011539167138358367,
-            "velocityX": 3.53169021710065,
-            "velocityY": -1.3556539601925897,
-            "timestamp": 1.6358613580402384
-        },
-        {
-            "x": 3.696013245125145,
-            "y": 2.1841742759949323,
-            "heading": -3.2929346115633122,
-            "angularVelocity": -0.0000011539171376230348,
-            "velocityX": 3.531690494071608,
-            "velocityY": -1.355653238639736,
-            "timestamp": 1.6710331551265674
-        },
-        {
-            "x": 3.8202291563227684,
-            "y": 2.1364935408143535,
-            "heading": -3.292934652148666,
-            "angularVelocity": -0.0000011539175501305985,
-            "velocityX": 3.531690771806052,
-            "velocityY": -1.3556525150974317,
-            "timestamp": 1.7062049522128964
-        },
-        {
-            "x": 3.9444450773804367,
-            "y": 2.0888128313207726,
-            "heading": -3.292934692734033,
-            "angularVelocity": -0.0000011539179284682577,
-            "velocityX": 3.5316910521455807,
-            "velocityY": -1.355651784768035,
-            "timestamp": 1.7413767492992254
-        },
-        {
-            "x": 4.068661008518651,
-            "y": 2.041132148088645,
-            "heading": -3.292934733319412,
-            "angularVelocity": -0.0000011539182628370223,
-            "velocityX": 3.531691338754367,
-            "velocityY": -1.3556510381057705,
-            "timestamp": 1.7765485463855544
-        },
-        {
-            "x": 4.192876950169643,
-            "y": 1.9934514922440194,
-            "heading": -3.2929347739048014,
-            "angularVelocity": -0.0000011539185452860982,
-            "velocityX": 3.5316916376522904,
-            "velocityY": -1.3556502594278441,
-            "timestamp": 1.8117203434718834
-        },
-        {
-            "x": 4.317092949197956,
-            "y": 1.945770985877502,
-            "heading": -3.292934814490671,
-            "angularVelocity": -0.0000011539322132251563,
-            "velocityX": 3.5316932689969183,
-            "velocityY": -1.3556460094855505,
-            "timestamp": 1.8468921405582124
-        },
-        {
-            "x": 4.4381547246220405,
-            "y": 1.900646588966398,
-            "heading": -3.304224160857315,
-            "angularVelocity": -0.3209772403421673,
-            "velocityX": 3.442012790160775,
-            "velocityY": -1.2829710350126964,
-            "timestamp": 1.8820639376445414
-        },
-        {
-            "x": 4.551299156991204,
-            "y": 1.858106592529644,
-            "heading": -3.340115682679576,
-            "angularVelocity": -1.0204631208967,
-            "velocityX": 3.2169079132195413,
-            "velocityY": -1.2094916939370357,
-            "timestamp": 1.9172357347308704
-        },
-        {
-            "x": 4.6559354752004225,
-            "y": 1.8184604535185338,
-            "heading": -3.3958506942974975,
-            "angularVelocity": -1.5846506643126916,
-            "velocityX": 2.9750063083893172,
-            "velocityY": -1.127213912721008,
-            "timestamp": 1.9524075318171994
-        },
-        {
-            "x": 4.750941844820644,
-            "y": 1.7823637508813854,
-            "heading": -3.4552749718768117,
-            "angularVelocity": -1.6895433984637673,
-            "velocityX": 2.7012088517123067,
-            "velocityY": -1.0262967953712765,
-            "timestamp": 1.9875793289035284
-        },
-        {
-            "x": 4.836288167632038,
-            "y": 1.7498748942497964,
-            "heading": -3.5138218394398413,
-            "angularVelocity": -1.664596990006722,
-            "velocityX": 2.4265556463296942,
-            "velocityY": -0.9237189829068263,
-            "timestamp": 2.0227511259898576
-        },
-        {
-            "x": 4.9120167698263195,
-            "y": 1.7210024537073532,
-            "heading": -3.5691368807810755,
-            "angularVelocity": -1.57271012355337,
-            "velocityX": 2.153105853772738,
-            "velocityY": -0.820897506930789,
-            "timestamp": 2.0579229230761866
-        },
-        {
-            "x": 4.9781740579406755,
-            "y": 1.695744953740728,
-            "heading": -3.6197442140788145,
-            "angularVelocity": -1.438861175433386,
-            "velocityX": 1.8809754858977845,
-            "velocityY": -0.718117982559453,
-            "timestamp": 2.0930947201625156
-        },
-        {
-            "x": 5.034800882324283,
-            "y": 1.6740995356540391,
-            "heading": -3.6646343712421663,
-            "angularVelocity": -1.2763111607055304,
-            "velocityX": 1.6100065698837365,
-            "velocityY": -0.6154197362608602,
-            "timestamp": 2.1282665172488446
-        },
-        {
-            "x": 5.081931555960545,
-            "y": 1.6560639982794174,
-            "heading": -3.703081117816373,
-            "angularVelocity": -1.0931129415946466,
-            "velocityX": 1.3400132361897832,
-            "velocityY": -0.5127840732833052,
-            "timestamp": 2.1634383143351736
-        },
-        {
-            "x": 5.11959462557562,
-            "y": 1.64163698634227,
-            "heading": -3.734541789094988,
-            "angularVelocity": -0.8944857495167222,
-            "velocityX": 1.0708315393333863,
-            "velocityY": -0.41018694329825167,
-            "timestamp": 2.1986101114215026
-        },
-        {
-            "x": 5.1478139750030305,
-            "y": 1.6308177057465072,
-            "heading": -3.758596048910276,
-            "angularVelocity": -0.6839076137124058,
-            "velocityX": 0.8023289045523005,
-            "velocityY": -0.30761239095081433,
-            "timestamp": 2.2337819085078316
-        },
-        {
-            "x": 5.166609893194281,
-            "y": 1.6236055874501054,
-            "heading": -3.7749054122717336,
-            "angularVelocity": -0.4637057162995195,
-            "velocityX": 0.5344031226245449,
-            "velocityY": -0.20505401753284183,
-            "timestamp": 2.2689537055941607
+            "heading": -0.1517333779044128,
+            "angularVelocity": -0.000002553517929494838,
+            "velocityX": 1.7881643122906952,
+            "velocityY": -3.333631186155945,
+            "timestamp": 1.1786038520178528
+        },
+        {
+            "x": 2.2421653916580504,
+            "y": 3.058317386598826,
+            "heading": -0.1517334835913584,
+            "angularVelocity": -0.0000030048345176781924,
+            "velocityX": 2.041035009880172,
+            "velocityY": -3.1850905776457417,
+            "timestamp": 1.213776153524627
+        },
+        {
+            "x": 2.322415894828655,
+            "y": 2.9521882028356305,
+            "heading": -0.15173356325163515,
+            "angularVelocity": -0.000002264858236060329,
+            "velocityX": 2.2816392369190814,
+            "velocityY": -3.0174079948323826,
+            "timestamp": 1.2489484550314012
+        },
+        {
+            "x": 2.4106466853387967,
+            "y": 2.8525946456824203,
+            "heading": -0.1517336276365959,
+            "angularVelocity": -0.0000018305586497777263,
+            "velocityX": 2.5085304836576707,
+            "velocityY": -2.8315905666289245,
+            "timestamp": 1.2841207565381754
+        },
+        {
+            "x": 2.5063274836460994,
+            "y": 2.7601352747554455,
+            "heading": -0.15173368243148194,
+            "angularVelocity": -0.0000015578987915956282,
+            "velocityX": 2.720345107040385,
+            "velocityY": -2.628755212654101,
+            "timestamp": 1.3192930580449496
+        },
+        {
+            "x": 2.6088832332920924,
+            "y": 2.67536576592715,
+            "heading": -0.15173373101035148,
+            "angularVelocity": -0.000001381168347773627,
+            "velocityX": 2.915810033819959,
+            "velocityY": -2.410121180496788,
+            "timestamp": 1.3544653595517238
+        },
+        {
+            "x": 2.7176975542058304,
+            "y": 2.598795565150263,
+            "heading": -0.1517337755722331,
+            "angularVelocity": -0.0000012669595030854418,
+            "velocityX": 3.0937503732242226,
+            "velocityY": -2.1770028544233733,
+            "timestamp": 1.389637661058498
+        },
+        {
+            "x": 2.8321164386506434,
+            "y": 2.5308848045360426,
+            "heading": -0.15173381767917685,
+            "angularVelocity": -0.0000011971620259359605,
+            "velocityX": 3.2530963156555406,
+            "velocityY": -1.930802299108592,
+            "timestamp": 1.4248099625652721
+        },
+        {
+            "x": 2.951452140465328,
+            "y": 2.4720414272198683,
+            "heading": -0.15173385854226368,
+            "angularVelocity": -0.0000011617973536146762,
+            "velocityX": 3.3928886283344455,
+            "velocityY": -1.6730033235055968,
+            "timestamp": 1.4599822640720463
+        },
+        {
+            "x": 3.0749866313948626,
+            "y": 2.4226169148084327,
+            "heading": -0.151733899192453,
+            "angularVelocity": -0.000001155744366153098,
+            "velocityX": 3.512266347021429,
+            "velocityY": -1.4052112114959712,
+            "timestamp": 1.4951545655788205
+        },
+        {
+            "x": 3.1992028583958065,
+            "y": 2.3749316777596947,
+            "heading": -0.1517339398689439,
+            "angularVelocity": -0.0000011564921588237654,
+            "velocityX": 3.5316491011263524,
+            "velocityY": -1.3557610678264387,
+            "timestamp": 1.5303268670855947
+        },
+        {
+            "x": 3.3234191450837622,
+            "y": 2.3272465961911335,
+            "heading": -0.15173398054544393,
+            "angularVelocity": -0.0000011564924185481092,
+            "velocityX": 3.5316507981154315,
+            "velocityY": -1.355756647297499,
+            "timestamp": 1.565499168592369
+        },
+        {
+            "x": 3.4476354415156054,
+            "y": 2.2795615400047207,
+            "heading": -0.15173402122195195,
+            "angularVelocity": -0.0000011564926460273957,
+            "velocityX": 3.5316510751484063,
+            "velocityY": -1.3557559256458895,
+            "timestamp": 1.6006714700991431
+        },
+        {
+            "x": 3.57185174767356,
+            "y": 2.231876509154167,
+            "heading": -0.15173406189847174,
+            "angularVelocity": -0.0000011564929795008858,
+            "velocityX": 3.531651351675977,
+            "velocityY": -1.355755205310334,
+            "timestamp": 1.6358437716059173
+        },
+        {
+            "x": 3.6960680635749434,
+            "y": 2.1841915036846027,
+            "heading": -0.15173410257500647,
+            "angularVelocity": -0.000001156493404477995,
+            "velocityX": 3.5316516286959314,
+            "velocityY": -1.3557544836916666,
+            "timestamp": 1.6710160731126915
+        },
+        {
+            "x": 3.820284389246338,
+            "y": 2.136506523665285,
+            "heading": -0.15173414325155568,
+            "angularVelocity": -0.0000011564938164652249,
+            "velocityX": 3.5316519064716276,
+            "velocityY": -1.3557537601039011,
+            "timestamp": 1.7061883746194657
+        },
+        {
+            "x": 3.9445007247793735,
+            "y": 2.0888215693349235,
+            "heading": -0.15173418392811813,
+            "angularVelocity": -0.0000011564941935950735,
+            "velocityX": 3.5316521868525257,
+            "velocityY": -1.3557530297293134,
+            "timestamp": 1.74136067612624
+        },
+        {
+            "x": 4.068717070394662,
+            "y": 2.0411366412682117,
+            "heading": -0.1517342246046924,
+            "angularVelocity": -0.0000011564945294151802,
+            "velocityX": 3.531652473505735,
+            "velocityY": -1.3557522830153055,
+            "timestamp": 1.776532977633014
+        },
+        {
+            "x": 4.192933426524707,
+            "y": 1.9934517405918115,
+            "heading": -0.15173426528127656,
+            "angularVelocity": -0.0000011564948108738022,
+            "velocityX": 3.5316527724556495,
+            "velocityY": -1.3557515042686648,
+            "timestamp": 1.8117052791397883
+        },
+        {
+            "x": 4.3171498402859925,
+            "y": 1.9457669900421575,
+            "heading": -0.1517343059583456,
+            "angularVelocity": -0.0000011565085966842365,
+            "velocityX": 3.531654410996122,
+            "velocityY": -1.3557472359455842,
+            "timestamp": 1.8468775806465625
+        },
+        {
+            "x": 4.438199866372425,
+            "y": 1.9006463371040783,
+            "heading": -0.16306820061092758,
+            "angularVelocity": -0.3222392100329015,
+            "velocityX": 3.441629375976964,
+            "velocityY": -1.282846188765579,
+            "timestamp": 1.8820498821533367
+        },
+        {
+            "x": 4.551333178258229,
+            "y": 1.8581105785728176,
+            "heading": -0.19901052601489952,
+            "angularVelocity": -1.0218929061849813,
+            "velocityX": 3.2165456066050515,
+            "velocityY": -1.2093538582645278,
+            "timestamp": 1.9172221836601109
+        },
+        {
+            "x": 4.6559660625940325,
+            "y": 1.8184646011809935,
+            "heading": -0.2548690023306322,
+            "angularVelocity": -1.5881382202121255,
+            "velocityX": 2.9748660125539748,
+            "velocityY": -1.1271931518097666,
+            "timestamp": 1.952394485166885
+        },
+        {
+            "x": 4.750968691238499,
+            "y": 1.7823676048635404,
+            "heading": -0.3144227593810797,
+            "angularVelocity": -1.693200458860429,
+            "velocityX": 2.7010637511499986,
+            "velocityY": -1.0262904265875403,
+            "timestamp": 1.9875667866736593
+        },
+        {
+            "x": 4.836310937621718,
+            "y": 1.749878315803868,
+            "heading": -0.37309459063768413,
+            "angularVelocity": -1.6681260180060764,
+            "velocityX": 2.426404947278819,
+            "velocityY": -0.9237180300360781,
+            "timestamp": 2.0227390881804337
+        },
+        {
+            "x": 4.912035358987092,
+            "y": 1.7210053742684595,
+            "heading": -0.4285254423369764,
+            "angularVelocity": -1.5759802266171352,
+            "velocityX": 2.1529561081121917,
+            "velocityY": -0.8208999780650579,
+            "timestamp": 2.057911389687208
+        },
+        {
+            "x": 4.978188578205751,
+            "y": 1.6957473341798728,
+            "heading": -0.4792367761737124,
+            "angularVelocity": -1.4417974276425698,
+            "velocityX": 1.8808328253957978,
+            "velocityY": -0.71812304019178,
+            "timestamp": 2.0930836911939825
+        },
+        {
+            "x": 5.034811621143831,
+            "y": 1.6741013653395114,
+            "heading": -0.5242175587960242,
+            "angularVelocity": -1.278869471013956,
+            "velocityX": 1.6098759680874073,
+            "velocityY": -0.6154265690060781,
+            "timestamp": 2.128255992700757
+        },
+        {
+            "x": 5.081938937252143,
+            "y": 1.656065298518901,
+            "heading": -0.5627406592667319,
+            "angularVelocity": -1.0952681178195878,
+            "velocityX": 1.3398985590759445,
+            "velocityY": -0.5127917721601607,
+            "timestamp": 2.1634282942075314
+        },
+        {
+            "x": 5.11959917834615,
+            "y": 1.6416378108353973,
+            "heading": -0.5942629145071194,
+            "angularVelocity": -0.8962238434785516,
+            "velocityX": 1.0707357631047747,
+            "velocityY": -0.41019458680362747,
+            "timestamp": 2.1986005957143058
+        },
+        {
+            "x": 5.147816310703936,
+            "y": 1.63081813815951,
+            "heading": -0.6183637008514038,
+            "angularVelocity": -0.6852206228143011,
+            "velocityX": 0.8022543634897267,
+            "velocityY": -0.3076191267666608,
+            "timestamp": 2.23377289722108
+        },
+        {
+            "x": 5.1666106911988186,
+            "y": 1.6236057376407542,
+            "heading": -0.6347043261398958,
+            "angularVelocity": -0.4645878884367218,
+            "velocityX": 0.5343517395716112,
+            "velocityY": -0.20505910076332604,
+            "timestamp": 2.2689451987278546
         },
         {
             "x": 5.176,
             "y": 1.62,
-            "heading": -3.7831853071795862,
-            "angularVelocity": -0.2354129044793956,
-            "velocityX": 0.2669783060180454,
-            "velocityY": -0.10251359750698616,
-            "timestamp": 2.3041255026804897
+            "heading": -0.643,
+            "angularVelocity": -0.23585814702817923,
+            "velocityX": 0.26695178873562564,
+            "velocityY": -0.10251639745723456,
+            "timestamp": 2.304117500234629
         },
         {
             "x": 5.176,
             "y": 1.62,
-            "heading": -3.7831853071795862,
-            "angularVelocity": -9.348588807340857e-32,
-            "velocityX": 3.3761299345246814e-33,
-            "velocityY": 7.633245142283867e-33,
-            "timestamp": 2.3392972997668187
+            "heading": -0.643,
+            "angularVelocity": -2.979902278450704e-33,
+            "velocityX": -7.596111021745797e-35,
+            "velocityY": -9.748244176889607e-35,
+            "timestamp": 2.3392898017414034
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.1.traj
index f0ca2992..a5fd0bb0 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.1.traj	
@@ -3,101 +3,101 @@
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
+            "heading": 0.39,
+            "angularVelocity": 1.792103156985809e-33,
+            "velocityX": -7.27073415334495e-34,
+            "velocityY": -7.853383039035825e-35,
             "timestamp": 0
         },
         {
-            "x": 2.2980013304879616,
-            "y": 6.709118488011682,
-            "heading": -2.76564785007466,
-            "angularVelocity": -0.20597421909793953,
-            "velocityX": -0.026308687136743093,
-            "velocityY": -0.6302691420205844,
-            "timestamp": 0.07596994489499284
+            "x": 2.298001341213542,
+            "y": 6.709118496985434,
+            "heading": 0.374359095500822,
+            "angularVelocity": -0.2058832051525312,
+            "velocityX": -0.02630859852023658,
+            "velocityY": -0.6302702831973344,
+            "timestamp": 0.07596979310473732
         },
         {
-            "x": 2.294004010549845,
-            "y": 6.6133557271698225,
-            "heading": -2.796958855018623,
-            "angularVelocity": -0.41214989674195696,
-            "velocityX": -0.05261712304308655,
-            "velocityY": -1.2605348203717124,
-            "timestamp": 0.15193988978998568
+            "x": 2.294004039374054,
+            "y": 6.613355753676287,
+            "heading": 0.3430620070158405,
+            "angularVelocity": -0.4119675361210102,
+            "velocityX": -0.0526169899393711,
+            "velocityY": -1.2605371081782701,
+            "timestamp": 0.15193958620947465
         },
         {
-            "x": 2.2880079544710785,
-            "y": 6.469712199120552,
-            "heading": -2.8439591282551087,
-            "angularVelocity": -0.6186693079934498,
-            "velocityX": -0.07892668721893853,
-            "velocityY": -1.8907941587665607,
-            "timestamp": 0.22790983468497852
+            "x": 2.2880080040813597,
+            "y": 6.469712250400375,
+            "heading": 0.296082691458658,
+            "angularVelocity": -0.6183946755312791,
+            "velocityX": -0.07892657130746197,
+            "velocityY": -1.8907976105433808,
+            "timestamp": 0.22790937931421196
         },
         {
-            "x": 2.2800128266474267,
-            "y": 6.278188582998367,
-            "heading": -2.9066839984131234,
-            "angularVelocity": -0.8256537535299542,
-            "velocityX": -0.10524066898696272,
-            "velocityY": -2.5210445576459732,
-            "timestamp": 0.30387977957997137
+            "x": 2.280012894319733,
+            "y": 6.278188663497132,
+            "heading": 0.23338596481601495,
+            "angularVelocity": -0.8252849465603376,
+            "velocityX": -0.10524064150870725,
+            "velocityY": -2.5210492101669324,
+            "timestamp": 0.3038791724189493
         },
         {
-            "x": 2.2700178539327394,
-            "y": 6.038785706839934,
-            "heading": -2.985174219418862,
-            "angularVelocity": -1.0331746470822045,
-            "velocityX": -0.13156482775526857,
-            "velocityY": -3.1512840569956957,
-            "timestamp": 0.37984972447496423
+            "x": 2.270017931664709,
+            "y": 6.038785816229407,
+            "heading": 0.1549313176962556,
+            "angularVelocity": -1.0327084478378958,
+            "velocityX": -0.13156495820969516,
+            "velocityY": -3.1512899730774246,
+            "timestamp": 0.37984896552368663
         },
         {
-            "x": 2.2620112641506833,
-            "y": 5.847271276792759,
-            "heading": -3.048371938278793,
-            "angularVelocity": -0.8318779083923777,
-            "velocityX": -0.10539154389440776,
-            "velocityY": -2.520923640419774,
-            "timestamp": 0.4558196693699571
+            "x": 2.2620113109689415,
+            "y": 5.847271344563018,
+            "heading": 0.09176539516362914,
+            "angularVelocity": -0.8314610314331311,
+            "velocityX": -0.10539216139142096,
+            "velocityY": -2.5209292251507525,
+            "timestamp": 0.45581875862842397
         },
         {
-            "x": 2.256005772930322,
-            "y": 5.703635612537123,
-            "heading": -3.0957774424626403,
-            "angularVelocity": -0.624003403574559,
-            "velocityX": -0.0790508829335374,
-            "velocityY": -1.8906906468626763,
-            "timestamp": 0.53178961426495
+            "x": 2.2560057963370115,
+            "y": 5.703635646814395,
+            "heading": 0.0443838554377995,
+            "angularVelocity": -0.6236892031613949,
+            "velocityX": -0.079051349049357,
+            "velocityY": -1.8906948653999391,
+            "timestamp": 0.5317885517331613
         },
         {
-            "x": 2.25200192771716,
-            "y": 5.607878538669833,
-            "heading": -3.127382702013982,
-            "angularVelocity": -0.4160231996354262,
-            "velocityX": -0.05270301589261462,
-            "velocityY": -1.260459962155393,
-            "timestamp": 0.6077595591599428
+            "x": 2.252001935551058,
+            "y": 5.607878550007709,
+            "heading": 0.012794605457377246,
+            "angularVelocity": -0.4158132948561684,
+            "velocityX": -0.05270332618167983,
+            "velocityY": -1.2604627825518706,
+            "timestamp": 0.6077583448378986
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.20801127587293636,
-            "velocityX": -0.026351575217375103,
-            "velocityY": -0.6302300039313167,
-            "timestamp": 0.6837295040549357
+            "heading": -0.003,
+            "angularVelocity": -0.20790639031492017,
+            "velocityX": -0.026351730987313093,
+            "velocityY": -0.6302314123944018,
+            "timestamp": 0.6837281379426359
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": 2.276477315177118e-32,
-            "velocityX": 0,
-            "velocityY": -9.616776104992252e-34,
-            "timestamp": 0.7596994489499286
+            "heading": -0.003,
+            "angularVelocity": -6.008173298568887e-34,
+            "velocityX": -7.305982513747354e-33,
+            "velocityY": -3.8813573012664696e-35,
+            "timestamp": 0.7596979310473732
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.traj
index f0ca2992..a5fd0bb0 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.traj	
@@ -3,101 +3,101 @@
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
+            "heading": 0.39,
+            "angularVelocity": 1.792103156985809e-33,
+            "velocityX": -7.27073415334495e-34,
+            "velocityY": -7.853383039035825e-35,
             "timestamp": 0
         },
         {
-            "x": 2.2980013304879616,
-            "y": 6.709118488011682,
-            "heading": -2.76564785007466,
-            "angularVelocity": -0.20597421909793953,
-            "velocityX": -0.026308687136743093,
-            "velocityY": -0.6302691420205844,
-            "timestamp": 0.07596994489499284
+            "x": 2.298001341213542,
+            "y": 6.709118496985434,
+            "heading": 0.374359095500822,
+            "angularVelocity": -0.2058832051525312,
+            "velocityX": -0.02630859852023658,
+            "velocityY": -0.6302702831973344,
+            "timestamp": 0.07596979310473732
         },
         {
-            "x": 2.294004010549845,
-            "y": 6.6133557271698225,
-            "heading": -2.796958855018623,
-            "angularVelocity": -0.41214989674195696,
-            "velocityX": -0.05261712304308655,
-            "velocityY": -1.2605348203717124,
-            "timestamp": 0.15193988978998568
+            "x": 2.294004039374054,
+            "y": 6.613355753676287,
+            "heading": 0.3430620070158405,
+            "angularVelocity": -0.4119675361210102,
+            "velocityX": -0.0526169899393711,
+            "velocityY": -1.2605371081782701,
+            "timestamp": 0.15193958620947465
         },
         {
-            "x": 2.2880079544710785,
-            "y": 6.469712199120552,
-            "heading": -2.8439591282551087,
-            "angularVelocity": -0.6186693079934498,
-            "velocityX": -0.07892668721893853,
-            "velocityY": -1.8907941587665607,
-            "timestamp": 0.22790983468497852
+            "x": 2.2880080040813597,
+            "y": 6.469712250400375,
+            "heading": 0.296082691458658,
+            "angularVelocity": -0.6183946755312791,
+            "velocityX": -0.07892657130746197,
+            "velocityY": -1.8907976105433808,
+            "timestamp": 0.22790937931421196
         },
         {
-            "x": 2.2800128266474267,
-            "y": 6.278188582998367,
-            "heading": -2.9066839984131234,
-            "angularVelocity": -0.8256537535299542,
-            "velocityX": -0.10524066898696272,
-            "velocityY": -2.5210445576459732,
-            "timestamp": 0.30387977957997137
+            "x": 2.280012894319733,
+            "y": 6.278188663497132,
+            "heading": 0.23338596481601495,
+            "angularVelocity": -0.8252849465603376,
+            "velocityX": -0.10524064150870725,
+            "velocityY": -2.5210492101669324,
+            "timestamp": 0.3038791724189493
         },
         {
-            "x": 2.2700178539327394,
-            "y": 6.038785706839934,
-            "heading": -2.985174219418862,
-            "angularVelocity": -1.0331746470822045,
-            "velocityX": -0.13156482775526857,
-            "velocityY": -3.1512840569956957,
-            "timestamp": 0.37984972447496423
+            "x": 2.270017931664709,
+            "y": 6.038785816229407,
+            "heading": 0.1549313176962556,
+            "angularVelocity": -1.0327084478378958,
+            "velocityX": -0.13156495820969516,
+            "velocityY": -3.1512899730774246,
+            "timestamp": 0.37984896552368663
         },
         {
-            "x": 2.2620112641506833,
-            "y": 5.847271276792759,
-            "heading": -3.048371938278793,
-            "angularVelocity": -0.8318779083923777,
-            "velocityX": -0.10539154389440776,
-            "velocityY": -2.520923640419774,
-            "timestamp": 0.4558196693699571
+            "x": 2.2620113109689415,
+            "y": 5.847271344563018,
+            "heading": 0.09176539516362914,
+            "angularVelocity": -0.8314610314331311,
+            "velocityX": -0.10539216139142096,
+            "velocityY": -2.5209292251507525,
+            "timestamp": 0.45581875862842397
         },
         {
-            "x": 2.256005772930322,
-            "y": 5.703635612537123,
-            "heading": -3.0957774424626403,
-            "angularVelocity": -0.624003403574559,
-            "velocityX": -0.0790508829335374,
-            "velocityY": -1.8906906468626763,
-            "timestamp": 0.53178961426495
+            "x": 2.2560057963370115,
+            "y": 5.703635646814395,
+            "heading": 0.0443838554377995,
+            "angularVelocity": -0.6236892031613949,
+            "velocityX": -0.079051349049357,
+            "velocityY": -1.8906948653999391,
+            "timestamp": 0.5317885517331613
         },
         {
-            "x": 2.25200192771716,
-            "y": 5.607878538669833,
-            "heading": -3.127382702013982,
-            "angularVelocity": -0.4160231996354262,
-            "velocityX": -0.05270301589261462,
-            "velocityY": -1.260459962155393,
-            "timestamp": 0.6077595591599428
+            "x": 2.252001935551058,
+            "y": 5.607878550007709,
+            "heading": 0.012794605457377246,
+            "angularVelocity": -0.4158132948561684,
+            "velocityX": -0.05270332618167983,
+            "velocityY": -1.2604627825518706,
+            "timestamp": 0.6077583448378986
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.20801127587293636,
-            "velocityX": -0.026351575217375103,
-            "velocityY": -0.6302300039313167,
-            "timestamp": 0.6837295040549357
+            "heading": -0.003,
+            "angularVelocity": -0.20790639031492017,
+            "velocityX": -0.026351730987313093,
+            "velocityY": -0.6302314123944018,
+            "timestamp": 0.6837281379426359
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": -3.143185307179586,
-            "angularVelocity": 2.276477315177118e-32,
-            "velocityX": 0,
-            "velocityY": -9.616776104992252e-34,
-            "timestamp": 0.7596994489499286
+            "heading": -0.003,
+            "angularVelocity": -6.008173298568887e-34,
+            "velocityX": -7.305982513747354e-33,
+            "velocityY": -3.8813573012664696e-35,
+            "timestamp": 0.7596979310473732
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.1.traj
deleted file mode 100644
index e9619fbb..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": -3.577903969873187e-35,
-            "velocityX": -4.511566930271669e-37,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3001399949944257,
-            "y": 6.713175615322878,
-            "heading": -2.7771678602975163,
-            "angularVelocity": -0.3719998319011172,
-            "velocityX": 0.001916901582349506,
-            "velocityY": -0.6000716859748281,
-            "timestamp": 0.07303191552177325
-        },
-        {
-            "x": 2.3004163850099326,
-            "y": 6.6254379467440225,
-            "heading": -2.829600510901429,
-            "angularVelocity": -0.7179416044247195,
-            "velocityX": 0.0037845100122603365,
-            "velocityY": -1.2013606373599366,
-            "timestamp": 0.1460638310435465
-        },
-        {
-            "x": 2.3008288453246886,
-            "y": 6.493659773055691,
-            "heading": -2.9042529721112227,
-            "angularVelocity": -1.022189554750722,
-            "velocityX": 0.005647672141821651,
-            "velocityY": -1.8043915834173219,
-            "timestamp": 0.21909574656531977
-        },
-        {
-            "x": 2.301391538953172,
-            "y": 6.317653855065593,
-            "heading": -2.995610403714753,
-            "angularVelocity": -1.2509247628359643,
-            "velocityX": 0.007704763382735362,
-            "velocityY": -2.4099863290265735,
-            "timestamp": 0.292127662087093
-        },
-        {
-            "x": 2.3021600106867623,
-            "y": 6.097194240566218,
-            "heading": -3.0915567903699093,
-            "angularVelocity": -1.3137596894408665,
-            "velocityX": 0.010522409662952134,
-            "velocityY": -3.0186749577128253,
-            "timestamp": 0.36515957760886625
-        },
-        {
-            "x": 2.303349915614448,
-            "y": 5.834382556129677,
-            "heading": -3.1370632737304405,
-            "angularVelocity": -0.6231040639617954,
-            "velocityX": 0.016292944244779627,
-            "velocityY": -3.598586762498201,
-            "timestamp": 0.4381914931306395
-        },
-        {
-            "x": 2.3036783877036533,
-            "y": 5.558107366997832,
-            "heading": -3.137063302917444,
-            "angularVelocity": -3.99647241237663e-7,
-            "velocityX": 0.004497651292023743,
-            "velocityY": -3.7829377356188716,
-            "timestamp": 0.5112234086524128
-        },
-        {
-            "x": 2.3040068828134284,
-            "y": 5.281832177893351,
-            "heading": -3.1370633321044417,
-            "angularVelocity": -3.996471578074284e-7,
-            "velocityX": 0.0044979665044766115,
-            "velocityY": -3.782937735244173,
-            "timestamp": 0.584255324174186
-        },
-        {
-            "x": 2.302092596714716,
-            "y": 5.019360768744023,
-            "heading": -3.182327854705923,
-            "angularVelocity": -0.6197909814920075,
-            "velocityX": -0.026211637542794358,
-            "velocityY": -3.5939274942210355,
-            "timestamp": 0.6572872396959593
-        },
-        {
-            "x": 2.3012286144088434,
-            "y": 4.799012511457164,
-            "heading": -3.280568867162049,
-            "angularVelocity": -1.3451791830222102,
-            "velocityX": -0.01183020190145165,
-            "velocityY": -3.017150183075313,
-            "timestamp": 0.7303191552177325
-        },
-        {
-            "x": 2.3006858203530896,
-            "y": 4.623118549812072,
-            "heading": -3.3743826060457494,
-            "angularVelocity": -1.284558103309387,
-            "velocityX": -0.007432285623012945,
-            "velocityY": -2.408453350681352,
-            "timestamp": 0.8033510707395057
-        },
-        {
-            "x": 2.3003278174317203,
-            "y": 4.491440647891757,
-            "heading": -3.4511285642395753,
-            "angularVelocity": -1.0508550630983442,
-            "velocityX": -0.00490200645582827,
-            "velocityY": -1.8030185978218984,
-            "timestamp": 0.876382986261279
-        },
-        {
-            "x": 2.3001060473289097,
-            "y": 4.403780156277999,
-            "heading": -3.505127434733723,
-            "angularVelocity": -0.7393872953811381,
-            "velocityX": -0.0030366190072672575,
-            "velocityY": -1.2003038806728776,
-            "timestamp": 0.9494149017830522
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -3.5331853071795862,
-            "angularVelocity": -0.38418645116186434,
-            "velocityX": -0.001452068293050597,
-            "velocityY": -0.5994660822629829,
-            "timestamp": 1.0224468173048256
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -3.5331853071795862,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 8.704854669398703e-34,
-            "timestamp": 1.095478732826599
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.traj
deleted file mode 100644
index e9619fbb..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": -2.75,
-            "angularVelocity": -3.577903969873187e-35,
-            "velocityX": -4.511566930271669e-37,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3001399949944257,
-            "y": 6.713175615322878,
-            "heading": -2.7771678602975163,
-            "angularVelocity": -0.3719998319011172,
-            "velocityX": 0.001916901582349506,
-            "velocityY": -0.6000716859748281,
-            "timestamp": 0.07303191552177325
-        },
-        {
-            "x": 2.3004163850099326,
-            "y": 6.6254379467440225,
-            "heading": -2.829600510901429,
-            "angularVelocity": -0.7179416044247195,
-            "velocityX": 0.0037845100122603365,
-            "velocityY": -1.2013606373599366,
-            "timestamp": 0.1460638310435465
-        },
-        {
-            "x": 2.3008288453246886,
-            "y": 6.493659773055691,
-            "heading": -2.9042529721112227,
-            "angularVelocity": -1.022189554750722,
-            "velocityX": 0.005647672141821651,
-            "velocityY": -1.8043915834173219,
-            "timestamp": 0.21909574656531977
-        },
-        {
-            "x": 2.301391538953172,
-            "y": 6.317653855065593,
-            "heading": -2.995610403714753,
-            "angularVelocity": -1.2509247628359643,
-            "velocityX": 0.007704763382735362,
-            "velocityY": -2.4099863290265735,
-            "timestamp": 0.292127662087093
-        },
-        {
-            "x": 2.3021600106867623,
-            "y": 6.097194240566218,
-            "heading": -3.0915567903699093,
-            "angularVelocity": -1.3137596894408665,
-            "velocityX": 0.010522409662952134,
-            "velocityY": -3.0186749577128253,
-            "timestamp": 0.36515957760886625
-        },
-        {
-            "x": 2.303349915614448,
-            "y": 5.834382556129677,
-            "heading": -3.1370632737304405,
-            "angularVelocity": -0.6231040639617954,
-            "velocityX": 0.016292944244779627,
-            "velocityY": -3.598586762498201,
-            "timestamp": 0.4381914931306395
-        },
-        {
-            "x": 2.3036783877036533,
-            "y": 5.558107366997832,
-            "heading": -3.137063302917444,
-            "angularVelocity": -3.99647241237663e-7,
-            "velocityX": 0.004497651292023743,
-            "velocityY": -3.7829377356188716,
-            "timestamp": 0.5112234086524128
-        },
-        {
-            "x": 2.3040068828134284,
-            "y": 5.281832177893351,
-            "heading": -3.1370633321044417,
-            "angularVelocity": -3.996471578074284e-7,
-            "velocityX": 0.0044979665044766115,
-            "velocityY": -3.782937735244173,
-            "timestamp": 0.584255324174186
-        },
-        {
-            "x": 2.302092596714716,
-            "y": 5.019360768744023,
-            "heading": -3.182327854705923,
-            "angularVelocity": -0.6197909814920075,
-            "velocityX": -0.026211637542794358,
-            "velocityY": -3.5939274942210355,
-            "timestamp": 0.6572872396959593
-        },
-        {
-            "x": 2.3012286144088434,
-            "y": 4.799012511457164,
-            "heading": -3.280568867162049,
-            "angularVelocity": -1.3451791830222102,
-            "velocityX": -0.01183020190145165,
-            "velocityY": -3.017150183075313,
-            "timestamp": 0.7303191552177325
-        },
-        {
-            "x": 2.3006858203530896,
-            "y": 4.623118549812072,
-            "heading": -3.3743826060457494,
-            "angularVelocity": -1.284558103309387,
-            "velocityX": -0.007432285623012945,
-            "velocityY": -2.408453350681352,
-            "timestamp": 0.8033510707395057
-        },
-        {
-            "x": 2.3003278174317203,
-            "y": 4.491440647891757,
-            "heading": -3.4511285642395753,
-            "angularVelocity": -1.0508550630983442,
-            "velocityX": -0.00490200645582827,
-            "velocityY": -1.8030185978218984,
-            "timestamp": 0.876382986261279
-        },
-        {
-            "x": 2.3001060473289097,
-            "y": 4.403780156277999,
-            "heading": -3.505127434733723,
-            "angularVelocity": -0.7393872953811381,
-            "velocityX": -0.0030366190072672575,
-            "velocityY": -1.2003038806728776,
-            "timestamp": 0.9494149017830522
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -3.5331853071795862,
-            "angularVelocity": -0.38418645116186434,
-            "velocityX": -0.001452068293050597,
-            "velocityY": -0.5994660822629829,
-            "timestamp": 1.0224468173048256
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -3.5331853071795862,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 8.704854669398703e-34,
-            "timestamp": 1.095478732826599
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.1.traj
index e091b127..20e2615d 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.1.traj	
@@ -3,191 +3,191 @@
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 3.909956769253128e-30,
-            "velocityX": -2.302876892990051e-31,
-            "velocityY": 1.6352823102584027e-31,
+            "heading": 0,
+            "angularVelocity": -9.278807284273418e-29,
+            "velocityX": -7.385267906050254e-32,
+            "velocityY": -1.1558173496237395e-34,
             "timestamp": 0
         },
         {
-            "x": 2.326889522840126,
-            "y": 5.587767009829938,
-            "heading": 3.1412161105492817,
-            "angularVelocity": 0.012281818163829663,
-            "velocityX": 0.7765273796780119,
-            "velocityY": 0.2804262868111325,
-            "timestamp": 0.0990171433131028
-        },
-        {
-            "x": 2.4806685686648633,
-            "y": 5.6433010199081535,
-            "heading": 3.143648327345736,
-            "angularVelocity": 0.024563592879525555,
-            "velocityX": 1.5530547608152194,
-            "velocityY": 0.560852476854545,
-            "timestamp": 0.1980342866262056
-        },
-        {
-            "x": 2.711337134772754,
-            "y": 5.72660201934395,
-            "heading": 3.147296644122033,
-            "angularVelocity": 0.03684530429983614,
-            "velocityX": 2.3295821146697007,
-            "velocityY": 0.8412785569099963,
-            "timestamp": 0.2970514299393084
-        },
-        {
-            "x": 3.0188952067968695,
-            "y": 5.837669992778753,
-            "heading": 3.1521610500975985,
-            "angularVelocity": 0.04912690684429555,
-            "velocityX": 3.106109323428804,
-            "velocityY": 1.1217044818551853,
-            "timestamp": 0.3960685732524112
-        },
-        {
-            "x": 3.3712021943007997,
-            "y": 5.964897910508787,
-            "heading": 3.1521610506223987,
-            "angularVelocity": 5.300094570798509e-9,
-            "velocityX": 3.558040312169958,
-            "velocityY": 1.2849079813151831,
-            "timestamp": 0.495085716565514
-        },
-        {
-            "x": 3.7235092123405478,
-            "y": 6.092125743682096,
-            "heading": 3.1521610511472007,
-            "angularVelocity": 5.300112966968611e-9,
-            "velocityX": 3.558040620559158,
-            "velocityY": 1.2849071273547275,
-            "timestamp": 0.5941028598786168
-        },
-        {
-            "x": 4.075816261929694,
-            "y": 6.219353489491835,
-            "heading": 3.1521610516720138,
-            "angularVelocity": 5.300218907643243e-9,
-            "velocityX": 3.5580409391847763,
-            "velocityY": 1.284906245047206,
-            "timestamp": 0.6931200031917195
-        },
-        {
-            "x": 4.428123345781717,
-            "y": 6.346581140424019,
-            "heading": 3.152161052196838,
-            "angularVelocity": 5.300337345645622e-9,
-            "velocityX": 3.558041285214527,
-            "velocityY": 1.284905286853995,
-            "timestamp": 0.7921371465048224
-        },
-        {
-            "x": 4.780430481129625,
-            "y": 6.473808648758581,
-            "heading": 3.152161052721672,
-            "angularVelocity": 5.300431772031768e-9,
-            "velocityX": 3.558041805284913,
-            "velocityY": 1.2849038467233598,
-            "timestamp": 0.8911542898179252
-        },
-        {
-            "x": 5.134794229593029,
-            "y": 6.595190238737374,
-            "heading": 3.1521610521993333,
-            "angularVelocity": -5.2752356524836585e-9,
-            "velocityX": 3.5788120784587116,
-            "velocityY": 1.225864389916352,
-            "timestamp": 0.9901714331310281
-        },
-        {
-            "x": 5.489158282910728,
-            "y": 6.716570938689376,
-            "heading": 3.152161051676551,
-            "angularVelocity": -5.279713305479767e-9,
-            "velocityX": 3.578815157261821,
-            "velocityY": 1.225855401303425,
-            "timestamp": 1.089188576444131
-        },
-        {
-            "x": 5.843522381964551,
-            "y": 6.837951505117218,
-            "heading": 3.1521610511537586,
-            "angularVelocity": -5.279818233241214e-9,
-            "velocityX": 3.5788156191628895,
-            "velocityY": 1.2258540528080475,
-            "timestamp": 1.1882057197572338
-        },
-        {
-            "x": 6.19788651637458,
-            "y": 6.959331968324431,
-            "heading": 3.1521610506309807,
-            "angularVelocity": -5.279669747527449e-9,
-            "velocityX": 3.5788159762344476,
-            "velocityY": 1.2258530103559488,
-            "timestamp": 1.2872228630703366
-        },
-        {
-            "x": 6.5522506780354695,
-            "y": 7.080712351974079,
-            "heading": 3.1521610501082176,
-            "angularVelocity": -5.2795212284316414e-9,
-            "velocityX": 3.5788162514480155,
-            "velocityY": 1.2258522068833249,
-            "timestamp": 1.3862400063834395
-        },
-        {
-            "x": 6.9066148601448845,
-            "y": 7.202092675925132,
-            "heading": 3.1521610495854597,
-            "angularVelocity": -5.279467664817609e-9,
-            "velocityX": 3.5788164579630073,
-            "velocityY": 1.2258516039716054,
-            "timestamp": 1.4852571496965423
-        },
-        {
-            "x": 7.215968896352917,
-            "y": 7.308055607481538,
-            "heading": 3.147296630745593,
-            "angularVelocity": -0.049127036764239344,
-            "velocityX": 3.1242472349441757,
-            "velocityY": 1.0701473301581856,
-            "timestamp": 1.5842742930096452
-        },
-        {
-            "x": 7.447984442479348,
-            "y": 7.3875278059692215,
-            "heading": 3.1436483149588383,
-            "angularVelocity": -0.0368452943061847,
-            "velocityX": 2.3431856177952266,
-            "velocityY": 0.8026104958045841,
-            "timestamp": 1.683291436322748
-        },
-        {
-            "x": 7.6026614794307905,
-            "y": 7.440509269529631,
-            "heading": 3.141216104773647,
-            "angularVelocity": -0.024563526110654314,
-            "velocityX": 1.5621238078171693,
-            "velocityY": 0.5350736426810189,
-            "timestamp": 1.7823085796358509
+            "x": 2.3268918247049313,
+            "y": 5.587760380936702,
+            "heading": 0.001219352926916483,
+            "angularVelocity": 0.01231456999280599,
+            "velocityX": 0.776551018417967,
+            "velocityY": 0.2803594812685564,
+            "timestamp": 0.09901709337987798
+        },
+        {
+            "x": 2.480675474257635,
+            "y": 5.643281133231001,
+            "heading": 0.0036580544911558616,
+            "angularVelocity": 0.024629096663863686,
+            "velocityX": 1.5531020382785252,
+            "velocityY": 0.5607188657951702,
+            "timestamp": 0.19803418675975595
+        },
+        {
+            "x": 2.7113509459548553,
+            "y": 5.726562245995426,
+            "heading": 0.007316098440537021,
+            "angularVelocity": 0.03694356019247884,
+            "velocityX": 2.3296530308381804,
+            "velocityY": 0.8410781403663149,
+            "timestamp": 0.29705128013963394
+        },
+        {
+            "x": 3.0189182254277274,
+            "y": 5.837603703876164,
+            "heading": 0.01219347400929413,
+            "angularVelocity": 0.04925791499475694,
+            "velocityX": 3.1062038782828476,
+            "velocityY": 1.1214372598752256,
+            "timestamp": 0.3960683735195119
+        },
+        {
+            "x": 3.3712359540410324,
+            "y": 5.964801318197582,
+            "heading": 0.012193474535478832,
+            "angularVelocity": 5.31407952800269e-9,
+            "velocityX": 3.5581505837748795,
+            "velocityY": 1.2846025870848905,
+            "timestamp": 0.49508546689938987
+        },
+        {
+            "x": 3.723553713186701,
+            "y": 6.0919988479491165,
+            "heading": 0.012193475061665435,
+            "angularVelocity": 5.314098710197071e-9,
+            "velocityX": 3.5581508921293548,
+            "velocityY": 1.2846017329911394,
+            "timestamp": 0.5941025602792679
+        },
+        {
+            "x": 4.075871503881281,
+            "y": 6.219196290314954,
+            "heading": 0.012193475587862622,
+            "angularVelocity": 5.314205602691753e-9,
+            "velocityX": 3.558151210750214,
+            "velocityY": 1.2846008504596915,
+            "timestamp": 0.6931196536591459
+        },
+        {
+            "x": 4.428189328839488,
+            "y": 6.346393637775665,
+            "heading": 0.012193476114071436,
+            "angularVelocity": 5.3143230401144125e-9,
+            "velocityX": 3.5581515567877036,
+            "velocityY": 1.2845998919875412,
+            "timestamp": 0.7921367470390238
+        },
+        {
+            "x": 4.780507205251625,
+            "y": 6.473590842716609,
+            "heading": 0.012193476640289547,
+            "angularVelocity": 5.314416922842863e-9,
+            "velocityX": 3.5581520764346486,
+            "velocityY": 1.2845984526424705,
+            "timestamp": 0.8911538404189018
+        },
+        {
+            "x": 5.134861642335281,
+            "y": 6.594999030315895,
+            "heading": 0.012193476116517958,
+            "angularVelocity": -5.289708644023114e-9,
+            "velocityX": 3.5787198451098035,
+            "velocityY": 1.2261336245600862,
+            "timestamp": 0.9901709337987797
+        },
+        {
+            "x": 5.489216369156675,
+            "y": 6.716406372237666,
+            "heading": 0.012193475592327948,
+            "angularVelocity": -5.293934612313601e-9,
+            "velocityX": 3.578722771248356,
+            "velocityY": 1.2261250838377307,
+            "timestamp": 1.0891880271786578
+        },
+        {
+            "x": 5.843571141725175,
+            "y": 6.837813580636051,
+            "heading": 0.012193475068127738,
+            "angularVelocity": -5.2940376086876394e-9,
+            "velocityX": 3.5787232332605625,
+            "velocityY": 1.2261237353494956,
+            "timestamp": 1.1882051205585358
+        },
+        {
+            "x": 6.1979259496904,
+            "y": 6.959220685720912,
+            "heading": 0.012193474543942472,
+            "angularVelocity": -5.293886679384445e-9,
+            "velocityX": 3.5787235907415074,
+            "velocityY": 1.2261226919587003,
+            "timestamp": 1.2872222139384137
+        },
+        {
+            "x": 6.552280784920285,
+            "y": 7.080627711227486,
+            "heading": 0.012193474019771769,
+            "angularVelocity": -5.293739610148028e-9,
+            "velocityX": 3.5787238660945766,
+            "velocityY": 1.2261218882763663,
+            "timestamp": 1.3862393073182917
+        },
+        {
+            "x": 6.906635640601096,
+            "y": 7.202034677043121,
+            "heading": 0.01219347349560639,
+            "angularVelocity": -5.293685828527132e-9,
+            "velocityX": 3.5787240726339373,
+            "velocityY": 1.2261212854416952,
+            "timestamp": 1.4852564006981697
+        },
+        {
+            "x": 7.215981364626797,
+            "y": 7.3080208081486955,
+            "heading": 0.0073160851772923075,
+            "angularVelocity": -0.0492580437559288,
+            "velocityX": 3.124164863524117,
+            "velocityY": 1.0703821682480574,
+            "timestamp": 1.5842734940780476
+        },
+        {
+            "x": 7.447990676616371,
+            "y": 7.387510406301363,
+            "heading": 0.003658042213644346,
+            "angularVelocity": -0.03694355023729677,
+            "velocityX": 2.343123839229173,
+            "velocityY": 0.8027866244033968,
+            "timestamp": 1.6832905874579256
+        },
+        {
+            "x": 7.602663557476493,
+            "y": 7.440503469639918,
+            "heading": 0.0012193472043836597,
+            "angularVelocity": -0.024629030463387942,
+            "velocityX": 1.5620826221056736,
+            "velocityY": 0.5351910617619017,
+            "timestamp": 1.7823076808378036
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": -0.012281759834182485,
-            "velocityX": 0.7810619250512634,
-            "velocityY": 0.2675368081121253,
-            "timestamp": 1.8813257229489537
+            "heading": 1.0919443501059542e-30,
+            "angularVelocity": -0.01231451219942279,
+            "velocityX": 0.781041332195102,
+            "velocityY": 0.26759551765904444,
+            "timestamp": 1.8813247742176815
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 1.8685058153122784e-28,
+            "heading": 0,
+            "angularVelocity": -8.461898211343436e-30,
             "velocityX": 0,
-            "velocityY": -1.820396114239373e-31,
-            "timestamp": 1.9803428662620566
+            "velocityY": -1.0908466908101513e-31,
+            "timestamp": 1.9803418675975595
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.traj
index e091b127..20e2615d 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.traj	
@@ -3,191 +3,191 @@
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 3.909956769253128e-30,
-            "velocityX": -2.302876892990051e-31,
-            "velocityY": 1.6352823102584027e-31,
+            "heading": 0,
+            "angularVelocity": -9.278807284273418e-29,
+            "velocityX": -7.385267906050254e-32,
+            "velocityY": -1.1558173496237395e-34,
             "timestamp": 0
         },
         {
-            "x": 2.326889522840126,
-            "y": 5.587767009829938,
-            "heading": 3.1412161105492817,
-            "angularVelocity": 0.012281818163829663,
-            "velocityX": 0.7765273796780119,
-            "velocityY": 0.2804262868111325,
-            "timestamp": 0.0990171433131028
-        },
-        {
-            "x": 2.4806685686648633,
-            "y": 5.6433010199081535,
-            "heading": 3.143648327345736,
-            "angularVelocity": 0.024563592879525555,
-            "velocityX": 1.5530547608152194,
-            "velocityY": 0.560852476854545,
-            "timestamp": 0.1980342866262056
-        },
-        {
-            "x": 2.711337134772754,
-            "y": 5.72660201934395,
-            "heading": 3.147296644122033,
-            "angularVelocity": 0.03684530429983614,
-            "velocityX": 2.3295821146697007,
-            "velocityY": 0.8412785569099963,
-            "timestamp": 0.2970514299393084
-        },
-        {
-            "x": 3.0188952067968695,
-            "y": 5.837669992778753,
-            "heading": 3.1521610500975985,
-            "angularVelocity": 0.04912690684429555,
-            "velocityX": 3.106109323428804,
-            "velocityY": 1.1217044818551853,
-            "timestamp": 0.3960685732524112
-        },
-        {
-            "x": 3.3712021943007997,
-            "y": 5.964897910508787,
-            "heading": 3.1521610506223987,
-            "angularVelocity": 5.300094570798509e-9,
-            "velocityX": 3.558040312169958,
-            "velocityY": 1.2849079813151831,
-            "timestamp": 0.495085716565514
-        },
-        {
-            "x": 3.7235092123405478,
-            "y": 6.092125743682096,
-            "heading": 3.1521610511472007,
-            "angularVelocity": 5.300112966968611e-9,
-            "velocityX": 3.558040620559158,
-            "velocityY": 1.2849071273547275,
-            "timestamp": 0.5941028598786168
-        },
-        {
-            "x": 4.075816261929694,
-            "y": 6.219353489491835,
-            "heading": 3.1521610516720138,
-            "angularVelocity": 5.300218907643243e-9,
-            "velocityX": 3.5580409391847763,
-            "velocityY": 1.284906245047206,
-            "timestamp": 0.6931200031917195
-        },
-        {
-            "x": 4.428123345781717,
-            "y": 6.346581140424019,
-            "heading": 3.152161052196838,
-            "angularVelocity": 5.300337345645622e-9,
-            "velocityX": 3.558041285214527,
-            "velocityY": 1.284905286853995,
-            "timestamp": 0.7921371465048224
-        },
-        {
-            "x": 4.780430481129625,
-            "y": 6.473808648758581,
-            "heading": 3.152161052721672,
-            "angularVelocity": 5.300431772031768e-9,
-            "velocityX": 3.558041805284913,
-            "velocityY": 1.2849038467233598,
-            "timestamp": 0.8911542898179252
-        },
-        {
-            "x": 5.134794229593029,
-            "y": 6.595190238737374,
-            "heading": 3.1521610521993333,
-            "angularVelocity": -5.2752356524836585e-9,
-            "velocityX": 3.5788120784587116,
-            "velocityY": 1.225864389916352,
-            "timestamp": 0.9901714331310281
-        },
-        {
-            "x": 5.489158282910728,
-            "y": 6.716570938689376,
-            "heading": 3.152161051676551,
-            "angularVelocity": -5.279713305479767e-9,
-            "velocityX": 3.578815157261821,
-            "velocityY": 1.225855401303425,
-            "timestamp": 1.089188576444131
-        },
-        {
-            "x": 5.843522381964551,
-            "y": 6.837951505117218,
-            "heading": 3.1521610511537586,
-            "angularVelocity": -5.279818233241214e-9,
-            "velocityX": 3.5788156191628895,
-            "velocityY": 1.2258540528080475,
-            "timestamp": 1.1882057197572338
-        },
-        {
-            "x": 6.19788651637458,
-            "y": 6.959331968324431,
-            "heading": 3.1521610506309807,
-            "angularVelocity": -5.279669747527449e-9,
-            "velocityX": 3.5788159762344476,
-            "velocityY": 1.2258530103559488,
-            "timestamp": 1.2872228630703366
-        },
-        {
-            "x": 6.5522506780354695,
-            "y": 7.080712351974079,
-            "heading": 3.1521610501082176,
-            "angularVelocity": -5.2795212284316414e-9,
-            "velocityX": 3.5788162514480155,
-            "velocityY": 1.2258522068833249,
-            "timestamp": 1.3862400063834395
-        },
-        {
-            "x": 6.9066148601448845,
-            "y": 7.202092675925132,
-            "heading": 3.1521610495854597,
-            "angularVelocity": -5.279467664817609e-9,
-            "velocityX": 3.5788164579630073,
-            "velocityY": 1.2258516039716054,
-            "timestamp": 1.4852571496965423
-        },
-        {
-            "x": 7.215968896352917,
-            "y": 7.308055607481538,
-            "heading": 3.147296630745593,
-            "angularVelocity": -0.049127036764239344,
-            "velocityX": 3.1242472349441757,
-            "velocityY": 1.0701473301581856,
-            "timestamp": 1.5842742930096452
-        },
-        {
-            "x": 7.447984442479348,
-            "y": 7.3875278059692215,
-            "heading": 3.1436483149588383,
-            "angularVelocity": -0.0368452943061847,
-            "velocityX": 2.3431856177952266,
-            "velocityY": 0.8026104958045841,
-            "timestamp": 1.683291436322748
-        },
-        {
-            "x": 7.6026614794307905,
-            "y": 7.440509269529631,
-            "heading": 3.141216104773647,
-            "angularVelocity": -0.024563526110654314,
-            "velocityX": 1.5621238078171693,
-            "velocityY": 0.5350736426810189,
-            "timestamp": 1.7823085796358509
+            "x": 2.3268918247049313,
+            "y": 5.587760380936702,
+            "heading": 0.001219352926916483,
+            "angularVelocity": 0.01231456999280599,
+            "velocityX": 0.776551018417967,
+            "velocityY": 0.2803594812685564,
+            "timestamp": 0.09901709337987798
+        },
+        {
+            "x": 2.480675474257635,
+            "y": 5.643281133231001,
+            "heading": 0.0036580544911558616,
+            "angularVelocity": 0.024629096663863686,
+            "velocityX": 1.5531020382785252,
+            "velocityY": 0.5607188657951702,
+            "timestamp": 0.19803418675975595
+        },
+        {
+            "x": 2.7113509459548553,
+            "y": 5.726562245995426,
+            "heading": 0.007316098440537021,
+            "angularVelocity": 0.03694356019247884,
+            "velocityX": 2.3296530308381804,
+            "velocityY": 0.8410781403663149,
+            "timestamp": 0.29705128013963394
+        },
+        {
+            "x": 3.0189182254277274,
+            "y": 5.837603703876164,
+            "heading": 0.01219347400929413,
+            "angularVelocity": 0.04925791499475694,
+            "velocityX": 3.1062038782828476,
+            "velocityY": 1.1214372598752256,
+            "timestamp": 0.3960683735195119
+        },
+        {
+            "x": 3.3712359540410324,
+            "y": 5.964801318197582,
+            "heading": 0.012193474535478832,
+            "angularVelocity": 5.31407952800269e-9,
+            "velocityX": 3.5581505837748795,
+            "velocityY": 1.2846025870848905,
+            "timestamp": 0.49508546689938987
+        },
+        {
+            "x": 3.723553713186701,
+            "y": 6.0919988479491165,
+            "heading": 0.012193475061665435,
+            "angularVelocity": 5.314098710197071e-9,
+            "velocityX": 3.5581508921293548,
+            "velocityY": 1.2846017329911394,
+            "timestamp": 0.5941025602792679
+        },
+        {
+            "x": 4.075871503881281,
+            "y": 6.219196290314954,
+            "heading": 0.012193475587862622,
+            "angularVelocity": 5.314205602691753e-9,
+            "velocityX": 3.558151210750214,
+            "velocityY": 1.2846008504596915,
+            "timestamp": 0.6931196536591459
+        },
+        {
+            "x": 4.428189328839488,
+            "y": 6.346393637775665,
+            "heading": 0.012193476114071436,
+            "angularVelocity": 5.3143230401144125e-9,
+            "velocityX": 3.5581515567877036,
+            "velocityY": 1.2845998919875412,
+            "timestamp": 0.7921367470390238
+        },
+        {
+            "x": 4.780507205251625,
+            "y": 6.473590842716609,
+            "heading": 0.012193476640289547,
+            "angularVelocity": 5.314416922842863e-9,
+            "velocityX": 3.5581520764346486,
+            "velocityY": 1.2845984526424705,
+            "timestamp": 0.8911538404189018
+        },
+        {
+            "x": 5.134861642335281,
+            "y": 6.594999030315895,
+            "heading": 0.012193476116517958,
+            "angularVelocity": -5.289708644023114e-9,
+            "velocityX": 3.5787198451098035,
+            "velocityY": 1.2261336245600862,
+            "timestamp": 0.9901709337987797
+        },
+        {
+            "x": 5.489216369156675,
+            "y": 6.716406372237666,
+            "heading": 0.012193475592327948,
+            "angularVelocity": -5.293934612313601e-9,
+            "velocityX": 3.578722771248356,
+            "velocityY": 1.2261250838377307,
+            "timestamp": 1.0891880271786578
+        },
+        {
+            "x": 5.843571141725175,
+            "y": 6.837813580636051,
+            "heading": 0.012193475068127738,
+            "angularVelocity": -5.2940376086876394e-9,
+            "velocityX": 3.5787232332605625,
+            "velocityY": 1.2261237353494956,
+            "timestamp": 1.1882051205585358
+        },
+        {
+            "x": 6.1979259496904,
+            "y": 6.959220685720912,
+            "heading": 0.012193474543942472,
+            "angularVelocity": -5.293886679384445e-9,
+            "velocityX": 3.5787235907415074,
+            "velocityY": 1.2261226919587003,
+            "timestamp": 1.2872222139384137
+        },
+        {
+            "x": 6.552280784920285,
+            "y": 7.080627711227486,
+            "heading": 0.012193474019771769,
+            "angularVelocity": -5.293739610148028e-9,
+            "velocityX": 3.5787238660945766,
+            "velocityY": 1.2261218882763663,
+            "timestamp": 1.3862393073182917
+        },
+        {
+            "x": 6.906635640601096,
+            "y": 7.202034677043121,
+            "heading": 0.01219347349560639,
+            "angularVelocity": -5.293685828527132e-9,
+            "velocityX": 3.5787240726339373,
+            "velocityY": 1.2261212854416952,
+            "timestamp": 1.4852564006981697
+        },
+        {
+            "x": 7.215981364626797,
+            "y": 7.3080208081486955,
+            "heading": 0.0073160851772923075,
+            "angularVelocity": -0.0492580437559288,
+            "velocityX": 3.124164863524117,
+            "velocityY": 1.0703821682480574,
+            "timestamp": 1.5842734940780476
+        },
+        {
+            "x": 7.447990676616371,
+            "y": 7.387510406301363,
+            "heading": 0.003658042213644346,
+            "angularVelocity": -0.03694355023729677,
+            "velocityX": 2.343123839229173,
+            "velocityY": 0.8027866244033968,
+            "timestamp": 1.6832905874579256
+        },
+        {
+            "x": 7.602663557476493,
+            "y": 7.440503469639918,
+            "heading": 0.0012193472043836597,
+            "angularVelocity": -0.024629030463387942,
+            "velocityX": 1.5620826221056736,
+            "velocityY": 0.5351910617619017,
+            "timestamp": 1.7823076808378036
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": -0.012281759834182485,
-            "velocityX": 0.7810619250512634,
-            "velocityY": 0.2675368081121253,
-            "timestamp": 1.8813257229489537
+            "heading": 1.0919443501059542e-30,
+            "angularVelocity": -0.01231451219942279,
+            "velocityX": 0.781041332195102,
+            "velocityY": 0.26759551765904444,
+            "timestamp": 1.8813247742176815
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 1.8685058153122784e-28,
+            "heading": 0,
+            "angularVelocity": -8.461898211343436e-30,
             "velocityX": 0,
-            "velocityY": -1.820396114239373e-31,
-            "timestamp": 1.9803428662620566
+            "velocityY": -1.0908466908101513e-31,
+            "timestamp": 1.9803418675975595
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.1.traj
index ae5af9d6..e1aa1d32 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.1.traj	
@@ -3,524 +3,524 @@
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": 4.173822195239149e-34,
+            "velocityY": -1.3324658216077414e-32,
             "timestamp": 0
         },
         {
-            "x": 2.260404640394534,
-            "y": 5.564834920701341,
-            "heading": 3.1398005529486164,
-            "angularVelocity": -0.005376730415880204,
-            "velocityX": 0.2804902157612115,
-            "velocityY": 0.1303406844718941,
-            "timestamp": 0.037094486045788126
-        },
-        {
-            "x": 2.2812139214883365,
-            "y": 5.574504760754136,
-            "heading": 3.139401659951885,
-            "angularVelocity": -0.010753431015026859,
-            "velocityX": 0.5609804397374861,
-            "velocityY": 0.26068133255328624,
-            "timestamp": 0.07418897209157625
-        },
-        {
-            "x": 2.3124278435365335,
-            "y": 5.58900951877657,
-            "heading": 3.1388033223810634,
-            "angularVelocity": -0.016130094647582943,
-            "velocityX": 0.8414706705915215,
-            "velocityY": 0.3910219433834154,
-            "timestamp": 0.11128345813736437
-        },
-        {
-            "x": 2.3540464067273468,
-            "y": 5.608349193345913,
-            "heading": 3.1380055419662973,
-            "angularVelocity": -0.02150671163853717,
-            "velocityX": 1.1219609065196614,
-            "velocityY": 0.5213625158593963,
-            "timestamp": 0.1483779441831525
-        },
-        {
-            "x": 2.4060696111557784,
-            "y": 5.632523782985854,
-            "heading": 3.1370083209408497,
-            "angularVelocity": -0.026883268424770904,
-            "velocityX": 1.4024511450088724,
-            "velocityY": 0.651703048536671,
-            "timestamp": 0.18547243022894064
-        },
-        {
-            "x": 2.4684974567819364,
-            "y": 5.661533286147284,
-            "heading": 3.1358116622714935,
-            "angularVelocity": -0.032259745232204366,
-            "velocityX": 1.6829413824226938,
-            "velocityY": 0.7820435394527967,
-            "timestamp": 0.22256691627472877
-        },
-        {
-            "x": 2.541329943361364,
-            "y": 5.695377701176762,
-            "heading": 3.1344155700455096,
-            "angularVelocity": -0.03763611185395848,
-            "velocityX": 1.9634316132463963,
-            "velocityY": 0.9123839857951219,
-            "timestamp": 0.2596614023205169
-        },
-        {
-            "x": 2.6245670703196087,
-            "y": 5.734057026259781,
-            "heading": 3.1328200501674663,
-            "angularVelocity": -0.04301231929927239,
-            "velocityX": 2.243921828583893,
-            "velocityY": 1.0427243832216457,
-            "timestamp": 0.296755888366305
-        },
-        {
-            "x": 2.7182088365016934,
-            "y": 5.777571259306566,
-            "heading": 3.1310251117440115,
-            "angularVelocity": -0.04838828124588956,
-            "velocityX": 2.5244120127853087,
-            "velocityY": 1.17306472431166,
-            "timestamp": 0.33385037441209314
-        },
-        {
-            "x": 2.822255239588457,
-            "y": 5.825920397683519,
-            "heading": 3.1290307702747278,
-            "angularVelocity": -0.053763825351902786,
-            "velocityX": 2.804902134466352,
-            "velocityY": 1.3034049944046322,
-            "timestamp": 0.3709448604578813
-        },
-        {
-            "x": 2.9367062743473644,
-            "y": 5.879104437402994,
-            "heading": 3.126837057015217,
-            "angularVelocity": -0.05913852686363322,
-            "velocityX": 3.0853921150877586,
-            "velocityY": 1.4337451569978974,
-            "timestamp": 0.4080393465036694
-        },
-        {
-            "x": 3.0615619238881426,
-            "y": 5.937123369060452,
-            "heading": 3.1244440632726094,
-            "angularVelocity": -0.06451076690090568,
-            "velocityX": 3.3658816403780625,
-            "velocityY": 1.5640850660618872,
-            "timestamp": 0.44513383254945754
-        },
-        {
-            "x": 3.188819545942614,
-            "y": 5.996258327229638,
-            "heading": 3.124444062637546,
-            "angularVelocity": -1.7120162809754954e-8,
-            "velocityX": 3.430634458646746,
-            "velocityY": 1.594171114709378,
-            "timestamp": 0.4822283185952457
-        },
-        {
-            "x": 3.3160771824859174,
-            "y": 6.055393254219169,
-            "heading": 3.1244440620025067,
-            "angularVelocity": -1.711950789942117e-8,
-            "velocityX": 3.4306348492393433,
-            "velocityY": 1.5941702741624886,
-            "timestamp": 0.5193228046410338
-        },
-        {
-            "x": 3.4433348335743874,
-            "y": 6.114528149907673,
-            "heading": 3.1244440613674698,
-            "angularVelocity": -1.7119436749370044e-8,
-            "velocityX": 3.4306352413506347,
-            "velocityY": 1.5941694303436482,
-            "timestamp": 0.5564172906868219
-        },
-        {
-            "x": 3.570592499306144,
-            "y": 6.173663014083979,
-            "heading": 3.124444060732438,
-            "angularVelocity": -1.7119304165961875e-8,
-            "velocityX": 3.4306356361070582,
-            "velocityY": 1.5941685808319566,
-            "timestamp": 0.59351177673261
-        },
-        {
-            "x": 3.6978501798346053,
-            "y": 6.232797846417909,
-            "heading": 3.1244440600974133,
-            "angularVelocity": -1.7119116923870704e-8,
-            "velocityX": 3.430636034999362,
-            "velocityY": 1.5941677224193291,
-            "timestamp": 0.6306062627783982
-        },
-        {
-            "x": 3.825107875386636,
-            "y": 6.291932646421232,
-            "heading": 3.1244440594623972,
-            "angularVelocity": -1.7118879018616895e-8,
-            "velocityX": 3.430636440007508,
-            "velocityY": 1.594166850844881,
-            "timestamp": 0.6677007488241863
-        },
-        {
-            "x": 3.9523655862834066,
-            "y": 6.351067413402769,
-            "heading": 3.1244440588273914,
-            "angularVelocity": -1.711859691944282e-8,
-            "velocityX": 3.4306368536738447,
-            "velocityY": 1.5941659606374654,
-            "timestamp": 0.7047952348699744
-        },
-        {
-            "x": 4.079623312940078,
-            "y": 6.410202146469073,
-            "heading": 3.1244440581923967,
-            "angularVelocity": -1.711830453291075e-8,
-            "velocityX": 3.4306372785321515,
-            "velocityY": 1.5941650463443597,
-            "timestamp": 0.7418897209157626
-        },
-        {
-            "x": 4.206881055880688,
-            "y": 6.469336844492392,
-            "heading": 3.124444057557412,
-            "angularVelocity": -1.711804273623487e-8,
-            "velocityX": 3.430637717517568,
-            "velocityY": 1.5941641016490944,
-            "timestamp": 0.7789842069615507
-        },
-        {
-            "x": 4.334138815794606,
-            "y": 6.528471505989175,
-            "heading": 3.1244440569224348,
-            "angularVelocity": -1.7117826690263867e-8,
-            "velocityX": 3.4306381750871577,
-            "velocityY": 1.59416311696,
-            "timestamp": 0.8160786930073388
-        },
-        {
-            "x": 4.461396593547393,
-            "y": 6.587606129096708,
-            "heading": 3.1244440562874636,
-            "angularVelocity": -1.7117677202487012e-8,
-            "velocityX": 3.430638655990672,
-            "velocityY": 1.5941620820555098,
-            "timestamp": 0.853173179053127
-        },
-        {
-            "x": 4.5886544601407655,
-            "y": 6.646740561018822,
-            "heading": 3.124444055652494,
-            "angularVelocity": -1.711762346285072e-8,
-            "velocityX": 3.4306410509715453,
-            "velocityY": 1.5941569280437173,
-            "timestamp": 0.8902676650989151
-        },
-        {
-            "x": 4.7169368908362825,
-            "y": 6.703618180644166,
-            "heading": 3.1244440550164043,
-            "angularVelocity": -1.7147829637187305e-8,
-            "velocityX": 3.458261439103553,
-            "velocityY": 1.5333173656899308,
-            "timestamp": 0.9273621511447032
-        },
-        {
-            "x": 4.8494363294608345,
-            "y": 6.749827966380956,
-            "heading": 3.124444054367944,
-            "angularVelocity": -1.7481320274300667e-8,
-            "velocityX": 3.5719443170340583,
-            "velocityY": 1.245731931148746,
-            "timestamp": 0.9644566371904914
-        },
-        {
-            "x": 4.985267831359121,
-            "y": 6.785059321241209,
-            "heading": 3.124444053688418,
-            "angularVelocity": -1.8318771799358465e-8,
-            "velocityX": 3.6617706936448973,
-            "velocityY": 0.9497733656900947,
-            "timestamp": 1.0015511232362795
-        },
-        {
-            "x": 5.12352345692124,
-            "y": 6.809076540845587,
-            "heading": 3.1244440529552464,
-            "angularVelocity": -1.9764980034334986e-8,
-            "velocityX": 3.7271206667066523,
-            "velocityY": 0.6474606380779142,
-            "timestamp": 1.0386456092820675
-        },
-        {
-            "x": 5.26327900081687,
-            "y": 6.821719014416903,
-            "heading": 3.124444052138408,
-            "angularVelocity": -2.2020481247534803e-8,
-            "velocityX": 3.767555741926746,
-            "velocityY": 0.3408181354961927,
-            "timestamp": 1.0757400953278555
+            "x": 2.2604047390881647,
+            "y": 5.564834315872384,
+            "heading": -0.00020682184904158323,
+            "angularVelocity": -0.005575579259585174,
+            "velocityX": 0.2804947723375417,
+            "velocityY": 0.13032526030129518,
+            "timestamp": 0.037094235309470626
+        },
+        {
+            "x": 2.2812142175691745,
+            "y": 5.574502946267583,
+            "heading": -0.0006204643713099006,
+            "angularVelocity": -0.011151126821122416,
+            "velocityX": 0.5609895528887481,
+            "velocityY": 0.2606504842204101,
+            "timestamp": 0.07418847061894125
+        },
+        {
+            "x": 2.3124284356981284,
+            "y": 5.589005889804122,
+            "heading": -0.0012409261115257607,
+            "angularVelocity": -0.01672663515070949,
+            "velocityX": 0.8414843403169981,
+            "velocityY": 0.39097567089719987,
+            "timestamp": 0.11128270592841188
+        },
+        {
+            "x": 2.3540473936632536,
+            "y": 5.608343145059642,
+            "heading": -0.0020682052367836136,
+            "angularVelocity": -0.022302094068156527,
+            "velocityX": 1.121979132819583,
+            "velocityY": 0.5213008192295232,
+            "timestamp": 0.1483769412378825
+        },
+        {
+            "x": 2.4060710915596104,
+            "y": 5.632514710558234,
+            "heading": -0.0031022993856005916,
+            "angularVelocity": -0.027877489323882353,
+            "velocityX": 1.402473927884819,
+            "velocityY": 0.6516259277737647,
+            "timestamp": 0.18547117654735312
+        },
+        {
+            "x": 2.46849952934744,
+            "y": 5.661520584751242,
+            "heading": -0.004343205426988983,
+            "angularVelocity": -0.03345280017329315,
+            "velocityX": 1.6829687218782206,
+            "velocityY": 0.781950994568762,
+            "timestamp": 0.22256541185682374
+        },
+        {
+            "x": 2.5413327067825313,
+            "y": 5.695360765985745,
+            "heading": -0.005790919055957113,
+            "angularVelocity": -0.039027994967159495,
+            "velocityX": 1.9634635092880914,
+            "velocityY": 0.9122760168036823,
+            "timestamp": 0.25965964716629436
+        },
+        {
+            "x": 2.6245706232908623,
+            "y": 5.734035252447861,
+            "heading": -0.007445434065373688,
+            "angularVelocity": -0.044603022426903935,
+            "velocityX": 2.2439582812232635,
+            "velocityY": 1.042600990139337,
+            "timestamp": 0.296753882475765
+        },
+        {
+            "x": 2.7182132777182155,
+            "y": 5.777544042048614,
+            "heading": -0.009306740898831883,
+            "angularVelocity": -0.05017779226152841,
+            "velocityX": 2.5244530220426173,
+            "velocityY": 1.1729259071596103,
+            "timestamp": 0.33384811778523565
+        },
+        {
+            "x": 2.8222606677468502,
+            "y": 5.825887132155531,
+            "heading": -0.011374823315541767,
+            "angularVelocity": -0.05575212427098156,
+            "velocityX": 2.804947700379475,
+            "velocityY": 1.3032507532126736,
+            "timestamp": 0.3709423530947063
+        },
+        {
+            "x": 2.9367127881472834,
+            "y": 5.879064518782887,
+            "heading": -0.013649648601341033,
+            "angularVelocity": -0.0613255743600492,
+            "velocityX": 3.0854422377380093,
+            "velocityY": 1.4335754918171768,
+            "timestamp": 0.40803658840417695
+        },
+        {
+            "x": 3.061569622038521,
+            "y": 5.9370761925311655,
+            "heading": -0.016131121234305167,
+            "angularVelocity": -0.06689644933404303,
+            "velocityX": 3.365936320012513,
+            "velocityY": 1.563899977025988,
+            "timestamp": 0.4451308237136476
+        },
+        {
+            "x": 3.188829425192231,
+            "y": 5.996204205741329,
+            "heading": -0.01613112189285862,
+            "angularVelocity": -1.7753525563113908e-8,
+            "velocityX": 3.430716446693241,
+            "velocityY": 1.5939946656635118,
+            "timestamp": 0.48222505902311824
+        },
+        {
+            "x": 3.3160892428339666,
+            "y": 6.055332187769375,
+            "heading": -0.01613112255138683,
+            "angularVelocity": -1.775284497104555e-8,
+            "velocityX": 3.4307168372667594,
+            "velocityY": 1.5939938250445522,
+            "timestamp": 0.5193192943325888
+        },
+        {
+            "x": 3.443349075020492,
+            "y": 6.114460138492994,
+            "heading": -0.016131123209912376,
+            "angularVelocity": -1.775277322447583e-8,
+            "velocityX": 3.43071722937053,
+            "velocityY": 1.593992981128321,
+            "timestamp": 0.5564135296420595
+        },
+        {
+            "x": 3.5706089218502877,
+            "y": 6.173588057700207,
+            "heading": -0.01613112386843302,
+            "angularVelocity": -1.775264097704561e-8,
+            "velocityX": 3.430717624129179,
+            "velocityY": 1.5939921314975005,
+            "timestamp": 0.5935077649515301
+        },
+        {
+            "x": 3.697868783477107,
+            "y": 6.2327159450600735,
+            "heading": -0.016131124526946636,
+            "angularVelocity": -1.7752451586110053e-8,
+            "velocityX": 3.430718023032762,
+            "velocityY": 1.5939912729450545,
+            "timestamp": 0.6306020002610008
+        },
+        {
+            "x": 3.8251286601281493,
+            "y": 6.291843800083575,
+            "heading": -0.016131125185451407,
+            "angularVelocity": -1.7752213148650806e-8,
+            "velocityX": 3.4307184280612866,
+            "velocityY": 1.5939904012094348,
+            "timestamp": 0.6676962355704714
+        },
+        {
+            "x": 3.95238855212494,
+            "y": 6.350971622078674,
+            "heading": -0.016131125843945708,
+            "angularVelocity": -1.775193091205886e-8,
+            "velocityX": 3.430718841757593,
+            "velocityY": 1.5939895108176958,
+            "timestamp": 0.7047904708799421
+        },
+        {
+            "x": 4.079648459881884,
+            "y": 6.410099410153433,
+            "heading": -0.01613112650242917,
+            "angularVelocity": -1.7751638714463086e-8,
+            "velocityX": 3.430719266625584,
+            "velocityY": 1.5939885963806835,
+            "timestamp": 0.7418847061894127
+        },
+        {
+            "x": 4.2069083839232695,
+            "y": 6.469227163179404,
+            "heading": -0.01613112716090301,
+            "angularVelocity": -1.7751379248608663e-8,
+            "velocityX": 3.430719705627546,
+            "velocityY": 1.5939876515226545,
+            "timestamp": 0.7789789414988834
+        },
+        {
+            "x": 4.334168324938812,
+            "y": 6.528354879672101,
+            "heading": -0.016131127819368877,
+            "angularVelocity": -1.7751164242273923e-8,
+            "velocityX": 3.430720163223074,
+            "velocityY": 1.5939866666451934,
+            "timestamp": 0.816073176808354
+        },
+        {
+            "x": 4.461428283794271,
+            "y": 6.58748255776812,
+            "heading": -0.01613112847782922,
+            "angularVelocity": -1.775101542628313e-8,
+            "velocityX": 3.430720644158123,
+            "velocityY": 1.5939856315335348,
+            "timestamp": 0.8531674121178247
+        },
+        {
+            "x": 4.58868833054201,
+            "y": 6.646610046694028,
+            "heading": -0.016131129136287645,
+            "angularVelocity": -1.7750963701844846e-8,
+            "velocityX": 3.430723013590394,
+            "velocityY": 1.5939805318162525,
+            "timestamp": 0.8902616474272953
+        },
+        {
+            "x": 4.716960790002456,
+            "y": 6.703507810971908,
+            "heading": -0.016131129795891656,
+            "angularVelocity": -1.7781846861521853e-8,
+            "velocityX": 3.4580160068077612,
+            "velocityY": 1.5338707969903813,
+            "timestamp": 0.927355882736766
+        },
+        {
+            "x": 4.849451901724698,
+            "y": 6.749738586923234,
+            "heading": -0.016131130468276335,
+            "angularVelocity": -1.8126392792981438e-8,
+            "velocityX": 3.5717439816966663,
+            "velocityY": 1.2463062135027452,
+            "timestamp": 0.9644501180462366
+        },
+        {
+            "x": 4.985276799458285,
+            "y": 6.784991618369029,
+            "heading": -0.016131131172819703,
+            "angularVelocity": -1.899333848953808e-8,
+            "velocityX": 3.661617407676018,
+            "velocityY": 0.950364150970728,
+            "timestamp": 1.0015443533557071
+        },
+        {
+            "x": 5.12352760039525,
+            "y": 6.80903105836077,
+            "heading": -0.01613113193292308,
+            "angularVelocity": -2.0491145599848975e-8,
+            "velocityX": 3.7270157959468126,
+            "velocityY": 0.6480640399022476,
+            "timestamp": 1.0386385886651777
+        },
+        {
+            "x": 5.263280144021708,
+            "y": 6.821696149590996,
+            "heading": -0.01613113277968929,
+            "angularVelocity": -2.2827434072857776e-8,
+            "velocityX": 3.767500326143083,
+            "velocityY": 0.341430174380559,
+            "timestamp": 1.0757328239746482
         },
         {
             "x": 5.403600215911865,
             "y": 6.822902202606201,
-            "heading": 3.124444051205612,
-            "angularVelocity": -2.5146473765828148e-8,
-            "velocityX": 3.782805210504538,
-            "velocityY": 0.03189660554501852,
-            "timestamp": 1.1128345813736435
-        },
-        {
-            "x": 5.530809022252107,
-            "y": 6.8145114887459926,
-            "heading": 3.1244440503418645,
-            "angularVelocity": -2.5630462052464063e-8,
-            "velocityX": 3.774737065106383,
-            "velocityY": -0.24898227978112425,
-            "timestamp": 1.1465346256803473
-        },
-        {
-            "x": 5.657044071820609,
-            "y": 6.79670145003808,
-            "heading": 3.1244440495837504,
-            "angularVelocity": -2.2495928655681543e-8,
-            "velocityX": 3.745842243399973,
-            "velocityY": -0.5284871006643926,
-            "timestamp": 1.180234669987051
-        },
-        {
-            "x": 5.781608875607717,
-            "y": 6.769570371843818,
-            "heading": 3.124444048897901,
-            "angularVelocity": -2.0351591481890114e-8,
-            "velocityX": 3.6962801192023655,
-            "velocityY": -0.8050754458167036,
-            "timestamp": 1.2139347142937547
-        },
-        {
-            "x": 5.903816171014302,
-            "y": 6.733267988087913,
-            "heading": 3.1244440482605893,
-            "angularVelocity": -1.891130244410514e-8,
-            "velocityX": 3.6263244728813384,
-            "velocityY": -1.0772206536441533,
-            "timestamp": 1.2476347586004584
-        },
-        {
-            "x": 6.022991737743217,
-            "y": 6.687994699591429,
-            "heading": 3.124444047653726,
-            "angularVelocity": -1.8007800496265653e-8,
-            "velocityX": 3.5363623158563073,
-            "velocityY": -1.3434192573888535,
-            "timestamp": 1.281334802907162
-        },
-        {
-            "x": 6.1384782869774295,
-            "y": 6.6340007793146,
-            "heading": 3.1244440470624193,
-            "angularVelocity": -1.754616693571922e-8,
-            "velocityX": 3.4268960652742955,
-            "velocityY": -1.6021913735611486,
-            "timestamp": 1.3150348472138658
-        },
-        {
-            "x": 6.251164558797792,
-            "y": 6.57438290738676,
-            "heading": 3.1244440464747822,
-            "angularVelocity": -1.7437275134789687e-8,
-            "velocityX": 3.343801889244,
-            "velocityY": -1.769073992462997,
-            "timestamp": 1.3487348915205695
-        },
-        {
-            "x": 6.363850200907552,
-            "y": 6.514763845231331,
-            "heading": 3.124444045887316,
-            "angularVelocity": -1.7432210000565644e-8,
-            "velocityX": 3.343783203493965,
-            "velocityY": -1.769109310742662,
-            "timestamp": 1.3824349358272732
-        },
-        {
-            "x": 6.475774212207488,
-            "y": 6.453726953025987,
-            "heading": 3.124444046521184,
-            "angularVelocity": 1.880910439494107e-8,
-            "velocityX": 3.3211829124412344,
-            "velocityY": -1.8111813637349317,
-            "timestamp": 1.4161349801339769
-        },
-        {
-            "x": 6.587697896511305,
-            "y": 6.392689461208172,
-            "heading": 3.1244440471552553,
-            "angularVelocity": 1.881516364731064e-8,
-            "velocityX": 3.3211732093049866,
-            "velocityY": -1.8111991563664802,
-            "timestamp": 1.4498350244406806
-        },
-        {
-            "x": 6.699621529072565,
-            "y": 6.331651874510795,
-            "heading": 3.124444047789331,
-            "angularVelocity": 1.881528213317004e-8,
-            "velocityX": 3.321171673919603,
-            "velocityY": -1.811201971780052,
-            "timestamp": 1.4835350687473843
-        },
-        {
-            "x": 6.811545132450604,
-            "y": 6.2706142343006634,
-            "heading": 3.124444048423398,
-            "angularVelocity": 1.881501023244474e-8,
-            "velocityX": 3.3211708079497897,
-            "velocityY": -1.8112035596935343,
-            "timestamp": 1.517235113054088
-        },
-        {
-            "x": 6.923468714015748,
-            "y": 6.209576554092949,
-            "heading": 3.1244440490575007,
-            "angularVelocity": 1.8816081178773256e-8,
-            "velocityX": 3.3211701606836845,
-            "velocityY": -1.8112047465638819,
-            "timestamp": 1.5509351573607917
-        },
-        {
-            "x": 7.031544581287166,
-            "y": 6.150637092992946,
-            "heading": 3.126666281190466,
-            "angularVelocity": 0.06594151962352889,
-            "velocityX": 3.2069948124643886,
-            "velocityY": -1.7489431338307682,
-            "timestamp": 1.5846352016674954
-        },
-        {
-            "x": 7.131306941921243,
-            "y": 6.096231417458986,
-            "heading": 3.1287176070894986,
-            "angularVelocity": 0.060870124690714414,
-            "velocityX": 2.960303545186476,
-            "velocityY": -1.6144096143855096,
-            "timestamp": 1.618335245974199
-        },
-        {
-            "x": 7.2227557801672635,
-            "y": 6.046359537502576,
-            "heading": 3.1305980016068635,
-            "angularVelocity": 0.05579798353529268,
-            "velocityX": 2.7136118105289606,
-            "velocityY": -1.4798757978632517,
-            "timestamp": 1.6520352902809028
-        },
-        {
-            "x": 7.305891091138876,
-            "y": 6.001021457128046,
-            "heading": 3.132307456329253,
-            "angularVelocity": 0.05072559272714715,
-            "velocityX": 2.466919930876006,
-            "velocityY": -1.3453418625182645,
-            "timestamp": 1.6857353345876065
-        },
-        {
-            "x": 7.380712872646774,
-            "y": 5.9602171788044025,
-            "heading": 3.1338459674980714,
-            "angularVelocity": 0.04565309038812635,
-            "velocityX": 2.2202279862586183,
-            "velocityY": -1.2108078539091887,
-            "timestamp": 1.7194353788943102
-        },
-        {
-            "x": 7.447221123569205,
-            "y": 5.923946704366036,
-            "heading": 3.135213533279876,
-            "angularVelocity": 0.04058053364434269,
-            "velocityX": 1.9735360083547677,
-            "velocityY": -1.0762737908671516,
-            "timestamp": 1.753135423201014
-        },
-        {
-            "x": 7.505415843310863,
-            "y": 5.892210035316634,
-            "heading": 3.136410152813217,
-            "angularVelocity": 0.035507951338280094,
-            "velocityX": 1.7268440127861169,
-            "velocityY": -0.941739683205353,
-            "timestamp": 1.7868354675077176
-        },
-        {
-            "x": 7.555297031572268,
-            "y": 5.865007172961733,
-            "heading": 3.1374358257913886,
-            "angularVelocity": 0.030435359931185808,
-            "velocityX": 1.4801520083308155,
-            "velocityY": -0.807205536803692,
-            "timestamp": 1.8205355118144213
-        },
-        {
-            "x": 7.596864688235311,
-            "y": 5.8423381184765635,
-            "heading": 3.138290552251957,
-            "angularVelocity": 0.025362769638798365,
-            "velocityX": 1.2334600003707947,
-            "velocityY": -0.6726713555287516,
-            "timestamp": 1.854235556121125
-        },
-        {
-            "x": 7.6301188133003475,
-            "y": 5.824202872944982,
-            "heading": 3.1389743324564656,
-            "angularVelocity": 0.020290187107322288,
-            "velocityX": 0.9867679924213419,
-            "velocityY": -0.5381371420919633,
-            "timestamp": 1.8879356004278287
-        },
-        {
-            "x": 7.655059406849112,
-            "y": 5.8106014373839985,
-            "heading": 3.1394871668082627,
-            "angularVelocity": 0.015217616544653215,
-            "velocityX": 0.7400759868972333,
-            "velocityY": -0.4036028984768356,
-            "timestamp": 1.9216356447345324
-        },
-        {
-            "x": 7.671686469021436,
-            "y": 5.801533812759989,
-            "heading": 3.1398290558033333,
-            "angularVelocity": 0.010145060699586378,
-            "velocityX": 0.4933839855224485,
-            "velocityY": -0.2690686261859067,
-            "timestamp": 1.9553356890412361
+            "heading": -0.016131133746573465,
+            "angularVelocity": -2.6065618224331403e-8,
+            "velocityX": 3.782799961220162,
+            "velocityY": 0.03251321951088522,
+            "timestamp": 1.1128270592841187
+        },
+        {
+            "x": 5.530811600424101,
+            "y": 6.814532053807006,
+            "heading": -0.016131134641902804,
+            "angularVelocity": -2.6567346760379892e-8,
+            "velocityX": 3.7747774237594087,
+            "velocityY": -0.24836966315439601,
+            "timestamp": 1.1465274262810004
+        },
+        {
+            "x": 5.6570507349000225,
+            "y": 6.796742242248266,
+            "heading": -0.016131135427811184,
+            "angularVelocity": -2.332046941486525e-8,
+            "velocityX": 3.7459275884919543,
+            "velocityY": -0.5278818346513846,
+            "timestamp": 1.180227793277882
+        },
+        {
+            "x": 5.781621094465109,
+            "y": 6.769630943607204,
+            "heading": -0.01613113613886582,
+            "angularVelocity": -2.1099314246183972e-8,
+            "velocityX": 3.696409584400467,
+            "velocityY": -0.8044808130300218,
+            "timestamp": 1.2139281602747636
+        },
+        {
+            "x": 5.903835372739644,
+            "y": 6.733347785640362,
+            "heading": -0.016131136799649604,
+            "angularVelocity": -1.9607613962388268e-8,
+            "velocityX": 3.6264969543460657,
+            "velocityY": -1.0766398469844052,
+            "timestamp": 1.2476285272716452
+        },
+        {
+            "x": 6.023019298132563,
+            "y": 6.68809306728661,
+            "heading": -0.01613113742890953,
+            "angularVelocity": -1.8672198003746715e-8,
+            "velocityX": 3.536576483097275,
+            "velocityY": -1.3428553569739705,
+            "timestamp": 1.2813288942685268
+        },
+        {
+            "x": 6.13851552436941,
+            "y": 6.634116966515058,
+            "heading": -0.016131138042081584,
+            "angularVelocity": -1.819481831325098e-8,
+            "velocityX": 3.4271504000990243,
+            "velocityY": -1.601647269198795,
+            "timestamp": 1.3150292612654084
+        },
+        {
+            "x": 6.251225914876305,
+            "y": 6.574542092363786,
+            "heading": -0.016131138651474624,
+            "angularVelocity": -1.808268258780769e-8,
+            "velocityX": 3.344485551665478,
+            "velocityY": -1.7677811685785219,
+            "timestamp": 1.34872962826229
+        },
+        {
+            "x": 6.363935678841957,
+            "y": 6.51496603286697,
+            "heading": -0.016131139260695985,
+            "angularVelocity": -1.807758837477246e-8,
+            "velocityX": 3.344466960139662,
+            "velocityY": -1.7678163416537895,
+            "timestamp": 1.3824299952591717
+        },
+        {
+            "x": 6.475851865782255,
+            "y": 6.4539122462129415,
+            "heading": -0.016131138603390965,
+            "angularVelocity": 1.950438761664722e-8,
+            "velocityX": 3.320918936896271,
+            "velocityY": -1.8116653346735878,
+            "timestamp": 1.4161303622560533
+        },
+        {
+            "x": 6.587767727105175,
+            "y": 6.3928578626815735,
+            "heading": -0.016131137945887282,
+            "angularVelocity": 1.9510282594383425e-8,
+            "velocityX": 3.320909274764734,
+            "velocityY": -1.811683045974467,
+            "timestamp": 1.4498307292529349
+        },
+        {
+            "x": 6.699683536860704,
+            "y": 6.331803384624609,
+            "heading": -0.016131137288379963,
+            "angularVelocity": 1.951039048402864e-8,
+            "velocityX": 3.3209077445917723,
+            "velocityY": -1.811685850857758,
+            "timestamp": 1.4835310962498165
+        },
+        {
+            "x": 6.811599317469464,
+            "y": 6.270748853140253,
+            "heading": -0.0161311366308819,
+            "angularVelocity": 1.951011573787136e-8,
+            "velocityX": 3.320906879711909,
+            "velocityY": -1.811687436223046,
+            "timestamp": 1.517231463246698
+        },
+        {
+            "x": 6.923515076276675,
+            "y": 6.209694281692974,
+            "heading": -0.016131135973346068,
+            "angularVelocity": 1.951123652121546e-8,
+            "velocityX": 3.3209062327887042,
+            "velocityY": -1.8116886220535497,
+            "timestamp": 1.5509318302435797
+        },
+        {
+            "x": 7.031584320380035,
+            "y": 6.150738002364414,
+            "heading": -0.01382673748429001,
+            "angularVelocity": 0.06837903246775291,
+            "velocityX": 3.2067675735804393,
+            "velocityY": -1.7494254390171946,
+            "timestamp": 1.5846321972404613
+        },
+        {
+            "x": 7.1313405673144,
+            "y": 6.0963168023113505,
+            "heading": -0.011699564081675173,
+            "angularVelocity": 0.06312018509503639,
+            "velocityX": 2.9600937860289633,
+            "velocityY": -1.6148548191804175,
+            "timestamp": 1.618332564237343
+        },
+        {
+            "x": 7.222783801332874,
+            "y": 6.046430691545947,
+            "heading": -0.009749641962936324,
+            "angularVelocity": 0.05786056035883372,
+            "velocityX": 2.7134195312156404,
+            "velocityY": -1.480283902250094,
+            "timestamp": 1.6520329312342246
+        },
+        {
+            "x": 7.305914017550448,
+            "y": 6.00107967407256,
+            "heading": -0.007976979919226685,
+            "angularVelocity": 0.05260067476039131,
+            "velocityX": 2.4667451314481808,
+            "velocityY": -1.3457128664973763,
+            "timestamp": 1.6857332982311062
+        },
+        {
+            "x": 7.380731213778514,
+            "y": 5.960263752360125,
+            "heading": -0.006381581900532674,
+            "angularVelocity": 0.047340671952945675,
+            "velocityX": 2.2200706667375205,
+            "velocityY": -1.2111417574833037,
+            "timestamp": 1.7194336652279878
+        },
+        {
+            "x": 7.447235388895749,
+            "y": 5.923982928242947,
+            "heading": -0.004963449854323965,
+            "angularVelocity": 0.042080611357711836,
+            "velocityX": 1.973396168753552,
+            "velocityY": -1.076570594039371,
+            "timestamp": 1.7531340322248694
+        },
+        {
+            "x": 7.505426542307151,
+            "y": 5.892237203224628,
+            "heading": -0.00372258471684395,
+            "angularVelocity": 0.03682052298107744,
+            "velocityX": 1.72672165311393,
+            "velocityY": -0.9419993859786789,
+            "timestamp": 1.786834399221751
+        },
+        {
+            "x": 7.5553046737134695,
+            "y": 5.865026578610625,
+            "heading": -0.002658986847128457,
+            "angularVelocity": 0.03156042395067696,
+            "velocityX": 1.4800471285945758,
+            "velocityY": -0.8074281391808545,
+            "timestamp": 1.8205347662186326
+        },
+        {
+            "x": 7.596869782996782,
+            "y": 5.842351055576096,
+            "heading": -0.0017726562460022867,
+            "angularVelocity": 0.026300324895801946,
+            "velocityX": 1.233372600576068,
+            "velocityY": -0.6728568575121819,
+            "timestamp": 1.8542351332155143
+        },
+        {
+            "x": 7.630121870157635,
+            "y": 5.82421063520489,
+            "heading": -0.0010635926829587166,
+            "angularVelocity": 0.021040232680814546,
+            "velocityX": 0.9866980725738072,
+            "velocityY": -0.5382855436821761,
+            "timestamp": 1.8879355002123959
+        },
+        {
+            "x": 7.655060935277959,
+            "y": 5.8106053185140585,
+            "heading": -0.0005317957806907868,
+            "angularVelocity": 0.015780151661757422,
+            "velocityX": 0.7400235470026807,
+            "velocityY": -0.40371419967297517,
+            "timestamp": 1.9216358672092775
+        },
+        {
+            "x": 7.671686978497787,
+            "y": 5.801535106470048,
+            "heading": -0.00017726506577651584,
+            "angularVelocity": 0.010520084690677335,
+            "velocityX": 0.4933490255867586,
+            "velocityY": -0.2691428269861255,
+            "timestamp": 1.955336234206159
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 0.0050725214219634815,
-            "velocityX": 0.24669198956838576,
-            "velocityY": -0.1345343263862727,
-            "timestamp": 1.9890357333479398
+            "heading": 3.986792057163996e-31,
+            "angularVelocity": 0.005260033690221658,
+            "velocityX": 0.24667450959752207,
+            "velocityY": -0.13457142678797143,
+            "timestamp": 1.9890366012030407
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 1.6384700465306213e-30,
-            "velocityX": 7.518277191788514e-33,
-            "velocityY": 3.939051930235097e-33,
-            "timestamp": 2.0227357776546437
+            "heading": 0,
+            "angularVelocity": -1.0163368054249497e-29,
+            "velocityX": -1.0830525743862323e-33,
+            "velocityY": 5.9205016331337416e-33,
+            "timestamp": 2.022736968199922
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.traj
index ae5af9d6..e1aa1d32 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.traj	
@@ -3,524 +3,524 @@
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": 4.173822195239149e-34,
+            "velocityY": -1.3324658216077414e-32,
             "timestamp": 0
         },
         {
-            "x": 2.260404640394534,
-            "y": 5.564834920701341,
-            "heading": 3.1398005529486164,
-            "angularVelocity": -0.005376730415880204,
-            "velocityX": 0.2804902157612115,
-            "velocityY": 0.1303406844718941,
-            "timestamp": 0.037094486045788126
-        },
-        {
-            "x": 2.2812139214883365,
-            "y": 5.574504760754136,
-            "heading": 3.139401659951885,
-            "angularVelocity": -0.010753431015026859,
-            "velocityX": 0.5609804397374861,
-            "velocityY": 0.26068133255328624,
-            "timestamp": 0.07418897209157625
-        },
-        {
-            "x": 2.3124278435365335,
-            "y": 5.58900951877657,
-            "heading": 3.1388033223810634,
-            "angularVelocity": -0.016130094647582943,
-            "velocityX": 0.8414706705915215,
-            "velocityY": 0.3910219433834154,
-            "timestamp": 0.11128345813736437
-        },
-        {
-            "x": 2.3540464067273468,
-            "y": 5.608349193345913,
-            "heading": 3.1380055419662973,
-            "angularVelocity": -0.02150671163853717,
-            "velocityX": 1.1219609065196614,
-            "velocityY": 0.5213625158593963,
-            "timestamp": 0.1483779441831525
-        },
-        {
-            "x": 2.4060696111557784,
-            "y": 5.632523782985854,
-            "heading": 3.1370083209408497,
-            "angularVelocity": -0.026883268424770904,
-            "velocityX": 1.4024511450088724,
-            "velocityY": 0.651703048536671,
-            "timestamp": 0.18547243022894064
-        },
-        {
-            "x": 2.4684974567819364,
-            "y": 5.661533286147284,
-            "heading": 3.1358116622714935,
-            "angularVelocity": -0.032259745232204366,
-            "velocityX": 1.6829413824226938,
-            "velocityY": 0.7820435394527967,
-            "timestamp": 0.22256691627472877
-        },
-        {
-            "x": 2.541329943361364,
-            "y": 5.695377701176762,
-            "heading": 3.1344155700455096,
-            "angularVelocity": -0.03763611185395848,
-            "velocityX": 1.9634316132463963,
-            "velocityY": 0.9123839857951219,
-            "timestamp": 0.2596614023205169
-        },
-        {
-            "x": 2.6245670703196087,
-            "y": 5.734057026259781,
-            "heading": 3.1328200501674663,
-            "angularVelocity": -0.04301231929927239,
-            "velocityX": 2.243921828583893,
-            "velocityY": 1.0427243832216457,
-            "timestamp": 0.296755888366305
-        },
-        {
-            "x": 2.7182088365016934,
-            "y": 5.777571259306566,
-            "heading": 3.1310251117440115,
-            "angularVelocity": -0.04838828124588956,
-            "velocityX": 2.5244120127853087,
-            "velocityY": 1.17306472431166,
-            "timestamp": 0.33385037441209314
-        },
-        {
-            "x": 2.822255239588457,
-            "y": 5.825920397683519,
-            "heading": 3.1290307702747278,
-            "angularVelocity": -0.053763825351902786,
-            "velocityX": 2.804902134466352,
-            "velocityY": 1.3034049944046322,
-            "timestamp": 0.3709448604578813
-        },
-        {
-            "x": 2.9367062743473644,
-            "y": 5.879104437402994,
-            "heading": 3.126837057015217,
-            "angularVelocity": -0.05913852686363322,
-            "velocityX": 3.0853921150877586,
-            "velocityY": 1.4337451569978974,
-            "timestamp": 0.4080393465036694
-        },
-        {
-            "x": 3.0615619238881426,
-            "y": 5.937123369060452,
-            "heading": 3.1244440632726094,
-            "angularVelocity": -0.06451076690090568,
-            "velocityX": 3.3658816403780625,
-            "velocityY": 1.5640850660618872,
-            "timestamp": 0.44513383254945754
-        },
-        {
-            "x": 3.188819545942614,
-            "y": 5.996258327229638,
-            "heading": 3.124444062637546,
-            "angularVelocity": -1.7120162809754954e-8,
-            "velocityX": 3.430634458646746,
-            "velocityY": 1.594171114709378,
-            "timestamp": 0.4822283185952457
-        },
-        {
-            "x": 3.3160771824859174,
-            "y": 6.055393254219169,
-            "heading": 3.1244440620025067,
-            "angularVelocity": -1.711950789942117e-8,
-            "velocityX": 3.4306348492393433,
-            "velocityY": 1.5941702741624886,
-            "timestamp": 0.5193228046410338
-        },
-        {
-            "x": 3.4433348335743874,
-            "y": 6.114528149907673,
-            "heading": 3.1244440613674698,
-            "angularVelocity": -1.7119436749370044e-8,
-            "velocityX": 3.4306352413506347,
-            "velocityY": 1.5941694303436482,
-            "timestamp": 0.5564172906868219
-        },
-        {
-            "x": 3.570592499306144,
-            "y": 6.173663014083979,
-            "heading": 3.124444060732438,
-            "angularVelocity": -1.7119304165961875e-8,
-            "velocityX": 3.4306356361070582,
-            "velocityY": 1.5941685808319566,
-            "timestamp": 0.59351177673261
-        },
-        {
-            "x": 3.6978501798346053,
-            "y": 6.232797846417909,
-            "heading": 3.1244440600974133,
-            "angularVelocity": -1.7119116923870704e-8,
-            "velocityX": 3.430636034999362,
-            "velocityY": 1.5941677224193291,
-            "timestamp": 0.6306062627783982
-        },
-        {
-            "x": 3.825107875386636,
-            "y": 6.291932646421232,
-            "heading": 3.1244440594623972,
-            "angularVelocity": -1.7118879018616895e-8,
-            "velocityX": 3.430636440007508,
-            "velocityY": 1.594166850844881,
-            "timestamp": 0.6677007488241863
-        },
-        {
-            "x": 3.9523655862834066,
-            "y": 6.351067413402769,
-            "heading": 3.1244440588273914,
-            "angularVelocity": -1.711859691944282e-8,
-            "velocityX": 3.4306368536738447,
-            "velocityY": 1.5941659606374654,
-            "timestamp": 0.7047952348699744
-        },
-        {
-            "x": 4.079623312940078,
-            "y": 6.410202146469073,
-            "heading": 3.1244440581923967,
-            "angularVelocity": -1.711830453291075e-8,
-            "velocityX": 3.4306372785321515,
-            "velocityY": 1.5941650463443597,
-            "timestamp": 0.7418897209157626
-        },
-        {
-            "x": 4.206881055880688,
-            "y": 6.469336844492392,
-            "heading": 3.124444057557412,
-            "angularVelocity": -1.711804273623487e-8,
-            "velocityX": 3.430637717517568,
-            "velocityY": 1.5941641016490944,
-            "timestamp": 0.7789842069615507
-        },
-        {
-            "x": 4.334138815794606,
-            "y": 6.528471505989175,
-            "heading": 3.1244440569224348,
-            "angularVelocity": -1.7117826690263867e-8,
-            "velocityX": 3.4306381750871577,
-            "velocityY": 1.59416311696,
-            "timestamp": 0.8160786930073388
-        },
-        {
-            "x": 4.461396593547393,
-            "y": 6.587606129096708,
-            "heading": 3.1244440562874636,
-            "angularVelocity": -1.7117677202487012e-8,
-            "velocityX": 3.430638655990672,
-            "velocityY": 1.5941620820555098,
-            "timestamp": 0.853173179053127
-        },
-        {
-            "x": 4.5886544601407655,
-            "y": 6.646740561018822,
-            "heading": 3.124444055652494,
-            "angularVelocity": -1.711762346285072e-8,
-            "velocityX": 3.4306410509715453,
-            "velocityY": 1.5941569280437173,
-            "timestamp": 0.8902676650989151
-        },
-        {
-            "x": 4.7169368908362825,
-            "y": 6.703618180644166,
-            "heading": 3.1244440550164043,
-            "angularVelocity": -1.7147829637187305e-8,
-            "velocityX": 3.458261439103553,
-            "velocityY": 1.5333173656899308,
-            "timestamp": 0.9273621511447032
-        },
-        {
-            "x": 4.8494363294608345,
-            "y": 6.749827966380956,
-            "heading": 3.124444054367944,
-            "angularVelocity": -1.7481320274300667e-8,
-            "velocityX": 3.5719443170340583,
-            "velocityY": 1.245731931148746,
-            "timestamp": 0.9644566371904914
-        },
-        {
-            "x": 4.985267831359121,
-            "y": 6.785059321241209,
-            "heading": 3.124444053688418,
-            "angularVelocity": -1.8318771799358465e-8,
-            "velocityX": 3.6617706936448973,
-            "velocityY": 0.9497733656900947,
-            "timestamp": 1.0015511232362795
-        },
-        {
-            "x": 5.12352345692124,
-            "y": 6.809076540845587,
-            "heading": 3.1244440529552464,
-            "angularVelocity": -1.9764980034334986e-8,
-            "velocityX": 3.7271206667066523,
-            "velocityY": 0.6474606380779142,
-            "timestamp": 1.0386456092820675
-        },
-        {
-            "x": 5.26327900081687,
-            "y": 6.821719014416903,
-            "heading": 3.124444052138408,
-            "angularVelocity": -2.2020481247534803e-8,
-            "velocityX": 3.767555741926746,
-            "velocityY": 0.3408181354961927,
-            "timestamp": 1.0757400953278555
+            "x": 2.2604047390881647,
+            "y": 5.564834315872384,
+            "heading": -0.00020682184904158323,
+            "angularVelocity": -0.005575579259585174,
+            "velocityX": 0.2804947723375417,
+            "velocityY": 0.13032526030129518,
+            "timestamp": 0.037094235309470626
+        },
+        {
+            "x": 2.2812142175691745,
+            "y": 5.574502946267583,
+            "heading": -0.0006204643713099006,
+            "angularVelocity": -0.011151126821122416,
+            "velocityX": 0.5609895528887481,
+            "velocityY": 0.2606504842204101,
+            "timestamp": 0.07418847061894125
+        },
+        {
+            "x": 2.3124284356981284,
+            "y": 5.589005889804122,
+            "heading": -0.0012409261115257607,
+            "angularVelocity": -0.01672663515070949,
+            "velocityX": 0.8414843403169981,
+            "velocityY": 0.39097567089719987,
+            "timestamp": 0.11128270592841188
+        },
+        {
+            "x": 2.3540473936632536,
+            "y": 5.608343145059642,
+            "heading": -0.0020682052367836136,
+            "angularVelocity": -0.022302094068156527,
+            "velocityX": 1.121979132819583,
+            "velocityY": 0.5213008192295232,
+            "timestamp": 0.1483769412378825
+        },
+        {
+            "x": 2.4060710915596104,
+            "y": 5.632514710558234,
+            "heading": -0.0031022993856005916,
+            "angularVelocity": -0.027877489323882353,
+            "velocityX": 1.402473927884819,
+            "velocityY": 0.6516259277737647,
+            "timestamp": 0.18547117654735312
+        },
+        {
+            "x": 2.46849952934744,
+            "y": 5.661520584751242,
+            "heading": -0.004343205426988983,
+            "angularVelocity": -0.03345280017329315,
+            "velocityX": 1.6829687218782206,
+            "velocityY": 0.781950994568762,
+            "timestamp": 0.22256541185682374
+        },
+        {
+            "x": 2.5413327067825313,
+            "y": 5.695360765985745,
+            "heading": -0.005790919055957113,
+            "angularVelocity": -0.039027994967159495,
+            "velocityX": 1.9634635092880914,
+            "velocityY": 0.9122760168036823,
+            "timestamp": 0.25965964716629436
+        },
+        {
+            "x": 2.6245706232908623,
+            "y": 5.734035252447861,
+            "heading": -0.007445434065373688,
+            "angularVelocity": -0.044603022426903935,
+            "velocityX": 2.2439582812232635,
+            "velocityY": 1.042600990139337,
+            "timestamp": 0.296753882475765
+        },
+        {
+            "x": 2.7182132777182155,
+            "y": 5.777544042048614,
+            "heading": -0.009306740898831883,
+            "angularVelocity": -0.05017779226152841,
+            "velocityX": 2.5244530220426173,
+            "velocityY": 1.1729259071596103,
+            "timestamp": 0.33384811778523565
+        },
+        {
+            "x": 2.8222606677468502,
+            "y": 5.825887132155531,
+            "heading": -0.011374823315541767,
+            "angularVelocity": -0.05575212427098156,
+            "velocityX": 2.804947700379475,
+            "velocityY": 1.3032507532126736,
+            "timestamp": 0.3709423530947063
+        },
+        {
+            "x": 2.9367127881472834,
+            "y": 5.879064518782887,
+            "heading": -0.013649648601341033,
+            "angularVelocity": -0.0613255743600492,
+            "velocityX": 3.0854422377380093,
+            "velocityY": 1.4335754918171768,
+            "timestamp": 0.40803658840417695
+        },
+        {
+            "x": 3.061569622038521,
+            "y": 5.9370761925311655,
+            "heading": -0.016131121234305167,
+            "angularVelocity": -0.06689644933404303,
+            "velocityX": 3.365936320012513,
+            "velocityY": 1.563899977025988,
+            "timestamp": 0.4451308237136476
+        },
+        {
+            "x": 3.188829425192231,
+            "y": 5.996204205741329,
+            "heading": -0.01613112189285862,
+            "angularVelocity": -1.7753525563113908e-8,
+            "velocityX": 3.430716446693241,
+            "velocityY": 1.5939946656635118,
+            "timestamp": 0.48222505902311824
+        },
+        {
+            "x": 3.3160892428339666,
+            "y": 6.055332187769375,
+            "heading": -0.01613112255138683,
+            "angularVelocity": -1.775284497104555e-8,
+            "velocityX": 3.4307168372667594,
+            "velocityY": 1.5939938250445522,
+            "timestamp": 0.5193192943325888
+        },
+        {
+            "x": 3.443349075020492,
+            "y": 6.114460138492994,
+            "heading": -0.016131123209912376,
+            "angularVelocity": -1.775277322447583e-8,
+            "velocityX": 3.43071722937053,
+            "velocityY": 1.593992981128321,
+            "timestamp": 0.5564135296420595
+        },
+        {
+            "x": 3.5706089218502877,
+            "y": 6.173588057700207,
+            "heading": -0.01613112386843302,
+            "angularVelocity": -1.775264097704561e-8,
+            "velocityX": 3.430717624129179,
+            "velocityY": 1.5939921314975005,
+            "timestamp": 0.5935077649515301
+        },
+        {
+            "x": 3.697868783477107,
+            "y": 6.2327159450600735,
+            "heading": -0.016131124526946636,
+            "angularVelocity": -1.7752451586110053e-8,
+            "velocityX": 3.430718023032762,
+            "velocityY": 1.5939912729450545,
+            "timestamp": 0.6306020002610008
+        },
+        {
+            "x": 3.8251286601281493,
+            "y": 6.291843800083575,
+            "heading": -0.016131125185451407,
+            "angularVelocity": -1.7752213148650806e-8,
+            "velocityX": 3.4307184280612866,
+            "velocityY": 1.5939904012094348,
+            "timestamp": 0.6676962355704714
+        },
+        {
+            "x": 3.95238855212494,
+            "y": 6.350971622078674,
+            "heading": -0.016131125843945708,
+            "angularVelocity": -1.775193091205886e-8,
+            "velocityX": 3.430718841757593,
+            "velocityY": 1.5939895108176958,
+            "timestamp": 0.7047904708799421
+        },
+        {
+            "x": 4.079648459881884,
+            "y": 6.410099410153433,
+            "heading": -0.01613112650242917,
+            "angularVelocity": -1.7751638714463086e-8,
+            "velocityX": 3.430719266625584,
+            "velocityY": 1.5939885963806835,
+            "timestamp": 0.7418847061894127
+        },
+        {
+            "x": 4.2069083839232695,
+            "y": 6.469227163179404,
+            "heading": -0.01613112716090301,
+            "angularVelocity": -1.7751379248608663e-8,
+            "velocityX": 3.430719705627546,
+            "velocityY": 1.5939876515226545,
+            "timestamp": 0.7789789414988834
+        },
+        {
+            "x": 4.334168324938812,
+            "y": 6.528354879672101,
+            "heading": -0.016131127819368877,
+            "angularVelocity": -1.7751164242273923e-8,
+            "velocityX": 3.430720163223074,
+            "velocityY": 1.5939866666451934,
+            "timestamp": 0.816073176808354
+        },
+        {
+            "x": 4.461428283794271,
+            "y": 6.58748255776812,
+            "heading": -0.01613112847782922,
+            "angularVelocity": -1.775101542628313e-8,
+            "velocityX": 3.430720644158123,
+            "velocityY": 1.5939856315335348,
+            "timestamp": 0.8531674121178247
+        },
+        {
+            "x": 4.58868833054201,
+            "y": 6.646610046694028,
+            "heading": -0.016131129136287645,
+            "angularVelocity": -1.7750963701844846e-8,
+            "velocityX": 3.430723013590394,
+            "velocityY": 1.5939805318162525,
+            "timestamp": 0.8902616474272953
+        },
+        {
+            "x": 4.716960790002456,
+            "y": 6.703507810971908,
+            "heading": -0.016131129795891656,
+            "angularVelocity": -1.7781846861521853e-8,
+            "velocityX": 3.4580160068077612,
+            "velocityY": 1.5338707969903813,
+            "timestamp": 0.927355882736766
+        },
+        {
+            "x": 4.849451901724698,
+            "y": 6.749738586923234,
+            "heading": -0.016131130468276335,
+            "angularVelocity": -1.8126392792981438e-8,
+            "velocityX": 3.5717439816966663,
+            "velocityY": 1.2463062135027452,
+            "timestamp": 0.9644501180462366
+        },
+        {
+            "x": 4.985276799458285,
+            "y": 6.784991618369029,
+            "heading": -0.016131131172819703,
+            "angularVelocity": -1.899333848953808e-8,
+            "velocityX": 3.661617407676018,
+            "velocityY": 0.950364150970728,
+            "timestamp": 1.0015443533557071
+        },
+        {
+            "x": 5.12352760039525,
+            "y": 6.80903105836077,
+            "heading": -0.01613113193292308,
+            "angularVelocity": -2.0491145599848975e-8,
+            "velocityX": 3.7270157959468126,
+            "velocityY": 0.6480640399022476,
+            "timestamp": 1.0386385886651777
+        },
+        {
+            "x": 5.263280144021708,
+            "y": 6.821696149590996,
+            "heading": -0.01613113277968929,
+            "angularVelocity": -2.2827434072857776e-8,
+            "velocityX": 3.767500326143083,
+            "velocityY": 0.341430174380559,
+            "timestamp": 1.0757328239746482
         },
         {
             "x": 5.403600215911865,
             "y": 6.822902202606201,
-            "heading": 3.124444051205612,
-            "angularVelocity": -2.5146473765828148e-8,
-            "velocityX": 3.782805210504538,
-            "velocityY": 0.03189660554501852,
-            "timestamp": 1.1128345813736435
-        },
-        {
-            "x": 5.530809022252107,
-            "y": 6.8145114887459926,
-            "heading": 3.1244440503418645,
-            "angularVelocity": -2.5630462052464063e-8,
-            "velocityX": 3.774737065106383,
-            "velocityY": -0.24898227978112425,
-            "timestamp": 1.1465346256803473
-        },
-        {
-            "x": 5.657044071820609,
-            "y": 6.79670145003808,
-            "heading": 3.1244440495837504,
-            "angularVelocity": -2.2495928655681543e-8,
-            "velocityX": 3.745842243399973,
-            "velocityY": -0.5284871006643926,
-            "timestamp": 1.180234669987051
-        },
-        {
-            "x": 5.781608875607717,
-            "y": 6.769570371843818,
-            "heading": 3.124444048897901,
-            "angularVelocity": -2.0351591481890114e-8,
-            "velocityX": 3.6962801192023655,
-            "velocityY": -0.8050754458167036,
-            "timestamp": 1.2139347142937547
-        },
-        {
-            "x": 5.903816171014302,
-            "y": 6.733267988087913,
-            "heading": 3.1244440482605893,
-            "angularVelocity": -1.891130244410514e-8,
-            "velocityX": 3.6263244728813384,
-            "velocityY": -1.0772206536441533,
-            "timestamp": 1.2476347586004584
-        },
-        {
-            "x": 6.022991737743217,
-            "y": 6.687994699591429,
-            "heading": 3.124444047653726,
-            "angularVelocity": -1.8007800496265653e-8,
-            "velocityX": 3.5363623158563073,
-            "velocityY": -1.3434192573888535,
-            "timestamp": 1.281334802907162
-        },
-        {
-            "x": 6.1384782869774295,
-            "y": 6.6340007793146,
-            "heading": 3.1244440470624193,
-            "angularVelocity": -1.754616693571922e-8,
-            "velocityX": 3.4268960652742955,
-            "velocityY": -1.6021913735611486,
-            "timestamp": 1.3150348472138658
-        },
-        {
-            "x": 6.251164558797792,
-            "y": 6.57438290738676,
-            "heading": 3.1244440464747822,
-            "angularVelocity": -1.7437275134789687e-8,
-            "velocityX": 3.343801889244,
-            "velocityY": -1.769073992462997,
-            "timestamp": 1.3487348915205695
-        },
-        {
-            "x": 6.363850200907552,
-            "y": 6.514763845231331,
-            "heading": 3.124444045887316,
-            "angularVelocity": -1.7432210000565644e-8,
-            "velocityX": 3.343783203493965,
-            "velocityY": -1.769109310742662,
-            "timestamp": 1.3824349358272732
-        },
-        {
-            "x": 6.475774212207488,
-            "y": 6.453726953025987,
-            "heading": 3.124444046521184,
-            "angularVelocity": 1.880910439494107e-8,
-            "velocityX": 3.3211829124412344,
-            "velocityY": -1.8111813637349317,
-            "timestamp": 1.4161349801339769
-        },
-        {
-            "x": 6.587697896511305,
-            "y": 6.392689461208172,
-            "heading": 3.1244440471552553,
-            "angularVelocity": 1.881516364731064e-8,
-            "velocityX": 3.3211732093049866,
-            "velocityY": -1.8111991563664802,
-            "timestamp": 1.4498350244406806
-        },
-        {
-            "x": 6.699621529072565,
-            "y": 6.331651874510795,
-            "heading": 3.124444047789331,
-            "angularVelocity": 1.881528213317004e-8,
-            "velocityX": 3.321171673919603,
-            "velocityY": -1.811201971780052,
-            "timestamp": 1.4835350687473843
-        },
-        {
-            "x": 6.811545132450604,
-            "y": 6.2706142343006634,
-            "heading": 3.124444048423398,
-            "angularVelocity": 1.881501023244474e-8,
-            "velocityX": 3.3211708079497897,
-            "velocityY": -1.8112035596935343,
-            "timestamp": 1.517235113054088
-        },
-        {
-            "x": 6.923468714015748,
-            "y": 6.209576554092949,
-            "heading": 3.1244440490575007,
-            "angularVelocity": 1.8816081178773256e-8,
-            "velocityX": 3.3211701606836845,
-            "velocityY": -1.8112047465638819,
-            "timestamp": 1.5509351573607917
-        },
-        {
-            "x": 7.031544581287166,
-            "y": 6.150637092992946,
-            "heading": 3.126666281190466,
-            "angularVelocity": 0.06594151962352889,
-            "velocityX": 3.2069948124643886,
-            "velocityY": -1.7489431338307682,
-            "timestamp": 1.5846352016674954
-        },
-        {
-            "x": 7.131306941921243,
-            "y": 6.096231417458986,
-            "heading": 3.1287176070894986,
-            "angularVelocity": 0.060870124690714414,
-            "velocityX": 2.960303545186476,
-            "velocityY": -1.6144096143855096,
-            "timestamp": 1.618335245974199
-        },
-        {
-            "x": 7.2227557801672635,
-            "y": 6.046359537502576,
-            "heading": 3.1305980016068635,
-            "angularVelocity": 0.05579798353529268,
-            "velocityX": 2.7136118105289606,
-            "velocityY": -1.4798757978632517,
-            "timestamp": 1.6520352902809028
-        },
-        {
-            "x": 7.305891091138876,
-            "y": 6.001021457128046,
-            "heading": 3.132307456329253,
-            "angularVelocity": 0.05072559272714715,
-            "velocityX": 2.466919930876006,
-            "velocityY": -1.3453418625182645,
-            "timestamp": 1.6857353345876065
-        },
-        {
-            "x": 7.380712872646774,
-            "y": 5.9602171788044025,
-            "heading": 3.1338459674980714,
-            "angularVelocity": 0.04565309038812635,
-            "velocityX": 2.2202279862586183,
-            "velocityY": -1.2108078539091887,
-            "timestamp": 1.7194353788943102
-        },
-        {
-            "x": 7.447221123569205,
-            "y": 5.923946704366036,
-            "heading": 3.135213533279876,
-            "angularVelocity": 0.04058053364434269,
-            "velocityX": 1.9735360083547677,
-            "velocityY": -1.0762737908671516,
-            "timestamp": 1.753135423201014
-        },
-        {
-            "x": 7.505415843310863,
-            "y": 5.892210035316634,
-            "heading": 3.136410152813217,
-            "angularVelocity": 0.035507951338280094,
-            "velocityX": 1.7268440127861169,
-            "velocityY": -0.941739683205353,
-            "timestamp": 1.7868354675077176
-        },
-        {
-            "x": 7.555297031572268,
-            "y": 5.865007172961733,
-            "heading": 3.1374358257913886,
-            "angularVelocity": 0.030435359931185808,
-            "velocityX": 1.4801520083308155,
-            "velocityY": -0.807205536803692,
-            "timestamp": 1.8205355118144213
-        },
-        {
-            "x": 7.596864688235311,
-            "y": 5.8423381184765635,
-            "heading": 3.138290552251957,
-            "angularVelocity": 0.025362769638798365,
-            "velocityX": 1.2334600003707947,
-            "velocityY": -0.6726713555287516,
-            "timestamp": 1.854235556121125
-        },
-        {
-            "x": 7.6301188133003475,
-            "y": 5.824202872944982,
-            "heading": 3.1389743324564656,
-            "angularVelocity": 0.020290187107322288,
-            "velocityX": 0.9867679924213419,
-            "velocityY": -0.5381371420919633,
-            "timestamp": 1.8879356004278287
-        },
-        {
-            "x": 7.655059406849112,
-            "y": 5.8106014373839985,
-            "heading": 3.1394871668082627,
-            "angularVelocity": 0.015217616544653215,
-            "velocityX": 0.7400759868972333,
-            "velocityY": -0.4036028984768356,
-            "timestamp": 1.9216356447345324
-        },
-        {
-            "x": 7.671686469021436,
-            "y": 5.801533812759989,
-            "heading": 3.1398290558033333,
-            "angularVelocity": 0.010145060699586378,
-            "velocityX": 0.4933839855224485,
-            "velocityY": -0.2690686261859067,
-            "timestamp": 1.9553356890412361
+            "heading": -0.016131133746573465,
+            "angularVelocity": -2.6065618224331403e-8,
+            "velocityX": 3.782799961220162,
+            "velocityY": 0.03251321951088522,
+            "timestamp": 1.1128270592841187
+        },
+        {
+            "x": 5.530811600424101,
+            "y": 6.814532053807006,
+            "heading": -0.016131134641902804,
+            "angularVelocity": -2.6567346760379892e-8,
+            "velocityX": 3.7747774237594087,
+            "velocityY": -0.24836966315439601,
+            "timestamp": 1.1465274262810004
+        },
+        {
+            "x": 5.6570507349000225,
+            "y": 6.796742242248266,
+            "heading": -0.016131135427811184,
+            "angularVelocity": -2.332046941486525e-8,
+            "velocityX": 3.7459275884919543,
+            "velocityY": -0.5278818346513846,
+            "timestamp": 1.180227793277882
+        },
+        {
+            "x": 5.781621094465109,
+            "y": 6.769630943607204,
+            "heading": -0.01613113613886582,
+            "angularVelocity": -2.1099314246183972e-8,
+            "velocityX": 3.696409584400467,
+            "velocityY": -0.8044808130300218,
+            "timestamp": 1.2139281602747636
+        },
+        {
+            "x": 5.903835372739644,
+            "y": 6.733347785640362,
+            "heading": -0.016131136799649604,
+            "angularVelocity": -1.9607613962388268e-8,
+            "velocityX": 3.6264969543460657,
+            "velocityY": -1.0766398469844052,
+            "timestamp": 1.2476285272716452
+        },
+        {
+            "x": 6.023019298132563,
+            "y": 6.68809306728661,
+            "heading": -0.01613113742890953,
+            "angularVelocity": -1.8672198003746715e-8,
+            "velocityX": 3.536576483097275,
+            "velocityY": -1.3428553569739705,
+            "timestamp": 1.2813288942685268
+        },
+        {
+            "x": 6.13851552436941,
+            "y": 6.634116966515058,
+            "heading": -0.016131138042081584,
+            "angularVelocity": -1.819481831325098e-8,
+            "velocityX": 3.4271504000990243,
+            "velocityY": -1.601647269198795,
+            "timestamp": 1.3150292612654084
+        },
+        {
+            "x": 6.251225914876305,
+            "y": 6.574542092363786,
+            "heading": -0.016131138651474624,
+            "angularVelocity": -1.808268258780769e-8,
+            "velocityX": 3.344485551665478,
+            "velocityY": -1.7677811685785219,
+            "timestamp": 1.34872962826229
+        },
+        {
+            "x": 6.363935678841957,
+            "y": 6.51496603286697,
+            "heading": -0.016131139260695985,
+            "angularVelocity": -1.807758837477246e-8,
+            "velocityX": 3.344466960139662,
+            "velocityY": -1.7678163416537895,
+            "timestamp": 1.3824299952591717
+        },
+        {
+            "x": 6.475851865782255,
+            "y": 6.4539122462129415,
+            "heading": -0.016131138603390965,
+            "angularVelocity": 1.950438761664722e-8,
+            "velocityX": 3.320918936896271,
+            "velocityY": -1.8116653346735878,
+            "timestamp": 1.4161303622560533
+        },
+        {
+            "x": 6.587767727105175,
+            "y": 6.3928578626815735,
+            "heading": -0.016131137945887282,
+            "angularVelocity": 1.9510282594383425e-8,
+            "velocityX": 3.320909274764734,
+            "velocityY": -1.811683045974467,
+            "timestamp": 1.4498307292529349
+        },
+        {
+            "x": 6.699683536860704,
+            "y": 6.331803384624609,
+            "heading": -0.016131137288379963,
+            "angularVelocity": 1.951039048402864e-8,
+            "velocityX": 3.3209077445917723,
+            "velocityY": -1.811685850857758,
+            "timestamp": 1.4835310962498165
+        },
+        {
+            "x": 6.811599317469464,
+            "y": 6.270748853140253,
+            "heading": -0.0161311366308819,
+            "angularVelocity": 1.951011573787136e-8,
+            "velocityX": 3.320906879711909,
+            "velocityY": -1.811687436223046,
+            "timestamp": 1.517231463246698
+        },
+        {
+            "x": 6.923515076276675,
+            "y": 6.209694281692974,
+            "heading": -0.016131135973346068,
+            "angularVelocity": 1.951123652121546e-8,
+            "velocityX": 3.3209062327887042,
+            "velocityY": -1.8116886220535497,
+            "timestamp": 1.5509318302435797
+        },
+        {
+            "x": 7.031584320380035,
+            "y": 6.150738002364414,
+            "heading": -0.01382673748429001,
+            "angularVelocity": 0.06837903246775291,
+            "velocityX": 3.2067675735804393,
+            "velocityY": -1.7494254390171946,
+            "timestamp": 1.5846321972404613
+        },
+        {
+            "x": 7.1313405673144,
+            "y": 6.0963168023113505,
+            "heading": -0.011699564081675173,
+            "angularVelocity": 0.06312018509503639,
+            "velocityX": 2.9600937860289633,
+            "velocityY": -1.6148548191804175,
+            "timestamp": 1.618332564237343
+        },
+        {
+            "x": 7.222783801332874,
+            "y": 6.046430691545947,
+            "heading": -0.009749641962936324,
+            "angularVelocity": 0.05786056035883372,
+            "velocityX": 2.7134195312156404,
+            "velocityY": -1.480283902250094,
+            "timestamp": 1.6520329312342246
+        },
+        {
+            "x": 7.305914017550448,
+            "y": 6.00107967407256,
+            "heading": -0.007976979919226685,
+            "angularVelocity": 0.05260067476039131,
+            "velocityX": 2.4667451314481808,
+            "velocityY": -1.3457128664973763,
+            "timestamp": 1.6857332982311062
+        },
+        {
+            "x": 7.380731213778514,
+            "y": 5.960263752360125,
+            "heading": -0.006381581900532674,
+            "angularVelocity": 0.047340671952945675,
+            "velocityX": 2.2200706667375205,
+            "velocityY": -1.2111417574833037,
+            "timestamp": 1.7194336652279878
+        },
+        {
+            "x": 7.447235388895749,
+            "y": 5.923982928242947,
+            "heading": -0.004963449854323965,
+            "angularVelocity": 0.042080611357711836,
+            "velocityX": 1.973396168753552,
+            "velocityY": -1.076570594039371,
+            "timestamp": 1.7531340322248694
+        },
+        {
+            "x": 7.505426542307151,
+            "y": 5.892237203224628,
+            "heading": -0.00372258471684395,
+            "angularVelocity": 0.03682052298107744,
+            "velocityX": 1.72672165311393,
+            "velocityY": -0.9419993859786789,
+            "timestamp": 1.786834399221751
+        },
+        {
+            "x": 7.5553046737134695,
+            "y": 5.865026578610625,
+            "heading": -0.002658986847128457,
+            "angularVelocity": 0.03156042395067696,
+            "velocityX": 1.4800471285945758,
+            "velocityY": -0.8074281391808545,
+            "timestamp": 1.8205347662186326
+        },
+        {
+            "x": 7.596869782996782,
+            "y": 5.842351055576096,
+            "heading": -0.0017726562460022867,
+            "angularVelocity": 0.026300324895801946,
+            "velocityX": 1.233372600576068,
+            "velocityY": -0.6728568575121819,
+            "timestamp": 1.8542351332155143
+        },
+        {
+            "x": 7.630121870157635,
+            "y": 5.82421063520489,
+            "heading": -0.0010635926829587166,
+            "angularVelocity": 0.021040232680814546,
+            "velocityX": 0.9866980725738072,
+            "velocityY": -0.5382855436821761,
+            "timestamp": 1.8879355002123959
+        },
+        {
+            "x": 7.655060935277959,
+            "y": 5.8106053185140585,
+            "heading": -0.0005317957806907868,
+            "angularVelocity": 0.015780151661757422,
+            "velocityX": 0.7400235470026807,
+            "velocityY": -0.40371419967297517,
+            "timestamp": 1.9216358672092775
+        },
+        {
+            "x": 7.671686978497787,
+            "y": 5.801535106470048,
+            "heading": -0.00017726506577651584,
+            "angularVelocity": 0.010520084690677335,
+            "velocityX": 0.4933490255867586,
+            "velocityY": -0.2691428269861255,
+            "timestamp": 1.955336234206159
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 0.0050725214219634815,
-            "velocityX": 0.24669198956838576,
-            "velocityY": -0.1345343263862727,
-            "timestamp": 1.9890357333479398
+            "heading": 3.986792057163996e-31,
+            "angularVelocity": 0.005260033690221658,
+            "velocityX": 0.24667450959752207,
+            "velocityY": -0.13457142678797143,
+            "timestamp": 1.9890366012030407
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 1.6384700465306213e-30,
-            "velocityX": 7.518277191788514e-33,
-            "velocityY": 3.939051930235097e-33,
-            "timestamp": 2.0227357776546437
+            "heading": 0,
+            "angularVelocity": -1.0163368054249497e-29,
+            "velocityX": -1.0830525743862323e-33,
+            "velocityY": 5.9205016331337416e-33,
+            "timestamp": 2.022736968199922
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.1.traj
index 14bb5c7a..72a99c84 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.1.traj	
@@ -3,569 +3,569 @@
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 1.2492921998824363e-31,
-            "velocityX": 1.8348820495773462e-32,
-            "velocityY": 2.100679182506497e-31,
+            "heading": 0,
+            "angularVelocity": 8.750120685231394e-32,
+            "velocityX": 3.618098191401073e-32,
+            "velocityY": 7.301888602720515e-32,
             "timestamp": 0
         },
         {
-            "x": 2.256418341264554,
-            "y": 5.5583910221605555,
-            "heading": 3.140000000000001,
-            "angularVelocity": 2.3717958634313657e-14,
-            "velocityX": 0.2278392560297752,
-            "velocityY": -0.05711574046896039,
-            "timestamp": 0.028170480260500258
-        },
-        {
-            "x": 2.269255024758466,
-            "y": 5.555173071001971,
-            "heading": 3.1400000000000023,
-            "angularVelocity": 4.731676431156196e-14,
-            "velocityX": 0.4556785463083383,
-            "velocityY": -0.11423132047543481,
-            "timestamp": 0.056340960521000516
-        },
-        {
-            "x": 2.2885100515714525,
-            "y": 5.550346151638775,
-            "heading": 3.140000000000004,
-            "angularVelocity": 7.082238243214026e-14,
-            "velocityX": 0.6835178752697619,
-            "velocityY": -0.17134671892564213,
-            "timestamp": 0.08451144078150077
-        },
-        {
-            "x": 2.314183422941926,
-            "y": 5.543910269896753,
-            "heading": 3.140000000000007,
-            "angularVelocity": 9.417090371236357e-14,
-            "velocityX": 0.9113572481926052,
-            "velocityY": -0.22846191057115275,
-            "timestamp": 0.11268192104200103
-        },
-        {
-            "x": 2.3462751402864916,
-            "y": 5.535865432460115,
-            "heading": 3.1400000000000103,
-            "angularVelocity": 1.1735309251828409e-13,
-            "velocityX": 1.1391966714022896,
-            "velocityY": -0.28557686493967693,
-            "timestamp": 0.1408524013025013
-        },
-        {
-            "x": 2.384785205236557,
-            "y": 5.526211647057785,
-            "heading": 3.140000000000014,
-            "angularVelocity": 1.4030928785894893e-13,
-            "velocityX": 1.367036152523929,
-            "velocityY": -0.3426915449455824,
-            "timestamp": 0.16902288156300155
-        },
-        {
-            "x": 2.4297136196836084,
-            "y": 5.514948922700363,
-            "heading": 3.1400000000000188,
-            "angularVelocity": 1.629004634033803e-13,
-            "velocityX": 1.5948757007898307,
-            "velocityY": -0.39980590509187397,
-            "timestamp": 0.1971933618235018
-        },
-        {
-            "x": 2.481060385834354,
-            "y": 5.50207726998147,
-            "heading": 3.1400000000000237,
-            "angularVelocity": 1.8496997642686924e-13,
-            "velocityX": 1.822715327389804,
-            "velocityY": -0.4569198891841572,
-            "timestamp": 0.22536384208400206
-        },
-        {
-            "x": 2.5388255062752263,
-            "y": 5.487596701457088,
-            "heading": 3.14000000000003,
-            "angularVelocity": 2.0646227541738248e-13,
-            "velocityX": 2.0505550458033337,
-            "velocityY": -0.5140334275623145,
-            "timestamp": 0.25353432234450235
-        },
-        {
-            "x": 2.6030089840400077,
-            "y": 5.471507232107069,
-            "heading": 3.140000000000036,
-            "angularVelocity": 2.2698477398227208e-13,
-            "velocityX": 2.278394871910569,
-            "velocityY": -0.5711464341833905,
-            "timestamp": 0.28170480260500264
-        },
-        {
-            "x": 2.673610822657147,
-            "y": 5.453808879843062,
-            "heading": 3.140000000000043,
-            "angularVelocity": 2.462514676500777e-13,
-            "velocityX": 2.506234823271173,
-            "velocityY": -0.6282588049740611,
-            "timestamp": 0.3098752828655029
-        },
-        {
-            "x": 2.7506310261005877,
-            "y": 5.434501665888006,
-            "heading": 3.1400000000000508,
-            "angularVelocity": 2.636502833251031e-13,
-            "velocityX": 2.7340749156994817,
-            "velocityY": -0.6853704223895046,
-            "timestamp": 0.3380457631260032
-        },
-        {
-            "x": 2.834069598399475,
-            "y": 5.413585614377554,
-            "heading": 3.1400000000000587,
-            "angularVelocity": 2.7876299092334793e-13,
-            "velocityX": 2.9619151511549577,
-            "velocityY": -0.7424811830340069,
-            "timestamp": 0.3662162433865035
-        },
-        {
-            "x": 2.9239265420507494,
-            "y": 5.391060748829609,
-            "heading": 3.1400000000000667,
-            "angularVelocity": 2.92402388167881e-13,
-            "velocityX": 3.189755475247231,
-            "velocityY": -0.7995911088362938,
-            "timestamp": 0.3943867236470038
-        },
-        {
-            "x": 3.0202018518398366,
-            "y": 5.366927076318611,
-            "heading": 3.1400000000000756,
-            "angularVelocity": 3.135357172375438e-13,
-            "velocityX": 3.41759561423172,
-            "velocityY": -0.8567007835090982,
-            "timestamp": 0.42255720390750406
-        },
-        {
-            "x": 3.122895489704816,
-            "y": 5.341184519185279,
-            "heading": 3.140000000000087,
-            "angularVelocity": 4.18703156863577e-13,
-            "velocityX": 3.645434402088397,
-            "velocityY": -0.9138132149428461,
-            "timestamp": 0.45072768416800435
-        },
-        {
-            "x": 3.2262642847828022,
-            "y": 5.315271868839329,
-            "heading": 3.140000000000087,
-            "angularVelocity": 1.5556336116797473e-16,
-            "velocityX": 3.669401235694458,
-            "velocityY": -0.9198512097177582,
-            "timestamp": 0.47889816442850464
-        },
-        {
-            "x": 3.3296318094364086,
-            "y": 5.289354150507015,
-            "heading": 3.140000000000087,
-            "angularVelocity": -1.6083670828215077e-16,
-            "velocityX": 3.669356137976281,
-            "velocityY": -0.9200311138697771,
-            "timestamp": 0.5070686446890049
-        },
-        {
-            "x": 3.4329975109343875,
-            "y": 5.263429161959932,
-            "heading": 3.140000000000087,
-            "angularVelocity": 1.7787924056306163e-16,
-            "velocityX": 3.669291419320054,
-            "velocityY": -0.9202891930612488,
-            "timestamp": 0.5352391249495052
-        },
-        {
-            "x": 3.536360974744539,
-            "y": 5.23749525314096,
-            "heading": 3.140000000000087,
-            "angularVelocity": 1.5501276329034833e-16,
-            "velocityX": 3.669211985536661,
-            "velocityY": -0.9206058462317519,
-            "timestamp": 0.5634096052100055
-        },
-        {
-            "x": 3.6397250084453354,
-            "y": 5.211563615834452,
-            "heading": 3.140000000000087,
-            "angularVelocity": 2.0671720135979797e-16,
-            "velocityX": 3.6692322156015877,
-            "velocityY": -0.9205252117362434,
-            "timestamp": 0.5915800854705058
-        },
-        {
-            "x": 3.743083009106784,
-            "y": 5.185607942562298,
-            "heading": 3.140000000000087,
-            "angularVelocity": 1.5977153188511187e-16,
-            "velocityX": 3.6690180538517057,
-            "velocityY": -0.921378444106565,
-            "timestamp": 0.6197505657310061
-        },
-        {
-            "x": 3.8464299042889807,
-            "y": 5.159608086186739,
-            "heading": 3.140000000000087,
-            "angularVelocity": -2.0588084435766592e-17,
-            "velocityX": 3.6686238298573155,
-            "velocityY": -0.9229468626423776,
-            "timestamp": 0.6479210459915064
-        },
-        {
-            "x": 3.949030591223734,
-            "y": 5.130804053392629,
-            "heading": 3.140000000000087,
-            "angularVelocity": 7.696524756540132e-18,
-            "velocityX": 3.642134815806339,
-            "velocityY": -1.0224899443584623,
-            "timestamp": 0.6760915262520066
-        },
-        {
-            "x": 4.049738686087192,
-            "y": 5.095955052913767,
-            "heading": 3.140000000000087,
-            "angularVelocity": 3.5716950666970086e-16,
-            "velocityX": 3.5749512941271244,
-            "velocityY": -1.2370751281698067,
-            "timestamp": 0.7042620065125069
+            "x": 2.256418341264449,
+            "y": 5.558391022160101,
+            "heading": -2.5296452464584536e-15,
+            "angularVelocity": -8.979773235851594e-14,
+            "velocityX": 0.22783925602590605,
+            "velocityY": -0.05711574048502746,
+            "timestamp": 0.0281704802605187
+        },
+        {
+            "x": 2.2692550247581535,
+            "y": 5.555173071000616,
+            "heading": -7.593688933611638e-15,
+            "angularVelocity": -1.7976419432911875e-13,
+            "velocityX": 0.4556785463006615,
+            "velocityY": -0.11423132050732354,
+            "timestamp": 0.0563409605210374
+        },
+        {
+            "x": 2.2885100515708303,
+            "y": 5.55034615163608,
+            "heading": -1.5195474783343342e-14,
+            "angularVelocity": -2.6984935224723246e-13,
+            "velocityX": 0.6835178752583485,
+            "velocityY": -0.1713467189730698,
+            "timestamp": 0.0845114407815561
+        },
+        {
+            "x": 2.314183422940896,
+            "y": 5.54391026989229,
+            "heading": -2.533887633402318e-14,
+            "angularVelocity": -3.6007201361891127e-13,
+            "velocityX": 0.9113572481775378,
+            "velocityY": -0.2284619106337897,
+            "timestamp": 0.1126819210420748
+        },
+        {
+            "x": 2.346275140284958,
+            "y": 5.535865432453464,
+            "heading": -3.8024880419905343e-14,
+            "angularVelocity": -4.5032970578145327e-13,
+            "velocityX": 1.139196671383666,
+            "velocityY": -0.2855768650171331,
+            "timestamp": 0.14085240130259352
+        },
+        {
+            "x": 2.3847852052344276,
+            "y": 5.526211647048542,
+            "heading": -5.3249348657163737e-14,
+            "angularVelocity": -5.404404929621474e-13,
+            "velocityX": 1.3670361525018668,
+            "velocityY": -0.3426915450373883,
+            "timestamp": 0.16902288156311224
+        },
+        {
+            "x": 2.4297136196807942,
+            "y": 5.514948922688138,
+            "heading": -7.10022499667577e-14,
+            "angularVelocity": -6.301951953680268e-13,
+            "velocityX": 1.594875700764475,
+            "velocityY": -0.3998059051974526,
+            "timestamp": 0.19719336182363095
+        },
+        {
+            "x": 2.4810603858307716,
+            "y": 5.502077269965895,
+            "heading": -9.12659901579941e-14,
+            "angularVelocity": -7.193253365088161e-13,
+            "velocityX": 1.8227153273613368,
+            "velocityY": -0.45691988930278143,
+            "timestamp": 0.22536384208414967
+        },
+        {
+            "x": 2.5388255062707983,
+            "y": 5.4875967014378215,
+            "heading": -1.1400857239830863e-13,
+            "angularVelocity": -8.073196492084524e-13,
+            "velocityX": 2.050555045771992,
+            "velocityY": -0.5140334276930385,
+            "timestamp": 0.2535343223446684
+        },
+        {
+            "x": 2.603008984034667,
+            "y": 5.4715072320838045,
+            "heading": -1.3917722607752625e-13,
+            "angularVelocity": -8.934407027436765e-13,
+            "velocityX": 2.2783948718766736,
+            "velocityY": -0.571146434324935,
+            "timestamp": 0.2817048026051871
+        },
+        {
+            "x": 2.673610822650838,
+            "y": 5.453808879815544,
+            "heading": -1.6668826583849139e-13,
+            "angularVelocity": -9.76591080715578e-13,
+            "velocityX": 2.5062348232351805,
+            "velocityY": -0.6282588051246059,
+            "timestamp": 0.3098752828657058
+        },
+        {
+            "x": 2.7506310260932763,
+            "y": 5.434501665856061,
+            "heading": -1.9641690622397207e-13,
+            "angularVelocity": -1.0553118055097966e-12,
+            "velocityX": 2.7340749156620854,
+            "velocityY": -0.6853704225462842,
+            "timestamp": 0.33804576312622453
+        },
+        {
+            "x": 2.8340695983911575,
+            "y": 5.413585614341132,
+            "heading": -2.282023162882806e-13,
+            "angularVelocity": -1.128323328886794e-12,
+            "velocityX": 2.9619151511173074,
+            "velocityY": -0.7424811831924288,
+            "timestamp": 0.36621624338674325
+        },
+        {
+            "x": 2.9239265420414853,
+            "y": 5.391060748788907,
+            "heading": -2.61980248572899e-13,
+            "angularVelocity": -1.1990541861941579e-12,
+            "velocityX": 3.189755475211519,
+            "velocityY": -0.7995911089877218,
+            "timestamp": 0.39438672364726196
+        },
+        {
+            "x": 3.020201851829833,
+            "y": 5.366927076274376,
+            "heading": -2.9867647823192555e-13,
+            "angularVelocity": -1.302648350982896e-12,
+            "velocityX": 3.417595614203248,
+            "velocityY": -0.8567007836339452,
+            "timestamp": 0.4225572039077807
+        },
+        {
+            "x": 3.12289548969471,
+            "y": 5.341184519140017,
+            "heading": -3.474157859958988e-13,
+            "angularVelocity": -1.7301553723681501e-12,
+            "velocityX": 3.6454344020823566,
+            "velocityY": -0.9138132149787032,
+            "timestamp": 0.4507276841682994
+        },
+        {
+            "x": 3.2262642847742486,
+            "y": 5.315271868799975,
+            "heading": -3.474349355086831e-13,
+            "angularVelocity": -6.797723212352284e-16,
+            "velocityX": 3.6694012357471815,
+            "velocityY": -0.9198512095073661,
+            "timestamp": 0.4788981644288181
+        },
+        {
+            "x": 3.3296318094304937,
+            "y": 5.289354150477899,
+            "heading": -3.47428956348503e-13,
+            "angularVelocity": 2.1224913898406702e-16,
+            "velocityX": 3.669356138067555,
+            "velocityY": -0.920031113505745,
+            "timestamp": 0.5070686446893368
+        },
+        {
+            "x": 3.4329975109325144,
+            "y": 5.263429161946644,
+            "heading": -3.4742781851400116e-13,
+            "angularVelocity": 4.0391022494337805e-17,
+            "velocityX": 3.669291419461124,
+            "velocityY": -0.9202891924987843,
+            "timestamp": 0.5352391249498555
+        },
+        {
+            "x": 3.536360974747324,
+            "y": 5.237495253145949,
+            "heading": -3.474322284236003e-13,
+            "angularVelocity": -1.565436427905273e-16,
+            "velocityX": 3.669211985699591,
+            "velocityY": -0.9206058455823695,
+            "timestamp": 0.5634096052103742
+        },
+        {
+            "x": 3.6397250084501422,
+            "y": 5.211563615847215,
+            "heading": -3.474363074560882e-13,
+            "angularVelocity": -1.4479811668640362e-16,
+            "velocityX": 3.6692322156709736,
+            "velocityY": -0.9205252114596718,
+            "timestamp": 0.5915800854708929
+        },
+        {
+            "x": 3.7430830091153613,
+            "y": 5.185607942589789,
+            "heading": -3.474405753435316e-13,
+            "angularVelocity": -1.515021186684875e-16,
+            "velocityX": 3.669018053983158,
+            "velocityY": -0.9213784435831122,
+            "timestamp": 0.6197505657314116
+        },
+        {
+            "x": 3.8464299042958676,
+            "y": 5.1596080862072276,
+            "heading": -3.474364187384818e-13,
+            "angularVelocity": 1.4755179936907513e-16,
+            "velocityX": 3.668623829794913,
+            "velocityY": -0.92294686289042,
+            "timestamp": 0.6479210459919303
+        },
+        {
+            "x": 3.9490305912287407,
+            "y": 5.130804053406161,
+            "heading": -3.4744608105895375e-13,
+            "angularVelocity": -3.429945241470833e-16,
+            "velocityX": 3.642134815737201,
+            "velocityY": -1.0224899446047346,
+            "timestamp": 0.6760915262524491
+        },
+        {
+            "x": 4.049738686089906,
+            "y": 5.09595505292046,
+            "heading": -3.4745592092862155e-13,
+            "angularVelocity": -3.4929719255187864e-16,
+            "velocityX": 3.57495129404341,
+            "velocityY": -1.237075128411728,
+            "timestamp": 0.7042620065129678
         },
         {
             "x": 4.148105144500732,
             "y": 5.054959774017334,
-            "heading": 3.140000000000087,
-            "angularVelocity": 3.8010173489100904e-16,
-            "velocityX": 3.4918275266846264,
-            "velocityY": -1.455256655809122,
-            "timestamp": 0.7324324867730072
-        },
-        {
-            "x": 4.301558125078007,
-            "y": 4.971822103513501,
-            "heading": 3.140000000000087,
-            "angularVelocity": -3.218591761776222e-17,
-            "velocityX": 3.32615174670464,
-            "velocityY": -1.8020406441293784,
-            "timestamp": 0.7785677783240725
-        },
-        {
-            "x": 4.445898119119771,
-            "y": 4.873711676762074,
-            "heading": 3.140000000000087,
-            "angularVelocity": -4.812512433810467e-17,
-            "velocityX": 3.128624295828435,
-            "velocityY": -2.126580833303964,
-            "timestamp": 0.8247030698751379
-        },
-        {
-            "x": 4.5875715760336115,
-            "y": 4.7717885618775915,
-            "heading": 3.140000000000087,
-            "angularVelocity": -6.437963156007962e-17,
-            "velocityX": 3.0708260888967014,
-            "velocityY": -2.2092222994111217,
-            "timestamp": 0.8708383614262032
-        },
-        {
-            "x": 4.729219222551534,
-            "y": 4.669829579874243,
-            "heading": 3.140000000000087,
-            "angularVelocity": -5.401066324195286e-16,
-            "velocityX": 3.0702666386935125,
-            "velocityY": -2.2099997329304943,
-            "timestamp": 0.9169736529772685
-        },
-        {
-            "x": 4.870877206247071,
-            "y": 4.567884960570754,
-            "heading": 3.140000000000087,
-            "angularVelocity": -5.491807811904997e-16,
-            "velocityX": 3.0704907009776297,
-            "velocityY": -2.209688415909936,
-            "timestamp": 0.9631089445283338
-        },
-        {
-            "x": 5.012587244990002,
-            "y": 4.46601271378786,
-            "heading": 3.140000000000087,
-            "angularVelocity": 2.3992830315072076e-16,
-            "velocityX": 3.0716190139795567,
-            "velocityY": -2.2081197139288955,
-            "timestamp": 1.0092442360793992
-        },
-        {
-            "x": 5.160453670190759,
-            "y": 4.373302358536228,
-            "heading": 3.140000000000087,
-            "angularVelocity": 3.2225913830486856e-16,
-            "velocityX": 3.205061033110239,
-            "velocityY": -2.009532228659069,
-            "timestamp": 1.0553795276304645
-        },
-        {
-            "x": 5.316942928230888,
-            "y": 4.296031397168284,
-            "heading": 3.140000000000087,
-            "angularVelocity": 3.540598605731943e-16,
-            "velocityX": 3.391964216104365,
-            "velocityY": -1.674877491153309,
-            "timestamp": 1.1015148191815298
+            "heading": -3.474586084587748e-13,
+            "angularVelocity": -9.540235481877701e-17,
+            "velocityX": 3.491827526585996,
+            "velocityY": -1.4552566560457803,
+            "timestamp": 0.7324324867734865
+        },
+        {
+            "x": 4.301558125072517,
+            "y": 4.97182210350292,
+            "heading": -3.4745624581283064e-13,
+            "angularVelocity": 5.1211249699831376e-17,
+            "velocityX": 3.3261517465815813,
+            "velocityY": -1.8020406443565171,
+            "timestamp": 0.7785677783246081
+        },
+        {
+            "x": 4.445898119107738,
+            "y": 4.873711676741488,
+            "heading": -3.4747329562610653e-13,
+            "angularVelocity": -3.6956119081305154e-16,
+            "velocityX": 3.1286242956827928,
+            "velocityY": -2.1265808335182323,
+            "timestamp": 0.8247030698757297
+        },
+        {
+            "x": 4.587571576014754,
+            "y": 4.771788561847154,
+            "heading": -3.4746251546031876e-13,
+            "angularVelocity": 2.3366419539300746e-16,
+            "velocityX": 3.0708260887450245,
+            "velocityY": -2.2092222996219535,
+            "timestamp": 0.8708383614268513
+        },
+        {
+            "x": 4.729219222551993,
+            "y": 4.66982957987028,
+            "heading": -3.474587781166732e-13,
+            "angularVelocity": 8.100834549122579e-17,
+            "velocityX": 3.070266639108493,
+            "velocityY": -2.209999732353969,
+            "timestamp": 0.916973652977973
+        },
+        {
+            "x": 4.870877206231954,
+            "y": 4.567884960544782,
+            "heading": -3.47487867941798e-13,
+            "angularVelocity": -6.305330291624225e-16,
+            "velocityX": 3.070490700636254,
+            "velocityY": -2.209688416384298,
+            "timestamp": 0.9631089445290946
+        },
+        {
+            "x": 5.012587244979431,
+            "y": 4.466012713767847,
+            "heading": -3.4750411523417436e-13,
+            "angularVelocity": -3.5216624493767476e-16,
+            "velocityX": 3.0716190140743462,
+            "velocityY": -2.208119713797033,
+            "timestamp": 1.009244236080216
+        },
+        {
+            "x": 5.160453670184351,
+            "y": 4.373302358522453,
+            "heading": -3.47491376477911e-13,
+            "angularVelocity": 2.7611738924244047e-16,
+            "velocityX": 3.2050610331965634,
+            "velocityY": -2.009532228521384,
+            "timestamp": 1.0553795276313376
+        },
+        {
+            "x": 5.316942928228011,
+            "y": 4.29603139716118,
+            "heading": -3.4750002331748596e-13,
+            "angularVelocity": -1.8742353831509497e-16,
+            "velocityX": 3.3919642161767567,
+            "velocityY": -1.6748774910066955,
+            "timestamp": 1.101514819182459
         },
         {
             "x": 5.480467319488525,
             "y": 4.235044002532959,
-            "heading": 3.140000000000087,
-            "angularVelocity": 3.7214299347620075e-16,
-            "velocityX": 3.544453405635226,
-            "velocityY": -1.3219249859178432,
-            "timestamp": 1.1476501107325952
-        },
-        {
-            "x": 5.591417678770796,
-            "y": 4.201991970754989,
-            "heading": 3.140000000000087,
-            "angularVelocity": 2.3864185821028e-16,
-            "velocityX": 3.6254884971019536,
-            "velocityY": -1.0800304009303623,
-            "timestamp": 1.1782529824203136
-        },
-        {
-            "x": 5.704342870000873,
-            "y": 4.176490587908602,
-            "heading": 3.140000000000087,
-            "angularVelocity": 1.1130552624117688e-16,
-            "velocityX": 3.6900194328910065,
-            "velocityY": -0.8333003225044783,
-            "timestamp": 1.208855854108032
-        },
-        {
-            "x": 5.818728492964508,
-            "y": 4.158648255815286,
-            "heading": 3.140000000000087,
-            "angularVelocity": 1.0429128791162552e-16,
-            "velocityX": 3.737741481609135,
-            "velocityY": -0.583028033314901,
-            "timestamp": 1.2394587257957506
-        },
-        {
-            "x": 5.934052260793966,
-            "y": 4.148506842860091,
-            "heading": 3.140000000000087,
-            "angularVelocity": 7.090230424723168e-16,
-            "velocityX": 3.7683969336671863,
-            "velocityY": -0.3313876246402188,
-            "timestamp": 1.270061597483469
-        },
-        {
-            "x": 6.049784939893549,
-            "y": 4.145614314312393,
-            "heading": 3.140000000000087,
-            "angularVelocity": 5.904438564058107e-16,
-            "velocityX": 3.7817587931139536,
-            "velocityY": -0.0945182065847883,
-            "timestamp": 1.3006644691711875
-        },
-        {
-            "x": 6.1655453259799495,
-            "y": 4.144216905752427,
-            "heading": 3.140000000000087,
-            "angularVelocity": -4.945243197986353e-16,
-            "velocityX": 3.7826641652342365,
-            "velocityY": -0.04566266116445697,
-            "timestamp": 1.331267340858906
-        },
-        {
-            "x": 6.281306292238703,
-            "y": 4.142868414104831,
-            "heading": 3.140000000000087,
-            "angularVelocity": 1.3561359601509856e-16,
-            "velocityX": 3.782683123335836,
-            "velocityY": -0.04406421926917252,
-            "timestamp": 1.3618702125466244
-        },
-        {
-            "x": 6.397067393015407,
-            "y": 4.141531519361414,
-            "heading": 3.140000000000087,
-            "angularVelocity": -2.815905629868065e-16,
-            "velocityX": 3.782687518934728,
-            "velocityY": -0.04368527104047988,
-            "timestamp": 1.392473084234343
-        },
-        {
-            "x": 6.512828617959603,
-            "y": 4.140205420378013,
-            "heading": 3.140000000000087,
-            "angularVelocity": 4.8810263606010396e-17,
-            "velocityX": 3.782691576315187,
-            "velocityY": -0.043332501527048845,
-            "timestamp": 1.4230759559220614
-        },
-        {
-            "x": 6.628589931635543,
-            "y": 4.1388870909429585,
-            "heading": 3.140000000000087,
-            "angularVelocity": -3.260693519621353e-16,
-            "velocityX": 3.7826944757734187,
-            "velocityY": -0.04307861853318547,
-            "timestamp": 1.4536788276097798
-        },
-        {
-            "x": 6.744351296534275,
-            "y": 4.137573267441446,
-            "heading": 3.140000000000087,
-            "angularVelocity": -5.381950425280429e-16,
-            "velocityX": 3.782696149563972,
-            "velocityY": -0.04293137960615145,
-            "timestamp": 1.4842816992974983
-        },
-        {
-            "x": 6.860112695906051,
-            "y": 4.136261713626517,
-            "heading": 3.140000000000087,
-            "angularVelocity": 6.589038089157793e-16,
-            "velocityX": 3.7826972760284443,
-            "velocityY": -0.04285721378647441,
-            "timestamp": 1.5148845709852168
-        },
-        {
-            "x": 6.969430224754608,
-            "y": 4.135029589996691,
-            "heading": 3.1400000000000587,
-            "angularVelocity": -9.14507649037293e-13,
-            "velocityX": 3.5721330326141576,
-            "velocityY": -0.04026169968457299,
-            "timestamp": 1.5454874426729353
-        },
-        {
-            "x": 7.070940187047598,
-            "y": 4.13388425582119,
-            "heading": 3.140000000000041,
-            "angularVelocity": -5.92710722161247e-13,
-            "velocityX": 3.3170077412616483,
-            "velocityY": -0.037425709147242185,
-            "timestamp": 1.5760903143606537
-        },
-        {
-            "x": 7.16464169000894,
-            "y": 4.132826276442613,
-            "heading": 3.1400000000000285,
-            "angularVelocity": -4.0082806789240517e-13,
-            "velocityX": 3.06185327695782,
-            "velocityY": -0.034571245100923735,
-            "timestamp": 1.6066931860483722
-        },
-        {
-            "x": 7.250534738512908,
-            "y": 4.131855992104498,
-            "heading": 3.14000000000002,
-            "angularVelocity": -2.818757978482064e-13,
-            "velocityX": 2.8066989719287347,
-            "velocityY": -0.031705663029917695,
-            "timestamp": 1.6372960577360907
-        },
-        {
-            "x": 7.328619329487215,
-            "y": 4.130973602817362,
-            "heading": 3.140000000000014,
-            "angularVelocity": -2.020529664357193e-13,
-            "velocityX": 2.5515445665075394,
-            "velocityY": -0.028833545281456593,
-            "timestamp": 1.6678989294238091
-        },
-        {
-            "x": 7.398895462248533,
-            "y": 4.130179235854988,
-            "heading": 3.1400000000000095,
-            "angularVelocity": -1.455432238063259e-13,
-            "velocityX": 2.296390138757389,
-            "velocityY": -0.02595726864966503,
-            "timestamp": 1.6985018011115276
-        },
-        {
-            "x": 7.4613631364100685,
-            "y": 4.129472977966136,
-            "heading": 3.1400000000000063,
-            "angularVelocity": -1.0415995882589759e-13,
-            "velocityX": 2.0412356983681157,
-            "velocityY": -0.0230781573571433,
-            "timestamp": 1.729104672799246
-        },
-        {
-            "x": 7.5160223517191325,
-            "y": 4.128854891515294,
-            "heading": 3.1400000000000037,
-            "angularVelocity": -7.336162887662554e-14,
-            "velocityX": 1.7860812497217864,
-            "velocityY": -0.02019700820070875,
-            "timestamp": 1.7597075444869645
-        },
-        {
-            "x": 7.562873107995996,
-            "y": 4.12832502323739,
-            "heading": 3.1400000000000023,
-            "angularVelocity": -5.027990878360554e-14,
-            "velocityX": 1.5309267952025598,
-            "velocityY": -0.017314331902335514,
-            "timestamp": 1.790310416174683
-        },
-        {
-            "x": 7.6019154051050695,
-            "y": 4.127883409326492,
-            "heading": 3.1400000000000015,
-            "angularVelocity": -3.297949891981295e-14,
-            "velocityX": 1.2757723362526898,
-            "velocityY": -0.014430472902237576,
-            "timestamp": 1.8209132878624015
-        },
-        {
-            "x": 7.63314924293966,
-            "y": 4.127530078568267,
-            "heading": 3.1400000000000006,
-            "angularVelocity": -2.0267021915169143e-14,
-            "velocityX": 1.0206178738164648,
-            "velocityY": -0.011545673282216062,
-            "timestamp": 1.85151615955012
-        },
-        {
-            "x": 7.656574621413162,
-            "y": 4.127265054361888,
-            "heading": 3.14,
-            "angularVelocity": -1.117466715568672e-14,
-            "velocityX": 0.7654634085502764,
-            "velocityY": -0.008660109055131773,
-            "timestamp": 1.8821190312378384
-        },
-        {
-            "x": 7.672191540453593,
-            "y": 4.127088356077733,
-            "heading": 3.14,
-            "angularVelocity": -5.057381822634304e-15,
-            "velocityX": 0.5103089409318914,
-            "velocityY": -0.005773911868497247,
-            "timestamp": 1.9127219029255569
+            "heading": -3.475163170309309e-13,
+            "angularVelocity": -3.5317243928660403e-16,
+            "velocityX": 3.5444534056932553,
+            "velocityY": -1.321924985762245,
+            "timestamp": 1.1476501107335806
+        },
+        {
+            "x": 5.5914176787721175,
+            "y": 4.201991970759951,
+            "heading": -3.4752963658489967e-13,
+            "angularVelocity": -4.352387405096259e-16,
+            "velocityX": 3.625488497149835,
+            "velocityY": -1.0800304007696078,
+            "timestamp": 1.1782529824212593
+        },
+        {
+            "x": 5.704342870003183,
+            "y": 4.176490587918632,
+            "heading": -3.4752519346531394e-13,
+            "angularVelocity": 1.4518636145588672e-16,
+            "velocityX": 3.6900194329281493,
+            "velocityY": -0.8333003223399679,
+            "timestamp": 1.208855854108938
+        },
+        {
+            "x": 5.818728492967466,
+            "y": 4.15864825583045,
+            "heading": -3.47528320752536e-13,
+            "angularVelocity": -1.0218933861720526e-16,
+            "velocityX": 3.737741481635173,
+            "velocityY": -0.5830280331479286,
+            "timestamp": 1.2394587257966168
+        },
+        {
+            "x": 5.934052260797228,
+            "y": 4.148506842880417,
+            "heading": -3.475166383708343e-13,
+            "angularVelocity": 3.817413550372978e-16,
+            "velocityX": 3.768396933681978,
+            "velocityY": -0.3313876244719343,
+            "timestamp": 1.2700615974842955
+        },
+        {
+            "x": 6.049784939896788,
+            "y": 4.145614314337883,
+            "heading": -3.4752553093783573e-13,
+            "angularVelocity": -2.9057949488904467e-16,
+            "velocityX": 3.7817587931181613,
+            "velocityY": -0.0945182064161531,
+            "timestamp": 1.3006644691719742
+        },
+        {
+            "x": 6.1655453259831,
+            "y": 4.144216905783092,
+            "heading": -3.4753489515012823e-13,
+            "angularVelocity": -3.0599129336581387e-16,
+            "velocityX": 3.7826641652362696,
+            "velocityY": -0.04566266099543126,
+            "timestamp": 1.331267340859653
+        },
+        {
+            "x": 6.281306292241875,
+            "y": 4.142868414150292,
+            "heading": -3.4754223223995867e-13,
+            "angularVelocity": -2.397516775868154e-16,
+            "velocityX": 3.7826831233414593,
+            "velocityY": -0.04406421878574171,
+            "timestamp": 1.3618702125473316
+        },
+        {
+            "x": 6.39706739301833,
+            "y": 4.141531519398385,
+            "heading": -3.475633687164305e-13,
+            "angularVelocity": -6.906697084916283e-16,
+            "velocityX": 3.7826875189315166,
+            "velocityY": -0.04368527131792852,
+            "timestamp": 1.3924730842350104
+        },
+        {
+            "x": 6.512828617962261,
+            "y": 4.14020542040497,
+            "heading": -3.4758398313992887e-13,
+            "angularVelocity": -6.73610754719036e-16,
+            "velocityX": 3.7826915763114313,
+            "velocityY": -0.043332501854336236,
+            "timestamp": 1.423075955922689
+        },
+        {
+            "x": 6.628589931637748,
+            "y": 4.138887090943481,
+            "heading": -3.4757335612235213e-13,
+            "angularVelocity": 3.472555676974266e-16,
+            "velocityX": 3.7826944757635745,
+            "velocityY": -0.04307861939704374,
+            "timestamp": 1.4536788276103678
+        },
+        {
+            "x": 6.744351296535766,
+            "y": 4.137573267392239,
+            "heading": -3.475819448429605e-13,
+            "angularVelocity": -2.806508055564177e-16,
+            "velocityX": 3.782696149545524,
+            "velocityY": -0.042931381231176444,
+            "timestamp": 1.4842816992980465
+        },
+        {
+            "x": 6.860112695906494,
+            "y": 4.136261713498386,
+            "heading": -3.475979530506806e-13,
+            "angularVelocity": -5.230943272138926e-16,
+            "velocityX": 3.7826972759991557,
+            "velocityY": -0.04285721636551622,
+            "timestamp": 1.5148845709857253
+        },
+        {
+            "x": 6.9694302247553,
+            "y": 4.135029589899985,
+            "heading": -2.392571520797574e-13,
+            "angularVelocity": 3.5402168172320343e-12,
+            "velocityX": 3.5721330326269505,
+            "velocityY": -0.04026169865772549,
+            "timestamp": 1.545487442673404
+        },
+        {
+            "x": 7.070940187048315,
+            "y": 4.133884255746116,
+            "heading": -1.6833543298116243e-13,
+            "angularVelocity": 2.317485846705618e-12,
+            "velocityX": 3.3170077412667367,
+            "velocityY": -0.03742570844047695,
+            "timestamp": 1.5760903143610827
+        },
+        {
+            "x": 7.164641690009617,
+            "y": 4.132826276383854,
+            "heading": -1.1981581485168293e-13,
+            "angularVelocity": 1.5854597325485145e-12,
+            "velocityX": 3.061853276960511,
+            "velocityY": -0.034571244567840914,
+            "timestamp": 1.6066931860487614
+        },
+        {
+            "x": 7.250534738513507,
+            "y": 4.131855992058627,
+            "heading": -8.523066967233492e-14,
+            "angularVelocity": 1.1301275060622065e-12,
+            "velocityX": 2.8066989719298543,
+            "velocityY": -0.03170566260882202,
+            "timestamp": 1.6372960577364402
+        },
+        {
+            "x": 7.328619329487729,
+            "y": 4.1309736027819035,
+            "heading": -6.003549662940244e-14,
+            "angularVelocity": 8.232944437274219e-13,
+            "velocityX": 2.5515445665080714,
+            "velocityY": -0.028833544941282405,
+            "timestamp": 1.6678989294241189
+        },
+        {
+            "x": 7.398895462248961,
+            "y": 4.130179235828021,
+            "heading": -4.153185117983844e-14,
+            "angularVelocity": 6.046375734752488e-13,
+            "velocityX": 2.2963901387575416,
+            "velocityY": -0.02595726837221309,
+            "timestamp": 1.6985018011117976
+        },
+        {
+            "x": 7.461363136410412,
+            "y": 4.129472977946104,
+            "heading": -2.797073854746349e-14,
+            "angularVelocity": 4.431320366500053e-13,
+            "velocityX": 2.041235698368024,
+            "velocityY": -0.0230781571305896,
+            "timestamp": 1.7291046727994763
+        },
+        {
+            "x": 7.516022351719397,
+            "y": 4.1288548915008905,
+            "heading": -1.8141238812912564e-14,
+            "angularVelocity": 3.211953369611228e-13,
+            "velocityX": 1.7860812497215437,
+            "velocityY": -0.02019700801679021,
+            "timestamp": 1.759707544487155
+        },
+        {
+            "x": 7.5628731079961895,
+            "y": 4.128325023227497,
+            "heading": -1.1163529343344889e-14,
+            "angularVelocity": 2.2800831618830555e-13,
+            "velocityX": 1.5309267952022354,
+            "velocityY": -0.017314331754962387,
+            "timestamp": 1.7903104161748338
+        },
+        {
+            "x": 7.601915405105201,
+            "y": 4.127883409320133,
+            "heading": -6.372564950074351e-15,
+            "angularVelocity": 1.5655276164796086e-13,
+            "velocityX": 1.2757723362523365,
+            "velocityY": -0.014430472786738478,
+            "timestamp": 1.8209132878625125
+        },
+        {
+            "x": 7.63314924293974,
+            "y": 4.127530078564582,
+            "heading": -3.2435183420078772e-15,
+            "angularVelocity": 1.0224681844586284e-13,
+            "velocityX": 1.020617873816125,
+            "velocityY": -0.011545673194894595,
+            "timestamp": 1.8515161595501912
+        },
+        {
+            "x": 7.656574621413203,
+            "y": 4.127265054360105,
+            "heading": -1.3581702257498648e-15,
+            "angularVelocity": 6.160689169092944e-14,
+            "velocityX": 0.7654634085499833,
+            "velocityY": -0.008660108992989924,
+            "timestamp": 1.88211903123787
+        },
+        {
+            "x": 7.672191540453606,
+            "y": 4.127088356077157,
+            "heading": -3.7222995775515084e-16,
+            "angularVelocity": 3.2217238765593925e-14,
+            "velocityX": 0.5103089409316736,
+            "velocityY": -0.0057739118290542285,
+            "timestamp": 1.9127219029255487
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": -1.4274059532547784e-15,
-            "velocityX": 0.2551544713218168,
-            "velocityY": -0.002887182568422342,
-            "timestamp": 1.9433247746132754
+            "heading": 2.9643987227812266e-34,
+            "angularVelocity": 1.2163231774130929e-14,
+            "velocityX": 0.25515447132169783,
+            "velocityY": -0.0028871825495910644,
+            "timestamp": 1.9433247746132274
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 3.6920170793948963e-32,
-            "velocityX": 1.993299363399013e-33,
-            "velocityY": 1.7597210552293873e-31,
-            "timestamp": 1.9739276463009938
+            "heading": -1.7511816257564745e-34,
+            "angularVelocity": -1.536305146096975e-32,
+            "velocityX": 2.290424541792184e-34,
+            "velocityY": 7.21466542895957e-33,
+            "timestamp": 1.9739276463009061
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.traj
index 14bb5c7a..72a99c84 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.traj	
@@ -3,569 +3,569 @@
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 1.2492921998824363e-31,
-            "velocityX": 1.8348820495773462e-32,
-            "velocityY": 2.100679182506497e-31,
+            "heading": 0,
+            "angularVelocity": 8.750120685231394e-32,
+            "velocityX": 3.618098191401073e-32,
+            "velocityY": 7.301888602720515e-32,
             "timestamp": 0
         },
         {
-            "x": 2.256418341264554,
-            "y": 5.5583910221605555,
-            "heading": 3.140000000000001,
-            "angularVelocity": 2.3717958634313657e-14,
-            "velocityX": 0.2278392560297752,
-            "velocityY": -0.05711574046896039,
-            "timestamp": 0.028170480260500258
-        },
-        {
-            "x": 2.269255024758466,
-            "y": 5.555173071001971,
-            "heading": 3.1400000000000023,
-            "angularVelocity": 4.731676431156196e-14,
-            "velocityX": 0.4556785463083383,
-            "velocityY": -0.11423132047543481,
-            "timestamp": 0.056340960521000516
-        },
-        {
-            "x": 2.2885100515714525,
-            "y": 5.550346151638775,
-            "heading": 3.140000000000004,
-            "angularVelocity": 7.082238243214026e-14,
-            "velocityX": 0.6835178752697619,
-            "velocityY": -0.17134671892564213,
-            "timestamp": 0.08451144078150077
-        },
-        {
-            "x": 2.314183422941926,
-            "y": 5.543910269896753,
-            "heading": 3.140000000000007,
-            "angularVelocity": 9.417090371236357e-14,
-            "velocityX": 0.9113572481926052,
-            "velocityY": -0.22846191057115275,
-            "timestamp": 0.11268192104200103
-        },
-        {
-            "x": 2.3462751402864916,
-            "y": 5.535865432460115,
-            "heading": 3.1400000000000103,
-            "angularVelocity": 1.1735309251828409e-13,
-            "velocityX": 1.1391966714022896,
-            "velocityY": -0.28557686493967693,
-            "timestamp": 0.1408524013025013
-        },
-        {
-            "x": 2.384785205236557,
-            "y": 5.526211647057785,
-            "heading": 3.140000000000014,
-            "angularVelocity": 1.4030928785894893e-13,
-            "velocityX": 1.367036152523929,
-            "velocityY": -0.3426915449455824,
-            "timestamp": 0.16902288156300155
-        },
-        {
-            "x": 2.4297136196836084,
-            "y": 5.514948922700363,
-            "heading": 3.1400000000000188,
-            "angularVelocity": 1.629004634033803e-13,
-            "velocityX": 1.5948757007898307,
-            "velocityY": -0.39980590509187397,
-            "timestamp": 0.1971933618235018
-        },
-        {
-            "x": 2.481060385834354,
-            "y": 5.50207726998147,
-            "heading": 3.1400000000000237,
-            "angularVelocity": 1.8496997642686924e-13,
-            "velocityX": 1.822715327389804,
-            "velocityY": -0.4569198891841572,
-            "timestamp": 0.22536384208400206
-        },
-        {
-            "x": 2.5388255062752263,
-            "y": 5.487596701457088,
-            "heading": 3.14000000000003,
-            "angularVelocity": 2.0646227541738248e-13,
-            "velocityX": 2.0505550458033337,
-            "velocityY": -0.5140334275623145,
-            "timestamp": 0.25353432234450235
-        },
-        {
-            "x": 2.6030089840400077,
-            "y": 5.471507232107069,
-            "heading": 3.140000000000036,
-            "angularVelocity": 2.2698477398227208e-13,
-            "velocityX": 2.278394871910569,
-            "velocityY": -0.5711464341833905,
-            "timestamp": 0.28170480260500264
-        },
-        {
-            "x": 2.673610822657147,
-            "y": 5.453808879843062,
-            "heading": 3.140000000000043,
-            "angularVelocity": 2.462514676500777e-13,
-            "velocityX": 2.506234823271173,
-            "velocityY": -0.6282588049740611,
-            "timestamp": 0.3098752828655029
-        },
-        {
-            "x": 2.7506310261005877,
-            "y": 5.434501665888006,
-            "heading": 3.1400000000000508,
-            "angularVelocity": 2.636502833251031e-13,
-            "velocityX": 2.7340749156994817,
-            "velocityY": -0.6853704223895046,
-            "timestamp": 0.3380457631260032
-        },
-        {
-            "x": 2.834069598399475,
-            "y": 5.413585614377554,
-            "heading": 3.1400000000000587,
-            "angularVelocity": 2.7876299092334793e-13,
-            "velocityX": 2.9619151511549577,
-            "velocityY": -0.7424811830340069,
-            "timestamp": 0.3662162433865035
-        },
-        {
-            "x": 2.9239265420507494,
-            "y": 5.391060748829609,
-            "heading": 3.1400000000000667,
-            "angularVelocity": 2.92402388167881e-13,
-            "velocityX": 3.189755475247231,
-            "velocityY": -0.7995911088362938,
-            "timestamp": 0.3943867236470038
-        },
-        {
-            "x": 3.0202018518398366,
-            "y": 5.366927076318611,
-            "heading": 3.1400000000000756,
-            "angularVelocity": 3.135357172375438e-13,
-            "velocityX": 3.41759561423172,
-            "velocityY": -0.8567007835090982,
-            "timestamp": 0.42255720390750406
-        },
-        {
-            "x": 3.122895489704816,
-            "y": 5.341184519185279,
-            "heading": 3.140000000000087,
-            "angularVelocity": 4.18703156863577e-13,
-            "velocityX": 3.645434402088397,
-            "velocityY": -0.9138132149428461,
-            "timestamp": 0.45072768416800435
-        },
-        {
-            "x": 3.2262642847828022,
-            "y": 5.315271868839329,
-            "heading": 3.140000000000087,
-            "angularVelocity": 1.5556336116797473e-16,
-            "velocityX": 3.669401235694458,
-            "velocityY": -0.9198512097177582,
-            "timestamp": 0.47889816442850464
-        },
-        {
-            "x": 3.3296318094364086,
-            "y": 5.289354150507015,
-            "heading": 3.140000000000087,
-            "angularVelocity": -1.6083670828215077e-16,
-            "velocityX": 3.669356137976281,
-            "velocityY": -0.9200311138697771,
-            "timestamp": 0.5070686446890049
-        },
-        {
-            "x": 3.4329975109343875,
-            "y": 5.263429161959932,
-            "heading": 3.140000000000087,
-            "angularVelocity": 1.7787924056306163e-16,
-            "velocityX": 3.669291419320054,
-            "velocityY": -0.9202891930612488,
-            "timestamp": 0.5352391249495052
-        },
-        {
-            "x": 3.536360974744539,
-            "y": 5.23749525314096,
-            "heading": 3.140000000000087,
-            "angularVelocity": 1.5501276329034833e-16,
-            "velocityX": 3.669211985536661,
-            "velocityY": -0.9206058462317519,
-            "timestamp": 0.5634096052100055
-        },
-        {
-            "x": 3.6397250084453354,
-            "y": 5.211563615834452,
-            "heading": 3.140000000000087,
-            "angularVelocity": 2.0671720135979797e-16,
-            "velocityX": 3.6692322156015877,
-            "velocityY": -0.9205252117362434,
-            "timestamp": 0.5915800854705058
-        },
-        {
-            "x": 3.743083009106784,
-            "y": 5.185607942562298,
-            "heading": 3.140000000000087,
-            "angularVelocity": 1.5977153188511187e-16,
-            "velocityX": 3.6690180538517057,
-            "velocityY": -0.921378444106565,
-            "timestamp": 0.6197505657310061
-        },
-        {
-            "x": 3.8464299042889807,
-            "y": 5.159608086186739,
-            "heading": 3.140000000000087,
-            "angularVelocity": -2.0588084435766592e-17,
-            "velocityX": 3.6686238298573155,
-            "velocityY": -0.9229468626423776,
-            "timestamp": 0.6479210459915064
-        },
-        {
-            "x": 3.949030591223734,
-            "y": 5.130804053392629,
-            "heading": 3.140000000000087,
-            "angularVelocity": 7.696524756540132e-18,
-            "velocityX": 3.642134815806339,
-            "velocityY": -1.0224899443584623,
-            "timestamp": 0.6760915262520066
-        },
-        {
-            "x": 4.049738686087192,
-            "y": 5.095955052913767,
-            "heading": 3.140000000000087,
-            "angularVelocity": 3.5716950666970086e-16,
-            "velocityX": 3.5749512941271244,
-            "velocityY": -1.2370751281698067,
-            "timestamp": 0.7042620065125069
+            "x": 2.256418341264449,
+            "y": 5.558391022160101,
+            "heading": -2.5296452464584536e-15,
+            "angularVelocity": -8.979773235851594e-14,
+            "velocityX": 0.22783925602590605,
+            "velocityY": -0.05711574048502746,
+            "timestamp": 0.0281704802605187
+        },
+        {
+            "x": 2.2692550247581535,
+            "y": 5.555173071000616,
+            "heading": -7.593688933611638e-15,
+            "angularVelocity": -1.7976419432911875e-13,
+            "velocityX": 0.4556785463006615,
+            "velocityY": -0.11423132050732354,
+            "timestamp": 0.0563409605210374
+        },
+        {
+            "x": 2.2885100515708303,
+            "y": 5.55034615163608,
+            "heading": -1.5195474783343342e-14,
+            "angularVelocity": -2.6984935224723246e-13,
+            "velocityX": 0.6835178752583485,
+            "velocityY": -0.1713467189730698,
+            "timestamp": 0.0845114407815561
+        },
+        {
+            "x": 2.314183422940896,
+            "y": 5.54391026989229,
+            "heading": -2.533887633402318e-14,
+            "angularVelocity": -3.6007201361891127e-13,
+            "velocityX": 0.9113572481775378,
+            "velocityY": -0.2284619106337897,
+            "timestamp": 0.1126819210420748
+        },
+        {
+            "x": 2.346275140284958,
+            "y": 5.535865432453464,
+            "heading": -3.8024880419905343e-14,
+            "angularVelocity": -4.5032970578145327e-13,
+            "velocityX": 1.139196671383666,
+            "velocityY": -0.2855768650171331,
+            "timestamp": 0.14085240130259352
+        },
+        {
+            "x": 2.3847852052344276,
+            "y": 5.526211647048542,
+            "heading": -5.3249348657163737e-14,
+            "angularVelocity": -5.404404929621474e-13,
+            "velocityX": 1.3670361525018668,
+            "velocityY": -0.3426915450373883,
+            "timestamp": 0.16902288156311224
+        },
+        {
+            "x": 2.4297136196807942,
+            "y": 5.514948922688138,
+            "heading": -7.10022499667577e-14,
+            "angularVelocity": -6.301951953680268e-13,
+            "velocityX": 1.594875700764475,
+            "velocityY": -0.3998059051974526,
+            "timestamp": 0.19719336182363095
+        },
+        {
+            "x": 2.4810603858307716,
+            "y": 5.502077269965895,
+            "heading": -9.12659901579941e-14,
+            "angularVelocity": -7.193253365088161e-13,
+            "velocityX": 1.8227153273613368,
+            "velocityY": -0.45691988930278143,
+            "timestamp": 0.22536384208414967
+        },
+        {
+            "x": 2.5388255062707983,
+            "y": 5.4875967014378215,
+            "heading": -1.1400857239830863e-13,
+            "angularVelocity": -8.073196492084524e-13,
+            "velocityX": 2.050555045771992,
+            "velocityY": -0.5140334276930385,
+            "timestamp": 0.2535343223446684
+        },
+        {
+            "x": 2.603008984034667,
+            "y": 5.4715072320838045,
+            "heading": -1.3917722607752625e-13,
+            "angularVelocity": -8.934407027436765e-13,
+            "velocityX": 2.2783948718766736,
+            "velocityY": -0.571146434324935,
+            "timestamp": 0.2817048026051871
+        },
+        {
+            "x": 2.673610822650838,
+            "y": 5.453808879815544,
+            "heading": -1.6668826583849139e-13,
+            "angularVelocity": -9.76591080715578e-13,
+            "velocityX": 2.5062348232351805,
+            "velocityY": -0.6282588051246059,
+            "timestamp": 0.3098752828657058
+        },
+        {
+            "x": 2.7506310260932763,
+            "y": 5.434501665856061,
+            "heading": -1.9641690622397207e-13,
+            "angularVelocity": -1.0553118055097966e-12,
+            "velocityX": 2.7340749156620854,
+            "velocityY": -0.6853704225462842,
+            "timestamp": 0.33804576312622453
+        },
+        {
+            "x": 2.8340695983911575,
+            "y": 5.413585614341132,
+            "heading": -2.282023162882806e-13,
+            "angularVelocity": -1.128323328886794e-12,
+            "velocityX": 2.9619151511173074,
+            "velocityY": -0.7424811831924288,
+            "timestamp": 0.36621624338674325
+        },
+        {
+            "x": 2.9239265420414853,
+            "y": 5.391060748788907,
+            "heading": -2.61980248572899e-13,
+            "angularVelocity": -1.1990541861941579e-12,
+            "velocityX": 3.189755475211519,
+            "velocityY": -0.7995911089877218,
+            "timestamp": 0.39438672364726196
+        },
+        {
+            "x": 3.020201851829833,
+            "y": 5.366927076274376,
+            "heading": -2.9867647823192555e-13,
+            "angularVelocity": -1.302648350982896e-12,
+            "velocityX": 3.417595614203248,
+            "velocityY": -0.8567007836339452,
+            "timestamp": 0.4225572039077807
+        },
+        {
+            "x": 3.12289548969471,
+            "y": 5.341184519140017,
+            "heading": -3.474157859958988e-13,
+            "angularVelocity": -1.7301553723681501e-12,
+            "velocityX": 3.6454344020823566,
+            "velocityY": -0.9138132149787032,
+            "timestamp": 0.4507276841682994
+        },
+        {
+            "x": 3.2262642847742486,
+            "y": 5.315271868799975,
+            "heading": -3.474349355086831e-13,
+            "angularVelocity": -6.797723212352284e-16,
+            "velocityX": 3.6694012357471815,
+            "velocityY": -0.9198512095073661,
+            "timestamp": 0.4788981644288181
+        },
+        {
+            "x": 3.3296318094304937,
+            "y": 5.289354150477899,
+            "heading": -3.47428956348503e-13,
+            "angularVelocity": 2.1224913898406702e-16,
+            "velocityX": 3.669356138067555,
+            "velocityY": -0.920031113505745,
+            "timestamp": 0.5070686446893368
+        },
+        {
+            "x": 3.4329975109325144,
+            "y": 5.263429161946644,
+            "heading": -3.4742781851400116e-13,
+            "angularVelocity": 4.0391022494337805e-17,
+            "velocityX": 3.669291419461124,
+            "velocityY": -0.9202891924987843,
+            "timestamp": 0.5352391249498555
+        },
+        {
+            "x": 3.536360974747324,
+            "y": 5.237495253145949,
+            "heading": -3.474322284236003e-13,
+            "angularVelocity": -1.565436427905273e-16,
+            "velocityX": 3.669211985699591,
+            "velocityY": -0.9206058455823695,
+            "timestamp": 0.5634096052103742
+        },
+        {
+            "x": 3.6397250084501422,
+            "y": 5.211563615847215,
+            "heading": -3.474363074560882e-13,
+            "angularVelocity": -1.4479811668640362e-16,
+            "velocityX": 3.6692322156709736,
+            "velocityY": -0.9205252114596718,
+            "timestamp": 0.5915800854708929
+        },
+        {
+            "x": 3.7430830091153613,
+            "y": 5.185607942589789,
+            "heading": -3.474405753435316e-13,
+            "angularVelocity": -1.515021186684875e-16,
+            "velocityX": 3.669018053983158,
+            "velocityY": -0.9213784435831122,
+            "timestamp": 0.6197505657314116
+        },
+        {
+            "x": 3.8464299042958676,
+            "y": 5.1596080862072276,
+            "heading": -3.474364187384818e-13,
+            "angularVelocity": 1.4755179936907513e-16,
+            "velocityX": 3.668623829794913,
+            "velocityY": -0.92294686289042,
+            "timestamp": 0.6479210459919303
+        },
+        {
+            "x": 3.9490305912287407,
+            "y": 5.130804053406161,
+            "heading": -3.4744608105895375e-13,
+            "angularVelocity": -3.429945241470833e-16,
+            "velocityX": 3.642134815737201,
+            "velocityY": -1.0224899446047346,
+            "timestamp": 0.6760915262524491
+        },
+        {
+            "x": 4.049738686089906,
+            "y": 5.09595505292046,
+            "heading": -3.4745592092862155e-13,
+            "angularVelocity": -3.4929719255187864e-16,
+            "velocityX": 3.57495129404341,
+            "velocityY": -1.237075128411728,
+            "timestamp": 0.7042620065129678
         },
         {
             "x": 4.148105144500732,
             "y": 5.054959774017334,
-            "heading": 3.140000000000087,
-            "angularVelocity": 3.8010173489100904e-16,
-            "velocityX": 3.4918275266846264,
-            "velocityY": -1.455256655809122,
-            "timestamp": 0.7324324867730072
-        },
-        {
-            "x": 4.301558125078007,
-            "y": 4.971822103513501,
-            "heading": 3.140000000000087,
-            "angularVelocity": -3.218591761776222e-17,
-            "velocityX": 3.32615174670464,
-            "velocityY": -1.8020406441293784,
-            "timestamp": 0.7785677783240725
-        },
-        {
-            "x": 4.445898119119771,
-            "y": 4.873711676762074,
-            "heading": 3.140000000000087,
-            "angularVelocity": -4.812512433810467e-17,
-            "velocityX": 3.128624295828435,
-            "velocityY": -2.126580833303964,
-            "timestamp": 0.8247030698751379
-        },
-        {
-            "x": 4.5875715760336115,
-            "y": 4.7717885618775915,
-            "heading": 3.140000000000087,
-            "angularVelocity": -6.437963156007962e-17,
-            "velocityX": 3.0708260888967014,
-            "velocityY": -2.2092222994111217,
-            "timestamp": 0.8708383614262032
-        },
-        {
-            "x": 4.729219222551534,
-            "y": 4.669829579874243,
-            "heading": 3.140000000000087,
-            "angularVelocity": -5.401066324195286e-16,
-            "velocityX": 3.0702666386935125,
-            "velocityY": -2.2099997329304943,
-            "timestamp": 0.9169736529772685
-        },
-        {
-            "x": 4.870877206247071,
-            "y": 4.567884960570754,
-            "heading": 3.140000000000087,
-            "angularVelocity": -5.491807811904997e-16,
-            "velocityX": 3.0704907009776297,
-            "velocityY": -2.209688415909936,
-            "timestamp": 0.9631089445283338
-        },
-        {
-            "x": 5.012587244990002,
-            "y": 4.46601271378786,
-            "heading": 3.140000000000087,
-            "angularVelocity": 2.3992830315072076e-16,
-            "velocityX": 3.0716190139795567,
-            "velocityY": -2.2081197139288955,
-            "timestamp": 1.0092442360793992
-        },
-        {
-            "x": 5.160453670190759,
-            "y": 4.373302358536228,
-            "heading": 3.140000000000087,
-            "angularVelocity": 3.2225913830486856e-16,
-            "velocityX": 3.205061033110239,
-            "velocityY": -2.009532228659069,
-            "timestamp": 1.0553795276304645
-        },
-        {
-            "x": 5.316942928230888,
-            "y": 4.296031397168284,
-            "heading": 3.140000000000087,
-            "angularVelocity": 3.540598605731943e-16,
-            "velocityX": 3.391964216104365,
-            "velocityY": -1.674877491153309,
-            "timestamp": 1.1015148191815298
+            "heading": -3.474586084587748e-13,
+            "angularVelocity": -9.540235481877701e-17,
+            "velocityX": 3.491827526585996,
+            "velocityY": -1.4552566560457803,
+            "timestamp": 0.7324324867734865
+        },
+        {
+            "x": 4.301558125072517,
+            "y": 4.97182210350292,
+            "heading": -3.4745624581283064e-13,
+            "angularVelocity": 5.1211249699831376e-17,
+            "velocityX": 3.3261517465815813,
+            "velocityY": -1.8020406443565171,
+            "timestamp": 0.7785677783246081
+        },
+        {
+            "x": 4.445898119107738,
+            "y": 4.873711676741488,
+            "heading": -3.4747329562610653e-13,
+            "angularVelocity": -3.6956119081305154e-16,
+            "velocityX": 3.1286242956827928,
+            "velocityY": -2.1265808335182323,
+            "timestamp": 0.8247030698757297
+        },
+        {
+            "x": 4.587571576014754,
+            "y": 4.771788561847154,
+            "heading": -3.4746251546031876e-13,
+            "angularVelocity": 2.3366419539300746e-16,
+            "velocityX": 3.0708260887450245,
+            "velocityY": -2.2092222996219535,
+            "timestamp": 0.8708383614268513
+        },
+        {
+            "x": 4.729219222551993,
+            "y": 4.66982957987028,
+            "heading": -3.474587781166732e-13,
+            "angularVelocity": 8.100834549122579e-17,
+            "velocityX": 3.070266639108493,
+            "velocityY": -2.209999732353969,
+            "timestamp": 0.916973652977973
+        },
+        {
+            "x": 4.870877206231954,
+            "y": 4.567884960544782,
+            "heading": -3.47487867941798e-13,
+            "angularVelocity": -6.305330291624225e-16,
+            "velocityX": 3.070490700636254,
+            "velocityY": -2.209688416384298,
+            "timestamp": 0.9631089445290946
+        },
+        {
+            "x": 5.012587244979431,
+            "y": 4.466012713767847,
+            "heading": -3.4750411523417436e-13,
+            "angularVelocity": -3.5216624493767476e-16,
+            "velocityX": 3.0716190140743462,
+            "velocityY": -2.208119713797033,
+            "timestamp": 1.009244236080216
+        },
+        {
+            "x": 5.160453670184351,
+            "y": 4.373302358522453,
+            "heading": -3.47491376477911e-13,
+            "angularVelocity": 2.7611738924244047e-16,
+            "velocityX": 3.2050610331965634,
+            "velocityY": -2.009532228521384,
+            "timestamp": 1.0553795276313376
+        },
+        {
+            "x": 5.316942928228011,
+            "y": 4.29603139716118,
+            "heading": -3.4750002331748596e-13,
+            "angularVelocity": -1.8742353831509497e-16,
+            "velocityX": 3.3919642161767567,
+            "velocityY": -1.6748774910066955,
+            "timestamp": 1.101514819182459
         },
         {
             "x": 5.480467319488525,
             "y": 4.235044002532959,
-            "heading": 3.140000000000087,
-            "angularVelocity": 3.7214299347620075e-16,
-            "velocityX": 3.544453405635226,
-            "velocityY": -1.3219249859178432,
-            "timestamp": 1.1476501107325952
-        },
-        {
-            "x": 5.591417678770796,
-            "y": 4.201991970754989,
-            "heading": 3.140000000000087,
-            "angularVelocity": 2.3864185821028e-16,
-            "velocityX": 3.6254884971019536,
-            "velocityY": -1.0800304009303623,
-            "timestamp": 1.1782529824203136
-        },
-        {
-            "x": 5.704342870000873,
-            "y": 4.176490587908602,
-            "heading": 3.140000000000087,
-            "angularVelocity": 1.1130552624117688e-16,
-            "velocityX": 3.6900194328910065,
-            "velocityY": -0.8333003225044783,
-            "timestamp": 1.208855854108032
-        },
-        {
-            "x": 5.818728492964508,
-            "y": 4.158648255815286,
-            "heading": 3.140000000000087,
-            "angularVelocity": 1.0429128791162552e-16,
-            "velocityX": 3.737741481609135,
-            "velocityY": -0.583028033314901,
-            "timestamp": 1.2394587257957506
-        },
-        {
-            "x": 5.934052260793966,
-            "y": 4.148506842860091,
-            "heading": 3.140000000000087,
-            "angularVelocity": 7.090230424723168e-16,
-            "velocityX": 3.7683969336671863,
-            "velocityY": -0.3313876246402188,
-            "timestamp": 1.270061597483469
-        },
-        {
-            "x": 6.049784939893549,
-            "y": 4.145614314312393,
-            "heading": 3.140000000000087,
-            "angularVelocity": 5.904438564058107e-16,
-            "velocityX": 3.7817587931139536,
-            "velocityY": -0.0945182065847883,
-            "timestamp": 1.3006644691711875
-        },
-        {
-            "x": 6.1655453259799495,
-            "y": 4.144216905752427,
-            "heading": 3.140000000000087,
-            "angularVelocity": -4.945243197986353e-16,
-            "velocityX": 3.7826641652342365,
-            "velocityY": -0.04566266116445697,
-            "timestamp": 1.331267340858906
-        },
-        {
-            "x": 6.281306292238703,
-            "y": 4.142868414104831,
-            "heading": 3.140000000000087,
-            "angularVelocity": 1.3561359601509856e-16,
-            "velocityX": 3.782683123335836,
-            "velocityY": -0.04406421926917252,
-            "timestamp": 1.3618702125466244
-        },
-        {
-            "x": 6.397067393015407,
-            "y": 4.141531519361414,
-            "heading": 3.140000000000087,
-            "angularVelocity": -2.815905629868065e-16,
-            "velocityX": 3.782687518934728,
-            "velocityY": -0.04368527104047988,
-            "timestamp": 1.392473084234343
-        },
-        {
-            "x": 6.512828617959603,
-            "y": 4.140205420378013,
-            "heading": 3.140000000000087,
-            "angularVelocity": 4.8810263606010396e-17,
-            "velocityX": 3.782691576315187,
-            "velocityY": -0.043332501527048845,
-            "timestamp": 1.4230759559220614
-        },
-        {
-            "x": 6.628589931635543,
-            "y": 4.1388870909429585,
-            "heading": 3.140000000000087,
-            "angularVelocity": -3.260693519621353e-16,
-            "velocityX": 3.7826944757734187,
-            "velocityY": -0.04307861853318547,
-            "timestamp": 1.4536788276097798
-        },
-        {
-            "x": 6.744351296534275,
-            "y": 4.137573267441446,
-            "heading": 3.140000000000087,
-            "angularVelocity": -5.381950425280429e-16,
-            "velocityX": 3.782696149563972,
-            "velocityY": -0.04293137960615145,
-            "timestamp": 1.4842816992974983
-        },
-        {
-            "x": 6.860112695906051,
-            "y": 4.136261713626517,
-            "heading": 3.140000000000087,
-            "angularVelocity": 6.589038089157793e-16,
-            "velocityX": 3.7826972760284443,
-            "velocityY": -0.04285721378647441,
-            "timestamp": 1.5148845709852168
-        },
-        {
-            "x": 6.969430224754608,
-            "y": 4.135029589996691,
-            "heading": 3.1400000000000587,
-            "angularVelocity": -9.14507649037293e-13,
-            "velocityX": 3.5721330326141576,
-            "velocityY": -0.04026169968457299,
-            "timestamp": 1.5454874426729353
-        },
-        {
-            "x": 7.070940187047598,
-            "y": 4.13388425582119,
-            "heading": 3.140000000000041,
-            "angularVelocity": -5.92710722161247e-13,
-            "velocityX": 3.3170077412616483,
-            "velocityY": -0.037425709147242185,
-            "timestamp": 1.5760903143606537
-        },
-        {
-            "x": 7.16464169000894,
-            "y": 4.132826276442613,
-            "heading": 3.1400000000000285,
-            "angularVelocity": -4.0082806789240517e-13,
-            "velocityX": 3.06185327695782,
-            "velocityY": -0.034571245100923735,
-            "timestamp": 1.6066931860483722
-        },
-        {
-            "x": 7.250534738512908,
-            "y": 4.131855992104498,
-            "heading": 3.14000000000002,
-            "angularVelocity": -2.818757978482064e-13,
-            "velocityX": 2.8066989719287347,
-            "velocityY": -0.031705663029917695,
-            "timestamp": 1.6372960577360907
-        },
-        {
-            "x": 7.328619329487215,
-            "y": 4.130973602817362,
-            "heading": 3.140000000000014,
-            "angularVelocity": -2.020529664357193e-13,
-            "velocityX": 2.5515445665075394,
-            "velocityY": -0.028833545281456593,
-            "timestamp": 1.6678989294238091
-        },
-        {
-            "x": 7.398895462248533,
-            "y": 4.130179235854988,
-            "heading": 3.1400000000000095,
-            "angularVelocity": -1.455432238063259e-13,
-            "velocityX": 2.296390138757389,
-            "velocityY": -0.02595726864966503,
-            "timestamp": 1.6985018011115276
-        },
-        {
-            "x": 7.4613631364100685,
-            "y": 4.129472977966136,
-            "heading": 3.1400000000000063,
-            "angularVelocity": -1.0415995882589759e-13,
-            "velocityX": 2.0412356983681157,
-            "velocityY": -0.0230781573571433,
-            "timestamp": 1.729104672799246
-        },
-        {
-            "x": 7.5160223517191325,
-            "y": 4.128854891515294,
-            "heading": 3.1400000000000037,
-            "angularVelocity": -7.336162887662554e-14,
-            "velocityX": 1.7860812497217864,
-            "velocityY": -0.02019700820070875,
-            "timestamp": 1.7597075444869645
-        },
-        {
-            "x": 7.562873107995996,
-            "y": 4.12832502323739,
-            "heading": 3.1400000000000023,
-            "angularVelocity": -5.027990878360554e-14,
-            "velocityX": 1.5309267952025598,
-            "velocityY": -0.017314331902335514,
-            "timestamp": 1.790310416174683
-        },
-        {
-            "x": 7.6019154051050695,
-            "y": 4.127883409326492,
-            "heading": 3.1400000000000015,
-            "angularVelocity": -3.297949891981295e-14,
-            "velocityX": 1.2757723362526898,
-            "velocityY": -0.014430472902237576,
-            "timestamp": 1.8209132878624015
-        },
-        {
-            "x": 7.63314924293966,
-            "y": 4.127530078568267,
-            "heading": 3.1400000000000006,
-            "angularVelocity": -2.0267021915169143e-14,
-            "velocityX": 1.0206178738164648,
-            "velocityY": -0.011545673282216062,
-            "timestamp": 1.85151615955012
-        },
-        {
-            "x": 7.656574621413162,
-            "y": 4.127265054361888,
-            "heading": 3.14,
-            "angularVelocity": -1.117466715568672e-14,
-            "velocityX": 0.7654634085502764,
-            "velocityY": -0.008660109055131773,
-            "timestamp": 1.8821190312378384
-        },
-        {
-            "x": 7.672191540453593,
-            "y": 4.127088356077733,
-            "heading": 3.14,
-            "angularVelocity": -5.057381822634304e-15,
-            "velocityX": 0.5103089409318914,
-            "velocityY": -0.005773911868497247,
-            "timestamp": 1.9127219029255569
+            "heading": -3.475163170309309e-13,
+            "angularVelocity": -3.5317243928660403e-16,
+            "velocityX": 3.5444534056932553,
+            "velocityY": -1.321924985762245,
+            "timestamp": 1.1476501107335806
+        },
+        {
+            "x": 5.5914176787721175,
+            "y": 4.201991970759951,
+            "heading": -3.4752963658489967e-13,
+            "angularVelocity": -4.352387405096259e-16,
+            "velocityX": 3.625488497149835,
+            "velocityY": -1.0800304007696078,
+            "timestamp": 1.1782529824212593
+        },
+        {
+            "x": 5.704342870003183,
+            "y": 4.176490587918632,
+            "heading": -3.4752519346531394e-13,
+            "angularVelocity": 1.4518636145588672e-16,
+            "velocityX": 3.6900194329281493,
+            "velocityY": -0.8333003223399679,
+            "timestamp": 1.208855854108938
+        },
+        {
+            "x": 5.818728492967466,
+            "y": 4.15864825583045,
+            "heading": -3.47528320752536e-13,
+            "angularVelocity": -1.0218933861720526e-16,
+            "velocityX": 3.737741481635173,
+            "velocityY": -0.5830280331479286,
+            "timestamp": 1.2394587257966168
+        },
+        {
+            "x": 5.934052260797228,
+            "y": 4.148506842880417,
+            "heading": -3.475166383708343e-13,
+            "angularVelocity": 3.817413550372978e-16,
+            "velocityX": 3.768396933681978,
+            "velocityY": -0.3313876244719343,
+            "timestamp": 1.2700615974842955
+        },
+        {
+            "x": 6.049784939896788,
+            "y": 4.145614314337883,
+            "heading": -3.4752553093783573e-13,
+            "angularVelocity": -2.9057949488904467e-16,
+            "velocityX": 3.7817587931181613,
+            "velocityY": -0.0945182064161531,
+            "timestamp": 1.3006644691719742
+        },
+        {
+            "x": 6.1655453259831,
+            "y": 4.144216905783092,
+            "heading": -3.4753489515012823e-13,
+            "angularVelocity": -3.0599129336581387e-16,
+            "velocityX": 3.7826641652362696,
+            "velocityY": -0.04566266099543126,
+            "timestamp": 1.331267340859653
+        },
+        {
+            "x": 6.281306292241875,
+            "y": 4.142868414150292,
+            "heading": -3.4754223223995867e-13,
+            "angularVelocity": -2.397516775868154e-16,
+            "velocityX": 3.7826831233414593,
+            "velocityY": -0.04406421878574171,
+            "timestamp": 1.3618702125473316
+        },
+        {
+            "x": 6.39706739301833,
+            "y": 4.141531519398385,
+            "heading": -3.475633687164305e-13,
+            "angularVelocity": -6.906697084916283e-16,
+            "velocityX": 3.7826875189315166,
+            "velocityY": -0.04368527131792852,
+            "timestamp": 1.3924730842350104
+        },
+        {
+            "x": 6.512828617962261,
+            "y": 4.14020542040497,
+            "heading": -3.4758398313992887e-13,
+            "angularVelocity": -6.73610754719036e-16,
+            "velocityX": 3.7826915763114313,
+            "velocityY": -0.043332501854336236,
+            "timestamp": 1.423075955922689
+        },
+        {
+            "x": 6.628589931637748,
+            "y": 4.138887090943481,
+            "heading": -3.4757335612235213e-13,
+            "angularVelocity": 3.472555676974266e-16,
+            "velocityX": 3.7826944757635745,
+            "velocityY": -0.04307861939704374,
+            "timestamp": 1.4536788276103678
+        },
+        {
+            "x": 6.744351296535766,
+            "y": 4.137573267392239,
+            "heading": -3.475819448429605e-13,
+            "angularVelocity": -2.806508055564177e-16,
+            "velocityX": 3.782696149545524,
+            "velocityY": -0.042931381231176444,
+            "timestamp": 1.4842816992980465
+        },
+        {
+            "x": 6.860112695906494,
+            "y": 4.136261713498386,
+            "heading": -3.475979530506806e-13,
+            "angularVelocity": -5.230943272138926e-16,
+            "velocityX": 3.7826972759991557,
+            "velocityY": -0.04285721636551622,
+            "timestamp": 1.5148845709857253
+        },
+        {
+            "x": 6.9694302247553,
+            "y": 4.135029589899985,
+            "heading": -2.392571520797574e-13,
+            "angularVelocity": 3.5402168172320343e-12,
+            "velocityX": 3.5721330326269505,
+            "velocityY": -0.04026169865772549,
+            "timestamp": 1.545487442673404
+        },
+        {
+            "x": 7.070940187048315,
+            "y": 4.133884255746116,
+            "heading": -1.6833543298116243e-13,
+            "angularVelocity": 2.317485846705618e-12,
+            "velocityX": 3.3170077412667367,
+            "velocityY": -0.03742570844047695,
+            "timestamp": 1.5760903143610827
+        },
+        {
+            "x": 7.164641690009617,
+            "y": 4.132826276383854,
+            "heading": -1.1981581485168293e-13,
+            "angularVelocity": 1.5854597325485145e-12,
+            "velocityX": 3.061853276960511,
+            "velocityY": -0.034571244567840914,
+            "timestamp": 1.6066931860487614
+        },
+        {
+            "x": 7.250534738513507,
+            "y": 4.131855992058627,
+            "heading": -8.523066967233492e-14,
+            "angularVelocity": 1.1301275060622065e-12,
+            "velocityX": 2.8066989719298543,
+            "velocityY": -0.03170566260882202,
+            "timestamp": 1.6372960577364402
+        },
+        {
+            "x": 7.328619329487729,
+            "y": 4.1309736027819035,
+            "heading": -6.003549662940244e-14,
+            "angularVelocity": 8.232944437274219e-13,
+            "velocityX": 2.5515445665080714,
+            "velocityY": -0.028833544941282405,
+            "timestamp": 1.6678989294241189
+        },
+        {
+            "x": 7.398895462248961,
+            "y": 4.130179235828021,
+            "heading": -4.153185117983844e-14,
+            "angularVelocity": 6.046375734752488e-13,
+            "velocityX": 2.2963901387575416,
+            "velocityY": -0.02595726837221309,
+            "timestamp": 1.6985018011117976
+        },
+        {
+            "x": 7.461363136410412,
+            "y": 4.129472977946104,
+            "heading": -2.797073854746349e-14,
+            "angularVelocity": 4.431320366500053e-13,
+            "velocityX": 2.041235698368024,
+            "velocityY": -0.0230781571305896,
+            "timestamp": 1.7291046727994763
+        },
+        {
+            "x": 7.516022351719397,
+            "y": 4.1288548915008905,
+            "heading": -1.8141238812912564e-14,
+            "angularVelocity": 3.211953369611228e-13,
+            "velocityX": 1.7860812497215437,
+            "velocityY": -0.02019700801679021,
+            "timestamp": 1.759707544487155
+        },
+        {
+            "x": 7.5628731079961895,
+            "y": 4.128325023227497,
+            "heading": -1.1163529343344889e-14,
+            "angularVelocity": 2.2800831618830555e-13,
+            "velocityX": 1.5309267952022354,
+            "velocityY": -0.017314331754962387,
+            "timestamp": 1.7903104161748338
+        },
+        {
+            "x": 7.601915405105201,
+            "y": 4.127883409320133,
+            "heading": -6.372564950074351e-15,
+            "angularVelocity": 1.5655276164796086e-13,
+            "velocityX": 1.2757723362523365,
+            "velocityY": -0.014430472786738478,
+            "timestamp": 1.8209132878625125
+        },
+        {
+            "x": 7.63314924293974,
+            "y": 4.127530078564582,
+            "heading": -3.2435183420078772e-15,
+            "angularVelocity": 1.0224681844586284e-13,
+            "velocityX": 1.020617873816125,
+            "velocityY": -0.011545673194894595,
+            "timestamp": 1.8515161595501912
+        },
+        {
+            "x": 7.656574621413203,
+            "y": 4.127265054360105,
+            "heading": -1.3581702257498648e-15,
+            "angularVelocity": 6.160689169092944e-14,
+            "velocityX": 0.7654634085499833,
+            "velocityY": -0.008660108992989924,
+            "timestamp": 1.88211903123787
+        },
+        {
+            "x": 7.672191540453606,
+            "y": 4.127088356077157,
+            "heading": -3.7222995775515084e-16,
+            "angularVelocity": 3.2217238765593925e-14,
+            "velocityX": 0.5103089409316736,
+            "velocityY": -0.0057739118290542285,
+            "timestamp": 1.9127219029255487
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": -1.4274059532547784e-15,
-            "velocityX": 0.2551544713218168,
-            "velocityY": -0.002887182568422342,
-            "timestamp": 1.9433247746132754
+            "heading": 2.9643987227812266e-34,
+            "angularVelocity": 1.2163231774130929e-14,
+            "velocityX": 0.25515447132169783,
+            "velocityY": -0.0028871825495910644,
+            "timestamp": 1.9433247746132274
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 3.6920170793948963e-32,
-            "velocityX": 1.993299363399013e-33,
-            "velocityY": 1.7597210552293873e-31,
-            "timestamp": 1.9739276463009938
+            "heading": -1.7511816257564745e-34,
+            "angularVelocity": -1.536305146096975e-32,
+            "velocityX": 2.290424541792184e-34,
+            "velocityY": 7.21466542895957e-33,
+            "timestamp": 1.9739276463009061
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.1.traj
index 135fb8ca..5f6c2964 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.1.traj	
@@ -3,587 +3,587 @@
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
             "velocityX": 0,
             "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 2.254338747446347,
-            "y": 5.559909413116517,
-            "heading": 3.1115254109417156,
-            "angularVelocity": -0.9384273980850008,
-            "velocityX": 0.14299063170637197,
-            "velocityY": -0.0029854412716235888,
-            "timestamp": 0.030342879072361703
-        },
-        {
-            "x": 2.263150716185563,
-            "y": 5.559590842447939,
-            "heading": 3.055209315133754,
-            "angularVelocity": -1.855990516709349,
-            "velocityX": 0.29041307247742426,
-            "velocityY": -0.010499025745671282,
-            "timestamp": 0.060685758144723406
-        },
-        {
-            "x": 2.2766003091950555,
-            "y": 5.558840744374995,
-            "heading": 2.9719321573579647,
-            "angularVelocity": -2.744537114529888,
-            "velocityX": 0.44325368655420766,
-            "velocityY": -0.024720728417179556,
-            "timestamp": 0.09102863721708511
-        },
-        {
-            "x": 2.2949251974733853,
-            "y": 5.557404975825004,
-            "heading": 2.863139887890429,
-            "angularVelocity": -3.5854300182948253,
-            "velocityX": 0.6039271433217751,
-            "velocityY": -0.04731813835356722,
-            "timestamp": 0.12137151628944681
-        },
-        {
-            "x": 2.3185983013070146,
-            "y": 5.55505416569458,
-            "heading": 2.7318619262997976,
-            "angularVelocity": -4.326483366247482,
-            "velocityX": 0.7801864739721567,
-            "velocityY": -0.07747485414348045,
-            "timestamp": 0.15171439536180853
-        },
-        {
-            "x": 2.348656357948328,
-            "y": 5.552070493546886,
-            "heading": 2.5849479304647764,
-            "angularVelocity": -4.841794856864455,
-            "velocityX": 0.9906132035009321,
-            "velocityY": -0.0983318735370822,
-            "timestamp": 0.18205727443417025
-        },
-        {
-            "x": 2.38579846451748,
-            "y": 5.549354804791822,
-            "heading": 2.427729518710298,
-            "angularVelocity": -5.181394006137112,
-            "velocityX": 1.2240798403004254,
-            "velocityY": -0.08950003553016357,
-            "timestamp": 0.21240015350653196
-        },
-        {
-            "x": 2.430432513687245,
-            "y": 5.547202777187511,
-            "heading": 2.264898570795434,
-            "angularVelocity": -5.366364461544502,
-            "velocityX": 1.470989257918541,
-            "velocityY": -0.07092364568235572,
-            "timestamp": 0.24274303257889368
-        },
-        {
-            "x": 2.4823970400851527,
-            "y": 5.546086320842798,
-            "heading": 2.109408164193531,
-            "angularVelocity": -5.12444472494155,
-            "velocityX": 1.7125773158830133,
-            "velocityY": -0.03679467403373135,
-            "timestamp": 0.2730859116512554
-        },
-        {
-            "x": 2.541153647389717,
-            "y": 5.546054192747997,
-            "heading": 1.9668933595072364,
-            "angularVelocity": -4.696812202508044,
-            "velocityX": 1.936421628430232,
-            "velocityY": -0.0010588347507973917,
-            "timestamp": 0.3034287907236171
-        },
-        {
-            "x": 2.60683848805178,
-            "y": 5.546562966972454,
-            "heading": 1.8368003827209543,
-            "angularVelocity": -4.287430222954007,
-            "velocityX": 2.164753071236843,
-            "velocityY": 0.016767499987179104,
-            "timestamp": 0.33377166979597883
-        },
-        {
-            "x": 2.679458734246464,
-            "y": 5.546832906614029,
-            "heading": 1.7192242968736162,
-            "angularVelocity": -3.874915282987571,
-            "velocityX": 2.3933208849924683,
-            "velocityY": 0.008896309441567017,
-            "timestamp": 0.36411454886834055
-        },
-        {
-            "x": 2.7588038922481295,
-            "y": 5.545750285790584,
-            "heading": 1.6147732040322635,
-            "angularVelocity": -3.442359328930443,
-            "velocityX": 2.6149515282463223,
-            "velocityY": -0.035679568206544436,
-            "timestamp": 0.39445742794070227
-        },
-        {
-            "x": 2.844227039552491,
-            "y": 5.54174873240555,
-            "heading": 1.5239846755378514,
-            "angularVelocity": -2.992086818060329,
-            "velocityX": 2.8152617653929393,
-            "velocityY": -0.13187784110699777,
-            "timestamp": 0.424800307013064
-        },
-        {
-            "x": 2.9352842549562403,
-            "y": 5.535535053694752,
-            "heading": 1.4520400757805025,
-            "angularVelocity": -2.371053833941581,
-            "velocityX": 3.0009418416293347,
-            "velocityY": -0.20478210706307357,
-            "timestamp": 0.4551431860854257
-        },
-        {
-            "x": 3.0322214391168845,
-            "y": 5.5275999113119605,
-            "heading": 1.3988891918450945,
-            "angularVelocity": -1.7516756998785012,
-            "velocityX": 3.1947259826421948,
-            "velocityY": -0.26151580289620013,
-            "timestamp": 0.4854860651577874
-        },
-        {
-            "x": 3.1352234766500766,
-            "y": 5.5186341666131185,
-            "heading": 1.36445468986127,
-            "angularVelocity": -1.1348462320172443,
-            "velocityX": 3.394603303383066,
-            "velocityY": -0.2954810147534299,
-            "timestamp": 0.5158289442301491
-        },
-        {
-            "x": 3.2443643044344928,
-            "y": 5.509491432490049,
-            "heading": 1.3485232387156947,
-            "angularVelocity": -0.525047445484061,
-            "velocityX": 3.5969173368201925,
-            "velocityY": -0.3013139953287366,
-            "timestamp": 0.5461718233025108
-        },
-        {
-            "x": 3.358782949449409,
-            "y": 5.500338621504484,
-            "heading": 1.3485196823103536,
-            "angularVelocity": -0.00011720724762443023,
-            "velocityX": 3.7708565736972712,
-            "velocityY": -0.3016460950767756,
-            "timestamp": 0.5765147023748725
-        },
-        {
-            "x": 3.4684760810380606,
-            "y": 5.486343295947389,
-            "heading": 1.335475696910559,
-            "angularVelocity": -0.42988621378634684,
-            "velocityX": 3.615119426441224,
-            "velocityY": -0.4612392094935691,
-            "timestamp": 0.6068575814472341
-        },
-        {
-            "x": 3.573383189765519,
-            "y": 5.467459869277216,
-            "heading": 1.309845724977452,
-            "angularVelocity": -0.8446783138799892,
-            "velocityX": 3.457388090209756,
-            "velocityY": -0.6223347041373206,
-            "timestamp": 0.6372004605195958
-        },
-        {
-            "x": 3.6735704123100494,
-            "y": 5.443371550860819,
-            "heading": 1.272922802934567,
-            "angularVelocity": -1.216856250022656,
-            "velocityX": 3.301836398108563,
-            "velocityY": -0.7938705605012504,
-            "timestamp": 0.6675433395919574
-        },
-        {
-            "x": 3.7692345180845526,
-            "y": 5.4136136865914954,
-            "heading": 1.2267696953368106,
-            "angularVelocity": -1.521052352602741,
-            "velocityX": 3.152769568977396,
-            "velocityY": -0.9807198650582027,
-            "timestamp": 0.6978862186643191
-        },
-        {
-            "x": 3.860739036058752,
-            "y": 5.377644621585128,
-            "heading": 1.1742567314728194,
-            "angularVelocity": -1.730651983905632,
-            "velocityX": 3.0156834411124778,
-            "velocityY": -1.1854203063785596,
-            "timestamp": 0.7282290977366808
-        },
-        {
-            "x": 3.9485394410884225,
-            "y": 5.335005141499111,
-            "heading": 1.1187025848702756,
-            "angularVelocity": -1.830879214528657,
-            "velocityX": 2.893608243973284,
-            "velocityY": -1.4052549194270891,
-            "timestamp": 0.7585719768090424
+            "x": 2.253608767824088,
+            "y": 5.559959646221142,
+            "heading": 0.04333845773860565,
+            "angularVelocity": 1.2657316926377489,
+            "velocityX": 0.10539673178657698,
+            "velocityY": -0.0011785619397610312,
+            "timestamp": 0.03423984560921402
+        },
+        {
+            "x": 2.260842860117559,
+            "y": 5.560038303784881,
+            "heading": 0.12991520296053788,
+            "angularVelocity": 2.5285378389274697,
+            "velocityX": 0.211277012637124,
+            "velocityY": 0.002297252290136546,
+            "timestamp": 0.06847969121842803
+        },
+        {
+            "x": 2.271629728646886,
+            "y": 5.560441749821537,
+            "heading": 0.2597959154554935,
+            "angularVelocity": 3.7932622120236568,
+            "velocityX": 0.3150384687021929,
+            "velocityY": 0.011782939714764799,
+            "timestamp": 0.10271953682764205
+        },
+        {
+            "x": 2.285721166472015,
+            "y": 5.561272024294721,
+            "heading": 0.433416913958427,
+            "angularVelocity": 5.070729596286839,
+            "velocityX": 0.41155085761651194,
+            "velocityY": 0.024248779701277118,
+            "timestamp": 0.13695938243685607
+        },
+        {
+            "x": 2.302746549134424,
+            "y": 5.561830584813036,
+            "heading": 0.6513804515506975,
+            "angularVelocity": 6.36578622695705,
+            "velocityX": 0.497238885266109,
+            "velocityY": 0.01631317280720512,
+            "timestamp": 0.17119922804607007
+        },
+        {
+            "x": 2.325552284167229,
+            "y": 5.559809655953846,
+            "heading": 0.9008347442106217,
+            "angularVelocity": 7.285497005652254,
+            "velocityX": 0.6660583488924382,
+            "velocityY": -0.05902272113767896,
+            "timestamp": 0.20543907365528408
+        },
+        {
+            "x": 2.357593597831257,
+            "y": 5.5547755319598515,
+            "heading": 1.1463891087836293,
+            "angularVelocity": 7.171596723173552,
+            "velocityX": 0.9357902494573596,
+            "velocityY": -0.14702531230571617,
+            "timestamp": 0.23967891926449808
+        },
+        {
+            "x": 2.398826095012215,
+            "y": 5.54977146589545,
+            "heading": 1.3795454342192353,
+            "angularVelocity": 6.809502825937482,
+            "velocityX": 1.2042255578939407,
+            "velocityY": -0.14614744825411982,
+            "timestamp": 0.2739187648737121
+        },
+        {
+            "x": 2.4467424620473803,
+            "y": 5.551292722520974,
+            "heading": 1.6025344347532522,
+            "angularVelocity": 6.512558586829906,
+            "velocityX": 1.399432917485785,
+            "velocityY": 0.04442942421196648,
+            "timestamp": 0.3081586104829261
+        },
+        {
+            "x": 2.499282041125752,
+            "y": 5.560003512593039,
+            "heading": 1.8091707816342142,
+            "angularVelocity": 6.034967249541443,
+            "velocityX": 1.5344572425359657,
+            "velocityY": 0.2544050627880495,
+            "timestamp": 0.3423984560921401
+        },
+        {
+            "x": 2.558927262082263,
+            "y": 5.571143049666431,
+            "heading": 1.9899644077603311,
+            "angularVelocity": 5.2802114877955235,
+            "velocityX": 1.7419827658469296,
+            "velocityY": 0.3253384142127747,
+            "timestamp": 0.3766383017013541
+        },
+        {
+            "x": 2.624771333399717,
+            "y": 5.5772713811756995,
+            "heading": 2.148730759711285,
+            "angularVelocity": 4.63688866366352,
+            "velocityX": 1.9230247726273466,
+            "velocityY": 0.17898245159199333,
+            "timestamp": 0.4108781473105681
+        },
+        {
+            "x": 2.6962684360037885,
+            "y": 5.576686266483743,
+            "heading": 2.2908744479705683,
+            "angularVelocity": 4.151411483614645,
+            "velocityX": 2.0881257298902987,
+            "velocityY": -0.0170887070763856,
+            "timestamp": 0.4451179929197821
+        },
+        {
+            "x": 2.773687793639176,
+            "y": 5.569334398339137,
+            "heading": 2.4184902740666434,
+            "angularVelocity": 3.727114530613817,
+            "velocityX": 2.2610895656186547,
+            "velocityY": -0.2147167434256298,
+            "timestamp": 0.4793578385289961
+        },
+        {
+            "x": 2.8582271385194487,
+            "y": 5.556743437974915,
+            "heading": 2.5300657438997165,
+            "angularVelocity": 3.258644069441941,
+            "velocityX": 2.4690340559690878,
+            "velocityY": -0.36772830426646747,
+            "timestamp": 0.5135976841382102
+        },
+        {
+            "x": 2.9508927177061977,
+            "y": 5.541492370817128,
+            "heading": 2.6233336392816216,
+            "angularVelocity": 2.7239578252305745,
+            "velocityX": 2.706366735538439,
+            "velocityY": -0.445418689437171,
+            "timestamp": 0.5478375297474242
+        },
+        {
+            "x": 3.0518533392296137,
+            "y": 5.525810656425357,
+            "heading": 2.697205719709221,
+            "angularVelocity": 2.1574887127329996,
+            "velocityX": 2.948629578406965,
+            "velocityY": -0.45799606022611716,
+            "timestamp": 0.5820773753566382
+        },
+        {
+            "x": 3.1608544462331407,
+            "y": 5.511189976920693,
+            "heading": 2.751249379299885,
+            "angularVelocity": 1.5783850256649679,
+            "velocityX": 3.183457900119567,
+            "velocityY": -0.42700775206563796,
+            "timestamp": 0.6163172209658522
+        },
+        {
+            "x": 3.2775207255393237,
+            "y": 5.498694402974223,
+            "heading": 2.785399702937351,
+            "angularVelocity": 0.9973854446433661,
+            "velocityX": 3.40732492306531,
+            "velocityY": -0.36494247342945346,
+            "timestamp": 0.6505570665750662
+        },
+        {
+            "x": 3.401135401206032,
+            "y": 5.488837187261409,
+            "heading": 2.800896234569334,
+            "angularVelocity": 0.4525876608454414,
+            "velocityX": 3.610257974803583,
+            "velocityY": -0.28788727102673717,
+            "timestamp": 0.6847969121842802
+        },
+        {
+            "x": 3.5179017921083777,
+            "y": 5.474779629986763,
+            "heading": 2.835487088185811,
+            "angularVelocity": 1.0102514483058458,
+            "velocityX": 3.410248756230483,
+            "velocityY": -0.4105613510962388,
+            "timestamp": 0.7190367577934942
+        },
+        {
+            "x": 3.6287098119916026,
+            "y": 5.4546983370085105,
+            "heading": 2.8876881760682602,
+            "angularVelocity": 1.5245713569573334,
+            "velocityX": 3.2362301263825226,
+            "velocityY": -0.5864890048700768,
+            "timestamp": 0.7532766034027082
+        },
+        {
+            "x": 3.7356372636451365,
+            "y": 5.425785607175325,
+            "heading": 2.9455736884009496,
+            "angularVelocity": 1.6905891747687094,
+            "velocityX": 3.1228952628442794,
+            "velocityY": -0.8444176461299453,
+            "timestamp": 0.7875164490119222
+        },
+        {
+            "x": 3.8388362586877394,
+            "y": 5.387864157402447,
+            "heading": 3.0009390226743027,
+            "angularVelocity": 1.6169855117119507,
+            "velocityX": 3.0140029315678847,
+            "velocityY": -1.1075239709221774,
+            "timestamp": 0.8217562946211362
+        },
+        {
+            "x": 3.9379980862804507,
+            "y": 5.341070528950818,
+            "heading": 3.053673275383994,
+            "angularVelocity": 1.5401428298350917,
+            "velocityX": 2.896094472050628,
+            "velocityY": -1.3666425072616961,
+            "timestamp": 0.8559961402303502
         },
         {
             "x": 4.032804489135742,
             "y": 5.285560607910156,
-            "heading": 1.064415461383156,
-            "angularVelocity": -1.7891223623722634,
-            "velocityX": 2.7770946799861798,
-            "velocityY": -1.6295267654410455,
-            "timestamp": 0.7889148558814041
-        },
-        {
-            "x": 4.1733297943826075,
-            "y": 5.174158598481438,
-            "heading": 0.9768931525481981,
-            "angularVelocity": -1.5934853179105337,
-            "velocityX": 2.55849067153879,
-            "velocityY": -2.0282539248952025,
-            "timestamp": 0.8438399360507789
-        },
-        {
-            "x": 4.303490854924698,
-            "y": 5.0407757547823575,
-            "heading": 0.9131112872795231,
-            "angularVelocity": -1.1612521105474607,
-            "velocityX": 2.369792818521288,
-            "velocityY": -2.428450596480911,
-            "timestamp": 0.8987650162201537
-        },
-        {
-            "x": 4.424699657845323,
-            "y": 4.885599604328108,
-            "heading": 0.881895788498386,
-            "angularVelocity": -0.5683286885495016,
-            "velocityX": 2.206802476152012,
-            "velocityY": -2.8252330260734433,
-            "timestamp": 0.9536900963895285
-        },
-        {
-            "x": 4.534874604263042,
-            "y": 4.709437065112464,
-            "heading": 0.8818953110956026,
-            "angularVelocity": -0.000008691890515565163,
-            "velocityX": 2.005913256347883,
-            "velocityY": -3.2073242073093726,
-            "timestamp": 1.0086151765589033
-        },
-        {
-            "x": 4.656392865207935,
-            "y": 4.550190331352248,
-            "heading": 0.8602662015486029,
-            "angularVelocity": -0.3937929536070082,
-            "velocityX": 2.2124366604502335,
-            "velocityY": -2.899344584826112,
-            "timestamp": 1.0635402567282781
-        },
-        {
-            "x": 4.786084989350276,
-            "y": 4.412405730848589,
-            "heading": 0.8009192530896463,
-            "angularVelocity": -1.0805072705573824,
-            "velocityX": 2.3612550722257284,
-            "velocityY": -2.508591704896303,
-            "timestamp": 1.118465336897653
-        },
-        {
-            "x": 4.924876209285355,
-            "y": 4.297327162575611,
-            "heading": 0.7205421769770328,
-            "angularVelocity": -1.463394789133692,
-            "velocityX": 2.5269188412121024,
-            "velocityY": -2.0951916304556226,
-            "timestamp": 1.1733904170670277
-        },
-        {
-            "x": 5.072286801869267,
-            "y": 4.205735025902594,
-            "heading": 0.6308477205816693,
-            "angularVelocity": -1.6330327806307947,
-            "velocityX": 2.6838484737634416,
-            "velocityY": -1.6675831221469524,
-            "timestamp": 1.2283154972364025
-        },
-        {
-            "x": 5.227765920434763,
-            "y": 4.1379313687941846,
-            "heading": 0.5364496515071664,
-            "angularVelocity": -1.7186696639022372,
-            "velocityX": 2.830749050998892,
-            "velocityY": -1.2344753416712253,
-            "timestamp": 1.2832405774057773
+            "heading": 3.1036105469083837,
+            "angularVelocity": 1.4584549268806075,
+            "velocityX": 2.76889107320562,
+            "velocityY": -1.62120827512502,
+            "timestamp": 0.8902359858395642
+        },
+        {
+            "x": 4.173725003261308,
+            "y": 5.173637687034127,
+            "heading": 3.175484729095798,
+            "angularVelocity": 1.2994086167535415,
+            "velocityX": 2.547692714675012,
+            "velocityY": -2.023447131813128,
+            "timestamp": 0.9455489807364162
+        },
+        {
+            "x": 4.301872286760783,
+            "y": 5.039921697201636,
+            "heading": 3.2346580028937466,
+            "angularVelocity": 1.0697897285853943,
+            "velocityX": 2.3167663175433564,
+            "velocityY": -2.4174425934058767,
+            "timestamp": 1.0008619756332682
+        },
+        {
+            "x": 4.416442024233276,
+            "y": 4.885133369968262,
+            "heading": 3.2770092965554323,
+            "angularVelocity": 0.765666255111711,
+            "velocityX": 2.0712987551323803,
+            "velocityY": -2.7984079965661373,
+            "timestamp": 1.0561749705301202
+        },
+        {
+            "x": 4.517908742039174,
+            "y": 4.711097539386313,
+            "heading": 3.282986865137276,
+            "angularVelocity": 0.10806806959180061,
+            "velocityX": 1.834410123608644,
+            "velocityY": -3.146382344808711,
+            "timestamp": 1.1114879654269723
+        },
+        {
+            "x": 4.627451956204509,
+            "y": 4.5560902871099245,
+            "heading": 3.2338916761611958,
+            "angularVelocity": -0.8875886953442504,
+            "velocityX": 1.9804245705663106,
+            "velocityY": -2.802365928032756,
+            "timestamp": 1.1668009603238243
+        },
+        {
+            "x": 4.751713919325069,
+            "y": 4.421499213962681,
+            "heading": 3.169974564154514,
+            "angularVelocity": -1.1555532678328913,
+            "velocityX": 2.246523865726009,
+            "velocityY": -2.433263167149585,
+            "timestamp": 1.2221139552206763
+        },
+        {
+            "x": 4.891049798654671,
+            "y": 4.307366204928411,
+            "heading": 3.098048256171184,
+            "angularVelocity": -1.3003509955925967,
+            "velocityX": 2.5190442063286667,
+            "velocityY": -2.063403170396147,
+            "timestamp": 1.2774269501175284
+        },
+        {
+            "x": 5.0446094980944505,
+            "y": 4.214363163694223,
+            "heading": 3.021068837736617,
+            "angularVelocity": -1.3917058473893738,
+            "velocityX": 2.7761957154216415,
+            "velocityY": -1.6813958710357464,
+            "timestamp": 1.3327399450143804
+        },
+        {
+            "x": 5.211468562298102,
+            "y": 4.143120621239898,
+            "heading": 2.9418200950482523,
+            "angularVelocity": -1.432732811451417,
+            "velocityX": 3.016634057056783,
+            "velocityY": -1.2879892435254758,
+            "timestamp": 1.3880529399112325
         },
         {
             "x": 5.390789031982422,
             "y": 4.09412145614624,
-            "heading": 0.44081122980213755,
-            "angularVelocity": -1.7412522914869581,
-            "velocityX": 2.968099655839153,
-            "velocityY": -0.7976303814732078,
-            "timestamp": 1.3381656575751522
-        },
-        {
-            "x": 5.527208209694737,
-            "y": 4.074474240030864,
-            "heading": 0.3646632920053755,
-            "angularVelocity": -1.7150127332494942,
-            "velocityX": 3.0724486257327652,
-            "velocityY": -0.44249689204593257,
-            "timestamp": 1.3825664578260528
-        },
-        {
-            "x": 5.669753286288942,
-            "y": 4.0699039581909835,
-            "heading": 0.2967653403322419,
-            "angularVelocity": -1.529205583896121,
-            "velocityX": 3.2104168345775173,
-            "velocityY": -0.10293242045310311,
-            "timestamp": 1.4269672580769535
-        },
-        {
-            "x": 5.825688318788907,
-            "y": 4.069335183916362,
-            "heading": 0.2632559067133074,
-            "angularVelocity": -0.7547033708757247,
-            "velocityX": 3.5119869826400643,
-            "velocityY": -0.012810000527210512,
-            "timestamp": 1.4713680583278541
-        },
-        {
-            "x": 5.986467377363025,
-            "y": 4.053794613625281,
-            "heading": 0.24734267435752091,
-            "angularVelocity": -0.35839967446225596,
-            "velocityX": 3.6210847026536497,
-            "velocityY": -0.3500065359917586,
-            "timestamp": 1.5157688585787548
-        },
-        {
-            "x": 6.146135354603106,
-            "y": 4.023716272835588,
-            "heading": 0.25998224016695326,
-            "angularVelocity": 0.28466977482406836,
-            "velocityX": 3.5960607993060463,
-            "velocityY": -0.6774278981398296,
-            "timestamp": 1.5601696588296554
-        },
-        {
-            "x": 6.299693722526358,
-            "y": 3.978509002442476,
-            "heading": 0.279745523920822,
-            "angularVelocity": 0.4451109809325518,
-            "velocityX": 3.4584594659447885,
-            "velocityY": -1.0181634145703302,
-            "timestamp": 1.604570459080556
-        },
-        {
-            "x": 6.446348354717166,
-            "y": 3.918416502871273,
-            "heading": 0.30303477341760615,
-            "angularVelocity": 0.5245231924915958,
-            "velocityX": 3.3029727248628453,
-            "velocityY": -1.3534102816082356,
-            "timestamp": 1.6489712593314567
-        },
-        {
-            "x": 6.585501043822193,
-            "y": 3.8437018977675175,
-            "heading": 0.32757979886137184,
-            "angularVelocity": 0.5528059247821282,
-            "velocityX": 3.134013088023211,
-            "velocityY": -1.6827310472234156,
-            "timestamp": 1.6933720595823574
-        },
-        {
-            "x": 6.716485001612935,
-            "y": 3.754724015839551,
-            "heading": 0.3514161297676369,
-            "angularVelocity": 0.5368446237808859,
-            "velocityX": 2.950035968959475,
-            "velocityY": -2.0039702308329863,
-            "timestamp": 1.737772859833258
-        },
-        {
-            "x": 6.838464987128356,
-            "y": 3.6520072183603935,
-            "heading": 0.37305697715168934,
-            "angularVelocity": 0.48739768791922733,
-            "velocityX": 2.747247455589403,
-            "velocityY": -2.313399688715609,
-            "timestamp": 1.7821736600841587
-        },
-        {
-            "x": 6.950225204946805,
-            "y": 3.536435013339574,
-            "heading": 0.39217088238591646,
-            "angularVelocity": 0.430485602201265,
-            "velocityX": 2.5170766559817053,
-            "velocityY": -2.60293067619823,
-            "timestamp": 1.8265744603350593
-        },
-        {
-            "x": 7.049675913816151,
-            "y": 3.409971379240331,
-            "heading": 0.4121286773514249,
-            "angularVelocity": 0.44949178511942617,
-            "velocityX": 2.23984046024776,
-            "velocityY": -2.848228711748902,
-            "timestamp": 1.87097526058596
-        },
-        {
-            "x": 7.134078905097545,
-            "y": 3.2784202019721485,
-            "heading": 0.44760621801548245,
-            "angularVelocity": 0.799029307210242,
-            "velocityX": 1.9009340102981855,
-            "velocityY": -2.9628109521632653,
-            "timestamp": 1.9153760608368606
-        },
-        {
-            "x": 7.206535248844668,
-            "y": 3.1515334712387078,
-            "heading": 0.5188452438191252,
-            "angularVelocity": 1.6044536450037898,
-            "velocityX": 1.6318702216556016,
-            "velocityY": -2.857757743473708,
-            "timestamp": 1.9597768610877613
-        },
-        {
-            "x": 7.286286308378336,
-            "y": 3.038022763329368,
-            "heading": 0.612459613040285,
-            "angularVelocity": 2.1083937382245868,
-            "velocityX": 1.796162661100898,
-            "velocityY": -2.556501397900756,
-            "timestamp": 2.004177661338662
-        },
-        {
-            "x": 7.36964311440034,
-            "y": 2.9389690479632176,
-            "heading": 0.7307138885874557,
-            "angularVelocity": 2.6633365812989322,
-            "velocityX": 1.8773717039100728,
-            "velocityY": -2.2308993262827714,
-            "timestamp": 2.0485784615895626
-        },
-        {
-            "x": 7.443994240643498,
-            "y": 2.8513361754602453,
-            "heading": 0.8783950719218284,
-            "angularVelocity": 3.326092829405186,
-            "velocityX": 1.674544733946552,
-            "velocityY": -1.9736777717467908,
-            "timestamp": 2.0929792618404632
-        },
-        {
-            "x": 7.506362936213548,
-            "y": 2.772668193478103,
-            "heading": 1.052033337356815,
-            "angularVelocity": 3.9107012588464345,
-            "velocityX": 1.4046750332790265,
-            "velocityY": -1.7717694622079865,
-            "timestamp": 2.137380062091364
-        },
-        {
-            "x": 7.555798075190431,
-            "y": 2.7014303099088472,
-            "heading": 1.2519057769914284,
-            "angularVelocity": 4.501550388848195,
-            "velocityX": 1.1133839637469243,
-            "velocityY": -1.6044279194677442,
-            "timestamp": 2.1817808623422645
-        },
-        {
-            "x": 7.592358210662926,
-            "y": 2.6364422153707756,
-            "heading": 1.482218675635827,
-            "angularVelocity": 5.187133955760781,
-            "velocityX": 0.8234116337070656,
-            "velocityY": -1.4636694422360859,
-            "timestamp": 2.226181662593165
-        },
-        {
-            "x": 7.621254126448735,
-            "y": 2.5813646495023557,
-            "heading": 1.7524007538852981,
-            "angularVelocity": 6.085072267227659,
-            "velocityX": 0.6507971843417887,
-            "velocityY": -1.2404633600562773,
-            "timestamp": 2.270582462844066
-        },
-        {
-            "x": 7.645052051647252,
-            "y": 2.5375067096061756,
-            "heading": 2.067951038154778,
-            "angularVelocity": 7.10686029275066,
-            "velocityX": 0.5359796459532341,
-            "velocityY": -0.9877736357981667,
-            "timestamp": 2.3149832630949665
-        },
-        {
-            "x": 7.661427436541854,
-            "y": 2.5075370043722347,
-            "heading": 2.400674072151774,
-            "angularVelocity": 7.493626964307851,
-            "velocityX": 0.36880832782444795,
-            "velocityY": -0.6749811954871879,
-            "timestamp": 2.359384063345867
-        },
-        {
-            "x": 7.6692908711764884,
-            "y": 2.4875264930527843,
-            "heading": 2.6956822674348517,
-            "angularVelocity": 6.644208969569037,
-            "velocityX": 0.17710119164969557,
-            "velocityY": -0.4506790689891867,
-            "timestamp": 2.403784863596768
-        },
-        {
-            "x": 7.674629616295112,
-            "y": 2.472122256604734,
-            "heading": 2.9172973649743583,
-            "angularVelocity": 4.991241065188027,
-            "velocityX": 0.12023984001312674,
-            "velocityY": -0.3469360092836942,
-            "timestamp": 2.4481856638476684
-        },
-        {
-            "x": 7.678206340030146,
-            "y": 2.4619495353215974,
-            "heading": 3.0655099943755024,
-            "angularVelocity": 3.3380621196830216,
-            "velocityX": 0.0805553889754595,
-            "velocityY": -0.22911121479010477,
-            "timestamp": 2.492586464098569
+            "heading": 2.865640793449113,
+            "angularVelocity": -1.3772405876991196,
+            "velocityX": 3.2419229878750393,
+            "velocityY": -0.8858526858838836,
+            "timestamp": 1.4433659348080845
+        },
+        {
+            "x": 5.535592658236132,
+            "y": 4.049337172847784,
+            "heading": 2.841642220937184,
+            "angularVelocity": -0.5722130304709164,
+            "velocityX": 3.452643767066762,
+            "velocityY": -1.0678197818199107,
+            "timestamp": 1.4853058614332775
+        },
+        {
+            "x": 5.681734895850587,
+            "y": 4.003264514213434,
+            "heading": 2.8233979117656802,
+            "angularVelocity": -0.4350105171749157,
+            "velocityX": 3.484561118108144,
+            "velocityY": -1.098539323783022,
+            "timestamp": 1.5272457880584704
+        },
+        {
+            "x": 5.827742551539107,
+            "y": 3.9572785916007636,
+            "heading": 2.804954082691072,
+            "angularVelocity": -0.4397677954812897,
+            "velocityX": 3.481352196758831,
+            "velocityY": -1.096471222366128,
+            "timestamp": 1.5691857146836634
+        },
+        {
+            "x": 5.9679573209760495,
+            "y": 3.908501280249766,
+            "heading": 2.7717324952229117,
+            "angularVelocity": -0.7921231661908814,
+            "velocityX": 3.3432287731451744,
+            "velocityY": -1.1630280564605782,
+            "timestamp": 1.6111256413088564
+        },
+        {
+            "x": 6.096940014064459,
+            "y": 3.8543767396001813,
+            "heading": 2.711734513417447,
+            "angularVelocity": -1.4305695463335533,
+            "velocityX": 3.07541532537948,
+            "velocityY": -1.2905254015650245,
+            "timestamp": 1.6530655679340494
+        },
+        {
+            "x": 6.215088187160598,
+            "y": 3.7918967611554555,
+            "heading": 2.632459037184743,
+            "angularVelocity": -1.890214948184595,
+            "velocityX": 2.8170810633981405,
+            "velocityY": -1.4897493503766595,
+            "timestamp": 1.6950054945592423
+        },
+        {
+            "x": 6.32424952389229,
+            "y": 3.717983083464812,
+            "heading": 2.5463998013121922,
+            "angularVelocity": -2.0519643880553406,
+            "velocityX": 2.6028022821127137,
+            "velocityY": -1.7623702194615716,
+            "timestamp": 1.7369454211844353
+        },
+        {
+            "x": 6.43630340548619,
+            "y": 3.6322698094657313,
+            "heading": 2.4886072976482807,
+            "angularVelocity": -1.3779829464268867,
+            "velocityX": 2.6717710451737124,
+            "velocityY": -2.043715401914737,
+            "timestamp": 1.7788853478096283
+        },
+        {
+            "x": 6.552059359988985,
+            "y": 3.5378523447078845,
+            "heading": 2.4573864362362134,
+            "angularVelocity": -0.7444185987992016,
+            "velocityX": 2.7600418936656252,
+            "velocityY": -2.251254886581771,
+            "timestamp": 1.8208252744348212
+        },
+        {
+            "x": 6.67032629558411,
+            "y": 3.432112823682954,
+            "heading": 2.4573457680499984,
+            "angularVelocity": -0.0009696770950155036,
+            "velocityX": 2.8199127922194083,
+            "velocityY": -2.521213782033988,
+            "timestamp": 1.8627652010600142
+        },
+        {
+            "x": 6.787202921818474,
+            "y": 3.3248215481814336,
+            "heading": 2.4573432508654287,
+            "angularVelocity": -0.00006001881195361165,
+            "velocityX": 2.7867627733081557,
+            "velocityY": -2.558213238195593,
+            "timestamp": 1.9047051276852072
+        },
+        {
+            "x": 6.904079764497574,
+            "y": 3.217530508972575,
+            "heading": 2.4573407325391345,
+            "angularVelocity": -0.000060046034804859665,
+            "velocityX": 2.7867679341358973,
+            "velocityY": -2.5582076041213115,
+            "timestamp": 1.9466450543104001
+        },
+        {
+            "x": 7.017539426879104,
+            "y": 3.1193532157248125,
+            "heading": 2.428286840001792,
+            "angularVelocity": -0.6927501995172902,
+            "velocityX": 2.7052899590285975,
+            "velocityY": -2.3409028376503684,
+            "timestamp": 1.988584980935593
+        },
+        {
+            "x": 7.118887938132189,
+            "y": 3.020968055873212,
+            "heading": 2.3710302150679574,
+            "angularVelocity": -1.365205653446252,
+            "velocityX": 2.4165161794108876,
+            "velocityY": -2.345859131582302,
+            "timestamp": 2.030524907560786
+        },
+        {
+            "x": 7.207551237715759,
+            "y": 2.9223074295194613,
+            "heading": 2.2882909026899103,
+            "angularVelocity": -1.9728053679604207,
+            "velocityX": 2.1140547139229047,
+            "velocityY": -2.3524272523282552,
+            "timestamp": 2.0724648341859786
+        },
+        {
+            "x": 7.28673413242529,
+            "y": 2.8318719270188626,
+            "heading": 2.1761461762136682,
+            "angularVelocity": -2.6739371167349164,
+            "velocityX": 1.8880074688058086,
+            "velocityY": -2.1563104606452623,
+            "timestamp": 2.1144047608111713
+        },
+        {
+            "x": 7.3579924776067,
+            "y": 2.7517426977667294,
+            "heading": 2.0378729016459984,
+            "angularVelocity": -3.2969364921256217,
+            "velocityX": 1.6990574594521515,
+            "velocityY": -1.9105715174046114,
+            "timestamp": 2.156344687436364
+        },
+        {
+            "x": 7.4224137082831785,
+            "y": 2.6828558926645387,
+            "heading": 1.874977709266495,
+            "angularVelocity": -3.884012335912244,
+            "velocityX": 1.5360358460374843,
+            "velocityY": -1.6425113405136766,
+            "timestamp": 2.198284614061557
+        },
+        {
+            "x": 7.480883603746462,
+            "y": 2.6255140291502124,
+            "heading": 1.6865689006450588,
+            "angularVelocity": -4.4923495051672475,
+            "velocityX": 1.3941344243592637,
+            "velocityY": -1.3672380504328716,
+            "timestamp": 2.2402245406867496
+        },
+        {
+            "x": 7.534437188323511,
+            "y": 2.579534136686106,
+            "heading": 1.4695142507662173,
+            "angularVelocity": -5.175370281846384,
+            "velocityX": 1.27691173748694,
+            "velocityY": -1.0963274417482318,
+            "timestamp": 2.2821644673119423
+        },
+        {
+            "x": 7.579534895925645,
+            "y": 2.5413039154996566,
+            "heading": 1.2172257697546927,
+            "angularVelocity": -6.015472636997345,
+            "velocityX": 1.0752929542572058,
+            "velocityY": -0.9115471643072651,
+            "timestamp": 2.324104393937135
+        },
+        {
+            "x": 7.615712715030757,
+            "y": 2.5097826728946813,
+            "heading": 0.9253708991340255,
+            "angularVelocity": -6.958878903840344,
+            "velocityX": 0.8626104530039771,
+            "velocityY": -0.7515807761580874,
+            "timestamp": 2.366044320562328
+        },
+        {
+            "x": 7.6401514150011085,
+            "y": 2.4865499215178333,
+            "heading": 0.6440959436378817,
+            "angularVelocity": -6.706615345559152,
+            "velocityX": 0.5827072657697844,
+            "velocityY": -0.5539530763721487,
+            "timestamp": 2.4079842471875206
+        },
+        {
+            "x": 7.656070937434432,
+            "y": 2.471863188851966,
+            "heading": 0.3945229348796906,
+            "angularVelocity": -5.950725927314207,
+            "velocityX": 0.37957916749813,
+            "velocityY": -0.3501849871393242,
+            "timestamp": 2.4499241738127133
+        },
+        {
+            "x": 7.668072859318959,
+            "y": 2.4642781587251137,
+            "heading": 0.19785094472919199,
+            "angularVelocity": -4.689373729909185,
+            "velocityX": 0.28616935818189254,
+            "velocityY": -0.18085463512222474,
+            "timestamp": 2.491864100437906
+        },
+        {
+            "x": 7.676136280306529,
+            "y": 2.459442710773055,
+            "heading": 0.06623696286278732,
+            "angularVelocity": -3.1381547956105695,
+            "velocityX": 0.19226120874341576,
+            "velocityY": -0.11529462116784107,
+            "timestamp": 2.533804027063099
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 1.6776725915652129,
-            "velocityX": 0.04039701896629419,
-            "velocityY": -0.11147401158601906,
-            "timestamp": 2.5369872643494698
+            "heading": 2.1057025676027217e-33,
+            "angularVelocity": -1.57932948845455,
+            "velocityX": 0.09212509425682942,
+            "velocityY": -0.058243086471875524,
+            "timestamp": 2.5757439536882916
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -5.831834401931518e-35,
-            "velocityX": -1.3540056730700436e-35,
-            "velocityY": -1.938442767179852e-35,
-            "timestamp": 2.5813880646003704
+            "heading": -7.524501141381425e-34,
+            "angularVelocity": -5.907340858361994e-32,
+            "velocityX": -7.634877430876997e-33,
+            "velocityY": -7.90956322394541e-33,
+            "timestamp": 2.6176838803134843
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.traj
index 135fb8ca..5f6c2964 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.traj	
@@ -3,587 +3,587 @@
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
             "velocityX": 0,
             "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 2.254338747446347,
-            "y": 5.559909413116517,
-            "heading": 3.1115254109417156,
-            "angularVelocity": -0.9384273980850008,
-            "velocityX": 0.14299063170637197,
-            "velocityY": -0.0029854412716235888,
-            "timestamp": 0.030342879072361703
-        },
-        {
-            "x": 2.263150716185563,
-            "y": 5.559590842447939,
-            "heading": 3.055209315133754,
-            "angularVelocity": -1.855990516709349,
-            "velocityX": 0.29041307247742426,
-            "velocityY": -0.010499025745671282,
-            "timestamp": 0.060685758144723406
-        },
-        {
-            "x": 2.2766003091950555,
-            "y": 5.558840744374995,
-            "heading": 2.9719321573579647,
-            "angularVelocity": -2.744537114529888,
-            "velocityX": 0.44325368655420766,
-            "velocityY": -0.024720728417179556,
-            "timestamp": 0.09102863721708511
-        },
-        {
-            "x": 2.2949251974733853,
-            "y": 5.557404975825004,
-            "heading": 2.863139887890429,
-            "angularVelocity": -3.5854300182948253,
-            "velocityX": 0.6039271433217751,
-            "velocityY": -0.04731813835356722,
-            "timestamp": 0.12137151628944681
-        },
-        {
-            "x": 2.3185983013070146,
-            "y": 5.55505416569458,
-            "heading": 2.7318619262997976,
-            "angularVelocity": -4.326483366247482,
-            "velocityX": 0.7801864739721567,
-            "velocityY": -0.07747485414348045,
-            "timestamp": 0.15171439536180853
-        },
-        {
-            "x": 2.348656357948328,
-            "y": 5.552070493546886,
-            "heading": 2.5849479304647764,
-            "angularVelocity": -4.841794856864455,
-            "velocityX": 0.9906132035009321,
-            "velocityY": -0.0983318735370822,
-            "timestamp": 0.18205727443417025
-        },
-        {
-            "x": 2.38579846451748,
-            "y": 5.549354804791822,
-            "heading": 2.427729518710298,
-            "angularVelocity": -5.181394006137112,
-            "velocityX": 1.2240798403004254,
-            "velocityY": -0.08950003553016357,
-            "timestamp": 0.21240015350653196
-        },
-        {
-            "x": 2.430432513687245,
-            "y": 5.547202777187511,
-            "heading": 2.264898570795434,
-            "angularVelocity": -5.366364461544502,
-            "velocityX": 1.470989257918541,
-            "velocityY": -0.07092364568235572,
-            "timestamp": 0.24274303257889368
-        },
-        {
-            "x": 2.4823970400851527,
-            "y": 5.546086320842798,
-            "heading": 2.109408164193531,
-            "angularVelocity": -5.12444472494155,
-            "velocityX": 1.7125773158830133,
-            "velocityY": -0.03679467403373135,
-            "timestamp": 0.2730859116512554
-        },
-        {
-            "x": 2.541153647389717,
-            "y": 5.546054192747997,
-            "heading": 1.9668933595072364,
-            "angularVelocity": -4.696812202508044,
-            "velocityX": 1.936421628430232,
-            "velocityY": -0.0010588347507973917,
-            "timestamp": 0.3034287907236171
-        },
-        {
-            "x": 2.60683848805178,
-            "y": 5.546562966972454,
-            "heading": 1.8368003827209543,
-            "angularVelocity": -4.287430222954007,
-            "velocityX": 2.164753071236843,
-            "velocityY": 0.016767499987179104,
-            "timestamp": 0.33377166979597883
-        },
-        {
-            "x": 2.679458734246464,
-            "y": 5.546832906614029,
-            "heading": 1.7192242968736162,
-            "angularVelocity": -3.874915282987571,
-            "velocityX": 2.3933208849924683,
-            "velocityY": 0.008896309441567017,
-            "timestamp": 0.36411454886834055
-        },
-        {
-            "x": 2.7588038922481295,
-            "y": 5.545750285790584,
-            "heading": 1.6147732040322635,
-            "angularVelocity": -3.442359328930443,
-            "velocityX": 2.6149515282463223,
-            "velocityY": -0.035679568206544436,
-            "timestamp": 0.39445742794070227
-        },
-        {
-            "x": 2.844227039552491,
-            "y": 5.54174873240555,
-            "heading": 1.5239846755378514,
-            "angularVelocity": -2.992086818060329,
-            "velocityX": 2.8152617653929393,
-            "velocityY": -0.13187784110699777,
-            "timestamp": 0.424800307013064
-        },
-        {
-            "x": 2.9352842549562403,
-            "y": 5.535535053694752,
-            "heading": 1.4520400757805025,
-            "angularVelocity": -2.371053833941581,
-            "velocityX": 3.0009418416293347,
-            "velocityY": -0.20478210706307357,
-            "timestamp": 0.4551431860854257
-        },
-        {
-            "x": 3.0322214391168845,
-            "y": 5.5275999113119605,
-            "heading": 1.3988891918450945,
-            "angularVelocity": -1.7516756998785012,
-            "velocityX": 3.1947259826421948,
-            "velocityY": -0.26151580289620013,
-            "timestamp": 0.4854860651577874
-        },
-        {
-            "x": 3.1352234766500766,
-            "y": 5.5186341666131185,
-            "heading": 1.36445468986127,
-            "angularVelocity": -1.1348462320172443,
-            "velocityX": 3.394603303383066,
-            "velocityY": -0.2954810147534299,
-            "timestamp": 0.5158289442301491
-        },
-        {
-            "x": 3.2443643044344928,
-            "y": 5.509491432490049,
-            "heading": 1.3485232387156947,
-            "angularVelocity": -0.525047445484061,
-            "velocityX": 3.5969173368201925,
-            "velocityY": -0.3013139953287366,
-            "timestamp": 0.5461718233025108
-        },
-        {
-            "x": 3.358782949449409,
-            "y": 5.500338621504484,
-            "heading": 1.3485196823103536,
-            "angularVelocity": -0.00011720724762443023,
-            "velocityX": 3.7708565736972712,
-            "velocityY": -0.3016460950767756,
-            "timestamp": 0.5765147023748725
-        },
-        {
-            "x": 3.4684760810380606,
-            "y": 5.486343295947389,
-            "heading": 1.335475696910559,
-            "angularVelocity": -0.42988621378634684,
-            "velocityX": 3.615119426441224,
-            "velocityY": -0.4612392094935691,
-            "timestamp": 0.6068575814472341
-        },
-        {
-            "x": 3.573383189765519,
-            "y": 5.467459869277216,
-            "heading": 1.309845724977452,
-            "angularVelocity": -0.8446783138799892,
-            "velocityX": 3.457388090209756,
-            "velocityY": -0.6223347041373206,
-            "timestamp": 0.6372004605195958
-        },
-        {
-            "x": 3.6735704123100494,
-            "y": 5.443371550860819,
-            "heading": 1.272922802934567,
-            "angularVelocity": -1.216856250022656,
-            "velocityX": 3.301836398108563,
-            "velocityY": -0.7938705605012504,
-            "timestamp": 0.6675433395919574
-        },
-        {
-            "x": 3.7692345180845526,
-            "y": 5.4136136865914954,
-            "heading": 1.2267696953368106,
-            "angularVelocity": -1.521052352602741,
-            "velocityX": 3.152769568977396,
-            "velocityY": -0.9807198650582027,
-            "timestamp": 0.6978862186643191
-        },
-        {
-            "x": 3.860739036058752,
-            "y": 5.377644621585128,
-            "heading": 1.1742567314728194,
-            "angularVelocity": -1.730651983905632,
-            "velocityX": 3.0156834411124778,
-            "velocityY": -1.1854203063785596,
-            "timestamp": 0.7282290977366808
-        },
-        {
-            "x": 3.9485394410884225,
-            "y": 5.335005141499111,
-            "heading": 1.1187025848702756,
-            "angularVelocity": -1.830879214528657,
-            "velocityX": 2.893608243973284,
-            "velocityY": -1.4052549194270891,
-            "timestamp": 0.7585719768090424
+            "x": 2.253608767824088,
+            "y": 5.559959646221142,
+            "heading": 0.04333845773860565,
+            "angularVelocity": 1.2657316926377489,
+            "velocityX": 0.10539673178657698,
+            "velocityY": -0.0011785619397610312,
+            "timestamp": 0.03423984560921402
+        },
+        {
+            "x": 2.260842860117559,
+            "y": 5.560038303784881,
+            "heading": 0.12991520296053788,
+            "angularVelocity": 2.5285378389274697,
+            "velocityX": 0.211277012637124,
+            "velocityY": 0.002297252290136546,
+            "timestamp": 0.06847969121842803
+        },
+        {
+            "x": 2.271629728646886,
+            "y": 5.560441749821537,
+            "heading": 0.2597959154554935,
+            "angularVelocity": 3.7932622120236568,
+            "velocityX": 0.3150384687021929,
+            "velocityY": 0.011782939714764799,
+            "timestamp": 0.10271953682764205
+        },
+        {
+            "x": 2.285721166472015,
+            "y": 5.561272024294721,
+            "heading": 0.433416913958427,
+            "angularVelocity": 5.070729596286839,
+            "velocityX": 0.41155085761651194,
+            "velocityY": 0.024248779701277118,
+            "timestamp": 0.13695938243685607
+        },
+        {
+            "x": 2.302746549134424,
+            "y": 5.561830584813036,
+            "heading": 0.6513804515506975,
+            "angularVelocity": 6.36578622695705,
+            "velocityX": 0.497238885266109,
+            "velocityY": 0.01631317280720512,
+            "timestamp": 0.17119922804607007
+        },
+        {
+            "x": 2.325552284167229,
+            "y": 5.559809655953846,
+            "heading": 0.9008347442106217,
+            "angularVelocity": 7.285497005652254,
+            "velocityX": 0.6660583488924382,
+            "velocityY": -0.05902272113767896,
+            "timestamp": 0.20543907365528408
+        },
+        {
+            "x": 2.357593597831257,
+            "y": 5.5547755319598515,
+            "heading": 1.1463891087836293,
+            "angularVelocity": 7.171596723173552,
+            "velocityX": 0.9357902494573596,
+            "velocityY": -0.14702531230571617,
+            "timestamp": 0.23967891926449808
+        },
+        {
+            "x": 2.398826095012215,
+            "y": 5.54977146589545,
+            "heading": 1.3795454342192353,
+            "angularVelocity": 6.809502825937482,
+            "velocityX": 1.2042255578939407,
+            "velocityY": -0.14614744825411982,
+            "timestamp": 0.2739187648737121
+        },
+        {
+            "x": 2.4467424620473803,
+            "y": 5.551292722520974,
+            "heading": 1.6025344347532522,
+            "angularVelocity": 6.512558586829906,
+            "velocityX": 1.399432917485785,
+            "velocityY": 0.04442942421196648,
+            "timestamp": 0.3081586104829261
+        },
+        {
+            "x": 2.499282041125752,
+            "y": 5.560003512593039,
+            "heading": 1.8091707816342142,
+            "angularVelocity": 6.034967249541443,
+            "velocityX": 1.5344572425359657,
+            "velocityY": 0.2544050627880495,
+            "timestamp": 0.3423984560921401
+        },
+        {
+            "x": 2.558927262082263,
+            "y": 5.571143049666431,
+            "heading": 1.9899644077603311,
+            "angularVelocity": 5.2802114877955235,
+            "velocityX": 1.7419827658469296,
+            "velocityY": 0.3253384142127747,
+            "timestamp": 0.3766383017013541
+        },
+        {
+            "x": 2.624771333399717,
+            "y": 5.5772713811756995,
+            "heading": 2.148730759711285,
+            "angularVelocity": 4.63688866366352,
+            "velocityX": 1.9230247726273466,
+            "velocityY": 0.17898245159199333,
+            "timestamp": 0.4108781473105681
+        },
+        {
+            "x": 2.6962684360037885,
+            "y": 5.576686266483743,
+            "heading": 2.2908744479705683,
+            "angularVelocity": 4.151411483614645,
+            "velocityX": 2.0881257298902987,
+            "velocityY": -0.0170887070763856,
+            "timestamp": 0.4451179929197821
+        },
+        {
+            "x": 2.773687793639176,
+            "y": 5.569334398339137,
+            "heading": 2.4184902740666434,
+            "angularVelocity": 3.727114530613817,
+            "velocityX": 2.2610895656186547,
+            "velocityY": -0.2147167434256298,
+            "timestamp": 0.4793578385289961
+        },
+        {
+            "x": 2.8582271385194487,
+            "y": 5.556743437974915,
+            "heading": 2.5300657438997165,
+            "angularVelocity": 3.258644069441941,
+            "velocityX": 2.4690340559690878,
+            "velocityY": -0.36772830426646747,
+            "timestamp": 0.5135976841382102
+        },
+        {
+            "x": 2.9508927177061977,
+            "y": 5.541492370817128,
+            "heading": 2.6233336392816216,
+            "angularVelocity": 2.7239578252305745,
+            "velocityX": 2.706366735538439,
+            "velocityY": -0.445418689437171,
+            "timestamp": 0.5478375297474242
+        },
+        {
+            "x": 3.0518533392296137,
+            "y": 5.525810656425357,
+            "heading": 2.697205719709221,
+            "angularVelocity": 2.1574887127329996,
+            "velocityX": 2.948629578406965,
+            "velocityY": -0.45799606022611716,
+            "timestamp": 0.5820773753566382
+        },
+        {
+            "x": 3.1608544462331407,
+            "y": 5.511189976920693,
+            "heading": 2.751249379299885,
+            "angularVelocity": 1.5783850256649679,
+            "velocityX": 3.183457900119567,
+            "velocityY": -0.42700775206563796,
+            "timestamp": 0.6163172209658522
+        },
+        {
+            "x": 3.2775207255393237,
+            "y": 5.498694402974223,
+            "heading": 2.785399702937351,
+            "angularVelocity": 0.9973854446433661,
+            "velocityX": 3.40732492306531,
+            "velocityY": -0.36494247342945346,
+            "timestamp": 0.6505570665750662
+        },
+        {
+            "x": 3.401135401206032,
+            "y": 5.488837187261409,
+            "heading": 2.800896234569334,
+            "angularVelocity": 0.4525876608454414,
+            "velocityX": 3.610257974803583,
+            "velocityY": -0.28788727102673717,
+            "timestamp": 0.6847969121842802
+        },
+        {
+            "x": 3.5179017921083777,
+            "y": 5.474779629986763,
+            "heading": 2.835487088185811,
+            "angularVelocity": 1.0102514483058458,
+            "velocityX": 3.410248756230483,
+            "velocityY": -0.4105613510962388,
+            "timestamp": 0.7190367577934942
+        },
+        {
+            "x": 3.6287098119916026,
+            "y": 5.4546983370085105,
+            "heading": 2.8876881760682602,
+            "angularVelocity": 1.5245713569573334,
+            "velocityX": 3.2362301263825226,
+            "velocityY": -0.5864890048700768,
+            "timestamp": 0.7532766034027082
+        },
+        {
+            "x": 3.7356372636451365,
+            "y": 5.425785607175325,
+            "heading": 2.9455736884009496,
+            "angularVelocity": 1.6905891747687094,
+            "velocityX": 3.1228952628442794,
+            "velocityY": -0.8444176461299453,
+            "timestamp": 0.7875164490119222
+        },
+        {
+            "x": 3.8388362586877394,
+            "y": 5.387864157402447,
+            "heading": 3.0009390226743027,
+            "angularVelocity": 1.6169855117119507,
+            "velocityX": 3.0140029315678847,
+            "velocityY": -1.1075239709221774,
+            "timestamp": 0.8217562946211362
+        },
+        {
+            "x": 3.9379980862804507,
+            "y": 5.341070528950818,
+            "heading": 3.053673275383994,
+            "angularVelocity": 1.5401428298350917,
+            "velocityX": 2.896094472050628,
+            "velocityY": -1.3666425072616961,
+            "timestamp": 0.8559961402303502
         },
         {
             "x": 4.032804489135742,
             "y": 5.285560607910156,
-            "heading": 1.064415461383156,
-            "angularVelocity": -1.7891223623722634,
-            "velocityX": 2.7770946799861798,
-            "velocityY": -1.6295267654410455,
-            "timestamp": 0.7889148558814041
-        },
-        {
-            "x": 4.1733297943826075,
-            "y": 5.174158598481438,
-            "heading": 0.9768931525481981,
-            "angularVelocity": -1.5934853179105337,
-            "velocityX": 2.55849067153879,
-            "velocityY": -2.0282539248952025,
-            "timestamp": 0.8438399360507789
-        },
-        {
-            "x": 4.303490854924698,
-            "y": 5.0407757547823575,
-            "heading": 0.9131112872795231,
-            "angularVelocity": -1.1612521105474607,
-            "velocityX": 2.369792818521288,
-            "velocityY": -2.428450596480911,
-            "timestamp": 0.8987650162201537
-        },
-        {
-            "x": 4.424699657845323,
-            "y": 4.885599604328108,
-            "heading": 0.881895788498386,
-            "angularVelocity": -0.5683286885495016,
-            "velocityX": 2.206802476152012,
-            "velocityY": -2.8252330260734433,
-            "timestamp": 0.9536900963895285
-        },
-        {
-            "x": 4.534874604263042,
-            "y": 4.709437065112464,
-            "heading": 0.8818953110956026,
-            "angularVelocity": -0.000008691890515565163,
-            "velocityX": 2.005913256347883,
-            "velocityY": -3.2073242073093726,
-            "timestamp": 1.0086151765589033
-        },
-        {
-            "x": 4.656392865207935,
-            "y": 4.550190331352248,
-            "heading": 0.8602662015486029,
-            "angularVelocity": -0.3937929536070082,
-            "velocityX": 2.2124366604502335,
-            "velocityY": -2.899344584826112,
-            "timestamp": 1.0635402567282781
-        },
-        {
-            "x": 4.786084989350276,
-            "y": 4.412405730848589,
-            "heading": 0.8009192530896463,
-            "angularVelocity": -1.0805072705573824,
-            "velocityX": 2.3612550722257284,
-            "velocityY": -2.508591704896303,
-            "timestamp": 1.118465336897653
-        },
-        {
-            "x": 4.924876209285355,
-            "y": 4.297327162575611,
-            "heading": 0.7205421769770328,
-            "angularVelocity": -1.463394789133692,
-            "velocityX": 2.5269188412121024,
-            "velocityY": -2.0951916304556226,
-            "timestamp": 1.1733904170670277
-        },
-        {
-            "x": 5.072286801869267,
-            "y": 4.205735025902594,
-            "heading": 0.6308477205816693,
-            "angularVelocity": -1.6330327806307947,
-            "velocityX": 2.6838484737634416,
-            "velocityY": -1.6675831221469524,
-            "timestamp": 1.2283154972364025
-        },
-        {
-            "x": 5.227765920434763,
-            "y": 4.1379313687941846,
-            "heading": 0.5364496515071664,
-            "angularVelocity": -1.7186696639022372,
-            "velocityX": 2.830749050998892,
-            "velocityY": -1.2344753416712253,
-            "timestamp": 1.2832405774057773
+            "heading": 3.1036105469083837,
+            "angularVelocity": 1.4584549268806075,
+            "velocityX": 2.76889107320562,
+            "velocityY": -1.62120827512502,
+            "timestamp": 0.8902359858395642
+        },
+        {
+            "x": 4.173725003261308,
+            "y": 5.173637687034127,
+            "heading": 3.175484729095798,
+            "angularVelocity": 1.2994086167535415,
+            "velocityX": 2.547692714675012,
+            "velocityY": -2.023447131813128,
+            "timestamp": 0.9455489807364162
+        },
+        {
+            "x": 4.301872286760783,
+            "y": 5.039921697201636,
+            "heading": 3.2346580028937466,
+            "angularVelocity": 1.0697897285853943,
+            "velocityX": 2.3167663175433564,
+            "velocityY": -2.4174425934058767,
+            "timestamp": 1.0008619756332682
+        },
+        {
+            "x": 4.416442024233276,
+            "y": 4.885133369968262,
+            "heading": 3.2770092965554323,
+            "angularVelocity": 0.765666255111711,
+            "velocityX": 2.0712987551323803,
+            "velocityY": -2.7984079965661373,
+            "timestamp": 1.0561749705301202
+        },
+        {
+            "x": 4.517908742039174,
+            "y": 4.711097539386313,
+            "heading": 3.282986865137276,
+            "angularVelocity": 0.10806806959180061,
+            "velocityX": 1.834410123608644,
+            "velocityY": -3.146382344808711,
+            "timestamp": 1.1114879654269723
+        },
+        {
+            "x": 4.627451956204509,
+            "y": 4.5560902871099245,
+            "heading": 3.2338916761611958,
+            "angularVelocity": -0.8875886953442504,
+            "velocityX": 1.9804245705663106,
+            "velocityY": -2.802365928032756,
+            "timestamp": 1.1668009603238243
+        },
+        {
+            "x": 4.751713919325069,
+            "y": 4.421499213962681,
+            "heading": 3.169974564154514,
+            "angularVelocity": -1.1555532678328913,
+            "velocityX": 2.246523865726009,
+            "velocityY": -2.433263167149585,
+            "timestamp": 1.2221139552206763
+        },
+        {
+            "x": 4.891049798654671,
+            "y": 4.307366204928411,
+            "heading": 3.098048256171184,
+            "angularVelocity": -1.3003509955925967,
+            "velocityX": 2.5190442063286667,
+            "velocityY": -2.063403170396147,
+            "timestamp": 1.2774269501175284
+        },
+        {
+            "x": 5.0446094980944505,
+            "y": 4.214363163694223,
+            "heading": 3.021068837736617,
+            "angularVelocity": -1.3917058473893738,
+            "velocityX": 2.7761957154216415,
+            "velocityY": -1.6813958710357464,
+            "timestamp": 1.3327399450143804
+        },
+        {
+            "x": 5.211468562298102,
+            "y": 4.143120621239898,
+            "heading": 2.9418200950482523,
+            "angularVelocity": -1.432732811451417,
+            "velocityX": 3.016634057056783,
+            "velocityY": -1.2879892435254758,
+            "timestamp": 1.3880529399112325
         },
         {
             "x": 5.390789031982422,
             "y": 4.09412145614624,
-            "heading": 0.44081122980213755,
-            "angularVelocity": -1.7412522914869581,
-            "velocityX": 2.968099655839153,
-            "velocityY": -0.7976303814732078,
-            "timestamp": 1.3381656575751522
-        },
-        {
-            "x": 5.527208209694737,
-            "y": 4.074474240030864,
-            "heading": 0.3646632920053755,
-            "angularVelocity": -1.7150127332494942,
-            "velocityX": 3.0724486257327652,
-            "velocityY": -0.44249689204593257,
-            "timestamp": 1.3825664578260528
-        },
-        {
-            "x": 5.669753286288942,
-            "y": 4.0699039581909835,
-            "heading": 0.2967653403322419,
-            "angularVelocity": -1.529205583896121,
-            "velocityX": 3.2104168345775173,
-            "velocityY": -0.10293242045310311,
-            "timestamp": 1.4269672580769535
-        },
-        {
-            "x": 5.825688318788907,
-            "y": 4.069335183916362,
-            "heading": 0.2632559067133074,
-            "angularVelocity": -0.7547033708757247,
-            "velocityX": 3.5119869826400643,
-            "velocityY": -0.012810000527210512,
-            "timestamp": 1.4713680583278541
-        },
-        {
-            "x": 5.986467377363025,
-            "y": 4.053794613625281,
-            "heading": 0.24734267435752091,
-            "angularVelocity": -0.35839967446225596,
-            "velocityX": 3.6210847026536497,
-            "velocityY": -0.3500065359917586,
-            "timestamp": 1.5157688585787548
-        },
-        {
-            "x": 6.146135354603106,
-            "y": 4.023716272835588,
-            "heading": 0.25998224016695326,
-            "angularVelocity": 0.28466977482406836,
-            "velocityX": 3.5960607993060463,
-            "velocityY": -0.6774278981398296,
-            "timestamp": 1.5601696588296554
-        },
-        {
-            "x": 6.299693722526358,
-            "y": 3.978509002442476,
-            "heading": 0.279745523920822,
-            "angularVelocity": 0.4451109809325518,
-            "velocityX": 3.4584594659447885,
-            "velocityY": -1.0181634145703302,
-            "timestamp": 1.604570459080556
-        },
-        {
-            "x": 6.446348354717166,
-            "y": 3.918416502871273,
-            "heading": 0.30303477341760615,
-            "angularVelocity": 0.5245231924915958,
-            "velocityX": 3.3029727248628453,
-            "velocityY": -1.3534102816082356,
-            "timestamp": 1.6489712593314567
-        },
-        {
-            "x": 6.585501043822193,
-            "y": 3.8437018977675175,
-            "heading": 0.32757979886137184,
-            "angularVelocity": 0.5528059247821282,
-            "velocityX": 3.134013088023211,
-            "velocityY": -1.6827310472234156,
-            "timestamp": 1.6933720595823574
-        },
-        {
-            "x": 6.716485001612935,
-            "y": 3.754724015839551,
-            "heading": 0.3514161297676369,
-            "angularVelocity": 0.5368446237808859,
-            "velocityX": 2.950035968959475,
-            "velocityY": -2.0039702308329863,
-            "timestamp": 1.737772859833258
-        },
-        {
-            "x": 6.838464987128356,
-            "y": 3.6520072183603935,
-            "heading": 0.37305697715168934,
-            "angularVelocity": 0.48739768791922733,
-            "velocityX": 2.747247455589403,
-            "velocityY": -2.313399688715609,
-            "timestamp": 1.7821736600841587
-        },
-        {
-            "x": 6.950225204946805,
-            "y": 3.536435013339574,
-            "heading": 0.39217088238591646,
-            "angularVelocity": 0.430485602201265,
-            "velocityX": 2.5170766559817053,
-            "velocityY": -2.60293067619823,
-            "timestamp": 1.8265744603350593
-        },
-        {
-            "x": 7.049675913816151,
-            "y": 3.409971379240331,
-            "heading": 0.4121286773514249,
-            "angularVelocity": 0.44949178511942617,
-            "velocityX": 2.23984046024776,
-            "velocityY": -2.848228711748902,
-            "timestamp": 1.87097526058596
-        },
-        {
-            "x": 7.134078905097545,
-            "y": 3.2784202019721485,
-            "heading": 0.44760621801548245,
-            "angularVelocity": 0.799029307210242,
-            "velocityX": 1.9009340102981855,
-            "velocityY": -2.9628109521632653,
-            "timestamp": 1.9153760608368606
-        },
-        {
-            "x": 7.206535248844668,
-            "y": 3.1515334712387078,
-            "heading": 0.5188452438191252,
-            "angularVelocity": 1.6044536450037898,
-            "velocityX": 1.6318702216556016,
-            "velocityY": -2.857757743473708,
-            "timestamp": 1.9597768610877613
-        },
-        {
-            "x": 7.286286308378336,
-            "y": 3.038022763329368,
-            "heading": 0.612459613040285,
-            "angularVelocity": 2.1083937382245868,
-            "velocityX": 1.796162661100898,
-            "velocityY": -2.556501397900756,
-            "timestamp": 2.004177661338662
-        },
-        {
-            "x": 7.36964311440034,
-            "y": 2.9389690479632176,
-            "heading": 0.7307138885874557,
-            "angularVelocity": 2.6633365812989322,
-            "velocityX": 1.8773717039100728,
-            "velocityY": -2.2308993262827714,
-            "timestamp": 2.0485784615895626
-        },
-        {
-            "x": 7.443994240643498,
-            "y": 2.8513361754602453,
-            "heading": 0.8783950719218284,
-            "angularVelocity": 3.326092829405186,
-            "velocityX": 1.674544733946552,
-            "velocityY": -1.9736777717467908,
-            "timestamp": 2.0929792618404632
-        },
-        {
-            "x": 7.506362936213548,
-            "y": 2.772668193478103,
-            "heading": 1.052033337356815,
-            "angularVelocity": 3.9107012588464345,
-            "velocityX": 1.4046750332790265,
-            "velocityY": -1.7717694622079865,
-            "timestamp": 2.137380062091364
-        },
-        {
-            "x": 7.555798075190431,
-            "y": 2.7014303099088472,
-            "heading": 1.2519057769914284,
-            "angularVelocity": 4.501550388848195,
-            "velocityX": 1.1133839637469243,
-            "velocityY": -1.6044279194677442,
-            "timestamp": 2.1817808623422645
-        },
-        {
-            "x": 7.592358210662926,
-            "y": 2.6364422153707756,
-            "heading": 1.482218675635827,
-            "angularVelocity": 5.187133955760781,
-            "velocityX": 0.8234116337070656,
-            "velocityY": -1.4636694422360859,
-            "timestamp": 2.226181662593165
-        },
-        {
-            "x": 7.621254126448735,
-            "y": 2.5813646495023557,
-            "heading": 1.7524007538852981,
-            "angularVelocity": 6.085072267227659,
-            "velocityX": 0.6507971843417887,
-            "velocityY": -1.2404633600562773,
-            "timestamp": 2.270582462844066
-        },
-        {
-            "x": 7.645052051647252,
-            "y": 2.5375067096061756,
-            "heading": 2.067951038154778,
-            "angularVelocity": 7.10686029275066,
-            "velocityX": 0.5359796459532341,
-            "velocityY": -0.9877736357981667,
-            "timestamp": 2.3149832630949665
-        },
-        {
-            "x": 7.661427436541854,
-            "y": 2.5075370043722347,
-            "heading": 2.400674072151774,
-            "angularVelocity": 7.493626964307851,
-            "velocityX": 0.36880832782444795,
-            "velocityY": -0.6749811954871879,
-            "timestamp": 2.359384063345867
-        },
-        {
-            "x": 7.6692908711764884,
-            "y": 2.4875264930527843,
-            "heading": 2.6956822674348517,
-            "angularVelocity": 6.644208969569037,
-            "velocityX": 0.17710119164969557,
-            "velocityY": -0.4506790689891867,
-            "timestamp": 2.403784863596768
-        },
-        {
-            "x": 7.674629616295112,
-            "y": 2.472122256604734,
-            "heading": 2.9172973649743583,
-            "angularVelocity": 4.991241065188027,
-            "velocityX": 0.12023984001312674,
-            "velocityY": -0.3469360092836942,
-            "timestamp": 2.4481856638476684
-        },
-        {
-            "x": 7.678206340030146,
-            "y": 2.4619495353215974,
-            "heading": 3.0655099943755024,
-            "angularVelocity": 3.3380621196830216,
-            "velocityX": 0.0805553889754595,
-            "velocityY": -0.22911121479010477,
-            "timestamp": 2.492586464098569
+            "heading": 2.865640793449113,
+            "angularVelocity": -1.3772405876991196,
+            "velocityX": 3.2419229878750393,
+            "velocityY": -0.8858526858838836,
+            "timestamp": 1.4433659348080845
+        },
+        {
+            "x": 5.535592658236132,
+            "y": 4.049337172847784,
+            "heading": 2.841642220937184,
+            "angularVelocity": -0.5722130304709164,
+            "velocityX": 3.452643767066762,
+            "velocityY": -1.0678197818199107,
+            "timestamp": 1.4853058614332775
+        },
+        {
+            "x": 5.681734895850587,
+            "y": 4.003264514213434,
+            "heading": 2.8233979117656802,
+            "angularVelocity": -0.4350105171749157,
+            "velocityX": 3.484561118108144,
+            "velocityY": -1.098539323783022,
+            "timestamp": 1.5272457880584704
+        },
+        {
+            "x": 5.827742551539107,
+            "y": 3.9572785916007636,
+            "heading": 2.804954082691072,
+            "angularVelocity": -0.4397677954812897,
+            "velocityX": 3.481352196758831,
+            "velocityY": -1.096471222366128,
+            "timestamp": 1.5691857146836634
+        },
+        {
+            "x": 5.9679573209760495,
+            "y": 3.908501280249766,
+            "heading": 2.7717324952229117,
+            "angularVelocity": -0.7921231661908814,
+            "velocityX": 3.3432287731451744,
+            "velocityY": -1.1630280564605782,
+            "timestamp": 1.6111256413088564
+        },
+        {
+            "x": 6.096940014064459,
+            "y": 3.8543767396001813,
+            "heading": 2.711734513417447,
+            "angularVelocity": -1.4305695463335533,
+            "velocityX": 3.07541532537948,
+            "velocityY": -1.2905254015650245,
+            "timestamp": 1.6530655679340494
+        },
+        {
+            "x": 6.215088187160598,
+            "y": 3.7918967611554555,
+            "heading": 2.632459037184743,
+            "angularVelocity": -1.890214948184595,
+            "velocityX": 2.8170810633981405,
+            "velocityY": -1.4897493503766595,
+            "timestamp": 1.6950054945592423
+        },
+        {
+            "x": 6.32424952389229,
+            "y": 3.717983083464812,
+            "heading": 2.5463998013121922,
+            "angularVelocity": -2.0519643880553406,
+            "velocityX": 2.6028022821127137,
+            "velocityY": -1.7623702194615716,
+            "timestamp": 1.7369454211844353
+        },
+        {
+            "x": 6.43630340548619,
+            "y": 3.6322698094657313,
+            "heading": 2.4886072976482807,
+            "angularVelocity": -1.3779829464268867,
+            "velocityX": 2.6717710451737124,
+            "velocityY": -2.043715401914737,
+            "timestamp": 1.7788853478096283
+        },
+        {
+            "x": 6.552059359988985,
+            "y": 3.5378523447078845,
+            "heading": 2.4573864362362134,
+            "angularVelocity": -0.7444185987992016,
+            "velocityX": 2.7600418936656252,
+            "velocityY": -2.251254886581771,
+            "timestamp": 1.8208252744348212
+        },
+        {
+            "x": 6.67032629558411,
+            "y": 3.432112823682954,
+            "heading": 2.4573457680499984,
+            "angularVelocity": -0.0009696770950155036,
+            "velocityX": 2.8199127922194083,
+            "velocityY": -2.521213782033988,
+            "timestamp": 1.8627652010600142
+        },
+        {
+            "x": 6.787202921818474,
+            "y": 3.3248215481814336,
+            "heading": 2.4573432508654287,
+            "angularVelocity": -0.00006001881195361165,
+            "velocityX": 2.7867627733081557,
+            "velocityY": -2.558213238195593,
+            "timestamp": 1.9047051276852072
+        },
+        {
+            "x": 6.904079764497574,
+            "y": 3.217530508972575,
+            "heading": 2.4573407325391345,
+            "angularVelocity": -0.000060046034804859665,
+            "velocityX": 2.7867679341358973,
+            "velocityY": -2.5582076041213115,
+            "timestamp": 1.9466450543104001
+        },
+        {
+            "x": 7.017539426879104,
+            "y": 3.1193532157248125,
+            "heading": 2.428286840001792,
+            "angularVelocity": -0.6927501995172902,
+            "velocityX": 2.7052899590285975,
+            "velocityY": -2.3409028376503684,
+            "timestamp": 1.988584980935593
+        },
+        {
+            "x": 7.118887938132189,
+            "y": 3.020968055873212,
+            "heading": 2.3710302150679574,
+            "angularVelocity": -1.365205653446252,
+            "velocityX": 2.4165161794108876,
+            "velocityY": -2.345859131582302,
+            "timestamp": 2.030524907560786
+        },
+        {
+            "x": 7.207551237715759,
+            "y": 2.9223074295194613,
+            "heading": 2.2882909026899103,
+            "angularVelocity": -1.9728053679604207,
+            "velocityX": 2.1140547139229047,
+            "velocityY": -2.3524272523282552,
+            "timestamp": 2.0724648341859786
+        },
+        {
+            "x": 7.28673413242529,
+            "y": 2.8318719270188626,
+            "heading": 2.1761461762136682,
+            "angularVelocity": -2.6739371167349164,
+            "velocityX": 1.8880074688058086,
+            "velocityY": -2.1563104606452623,
+            "timestamp": 2.1144047608111713
+        },
+        {
+            "x": 7.3579924776067,
+            "y": 2.7517426977667294,
+            "heading": 2.0378729016459984,
+            "angularVelocity": -3.2969364921256217,
+            "velocityX": 1.6990574594521515,
+            "velocityY": -1.9105715174046114,
+            "timestamp": 2.156344687436364
+        },
+        {
+            "x": 7.4224137082831785,
+            "y": 2.6828558926645387,
+            "heading": 1.874977709266495,
+            "angularVelocity": -3.884012335912244,
+            "velocityX": 1.5360358460374843,
+            "velocityY": -1.6425113405136766,
+            "timestamp": 2.198284614061557
+        },
+        {
+            "x": 7.480883603746462,
+            "y": 2.6255140291502124,
+            "heading": 1.6865689006450588,
+            "angularVelocity": -4.4923495051672475,
+            "velocityX": 1.3941344243592637,
+            "velocityY": -1.3672380504328716,
+            "timestamp": 2.2402245406867496
+        },
+        {
+            "x": 7.534437188323511,
+            "y": 2.579534136686106,
+            "heading": 1.4695142507662173,
+            "angularVelocity": -5.175370281846384,
+            "velocityX": 1.27691173748694,
+            "velocityY": -1.0963274417482318,
+            "timestamp": 2.2821644673119423
+        },
+        {
+            "x": 7.579534895925645,
+            "y": 2.5413039154996566,
+            "heading": 1.2172257697546927,
+            "angularVelocity": -6.015472636997345,
+            "velocityX": 1.0752929542572058,
+            "velocityY": -0.9115471643072651,
+            "timestamp": 2.324104393937135
+        },
+        {
+            "x": 7.615712715030757,
+            "y": 2.5097826728946813,
+            "heading": 0.9253708991340255,
+            "angularVelocity": -6.958878903840344,
+            "velocityX": 0.8626104530039771,
+            "velocityY": -0.7515807761580874,
+            "timestamp": 2.366044320562328
+        },
+        {
+            "x": 7.6401514150011085,
+            "y": 2.4865499215178333,
+            "heading": 0.6440959436378817,
+            "angularVelocity": -6.706615345559152,
+            "velocityX": 0.5827072657697844,
+            "velocityY": -0.5539530763721487,
+            "timestamp": 2.4079842471875206
+        },
+        {
+            "x": 7.656070937434432,
+            "y": 2.471863188851966,
+            "heading": 0.3945229348796906,
+            "angularVelocity": -5.950725927314207,
+            "velocityX": 0.37957916749813,
+            "velocityY": -0.3501849871393242,
+            "timestamp": 2.4499241738127133
+        },
+        {
+            "x": 7.668072859318959,
+            "y": 2.4642781587251137,
+            "heading": 0.19785094472919199,
+            "angularVelocity": -4.689373729909185,
+            "velocityX": 0.28616935818189254,
+            "velocityY": -0.18085463512222474,
+            "timestamp": 2.491864100437906
+        },
+        {
+            "x": 7.676136280306529,
+            "y": 2.459442710773055,
+            "heading": 0.06623696286278732,
+            "angularVelocity": -3.1381547956105695,
+            "velocityX": 0.19226120874341576,
+            "velocityY": -0.11529462116784107,
+            "timestamp": 2.533804027063099
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 1.6776725915652129,
-            "velocityX": 0.04039701896629419,
-            "velocityY": -0.11147401158601906,
-            "timestamp": 2.5369872643494698
+            "heading": 2.1057025676027217e-33,
+            "angularVelocity": -1.57932948845455,
+            "velocityX": 0.09212509425682942,
+            "velocityY": -0.058243086471875524,
+            "timestamp": 2.5757439536882916
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -5.831834401931518e-35,
-            "velocityX": -1.3540056730700436e-35,
-            "velocityY": -1.938442767179852e-35,
-            "timestamp": 2.5813880646003704
+            "heading": -7.524501141381425e-34,
+            "angularVelocity": -5.907340858361994e-32,
+            "velocityX": -7.634877430876997e-33,
+            "velocityY": -7.90956322394541e-33,
+            "timestamp": 2.6176838803134843
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.1.traj
index ac17f8d7..cfa6b046 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.1.traj	
@@ -3,623 +3,623 @@
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": -2.730529759762368e-29,
+            "velocityY": 6.892560738534845e-27,
             "timestamp": 0
         },
         {
-            "x": 2.2482118663055717,
-            "y": 5.55085896880981,
-            "heading": 3.1399999903330857,
-            "angularVelocity": -2.8923280730678937e-7,
-            "velocityX": -0.05350072563170815,
-            "velocityY": -0.2734984543883791,
-            "timestamp": 0.03342260639319256
-        },
-        {
-            "x": 2.24463557685663,
-            "y": 5.5325769109933445,
-            "heading": 3.1399999700354155,
-            "angularVelocity": -6.073036282303753e-7,
-            "velocityX": -0.10700211129830166,
-            "velocityY": -0.5469967722250598,
-            "timestamp": 0.06684521278638512
-        },
-        {
-            "x": 2.2392711054637595,
-            "y": 5.505153831967315,
-            "heading": 3.139999938021822,
-            "angularVelocity": -9.578425018869592e-7,
-            "velocityX": -0.16050428054878876,
-            "velocityY": -0.8204949279944175,
-            "timestamp": 0.1002678191795777
-        },
-        {
-            "x": 2.2321184205712044,
-            "y": 5.468589738256669,
-            "heading": 3.139999893047122,
-            "angularVelocity": -0.0000013456371104363374,
-            "velocityX": -0.2140073939433933,
-            "velocityY": -1.0939928885381913,
-            "timestamp": 0.13369042557277025
-        },
-        {
-            "x": 2.2231774834707245,
-            "y": 5.422884637863401,
-            "heading": 3.1399998336497137,
-            "angularVelocity": -0.000001777162647778402,
-            "velocityX": -0.2675116654835123,
-            "velocityY": -1.3674906096633432,
-            "timestamp": 0.1671130319659628
-        },
-        {
-            "x": 2.2124482456246803,
-            "y": 5.368038540819271,
-            "heading": 3.139999758066302,
-            "angularVelocity": -0.0000022614457562380146,
-            "velocityX": -0.3210173892634857,
-            "velocityY": -1.6409880306395483,
-            "timestamp": 0.20053563835915536
-        },
-        {
-            "x": 2.199930644465073,
-            "y": 5.30405146005317,
-            "heading": 3.139999664095155,
-            "angularVelocity": -0.0000028116043956655657,
-            "velocityX": -0.3745249850698391,
-            "velocityY": -1.9144850647849765,
-            "timestamp": 0.23395824475234792
-        },
-        {
-            "x": 2.1856245964060905,
-            "y": 5.230923412833771,
-            "heading": 3.1399995488597034,
-            "angularVelocity": -0.0000034478296167107145,
-            "velocityX": -0.4280350817528267,
-            "velocityY": -2.1879815822539745,
-            "timestamp": 0.2673808511455405
-        },
-        {
-            "x": 2.1695299843090114,
-            "y": 5.148654423357543,
-            "heading": 3.139999408375343,
-            "angularVelocity": -0.000004203273662517842,
-            "velocityX": -0.4815486832110918,
-            "velocityY": -2.461477375767798,
-            "timestamp": 0.30080345753873305
-        },
-        {
-            "x": 2.15164663254415,
-            "y": 5.057244527897619,
-            "heading": 3.139999236662074,
-            "angularVelocity": -0.000005137638535462322,
-            "velocityX": -0.535067539486715,
-            "velocityY": -2.7349720839942284,
-            "timestamp": 0.3342260639319256
-        },
-        {
-            "x": 2.1319742493327887,
-            "y": 4.956693786709351,
-            "heading": 3.1399990236359914,
-            "angularVelocity": -0.000006373712524248939,
-            "velocityX": -0.5885951257435309,
-            "velocityY": -3.008464989388312,
-            "timestamp": 0.36764867032511817
-        },
-        {
-            "x": 2.110512257081521,
-            "y": 4.847002319075526,
-            "heading": 3.13999874874802,
-            "angularVelocity": -0.000008224612190539215,
-            "velocityX": -0.6421399935196433,
-            "velocityY": -3.28195432585699,
-            "timestamp": 0.4010712767183107
-        },
-        {
-            "x": 2.087258999095236,
-            "y": 4.728170467468909,
-            "heading": 3.139998349832889,
-            "angularVelocity": -0.00001193548864433131,
-            "velocityX": -0.6957344293958355,
-            "velocityY": -3.555433415585576,
-            "timestamp": 0.4344938831115033
-        },
-        {
-            "x": 2.0629644962421034,
-            "y": 4.604090796237193,
-            "heading": 3.1399983498325463,
-            "angularVelocity": -1.0248884056674418e-11,
-            "velocityX": -0.7268883391614586,
-            "velocityY": -3.712447490549055,
-            "timestamp": 0.46791648950469583
-        },
-        {
-            "x": 2.0387448816492295,
-            "y": 4.479996482972041,
-            "heading": 3.139998349833487,
-            "angularVelocity": 2.8141899837441888e-11,
-            "velocityX": -0.7246476922026518,
-            "velocityY": -3.712885578275693,
-            "timestamp": 0.5013390958978884
-        },
-        {
-            "x": 2.0146234361555044,
-            "y": 4.355883050210464,
-            "heading": 3.139998349833582,
-            "angularVelocity": 2.8336640394810287e-12,
-            "velocityX": -0.7217104856241171,
-            "velocityY": -3.713457631102453,
-            "timestamp": 0.534761702291081
-        },
-        {
-            "x": 1.9906204010106838,
-            "y": 4.231746663125483,
-            "heading": 3.1399983498337,
-            "angularVelocity": 3.5336881649516105e-12,
-            "velocityX": -0.7181676635393196,
-            "velocityY": -3.714144421443354,
-            "timestamp": 0.5681843086842736
-        },
-        {
-            "x": 1.9667619480914416,
-            "y": 4.107582407046251,
-            "heading": 3.139998349833824,
-            "angularVelocity": 3.708930519775865e-12,
-            "velocityX": -0.7138417823660727,
-            "velocityY": -3.7149782580858504,
-            "timestamp": 0.6016069150774662
-        },
-        {
-            "x": 1.9503914931986956,
-            "y": 3.982210973196183,
-            "heading": 3.1399983498339594,
-            "angularVelocity": 4.046203674508551e-12,
-            "velocityX": -0.4898018634439243,
-            "velocityY": -3.751096858670778,
-            "timestamp": 0.6350295214706588
-        },
-        {
-            "x": 1.943224502199385,
-            "y": 3.8559785593201936,
-            "heading": 3.1399983498340998,
-            "angularVelocity": 4.203331220184306e-12,
-            "velocityX": -0.21443543076194166,
-            "velocityY": -3.7768572681307018,
-            "timestamp": 0.6684521278638514
-        },
-        {
-            "x": 1.9453593714612354,
-            "y": 3.7295608780305693,
-            "heading": 3.1399983498342436,
-            "angularVelocity": 4.304427970302576e-12,
-            "velocityX": 0.06387500833733431,
-            "velocityY": -3.7824004448488933,
-            "timestamp": 0.701874734257044
-        },
-        {
-            "x": 1.9567930300665735,
-            "y": 3.6036432110345795,
-            "heading": 3.139998349834389,
-            "angularVelocity": 4.350609924565741e-12,
-            "velocityX": 0.3420935659816486,
-            "velocityY": -3.767440082758668,
-            "timestamp": 0.7352973406502367
-        },
-        {
-            "x": 1.9774653925716308,
-            "y": 3.4789089323576285,
-            "heading": 3.1399983498345336,
-            "angularVelocity": 4.327887690330789e-12,
-            "velocityX": 0.6185143750313882,
-            "velocityY": -3.732033259450771,
-            "timestamp": 0.7687199470434293
-        },
-        {
-            "x": 2.0072643339986427,
-            "y": 3.3560349759367867,
-            "heading": 3.1399983498346824,
-            "angularVelocity": 4.434032141487508e-12,
-            "velocityX": 0.8915804194501273,
-            "velocityY": -3.6763726615238776,
-            "timestamp": 0.8021425534366219
-        },
-        {
-            "x": 2.0460281463567345,
-            "y": 3.235688177587346,
-            "heading": 3.1399983498348414,
-            "angularVelocity": 4.757708405328401e-12,
-            "velocityX": 1.1598081819894186,
-            "velocityY": -3.600760423457161,
-            "timestamp": 0.8355651598298145
-        },
-        {
-            "x": 2.093546464867386,
-            "y": 3.118521659475836,
-            "heading": 3.139998349835024,
-            "angularVelocity": 5.453307843013083e-12,
-            "velocityX": 1.421741857944163,
-            "velocityY": -3.5056068558230677,
-            "timestamp": 0.8689877662230071
+            "x": 2.24821488195175,
+            "y": 5.550858605907564,
+            "heading": -1.0860081979744884e-8,
+            "angularVelocity": -3.249360952601713e-7,
+            "velocityX": -0.053411135206172675,
+            "velocityY": -0.27351257600550494,
+            "timestamp": 0.0334222075852833
+        },
+        {
+            "x": 2.2446446457975586,
+            "y": 5.532575817984525,
+            "heading": -3.253781151640391e-8,
+            "angularVelocity": -6.486025430910346e-7,
+            "velocityX": -0.10682227213842765,
+            "velocityY": -0.5470251441769277,
+            "timestamp": 0.0668444151705666
+        },
+        {
+            "x": 2.2392892913368425,
+            "y": 5.505151636566201,
+            "heading": -6.492851000422694e-8,
+            "angularVelocity": -9.691369834959166e-7,
+            "velocityX": -0.1602334150722015,
+            "velocityY": -0.8205377023156053,
+            "timestamp": 0.10026662275584991
+        },
+        {
+            "x": 2.2321488181807467,
+            "y": 5.468586062083933,
+            "heading": -1.0785000725838172e-7,
+            "angularVelocity": -0.000001284220850944888,
+            "velocityX": -0.21364456964054193,
+            "velocityY": -1.0940502475484135,
+            "timestamp": 0.1336888303411332
+        },
+        {
+            "x": 2.2232232256851456,
+            "y": 5.42287909509794,
+            "heading": -1.6102300741445552e-7,
+            "angularVelocity": -0.0000015909481231905412,
+            "velocityX": -0.26705574348127187,
+            "velocityY": -1.3675627760193463,
+            "timestamp": 0.16711103792641652
+        },
+        {
+            "x": 2.212512512848178,
+            "y": 5.368030736346982,
+            "heading": -2.2404505707142586e-7,
+            "angularVelocity": -0.0000018856338989468898,
+            "velocityX": -0.32046694729784664,
+            "velocityY": -1.6410752823864576,
+            "timestamp": 0.20053324551169982
+        },
+        {
+            "x": 2.200016678146418,
+            "y": 5.304040986826963,
+            "heading": -2.963548114247805e-7,
+            "angularVelocity": -0.000002163524111560841,
+            "velocityX": -0.3738781966956197,
+            "velocityY": -1.9145877589558649,
+            "timestamp": 0.23395545309698312
+        },
+        {
+            "x": 2.1857357192573095,
+            "y": 5.230909847922857,
+            "heading": -3.7718073304542066e-7,
+            "angularVelocity": -0.0000024183297502514874,
+            "velocityX": -0.4272895155847271,
+            "velocityY": -2.1881001940865983,
+            "timestamp": 0.2673776606822664
+        },
+        {
+            "x": 2.169669632551276,
+            "y": 5.148637321647842,
+            "heading": -4.654625982128869e-7,
+            "angularVelocity": -0.0000026414132559778984,
+            "velocityX": -0.48070094307146977,
+            "velocityY": -2.4616125689825754,
+            "timestamp": 0.30079986826754973
+        },
+        {
+            "x": 2.15181841205741,
+            "y": 5.057223411125392,
+            "heading": -5.597189955047307e-7,
+            "angularVelocity": -0.0000028201725975039026,
+            "velocityX": -0.5341125492089794,
+            "velocityY": -2.735124850423542,
+            "timestamp": 0.33422207585283303
+        },
+        {
+            "x": 2.1321820469941515,
+            "y": 4.956668121725741,
+            "heading": -6.57784320236276e-7,
+            "angularVelocity": -0.0000029341365576412115,
+            "velocityX": -0.5875244779329194,
+            "velocityY": -3.008636971185932,
+            "timestamp": 0.36764428343811634
+        },
+        {
+            "x": 2.1107605140888954,
+            "y": 4.846971464532025,
+            "heading": -7.56114109936986e-7,
+            "angularVelocity": -0.0000029420493990798148,
+            "velocityX": -0.6409371029779476,
+            "velocityY": -3.282148760338516,
+            "timestamp": 0.40106649102339964
+        },
+        {
+            "x": 2.087553735835974,
+            "y": 4.728133474365802,
+            "heading": -8.465523402036467e-7,
+            "angularVelocity": -0.0000027059321418483588,
+            "velocityX": -0.6943520470083785,
+            "velocityY": -3.5556595076225634,
+            "timestamp": 0.43448869860868294
+        },
+        {
+            "x": 2.0633251233427066,
+            "y": 4.604042454619442,
+            "heading": -8.465523885072994e-7,
+            "angularVelocity": -1.4452562228577176e-12,
+            "velocityX": -0.724925558303899,
+            "velocityY": -3.712831339152857,
+            "timestamp": 0.46791090619396625
+        },
+        {
+            "x": 2.039088654512122,
+            "y": 4.479952969280304,
+            "heading": -8.465523654496814e-7,
+            "angularVelocity": 6.898891300327124e-13,
+            "velocityX": -0.725160621670614,
+            "velocityY": -3.7127854293461913,
+            "timestamp": 0.5013331137792496
+        },
+        {
+            "x": 2.0148473415026222,
+            "y": 4.355864430051467,
+            "heading": -8.465523174606992e-7,
+            "angularVelocity": 1.4358411666699279e-12,
+            "velocityX": -0.7253055606758912,
+            "velocityY": -3.712757121497041,
+            "timestamp": 0.5347553213645329
+        },
+        {
+            "x": 1.9907968545860597,
+            "y": 4.231738764257757,
+            "heading": -8.465522534990754e-7,
+            "angularVelocity": 1.9137462023851052e-12,
+            "velocityX": -0.7195960008785613,
+            "velocityY": -3.7138679567028667,
+            "timestamp": 0.5681775289498161
+        },
+        {
+            "x": 1.9669031646192998,
+            "y": 4.107582820499775,
+            "heading": -8.465521766194915e-7,
+            "angularVelocity": 2.3002544763334585e-12,
+            "velocityX": -0.7149046006511871,
+            "velocityY": -3.7147738802470687,
+            "timestamp": 0.6015997365350993
+        },
+        {
+            "x": 1.9504985155072192,
+            "y": 3.982217377752346,
+            "heading": -8.465520941748615e-7,
+            "angularVelocity": 2.4667619198655976e-12,
+            "velocityX": -0.4908308067350749,
+            "velocityY": -3.7509623631996885,
+            "timestamp": 0.6350219441203826
+        },
+        {
+            "x": 1.94330794196602,
+            "y": 3.855987816020002,
+            "heading": -8.465520093116816e-7,
+            "angularVelocity": 2.5391254773053977e-12,
+            "velocityX": -0.2151435844765948,
+            "velocityY": -3.7768169984060798,
+            "timestamp": 0.6684441517056658
+        },
+        {
+            "x": 1.9454293697222904,
+            "y": 3.7295714172218672,
+            "heading": -8.465519250884413e-7,
+            "angularVelocity": 2.519978339764665e-12,
+            "velocityX": 0.0634735976376205,
+            "velocityY": -3.7824072056149025,
+            "timestamp": 0.7018663592909491
+        },
+        {
+            "x": 1.9568513812515056,
+            "y": 3.603654207925799,
+            "heading": -8.465518451486746e-7,
+            "angularVelocity": 2.391815832767542e-12,
+            "velocityX": 0.34174916483985474,
+            "velocityY": -3.767471342961152,
+            "timestamp": 0.7352885668762323
+        },
+        {
+            "x": 1.9775120291721782,
+            "y": 3.4789195174340257,
+            "heading": -8.465517739710846e-7,
+            "angularVelocity": 2.129649524174011e-12,
+            "velocityX": 0.6181712523959566,
+            "velocityY": -3.7320901132416737,
+            "timestamp": 0.7687107744615156
+        },
+        {
+            "x": 2.0072992100178335,
+            "y": 3.3560442616931043,
+            "heading": -8.465517105809759e-7,
+            "angularVelocity": 1.8966463157464128e-12,
+            "velocityX": 0.8912391789143821,
+            "velocityY": -3.6764554055067418,
+            "timestamp": 0.8021329820467988
+        },
+        {
+            "x": 2.046051283468891,
+            "y": 3.235695267642527,
+            "heading": -8.465516527951994e-7,
+            "angularVelocity": 1.7289634182989629e-12,
+            "velocityX": 1.1594707905613406,
+            "velocityY": -3.600869085126494,
+            "timestamp": 0.8355551896320821
+        },
+        {
+            "x": 2.0935579534966013,
+            "y": 3.1185256537724477,
+            "heading": -8.46551596321545e-7,
+            "angularVelocity": 1.6897044173558387e-12,
+            "velocityX": 1.421410297528723,
+            "velocityY": -3.5057413120014775,
+            "timestamp": 0.8689773972173653
         },
         {
             "x": 2.1495614051818848,
             "y": 3.005171298980713,
-            "heading": 3.1399983498353623,
-            "angularVelocity": 1.0122205936259764e-11,
-            "velocityX": 1.67595966800623,
-            "velocityY": -3.391427920419207,
-            "timestamp": 0.9024103726161997
-        },
-        {
-            "x": 2.2887605263975965,
-            "y": 2.8026901614733335,
-            "heading": 3.1399983498355466,
-            "angularVelocity": 2.834267287592998e-12,
-            "velocityX": 2.1430749184844986,
-            "velocityY": -3.117349042649137,
-            "timestamp": 0.9673633614337364
-        },
-        {
-            "x": 2.4554470535268806,
-            "y": 2.6221614552311943,
-            "heading": 3.1399983498356696,
-            "angularVelocity": 1.896559244966036e-12,
-            "velocityX": 2.566264157563543,
-            "velocityY": -2.7793748914229504,
-            "timestamp": 1.0323163502512729
-        },
-        {
-            "x": 2.646204500588454,
-            "y": 2.4672853384780784,
-            "heading": 3.139998349835769,
-            "angularVelocity": 1.528915692972437e-12,
-            "velocityX": 2.9368540314174165,
-            "velocityY": -2.384434027946127,
-            "timestamp": 1.0972693390688095
-        },
-        {
-            "x": 2.8571229789617445,
-            "y": 2.341236093184693,
-            "heading": 3.1399983498358592,
-            "angularVelocity": 1.3872590523351098e-12,
-            "velocityX": 3.247248236192457,
-            "velocityY": -1.940622711722675,
-            "timestamp": 1.1622223278863462
-        },
-        {
-            "x": 3.0838578351814623,
-            "y": 2.2465454263799622,
-            "heading": 3.139998349835948,
-            "angularVelocity": 1.37081210529667e-12,
-            "velocityX": 3.490753240880161,
-            "velocityY": -1.4578338661343044,
-            "timestamp": 1.2271753167038828
-        },
-        {
-            "x": 3.3145391093778356,
-            "y": 2.1619217463740292,
-            "heading": 3.1399983498360386,
-            "angularVelocity": 1.3907396736325547e-12,
-            "velocityX": 3.551511306806079,
-            "velocityY": -1.3028450506530576,
-            "timestamp": 1.2921283055214194
-        },
-        {
-            "x": 3.5452163544414024,
-            "y": 2.0772870838321214,
-            "heading": 3.1399983498361257,
-            "angularVelocity": 1.3371940783811797e-12,
-            "velocityX": 3.5514492752906865,
-            "velocityY": -1.3030141350149058,
-            "timestamp": 1.357081294338956
-        },
-        {
-            "x": 3.7758941865765565,
-            "y": 1.9926540214142978,
-            "heading": 3.139998349836204,
-            "angularVelocity": 1.2081341732104707e-12,
-            "velocityX": 3.5514583136980007,
-            "velocityY": -1.302989499904737,
-            "timestamp": 1.4220342831564927
-        },
-        {
-            "x": 4.006572310105761,
-            "y": 1.908021753240333,
-            "heading": 3.1399983498362696,
-            "angularVelocity": 1.0139841411422692e-12,
-            "velocityX": 3.5514627999275414,
-            "velocityY": -1.3029772719259731,
-            "timestamp": 1.4869872719740294
-        },
-        {
-            "x": 4.237248920507999,
-            "y": 1.8233853608613613,
-            "heading": 3.1399983498363224,
-            "angularVelocity": 8.135622214219814e-13,
-            "velocityX": 3.5514395042053684,
-            "velocityY": -1.3030407671488682,
-            "timestamp": 1.551940260791566
-        },
-        {
-            "x": 4.46792193477352,
-            "y": 1.7387391679278503,
-            "heading": 3.1399983498363646,
-            "angularVelocity": 6.458697792259404e-13,
-            "velocityX": 3.551384138968564,
-            "velocityY": -1.3031916540771904,
-            "timestamp": 1.6168932496091026
-        },
-        {
-            "x": 4.698662422545312,
-            "y": 1.6542770762611563,
-            "heading": 3.139998349836401,
-            "angularVelocity": 5.573297491220617e-13,
-            "velocityX": 3.5524229442068638,
-            "velocityY": -1.3003572769873404,
-            "timestamp": 1.6818462384266393
+            "heading": -8.465515057383081e-7,
+            "angularVelocity": 2.7102707524537993e-12,
+            "velocityX": 1.6756359238847198,
+            "velocityY": -3.3915878986274275,
+            "timestamp": 0.9023996048026486
+        },
+        {
+            "x": 2.288743031901554,
+            "y": 2.8026748842621148,
+            "heading": -8.46551464739476e-7,
+            "angularVelocity": 6.312009526868921e-13,
+            "velocityX": 2.142782210187666,
+            "velocityY": -3.117550249366458,
+            "timestamp": 0.9673533019435205
+        },
+        {
+            "x": 2.455414708004788,
+            "y": 2.6221288200825,
+            "heading": -8.46551435217149e-7,
+            "angularVelocity": 4.5451345461171864e-13,
+            "velocityX": 2.566007532127087,
+            "velocityY": -2.779611817758129,
+            "timestamp": 1.0323069990843925
+        },
+        {
+            "x": 2.646160177401748,
+            "y": 2.4672337011827246,
+            "heading": -8.465514075797998e-7,
+            "angularVelocity": 4.254931139136813e-13,
+            "velocityX": 2.9366376017527633,
+            "velocityY": -2.384700574689416,
+            "timestamp": 1.0972606962252645
+        },
+        {
+            "x": 2.857069726290126,
+            "y": 2.341164292711998,
+            "heading": -8.465513787578765e-7,
+            "angularVelocity": 4.4373032078954734e-13,
+            "velocityX": 3.2470753501649696,
+            "velocityY": -1.9409119729950552,
+            "timestamp": 1.1622143933661364
+        },
+        {
+            "x": 3.0838173053505575,
+            "y": 2.2464971410417167,
+            "heading": -8.465513471236816e-7,
+            "angularVelocity": 4.870268741320482e-13,
+            "velocityX": 3.490911049579749,
+            "velocityY": -1.457455939200772,
+            "timestamp": 1.2271680905070084
+        },
+        {
+            "x": 3.3145112489857773,
+            "y": 2.1619002222864983,
+            "heading": -8.465513138187862e-7,
+            "angularVelocity": 5.127482864627404e-13,
+            "velocityX": 3.5516676307835056,
+            "velocityY": -1.3024188380298123,
+            "timestamp": 1.2921217876478803
+        },
+        {
+            "x": 3.5452013267301767,
+            "y": 2.0772927620920076,
+            "heading": -8.465512839726007e-7,
+            "angularVelocity": 4.594994306997602e-13,
+            "velocityX": 3.5516081131487938,
+            "velocityY": -1.302581129610464,
+            "timestamp": 1.3570754847887523
+        },
+        {
+            "x": 3.77589133724735,
+            "y": 1.9926851185956267,
+            "heading": -8.465512625373994e-7,
+            "angularVelocity": 3.3000743037279784e-13,
+            "velocityX": 3.5516070781466733,
+            "velocityY": -1.302583951649345,
+            "timestamp": 1.4220291819296242
+        },
+        {
+            "x": 4.006581361028944,
+            "y": 1.9080775112659911,
+            "heading": -8.465512535495126e-7,
+            "angularVelocity": 1.3837377280671698e-13,
+            "velocityX": 3.5516072823601217,
+            "velocityY": -1.302583394841325,
+            "timestamp": 1.4869828790704962
+        },
+        {
+            "x": 4.2372714040114055,
+            "y": 1.823469956289169,
+            "heading": -8.465512575529088e-7,
+            "angularVelocity": -6.163459506326445e-14,
+            "velocityX": 3.551607577968653,
+            "velocityY": -1.3025825888392473,
+            "timestamp": 1.5519365762113682
+        },
+        {
+            "x": 4.467961461705261,
+            "y": 1.7388624414245573,
+            "heading": -8.465512722492451e-7,
+            "angularVelocity": -2.2625863777166434e-13,
+            "velocityX": 3.5516078044591044,
+            "velocityY": -1.3025819712885556,
+            "timestamp": 1.6168902733522401
+        },
+        {
+            "x": 4.698651553926265,
+            "y": 1.6542550207002664,
+            "heading": -8.465512926518018e-7,
+            "angularVelocity": -3.141092229421582e-13,
+            "velocityX": 3.551608336024854,
+            "velocityY": -1.3025805219428788,
+            "timestamp": 1.681843970493112
         },
         {
             "x": 4.929586887359619,
             "y": 1.5703192949295044,
-            "heading": 3.139998349836444,
-            "angularVelocity": 6.621983947605345e-13,
-            "velocityX": 3.5552554088546553,
-            "velocityY": -1.2925930408850796,
-            "timestamp": 1.746799227244176
-        },
-        {
-            "x": 5.059491461224377,
-            "y": 1.5327116535816125,
-            "heading": 3.1399983498365303,
-            "angularVelocity": 2.411376289586005e-12,
-            "velocityX": 3.6337293619618993,
-            "velocityY": -1.0519721248789429,
-            "timestamp": 1.7825488830178295
-        },
-        {
-            "x": 5.189524196366832,
-            "y": 1.4955495654455135,
-            "heading": 3.1399983498366377,
-            "angularVelocity": 3.0099354211020166e-12,
-            "velocityX": 3.6373143273250914,
-            "velocityY": -1.0395089779716884,
-            "timestamp": 1.8182985387914832
-        },
-        {
-            "x": 5.319583815273274,
-            "y": 1.4584816742976305,
-            "heading": 3.1399983498367723,
-            "angularVelocity": 3.767098328405853e-12,
-            "velocityX": 3.6380663279640872,
-            "velocityY": -1.036874071811995,
-            "timestamp": 1.8540481945651368
-        },
-        {
-            "x": 5.4496589768626915,
-            "y": 1.4214683610582035,
-            "heading": 3.139998349836922,
-            "angularVelocity": 4.1821295987378e-12,
-            "velocityX": 3.6385010925132435,
-            "velocityY": -1.0353474023301,
-            "timestamp": 1.8897978503387904
-        },
-        {
-            "x": 5.579738820656374,
-            "y": 1.3844715066642421,
-            "heading": 3.139998349837078,
-            "angularVelocity": 4.36086747938318e-12,
-            "velocityX": 3.6386320645231356,
-            "velocityY": -1.0348870106108623,
-            "timestamp": 1.925547506112444
-        },
-        {
-            "x": 5.709817887256256,
-            "y": 1.3474719197526863,
-            "heading": 3.1399983498372332,
-            "angularVelocity": 4.346123552895358e-12,
-            "velocityX": 3.6386103246271904,
-            "velocityY": -1.0349634454051337,
-            "timestamp": 1.9612971618860977
-        },
-        {
-            "x": 5.839896082809478,
-            "y": 1.3104692706317995,
-            "heading": 3.139998349837383,
-            "angularVelocity": 4.183764792386501e-12,
-            "velocityX": 3.638585959451041,
-            "velocityY": -1.0350491024349304,
-            "timestamp": 1.9970468176597513
-        },
-        {
-            "x": 5.969973859581082,
-            "y": 1.2734651493862033,
-            "heading": 3.139998349837523,
-            "angularVelocity": 3.914068769730236e-12,
-            "velocityX": 3.6385742451671077,
-            "velocityY": -1.0350902811450293,
-            "timestamp": 2.032796473433405
-        },
-        {
-            "x": 6.100051488327663,
-            "y": 1.2364605078211643,
-            "heading": 3.1399983498376502,
-            "angularVelocity": 3.5614211180657343e-12,
-            "velocityX": 3.6385701045671635,
-            "velocityY": -1.0351048356752235,
-            "timestamp": 2.068546129207059
-        },
-        {
-            "x": 6.230129108868512,
-            "y": 1.199455837420786,
-            "heading": 3.1399983498377644,
-            "angularVelocity": 3.192303325422517e-12,
-            "velocityX": 3.6385698750339617,
-            "velocityY": -1.0351056422659504,
-            "timestamp": 2.1042957849807125
-        },
-        {
-            "x": 6.360206785724816,
-            "y": 1.162451364982862,
-            "heading": 3.1399983498378674,
-            "angularVelocity": 2.8800276139093587e-12,
-            "velocityX": 3.638571450306583,
-            "velocityY": -1.0351001048013342,
-            "timestamp": 2.140045440754366
-        },
-        {
-            "x": 6.490284555094277,
-            "y": 1.1254472177490595,
-            "heading": 3.1399983498379616,
-            "angularVelocity": 2.621534784795507e-12,
-            "velocityX": 3.638574038112148,
-            "velocityY": -1.0350910080949738,
-            "timestamp": 2.1757950965280197
-        },
-        {
-            "x": 6.6203624349981105,
-            "y": 1.0884434590721859,
-            "heading": 3.1399983498380477,
-            "angularVelocity": 2.4107623793093176e-12,
-            "velocityX": 3.638577130012462,
-            "velocityY": -1.0350801392650475,
-            "timestamp": 2.2115447523016734
-        },
-        {
-            "x": 6.75044029323431,
-            "y": 1.0514396242316386,
-            "heading": 3.1399983498381276,
-            "angularVelocity": 2.2461604882641728e-12,
-            "velocityX": 3.6385765239189465,
-            "velocityY": -1.0350822697380073,
-            "timestamp": 2.247294408075327
-        },
-        {
-            "x": 6.880516952547443,
-            "y": 1.0144315751292232,
-            "heading": 3.139998349838195,
-            "angularVelocity": 1.892677955933963e-12,
-            "velocityX": 3.6385429872864736,
-            "velocityY": -1.0352001523239789,
-            "timestamp": 2.2830440638489806
-        },
-        {
-            "x": 7.003514384307231,
-            "y": 0.97944241190024,
-            "heading": 3.1399986104136794,
-            "angularVelocity": 0.000007288894915666013,
-            "velocityX": 3.440520729445303,
-            "velocityY": -0.9787272764389812,
-            "timestamp": 2.3187937196226343
-        },
-        {
-            "x": 7.116262004659963,
-            "y": 0.9473688227429191,
-            "heading": 3.139998845492451,
-            "angularVelocity": 0.000006575693298715729,
-            "velocityX": 3.1538099574045058,
-            "velocityY": -0.8971719716796405,
-            "timestamp": 2.354543375396288
-        },
-        {
-            "x": 7.218759831907218,
-            "y": 0.9182109395297621,
-            "heading": 3.1399990575434797,
-            "angularVelocity": 0.00000593155441577883,
-            "velocityX": 2.8670996973009437,
-            "velocityY": -0.8156129781434808,
-            "timestamp": 2.3902930311699415
-        },
-        {
-            "x": 7.3110078714654865,
-            "y": 0.8919688038160335,
-            "heading": 3.1399992473559544,
-            "angularVelocity": 0.000005309490974163329,
-            "velocityX": 2.5803895887090773,
-            "velocityY": -0.7340528222110998,
-            "timestamp": 2.426042686943595
-        },
-        {
-            "x": 7.393006125886103,
-            "y": 0.8686424358274811,
-            "heading": 3.139999415401417,
-            "angularVelocity": 0.000004700617645814799,
-            "velocityX": 2.293679551483878,
-            "velocityY": -0.6524921005180893,
-            "timestamp": 2.461792342717249
-        },
-        {
-            "x": 7.464754596626899,
-            "y": 0.8482318474429659,
-            "heading": 3.1399995619997028,
-            "angularVelocity": 0.000004100690828648421,
-            "velocityX": 2.0069695550376028,
-            "velocityY": -0.5709310465460533,
-            "timestamp": 2.4975419984909024
-        },
-        {
-            "x": 7.526253284614184,
-            "y": 0.8307370464214682,
-            "heading": 3.1399996873763563,
-            "angularVelocity": 0.00000350707302580956,
-            "velocityX": 1.7202595845023039,
-            "velocityY": -0.4893697755375005,
-            "timestamp": 2.533291654264556
-        },
-        {
-            "x": 7.577502190476697,
-            "y": 0.8161580381894111,
-            "heading": 3.139999791690666,
-            "angularVelocity": 0.0000029179108844478315,
-            "velocityX": 1.433549631554256,
-            "velocityY": -0.40780835273943755,
-            "timestamp": 2.5690413100382097
-        },
-        {
-            "x": 7.618501314660242,
-            "y": 0.8044948267260937,
-            "heading": 3.1399998750527174,
-            "angularVelocity": 0.000002331828092755085,
-            "velocityX": 1.1468396910764296,
-            "velocityY": -0.3262468186312345,
-            "timestamp": 2.6047909658118633
-        },
-        {
-            "x": 7.649250657490367,
-            "y": 0.7957474150520292,
-            "heading": 3.139999937534455,
-            "angularVelocity": 0.0000017477577406037154,
-            "velocityX": 0.860129759704899,
-            "velocityY": -0.24468520003233896,
-            "timestamp": 2.640540621585517
-        },
-        {
-            "x": 7.669750219209449,
-            "y": 0.789915805520138,
-            "heading": 3.1399999791776474,
-            "angularVelocity": 0.0000011648557571115021,
-            "velocityX": 0.5734198351132136,
-            "velocityY": -0.16312351561685162,
-            "timestamp": 2.6762902773591706
+            "heading": -8.465513064459796e-7,
+            "angularVelocity": -2.1236938120360531e-13,
+            "velocityX": 3.555383967330626,
+            "velocityY": -1.2922393869021438,
+            "timestamp": 1.746797667633984
+        },
+        {
+            "x": 5.059495672066219,
+            "y": 1.5327262216225606,
+            "heading": -8.465512750899748e-7,
+            "angularVelocity": 8.770995258558909e-13,
+            "velocityX": 3.633847296905686,
+            "velocityY": -1.051564666141291,
+            "timestamp": 1.7825473219513848
+        },
+        {
+            "x": 5.189548300230332,
+            "y": 1.4956338313822377,
+            "heading": -8.465512186056332e-7,
+            "angularVelocity": 1.579996862146135e-12,
+            "velocityX": 3.637870929029155,
+            "velocityY": -1.0375594099736098,
+            "timestamp": 1.8182969762687855
+        },
+        {
+            "x": 5.319623170079142,
+            "y": 1.4586195133360007,
+            "heading": -8.46551136487956e-7,
+            "angularVelocity": 2.2970201754243682e-12,
+            "velocityX": 3.638493080071459,
+            "velocityY": -1.0353755512600933,
+            "timestamp": 1.8540466305861862
+        },
+        {
+            "x": 5.4496989055100356,
+            "y": 1.42160823723968,
+            "heading": -8.465510387564672e-7,
+            "angularVelocity": 2.7337743725450157e-12,
+            "velocityX": 3.6385172923918394,
+            "velocityY": -1.0352904609291835,
+            "timestamp": 1.889796284903587
+        },
+        {
+            "x": 5.57977446957584,
+            "y": 1.3845963588884114,
+            "heading": -8.465509335637302e-7,
+            "angularVelocity": 2.9424826347181554e-12,
+            "velocityX": 3.6385124989164392,
+            "velocityY": -1.03530730738433,
+            "timestamp": 1.9255459392209877
+        },
+        {
+            "x": 5.709850057975644,
+            "y": 1.3475845660573411,
+            "heading": -8.465508277220688e-7,
+            "angularVelocity": 2.960634540295608e-12,
+            "velocityX": 3.6385131795943324,
+            "velocityY": -1.0353049151878095,
+            "timestamp": 1.9612955935383884
+        },
+        {
+            "x": 5.839925662966751,
+            "y": 1.3105728315354166,
+            "heading": -8.465507264753305e-7,
+            "angularVelocity": 2.832103981671134e-12,
+            "velocityX": 3.6385136436912036,
+            "velocityY": -1.0353032841469862,
+            "timestamp": 1.9970452478557892
+        },
+        {
+            "x": 5.970001275498046,
+            "y": 1.273561123513075,
+            "heading": -8.465506337302748e-7,
+            "angularVelocity": 2.594292378130792e-12,
+            "velocityX": 3.638513854607602,
+            "velocityY": -1.0353025428927405,
+            "timestamp": 2.03279490217319
+        },
+        {
+            "x": 6.100076894564937,
+            "y": 1.2365494384597497,
+            "heading": -8.46550552278632e-7,
+            "angularVelocity": 2.2783896618382965e-12,
+            "velocityX": 3.6385140374232323,
+            "velocityY": -1.0353019003965735,
+            "timestamp": 2.0685445564905907
+        },
+        {
+            "x": 6.230152520105601,
+            "y": 1.1995377761581834,
+            "heading": -8.465504825990077e-7,
+            "angularVelocity": 1.9490992420524453e-12,
+            "velocityX": 3.6385142185095396,
+            "velocityY": -1.03530126397757,
+            "timestamp": 2.1042942108079914
+        },
+        {
+            "x": 6.3602281516940815,
+            "y": 1.16252613511139,
+            "heading": -8.465504225140753e-7,
+            "angularVelocity": 1.680713660601537e-12,
+            "velocityX": 3.6385143876808734,
+            "velocityY": -1.0353006694327234,
+            "timestamp": 2.140043865125392
+        },
+        {
+            "x": 6.490303788775648,
+            "y": 1.1255145133698101,
+            "heading": -8.465503700121188e-7,
+            "angularVelocity": 1.4686003981542915e-12,
+            "velocityX": 3.638514541335137,
+            "velocityY": -1.0353001294215285,
+            "timestamp": 2.175793519442793
+        },
+        {
+            "x": 6.62037943067546,
+            "y": 1.0885029085617548,
+            "heading": -8.465503232940394e-7,
+            "angularVelocity": 1.306812060469801e-12,
+            "velocityX": 3.638514676112538,
+            "velocityY": -1.0352996557519205,
+            "timestamp": 2.2115431737601936
+        },
+        {
+            "x": 6.7504550766965234,
+            "y": 1.0514913182376744,
+            "heading": -8.465502808582129e-7,
+            "angularVelocity": 1.187027603302574e-12,
+            "velocityX": 3.638514791393393,
+            "velocityY": -1.0352992506018621,
+            "timestamp": 2.2472928280775943
+        },
+        {
+            "x": 6.880530727542945,
+            "y": 1.0144797448710672,
+            "heading": -8.465502433332782e-7,
+            "angularVelocity": 1.0496586699357012e-12,
+            "velocityX": 3.6385149263697616,
+            "velocityY": -1.0352987762623522,
+            "timestamp": 2.283042482394995
+        },
+        {
+            "x": 7.003525953233288,
+            "y": 0.9794828656529073,
+            "heading": -7.145353891597533e-7,
+            "angularVelocity": 0.000003692758900357643,
+            "velocityX": 3.440459160761053,
+            "velocityY": -0.9789431502593731,
+            "timestamp": 2.318792136712396
+        },
+        {
+            "x": 7.116271603762692,
+            "y": 0.9474023877264399,
+            "heading": -5.944969754078434e-7,
+            "angularVelocity": 0.0000033577503345576707,
+            "velocityX": 3.153754985388111,
+            "velocityY": -0.8973647029323183,
+            "timestamp": 2.3545417910297965
+        },
+        {
+            "x": 7.218767661219051,
+            "y": 0.9182383156679585,
+            "heading": -4.859087151281335e-7,
+            "angularVelocity": 0.0000030374632243726353,
+            "velocityX": 2.867050308972391,
+            "velocityY": -0.8157861275958178,
+            "timestamp": 2.3902914453471973
+        },
+        {
+            "x": 7.311014119553375,
+            "y": 0.8919906507276494,
+            "heading": -3.8850367492824467e-7,
+            "angularVelocity": 0.000002724642854703664,
+            "velocityX": 2.580345463352478,
+            "velocityY": -0.7342075172887301,
+            "timestamp": 2.426041099664598
+        },
+        {
+            "x": 7.393010975686544,
+            "y": 0.8686593933386082,
+            "heading": -3.0210953963079075e-7,
+            "angularVelocity": 0.0000024166425366030095,
+            "velocityX": 2.293640531602499,
+            "velocityY": -0.6526288948669626,
+            "timestamp": 2.4617907539819988
+        },
+        {
+            "x": 7.464758227730078,
+            "y": 0.8482445436165693,
+            "heading": -2.2661033918382436e-7,
+            "angularVelocity": 0.000002111886167406066,
+            "velocityX": 2.006935547027439,
+            "velocityY": -0.5710502692078399,
+            "timestamp": 2.4975404082993995
+        },
+        {
+            "x": 7.526255874392654,
+            "y": 0.8307461015250325,
+            "heading": -1.6193015142138781e-7,
+            "angularVelocity": 0.0000018092535156938384,
+            "velocityX": 1.7202305263310733,
+            "velocityY": -0.48947164456971237,
+            "timestamp": 2.5332900626168002
+        },
+        {
+            "x": 7.577503914725456,
+            "y": 0.8161640669449958,
+            "heading": -1.0802379214331377e-7,
+            "angularVelocity": 0.0000015078847699136293,
+            "velocityX": 1.4335254790941394,
+            "velocityY": -0.4078930232603403,
+            "timestamp": 2.569039716934201
+        },
+        {
+            "x": 7.618502347994633,
+            "y": 0.8044984397090525,
+            "heading": -6.487055743300418e-8,
+            "angularVelocity": 0.0000012070951602673883,
+            "velocityX": 1.1468204113297347,
+            "velocityY": -0.3263144066337203,
+            "timestamp": 2.6047893712516017
+        },
+        {
+            "x": 7.649251173610278,
+            "y": 0.7957492196197445,
+            "heading": -3.246962719134405e-8,
+            "angularVelocity": 9.063284906782187e-7,
+            "velocityX": 0.8601153270642448,
+            "velocityY": -0.24473579553046104,
+            "timestamp": 2.6405390255690024
+        },
+        {
+            "x": 7.669750391083677,
+            "y": 0.7899164064601075,
+            "heading": -1.0836483282929717e-8,
+            "angularVelocity": 6.051287577519143e-7,
+            "velocityX": 0.573410229128317,
+            "velocityY": -0.16315719049619,
+            "timestamp": 2.676288679886403
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 5.824490440069194e-7,
-            "velocityX": 0.2867099156267607,
-            "velocityY": -0.0815617789049158,
-            "timestamp": 2.712039933132824
+            "heading": 2.0494644799121527e-30,
+            "angularVelocity": 3.031213445055282e-7,
+            "velocityX": 0.28670511958863415,
+            "velocityY": -0.08157859189950141,
+            "timestamp": 2.712038334203804
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": -6.025226692032396e-31,
-            "velocityX": 4.372989448335815e-32,
-            "velocityY": 7.524777583668013e-32,
-            "timestamp": 2.747789588906478
+            "heading": -1.7051999701665118e-29,
+            "angularVelocity": -2.5846596071248955e-28,
+            "velocityX": 4.524644517512267e-29,
+            "velocityY": 1.710870028377411e-29,
+            "timestamp": 2.7477879885212046
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.traj
index ac17f8d7..cfa6b046 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.traj	
@@ -3,623 +3,623 @@
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": -2.730529759762368e-29,
+            "velocityY": 6.892560738534845e-27,
             "timestamp": 0
         },
         {
-            "x": 2.2482118663055717,
-            "y": 5.55085896880981,
-            "heading": 3.1399999903330857,
-            "angularVelocity": -2.8923280730678937e-7,
-            "velocityX": -0.05350072563170815,
-            "velocityY": -0.2734984543883791,
-            "timestamp": 0.03342260639319256
-        },
-        {
-            "x": 2.24463557685663,
-            "y": 5.5325769109933445,
-            "heading": 3.1399999700354155,
-            "angularVelocity": -6.073036282303753e-7,
-            "velocityX": -0.10700211129830166,
-            "velocityY": -0.5469967722250598,
-            "timestamp": 0.06684521278638512
-        },
-        {
-            "x": 2.2392711054637595,
-            "y": 5.505153831967315,
-            "heading": 3.139999938021822,
-            "angularVelocity": -9.578425018869592e-7,
-            "velocityX": -0.16050428054878876,
-            "velocityY": -0.8204949279944175,
-            "timestamp": 0.1002678191795777
-        },
-        {
-            "x": 2.2321184205712044,
-            "y": 5.468589738256669,
-            "heading": 3.139999893047122,
-            "angularVelocity": -0.0000013456371104363374,
-            "velocityX": -0.2140073939433933,
-            "velocityY": -1.0939928885381913,
-            "timestamp": 0.13369042557277025
-        },
-        {
-            "x": 2.2231774834707245,
-            "y": 5.422884637863401,
-            "heading": 3.1399998336497137,
-            "angularVelocity": -0.000001777162647778402,
-            "velocityX": -0.2675116654835123,
-            "velocityY": -1.3674906096633432,
-            "timestamp": 0.1671130319659628
-        },
-        {
-            "x": 2.2124482456246803,
-            "y": 5.368038540819271,
-            "heading": 3.139999758066302,
-            "angularVelocity": -0.0000022614457562380146,
-            "velocityX": -0.3210173892634857,
-            "velocityY": -1.6409880306395483,
-            "timestamp": 0.20053563835915536
-        },
-        {
-            "x": 2.199930644465073,
-            "y": 5.30405146005317,
-            "heading": 3.139999664095155,
-            "angularVelocity": -0.0000028116043956655657,
-            "velocityX": -0.3745249850698391,
-            "velocityY": -1.9144850647849765,
-            "timestamp": 0.23395824475234792
-        },
-        {
-            "x": 2.1856245964060905,
-            "y": 5.230923412833771,
-            "heading": 3.1399995488597034,
-            "angularVelocity": -0.0000034478296167107145,
-            "velocityX": -0.4280350817528267,
-            "velocityY": -2.1879815822539745,
-            "timestamp": 0.2673808511455405
-        },
-        {
-            "x": 2.1695299843090114,
-            "y": 5.148654423357543,
-            "heading": 3.139999408375343,
-            "angularVelocity": -0.000004203273662517842,
-            "velocityX": -0.4815486832110918,
-            "velocityY": -2.461477375767798,
-            "timestamp": 0.30080345753873305
-        },
-        {
-            "x": 2.15164663254415,
-            "y": 5.057244527897619,
-            "heading": 3.139999236662074,
-            "angularVelocity": -0.000005137638535462322,
-            "velocityX": -0.535067539486715,
-            "velocityY": -2.7349720839942284,
-            "timestamp": 0.3342260639319256
-        },
-        {
-            "x": 2.1319742493327887,
-            "y": 4.956693786709351,
-            "heading": 3.1399990236359914,
-            "angularVelocity": -0.000006373712524248939,
-            "velocityX": -0.5885951257435309,
-            "velocityY": -3.008464989388312,
-            "timestamp": 0.36764867032511817
-        },
-        {
-            "x": 2.110512257081521,
-            "y": 4.847002319075526,
-            "heading": 3.13999874874802,
-            "angularVelocity": -0.000008224612190539215,
-            "velocityX": -0.6421399935196433,
-            "velocityY": -3.28195432585699,
-            "timestamp": 0.4010712767183107
-        },
-        {
-            "x": 2.087258999095236,
-            "y": 4.728170467468909,
-            "heading": 3.139998349832889,
-            "angularVelocity": -0.00001193548864433131,
-            "velocityX": -0.6957344293958355,
-            "velocityY": -3.555433415585576,
-            "timestamp": 0.4344938831115033
-        },
-        {
-            "x": 2.0629644962421034,
-            "y": 4.604090796237193,
-            "heading": 3.1399983498325463,
-            "angularVelocity": -1.0248884056674418e-11,
-            "velocityX": -0.7268883391614586,
-            "velocityY": -3.712447490549055,
-            "timestamp": 0.46791648950469583
-        },
-        {
-            "x": 2.0387448816492295,
-            "y": 4.479996482972041,
-            "heading": 3.139998349833487,
-            "angularVelocity": 2.8141899837441888e-11,
-            "velocityX": -0.7246476922026518,
-            "velocityY": -3.712885578275693,
-            "timestamp": 0.5013390958978884
-        },
-        {
-            "x": 2.0146234361555044,
-            "y": 4.355883050210464,
-            "heading": 3.139998349833582,
-            "angularVelocity": 2.8336640394810287e-12,
-            "velocityX": -0.7217104856241171,
-            "velocityY": -3.713457631102453,
-            "timestamp": 0.534761702291081
-        },
-        {
-            "x": 1.9906204010106838,
-            "y": 4.231746663125483,
-            "heading": 3.1399983498337,
-            "angularVelocity": 3.5336881649516105e-12,
-            "velocityX": -0.7181676635393196,
-            "velocityY": -3.714144421443354,
-            "timestamp": 0.5681843086842736
-        },
-        {
-            "x": 1.9667619480914416,
-            "y": 4.107582407046251,
-            "heading": 3.139998349833824,
-            "angularVelocity": 3.708930519775865e-12,
-            "velocityX": -0.7138417823660727,
-            "velocityY": -3.7149782580858504,
-            "timestamp": 0.6016069150774662
-        },
-        {
-            "x": 1.9503914931986956,
-            "y": 3.982210973196183,
-            "heading": 3.1399983498339594,
-            "angularVelocity": 4.046203674508551e-12,
-            "velocityX": -0.4898018634439243,
-            "velocityY": -3.751096858670778,
-            "timestamp": 0.6350295214706588
-        },
-        {
-            "x": 1.943224502199385,
-            "y": 3.8559785593201936,
-            "heading": 3.1399983498340998,
-            "angularVelocity": 4.203331220184306e-12,
-            "velocityX": -0.21443543076194166,
-            "velocityY": -3.7768572681307018,
-            "timestamp": 0.6684521278638514
-        },
-        {
-            "x": 1.9453593714612354,
-            "y": 3.7295608780305693,
-            "heading": 3.1399983498342436,
-            "angularVelocity": 4.304427970302576e-12,
-            "velocityX": 0.06387500833733431,
-            "velocityY": -3.7824004448488933,
-            "timestamp": 0.701874734257044
-        },
-        {
-            "x": 1.9567930300665735,
-            "y": 3.6036432110345795,
-            "heading": 3.139998349834389,
-            "angularVelocity": 4.350609924565741e-12,
-            "velocityX": 0.3420935659816486,
-            "velocityY": -3.767440082758668,
-            "timestamp": 0.7352973406502367
-        },
-        {
-            "x": 1.9774653925716308,
-            "y": 3.4789089323576285,
-            "heading": 3.1399983498345336,
-            "angularVelocity": 4.327887690330789e-12,
-            "velocityX": 0.6185143750313882,
-            "velocityY": -3.732033259450771,
-            "timestamp": 0.7687199470434293
-        },
-        {
-            "x": 2.0072643339986427,
-            "y": 3.3560349759367867,
-            "heading": 3.1399983498346824,
-            "angularVelocity": 4.434032141487508e-12,
-            "velocityX": 0.8915804194501273,
-            "velocityY": -3.6763726615238776,
-            "timestamp": 0.8021425534366219
-        },
-        {
-            "x": 2.0460281463567345,
-            "y": 3.235688177587346,
-            "heading": 3.1399983498348414,
-            "angularVelocity": 4.757708405328401e-12,
-            "velocityX": 1.1598081819894186,
-            "velocityY": -3.600760423457161,
-            "timestamp": 0.8355651598298145
-        },
-        {
-            "x": 2.093546464867386,
-            "y": 3.118521659475836,
-            "heading": 3.139998349835024,
-            "angularVelocity": 5.453307843013083e-12,
-            "velocityX": 1.421741857944163,
-            "velocityY": -3.5056068558230677,
-            "timestamp": 0.8689877662230071
+            "x": 2.24821488195175,
+            "y": 5.550858605907564,
+            "heading": -1.0860081979744884e-8,
+            "angularVelocity": -3.249360952601713e-7,
+            "velocityX": -0.053411135206172675,
+            "velocityY": -0.27351257600550494,
+            "timestamp": 0.0334222075852833
+        },
+        {
+            "x": 2.2446446457975586,
+            "y": 5.532575817984525,
+            "heading": -3.253781151640391e-8,
+            "angularVelocity": -6.486025430910346e-7,
+            "velocityX": -0.10682227213842765,
+            "velocityY": -0.5470251441769277,
+            "timestamp": 0.0668444151705666
+        },
+        {
+            "x": 2.2392892913368425,
+            "y": 5.505151636566201,
+            "heading": -6.492851000422694e-8,
+            "angularVelocity": -9.691369834959166e-7,
+            "velocityX": -0.1602334150722015,
+            "velocityY": -0.8205377023156053,
+            "timestamp": 0.10026662275584991
+        },
+        {
+            "x": 2.2321488181807467,
+            "y": 5.468586062083933,
+            "heading": -1.0785000725838172e-7,
+            "angularVelocity": -0.000001284220850944888,
+            "velocityX": -0.21364456964054193,
+            "velocityY": -1.0940502475484135,
+            "timestamp": 0.1336888303411332
+        },
+        {
+            "x": 2.2232232256851456,
+            "y": 5.42287909509794,
+            "heading": -1.6102300741445552e-7,
+            "angularVelocity": -0.0000015909481231905412,
+            "velocityX": -0.26705574348127187,
+            "velocityY": -1.3675627760193463,
+            "timestamp": 0.16711103792641652
+        },
+        {
+            "x": 2.212512512848178,
+            "y": 5.368030736346982,
+            "heading": -2.2404505707142586e-7,
+            "angularVelocity": -0.0000018856338989468898,
+            "velocityX": -0.32046694729784664,
+            "velocityY": -1.6410752823864576,
+            "timestamp": 0.20053324551169982
+        },
+        {
+            "x": 2.200016678146418,
+            "y": 5.304040986826963,
+            "heading": -2.963548114247805e-7,
+            "angularVelocity": -0.000002163524111560841,
+            "velocityX": -0.3738781966956197,
+            "velocityY": -1.9145877589558649,
+            "timestamp": 0.23395545309698312
+        },
+        {
+            "x": 2.1857357192573095,
+            "y": 5.230909847922857,
+            "heading": -3.7718073304542066e-7,
+            "angularVelocity": -0.0000024183297502514874,
+            "velocityX": -0.4272895155847271,
+            "velocityY": -2.1881001940865983,
+            "timestamp": 0.2673776606822664
+        },
+        {
+            "x": 2.169669632551276,
+            "y": 5.148637321647842,
+            "heading": -4.654625982128869e-7,
+            "angularVelocity": -0.0000026414132559778984,
+            "velocityX": -0.48070094307146977,
+            "velocityY": -2.4616125689825754,
+            "timestamp": 0.30079986826754973
+        },
+        {
+            "x": 2.15181841205741,
+            "y": 5.057223411125392,
+            "heading": -5.597189955047307e-7,
+            "angularVelocity": -0.0000028201725975039026,
+            "velocityX": -0.5341125492089794,
+            "velocityY": -2.735124850423542,
+            "timestamp": 0.33422207585283303
+        },
+        {
+            "x": 2.1321820469941515,
+            "y": 4.956668121725741,
+            "heading": -6.57784320236276e-7,
+            "angularVelocity": -0.0000029341365576412115,
+            "velocityX": -0.5875244779329194,
+            "velocityY": -3.008636971185932,
+            "timestamp": 0.36764428343811634
+        },
+        {
+            "x": 2.1107605140888954,
+            "y": 4.846971464532025,
+            "heading": -7.56114109936986e-7,
+            "angularVelocity": -0.0000029420493990798148,
+            "velocityX": -0.6409371029779476,
+            "velocityY": -3.282148760338516,
+            "timestamp": 0.40106649102339964
+        },
+        {
+            "x": 2.087553735835974,
+            "y": 4.728133474365802,
+            "heading": -8.465523402036467e-7,
+            "angularVelocity": -0.0000027059321418483588,
+            "velocityX": -0.6943520470083785,
+            "velocityY": -3.5556595076225634,
+            "timestamp": 0.43448869860868294
+        },
+        {
+            "x": 2.0633251233427066,
+            "y": 4.604042454619442,
+            "heading": -8.465523885072994e-7,
+            "angularVelocity": -1.4452562228577176e-12,
+            "velocityX": -0.724925558303899,
+            "velocityY": -3.712831339152857,
+            "timestamp": 0.46791090619396625
+        },
+        {
+            "x": 2.039088654512122,
+            "y": 4.479952969280304,
+            "heading": -8.465523654496814e-7,
+            "angularVelocity": 6.898891300327124e-13,
+            "velocityX": -0.725160621670614,
+            "velocityY": -3.7127854293461913,
+            "timestamp": 0.5013331137792496
+        },
+        {
+            "x": 2.0148473415026222,
+            "y": 4.355864430051467,
+            "heading": -8.465523174606992e-7,
+            "angularVelocity": 1.4358411666699279e-12,
+            "velocityX": -0.7253055606758912,
+            "velocityY": -3.712757121497041,
+            "timestamp": 0.5347553213645329
+        },
+        {
+            "x": 1.9907968545860597,
+            "y": 4.231738764257757,
+            "heading": -8.465522534990754e-7,
+            "angularVelocity": 1.9137462023851052e-12,
+            "velocityX": -0.7195960008785613,
+            "velocityY": -3.7138679567028667,
+            "timestamp": 0.5681775289498161
+        },
+        {
+            "x": 1.9669031646192998,
+            "y": 4.107582820499775,
+            "heading": -8.465521766194915e-7,
+            "angularVelocity": 2.3002544763334585e-12,
+            "velocityX": -0.7149046006511871,
+            "velocityY": -3.7147738802470687,
+            "timestamp": 0.6015997365350993
+        },
+        {
+            "x": 1.9504985155072192,
+            "y": 3.982217377752346,
+            "heading": -8.465520941748615e-7,
+            "angularVelocity": 2.4667619198655976e-12,
+            "velocityX": -0.4908308067350749,
+            "velocityY": -3.7509623631996885,
+            "timestamp": 0.6350219441203826
+        },
+        {
+            "x": 1.94330794196602,
+            "y": 3.855987816020002,
+            "heading": -8.465520093116816e-7,
+            "angularVelocity": 2.5391254773053977e-12,
+            "velocityX": -0.2151435844765948,
+            "velocityY": -3.7768169984060798,
+            "timestamp": 0.6684441517056658
+        },
+        {
+            "x": 1.9454293697222904,
+            "y": 3.7295714172218672,
+            "heading": -8.465519250884413e-7,
+            "angularVelocity": 2.519978339764665e-12,
+            "velocityX": 0.0634735976376205,
+            "velocityY": -3.7824072056149025,
+            "timestamp": 0.7018663592909491
+        },
+        {
+            "x": 1.9568513812515056,
+            "y": 3.603654207925799,
+            "heading": -8.465518451486746e-7,
+            "angularVelocity": 2.391815832767542e-12,
+            "velocityX": 0.34174916483985474,
+            "velocityY": -3.767471342961152,
+            "timestamp": 0.7352885668762323
+        },
+        {
+            "x": 1.9775120291721782,
+            "y": 3.4789195174340257,
+            "heading": -8.465517739710846e-7,
+            "angularVelocity": 2.129649524174011e-12,
+            "velocityX": 0.6181712523959566,
+            "velocityY": -3.7320901132416737,
+            "timestamp": 0.7687107744615156
+        },
+        {
+            "x": 2.0072992100178335,
+            "y": 3.3560442616931043,
+            "heading": -8.465517105809759e-7,
+            "angularVelocity": 1.8966463157464128e-12,
+            "velocityX": 0.8912391789143821,
+            "velocityY": -3.6764554055067418,
+            "timestamp": 0.8021329820467988
+        },
+        {
+            "x": 2.046051283468891,
+            "y": 3.235695267642527,
+            "heading": -8.465516527951994e-7,
+            "angularVelocity": 1.7289634182989629e-12,
+            "velocityX": 1.1594707905613406,
+            "velocityY": -3.600869085126494,
+            "timestamp": 0.8355551896320821
+        },
+        {
+            "x": 2.0935579534966013,
+            "y": 3.1185256537724477,
+            "heading": -8.46551596321545e-7,
+            "angularVelocity": 1.6897044173558387e-12,
+            "velocityX": 1.421410297528723,
+            "velocityY": -3.5057413120014775,
+            "timestamp": 0.8689773972173653
         },
         {
             "x": 2.1495614051818848,
             "y": 3.005171298980713,
-            "heading": 3.1399983498353623,
-            "angularVelocity": 1.0122205936259764e-11,
-            "velocityX": 1.67595966800623,
-            "velocityY": -3.391427920419207,
-            "timestamp": 0.9024103726161997
-        },
-        {
-            "x": 2.2887605263975965,
-            "y": 2.8026901614733335,
-            "heading": 3.1399983498355466,
-            "angularVelocity": 2.834267287592998e-12,
-            "velocityX": 2.1430749184844986,
-            "velocityY": -3.117349042649137,
-            "timestamp": 0.9673633614337364
-        },
-        {
-            "x": 2.4554470535268806,
-            "y": 2.6221614552311943,
-            "heading": 3.1399983498356696,
-            "angularVelocity": 1.896559244966036e-12,
-            "velocityX": 2.566264157563543,
-            "velocityY": -2.7793748914229504,
-            "timestamp": 1.0323163502512729
-        },
-        {
-            "x": 2.646204500588454,
-            "y": 2.4672853384780784,
-            "heading": 3.139998349835769,
-            "angularVelocity": 1.528915692972437e-12,
-            "velocityX": 2.9368540314174165,
-            "velocityY": -2.384434027946127,
-            "timestamp": 1.0972693390688095
-        },
-        {
-            "x": 2.8571229789617445,
-            "y": 2.341236093184693,
-            "heading": 3.1399983498358592,
-            "angularVelocity": 1.3872590523351098e-12,
-            "velocityX": 3.247248236192457,
-            "velocityY": -1.940622711722675,
-            "timestamp": 1.1622223278863462
-        },
-        {
-            "x": 3.0838578351814623,
-            "y": 2.2465454263799622,
-            "heading": 3.139998349835948,
-            "angularVelocity": 1.37081210529667e-12,
-            "velocityX": 3.490753240880161,
-            "velocityY": -1.4578338661343044,
-            "timestamp": 1.2271753167038828
-        },
-        {
-            "x": 3.3145391093778356,
-            "y": 2.1619217463740292,
-            "heading": 3.1399983498360386,
-            "angularVelocity": 1.3907396736325547e-12,
-            "velocityX": 3.551511306806079,
-            "velocityY": -1.3028450506530576,
-            "timestamp": 1.2921283055214194
-        },
-        {
-            "x": 3.5452163544414024,
-            "y": 2.0772870838321214,
-            "heading": 3.1399983498361257,
-            "angularVelocity": 1.3371940783811797e-12,
-            "velocityX": 3.5514492752906865,
-            "velocityY": -1.3030141350149058,
-            "timestamp": 1.357081294338956
-        },
-        {
-            "x": 3.7758941865765565,
-            "y": 1.9926540214142978,
-            "heading": 3.139998349836204,
-            "angularVelocity": 1.2081341732104707e-12,
-            "velocityX": 3.5514583136980007,
-            "velocityY": -1.302989499904737,
-            "timestamp": 1.4220342831564927
-        },
-        {
-            "x": 4.006572310105761,
-            "y": 1.908021753240333,
-            "heading": 3.1399983498362696,
-            "angularVelocity": 1.0139841411422692e-12,
-            "velocityX": 3.5514627999275414,
-            "velocityY": -1.3029772719259731,
-            "timestamp": 1.4869872719740294
-        },
-        {
-            "x": 4.237248920507999,
-            "y": 1.8233853608613613,
-            "heading": 3.1399983498363224,
-            "angularVelocity": 8.135622214219814e-13,
-            "velocityX": 3.5514395042053684,
-            "velocityY": -1.3030407671488682,
-            "timestamp": 1.551940260791566
-        },
-        {
-            "x": 4.46792193477352,
-            "y": 1.7387391679278503,
-            "heading": 3.1399983498363646,
-            "angularVelocity": 6.458697792259404e-13,
-            "velocityX": 3.551384138968564,
-            "velocityY": -1.3031916540771904,
-            "timestamp": 1.6168932496091026
-        },
-        {
-            "x": 4.698662422545312,
-            "y": 1.6542770762611563,
-            "heading": 3.139998349836401,
-            "angularVelocity": 5.573297491220617e-13,
-            "velocityX": 3.5524229442068638,
-            "velocityY": -1.3003572769873404,
-            "timestamp": 1.6818462384266393
+            "heading": -8.465515057383081e-7,
+            "angularVelocity": 2.7102707524537993e-12,
+            "velocityX": 1.6756359238847198,
+            "velocityY": -3.3915878986274275,
+            "timestamp": 0.9023996048026486
+        },
+        {
+            "x": 2.288743031901554,
+            "y": 2.8026748842621148,
+            "heading": -8.46551464739476e-7,
+            "angularVelocity": 6.312009526868921e-13,
+            "velocityX": 2.142782210187666,
+            "velocityY": -3.117550249366458,
+            "timestamp": 0.9673533019435205
+        },
+        {
+            "x": 2.455414708004788,
+            "y": 2.6221288200825,
+            "heading": -8.46551435217149e-7,
+            "angularVelocity": 4.5451345461171864e-13,
+            "velocityX": 2.566007532127087,
+            "velocityY": -2.779611817758129,
+            "timestamp": 1.0323069990843925
+        },
+        {
+            "x": 2.646160177401748,
+            "y": 2.4672337011827246,
+            "heading": -8.465514075797998e-7,
+            "angularVelocity": 4.254931139136813e-13,
+            "velocityX": 2.9366376017527633,
+            "velocityY": -2.384700574689416,
+            "timestamp": 1.0972606962252645
+        },
+        {
+            "x": 2.857069726290126,
+            "y": 2.341164292711998,
+            "heading": -8.465513787578765e-7,
+            "angularVelocity": 4.4373032078954734e-13,
+            "velocityX": 3.2470753501649696,
+            "velocityY": -1.9409119729950552,
+            "timestamp": 1.1622143933661364
+        },
+        {
+            "x": 3.0838173053505575,
+            "y": 2.2464971410417167,
+            "heading": -8.465513471236816e-7,
+            "angularVelocity": 4.870268741320482e-13,
+            "velocityX": 3.490911049579749,
+            "velocityY": -1.457455939200772,
+            "timestamp": 1.2271680905070084
+        },
+        {
+            "x": 3.3145112489857773,
+            "y": 2.1619002222864983,
+            "heading": -8.465513138187862e-7,
+            "angularVelocity": 5.127482864627404e-13,
+            "velocityX": 3.5516676307835056,
+            "velocityY": -1.3024188380298123,
+            "timestamp": 1.2921217876478803
+        },
+        {
+            "x": 3.5452013267301767,
+            "y": 2.0772927620920076,
+            "heading": -8.465512839726007e-7,
+            "angularVelocity": 4.594994306997602e-13,
+            "velocityX": 3.5516081131487938,
+            "velocityY": -1.302581129610464,
+            "timestamp": 1.3570754847887523
+        },
+        {
+            "x": 3.77589133724735,
+            "y": 1.9926851185956267,
+            "heading": -8.465512625373994e-7,
+            "angularVelocity": 3.3000743037279784e-13,
+            "velocityX": 3.5516070781466733,
+            "velocityY": -1.302583951649345,
+            "timestamp": 1.4220291819296242
+        },
+        {
+            "x": 4.006581361028944,
+            "y": 1.9080775112659911,
+            "heading": -8.465512535495126e-7,
+            "angularVelocity": 1.3837377280671698e-13,
+            "velocityX": 3.5516072823601217,
+            "velocityY": -1.302583394841325,
+            "timestamp": 1.4869828790704962
+        },
+        {
+            "x": 4.2372714040114055,
+            "y": 1.823469956289169,
+            "heading": -8.465512575529088e-7,
+            "angularVelocity": -6.163459506326445e-14,
+            "velocityX": 3.551607577968653,
+            "velocityY": -1.3025825888392473,
+            "timestamp": 1.5519365762113682
+        },
+        {
+            "x": 4.467961461705261,
+            "y": 1.7388624414245573,
+            "heading": -8.465512722492451e-7,
+            "angularVelocity": -2.2625863777166434e-13,
+            "velocityX": 3.5516078044591044,
+            "velocityY": -1.3025819712885556,
+            "timestamp": 1.6168902733522401
+        },
+        {
+            "x": 4.698651553926265,
+            "y": 1.6542550207002664,
+            "heading": -8.465512926518018e-7,
+            "angularVelocity": -3.141092229421582e-13,
+            "velocityX": 3.551608336024854,
+            "velocityY": -1.3025805219428788,
+            "timestamp": 1.681843970493112
         },
         {
             "x": 4.929586887359619,
             "y": 1.5703192949295044,
-            "heading": 3.139998349836444,
-            "angularVelocity": 6.621983947605345e-13,
-            "velocityX": 3.5552554088546553,
-            "velocityY": -1.2925930408850796,
-            "timestamp": 1.746799227244176
-        },
-        {
-            "x": 5.059491461224377,
-            "y": 1.5327116535816125,
-            "heading": 3.1399983498365303,
-            "angularVelocity": 2.411376289586005e-12,
-            "velocityX": 3.6337293619618993,
-            "velocityY": -1.0519721248789429,
-            "timestamp": 1.7825488830178295
-        },
-        {
-            "x": 5.189524196366832,
-            "y": 1.4955495654455135,
-            "heading": 3.1399983498366377,
-            "angularVelocity": 3.0099354211020166e-12,
-            "velocityX": 3.6373143273250914,
-            "velocityY": -1.0395089779716884,
-            "timestamp": 1.8182985387914832
-        },
-        {
-            "x": 5.319583815273274,
-            "y": 1.4584816742976305,
-            "heading": 3.1399983498367723,
-            "angularVelocity": 3.767098328405853e-12,
-            "velocityX": 3.6380663279640872,
-            "velocityY": -1.036874071811995,
-            "timestamp": 1.8540481945651368
-        },
-        {
-            "x": 5.4496589768626915,
-            "y": 1.4214683610582035,
-            "heading": 3.139998349836922,
-            "angularVelocity": 4.1821295987378e-12,
-            "velocityX": 3.6385010925132435,
-            "velocityY": -1.0353474023301,
-            "timestamp": 1.8897978503387904
-        },
-        {
-            "x": 5.579738820656374,
-            "y": 1.3844715066642421,
-            "heading": 3.139998349837078,
-            "angularVelocity": 4.36086747938318e-12,
-            "velocityX": 3.6386320645231356,
-            "velocityY": -1.0348870106108623,
-            "timestamp": 1.925547506112444
-        },
-        {
-            "x": 5.709817887256256,
-            "y": 1.3474719197526863,
-            "heading": 3.1399983498372332,
-            "angularVelocity": 4.346123552895358e-12,
-            "velocityX": 3.6386103246271904,
-            "velocityY": -1.0349634454051337,
-            "timestamp": 1.9612971618860977
-        },
-        {
-            "x": 5.839896082809478,
-            "y": 1.3104692706317995,
-            "heading": 3.139998349837383,
-            "angularVelocity": 4.183764792386501e-12,
-            "velocityX": 3.638585959451041,
-            "velocityY": -1.0350491024349304,
-            "timestamp": 1.9970468176597513
-        },
-        {
-            "x": 5.969973859581082,
-            "y": 1.2734651493862033,
-            "heading": 3.139998349837523,
-            "angularVelocity": 3.914068769730236e-12,
-            "velocityX": 3.6385742451671077,
-            "velocityY": -1.0350902811450293,
-            "timestamp": 2.032796473433405
-        },
-        {
-            "x": 6.100051488327663,
-            "y": 1.2364605078211643,
-            "heading": 3.1399983498376502,
-            "angularVelocity": 3.5614211180657343e-12,
-            "velocityX": 3.6385701045671635,
-            "velocityY": -1.0351048356752235,
-            "timestamp": 2.068546129207059
-        },
-        {
-            "x": 6.230129108868512,
-            "y": 1.199455837420786,
-            "heading": 3.1399983498377644,
-            "angularVelocity": 3.192303325422517e-12,
-            "velocityX": 3.6385698750339617,
-            "velocityY": -1.0351056422659504,
-            "timestamp": 2.1042957849807125
-        },
-        {
-            "x": 6.360206785724816,
-            "y": 1.162451364982862,
-            "heading": 3.1399983498378674,
-            "angularVelocity": 2.8800276139093587e-12,
-            "velocityX": 3.638571450306583,
-            "velocityY": -1.0351001048013342,
-            "timestamp": 2.140045440754366
-        },
-        {
-            "x": 6.490284555094277,
-            "y": 1.1254472177490595,
-            "heading": 3.1399983498379616,
-            "angularVelocity": 2.621534784795507e-12,
-            "velocityX": 3.638574038112148,
-            "velocityY": -1.0350910080949738,
-            "timestamp": 2.1757950965280197
-        },
-        {
-            "x": 6.6203624349981105,
-            "y": 1.0884434590721859,
-            "heading": 3.1399983498380477,
-            "angularVelocity": 2.4107623793093176e-12,
-            "velocityX": 3.638577130012462,
-            "velocityY": -1.0350801392650475,
-            "timestamp": 2.2115447523016734
-        },
-        {
-            "x": 6.75044029323431,
-            "y": 1.0514396242316386,
-            "heading": 3.1399983498381276,
-            "angularVelocity": 2.2461604882641728e-12,
-            "velocityX": 3.6385765239189465,
-            "velocityY": -1.0350822697380073,
-            "timestamp": 2.247294408075327
-        },
-        {
-            "x": 6.880516952547443,
-            "y": 1.0144315751292232,
-            "heading": 3.139998349838195,
-            "angularVelocity": 1.892677955933963e-12,
-            "velocityX": 3.6385429872864736,
-            "velocityY": -1.0352001523239789,
-            "timestamp": 2.2830440638489806
-        },
-        {
-            "x": 7.003514384307231,
-            "y": 0.97944241190024,
-            "heading": 3.1399986104136794,
-            "angularVelocity": 0.000007288894915666013,
-            "velocityX": 3.440520729445303,
-            "velocityY": -0.9787272764389812,
-            "timestamp": 2.3187937196226343
-        },
-        {
-            "x": 7.116262004659963,
-            "y": 0.9473688227429191,
-            "heading": 3.139998845492451,
-            "angularVelocity": 0.000006575693298715729,
-            "velocityX": 3.1538099574045058,
-            "velocityY": -0.8971719716796405,
-            "timestamp": 2.354543375396288
-        },
-        {
-            "x": 7.218759831907218,
-            "y": 0.9182109395297621,
-            "heading": 3.1399990575434797,
-            "angularVelocity": 0.00000593155441577883,
-            "velocityX": 2.8670996973009437,
-            "velocityY": -0.8156129781434808,
-            "timestamp": 2.3902930311699415
-        },
-        {
-            "x": 7.3110078714654865,
-            "y": 0.8919688038160335,
-            "heading": 3.1399992473559544,
-            "angularVelocity": 0.000005309490974163329,
-            "velocityX": 2.5803895887090773,
-            "velocityY": -0.7340528222110998,
-            "timestamp": 2.426042686943595
-        },
-        {
-            "x": 7.393006125886103,
-            "y": 0.8686424358274811,
-            "heading": 3.139999415401417,
-            "angularVelocity": 0.000004700617645814799,
-            "velocityX": 2.293679551483878,
-            "velocityY": -0.6524921005180893,
-            "timestamp": 2.461792342717249
-        },
-        {
-            "x": 7.464754596626899,
-            "y": 0.8482318474429659,
-            "heading": 3.1399995619997028,
-            "angularVelocity": 0.000004100690828648421,
-            "velocityX": 2.0069695550376028,
-            "velocityY": -0.5709310465460533,
-            "timestamp": 2.4975419984909024
-        },
-        {
-            "x": 7.526253284614184,
-            "y": 0.8307370464214682,
-            "heading": 3.1399996873763563,
-            "angularVelocity": 0.00000350707302580956,
-            "velocityX": 1.7202595845023039,
-            "velocityY": -0.4893697755375005,
-            "timestamp": 2.533291654264556
-        },
-        {
-            "x": 7.577502190476697,
-            "y": 0.8161580381894111,
-            "heading": 3.139999791690666,
-            "angularVelocity": 0.0000029179108844478315,
-            "velocityX": 1.433549631554256,
-            "velocityY": -0.40780835273943755,
-            "timestamp": 2.5690413100382097
-        },
-        {
-            "x": 7.618501314660242,
-            "y": 0.8044948267260937,
-            "heading": 3.1399998750527174,
-            "angularVelocity": 0.000002331828092755085,
-            "velocityX": 1.1468396910764296,
-            "velocityY": -0.3262468186312345,
-            "timestamp": 2.6047909658118633
-        },
-        {
-            "x": 7.649250657490367,
-            "y": 0.7957474150520292,
-            "heading": 3.139999937534455,
-            "angularVelocity": 0.0000017477577406037154,
-            "velocityX": 0.860129759704899,
-            "velocityY": -0.24468520003233896,
-            "timestamp": 2.640540621585517
-        },
-        {
-            "x": 7.669750219209449,
-            "y": 0.789915805520138,
-            "heading": 3.1399999791776474,
-            "angularVelocity": 0.0000011648557571115021,
-            "velocityX": 0.5734198351132136,
-            "velocityY": -0.16312351561685162,
-            "timestamp": 2.6762902773591706
+            "heading": -8.465513064459796e-7,
+            "angularVelocity": -2.1236938120360531e-13,
+            "velocityX": 3.555383967330626,
+            "velocityY": -1.2922393869021438,
+            "timestamp": 1.746797667633984
+        },
+        {
+            "x": 5.059495672066219,
+            "y": 1.5327262216225606,
+            "heading": -8.465512750899748e-7,
+            "angularVelocity": 8.770995258558909e-13,
+            "velocityX": 3.633847296905686,
+            "velocityY": -1.051564666141291,
+            "timestamp": 1.7825473219513848
+        },
+        {
+            "x": 5.189548300230332,
+            "y": 1.4956338313822377,
+            "heading": -8.465512186056332e-7,
+            "angularVelocity": 1.579996862146135e-12,
+            "velocityX": 3.637870929029155,
+            "velocityY": -1.0375594099736098,
+            "timestamp": 1.8182969762687855
+        },
+        {
+            "x": 5.319623170079142,
+            "y": 1.4586195133360007,
+            "heading": -8.46551136487956e-7,
+            "angularVelocity": 2.2970201754243682e-12,
+            "velocityX": 3.638493080071459,
+            "velocityY": -1.0353755512600933,
+            "timestamp": 1.8540466305861862
+        },
+        {
+            "x": 5.4496989055100356,
+            "y": 1.42160823723968,
+            "heading": -8.465510387564672e-7,
+            "angularVelocity": 2.7337743725450157e-12,
+            "velocityX": 3.6385172923918394,
+            "velocityY": -1.0352904609291835,
+            "timestamp": 1.889796284903587
+        },
+        {
+            "x": 5.57977446957584,
+            "y": 1.3845963588884114,
+            "heading": -8.465509335637302e-7,
+            "angularVelocity": 2.9424826347181554e-12,
+            "velocityX": 3.6385124989164392,
+            "velocityY": -1.03530730738433,
+            "timestamp": 1.9255459392209877
+        },
+        {
+            "x": 5.709850057975644,
+            "y": 1.3475845660573411,
+            "heading": -8.465508277220688e-7,
+            "angularVelocity": 2.960634540295608e-12,
+            "velocityX": 3.6385131795943324,
+            "velocityY": -1.0353049151878095,
+            "timestamp": 1.9612955935383884
+        },
+        {
+            "x": 5.839925662966751,
+            "y": 1.3105728315354166,
+            "heading": -8.465507264753305e-7,
+            "angularVelocity": 2.832103981671134e-12,
+            "velocityX": 3.6385136436912036,
+            "velocityY": -1.0353032841469862,
+            "timestamp": 1.9970452478557892
+        },
+        {
+            "x": 5.970001275498046,
+            "y": 1.273561123513075,
+            "heading": -8.465506337302748e-7,
+            "angularVelocity": 2.594292378130792e-12,
+            "velocityX": 3.638513854607602,
+            "velocityY": -1.0353025428927405,
+            "timestamp": 2.03279490217319
+        },
+        {
+            "x": 6.100076894564937,
+            "y": 1.2365494384597497,
+            "heading": -8.46550552278632e-7,
+            "angularVelocity": 2.2783896618382965e-12,
+            "velocityX": 3.6385140374232323,
+            "velocityY": -1.0353019003965735,
+            "timestamp": 2.0685445564905907
+        },
+        {
+            "x": 6.230152520105601,
+            "y": 1.1995377761581834,
+            "heading": -8.465504825990077e-7,
+            "angularVelocity": 1.9490992420524453e-12,
+            "velocityX": 3.6385142185095396,
+            "velocityY": -1.03530126397757,
+            "timestamp": 2.1042942108079914
+        },
+        {
+            "x": 6.3602281516940815,
+            "y": 1.16252613511139,
+            "heading": -8.465504225140753e-7,
+            "angularVelocity": 1.680713660601537e-12,
+            "velocityX": 3.6385143876808734,
+            "velocityY": -1.0353006694327234,
+            "timestamp": 2.140043865125392
+        },
+        {
+            "x": 6.490303788775648,
+            "y": 1.1255145133698101,
+            "heading": -8.465503700121188e-7,
+            "angularVelocity": 1.4686003981542915e-12,
+            "velocityX": 3.638514541335137,
+            "velocityY": -1.0353001294215285,
+            "timestamp": 2.175793519442793
+        },
+        {
+            "x": 6.62037943067546,
+            "y": 1.0885029085617548,
+            "heading": -8.465503232940394e-7,
+            "angularVelocity": 1.306812060469801e-12,
+            "velocityX": 3.638514676112538,
+            "velocityY": -1.0352996557519205,
+            "timestamp": 2.2115431737601936
+        },
+        {
+            "x": 6.7504550766965234,
+            "y": 1.0514913182376744,
+            "heading": -8.465502808582129e-7,
+            "angularVelocity": 1.187027603302574e-12,
+            "velocityX": 3.638514791393393,
+            "velocityY": -1.0352992506018621,
+            "timestamp": 2.2472928280775943
+        },
+        {
+            "x": 6.880530727542945,
+            "y": 1.0144797448710672,
+            "heading": -8.465502433332782e-7,
+            "angularVelocity": 1.0496586699357012e-12,
+            "velocityX": 3.6385149263697616,
+            "velocityY": -1.0352987762623522,
+            "timestamp": 2.283042482394995
+        },
+        {
+            "x": 7.003525953233288,
+            "y": 0.9794828656529073,
+            "heading": -7.145353891597533e-7,
+            "angularVelocity": 0.000003692758900357643,
+            "velocityX": 3.440459160761053,
+            "velocityY": -0.9789431502593731,
+            "timestamp": 2.318792136712396
+        },
+        {
+            "x": 7.116271603762692,
+            "y": 0.9474023877264399,
+            "heading": -5.944969754078434e-7,
+            "angularVelocity": 0.0000033577503345576707,
+            "velocityX": 3.153754985388111,
+            "velocityY": -0.8973647029323183,
+            "timestamp": 2.3545417910297965
+        },
+        {
+            "x": 7.218767661219051,
+            "y": 0.9182383156679585,
+            "heading": -4.859087151281335e-7,
+            "angularVelocity": 0.0000030374632243726353,
+            "velocityX": 2.867050308972391,
+            "velocityY": -0.8157861275958178,
+            "timestamp": 2.3902914453471973
+        },
+        {
+            "x": 7.311014119553375,
+            "y": 0.8919906507276494,
+            "heading": -3.8850367492824467e-7,
+            "angularVelocity": 0.000002724642854703664,
+            "velocityX": 2.580345463352478,
+            "velocityY": -0.7342075172887301,
+            "timestamp": 2.426041099664598
+        },
+        {
+            "x": 7.393010975686544,
+            "y": 0.8686593933386082,
+            "heading": -3.0210953963079075e-7,
+            "angularVelocity": 0.0000024166425366030095,
+            "velocityX": 2.293640531602499,
+            "velocityY": -0.6526288948669626,
+            "timestamp": 2.4617907539819988
+        },
+        {
+            "x": 7.464758227730078,
+            "y": 0.8482445436165693,
+            "heading": -2.2661033918382436e-7,
+            "angularVelocity": 0.000002111886167406066,
+            "velocityX": 2.006935547027439,
+            "velocityY": -0.5710502692078399,
+            "timestamp": 2.4975404082993995
+        },
+        {
+            "x": 7.526255874392654,
+            "y": 0.8307461015250325,
+            "heading": -1.6193015142138781e-7,
+            "angularVelocity": 0.0000018092535156938384,
+            "velocityX": 1.7202305263310733,
+            "velocityY": -0.48947164456971237,
+            "timestamp": 2.5332900626168002
+        },
+        {
+            "x": 7.577503914725456,
+            "y": 0.8161640669449958,
+            "heading": -1.0802379214331377e-7,
+            "angularVelocity": 0.0000015078847699136293,
+            "velocityX": 1.4335254790941394,
+            "velocityY": -0.4078930232603403,
+            "timestamp": 2.569039716934201
+        },
+        {
+            "x": 7.618502347994633,
+            "y": 0.8044984397090525,
+            "heading": -6.487055743300418e-8,
+            "angularVelocity": 0.0000012070951602673883,
+            "velocityX": 1.1468204113297347,
+            "velocityY": -0.3263144066337203,
+            "timestamp": 2.6047893712516017
+        },
+        {
+            "x": 7.649251173610278,
+            "y": 0.7957492196197445,
+            "heading": -3.246962719134405e-8,
+            "angularVelocity": 9.063284906782187e-7,
+            "velocityX": 0.8601153270642448,
+            "velocityY": -0.24473579553046104,
+            "timestamp": 2.6405390255690024
+        },
+        {
+            "x": 7.669750391083677,
+            "y": 0.7899164064601075,
+            "heading": -1.0836483282929717e-8,
+            "angularVelocity": 6.051287577519143e-7,
+            "velocityX": 0.573410229128317,
+            "velocityY": -0.16315719049619,
+            "timestamp": 2.676288679886403
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 5.824490440069194e-7,
-            "velocityX": 0.2867099156267607,
-            "velocityY": -0.0815617789049158,
-            "timestamp": 2.712039933132824
+            "heading": 2.0494644799121527e-30,
+            "angularVelocity": 3.031213445055282e-7,
+            "velocityX": 0.28670511958863415,
+            "velocityY": -0.08157859189950141,
+            "timestamp": 2.712038334203804
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": -6.025226692032396e-31,
-            "velocityX": 4.372989448335815e-32,
-            "velocityY": 7.524777583668013e-32,
-            "timestamp": 2.747789588906478
+            "heading": -1.7051999701665118e-29,
+            "angularVelocity": -2.5846596071248955e-28,
+            "velocityX": 4.524644517512267e-29,
+            "velocityY": 1.710870028377411e-29,
+            "timestamp": 2.7477879885212046
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.1.traj
index a83dc4bd..2aaf78f3 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.1.traj	
@@ -3,146 +3,146 @@
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 3.1925891483075246e-34,
+            "heading": 0,
+            "angularVelocity": 4.4337460853781175e-35,
             "velocityX": 0,
             "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 2.3070751616194327,
-            "y": 5.580871513694386,
-            "heading": 3.1487661024318196,
-            "angularVelocity": 0.1026391987797076,
-            "velocityX": 0.668272918826128,
-            "velocityY": 0.24437718582154933,
-            "timestamp": 0.08540696474681245
+            "x": 2.3070751783284065,
+            "y": 5.580871519986164,
+            "heading": 0.00875997642576413,
+            "angularVelocity": 0.10256751513529143,
+            "velocityX": 0.6682733985254588,
+            "velocityY": 0.24437736336610263,
+            "timestamp": 0.08540692844327274
         },
         {
-            "x": 2.4212254758610405,
-            "y": 5.622614541188738,
-            "heading": 3.166299183933703,
-            "angularVelocity": 0.20528866180714583,
-            "velocityX": 1.3365457323065477,
-            "velocityY": 0.48875437287928064,
-            "timestamp": 0.1708139294936249
+            "x": 2.421225525991661,
+            "y": 5.622614560146346,
+            "heading": 0.026280802198455293,
+            "angularVelocity": 0.20514525100066663,
+            "velocityX": 1.3365466917484683,
+            "velocityY": 0.48875472893171557,
+            "timestamp": 0.1708138568865455
         },
         {
-            "x": 2.592450926566031,
-            "y": 5.685229083532584,
-            "heading": 3.1926007136716263,
-            "angularVelocity": 0.307955326780356,
-            "velocityX": 2.0048183565893676,
-            "velocityY": 0.7331315722255889,
-            "timestamp": 0.25622089424043737
+            "x": 2.592451026856299,
+            "y": 5.685229121649564,
+            "heading": 0.052563939294491345,
+            "angularVelocity": 0.30774010463909035,
+            "velocityX": 2.0048197960703633,
+            "velocityY": 0.7331321081849562,
+            "timestamp": 0.25622078532981823
         },
         {
-            "x": 2.820751488987388,
-            "y": 5.768715144253157,
-            "heading": 3.2276726330952217,
-            "angularVelocity": 0.4106447235019468,
-            "velocityX": 2.6730906911181096,
-            "velocityY": 0.9775088128710195,
-            "timestamp": 0.3416278589872498
+            "x": 2.8207516562305845,
+            "y": 5.768715208168554,
+            "heading": 0.0876113178799828,
+            "angularVelocity": 0.41035755792072426,
+            "velocityX": 2.673092611285307,
+            "velocityY": 0.9775095304409794,
+            "timestamp": 0.341627713773091
         },
         {
-            "x": 3.106127123163594,
-            "y": 5.873072729004609,
-            "heading": 3.271517021699339,
-            "angularVelocity": 0.5133584682946327,
-            "velocityX": 3.341362557751549,
-            "velocityY": 1.2218861197188997,
-            "timestamp": 0.4270348237340622
+            "x": 3.106127374237044,
+            "y": 5.873072825511933,
+            "heading": 0.13142500230326418,
+            "angularVelocity": 0.5129991819385286,
+            "velocityX": 3.341364959588794,
+            "velocityY": 1.221887020708095,
+            "timestamp": 0.4270346422163637
         },
         {
-            "x": 3.4095640006306436,
-            "y": 5.984036075660027,
-            "heading": 3.2715170269491542,
-            "angularVelocity": 6.146823646853408e-8,
-            "velocityX": 3.5528352794948703,
-            "velocityY": 1.2992306538976923,
-            "timestamp": 0.5124417884808746
+            "x": 3.40956412390744,
+            "y": 5.984036121763709,
+            "heading": 0.13142500754936084,
+            "angularVelocity": 6.142471997127927e-8,
+            "velocityX": 3.5528352933560723,
+            "velocityY": 1.299230615997127,
+            "timestamp": 0.5124415706596365
         },
         {
-            "x": 3.7130008780817154,
-            "y": 6.094999422359182,
-            "heading": 3.271517032198949,
-            "angularVelocity": 6.146799253439994e-8,
-            "velocityX": 3.5528352793077906,
-            "velocityY": 1.299230654409779,
-            "timestamp": 0.597848753227687
+            "x": 3.7130008735619784,
+            "y": 6.094999418058892,
+            "heading": 0.1314250127954367,
+            "angularVelocity": 6.142447634082945e-8,
+            "velocityX": 3.5528352931703995,
+            "velocityY": 1.2992306165053664,
+            "timestamp": 0.5978484991029092
         },
         {
-            "x": 4.016437755532787,
-            "y": 6.205962769058336,
-            "heading": 3.271517037448744,
-            "angularVelocity": 6.146799251320457e-8,
-            "velocityX": 3.55283527930779,
-            "velocityY": 1.2992306544097794,
-            "timestamp": 0.6832557179744995
+            "x": 4.016437623216516,
+            "y": 6.205962714354075,
+            "heading": 0.13142501804151255,
+            "angularVelocity": 6.142447606684555e-8,
+            "velocityX": 3.552835293170399,
+            "velocityY": 1.2992306165053664,
+            "timestamp": 0.683255427546182
         },
         {
-            "x": 4.319874632996578,
-            "y": 6.3169261157226675,
-            "heading": 3.2715170426985596,
-            "angularVelocity": 6.146823670561877e-8,
-            "velocityX": 3.5528352794567044,
-            "velocityY": 1.2992306540020593,
-            "timestamp": 0.7686626827213119
+            "x": 4.3198743728836435,
+            "y": 6.316926010614789,
+            "heading": 0.13142502328760913,
+            "angularVelocity": 6.142471942956278e-8,
+            "velocityX": 3.5528352933178016,
+            "velocityY": 1.2992306161017806,
+            "timestamp": 0.7686623559894548
         },
         {
-            "x": 4.605249758095724,
-            "y": 6.421284043313642,
-            "heading": 3.315404813019647,
-            "angularVelocity": 0.5138664095040972,
-            "velocityX": 3.3413565971479624,
-            "velocityY": 1.221890133905845,
-            "timestamp": 0.8540696474681243
+            "x": 4.605249584503867,
+            "y": 6.421283973215489,
+            "heading": 0.17528171773503587,
+            "angularVelocity": 0.5135027713419801,
+            "velocityX": 3.3413590304886087,
+            "velocityY": 1.2218910632059061,
+            "timestamp": 0.8540692844327276
         },
         {
-            "x": 4.833549856620357,
-            "y": 6.504770412469219,
-            "heading": 3.3505164790016466,
-            "angularVelocity": 0.41111010192303765,
-            "velocityX": 2.673085259515143,
-            "velocityY": 0.9775124242275821,
-            "timestamp": 0.9394766122149367
+            "x": 4.83354975240058,
+            "y": 6.5047703704067485,
+            "heading": 0.21036850612417926,
+            "angularVelocity": 0.4108189935954435,
+            "velocityX": 2.6730872080050085,
+            "velocityY": 0.9775131679944534,
+            "timestamp": 0.9394762128760004
         },
         {
-            "x": 5.004774927843149,
-            "y": 6.567385202446666,
-            "heading": 3.3768507903488065,
-            "angularVelocity": 0.3083391550704053,
-            "velocityX": 2.0048139133662572,
-            "velocityY": 0.7331344716800138,
-            "timestamp": 1.0248835769617493
+            "x": 5.004774875721697,
+            "y": 6.567385181417454,
+            "heading": 0.23668415165777779,
+            "angularVelocity": 0.3081207346202291,
+            "velocityX": 2.0048153755446823,
+            "velocityY": 0.7331350295812797,
+            "timestamp": 1.0248831413192732
         },
         {
-            "x": 5.118924975241942,
-            "y": 6.609128400529722,
-            "heading": 3.3944071451755278,
-            "angularVelocity": 0.20556116095176463,
-            "velocityX": 1.3365426079382265,
-            "velocityY": 0.4887563702422027,
-            "timestamp": 1.1102905417085618
+            "x": 5.118924957871047,
+            "y": 6.6091283935214395,
+            "heading": 0.25422806054578806,
+            "angularVelocity": 0.20541552316405948,
+            "velocityX": 1.3365435829385781,
+            "velocityY": 0.48875674216187737,
+            "timestamp": 1.110290069762546
         },
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.10278040005380541,
-            "velocityX": 0.6682713163646097,
-            "velocityY": 0.24437819014118428,
-            "timestamp": 1.1956975064553743
+            "heading": 0.263,
+            "angularVelocity": 0.10270758607175808,
+            "velocityX": 0.6682718038134577,
+            "velocityY": 0.2443783760754632,
+            "timestamp": 1.1956969982058188
         },
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": -1.3221581062655587e-34,
-            "velocityX": 2.469071735487476e-35,
+            "heading": 0.263,
+            "angularVelocity": 1.86588452749739e-32,
+            "velocityX": -1.0770205854918045e-34,
             "velocityY": 0,
-            "timestamp": 1.2811044712021868
+            "timestamp": 1.2811039266490916
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.traj
index a83dc4bd..2aaf78f3 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.traj	
@@ -3,146 +3,146 @@
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 3.1925891483075246e-34,
+            "heading": 0,
+            "angularVelocity": 4.4337460853781175e-35,
             "velocityX": 0,
             "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 2.3070751616194327,
-            "y": 5.580871513694386,
-            "heading": 3.1487661024318196,
-            "angularVelocity": 0.1026391987797076,
-            "velocityX": 0.668272918826128,
-            "velocityY": 0.24437718582154933,
-            "timestamp": 0.08540696474681245
+            "x": 2.3070751783284065,
+            "y": 5.580871519986164,
+            "heading": 0.00875997642576413,
+            "angularVelocity": 0.10256751513529143,
+            "velocityX": 0.6682733985254588,
+            "velocityY": 0.24437736336610263,
+            "timestamp": 0.08540692844327274
         },
         {
-            "x": 2.4212254758610405,
-            "y": 5.622614541188738,
-            "heading": 3.166299183933703,
-            "angularVelocity": 0.20528866180714583,
-            "velocityX": 1.3365457323065477,
-            "velocityY": 0.48875437287928064,
-            "timestamp": 0.1708139294936249
+            "x": 2.421225525991661,
+            "y": 5.622614560146346,
+            "heading": 0.026280802198455293,
+            "angularVelocity": 0.20514525100066663,
+            "velocityX": 1.3365466917484683,
+            "velocityY": 0.48875472893171557,
+            "timestamp": 0.1708138568865455
         },
         {
-            "x": 2.592450926566031,
-            "y": 5.685229083532584,
-            "heading": 3.1926007136716263,
-            "angularVelocity": 0.307955326780356,
-            "velocityX": 2.0048183565893676,
-            "velocityY": 0.7331315722255889,
-            "timestamp": 0.25622089424043737
+            "x": 2.592451026856299,
+            "y": 5.685229121649564,
+            "heading": 0.052563939294491345,
+            "angularVelocity": 0.30774010463909035,
+            "velocityX": 2.0048197960703633,
+            "velocityY": 0.7331321081849562,
+            "timestamp": 0.25622078532981823
         },
         {
-            "x": 2.820751488987388,
-            "y": 5.768715144253157,
-            "heading": 3.2276726330952217,
-            "angularVelocity": 0.4106447235019468,
-            "velocityX": 2.6730906911181096,
-            "velocityY": 0.9775088128710195,
-            "timestamp": 0.3416278589872498
+            "x": 2.8207516562305845,
+            "y": 5.768715208168554,
+            "heading": 0.0876113178799828,
+            "angularVelocity": 0.41035755792072426,
+            "velocityX": 2.673092611285307,
+            "velocityY": 0.9775095304409794,
+            "timestamp": 0.341627713773091
         },
         {
-            "x": 3.106127123163594,
-            "y": 5.873072729004609,
-            "heading": 3.271517021699339,
-            "angularVelocity": 0.5133584682946327,
-            "velocityX": 3.341362557751549,
-            "velocityY": 1.2218861197188997,
-            "timestamp": 0.4270348237340622
+            "x": 3.106127374237044,
+            "y": 5.873072825511933,
+            "heading": 0.13142500230326418,
+            "angularVelocity": 0.5129991819385286,
+            "velocityX": 3.341364959588794,
+            "velocityY": 1.221887020708095,
+            "timestamp": 0.4270346422163637
         },
         {
-            "x": 3.4095640006306436,
-            "y": 5.984036075660027,
-            "heading": 3.2715170269491542,
-            "angularVelocity": 6.146823646853408e-8,
-            "velocityX": 3.5528352794948703,
-            "velocityY": 1.2992306538976923,
-            "timestamp": 0.5124417884808746
+            "x": 3.40956412390744,
+            "y": 5.984036121763709,
+            "heading": 0.13142500754936084,
+            "angularVelocity": 6.142471997127927e-8,
+            "velocityX": 3.5528352933560723,
+            "velocityY": 1.299230615997127,
+            "timestamp": 0.5124415706596365
         },
         {
-            "x": 3.7130008780817154,
-            "y": 6.094999422359182,
-            "heading": 3.271517032198949,
-            "angularVelocity": 6.146799253439994e-8,
-            "velocityX": 3.5528352793077906,
-            "velocityY": 1.299230654409779,
-            "timestamp": 0.597848753227687
+            "x": 3.7130008735619784,
+            "y": 6.094999418058892,
+            "heading": 0.1314250127954367,
+            "angularVelocity": 6.142447634082945e-8,
+            "velocityX": 3.5528352931703995,
+            "velocityY": 1.2992306165053664,
+            "timestamp": 0.5978484991029092
         },
         {
-            "x": 4.016437755532787,
-            "y": 6.205962769058336,
-            "heading": 3.271517037448744,
-            "angularVelocity": 6.146799251320457e-8,
-            "velocityX": 3.55283527930779,
-            "velocityY": 1.2992306544097794,
-            "timestamp": 0.6832557179744995
+            "x": 4.016437623216516,
+            "y": 6.205962714354075,
+            "heading": 0.13142501804151255,
+            "angularVelocity": 6.142447606684555e-8,
+            "velocityX": 3.552835293170399,
+            "velocityY": 1.2992306165053664,
+            "timestamp": 0.683255427546182
         },
         {
-            "x": 4.319874632996578,
-            "y": 6.3169261157226675,
-            "heading": 3.2715170426985596,
-            "angularVelocity": 6.146823670561877e-8,
-            "velocityX": 3.5528352794567044,
-            "velocityY": 1.2992306540020593,
-            "timestamp": 0.7686626827213119
+            "x": 4.3198743728836435,
+            "y": 6.316926010614789,
+            "heading": 0.13142502328760913,
+            "angularVelocity": 6.142471942956278e-8,
+            "velocityX": 3.5528352933178016,
+            "velocityY": 1.2992306161017806,
+            "timestamp": 0.7686623559894548
         },
         {
-            "x": 4.605249758095724,
-            "y": 6.421284043313642,
-            "heading": 3.315404813019647,
-            "angularVelocity": 0.5138664095040972,
-            "velocityX": 3.3413565971479624,
-            "velocityY": 1.221890133905845,
-            "timestamp": 0.8540696474681243
+            "x": 4.605249584503867,
+            "y": 6.421283973215489,
+            "heading": 0.17528171773503587,
+            "angularVelocity": 0.5135027713419801,
+            "velocityX": 3.3413590304886087,
+            "velocityY": 1.2218910632059061,
+            "timestamp": 0.8540692844327276
         },
         {
-            "x": 4.833549856620357,
-            "y": 6.504770412469219,
-            "heading": 3.3505164790016466,
-            "angularVelocity": 0.41111010192303765,
-            "velocityX": 2.673085259515143,
-            "velocityY": 0.9775124242275821,
-            "timestamp": 0.9394766122149367
+            "x": 4.83354975240058,
+            "y": 6.5047703704067485,
+            "heading": 0.21036850612417926,
+            "angularVelocity": 0.4108189935954435,
+            "velocityX": 2.6730872080050085,
+            "velocityY": 0.9775131679944534,
+            "timestamp": 0.9394762128760004
         },
         {
-            "x": 5.004774927843149,
-            "y": 6.567385202446666,
-            "heading": 3.3768507903488065,
-            "angularVelocity": 0.3083391550704053,
-            "velocityX": 2.0048139133662572,
-            "velocityY": 0.7331344716800138,
-            "timestamp": 1.0248835769617493
+            "x": 5.004774875721697,
+            "y": 6.567385181417454,
+            "heading": 0.23668415165777779,
+            "angularVelocity": 0.3081207346202291,
+            "velocityX": 2.0048153755446823,
+            "velocityY": 0.7331350295812797,
+            "timestamp": 1.0248831413192732
         },
         {
-            "x": 5.118924975241942,
-            "y": 6.609128400529722,
-            "heading": 3.3944071451755278,
-            "angularVelocity": 0.20556116095176463,
-            "velocityX": 1.3365426079382265,
-            "velocityY": 0.4887563702422027,
-            "timestamp": 1.1102905417085618
+            "x": 5.118924957871047,
+            "y": 6.6091283935214395,
+            "heading": 0.25422806054578806,
+            "angularVelocity": 0.20541552316405948,
+            "velocityX": 1.3365435829385781,
+            "velocityY": 0.48875674216187737,
+            "timestamp": 1.110290069762546
         },
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.10278040005380541,
-            "velocityX": 0.6682713163646097,
-            "velocityY": 0.24437819014118428,
-            "timestamp": 1.1956975064553743
+            "heading": 0.263,
+            "angularVelocity": 0.10270758607175808,
+            "velocityX": 0.6682718038134577,
+            "velocityY": 0.2443783760754632,
+            "timestamp": 1.1956969982058188
         },
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": -1.3221581062655587e-34,
-            "velocityX": 2.469071735487476e-35,
+            "heading": 0.263,
+            "angularVelocity": 1.86588452749739e-32,
+            "velocityX": -1.0770205854918045e-34,
             "velocityY": 0,
-            "timestamp": 1.2811044712021868
+            "timestamp": 1.2811039266490916
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.1.traj
index fa09d8a2..96b0e72c 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.1.traj	
@@ -3,8 +3,8 @@
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -1.2603829918079947e-39,
+            "heading": 0,
+            "angularVelocity": 0,
             "velocityX": 0,
             "velocityY": 0,
             "timestamp": 0
@@ -12,91 +12,91 @@
         {
             "x": 2.3319036557469652,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 1.854813108024866e-16,
+            "heading": -1.815106931159875e-19,
+            "angularVelocity": -1.831407645203353e-18,
             "velocityX": 0.8263919813283321,
-            "velocityY": 1.2609253605375368e-21,
+            "velocityY": 8.319303153097966e-34,
             "timestamp": 0.099109935233537
         },
         {
             "x": 2.4957109641637625,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 3.709616393980982e-16,
+            "heading": -5.438547016393454e-19,
+            "angularVelocity": -3.655980680963076e-18,
             "velocityX": 1.6527839316089838,
-            "velocityY": 2.5218434283644083e-21,
+            "velocityY": 1.3987786573815764e-33,
             "timestamp": 0.198219870467074
         },
         {
             "x": 2.741421919096125,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 5.564379296417211e-16,
+            "heading": -1.0860329369861945e-18,
+            "angularVelocity": -5.4704731072674304e-18,
             "velocityX": 2.4791758197942864,
-            "velocityY": 3.782763291910568e-21,
+            "velocityY": 2.272018181833636e-33,
             "timestamp": 0.297329805700611
         },
         {
             "x": 3.0690365020812695,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 7.41869431396362e-16,
+            "heading": -1.80338172905721e-18,
+            "angularVelocity": -7.237910009033217e-18,
             "velocityX": 3.3055675216936846,
-            "velocityY": 5.0436652946129374e-21,
+            "velocityY": 3.0574601440031788e-33,
             "timestamp": 0.396439740934148
         },
         {
             "x": 3.4439634979187304,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 5.10358456113492e-16,
-            "velocityX": 3.7829405796099507,
-            "velocityY": -2.5218394750934443e-20,
+            "heading": 1.7928981709293245e-18,
+            "angularVelocity": 3.6285765816640505e-17,
+            "velocityX": 3.7829405796099502,
+            "velocityY": 2.997011014948752e-33,
             "timestamp": 0.495549676167685
         },
         {
             "x": 3.771578080903875,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 7.390704390186125e-16,
-            "velocityX": 3.3055675216936846,
-            "velocityY": 5.043665294641482e-21,
+            "heading": 1.0808328664707184e-18,
+            "angularVelocity": -7.184600643588333e-18,
+            "velocityX": 3.305567521693685,
+            "velocityY": -4.3766441461200414e-33,
             "timestamp": 0.594659611401222
         },
         {
             "x": 4.017289035836238,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 5.549819222123311e-16,
+            "heading": 5.416591039281022e-19,
+            "angularVelocity": -5.4401585595028644e-18,
             "velocityX": 2.479175819794286,
-            "velocityY": 3.782763291928162e-21,
+            "velocityY": -5.119323199602909e-33,
             "timestamp": 0.6937695466347591
         },
         {
             "x": 4.181096344253034,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 3.701778935205264e-16,
+            "heading": 1.8086099379948042e-19,
+            "angularVelocity": -3.64038286599245e-18,
             "velocityX": 1.6527839316089838,
-            "velocityY": 2.5218434283731754e-21,
+            "velocityY": -5.9637817525889115e-33,
             "timestamp": 0.792879481868296
         },
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 1.8514488061077656e-16,
+            "heading": 0,
+            "angularVelocity": -1.8248523050897816e-18,
             "velocityX": 0.8263919813283321,
-            "velocityY": 1.260925360546343e-21,
+            "velocityY": -3.8230331267939286e-33,
             "timestamp": 0.891989417101833
         },
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": 3.337507404137172e-39,
+            "velocityY": 2.0684896902092895e-40,
             "timestamp": 0.9910993523353699
         }
     ]
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.traj
index fa09d8a2..96b0e72c 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.traj	
@@ -3,8 +3,8 @@
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -1.2603829918079947e-39,
+            "heading": 0,
+            "angularVelocity": 0,
             "velocityX": 0,
             "velocityY": 0,
             "timestamp": 0
@@ -12,91 +12,91 @@
         {
             "x": 2.3319036557469652,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 1.854813108024866e-16,
+            "heading": -1.815106931159875e-19,
+            "angularVelocity": -1.831407645203353e-18,
             "velocityX": 0.8263919813283321,
-            "velocityY": 1.2609253605375368e-21,
+            "velocityY": 8.319303153097966e-34,
             "timestamp": 0.099109935233537
         },
         {
             "x": 2.4957109641637625,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 3.709616393980982e-16,
+            "heading": -5.438547016393454e-19,
+            "angularVelocity": -3.655980680963076e-18,
             "velocityX": 1.6527839316089838,
-            "velocityY": 2.5218434283644083e-21,
+            "velocityY": 1.3987786573815764e-33,
             "timestamp": 0.198219870467074
         },
         {
             "x": 2.741421919096125,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 5.564379296417211e-16,
+            "heading": -1.0860329369861945e-18,
+            "angularVelocity": -5.4704731072674304e-18,
             "velocityX": 2.4791758197942864,
-            "velocityY": 3.782763291910568e-21,
+            "velocityY": 2.272018181833636e-33,
             "timestamp": 0.297329805700611
         },
         {
             "x": 3.0690365020812695,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 7.41869431396362e-16,
+            "heading": -1.80338172905721e-18,
+            "angularVelocity": -7.237910009033217e-18,
             "velocityX": 3.3055675216936846,
-            "velocityY": 5.0436652946129374e-21,
+            "velocityY": 3.0574601440031788e-33,
             "timestamp": 0.396439740934148
         },
         {
             "x": 3.4439634979187304,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 5.10358456113492e-16,
-            "velocityX": 3.7829405796099507,
-            "velocityY": -2.5218394750934443e-20,
+            "heading": 1.7928981709293245e-18,
+            "angularVelocity": 3.6285765816640505e-17,
+            "velocityX": 3.7829405796099502,
+            "velocityY": 2.997011014948752e-33,
             "timestamp": 0.495549676167685
         },
         {
             "x": 3.771578080903875,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 7.390704390186125e-16,
-            "velocityX": 3.3055675216936846,
-            "velocityY": 5.043665294641482e-21,
+            "heading": 1.0808328664707184e-18,
+            "angularVelocity": -7.184600643588333e-18,
+            "velocityX": 3.305567521693685,
+            "velocityY": -4.3766441461200414e-33,
             "timestamp": 0.594659611401222
         },
         {
             "x": 4.017289035836238,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 5.549819222123311e-16,
+            "heading": 5.416591039281022e-19,
+            "angularVelocity": -5.4401585595028644e-18,
             "velocityX": 2.479175819794286,
-            "velocityY": 3.782763291928162e-21,
+            "velocityY": -5.119323199602909e-33,
             "timestamp": 0.6937695466347591
         },
         {
             "x": 4.181096344253034,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 3.701778935205264e-16,
+            "heading": 1.8086099379948042e-19,
+            "angularVelocity": -3.64038286599245e-18,
             "velocityX": 1.6527839316089838,
-            "velocityY": 2.5218434283731754e-21,
+            "velocityY": -5.9637817525889115e-33,
             "timestamp": 0.792879481868296
         },
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 1.8514488061077656e-16,
+            "heading": 0,
+            "angularVelocity": -1.8248523050897816e-18,
             "velocityX": 0.8263919813283321,
-            "velocityY": 1.260925360546343e-21,
+            "velocityY": -3.8230331267939286e-33,
             "timestamp": 0.891989417101833
         },
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": 3.337507404137172e-39,
+            "velocityY": 2.0684896902092895e-40,
             "timestamp": 0.9910993523353699
         }
     ]
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.1.traj
index d6861c07..7d72e43b 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.1.traj	
@@ -3,470 +3,524 @@
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": 1.712760733525474e-32,
+            "velocityY": -4.99624931676796e-32,
             "timestamp": 0
         },
         {
-            "x": 2.2447404981946972,
-            "y": 5.548452449977017,
-            "heading": 3.1382782323729086,
-            "angularVelocity": -0.0441204598358695,
-            "velocityX": -0.13477523592974824,
-            "velocityY": -0.2959070718806661,
-            "timestamp": 0.03902424484008447
-        },
-        {
-            "x": 2.2342214959104107,
-            "y": 5.525357349752352,
-            "heading": 3.134834682439341,
-            "angularVelocity": -0.08824129583236773,
-            "velocityX": -0.269550437872442,
-            "velocityY": -0.5918141483404872,
-            "timestamp": 0.07804848968016893
-        },
-        {
-            "x": 2.218442994608338,
-            "y": 5.490714699360087,
-            "heading": 3.129669313928024,
-            "angularVelocity": -0.13236306128370928,
-            "velocityX": -0.40432560237181914,
-            "velocityY": -0.8877212239269984,
-            "timestamp": 0.11707273452025341
-        },
-        {
-            "x": 2.1974049958845625,
-            "y": 5.44452449907313,
-            "heading": 3.1227820694139163,
-            "angularVelocity": -0.17648629825721382,
-            "velocityX": -0.539100725971382,
-            "velocityY": -1.1836282925201553,
-            "timestamp": 0.15609697936033787
-        },
-        {
-            "x": 2.17110750147078,
-            "y": 5.386786749443186,
-            "heading": 3.114172871086463,
-            "angularVelocity": -0.22061152913360044,
-            "velocityX": -0.6738758051961007,
-            "velocityY": -1.479535346975897,
-            "timestamp": 0.19512122420042233
-        },
-        {
-            "x": 2.139550513239859,
-            "y": 5.317501451364342,
-            "heading": 3.10384162215273,
-            "angularVelocity": -0.2647392403381948,
-            "velocityX": -0.8086508364284021,
-            "velocityY": -1.775442378520474,
-            "timestamp": 0.2341454690405068
-        },
-        {
-            "x": 2.1027340332257456,
-            "y": 5.236668606182096,
-            "heading": 3.091788209481888,
-            "angularVelocity": -0.30886985053106986,
-            "velocityX": -0.9434258155405923,
-            "velocityY": -2.0713493755866326,
-            "timestamp": 0.27316971388059125
-        },
-        {
-            "x": 2.0606580636823133,
-            "y": 5.1442882159029235,
-            "heading": 3.078012508890752,
-            "angularVelocity": -0.35300364292993813,
-            "velocityX": -1.078200736897071,
-            "velocityY": -2.3672563212366358,
-            "timestamp": 0.31219395872067573
-        },
-        {
-            "x": 2.013322607256609,
-            "y": 5.040360283671343,
-            "heading": 3.0625143970359345,
-            "angularVelocity": -0.3971405960148595,
-            "velocityX": -1.212975590422751,
-            "velocityY": -2.6631631863080094,
-            "timestamp": 0.3512182035607602
-        },
-        {
-            "x": 1.960727667582776,
-            "y": 4.92488481518758,
-            "heading": 3.0452937850632114,
-            "angularVelocity": -0.44127982599779075,
-            "velocityX": -1.3477503508231259,
-            "velocityY": -2.9590699053106113,
-            "timestamp": 0.3902424484008447
-        },
-        {
-            "x": 1.902873252468991,
-            "y": 4.797861825841284,
-            "heading": 3.026350773365474,
-            "angularVelocity": -0.4854164834032721,
-            "velocityX": -1.482524911138253,
-            "velocityY": -3.25497622995194,
-            "timestamp": 0.4292666932409292
-        },
-        {
-            "x": 1.8416825412927555,
-            "y": 4.663514351826141,
-            "heading": 3.02635076853487,
-            "angularVelocity": -1.2378468871067517e-7,
-            "velocityX": -1.5680178162827485,
-            "velocityY": -3.4426668489212484,
-            "timestamp": 0.4682909380810137
-        },
-        {
-            "x": 1.7807776273702332,
-            "y": 4.529037073556694,
-            "heading": 3.0263507637038423,
-            "angularVelocity": -1.237955425988301e-7,
-            "velocityX": -1.5606942343751464,
-            "velocityY": -3.445993095328429,
-            "timestamp": 0.5073151829210981
-        },
-        {
-            "x": 1.7316376139293153,
-            "y": 4.389829293964471,
-            "heading": 3.02635075879599,
-            "angularVelocity": -1.2576418143070467e-7,
-            "velocityX": -1.2592175362335267,
-            "velocityY": -3.5672126433882343,
-            "timestamp": 0.5463394277611826
-        },
-        {
-            "x": 1.6946418503710443,
-            "y": 4.246913724355564,
-            "heading": 3.026350753656809,
-            "angularVelocity": -1.3169201974173754e-7,
-            "velocityX": -0.9480199734770174,
-            "velocityY": -3.662225116579624,
-            "timestamp": 0.5853636726012671
-        },
-        {
-            "x": 1.6700643070030454,
-            "y": 4.101347620951498,
-            "heading": 3.02635074809997,
-            "angularVelocity": -1.423945209571316e-7,
-            "velocityX": -0.6298018954293696,
-            "velocityY": -3.7301452981493113,
-            "timestamp": 0.6243879174413516
-        },
-        {
-            "x": 1.6580868892212808,
-            "y": 3.9542079360929074,
-            "heading": 3.0263507418753948,
-            "angularVelocity": -1.595053312793056e-7,
-            "velocityX": -0.3069224742421484,
-            "velocityY": -3.7704684731645255,
-            "timestamp": 0.6634121622814361
-        },
-        {
-            "x": 1.6587982403438444,
-            "y": 3.806583282470554,
-            "heading": 3.0263507346134717,
-            "angularVelocity": -1.8608747530006848e-7,
-            "velocityX": 0.018228440434743125,
-            "velocityY": -3.7828958440399596,
-            "timestamp": 0.7024364071215206
-        },
-        {
-            "x": 1.6721931105563068,
-            "y": 3.659565885100826,
-            "heading": 3.0263506736368497,
-            "angularVelocity": -0.0000015625317616489644,
-            "velocityX": 0.34324482811563534,
-            "velocityY": -3.767334844586533,
-            "timestamp": 0.741460651961605
-        },
-        {
-            "x": 1.6979565964079348,
-            "y": 3.5152981288336416,
-            "heading": 3.0233678381587312,
-            "angularVelocity": -0.07643544392291168,
-            "velocityX": 0.6601917848050372,
-            "velocityY": -3.6968750288025483,
-            "timestamp": 0.7804848968016895
-        },
-        {
-            "x": 1.7349981432705297,
-            "y": 3.376556575471776,
-            "heading": 3.013707601859302,
-            "angularVelocity": -0.2475444775167341,
-            "velocityX": 0.9491931750224177,
-            "velocityY": -3.5552655517207494,
-            "timestamp": 0.819509141641774
-        },
-        {
-            "x": 1.7820959712007873,
-            "y": 3.2454658997474337,
-            "heading": 2.9995738602901865,
-            "angularVelocity": -0.3621784771756839,
-            "velocityX": 1.2068863375389995,
-            "velocityY": -3.3592110817655803,
-            "timestamp": 0.8585333864818585
-        },
-        {
-            "x": 1.8383454622663515,
-            "y": 3.1231345959204413,
-            "heading": 2.9823282080134397,
-            "angularVelocity": -0.44192148617905314,
-            "velocityX": 1.441398579167467,
-            "velocityY": -3.1347513405650798,
-            "timestamp": 0.897557631321943
-        },
-        {
-            "x": 1.9031002792591356,
-            "y": 3.0102116491438333,
-            "heading": 2.962874002379933,
-            "angularVelocity": -0.4985158768152219,
-            "velocityX": 1.6593483681274654,
-            "velocityY": -2.893661292853978,
-            "timestamp": 0.9365818761620275
-        },
-        {
-            "x": 1.9758873013923517,
-            "y": 2.9071136453404876,
-            "heading": 2.9418434761111434,
-            "angularVelocity": -0.5389092435989264,
-            "velocityX": 1.8651743917531916,
-            "velocityY": -2.6418961910942005,
-            "timestamp": 0.975606121002112
-        },
-        {
-            "x": 2.0563490115304868,
-            "y": 2.814127161728483,
-            "heading": 2.919699686688593,
-            "angularVelocity": -0.5674367182066591,
-            "velocityX": 2.0618390046462407,
-            "velocityY": -2.3827875207591935,
-            "timestamp": 1.0146303658421965
-        },
-        {
-            "x": 2.1442072150434264,
-            "y": 2.731459952378173,
-            "heading": 2.8967947458212753,
-            "angularVelocity": -0.586941296652354,
-            "velocityX": 2.2513748535810034,
-            "velocityY": -2.11835513253537,
-            "timestamp": 1.0536546106822808
+            "x": 2.2447003077480563,
+            "y": 5.548424158968356,
+            "heading": -0.001651505925702968,
+            "angularVelocity": -0.042250731032121805,
+            "velocityX": -0.13558284496887443,
+            "velocityY": -0.2961465280181771,
+            "timestamp": 0.03908822132445813
+        },
+        {
+            "x": 2.234100924566356,
+            "y": 5.525272476708423,
+            "heading": -0.0049545293965937804,
+            "angularVelocity": -0.08450175932731885,
+            "velocityX": -0.27116565611205723,
+            "velocityY": -0.5922930610671583,
+            "timestamp": 0.07817644264891627
+        },
+        {
+            "x": 2.2182018518980375,
+            "y": 5.490544953203296,
+            "heading": -0.009909100000268336,
+            "angularVelocity": -0.12675354456654908,
+            "velocityX": -0.4067484303351874,
+            "velocityY": -0.8884395945486457,
+            "timestamp": 0.1172646639733744
+        },
+        {
+            "x": 2.1970030913105703,
+            "y": 5.444241588640874,
+            "heading": -0.016515264878529855,
+            "angularVelocity": -0.16900653584180803,
+            "velocityX": -0.5423311644575062,
+            "velocityY": -1.1845861232230646,
+            "timestamp": 0.15635288529783253
+        },
+        {
+            "x": 2.170504644501127,
+            "y": 5.386362383452732,
+            "heading": -0.024773088013563174,
+            "angularVelocity": -0.21126116398317848,
+            "velocityX": -0.6779138551608025,
+            "velocityY": -1.480732640856411,
+            "timestamp": 0.19544110662229067
+        },
+        {
+            "x": 2.1387065133083243,
+            "y": 5.31690733837683,
+            "heading": -0.03468264893636917,
+            "angularVelocity": -0.2535178267784212,
+            "velocityX": -0.8134964988264785,
+            "velocityY": -1.7768791396104282,
+            "timestamp": 0.2345293279467488
+        },
+        {
+            "x": 2.1016086997403236,
+            "y": 5.235876454566401,
+            "heading": -0.04624404029676127,
+            "angularVelocity": -0.2957768598481422,
+            "velocityX": -0.9490790911170489,
+            "velocityY": -2.0730256088611454,
+            "timestamp": 0.27361754927120696
+        },
+        {
+            "x": 2.0592112060450187,
+            "y": 5.143269733801358,
+            "heading": -0.05945736298631538,
+            "angularVelocity": -0.33803847404262444,
+            "velocityX": -1.0846616258994186,
+            "velocityY": -2.369172032575987,
+            "timestamp": 0.31270577059566507
+        },
+        {
+            "x": 2.011514034901109,
+            "y": 5.039087178972333,
+            "heading": -0.07432271508161363,
+            "angularVelocity": -0.3803025973454465,
+            "velocityX": -1.2202440921521842,
+            "velocityY": -2.6653183823394446,
+            "timestamp": 0.3517939919201232
+        },
+        {
+            "x": 1.9585171900495353,
+            "y": 4.923328795522167,
+            "heading": -0.09084016031123929,
+            "angularVelocity": -0.4225683510264515,
+            "velocityX": -1.3558264626999135,
+            "velocityY": -2.9614645928578263,
+            "timestamp": 0.3908822132445813
+        },
+        {
+            "x": 1.9002206796290688,
+            "y": 4.795994598697166,
+            "heading": -0.10900958241827326,
+            "angularVelocity": -0.46483113048874997,
+            "velocityX": -1.491408625032185,
+            "velocityY": -3.257610413327559,
+            "timestamp": 0.4299704345690394
+        },
+        {
+            "x": 1.8386671955063139,
+            "y": 4.661546724325594,
+            "heading": -0.10900958703871873,
+            "angularVelocity": -1.182055697467884e-7,
+            "velocityX": -1.5747322860207698,
+            "velocityY": -3.439600724104782,
+            "timestamp": 0.4690586558934975
+        },
+        {
+            "x": 1.7777943297173011,
+            "y": 4.526789325478927,
+            "heading": -0.10900959166091434,
+            "angularVelocity": -1.1825034389504455e-7,
+            "velocityX": -1.5573199221351706,
+            "velocityY": -3.447519336526256,
+            "timestamp": 0.5081468772179556
+        },
+        {
+            "x": 1.728739704549767,
+            "y": 4.38729485969642,
+            "heading": -0.10900959636164997,
+            "angularVelocity": -1.2025964513922604e-7,
+            "velocityX": -1.2549720479816096,
+            "velocityY": -3.5687084511883413,
+            "timestamp": 0.5472350985424137
+        },
+        {
+            "x": 1.6918738865194354,
+            "y": 4.244095790476986,
+            "heading": -0.10900960128999353,
+            "angularVelocity": -1.2608257379035844e-7,
+            "velocityX": -0.9431439134643449,
+            "velocityY": -3.6634838927766116,
+            "timestamp": 0.5863233198668718
+        },
+        {
+            "x": 1.6674707596733624,
+            "y": 4.098254956364247,
+            "heading": -0.10900960662650709,
+            "angularVelocity": -1.3652485001388814e-7,
+            "velocityX": -0.6243089610931135,
+            "velocityY": -3.7310685718382324,
+            "timestamp": 0.6254115411913299
+        },
+        {
+            "x": 1.6557115284805652,
+            "y": 3.9508548842430677,
+            "heading": -0.1090096126146074,
+            "angularVelocity": -1.531944945007552e-7,
+            "velocityX": -0.3008382268193907,
+            "velocityY": -3.7709587985000237,
+            "timestamp": 0.664499762515788
+        },
+        {
+            "x": 1.6566835066189385,
+            "y": 3.8029896922639588,
+            "heading": -0.10900961961577929,
+            "angularVelocity": -1.79112061302596e-7,
+            "velocityX": 0.024866266754091815,
+            "velocityY": -3.7828580316236344,
+            "timestamp": 0.7035879838402461
+        },
+        {
+            "x": 1.6703794917330834,
+            "y": 3.6557569809887474,
+            "heading": -0.10900969829200789,
+            "angularVelocity": -0.000002012786100988185,
+            "velocityX": 0.3503865013576149,
+            "velocityY": -3.766677180142832,
+            "timestamp": 0.7426762051647042
+        },
+        {
+            "x": 1.6964297342683634,
+            "y": 3.5114772969924033,
+            "heading": -0.1124967438020359,
+            "angularVelocity": -0.0892096235606636,
+            "velocityX": 0.6664473760284892,
+            "velocityY": -3.6911294274233937,
+            "timestamp": 0.7817644264891623
+        },
+        {
+            "x": 1.7337062034817727,
+            "y": 3.3729422790966246,
+            "heading": -0.12243166775651844,
+            "angularVelocity": -0.2541666931326176,
+            "velocityX": 0.9536496660724848,
+            "velocityY": -3.54416274779685,
+            "timestamp": 0.8208526478136204
+        },
+        {
+            "x": 1.7809992442628195,
+            "y": 3.2421845220219927,
+            "heading": -0.1367067464310455,
+            "angularVelocity": -0.3652015413039525,
+            "velocityX": 1.2099051626962352,
+            "velocityY": -3.3451958836719715,
+            "timestamp": 0.8599408691380785
+        },
+        {
+            "x": 1.8374227958909133,
+            "y": 3.120270451905998,
+            "heading": -0.154016140064332,
+            "angularVelocity": -0.4428288892862246,
+            "velocityX": 1.443492431128254,
+            "velocityY": -3.118946475052685,
+            "timestamp": 0.8990290904625367
+        },
+        {
+            "x": 1.9023445230612375,
+            "y": 3.007828092475748,
+            "heading": -0.17349166731783877,
+            "angularVelocity": -0.4982454200677405,
+            "velocityX": 1.660902567846671,
+            "velocityY": -2.8766302384778886,
+            "timestamp": 0.9381173117869948
+        },
+        {
+            "x": 1.9753011558084586,
+            "y": 2.90526251520286,
+            "heading": -0.19452421295735298,
+            "angularVelocity": -0.5380788617862533,
+            "velocityX": 1.866460797529241,
+            "velocityY": -2.623950995915806,
+            "timestamp": 0.9772055331114529
+        },
+        {
+            "x": 2.0559421744907826,
+            "y": 2.8128534355639654,
+            "heading": -0.21666661660279088,
+            "angularVelocity": -0.5664725304751266,
+            "velocityX": 2.0630516291077963,
+            "velocityY": -2.3641157491367464,
+            "timestamp": 1.016293754435911
+        },
+        {
+            "x": 2.1439944855858766,
+            "y": 2.7308042438703497,
+            "heading": -0.2395781801271676,
+            "angularVelocity": -0.586150066389446,
+            "velocityX": 2.252655866948419,
+            "velocityY": -2.099077136627731,
+            "timestamp": 1.055381975760369
         },
         {
             "x": 2.2392399311065674,
             "y": 2.659268856048584,
-            "heading": 2.8734047162081335,
-            "angularVelocity": -0.5993717420795943,
-            "velocityX": 2.4352224226909684,
-            "velocityY": -1.849903736136748,
-            "timestamp": 1.0926788555223652
-        },
-        {
-            "x": 2.338491380229597,
-            "y": 2.598983086121257,
-            "heading": 2.850361085284195,
-            "angularVelocity": -0.6059434386623828,
-            "velocityX": 2.609864937185211,
-            "velocityY": -1.5852435257599564,
-            "timestamp": 1.1307081986155205
-        },
-        {
-            "x": 2.444184622569516,
-            "y": 2.5488913725009676,
-            "heading": 2.8272194988809463,
-            "angularVelocity": -0.608519225445519,
-            "velocityX": 2.779255010558571,
-            "velocityY": -1.3171858766423765,
-            "timestamp": 1.1687375417086758
-        },
-        {
-            "x": 2.5560703795596873,
-            "y": 2.509147043381333,
-            "heading": 2.804166943788384,
-            "angularVelocity": -0.6061781039996946,
-            "velocityX": 2.9420901832588977,
-            "velocityY": -1.0450963883935758,
-            "timestamp": 1.2067668848018311
-        },
-        {
-            "x": 2.67382962144658,
-            "y": 2.4799341019260455,
-            "heading": 2.7814386686411225,
-            "angularVelocity": -0.5976510057401518,
-            "velocityX": 3.096536314035744,
-            "velocityY": -0.7681684478149349,
-            "timestamp": 1.2447962278949865
-        },
-        {
-            "x": 2.7970412331199452,
-            "y": 2.46147554775192,
-            "heading": 2.7593387310325537,
-            "angularVelocity": -0.5811285657613138,
-            "velocityX": 3.239909018978801,
-            "velocityY": -0.48537662427964845,
-            "timestamp": 1.2828255709881418
-        },
-        {
-            "x": 2.925127627515394,
-            "y": 2.4540428102885845,
-            "heading": 2.738273276977094,
-            "angularVelocity": -0.5539263195754057,
-            "velocityX": 3.3680937922509844,
-            "velocityY": -0.19544743239797485,
-            "timestamp": 1.320854914081297
-        },
-        {
-            "x": 3.057257978674903,
-            "y": 2.457961811901398,
-            "heading": 2.7188066654897645,
-            "angularVelocity": -0.5118839796850494,
-            "velocityX": 3.47443159446217,
-            "velocityY": 0.10305204597435572,
-            "timestamp": 1.3588842571744524
-        },
-        {
-            "x": 3.192166604008838,
-            "y": 2.4735957350107154,
-            "heading": 2.701757241393799,
-            "angularVelocity": -0.4483228662195614,
-            "velocityX": 3.5474876598173757,
-            "velocityY": 0.41110158203408753,
-            "timestamp": 1.3969136002676077
-        },
-        {
-            "x": 3.3278072233432696,
-            "y": 2.5012232638000396,
-            "heading": 2.6883398256989803,
-            "angularVelocity": -0.3528174457801884,
-            "velocityX": 3.566735796675976,
-            "velocityY": 0.7264792537080078,
-            "timestamp": 1.434942943360763
-        },
-        {
-            "x": 3.4613865978283758,
-            "y": 2.5407156767305445,
-            "heading": 2.677154917488752,
-            "angularVelocity": -0.2941125799315481,
-            "velocityX": 3.512534364790241,
-            "velocityY": 1.0384721301591493,
-            "timestamp": 1.4729722864539183
-        },
-        {
-            "x": 3.5888930381916024,
-            "y": 2.590617982563406,
-            "heading": 2.6645014610802713,
-            "angularVelocity": -0.3327287662446667,
-            "velocityX": 3.35284362001453,
-            "velocityY": 1.3122053071130373,
-            "timestamp": 1.5110016295470736
-        },
-        {
-            "x": 3.7058440395321557,
-            "y": 2.646348954214802,
-            "heading": 2.6512815676345496,
-            "angularVelocity": -0.34762350255007635,
-            "velocityX": 3.0752832373169756,
-            "velocityY": 1.4654728985163021,
-            "timestamp": 1.549030972640229
-        },
-        {
-            "x": 3.810776558765862,
-            "y": 2.7030629098701224,
-            "heading": 2.6383327014335145,
-            "angularVelocity": -0.34049670985152497,
-            "velocityX": 2.7592514279479117,
-            "velocityY": 1.491320939107423,
-            "timestamp": 1.5870603157333842
-        },
-        {
-            "x": 3.9038282779644375,
-            "y": 2.757723211117941,
-            "heading": 2.626141706261417,
-            "angularVelocity": -0.3205681239939803,
-            "velocityX": 2.446840035354867,
-            "velocityY": 1.4373191015665727,
-            "timestamp": 1.6250896588265396
-        },
-        {
-            "x": 3.9854403181685942,
-            "y": 2.8085799309132606,
-            "heading": 2.6149760707773027,
-            "angularVelocity": -0.2936057942615943,
-            "velocityX": 2.1460281342290126,
-            "velocityY": 1.337302084623056,
-            "timestamp": 1.6631190019196949
-        },
-        {
-            "x": 4.056026736802803,
-            "y": 2.8545641040147816,
-            "heading": 2.6049941853375604,
-            "angularVelocity": -0.26247851337555306,
-            "velocityX": 1.8561040736703007,
-            "velocityY": 1.209176108797926,
-            "timestamp": 1.7011483450128502
-        },
-        {
-            "x": 4.115926253855544,
-            "y": 2.894973650540783,
-            "heading": 2.5962978923918545,
-            "angularVelocity": -0.22867323593790462,
-            "velocityX": 1.5750868193019991,
-            "velocityY": 1.0625886023594207,
-            "timestamp": 1.7391776881060055
-        },
-        {
-            "x": 4.1654099219914915,
-            "y": 2.9293183863277776,
-            "heading": 2.588957208164297,
-            "angularVelocity": -0.19302684796776065,
-            "velocityX": 1.301197026063159,
-            "velocityY": 0.9031114658716605,
-            "timestamp": 1.7772070311991608
-        },
-        {
-            "x": 4.20469589755594,
-            "y": 2.957239109446396,
-            "heading": 2.5830228338283105,
-            "angularVelocity": -0.15604724808021303,
-            "velocityX": 1.0330437596099378,
-            "velocityY": 0.7341889406352728,
-            "timestamp": 1.815236374292316
-        },
-        {
-            "x": 4.23396207855711,
-            "y": 2.9784623659467853,
-            "heading": 2.578532998453876,
-            "angularVelocity": -0.11806239627740928,
-            "velocityX": 0.769568407465833,
-            "velocityY": 0.5580758113124009,
-            "timestamp": 1.8532657173854714
-        },
-        {
-            "x": 4.253355635131929,
-            "y": 2.992773543728212,
-            "heading": 2.575517463166205,
-            "angularVelocity": -0.0792949612693492,
-            "velocityX": 0.5099629653689784,
-            "velocityY": 0.3763193528315992,
-            "timestamp": 1.8912950604786267
+            "heading": -0.2629913490343549,
+            "angularVelocity": -0.5989827143282399,
+            "velocityX": 2.4366789353272003,
+            "velocityY": -1.830100869209989,
+            "timestamp": 1.0944701970848272
+        },
+        {
+            "x": 2.3166964235934535,
+            "y": 2.610513248595422,
+            "heading": -0.2811797949710667,
+            "angularVelocity": -0.6046182408202432,
+            "velocityX": 2.5747998696792043,
+            "velocityY": -1.620728330008343,
+            "timestamp": 1.124552726371468
+        },
+        {
+            "x": 2.398213254515867,
+            "y": 2.5681176642029904,
+            "heading": -0.29946946527559826,
+            "angularVelocity": -0.6079831296849904,
+            "velocityX": 2.7097731758410535,
+            "velocityY": -1.409309170398157,
+            "timestamp": 1.154635255658109
+        },
+        {
+            "x": 2.483678189488588,
+            "y": 2.5321524360945276,
+            "heading": -0.31778023201379,
+            "angularVelocity": -0.6086844149213316,
+            "velocityX": 2.8410155993982453,
+            "velocityY": -1.1955520018204469,
+            "timestamp": 1.1847177849447499
+        },
+        {
+            "x": 2.5729562458095865,
+            "y": 2.5026984837196036,
+            "heading": -0.33601688083426684,
+            "angularVelocity": -0.6062205955726221,
+            "velocityX": 2.9677709434035733,
+            "velocityY": -0.9791049181495252,
+            "timestamp": 1.2148003142313908
+        },
+        {
+            "x": 2.665882293188928,
+            "y": 2.479849558811985,
+            "heading": -0.35406456410202075,
+            "angularVelocity": -0.5999390242685017,
+            "velocityX": 3.08903704518594,
+            "velocityY": -0.7595413500609144,
+            "timestamp": 1.2448828435180317
+        },
+        {
+            "x": 2.7622502226261916,
+            "y": 2.4637149411085275,
+            "heading": -0.37178230702948656,
+            "angularVelocity": -0.5889711851900935,
+            "velocityX": 3.2034516951353296,
+            "velocityY": -0.5363451174508257,
+            "timestamp": 1.2749653728046726
+        },
+        {
+            "x": 2.8617965704136163,
+            "y": 2.4544224370162078,
+            "heading": -0.3889934776908366,
+            "angularVelocity": -0.57213176782302,
+            "velocityX": 3.309108314626244,
+            "velocityY": -0.30890035886875983,
+            "timestamp": 1.3050479020913135
+        },
+        {
+            "x": 2.96417486527032,
+            "y": 2.452120989561553,
+            "heading": -0.40547141981393114,
+            "angularVelocity": -0.5477578685651567,
+            "velocityX": 3.4032475754010525,
+            "velocityY": -0.07650445322366382,
+            "timestamp": 1.3351304313779544
+        },
+        {
+            "x": 3.068913958464018,
+            "y": 2.456980552046256,
+            "heading": -0.4209173641629785,
+            "angularVelocity": -0.5134523165215639,
+            "velocityX": 3.4817249638713506,
+            "velocityY": 0.16154102065001244,
+            "timestamp": 1.3652129606645953
+        },
+        {
+            "x": 3.175348297900835,
+            "y": 2.469181868617686,
+            "heading": -0.43492680962689906,
+            "angularVelocity": -0.4657003847794821,
+            "velocityX": 3.5380781457121575,
+            "velocityY": 0.4055947708108251,
+            "timestamp": 1.3952954899512362
+        },
+        {
+            "x": 3.2825012821379658,
+            "y": 2.488873812540701,
+            "heading": -0.44694408076253883,
+            "angularVelocity": -0.3994767534718848,
+            "velocityX": 3.5619672540204803,
+            "velocityY": 0.6545973490252659,
+            "timestamp": 1.425378019237877
+        },
+        {
+            "x": 3.3889723525651156,
+            "y": 2.516050236824646,
+            "heading": -0.4565561347315361,
+            "angularVelocity": -0.3195227993435668,
+            "velocityX": 3.539299153094488,
+            "velocityY": 0.9033955896790181,
+            "timestamp": 1.455460548524518
+        },
+        {
+            "x": 3.4932758480756925,
+            "y": 2.5504542246131905,
+            "heading": -0.4662703499641935,
+            "angularVelocity": -0.32291883239253294,
+            "velocityX": 3.4672448754798637,
+            "velocityY": 1.1436534295611822,
+            "timestamp": 1.485543077811159
+        },
+        {
+            "x": 3.5930108067726363,
+            "y": 2.5908608883384305,
+            "heading": -0.4766346317588353,
+            "angularVelocity": -0.3445282707409342,
+            "velocityX": 3.315378096922126,
+            "velocityY": 1.3431936969204072,
+            "timestamp": 1.5156256070977998
+        },
+        {
+            "x": 3.686096614652251,
+            "y": 2.634834387797071,
+            "heading": -0.48720467819938723,
+            "angularVelocity": -0.3513682755805826,
+            "velocityX": 3.094347785475507,
+            "velocityY": 1.4617620426674243,
+            "timestamp": 1.5457081363844407
+        },
+        {
+            "x": 3.771715868998783,
+            "y": 2.679896340499022,
+            "heading": -0.4975881116213702,
+            "angularVelocity": -0.34516490694787005,
+            "velocityX": 2.8461454663839936,
+            "velocityY": 1.497944280967274,
+            "timestamp": 1.5757906656710816
+        },
+        {
+            "x": 3.8498203373891897,
+            "y": 2.724287723719235,
+            "heading": -0.507530883254748,
+            "angularVelocity": -0.33051647814035945,
+            "velocityX": 2.596339810598824,
+            "velocityY": 1.475653286903962,
+            "timestamp": 1.6058731949577225
+        },
+        {
+            "x": 3.920599661686896,
+            "y": 2.7668759161025953,
+            "heading": -0.5168796397769617,
+            "angularVelocity": -0.3107702957131982,
+            "velocityX": 2.35283820796086,
+            "velocityY": 1.415711823216824,
+            "timestamp": 1.6359557242443634
+        },
+        {
+            "x": 3.984278053809844,
+            "y": 2.806919241781621,
+            "heading": -0.525538519539268,
+            "angularVelocity": -0.287837491315874,
+            "velocityX": 2.11678983227083,
+            "velocityY": 1.3311156551191636,
+            "timestamp": 1.6660382535310043
+        },
+        {
+            "x": 4.041059171114166,
+            "y": 2.843909698702473,
+            "heading": -0.5334440710807248,
+            "angularVelocity": -0.26279544070665484,
+            "velocityX": 1.8875114111345854,
+            "velocityY": 1.229632540814369,
+            "timestamp": 1.6961207828176452
+        },
+        {
+            "x": 4.091116341717457,
+            "y": 2.8774832028649056,
+            "heading": -0.5405519914461838,
+            "angularVelocity": -0.23628067632639732,
+            "velocityX": 1.6639947434713853,
+            "velocityY": 1.11604658778956,
+            "timestamp": 1.7262033121042861
+        },
+        {
+            "x": 4.1345948250930045,
+            "y": 2.9073683770064043,
+            "heading": -0.5468299348831553,
+            "angularVelocity": -0.20869067813911088,
+            "velocityX": 1.445306774615446,
+            "velocityY": 0.9934395428236782,
+            "timestamp": 1.756285841390927
+        },
+        {
+            "x": 4.171616484177909,
+            "y": 2.933356232054327,
+            "heading": -0.552253409487953,
+            "angularVelocity": -0.18028652288901045,
+            "velocityX": 1.2306697595852323,
+            "velocityY": 0.863885306993297,
+            "timestamp": 1.786368370677568
+        },
+        {
+            "x": 4.202284260430688,
+            "y": 2.9552814196152344,
+            "heading": -0.5568033106587852,
+            "angularVelocity": -0.15124729464987938,
+            "velocityX": 1.0194547127524736,
+            "velocityY": 0.7288345787681281,
+            "timestamp": 1.8164508999642088
+        },
+        {
+            "x": 4.226685878711894,
+            "y": 2.973010149818714,
+            "heading": -0.5604643670760648,
+            "angularVelocity": -0.12170041895060278,
+            "velocityX": 0.8111558057069341,
+            "velocityY": 0.5893364229633967,
+            "timestamp": 1.8465334292508497
+        },
+        {
+            "x": 4.244896778926994,
+            "y": 2.98643211128414,
+            "heading": -0.5632241207030344,
+            "angularVelocity": -0.09173941461743668,
+            "velocityX": 0.6053646633758383,
+            "velocityY": 0.4461713088529227,
+            "timestamp": 1.8766159585374906
+        },
+        {
+            "x": 4.256982406189367,
+            "y": 2.9954548894256625,
+            "heading": -0.565072233243941,
+            "angularVelocity": -0.06143474583855434,
+            "velocityX": 0.4017490400230914,
+            "velocityY": 0.2999341596429636,
+            "timestamp": 1.9066984878241315
         },
         {
             "x": 4.263,
             "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -0.03990242909239911,
-            "velocityX": 0.25360324643122656,
-            "velocityY": 0.190023168532958,
-            "timestamp": 1.929324403571782
+            "heading": -0.566,
+            "angularVelocity": -0.030840716457660636,
+            "velocityX": 0.20003616561938206,
+            "velocityY": 0.15108804618886046,
+            "timestamp": 1.9367810171107724
         },
         {
             "x": 4.263,
             "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -5.006919316497686e-30,
-            "velocityX": -1.2207915195710053e-31,
-            "velocityY": 7.406976698260565e-32,
-            "timestamp": 1.9673537466649373
+            "heading": -0.566,
+            "angularVelocity": -4.9732030554004765e-30,
+            "velocityX": -4.223945033236224e-32,
+            "velocityY": 9.969983495378875e-32,
+            "timestamp": 1.9668635463974133
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.traj
index d6861c07..7d72e43b 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.traj	
@@ -3,470 +3,524 @@
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": 1.712760733525474e-32,
+            "velocityY": -4.99624931676796e-32,
             "timestamp": 0
         },
         {
-            "x": 2.2447404981946972,
-            "y": 5.548452449977017,
-            "heading": 3.1382782323729086,
-            "angularVelocity": -0.0441204598358695,
-            "velocityX": -0.13477523592974824,
-            "velocityY": -0.2959070718806661,
-            "timestamp": 0.03902424484008447
-        },
-        {
-            "x": 2.2342214959104107,
-            "y": 5.525357349752352,
-            "heading": 3.134834682439341,
-            "angularVelocity": -0.08824129583236773,
-            "velocityX": -0.269550437872442,
-            "velocityY": -0.5918141483404872,
-            "timestamp": 0.07804848968016893
-        },
-        {
-            "x": 2.218442994608338,
-            "y": 5.490714699360087,
-            "heading": 3.129669313928024,
-            "angularVelocity": -0.13236306128370928,
-            "velocityX": -0.40432560237181914,
-            "velocityY": -0.8877212239269984,
-            "timestamp": 0.11707273452025341
-        },
-        {
-            "x": 2.1974049958845625,
-            "y": 5.44452449907313,
-            "heading": 3.1227820694139163,
-            "angularVelocity": -0.17648629825721382,
-            "velocityX": -0.539100725971382,
-            "velocityY": -1.1836282925201553,
-            "timestamp": 0.15609697936033787
-        },
-        {
-            "x": 2.17110750147078,
-            "y": 5.386786749443186,
-            "heading": 3.114172871086463,
-            "angularVelocity": -0.22061152913360044,
-            "velocityX": -0.6738758051961007,
-            "velocityY": -1.479535346975897,
-            "timestamp": 0.19512122420042233
-        },
-        {
-            "x": 2.139550513239859,
-            "y": 5.317501451364342,
-            "heading": 3.10384162215273,
-            "angularVelocity": -0.2647392403381948,
-            "velocityX": -0.8086508364284021,
-            "velocityY": -1.775442378520474,
-            "timestamp": 0.2341454690405068
-        },
-        {
-            "x": 2.1027340332257456,
-            "y": 5.236668606182096,
-            "heading": 3.091788209481888,
-            "angularVelocity": -0.30886985053106986,
-            "velocityX": -0.9434258155405923,
-            "velocityY": -2.0713493755866326,
-            "timestamp": 0.27316971388059125
-        },
-        {
-            "x": 2.0606580636823133,
-            "y": 5.1442882159029235,
-            "heading": 3.078012508890752,
-            "angularVelocity": -0.35300364292993813,
-            "velocityX": -1.078200736897071,
-            "velocityY": -2.3672563212366358,
-            "timestamp": 0.31219395872067573
-        },
-        {
-            "x": 2.013322607256609,
-            "y": 5.040360283671343,
-            "heading": 3.0625143970359345,
-            "angularVelocity": -0.3971405960148595,
-            "velocityX": -1.212975590422751,
-            "velocityY": -2.6631631863080094,
-            "timestamp": 0.3512182035607602
-        },
-        {
-            "x": 1.960727667582776,
-            "y": 4.92488481518758,
-            "heading": 3.0452937850632114,
-            "angularVelocity": -0.44127982599779075,
-            "velocityX": -1.3477503508231259,
-            "velocityY": -2.9590699053106113,
-            "timestamp": 0.3902424484008447
-        },
-        {
-            "x": 1.902873252468991,
-            "y": 4.797861825841284,
-            "heading": 3.026350773365474,
-            "angularVelocity": -0.4854164834032721,
-            "velocityX": -1.482524911138253,
-            "velocityY": -3.25497622995194,
-            "timestamp": 0.4292666932409292
-        },
-        {
-            "x": 1.8416825412927555,
-            "y": 4.663514351826141,
-            "heading": 3.02635076853487,
-            "angularVelocity": -1.2378468871067517e-7,
-            "velocityX": -1.5680178162827485,
-            "velocityY": -3.4426668489212484,
-            "timestamp": 0.4682909380810137
-        },
-        {
-            "x": 1.7807776273702332,
-            "y": 4.529037073556694,
-            "heading": 3.0263507637038423,
-            "angularVelocity": -1.237955425988301e-7,
-            "velocityX": -1.5606942343751464,
-            "velocityY": -3.445993095328429,
-            "timestamp": 0.5073151829210981
-        },
-        {
-            "x": 1.7316376139293153,
-            "y": 4.389829293964471,
-            "heading": 3.02635075879599,
-            "angularVelocity": -1.2576418143070467e-7,
-            "velocityX": -1.2592175362335267,
-            "velocityY": -3.5672126433882343,
-            "timestamp": 0.5463394277611826
-        },
-        {
-            "x": 1.6946418503710443,
-            "y": 4.246913724355564,
-            "heading": 3.026350753656809,
-            "angularVelocity": -1.3169201974173754e-7,
-            "velocityX": -0.9480199734770174,
-            "velocityY": -3.662225116579624,
-            "timestamp": 0.5853636726012671
-        },
-        {
-            "x": 1.6700643070030454,
-            "y": 4.101347620951498,
-            "heading": 3.02635074809997,
-            "angularVelocity": -1.423945209571316e-7,
-            "velocityX": -0.6298018954293696,
-            "velocityY": -3.7301452981493113,
-            "timestamp": 0.6243879174413516
-        },
-        {
-            "x": 1.6580868892212808,
-            "y": 3.9542079360929074,
-            "heading": 3.0263507418753948,
-            "angularVelocity": -1.595053312793056e-7,
-            "velocityX": -0.3069224742421484,
-            "velocityY": -3.7704684731645255,
-            "timestamp": 0.6634121622814361
-        },
-        {
-            "x": 1.6587982403438444,
-            "y": 3.806583282470554,
-            "heading": 3.0263507346134717,
-            "angularVelocity": -1.8608747530006848e-7,
-            "velocityX": 0.018228440434743125,
-            "velocityY": -3.7828958440399596,
-            "timestamp": 0.7024364071215206
-        },
-        {
-            "x": 1.6721931105563068,
-            "y": 3.659565885100826,
-            "heading": 3.0263506736368497,
-            "angularVelocity": -0.0000015625317616489644,
-            "velocityX": 0.34324482811563534,
-            "velocityY": -3.767334844586533,
-            "timestamp": 0.741460651961605
-        },
-        {
-            "x": 1.6979565964079348,
-            "y": 3.5152981288336416,
-            "heading": 3.0233678381587312,
-            "angularVelocity": -0.07643544392291168,
-            "velocityX": 0.6601917848050372,
-            "velocityY": -3.6968750288025483,
-            "timestamp": 0.7804848968016895
-        },
-        {
-            "x": 1.7349981432705297,
-            "y": 3.376556575471776,
-            "heading": 3.013707601859302,
-            "angularVelocity": -0.2475444775167341,
-            "velocityX": 0.9491931750224177,
-            "velocityY": -3.5552655517207494,
-            "timestamp": 0.819509141641774
-        },
-        {
-            "x": 1.7820959712007873,
-            "y": 3.2454658997474337,
-            "heading": 2.9995738602901865,
-            "angularVelocity": -0.3621784771756839,
-            "velocityX": 1.2068863375389995,
-            "velocityY": -3.3592110817655803,
-            "timestamp": 0.8585333864818585
-        },
-        {
-            "x": 1.8383454622663515,
-            "y": 3.1231345959204413,
-            "heading": 2.9823282080134397,
-            "angularVelocity": -0.44192148617905314,
-            "velocityX": 1.441398579167467,
-            "velocityY": -3.1347513405650798,
-            "timestamp": 0.897557631321943
-        },
-        {
-            "x": 1.9031002792591356,
-            "y": 3.0102116491438333,
-            "heading": 2.962874002379933,
-            "angularVelocity": -0.4985158768152219,
-            "velocityX": 1.6593483681274654,
-            "velocityY": -2.893661292853978,
-            "timestamp": 0.9365818761620275
-        },
-        {
-            "x": 1.9758873013923517,
-            "y": 2.9071136453404876,
-            "heading": 2.9418434761111434,
-            "angularVelocity": -0.5389092435989264,
-            "velocityX": 1.8651743917531916,
-            "velocityY": -2.6418961910942005,
-            "timestamp": 0.975606121002112
-        },
-        {
-            "x": 2.0563490115304868,
-            "y": 2.814127161728483,
-            "heading": 2.919699686688593,
-            "angularVelocity": -0.5674367182066591,
-            "velocityX": 2.0618390046462407,
-            "velocityY": -2.3827875207591935,
-            "timestamp": 1.0146303658421965
-        },
-        {
-            "x": 2.1442072150434264,
-            "y": 2.731459952378173,
-            "heading": 2.8967947458212753,
-            "angularVelocity": -0.586941296652354,
-            "velocityX": 2.2513748535810034,
-            "velocityY": -2.11835513253537,
-            "timestamp": 1.0536546106822808
+            "x": 2.2447003077480563,
+            "y": 5.548424158968356,
+            "heading": -0.001651505925702968,
+            "angularVelocity": -0.042250731032121805,
+            "velocityX": -0.13558284496887443,
+            "velocityY": -0.2961465280181771,
+            "timestamp": 0.03908822132445813
+        },
+        {
+            "x": 2.234100924566356,
+            "y": 5.525272476708423,
+            "heading": -0.0049545293965937804,
+            "angularVelocity": -0.08450175932731885,
+            "velocityX": -0.27116565611205723,
+            "velocityY": -0.5922930610671583,
+            "timestamp": 0.07817644264891627
+        },
+        {
+            "x": 2.2182018518980375,
+            "y": 5.490544953203296,
+            "heading": -0.009909100000268336,
+            "angularVelocity": -0.12675354456654908,
+            "velocityX": -0.4067484303351874,
+            "velocityY": -0.8884395945486457,
+            "timestamp": 0.1172646639733744
+        },
+        {
+            "x": 2.1970030913105703,
+            "y": 5.444241588640874,
+            "heading": -0.016515264878529855,
+            "angularVelocity": -0.16900653584180803,
+            "velocityX": -0.5423311644575062,
+            "velocityY": -1.1845861232230646,
+            "timestamp": 0.15635288529783253
+        },
+        {
+            "x": 2.170504644501127,
+            "y": 5.386362383452732,
+            "heading": -0.024773088013563174,
+            "angularVelocity": -0.21126116398317848,
+            "velocityX": -0.6779138551608025,
+            "velocityY": -1.480732640856411,
+            "timestamp": 0.19544110662229067
+        },
+        {
+            "x": 2.1387065133083243,
+            "y": 5.31690733837683,
+            "heading": -0.03468264893636917,
+            "angularVelocity": -0.2535178267784212,
+            "velocityX": -0.8134964988264785,
+            "velocityY": -1.7768791396104282,
+            "timestamp": 0.2345293279467488
+        },
+        {
+            "x": 2.1016086997403236,
+            "y": 5.235876454566401,
+            "heading": -0.04624404029676127,
+            "angularVelocity": -0.2957768598481422,
+            "velocityX": -0.9490790911170489,
+            "velocityY": -2.0730256088611454,
+            "timestamp": 0.27361754927120696
+        },
+        {
+            "x": 2.0592112060450187,
+            "y": 5.143269733801358,
+            "heading": -0.05945736298631538,
+            "angularVelocity": -0.33803847404262444,
+            "velocityX": -1.0846616258994186,
+            "velocityY": -2.369172032575987,
+            "timestamp": 0.31270577059566507
+        },
+        {
+            "x": 2.011514034901109,
+            "y": 5.039087178972333,
+            "heading": -0.07432271508161363,
+            "angularVelocity": -0.3803025973454465,
+            "velocityX": -1.2202440921521842,
+            "velocityY": -2.6653183823394446,
+            "timestamp": 0.3517939919201232
+        },
+        {
+            "x": 1.9585171900495353,
+            "y": 4.923328795522167,
+            "heading": -0.09084016031123929,
+            "angularVelocity": -0.4225683510264515,
+            "velocityX": -1.3558264626999135,
+            "velocityY": -2.9614645928578263,
+            "timestamp": 0.3908822132445813
+        },
+        {
+            "x": 1.9002206796290688,
+            "y": 4.795994598697166,
+            "heading": -0.10900958241827326,
+            "angularVelocity": -0.46483113048874997,
+            "velocityX": -1.491408625032185,
+            "velocityY": -3.257610413327559,
+            "timestamp": 0.4299704345690394
+        },
+        {
+            "x": 1.8386671955063139,
+            "y": 4.661546724325594,
+            "heading": -0.10900958703871873,
+            "angularVelocity": -1.182055697467884e-7,
+            "velocityX": -1.5747322860207698,
+            "velocityY": -3.439600724104782,
+            "timestamp": 0.4690586558934975
+        },
+        {
+            "x": 1.7777943297173011,
+            "y": 4.526789325478927,
+            "heading": -0.10900959166091434,
+            "angularVelocity": -1.1825034389504455e-7,
+            "velocityX": -1.5573199221351706,
+            "velocityY": -3.447519336526256,
+            "timestamp": 0.5081468772179556
+        },
+        {
+            "x": 1.728739704549767,
+            "y": 4.38729485969642,
+            "heading": -0.10900959636164997,
+            "angularVelocity": -1.2025964513922604e-7,
+            "velocityX": -1.2549720479816096,
+            "velocityY": -3.5687084511883413,
+            "timestamp": 0.5472350985424137
+        },
+        {
+            "x": 1.6918738865194354,
+            "y": 4.244095790476986,
+            "heading": -0.10900960128999353,
+            "angularVelocity": -1.2608257379035844e-7,
+            "velocityX": -0.9431439134643449,
+            "velocityY": -3.6634838927766116,
+            "timestamp": 0.5863233198668718
+        },
+        {
+            "x": 1.6674707596733624,
+            "y": 4.098254956364247,
+            "heading": -0.10900960662650709,
+            "angularVelocity": -1.3652485001388814e-7,
+            "velocityX": -0.6243089610931135,
+            "velocityY": -3.7310685718382324,
+            "timestamp": 0.6254115411913299
+        },
+        {
+            "x": 1.6557115284805652,
+            "y": 3.9508548842430677,
+            "heading": -0.1090096126146074,
+            "angularVelocity": -1.531944945007552e-7,
+            "velocityX": -0.3008382268193907,
+            "velocityY": -3.7709587985000237,
+            "timestamp": 0.664499762515788
+        },
+        {
+            "x": 1.6566835066189385,
+            "y": 3.8029896922639588,
+            "heading": -0.10900961961577929,
+            "angularVelocity": -1.79112061302596e-7,
+            "velocityX": 0.024866266754091815,
+            "velocityY": -3.7828580316236344,
+            "timestamp": 0.7035879838402461
+        },
+        {
+            "x": 1.6703794917330834,
+            "y": 3.6557569809887474,
+            "heading": -0.10900969829200789,
+            "angularVelocity": -0.000002012786100988185,
+            "velocityX": 0.3503865013576149,
+            "velocityY": -3.766677180142832,
+            "timestamp": 0.7426762051647042
+        },
+        {
+            "x": 1.6964297342683634,
+            "y": 3.5114772969924033,
+            "heading": -0.1124967438020359,
+            "angularVelocity": -0.0892096235606636,
+            "velocityX": 0.6664473760284892,
+            "velocityY": -3.6911294274233937,
+            "timestamp": 0.7817644264891623
+        },
+        {
+            "x": 1.7337062034817727,
+            "y": 3.3729422790966246,
+            "heading": -0.12243166775651844,
+            "angularVelocity": -0.2541666931326176,
+            "velocityX": 0.9536496660724848,
+            "velocityY": -3.54416274779685,
+            "timestamp": 0.8208526478136204
+        },
+        {
+            "x": 1.7809992442628195,
+            "y": 3.2421845220219927,
+            "heading": -0.1367067464310455,
+            "angularVelocity": -0.3652015413039525,
+            "velocityX": 1.2099051626962352,
+            "velocityY": -3.3451958836719715,
+            "timestamp": 0.8599408691380785
+        },
+        {
+            "x": 1.8374227958909133,
+            "y": 3.120270451905998,
+            "heading": -0.154016140064332,
+            "angularVelocity": -0.4428288892862246,
+            "velocityX": 1.443492431128254,
+            "velocityY": -3.118946475052685,
+            "timestamp": 0.8990290904625367
+        },
+        {
+            "x": 1.9023445230612375,
+            "y": 3.007828092475748,
+            "heading": -0.17349166731783877,
+            "angularVelocity": -0.4982454200677405,
+            "velocityX": 1.660902567846671,
+            "velocityY": -2.8766302384778886,
+            "timestamp": 0.9381173117869948
+        },
+        {
+            "x": 1.9753011558084586,
+            "y": 2.90526251520286,
+            "heading": -0.19452421295735298,
+            "angularVelocity": -0.5380788617862533,
+            "velocityX": 1.866460797529241,
+            "velocityY": -2.623950995915806,
+            "timestamp": 0.9772055331114529
+        },
+        {
+            "x": 2.0559421744907826,
+            "y": 2.8128534355639654,
+            "heading": -0.21666661660279088,
+            "angularVelocity": -0.5664725304751266,
+            "velocityX": 2.0630516291077963,
+            "velocityY": -2.3641157491367464,
+            "timestamp": 1.016293754435911
+        },
+        {
+            "x": 2.1439944855858766,
+            "y": 2.7308042438703497,
+            "heading": -0.2395781801271676,
+            "angularVelocity": -0.586150066389446,
+            "velocityX": 2.252655866948419,
+            "velocityY": -2.099077136627731,
+            "timestamp": 1.055381975760369
         },
         {
             "x": 2.2392399311065674,
             "y": 2.659268856048584,
-            "heading": 2.8734047162081335,
-            "angularVelocity": -0.5993717420795943,
-            "velocityX": 2.4352224226909684,
-            "velocityY": -1.849903736136748,
-            "timestamp": 1.0926788555223652
-        },
-        {
-            "x": 2.338491380229597,
-            "y": 2.598983086121257,
-            "heading": 2.850361085284195,
-            "angularVelocity": -0.6059434386623828,
-            "velocityX": 2.609864937185211,
-            "velocityY": -1.5852435257599564,
-            "timestamp": 1.1307081986155205
-        },
-        {
-            "x": 2.444184622569516,
-            "y": 2.5488913725009676,
-            "heading": 2.8272194988809463,
-            "angularVelocity": -0.608519225445519,
-            "velocityX": 2.779255010558571,
-            "velocityY": -1.3171858766423765,
-            "timestamp": 1.1687375417086758
-        },
-        {
-            "x": 2.5560703795596873,
-            "y": 2.509147043381333,
-            "heading": 2.804166943788384,
-            "angularVelocity": -0.6061781039996946,
-            "velocityX": 2.9420901832588977,
-            "velocityY": -1.0450963883935758,
-            "timestamp": 1.2067668848018311
-        },
-        {
-            "x": 2.67382962144658,
-            "y": 2.4799341019260455,
-            "heading": 2.7814386686411225,
-            "angularVelocity": -0.5976510057401518,
-            "velocityX": 3.096536314035744,
-            "velocityY": -0.7681684478149349,
-            "timestamp": 1.2447962278949865
-        },
-        {
-            "x": 2.7970412331199452,
-            "y": 2.46147554775192,
-            "heading": 2.7593387310325537,
-            "angularVelocity": -0.5811285657613138,
-            "velocityX": 3.239909018978801,
-            "velocityY": -0.48537662427964845,
-            "timestamp": 1.2828255709881418
-        },
-        {
-            "x": 2.925127627515394,
-            "y": 2.4540428102885845,
-            "heading": 2.738273276977094,
-            "angularVelocity": -0.5539263195754057,
-            "velocityX": 3.3680937922509844,
-            "velocityY": -0.19544743239797485,
-            "timestamp": 1.320854914081297
-        },
-        {
-            "x": 3.057257978674903,
-            "y": 2.457961811901398,
-            "heading": 2.7188066654897645,
-            "angularVelocity": -0.5118839796850494,
-            "velocityX": 3.47443159446217,
-            "velocityY": 0.10305204597435572,
-            "timestamp": 1.3588842571744524
-        },
-        {
-            "x": 3.192166604008838,
-            "y": 2.4735957350107154,
-            "heading": 2.701757241393799,
-            "angularVelocity": -0.4483228662195614,
-            "velocityX": 3.5474876598173757,
-            "velocityY": 0.41110158203408753,
-            "timestamp": 1.3969136002676077
-        },
-        {
-            "x": 3.3278072233432696,
-            "y": 2.5012232638000396,
-            "heading": 2.6883398256989803,
-            "angularVelocity": -0.3528174457801884,
-            "velocityX": 3.566735796675976,
-            "velocityY": 0.7264792537080078,
-            "timestamp": 1.434942943360763
-        },
-        {
-            "x": 3.4613865978283758,
-            "y": 2.5407156767305445,
-            "heading": 2.677154917488752,
-            "angularVelocity": -0.2941125799315481,
-            "velocityX": 3.512534364790241,
-            "velocityY": 1.0384721301591493,
-            "timestamp": 1.4729722864539183
-        },
-        {
-            "x": 3.5888930381916024,
-            "y": 2.590617982563406,
-            "heading": 2.6645014610802713,
-            "angularVelocity": -0.3327287662446667,
-            "velocityX": 3.35284362001453,
-            "velocityY": 1.3122053071130373,
-            "timestamp": 1.5110016295470736
-        },
-        {
-            "x": 3.7058440395321557,
-            "y": 2.646348954214802,
-            "heading": 2.6512815676345496,
-            "angularVelocity": -0.34762350255007635,
-            "velocityX": 3.0752832373169756,
-            "velocityY": 1.4654728985163021,
-            "timestamp": 1.549030972640229
-        },
-        {
-            "x": 3.810776558765862,
-            "y": 2.7030629098701224,
-            "heading": 2.6383327014335145,
-            "angularVelocity": -0.34049670985152497,
-            "velocityX": 2.7592514279479117,
-            "velocityY": 1.491320939107423,
-            "timestamp": 1.5870603157333842
-        },
-        {
-            "x": 3.9038282779644375,
-            "y": 2.757723211117941,
-            "heading": 2.626141706261417,
-            "angularVelocity": -0.3205681239939803,
-            "velocityX": 2.446840035354867,
-            "velocityY": 1.4373191015665727,
-            "timestamp": 1.6250896588265396
-        },
-        {
-            "x": 3.9854403181685942,
-            "y": 2.8085799309132606,
-            "heading": 2.6149760707773027,
-            "angularVelocity": -0.2936057942615943,
-            "velocityX": 2.1460281342290126,
-            "velocityY": 1.337302084623056,
-            "timestamp": 1.6631190019196949
-        },
-        {
-            "x": 4.056026736802803,
-            "y": 2.8545641040147816,
-            "heading": 2.6049941853375604,
-            "angularVelocity": -0.26247851337555306,
-            "velocityX": 1.8561040736703007,
-            "velocityY": 1.209176108797926,
-            "timestamp": 1.7011483450128502
-        },
-        {
-            "x": 4.115926253855544,
-            "y": 2.894973650540783,
-            "heading": 2.5962978923918545,
-            "angularVelocity": -0.22867323593790462,
-            "velocityX": 1.5750868193019991,
-            "velocityY": 1.0625886023594207,
-            "timestamp": 1.7391776881060055
-        },
-        {
-            "x": 4.1654099219914915,
-            "y": 2.9293183863277776,
-            "heading": 2.588957208164297,
-            "angularVelocity": -0.19302684796776065,
-            "velocityX": 1.301197026063159,
-            "velocityY": 0.9031114658716605,
-            "timestamp": 1.7772070311991608
-        },
-        {
-            "x": 4.20469589755594,
-            "y": 2.957239109446396,
-            "heading": 2.5830228338283105,
-            "angularVelocity": -0.15604724808021303,
-            "velocityX": 1.0330437596099378,
-            "velocityY": 0.7341889406352728,
-            "timestamp": 1.815236374292316
-        },
-        {
-            "x": 4.23396207855711,
-            "y": 2.9784623659467853,
-            "heading": 2.578532998453876,
-            "angularVelocity": -0.11806239627740928,
-            "velocityX": 0.769568407465833,
-            "velocityY": 0.5580758113124009,
-            "timestamp": 1.8532657173854714
-        },
-        {
-            "x": 4.253355635131929,
-            "y": 2.992773543728212,
-            "heading": 2.575517463166205,
-            "angularVelocity": -0.0792949612693492,
-            "velocityX": 0.5099629653689784,
-            "velocityY": 0.3763193528315992,
-            "timestamp": 1.8912950604786267
+            "heading": -0.2629913490343549,
+            "angularVelocity": -0.5989827143282399,
+            "velocityX": 2.4366789353272003,
+            "velocityY": -1.830100869209989,
+            "timestamp": 1.0944701970848272
+        },
+        {
+            "x": 2.3166964235934535,
+            "y": 2.610513248595422,
+            "heading": -0.2811797949710667,
+            "angularVelocity": -0.6046182408202432,
+            "velocityX": 2.5747998696792043,
+            "velocityY": -1.620728330008343,
+            "timestamp": 1.124552726371468
+        },
+        {
+            "x": 2.398213254515867,
+            "y": 2.5681176642029904,
+            "heading": -0.29946946527559826,
+            "angularVelocity": -0.6079831296849904,
+            "velocityX": 2.7097731758410535,
+            "velocityY": -1.409309170398157,
+            "timestamp": 1.154635255658109
+        },
+        {
+            "x": 2.483678189488588,
+            "y": 2.5321524360945276,
+            "heading": -0.31778023201379,
+            "angularVelocity": -0.6086844149213316,
+            "velocityX": 2.8410155993982453,
+            "velocityY": -1.1955520018204469,
+            "timestamp": 1.1847177849447499
+        },
+        {
+            "x": 2.5729562458095865,
+            "y": 2.5026984837196036,
+            "heading": -0.33601688083426684,
+            "angularVelocity": -0.6062205955726221,
+            "velocityX": 2.9677709434035733,
+            "velocityY": -0.9791049181495252,
+            "timestamp": 1.2148003142313908
+        },
+        {
+            "x": 2.665882293188928,
+            "y": 2.479849558811985,
+            "heading": -0.35406456410202075,
+            "angularVelocity": -0.5999390242685017,
+            "velocityX": 3.08903704518594,
+            "velocityY": -0.7595413500609144,
+            "timestamp": 1.2448828435180317
+        },
+        {
+            "x": 2.7622502226261916,
+            "y": 2.4637149411085275,
+            "heading": -0.37178230702948656,
+            "angularVelocity": -0.5889711851900935,
+            "velocityX": 3.2034516951353296,
+            "velocityY": -0.5363451174508257,
+            "timestamp": 1.2749653728046726
+        },
+        {
+            "x": 2.8617965704136163,
+            "y": 2.4544224370162078,
+            "heading": -0.3889934776908366,
+            "angularVelocity": -0.57213176782302,
+            "velocityX": 3.309108314626244,
+            "velocityY": -0.30890035886875983,
+            "timestamp": 1.3050479020913135
+        },
+        {
+            "x": 2.96417486527032,
+            "y": 2.452120989561553,
+            "heading": -0.40547141981393114,
+            "angularVelocity": -0.5477578685651567,
+            "velocityX": 3.4032475754010525,
+            "velocityY": -0.07650445322366382,
+            "timestamp": 1.3351304313779544
+        },
+        {
+            "x": 3.068913958464018,
+            "y": 2.456980552046256,
+            "heading": -0.4209173641629785,
+            "angularVelocity": -0.5134523165215639,
+            "velocityX": 3.4817249638713506,
+            "velocityY": 0.16154102065001244,
+            "timestamp": 1.3652129606645953
+        },
+        {
+            "x": 3.175348297900835,
+            "y": 2.469181868617686,
+            "heading": -0.43492680962689906,
+            "angularVelocity": -0.4657003847794821,
+            "velocityX": 3.5380781457121575,
+            "velocityY": 0.4055947708108251,
+            "timestamp": 1.3952954899512362
+        },
+        {
+            "x": 3.2825012821379658,
+            "y": 2.488873812540701,
+            "heading": -0.44694408076253883,
+            "angularVelocity": -0.3994767534718848,
+            "velocityX": 3.5619672540204803,
+            "velocityY": 0.6545973490252659,
+            "timestamp": 1.425378019237877
+        },
+        {
+            "x": 3.3889723525651156,
+            "y": 2.516050236824646,
+            "heading": -0.4565561347315361,
+            "angularVelocity": -0.3195227993435668,
+            "velocityX": 3.539299153094488,
+            "velocityY": 0.9033955896790181,
+            "timestamp": 1.455460548524518
+        },
+        {
+            "x": 3.4932758480756925,
+            "y": 2.5504542246131905,
+            "heading": -0.4662703499641935,
+            "angularVelocity": -0.32291883239253294,
+            "velocityX": 3.4672448754798637,
+            "velocityY": 1.1436534295611822,
+            "timestamp": 1.485543077811159
+        },
+        {
+            "x": 3.5930108067726363,
+            "y": 2.5908608883384305,
+            "heading": -0.4766346317588353,
+            "angularVelocity": -0.3445282707409342,
+            "velocityX": 3.315378096922126,
+            "velocityY": 1.3431936969204072,
+            "timestamp": 1.5156256070977998
+        },
+        {
+            "x": 3.686096614652251,
+            "y": 2.634834387797071,
+            "heading": -0.48720467819938723,
+            "angularVelocity": -0.3513682755805826,
+            "velocityX": 3.094347785475507,
+            "velocityY": 1.4617620426674243,
+            "timestamp": 1.5457081363844407
+        },
+        {
+            "x": 3.771715868998783,
+            "y": 2.679896340499022,
+            "heading": -0.4975881116213702,
+            "angularVelocity": -0.34516490694787005,
+            "velocityX": 2.8461454663839936,
+            "velocityY": 1.497944280967274,
+            "timestamp": 1.5757906656710816
+        },
+        {
+            "x": 3.8498203373891897,
+            "y": 2.724287723719235,
+            "heading": -0.507530883254748,
+            "angularVelocity": -0.33051647814035945,
+            "velocityX": 2.596339810598824,
+            "velocityY": 1.475653286903962,
+            "timestamp": 1.6058731949577225
+        },
+        {
+            "x": 3.920599661686896,
+            "y": 2.7668759161025953,
+            "heading": -0.5168796397769617,
+            "angularVelocity": -0.3107702957131982,
+            "velocityX": 2.35283820796086,
+            "velocityY": 1.415711823216824,
+            "timestamp": 1.6359557242443634
+        },
+        {
+            "x": 3.984278053809844,
+            "y": 2.806919241781621,
+            "heading": -0.525538519539268,
+            "angularVelocity": -0.287837491315874,
+            "velocityX": 2.11678983227083,
+            "velocityY": 1.3311156551191636,
+            "timestamp": 1.6660382535310043
+        },
+        {
+            "x": 4.041059171114166,
+            "y": 2.843909698702473,
+            "heading": -0.5334440710807248,
+            "angularVelocity": -0.26279544070665484,
+            "velocityX": 1.8875114111345854,
+            "velocityY": 1.229632540814369,
+            "timestamp": 1.6961207828176452
+        },
+        {
+            "x": 4.091116341717457,
+            "y": 2.8774832028649056,
+            "heading": -0.5405519914461838,
+            "angularVelocity": -0.23628067632639732,
+            "velocityX": 1.6639947434713853,
+            "velocityY": 1.11604658778956,
+            "timestamp": 1.7262033121042861
+        },
+        {
+            "x": 4.1345948250930045,
+            "y": 2.9073683770064043,
+            "heading": -0.5468299348831553,
+            "angularVelocity": -0.20869067813911088,
+            "velocityX": 1.445306774615446,
+            "velocityY": 0.9934395428236782,
+            "timestamp": 1.756285841390927
+        },
+        {
+            "x": 4.171616484177909,
+            "y": 2.933356232054327,
+            "heading": -0.552253409487953,
+            "angularVelocity": -0.18028652288901045,
+            "velocityX": 1.2306697595852323,
+            "velocityY": 0.863885306993297,
+            "timestamp": 1.786368370677568
+        },
+        {
+            "x": 4.202284260430688,
+            "y": 2.9552814196152344,
+            "heading": -0.5568033106587852,
+            "angularVelocity": -0.15124729464987938,
+            "velocityX": 1.0194547127524736,
+            "velocityY": 0.7288345787681281,
+            "timestamp": 1.8164508999642088
+        },
+        {
+            "x": 4.226685878711894,
+            "y": 2.973010149818714,
+            "heading": -0.5604643670760648,
+            "angularVelocity": -0.12170041895060278,
+            "velocityX": 0.8111558057069341,
+            "velocityY": 0.5893364229633967,
+            "timestamp": 1.8465334292508497
+        },
+        {
+            "x": 4.244896778926994,
+            "y": 2.98643211128414,
+            "heading": -0.5632241207030344,
+            "angularVelocity": -0.09173941461743668,
+            "velocityX": 0.6053646633758383,
+            "velocityY": 0.4461713088529227,
+            "timestamp": 1.8766159585374906
+        },
+        {
+            "x": 4.256982406189367,
+            "y": 2.9954548894256625,
+            "heading": -0.565072233243941,
+            "angularVelocity": -0.06143474583855434,
+            "velocityX": 0.4017490400230914,
+            "velocityY": 0.2999341596429636,
+            "timestamp": 1.9066984878241315
         },
         {
             "x": 4.263,
             "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -0.03990242909239911,
-            "velocityX": 0.25360324643122656,
-            "velocityY": 0.190023168532958,
-            "timestamp": 1.929324403571782
+            "heading": -0.566,
+            "angularVelocity": -0.030840716457660636,
+            "velocityX": 0.20003616561938206,
+            "velocityY": 0.15108804618886046,
+            "timestamp": 1.9367810171107724
         },
         {
             "x": 4.263,
             "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -5.006919316497686e-30,
-            "velocityX": -1.2207915195710053e-31,
-            "velocityY": 7.406976698260565e-32,
-            "timestamp": 1.9673537466649373
+            "heading": -0.566,
+            "angularVelocity": -4.9732030554004765e-30,
+            "velocityX": -4.223945033236224e-32,
+            "velocityY": 9.969983495378875e-32,
+            "timestamp": 1.9668635463974133
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.1.traj
index 9212064d..0ce351ef 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.1.traj	
@@ -3,497 +3,551 @@
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": -4.120593510226553e-35,
+            "velocityY": 6.585905017096791e-33,
             "timestamp": 0
         },
         {
-            "x": 2.2473912878554576,
-            "y": 5.551785895834254,
-            "heading": 3.1358246059620716,
-            "angularVelocity": -0.12928713365744599,
-            "velocityX": -0.08077630820983339,
-            "velocityY": -0.2543419791057163,
-            "timestamp": 0.03229551092834574
-        },
-        {
-            "x": 2.242172157117942,
-            "y": 5.535355099551312,
-            "heading": 3.127553990435098,
-            "angularVelocity": -0.25609180004378934,
-            "velocityX": -0.16160545498398599,
-            "velocityY": -0.5087640916843489,
-            "timestamp": 0.06459102185669148
-        },
-        {
-            "x": 2.2343406441985514,
-            "y": 5.510704667985898,
-            "heading": 3.115281361261821,
-            "angularVelocity": -0.38001037359361073,
-            "velocityX": -0.24249540243429488,
-            "velocityY": -0.7632773365965662,
-            "timestamp": 0.09688653278503723
-        },
-        {
-            "x": 2.2238944664634275,
-            "y": 5.477831199999642,
-            "heading": 3.0991173688763074,
-            "angularVelocity": -0.5005027609371739,
-            "velocityX": -0.3234560294866074,
-            "velocityY": -1.017895894546866,
-            "timestamp": 0.12918204371338296
-        },
-        {
-            "x": 2.2108309419755985,
-            "y": 5.43673070422605,
-            "heading": 3.0791962484868605,
-            "angularVelocity": -0.6168386818107923,
-            "velocityX": -0.40449970018474013,
-            "velocityY": -1.272638041391628,
-            "timestamp": 0.1614775546417287
-        },
-        {
-            "x": 2.19514688330712,
-            "y": 5.387398428523646,
-            "heading": 3.0556846451348814,
-            "angularVelocity": -0.7280145963364562,
-            "velocityX": -0.4856420665793569,
-            "velocityY": -1.5275273338098838,
-            "timestamp": 0.19377306557007443
-        },
-        {
-            "x": 2.17683845193207,
-            "y": 5.329828642238818,
-            "heading": 3.028794799017408,
-            "angularVelocity": -0.832618693574285,
-            "velocityX": -0.5669032893045367,
-            "velocityY": -1.782594070505913,
-            "timestamp": 0.22606857649842016
-        },
-        {
-            "x": 2.1559009488672944,
-            "y": 5.264014369161548,
-            "heading": 2.9988052787110364,
-            "angularVelocity": -0.9285971778836174,
-            "velocityX": -0.6483100116059459,
-            "velocityY": -2.037876818957682,
-            "timestamp": 0.2583640874267659
-        },
-        {
-            "x": 2.132328497947263,
-            "y": 5.189947104033879,
-            "heading": 2.966095877804453,
-            "angularVelocity": -1.012815712349497,
-            "velocityX": -0.7298986838242586,
-            "velocityY": -2.293422924690741,
-            "timestamp": 0.2906595983551117
-        },
-        {
-            "x": 2.1061135495185,
-            "y": 5.107616699879427,
-            "heading": 2.9312120529633434,
-            "angularVelocity": -1.0801446962243946,
-            "velocityX": -0.8117211239334865,
-            "velocityY": -2.549283221966035,
-            "timestamp": 0.32295510928345744
-        },
-        {
-            "x": 2.0772461397099717,
-            "y": 5.017012347335229,
-            "heading": 2.89500074789377,
-            "angularVelocity": -1.1212488679902226,
-            "velocityX": -0.8938520859006283,
-            "velocityY": -2.805478220958382,
-            "timestamp": 0.3552506202118032
-        },
-        {
-            "x": 2.0457134444911373,
-            "y": 4.918129964629122,
-            "heading": 2.858963174705018,
-            "angularVelocity": -1.1158694243515237,
-            "velocityX": -0.976380131864019,
-            "velocityY": -3.0617996081715195,
-            "timestamp": 0.38754613114014896
-        },
-        {
-            "x": 2.0115103463196724,
-            "y": 4.811035711883079,
-            "heading": 2.8266343874462354,
-            "angularVelocity": -1.0010303701499224,
-            "velocityX": -1.0590666376931392,
-            "velocityY": -3.3160724096811367,
-            "timestamp": 0.4198416420684947
-        },
-        {
-            "x": 1.9751456955721198,
-            "y": 4.697500624139868,
-            "heading": 2.816667462304015,
-            "angularVelocity": -0.30861642549437257,
-            "velocityX": -1.1259970721081056,
-            "velocityY": -3.5155067834385165,
-            "timestamp": 0.4521371529968405
-        },
-        {
-            "x": 1.9382198046413848,
-            "y": 4.581042609670271,
-            "heading": 2.816667439698893,
-            "angularVelocity": -6.999462561903041e-7,
-            "velocityX": -1.1433753444143555,
-            "velocityY": -3.6060124494695995,
-            "timestamp": 0.48443266392518625
-        },
-        {
-            "x": 1.901294488957654,
-            "y": 4.4645844128058085,
-            "heading": 2.816667417094136,
-            "angularVelocity": -6.999349526609492e-7,
-            "velocityX": -1.1433575324340735,
-            "velocityY": -3.6060180971544047,
-            "timestamp": 0.516728174853532
-        },
-        {
-            "x": 1.8682421277834205,
-            "y": 4.346968376660799,
-            "heading": 2.816667394414886,
-            "angularVelocity": -7.022415636891642e-7,
-            "velocityX": -1.0234351531879067,
-            "velocityY": -3.6418694971559726,
-            "timestamp": 0.5490236857818777
-        },
-        {
-            "x": 1.8436394280164332,
-            "y": 4.227299259504863,
-            "heading": 2.81666737130494,
-            "angularVelocity": -7.15577656003266e-7,
-            "velocityX": -0.7617993665303343,
-            "velocityY": -3.705441211982887,
-            "timestamp": 0.5813191967102234
-        },
-        {
-            "x": 1.8276118980147091,
-            "y": 4.1061831643448095,
-            "heading": 2.8166673472723573,
-            "angularVelocity": -7.441462228176694e-7,
-            "velocityX": -0.4962773320813651,
-            "velocityY": -3.7502455195328888,
-            "timestamp": 0.6136147076385691
-        },
-        {
-            "x": 1.8202409034217624,
-            "y": 3.984233755462536,
-            "heading": 2.8166673217380835,
-            "angularVelocity": -7.906446726868519e-7,
-            "velocityX": -0.22823588731266858,
-            "velocityY": -3.776048292062723,
-            "timestamp": 0.6459102185669148
-        },
-        {
-            "x": 1.821563847305773,
-            "y": 3.8620689519300933,
-            "heading": 2.816667293968721,
-            "angularVelocity": -8.598520920997921e-7,
-            "velocityX": 0.04096370814339873,
-            "velocityY": -3.782717784013104,
-            "timestamp": 0.6782057294952605
-        },
-        {
-            "x": 1.831574050917961,
-            "y": 3.740307773577626,
-            "heading": 2.816667262971477,
-            "angularVelocity": -9.5980040013024e-7,
-            "velocityX": 0.30995650245006834,
-            "velocityY": -3.7702199114489767,
-            "timestamp": 0.7105012404236062
-        },
-        {
-            "x": 1.8502208046317663,
-            "y": 3.6195671988548175,
-            "heading": 2.816667227314634,
-            "angularVelocity": -0.00000110408047182699,
-            "velocityX": 0.5773791210542256,
-            "velocityY": -3.738617883788745,
-            "timestamp": 0.7427967513519519
-        },
-        {
-            "x": 1.8774096306565946,
-            "y": 3.5004590367763506,
-            "heading": 2.8166671847943117,
-            "angularVelocity": -0.0000013166016324392044,
-            "velocityX": 0.841876324085786,
-            "velocityY": -3.688071767705825,
-            "timestamp": 0.7750922622802976
-        },
-        {
-            "x": 1.913002764036886,
-            "y": 3.383586826300032,
-            "heading": 2.8166671317574883,
-            "angularVelocity": -0.0000016422351652599994,
-            "velocityX": 1.1021077653566822,
-            "velocityY": -3.618837637702142,
-            "timestamp": 0.8073877732086433
-        },
-        {
-            "x": 1.9568198515105035,
-            "y": 3.2695427783414357,
-            "heading": 2.8166670615606066,
-            "angularVelocity": -0.0000021735801416730847,
-            "velocityX": 1.3567547381688734,
-            "velocityY": -3.53126625590866,
-            "timestamp": 0.839683284136989
+            "x": 2.24734688866176,
+            "y": 5.551784954914585,
+            "heading": -0.0040460391001905256,
+            "angularVelocity": -0.1252124844254258,
+            "velocityX": -0.08210564799104332,
+            "velocityY": -0.2542304163009863,
+            "timestamp": 0.03231338407473799
+        },
+        {
+            "x": 2.242039203719346,
+            "y": 5.535352462397274,
+            "heading": -0.012062241013264058,
+            "angularVelocity": -0.24807683078112674,
+            "velocityX": -0.16425654862202216,
+            "velocityY": -0.5085351778477981,
+            "timestamp": 0.06462676814947597
+        },
+        {
+            "x": 2.2340752644376702,
+            "y": 5.510699787230797,
+            "heading": -0.023960373976211733,
+            "angularVelocity": -0.3682106750388487,
+            "velocityX": -0.24645946284226605,
+            "velocityY": -0.7629245859689895,
+            "timestamp": 0.09694015222421395
+        },
+        {
+            "x": 2.223453118891146,
+            "y": 5.477823766232339,
+            "heading": -0.039635722986674345,
+            "angularVelocity": -0.4851039115621088,
+            "velocityX": -0.32872278316489917,
+            "velocityY": -1.0174118848839497,
+            "timestamp": 0.12925353629895195
+        },
+        {
+            "x": 2.210170475892355,
+            "y": 5.4367206851300205,
+            "heading": -0.05896132885919571,
+            "angularVelocity": -0.5980681511978081,
+            "velocityX": -0.4110570087017019,
+            "velocityY": -1.2720141291067089,
+            "timestamp": 0.16156692037368994
+        },
+        {
+            "x": 2.194224615527819,
+            "y": 5.387386118415271,
+            "heading": -0.08177969695024358,
+            "angularVelocity": -0.706158415295445,
+            "velocityX": -0.4934754072075649,
+            "velocityY": -1.5267533292286215,
+            "timestamp": 0.19388030444842794
+        },
+        {
+            "x": 2.1756122677867826,
+            "y": 5.329814722150547,
+            "heading": -0.10789038891803496,
+            "angularVelocity": -0.8080457282778595,
+            "velocityX": -0.575995002503841,
+            "velocityY": -1.7816579078058363,
+            "timestamp": 0.22619368852316593
+        },
+        {
+            "x": 2.1543294408518685,
+            "y": 5.263999973874467,
+            "heading": -0.13703048715481858,
+            "angularVelocity": -0.901796548742399,
+            "velocityX": -0.6586381322887348,
+            "velocityY": -2.0367643365317547,
+            "timestamp": 0.2585070725979039
+        },
+        {
+            "x": 2.1303711674850674,
+            "y": 5.189933880722087,
+            "heading": -0.16884170077604577,
+            "angularVelocity": -0.984459366671516,
+            "velocityX": -0.7414349828352131,
+            "velocityY": -2.2921181198809735,
+            "timestamp": 0.2908204566726419
+        },
+        {
+            "x": 2.103731122650155,
+            "y": 5.107606803578156,
+            "heading": -0.20280968351747503,
+            "angularVelocity": -1.0512047473228812,
+            "velocityX": -0.8244275738281073,
+            "velocityY": -2.547770204244666,
+            "timestamp": 0.3231338407473799
+        },
+        {
+            "x": 2.0744011006982923,
+            "y": 5.0170081629947525,
+            "heading": -0.23813652479783928,
+            "angularVelocity": -1.0932572459343544,
+            "velocityX": -0.9076741044523434,
+            "velocityY": -2.8037496900311267,
+            "timestamp": 0.3554472248221179
+        },
+        {
+            "x": 2.0423709680329383,
+            "y": 4.918132526359071,
+            "heading": -0.27341203074981035,
+            "angularVelocity": -1.0916685751756738,
+            "velocityX": -0.991234238768389,
+            "velocityY": -3.05989729849995,
+            "timestamp": 0.3877606088968559
+        },
+        {
+            "x": 2.007638970172759,
+            "y": 4.811032750261161,
+            "heading": -0.30535515788271084,
+            "angularVelocity": -0.9885416847402974,
+            "velocityX": -1.0748486688935617,
+            "velocityY": -3.3144091578337234,
+            "timestamp": 0.42007399297159387
+        },
+        {
+            "x": 1.9707337326379286,
+            "y": 4.697468787945422,
+            "heading": -0.31502999189110337,
+            "angularVelocity": -0.2994064003329394,
+            "velocityX": -1.1421037626226502,
+            "velocityY": -3.5144558692173984,
+            "timestamp": 0.45238737704633186
+        },
+        {
+            "x": 1.9331993823421831,
+            "y": 4.581134405528862,
+            "heading": -0.31503001351145116,
+            "angularVelocity": -6.690833652147531e-7,
+            "velocityX": -1.1615728705149717,
+            "velocityY": -3.6001918631452843,
+            "timestamp": 0.48470076112106986
+        },
+        {
+            "x": 1.8956658919440634,
+            "y": 4.464799745676637,
+            "heading": -0.31503003513146777,
+            "angularVelocity": -6.690731172580015e-7,
+            "velocityX": -1.1615462593242614,
+            "velocityY": -3.600200448927068,
+            "timestamp": 0.5170141451958078
+        },
+        {
+            "x": 1.8630741103436357,
+            "y": 4.346985095772027,
+            "heading": -0.31503005685343444,
+            "angularVelocity": -6.722281571161063e-7,
+            "velocityX": -1.008615548437953,
+            "velocityY": -3.646001595874795,
+            "timestamp": 0.5493275292705457
+        },
+        {
+            "x": 1.8389499213812899,
+            "y": 4.227149627930832,
+            "heading": -0.3150300790516974,
+            "angularVelocity": -6.869680669187691e-7,
+            "velocityX": -0.746569560976628,
+            "velocityY": -3.708539704910713,
+            "timestamp": 0.5816409133452837
+        },
+        {
+            "x": 1.8234163358184028,
+            "y": 4.105901026539528,
+            "heading": -0.3150301022065626,
+            "angularVelocity": -7.165719666567231e-7,
+            "velocityX": -0.48071676822704484,
+            "velocityY": -3.752271848434857,
+            "timestamp": 0.6139542974200216
+        },
+        {
+            "x": 1.8165522814590422,
+            "y": 3.983854314068792,
+            "heading": -0.3150301268891266,
+            "angularVelocity": -7.638495563130961e-7,
+            "velocityX": -0.21242140233547732,
+            "velocityY": -3.7769709352772667,
+            "timestamp": 0.6462676814947595
+        },
+        {
+            "x": 1.818392625605847,
+            "y": 3.861628588886844,
+            "heading": -0.3150301538292836,
+            "angularVelocity": -8.337151223773892e-7,
+            "velocityX": 0.05695299949234183,
+            "velocityY": -3.7825108289262035,
+            "timestamp": 0.6785810655694975
+        },
+        {
+            "x": 1.828928055424025,
+            "y": 3.7398438657688415,
+            "heading": -0.315030184023291,
+            "angularVelocity": -9.344118022266017e-7,
+            "velocityX": 0.32603919768385625,
+            "velocityY": -3.768863169401422,
+            "timestamp": 0.7108944496442354
+        },
+        {
+            "x": 1.848105139864153,
+            "y": 3.6191179259506723,
+            "heading": -0.31503021891945676,
+            "angularVelocity": -0.0000010799291597080048,
+            "velocityX": 0.593471868986944,
+            "velocityY": -3.7360970778839238,
+            "timestamp": 0.7432078337189734
+        },
+        {
+            "x": 1.8758266057917121,
+            "y": 3.5000631817385077,
+            "heading": -0.3150302607662258,
+            "angularVelocity": -0.0000012950289872625492,
+            "velocityX": 0.8578942355106304,
+            "velocityY": -3.6843787062599658,
+            "timestamp": 0.7755212177937113
+        },
+        {
+            "x": 1.911951833448642,
+            "y": 3.3832835694780297,
+            "heading": -0.3150303133273049,
+            "angularVelocity": -0.000001626603978371328,
+            "velocityX": 1.1179648523774262,
+            "velocityY": -3.6139703594763453,
+            "timestamp": 0.8078346018684492
+        },
+        {
+            "x": 1.9562975704379837,
+            "y": 3.2693714862411047,
+            "heading": -0.315030383530948,
+            "angularVelocity": -0.000002172587152219365,
+            "velocityX": 1.3723643703418302,
+            "velocityY": -3.525229142619559,
+            "timestamp": 0.8401479859431872
         },
         {
             "x": 2.008638858795166,
             "y": 3.158904790878296,
-            "heading": 2.8166669305303302,
-            "angularVelocity": -0.000004057228785819985,
-            "velocityX": 1.6045266290919962,
-            "velocityY": -3.4258008089301617,
-            "timestamp": 0.8719787950653347
-        },
-        {
-            "x": 2.095074231369689,
-            "y": 3.013583476455127,
-            "heading": 2.8166668472664824,
-            "angularVelocity": -0.000001862874616466948,
-            "velocityX": 1.9338316217798797,
-            "velocityY": -3.2512956765222865,
-            "timestamp": 0.9166752265441874
-        },
-        {
-            "x": 2.195389426610098,
-            "y": 2.8774723973475123,
-            "heading": 2.8166667934576224,
-            "angularVelocity": -0.0000012038737383971035,
-            "velocityX": 2.2443669868336342,
-            "velocityY": -3.0452336932538677,
-            "timestamp": 0.9613716580230401
-        },
-        {
-            "x": 2.308610813706186,
-            "y": 2.751892601119491,
-            "heading": 2.8166667540699692,
-            "angularVelocity": -8.812259002769856e-7,
-            "velocityX": 2.5331191629841037,
-            "velocityY": -2.8096157136713913,
-            "timestamp": 1.006068089501893
-        },
-        {
-            "x": 2.433639499231453,
-            "y": 2.638062914068913,
-            "heading": 2.816666722710724,
-            "angularVelocity": -7.016051243778922e-7,
-            "velocityX": 2.7972856308321177,
-            "velocityY": -2.5467287495744175,
-            "timestamp": 1.0507645209807457
-        },
-        {
-            "x": 2.5692619900937426,
-            "y": 2.537088109244684,
-            "heading": 2.8166666961232654,
-            "angularVelocity": -5.94845213173575e-7,
-            "velocityX": 3.034302434780658,
-            "velocityY": -2.259124531496504,
-            "timestamp": 1.0954609524595984
-        },
-        {
-            "x": 2.7141619661921816,
-            "y": 2.449948169817928,
-            "heading": 2.8166666724078224,
-            "angularVelocity": -5.305891904122224e-7,
-            "velocityX": 3.241869010661312,
-            "velocityY": -1.9495950021868031,
-            "timestamp": 1.1401573839384511
-        },
-        {
-            "x": 2.866933035141265,
-            "y": 2.377488711333149,
-            "heading": 2.8166666503106814,
-            "angularVelocity": -4.943826596232375e-7,
-            "velocityX": 3.4179701576705006,
-            "velocityY": -1.6211463888132076,
-            "timestamp": 1.1848538154173038
-        },
-        {
-            "x": 3.0260921573816075,
-            "y": 2.3204120378316904,
-            "heading": 2.8166666288878814,
-            "angularVelocity": -4.79295537569982e-7,
-            "velocityX": 3.560891037031571,
-            "velocityY": -1.2769850212418752,
-            "timestamp": 1.2295502468961566
-        },
-        {
-            "x": 3.1869067054641667,
-            "y": 2.2681812614436785,
-            "heading": 2.8166666075502422,
-            "angularVelocity": -4.773902106314294e-7,
-            "velocityX": 3.597928128974802,
-            "velocityY": -1.1685670345455579,
-            "timestamp": 1.2742466783750093
-        },
-        {
-            "x": 3.3477213293158035,
-            "y": 2.2159507183434,
-            "heading": 2.8166665862126,
-            "angularVelocity": -4.773902944556367e-7,
-            "velocityX": 3.597929824167796,
-            "velocityY": -1.1685618151639285,
-            "timestamp": 1.318943109853862
-        },
-        {
-            "x": 3.508535966619386,
-            "y": 2.163720216660845,
-            "heading": 2.8166665648749514,
-            "angularVelocity": -4.773904335055608e-7,
-            "velocityX": 3.5979301251302083,
-            "velocityY": -1.1685608885189713,
-            "timestamp": 1.3636395413327147
-        },
-        {
-            "x": 3.669350617545261,
-            "y": 2.111489756920536,
-            "heading": 2.816666543537298,
-            "angularVelocity": -4.773905387074157e-7,
-            "velocityX": 3.5979304299038075,
-            "velocityY": -1.1685599501387545,
-            "timestamp": 1.4083359728115674
-        },
-        {
-            "x": 3.830165282253442,
-            "y": 2.0592593396151875,
-            "heading": 2.816666522199642,
-            "angularVelocity": -4.773905991181995e-7,
-            "velocityX": 3.597930738257429,
-            "velocityY": -1.168559000734998,
-            "timestamp": 1.4530324042904201
-        },
-        {
-            "x": 3.990979960989088,
-            "y": 2.0070289654996674,
-            "heading": 2.816666500861985,
-            "angularVelocity": -4.773906042282552e-7,
-            "velocityX": 3.597931052096018,
-            "velocityY": -1.1685580344424535,
-            "timestamp": 1.4977288357692728
-        },
-        {
-            "x": 4.151794654203474,
-            "y": 1.9547986359634688,
-            "heading": 2.8166664795243315,
-            "angularVelocity": -4.773905411161549e-7,
-            "velocityX": 3.597931376031047,
-            "velocityY": -1.1685570370625784,
-            "timestamp": 1.5424252672481256
-        },
-        {
-            "x": 4.312609353229998,
-            "y": 1.902568324322763,
-            "heading": 2.8166664581865346,
-            "angularVelocity": -4.773937498526183e-7,
-            "velocityX": 3.597931506066887,
-            "velocityY": -1.1685566366840177,
-            "timestamp": 1.5871216987269783
-        },
-        {
-            "x": 4.468360661654424,
-            "y": 1.8512590257414445,
-            "heading": 2.7996529762164895,
-            "angularVelocity": -0.38064519710247374,
-            "velocityX": 3.4846475047592103,
-            "velocityY": -1.1479506726525697,
-            "timestamp": 1.631818130205831
-        },
-        {
-            "x": 4.61007924771562,
-            "y": 1.8048518324023994,
-            "heading": 2.753515893557521,
-            "angularVelocity": -1.0322319060481902,
-            "velocityX": 3.1706912917253294,
-            "velocityY": -1.0382751330159734,
-            "timestamp": 1.6765145616846837
-        },
-        {
-            "x": 4.736004026753032,
-            "y": 1.7636749139398393,
-            "heading": 2.704438737111309,
-            "angularVelocity": -1.0980106201416129,
-            "velocityX": 2.8173340660761905,
-            "velocityY": -0.9212574046776457,
-            "timestamp": 1.7212109931635364
-        },
-        {
-            "x": 4.846114017481673,
-            "y": 1.7276966103545506,
-            "heading": 2.657457847080447,
-            "angularVelocity": -1.05111053559365,
-            "velocityX": 2.4635074229748755,
-            "velocityY": -0.8049480102748526,
-            "timestamp": 1.7659074246423891
-        },
-        {
-            "x": 4.9404369366183385,
-            "y": 1.6968910625978524,
-            "heading": 2.6148397238370804,
-            "angularVelocity": -0.9535016965175586,
-            "velocityX": 2.110300890157008,
-            "velocityY": -0.6892171642667541,
-            "timestamp": 1.8106038561212419
-        },
-        {
-            "x": 5.018999070193965,
-            "y": 1.6712409213417598,
-            "heading": 2.5778779941552346,
-            "angularVelocity": -0.8269503505964041,
-            "velocityX": 1.7576824586722803,
-            "velocityY": -0.5738744773892818,
-            "timestamp": 1.8553002876000946
-        },
-        {
-            "x": 5.08182145352524,
-            "y": 1.6507342817906896,
-            "heading": 2.547405196833757,
-            "angularVelocity": -0.6817724886134384,
-            "velocityX": 1.4055346534990782,
-            "velocityY": -0.45879813829819405,
-            "timestamp": 1.8999967190789473
-        },
-        {
-            "x": 5.128920654811687,
-            "y": 1.6353626111579855,
-            "heading": 2.524000017522932,
-            "angularVelocity": -0.52364760533286,
-            "velocityX": 1.0537575311516838,
-            "velocityY": -0.34391270452937267,
-            "timestamp": 1.9446931505578
-        },
-        {
-            "x": 5.160309902680238,
-            "y": 1.6251195157583214,
-            "heading": 2.5080885464702414,
-            "angularVelocity": -0.35598974070712264,
-            "velocityX": 0.702276374869034,
-            "velocityY": -0.2291703176462827,
-            "timestamp": 1.9893895820366527
+            "heading": -0.3150304952316807,
+            "angularVelocity": -0.00000345679463513026,
+            "velocityX": 1.6198021301675205,
+            "velocityY": -3.418604968990847,
+            "timestamp": 0.8724613700179251
+        },
+        {
+            "x": 2.0775165593610554,
+            "y": 3.0394044439018932,
+            "heading": -0.31503058092883834,
+            "angularVelocity": -0.0000023503872475309846,
+            "velocityX": 1.8890856295919138,
+            "velocityY": -3.277496001603852,
+            "timestamp": 0.9089222373745671
+        },
+        {
+            "x": 2.155767716832464,
+            "y": 2.9258208551273497,
+            "heading": -0.31503063899444206,
+            "angularVelocity": -0.0000015925458689218539,
+            "velocityX": 2.146168293419776,
+            "velocityY": -3.11521905563918,
+            "timestamp": 0.9453831047312091
+        },
+        {
+            "x": 2.242886939864493,
+            "y": 2.818887609111427,
+            "heading": -0.31503068232040304,
+            "angularVelocity": -0.000001188286624986991,
+            "velocityX": 2.38938975806231,
+            "velocityY": -2.9328223316783686,
+            "timestamp": 0.9818439720878511
+        },
+        {
+            "x": 2.3383115633038396,
+            "y": 2.719295334161189,
+            "heading": -0.31503071689328305,
+            "angularVelocity": -9.482188022909844e-7,
+            "velocityX": 2.6171791939547426,
+            "velocityY": -2.7314839763979357,
+            "timestamp": 1.0183048394444931
+        },
+        {
+            "x": 2.4414252810789625,
+            "y": 2.627687242403576,
+            "heading": -0.3150307459166021,
+            "angularVelocity": -7.96012851101892e-7,
+            "velocityX": 2.8280654095942417,
+            "velocityY": -2.5125044574926996,
+            "timestamp": 1.0547657068011351
+        },
+        {
+            "x": 2.5515621254678895,
+            "y": 2.5446549735283583,
+            "heading": -0.3150307712925926,
+            "angularVelocity": -6.959787951410741e-7,
+            "velocityX": 3.020686351523773,
+            "velocityY": -2.2772982349278084,
+            "timestamp": 1.0912265741577771
+        },
+        {
+            "x": 2.668010765906386,
+            "y": 2.470734767010798,
+            "heading": -0.31503079425241676,
+            "angularVelocity": -6.297114091058936e-7,
+            "velocityX": 3.1937978682584367,
+            "velocityY": -2.027384751835701,
+            "timestamp": 1.1276874415144191
+        },
+        {
+            "x": 2.790019095895906,
+            "y": 2.406403976394343,
+            "heading": -0.31503081565982244,
+            "angularVelocity": -5.871337458164478e-7,
+            "velocityX": 3.3462816119017478,
+            "velocityY": -1.7643790529502215,
+            "timestamp": 1.1641483088710611
+        },
+        {
+            "x": 2.916799054801627,
+            "y": 2.352077873335088,
+            "heading": -0.31503083617399774,
+            "angularVelocity": -5.626354173508468e-7,
+            "velocityX": 3.477151480397404,
+            "velocityY": -1.4899838374074656,
+            "timestamp": 1.2006091762277031
+        },
+        {
+            "x": 3.0475310715630033,
+            "y": 2.308104889106193,
+            "heading": -0.31503085634737477,
+            "angularVelocity": -5.532884563345105e-7,
+            "velocityX": 3.585543247850392,
+            "velocityY": -1.2060323140087088,
+            "timestamp": 1.2370700435843451
+        },
+        {
+            "x": 3.1788176786437843,
+            "y": 2.2658164704041117,
+            "heading": -0.3150308764961414,
+            "angularVelocity": -5.526134756982608e-7,
+            "velocityX": 3.600753810835078,
+            "velocityY": -1.1598302993847416,
+            "timestamp": 1.2735309109409871
+        },
+        {
+            "x": 3.3101043321458232,
+            "y": 2.2235281958195148,
+            "heading": -0.3150308966449121,
+            "angularVelocity": -5.526135855693721e-7,
+            "velocityX": 3.600755084015385,
+            "velocityY": -1.159826346722746,
+            "timestamp": 1.3099917782976291
+        },
+        {
+            "x": 3.441390996396766,
+            "y": 2.181239954605585,
+            "heading": -0.31503091679368933,
+            "angularVelocity": -5.526137662420421e-7,
+            "velocityX": 3.6007553788219733,
+            "velocityY": -1.1598254314766572,
+            "timestamp": 1.3464526456542711
+        },
+        {
+            "x": 3.5726776715245805,
+            "y": 2.1389517471596347,
+            "heading": -0.31503093694247225,
+            "angularVelocity": -5.526139217103117e-7,
+            "velocityX": 3.600755677138282,
+            "velocityY": -1.1598245053335783,
+            "timestamp": 1.3829135130109131
+        },
+        {
+            "x": 3.7039643576361168,
+            "y": 2.0966635738134185,
+            "heading": -0.3150309570912594,
+            "angularVelocity": -5.526140383605173e-7,
+            "velocityX": 3.6007559783851373,
+            "velocityY": -1.159823570091575,
+            "timestamp": 1.4193743803675551
+        },
+        {
+            "x": 3.835251054850112,
+            "y": 2.0543754349355976,
+            "heading": -0.31503097724004914,
+            "angularVelocity": -5.526141098258176e-7,
+            "velocityX": 3.600756282888565,
+            "velocityY": -1.1598226247384584,
+            "timestamp": 1.4558352477241971
+        },
+        {
+            "x": 3.966537763342614,
+            "y": 2.012087331072757,
+            "heading": -0.3150309973888395,
+            "angularVelocity": -5.526141250500724e-7,
+            "velocityX": 3.6007565922204043,
+            "velocityY": -1.1598216643943178,
+            "timestamp": 1.4922961150808391
+        },
+        {
+            "x": 4.097824483400825,
+            "y": 1.9697992631165677,
+            "heading": -0.315031017537628,
+            "angularVelocity": -5.526140761706961e-7,
+            "velocityX": 3.6007569094292164,
+            "velocityY": -1.1598206795946073,
+            "timestamp": 1.5287569824374811
+        },
+        {
+            "x": 4.2291112154854185,
+            "y": 1.9275112324972625,
+            "heading": -0.3150310376864126,
+            "angularVelocity": -5.526139663753558e-7,
+            "velocityX": 3.600757239272763,
+            "velocityY": -1.1598196555683948,
+            "timestamp": 1.5652178497941232
+        },
+        {
+            "x": 4.360397911943487,
+            "y": 1.8852230912740195,
+            "heading": -0.3150310578362786,
+            "angularVelocity": -5.526436278671969e-7,
+            "velocityX": 3.600756262156026,
+            "velocityY": -1.159822689065559,
+            "timestamp": 1.6016787171507652
+        },
+        {
+            "x": 4.485161433583451,
+            "y": 1.8443297112343198,
+            "heading": -0.3370639022790847,
+            "angularVelocity": -0.6042874467931063,
+            "velocityX": 3.4218473307173203,
+            "velocityY": -1.121569041122948,
+            "timestamp": 1.6381395845074072
+        },
+        {
+            "x": 4.600444501590908,
+            "y": 1.8067758967734413,
+            "heading": -0.37703885598165165,
+            "angularVelocity": -1.0963796695112746,
+            "velocityX": 3.1618301034864644,
+            "velocityY": -1.029975894252485,
+            "timestamp": 1.6746004518640492
+        },
+        {
+            "x": 4.705219738357152,
+            "y": 1.7727109130585983,
+            "heading": -0.41891394403468696,
+            "angularVelocity": -1.1484940180779766,
+            "velocityX": 2.873635334600916,
+            "velocityY": -0.9342889016225668,
+            "timestamp": 1.7110613192206912
+        },
+        {
+            "x": 4.799468915908326,
+            "y": 1.742101106125887,
+            "heading": -0.4594847392760727,
+            "angularVelocity": -1.112721615877786,
+            "velocityX": 2.5849406331801226,
+            "velocityY": -0.8395249249915333,
+            "timestamp": 1.7475221865773332
+        },
+        {
+            "x": 4.883206605537126,
+            "y": 1.7149241083644653,
+            "heading": -0.4972998071219025,
+            "angularVelocity": -1.0371412033604122,
+            "velocityX": 2.2966455737248266,
+            "velocityY": -0.7453744173332496,
+            "timestamp": 1.7839830539339752
+        },
+        {
+            "x": 4.956447647832475,
+            "y": 1.6911654776329053,
+            "heading": -0.531527000493052,
+            "angularVelocity": -0.9387377715499247,
+            "velocityX": 2.008757542132538,
+            "velocityY": -0.6516200094519161,
+            "timestamp": 1.8204439212906172
+        },
+        {
+            "x": 5.019204190370849,
+            "y": 1.670815407546972,
+            "heading": -0.5616288284566417,
+            "angularVelocity": -0.8255927559032131,
+            "velocityX": 1.721202678052634,
+            "velocityY": -0.5581345579873002,
+            "timestamp": 1.8569047886472592
+        },
+        {
+            "x": 5.071485904769321,
+            "y": 1.6538669030565063,
+            "heading": -0.5872312038868318,
+            "angularVelocity": -0.7021877779198676,
+            "velocityX": 1.433913074175601,
+            "velocityY": -0.46484095742112364,
+            "timestamp": 1.8933656560039012
+        },
+        {
+            "x": 5.113300541419085,
+            "y": 1.6403147551859525,
+            "heading": -0.6080594891340769,
+            "angularVelocity": -0.5712504050853902,
+            "velocityX": 1.14683603768266,
+            "velocityY": -0.3716902216832496,
+            "timestamp": 1.9298265233605432
+        },
+        {
+            "x": 5.144654410764751,
+            "y": 1.6301549374248205,
+            "heading": -0.6239033656036709,
+            "angularVelocity": -0.43454469457947725,
+            "velocityX": 0.8599320756409368,
+            "velocityY": -0.2786499197003079,
+            "timestamp": 1.9662873907171852
+        },
+        {
+            "x": 5.165552750765901,
+            "y": 1.6233842339153202,
+            "heading": -0.634595855168354,
+            "angularVelocity": -0.29325933089016243,
+            "velocityX": 0.57317177336273,
+            "velocityY": -0.18569781797215093,
+            "timestamp": 2.002748258073827
         },
         {
             "x": 5.176,
             "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.18096626962419662,
-            "velocityX": 0.35103691280557286,
-            "velocityY": -0.1145396978893815,
-            "timestamp": 2.0340860135155054
+            "heading": -0.64,
+            "angularVelocity": -0.14821767070941108,
+            "velocityX": 0.28653320646240676,
+            "velocityY": -0.09281825037833577,
+            "timestamp": 2.039209125430469
         },
         {
             "x": 5.176,
             "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 7.15236707545134e-33,
-            "velocityX": -4.116164438326626e-35,
-            "velocityY": 1.5643849141244448e-34,
-            "timestamp": 2.078782444994358
+            "heading": -0.64,
+            "angularVelocity": 2.131559152063316e-30,
+            "velocityX": 2.0336113095175017e-32,
+            "velocityY": 7.340802046004562e-32,
+            "timestamp": 2.075669992787111
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.traj
index 9212064d..0ce351ef 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.traj	
@@ -3,497 +3,551 @@
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": -4.120593510226553e-35,
+            "velocityY": 6.585905017096791e-33,
             "timestamp": 0
         },
         {
-            "x": 2.2473912878554576,
-            "y": 5.551785895834254,
-            "heading": 3.1358246059620716,
-            "angularVelocity": -0.12928713365744599,
-            "velocityX": -0.08077630820983339,
-            "velocityY": -0.2543419791057163,
-            "timestamp": 0.03229551092834574
-        },
-        {
-            "x": 2.242172157117942,
-            "y": 5.535355099551312,
-            "heading": 3.127553990435098,
-            "angularVelocity": -0.25609180004378934,
-            "velocityX": -0.16160545498398599,
-            "velocityY": -0.5087640916843489,
-            "timestamp": 0.06459102185669148
-        },
-        {
-            "x": 2.2343406441985514,
-            "y": 5.510704667985898,
-            "heading": 3.115281361261821,
-            "angularVelocity": -0.38001037359361073,
-            "velocityX": -0.24249540243429488,
-            "velocityY": -0.7632773365965662,
-            "timestamp": 0.09688653278503723
-        },
-        {
-            "x": 2.2238944664634275,
-            "y": 5.477831199999642,
-            "heading": 3.0991173688763074,
-            "angularVelocity": -0.5005027609371739,
-            "velocityX": -0.3234560294866074,
-            "velocityY": -1.017895894546866,
-            "timestamp": 0.12918204371338296
-        },
-        {
-            "x": 2.2108309419755985,
-            "y": 5.43673070422605,
-            "heading": 3.0791962484868605,
-            "angularVelocity": -0.6168386818107923,
-            "velocityX": -0.40449970018474013,
-            "velocityY": -1.272638041391628,
-            "timestamp": 0.1614775546417287
-        },
-        {
-            "x": 2.19514688330712,
-            "y": 5.387398428523646,
-            "heading": 3.0556846451348814,
-            "angularVelocity": -0.7280145963364562,
-            "velocityX": -0.4856420665793569,
-            "velocityY": -1.5275273338098838,
-            "timestamp": 0.19377306557007443
-        },
-        {
-            "x": 2.17683845193207,
-            "y": 5.329828642238818,
-            "heading": 3.028794799017408,
-            "angularVelocity": -0.832618693574285,
-            "velocityX": -0.5669032893045367,
-            "velocityY": -1.782594070505913,
-            "timestamp": 0.22606857649842016
-        },
-        {
-            "x": 2.1559009488672944,
-            "y": 5.264014369161548,
-            "heading": 2.9988052787110364,
-            "angularVelocity": -0.9285971778836174,
-            "velocityX": -0.6483100116059459,
-            "velocityY": -2.037876818957682,
-            "timestamp": 0.2583640874267659
-        },
-        {
-            "x": 2.132328497947263,
-            "y": 5.189947104033879,
-            "heading": 2.966095877804453,
-            "angularVelocity": -1.012815712349497,
-            "velocityX": -0.7298986838242586,
-            "velocityY": -2.293422924690741,
-            "timestamp": 0.2906595983551117
-        },
-        {
-            "x": 2.1061135495185,
-            "y": 5.107616699879427,
-            "heading": 2.9312120529633434,
-            "angularVelocity": -1.0801446962243946,
-            "velocityX": -0.8117211239334865,
-            "velocityY": -2.549283221966035,
-            "timestamp": 0.32295510928345744
-        },
-        {
-            "x": 2.0772461397099717,
-            "y": 5.017012347335229,
-            "heading": 2.89500074789377,
-            "angularVelocity": -1.1212488679902226,
-            "velocityX": -0.8938520859006283,
-            "velocityY": -2.805478220958382,
-            "timestamp": 0.3552506202118032
-        },
-        {
-            "x": 2.0457134444911373,
-            "y": 4.918129964629122,
-            "heading": 2.858963174705018,
-            "angularVelocity": -1.1158694243515237,
-            "velocityX": -0.976380131864019,
-            "velocityY": -3.0617996081715195,
-            "timestamp": 0.38754613114014896
-        },
-        {
-            "x": 2.0115103463196724,
-            "y": 4.811035711883079,
-            "heading": 2.8266343874462354,
-            "angularVelocity": -1.0010303701499224,
-            "velocityX": -1.0590666376931392,
-            "velocityY": -3.3160724096811367,
-            "timestamp": 0.4198416420684947
-        },
-        {
-            "x": 1.9751456955721198,
-            "y": 4.697500624139868,
-            "heading": 2.816667462304015,
-            "angularVelocity": -0.30861642549437257,
-            "velocityX": -1.1259970721081056,
-            "velocityY": -3.5155067834385165,
-            "timestamp": 0.4521371529968405
-        },
-        {
-            "x": 1.9382198046413848,
-            "y": 4.581042609670271,
-            "heading": 2.816667439698893,
-            "angularVelocity": -6.999462561903041e-7,
-            "velocityX": -1.1433753444143555,
-            "velocityY": -3.6060124494695995,
-            "timestamp": 0.48443266392518625
-        },
-        {
-            "x": 1.901294488957654,
-            "y": 4.4645844128058085,
-            "heading": 2.816667417094136,
-            "angularVelocity": -6.999349526609492e-7,
-            "velocityX": -1.1433575324340735,
-            "velocityY": -3.6060180971544047,
-            "timestamp": 0.516728174853532
-        },
-        {
-            "x": 1.8682421277834205,
-            "y": 4.346968376660799,
-            "heading": 2.816667394414886,
-            "angularVelocity": -7.022415636891642e-7,
-            "velocityX": -1.0234351531879067,
-            "velocityY": -3.6418694971559726,
-            "timestamp": 0.5490236857818777
-        },
-        {
-            "x": 1.8436394280164332,
-            "y": 4.227299259504863,
-            "heading": 2.81666737130494,
-            "angularVelocity": -7.15577656003266e-7,
-            "velocityX": -0.7617993665303343,
-            "velocityY": -3.705441211982887,
-            "timestamp": 0.5813191967102234
-        },
-        {
-            "x": 1.8276118980147091,
-            "y": 4.1061831643448095,
-            "heading": 2.8166673472723573,
-            "angularVelocity": -7.441462228176694e-7,
-            "velocityX": -0.4962773320813651,
-            "velocityY": -3.7502455195328888,
-            "timestamp": 0.6136147076385691
-        },
-        {
-            "x": 1.8202409034217624,
-            "y": 3.984233755462536,
-            "heading": 2.8166673217380835,
-            "angularVelocity": -7.906446726868519e-7,
-            "velocityX": -0.22823588731266858,
-            "velocityY": -3.776048292062723,
-            "timestamp": 0.6459102185669148
-        },
-        {
-            "x": 1.821563847305773,
-            "y": 3.8620689519300933,
-            "heading": 2.816667293968721,
-            "angularVelocity": -8.598520920997921e-7,
-            "velocityX": 0.04096370814339873,
-            "velocityY": -3.782717784013104,
-            "timestamp": 0.6782057294952605
-        },
-        {
-            "x": 1.831574050917961,
-            "y": 3.740307773577626,
-            "heading": 2.816667262971477,
-            "angularVelocity": -9.5980040013024e-7,
-            "velocityX": 0.30995650245006834,
-            "velocityY": -3.7702199114489767,
-            "timestamp": 0.7105012404236062
-        },
-        {
-            "x": 1.8502208046317663,
-            "y": 3.6195671988548175,
-            "heading": 2.816667227314634,
-            "angularVelocity": -0.00000110408047182699,
-            "velocityX": 0.5773791210542256,
-            "velocityY": -3.738617883788745,
-            "timestamp": 0.7427967513519519
-        },
-        {
-            "x": 1.8774096306565946,
-            "y": 3.5004590367763506,
-            "heading": 2.8166671847943117,
-            "angularVelocity": -0.0000013166016324392044,
-            "velocityX": 0.841876324085786,
-            "velocityY": -3.688071767705825,
-            "timestamp": 0.7750922622802976
-        },
-        {
-            "x": 1.913002764036886,
-            "y": 3.383586826300032,
-            "heading": 2.8166671317574883,
-            "angularVelocity": -0.0000016422351652599994,
-            "velocityX": 1.1021077653566822,
-            "velocityY": -3.618837637702142,
-            "timestamp": 0.8073877732086433
-        },
-        {
-            "x": 1.9568198515105035,
-            "y": 3.2695427783414357,
-            "heading": 2.8166670615606066,
-            "angularVelocity": -0.0000021735801416730847,
-            "velocityX": 1.3567547381688734,
-            "velocityY": -3.53126625590866,
-            "timestamp": 0.839683284136989
+            "x": 2.24734688866176,
+            "y": 5.551784954914585,
+            "heading": -0.0040460391001905256,
+            "angularVelocity": -0.1252124844254258,
+            "velocityX": -0.08210564799104332,
+            "velocityY": -0.2542304163009863,
+            "timestamp": 0.03231338407473799
+        },
+        {
+            "x": 2.242039203719346,
+            "y": 5.535352462397274,
+            "heading": -0.012062241013264058,
+            "angularVelocity": -0.24807683078112674,
+            "velocityX": -0.16425654862202216,
+            "velocityY": -0.5085351778477981,
+            "timestamp": 0.06462676814947597
+        },
+        {
+            "x": 2.2340752644376702,
+            "y": 5.510699787230797,
+            "heading": -0.023960373976211733,
+            "angularVelocity": -0.3682106750388487,
+            "velocityX": -0.24645946284226605,
+            "velocityY": -0.7629245859689895,
+            "timestamp": 0.09694015222421395
+        },
+        {
+            "x": 2.223453118891146,
+            "y": 5.477823766232339,
+            "heading": -0.039635722986674345,
+            "angularVelocity": -0.4851039115621088,
+            "velocityX": -0.32872278316489917,
+            "velocityY": -1.0174118848839497,
+            "timestamp": 0.12925353629895195
+        },
+        {
+            "x": 2.210170475892355,
+            "y": 5.4367206851300205,
+            "heading": -0.05896132885919571,
+            "angularVelocity": -0.5980681511978081,
+            "velocityX": -0.4110570087017019,
+            "velocityY": -1.2720141291067089,
+            "timestamp": 0.16156692037368994
+        },
+        {
+            "x": 2.194224615527819,
+            "y": 5.387386118415271,
+            "heading": -0.08177969695024358,
+            "angularVelocity": -0.706158415295445,
+            "velocityX": -0.4934754072075649,
+            "velocityY": -1.5267533292286215,
+            "timestamp": 0.19388030444842794
+        },
+        {
+            "x": 2.1756122677867826,
+            "y": 5.329814722150547,
+            "heading": -0.10789038891803496,
+            "angularVelocity": -0.8080457282778595,
+            "velocityX": -0.575995002503841,
+            "velocityY": -1.7816579078058363,
+            "timestamp": 0.22619368852316593
+        },
+        {
+            "x": 2.1543294408518685,
+            "y": 5.263999973874467,
+            "heading": -0.13703048715481858,
+            "angularVelocity": -0.901796548742399,
+            "velocityX": -0.6586381322887348,
+            "velocityY": -2.0367643365317547,
+            "timestamp": 0.2585070725979039
+        },
+        {
+            "x": 2.1303711674850674,
+            "y": 5.189933880722087,
+            "heading": -0.16884170077604577,
+            "angularVelocity": -0.984459366671516,
+            "velocityX": -0.7414349828352131,
+            "velocityY": -2.2921181198809735,
+            "timestamp": 0.2908204566726419
+        },
+        {
+            "x": 2.103731122650155,
+            "y": 5.107606803578156,
+            "heading": -0.20280968351747503,
+            "angularVelocity": -1.0512047473228812,
+            "velocityX": -0.8244275738281073,
+            "velocityY": -2.547770204244666,
+            "timestamp": 0.3231338407473799
+        },
+        {
+            "x": 2.0744011006982923,
+            "y": 5.0170081629947525,
+            "heading": -0.23813652479783928,
+            "angularVelocity": -1.0932572459343544,
+            "velocityX": -0.9076741044523434,
+            "velocityY": -2.8037496900311267,
+            "timestamp": 0.3554472248221179
+        },
+        {
+            "x": 2.0423709680329383,
+            "y": 4.918132526359071,
+            "heading": -0.27341203074981035,
+            "angularVelocity": -1.0916685751756738,
+            "velocityX": -0.991234238768389,
+            "velocityY": -3.05989729849995,
+            "timestamp": 0.3877606088968559
+        },
+        {
+            "x": 2.007638970172759,
+            "y": 4.811032750261161,
+            "heading": -0.30535515788271084,
+            "angularVelocity": -0.9885416847402974,
+            "velocityX": -1.0748486688935617,
+            "velocityY": -3.3144091578337234,
+            "timestamp": 0.42007399297159387
+        },
+        {
+            "x": 1.9707337326379286,
+            "y": 4.697468787945422,
+            "heading": -0.31502999189110337,
+            "angularVelocity": -0.2994064003329394,
+            "velocityX": -1.1421037626226502,
+            "velocityY": -3.5144558692173984,
+            "timestamp": 0.45238737704633186
+        },
+        {
+            "x": 1.9331993823421831,
+            "y": 4.581134405528862,
+            "heading": -0.31503001351145116,
+            "angularVelocity": -6.690833652147531e-7,
+            "velocityX": -1.1615728705149717,
+            "velocityY": -3.6001918631452843,
+            "timestamp": 0.48470076112106986
+        },
+        {
+            "x": 1.8956658919440634,
+            "y": 4.464799745676637,
+            "heading": -0.31503003513146777,
+            "angularVelocity": -6.690731172580015e-7,
+            "velocityX": -1.1615462593242614,
+            "velocityY": -3.600200448927068,
+            "timestamp": 0.5170141451958078
+        },
+        {
+            "x": 1.8630741103436357,
+            "y": 4.346985095772027,
+            "heading": -0.31503005685343444,
+            "angularVelocity": -6.722281571161063e-7,
+            "velocityX": -1.008615548437953,
+            "velocityY": -3.646001595874795,
+            "timestamp": 0.5493275292705457
+        },
+        {
+            "x": 1.8389499213812899,
+            "y": 4.227149627930832,
+            "heading": -0.3150300790516974,
+            "angularVelocity": -6.869680669187691e-7,
+            "velocityX": -0.746569560976628,
+            "velocityY": -3.708539704910713,
+            "timestamp": 0.5816409133452837
+        },
+        {
+            "x": 1.8234163358184028,
+            "y": 4.105901026539528,
+            "heading": -0.3150301022065626,
+            "angularVelocity": -7.165719666567231e-7,
+            "velocityX": -0.48071676822704484,
+            "velocityY": -3.752271848434857,
+            "timestamp": 0.6139542974200216
+        },
+        {
+            "x": 1.8165522814590422,
+            "y": 3.983854314068792,
+            "heading": -0.3150301268891266,
+            "angularVelocity": -7.638495563130961e-7,
+            "velocityX": -0.21242140233547732,
+            "velocityY": -3.7769709352772667,
+            "timestamp": 0.6462676814947595
+        },
+        {
+            "x": 1.818392625605847,
+            "y": 3.861628588886844,
+            "heading": -0.3150301538292836,
+            "angularVelocity": -8.337151223773892e-7,
+            "velocityX": 0.05695299949234183,
+            "velocityY": -3.7825108289262035,
+            "timestamp": 0.6785810655694975
+        },
+        {
+            "x": 1.828928055424025,
+            "y": 3.7398438657688415,
+            "heading": -0.315030184023291,
+            "angularVelocity": -9.344118022266017e-7,
+            "velocityX": 0.32603919768385625,
+            "velocityY": -3.768863169401422,
+            "timestamp": 0.7108944496442354
+        },
+        {
+            "x": 1.848105139864153,
+            "y": 3.6191179259506723,
+            "heading": -0.31503021891945676,
+            "angularVelocity": -0.0000010799291597080048,
+            "velocityX": 0.593471868986944,
+            "velocityY": -3.7360970778839238,
+            "timestamp": 0.7432078337189734
+        },
+        {
+            "x": 1.8758266057917121,
+            "y": 3.5000631817385077,
+            "heading": -0.3150302607662258,
+            "angularVelocity": -0.0000012950289872625492,
+            "velocityX": 0.8578942355106304,
+            "velocityY": -3.6843787062599658,
+            "timestamp": 0.7755212177937113
+        },
+        {
+            "x": 1.911951833448642,
+            "y": 3.3832835694780297,
+            "heading": -0.3150303133273049,
+            "angularVelocity": -0.000001626603978371328,
+            "velocityX": 1.1179648523774262,
+            "velocityY": -3.6139703594763453,
+            "timestamp": 0.8078346018684492
+        },
+        {
+            "x": 1.9562975704379837,
+            "y": 3.2693714862411047,
+            "heading": -0.315030383530948,
+            "angularVelocity": -0.000002172587152219365,
+            "velocityX": 1.3723643703418302,
+            "velocityY": -3.525229142619559,
+            "timestamp": 0.8401479859431872
         },
         {
             "x": 2.008638858795166,
             "y": 3.158904790878296,
-            "heading": 2.8166669305303302,
-            "angularVelocity": -0.000004057228785819985,
-            "velocityX": 1.6045266290919962,
-            "velocityY": -3.4258008089301617,
-            "timestamp": 0.8719787950653347
-        },
-        {
-            "x": 2.095074231369689,
-            "y": 3.013583476455127,
-            "heading": 2.8166668472664824,
-            "angularVelocity": -0.000001862874616466948,
-            "velocityX": 1.9338316217798797,
-            "velocityY": -3.2512956765222865,
-            "timestamp": 0.9166752265441874
-        },
-        {
-            "x": 2.195389426610098,
-            "y": 2.8774723973475123,
-            "heading": 2.8166667934576224,
-            "angularVelocity": -0.0000012038737383971035,
-            "velocityX": 2.2443669868336342,
-            "velocityY": -3.0452336932538677,
-            "timestamp": 0.9613716580230401
-        },
-        {
-            "x": 2.308610813706186,
-            "y": 2.751892601119491,
-            "heading": 2.8166667540699692,
-            "angularVelocity": -8.812259002769856e-7,
-            "velocityX": 2.5331191629841037,
-            "velocityY": -2.8096157136713913,
-            "timestamp": 1.006068089501893
-        },
-        {
-            "x": 2.433639499231453,
-            "y": 2.638062914068913,
-            "heading": 2.816666722710724,
-            "angularVelocity": -7.016051243778922e-7,
-            "velocityX": 2.7972856308321177,
-            "velocityY": -2.5467287495744175,
-            "timestamp": 1.0507645209807457
-        },
-        {
-            "x": 2.5692619900937426,
-            "y": 2.537088109244684,
-            "heading": 2.8166666961232654,
-            "angularVelocity": -5.94845213173575e-7,
-            "velocityX": 3.034302434780658,
-            "velocityY": -2.259124531496504,
-            "timestamp": 1.0954609524595984
-        },
-        {
-            "x": 2.7141619661921816,
-            "y": 2.449948169817928,
-            "heading": 2.8166666724078224,
-            "angularVelocity": -5.305891904122224e-7,
-            "velocityX": 3.241869010661312,
-            "velocityY": -1.9495950021868031,
-            "timestamp": 1.1401573839384511
-        },
-        {
-            "x": 2.866933035141265,
-            "y": 2.377488711333149,
-            "heading": 2.8166666503106814,
-            "angularVelocity": -4.943826596232375e-7,
-            "velocityX": 3.4179701576705006,
-            "velocityY": -1.6211463888132076,
-            "timestamp": 1.1848538154173038
-        },
-        {
-            "x": 3.0260921573816075,
-            "y": 2.3204120378316904,
-            "heading": 2.8166666288878814,
-            "angularVelocity": -4.79295537569982e-7,
-            "velocityX": 3.560891037031571,
-            "velocityY": -1.2769850212418752,
-            "timestamp": 1.2295502468961566
-        },
-        {
-            "x": 3.1869067054641667,
-            "y": 2.2681812614436785,
-            "heading": 2.8166666075502422,
-            "angularVelocity": -4.773902106314294e-7,
-            "velocityX": 3.597928128974802,
-            "velocityY": -1.1685670345455579,
-            "timestamp": 1.2742466783750093
-        },
-        {
-            "x": 3.3477213293158035,
-            "y": 2.2159507183434,
-            "heading": 2.8166665862126,
-            "angularVelocity": -4.773902944556367e-7,
-            "velocityX": 3.597929824167796,
-            "velocityY": -1.1685618151639285,
-            "timestamp": 1.318943109853862
-        },
-        {
-            "x": 3.508535966619386,
-            "y": 2.163720216660845,
-            "heading": 2.8166665648749514,
-            "angularVelocity": -4.773904335055608e-7,
-            "velocityX": 3.5979301251302083,
-            "velocityY": -1.1685608885189713,
-            "timestamp": 1.3636395413327147
-        },
-        {
-            "x": 3.669350617545261,
-            "y": 2.111489756920536,
-            "heading": 2.816666543537298,
-            "angularVelocity": -4.773905387074157e-7,
-            "velocityX": 3.5979304299038075,
-            "velocityY": -1.1685599501387545,
-            "timestamp": 1.4083359728115674
-        },
-        {
-            "x": 3.830165282253442,
-            "y": 2.0592593396151875,
-            "heading": 2.816666522199642,
-            "angularVelocity": -4.773905991181995e-7,
-            "velocityX": 3.597930738257429,
-            "velocityY": -1.168559000734998,
-            "timestamp": 1.4530324042904201
-        },
-        {
-            "x": 3.990979960989088,
-            "y": 2.0070289654996674,
-            "heading": 2.816666500861985,
-            "angularVelocity": -4.773906042282552e-7,
-            "velocityX": 3.597931052096018,
-            "velocityY": -1.1685580344424535,
-            "timestamp": 1.4977288357692728
-        },
-        {
-            "x": 4.151794654203474,
-            "y": 1.9547986359634688,
-            "heading": 2.8166664795243315,
-            "angularVelocity": -4.773905411161549e-7,
-            "velocityX": 3.597931376031047,
-            "velocityY": -1.1685570370625784,
-            "timestamp": 1.5424252672481256
-        },
-        {
-            "x": 4.312609353229998,
-            "y": 1.902568324322763,
-            "heading": 2.8166664581865346,
-            "angularVelocity": -4.773937498526183e-7,
-            "velocityX": 3.597931506066887,
-            "velocityY": -1.1685566366840177,
-            "timestamp": 1.5871216987269783
-        },
-        {
-            "x": 4.468360661654424,
-            "y": 1.8512590257414445,
-            "heading": 2.7996529762164895,
-            "angularVelocity": -0.38064519710247374,
-            "velocityX": 3.4846475047592103,
-            "velocityY": -1.1479506726525697,
-            "timestamp": 1.631818130205831
-        },
-        {
-            "x": 4.61007924771562,
-            "y": 1.8048518324023994,
-            "heading": 2.753515893557521,
-            "angularVelocity": -1.0322319060481902,
-            "velocityX": 3.1706912917253294,
-            "velocityY": -1.0382751330159734,
-            "timestamp": 1.6765145616846837
-        },
-        {
-            "x": 4.736004026753032,
-            "y": 1.7636749139398393,
-            "heading": 2.704438737111309,
-            "angularVelocity": -1.0980106201416129,
-            "velocityX": 2.8173340660761905,
-            "velocityY": -0.9212574046776457,
-            "timestamp": 1.7212109931635364
-        },
-        {
-            "x": 4.846114017481673,
-            "y": 1.7276966103545506,
-            "heading": 2.657457847080447,
-            "angularVelocity": -1.05111053559365,
-            "velocityX": 2.4635074229748755,
-            "velocityY": -0.8049480102748526,
-            "timestamp": 1.7659074246423891
-        },
-        {
-            "x": 4.9404369366183385,
-            "y": 1.6968910625978524,
-            "heading": 2.6148397238370804,
-            "angularVelocity": -0.9535016965175586,
-            "velocityX": 2.110300890157008,
-            "velocityY": -0.6892171642667541,
-            "timestamp": 1.8106038561212419
-        },
-        {
-            "x": 5.018999070193965,
-            "y": 1.6712409213417598,
-            "heading": 2.5778779941552346,
-            "angularVelocity": -0.8269503505964041,
-            "velocityX": 1.7576824586722803,
-            "velocityY": -0.5738744773892818,
-            "timestamp": 1.8553002876000946
-        },
-        {
-            "x": 5.08182145352524,
-            "y": 1.6507342817906896,
-            "heading": 2.547405196833757,
-            "angularVelocity": -0.6817724886134384,
-            "velocityX": 1.4055346534990782,
-            "velocityY": -0.45879813829819405,
-            "timestamp": 1.8999967190789473
-        },
-        {
-            "x": 5.128920654811687,
-            "y": 1.6353626111579855,
-            "heading": 2.524000017522932,
-            "angularVelocity": -0.52364760533286,
-            "velocityX": 1.0537575311516838,
-            "velocityY": -0.34391270452937267,
-            "timestamp": 1.9446931505578
-        },
-        {
-            "x": 5.160309902680238,
-            "y": 1.6251195157583214,
-            "heading": 2.5080885464702414,
-            "angularVelocity": -0.35598974070712264,
-            "velocityX": 0.702276374869034,
-            "velocityY": -0.2291703176462827,
-            "timestamp": 1.9893895820366527
+            "heading": -0.3150304952316807,
+            "angularVelocity": -0.00000345679463513026,
+            "velocityX": 1.6198021301675205,
+            "velocityY": -3.418604968990847,
+            "timestamp": 0.8724613700179251
+        },
+        {
+            "x": 2.0775165593610554,
+            "y": 3.0394044439018932,
+            "heading": -0.31503058092883834,
+            "angularVelocity": -0.0000023503872475309846,
+            "velocityX": 1.8890856295919138,
+            "velocityY": -3.277496001603852,
+            "timestamp": 0.9089222373745671
+        },
+        {
+            "x": 2.155767716832464,
+            "y": 2.9258208551273497,
+            "heading": -0.31503063899444206,
+            "angularVelocity": -0.0000015925458689218539,
+            "velocityX": 2.146168293419776,
+            "velocityY": -3.11521905563918,
+            "timestamp": 0.9453831047312091
+        },
+        {
+            "x": 2.242886939864493,
+            "y": 2.818887609111427,
+            "heading": -0.31503068232040304,
+            "angularVelocity": -0.000001188286624986991,
+            "velocityX": 2.38938975806231,
+            "velocityY": -2.9328223316783686,
+            "timestamp": 0.9818439720878511
+        },
+        {
+            "x": 2.3383115633038396,
+            "y": 2.719295334161189,
+            "heading": -0.31503071689328305,
+            "angularVelocity": -9.482188022909844e-7,
+            "velocityX": 2.6171791939547426,
+            "velocityY": -2.7314839763979357,
+            "timestamp": 1.0183048394444931
+        },
+        {
+            "x": 2.4414252810789625,
+            "y": 2.627687242403576,
+            "heading": -0.3150307459166021,
+            "angularVelocity": -7.96012851101892e-7,
+            "velocityX": 2.8280654095942417,
+            "velocityY": -2.5125044574926996,
+            "timestamp": 1.0547657068011351
+        },
+        {
+            "x": 2.5515621254678895,
+            "y": 2.5446549735283583,
+            "heading": -0.3150307712925926,
+            "angularVelocity": -6.959787951410741e-7,
+            "velocityX": 3.020686351523773,
+            "velocityY": -2.2772982349278084,
+            "timestamp": 1.0912265741577771
+        },
+        {
+            "x": 2.668010765906386,
+            "y": 2.470734767010798,
+            "heading": -0.31503079425241676,
+            "angularVelocity": -6.297114091058936e-7,
+            "velocityX": 3.1937978682584367,
+            "velocityY": -2.027384751835701,
+            "timestamp": 1.1276874415144191
+        },
+        {
+            "x": 2.790019095895906,
+            "y": 2.406403976394343,
+            "heading": -0.31503081565982244,
+            "angularVelocity": -5.871337458164478e-7,
+            "velocityX": 3.3462816119017478,
+            "velocityY": -1.7643790529502215,
+            "timestamp": 1.1641483088710611
+        },
+        {
+            "x": 2.916799054801627,
+            "y": 2.352077873335088,
+            "heading": -0.31503083617399774,
+            "angularVelocity": -5.626354173508468e-7,
+            "velocityX": 3.477151480397404,
+            "velocityY": -1.4899838374074656,
+            "timestamp": 1.2006091762277031
+        },
+        {
+            "x": 3.0475310715630033,
+            "y": 2.308104889106193,
+            "heading": -0.31503085634737477,
+            "angularVelocity": -5.532884563345105e-7,
+            "velocityX": 3.585543247850392,
+            "velocityY": -1.2060323140087088,
+            "timestamp": 1.2370700435843451
+        },
+        {
+            "x": 3.1788176786437843,
+            "y": 2.2658164704041117,
+            "heading": -0.3150308764961414,
+            "angularVelocity": -5.526134756982608e-7,
+            "velocityX": 3.600753810835078,
+            "velocityY": -1.1598302993847416,
+            "timestamp": 1.2735309109409871
+        },
+        {
+            "x": 3.3101043321458232,
+            "y": 2.2235281958195148,
+            "heading": -0.3150308966449121,
+            "angularVelocity": -5.526135855693721e-7,
+            "velocityX": 3.600755084015385,
+            "velocityY": -1.159826346722746,
+            "timestamp": 1.3099917782976291
+        },
+        {
+            "x": 3.441390996396766,
+            "y": 2.181239954605585,
+            "heading": -0.31503091679368933,
+            "angularVelocity": -5.526137662420421e-7,
+            "velocityX": 3.6007553788219733,
+            "velocityY": -1.1598254314766572,
+            "timestamp": 1.3464526456542711
+        },
+        {
+            "x": 3.5726776715245805,
+            "y": 2.1389517471596347,
+            "heading": -0.31503093694247225,
+            "angularVelocity": -5.526139217103117e-7,
+            "velocityX": 3.600755677138282,
+            "velocityY": -1.1598245053335783,
+            "timestamp": 1.3829135130109131
+        },
+        {
+            "x": 3.7039643576361168,
+            "y": 2.0966635738134185,
+            "heading": -0.3150309570912594,
+            "angularVelocity": -5.526140383605173e-7,
+            "velocityX": 3.6007559783851373,
+            "velocityY": -1.159823570091575,
+            "timestamp": 1.4193743803675551
+        },
+        {
+            "x": 3.835251054850112,
+            "y": 2.0543754349355976,
+            "heading": -0.31503097724004914,
+            "angularVelocity": -5.526141098258176e-7,
+            "velocityX": 3.600756282888565,
+            "velocityY": -1.1598226247384584,
+            "timestamp": 1.4558352477241971
+        },
+        {
+            "x": 3.966537763342614,
+            "y": 2.012087331072757,
+            "heading": -0.3150309973888395,
+            "angularVelocity": -5.526141250500724e-7,
+            "velocityX": 3.6007565922204043,
+            "velocityY": -1.1598216643943178,
+            "timestamp": 1.4922961150808391
+        },
+        {
+            "x": 4.097824483400825,
+            "y": 1.9697992631165677,
+            "heading": -0.315031017537628,
+            "angularVelocity": -5.526140761706961e-7,
+            "velocityX": 3.6007569094292164,
+            "velocityY": -1.1598206795946073,
+            "timestamp": 1.5287569824374811
+        },
+        {
+            "x": 4.2291112154854185,
+            "y": 1.9275112324972625,
+            "heading": -0.3150310376864126,
+            "angularVelocity": -5.526139663753558e-7,
+            "velocityX": 3.600757239272763,
+            "velocityY": -1.1598196555683948,
+            "timestamp": 1.5652178497941232
+        },
+        {
+            "x": 4.360397911943487,
+            "y": 1.8852230912740195,
+            "heading": -0.3150310578362786,
+            "angularVelocity": -5.526436278671969e-7,
+            "velocityX": 3.600756262156026,
+            "velocityY": -1.159822689065559,
+            "timestamp": 1.6016787171507652
+        },
+        {
+            "x": 4.485161433583451,
+            "y": 1.8443297112343198,
+            "heading": -0.3370639022790847,
+            "angularVelocity": -0.6042874467931063,
+            "velocityX": 3.4218473307173203,
+            "velocityY": -1.121569041122948,
+            "timestamp": 1.6381395845074072
+        },
+        {
+            "x": 4.600444501590908,
+            "y": 1.8067758967734413,
+            "heading": -0.37703885598165165,
+            "angularVelocity": -1.0963796695112746,
+            "velocityX": 3.1618301034864644,
+            "velocityY": -1.029975894252485,
+            "timestamp": 1.6746004518640492
+        },
+        {
+            "x": 4.705219738357152,
+            "y": 1.7727109130585983,
+            "heading": -0.41891394403468696,
+            "angularVelocity": -1.1484940180779766,
+            "velocityX": 2.873635334600916,
+            "velocityY": -0.9342889016225668,
+            "timestamp": 1.7110613192206912
+        },
+        {
+            "x": 4.799468915908326,
+            "y": 1.742101106125887,
+            "heading": -0.4594847392760727,
+            "angularVelocity": -1.112721615877786,
+            "velocityX": 2.5849406331801226,
+            "velocityY": -0.8395249249915333,
+            "timestamp": 1.7475221865773332
+        },
+        {
+            "x": 4.883206605537126,
+            "y": 1.7149241083644653,
+            "heading": -0.4972998071219025,
+            "angularVelocity": -1.0371412033604122,
+            "velocityX": 2.2966455737248266,
+            "velocityY": -0.7453744173332496,
+            "timestamp": 1.7839830539339752
+        },
+        {
+            "x": 4.956447647832475,
+            "y": 1.6911654776329053,
+            "heading": -0.531527000493052,
+            "angularVelocity": -0.9387377715499247,
+            "velocityX": 2.008757542132538,
+            "velocityY": -0.6516200094519161,
+            "timestamp": 1.8204439212906172
+        },
+        {
+            "x": 5.019204190370849,
+            "y": 1.670815407546972,
+            "heading": -0.5616288284566417,
+            "angularVelocity": -0.8255927559032131,
+            "velocityX": 1.721202678052634,
+            "velocityY": -0.5581345579873002,
+            "timestamp": 1.8569047886472592
+        },
+        {
+            "x": 5.071485904769321,
+            "y": 1.6538669030565063,
+            "heading": -0.5872312038868318,
+            "angularVelocity": -0.7021877779198676,
+            "velocityX": 1.433913074175601,
+            "velocityY": -0.46484095742112364,
+            "timestamp": 1.8933656560039012
+        },
+        {
+            "x": 5.113300541419085,
+            "y": 1.6403147551859525,
+            "heading": -0.6080594891340769,
+            "angularVelocity": -0.5712504050853902,
+            "velocityX": 1.14683603768266,
+            "velocityY": -0.3716902216832496,
+            "timestamp": 1.9298265233605432
+        },
+        {
+            "x": 5.144654410764751,
+            "y": 1.6301549374248205,
+            "heading": -0.6239033656036709,
+            "angularVelocity": -0.43454469457947725,
+            "velocityX": 0.8599320756409368,
+            "velocityY": -0.2786499197003079,
+            "timestamp": 1.9662873907171852
+        },
+        {
+            "x": 5.165552750765901,
+            "y": 1.6233842339153202,
+            "heading": -0.634595855168354,
+            "angularVelocity": -0.29325933089016243,
+            "velocityX": 0.57317177336273,
+            "velocityY": -0.18569781797215093,
+            "timestamp": 2.002748258073827
         },
         {
             "x": 5.176,
             "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.18096626962419662,
-            "velocityX": 0.35103691280557286,
-            "velocityY": -0.1145396978893815,
-            "timestamp": 2.0340860135155054
+            "heading": -0.64,
+            "angularVelocity": -0.14821767070941108,
+            "velocityX": 0.28653320646240676,
+            "velocityY": -0.09281825037833577,
+            "timestamp": 2.039209125430469
         },
         {
             "x": 5.176,
             "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 7.15236707545134e-33,
-            "velocityX": -4.116164438326626e-35,
-            "velocityY": 1.5643849141244448e-34,
-            "timestamp": 2.078782444994358
+            "heading": -0.64,
+            "angularVelocity": 2.131559152063316e-30,
+            "velocityX": 2.0336113095175017e-32,
+            "velocityY": 7.340802046004562e-32,
+            "timestamp": 2.075669992787111
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj
index ca906489..1bd4bff0 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj	
@@ -3,101 +3,101 @@
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
-            "velocityX": 7.733955963828693e-33,
-            "velocityY": 1.1033116642945717e-34,
+            "velocityX": -1.6570559277738772e-34,
+            "velocityY": -1.2609465104041817e-35,
             "timestamp": 0
         },
         {
-            "x": 2.2520020198561768,
-            "y": 5.607878032622388,
-            "heading": 3.1558282209608954,
-            "angularVelocity": 0.2083481053076329,
-            "velocityX": 0.026352743296497748,
-            "velocityY": 0.6302222724446542,
-            "timestamp": 0.07597007391799751
+            "x": 2.25200199902585,
+            "y": 5.607878029194235,
+            "heading": 0.015821088838010276,
+            "angularVelocity": 0.20825462300812525,
+            "velocityX": 0.026352519517453685,
+            "velocityY": 0.6302234329322915,
+            "timestamp": 0.0759699285878182
         },
         {
-            "x": 2.2560055614843093,
-            "y": 5.703634205053828,
-            "heading": 3.187479734547443,
-            "angularVelocity": 0.41663133855460127,
-            "velocityX": 0.052698930271593884,
-            "velocityY": 1.2604459557957022,
-            "timestamp": 0.15194014783599502
+            "x": 2.2560055011308777,
+            "y": 5.703634196198671,
+            "heading": 0.04745824894413579,
+            "angularVelocity": 0.4164431992265759,
+            "velocityX": 0.05269851083774254,
+            "velocityY": 1.2604482955877103,
+            "timestamp": 0.1519398571756364
         },
         {
-            "x": 2.2620099218556464,
-            "y": 5.847268779856728,
-            "heading": 3.234941916913622,
-            "angularVelocity": 0.6247484031331855,
-            "velocityX": 0.07903586322448838,
-            "velocityY": 1.8906730952761694,
-            "timestamp": 0.22791022175399253
+            "x": 2.2620098071430843,
+            "y": 5.847268765721161,
+            "heading": 0.09489872499810467,
+            "angularVelocity": 0.6244638758496344,
+            "velocityX": 0.07903529888495084,
+            "velocityY": 1.8906766426199095,
+            "timestamp": 0.2279097857634546
         },
         {
-            "x": 2.270014272624616,
-            "y": 6.038782285632903,
-            "heading": 3.2981885303415055,
-            "angularVelocity": 0.8325200985871338,
-            "velocityX": 0.10536189260009911,
-            "velocityY": 2.520907192783506,
-            "timestamp": 0.30388029567199004
+            "x": 2.270014094994434,
+            "y": 6.038782268539179,
+            "heading": 0.1581161373911332,
+            "angularVelocity": 0.8321373149634034,
+            "velocityX": 0.10536126596587263,
+            "velocityY": 2.5209119763306935,
+            "timestamp": 0.3038797143512728
         },
         {
-            "x": 2.2800095231252646,
-            "y": 6.278187238938093,
-            "heading": 3.376569786131263,
-            "angularVelocity": 1.0317385747756829,
-            "velocityX": 0.13156826083172354,
-            "velocityY": 3.151306046688924,
-            "timestamp": 0.3798503695899875
+            "x": 2.28000939681014,
+            "y": 6.278187229695624,
+            "heading": 0.23645967758045489,
+            "angularVelocity": 1.031244094151802,
+            "velocityX": 0.13156918798668546,
+            "velocityY": 3.1513121784720814,
+            "timestamp": 0.379849642939091
         },
         {
-            "x": 2.2880056735413383,
-            "y": 6.469712014906391,
-            "heading": 3.4392336964848362,
-            "angularVelocity": 0.8248499326354866,
-            "velocityX": 0.10525395071624018,
-            "velocityY": 2.5210555432002106,
-            "timestamp": 0.455820443507985
+            "x": 2.2880055947499205,
+            "y": 6.469712010307998,
+            "heading": 0.29909347070783976,
+            "angularVelocity": 0.824455074417803,
+            "velocityX": 0.10525477762609038,
+            "velocityY": 2.521060427100167,
+            "timestamp": 0.4558195715269092
         },
         {
-            "x": 2.2940028218748654,
-            "y": 6.613355955414726,
-            "heading": 3.486212731686583,
-            "angularVelocity": 0.6183886993772851,
-            "velocityX": 0.07894093060907224,
-            "velocityY": 1.8907963767862817,
-            "timestamp": 0.5317905174259825
+            "x": 2.29400278172627,
+            "y": 6.613355953306298,
+            "heading": 0.3460499566805711,
+            "angularVelocity": 0.6180930645268612,
+            "velocityX": 0.07894159028222533,
+            "velocityY": 1.8908000266480898,
+            "timestamp": 0.5317895001147275
         },
         {
-            "x": 2.29800094256259,
-            "y": 6.709118670546653,
-            "heading": 3.517527025119772,
-            "angularVelocity": 0.412192483411159,
-            "velocityX": 0.052627574010790594,
-            "velocityY": 1.2605320778717877,
-            "timestamp": 0.60776059134398
+            "x": 2.298000929157321,
+            "y": 6.709118669792275,
+            "heading": 0.37734923040208523,
+            "angularVelocity": 0.4119955659209739,
+            "velocityX": 0.052628026712297535,
+            "velocityY": 1.2605345070882814,
+            "timestamp": 0.6077594287025456
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 0.20611118631681047,
-            "velocityX": 0.026313748747538647,
-            "velocityY": 0.6302656688873288,
-            "timestamp": 0.6837306652619775
+            "heading": 0.393,
+            "angularVelocity": 0.20601269329644184,
+            "velocityX": 0.026313975540574065,
+            "velocityY": 0.6302668845130636,
+            "timestamp": 0.6837293572903638
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": -1.7034359069625615e-32,
-            "velocityX": 1.3795062534340913e-33,
-            "velocityY": 0,
-            "timestamp": 0.7597007391799749
+            "heading": 0.393,
+            "angularVelocity": 1.653751635278537e-33,
+            "velocityX": 2.0986524182646467e-33,
+            "velocityY": -2.349121999954591e-34,
+            "timestamp": 0.7596992858781819
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.traj
index ca906489..1bd4bff0 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.traj	
@@ -3,101 +3,101 @@
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
-            "velocityX": 7.733955963828693e-33,
-            "velocityY": 1.1033116642945717e-34,
+            "velocityX": -1.6570559277738772e-34,
+            "velocityY": -1.2609465104041817e-35,
             "timestamp": 0
         },
         {
-            "x": 2.2520020198561768,
-            "y": 5.607878032622388,
-            "heading": 3.1558282209608954,
-            "angularVelocity": 0.2083481053076329,
-            "velocityX": 0.026352743296497748,
-            "velocityY": 0.6302222724446542,
-            "timestamp": 0.07597007391799751
+            "x": 2.25200199902585,
+            "y": 5.607878029194235,
+            "heading": 0.015821088838010276,
+            "angularVelocity": 0.20825462300812525,
+            "velocityX": 0.026352519517453685,
+            "velocityY": 0.6302234329322915,
+            "timestamp": 0.0759699285878182
         },
         {
-            "x": 2.2560055614843093,
-            "y": 5.703634205053828,
-            "heading": 3.187479734547443,
-            "angularVelocity": 0.41663133855460127,
-            "velocityX": 0.052698930271593884,
-            "velocityY": 1.2604459557957022,
-            "timestamp": 0.15194014783599502
+            "x": 2.2560055011308777,
+            "y": 5.703634196198671,
+            "heading": 0.04745824894413579,
+            "angularVelocity": 0.4164431992265759,
+            "velocityX": 0.05269851083774254,
+            "velocityY": 1.2604482955877103,
+            "timestamp": 0.1519398571756364
         },
         {
-            "x": 2.2620099218556464,
-            "y": 5.847268779856728,
-            "heading": 3.234941916913622,
-            "angularVelocity": 0.6247484031331855,
-            "velocityX": 0.07903586322448838,
-            "velocityY": 1.8906730952761694,
-            "timestamp": 0.22791022175399253
+            "x": 2.2620098071430843,
+            "y": 5.847268765721161,
+            "heading": 0.09489872499810467,
+            "angularVelocity": 0.6244638758496344,
+            "velocityX": 0.07903529888495084,
+            "velocityY": 1.8906766426199095,
+            "timestamp": 0.2279097857634546
         },
         {
-            "x": 2.270014272624616,
-            "y": 6.038782285632903,
-            "heading": 3.2981885303415055,
-            "angularVelocity": 0.8325200985871338,
-            "velocityX": 0.10536189260009911,
-            "velocityY": 2.520907192783506,
-            "timestamp": 0.30388029567199004
+            "x": 2.270014094994434,
+            "y": 6.038782268539179,
+            "heading": 0.1581161373911332,
+            "angularVelocity": 0.8321373149634034,
+            "velocityX": 0.10536126596587263,
+            "velocityY": 2.5209119763306935,
+            "timestamp": 0.3038797143512728
         },
         {
-            "x": 2.2800095231252646,
-            "y": 6.278187238938093,
-            "heading": 3.376569786131263,
-            "angularVelocity": 1.0317385747756829,
-            "velocityX": 0.13156826083172354,
-            "velocityY": 3.151306046688924,
-            "timestamp": 0.3798503695899875
+            "x": 2.28000939681014,
+            "y": 6.278187229695624,
+            "heading": 0.23645967758045489,
+            "angularVelocity": 1.031244094151802,
+            "velocityX": 0.13156918798668546,
+            "velocityY": 3.1513121784720814,
+            "timestamp": 0.379849642939091
         },
         {
-            "x": 2.2880056735413383,
-            "y": 6.469712014906391,
-            "heading": 3.4392336964848362,
-            "angularVelocity": 0.8248499326354866,
-            "velocityX": 0.10525395071624018,
-            "velocityY": 2.5210555432002106,
-            "timestamp": 0.455820443507985
+            "x": 2.2880055947499205,
+            "y": 6.469712010307998,
+            "heading": 0.29909347070783976,
+            "angularVelocity": 0.824455074417803,
+            "velocityX": 0.10525477762609038,
+            "velocityY": 2.521060427100167,
+            "timestamp": 0.4558195715269092
         },
         {
-            "x": 2.2940028218748654,
-            "y": 6.613355955414726,
-            "heading": 3.486212731686583,
-            "angularVelocity": 0.6183886993772851,
-            "velocityX": 0.07894093060907224,
-            "velocityY": 1.8907963767862817,
-            "timestamp": 0.5317905174259825
+            "x": 2.29400278172627,
+            "y": 6.613355953306298,
+            "heading": 0.3460499566805711,
+            "angularVelocity": 0.6180930645268612,
+            "velocityX": 0.07894159028222533,
+            "velocityY": 1.8908000266480898,
+            "timestamp": 0.5317895001147275
         },
         {
-            "x": 2.29800094256259,
-            "y": 6.709118670546653,
-            "heading": 3.517527025119772,
-            "angularVelocity": 0.412192483411159,
-            "velocityX": 0.052627574010790594,
-            "velocityY": 1.2605320778717877,
-            "timestamp": 0.60776059134398
+            "x": 2.298000929157321,
+            "y": 6.709118669792275,
+            "heading": 0.37734923040208523,
+            "angularVelocity": 0.4119955659209739,
+            "velocityX": 0.052628026712297535,
+            "velocityY": 1.2605345070882814,
+            "timestamp": 0.6077594287025456
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 0.20611118631681047,
-            "velocityX": 0.026313748747538647,
-            "velocityY": 0.6302656688873288,
-            "timestamp": 0.6837306652619775
+            "heading": 0.393,
+            "angularVelocity": 0.20601269329644184,
+            "velocityX": 0.026313975540574065,
+            "velocityY": 0.6302668845130636,
+            "timestamp": 0.6837293572903638
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": -1.7034359069625615e-32,
-            "velocityX": 1.3795062534340913e-33,
-            "velocityY": 0,
-            "timestamp": 0.7597007391799749
+            "heading": 0.393,
+            "angularVelocity": 1.653751635278537e-33,
+            "velocityX": 2.0986524182646467e-33,
+            "velocityY": -2.349121999954591e-34,
+            "timestamp": 0.7596992858781819
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj
index 5e33e1a6..a9c39200 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj	
@@ -3,101 +3,101 @@
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
+            "heading": -6.311192632400173e-33,
             "angularVelocity": 0,
-            "velocityX": -1.4697340738263578e-34,
-            "velocityY": 1.0017538611600725e-35,
+            "velocityX": 0,
+            "velocityY": 1.0022615189154172e-34,
             "timestamp": 0
         },
         {
-            "x": 2.2520018980434946,
-            "y": 5.512001884856013,
-            "heading": 3.124302281529173,
-            "angularVelocity": -0.20638131373671467,
-            "velocityX": 0.026319388320742283,
-            "velocityY": -0.6310416433261579,
-            "timestamp": 0.07606172374138878
+            "x": 2.2520019144790657,
+            "y": 5.512001876818988,
+            "heading": -0.015697245285858377,
+            "angularVelocity": -0.2063751026737087,
+            "velocityX": 0.02631960567841837,
+            "velocityY": -0.6310417795758498,
+            "timestamp": 0.07606172005484914
         },
         {
-            "x": 2.2560052212232096,
-            "y": 5.416005549224728,
-            "heading": 3.092911780798619,
-            "angularVelocity": -0.4126977300341266,
-            "velocityX": 0.05263255922684882,
-            "velocityY": -1.2620846716237228,
-            "timestamp": 0.15212344748277756
+            "x": 2.256005269562975,
+            "y": 5.416005526715618,
+            "heading": -0.04708689462727081,
+            "angularVelocity": -0.41268655663817416,
+            "velocityX": 0.05263298122921632,
+            "velocityY": -1.2620849230617632,
+            "timestamp": 0.15212344010969828
         },
         {
-            "x": 2.262009303064208,
-            "y": 5.272010738006526,
-            "heading": 3.045840934307484,
-            "angularVelocity": -0.618850640976494,
-            "velocityX": 0.07893696784222709,
-            "velocityY": -1.8931310537713693,
-            "timestamp": 0.22818517122416634
+            "x": 2.26200939656581,
+            "y": 5.272010697248206,
+            "heading": -0.09415666260883829,
+            "angularVelocity": -0.6188364915705936,
+            "velocityX": 0.07893756542062844,
+            "velocityY": -1.8931313854535041,
+            "timestamp": 0.22818516016454743
         },
         {
-            "x": 2.2700133593824035,
-            "y": 5.080016941067703,
-            "heading": 2.9831153695364567,
-            "angularVelocity": -0.8246666218648364,
-            "velocityX": 0.10523106661912582,
-            "velocityY": -2.524184142757597,
-            "timestamp": 0.3042468949655551
+            "x": 2.2700135068749114,
+            "y": 5.080016881567182,
+            "heading": -0.15688110795154597,
+            "angularVelocity": -0.8246519444666291,
+            "velocityX": 0.10523178154963624,
+            "velocityY": -2.5241845115068955,
+            "timestamp": 0.30424688021939655
         },
         {
-            "x": 2.2800088926734445,
-            "y": 4.840012206333479,
-            "heading": 2.90536283495199,
-            "angularVelocity": -1.0222294573395003,
-            "velocityX": 0.1314134468609495,
-            "velocityY": -3.1553943682665504,
-            "timestamp": 0.3803086187069439
+            "x": 2.280008999433411,
+            "y": 4.840012166841925,
+            "heading": -0.23463480916086144,
+            "angularVelocity": -1.0222448447556298,
+            "velocityX": 0.13141291771066904,
+            "velocityY": -3.155394258139129,
+            "timestamp": 0.3803086002742457
         },
         {
-            "x": 2.288005283531118,
-            "y": 4.648007638478312,
-            "heading": 2.8432005737453654,
-            "angularVelocity": -0.8172607475736078,
-            "velocityX": 0.10513028714497022,
-            "velocityY": -2.524325750333843,
-            "timestamp": 0.45637034244833263
+            "x": 2.2880053509029836,
+            "y": 4.648007615057375,
+            "heading": -0.29679802026954394,
+            "angularVelocity": -0.8172732757536348,
+            "velocityX": 0.10512977439645693,
+            "velocityY": -2.524325661398308,
+            "timestamp": 0.4563703203290948
         },
         {
-            "x": 2.2940026200964168,
-            "y": 4.504003869225784,
-            "heading": 2.7965971516354546,
-            "angularVelocity": -0.6127053111281423,
-            "velocityX": 0.07884828623776054,
-            "velocityY": -1.8932488269940233,
-            "timestamp": 0.5324320661897214
+            "x": 2.2940026546256975,
+            "y": 4.504003857619058,
+            "heading": -0.34340214900619426,
+            "angularVelocity": -0.6127146310002387,
+            "velocityX": 0.07884785827074504,
+            "velocityY": -1.893248763431504,
+            "timestamp": 0.532432040383944
         },
         {
-            "x": 2.2980008723577208,
-            "y": 4.408001271885206,
-            "heading": 2.7655331089053763,
-            "angularVelocity": -0.40840571580650337,
-            "velocityX": 0.052565890761270766,
-            "velocityY": -1.262166995675615,
-            "timestamp": 0.6084937899311101
+            "x": 2.2980008838862798,
+            "y": 4.408001268016137,
+            "heading": -0.37446665790253814,
+            "angularVelocity": -0.4084118643904297,
+            "velocityX": 0.05256559091352319,
+            "velocityY": -1.26216695512135,
+            "timestamp": 0.6084937604387931
         },
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": -0.20421715603224325,
-            "velocityX": 0.026282965254326718,
-            "velocityY": -0.6310831456885047,
-            "timestamp": 0.6845555136724989
+            "heading": -0.39,
+            "angularVelocity": -0.20422023175732293,
+            "velocityX": 0.02628281495972678,
+            "velocityY": -0.6310831254081802,
+            "timestamp": 0.6845554804936422
         },
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
+            "heading": -0.39,
             "angularVelocity": 0,
-            "velocityX": 2.533367966722586e-34,
-            "velocityY": 7.359145603780692e-32,
-            "timestamp": 0.7606172374138876
+            "velocityX": -1.9690468515346402e-33,
+            "velocityY": 7.92544572453684e-31,
+            "timestamp": 0.7606172005484914
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.traj
index 5e33e1a6..a9c39200 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.traj	
@@ -3,101 +3,101 @@
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
+            "heading": -6.311192632400173e-33,
             "angularVelocity": 0,
-            "velocityX": -1.4697340738263578e-34,
-            "velocityY": 1.0017538611600725e-35,
+            "velocityX": 0,
+            "velocityY": 1.0022615189154172e-34,
             "timestamp": 0
         },
         {
-            "x": 2.2520018980434946,
-            "y": 5.512001884856013,
-            "heading": 3.124302281529173,
-            "angularVelocity": -0.20638131373671467,
-            "velocityX": 0.026319388320742283,
-            "velocityY": -0.6310416433261579,
-            "timestamp": 0.07606172374138878
+            "x": 2.2520019144790657,
+            "y": 5.512001876818988,
+            "heading": -0.015697245285858377,
+            "angularVelocity": -0.2063751026737087,
+            "velocityX": 0.02631960567841837,
+            "velocityY": -0.6310417795758498,
+            "timestamp": 0.07606172005484914
         },
         {
-            "x": 2.2560052212232096,
-            "y": 5.416005549224728,
-            "heading": 3.092911780798619,
-            "angularVelocity": -0.4126977300341266,
-            "velocityX": 0.05263255922684882,
-            "velocityY": -1.2620846716237228,
-            "timestamp": 0.15212344748277756
+            "x": 2.256005269562975,
+            "y": 5.416005526715618,
+            "heading": -0.04708689462727081,
+            "angularVelocity": -0.41268655663817416,
+            "velocityX": 0.05263298122921632,
+            "velocityY": -1.2620849230617632,
+            "timestamp": 0.15212344010969828
         },
         {
-            "x": 2.262009303064208,
-            "y": 5.272010738006526,
-            "heading": 3.045840934307484,
-            "angularVelocity": -0.618850640976494,
-            "velocityX": 0.07893696784222709,
-            "velocityY": -1.8931310537713693,
-            "timestamp": 0.22818517122416634
+            "x": 2.26200939656581,
+            "y": 5.272010697248206,
+            "heading": -0.09415666260883829,
+            "angularVelocity": -0.6188364915705936,
+            "velocityX": 0.07893756542062844,
+            "velocityY": -1.8931313854535041,
+            "timestamp": 0.22818516016454743
         },
         {
-            "x": 2.2700133593824035,
-            "y": 5.080016941067703,
-            "heading": 2.9831153695364567,
-            "angularVelocity": -0.8246666218648364,
-            "velocityX": 0.10523106661912582,
-            "velocityY": -2.524184142757597,
-            "timestamp": 0.3042468949655551
+            "x": 2.2700135068749114,
+            "y": 5.080016881567182,
+            "heading": -0.15688110795154597,
+            "angularVelocity": -0.8246519444666291,
+            "velocityX": 0.10523178154963624,
+            "velocityY": -2.5241845115068955,
+            "timestamp": 0.30424688021939655
         },
         {
-            "x": 2.2800088926734445,
-            "y": 4.840012206333479,
-            "heading": 2.90536283495199,
-            "angularVelocity": -1.0222294573395003,
-            "velocityX": 0.1314134468609495,
-            "velocityY": -3.1553943682665504,
-            "timestamp": 0.3803086187069439
+            "x": 2.280008999433411,
+            "y": 4.840012166841925,
+            "heading": -0.23463480916086144,
+            "angularVelocity": -1.0222448447556298,
+            "velocityX": 0.13141291771066904,
+            "velocityY": -3.155394258139129,
+            "timestamp": 0.3803086002742457
         },
         {
-            "x": 2.288005283531118,
-            "y": 4.648007638478312,
-            "heading": 2.8432005737453654,
-            "angularVelocity": -0.8172607475736078,
-            "velocityX": 0.10513028714497022,
-            "velocityY": -2.524325750333843,
-            "timestamp": 0.45637034244833263
+            "x": 2.2880053509029836,
+            "y": 4.648007615057375,
+            "heading": -0.29679802026954394,
+            "angularVelocity": -0.8172732757536348,
+            "velocityX": 0.10512977439645693,
+            "velocityY": -2.524325661398308,
+            "timestamp": 0.4563703203290948
         },
         {
-            "x": 2.2940026200964168,
-            "y": 4.504003869225784,
-            "heading": 2.7965971516354546,
-            "angularVelocity": -0.6127053111281423,
-            "velocityX": 0.07884828623776054,
-            "velocityY": -1.8932488269940233,
-            "timestamp": 0.5324320661897214
+            "x": 2.2940026546256975,
+            "y": 4.504003857619058,
+            "heading": -0.34340214900619426,
+            "angularVelocity": -0.6127146310002387,
+            "velocityX": 0.07884785827074504,
+            "velocityY": -1.893248763431504,
+            "timestamp": 0.532432040383944
         },
         {
-            "x": 2.2980008723577208,
-            "y": 4.408001271885206,
-            "heading": 2.7655331089053763,
-            "angularVelocity": -0.40840571580650337,
-            "velocityX": 0.052565890761270766,
-            "velocityY": -1.262166995675615,
-            "timestamp": 0.6084937899311101
+            "x": 2.2980008838862798,
+            "y": 4.408001268016137,
+            "heading": -0.37446665790253814,
+            "angularVelocity": -0.4084118643904297,
+            "velocityX": 0.05256559091352319,
+            "velocityY": -1.26216695512135,
+            "timestamp": 0.6084937604387931
         },
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": -0.20421715603224325,
-            "velocityX": 0.026282965254326718,
-            "velocityY": -0.6310831456885047,
-            "timestamp": 0.6845555136724989
+            "heading": -0.39,
+            "angularVelocity": -0.20422023175732293,
+            "velocityX": 0.02628281495972678,
+            "velocityY": -0.6310831254081802,
+            "timestamp": 0.6845554804936422
         },
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
+            "heading": -0.39,
             "angularVelocity": 0,
-            "velocityX": 2.533367966722586e-34,
-            "velocityY": 7.359145603780692e-32,
-            "timestamp": 0.7606172374138876
+            "velocityX": -1.9690468515346402e-33,
+            "velocityY": 7.92544572453684e-31,
+            "timestamp": 0.7606172005484914
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.1.traj
index ff4b41b1..5529ce29 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.1.traj	
@@ -3,515 +3,551 @@
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
+            "heading": -0.39,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": -5.253317174178206e-37,
+            "velocityY": 1.095053106710567e-32,
             "timestamp": 0
         },
         {
-            "x": 2.3042260943612694,
-            "y": 4.3705069747477046,
-            "heading": 2.7457396895687083,
-            "angularVelocity": -0.11528375442815593,
-            "velocityX": 0.11435786954779384,
-            "velocityY": 0.2843181303644528,
-            "timestamp": 0.03695499381005128
-        },
-        {
-            "x": 2.312934956515594,
-            "y": 4.391423172843284,
-            "heading": 2.737520345774514,
-            "angularVelocity": -0.2224149687709801,
-            "velocityX": 0.23566130734830049,
-            "velocityY": 0.5659911134903584,
-            "timestamp": 0.07390998762010256
-        },
-        {
-            "x": 2.326431561960596,
-            "y": 4.422622192917349,
-            "heading": 2.725704837211404,
-            "angularVelocity": -0.31972697989997184,
-            "velocityX": 0.3652173645157235,
-            "velocityY": 0.8442436828545633,
-            "timestamp": 0.11086498143015384
-        },
-        {
-            "x": 2.3450829408251197,
-            "y": 4.463936524002756,
-            "heading": 2.7107373323748916,
-            "angularVelocity": -0.40501981717128094,
-            "velocityX": 0.5047052357900914,
-            "velocityY": 1.1179634151141484,
-            "timestamp": 0.14781997524020513
-        },
-        {
-            "x": 2.369336863920227,
-            "y": 4.515137741117879,
-            "heading": 2.6931719166575077,
-            "angularVelocity": -0.4753191356943592,
-            "velocityX": 0.6563097593730617,
-            "velocityY": 1.3855019805522522,
-            "timestamp": 0.18477496905025642
-        },
-        {
-            "x": 2.3997470229979276,
-            "y": 4.575903866240714,
-            "heading": 2.6737152145246563,
-            "angularVelocity": -0.5264972369596066,
-            "velocityX": 0.8228971498144095,
-            "velocityY": 1.6443278392948362,
-            "timestamp": 0.22172996286030772
-        },
-        {
-            "x": 2.4370055110126323,
-            "y": 4.645763622209906,
-            "heading": 2.6532916288606927,
-            "angularVelocity": -0.5526610495171655,
-            "velocityX": 1.0082125356646991,
-            "velocityY": 1.890400965246281,
-            "timestamp": 0.258684956670359
-        },
-        {
-            "x": 2.4818450920186503,
-            "y": 4.724119837508184,
-            "heading": 2.6331026993833615,
-            "angularVelocity": -0.5463112666478134,
-            "velocityX": 1.213356474540179,
-            "velocityY": 2.1203146644004023,
-            "timestamp": 0.2956399504804103
-        },
-        {
-            "x": 2.5345067531707874,
-            "y": 4.810746963179403,
-            "heading": 2.6138856788459637,
-            "angularVelocity": -0.5200114668175445,
-            "velocityX": 1.425021511918485,
-            "velocityY": 2.3441250218165925,
-            "timestamp": 0.3325949442904616
-        },
-        {
-            "x": 2.595354080808564,
-            "y": 4.90526717496031,
-            "heading": 2.5968144984302115,
-            "angularVelocity": -0.46194515695220434,
-            "velocityX": 1.6465251746633045,
-            "velocityY": 2.557711476471661,
-            "timestamp": 0.3695499381005129
-        },
-        {
-            "x": 2.6649832778781826,
-            "y": 5.00693656288579,
-            "heading": 2.584029518775602,
-            "angularVelocity": -0.3459608116923114,
-            "velocityX": 1.8841620547283313,
-            "velocityY": 2.7511677703982533,
-            "timestamp": 0.4065049319105642
-        },
-        {
-            "x": 2.7444162884451204,
-            "y": 5.113834606219336,
-            "heading": 2.580154938465408,
-            "angularVelocity": -0.10484591960993231,
-            "velocityX": 2.149452682233515,
-            "velocityY": 2.8926548840192674,
-            "timestamp": 0.4434599257206155
-        },
-        {
-            "x": 2.835170549378822,
-            "y": 5.2201703041601135,
-            "heading": 2.5801550588658806,
-            "angularVelocity": 0.0000032580298363436347,
-            "velocityX": 2.4558050638616957,
-            "velocityY": 2.877437850141254,
-            "timestamp": 0.4804149195306668
-        },
-        {
-            "x": 2.934277899482491,
-            "y": 5.318767249922712,
-            "heading": 2.580155115984828,
-            "angularVelocity": 0.0000015456354283753369,
-            "velocityX": 2.681839174783285,
-            "velocityY": 2.6680276627669444,
-            "timestamp": 0.517369913340718
-        },
-        {
-            "x": 3.041080419735732,
-            "y": 5.408971720433865,
-            "heading": 2.5801551701001046,
-            "angularVelocity": 0.0000014643562563831181,
-            "velocityX": 2.8900700349783843,
-            "velocityY": 2.440927766753384,
-            "timestamp": 0.5543249071507693
-        },
-        {
-            "x": 3.1521380355083095,
-            "y": 5.493882443444743,
-            "heading": 2.5801552231562916,
-            "angularVelocity": 0.0000014356973685406667,
-            "velocityX": 3.005212674189958,
-            "velocityY": 2.2976792648733673,
-            "timestamp": 0.5912799009608206
-        },
-        {
-            "x": 3.263196121755215,
-            "y": 5.578792551102765,
-            "heading": 2.580155276212398,
-            "angularVelocity": 0.0000014356951704765205,
-            "velocityX": 3.005225405198131,
-            "velocityY": 2.29766261346058,
-            "timestamp": 0.6282348947708719
-        },
-        {
-            "x": 3.374254224320857,
-            "y": 5.6637026374167005,
-            "heading": 2.580155329268507,
-            "angularVelocity": 0.0000014356952512853246,
-            "velocityX": 3.0052258467821833,
-            "velocityY": 2.297662035890833,
-            "timestamp": 0.6651898885809232
-        },
-        {
-            "x": 3.485312339954463,
-            "y": 5.748612706638388,
-            "heading": 2.5801553823246177,
-            "angularVelocity": 0.0000014356952943350777,
-            "velocityX": 3.0052262004005508,
-            "velocityY": 2.2976615733755907,
-            "timestamp": 0.7021448823909745
-        },
-        {
-            "x": 3.5963704664420417,
-            "y": 5.833522761663614,
-            "heading": 2.58015543538073,
-            "angularVelocity": 0.0000014356953270064253,
-            "velocityX": 3.0052264941084177,
-            "velocityY": 2.297661189220169,
-            "timestamp": 0.7390998762010258
-        },
-        {
-            "x": 3.70742860215257,
-            "y": 5.918432804625673,
-            "heading": 2.5801554884368434,
-            "angularVelocity": 0.0000014356953601191765,
-            "velocityX": 3.0052267436809124,
-            "velocityY": 2.2976608627915387,
-            "timestamp": 0.7760548700110771
-        },
-        {
-            "x": 3.8184867458225393,
-            "y": 6.00334283717717,
-            "heading": 2.580155541492958,
-            "angularVelocity": 0.0000014356953984229,
-            "velocityX": 3.005226959062899,
-            "velocityY": 2.2976605810823423,
-            "timestamp": 0.8130098638211284
-        },
-        {
-            "x": 3.9295448964619117,
-            "y": 6.088252860613022,
-            "heading": 2.580155594549074,
-            "angularVelocity": 0.0000014356954384844398,
-            "velocityX": 3.0052271476545696,
-            "velocityY": 2.2976603344135205,
-            "timestamp": 0.8499648576311797
-        },
-        {
-            "x": 4.040603429478696,
-            "y": 6.173162383915615,
-            "heading": 2.5801556476051415,
-            "angularVelocity": 0.0000014356941260173984,
-            "velocityX": 3.005237494765267,
-            "velocityY": 2.2976468008363797,
-            "timestamp": 0.886919851441231
-        },
-        {
-            "x": 4.155351262344264,
-            "y": 6.253015747837148,
-            "heading": 2.580155700374003,
-            "angularVelocity": 0.0000014279223609881846,
-            "velocityX": 3.105069735781099,
-            "velocityY": 2.1608274197522537,
-            "timestamp": 0.9238748452512823
+            "x": 2.3031602212725026,
+            "y": 4.367935645195281,
+            "heading": -0.3933962364434778,
+            "angularVelocity": -0.10578752662991632,
+            "velocityX": 0.0984360180997807,
+            "velocityY": 0.24718310735800264,
+            "timestamp": 0.032104318454850914
+        },
+        {
+            "x": 2.309652031024921,
+            "y": 4.383743043384139,
+            "heading": -0.39998627296935113,
+            "angularVelocity": -0.20526947286363117,
+            "velocityX": 0.20220986038211264,
+            "velocityY": 0.4923760711845366,
+            "timestamp": 0.06420863690970183
+        },
+        {
+            "x": 2.3196739941431104,
+            "y": 4.407342390021744,
+            "heading": -0.40953275493580865,
+            "angularVelocity": -0.29735818811675285,
+            "velocityX": 0.31216869257896596,
+            "velocityY": 0.7350832465356143,
+            "timestamp": 0.09631295536455274
+        },
+        {
+            "x": 2.333458307096076,
+            "y": 4.438632228164309,
+            "heading": -0.4217541925110665,
+            "angularVelocity": -0.3806789292987277,
+            "velocityX": 0.4293600866297902,
+            "velocityY": 0.9746301945817274,
+            "timestamp": 0.12841727381940365
+        },
+        {
+            "x": 2.3512791398330983,
+            "y": 4.477480960055527,
+            "heading": -0.4363123371700568,
+            "angularVelocity": -0.4534637506622021,
+            "velocityX": 0.5550914517025046,
+            "velocityY": 1.21007807550416,
+            "timestamp": 0.16052159227425455
+        },
+        {
+            "x": 2.3734634122157945,
+            "y": 4.523713969255995,
+            "heading": -0.45279460426513807,
+            "angularVelocity": -0.5133971966500652,
+            "velocityX": 0.691005866201282,
+            "velocityY": 1.4400869236793286,
+            "timestamp": 0.19262591072910545
+        },
+        {
+            "x": 2.4004044405613056,
+            "y": 4.577093572392249,
+            "heading": -0.4706891378053212,
+            "angularVelocity": -0.5573871180398601,
+            "velocityX": 0.8391714773013875,
+            "velocityY": 1.6626923013900174,
+            "timestamp": 0.22473022918395635
+        },
+        {
+            "x": 2.432578225951104,
+            "y": 4.63728707308353,
+            "heading": -0.48934877664951465,
+            "angularVelocity": -0.581218968109697,
+            "velocityX": 1.002163787873114,
+            "velocityY": 1.8749347000134435,
+            "timestamp": 0.25683454763880725
+        },
+        {
+            "x": 2.4705256917588616,
+            "y": 4.703846202675345,
+            "heading": -0.5079484621261662,
+            "angularVelocity": -0.5793515131868818,
+            "velocityX": 1.1820050271780043,
+            "velocityY": 2.073214221489191,
+            "timestamp": 0.28893886609365815
+        },
+        {
+            "x": 2.5143889510907504,
+            "y": 4.77663720854218,
+            "heading": -0.5259955708448517,
+            "angularVelocity": -0.5621395995079442,
+            "velocityX": 1.366272870535308,
+            "velocityY": 2.267327554989265,
+            "timestamp": 0.32104318454850905
+        },
+        {
+            "x": 2.564366566619072,
+            "y": 4.855456871813396,
+            "heading": -0.5427715655324792,
+            "angularVelocity": -0.5225463580925951,
+            "velocityX": 1.5567256348583118,
+            "velocityY": 2.4551109341275055,
+            "timestamp": 0.35314750300335995
+        },
+        {
+            "x": 2.620751841763659,
+            "y": 4.939963572730219,
+            "heading": -0.5571329301868887,
+            "angularVelocity": -0.44733435704626356,
+            "velocityX": 1.7563143482981505,
+            "velocityY": 2.6322533847172953,
+            "timestamp": 0.38525182145821085
+        },
+        {
+            "x": 2.6840019979529464,
+            "y": 5.029487269137319,
+            "heading": -0.5670183536644556,
+            "angularVelocity": -0.30791569338152147,
+            "velocityX": 1.9701448039844656,
+            "velocityY": 2.788525055686805,
+            "timestamp": 0.41735613991306175
+        },
+        {
+            "x": 2.7547918742113016,
+            "y": 5.122370588176559,
+            "heading": -0.568384562504894,
+            "angularVelocity": -0.0425552980468726,
+            "velocityX": 2.2049954543625816,
+            "velocityY": 2.893172118569162,
+            "timestamp": 0.44946045836791265
+        },
+        {
+            "x": 2.8341176789452183,
+            "y": 5.214333601098647,
+            "heading": -0.5683844391321649,
+            "angularVelocity": 0.0000038428702119811485,
+            "velocityX": 2.4708764599838617,
+            "velocityY": 2.864506002562167,
+            "timestamp": 0.48156477682276355
+        },
+        {
+            "x": 2.9197482153809866,
+            "y": 5.3004567509455125,
+            "heading": -0.5683843814173083,
+            "angularVelocity": 0.0000017977287583592433,
+            "velocityX": 2.6672591276526463,
+            "velocityY": 2.6826032755681406,
+            "timestamp": 0.5136690952776145
+        },
+        {
+            "x": 3.0112544304014244,
+            "y": 5.380309105627853,
+            "heading": -0.5683843265105415,
+            "angularVelocity": 0.0000017102610943456471,
+            "velocityX": 2.850277452521701,
+            "velocityY": 2.487277678691065,
+            "timestamp": 0.5457734137324655
+        },
+        {
+            "x": 3.107551803887462,
+            "y": 5.454313172250514,
+            "heading": -0.5683842730108181,
+            "angularVelocity": 0.0000016664338651344868,
+            "velocityX": 2.999514648518804,
+            "velocityY": 2.305112526426488,
+            "timestamp": 0.5778777321873164
+        },
+        {
+            "x": 3.2038516202489813,
+            "y": 5.528314059986814,
+            "heading": -0.5683842195116714,
+            "angularVelocity": 0.000001666415897857932,
+            "velocityX": 2.9995907403219917,
+            "velocityY": 2.305013509019602,
+            "timestamp": 0.6099820506421674
+        },
+        {
+            "x": 3.3001514533185667,
+            "y": 5.602314925980335,
+            "heading": -0.5683841660125195,
+            "angularVelocity": 0.000001666416059709123,
+            "velocityX": 2.9995912607524815,
+            "velocityY": 2.305012831765639,
+            "timestamp": 0.6420863690970183
+        },
+        {
+            "x": 3.3964512995777554,
+            "y": 5.676315774809768,
+            "heading": -0.568384112513365,
+            "angularVelocity": 0.0000016664161452516387,
+            "velocityX": 2.999591671588281,
+            "velocityY": 2.30501229713074,
+            "timestamp": 0.6741906875518693
+        },
+        {
+            "x": 3.492751156794414,
+            "y": 5.750316609379858,
+            "heading": -0.5683840590142086,
+            "angularVelocity": 0.000001666416198927138,
+            "velocityX": 2.999592012896559,
+            "velocityY": 2.3050118529742454,
+            "timestamp": 0.7062950060067202
+        },
+        {
+            "x": 3.5890510233460025,
+            "y": 5.824317431802072,
+            "heading": -0.5683840055150509,
+            "angularVelocity": 0.0000016664162437925357,
+            "velocityX": 2.999592303665221,
+            "velocityY": 2.305011474586604,
+            "timestamp": 0.7383993244615712
+        },
+        {
+            "x": 3.6853508979893648,
+            "y": 5.898318243694168,
+            "heading": -0.5683839520158918,
+            "angularVelocity": 0.0000016664162889267752,
+            "velocityX": 2.9995925557115117,
+            "velocityY": 2.3050111465896648,
+            "timestamp": 0.7705036429164221
+        },
+        {
+            "x": 3.7816507797340138,
+            "y": 5.972319046345101,
+            "heading": -0.5683838985167311,
+            "angularVelocity": 0.0000016664163350532929,
+            "velocityX": 2.9995927769056334,
+            "velocityY": 2.305010858741686,
+            "timestamp": 0.8026079613712731
+        },
+        {
+            "x": 3.877950667767949,
+            "y": 6.046319840811553,
+            "heading": -0.5683838450175687,
+            "angularVelocity": 0.0000016664163859759036,
+            "velocityX": 2.999592972807203,
+            "velocityY": 2.305010603807763,
+            "timestamp": 0.8347122798261241
+        },
+        {
+            "x": 3.9742505615842325,
+            "y": 6.120320627753221,
+            "heading": -0.5683837915184047,
+            "angularVelocity": 0.0000016664164376020863,
+            "velocityX": 2.999593152918432,
+            "velocityY": 2.3050103694223405,
+            "timestamp": 0.866816598280975
+        },
+        {
+            "x": 4.070552264919673,
+            "y": 6.194319059842733,
+            "heading": -0.5683837380195041,
+            "angularVelocity": 0.0000016664082331173329,
+            "velocityX": 2.999649516649054,
+            "velocityY": 2.304937019409932,
+            "timestamp": 0.898920916735826
+        },
+        {
+            "x": 4.171102213813095,
+            "y": 6.262432889296395,
+            "heading": -0.5683836849080734,
+            "angularVelocity": 0.0000016543391427080076,
+            "velocityX": 3.131975812999348,
+            "velocityY": 2.1216407241116846,
+            "timestamp": 0.9310252351906769
         },
         {
             "x": 4.276216983795166,
             "y": 6.32326602935791,
-            "heading": 2.5801557537673054,
-            "angularVelocity": 0.0000014448196748631945,
-            "velocityX": 3.2706194478654775,
-            "velocityY": 1.9009685641363763,
-            "timestamp": 0.9608298390613336
-        },
-        {
-            "x": 4.413687737524023,
-            "y": 6.387653258704535,
-            "heading": 2.580155801751457,
-            "angularVelocity": 0.0000011957738462706154,
-            "velocityX": 3.425796355950964,
-            "velocityY": 1.604541545618551,
-            "timestamp": 1.0009579551094623
-        },
-        {
-            "x": 4.556309721411339,
-            "y": 6.439642616989784,
-            "heading": 2.580155847910912,
-            "angularVelocity": 0.0000011503020593677978,
-            "velocityX": 3.55416595476994,
-            "velocityY": 1.2955843285264883,
-            "timestamp": 1.041086071157591
-        },
-        {
-            "x": 4.700491299069936,
-            "y": 6.48713573606665,
-            "heading": 2.580155893776208,
-            "angularVelocity": 0.000001142971587913107,
-            "velocityX": 3.593031317136076,
-            "velocityY": 1.1835372241224518,
-            "timestamp": 1.08121418720572
-        },
-        {
-            "x": 4.844672963256385,
-            "y": 6.5346285924578735,
-            "heading": 2.5801559396414997,
-            "angularVelocity": 0.0000011429714605909094,
-            "velocityX": 3.5930334734259763,
-            "velocityY": 1.1835306779481511,
-            "timestamp": 1.1213423032538488
-        },
-        {
-            "x": 4.988854630036052,
-            "y": 6.58212144097644,
-            "heading": 2.5801559855067953,
-            "angularVelocity": 0.0000011429715641749761,
-            "velocityX": 3.5930335380494514,
-            "velocityY": 1.18353048176008,
-            "timestamp": 1.1614704193019776
-        },
-        {
-            "x": 5.133036299227264,
-            "y": 6.62961428217388,
-            "heading": 2.580156031372096,
-            "angularVelocity": 0.0000011429716937026712,
-            "velocityX": 3.593033598145609,
-            "velocityY": 1.1835302993162664,
-            "timestamp": 1.2015985353501064
-        },
-        {
-            "x": 5.277217970668299,
-            "y": 6.677107116541164,
-            "heading": 2.580156077237403,
-            "angularVelocity": 0.0000011429718454732932,
-            "velocityX": 3.593033654211569,
-            "velocityY": 1.183530129107538,
-            "timestamp": 1.2417266513982352
-        },
-        {
-            "x": 5.421399644212107,
-            "y": 6.724599944524706,
-            "heading": 2.5801561231027166,
-            "angularVelocity": 0.0000011429720157589239,
-            "velocityX": 3.593033706613086,
-            "velocityY": 1.183529970023511,
-            "timestamp": 1.281854767446364
-        },
-        {
-            "x": 5.565581319724973,
-            "y": 6.772092766530448,
-            "heading": 2.580156168968038,
-            "angularVelocity": 0.0000011429722040135848,
-            "velocityX": 3.593033755682377,
-            "velocityY": 1.1835298210556366,
-            "timestamp": 1.3219828834944929
-        },
-        {
-            "x": 5.709762997085268,
-            "y": 6.819585582927637,
-            "heading": 2.5801562148333677,
-            "angularVelocity": 0.0000011429724102352075,
-            "velocityX": 3.5930338017206545,
-            "velocityY": 1.1835296812894733,
-            "timestamp": 1.3621109995426217
-        },
-        {
-            "x": 5.853944676177738,
-            "y": 6.867078394066171,
-            "heading": 2.580156260698706,
-            "angularVelocity": 0.0000011429726261958928,
-            "velocityX": 3.5930338448867536,
-            "velocityY": 1.183529550242829,
-            "timestamp": 1.4022391155907505
-        },
-        {
-            "x": 5.998126356881069,
-            "y": 6.91457120031434,
-            "heading": 2.5801563065640525,
-            "angularVelocity": 0.0000011429728355759127,
-            "velocityX": 3.593033885029711,
-            "velocityY": 1.183529428374024,
-            "timestamp": 1.4423672316388794
-        },
-        {
-            "x": 6.142308039084736,
-            "y": 6.962064002007683,
-            "heading": 2.580156352429407,
-            "angularVelocity": 0.0000011429730341793388,
-            "velocityX": 3.5930339224183525,
-            "velocityY": 1.1835293148669426,
-            "timestamp": 1.4824953476870082
-        },
-        {
-            "x": 6.286489722687973,
-            "y": 7.009556799452112,
-            "heading": 2.5801563982947693,
-            "angularVelocity": 0.0000011429732271830896,
-            "velocityX": 3.5930339572958836,
-            "velocityY": 1.1835292089832443,
-            "timestamp": 1.522623463735137
-        },
-        {
-            "x": 6.430671407598782,
-            "y": 7.057049592926921,
-            "heading": 2.580156444160139,
-            "angularVelocity": 0.000001142973413162616,
-            "velocityX": 3.5930339898808183,
-            "velocityY": 1.183529110059557,
-            "timestamp": 1.5627515797832658
-        },
-        {
-            "x": 6.574853093733082,
-            "y": 7.104542382687364,
-            "heading": 2.580156490025516,
-            "angularVelocity": 0.0000011429735908015805,
-            "velocityX": 3.593034020370455,
-            "velocityY": 1.1835290174969095,
-            "timestamp": 1.6028796958313947
-        },
-        {
-            "x": 6.719034781004445,
-            "y": 7.15203516899583,
-            "heading": 2.5801565358908998,
-            "angularVelocity": 0.0000011429737623342271,
-            "velocityX": 3.593034048706264,
-            "velocityY": 1.1835289314729884,
-            "timestamp": 1.6430078118795235
-        },
-        {
-            "x": 6.863216267597302,
-            "y": 7.199528564522877,
-            "heading": 2.580156581764359,
-            "angularVelocity": 0.0000011431750045992354,
-            "velocityX": 3.593029047761165,
-            "velocityY": 1.1835441133115774,
-            "timestamp": 1.6831359279276523
-        },
-        {
-            "x": 6.997116821056572,
-            "y": 7.244893026525659,
-            "heading": 2.605743326369695,
-            "angularVelocity": 0.6376263608948962,
-            "velocityX": 3.3368263114738097,
-            "velocityY": 1.1304907000461517,
-            "timestamp": 1.7232640439757811
-        },
-        {
-            "x": 7.1211832753718545,
-            "y": 7.2851403173258245,
-            "heading": 2.65804915297094,
-            "angularVelocity": 1.3034707769113922,
-            "velocityX": 3.091758760029488,
-            "velocityY": 1.0029698566435126,
-            "timestamp": 1.76339216002391
-        },
-        {
-            "x": 7.233669414925018,
-            "y": 7.321896904745842,
-            "heading": 2.730695495833792,
-            "angularVelocity": 1.8103601668147513,
-            "velocityX": 2.8031751956221984,
-            "velocityY": 0.9159808892082741,
-            "timestamp": 1.8035202760720388
-        },
-        {
-            "x": 7.333463334152891,
-            "y": 7.354467297173912,
-            "heading": 2.807930546248393,
-            "angularVelocity": 1.9247115992678774,
-            "velocityX": 2.486882740973504,
-            "velocityY": 0.811660143451645,
-            "timestamp": 1.8436483921201676
-        },
-        {
-            "x": 7.42052379329864,
-            "y": 7.382839226266775,
-            "heading": 2.882629127741458,
-            "angularVelocity": 1.8615023292763984,
-            "velocityX": 2.169562584032867,
-            "velocityY": 0.7070336683345496,
-            "timestamp": 1.8837765081682964
-        },
-        {
-            "x": 7.494937832171454,
-            "y": 7.407046736601957,
-            "heading": 2.951143873381968,
-            "angularVelocity": 1.7074000074744335,
-            "velocityX": 1.8544114750755487,
-            "velocityY": 0.6032555903234524,
-            "timestamp": 1.9239046242164253
-        },
-        {
-            "x": 7.556793519582923,
-            "y": 7.427131383067914,
-            "heading": 3.011211531798935,
-            "angularVelocity": 1.4968970470710155,
-            "velocityX": 1.5414550570298573,
-            "velocityY": 0.5005130677420416,
-            "timestamp": 1.964032740264554
-        },
-        {
-            "x": 7.606167226555021,
-            "y": 7.443134327538265,
-            "heading": 3.0612259719014916,
-            "angularVelocity": 1.2463690057756673,
-            "velocityX": 1.2304018188364594,
-            "velocityY": 0.3987963065885285,
-            "timestamp": 2.0041608563126827
-        },
-        {
-            "x": 7.643124291158215,
-            "y": 7.455092771789357,
-            "heading": 3.0999636114018587,
-            "angularVelocity": 0.9653490698119509,
-            "velocityX": 0.9209768173235189,
-            "velocityY": 0.2980066205139035,
-            "timestamp": 2.0442889723608113
-        },
-        {
-            "x": 7.667720023098554,
-            "y": 7.46303884043931,
-            "heading": 3.1264689002845456,
-            "angularVelocity": 0.6605166524861815,
-            "velocityX": 0.6129301438133489,
-            "velocityY": 0.1980174858052883,
-            "timestamp": 2.08441708840894
+            "heading": -0.5683836309724583,
+            "angularVelocity": 0.0000016800112198234746,
+            "velocityX": 3.274162948822482,
+            "velocityY": 1.8948584797732355,
+            "timestamp": 0.9631295536455279
+        },
+        {
+            "x": 4.413834682670106,
+            "y": 6.38740625436239,
+            "heading": -0.5683835821873748,
+            "angularVelocity": 0.0000012155044166425132,
+            "velocityX": 3.428812837046335,
+            "velocityY": 1.5980853383274327,
+            "timestamp": 1.0032652231659112
+        },
+        {
+            "x": 4.5565820466376845,
+            "y": 6.439134291384663,
+            "heading": -0.5683835352510233,
+            "angularVelocity": 0.0000011694423477450916,
+            "velocityX": 3.556620972650851,
+            "velocityY": 1.2888295533727718,
+            "timestamp": 1.0434008926862943
+        },
+        {
+            "x": 4.7007810920363475,
+            "y": 6.4866657027970485,
+            "heading": -0.5683834885999294,
+            "angularVelocity": 0.000001162335011018839,
+            "velocityX": 3.5927903314389917,
+            "velocityY": 1.184268556632563,
+            "timestamp": 1.0835365622066775
+        },
+        {
+            "x": 4.84498021653845,
+            "y": 6.534196874227661,
+            "heading": -0.5683834419488406,
+            "angularVelocity": 0.0000011623348856187121,
+            "velocityX": 3.5927923023401758,
+            "velocityY": 1.1842625773683353,
+            "timestamp": 1.1236722317270607
+        },
+        {
+            "x": 4.989179343628637,
+            "y": 6.581728037806586,
+            "heading": -0.5683833952977476,
+            "angularVelocity": 0.0000011623349868626642,
+            "velocityX": 3.5927923668235673,
+            "velocityY": 1.1842623817396558,
+            "timestamp": 1.1638079012474438
+        },
+        {
+            "x": 5.133378473125504,
+            "y": 6.629259194084157,
+            "heading": -0.5683833486466496,
+            "angularVelocity": 0.0000011623351115999292,
+            "velocityX": 3.592792426787221,
+            "velocityY": 1.1842621998228242,
+            "timestamp": 1.203943570767827
+        },
+        {
+            "x": 5.277577604867278,
+            "y": 6.676790343551162,
+            "heading": -0.5683833019955459,
+            "angularVelocity": 0.0000011623352597981392,
+            "velocityX": 3.5927924827201663,
+            "velocityY": 1.184262030134241,
+            "timestamp": 1.2440792402882102
+        },
+        {
+            "x": 5.421776738706852,
+            "y": 6.7243214866538885,
+            "heading": -0.5683832553444353,
+            "angularVelocity": 0.0000011623354265532326,
+            "velocityX": 3.5927925349879053,
+            "velocityY": 1.1842618715650601,
+            "timestamp": 1.2842149098085933
+        },
+        {
+            "x": 5.565975874510482,
+            "y": 6.771852623798087,
+            "heading": -0.5683832086933173,
+            "angularVelocity": 0.0000011623356113966992,
+            "velocityX": 3.5927925839233144,
+            "velocityY": 1.18426172310541,
+            "timestamp": 1.3243505793289765
+        },
+        {
+            "x": 5.710175012156514,
+            "y": 6.819383755352828,
+            "heading": -0.5683831620421912,
+            "angularVelocity": 0.0000011623358144790056,
+            "velocityX": 3.5927926298276605,
+            "velocityY": 1.1842615838413222,
+            "timestamp": 1.3644862488493597
+        },
+        {
+            "x": 5.854374151534237,
+            "y": 6.866914881653982,
+            "heading": -0.5683831153910563,
+            "angularVelocity": 0.000001162336033125057,
+            "velocityX": 3.592792672973599,
+            "velocityY": 1.1842614529456208,
+            "timestamp": 1.4046219183697428
+        },
+        {
+            "x": 5.998573292523273,
+            "y": 6.9144460030667485,
+            "heading": -0.568383068739913,
+            "angularVelocity": 0.000001162336242705986,
+            "velocityX": 3.59279271312027,
+            "velocityY": 1.184261331149038,
+            "timestamp": 1.444757587890126
+        },
+        {
+            "x": 6.142772435013044,
+            "y": 6.9619771199265985,
+            "heading": -0.5683830220887615,
+            "angularVelocity": 0.0000011623364467362213,
+            "velocityX": 3.5927927505118338,
+            "velocityY": 1.1842612177108516,
+            "timestamp": 1.4848932574105091
+        },
+        {
+            "x": 6.286971578902684,
+            "y": 7.00950823253954,
+            "heading": -0.5683829754376022,
+            "angularVelocity": 0.0000011623366413601916,
+            "velocityX": 3.59279278539024,
+            "velocityY": 1.1842611118970372,
+            "timestamp": 1.5250289269308923
+        },
+        {
+            "x": 6.431170724100105,
+            "y": 7.057039341184945,
+            "heading": -0.5683829287864354,
+            "angularVelocity": 0.0000011623368281432719,
+            "velocityX": 3.592792817974252,
+            "velocityY": 1.1842610130439195,
+            "timestamp": 1.5651645964512755
+        },
+        {
+            "x": 6.575369870521147,
+            "y": 7.10457044611814,
+            "heading": -0.5683828821352613,
+            "angularVelocity": 0.0000011623370098744323,
+            "velocityX": 3.5927928484613587,
+            "velocityY": 1.1842609205523604,
+            "timestamp": 1.6053002659716586
+        },
+        {
+            "x": 6.719569018075523,
+            "y": 7.152101547613029,
+            "heading": -0.5683828354840802,
+            "angularVelocity": 0.0000011623371832362076,
+            "velocityX": 3.5927928766989634,
+            "velocityY": 1.1842608348853054,
+            "timestamp": 1.6454359354920418
+        },
+        {
+            "x": 6.8637678827817385,
+            "y": 7.1996335071853474,
+            "heading": -0.5683827888209949,
+            "angularVelocity": 0.0000011626337817909648,
+            "velocityX": 3.592785829397527,
+            "velocityY": 1.1842822143076228,
+            "timestamp": 1.685571605012425
+        },
+        {
+            "x": 6.99756534811195,
+            "y": 7.244965845733408,
+            "heading": -0.5424482126349314,
+            "angularVelocity": 0.6461727559544621,
+            "velocityX": 3.3336298342366253,
+            "velocityY": 1.1294775716906411,
+            "timestamp": 1.7257072745328081
+        },
+        {
+            "x": 7.121516587135396,
+            "y": 7.285176196921745,
+            "heading": -0.489772109644447,
+            "angularVelocity": 1.3124510845329824,
+            "velocityX": 3.088306249893159,
+            "velocityY": 1.0018607305881775,
+            "timestamp": 1.7658429440531913
+        },
+        {
+            "x": 7.233956124618328,
+            "y": 7.321947450112657,
+            "heading": -0.4161805837381191,
+            "angularVelocity": 1.8335691614401448,
+            "velocityX": 2.8014865287304875,
+            "velocityY": 0.9161739079059802,
+            "timestamp": 1.8059786135735745
+        },
+        {
+            "x": 7.333707629732149,
+            "y": 7.35452494917019,
+            "heading": -0.3377597360881954,
+            "angularVelocity": 1.953894094381483,
+            "velocityX": 2.4853579448365832,
+            "velocityY": 0.8116844554190893,
+            "timestamp": 1.8461142830939576
+        },
+        {
+            "x": 7.420722566948463,
+            "y": 7.382895867936689,
+            "heading": -0.26184604843223563,
+            "angularVelocity": 1.8914269666638126,
+            "velocityX": 2.1680200743162716,
+            "velocityY": 0.7068754328887092,
+            "timestamp": 1.8862499526143408
+        },
+        {
+            "x": 7.495090208476241,
+            "y": 7.40709623575076,
+            "heading": -0.19218016370533045,
+            "angularVelocity": 1.7357598754276362,
+            "velocityX": 1.8529064649092093,
+            "velocityY": 0.6029640990984653,
+            "timestamp": 1.926385622134724
+        },
+        {
+            "x": 7.556901497509452,
+            "y": 7.427169942598205,
+            "heading": -0.13107853540878284,
+            "angularVelocity": 1.522377202790068,
+            "velocityX": 1.5400587500308252,
+            "velocityY": 0.500146305949861,
+            "timestamp": 1.9665212916551071
+        },
+        {
+            "x": 7.606235479245265,
+            "y": 7.443160506733181,
+            "heading": -0.08018624320935208,
+            "angularVelocity": 1.2680065589434022,
+            "velocityX": 1.2291804852229526,
+            "velocityY": 0.3984127915657456,
+            "timestamp": 2.0066569611754903
+        },
+        {
+            "x": 7.643159935464096,
+            "y": 7.455107253348189,
+            "heading": -0.040758426269481916,
+            "angularVelocity": 0.982363503861482,
+            "velocityX": 0.9199910369024534,
+            "velocityY": 0.29765908374694494,
+            "timestamp": 2.0467926306958737
+        },
+        {
+            "x": 7.667732329164437,
+            "y": 7.4630440972663505,
+            "heading": -0.013776014916518935,
+            "angularVelocity": 0.6722800859036306,
+            "velocityX": 0.6122333075286244,
+            "velocityY": 0.1977503804721968,
+            "timestamp": 2.086928300216257
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 0.3371974826634153,
-            "velocityX": 0.30601927303833204,
-            "velocityY": 0.0987128216021636,
-            "timestamp": 2.1245452044570685
+            "heading": 2.662292212835229e-35,
+            "angularVelocity": 0.3432362056280802,
+            "velocityX": 0.30565506897381545,
+            "velocityY": 0.09856326756028964,
+            "timestamp": 2.1270639697366405
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": -8.758785880018889e-35,
-            "velocityX": 1.2314161274227302e-35,
-            "velocityY": -4.94083755736297e-36,
-            "timestamp": 2.164673320505197
+            "heading": -1.5591265591207004e-41,
+            "angularVelocity": 3.563315968840566e-34,
+            "velocityX": -7.620676928265867e-35,
+            "velocityY": -1.1745137620932586e-33,
+            "timestamp": 2.167199639257024
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.traj
index ff4b41b1..5529ce29 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.traj	
@@ -3,515 +3,551 @@
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
+            "heading": -0.39,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": -5.253317174178206e-37,
+            "velocityY": 1.095053106710567e-32,
             "timestamp": 0
         },
         {
-            "x": 2.3042260943612694,
-            "y": 4.3705069747477046,
-            "heading": 2.7457396895687083,
-            "angularVelocity": -0.11528375442815593,
-            "velocityX": 0.11435786954779384,
-            "velocityY": 0.2843181303644528,
-            "timestamp": 0.03695499381005128
-        },
-        {
-            "x": 2.312934956515594,
-            "y": 4.391423172843284,
-            "heading": 2.737520345774514,
-            "angularVelocity": -0.2224149687709801,
-            "velocityX": 0.23566130734830049,
-            "velocityY": 0.5659911134903584,
-            "timestamp": 0.07390998762010256
-        },
-        {
-            "x": 2.326431561960596,
-            "y": 4.422622192917349,
-            "heading": 2.725704837211404,
-            "angularVelocity": -0.31972697989997184,
-            "velocityX": 0.3652173645157235,
-            "velocityY": 0.8442436828545633,
-            "timestamp": 0.11086498143015384
-        },
-        {
-            "x": 2.3450829408251197,
-            "y": 4.463936524002756,
-            "heading": 2.7107373323748916,
-            "angularVelocity": -0.40501981717128094,
-            "velocityX": 0.5047052357900914,
-            "velocityY": 1.1179634151141484,
-            "timestamp": 0.14781997524020513
-        },
-        {
-            "x": 2.369336863920227,
-            "y": 4.515137741117879,
-            "heading": 2.6931719166575077,
-            "angularVelocity": -0.4753191356943592,
-            "velocityX": 0.6563097593730617,
-            "velocityY": 1.3855019805522522,
-            "timestamp": 0.18477496905025642
-        },
-        {
-            "x": 2.3997470229979276,
-            "y": 4.575903866240714,
-            "heading": 2.6737152145246563,
-            "angularVelocity": -0.5264972369596066,
-            "velocityX": 0.8228971498144095,
-            "velocityY": 1.6443278392948362,
-            "timestamp": 0.22172996286030772
-        },
-        {
-            "x": 2.4370055110126323,
-            "y": 4.645763622209906,
-            "heading": 2.6532916288606927,
-            "angularVelocity": -0.5526610495171655,
-            "velocityX": 1.0082125356646991,
-            "velocityY": 1.890400965246281,
-            "timestamp": 0.258684956670359
-        },
-        {
-            "x": 2.4818450920186503,
-            "y": 4.724119837508184,
-            "heading": 2.6331026993833615,
-            "angularVelocity": -0.5463112666478134,
-            "velocityX": 1.213356474540179,
-            "velocityY": 2.1203146644004023,
-            "timestamp": 0.2956399504804103
-        },
-        {
-            "x": 2.5345067531707874,
-            "y": 4.810746963179403,
-            "heading": 2.6138856788459637,
-            "angularVelocity": -0.5200114668175445,
-            "velocityX": 1.425021511918485,
-            "velocityY": 2.3441250218165925,
-            "timestamp": 0.3325949442904616
-        },
-        {
-            "x": 2.595354080808564,
-            "y": 4.90526717496031,
-            "heading": 2.5968144984302115,
-            "angularVelocity": -0.46194515695220434,
-            "velocityX": 1.6465251746633045,
-            "velocityY": 2.557711476471661,
-            "timestamp": 0.3695499381005129
-        },
-        {
-            "x": 2.6649832778781826,
-            "y": 5.00693656288579,
-            "heading": 2.584029518775602,
-            "angularVelocity": -0.3459608116923114,
-            "velocityX": 1.8841620547283313,
-            "velocityY": 2.7511677703982533,
-            "timestamp": 0.4065049319105642
-        },
-        {
-            "x": 2.7444162884451204,
-            "y": 5.113834606219336,
-            "heading": 2.580154938465408,
-            "angularVelocity": -0.10484591960993231,
-            "velocityX": 2.149452682233515,
-            "velocityY": 2.8926548840192674,
-            "timestamp": 0.4434599257206155
-        },
-        {
-            "x": 2.835170549378822,
-            "y": 5.2201703041601135,
-            "heading": 2.5801550588658806,
-            "angularVelocity": 0.0000032580298363436347,
-            "velocityX": 2.4558050638616957,
-            "velocityY": 2.877437850141254,
-            "timestamp": 0.4804149195306668
-        },
-        {
-            "x": 2.934277899482491,
-            "y": 5.318767249922712,
-            "heading": 2.580155115984828,
-            "angularVelocity": 0.0000015456354283753369,
-            "velocityX": 2.681839174783285,
-            "velocityY": 2.6680276627669444,
-            "timestamp": 0.517369913340718
-        },
-        {
-            "x": 3.041080419735732,
-            "y": 5.408971720433865,
-            "heading": 2.5801551701001046,
-            "angularVelocity": 0.0000014643562563831181,
-            "velocityX": 2.8900700349783843,
-            "velocityY": 2.440927766753384,
-            "timestamp": 0.5543249071507693
-        },
-        {
-            "x": 3.1521380355083095,
-            "y": 5.493882443444743,
-            "heading": 2.5801552231562916,
-            "angularVelocity": 0.0000014356973685406667,
-            "velocityX": 3.005212674189958,
-            "velocityY": 2.2976792648733673,
-            "timestamp": 0.5912799009608206
-        },
-        {
-            "x": 3.263196121755215,
-            "y": 5.578792551102765,
-            "heading": 2.580155276212398,
-            "angularVelocity": 0.0000014356951704765205,
-            "velocityX": 3.005225405198131,
-            "velocityY": 2.29766261346058,
-            "timestamp": 0.6282348947708719
-        },
-        {
-            "x": 3.374254224320857,
-            "y": 5.6637026374167005,
-            "heading": 2.580155329268507,
-            "angularVelocity": 0.0000014356952512853246,
-            "velocityX": 3.0052258467821833,
-            "velocityY": 2.297662035890833,
-            "timestamp": 0.6651898885809232
-        },
-        {
-            "x": 3.485312339954463,
-            "y": 5.748612706638388,
-            "heading": 2.5801553823246177,
-            "angularVelocity": 0.0000014356952943350777,
-            "velocityX": 3.0052262004005508,
-            "velocityY": 2.2976615733755907,
-            "timestamp": 0.7021448823909745
-        },
-        {
-            "x": 3.5963704664420417,
-            "y": 5.833522761663614,
-            "heading": 2.58015543538073,
-            "angularVelocity": 0.0000014356953270064253,
-            "velocityX": 3.0052264941084177,
-            "velocityY": 2.297661189220169,
-            "timestamp": 0.7390998762010258
-        },
-        {
-            "x": 3.70742860215257,
-            "y": 5.918432804625673,
-            "heading": 2.5801554884368434,
-            "angularVelocity": 0.0000014356953601191765,
-            "velocityX": 3.0052267436809124,
-            "velocityY": 2.2976608627915387,
-            "timestamp": 0.7760548700110771
-        },
-        {
-            "x": 3.8184867458225393,
-            "y": 6.00334283717717,
-            "heading": 2.580155541492958,
-            "angularVelocity": 0.0000014356953984229,
-            "velocityX": 3.005226959062899,
-            "velocityY": 2.2976605810823423,
-            "timestamp": 0.8130098638211284
-        },
-        {
-            "x": 3.9295448964619117,
-            "y": 6.088252860613022,
-            "heading": 2.580155594549074,
-            "angularVelocity": 0.0000014356954384844398,
-            "velocityX": 3.0052271476545696,
-            "velocityY": 2.2976603344135205,
-            "timestamp": 0.8499648576311797
-        },
-        {
-            "x": 4.040603429478696,
-            "y": 6.173162383915615,
-            "heading": 2.5801556476051415,
-            "angularVelocity": 0.0000014356941260173984,
-            "velocityX": 3.005237494765267,
-            "velocityY": 2.2976468008363797,
-            "timestamp": 0.886919851441231
-        },
-        {
-            "x": 4.155351262344264,
-            "y": 6.253015747837148,
-            "heading": 2.580155700374003,
-            "angularVelocity": 0.0000014279223609881846,
-            "velocityX": 3.105069735781099,
-            "velocityY": 2.1608274197522537,
-            "timestamp": 0.9238748452512823
+            "x": 2.3031602212725026,
+            "y": 4.367935645195281,
+            "heading": -0.3933962364434778,
+            "angularVelocity": -0.10578752662991632,
+            "velocityX": 0.0984360180997807,
+            "velocityY": 0.24718310735800264,
+            "timestamp": 0.032104318454850914
+        },
+        {
+            "x": 2.309652031024921,
+            "y": 4.383743043384139,
+            "heading": -0.39998627296935113,
+            "angularVelocity": -0.20526947286363117,
+            "velocityX": 0.20220986038211264,
+            "velocityY": 0.4923760711845366,
+            "timestamp": 0.06420863690970183
+        },
+        {
+            "x": 2.3196739941431104,
+            "y": 4.407342390021744,
+            "heading": -0.40953275493580865,
+            "angularVelocity": -0.29735818811675285,
+            "velocityX": 0.31216869257896596,
+            "velocityY": 0.7350832465356143,
+            "timestamp": 0.09631295536455274
+        },
+        {
+            "x": 2.333458307096076,
+            "y": 4.438632228164309,
+            "heading": -0.4217541925110665,
+            "angularVelocity": -0.3806789292987277,
+            "velocityX": 0.4293600866297902,
+            "velocityY": 0.9746301945817274,
+            "timestamp": 0.12841727381940365
+        },
+        {
+            "x": 2.3512791398330983,
+            "y": 4.477480960055527,
+            "heading": -0.4363123371700568,
+            "angularVelocity": -0.4534637506622021,
+            "velocityX": 0.5550914517025046,
+            "velocityY": 1.21007807550416,
+            "timestamp": 0.16052159227425455
+        },
+        {
+            "x": 2.3734634122157945,
+            "y": 4.523713969255995,
+            "heading": -0.45279460426513807,
+            "angularVelocity": -0.5133971966500652,
+            "velocityX": 0.691005866201282,
+            "velocityY": 1.4400869236793286,
+            "timestamp": 0.19262591072910545
+        },
+        {
+            "x": 2.4004044405613056,
+            "y": 4.577093572392249,
+            "heading": -0.4706891378053212,
+            "angularVelocity": -0.5573871180398601,
+            "velocityX": 0.8391714773013875,
+            "velocityY": 1.6626923013900174,
+            "timestamp": 0.22473022918395635
+        },
+        {
+            "x": 2.432578225951104,
+            "y": 4.63728707308353,
+            "heading": -0.48934877664951465,
+            "angularVelocity": -0.581218968109697,
+            "velocityX": 1.002163787873114,
+            "velocityY": 1.8749347000134435,
+            "timestamp": 0.25683454763880725
+        },
+        {
+            "x": 2.4705256917588616,
+            "y": 4.703846202675345,
+            "heading": -0.5079484621261662,
+            "angularVelocity": -0.5793515131868818,
+            "velocityX": 1.1820050271780043,
+            "velocityY": 2.073214221489191,
+            "timestamp": 0.28893886609365815
+        },
+        {
+            "x": 2.5143889510907504,
+            "y": 4.77663720854218,
+            "heading": -0.5259955708448517,
+            "angularVelocity": -0.5621395995079442,
+            "velocityX": 1.366272870535308,
+            "velocityY": 2.267327554989265,
+            "timestamp": 0.32104318454850905
+        },
+        {
+            "x": 2.564366566619072,
+            "y": 4.855456871813396,
+            "heading": -0.5427715655324792,
+            "angularVelocity": -0.5225463580925951,
+            "velocityX": 1.5567256348583118,
+            "velocityY": 2.4551109341275055,
+            "timestamp": 0.35314750300335995
+        },
+        {
+            "x": 2.620751841763659,
+            "y": 4.939963572730219,
+            "heading": -0.5571329301868887,
+            "angularVelocity": -0.44733435704626356,
+            "velocityX": 1.7563143482981505,
+            "velocityY": 2.6322533847172953,
+            "timestamp": 0.38525182145821085
+        },
+        {
+            "x": 2.6840019979529464,
+            "y": 5.029487269137319,
+            "heading": -0.5670183536644556,
+            "angularVelocity": -0.30791569338152147,
+            "velocityX": 1.9701448039844656,
+            "velocityY": 2.788525055686805,
+            "timestamp": 0.41735613991306175
+        },
+        {
+            "x": 2.7547918742113016,
+            "y": 5.122370588176559,
+            "heading": -0.568384562504894,
+            "angularVelocity": -0.0425552980468726,
+            "velocityX": 2.2049954543625816,
+            "velocityY": 2.893172118569162,
+            "timestamp": 0.44946045836791265
+        },
+        {
+            "x": 2.8341176789452183,
+            "y": 5.214333601098647,
+            "heading": -0.5683844391321649,
+            "angularVelocity": 0.0000038428702119811485,
+            "velocityX": 2.4708764599838617,
+            "velocityY": 2.864506002562167,
+            "timestamp": 0.48156477682276355
+        },
+        {
+            "x": 2.9197482153809866,
+            "y": 5.3004567509455125,
+            "heading": -0.5683843814173083,
+            "angularVelocity": 0.0000017977287583592433,
+            "velocityX": 2.6672591276526463,
+            "velocityY": 2.6826032755681406,
+            "timestamp": 0.5136690952776145
+        },
+        {
+            "x": 3.0112544304014244,
+            "y": 5.380309105627853,
+            "heading": -0.5683843265105415,
+            "angularVelocity": 0.0000017102610943456471,
+            "velocityX": 2.850277452521701,
+            "velocityY": 2.487277678691065,
+            "timestamp": 0.5457734137324655
+        },
+        {
+            "x": 3.107551803887462,
+            "y": 5.454313172250514,
+            "heading": -0.5683842730108181,
+            "angularVelocity": 0.0000016664338651344868,
+            "velocityX": 2.999514648518804,
+            "velocityY": 2.305112526426488,
+            "timestamp": 0.5778777321873164
+        },
+        {
+            "x": 3.2038516202489813,
+            "y": 5.528314059986814,
+            "heading": -0.5683842195116714,
+            "angularVelocity": 0.000001666415897857932,
+            "velocityX": 2.9995907403219917,
+            "velocityY": 2.305013509019602,
+            "timestamp": 0.6099820506421674
+        },
+        {
+            "x": 3.3001514533185667,
+            "y": 5.602314925980335,
+            "heading": -0.5683841660125195,
+            "angularVelocity": 0.000001666416059709123,
+            "velocityX": 2.9995912607524815,
+            "velocityY": 2.305012831765639,
+            "timestamp": 0.6420863690970183
+        },
+        {
+            "x": 3.3964512995777554,
+            "y": 5.676315774809768,
+            "heading": -0.568384112513365,
+            "angularVelocity": 0.0000016664161452516387,
+            "velocityX": 2.999591671588281,
+            "velocityY": 2.30501229713074,
+            "timestamp": 0.6741906875518693
+        },
+        {
+            "x": 3.492751156794414,
+            "y": 5.750316609379858,
+            "heading": -0.5683840590142086,
+            "angularVelocity": 0.000001666416198927138,
+            "velocityX": 2.999592012896559,
+            "velocityY": 2.3050118529742454,
+            "timestamp": 0.7062950060067202
+        },
+        {
+            "x": 3.5890510233460025,
+            "y": 5.824317431802072,
+            "heading": -0.5683840055150509,
+            "angularVelocity": 0.0000016664162437925357,
+            "velocityX": 2.999592303665221,
+            "velocityY": 2.305011474586604,
+            "timestamp": 0.7383993244615712
+        },
+        {
+            "x": 3.6853508979893648,
+            "y": 5.898318243694168,
+            "heading": -0.5683839520158918,
+            "angularVelocity": 0.0000016664162889267752,
+            "velocityX": 2.9995925557115117,
+            "velocityY": 2.3050111465896648,
+            "timestamp": 0.7705036429164221
+        },
+        {
+            "x": 3.7816507797340138,
+            "y": 5.972319046345101,
+            "heading": -0.5683838985167311,
+            "angularVelocity": 0.0000016664163350532929,
+            "velocityX": 2.9995927769056334,
+            "velocityY": 2.305010858741686,
+            "timestamp": 0.8026079613712731
+        },
+        {
+            "x": 3.877950667767949,
+            "y": 6.046319840811553,
+            "heading": -0.5683838450175687,
+            "angularVelocity": 0.0000016664163859759036,
+            "velocityX": 2.999592972807203,
+            "velocityY": 2.305010603807763,
+            "timestamp": 0.8347122798261241
+        },
+        {
+            "x": 3.9742505615842325,
+            "y": 6.120320627753221,
+            "heading": -0.5683837915184047,
+            "angularVelocity": 0.0000016664164376020863,
+            "velocityX": 2.999593152918432,
+            "velocityY": 2.3050103694223405,
+            "timestamp": 0.866816598280975
+        },
+        {
+            "x": 4.070552264919673,
+            "y": 6.194319059842733,
+            "heading": -0.5683837380195041,
+            "angularVelocity": 0.0000016664082331173329,
+            "velocityX": 2.999649516649054,
+            "velocityY": 2.304937019409932,
+            "timestamp": 0.898920916735826
+        },
+        {
+            "x": 4.171102213813095,
+            "y": 6.262432889296395,
+            "heading": -0.5683836849080734,
+            "angularVelocity": 0.0000016543391427080076,
+            "velocityX": 3.131975812999348,
+            "velocityY": 2.1216407241116846,
+            "timestamp": 0.9310252351906769
         },
         {
             "x": 4.276216983795166,
             "y": 6.32326602935791,
-            "heading": 2.5801557537673054,
-            "angularVelocity": 0.0000014448196748631945,
-            "velocityX": 3.2706194478654775,
-            "velocityY": 1.9009685641363763,
-            "timestamp": 0.9608298390613336
-        },
-        {
-            "x": 4.413687737524023,
-            "y": 6.387653258704535,
-            "heading": 2.580155801751457,
-            "angularVelocity": 0.0000011957738462706154,
-            "velocityX": 3.425796355950964,
-            "velocityY": 1.604541545618551,
-            "timestamp": 1.0009579551094623
-        },
-        {
-            "x": 4.556309721411339,
-            "y": 6.439642616989784,
-            "heading": 2.580155847910912,
-            "angularVelocity": 0.0000011503020593677978,
-            "velocityX": 3.55416595476994,
-            "velocityY": 1.2955843285264883,
-            "timestamp": 1.041086071157591
-        },
-        {
-            "x": 4.700491299069936,
-            "y": 6.48713573606665,
-            "heading": 2.580155893776208,
-            "angularVelocity": 0.000001142971587913107,
-            "velocityX": 3.593031317136076,
-            "velocityY": 1.1835372241224518,
-            "timestamp": 1.08121418720572
-        },
-        {
-            "x": 4.844672963256385,
-            "y": 6.5346285924578735,
-            "heading": 2.5801559396414997,
-            "angularVelocity": 0.0000011429714605909094,
-            "velocityX": 3.5930334734259763,
-            "velocityY": 1.1835306779481511,
-            "timestamp": 1.1213423032538488
-        },
-        {
-            "x": 4.988854630036052,
-            "y": 6.58212144097644,
-            "heading": 2.5801559855067953,
-            "angularVelocity": 0.0000011429715641749761,
-            "velocityX": 3.5930335380494514,
-            "velocityY": 1.18353048176008,
-            "timestamp": 1.1614704193019776
-        },
-        {
-            "x": 5.133036299227264,
-            "y": 6.62961428217388,
-            "heading": 2.580156031372096,
-            "angularVelocity": 0.0000011429716937026712,
-            "velocityX": 3.593033598145609,
-            "velocityY": 1.1835302993162664,
-            "timestamp": 1.2015985353501064
-        },
-        {
-            "x": 5.277217970668299,
-            "y": 6.677107116541164,
-            "heading": 2.580156077237403,
-            "angularVelocity": 0.0000011429718454732932,
-            "velocityX": 3.593033654211569,
-            "velocityY": 1.183530129107538,
-            "timestamp": 1.2417266513982352
-        },
-        {
-            "x": 5.421399644212107,
-            "y": 6.724599944524706,
-            "heading": 2.5801561231027166,
-            "angularVelocity": 0.0000011429720157589239,
-            "velocityX": 3.593033706613086,
-            "velocityY": 1.183529970023511,
-            "timestamp": 1.281854767446364
-        },
-        {
-            "x": 5.565581319724973,
-            "y": 6.772092766530448,
-            "heading": 2.580156168968038,
-            "angularVelocity": 0.0000011429722040135848,
-            "velocityX": 3.593033755682377,
-            "velocityY": 1.1835298210556366,
-            "timestamp": 1.3219828834944929
-        },
-        {
-            "x": 5.709762997085268,
-            "y": 6.819585582927637,
-            "heading": 2.5801562148333677,
-            "angularVelocity": 0.0000011429724102352075,
-            "velocityX": 3.5930338017206545,
-            "velocityY": 1.1835296812894733,
-            "timestamp": 1.3621109995426217
-        },
-        {
-            "x": 5.853944676177738,
-            "y": 6.867078394066171,
-            "heading": 2.580156260698706,
-            "angularVelocity": 0.0000011429726261958928,
-            "velocityX": 3.5930338448867536,
-            "velocityY": 1.183529550242829,
-            "timestamp": 1.4022391155907505
-        },
-        {
-            "x": 5.998126356881069,
-            "y": 6.91457120031434,
-            "heading": 2.5801563065640525,
-            "angularVelocity": 0.0000011429728355759127,
-            "velocityX": 3.593033885029711,
-            "velocityY": 1.183529428374024,
-            "timestamp": 1.4423672316388794
-        },
-        {
-            "x": 6.142308039084736,
-            "y": 6.962064002007683,
-            "heading": 2.580156352429407,
-            "angularVelocity": 0.0000011429730341793388,
-            "velocityX": 3.5930339224183525,
-            "velocityY": 1.1835293148669426,
-            "timestamp": 1.4824953476870082
-        },
-        {
-            "x": 6.286489722687973,
-            "y": 7.009556799452112,
-            "heading": 2.5801563982947693,
-            "angularVelocity": 0.0000011429732271830896,
-            "velocityX": 3.5930339572958836,
-            "velocityY": 1.1835292089832443,
-            "timestamp": 1.522623463735137
-        },
-        {
-            "x": 6.430671407598782,
-            "y": 7.057049592926921,
-            "heading": 2.580156444160139,
-            "angularVelocity": 0.000001142973413162616,
-            "velocityX": 3.5930339898808183,
-            "velocityY": 1.183529110059557,
-            "timestamp": 1.5627515797832658
-        },
-        {
-            "x": 6.574853093733082,
-            "y": 7.104542382687364,
-            "heading": 2.580156490025516,
-            "angularVelocity": 0.0000011429735908015805,
-            "velocityX": 3.593034020370455,
-            "velocityY": 1.1835290174969095,
-            "timestamp": 1.6028796958313947
-        },
-        {
-            "x": 6.719034781004445,
-            "y": 7.15203516899583,
-            "heading": 2.5801565358908998,
-            "angularVelocity": 0.0000011429737623342271,
-            "velocityX": 3.593034048706264,
-            "velocityY": 1.1835289314729884,
-            "timestamp": 1.6430078118795235
-        },
-        {
-            "x": 6.863216267597302,
-            "y": 7.199528564522877,
-            "heading": 2.580156581764359,
-            "angularVelocity": 0.0000011431750045992354,
-            "velocityX": 3.593029047761165,
-            "velocityY": 1.1835441133115774,
-            "timestamp": 1.6831359279276523
-        },
-        {
-            "x": 6.997116821056572,
-            "y": 7.244893026525659,
-            "heading": 2.605743326369695,
-            "angularVelocity": 0.6376263608948962,
-            "velocityX": 3.3368263114738097,
-            "velocityY": 1.1304907000461517,
-            "timestamp": 1.7232640439757811
-        },
-        {
-            "x": 7.1211832753718545,
-            "y": 7.2851403173258245,
-            "heading": 2.65804915297094,
-            "angularVelocity": 1.3034707769113922,
-            "velocityX": 3.091758760029488,
-            "velocityY": 1.0029698566435126,
-            "timestamp": 1.76339216002391
-        },
-        {
-            "x": 7.233669414925018,
-            "y": 7.321896904745842,
-            "heading": 2.730695495833792,
-            "angularVelocity": 1.8103601668147513,
-            "velocityX": 2.8031751956221984,
-            "velocityY": 0.9159808892082741,
-            "timestamp": 1.8035202760720388
-        },
-        {
-            "x": 7.333463334152891,
-            "y": 7.354467297173912,
-            "heading": 2.807930546248393,
-            "angularVelocity": 1.9247115992678774,
-            "velocityX": 2.486882740973504,
-            "velocityY": 0.811660143451645,
-            "timestamp": 1.8436483921201676
-        },
-        {
-            "x": 7.42052379329864,
-            "y": 7.382839226266775,
-            "heading": 2.882629127741458,
-            "angularVelocity": 1.8615023292763984,
-            "velocityX": 2.169562584032867,
-            "velocityY": 0.7070336683345496,
-            "timestamp": 1.8837765081682964
-        },
-        {
-            "x": 7.494937832171454,
-            "y": 7.407046736601957,
-            "heading": 2.951143873381968,
-            "angularVelocity": 1.7074000074744335,
-            "velocityX": 1.8544114750755487,
-            "velocityY": 0.6032555903234524,
-            "timestamp": 1.9239046242164253
-        },
-        {
-            "x": 7.556793519582923,
-            "y": 7.427131383067914,
-            "heading": 3.011211531798935,
-            "angularVelocity": 1.4968970470710155,
-            "velocityX": 1.5414550570298573,
-            "velocityY": 0.5005130677420416,
-            "timestamp": 1.964032740264554
-        },
-        {
-            "x": 7.606167226555021,
-            "y": 7.443134327538265,
-            "heading": 3.0612259719014916,
-            "angularVelocity": 1.2463690057756673,
-            "velocityX": 1.2304018188364594,
-            "velocityY": 0.3987963065885285,
-            "timestamp": 2.0041608563126827
-        },
-        {
-            "x": 7.643124291158215,
-            "y": 7.455092771789357,
-            "heading": 3.0999636114018587,
-            "angularVelocity": 0.9653490698119509,
-            "velocityX": 0.9209768173235189,
-            "velocityY": 0.2980066205139035,
-            "timestamp": 2.0442889723608113
-        },
-        {
-            "x": 7.667720023098554,
-            "y": 7.46303884043931,
-            "heading": 3.1264689002845456,
-            "angularVelocity": 0.6605166524861815,
-            "velocityX": 0.6129301438133489,
-            "velocityY": 0.1980174858052883,
-            "timestamp": 2.08441708840894
+            "heading": -0.5683836309724583,
+            "angularVelocity": 0.0000016800112198234746,
+            "velocityX": 3.274162948822482,
+            "velocityY": 1.8948584797732355,
+            "timestamp": 0.9631295536455279
+        },
+        {
+            "x": 4.413834682670106,
+            "y": 6.38740625436239,
+            "heading": -0.5683835821873748,
+            "angularVelocity": 0.0000012155044166425132,
+            "velocityX": 3.428812837046335,
+            "velocityY": 1.5980853383274327,
+            "timestamp": 1.0032652231659112
+        },
+        {
+            "x": 4.5565820466376845,
+            "y": 6.439134291384663,
+            "heading": -0.5683835352510233,
+            "angularVelocity": 0.0000011694423477450916,
+            "velocityX": 3.556620972650851,
+            "velocityY": 1.2888295533727718,
+            "timestamp": 1.0434008926862943
+        },
+        {
+            "x": 4.7007810920363475,
+            "y": 6.4866657027970485,
+            "heading": -0.5683834885999294,
+            "angularVelocity": 0.000001162335011018839,
+            "velocityX": 3.5927903314389917,
+            "velocityY": 1.184268556632563,
+            "timestamp": 1.0835365622066775
+        },
+        {
+            "x": 4.84498021653845,
+            "y": 6.534196874227661,
+            "heading": -0.5683834419488406,
+            "angularVelocity": 0.0000011623348856187121,
+            "velocityX": 3.5927923023401758,
+            "velocityY": 1.1842625773683353,
+            "timestamp": 1.1236722317270607
+        },
+        {
+            "x": 4.989179343628637,
+            "y": 6.581728037806586,
+            "heading": -0.5683833952977476,
+            "angularVelocity": 0.0000011623349868626642,
+            "velocityX": 3.5927923668235673,
+            "velocityY": 1.1842623817396558,
+            "timestamp": 1.1638079012474438
+        },
+        {
+            "x": 5.133378473125504,
+            "y": 6.629259194084157,
+            "heading": -0.5683833486466496,
+            "angularVelocity": 0.0000011623351115999292,
+            "velocityX": 3.592792426787221,
+            "velocityY": 1.1842621998228242,
+            "timestamp": 1.203943570767827
+        },
+        {
+            "x": 5.277577604867278,
+            "y": 6.676790343551162,
+            "heading": -0.5683833019955459,
+            "angularVelocity": 0.0000011623352597981392,
+            "velocityX": 3.5927924827201663,
+            "velocityY": 1.184262030134241,
+            "timestamp": 1.2440792402882102
+        },
+        {
+            "x": 5.421776738706852,
+            "y": 6.7243214866538885,
+            "heading": -0.5683832553444353,
+            "angularVelocity": 0.0000011623354265532326,
+            "velocityX": 3.5927925349879053,
+            "velocityY": 1.1842618715650601,
+            "timestamp": 1.2842149098085933
+        },
+        {
+            "x": 5.565975874510482,
+            "y": 6.771852623798087,
+            "heading": -0.5683832086933173,
+            "angularVelocity": 0.0000011623356113966992,
+            "velocityX": 3.5927925839233144,
+            "velocityY": 1.18426172310541,
+            "timestamp": 1.3243505793289765
+        },
+        {
+            "x": 5.710175012156514,
+            "y": 6.819383755352828,
+            "heading": -0.5683831620421912,
+            "angularVelocity": 0.0000011623358144790056,
+            "velocityX": 3.5927926298276605,
+            "velocityY": 1.1842615838413222,
+            "timestamp": 1.3644862488493597
+        },
+        {
+            "x": 5.854374151534237,
+            "y": 6.866914881653982,
+            "heading": -0.5683831153910563,
+            "angularVelocity": 0.000001162336033125057,
+            "velocityX": 3.592792672973599,
+            "velocityY": 1.1842614529456208,
+            "timestamp": 1.4046219183697428
+        },
+        {
+            "x": 5.998573292523273,
+            "y": 6.9144460030667485,
+            "heading": -0.568383068739913,
+            "angularVelocity": 0.000001162336242705986,
+            "velocityX": 3.59279271312027,
+            "velocityY": 1.184261331149038,
+            "timestamp": 1.444757587890126
+        },
+        {
+            "x": 6.142772435013044,
+            "y": 6.9619771199265985,
+            "heading": -0.5683830220887615,
+            "angularVelocity": 0.0000011623364467362213,
+            "velocityX": 3.5927927505118338,
+            "velocityY": 1.1842612177108516,
+            "timestamp": 1.4848932574105091
+        },
+        {
+            "x": 6.286971578902684,
+            "y": 7.00950823253954,
+            "heading": -0.5683829754376022,
+            "angularVelocity": 0.0000011623366413601916,
+            "velocityX": 3.59279278539024,
+            "velocityY": 1.1842611118970372,
+            "timestamp": 1.5250289269308923
+        },
+        {
+            "x": 6.431170724100105,
+            "y": 7.057039341184945,
+            "heading": -0.5683829287864354,
+            "angularVelocity": 0.0000011623368281432719,
+            "velocityX": 3.592792817974252,
+            "velocityY": 1.1842610130439195,
+            "timestamp": 1.5651645964512755
+        },
+        {
+            "x": 6.575369870521147,
+            "y": 7.10457044611814,
+            "heading": -0.5683828821352613,
+            "angularVelocity": 0.0000011623370098744323,
+            "velocityX": 3.5927928484613587,
+            "velocityY": 1.1842609205523604,
+            "timestamp": 1.6053002659716586
+        },
+        {
+            "x": 6.719569018075523,
+            "y": 7.152101547613029,
+            "heading": -0.5683828354840802,
+            "angularVelocity": 0.0000011623371832362076,
+            "velocityX": 3.5927928766989634,
+            "velocityY": 1.1842608348853054,
+            "timestamp": 1.6454359354920418
+        },
+        {
+            "x": 6.8637678827817385,
+            "y": 7.1996335071853474,
+            "heading": -0.5683827888209949,
+            "angularVelocity": 0.0000011626337817909648,
+            "velocityX": 3.592785829397527,
+            "velocityY": 1.1842822143076228,
+            "timestamp": 1.685571605012425
+        },
+        {
+            "x": 6.99756534811195,
+            "y": 7.244965845733408,
+            "heading": -0.5424482126349314,
+            "angularVelocity": 0.6461727559544621,
+            "velocityX": 3.3336298342366253,
+            "velocityY": 1.1294775716906411,
+            "timestamp": 1.7257072745328081
+        },
+        {
+            "x": 7.121516587135396,
+            "y": 7.285176196921745,
+            "heading": -0.489772109644447,
+            "angularVelocity": 1.3124510845329824,
+            "velocityX": 3.088306249893159,
+            "velocityY": 1.0018607305881775,
+            "timestamp": 1.7658429440531913
+        },
+        {
+            "x": 7.233956124618328,
+            "y": 7.321947450112657,
+            "heading": -0.4161805837381191,
+            "angularVelocity": 1.8335691614401448,
+            "velocityX": 2.8014865287304875,
+            "velocityY": 0.9161739079059802,
+            "timestamp": 1.8059786135735745
+        },
+        {
+            "x": 7.333707629732149,
+            "y": 7.35452494917019,
+            "heading": -0.3377597360881954,
+            "angularVelocity": 1.953894094381483,
+            "velocityX": 2.4853579448365832,
+            "velocityY": 0.8116844554190893,
+            "timestamp": 1.8461142830939576
+        },
+        {
+            "x": 7.420722566948463,
+            "y": 7.382895867936689,
+            "heading": -0.26184604843223563,
+            "angularVelocity": 1.8914269666638126,
+            "velocityX": 2.1680200743162716,
+            "velocityY": 0.7068754328887092,
+            "timestamp": 1.8862499526143408
+        },
+        {
+            "x": 7.495090208476241,
+            "y": 7.40709623575076,
+            "heading": -0.19218016370533045,
+            "angularVelocity": 1.7357598754276362,
+            "velocityX": 1.8529064649092093,
+            "velocityY": 0.6029640990984653,
+            "timestamp": 1.926385622134724
+        },
+        {
+            "x": 7.556901497509452,
+            "y": 7.427169942598205,
+            "heading": -0.13107853540878284,
+            "angularVelocity": 1.522377202790068,
+            "velocityX": 1.5400587500308252,
+            "velocityY": 0.500146305949861,
+            "timestamp": 1.9665212916551071
+        },
+        {
+            "x": 7.606235479245265,
+            "y": 7.443160506733181,
+            "heading": -0.08018624320935208,
+            "angularVelocity": 1.2680065589434022,
+            "velocityX": 1.2291804852229526,
+            "velocityY": 0.3984127915657456,
+            "timestamp": 2.0066569611754903
+        },
+        {
+            "x": 7.643159935464096,
+            "y": 7.455107253348189,
+            "heading": -0.040758426269481916,
+            "angularVelocity": 0.982363503861482,
+            "velocityX": 0.9199910369024534,
+            "velocityY": 0.29765908374694494,
+            "timestamp": 2.0467926306958737
+        },
+        {
+            "x": 7.667732329164437,
+            "y": 7.4630440972663505,
+            "heading": -0.013776014916518935,
+            "angularVelocity": 0.6722800859036306,
+            "velocityX": 0.6122333075286244,
+            "velocityY": 0.1977503804721968,
+            "timestamp": 2.086928300216257
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 0.3371974826634153,
-            "velocityX": 0.30601927303833204,
-            "velocityY": 0.0987128216021636,
-            "timestamp": 2.1245452044570685
+            "heading": 2.662292212835229e-35,
+            "angularVelocity": 0.3432362056280802,
+            "velocityX": 0.30565506897381545,
+            "velocityY": 0.09856326756028964,
+            "timestamp": 2.1270639697366405
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": -8.758785880018889e-35,
-            "velocityX": 1.2314161274227302e-35,
-            "velocityY": -4.94083755736297e-36,
-            "timestamp": 2.164673320505197
+            "heading": -1.5591265591207004e-41,
+            "angularVelocity": 3.563315968840566e-34,
+            "velocityX": -7.620676928265867e-35,
+            "velocityY": -1.1745137620932586e-33,
+            "timestamp": 2.167199639257024
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.1.traj
index 66059810..0646af70 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.1.traj	
@@ -3,515 +3,551 @@
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
+            "heading": -0.39,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": 3.853182471863691e-34,
+            "velocityY": -3.138879459525789e-33,
             "timestamp": 0
         },
         {
-            "x": 2.30610760298278,
-            "y": 4.37532703674674,
-            "heading": 2.7433864075571766,
-            "angularVelocity": -0.14821621401149343,
-            "velocityX": 0.13687656120621225,
-            "velocityY": 0.3434918886001163,
-            "timestamp": 0.04462124799861899
-        },
-        {
-            "x": 2.318848696543319,
-            "y": 4.405778929719923,
-            "heading": 2.7307174870785778,
-            "angularVelocity": -0.28392124933375007,
-            "velocityX": 0.28553870929233327,
-            "velocityY": 0.6824527403206025,
-            "timestamp": 0.08924249599723798
-        },
-        {
-            "x": 2.3388726580644836,
-            "y": 4.451074836672454,
-            "heading": 2.7126983162955036,
-            "angularVelocity": -0.4038248948938342,
-            "velocityX": 0.44875395510641974,
-            "velocityY": 1.0151196791701815,
-            "timestamp": 0.13386374399585696
-        },
-        {
-            "x": 2.3669952727104597,
-            "y": 4.510809884172449,
-            "heading": 2.6902411170995073,
-            "angularVelocity": -0.5032848744322687,
-            "velocityX": 0.6302516381174824,
-            "velocityY": 1.3387130611372482,
-            "timestamp": 0.17848499199447596
-        },
-        {
-            "x": 2.404258116570724,
-            "y": 4.5843745316547775,
-            "heading": 2.664559931990299,
-            "angularVelocity": -0.5755371322201343,
-            "velocityX": 0.8350919243992831,
-            "velocityY": 1.648646122237679,
-            "timestamp": 0.22310623999309495
-        },
-        {
-            "x": 2.452003610186855,
-            "y": 4.670805411588522,
-            "heading": 2.637322930497274,
-            "angularVelocity": -0.6104042964882519,
-            "velocityX": 1.070016993195909,
-            "velocityY": 1.9369893001742093,
-            "timestamp": 0.2677274879917139
-        },
-        {
-            "x": 2.5115645242184845,
-            "y": 4.768897077167795,
-            "heading": 2.6107796946069493,
-            "angularVelocity": -0.5948564211190585,
-            "velocityX": 1.3348105824712122,
-            "velocityY": 2.1983173931466227,
-            "timestamp": 0.3123487359903329
-        },
-        {
-            "x": 2.5833793756558516,
-            "y": 4.878178047966119,
-            "heading": 2.586043040896354,
-            "angularVelocity": -0.5543693827514722,
-            "velocityX": 1.60943170929665,
-            "velocityY": 2.449079210014641,
-            "timestamp": 0.3569699839889519
-        },
-        {
-            "x": 2.6682042395216055,
-            "y": 4.997714430694696,
-            "heading": 2.5651891379462763,
-            "angularVelocity": -0.46735364619839126,
-            "velocityX": 1.9009971184216836,
-            "velocityY": 2.678911686474495,
-            "timestamp": 0.4015912319875709
-        },
-        {
-            "x": 2.76752425780781,
-            "y": 5.124957228979585,
-            "heading": 2.5531720535122697,
-            "angularVelocity": -0.269313050911911,
-            "velocityX": 2.2258458187740118,
-            "velocityY": 2.8516189930148728,
-            "timestamp": 0.4462124799861899
-        },
-        {
-            "x": 2.882656696791853,
-            "y": 5.248398665617137,
-            "heading": 2.553172127817037,
-            "angularVelocity": 0.0000016652328226055909,
-            "velocityX": 2.580215573253489,
-            "velocityY": 2.76642725549436,
-            "timestamp": 0.4908337279848089
-        },
-        {
-            "x": 3.0093581189008844,
-            "y": 5.359933509101588,
-            "heading": 2.5531721828704046,
-            "angularVelocity": 0.000001233792641064896,
-            "velocityX": 2.8394862939053795,
-            "velocityY": 2.4995904078680997,
-            "timestamp": 0.5354549759834278
-        },
-        {
-            "x": 3.146402398989035,
-            "y": 5.458483685796272,
-            "heading": 2.553172234700022,
-            "angularVelocity": 0.0000011615456718217746,
-            "velocityX": 3.071278510461917,
-            "velocityY": 2.20859301599395,
-            "timestamp": 0.5800762239820468
-        },
-        {
-            "x": 3.286524028997536,
-            "y": 5.552607013873938,
-            "heading": 2.5531722858926313,
-            "angularVelocity": 0.0000011472697806901915,
-            "velocityX": 3.140244531323671,
-            "velocityY": 2.1093835851605776,
-            "timestamp": 0.6246974719806658
-        },
-        {
-            "x": 3.426645803515369,
-            "y": 5.646730126819845,
-            "heading": 2.5531723370852246,
-            "angularVelocity": 0.000001147269414679425,
-            "velocityX": 3.140247769900325,
-            "velocityY": 2.109378763875461,
-            "timestamp": 0.6693187199792848
-        },
-        {
-            "x": 3.5667675932030476,
-            "y": 5.740853217182279,
-            "heading": 2.5531723882778183,
-            "angularVelocity": 0.0000011472694285852317,
-            "velocityX": 3.1402481098694217,
-            "velocityY": 2.109378257760682,
-            "timestamp": 0.7139399679779038
-        },
-        {
-            "x": 3.70688939501189,
-            "y": 5.834976289499829,
-            "heading": 2.553172439470412,
-            "angularVelocity": 0.0000011472694368681445,
-            "velocityX": 3.1402483815149935,
-            "velocityY": 2.109377853359542,
-            "timestamp": 0.7585612159765228
-        },
-        {
-            "x": 3.8470112068261555,
-            "y": 5.929099346922214,
-            "heading": 2.553172490663007,
-            "angularVelocity": 0.0000011472694500175128,
-            "velocityX": 3.140248605744995,
-            "velocityY": 2.1093775195462685,
-            "timestamp": 0.8031824639751418
-        },
-        {
-            "x": 3.9871330270690666,
-            "y": 6.023222391796794,
-            "heading": 2.5531725418556026,
-            "angularVelocity": 0.000001147269470343035,
-            "velocityX": 3.1402487946380906,
-            "velocityY": 2.109377238339292,
-            "timestamp": 0.8478037119737608
-        },
-        {
-            "x": 4.127254854511192,
-            "y": 6.117345425953829,
-            "heading": 2.553172593048199,
-            "angularVelocity": 0.000001147269495912955,
-            "velocityX": 3.1402489559785947,
-            "velocityY": 2.109376998150064,
-            "timestamp": 0.8924249599723798
-        },
-        {
-            "x": 4.267376688165873,
-            "y": 6.21146845086217,
-            "heading": 2.5531726442407967,
-            "angularVelocity": 0.0000011472695239009516,
-            "velocityX": 3.1402490952072233,
-            "velocityY": 2.1093767908789607,
-            "timestamp": 0.9370462079709988
-        },
-        {
-            "x": 4.407498527228544,
-            "y": 6.305591467719579,
-            "heading": 2.553172695433396,
-            "angularVelocity": 0.0000011472695542073644,
-            "velocityX": 3.140249216404848,
-            "velocityY": 2.1093766104507625,
-            "timestamp": 0.9816674559696178
-        },
-        {
-            "x": 4.547620436764314,
-            "y": 6.399714379662917,
-            "heading": 2.5531727466259886,
-            "angularVelocity": 0.0000011472694062933976,
-            "velocityX": 3.1402507957667765,
-            "velocityY": 2.1093742592374776,
-            "timestamp": 1.0262887039682367
-        },
-        {
-            "x": 4.689466118908105,
-            "y": 6.4912188851781,
-            "heading": 2.553172797709037,
-            "angularVelocity": 0.0000011448144250292167,
-            "velocityX": 3.1788820014218735,
-            "velocityY": 2.050693551153348,
-            "timestamp": 1.0709099519668557
-        },
-        {
-            "x": 4.839613537732198,
-            "y": 6.568348804669705,
-            "heading": 2.5531728491363963,
-            "angularVelocity": 0.0000011525307241082326,
-            "velocityX": 3.364930959096914,
-            "velocityY": 1.7285468907994308,
-            "timestamp": 1.1155311999654747
-        },
-        {
-            "x": 4.996611329828,
-            "y": 6.630356582585623,
-            "heading": 2.5531729033694757,
-            "angularVelocity": 0.0000012154092882901391,
-            "velocityX": 3.5184536322395523,
-            "velocityY": 1.3896468767040242,
-            "timestamp": 1.1601524479640937
-        },
-        {
-            "x": 5.158940949366324,
-            "y": 6.676642268737186,
-            "heading": 2.553172963573569,
-            "angularVelocity": 0.0000013492247785707482,
-            "velocityX": 3.637944405843334,
-            "velocityY": 1.037301470209733,
-            "timestamp": 1.2047736959627127
-        },
-        {
-            "x": 5.325032205172685,
-            "y": 6.706758095236814,
-            "heading": 2.5531730345823234,
-            "angularVelocity": 0.0000015913663950616823,
-            "velocityX": 3.7222458639324056,
-            "velocityY": 0.6749212057125495,
-            "timestamp": 1.2493949439613317
+            "x": 2.304641315068496,
+            "y": 4.371901353899003,
+            "heading": -0.39554860460981267,
+            "angularVelocity": -0.1412451028521288,
+            "velocityX": 0.11814916908296796,
+            "velocityY": 0.3029604871416256,
+            "timestamp": 0.039283518492098594
+        },
+        {
+            "x": 2.314310378834228,
+            "y": 4.395560050627161,
+            "heading": -0.4062436571914542,
+            "angularVelocity": -0.27225291909092403,
+            "velocityX": 0.24613538035494495,
+            "velocityY": 0.6022550330596644,
+            "timestamp": 0.07856703698419719
+        },
+        {
+            "x": 2.3294686593461313,
+            "y": 4.43078385259534,
+            "heading": -0.42159423390969814,
+            "angularVelocity": -0.3907637937605655,
+            "velocityX": 0.385868707635076,
+            "velocityY": 0.8966559850097278,
+            "timestamp": 0.11785055547629578
+        },
+        {
+            "x": 2.3506733572393026,
+            "y": 4.477309554936099,
+            "heading": -0.44099148005781696,
+            "angularVelocity": -0.49377568233887975,
+            "velocityX": 0.5397861165984255,
+            "velocityY": 1.1843568022074944,
+            "timestamp": 0.15713407396839438
+        },
+        {
+            "x": 2.3786040121360466,
+            "y": 4.534766426387325,
+            "heading": -0.4636665715847995,
+            "angularVelocity": -0.5772164102747018,
+            "velocityX": 0.7110018646207296,
+            "velocityY": 1.462620296162777,
+            "timestamp": 0.19641759246049298
+        },
+        {
+            "x": 2.4140943920114943,
+            "y": 4.602616046670193,
+            "heading": -0.48862953085625704,
+            "angularVelocity": -0.6354562989682879,
+            "velocityX": 0.903441983756771,
+            "velocityY": 1.727177780588029,
+            "timestamp": 0.23570111095259158
+        },
+        {
+            "x": 2.458163120855333,
+            "y": 4.68005234098581,
+            "heading": -0.5145799632934547,
+            "angularVelocity": -0.6605933845360225,
+            "velocityX": 1.1218121628463973,
+            "velocityY": 1.9712158505149888,
+            "timestamp": 0.27498462944469015
+        },
+        {
+            "x": 2.5115097740388146,
+            "y": 4.766403162212065,
+            "heading": -0.5400224328341426,
+            "angularVelocity": -0.6476626971635644,
+            "velocityX": 1.3579907103842537,
+            "velocityY": 2.1981437646332993,
+            "timestamp": 0.31426814793678876
+        },
+        {
+            "x": 2.5744195202560527,
+            "y": 4.861351309968927,
+            "heading": -0.563860333505168,
+            "angularVelocity": -0.6068168429413633,
+            "velocityX": 1.6014285031491418,
+            "velocityY": 2.416997035944537,
+            "timestamp": 0.35355166642888736
+        },
+        {
+            "x": 2.6473242968990576,
+            "y": 4.964346697364103,
+            "heading": -0.5842892856317753,
+            "angularVelocity": -0.5200387569839264,
+            "velocityX": 1.8558616804566808,
+            "velocityY": 2.6218473127835553,
+            "timestamp": 0.39283518492098596
+        },
+        {
+            "x": 2.7309171688764238,
+            "y": 5.07424676618957,
+            "heading": -0.5978668700067779,
+            "angularVelocity": -0.3456305569399124,
+            "velocityX": 2.127937496081736,
+            "velocityY": 2.7976126641403014,
+            "timestamp": 0.43211870341308456
+        },
+        {
+            "x": 2.826173576921219,
+            "y": 5.187875652325513,
+            "heading": -0.5974605263008922,
+            "angularVelocity": 0.010343872481393205,
+            "velocityX": 2.4248441000503926,
+            "velocityY": 2.8925333192550147,
+            "timestamp": 0.47140222190518316
+        },
+        {
+            "x": 2.9311441629204515,
+            "y": 5.293067244130248,
+            "heading": -0.5974604636085413,
+            "angularVelocity": 0.000001595894492248037,
+            "velocityX": 2.672127905761066,
+            "velocityY": 2.677753822532693,
+            "timestamp": 0.5106857403972818
+        },
+        {
+            "x": 3.044820699228354,
+            "y": 5.388784226639372,
+            "heading": -0.597460407679312,
+            "angularVelocity": 0.0000014237326879146737,
+            "velocityX": 2.8937463005192323,
+            "velocityY": 2.436568468997834,
+            "timestamp": 0.5499692588893803
+        },
+        {
+            "x": 3.1663502143390825,
+            "y": 5.474310087323753,
+            "heading": -0.5974603544159754,
+            "angularVelocity": 0.0000013558698005366095,
+            "velocityX": 3.0936514796952492,
+            "velocityY": 2.177143595261183,
+            "timestamp": 0.5892527773814789
+        },
+        {
+            "x": 3.289918343405979,
+            "y": 5.556863181758907,
+            "heading": -0.5974603016382944,
+            "angularVelocity": 0.0000013435069733969365,
+            "velocityX": 3.145546371864123,
+            "velocityY": 2.1014689519673895,
+            "timestamp": 0.6285362958735774
+        },
+        {
+            "x": 3.413486607435696,
+            "y": 5.639416074177005,
+            "heading": -0.5974602488606314,
+            "angularVelocity": 0.0000013435065140042136,
+            "velocityX": 3.145549807473418,
+            "velocityY": 2.101463809427605,
+            "timestamp": 0.667819814365676
+        },
+        {
+            "x": 3.5370548851900594,
+            "y": 5.721968946051526,
+            "heading": -0.5974601960829673,
+            "angularVelocity": 0.0000013435065431603766,
+            "velocityX": 3.145550156847558,
+            "velocityY": 2.1014632864709872,
+            "timestamp": 0.7071033328577745
+        },
+        {
+            "x": 3.6606231741521,
+            "y": 5.804521801149959,
+            "heading": -0.5974601433053026,
+            "angularVelocity": 0.0000013435065598734946,
+            "velocityX": 3.1455504421498475,
+            "velocityY": 2.1014628594194082,
+            "timestamp": 0.746386851349873
+        },
+        {
+            "x": 3.7841914725218415,
+            "y": 5.887074642166574,
+            "heading": -0.597460090527637,
+            "angularVelocity": 0.0000013435065802330538,
+            "velocityX": 3.1455506816319834,
+            "velocityY": 2.101462500953099,
+            "timestamp": 0.7856703698419716
+        },
+        {
+            "x": 3.9077597789317933,
+            "y": 5.969627471148282,
+            "heading": -0.5974600377499704,
+            "angularVelocity": 0.0000013435066065618314,
+            "velocityX": 3.14555088630331,
+            "velocityY": 2.1014621945928877,
+            "timestamp": 0.8249538883340701
+        },
+        {
+            "x": 4.031328092300017,
+            "y": 6.052180289714568,
+            "heading": -0.5974599849723026,
+            "angularVelocity": 0.0000013435066364860128,
+            "velocityX": 3.1455510634328734,
+            "velocityY": 2.101461929458217,
+            "timestamp": 0.8642374068261687
+        },
+        {
+            "x": 4.154896411746533,
+            "y": 6.134733099182619,
+            "heading": -0.5974599321946334,
+            "angularVelocity": 0.0000013435066718879358,
+            "velocityX": 3.14555121816168,
+            "velocityY": 2.1014616978538454,
+            "timestamp": 0.9035209253182672
+        },
+        {
+            "x": 4.27846473654201,
+            "y": 6.217285900644126,
+            "heading": -0.5974598794169629,
+            "angularVelocity": 0.000001343506708988157,
+            "velocityX": 3.14555135432466,
+            "velocityY": 2.1014614940395124,
+            "timestamp": 0.9428044438103658
+        },
+        {
+            "x": 4.4020330660739395,
+            "y": 6.299838695015915,
+            "heading": -0.5974598266392908,
+            "angularVelocity": 0.0000013435067467217265,
+            "velocityX": 3.1455514748956417,
+            "velocityY": 2.1014613135638847,
+            "timestamp": 0.9820879623024643
+        },
+        {
+            "x": 4.525601399950297,
+            "y": 6.382391482884783,
+            "heading": -0.5974597738616171,
+            "angularVelocity": 0.0000013435067857429384,
+            "velocityX": 3.1455515854872873,
+            "velocityY": 2.101461148025713,
+            "timestamp": 1.0213714807945629
+        },
+        {
+            "x": 4.6491721527053596,
+            "y": 6.464940649963802,
+            "heading": -0.5974597210842042,
+            "angularVelocity": 0.0000013435001464771913,
+            "velocityX": 3.1456131603868194,
+            "velocityY": 2.101368977314248,
+            "timestamp": 1.0606549992866614
+        },
+        {
+            "x": 4.778837800088318,
+            "y": 6.537541722502267,
+            "heading": -0.5974596685786843,
+            "angularVelocity": 0.0000013365788503576918,
+            "velocityX": 3.300764604603488,
+            "velocityY": 1.8481305984081453,
+            "timestamp": 1.09993851777876
+        },
+        {
+            "x": 4.914297553492409,
+            "y": 6.59865432938236,
+            "heading": -0.5974596146272461,
+            "angularVelocity": 0.0000013733860988547744,
+            "velocityX": 3.4482591835906553,
+            "velocityY": 1.5556805812161723,
+            "timestamp": 1.1392220362708585
+        },
+        {
+            "x": 5.054536029054771,
+            "y": 6.647820017049671,
+            "heading": -0.5974595570647036,
+            "angularVelocity": 0.0000014653102566024163,
+            "velocityX": 3.5699061831898264,
+            "velocityY": 1.2515601848929776,
+            "timestamp": 1.178505554762957
+        },
+        {
+            "x": 5.198501873691458,
+            "y": 6.684670110841039,
+            "heading": -0.5974594930097576,
+            "angularVelocity": 0.0000016305806703071774,
+            "velocityX": 3.6647899720507366,
+            "velocityY": 0.9380548180449542,
+            "timestamp": 1.2177890732550556
+        },
+        {
+            "x": 5.345115761176417,
+            "y": 6.7089283098414425,
+            "heading": -0.5974594180971905,
+            "angularVelocity": 0.0000019069719319939646,
+            "velocityX": 3.7321984667555648,
+            "velocityY": 0.6175159438731035,
+            "timestamp": 1.2570725917471541
         },
         {
             "x": 5.493278503417969,
             "y": 6.720412731170654,
-            "heading": 2.553173120074835,
-            "angularVelocity": 0.000001915959677785522,
-            "velocityX": 3.7705421921522753,
-            "velocityY": 0.30601196842950656,
-            "timestamp": 1.2940161919599507
-        },
-        {
-            "x": 5.617518941656642,
-            "y": 6.721477956029719,
-            "heading": 2.5531732061618477,
-            "angularVelocity": 0.0000026211268995160273,
-            "velocityX": 3.782800031375301,
-            "velocityY": 0.03243334205370069,
-            "timestamp": 1.32685970396381
-        },
-        {
-            "x": 5.741510905915031,
-            "y": 6.713552321996202,
-            "heading": 2.5531732789431283,
-            "angularVelocity": 0.0000022160017651615355,
-            "velocityX": 3.7752346412837734,
-            "velocityY": -0.24131505889453328,
-            "timestamp": 1.3597032159676692
-        },
-        {
-            "x": 5.8646046105180325,
-            "y": 6.696677372566761,
-            "heading": 2.553173343002795,
-            "angularVelocity": 0.0000019504511878675683,
-            "velocityX": 3.7478849578735156,
-            "velocityY": -0.5137985678101988,
-            "timestamp": 1.3925467279715285
-        },
-        {
-            "x": 5.986154981292517,
-            "y": 6.670941556957854,
-            "heading": 2.5531734012532086,
-            "angularVelocity": 0.0000017735744392757528,
-            "velocityX": 3.7008944341945598,
-            "velocityY": -0.7835890268336883,
-            "timestamp": 1.4253902399753877
-        },
-        {
-            "x": 6.105525040034572,
-            "y": 6.636479774106562,
-            "heading": 2.553173455691931,
-            "angularVelocity": 0.000001657518306287931,
-            "velocityX": 3.634509571571689,
-            "velocityY": -1.0492721621013206,
-            "timestamp": 1.458233751979247
-        },
-        {
-            "x": 6.222089256538776,
-            "y": 6.593472690949968,
-            "heading": 2.5531735077894595,
-            "angularVelocity": 0.0000015862350106976908,
-            "velocityX": 3.549078931951851,
-            "velocityY": -1.3094544563790795,
-            "timestamp": 1.4910772639831062
-        },
-        {
-            "x": 6.335236901536747,
-            "y": 6.54214592976541,
-            "heading": 2.553173558707783,
-            "angularVelocity": 0.000001550331263346088,
-            "velocityX": 3.4450531655894774,
-            "velocityY": -1.5627671358204458,
-            "timestamp": 1.5239207759869655
-        },
-        {
-            "x": 6.444377333823394,
-            "y": 6.482772601626653,
-            "heading": 2.5531736094378257,
-            "angularVelocity": 0.0000015445985995778538,
-            "velocityX": 3.3230439020592755,
-            "velocityY": -1.8077642893915407,
-            "timestamp": 1.5567642879908248
-        },
-        {
-            "x": 6.553087073787246,
-            "y": 6.422614322949958,
-            "heading": 2.5531736601729174,
-            "angularVelocity": 0.0000015447523320156895,
-            "velocityX": 3.3099304346952643,
-            "velocityY": -1.8316640032170186,
-            "timestamp": 1.589607799994684
-        },
-        {
-            "x": 6.661796772216766,
-            "y": 6.362455969218214,
-            "heading": 2.553173710908019,
-            "angularVelocity": 0.0000015447526253443646,
-            "velocityX": 3.3099291700822753,
-            "velocityY": -1.8316662884491148,
-            "timestamp": 1.6224513119985433
-        },
-        {
-            "x": 6.770506467400396,
-            "y": 6.302297609620959,
-            "heading": 2.5531737616431265,
-            "angularVelocity": 0.0000015447528238732798,
-            "velocityX": 3.3099290712533134,
-            "velocityY": -1.831666467038753,
-            "timestamp": 1.6552948240024026
-        },
-        {
-            "x": 6.879216090079623,
-            "y": 6.242139119005013,
-            "heading": 2.5531738123794403,
-            "angularVelocity": 0.0000015447895469442916,
-            "velocityX": 3.309926863681755,
-            "velocityY": -1.831670456219097,
-            "timestamp": 1.6881383360062618
-        },
-        {
-            "x": 6.983740227457298,
-            "y": 6.1833650705733145,
-            "heading": 2.56712091506752,
-            "angularVelocity": 0.4246532065888785,
-            "velocityX": 3.18248966083146,
-            "velocityY": -1.7895177721795115,
-            "timestamp": 1.720981848010121
-        },
-        {
-            "x": 7.082197224690116,
-            "y": 6.128185260935312,
-            "heading": 2.6040317014746788,
-            "angularVelocity": 1.1238379867177672,
-            "velocityX": 2.9977609343741736,
-            "velocityY": -1.6800824964004346,
-            "timestamp": 1.7538253600139804
-        },
-        {
-            "x": 7.174469097455313,
-            "y": 6.076712532186898,
-            "heading": 2.662828331449144,
-            "angularVelocity": 1.7902053217559901,
-            "velocityX": 2.809439890422065,
-            "velocityY": -1.567211470635817,
-            "timestamp": 1.7866688720178396
-        },
-        {
-            "x": 7.2591302790362535,
-            "y": 6.029614181571086,
-            "heading": 2.728764792304138,
-            "angularVelocity": 2.0075947069012146,
-            "velocityX": 2.577714026776202,
-            "velocityY": -1.4340229695982882,
-            "timestamp": 1.819512384021699
-        },
-        {
-            "x": 7.335970518862824,
-            "y": 5.986956845665554,
-            "heading": 2.7950149925408634,
-            "angularVelocity": 2.0171472596761757,
-            "velocityX": 2.339586577024463,
-            "velocityY": -1.2988055571072747,
-            "timestamp": 1.8523558960255582
-        },
-        {
-            "x": 7.405004233411928,
-            "y": 5.94870169343351,
-            "heading": 2.858549177078574,
-            "angularVelocity": 1.9344516058527972,
-            "velocityX": 2.1018980717102513,
-            "velocityY": -1.1647704492609365,
-            "timestamp": 1.8851994080294174
-        },
-        {
-            "x": 7.466271161714403,
-            "y": 5.914805217934882,
-            "heading": 2.9175764455342432,
-            "angularVelocity": 1.7972276670270115,
-            "velocityX": 1.8654195171112133,
-            "velocityY": -1.0320600152214254,
-            "timestamp": 1.9180429200332767
-        },
-        {
-            "x": 7.519808154158724,
-            "y": 5.885228450835136,
-            "heading": 2.9709348371498114,
-            "angularVelocity": 1.6246250282034163,
-            "velocityX": 1.6300629615380646,
-            "velocityY": -0.9005360661877405,
-            "timestamp": 1.950886432037136
-        },
-        {
-            "x": 7.565644888760801,
-            "y": 5.8599385495006775,
-            "heading": 3.0178335837303445,
-            "angularVelocity": 1.427945542943834,
-            "velocityX": 1.3956100247954721,
-            "velocityY": -0.770012090408821,
-            "timestamp": 1.9837299440409952
-        },
-        {
-            "x": 7.603804643651593,
-            "y": 5.838908591345399,
-            "heading": 3.057713507827927,
-            "angularVelocity": 1.214240550550639,
-            "velocityX": 1.1618658469383054,
-            "velocityY": -0.6403078377491017,
-            "timestamp": 2.0165734560448545
-        },
-        {
-            "x": 7.6343059239967275,
-            "y": 5.822116657822096,
-            "heading": 3.090162525369235,
-            "angularVelocity": 0.987988663864437,
-            "velocityX": 0.9286851035160462,
-            "velocityY": -0.5112709481656601,
-            "timestamp": 2.0494169680487135
-        },
-        {
-            "x": 7.657163897668815,
-            "y": 5.809544651656199,
-            "heading": 3.1148617613725005,
-            "angularVelocity": 0.7520278586639367,
-            "velocityX": 0.6959661825873334,
-            "velocityY": -0.3827850737892687,
-            "timestamp": 2.0822604800525726
-        },
-        {
-            "x": 7.67239146004521,
-            "y": 5.801177089276775,
-            "heading": 3.1315500981004414,
-            "angularVelocity": 0.5081166936708882,
-            "velocityX": 0.4636398925488508,
-            "velocityY": -0.2547706341039437,
-            "timestamp": 2.1151039920564316
+            "heading": -0.5974593280342978,
+            "angularVelocity": 0.0000022926381384477543,
+            "velocityX": 3.7716260642834416,
+            "velocityY": 0.2923470648746024,
+            "timestamp": 1.2963561102392527
+        },
+        {
+            "x": 5.617528419554277,
+            "y": 6.721027319451783,
+            "heading": -0.5974592417476945,
+            "angularVelocity": 0.0000026270679336343826,
+            "velocityX": 3.7828927995748978,
+            "velocityY": 0.018711655155499447,
+            "timestamp": 1.3292013222818118
+        },
+        {
+            "x": 5.7414972044827435,
+            "y": 6.712651079745108,
+            "heading": -0.5974591685782128,
+            "angularVelocity": 0.0000022277061710620633,
+            "velocityX": 3.7743335244066216,
+            "velocityY": -0.25502163589934396,
+            "timestamp": 1.3620465343243708
+        },
+        {
+            "x": 5.864535126960113,
+            "y": 6.695327921921503,
+            "heading": -0.597459104000633,
+            "angularVelocity": 0.000001966118527479946,
+            "velocityX": 3.745992637160019,
+            "velocityY": -0.5274180541456184,
+            "timestamp": 1.39489174636693
+        },
+        {
+            "x": 5.985997338800097,
+            "y": 6.669148654285341,
+            "heading": -0.5974590451258569,
+            "angularVelocity": 0.0000017924918823644274,
+            "velocityX": 3.6980188065957362,
+            "velocityY": -0.7970497374814797,
+            "timestamp": 1.427736958409489
+        },
+        {
+            "x": 6.105247258928482,
+            "y": 6.634250515985412,
+            "heading": -0.5974589899613981,
+            "angularVelocity": 0.0000016795281661896423,
+            "velocityX": 3.6306637318675383,
+            "velocityY": -1.062503059946642,
+            "timestamp": 1.460582170452048
+        },
+        {
+            "x": 6.221659925637703,
+            "y": 6.5908164867561565,
+            "heading": -0.5974589370314296,
+            "angularVelocity": 0.0000016114972403778573,
+            "velocityX": 3.544281174328253,
+            "velocityY": -1.3223854110871922,
+            "timestamp": 1.4934273824946072
+        },
+        {
+            "x": 6.334625365293315,
+            "y": 6.539074496952364,
+            "heading": -0.5974588851619594,
+            "angularVelocity": 0.000001579209477235118,
+            "velocityX": 3.439327458421827,
+            "velocityY": -1.5753282316046957,
+            "timestamp": 1.5262725945371662
+        },
+        {
+            "x": 6.443562887659525,
+            "y": 6.4793163144818955,
+            "heading": -0.5974588333468978,
+            "angularVelocity": 0.000001577552964572491,
+            "velocityX": 3.3166941417319027,
+            "velocityY": -1.819387933752184,
+            "timestamp": 1.5591178065797253
+        },
+        {
+            "x": 6.552407332287041,
+            "y": 6.419388766569548,
+            "heading": -0.5974587815290724,
+            "angularVelocity": 0.0000015776371105880298,
+            "velocityX": 3.313860312014723,
+            "velocityY": -1.82454440649205,
+            "timestamp": 1.5919630186222844
+        },
+        {
+            "x": 6.661251765496281,
+            "y": 6.359461197918566,
+            "heading": -0.5974587297112393,
+            "angularVelocity": 0.0000015776373457032493,
+            "velocityX": 3.3138599643757076,
+            "velocityY": -1.8245450378971961,
+            "timestamp": 1.6248082306648435
+        },
+        {
+            "x": 6.7700961954772865,
+            "y": 6.299533623404254,
+            "heading": -0.5974586778933993,
+            "angularVelocity": 0.0000015776375522202912,
+            "velocityX": 3.3138598660894165,
+            "velocityY": -1.8245452164111537,
+            "timestamp": 1.6576534427074026
+        },
+        {
+            "x": 6.878940549868732,
+            "y": 6.239605911600315,
+            "heading": -0.5974586260742238,
+            "angularVelocity": 0.0000015776782163840074,
+            "velocityX": 3.313857564701912,
+            "velocityY": -1.8245493963088628,
+            "timestamp": 1.6904986547499616
+        },
+        {
+            "x": 6.983465419726436,
+            "y": 6.181138244955353,
+            "heading": -0.5831453971579332,
+            "angularVelocity": 0.4357782466962171,
+            "velocityX": 3.182347238991671,
+            "velocityY": -1.7800970981449478,
+            "timestamp": 1.7233438667925207
+        },
+        {
+            "x": 7.08190934617205,
+            "y": 6.126284607678917,
+            "heading": -0.5460039664027865,
+            "angularVelocity": 1.1308019782912204,
+            "velocityX": 2.997207821891462,
+            "velocityY": -1.67006494600528,
+            "timestamp": 1.7561890788350798
+        },
+        {
+            "x": 7.174234078294548,
+            "y": 6.075101338854014,
+            "heading": -0.4864629486170815,
+            "angularVelocity": 1.8127761729340726,
+            "velocityX": 2.8109038237566843,
+            "velocityY": -1.5583175032799648,
+            "timestamp": 1.7890342908776389
+        },
+        {
+            "x": 7.258956033298187,
+            "y": 6.028257642494675,
+            "heading": -0.41936488473987615,
+            "angularVelocity": 2.0428567728619895,
+            "velocityX": 2.5794309043853687,
+            "velocityY": -1.4261955836547433,
+            "timestamp": 1.821879502920198
+        },
+        {
+            "x": 7.335844716602702,
+            "y": 5.985834491701992,
+            "heading": -0.35186229403264746,
+            "angularVelocity": 2.0551729311350044,
+            "velocityX": 2.3409403843979617,
+            "velocityY": -1.2916083701234404,
+            "timestamp": 1.854724714962757
+        },
+        {
+            "x": 7.404915151007572,
+            "y": 5.947794038432469,
+            "heading": -0.2870870788105502,
+            "angularVelocity": 1.9721356993583767,
+            "velocityX": 2.1029072461275145,
+            "velocityY": -1.1581734719873862,
+            "timestamp": 1.8875699270053161
+        },
+        {
+            "x": 7.466209510665893,
+            "y": 5.914092129035952,
+            "heading": -0.22688750133470567,
+            "angularVelocity": 1.8328265744751362,
+            "velocityX": 1.866158135283738,
+            "velocityY": -1.0260828687243908,
+            "timestamp": 1.9204151390478752
+        },
+        {
+            "x": 7.519766848297347,
+            "y": 5.8846886900937605,
+            "heading": -0.17246123732016092,
+            "angularVelocity": 1.657053208974533,
+            "velocityX": 1.6305980172109753,
+            "velocityY": -0.8952123342686217,
+            "timestamp": 1.9532603510904343
+        },
+        {
+            "x": 7.565618503926309,
+            "y": 5.859549729252428,
+            "heading": -0.12462132977080041,
+            "angularVelocity": 1.4565260680124767,
+            "velocityX": 1.395992072438578,
+            "velocityY": -0.7653767254946843,
+            "timestamp": 1.9861055631329934
+        },
+        {
+            "x": 7.603788954978725,
+            "y": 5.838647308998552,
+            "heading": -0.08394075693399232,
+            "angularVelocity": 1.2385541242393951,
+            "velocityX": 1.1621313634075436,
+            "velocityY": -0.6363916977236806,
+            "timestamp": 2.0189507751755524
+        },
+        {
+            "x": 7.634297586862898,
+            "y": 5.821958675739452,
+            "heading": -0.050840342274680116,
+            "angularVelocity": 1.0077698574891136,
+            "velocityX": 0.928860859374263,
+            "velocityY": -0.5080994221459559,
+            "timestamp": 2.0517959872181115
+        },
+        {
+            "x": 7.657160251333572,
+            "y": 5.809465047264352,
+            "heading": -0.025644988874275507,
+            "angularVelocity": 0.7670936442051631,
+            "velocityX": 0.6960729752956952,
+            "velocityY": -0.3803789867120345,
+            "timestamp": 2.0846411992606706
+        },
+        {
+            "x": 7.672390418071531,
+            "y": 5.801150340997017,
+            "heading": -0.00862072816733467,
+            "angularVelocity": 0.5183178810016766,
+            "velocityX": 0.46369518693407513,
+            "velocityY": -0.2531481987862983,
+            "timestamp": 2.1174864113032297
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 0.257277659544025,
-            "velocityX": 0.2316603642721254,
-            "velocityY": -0.1271815656097946,
-            "timestamp": 2.1479475040602907
+            "heading": 8.329701949714689e-33,
+            "angularVelocity": 0.26246529193262896,
+            "velocityX": 0.2316800975013136,
+            "velocityY": -0.12636060901771562,
+            "timestamp": 2.1503316233457888
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": -3.3673783800968066e-32,
-            "velocityX": -2.4380081043450124e-32,
-            "velocityY": -4.2407952262773785e-32,
-            "timestamp": 2.1807910160641497
+            "heading": 5.789840595031671e-40,
+            "angularVelocity": 1.2947513524749855e-30,
+            "velocityX": 9.16355000594536e-32,
+            "velocityY": -1.9486684829089695e-31,
+            "timestamp": 2.183176835388348
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.traj
index 66059810..0646af70 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.traj	
@@ -3,515 +3,551 @@
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
+            "heading": -0.39,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": 3.853182471863691e-34,
+            "velocityY": -3.138879459525789e-33,
             "timestamp": 0
         },
         {
-            "x": 2.30610760298278,
-            "y": 4.37532703674674,
-            "heading": 2.7433864075571766,
-            "angularVelocity": -0.14821621401149343,
-            "velocityX": 0.13687656120621225,
-            "velocityY": 0.3434918886001163,
-            "timestamp": 0.04462124799861899
-        },
-        {
-            "x": 2.318848696543319,
-            "y": 4.405778929719923,
-            "heading": 2.7307174870785778,
-            "angularVelocity": -0.28392124933375007,
-            "velocityX": 0.28553870929233327,
-            "velocityY": 0.6824527403206025,
-            "timestamp": 0.08924249599723798
-        },
-        {
-            "x": 2.3388726580644836,
-            "y": 4.451074836672454,
-            "heading": 2.7126983162955036,
-            "angularVelocity": -0.4038248948938342,
-            "velocityX": 0.44875395510641974,
-            "velocityY": 1.0151196791701815,
-            "timestamp": 0.13386374399585696
-        },
-        {
-            "x": 2.3669952727104597,
-            "y": 4.510809884172449,
-            "heading": 2.6902411170995073,
-            "angularVelocity": -0.5032848744322687,
-            "velocityX": 0.6302516381174824,
-            "velocityY": 1.3387130611372482,
-            "timestamp": 0.17848499199447596
-        },
-        {
-            "x": 2.404258116570724,
-            "y": 4.5843745316547775,
-            "heading": 2.664559931990299,
-            "angularVelocity": -0.5755371322201343,
-            "velocityX": 0.8350919243992831,
-            "velocityY": 1.648646122237679,
-            "timestamp": 0.22310623999309495
-        },
-        {
-            "x": 2.452003610186855,
-            "y": 4.670805411588522,
-            "heading": 2.637322930497274,
-            "angularVelocity": -0.6104042964882519,
-            "velocityX": 1.070016993195909,
-            "velocityY": 1.9369893001742093,
-            "timestamp": 0.2677274879917139
-        },
-        {
-            "x": 2.5115645242184845,
-            "y": 4.768897077167795,
-            "heading": 2.6107796946069493,
-            "angularVelocity": -0.5948564211190585,
-            "velocityX": 1.3348105824712122,
-            "velocityY": 2.1983173931466227,
-            "timestamp": 0.3123487359903329
-        },
-        {
-            "x": 2.5833793756558516,
-            "y": 4.878178047966119,
-            "heading": 2.586043040896354,
-            "angularVelocity": -0.5543693827514722,
-            "velocityX": 1.60943170929665,
-            "velocityY": 2.449079210014641,
-            "timestamp": 0.3569699839889519
-        },
-        {
-            "x": 2.6682042395216055,
-            "y": 4.997714430694696,
-            "heading": 2.5651891379462763,
-            "angularVelocity": -0.46735364619839126,
-            "velocityX": 1.9009971184216836,
-            "velocityY": 2.678911686474495,
-            "timestamp": 0.4015912319875709
-        },
-        {
-            "x": 2.76752425780781,
-            "y": 5.124957228979585,
-            "heading": 2.5531720535122697,
-            "angularVelocity": -0.269313050911911,
-            "velocityX": 2.2258458187740118,
-            "velocityY": 2.8516189930148728,
-            "timestamp": 0.4462124799861899
-        },
-        {
-            "x": 2.882656696791853,
-            "y": 5.248398665617137,
-            "heading": 2.553172127817037,
-            "angularVelocity": 0.0000016652328226055909,
-            "velocityX": 2.580215573253489,
-            "velocityY": 2.76642725549436,
-            "timestamp": 0.4908337279848089
-        },
-        {
-            "x": 3.0093581189008844,
-            "y": 5.359933509101588,
-            "heading": 2.5531721828704046,
-            "angularVelocity": 0.000001233792641064896,
-            "velocityX": 2.8394862939053795,
-            "velocityY": 2.4995904078680997,
-            "timestamp": 0.5354549759834278
-        },
-        {
-            "x": 3.146402398989035,
-            "y": 5.458483685796272,
-            "heading": 2.553172234700022,
-            "angularVelocity": 0.0000011615456718217746,
-            "velocityX": 3.071278510461917,
-            "velocityY": 2.20859301599395,
-            "timestamp": 0.5800762239820468
-        },
-        {
-            "x": 3.286524028997536,
-            "y": 5.552607013873938,
-            "heading": 2.5531722858926313,
-            "angularVelocity": 0.0000011472697806901915,
-            "velocityX": 3.140244531323671,
-            "velocityY": 2.1093835851605776,
-            "timestamp": 0.6246974719806658
-        },
-        {
-            "x": 3.426645803515369,
-            "y": 5.646730126819845,
-            "heading": 2.5531723370852246,
-            "angularVelocity": 0.000001147269414679425,
-            "velocityX": 3.140247769900325,
-            "velocityY": 2.109378763875461,
-            "timestamp": 0.6693187199792848
-        },
-        {
-            "x": 3.5667675932030476,
-            "y": 5.740853217182279,
-            "heading": 2.5531723882778183,
-            "angularVelocity": 0.0000011472694285852317,
-            "velocityX": 3.1402481098694217,
-            "velocityY": 2.109378257760682,
-            "timestamp": 0.7139399679779038
-        },
-        {
-            "x": 3.70688939501189,
-            "y": 5.834976289499829,
-            "heading": 2.553172439470412,
-            "angularVelocity": 0.0000011472694368681445,
-            "velocityX": 3.1402483815149935,
-            "velocityY": 2.109377853359542,
-            "timestamp": 0.7585612159765228
-        },
-        {
-            "x": 3.8470112068261555,
-            "y": 5.929099346922214,
-            "heading": 2.553172490663007,
-            "angularVelocity": 0.0000011472694500175128,
-            "velocityX": 3.140248605744995,
-            "velocityY": 2.1093775195462685,
-            "timestamp": 0.8031824639751418
-        },
-        {
-            "x": 3.9871330270690666,
-            "y": 6.023222391796794,
-            "heading": 2.5531725418556026,
-            "angularVelocity": 0.000001147269470343035,
-            "velocityX": 3.1402487946380906,
-            "velocityY": 2.109377238339292,
-            "timestamp": 0.8478037119737608
-        },
-        {
-            "x": 4.127254854511192,
-            "y": 6.117345425953829,
-            "heading": 2.553172593048199,
-            "angularVelocity": 0.000001147269495912955,
-            "velocityX": 3.1402489559785947,
-            "velocityY": 2.109376998150064,
-            "timestamp": 0.8924249599723798
-        },
-        {
-            "x": 4.267376688165873,
-            "y": 6.21146845086217,
-            "heading": 2.5531726442407967,
-            "angularVelocity": 0.0000011472695239009516,
-            "velocityX": 3.1402490952072233,
-            "velocityY": 2.1093767908789607,
-            "timestamp": 0.9370462079709988
-        },
-        {
-            "x": 4.407498527228544,
-            "y": 6.305591467719579,
-            "heading": 2.553172695433396,
-            "angularVelocity": 0.0000011472695542073644,
-            "velocityX": 3.140249216404848,
-            "velocityY": 2.1093766104507625,
-            "timestamp": 0.9816674559696178
-        },
-        {
-            "x": 4.547620436764314,
-            "y": 6.399714379662917,
-            "heading": 2.5531727466259886,
-            "angularVelocity": 0.0000011472694062933976,
-            "velocityX": 3.1402507957667765,
-            "velocityY": 2.1093742592374776,
-            "timestamp": 1.0262887039682367
-        },
-        {
-            "x": 4.689466118908105,
-            "y": 6.4912188851781,
-            "heading": 2.553172797709037,
-            "angularVelocity": 0.0000011448144250292167,
-            "velocityX": 3.1788820014218735,
-            "velocityY": 2.050693551153348,
-            "timestamp": 1.0709099519668557
-        },
-        {
-            "x": 4.839613537732198,
-            "y": 6.568348804669705,
-            "heading": 2.5531728491363963,
-            "angularVelocity": 0.0000011525307241082326,
-            "velocityX": 3.364930959096914,
-            "velocityY": 1.7285468907994308,
-            "timestamp": 1.1155311999654747
-        },
-        {
-            "x": 4.996611329828,
-            "y": 6.630356582585623,
-            "heading": 2.5531729033694757,
-            "angularVelocity": 0.0000012154092882901391,
-            "velocityX": 3.5184536322395523,
-            "velocityY": 1.3896468767040242,
-            "timestamp": 1.1601524479640937
-        },
-        {
-            "x": 5.158940949366324,
-            "y": 6.676642268737186,
-            "heading": 2.553172963573569,
-            "angularVelocity": 0.0000013492247785707482,
-            "velocityX": 3.637944405843334,
-            "velocityY": 1.037301470209733,
-            "timestamp": 1.2047736959627127
-        },
-        {
-            "x": 5.325032205172685,
-            "y": 6.706758095236814,
-            "heading": 2.5531730345823234,
-            "angularVelocity": 0.0000015913663950616823,
-            "velocityX": 3.7222458639324056,
-            "velocityY": 0.6749212057125495,
-            "timestamp": 1.2493949439613317
+            "x": 2.304641315068496,
+            "y": 4.371901353899003,
+            "heading": -0.39554860460981267,
+            "angularVelocity": -0.1412451028521288,
+            "velocityX": 0.11814916908296796,
+            "velocityY": 0.3029604871416256,
+            "timestamp": 0.039283518492098594
+        },
+        {
+            "x": 2.314310378834228,
+            "y": 4.395560050627161,
+            "heading": -0.4062436571914542,
+            "angularVelocity": -0.27225291909092403,
+            "velocityX": 0.24613538035494495,
+            "velocityY": 0.6022550330596644,
+            "timestamp": 0.07856703698419719
+        },
+        {
+            "x": 2.3294686593461313,
+            "y": 4.43078385259534,
+            "heading": -0.42159423390969814,
+            "angularVelocity": -0.3907637937605655,
+            "velocityX": 0.385868707635076,
+            "velocityY": 0.8966559850097278,
+            "timestamp": 0.11785055547629578
+        },
+        {
+            "x": 2.3506733572393026,
+            "y": 4.477309554936099,
+            "heading": -0.44099148005781696,
+            "angularVelocity": -0.49377568233887975,
+            "velocityX": 0.5397861165984255,
+            "velocityY": 1.1843568022074944,
+            "timestamp": 0.15713407396839438
+        },
+        {
+            "x": 2.3786040121360466,
+            "y": 4.534766426387325,
+            "heading": -0.4636665715847995,
+            "angularVelocity": -0.5772164102747018,
+            "velocityX": 0.7110018646207296,
+            "velocityY": 1.462620296162777,
+            "timestamp": 0.19641759246049298
+        },
+        {
+            "x": 2.4140943920114943,
+            "y": 4.602616046670193,
+            "heading": -0.48862953085625704,
+            "angularVelocity": -0.6354562989682879,
+            "velocityX": 0.903441983756771,
+            "velocityY": 1.727177780588029,
+            "timestamp": 0.23570111095259158
+        },
+        {
+            "x": 2.458163120855333,
+            "y": 4.68005234098581,
+            "heading": -0.5145799632934547,
+            "angularVelocity": -0.6605933845360225,
+            "velocityX": 1.1218121628463973,
+            "velocityY": 1.9712158505149888,
+            "timestamp": 0.27498462944469015
+        },
+        {
+            "x": 2.5115097740388146,
+            "y": 4.766403162212065,
+            "heading": -0.5400224328341426,
+            "angularVelocity": -0.6476626971635644,
+            "velocityX": 1.3579907103842537,
+            "velocityY": 2.1981437646332993,
+            "timestamp": 0.31426814793678876
+        },
+        {
+            "x": 2.5744195202560527,
+            "y": 4.861351309968927,
+            "heading": -0.563860333505168,
+            "angularVelocity": -0.6068168429413633,
+            "velocityX": 1.6014285031491418,
+            "velocityY": 2.416997035944537,
+            "timestamp": 0.35355166642888736
+        },
+        {
+            "x": 2.6473242968990576,
+            "y": 4.964346697364103,
+            "heading": -0.5842892856317753,
+            "angularVelocity": -0.5200387569839264,
+            "velocityX": 1.8558616804566808,
+            "velocityY": 2.6218473127835553,
+            "timestamp": 0.39283518492098596
+        },
+        {
+            "x": 2.7309171688764238,
+            "y": 5.07424676618957,
+            "heading": -0.5978668700067779,
+            "angularVelocity": -0.3456305569399124,
+            "velocityX": 2.127937496081736,
+            "velocityY": 2.7976126641403014,
+            "timestamp": 0.43211870341308456
+        },
+        {
+            "x": 2.826173576921219,
+            "y": 5.187875652325513,
+            "heading": -0.5974605263008922,
+            "angularVelocity": 0.010343872481393205,
+            "velocityX": 2.4248441000503926,
+            "velocityY": 2.8925333192550147,
+            "timestamp": 0.47140222190518316
+        },
+        {
+            "x": 2.9311441629204515,
+            "y": 5.293067244130248,
+            "heading": -0.5974604636085413,
+            "angularVelocity": 0.000001595894492248037,
+            "velocityX": 2.672127905761066,
+            "velocityY": 2.677753822532693,
+            "timestamp": 0.5106857403972818
+        },
+        {
+            "x": 3.044820699228354,
+            "y": 5.388784226639372,
+            "heading": -0.597460407679312,
+            "angularVelocity": 0.0000014237326879146737,
+            "velocityX": 2.8937463005192323,
+            "velocityY": 2.436568468997834,
+            "timestamp": 0.5499692588893803
+        },
+        {
+            "x": 3.1663502143390825,
+            "y": 5.474310087323753,
+            "heading": -0.5974603544159754,
+            "angularVelocity": 0.0000013558698005366095,
+            "velocityX": 3.0936514796952492,
+            "velocityY": 2.177143595261183,
+            "timestamp": 0.5892527773814789
+        },
+        {
+            "x": 3.289918343405979,
+            "y": 5.556863181758907,
+            "heading": -0.5974603016382944,
+            "angularVelocity": 0.0000013435069733969365,
+            "velocityX": 3.145546371864123,
+            "velocityY": 2.1014689519673895,
+            "timestamp": 0.6285362958735774
+        },
+        {
+            "x": 3.413486607435696,
+            "y": 5.639416074177005,
+            "heading": -0.5974602488606314,
+            "angularVelocity": 0.0000013435065140042136,
+            "velocityX": 3.145549807473418,
+            "velocityY": 2.101463809427605,
+            "timestamp": 0.667819814365676
+        },
+        {
+            "x": 3.5370548851900594,
+            "y": 5.721968946051526,
+            "heading": -0.5974601960829673,
+            "angularVelocity": 0.0000013435065431603766,
+            "velocityX": 3.145550156847558,
+            "velocityY": 2.1014632864709872,
+            "timestamp": 0.7071033328577745
+        },
+        {
+            "x": 3.6606231741521,
+            "y": 5.804521801149959,
+            "heading": -0.5974601433053026,
+            "angularVelocity": 0.0000013435065598734946,
+            "velocityX": 3.1455504421498475,
+            "velocityY": 2.1014628594194082,
+            "timestamp": 0.746386851349873
+        },
+        {
+            "x": 3.7841914725218415,
+            "y": 5.887074642166574,
+            "heading": -0.597460090527637,
+            "angularVelocity": 0.0000013435065802330538,
+            "velocityX": 3.1455506816319834,
+            "velocityY": 2.101462500953099,
+            "timestamp": 0.7856703698419716
+        },
+        {
+            "x": 3.9077597789317933,
+            "y": 5.969627471148282,
+            "heading": -0.5974600377499704,
+            "angularVelocity": 0.0000013435066065618314,
+            "velocityX": 3.14555088630331,
+            "velocityY": 2.1014621945928877,
+            "timestamp": 0.8249538883340701
+        },
+        {
+            "x": 4.031328092300017,
+            "y": 6.052180289714568,
+            "heading": -0.5974599849723026,
+            "angularVelocity": 0.0000013435066364860128,
+            "velocityX": 3.1455510634328734,
+            "velocityY": 2.101461929458217,
+            "timestamp": 0.8642374068261687
+        },
+        {
+            "x": 4.154896411746533,
+            "y": 6.134733099182619,
+            "heading": -0.5974599321946334,
+            "angularVelocity": 0.0000013435066718879358,
+            "velocityX": 3.14555121816168,
+            "velocityY": 2.1014616978538454,
+            "timestamp": 0.9035209253182672
+        },
+        {
+            "x": 4.27846473654201,
+            "y": 6.217285900644126,
+            "heading": -0.5974598794169629,
+            "angularVelocity": 0.000001343506708988157,
+            "velocityX": 3.14555135432466,
+            "velocityY": 2.1014614940395124,
+            "timestamp": 0.9428044438103658
+        },
+        {
+            "x": 4.4020330660739395,
+            "y": 6.299838695015915,
+            "heading": -0.5974598266392908,
+            "angularVelocity": 0.0000013435067467217265,
+            "velocityX": 3.1455514748956417,
+            "velocityY": 2.1014613135638847,
+            "timestamp": 0.9820879623024643
+        },
+        {
+            "x": 4.525601399950297,
+            "y": 6.382391482884783,
+            "heading": -0.5974597738616171,
+            "angularVelocity": 0.0000013435067857429384,
+            "velocityX": 3.1455515854872873,
+            "velocityY": 2.101461148025713,
+            "timestamp": 1.0213714807945629
+        },
+        {
+            "x": 4.6491721527053596,
+            "y": 6.464940649963802,
+            "heading": -0.5974597210842042,
+            "angularVelocity": 0.0000013435001464771913,
+            "velocityX": 3.1456131603868194,
+            "velocityY": 2.101368977314248,
+            "timestamp": 1.0606549992866614
+        },
+        {
+            "x": 4.778837800088318,
+            "y": 6.537541722502267,
+            "heading": -0.5974596685786843,
+            "angularVelocity": 0.0000013365788503576918,
+            "velocityX": 3.300764604603488,
+            "velocityY": 1.8481305984081453,
+            "timestamp": 1.09993851777876
+        },
+        {
+            "x": 4.914297553492409,
+            "y": 6.59865432938236,
+            "heading": -0.5974596146272461,
+            "angularVelocity": 0.0000013733860988547744,
+            "velocityX": 3.4482591835906553,
+            "velocityY": 1.5556805812161723,
+            "timestamp": 1.1392220362708585
+        },
+        {
+            "x": 5.054536029054771,
+            "y": 6.647820017049671,
+            "heading": -0.5974595570647036,
+            "angularVelocity": 0.0000014653102566024163,
+            "velocityX": 3.5699061831898264,
+            "velocityY": 1.2515601848929776,
+            "timestamp": 1.178505554762957
+        },
+        {
+            "x": 5.198501873691458,
+            "y": 6.684670110841039,
+            "heading": -0.5974594930097576,
+            "angularVelocity": 0.0000016305806703071774,
+            "velocityX": 3.6647899720507366,
+            "velocityY": 0.9380548180449542,
+            "timestamp": 1.2177890732550556
+        },
+        {
+            "x": 5.345115761176417,
+            "y": 6.7089283098414425,
+            "heading": -0.5974594180971905,
+            "angularVelocity": 0.0000019069719319939646,
+            "velocityX": 3.7321984667555648,
+            "velocityY": 0.6175159438731035,
+            "timestamp": 1.2570725917471541
         },
         {
             "x": 5.493278503417969,
             "y": 6.720412731170654,
-            "heading": 2.553173120074835,
-            "angularVelocity": 0.000001915959677785522,
-            "velocityX": 3.7705421921522753,
-            "velocityY": 0.30601196842950656,
-            "timestamp": 1.2940161919599507
-        },
-        {
-            "x": 5.617518941656642,
-            "y": 6.721477956029719,
-            "heading": 2.5531732061618477,
-            "angularVelocity": 0.0000026211268995160273,
-            "velocityX": 3.782800031375301,
-            "velocityY": 0.03243334205370069,
-            "timestamp": 1.32685970396381
-        },
-        {
-            "x": 5.741510905915031,
-            "y": 6.713552321996202,
-            "heading": 2.5531732789431283,
-            "angularVelocity": 0.0000022160017651615355,
-            "velocityX": 3.7752346412837734,
-            "velocityY": -0.24131505889453328,
-            "timestamp": 1.3597032159676692
-        },
-        {
-            "x": 5.8646046105180325,
-            "y": 6.696677372566761,
-            "heading": 2.553173343002795,
-            "angularVelocity": 0.0000019504511878675683,
-            "velocityX": 3.7478849578735156,
-            "velocityY": -0.5137985678101988,
-            "timestamp": 1.3925467279715285
-        },
-        {
-            "x": 5.986154981292517,
-            "y": 6.670941556957854,
-            "heading": 2.5531734012532086,
-            "angularVelocity": 0.0000017735744392757528,
-            "velocityX": 3.7008944341945598,
-            "velocityY": -0.7835890268336883,
-            "timestamp": 1.4253902399753877
-        },
-        {
-            "x": 6.105525040034572,
-            "y": 6.636479774106562,
-            "heading": 2.553173455691931,
-            "angularVelocity": 0.000001657518306287931,
-            "velocityX": 3.634509571571689,
-            "velocityY": -1.0492721621013206,
-            "timestamp": 1.458233751979247
-        },
-        {
-            "x": 6.222089256538776,
-            "y": 6.593472690949968,
-            "heading": 2.5531735077894595,
-            "angularVelocity": 0.0000015862350106976908,
-            "velocityX": 3.549078931951851,
-            "velocityY": -1.3094544563790795,
-            "timestamp": 1.4910772639831062
-        },
-        {
-            "x": 6.335236901536747,
-            "y": 6.54214592976541,
-            "heading": 2.553173558707783,
-            "angularVelocity": 0.000001550331263346088,
-            "velocityX": 3.4450531655894774,
-            "velocityY": -1.5627671358204458,
-            "timestamp": 1.5239207759869655
-        },
-        {
-            "x": 6.444377333823394,
-            "y": 6.482772601626653,
-            "heading": 2.5531736094378257,
-            "angularVelocity": 0.0000015445985995778538,
-            "velocityX": 3.3230439020592755,
-            "velocityY": -1.8077642893915407,
-            "timestamp": 1.5567642879908248
-        },
-        {
-            "x": 6.553087073787246,
-            "y": 6.422614322949958,
-            "heading": 2.5531736601729174,
-            "angularVelocity": 0.0000015447523320156895,
-            "velocityX": 3.3099304346952643,
-            "velocityY": -1.8316640032170186,
-            "timestamp": 1.589607799994684
-        },
-        {
-            "x": 6.661796772216766,
-            "y": 6.362455969218214,
-            "heading": 2.553173710908019,
-            "angularVelocity": 0.0000015447526253443646,
-            "velocityX": 3.3099291700822753,
-            "velocityY": -1.8316662884491148,
-            "timestamp": 1.6224513119985433
-        },
-        {
-            "x": 6.770506467400396,
-            "y": 6.302297609620959,
-            "heading": 2.5531737616431265,
-            "angularVelocity": 0.0000015447528238732798,
-            "velocityX": 3.3099290712533134,
-            "velocityY": -1.831666467038753,
-            "timestamp": 1.6552948240024026
-        },
-        {
-            "x": 6.879216090079623,
-            "y": 6.242139119005013,
-            "heading": 2.5531738123794403,
-            "angularVelocity": 0.0000015447895469442916,
-            "velocityX": 3.309926863681755,
-            "velocityY": -1.831670456219097,
-            "timestamp": 1.6881383360062618
-        },
-        {
-            "x": 6.983740227457298,
-            "y": 6.1833650705733145,
-            "heading": 2.56712091506752,
-            "angularVelocity": 0.4246532065888785,
-            "velocityX": 3.18248966083146,
-            "velocityY": -1.7895177721795115,
-            "timestamp": 1.720981848010121
-        },
-        {
-            "x": 7.082197224690116,
-            "y": 6.128185260935312,
-            "heading": 2.6040317014746788,
-            "angularVelocity": 1.1238379867177672,
-            "velocityX": 2.9977609343741736,
-            "velocityY": -1.6800824964004346,
-            "timestamp": 1.7538253600139804
-        },
-        {
-            "x": 7.174469097455313,
-            "y": 6.076712532186898,
-            "heading": 2.662828331449144,
-            "angularVelocity": 1.7902053217559901,
-            "velocityX": 2.809439890422065,
-            "velocityY": -1.567211470635817,
-            "timestamp": 1.7866688720178396
-        },
-        {
-            "x": 7.2591302790362535,
-            "y": 6.029614181571086,
-            "heading": 2.728764792304138,
-            "angularVelocity": 2.0075947069012146,
-            "velocityX": 2.577714026776202,
-            "velocityY": -1.4340229695982882,
-            "timestamp": 1.819512384021699
-        },
-        {
-            "x": 7.335970518862824,
-            "y": 5.986956845665554,
-            "heading": 2.7950149925408634,
-            "angularVelocity": 2.0171472596761757,
-            "velocityX": 2.339586577024463,
-            "velocityY": -1.2988055571072747,
-            "timestamp": 1.8523558960255582
-        },
-        {
-            "x": 7.405004233411928,
-            "y": 5.94870169343351,
-            "heading": 2.858549177078574,
-            "angularVelocity": 1.9344516058527972,
-            "velocityX": 2.1018980717102513,
-            "velocityY": -1.1647704492609365,
-            "timestamp": 1.8851994080294174
-        },
-        {
-            "x": 7.466271161714403,
-            "y": 5.914805217934882,
-            "heading": 2.9175764455342432,
-            "angularVelocity": 1.7972276670270115,
-            "velocityX": 1.8654195171112133,
-            "velocityY": -1.0320600152214254,
-            "timestamp": 1.9180429200332767
-        },
-        {
-            "x": 7.519808154158724,
-            "y": 5.885228450835136,
-            "heading": 2.9709348371498114,
-            "angularVelocity": 1.6246250282034163,
-            "velocityX": 1.6300629615380646,
-            "velocityY": -0.9005360661877405,
-            "timestamp": 1.950886432037136
-        },
-        {
-            "x": 7.565644888760801,
-            "y": 5.8599385495006775,
-            "heading": 3.0178335837303445,
-            "angularVelocity": 1.427945542943834,
-            "velocityX": 1.3956100247954721,
-            "velocityY": -0.770012090408821,
-            "timestamp": 1.9837299440409952
-        },
-        {
-            "x": 7.603804643651593,
-            "y": 5.838908591345399,
-            "heading": 3.057713507827927,
-            "angularVelocity": 1.214240550550639,
-            "velocityX": 1.1618658469383054,
-            "velocityY": -0.6403078377491017,
-            "timestamp": 2.0165734560448545
-        },
-        {
-            "x": 7.6343059239967275,
-            "y": 5.822116657822096,
-            "heading": 3.090162525369235,
-            "angularVelocity": 0.987988663864437,
-            "velocityX": 0.9286851035160462,
-            "velocityY": -0.5112709481656601,
-            "timestamp": 2.0494169680487135
-        },
-        {
-            "x": 7.657163897668815,
-            "y": 5.809544651656199,
-            "heading": 3.1148617613725005,
-            "angularVelocity": 0.7520278586639367,
-            "velocityX": 0.6959661825873334,
-            "velocityY": -0.3827850737892687,
-            "timestamp": 2.0822604800525726
-        },
-        {
-            "x": 7.67239146004521,
-            "y": 5.801177089276775,
-            "heading": 3.1315500981004414,
-            "angularVelocity": 0.5081166936708882,
-            "velocityX": 0.4636398925488508,
-            "velocityY": -0.2547706341039437,
-            "timestamp": 2.1151039920564316
+            "heading": -0.5974593280342978,
+            "angularVelocity": 0.0000022926381384477543,
+            "velocityX": 3.7716260642834416,
+            "velocityY": 0.2923470648746024,
+            "timestamp": 1.2963561102392527
+        },
+        {
+            "x": 5.617528419554277,
+            "y": 6.721027319451783,
+            "heading": -0.5974592417476945,
+            "angularVelocity": 0.0000026270679336343826,
+            "velocityX": 3.7828927995748978,
+            "velocityY": 0.018711655155499447,
+            "timestamp": 1.3292013222818118
+        },
+        {
+            "x": 5.7414972044827435,
+            "y": 6.712651079745108,
+            "heading": -0.5974591685782128,
+            "angularVelocity": 0.0000022277061710620633,
+            "velocityX": 3.7743335244066216,
+            "velocityY": -0.25502163589934396,
+            "timestamp": 1.3620465343243708
+        },
+        {
+            "x": 5.864535126960113,
+            "y": 6.695327921921503,
+            "heading": -0.597459104000633,
+            "angularVelocity": 0.000001966118527479946,
+            "velocityX": 3.745992637160019,
+            "velocityY": -0.5274180541456184,
+            "timestamp": 1.39489174636693
+        },
+        {
+            "x": 5.985997338800097,
+            "y": 6.669148654285341,
+            "heading": -0.5974590451258569,
+            "angularVelocity": 0.0000017924918823644274,
+            "velocityX": 3.6980188065957362,
+            "velocityY": -0.7970497374814797,
+            "timestamp": 1.427736958409489
+        },
+        {
+            "x": 6.105247258928482,
+            "y": 6.634250515985412,
+            "heading": -0.5974589899613981,
+            "angularVelocity": 0.0000016795281661896423,
+            "velocityX": 3.6306637318675383,
+            "velocityY": -1.062503059946642,
+            "timestamp": 1.460582170452048
+        },
+        {
+            "x": 6.221659925637703,
+            "y": 6.5908164867561565,
+            "heading": -0.5974589370314296,
+            "angularVelocity": 0.0000016114972403778573,
+            "velocityX": 3.544281174328253,
+            "velocityY": -1.3223854110871922,
+            "timestamp": 1.4934273824946072
+        },
+        {
+            "x": 6.334625365293315,
+            "y": 6.539074496952364,
+            "heading": -0.5974588851619594,
+            "angularVelocity": 0.000001579209477235118,
+            "velocityX": 3.439327458421827,
+            "velocityY": -1.5753282316046957,
+            "timestamp": 1.5262725945371662
+        },
+        {
+            "x": 6.443562887659525,
+            "y": 6.4793163144818955,
+            "heading": -0.5974588333468978,
+            "angularVelocity": 0.000001577552964572491,
+            "velocityX": 3.3166941417319027,
+            "velocityY": -1.819387933752184,
+            "timestamp": 1.5591178065797253
+        },
+        {
+            "x": 6.552407332287041,
+            "y": 6.419388766569548,
+            "heading": -0.5974587815290724,
+            "angularVelocity": 0.0000015776371105880298,
+            "velocityX": 3.313860312014723,
+            "velocityY": -1.82454440649205,
+            "timestamp": 1.5919630186222844
+        },
+        {
+            "x": 6.661251765496281,
+            "y": 6.359461197918566,
+            "heading": -0.5974587297112393,
+            "angularVelocity": 0.0000015776373457032493,
+            "velocityX": 3.3138599643757076,
+            "velocityY": -1.8245450378971961,
+            "timestamp": 1.6248082306648435
+        },
+        {
+            "x": 6.7700961954772865,
+            "y": 6.299533623404254,
+            "heading": -0.5974586778933993,
+            "angularVelocity": 0.0000015776375522202912,
+            "velocityX": 3.3138598660894165,
+            "velocityY": -1.8245452164111537,
+            "timestamp": 1.6576534427074026
+        },
+        {
+            "x": 6.878940549868732,
+            "y": 6.239605911600315,
+            "heading": -0.5974586260742238,
+            "angularVelocity": 0.0000015776782163840074,
+            "velocityX": 3.313857564701912,
+            "velocityY": -1.8245493963088628,
+            "timestamp": 1.6904986547499616
+        },
+        {
+            "x": 6.983465419726436,
+            "y": 6.181138244955353,
+            "heading": -0.5831453971579332,
+            "angularVelocity": 0.4357782466962171,
+            "velocityX": 3.182347238991671,
+            "velocityY": -1.7800970981449478,
+            "timestamp": 1.7233438667925207
+        },
+        {
+            "x": 7.08190934617205,
+            "y": 6.126284607678917,
+            "heading": -0.5460039664027865,
+            "angularVelocity": 1.1308019782912204,
+            "velocityX": 2.997207821891462,
+            "velocityY": -1.67006494600528,
+            "timestamp": 1.7561890788350798
+        },
+        {
+            "x": 7.174234078294548,
+            "y": 6.075101338854014,
+            "heading": -0.4864629486170815,
+            "angularVelocity": 1.8127761729340726,
+            "velocityX": 2.8109038237566843,
+            "velocityY": -1.5583175032799648,
+            "timestamp": 1.7890342908776389
+        },
+        {
+            "x": 7.258956033298187,
+            "y": 6.028257642494675,
+            "heading": -0.41936488473987615,
+            "angularVelocity": 2.0428567728619895,
+            "velocityX": 2.5794309043853687,
+            "velocityY": -1.4261955836547433,
+            "timestamp": 1.821879502920198
+        },
+        {
+            "x": 7.335844716602702,
+            "y": 5.985834491701992,
+            "heading": -0.35186229403264746,
+            "angularVelocity": 2.0551729311350044,
+            "velocityX": 2.3409403843979617,
+            "velocityY": -1.2916083701234404,
+            "timestamp": 1.854724714962757
+        },
+        {
+            "x": 7.404915151007572,
+            "y": 5.947794038432469,
+            "heading": -0.2870870788105502,
+            "angularVelocity": 1.9721356993583767,
+            "velocityX": 2.1029072461275145,
+            "velocityY": -1.1581734719873862,
+            "timestamp": 1.8875699270053161
+        },
+        {
+            "x": 7.466209510665893,
+            "y": 5.914092129035952,
+            "heading": -0.22688750133470567,
+            "angularVelocity": 1.8328265744751362,
+            "velocityX": 1.866158135283738,
+            "velocityY": -1.0260828687243908,
+            "timestamp": 1.9204151390478752
+        },
+        {
+            "x": 7.519766848297347,
+            "y": 5.8846886900937605,
+            "heading": -0.17246123732016092,
+            "angularVelocity": 1.657053208974533,
+            "velocityX": 1.6305980172109753,
+            "velocityY": -0.8952123342686217,
+            "timestamp": 1.9532603510904343
+        },
+        {
+            "x": 7.565618503926309,
+            "y": 5.859549729252428,
+            "heading": -0.12462132977080041,
+            "angularVelocity": 1.4565260680124767,
+            "velocityX": 1.395992072438578,
+            "velocityY": -0.7653767254946843,
+            "timestamp": 1.9861055631329934
+        },
+        {
+            "x": 7.603788954978725,
+            "y": 5.838647308998552,
+            "heading": -0.08394075693399232,
+            "angularVelocity": 1.2385541242393951,
+            "velocityX": 1.1621313634075436,
+            "velocityY": -0.6363916977236806,
+            "timestamp": 2.0189507751755524
+        },
+        {
+            "x": 7.634297586862898,
+            "y": 5.821958675739452,
+            "heading": -0.050840342274680116,
+            "angularVelocity": 1.0077698574891136,
+            "velocityX": 0.928860859374263,
+            "velocityY": -0.5080994221459559,
+            "timestamp": 2.0517959872181115
+        },
+        {
+            "x": 7.657160251333572,
+            "y": 5.809465047264352,
+            "heading": -0.025644988874275507,
+            "angularVelocity": 0.7670936442051631,
+            "velocityX": 0.6960729752956952,
+            "velocityY": -0.3803789867120345,
+            "timestamp": 2.0846411992606706
+        },
+        {
+            "x": 7.672390418071531,
+            "y": 5.801150340997017,
+            "heading": -0.00862072816733467,
+            "angularVelocity": 0.5183178810016766,
+            "velocityX": 0.46369518693407513,
+            "velocityY": -0.2531481987862983,
+            "timestamp": 2.1174864113032297
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 0.257277659544025,
-            "velocityX": 0.2316603642721254,
-            "velocityY": -0.1271815656097946,
-            "timestamp": 2.1479475040602907
+            "heading": 8.329701949714689e-33,
+            "angularVelocity": 0.26246529193262896,
+            "velocityX": 0.2316800975013136,
+            "velocityY": -0.12636060901771562,
+            "timestamp": 2.1503316233457888
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": -3.3673783800968066e-32,
-            "velocityX": -2.4380081043450124e-32,
-            "velocityY": -4.2407952262773785e-32,
-            "timestamp": 2.1807910160641497
+            "heading": 5.789840595031671e-40,
+            "angularVelocity": 1.2947513524749855e-30,
+            "velocityX": 9.16355000594536e-32,
+            "velocityY": -1.9486684829089695e-31,
+            "timestamp": 2.183176835388348
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.1.traj
index af794028..ea265506 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.1.traj	
@@ -3,866 +3,308 @@
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
+            "heading": -0.39,
             "angularVelocity": 0,
-            "velocityX": -5.720774693888146e-34,
-            "velocityY": 0,
+            "velocityX": 0,
+            "velocityY": 1.9823938961383557e-29,
             "timestamp": 0
         },
         {
-            "x": 2.3121576423431627,
-            "y": 4.3971322791414,
-            "heading": 2.7106310928070285,
-            "angularVelocity": -0.5647989987388474,
-            "velocityX": 0.1744174454420521,
-            "velocityY": 0.5327116136893348,
-            "timestamp": 0.06970427936465769
-        },
-        {
-            "x": 2.3388751368329244,
-            "y": 4.470343590887415,
-            "heading": 2.629622228062893,
-            "angularVelocity": -1.1621792160039144,
-            "velocityX": 0.38329776497637447,
-            "velocityY": 1.0503130139687884,
-            "timestamp": 0.13940855872931537
-        },
-        {
-            "x": 2.3840539747737246,
-            "y": 4.577503337755321,
-            "heading": 2.5035786127839184,
-            "angularVelocity": -1.8082622247563562,
-            "velocityX": 0.6481501329989676,
-            "velocityY": 1.5373481778256302,
-            "timestamp": 0.20911283809397307
-        },
-        {
-            "x": 2.4545399331182174,
-            "y": 4.713290189810374,
-            "heading": 2.3271901249556994,
-            "angularVelocity": -2.5305259510028497,
-            "velocityX": 1.011214217935542,
-            "velocityY": 1.948041831760208,
-            "timestamp": 0.27881711745863075
-        },
-        {
-            "x": 2.555797664071962,
-            "y": 4.87530128954077,
-            "heading": 2.141593101929859,
-            "angularVelocity": -2.662634557268572,
-            "velocityX": 1.4526759601661592,
-            "velocityY": 2.3242633193700435,
-            "timestamp": 0.3485213968232884
-        },
-        {
-            "x": 2.6889975632138237,
-            "y": 5.05237912689222,
-            "heading": 2.0338949096526036,
-            "angularVelocity": -1.545072888765306,
-            "velocityX": 1.910928573624968,
-            "velocityY": 2.54041558087227,
-            "timestamp": 0.4182256761879461
-        },
-        {
-            "x": 2.8587006072828576,
-            "y": 5.214124992637136,
-            "heading": 1.956646376370765,
-            "angularVelocity": -1.108232291990471,
-            "velocityX": 2.4346144256256212,
-            "velocityY": 2.3204581873480494,
-            "timestamp": 0.48792995555260377
-        },
-        {
-            "x": 3.035891081202655,
-            "y": 5.338211520127435,
-            "heading": 1.8312106553523118,
-            "angularVelocity": -1.7995411782716633,
-            "velocityX": 2.5420315012917163,
-            "velocityY": 1.7801852141837777,
-            "timestamp": 0.5576342349172615
-        },
-        {
-            "x": 3.2127166120567576,
-            "y": 5.42406293198425,
-            "heading": 1.6557539162164756,
-            "angularVelocity": -2.5171587847273322,
-            "velocityX": 2.536795910750906,
-            "velocityY": 1.231651953643812,
-            "timestamp": 0.6273385142819192
-        },
-        {
-            "x": 3.3872389413542425,
-            "y": 5.4708692312967075,
-            "heading": 1.441472502737752,
-            "angularVelocity": -3.0741500440412146,
-            "velocityX": 2.503753440796252,
-            "velocityY": 0.6714982170261123,
-            "timestamp": 0.697042793646577
+            "x": 2.312764146242865,
+            "y": 4.398513079462153,
+            "heading": -0.39804543849617763,
+            "angularVelocity": -0.11524733034669515,
+            "velocityX": 0.18284072142228638,
+            "velocityY": 0.5516827447028667,
+            "timestamp": 0.06981019405093988
+        },
+        {
+            "x": 2.339895425931373,
+            "y": 4.474992910905513,
+            "heading": -0.4126392714825622,
+            "angularVelocity": -0.20905017072795415,
+            "velocityX": 0.3886435220157258,
+            "velocityY": 1.0955395910772,
+            "timestamp": 0.13962038810187977
+        },
+        {
+            "x": 2.3838571031189573,
+            "y": 4.588440455462814,
+            "heading": -0.43142102712692415,
+            "angularVelocity": -0.2690403013169746,
+            "velocityX": 0.6297314852832604,
+            "velocityY": 1.6250856497386854,
+            "timestamp": 0.20943058215281965
+        },
+        {
+            "x": 2.448821264845437,
+            "y": 4.736675156040894,
+            "heading": -0.45019318323311186,
+            "angularVelocity": -0.26890279219946406,
+            "velocityX": 0.9305827409539079,
+            "velocityY": 2.1233961972720627,
+            "timestamp": 0.27924077620375953
+        },
+        {
+            "x": 2.542623225560298,
+            "y": 4.913429983875953,
+            "heading": -0.4600490129319591,
+            "angularVelocity": -0.14118037962391586,
+            "velocityX": 1.343671393412285,
+            "velocityY": 2.5319343433823196,
+            "timestamp": 0.34905097025469944
+        },
+        {
+            "x": 2.675927389267578,
+            "y": 5.095797559489528,
+            "heading": -0.44245218860567326,
+            "angularVelocity": 0.25206668688824874,
+            "velocityX": 1.9095228930350086,
+            "velocityY": 2.6123344605070336,
+            "timestamp": 0.41886116430563936
+        },
+        {
+            "x": 2.83772713712192,
+            "y": 5.250967901017311,
+            "heading": -0.38879601501351296,
+            "angularVelocity": 0.7686008371275667,
+            "velocityX": 2.3177094699953265,
+            "velocityY": 2.2227461710524636,
+            "timestamp": 0.48867135835657927
+        },
+        {
+            "x": 3.013235940099558,
+            "y": 5.368218424760011,
+            "heading": -0.31718431053283597,
+            "angularVelocity": 1.0258058360734288,
+            "velocityX": 2.514085591121856,
+            "velocityY": 1.6795616361728827,
+            "timestamp": 0.5584815524075192
+        },
+        {
+            "x": 3.1945050869972476,
+            "y": 5.445323486359522,
+            "heading": -0.23653218942173773,
+            "angularVelocity": 1.1553057859494003,
+            "velocityX": 2.5965999573717062,
+            "velocityY": 1.1044957351381008,
+            "timestamp": 0.6282917464584591
+        },
+        {
+            "x": 3.377225953863421,
+            "y": 5.481834951968023,
+            "heading": -0.15180568644047004,
+            "angularVelocity": 1.2136694952369833,
+            "velocityX": 2.6173952006491676,
+            "velocityY": 0.523010515923758,
+            "timestamp": 0.698101940509399
         },
         {
             "x": 3.558790922164917,
             "y": 5.477728366851807,
-            "heading": 1.205442867585498,
-            "angularVelocity": -3.386157023695285,
-            "velocityX": 2.461139866509503,
-            "velocityY": 0.09840336371911824,
-            "timestamp": 0.7667470730112347
-        },
-        {
-            "x": 3.6181705627536003,
-            "y": 5.4752230037777725,
-            "heading": 1.1222490393160696,
-            "angularVelocity": -3.4253809174942864,
-            "velocityX": 2.44486750990022,
-            "velocityY": -0.10315456138644599,
-            "timestamp": 0.7910345406749146
-        },
-        {
-            "x": 3.6770128805090945,
-            "y": 5.467829320967843,
-            "heading": 1.0387507075885118,
-            "angularVelocity": -3.4379183900025705,
-            "velocityX": 2.4227440493307753,
-            "velocityY": -0.3044237839989221,
-            "timestamp": 0.8153220083385946
-        },
-        {
-            "x": 3.735181283810321,
-            "y": 5.455564580406075,
-            "heading": 0.9556144237478981,
-            "angularVelocity": -3.4230116120725995,
-            "velocityX": 2.394996633931211,
-            "velocityY": -0.5049822703463023,
-            "timestamp": 0.8396094760022745
-        },
-        {
-            "x": 3.7925490950380945,
-            "y": 5.438455755513165,
-            "heading": 0.8735217241735449,
-            "angularVelocity": -3.380043597427691,
-            "velocityX": 2.362033457838118,
-            "velocityY": -0.7044301666120085,
-            "timestamp": 0.8638969436659545
-        },
-        {
-            "x": 3.8490034281899477,
-            "y": 5.416538531692629,
-            "heading": 0.7931634356424634,
-            "angularVelocity": -3.308631827897467,
-            "velocityX": 2.3244223701541378,
-            "velocityY": -0.902408769989304,
-            "timestamp": 0.8881844113296344
-        },
-        {
-            "x": 3.9044487728906376,
-            "y": 5.389855903636327,
-            "heading": 0.7152329727062241,
-            "angularVelocity": -3.2086697557513846,
-            "velocityX": 2.282878786231156,
-            "velocityY": -1.0986171315094786,
-            "timestamp": 0.9124718789933144
-        },
-        {
-            "x": 3.958810912211869,
-            "y": 5.358456406059338,
-            "heading": 0.6404209450438546,
-            "angularVelocity": -3.080272867403366,
-            "velocityX": 2.2382794317633223,
-            "velocityY": -1.2928271490380632,
-            "timestamp": 0.9367593466569943
-        },
-        {
-            "x": 4.012042256408639,
-            "y": 5.322391998548557,
-            "heading": 0.5694141014307214,
-            "angularVelocity": -2.923600129762354,
-            "velocityX": 2.1917206410273016,
-            "velocityY": -1.4848978086221774,
-            "timestamp": 0.9610468143206743
-        },
-        {
-            "x": 4.064130365340956,
-            "y": 5.281715631109981,
-            "heading": 0.5029022798709881,
-            "angularVelocity": -2.738524348472814,
-            "velocityX": 2.1446496462128213,
-            "velocityY": -1.6747883312431175,
-            "timestamp": 0.9853342819843542
-        },
-        {
-            "x": 4.1151123432623455,
-            "y": 5.236478617606072,
-            "heading": 0.44159733021441666,
-            "angularVelocity": -2.524139218855187,
-            "velocityX": 2.09910636330477,
-            "velocityY": -1.8625660826533157,
-            "timestamp": 1.0096217496480342
-        },
-        {
-            "x": 4.165095928896697,
-            "y": 5.186728422848365,
-            "heading": 0.38626165615570274,
-            "angularVelocity": -2.278363262278863,
-            "velocityX": 2.057999060523636,
-            "velocityY": -2.0483895417431537,
-            "timestamp": 1.0339092173117141
-        },
-        {
-            "x": 4.214269146284126,
-            "y": 5.132508462649398,
-            "heading": 0.33770705635311316,
-            "angularVelocity": -1.9991627153126146,
-            "velocityX": 2.0246333651722517,
-            "velocityY": -2.232425419964549,
-            "timestamp": 1.058196684975394
-        },
-        {
-            "x": 4.262857394718322,
-            "y": 5.073859999949952,
-            "heading": 0.2966997330272638,
-            "angularVelocity": -1.6884149427884858,
-            "velocityX": 2.000548147177036,
-            "velocityY": -2.4147623585784754,
-            "timestamp": 1.082484152639074
-        },
-        {
-            "x": 4.311032470035773,
-            "y": 5.010821065356263,
-            "heading": 0.26381895203232264,
-            "angularVelocity": -1.353816768806731,
-            "velocityX": 1.9835363647027058,
-            "velocityY": -2.59553344410457,
-            "timestamp": 1.106771620302754
-        },
-        {
-            "x": 4.358621953046075,
-            "y": 4.943414538605821,
-            "heading": 0.23906687218938943,
-            "angularVelocity": -1.019129708608845,
-            "velocityX": 1.9594254810463076,
-            "velocityY": -2.77536249080602,
-            "timestamp": 1.131059087966434
-        },
-        {
-            "x": 4.401785346523604,
-            "y": 4.873960680394997,
-            "heading": 0.21204412295947933,
-            "angularVelocity": -1.1126211099529368,
-            "velocityX": 1.7771878927533025,
-            "velocityY": -2.8596582884878865,
-            "timestamp": 1.155346555630114
-        },
-        {
-            "x": 4.442527933324705,
-            "y": 4.807784869243466,
-            "heading": 0.1747999499890042,
-            "angularVelocity": -1.5334728793554109,
-            "velocityX": 1.6775148140301646,
-            "velocityY": -2.7246896246203156,
-            "timestamp": 1.1796340232937939
-        },
-        {
-            "x": 4.4813722294916225,
-            "y": 4.745319922619003,
-            "heading": 0.12759770745686402,
-            "angularVelocity": -1.9434814360134838,
-            "velocityX": 1.5993555484998627,
-            "velocityY": -2.571900351631775,
-            "timestamp": 1.2039214909574738
-        },
-        {
-            "x": 4.518612325734112,
-            "y": 4.686765196550011,
-            "heading": 0.07078885768669944,
-            "angularVelocity": -2.3390190594105262,
-            "velocityX": 1.5333050262040608,
-            "velocityY": -2.410902893616876,
-            "timestamp": 1.2282089586211538
-        },
-        {
-            "x": 4.554475342160425,
-            "y": 4.632242142038935,
-            "heading": 0.004632855038261056,
-            "angularVelocity": -2.723874039258911,
-            "velocityX": 1.47660583321928,
-            "velocityY": -2.244904873002102,
-            "timestamp": 1.2524964262848337
-        },
-        {
-            "x": 4.589156761961767,
-            "y": 4.581819399142938,
-            "heading": -0.07076666263842435,
-            "angularVelocity": -3.1044618863019378,
-            "velocityX": 1.4279553670061842,
-            "velocityY": -2.076080701134665,
-            "timestamp": 1.2767838939485137
-        },
-        {
-            "x": 4.6228372561227475,
-            "y": 4.535518314545829,
-            "heading": -0.1555015238515939,
-            "angularVelocity": -3.488830634240387,
-            "velocityX": 1.3867437572072365,
-            "velocityY": -1.9063776116251314,
-            "timestamp": 1.3010713616121936
-        },
-        {
-            "x": 4.654626762222228,
-            "y": 4.492711334026638,
-            "heading": -0.25016165034544124,
-            "angularVelocity": -3.8974885239025583,
-            "velocityX": 1.3088851641383707,
-            "velocityY": -1.762513124544663,
-            "timestamp": 1.3253588292758736
-        },
-        {
-            "x": 4.685559266058307,
-            "y": 4.453815281857176,
-            "heading": -0.35503604485130436,
-            "angularVelocity": -4.318045666930297,
-            "velocityX": 1.2735993832052854,
-            "velocityY": -1.6014865241643912,
-            "timestamp": 1.3496462969395535
-        },
-        {
-            "x": 4.716254412162635,
-            "y": 4.418834840133045,
-            "heading": -0.4707020428721612,
-            "angularVelocity": -4.762373732104905,
-            "velocityX": 1.2638265351241147,
-            "velocityY": -1.440267145530449,
-            "timestamp": 1.3739337646032335
-        },
-        {
-            "x": 4.747074366156251,
-            "y": 4.387541660715592,
-            "heading": -0.5981615568154537,
-            "angularVelocity": -5.247954035730924,
-            "velocityX": 1.268965312497503,
-            "velocityY": -1.2884496585146104,
-            "timestamp": 1.3982212322669134
-        },
-        {
-            "x": 4.7760218641280225,
-            "y": 4.359163482704458,
-            "heading": -0.7400182505648043,
-            "angularVelocity": -5.840736288925144,
-            "velocityX": 1.191869748325327,
-            "velocityY": -1.1684288540946337,
-            "timestamp": 1.4225086999305934
-        },
-        {
-            "x": 4.800681197551656,
-            "y": 4.328735146997818,
-            "heading": -0.8864907082583147,
-            "angularVelocity": -6.030783436206011,
-            "velocityX": 1.015311014104143,
-            "velocityY": -1.2528410177625973,
-            "timestamp": 1.4467961675942733
-        },
-        {
-            "x": 4.821257943768876,
-            "y": 4.295565486099031,
-            "heading": -1.0326334280053908,
-            "angularVelocity": -6.017206971545368,
-            "velocityX": 0.8472166181404436,
-            "velocityY": -1.3657109649348727,
-            "timestamp": 1.4710836352579533
-        },
-        {
-            "x": 4.842407281192705,
-            "y": 4.263513715444916,
-            "heading": -1.1735548575556736,
-            "angularVelocity": -5.802228190344444,
-            "velocityX": 0.8707922010105955,
-            "velocityY": -1.319683513240253,
-            "timestamp": 1.4953711029216332
-        },
-        {
-            "x": 4.867138629271623,
-            "y": 4.234341103924059,
-            "heading": -1.3080892458596534,
-            "angularVelocity": -5.539251360699312,
-            "velocityX": 1.0182761093657138,
-            "velocityY": -1.2011384605766136,
-            "timestamp": 1.5196585705853132
-        },
-        {
-            "x": 4.89576478236693,
-            "y": 4.207846540583478,
-            "heading": -1.4377333776779964,
-            "angularVelocity": -5.337902395325306,
-            "velocityX": 1.1786388557138734,
-            "velocityY": -1.0908738493227768,
-            "timestamp": 1.5439460382489931
-        },
-        {
-            "x": 4.928534232838641,
-            "y": 4.183793595288781,
-            "heading": -1.5636936940682342,
-            "angularVelocity": -5.1862268283573245,
-            "velocityX": 1.3492329017369804,
-            "velocityY": -0.9903438937220402,
-            "timestamp": 1.568233505912673
-        },
-        {
-            "x": 4.965642273050373,
-            "y": 4.1619364014492675,
-            "heading": -1.6870176099857392,
-            "angularVelocity": -5.077676998904523,
-            "velocityX": 1.5278678174928029,
-            "velocityY": -0.8999371256888635,
-            "timestamp": 1.592520973576353
-        },
-        {
-            "x": 5.007242132688007,
-            "y": 4.142028453113074,
-            "heading": -1.8086864696957439,
-            "angularVelocity": -5.009532545542041,
-            "velocityX": 1.712811735405568,
-            "velocityY": -0.8196798699587677,
-            "timestamp": 1.616808441240033
-        },
-        {
-            "x": 5.0534540304279405,
-            "y": 4.123819388018015,
-            "heading": -1.9296932957732547,
-            "angularVelocity": -4.982274305133403,
-            "velocityX": 1.902705476744277,
-            "velocityY": -0.749730904316916,
-            "timestamp": 1.641095908903713
-        },
-        {
-            "x": 5.102543724383187,
-            "y": 4.108450898288225,
-            "heading": -2.039853637710442,
-            "angularVelocity": -4.535686612644417,
-            "velocityX": 2.0211944133087334,
-            "velocityY": -0.6327744803453944,
-            "timestamp": 1.6653833765673929
-        },
-        {
-            "x": 5.15444764406018,
-            "y": 4.096180097409251,
-            "heading": -2.1398530735226235,
-            "angularVelocity": -4.117326565161934,
-            "velocityX": 2.1370659302868042,
-            "velocityY": -0.5052317948043931,
-            "timestamp": 1.6896708442310728
-        },
-        {
-            "x": 5.20922556245635,
-            "y": 4.087205112082737,
-            "heading": -2.231114838342211,
-            "angularVelocity": -3.757566086482643,
-            "velocityX": 2.2553985106519345,
-            "velocityY": -0.36953153991984866,
-            "timestamp": 1.7139583118947528
-        },
-        {
-            "x": 5.266890046930524,
-            "y": 4.081668558801882,
-            "heading": -2.314693981916533,
-            "angularVelocity": -3.4412456964094225,
-            "velocityX": 2.3742485331398995,
-            "velocityY": -0.22795926514540626,
-            "timestamp": 1.7382457795584327
-        },
-        {
-            "x": 5.327424168986788,
-            "y": 4.079676285408984,
-            "heading": -2.3913241555270592,
-            "angularVelocity": -3.1551322958679826,
-            "velocityX": 2.492401550236051,
-            "velocityY": -0.08202886445327343,
-            "timestamp": 1.7625332472221127
+            "heading": -0.06601521216801902,
+            "angularVelocity": 1.2289104110775417,
+            "velocityX": 2.6008374674955865,
+            "velocityY": -0.05882500648124433,
+            "timestamp": 0.7679121345603389
+        },
+        {
+            "x": 3.7207755893743903,
+            "y": 5.440802283745667,
+            "heading": 0.01087618717506099,
+            "angularVelocity": 1.2168377614801031,
+            "velocityX": 2.5634734381227786,
+            "velocityY": -0.5843703286511746,
+            "timestamp": 0.8311016588990232
+        },
+        {
+            "x": 3.8800031263189014,
+            "y": 5.370704697701765,
+            "heading": 0.08523277253593173,
+            "angularVelocity": 1.1767232961398173,
+            "velocityX": 2.519840726939224,
+            "velocityY": -1.1093228945227975,
+            "timestamp": 0.8942911832377075
+        },
+        {
+            "x": 4.035880089753818,
+            "y": 5.267514783368049,
+            "heading": 0.15438353932229767,
+            "angularVelocity": 1.0943390935099226,
+            "velocityX": 2.4668165343191313,
+            "velocityY": -1.633022489291642,
+            "timestamp": 0.9574807075763918
+        },
+        {
+            "x": 4.187424190703861,
+            "y": 5.131425904894565,
+            "heading": 0.2138534475851711,
+            "angularVelocity": 0.9411355581283213,
+            "velocityX": 2.3982472179488545,
+            "velocityY": -2.153662017503815,
+            "timestamp": 1.0206702319150762
+        },
+        {
+            "x": 4.332733928007212,
+            "y": 4.96304651892829,
+            "heading": 0.25476003945729764,
+            "angularVelocity": 0.6473635034690916,
+            "velocityX": 2.2995858699928062,
+            "velocityY": -2.664672471059999,
+            "timestamp": 1.0838597562537606
+        },
+        {
+            "x": 4.46716744223408,
+            "y": 4.765388489141315,
+            "heading": 0.2547605945579538,
+            "angularVelocity": 0.000008784690993212257,
+            "velocityX": 2.12746520302083,
+            "velocityY": -3.1280189533989655,
+            "timestamp": 1.147049280592445
+        },
+        {
+            "x": 4.612743650855858,
+            "y": 4.575786978724626,
+            "heading": 0.2547605407972418,
+            "angularVelocity": -8.507852266556338e-7,
+            "velocityX": 2.303802887345736,
+            "velocityY": -3.0005212478052368,
+            "timestamp": 1.2102388049311295
+        },
+        {
+            "x": 4.782527461386278,
+            "y": 4.409030186652972,
+            "heading": 0.25208197397278675,
+            "angularVelocity": -0.04238941267495421,
+            "velocityX": 2.6868980627285057,
+            "velocityY": -2.638994260793031,
+            "timestamp": 1.2734283292698139
+        },
+        {
+            "x": 4.970403968588328,
+            "y": 4.270066434839271,
+            "heading": 0.23891743895373094,
+            "angularVelocity": -0.2083341369570075,
+            "velocityX": 2.9732223682415455,
+            "velocityY": -2.1991580609051504,
+            "timestamp": 1.3366178536084983
+        },
+        {
+            "x": 5.173968377386639,
+            "y": 4.1604195633547185,
+            "heading": 0.21971651393307054,
+            "angularVelocity": -0.3038624711945638,
+            "velocityX": 3.2214898106793903,
+            "velocityY": -1.7352064702582655,
+            "timestamp": 1.3998073779471827
         },
         {
             "x": 5.390789031982422,
             "y": 4.081310272216797,
-            "heading": -2.461469590575517,
-            "angularVelocity": -2.888132925992722,
-            "velocityX": 2.608953056493045,
-            "velocityY": 0.06727695247771216,
-            "timestamp": 1.7868207148857926
-        },
-        {
-            "x": 5.47852793656002,
-            "y": 4.0897392429132315,
-            "heading": -2.541079365075053,
-            "angularVelocity": -2.4976756873239485,
-            "velocityX": 2.752718873700339,
-            "velocityY": 0.2644503807478311,
-            "timestamp": 1.8186942583180192
-        },
-        {
-            "x": 5.570818505990881,
-            "y": 4.101985641066581,
-            "heading": -2.595195903422616,
-            "angularVelocity": -1.6978513375091853,
-            "velocityX": 2.8955227280299005,
-            "velocityY": 0.384218283711974,
-            "timestamp": 1.8505678017502458
-        },
-        {
-            "x": 5.666172128712358,
-            "y": 4.1161647975520586,
-            "heading": -2.6133567142303673,
-            "angularVelocity": -0.5697769639691046,
-            "velocityX": 2.9916229089567223,
-            "velocityY": 0.4448566101734819,
-            "timestamp": 1.8824413451824724
-        },
-        {
-            "x": 5.762848661651843,
-            "y": 4.1299008615103485,
-            "heading": -2.5899687129898252,
-            "angularVelocity": 0.7337747461392797,
-            "velocityX": 3.033127871240633,
-            "velocityY": 0.43095503289420706,
-            "timestamp": 1.914314888614699
-        },
-        {
-            "x": 5.85803719097752,
-            "y": 4.141633583817035,
-            "heading": -2.5274756720169904,
-            "angularVelocity": 1.960655585900415,
-            "velocityX": 2.9864432716142737,
-            "velocityY": 0.36810222658906844,
-            "timestamp": 1.9461884320469256
-        },
-        {
-            "x": 5.9473700887935115,
-            "y": 4.148469896281193,
-            "heading": -2.4508224753320094,
-            "angularVelocity": 2.404916066140258,
-            "velocityX": 2.8027287899740645,
-            "velocityY": 0.21448234893285298,
-            "timestamp": 1.9780619754791522
-        },
-        {
-            "x": 6.0307522620060645,
-            "y": 4.150565536032173,
-            "heading": -2.3598343948212093,
-            "angularVelocity": 2.854658463194389,
-            "velocityX": 2.6160308592563855,
-            "velocityY": 0.06574856527754983,
-            "timestamp": 2.009935518911379
-        },
-        {
-            "x": 6.108380396897077,
-            "y": 4.1477533333139265,
-            "heading": -2.2542021077222087,
-            "angularVelocity": 3.3141055472419727,
-            "velocityX": 2.4355037605428165,
-            "velocityY": -0.0882299994108409,
-            "timestamp": 2.0418090623436056
-        },
-        {
-            "x": 6.180764914708381,
-            "y": 4.139596420595998,
-            "heading": -2.1333489972804904,
-            "angularVelocity": 3.7916433953661524,
-            "velocityX": 2.270990609036518,
-            "velocityY": -0.2559148384387137,
-            "timestamp": 2.0736826057758324
-        },
-        {
-            "x": 6.2487782508806635,
-            "y": 4.1255186451287145,
-            "heading": -1.9962477740611584,
-            "angularVelocity": 4.301411404441215,
-            "velocityX": 2.133849231946881,
-            "velocityY": -0.4416758838633167,
-            "timestamp": 2.1055561492080592
-        },
-        {
-            "x": 6.318999325259612,
-            "y": 4.108403885933248,
-            "heading": -1.8570700146176022,
-            "angularVelocity": 4.366560616000945,
-            "velocityX": 2.2031147722330044,
-            "velocityY": -0.5369581587895442,
-            "timestamp": 2.137429692640286
-        },
-        {
-            "x": 6.393124517437766,
-            "y": 4.09868995620756,
-            "heading": -1.7226225348946564,
-            "angularVelocity": 4.218152901914545,
-            "velocityX": 2.3256024964958177,
-            "velocityY": -0.3047646630925519,
-            "timestamp": 2.169303236072513
-        },
-        {
-            "x": 6.468812945370671,
-            "y": 4.097296283093324,
-            "heading": -1.5892075925845424,
-            "angularVelocity": 4.185758090994726,
-            "velocityX": 2.3746474280101166,
-            "velocityY": -0.04372507616542974,
-            "timestamp": 2.2011767795047397
-        },
-        {
-            "x": 6.543776258333572,
-            "y": 4.104341851159788,
-            "heading": -1.4561024343589095,
-            "angularVelocity": 4.176038930489708,
-            "velocityX": 2.351897683490888,
-            "velocityY": 0.22104753057800555,
-            "timestamp": 2.2330503229369665
-        },
-        {
-            "x": 6.614153245906366,
-            "y": 4.118263151934315,
-            "heading": -1.3162963129525131,
-            "angularVelocity": 4.386274833347886,
-            "velocityX": 2.208006390078291,
-            "velocityY": 0.43676664956088845,
-            "timestamp": 2.2649238663691933
-        },
-        {
-            "x": 6.676725935567768,
-            "y": 4.132111873679476,
-            "heading": -1.164406226014857,
-            "angularVelocity": 4.765396958785676,
-            "velocityX": 1.9631544824770328,
-            "velocityY": 0.4344895563497056,
-            "timestamp": 2.29679740980142
-        },
-        {
-            "x": 6.732923820729643,
-            "y": 4.140964584087909,
-            "heading": -1.0033332436559739,
-            "angularVelocity": 5.053500960800803,
-            "velocityX": 1.7631514764390552,
-            "velocityY": 0.27774478313828127,
-            "timestamp": 2.328670953233647
-        },
-        {
-            "x": 6.785034236650818,
-            "y": 4.142631098888223,
-            "heading": -0.8355937264762496,
-            "angularVelocity": 5.262656708890626,
-            "velocityX": 1.6349112872241036,
-            "velocityY": 0.0522852065023047,
-            "timestamp": 2.3605444966658737
-        },
-        {
-            "x": 6.835668705020557,
-            "y": 4.136059975961784,
-            "heading": -0.6631765444972689,
-            "angularVelocity": 5.409413683345085,
-            "velocityX": 1.5886049342899025,
-            "velocityY": -0.2061622969661811,
-            "timestamp": 2.3924180400981006
-        },
-        {
-            "x": 6.8889189304898775,
-            "y": 4.121434248751472,
-            "heading": -0.4913644813841438,
-            "angularVelocity": 5.390428694520651,
-            "velocityX": 1.6706716522606933,
-            "velocityY": -0.45886731236551676,
-            "timestamp": 2.4242915835303274
-        },
-        {
-            "x": 6.949941444507073,
-            "y": 4.105368374325079,
-            "heading": -0.330980540513716,
-            "angularVelocity": 5.031882985067382,
-            "velocityX": 1.9145192986449624,
-            "velocityY": -0.5040504661979253,
-            "timestamp": 2.456165126962554
-        },
-        {
-            "x": 7.016508340627581,
-            "y": 4.095339734393401,
-            "heading": -0.17821539872753786,
-            "angularVelocity": 4.792850914458417,
-            "velocityX": 2.0884686467963878,
-            "velocityY": -0.31463837564850267,
-            "timestamp": 2.488038670394781
-        },
-        {
-            "x": 7.085360516906702,
-            "y": 4.093448303803928,
-            "heading": -0.02746406984555507,
-            "angularVelocity": 4.7296695832557045,
-            "velocityX": 2.1601669869408497,
-            "velocityY": -0.05934171057873482,
-            "timestamp": 2.519912213827008
-        },
-        {
-            "x": 7.153846633279139,
-            "y": 4.100009116269512,
-            "heading": 0.121772089508303,
-            "angularVelocity": 4.682132680703696,
-            "velocityX": 2.1486822297640367,
-            "velocityY": 0.20583881674575163,
-            "timestamp": 2.5517857572592346
-        },
-        {
-            "x": 7.2178667451999,
-            "y": 4.113588367220929,
-            "heading": 0.27676556654047163,
-            "angularVelocity": 4.862762665899784,
-            "velocityX": 2.008565883391336,
-            "velocityY": 0.4260351843305824,
-            "timestamp": 2.5836593006914614
-        },
-        {
-            "x": 7.274040071387795,
-            "y": 4.126189441281968,
-            "heading": 0.44297350868055696,
-            "angularVelocity": 5.214605100104267,
-            "velocityX": 1.7623809636144747,
-            "velocityY": 0.3953458795013916,
-            "timestamp": 2.6155328441236882
-        },
-        {
-            "x": 7.324153742240104,
-            "y": 4.133335840122352,
-            "heading": 0.617641408977622,
-            "angularVelocity": 5.480027680902935,
-            "velocityX": 1.5722654419916315,
-            "velocityY": 0.22421099353388252,
-            "timestamp": 2.647406387555915
-        },
-        {
-            "x": 7.370062418422902,
-            "y": 4.133468677579737,
-            "heading": 0.8003841200365157,
-            "angularVelocity": 5.733366653991959,
-            "velocityX": 1.440338012007226,
-            "velocityY": 0.00416764008895702,
-            "timestamp": 2.679279930988142
-        },
-        {
-            "x": 7.413378960747577,
-            "y": 4.125978597308371,
-            "heading": 0.9928213817572759,
-            "angularVelocity": 6.037523318671553,
-            "velocityX": 1.3590124492050755,
-            "velocityY": -0.2349936488013014,
-            "timestamp": 2.7111534744203687
-        },
-        {
-            "x": 7.454276397546973,
-            "y": 4.11151825545799,
-            "heading": 1.2015436063625509,
-            "angularVelocity": 6.548447462362883,
-            "velocityX": 1.2831154743229476,
-            "velocityY": -0.4536785149454069,
-            "timestamp": 2.7430270178525955
-        },
-        {
-            "x": 7.496754891759898,
-            "y": 4.105364802951648,
-            "heading": 1.412047118709339,
-            "angularVelocity": 6.604333553135948,
-            "velocityX": 1.3327195422513116,
-            "velocityY": -0.19305831243477098,
-            "timestamp": 2.7749005612848223
-        },
-        {
-            "x": 7.53888503669894,
-            "y": 4.1076484340015265,
-            "heading": 1.6249494045277855,
-            "angularVelocity": 6.679592630519619,
-            "velocityX": 1.3217904381614993,
-            "velocityY": 0.07164660103553838,
-            "timestamp": 2.806774104717049
-        },
-        {
-            "x": 7.576001000454075,
-            "y": 4.116500503183424,
-            "heading": 1.8445622032463762,
-            "angularVelocity": 6.890128146108359,
-            "velocityX": 1.1644756044791265,
-            "velocityY": 0.277724665308084,
-            "timestamp": 2.838647648149276
-        },
-        {
-            "x": 7.60515186851165,
-            "y": 4.124308875688622,
-            "heading": 2.0739714188941045,
-            "angularVelocity": 7.1974807612943055,
-            "velocityX": 0.9145788299176564,
-            "velocityY": 0.24497974383681875,
-            "timestamp": 2.8705211915815028
-        },
-        {
-            "x": 7.62773108442525,
-            "y": 4.127593662867362,
-            "heading": 2.3140115714104335,
-            "angularVelocity": 7.53101559061771,
-            "velocityX": 0.708399929289633,
-            "velocityY": 0.10305685609521825,
-            "timestamp": 2.9023947350137296
-        },
-        {
-            "x": 7.642808233685486,
-            "y": 4.127168273515703,
-            "heading": 2.549384793893863,
-            "angularVelocity": 7.384595408537156,
-            "velocityX": 0.473030219946994,
-            "velocityY": -0.013346158156635379,
-            "timestamp": 2.9342682784459564
-        },
-        {
-            "x": 7.655297430086512,
-            "y": 4.12694395453313,
-            "heading": 2.7459505912038105,
-            "angularVelocity": 6.167051922793274,
-            "velocityX": 0.39183583173240866,
-            "velocityY": -0.007037779876922506,
-            "timestamp": 2.966141821878183
-        },
-        {
-            "x": 7.665239437445753,
-            "y": 4.126897699920467,
-            "heading": 2.903400180799407,
-            "angularVelocity": 4.939820698956328,
-            "velocityX": 0.31192036682023955,
-            "velocityY": -0.001451191417126478,
-            "timestamp": 2.99801536531041
-        },
-        {
-            "x": 7.672649475189336,
-            "y": 4.12693483442264,
-            "heading": 3.0216231319761504,
-            "angularVelocity": 3.7091248241076054,
-            "velocityX": 0.23248239591994224,
-            "velocityY": 0.001165057228464201,
-            "timestamp": 3.029888908742637
-        },
-        {
-            "x": 7.677558550585934,
-            "y": 4.1269805608298435,
-            "heading": 3.1005189939079423,
-            "angularVelocity": 2.4752774067793823,
-            "velocityX": 0.15401724653037796,
-            "velocityY": 0.0014346195082003044,
-            "timestamp": 3.0617624521748636
+            "heading": 0.19899217158132915,
+            "angularVelocity": -0.3279711719205329,
+            "velocityX": 3.4312753081359326,
+            "velocityY": -1.25193680386463,
+            "timestamp": 1.462996902285867
+        },
+        {
+            "x": 5.681530534379024,
+            "y": 4.031634954440038,
+            "heading": 0.1807325834147537,
+            "angularVelocity": -0.22892527550150313,
+            "velocityX": 3.645102941474752,
+            "velocityY": -0.6227925681552634,
+            "timestamp": 1.542759122191511
+        },
+        {
+            "x": 5.983259555492767,
+            "y": 4.033604453850848,
+            "heading": 0.1807316659794543,
+            "angularVelocity": -0.000011502133581259988,
+            "velocityX": 3.7828563632073102,
+            "velocityY": 0.024692133859281144,
+            "timestamp": 1.6225213420971547
+        },
+        {
+            "x": 6.284539191755923,
+            "y": 4.050188554583329,
+            "heading": 0.18073165818528908,
+            "angularVelocity": -9.771750653524246e-8,
+            "velocityX": 3.777222306745724,
+            "velocityY": 0.20791924738433482,
+            "timestamp": 1.7022835620027985
+        },
+        {
+            "x": 6.585818825161056,
+            "y": 4.0667727072366,
+            "heading": 0.18073165039112304,
+            "angularVelocity": -9.77175162921636e-8,
+            "velocityX": 3.777222270913928,
+            "velocityY": 0.20791989832899174,
+            "timestamp": 1.7820457819084423
+        },
+        {
+            "x": 6.88709845856161,
+            "y": 4.083356859971397,
+            "heading": 0.180731642596831,
+            "angularVelocity": -9.771909620772691e-8,
+            "velocityX": 3.777222270856537,
+            "velocityY": 0.20791989935110283,
+            "timestamp": 1.8618080018140861
+        },
+        {
+            "x": 7.15139892307148,
+            "y": 4.097904823415219,
+            "heading": 0.12048670978727856,
+            "angularVelocity": -0.7553066211144209,
+            "velocityX": 3.313604671767403,
+            "velocityY": 0.1823916568650699,
+            "timestamp": 1.94157022171973
+        },
+        {
+            "x": 7.3628393316310445,
+            "y": 4.109542993215659,
+            "heading": 0.07229126938267637,
+            "angularVelocity": -0.6042389550038586,
+            "velocityX": 2.650884200686756,
+            "velocityY": 0.1459108060709806,
+            "timestamp": 2.0213324416253737
+        },
+        {
+            "x": 7.521419659615098,
+            "y": 4.118271521022388,
+            "heading": 0.03614542857914898,
+            "angularVelocity": -0.4531699449471921,
+            "velocityX": 1.98816342087339,
+            "velocityY": 0.10943185654883955,
+            "timestamp": 2.1010946615310178
+        },
+        {
+            "x": 7.627139886542953,
+            "y": 4.12409050689369,
+            "heading": 0.012048510432709781,
+            "angularVelocity": -0.302109421915929,
+            "velocityX": 1.3254423842882344,
+            "velocityY": 0.07295416148382058,
+            "timestamp": 2.180856881436662
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 1.238676401825463,
-            "velocityX": 0.07659799166218657,
-            "velocityY": 0.0006098841880359634,
-            "timestamp": 3.0936359956070905
+            "heading": 2.791295127527068e-32,
+            "angularVelocity": -0.15105535486544666,
+            "velocityX": 0.6627211920578459,
+            "velocityY": 0.03647708288149429,
+            "timestamp": 2.260619101342306
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": -2.349124968959552e-31,
-            "velocityX": 6.522838948908763e-32,
-            "velocityY": -6.176555916296272e-32,
-            "timestamp": 3.1255095390393173
+            "heading": 0,
+            "angularVelocity": -1.938584239537671e-30,
+            "velocityX": -1.689352589413745e-32,
+            "velocityY": 0,
+            "timestamp": 2.34038132124795
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.traj
index af794028..ea265506 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.traj	
@@ -3,866 +3,308 @@
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
+            "heading": -0.39,
             "angularVelocity": 0,
-            "velocityX": -5.720774693888146e-34,
-            "velocityY": 0,
+            "velocityX": 0,
+            "velocityY": 1.9823938961383557e-29,
             "timestamp": 0
         },
         {
-            "x": 2.3121576423431627,
-            "y": 4.3971322791414,
-            "heading": 2.7106310928070285,
-            "angularVelocity": -0.5647989987388474,
-            "velocityX": 0.1744174454420521,
-            "velocityY": 0.5327116136893348,
-            "timestamp": 0.06970427936465769
-        },
-        {
-            "x": 2.3388751368329244,
-            "y": 4.470343590887415,
-            "heading": 2.629622228062893,
-            "angularVelocity": -1.1621792160039144,
-            "velocityX": 0.38329776497637447,
-            "velocityY": 1.0503130139687884,
-            "timestamp": 0.13940855872931537
-        },
-        {
-            "x": 2.3840539747737246,
-            "y": 4.577503337755321,
-            "heading": 2.5035786127839184,
-            "angularVelocity": -1.8082622247563562,
-            "velocityX": 0.6481501329989676,
-            "velocityY": 1.5373481778256302,
-            "timestamp": 0.20911283809397307
-        },
-        {
-            "x": 2.4545399331182174,
-            "y": 4.713290189810374,
-            "heading": 2.3271901249556994,
-            "angularVelocity": -2.5305259510028497,
-            "velocityX": 1.011214217935542,
-            "velocityY": 1.948041831760208,
-            "timestamp": 0.27881711745863075
-        },
-        {
-            "x": 2.555797664071962,
-            "y": 4.87530128954077,
-            "heading": 2.141593101929859,
-            "angularVelocity": -2.662634557268572,
-            "velocityX": 1.4526759601661592,
-            "velocityY": 2.3242633193700435,
-            "timestamp": 0.3485213968232884
-        },
-        {
-            "x": 2.6889975632138237,
-            "y": 5.05237912689222,
-            "heading": 2.0338949096526036,
-            "angularVelocity": -1.545072888765306,
-            "velocityX": 1.910928573624968,
-            "velocityY": 2.54041558087227,
-            "timestamp": 0.4182256761879461
-        },
-        {
-            "x": 2.8587006072828576,
-            "y": 5.214124992637136,
-            "heading": 1.956646376370765,
-            "angularVelocity": -1.108232291990471,
-            "velocityX": 2.4346144256256212,
-            "velocityY": 2.3204581873480494,
-            "timestamp": 0.48792995555260377
-        },
-        {
-            "x": 3.035891081202655,
-            "y": 5.338211520127435,
-            "heading": 1.8312106553523118,
-            "angularVelocity": -1.7995411782716633,
-            "velocityX": 2.5420315012917163,
-            "velocityY": 1.7801852141837777,
-            "timestamp": 0.5576342349172615
-        },
-        {
-            "x": 3.2127166120567576,
-            "y": 5.42406293198425,
-            "heading": 1.6557539162164756,
-            "angularVelocity": -2.5171587847273322,
-            "velocityX": 2.536795910750906,
-            "velocityY": 1.231651953643812,
-            "timestamp": 0.6273385142819192
-        },
-        {
-            "x": 3.3872389413542425,
-            "y": 5.4708692312967075,
-            "heading": 1.441472502737752,
-            "angularVelocity": -3.0741500440412146,
-            "velocityX": 2.503753440796252,
-            "velocityY": 0.6714982170261123,
-            "timestamp": 0.697042793646577
+            "x": 2.312764146242865,
+            "y": 4.398513079462153,
+            "heading": -0.39804543849617763,
+            "angularVelocity": -0.11524733034669515,
+            "velocityX": 0.18284072142228638,
+            "velocityY": 0.5516827447028667,
+            "timestamp": 0.06981019405093988
+        },
+        {
+            "x": 2.339895425931373,
+            "y": 4.474992910905513,
+            "heading": -0.4126392714825622,
+            "angularVelocity": -0.20905017072795415,
+            "velocityX": 0.3886435220157258,
+            "velocityY": 1.0955395910772,
+            "timestamp": 0.13962038810187977
+        },
+        {
+            "x": 2.3838571031189573,
+            "y": 4.588440455462814,
+            "heading": -0.43142102712692415,
+            "angularVelocity": -0.2690403013169746,
+            "velocityX": 0.6297314852832604,
+            "velocityY": 1.6250856497386854,
+            "timestamp": 0.20943058215281965
+        },
+        {
+            "x": 2.448821264845437,
+            "y": 4.736675156040894,
+            "heading": -0.45019318323311186,
+            "angularVelocity": -0.26890279219946406,
+            "velocityX": 0.9305827409539079,
+            "velocityY": 2.1233961972720627,
+            "timestamp": 0.27924077620375953
+        },
+        {
+            "x": 2.542623225560298,
+            "y": 4.913429983875953,
+            "heading": -0.4600490129319591,
+            "angularVelocity": -0.14118037962391586,
+            "velocityX": 1.343671393412285,
+            "velocityY": 2.5319343433823196,
+            "timestamp": 0.34905097025469944
+        },
+        {
+            "x": 2.675927389267578,
+            "y": 5.095797559489528,
+            "heading": -0.44245218860567326,
+            "angularVelocity": 0.25206668688824874,
+            "velocityX": 1.9095228930350086,
+            "velocityY": 2.6123344605070336,
+            "timestamp": 0.41886116430563936
+        },
+        {
+            "x": 2.83772713712192,
+            "y": 5.250967901017311,
+            "heading": -0.38879601501351296,
+            "angularVelocity": 0.7686008371275667,
+            "velocityX": 2.3177094699953265,
+            "velocityY": 2.2227461710524636,
+            "timestamp": 0.48867135835657927
+        },
+        {
+            "x": 3.013235940099558,
+            "y": 5.368218424760011,
+            "heading": -0.31718431053283597,
+            "angularVelocity": 1.0258058360734288,
+            "velocityX": 2.514085591121856,
+            "velocityY": 1.6795616361728827,
+            "timestamp": 0.5584815524075192
+        },
+        {
+            "x": 3.1945050869972476,
+            "y": 5.445323486359522,
+            "heading": -0.23653218942173773,
+            "angularVelocity": 1.1553057859494003,
+            "velocityX": 2.5965999573717062,
+            "velocityY": 1.1044957351381008,
+            "timestamp": 0.6282917464584591
+        },
+        {
+            "x": 3.377225953863421,
+            "y": 5.481834951968023,
+            "heading": -0.15180568644047004,
+            "angularVelocity": 1.2136694952369833,
+            "velocityX": 2.6173952006491676,
+            "velocityY": 0.523010515923758,
+            "timestamp": 0.698101940509399
         },
         {
             "x": 3.558790922164917,
             "y": 5.477728366851807,
-            "heading": 1.205442867585498,
-            "angularVelocity": -3.386157023695285,
-            "velocityX": 2.461139866509503,
-            "velocityY": 0.09840336371911824,
-            "timestamp": 0.7667470730112347
-        },
-        {
-            "x": 3.6181705627536003,
-            "y": 5.4752230037777725,
-            "heading": 1.1222490393160696,
-            "angularVelocity": -3.4253809174942864,
-            "velocityX": 2.44486750990022,
-            "velocityY": -0.10315456138644599,
-            "timestamp": 0.7910345406749146
-        },
-        {
-            "x": 3.6770128805090945,
-            "y": 5.467829320967843,
-            "heading": 1.0387507075885118,
-            "angularVelocity": -3.4379183900025705,
-            "velocityX": 2.4227440493307753,
-            "velocityY": -0.3044237839989221,
-            "timestamp": 0.8153220083385946
-        },
-        {
-            "x": 3.735181283810321,
-            "y": 5.455564580406075,
-            "heading": 0.9556144237478981,
-            "angularVelocity": -3.4230116120725995,
-            "velocityX": 2.394996633931211,
-            "velocityY": -0.5049822703463023,
-            "timestamp": 0.8396094760022745
-        },
-        {
-            "x": 3.7925490950380945,
-            "y": 5.438455755513165,
-            "heading": 0.8735217241735449,
-            "angularVelocity": -3.380043597427691,
-            "velocityX": 2.362033457838118,
-            "velocityY": -0.7044301666120085,
-            "timestamp": 0.8638969436659545
-        },
-        {
-            "x": 3.8490034281899477,
-            "y": 5.416538531692629,
-            "heading": 0.7931634356424634,
-            "angularVelocity": -3.308631827897467,
-            "velocityX": 2.3244223701541378,
-            "velocityY": -0.902408769989304,
-            "timestamp": 0.8881844113296344
-        },
-        {
-            "x": 3.9044487728906376,
-            "y": 5.389855903636327,
-            "heading": 0.7152329727062241,
-            "angularVelocity": -3.2086697557513846,
-            "velocityX": 2.282878786231156,
-            "velocityY": -1.0986171315094786,
-            "timestamp": 0.9124718789933144
-        },
-        {
-            "x": 3.958810912211869,
-            "y": 5.358456406059338,
-            "heading": 0.6404209450438546,
-            "angularVelocity": -3.080272867403366,
-            "velocityX": 2.2382794317633223,
-            "velocityY": -1.2928271490380632,
-            "timestamp": 0.9367593466569943
-        },
-        {
-            "x": 4.012042256408639,
-            "y": 5.322391998548557,
-            "heading": 0.5694141014307214,
-            "angularVelocity": -2.923600129762354,
-            "velocityX": 2.1917206410273016,
-            "velocityY": -1.4848978086221774,
-            "timestamp": 0.9610468143206743
-        },
-        {
-            "x": 4.064130365340956,
-            "y": 5.281715631109981,
-            "heading": 0.5029022798709881,
-            "angularVelocity": -2.738524348472814,
-            "velocityX": 2.1446496462128213,
-            "velocityY": -1.6747883312431175,
-            "timestamp": 0.9853342819843542
-        },
-        {
-            "x": 4.1151123432623455,
-            "y": 5.236478617606072,
-            "heading": 0.44159733021441666,
-            "angularVelocity": -2.524139218855187,
-            "velocityX": 2.09910636330477,
-            "velocityY": -1.8625660826533157,
-            "timestamp": 1.0096217496480342
-        },
-        {
-            "x": 4.165095928896697,
-            "y": 5.186728422848365,
-            "heading": 0.38626165615570274,
-            "angularVelocity": -2.278363262278863,
-            "velocityX": 2.057999060523636,
-            "velocityY": -2.0483895417431537,
-            "timestamp": 1.0339092173117141
-        },
-        {
-            "x": 4.214269146284126,
-            "y": 5.132508462649398,
-            "heading": 0.33770705635311316,
-            "angularVelocity": -1.9991627153126146,
-            "velocityX": 2.0246333651722517,
-            "velocityY": -2.232425419964549,
-            "timestamp": 1.058196684975394
-        },
-        {
-            "x": 4.262857394718322,
-            "y": 5.073859999949952,
-            "heading": 0.2966997330272638,
-            "angularVelocity": -1.6884149427884858,
-            "velocityX": 2.000548147177036,
-            "velocityY": -2.4147623585784754,
-            "timestamp": 1.082484152639074
-        },
-        {
-            "x": 4.311032470035773,
-            "y": 5.010821065356263,
-            "heading": 0.26381895203232264,
-            "angularVelocity": -1.353816768806731,
-            "velocityX": 1.9835363647027058,
-            "velocityY": -2.59553344410457,
-            "timestamp": 1.106771620302754
-        },
-        {
-            "x": 4.358621953046075,
-            "y": 4.943414538605821,
-            "heading": 0.23906687218938943,
-            "angularVelocity": -1.019129708608845,
-            "velocityX": 1.9594254810463076,
-            "velocityY": -2.77536249080602,
-            "timestamp": 1.131059087966434
-        },
-        {
-            "x": 4.401785346523604,
-            "y": 4.873960680394997,
-            "heading": 0.21204412295947933,
-            "angularVelocity": -1.1126211099529368,
-            "velocityX": 1.7771878927533025,
-            "velocityY": -2.8596582884878865,
-            "timestamp": 1.155346555630114
-        },
-        {
-            "x": 4.442527933324705,
-            "y": 4.807784869243466,
-            "heading": 0.1747999499890042,
-            "angularVelocity": -1.5334728793554109,
-            "velocityX": 1.6775148140301646,
-            "velocityY": -2.7246896246203156,
-            "timestamp": 1.1796340232937939
-        },
-        {
-            "x": 4.4813722294916225,
-            "y": 4.745319922619003,
-            "heading": 0.12759770745686402,
-            "angularVelocity": -1.9434814360134838,
-            "velocityX": 1.5993555484998627,
-            "velocityY": -2.571900351631775,
-            "timestamp": 1.2039214909574738
-        },
-        {
-            "x": 4.518612325734112,
-            "y": 4.686765196550011,
-            "heading": 0.07078885768669944,
-            "angularVelocity": -2.3390190594105262,
-            "velocityX": 1.5333050262040608,
-            "velocityY": -2.410902893616876,
-            "timestamp": 1.2282089586211538
-        },
-        {
-            "x": 4.554475342160425,
-            "y": 4.632242142038935,
-            "heading": 0.004632855038261056,
-            "angularVelocity": -2.723874039258911,
-            "velocityX": 1.47660583321928,
-            "velocityY": -2.244904873002102,
-            "timestamp": 1.2524964262848337
-        },
-        {
-            "x": 4.589156761961767,
-            "y": 4.581819399142938,
-            "heading": -0.07076666263842435,
-            "angularVelocity": -3.1044618863019378,
-            "velocityX": 1.4279553670061842,
-            "velocityY": -2.076080701134665,
-            "timestamp": 1.2767838939485137
-        },
-        {
-            "x": 4.6228372561227475,
-            "y": 4.535518314545829,
-            "heading": -0.1555015238515939,
-            "angularVelocity": -3.488830634240387,
-            "velocityX": 1.3867437572072365,
-            "velocityY": -1.9063776116251314,
-            "timestamp": 1.3010713616121936
-        },
-        {
-            "x": 4.654626762222228,
-            "y": 4.492711334026638,
-            "heading": -0.25016165034544124,
-            "angularVelocity": -3.8974885239025583,
-            "velocityX": 1.3088851641383707,
-            "velocityY": -1.762513124544663,
-            "timestamp": 1.3253588292758736
-        },
-        {
-            "x": 4.685559266058307,
-            "y": 4.453815281857176,
-            "heading": -0.35503604485130436,
-            "angularVelocity": -4.318045666930297,
-            "velocityX": 1.2735993832052854,
-            "velocityY": -1.6014865241643912,
-            "timestamp": 1.3496462969395535
-        },
-        {
-            "x": 4.716254412162635,
-            "y": 4.418834840133045,
-            "heading": -0.4707020428721612,
-            "angularVelocity": -4.762373732104905,
-            "velocityX": 1.2638265351241147,
-            "velocityY": -1.440267145530449,
-            "timestamp": 1.3739337646032335
-        },
-        {
-            "x": 4.747074366156251,
-            "y": 4.387541660715592,
-            "heading": -0.5981615568154537,
-            "angularVelocity": -5.247954035730924,
-            "velocityX": 1.268965312497503,
-            "velocityY": -1.2884496585146104,
-            "timestamp": 1.3982212322669134
-        },
-        {
-            "x": 4.7760218641280225,
-            "y": 4.359163482704458,
-            "heading": -0.7400182505648043,
-            "angularVelocity": -5.840736288925144,
-            "velocityX": 1.191869748325327,
-            "velocityY": -1.1684288540946337,
-            "timestamp": 1.4225086999305934
-        },
-        {
-            "x": 4.800681197551656,
-            "y": 4.328735146997818,
-            "heading": -0.8864907082583147,
-            "angularVelocity": -6.030783436206011,
-            "velocityX": 1.015311014104143,
-            "velocityY": -1.2528410177625973,
-            "timestamp": 1.4467961675942733
-        },
-        {
-            "x": 4.821257943768876,
-            "y": 4.295565486099031,
-            "heading": -1.0326334280053908,
-            "angularVelocity": -6.017206971545368,
-            "velocityX": 0.8472166181404436,
-            "velocityY": -1.3657109649348727,
-            "timestamp": 1.4710836352579533
-        },
-        {
-            "x": 4.842407281192705,
-            "y": 4.263513715444916,
-            "heading": -1.1735548575556736,
-            "angularVelocity": -5.802228190344444,
-            "velocityX": 0.8707922010105955,
-            "velocityY": -1.319683513240253,
-            "timestamp": 1.4953711029216332
-        },
-        {
-            "x": 4.867138629271623,
-            "y": 4.234341103924059,
-            "heading": -1.3080892458596534,
-            "angularVelocity": -5.539251360699312,
-            "velocityX": 1.0182761093657138,
-            "velocityY": -1.2011384605766136,
-            "timestamp": 1.5196585705853132
-        },
-        {
-            "x": 4.89576478236693,
-            "y": 4.207846540583478,
-            "heading": -1.4377333776779964,
-            "angularVelocity": -5.337902395325306,
-            "velocityX": 1.1786388557138734,
-            "velocityY": -1.0908738493227768,
-            "timestamp": 1.5439460382489931
-        },
-        {
-            "x": 4.928534232838641,
-            "y": 4.183793595288781,
-            "heading": -1.5636936940682342,
-            "angularVelocity": -5.1862268283573245,
-            "velocityX": 1.3492329017369804,
-            "velocityY": -0.9903438937220402,
-            "timestamp": 1.568233505912673
-        },
-        {
-            "x": 4.965642273050373,
-            "y": 4.1619364014492675,
-            "heading": -1.6870176099857392,
-            "angularVelocity": -5.077676998904523,
-            "velocityX": 1.5278678174928029,
-            "velocityY": -0.8999371256888635,
-            "timestamp": 1.592520973576353
-        },
-        {
-            "x": 5.007242132688007,
-            "y": 4.142028453113074,
-            "heading": -1.8086864696957439,
-            "angularVelocity": -5.009532545542041,
-            "velocityX": 1.712811735405568,
-            "velocityY": -0.8196798699587677,
-            "timestamp": 1.616808441240033
-        },
-        {
-            "x": 5.0534540304279405,
-            "y": 4.123819388018015,
-            "heading": -1.9296932957732547,
-            "angularVelocity": -4.982274305133403,
-            "velocityX": 1.902705476744277,
-            "velocityY": -0.749730904316916,
-            "timestamp": 1.641095908903713
-        },
-        {
-            "x": 5.102543724383187,
-            "y": 4.108450898288225,
-            "heading": -2.039853637710442,
-            "angularVelocity": -4.535686612644417,
-            "velocityX": 2.0211944133087334,
-            "velocityY": -0.6327744803453944,
-            "timestamp": 1.6653833765673929
-        },
-        {
-            "x": 5.15444764406018,
-            "y": 4.096180097409251,
-            "heading": -2.1398530735226235,
-            "angularVelocity": -4.117326565161934,
-            "velocityX": 2.1370659302868042,
-            "velocityY": -0.5052317948043931,
-            "timestamp": 1.6896708442310728
-        },
-        {
-            "x": 5.20922556245635,
-            "y": 4.087205112082737,
-            "heading": -2.231114838342211,
-            "angularVelocity": -3.757566086482643,
-            "velocityX": 2.2553985106519345,
-            "velocityY": -0.36953153991984866,
-            "timestamp": 1.7139583118947528
-        },
-        {
-            "x": 5.266890046930524,
-            "y": 4.081668558801882,
-            "heading": -2.314693981916533,
-            "angularVelocity": -3.4412456964094225,
-            "velocityX": 2.3742485331398995,
-            "velocityY": -0.22795926514540626,
-            "timestamp": 1.7382457795584327
-        },
-        {
-            "x": 5.327424168986788,
-            "y": 4.079676285408984,
-            "heading": -2.3913241555270592,
-            "angularVelocity": -3.1551322958679826,
-            "velocityX": 2.492401550236051,
-            "velocityY": -0.08202886445327343,
-            "timestamp": 1.7625332472221127
+            "heading": -0.06601521216801902,
+            "angularVelocity": 1.2289104110775417,
+            "velocityX": 2.6008374674955865,
+            "velocityY": -0.05882500648124433,
+            "timestamp": 0.7679121345603389
+        },
+        {
+            "x": 3.7207755893743903,
+            "y": 5.440802283745667,
+            "heading": 0.01087618717506099,
+            "angularVelocity": 1.2168377614801031,
+            "velocityX": 2.5634734381227786,
+            "velocityY": -0.5843703286511746,
+            "timestamp": 0.8311016588990232
+        },
+        {
+            "x": 3.8800031263189014,
+            "y": 5.370704697701765,
+            "heading": 0.08523277253593173,
+            "angularVelocity": 1.1767232961398173,
+            "velocityX": 2.519840726939224,
+            "velocityY": -1.1093228945227975,
+            "timestamp": 0.8942911832377075
+        },
+        {
+            "x": 4.035880089753818,
+            "y": 5.267514783368049,
+            "heading": 0.15438353932229767,
+            "angularVelocity": 1.0943390935099226,
+            "velocityX": 2.4668165343191313,
+            "velocityY": -1.633022489291642,
+            "timestamp": 0.9574807075763918
+        },
+        {
+            "x": 4.187424190703861,
+            "y": 5.131425904894565,
+            "heading": 0.2138534475851711,
+            "angularVelocity": 0.9411355581283213,
+            "velocityX": 2.3982472179488545,
+            "velocityY": -2.153662017503815,
+            "timestamp": 1.0206702319150762
+        },
+        {
+            "x": 4.332733928007212,
+            "y": 4.96304651892829,
+            "heading": 0.25476003945729764,
+            "angularVelocity": 0.6473635034690916,
+            "velocityX": 2.2995858699928062,
+            "velocityY": -2.664672471059999,
+            "timestamp": 1.0838597562537606
+        },
+        {
+            "x": 4.46716744223408,
+            "y": 4.765388489141315,
+            "heading": 0.2547605945579538,
+            "angularVelocity": 0.000008784690993212257,
+            "velocityX": 2.12746520302083,
+            "velocityY": -3.1280189533989655,
+            "timestamp": 1.147049280592445
+        },
+        {
+            "x": 4.612743650855858,
+            "y": 4.575786978724626,
+            "heading": 0.2547605407972418,
+            "angularVelocity": -8.507852266556338e-7,
+            "velocityX": 2.303802887345736,
+            "velocityY": -3.0005212478052368,
+            "timestamp": 1.2102388049311295
+        },
+        {
+            "x": 4.782527461386278,
+            "y": 4.409030186652972,
+            "heading": 0.25208197397278675,
+            "angularVelocity": -0.04238941267495421,
+            "velocityX": 2.6868980627285057,
+            "velocityY": -2.638994260793031,
+            "timestamp": 1.2734283292698139
+        },
+        {
+            "x": 4.970403968588328,
+            "y": 4.270066434839271,
+            "heading": 0.23891743895373094,
+            "angularVelocity": -0.2083341369570075,
+            "velocityX": 2.9732223682415455,
+            "velocityY": -2.1991580609051504,
+            "timestamp": 1.3366178536084983
+        },
+        {
+            "x": 5.173968377386639,
+            "y": 4.1604195633547185,
+            "heading": 0.21971651393307054,
+            "angularVelocity": -0.3038624711945638,
+            "velocityX": 3.2214898106793903,
+            "velocityY": -1.7352064702582655,
+            "timestamp": 1.3998073779471827
         },
         {
             "x": 5.390789031982422,
             "y": 4.081310272216797,
-            "heading": -2.461469590575517,
-            "angularVelocity": -2.888132925992722,
-            "velocityX": 2.608953056493045,
-            "velocityY": 0.06727695247771216,
-            "timestamp": 1.7868207148857926
-        },
-        {
-            "x": 5.47852793656002,
-            "y": 4.0897392429132315,
-            "heading": -2.541079365075053,
-            "angularVelocity": -2.4976756873239485,
-            "velocityX": 2.752718873700339,
-            "velocityY": 0.2644503807478311,
-            "timestamp": 1.8186942583180192
-        },
-        {
-            "x": 5.570818505990881,
-            "y": 4.101985641066581,
-            "heading": -2.595195903422616,
-            "angularVelocity": -1.6978513375091853,
-            "velocityX": 2.8955227280299005,
-            "velocityY": 0.384218283711974,
-            "timestamp": 1.8505678017502458
-        },
-        {
-            "x": 5.666172128712358,
-            "y": 4.1161647975520586,
-            "heading": -2.6133567142303673,
-            "angularVelocity": -0.5697769639691046,
-            "velocityX": 2.9916229089567223,
-            "velocityY": 0.4448566101734819,
-            "timestamp": 1.8824413451824724
-        },
-        {
-            "x": 5.762848661651843,
-            "y": 4.1299008615103485,
-            "heading": -2.5899687129898252,
-            "angularVelocity": 0.7337747461392797,
-            "velocityX": 3.033127871240633,
-            "velocityY": 0.43095503289420706,
-            "timestamp": 1.914314888614699
-        },
-        {
-            "x": 5.85803719097752,
-            "y": 4.141633583817035,
-            "heading": -2.5274756720169904,
-            "angularVelocity": 1.960655585900415,
-            "velocityX": 2.9864432716142737,
-            "velocityY": 0.36810222658906844,
-            "timestamp": 1.9461884320469256
-        },
-        {
-            "x": 5.9473700887935115,
-            "y": 4.148469896281193,
-            "heading": -2.4508224753320094,
-            "angularVelocity": 2.404916066140258,
-            "velocityX": 2.8027287899740645,
-            "velocityY": 0.21448234893285298,
-            "timestamp": 1.9780619754791522
-        },
-        {
-            "x": 6.0307522620060645,
-            "y": 4.150565536032173,
-            "heading": -2.3598343948212093,
-            "angularVelocity": 2.854658463194389,
-            "velocityX": 2.6160308592563855,
-            "velocityY": 0.06574856527754983,
-            "timestamp": 2.009935518911379
-        },
-        {
-            "x": 6.108380396897077,
-            "y": 4.1477533333139265,
-            "heading": -2.2542021077222087,
-            "angularVelocity": 3.3141055472419727,
-            "velocityX": 2.4355037605428165,
-            "velocityY": -0.0882299994108409,
-            "timestamp": 2.0418090623436056
-        },
-        {
-            "x": 6.180764914708381,
-            "y": 4.139596420595998,
-            "heading": -2.1333489972804904,
-            "angularVelocity": 3.7916433953661524,
-            "velocityX": 2.270990609036518,
-            "velocityY": -0.2559148384387137,
-            "timestamp": 2.0736826057758324
-        },
-        {
-            "x": 6.2487782508806635,
-            "y": 4.1255186451287145,
-            "heading": -1.9962477740611584,
-            "angularVelocity": 4.301411404441215,
-            "velocityX": 2.133849231946881,
-            "velocityY": -0.4416758838633167,
-            "timestamp": 2.1055561492080592
-        },
-        {
-            "x": 6.318999325259612,
-            "y": 4.108403885933248,
-            "heading": -1.8570700146176022,
-            "angularVelocity": 4.366560616000945,
-            "velocityX": 2.2031147722330044,
-            "velocityY": -0.5369581587895442,
-            "timestamp": 2.137429692640286
-        },
-        {
-            "x": 6.393124517437766,
-            "y": 4.09868995620756,
-            "heading": -1.7226225348946564,
-            "angularVelocity": 4.218152901914545,
-            "velocityX": 2.3256024964958177,
-            "velocityY": -0.3047646630925519,
-            "timestamp": 2.169303236072513
-        },
-        {
-            "x": 6.468812945370671,
-            "y": 4.097296283093324,
-            "heading": -1.5892075925845424,
-            "angularVelocity": 4.185758090994726,
-            "velocityX": 2.3746474280101166,
-            "velocityY": -0.04372507616542974,
-            "timestamp": 2.2011767795047397
-        },
-        {
-            "x": 6.543776258333572,
-            "y": 4.104341851159788,
-            "heading": -1.4561024343589095,
-            "angularVelocity": 4.176038930489708,
-            "velocityX": 2.351897683490888,
-            "velocityY": 0.22104753057800555,
-            "timestamp": 2.2330503229369665
-        },
-        {
-            "x": 6.614153245906366,
-            "y": 4.118263151934315,
-            "heading": -1.3162963129525131,
-            "angularVelocity": 4.386274833347886,
-            "velocityX": 2.208006390078291,
-            "velocityY": 0.43676664956088845,
-            "timestamp": 2.2649238663691933
-        },
-        {
-            "x": 6.676725935567768,
-            "y": 4.132111873679476,
-            "heading": -1.164406226014857,
-            "angularVelocity": 4.765396958785676,
-            "velocityX": 1.9631544824770328,
-            "velocityY": 0.4344895563497056,
-            "timestamp": 2.29679740980142
-        },
-        {
-            "x": 6.732923820729643,
-            "y": 4.140964584087909,
-            "heading": -1.0033332436559739,
-            "angularVelocity": 5.053500960800803,
-            "velocityX": 1.7631514764390552,
-            "velocityY": 0.27774478313828127,
-            "timestamp": 2.328670953233647
-        },
-        {
-            "x": 6.785034236650818,
-            "y": 4.142631098888223,
-            "heading": -0.8355937264762496,
-            "angularVelocity": 5.262656708890626,
-            "velocityX": 1.6349112872241036,
-            "velocityY": 0.0522852065023047,
-            "timestamp": 2.3605444966658737
-        },
-        {
-            "x": 6.835668705020557,
-            "y": 4.136059975961784,
-            "heading": -0.6631765444972689,
-            "angularVelocity": 5.409413683345085,
-            "velocityX": 1.5886049342899025,
-            "velocityY": -0.2061622969661811,
-            "timestamp": 2.3924180400981006
-        },
-        {
-            "x": 6.8889189304898775,
-            "y": 4.121434248751472,
-            "heading": -0.4913644813841438,
-            "angularVelocity": 5.390428694520651,
-            "velocityX": 1.6706716522606933,
-            "velocityY": -0.45886731236551676,
-            "timestamp": 2.4242915835303274
-        },
-        {
-            "x": 6.949941444507073,
-            "y": 4.105368374325079,
-            "heading": -0.330980540513716,
-            "angularVelocity": 5.031882985067382,
-            "velocityX": 1.9145192986449624,
-            "velocityY": -0.5040504661979253,
-            "timestamp": 2.456165126962554
-        },
-        {
-            "x": 7.016508340627581,
-            "y": 4.095339734393401,
-            "heading": -0.17821539872753786,
-            "angularVelocity": 4.792850914458417,
-            "velocityX": 2.0884686467963878,
-            "velocityY": -0.31463837564850267,
-            "timestamp": 2.488038670394781
-        },
-        {
-            "x": 7.085360516906702,
-            "y": 4.093448303803928,
-            "heading": -0.02746406984555507,
-            "angularVelocity": 4.7296695832557045,
-            "velocityX": 2.1601669869408497,
-            "velocityY": -0.05934171057873482,
-            "timestamp": 2.519912213827008
-        },
-        {
-            "x": 7.153846633279139,
-            "y": 4.100009116269512,
-            "heading": 0.121772089508303,
-            "angularVelocity": 4.682132680703696,
-            "velocityX": 2.1486822297640367,
-            "velocityY": 0.20583881674575163,
-            "timestamp": 2.5517857572592346
-        },
-        {
-            "x": 7.2178667451999,
-            "y": 4.113588367220929,
-            "heading": 0.27676556654047163,
-            "angularVelocity": 4.862762665899784,
-            "velocityX": 2.008565883391336,
-            "velocityY": 0.4260351843305824,
-            "timestamp": 2.5836593006914614
-        },
-        {
-            "x": 7.274040071387795,
-            "y": 4.126189441281968,
-            "heading": 0.44297350868055696,
-            "angularVelocity": 5.214605100104267,
-            "velocityX": 1.7623809636144747,
-            "velocityY": 0.3953458795013916,
-            "timestamp": 2.6155328441236882
-        },
-        {
-            "x": 7.324153742240104,
-            "y": 4.133335840122352,
-            "heading": 0.617641408977622,
-            "angularVelocity": 5.480027680902935,
-            "velocityX": 1.5722654419916315,
-            "velocityY": 0.22421099353388252,
-            "timestamp": 2.647406387555915
-        },
-        {
-            "x": 7.370062418422902,
-            "y": 4.133468677579737,
-            "heading": 0.8003841200365157,
-            "angularVelocity": 5.733366653991959,
-            "velocityX": 1.440338012007226,
-            "velocityY": 0.00416764008895702,
-            "timestamp": 2.679279930988142
-        },
-        {
-            "x": 7.413378960747577,
-            "y": 4.125978597308371,
-            "heading": 0.9928213817572759,
-            "angularVelocity": 6.037523318671553,
-            "velocityX": 1.3590124492050755,
-            "velocityY": -0.2349936488013014,
-            "timestamp": 2.7111534744203687
-        },
-        {
-            "x": 7.454276397546973,
-            "y": 4.11151825545799,
-            "heading": 1.2015436063625509,
-            "angularVelocity": 6.548447462362883,
-            "velocityX": 1.2831154743229476,
-            "velocityY": -0.4536785149454069,
-            "timestamp": 2.7430270178525955
-        },
-        {
-            "x": 7.496754891759898,
-            "y": 4.105364802951648,
-            "heading": 1.412047118709339,
-            "angularVelocity": 6.604333553135948,
-            "velocityX": 1.3327195422513116,
-            "velocityY": -0.19305831243477098,
-            "timestamp": 2.7749005612848223
-        },
-        {
-            "x": 7.53888503669894,
-            "y": 4.1076484340015265,
-            "heading": 1.6249494045277855,
-            "angularVelocity": 6.679592630519619,
-            "velocityX": 1.3217904381614993,
-            "velocityY": 0.07164660103553838,
-            "timestamp": 2.806774104717049
-        },
-        {
-            "x": 7.576001000454075,
-            "y": 4.116500503183424,
-            "heading": 1.8445622032463762,
-            "angularVelocity": 6.890128146108359,
-            "velocityX": 1.1644756044791265,
-            "velocityY": 0.277724665308084,
-            "timestamp": 2.838647648149276
-        },
-        {
-            "x": 7.60515186851165,
-            "y": 4.124308875688622,
-            "heading": 2.0739714188941045,
-            "angularVelocity": 7.1974807612943055,
-            "velocityX": 0.9145788299176564,
-            "velocityY": 0.24497974383681875,
-            "timestamp": 2.8705211915815028
-        },
-        {
-            "x": 7.62773108442525,
-            "y": 4.127593662867362,
-            "heading": 2.3140115714104335,
-            "angularVelocity": 7.53101559061771,
-            "velocityX": 0.708399929289633,
-            "velocityY": 0.10305685609521825,
-            "timestamp": 2.9023947350137296
-        },
-        {
-            "x": 7.642808233685486,
-            "y": 4.127168273515703,
-            "heading": 2.549384793893863,
-            "angularVelocity": 7.384595408537156,
-            "velocityX": 0.473030219946994,
-            "velocityY": -0.013346158156635379,
-            "timestamp": 2.9342682784459564
-        },
-        {
-            "x": 7.655297430086512,
-            "y": 4.12694395453313,
-            "heading": 2.7459505912038105,
-            "angularVelocity": 6.167051922793274,
-            "velocityX": 0.39183583173240866,
-            "velocityY": -0.007037779876922506,
-            "timestamp": 2.966141821878183
-        },
-        {
-            "x": 7.665239437445753,
-            "y": 4.126897699920467,
-            "heading": 2.903400180799407,
-            "angularVelocity": 4.939820698956328,
-            "velocityX": 0.31192036682023955,
-            "velocityY": -0.001451191417126478,
-            "timestamp": 2.99801536531041
-        },
-        {
-            "x": 7.672649475189336,
-            "y": 4.12693483442264,
-            "heading": 3.0216231319761504,
-            "angularVelocity": 3.7091248241076054,
-            "velocityX": 0.23248239591994224,
-            "velocityY": 0.001165057228464201,
-            "timestamp": 3.029888908742637
-        },
-        {
-            "x": 7.677558550585934,
-            "y": 4.1269805608298435,
-            "heading": 3.1005189939079423,
-            "angularVelocity": 2.4752774067793823,
-            "velocityX": 0.15401724653037796,
-            "velocityY": 0.0014346195082003044,
-            "timestamp": 3.0617624521748636
+            "heading": 0.19899217158132915,
+            "angularVelocity": -0.3279711719205329,
+            "velocityX": 3.4312753081359326,
+            "velocityY": -1.25193680386463,
+            "timestamp": 1.462996902285867
+        },
+        {
+            "x": 5.681530534379024,
+            "y": 4.031634954440038,
+            "heading": 0.1807325834147537,
+            "angularVelocity": -0.22892527550150313,
+            "velocityX": 3.645102941474752,
+            "velocityY": -0.6227925681552634,
+            "timestamp": 1.542759122191511
+        },
+        {
+            "x": 5.983259555492767,
+            "y": 4.033604453850848,
+            "heading": 0.1807316659794543,
+            "angularVelocity": -0.000011502133581259988,
+            "velocityX": 3.7828563632073102,
+            "velocityY": 0.024692133859281144,
+            "timestamp": 1.6225213420971547
+        },
+        {
+            "x": 6.284539191755923,
+            "y": 4.050188554583329,
+            "heading": 0.18073165818528908,
+            "angularVelocity": -9.771750653524246e-8,
+            "velocityX": 3.777222306745724,
+            "velocityY": 0.20791924738433482,
+            "timestamp": 1.7022835620027985
+        },
+        {
+            "x": 6.585818825161056,
+            "y": 4.0667727072366,
+            "heading": 0.18073165039112304,
+            "angularVelocity": -9.77175162921636e-8,
+            "velocityX": 3.777222270913928,
+            "velocityY": 0.20791989832899174,
+            "timestamp": 1.7820457819084423
+        },
+        {
+            "x": 6.88709845856161,
+            "y": 4.083356859971397,
+            "heading": 0.180731642596831,
+            "angularVelocity": -9.771909620772691e-8,
+            "velocityX": 3.777222270856537,
+            "velocityY": 0.20791989935110283,
+            "timestamp": 1.8618080018140861
+        },
+        {
+            "x": 7.15139892307148,
+            "y": 4.097904823415219,
+            "heading": 0.12048670978727856,
+            "angularVelocity": -0.7553066211144209,
+            "velocityX": 3.313604671767403,
+            "velocityY": 0.1823916568650699,
+            "timestamp": 1.94157022171973
+        },
+        {
+            "x": 7.3628393316310445,
+            "y": 4.109542993215659,
+            "heading": 0.07229126938267637,
+            "angularVelocity": -0.6042389550038586,
+            "velocityX": 2.650884200686756,
+            "velocityY": 0.1459108060709806,
+            "timestamp": 2.0213324416253737
+        },
+        {
+            "x": 7.521419659615098,
+            "y": 4.118271521022388,
+            "heading": 0.03614542857914898,
+            "angularVelocity": -0.4531699449471921,
+            "velocityX": 1.98816342087339,
+            "velocityY": 0.10943185654883955,
+            "timestamp": 2.1010946615310178
+        },
+        {
+            "x": 7.627139886542953,
+            "y": 4.12409050689369,
+            "heading": 0.012048510432709781,
+            "angularVelocity": -0.302109421915929,
+            "velocityX": 1.3254423842882344,
+            "velocityY": 0.07295416148382058,
+            "timestamp": 2.180856881436662
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 1.238676401825463,
-            "velocityX": 0.07659799166218657,
-            "velocityY": 0.0006098841880359634,
-            "timestamp": 3.0936359956070905
+            "heading": 2.791295127527068e-32,
+            "angularVelocity": -0.15105535486544666,
+            "velocityX": 0.6627211920578459,
+            "velocityY": 0.03647708288149429,
+            "timestamp": 2.260619101342306
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": -2.349124968959552e-31,
-            "velocityX": 6.522838948908763e-32,
-            "velocityY": -6.176555916296272e-32,
-            "timestamp": 3.1255095390393173
+            "heading": 0,
+            "angularVelocity": -1.938584239537671e-30,
+            "velocityX": -1.689352589413745e-32,
+            "velocityY": 0,
+            "timestamp": 2.34038132124795
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.1.traj
index f2e9d5e3..ce59bb95 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.1.traj	
@@ -3,587 +3,623 @@
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
+            "heading": -0.39,
             "angularVelocity": 0,
             "velocityX": 0,
             "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 2.291384520253252,
-            "y": 4.354032854764245,
-            "heading": 2.74598551663602,
-            "angularVelocity": -0.1129108392120503,
-            "velocityX": -0.24231786763597,
-            "velocityY": -0.1678311541441644,
-            "timestamp": 0.03555445510807641
-        },
-        {
-            "x": 2.274618850931762,
-            "y": 4.341572242021881,
-            "heading": 2.7363109696234753,
-            "angularVelocity": -0.2721050564025352,
-            "velocityX": -0.47154904415004567,
-            "velocityY": -0.3504655803186264,
-            "timestamp": 0.07110891021615282
-        },
-        {
-            "x": 2.250443983775186,
-            "y": 4.321957452674385,
-            "heading": 2.718567267740621,
-            "angularVelocity": -0.4990570613139176,
-            "velocityX": -0.6799391829544631,
-            "velocityY": -0.5516830250350497,
-            "timestamp": 0.10666336532422922
-        },
-        {
-            "x": 2.2201183125080237,
-            "y": 4.294438762451095,
-            "heading": 2.688674932435733,
-            "angularVelocity": -0.8407479516708394,
-            "velocityX": -0.8529359028279262,
-            "velocityY": -0.7739871174973922,
-            "timestamp": 0.14221782043230563
-        },
-        {
-            "x": 2.1856076192251765,
-            "y": 4.258557396291605,
-            "heading": 2.63987728987899,
-            "angularVelocity": -1.3724761751631571,
-            "velocityX": -0.9706432900727432,
-            "velocityY": -1.009194657896466,
-            "timestamp": 0.17777227554038205
-        },
-        {
-            "x": 2.1489602975624615,
-            "y": 4.2143768004023965,
-            "heading": 2.56722210682279,
-            "angularVelocity": -2.0434902696538804,
-            "velocityX": -1.0307378231874669,
-            "velocityY": -1.2426177185084084,
-            "timestamp": 0.21332673064845847
-        },
-        {
-            "x": 2.1162123400405237,
-            "y": 4.163459966768879,
-            "heading": 2.4666019501957965,
-            "angularVelocity": -2.8300294947885973,
-            "velocityX": -0.9210648123390461,
-            "velocityY": -1.4320802689492316,
-            "timestamp": 0.2488811857565349
-        },
-        {
-            "x": 2.0883593879928255,
-            "y": 4.106985689754014,
-            "heading": 2.334003256666977,
-            "angularVelocity": -3.7294536824078297,
-            "velocityX": -0.7833885222831486,
-            "velocityY": -1.588388201793505,
-            "timestamp": 0.2844356408646113
-        },
-        {
-            "x": 2.0671900205502793,
-            "y": 4.044210359842601,
-            "heading": 2.181383778591042,
-            "angularVelocity": -4.2925556758502195,
-            "velocityX": -0.5954068872155824,
-            "velocityY": -1.765610799563443,
-            "timestamp": 0.31999009597268774
-        },
-        {
-            "x": 2.0546372297458078,
-            "y": 3.9756937078562617,
-            "heading": 2.021719977202011,
-            "angularVelocity": -4.49068340110105,
-            "velocityX": -0.35305816855621736,
-            "velocityY": -1.927090480730638,
-            "timestamp": 0.35554455108076416
-        },
-        {
-            "x": 2.0513227865299846,
-            "y": 3.902144544449752,
-            "heading": 1.8643846688260233,
-            "angularVelocity": -4.42519250816047,
-            "velocityX": -0.0932216006615165,
-            "velocityY": -2.068634245214532,
-            "timestamp": 0.3910990061888406
-        },
-        {
-            "x": 2.056389020104272,
-            "y": 3.822243076047335,
-            "heading": 1.7096006131055106,
-            "angularVelocity": -4.353436306364677,
-            "velocityX": 0.14249222942348805,
-            "velocityY": -2.247298352893811,
-            "timestamp": 0.426653461296917
-        },
-        {
-            "x": 2.0690751873782185,
-            "y": 3.7373642087091223,
-            "heading": 1.5737353715537044,
-            "angularVelocity": -3.821328189078159,
-            "velocityX": 0.35680949786416494,
-            "velocityY": -2.3872920307793386,
-            "timestamp": 0.4622079164049934
-        },
-        {
-            "x": 2.089274580938585,
-            "y": 3.648200269765939,
-            "heading": 1.460663871168444,
-            "angularVelocity": -3.1802343768608408,
-            "velocityX": 0.5681255274188619,
-            "velocityY": -2.507813399815797,
-            "timestamp": 0.49776237151306985
-        },
-        {
-            "x": 2.1169367654773477,
-            "y": 3.55409496646177,
-            "heading": 1.3686138295927854,
-            "angularVelocity": -2.5889875487010094,
-            "velocityX": 0.7780230200315752,
-            "velocityY": -2.646793573916784,
-            "timestamp": 0.5333168266211462
-        },
-        {
-            "x": 2.1519045434306308,
-            "y": 3.4545849603291003,
-            "heading": 1.296733638399589,
-            "angularVelocity": -2.0216929488779694,
-            "velocityX": 0.9834991943200934,
-            "velocityY": -2.798805545751856,
-            "timestamp": 0.5688712817292226
-        },
-        {
-            "x": 2.1939849352834964,
-            "y": 3.3493550759903967,
-            "heading": 1.244789692250972,
-            "angularVelocity": -1.4609687025358928,
-            "velocityX": 1.1835476517626813,
-            "velocityY": -2.959682099439628,
-            "timestamp": 0.604425736837299
-        },
-        {
-            "x": 2.243083202431903,
-            "y": 3.2382839213654937,
-            "heading": 1.2128218877185057,
-            "angularVelocity": -0.8991223303884791,
-            "velocityX": 1.3809315034968324,
-            "velocityY": -3.123972911053636,
-            "timestamp": 0.6399801919453755
-        },
-        {
-            "x": 2.299610340542871,
-            "y": 3.1217794185343526,
-            "heading": 1.2005607333325377,
-            "angularVelocity": -0.3448556404168553,
-            "velocityX": 1.5898749661368734,
-            "velocityY": -3.276790559073325,
-            "timestamp": 0.6755346470534519
-        },
-        {
-            "x": 2.3661237313117724,
-            "y": 3.0048767909387704,
-            "heading": 1.2005598156629014,
-            "angularVelocity": -0.000025810257353301716,
-            "velocityX": 1.8707470151551455,
-            "velocityY": -3.2879881646400775,
-            "timestamp": 0.7110891021615283
-        },
-        {
-            "x": 2.4413914156279892,
-            "y": 2.893838891515528,
-            "heading": 1.1996733189380515,
-            "angularVelocity": -0.02493349208011108,
-            "velocityX": 2.116969141769213,
-            "velocityY": -3.123037579558325,
-            "timestamp": 0.7466435572696047
-        },
-        {
-            "x": 2.522968201294396,
-            "y": 2.7909984002833275,
-            "heading": 1.191481222958301,
-            "angularVelocity": -0.2304098306344059,
-            "velocityX": 2.2944181093040097,
-            "velocityY": -2.89247833835709,
-            "timestamp": 0.7821980123776812
+            "x": 2.298402468716654,
+            "y": 4.3526843009931255,
+            "heading": -0.37533408415570846,
+            "angularVelocity": 0.4620245693037669,
+            "velocityX": -0.050327487964169394,
+            "velocityY": -0.2304685720750862,
+            "timestamp": 0.03174271850172795
+        },
+        {
+            "x": 2.2952744131725273,
+            "y": 4.338010072410149,
+            "heading": -0.34621912169362346,
+            "angularVelocity": 0.9172170449263883,
+            "velocityX": -0.09854403440450425,
+            "velocityY": -0.462286447903903,
+            "timestamp": 0.0634854370034559
+        },
+        {
+            "x": 2.2907034035575107,
+            "y": 4.3159283160610205,
+            "heading": -0.3028943022400045,
+            "angularVelocity": 1.364874260887912,
+            "velocityX": -0.14400183194038138,
+            "velocityY": -0.6956479278208441,
+            "timestamp": 0.09522815550518385
+        },
+        {
+            "x": 2.2847999846220546,
+            "y": 4.286378308280002,
+            "heading": -0.24568561151458213,
+            "angularVelocity": 1.8022618548662834,
+            "velocityX": -0.1859771063759033,
+            "velocityY": -0.9309224028625668,
+            "timestamp": 0.1269708740069118
+        },
+        {
+            "x": 2.2777013130983144,
+            "y": 4.249280679670162,
+            "heading": -0.1750916915555501,
+            "angularVelocity": 2.223940585151498,
+            "velocityX": -0.22363149278956135,
+            "velocityY": -1.1686972748670326,
+            "timestamp": 0.15871359250863973
+        },
+        {
+            "x": 2.2695785209744463,
+            "y": 4.204529290238701,
+            "heading": -0.09189311777806415,
+            "angularVelocity": 2.6210286233977427,
+            "velocityX": -0.25589465890975394,
+            "velocityY": -1.409815905623355,
+            "timestamp": 0.19045631101036767
+        },
+        {
+            "x": 2.2606569711878857,
+            "y": 4.151982062664549,
+            "heading": 0.002706319559794192,
+            "angularVelocity": 2.9801933105606198,
+            "velocityX": -0.2810581515277304,
+            "velocityY": -1.6554104391308149,
+            "timestamp": 0.2221990295120956
+        },
+        {
+            "x": 2.251280347316294,
+            "y": 4.091451352956859,
+            "heading": 0.10685098530745366,
+            "angularVelocity": 3.2808993893194844,
+            "velocityX": -0.2953944814487593,
+            "velocityY": -1.90691637530654,
+            "timestamp": 0.2539417480138236
+        },
+        {
+            "x": 2.242133674900212,
+            "y": 4.022709367456616,
+            "heading": 0.21739538615105536,
+            "angularVelocity": 3.4825120865934673,
+            "velocityX": -0.28815025454054716,
+            "velocityY": -2.165598560706175,
+            "timestamp": 0.2856844665155515
+        },
+        {
+            "x": 2.2351422077578347,
+            "y": 3.945850682832356,
+            "heading": 0.32707167243976176,
+            "angularVelocity": 3.455163623831899,
+            "velocityX": -0.22025420229828702,
+            "velocityY": -2.4213012700873704,
+            "timestamp": 0.31742718501727946
+        },
+        {
+            "x": 2.2344515310594897,
+            "y": 3.864378163543378,
+            "heading": 0.4256089749961829,
+            "angularVelocity": 3.1042490122910227,
+            "velocityX": -0.021758586880562782,
+            "velocityY": -2.566652231898267,
+            "timestamp": 0.3491699035190074
+        },
+        {
+            "x": 2.2416429003575074,
+            "y": 3.7806290345563274,
+            "heading": 0.5177286971400864,
+            "angularVelocity": 2.9020741288711216,
+            "velocityX": 0.22655177746121863,
+            "velocityY": -2.638372922674897,
+            "timestamp": 0.38091262202073534
+        },
+        {
+            "x": 2.256915898193716,
+            "y": 3.695399323053411,
+            "heading": 0.6036240465401915,
+            "angularVelocity": 2.705985922265222,
+            "velocityX": 0.4811496480800017,
+            "velocityY": -2.6850161399464656,
+            "timestamp": 0.4126553405224633
+        },
+        {
+            "x": 2.280071045497703,
+            "y": 3.6084777900686467,
+            "heading": 0.6811513337134051,
+            "angularVelocity": 2.4423644486842004,
+            "velocityX": 0.729463272111561,
+            "velocityY": -2.7383140791811433,
+            "timestamp": 0.4443980590241912
+        },
+        {
+            "x": 2.3096855944046597,
+            "y": 3.5244760350530098,
+            "heading": 0.76088689172136,
+            "angularVelocity": 2.511932240574098,
+            "velocityX": 0.932955660534971,
+            "velocityY": -2.6463314731869354,
+            "timestamp": 0.47614077752591916
+        },
+        {
+            "x": 2.3348942368332115,
+            "y": 3.446420745412462,
+            "heading": 0.8559069862879931,
+            "angularVelocity": 2.993445396350149,
+            "velocityX": 0.7941551202420774,
+            "velocityY": -2.458998262429792,
+            "timestamp": 0.5078834960276472
+        },
+        {
+            "x": 2.355075569752848,
+            "y": 3.373868279566975,
+            "heading": 0.9657696902827837,
+            "angularVelocity": 3.4610363944982643,
+            "velocityX": 0.6357783413709821,
+            "velocityY": -2.285641220097014,
+            "timestamp": 0.5396262145293751
+        },
+        {
+            "x": 2.370025604730125,
+            "y": 3.306495756130267,
+            "heading": 1.091029644988018,
+            "angularVelocity": 3.9461004166489144,
+            "velocityX": 0.4709752561508733,
+            "velocityY": -2.1224560030370347,
+            "timestamp": 0.571368933031103
+        },
+        {
+            "x": 2.3800631598495494,
+            "y": 3.2441375159647747,
+            "heading": 1.2334110423596254,
+            "angularVelocity": 4.485482154399005,
+            "velocityX": 0.31621598883778346,
+            "velocityY": -1.9644895934825695,
+            "timestamp": 0.603111651532831
+        },
+        {
+            "x": 2.3890320547539976,
+            "y": 3.1882487557001293,
+            "heading": 1.3963738258608078,
+            "angularVelocity": 5.133863487221998,
+            "velocityX": 0.28254967840764567,
+            "velocityY": -1.760679705539431,
+            "timestamp": 0.6348543700345589
+        },
+        {
+            "x": 2.399614951757013,
+            "y": 3.1248820965808815,
+            "heading": 1.5465533770663946,
+            "angularVelocity": 4.7311496397956585,
+            "velocityX": 0.33339605120587323,
+            "velocityY": -1.9962581061165,
+            "timestamp": 0.6665970885362869
+        },
+        {
+            "x": 2.4156041612276775,
+            "y": 3.056078363317597,
+            "heading": 1.6846738705269053,
+            "angularVelocity": 4.351249671731503,
+            "velocityX": 0.5037126694046519,
+            "velocityY": -2.167543818263053,
+            "timestamp": 0.6983398070380148
+        },
+        {
+            "x": 2.4386535219180887,
+            "y": 2.9838461830329104,
+            "heading": 1.8122491249375776,
+            "angularVelocity": 4.019039969866676,
+            "velocityX": 0.7261306459671242,
+            "velocityY": -2.275551171861821,
+            "timestamp": 0.7300825255397427
+        },
+        {
+            "x": 2.4695800740474128,
+            "y": 2.910121142766613,
+            "heading": 1.9308538821800434,
+            "angularVelocity": 3.736439814882526,
+            "velocityX": 0.9742880757878827,
+            "velocityY": -2.3225811696715355,
+            "timestamp": 0.7618252440414707
+        },
+        {
+            "x": 2.508646228883482,
+            "y": 2.8366969767650785,
+            "heading": 2.0420489888957944,
+            "angularVelocity": 3.503011461028351,
+            "velocityX": 1.2307123233299608,
+            "velocityY": -2.313102641084062,
+            "timestamp": 0.7935679625431986
+        },
+        {
+            "x": 2.555734299118471,
+            "y": 2.7651139971749186,
+            "heading": 2.147169748045839,
+            "angularVelocity": 3.3116495408016253,
+            "velocityX": 1.4834290337302587,
+            "velocityY": -2.255099215470886,
+            "timestamp": 0.8253106810449266
         },
         {
             "x": 2.6104869842529297,
             "y": 2.6966493129730225,
-            "heading": 1.1763516697745318,
-            "angularVelocity": -0.4255318535406987,
-            "velocityX": 2.4615419556423808,
-            "velocityY": -2.653650211302861,
-            "timestamp": 0.8177524674857576
-        },
-        {
-            "x": 2.7952393085826728,
-            "y": 2.5500987298885027,
-            "heading": 1.131439771707853,
-            "angularVelocity": -0.6752506092749339,
-            "velocityX": 2.777752108882206,
-            "velocityY": -2.203388740562692,
-            "timestamp": 0.884263918863645
-        },
-        {
-            "x": 2.9911597283478026,
-            "y": 2.437065890505019,
-            "heading": 1.0475109017935547,
-            "angularVelocity": -1.2618709737283185,
-            "velocityX": 2.9456644789180824,
-            "velocityY": -1.6994492984566387,
-            "timestamp": 0.9507753702415324
-        },
-        {
-            "x": 3.2086737929320703,
-            "y": 2.3383672587643702,
-            "heading": 1.0088878551520015,
-            "angularVelocity": -0.5806976970343763,
-            "velocityX": 3.2703250354356728,
-            "velocityY": -1.4839344157427674,
-            "timestamp": 1.0172868216194197
-        },
-        {
-            "x": 3.4256073991681717,
-            "y": 2.2401993070344908,
-            "heading": 0.9691349707945731,
-            "angularVelocity": -0.5976848126733959,
-            "velocityX": 3.2615978413038147,
-            "velocityY": -1.4759556391595554,
-            "timestamp": 1.083798272997307
-        },
-        {
-            "x": 3.641898030119061,
-            "y": 2.142612874932996,
-            "heading": 0.9280602960666862,
-            "angularVelocity": -0.6175579374222298,
-            "velocityX": 3.251930704714679,
-            "velocityY": -1.4672124886743745,
-            "timestamp": 1.1503097243751943
-        },
-        {
-            "x": 3.8574674811397935,
-            "y": 2.045673041019181,
-            "heading": 0.8854247472051884,
-            "angularVelocity": -0.6410256877310082,
-            "velocityX": 3.2410877609025053,
-            "velocityY": -1.4574908817287378,
-            "timestamp": 1.2168211757530816
-        },
-        {
-            "x": 4.072209689171901,
-            "y": 1.949463725749032,
-            "heading": 0.8409091268268268,
-            "angularVelocity": -0.6692925722736718,
-            "velocityX": 3.2286501584823513,
-            "velocityY": -1.446507530312815,
-            "timestamp": 1.2833326271309689
-        },
-        {
-            "x": 4.28597464141242,
-            "y": 1.8540976935596791,
-            "heading": 0.7940685595150717,
-            "angularVelocity": -0.7042481609013259,
-            "velocityX": 3.2139571128286692,
-            "velocityY": -1.4338287650276498,
-            "timestamp": 1.3498440785088561
-        },
-        {
-            "x": 4.498537652945423,
-            "y": 1.7597351759166577,
-            "heading": 0.7442459258691415,
-            "angularVelocity": -0.7490835429655718,
-            "velocityX": 3.195885928354179,
-            "velocityY": -1.4187409188666942,
-            "timestamp": 1.4163555298867434
-        },
-        {
-            "x": 4.709533812148138,
-            "y": 1.6666229170102642,
-            "heading": 0.6903878152301356,
-            "angularVelocity": -0.8097569594897124,
-            "velocityX": 3.1723282958288084,
-            "velocityY": -1.399943272585839,
-            "timestamp": 1.4828669812646307
-        },
-        {
-            "x": 4.924860666444789,
-            "y": 1.5757431293365876,
-            "heading": 0.6452055578690052,
-            "angularVelocity": -0.679315462602457,
-            "velocityX": 3.2374403179576423,
-            "velocityY": -1.3663780565746433,
-            "timestamp": 1.549378432642518
-        },
-        {
-            "x": 5.15821860461426,
-            "y": 1.5166188222705783,
-            "heading": 0.6185000496396688,
-            "angularVelocity": -0.4015174481399904,
-            "velocityX": 3.5085377530500543,
-            "velocityY": -0.8889342487820968,
-            "timestamp": 1.6158898840204052
-        },
-        {
-            "x": 5.39961745545984,
-            "y": 1.4934726786236598,
-            "heading": 0.5961273041444063,
-            "angularVelocity": -0.33637433903149927,
-            "velocityX": 3.6294329148534703,
-            "velocityY": -0.3480023840618476,
-            "timestamp": 1.6824013353982925
+            "heading": 2.247054267099527,
+            "angularVelocity": 3.1466907614812087,
+            "velocityX": 1.7248896036260801,
+            "velocityY": -2.1568626580665695,
+            "timestamp": 0.8570533995466545
+        },
+        {
+            "x": 2.751018773357776,
+            "y": 2.5737681266542305,
+            "heading": 2.423101617636397,
+            "angularVelocity": 2.7414576820608,
+            "velocityX": 2.188399607493837,
+            "velocityY": -1.9135395743649004,
+            "timestamp": 0.9212700940770386
+        },
+        {
+            "x": 2.920945282929504,
+            "y": 2.4615612619997704,
+            "heading": 2.5486951706813996,
+            "angularVelocity": 1.9557772937936253,
+            "velocityX": 2.646142265876451,
+            "velocityY": -1.7473161064272718,
+            "timestamp": 0.9854867886074228
+        },
+        {
+            "x": 3.118708267205298,
+            "y": 2.3583606370011725,
+            "heading": 2.6122511958810777,
+            "angularVelocity": 0.9897118757741349,
+            "velocityX": 3.079619493373657,
+            "velocityY": -1.6070684695514719,
+            "timestamp": 1.049703483137807
+        },
+        {
+            "x": 3.3434611246912245,
+            "y": 2.26616504003377,
+            "heading": 2.6122519169792926,
+            "angularVelocity": 0.000011229139401682067,
+            "velocityX": 3.499913209945475,
+            "velocityY": -1.4356951512629466,
+            "timestamp": 1.113920177668191
+        },
+        {
+            "x": 3.5722970045795748,
+            "y": 2.184629282343257,
+            "heading": 2.6122522165022306,
+            "angularVelocity": 0.000004664253435080619,
+            "velocityX": 3.5634951559220656,
+            "velocityY": -1.269697207039109,
+            "timestamp": 1.1781368721985752
+        },
+        {
+            "x": 3.80113295504209,
+            "y": 2.103093722723815,
+            "heading": 2.6122525160244625,
+            "angularVelocity": 0.000004664242440601037,
+            "velocityX": 3.56349625492233,
+            "velocityY": -1.2696941226220015,
+            "timestamp": 1.2423535667289594
+        },
+        {
+            "x": 4.029968923086795,
+            "y": 2.0215582124500746,
+            "heading": 2.612252815546594,
+            "angularVelocity": 0.00000466424087155042,
+            "velocityX": 3.563496528717025,
+            "velocityY": -1.2696933541971844,
+            "timestamp": 1.3065702612593435
+        },
+        {
+            "x": 4.258804909490432,
+            "y": 1.940022753702107,
+            "heading": 2.6122531150686714,
+            "angularVelocity": 0.000004664240033500949,
+            "velocityX": 3.5634968146073587,
+            "velocityY": -1.2696925518237052,
+            "timestamp": 1.3707869557897276
+        },
+        {
+            "x": 4.487640916105967,
+            "y": 1.8584873516805105,
+            "heading": 2.6122534145907514,
+            "angularVelocity": 0.000004664240074560689,
+            "velocityX": 3.563497129352575,
+            "velocityY": -1.2696916684650783,
+            "timestamp": 1.4350036503201118
+        },
+        {
+            "x": 4.716476945604857,
+            "y": 1.7769520138830965,
+            "heading": 2.6122537141129194,
+            "angularVelocity": 0.000004664241438058459,
+            "velocityX": 3.5634974856984654,
+            "velocityY": -1.2696906683484759,
+            "timestamp": 1.499220344850496
+        },
+        {
+            "x": 4.945312999982612,
+            "y": 1.6954167459104819,
+            "heading": 2.612254013635151,
+            "angularVelocity": 0.000004664242435524347,
+            "velocityX": 3.563497873118986,
+            "velocityY": -1.269689581017522,
+            "timestamp": 1.56343703938088
+        },
+        {
+            "x": 5.174149135873993,
+            "y": 1.613881706713074,
+            "heading": 2.6122543131567744,
+            "angularVelocity": 0.0000046642329655920085,
+            "velocityX": 3.563499142471556,
+            "velocityY": -1.269686018467187,
+            "timestamp": 1.6276537339112642
+        },
+        {
+            "x": 5.406651475687904,
+            "y": 1.5434785463415874,
+            "heading": 2.61225458420666,
+            "angularVelocity": 0.000004220863245612629,
+            "velocityX": 3.6205902766281897,
+            "velocityY": -1.096337344772168,
+            "timestamp": 1.6918704284416484
         },
         {
             "x": 5.646764755249023,
             "y": 1.5066063404083252,
-            "heading": 0.585736957474559,
-            "angularVelocity": -0.15621891350429284,
-            "velocityX": 3.7158608731150236,
-            "velocityY": 0.19746466980619687,
-            "timestamp": 1.7489127867761798
-        },
-        {
-            "x": 5.78386589002804,
-            "y": 1.5248142647082106,
-            "heading": 0.5861133793107512,
-            "angularVelocity": 0.010285140296820981,
-            "velocityX": 3.7460749363537476,
-            "velocityY": 0.4975031678094133,
-            "timestamp": 1.7855113965499783
-        },
-        {
-            "x": 5.9191942381460025,
-            "y": 1.554048616452813,
-            "heading": 0.586114299955562,
-            "angularVelocity": 0.00002515518530364309,
-            "velocityX": 3.69763630242715,
-            "velocityY": 0.7987831211428086,
-            "timestamp": 1.8221100063237767
-        },
-        {
-            "x": 6.0517259817313,
-            "y": 1.5940956419716419,
-            "heading": 0.5861150757223451,
-            "angularVelocity": 0.000021196618886827452,
-            "velocityX": 3.6212234400274688,
-            "velocityY": 1.0942225884082484,
-            "timestamp": 1.8587086160975752
-        },
-        {
-            "x": 6.180598624537076,
-            "y": 1.644694580267861,
-            "heading": 0.5861160659567196,
-            "angularVelocity": 0.00002705661173256264,
-            "velocityX": 3.521244211249743,
-            "velocityY": 1.3825371676397282,
-            "timestamp": 1.8953072258713737
-        },
-        {
-            "x": 6.304973627271485,
-            "y": 1.7055153509385155,
-            "heading": 0.5861182648046722,
-            "angularVelocity": 0.00006008009501406976,
-            "velocityX": 3.3983532025703247,
-            "velocityY": 1.6618328140485057,
-            "timestamp": 1.9319058356451722
-        },
-        {
-            "x": 6.427040241028168,
-            "y": 1.7708422617715522,
-            "heading": 0.5861256207319434,
-            "angularVelocity": 0.0002009892538702712,
-            "velocityX": 3.335280069683768,
-            "velocityY": 1.784956074473775,
-            "timestamp": 1.9685044454189706
-        },
-        {
-            "x": 6.5491066329086145,
-            "y": 1.8361695861725622,
-            "heading": 0.5861329781592151,
-            "angularVelocity": 0.0002010302390486887,
-            "velocityX": 3.3352740072612326,
-            "velocityY": 1.7849673745744064,
-            "timestamp": 2.005103055192769
-        },
-        {
-            "x": 6.6711729055545455,
-            "y": 1.9014971320931282,
-            "heading": 0.586140337461326,
-            "angularVelocity": 0.00020108146611328895,
-            "velocityX": 3.3352707493638243,
-            "velocityY": 1.7849734272511966,
-            "timestamp": 2.0417016649665674
-        },
-        {
-            "x": 6.789839361593639,
-            "y": 1.9681130226753558,
-            "heading": 0.59368067703471,
-            "angularVelocity": 0.20602803275828666,
-            "velocityX": 3.2423760567006434,
-            "velocityY": 1.820175438191616,
-            "timestamp": 2.0783002747403656
-        },
-        {
-            "x": 6.899503815249364,
-            "y": 2.0364566539738247,
-            "heading": 0.622995969771871,
-            "angularVelocity": 0.8009947076773526,
-            "velocityX": 2.9964103645881037,
-            "velocityY": 1.8673832618472042,
-            "timestamp": 2.114898884514164
-        },
-        {
-            "x": 7.0002752102658246,
-            "y": 2.1033967313761632,
-            "heading": 0.6764261486140803,
-            "angularVelocity": 1.4598964051487269,
-            "velocityX": 2.753421390574382,
-            "velocityY": 1.829033338043951,
-            "timestamp": 2.151497494287962
-        },
-        {
-            "x": 7.092625779879376,
-            "y": 2.166299323632935,
-            "heading": 0.7535384043827013,
-            "angularVelocity": 2.1069722660292705,
-            "velocityX": 2.5233354541151716,
-            "velocityY": 1.7187153458983286,
-            "timestamp": 2.1880961040617604
-        },
-        {
-            "x": 7.177366976931162,
-            "y": 2.223194562450384,
-            "heading": 0.8522168962516827,
-            "angularVelocity": 2.696236072322813,
-            "velocityX": 2.3154212024865664,
-            "velocityY": 1.5545737712196184,
-            "timestamp": 2.2246947138355586
-        },
-        {
-            "x": 7.2553185898803205,
-            "y": 2.2728300819136558,
-            "heading": 0.9705292362405307,
-            "angularVelocity": 3.2327003872576037,
-            "velocityX": 2.1299063934654243,
-            "velocityY": 1.3562132488105336,
-            "timestamp": 2.261293323609357
-        },
-        {
-            "x": 7.327198001580843,
-            "y": 2.3144960913605033,
-            "heading": 1.1077214572134357,
-            "angularVelocity": 3.748563724710752,
-            "velocityX": 1.9639929534148923,
-            "velocityY": 1.1384588022432733,
-            "timestamp": 2.297891933383155
-        },
-        {
-            "x": 7.393635990725555,
-            "y": 2.347893157970517,
-            "heading": 1.2645826825431576,
-            "angularVelocity": 4.285988629055024,
-            "velocityX": 1.8153145585403265,
-            "velocityY": 0.9125228203046082,
-            "timestamp": 2.3344905431569534
-        },
-        {
-            "x": 7.455288023435831,
-            "y": 2.373088898730826,
-            "heading": 1.443531300083093,
-            "angularVelocity": 4.889492214211044,
-            "velocityX": 1.684545754369454,
-            "velocityY": 0.6884343672077653,
-            "timestamp": 2.3710891529307516
-        },
-        {
-            "x": 7.509792212794572,
-            "y": 2.3938656260636906,
-            "heading": 1.6501307836273436,
-            "angularVelocity": 5.64500905420071,
-            "velocityX": 1.4892420694559032,
-            "velocityY": 0.5676917090916096,
-            "timestamp": 2.40768776270455
-        },
-        {
-            "x": 7.556730666272478,
-            "y": 2.4114332825613682,
-            "heading": 1.888437501864436,
-            "angularVelocity": 6.51135984973125,
-            "velocityX": 1.2825201221580125,
-            "velocityY": 0.48000884750149375,
-            "timestamp": 2.444286372478348
-        },
-        {
-            "x": 7.593462104085248,
-            "y": 2.4287428163755616,
-            "heading": 2.1429512418884697,
-            "angularVelocity": 6.954191473312333,
-            "velocityX": 1.0036293192499064,
-            "velocityY": 0.4729560472700227,
-            "timestamp": 2.4808849822521464
-        },
-        {
-            "x": 7.620818492305724,
-            "y": 2.4405113575302453,
-            "heading": 2.4064355131818194,
-            "angularVelocity": 7.199297266257987,
-            "velocityX": 0.7474706932737378,
-            "velocityY": 0.32155705441874516,
-            "timestamp": 2.5174835920259446
-        },
-        {
-            "x": 7.6400541302749785,
-            "y": 2.446864161128428,
-            "heading": 2.650140298596015,
-            "angularVelocity": 6.658853626420207,
-            "velocityX": 0.5255838428875319,
-            "velocityY": 0.17358046213904924,
-            "timestamp": 2.554082201799743
-        },
-        {
-            "x": 7.6561677382990485,
-            "y": 2.4508382476782002,
-            "heading": 2.845393638086096,
-            "angularVelocity": 5.334993342557683,
-            "velocityX": 0.4402792380273905,
-            "velocityY": 0.10858572427572141,
-            "timestamp": 2.590680811573541
-        },
-        {
-            "x": 7.668208670546753,
-            "y": 2.453890313590812,
-            "heading": 2.992258715743694,
-            "angularVelocity": 4.01285946557297,
-            "velocityX": 0.3289997167139778,
-            "velocityY": 0.08339294665767145,
-            "timestamp": 2.6272794213473394
-        },
-        {
-            "x": 7.676124567266394,
-            "y": 2.4559646616612736,
-            "heading": 3.0905802273604146,
-            "angularVelocity": 2.686482143021458,
-            "velocityX": 0.21628954674959436,
-            "velocityY": 0.056678329676522185,
-            "timestamp": 2.6638780311211376
+            "heading": 2.6122548161665473,
+            "angularVelocity": 0.0000036121430551488755,
+            "velocityX": 3.7391099202016678,
+            "velocityY": -0.5741841152508433,
+            "timestamp": 1.7560871229720325
+        },
+        {
+            "x": 5.787436421990979,
+            "y": 1.496717214982701,
+            "heading": 2.612254832075579,
+            "angularVelocity": 4.2677202387530557e-7,
+            "velocityX": 3.773625708136921,
+            "velocityY": -0.26528339929021244,
+            "timestamp": 1.7933647115888978
+        },
+        {
+            "x": 5.926771623718837,
+            "y": 1.4982765413849686,
+            "heading": 2.6080354125638485,
+            "angularVelocity": -0.11318917527356848,
+            "velocityX": 3.7377740057150746,
+            "velocityY": 0.04183013065287323,
+            "timestamp": 1.830642300205763
+        },
+        {
+            "x": 6.064421169286406,
+            "y": 1.511276136415188,
+            "heading": 2.60121232475526,
+            "angularVelocity": -0.18303458087688948,
+            "velocityX": 3.6925549820916683,
+            "velocityY": 0.34872413996054913,
+            "timestamp": 1.8679198888226283
+        },
+        {
+            "x": 6.200023778520788,
+            "y": 1.5356760041928015,
+            "heading": 2.5932728220304684,
+            "angularVelocity": -0.2129832700927332,
+            "velocityX": 3.6376443398227996,
+            "velocityY": 0.6545452289951588,
+            "timestamp": 1.9051974774394935
+        },
+        {
+            "x": 6.333175231231244,
+            "y": 1.5714002898334831,
+            "heading": 2.5855391112934316,
+            "angularVelocity": -0.20746274166290535,
+            "velocityX": 3.5718901798871894,
+            "velocityY": 0.9583314523868992,
+            "timestamp": 1.9424750660563588
+        },
+        {
+            "x": 6.463368734714764,
+            "y": 1.6183219287773098,
+            "heading": 2.5790588590244545,
+            "angularVelocity": -0.17383775371256402,
+            "velocityX": 3.4925409157130343,
+            "velocityY": 1.258709071181673,
+            "timestamp": 1.979752654673224
+        },
+        {
+            "x": 6.589861303526444,
+            "y": 1.6762129661852994,
+            "heading": 2.574283804517795,
+            "angularVelocity": -0.12809451157737914,
+            "velocityX": 3.393260495247066,
+            "velocityY": 1.5529716260079764,
+            "timestamp": 2.017030243290089
+        },
+        {
+            "x": 6.711340738368746,
+            "y": 1.7445488061219798,
+            "heading": 2.570091844488755,
+            "angularVelocity": -0.11245255352014773,
+            "velocityX": 3.258779318878531,
+            "velocityY": 1.8331614911851861,
+            "timestamp": 2.054307831906954
+        },
+        {
+            "x": 6.825305386557433,
+            "y": 1.8215960437325822,
+            "heading": 2.560973344137785,
+            "angularVelocity": -0.24461078866147912,
+            "velocityX": 3.057189384216965,
+            "velocityY": 2.0668514372666396,
+            "timestamp": 2.091585420523819
+        },
+        {
+            "x": 6.929260415482823,
+            "y": 1.9025407238929235,
+            "heading": 2.5356026772435865,
+            "angularVelocity": -0.680587662333947,
+            "velocityX": 2.788673645010342,
+            "velocityY": 2.171403332771371,
+            "timestamp": 2.128863009140684
+        },
+        {
+            "x": 7.023455735639986,
+            "y": 1.9838668818491878,
+            "heading": 2.488274688562586,
+            "angularVelocity": -1.2696097155701718,
+            "velocityX": 2.5268619471418052,
+            "velocityY": 2.1816367681967113,
+            "timestamp": 2.166140597757549
+        },
+        {
+            "x": 7.110101814938688,
+            "y": 2.060020595789651,
+            "heading": 2.4125158730705962,
+            "angularVelocity": -2.0322885224854694,
+            "velocityX": 2.3243477519224953,
+            "velocityY": 2.0428819772427436,
+            "timestamp": 2.2034181863744142
+        },
+        {
+            "x": 7.191835709933807,
+            "y": 2.1285351921147058,
+            "heading": 2.3080325133818067,
+            "angularVelocity": -2.8028465242925935,
+            "velocityX": 2.1925746280203615,
+            "velocityY": 1.8379567688575535,
+            "timestamp": 2.2406957749912793
+        },
+        {
+            "x": 7.274315415796274,
+            "y": 2.1867028423762775,
+            "heading": 2.1850264648931392,
+            "angularVelocity": -3.2997319046816447,
+            "velocityX": 2.2125815784434106,
+            "velocityY": 1.5603919786607532,
+            "timestamp": 2.2779733636081443
+        },
+        {
+            "x": 7.351412179448119,
+            "y": 2.2360068924730196,
+            "heading": 2.043511695780604,
+            "angularVelocity": -3.796242578000636,
+            "velocityX": 2.0681800114335913,
+            "velocityY": 1.322619083639869,
+            "timestamp": 2.3152509522250093
+        },
+        {
+            "x": 7.420172175937219,
+            "y": 2.2789013315216375,
+            "heading": 1.8838780266002726,
+            "angularVelocity": -4.282296014933475,
+            "velocityX": 1.8445398170951257,
+            "velocityY": 1.1506763350355667,
+            "timestamp": 2.3525285408418743
+        },
+        {
+            "x": 7.479537718839642,
+            "y": 2.3170603162912466,
+            "heading": 1.7061327210648876,
+            "angularVelocity": -4.768154597182533,
+            "velocityX": 1.592526370537947,
+            "velocityY": 1.0236441300375574,
+            "timestamp": 2.3898061294587394
+        },
+        {
+            "x": 7.529281054923766,
+            "y": 2.3515994880332096,
+            "heading": 1.5086439015102746,
+            "angularVelocity": -5.2977895535137876,
+            "velocityX": 1.3344032682848619,
+            "velocityY": 0.9265398601007353,
+            "timestamp": 2.4270837180756044
+        },
+        {
+            "x": 7.569719945861703,
+            "y": 2.383438536522646,
+            "heading": 1.288404786035775,
+            "angularVelocity": -5.908083748068905,
+            "velocityX": 1.0848043674059311,
+            "velocityY": 0.8541069760888933,
+            "timestamp": 2.4643613066924694
+        },
+        {
+            "x": 7.603202598509127,
+            "y": 2.4091886515491456,
+            "heading": 1.0408456353949866,
+            "angularVelocity": -6.640964714353526,
+            "velocityX": 0.8981979223912477,
+            "velocityY": 0.6907666504707783,
+            "timestamp": 2.5016388953093345
+        },
+        {
+            "x": 7.630132008026638,
+            "y": 2.427100499568469,
+            "heading": 0.7720308301798764,
+            "angularVelocity": -7.211164004677401,
+            "velocityX": 0.7224021326670985,
+            "velocityY": 0.48049910640464033,
+            "timestamp": 2.5389164839261995
+        },
+        {
+            "x": 7.649623319500903,
+            "y": 2.438227448776781,
+            "heading": 0.5254269726030665,
+            "angularVelocity": -6.615338242807932,
+            "velocityX": 0.5228694289911082,
+            "velocityY": 0.29848897477445496,
+            "timestamp": 2.5761940725430645
+        },
+        {
+            "x": 7.66290885295536,
+            "y": 2.4458359453096237,
+            "heading": 0.3183730749926382,
+            "angularVelocity": -5.554380132752273,
+            "velocityX": 0.3563946582222695,
+            "velocityY": 0.2041037742822349,
+            "timestamp": 2.6134716611599296
+        },
+        {
+            "x": 7.671732744074589,
+            "y": 2.451418069701132,
+            "heading": 0.15988099310288126,
+            "angularVelocity": -4.251672057404801,
+            "velocityX": 0.23670766931625417,
+            "velocityY": 0.14974478228408286,
+            "timestamp": 2.6507492497767946
+        },
+        {
+            "x": 7.677275184086734,
+            "y": 2.4551558590330185,
+            "heading": 0.05341064749493927,
+            "angularVelocity": -2.8561489505727446,
+            "velocityX": 0.14868021827027583,
+            "velocityY": 0.10026907508163654,
+            "timestamp": 2.6880268383936596
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 1.350318302936367,
-            "velocityX": 0.10589016242853574,
-            "velocityY": 0.028289007290860937,
-            "timestamp": 2.700476640894936
+            "heading": 1.0247603884557974e-38,
+            "angularVelocity": -1.4327817189005392,
+            "velocityX": 0.07309528363731636,
+            "velocityY": 0.04947050051802475,
+            "timestamp": 2.7253044270105247
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -2.3806959148386727e-33,
-            "velocityX": 4.3916568461332404e-35,
-            "velocityY": 1.2700164208802373e-34,
-            "timestamp": 2.737075250668734
+            "heading": 0,
+            "angularVelocity": -2.6319896813971663e-37,
+            "velocityX": 5.227147746061896e-36,
+            "velocityY": -6.467829854732196e-35,
+            "timestamp": 2.7625820156273897
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.traj
index f2e9d5e3..ce59bb95 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.traj	
@@ -3,587 +3,623 @@
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
+            "heading": -0.39,
             "angularVelocity": 0,
             "velocityX": 0,
             "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 2.291384520253252,
-            "y": 4.354032854764245,
-            "heading": 2.74598551663602,
-            "angularVelocity": -0.1129108392120503,
-            "velocityX": -0.24231786763597,
-            "velocityY": -0.1678311541441644,
-            "timestamp": 0.03555445510807641
-        },
-        {
-            "x": 2.274618850931762,
-            "y": 4.341572242021881,
-            "heading": 2.7363109696234753,
-            "angularVelocity": -0.2721050564025352,
-            "velocityX": -0.47154904415004567,
-            "velocityY": -0.3504655803186264,
-            "timestamp": 0.07110891021615282
-        },
-        {
-            "x": 2.250443983775186,
-            "y": 4.321957452674385,
-            "heading": 2.718567267740621,
-            "angularVelocity": -0.4990570613139176,
-            "velocityX": -0.6799391829544631,
-            "velocityY": -0.5516830250350497,
-            "timestamp": 0.10666336532422922
-        },
-        {
-            "x": 2.2201183125080237,
-            "y": 4.294438762451095,
-            "heading": 2.688674932435733,
-            "angularVelocity": -0.8407479516708394,
-            "velocityX": -0.8529359028279262,
-            "velocityY": -0.7739871174973922,
-            "timestamp": 0.14221782043230563
-        },
-        {
-            "x": 2.1856076192251765,
-            "y": 4.258557396291605,
-            "heading": 2.63987728987899,
-            "angularVelocity": -1.3724761751631571,
-            "velocityX": -0.9706432900727432,
-            "velocityY": -1.009194657896466,
-            "timestamp": 0.17777227554038205
-        },
-        {
-            "x": 2.1489602975624615,
-            "y": 4.2143768004023965,
-            "heading": 2.56722210682279,
-            "angularVelocity": -2.0434902696538804,
-            "velocityX": -1.0307378231874669,
-            "velocityY": -1.2426177185084084,
-            "timestamp": 0.21332673064845847
-        },
-        {
-            "x": 2.1162123400405237,
-            "y": 4.163459966768879,
-            "heading": 2.4666019501957965,
-            "angularVelocity": -2.8300294947885973,
-            "velocityX": -0.9210648123390461,
-            "velocityY": -1.4320802689492316,
-            "timestamp": 0.2488811857565349
-        },
-        {
-            "x": 2.0883593879928255,
-            "y": 4.106985689754014,
-            "heading": 2.334003256666977,
-            "angularVelocity": -3.7294536824078297,
-            "velocityX": -0.7833885222831486,
-            "velocityY": -1.588388201793505,
-            "timestamp": 0.2844356408646113
-        },
-        {
-            "x": 2.0671900205502793,
-            "y": 4.044210359842601,
-            "heading": 2.181383778591042,
-            "angularVelocity": -4.2925556758502195,
-            "velocityX": -0.5954068872155824,
-            "velocityY": -1.765610799563443,
-            "timestamp": 0.31999009597268774
-        },
-        {
-            "x": 2.0546372297458078,
-            "y": 3.9756937078562617,
-            "heading": 2.021719977202011,
-            "angularVelocity": -4.49068340110105,
-            "velocityX": -0.35305816855621736,
-            "velocityY": -1.927090480730638,
-            "timestamp": 0.35554455108076416
-        },
-        {
-            "x": 2.0513227865299846,
-            "y": 3.902144544449752,
-            "heading": 1.8643846688260233,
-            "angularVelocity": -4.42519250816047,
-            "velocityX": -0.0932216006615165,
-            "velocityY": -2.068634245214532,
-            "timestamp": 0.3910990061888406
-        },
-        {
-            "x": 2.056389020104272,
-            "y": 3.822243076047335,
-            "heading": 1.7096006131055106,
-            "angularVelocity": -4.353436306364677,
-            "velocityX": 0.14249222942348805,
-            "velocityY": -2.247298352893811,
-            "timestamp": 0.426653461296917
-        },
-        {
-            "x": 2.0690751873782185,
-            "y": 3.7373642087091223,
-            "heading": 1.5737353715537044,
-            "angularVelocity": -3.821328189078159,
-            "velocityX": 0.35680949786416494,
-            "velocityY": -2.3872920307793386,
-            "timestamp": 0.4622079164049934
-        },
-        {
-            "x": 2.089274580938585,
-            "y": 3.648200269765939,
-            "heading": 1.460663871168444,
-            "angularVelocity": -3.1802343768608408,
-            "velocityX": 0.5681255274188619,
-            "velocityY": -2.507813399815797,
-            "timestamp": 0.49776237151306985
-        },
-        {
-            "x": 2.1169367654773477,
-            "y": 3.55409496646177,
-            "heading": 1.3686138295927854,
-            "angularVelocity": -2.5889875487010094,
-            "velocityX": 0.7780230200315752,
-            "velocityY": -2.646793573916784,
-            "timestamp": 0.5333168266211462
-        },
-        {
-            "x": 2.1519045434306308,
-            "y": 3.4545849603291003,
-            "heading": 1.296733638399589,
-            "angularVelocity": -2.0216929488779694,
-            "velocityX": 0.9834991943200934,
-            "velocityY": -2.798805545751856,
-            "timestamp": 0.5688712817292226
-        },
-        {
-            "x": 2.1939849352834964,
-            "y": 3.3493550759903967,
-            "heading": 1.244789692250972,
-            "angularVelocity": -1.4609687025358928,
-            "velocityX": 1.1835476517626813,
-            "velocityY": -2.959682099439628,
-            "timestamp": 0.604425736837299
-        },
-        {
-            "x": 2.243083202431903,
-            "y": 3.2382839213654937,
-            "heading": 1.2128218877185057,
-            "angularVelocity": -0.8991223303884791,
-            "velocityX": 1.3809315034968324,
-            "velocityY": -3.123972911053636,
-            "timestamp": 0.6399801919453755
-        },
-        {
-            "x": 2.299610340542871,
-            "y": 3.1217794185343526,
-            "heading": 1.2005607333325377,
-            "angularVelocity": -0.3448556404168553,
-            "velocityX": 1.5898749661368734,
-            "velocityY": -3.276790559073325,
-            "timestamp": 0.6755346470534519
-        },
-        {
-            "x": 2.3661237313117724,
-            "y": 3.0048767909387704,
-            "heading": 1.2005598156629014,
-            "angularVelocity": -0.000025810257353301716,
-            "velocityX": 1.8707470151551455,
-            "velocityY": -3.2879881646400775,
-            "timestamp": 0.7110891021615283
-        },
-        {
-            "x": 2.4413914156279892,
-            "y": 2.893838891515528,
-            "heading": 1.1996733189380515,
-            "angularVelocity": -0.02493349208011108,
-            "velocityX": 2.116969141769213,
-            "velocityY": -3.123037579558325,
-            "timestamp": 0.7466435572696047
-        },
-        {
-            "x": 2.522968201294396,
-            "y": 2.7909984002833275,
-            "heading": 1.191481222958301,
-            "angularVelocity": -0.2304098306344059,
-            "velocityX": 2.2944181093040097,
-            "velocityY": -2.89247833835709,
-            "timestamp": 0.7821980123776812
+            "x": 2.298402468716654,
+            "y": 4.3526843009931255,
+            "heading": -0.37533408415570846,
+            "angularVelocity": 0.4620245693037669,
+            "velocityX": -0.050327487964169394,
+            "velocityY": -0.2304685720750862,
+            "timestamp": 0.03174271850172795
+        },
+        {
+            "x": 2.2952744131725273,
+            "y": 4.338010072410149,
+            "heading": -0.34621912169362346,
+            "angularVelocity": 0.9172170449263883,
+            "velocityX": -0.09854403440450425,
+            "velocityY": -0.462286447903903,
+            "timestamp": 0.0634854370034559
+        },
+        {
+            "x": 2.2907034035575107,
+            "y": 4.3159283160610205,
+            "heading": -0.3028943022400045,
+            "angularVelocity": 1.364874260887912,
+            "velocityX": -0.14400183194038138,
+            "velocityY": -0.6956479278208441,
+            "timestamp": 0.09522815550518385
+        },
+        {
+            "x": 2.2847999846220546,
+            "y": 4.286378308280002,
+            "heading": -0.24568561151458213,
+            "angularVelocity": 1.8022618548662834,
+            "velocityX": -0.1859771063759033,
+            "velocityY": -0.9309224028625668,
+            "timestamp": 0.1269708740069118
+        },
+        {
+            "x": 2.2777013130983144,
+            "y": 4.249280679670162,
+            "heading": -0.1750916915555501,
+            "angularVelocity": 2.223940585151498,
+            "velocityX": -0.22363149278956135,
+            "velocityY": -1.1686972748670326,
+            "timestamp": 0.15871359250863973
+        },
+        {
+            "x": 2.2695785209744463,
+            "y": 4.204529290238701,
+            "heading": -0.09189311777806415,
+            "angularVelocity": 2.6210286233977427,
+            "velocityX": -0.25589465890975394,
+            "velocityY": -1.409815905623355,
+            "timestamp": 0.19045631101036767
+        },
+        {
+            "x": 2.2606569711878857,
+            "y": 4.151982062664549,
+            "heading": 0.002706319559794192,
+            "angularVelocity": 2.9801933105606198,
+            "velocityX": -0.2810581515277304,
+            "velocityY": -1.6554104391308149,
+            "timestamp": 0.2221990295120956
+        },
+        {
+            "x": 2.251280347316294,
+            "y": 4.091451352956859,
+            "heading": 0.10685098530745366,
+            "angularVelocity": 3.2808993893194844,
+            "velocityX": -0.2953944814487593,
+            "velocityY": -1.90691637530654,
+            "timestamp": 0.2539417480138236
+        },
+        {
+            "x": 2.242133674900212,
+            "y": 4.022709367456616,
+            "heading": 0.21739538615105536,
+            "angularVelocity": 3.4825120865934673,
+            "velocityX": -0.28815025454054716,
+            "velocityY": -2.165598560706175,
+            "timestamp": 0.2856844665155515
+        },
+        {
+            "x": 2.2351422077578347,
+            "y": 3.945850682832356,
+            "heading": 0.32707167243976176,
+            "angularVelocity": 3.455163623831899,
+            "velocityX": -0.22025420229828702,
+            "velocityY": -2.4213012700873704,
+            "timestamp": 0.31742718501727946
+        },
+        {
+            "x": 2.2344515310594897,
+            "y": 3.864378163543378,
+            "heading": 0.4256089749961829,
+            "angularVelocity": 3.1042490122910227,
+            "velocityX": -0.021758586880562782,
+            "velocityY": -2.566652231898267,
+            "timestamp": 0.3491699035190074
+        },
+        {
+            "x": 2.2416429003575074,
+            "y": 3.7806290345563274,
+            "heading": 0.5177286971400864,
+            "angularVelocity": 2.9020741288711216,
+            "velocityX": 0.22655177746121863,
+            "velocityY": -2.638372922674897,
+            "timestamp": 0.38091262202073534
+        },
+        {
+            "x": 2.256915898193716,
+            "y": 3.695399323053411,
+            "heading": 0.6036240465401915,
+            "angularVelocity": 2.705985922265222,
+            "velocityX": 0.4811496480800017,
+            "velocityY": -2.6850161399464656,
+            "timestamp": 0.4126553405224633
+        },
+        {
+            "x": 2.280071045497703,
+            "y": 3.6084777900686467,
+            "heading": 0.6811513337134051,
+            "angularVelocity": 2.4423644486842004,
+            "velocityX": 0.729463272111561,
+            "velocityY": -2.7383140791811433,
+            "timestamp": 0.4443980590241912
+        },
+        {
+            "x": 2.3096855944046597,
+            "y": 3.5244760350530098,
+            "heading": 0.76088689172136,
+            "angularVelocity": 2.511932240574098,
+            "velocityX": 0.932955660534971,
+            "velocityY": -2.6463314731869354,
+            "timestamp": 0.47614077752591916
+        },
+        {
+            "x": 2.3348942368332115,
+            "y": 3.446420745412462,
+            "heading": 0.8559069862879931,
+            "angularVelocity": 2.993445396350149,
+            "velocityX": 0.7941551202420774,
+            "velocityY": -2.458998262429792,
+            "timestamp": 0.5078834960276472
+        },
+        {
+            "x": 2.355075569752848,
+            "y": 3.373868279566975,
+            "heading": 0.9657696902827837,
+            "angularVelocity": 3.4610363944982643,
+            "velocityX": 0.6357783413709821,
+            "velocityY": -2.285641220097014,
+            "timestamp": 0.5396262145293751
+        },
+        {
+            "x": 2.370025604730125,
+            "y": 3.306495756130267,
+            "heading": 1.091029644988018,
+            "angularVelocity": 3.9461004166489144,
+            "velocityX": 0.4709752561508733,
+            "velocityY": -2.1224560030370347,
+            "timestamp": 0.571368933031103
+        },
+        {
+            "x": 2.3800631598495494,
+            "y": 3.2441375159647747,
+            "heading": 1.2334110423596254,
+            "angularVelocity": 4.485482154399005,
+            "velocityX": 0.31621598883778346,
+            "velocityY": -1.9644895934825695,
+            "timestamp": 0.603111651532831
+        },
+        {
+            "x": 2.3890320547539976,
+            "y": 3.1882487557001293,
+            "heading": 1.3963738258608078,
+            "angularVelocity": 5.133863487221998,
+            "velocityX": 0.28254967840764567,
+            "velocityY": -1.760679705539431,
+            "timestamp": 0.6348543700345589
+        },
+        {
+            "x": 2.399614951757013,
+            "y": 3.1248820965808815,
+            "heading": 1.5465533770663946,
+            "angularVelocity": 4.7311496397956585,
+            "velocityX": 0.33339605120587323,
+            "velocityY": -1.9962581061165,
+            "timestamp": 0.6665970885362869
+        },
+        {
+            "x": 2.4156041612276775,
+            "y": 3.056078363317597,
+            "heading": 1.6846738705269053,
+            "angularVelocity": 4.351249671731503,
+            "velocityX": 0.5037126694046519,
+            "velocityY": -2.167543818263053,
+            "timestamp": 0.6983398070380148
+        },
+        {
+            "x": 2.4386535219180887,
+            "y": 2.9838461830329104,
+            "heading": 1.8122491249375776,
+            "angularVelocity": 4.019039969866676,
+            "velocityX": 0.7261306459671242,
+            "velocityY": -2.275551171861821,
+            "timestamp": 0.7300825255397427
+        },
+        {
+            "x": 2.4695800740474128,
+            "y": 2.910121142766613,
+            "heading": 1.9308538821800434,
+            "angularVelocity": 3.736439814882526,
+            "velocityX": 0.9742880757878827,
+            "velocityY": -2.3225811696715355,
+            "timestamp": 0.7618252440414707
+        },
+        {
+            "x": 2.508646228883482,
+            "y": 2.8366969767650785,
+            "heading": 2.0420489888957944,
+            "angularVelocity": 3.503011461028351,
+            "velocityX": 1.2307123233299608,
+            "velocityY": -2.313102641084062,
+            "timestamp": 0.7935679625431986
+        },
+        {
+            "x": 2.555734299118471,
+            "y": 2.7651139971749186,
+            "heading": 2.147169748045839,
+            "angularVelocity": 3.3116495408016253,
+            "velocityX": 1.4834290337302587,
+            "velocityY": -2.255099215470886,
+            "timestamp": 0.8253106810449266
         },
         {
             "x": 2.6104869842529297,
             "y": 2.6966493129730225,
-            "heading": 1.1763516697745318,
-            "angularVelocity": -0.4255318535406987,
-            "velocityX": 2.4615419556423808,
-            "velocityY": -2.653650211302861,
-            "timestamp": 0.8177524674857576
-        },
-        {
-            "x": 2.7952393085826728,
-            "y": 2.5500987298885027,
-            "heading": 1.131439771707853,
-            "angularVelocity": -0.6752506092749339,
-            "velocityX": 2.777752108882206,
-            "velocityY": -2.203388740562692,
-            "timestamp": 0.884263918863645
-        },
-        {
-            "x": 2.9911597283478026,
-            "y": 2.437065890505019,
-            "heading": 1.0475109017935547,
-            "angularVelocity": -1.2618709737283185,
-            "velocityX": 2.9456644789180824,
-            "velocityY": -1.6994492984566387,
-            "timestamp": 0.9507753702415324
-        },
-        {
-            "x": 3.2086737929320703,
-            "y": 2.3383672587643702,
-            "heading": 1.0088878551520015,
-            "angularVelocity": -0.5806976970343763,
-            "velocityX": 3.2703250354356728,
-            "velocityY": -1.4839344157427674,
-            "timestamp": 1.0172868216194197
-        },
-        {
-            "x": 3.4256073991681717,
-            "y": 2.2401993070344908,
-            "heading": 0.9691349707945731,
-            "angularVelocity": -0.5976848126733959,
-            "velocityX": 3.2615978413038147,
-            "velocityY": -1.4759556391595554,
-            "timestamp": 1.083798272997307
-        },
-        {
-            "x": 3.641898030119061,
-            "y": 2.142612874932996,
-            "heading": 0.9280602960666862,
-            "angularVelocity": -0.6175579374222298,
-            "velocityX": 3.251930704714679,
-            "velocityY": -1.4672124886743745,
-            "timestamp": 1.1503097243751943
-        },
-        {
-            "x": 3.8574674811397935,
-            "y": 2.045673041019181,
-            "heading": 0.8854247472051884,
-            "angularVelocity": -0.6410256877310082,
-            "velocityX": 3.2410877609025053,
-            "velocityY": -1.4574908817287378,
-            "timestamp": 1.2168211757530816
-        },
-        {
-            "x": 4.072209689171901,
-            "y": 1.949463725749032,
-            "heading": 0.8409091268268268,
-            "angularVelocity": -0.6692925722736718,
-            "velocityX": 3.2286501584823513,
-            "velocityY": -1.446507530312815,
-            "timestamp": 1.2833326271309689
-        },
-        {
-            "x": 4.28597464141242,
-            "y": 1.8540976935596791,
-            "heading": 0.7940685595150717,
-            "angularVelocity": -0.7042481609013259,
-            "velocityX": 3.2139571128286692,
-            "velocityY": -1.4338287650276498,
-            "timestamp": 1.3498440785088561
-        },
-        {
-            "x": 4.498537652945423,
-            "y": 1.7597351759166577,
-            "heading": 0.7442459258691415,
-            "angularVelocity": -0.7490835429655718,
-            "velocityX": 3.195885928354179,
-            "velocityY": -1.4187409188666942,
-            "timestamp": 1.4163555298867434
-        },
-        {
-            "x": 4.709533812148138,
-            "y": 1.6666229170102642,
-            "heading": 0.6903878152301356,
-            "angularVelocity": -0.8097569594897124,
-            "velocityX": 3.1723282958288084,
-            "velocityY": -1.399943272585839,
-            "timestamp": 1.4828669812646307
-        },
-        {
-            "x": 4.924860666444789,
-            "y": 1.5757431293365876,
-            "heading": 0.6452055578690052,
-            "angularVelocity": -0.679315462602457,
-            "velocityX": 3.2374403179576423,
-            "velocityY": -1.3663780565746433,
-            "timestamp": 1.549378432642518
-        },
-        {
-            "x": 5.15821860461426,
-            "y": 1.5166188222705783,
-            "heading": 0.6185000496396688,
-            "angularVelocity": -0.4015174481399904,
-            "velocityX": 3.5085377530500543,
-            "velocityY": -0.8889342487820968,
-            "timestamp": 1.6158898840204052
-        },
-        {
-            "x": 5.39961745545984,
-            "y": 1.4934726786236598,
-            "heading": 0.5961273041444063,
-            "angularVelocity": -0.33637433903149927,
-            "velocityX": 3.6294329148534703,
-            "velocityY": -0.3480023840618476,
-            "timestamp": 1.6824013353982925
+            "heading": 2.247054267099527,
+            "angularVelocity": 3.1466907614812087,
+            "velocityX": 1.7248896036260801,
+            "velocityY": -2.1568626580665695,
+            "timestamp": 0.8570533995466545
+        },
+        {
+            "x": 2.751018773357776,
+            "y": 2.5737681266542305,
+            "heading": 2.423101617636397,
+            "angularVelocity": 2.7414576820608,
+            "velocityX": 2.188399607493837,
+            "velocityY": -1.9135395743649004,
+            "timestamp": 0.9212700940770386
+        },
+        {
+            "x": 2.920945282929504,
+            "y": 2.4615612619997704,
+            "heading": 2.5486951706813996,
+            "angularVelocity": 1.9557772937936253,
+            "velocityX": 2.646142265876451,
+            "velocityY": -1.7473161064272718,
+            "timestamp": 0.9854867886074228
+        },
+        {
+            "x": 3.118708267205298,
+            "y": 2.3583606370011725,
+            "heading": 2.6122511958810777,
+            "angularVelocity": 0.9897118757741349,
+            "velocityX": 3.079619493373657,
+            "velocityY": -1.6070684695514719,
+            "timestamp": 1.049703483137807
+        },
+        {
+            "x": 3.3434611246912245,
+            "y": 2.26616504003377,
+            "heading": 2.6122519169792926,
+            "angularVelocity": 0.000011229139401682067,
+            "velocityX": 3.499913209945475,
+            "velocityY": -1.4356951512629466,
+            "timestamp": 1.113920177668191
+        },
+        {
+            "x": 3.5722970045795748,
+            "y": 2.184629282343257,
+            "heading": 2.6122522165022306,
+            "angularVelocity": 0.000004664253435080619,
+            "velocityX": 3.5634951559220656,
+            "velocityY": -1.269697207039109,
+            "timestamp": 1.1781368721985752
+        },
+        {
+            "x": 3.80113295504209,
+            "y": 2.103093722723815,
+            "heading": 2.6122525160244625,
+            "angularVelocity": 0.000004664242440601037,
+            "velocityX": 3.56349625492233,
+            "velocityY": -1.2696941226220015,
+            "timestamp": 1.2423535667289594
+        },
+        {
+            "x": 4.029968923086795,
+            "y": 2.0215582124500746,
+            "heading": 2.612252815546594,
+            "angularVelocity": 0.00000466424087155042,
+            "velocityX": 3.563496528717025,
+            "velocityY": -1.2696933541971844,
+            "timestamp": 1.3065702612593435
+        },
+        {
+            "x": 4.258804909490432,
+            "y": 1.940022753702107,
+            "heading": 2.6122531150686714,
+            "angularVelocity": 0.000004664240033500949,
+            "velocityX": 3.5634968146073587,
+            "velocityY": -1.2696925518237052,
+            "timestamp": 1.3707869557897276
+        },
+        {
+            "x": 4.487640916105967,
+            "y": 1.8584873516805105,
+            "heading": 2.6122534145907514,
+            "angularVelocity": 0.000004664240074560689,
+            "velocityX": 3.563497129352575,
+            "velocityY": -1.2696916684650783,
+            "timestamp": 1.4350036503201118
+        },
+        {
+            "x": 4.716476945604857,
+            "y": 1.7769520138830965,
+            "heading": 2.6122537141129194,
+            "angularVelocity": 0.000004664241438058459,
+            "velocityX": 3.5634974856984654,
+            "velocityY": -1.2696906683484759,
+            "timestamp": 1.499220344850496
+        },
+        {
+            "x": 4.945312999982612,
+            "y": 1.6954167459104819,
+            "heading": 2.612254013635151,
+            "angularVelocity": 0.000004664242435524347,
+            "velocityX": 3.563497873118986,
+            "velocityY": -1.269689581017522,
+            "timestamp": 1.56343703938088
+        },
+        {
+            "x": 5.174149135873993,
+            "y": 1.613881706713074,
+            "heading": 2.6122543131567744,
+            "angularVelocity": 0.0000046642329655920085,
+            "velocityX": 3.563499142471556,
+            "velocityY": -1.269686018467187,
+            "timestamp": 1.6276537339112642
+        },
+        {
+            "x": 5.406651475687904,
+            "y": 1.5434785463415874,
+            "heading": 2.61225458420666,
+            "angularVelocity": 0.000004220863245612629,
+            "velocityX": 3.6205902766281897,
+            "velocityY": -1.096337344772168,
+            "timestamp": 1.6918704284416484
         },
         {
             "x": 5.646764755249023,
             "y": 1.5066063404083252,
-            "heading": 0.585736957474559,
-            "angularVelocity": -0.15621891350429284,
-            "velocityX": 3.7158608731150236,
-            "velocityY": 0.19746466980619687,
-            "timestamp": 1.7489127867761798
-        },
-        {
-            "x": 5.78386589002804,
-            "y": 1.5248142647082106,
-            "heading": 0.5861133793107512,
-            "angularVelocity": 0.010285140296820981,
-            "velocityX": 3.7460749363537476,
-            "velocityY": 0.4975031678094133,
-            "timestamp": 1.7855113965499783
-        },
-        {
-            "x": 5.9191942381460025,
-            "y": 1.554048616452813,
-            "heading": 0.586114299955562,
-            "angularVelocity": 0.00002515518530364309,
-            "velocityX": 3.69763630242715,
-            "velocityY": 0.7987831211428086,
-            "timestamp": 1.8221100063237767
-        },
-        {
-            "x": 6.0517259817313,
-            "y": 1.5940956419716419,
-            "heading": 0.5861150757223451,
-            "angularVelocity": 0.000021196618886827452,
-            "velocityX": 3.6212234400274688,
-            "velocityY": 1.0942225884082484,
-            "timestamp": 1.8587086160975752
-        },
-        {
-            "x": 6.180598624537076,
-            "y": 1.644694580267861,
-            "heading": 0.5861160659567196,
-            "angularVelocity": 0.00002705661173256264,
-            "velocityX": 3.521244211249743,
-            "velocityY": 1.3825371676397282,
-            "timestamp": 1.8953072258713737
-        },
-        {
-            "x": 6.304973627271485,
-            "y": 1.7055153509385155,
-            "heading": 0.5861182648046722,
-            "angularVelocity": 0.00006008009501406976,
-            "velocityX": 3.3983532025703247,
-            "velocityY": 1.6618328140485057,
-            "timestamp": 1.9319058356451722
-        },
-        {
-            "x": 6.427040241028168,
-            "y": 1.7708422617715522,
-            "heading": 0.5861256207319434,
-            "angularVelocity": 0.0002009892538702712,
-            "velocityX": 3.335280069683768,
-            "velocityY": 1.784956074473775,
-            "timestamp": 1.9685044454189706
-        },
-        {
-            "x": 6.5491066329086145,
-            "y": 1.8361695861725622,
-            "heading": 0.5861329781592151,
-            "angularVelocity": 0.0002010302390486887,
-            "velocityX": 3.3352740072612326,
-            "velocityY": 1.7849673745744064,
-            "timestamp": 2.005103055192769
-        },
-        {
-            "x": 6.6711729055545455,
-            "y": 1.9014971320931282,
-            "heading": 0.586140337461326,
-            "angularVelocity": 0.00020108146611328895,
-            "velocityX": 3.3352707493638243,
-            "velocityY": 1.7849734272511966,
-            "timestamp": 2.0417016649665674
-        },
-        {
-            "x": 6.789839361593639,
-            "y": 1.9681130226753558,
-            "heading": 0.59368067703471,
-            "angularVelocity": 0.20602803275828666,
-            "velocityX": 3.2423760567006434,
-            "velocityY": 1.820175438191616,
-            "timestamp": 2.0783002747403656
-        },
-        {
-            "x": 6.899503815249364,
-            "y": 2.0364566539738247,
-            "heading": 0.622995969771871,
-            "angularVelocity": 0.8009947076773526,
-            "velocityX": 2.9964103645881037,
-            "velocityY": 1.8673832618472042,
-            "timestamp": 2.114898884514164
-        },
-        {
-            "x": 7.0002752102658246,
-            "y": 2.1033967313761632,
-            "heading": 0.6764261486140803,
-            "angularVelocity": 1.4598964051487269,
-            "velocityX": 2.753421390574382,
-            "velocityY": 1.829033338043951,
-            "timestamp": 2.151497494287962
-        },
-        {
-            "x": 7.092625779879376,
-            "y": 2.166299323632935,
-            "heading": 0.7535384043827013,
-            "angularVelocity": 2.1069722660292705,
-            "velocityX": 2.5233354541151716,
-            "velocityY": 1.7187153458983286,
-            "timestamp": 2.1880961040617604
-        },
-        {
-            "x": 7.177366976931162,
-            "y": 2.223194562450384,
-            "heading": 0.8522168962516827,
-            "angularVelocity": 2.696236072322813,
-            "velocityX": 2.3154212024865664,
-            "velocityY": 1.5545737712196184,
-            "timestamp": 2.2246947138355586
-        },
-        {
-            "x": 7.2553185898803205,
-            "y": 2.2728300819136558,
-            "heading": 0.9705292362405307,
-            "angularVelocity": 3.2327003872576037,
-            "velocityX": 2.1299063934654243,
-            "velocityY": 1.3562132488105336,
-            "timestamp": 2.261293323609357
-        },
-        {
-            "x": 7.327198001580843,
-            "y": 2.3144960913605033,
-            "heading": 1.1077214572134357,
-            "angularVelocity": 3.748563724710752,
-            "velocityX": 1.9639929534148923,
-            "velocityY": 1.1384588022432733,
-            "timestamp": 2.297891933383155
-        },
-        {
-            "x": 7.393635990725555,
-            "y": 2.347893157970517,
-            "heading": 1.2645826825431576,
-            "angularVelocity": 4.285988629055024,
-            "velocityX": 1.8153145585403265,
-            "velocityY": 0.9125228203046082,
-            "timestamp": 2.3344905431569534
-        },
-        {
-            "x": 7.455288023435831,
-            "y": 2.373088898730826,
-            "heading": 1.443531300083093,
-            "angularVelocity": 4.889492214211044,
-            "velocityX": 1.684545754369454,
-            "velocityY": 0.6884343672077653,
-            "timestamp": 2.3710891529307516
-        },
-        {
-            "x": 7.509792212794572,
-            "y": 2.3938656260636906,
-            "heading": 1.6501307836273436,
-            "angularVelocity": 5.64500905420071,
-            "velocityX": 1.4892420694559032,
-            "velocityY": 0.5676917090916096,
-            "timestamp": 2.40768776270455
-        },
-        {
-            "x": 7.556730666272478,
-            "y": 2.4114332825613682,
-            "heading": 1.888437501864436,
-            "angularVelocity": 6.51135984973125,
-            "velocityX": 1.2825201221580125,
-            "velocityY": 0.48000884750149375,
-            "timestamp": 2.444286372478348
-        },
-        {
-            "x": 7.593462104085248,
-            "y": 2.4287428163755616,
-            "heading": 2.1429512418884697,
-            "angularVelocity": 6.954191473312333,
-            "velocityX": 1.0036293192499064,
-            "velocityY": 0.4729560472700227,
-            "timestamp": 2.4808849822521464
-        },
-        {
-            "x": 7.620818492305724,
-            "y": 2.4405113575302453,
-            "heading": 2.4064355131818194,
-            "angularVelocity": 7.199297266257987,
-            "velocityX": 0.7474706932737378,
-            "velocityY": 0.32155705441874516,
-            "timestamp": 2.5174835920259446
-        },
-        {
-            "x": 7.6400541302749785,
-            "y": 2.446864161128428,
-            "heading": 2.650140298596015,
-            "angularVelocity": 6.658853626420207,
-            "velocityX": 0.5255838428875319,
-            "velocityY": 0.17358046213904924,
-            "timestamp": 2.554082201799743
-        },
-        {
-            "x": 7.6561677382990485,
-            "y": 2.4508382476782002,
-            "heading": 2.845393638086096,
-            "angularVelocity": 5.334993342557683,
-            "velocityX": 0.4402792380273905,
-            "velocityY": 0.10858572427572141,
-            "timestamp": 2.590680811573541
-        },
-        {
-            "x": 7.668208670546753,
-            "y": 2.453890313590812,
-            "heading": 2.992258715743694,
-            "angularVelocity": 4.01285946557297,
-            "velocityX": 0.3289997167139778,
-            "velocityY": 0.08339294665767145,
-            "timestamp": 2.6272794213473394
-        },
-        {
-            "x": 7.676124567266394,
-            "y": 2.4559646616612736,
-            "heading": 3.0905802273604146,
-            "angularVelocity": 2.686482143021458,
-            "velocityX": 0.21628954674959436,
-            "velocityY": 0.056678329676522185,
-            "timestamp": 2.6638780311211376
+            "heading": 2.6122548161665473,
+            "angularVelocity": 0.0000036121430551488755,
+            "velocityX": 3.7391099202016678,
+            "velocityY": -0.5741841152508433,
+            "timestamp": 1.7560871229720325
+        },
+        {
+            "x": 5.787436421990979,
+            "y": 1.496717214982701,
+            "heading": 2.612254832075579,
+            "angularVelocity": 4.2677202387530557e-7,
+            "velocityX": 3.773625708136921,
+            "velocityY": -0.26528339929021244,
+            "timestamp": 1.7933647115888978
+        },
+        {
+            "x": 5.926771623718837,
+            "y": 1.4982765413849686,
+            "heading": 2.6080354125638485,
+            "angularVelocity": -0.11318917527356848,
+            "velocityX": 3.7377740057150746,
+            "velocityY": 0.04183013065287323,
+            "timestamp": 1.830642300205763
+        },
+        {
+            "x": 6.064421169286406,
+            "y": 1.511276136415188,
+            "heading": 2.60121232475526,
+            "angularVelocity": -0.18303458087688948,
+            "velocityX": 3.6925549820916683,
+            "velocityY": 0.34872413996054913,
+            "timestamp": 1.8679198888226283
+        },
+        {
+            "x": 6.200023778520788,
+            "y": 1.5356760041928015,
+            "heading": 2.5932728220304684,
+            "angularVelocity": -0.2129832700927332,
+            "velocityX": 3.6376443398227996,
+            "velocityY": 0.6545452289951588,
+            "timestamp": 1.9051974774394935
+        },
+        {
+            "x": 6.333175231231244,
+            "y": 1.5714002898334831,
+            "heading": 2.5855391112934316,
+            "angularVelocity": -0.20746274166290535,
+            "velocityX": 3.5718901798871894,
+            "velocityY": 0.9583314523868992,
+            "timestamp": 1.9424750660563588
+        },
+        {
+            "x": 6.463368734714764,
+            "y": 1.6183219287773098,
+            "heading": 2.5790588590244545,
+            "angularVelocity": -0.17383775371256402,
+            "velocityX": 3.4925409157130343,
+            "velocityY": 1.258709071181673,
+            "timestamp": 1.979752654673224
+        },
+        {
+            "x": 6.589861303526444,
+            "y": 1.6762129661852994,
+            "heading": 2.574283804517795,
+            "angularVelocity": -0.12809451157737914,
+            "velocityX": 3.393260495247066,
+            "velocityY": 1.5529716260079764,
+            "timestamp": 2.017030243290089
+        },
+        {
+            "x": 6.711340738368746,
+            "y": 1.7445488061219798,
+            "heading": 2.570091844488755,
+            "angularVelocity": -0.11245255352014773,
+            "velocityX": 3.258779318878531,
+            "velocityY": 1.8331614911851861,
+            "timestamp": 2.054307831906954
+        },
+        {
+            "x": 6.825305386557433,
+            "y": 1.8215960437325822,
+            "heading": 2.560973344137785,
+            "angularVelocity": -0.24461078866147912,
+            "velocityX": 3.057189384216965,
+            "velocityY": 2.0668514372666396,
+            "timestamp": 2.091585420523819
+        },
+        {
+            "x": 6.929260415482823,
+            "y": 1.9025407238929235,
+            "heading": 2.5356026772435865,
+            "angularVelocity": -0.680587662333947,
+            "velocityX": 2.788673645010342,
+            "velocityY": 2.171403332771371,
+            "timestamp": 2.128863009140684
+        },
+        {
+            "x": 7.023455735639986,
+            "y": 1.9838668818491878,
+            "heading": 2.488274688562586,
+            "angularVelocity": -1.2696097155701718,
+            "velocityX": 2.5268619471418052,
+            "velocityY": 2.1816367681967113,
+            "timestamp": 2.166140597757549
+        },
+        {
+            "x": 7.110101814938688,
+            "y": 2.060020595789651,
+            "heading": 2.4125158730705962,
+            "angularVelocity": -2.0322885224854694,
+            "velocityX": 2.3243477519224953,
+            "velocityY": 2.0428819772427436,
+            "timestamp": 2.2034181863744142
+        },
+        {
+            "x": 7.191835709933807,
+            "y": 2.1285351921147058,
+            "heading": 2.3080325133818067,
+            "angularVelocity": -2.8028465242925935,
+            "velocityX": 2.1925746280203615,
+            "velocityY": 1.8379567688575535,
+            "timestamp": 2.2406957749912793
+        },
+        {
+            "x": 7.274315415796274,
+            "y": 2.1867028423762775,
+            "heading": 2.1850264648931392,
+            "angularVelocity": -3.2997319046816447,
+            "velocityX": 2.2125815784434106,
+            "velocityY": 1.5603919786607532,
+            "timestamp": 2.2779733636081443
+        },
+        {
+            "x": 7.351412179448119,
+            "y": 2.2360068924730196,
+            "heading": 2.043511695780604,
+            "angularVelocity": -3.796242578000636,
+            "velocityX": 2.0681800114335913,
+            "velocityY": 1.322619083639869,
+            "timestamp": 2.3152509522250093
+        },
+        {
+            "x": 7.420172175937219,
+            "y": 2.2789013315216375,
+            "heading": 1.8838780266002726,
+            "angularVelocity": -4.282296014933475,
+            "velocityX": 1.8445398170951257,
+            "velocityY": 1.1506763350355667,
+            "timestamp": 2.3525285408418743
+        },
+        {
+            "x": 7.479537718839642,
+            "y": 2.3170603162912466,
+            "heading": 1.7061327210648876,
+            "angularVelocity": -4.768154597182533,
+            "velocityX": 1.592526370537947,
+            "velocityY": 1.0236441300375574,
+            "timestamp": 2.3898061294587394
+        },
+        {
+            "x": 7.529281054923766,
+            "y": 2.3515994880332096,
+            "heading": 1.5086439015102746,
+            "angularVelocity": -5.2977895535137876,
+            "velocityX": 1.3344032682848619,
+            "velocityY": 0.9265398601007353,
+            "timestamp": 2.4270837180756044
+        },
+        {
+            "x": 7.569719945861703,
+            "y": 2.383438536522646,
+            "heading": 1.288404786035775,
+            "angularVelocity": -5.908083748068905,
+            "velocityX": 1.0848043674059311,
+            "velocityY": 0.8541069760888933,
+            "timestamp": 2.4643613066924694
+        },
+        {
+            "x": 7.603202598509127,
+            "y": 2.4091886515491456,
+            "heading": 1.0408456353949866,
+            "angularVelocity": -6.640964714353526,
+            "velocityX": 0.8981979223912477,
+            "velocityY": 0.6907666504707783,
+            "timestamp": 2.5016388953093345
+        },
+        {
+            "x": 7.630132008026638,
+            "y": 2.427100499568469,
+            "heading": 0.7720308301798764,
+            "angularVelocity": -7.211164004677401,
+            "velocityX": 0.7224021326670985,
+            "velocityY": 0.48049910640464033,
+            "timestamp": 2.5389164839261995
+        },
+        {
+            "x": 7.649623319500903,
+            "y": 2.438227448776781,
+            "heading": 0.5254269726030665,
+            "angularVelocity": -6.615338242807932,
+            "velocityX": 0.5228694289911082,
+            "velocityY": 0.29848897477445496,
+            "timestamp": 2.5761940725430645
+        },
+        {
+            "x": 7.66290885295536,
+            "y": 2.4458359453096237,
+            "heading": 0.3183730749926382,
+            "angularVelocity": -5.554380132752273,
+            "velocityX": 0.3563946582222695,
+            "velocityY": 0.2041037742822349,
+            "timestamp": 2.6134716611599296
+        },
+        {
+            "x": 7.671732744074589,
+            "y": 2.451418069701132,
+            "heading": 0.15988099310288126,
+            "angularVelocity": -4.251672057404801,
+            "velocityX": 0.23670766931625417,
+            "velocityY": 0.14974478228408286,
+            "timestamp": 2.6507492497767946
+        },
+        {
+            "x": 7.677275184086734,
+            "y": 2.4551558590330185,
+            "heading": 0.05341064749493927,
+            "angularVelocity": -2.8561489505727446,
+            "velocityX": 0.14868021827027583,
+            "velocityY": 0.10026907508163654,
+            "timestamp": 2.6880268383936596
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 1.350318302936367,
-            "velocityX": 0.10589016242853574,
-            "velocityY": 0.028289007290860937,
-            "timestamp": 2.700476640894936
+            "heading": 1.0247603884557974e-38,
+            "angularVelocity": -1.4327817189005392,
+            "velocityX": 0.07309528363731636,
+            "velocityY": 0.04947050051802475,
+            "timestamp": 2.7253044270105247
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -2.3806959148386727e-33,
-            "velocityX": 4.3916568461332404e-35,
-            "velocityY": 1.2700164208802373e-34,
-            "timestamp": 2.737075250668734
+            "heading": 0,
+            "angularVelocity": -2.6319896813971663e-37,
+            "velocityX": 5.227147746061896e-36,
+            "velocityY": -6.467829854732196e-35,
+            "timestamp": 2.7625820156273897
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.1.traj
index a8fe3b09..8826d851 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.1.traj	
@@ -3,533 +3,569 @@
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
+            "heading": -0.39,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": 2.0996459321900828e-33,
+            "velocityY": -4.609910300520975e-35,
             "timestamp": 0
         },
         {
-            "x": 2.2983894677520005,
-            "y": 4.3533419612939515,
-            "heading": 2.758567124910339,
-            "angularVelocity": 0.29099245314259387,
-            "velocityX": -0.05470361814673814,
-            "velocityY": -0.226148099446212,
-            "timestamp": 0.02944105531884974
-        },
-        {
-            "x": 2.295387593245339,
-            "y": 4.339939574817211,
-            "heading": 2.775252952592925,
-            "angularVelocity": 0.5667537220346529,
-            "velocityX": -0.10196219103394874,
-            "velocityY": -0.45522778757728155,
-            "timestamp": 0.05888211063769948
-        },
-        {
-            "x": 2.2912544329244677,
-            "y": 4.31970469118274,
-            "heading": 2.7995524599349486,
-            "angularVelocity": 0.8253612881351188,
-            "velocityX": -0.14038764154710928,
-            "velocityY": -0.687301573103468,
-            "timestamp": 0.08832316595654921
-        },
-        {
-            "x": 2.286299680545296,
-            "y": 4.292550560538348,
-            "heading": 2.8308875928825827,
-            "angularVelocity": 1.064334569813193,
-            "velocityX": -0.1682939801413744,
-            "velocityY": -0.922321919180889,
-            "timestamp": 0.11776422127539896
-        },
-        {
-            "x": 2.280893923712392,
-            "y": 4.258397506462172,
-            "heading": 2.868587408224874,
-            "angularVelocity": 1.2805184778194243,
-            "velocityX": -0.18361287577360647,
-            "velocityY": -1.1600485684461586,
-            "timestamp": 0.1472052765942487
-        },
-        {
-            "x": 2.2754825429318224,
-            "y": 4.217182700090041,
-            "heading": 2.9118655493454413,
-            "angularVelocity": 1.4699928603733934,
-            "velocityX": -0.18380389975713432,
-            "velocityY": -1.3999092738276546,
-            "timestamp": 0.17664633191309845
-        },
-        {
-            "x": 2.2706016921590138,
-            "y": 4.168876446645306,
-            "heading": 2.959795755168493,
-            "angularVelocity": 1.6280056982999451,
-            "velocityX": -0.16578382534010866,
-            "velocityY": -1.6407785971519722,
-            "timestamp": 0.2060873872319482
-        },
-        {
-            "x": 2.2668942123670868,
-            "y": 4.113508113408168,
-            "heading": 3.0112851676807657,
-            "angularVelocity": 1.748898331076693,
-            "velocityX": -0.12592890274395677,
-            "velocityY": -1.880650426334687,
-            "timestamp": 0.23552844255079794
-        },
-        {
-            "x": 2.2651203704726766,
-            "y": 4.051204935167073,
-            "heading": 3.0650426993920417,
-            "angularVelocity": 1.8259376618492857,
-            "velocityX": -0.06025062196987639,
-            "velocityY": -2.1162005765875382,
-            "timestamp": 0.2649694978696477
-        },
-        {
-            "x": 2.2661533431015752,
-            "y": 3.9822449997835685,
-            "heading": 3.1195407017354904,
-            "angularVelocity": 1.8510886159897915,
-            "velocityX": 0.03508612778011459,
-            "velocityY": -2.3423051462205073,
-            "timestamp": 0.2944105531884974
-        },
-        {
-            "x": 2.2709425124463385,
-            "y": 3.9071174668882147,
-            "heading": 3.1729959824105403,
-            "angularVelocity": 1.8156713506402375,
-            "velocityX": 0.16266975802653263,
-            "velocityY": -2.5517948348561,
-            "timestamp": 0.3238516085073471
-        },
-        {
-            "x": 2.2797199160454222,
-            "y": 3.825989687229236,
-            "heading": 3.2244176609184314,
-            "angularVelocity": 1.7465976661158726,
-            "velocityX": 0.2981348156179574,
-            "velocityY": -2.7556002589022834,
-            "timestamp": 0.35329266382619684
-        },
-        {
-            "x": 2.292782229397934,
-            "y": 3.7391196184482327,
-            "heading": 3.272453259481913,
-            "angularVelocity": 1.6315854864321535,
-            "velocityX": 0.4436768047559834,
-            "velocityY": -2.9506438488767315,
-            "timestamp": 0.38273371914504656
-        },
-        {
-            "x": 2.31050581940653,
-            "y": 3.6469279173399998,
-            "heading": 3.3152109067306803,
-            "angularVelocity": 1.4523136750940884,
-            "velocityX": 0.6020025374990144,
-            "velocityY": -3.131399337074948,
-            "timestamp": 0.4121747744638963
-        },
-        {
-            "x": 2.3333478257637266,
-            "y": 3.5501341924969156,
-            "heading": 3.350152812747203,
-            "angularVelocity": 1.186842850505799,
-            "velocityX": 0.7758555564606926,
-            "velocityY": -3.287712474800853,
-            "timestamp": 0.441615829782746
-        },
-        {
-            "x": 2.3619457015355163,
-            "y": 3.450260447984075,
-            "heading": 3.374288635448379,
-            "angularVelocity": 0.8198015471858178,
-            "velocityX": 0.9713604170119489,
-            "velocityY": -3.3923289580212845,
-            "timestamp": 0.4710568851015957
-        },
-        {
-            "x": 2.396816597338257,
-            "y": 3.3487121183154565,
-            "heading": 3.38694435959391,
-            "angularVelocity": 0.42986652511155254,
-            "velocityX": 1.184430905247282,
-            "velocityY": -3.4492082083620734,
-            "timestamp": 0.5004979404204455
-        },
-        {
-            "x": 2.4384933115651135,
-            "y": 3.2467782441206525,
-            "heading": 3.390761607070638,
-            "angularVelocity": 0.12965729099677573,
-            "velocityX": 1.4155985162723614,
-            "velocityY": -3.462303680722349,
-            "timestamp": 0.5299389957392953
-        },
-        {
-            "x": 2.487106839227099,
-            "y": 3.1465743963564163,
-            "heading": 3.3907619234819757,
-            "angularVelocity": 0.000010747282459786006,
-            "velocityX": 1.6512155266004023,
-            "velocityY": -3.403541302409769,
-            "timestamp": 0.559380051058145
-        },
-        {
-            "x": 2.5421170689241235,
-            "y": 3.049734421677047,
-            "heading": 3.39076189132422,
-            "angularVelocity": -0.0000010922759119129131,
-            "velocityX": 1.8684870192748764,
-            "velocityY": -3.2892834047754955,
-            "timestamp": 0.5888211063769948
-        },
-        {
-            "x": 2.603292311228317,
-            "y": 2.9566661889623873,
-            "heading": 3.3907618600542158,
-            "angularVelocity": -0.0000010621224075785883,
-            "velocityX": 2.077888908589026,
-            "velocityY": -3.161171761905971,
-            "timestamp": 0.6182621616958446
-        },
-        {
-            "x": 2.670374957735173,
-            "y": 2.8677616065314258,
-            "heading": 3.3907618290531647,
-            "angularVelocity": -0.0000010529870817317445,
-            "velocityX": 2.2785408260792437,
-            "velocityY": -3.019748492984228,
-            "timestamp": 0.6477032170146944
+            "x": 2.298808348352727,
+            "y": 4.35514296819253,
+            "heading": -0.3837399750205753,
+            "angularVelocity": 0.24884159467028183,
+            "velocityX": -0.047369219319970686,
+            "velocityY": -0.19307136062676852,
+            "timestamp": 0.02515666638336474
+        },
+        {
+            "x": 2.2965596540188837,
+            "y": 4.3453750993410925,
+            "heading": -0.37149402212882315,
+            "angularVelocity": 0.48678758564965857,
+            "velocityX": -0.08938761199815681,
+            "velocityY": -0.38828152755153583,
+            "timestamp": 0.05031333276672948
+        },
+        {
+            "x": 2.2934096466067952,
+            "y": 4.330641358670927,
+            "heading": -0.3535645425221108,
+            "angularVelocity": 0.7127128584321681,
+            "velocityX": -0.1252156133919033,
+            "velocityY": -0.5856793760205414,
+            "timestamp": 0.07546999915009422
+        },
+        {
+            "x": 2.289538880665139,
+            "y": 4.310886769515238,
+            "heading": -0.33028900557963653,
+            "angularVelocity": 0.9252234214095049,
+            "velocityX": -0.15386640990779057,
+            "velocityY": -0.7852625961900879,
+            "timestamp": 0.10062666553345896
+        },
+        {
+            "x": 2.2851573432175654,
+            "y": 4.286058471237428,
+            "heading": -0.3020481691352866,
+            "angularVelocity": 1.1225985197714665,
+            "velocityX": -0.17417003432819553,
+            "velocityY": -0.9869470739663532,
+            "timestamp": 0.1257833319168237
+        },
+        {
+            "x": 2.280510060472114,
+            "y": 4.25610900707676,
+            "heading": -0.26927550139535,
+            "angularVelocity": 1.3027428690475438,
+            "velocityX": -0.18473364771909595,
+            "velocityY": -1.1905180004483138,
+            "timestamp": 0.15093999830018845
+        },
+        {
+            "x": 2.2758836751536,
+            "y": 4.2210014881511375,
+            "heading": -0.23246752663964654,
+            "angularVelocity": 1.463149933889621,
+            "velocityX": -0.1839029562984313,
+            "velocityY": -1.3955552930033008,
+            "timestamp": 0.1760966646835532
+        },
+        {
+            "x": 2.271613693612169,
+            "y": 4.180717494993525,
+            "heading": -0.19219487185489256,
+            "angularVelocity": 1.6008740653882818,
+            "velocityX": -0.16973558723403512,
+            "velocityY": -1.6013247758554696,
+            "timestamp": 0.20125333106691795
+        },
+        {
+            "x": 2.268091592763015,
+            "y": 4.135268758759812,
+            "heading": -0.1491141249151469,
+            "angularVelocity": 1.7124982413501877,
+            "velocityX": -0.1400066604803655,
+            "velocityY": -1.8066279347634933,
+            "timestamp": 0.2264099974502827
+        },
+        {
+            "x": 2.2657701315686967,
+            "y": 4.084713672798125,
+            "heading": -0.10398137252966654,
+            "angularVelocity": 1.7940672940403972,
+            "velocityX": -0.09228015981695388,
+            "velocityY": -2.0096099058307106,
+            "timestamp": 0.25156666383364745
+        },
+        {
+            "x": 2.265163909208295,
+            "y": 4.029179243747037,
+            "heading": -0.057668550112221675,
+            "angularVelocity": 1.8409761337881332,
+            "velocityX": -0.02409788129965802,
+            "velocityY": -2.2075432493635576,
+            "timestamp": 0.2767233302170122
+        },
+        {
+            "x": 2.266840363517046,
+            "y": 3.9688874958283327,
+            "heading": -0.011179237419756825,
+            "angularVelocity": 1.8479917801512262,
+            "velocityX": 0.06664055893588815,
+            "velocityY": -2.3966509314037507,
+            "timestamp": 0.3018799966003769
+        },
+        {
+            "x": 2.2713096946462064,
+            "y": 3.9041248350083095,
+            "heading": 0.03446639703260334,
+            "angularVelocity": 1.8144548151476838,
+            "velocityX": 0.1776599117328043,
+            "velocityY": -2.574373719995273,
+            "timestamp": 0.3270366629837416
+        },
+        {
+            "x": 2.2787160490905882,
+            "y": 3.8349972418338227,
+            "heading": 0.07865637806724814,
+            "angularVelocity": 1.7565912892125506,
+            "velocityX": 0.29440921668697884,
+            "velocityY": -2.74788368701343,
+            "timestamp": 0.35219332936710634
+        },
+        {
+            "x": 2.289237241442295,
+            "y": 3.7616571782461574,
+            "heading": 0.12059430680114623,
+            "angularVelocity": 1.667070195025133,
+            "velocityX": 0.41822681079334173,
+            "velocityY": -2.915333155436007,
+            "timestamp": 0.37734999575047107
+        },
+        {
+            "x": 2.303092280593543,
+            "y": 3.684333317171128,
+            "heading": 0.15922184993862323,
+            "angularVelocity": 1.5354794052927534,
+            "velocityX": 0.550750204343837,
+            "velocityY": -3.073692670431113,
+            "timestamp": 0.4025066621338358
+        },
+        {
+            "x": 2.3205444941015814,
+            "y": 3.6033821039148743,
+            "heading": 0.19314125064776663,
+            "angularVelocity": 1.3483265307192482,
+            "velocityX": 0.6937411039317692,
+            "velocityY": -3.217883165544707,
+            "timestamp": 0.4276633285172005
+        },
+        {
+            "x": 2.341904587688066,
+            "y": 3.5193842561187316,
+            "heading": 0.22066827104785883,
+            "angularVelocity": 1.0942236932591152,
+            "velocityX": 0.8490828339882706,
+            "velocityY": -3.3389896147642064,
+            "timestamp": 0.45281999490056524
+        },
+        {
+            "x": 2.3675851289681646,
+            "y": 3.4337423994061407,
+            "heading": 0.23918679984335672,
+            "angularVelocity": 0.7361280907928144,
+            "velocityX": 1.0208244959308244,
+            "velocityY": -3.40434044032252,
+            "timestamp": 0.47797666128392996
+        },
+        {
+            "x": 2.3978885412195137,
+            "y": 3.3469640370766824,
+            "heading": 0.24945072521405584,
+            "angularVelocity": 0.40800021808479553,
+            "velocityX": 1.2045877537807594,
+            "velocityY": -3.4495175555868487,
+            "timestamp": 0.5031333276672947
+        },
+        {
+            "x": 2.4331457577160354,
+            "y": 3.2598611919071945,
+            "heading": 0.25313177220508776,
+            "angularVelocity": 0.1463249118518414,
+            "velocityX": 1.4015059054023244,
+            "velocityY": -3.462416038839161,
+            "timestamp": 0.5282899940506595
+        },
+        {
+            "x": 2.473498104501041,
+            "y": 3.173715186571369,
+            "heading": 0.2532432496800442,
+            "angularVelocity": 0.004431329384332257,
+            "velocityX": 1.604041893710102,
+            "velocityY": -3.424380799229854,
+            "timestamp": 0.5534466604340242
+        },
+        {
+            "x": 2.5185817721335604,
+            "y": 3.089905547060991,
+            "heading": 0.25324321969105484,
+            "angularVelocity": -0.0000011920891614956396,
+            "velocityX": 1.7921161311870697,
+            "velocityY": -3.3315081669882356,
+            "timestamp": 0.578603326817389
+        },
+        {
+            "x": 2.568241478882672,
+            "y": 3.0087236123099395,
+            "heading": 0.2532431890633549,
+            "angularVelocity": -0.0000012174784783024135,
+            "velocityX": 1.97401778090718,
+            "velocityY": -3.2270545514223747,
+            "timestamp": 0.6037599932007538
+        },
+        {
+            "x": 2.6223245423144714,
+            "y": 2.9304189809174512,
+            "heading": 0.25324315880142845,
+            "angularVelocity": -0.0000012029386552177117,
+            "velocityX": 2.1498501672527754,
+            "velocityY": -3.1126791681853656,
+            "timestamp": 0.6289166595841186
+        },
+        {
+            "x": 2.6806646810204167,
+            "y": 2.855232404738762,
+            "heading": 0.25324312847978453,
+            "angularVelocity": -0.0000012053124772832063,
+            "velocityX": 2.319072718813166,
+            "velocityY": -2.9887336832676343,
+            "timestamp": 0.6540733259674834
         },
         {
             "x": 2.7430825233459473,
             "y": 2.7833950519561768,
-            "heading": 3.3907617949090247,
-            "angularVelocity": -0.0000011597457911507082,
-            "velocityX": 2.4695978056269863,
-            "velocityY": -2.8656090504076794,
-            "timestamp": 0.6771442723335441
-        },
-        {
-            "x": 2.8743019890864545,
-            "y": 2.6600421338324565,
-            "heading": 3.3907617697237464,
-            "angularVelocity": -5.290207073739654e-7,
-            "velocityX": 2.7562853520145434,
-            "velocityY": -2.5910472919087915,
-            "timestamp": 0.7247516311451417
-        },
-        {
-            "x": 3.0177249974626466,
-            "y": 2.5511186165163586,
-            "heading": 3.390761748438384,
-            "angularVelocity": -4.4710235312140834e-7,
-            "velocityX": 3.0126226691923277,
-            "velocityY": -2.287955476529456,
-            "timestamp": 0.7723589899567394
-        },
-        {
-            "x": 3.171772290811784,
-            "y": 2.457823792449377,
-            "heading": 3.3907617293701717,
-            "angularVelocity": -4.005307751758197e-7,
-            "velocityX": 3.235787432753145,
-            "velocityY": -1.95967233629129,
-            "timestamp": 0.819966348768337
-        },
-        {
-            "x": 3.3347475520740515,
-            "y": 2.3811846546739925,
-            "heading": 3.3907617114029063,
-            "angularVelocity": -3.7740521255041523e-7,
-            "velocityX": 3.4233207917967023,
-            "velocityY": -1.6098170469543955,
-            "timestamp": 0.8675737075799346
-        },
-        {
-            "x": 3.5038153046646756,
-            "y": 2.3191315453490344,
-            "heading": 3.3907616937093077,
-            "angularVelocity": -3.7165679655198125e-7,
-            "velocityX": 3.551294522758467,
-            "velocityY": -1.3034352434994065,
-            "timestamp": 0.9151810663915322
-        },
-        {
-            "x": 3.6728836971491297,
-            "y": 2.257080179484392,
-            "heading": 3.3907616760157264,
-            "angularVelocity": -3.7165643530673955e-7,
-            "velocityX": 3.551307963828218,
-            "velocityY": -1.303398621843431,
-            "timestamp": 0.9627884252031298
-        },
-        {
-            "x": 3.8419520974106347,
-            "y": 2.195028834809519,
-            "heading": 3.3907616583221465,
-            "angularVelocity": -3.71656403732769e-7,
-            "velocityX": 3.5513081271863745,
-            "velocityY": -1.3033981767490248,
-            "timestamp": 1.0103957840147275
-        },
-        {
-            "x": 4.011020504671481,
-            "y": 2.1329775092054373,
-            "heading": 3.390761640628565,
-            "angularVelocity": -3.7165644168556193e-7,
-            "velocityX": 3.5513082742086395,
-            "velocityY": -1.3033977761640818,
-            "timestamp": 1.0580031428263252
-        },
-        {
-            "x": 4.180088918243229,
-            "y": 2.070926200796389,
-            "heading": 3.390761622934979,
-            "angularVelocity": -3.716565361157419e-7,
-            "velocityX": 3.55130840677009,
-            "velocityY": -1.3033974149797176,
-            "timestamp": 1.105610501637923
-        },
-        {
-            "x": 4.349157337505363,
-            "y": 2.008874907891698,
-            "heading": 3.3907616052413863,
-            "angularVelocity": -3.716566778801587e-7,
-            "velocityX": 3.551308526297583,
-            "velocityY": -1.3033970893082818,
-            "timestamp": 1.1532178604495207
-        },
-        {
-            "x": 4.518225761904485,
-            "y": 1.9468236289835406,
-            "heading": 3.390761587547785,
-            "angularVelocity": -3.716568562531517e-7,
-            "velocityX": 3.551308634200803,
-            "velocityY": -1.303396795308895,
-            "timestamp": 1.2008252192611184
-        },
-        {
-            "x": 4.687294190885785,
-            "y": 1.8847723625602526,
-            "heading": 3.3907615698541753,
-            "angularVelocity": -3.716570319734318e-7,
-            "velocityX": 3.5513087304501405,
-            "velocityY": -1.3033965330622725,
-            "timestamp": 1.2484325780727161
-        },
-        {
-            "x": 4.8563626239355635,
-            "y": 1.8227211072221874,
-            "heading": 3.390761552160558,
-            "angularVelocity": -3.716571921571891e-7,
-            "velocityX": 3.5513088159091875,
-            "velocityY": -1.3033963002154438,
-            "timestamp": 1.2960399368843138
-        },
-        {
-            "x": 5.025431060602707,
-            "y": 1.7606698617402123,
-            "heading": 3.3907615344669337,
-            "angularVelocity": -3.716573365191525e-7,
-            "velocityX": 3.551308891892495,
-            "velocityY": -1.3033960931867248,
-            "timestamp": 1.3436472956959116
-        },
-        {
-            "x": 5.1944995004938015,
-            "y": 1.6986186250424111,
-            "heading": 3.3907615167733036,
-            "angularVelocity": -3.7165746535883894e-7,
-            "velocityX": 3.551308959612086,
-            "velocityY": -1.3033959086737847,
-            "timestamp": 1.3912546545075093
-        },
-        {
-            "x": 5.363567943266909,
-            "y": 1.6365673961971205,
-            "heading": 3.3907614990796673,
-            "angularVelocity": -3.716575836762919e-7,
-            "velocityX": 3.551309020149218,
-            "velocityY": -1.3033957437305734,
-            "timestamp": 1.438862013319107
-        },
-        {
-            "x": 5.532636388625086,
-            "y": 1.574516174395274,
-            "heading": 3.390761481386026,
-            "angularVelocity": -3.716576912991928e-7,
-            "velocityX": 3.5513090744490245,
-            "velocityY": -1.3033955957819334,
-            "timestamp": 1.4864693721307047
-        },
-        {
-            "x": 5.701704836310244,
-            "y": 1.5124649589336634,
-            "heading": 3.3907614636923804,
-            "angularVelocity": -3.7165778948932697e-7,
-            "velocityX": 3.551309123327601,
-            "velocityY": -1.3033954626042876,
-            "timestamp": 1.5340767309423025
-        },
-        {
-            "x": 5.870773286097621,
-            "y": 1.4504137491998943,
-            "heading": 3.3907614459987303,
-            "angularVelocity": -3.716578803925389e-7,
-            "velocityX": 3.551309167485058,
-            "velocityY": -1.3033953422900804,
-            "timestamp": 1.5816840897539002
-        },
-        {
-            "x": 6.039841737790961,
-            "y": 1.388362544659232,
-            "heading": 3.3907614283050767,
-            "angularVelocity": -3.716579611998838e-7,
-            "velocityX": 3.551309207520092,
-            "velocityY": -1.3033952332080676,
-            "timestamp": 1.629291448565498
-        },
-        {
-            "x": 6.208910191218329,
-            "y": 1.3263113448432136,
-            "heading": 3.390761410611419,
-            "angularVelocity": -3.7165803844263857e-7,
-            "velocityX": 3.551309243943619,
-            "velocityY": -1.303395133966182,
-            "timestamp": 1.6768988073770956
-        },
-        {
-            "x": 6.377978646228594,
-            "y": 1.2642601493400643,
-            "heading": 3.3907613929177582,
-            "angularVelocity": -3.7165810831214996e-7,
-            "velocityX": 3.5513092771926584,
-            "velocityY": -1.3033950433736952,
-            "timestamp": 1.7245061661886933
-        },
-        {
-            "x": 6.547047102688449,
-            "y": 1.2022089577865573,
-            "heading": 3.390761375224094,
-            "angularVelocity": -3.716581745376039e-7,
-            "velocityX": 3.551309307641484,
-            "velocityY": -1.3033949604108395,
-            "timestamp": 1.772113525000291
-        },
-        {
-            "x": 6.716115560480058,
-            "y": 1.1401577698616354,
-            "heading": 3.3907613575304274,
-            "angularVelocity": -3.716582357126045e-7,
-            "velocityX": 3.5513093356151892,
-            "velocityY": -1.3033948841918472,
-            "timestamp": 1.8197208838118888
-        },
-        {
-            "x": 6.885184026346865,
-            "y": 1.0781066039393965,
-            "heading": 3.3907613398363963,
-            "angularVelocity": -3.7166587620925834e-7,
-            "velocityX": 3.5513095052359795,
-            "velocityY": -1.3033944220220548,
-            "timestamp": 1.8673282426234865
-        },
-        {
-            "x": 7.044084736711506,
-            "y": 1.0199905851581765,
-            "heading": 3.36350523152736,
-            "angularVelocity": -0.5725188077939848,
-            "velocityX": 3.337734214441043,
-            "velocityY": -1.220736042325084,
-            "timestamp": 1.9149356014350842
-        },
-        {
-            "x": 7.185551132214723,
-            "y": 0.9681799151008591,
-            "heading": 3.323163851941908,
-            "angularVelocity": -0.8473769726461788,
-            "velocityX": 2.9715237105057524,
-            "velocityY": -1.0882912085577785,
-            "timestamp": 1.962542960246682
-        },
-        {
-            "x": 7.3092683658418975,
-            "y": 0.9228556276324137,
-            "heading": 3.282206369227669,
-            "angularVelocity": -0.8603183149967459,
-            "velocityX": 2.598699795902896,
-            "velocityY": -0.952043730210126,
-            "timestamp": 2.0101503190582797
-        },
-        {
-            "x": 7.415256126479253,
-            "y": 0.8840204066687376,
-            "heading": 3.244221221054594,
-            "angularVelocity": -0.7978839641870918,
-            "velocityX": 2.226289449427224,
-            "velocityY": -0.815739875790283,
-            "timestamp": 2.057757677869877
-        },
-        {
-            "x": 7.503541109453659,
-            "y": 0.8516687069454374,
-            "heading": 3.210907204397792,
-            "angularVelocity": -0.6997661178524867,
-            "velocityX": 1.8544398424576745,
-            "velocityY": -0.6795525005142472,
-            "timestamp": 2.1053650366814747
-        },
-        {
-            "x": 7.574144027018988,
-            "y": 0.8257950136289538,
-            "heading": 3.1832598866385293,
-            "angularVelocity": -0.5807362233362916,
-            "velocityX": 1.4830252996124917,
-            "velocityY": -0.5434809651776052,
-            "timestamp": 2.152972395493072
-        },
-        {
-            "x": 7.627080539080189,
-            "y": 0.8063947169930168,
-            "heading": 3.161935751906697,
-            "angularVelocity": -0.44791677724069034,
-            "velocityX": 1.1119396955141558,
-            "velocityY": -0.407506257860514,
-            "timestamp": 2.2005797543046697
-        },
-        {
-            "x": 7.662362703782539,
-            "y": 0.7934640657363813,
-            "heading": 3.147400394630243,
-            "angularVelocity": -0.3053174475395025,
-            "velocityX": 0.7411073746387762,
-            "velocityY": -0.27161034721139543,
-            "timestamp": 2.248187113116267
+            "heading": 0.2532430937019675,
+            "angularVelocity": -0.0000013824493495341482,
+            "velocityX": 2.4811650865953045,
+            "velocityY": -2.8555990562441647,
+            "timestamp": 0.6792299923508481
+        },
+        {
+            "x": 2.8747857816965947,
+            "y": 2.6605842580771375,
+            "heading": 0.2532430684120768,
+            "angularVelocity": -5.312693002073383e-7,
+            "velocityX": 2.7667141273306664,
+            "velocityY": -2.579908520631989,
+            "timestamp": 0.7268327631068262
+        },
+        {
+            "x": 3.0186320127358077,
+            "y": 2.5522491183421203,
+            "heading": 0.2532430469824583,
+            "angularVelocity": -4.5017586399077444e-7,
+            "velocityX": 3.021803747025554,
+            "velocityY": -2.2758158404343,
+            "timestamp": 0.7744355338628043
+        },
+        {
+            "x": 3.1730376030001257,
+            "y": 2.4595822138278454,
+            "heading": 0.2532430277420137,
+            "angularVelocity": -4.041874945252969e-7,
+            "velocityX": 3.2436261127705017,
+            "velocityY": -1.946670394236201,
+            "timestamp": 0.8220383046187824
+        },
+        {
+            "x": 3.336302611834685,
+            "y": 2.38360340882859,
+            "heading": 0.25324300957525164,
+            "angularVelocity": -3.816324501860361e-7,
+            "velocityX": 3.4297375182526793,
+            "velocityY": -1.5961004746706646,
+            "timestamp": 0.8696410753747604
+        },
+        {
+            "x": 3.505316504811254,
+            "y": 2.3214540241877932,
+            "heading": 0.25324299166355524,
+            "angularVelocity": -3.7627424124720095e-7,
+            "velocityX": 3.550505365390803,
+            "velocityY": -1.3055833442844713,
+            "timestamp": 0.9172438461307385
+        },
+        {
+            "x": 3.6743308817857088,
+            "y": 2.2593059557844626,
+            "heading": 0.25324297375187266,
+            "angularVelocity": -3.76273948990246e-7,
+            "velocityX": 3.5505155328218008,
+            "velocityY": -1.3055556938463668,
+            "timestamp": 0.9648466168867166
+        },
+        {
+            "x": 3.843345266545321,
+            "y": 2.1971579085532174,
+            "heading": 0.2532429558401917,
+            "angularVelocity": -3.762739157561073e-7,
+            "velocityX": 3.5505156963660025,
+            "velocityY": -1.3055552490805507,
+            "timestamp": 1.0124493876426945
+        },
+        {
+            "x": 4.012359658311276,
+            "y": 2.135009880376043,
+            "heading": 0.25324293792850894,
+            "angularVelocity": -3.7627395316214525e-7,
+            "velocityX": 3.5505158435494915,
+            "velocityY": -1.3055548488082471,
+            "timestamp": 1.0600521583986726
+        },
+        {
+            "x": 4.181374056391421,
+            "y": 2.07286186937061,
+            "heading": 0.25324292001682175,
+            "angularVelocity": -3.7627404706616014e-7,
+            "velocityX": 3.5505159761928384,
+            "velocityY": -1.30555448807838,
+            "timestamp": 1.1076549291546507
+        },
+        {
+            "x": 4.3503884601627245,
+            "y": 2.010713873842556,
+            "heading": 0.25324290210512795,
+            "angularVelocity": -3.7627418619029544e-7,
+            "velocityX": 3.5505160957480255,
+            "velocityY": -1.30555416294226,
+            "timestamp": 1.1552576999106288
+        },
+        {
+            "x": 4.51940286907005,
+            "y": 1.9485658922821711,
+            "heading": 0.2532428841934256,
+            "angularVelocity": -3.7627436389492574e-7,
+            "velocityX": 3.5505162036413855,
+            "velocityY": -1.3055538695209365,
+            "timestamp": 1.2028604706666068
+        },
+        {
+            "x": 4.688417282561973,
+            "y": 1.8864179231898264,
+            "heading": 0.25324286628171494,
+            "angularVelocity": -3.7627454024029507e-7,
+            "velocityX": 3.550516299950831,
+            "velocityY": -1.3055536076025602,
+            "timestamp": 1.250463241422585
+        },
+        {
+            "x": 4.857431700124039,
+            "y": 1.824269965166445,
+            "heading": 0.2532428483699966,
+            "angularVelocity": -3.7627470129766985e-7,
+            "velocityX": 3.5505163854530717,
+            "velocityY": -1.3055533750748494,
+            "timestamp": 1.298066012178563
+        },
+        {
+            "x": 5.02644612130461,
+            "y": 1.7621220169837726,
+            "heading": 0.2532428304582713,
+            "angularVelocity": -3.76274846815446e-7,
+            "velocityX": 3.5505164614676405,
+            "velocityY": -1.3055531683492934,
+            "timestamp": 1.345668782934541
+        },
+        {
+            "x": 5.1954605457099055,
+            "y": 1.6999740775709098,
+            "heading": 0.25324281254653974,
+            "angularVelocity": -3.7627497879654404e-7,
+            "velocityX": 3.5505165292100247,
+            "velocityY": -1.3055529841203253,
+            "timestamp": 1.3932715536905191
+        },
+        {
+            "x": 5.364474972997734,
+            "y": 1.6378261459972472,
+            "heading": 0.25324279463480254,
+            "angularVelocity": -3.7627509767034604e-7,
+            "velocityX": 3.550516589763906,
+            "velocityY": -1.3055528194408317,
+            "timestamp": 1.4408743244464972
+        },
+        {
+            "x": 5.533489402870974,
+            "y": 1.5756782214547507,
+            "heading": 0.2532427767230601,
+            "angularVelocity": -3.762752068122207e-7,
+            "velocityX": 3.5505166440761182,
+            "velocityY": -1.3055526717358554,
+            "timestamp": 1.4884770952024753
+        },
+        {
+            "x": 5.702503835071411,
+            "y": 1.513530303241192,
+            "heading": 0.25324275881131303,
+            "angularVelocity": -3.7627530496428226e-7,
+            "velocityX": 3.550516692963962,
+            "velocityY": -1.3055525387827163,
+            "timestamp": 1.5360798659584534
+        },
+        {
+            "x": 5.871518269374186,
+            "y": 1.4513823907450571,
+            "heading": 0.2532427408995616,
+            "angularVelocity": -3.762753964251323e-7,
+            "velocityX": 3.5505167371281625,
+            "velocityY": -1.3055524186757583,
+            "timestamp": 1.5836826367144314
+        },
+        {
+            "x": 6.040532705582966,
+            "y": 1.3892344834324082,
+            "heading": 0.25324272298780626,
+            "angularVelocity": -3.7627547903960503e-7,
+            "velocityX": 3.550516777167954,
+            "velocityY": -1.3055523097853368,
+            "timestamp": 1.6312854074704095
+        },
+        {
+            "x": 6.20954714352589,
+            "y": 1.3270865808358587,
+            "heading": 0.2532427050760471,
+            "angularVelocity": -3.7627555795965075e-7,
+            "velocityX": 3.5505168135974197,
+            "velocityY": -1.3055522107133783,
+            "timestamp": 1.6788881782263876
+        },
+        {
+            "x": 6.378561583051887,
+            "y": 1.2649386825445668,
+            "heading": 0.25324268716428455,
+            "angularVelocity": -3.7627562961930894e-7,
+            "velocityX": 3.5505168468533315,
+            "velocityY": -1.3055521202720615,
+            "timestamp": 1.7264909489823657
+        },
+        {
+            "x": 6.547576024027691,
+            "y": 1.2027907881961044,
+            "heading": 0.25324266925251887,
+            "angularVelocity": -3.7627569544162935e-7,
+            "velocityX": 3.5505168773096716,
+            "velocityY": -1.3055520374443248,
+            "timestamp": 1.7740937197383437
+        },
+        {
+            "x": 6.716590466335514,
+            "y": 1.1406428974701475,
+            "heading": 0.25324265134075025,
+            "angularVelocity": -3.762757576316506e-7,
+            "velocityX": 3.550516905291665,
+            "velocityY": -1.3055519613457025,
+            "timestamp": 1.8216964904943218
+        },
+        {
+            "x": 6.88560491686231,
+            "y": 1.0784950290966298,
+            "heading": 0.253242633428611,
+            "angularVelocity": -3.7628354454135297e-7,
+            "velocityX": 3.550517077949107,
+            "velocityY": -1.3055514917839766,
+            "timestamp": 1.8692992612502999
+        },
+        {
+            "x": 7.0444139114910005,
+            "y": 1.0203058482073266,
+            "heading": 0.2259027129119336,
+            "angularVelocity": -0.574334646544581,
+            "velocityX": 3.336129223292024,
+            "velocityY": -1.2223906290579827,
+            "timestamp": 1.916902032006278
+        },
+        {
+            "x": 7.185810534595712,
+            "y": 0.9684234171781619,
+            "heading": 0.18518533646408028,
+            "angularVelocity": -0.8553572786042075,
+            "velocityX": 2.9703443908662663,
+            "velocityY": -1.089903596055891,
+            "timestamp": 1.964504802762256
+        },
+        {
+            "x": 7.3094655243228,
+            "y": 0.9230369546143523,
+            "heading": 0.14380134057962407,
+            "angularVelocity": -0.8693610734677483,
+            "velocityX": 2.59764269523238,
+            "velocityY": -0.953441613650398,
+            "timestamp": 2.012107573518234
+        },
+        {
+            "x": 7.415398593682274,
+            "y": 0.8841490900710189,
+            "heading": 0.10540313701630608,
+            "angularVelocity": -0.8066379950897271,
+            "velocityX": 2.2253551143589885,
+            "velocityY": -0.8169243917057022,
+            "timestamp": 2.059710344274212
+        },
+        {
+            "x": 7.503637039429732,
+            "y": 0.8517540010892736,
+            "heading": 0.07171800478429124,
+            "angularVelocity": -0.70762965468317,
+            "velocityX": 1.8536409613588987,
+            "velocityY": -0.680529483206126,
+            "timestamp": 2.1073131150301903
+        },
+        {
+            "x": 7.574202084965293,
+            "y": 0.8258459262172375,
+            "heading": 0.04375776286271558,
+            "angularVelocity": -0.5873658502969467,
+            "velocityX": 1.482372652157004,
+            "velocityY": -0.5442556065663985,
+            "timestamp": 2.1549158857861683
+        },
+        {
+            "x": 7.627109788299601,
+            "y": 0.8064200554899893,
+            "heading": 0.022189516524924582,
+            "angularVelocity": -0.4530880449870122,
+            "velocityX": 1.1114416764840027,
+            "velocityY": -0.40808277372822227,
+            "timestamp": 2.2025186565421464
+        },
+        {
+            "x": 7.66237251850946,
+            "y": 0.7934724766877013,
+            "heading": 0.0074863600733853135,
+            "angularVelocity": -0.3088718622474896,
+            "velocityX": 0.7407705402406998,
+            "velocityY": -0.27199212559831976,
+            "timestamp": 2.2501214272981245
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": -0.1554464438896817,
-            "velocityX": 0.370474158989983,
-            "velocityY": -0.13577870937899197,
-            "timestamp": 2.2957944719278647
+            "heading": -1.0670900456397286e-32,
+            "angularVelocity": -0.1572673177316084,
+            "velocityX": 0.37030368633165434,
+            "velocityY": -0.13596848639085585,
+            "timestamp": 2.2977241980541026
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 1.845639156363882e-34,
-            "velocityX": 1.6232927575664752e-34,
-            "velocityY": 1.3237538522972492e-34,
-            "timestamp": 2.343401830739462
+            "heading": 0,
+            "angularVelocity": 2.230662363627099e-31,
+            "velocityX": 2.3924515989499496e-33,
+            "velocityY": 1.2585115119762557e-33,
+            "timestamp": 2.3453269688100806
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.traj
index a8fe3b09..8826d851 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.traj	
@@ -3,533 +3,569 @@
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
+            "heading": -0.39,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": 2.0996459321900828e-33,
+            "velocityY": -4.609910300520975e-35,
             "timestamp": 0
         },
         {
-            "x": 2.2983894677520005,
-            "y": 4.3533419612939515,
-            "heading": 2.758567124910339,
-            "angularVelocity": 0.29099245314259387,
-            "velocityX": -0.05470361814673814,
-            "velocityY": -0.226148099446212,
-            "timestamp": 0.02944105531884974
-        },
-        {
-            "x": 2.295387593245339,
-            "y": 4.339939574817211,
-            "heading": 2.775252952592925,
-            "angularVelocity": 0.5667537220346529,
-            "velocityX": -0.10196219103394874,
-            "velocityY": -0.45522778757728155,
-            "timestamp": 0.05888211063769948
-        },
-        {
-            "x": 2.2912544329244677,
-            "y": 4.31970469118274,
-            "heading": 2.7995524599349486,
-            "angularVelocity": 0.8253612881351188,
-            "velocityX": -0.14038764154710928,
-            "velocityY": -0.687301573103468,
-            "timestamp": 0.08832316595654921
-        },
-        {
-            "x": 2.286299680545296,
-            "y": 4.292550560538348,
-            "heading": 2.8308875928825827,
-            "angularVelocity": 1.064334569813193,
-            "velocityX": -0.1682939801413744,
-            "velocityY": -0.922321919180889,
-            "timestamp": 0.11776422127539896
-        },
-        {
-            "x": 2.280893923712392,
-            "y": 4.258397506462172,
-            "heading": 2.868587408224874,
-            "angularVelocity": 1.2805184778194243,
-            "velocityX": -0.18361287577360647,
-            "velocityY": -1.1600485684461586,
-            "timestamp": 0.1472052765942487
-        },
-        {
-            "x": 2.2754825429318224,
-            "y": 4.217182700090041,
-            "heading": 2.9118655493454413,
-            "angularVelocity": 1.4699928603733934,
-            "velocityX": -0.18380389975713432,
-            "velocityY": -1.3999092738276546,
-            "timestamp": 0.17664633191309845
-        },
-        {
-            "x": 2.2706016921590138,
-            "y": 4.168876446645306,
-            "heading": 2.959795755168493,
-            "angularVelocity": 1.6280056982999451,
-            "velocityX": -0.16578382534010866,
-            "velocityY": -1.6407785971519722,
-            "timestamp": 0.2060873872319482
-        },
-        {
-            "x": 2.2668942123670868,
-            "y": 4.113508113408168,
-            "heading": 3.0112851676807657,
-            "angularVelocity": 1.748898331076693,
-            "velocityX": -0.12592890274395677,
-            "velocityY": -1.880650426334687,
-            "timestamp": 0.23552844255079794
-        },
-        {
-            "x": 2.2651203704726766,
-            "y": 4.051204935167073,
-            "heading": 3.0650426993920417,
-            "angularVelocity": 1.8259376618492857,
-            "velocityX": -0.06025062196987639,
-            "velocityY": -2.1162005765875382,
-            "timestamp": 0.2649694978696477
-        },
-        {
-            "x": 2.2661533431015752,
-            "y": 3.9822449997835685,
-            "heading": 3.1195407017354904,
-            "angularVelocity": 1.8510886159897915,
-            "velocityX": 0.03508612778011459,
-            "velocityY": -2.3423051462205073,
-            "timestamp": 0.2944105531884974
-        },
-        {
-            "x": 2.2709425124463385,
-            "y": 3.9071174668882147,
-            "heading": 3.1729959824105403,
-            "angularVelocity": 1.8156713506402375,
-            "velocityX": 0.16266975802653263,
-            "velocityY": -2.5517948348561,
-            "timestamp": 0.3238516085073471
-        },
-        {
-            "x": 2.2797199160454222,
-            "y": 3.825989687229236,
-            "heading": 3.2244176609184314,
-            "angularVelocity": 1.7465976661158726,
-            "velocityX": 0.2981348156179574,
-            "velocityY": -2.7556002589022834,
-            "timestamp": 0.35329266382619684
-        },
-        {
-            "x": 2.292782229397934,
-            "y": 3.7391196184482327,
-            "heading": 3.272453259481913,
-            "angularVelocity": 1.6315854864321535,
-            "velocityX": 0.4436768047559834,
-            "velocityY": -2.9506438488767315,
-            "timestamp": 0.38273371914504656
-        },
-        {
-            "x": 2.31050581940653,
-            "y": 3.6469279173399998,
-            "heading": 3.3152109067306803,
-            "angularVelocity": 1.4523136750940884,
-            "velocityX": 0.6020025374990144,
-            "velocityY": -3.131399337074948,
-            "timestamp": 0.4121747744638963
-        },
-        {
-            "x": 2.3333478257637266,
-            "y": 3.5501341924969156,
-            "heading": 3.350152812747203,
-            "angularVelocity": 1.186842850505799,
-            "velocityX": 0.7758555564606926,
-            "velocityY": -3.287712474800853,
-            "timestamp": 0.441615829782746
-        },
-        {
-            "x": 2.3619457015355163,
-            "y": 3.450260447984075,
-            "heading": 3.374288635448379,
-            "angularVelocity": 0.8198015471858178,
-            "velocityX": 0.9713604170119489,
-            "velocityY": -3.3923289580212845,
-            "timestamp": 0.4710568851015957
-        },
-        {
-            "x": 2.396816597338257,
-            "y": 3.3487121183154565,
-            "heading": 3.38694435959391,
-            "angularVelocity": 0.42986652511155254,
-            "velocityX": 1.184430905247282,
-            "velocityY": -3.4492082083620734,
-            "timestamp": 0.5004979404204455
-        },
-        {
-            "x": 2.4384933115651135,
-            "y": 3.2467782441206525,
-            "heading": 3.390761607070638,
-            "angularVelocity": 0.12965729099677573,
-            "velocityX": 1.4155985162723614,
-            "velocityY": -3.462303680722349,
-            "timestamp": 0.5299389957392953
-        },
-        {
-            "x": 2.487106839227099,
-            "y": 3.1465743963564163,
-            "heading": 3.3907619234819757,
-            "angularVelocity": 0.000010747282459786006,
-            "velocityX": 1.6512155266004023,
-            "velocityY": -3.403541302409769,
-            "timestamp": 0.559380051058145
-        },
-        {
-            "x": 2.5421170689241235,
-            "y": 3.049734421677047,
-            "heading": 3.39076189132422,
-            "angularVelocity": -0.0000010922759119129131,
-            "velocityX": 1.8684870192748764,
-            "velocityY": -3.2892834047754955,
-            "timestamp": 0.5888211063769948
-        },
-        {
-            "x": 2.603292311228317,
-            "y": 2.9566661889623873,
-            "heading": 3.3907618600542158,
-            "angularVelocity": -0.0000010621224075785883,
-            "velocityX": 2.077888908589026,
-            "velocityY": -3.161171761905971,
-            "timestamp": 0.6182621616958446
-        },
-        {
-            "x": 2.670374957735173,
-            "y": 2.8677616065314258,
-            "heading": 3.3907618290531647,
-            "angularVelocity": -0.0000010529870817317445,
-            "velocityX": 2.2785408260792437,
-            "velocityY": -3.019748492984228,
-            "timestamp": 0.6477032170146944
+            "x": 2.298808348352727,
+            "y": 4.35514296819253,
+            "heading": -0.3837399750205753,
+            "angularVelocity": 0.24884159467028183,
+            "velocityX": -0.047369219319970686,
+            "velocityY": -0.19307136062676852,
+            "timestamp": 0.02515666638336474
+        },
+        {
+            "x": 2.2965596540188837,
+            "y": 4.3453750993410925,
+            "heading": -0.37149402212882315,
+            "angularVelocity": 0.48678758564965857,
+            "velocityX": -0.08938761199815681,
+            "velocityY": -0.38828152755153583,
+            "timestamp": 0.05031333276672948
+        },
+        {
+            "x": 2.2934096466067952,
+            "y": 4.330641358670927,
+            "heading": -0.3535645425221108,
+            "angularVelocity": 0.7127128584321681,
+            "velocityX": -0.1252156133919033,
+            "velocityY": -0.5856793760205414,
+            "timestamp": 0.07546999915009422
+        },
+        {
+            "x": 2.289538880665139,
+            "y": 4.310886769515238,
+            "heading": -0.33028900557963653,
+            "angularVelocity": 0.9252234214095049,
+            "velocityX": -0.15386640990779057,
+            "velocityY": -0.7852625961900879,
+            "timestamp": 0.10062666553345896
+        },
+        {
+            "x": 2.2851573432175654,
+            "y": 4.286058471237428,
+            "heading": -0.3020481691352866,
+            "angularVelocity": 1.1225985197714665,
+            "velocityX": -0.17417003432819553,
+            "velocityY": -0.9869470739663532,
+            "timestamp": 0.1257833319168237
+        },
+        {
+            "x": 2.280510060472114,
+            "y": 4.25610900707676,
+            "heading": -0.26927550139535,
+            "angularVelocity": 1.3027428690475438,
+            "velocityX": -0.18473364771909595,
+            "velocityY": -1.1905180004483138,
+            "timestamp": 0.15093999830018845
+        },
+        {
+            "x": 2.2758836751536,
+            "y": 4.2210014881511375,
+            "heading": -0.23246752663964654,
+            "angularVelocity": 1.463149933889621,
+            "velocityX": -0.1839029562984313,
+            "velocityY": -1.3955552930033008,
+            "timestamp": 0.1760966646835532
+        },
+        {
+            "x": 2.271613693612169,
+            "y": 4.180717494993525,
+            "heading": -0.19219487185489256,
+            "angularVelocity": 1.6008740653882818,
+            "velocityX": -0.16973558723403512,
+            "velocityY": -1.6013247758554696,
+            "timestamp": 0.20125333106691795
+        },
+        {
+            "x": 2.268091592763015,
+            "y": 4.135268758759812,
+            "heading": -0.1491141249151469,
+            "angularVelocity": 1.7124982413501877,
+            "velocityX": -0.1400066604803655,
+            "velocityY": -1.8066279347634933,
+            "timestamp": 0.2264099974502827
+        },
+        {
+            "x": 2.2657701315686967,
+            "y": 4.084713672798125,
+            "heading": -0.10398137252966654,
+            "angularVelocity": 1.7940672940403972,
+            "velocityX": -0.09228015981695388,
+            "velocityY": -2.0096099058307106,
+            "timestamp": 0.25156666383364745
+        },
+        {
+            "x": 2.265163909208295,
+            "y": 4.029179243747037,
+            "heading": -0.057668550112221675,
+            "angularVelocity": 1.8409761337881332,
+            "velocityX": -0.02409788129965802,
+            "velocityY": -2.2075432493635576,
+            "timestamp": 0.2767233302170122
+        },
+        {
+            "x": 2.266840363517046,
+            "y": 3.9688874958283327,
+            "heading": -0.011179237419756825,
+            "angularVelocity": 1.8479917801512262,
+            "velocityX": 0.06664055893588815,
+            "velocityY": -2.3966509314037507,
+            "timestamp": 0.3018799966003769
+        },
+        {
+            "x": 2.2713096946462064,
+            "y": 3.9041248350083095,
+            "heading": 0.03446639703260334,
+            "angularVelocity": 1.8144548151476838,
+            "velocityX": 0.1776599117328043,
+            "velocityY": -2.574373719995273,
+            "timestamp": 0.3270366629837416
+        },
+        {
+            "x": 2.2787160490905882,
+            "y": 3.8349972418338227,
+            "heading": 0.07865637806724814,
+            "angularVelocity": 1.7565912892125506,
+            "velocityX": 0.29440921668697884,
+            "velocityY": -2.74788368701343,
+            "timestamp": 0.35219332936710634
+        },
+        {
+            "x": 2.289237241442295,
+            "y": 3.7616571782461574,
+            "heading": 0.12059430680114623,
+            "angularVelocity": 1.667070195025133,
+            "velocityX": 0.41822681079334173,
+            "velocityY": -2.915333155436007,
+            "timestamp": 0.37734999575047107
+        },
+        {
+            "x": 2.303092280593543,
+            "y": 3.684333317171128,
+            "heading": 0.15922184993862323,
+            "angularVelocity": 1.5354794052927534,
+            "velocityX": 0.550750204343837,
+            "velocityY": -3.073692670431113,
+            "timestamp": 0.4025066621338358
+        },
+        {
+            "x": 2.3205444941015814,
+            "y": 3.6033821039148743,
+            "heading": 0.19314125064776663,
+            "angularVelocity": 1.3483265307192482,
+            "velocityX": 0.6937411039317692,
+            "velocityY": -3.217883165544707,
+            "timestamp": 0.4276633285172005
+        },
+        {
+            "x": 2.341904587688066,
+            "y": 3.5193842561187316,
+            "heading": 0.22066827104785883,
+            "angularVelocity": 1.0942236932591152,
+            "velocityX": 0.8490828339882706,
+            "velocityY": -3.3389896147642064,
+            "timestamp": 0.45281999490056524
+        },
+        {
+            "x": 2.3675851289681646,
+            "y": 3.4337423994061407,
+            "heading": 0.23918679984335672,
+            "angularVelocity": 0.7361280907928144,
+            "velocityX": 1.0208244959308244,
+            "velocityY": -3.40434044032252,
+            "timestamp": 0.47797666128392996
+        },
+        {
+            "x": 2.3978885412195137,
+            "y": 3.3469640370766824,
+            "heading": 0.24945072521405584,
+            "angularVelocity": 0.40800021808479553,
+            "velocityX": 1.2045877537807594,
+            "velocityY": -3.4495175555868487,
+            "timestamp": 0.5031333276672947
+        },
+        {
+            "x": 2.4331457577160354,
+            "y": 3.2598611919071945,
+            "heading": 0.25313177220508776,
+            "angularVelocity": 0.1463249118518414,
+            "velocityX": 1.4015059054023244,
+            "velocityY": -3.462416038839161,
+            "timestamp": 0.5282899940506595
+        },
+        {
+            "x": 2.473498104501041,
+            "y": 3.173715186571369,
+            "heading": 0.2532432496800442,
+            "angularVelocity": 0.004431329384332257,
+            "velocityX": 1.604041893710102,
+            "velocityY": -3.424380799229854,
+            "timestamp": 0.5534466604340242
+        },
+        {
+            "x": 2.5185817721335604,
+            "y": 3.089905547060991,
+            "heading": 0.25324321969105484,
+            "angularVelocity": -0.0000011920891614956396,
+            "velocityX": 1.7921161311870697,
+            "velocityY": -3.3315081669882356,
+            "timestamp": 0.578603326817389
+        },
+        {
+            "x": 2.568241478882672,
+            "y": 3.0087236123099395,
+            "heading": 0.2532431890633549,
+            "angularVelocity": -0.0000012174784783024135,
+            "velocityX": 1.97401778090718,
+            "velocityY": -3.2270545514223747,
+            "timestamp": 0.6037599932007538
+        },
+        {
+            "x": 2.6223245423144714,
+            "y": 2.9304189809174512,
+            "heading": 0.25324315880142845,
+            "angularVelocity": -0.0000012029386552177117,
+            "velocityX": 2.1498501672527754,
+            "velocityY": -3.1126791681853656,
+            "timestamp": 0.6289166595841186
+        },
+        {
+            "x": 2.6806646810204167,
+            "y": 2.855232404738762,
+            "heading": 0.25324312847978453,
+            "angularVelocity": -0.0000012053124772832063,
+            "velocityX": 2.319072718813166,
+            "velocityY": -2.9887336832676343,
+            "timestamp": 0.6540733259674834
         },
         {
             "x": 2.7430825233459473,
             "y": 2.7833950519561768,
-            "heading": 3.3907617949090247,
-            "angularVelocity": -0.0000011597457911507082,
-            "velocityX": 2.4695978056269863,
-            "velocityY": -2.8656090504076794,
-            "timestamp": 0.6771442723335441
-        },
-        {
-            "x": 2.8743019890864545,
-            "y": 2.6600421338324565,
-            "heading": 3.3907617697237464,
-            "angularVelocity": -5.290207073739654e-7,
-            "velocityX": 2.7562853520145434,
-            "velocityY": -2.5910472919087915,
-            "timestamp": 0.7247516311451417
-        },
-        {
-            "x": 3.0177249974626466,
-            "y": 2.5511186165163586,
-            "heading": 3.390761748438384,
-            "angularVelocity": -4.4710235312140834e-7,
-            "velocityX": 3.0126226691923277,
-            "velocityY": -2.287955476529456,
-            "timestamp": 0.7723589899567394
-        },
-        {
-            "x": 3.171772290811784,
-            "y": 2.457823792449377,
-            "heading": 3.3907617293701717,
-            "angularVelocity": -4.005307751758197e-7,
-            "velocityX": 3.235787432753145,
-            "velocityY": -1.95967233629129,
-            "timestamp": 0.819966348768337
-        },
-        {
-            "x": 3.3347475520740515,
-            "y": 2.3811846546739925,
-            "heading": 3.3907617114029063,
-            "angularVelocity": -3.7740521255041523e-7,
-            "velocityX": 3.4233207917967023,
-            "velocityY": -1.6098170469543955,
-            "timestamp": 0.8675737075799346
-        },
-        {
-            "x": 3.5038153046646756,
-            "y": 2.3191315453490344,
-            "heading": 3.3907616937093077,
-            "angularVelocity": -3.7165679655198125e-7,
-            "velocityX": 3.551294522758467,
-            "velocityY": -1.3034352434994065,
-            "timestamp": 0.9151810663915322
-        },
-        {
-            "x": 3.6728836971491297,
-            "y": 2.257080179484392,
-            "heading": 3.3907616760157264,
-            "angularVelocity": -3.7165643530673955e-7,
-            "velocityX": 3.551307963828218,
-            "velocityY": -1.303398621843431,
-            "timestamp": 0.9627884252031298
-        },
-        {
-            "x": 3.8419520974106347,
-            "y": 2.195028834809519,
-            "heading": 3.3907616583221465,
-            "angularVelocity": -3.71656403732769e-7,
-            "velocityX": 3.5513081271863745,
-            "velocityY": -1.3033981767490248,
-            "timestamp": 1.0103957840147275
-        },
-        {
-            "x": 4.011020504671481,
-            "y": 2.1329775092054373,
-            "heading": 3.390761640628565,
-            "angularVelocity": -3.7165644168556193e-7,
-            "velocityX": 3.5513082742086395,
-            "velocityY": -1.3033977761640818,
-            "timestamp": 1.0580031428263252
-        },
-        {
-            "x": 4.180088918243229,
-            "y": 2.070926200796389,
-            "heading": 3.390761622934979,
-            "angularVelocity": -3.716565361157419e-7,
-            "velocityX": 3.55130840677009,
-            "velocityY": -1.3033974149797176,
-            "timestamp": 1.105610501637923
-        },
-        {
-            "x": 4.349157337505363,
-            "y": 2.008874907891698,
-            "heading": 3.3907616052413863,
-            "angularVelocity": -3.716566778801587e-7,
-            "velocityX": 3.551308526297583,
-            "velocityY": -1.3033970893082818,
-            "timestamp": 1.1532178604495207
-        },
-        {
-            "x": 4.518225761904485,
-            "y": 1.9468236289835406,
-            "heading": 3.390761587547785,
-            "angularVelocity": -3.716568562531517e-7,
-            "velocityX": 3.551308634200803,
-            "velocityY": -1.303396795308895,
-            "timestamp": 1.2008252192611184
-        },
-        {
-            "x": 4.687294190885785,
-            "y": 1.8847723625602526,
-            "heading": 3.3907615698541753,
-            "angularVelocity": -3.716570319734318e-7,
-            "velocityX": 3.5513087304501405,
-            "velocityY": -1.3033965330622725,
-            "timestamp": 1.2484325780727161
-        },
-        {
-            "x": 4.8563626239355635,
-            "y": 1.8227211072221874,
-            "heading": 3.390761552160558,
-            "angularVelocity": -3.716571921571891e-7,
-            "velocityX": 3.5513088159091875,
-            "velocityY": -1.3033963002154438,
-            "timestamp": 1.2960399368843138
-        },
-        {
-            "x": 5.025431060602707,
-            "y": 1.7606698617402123,
-            "heading": 3.3907615344669337,
-            "angularVelocity": -3.716573365191525e-7,
-            "velocityX": 3.551308891892495,
-            "velocityY": -1.3033960931867248,
-            "timestamp": 1.3436472956959116
-        },
-        {
-            "x": 5.1944995004938015,
-            "y": 1.6986186250424111,
-            "heading": 3.3907615167733036,
-            "angularVelocity": -3.7165746535883894e-7,
-            "velocityX": 3.551308959612086,
-            "velocityY": -1.3033959086737847,
-            "timestamp": 1.3912546545075093
-        },
-        {
-            "x": 5.363567943266909,
-            "y": 1.6365673961971205,
-            "heading": 3.3907614990796673,
-            "angularVelocity": -3.716575836762919e-7,
-            "velocityX": 3.551309020149218,
-            "velocityY": -1.3033957437305734,
-            "timestamp": 1.438862013319107
-        },
-        {
-            "x": 5.532636388625086,
-            "y": 1.574516174395274,
-            "heading": 3.390761481386026,
-            "angularVelocity": -3.716576912991928e-7,
-            "velocityX": 3.5513090744490245,
-            "velocityY": -1.3033955957819334,
-            "timestamp": 1.4864693721307047
-        },
-        {
-            "x": 5.701704836310244,
-            "y": 1.5124649589336634,
-            "heading": 3.3907614636923804,
-            "angularVelocity": -3.7165778948932697e-7,
-            "velocityX": 3.551309123327601,
-            "velocityY": -1.3033954626042876,
-            "timestamp": 1.5340767309423025
-        },
-        {
-            "x": 5.870773286097621,
-            "y": 1.4504137491998943,
-            "heading": 3.3907614459987303,
-            "angularVelocity": -3.716578803925389e-7,
-            "velocityX": 3.551309167485058,
-            "velocityY": -1.3033953422900804,
-            "timestamp": 1.5816840897539002
-        },
-        {
-            "x": 6.039841737790961,
-            "y": 1.388362544659232,
-            "heading": 3.3907614283050767,
-            "angularVelocity": -3.716579611998838e-7,
-            "velocityX": 3.551309207520092,
-            "velocityY": -1.3033952332080676,
-            "timestamp": 1.629291448565498
-        },
-        {
-            "x": 6.208910191218329,
-            "y": 1.3263113448432136,
-            "heading": 3.390761410611419,
-            "angularVelocity": -3.7165803844263857e-7,
-            "velocityX": 3.551309243943619,
-            "velocityY": -1.303395133966182,
-            "timestamp": 1.6768988073770956
-        },
-        {
-            "x": 6.377978646228594,
-            "y": 1.2642601493400643,
-            "heading": 3.3907613929177582,
-            "angularVelocity": -3.7165810831214996e-7,
-            "velocityX": 3.5513092771926584,
-            "velocityY": -1.3033950433736952,
-            "timestamp": 1.7245061661886933
-        },
-        {
-            "x": 6.547047102688449,
-            "y": 1.2022089577865573,
-            "heading": 3.390761375224094,
-            "angularVelocity": -3.716581745376039e-7,
-            "velocityX": 3.551309307641484,
-            "velocityY": -1.3033949604108395,
-            "timestamp": 1.772113525000291
-        },
-        {
-            "x": 6.716115560480058,
-            "y": 1.1401577698616354,
-            "heading": 3.3907613575304274,
-            "angularVelocity": -3.716582357126045e-7,
-            "velocityX": 3.5513093356151892,
-            "velocityY": -1.3033948841918472,
-            "timestamp": 1.8197208838118888
-        },
-        {
-            "x": 6.885184026346865,
-            "y": 1.0781066039393965,
-            "heading": 3.3907613398363963,
-            "angularVelocity": -3.7166587620925834e-7,
-            "velocityX": 3.5513095052359795,
-            "velocityY": -1.3033944220220548,
-            "timestamp": 1.8673282426234865
-        },
-        {
-            "x": 7.044084736711506,
-            "y": 1.0199905851581765,
-            "heading": 3.36350523152736,
-            "angularVelocity": -0.5725188077939848,
-            "velocityX": 3.337734214441043,
-            "velocityY": -1.220736042325084,
-            "timestamp": 1.9149356014350842
-        },
-        {
-            "x": 7.185551132214723,
-            "y": 0.9681799151008591,
-            "heading": 3.323163851941908,
-            "angularVelocity": -0.8473769726461788,
-            "velocityX": 2.9715237105057524,
-            "velocityY": -1.0882912085577785,
-            "timestamp": 1.962542960246682
-        },
-        {
-            "x": 7.3092683658418975,
-            "y": 0.9228556276324137,
-            "heading": 3.282206369227669,
-            "angularVelocity": -0.8603183149967459,
-            "velocityX": 2.598699795902896,
-            "velocityY": -0.952043730210126,
-            "timestamp": 2.0101503190582797
-        },
-        {
-            "x": 7.415256126479253,
-            "y": 0.8840204066687376,
-            "heading": 3.244221221054594,
-            "angularVelocity": -0.7978839641870918,
-            "velocityX": 2.226289449427224,
-            "velocityY": -0.815739875790283,
-            "timestamp": 2.057757677869877
-        },
-        {
-            "x": 7.503541109453659,
-            "y": 0.8516687069454374,
-            "heading": 3.210907204397792,
-            "angularVelocity": -0.6997661178524867,
-            "velocityX": 1.8544398424576745,
-            "velocityY": -0.6795525005142472,
-            "timestamp": 2.1053650366814747
-        },
-        {
-            "x": 7.574144027018988,
-            "y": 0.8257950136289538,
-            "heading": 3.1832598866385293,
-            "angularVelocity": -0.5807362233362916,
-            "velocityX": 1.4830252996124917,
-            "velocityY": -0.5434809651776052,
-            "timestamp": 2.152972395493072
-        },
-        {
-            "x": 7.627080539080189,
-            "y": 0.8063947169930168,
-            "heading": 3.161935751906697,
-            "angularVelocity": -0.44791677724069034,
-            "velocityX": 1.1119396955141558,
-            "velocityY": -0.407506257860514,
-            "timestamp": 2.2005797543046697
-        },
-        {
-            "x": 7.662362703782539,
-            "y": 0.7934640657363813,
-            "heading": 3.147400394630243,
-            "angularVelocity": -0.3053174475395025,
-            "velocityX": 0.7411073746387762,
-            "velocityY": -0.27161034721139543,
-            "timestamp": 2.248187113116267
+            "heading": 0.2532430937019675,
+            "angularVelocity": -0.0000013824493495341482,
+            "velocityX": 2.4811650865953045,
+            "velocityY": -2.8555990562441647,
+            "timestamp": 0.6792299923508481
+        },
+        {
+            "x": 2.8747857816965947,
+            "y": 2.6605842580771375,
+            "heading": 0.2532430684120768,
+            "angularVelocity": -5.312693002073383e-7,
+            "velocityX": 2.7667141273306664,
+            "velocityY": -2.579908520631989,
+            "timestamp": 0.7268327631068262
+        },
+        {
+            "x": 3.0186320127358077,
+            "y": 2.5522491183421203,
+            "heading": 0.2532430469824583,
+            "angularVelocity": -4.5017586399077444e-7,
+            "velocityX": 3.021803747025554,
+            "velocityY": -2.2758158404343,
+            "timestamp": 0.7744355338628043
+        },
+        {
+            "x": 3.1730376030001257,
+            "y": 2.4595822138278454,
+            "heading": 0.2532430277420137,
+            "angularVelocity": -4.041874945252969e-7,
+            "velocityX": 3.2436261127705017,
+            "velocityY": -1.946670394236201,
+            "timestamp": 0.8220383046187824
+        },
+        {
+            "x": 3.336302611834685,
+            "y": 2.38360340882859,
+            "heading": 0.25324300957525164,
+            "angularVelocity": -3.816324501860361e-7,
+            "velocityX": 3.4297375182526793,
+            "velocityY": -1.5961004746706646,
+            "timestamp": 0.8696410753747604
+        },
+        {
+            "x": 3.505316504811254,
+            "y": 2.3214540241877932,
+            "heading": 0.25324299166355524,
+            "angularVelocity": -3.7627424124720095e-7,
+            "velocityX": 3.550505365390803,
+            "velocityY": -1.3055833442844713,
+            "timestamp": 0.9172438461307385
+        },
+        {
+            "x": 3.6743308817857088,
+            "y": 2.2593059557844626,
+            "heading": 0.25324297375187266,
+            "angularVelocity": -3.76273948990246e-7,
+            "velocityX": 3.5505155328218008,
+            "velocityY": -1.3055556938463668,
+            "timestamp": 0.9648466168867166
+        },
+        {
+            "x": 3.843345266545321,
+            "y": 2.1971579085532174,
+            "heading": 0.2532429558401917,
+            "angularVelocity": -3.762739157561073e-7,
+            "velocityX": 3.5505156963660025,
+            "velocityY": -1.3055552490805507,
+            "timestamp": 1.0124493876426945
+        },
+        {
+            "x": 4.012359658311276,
+            "y": 2.135009880376043,
+            "heading": 0.25324293792850894,
+            "angularVelocity": -3.7627395316214525e-7,
+            "velocityX": 3.5505158435494915,
+            "velocityY": -1.3055548488082471,
+            "timestamp": 1.0600521583986726
+        },
+        {
+            "x": 4.181374056391421,
+            "y": 2.07286186937061,
+            "heading": 0.25324292001682175,
+            "angularVelocity": -3.7627404706616014e-7,
+            "velocityX": 3.5505159761928384,
+            "velocityY": -1.30555448807838,
+            "timestamp": 1.1076549291546507
+        },
+        {
+            "x": 4.3503884601627245,
+            "y": 2.010713873842556,
+            "heading": 0.25324290210512795,
+            "angularVelocity": -3.7627418619029544e-7,
+            "velocityX": 3.5505160957480255,
+            "velocityY": -1.30555416294226,
+            "timestamp": 1.1552576999106288
+        },
+        {
+            "x": 4.51940286907005,
+            "y": 1.9485658922821711,
+            "heading": 0.2532428841934256,
+            "angularVelocity": -3.7627436389492574e-7,
+            "velocityX": 3.5505162036413855,
+            "velocityY": -1.3055538695209365,
+            "timestamp": 1.2028604706666068
+        },
+        {
+            "x": 4.688417282561973,
+            "y": 1.8864179231898264,
+            "heading": 0.25324286628171494,
+            "angularVelocity": -3.7627454024029507e-7,
+            "velocityX": 3.550516299950831,
+            "velocityY": -1.3055536076025602,
+            "timestamp": 1.250463241422585
+        },
+        {
+            "x": 4.857431700124039,
+            "y": 1.824269965166445,
+            "heading": 0.2532428483699966,
+            "angularVelocity": -3.7627470129766985e-7,
+            "velocityX": 3.5505163854530717,
+            "velocityY": -1.3055533750748494,
+            "timestamp": 1.298066012178563
+        },
+        {
+            "x": 5.02644612130461,
+            "y": 1.7621220169837726,
+            "heading": 0.2532428304582713,
+            "angularVelocity": -3.76274846815446e-7,
+            "velocityX": 3.5505164614676405,
+            "velocityY": -1.3055531683492934,
+            "timestamp": 1.345668782934541
+        },
+        {
+            "x": 5.1954605457099055,
+            "y": 1.6999740775709098,
+            "heading": 0.25324281254653974,
+            "angularVelocity": -3.7627497879654404e-7,
+            "velocityX": 3.5505165292100247,
+            "velocityY": -1.3055529841203253,
+            "timestamp": 1.3932715536905191
+        },
+        {
+            "x": 5.364474972997734,
+            "y": 1.6378261459972472,
+            "heading": 0.25324279463480254,
+            "angularVelocity": -3.7627509767034604e-7,
+            "velocityX": 3.550516589763906,
+            "velocityY": -1.3055528194408317,
+            "timestamp": 1.4408743244464972
+        },
+        {
+            "x": 5.533489402870974,
+            "y": 1.5756782214547507,
+            "heading": 0.2532427767230601,
+            "angularVelocity": -3.762752068122207e-7,
+            "velocityX": 3.5505166440761182,
+            "velocityY": -1.3055526717358554,
+            "timestamp": 1.4884770952024753
+        },
+        {
+            "x": 5.702503835071411,
+            "y": 1.513530303241192,
+            "heading": 0.25324275881131303,
+            "angularVelocity": -3.7627530496428226e-7,
+            "velocityX": 3.550516692963962,
+            "velocityY": -1.3055525387827163,
+            "timestamp": 1.5360798659584534
+        },
+        {
+            "x": 5.871518269374186,
+            "y": 1.4513823907450571,
+            "heading": 0.2532427408995616,
+            "angularVelocity": -3.762753964251323e-7,
+            "velocityX": 3.5505167371281625,
+            "velocityY": -1.3055524186757583,
+            "timestamp": 1.5836826367144314
+        },
+        {
+            "x": 6.040532705582966,
+            "y": 1.3892344834324082,
+            "heading": 0.25324272298780626,
+            "angularVelocity": -3.7627547903960503e-7,
+            "velocityX": 3.550516777167954,
+            "velocityY": -1.3055523097853368,
+            "timestamp": 1.6312854074704095
+        },
+        {
+            "x": 6.20954714352589,
+            "y": 1.3270865808358587,
+            "heading": 0.2532427050760471,
+            "angularVelocity": -3.7627555795965075e-7,
+            "velocityX": 3.5505168135974197,
+            "velocityY": -1.3055522107133783,
+            "timestamp": 1.6788881782263876
+        },
+        {
+            "x": 6.378561583051887,
+            "y": 1.2649386825445668,
+            "heading": 0.25324268716428455,
+            "angularVelocity": -3.7627562961930894e-7,
+            "velocityX": 3.5505168468533315,
+            "velocityY": -1.3055521202720615,
+            "timestamp": 1.7264909489823657
+        },
+        {
+            "x": 6.547576024027691,
+            "y": 1.2027907881961044,
+            "heading": 0.25324266925251887,
+            "angularVelocity": -3.7627569544162935e-7,
+            "velocityX": 3.5505168773096716,
+            "velocityY": -1.3055520374443248,
+            "timestamp": 1.7740937197383437
+        },
+        {
+            "x": 6.716590466335514,
+            "y": 1.1406428974701475,
+            "heading": 0.25324265134075025,
+            "angularVelocity": -3.762757576316506e-7,
+            "velocityX": 3.550516905291665,
+            "velocityY": -1.3055519613457025,
+            "timestamp": 1.8216964904943218
+        },
+        {
+            "x": 6.88560491686231,
+            "y": 1.0784950290966298,
+            "heading": 0.253242633428611,
+            "angularVelocity": -3.7628354454135297e-7,
+            "velocityX": 3.550517077949107,
+            "velocityY": -1.3055514917839766,
+            "timestamp": 1.8692992612502999
+        },
+        {
+            "x": 7.0444139114910005,
+            "y": 1.0203058482073266,
+            "heading": 0.2259027129119336,
+            "angularVelocity": -0.574334646544581,
+            "velocityX": 3.336129223292024,
+            "velocityY": -1.2223906290579827,
+            "timestamp": 1.916902032006278
+        },
+        {
+            "x": 7.185810534595712,
+            "y": 0.9684234171781619,
+            "heading": 0.18518533646408028,
+            "angularVelocity": -0.8553572786042075,
+            "velocityX": 2.9703443908662663,
+            "velocityY": -1.089903596055891,
+            "timestamp": 1.964504802762256
+        },
+        {
+            "x": 7.3094655243228,
+            "y": 0.9230369546143523,
+            "heading": 0.14380134057962407,
+            "angularVelocity": -0.8693610734677483,
+            "velocityX": 2.59764269523238,
+            "velocityY": -0.953441613650398,
+            "timestamp": 2.012107573518234
+        },
+        {
+            "x": 7.415398593682274,
+            "y": 0.8841490900710189,
+            "heading": 0.10540313701630608,
+            "angularVelocity": -0.8066379950897271,
+            "velocityX": 2.2253551143589885,
+            "velocityY": -0.8169243917057022,
+            "timestamp": 2.059710344274212
+        },
+        {
+            "x": 7.503637039429732,
+            "y": 0.8517540010892736,
+            "heading": 0.07171800478429124,
+            "angularVelocity": -0.70762965468317,
+            "velocityX": 1.8536409613588987,
+            "velocityY": -0.680529483206126,
+            "timestamp": 2.1073131150301903
+        },
+        {
+            "x": 7.574202084965293,
+            "y": 0.8258459262172375,
+            "heading": 0.04375776286271558,
+            "angularVelocity": -0.5873658502969467,
+            "velocityX": 1.482372652157004,
+            "velocityY": -0.5442556065663985,
+            "timestamp": 2.1549158857861683
+        },
+        {
+            "x": 7.627109788299601,
+            "y": 0.8064200554899893,
+            "heading": 0.022189516524924582,
+            "angularVelocity": -0.4530880449870122,
+            "velocityX": 1.1114416764840027,
+            "velocityY": -0.40808277372822227,
+            "timestamp": 2.2025186565421464
+        },
+        {
+            "x": 7.66237251850946,
+            "y": 0.7934724766877013,
+            "heading": 0.0074863600733853135,
+            "angularVelocity": -0.3088718622474896,
+            "velocityX": 0.7407705402406998,
+            "velocityY": -0.27199212559831976,
+            "timestamp": 2.2501214272981245
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": -0.1554464438896817,
-            "velocityX": 0.370474158989983,
-            "velocityY": -0.13577870937899197,
-            "timestamp": 2.2957944719278647
+            "heading": -1.0670900456397286e-32,
+            "angularVelocity": -0.1572673177316084,
+            "velocityX": 0.37030368633165434,
+            "velocityY": -0.13596848639085585,
+            "timestamp": 2.2977241980541026
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 1.845639156363882e-34,
-            "velocityX": 1.6232927575664752e-34,
-            "velocityY": 1.3237538522972492e-34,
-            "timestamp": 2.343401830739462
+            "heading": 0,
+            "angularVelocity": 2.230662363627099e-31,
+            "velocityX": 2.3924515989499496e-33,
+            "velocityY": 1.2585115119762557e-33,
+            "timestamp": 2.3453269688100806
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.1.traj
index 22f8f664..84a9867e 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.1.traj	
@@ -3,173 +3,173 @@
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": -1.3064230645361168e-30,
-            "velocityX": 1.1529790698064783e-31,
-            "velocityY": -2.8761439718242387e-32,
+            "heading": -0.39,
+            "angularVelocity": 8.834598343952546e-30,
+            "velocityX": -1.1855672444586933e-30,
+            "velocityY": 4.732277944627028e-32,
             "timestamp": 0
         },
         {
-            "x": 2.322180658668465,
-            "y": 4.411221384877813,
-            "heading": 2.7352853365602194,
-            "angularVelocity": -0.1796018847298344,
-            "velocityX": 0.2707291347647005,
-            "velocityY": 0.6251897843383167,
-            "timestamp": 0.08192933755631272
-        },
-        {
-            "x": 2.368773081487876,
-            "y": 4.512701566380937,
-            "heading": 2.7089075651304313,
-            "angularVelocity": -0.32195758218669895,
-            "velocityX": 0.5686903398600793,
-            "velocityY": 1.238630562994284,
-            "timestamp": 0.16385867511262545
-        },
-        {
-            "x": 2.4433012157913003,
-            "y": 4.662644128727173,
-            "heading": 2.675827445796147,
-            "angularVelocity": -0.4037640278922902,
-            "velocityX": 0.9096635774968738,
-            "velocityY": 1.8301449373147467,
-            "timestamp": 0.24578801266893818
-        },
-        {
-            "x": 2.5519618748459716,
-            "y": 4.856937153180571,
-            "heading": 2.6453653517833016,
-            "angularVelocity": -0.3718093532967731,
-            "velocityX": 1.3262728870471487,
-            "velocityY": 2.371470711817401,
-            "timestamp": 0.3277173502252509
-        },
-        {
-            "x": 2.7016003249644123,
-            "y": 5.089209181124471,
-            "heading": 2.6267184772011043,
-            "angularVelocity": -0.22759703835491757,
-            "velocityX": 1.8264330529413755,
-            "velocityY": 2.835028756143084,
-            "timestamp": 0.4096466877815636
-        },
-        {
-            "x": 2.9069631472288275,
-            "y": 5.3213406432232695,
-            "heading": 2.6267188101864813,
-            "angularVelocity": 0.00000406429963372636,
-            "velocityX": 2.5065846788186557,
-            "velocityY": 2.833313060040874,
-            "timestamp": 0.4915760253378763
-        },
-        {
-            "x": 3.1507251830977756,
-            "y": 5.512753871430069,
-            "heading": 2.626718909792111,
-            "angularVelocity": 0.0000012157504557439165,
-            "velocityX": 2.975271656522333,
-            "velocityY": 2.3363209555458044,
-            "timestamp": 0.5735053628941891
-        },
-        {
-            "x": 3.4078762515345984,
-            "y": 5.685763359739643,
-            "heading": 2.6267190032534695,
-            "angularVelocity": 0.0000011407556990966525,
-            "velocityX": 3.1386933680511517,
-            "velocityY": 2.1116915316282108,
-            "timestamp": 0.6554347004505018
-        },
-        {
-            "x": 3.665027465532842,
-            "y": 5.858772631695159,
-            "heading": 2.626719096714767,
-            "angularVelocity": 0.0000011407549466335633,
-            "velocityX": 3.1386951447214493,
-            "velocityY": 2.111688890888465,
-            "timestamp": 0.7373640380068145
-        },
-        {
-            "x": 3.922178727707041,
-            "y": 6.031781832044647,
-            "heading": 2.6267191901760296,
-            "angularVelocity": 0.0000011407545266438845,
-            "velocityX": 3.138695732739822,
-            "velocityY": 2.1116880168910543,
-            "timestamp": 0.8192933755631272
-        },
-        {
-            "x": 4.179330140012749,
-            "y": 6.204790809246775,
-            "heading": 2.6267192836372875,
-            "angularVelocity": 0.0000011407544682834688,
-            "velocityX": 3.1386975651909763,
-            "velocityY": 2.111685293234714,
-            "timestamp": 0.9012227131194399
-        },
-        {
-            "x": 4.443624087689148,
-            "y": 6.354175617474429,
-            "heading": 2.642647782574834,
-            "angularVelocity": 0.1944175238399595,
-            "velocityX": 3.225876780643333,
-            "velocityY": 1.823337191332418,
-            "timestamp": 0.9831520506757526
-        },
-        {
-            "x": 4.674751705763445,
-            "y": 6.469415592948305,
-            "heading": 2.769232553851288,
-            "angularVelocity": 1.5450481482222196,
-            "velocityX": 2.821060501257377,
-            "velocityY": 1.4065776547341065,
-            "timestamp": 1.0650813882320653
-        },
-        {
-            "x": 4.870834347570915,
-            "y": 6.549065174445882,
-            "heading": 2.9849459088839683,
-            "angularVelocity": 2.6329195556404166,
-            "velocityX": 2.39331413698659,
-            "velocityY": 0.9721741182495338,
-            "timestamp": 1.1470107257883781
-        },
-        {
-            "x": 5.022947063703052,
-            "y": 6.5945122449585005,
-            "heading": 3.181337324435562,
-            "angularVelocity": 2.397082918150139,
-            "velocityX": 1.856633053179344,
-            "velocityY": 0.5547105819253249,
-            "timestamp": 1.228940063344691
-        },
-        {
-            "x": 5.124965104320653,
-            "y": 6.619330120195266,
-            "heading": 3.326379958974982,
-            "angularVelocity": 1.7703381824578692,
-            "velocityX": 1.2451954777185934,
-            "velocityY": 0.3029180508106365,
-            "timestamp": 1.3108694009010038
+            "x": 2.3221690172235006,
+            "y": 4.411229758092553,
+            "heading": -0.4048175852865869,
+            "angularVelocity": -0.18084977247894177,
+            "velocityX": 0.27057456686796094,
+            "velocityY": 0.6252631529349166,
+            "timestamp": 0.08193311544440174
+        },
+        {
+            "x": 2.3687303268224738,
+            "y": 4.512732101891575,
+            "heading": -0.43137596230210584,
+            "angularVelocity": -0.3241470420293335,
+            "velocityX": 0.5682843786231674,
+            "velocityY": 1.2388439429951275,
+            "timestamp": 0.16386623088880348
+        },
+        {
+            "x": 2.4431953900618826,
+            "y": 4.662720774571155,
+            "heading": -0.464675218076173,
+            "angularVelocity": -0.40641998773593985,
+            "velocityX": 0.9088518462346009,
+            "velocityY": 1.8306233305795314,
+            "timestamp": 0.24579934633320522
+        },
+        {
+            "x": 2.551739981424586,
+            "y": 4.8571070042037645,
+            "heading": -0.49532439278782164,
+            "angularVelocity": -0.3740755437574734,
+            "velocityX": 1.3247951182371547,
+            "velocityY": 2.3724989410968234,
+            "timestamp": 0.32773246177760695
+        },
+        {
+            "x": 2.7012931711639805,
+            "y": 5.089437783878959,
+            "heading": -0.5138025644966426,
+            "angularVelocity": -0.22552751239337346,
+            "velocityX": 1.8253082276711035,
+            "velocityY": 2.835615104039969,
+            "timestamp": 0.4096655772220087
+        },
+        {
+            "x": 2.906580390144238,
+            "y": 5.321655181575939,
+            "heading": -0.513802224550241,
+            "angularVelocity": 0.0000041490720786488115,
+            "velocityX": 2.5055463577429817,
+            "velocityY": 2.834231268241715,
+            "timestamp": 0.49159869266641043
+        },
+        {
+            "x": 3.150285117410734,
+            "y": 5.513164499731697,
+            "heading": -0.5138021244683586,
+            "angularVelocity": 0.0000012215071026742433,
+            "velocityX": 2.974435012566654,
+            "velocityY": 2.3373860143974894,
+            "timestamp": 0.5735318081108122
+        },
+        {
+            "x": 3.407534970443545,
+            "y": 5.686052692352735,
+            "heading": -0.513802030650944,
+            "angularVelocity": 0.0000011450487881316094,
+            "velocityX": 3.1397543183546524,
+            "velocityY": 2.110113739521574,
+            "timestamp": 0.6554649235552139
+        },
+        {
+            "x": 3.664784970537928,
+            "y": 5.858940666152602,
+            "heading": -0.5138019368336011,
+            "angularVelocity": 0.0000011450479143905032,
+            "velocityX": 3.1397561132524885,
+            "velocityY": 2.1101110687922637,
+            "timestamp": 0.7373980389996156
+        },
+        {
+            "x": 3.922035018762957,
+            "y": 6.031828568336093,
+            "heading": -0.5138018430162984,
+            "angularVelocity": 0.0000011450474228928023,
+            "velocityX": 3.139756700690747,
+            "velocityY": 2.110110194708886,
+            "timestamp": 0.8193311544440174
+        },
+        {
+            "x": 4.1792852160642955,
+            "y": 6.204716248700005,
+            "heading": -0.5138017491990241,
+            "angularVelocity": 0.0000011450470770765712,
+            "velocityX": 3.1397585201786073,
+            "velocityY": 2.1101074873837833,
+            "timestamp": 0.9012642698884191
+        },
+        {
+            "x": 4.443584051036782,
+            "y": 6.354085007666646,
+            "heading": -0.49783246108316886,
+            "angularVelocity": 0.19490639443210175,
+            "velocityX": 3.225787687175579,
+            "velocityY": 1.8230572358499022,
+            "timestamp": 0.9831973853328209
+        },
+        {
+            "x": 4.674708985410259,
+            "y": 6.469317132198814,
+            "heading": -0.3712043024716245,
+            "angularVelocity": 1.545506452729414,
+            "velocityX": 2.8208976690299576,
+            "velocityY": 1.4064169769104102,
+            "timestamp": 1.0651305007772227
+        },
+        {
+            "x": 4.870816846139177,
+            "y": 6.548974010532027,
+            "heading": -0.15522222327575852,
+            "angularVelocity": 2.636077952417564,
+            "velocityX": 2.393511581553284,
+            "velocityY": 0.9722183503111966,
+            "timestamp": 1.1470636162216246
+        },
+        {
+            "x": 5.022935939261114,
+            "y": 6.594453280726005,
+            "heading": 0.041233837030506995,
+            "angularVelocity": 2.3977613842791428,
+            "velocityX": 1.856625276566729,
+            "velocityY": 0.5550780041415454,
+            "timestamp": 1.2289967316660264
+        },
+        {
+            "x": 5.124959991715153,
+            "y": 6.619307482178149,
+            "heading": 0.1862409016069847,
+            "angularVelocity": 1.7698224190545027,
+            "velocityX": 1.245211437410438,
+            "velocityY": 0.30334744769969435,
+            "timestamp": 1.3109298471104283
         },
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.93745843058738,
-            "velocityX": 0.6229135643171693,
-            "velocityY": 0.13023271178533075,
-            "timestamp": 1.3927987384573166
+            "heading": 0.263,
+            "angularVelocity": 0.9368507224054672,
+            "velocityX": 0.6229472418814781,
+            "velocityY": 0.13050300557783806,
+            "timestamp": 1.39286296255483
         },
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": -5.432893499766539e-33,
+            "heading": 0.263,
+            "angularVelocity": 1.6488266469177426e-31,
             "velocityX": 0,
-            "velocityY": 6.79272724985994e-33,
-            "timestamp": 1.4747280760136294
+            "velocityY": 1.9617182542723147e-31,
+            "timestamp": 1.474796077999232
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.traj
index 22f8f664..84a9867e 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.traj	
@@ -3,173 +3,173 @@
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": -1.3064230645361168e-30,
-            "velocityX": 1.1529790698064783e-31,
-            "velocityY": -2.8761439718242387e-32,
+            "heading": -0.39,
+            "angularVelocity": 8.834598343952546e-30,
+            "velocityX": -1.1855672444586933e-30,
+            "velocityY": 4.732277944627028e-32,
             "timestamp": 0
         },
         {
-            "x": 2.322180658668465,
-            "y": 4.411221384877813,
-            "heading": 2.7352853365602194,
-            "angularVelocity": -0.1796018847298344,
-            "velocityX": 0.2707291347647005,
-            "velocityY": 0.6251897843383167,
-            "timestamp": 0.08192933755631272
-        },
-        {
-            "x": 2.368773081487876,
-            "y": 4.512701566380937,
-            "heading": 2.7089075651304313,
-            "angularVelocity": -0.32195758218669895,
-            "velocityX": 0.5686903398600793,
-            "velocityY": 1.238630562994284,
-            "timestamp": 0.16385867511262545
-        },
-        {
-            "x": 2.4433012157913003,
-            "y": 4.662644128727173,
-            "heading": 2.675827445796147,
-            "angularVelocity": -0.4037640278922902,
-            "velocityX": 0.9096635774968738,
-            "velocityY": 1.8301449373147467,
-            "timestamp": 0.24578801266893818
-        },
-        {
-            "x": 2.5519618748459716,
-            "y": 4.856937153180571,
-            "heading": 2.6453653517833016,
-            "angularVelocity": -0.3718093532967731,
-            "velocityX": 1.3262728870471487,
-            "velocityY": 2.371470711817401,
-            "timestamp": 0.3277173502252509
-        },
-        {
-            "x": 2.7016003249644123,
-            "y": 5.089209181124471,
-            "heading": 2.6267184772011043,
-            "angularVelocity": -0.22759703835491757,
-            "velocityX": 1.8264330529413755,
-            "velocityY": 2.835028756143084,
-            "timestamp": 0.4096466877815636
-        },
-        {
-            "x": 2.9069631472288275,
-            "y": 5.3213406432232695,
-            "heading": 2.6267188101864813,
-            "angularVelocity": 0.00000406429963372636,
-            "velocityX": 2.5065846788186557,
-            "velocityY": 2.833313060040874,
-            "timestamp": 0.4915760253378763
-        },
-        {
-            "x": 3.1507251830977756,
-            "y": 5.512753871430069,
-            "heading": 2.626718909792111,
-            "angularVelocity": 0.0000012157504557439165,
-            "velocityX": 2.975271656522333,
-            "velocityY": 2.3363209555458044,
-            "timestamp": 0.5735053628941891
-        },
-        {
-            "x": 3.4078762515345984,
-            "y": 5.685763359739643,
-            "heading": 2.6267190032534695,
-            "angularVelocity": 0.0000011407556990966525,
-            "velocityX": 3.1386933680511517,
-            "velocityY": 2.1116915316282108,
-            "timestamp": 0.6554347004505018
-        },
-        {
-            "x": 3.665027465532842,
-            "y": 5.858772631695159,
-            "heading": 2.626719096714767,
-            "angularVelocity": 0.0000011407549466335633,
-            "velocityX": 3.1386951447214493,
-            "velocityY": 2.111688890888465,
-            "timestamp": 0.7373640380068145
-        },
-        {
-            "x": 3.922178727707041,
-            "y": 6.031781832044647,
-            "heading": 2.6267191901760296,
-            "angularVelocity": 0.0000011407545266438845,
-            "velocityX": 3.138695732739822,
-            "velocityY": 2.1116880168910543,
-            "timestamp": 0.8192933755631272
-        },
-        {
-            "x": 4.179330140012749,
-            "y": 6.204790809246775,
-            "heading": 2.6267192836372875,
-            "angularVelocity": 0.0000011407544682834688,
-            "velocityX": 3.1386975651909763,
-            "velocityY": 2.111685293234714,
-            "timestamp": 0.9012227131194399
-        },
-        {
-            "x": 4.443624087689148,
-            "y": 6.354175617474429,
-            "heading": 2.642647782574834,
-            "angularVelocity": 0.1944175238399595,
-            "velocityX": 3.225876780643333,
-            "velocityY": 1.823337191332418,
-            "timestamp": 0.9831520506757526
-        },
-        {
-            "x": 4.674751705763445,
-            "y": 6.469415592948305,
-            "heading": 2.769232553851288,
-            "angularVelocity": 1.5450481482222196,
-            "velocityX": 2.821060501257377,
-            "velocityY": 1.4065776547341065,
-            "timestamp": 1.0650813882320653
-        },
-        {
-            "x": 4.870834347570915,
-            "y": 6.549065174445882,
-            "heading": 2.9849459088839683,
-            "angularVelocity": 2.6329195556404166,
-            "velocityX": 2.39331413698659,
-            "velocityY": 0.9721741182495338,
-            "timestamp": 1.1470107257883781
-        },
-        {
-            "x": 5.022947063703052,
-            "y": 6.5945122449585005,
-            "heading": 3.181337324435562,
-            "angularVelocity": 2.397082918150139,
-            "velocityX": 1.856633053179344,
-            "velocityY": 0.5547105819253249,
-            "timestamp": 1.228940063344691
-        },
-        {
-            "x": 5.124965104320653,
-            "y": 6.619330120195266,
-            "heading": 3.326379958974982,
-            "angularVelocity": 1.7703381824578692,
-            "velocityX": 1.2451954777185934,
-            "velocityY": 0.3029180508106365,
-            "timestamp": 1.3108694009010038
+            "x": 2.3221690172235006,
+            "y": 4.411229758092553,
+            "heading": -0.4048175852865869,
+            "angularVelocity": -0.18084977247894177,
+            "velocityX": 0.27057456686796094,
+            "velocityY": 0.6252631529349166,
+            "timestamp": 0.08193311544440174
+        },
+        {
+            "x": 2.3687303268224738,
+            "y": 4.512732101891575,
+            "heading": -0.43137596230210584,
+            "angularVelocity": -0.3241470420293335,
+            "velocityX": 0.5682843786231674,
+            "velocityY": 1.2388439429951275,
+            "timestamp": 0.16386623088880348
+        },
+        {
+            "x": 2.4431953900618826,
+            "y": 4.662720774571155,
+            "heading": -0.464675218076173,
+            "angularVelocity": -0.40641998773593985,
+            "velocityX": 0.9088518462346009,
+            "velocityY": 1.8306233305795314,
+            "timestamp": 0.24579934633320522
+        },
+        {
+            "x": 2.551739981424586,
+            "y": 4.8571070042037645,
+            "heading": -0.49532439278782164,
+            "angularVelocity": -0.3740755437574734,
+            "velocityX": 1.3247951182371547,
+            "velocityY": 2.3724989410968234,
+            "timestamp": 0.32773246177760695
+        },
+        {
+            "x": 2.7012931711639805,
+            "y": 5.089437783878959,
+            "heading": -0.5138025644966426,
+            "angularVelocity": -0.22552751239337346,
+            "velocityX": 1.8253082276711035,
+            "velocityY": 2.835615104039969,
+            "timestamp": 0.4096655772220087
+        },
+        {
+            "x": 2.906580390144238,
+            "y": 5.321655181575939,
+            "heading": -0.513802224550241,
+            "angularVelocity": 0.0000041490720786488115,
+            "velocityX": 2.5055463577429817,
+            "velocityY": 2.834231268241715,
+            "timestamp": 0.49159869266641043
+        },
+        {
+            "x": 3.150285117410734,
+            "y": 5.513164499731697,
+            "heading": -0.5138021244683586,
+            "angularVelocity": 0.0000012215071026742433,
+            "velocityX": 2.974435012566654,
+            "velocityY": 2.3373860143974894,
+            "timestamp": 0.5735318081108122
+        },
+        {
+            "x": 3.407534970443545,
+            "y": 5.686052692352735,
+            "heading": -0.513802030650944,
+            "angularVelocity": 0.0000011450487881316094,
+            "velocityX": 3.1397543183546524,
+            "velocityY": 2.110113739521574,
+            "timestamp": 0.6554649235552139
+        },
+        {
+            "x": 3.664784970537928,
+            "y": 5.858940666152602,
+            "heading": -0.5138019368336011,
+            "angularVelocity": 0.0000011450479143905032,
+            "velocityX": 3.1397561132524885,
+            "velocityY": 2.1101110687922637,
+            "timestamp": 0.7373980389996156
+        },
+        {
+            "x": 3.922035018762957,
+            "y": 6.031828568336093,
+            "heading": -0.5138018430162984,
+            "angularVelocity": 0.0000011450474228928023,
+            "velocityX": 3.139756700690747,
+            "velocityY": 2.110110194708886,
+            "timestamp": 0.8193311544440174
+        },
+        {
+            "x": 4.1792852160642955,
+            "y": 6.204716248700005,
+            "heading": -0.5138017491990241,
+            "angularVelocity": 0.0000011450470770765712,
+            "velocityX": 3.1397585201786073,
+            "velocityY": 2.1101074873837833,
+            "timestamp": 0.9012642698884191
+        },
+        {
+            "x": 4.443584051036782,
+            "y": 6.354085007666646,
+            "heading": -0.49783246108316886,
+            "angularVelocity": 0.19490639443210175,
+            "velocityX": 3.225787687175579,
+            "velocityY": 1.8230572358499022,
+            "timestamp": 0.9831973853328209
+        },
+        {
+            "x": 4.674708985410259,
+            "y": 6.469317132198814,
+            "heading": -0.3712043024716245,
+            "angularVelocity": 1.545506452729414,
+            "velocityX": 2.8208976690299576,
+            "velocityY": 1.4064169769104102,
+            "timestamp": 1.0651305007772227
+        },
+        {
+            "x": 4.870816846139177,
+            "y": 6.548974010532027,
+            "heading": -0.15522222327575852,
+            "angularVelocity": 2.636077952417564,
+            "velocityX": 2.393511581553284,
+            "velocityY": 0.9722183503111966,
+            "timestamp": 1.1470636162216246
+        },
+        {
+            "x": 5.022935939261114,
+            "y": 6.594453280726005,
+            "heading": 0.041233837030506995,
+            "angularVelocity": 2.3977613842791428,
+            "velocityX": 1.856625276566729,
+            "velocityY": 0.5550780041415454,
+            "timestamp": 1.2289967316660264
+        },
+        {
+            "x": 5.124959991715153,
+            "y": 6.619307482178149,
+            "heading": 0.1862409016069847,
+            "angularVelocity": 1.7698224190545027,
+            "velocityX": 1.245211437410438,
+            "velocityY": 0.30334744769969435,
+            "timestamp": 1.3109298471104283
         },
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": 0.93745843058738,
-            "velocityX": 0.6229135643171693,
-            "velocityY": 0.13023271178533075,
-            "timestamp": 1.3927987384573166
+            "heading": 0.263,
+            "angularVelocity": 0.9368507224054672,
+            "velocityX": 0.6229472418814781,
+            "velocityY": 0.13050300557783806,
+            "timestamp": 1.39286296255483
         },
         {
             "x": 5.176,
             "y": 6.63,
-            "heading": 3.4031853071795863,
-            "angularVelocity": -5.432893499766539e-33,
+            "heading": 0.263,
+            "angularVelocity": 1.6488266469177426e-31,
             "velocityX": 0,
-            "velocityY": 6.79272724985994e-33,
-            "timestamp": 1.4747280760136294
+            "velocityY": 1.9617182542723147e-31,
+            "timestamp": 1.474796077999232
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.1.traj
index 06c38028..06a0b427 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.1.traj	
@@ -3,128 +3,128 @@
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
+            "heading": -0.39,
             "angularVelocity": 0,
-            "velocityX": -1.3306255005182603e-31,
-            "velocityY": -4.9176166375307755e-31,
+            "velocityX": -1.8814689011962518e-30,
+            "velocityY": -8.215910483342913e-30,
             "timestamp": 0
         },
         {
-            "x": 2.322374290257025,
-            "y": 4.418269020876138,
-            "heading": 2.7110352663465282,
-            "angularVelocity": -0.44706114894751375,
-            "velocityX": 0.25671100431863964,
-            "velocityY": 0.6685485303865802,
-            "timestamp": 0.08715750349857801
+            "x": 2.3223907686912884,
+            "y": 4.418261357303619,
+            "heading": -0.4292195342409252,
+            "angularVelocity": -0.4499474209247965,
+            "velocityX": 0.25687884418209317,
+            "velocityY": 0.6684053741511637,
+            "timestamp": 0.08716470506779908
         },
         {
-            "x": 2.3748782855305732,
-            "y": 4.5313296521256,
-            "heading": 2.6355865457570187,
-            "angularVelocity": -0.8656594964395732,
-            "velocityX": 0.6024036160513037,
-            "velocityY": 1.297199055859919,
-            "timestamp": 0.17431500699715602
+            "x": 2.37490967527786,
+            "y": 4.531319644696227,
+            "heading": -0.5050759172241152,
+            "angularVelocity": -0.8702648959138656,
+            "velocityX": 0.6025249158558101,
+            "velocityY": 1.2970649909807879,
+            "timestamp": 0.17432941013559816
         },
         {
-            "x": 2.469236718208719,
-            "y": 4.691158857574848,
-            "heading": 2.529430325019821,
-            "angularVelocity": -1.2179814298941005,
-            "velocityX": 1.0826197273959968,
-            "velocityY": 1.8337974245884079,
-            "timestamp": 0.26147251049573406
+            "x": 2.4692607215165214,
+            "y": 4.691180973762412,
+            "heading": -0.6116326818995184,
+            "angularVelocity": -1.222476053725161,
+            "velocityX": 1.0824455399151758,
+            "velocityY": 1.8340144550691926,
+            "timestamp": 0.26149411520339727
         },
         {
-            "x": 2.6203114191501853,
-            "y": 4.878748173786945,
-            "heading": 2.4055559201698435,
-            "angularVelocity": -1.421270686717166,
-            "velocityX": 1.7333527794761936,
-            "velocityY": 2.1523025405971743,
-            "timestamp": 0.34863001399431204
+            "x": 2.6203318015659547,
+            "y": 4.878827771954509,
+            "heading": -0.7357227336999919,
+            "angularVelocity": -1.4236272778523056,
+            "velocityX": 1.7331680286410147,
+            "velocityY": 2.1527841807775245,
+            "timestamp": 0.3486588202711963
         },
         {
-            "x": 2.832356831160347,
-            "y": 5.08316808870822,
-            "heading": 2.2936588957805637,
-            "angularVelocity": -1.2838484341294274,
-            "velocityX": 2.4328991021825375,
-            "velocityY": 2.345408102752848,
-            "timestamp": 0.43578751749289
+            "x": 2.8323811449649123,
+            "y": 5.083303607446834,
+            "heading": -0.8475943342447533,
+            "angularVelocity": -1.2834506863500095,
+            "velocityX": 2.432743198454233,
+            "velocityY": 2.3458558751879917,
+            "timestamp": 0.4358235253389954
         },
         {
-            "x": 3.098575041354179,
-            "y": 5.2672072069550175,
-            "heading": 2.274563036259226,
-            "angularVelocity": -0.2190959900732964,
-            "velocityX": 3.054449697474129,
-            "velocityY": 2.1115694100829816,
-            "timestamp": 0.522945020991468
+            "x": 3.0986117768734247,
+            "y": 5.267387942263234,
+            "heading": -0.8666588377121885,
+            "angularVelocity": -0.2187181549298817,
+            "velocityX": 3.054339846632081,
+            "velocityY": 2.111913700312692,
+            "timestamp": 0.5229882304067944
         },
         {
-            "x": 3.397459622153223,
-            "y": 5.3975878258937575,
-            "heading": 2.2845714271399826,
-            "angularVelocity": 0.11483108715843302,
-            "velocityX": 3.429246694794569,
-            "velocityY": 1.495919613402737,
-            "timestamp": 0.610102524490046
+            "x": 3.3974594561484373,
+            "y": 5.39777666903363,
+            "heading": -0.8564935513214242,
+            "angularVelocity": 0.11662158878249514,
+            "velocityX": 3.428540015623993,
+            "velocityY": 1.4958890375292995,
+            "timestamp": 0.6101529354745935
         },
         {
-            "x": 3.6781811959965687,
-            "y": 5.472346031669266,
-            "heading": 2.382872565425927,
-            "angularVelocity": 1.1278562870671076,
-            "velocityX": 3.220853771332781,
-            "velocityY": 0.8577368875271776,
-            "timestamp": 0.697260027988624
+            "x": 3.678181440932401,
+            "y": 5.472509264787258,
+            "heading": -0.7581228056155188,
+            "angularVelocity": 1.1285616767634317,
+            "velocityX": 3.2205923781375785,
+            "velocityY": 0.8573722092617639,
+            "timestamp": 0.6973176405423925
         },
         {
-            "x": 3.9150228109281926,
-            "y": 5.514569108720165,
-            "heading": 2.601398031866265,
-            "angularVelocity": 2.5072478865104606,
-            "velocityX": 2.717397876540693,
-            "velocityY": 0.484445691489864,
-            "timestamp": 0.784417531487202
+            "x": 3.9150331223150032,
+            "y": 5.514676359956983,
+            "heading": -0.5395207483783628,
+            "angularVelocity": 2.507919427560975,
+            "velocityX": 2.7172888521606593,
+            "velocityY": 0.483763412460658,
+            "timestamp": 0.7844823456101916
         },
         {
-            "x": 4.091172675855641,
-            "y": 5.53970946369802,
-            "heading": 2.8415186627639684,
-            "angularVelocity": 2.7550196054160887,
-            "velocityX": 2.0210522084346136,
-            "velocityY": 0.28844739659465746,
-            "timestamp": 0.8715750349857799
+            "x": 4.091189365639749,
+            "y": 5.539762978420037,
+            "heading": -0.2990438366325736,
+            "angularVelocity": 2.758879428992955,
+            "velocityX": 2.0209584049843006,
+            "velocityY": 0.28780707103340436,
+            "timestamp": 0.8716470506779906
         },
         {
-            "x": 4.206403236830247,
-            "y": 5.5538325492768035,
-            "heading": 3.0331865406048246,
-            "angularVelocity": 2.1990978418052483,
-            "velocityX": 1.322095704318632,
-            "velocityY": 0.16204096046663827,
-            "timestamp": 0.9587325384843579
+            "x": 4.206412444969991,
+            "y": 5.553849615471576,
+            "heading": -0.10702573317574342,
+            "angularVelocity": 2.202934126920672,
+            "velocityX": 1.3219006390329475,
+            "velocityY": 0.16160941565260478,
+            "timestamp": 0.9588117557457897
         },
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 1.2255222454474992,
-            "velocityX": 0.649361912605446,
-            "velocityY": 0.0707621314933191,
-            "timestamp": 1.045890041982936
+            "heading": 0,
+            "angularVelocity": 1.2278563105616622,
+            "velocityX": 0.6492026214738336,
+            "velocityY": 0.07056049261727199,
+            "timestamp": 1.0459764608135889
         },
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 2.336425691005573e-30,
-            "velocityX": -1.4618072975561068e-30,
-            "velocityY": 3.4366383722351896e-30,
-            "timestamp": 1.133047545481514
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 1.8862699039176484e-32,
+            "velocityY": 0,
+            "timestamp": 1.133141165881388
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.traj
index 06c38028..06a0b427 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.traj	
@@ -3,128 +3,128 @@
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
+            "heading": -0.39,
             "angularVelocity": 0,
-            "velocityX": -1.3306255005182603e-31,
-            "velocityY": -4.9176166375307755e-31,
+            "velocityX": -1.8814689011962518e-30,
+            "velocityY": -8.215910483342913e-30,
             "timestamp": 0
         },
         {
-            "x": 2.322374290257025,
-            "y": 4.418269020876138,
-            "heading": 2.7110352663465282,
-            "angularVelocity": -0.44706114894751375,
-            "velocityX": 0.25671100431863964,
-            "velocityY": 0.6685485303865802,
-            "timestamp": 0.08715750349857801
+            "x": 2.3223907686912884,
+            "y": 4.418261357303619,
+            "heading": -0.4292195342409252,
+            "angularVelocity": -0.4499474209247965,
+            "velocityX": 0.25687884418209317,
+            "velocityY": 0.6684053741511637,
+            "timestamp": 0.08716470506779908
         },
         {
-            "x": 2.3748782855305732,
-            "y": 4.5313296521256,
-            "heading": 2.6355865457570187,
-            "angularVelocity": -0.8656594964395732,
-            "velocityX": 0.6024036160513037,
-            "velocityY": 1.297199055859919,
-            "timestamp": 0.17431500699715602
+            "x": 2.37490967527786,
+            "y": 4.531319644696227,
+            "heading": -0.5050759172241152,
+            "angularVelocity": -0.8702648959138656,
+            "velocityX": 0.6025249158558101,
+            "velocityY": 1.2970649909807879,
+            "timestamp": 0.17432941013559816
         },
         {
-            "x": 2.469236718208719,
-            "y": 4.691158857574848,
-            "heading": 2.529430325019821,
-            "angularVelocity": -1.2179814298941005,
-            "velocityX": 1.0826197273959968,
-            "velocityY": 1.8337974245884079,
-            "timestamp": 0.26147251049573406
+            "x": 2.4692607215165214,
+            "y": 4.691180973762412,
+            "heading": -0.6116326818995184,
+            "angularVelocity": -1.222476053725161,
+            "velocityX": 1.0824455399151758,
+            "velocityY": 1.8340144550691926,
+            "timestamp": 0.26149411520339727
         },
         {
-            "x": 2.6203114191501853,
-            "y": 4.878748173786945,
-            "heading": 2.4055559201698435,
-            "angularVelocity": -1.421270686717166,
-            "velocityX": 1.7333527794761936,
-            "velocityY": 2.1523025405971743,
-            "timestamp": 0.34863001399431204
+            "x": 2.6203318015659547,
+            "y": 4.878827771954509,
+            "heading": -0.7357227336999919,
+            "angularVelocity": -1.4236272778523056,
+            "velocityX": 1.7331680286410147,
+            "velocityY": 2.1527841807775245,
+            "timestamp": 0.3486588202711963
         },
         {
-            "x": 2.832356831160347,
-            "y": 5.08316808870822,
-            "heading": 2.2936588957805637,
-            "angularVelocity": -1.2838484341294274,
-            "velocityX": 2.4328991021825375,
-            "velocityY": 2.345408102752848,
-            "timestamp": 0.43578751749289
+            "x": 2.8323811449649123,
+            "y": 5.083303607446834,
+            "heading": -0.8475943342447533,
+            "angularVelocity": -1.2834506863500095,
+            "velocityX": 2.432743198454233,
+            "velocityY": 2.3458558751879917,
+            "timestamp": 0.4358235253389954
         },
         {
-            "x": 3.098575041354179,
-            "y": 5.2672072069550175,
-            "heading": 2.274563036259226,
-            "angularVelocity": -0.2190959900732964,
-            "velocityX": 3.054449697474129,
-            "velocityY": 2.1115694100829816,
-            "timestamp": 0.522945020991468
+            "x": 3.0986117768734247,
+            "y": 5.267387942263234,
+            "heading": -0.8666588377121885,
+            "angularVelocity": -0.2187181549298817,
+            "velocityX": 3.054339846632081,
+            "velocityY": 2.111913700312692,
+            "timestamp": 0.5229882304067944
         },
         {
-            "x": 3.397459622153223,
-            "y": 5.3975878258937575,
-            "heading": 2.2845714271399826,
-            "angularVelocity": 0.11483108715843302,
-            "velocityX": 3.429246694794569,
-            "velocityY": 1.495919613402737,
-            "timestamp": 0.610102524490046
+            "x": 3.3974594561484373,
+            "y": 5.39777666903363,
+            "heading": -0.8564935513214242,
+            "angularVelocity": 0.11662158878249514,
+            "velocityX": 3.428540015623993,
+            "velocityY": 1.4958890375292995,
+            "timestamp": 0.6101529354745935
         },
         {
-            "x": 3.6781811959965687,
-            "y": 5.472346031669266,
-            "heading": 2.382872565425927,
-            "angularVelocity": 1.1278562870671076,
-            "velocityX": 3.220853771332781,
-            "velocityY": 0.8577368875271776,
-            "timestamp": 0.697260027988624
+            "x": 3.678181440932401,
+            "y": 5.472509264787258,
+            "heading": -0.7581228056155188,
+            "angularVelocity": 1.1285616767634317,
+            "velocityX": 3.2205923781375785,
+            "velocityY": 0.8573722092617639,
+            "timestamp": 0.6973176405423925
         },
         {
-            "x": 3.9150228109281926,
-            "y": 5.514569108720165,
-            "heading": 2.601398031866265,
-            "angularVelocity": 2.5072478865104606,
-            "velocityX": 2.717397876540693,
-            "velocityY": 0.484445691489864,
-            "timestamp": 0.784417531487202
+            "x": 3.9150331223150032,
+            "y": 5.514676359956983,
+            "heading": -0.5395207483783628,
+            "angularVelocity": 2.507919427560975,
+            "velocityX": 2.7172888521606593,
+            "velocityY": 0.483763412460658,
+            "timestamp": 0.7844823456101916
         },
         {
-            "x": 4.091172675855641,
-            "y": 5.53970946369802,
-            "heading": 2.8415186627639684,
-            "angularVelocity": 2.7550196054160887,
-            "velocityX": 2.0210522084346136,
-            "velocityY": 0.28844739659465746,
-            "timestamp": 0.8715750349857799
+            "x": 4.091189365639749,
+            "y": 5.539762978420037,
+            "heading": -0.2990438366325736,
+            "angularVelocity": 2.758879428992955,
+            "velocityX": 2.0209584049843006,
+            "velocityY": 0.28780707103340436,
+            "timestamp": 0.8716470506779906
         },
         {
-            "x": 4.206403236830247,
-            "y": 5.5538325492768035,
-            "heading": 3.0331865406048246,
-            "angularVelocity": 2.1990978418052483,
-            "velocityX": 1.322095704318632,
-            "velocityY": 0.16204096046663827,
-            "timestamp": 0.9587325384843579
+            "x": 4.206412444969991,
+            "y": 5.553849615471576,
+            "heading": -0.10702573317574342,
+            "angularVelocity": 2.202934126920672,
+            "velocityX": 1.3219006390329475,
+            "velocityY": 0.16160941565260478,
+            "timestamp": 0.9588117557457897
         },
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 1.2255222454474992,
-            "velocityX": 0.649361912605446,
-            "velocityY": 0.0707621314933191,
-            "timestamp": 1.045890041982936
+            "heading": 0,
+            "angularVelocity": 1.2278563105616622,
+            "velocityX": 0.6492026214738336,
+            "velocityY": 0.07056049261727199,
+            "timestamp": 1.0459764608135889
         },
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 2.336425691005573e-30,
-            "velocityX": -1.4618072975561068e-30,
-            "velocityY": 3.4366383722351896e-30,
-            "timestamp": 1.133047545481514
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 1.8862699039176484e-32,
+            "velocityY": 0,
+            "timestamp": 1.133141165881388
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.1.traj
index 3d841ff3..b542317e 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.1.traj	
@@ -3,119 +3,119 @@
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": -2.857623496877536e-30,
-            "velocityX": -2.5600502945380354e-30,
-            "velocityY": 7.096305433334873e-32,
+            "heading": -0.39,
+            "angularVelocity": 1.6493730429010574e-29,
+            "velocityX": 1.2833975714085328e-29,
+            "velocityY": -2.2715148176984883e-30,
             "timestamp": 0
         },
         {
-            "x": 2.2589562715242293,
-            "y": 4.250539211803029,
-            "heading": 2.836221335933998,
-            "angularVelocity": 0.7208664352087386,
-            "velocityX": -0.3431522593974162,
-            "velocityY": -0.9151633679526462,
-            "timestamp": 0.1196079214161022
+            "x": 2.2589715401081683,
+            "y": 4.250531667444243,
+            "heading": -0.30389257282100673,
+            "angularVelocity": 0.7199270532350494,
+            "velocityX": -0.3430307837128689,
+            "velocityY": -0.9152429315492105,
+            "timestamp": 0.11960576671208706
         },
         {
-            "x": 2.2003066234183892,
-            "y": 4.024396154786088,
-            "heading": 2.9853349315252267,
-            "angularVelocity": 1.2466866226394515,
-            "velocityX": -0.49034919603525046,
-            "velocityY": -1.8907030097966573,
-            "timestamp": 0.2392158428322044
+            "x": 2.20035382258247,
+            "y": 4.024377658967842,
+            "heading": -0.15502297939398071,
+            "angularVelocity": 1.244669028252838,
+            "velocityX": -0.49009106447848483,
+            "velocityY": -1.890828633880168,
+            "timestamp": 0.23921153342417412
         },
         {
-            "x": 2.1964987876971427,
-            "y": 3.692388997223788,
-            "heading": 3.120971884356915,
-            "angularVelocity": 1.1340131257679336,
-            "velocityX": -0.03183598273568803,
-            "velocityY": -2.7757957301782654,
-            "timestamp": 0.3588237642483066
+            "x": 2.196580641751606,
+            "y": 3.6923673074709873,
+            "heading": -0.01977955961101222,
+            "angularVelocity": 1.1307433036110668,
+            "velocityX": -0.031546813624408795,
+            "velocityY": -2.7758724401313977,
+            "timestamp": 0.3588173001362612
         },
         {
-            "x": 2.306992116857372,
-            "y": 3.3594977499513337,
-            "heading": 3.1303317506759742,
-            "angularVelocity": 0.07825456883065958,
-            "velocityX": 0.9237960818316977,
-            "velocityY": -2.7831872950485823,
-            "timestamp": 0.4784316856644088
+            "x": 2.3070960866935795,
+            "y": 3.359465621426977,
+            "heading": -0.010919467382492536,
+            "angularVelocity": 0.07407746693291484,
+            "velocityX": 0.9239976297129873,
+            "velocityY": -2.7833247108007275,
+            "timestamp": 0.47842306684834823
         },
         {
-            "x": 2.522086295400282,
-            "y": 3.073270003946497,
-            "heading": 3.020615571964542,
-            "angularVelocity": -0.9172985987240034,
-            "velocityX": 1.7983272010439872,
-            "velocityY": -2.39305008076411,
-            "timestamp": 0.598039607080511
+            "x": 2.522207133394474,
+            "y": 3.0732241883923836,
+            "heading": -0.12115396742434953,
+            "angularVelocity": -0.9216487053427286,
+            "velocityX": 1.7985006293108312,
+            "velocityY": -2.3932076262144095,
+            "timestamp": 0.5980288335604353
         },
         {
-            "x": 2.8292729817758957,
-            "y": 2.8628580598282483,
-            "heading": 2.911598621970614,
-            "angularVelocity": -0.9114525919611488,
-            "velocityX": 2.568280451149536,
-            "velocityY": -1.7591806765559896,
-            "timestamp": 0.7176475284966132
+            "x": 2.8293982195436245,
+            "y": 2.8628067869682225,
+            "heading": -0.23050381325401667,
+            "angularVelocity": -0.9142522876239637,
+            "velocityX": 2.5683635044839206,
+            "velocityY": -1.759257995734114,
+            "timestamp": 0.7176346002725225
         },
         {
-            "x": 3.212139121131616,
-            "y": 2.7446490517974147,
-            "heading": 2.8046662811978735,
-            "angularVelocity": -0.8940239033224252,
-            "velocityX": 3.2010098898363166,
-            "velocityY": -0.9883041744332437,
-            "timestamp": 0.8372554499127154
+            "x": 3.2122552089453738,
+            "y": 2.74459738527869,
+            "heading": -0.337506021977117,
+            "angularVelocity": -0.8946241612299765,
+            "velocityX": 3.200991055250252,
+            "velocityY": -0.9883252700855956,
+            "timestamp": 0.8372403669846096
         },
         {
-            "x": 3.61962025055875,
-            "y": 2.7431499033487023,
-            "heading": 2.702792215888405,
-            "angularVelocity": -0.8517334312273422,
-            "velocityX": 3.406807213124051,
-            "velocityY": -0.012533855876376997,
-            "timestamp": 0.9568633713288176
+            "x": 3.6197114657100724,
+            "y": 2.743095325026798,
+            "heading": -0.4390431608485269,
+            "angularVelocity": -0.8489318003875522,
+            "velocityX": 3.406660631552456,
+            "velocityY": -0.012558426681036643,
+            "timestamp": 0.9568461336966967
         },
         {
-            "x": 3.9493476486905155,
-            "y": 2.8320906325242436,
-            "heading": 2.608128750394254,
-            "angularVelocity": -0.7914481279620891,
-            "velocityX": 2.7567354588888784,
-            "velocityY": 0.7436023310372396,
-            "timestamp": 1.0764712927449198
+            "x": 3.949401767099036,
+            "y": 2.832014285774074,
+            "heading": -0.5329543188084961,
+            "angularVelocity": -0.7851724924441524,
+            "velocityX": 2.7564749631394143,
+            "velocityY": 0.7434337255770672,
+            "timestamp": 1.0764519004087838
         },
         {
-            "x": 4.162172137163363,
-            "y": 2.936272219295665,
-            "heading": 2.5785368758755864,
-            "angularVelocity": -0.2474073135651704,
-            "velocityX": 1.7793511161560736,
-            "velocityY": 0.8710258111499355,
-            "timestamp": 1.196079214161022
+            "x": 4.162198776267108,
+            "y": 2.936236025183353,
+            "heading": -0.5618569264034092,
+            "angularVelocity": -0.24164894711549667,
+            "velocityX": 1.7791534222619447,
+            "velocityY": 0.8713772109346234,
+            "timestamp": 1.196057667120871
         },
         {
             "x": 4.263,
             "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -0.037931232495828804,
-            "velocityX": 0.842986498242598,
-            "velocityY": 0.5328056866955708,
-            "timestamp": 1.315687135577124
+            "heading": -0.566,
+            "angularVelocity": -0.03463941338677448,
+            "velocityX": 0.8427789604454271,
+            "velocityY": 0.5331178969835093,
+            "timestamp": 1.315663433832958
         },
         {
             "x": 4.263,
             "y": 3,
-            "heading": 2.574,
+            "heading": -0.566,
             "angularVelocity": 0,
-            "velocityX": 1.3483931579260095e-31,
-            "velocityY": -3.3355137434500586e-29,
-            "timestamp": 1.4352950569932261
+            "velocityX": 1.4642265257925552e-30,
+            "velocityY": 5.326911647277941e-29,
+            "timestamp": 1.4352692005450451
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.traj
index 3d841ff3..b542317e 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.traj	
@@ -3,119 +3,119 @@
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": -2.857623496877536e-30,
-            "velocityX": -2.5600502945380354e-30,
-            "velocityY": 7.096305433334873e-32,
+            "heading": -0.39,
+            "angularVelocity": 1.6493730429010574e-29,
+            "velocityX": 1.2833975714085328e-29,
+            "velocityY": -2.2715148176984883e-30,
             "timestamp": 0
         },
         {
-            "x": 2.2589562715242293,
-            "y": 4.250539211803029,
-            "heading": 2.836221335933998,
-            "angularVelocity": 0.7208664352087386,
-            "velocityX": -0.3431522593974162,
-            "velocityY": -0.9151633679526462,
-            "timestamp": 0.1196079214161022
+            "x": 2.2589715401081683,
+            "y": 4.250531667444243,
+            "heading": -0.30389257282100673,
+            "angularVelocity": 0.7199270532350494,
+            "velocityX": -0.3430307837128689,
+            "velocityY": -0.9152429315492105,
+            "timestamp": 0.11960576671208706
         },
         {
-            "x": 2.2003066234183892,
-            "y": 4.024396154786088,
-            "heading": 2.9853349315252267,
-            "angularVelocity": 1.2466866226394515,
-            "velocityX": -0.49034919603525046,
-            "velocityY": -1.8907030097966573,
-            "timestamp": 0.2392158428322044
+            "x": 2.20035382258247,
+            "y": 4.024377658967842,
+            "heading": -0.15502297939398071,
+            "angularVelocity": 1.244669028252838,
+            "velocityX": -0.49009106447848483,
+            "velocityY": -1.890828633880168,
+            "timestamp": 0.23921153342417412
         },
         {
-            "x": 2.1964987876971427,
-            "y": 3.692388997223788,
-            "heading": 3.120971884356915,
-            "angularVelocity": 1.1340131257679336,
-            "velocityX": -0.03183598273568803,
-            "velocityY": -2.7757957301782654,
-            "timestamp": 0.3588237642483066
+            "x": 2.196580641751606,
+            "y": 3.6923673074709873,
+            "heading": -0.01977955961101222,
+            "angularVelocity": 1.1307433036110668,
+            "velocityX": -0.031546813624408795,
+            "velocityY": -2.7758724401313977,
+            "timestamp": 0.3588173001362612
         },
         {
-            "x": 2.306992116857372,
-            "y": 3.3594977499513337,
-            "heading": 3.1303317506759742,
-            "angularVelocity": 0.07825456883065958,
-            "velocityX": 0.9237960818316977,
-            "velocityY": -2.7831872950485823,
-            "timestamp": 0.4784316856644088
+            "x": 2.3070960866935795,
+            "y": 3.359465621426977,
+            "heading": -0.010919467382492536,
+            "angularVelocity": 0.07407746693291484,
+            "velocityX": 0.9239976297129873,
+            "velocityY": -2.7833247108007275,
+            "timestamp": 0.47842306684834823
         },
         {
-            "x": 2.522086295400282,
-            "y": 3.073270003946497,
-            "heading": 3.020615571964542,
-            "angularVelocity": -0.9172985987240034,
-            "velocityX": 1.7983272010439872,
-            "velocityY": -2.39305008076411,
-            "timestamp": 0.598039607080511
+            "x": 2.522207133394474,
+            "y": 3.0732241883923836,
+            "heading": -0.12115396742434953,
+            "angularVelocity": -0.9216487053427286,
+            "velocityX": 1.7985006293108312,
+            "velocityY": -2.3932076262144095,
+            "timestamp": 0.5980288335604353
         },
         {
-            "x": 2.8292729817758957,
-            "y": 2.8628580598282483,
-            "heading": 2.911598621970614,
-            "angularVelocity": -0.9114525919611488,
-            "velocityX": 2.568280451149536,
-            "velocityY": -1.7591806765559896,
-            "timestamp": 0.7176475284966132
+            "x": 2.8293982195436245,
+            "y": 2.8628067869682225,
+            "heading": -0.23050381325401667,
+            "angularVelocity": -0.9142522876239637,
+            "velocityX": 2.5683635044839206,
+            "velocityY": -1.759257995734114,
+            "timestamp": 0.7176346002725225
         },
         {
-            "x": 3.212139121131616,
-            "y": 2.7446490517974147,
-            "heading": 2.8046662811978735,
-            "angularVelocity": -0.8940239033224252,
-            "velocityX": 3.2010098898363166,
-            "velocityY": -0.9883041744332437,
-            "timestamp": 0.8372554499127154
+            "x": 3.2122552089453738,
+            "y": 2.74459738527869,
+            "heading": -0.337506021977117,
+            "angularVelocity": -0.8946241612299765,
+            "velocityX": 3.200991055250252,
+            "velocityY": -0.9883252700855956,
+            "timestamp": 0.8372403669846096
         },
         {
-            "x": 3.61962025055875,
-            "y": 2.7431499033487023,
-            "heading": 2.702792215888405,
-            "angularVelocity": -0.8517334312273422,
-            "velocityX": 3.406807213124051,
-            "velocityY": -0.012533855876376997,
-            "timestamp": 0.9568633713288176
+            "x": 3.6197114657100724,
+            "y": 2.743095325026798,
+            "heading": -0.4390431608485269,
+            "angularVelocity": -0.8489318003875522,
+            "velocityX": 3.406660631552456,
+            "velocityY": -0.012558426681036643,
+            "timestamp": 0.9568461336966967
         },
         {
-            "x": 3.9493476486905155,
-            "y": 2.8320906325242436,
-            "heading": 2.608128750394254,
-            "angularVelocity": -0.7914481279620891,
-            "velocityX": 2.7567354588888784,
-            "velocityY": 0.7436023310372396,
-            "timestamp": 1.0764712927449198
+            "x": 3.949401767099036,
+            "y": 2.832014285774074,
+            "heading": -0.5329543188084961,
+            "angularVelocity": -0.7851724924441524,
+            "velocityX": 2.7564749631394143,
+            "velocityY": 0.7434337255770672,
+            "timestamp": 1.0764519004087838
         },
         {
-            "x": 4.162172137163363,
-            "y": 2.936272219295665,
-            "heading": 2.5785368758755864,
-            "angularVelocity": -0.2474073135651704,
-            "velocityX": 1.7793511161560736,
-            "velocityY": 0.8710258111499355,
-            "timestamp": 1.196079214161022
+            "x": 4.162198776267108,
+            "y": 2.936236025183353,
+            "heading": -0.5618569264034092,
+            "angularVelocity": -0.24164894711549667,
+            "velocityX": 1.7791534222619447,
+            "velocityY": 0.8713772109346234,
+            "timestamp": 1.196057667120871
         },
         {
             "x": 4.263,
             "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -0.037931232495828804,
-            "velocityX": 0.842986498242598,
-            "velocityY": 0.5328056866955708,
-            "timestamp": 1.315687135577124
+            "heading": -0.566,
+            "angularVelocity": -0.03463941338677448,
+            "velocityX": 0.8427789604454271,
+            "velocityY": 0.5331178969835093,
+            "timestamp": 1.315663433832958
         },
         {
             "x": 4.263,
             "y": 3,
-            "heading": 2.574,
+            "heading": -0.566,
             "angularVelocity": 0,
-            "velocityX": 1.3483931579260095e-31,
-            "velocityY": -3.3355137434500586e-29,
-            "timestamp": 1.4352950569932261
+            "velocityX": 1.4642265257925552e-30,
+            "velocityY": 5.326911647277941e-29,
+            "timestamp": 1.4352692005450451
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.1.traj
deleted file mode 100644
index 1aabb2f0..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.1.traj	
+++ /dev/null
@@ -1,166 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": -2.979591758162217e-32,
-            "velocityX": -5.0524169564327655e-33,
-            "velocityY": 3.4190943066430706e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2859321803383907,
-            "y": 4.28632531036449,
-            "heading": 2.8392348343623115,
-            "angularVelocity": 0.9181215337942725,
-            "velocityX": -0.14474132503475426,
-            "velocityY": -0.7580259383384993,
-            "timestamp": 0.09719283458425766
-        },
-        {
-            "x": 2.269559639424535,
-            "y": 4.136180116377401,
-            "heading": 2.9969853519762393,
-            "angularVelocity": 1.623067361793753,
-            "velocityX": -0.1684541971009417,
-            "velocityY": -1.5448175231161345,
-            "timestamp": 0.19438566916851532
-        },
-        {
-            "x": 2.2724010855339243,
-            "y": 3.9101733835281753,
-            "heading": 3.1867879122348075,
-            "angularVelocity": 1.952845197595569,
-            "velocityX": 0.029235139828396976,
-            "velocityY": -2.325343568957208,
-            "timestamp": 0.291578503752773
-        },
-        {
-            "x": 2.3226715061490353,
-            "y": 3.6216059722814986,
-            "heading": 3.3538116791494788,
-            "angularVelocity": 1.7184781947055583,
-            "velocityX": 0.5172235260978133,
-            "velocityY": -2.969019398199714,
-            "timestamp": 0.38877133833703065
-        },
-        {
-            "x": 2.437426080371791,
-            "y": 3.296473518840458,
-            "heading": 3.427293312254084,
-            "angularVelocity": 0.7560396136085858,
-            "velocityX": 1.1806896538579041,
-            "velocityY": -3.3452306935155747,
-            "timestamp": 0.4859641729212883
-        },
-        {
-            "x": 2.6276283995685685,
-            "y": 2.9818185735083116,
-            "heading": 3.4272931426736677,
-            "angularVelocity": -0.000001744783111288276,
-            "velocityX": 1.9569582470803324,
-            "velocityY": -3.2374294532933714,
-            "timestamp": 0.583157007505546
-        },
-        {
-            "x": 2.8804858808256992,
-            "y": 2.7148958271590775,
-            "heading": 3.4272929522201836,
-            "angularVelocity": -0.00000195954243555236,
-            "velocityX": 2.6016062021313475,
-            "velocityY": -2.7463212436492483,
-            "timestamp": 0.6803498420898036
-        },
-        {
-            "x": 3.1843940400703103,
-            "y": 2.5079549973523347,
-            "heading": 3.4272928289705424,
-            "angularVelocity": -0.0000012680939050558275,
-            "velocityX": 3.126857659256241,
-            "velocityY": -2.1291778420902356,
-            "timestamp": 0.7775426766740613
-        },
-        {
-            "x": 3.5145691798617946,
-            "y": 2.3461864714628624,
-            "heading": 3.4272927145634307,
-            "angularVelocity": -0.0000011771146716870812,
-            "velocityX": 3.397114007465763,
-            "velocityY": -1.66440794304886,
-            "timestamp": 0.8747355112583189
-        },
-        {
-            "x": 3.844744525773579,
-            "y": 2.1844183662726744,
-            "heading": 3.427292600156351,
-            "angularVelocity": -0.0000011771143448425205,
-            "velocityX": 3.397116128201317,
-            "velocityY": -1.6644036145478303,
-            "timestamp": 0.9719283458425766
-        },
-        {
-            "x": 4.174919952487512,
-            "y": 2.022650426003138,
-            "heading": 3.42729248574922,
-            "angularVelocity": -0.0000011771148700868944,
-            "velocityX": 3.3971169595604263,
-            "velocityY": -1.6644019177082188,
-            "timestamp": 1.0691211804268343
-        },
-        {
-            "x": 4.503263696345804,
-            "y": 1.8851757396047895,
-            "heading": 3.3983831121261034,
-            "angularVelocity": -0.29744346634992885,
-            "velocityX": 3.3782710964525586,
-            "velocityY": -1.4144528965164656,
-            "timestamp": 1.166314015011092
-        },
-        {
-            "x": 4.772302673813676,
-            "y": 1.7735532891774939,
-            "heading": 3.2030632550848326,
-            "angularVelocity": -2.009611694902728,
-            "velocityX": 2.7680947738450667,
-            "velocityY": -1.1484637823843746,
-            "timestamp": 1.2635068495953496
-        },
-        {
-            "x": 4.977265937404966,
-            "y": 1.6951772631841453,
-            "heading": 2.8890004427677543,
-            "angularVelocity": -3.2313371007284832,
-            "velocityX": 2.1088310107223527,
-            "velocityY": -0.8063971621838382,
-            "timestamp": 1.3606996841796073
-        },
-        {
-            "x": 5.110692645903806,
-            "y": 1.6448171116124384,
-            "heading": 2.6382240127177106,
-            "angularVelocity": -2.5801946318649924,
-            "velocityX": 1.3728039630655127,
-            "velocityY": -0.5181467521461082,
-            "timestamp": 1.457892518763865
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -1.4221625833731872,
-            "velocityX": 0.6719358929652187,
-            "velocityY": -0.2553389014590782,
-            "timestamp": 1.5550853533481226
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 5.001833480552254e-32,
-            "velocityX": 0,
-            "velocityY": -3.22512482003026e-32,
-            "timestamp": 1.6522781879323802
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.traj
deleted file mode 100644
index 1aabb2f0..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.traj	
+++ /dev/null
@@ -1,166 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": -2.979591758162217e-32,
-            "velocityX": -5.0524169564327655e-33,
-            "velocityY": 3.4190943066430706e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2859321803383907,
-            "y": 4.28632531036449,
-            "heading": 2.8392348343623115,
-            "angularVelocity": 0.9181215337942725,
-            "velocityX": -0.14474132503475426,
-            "velocityY": -0.7580259383384993,
-            "timestamp": 0.09719283458425766
-        },
-        {
-            "x": 2.269559639424535,
-            "y": 4.136180116377401,
-            "heading": 2.9969853519762393,
-            "angularVelocity": 1.623067361793753,
-            "velocityX": -0.1684541971009417,
-            "velocityY": -1.5448175231161345,
-            "timestamp": 0.19438566916851532
-        },
-        {
-            "x": 2.2724010855339243,
-            "y": 3.9101733835281753,
-            "heading": 3.1867879122348075,
-            "angularVelocity": 1.952845197595569,
-            "velocityX": 0.029235139828396976,
-            "velocityY": -2.325343568957208,
-            "timestamp": 0.291578503752773
-        },
-        {
-            "x": 2.3226715061490353,
-            "y": 3.6216059722814986,
-            "heading": 3.3538116791494788,
-            "angularVelocity": 1.7184781947055583,
-            "velocityX": 0.5172235260978133,
-            "velocityY": -2.969019398199714,
-            "timestamp": 0.38877133833703065
-        },
-        {
-            "x": 2.437426080371791,
-            "y": 3.296473518840458,
-            "heading": 3.427293312254084,
-            "angularVelocity": 0.7560396136085858,
-            "velocityX": 1.1806896538579041,
-            "velocityY": -3.3452306935155747,
-            "timestamp": 0.4859641729212883
-        },
-        {
-            "x": 2.6276283995685685,
-            "y": 2.9818185735083116,
-            "heading": 3.4272931426736677,
-            "angularVelocity": -0.000001744783111288276,
-            "velocityX": 1.9569582470803324,
-            "velocityY": -3.2374294532933714,
-            "timestamp": 0.583157007505546
-        },
-        {
-            "x": 2.8804858808256992,
-            "y": 2.7148958271590775,
-            "heading": 3.4272929522201836,
-            "angularVelocity": -0.00000195954243555236,
-            "velocityX": 2.6016062021313475,
-            "velocityY": -2.7463212436492483,
-            "timestamp": 0.6803498420898036
-        },
-        {
-            "x": 3.1843940400703103,
-            "y": 2.5079549973523347,
-            "heading": 3.4272928289705424,
-            "angularVelocity": -0.0000012680939050558275,
-            "velocityX": 3.126857659256241,
-            "velocityY": -2.1291778420902356,
-            "timestamp": 0.7775426766740613
-        },
-        {
-            "x": 3.5145691798617946,
-            "y": 2.3461864714628624,
-            "heading": 3.4272927145634307,
-            "angularVelocity": -0.0000011771146716870812,
-            "velocityX": 3.397114007465763,
-            "velocityY": -1.66440794304886,
-            "timestamp": 0.8747355112583189
-        },
-        {
-            "x": 3.844744525773579,
-            "y": 2.1844183662726744,
-            "heading": 3.427292600156351,
-            "angularVelocity": -0.0000011771143448425205,
-            "velocityX": 3.397116128201317,
-            "velocityY": -1.6644036145478303,
-            "timestamp": 0.9719283458425766
-        },
-        {
-            "x": 4.174919952487512,
-            "y": 2.022650426003138,
-            "heading": 3.42729248574922,
-            "angularVelocity": -0.0000011771148700868944,
-            "velocityX": 3.3971169595604263,
-            "velocityY": -1.6644019177082188,
-            "timestamp": 1.0691211804268343
-        },
-        {
-            "x": 4.503263696345804,
-            "y": 1.8851757396047895,
-            "heading": 3.3983831121261034,
-            "angularVelocity": -0.29744346634992885,
-            "velocityX": 3.3782710964525586,
-            "velocityY": -1.4144528965164656,
-            "timestamp": 1.166314015011092
-        },
-        {
-            "x": 4.772302673813676,
-            "y": 1.7735532891774939,
-            "heading": 3.2030632550848326,
-            "angularVelocity": -2.009611694902728,
-            "velocityX": 2.7680947738450667,
-            "velocityY": -1.1484637823843746,
-            "timestamp": 1.2635068495953496
-        },
-        {
-            "x": 4.977265937404966,
-            "y": 1.6951772631841453,
-            "heading": 2.8890004427677543,
-            "angularVelocity": -3.2313371007284832,
-            "velocityX": 2.1088310107223527,
-            "velocityY": -0.8063971621838382,
-            "timestamp": 1.3606996841796073
-        },
-        {
-            "x": 5.110692645903806,
-            "y": 1.6448171116124384,
-            "heading": 2.6382240127177106,
-            "angularVelocity": -2.5801946318649924,
-            "velocityX": 1.3728039630655127,
-            "velocityY": -0.5181467521461082,
-            "timestamp": 1.457892518763865
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -1.4221625833731872,
-            "velocityX": 0.6719358929652187,
-            "velocityY": -0.2553389014590782,
-            "timestamp": 1.5550853533481226
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": 5.001833480552254e-32,
-            "velocityX": 0,
-            "velocityY": -3.22512482003026e-32,
-            "timestamp": 1.6522781879323802
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.1.traj
index edd74832..282ccdeb 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.1.traj	
@@ -3,146 +3,146 @@
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0,
-            "velocityX": 1.6975195374214185e-34,
-            "velocityY": -4.870070626129674e-33,
+            "heading": -0.39,
+            "angularVelocity": -1.4895643422557431e-33,
+            "velocityX": -9.369180406325922e-35,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 2.300139974954616,
-            "y": 4.403824386953961,
-            "heading": 2.777167813185812,
-            "angularVelocity": 0.371999186817317,
-            "velocityX": 0.0019166271843757909,
-            "velocityY": 0.6000717171507387,
-            "timestamp": 0.07303191552177725
+            "x": 2.300137763138577,
+            "y": 4.403828106227067,
+            "heading": -0.362912658197764,
+            "angularVelocity": 0.3708979956671865,
+            "velocityX": 0.001886345007500309,
+            "velocityY": 0.6001237357356949,
+            "timestamp": 0.07303178264285357
         },
         {
-            "x": 2.3004163368424435,
-            "y": 4.4915620593167,
-            "heading": 2.8296003827219796,
-            "angularVelocity": 0.7179404943929278,
-            "velocityX": 0.003784124869971189,
-            "velocityY": 1.2013606891712643,
-            "timestamp": 0.1460638310435545
+            "x": 2.3004080637884394,
+            "y": 4.491572820860884,
+            "heading": -0.31063890006100675,
+            "angularVelocity": 0.7157672487934603,
+            "velocityX": 0.0037011372320465844,
+            "velocityY": 1.201459302491822,
+            "timestamp": 0.14606356528570713
         },
         {
-            "x": 2.3008287718098006,
-            "y": 4.623340237169809,
-            "heading": 2.9042527526949042,
-            "angularVelocity": 1.022188305476718,
-            "velocityX": 0.005647325068915905,
-            "velocityY": 1.8043916404440152,
-            "timestamp": 0.21909574656533176
+            "x": 2.3008080200629966,
+            "y": 4.623360729086378,
+            "heading": -0.2362185458103229,
+            "angularVelocity": 1.019013250910508,
+            "velocityX": 0.005476468738453403,
+            "velocityY": 1.8045281582400208,
+            "timestamp": 0.2190953479285607
         },
         {
-            "x": 2.30139145489797,
-            "y": 4.799346158732557,
-            "heading": 2.995610111682823,
-            "angularVelocity": 1.2509237685362016,
-            "velocityX": 0.007704619057970502,
-            "velocityY": 2.409986377945481,
-            "timestamp": 0.292127662087109
+            "x": 2.3013470975241233,
+            "y": 4.799377996239057,
+            "heading": -0.1451530728621028,
+            "angularVelocity": 1.2469293457282353,
+            "velocityX": 0.0073814090471083236,
+            "velocityY": 2.4101461142398,
+            "timestamp": 0.29212713057141426
         },
         {
-            "x": 2.3021599471353418,
-            "y": 5.019805776345521,
-            "heading": 3.0915564646710374,
-            "angularVelocity": 1.3137592284513517,
-            "velocityX": 0.01052269041392973,
-            "velocityY": 3.0186750003459215,
-            "timestamp": 0.36515957760888623
+            "x": 2.30206992781241,
+            "y": 5.019849097252827,
+            "heading": -0.0495237523149135,
+            "angularVelocity": 1.3094205986295608,
+            "velocityX": 0.009897475621284645,
+            "velocityY": 3.0188377311277894,
+            "timestamp": 0.36515891321426786
         },
         {
-            "x": 2.303349941373319,
-            "y": 5.282617464741668,
-            "heading": 3.1370629336033304,
-            "angularVelocity": 0.6231038664010262,
-            "velocityX": 0.0162941671387857,
-            "velocityY": 3.598586816715496,
-            "timestamp": 0.43819149313066347
+            "x": 2.303151073626671,
+            "y": 5.282690240833408,
+            "heading": -0.004114697333369339,
+            "angularVelocity": 0.621771143169372,
+            "velocityX": 0.014803771387434566,
+            "velocityY": 3.598996684305928,
+            "timestamp": 0.43819069585712145
         },
         {
-            "x": 2.30367850771832,
-            "y": 5.558892653761466,
-            "heading": 3.1370629627902624,
-            "angularVelocity": 3.996462650884406e-7,
-            "velocityX": 0.004498941903047958,
-            "velocityY": 3.7829377340844195,
-            "timestamp": 0.5112234086524408
+            "x": 2.3035206151085394,
+            "y": 5.5589648754006,
+            "heading": -0.004114668066309664,
+            "angularVelocity": 4.007441502253877e-7,
+            "velocityX": 0.005060009060371044,
+            "velocityY": 3.7829370250792667,
+            "timestamp": 0.511222478499975
         },
         {
-            "x": 2.304007097232836,
-            "y": 5.835167842753699,
-            "heading": 3.137062991977204,
-            "angularVelocity": 3.996463893760175e-7,
-            "velocityX": 0.004499259154964363,
-            "velocityY": 3.7829377337070014,
-            "timestamp": 0.584255324174218
+            "x": 2.3038901811674446,
+            "y": 5.835239509934909,
+            "heading": -0.0041146387992405135,
+            "angularVelocity": 4.0074427997802584e-7,
+            "velocityX": 0.0050603455856026045,
+            "velocityY": 3.7829370246290135,
+            "timestamp": 0.5842542611428286
         },
         {
-            "x": 2.3020927945039733,
-            "y": 6.097639248519623,
-            "heading": 3.182327539673564,
-            "angularVelocity": 0.6197913251072079,
-            "velocityX": -0.026211865253511206,
-            "velocityY": 3.5939274478930847,
-            "timestamp": 0.6572872396959952
+            "x": 2.3020054253248463,
+            "y": 6.097682298903223,
+            "heading": 0.0412179398919569,
+            "angularVelocity": 0.6207239786667508,
+            "velocityX": -0.02580733722214711,
+            "velocityY": 3.593542146598477,
+            "timestamp": 0.6572860437856822
         },
         {
-            "x": 2.3012287765145363,
-            "y": 6.317987502538012,
-            "heading": 3.2805685838685283,
-            "angularVelocity": 1.3451796176107471,
-            "velocityX": -0.011830690503794171,
-            "velocityY": 3.0171501383211403,
-            "timestamp": 0.7303191552177725
+            "x": 2.301170577421719,
+            "y": 6.318019035246094,
+            "heading": 0.13972641476186656,
+            "angularVelocity": 1.3488439047372631,
+            "velocityX": -0.011431295703265835,
+            "velocityY": 3.016997920212089,
+            "timestamp": 0.7303178264285358
         },
         {
-            "x": 2.3006859363348666,
-            "y": 6.493881460544365,
-            "heading": 3.3743823922179033,
-            "angularVelocity": 1.284559054478048,
-            "velocityX": -0.007432917181360783,
-            "velocityY": 2.40845330085727,
-            "timestamp": 0.8033510707395497
+            "x": 2.3006511827054728,
+            "y": 6.4939017376421555,
+            "heading": 0.2337896338628955,
+            "angularVelocity": 1.2879764904688844,
+            "velocityX": -0.007111899743518039,
+            "velocityY": 2.4083035636166743,
+            "timestamp": 0.8033496090713894
         },
         {
-            "x": 2.3003278852317357,
-            "y": 6.625559358254098,
-            "heading": 3.451128438683188,
-            "angularVelocity": 1.050856271767924,
-            "velocityX": -0.004902666191527756,
-            "velocityY": 1.80301854016781,
-            "timestamp": 0.8763829862613269
+            "x": 2.300310606012852,
+            "y": 6.62557002983581,
+            "heading": 0.3107349410237032,
+            "angularVelocity": 1.0535865944433043,
+            "velocityX": -0.004663403798945386,
+            "velocityY": 1.8028902955518677,
+            "timestamp": 0.876381391714243
         },
         {
-            "x": 2.300106073487947,
-            "y": 6.713219846040187,
-            "heading": 3.5051273882405054,
-            "angularVelocity": 0.739388377964907,
-            "velocityX": -0.0030371891823418365,
-            "velocityY": 1.2003038282618885,
-            "timestamp": 0.9494149017831042
+            "x": 2.30010034874232,
+            "y": 6.713223552423264,
+            "heading": 0.36487146716810653,
+            "angularVelocity": 0.741273513877466,
+            "velocityX": -0.002878983134789396,
+            "velocityY": 1.2002106400182577,
+            "timestamp": 0.9494131743570966
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 0.3841870877769111,
-            "velocityX": -0.0014524264794266268,
-            "velocityY": 0.5994660505208633,
-            "timestamp": 1.0224468173048815
+            "heading": 0.393,
+            "angularVelocity": 0.38515467942840836,
+            "velocityX": -0.0013740420771443632,
+            "velocityY": 0.5994163909542732,
+            "timestamp": 1.02244495699995
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": -9.351204897899337e-33,
-            "velocityX": 1.378221818744695e-30,
+            "heading": 0.393,
+            "angularVelocity": 8.681406116042979e-33,
+            "velocityX": 1.0196837521674476e-33,
             "velocityY": 0,
-            "timestamp": 1.0954787328266589
+            "timestamp": 1.0954767396428036
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.traj
index edd74832..282ccdeb 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.traj	
@@ -3,146 +3,146 @@
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
-            "angularVelocity": 0,
-            "velocityX": 1.6975195374214185e-34,
-            "velocityY": -4.870070626129674e-33,
+            "heading": -0.39,
+            "angularVelocity": -1.4895643422557431e-33,
+            "velocityX": -9.369180406325922e-35,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 2.300139974954616,
-            "y": 4.403824386953961,
-            "heading": 2.777167813185812,
-            "angularVelocity": 0.371999186817317,
-            "velocityX": 0.0019166271843757909,
-            "velocityY": 0.6000717171507387,
-            "timestamp": 0.07303191552177725
+            "x": 2.300137763138577,
+            "y": 4.403828106227067,
+            "heading": -0.362912658197764,
+            "angularVelocity": 0.3708979956671865,
+            "velocityX": 0.001886345007500309,
+            "velocityY": 0.6001237357356949,
+            "timestamp": 0.07303178264285357
         },
         {
-            "x": 2.3004163368424435,
-            "y": 4.4915620593167,
-            "heading": 2.8296003827219796,
-            "angularVelocity": 0.7179404943929278,
-            "velocityX": 0.003784124869971189,
-            "velocityY": 1.2013606891712643,
-            "timestamp": 0.1460638310435545
+            "x": 2.3004080637884394,
+            "y": 4.491572820860884,
+            "heading": -0.31063890006100675,
+            "angularVelocity": 0.7157672487934603,
+            "velocityX": 0.0037011372320465844,
+            "velocityY": 1.201459302491822,
+            "timestamp": 0.14606356528570713
         },
         {
-            "x": 2.3008287718098006,
-            "y": 4.623340237169809,
-            "heading": 2.9042527526949042,
-            "angularVelocity": 1.022188305476718,
-            "velocityX": 0.005647325068915905,
-            "velocityY": 1.8043916404440152,
-            "timestamp": 0.21909574656533176
+            "x": 2.3008080200629966,
+            "y": 4.623360729086378,
+            "heading": -0.2362185458103229,
+            "angularVelocity": 1.019013250910508,
+            "velocityX": 0.005476468738453403,
+            "velocityY": 1.8045281582400208,
+            "timestamp": 0.2190953479285607
         },
         {
-            "x": 2.30139145489797,
-            "y": 4.799346158732557,
-            "heading": 2.995610111682823,
-            "angularVelocity": 1.2509237685362016,
-            "velocityX": 0.007704619057970502,
-            "velocityY": 2.409986377945481,
-            "timestamp": 0.292127662087109
+            "x": 2.3013470975241233,
+            "y": 4.799377996239057,
+            "heading": -0.1451530728621028,
+            "angularVelocity": 1.2469293457282353,
+            "velocityX": 0.0073814090471083236,
+            "velocityY": 2.4101461142398,
+            "timestamp": 0.29212713057141426
         },
         {
-            "x": 2.3021599471353418,
-            "y": 5.019805776345521,
-            "heading": 3.0915564646710374,
-            "angularVelocity": 1.3137592284513517,
-            "velocityX": 0.01052269041392973,
-            "velocityY": 3.0186750003459215,
-            "timestamp": 0.36515957760888623
+            "x": 2.30206992781241,
+            "y": 5.019849097252827,
+            "heading": -0.0495237523149135,
+            "angularVelocity": 1.3094205986295608,
+            "velocityX": 0.009897475621284645,
+            "velocityY": 3.0188377311277894,
+            "timestamp": 0.36515891321426786
         },
         {
-            "x": 2.303349941373319,
-            "y": 5.282617464741668,
-            "heading": 3.1370629336033304,
-            "angularVelocity": 0.6231038664010262,
-            "velocityX": 0.0162941671387857,
-            "velocityY": 3.598586816715496,
-            "timestamp": 0.43819149313066347
+            "x": 2.303151073626671,
+            "y": 5.282690240833408,
+            "heading": -0.004114697333369339,
+            "angularVelocity": 0.621771143169372,
+            "velocityX": 0.014803771387434566,
+            "velocityY": 3.598996684305928,
+            "timestamp": 0.43819069585712145
         },
         {
-            "x": 2.30367850771832,
-            "y": 5.558892653761466,
-            "heading": 3.1370629627902624,
-            "angularVelocity": 3.996462650884406e-7,
-            "velocityX": 0.004498941903047958,
-            "velocityY": 3.7829377340844195,
-            "timestamp": 0.5112234086524408
+            "x": 2.3035206151085394,
+            "y": 5.5589648754006,
+            "heading": -0.004114668066309664,
+            "angularVelocity": 4.007441502253877e-7,
+            "velocityX": 0.005060009060371044,
+            "velocityY": 3.7829370250792667,
+            "timestamp": 0.511222478499975
         },
         {
-            "x": 2.304007097232836,
-            "y": 5.835167842753699,
-            "heading": 3.137062991977204,
-            "angularVelocity": 3.996463893760175e-7,
-            "velocityX": 0.004499259154964363,
-            "velocityY": 3.7829377337070014,
-            "timestamp": 0.584255324174218
+            "x": 2.3038901811674446,
+            "y": 5.835239509934909,
+            "heading": -0.0041146387992405135,
+            "angularVelocity": 4.0074427997802584e-7,
+            "velocityX": 0.0050603455856026045,
+            "velocityY": 3.7829370246290135,
+            "timestamp": 0.5842542611428286
         },
         {
-            "x": 2.3020927945039733,
-            "y": 6.097639248519623,
-            "heading": 3.182327539673564,
-            "angularVelocity": 0.6197913251072079,
-            "velocityX": -0.026211865253511206,
-            "velocityY": 3.5939274478930847,
-            "timestamp": 0.6572872396959952
+            "x": 2.3020054253248463,
+            "y": 6.097682298903223,
+            "heading": 0.0412179398919569,
+            "angularVelocity": 0.6207239786667508,
+            "velocityX": -0.02580733722214711,
+            "velocityY": 3.593542146598477,
+            "timestamp": 0.6572860437856822
         },
         {
-            "x": 2.3012287765145363,
-            "y": 6.317987502538012,
-            "heading": 3.2805685838685283,
-            "angularVelocity": 1.3451796176107471,
-            "velocityX": -0.011830690503794171,
-            "velocityY": 3.0171501383211403,
-            "timestamp": 0.7303191552177725
+            "x": 2.301170577421719,
+            "y": 6.318019035246094,
+            "heading": 0.13972641476186656,
+            "angularVelocity": 1.3488439047372631,
+            "velocityX": -0.011431295703265835,
+            "velocityY": 3.016997920212089,
+            "timestamp": 0.7303178264285358
         },
         {
-            "x": 2.3006859363348666,
-            "y": 6.493881460544365,
-            "heading": 3.3743823922179033,
-            "angularVelocity": 1.284559054478048,
-            "velocityX": -0.007432917181360783,
-            "velocityY": 2.40845330085727,
-            "timestamp": 0.8033510707395497
+            "x": 2.3006511827054728,
+            "y": 6.4939017376421555,
+            "heading": 0.2337896338628955,
+            "angularVelocity": 1.2879764904688844,
+            "velocityX": -0.007111899743518039,
+            "velocityY": 2.4083035636166743,
+            "timestamp": 0.8033496090713894
         },
         {
-            "x": 2.3003278852317357,
-            "y": 6.625559358254098,
-            "heading": 3.451128438683188,
-            "angularVelocity": 1.050856271767924,
-            "velocityX": -0.004902666191527756,
-            "velocityY": 1.80301854016781,
-            "timestamp": 0.8763829862613269
+            "x": 2.300310606012852,
+            "y": 6.62557002983581,
+            "heading": 0.3107349410237032,
+            "angularVelocity": 1.0535865944433043,
+            "velocityX": -0.004663403798945386,
+            "velocityY": 1.8028902955518677,
+            "timestamp": 0.876381391714243
         },
         {
-            "x": 2.300106073487947,
-            "y": 6.713219846040187,
-            "heading": 3.5051273882405054,
-            "angularVelocity": 0.739388377964907,
-            "velocityX": -0.0030371891823418365,
-            "velocityY": 1.2003038282618885,
-            "timestamp": 0.9494149017831042
+            "x": 2.30010034874232,
+            "y": 6.713223552423264,
+            "heading": 0.36487146716810653,
+            "angularVelocity": 0.741273513877466,
+            "velocityX": -0.002878983134789396,
+            "velocityY": 1.2002106400182577,
+            "timestamp": 0.9494131743570966
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 0.3841870877769111,
-            "velocityX": -0.0014524264794266268,
-            "velocityY": 0.5994660505208633,
-            "timestamp": 1.0224468173048815
+            "heading": 0.393,
+            "angularVelocity": 0.38515467942840836,
+            "velocityX": -0.0013740420771443632,
+            "velocityY": 0.5994163909542732,
+            "timestamp": 1.02244495699995
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": -9.351204897899337e-33,
-            "velocityX": 1.378221818744695e-30,
+            "heading": 0.393,
+            "angularVelocity": 8.681406116042979e-33,
+            "velocityX": 1.0196837521674476e-33,
             "velocityY": 0,
-            "timestamp": 1.0954787328266589
+            "timestamp": 1.0954767396428036
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj
index 5479ee40..efed994e 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj	
@@ -3,101 +3,101 @@
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
+            "heading": -0.39,
             "angularVelocity": 0,
-            "velocityX": -7.080178799155261e-34,
-            "velocityY": 4.018324582530293e-34,
+            "velocityX": 1.7319348109741582e-34,
+            "velocityY": -4.404349434317184e-36,
             "timestamp": 0
         },
         {
-            "x": 2.298001228420339,
-            "y": 4.408001448490065,
-            "heading": 2.765522898073599,
-            "angularVelocity": 0.20408324361211075,
-            "velocityX": -0.026278326719846846,
-            "velocityY": 0.6310864929657463,
-            "timestamp": 0.07606160015322963
+            "x": 2.298001242299744,
+            "y": 4.408001442881322,
+            "heading": -0.3744767722715256,
+            "angularVelocity": 0.20408758634762672,
+            "velocityX": -0.02627814536217338,
+            "velocityY": 0.6310864460799332,
+            "timestamp": 0.07606159691669512
         },
         {
-            "x": 2.294003714931991,
-            "y": 4.50400409446738,
-            "heading": 2.7965834623359793,
-            "angularVelocity": 0.40836064715713705,
-            "velocityX": -0.052556263348324604,
-            "velocityY": 1.2621696859376152,
-            "timestamp": 0.15212320030645926
+            "x": 2.2940037533773614,
+            "y": 4.504004077466311,
+            "heading": -0.3434155460751892,
+            "angularVelocity": 0.4083693671374729,
+            "velocityX": -0.05255594261004741,
+            "velocityY": 1.26216958986719,
+            "timestamp": 0.15212319383339024
         },
         {
-            "x": 2.288007388147497,
-            "y": 4.648007477823444,
-            "heading": 2.8432069040547985,
-            "angularVelocity": 0.6129695092516301,
-            "velocityX": -0.07883513852474495,
-            "velocityY": 1.8932468297532885,
-            "timestamp": 0.2281848004596889
+            "x": 2.2880074570982525,
+            "y": 4.648007443871442,
+            "heading": -0.29679113248213845,
+            "angularVelocity": 0.6129823127972874,
+            "velocityX": -0.07883474081771351,
+            "velocityY": 1.8932466874557927,
+            "timestamp": 0.22818479075008535
         },
         {
-            "x": 2.2800119392615206,
-            "y": 4.840010948654735,
-            "heading": 2.9054273803096926,
-            "angularVelocity": 0.8180274426195046,
-            "velocityX": -0.10511807363859851,
-            "velocityY": 2.5243154291323253,
-            "timestamp": 0.3042464006129185
+            "x": 2.2800120385493474,
+            "y": 4.8400108934587145,
+            "heading": -0.2345694550660327,
+            "angularVelocity": 0.8180432693814297,
+            "velocityX": -0.10511767926279252,
+            "velocityY": 2.524315257245527,
+            "timestamp": 0.30424638766678047
         },
         {
-            "x": 2.270016643226651,
-            "y": 5.0800137105269405,
-            "heading": 2.98328446819424,
-            "angularVelocity": 1.0236057054768744,
-            "velocityX": -0.13141054112368558,
-            "velocityY": 3.155373557599477,
-            "timestamp": 0.3803080007661481
+            "x": 2.270016765601229,
+            "y": 5.0800136328565335,
+            "heading": -0.15671111441894314,
+            "angularVelocity": 1.0236222194014972,
+            "velocityX": -0.1314102431883619,
+            "velocityY": 3.1553733963892037,
+            "timestamp": 0.3803079845834756
         },
         {
-            "x": 2.262010502624602,
-            "y": 5.272008371939251,
-            "heading": 3.045963904195738,
-            "angularVelocity": 0.8240614958826565,
-            "velocityX": -0.10525864018007076,
-            "velocityY": 2.524199609599701,
-            "timestamp": 0.4563696009193777
+            "x": 2.2620105775052464,
+            "y": 5.272008323301556,
+            "heading": -0.09403333232435188,
+            "angularVelocity": 0.8240397866381615,
+            "velocityX": -0.10525926907308407,
+            "velocityY": 2.524200098708164,
+            "timestamp": 0.4563695815001707
         },
         {
-            "x": 2.2560053812396474,
-            "y": 5.416004211679494,
-            "heading": 3.092980743508332,
-            "angularVelocity": 0.6181416012531585,
-            "velocityX": -0.07895076323476027,
-            "velocityY": 1.8931476520367212,
-            "timestamp": 0.5324312010726073
+            "x": 2.256005419001405,
+            "y": 5.416004186972581,
+            "heading": -0.04701785122811225,
+            "angularVelocity": 0.6181237707608528,
+            "velocityX": -0.07895125460510179,
+            "velocityY": 1.8931480472166962,
+            "timestamp": 0.5324311784168658
         },
         {
-            "x": 2.2520017956431593,
-            "y": 5.512001402686665,
-            "heading": 3.124326930482601,
-            "angularVelocity": 0.412115797079227,
-            "velocityX": -0.05263609480240691,
-            "velocityY": 1.2620979681439777,
-            "timestamp": 0.6084928012258369
+            "x": 2.252001808264489,
+            "y": 5.512001394535746,
+            "heading": -0.015672604394819867,
+            "angularVelocity": 0.4121034543571656,
+            "velocityX": -0.05263642756936803,
+            "velocityY": 1.262098239513749,
+            "timestamp": 0.6084927753335609
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0.20605758340377522,
-            "velocityX": -0.026318084803984385,
-            "velocityY": 0.6310490078651869,
-            "timestamp": 0.6845544013790665
+            "heading": -1.7090620600656545e-34,
+            "angularVelocity": 0.2060514770940842,
+            "velocityX": -0.026318251859498508,
+            "velocityY": 0.6310491418793501,
+            "timestamp": 0.6845543722502561
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -1.7962813810508635e-31,
-            "velocityX": -5.242761228396242e-32,
-            "velocityY": 0,
-            "timestamp": 0.7606160015322961
+            "heading": 0,
+            "angularVelocity": 2.0052459094930362e-33,
+            "velocityX": -1.9224144442861376e-33,
+            "velocityY": 1.2850315433355203e-34,
+            "timestamp": 0.7606159691669512
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.traj
index 5479ee40..efed994e 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.traj	
@@ -3,101 +3,101 @@
         {
             "x": 2.3,
             "y": 4.36,
-            "heading": 2.75,
+            "heading": -0.39,
             "angularVelocity": 0,
-            "velocityX": -7.080178799155261e-34,
-            "velocityY": 4.018324582530293e-34,
+            "velocityX": 1.7319348109741582e-34,
+            "velocityY": -4.404349434317184e-36,
             "timestamp": 0
         },
         {
-            "x": 2.298001228420339,
-            "y": 4.408001448490065,
-            "heading": 2.765522898073599,
-            "angularVelocity": 0.20408324361211075,
-            "velocityX": -0.026278326719846846,
-            "velocityY": 0.6310864929657463,
-            "timestamp": 0.07606160015322963
+            "x": 2.298001242299744,
+            "y": 4.408001442881322,
+            "heading": -0.3744767722715256,
+            "angularVelocity": 0.20408758634762672,
+            "velocityX": -0.02627814536217338,
+            "velocityY": 0.6310864460799332,
+            "timestamp": 0.07606159691669512
         },
         {
-            "x": 2.294003714931991,
-            "y": 4.50400409446738,
-            "heading": 2.7965834623359793,
-            "angularVelocity": 0.40836064715713705,
-            "velocityX": -0.052556263348324604,
-            "velocityY": 1.2621696859376152,
-            "timestamp": 0.15212320030645926
+            "x": 2.2940037533773614,
+            "y": 4.504004077466311,
+            "heading": -0.3434155460751892,
+            "angularVelocity": 0.4083693671374729,
+            "velocityX": -0.05255594261004741,
+            "velocityY": 1.26216958986719,
+            "timestamp": 0.15212319383339024
         },
         {
-            "x": 2.288007388147497,
-            "y": 4.648007477823444,
-            "heading": 2.8432069040547985,
-            "angularVelocity": 0.6129695092516301,
-            "velocityX": -0.07883513852474495,
-            "velocityY": 1.8932468297532885,
-            "timestamp": 0.2281848004596889
+            "x": 2.2880074570982525,
+            "y": 4.648007443871442,
+            "heading": -0.29679113248213845,
+            "angularVelocity": 0.6129823127972874,
+            "velocityX": -0.07883474081771351,
+            "velocityY": 1.8932466874557927,
+            "timestamp": 0.22818479075008535
         },
         {
-            "x": 2.2800119392615206,
-            "y": 4.840010948654735,
-            "heading": 2.9054273803096926,
-            "angularVelocity": 0.8180274426195046,
-            "velocityX": -0.10511807363859851,
-            "velocityY": 2.5243154291323253,
-            "timestamp": 0.3042464006129185
+            "x": 2.2800120385493474,
+            "y": 4.8400108934587145,
+            "heading": -0.2345694550660327,
+            "angularVelocity": 0.8180432693814297,
+            "velocityX": -0.10511767926279252,
+            "velocityY": 2.524315257245527,
+            "timestamp": 0.30424638766678047
         },
         {
-            "x": 2.270016643226651,
-            "y": 5.0800137105269405,
-            "heading": 2.98328446819424,
-            "angularVelocity": 1.0236057054768744,
-            "velocityX": -0.13141054112368558,
-            "velocityY": 3.155373557599477,
-            "timestamp": 0.3803080007661481
+            "x": 2.270016765601229,
+            "y": 5.0800136328565335,
+            "heading": -0.15671111441894314,
+            "angularVelocity": 1.0236222194014972,
+            "velocityX": -0.1314102431883619,
+            "velocityY": 3.1553733963892037,
+            "timestamp": 0.3803079845834756
         },
         {
-            "x": 2.262010502624602,
-            "y": 5.272008371939251,
-            "heading": 3.045963904195738,
-            "angularVelocity": 0.8240614958826565,
-            "velocityX": -0.10525864018007076,
-            "velocityY": 2.524199609599701,
-            "timestamp": 0.4563696009193777
+            "x": 2.2620105775052464,
+            "y": 5.272008323301556,
+            "heading": -0.09403333232435188,
+            "angularVelocity": 0.8240397866381615,
+            "velocityX": -0.10525926907308407,
+            "velocityY": 2.524200098708164,
+            "timestamp": 0.4563695815001707
         },
         {
-            "x": 2.2560053812396474,
-            "y": 5.416004211679494,
-            "heading": 3.092980743508332,
-            "angularVelocity": 0.6181416012531585,
-            "velocityX": -0.07895076323476027,
-            "velocityY": 1.8931476520367212,
-            "timestamp": 0.5324312010726073
+            "x": 2.256005419001405,
+            "y": 5.416004186972581,
+            "heading": -0.04701785122811225,
+            "angularVelocity": 0.6181237707608528,
+            "velocityX": -0.07895125460510179,
+            "velocityY": 1.8931480472166962,
+            "timestamp": 0.5324311784168658
         },
         {
-            "x": 2.2520017956431593,
-            "y": 5.512001402686665,
-            "heading": 3.124326930482601,
-            "angularVelocity": 0.412115797079227,
-            "velocityX": -0.05263609480240691,
-            "velocityY": 1.2620979681439777,
-            "timestamp": 0.6084928012258369
+            "x": 2.252001808264489,
+            "y": 5.512001394535746,
+            "heading": -0.015672604394819867,
+            "angularVelocity": 0.4121034543571656,
+            "velocityX": -0.05263642756936803,
+            "velocityY": 1.262098239513749,
+            "timestamp": 0.6084927753335609
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 0.20605758340377522,
-            "velocityX": -0.026318084803984385,
-            "velocityY": 0.6310490078651869,
-            "timestamp": 0.6845544013790665
+            "heading": -1.7090620600656545e-34,
+            "angularVelocity": 0.2060514770940842,
+            "velocityX": -0.026318251859498508,
+            "velocityY": 0.6310491418793501,
+            "timestamp": 0.6845543722502561
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": -1.7962813810508635e-31,
-            "velocityX": -5.242761228396242e-32,
-            "velocityY": 0,
-            "timestamp": 0.7606160015322961
+            "heading": 0,
+            "angularVelocity": 2.0052459094930362e-33,
+            "velocityX": -1.9224144442861376e-33,
+            "velocityY": 1.2850315433355203e-34,
+            "timestamp": 0.7606159691669512
         }
     ]
 }
\ No newline at end of file

From 37f5c81de1a35f7243b9bbdb78852f0a56acc12e Mon Sep 17 00:00:00 2001
From: Awesomeplayer165 <dev@jacob2.dev>
Date: Sun, 18 Feb 2024 21:34:59 -0800
Subject: [PATCH 38/62] first pass at fine tuning

---
 .../frc/robot/utils/dynamicauto/data.chor     | 20466 +++++++++++-----
 .../dynamicauto/deploy/choreo/C1 To C2.1.traj |    94 +
 .../dynamicauto/deploy/choreo/C1 To C2.traj   |    94 +
 .../dynamicauto/deploy/choreo/C1 To C3.1.traj |   139 +
 .../dynamicauto/deploy/choreo/C1 To C3.traj   |   139 +
 .../dynamicauto/deploy/choreo/C1 To C4.1.traj |   175 +
 .../dynamicauto/deploy/choreo/C1 To C4.traj   |   175 +
 .../dynamicauto/deploy/choreo/C1 To C5.1.traj |   220 +
 .../dynamicauto/deploy/choreo/C1 To C5.traj   |   220 +
 .../dynamicauto/deploy/choreo/C1 To S1.1.traj |   176 +-
 .../dynamicauto/deploy/choreo/C1 To S1.traj   |   176 +-
 .../dynamicauto/deploy/choreo/C1 To S2.1.traj |   206 +-
 .../dynamicauto/deploy/choreo/C1 To S2.traj   |   206 +-
 .../dynamicauto/deploy/choreo/C1 To S3.1.traj |   265 +
 .../dynamicauto/deploy/choreo/C1 To S3.traj   |   265 +
 .../dynamicauto/deploy/choreo/C1 To S4.1.traj |   301 +
 .../dynamicauto/deploy/choreo/C1 To S4.traj   |   301 +
 .../dynamicauto/deploy/choreo/C1 To W1.1.traj |   360 +-
 .../dynamicauto/deploy/choreo/C1 To W1.traj   |   360 +-
 .../dynamicauto/deploy/choreo/C1 To W2.1.traj |   344 +-
 .../dynamicauto/deploy/choreo/C1 To W2.traj   |   344 +-
 .../dynamicauto/deploy/choreo/C2 To C1.1.traj |   126 +-
 .../dynamicauto/deploy/choreo/C2 To C1.traj   |   126 +-
 .../dynamicauto/deploy/choreo/C2 To C3.1.traj |   126 +-
 .../dynamicauto/deploy/choreo/C2 To C3.traj   |   126 +-
 .../dynamicauto/deploy/choreo/C2 To C4.1.traj |   186 +-
 .../dynamicauto/deploy/choreo/C2 To C4.traj   |   186 +-
 .../dynamicauto/deploy/choreo/C2 To C5.1.traj |   306 +-
 .../dynamicauto/deploy/choreo/C2 To C5.traj   |   306 +-
 .../dynamicauto/deploy/choreo/C2 To S1.1.traj |   178 +-
 .../dynamicauto/deploy/choreo/C2 To S1.traj   |   178 +-
 .../dynamicauto/deploy/choreo/C2 To S2.1.traj |   193 +
 .../dynamicauto/deploy/choreo/C2 To S2.traj   |   193 +
 .../dynamicauto/deploy/choreo/C2 To S4.1.traj |   256 +
 .../dynamicauto/deploy/choreo/C2 To S4.traj   |   256 +
 .../dynamicauto/deploy/choreo/C2 To W1.1.traj |   360 +-
 .../dynamicauto/deploy/choreo/C2 To W1.traj   |   360 +-
 .../dynamicauto/deploy/choreo/C2 To W2.1.traj |   184 +
 .../dynamicauto/deploy/choreo/C2 To W2.traj   |   184 +
 .../dynamicauto/deploy/choreo/C2 To W3.1.traj |   346 +
 .../dynamicauto/deploy/choreo/C2 To W3.traj   |   346 +
 .../dynamicauto/deploy/choreo/C3 To C1.1.traj |   188 +-
 .../dynamicauto/deploy/choreo/C3 To C1.traj   |   188 +-
 .../dynamicauto/deploy/choreo/C3 To C2.1.traj |    94 +
 .../dynamicauto/deploy/choreo/C3 To C2.traj   |    94 +
 .../dynamicauto/deploy/choreo/C3 To C4.1.traj |    94 +
 .../dynamicauto/deploy/choreo/C3 To C4.traj   |    94 +
 .../dynamicauto/deploy/choreo/C3 To C5.1.traj |   188 +-
 .../dynamicauto/deploy/choreo/C3 To C5.traj   |   188 +-
 .../dynamicauto/deploy/choreo/C3 To S4.1.traj |   238 +
 .../dynamicauto/deploy/choreo/C3 To S4.traj   |   238 +
 .../dynamicauto/deploy/choreo/C3 To W1.1.traj |   220 +
 .../dynamicauto/deploy/choreo/C3 To W1.traj   |   220 +
 .../dynamicauto/deploy/choreo/C4 To C1.1.traj |   310 +-
 .../dynamicauto/deploy/choreo/C4 To C1.traj   |   310 +-
 .../dynamicauto/deploy/choreo/C4 To C2.1.traj |   194 +-
 .../dynamicauto/deploy/choreo/C4 To C2.traj   |   194 +-
 .../dynamicauto/deploy/choreo/C4 To C3.1.traj |   126 +-
 .../dynamicauto/deploy/choreo/C4 To C3.traj   |   126 +-
 .../dynamicauto/deploy/choreo/C4 To C5.1.traj |    94 +
 .../dynamicauto/deploy/choreo/C4 To C5.traj   |    94 +
 .../dynamicauto/deploy/choreo/C4 To S3.1.traj |   229 +
 .../dynamicauto/deploy/choreo/C4 To S3.traj   |   229 +
 .../dynamicauto/deploy/choreo/C4 To S4.1.traj |   301 +-
 .../dynamicauto/deploy/choreo/C4 To S4.traj   |   301 +-
 .../dynamicauto/deploy/choreo/C4 To W2.1.traj |   256 +
 .../dynamicauto/deploy/choreo/C4 To W2.traj   |   256 +
 .../dynamicauto/deploy/choreo/C4 To W3.1.traj |   274 +
 .../dynamicauto/deploy/choreo/C4 To W3.traj   |   274 +
 .../dynamicauto/deploy/choreo/C5 To C1.1.traj |   400 +-
 .../dynamicauto/deploy/choreo/C5 To C1.traj   |   400 +-
 .../dynamicauto/deploy/choreo/C5 To C2.1.traj |   310 +-
 .../dynamicauto/deploy/choreo/C5 To C2.traj   |   310 +-
 .../dynamicauto/deploy/choreo/C5 To C3.1.traj |   139 +
 .../dynamicauto/deploy/choreo/C5 To C3.traj   |   139 +
 .../dynamicauto/deploy/choreo/C5 To C4.1.traj |   126 +-
 .../dynamicauto/deploy/choreo/C5 To C4.traj   |   126 +-
 .../dynamicauto/deploy/choreo/C5 To S3.1.traj |   393 +-
 .../dynamicauto/deploy/choreo/C5 To S3.traj   |   393 +-
 .../dynamicauto/deploy/choreo/C5 To S4.1.traj |   210 +-
 .../dynamicauto/deploy/choreo/C5 To S4.traj   |   210 +-
 .../dynamicauto/deploy/choreo/C5 To W1.1.traj |   537 +-
 .../dynamicauto/deploy/choreo/C5 To W1.traj   |   537 +-
 .../dynamicauto/deploy/choreo/C5 To W2.1.traj |   283 +
 .../dynamicauto/deploy/choreo/C5 To W2.traj   |   283 +
 .../dynamicauto/deploy/choreo/C5 To W3.1.traj |   292 +
 .../dynamicauto/deploy/choreo/C5 To W3.traj   |   292 +
 .../deploy/choreo/Center To C3.1.traj         |   382 +-
 .../deploy/choreo/Center To C3.traj           |   382 +-
 .../dynamicauto/deploy/choreo/S1 To C1.1.traj |   174 +-
 .../dynamicauto/deploy/choreo/S1 To C1.traj   |   174 +-
 .../dynamicauto/deploy/choreo/S1 To C2.1.traj |   174 +-
 .../dynamicauto/deploy/choreo/S1 To C2.traj   |   174 +-
 .../dynamicauto/deploy/choreo/S1 To C3.1.traj |   157 +
 .../dynamicauto/deploy/choreo/S1 To C3.traj   |   157 +
 .../dynamicauto/deploy/choreo/S1 To C4.1.traj |   247 +
 .../dynamicauto/deploy/choreo/S1 To C4.traj   |   247 +
 .../dynamicauto/deploy/choreo/S1 To C5.1.traj |   292 +
 .../dynamicauto/deploy/choreo/S1 To C5.traj   |   292 +
 .../dynamicauto/deploy/choreo/S1 To W1.1.traj |   176 +-
 .../dynamicauto/deploy/choreo/S1 To W1.traj   |   176 +-
 .../dynamicauto/deploy/choreo/S1 To W2.1.traj |   202 +-
 .../dynamicauto/deploy/choreo/S1 To W2.traj   |   202 +-
 .../dynamicauto/deploy/choreo/S1 To W3.1.traj |   175 +
 .../dynamicauto/deploy/choreo/S1 To W3.traj   |   175 +
 .../dynamicauto/deploy/choreo/S2 To C1.1.traj |   206 +-
 .../dynamicauto/deploy/choreo/S2 To C1.traj   |   206 +-
 .../dynamicauto/deploy/choreo/S2 To C2.1.traj |   202 +
 .../dynamicauto/deploy/choreo/S2 To C2.traj   |   202 +
 .../dynamicauto/deploy/choreo/S2 To W1.1.traj |   178 +-
 .../dynamicauto/deploy/choreo/S2 To W1.traj   |   178 +-
 .../dynamicauto/deploy/choreo/S2 To W2.1.traj |   136 +-
 .../dynamicauto/deploy/choreo/S2 To W2.traj   |   136 +-
 .../dynamicauto/deploy/choreo/S2 To W3.1.traj |   178 +-
 .../dynamicauto/deploy/choreo/S2 To W3.traj   |   178 +-
 .../dynamicauto/deploy/choreo/S3 To C4.1.traj |   238 +
 .../dynamicauto/deploy/choreo/S3 To C4.traj   |   238 +
 .../dynamicauto/deploy/choreo/S4 To C1.1.traj |   292 +
 .../dynamicauto/deploy/choreo/S4 To C1.traj   |   292 +
 .../dynamicauto/deploy/choreo/S4 To C3.1.traj |   238 +
 .../dynamicauto/deploy/choreo/S4 To C3.traj   |   238 +
 .../dynamicauto/deploy/choreo/S4 To C4.1.traj |   148 +
 .../dynamicauto/deploy/choreo/S4 To C4.traj   |   148 +
 .../dynamicauto/deploy/choreo/S4 To C5.1.traj |   206 +-
 .../dynamicauto/deploy/choreo/S4 To C5.traj   |   206 +-
 .../dynamicauto/deploy/choreo/S4 To W3.1.traj |   256 +
 .../dynamicauto/deploy/choreo/S4 To W3.traj   |   256 +
 .../dynamicauto/deploy/choreo/W1 To C3.1.traj |   193 +
 .../dynamicauto/deploy/choreo/W1 To C3.traj   |   193 +
 .../dynamicauto/deploy/choreo/W1 To C4.1.traj |   928 +-
 .../dynamicauto/deploy/choreo/W1 To C4.traj   |   928 +-
 .../dynamicauto/deploy/choreo/W1 To C5.1.traj |   616 +
 .../dynamicauto/deploy/choreo/W1 To C5.traj   |   616 +
 .../dynamicauto/deploy/choreo/W1 To S1.1.traj |   176 +-
 .../dynamicauto/deploy/choreo/W1 To S1.traj   |   176 +-
 .../dynamicauto/deploy/choreo/W1 To S2.1.traj |   176 +-
 .../dynamicauto/deploy/choreo/W1 To S2.traj   |   176 +-
 .../dynamicauto/deploy/choreo/W1 To S3.1.traj |   544 +
 .../dynamicauto/deploy/choreo/W1 To S3.traj   |   544 +
 .../dynamicauto/deploy/choreo/W1 To W3.1.traj |   148 +
 .../dynamicauto/deploy/choreo/W1 To W3.traj   |   148 +
 .../dynamicauto/deploy/choreo/W2 To C1.1.traj |   344 +-
 .../dynamicauto/deploy/choreo/W2 To C1.traj   |   344 +-
 .../dynamicauto/deploy/choreo/W2 To C2.1.traj |  1000 +-
 .../dynamicauto/deploy/choreo/W2 To C2.traj   |  1000 +-
 .../dynamicauto/deploy/choreo/W2 To W1.1.traj |   136 +-
 .../dynamicauto/deploy/choreo/W2 To W1.traj   |   136 +-
 .../dynamicauto/deploy/choreo/W2 To W3.1.traj |   134 +-
 .../dynamicauto/deploy/choreo/W2 To W3.traj   |   134 +-
 .../dynamicauto/deploy/choreo/W3 To S1.1.traj |   304 +-
 .../dynamicauto/deploy/choreo/W3 To S1.traj   |   304 +-
 .../dynamicauto/deploy/choreo/W3 To S2.1.traj |   174 +-
 .../dynamicauto/deploy/choreo/W3 To S2.traj   |   174 +-
 .../dynamicauto/deploy/choreo/W3 To S4.1.traj |   256 +
 .../dynamicauto/deploy/choreo/W3 To S4.traj   |   256 +
 .../dynamicauto/deploy/choreo/W3 To W1.1.traj |   200 +-
 .../dynamicauto/deploy/choreo/W3 To W1.traj   |   200 +-
 .../dynamicauto/deploy/choreo/W3 To W2.1.traj |   134 +-
 .../dynamicauto/deploy/choreo/W3 To W2.traj   |   134 +-
 159 files changed, 43077 insertions(+), 16903 deletions(-)
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C5.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C5.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.traj

diff --git a/src/main/java/frc/robot/utils/dynamicauto/data.chor b/src/main/java/frc/robot/utils/dynamicauto/data.chor
index a0af0f38..cc81043a 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/data.chor
+++ b/src/main/java/frc/robot/utils/dynamicauto/data.chor
@@ -4051,7 +4051,215 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 1.35,
+                    "y": 5.56,
+                    "heading": 0,
+                    "angularVelocity": 2.6743025901632004e-33,
+                    "velocityX": 2.068786142168461e-33,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 1.4007220754094205,
+                    "y": 5.545584664540576,
+                    "heading": -0.37232774466736046,
+                    "angularVelocity": -3.437582019103784,
+                    "velocityX": 0.4683005682394807,
+                    "velocityY": -0.13309214444637307,
+                    "timestamp": 0.10831094141120465
+                },
+                {
+                    "x": 1.5327694511201624,
+                    "y": 5.533363500300387,
+                    "heading": -0.9362708794209632,
+                    "angularVelocity": -5.206705134364786,
+                    "velocityX": 1.2191508446909478,
+                    "velocityY": -0.11283406903270367,
+                    "timestamp": 0.2166218828224093
+                },
+                {
+                    "x": 1.760106911270163,
+                    "y": 5.516367699163042,
+                    "heading": -1.4218044496098983,
+                    "angularVelocity": -4.482774905866594,
+                    "velocityX": 2.0989334705060814,
+                    "velocityY": -0.1569167520465022,
+                    "timestamp": 0.3249328242336139
+                },
+                {
+                    "x": 2.067387230758427,
+                    "y": 5.467280597023701,
+                    "heading": -1.7292049865361503,
+                    "angularVelocity": -2.838130044121764,
+                    "velocityX": 2.83702011527781,
+                    "velocityY": -0.453205387191498,
+                    "timestamp": 0.4332437656448186
+                },
+                {
+                    "x": 2.4488593918735733,
+                    "y": 5.393167697716653,
+                    "heading": -1.7983473850598053,
+                    "angularVelocity": -0.6383694723984937,
+                    "velocityX": 3.5220094677866336,
+                    "velocityY": -0.6842605035226905,
+                    "timestamp": 0.5415547070560233
+                },
+                {
+                    "x": 2.8531804118778052,
+                    "y": 5.326790341993422,
+                    "heading": -1.7983490370676205,
+                    "angularVelocity": -0.000015252455509683049,
+                    "velocityX": 3.7329656148885193,
+                    "velocityY": -0.6128407237384188,
+                    "timestamp": 0.6498656484672279
+                },
+                {
+                    "x": 3.257501404598323,
+                    "y": 5.2604128200678275,
+                    "heading": -1.7983506890680847,
+                    "angularVelocity": -0.00001525238764202395,
+                    "velocityX": 3.732965362986781,
+                    "velocityY": -0.6128422582312392,
+                    "timestamp": 0.7581765898784326
+                },
+                {
+                    "x": 3.6466232677858144,
+                    "y": 5.172310745117625,
+                    "heading": -1.8337945854134767,
+                    "angularVelocity": -0.32724206699329256,
+                    "velocityX": 3.5926367005728674,
+                    "velocityY": -0.8134180517896293,
+                    "timestamp": 0.8664875312896373
+                },
+                {
+                    "x": 4.014238623942389,
+                    "y": 5.062846847355338,
+                    "heading": -1.917513965365452,
+                    "angularVelocity": -0.7729540419571551,
+                    "velocityX": 3.394074055370967,
+                    "velocityY": -1.0106448742487064,
+                    "timestamp": 0.974798472700842
+                },
+                {
+                    "x": 4.354761888880782,
+                    "y": 4.880183720994759,
+                    "heading": -1.9914398534549198,
+                    "angularVelocity": -0.6825338892476871,
+                    "velocityX": 3.143941512294591,
+                    "velocityY": -1.6864697507068689,
+                    "timestamp": 1.0831094141120465
+                },
+                {
+                    "x": 4.661144802240214,
+                    "y": 4.608133464126214,
+                    "heading": -1.9914398486412812,
+                    "angularVelocity": 4.444277523534959e-8,
+                    "velocityX": 2.8287346538355926,
+                    "velocityY": -2.5117523061285136,
+                    "timestamp": 1.1914203555232512
+                },
+                {
+                    "x": 5.019380877625839,
+                    "y": 4.418922250546549,
+                    "heading": -1.9767149939198712,
+                    "angularVelocity": 0.13594983599585594,
+                    "velocityX": 3.3074781801182462,
+                    "velocityY": -1.7469261287400477,
+                    "timestamp": 1.2997312969344559
+                },
+                {
+                    "x": 5.376950498025801,
+                    "y": 4.3069722509438675,
+                    "heading": -1.8614424797513431,
+                    "angularVelocity": 1.0642739566900585,
+                    "velocityX": 3.3013250161167162,
+                    "velocityY": -1.0335982509621156,
+                    "timestamp": 1.4080422383456606
+                },
+                {
+                    "x": 5.749629435463048,
+                    "y": 4.240661073830716,
+                    "heading": -1.7582912287295662,
+                    "angularVelocity": 0.9523622422425545,
+                    "velocityX": 3.4408244687151703,
+                    "velocityY": -0.6122297179691145,
+                    "timestamp": 1.5163531797568652
+                },
+                {
+                    "x": 6.158173088993434,
+                    "y": 4.209455444526307,
+                    "heading": -1.7582906674190417,
+                    "angularVelocity": 0.000005182399092709644,
+                    "velocityX": 3.771951828756997,
+                    "velocityY": -0.2881115139230142,
+                    "timestamp": 1.62466412116807
+                },
+                {
+                    "x": 6.566716775041419,
+                    "y": 4.178250240929697,
+                    "heading": -1.7582901061123783,
+                    "angularVelocity": 0.000005182363444203767,
+                    "velocityX": 3.771952128981517,
+                    "velocityY": -0.2881075835001592,
+                    "timestamp": 1.7329750625792746
+                },
+                {
+                    "x": 6.948239527399369,
+                    "y": 4.140315578246664,
+                    "heading": -1.6699172621402418,
+                    "angularVelocity": 0.8159179748667066,
+                    "velocityX": 3.5224765604196056,
+                    "velocityY": -0.35023850950582514,
+                    "timestamp": 1.8412860039904793
+                },
+                {
+                    "x": 7.2588942750640015,
+                    "y": 4.14084991195257,
+                    "heading": -1.390296709754989,
+                    "angularVelocity": 2.5816464037891405,
+                    "velocityX": 2.86817512263351,
+                    "velocityY": 0.004933330824608595,
+                    "timestamp": 1.949596945401684
+                },
+                {
+                    "x": 7.4846066646178775,
+                    "y": 4.133171401549216,
+                    "heading": -0.9384850139510863,
+                    "angularVelocity": 4.17143171241205,
+                    "velocityX": 2.083929717653862,
+                    "velocityY": -0.07089321081793584,
+                    "timestamp": 2.0579078868128886
+                },
+                {
+                    "x": 7.620450068903259,
+                    "y": 4.128683952785512,
+                    "heading": -0.34333254233125515,
+                    "angularVelocity": 5.494850879010672,
+                    "velocityX": 1.2541983525897882,
+                    "velocityY": -0.04143116757398245,
+                    "timestamp": 2.166218828224093
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 9.09513036114211e-34,
+                    "angularVelocity": 3.1698786646843584,
+                    "velocityX": 0.5498053134877472,
+                    "velocityY": -0.015547393121808115,
+                    "timestamp": 2.2745297696352975
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 0,
+                    "angularVelocity": -7.032634787663221e-33,
+                    "velocityX": 0,
+                    "velocityY": -1.950631326224301e-32,
+                    "timestamp": 2.382840711046502
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -4075,11 +4283,6 @@
                     "y": 4.04,
                     "radius": 0.5
                 },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
                 {
                     "x": 5.61,
                     "y": 5.44,
@@ -9429,7 +9632,152 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 0.39,
+                    "angularVelocity": -2.4663146867319515e-36,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.3001378208847427,
+                    "y": 6.713171893956039,
+                    "heading": 0.36291265694481545,
+                    "angularVelocity": -0.37089801282350154,
+                    "velocityX": 0.0018871357066083216,
+                    "velocityY": -0.6001237332286558,
+                    "timestamp": 0.0730317826428328
+                },
+                {
+                    "x": 2.3004082222445,
+                    "y": 6.625427179140099,
+                    "heading": 0.3106389081779897,
+                    "angularVelocity": -0.7157671204942956,
+                    "velocityX": 0.0037025162192713427,
+                    "velocityY": -1.201459304985902,
+                    "timestamp": 0.1460635652856656
+                },
+                {
+                    "x": 2.300808308388558,
+                    "y": 6.493639269598709,
+                    "heading": 0.23621859479744584,
+                    "angularVelocity": -1.019012691289514,
+                    "velocityX": 0.005478246998500719,
+                    "velocityY": -1.804528176258669,
+                    "timestamp": 0.2190953479284984
+                },
+                {
+                    "x": 2.30134752968001,
+                    "y": 6.317621999647275,
+                    "heading": 0.14515321533996162,
+                    "angularVelocity": -1.2469280655909227,
+                    "velocityX": 0.007383378468093947,
+                    "velocityY": -2.4101461525629078,
+                    "timestamp": 0.2921271305713312
+                },
+                {
+                    "x": 2.3020704977057354,
+                    "y": 6.097150895386178,
+                    "heading": 0.04952404086318575,
+                    "angularVelocity": -1.309418598536164,
+                    "velocityX": 0.00989936161439861,
+                    "velocityY": -3.0188377755932185,
+                    "timestamp": 0.36515891321416405
+                },
+                {
+                    "x": 2.3031517302727234,
+                    "y": 5.834309739739576,
+                    "heading": 0.004115026920641748,
+                    "angularVelocity": -0.6217705812361183,
+                    "velocityX": 0.014804959263771778,
+                    "velocityY": -3.598996849522972,
+                    "timestamp": 0.4381906958569969
+                },
+                {
+                    "x": 2.303521267718626,
+                    "y": 5.558035105167069,
+                    "heading": 0.00411499765356137,
+                    "angularVelocity": -4.007444337013033e-7,
+                    "velocityX": 0.005059953797233799,
+                    "velocityY": -3.7829370251531125,
+                    "timestamp": 0.5112224784998297
+                },
+                {
+                    "x": 2.3038907744757537,
+                    "y": 5.281760470553516,
+                    "heading": 0.004114968386478979,
+                    "angularVelocity": -4.0074446127092794e-7,
+                    "velocityX": 0.005059533586019425,
+                    "velocityY": -3.782937025715139,
+                    "timestamp": 0.5842542611426625
+                },
+                {
+                    "x": 2.3020058670727765,
+                    "y": 5.019317688425813,
+                    "heading": -0.04121766182368216,
+                    "angularVelocity": -0.6207246840990261,
+                    "velocityX": -0.02580941248819387,
+                    "velocityY": -3.5935420529332935,
+                    "timestamp": 0.6572860437854954
+                },
+                {
+                    "x": 2.301170882315387,
+                    "y": 4.798980955675817,
+                    "heading": -0.139726223563735,
+                    "angularVelocity": -1.348845094221732,
+                    "velocityX": -0.011433169603334988,
+                    "velocityY": -3.016997871016921,
+                    "timestamp": 0.7303178264283282
+                },
+                {
+                    "x": 2.3006513716532107,
+                    "y": 4.623098256624685,
+                    "heading": -0.23378951684706242,
+                    "angularVelocity": -1.2879775062228829,
+                    "velocityX": -0.007113487352719777,
+                    "velocityY": -2.4083035178163246,
+                    "timestamp": 0.803349609071161
+                },
+                {
+                    "x": 2.300310703422249,
+                    "y": 4.491429967185545,
+                    "heading": -0.31073488166813007,
+                    "angularVelocity": -1.0535873839664371,
+                    "velocityX": -0.004664657203118652,
+                    "velocityY": -1.8028902578357346,
+                    "timestamp": 0.8763813917139939
+                },
+                {
+                    "x": 2.3001003821722423,
+                    "y": 4.403776446553788,
+                    "heading": -0.36487144716496317,
+                    "angularVelocity": -0.7412740527174575,
+                    "velocityX": -0.0028798591845282696,
+                    "velocityY": -1.2002106132398922,
+                    "timestamp": 0.9494131743568267
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": -0.393,
+                    "angularVelocity": -0.3851549533249317,
+                    "velocityX": -0.0013744998219966374,
+                    "velocityY": -0.5994163769475577,
+                    "timestamp": 1.0224449569996594
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": -0.393,
+                    "angularVelocity": -7.841220088894993e-36,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 1.0954767396424923
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -9447,23 +9795,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "W1 To S1": {
             "waypoints": [
@@ -9491,127 +9823,127 @@
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.39,
-                    "angularVelocity": 0,
-                    "velocityX": 1.3865554557202343e-36,
-                    "velocityY": 1.1876687369143486e-32,
+                    "angularVelocity": -4.966859894029903e-31,
+                    "velocityX": 7.6981862107013014e-34,
+                    "velocityY": 1.676179535721191e-31,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.373020964624076,
-                    "y": 6.753775569913306,
-                    "heading": 0.3835012481902214,
-                    "angularVelocity": -0.06940035874833077,
-                    "velocityX": 0.7797929955465333,
-                    "velocityY": -0.034433782259311436,
-                    "timestamp": 0.09364147285382916
-                },
-                {
-                    "x": 2.5190628897747724,
-                    "y": 6.747326705309919,
-                    "heading": 0.37050354798266344,
-                    "angularVelocity": -0.13880281686559404,
-                    "velocityX": 1.5595859473362035,
-                    "velocityY": -0.06886761182670828,
-                    "timestamp": 0.18728294570765833
-                },
-                {
-                    "x": 2.738125767671939,
-                    "y": 6.737653401682201,
-                    "heading": 0.35100657007014807,
-                    "angularVelocity": -0.20820879166370637,
-                    "velocityX": 2.3393788160414357,
-                    "velocityY": -0.10330148953128306,
-                    "timestamp": 0.2809244185614875
-                },
-                {
-                    "x": 3.0302095781027427,
-                    "y": 6.7247556545524185,
-                    "heading": 0.3250098803168658,
-                    "angularVelocity": -0.27761940261055373,
-                    "velocityX": 3.119171468893202,
-                    "velocityY": -0.13773541505367973,
-                    "timestamp": 0.37456589141531665
-                },
-                {
-                    "x": 3.384104830792732,
-                    "y": 6.709128195027275,
-                    "heading": 0.3250098773247812,
-                    "angularVelocity": -3.195254338233196e-8,
-                    "velocityX": 3.77925765053277,
-                    "velocityY": -0.16688609276314717,
-                    "timestamp": 0.46820736426914583
-                },
-                {
-                    "x": 3.738000080774581,
-                    "y": 6.693500674175882,
-                    "heading": 0.32500987433288897,
-                    "angularVelocity": -3.1950503021671023e-8,
-                    "velocityX": 3.7792576216124307,
-                    "velocityY": -0.16688674766770242,
-                    "timestamp": 0.561848837122975
-                },
-                {
-                    "x": 4.091895327748041,
-                    "y": 6.677873085197927,
-                    "heading": 0.32500987134099335,
-                    "angularVelocity": -3.1950540402322325e-8,
-                    "velocityX": 3.7792575894857667,
-                    "velocityY": -0.1668874751932408,
-                    "timestamp": 0.6554903099768041
-                },
-                {
-                    "x": 4.445790571342994,
-                    "y": 6.662245419680397,
-                    "heading": 0.32500986834901585,
-                    "angularVelocity": -3.1951399041908766e-8,
-                    "velocityX": 3.7792575534066395,
-                    "velocityY": -0.166888292561096,
-                    "timestamp": 0.7491317828306333
-                },
-                {
-                    "x": 4.737874328369047,
-                    "y": 6.649347277173169,
-                    "heading": 0.29900623239629953,
-                    "angularVelocity": -0.2776935812758607,
-                    "velocityX": 3.119170898582289,
-                    "velocityY": -0.13773963730152244,
-                    "timestamp": 0.8427732556844625
-                },
-                {
-                    "x": 4.956937160697527,
-                    "y": 6.639673651489352,
-                    "heading": 0.27950319129562423,
-                    "angularVelocity": -0.20827354062589903,
-                    "velocityX": 2.339378329412089,
-                    "velocityY": -0.10330492877809694,
-                    "timestamp": 0.9364147285382917
-                },
-                {
-                    "x": 5.102979052772278,
-                    "y": 6.633224554883952,
-                    "heading": 0.26650106474792545,
-                    "angularVelocity": -0.13885008588030126,
-                    "velocityX": 1.5595855941172054,
-                    "velocityY": -0.06887008938317173,
-                    "timestamp": 1.0300562013921208
+                    "x": 2.3730209621579745,
+                    "y": 6.753775517490119,
+                    "heading": 0.38350122654498725,
+                    "angularVelocity": -0.06940058989840046,
+                    "velocityX": 0.7797929692101098,
+                    "velocityY": -0.03443434208804683,
+                    "timestamp": 0.09364147285393071
+                },
+                {
+                    "x": 2.5190628825903247,
+                    "y": 6.747326552846718,
+                    "heading": 0.37050348328006333,
+                    "angularVelocity": -0.13880327667642306,
+                    "velocityX": 1.5595858969471437,
+                    "velocityY": -0.06886868015693057,
+                    "timestamp": 0.18728294570786141
+                },
+                {
+                    "x": 2.73812575373285,
+                    "y": 6.7376531065331395,
+                    "heading": 0.35100644039340695,
+                    "angularVelocity": -0.2082094855243831,
+                    "velocityX": 2.3393787439058484,
+                    "velocityY": -0.10330301327774684,
+                    "timestamp": 0.2809244185617921
+                },
+                {
+                    "x": 3.0302095555912687,
+                    "y": 6.7247551792664035,
+                    "heading": 0.3250096622235805,
+                    "angularVelocity": -0.27762034681324244,
+                    "velocityX": 3.1191713773449834,
+                    "velocityY": -0.13773733874150373,
+                    "timestamp": 0.37456589141572283
+                },
+                {
+                    "x": 3.3841048026937948,
+                    "y": 6.709127593205063,
+                    "heading": 0.3250096592315294,
+                    "angularVelocity": -3.195216141250517e-8,
+                    "velocityX": 3.779257590860006,
+                    "velocityY": -0.16688744404647712,
+                    "timestamp": 0.46820736426965354
+                },
+                {
+                    "x": 3.7380000497922845,
+                    "y": 6.693500007049851,
+                    "heading": 0.3250096562396269,
+                    "angularVelocity": -3.195060442591983e-8,
+                    "velocityX": 3.7792575908168353,
+                    "velocityY": -0.16688744504894126,
+                    "timestamp": 0.5618488371235842
+                },
+                {
+                    "x": 4.091895296890773,
+                    "y": 6.677872420894639,
+                    "heading": 0.3250096532477245,
+                    "angularVelocity": -3.195060369258429e-8,
+                    "velocityX": 3.7792575908168353,
+                    "velocityY": -0.16688744504893968,
+                    "timestamp": 0.6554903099775149
+                },
+                {
+                    "x": 4.445790543993225,
+                    "y": 6.6622448348316095,
+                    "heading": 0.3250096502556742,
+                    "angularVelocity": -3.195215253798021e-8,
+                    "velocityX": 3.7792575908592094,
+                    "velocityY": -0.1668874440645195,
+                    "timestamp": 0.7491317828314457
+                },
+                {
+                    "x": 4.737874310687331,
+                    "y": 6.649346902155386,
+                    "heading": 0.29900607193864137,
+                    "angularVelocity": -0.27769296578215763,
+                    "velocityX": 3.119171001824275,
+                    "velocityY": -0.1377373965095818,
+                    "timestamp": 0.8427732556853764
+                },
+                {
+                    "x": 4.956937151198913,
+                    "y": 6.639673451411702,
+                    "heading": 0.27950309637569815,
+                    "angularVelocity": -0.20827284074623165,
+                    "velocityX": 2.339378416797119,
+                    "velocityY": -0.10330306058697603,
+                    "timestamp": 0.9364147285393072
+                },
+                {
+                    "x": 5.10297904938153,
+                    "y": 6.633224483848421,
+                    "heading": 0.2665010284217395,
+                    "angularVelocity": -0.1388494601558462,
+                    "velocityX": 1.5595856593415998,
+                    "velocityY": -0.06886871133840435,
+                    "timestamp": 1.030056201393238
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
                     "heading": 0.26,
-                    "angularVelocity": -0.06942505868186148,
-                    "velocityX": 0.7797928097703705,
-                    "velocityY": -0.03443511497296222,
-                    "timestamp": 1.12369767424595
+                    "angularVelocity": -0.06942467075336838,
+                    "velocityX": 0.7797928459794067,
+                    "velocityY": -0.03443435638235996,
+                    "timestamp": 1.1236976742471687
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
                     "heading": 0.26,
-                    "angularVelocity": 0,
-                    "velocityX": -1.6658104354370778e-33,
-                    "velocityY": -5.427630465010738e-32,
-                    "timestamp": 1.217339147099779
+                    "angularVelocity": 2.7813300894409408e-31,
+                    "velocityX": -8.985802616555554e-33,
+                    "velocityY": -4.9789977648629575e-31,
+                    "timestamp": 1.2173391471010995
                 }
             ],
             "constraints": [
@@ -9631,23 +9963,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "W1 To S2": {
             "waypoints": [
@@ -9675,127 +9991,127 @@
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.39,
-                    "angularVelocity": 0,
-                    "velocityX": 1.1894430825117781e-34,
-                    "velocityY": 3.241165026752671e-33,
+                    "angularVelocity": -5.274350592003984e-33,
+                    "velocityX": 6.842220334639481e-35,
+                    "velocityY": 4.754828012658472e-35,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.347774772680024,
-                    "y": 6.727866824995534,
-                    "heading": 0.376906913073687,
-                    "angularVelocity": -0.15965534994810393,
-                    "velocityX": 0.5825591851522491,
-                    "velocityY": -0.35524603759330214,
-                    "timestamp": 0.08200844463131937
-                },
-                {
-                    "x": 2.443324407414016,
-                    "y": 6.669600558032611,
-                    "heading": 0.35072353035940795,
-                    "angularVelocity": -0.3192766651286972,
-                    "velocityX": 1.1651194601183832,
-                    "velocityY": -0.7104910625346852,
-                    "timestamp": 0.16401688926263874
-                },
-                {
-                    "x": 2.5866490395090382,
-                    "y": 6.582201337136711,
-                    "heading": 0.31145340371020835,
-                    "angularVelocity": -0.4788546694885386,
-                    "velocityX": 1.7476813850007613,
-                    "velocityY": -1.0657344044118373,
-                    "timestamp": 0.24602533389395811
-                },
-                {
-                    "x": 2.7777488318834505,
-                    "y": 6.4656693736120285,
-                    "heading": 0.25909914961330927,
-                    "angularVelocity": -0.6384007687533294,
-                    "velocityX": 2.330245296487799,
-                    "velocityY": -1.420975169673899,
-                    "timestamp": 0.3280337785252775
-                },
-                {
-                    "x": 3.016623938716172,
-                    "y": 6.320004968537675,
-                    "heading": 0.19366021259536087,
-                    "angularVelocity": -0.7979536413858164,
-                    "velocityX": 2.9128110880119595,
-                    "velocityY": -1.7762122636175968,
-                    "timestamp": 0.4100422231565969
-                },
-                {
-                    "x": 3.281496231375059,
-                    "y": 6.1584895705745515,
-                    "heading": 0.19366020431656966,
-                    "angularVelocity": -1.0095047213207674e-7,
-                    "velocityX": 3.2298172932026463,
-                    "velocityY": -1.9694971498269327,
-                    "timestamp": 0.4920506677879163
-                },
-                {
-                    "x": 3.54636850972689,
-                    "y": 5.99697414914899,
-                    "heading": 0.1936601960377879,
-                    "angularVelocity": -1.0095035712972839e-7,
-                    "velocityX": 3.2298171187443208,
-                    "velocityY": -1.9694974359247637,
-                    "timestamp": 0.5740591124192357
-                },
-                {
-                    "x": 3.7852456869653537,
-                    "y": 5.851315832693838,
-                    "heading": 0.1281196168762306,
-                    "angularVelocity": -0.7991930520838956,
-                    "velocityX": 2.91283633426253,
-                    "velocityY": -1.7761380198097898,
-                    "timestamp": 0.6560675570505551
-                },
-                {
-                    "x": 3.976347420420938,
-                    "y": 5.73478940159931,
-                    "heading": 0.07567676319198373,
-                    "angularVelocity": -0.63948114026563,
-                    "velocityX": 2.3302689657718703,
-                    "velocityY": -1.420907707960915,
-                    "timestamp": 0.7380760016818745
-                },
-                {
-                    "x": 4.119673708551739,
-                    "y": 5.647394681888429,
-                    "heading": 0.03633934856671714,
-                    "angularVelocity": -0.47967517006466276,
-                    "velocityX": 1.7477015784794376,
-                    "velocityY": -1.0656795175640326,
-                    "timestamp": 0.8200844463131939
-                },
-                {
-                    "x": 4.215224566025358,
-                    "y": 5.589131563991008,
-                    "heading": 0.01011289315165925,
-                    "angularVelocity": -0.31980188787828684,
-                    "velocityX": 1.1651343700417993,
-                    "velocityY": -0.7104526632513426,
-                    "timestamp": 0.9020928909445133
+                    "x": 2.3477747650996332,
+                    "y": 6.727866811231813,
+                    "heading": 0.37690695075838176,
+                    "angularVelocity": -0.1596548904259805,
+                    "velocityX": 0.5825590927179829,
+                    "velocityY": -0.3552462054262789,
+                    "timestamp": 0.08200844463131846
+                },
+                {
+                    "x": 2.4433243855136606,
+                    "y": 6.669600518122756,
+                    "heading": 0.35072364334029893,
+                    "angularVelocity": -0.3192757469769597,
+                    "velocityX": 1.16511928550267,
+                    "velocityY": -0.7104913813571495,
+                    "timestamp": 0.1640168892626369
+                },
+                {
+                    "x": 2.5866489974259967,
+                    "y": 6.582201260145896,
+                    "heading": 0.3114536293241488,
+                    "angularVelocity": -0.47885329605621113,
+                    "velocityX": 1.7476811388958082,
+                    "velocityY": -1.0657348565721152,
+                    "timestamp": 0.24602533389395537
+                },
+                {
+                    "x": 2.7777487646808345,
+                    "y": 6.465669250143918,
+                    "heading": 0.2590995246277934,
+                    "angularVelocity": -0.638398946983098,
+                    "velocityX": 2.330244990183096,
+                    "velocityY": -1.4209757364118492,
+                    "timestamp": 0.3280337785252738
+                },
+                {
+                    "x": 3.0166238424465086,
+                    "y": 6.320004790854863,
+                    "heading": 0.19366077275903337,
+                    "angularVelocity": -0.797951383701398,
+                    "velocityX": 2.912810733572304,
+                    "velocityY": -1.7762129247044165,
+                    "timestamp": 0.4100422231565923
+                },
+                {
+                    "x": 3.281496122930573,
+                    "y": 6.158489372926003,
+                    "heading": 0.19366076448026312,
+                    "angularVelocity": -1.0095021672660313e-7,
+                    "velocityX": 3.2298171447445223,
+                    "velocityY": -1.9694973932864837,
+                    "timestamp": 0.49205066778791073
+                },
+                {
+                    "x": 3.546368403715213,
+                    "y": 5.996973955490063,
+                    "heading": 0.193660756201493,
+                    "angularVelocity": -1.0095021513248387e-7,
+                    "velocityX": 3.2298171484097047,
+                    "velocityY": -1.9694973872758792,
+                    "timestamp": 0.5740591124192291
+                },
+                {
+                    "x": 3.785245611761769,
+                    "y": 5.851315694507869,
+                    "heading": 0.1281200370686441,
+                    "angularVelocity": -0.7991947588751046,
+                    "velocityX": 2.912836709932308,
+                    "velocityY": -1.7761373433799759,
+                    "timestamp": 0.6560675570505476
+                },
+                {
+                    "x": 3.9763473726141734,
+                    "y": 5.734789313275287,
+                    "heading": 0.07567704387560718,
+                    "angularVelocity": -0.6394828414171544,
+                    "velocityX": 2.3302692998450527,
+                    "velocityY": -1.4209070999510438,
+                    "timestamp": 0.7380760016818662
+                },
+                {
+                    "x": 4.119673683324307,
+                    "y": 5.647394635056781,
+                    "heading": 0.036339503004366135,
+                    "angularVelocity": -0.47967670949118707,
+                    "velocityX": 1.7477018538088223,
+                    "velocityY": -1.0656790116115789,
+                    "timestamp": 0.8200844463131847
+                },
+                {
+                    "x": 4.2152245571841105,
+                    "y": 5.589131547509505,
+                    "heading": 0.010112949222819228,
+                    "angularVelocity": -0.3198030873460947,
+                    "velocityX": 1.1651345698527396,
+                    "velocityY": -0.710452293165722,
+                    "timestamp": 0.9020928909445032
                 },
                 {
                     "x": 4.263,
                     "y": 5.56,
                     "heading": -0.003,
-                    "angularVelocity": -0.15989686440963644,
-                    "velocityX": 0.5825672488903126,
-                    "velocityY": -0.3552263931107682,
-                    "timestamp": 0.9841013355758327
+                    "angularVelocity": -0.15989754813385992,
+                    "velocityX": 0.5825673566993228,
+                    "velocityY": -0.3552261921375451,
+                    "timestamp": 0.9841013355758217
                 },
                 {
                     "x": 4.263,
                     "y": 5.56,
                     "heading": -0.003,
-                    "angularVelocity": -2.9357360091407933e-34,
-                    "velocityX": 2.8612808764100986e-34,
-                    "velocityY": 0,
-                    "timestamp": 1.066109780207152
+                    "angularVelocity": 2.1439897413450708e-33,
+                    "velocityX": 3.9230953244452345e-34,
+                    "velocityY": -1.8972025919917558e-33,
+                    "timestamp": 1.0661097802071402
                 }
             ],
             "constraints": [
@@ -9815,23 +10131,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "W1 To S3": {
             "waypoints": [
@@ -9845,8 +10145,8 @@
                     "controlIntervalCount": 30
                 },
                 {
-                    "x": 2.1914827823638916,
-                    "y": 2.376894474029541,
+                    "x": 2.131748914718628,
+                    "y": 2.531857967376709,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
@@ -9863,7 +10163,548 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 0.39,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 2.542786541644192e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.2937082670823616,
+                    "y": 6.738516922494428,
+                    "heading": 0.3838170687349535,
+                    "angularVelocity": -0.12752172771189255,
+                    "velocityX": -0.1297657401618881,
+                    "velocityY": -0.3812097977411177,
+                    "timestamp": 0.04848531599949176
+                },
+                {
+                    "x": 2.281123433600762,
+                    "y": 6.70154498788335,
+                    "heading": 0.37169259737448246,
+                    "angularVelocity": -0.2500648105569625,
+                    "velocityX": -0.25955968775719646,
+                    "velocityY": -0.7625387985814647,
+                    "timestamp": 0.09697063199898352
+                },
+                {
+                    "x": 2.262243832879689,
+                    "y": 6.646077048114969,
+                    "heading": 0.3539392998096368,
+                    "angularVelocity": -0.36615823159004923,
+                    "velocityX": -0.3893880102034898,
+                    "velocityY": -1.1440152265685524,
+                    "timestamp": 0.14545594799847528
+                },
+                {
+                    "x": 2.2370673940379584,
+                    "y": 6.572104077537847,
+                    "heading": 0.3309787534387302,
+                    "angularVelocity": -0.4735567026240549,
+                    "velocityX": -0.5192590441623445,
+                    "velocityY": -1.5256778068215682,
+                    "timestamp": 0.19394126399796704
+                },
+                {
+                    "x": 2.2055915440305824,
+                    "y": 6.479614437749851,
+                    "heading": 0.303410444299779,
+                    "angularVelocity": -0.5685908933466691,
+                    "velocityX": -0.6491831466623577,
+                    "velocityY": -1.9075804268039651,
+                    "timestamp": 0.2424265799974588
+                },
+                {
+                    "x": 2.167813255965874,
+                    "y": 6.3685929704461826,
+                    "heading": 0.2721534840825349,
+                    "angularVelocity": -0.6446685882563702,
+                    "velocityX": -0.7791696782014628,
+                    "velocityY": -2.2897956838082623,
+                    "timestamp": 0.29091189599695055
+                },
+                {
+                    "x": 2.123730000007902,
+                    "y": 6.239021112222755,
+                    "heading": 0.23878941688886735,
+                    "angularVelocity": -0.6881272505944181,
+                    "velocityX": -0.909208387103519,
+                    "velocityY": -2.6723938073299895,
+                    "timestamp": 0.3393972119964423
+                },
+                {
+                    "x": 2.0733472424162183,
+                    "y": 6.090890823809695,
+                    "heading": 0.2066566550746183,
+                    "angularVelocity": -0.6627318220196932,
+                    "velocityX": -1.0391343554894152,
+                    "velocityY": -3.0551577391940747,
+                    "timestamp": 0.38788252799593403
+                },
+                {
+                    "x": 2.0168156590767947,
+                    "y": 5.924489369921669,
+                    "heading": 0.187194468928521,
+                    "angularVelocity": -0.4014037187247154,
+                    "velocityX": -1.1659526636929207,
+                    "velocityY": -3.431996893448088,
+                    "timestamp": 0.4363678439954258
+                },
+                {
+                    "x": 1.9576664650342146,
+                    "y": 5.750871436780747,
+                    "heading": 0.18719445417707173,
+                    "angularVelocity": -3.042457077435804e-7,
+                    "velocityX": -1.2199403638687387,
+                    "velocityY": -3.580835342863344,
+                    "timestamp": 0.4848531599949175
+                },
+                {
+                    "x": 1.8985172819179166,
+                    "y": 5.577253499917319,
+                    "heading": 0.18719443942575115,
+                    "angularVelocity": -3.042430533078605e-7,
+                    "velocityX": -1.2199401385163373,
+                    "velocityY": -3.5808354196392784,
+                    "timestamp": 0.5333384759944093
+                },
+                {
+                    "x": 1.8393681174712941,
+                    "y": 5.403635556693396,
+                    "heading": 0.18719442467442912,
+                    "angularVelocity": -3.0424308271607693e-7,
+                    "velocityX": -1.219939753457998,
+                    "velocityY": -3.5808355508232466,
+                    "timestamp": 0.5818237919939011
+                },
+                {
+                    "x": 1.780218973506269,
+                    "y": 5.230017606491683,
+                    "heading": 0.18719440992310651,
+                    "angularVelocity": -3.0424309544377536e-7,
+                    "velocityX": -1.2199393310291196,
+                    "velocityY": -3.580835694738771,
+                    "timestamp": 0.6303091079933929
+                },
+                {
+                    "x": 1.7210698517072343,
+                    "y": 5.056399648738334,
+                    "heading": 0.1871943951717844,
+                    "angularVelocity": -3.042430844451726e-7,
+                    "velocityX": -1.2199388738599835,
+                    "velocityY": -3.580835850489736,
+                    "timestamp": 0.6787944239928847
+                },
+                {
+                    "x": 1.661920753431811,
+                    "y": 4.882781682970833,
+                    "heading": 0.18719438042046438,
+                    "angularVelocity": -3.042430423387178e-7,
+                    "velocityX": -1.2199383886901667,
+                    "velocityY": -3.580836015780049,
+                    "timestamp": 0.7272797399923765
+                },
+                {
+                    "x": 1.6027716795006517,
+                    "y": 4.709163708909599,
+                    "heading": 0.18719436566914785,
+                    "angularVelocity": -3.042429697472901e-7,
+                    "velocityX": -1.2199378865945683,
+                    "velocityY": -3.5808361868366427,
+                    "timestamp": 0.7757650559918683
+                },
+                {
+                    "x": 1.5436226301529286,
+                    "y": 4.5355457264731545,
+                    "heading": 0.18719435091783626,
+                    "angularVelocity": -3.042428676797686e-7,
+                    "velocityX": -1.219937379566087,
+                    "velocityY": -3.5808363595737087,
+                    "timestamp": 0.8242503719913601
+                },
+                {
+                    "x": 1.4844769425124074,
+                    "y": 4.361926598741778,
+                    "heading": 0.18719433616649686,
+                    "angularVelocity": -3.0424344145116126e-7,
+                    "velocityX": -1.2198680450801807,
+                    "velocityY": -3.58085998103429,
+                    "timestamp": 0.8727356879908519
+                },
+                {
+                    "x": 1.4424355744034427,
+                    "y": 4.183392744566331,
+                    "heading": 0.18719432108286724,
+                    "angularVelocity": -3.110968611299024e-7,
+                    "velocityX": -0.8670948562865489,
+                    "velocityY": -3.682225236552195,
+                    "timestamp": 0.9212210039903437
+                },
+                {
+                    "x": 1.4196862795998648,
+                    "y": 4.0013919676043574,
+                    "heading": 0.18719430492048,
+                    "angularVelocity": -3.3334602243497606e-7,
+                    "velocityX": -0.4691996810625877,
+                    "velocityY": -3.753729829541186,
+                    "timestamp": 0.9697063199898355
+                },
+                {
+                    "x": 1.4164891295727136,
+                    "y": 3.81800278977475,
+                    "heading": 0.18719428668091828,
+                    "angularVelocity": -3.761873328628986e-7,
+                    "velocityX": -0.06594058347012394,
+                    "velocityY": -3.7823653213178425,
+                    "timestamp": 1.0181916359893273
+                },
+                {
+                    "x": 1.432880701620732,
+                    "y": 3.635319656493888,
+                    "heading": 0.1871942648241357,
+                    "angularVelocity": -4.507917942103899e-7,
+                    "velocityX": 0.3380729136345315,
+                    "velocityY": -3.767803292913241,
+                    "timestamp": 1.066676951988819
+                },
+                {
+                    "x": 1.4686738067578264,
+                    "y": 3.455429018696199,
+                    "heading": 0.18719405074105777,
+                    "angularVelocity": -0.0000044154209051021175,
+                    "velocityX": 0.7382256751280081,
+                    "velocityY": -3.710208628927779,
+                    "timestamp": 1.1151622679883109
+                },
+                {
+                    "x": 1.5224561218642167,
+                    "y": 3.2829543962052377,
+                    "heading": 0.17869632281657946,
+                    "angularVelocity": -0.17526394846200616,
+                    "velocityX": 1.10924955315069,
+                    "velocityY": -3.557254788081333,
+                    "timestamp": 1.1636475839878027
+                },
+                {
+                    "x": 1.5921416539328836,
+                    "y": 3.121386413493124,
+                    "heading": 0.1576086367549348,
+                    "angularVelocity": -0.43492933122045885,
+                    "velocityX": 1.4372502402541338,
+                    "velocityY": -3.33230751169348,
+                    "timestamp": 1.2121328999872945
+                },
+                {
+                    "x": 1.6761645416915945,
+                    "y": 2.9727322431612,
+                    "heading": 0.12423651758368265,
+                    "angularVelocity": -0.6882933210462135,
+                    "velocityX": 1.7329553500274961,
+                    "velocityY": -3.0659626995783453,
+                    "timestamp": 1.2606182159867863
+                },
+                {
+                    "x": 1.7731411061925635,
+                    "y": 2.838565031466778,
+                    "heading": 0.08175388007293943,
+                    "angularVelocity": -0.8761959499529206,
+                    "velocityX": 2.0001223566816617,
+                    "velocityY": -2.767172058765521,
+                    "timestamp": 1.309103531986278
+                },
+                {
+                    "x": 1.8819216264400211,
+                    "y": 2.719965378093697,
+                    "heading": 0.033593056188374194,
+                    "angularVelocity": -0.9933074146707891,
+                    "velocityX": 2.2435765964482535,
+                    "velocityY": -2.4460942643771193,
+                    "timestamp": 1.3575888479857698
+                },
+                {
+                    "x": 2.0016674106146493,
+                    "y": 2.617587012881804,
+                    "heading": -0.0178597683247242,
+                    "angularVelocity": -1.0612042729434041,
+                    "velocityX": 2.4697329842357973,
+                    "velocityY": -2.1115334220519952,
+                    "timestamp": 1.4060741639852616
+                },
+                {
+                    "x": 2.131748914718628,
+                    "y": 2.531857967376709,
+                    "heading": -0.07087516069292028,
+                    "angularVelocity": -1.0934319242038333,
+                    "velocityX": 2.682905152268552,
+                    "velocityY": -1.7681445142231829,
+                    "timestamp": 1.4545594799847534
+                },
+                {
+                    "x": 2.218821194191698,
+                    "y": 2.484029060579093,
+                    "heading": -0.10483282223316899,
+                    "angularVelocity": -1.096901896110445,
+                    "velocityX": 2.812612651242981,
+                    "velocityY": -1.5449714785171498,
+                    "timestamp": 1.4855172727096597
+                },
+                {
+                    "x": 2.3097767814437278,
+                    "y": 2.4431840829179357,
+                    "heading": -0.13859592156415457,
+                    "angularVelocity": -1.0906171389530375,
+                    "velocityX": 2.938051432149048,
+                    "velocityY": -1.3193762883647426,
+                    "timestamp": 1.516475065434566
+                },
+                {
+                    "x": 2.4044569077078632,
+                    "y": 2.4094074987036276,
+                    "heading": -0.1718069818405491,
+                    "angularVelocity": -1.0727851488087659,
+                    "velocityX": 3.0583616572737307,
+                    "velocityY": -1.0910527282945428,
+                    "timestamp": 1.5474328581594723
+                },
+                {
+                    "x": 2.5026677503931767,
+                    "y": 2.3827944816766236,
+                    "heading": -0.20403633414457553,
+                    "angularVelocity": -1.0410739741365005,
+                    "velocityX": 3.1724110164298485,
+                    "velocityY": -0.859654861819398,
+                    "timestamp": 1.5783906508843786
+                },
+                {
+                    "x": 2.6041682782784212,
+                    "y": 2.363451815109364,
+                    "heading": -0.23475758891885987,
+                    "angularVelocity": -0.9923593405260972,
+                    "velocityX": 3.2786745743372427,
+                    "velocityY": -0.6248076773199155,
+                    "timestamp": 1.6093484436092849
+                },
+                {
+                    "x": 2.7086523080371263,
+                    "y": 2.3514976196224144,
+                    "heading": -0.26331031649970843,
+                    "angularVelocity": -0.9223114784903548,
+                    "velocityX": 3.3750477847899405,
+                    "velocityY": -0.38614495527174003,
+                    "timestamp": 1.6403062363341911
+                },
+                {
+                    "x": 2.8157215031828033,
+                    "y": 2.3470577985799963,
+                    "heading": -0.288841887845313,
+                    "angularVelocity": -0.8247219551529203,
+                    "velocityX": 3.458553912306755,
+                    "velocityY": -0.14341529714262294,
+                    "timestamp": 1.6712640290590974
+                },
+                {
+                    "x": 2.9248447015312338,
+                    "y": 2.3502540561946077,
+                    "heading": -0.3102182612745416,
+                    "angularVelocity": -0.6905005668986338,
+                    "velocityX": 3.524902415281217,
+                    "velocityY": 0.10324565588123534,
+                    "timestamp": 1.7022218217840037
+                },
+                {
+                    "x": 3.0353304312162184,
+                    "y": 2.361176073397281,
+                    "heading": -0.326082506948615,
+                    "angularVelocity": -0.5124475706430824,
+                    "velocityX": 3.568914963243171,
+                    "velocityY": 0.3528034863374462,
+                    "timestamp": 1.73317961450891
+                },
+                {
+                    "x": 3.1466093544989446,
+                    "y": 2.379927120590619,
+                    "heading": -0.3369287204650216,
+                    "angularVelocity": -0.35035487229182166,
+                    "velocityX": 3.594536738181179,
+                    "velocityY": 0.605697161937562,
+                    "timestamp": 1.7641374072338163
+                },
+                {
+                    "x": 3.2578769593015027,
+                    "y": 2.406586601459548,
+                    "heading": -0.34366338052401463,
+                    "angularVelocity": -0.21754328930658381,
+                    "velocityX": 3.5941711281316278,
+                    "velocityY": 0.8611557389062998,
+                    "timestamp": 1.7950951999587226
+                },
+                {
+                    "x": 3.3679824864036325,
+                    "y": 2.4411213005139327,
+                    "heading": -0.34790283328354016,
+                    "angularVelocity": -0.13694299193573645,
+                    "velocityX": 3.556633642474382,
+                    "velocityY": 1.1155413876329803,
+                    "timestamp": 1.8260529926836289
+                },
+                {
+                    "x": 3.475304114496225,
+                    "y": 2.4831460278092354,
+                    "heading": -0.3524511595124565,
+                    "angularVelocity": -0.14692023650373393,
+                    "velocityX": 3.466708012625811,
+                    "velocityY": 1.3574846136083376,
+                    "timestamp": 1.8570107854085351
+                },
+                {
+                    "x": 3.5778542240444473,
+                    "y": 2.531447362567012,
+                    "heading": -0.3616437420636369,
+                    "angularVelocity": -0.29693921116020117,
+                    "velocityX": 3.312578207999158,
+                    "velocityY": 1.560231867531513,
+                    "timestamp": 1.8879685781334414
+                },
+                {
+                    "x": 3.673558919058804,
+                    "y": 2.583451056106446,
+                    "heading": -0.37745622232401294,
+                    "angularVelocity": -0.5107754419475534,
+                    "velocityX": 3.091457322713648,
+                    "velocityY": 1.6798256258655577,
+                    "timestamp": 1.9189263708583477
+                },
+                {
+                    "x": 3.7613706261508923,
+                    "y": 2.636186152260406,
+                    "heading": -0.3968957996596081,
+                    "angularVelocity": -0.6279380932955558,
+                    "velocityX": 2.836497675157173,
+                    "velocityY": 1.7034514257095354,
+                    "timestamp": 1.949884163583254
+                },
+                {
+                    "x": 3.841306735175595,
+                    "y": 2.6876448087218088,
+                    "heading": -0.4179459699473055,
+                    "angularVelocity": -0.6799635386030257,
+                    "velocityX": 2.582099755467769,
+                    "velocityY": 1.6622198138851267,
+                    "timestamp": 1.9808419563081603
+                },
+                {
+                    "x": 3.913645075614716,
+                    "y": 2.736629100397619,
+                    "heading": -0.43934657697326235,
+                    "angularVelocity": -0.6912833617387507,
+                    "velocityX": 2.336676296079001,
+                    "velocityY": 1.582292772328868,
+                    "timestamp": 2.0117997490330666
+                },
+                {
+                    "x": 3.9786691618750996,
+                    "y": 2.7823974078733573,
+                    "heading": -0.46027689817629036,
+                    "angularVelocity": -0.6760921681319296,
+                    "velocityX": 2.100410931692797,
+                    "velocityY": 1.4784099074015198,
+                    "timestamp": 2.0427575417579726
+                },
+                {
+                    "x": 4.036618763930541,
+                    "y": 2.824460782764613,
+                    "heading": -0.48017689659950147,
+                    "angularVelocity": -0.6428106357887643,
+                    "velocityX": 1.8718906276836884,
+                    "velocityY": 1.3587330099747585,
+                    "timestamp": 2.0737153344828787
+                },
+                {
+                    "x": 4.08768908254176,
+                    "y": 2.862477844590781,
+                    "heading": -0.49864492956893264,
+                    "angularVelocity": -0.5965552238942878,
+                    "velocityX": 1.6496757073449355,
+                    "velocityY": 1.2280288250525657,
+                    "timestamp": 2.1046731272077848
+                },
+                {
+                    "x": 4.132038835388808,
+                    "y": 2.8961988047734253,
+                    "heading": -0.5153800748468587,
+                    "angularVelocity": -0.5405794084679139,
+                    "velocityX": 1.4325876925792385,
+                    "velocityY": 1.0892559583398733,
+                    "timestamp": 2.135630919932691
+                },
+                {
+                    "x": 4.169798307734006,
+                    "y": 2.9254338939938918,
+                    "heading": -0.5301487101337384,
+                    "angularVelocity": -0.47705711511170307,
+                    "velocityX": 1.2197081581622198,
+                    "velocityY": 0.944353154640467,
+                    "timestamp": 2.166588712657597
+                },
+                {
+                    "x": 4.201075770562755,
+                    "y": 2.9500345091711524,
+                    "heading": -0.5427642633485259,
+                    "angularVelocity": -0.4075081620800149,
+                    "velocityX": 1.0103259979397883,
+                    "velocityY": 0.7946501675963605,
+                    "timestamp": 2.197546505382503
+                },
+                {
+                    "x": 4.225962326087245,
+                    "y": 2.9698813733488647,
+                    "heading": -0.5530743601373866,
+                    "angularVelocity": -0.3330372058810695,
+                    "velocityX": 0.8038866254348108,
+                    "velocityY": 0.6410942909939014,
+                    "timestamp": 2.228504298107409
+                },
+                {
+                    "x": 4.24453552816763,
+                    "y": 2.984876771487546,
+                    "heading": -0.5609523103628415,
+                    "angularVelocity": -0.2544738992160421,
+                    "velocityX": 0.5999524011761236,
+                    "velocityY": 0.48438201883216997,
+                    "timestamp": 2.259462090832315
+                },
+                {
+                    "x": 4.256862100646349,
+                    "y": 2.9949392682905573,
+                    "heading": -0.5662912533122896,
+                    "angularVelocity": -0.17245877305093363,
+                    "velocityX": 0.398173493448939,
+                    "velocityY": 0.32503922008988784,
+                    "timestamp": 2.290419883557221
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": -0.569,
+                    "angularVelocity": -0.08749805620543083,
+                    "velocityX": 0.19826669841112912,
+                    "velocityY": 0.16347198117160036,
+                    "timestamp": 2.321377676282127
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": -0.569,
+                    "angularVelocity": 1.3974941712649426e-29,
+                    "velocityX": 2.480011116993708e-30,
+                    "velocityY": -4.1979337006948246e-30,
+                    "timestamp": 2.3523354690070333
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -11030,16 +11871,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 28
+                    "controlIntervalCount": 10
                 },
                 {
                     "x": 4.748738765716553,
                     "y": 4.2041850090026855,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 29
+                    "headingConstrained": true,
+                    "controlIntervalCount": 10
                 },
                 {
                     "x": 7.68,
@@ -11048,10 +11889,200 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 10
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 0.39,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.3668131239789423,
+                    "y": 6.6799532352680275,
+                    "heading": 0.31688592464549686,
+                    "angularVelocity": -0.6546901107445974,
+                    "velocityX": 0.5982690928509629,
+                    "velocityY": -0.6899048465062367,
+                    "timestamp": 0.11167737858654542
+                },
+                {
+                    "x": 2.5010623988117353,
+                    "y": 6.525329092787392,
+                    "heading": 0.1875032700506318,
+                    "angularVelocity": -1.1585395022018103,
+                    "velocityX": 1.202116995688263,
+                    "velocityY": -1.3845609955896643,
+                    "timestamp": 0.22335475717309083
+                },
+                {
+                    "x": 2.7034647042549174,
+                    "y": 6.29238592031536,
+                    "heading": 0.03684167640916454,
+                    "angularVelocity": -1.3490788873122188,
+                    "velocityX": 1.8123841014618285,
+                    "velocityY": -2.0858581694904785,
+                    "timestamp": 0.33503213575963625
+                },
+                {
+                    "x": 2.970402662431944,
+                    "y": 5.984552676414473,
+                    "heading": 2.692880535790558e-7,
+                    "angularVelocity": -0.32989140314179594,
+                    "velocityX": 2.39025988571325,
+                    "velocityY": -2.7564511971627637,
+                    "timestamp": 0.44670951434618167
+                },
+                {
+                    "x": 3.247779632307318,
+                    "y": 5.665896160181276,
+                    "heading": 2.2674846776964554e-7,
+                    "angularVelocity": -3.809149744305609e-7,
+                    "velocityX": 2.4837346057547505,
+                    "velocityY": -2.853366727141142,
+                    "timestamp": 0.5583868929327271
+                },
+                {
+                    "x": 3.5251566242922663,
+                    "y": 5.347239663193555,
+                    "heading": 1.842089099097335e-7,
+                    "angularVelocity": -3.8091472416520575e-7,
+                    "velocityX": 2.4837348037318896,
+                    "velocityY": -2.853366554810183,
+                    "timestamp": 0.6700642715192726
+                },
+                {
+                    "x": 3.8025335437012195,
+                    "y": 5.0285831030297485,
+                    "heading": 1.4166936288049968e-7,
+                    "angularVelocity": -3.8091462718327505e-7,
+                    "velocityX": 2.4837341538598574,
+                    "velocityY": -2.8533671205119875,
+                    "timestamp": 0.7817416501058181
+                },
+                {
+                    "x": 4.079910865419093,
+                    "y": 4.7099268930580935,
+                    "heading": 9.912982095474378e-8,
+                    "angularVelocity": -3.809145814844431e-7,
+                    "velocityX": 2.4837377562807075,
+                    "velocityY": -2.853363984763686,
+                    "timestamp": 0.8934190286923636
+                },
+                {
+                    "x": 4.382964098552189,
+                    "y": 4.415582373772677,
+                    "heading": 5.542611481257948e-8,
+                    "angularVelocity": -3.9133893269333644e-7,
+                    "velocityX": 2.713649236476739,
+                    "velocityY": -2.635668234792112,
+                    "timestamp": 1.005096407278909
+                },
+                {
+                    "x": 4.748738765716553,
+                    "y": 4.2041850090026855,
+                    "heading": 0,
+                    "angularVelocity": -4.963056575446211e-7,
+                    "velocityX": 3.27527984443961,
+                    "velocityY": -1.8929291450565913,
+                    "timestamp": 1.1167737858654545
+                },
+                {
+                    "x": 5.133803316447558,
+                    "y": 4.089523361787239,
+                    "heading": -1.387645596172588e-10,
+                    "angularVelocity": -1.3065520492479572e-9,
+                    "velocityX": 3.625615065458012,
+                    "velocityY": -1.0796085871456598,
+                    "timestamp": 1.2229804690844188
+                },
+                {
+                    "x": 5.534974194059096,
+                    "y": 4.0675249151482635,
+                    "heading": -2.526284831735191e-10,
+                    "angularVelocity": -1.0720975375429818e-9,
+                    "velocityX": 3.7772658504404184,
+                    "velocityY": -0.20712864739049783,
+                    "timestamp": 1.3291871523033831
+                },
+                {
+                    "x": 5.9365934232146325,
+                    "y": 4.0786604378791305,
+                    "heading": -3.6352969398434843e-10,
+                    "angularVelocity": -1.0442018095219078e-9,
+                    "velocityX": 3.781487350730336,
+                    "velocityY": 0.10484766488641149,
+                    "timestamp": 1.4353938355223474
+                },
+                {
+                    "x": 6.3382126442540345,
+                    "y": 4.089796253283455,
+                    "heading": -4.744184411388132e-10,
+                    "angularVelocity": -1.044084456790862e-9,
+                    "velocityX": 3.781487274312031,
+                    "velocityY": 0.1048504205837075,
+                    "timestamp": 1.5416005187413118
+                },
+                {
+                    "x": 6.739831868633552,
+                    "y": 4.100931948201603,
+                    "heading": -5.853040774969199e-10,
+                    "angularVelocity": -1.0440551668485128e-9,
+                    "velocityX": 3.7814873057612264,
+                    "velocityY": 0.10484928613381475,
+                    "timestamp": 1.647807201960276
+                },
+                {
+                    "x": 7.11589910333758,
+                    "y": 4.111359168329562,
+                    "heading": -0.0011999964635320353,
+                    "angularVelocity": -0.011298685184913941,
+                    "velocityX": 3.5408999067290075,
+                    "velocityY": 0.09817856854143046,
+                    "timestamp": 1.7540138851792404
+                },
+                {
+                    "x": 7.397949547057075,
+                    "y": 4.119179583905487,
+                    "heading": -0.002099997192055183,
+                    "angularVelocity": -0.008474049854922212,
+                    "velocityX": 2.6556750966226486,
+                    "velocityY": 0.0736339309252122,
+                    "timestamp": 1.8602205683982047
+                },
+                {
+                    "x": 7.585983181328134,
+                    "y": 4.124393194553852,
+                    "heading": -0.0026999988369627403,
+                    "angularVelocity": -0.005649377484767236,
+                    "velocityX": 1.7704501126675238,
+                    "velocityY": 0.049089289773120645,
+                    "timestamp": 1.966427251617169
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": -0.003,
+                    "angularVelocity": -0.0028246919491873987,
+                    "velocityX": 0.8852250707992916,
+                    "velocityY": 0.024544646034960003,
+                    "timestamp": 2.072633934836133
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": -0.003,
+                    "angularVelocity": 0,
+                    "velocityX": 1.6661554323284825e-27,
+                    "velocityY": 0,
+                    "timestamp": 2.1788406180550974
                 }
             ],
-            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -11066,8 +12097,8 @@
                     "type": "StopPoint"
                 }
             ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
+            "usesControlIntervalGuessing": false,
+            "defaultControlIntervalCount": 10,
             "usesDefaultFieldObstacles": true,
             "circleObstacles": [
                 {
@@ -11091,25 +12122,25 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 26
+                    "controlIntervalCount": 18
                 },
                 {
-                    "x": 4.169802665710449,
-                    "y": 4.898907661437988,
-                    "heading": 3.14,
+                    "x": 4.274456024169922,
+                    "y": 4.934326648712158,
+                    "heading": -0.8357152799414105,
                     "isInitialGuess": false,
                     "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 10
+                    "headingConstrained": true,
+                    "controlIntervalCount": 12
                 },
                 {
                     "x": 5.703982353210449,
                     "y": 3.987083911895752,
-                    "heading": 3.14,
+                    "heading": -0.31586356261422194,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
-                    "controlIntervalCount": 27
+                    "controlIntervalCount": 13
                 },
                 {
                     "x": 7.68,
@@ -11126,577 +12157,397 @@
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.39,
-                    "angularVelocity": 0,
-                    "velocityX": 1.0547928981710615e-31,
-                    "velocityY": 1.4824740100837611e-32,
+                    "angularVelocity": 1.592522039385819e-32,
+                    "velocityX": 0,
+                    "velocityY": -2.1348488243236776e-32,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.3072711000078927,
-                    "y": 6.749947927475375,
-                    "heading": 0.3875624240531776,
-                    "angularVelocity": -0.06985747897751753,
-                    "velocityX": 0.2083794421285326,
-                    "velocityY": -0.20210242424612682,
-                    "timestamp": 0.034893557318421635
-                },
-                {
-                    "x": 2.321814346754588,
-                    "y": 6.735842802544083,
-                    "heading": 0.3827646159677583,
-                    "angularVelocity": -0.1374983938048157,
-                    "velocityX": 0.41678888208448434,
-                    "velocityY": -0.40423293052572296,
-                    "timestamp": 0.06978711463684327
-                },
-                {
-                    "x": 2.343630936460155,
-                    "y": 6.714683501867812,
-                    "heading": 0.3756983254810237,
-                    "angularVelocity": -0.20250989093060998,
-                    "velocityX": 0.6252326040156703,
-                    "velocityY": -0.6063956300924075,
-                    "timestamp": 0.1046806719552649
-                },
-                {
-                    "x": 2.372722244735657,
-                    "y": 6.6864687287677045,
-                    "heading": 0.36647405587954873,
-                    "angularVelocity": -0.2643545201567945,
-                    "velocityX": 0.8337157490143364,
-                    "velocityY": -0.8085954906412398,
-                    "timestamp": 0.13957422927368654
-                },
-                {
-                    "x": 2.4090898628559576,
-                    "y": 6.651196976344128,
-                    "heading": 0.3552274150516352,
-                    "angularVelocity": -0.3223128191053197,
-                    "velocityX": 1.042244497699901,
-                    "velocityY": -1.0108385368022033,
-                    "timestamp": 0.17446778659210818
-                },
-                {
-                    "x": 2.452735639247045,
-                    "y": 6.608866484215248,
-                    "heading": 0.3421287688368095,
-                    "angularVelocity": -0.3753886740550377,
-                    "velocityX": 1.2508262196599174,
-                    "velocityY": -1.213132033016619,
-                    "timestamp": 0.20936134391052982
-                },
-                {
-                    "x": 2.5036617186589587,
-                    "y": 6.55947519544937,
-                    "heading": 0.3273986519293141,
-                    "angularVelocity": -0.42214431658760004,
-                    "velocityX": 1.459469407122573,
-                    "velocityY": -1.4154844779841917,
-                    "timestamp": 0.24425490122895147
-                },
-                {
-                    "x": 2.5618705496598224,
-                    "y": 6.503020741236739,
-                    "heading": 0.31133395345849213,
-                    "angularVelocity": -0.4603915365872077,
-                    "velocityX": 1.6681827670844436,
-                    "velocityY": -1.6179048096889317,
-                    "timestamp": 0.2791484585473731
-                },
-                {
-                    "x": 2.6273647520175327,
-                    "y": 6.43950055782557,
-                    "heading": 0.294356217256913,
-                    "angularVelocity": -0.486557906568497,
-                    "velocityX": 1.8769712058889019,
-                    "velocityY": -1.8203986148936577,
-                    "timestamp": 0.3140420158657947
-                },
-                {
-                    "x": 2.7001464092257668,
-                    "y": 6.368912559254549,
-                    "heading": 0.2771114307132975,
-                    "angularVelocity": -0.4942111916606252,
-                    "velocityX": 2.0858193546752704,
-                    "velocityY": -2.022952200799379,
-                    "timestamp": 0.34893557318421636
-                },
-                {
-                    "x": 2.7802136587601143,
-                    "y": 6.291258423996795,
-                    "heading": 0.2607138645949654,
-                    "angularVelocity": -0.46993105256354084,
-                    "velocityX": 2.294614126146364,
-                    "velocityY": -2.225457684039485,
-                    "timestamp": 0.383829130502638
-                },
-                {
-                    "x": 2.867539127268515,
-                    "y": 6.206563552711186,
-                    "heading": 0.24753249327015822,
-                    "angularVelocity": -0.37775945870237265,
-                    "velocityX": 2.5026244160637545,
-                    "velocityY": -2.427235220322282,
-                    "timestamp": 0.41872268782105965
-                },
-                {
-                    "x": 2.961704724093414,
-                    "y": 6.1150775036748035,
-                    "heading": 0.24574502539338514,
-                    "angularVelocity": -0.05122630118968667,
-                    "velocityX": 2.6986528190746,
-                    "velocityY": -2.6218607693541762,
-                    "timestamp": 0.4536162451394813
-                },
-                {
-                    "x": 3.0564622015652247,
-                    "y": 6.023180384324785,
-                    "heading": 0.2457450132145974,
-                    "angularVelocity": -3.4902683097432965e-7,
-                    "velocityX": 2.7156152812710346,
-                    "velocityY": -2.633641463133395,
-                    "timestamp": 0.48850980245790293
-                },
-                {
-                    "x": 3.1512196852998877,
-                    "y": 5.931283271432529,
-                    "heading": 0.24574500103583913,
-                    "angularVelocity": -3.490259866463667e-7,
-                    "velocityX": 2.7156154607555005,
-                    "velocityY": -2.6336412780630685,
-                    "timestamp": 0.5234033597763246
-                },
-                {
-                    "x": 3.2459771690350276,
-                    "y": 5.839386158540766,
-                    "heading": 0.2457449888570809,
-                    "angularVelocity": -3.4902598588384144e-7,
-                    "velocityX": 2.7156154607691696,
-                    "velocityY": -2.6336412780489744,
-                    "timestamp": 0.5582969170947462
-                },
-                {
-                    "x": 3.3407346527701676,
-                    "y": 5.747489045649002,
-                    "heading": 0.24574497667832262,
-                    "angularVelocity": -3.4902598639891e-7,
-                    "velocityX": 2.7156154607691705,
-                    "velocityY": -2.6336412780489735,
-                    "timestamp": 0.5931904744131677
-                },
-                {
-                    "x": 3.4354921365053075,
-                    "y": 5.655591932757238,
-                    "heading": 0.24574496449956437,
-                    "angularVelocity": -3.4902598640760607e-7,
-                    "velocityX": 2.71561546076917,
-                    "velocityY": -2.6336412780489735,
-                    "timestamp": 0.6280840317315893
-                },
-                {
-                    "x": 3.5302496202404474,
-                    "y": 5.563694819865473,
-                    "heading": 0.2457449523208061,
-                    "angularVelocity": -3.490259865548224e-7,
-                    "velocityX": 2.7156154607691705,
-                    "velocityY": -2.6336412780489735,
-                    "timestamp": 0.6629775890500109
-                },
-                {
-                    "x": 3.6250071039755873,
-                    "y": 5.471797706973709,
-                    "heading": 0.2457449401420478,
-                    "angularVelocity": -3.4902598644057735e-7,
-                    "velocityX": 2.71561546076917,
-                    "velocityY": -2.633641278048974,
-                    "timestamp": 0.6978711463684325
-                },
-                {
-                    "x": 3.719764587710727,
-                    "y": 5.379900594081945,
-                    "heading": 0.24574492796328956,
-                    "angularVelocity": -3.4902598592532335e-7,
-                    "velocityX": 2.715615460769166,
-                    "velocityY": -2.633641278048979,
-                    "timestamp": 0.7327647036868541
-                },
-                {
-                    "x": 3.8145220714436685,
-                    "y": 5.288003481187913,
-                    "heading": 0.24574491578453134,
-                    "angularVelocity": -3.490259859254125e-7,
-                    "velocityX": 2.7156154607061804,
-                    "velocityY": -2.6336412781139247,
-                    "timestamp": 0.7676582610052757
-                },
-                {
-                    "x": 3.909279526317127,
-                    "y": 5.196106338536137,
-                    "heading": 0.245744903605773,
-                    "angularVelocity": -3.4902598866436484e-7,
-                    "velocityX": 2.7156146336342,
-                    "velocityY": -2.6336421309288234,
-                    "timestamp": 0.8025518183236973
-                },
-                {
-                    "x": 4.003658642045134,
-                    "y": 5.103820680647278,
-                    "heading": 0.24574489142231412,
-                    "angularVelocity": -3.4916069991394096e-7,
-                    "velocityX": 2.7047719688402356,
-                    "velocityY": -2.644776428116158,
-                    "timestamp": 0.8374453756421188
-                },
-                {
-                    "x": 4.090673374879912,
-                    "y": 5.004560939161802,
-                    "heading": 0.24574487906407927,
-                    "angularVelocity": -3.541695320845138e-7,
-                    "velocityX": 2.493719171155964,
-                    "velocityY": -2.8446437999909007,
-                    "timestamp": 0.8723389329605404
-                },
-                {
-                    "x": 4.169802665710449,
-                    "y": 4.898907661437988,
-                    "heading": 0.24574486610274957,
-                    "angularVelocity": -3.7145337638188186e-7,
-                    "velocityX": 2.267733556325072,
-                    "velocityY": -3.0278735056925665,
-                    "timestamp": 0.907232490278962
-                },
-                {
-                    "x": 4.2677114826543,
-                    "y": 4.731769321945997,
-                    "heading": 0.24574485481420902,
-                    "angularVelocity": -2.2045907687430814e-7,
-                    "velocityX": 1.9121061102333996,
-                    "velocityY": -3.264121150398313,
-                    "timestamp": 0.9584371888152268
-                },
-                {
-                    "x": 4.383830202938794,
-                    "y": 4.576728030606808,
-                    "heading": 0.24574484074146902,
-                    "angularVelocity": -2.7483298164960767e-7,
-                    "velocityX": 2.2677356493419163,
-                    "velocityY": -3.02787235881067,
-                    "timestamp": 1.0096418873514916
-                },
-                {
-                    "x": 4.516679781731222,
-                    "y": 4.435758724265,
-                    "heading": 0.24574482059862868,
-                    "angularVelocity": -3.9337875070195686e-7,
-                    "velocityX": 2.59448024478339,
-                    "velocityY": -2.753054121429295,
-                    "timestamp": 1.0608465858877565
-                },
-                {
-                    "x": 4.664567883303712,
-                    "y": 4.3106571835035465,
-                    "heading": 0.24574435877996634,
-                    "angularVelocity": -0.000009019068084382453,
-                    "velocityX": 2.8881744410183567,
-                    "velocityY": -2.4431652629075082,
-                    "timestamp": 1.1120512844240213
-                },
-                {
-                    "x": 4.823636899736968,
-                    "y": 4.204273204304783,
-                    "heading": 0.2400133838174815,
-                    "angularVelocity": -0.1119228337693714,
-                    "velocityX": 3.1065316461261507,
-                    "velocityY": -2.0776214339670136,
-                    "timestamp": 1.1632559829602862
-                },
-                {
-                    "x": 4.9908096959943835,
-                    "y": 4.118113363926556,
-                    "heading": 0.22784637628221624,
-                    "angularVelocity": -0.23761506039622932,
-                    "velocityX": 3.26479407234511,
-                    "velocityY": -1.6826549680241993,
-                    "timestamp": 1.214460681496551
-                },
-                {
-                    "x": 5.163718040144699,
-                    "y": 4.053028192881653,
-                    "heading": 0.21228377101249774,
-                    "angularVelocity": -0.3039292431083551,
-                    "velocityX": 3.3768062129661107,
-                    "velocityY": -1.2710781023114057,
-                    "timestamp": 1.265665380032816
-                },
-                {
-                    "x": 5.340853559631665,
-                    "y": 4.009388363770341,
-                    "heading": 0.19524077553782296,
-                    "angularVelocity": -0.332840461165967,
-                    "velocityX": 3.4593606553803307,
-                    "velocityY": -0.8522622016884831,
-                    "timestamp": 1.3168700785690808
-                },
-                {
-                    "x": 5.521188360604893,
-                    "y": 3.987375039650421,
-                    "heading": 0.17800934218450973,
-                    "angularVelocity": -0.3365205507676074,
-                    "velocityX": 3.521840888204999,
-                    "velocityY": -0.42990828477058174,
-                    "timestamp": 1.3680747771053456
+                    "x": 2.311435758812473,
+                    "y": 6.743296066265471,
+                    "heading": 0.3325883455828011,
+                    "angularVelocity": -1.0899722636212008,
+                    "velocityX": 0.2171102722189281,
+                    "velocityY": -0.26017204737900074,
+                    "timestamp": 0.052672582902671385
+                },
+                {
+                    "x": 2.3359939039892414,
+                    "y": 6.71538911279917,
+                    "heading": 0.2289766220305027,
+                    "angularVelocity": -1.9670902363712421,
+                    "velocityX": 0.46624152117519013,
+                    "velocityY": -0.529819346772266,
+                    "timestamp": 0.10534516580534277
+                },
+                {
+                    "x": 2.3766647023789824,
+                    "y": 6.674615688796359,
+                    "heading": 0.08902544501665462,
+                    "angularVelocity": -2.6570023587499336,
+                    "velocityX": 0.7721436115045406,
+                    "velocityY": -0.7740919802272123,
+                    "timestamp": 0.15801774870801416
+                },
+                {
+                    "x": 2.4341603545379895,
+                    "y": 6.621640281380937,
+                    "heading": -0.08739056422595769,
+                    "angularVelocity": -3.349294823240299,
+                    "velocityX": 1.0915669783129407,
+                    "velocityY": -1.0057491866937662,
+                    "timestamp": 0.21069033161068554
+                },
+                {
+                    "x": 2.508963105088746,
+                    "y": 6.555426903517877,
+                    "heading": -0.2922468734649172,
+                    "angularVelocity": -3.8892398654057443,
+                    "velocityX": 1.4201458601143102,
+                    "velocityY": -1.2570748236404126,
+                    "timestamp": 0.2633629145133569
+                },
+                {
+                    "x": 2.602244928931498,
+                    "y": 6.475307627430128,
+                    "heading": -0.4990571722352239,
+                    "angularVelocity": -3.9263367652285313,
+                    "velocityX": 1.7709749304513536,
+                    "velocityY": -1.5210812090949455,
+                    "timestamp": 0.3160354974160283
+                },
+                {
+                    "x": 2.7117676530035184,
+                    "y": 6.382297804514516,
+                    "heading": -0.6662019146269795,
+                    "angularVelocity": -3.173277883497873,
+                    "velocityX": 2.0793118172009177,
+                    "velocityY": -1.7658109359755114,
+                    "timestamp": 0.3687080803186997
+                },
+                {
+                    "x": 2.833611348488951,
+                    "y": 6.27498915923685,
+                    "heading": -0.7793427065081729,
+                    "angularVelocity": -2.1480015910792667,
+                    "velocityX": 2.3132280357425485,
+                    "velocityY": -2.0372770683364947,
+                    "timestamp": 0.42138066322137113
+                },
+                {
+                    "x": 2.968521622075034,
+                    "y": 6.1546184694434425,
+                    "heading": -0.8356933529016568,
+                    "angularVelocity": -1.0698288044390758,
+                    "velocityX": 2.5612997531442825,
+                    "velocityY": -2.2852627146807634,
+                    "timestamp": 0.47405324612404254
+                },
+                {
+                    "x": 3.1177887530073343,
+                    "y": 6.022632523021301,
+                    "heading": -0.8357129106693775,
+                    "angularVelocity": -0.00037130830959681015,
+                    "velocityX": 2.8338676918144654,
+                    "velocityY": -2.5057807904736045,
+                    "timestamp": 0.5267258290267139
+                },
+                {
+                    "x": 3.269422043665646,
+                    "y": 5.893362401601714,
+                    "heading": -0.8357131785361996,
+                    "angularVelocity": -0.000005085507627644201,
+                    "velocityX": 2.8787897289658386,
+                    "velocityY": -2.45422028493365,
+                    "timestamp": 0.5793984119293853
+                },
+                {
+                    "x": 3.4210553878779164,
+                    "y": 5.764092343000596,
+                    "heading": -0.8357134464026715,
+                    "angularVelocity": -0.000005085500977911915,
+                    "velocityX": 2.8787907456989505,
+                    "velocityY": -2.4542190923119205,
+                    "timestamp": 0.6320709948320568
+                },
+                {
+                    "x": 3.572688732091381,
+                    "y": 5.634822284400852,
+                    "heading": -0.8357137142690544,
+                    "angularVelocity": -0.000005085499289329294,
+                    "velocityX": 2.878790745721617,
+                    "velocityY": -2.4542190922857987,
+                    "timestamp": 0.6847435777347282
+                },
+                {
+                    "x": 3.7243220763038787,
+                    "y": 5.50555222579995,
+                    "heading": -0.8357139821353484,
+                    "angularVelocity": -0.000005085497600843703,
+                    "velocityX": 2.8787907457032484,
+                    "velocityY": -2.4542190923078118,
+                    "timestamp": 0.7374161606373996
+                },
+                {
+                    "x": 3.875955376681643,
+                    "y": 5.376282115781124,
+                    "heading": -0.8357142500017658,
+                    "angularVelocity": -0.000005085499943938472,
+                    "velocityX": 2.878789913491687,
+                    "velocityY": -2.4542200684878277,
+                    "timestamp": 0.790088743540071
+                },
+                {
+                    "x": 4.025639474822054,
+                    "y": 5.244759916762783,
+                    "heading": -0.8357145286285158,
+                    "angularVelocity": -0.000005289787108985714,
+                    "velocityX": 2.8417839014463553,
+                    "velocityY": -2.4969764490450044,
+                    "timestamp": 0.8427613264427424
+                },
+                {
+                    "x": 4.159073025842643,
+                    "y": 5.096777164207917,
+                    "heading": -0.835714908739186,
+                    "angularVelocity": -0.000007216480551859566,
+                    "velocityX": 2.5332638664625207,
+                    "velocityY": -2.809483499761346,
+                    "timestamp": 0.8954339093454138
+                },
+                {
+                    "x": 4.274456024169922,
+                    "y": 4.934326648712158,
+                    "heading": -0.8357152799414105,
+                    "angularVelocity": -0.000007047351849068254,
+                    "velocityX": 2.19057034929318,
+                    "velocityY": -3.0841570043363045,
+                    "timestamp": 0.9481064922480852
+                },
+                {
+                    "x": 4.3556906912761315,
+                    "y": 4.793345794539918,
+                    "heading": -0.8357152378475459,
+                    "angularVelocity": 9.786625189030426e-7,
+                    "velocityX": 1.8886677315079998,
+                    "velocityY": -3.2777384277013035,
+                    "timestamp": 0.9911181166598327
+                },
+                {
+                    "x": 4.442446525481621,
+                    "y": 4.655693706781043,
+                    "heading": -0.8357151935315337,
+                    "angularVelocity": 0.0000010303263992905167,
+                    "velocityX": 2.0170322649286843,
+                    "velocityY": -3.200346177143663,
+                    "timestamp": 1.0341297410715804
+                },
+                {
+                    "x": 4.541847641111602,
+                    "y": 4.5268757636785075,
+                    "heading": -0.8357148699573956,
+                    "angularVelocity": 0.000007522946236810339,
+                    "velocityX": 2.311029099445763,
+                    "velocityY": -2.9949564766345187,
+                    "timestamp": 1.077141365483328
+                },
+                {
+                    "x": 4.65067034095477,
+                    "y": 4.409944405402937,
+                    "heading": -0.8255512517653536,
+                    "angularVelocity": 0.23629933375095027,
+                    "velocityX": 2.530076492843306,
+                    "velocityY": -2.7185989805033612,
+                    "timestamp": 1.1201529898950755
+                },
+                {
+                    "x": 4.7668457664948924,
+                    "y": 4.306168868140065,
+                    "heading": -0.803496324956771,
+                    "angularVelocity": 0.5127666557638517,
+                    "velocityX": 2.701023900608423,
+                    "velocityY": -2.4127323411326107,
+                    "timestamp": 1.163164614306823
+                },
+                {
+                    "x": 4.88912359332006,
+                    "y": 4.216234980411927,
+                    "heading": -0.7705722789003837,
+                    "angularVelocity": 0.7654685566210554,
+                    "velocityX": 2.8429018549638743,
+                    "velocityY": -2.0909205117948377,
+                    "timestamp": 1.2061762387185706
+                },
+                {
+                    "x": 5.016560626655114,
+                    "y": 4.140615450479534,
+                    "heading": -0.7285529163528696,
+                    "angularVelocity": 0.9769303792218019,
+                    "velocityX": 2.9628509752411754,
+                    "velocityY": -1.7581184381341082,
+                    "timestamp": 1.2491878631303182
+                },
+                {
+                    "x": 5.148349456504816,
+                    "y": 4.079665503809577,
+                    "heading": -0.6795461936491729,
+                    "angularVelocity": 1.1393832103284065,
+                    "velocityX": 3.0640281935900804,
+                    "velocityY": -1.4170575397591803,
+                    "timestamp": 1.2921994875420657
+                },
+                {
+                    "x": 5.28369745675312,
+                    "y": 4.033673765712218,
+                    "heading": -0.6261187752553977,
+                    "angularVelocity": 1.2421623020399764,
+                    "velocityX": 3.1467772282354725,
+                    "velocityY": -1.0692862389265216,
+                    "timestamp": 1.3352111119538133
+                },
+                {
+                    "x": 5.421794981888129,
+                    "y": 4.0028549700404525,
+                    "heading": -0.5712230670658915,
+                    "angularVelocity": 1.2762993479156466,
+                    "velocityX": 3.2107023862435504,
+                    "velocityY": -0.7165224771968601,
+                    "timestamp": 1.3782227363655608
+                },
+                {
+                    "x": 5.5620425748748135,
+                    "y": 3.9873099729843906,
+                    "heading": -0.5170015662419462,
+                    "angularVelocity": 1.2606243443606422,
+                    "velocityX": 3.260690450658235,
+                    "velocityY": -0.3614138565716755,
+                    "timestamp": 1.4212343607773084
                 },
                 {
                     "x": 5.703982353210449,
                     "y": 3.987083911895752,
-                    "heading": 0.16151347881375258,
-                    "angularVelocity": -0.32215526782320836,
-                    "velocityX": 3.5698675674478513,
-                    "velocityY": -0.005685567203615865,
-                    "timestamp": 1.4192794756416105
-                },
-                {
-                    "x": 5.829773721796989,
-                    "y": 3.97718762682745,
-                    "heading": 0.15213932045666498,
-                    "angularVelocity": -0.2709697131085786,
-                    "velocityX": 3.6361292138465493,
-                    "velocityY": -0.2860623240667741,
-                    "timestamp": 1.453874332201581
-                },
-                {
-                    "x": 5.956799770812804,
-                    "y": 3.9574240312185514,
-                    "heading": 0.1457481223879076,
-                    "angularVelocity": -0.18474417021149397,
-                    "velocityX": 3.671818924747189,
-                    "velocityY": -0.5712871095342833,
-                    "timestamp": 1.4884691887615515
-                },
-                {
-                    "x": 6.083503083107889,
-                    "y": 3.927762559716917,
-                    "heading": 0.14376941436575622,
-                    "angularVelocity": -0.05719659564771788,
-                    "velocityX": 3.6624898870571543,
-                    "velocityY": -0.8573954180216341,
-                    "timestamp": 1.523064045321522
-                },
-                {
-                    "x": 6.208289204844049,
-                    "y": 3.888323576656908,
-                    "heading": 0.14376927335150447,
-                    "angularVelocity": -0.000004076162347950456,
-                    "velocityX": 3.607071517115297,
-                    "velocityY": -1.1400244713152792,
-                    "timestamp": 1.5576589018814926
-                },
-                {
-                    "x": 6.329707486841884,
-                    "y": 3.8394909787467797,
-                    "heading": 0.14376925272536353,
-                    "angularVelocity": -5.962198716671582e-7,
-                    "velocityX": 3.5097206368627396,
-                    "velocityY": -1.4115565944167625,
-                    "timestamp": 1.592253758441463
-                },
-                {
-                    "x": 6.447051924752506,
-                    "y": 3.781548715234953,
-                    "heading": 0.14376923552481305,
-                    "angularVelocity": -4.971996473322837e-7,
-                    "velocityX": 3.3919619729367447,
-                    "velocityY": -1.6748808717094477,
-                    "timestamp": 1.6268486150014336
-                },
-                {
-                    "x": 6.559640243151509,
-                    "y": 3.714833691264027,
-                    "heading": 0.14376922059812475,
-                    "angularVelocity": -4.3147131718664413e-7,
-                    "velocityX": 3.254481434366676,
-                    "velocityY": -1.9284665584687428,
-                    "timestamp": 1.6614434715614042
-                },
-                {
-                    "x": 6.666817827672777,
-                    "y": 3.6397338254007683,
-                    "heading": 0.14376920720482705,
-                    "angularVelocity": -3.8714707984086e-7,
-                    "velocityX": 3.0980785925640646,
-                    "velocityY": -2.1708390590685647,
-                    "timestamp": 1.6960383281213747
-                },
-                {
-                    "x": 6.7679615406661,
-                    "y": 3.5566858017838987,
-                    "heading": 0.14376919483153264,
-                    "angularVelocity": -3.5766283306602865e-7,
-                    "velocityX": 2.923663314457974,
-                    "velocityY": -2.4005887543688678,
-                    "timestamp": 1.7306331846813452
-                },
-                {
-                    "x": 6.862483380166932,
-                    "y": 3.466172559439492,
-                    "heading": 0.1437691830947059,
-                    "angularVelocity": -3.392650779789794e-7,
-                    "velocityX": 2.7322512332715396,
-                    "velocityY": -2.6163785991568234,
-                    "timestamp": 1.7652280412413157
-                },
-                {
-                    "x": 6.949834140776404,
-                    "y": 3.3687206741301803,
-                    "heading": 0.14376917168351727,
-                    "angularVelocity": -3.2985217383486685e-7,
-                    "velocityX": 2.5249638037391158,
-                    "velocityY": -2.8169472285678507,
-                    "timestamp": 1.7998228978012862
-                },
-                {
-                    "x": 7.031656177612435,
-                    "y": 3.2665826319797073,
-                    "heading": 0.14376916034823112,
-                    "angularVelocity": -3.2765813438231676e-7,
-                    "velocityX": 2.3651503423404003,
-                    "velocityY": -2.952405424009072,
-                    "timestamp": 1.8344177543612568
-                },
-                {
-                    "x": 7.113477155727132,
-                    "y": 3.1644437417043827,
-                    "heading": 0.14376914901287846,
-                    "angularVelocity": -3.2766005667768915e-7,
-                    "velocityX": 2.3651197389085663,
-                    "velocityY": -2.952429939932413,
-                    "timestamp": 1.8690126109212273
-                },
-                {
-                    "x": 7.194171820782979,
-                    "y": 3.0635723954548135,
-                    "heading": 0.13949339206360356,
-                    "angularVelocity": -0.12359516339843184,
-                    "velocityX": 2.332562498588806,
-                    "velocityY": -2.91579027288948,
-                    "timestamp": 1.9036074674811978
-                },
-                {
-                    "x": 7.268927567281138,
-                    "y": 2.9702304150182344,
-                    "heading": 0.12535509322542532,
-                    "angularVelocity": -0.4086821060717307,
-                    "velocityX": 2.160891933995128,
-                    "velocityY": -2.6981461904537576,
-                    "timestamp": 1.9382023240411683
-                },
-                {
-                    "x": 7.337464553197734,
-                    "y": 2.884657135542947,
-                    "heading": 0.10855528307456563,
-                    "angularVelocity": -0.48561583487814164,
-                    "velocityX": 1.9811322471531543,
-                    "velocityY": -2.473583878775318,
-                    "timestamp": 1.9727971806011388
-                },
-                {
-                    "x": 7.399764936518732,
-                    "y": 2.806872483431395,
-                    "heading": 0.0911966728700688,
-                    "angularVelocity": -0.5017685266133554,
-                    "velocityX": 1.8008568184984473,
-                    "velocityY": -2.2484455738878646,
-                    "timestamp": 2.0073920371611096
-                },
-                {
-                    "x": 7.455827979936063,
-                    "y": 2.7368764943083312,
-                    "heading": 0.07427285825902008,
-                    "angularVelocity": -0.4892003116622579,
-                    "velocityX": 1.6205600771936914,
-                    "velocityY": -2.023306239230242,
-                    "timestamp": 2.0419868937210803
-                },
-                {
-                    "x": 7.505655491239628,
-                    "y": 2.674666404415114,
-                    "heading": 0.05836114831277334,
-                    "angularVelocity": -0.4599443827339935,
-                    "velocityX": 1.4403155919200792,
-                    "velocityY": -1.7982467938658737,
-                    "timestamp": 2.076581750281051
-                },
-                {
-                    "x": 7.549249543273319,
-                    "y": 2.6202393080696202,
-                    "heading": 0.04383838388032337,
-                    "angularVelocity": -0.4197954805008245,
-                    "velocityX": 1.2601310243365538,
-                    "velocityY": -1.5732713402393688,
-                    "timestamp": 2.111176606841022
-                },
-                {
-                    "x": 7.586612019746033,
-                    "y": 2.5735926396425732,
-                    "heading": 0.030969705386613037,
-                    "angularVelocity": -0.3719824208954968,
-                    "velocityX": 1.0800009073009074,
-                    "velocityY": -1.348370048772548,
-                    "timestamp": 2.1457714634009926
-                },
-                {
-                    "x": 7.6177445449139505,
-                    "y": 2.5347242182058114,
-                    "heading": 0.01995171749042388,
-                    "angularVelocity": -0.31848630090688007,
-                    "velocityX": 0.8999177410650449,
-                    "velocityY": -1.1235317992830327,
-                    "timestamp": 2.1803663199609633
-                },
-                {
-                    "x": 7.642648503297853,
-                    "y": 2.503632200129417,
-                    "heading": 0.010935986949086615,
-                    "angularVelocity": -0.26060898751548167,
-                    "velocityX": 0.7198745958298824,
-                    "velocityY": -0.8987468418172337,
-                    "timestamp": 2.214961176520934
-                },
-                {
-                    "x": 7.661325077102299,
-                    "y": 2.4803150199755253,
-                    "heading": 0.004042922192056018,
-                    "angularVelocity": -0.19925114431624774,
-                    "velocityX": 0.5398656234365572,
-                    "velocityY": -0.6740071349471043,
-                    "timestamp": 2.249556033080905
-                },
-                {
-                    "x": 7.673775282436928,
-                    "y": 2.464771338402636,
-                    "heading": -0.0006295042844945549,
-                    "angularVelocity": -0.13506130509461317,
-                    "velocityX": 0.3598860227400032,
-                    "velocityY": -0.44930614312403183,
-                    "timestamp": 2.2841508896408755
+                    "heading": -0.46507695218435896,
+                    "angularVelocity": 1.207222809362328,
+                    "velocityX": 3.3000329626441363,
+                    "velocityY": -0.005255813788261737,
+                    "timestamp": 1.464245985189056
+                },
+                {
+                    "x": 5.957415351644791,
+                    "y": 3.94389686592464,
+                    "heading": -0.40627705964587574,
+                    "angularVelocity": 0.8004851872173009,
+                    "velocityX": 3.450165509502976,
+                    "velocityY": -0.5879362884366123,
+                    "timestamp": 1.5377013013865826
+                },
+                {
+                    "x": 6.214659603999478,
+                    "y": 3.857155103461619,
+                    "heading": -0.3855287596647175,
+                    "angularVelocity": 0.2824615161326723,
+                    "velocityX": 3.502050847660077,
+                    "velocityY": -1.180877939858924,
+                    "timestamp": 1.6111566175841092
+                },
+                {
+                    "x": 6.460339332530576,
+                    "y": 3.727320070546642,
+                    "heading": -0.38552868222223635,
+                    "angularVelocity": 0.0000010542801416145007,
+                    "velocityX": 3.3446146752734305,
+                    "velocityY": -1.7675375947717866,
+                    "timestamp": 1.6846119337816359
+                },
+                {
+                    "x": 6.681847040612612,
+                    "y": 3.5595404285567858,
+                    "heading": -0.3855286464284339,
+                    "angularVelocity": 4.872867523834452e-7,
+                    "velocityX": 3.0155435923301614,
+                    "velocityY": -2.2841048228379357,
+                    "timestamp": 1.7580672499791625
+                },
+                {
+                    "x": 6.873376982747171,
+                    "y": 3.358214932798401,
+                    "heading": -0.3855286139444744,
+                    "angularVelocity": 4.422274806376815e-7,
+                    "velocityX": 2.607434724254966,
+                    "velocityY": -2.740788634235891,
+                    "timestamp": 1.8315225661766892
+                },
+                {
+                    "x": 7.058369006072979,
+                    "y": 3.1508658904491242,
+                    "heading": -0.3855285817822777,
+                    "angularVelocity": 4.3784709334515964e-7,
+                    "velocityX": 2.518429337753462,
+                    "velocityY": -2.8227915021385286,
+                    "timestamp": 1.9049778823742158
+                },
+                {
+                    "x": 7.235640342207717,
+                    "y": 2.9543486280986375,
+                    "heading": -0.3517392740895451,
+                    "angularVelocity": 0.4599981245995971,
+                    "velocityX": 2.413322075396734,
+                    "velocityY": -2.6753306979448217,
+                    "timestamp": 1.9784331985717425
+                },
+                {
+                    "x": 7.3842205894533075,
+                    "y": 2.7881464841241645,
+                    "heading": -0.2579536451103976,
+                    "angularVelocity": 1.2767711560447332,
+                    "velocityX": 2.0227296666458727,
+                    "velocityY": -2.2626292088586646,
+                    "timestamp": 2.051888514769269
+                },
+                {
+                    "x": 7.502781206912582,
+                    "y": 2.6554541566341396,
+                    "heading": -0.1644602811973769,
+                    "angularVelocity": 1.2727923416953275,
+                    "velocityX": 1.6140508760517314,
+                    "velocityY": -1.806435998903144,
+                    "timestamp": 2.125343830966796
+                },
+                {
+                    "x": 7.591494122264987,
+                    "y": 2.5561305844890163,
+                    "heading": -0.0867965293639426,
+                    "angularVelocity": 1.0572924582421084,
+                    "velocityX": 1.2077126604948574,
+                    "velocityY": -1.3521631555983635,
+                    "timestamp": 2.1987991471643227
+                },
+                {
+                    "x": 7.6505265885867635,
+                    "y": 2.4900164938031613,
+                    "heading": -0.031723308715129774,
+                    "angularVelocity": 0.7497513250193758,
+                    "velocityX": 0.8036513812429008,
+                    "velocityY": -0.900058622143418,
+                    "timestamp": 2.2722544633618496
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
                     "heading": -0.003,
-                    "angularVelocity": -0.06852162290068065,
-                    "velocityX": 0.17993187953478723,
-                    "velocityY": -0.22463854964001964,
-                    "timestamp": 2.3187457462008463
+                    "angularVelocity": 0.39103104039319253,
+                    "velocityX": 0.401242727401519,
+                    "velocityY": -0.4494772538229548,
+                    "timestamp": 2.3457097795593764
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
                     "heading": -0.003,
-                    "angularVelocity": -8.413636330536319e-34,
-                    "velocityX": 0,
-                    "velocityY": 4.110332962762654e-35,
-                    "timestamp": 2.353340602760817
+                    "angularVelocity": -1.5819357203294863e-32,
+                    "velocityX": 5.378736393413154e-34,
+                    "velocityY": 0,
+                    "timestamp": 2.4191650957569033
                 }
             ],
             "constraints": [
@@ -11755,7 +12606,620 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 0.39,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 4.884595084783414e-34,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.298507420370097,
+                    "y": 6.738728491162364,
+                    "heading": 0.38429180377773414,
+                    "angularVelocity": -0.12150700609071861,
+                    "velocityX": -0.031771662206367984,
+                    "velocityY": -0.3889348314553929,
+                    "timestamp": 0.04697832994094238
+                },
+                {
+                    "x": 2.295523042319471,
+                    "y": 6.702182630298916,
+                    "heading": 0.3729896107436911,
+                    "angularVelocity": -0.2405831166891495,
+                    "velocityX": -0.06352669527371438,
+                    "velocityY": -0.7779301841804652,
+                    "timestamp": 0.09395665988188476
+                },
+                {
+                    "x": 2.2910478858134478,
+                    "y": 6.647358841243747,
+                    "heading": 0.35624034482133005,
+                    "angularVelocity": -0.3565317443897418,
+                    "velocityX": -0.09526001694076108,
+                    "velocityY": -1.1670016606398963,
+                    "timestamp": 0.14093498982282715
+                },
+                {
+                    "x": 2.285083332518943,
+                    "y": 6.574252474793301,
+                    "heading": 0.33424119989038353,
+                    "angularVelocity": -0.46828282228427764,
+                    "velocityX": -0.12696392787914923,
+                    "velocityY": -1.556172101953168,
+                    "timestamp": 0.18791331976376952
+                },
+                {
+                    "x": 2.2776313636635503,
+                    "y": 6.48285724279221,
+                    "heading": 0.3072714314404645,
+                    "angularVelocity": -0.5740895532860236,
+                    "velocityX": -0.15862566559434335,
+                    "velocityY": -1.9454763955207768,
+                    "timestamp": 0.23489164970471188
+                },
+                {
+                    "x": 2.268695072443392,
+                    "y": 6.373164221156064,
+                    "heading": 0.2757564747192681,
+                    "angularVelocity": -0.6708402951065854,
+                    "velocityX": -0.1902215602681591,
+                    "velocityY": -2.334970650809517,
+                    "timestamp": 0.28186997964565425
+                },
+                {
+                    "x": 2.2582799597491072,
+                    "y": 6.245160021300657,
+                    "heading": 0.24041955506127732,
+                    "angularVelocity": -0.752196165815466,
+                    "velocityX": -0.22170036072754723,
+                    "velocityY": -2.7247499009931837,
+                    "timestamp": 0.3288483095865966
+                },
+                {
+                    "x": 2.2463982479830333,
+                    "y": 6.098824071496616,
+                    "heading": 0.20274918029785605,
+                    "angularVelocity": -0.8018670482917891,
+                    "velocityX": -0.25291899011757896,
+                    "velocityY": -3.114967049446051,
+                    "timestamp": 0.375826639527539
+                },
+                {
+                    "x": 2.2330953714575887,
+                    "y": 5.934154199517091,
+                    "heading": 0.1675471140087416,
+                    "angularVelocity": -0.7493256216934009,
+                    "velocityX": -0.28317048609790474,
+                    "velocityY": -3.505230436810659,
+                    "timestamp": 0.42280496946848134
+                },
+                {
+                    "x": 2.218530566609672,
+                    "y": 5.757035827571947,
+                    "heading": 0.16754710135001605,
+                    "angularVelocity": -2.6945882371217095e-7,
+                    "velocityX": -0.3100324099691601,
+                    "velocityY": -3.770214313020578,
+                    "timestamp": 0.4697832994094237
+                },
+                {
+                    "x": 2.203965673260931,
+                    "y": 5.579917462904045,
+                    "heading": 0.16754708869211127,
+                    "angularVelocity": -2.694413527531555e-7,
+                    "velocityX": -0.3100342938339967,
+                    "velocityY": -3.7702141581142206,
+                    "timestamp": 0.5167616293503661
+                },
+                {
+                    "x": 2.1894007799094983,
+                    "y": 5.402799098236364,
+                    "heading": 0.16754707603420654,
+                    "angularVelocity": -2.694413521606316e-7,
+                    "velocityX": -0.31003429389130516,
+                    "velocityY": -3.7702141581095083,
+                    "timestamp": 0.5637399592913085
+                },
+                {
+                    "x": 2.1748358865580655,
+                    "y": 5.225680733568683,
+                    "heading": 0.1675470633763018,
+                    "angularVelocity": -2.6944135250249435e-7,
+                    "velocityX": -0.3100342938913069,
+                    "velocityY": -3.770214158109508,
+                    "timestamp": 0.6107182892322509
+                },
+                {
+                    "x": 2.160270993206632,
+                    "y": 5.048562368901002,
+                    "heading": 0.16754705071839698,
+                    "angularVelocity": -2.694413536359467e-7,
+                    "velocityX": -0.31003429389130693,
+                    "velocityY": -3.7702141581095083,
+                    "timestamp": 0.6576966191731933
+                },
+                {
+                    "x": 2.1457060998551993,
+                    "y": 4.871444004233322,
+                    "heading": 0.1675470380604922,
+                    "angularVelocity": -2.6944135247689087e-7,
+                    "velocityX": -0.3100342938913068,
+                    "velocityY": -3.7702141581095083,
+                    "timestamp": 0.7046749491141358
+                },
+                {
+                    "x": 2.131141206503766,
+                    "y": 4.6943256395656405,
+                    "heading": 0.16754702540258745,
+                    "angularVelocity": -2.694413528693067e-7,
+                    "velocityX": -0.31003429389130693,
+                    "velocityY": -3.7702141581095083,
+                    "timestamp": 0.7516532790550782
+                },
+                {
+                    "x": 2.1165763131523327,
+                    "y": 4.51720727489796,
+                    "heading": 0.16754701274468267,
+                    "angularVelocity": -2.694413529636153e-7,
+                    "velocityX": -0.31003429389130693,
+                    "velocityY": -3.7702141581095083,
+                    "timestamp": 0.7986316089960206
+                },
+                {
+                    "x": 2.1020114198009,
+                    "y": 4.340088910230279,
+                    "heading": 0.16754700008677786,
+                    "angularVelocity": -2.694413530501801e-7,
+                    "velocityX": -0.3100342938913069,
+                    "velocityY": -3.770214158109508,
+                    "timestamp": 0.845609938936963
+                },
+                {
+                    "x": 2.0874465264494666,
+                    "y": 4.162970545562599,
+                    "heading": 0.16754698742887306,
+                    "angularVelocity": -2.694413534833812e-7,
+                    "velocityX": -0.31003429389130704,
+                    "velocityY": -3.7702141581095083,
+                    "timestamp": 0.8925882688779054
+                },
+                {
+                    "x": 2.0728816330980333,
+                    "y": 3.9858521808949186,
+                    "heading": 0.16754697477096828,
+                    "angularVelocity": -2.6944135303696276e-7,
+                    "velocityX": -0.31003429389130693,
+                    "velocityY": -3.770214158109508,
+                    "timestamp": 0.9395665988188479
+                },
+                {
+                    "x": 2.0583167397466005,
+                    "y": 3.8087338162272375,
+                    "heading": 0.16754696211306347,
+                    "angularVelocity": -2.694413537071192e-7,
+                    "velocityX": -0.31003429389130704,
+                    "velocityY": -3.7702141581095083,
+                    "timestamp": 0.9865449287597903
+                },
+                {
+                    "x": 2.043751846395167,
+                    "y": 3.631615451559557,
+                    "heading": 0.1675469494551587,
+                    "angularVelocity": -2.694413527332354e-7,
+                    "velocityX": -0.31003429389130716,
+                    "velocityY": -3.770214158109508,
+                    "timestamp": 1.0335232587007326
+                },
+                {
+                    "x": 2.0291869530437365,
+                    "y": 3.4544970868918763,
+                    "heading": 0.16754693679725388,
+                    "angularVelocity": -2.694413532802898e-7,
+                    "velocityX": -0.31003429389125564,
+                    "velocityY": -3.7702141581095123,
+                    "timestamp": 1.080501588641675
+                },
+                {
+                    "x": 2.0146220597715567,
+                    "y": 3.2773787222176782,
+                    "heading": 0.1675469241393491,
+                    "angularVelocity": -2.6944135307750017e-7,
+                    "velocityX": -0.31003429220428774,
+                    "velocityY": -3.7702141582482356,
+                    "timestamp": 1.1274799185826172
+                },
+                {
+                    "x": 2.0000597716588953,
+                    "y": 3.1002601433334096,
+                    "heading": 0.16754691148142445,
+                    "angularVelocity": -2.6944177624221033e-7,
+                    "velocityX": -0.30997883771037654,
+                    "velocityY": -3.770218718011654,
+                    "timestamp": 1.1744582485235595
+                },
+                {
+                    "x": 2.001608158601577,
+                    "y": 2.922550679334096,
+                    "heading": 0.16754689857982888,
+                    "angularVelocity": -2.7462865480192305e-7,
+                    "velocityX": 0.032959599556405594,
+                    "velocityY": -3.7827965409310345,
+                    "timestamp": 1.2214365784645018
+                },
+                {
+                    "x": 2.021524410839774,
+                    "y": 2.7459539818451146,
+                    "heading": 0.1675468848578902,
+                    "angularVelocity": -2.9209081516381327e-7,
+                    "velocityX": 0.4239455140962882,
+                    "velocityY": -3.7591097365739743,
+                    "timestamp": 1.2684149084054441
+                },
+                {
+                    "x": 2.0595952756969864,
+                    "y": 2.572363489269638,
+                    "heading": 0.16754686955661854,
+                    "angularVelocity": -3.257091448717022e-7,
+                    "velocityX": 0.8103920446953395,
+                    "velocityY": -3.6951184257444134,
+                    "timestamp": 1.3153932383463864
+                },
+                {
+                    "x": 2.1154126290237643,
+                    "y": 2.4036404273247642,
+                    "heading": 0.16754685154625998,
+                    "angularVelocity": -3.8337587986161977e-7,
+                    "velocityX": 1.1881510772508834,
+                    "velocityY": -3.5915083008906312,
+                    "timestamp": 1.3623715682873287
+                },
+                {
+                    "x": 2.1883780268761943,
+                    "y": 2.2415938375833533,
+                    "heading": 0.1675468289010436,
+                    "angularVelocity": -4.82035363826859e-7,
+                    "velocityX": 1.5531713865554018,
+                    "velocityY": -3.449390175111029,
+                    "timestamp": 1.409349898228271
+                },
+                {
+                    "x": 2.2777091497247044,
+                    "y": 2.0879611790897656,
+                    "heading": 0.16754679784052873,
+                    "angularVelocity": -6.611668595012802e-7,
+                    "velocityX": 1.9015389214731706,
+                    "velocityY": -3.270287783467881,
+                    "timestamp": 1.4563282281692134
+                },
+                {
+                    "x": 2.382448196411133,
+                    "y": 1.9443897008895872,
+                    "heading": 0.16754674836915184,
+                    "angularVelocity": -0.0000010530680204285256,
+                    "velocityX": 2.2295183080815923,
+                    "velocityY": -3.0561213729961265,
+                    "timestamp": 1.5033065581101557
+                },
+                {
+                    "x": 2.5069799221636964,
+                    "y": 1.8074962230648766,
+                    "heading": 0.1675467031876573,
+                    "angularVelocity": -9.235757728412815e-7,
+                    "velocityX": 2.545610454081449,
+                    "velocityY": -2.798302730812182,
+                    "timestamp": 1.55222673973217
+                },
+                {
+                    "x": 2.645527049823341,
+                    "y": 1.6848068830394283,
+                    "heading": 0.16754667440242366,
+                    "angularVelocity": -5.884122397185895e-7,
+                    "velocityX": 2.8321057499364835,
+                    "velocityY": -2.5079493975189484,
+                    "timestamp": 1.6011469213541845
+                },
+                {
+                    "x": 2.7964787325047507,
+                    "y": 1.577748144471377,
+                    "heading": 0.1675466534117406,
+                    "angularVelocity": -4.2908023560369677e-7,
+                    "velocityX": 3.0856729815058617,
+                    "velocityY": -2.1884370625450487,
+                    "timestamp": 1.6500671029761989
+                },
+                {
+                    "x": 2.958079900807193,
+                    "y": 1.4875647290195357,
+                    "heading": 0.1675466366326211,
+                    "angularVelocity": -3.429897224036844e-7,
+                    "velocityX": 3.303364029820377,
+                    "velocityY": -1.8434807979383812,
+                    "timestamp": 1.6989872845982132
+                },
+                {
+                    "x": 3.1284516660647324,
+                    "y": 1.4153051353233332,
+                    "heading": 0.16754662225394246,
+                    "angularVelocity": -2.9392120351406106e-7,
+                    "velocityX": 3.4826478481607013,
+                    "velocityY": -1.4770916889581902,
+                    "timestamp": 1.7479074662202276
+                },
+                {
+                    "x": 3.3056131601804433,
+                    "y": 1.3618094062641966,
+                    "heading": 0.16754660920712522,
+                    "angularVelocity": -2.666960100615425e-7,
+                    "velocityX": 3.6214398279336546,
+                    "velocityY": -1.0935308759169382,
+                    "timestamp": 1.796827647842242
+                },
+                {
+                    "x": 3.487504529935422,
+                    "y": 1.3276990268102373,
+                    "heading": 0.16754659675699374,
+                    "angularVelocity": -2.544988811687778e-7,
+                    "velocityX": 3.7181253978240782,
+                    "velocityY": -0.6972660019440657,
+                    "timestamp": 1.8457478294642564
+                },
+                {
+                    "x": 3.671043389503124,
+                    "y": 1.304003553111977,
+                    "heading": 0.16754658441216208,
+                    "angularVelocity": -2.5234639867690264e-7,
+                    "velocityX": 3.751802497092686,
+                    "velocityY": -0.48437010887133,
+                    "timestamp": 1.8946680110862708
+                },
+                {
+                    "x": 3.8545823032662914,
+                    "y": 1.2803084992011935,
+                    "heading": 0.16754657206733162,
+                    "angularVelocity": -2.523463739841797e-7,
+                    "velocityX": 3.751803604927206,
+                    "velocityY": -0.4843615278018566,
+                    "timestamp": 1.9435881927082852
+                },
+                {
+                    "x": 4.038121217030901,
+                    "y": 1.2566134453015836,
+                    "heading": 0.16754655972250113,
+                    "angularVelocity": -2.5234637491728997e-7,
+                    "velocityX": 3.7518036049566934,
+                    "velocityY": -0.48436152757345374,
+                    "timestamp": 1.9925083743302996
+                },
+                {
+                    "x": 4.221660130795511,
+                    "y": 1.232918391401974,
+                    "heading": 0.16754654737767066,
+                    "angularVelocity": -2.5234637402611237e-7,
+                    "velocityX": 3.7518036049566943,
+                    "velocityY": -0.4843615275734478,
+                    "timestamp": 2.041428555952314
+                },
+                {
+                    "x": 4.4051990445601215,
+                    "y": 1.2092233375023644,
+                    "heading": 0.16754653503284023,
+                    "angularVelocity": -2.5234637391390173e-7,
+                    "velocityX": 3.751803604956694,
+                    "velocityY": -0.4843615275734475,
+                    "timestamp": 2.090348737574329
+                },
+                {
+                    "x": 4.588737958324731,
+                    "y": 1.1855282836027548,
+                    "heading": 0.16754652268800974,
+                    "angularVelocity": -2.523463747404588e-7,
+                    "velocityX": 3.751803604956694,
+                    "velocityY": -0.4843615275734478,
+                    "timestamp": 2.1392689191963434
+                },
+                {
+                    "x": 4.77227687208934,
+                    "y": 1.1618332297031453,
+                    "heading": 0.16754651034317924,
+                    "angularVelocity": -2.523463747849071e-7,
+                    "velocityX": 3.751803604956694,
+                    "velocityY": -0.48436152757344797,
+                    "timestamp": 2.188189100818358
+                },
+                {
+                    "x": 4.95581578585395,
+                    "y": 1.1381381758035356,
+                    "heading": 0.16754649799834875,
+                    "angularVelocity": -2.52346374599026e-7,
+                    "velocityX": 3.751803604956694,
+                    "velocityY": -0.4843615275734478,
+                    "timestamp": 2.2371092824403727
+                },
+                {
+                    "x": 5.1393546996185595,
+                    "y": 1.114443121903926,
+                    "heading": 0.1675464856535183,
+                    "angularVelocity": -2.523463743979587e-7,
+                    "velocityX": 3.751803604956694,
+                    "velocityY": -0.48436152757344764,
+                    "timestamp": 2.2860294640623873
+                },
+                {
+                    "x": 5.32289361338317,
+                    "y": 1.0907480680043165,
+                    "heading": 0.16754647330868783,
+                    "angularVelocity": -2.523463747332428e-7,
+                    "velocityX": 3.7518036049566943,
+                    "velocityY": -0.48436152757344797,
+                    "timestamp": 2.334949645684402
+                },
+                {
+                    "x": 5.506432527147779,
+                    "y": 1.0670530141047068,
+                    "heading": 0.1675464609638573,
+                    "angularVelocity": -2.5234637523589347e-7,
+                    "velocityX": 3.7518036049566943,
+                    "velocityY": -0.48436152757344764,
+                    "timestamp": 2.3838698273064165
+                },
+                {
+                    "x": 5.689971440912389,
+                    "y": 1.043357960205097,
+                    "heading": 0.16754644861902682,
+                    "angularVelocity": -2.523463745889681e-7,
+                    "velocityX": 3.751803604956694,
+                    "velocityY": -0.4843615275734477,
+                    "timestamp": 2.432790008928431
+                },
+                {
+                    "x": 5.8735103546769984,
+                    "y": 1.0196629063054876,
+                    "heading": 0.16754643627419633,
+                    "angularVelocity": -2.5234637515727394e-7,
+                    "velocityX": 3.751803604956694,
+                    "velocityY": -0.4843615275734478,
+                    "timestamp": 2.4817101905504457
+                },
+                {
+                    "x": 6.057049268441609,
+                    "y": 0.995967852405878,
+                    "heading": 0.16754642392936583,
+                    "angularVelocity": -2.523463750187176e-7,
+                    "velocityX": 3.751803604956694,
+                    "velocityY": -0.4843615275734476,
+                    "timestamp": 2.5306303721724603
+                },
+                {
+                    "x": 6.240588182206218,
+                    "y": 0.9722727985062686,
+                    "heading": 0.16754641158453532,
+                    "angularVelocity": -2.523463753175626e-7,
+                    "velocityX": 3.751803604956694,
+                    "velocityY": -0.48436152757344764,
+                    "timestamp": 2.579550553794475
+                },
+                {
+                    "x": 6.424127095970827,
+                    "y": 0.948577744606659,
+                    "heading": 0.1675463992397048,
+                    "angularVelocity": -2.5234637555305e-7,
+                    "velocityX": 3.7518036049566934,
+                    "velocityY": -0.4843615275734476,
+                    "timestamp": 2.6284707354164896
+                },
+                {
+                    "x": 6.607666009735459,
+                    "y": 0.9248826907072107,
+                    "heading": 0.1675463868948743,
+                    "angularVelocity": -2.5234637524939476e-7,
+                    "velocityX": 3.7518036049571197,
+                    "velocityY": -0.48436152757015266,
+                    "timestamp": 2.677390917038504
+                },
+                {
+                    "x": 6.79120492428193,
+                    "y": 0.9011876428641385,
+                    "heading": 0.16754637454996243,
+                    "angularVelocity": -2.523480385625403e-7,
+                    "velocityX": 3.7518036209390857,
+                    "velocityY": -0.4843614037689742,
+                    "timestamp": 2.726311098660519
+                },
+                {
+                    "x": 6.9688913787459,
+                    "y": 0.8784350451744124,
+                    "heading": 0.1510406339197301,
+                    "angularVelocity": -0.33740145851798203,
+                    "velocityX": 3.6321707845828914,
+                    "velocityY": -0.4650963454209109,
+                    "timestamp": 2.7752312802825334
+                },
+                {
+                    "x": 7.1269915101358645,
+                    "y": 0.8581344466937117,
+                    "heading": 0.12402265057850995,
+                    "angularVelocity": -0.5522870611964742,
+                    "velocityX": 3.2317977192222487,
+                    "velocityY": -0.4149738984526803,
+                    "timestamp": 2.824151461904548
+                },
+                {
+                    "x": 7.265299167873697,
+                    "y": 0.8403581253912503,
+                    "heading": 0.0960275261585182,
+                    "angularVelocity": -0.5722612527545016,
+                    "velocityX": 2.8272106347943935,
+                    "velocityY": -0.3633739841730647,
+                    "timestamp": 2.8730716435265626
+                },
+                {
+                    "x": 7.383819928892336,
+                    "y": 0.8251162922066029,
+                    "heading": 0.06979646830524168,
+                    "angularVelocity": -0.5362011542792869,
+                    "velocityX": 2.4227375510254454,
+                    "velocityY": -0.3115653433671744,
+                    "timestamp": 2.9219918251485772
+                },
+                {
+                    "x": 7.482566860925546,
+                    "y": 0.8124124441531555,
+                    "heading": 0.04664460944238867,
+                    "angularVelocity": -0.47325782724474763,
+                    "velocityX": 2.01853159083065,
+                    "velocityY": -0.2596852184974389,
+                    "timestamp": 2.970912006770592
+                },
+                {
+                    "x": 7.5615507925764724,
+                    "y": 0.8022481812485672,
+                    "heading": 0.027346506217350703,
+                    "angularVelocity": -0.3944814304686405,
+                    "velocityX": 1.6145469831081642,
+                    "velocityY": -0.20777238692863387,
+                    "timestamp": 3.0198321883926065
+                },
+                {
+                    "x": 7.620780133649161,
+                    "y": 0.7946243691967044,
+                    "heading": 0.012413272398069258,
+                    "angularVelocity": -0.3052571213791523,
+                    "velocityX": 1.2107342840696753,
+                    "velocityY": -0.15584185910773468,
+                    "timestamp": 3.068752370014621
+                },
+                {
+                    "x": 7.660261458521305,
+                    "y": 0.7895415301241143,
+                    "heading": 0.002207571288741933,
+                    "angularVelocity": -0.2086194443876447,
+                    "velocityX": 0.8070559749184711,
+                    "velocityY": -0.10390065825721975,
+                    "timestamp": 3.1176725516366357
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": -0.003,
+                    "angularVelocity": -0.10645036702804238,
+                    "velocityX": 0.40348463199107626,
+                    "velocityY": -0.05195258970523994,
+                    "timestamp": 3.1665927332586503
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": -0.003,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": -1.0902826040652685e-32,
+                    "timestamp": 3.215512914880665
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -11773,13 +13237,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 4.816418571472168,
-                    "y": 4.104055919647217,
-                    "radius": 1.7841171466850674
-                }
-            ]
+            "circleObstacles": []
         },
         "W2 To W1": {
             "waypoints": [
@@ -11806,101 +13264,101 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": -1.6570559277738772e-34,
-                    "velocityY": -1.2609465104041817e-35,
+                    "heading": -5.451838641793523e-34,
+                    "angularVelocity": -2.0131697388730357e-33,
+                    "velocityX": -3.0924959567962797e-34,
+                    "velocityY": 4.789643826321005e-35,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.25200199902585,
-                    "y": 5.607878029194235,
-                    "heading": 0.015821088838010276,
-                    "angularVelocity": 0.20825462300812525,
-                    "velocityX": 0.026352519517453685,
-                    "velocityY": 0.6302234329322915,
-                    "timestamp": 0.0759699285878182
+                    "x": 2.2520020156777094,
+                    "y": 5.6078780285533805,
+                    "heading": 0.01582108665485387,
+                    "angularVelocity": 0.208254594271014,
+                    "velocityX": 0.026352738707599337,
+                    "velocityY": 0.630223424496655,
+                    "timestamp": 0.07596992858781765
                 },
                 {
-                    "x": 2.2560055011308777,
-                    "y": 5.703634196198671,
-                    "heading": 0.04745824894413579,
-                    "angularVelocity": 0.4164431992265759,
-                    "velocityX": 0.05269851083774254,
-                    "velocityY": 1.2604482955877103,
-                    "timestamp": 0.1519398571756364
+                    "x": 2.2560055471462697,
+                    "y": 5.703634194455967,
+                    "heading": 0.04745824180691435,
+                    "angularVelocity": 0.4164431340157103,
+                    "velocityX": 0.05269889735295001,
+                    "velocityY": 1.2604482810839739,
+                    "timestamp": 0.1519398571756353
                 },
                 {
-                    "x": 2.2620098071430843,
-                    "y": 5.847268765721161,
-                    "heading": 0.09489872499810467,
-                    "angularVelocity": 0.6244638758496344,
-                    "velocityX": 0.07903529888495084,
-                    "velocityY": 1.8906766426199095,
-                    "timestamp": 0.2279097857634546
+                    "x": 2.262009891384212,
+                    "y": 5.847268762594484,
+                    "heading": 0.09489870943903907,
+                    "angularVelocity": 0.6244637649920359,
+                    "velocityX": 0.07903580205425154,
+                    "velocityY": 1.8906766244025437,
+                    "timestamp": 0.22790978576345294
                 },
                 {
-                    "x": 2.270014094994434,
-                    "y": 6.038782268539179,
-                    "heading": 0.1581161373911332,
-                    "angularVelocity": 0.8321373149634034,
-                    "velocityX": 0.10536126596587263,
-                    "velocityY": 2.5209119763306935,
-                    "timestamp": 0.3038797143512728
+                    "x": 2.2700142224347877,
+                    "y": 6.038782263911744,
+                    "heading": 0.1581161098401003,
+                    "angularVelocity": 0.8321371571119077,
+                    "velocityX": 0.1053618346017344,
+                    "velocityY": 2.5209119565760743,
+                    "timestamp": 0.3038797143512706
                 },
                 {
-                    "x": 2.28000939681014,
-                    "y": 6.278187229695624,
-                    "heading": 0.23645967758045489,
-                    "angularVelocity": 1.031244094151802,
-                    "velocityX": 0.13156918798668546,
-                    "velocityY": 3.1513121784720814,
-                    "timestamp": 0.379849642939091
+                    "x": 2.280009491675263,
+                    "y": 6.278187226323642,
+                    "heading": 0.23645965423367718,
+                    "angularVelocity": 1.0312441494928544,
+                    "velocityX": 0.13156875919557368,
+                    "velocityY": 3.151312194997739,
+                    "timestamp": 0.37984964293908824
                 },
                 {
-                    "x": 2.2880055947499205,
-                    "y": 6.469712010307998,
-                    "heading": 0.29909347070783976,
-                    "angularVelocity": 0.824455074417803,
-                    "velocityX": 0.10525477762609038,
-                    "velocityY": 2.521060427100167,
-                    "timestamp": 0.4558195715269092
+                    "x": 2.288005657404111,
+                    "y": 6.469712008115255,
+                    "heading": 0.2990934539478391,
+                    "angularVelocity": 0.8244551611202343,
+                    "velocityX": 0.10525435363026732,
+                    "velocityY": 2.52106044262263,
+                    "timestamp": 0.4558195715269059
                 },
                 {
-                    "x": 2.29400278172627,
-                    "y": 6.613355953306298,
-                    "heading": 0.3460499566805711,
-                    "angularVelocity": 0.6180930645268612,
-                    "velocityX": 0.07894159028222533,
-                    "velocityY": 1.8908000266480898,
-                    "timestamp": 0.5317895001147275
+                    "x": 2.294002815862431,
+                    "y": 6.613355952122642,
+                    "heading": 0.3460499471183657,
+                    "angularVelocity": 0.6180931592721867,
+                    "velocityX": 0.07894121489645621,
+                    "velocityY": 1.8908000399308182,
+                    "timestamp": 0.5317895001147235
                 },
                 {
-                    "x": 2.298000929157321,
-                    "y": 6.709118669792275,
-                    "heading": 0.37734923040208523,
-                    "angularVelocity": 0.4119955659209739,
-                    "velocityX": 0.052628026712297535,
-                    "velocityY": 1.2605345070882814,
-                    "timestamp": 0.6077594287025456
+                    "x": 2.2980009414583966,
+                    "y": 6.709118669367686,
+                    "heading": 0.3773492268804029,
+                    "angularVelocity": 0.4119956454330042,
+                    "velocityX": 0.05262773929482027,
+                    "velocityY": 1.260534517079955,
+                    "timestamp": 0.6077594287025412
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.393,
-                    "angularVelocity": 0.20601269329644184,
-                    "velocityX": 0.026313975540574065,
-                    "velocityY": 0.6302668845130636,
-                    "timestamp": 0.6837293572903638
+                    "angularVelocity": 0.20601273965271094,
+                    "velocityX": 0.026313813620245378,
+                    "velocityY": 0.6302668901019914,
+                    "timestamp": 0.6837293572903589
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.393,
-                    "angularVelocity": 1.653751635278537e-33,
-                    "velocityX": 2.0986524182646467e-33,
-                    "velocityY": -2.349121999954591e-34,
-                    "timestamp": 0.7596992858781819
+                    "angularVelocity": 9.82651204832558e-34,
+                    "velocityX": 0,
+                    "velocityY": 3.8258931367738186e-36,
+                    "timestamp": 0.7596992858781766
                 }
             ],
             "constraints": [
@@ -11920,26 +13378,10 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "W2 To W3": {
-            "waypoints": [
+            "circleObstacles": []
+        },
+        "W2 To W3": {
+            "waypoints": [
                 {
                     "x": 2.25,
                     "y": 5.56,
@@ -11963,101 +13405,101 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": -6.311192632400173e-33,
-                    "angularVelocity": 0,
+                    "heading": 0,
+                    "angularVelocity": 5.7052362935559275e-34,
                     "velocityX": 0,
-                    "velocityY": 1.0022615189154172e-34,
+                    "velocityY": -1.2494033787640354e-35,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.2520019144790657,
-                    "y": 5.512001876818988,
-                    "heading": -0.015697245285858377,
-                    "angularVelocity": -0.2063751026737087,
-                    "velocityX": 0.02631960567841837,
-                    "velocityY": -0.6310417795758498,
-                    "timestamp": 0.07606172005484914
+                    "x": 2.2520019447146944,
+                    "y": 5.512001877264397,
+                    "heading": -0.01569721301150974,
+                    "angularVelocity": -0.20637467835572687,
+                    "velocityX": 0.02632000319280478,
+                    "velocityY": -0.6310417737196751,
+                    "timestamp": 0.07606172005488261
                 },
                 {
-                    "x": 2.256005269562975,
-                    "y": 5.416005526715618,
-                    "heading": -0.04708689462727081,
-                    "angularVelocity": -0.41268655663817416,
-                    "velocityX": 0.05263298122921632,
-                    "velocityY": -1.2620849230617632,
-                    "timestamp": 0.15212344010969828
+                    "x": 2.2560053559202022,
+                    "y": 5.416005527882177,
+                    "heading": -0.04708679827781808,
+                    "angularVelocity": -0.41268571422864314,
+                    "velocityX": 0.05263371907208186,
+                    "velocityY": -1.262084913580087,
+                    "timestamp": 0.15212344010976522
                 },
                 {
-                    "x": 2.26200939656581,
-                    "y": 5.272010697248206,
-                    "heading": -0.09415666260883829,
-                    "angularVelocity": -0.6188364915705936,
-                    "velocityX": 0.07893756542062844,
-                    "velocityY": -1.8931313854535041,
-                    "timestamp": 0.22818516016454743
+                    "x": 2.262009560249172,
+                    "y": 5.272010699235591,
+                    "heading": -0.0941564711571504,
+                    "angularVelocity": -0.6188352412405218,
+                    "velocityX": 0.07893858204412474,
+                    "velocityY": -1.8931313746610776,
+                    "timestamp": 0.22818516016464785
                 },
                 {
-                    "x": 2.2700135068749114,
-                    "y": 5.080016881567182,
-                    "heading": -0.15688110795154597,
-                    "angularVelocity": -0.8246519444666291,
-                    "velocityX": 0.10523178154963624,
-                    "velocityY": -2.5241845115068955,
-                    "timestamp": 0.30424688021939655
+                    "x": 2.2700137636187,
+                    "y": 5.080016884298785,
+                    "heading": -0.15688079244309955,
+                    "angularVelocity": -0.8246503134650409,
+                    "velocityX": 0.10523300503528431,
+                    "velocityY": -2.5241845017213995,
+                    "timestamp": 0.30424688021953045
                 },
                 {
-                    "x": 2.280008999433411,
-                    "y": 4.840012166841925,
-                    "heading": -0.23463480916086144,
-                    "angularVelocity": -1.0222448447556298,
-                    "velocityX": 0.13141291771066904,
-                    "velocityY": -3.155394258139129,
-                    "timestamp": 0.3803086002742457
+                    "x": 2.2800092014745617,
+                    "y": 4.840012168782865,
+                    "heading": -0.23463455266722952,
+                    "angularVelocity": -1.0222456206358,
+                    "velocityX": 0.13141219852311892,
+                    "velocityY": -3.155394268532757,
+                    "timestamp": 0.38030860027441304
                 },
                 {
-                    "x": 2.2880053509029836,
-                    "y": 4.648007615057375,
-                    "heading": -0.29679802026954394,
-                    "angularVelocity": -0.8172732757536348,
-                    "velocityX": 0.10512977439645693,
-                    "velocityY": -2.524325661398308,
-                    "timestamp": 0.4563703203290948
+                    "x": 2.2880054914438257,
+                    "y": 4.648007616430203,
+                    "heading": -0.2967978427641432,
+                    "angularVelocity": -0.8172743142287549,
+                    "velocityX": 0.10512896583845424,
+                    "velocityY": -2.524325668866294,
+                    "timestamp": 0.45637032032929564
                 },
                 {
-                    "x": 2.2940026546256975,
-                    "y": 4.504003857619058,
-                    "heading": -0.34340214900619426,
-                    "angularVelocity": -0.6127146310002387,
-                    "velocityX": 0.07884785827074504,
-                    "velocityY": -1.893248763431504,
-                    "timestamp": 0.532432040383944
+                    "x": 2.2940027351611283,
+                    "y": 4.5040038585170965,
+                    "heading": -0.3434020516948695,
+                    "angularVelocity": -0.6127156853289512,
+                    "velocityX": 0.07884706936650254,
+                    "velocityY": -1.8932487696728393,
+                    "timestamp": 0.5324320403841782
                 },
                 {
-                    "x": 2.2980008838862798,
-                    "y": 4.408001268016137,
-                    "heading": -0.37446665790253814,
-                    "angularVelocity": -0.4084118643904297,
-                    "velocityX": 0.05256559091352319,
-                    "velocityY": -1.26216695512135,
-                    "timestamp": 0.6084937604387931
+                    "x": 2.2980009144821483,
+                    "y": 4.408001268437703,
+                    "heading": -0.37446662411182524,
+                    "angularVelocity": -0.40841269950956927,
+                    "velocityX": 0.05256493434720428,
+                    "velocityY": -1.2621669613850792,
+                    "timestamp": 0.6084937604390609
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
                     "heading": -0.39,
-                    "angularVelocity": -0.20422023175732293,
-                    "velocityX": 0.02628281495972678,
-                    "velocityY": -0.6310831254081802,
-                    "timestamp": 0.6845554804936422
+                    "angularVelocity": -0.20422067601109492,
+                    "velocityX": 0.026282412709165117,
+                    "velocityY": -0.6310831309503169,
+                    "timestamp": 0.6845554804939435
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
                     "heading": -0.39,
-                    "angularVelocity": 0,
-                    "velocityX": -1.9690468515346402e-33,
-                    "velocityY": 7.92544572453684e-31,
-                    "timestamp": 0.7606172005484914
+                    "angularVelocity": 8.777318885868316e-35,
+                    "velocityX": -1.1404320857258046e-34,
+                    "velocityY": 1.134511753128188e-36,
+                    "timestamp": 0.7606172005488262
                 }
             ],
             "constraints": [
@@ -12077,23 +13519,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "W2 To S1": {
             "waypoints": [
@@ -13648,190 +15074,190 @@
                     "x": 2.25,
                     "y": 5.56,
                     "heading": 0,
-                    "angularVelocity": -9.278807284273418e-29,
-                    "velocityX": -7.385267906050254e-32,
-                    "velocityY": -1.1558173496237395e-34,
+                    "angularVelocity": 0,
+                    "velocityX": -3.731399929679607e-32,
+                    "velocityY": 5.877471754111438e-38,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.3268918247049313,
-                    "y": 5.587760380936702,
-                    "heading": 0.001219352926916483,
-                    "angularVelocity": 0.01231456999280599,
-                    "velocityX": 0.776551018417967,
-                    "velocityY": 0.2803594812685564,
-                    "timestamp": 0.09901709337987798
-                },
-                {
-                    "x": 2.480675474257635,
-                    "y": 5.643281133231001,
-                    "heading": 0.0036580544911558616,
-                    "angularVelocity": 0.024629096663863686,
-                    "velocityX": 1.5531020382785252,
-                    "velocityY": 0.5607188657951702,
-                    "timestamp": 0.19803418675975595
-                },
-                {
-                    "x": 2.7113509459548553,
-                    "y": 5.726562245995426,
-                    "heading": 0.007316098440537021,
-                    "angularVelocity": 0.03694356019247884,
-                    "velocityX": 2.3296530308381804,
-                    "velocityY": 0.8410781403663149,
-                    "timestamp": 0.29705128013963394
-                },
-                {
-                    "x": 3.0189182254277274,
-                    "y": 5.837603703876164,
-                    "heading": 0.01219347400929413,
-                    "angularVelocity": 0.04925791499475694,
-                    "velocityX": 3.1062038782828476,
-                    "velocityY": 1.1214372598752256,
-                    "timestamp": 0.3960683735195119
-                },
-                {
-                    "x": 3.3712359540410324,
-                    "y": 5.964801318197582,
-                    "heading": 0.012193474535478832,
-                    "angularVelocity": 5.31407952800269e-9,
-                    "velocityX": 3.5581505837748795,
-                    "velocityY": 1.2846025870848905,
-                    "timestamp": 0.49508546689938987
-                },
-                {
-                    "x": 3.723553713186701,
-                    "y": 6.0919988479491165,
-                    "heading": 0.012193475061665435,
-                    "angularVelocity": 5.314098710197071e-9,
-                    "velocityX": 3.5581508921293548,
-                    "velocityY": 1.2846017329911394,
-                    "timestamp": 0.5941025602792679
-                },
-                {
-                    "x": 4.075871503881281,
-                    "y": 6.219196290314954,
-                    "heading": 0.012193475587862622,
-                    "angularVelocity": 5.314205602691753e-9,
-                    "velocityX": 3.558151210750214,
-                    "velocityY": 1.2846008504596915,
-                    "timestamp": 0.6931196536591459
-                },
-                {
-                    "x": 4.428189328839488,
-                    "y": 6.346393637775665,
-                    "heading": 0.012193476114071436,
-                    "angularVelocity": 5.3143230401144125e-9,
-                    "velocityX": 3.5581515567877036,
-                    "velocityY": 1.2845998919875412,
-                    "timestamp": 0.7921367470390238
-                },
-                {
-                    "x": 4.780507205251625,
-                    "y": 6.473590842716609,
-                    "heading": 0.012193476640289547,
-                    "angularVelocity": 5.314416922842863e-9,
-                    "velocityX": 3.5581520764346486,
-                    "velocityY": 1.2845984526424705,
-                    "timestamp": 0.8911538404189018
-                },
-                {
-                    "x": 5.134861642335281,
-                    "y": 6.594999030315895,
-                    "heading": 0.012193476116517958,
-                    "angularVelocity": -5.289708644023114e-9,
-                    "velocityX": 3.5787198451098035,
-                    "velocityY": 1.2261336245600862,
-                    "timestamp": 0.9901709337987797
-                },
-                {
-                    "x": 5.489216369156675,
-                    "y": 6.716406372237666,
-                    "heading": 0.012193475592327948,
-                    "angularVelocity": -5.293934612313601e-9,
-                    "velocityX": 3.578722771248356,
-                    "velocityY": 1.2261250838377307,
-                    "timestamp": 1.0891880271786578
-                },
-                {
-                    "x": 5.843571141725175,
-                    "y": 6.837813580636051,
-                    "heading": 0.012193475068127738,
-                    "angularVelocity": -5.2940376086876394e-9,
-                    "velocityX": 3.5787232332605625,
-                    "velocityY": 1.2261237353494956,
-                    "timestamp": 1.1882051205585358
-                },
-                {
-                    "x": 6.1979259496904,
-                    "y": 6.959220685720912,
-                    "heading": 0.012193474543942472,
-                    "angularVelocity": -5.293886679384445e-9,
-                    "velocityX": 3.5787235907415074,
-                    "velocityY": 1.2261226919587003,
-                    "timestamp": 1.2872222139384137
-                },
-                {
-                    "x": 6.552280784920285,
-                    "y": 7.080627711227486,
-                    "heading": 0.012193474019771769,
-                    "angularVelocity": -5.293739610148028e-9,
-                    "velocityX": 3.5787238660945766,
-                    "velocityY": 1.2261218882763663,
-                    "timestamp": 1.3862393073182917
-                },
-                {
-                    "x": 6.906635640601096,
-                    "y": 7.202034677043121,
-                    "heading": 0.01219347349560639,
-                    "angularVelocity": -5.293685828527132e-9,
-                    "velocityX": 3.5787240726339373,
-                    "velocityY": 1.2261212854416952,
-                    "timestamp": 1.4852564006981697
-                },
-                {
-                    "x": 7.215981364626797,
-                    "y": 7.3080208081486955,
-                    "heading": 0.0073160851772923075,
-                    "angularVelocity": -0.0492580437559288,
-                    "velocityX": 3.124164863524117,
-                    "velocityY": 1.0703821682480574,
-                    "timestamp": 1.5842734940780476
-                },
-                {
-                    "x": 7.447990676616371,
-                    "y": 7.387510406301363,
-                    "heading": 0.003658042213644346,
-                    "angularVelocity": -0.03694355023729677,
-                    "velocityX": 2.343123839229173,
-                    "velocityY": 0.8027866244033968,
-                    "timestamp": 1.6832905874579256
-                },
-                {
-                    "x": 7.602663557476493,
-                    "y": 7.440503469639918,
-                    "heading": 0.0012193472043836597,
-                    "angularVelocity": -0.024629030463387942,
-                    "velocityX": 1.5620826221056736,
-                    "velocityY": 0.5351910617619017,
-                    "timestamp": 1.7823076808378036
+                    "x": 2.3271275380851444,
+                    "y": 5.587086964112038,
+                    "heading": 3.953847862678034e-18,
+                    "angularVelocity": 3.9932078666702264e-17,
+                    "velocityX": 0.7789533333445218,
+                    "velocityY": 0.2735661154121556,
+                    "timestamp": 0.09901432445765257
+                },
+                {
+                    "x": 2.4813826113143547,
+                    "y": 5.641260891303218,
+                    "heading": 1.1398162294527873e-19,
+                    "angularVelocity": -3.8780916638560175e-17,
+                    "velocityX": 1.557906636985473,
+                    "velocityY": 0.5471322203925046,
+                    "timestamp": 0.19802864891530514
+                },
+                {
+                    "x": 2.7127652138054734,
+                    "y": 5.722521779507741,
+                    "heading": 2.1553394674505063e-18,
+                    "angularVelocity": 2.0616792512846884e-17,
+                    "velocityX": 2.336859881219295,
+                    "velocityY": 0.8206983045092451,
+                    "timestamp": 0.2970429733729577
+                },
+                {
+                    "x": 3.0212753279120443,
+                    "y": 5.830869622528225,
+                    "heading": -1.0672476097054788e-17,
+                    "angularVelocity": -1.295551486240031e-16,
+                    "velocityX": 3.1158129472318716,
+                    "velocityY": 1.0942643260352094,
+                    "timestamp": 0.3960572978306103
+                },
+                {
+                    "x": 3.3746798137461993,
+                    "y": 5.9549842366139965,
+                    "heading": 1.8895880094915182e-18,
+                    "angularVelocity": 1.2687117924961395e-16,
+                    "velocityX": 3.5692258445423475,
+                    "velocityY": 1.2535015995473786,
+                    "timestamp": 0.49507162228826285
+                },
+                {
+                    "x": 3.7280842995803773,
+                    "y": 6.079098850699775,
+                    "heading": 1.0049319654091473e-17,
+                    "angularVelocity": 8.240960778697987e-17,
+                    "velocityX": 3.569225844542579,
+                    "velocityY": 1.25350159954746,
+                    "timestamp": 0.5940859467459154
+                },
+                {
+                    "x": 4.081488785414555,
+                    "y": 6.203213464785553,
+                    "heading": 8.136834826577652e-18,
+                    "angularVelocity": -1.931523381075779e-17,
+                    "velocityX": 3.569225844542579,
+                    "velocityY": 1.25350159954746,
+                    "timestamp": 0.693100271203568
+                },
+                {
+                    "x": 4.434893271248733,
+                    "y": 6.327328078871332,
+                    "heading": -2.4381204969164037e-18,
+                    "angularVelocity": -1.0680227715899811e-16,
+                    "velocityX": 3.5692258445425793,
+                    "velocityY": 1.2535015995474599,
+                    "timestamp": 0.7921145956612206
+                },
+                {
+                    "x": 4.788297757082911,
+                    "y": 6.4514426929571105,
+                    "heading": 3.192038150785043e-17,
+                    "angularVelocity": 3.47005367081625e-16,
+                    "velocityX": 3.569225844542579,
+                    "velocityY": 1.25350159954746,
+                    "timestamp": 0.8911289201188731
+                },
+                {
+                    "x": 5.141702242917089,
+                    "y": 6.575557307042889,
+                    "heading": 6.016026907559514e-17,
+                    "angularVelocity": 2.8521012209839933e-16,
+                    "velocityX": 3.569225844542579,
+                    "velocityY": 1.2535015995474599,
+                    "timestamp": 0.9901432445765257
+                },
+                {
+                    "x": 5.495106728751266,
+                    "y": 6.699671921128667,
+                    "heading": 2.4186102644429564e-17,
+                    "angularVelocity": -3.63322848771759e-16,
+                    "velocityX": 3.569225844542579,
+                    "velocityY": 1.2535015995474599,
+                    "timestamp": 1.0891575690341782
+                },
+                {
+                    "x": 5.848511214585444,
+                    "y": 6.823786535214446,
+                    "heading": -1.2421929874703125e-18,
+                    "angularVelocity": -2.568143121812034e-16,
+                    "velocityX": 3.5692258445425793,
+                    "velocityY": 1.2535015995474599,
+                    "timestamp": 1.1881718934918308
+                },
+                {
+                    "x": 6.2019157004196215,
+                    "y": 6.9479011493002245,
+                    "heading": 2.3184212364315377e-18,
+                    "angularVelocity": 3.596059704901295e-17,
+                    "velocityX": 3.569225844542579,
+                    "velocityY": 1.2535015995474599,
+                    "timestamp": 1.2871862179494835
+                },
+                {
+                    "x": 6.5553201862538,
+                    "y": 7.072015763386003,
+                    "heading": 3.1459219033425486e-17,
+                    "angularVelocity": 2.9430890890068574e-16,
+                    "velocityX": 3.569225844542579,
+                    "velocityY": 1.25350159954746,
+                    "timestamp": 1.3862005424071362
+                },
+                {
+                    "x": 6.9087246720879545,
+                    "y": 7.196130377471773,
+                    "heading": 2.9741502939674576e-17,
+                    "angularVelocity": -1.7348157482865667e-17,
+                    "velocityX": 3.5692258445423475,
+                    "velocityY": 1.2535015995473786,
+                    "timestamp": 1.4852148668647889
+                },
+                {
+                    "x": 7.217234786194527,
+                    "y": 7.304478220492258,
+                    "heading": 1.2383304928032073e-17,
+                    "angularVelocity": -1.753099667759744e-16,
+                    "velocityX": 3.115812947231872,
+                    "velocityY": 1.0942643260352094,
+                    "timestamp": 1.5842291913224416
+                },
+                {
+                    "x": 7.448617388685645,
+                    "y": 7.385739108696781,
+                    "heading": -3.803725574168796e-18,
+                    "angularVelocity": -1.6348170333424006e-16,
+                    "velocityX": 2.336859881219295,
+                    "velocityY": 0.8206983045092451,
+                    "timestamp": 1.6832435157800942
+                },
+                {
+                    "x": 7.602872461914855,
+                    "y": 7.439913035887961,
+                    "heading": -2.4779119751079125e-18,
+                    "angularVelocity": 1.3390118812558602e-17,
+                    "velocityX": 1.557906636985473,
+                    "velocityY": 0.5471322203925049,
+                    "timestamp": 1.782257840237747
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 1.0919443501059542e-30,
-                    "angularVelocity": -0.01231451219942279,
-                    "velocityX": 0.781041332195102,
-                    "velocityY": 0.26759551765904444,
-                    "timestamp": 1.8813247742176815
+                    "heading": 0,
+                    "angularVelocity": 2.5025792615041696e-17,
+                    "velocityX": 0.7789533333445218,
+                    "velocityY": 0.27356611541215564,
+                    "timestamp": 1.8812721646953996
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
                     "heading": 0,
-                    "angularVelocity": -8.461898211343436e-30,
-                    "velocityX": 0,
-                    "velocityY": -1.0908466908101513e-31,
-                    "timestamp": 1.9803418675975595
+                    "angularVelocity": 0,
+                    "velocityX": 1.3533974507170236e-34,
+                    "velocityY": 0,
+                    "timestamp": 1.9802864891530523
                 }
             ],
             "constraints": [
@@ -13851,23 +15277,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "W2 To C2": {
             "waypoints": [
@@ -13906,521 +15316,521 @@
                     "heading": 0,
                     "angularVelocity": 0,
                     "velocityX": 0,
-                    "velocityY": -1.3324658216077414e-32,
+                    "velocityY": -1.058013890027075e-32,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.2604047390881647,
-                    "y": 5.564834315872384,
-                    "heading": -0.00020682184904158323,
-                    "angularVelocity": -0.005575579259585174,
-                    "velocityX": 0.2804947723375417,
-                    "velocityY": 0.13032526030129518,
-                    "timestamp": 0.037094235309470626
-                },
-                {
-                    "x": 2.2812142175691745,
-                    "y": 5.574502946267583,
-                    "heading": -0.0006204643713099006,
-                    "angularVelocity": -0.011151126821122416,
-                    "velocityX": 0.5609895528887481,
-                    "velocityY": 0.2606504842204101,
-                    "timestamp": 0.07418847061894125
-                },
-                {
-                    "x": 2.3124284356981284,
-                    "y": 5.589005889804122,
-                    "heading": -0.0012409261115257607,
-                    "angularVelocity": -0.01672663515070949,
-                    "velocityX": 0.8414843403169981,
-                    "velocityY": 0.39097567089719987,
-                    "timestamp": 0.11128270592841188
-                },
-                {
-                    "x": 2.3540473936632536,
-                    "y": 5.608343145059642,
-                    "heading": -0.0020682052367836136,
-                    "angularVelocity": -0.022302094068156527,
-                    "velocityX": 1.121979132819583,
-                    "velocityY": 0.5213008192295232,
-                    "timestamp": 0.1483769412378825
-                },
-                {
-                    "x": 2.4060710915596104,
-                    "y": 5.632514710558234,
-                    "heading": -0.0031022993856005916,
-                    "angularVelocity": -0.027877489323882353,
-                    "velocityX": 1.402473927884819,
-                    "velocityY": 0.6516259277737647,
-                    "timestamp": 0.18547117654735312
-                },
-                {
-                    "x": 2.46849952934744,
-                    "y": 5.661520584751242,
-                    "heading": -0.004343205426988983,
-                    "angularVelocity": -0.03345280017329315,
-                    "velocityX": 1.6829687218782206,
-                    "velocityY": 0.781950994568762,
-                    "timestamp": 0.22256541185682374
-                },
-                {
-                    "x": 2.5413327067825313,
-                    "y": 5.695360765985745,
-                    "heading": -0.005790919055957113,
-                    "angularVelocity": -0.039027994967159495,
-                    "velocityX": 1.9634635092880914,
-                    "velocityY": 0.9122760168036823,
-                    "timestamp": 0.25965964716629436
-                },
-                {
-                    "x": 2.6245706232908623,
-                    "y": 5.734035252447861,
-                    "heading": -0.007445434065373688,
-                    "angularVelocity": -0.044603022426903935,
-                    "velocityX": 2.2439582812232635,
-                    "velocityY": 1.042600990139337,
-                    "timestamp": 0.296753882475765
-                },
-                {
-                    "x": 2.7182132777182155,
-                    "y": 5.777544042048614,
-                    "heading": -0.009306740898831883,
-                    "angularVelocity": -0.05017779226152841,
-                    "velocityX": 2.5244530220426173,
-                    "velocityY": 1.1729259071596103,
-                    "timestamp": 0.33384811778523565
-                },
-                {
-                    "x": 2.8222606677468502,
-                    "y": 5.825887132155531,
-                    "heading": -0.011374823315541767,
-                    "angularVelocity": -0.05575212427098156,
-                    "velocityX": 2.804947700379475,
-                    "velocityY": 1.3032507532126736,
-                    "timestamp": 0.3709423530947063
-                },
-                {
-                    "x": 2.9367127881472834,
-                    "y": 5.879064518782887,
-                    "heading": -0.013649648601341033,
-                    "angularVelocity": -0.0613255743600492,
-                    "velocityX": 3.0854422377380093,
-                    "velocityY": 1.4335754918171768,
-                    "timestamp": 0.40803658840417695
-                },
-                {
-                    "x": 3.061569622038521,
-                    "y": 5.9370761925311655,
-                    "heading": -0.016131121234305167,
-                    "angularVelocity": -0.06689644933404303,
-                    "velocityX": 3.365936320012513,
-                    "velocityY": 1.563899977025988,
-                    "timestamp": 0.4451308237136476
-                },
-                {
-                    "x": 3.188829425192231,
-                    "y": 5.996204205741329,
-                    "heading": -0.01613112189285862,
-                    "angularVelocity": -1.7753525563113908e-8,
-                    "velocityX": 3.430716446693241,
-                    "velocityY": 1.5939946656635118,
-                    "timestamp": 0.48222505902311824
-                },
-                {
-                    "x": 3.3160892428339666,
-                    "y": 6.055332187769375,
-                    "heading": -0.01613112255138683,
-                    "angularVelocity": -1.775284497104555e-8,
-                    "velocityX": 3.4307168372667594,
-                    "velocityY": 1.5939938250445522,
-                    "timestamp": 0.5193192943325888
-                },
-                {
-                    "x": 3.443349075020492,
-                    "y": 6.114460138492994,
-                    "heading": -0.016131123209912376,
-                    "angularVelocity": -1.775277322447583e-8,
-                    "velocityX": 3.43071722937053,
-                    "velocityY": 1.593992981128321,
-                    "timestamp": 0.5564135296420595
-                },
-                {
-                    "x": 3.5706089218502877,
-                    "y": 6.173588057700207,
-                    "heading": -0.01613112386843302,
-                    "angularVelocity": -1.775264097704561e-8,
-                    "velocityX": 3.430717624129179,
-                    "velocityY": 1.5939921314975005,
-                    "timestamp": 0.5935077649515301
-                },
-                {
-                    "x": 3.697868783477107,
-                    "y": 6.2327159450600735,
-                    "heading": -0.016131124526946636,
-                    "angularVelocity": -1.7752451586110053e-8,
-                    "velocityX": 3.430718023032762,
-                    "velocityY": 1.5939912729450545,
-                    "timestamp": 0.6306020002610008
-                },
-                {
-                    "x": 3.8251286601281493,
-                    "y": 6.291843800083575,
-                    "heading": -0.016131125185451407,
-                    "angularVelocity": -1.7752213148650806e-8,
-                    "velocityX": 3.4307184280612866,
-                    "velocityY": 1.5939904012094348,
-                    "timestamp": 0.6676962355704714
-                },
-                {
-                    "x": 3.95238855212494,
-                    "y": 6.350971622078674,
-                    "heading": -0.016131125843945708,
-                    "angularVelocity": -1.775193091205886e-8,
-                    "velocityX": 3.430718841757593,
-                    "velocityY": 1.5939895108176958,
-                    "timestamp": 0.7047904708799421
-                },
-                {
-                    "x": 4.079648459881884,
-                    "y": 6.410099410153433,
-                    "heading": -0.01613112650242917,
-                    "angularVelocity": -1.7751638714463086e-8,
-                    "velocityX": 3.430719266625584,
-                    "velocityY": 1.5939885963806835,
-                    "timestamp": 0.7418847061894127
-                },
-                {
-                    "x": 4.2069083839232695,
-                    "y": 6.469227163179404,
-                    "heading": -0.01613112716090301,
-                    "angularVelocity": -1.7751379248608663e-8,
-                    "velocityX": 3.430719705627546,
-                    "velocityY": 1.5939876515226545,
-                    "timestamp": 0.7789789414988834
-                },
-                {
-                    "x": 4.334168324938812,
-                    "y": 6.528354879672101,
-                    "heading": -0.016131127819368877,
-                    "angularVelocity": -1.7751164242273923e-8,
-                    "velocityX": 3.430720163223074,
-                    "velocityY": 1.5939866666451934,
-                    "timestamp": 0.816073176808354
-                },
-                {
-                    "x": 4.461428283794271,
-                    "y": 6.58748255776812,
-                    "heading": -0.01613112847782922,
-                    "angularVelocity": -1.775101542628313e-8,
-                    "velocityX": 3.430720644158123,
-                    "velocityY": 1.5939856315335348,
-                    "timestamp": 0.8531674121178247
-                },
-                {
-                    "x": 4.58868833054201,
-                    "y": 6.646610046694028,
-                    "heading": -0.016131129136287645,
-                    "angularVelocity": -1.7750963701844846e-8,
-                    "velocityX": 3.430723013590394,
-                    "velocityY": 1.5939805318162525,
-                    "timestamp": 0.8902616474272953
-                },
-                {
-                    "x": 4.716960790002456,
-                    "y": 6.703507810971908,
-                    "heading": -0.016131129795891656,
-                    "angularVelocity": -1.7781846861521853e-8,
-                    "velocityX": 3.4580160068077612,
-                    "velocityY": 1.5338707969903813,
-                    "timestamp": 0.927355882736766
-                },
-                {
-                    "x": 4.849451901724698,
-                    "y": 6.749738586923234,
-                    "heading": -0.016131130468276335,
-                    "angularVelocity": -1.8126392792981438e-8,
-                    "velocityX": 3.5717439816966663,
-                    "velocityY": 1.2463062135027452,
-                    "timestamp": 0.9644501180462366
-                },
-                {
-                    "x": 4.985276799458285,
-                    "y": 6.784991618369029,
-                    "heading": -0.016131131172819703,
-                    "angularVelocity": -1.899333848953808e-8,
-                    "velocityX": 3.661617407676018,
-                    "velocityY": 0.950364150970728,
-                    "timestamp": 1.0015443533557071
-                },
-                {
-                    "x": 5.12352760039525,
-                    "y": 6.80903105836077,
-                    "heading": -0.01613113193292308,
-                    "angularVelocity": -2.0491145599848975e-8,
-                    "velocityX": 3.7270157959468126,
-                    "velocityY": 0.6480640399022476,
-                    "timestamp": 1.0386385886651777
-                },
-                {
-                    "x": 5.263280144021708,
-                    "y": 6.821696149590996,
-                    "heading": -0.01613113277968929,
-                    "angularVelocity": -2.2827434072857776e-8,
-                    "velocityX": 3.767500326143083,
-                    "velocityY": 0.341430174380559,
-                    "timestamp": 1.0757328239746482
+                    "x": 2.26040474690827,
+                    "y": 5.564834298284281,
+                    "heading": -0.00020682462174428096,
+                    "angularVelocity": -0.005575654083684234,
+                    "velocityX": 0.28049498700644215,
+                    "velocityY": 0.1303247879443207,
+                    "timestamp": 0.037094234800108185
+                },
+                {
+                    "x": 2.2812142405469196,
+                    "y": 5.574502894544252,
+                    "heading": -0.0006204719876615776,
+                    "angularVelocity": -0.011151257551113,
+                    "velocityX": 0.5609899692172368,
+                    "velocityY": 0.26064956756953994,
+                    "timestamp": 0.07418846960021637
+                },
+                {
+                    "x": 2.312428480686622,
+                    "y": 5.589005788443611,
+                    "heading": -0.0012409398807264826,
+                    "angularVelocity": -0.016726801251156914,
+                    "velocityX": 0.8414849452457597,
+                    "velocityY": 0.39097433812857635,
+                    "timestamp": 0.11128270440032456
+                },
+                {
+                    "x": 2.3540474670292637,
+                    "y": 5.608342979609529,
+                    "heading": -0.002068225636714691,
+                    "angularVelocity": -0.022302273128063925,
+                    "velocityX": 1.1219799132376411,
+                    "velocityY": 0.5212990986367875,
+                    "timestamp": 0.14837693920043274
+                },
+                {
+                    "x": 2.406071199181798,
+                    "y": 5.632514467619691,
+                    "heading": -0.0031023259847413323,
+                    "angularVelocity": -0.02787765682725579,
+                    "velocityX": 1.4024748706336019,
+                    "velocityY": 0.6516238477600773,
+                    "timestamp": 0.18547117400054092
+                },
+                {
+                    "x": 2.4684996766150022,
+                    "y": 5.66152025198224,
+                    "heading": -0.004343236799506005,
+                    "angularVelocity": -0.03345292931508493,
+                    "velocityX": 1.682969813762594,
+                    "velocityY": 0.7819485836236739,
+                    "timestamp": 0.2225654088006491
+                },
+                {
+                    "x": 2.5413328985945514,
+                    "y": 5.69536033210275,
+                    "heading": -0.005790952692459236,
+                    "angularVelocity": -0.03902805653641911,
+                    "velocityX": 1.9634647370954057,
+                    "velocityY": 0.9122733034625292,
+                    "timestamp": 0.2596596436007573
+                },
+                {
+                    "x": 2.6245708640566887,
+                    "y": 5.7340347072253275,
+                    "heading": -0.00744546628275827,
+                    "angularVelocity": -0.04460298478228464,
+                    "velocityX": 2.2439596317510366,
+                    "velocityY": 1.0425980029237354,
+                    "timestamp": 0.2967538784008655
+                },
+                {
+                    "x": 2.7182135713591755,
+                    "y": 5.777543376315584,
+                    "heading": -0.009306766753500902,
+                    "angularVelocity": -0.05017762142212937,
+                    "velocityX": 2.5244544821346007,
+                    "velocityY": 1.1729226744995158,
+                    "timestamp": 0.3338481132009737
+                },
+                {
+                    "x": 2.8222610176995655,
+                    "y": 5.8258863377887575,
+                    "heading": -0.011374836524793099,
+                    "angularVelocity": -0.055751784136736884,
+                    "velocityX": 2.8049492569688192,
+                    "velocityY": 1.303247303352706,
+                    "timestamp": 0.3709423480010819
+                },
+                {
+                    "x": 2.9367131973687606,
+                    "y": 5.879063588696104,
+                    "heading": -0.013649641477510719,
+                    "angularVelocity": -0.061325027055469855,
+                    "velocityX": 3.0854438778950355,
+                    "velocityY": 1.4335718527125696,
+                    "timestamp": 0.4080365828011901
+                },
+                {
+                    "x": 3.06157009301301,
+                    "y": 5.937075120660461,
+                    "heading": -0.01613108463550858,
+                    "angularVelocity": -0.06689565565567898,
+                    "velocityX": 3.3659380309924987,
+                    "velocityY": 1.5638961762378367,
+                    "timestamp": 0.4451308176012983
+                },
+                {
+                    "x": 3.1888299168589427,
+                    "y": 5.996203084762226,
+                    "heading": -0.01613108529402469,
+                    "angularVelocity": -1.7752519083825314e-8,
+                    "velocityX": 3.430717051630934,
+                    "velocityY": 1.5939933636693306,
+                    "timestamp": 0.4822250524014065
+                },
+                {
+                    "x": 3.316089744181463,
+                    "y": 6.055331041381533,
+                    "heading": -0.016131085952509703,
+                    "angularVelocity": -1.7751680763659775e-8,
+                    "velocityX": 3.4307171453540595,
+                    "velocityY": 1.5939931619544851,
+                    "timestamp": 0.5193192872015147
+                },
+                {
+                    "x": 3.443349575355442,
+                    "y": 6.114458989711434,
+                    "heading": -0.01613108661098856,
+                    "angularVelocity": -1.7751514817327164e-8,
+                    "velocityX": 3.430717249183109,
+                    "velocityY": 1.5939929384856546,
+                    "timestamp": 0.5564135220016229
+                },
+                {
+                    "x": 3.5706094108109987,
+                    "y": 6.173586928826195,
+                    "heading": -0.016131087269461084,
+                    "angularVelocity": -1.7751344035751635e-8,
+                    "velocityX": 3.4307173646074394,
+                    "velocityY": 1.5939926900605101,
+                    "timestamp": 0.5935077568017311
+                },
+                {
+                    "x": 3.6978692510432434,
+                    "y": 6.232714857660196,
+                    "heading": -0.01613108792792704,
+                    "angularVelocity": -1.7751166882414398e-8,
+                    "velocityX": 3.4307174933791567,
+                    "velocityY": 1.5939924129080143,
+                    "timestamp": 0.6306019916018393
+                },
+                {
+                    "x": 3.825129096624906,
+                    "y": 6.291842774980762,
+                    "heading": -0.016131088586386323,
+                    "angularVelocity": -1.775098711225461e-8,
+                    "velocityX": 3.430717637590709,
+                    "velocityY": 1.5939921025246122,
+                    "timestamp": 0.6676962264019475
+                },
+                {
+                    "x": 3.952388948222243,
+                    "y": 6.3509706793539165,
+                    "heading": -0.016131089244838845,
+                    "angularVelocity": -1.775080474504649e-8,
+                    "velocityX": 3.4307177997634843,
+                    "velocityY": 1.5939917534835466,
+                    "timestamp": 0.7047904612020557
+                },
+                {
+                    "x": 4.07964880661482,
+                    "y": 6.410098569101814,
+                    "heading": -0.016131089903284688,
+                    "angularVelocity": -1.775062484471552e-8,
+                    "velocityX": 3.430717982952055,
+                    "velocityY": 1.5939913592104822,
+                    "timestamp": 0.7418846960021639
+                },
+                {
+                    "x": 4.206908672718448,
+                    "y": 6.469226442253365,
+                    "heading": -0.016131090561724272,
+                    "angularVelocity": -1.7750456035932977e-8,
+                    "velocityX": 3.4307181908294035,
+                    "velocityY": 1.5939909118000826,
+                    "timestamp": 0.7789789308022721
+                },
+                {
+                    "x": 4.33416854760619,
+                    "y": 6.52835429649903,
+                    "heading": -0.016131091220158388,
+                    "angularVelocity": -1.775030869099235e-8,
+                    "velocityX": 3.43071842763476,
+                    "velocityY": 1.593990402128264,
+                    "timestamp": 0.8160731656023803
+                },
+                {
+                    "x": 4.461428432515134,
+                    "y": 6.587482129176235,
+                    "heading": -0.016131091878588796,
+                    "angularVelocity": -1.7750208582663698e-8,
+                    "velocityX": 3.430718697790006,
+                    "velocityY": 1.5939898206778003,
+                    "timestamp": 0.8531674004024885
+                },
+                {
+                    "x": 4.588688397859823,
+                    "y": 6.646609788732575,
+                    "heading": -0.01613109253701839,
+                    "angularVelocity": -1.7750186861935105e-8,
+                    "velocityX": 3.4307208662063466,
+                    "velocityY": 1.5939851536219738,
+                    "timestamp": 0.8902616352025967
+                },
+                {
+                    "x": 4.7169608374986005,
+                    "y": 6.703507592945478,
+                    "heading": -0.016131093196594153,
+                    "angularVelocity": -1.7781085471466767e-8,
+                    "velocityX": 3.4580155199319775,
+                    "velocityY": 1.533871894635887,
+                    "timestamp": 0.9273558700027049
+                },
+                {
+                    "x": 4.84945193271468,
+                    "y": 6.749738410352506,
+                    "heading": -0.016131093868950192,
+                    "angularVelocity": -1.8125621008136676e-8,
+                    "velocityX": 3.571743585763167,
+                    "velocityY": 1.2463073481945173,
+                    "timestamp": 0.9644501048028131
+                },
+                {
+                    "x": 4.985276817345946,
+                    "y": 6.784991484609654,
+                    "heading": -0.0161310945734632,
+                    "angularVelocity": -1.8992520332334574e-8,
+                    "velocityX": 3.661617104738627,
+                    "velocityY": 0.9503653181449264,
+                    "timestamp": 1.0015443396029213
+                },
+                {
+                    "x": 5.123527608694991,
+                    "y": 6.809030968493646,
+                    "heading": -0.01613109533353302,
+                    "angularVelocity": -2.0490241268499195e-8,
+                    "velocityX": 3.7270155886499414,
+                    "velocityY": 0.6480652320645928,
+                    "timestamp": 1.0386385744030295
+                },
+                {
+                    "x": 5.263280146337045,
+                    "y": 6.821696104409015,
+                    "heading": -0.016131096180260384,
+                    "angularVelocity": -2.2826387114294202e-8,
+                    "velocityX": 3.767500216547063,
+                    "velocityY": 0.34143138370740883,
+                    "timestamp": 1.0757328092031377
                 },
                 {
                     "x": 5.403600215911865,
                     "y": 6.822902202606201,
-                    "heading": -0.016131133746573465,
-                    "angularVelocity": -2.6065618224331403e-8,
-                    "velocityX": 3.782799961220162,
-                    "velocityY": 0.03251321951088522,
-                    "timestamp": 1.1128270592841187
-                },
-                {
-                    "x": 5.530811600424101,
-                    "y": 6.814532053807006,
-                    "heading": -0.016131134641902804,
-                    "angularVelocity": -2.6567346760379892e-8,
-                    "velocityX": 3.7747774237594087,
-                    "velocityY": -0.24836966315439601,
-                    "timestamp": 1.1465274262810004
-                },
-                {
-                    "x": 5.6570507349000225,
-                    "y": 6.796742242248266,
-                    "heading": -0.016131135427811184,
-                    "angularVelocity": -2.332046941486525e-8,
-                    "velocityX": 3.7459275884919543,
-                    "velocityY": -0.5278818346513846,
-                    "timestamp": 1.180227793277882
-                },
-                {
-                    "x": 5.781621094465109,
-                    "y": 6.769630943607204,
-                    "heading": -0.01613113613886582,
-                    "angularVelocity": -2.1099314246183972e-8,
-                    "velocityX": 3.696409584400467,
-                    "velocityY": -0.8044808130300218,
-                    "timestamp": 1.2139281602747636
-                },
-                {
-                    "x": 5.903835372739644,
-                    "y": 6.733347785640362,
-                    "heading": -0.016131136799649604,
-                    "angularVelocity": -1.9607613962388268e-8,
-                    "velocityX": 3.6264969543460657,
-                    "velocityY": -1.0766398469844052,
-                    "timestamp": 1.2476285272716452
-                },
-                {
-                    "x": 6.023019298132563,
-                    "y": 6.68809306728661,
-                    "heading": -0.01613113742890953,
-                    "angularVelocity": -1.8672198003746715e-8,
-                    "velocityX": 3.536576483097275,
-                    "velocityY": -1.3428553569739705,
-                    "timestamp": 1.2813288942685268
-                },
-                {
-                    "x": 6.13851552436941,
-                    "y": 6.634116966515058,
-                    "heading": -0.016131138042081584,
-                    "angularVelocity": -1.819481831325098e-8,
-                    "velocityX": 3.4271504000990243,
-                    "velocityY": -1.601647269198795,
-                    "timestamp": 1.3150292612654084
-                },
-                {
-                    "x": 6.251225914876305,
-                    "y": 6.574542092363786,
-                    "heading": -0.016131138651474624,
-                    "angularVelocity": -1.808268258780769e-8,
-                    "velocityX": 3.344485551665478,
-                    "velocityY": -1.7677811685785219,
-                    "timestamp": 1.34872962826229
-                },
-                {
-                    "x": 6.363935678841957,
-                    "y": 6.51496603286697,
-                    "heading": -0.016131139260695985,
-                    "angularVelocity": -1.807758837477246e-8,
-                    "velocityX": 3.344466960139662,
-                    "velocityY": -1.7678163416537895,
-                    "timestamp": 1.3824299952591717
-                },
-                {
-                    "x": 6.475851865782255,
-                    "y": 6.4539122462129415,
-                    "heading": -0.016131138603390965,
-                    "angularVelocity": 1.950438761664722e-8,
-                    "velocityX": 3.320918936896271,
-                    "velocityY": -1.8116653346735878,
-                    "timestamp": 1.4161303622560533
-                },
-                {
-                    "x": 6.587767727105175,
-                    "y": 6.3928578626815735,
-                    "heading": -0.016131137945887282,
-                    "angularVelocity": 1.9510282594383425e-8,
-                    "velocityX": 3.320909274764734,
-                    "velocityY": -1.811683045974467,
-                    "timestamp": 1.4498307292529349
-                },
-                {
-                    "x": 6.699683536860704,
-                    "y": 6.331803384624609,
-                    "heading": -0.016131137288379963,
-                    "angularVelocity": 1.951039048402864e-8,
-                    "velocityX": 3.3209077445917723,
-                    "velocityY": -1.811685850857758,
-                    "timestamp": 1.4835310962498165
-                },
-                {
-                    "x": 6.811599317469464,
-                    "y": 6.270748853140253,
-                    "heading": -0.0161311366308819,
-                    "angularVelocity": 1.951011573787136e-8,
-                    "velocityX": 3.320906879711909,
-                    "velocityY": -1.811687436223046,
-                    "timestamp": 1.517231463246698
-                },
-                {
-                    "x": 6.923515076276675,
-                    "y": 6.209694281692974,
-                    "heading": -0.016131135973346068,
-                    "angularVelocity": 1.951123652121546e-8,
-                    "velocityX": 3.3209062327887042,
-                    "velocityY": -1.8116886220535497,
-                    "timestamp": 1.5509318302435797
-                },
-                {
-                    "x": 7.031584320380035,
-                    "y": 6.150738002364414,
-                    "heading": -0.01382673748429001,
-                    "angularVelocity": 0.06837903246775291,
-                    "velocityX": 3.2067675735804393,
-                    "velocityY": -1.7494254390171946,
-                    "timestamp": 1.5846321972404613
-                },
-                {
-                    "x": 7.1313405673144,
-                    "y": 6.0963168023113505,
-                    "heading": -0.011699564081675173,
-                    "angularVelocity": 0.06312018509503639,
-                    "velocityX": 2.9600937860289633,
-                    "velocityY": -1.6148548191804175,
-                    "timestamp": 1.618332564237343
-                },
-                {
-                    "x": 7.222783801332874,
-                    "y": 6.046430691545947,
-                    "heading": -0.009749641962936324,
-                    "angularVelocity": 0.05786056035883372,
-                    "velocityX": 2.7134195312156404,
-                    "velocityY": -1.480283902250094,
-                    "timestamp": 1.6520329312342246
-                },
-                {
-                    "x": 7.305914017550448,
-                    "y": 6.00107967407256,
-                    "heading": -0.007976979919226685,
-                    "angularVelocity": 0.05260067476039131,
-                    "velocityX": 2.4667451314481808,
-                    "velocityY": -1.3457128664973763,
-                    "timestamp": 1.6857332982311062
-                },
-                {
-                    "x": 7.380731213778514,
-                    "y": 5.960263752360125,
-                    "heading": -0.006381581900532674,
-                    "angularVelocity": 0.047340671952945675,
-                    "velocityX": 2.2200706667375205,
-                    "velocityY": -1.2111417574833037,
-                    "timestamp": 1.7194336652279878
-                },
-                {
-                    "x": 7.447235388895749,
-                    "y": 5.923982928242947,
-                    "heading": -0.004963449854323965,
-                    "angularVelocity": 0.042080611357711836,
-                    "velocityX": 1.973396168753552,
-                    "velocityY": -1.076570594039371,
-                    "timestamp": 1.7531340322248694
-                },
-                {
-                    "x": 7.505426542307151,
-                    "y": 5.892237203224628,
-                    "heading": -0.00372258471684395,
-                    "angularVelocity": 0.03682052298107744,
-                    "velocityX": 1.72672165311393,
-                    "velocityY": -0.9419993859786789,
-                    "timestamp": 1.786834399221751
-                },
-                {
-                    "x": 7.5553046737134695,
-                    "y": 5.865026578610625,
-                    "heading": -0.002658986847128457,
-                    "angularVelocity": 0.03156042395067696,
-                    "velocityX": 1.4800471285945758,
-                    "velocityY": -0.8074281391808545,
-                    "timestamp": 1.8205347662186326
-                },
-                {
-                    "x": 7.596869782996782,
-                    "y": 5.842351055576096,
-                    "heading": -0.0017726562460022867,
-                    "angularVelocity": 0.026300324895801946,
-                    "velocityX": 1.233372600576068,
-                    "velocityY": -0.6728568575121819,
-                    "timestamp": 1.8542351332155143
-                },
-                {
-                    "x": 7.630121870157635,
-                    "y": 5.82421063520489,
-                    "heading": -0.0010635926829587166,
-                    "angularVelocity": 0.021040232680814546,
-                    "velocityX": 0.9866980725738072,
-                    "velocityY": -0.5382855436821761,
-                    "timestamp": 1.8879355002123959
-                },
-                {
-                    "x": 7.655060935277959,
-                    "y": 5.8106053185140585,
-                    "heading": -0.0005317957806907868,
-                    "angularVelocity": 0.015780151661757422,
-                    "velocityX": 0.7400235470026807,
-                    "velocityY": -0.40371419967297517,
-                    "timestamp": 1.9216358672092775
-                },
-                {
-                    "x": 7.671686978497787,
-                    "y": 5.801535106470048,
-                    "heading": -0.00017726506577651584,
-                    "angularVelocity": 0.010520084690677335,
-                    "velocityX": 0.4933490255867586,
-                    "velocityY": -0.2691428269861255,
-                    "timestamp": 1.955336234206159
+                    "heading": -0.0161310971470982,
+                    "angularVelocity": -2.6064368823146806e-8,
+                    "velocityX": 3.7827999507462917,
+                    "velocityY": 0.032514437989762196,
+                    "timestamp": 1.112827044003246
+                },
+                {
+                    "x": 5.530811605245593,
+                    "y": 6.814532094483614,
+                    "heading": -0.016131098042384447,
+                    "angularVelocity": -2.6566067685833464e-8,
+                    "velocityX": 3.7747775034488793,
+                    "velocityY": -0.2483684519763222,
+                    "timestamp": 1.1465274115659696
+                },
+                {
+                    "x": 5.657050747526861,
+                    "y": 6.796742322973506,
+                    "heading": -0.016131098828254187,
+                    "angularVelocity": -2.3319322572510494e-8,
+                    "velocityX": 3.745927757206515,
+                    "velocityY": -0.5278806374143462,
+                    "timestamp": 1.1802277791286933
+                },
+                {
+                    "x": 5.781621117814577,
+                    "y": 6.76963106353518,
+                    "heading": -0.016131099539274135,
+                    "angularVelocity": -2.109828463295381e-8,
+                    "velocityX": 3.6964098405118033,
+                    "velocityY": -0.8044796362492181,
+                    "timestamp": 1.213928146691417
+                },
+                {
+                    "x": 5.903835409646495,
+                    "y": 6.733347943713289,
+                    "heading": -0.016131100200026813,
+                    "angularVelocity": -1.96066906061588e-8,
+                    "velocityX": 3.626497295747634,
+                    "velocityY": -1.0766386970219277,
+                    "timestamp": 1.2476285142541406
+                },
+                {
+                    "x": 6.023019351333038,
+                    "y": 6.68809326224078,
+                    "heading": -0.016131100829259,
+                    "angularVelocity": -1.86713746487578e-8,
+                    "velocityX": 3.5365769072018636,
+                    "velocityY": -1.3428542400399708,
+                    "timestamp": 1.2813288818168642
+                },
+                {
+                    "x": 6.138515596479519,
+                    "y": 6.634117196875684,
+                    "heading": -0.016131101442406693,
+                    "angularVelocity": -1.81940951537302e-8,
+                    "velocityX": 3.4271509036665004,
+                    "velocityY": -1.601646191681265,
+                    "timestamp": 1.3150292493795879
+                },
+                {
+                    "x": 6.2512259314426855,
+                    "y": 6.5745422130600915,
+                    "heading": -0.016131102051778846,
+                    "angularVelocity": -1.80820624580199e-8,
+                    "velocityX": 3.3444838473464196,
+                    "velocityY": -1.7677843929954529,
+                    "timestamp": 1.3487296169423115
+                },
+                {
+                    "x": 6.363935646775815,
+                    "y": 6.514966056976746,
+                    "heading": -0.01613110266098242,
+                    "angularVelocity": -1.807706019437417e-8,
+                    "velocityX": 3.344465460898959,
+                    "velocityY": -1.7678191780093255,
+                    "timestamp": 1.3824299845050352
+                },
+                {
+                    "x": 6.4758517936606514,
+                    "y": 6.453912192428435,
+                    "heading": -0.016131102003666662,
+                    "angularVelocity": 1.950470585863807e-8,
+                    "velocityX": 3.320917692560386,
+                    "velocityY": -1.811667615632877,
+                    "timestamp": 1.4161303520677588
+                },
+                {
+                    "x": 6.587767622018337,
+                    "y": 6.392857744000494,
+                    "heading": -0.016131101346154864,
+                    "angularVelocity": 1.951052313957626e-8,
+                    "velocityX": 3.32090824081921,
+                    "velocityY": -1.8116849412489389,
+                    "timestamp": 1.4498307196304825
+                },
+                {
+                    "x": 6.699683405881235,
+                    "y": 6.331803214011495,
+                    "heading": -0.016131100688641712,
+                    "angularVelocity": 1.9510563155904532e-8,
+                    "velocityX": 3.3209069205135386,
+                    "velocityY": -1.811687361432026,
+                    "timestamp": 1.4835310871932061
+                },
+                {
+                    "x": 6.811599167638959,
+                    "y": 6.270748643502715,
+                    "heading": -0.016131100031139815,
+                    "angularVelocity": 1.9510229240352237e-8,
+                    "velocityX": 3.320906264580792,
+                    "velocityY": -1.8116885637862563,
+                    "timestamp": 1.5172314547559298
+                },
+                {
+                    "x": 6.923514914598752,
+                    "y": 6.209694045868981,
+                    "heading": -0.016131099373601954,
+                    "angularVelocity": 1.95112965056638e-8,
+                    "velocityX": 3.3209058254778503,
+                    "velocityY": -1.8116893686722635,
+                    "timestamp": 1.5509318223186535
+                },
+                {
+                    "x": 7.031584175346113,
+                    "y": 6.1507377884047765,
+                    "heading": -0.013826704963615632,
+                    "angularVelocity": 0.0683789102803133,
+                    "velocityX": 3.2067680136194574,
+                    "velocityY": -1.7494247608568367,
+                    "timestamp": 1.584632189881377
+                },
+                {
+                    "x": 7.131340439149152,
+                    "y": 6.096316611291437,
+                    "heading": -0.011699535798216875,
+                    "angularVelocity": 0.06312005830317652,
+                    "velocityX": 2.960094236876553,
+                    "velocityY": -1.614854111369865,
+                    "timestamp": 1.6183325574441008
+                },
+                {
+                    "x": 7.222783690004588,
+                    "y": 6.046430524071638,
+                    "heading": -0.00974961791861361,
+                    "angularVelocity": 0.057860433598327515,
+                    "velocityX": 2.713419985263968,
+                    "velocityY": -1.4802831787205646,
+                    "timestamp": 1.6520329250068244
+                },
+                {
+                    "x": 7.305913922772626,
+                    "y": 6.001079530283105,
+                    "heading": -0.00797695998479322,
+                    "angularVelocity": 0.05260055192336173,
+                    "velocityX": 2.466745581136908,
+                    "velocityY": -1.3457121410953312,
+                    "timestamp": 1.685733292569548
+                },
+                {
+                    "x": 7.380731135011445,
+                    "y": 5.960263631930958,
+                    "heading": -0.00638156583911186,
+                    "angularVelocity": 0.04734055623313652,
+                    "velocityX": 2.2200711045530213,
+                    "velocityY": -1.2111410439717167,
+                    "timestamp": 1.7194336601322717
+                },
+                {
+                    "x": 7.447235325348076,
+                    "y": 5.923982830388488,
+                    "heading": -0.004963437343393232,
+                    "angularVelocity": 0.042080505296519764,
+                    "velocityX": 1.9733965872286794,
+                    "velocityY": -1.0765699060980205,
+                    "timestamp": 1.7531340276949954
+                },
+                {
+                    "x": 7.505426492937412,
+                    "y": 5.892237126701046,
+                    "heading": -0.0037225753686417596,
+                    "angularVelocity": 0.0368204285143562,
+                    "velocityX": 1.72672204482727,
+                    "velocityY": -0.9419987372053683,
+                    "timestamp": 1.786834395257719
+                },
+                {
+                    "x": 7.555304637231595,
+                    "y": 5.8650265217185185,
+                    "heading": -0.002658980227466916,
+                    "angularVelocity": 0.03156034245604838,
+                    "velocityX": 1.4800474861690802,
+                    "velocityY": -0.8074275430937743,
+                    "timestamp": 1.8205347628204427
+                },
+                {
+                    "x": 7.596869757865521,
+                    "y": 5.842351016163066,
+                    "heading": -0.0017726518914738514,
+                    "angularVelocity": 0.02630025724030852,
+                    "velocityX": 1.233372916677093,
+                    "velocityY": -0.6728563275533557,
+                    "timestamp": 1.8542351303831663
+                },
+                {
+                    "x": 7.630121854593904,
+                    "y": 5.824210610667954,
+                    "heading": -0.001063590116536395,
+                    "angularVelocity": 0.021040179268589657,
+                    "velocityX": 0.9866983399066315,
+                    "velocityY": -0.5382850932218669,
+                    "timestamp": 1.88793549794589
+                },
+                {
+                    "x": 7.655060927254085,
+                    "y": 5.810605305801891,
+                    "heading": -0.000531794525727047,
+                    "angularVelocity": 0.01578011248154109,
+                    "velocityX": 0.7400237583095566,
+                    "velocityY": -0.4037138420149424,
+                    "timestamp": 1.9216358655086136
+                },
+                {
+                    "x": 7.671686975742638,
+                    "y": 5.8015351020850225,
+                    "heading": -0.00017726465841382563,
+                    "angularVelocity": 0.010520059362948236,
+                    "velocityX": 0.49334917364355607,
+                    "velocityY": -0.26914257537363395,
+                    "timestamp": 1.9553362330713373
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.986792057163996e-31,
-                    "angularVelocity": 0.005260033690221658,
-                    "velocityX": 0.24667450959752207,
-                    "velocityY": -0.13457142678797143,
-                    "timestamp": 1.9890366012030407
+                    "heading": 3.6111480760753294e-31,
+                    "angularVelocity": 0.005260021514121506,
+                    "velocityX": 0.24667458721003926,
+                    "velocityY": -0.1345712944104092,
+                    "timestamp": 1.989036600634061
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 0,
-                    "angularVelocity": -1.0163368054249497e-29,
-                    "velocityX": -1.0830525743862323e-33,
-                    "velocityY": 5.9205016331337416e-33,
-                    "timestamp": 2.022736968199922
+                    "heading": -8.452347473573201e-32,
+                    "angularVelocity": -1.5098225699671103e-29,
+                    "velocityX": 2.1451735090517096e-32,
+                    "velocityY": 9.12030277003484e-33,
+                    "timestamp": 2.0227369681967846
                 }
             ],
             "constraints": [
@@ -14441,16 +15851,6 @@
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
             "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
                 {
                     "x": 5.61,
                     "y": 5.44,
@@ -16464,145 +17864,145 @@
                     "x": 2.3,
                     "y": 4.36,
                     "heading": -0.39,
-                    "angularVelocity": -1.4895643422557431e-33,
-                    "velocityX": -9.369180406325922e-35,
+                    "angularVelocity": 1.4326217036715317e-32,
+                    "velocityX": -1.74824017803789e-33,
                     "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.300137763138577,
-                    "y": 4.403828106227067,
-                    "heading": -0.362912658197764,
-                    "angularVelocity": 0.3708979956671865,
-                    "velocityX": 0.001886345007500309,
-                    "velocityY": 0.6001237357356949,
-                    "timestamp": 0.07303178264285357
-                },
-                {
-                    "x": 2.3004080637884394,
-                    "y": 4.491572820860884,
-                    "heading": -0.31063890006100675,
-                    "angularVelocity": 0.7157672487934603,
-                    "velocityX": 0.0037011372320465844,
-                    "velocityY": 1.201459302491822,
-                    "timestamp": 0.14606356528570713
-                },
-                {
-                    "x": 2.3008080200629966,
-                    "y": 4.623360729086378,
-                    "heading": -0.2362185458103229,
-                    "angularVelocity": 1.019013250910508,
-                    "velocityX": 0.005476468738453403,
-                    "velocityY": 1.8045281582400208,
-                    "timestamp": 0.2190953479285607
-                },
-                {
-                    "x": 2.3013470975241233,
-                    "y": 4.799377996239057,
-                    "heading": -0.1451530728621028,
-                    "angularVelocity": 1.2469293457282353,
-                    "velocityX": 0.0073814090471083236,
-                    "velocityY": 2.4101461142398,
-                    "timestamp": 0.29212713057141426
-                },
-                {
-                    "x": 2.30206992781241,
-                    "y": 5.019849097252827,
-                    "heading": -0.0495237523149135,
-                    "angularVelocity": 1.3094205986295608,
-                    "velocityX": 0.009897475621284645,
-                    "velocityY": 3.0188377311277894,
-                    "timestamp": 0.36515891321426786
-                },
-                {
-                    "x": 2.303151073626671,
-                    "y": 5.282690240833408,
-                    "heading": -0.004114697333369339,
-                    "angularVelocity": 0.621771143169372,
-                    "velocityX": 0.014803771387434566,
-                    "velocityY": 3.598996684305928,
-                    "timestamp": 0.43819069585712145
-                },
-                {
-                    "x": 2.3035206151085394,
-                    "y": 5.5589648754006,
-                    "heading": -0.004114668066309664,
-                    "angularVelocity": 4.007441502253877e-7,
-                    "velocityX": 0.005060009060371044,
-                    "velocityY": 3.7829370250792667,
-                    "timestamp": 0.511222478499975
-                },
-                {
-                    "x": 2.3038901811674446,
-                    "y": 5.835239509934909,
-                    "heading": -0.0041146387992405135,
-                    "angularVelocity": 4.0074427997802584e-7,
-                    "velocityX": 0.0050603455856026045,
-                    "velocityY": 3.7829370246290135,
-                    "timestamp": 0.5842542611428286
-                },
-                {
-                    "x": 2.3020054253248463,
-                    "y": 6.097682298903223,
-                    "heading": 0.0412179398919569,
-                    "angularVelocity": 0.6207239786667508,
-                    "velocityX": -0.02580733722214711,
-                    "velocityY": 3.593542146598477,
-                    "timestamp": 0.6572860437856822
-                },
-                {
-                    "x": 2.301170577421719,
-                    "y": 6.318019035246094,
-                    "heading": 0.13972641476186656,
-                    "angularVelocity": 1.3488439047372631,
-                    "velocityX": -0.011431295703265835,
-                    "velocityY": 3.016997920212089,
-                    "timestamp": 0.7303178264285358
-                },
-                {
-                    "x": 2.3006511827054728,
-                    "y": 6.4939017376421555,
-                    "heading": 0.2337896338628955,
-                    "angularVelocity": 1.2879764904688844,
-                    "velocityX": -0.007111899743518039,
-                    "velocityY": 2.4083035636166743,
-                    "timestamp": 0.8033496090713894
-                },
-                {
-                    "x": 2.300310606012852,
-                    "y": 6.62557002983581,
-                    "heading": 0.3107349410237032,
-                    "angularVelocity": 1.0535865944433043,
-                    "velocityX": -0.004663403798945386,
-                    "velocityY": 1.8028902955518677,
-                    "timestamp": 0.876381391714243
-                },
-                {
-                    "x": 2.30010034874232,
-                    "y": 6.713223552423264,
-                    "heading": 0.36487146716810653,
-                    "angularVelocity": 0.741273513877466,
-                    "velocityX": -0.002878983134789396,
-                    "velocityY": 1.2002106400182577,
-                    "timestamp": 0.9494131743570966
+                    "x": 2.3001377838932537,
+                    "y": 4.403828106793783,
+                    "heading": -0.3629126711329447,
+                    "angularVelocity": 0.37089781855009624,
+                    "velocityX": 0.0018866291944089578,
+                    "velocityY": 0.600123743495633,
+                    "timestamp": 0.07303178264284323
+                },
+                {
+                    "x": 2.300408122825978,
+                    "y": 4.49157282256222,
+                    "heading": -0.3106389398567459,
+                    "angularVelocity": 0.7157668810008344,
+                    "velocityX": 0.0037016614265912673,
+                    "velocityY": 1.2014593180279707,
+                    "timestamp": 0.14606356528568645
+                },
+                {
+                    "x": 2.3008081314200264,
+                    "y": 4.623360732427042,
+                    "heading": -0.2362186265841639,
+                    "angularVelocity": 1.019012689811082,
+                    "velocityX": 0.005477185131915077,
+                    "velocityY": 1.8045281806870437,
+                    "timestamp": 0.2190953479285297
+                },
+                {
+                    "x": 2.301347271135313,
+                    "y": 4.79937800153835,
+                    "heading": -0.14515320669009546,
+                    "angularVelocity": 1.2469286192754925,
+                    "velocityX": 0.007382261472700921,
+                    "velocityY": 2.410146141058999,
+                    "timestamp": 0.2921271305713729
+                },
+                {
+                    "x": 2.302070167703843,
+                    "y": 5.019849104537176,
+                    "heading": -0.04952394454697364,
+                    "angularVelocity": 1.3094197989222025,
+                    "velocityX": 0.009898383174695338,
+                    "velocityY": 3.018837758308937,
+                    "timestamp": 0.3651589132142161
+                },
+                {
+                    "x": 2.3031513688730865,
+                    "y": 5.282690253403799,
+                    "heading": -0.004114907781753178,
+                    "angularVelocity": 0.6217708937393756,
+                    "velocityX": 0.014804529344862738,
+                    "velocityY": 3.598996756686451,
+                    "timestamp": 0.43819069585705933
+                },
+                {
+                    "x": 2.303520931398292,
+                    "y": 5.5589648879428,
+                    "heading": -0.0041148785146688655,
+                    "angularVelocity": 4.00744487575221e-7,
+                    "velocityX": 0.005060297199824665,
+                    "velocityY": 3.7829370246937892,
+                    "timestamp": 0.5112224784999025
+                },
+                {
+                    "x": 2.3038904865521834,
+                    "y": 5.835239522491655,
+                    "heading": -0.004114849247579104,
+                    "angularVelocity": 4.0074456218901027e-7,
+                    "velocityX": 0.005060196266858216,
+                    "velocityY": 3.7829370248287195,
+                    "timestamp": 0.5842542611427458
+                },
+                {
+                    "x": 2.302005656251481,
+                    "y": 6.097682306820458,
+                    "heading": 0.0412177605138735,
+                    "angularVelocity": 0.6207244041015477,
+                    "velocityX": -0.025808356752290516,
+                    "velocityY": 3.5935420830717044,
+                    "timestamp": 0.6572860437855891
+                },
+                {
+                    "x": 2.3011707390613565,
+                    "y": 6.318019040898657,
+                    "heading": 0.13972629361592653,
+                    "angularVelocity": 1.3488447020908976,
+                    "velocityX": -0.01143224442716972,
+                    "velocityY": 3.016997889203128,
+                    "timestamp": 0.7303178264284323
+                },
+                {
+                    "x": 2.3006512841511224,
+                    "y": 6.493901741171369,
+                    "heading": 0.23378956116370217,
+                    "angularVelocity": 1.2879771538343168,
+                    "velocityX": -0.007112723959845375,
+                    "velocityY": 2.408303534542668,
+                    "timestamp": 0.8033496090712756
+                },
+                {
+                    "x": 2.300310658917655,
+                    "y": 6.625570031648912,
+                    "heading": 0.310734904704345,
+                    "angularVelocity": 1.053587092580481,
+                    "velocityX": -0.00466406845267762,
+                    "velocityY": 1.802890272053981,
+                    "timestamp": 0.8763813917141189
+                },
+                {
+                    "x": 2.3001003670918414,
+                    "y": 6.7132235530415985,
+                    "heading": 0.3648714550351714,
+                    "angularVelocity": 0.7412738450542471,
+                    "velocityX": -0.0028794562888088465,
+                    "velocityY": 1.2002106236588805,
+                    "timestamp": 0.9494131743569622
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.393,
-                    "angularVelocity": 0.38515467942840836,
-                    "velocityX": -0.0013740420771443632,
-                    "velocityY": 0.5994163909542732,
-                    "timestamp": 1.02244495699995
+                    "angularVelocity": 0.38515484556072405,
+                    "velocityX": -0.0013742933310585549,
+                    "velocityY": 0.5994163824877027,
+                    "timestamp": 1.0224449569998053
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.393,
-                    "angularVelocity": 8.681406116042979e-33,
-                    "velocityX": 1.0196837521674476e-33,
+                    "angularVelocity": 2.8585488767662063e-32,
+                    "velocityX": -1.0732467562704116e-31,
                     "velocityY": 0,
-                    "timestamp": 1.0954767396428036
+                    "timestamp": 1.0954767396426486
                 }
             ],
             "constraints": [
@@ -16627,16 +18027,6 @@
                     "x": 3.42,
                     "y": 4.04,
                     "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
                 }
             ]
         },
@@ -16667,99 +18057,99 @@
                     "y": 4.36,
                     "heading": -0.39,
                     "angularVelocity": 0,
-                    "velocityX": 1.7319348109741582e-34,
-                    "velocityY": -4.404349434317184e-36,
+                    "velocityX": 2.0150450292109907e-34,
+                    "velocityY": 3.039389955204995e-35,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.298001242299744,
-                    "y": 4.408001442881322,
-                    "heading": -0.3744767722715256,
-                    "angularVelocity": 0.20408758634762672,
-                    "velocityX": -0.02627814536217338,
-                    "velocityY": 0.6310864460799332,
-                    "timestamp": 0.07606159691669512
+                    "x": 2.2980012520823867,
+                    "y": 4.4080014432651184,
+                    "heading": -0.3744767713085023,
+                    "angularVelocity": 0.2040875990087458,
+                    "velocityX": -0.02627801674742958,
+                    "velocityY": 0.6310864511258738,
+                    "timestamp": 0.07606159691668693
                 },
                 {
-                    "x": 2.2940037533773614,
-                    "y": 4.504004077466311,
-                    "heading": -0.3434155460751892,
-                    "angularVelocity": 0.4083693671374729,
-                    "velocityX": -0.05255594261004741,
-                    "velocityY": 1.26216958986719,
-                    "timestamp": 0.15212319383339024
+                    "x": 2.294003780157748,
+                    "y": 4.504004078538053,
+                    "heading": -0.34341554427294535,
+                    "angularVelocity": 0.40836937817095076,
+                    "velocityX": -0.05255571913664827,
+                    "velocityY": 1.262169598911913,
+                    "timestamp": 0.15212319383337386
                 },
                 {
-                    "x": 2.2880074570982525,
-                    "y": 4.648007443871442,
-                    "heading": -0.29679113248213845,
-                    "angularVelocity": 0.6129823127972874,
-                    "velocityX": -0.07883474081771351,
-                    "velocityY": 1.8932466874557927,
-                    "timestamp": 0.22818479075008535
+                    "x": 2.2880075055108158,
+                    "y": 4.648007445856594,
+                    "heading": -0.296791131111893,
+                    "angularVelocity": 0.6129823071177671,
+                    "velocityX": -0.07883445641432706,
+                    "velocityY": 1.8932466994648223,
+                    "timestamp": 0.2281847907500608
                 },
                 {
-                    "x": 2.2800120385493474,
-                    "y": 4.8400108934587145,
-                    "heading": -0.2345694550660327,
-                    "angularVelocity": 0.8180432693814297,
-                    "velocityX": -0.10511767926279252,
-                    "velocityY": 2.524315257245527,
-                    "timestamp": 0.30424638766678047
+                    "x": 2.2800121106149036,
+                    "y": 4.840010896492559,
+                    "heading": -0.2345694561462187,
+                    "angularVelocity": 0.8180432371651087,
+                    "velocityX": -0.10511736829125365,
+                    "velocityY": 2.5243152710332057,
+                    "timestamp": 0.3042463876667477
                 },
                 {
-                    "x": 2.270016765601229,
-                    "y": 5.0800136328565335,
-                    "heading": -0.15671111441894314,
-                    "angularVelocity": 1.0236222194014972,
-                    "velocityX": -0.1314102431883619,
-                    "velocityY": 3.1553733963892037,
-                    "timestamp": 0.3803079845834756
+                    "x": 2.270016860554186,
+                    "y": 5.080013636963748,
+                    "heading": -0.15671112019223,
+                    "angularVelocity": 1.023622157700289,
+                    "velocityX": -0.13140994228224737,
+                    "velocityY": 3.1553734105013973,
+                    "timestamp": 0.3803079845834346
                 },
                 {
-                    "x": 2.2620105775052464,
-                    "y": 5.272008323301556,
-                    "heading": -0.09403333232435188,
-                    "angularVelocity": 0.8240397866381615,
-                    "velocityX": -0.10525926907308407,
-                    "velocityY": 2.524200098708164,
-                    "timestamp": 0.4563695815001707
+                    "x": 2.2620106394348882,
+                    "y": 5.2720083260038075,
+                    "heading": -0.09403333701953483,
+                    "angularVelocity": 0.82403980081234,
+                    "velocityX": -0.10525970323851294,
+                    "velocityY": 2.524200080237073,
+                    "timestamp": 0.4563695815001215
                 },
                 {
-                    "x": 2.256005419001405,
-                    "y": 5.416004186972581,
-                    "heading": -0.04701785122811225,
-                    "angularVelocity": 0.6181237707608528,
-                    "velocityX": -0.07895125460510179,
-                    "velocityY": 1.8931480472166962,
-                    "timestamp": 0.5324311784168658
+                    "x": 2.256005452365101,
+                    "y": 5.416004188429854,
+                    "heading": -0.0470178538159679,
+                    "angularVelocity": 0.6181237984664553,
+                    "velocityX": -0.07895163016843816,
+                    "velocityY": 1.8931480308488688,
+                    "timestamp": 0.5324311784168084
                 },
                 {
-                    "x": 2.252001808264489,
-                    "y": 5.512001394535746,
-                    "heading": -0.015672604394819867,
-                    "angularVelocity": 0.4121034543571656,
-                    "velocityX": -0.05263642756936803,
-                    "velocityY": 1.262098239513749,
-                    "timestamp": 0.6084927753335609
+                    "x": 2.2520018201736485,
+                    "y": 5.5120013950535185,
+                    "heading": -0.01567260522346991,
+                    "angularVelocity": 0.4121034774859067,
+                    "velocityX": -0.05263670963729669,
+                    "velocityY": 1.262098227162051,
+                    "timestamp": 0.6084927753334953
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": -1.7090620600656545e-34,
-                    "angularVelocity": 0.2060514770940842,
-                    "velocityX": -0.026318251859498508,
-                    "velocityY": 0.6310491418793501,
-                    "timestamp": 0.6845543722502561
+                    "heading": 1.3656663814986182e-35,
+                    "angularVelocity": 0.20605148798856665,
+                    "velocityX": -0.026318408432062,
+                    "velocityY": 0.6310491350721452,
+                    "timestamp": 0.6845543722501822
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
                     "heading": 0,
-                    "angularVelocity": 2.0052459094930362e-33,
-                    "velocityX": -1.9224144442861376e-33,
-                    "velocityY": 1.2850315433355203e-34,
-                    "timestamp": 0.7606159691669512
+                    "angularVelocity": 6.6551444751621536e-34,
+                    "velocityX": 3.9479513180483775e-34,
+                    "velocityY": 0,
+                    "timestamp": 0.7606159691668691
                 }
             ],
             "constraints": [
@@ -16784,16 +18174,6 @@
                     "x": 3.42,
                     "y": 4.04,
                     "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
                 }
             ]
         },
@@ -16823,172 +18203,172 @@
                     "x": 2.3,
                     "y": 4.36,
                     "heading": -0.39,
-                    "angularVelocity": 8.834598343952546e-30,
-                    "velocityX": -1.1855672444586933e-30,
-                    "velocityY": 4.732277944627028e-32,
+                    "angularVelocity": 5.020065556819226e-31,
+                    "velocityX": -2.911156118414847e-31,
+                    "velocityY": 8.635491730351355e-32,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.3221690172235006,
-                    "y": 4.411229758092553,
-                    "heading": -0.4048175852865869,
-                    "angularVelocity": -0.18084977247894177,
-                    "velocityX": 0.27057456686796094,
-                    "velocityY": 0.6252631529349166,
-                    "timestamp": 0.08193311544440174
-                },
-                {
-                    "x": 2.3687303268224738,
-                    "y": 4.512732101891575,
-                    "heading": -0.43137596230210584,
-                    "angularVelocity": -0.3241470420293335,
-                    "velocityX": 0.5682843786231674,
-                    "velocityY": 1.2388439429951275,
-                    "timestamp": 0.16386623088880348
-                },
-                {
-                    "x": 2.4431953900618826,
-                    "y": 4.662720774571155,
-                    "heading": -0.464675218076173,
-                    "angularVelocity": -0.40641998773593985,
-                    "velocityX": 0.9088518462346009,
-                    "velocityY": 1.8306233305795314,
-                    "timestamp": 0.24579934633320522
-                },
-                {
-                    "x": 2.551739981424586,
-                    "y": 4.8571070042037645,
-                    "heading": -0.49532439278782164,
-                    "angularVelocity": -0.3740755437574734,
-                    "velocityX": 1.3247951182371547,
-                    "velocityY": 2.3724989410968234,
-                    "timestamp": 0.32773246177760695
-                },
-                {
-                    "x": 2.7012931711639805,
-                    "y": 5.089437783878959,
-                    "heading": -0.5138025644966426,
-                    "angularVelocity": -0.22552751239337346,
-                    "velocityX": 1.8253082276711035,
-                    "velocityY": 2.835615104039969,
-                    "timestamp": 0.4096655772220087
-                },
-                {
-                    "x": 2.906580390144238,
-                    "y": 5.321655181575939,
-                    "heading": -0.513802224550241,
-                    "angularVelocity": 0.0000041490720786488115,
-                    "velocityX": 2.5055463577429817,
-                    "velocityY": 2.834231268241715,
-                    "timestamp": 0.49159869266641043
-                },
-                {
-                    "x": 3.150285117410734,
-                    "y": 5.513164499731697,
-                    "heading": -0.5138021244683586,
-                    "angularVelocity": 0.0000012215071026742433,
-                    "velocityX": 2.974435012566654,
-                    "velocityY": 2.3373860143974894,
-                    "timestamp": 0.5735318081108122
-                },
-                {
-                    "x": 3.407534970443545,
-                    "y": 5.686052692352735,
-                    "heading": -0.513802030650944,
-                    "angularVelocity": 0.0000011450487881316094,
-                    "velocityX": 3.1397543183546524,
-                    "velocityY": 2.110113739521574,
-                    "timestamp": 0.6554649235552139
-                },
-                {
-                    "x": 3.664784970537928,
-                    "y": 5.858940666152602,
-                    "heading": -0.5138019368336011,
-                    "angularVelocity": 0.0000011450479143905032,
-                    "velocityX": 3.1397561132524885,
-                    "velocityY": 2.1101110687922637,
-                    "timestamp": 0.7373980389996156
-                },
-                {
-                    "x": 3.922035018762957,
-                    "y": 6.031828568336093,
-                    "heading": -0.5138018430162984,
-                    "angularVelocity": 0.0000011450474228928023,
-                    "velocityX": 3.139756700690747,
-                    "velocityY": 2.110110194708886,
-                    "timestamp": 0.8193311544440174
-                },
-                {
-                    "x": 4.1792852160642955,
-                    "y": 6.204716248700005,
-                    "heading": -0.5138017491990241,
-                    "angularVelocity": 0.0000011450470770765712,
-                    "velocityX": 3.1397585201786073,
-                    "velocityY": 2.1101074873837833,
-                    "timestamp": 0.9012642698884191
-                },
-                {
-                    "x": 4.443584051036782,
-                    "y": 6.354085007666646,
-                    "heading": -0.49783246108316886,
-                    "angularVelocity": 0.19490639443210175,
-                    "velocityX": 3.225787687175579,
-                    "velocityY": 1.8230572358499022,
-                    "timestamp": 0.9831973853328209
-                },
-                {
-                    "x": 4.674708985410259,
-                    "y": 6.469317132198814,
-                    "heading": -0.3712043024716245,
-                    "angularVelocity": 1.545506452729414,
-                    "velocityX": 2.8208976690299576,
-                    "velocityY": 1.4064169769104102,
-                    "timestamp": 1.0651305007772227
-                },
-                {
-                    "x": 4.870816846139177,
-                    "y": 6.548974010532027,
-                    "heading": -0.15522222327575852,
-                    "angularVelocity": 2.636077952417564,
-                    "velocityX": 2.393511581553284,
-                    "velocityY": 0.9722183503111966,
-                    "timestamp": 1.1470636162216246
-                },
-                {
-                    "x": 5.022935939261114,
-                    "y": 6.594453280726005,
-                    "heading": 0.041233837030506995,
-                    "angularVelocity": 2.3977613842791428,
-                    "velocityX": 1.856625276566729,
-                    "velocityY": 0.5550780041415454,
-                    "timestamp": 1.2289967316660264
-                },
-                {
-                    "x": 5.124959991715153,
-                    "y": 6.619307482178149,
-                    "heading": 0.1862409016069847,
-                    "angularVelocity": 1.7698224190545027,
-                    "velocityX": 1.245211437410438,
-                    "velocityY": 0.30334744769969435,
-                    "timestamp": 1.3109298471104283
+                    "x": 2.3221690164859408,
+                    "y": 4.411229757983623,
+                    "heading": -0.4048176043167742,
+                    "angularVelocity": -0.1808500047435309,
+                    "velocityX": 0.27057455786553103,
+                    "velocityY": 0.6252631516043967,
+                    "timestamp": 0.08193311544453755
+                },
+                {
+                    "x": 2.3687303275118676,
+                    "y": 4.5127321003230465,
+                    "heading": -0.4313760168590284,
+                    "angularVelocity": -0.32414747563532126,
+                    "velocityX": 0.5682843960383053,
+                    "velocityY": 1.2388439251785637,
+                    "timestamp": 0.1638662308890751
+                },
+                {
+                    "x": 2.4431953998066236,
+                    "y": 4.662720767025029,
+                    "heading": -0.4646753197408115,
+                    "angularVelocity": -0.40642056268856935,
+                    "velocityX": 0.9088519567543129,
+                    "velocityY": 1.830623257619443,
+                    "timestamp": 0.24579934633361264
+                },
+                {
+                    "x": 2.551740018878988,
+                    "y": 4.857106976368847,
+                    "heading": -0.4953245435557511,
+                    "angularVelocity": -0.3740761430663416,
+                    "velocityX": 1.3247954564335083,
+                    "velocityY": 2.3724986934666186,
+                    "timestamp": 0.3277324617781502
+                },
+                {
+                    "x": 2.701293241756868,
+                    "y": 5.089437731072216,
+                    "heading": -0.5138028115710582,
+                    "angularVelocity": -0.2255286878210994,
+                    "velocityX": 1.8253086321258618,
+                    "velocityY": 2.835614799252222,
+                    "timestamp": 0.4096655772226877
+                },
+                {
+                    "x": 2.9065804913658706,
+                    "y": 5.321655101693131,
+                    "heading": -0.5138024716246538,
+                    "angularVelocity": 0.000004149072112901325,
+                    "velocityX": 2.5055467315650244,
+                    "velocityY": 2.8342309377715424,
+                    "timestamp": 0.4915986926672252
+                },
+                {
+                    "x": 3.1502852438919966,
+                    "y": 5.513164387705616,
+                    "heading": -0.5138023715427595,
+                    "angularVelocity": 0.0000012215072474193347,
+                    "velocityX": 2.974435320857471,
+                    "velocityY": 2.3373856220824885,
+                    "timestamp": 0.5735318081117627
+                },
+                {
+                    "x": 3.4075351073224858,
+                    "y": 5.686052564856294,
+                    "heading": -0.5138022777253332,
+                    "angularVelocity": 0.0000011450489327031063,
+                    "velocityX": 3.1397544452539083,
+                    "velocityY": 2.110113550701131,
+                    "timestamp": 0.6554649235563003
+                },
+                {
+                    "x": 3.664785106053728,
+                    "y": 5.858940540685368,
+                    "heading": -0.5138021839079745,
+                    "angularVelocity": 0.0000011450481052658978,
+                    "velocityX": 3.13975609661006,
+                    "velocityY": 2.110111093555414,
+                    "timestamp": 0.7373980390008378
+                },
+                {
+                    "x": 3.922035141067721,
+                    "y": 6.031828462527248,
+                    "heading": -0.5138020900906538,
+                    "angularVelocity": 0.000001145047641454271,
+                    "velocityX": 3.1397565394438236,
+                    "velocityY": 2.1101104346375243,
+                    "timestamp": 0.8193311544453753
+                },
+                {
+                    "x": 4.179285313417939,
+                    "y": 6.204716180018366,
+                    "heading": -0.5138019962733545,
+                    "angularVelocity": 0.00000114504738127865,
+                    "velocityX": 3.139758215643051,
+                    "velocityY": 2.110107940520708,
+                    "timestamp": 0.9012642698899128
+                },
+                {
+                    "x": 4.443584109358565,
+                    "y": 6.354084975447565,
+                    "heading": -0.4978326665645833,
+                    "angularVelocity": 0.19490690207652947,
+                    "velocityX": 3.225787210783358,
+                    "velocityY": 1.8230576808752068,
+                    "timestamp": 0.9831973853344503
+                },
+                {
+                    "x": 4.674709010100242,
+                    "y": 6.4693171293955105,
+                    "heading": -0.3712044647358083,
+                    "angularVelocity": 1.5455069801964878,
+                    "velocityX": 2.8208972585465766,
+                    "velocityY": 1.406417335929929,
+                    "timestamp": 1.065130500778988
+                },
+                {
+                    "x": 4.870816853917273,
+                    "y": 6.548974021375514,
+                    "heading": -0.15522232897542676,
+                    "angularVelocity": 2.636078642787472,
+                    "velocityX": 2.3935113751384223,
+                    "velocityY": 0.9722185168697003,
+                    "timestamp": 1.1470636162235255
+                },
+                {
+                    "x": 5.022935942406859,
+                    "y": 6.594453289671109,
+                    "heading": 0.041233782182786366,
+                    "angularVelocity": 2.3977620049271122,
+                    "velocityX": 1.856625220025449,
+                    "velocityY": 0.5550779809707214,
+                    "timestamp": 1.228996731668063
+                },
+                {
+                    "x": 5.124959992690689,
+                    "y": 6.619307485763033,
+                    "heading": 0.18624088374392397,
+                    "angularVelocity": 1.769822870452196,
+                    "velocityX": 1.2452114109208137,
+                    "velocityY": 0.3033473822772884,
+                    "timestamp": 1.3109298471126005
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
                     "heading": 0.263,
-                    "angularVelocity": 0.9368507224054672,
-                    "velocityX": 0.6229472418814781,
-                    "velocityY": 0.13050300557783806,
-                    "timestamp": 1.39286296255483
+                    "angularVelocity": 0.9368509404239479,
+                    "velocityX": 0.6229472299739557,
+                    "velocityY": 0.13050296182384175,
+                    "timestamp": 1.392862962557138
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
                     "heading": 0.263,
-                    "angularVelocity": 1.6488266469177426e-31,
+                    "angularVelocity": 1.5668772442975024e-30,
                     "velocityX": 0,
-                    "velocityY": 1.9617182542723147e-31,
-                    "timestamp": 1.474796077999232
+                    "velocityY": -2.8584411322391187e-31,
+                    "timestamp": 1.4747960780016756
                 }
             ],
             "constraints": [
@@ -17014,11 +18394,6 @@
                     "y": 4.04,
                     "radius": 0.5
                 },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
                 {
                     "x": 5.61,
                     "y": 5.44,
@@ -17053,126 +18428,126 @@
                     "y": 4.36,
                     "heading": -0.39,
                     "angularVelocity": 0,
-                    "velocityX": -1.8814689011962518e-30,
-                    "velocityY": -8.215910483342913e-30,
+                    "velocityX": 0,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.3223907686912884,
-                    "y": 4.418261357303619,
-                    "heading": -0.4292195342409252,
-                    "angularVelocity": -0.4499474209247965,
-                    "velocityX": 0.25687884418209317,
-                    "velocityY": 0.6684053741511637,
-                    "timestamp": 0.08716470506779908
-                },
-                {
-                    "x": 2.37490967527786,
-                    "y": 4.531319644696227,
-                    "heading": -0.5050759172241152,
-                    "angularVelocity": -0.8702648959138656,
-                    "velocityX": 0.6025249158558101,
-                    "velocityY": 1.2970649909807879,
-                    "timestamp": 0.17432941013559816
-                },
-                {
-                    "x": 2.4692607215165214,
-                    "y": 4.691180973762412,
-                    "heading": -0.6116326818995184,
-                    "angularVelocity": -1.222476053725161,
-                    "velocityX": 1.0824455399151758,
-                    "velocityY": 1.8340144550691926,
-                    "timestamp": 0.26149411520339727
-                },
-                {
-                    "x": 2.6203318015659547,
-                    "y": 4.878827771954509,
-                    "heading": -0.7357227336999919,
-                    "angularVelocity": -1.4236272778523056,
-                    "velocityX": 1.7331680286410147,
-                    "velocityY": 2.1527841807775245,
-                    "timestamp": 0.3486588202711963
-                },
-                {
-                    "x": 2.8323811449649123,
-                    "y": 5.083303607446834,
-                    "heading": -0.8475943342447533,
-                    "angularVelocity": -1.2834506863500095,
-                    "velocityX": 2.432743198454233,
-                    "velocityY": 2.3458558751879917,
-                    "timestamp": 0.4358235253389954
-                },
-                {
-                    "x": 3.0986117768734247,
-                    "y": 5.267387942263234,
-                    "heading": -0.8666588377121885,
-                    "angularVelocity": -0.2187181549298817,
-                    "velocityX": 3.054339846632081,
-                    "velocityY": 2.111913700312692,
-                    "timestamp": 0.5229882304067944
-                },
-                {
-                    "x": 3.3974594561484373,
-                    "y": 5.39777666903363,
-                    "heading": -0.8564935513214242,
-                    "angularVelocity": 0.11662158878249514,
-                    "velocityX": 3.428540015623993,
-                    "velocityY": 1.4958890375292995,
-                    "timestamp": 0.6101529354745935
-                },
-                {
-                    "x": 3.678181440932401,
-                    "y": 5.472509264787258,
-                    "heading": -0.7581228056155188,
-                    "angularVelocity": 1.1285616767634317,
-                    "velocityX": 3.2205923781375785,
-                    "velocityY": 0.8573722092617639,
-                    "timestamp": 0.6973176405423925
-                },
-                {
-                    "x": 3.9150331223150032,
-                    "y": 5.514676359956983,
-                    "heading": -0.5395207483783628,
-                    "angularVelocity": 2.507919427560975,
-                    "velocityX": 2.7172888521606593,
-                    "velocityY": 0.483763412460658,
-                    "timestamp": 0.7844823456101916
-                },
-                {
-                    "x": 4.091189365639749,
-                    "y": 5.539762978420037,
-                    "heading": -0.2990438366325736,
-                    "angularVelocity": 2.758879428992955,
-                    "velocityX": 2.0209584049843006,
-                    "velocityY": 0.28780707103340436,
-                    "timestamp": 0.8716470506779906
-                },
-                {
-                    "x": 4.206412444969991,
-                    "y": 5.553849615471576,
-                    "heading": -0.10702573317574342,
-                    "angularVelocity": 2.202934126920672,
-                    "velocityX": 1.3219006390329475,
-                    "velocityY": 0.16160941565260478,
-                    "timestamp": 0.9588117557457897
+                    "x": 2.3223907681513127,
+                    "y": 4.418261357040706,
+                    "heading": -0.429219543622513,
+                    "angularVelocity": -0.4499475285553182,
+                    "velocityX": 0.2568788379871729,
+                    "velocityY": 0.6684053711347925,
+                    "timestamp": 0.08716470506781053
+                },
+                {
+                    "x": 2.374909672838796,
+                    "y": 4.531319644218887,
+                    "heading": -0.5050759499486299,
+                    "angularVelocity": -0.8702651637162607,
+                    "velocityX": 0.6025248940683728,
+                    "velocityY": 1.297064988520603,
+                    "timestamp": 0.17432941013562106
+                },
+                {
+                    "x": 2.469260718438912,
+                    "y": 4.691180971030917,
+                    "heading": -0.6116327562753044,
+                    "angularVelocity": -1.2224765315706085,
+                    "velocityX": 1.082445532589298,
+                    "velocityY": 1.834014429208093,
+                    "timestamp": 0.2614941152034316
+                },
+                {
+                    "x": 2.620331804373909,
+                    "y": 4.8788277526908095,
+                    "heading": -0.7357228691689549,
+                    "angularVelocity": -1.4236279787456398,
+                    "velocityX": 1.7331680961631113,
+                    "velocityY": 2.1527839911109723,
+                    "timestamp": 0.3486588202712421
+                },
+                {
+                    "x": 2.832381156144981,
+                    "y": 5.083303561006889,
+                    "heading": -0.84759450212177,
+                    "angularVelocity": -1.2834510581523242,
+                    "velocityX": 2.432743294503276,
+                    "velocityY": 2.3458555634072913,
+                    "timestamp": 0.43582352533905266
+                },
+                {
+                    "x": 3.098611796220168,
+                    "y": 5.267387874253087,
+                    "heading": -0.8666590247194526,
+                    "angularVelocity": -0.21871837440225492,
+                    "velocityX": 3.0543399403241303,
+                    "velocityY": 2.111913452847551,
+                    "timestamp": 0.5229882304068632
+                },
+                {
+                    "x": 3.3974595011437105,
+                    "y": 5.397776588765608,
+                    "heading": -0.8564937886773937,
+                    "angularVelocity": 0.11662101115527058,
+                    "velocityX": 3.42854030987716,
+                    "velocityY": 1.4958888969002289,
+                    "timestamp": 0.6101529354746736
+                },
+                {
+                    "x": 3.678181484629676,
+                    "y": 5.472509187071852,
+                    "heading": -0.7581230365630254,
+                    "angularVelocity": 1.1285617502845884,
+                    "velocityX": 3.220592363245829,
+                    "velocityY": 0.8573722385466142,
+                    "timestamp": 0.6973176405424841
+                },
+                {
+                    "x": 3.915033154173224,
+                    "y": 5.514676302538828,
+                    "heading": -0.5395209628894968,
+                    "angularVelocity": 2.5079196161274817,
+                    "velocityX": 2.7172887163363586,
+                    "velocityY": 0.4837636453214852,
+                    "timestamp": 0.7844823456102946
+                },
+                {
+                    "x": 4.091189385673594,
+                    "y": 5.53976294658506,
+                    "heading": -0.29904398170320484,
+                    "angularVelocity": 2.7588802256510867,
+                    "velocityX": 2.0209582693284847,
+                    "velocityY": 0.2878073645372324,
+                    "timestamp": 0.8716470506781051
+                },
+                {
+                    "x": 4.2064124532392215,
+                    "y": 5.553849604220975,
+                    "heading": -0.10702579424844161,
+                    "angularVelocity": 2.2029350905894844,
+                    "velocityX": 1.3219005040628353,
+                    "velocityY": 0.161609651807531,
+                    "timestamp": 0.9588117557459156
                 },
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 1.2278563105616622,
-                    "velocityX": 0.6492026214738336,
-                    "velocityY": 0.07056049261727199,
-                    "timestamp": 1.0459764608135889
+                    "heading": 2.9287791201697906e-29,
+                    "angularVelocity": 1.2278570112200775,
+                    "velocityX": 0.6492025266047246,
+                    "velocityY": 0.0705606216901654,
+                    "timestamp": 1.045976460813726
                 },
                 {
                     "x": 4.263,
                     "y": 5.56,
                     "heading": 0,
                     "angularVelocity": 0,
-                    "velocityX": 1.8862699039176484e-32,
-                    "velocityY": 0,
-                    "timestamp": 1.133141165881388
+                    "velocityX": 3.0874616220611523e-31,
+                    "velocityY": -3.4071713040637054e-30,
+                    "timestamp": 1.1331411658815367
                 }
             ],
             "constraints": [
@@ -17197,16 +18572,6 @@
                     "x": 3.42,
                     "y": 4.04,
                     "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
                 }
             ]
         },
@@ -17377,11 +18742,6 @@
                     "x": 5.61,
                     "y": 2.81,
                     "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
                 }
             ]
         },
@@ -17394,7 +18754,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 17
+                    "controlIntervalCount": 11
+                },
+                {
+                    "x": 2.4722259044647217,
+                    "y": 2.73530650138855,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 16
                 },
                 {
                     "x": 5.176,
@@ -17406,7 +18775,260 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": -0.39,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 9.57151893871228e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 2.2927892107770984,
+                    "y": 4.332309394440523,
+                    "heading": -0.3722824674321399,
+                    "angularVelocity": 0.3002660031020358,
+                    "velocityX": -0.12220408518393268,
+                    "velocityY": -0.4692835993371687,
+                    "timestamp": 0.059006122520770025
+                },
+                {
+                    "x": 2.2795380946752672,
+                    "y": 4.276530177721837,
+                    "heading": -0.3395678297253276,
+                    "angularVelocity": 0.5544278510301563,
+                    "velocityX": -0.224571883996739,
+                    "velocityY": -0.9453123563415277,
+                    "timestamp": 0.11801224504154005
+                },
+                {
+                    "x": 2.2618545093830753,
+                    "y": 4.192237257969867,
+                    "heading": -0.2954402922678697,
+                    "angularVelocity": 0.7478467584770477,
+                    "velocityX": -0.29969068524994186,
+                    "velocityY": -1.4285453127732544,
+                    "timestamp": 0.17701836756231007
+                },
+                {
+                    "x": 2.2420434066604593,
+                    "y": 4.079048226765037,
+                    "heading": -0.24482967906698166,
+                    "angularVelocity": 0.8577179966887506,
+                    "velocityX": -0.33574656114103973,
+                    "velocityY": -1.9182590953165608,
+                    "timestamp": 0.2360244900830801
+                },
+                {
+                    "x": 2.2234541825747725,
+                    "y": 3.936854095556344,
+                    "heading": -0.19463836205748358,
+                    "angularVelocity": 0.8506120189787272,
+                    "velocityX": -0.3150389026010499,
+                    "velocityY": -2.40981994976269,
+                    "timestamp": 0.2950306126038501
+                },
+                {
+                    "x": 2.211142623620853,
+                    "y": 3.7664761504723194,
+                    "heading": -0.15521354512815658,
+                    "angularVelocity": 0.6681479013546628,
+                    "velocityX": -0.2086488389333828,
+                    "velocityY": -2.8874621446961175,
+                    "timestamp": 0.35403673512462014
+                },
+                {
+                    "x": 2.2125400151692816,
+                    "y": 3.5715383450180926,
+                    "heading": -0.14131017152189063,
+                    "angularVelocity": 0.23562594883900537,
+                    "velocityX": 0.023682144983125607,
+                    "velocityY": -3.303687772156347,
+                    "timestamp": 0.41304285764539017
+                },
+                {
+                    "x": 2.235363094184648,
+                    "y": 3.3576277458759463,
+                    "heading": -0.144791073330025,
+                    "angularVelocity": -0.058992214018278255,
+                    "velocityX": 0.38679170974728244,
+                    "velocityY": -3.6252271798888405,
+                    "timestamp": 0.4720489801661602
+                },
+                {
+                    "x": 2.287018648077521,
+                    "y": 3.1404706845748738,
+                    "heading": -0.14479216409186968,
+                    "angularVelocity": -0.000018485570624385508,
+                    "velocityX": 0.8754270181825738,
+                    "velocityY": -3.6802462528296687,
+                    "timestamp": 0.5310551026869302
+                },
+                {
+                    "x": 2.3664206535422005,
+                    "y": 2.9318538879811458,
+                    "heading": -0.14479229465212481,
+                    "angularVelocity": -0.000002212656069584398,
+                    "velocityX": 1.3456570618876043,
+                    "velocityY": -3.53551102295012,
+                    "timestamp": 0.5900612252077002
+                },
+                {
+                    "x": 2.4722259044647217,
+                    "y": 2.73530650138855,
+                    "heading": -0.14479252387880184,
+                    "angularVelocity": -0.000003884794784189704,
+                    "velocityX": 1.7931232625102267,
+                    "velocityY": -3.330965977698533,
+                    "timestamp": 0.6490673477284702
+                },
+                {
+                    "x": 2.6222628757424284,
+                    "y": 2.533912575740545,
+                    "heading": -0.14479267276909355,
+                    "angularVelocity": -0.0000022427572708026126,
+                    "velocityX": 2.260029880862475,
+                    "velocityY": -3.033627551346843,
+                    "timestamp": 0.7154545103709612
+                },
+                {
+                    "x": 2.800083925097921,
+                    "y": 2.356570260194387,
+                    "heading": -0.14479275333723068,
+                    "angularVelocity": -0.0000012136101911737535,
+                    "velocityX": 2.678545704884188,
+                    "velocityY": -2.671334464182248,
+                    "timestamp": 0.7818416730134522
+                },
+                {
+                    "x": 3.001881619185789,
+                    "y": 2.207076716019015,
+                    "heading": -0.14479280967120484,
+                    "angularVelocity": -8.485672817028763e-7,
+                    "velocityX": 3.0397095772053357,
+                    "velocityY": -2.2518441551784094,
+                    "timestamp": 0.8482288356559432
+                },
+                {
+                    "x": 3.2233351494929763,
+                    "y": 2.0886327789398735,
+                    "heading": -0.14479285532879432,
+                    "angularVelocity": -6.877472642883405e-7,
+                    "velocityX": 3.3357884490373766,
+                    "velocityY": -1.7841391673415428,
+                    "timestamp": 0.9146159982984342
+                },
+                {
+                    "x": 3.4597028157497376,
+                    "y": 2.003774307484127,
+                    "heading": -0.14479289641721121,
+                    "angularVelocity": -6.189211176196774e-7,
+                    "velocityX": 3.5604423633775646,
+                    "velocityY": -1.2782361540698368,
+                    "timestamp": 0.9810031609409252
+                },
+                {
+                    "x": 3.704835569204283,
+                    "y": 1.949179734375487,
+                    "heading": -0.14479293648657837,
+                    "angularVelocity": -6.035710154638316e-7,
+                    "velocityX": 3.692472214464675,
+                    "velocityY": -0.8223664174750668,
+                    "timestamp": 1.047390323583416
+                },
+                {
+                    "x": 3.9499686718209266,
+                    "y": 1.8945867290481686,
+                    "heading": -0.1447929765559331,
+                    "angularVelocity": -6.035708281824289e-7,
+                    "velocityX": 3.692477473946853,
+                    "velocityY": -0.8223428017448724,
+                    "timestamp": 1.113777486225907
+                },
+                {
+                    "x": 4.195101784677066,
+                    "y": 1.8399937696982192,
+                    "heading": -0.14479301662532418,
+                    "angularVelocity": -6.035713756667977e-7,
+                    "velocityX": 3.692477628185933,
+                    "velocityY": -0.8223421091807183,
+                    "timestamp": 1.1801646488683981
+                },
+                {
+                    "x": 4.435904668374261,
+                    "y": 1.7860806487644325,
+                    "heading": -0.15898872227240893,
+                    "angularVelocity": -0.21383208864538455,
+                    "velocityX": 3.627250723064772,
+                    "velocityY": -0.8121015989811171,
+                    "timestamp": 1.2465518115108891
+                },
+                {
+                    "x": 4.648245693564204,
+                    "y": 1.7385415440023928,
+                    "heading": -0.26462575063007965,
+                    "angularVelocity": -1.5912267395211415,
+                    "velocityX": 3.1985253886123286,
+                    "velocityY": -0.7160888170209653,
+                    "timestamp": 1.3129389741533801
+                },
+                {
+                    "x": 4.824757883271093,
+                    "y": 1.6989793937314068,
+                    "heading": -0.37654836939407044,
+                    "angularVelocity": -1.6859075506317058,
+                    "velocityX": 2.6588301515075172,
+                    "velocityY": -0.5959307296206723,
+                    "timestamp": 1.3793261367958711
+                },
+                {
+                    "x": 4.96555652453039,
+                    "y": 1.6673723665346096,
+                    "heading": -0.4762340168485147,
+                    "angularVelocity": -1.501580177349536,
+                    "velocityX": 2.1208715006774397,
+                    "velocityY": -0.47610149219673153,
+                    "timestamp": 1.4457132994383621
+                },
+                {
+                    "x": 5.070906714490224,
+                    "y": 1.643682366519874,
+                    "heading": -0.5558849180153355,
+                    "angularVelocity": -1.1997937251175768,
+                    "velocityX": 1.5869060487969315,
+                    "velocityY": -0.3568460990313944,
+                    "timestamp": 1.5121004620808531
+                },
+                {
+                    "x": 5.141006607602348,
+                    "y": 1.6278935430132826,
+                    "heading": -0.6113350442973207,
+                    "angularVelocity": -0.8352537459779052,
+                    "velocityX": 1.055925427776252,
+                    "velocityY": -0.23782946699526453,
+                    "timestamp": 1.5784876247233441
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": -0.64,
+                    "angularVelocity": -0.4317846186174008,
+                    "velocityX": 0.5271108299370921,
+                    "velocityY": -0.11890164753374041,
+                    "timestamp": 1.6448747873658351
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": -0.64,
+                    "angularVelocity": 1.0662498702136072e-32,
+                    "velocityX": 0,
+                    "velocityY": 8.479039452208929e-33,
+                    "timestamp": 1.7112619500083261
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -17434,11 +19056,6 @@
                     "x": 5.61,
                     "y": 2.81,
                     "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
                 }
             ]
         },
@@ -20378,127 +21995,127 @@
                     "x": 5.176,
                     "y": 6.63,
                     "heading": 0.26,
-                    "angularVelocity": 2.3848638783294046e-33,
-                    "velocityX": 3.9311043555760955e-34,
-                    "velocityY": 6.649026992883858e-33,
+                    "angularVelocity": -5.95142511939464e-31,
+                    "velocityX": 0,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 5.1029790541350515,
-                    "y": 6.633224554642693,
-                    "heading": 0.2665012951102971,
-                    "angularVelocity": 0.06942751851370552,
-                    "velocityX": -0.7797927928070008,
-                    "velocityY": 0.03443511229011806,
-                    "timestamp": 0.09364147314326547
-                },
-                {
-                    "x": 4.956937163671988,
-                    "y": 6.639673650268821,
-                    "heading": 0.2795036636182218,
-                    "angularVelocity": 0.13885266935138055,
-                    "velocityX": -1.5595855720854526,
-                    "velocityY": 0.06887007871263316,
-                    "timestamp": 0.18728294628653094
-                },
-                {
-                    "x": 4.737874332057849,
-                    "y": 6.649347273946163,
-                    "heading": 0.29900673147988355,
-                    "angularVelocity": 0.20827382576349504,
-                    "velocityX": -2.3393783145528313,
-                    "velocityY": 0.10330490703159452,
-                    "timestamp": 0.2809244194297964
-                },
-                {
-                    "x": 4.445790573670186,
-                    "y": 6.662245413482842,
-                    "heading": 0.325009942942769,
-                    "angularVelocity": 0.27768904727835797,
-                    "velocityX": -3.1191709034819257,
-                    "velocityY": 0.13773960515320766,
-                    "timestamp": 0.3745658925730619
-                },
-                {
-                    "x": 4.091895329009691,
-                    "y": 6.677873079689744,
-                    "heading": 0.32500994593496846,
-                    "angularVelocity": 3.195376913904642e-8,
-                    "velocityX": -3.7792575531042867,
-                    "velocityY": 0.16688829940708347,
-                    "timestamp": 0.4682073657163274
-                },
-                {
-                    "x": 3.738000080970794,
-                    "y": 6.693500669359476,
-                    "heading": 0.3250099489270859,
-                    "angularVelocity": 3.1952908210754574e-8,
-                    "velocityX": -3.779257589182282,
-                    "velocityY": 0.1668874820649196,
-                    "timestamp": 0.5618488388595928
-                },
-                {
-                    "x": 3.3841048299235403,
-                    "y": 6.709128190903334,
-                    "heading": 0.3250099519191997,
-                    "angularVelocity": 3.195286925539051e-8,
-                    "velocityX": -3.779257621308623,
-                    "velocityY": 0.16688675454671412,
-                    "timestamp": 0.6554903120028582
-                },
-                {
-                    "x": 3.030209576168151,
-                    "y": 6.724755651121081,
-                    "heading": 0.32500995491150664,
-                    "angularVelocity": 3.195491758844704e-8,
-                    "velocityX": -3.7792576502288973,
-                    "velocityY": 0.16688609964363924,
-                    "timestamp": 0.7491317851461237
-                },
-                {
-                    "x": 2.7381257642201002,
-                    "y": 6.737653399625744,
-                    "heading": 0.3510061559504441,
-                    "angularVelocity": 0.2776141827578367,
-                    "velocityX": -3.119171475454889,
-                    "velocityY": 0.13773542931034466,
-                    "timestamp": 0.8427732582893891
-                },
-                {
-                    "x": 2.519062886912394,
-                    "y": 6.747326704432178,
-                    "heading": 0.37050311197995217,
-                    "angularVelocity": 0.20820855733088625,
-                    "velocityX": -2.339378802515782,
-                    "velocityY": 0.10330150179952581,
-                    "timestamp": 0.9364147314326545
-                },
-                {
-                    "x": 2.373020963294867,
-                    "y": 6.7537755697062725,
-                    "heading": 0.383501026964712,
-                    "angularVelocity": 0.13880511004851873,
-                    "velocityX": -1.5595859261429292,
-                    "velocityY": 0.06886761877636173,
-                    "timestamp": 1.03005620457592
+                    "x": 5.1029790507443495,
+                    "y": 6.633224483607701,
+                    "heading": 0.26650125878935643,
+                    "angularVelocity": 0.06942713064122713,
+                    "velocityX": -0.7797928290155524,
+                    "velocityY": 0.034434353705267144,
+                    "timestamp": 0.09364147314336706
+                },
+                {
+                    "x": 4.9569371541735245,
+                    "y": 6.639673450192742,
+                    "heading": 0.2795035687162854,
+                    "angularVelocity": 0.13885204376302582,
+                    "velocityX": -1.559585637308702,
+                    "velocityY": 0.06886870067889261,
+                    "timestamp": 0.18728294628673411
+                },
+                {
+                    "x": 4.737874314376445,
+                    "y": 6.649346898931416,
+                    "heading": 0.2990065710588269,
+                    "angularVelocity": 0.20827312608258508,
+                    "velocityX": -2.339378401936154,
+                    "velocityY": 0.1033030388561254,
+                    "timestamp": 0.28092441943010116
+                },
+                {
+                    "x": 4.445790546320906,
+                    "y": 6.66224482863892,
+                    "heading": 0.3250097249050781,
+                    "angularVelocity": 0.2776884319880837,
+                    "velocityX": -3.119171006722006,
+                    "velocityY": 0.13773736438080994,
+                    "timestamp": 0.37456589257346823
+                },
+                {
+                    "x": 4.09189529815297,
+                    "y": 6.677872415392626,
+                    "heading": 0.32500972789735083,
+                    "angularVelocity": 3.195452790688951e-8,
+                    "velocityX": -3.7792575905562438,
+                    "velocityY": 0.16688745092443205,
+                    "timestamp": 0.4682073657168353
+                },
+                {
+                    "x": 3.7380000499890724,
+                    "y": 6.693500002240241,
+                    "heading": 0.32500973088947493,
+                    "angularVelocity": 3.1952970747065184e-8,
+                    "velocityX": -3.7792575905130557,
+                    "velocityY": 0.16688745192729154,
+                    "timestamp": 0.5618488388602023
+                },
+                {
+                    "x": 3.384104801825175,
+                    "y": 6.7091275890878554,
+                    "heading": 0.3250097338815991,
+                    "angularVelocity": 3.195297127919844e-8,
+                    "velocityX": -3.7792575905130557,
+                    "velocityY": 0.1668874519272919,
+                    "timestamp": 0.6554903120035693
+                },
+                {
+                    "x": 3.030209553657211,
+                    "y": 6.724755175840924,
+                    "heading": 0.32500973687387263,
+                    "angularVelocity": 3.195453683514771e-8,
+                    "velocityX": -3.7792575905565435,
+                    "velocityY": 0.16688745091763021,
+                    "timestamp": 0.7491317851469363
+                },
+                {
+                    "x": 2.7381257502813003,
+                    "y": 6.7376531044804615,
+                    "heading": 0.35100602630019134,
+                    "angularVelocity": 0.2776151266490911,
+                    "velocityX": -3.119171383909283,
+                    "velocityY": 0.13773735297596457,
+                    "timestamp": 0.8427732582903034
+                },
+                {
+                    "x": 2.5190628797280783,
+                    "y": 6.747326551971013,
+                    "heading": 0.37050304728737704,
+                    "angularVelocity": 0.20820925101574836,
+                    "velocityX": -2.3393787303818834,
+                    "velocityY": 0.10330302552738789,
+                    "timestamp": 0.9364147314336704
+                },
+                {
+                    "x": 2.3730209608288053,
+                    "y": 6.7537755172838185,
+                    "heading": 0.3835010053218344,
+                    "angularVelocity": 0.13880556977745576,
+                    "velocityX": -1.559585875754842,
+                    "velocityY": 0.06886868709264785,
+                    "timestamp": 1.0300562045770374
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.39,
-                    "angularVelocity": 0.06940272100743458,
-                    "velocityX": -0.7797929789416104,
-                    "velocityY": 0.034433784363791235,
-                    "timestamp": 1.1236976777191856
+                    "angularVelocity": 0.06940295213233785,
+                    "velocityX": -0.7797929526056125,
+                    "velocityY": 0.03443434418469861,
+                    "timestamp": 1.1236976777204044
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.39,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 2.0630865476520398e-33,
-                    "timestamp": 1.2173391508624511
+                    "angularVelocity": -1.6940949553652625e-30,
+                    "velocityX": -1.6586267397188983e-32,
+                    "velocityY": 0,
+                    "timestamp": 1.2173391508637714
                 }
             ],
             "constraints": [
@@ -20518,23 +22135,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "S1 To W2": {
             "waypoints": [
@@ -20563,144 +22164,144 @@
                     "y": 6.63,
                     "heading": 0.26,
                     "angularVelocity": 0,
-                    "velocityX": -1.9284905870694258e-35,
-                    "velocityY": -6.933375493218394e-34,
+                    "velocityX": 0,
+                    "velocityY": 3.363577949177338e-35,
                     "timestamp": 0
                 },
                 {
-                    "x": 5.118924935599245,
-                    "y": 6.609128491648739,
-                    "heading": 0.2512266763512042,
-                    "angularVelocity": -0.10272378997697001,
-                    "velocityX": -0.6682720441106688,
-                    "velocityY": -0.24437721964946355,
-                    "timestamp": 0.08540693106010512
-                },
-                {
-                    "x": 5.004774806713452,
-                    "y": 6.567385472222137,
-                    "heading": 0.23368042064504363,
-                    "angularVelocity": -0.2054429949463047,
-                    "velocityX": -1.3365440892081688,
-                    "velocityY": -0.4887544711942054,
-                    "timestamp": 0.17081386212021024
-                },
-                {
-                    "x": 4.833549609463453,
-                    "y": 6.504770942290587,
-                    "heading": 0.2073620554535925,
-                    "angularVelocity": -0.3081525687058067,
-                    "velocityX": -2.0048161797255077,
-                    "velocityY": -0.7331317160604409,
-                    "timestamp": 0.2562207931803154
-                },
-                {
-                    "x": 4.605249336717623,
-                    "y": 6.421284904520194,
-                    "heading": 0.17227303630641458,
-                    "angularVelocity": -0.4108451001767527,
-                    "velocityX": -2.673088353744534,
-                    "velocityY": -0.9775089297101582,
-                    "timestamp": 0.3416277242404205
-                },
-                {
-                    "x": 4.319873982796504,
-                    "y": 6.316927367787122,
-                    "heading": 0.12841583542221255,
-                    "angularVelocity": -0.5135086853002309,
-                    "velocityX": -3.341360594262375,
-                    "velocityY": -1.22188603943198,
-                    "timestamp": 0.4270346553005256
-                },
-                {
-                    "x": 4.016437213590231,
-                    "y": 6.205964096134007,
-                    "heading": 0.12841583017477892,
-                    "angularVelocity": -6.144037201192911e-8,
-                    "velocityX": -3.552835413237479,
-                    "velocityY": -1.2992302881721054,
-                    "timestamp": 0.5124415863606308
-                },
-                {
-                    "x": 3.7130004751736263,
-                    "y": 6.095000740284403,
-                    "heading": 0.12841582492737386,
-                    "angularVelocity": -6.144003768621477e-8,
-                    "velocityX": -3.5528350527319725,
-                    "velocityY": -1.2992312739994434,
-                    "timestamp": 0.5978485174207359
-                },
-                {
-                    "x": 3.409563765577162,
-                    "y": 5.984037305624233,
-                    "heading": 0.12841581967997912,
-                    "angularVelocity": -6.143991704551595e-8,
-                    "velocityX": -3.5528347152869912,
-                    "velocityY": -1.2992321967649234,
-                    "timestamp": 0.683255448480841
-                },
-                {
-                    "x": 3.1061270838779644,
-                    "y": 5.873073794677257,
-                    "heading": 0.12841581443257097,
-                    "angularVelocity": -6.1440073698458e-8,
-                    "velocityX": -3.5528343886476135,
-                    "velocityY": -1.2992330899805498,
-                    "timestamp": 0.7686623795409461
-                },
-                {
-                    "x": 2.8207514220046077,
-                    "y": 5.76871589253347,
-                    "heading": 0.08460828566897327,
-                    "angularVelocity": -0.512927091745846,
-                    "velocityX": -3.3413641999678414,
-                    "velocityY": -1.2218903178987286,
-                    "timestamp": 0.8540693106010512
-                },
-                {
-                    "x": 2.592450862147759,
-                    "y": 5.685229551240001,
-                    "heading": 0.0495641189011112,
-                    "angularVelocity": -0.4103199392939169,
-                    "velocityX": -2.673091715427432,
-                    "velocityY": -0.9775124835561102,
-                    "timestamp": 0.9394762416611563
-                },
-                {
-                    "x": 2.421225431748781,
-                    "y": 5.622614782821321,
-                    "heading": 0.02328193601640482,
-                    "angularVelocity": -0.30772892268205126,
-                    "velocityX": -2.004818909585662,
-                    "velocityY": -0.7331345084231428,
-                    "timestamp": 1.0248831727212615
-                },
-                {
-                    "x": 2.3070751430245986,
-                    "y": 5.580871596442558,
-                    "heading": 0.005760710543037936,
-                    "angularVelocity": -0.2051499246710589,
-                    "velocityX": -1.3365459607001804,
-                    "velocityY": -0.48875642597890145,
-                    "timestamp": 1.1102901037813666
+                    "x": 5.118924970919473,
+                    "y": 6.609128397315466,
+                    "heading": 0.25122660793754975,
+                    "angularVelocity": -0.10272459100864365,
+                    "velocityX": -0.6682716305586017,
+                    "velocityY": -0.24437832416494054,
+                    "timestamp": 0.08540693106008139
+                },
+                {
+                    "x": 5.0047749070529814,
+                    "y": 6.567385203608531,
+                    "heading": 0.23368024533248913,
+                    "angularVelocity": -0.20544424658833893,
+                    "velocityX": -1.3365433279201937,
+                    "velocityY": -0.48875651178205104,
+                    "timestamp": 0.17081386212016278
+                },
+                {
+                    "x": 4.833549798383878,
+                    "y": 6.504770434941582,
+                    "heading": 0.20736177398264571,
+                    "angularVelocity": -0.30815381167752187,
+                    "velocityX": -2.004815142563213,
+                    "velocityY": -0.7331345113302418,
+                    "timestamp": 0.25622079318024416
+                },
+                {
+                    "x": 4.605249631158782,
+                    "y": 6.421284110658292,
+                    "heading": 0.17227269285622732,
+                    "angularVelocity": -0.4108458258702004,
+                    "velocityX": -2.6730871182397737,
+                    "velocityY": -0.977512284390117,
+                    "timestamp": 0.34162772424032556
+                },
+                {
+                    "x": 4.319874394275373,
+                    "y": 6.316926253811125,
+                    "heading": 0.12841549400557886,
+                    "angularVelocity": -0.5135086614902031,
+                    "velocityX": -3.3413592239095435,
+                    "velocityY": -1.2218897875367427,
+                    "timestamp": 0.42703465530040696
+                },
+                {
+                    "x": 4.016437653324315,
+                    "y": 6.205962904892466,
+                    "heading": 0.1284154887581796,
+                    "angularVelocity": -6.143996998796484e-8,
+                    "velocityX": -3.552835082407984,
+                    "velocityY": -1.2992311928477889,
+                    "timestamp": 0.5124415863604883
+                },
+                {
+                    "x": 3.713000912388581,
+                    "y": 6.094999555931864,
+                    "heading": 0.12841548351080107,
+                    "angularVelocity": -6.143972697353832e-8,
+                    "velocityX": -3.5528350822285746,
+                    "velocityY": -1.299231193338901,
+                    "timestamp": 0.5978485174205697
+                },
+                {
+                    "x": 3.4095641714528466,
+                    "y": 5.98403620697126,
+                    "heading": 0.12841547826342262,
+                    "angularVelocity": -6.143972595662007e-8,
+                    "velocityX": -3.5528350822285746,
+                    "velocityY": -1.299231193338901,
+                    "timestamp": 0.683255448480651
+                },
+                {
+                    "x": 3.106127430499178,
+                    "y": 5.873072858059743,
+                    "heading": 0.12841547301602332,
+                    "angularVelocity": -6.143997022352915e-8,
+                    "velocityX": -3.55283508243856,
+                    "velocityY": -1.2992311927641775,
+                    "timestamp": 0.7686623795407324
+                },
+                {
+                    "x": 2.820751661400481,
+                    "y": 5.768715245814204,
+                    "heading": 0.08460804498823243,
+                    "angularVelocity": -0.5129259122655233,
+                    "velocityX": -3.3413654554329186,
+                    "velocityY": -1.2218869235814813,
+                    "timestamp": 0.8540693106008137
+                },
+                {
+                    "x": 2.5924510107246115,
+                    "y": 5.685229149936981,
+                    "heading": 0.04956396734437731,
+                    "angularVelocity": -0.41031889577208425,
+                    "velocityX": -2.6730927787964465,
+                    "velocityY": -0.977509610063062,
+                    "timestamp": 0.939476241660895
+                },
+                {
+                    "x": 2.4212255084791887,
+                    "y": 5.622614575599846,
+                    "heading": 0.02328185696555881,
+                    "angularVelocity": -0.30772807373595673,
+                    "velocityX": -2.00481975081121,
+                    "velocityY": -0.7331322359901695,
+                    "timestamp": 1.0248831727209764
+                },
+                {
+                    "x": 2.307075169405903,
+                    "y": 5.580871525201533,
+                    "heading": 0.005760683198102726,
+                    "angularVelocity": -0.20514931926462068,
+                    "velocityX": -1.3365465502206675,
+                    "velocityY": -0.48875483383130996,
+                    "timestamp": 1.1102901037810577
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
                     "heading": -0.003,
-                    "angularVelocity": -0.10257610751605846,
-                    "velocityX": -0.6682729646898584,
-                    "velocityY": -0.24437825107975505,
-                    "timestamp": 1.1956970348414717
+                    "angularVelocity": -0.10257578734376759,
+                    "velocityX": -0.6682732735795421,
+                    "velocityY": -0.24437741694350015,
+                    "timestamp": 1.195697034841139
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
                     "heading": -0.003,
-                    "angularVelocity": -5.146817856452735e-35,
-                    "velocityX": 4.663929716319662e-34,
-                    "velocityY": 0,
-                    "timestamp": 1.2811039659015768
+                    "angularVelocity": 2.4865352537032555e-34,
+                    "velocityX": 0,
+                    "velocityY": -6.568641934425214e-34,
+                    "timestamp": 1.2811039659012204
                 }
             ],
             "constraints": [
@@ -20720,23 +22321,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "S1 To W3": {
             "waypoints": [
@@ -20759,7 +22344,179 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 0.26,
+                    "angularVelocity": 2.521971410376041e-31,
+                    "velocityX": -3.46085485178236e-32,
+                    "velocityY": -2.8734015412732535e-37,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.132269964046874,
+                    "y": 6.606195610268081,
+                    "heading": 0.16914444930535336,
+                    "angularVelocity": -1.1148212857556328,
+                    "velocityX": -0.5365789380469493,
+                    "velocityY": -0.29208606589986635,
+                    "timestamp": 0.08149786145594107
+                },
+                {
+                    "x": 5.043666795437562,
+                    "y": 6.5570487224925476,
+                    "heading": 0.005335724612659364,
+                    "angularVelocity": -2.0099757437347154,
+                    "velocityX": -1.0871839705537814,
+                    "velocityY": -0.6030451216453527,
+                    "timestamp": 0.16299572291188213
+                },
+                {
+                    "x": 4.909391168326261,
+                    "y": 6.4806746643853055,
+                    "heading": -0.2131663273161798,
+                    "angularVelocity": -2.681077123072297,
+                    "velocityX": -1.647596939508567,
+                    "velocityY": -0.9371295975481627,
+                    "timestamp": 0.2444935843678232
+                },
+                {
+                    "x": 4.728031239930722,
+                    "y": 6.375889843725219,
+                    "heading": -0.4550458803770597,
+                    "angularVelocity": -2.967925154596154,
+                    "velocityX": -2.225333587355345,
+                    "velocityY": -1.2857370584715861,
+                    "timestamp": 0.32599144582376427
+                },
+                {
+                    "x": 4.50844214888531,
+                    "y": 6.243100931440797,
+                    "heading": -0.5880935326141025,
+                    "angularVelocity": -1.6325293677671568,
+                    "velocityX": -2.6944153763362864,
+                    "velocityY": -1.6293545611158133,
+                    "timestamp": 0.40748930727970534
+                },
+                {
+                    "x": 4.251218650398233,
+                    "y": 6.083818287155582,
+                    "heading": -0.6031902279140122,
+                    "angularVelocity": -0.1852403858237583,
+                    "velocityX": -3.15619936390767,
+                    "velocityY": -1.9544395575499292,
+                    "timestamp": 0.4889871687356464
+                },
+                {
+                    "x": 3.9855391766757995,
+                    "y": 5.92740773635831,
+                    "heading": -0.6031903133870022,
+                    "angularVelocity": -0.0000010487758630839914,
+                    "velocityX": -3.259956383837921,
+                    "velocityY": -1.9191982219291623,
+                    "timestamp": 0.5704850301915875
+                },
+                {
+                    "x": 3.719859695364488,
+                    "y": 5.770997198451498,
+                    "heading": -0.603190398859952,
+                    "angularVelocity": -0.0000010487753700599598,
+                    "velocityX": -3.259956476955436,
+                    "velocityY": -1.919198063759863,
+                    "timestamp": 0.6519828916475285
+                },
+                {
+                    "x": 3.4541802321431443,
+                    "y": 5.614586629816999,
+                    "heading": -0.6031904843329001,
+                    "angularVelocity": -0.000001048775349610677,
+                    "velocityX": -3.259956254986808,
+                    "velocityY": -1.9191984407966034,
+                    "timestamp": 0.7334807531034696
+                },
+                {
+                    "x": 3.1885009063173535,
+                    "y": 5.458175827802239,
+                    "heading": -0.6031905698058837,
+                    "angularVelocity": -0.000001048775783641969,
+                    "velocityX": -3.259954569107567,
+                    "velocityY": -1.9192013044332228,
+                    "timestamp": 0.8149786145594107
+                },
+                {
+                    "x": 2.937386380849553,
+                    "y": 5.279314604480798,
+                    "heading": -0.603190661977047,
+                    "angularVelocity": -0.0000011309641953921016,
+                    "velocityX": -3.0812406728433848,
+                    "velocityY": -2.1946738248847923,
+                    "timestamp": 0.8964764760153517
+                },
+                {
+                    "x": 2.722322578778117,
+                    "y": 5.058413395221267,
+                    "heading": -0.6031910253459357,
+                    "angularVelocity": -0.00000445863096683889,
+                    "velocityX": -2.6388889012468426,
+                    "velocityY": -2.710515408787179,
+                    "timestamp": 0.9779743374712928
+                },
+                {
+                    "x": 2.561151389441887,
+                    "y": 4.8410288515668665,
+                    "heading": -0.5595876383179096,
+                    "angularVelocity": 0.5350249227287813,
+                    "velocityX": -1.9776124975176395,
+                    "velocityY": -2.667365005300435,
+                    "timestamp": 1.0594721989272338
+                },
+                {
+                    "x": 2.4444023928247787,
+                    "y": 4.656624455699537,
+                    "heading": -0.5072145719014195,
+                    "angularVelocity": 0.6426311743750899,
+                    "velocityX": -1.432540615562337,
+                    "velocityY": -2.262689996681954,
+                    "timestamp": 1.1409700603831747
+                },
+                {
+                    "x": 2.367465069003143,
+                    "y": 4.510717874734717,
+                    "heading": -0.4552059179368463,
+                    "angularVelocity": 0.6381597386170672,
+                    "velocityX": -0.9440410146618312,
+                    "velocityY": -1.7903117745451358,
+                    "timestamp": 1.2224679218391157
+                },
+                {
+                    "x": 2.321242179529733,
+                    "y": 4.410775659264178,
+                    "heading": -0.4150276653385325,
+                    "angularVelocity": 0.49299763061923907,
+                    "velocityX": -0.5671669004271778,
+                    "velocityY": -1.2263170307182782,
+                    "timestamp": 1.3039657832950566
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": -0.393,
+                    "angularVelocity": 0.27028519454392347,
+                    "velocityX": -0.26064707895700284,
+                    "velocityY": -0.6230305723006956,
+                    "timestamp": 1.3854636447509976
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": -0.393,
+                    "angularVelocity": -9.678618240747916e-31,
+                    "velocityX": 0,
+                    "velocityY": -3.108003047332484e-31,
+                    "timestamp": 1.4669615062069385
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -20782,16 +22539,6 @@
                     "x": 3.42,
                     "y": 4.04,
                     "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
                 }
             ]
         },
@@ -20822,126 +22569,126 @@
                     "y": 6.63,
                     "heading": 0.26,
                     "angularVelocity": 0,
-                    "velocityX": -1.5159307977916785e-33,
-                    "velocityY": 2.4731304546687864e-31,
+                    "velocityX": 0,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 5.238209862421909,
-                    "y": 6.650800928993829,
-                    "heading": 0.2470076817123004,
-                    "angularVelocity": -0.14636981892397002,
-                    "velocityX": 0.7008484626165542,
-                    "velocityY": 0.23434064212588615,
-                    "timestamp": 0.08876364255629132
-                },
-                {
-                    "x": 5.362652882936083,
-                    "y": 6.6924048077649365,
-                    "heading": 0.22258636188758224,
-                    "angularVelocity": -0.27512750853804663,
-                    "velocityX": 1.4019593713167977,
-                    "velocityY": 0.4687040501427167,
-                    "timestamp": 0.17752728511258264
-                },
-                {
-                    "x": 5.549362119474242,
-                    "y": 6.7548129470224065,
-                    "heading": 0.1893705540696427,
-                    "angularVelocity": -0.37420510089603504,
-                    "velocityX": 2.1034427065056938,
-                    "velocityY": 0.7030822244423112,
-                    "timestamp": 0.26629092766887397
-                },
-                {
-                    "x": 5.798380890690993,
-                    "y": 6.838019653320762,
-                    "heading": 0.15275441561528638,
-                    "angularVelocity": -0.4125127969134817,
-                    "velocityX": 2.80541406419279,
-                    "velocityY": 0.9373962571031511,
-                    "timestamp": 0.3550545702251653
-                },
-                {
-                    "x": 6.109622726468052,
-                    "y": 6.941929683454389,
-                    "heading": 0.13026224178151366,
-                    "angularVelocity": -0.2533939931523947,
-                    "velocityX": 3.5064112604341973,
-                    "velocityY": 1.17063729182453,
-                    "timestamp": 0.4438182127814566
-                },
-                {
-                    "x": 6.428044865529091,
-                    "y": 7.048515983686504,
-                    "heading": 0.13026223073820456,
-                    "angularVelocity": -1.2441305517804787e-7,
-                    "velocityX": 3.587303651482315,
-                    "velocityY": 1.2007878131408318,
-                    "timestamp": 0.5325818553377479
-                },
-                {
-                    "x": 6.746467025502362,
-                    "y": 7.155102221444599,
-                    "heading": 0.13026221969501808,
-                    "angularVelocity": -1.244116833309462e-7,
-                    "velocityX": 3.5873038870770317,
-                    "velocityY": 1.2007871093161688,
-                    "timestamp": 0.6213454978940393
-                },
-                {
-                    "x": 7.057671328205741,
-                    "y": 7.258999778631647,
-                    "heading": 0.10797327486362365,
-                    "angularVelocity": -0.25110444085082756,
-                    "velocityX": 3.505988417567436,
-                    "velocityY": 1.1704967731634406,
-                    "timestamp": 0.7101091404503306
-                },
-                {
-                    "x": 7.306674452232408,
-                    "y": 7.342201135769052,
-                    "heading": 0.07025598564329198,
-                    "angularVelocity": -0.4249182225204012,
-                    "velocityX": 2.8052377849284973,
-                    "velocityY": 0.9373359941155657,
-                    "timestamp": 0.7988727830066219
-                },
-                {
-                    "x": 7.493367837286706,
-                    "y": 7.404603419315824,
-                    "heading": 0.03585067831804618,
-                    "angularVelocity": -0.38760585227578265,
-                    "velocityX": 2.103264125692411,
-                    "velocityY": 0.7030162547322278,
-                    "timestamp": 0.8876364255629132
-                },
-                {
-                    "x": 7.617797724211195,
-                    "y": 7.446202169802599,
-                    "heading": 0.01050114896385133,
-                    "angularVelocity": -0.2855845999919636,
-                    "velocityX": 1.401811409955239,
-                    "velocityY": 0.46864627552974614,
-                    "timestamp": 0.9764000681192045
+                    "x": 5.238209882810899,
+                    "y": 6.650800869780916,
+                    "heading": 0.2470077179050236,
+                    "angularVelocity": -0.14636941117769697,
+                    "velocityX": 0.7008486923098889,
+                    "velocityY": 0.23433997503541223,
+                    "timestamp": 0.08876364255722656
+                },
+                {
+                    "x": 5.362652939631404,
+                    "y": 6.692404642961414,
+                    "heading": 0.22258646311132924,
+                    "angularVelocity": -0.2751267759001645,
+                    "velocityX": 1.401959780326283,
+                    "velocityY": 0.4687028605622118,
+                    "timestamp": 0.17752728511445312
+                },
+                {
+                    "x": 5.549362223961953,
+                    "y": 6.754812642679732,
+                    "heading": 0.18937073322571307,
+                    "angularVelocity": -0.37420422291202055,
+                    "velocityX": 2.1034432449090223,
+                    "velocityY": 0.7030806523975865,
+                    "timestamp": 0.26629092767167967
+                },
+                {
+                    "x": 5.798381050208957,
+                    "y": 6.8380191875714225,
+                    "heading": 0.15275465831989177,
+                    "angularVelocity": -0.4125120809759026,
+                    "velocityX": 2.8054146841303256,
+                    "velocityY": 0.9373944386978721,
+                    "timestamp": 0.35505457022890624
+                },
+                {
+                    "x": 6.109622945204874,
+                    "y": 6.941929047341865,
+                    "heading": 0.1302624797347834,
+                    "angularVelocity": -0.25339404667863696,
+                    "velocityX": 3.506411927553799,
+                    "velocityY": 1.1706353725121932,
+                    "timestamp": 0.4438182127861328
+                },
+                {
+                    "x": 6.4280450770381,
+                    "y": 7.048515369175043,
+                    "heading": 0.1302624686930749,
+                    "angularVelocity": -1.2439475582812342e-7,
+                    "velocityX": 3.587303570016191,
+                    "velocityY": 1.2007880564873126,
+                    "timestamp": 0.5325818553433593
+                },
+                {
+                    "x": 6.746467208870004,
+                    "y": 7.1551016910121685,
+                    "heading": 0.13026245765136438,
+                    "angularVelocity": -1.2439477842907252e-7,
+                    "velocityX": 3.5873035700012967,
+                    "velocityY": 1.2007880565318134,
+                    "timestamp": 0.6213454979005859
+                },
+                {
+                    "x": 7.057671454315644,
+                    "y": 7.258999412253756,
+                    "heading": 0.10797351007767278,
+                    "angularVelocity": -0.2511044717394208,
+                    "velocityX": 3.5059877724700192,
+                    "velocityY": 1.1704986213770205,
+                    "timestamp": 0.7101091404578125
+                },
+                {
+                    "x": 7.3066745305511045,
+                    "y": 7.342200909102172,
+                    "heading": 0.07025614763700679,
+                    "angularVelocity": -0.4249190474112408,
+                    "velocityX": 2.8052372464872497,
+                    "velocityY": 0.9373375680789704,
+                    "timestamp": 0.7988727830150391
+                },
+                {
+                    "x": 7.49336787770315,
+                    "y": 7.4046033027166684,
+                    "heading": 0.03585076588111199,
+                    "angularVelocity": -0.3876066908033862,
+                    "velocityX": 2.103263698666567,
+                    "velocityY": 0.7030174947390144,
+                    "timestamp": 0.8876364255722656
+                },
+                {
+                    "x": 7.617797738081694,
+                    "y": 7.446202129881464,
+                    "heading": 0.010501179707511226,
+                    "angularVelocity": -0.2855852401136722,
+                    "velocityX": 1.4018111108759532,
+                    "velocityY": 0.4686471393732002,
+                    "timestamp": 0.9764000681294922
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
                     "heading": -0.003,
-                    "angularVelocity": -0.15210224112604764,
-                    "velocityX": 0.7007629925666751,
-                    "velocityY": 0.2343057314716256,
-                    "timestamp": 1.0651637106754959
+                    "angularVelocity": -0.15210258748116404,
+                    "velocityX": 0.7007628362953491,
+                    "velocityY": 0.23430618121741137,
+                    "timestamp": 1.0651637106867187
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
                     "heading": -0.003,
-                    "angularVelocity": 2.057519635914241e-30,
-                    "velocityX": 3.0968464213310355e-31,
-                    "velocityY": -5.4576131966501765e-31,
-                    "timestamp": 1.1539273532317873
+                    "angularVelocity": -3.7910464085672074e-31,
+                    "velocityX": 1.2889893239032156e-32,
+                    "velocityY": 1.1630630989344696e-31,
+                    "timestamp": 1.1539273532439451
                 }
             ],
             "constraints": [
@@ -20961,26 +22708,10 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "S1 To C2": {
-            "waypoints": [
+            "circleObstacles": []
+        },
+        "S1 To C2": {
+            "waypoints": [
                 {
                     "x": 5.176,
                     "y": 6.63,
@@ -21006,126 +22737,126 @@
                     "y": 6.63,
                     "heading": 0.26,
                     "angularVelocity": 0,
-                    "velocityX": -4.471050480852603e-35,
-                    "velocityY": 4.2685150703849235e-34,
+                    "velocityX": 0,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 5.242063425720018,
-                    "y": 6.625917381735586,
-                    "heading": 0.21680591867144897,
-                    "angularVelocity": -0.4809077162576206,
-                    "velocityX": 0.7355269567955554,
-                    "velocityY": -0.04545443647608383,
-                    "timestamp": 0.08981781715769374
-                },
-                {
-                    "x": 5.374390186000276,
-                    "y": 6.616899298560691,
-                    "heading": 0.13592925344567436,
-                    "angularVelocity": -0.9004523577296353,
-                    "velocityX": 1.473279628338445,
-                    "velocityY": -0.10040416768380533,
-                    "timestamp": 0.17963563431538748
-                },
-                {
-                    "x": 5.573225936929258,
-                    "y": 6.6014853361346235,
-                    "heading": 0.027174299165301757,
-                    "angularVelocity": -1.2108394272088667,
-                    "velocityX": 2.2137673484079947,
-                    "velocityY": -0.17161363873946028,
-                    "timestamp": 0.2694534514730812
-                },
-                {
-                    "x": 5.838620178684051,
-                    "y": 6.5765367651917135,
-                    "heading": -0.08847883132007715,
-                    "angularVelocity": -1.287641295961646,
-                    "velocityX": 2.9548061860470294,
-                    "velocityY": -0.27776861799154184,
-                    "timestamp": 0.35927126863077496
-                },
-                {
-                    "x": 6.164628937797069,
-                    "y": 6.53182752325973,
-                    "heading": -0.12460373302334064,
-                    "angularVelocity": -0.4022019555411679,
-                    "velocityX": 3.6296669127534233,
-                    "velocityY": -0.4977769817483746,
-                    "timestamp": 0.44908908578846873
-                },
-                {
-                    "x": 6.48574789195848,
-                    "y": 6.420787077372237,
-                    "heading": -0.12460373465453414,
-                    "angularVelocity": -1.816113510684907e-8,
-                    "velocityX": 3.5752255434756393,
-                    "velocityY": -1.2362852872781145,
-                    "timestamp": 0.5389069029461625
-                },
-                {
-                    "x": 6.786885646715688,
-                    "y": 6.263423119459949,
-                    "heading": -0.12460372224054271,
-                    "angularVelocity": 1.3821301643987816e-7,
-                    "velocityX": 3.3527618938734416,
-                    "velocityY": -1.7520349847291758,
-                    "timestamp": 0.6287247201038563
-                },
-                {
-                    "x": 7.084493676309923,
-                    "y": 6.108042134366637,
-                    "heading": -0.11377963368638179,
-                    "angularVelocity": 0.12051159665968046,
-                    "velocityX": 3.313463174814436,
-                    "velocityY": -1.729957262494017,
-                    "timestamp": 0.71854253726155
-                },
-                {
-                    "x": 7.322771133319765,
-                    "y": 5.983596419357133,
-                    "heading": -0.07830445075984473,
-                    "angularVelocity": 0.3949682150953753,
-                    "velocityX": 2.652897437837947,
-                    "velocityY": -1.3855348409437966,
-                    "timestamp": 0.8083603544192438
-                },
-                {
-                    "x": 7.5014211847430525,
-                    "y": 5.8902827728941185,
-                    "heading": -0.04357887601405372,
-                    "angularVelocity": 0.38662234114221544,
-                    "velocityX": 1.9890268665695783,
-                    "velocityY": -1.0389213344962835,
-                    "timestamp": 0.8981781715769376
-                },
-                {
-                    "x": 7.620483882250166,
-                    "y": 5.828089677839219,
-                    "heading": -0.01724918141894823,
-                    "angularVelocity": 0.293145563189075,
-                    "velocityX": 1.3256022165187353,
-                    "velocityY": -0.6924360558184839,
-                    "timestamp": 0.9879959887346313
+                    "x": 5.242063425541118,
+                    "y": 6.625917379024557,
+                    "heading": 0.21680591849920633,
+                    "angularVelocity": -0.48090771817524164,
+                    "velocityX": 0.735526954803639,
+                    "velocityY": -0.04545446665971497,
+                    "timestamp": 0.08981781715770647
+                },
+                {
+                    "x": 5.374390185654174,
+                    "y": 6.616899293663565,
+                    "heading": 0.13592925245739512,
+                    "angularVelocity": -0.9004523668149724,
+                    "velocityX": 1.473279626476675,
+                    "velocityY": -0.10040419202302918,
+                    "timestamp": 0.17963563431541293
+                },
+                {
+                    "x": 5.573225936717345,
+                    "y": 6.601485332040172,
+                    "heading": 0.027174297785337913,
+                    "angularVelocity": -1.2108394315695736,
+                    "velocityX": 2.2137673499017074,
+                    "velocityY": -0.1716136298027423,
+                    "timestamp": 0.2694534514731194
+                },
+                {
+                    "x": 5.8386201791233034,
+                    "y": 6.576536765329541,
+                    "heading": -0.0884788299359431,
+                    "angularVelocity": -1.287641265186969,
+                    "velocityX": 2.9548061932964313,
+                    "velocityY": -0.27776857087079165,
+                    "timestamp": 0.35927126863082587
+                },
+                {
+                    "x": 6.16462893780028,
+                    "y": 6.53182752327052,
+                    "heading": -0.12460373299787371,
+                    "angularVelocity": -0.4022019706680331,
+                    "velocityX": 3.6296669078981743,
+                    "velocityY": -0.49777698316268804,
+                    "timestamp": 0.44908908578853235
+                },
+                {
+                    "x": 6.485747891959683,
+                    "y": 6.420787077377064,
+                    "heading": -0.12460373462910332,
+                    "angularVelocity": -1.816153684215185e-8,
+                    "velocityX": 3.5752255434527758,
+                    "velocityY": -1.2362852873443355,
+                    "timestamp": 0.5389069029462388
+                },
+                {
+                    "x": 6.7868856142210525,
+                    "y": 6.263423057279399,
+                    "heading": -0.12460372221510443,
+                    "angularVelocity": 1.382130993675135e-7,
+                    "velocityX": 3.352761532075727,
+                    "velocityY": -1.7520356770790513,
+                    "timestamp": 0.6287247201039452
+                },
+                {
+                    "x": 7.0844936481565455,
+                    "y": 6.108042080528118,
+                    "heading": -0.11377963349551397,
+                    "angularVelocity": 0.12051159850149781,
+                    "velocityX": 3.313463223148008,
+                    "velocityY": -1.7299571696165401,
+                    "timestamp": 0.7185425372616516
+                },
+                {
+                    "x": 7.322771112714566,
+                    "y": 5.983596379959823,
+                    "heading": -0.07830444960996434,
+                    "angularVelocity": 0.39496822577262786,
+                    "velocityX": 2.652897521876331,
+                    "velocityY": -1.3855346801602506,
+                    "timestamp": 0.808360354419358
+                },
+                {
+                    "x": 7.501421172621153,
+                    "y": 5.8902827497271355,
+                    "heading": -0.04357887494407857,
+                    "angularVelocity": 0.3866223402525239,
+                    "velocityX": 1.9890269610193678,
+                    "velocityY": -1.038921153793384,
+                    "timestamp": 0.8981781715770645
+                },
+                {
+                    "x": 7.62048387761172,
+                    "y": 5.828089668977501,
+                    "heading": -0.01724918093308134,
+                    "angularVelocity": 0.29314555668577735,
+                    "velocityX": 1.3256022998366965,
+                    "velocityY": -0.6924358965486016,
+                    "timestamp": 0.9879959887347709
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
                     "heading": -0.003,
-                    "angularVelocity": 0.1586453764950761,
-                    "velocityX": 0.6626315316185003,
-                    "velocityY": -0.346141543215585,
-                    "timestamp": 1.077813805892325
+                    "angularVelocity": 0.15864537108558247,
+                    "velocityX": 0.6626315832612425,
+                    "velocityY": -0.346141444552285,
+                    "timestamp": 1.0778138058924773
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
                     "heading": -0.003,
-                    "angularVelocity": 2.21164904203514e-32,
-                    "velocityX": 6.504499750841485e-34,
-                    "velocityY": 5.343957154186367e-33,
-                    "timestamp": 1.1676316230500188
+                    "angularVelocity": 0,
+                    "velocityX": 4.386752331989148e-33,
+                    "velocityY": -2.33652586669764e-33,
+                    "timestamp": 1.1676316230501838
                 }
             ],
             "constraints": [
@@ -21146,16 +22877,6 @@
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
             "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
                 {
                     "x": 5.61,
                     "y": 5.44,
@@ -21184,7 +22905,161 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 0.26,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.246662494261999,
+                    "y": 6.628692732737936,
+                    "heading": 0.18398762968545185,
+                    "angularVelocity": -0.8088463398355284,
+                    "velocityX": 0.7519183997414132,
+                    "velocityY": -0.013910608703967415,
+                    "timestamp": 0.09397628025368156
+                },
+                {
+                    "x": 5.3885394117424426,
+                    "y": 6.6228569584156665,
+                    "heading": 0.04133864379940616,
+                    "angularVelocity": -1.5179254328962157,
+                    "velocityX": 1.509709866122152,
+                    "velocityY": -0.0620983753189278,
+                    "timestamp": 0.18795256050736311
+                },
+                {
+                    "x": 5.602094427902648,
+                    "y": 6.606624752166298,
+                    "heading": -0.14949344735311892,
+                    "angularVelocity": -2.0306410366252936,
+                    "velocityX": 2.272435295201409,
+                    "velocityY": -0.172726630651381,
+                    "timestamp": 0.2819288407610447
+                },
+                {
+                    "x": 5.885007714232348,
+                    "y": 6.565697735942791,
+                    "heading": -0.3456589598447775,
+                    "angularVelocity": -2.087393882393782,
+                    "velocityX": 3.010475468554385,
+                    "velocityY": -0.4355036836213049,
+                    "timestamp": 0.37590512101472623
+                },
+                {
+                    "x": 6.196762938930662,
+                    "y": 6.464967292979978,
+                    "heading": -0.4305176558630451,
+                    "angularVelocity": -0.9029799412064233,
+                    "velocityX": 3.3173820442430326,
+                    "velocityY": -1.0718709305252299,
+                    "timestamp": 0.4698814012684078
+                },
+                {
+                    "x": 6.507653773495931,
+                    "y": 6.294193106326001,
+                    "heading": -0.4331106719684992,
+                    "angularVelocity": -0.027592240280786797,
+                    "velocityX": 3.3081840835372915,
+                    "velocityY": -1.8172052159649787,
+                    "timestamp": 0.5638576815220894
+                },
+                {
+                    "x": 6.777249226823148,
+                    "y": 6.0624534109685,
+                    "heading": -0.433110646067504,
+                    "angularVelocity": 2.7561204856502443e-7,
+                    "velocityX": 2.8687606340606973,
+                    "velocityY": -2.465938157287546,
+                    "timestamp": 0.6578339617757709
+                },
+                {
+                    "x": 6.99331744885121,
+                    "y": 5.780142381909384,
+                    "heading": -0.4331105937998759,
+                    "angularVelocity": 5.561789419211182e-7,
+                    "velocityX": 2.2991782761011916,
+                    "velocityY": -3.004066859180208,
+                    "timestamp": 0.7518102420294525
+                },
+                {
+                    "x": 7.1465883664001515,
+                    "y": 5.459372959434879,
+                    "heading": -0.43311054984028735,
+                    "angularVelocity": 4.677732342413875e-7,
+                    "velocityX": 1.6309532270823943,
+                    "velocityY": -3.4133019694822235,
+                    "timestamp": 0.845786522283134
+                },
+                {
+                    "x": 7.2797482024400875,
+                    "y": 5.1297467242247805,
+                    "heading": -0.4331105067287741,
+                    "angularVelocity": 4.5874887908824285e-7,
+                    "velocityX": 1.4169515507581365,
+                    "velocityY": -3.507547163180951,
+                    "timestamp": 0.9397628025368155
+                },
+                {
+                    "x": 7.407925031698652,
+                    "y": 4.799268903667217,
+                    "heading": -0.4305589051420175,
+                    "angularVelocity": 0.027151549091629798,
+                    "velocityX": 1.3639274603395894,
+                    "velocityY": -3.5166088683810885,
+                    "timestamp": 1.033739082790497
+                },
+                {
+                    "x": 7.517390810231398,
+                    "y": 4.529437032753176,
+                    "heading": -0.29559925755962413,
+                    "angularVelocity": 1.4361033147734772,
+                    "velocityX": 1.164823487770018,
+                    "velocityY": -2.871276349581525,
+                    "timestamp": 1.1277153630441787
+                },
+                {
+                    "x": 7.59899827381295,
+                    "y": 4.327770783855267,
+                    "heading": -0.16069313013903275,
+                    "angularVelocity": 1.4355338076419173,
+                    "velocityX": 0.8683836321384404,
+                    "velocityY": -2.1459271249460685,
+                    "timestamp": 1.2216916432978604
+                },
+                {
+                    "x": 7.653087401223331,
+                    "y": 4.193799869992724,
+                    "heading": -0.05829734753961687,
+                    "angularVelocity": 1.0895917812772173,
+                    "velocityX": 0.5755614849233384,
+                    "velocityY": -1.4255822160751557,
+                    "timestamp": 1.315667923551542
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": -0.003,
+                    "angularVelocity": 0.5884181347713066,
+                    "velocityX": 0.28637650590148805,
+                    "velocityY": -0.7108162805806236,
+                    "timestamp": 1.4096442038052237
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": -0.003,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 2.8051389340682807e-33,
+                    "timestamp": 1.5036204840589054
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -21203,16 +23078,6 @@
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
             "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
                 {
                     "x": 5.61,
                     "y": 5.44,
@@ -21229,7 +23094,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 19
+                    "controlIntervalCount": 11
+                },
+                {
+                    "x": 6.950631618499756,
+                    "y": 6.338046073913574,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 15
                 },
                 {
                     "x": 7.68,
@@ -21241,7 +23115,251 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 0.26,
+                    "angularVelocity": 0,
+                    "velocityX": -1.7123315632321391e-31,
+                    "velocityY": 4.571975133912053e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.2081944432473115,
+                    "y": 6.63354239454729,
+                    "heading": 0.2524699257138384,
+                    "angularVelocity": -0.12069194203891866,
+                    "velocityX": 0.5160121574790498,
+                    "velocityY": 0.0567774581143745,
+                    "timestamp": 0.062390861883169
+                },
+                {
+                    "x": 5.2726441787772975,
+                    "y": 6.640106281427696,
+                    "heading": 0.23785051964847434,
+                    "angularVelocity": -0.2343196683632906,
+                    "velocityX": 1.0329996025807835,
+                    "velocityY": 0.1052059016703473,
+                    "timestamp": 0.124781723766338
+                },
+                {
+                    "x": 5.369418980987113,
+                    "y": 6.648966513737627,
+                    "heading": 0.21675536507543205,
+                    "angularVelocity": -0.33811288923278293,
+                    "velocityX": 1.551105390898958,
+                    "velocityY": 0.14201169918963874,
+                    "timestamp": 0.187172585649507
+                },
+                {
+                    "x": 5.498590594297912,
+                    "y": 6.659046571836944,
+                    "heading": 0.19009632617593986,
+                    "angularVelocity": -0.42729076173705777,
+                    "velocityX": 2.070361097955043,
+                    "velocityY": 0.16156305258605722,
+                    "timestamp": 0.249563447532676
+                },
+                {
+                    "x": 5.6601948909824875,
+                    "y": 6.668590441412665,
+                    "heading": 0.15936442465931713,
+                    "angularVelocity": -0.49257055583183496,
+                    "velocityX": 2.5901917653772815,
+                    "velocityY": 0.15296902924013772,
+                    "timestamp": 0.311954309415845
+                },
+                {
+                    "x": 5.854024302844021,
+                    "y": 6.674275452040902,
+                    "heading": 0.12740102584180835,
+                    "angularVelocity": -0.512308980077189,
+                    "velocityX": 3.106695532183737,
+                    "velocityY": 0.09111928344386595,
+                    "timestamp": 0.374345171299014
+                },
+                {
+                    "x": 6.078018113514707,
+                    "y": 6.668089747759068,
+                    "heading": 0.10126004011963459,
+                    "angularVelocity": -0.4189874115076864,
+                    "velocityX": 3.590170161299073,
+                    "velocityY": -0.09914439543117176,
+                    "timestamp": 0.436736033182183
+                },
+                {
+                    "x": 6.311203816285634,
+                    "y": 6.63161682590639,
+                    "heading": 0.10125992071184643,
+                    "angularVelocity": -0.000001913866624365403,
+                    "velocityX": 3.737497699704501,
+                    "velocityY": -0.584587562213517,
+                    "timestamp": 0.499126895065352
+                },
+                {
+                    "x": 6.53718081427167,
+                    "y": 6.563497401833266,
+                    "heading": 0.10125989664992657,
+                    "angularVelocity": -3.856641685769623e-7,
+                    "velocityX": 3.621956664249834,
+                    "velocityY": -1.0918173273625358,
+                    "timestamp": 0.561517756948521
+                },
+                {
+                    "x": 6.7516755548198235,
+                    "y": 6.465019697743108,
+                    "heading": 0.10125985817262735,
+                    "angularVelocity": -6.167136938553595e-7,
+                    "velocityX": 3.4379191771674527,
+                    "velocityY": -1.5783994821960954,
+                    "timestamp": 0.62390861883169
+                },
+                {
+                    "x": 6.950631618499756,
+                    "y": 6.338046073913574,
+                    "heading": 0.10125971935519859,
+                    "angularVelocity": -0.000002224964167076665,
+                    "velocityX": 3.1888654471946203,
+                    "velocityY": -2.035131748416977,
+                    "timestamp": 0.686299480714859
+                },
+                {
+                    "x": 7.196058677643184,
+                    "y": 6.101503366378845,
+                    "heading": 0.10125966337035743,
+                    "angularVelocity": -6.213285755242982e-7,
+                    "velocityX": 2.723788105663008,
+                    "velocityY": -2.625188174087521,
+                    "timestamp": 0.7764045291337008
+                },
+                {
+                    "x": 7.389899318639429,
+                    "y": 5.821123760827136,
+                    "heading": 0.10125964077104323,
+                    "angularVelocity": -2.508107435019506e-7,
+                    "velocityX": 2.1512739230237647,
+                    "velocityY": -3.1116969633976366,
+                    "timestamp": 0.8665095775525427
+                },
+                {
+                    "x": 7.524507781478929,
+                    "y": 5.507966472295818,
+                    "heading": 0.101259626680438,
+                    "angularVelocity": -1.5637975290365853e-7,
+                    "velocityX": 1.4939058931947202,
+                    "velocityY": -3.475468844716046,
+                    "timestamp": 0.9566146259713845
+                },
+                {
+                    "x": 7.594574766806078,
+                    "y": 5.174383579102459,
+                    "heading": 0.10125961555168732,
+                    "angularVelocity": -1.235086255399969e-7,
+                    "velocityX": 0.7776144240159958,
+                    "velocityY": -3.702155418004344,
+                    "timestamp": 1.0467196743902263
+                },
+                {
+                    "x": 7.605242469312774,
+                    "y": 4.833688510035603,
+                    "heading": 0.10125960514477245,
+                    "angularVelocity": -1.1549757817967306e-7,
+                    "velocityX": 0.11839184034432584,
+                    "velocityY": -3.781087464524514,
+                    "timestamp": 1.136824722809068
+                },
+                {
+                    "x": 7.615908675824772,
+                    "y": 4.492993394135096,
+                    "heading": 0.10125959473787158,
+                    "angularVelocity": -1.1549742264017462e-7,
+                    "velocityX": 0.11837523756082047,
+                    "velocityY": -3.781087984291716,
+                    "timestamp": 1.2269297712279097
+                },
+                {
+                    "x": 7.6265748461946865,
+                    "y": 4.152298277103123,
+                    "heading": 0.10125958433097336,
+                    "angularVelocity": -1.1549739343149193e-7,
+                    "velocityX": 0.11837483645025074,
+                    "velocityY": -3.781087996848918,
+                    "timestamp": 1.3170348196467514
+                },
+                {
+                    "x": 7.637240986091898,
+                    "y": 3.8116031591171406,
+                    "heading": 0.10125957392407539,
+                    "angularVelocity": -1.1549739036497336e-7,
+                    "velocityX": 0.11837449825941278,
+                    "velocityY": -3.78108800743665,
+                    "timestamp": 1.407139868065593
+                },
+                {
+                    "x": 7.647907101492937,
+                    "y": 3.470908040364265,
+                    "heading": 0.10125956351717343,
+                    "angularVelocity": -1.1549743491773867e-7,
+                    "velocityX": 0.11837422639702805,
+                    "velocityY": -3.781088015947766,
+                    "timestamp": 1.4972449164844348
+                },
+                {
+                    "x": 7.658649980665503,
+                    "y": 3.1330359890940187,
+                    "heading": 0.09207038783752043,
+                    "angularVelocity": -0.10198291706072676,
+                    "velocityX": 0.11922616280763662,
+                    "velocityY": -3.7497571689844853,
+                    "timestamp": 1.5873499649032765
+                },
+                {
+                    "x": 7.667206120655169,
+                    "y": 2.862563871388905,
+                    "heading": 0.061604548821908564,
+                    "angularVelocity": -0.33811467337536444,
+                    "velocityX": 0.09495738740298482,
+                    "velocityY": -3.0017421049246735,
+                    "timestamp": 1.6774550133221182
+                },
+                {
+                    "x": 7.673608699461006,
+                    "y": 2.659753859006467,
+                    "heading": 0.031813701428875164,
+                    "angularVelocity": -0.33062351017837394,
+                    "velocityX": 0.07105682665061783,
+                    "velocityY": -2.250817417462574,
+                    "timestamp": 1.76756006174096
+                },
+                {
+                    "x": 7.677870998153504,
+                    "y": 2.524576540197495,
+                    "heading": 0.009226184845951672,
+                    "angularVelocity": -0.25067981183391225,
+                    "velocityX": 0.04730366130750142,
+                    "velocityY": -1.5002191462193974,
+                    "timestamp": 1.8576651101598016
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": -0.003,
+                    "angularVelocity": -0.1356881224803119,
+                    "velocityX": 0.023627997363707604,
+                    "velocityY": -0.7499750722442785,
+                    "timestamp": 1.9477701585786433
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": -0.003,
+                    "angularVelocity": 0,
+                    "velocityX": -1.899719241029588e-32,
+                    "velocityY": -4.254972095731821e-34,
+                    "timestamp": 2.037875206997485
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -21260,16 +23378,6 @@
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
             "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
                 {
                     "x": 5.61,
                     "y": 5.44,
@@ -21286,7 +23394,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 23
+                    "controlIntervalCount": 12
+                },
+                {
+                    "x": 7.0632171630859375,
+                    "y": 6.06283712387085,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 19
                 },
                 {
                     "x": 7.68,
@@ -21298,7 +23415,296 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 0.26,
+                    "angularVelocity": -8.010592837331029e-32,
+                    "velocityX": 9.396271853657066e-34,
+                    "velocityY": -2.4298772375808502e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.207063492876026,
+                    "y": 6.6289176408913075,
+                    "heading": 0.24374543730705361,
+                    "angularVelocity": -0.264834788594711,
+                    "velocityX": 0.5061159579768882,
+                    "velocityY": -0.01763482359684202,
+                    "timestamp": 0.06137623678217253
+                },
+                {
+                    "x": 5.269220913885746,
+                    "y": 6.626580685647671,
+                    "heading": 0.2121773997323976,
+                    "angularVelocity": -0.514336479877237,
+                    "velocityX": 1.0127277961065038,
+                    "velocityY": -0.03807589657100854,
+                    "timestamp": 0.12275247356434506
+                },
+                {
+                    "x": 5.36250981468421,
+                    "y": 6.6227475288112485,
+                    "heading": 0.16662318019476377,
+                    "angularVelocity": -0.7422126530713462,
+                    "velocityX": 1.5199514615005028,
+                    "velocityY": -0.06245343535846346,
+                    "timestamp": 0.1841287103465176
+                },
+                {
+                    "x": 5.486975524936631,
+                    "y": 6.617054798693029,
+                    "heading": 0.10910232843404971,
+                    "angularVelocity": -0.9371844019185364,
+                    "velocityX": 2.0279136808950478,
+                    "velocityY": -0.0927513711605005,
+                    "timestamp": 0.24550494712869012
+                },
+                {
+                    "x": 5.642662315095453,
+                    "y": 6.608892934358269,
+                    "heading": 0.04300979987381644,
+                    "angularVelocity": -1.076842309423355,
+                    "velocityX": 2.536597196588689,
+                    "velocityY": -0.1329808532205177,
+                    "timestamp": 0.30688118391086266
+                },
+                {
+                    "x": 5.829535978248522,
+                    "y": 6.597025931717244,
+                    "heading": -0.024847542112422006,
+                    "angularVelocity": -1.1055963275666998,
+                    "velocityX": 3.0447233807490215,
+                    "velocityY": -0.19334848897856358,
+                    "timestamp": 0.3682574206930352
+                },
+                {
+                    "x": 6.046434546533161,
+                    "y": 6.5777851666622515,
+                    "heading": -0.07267029178585922,
+                    "angularVelocity": -0.7791737027338695,
+                    "velocityX": 3.533917679808606,
+                    "velocityY": -0.31348883645767445,
+                    "timestamp": 0.4296336574752077
+                },
+                {
+                    "x": 6.274079716361941,
+                    "y": 6.532107529947078,
+                    "heading": -0.07267036448034916,
+                    "angularVelocity": -0.0000011844077407082938,
+                    "velocityX": 3.709011528952235,
+                    "velocityY": -0.7442234830605138,
+                    "timestamp": 0.49100989425738023
+                },
+                {
+                    "x": 6.493476634626264,
+                    "y": 6.456122214314626,
+                    "heading": -0.07267035803800323,
+                    "angularVelocity": 1.049648246960038e-7,
+                    "velocityX": 3.5746231728571716,
+                    "velocityY": -1.238025001469686,
+                    "timestamp": 0.5523861310395528
+                },
+                {
+                    "x": 6.700610073343003,
+                    "y": 6.351219816335689,
+                    "heading": -0.07267035070446132,
+                    "angularVelocity": 1.1948503655702063e-7,
+                    "velocityX": 3.3748149051866063,
+                    "velocityY": -1.7091695985083282,
+                    "timestamp": 0.6137623678217253
+                },
+                {
+                    "x": 6.891689244621814,
+                    "y": 6.21932014936447,
+                    "heading": -0.07267034149477326,
+                    "angularVelocity": 1.5005299351249212e-7,
+                    "velocityX": 3.1132435173071933,
+                    "velocityY": -2.1490347712151214,
+                    "timestamp": 0.6751386046038979
+                },
+                {
+                    "x": 7.0632171630859375,
+                    "y": 6.06283712387085,
+                    "heading": -0.07267032569899272,
+                    "angularVelocity": 2.573598736854518e-7,
+                    "velocityX": 2.7946959190881038,
+                    "velocityY": -2.549570219643589,
+                    "timestamp": 0.7365148413860704
+                },
+                {
+                    "x": 7.263317671759933,
+                    "y": 5.7907628781558955,
+                    "heading": -0.07267031513885969,
+                    "angularVelocity": 1.182832965996254e-7,
+                    "velocityX": 2.24131152203967,
+                    "velocityY": -3.0474842158680824,
+                    "timestamp": 0.8257931530827405
+                },
+                {
+                    "x": 7.406264443711533,
+                    "y": 5.484771313458312,
+                    "heading": -0.07267030869611313,
+                    "angularVelocity": 7.216474459034548e-8,
+                    "velocityX": 1.6011365944875051,
+                    "velocityY": -3.4273896860551396,
+                    "timestamp": 0.9150714647794106
+                },
+                {
+                    "x": 7.486522259331217,
+                    "y": 5.1567114044359705,
+                    "heading": -0.07267030368970051,
+                    "angularVelocity": 5.607647052205946e-8,
+                    "velocityX": 0.8989620669840581,
+                    "velocityY": -3.6745756364317224,
+                    "timestamp": 1.0043497764760807
+                },
+                {
+                    "x": 7.50149841248159,
+                    "y": 4.81930906784098,
+                    "heading": -0.07267029905812865,
+                    "angularVelocity": 5.187790600157932e-8,
+                    "velocityX": 0.1677468230047605,
+                    "velocityY": -3.7792195011633143,
+                    "timestamp": 1.093628088172751
+                },
+                {
+                    "x": 7.516449925771596,
+                    "y": 4.481905638472984,
+                    "heading": -0.0726702944266145,
+                    "angularVelocity": 5.1877259457701335e-8,
+                    "velocityX": 0.16747083368699095,
+                    "velocityY": -3.7792317412357557,
+                    "timestamp": 1.1829063998694211
+                },
+                {
+                    "x": 7.531401402222247,
+                    "y": 4.144502207472513,
+                    "heading": -0.07267028979509418,
+                    "angularVelocity": 5.187732873980297e-8,
+                    "velocityX": 0.16747042105201052,
+                    "velocityY": -3.779231759520992,
+                    "timestamp": 1.2721847115660914
+                },
+                {
+                    "x": 7.546352847973806,
+                    "y": 3.807098775111667,
+                    "heading": -0.07267028516356967,
+                    "angularVelocity": 5.1877375603275234e-8,
+                    "velocityX": 0.16747007719365256,
+                    "velocityY": -3.7792317747584647,
+                    "timestamp": 1.3614630232627616
+                },
+                {
+                    "x": 7.561304268557873,
+                    "y": 3.4696953416355707,
+                    "heading": -0.07267028053204275,
+                    "angularVelocity": 5.1877402813012475e-8,
+                    "velocityX": 0.16746979529438435,
+                    "velocityY": -3.7792317872503007,
+                    "timestamp": 1.4507413349594318
+                },
+                {
+                    "x": 7.576255668582546,
+                    "y": 3.132291907248425,
+                    "heading": -0.07267027590051456,
+                    "angularVelocity": 5.1877416880821484e-8,
+                    "velocityX": 0.16746956501007867,
+                    "velocityY": -3.779231797454896,
+                    "timestamp": 1.540019646656102
+                },
+                {
+                    "x": 7.591207051730301,
+                    "y": 2.794888472113414,
+                    "heading": -0.07267027126898577,
+                    "angularVelocity": 5.1877423431260937e-8,
+                    "velocityX": 0.16746937597291248,
+                    "velocityY": -3.779231805831691,
+                    "timestamp": 1.6292979583527722
+                },
+                {
+                    "x": 7.60615842090353,
+                    "y": 2.457485036359151,
+                    "heading": -0.07267026663745696,
+                    "angularVelocity": 5.1877423851232326e-8,
+                    "velocityX": 0.16746921944524512,
+                    "velocityY": -3.779231812767884,
+                    "timestamp": 1.7185762700494425
+                },
+                {
+                    "x": 7.621109778388665,
+                    "y": 2.1200816000869556,
+                    "heading": -0.07267026200592838,
+                    "angularVelocity": 5.187742117887202e-8,
+                    "velocityX": 0.1674690885277524,
+                    "velocityY": -3.779231818569208,
+                    "timestamp": 1.8078545817461127
+                },
+                {
+                    "x": 7.636061125621916,
+                    "y": 1.7826781633604734,
+                    "heading": -0.07267025737439563,
+                    "angularVelocity": 5.1877468094637746e-8,
+                    "velocityX": 0.16746897369713376,
+                    "velocityY": -3.7792318236576405,
+                    "timestamp": 1.897132893442783
+                },
+                {
+                    "x": 7.6506850878742485,
+                    "y": 1.4507667606163608,
+                    "heading": -0.05536549896524321,
+                    "angularVelocity": 0.19382936438075055,
+                    "velocityX": 0.16380195788222543,
+                    "velocityY": -3.7177159428351065,
+                    "timestamp": 1.9864112051394531
+                },
+                {
+                    "x": 7.662404359721407,
+                    "y": 1.1852476637582392,
+                    "heading": -0.0353858682020255,
+                    "angularVelocity": 0.2237904187872895,
+                    "velocityX": 0.13126672788097452,
+                    "velocityY": -2.974060461181691,
+                    "timestamp": 2.0756895168361233
+                },
+                {
+                    "x": 7.6711999602966845,
+                    "y": 0.9861189087552604,
+                    "heading": -0.018423007440779238,
+                    "angularVelocity": 0.18999979321832794,
+                    "velocityX": 0.0985188945458782,
+                    "velocityY": -2.2304269784976873,
+                    "timestamp": 2.1649678285327933
+                },
+                {
+                    "x": 7.677066105120016,
+                    "y": 0.8533716446291538,
+                    "heading": -0.006322956528169105,
+                    "angularVelocity": 0.13553180702747777,
+                    "velocityX": 0.06570626966225444,
+                    "velocityY": -1.4868926349898441,
+                    "timestamp": 2.2542461402294633
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": -2.444948224573009e-31,
+                    "angularVelocity": 0.0708229849781762,
+                    "velocityX": 0.03286234724008497,
+                    "velocityY": -0.7434240563896022,
+                    "timestamp": 2.3435244519261333
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 0,
+                    "angularVelocity": 1.883272219480887e-30,
+                    "velocityX": 0,
+                    "velocityY": -3.281240907044904e-32,
+                    "timestamp": 2.4328027636228033
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -21317,16 +23723,6 @@
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
             "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
                 {
                     "x": 5.61,
                     "y": 5.44,
@@ -21359,128 +23755,128 @@
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": -7.967068308454128e-24,
-                    "angularVelocity": 3.748757739861167e-23,
-                    "velocityX": 2.372714537320914e-25,
-                    "velocityY": 2.748536601620217e-25,
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": 8.330960047205791e-35,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 4.215224464845852,
-                    "y": 5.589131496345612,
-                    "heading": 0.013109636548148297,
-                    "angularVelocity": 0.15985714241706056,
-                    "velocityX": -0.5825684410958524,
-                    "velocityY": 0.3552255429080149,
-                    "timestamp": 0.08200845048228003
-                },
-                {
-                    "x": 4.119673396532112,
-                    "y": 5.647394589751744,
-                    "heading": 0.03932466572409392,
-                    "angularVelocity": 0.31966253504094716,
-                    "velocityX": -1.1651368578703514,
-                    "velocityY": 0.7104523139200205,
-                    "timestamp": 0.16401690096456006
-                },
-                {
-                    "x": 3.9763468054990545,
-                    "y": 5.734789450682316,
-                    "heading": 0.07863876132846631,
-                    "angularVelocity": 0.47939078684174374,
-                    "velocityX": -1.7477051473375471,
-                    "velocityY": 1.06568116354614,
-                    "timestamp": 0.2460253514468401
-                },
-                {
-                    "x": 3.785244738141483,
-                    "y": 5.8513163218996445,
-                    "heading": 0.131044995583191,
-                    "angularVelocity": 0.6390345622497571,
-                    "velocityX": -2.330272871072761,
-                    "velocityY": 1.4209129733832384,
-                    "timestamp": 0.32803380192912013
-                },
-                {
-                    "x": 3.5463673151125503,
-                    "y": 5.996975506896697,
-                    "heading": 0.19653801558690542,
-                    "angularVelocity": 0.7986130650995023,
-                    "velocityX": -2.9128391235796927,
-                    "velocityY": 1.776148483972697,
-                    "timestamp": 0.41004225241140013
-                },
-                {
-                    "x": 3.2814945553511796,
-                    "y": 6.158490181857634,
-                    "heading": 0.19653800981726008,
-                    "angularVelocity": -7.035427786522906e-8,
-                    "velocityX": -3.2298227585534365,
-                    "velocityY": 1.9694881931202608,
-                    "timestamp": 0.49205070289368014
-                },
-                {
-                    "x": 3.016621781154659,
-                    "y": 6.320004832717382,
-                    "heading": 0.19653801616374372,
-                    "angularVelocity": 7.738816679079332e-8,
-                    "velocityX": -3.229822934573717,
-                    "velocityY": 1.9694878992335856,
-                    "timestamp": 0.5740591533759601
-                },
-                {
-                    "x": 2.7777476228533557,
-                    "y": 6.465669683581835,
-                    "heading": 0.2620170313059908,
-                    "angularVelocity": 0.7984422917049926,
-                    "velocityX": -2.9127993139306705,
-                    "velocityY": 1.7762175727966987,
-                    "timestamp": 0.6560676038582401
-                },
-                {
-                    "x": 2.5866484818505064,
-                    "y": 6.582201734022544,
-                    "heading": 0.31440674809174973,
-                    "angularVelocity": 0.6388331504578179,
-                    "velocityX": -2.330237187497417,
-                    "velocityY": 1.4209761281355728,
-                    "timestamp": 0.7380760543405201
-                },
-                {
-                    "x": 2.443324221748551,
-                    "y": 6.669600847656536,
-                    "heading": 0.3537026369190334,
-                    "angularVelocity": 0.47916877585407597,
-                    "velocityX": -1.7476767242776265,
-                    "velocityY": 1.0657330204388804,
-                    "timestamp": 0.8200845048228002
-                },
-                {
-                    "x": 2.347774743561573,
-                    "y": 6.727866948074407,
-                    "heading": 0.37990101547487837,
-                    "angularVelocity": 0.31945949962200243,
-                    "velocityX": -1.1651174680787846,
-                    "velocityY": 0.7104889810161812,
-                    "timestamp": 0.9020929553050802
+                    "x": 4.215224456008129,
+                    "y": 5.5891314798928855,
+                    "heading": 0.013109691961439362,
+                    "angularVelocity": 0.15985781811927008,
+                    "velocityX": -0.5825685488618567,
+                    "velocityY": 0.35522534228568337,
+                    "timestamp": 0.08200845048227925
+                },
+                {
+                    "x": 4.11967337131145,
+                    "y": 5.647394543004508,
+                    "heading": 0.03932481805987993,
+                    "angularVelocity": 0.31966371690079975,
+                    "velocityX": -1.1651370576417237,
+                    "velocityY": 0.7104519445128596,
+                    "timestamp": 0.1640169009645585
+                },
+                {
+                    "x": 3.976346757700282,
+                    "y": 5.734789362523958,
+                    "heading": 0.07863903763442207,
+                    "angularVelocity": 0.47939229851730114,
+                    "velocityX": -1.7477054226519961,
+                    "velocityY": 1.0656806585844962,
+                    "timestamp": 0.24602535144683776
+                },
+                {
+                    "x": 3.7852446629452063,
+                    "y": 5.851316183985278,
+                    "heading": 0.13104540832787342,
+                    "angularVelocity": 0.6390362259652201,
+                    "velocityX": -2.3302732051542674,
+                    "velocityY": 1.4209123666652002,
+                    "timestamp": 0.328033801929117
+                },
+                {
+                    "x": 3.5463672091086966,
+                    "y": 5.996975313640393,
+                    "heading": 0.19653856458379848,
+                    "angularVelocity": 0.7986147265406159,
+                    "velocityX": -2.912839499243156,
+                    "velocityY": 1.77614780914058,
+                    "timestamp": 0.41004225241139625
+                },
+                {
+                    "x": 3.281494446824011,
+                    "y": 6.15848998396632,
+                    "heading": 0.19653857286419524,
+                    "angularVelocity": 1.009700426817188e-7,
+                    "velocityX": -3.229822789322431,
+                    "velocityY": 1.9694881366015824,
+                    "timestamp": 0.4920507028936755
+                },
+                {
+                    "x": 3.016621684855716,
+                    "y": 6.320004654811106,
+                    "heading": 0.1965385811445921,
+                    "angularVelocity": 1.0097004416983622e-7,
+                    "velocityX": -3.229822785464406,
+                    "velocityY": 1.969488142928474,
+                    "timestamp": 0.5740591533759547
+                },
+                {
+                    "x": 2.77774755563319,
+                    "y": 6.465669559960414,
+                    "heading": 0.2620174097526663,
+                    "angularVelocity": 0.7984400171324189,
+                    "velocityX": -2.9127989593480157,
+                    "velocityY": 1.7762182347389206,
+                    "timestamp": 0.6560676038582339
+                },
+                {
+                    "x": 2.5866484397575076,
+                    "y": 6.582201656937504,
+                    "heading": 0.3144069758542471,
+                    "angularVelocity": 0.6388313130352502,
+                    "velocityX": -2.330236881100143,
+                    "velocityY": 1.4209766955939638,
+                    "timestamp": 0.7380760543405132
+                },
+                {
+                    "x": 2.443324199843316,
+                    "y": 6.6696008076986475,
+                    "heading": 0.35370275100271265,
+                    "angularVelocity": 0.4791673896698825,
+                    "velocityX": -1.7476764781107736,
+                    "velocityY": 1.0657334731623573,
+                    "timestamp": 0.8200845048227924
+                },
+                {
+                    "x": 2.347774735979553,
+                    "y": 6.727866934294423,
+                    "heading": 0.37990105353306003,
+                    "angularVelocity": 0.3194585725773273,
+                    "velocityX": -1.1651172934234333,
+                    "velocityY": 0.7104893002260324,
+                    "timestamp": 0.9020929553050716
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.393,
-                    "angularVelocity": 0.15972725308292482,
-                    "velocityX": -0.5825587885226076,
-                    "velocityY": 0.355244511440778,
-                    "timestamp": 0.9841014057873602
+                    "angularVelocity": 0.15972678900658524,
+                    "velocityX": -0.5825586960684825,
+                    "velocityY": 0.3552446794720561,
+                    "timestamp": 0.9841014057873508
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.393,
-                    "angularVelocity": 1.2555409321863598e-23,
-                    "velocityX": -4.0981987257829375e-24,
-                    "velocityY": -7.824281389954736e-24,
-                    "timestamp": 1.0661098562696403
+                    "angularVelocity": -4.59054414969708e-34,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 1.06610985626963
                 }
             ],
             "constraints": [
@@ -21500,23 +23896,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "S2 To W2": {
             "waypoints": [
@@ -21543,101 +23923,101 @@
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": -1.042790690974161e-34,
-                    "angularVelocity": 1.2672485017804847e-33,
+                    "heading": 0,
+                    "angularVelocity": 0,
                     "velocityX": 0,
-                    "velocityY": 1.0649309840552648e-32,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 4.181096344253041,
-                    "y": 5.560000047617906,
-                    "heading": -1.1491589479255403e-8,
-                    "angularVelocity": -1.1594790933974305e-7,
-                    "velocityX": -0.8263919813282988,
-                    "velocityY": 4.80455414778758e-7,
-                    "timestamp": 0.09910993523353247
+                    "x": 4.181096344253034,
+                    "y": 5.56,
+                    "heading": 2.5710705899848466e-19,
+                    "angularVelocity": 2.5941602968880693e-18,
+                    "velocityX": -0.8263919813283321,
+                    "velocityY": -1.4479549544654444e-34,
+                    "timestamp": 0.099109935233537
                 },
                 {
-                    "x": 4.017289035836251,
-                    "y": 5.560000133094268,
-                    "heading": -2.0027929206394758e-8,
-                    "angularVelocity": -8.613001771881656e-8,
-                    "velocityX": -1.6527839316089912,
-                    "velocityY": 8.624398858779209e-7,
-                    "timestamp": 0.19821987046706493
+                    "x": 4.017289035836238,
+                    "y": 5.56,
+                    "heading": 7.7182898862159e-19,
+                    "angularVelocity": 5.193444314378615e-18,
+                    "velocityX": -1.6527839316089838,
+                    "velocityY": -7.36495654370163e-34,
+                    "timestamp": 0.198219870467074
                 },
                 {
-                    "x": 3.771578080903882,
-                    "y": 5.560000246435489,
-                    "heading": -1.061567052726606e-8,
-                    "angularVelocity": 9.49678480632688e-8,
-                    "velocityX": -2.4791758197944636,
-                    "velocityY": 0.0000011435908848510677,
-                    "timestamp": 0.2973298057005974
+                    "x": 3.771578080903875,
+                    "y": 5.56,
+                    "heading": 1.545129672215716e-18,
+                    "angularVelocity": 7.802453727507411e-18,
+                    "velocityX": -2.479175819794286,
+                    "velocityY": -8.54847043233614e-34,
+                    "timestamp": 0.297329805700611
                 },
                 {
-                    "x": 3.443963497918667,
-                    "y": 5.560000377513817,
-                    "heading": 3.109895895304223e-8,
-                    "angularVelocity": 4.208924841656569e-7,
-                    "velocityX": -3.30556752169455,
-                    "velocityY": 0.0000013225548252865442,
-                    "timestamp": 0.39643974093412987
+                    "x": 3.4439634979187304,
+                    "y": 5.56,
+                    "heading": 2.5806642382479214e-18,
+                    "angularVelocity": 1.0448342676399075e-17,
+                    "velocityX": -3.3055675216936846,
+                    "velocityY": -5.0910568375180236e-34,
+                    "timestamp": 0.396439740934148
                 },
                 {
-                    "x": 3.0690365020812136,
-                    "y": 5.5600003676341405,
-                    "heading": 3.1098967052884415e-8,
-                    "angularVelocity": 8.336617080453033e-14,
-                    "velocityX": -3.7829405796100413,
-                    "velocityY": -9.968399941565162e-8,
-                    "timestamp": 0.4955496761676623
+                    "x": 3.0690365020812695,
+                    "y": 5.56,
+                    "heading": -2.5850068638158606e-18,
+                    "angularVelocity": -5.212061828001078e-17,
+                    "velocityX": -3.7829405796099502,
+                    "velocityY": -4.536187828596351e-34,
+                    "timestamp": 0.495549676167685
                 },
                 {
-                    "x": 2.7414219190960516,
-                    "y": 5.560000235969226,
-                    "heading": 3.331211577627848e-8,
-                    "angularVelocity": 2.233025994415073e-8,
-                    "velocityX": -3.3055675216940097,
-                    "velocityY": -0.0000013284733736467011,
-                    "timestamp": 0.5946596114011948
+                    "x": 2.741421919096125,
+                    "y": 5.56,
+                    "heading": -1.5478395588580842e-18,
+                    "angularVelocity": 1.0464816695997814e-17,
+                    "velocityX": -3.305567521693684,
+                    "velocityY": -2.872506388563931e-33,
+                    "timestamp": 0.594659611401222
                 },
                 {
-                    "x": 2.495710964163716,
-                    "y": 5.560000125328881,
-                    "heading": 2.2528078300409453e-8,
-                    "angularVelocity": -1.088088286759601e-7,
-                    "velocityX": -2.4791758197941256,
-                    "velocityY": -0.0000011163395746408495,
-                    "timestamp": 0.6937695466347273
+                    "x": 2.4957109641637625,
+                    "y": 5.56,
+                    "heading": -7.728706253244835e-19,
+                    "angularVelocity": 7.819286045545264e-18,
+                    "velocityX": -2.479175819794286,
+                    "velocityY": -1.6595590840772702e-33,
+                    "timestamp": 0.6937695466347591
                 },
                 {
-                    "x": 2.3319036557469475,
-                    "y": 5.560000044126654,
-                    "heading": 9.093200341480335e-9,
-                    "angularVelocity": -1.3555529933337065e-7,
-                    "velocityX": -1.6527839316087698,
-                    "velocityY": -8.193146909757129e-7,
-                    "timestamp": 0.7928794818682598
+                    "x": 2.3319036557469652,
+                    "y": 5.56,
+                    "heading": -2.5738443375571516e-19,
+                    "angularVelocity": 5.201155568994596e-18,
+                    "velocityX": -1.6527839316089838,
+                    "velocityY": -1.4347514653062086e-33,
+                    "timestamp": 0.792879481868296
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 7.024905960060336e-33,
-                    "angularVelocity": -9.17486204713572e-8,
-                    "velocityX": -0.826391981328188,
-                    "velocityY": -4.4522936458958866e-7,
-                    "timestamp": 0.8919894171017924
+                    "heading": 0,
+                    "angularVelocity": 2.5969589542999405e-18,
+                    "velocityX": -0.8263919813283321,
+                    "velocityY": 2.9921012122849815e-33,
+                    "timestamp": 0.891989417101833
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
                     "heading": 0,
-                    "angularVelocity": 2.1129142513587384e-31,
-                    "velocityX": -1.9174161859426788e-38,
-                    "velocityY": -1.530570219467473e-32,
-                    "timestamp": 0.9910993523353249
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 1.808288986442481e-43,
+                    "timestamp": 0.9910993523353699
                 }
             ],
             "constraints": [
@@ -21657,23 +24037,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "S2 To W3": {
             "waypoints": [
@@ -21700,128 +24064,128 @@
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 3.6296173977413556e-31,
-                    "angularVelocity": 2.1802637913498067e-31,
-                    "velocityX": -2.5358916422596453e-32,
-                    "velocityY": 1.2879635720357984e-31,
+                    "heading": 9.014129666558106e-32,
+                    "angularVelocity": 9.039417129994225e-31,
+                    "velocityX": 0,
+                    "velocityY": -2.779554313855136e-31,
                     "timestamp": 0
                 },
                 {
-                    "x": 4.206523470432328,
-                    "y": 5.554409314419878,
-                    "heading": -0.10760113621403576,
-                    "angularVelocity": -1.2344151326641672,
-                    "velocityX": -0.6479065667114609,
-                    "velocityY": -0.06413711903878926,
-                    "timestamp": 0.08716770668697735
-                },
-                {
-                    "x": 4.091196970051195,
-                    "y": 5.54119730250816,
-                    "heading": -0.2959135822275083,
-                    "angularVelocity": -2.1603464536437884,
-                    "velocityX": -1.3230415800116804,
-                    "velocityY": -0.15157002993279564,
-                    "timestamp": 0.1743354133739547
-                },
-                {
-                    "x": 3.9151950635806365,
-                    "y": 5.5163708141067165,
-                    "heading": -0.5340651676870422,
-                    "angularVelocity": -2.732107961894037,
-                    "velocityX": -2.0191182395401066,
-                    "velocityY": -0.28481291231621586,
-                    "timestamp": 0.261503120060932
-                },
-                {
-                    "x": 3.678758755729211,
-                    "y": 5.473177786204715,
-                    "heading": -0.7546652286499241,
-                    "angularVelocity": -2.5307544427555704,
-                    "velocityX": -2.712430059683426,
-                    "velocityY": -0.49551639642315154,
-                    "timestamp": 0.3486708267479094
-                },
-                {
-                    "x": 3.3968844052666176,
-                    "y": 5.398848722834353,
-                    "heading": -0.8527881698077676,
-                    "angularVelocity": -1.1256799666670962,
-                    "velocityX": -3.2337015756857643,
-                    "velocityY": -0.8527133062853276,
-                    "timestamp": 0.43583853343488677
-                },
-                {
-                    "x": 3.0986154730622166,
-                    "y": 5.268356785097819,
-                    "heading": -0.8643849747424638,
-                    "angularVelocity": -0.1330401518573905,
-                    "velocityX": -3.4217824873550513,
-                    "velocityY": -1.4970215770977777,
-                    "timestamp": 0.5230062401218641
-                },
-                {
-                    "x": 2.8320964566491864,
-                    "y": 5.083619646782129,
-                    "heading": -0.847429861393553,
-                    "angularVelocity": 0.19451140787489107,
-                    "velocityX": -3.0575430574319302,
-                    "velocityY": -2.11933002871221,
-                    "timestamp": 0.6101739468088415
-                },
-                {
-                    "x": 2.6201643415258493,
-                    "y": 4.879059787480057,
-                    "heading": -0.7372423358568471,
-                    "angularVelocity": 1.2640865490747921,
-                    "velocityX": -2.4313145679557007,
-                    "velocityY": -2.3467390284415206,
-                    "timestamp": 0.6973416534958189
-                },
-                {
-                    "x": 2.4692268443526784,
-                    "y": 4.691160871442081,
-                    "heading": -0.6128307115212047,
-                    "angularVelocity": 1.4272673799072706,
-                    "velocityX": -1.7315758657640712,
-                    "velocityY": -2.1556023805091904,
-                    "timestamp": 0.7845093601827963
-                },
-                {
-                    "x": 2.3748885078351467,
-                    "y": 4.53130895090854,
-                    "heading": -0.5057978375616147,
-                    "angularVelocity": 1.227895949401916,
-                    "velocityX": -1.0822624582324516,
-                    "velocityY": -1.8338433648091068,
-                    "timestamp": 0.8716770668697736
-                },
-                {
-                    "x": 2.322390449292023,
-                    "y": 4.41825305597818,
-                    "heading": -0.4294901557678491,
-                    "angularVelocity": 0.8754122907900764,
-                    "velocityX": -0.6022649962748851,
-                    "velocityY": -1.2969928799015857,
-                    "timestamp": 0.958844773556751
+                    "x": 4.206523478699805,
+                    "y": 5.55440930401173,
+                    "heading": -0.10760119525493232,
+                    "angularVelocity": -1.2344158099898592,
+                    "velocityX": -0.6479064718660169,
+                    "velocityY": -0.06413723844253004,
+                    "timestamp": 0.08716770668695249
+                },
+                {
+                    "x": 4.091196989898444,
+                    "y": 5.541197271810027,
+                    "heading": -0.295913717265277,
+                    "angularVelocity": -2.1603473254910437,
+                    "velocityX": -1.3230414471673093,
+                    "velocityY": -0.15157026270235532,
+                    "timestamp": 0.17433541337390498
+                },
+                {
+                    "x": 3.9151950960136452,
+                    "y": 5.516370757100726,
+                    "heading": -0.5340653762567812,
+                    "angularVelocity": -2.7321088054637612,
+                    "velocityX": -2.019118095155108,
+                    "velocityY": -0.2848132141236808,
+                    "timestamp": 0.26150312006085746
+                },
+                {
+                    "x": 3.678758800597895,
+                    "y": 5.473177706772957,
+                    "heading": -0.7546654655667131,
+                    "angularVelocity": -2.530754767957573,
+                    "velocityX": -2.712429917020411,
+                    "velocityY": -0.4955166536948123,
+                    "timestamp": 0.34867082674780997
+                },
+                {
+                    "x": 3.3968844521889907,
+                    "y": 5.398848640381046,
+                    "heading": -0.8527884173152881,
+                    "angularVelocity": -1.1256800881657552,
+                    "velocityX": -3.233701552126478,
+                    "velocityY": -0.8527133409492049,
+                    "timestamp": 0.4358385334347625
+                },
+                {
+                    "x": 3.098615494079836,
+                    "y": 5.268356714792353,
+                    "heading": -0.864385170870599,
+                    "angularVelocity": -0.1330395624260059,
+                    "velocityX": -3.421782784538955,
+                    "velocityY": -1.4970214377364781,
+                    "timestamp": 0.5230062401217149
+                },
+                {
+                    "x": 2.832096469446971,
+                    "y": 5.0836195976119996,
+                    "heading": -0.8474300397750907,
+                    "angularVelocity": 0.19451161146636953,
+                    "velocityX": -3.0575431517318843,
+                    "velocityY": -2.1193297862453186,
+                    "timestamp": 0.6101739468086674
+                },
+                {
+                    "x": 2.620164345186853,
+                    "y": 4.879059766080367,
+                    "heading": -0.7372424721392209,
+                    "angularVelocity": 1.2640870320425897,
+                    "velocityX": -2.431314672774813,
+                    "velocityY": -2.346738709855847,
+                    "timestamp": 0.6973416534956199
+                },
+                {
+                    "x": 2.469226841034766,
+                    "y": 4.6911608685478585,
+                    "heading": -0.6128307853924054,
+                    "angularVelocity": 1.427268095897297,
+                    "velocityX": -1.7315759458276552,
+                    "velocityY": -2.155602168212534,
+                    "timestamp": 0.7845093601825724
+                },
+                {
+                    "x": 2.3748885049182054,
+                    "y": 4.5313089505917965,
+                    "heading": -0.5057978702393489,
+                    "angularVelocity": 1.2278964219794164,
+                    "velocityX": -1.0822624536327627,
+                    "velocityY": -1.8338433352404204,
+                    "timestamp": 0.871677066869525
+                },
+                {
+                    "x": 2.3223904484665,
+                    "y": 4.418253055823363,
+                    "heading": -0.42949016513376354,
+                    "angularVelocity": 0.87541255822675,
+                    "velocityX": -0.6022649722820163,
+                    "velocityY": -1.2969928780443263,
+                    "timestamp": 0.9588447735564775
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
                     "heading": -0.39,
-                    "angularVelocity": 0.45303653461548066,
-                    "velocityX": -0.2568663343688406,
-                    "velocityY": -0.6682871236634659,
-                    "timestamp": 1.0460124802437283
+                    "angularVelocity": 0.4530366420626772,
+                    "velocityX": -0.256866324898396,
+                    "velocityY": -0.6682871218875669,
+                    "timestamp": 1.0460124802434299
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
                     "heading": -0.39,
-                    "angularVelocity": 0,
-                    "velocityX": 3.506966732338881e-31,
-                    "velocityY": 5.081769191491498e-30,
-                    "timestamp": 1.1331801869307057
+                    "angularVelocity": -3.4100744325418914e-31,
+                    "velocityX": -9.400788860397695e-33,
+                    "velocityY": 0,
+                    "timestamp": 1.1331801869303824
                 }
             ],
             "constraints": [
@@ -21846,16 +24210,6 @@
                     "x": 3.42,
                     "y": 4.04,
                     "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
                 }
             ]
         },
@@ -21884,146 +24238,146 @@
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": -5.481945073939837e-30,
-                    "angularVelocity": 8.103891066426341e-30,
-                    "velocityX": -1.0115776003849141e-29,
-                    "velocityY": 1.8597851705411875e-30,
+                    "heading": 0,
+                    "angularVelocity": 5.380258045140596e-28,
+                    "velocityX": 0,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 4.304265157789358,
-                    "y": 5.638698398929109,
-                    "heading": -0.04261689056565391,
-                    "angularVelocity": -0.41098438981919094,
-                    "velocityX": 0.39794868817859114,
-                    "velocityY": 0.7589435323492035,
-                    "timestamp": 0.10369466972795005
-                },
-                {
-                    "x": 4.388659791773407,
-                    "y": 5.795293123128996,
-                    "heading": -0.12280542510902584,
-                    "angularVelocity": -0.7733139490558338,
-                    "velocityX": 0.8138762986126182,
-                    "velocityY": 1.5101521091752137,
-                    "timestamp": 0.2073893394559001
-                },
-                {
-                    "x": 4.519821289569113,
-                    "y": 6.028001369445331,
-                    "heading": -0.2301897087601478,
-                    "angularVelocity": -1.035581519598341,
-                    "velocityX": 1.2648817739601788,
-                    "velocityY": 2.244167872146774,
-                    "timestamp": 0.31108400918385015
-                },
-                {
-                    "x": 4.707676223581395,
-                    "y": 6.330157885665521,
-                    "heading": -0.33387515355278086,
-                    "angularVelocity": -0.9999110375168158,
-                    "velocityX": 1.811616108187066,
-                    "velocityY": 2.9139059607683615,
-                    "timestamp": 0.4147786789118002
-                },
-                {
-                    "x": 4.978690084088412,
-                    "y": 6.613755913169546,
-                    "heading": -0.33387515567200693,
-                    "angularVelocity": -2.0437175255333274e-8,
-                    "velocityX": 2.613575617898612,
-                    "velocityY": 2.734933514403967,
-                    "timestamp": 0.5184733486397503
-                },
-                {
-                    "x": 5.307019069498364,
-                    "y": 6.828410126616754,
-                    "heading": -0.33387512130937075,
-                    "angularVelocity": 3.313828600417541e-7,
-                    "velocityX": 3.1663053295925763,
-                    "velocityY": 2.0700602452408625,
-                    "timestamp": 0.6221680183677003
-                },
-                {
-                    "x": 5.675511663962328,
-                    "y": 6.962907583051813,
-                    "heading": -0.33387509354517536,
-                    "angularVelocity": 2.6774949424459703e-7,
-                    "velocityX": 3.5536310152752564,
-                    "velocityY": 1.2970527490748707,
-                    "timestamp": 0.7258626880956504
-                },
-                {
-                    "x": 6.0558437987294225,
-                    "y": 7.05894833386441,
-                    "heading": -0.33387506661120775,
-                    "angularVelocity": 2.597430292697957e-7,
-                    "velocityX": 3.6678079573899045,
-                    "velocityY": 0.9261879232999765,
-                    "timestamp": 0.8295573578236004
-                },
-                {
-                    "x": 6.43617610221841,
-                    "y": 7.15498841651687,
-                    "heading": -0.333875039677234,
-                    "angularVelocity": 2.5974308807396863e-7,
-                    "velocityX": 3.667809584492749,
-                    "velocityY": 0.9261814797658935,
-                    "timestamp": 0.9332520275515505
-                },
-                {
-                    "x": 6.8165084241422855,
-                    "y": 7.251028426164244,
-                    "heading": -0.33387501274320475,
-                    "angularVelocity": 2.5974362337794873e-7,
-                    "velocityX": 3.667809762273227,
-                    "velocityY": 0.9261807757269309,
-                    "timestamp": 1.0369466972795005
-                },
-                {
-                    "x": 7.162466794959021,
-                    "y": 7.337325744518129,
-                    "heading": -0.24283225626293373,
-                    "angularVelocity": 0.8779887791641197,
-                    "velocityX": 3.3363177849390095,
-                    "velocityY": 0.8322252106139182,
-                    "timestamp": 1.1406413670074507
-                },
-                {
-                    "x": 7.421550082994963,
-                    "y": 7.402199028754473,
-                    "heading": -0.13158720138626476,
-                    "angularVelocity": 1.0728136284004517,
-                    "velocityX": 2.498520789117373,
-                    "velocityY": 0.6256183119782293,
-                    "timestamp": 1.2443360367354006
-                },
-                {
-                    "x": 7.5939307756893495,
-                    "y": 7.445412774465034,
-                    "heading": -0.046073125428300596,
-                    "angularVelocity": 0.8246718580841027,
-                    "velocityX": 1.6623872099370305,
-                    "velocityY": 0.41674028013136566,
-                    "timestamp": 1.3480307064633505
+                    "x": 4.304265282414897,
+                    "y": 5.638698322322049,
+                    "heading": -0.04261698283734242,
+                    "angularVelocity": -0.41098530784979936,
+                    "velocityX": 0.3979499173257931,
+                    "velocityY": 0.7589428456312429,
+                    "timestamp": 0.10369466261532896
+                },
+                {
+                    "x": 4.38866009880019,
+                    "y": 5.795292929312062,
+                    "heading": -0.1228057028496872,
+                    "angularVelocity": -0.7733157907057984,
+                    "velocityX": 0.8138781134605525,
+                    "velocityY": 1.510151082422871,
+                    "timestamp": 0.20738932523065792
+                },
+                {
+                    "x": 4.519821720835651,
+                    "y": 6.028001091828693,
+                    "heading": -0.23019023538729733,
+                    "angularVelocity": -1.0355839908170448,
+                    "velocityX": 1.264883058851596,
+                    "velocityY": 2.2441672179396264,
+                    "timestamp": 0.3110839878459869
+                },
+                {
+                    "x": 4.707676475524912,
+                    "y": 6.330157785456564,
+                    "heading": -0.33387571773962255,
+                    "angularVelocity": -0.9999114683169592,
+                    "velocityX": 1.8116145031122473,
+                    "velocityY": 2.9139078715050806,
+                    "timestamp": 0.41477865046131585
+                },
+                {
+                    "x": 4.978690211282069,
+                    "y": 6.613755936139097,
+                    "heading": -0.3338757185080882,
+                    "angularVelocity": -7.410850442524386e-9,
+                    "velocityX": 2.6135745941189206,
+                    "velocityY": 2.7349348898948014,
+                    "timestamp": 0.5184733130766448
+                },
+                {
+                    "x": 5.307019127730767,
+                    "y": 6.828410259978494,
+                    "heading": -0.3338757060420003,
+                    "angularVelocity": 1.2021918642580205e-7,
+                    "velocityX": 3.166304881734213,
+                    "velocityY": 2.070061451819255,
+                    "timestamp": 0.6221679756919738
+                },
+                {
+                    "x": 5.675511606586513,
+                    "y": 6.962908031631864,
+                    "heading": -0.3338756959696621,
+                    "angularVelocity": 9.713458668099172e-8,
+                    "velocityX": 3.5536301441350284,
+                    "velocityY": 1.2970558779125847,
+                    "timestamp": 0.7258626383073028
+                },
+                {
+                    "x": 6.055844888125957,
+                    "y": 7.058944236891981,
+                    "heading": -0.3338756861984713,
+                    "angularVelocity": 9.423041207795053e-8,
+                    "velocityX": 3.6678192680981745,
+                    "velocityY": 0.9261441508940896,
+                    "timestamp": 0.8295573009226318
+                },
+                {
+                    "x": 6.436176952400414,
+                    "y": 7.154985262770466,
+                    "heading": -0.3338756764272739,
+                    "angularVelocity": 9.423047569354886e-8,
+                    "velocityX": 3.667807529162388,
+                    "velocityY": 0.9261906394812687,
+                    "timestamp": 0.9332519635379608
+                },
+                {
+                    "x": 6.8165090646020285,
+                    "y": 7.2510260988513195,
+                    "heading": -0.33387566665551793,
+                    "angularVelocity": 9.423586238606553e-8,
+                    "velocityX": 3.6678079913574164,
+                    "velocityY": 0.9261888091302356,
+                    "timestamp": 1.0369466261532898
+                },
+                {
+                    "x": 7.162467192455402,
+                    "y": 7.33732437680055,
+                    "heading": -0.24283297058924916,
+                    "angularVelocity": 0.8779882567727307,
+                    "velocityX": 3.336315670718354,
+                    "velocityY": 0.8322345217454595,
+                    "timestamp": 1.1406412887686188
+                },
+                {
+                    "x": 7.421550289934561,
+                    "y": 7.4021983509049765,
+                    "heading": -0.1315877257451376,
+                    "angularVelocity": 1.0728155339758783,
+                    "velocityX": 2.4985191228237835,
+                    "velocityY": 0.6256250077700213,
+                    "timestamp": 1.2443359513839478
+                },
+                {
+                    "x": 7.593930847137365,
+                    "y": 7.445412549605325,
+                    "heading": -0.046073342510862675,
+                    "angularVelocity": 0.8246748779298502,
+                    "velocityX": 1.662386017323544,
+                    "velocityY": 0.41674467721309977,
+                    "timestamp": 1.3480306139992768
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 6.33230436519987e-31,
-                    "angularVelocity": 0.444315272416587,
-                    "velocityX": 0.8300255407192906,
-                    "velocityY": 0.2081806672570287,
-                    "timestamp": 1.4517253761913005
+                    "heading": 0,
+                    "angularVelocity": 0.44431739637147044,
+                    "velocityX": 0.8300249086292989,
+                    "velocityY": 0.20818285001567757,
+                    "timestamp": 1.4517252766146058
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
                     "heading": 0,
                     "angularVelocity": 0,
-                    "velocityX": 2.6078100454164866e-31,
-                    "velocityY": 7.556791844119902e-29,
-                    "timestamp": 1.5554200459192504
+                    "velocityX": 6.677691966309128e-30,
+                    "velocityY": 0,
+                    "timestamp": 1.5554199392299348
                 }
             ],
             "constraints": [
@@ -22044,16 +24398,6 @@
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
             "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
                 {
                     "x": 5.61,
                     "y": 5.44,
@@ -22070,7 +24414,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 14
+                    "controlIntervalCount": 9
+                },
+                {
+                    "x": 5.261847496032715,
+                    "y": 6.900974273681641,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 12
                 },
                 {
                     "x": 7.68,
@@ -22082,7 +24435,206 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.272659932573731,
+                    "y": 5.605321030989923,
+                    "heading": -5.269773313916661e-8,
+                    "angularVelocity": -7.069117507058631e-7,
+                    "velocityX": 0.1295790656325301,
+                    "velocityY": 0.6079397351586845,
+                    "timestamp": 0.07454855863477708
+                },
+                {
+                    "x": 4.294668340488188,
+                    "y": 5.695305527660525,
+                    "heading": -1.390931476532318e-7,
+                    "angularVelocity": -0.0000011589489187991593,
+                    "velocityX": 0.2952224471421504,
+                    "velocityY": 1.2070588395619357,
+                    "timestamp": 0.14909711726955416
+                },
+                {
+                    "x": 4.333068049168991,
+                    "y": 5.8286332482919585,
+                    "heading": -2.3393980900082764e-7,
+                    "angularVelocity": -0.0000012723197345504802,
+                    "velocityX": 0.5150965944736963,
+                    "velocityY": 1.7884681217524945,
+                    "timestamp": 0.22364567590433124
+                },
+                {
+                    "x": 4.394361213548298,
+                    "y": 6.0022499148295365,
+                    "heading": -3.0377830712505715e-7,
+                    "angularVelocity": -9.368513338342214e-7,
+                    "velocityX": 0.8221911395208825,
+                    "velocityY": 2.3289070872699873,
+                    "timestamp": 0.2981942345391083
+                },
+                {
+                    "x": 4.489214393007107,
+                    "y": 6.207820209268236,
+                    "heading": -3.104589480689307e-7,
+                    "angularVelocity": -8.962358439949817e-8,
+                    "velocityX": 1.2723677183517785,
+                    "velocityY": 2.75753546703125,
+                    "timestamp": 0.37274279317388537
+                },
+                {
+                    "x": 4.629403012529928,
+                    "y": 6.42298249437822,
+                    "heading": -2.4199647648725644e-7,
+                    "angularVelocity": 9.18393943226279e-7,
+                    "velocityX": 1.880500737963715,
+                    "velocityY": 2.8862031546566858,
+                    "timestamp": 0.4472913518086624
+                },
+                {
+                    "x": 4.811531801184116,
+                    "y": 6.618438666310206,
+                    "heading": -1.472447710538824e-7,
+                    "angularVelocity": 0.0000012710752897138218,
+                    "velocityX": 2.443089336474201,
+                    "velocityY": 2.6218638627509074,
+                    "timestamp": 0.5218399104434395
+                },
+                {
+                    "x": 5.024847227590037,
+                    "y": 6.779620899070013,
+                    "heading": -6.254572131926062e-8,
+                    "angularVelocity": 0.000001136235548213432,
+                    "velocityX": 2.861429252247402,
+                    "velocityY": 2.1621106520395927,
+                    "timestamp": 0.5963884690782165
+                },
+                {
+                    "x": 5.261847496032715,
+                    "y": 6.900974273681641,
+                    "heading": 1.6289151195668303e-9,
+                    "angularVelocity": 8.609140934997053e-7,
+                    "velocityX": 3.1791395133448552,
+                    "velocityY": 1.6278433384167708,
+                    "timestamp": 0.6709370277129936
+                },
+                {
+                    "x": 5.5631639462020495,
+                    "y": 6.984947323321856,
+                    "heading": 4.965855378058324e-8,
+                    "angularVelocity": 5.539886921154719e-7,
+                    "velocityX": 3.4757171947403775,
+                    "velocityY": 0.9686380297960585,
+                    "timestamp": 0.7576289054784
+                },
+                {
+                    "x": 5.883696640724643,
+                    "y": 7.009274337947639,
+                    "heading": 7.188189606013307e-8,
+                    "angularVelocity": 2.563216327096045e-7,
+                    "velocityX": 3.697378610459799,
+                    "velocityY": 0.2806146929817695,
+                    "timestamp": 0.8443207832438064
+                },
+                {
+                    "x": 6.209423466668327,
+                    "y": 6.971151903870608,
+                    "heading": 7.193737396357795e-8,
+                    "angularVelocity": 6.438452889791163e-10,
+                    "velocityX": 3.7572934667332714,
+                    "velocityY": -0.4397463179458525,
+                    "timestamp": 0.9310126610092129
+                },
+                {
+                    "x": 6.521952745942683,
+                    "y": 6.871769894938554,
+                    "heading": 7.193784305655878e-8,
+                    "angularVelocity": 5.630908705768223e-12,
+                    "velocityX": 3.6050583669434406,
+                    "velocityY": -1.1463820079014435,
+                    "timestamp": 1.0177045387746193
+                },
+                {
+                    "x": 6.809873374118548,
+                    "y": 6.714756988695251,
+                    "heading": 7.193831945924844e-8,
+                    "angularVelocity": 5.91084758847972e-12,
+                    "velocityX": 3.321194967875227,
+                    "velocityY": -1.8111605177424126,
+                    "timestamp": 1.1043964165400257
+                },
+                {
+                    "x": 7.062672913559571,
+                    "y": 6.505846154651799,
+                    "heading": 7.193893641048392e-8,
+                    "angularVelocity": 7.48396872358976e-12,
+                    "velocityX": 2.9160694861627596,
+                    "velocityY": -2.4098086166543093,
+                    "timestamp": 1.1910882943054322
+                },
+                {
+                    "x": 7.268448732261103,
+                    "y": 6.269564240975314,
+                    "heading": 6.958123303548702e-8,
+                    "angularVelocity": -2.7156604096657526e-8,
+                    "velocityX": 2.3736458816624144,
+                    "velocityY": -2.7255369226670942,
+                    "timestamp": 1.2777801720708386
+                },
+                {
+                    "x": 7.433069281193563,
+                    "y": 6.080538590855167,
+                    "heading": 5.4913746267249155e-8,
+                    "angularVelocity": -1.6915879682512025e-7,
+                    "velocityX": 1.8989154825033356,
+                    "velocityY": -2.180430912690726,
+                    "timestamp": 1.364472049836245
+                },
+                {
+                    "x": 7.556534655605292,
+                    "y": 5.9387693118426395,
+                    "heading": 3.4027865091828515e-8,
+                    "angularVelocity": -2.408962130800618e-7,
+                    "velocityX": 1.424186181835385,
+                    "velocityY": -1.635323662267605,
+                    "timestamp": 1.4511639276016515
+                },
+                {
+                    "x": 7.638844889158882,
+                    "y": 5.8442564415645775,
+                    "heading": 1.3493455787871544e-8,
+                    "angularVelocity": -2.368497714278514e-7,
+                    "velocityX": 0.9494572693998051,
+                    "velocityY": -1.0902159778725726,
+                    "timestamp": 1.537855805367058
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": -2.563219583366834e-29,
+                    "angularVelocity": -1.5563981959470815e-7,
+                    "velocityX": 0.4747285662416774,
+                    "velocityY": -0.5451080630467725,
+                    "timestamp": 1.6245476831324643
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 0,
+                    "angularVelocity": 4.328374921762355e-28,
+                    "velocityX": 0,
+                    "velocityY": 6.636862452110489e-29,
+                    "timestamp": 1.7112395608978708
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -22101,16 +24653,6 @@
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
             "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
                 {
                     "x": 5.61,
                     "y": 5.44,
@@ -24188,7 +26730,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 17
+                    "controlIntervalCount": 13
+                },
+                {
+                    "x": 5.136752128601074,
+                    "y": 1.409298062324524,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 12
                 },
                 {
                     "x": 7.68,
@@ -24200,7 +26751,242 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": -0.566,
+                    "angularVelocity": 0,
+                    "velocityX": 5.832812358477626e-33,
+                    "velocityY": -2.114400767819492e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 4.2637473414422145,
+                    "y": 2.974020167404826,
+                    "heading": -0.5570803398431683,
+                    "angularVelocity": 0.15939480257853877,
+                    "velocityX": 0.013355031418934325,
+                    "velocityY": -0.46426099366130524,
+                    "timestamp": 0.05595954204614445
+                },
+                {
+                    "x": 4.266239342094047,
+                    "y": 2.922097369670203,
+                    "heading": -0.5396590874905302,
+                    "angularVelocity": 0.31131870840312753,
+                    "velocityX": 0.04453218451605929,
+                    "velocityY": -0.9278631639230981,
+                    "timestamp": 0.1119190840922889
+                },
+                {
+                    "x": 4.271816064940073,
+                    "y": 2.844342168020101,
+                    "heading": -0.5143157133438189,
+                    "angularVelocity": 0.4528874472527088,
+                    "velocityX": 0.0996563345966598,
+                    "velocityY": -1.3894895992176792,
+                    "timestamp": 0.16787862613843335
+                },
+                {
+                    "x": 4.282354343554946,
+                    "y": 2.7410289237168692,
+                    "heading": -0.48190029722305777,
+                    "angularVelocity": 0.5792652143941603,
+                    "velocityX": 0.18831960072480589,
+                    "velocityY": -1.8462131841257783,
+                    "timestamp": 0.2238381681845778
+                },
+                {
+                    "x": 4.3006106845058465,
+                    "y": 2.6128237251760056,
+                    "heading": -0.4437500478548854,
+                    "angularVelocity": 0.6817469902936669,
+                    "velocityX": 0.32624178617911886,
+                    "velocityY": -2.2910337335345985,
+                    "timestamp": 0.27979771023072225
+                },
+                {
+                    "x": 4.330801306241698,
+                    "y": 2.4614150778969766,
+                    "heading": -0.40214700400499553,
+                    "angularVelocity": 0.7434486117771366,
+                    "velocityX": 0.5395080201148661,
+                    "velocityY": -2.705680599640681,
+                    "timestamp": 0.3357572522768667
+                },
+                {
+                    "x": 4.379172513113433,
+                    "y": 2.2912661424924936,
+                    "heading": -0.3611794534128463,
+                    "angularVelocity": 0.7320923133783194,
+                    "velocityX": 0.864396045840541,
+                    "velocityY": -3.0405705476320635,
+                    "timestamp": 0.39171679432301115
+                },
+                {
+                    "x": 4.451949891834961,
+                    "y": 2.111933259909663,
+                    "heading": -0.32494756197390784,
+                    "angularVelocity": 0.6474658318159084,
+                    "velocityX": 1.3005356380778725,
+                    "velocityY": -3.2046881733762564,
+                    "timestamp": 0.4476763363691556
+                },
+                {
+                    "x": 4.550285732793113,
+                    "y": 1.9373903674143325,
+                    "heading": -0.2971520965733793,
+                    "angularVelocity": 0.49670644869849423,
+                    "velocityX": 1.7572667209653445,
+                    "velocityY": -3.1190907951212283,
+                    "timestamp": 0.5036358784153001
+                },
+                {
+                    "x": 4.671000177640236,
+                    "y": 1.7762856565305696,
+                    "heading": -0.2715476133805437,
+                    "angularVelocity": 0.457553479828869,
+                    "velocityX": 2.157173565637473,
+                    "velocityY": -2.878949773229245,
+                    "timestamp": 0.5595954204614446
+                },
+                {
+                    "x": 4.810884164899462,
+                    "y": 1.6328902215117949,
+                    "heading": -0.24301377044973924,
+                    "angularVelocity": 0.5099012945331528,
+                    "velocityX": 2.4997343106181424,
+                    "velocityY": -2.5624840693036233,
+                    "timestamp": 0.6155549625075891
+                },
+                {
+                    "x": 4.966899091534391,
+                    "y": 1.5100232904396305,
+                    "heading": -0.21315700918191657,
+                    "angularVelocity": 0.5335419157506132,
+                    "velocityX": 2.7879950573269263,
+                    "velocityY": -2.1956386092446096,
+                    "timestamp": 0.6715145045537336
+                },
+                {
+                    "x": 5.136752128601074,
+                    "y": 1.409298062324524,
+                    "heading": -0.1837509654270181,
+                    "angularVelocity": 0.5254875697633724,
+                    "velocityX": 3.035282828558936,
+                    "velocityY": -1.7999651968567865,
+                    "timestamp": 0.7274740465998781
+                },
+                {
+                    "x": 5.4382274814533025,
+                    "y": 1.3074419969375322,
+                    "heading": -0.14111678525881038,
+                    "angularVelocity": 0.4782734646509064,
+                    "velocityX": 3.3819733590888914,
+                    "velocityY": -1.142629061849971,
+                    "timestamp": 0.8166158939305274
+                },
+                {
+                    "x": 5.764139683697637,
+                    "y": 1.267066681764717,
+                    "heading": -0.11133632809533998,
+                    "angularVelocity": 0.3340794257159682,
+                    "velocityX": 3.6561077877984602,
+                    "velocityY": -0.4529333459183563,
+                    "timestamp": 0.9057577412611766
+                },
+                {
+                    "x": 6.100462721846291,
+                    "y": 1.2915989985388854,
+                    "heading": -0.11133120767369757,
+                    "angularVelocity": 0.000057441278584387276,
+                    "velocityX": 3.7728973340784897,
+                    "velocityY": 0.2752053890377148,
+                    "timestamp": 0.9948995885918258
+                },
+                {
+                    "x": 6.42549869748969,
+                    "y": 1.3814198753662668,
+                    "heading": -0.1113311879575033,
+                    "angularVelocity": 2.2117776172639892e-7,
+                    "velocityX": 3.6462782113742715,
+                    "velocityY": 1.0076174043625032,
+                    "timestamp": 1.084041435922475
+                },
+                {
+                    "x": 6.7266979846048365,
+                    "y": 1.533061465523067,
+                    "heading": -0.11133117504978503,
+                    "angularVelocity": 1.4479976182990285e-7,
+                    "velocityX": 3.3788764327254777,
+                    "velocityY": 1.7011268522889103,
+                    "timestamp": 1.1731832832531242
+                },
+                {
+                    "x": 6.992432961420095,
+                    "y": 1.740669527522903,
+                    "heading": -0.11133116451645807,
+                    "angularVelocity": 1.1816366034373416e-7,
+                    "velocityX": 2.981035111708839,
+                    "velocityY": 2.3289629754897407,
+                    "timestamp": 1.2623251305837733
+                },
+                {
+                    "x": 7.221595592407175,
+                    "y": 1.979432543612985,
+                    "heading": -0.09587073631794915,
+                    "angularVelocity": 0.17343625537800947,
+                    "velocityX": 2.5707637641507293,
+                    "velocityY": 2.678461611912118,
+                    "timestamp": 1.3514669779144224
+                },
+                {
+                    "x": 7.404993837760709,
+                    "y": 2.170501480308595,
+                    "heading": -0.06411284637687509,
+                    "angularVelocity": 0.3562624164974272,
+                    "velocityX": 2.0573754173309005,
+                    "velocityY": 2.1434258142181197,
+                    "timestamp": 1.4406088252450715
+                },
+                {
+                    "x": 7.54251389523076,
+                    "y": 2.3137697288578556,
+                    "heading": -0.0342508837604198,
+                    "angularVelocity": 0.33499376006509896,
+                    "velocityX": 1.5427104282454527,
+                    "velocityY": 1.6071940714649837,
+                    "timestamp": 1.5297506725757206
+                },
+                {
+                    "x": 7.634176122098128,
+                    "y": 2.4092619199619465,
+                    "heading": -0.011960250388889534,
+                    "angularVelocity": 0.25005801471490263,
+                    "velocityX": 1.028273808679041,
+                    "velocityY": 1.0712386377846093,
+                    "timestamp": 1.6188925199063697
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 9.313254165846089e-33,
+                    "angularVelocity": 0.1341709954082624,
+                    "velocityX": 0.5140557355952123,
+                    "velocityY": 0.5355294002487934,
+                    "timestamp": 1.7080343672370188
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 0,
+                    "angularVelocity": -1.1876173054192403e-31,
+                    "velocityX": 4.5955148793706025e-32,
+                    "velocityY": 0,
+                    "timestamp": 1.797176214567668
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -25042,6 +27828,15 @@
                     "headingConstrained": true,
                     "controlIntervalCount": 17
                 },
+                {
+                    "x": 2.1829307079315186,
+                    "y": 3.053825616836548,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 10
+                },
                 {
                     "x": 2.3,
                     "y": 4.36,
@@ -25052,7 +27847,260 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": -0.64,
+                    "angularVelocity": -2.137980318394988e-34,
+                    "velocityX": 1.3258659962604387e-36,
+                    "velocityY": 7.61509960712835e-36,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.139985955005962,
+                    "y": 1.6266018860262446,
+                    "heading": -0.6327443563347177,
+                    "angularVelocity": 0.10940864810395588,
+                    "velocityX": -0.5430597418126536,
+                    "velocityY": 0.09955056482775779,
+                    "timestamp": 0.06631691178916982
+                },
+                {
+                    "x": 5.067957879081044,
+                    "y": 1.6398056681484225,
+                    "heading": -0.6182319813670383,
+                    "angularVelocity": 0.2188336968074761,
+                    "velocityX": -1.0861192715653594,
+                    "velocityY": 0.19910128149746392,
+                    "timestamp": 0.13263382357833964
+                },
+                {
+                    "x": 4.959915797050431,
+                    "y": 1.6596113536414707,
+                    "heading": -0.5964609094580124,
+                    "angularVelocity": 0.328288385596709,
+                    "velocityX": -1.6291784269764191,
+                    "velocityY": 0.2986521078667379,
+                    "timestamp": 0.19895073536750946
+                },
+                {
+                    "x": 4.815859744494909,
+                    "y": 1.686018943789264,
+                    "heading": -0.5674282989321845,
+                    "angularVelocity": 0.43778592432238833,
+                    "velocityX": -2.172237045860861,
+                    "velocityY": 0.39820295359570684,
+                    "timestamp": 0.2652676471566793
+                },
+                {
+                    "x": 4.635789767966758,
+                    "y": 1.719028429378711,
+                    "heading": -0.5311304969514992,
+                    "angularVelocity": 0.5473385445944894,
+                    "velocityX": -2.7152949627783816,
+                    "velocityY": 0.49775366039945246,
+                    "timestamp": 0.3315845589458491
+                },
+                {
+                    "x": 4.41970592970577,
+                    "y": 1.758639783689655,
+                    "heading": -0.48756329690665473,
+                    "angularVelocity": 0.6569545967904882,
+                    "velocityX": -3.258351941175237,
+                    "velocityY": 0.5973039642870227,
+                    "timestamp": 0.3979014707350189
+                },
+                {
+                    "x": 4.172944378919847,
+                    "y": 1.8038720825822125,
+                    "heading": -0.4875632898742738,
+                    "angularVelocity": 1.0604204507812919e-7,
+                    "velocityX": -3.7209445393116334,
+                    "velocityY": 0.6820628052819626,
+                    "timestamp": 0.46421838252418873
+                },
+                {
+                    "x": 3.9261828364502485,
+                    "y": 1.849104426845956,
+                    "heading": -0.4875632828424199,
+                    "angularVelocity": 1.0603409772331237e-7,
+                    "velocityX": -3.7209444139088643,
+                    "velocityY": 0.6820634894390606,
+                    "timestamp": 0.5305352943133586
+                },
+                {
+                    "x": 3.6794213056437455,
+                    "y": 1.8943368347367484,
+                    "heading": -0.487563275810563,
+                    "angularVelocity": 1.0603414195839125e-7,
+                    "velocityX": -3.720944238039768,
+                    "velocityY": 0.6820644488783205,
+                    "timestamp": 0.5968522061025283
+                },
+                {
+                    "x": 3.4326599282156307,
+                    "y": 1.9395700793621615,
+                    "heading": -0.4875632687787025,
+                    "angularVelocity": 1.0603419804191013e-7,
+                    "velocityX": -3.720941925230195,
+                    "velocityY": 0.6820770660916139,
+                    "timestamp": 0.6631691178916981
+                },
+                {
+                    "x": 3.192328571865682,
+                    "y": 2.011528836252569,
+                    "heading": -0.4875632615410612,
+                    "angularVelocity": 1.0913718718412795e-7,
+                    "velocityX": -3.6239829308396314,
+                    "velocityY": 1.0850740022269671,
+                    "timestamp": 0.7294860296808678
+                },
+                {
+                    "x": 2.965054771843869,
+                    "y": 2.1177542675713954,
+                    "heading": -0.48756325344928275,
+                    "angularVelocity": 1.2201681646141545e-7,
+                    "velocityX": -3.4270866041583203,
+                    "velocityY": 1.6017849512735258,
+                    "timestamp": 0.7958029414700376
+                },
+                {
+                    "x": 2.7556945515371734,
+                    "y": 2.2559769801698577,
+                    "heading": -0.48756324342419455,
+                    "angularVelocity": 1.5116940667572282e-7,
+                    "velocityX": -3.156965767228115,
+                    "velocityY": 2.084275471660842,
+                    "timestamp": 0.8621198532592074
+                },
+                {
+                    "x": 2.5687211592217127,
+                    "y": 2.4232437322251608,
+                    "heading": -0.4875632292213124,
+                    "angularVelocity": 2.1416682100483036e-7,
+                    "velocityX": -2.8193923280063653,
+                    "velocityY": 2.5222337340898235,
+                    "timestamp": 0.9284367650483771
+                },
+                {
+                    "x": 2.4081295337898796,
+                    "y": 2.6159806661875145,
+                    "heading": -0.48756314249031313,
+                    "angularVelocity": 0.0000013078262679985986,
+                    "velocityX": -2.421578766248568,
+                    "velocityY": 2.906301405817713,
+                    "timestamp": 0.9947536768375469
+                },
+                {
+                    "x": 2.2783797783684188,
+                    "y": 2.8285106690600896,
+                    "heading": -0.4828540771785898,
+                    "angularVelocity": 0.07100851328382284,
+                    "velocityX": -1.9565108193510647,
+                    "velocityY": 3.2047632668456583,
+                    "timestamp": 1.0610705886267167
+                },
+                {
+                    "x": 2.1829307079315186,
+                    "y": 3.053825616836548,
+                    "heading": -0.47038335932986286,
+                    "angularVelocity": 0.18804732476646344,
+                    "velocityX": -1.4392870213912456,
+                    "velocityY": 3.3975488559051734,
+                    "timestamp": 1.1273875004158864
+                },
+                {
+                    "x": 2.124568521000596,
+                    "y": 3.2749812318761844,
+                    "heading": -0.4550563591461243,
+                    "angularVelocity": 0.24322153086635045,
+                    "velocityX": -0.9261395106595871,
+                    "velocityY": 3.5094804335357668,
+                    "timestamp": 1.1904041261609186
+                },
+                {
+                    "x": 2.099309698866093,
+                    "y": 3.4965263857952165,
+                    "heading": -0.4375068558264715,
+                    "angularVelocity": 0.2784900510328627,
+                    "velocityX": -0.40082790590377965,
+                    "velocityY": 3.5156619590425686,
+                    "timestamp": 1.2534207519059508
+                },
+                {
+                    "x": 2.105558929434475,
+                    "y": 3.707892405315209,
+                    "heading": -0.4199092504656253,
+                    "angularVelocity": 0.2792533740547583,
+                    "velocityX": 0.09916796550908585,
+                    "velocityY": 3.354131025281934,
+                    "timestamp": 1.316437377650983
+                },
+                {
+                    "x": 2.135680280502732,
+                    "y": 3.8963278332771982,
+                    "heading": -0.40525850037923283,
+                    "angularVelocity": 0.23249023433387317,
+                    "velocityX": 0.4779905415775436,
+                    "velocityY": 2.990249410122441,
+                    "timestamp": 1.3794540033960152
+                },
+                {
+                    "x": 2.177481885950485,
+                    "y": 4.0538159629335215,
+                    "heading": -0.39590861500997965,
+                    "angularVelocity": 0.14837172347315433,
+                    "velocityX": 0.6633424902324003,
+                    "velocityY": 2.4991520538330105,
+                    "timestamp": 1.4424706291410474
+                },
+                {
+                    "x": 2.2205433619064863,
+                    "y": 4.17824561445827,
+                    "heading": -0.39150641146352744,
+                    "angularVelocity": 0.06985781124276201,
+                    "velocityX": 0.6833351587282585,
+                    "velocityY": 1.9745527478446014,
+                    "timestamp": 1.5054872548860796
+                },
+                {
+                    "x": 2.2579292824582606,
+                    "y": 4.270063380418578,
+                    "heading": -0.38992694464863337,
+                    "angularVelocity": 0.0250642873403699,
+                    "velocityX": 0.5932707457717501,
+                    "velocityY": 1.4570403425249505,
+                    "timestamp": 1.5685038806311118
+                },
+                {
+                    "x": 2.285342089257776,
+                    "y": 4.3303097045050025,
+                    "heading": -0.38974725509942826,
+                    "angularVelocity": 0.002851462563739305,
+                    "velocityX": 0.43500911823537725,
+                    "velocityY": 0.9560385592555154,
+                    "timestamp": 1.631520506376144
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": -0.39,
+                    "angularVelocity": -0.004010765374750144,
+                    "velocityX": 0.23260386555018367,
+                    "velocityY": 0.47115019479344145,
+                    "timestamp": 1.6945371321211762
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": -0.39,
+                    "angularVelocity": -1.6105528525551922e-32,
+                    "velocityX": 0,
+                    "velocityY": -4.5276793712997926e-36,
+                    "timestamp": 1.7575537578662084
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -25097,7 +28145,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 25
+                    "controlIntervalCount": 14
+                },
+                {
+                    "x": 5.034329891204834,
+                    "y": 3.981222629547119,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 17
                 },
                 {
                     "x": 7.68,
@@ -25109,7 +28166,296 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": -0.64,
+                    "angularVelocity": 1.1411819845129358e-30,
+                    "velocityX": 2.0257584812603577e-32,
+                    "velocityY": 3.042151659469263e-32,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.128157808100627,
+                    "y": 1.6381505638691025,
+                    "heading": -0.6351424860741187,
+                    "angularVelocity": 0.06199634399134786,
+                    "velocityX": -0.6106088487961494,
+                    "velocityY": 0.23165524966799825,
+                    "timestamp": 0.07835161903352221
+                },
+                {
+                    "x": 5.034138141125347,
+                    "y": 1.6783506940439978,
+                    "heading": -0.625731099901857,
+                    "angularVelocity": 0.12011731586854772,
+                    "velocityX": -1.199970953185457,
+                    "velocityY": 0.5130733821555872,
+                    "timestamp": 0.15670323806704442
+                },
+                {
+                    "x": 4.897649658060979,
+                    "y": 1.7471051706265737,
+                    "heading": -0.6123788413002932,
+                    "angularVelocity": 0.1704145844880519,
+                    "velocityX": -1.74199952404266,
+                    "velocityY": 0.8775118808095047,
+                    "timestamp": 0.23505485710056662
+                },
+                {
+                    "x": 4.7291295198915435,
+                    "y": 1.855780594555545,
+                    "heading": -0.5966040827715752,
+                    "angularVelocity": 0.20133289807283902,
+                    "velocityX": -2.150818837544829,
+                    "velocityY": 1.3870220586313022,
+                    "timestamp": 0.31340647613408884
+                },
+                {
+                    "x": 4.559409610033689,
+                    "y": 2.0156280635815147,
+                    "heading": -0.5822183456142529,
+                    "angularVelocity": 0.18360484869071408,
+                    "velocityX": -2.1661314973624393,
+                    "velocityY": 2.0401297509574126,
+                    "timestamp": 0.39175809516761106
+                },
+                {
+                    "x": 4.423281260574217,
+                    "y": 2.2140742145732073,
+                    "heading": -0.5727758831618049,
+                    "angularVelocity": 0.1205139417528553,
+                    "velocityX": -1.7374031467203144,
+                    "velocityY": 2.532763884646578,
+                    "timestamp": 0.4701097142011333
+                },
+                {
+                    "x": 4.3324586171803325,
+                    "y": 2.4362881098121476,
+                    "heading": -0.5691499320560302,
+                    "angularVelocity": 0.04627793465535804,
+                    "velocityX": -1.1591674111421555,
+                    "velocityY": 2.8361110846205655,
+                    "timestamp": 0.5484613332346555
+                },
+                {
+                    "x": 4.290356765831772,
+                    "y": 2.674111890908862,
+                    "heading": -0.5714790836196856,
+                    "angularVelocity": -0.02972691046318988,
+                    "velocityX": -0.5373450079001806,
+                    "velocityY": 3.0353397164002804,
+                    "timestamp": 0.6268129522681777
+                },
+                {
+                    "x": 4.298268630722052,
+                    "y": 2.9211188280591673,
+                    "heading": -0.5947192771376703,
+                    "angularVelocity": -0.29661408155511804,
+                    "velocityX": 0.1009789585444941,
+                    "velocityY": 3.1525441362561364,
+                    "timestamp": 0.7051645713016998
+                },
+                {
+                    "x": 4.356924477851345,
+                    "y": 3.1722484967201265,
+                    "heading": -0.6368890523591307,
+                    "angularVelocity": -0.5382119188043637,
+                    "velocityX": 0.7486232939768359,
+                    "velocityY": 3.2051624683532776,
+                    "timestamp": 0.783516190335222
+                },
+                {
+                    "x": 4.466386404708156,
+                    "y": 3.418800428561669,
+                    "heading": -0.694172117002664,
+                    "angularVelocity": -0.7311025011368958,
+                    "velocityX": 1.3970601782967371,
+                    "velocityY": 3.1467369134523744,
+                    "timestamp": 0.8618678093687442
+                },
+                {
+                    "x": 4.622742851027307,
+                    "y": 3.644917946871822,
+                    "heading": -0.7581397869413499,
+                    "angularVelocity": -0.8164179723116849,
+                    "velocityX": 1.9955739045067467,
+                    "velocityY": 2.8859329404975034,
+                    "timestamp": 0.9402194284022664
+                },
+                {
+                    "x": 4.8130531116785615,
+                    "y": 3.833426042269654,
+                    "heading": -0.8482959054101304,
+                    "angularVelocity": -1.1506605681014435,
+                    "velocityX": 2.4289256941816704,
+                    "velocityY": 2.4059246984696085,
+                    "timestamp": 1.0185710474357885
+                },
+                {
+                    "x": 5.034329891204834,
+                    "y": 3.981222629547119,
+                    "heading": -0.9454274313666489,
+                    "angularVelocity": -1.2396875412997066,
+                    "velocityX": 2.824150697276595,
+                    "velocityY": 1.8863246107809373,
+                    "timestamp": 1.0969226664693108
+                },
+                {
+                    "x": 5.3307679727813255,
+                    "y": 4.095215339701575,
+                    "heading": -1.0325645415374454,
+                    "angularVelocity": -0.9491279076997933,
+                    "velocityX": 3.2289073573561264,
+                    "velocityY": 1.241648503948074,
+                    "timestamp": 1.1887302189223545
+                },
+                {
+                    "x": 5.6706736122505745,
+                    "y": 4.158722341660029,
+                    "heading": -1.0366531166779425,
+                    "angularVelocity": -0.04453419170049298,
+                    "velocityX": 3.7023712144281196,
+                    "velocityY": 0.691740496958957,
+                    "timestamp": 1.2805377713753983
+                },
+                {
+                    "x": 5.985432145120126,
+                    "y": 4.2874622186845945,
+                    "heading": -1.0150469515971576,
+                    "angularVelocity": 0.23534191363870294,
+                    "velocityX": 3.4284601262030105,
+                    "velocityY": 1.402279807975625,
+                    "timestamp": 1.372345323828442
+                },
+                {
+                    "x": 6.260473058973373,
+                    "y": 4.473603429076733,
+                    "heading": -0.9641067919300658,
+                    "angularVelocity": 0.5548580515001295,
+                    "velocityX": 2.9958419160985765,
+                    "velocityY": 2.027515225257119,
+                    "timestamp": 1.4641528762814857
+                },
+                {
+                    "x": 6.487005993334624,
+                    "y": 4.6803674717401,
+                    "heading": -0.8309910438969466,
+                    "angularVelocity": 1.4499433268434323,
+                    "velocityX": 2.4674760224886074,
+                    "velocityY": 2.2521463337029823,
+                    "timestamp": 1.5559604287345294
+                },
+                {
+                    "x": 6.71888754498383,
+                    "y": 4.9198952355114685,
+                    "heading": -0.7843851972685187,
+                    "angularVelocity": 0.507647196588375,
+                    "velocityX": 2.5257350343568326,
+                    "velocityY": 2.609020253468568,
+                    "timestamp": 1.6477679811875732
+                },
+                {
+                    "x": 6.906293835583718,
+                    "y": 5.2122954621107285,
+                    "heading": -0.7843850302000089,
+                    "angularVelocity": 0.0000018197686939661552,
+                    "velocityX": 2.0412949217411986,
+                    "velocityY": 3.1849256274293083,
+                    "timestamp": 1.739575533640617
+                },
+                {
+                    "x": 7.030998336898649,
+                    "y": 5.536437116039629,
+                    "heading": -0.7843849409482478,
+                    "angularVelocity": 9.721614257680115e-7,
+                    "velocityX": 1.3583250830993825,
+                    "velocityY": 3.530664365490929,
+                    "timestamp": 1.8313830860936606
+                },
+                {
+                    "x": 7.139916048661206,
+                    "y": 5.86621879705104,
+                    "heading": -0.7843848550588985,
+                    "angularVelocity": 9.355368597772783e-7,
+                    "velocityX": 1.1863698448802928,
+                    "velocityY": 3.592097514854049,
+                    "timestamp": 1.9231906385467044
+                },
+                {
+                    "x": 7.248833621341376,
+                    "y": 6.196000523997438,
+                    "heading": -0.7843847691695015,
+                    "angularVelocity": 9.355373786921671e-7,
+                    "velocityX": 1.1863683299462482,
+                    "velocityY": 3.592098015194004,
+                    "timestamp": 2.0149981909997483
+                },
+                {
+                    "x": 7.357751176995467,
+                    "y": 6.5257822565670125,
+                    "heading": -0.7843846832799689,
+                    "angularVelocity": 9.355388566444766e-7,
+                    "velocityX": 1.1863681444922285,
+                    "velocityY": 3.5920980764436137,
+                    "timestamp": 2.1068057434527923
+                },
+                {
+                    "x": 7.4669549324183055,
+                    "y": 6.834400664448086,
+                    "heading": -0.7318192896940902,
+                    "angularVelocity": 0.5725606682823186,
+                    "velocityX": 1.189485532562175,
+                    "velocityY": 3.361579735380924,
+                    "timestamp": 2.198613295905836
+                },
+                {
+                    "x": 7.555413306559794,
+                    "y": 7.089894078912264,
+                    "heading": -0.5139853289463429,
+                    "angularVelocity": 2.3727237566774426,
+                    "velocityX": 0.9635195773978621,
+                    "velocityY": 2.782923709842436,
+                    "timestamp": 2.29042084835888
+                },
+                {
+                    "x": 7.619158448834915,
+                    "y": 7.279787689667393,
+                    "heading": -0.2782484542424907,
+                    "angularVelocity": 2.5677285626846773,
+                    "velocityX": 0.6943344046528581,
+                    "velocityY": 2.0683876835977384,
+                    "timestamp": 2.382228400811924
+                },
+                {
+                    "x": 7.660128944224933,
+                    "y": 7.404974004983973,
+                    "heading": -0.09793823848426357,
+                    "angularVelocity": 1.964001990472943,
+                    "velocityX": 0.44626497815605787,
+                    "velocityY": 1.3635731698719276,
+                    "timestamp": 2.474035953264968
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 2.533782828853193e-32,
+                    "angularVelocity": 1.0667775783953661,
+                    "velocityX": 0.21644249567844834,
+                    "velocityY": 0.6756088508922024,
+                    "timestamp": 2.565843505718012
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 0,
+                    "angularVelocity": 2.4912680070874024e-31,
+                    "velocityX": 0,
+                    "velocityY": -3.645397861822831e-32,
+                    "timestamp": 2.657651058171056
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -25410,7 +28756,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 18
+                    "controlIntervalCount": 12
+                },
+                {
+                    "x": 6.529238224029541,
+                    "y": 1.531233787536621,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 13
                 },
                 {
                     "x": 7.68,
@@ -25422,7 +28777,242 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": -0.64,
+                    "angularVelocity": 0,
+                    "velocityX": 2.653642496841617e-31,
+                    "velocityY": -1.2761459725684476e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.194122324075795,
+                    "y": 1.6116215778070426,
+                    "heading": -0.6335243646936302,
+                    "angularVelocity": 0.13206571622689564,
+                    "velocityX": 0.3695911822570569,
+                    "velocityY": -0.1708716250073132,
+                    "timestamp": 0.049033431926404414
+                },
+                {
+                    "x": 5.2306889078335095,
+                    "y": 1.5955898422898989,
+                    "heading": -0.6207226842720189,
+                    "angularVelocity": 0.2610806529090014,
+                    "velocityX": 0.745747999295838,
+                    "velocityY": -0.3269551994890449,
+                    "timestamp": 0.09806686385280883
+                },
+                {
+                    "x": 5.286073830286703,
+                    "y": 1.5728623577205654,
+                    "heading": -0.6018022859553105,
+                    "angularVelocity": 0.38586730672362957,
+                    "velocityX": 1.1295338767299938,
+                    "velocityY": -0.4635099701654054,
+                    "timestamp": 0.14710029577921324
+                },
+                {
+                    "x": 5.360700050095615,
+                    "y": 1.5447484727758143,
+                    "heading": -0.577064216904969,
+                    "angularVelocity": 0.5045143298894789,
+                    "velocityX": 1.5219456782249914,
+                    "velocityY": -0.5733615584348081,
+                    "timestamp": 0.19613372770561766
+                },
+                {
+                    "x": 5.45500431506412,
+                    "y": 1.513111103841015,
+                    "heading": -0.5469692052395991,
+                    "angularVelocity": 0.6137651492712723,
+                    "velocityX": 1.9232646230039372,
+                    "velocityY": -0.6452203668359155,
+                    "timestamp": 0.24516715963202207
+                },
+                {
+                    "x": 5.5692993756365,
+                    "y": 1.480706991826158,
+                    "heading": -0.5122625268421303,
+                    "angularVelocity": 0.7078166270271339,
+                    "velocityX": 2.330961878090668,
+                    "velocityY": -0.6608575158164777,
+                    "timestamp": 0.2942005915584265
+                },
+                {
+                    "x": 5.703331364670941,
+                    "y": 1.4516882815399292,
+                    "heading": -0.47420565589215874,
+                    "angularVelocity": 0.7761412867722233,
+                    "velocityX": 2.733481703578104,
+                    "velocityY": -0.5918147913809916,
+                    "timestamp": 0.3432340234848309
+                },
+                {
+                    "x": 5.855159622936256,
+                    "y": 1.4318923943368613,
+                    "heading": -0.4349045247507558,
+                    "angularVelocity": 0.8015170384282412,
+                    "velocityX": 3.096423242273411,
+                    "velocityY": -0.40372224470801127,
+                    "timestamp": 0.3922674554112353
+                },
+                {
+                    "x": 6.019815732483991,
+                    "y": 1.4274954608427641,
+                    "heading": -0.3972296573283927,
+                    "angularVelocity": 0.768350611865634,
+                    "velocityX": 3.358037630221609,
+                    "velocityY": -0.08967215471871418,
+                    "timestamp": 0.4413008873376397
+                },
+                {
+                    "x": 6.190507691154086,
+                    "y": 1.4421792499476667,
+                    "heading": -0.36375877276285506,
+                    "angularVelocity": 0.6826135404074718,
+                    "velocityX": 3.4811342376834093,
+                    "velocityY": 0.2994648452702519,
+                    "timestamp": 0.49033431926404414
+                },
+                {
+                    "x": 6.361514712702491,
+                    "y": 1.4768435219754552,
+                    "heading": -0.33601787986089265,
+                    "angularVelocity": 0.5657546659914753,
+                    "velocityX": 3.487559708345244,
+                    "velocityY": 0.7069517809772359,
+                    "timestamp": 0.5393677511904486
+                },
+                {
+                    "x": 6.529238224029541,
+                    "y": 1.531233787536621,
+                    "heading": -0.3133159497925571,
+                    "angularVelocity": 0.4629888052692109,
+                    "velocityX": 3.4205949846286416,
+                    "velocityY": 1.1092485968081267,
+                    "timestamp": 0.588401183116853
+                },
+                {
+                    "x": 6.794330666624461,
+                    "y": 1.6764087741891647,
+                    "heading": -0.29089151826229404,
+                    "angularVelocity": 0.27241251742532613,
+                    "velocityX": 3.2203491776466797,
+                    "velocityY": 1.7635891250039288,
+                    "timestamp": 0.6707191020664587
+                },
+                {
+                    "x": 7.034738463760469,
+                    "y": 1.8721289187979775,
+                    "heading": -0.28740556521383775,
+                    "angularVelocity": 0.042347438965477144,
+                    "velocityX": 2.9204795286821774,
+                    "velocityY": 2.377612883150898,
+                    "timestamp": 0.7530370210160644
+                },
+                {
+                    "x": 7.236654527584481,
+                    "y": 2.109198968129657,
+                    "heading": -0.2874055261243408,
+                    "angularVelocity": 4.748601214403234e-7,
+                    "velocityX": 2.452881054338489,
+                    "velocityY": 2.879932490479633,
+                    "timestamp": 0.8353549399656701
+                },
+                {
+                    "x": 7.392410351066591,
+                    "y": 2.3788514084218195,
+                    "heading": -0.2874055078665042,
+                    "angularVelocity": 2.2179662469221753e-7,
+                    "velocityX": 1.8921253776776576,
+                    "velocityY": 3.2757441360636306,
+                    "timestamp": 0.9176728589152758
+                },
+                {
+                    "x": 7.49687844031107,
+                    "y": 2.6722091028580075,
+                    "heading": -0.28740549372573876,
+                    "angularVelocity": 1.7178234776742224e-7,
+                    "velocityX": 1.2690807855393922,
+                    "velocityY": 3.5637161164844247,
+                    "timestamp": 0.9999907778648816
+                },
+                {
+                    "x": 7.546620338444338,
+                    "y": 2.979614475892834,
+                    "heading": -0.28740548096651763,
+                    "angularVelocity": 1.5499931641741705e-7,
+                    "velocityX": 0.6042657390764874,
+                    "velocityY": 3.7343676438541644,
+                    "timestamp": 1.0823086968144873
+                },
+                {
+                    "x": 7.582493232504898,
+                    "y": 3.288945132948743,
+                    "heading": -0.2874054683444637,
+                    "angularVelocity": 1.5333300623146833e-7,
+                    "velocityX": 0.43578475401492967,
+                    "velocityY": 3.7577560390622633,
+                    "timestamp": 1.164626615764093
+                },
+                {
+                    "x": 7.6150468277369106,
+                    "y": 3.5683420563080905,
+                    "heading": -0.1936159509616472,
+                    "angularVelocity": 1.1393572454157397,
+                    "velocityX": 0.39546183440153926,
+                    "velocityY": 3.3941203437178378,
+                    "timestamp": 1.2469445347136987
+                },
+                {
+                    "x": 7.641044736890772,
+                    "y": 3.791820131624945,
+                    "heading": -0.11680359430817472,
+                    "angularVelocity": 0.9331183007718732,
+                    "velocityX": 0.31582320697043825,
+                    "velocityY": 2.714816873027222,
+                    "timestamp": 1.3292624536633044
+                },
+                {
+                    "x": 7.660527962829294,
+                    "y": 3.959415202649684,
+                    "heading": -0.05861701943494889,
+                    "angularVelocity": 0.7068518691380767,
+                    "velocityX": 0.23668268327338735,
+                    "velocityY": 2.035948833052459,
+                    "timestamp": 1.4115803726129101
+                },
+                {
+                    "x": 7.673510674167818,
+                    "y": 4.0711397209924485,
+                    "heading": -0.019593791766857216,
+                    "angularVelocity": 0.4740550801809447,
+                    "velocityX": 0.1577142802460941,
+                    "velocityY": 1.3572320555281903,
+                    "timestamp": 1.4938982915625159
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": -2.071552209824535e-33,
+                    "angularVelocity": 0.23802583953553896,
+                    "velocityX": 0.07883248161483059,
+                    "velocityY": 0.6785919726876813,
+                    "timestamp": 1.5762162105121216
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 0,
+                    "angularVelocity": -4.785738309812717e-31,
+                    "velocityX": -1.2484093100405624e-30,
+                    "velocityY": 4.284452606087597e-31,
+                    "timestamp": 1.6585341294617273
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -25479,7 +29069,152 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": -0.64,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 5.222401190948388,
+                    "y": 1.635609178560311,
+                    "heading": -0.6120137040899031,
+                    "angularVelocity": 0.36292240482976423,
+                    "velocityX": 0.6017242103081577,
+                    "velocityY": 0.20241766322785323,
+                    "timestamp": 0.07711371780208913
+                },
+                {
+                    "x": 5.315327100770923,
+                    "y": 1.6668589219810437,
+                    "heading": -0.5590312951883875,
+                    "angularVelocity": 0.6870685321838068,
+                    "velocityX": 1.205050313629373,
+                    "velocityY": 0.4052423396435706,
+                    "timestamp": 0.15422743560417826
+                },
+                {
+                    "x": 5.454938302762681,
+                    "y": 1.713800132384211,
+                    "heading": -0.4856728473163301,
+                    "angularVelocity": 0.9513021802467544,
+                    "velocityX": 1.8104587091763231,
+                    "velocityY": 0.6087271077195515,
+                    "timestamp": 0.23134115340626737
+                },
+                {
+                    "x": 5.641435450684747,
+                    "y": 1.7765099385015581,
+                    "heading": -0.4002091399727602,
+                    "angularVelocity": 1.1082815065785667,
+                    "velocityX": 2.418469154874715,
+                    "velocityY": 0.8132120704942623,
+                    "timestamp": 0.3084548712083565
+                },
+                {
+                    "x": 5.8748863814192855,
+                    "y": 1.8550365409977465,
+                    "heading": -0.32154781798640775,
+                    "angularVelocity": 1.0200691164734093,
+                    "velocityX": 3.0273592998548686,
+                    "velocityY": 1.0183220928048784,
+                    "timestamp": 0.38556858901044566
+                },
+                {
+                    "x": 6.151206632813102,
+                    "y": 1.945953456728306,
+                    "heading": -0.3195100296755088,
+                    "angularVelocity": 0.026425756259478697,
+                    "velocityX": 3.583282705976986,
+                    "velocityY": 1.178997956808355,
+                    "timestamp": 0.4626823068125348
+                },
+                {
+                    "x": 6.4277763897122595,
+                    "y": 2.0387313847088295,
+                    "heading": -0.31950999788997536,
+                    "angularVelocity": 4.121903903341971e-7,
+                    "velocityX": 3.5865182587742526,
+                    "velocityY": 1.2031313056210848,
+                    "timestamp": 0.539796024614624
+                },
+                {
+                    "x": 6.7043461469693755,
+                    "y": 2.1315093116222874,
+                    "heading": -0.31950996610444216,
+                    "angularVelocity": 4.121903870025697e-7,
+                    "velocityX": 3.5865182634161936,
+                    "velocityY": 1.2031312917835268,
+                    "timestamp": 0.616909742416713
+                },
+                {
+                    "x": 6.980831721332989,
+                    "y": 2.2223162260189957,
+                    "heading": -0.31777519026878787,
+                    "angularVelocity": 0.022496332495664188,
+                    "velocityX": 3.5854265913259273,
+                    "velocityY": 1.1775714747635835,
+                    "timestamp": 0.6940234602188021
+                },
+                {
+                    "x": 7.214355031558839,
+                    "y": 2.3007720927806434,
+                    "heading": -0.23870969056825803,
+                    "angularVelocity": 1.0253104370281303,
+                    "velocityX": 3.028297907062184,
+                    "velocityY": 1.0174048016074528,
+                    "timestamp": 0.7711371780208912
+                },
+                {
+                    "x": 7.400917361819277,
+                    "y": 2.3634042679352882,
+                    "heading": -0.15373574527352316,
+                    "angularVelocity": 1.1019303402387481,
+                    "velocityX": 2.419314430400663,
+                    "velocityY": 0.8122053629341165,
+                    "timestamp": 0.8482508958229803
+                },
+                {
+                    "x": 7.540592119323217,
+                    "y": 2.4102654434053137,
+                    "heading": -0.08076255152981597,
+                    "angularVelocity": 0.9463062581289249,
+                    "velocityX": 1.8112828882458099,
+                    "velocityY": 0.6076892258040765,
+                    "timestamp": 0.9253646136250694
+                },
+                {
+                    "x": 7.633568850123268,
+                    "y": 2.4414404537843537,
+                    "heading": -0.02793953661151031,
+                    "angularVelocity": 0.6850015331107969,
+                    "velocityX": 1.2057093530190521,
+                    "velocityY": 0.4042732119212608,
+                    "timestamp": 1.0024783314271586
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": -3.3818227179407965e-32,
+                    "angularVelocity": 0.3623160367292381,
+                    "velocityX": 0.6021127135368662,
+                    "velocityY": 0.20177403786417583,
+                    "timestamp": 1.0795920492292477
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": -1.7311819232040874e-32,
+                    "angularVelocity": 2.140534191184349e-31,
+                    "velocityX": 0,
+                    "velocityY": -2.0522160776390996e-31,
+                    "timestamp": 1.1567057670313368
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -25497,23 +29232,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "S4 To C5": {
             "waypoints": [
@@ -25542,144 +29261,144 @@
                     "y": 1.62,
                     "heading": -0.64,
                     "angularVelocity": 0,
-                    "velocityX": -3.378617665126574e-35,
-                    "velocityY": 1.1970750708579272e-32,
+                    "velocityX": 0,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 5.2224556017723405,
-                    "y": 1.6045081405552968,
-                    "heading": -0.6133597294207822,
-                    "angularVelocity": 0.3456555725750699,
-                    "velocityX": 0.6027580531582283,
-                    "velocityY": -0.20100574919793807,
-                    "timestamp": 0.07707172310503405
-                },
-                {
-                    "x": 5.3154629271275695,
-                    "y": 1.5735046785143454,
-                    "heading": -0.5625081883046615,
-                    "angularVelocity": 0.6597950463209403,
-                    "velocityX": 1.2067632798150456,
-                    "velocityY": -0.4022676643507709,
-                    "timestamp": 0.1541434462100681
-                },
-                {
-                    "x": 5.455153059548346,
-                    "y": 1.5269582787185583,
-                    "heading": -0.4912835813545399,
-                    "angularVelocity": 0.9241340932919854,
-                    "velocityX": 1.812469305122525,
-                    "velocityY": -0.6039361509065161,
-                    "timestamp": 0.23121516931510214
-                },
-                {
-                    "x": 5.641700996428857,
-                    "y": 1.4648196222358638,
-                    "heading": -0.40650985692768693,
-                    "angularVelocity": 1.0999329067980297,
-                    "velocityX": 2.4204459088877828,
-                    "velocityY": -0.8062445470177368,
-                    "timestamp": 0.3082868924201362
-                },
-                {
-                    "x": 5.875244676488674,
-                    "y": 1.3870501062573488,
-                    "heading": -0.3232929818469195,
-                    "angularVelocity": 1.0797328997998226,
-                    "velocityX": 3.0302122575038553,
-                    "velocityY": -1.0090538117660863,
-                    "timestamp": 0.38535861552517026
-                },
-                {
-                    "x": 6.151314773692177,
-                    "y": 1.2951594028189055,
-                    "heading": -0.3212316148216143,
-                    "angularVelocity": 0.026746087180327376,
-                    "velocityX": 3.581989425969788,
-                    "velocityY": -1.1922751916836483,
-                    "timestamp": 0.46243033863020433
-                },
-                {
-                    "x": 6.427941619479976,
-                    "y": 1.2030534747397732,
-                    "heading": -0.3212315858589365,
-                    "angularVelocity": 3.7578863628089786e-7,
-                    "velocityX": 3.5892131983452513,
-                    "velocityY": -1.1950677157381002,
-                    "timestamp": 0.5395020617352384
-                },
-                {
-                    "x": 6.704568483880494,
-                    "y": 1.1109476025613012,
-                    "heading": -0.3212315568962456,
-                    "angularVelocity": 3.75788807477423e-7,
-                    "velocityX": 3.5892134398439306,
-                    "velocityY": -1.195066990431097,
-                    "timestamp": 0.6165737848402725
-                },
-                {
-                    "x": 6.980707555601848,
-                    "y": 1.0195121848796085,
-                    "heading": -0.31890263703438493,
-                    "angularVelocity": 0.03021756576905414,
-                    "velocityX": 3.582884365320709,
-                    "velocityY": -1.186367892113736,
-                    "timestamp": 0.6936455079453065
-                },
-                {
-                    "x": 7.214256878757648,
-                    "y": 0.9418349206236948,
-                    "heading": -0.2350677354758115,
-                    "angularVelocity": 1.0877517483853638,
-                    "velocityX": 3.030285476263674,
-                    "velocityY": -1.0078568523770295,
-                    "timestamp": 0.7707172310503406
-                },
-                {
-                    "x": 7.4008131329346885,
-                    "y": 0.8797967164596704,
-                    "heading": -0.14969694055158067,
-                    "angularVelocity": 1.1076798530621503,
-                    "velocityX": 2.420553825205123,
-                    "velocityY": -0.8049411854913137,
-                    "timestamp": 0.8477889541553747
-                },
-                {
-                    "x": 7.540515620024687,
-                    "y": 0.8333523210527743,
-                    "heading": -0.07802156861665294,
-                    "angularVelocity": 0.9299827361748209,
-                    "velocityX": 1.8126296060568263,
-                    "velocityY": -0.6026126513818976,
-                    "timestamp": 0.9248606772604088
-                },
-                {
-                    "x": 7.633536076900545,
-                    "y": 0.8024373784171116,
-                    "heading": -0.026843164489729465,
-                    "angularVelocity": 0.6640360701054664,
-                    "velocityX": 1.2069336603398442,
-                    "velocityY": -0.40111913150730505,
-                    "timestamp": 1.0019324003654428
+                    "x": 5.22245562506892,
+                    "y": 1.6045082039129626,
+                    "heading": -0.6133597798196372,
+                    "angularVelocity": 0.3456549186542876,
+                    "velocityX": 0.6027583554308628,
+                    "velocityY": -0.20100492713723658,
+                    "timestamp": 0.07707172310487943
+                },
+                {
+                    "x": 5.31546298986369,
+                    "y": 1.573504853521539,
+                    "heading": -0.5625082909022232,
+                    "angularVelocity": 0.6597943690478437,
+                    "velocityX": 1.2067637915426601,
+                    "velocityY": -0.4022662157070755,
+                    "timestamp": 0.15414344620975887
+                },
+                {
+                    "x": 5.455153171011431,
+                    "y": 1.526958597742617,
+                    "heading": -0.4912836919139272,
+                    "angularVelocity": 0.924133989989731,
+                    "velocityX": 1.8124699373549893,
+                    "velocityY": -0.6039342822993798,
+                    "timestamp": 0.2312151693146383
+                },
+                {
+                    "x": 5.641701159953251,
+                    "y": 1.4648201004985182,
+                    "heading": -0.406509898431932,
+                    "angularVelocity": 1.0999338027856826,
+                    "velocityX": 2.420446584384324,
+                    "velocityY": -0.8062424809101578,
+                    "timestamp": 0.30828689241951773
+                },
+                {
+                    "x": 5.875244890448534,
+                    "y": 1.3870507382512116,
+                    "heading": -0.323292875073049,
+                    "angularVelocity": 1.0797348236997513,
+                    "velocityX": 3.03021291190643,
+                    "velocityY": -1.0090518171168696,
+                    "timestamp": 0.38535861552439715
+                },
+                {
+                    "x": 6.151315012657669,
+                    "y": 1.295160148354896,
+                    "heading": -0.32123146568205607,
+                    "angularVelocity": 0.026746636872095383,
+                    "velocityX": 3.5819897504232276,
+                    "velocityY": -1.192273718485189,
+                    "timestamp": 0.46243033862927657
+                },
+                {
+                    "x": 6.427941847872596,
+                    "y": 1.2030541885235448,
+                    "heading": -0.3212314367193528,
+                    "angularVelocity": 3.757889680371681e-7,
+                    "velocityX": 3.5892130611702004,
+                    "velocityY": -1.1950681277232245,
+                    "timestamp": 0.539502061734156
+                },
+                {
+                    "x": 6.704568684105482,
+                    "y": 1.1109482317494896,
+                    "heading": -0.3212314077566493,
+                    "angularVelocity": 3.7578897090221527e-7,
+                    "velocityX": 3.589213074378161,
+                    "velocityY": -1.1950680880550335,
+                    "timestamp": 0.6165737848390355
+                },
+                {
+                    "x": 6.9807077062893965,
+                    "y": 1.0195126541212949,
+                    "heading": -0.3189025007582472,
+                    "angularVelocity": 0.030217398866675224,
+                    "velocityX": 3.582883722583232,
+                    "velocityY": -1.1863699674103443,
+                    "timestamp": 0.6936455079439149
+                },
+                {
+                    "x": 7.214256981896637,
+                    "y": 0.9418352430707998,
+                    "heading": -0.2350676331360113,
+                    "angularVelocity": 1.0877513080660361,
+                    "velocityX": 3.0302848593306506,
+                    "velocityY": -1.0078587570280646,
+                    "timestamp": 0.7707172310487943
+                },
+                {
+                    "x": 7.4008131962231865,
+                    "y": 0.8797969152412516,
+                    "heading": -0.14969687190170503,
+                    "angularVelocity": 1.1076794159400525,
+                    "velocityX": 2.4205533081527695,
+                    "velocityY": -0.8049427900441033,
+                    "timestamp": 0.8477889541536737
+                },
+                {
+                    "x": 7.54051565231881,
+                    "y": 0.8333524229780926,
+                    "heading": -0.0780215306756512,
+                    "angularVelocity": 0.9299823377312819,
+                    "velocityX": 1.812629203910703,
+                    "velocityY": -0.6026139080860716,
+                    "timestamp": 0.9248606772585531
+                },
+                {
+                    "x": 7.633536087871107,
+                    "y": 0.802437413208921,
+                    "heading": -0.026843150662094223,
+                    "angularVelocity": 0.6640357572376224,
+                    "velocityX": 1.2069333836706095,
+                    "velocityY": -0.401120002560503,
+                    "timestamp": 1.0019324003634327
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 1.4987800218197315e-33,
-                    "angularVelocity": 0.3482881063025846,
-                    "velocityX": 0.6028660217721296,
-                    "velocityY": -0.20029886182865572,
-                    "timestamp": 1.0790041234704768
+                    "heading": 3.8658238709310755e-33,
+                    "angularVelocity": 0.3482879268907221,
+                    "velocityX": 0.6028658794310917,
+                    "velocityY": -0.20029931325025216,
+                    "timestamp": 1.079004123468312
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 0,
-                    "angularVelocity": -8.136191152722898e-32,
-                    "velocityX": 4.4028618628466005e-33,
-                    "velocityY": 0,
-                    "timestamp": 1.1560758465755108
+                    "heading": 4.381697144346615e-34,
+                    "angularVelocity": -4.4473487408728983e-32,
+                    "velocityX": 0,
+                    "velocityY": -1.5918634852021244e-32,
+                    "timestamp": 1.1560758465731915
                 }
             ],
             "constraints": [
@@ -25699,23 +29418,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "C1 To W1": {
             "waypoints": [
@@ -25743,199 +29446,199 @@
                     "x": 7.68,
                     "y": 7.467,
                     "heading": 0,
-                    "angularVelocity": -1.8065144428954335e-34,
-                    "velocityX": -4.551747158955398e-35,
-                    "velocityY": 1.585845739465702e-34,
+                    "angularVelocity": 0,
+                    "velocityX": 3.136513796099116e-33,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.613270090763082,
-                    "y": 7.458186379338955,
-                    "heading": 0.02268667453207945,
-                    "angularVelocity": 0.251947675366546,
-                    "velocityX": -0.7410713935129049,
-                    "velocityY": -0.09787997945546167,
-                    "timestamp": 0.09004518298918118
-                },
-                {
-                    "x": 7.479732324735008,
-                    "y": 7.440537819966174,
-                    "heading": 0.06475236187583014,
-                    "angularVelocity": 0.4671619952041724,
-                    "velocityX": -1.4830084363770935,
-                    "velocityY": -0.19599670728529434,
-                    "timestamp": 0.18009036597836237
-                },
-                {
-                    "x": 7.279281674321286,
-                    "y": 7.414023565204846,
-                    "heading": 0.12053400112707001,
-                    "angularVelocity": 0.6194849896406114,
-                    "velocityX": -2.226111866948016,
-                    "velocityY": -0.2944550044894082,
-                    "timestamp": 0.27013554896754355
-                },
-                {
-                    "x": 7.011816314313711,
-                    "y": 7.378595756973036,
-                    "heading": 0.17807802747436277,
-                    "angularVelocity": 0.63905724256462,
-                    "velocityX": -2.970346121010285,
-                    "velocityY": -0.39344479133399113,
-                    "timestamp": 0.36018073195672473
-                },
-                {
-                    "x": 6.678420949429204,
-                    "y": 7.334257918470772,
-                    "heading": 0.1920910018934364,
-                    "angularVelocity": 0.15562158856134758,
-                    "velocityX": -3.702534148046146,
-                    "velocityY": -0.4923954511546947,
-                    "timestamp": 0.4502259149459059
-                },
-                {
-                    "x": 6.340697263329751,
-                    "y": 7.289813527368774,
-                    "heading": 0.19209102131154623,
-                    "angularVelocity": 2.156485134469417e-7,
-                    "velocityX": -3.7506024740938,
-                    "velocityY": -0.49357877486170487,
-                    "timestamp": 0.5402710979350871
-                },
-                {
-                    "x": 6.002973586353306,
-                    "y": 7.245369066942952,
-                    "heading": 0.1920910407296517,
-                    "angularVelocity": 2.1564846490105217e-7,
-                    "velocityX": -3.7506023727779296,
-                    "velocityY": -0.49357954473991933,
-                    "timestamp": 0.6303162809242683
-                },
-                {
-                    "x": 5.665249920039304,
-                    "y": 7.200924525495619,
-                    "heading": 0.1920910601477644,
-                    "angularVelocity": 2.156485448418072e-7,
-                    "velocityX": -3.750602254365785,
-                    "velocityY": -0.4935804445271851,
-                    "timestamp": 0.7203614639134495
-                },
-                {
-                    "x": 5.327526265186639,
-                    "y": 7.156479896956337,
-                    "heading": 0.1920910795658876,
-                    "angularVelocity": 2.1564866168111011e-7,
-                    "velocityX": -3.7506021270814847,
-                    "velocityY": -0.4935814117299572,
-                    "timestamp": 0.8104066469026306
-                },
-                {
-                    "x": 4.989802622149171,
-                    "y": 7.1120351786364,
-                    "heading": 0.19209109898401447,
-                    "angularVelocity": 2.1564870237166035e-7,
-                    "velocityX": -3.75060199586739,
-                    "velocityY": -0.4935824087922295,
-                    "timestamp": 0.9004518298918118
-                },
-                {
-                    "x": 4.652078990957916,
-                    "y": 7.067590370300322,
-                    "heading": 0.19209111840213633,
-                    "angularVelocity": 2.156486466006611e-7,
-                    "velocityX": -3.75060186430886,
-                    "velocityY": -0.49358340846970244,
-                    "timestamp": 0.990497012880993
-                },
-                {
-                    "x": 4.314355371293917,
-                    "y": 7.023145474371938,
-                    "heading": 0.19209113782025056,
-                    "angularVelocity": 2.156485622267904e-7,
-                    "velocityX": -3.7506017362924924,
-                    "velocityY": -0.4935843812291933,
-                    "timestamp": 1.0805421958701742
-                },
-                {
-                    "x": 3.97663176247589,
-                    "y": 6.97870049602832,
-                    "heading": 0.19209115723836015,
-                    "angularVelocity": 2.1564851021035735e-7,
-                    "velocityX": -3.7506016158421813,
-                    "velocityY": -0.4935852964945219,
-                    "timestamp": 1.1705873788593553
-                },
-                {
-                    "x": 3.638908163731045,
-                    "y": 6.934255441141791,
-                    "heading": 0.1920911766564701,
-                    "angularVelocity": 2.1564851460850062e-7,
-                    "velocityX": -3.7506015039740865,
-                    "velocityY": -0.49358614654454386,
-                    "timestamp": 1.2606325618485363
-                },
-                {
-                    "x": 3.301184574635452,
-                    "y": 6.88981031293383,
-                    "heading": 0.1920911960745901,
-                    "angularVelocity": 2.156486262427343e-7,
-                    "velocityX": -3.750601396813981,
-                    "velocityY": -0.49358696081834263,
-                    "timestamp": 1.3506777448377174
-                },
-                {
-                    "x": 2.967975728993463,
-                    "y": 6.845441681932465,
-                    "heading": 0.20651305517000687,
-                    "angularVelocity": 0.16016247195754518,
-                    "velocityX": -3.70046275192781,
-                    "velocityY": -0.49273741835468604,
-                    "timestamp": 1.4407229278268985
-                },
-                {
-                    "x": 2.7005739783902083,
-                    "y": 6.810001707269663,
-                    "heading": 0.26681564809465086,
-                    "angularVelocity": 0.6696926023448618,
-                    "velocityX": -2.969639704495717,
-                    "velocityY": -0.3935799060684999,
-                    "timestamp": 1.5307681108160796
-                },
-                {
-                    "x": 2.5001863972917366,
-                    "y": 6.7834775390625595,
-                    "heading": 0.32524314681975514,
-                    "angularVelocity": 0.6488686766523009,
-                    "velocityX": -2.2254114484119376,
-                    "velocityY": -0.2945650986159953,
-                    "timestamp": 1.6208132938052606
-                },
-                {
-                    "x": 2.3667002159439683,
-                    "y": 6.765819938844314,
-                    "heading": 0.3692681668658828,
-                    "angularVelocity": 0.4889214346026397,
-                    "velocityX": -1.482435560865111,
-                    "velocityY": -0.1960971107179338,
-                    "timestamp": 1.7108584767944417
+                    "x": 7.613270105003341,
+                    "y": 7.458186274184093,
+                    "heading": 0.022686687743237347,
+                    "angularVelocity": 0.25194782208363986,
+                    "velocityX": -0.7410712353675238,
+                    "velocityY": -0.0978811472565877,
+                    "timestamp": 0.09004518298914281
+                },
+                {
+                    "x": 7.479732366880497,
+                    "y": 7.440537508755927,
+                    "heading": 0.06475240251532689,
+                    "angularVelocity": 0.4671622998107697,
+                    "velocityX": -1.483008126475186,
+                    "velocityY": -0.19599899564082535,
+                    "timestamp": 0.18009036597828562
+                },
+                {
+                    "x": 7.2792817573683966,
+                    "y": 7.414022951557291,
+                    "heading": 0.12053408331695856,
+                    "angularVelocity": 0.6194854510802376,
+                    "velocityX": -2.226111412714547,
+                    "velocityY": -0.2944583632178676,
+                    "timestamp": 0.2701355489674284
+                },
+                {
+                    "x": 7.01181645042657,
+                    "y": 7.378594749595234,
+                    "heading": 0.17807816149187164,
+                    "angularVelocity": 0.6390578181383835,
+                    "velocityX": -2.9703455316879728,
+                    "velocityY": -0.39344916391950197,
+                    "timestamp": 0.36018073195657124
+                },
+                {
+                    "x": 6.678421151155383,
+                    "y": 7.334256434894407,
+                    "heading": 0.1920911587056688,
+                    "angularVelocity": 0.15562184170903165,
+                    "velocityX": -3.7025334193766524,
+                    "velocityY": -0.49240073959507324,
+                    "timestamp": 0.45022591494571407
+                },
+                {
+                    "x": 6.340697514229505,
+                    "y": 7.289811670135368,
+                    "heading": 0.1920911781237931,
+                    "angularVelocity": 2.1564867368321945e-7,
+                    "velocityX": -3.7506019279965175,
+                    "velocityY": -0.49358292452353475,
+                    "timestamp": 0.5402710979348568
+                },
+                {
+                    "x": 6.002973877037569,
+                    "y": 7.245366907398008,
+                    "heading": 0.19209119754191162,
+                    "angularVelocity": 2.1564861007875057e-7,
+                    "velocityX": -3.750601930951228,
+                    "velocityY": -0.49358290207171196,
+                    "timestamp": 0.6303162809239996
+                },
+                {
+                    "x": 5.665250239845632,
+                    "y": 7.200922144660658,
+                    "heading": 0.19209121696003015,
+                    "angularVelocity": 2.1564860966767442e-7,
+                    "velocityX": -3.7506019309512406,
+                    "velocityY": -0.493582902071613,
+                    "timestamp": 0.7203614639131424
+                },
+                {
+                    "x": 5.327526602653695,
+                    "y": 7.156477381923307,
+                    "heading": 0.19209123637814873,
+                    "angularVelocity": 2.1564861026621434e-7,
+                    "velocityX": -3.7506019309512415,
+                    "velocityY": -0.493582902071613,
+                    "timestamp": 0.8104066469022851
+                },
+                {
+                    "x": 4.989802965461758,
+                    "y": 7.112032619185957,
+                    "heading": 0.19209125579626718,
+                    "angularVelocity": 2.156486089426075e-7,
+                    "velocityX": -3.7506019309512415,
+                    "velocityY": -0.49358290207161315,
+                    "timestamp": 0.9004518298914279
+                },
+                {
+                    "x": 4.652079328269822,
+                    "y": 7.067587856448606,
+                    "heading": 0.19209127521438563,
+                    "angularVelocity": 2.156486089612396e-7,
+                    "velocityX": -3.750601930951241,
+                    "velocityY": -0.4935829020716131,
+                    "timestamp": 0.9904970128805707
+                },
+                {
+                    "x": 4.314355691077885,
+                    "y": 7.023143093711255,
+                    "heading": 0.19209129463250418,
+                    "angularVelocity": 2.1564860999442945e-7,
+                    "velocityX": -3.7506019309512415,
+                    "velocityY": -0.4935829020716131,
+                    "timestamp": 1.0805421958697135
+                },
+                {
+                    "x": 3.976632053885947,
+                    "y": 6.978698330973905,
+                    "heading": 0.1920913140506227,
+                    "angularVelocity": 2.1564860984546372e-7,
+                    "velocityX": -3.750601930951241,
+                    "velocityY": -0.49358290207161315,
+                    "timestamp": 1.1705873788588563
+                },
+                {
+                    "x": 3.638908416694012,
+                    "y": 6.934253568236545,
+                    "heading": 0.19209133346874124,
+                    "angularVelocity": 2.1564860987115903e-7,
+                    "velocityX": -3.750601930951227,
+                    "velocityY": -0.4935829020717237,
+                    "timestamp": 1.2606325618479992
+                },
+                {
+                    "x": 3.3011847797995815,
+                    "y": 6.8898088032385445,
+                    "heading": 0.19209135288686577,
+                    "angularVelocity": 2.1564867640744468e-7,
+                    "velocityX": -3.7506019276472724,
+                    "velocityY": -0.4935829271773418,
+                    "timestamp": 1.350677744837142
+                },
+                {
+                    "x": 2.967975868183031,
+                    "y": 6.845440652800829,
+                    "heading": 0.2065131865620181,
+                    "angularVelocity": 0.16016218965195775,
+                    "velocityX": -3.7004634846122446,
+                    "velocityY": -0.492732081438091,
+                    "timestamp": 1.440722927826285
+                },
+                {
+                    "x": 2.7005740636525677,
+                    "y": 6.810001078107782,
+                    "heading": 0.266815731322117,
+                    "angularVelocity": 0.6696920674520688,
+                    "velocityX": -2.9696403033875236,
+                    "velocityY": -0.39357546419024264,
+                    "timestamp": 1.5307681108154279
+                },
+                {
+                    "x": 2.5001864407535797,
+                    "y": 6.78347721885534,
+                    "heading": 0.32524318999567287,
+                    "angularVelocity": 0.6488682318586739,
+                    "velocityX": -2.2254119126300136,
+                    "velocityY": -0.29456166750908425,
+                    "timestamp": 1.6208132938045707
+                },
+                {
+                    "x": 2.366700230698382,
+                    "y": 6.765819830273621,
+                    "heading": 0.3692681816828941,
+                    "angularVelocity": 0.48892111966199864,
+                    "velocityX": -1.4824358796771253,
+                    "velocityY": -0.19609476038099796,
+                    "timestamp": 1.7108584767937136
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.393,
-                    "angularVelocity": 0.2635547215998056,
-                    "velocityX": -0.7407416335861364,
-                    "velocityY": -0.09795014626572365,
-                    "timestamp": 1.8009036597836228
+                    "angularVelocity": 0.26355455704906927,
+                    "velocityX": -0.740741797442122,
+                    "velocityY": -0.09794894053004785,
+                    "timestamp": 1.8009036597828565
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.393,
-                    "angularVelocity": 9.742765950850105e-34,
-                    "velocityX": 0,
-                    "velocityY": -8.251938001070805e-34,
-                    "timestamp": 1.8909488427728038
+                    "angularVelocity": 0,
+                    "velocityX": 6.927431243507319e-35,
+                    "velocityY": 0,
+                    "timestamp": 1.8909488427719994
                 }
             ],
             "constraints": [
@@ -25955,26 +29658,10 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C1 To W2": {
-            "waypoints": [
+            "circleObstacles": []
+        },
+        "C1 To W2": {
+            "waypoints": [
                 {
                     "x": 7.68,
                     "y": 7.467,
@@ -25999,190 +29686,190 @@
                     "x": 7.68,
                     "y": 7.467,
                     "heading": 0,
-                    "angularVelocity": 1.1003498306069892e-28,
-                    "velocityX": -2.6796850098783343e-31,
-                    "velocityY": 7.1039345927107565e-31,
+                    "angularVelocity": 0,
+                    "velocityX": -2.4374604803646449e-32,
+                    "velocityY": 2.350988701644575e-38,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.602663557475489,
-                    "y": 7.440503469641943,
-                    "heading": 0.0012193469853657303,
-                    "angularVelocity": 0.01231450998750236,
-                    "velocityX": -0.781041332205243,
-                    "velocityY": -0.26759551763859557,
-                    "timestamp": 0.09901709337987814
-                },
-                {
-                    "x": 7.447990676613656,
-                    "y": 7.387510406307137,
-                    "heading": 0.00365804177605024,
-                    "angularVelocity": 0.024629028255928873,
-                    "velocityX": -1.562082622122956,
-                    "velocityY": -0.5351910617240387,
-                    "timestamp": 0.1980341867597563
-                },
-                {
-                    "x": 7.2159813646219675,
-                    "y": 7.3080208081596165,
-                    "heading": 0.0073160847395553,
-                    "angularVelocity": 0.03694355023585351,
-                    "velocityX": -2.3431238392505196,
-                    "velocityY": -0.8027866243514209,
-                    "timestamp": 0.29705128013963444
-                },
-                {
-                    "x": 6.906635640594062,
-                    "y": 7.202034677060231,
-                    "heading": 0.012193473491592185,
-                    "angularVelocity": 0.049258048136210826,
-                    "velocityX": -3.1241648635463743,
-                    "velocityY": -1.0703821681855634,
-                    "timestamp": 0.3960683735195126
-                },
-                {
-                    "x": 6.5522807849123526,
-                    "y": 7.080627711247213,
-                    "heading": 0.012193474015757445,
-                    "angularVelocity": 5.293684623965923e-9,
-                    "velocityX": -3.5787240726429963,
-                    "velocityY": -1.2261212854152546,
-                    "timestamp": 0.4950854668993907
-                },
-                {
-                    "x": 6.197925949682556,
-                    "y": 6.959220685740379,
-                    "heading": 0.012193474539927869,
-                    "angularVelocity": 5.293736792766535e-9,
-                    "velocityX": -3.578723866093675,
-                    "velocityY": -1.2261218882789995,
-                    "timestamp": 0.5941025602792689
-                },
-                {
-                    "x": 5.843571141718115,
-                    "y": 6.8378135806532265,
-                    "heading": 0.012193475064112974,
-                    "angularVelocity": 5.2938850654703056e-9,
-                    "velocityX": -3.578723590733579,
-                    "velocityY": -1.2261226919818409,
-                    "timestamp": 0.6931196536591471
-                },
-                {
-                    "x": 5.489216369151057,
-                    "y": 6.716406372250631,
-                    "heading": 0.012193475588313017,
-                    "angularVelocity": 5.294035915259789e-9,
-                    "velocityX": -3.578723233245997,
-                    "velocityY": -1.2261237353920091,
-                    "timestamp": 0.7921367470390253
-                },
-                {
-                    "x": 5.134861642331667,
-                    "y": 6.594999030323008,
-                    "heading": 0.012193476112502804,
-                    "angularVelocity": 5.293932348218223e-9,
-                    "velocityX": -3.5787227712281067,
-                    "velocityY": -1.2261250838968347,
-                    "timestamp": 0.8911538404189034
-                },
-                {
-                    "x": 4.780507205250472,
-                    "y": 6.473590842716537,
-                    "heading": 0.012193476636274196,
-                    "angularVelocity": 5.2897066685502164e-9,
-                    "velocityX": -3.578719845084944,
-                    "velocityY": -1.2261336246326482,
-                    "timestamp": 0.9901709337987816
-                },
-                {
-                    "x": 4.428189328838181,
-                    "y": 6.346393637776016,
-                    "heading": 0.012193476110055806,
-                    "angularVelocity": -5.314419742287748e-9,
-                    "velocityX": -3.558152076436196,
-                    "velocityY": -1.2845984526381864,
-                    "timestamp": 1.0891880271786598
-                },
-                {
-                    "x": 4.075871503880035,
-                    "y": 6.219196290315138,
-                    "heading": 0.012193475583846744,
-                    "angularVelocity": -5.314325547246037e-9,
-                    "velocityX": -3.5581515567870925,
-                    "velocityY": -1.2845998919892312,
-                    "timestamp": 1.188205120558538
-                },
-                {
-                    "x": 3.723553713185669,
-                    "y": 6.091998847948706,
-                    "heading": 0.012193475057649396,
-                    "angularVelocity": -5.314207229499686e-9,
-                    "velocityX": -3.5581512107480435,
-                    "velocityY": -1.2846008504657016,
-                    "timestamp": 1.2872222139384162
-                },
-                {
-                    "x": 3.3712359540403063,
-                    "y": 5.96480131819632,
-                    "heading": 0.012193474531462604,
-                    "angularVelocity": -5.314100634839972e-9,
-                    "velocityX": -3.5581508921262577,
-                    "velocityY": -1.284601732999717,
-                    "timestamp": 1.3862393073182944
-                },
-                {
-                    "x": 3.018918225427334,
-                    "y": 5.837603703873982,
-                    "heading": 0.012193474005277722,
-                    "angularVelocity": -5.314081331919232e-9,
-                    "velocityX": -3.558150583771516,
-                    "velocityY": -1.2846025870942077,
-                    "timestamp": 1.4852564006981726
-                },
-                {
-                    "x": 2.7113509459543246,
-                    "y": 5.726562245993832,
-                    "heading": 0.0073160988837450616,
-                    "angularVelocity": -0.04925791047811804,
-                    "velocityX": -3.1062038782842247,
-                    "velocityY": -1.121437259869279,
-                    "timestamp": 1.5842734940780507
-                },
-                {
-                    "x": 2.48067547425723,
-                    "y": 5.6432811332300385,
-                    "heading": 0.003658054936391331,
-                    "angularVelocity": -0.03694356017200323,
-                    "velocityX": -2.329653030836908,
-                    "velocityY": -0.8410781403599302,
-                    "timestamp": 1.683290587457929
-                },
-                {
-                    "x": 2.326891824704753,
-                    "y": 5.5877603809363166,
-                    "heading": 0.0012193531500403563,
-                    "angularVelocity": -0.024629098907027813,
-                    "velocityX": -1.5531020382762346,
-                    "velocityY": -0.560718865789336,
-                    "timestamp": 1.782307680837807
+                    "x": 7.602872461914855,
+                    "y": 7.439913035887961,
+                    "heading": 5.3325436298288445e-18,
+                    "angularVelocity": 5.3856284215509377e-17,
+                    "velocityX": -0.7789533333445217,
+                    "velocityY": -0.2735661154121556,
+                    "timestamp": 0.09901432445765257
+                },
+                {
+                    "x": 7.448617388685645,
+                    "y": 7.385739108696781,
+                    "heading": -4.4022234029758545e-18,
+                    "angularVelocity": -9.831675377471393e-17,
+                    "velocityX": -1.557906636985473,
+                    "velocityY": -0.5471322203925048,
+                    "timestamp": 0.19802864891530514
+                },
+                {
+                    "x": 7.217234786194527,
+                    "y": 7.304478220492258,
+                    "heading": -7.053417683545123e-18,
+                    "angularVelocity": -2.6775864730895626e-17,
+                    "velocityX": -2.3368598812192953,
+                    "velocityY": -0.8206983045092451,
+                    "timestamp": 0.2970429733729577
+                },
+                {
+                    "x": 6.9087246720879545,
+                    "y": 7.196130377471774,
+                    "heading": 2.6673179344101e-18,
+                    "angularVelocity": 9.817504407722252e-17,
+                    "velocityX": -3.1158129472318725,
+                    "velocityY": -1.0942643260352094,
+                    "timestamp": 0.3960572978306103
+                },
+                {
+                    "x": 6.5553201862538,
+                    "y": 7.072015763386003,
+                    "heading": 8.41895708879283e-18,
+                    "angularVelocity": 5.808896021749495e-17,
+                    "velocityX": -3.5692258445423475,
+                    "velocityY": -1.2535015995473784,
+                    "timestamp": 0.49507162228826285
+                },
+                {
+                    "x": 6.2019157004196215,
+                    "y": 6.9479011493002245,
+                    "heading": -3.69857396739816e-19,
+                    "angularVelocity": -8.876306064099344e-17,
+                    "velocityX": -3.569225844542579,
+                    "velocityY": -1.2535015995474599,
+                    "timestamp": 0.5940859467459154
+                },
+                {
+                    "x": 5.848511214585444,
+                    "y": 6.823786535214446,
+                    "heading": -8.032367976884792e-18,
+                    "angularVelocity": -7.738789939855791e-17,
+                    "velocityX": -3.569225844542579,
+                    "velocityY": -1.2535015995474599,
+                    "timestamp": 0.693100271203568
+                },
+                {
+                    "x": 5.495106728751266,
+                    "y": 6.699671921128667,
+                    "heading": -3.490709714785344e-17,
+                    "angularVelocity": -2.714226382747745e-16,
+                    "velocityX": -3.569225844542579,
+                    "velocityY": -1.2535015995474599,
+                    "timestamp": 0.7921145956612206
+                },
+                {
+                    "x": 5.141702242917089,
+                    "y": 6.575557307042889,
+                    "heading": -1.2741337636064699e-17,
+                    "angularVelocity": 2.238641694845762e-16,
+                    "velocityX": -3.569225844542579,
+                    "velocityY": -1.2535015995474599,
+                    "timestamp": 0.8911289201188731
+                },
+                {
+                    "x": 4.788297757082911,
+                    "y": 6.4514426929571105,
+                    "heading": -2.2645871045802615e-17,
+                    "angularVelocity": -1.0003131833516608e-16,
+                    "velocityX": -3.569225844542579,
+                    "velocityY": -1.2535015995474597,
+                    "timestamp": 0.9901432445765257
+                },
+                {
+                    "x": 4.4348932712487334,
+                    "y": 6.327328078871332,
+                    "heading": -7.505924377434873e-18,
+                    "angularVelocity": 1.529066299375997e-16,
+                    "velocityX": -3.569225844542579,
+                    "velocityY": -1.25350159954746,
+                    "timestamp": 1.0891575690341782
+                },
+                {
+                    "x": 4.081488785414556,
+                    "y": 6.203213464785553,
+                    "heading": -3.3249179069580955e-17,
+                    "angularVelocity": -2.5999525658031657e-16,
+                    "velocityX": -3.569225844542579,
+                    "velocityY": -1.25350159954746,
+                    "timestamp": 1.1881718934918308
+                },
+                {
+                    "x": 3.728084299580378,
+                    "y": 6.079098850699775,
+                    "heading": -3.31250701341244e-17,
+                    "angularVelocity": 1.2534442479544336e-18,
+                    "velocityX": -3.569225844542579,
+                    "velocityY": -1.2535015995474597,
+                    "timestamp": 1.2871862179494835
+                },
+                {
+                    "x": 3.3746798137461993,
+                    "y": 5.9549842366139965,
+                    "heading": -3.474935359347948e-17,
+                    "angularVelocity": -1.6404530025230332e-17,
+                    "velocityX": -3.569225844542579,
+                    "velocityY": -1.2535015995474599,
+                    "timestamp": 1.3862005424071362
+                },
+                {
+                    "x": 3.021275327912045,
+                    "y": 5.830869622528225,
+                    "heading": -2.4652412430964293e-17,
+                    "angularVelocity": 1.0197454982216833e-16,
+                    "velocityX": -3.5692258445423475,
+                    "velocityY": -1.2535015995473784,
+                    "timestamp": 1.4852148668647889
+                },
+                {
+                    "x": 2.7127652138054734,
+                    "y": 5.722521779507741,
+                    "heading": 1.414690860074665e-18,
+                    "angularVelocity": 2.632659789801892e-16,
+                    "velocityX": -3.115812947231872,
+                    "velocityY": -1.0942643260352094,
+                    "timestamp": 1.5842291913224416
+                },
+                {
+                    "x": 2.4813826113143547,
+                    "y": 5.641260891303218,
+                    "heading": 8.307803002216867e-18,
+                    "angularVelocity": 6.961732115429119e-17,
+                    "velocityX": -2.336859881219295,
+                    "velocityY": -0.8206983045092451,
+                    "timestamp": 1.6832435157800942
+                },
+                {
+                    "x": 2.3271275380851444,
+                    "y": 5.587086964112038,
+                    "heading": -3.972937436005726e-18,
+                    "angularVelocity": -1.2402993582700068e-16,
+                    "velocityX": -1.557906636985473,
+                    "velocityY": -0.5471322203925048,
+                    "timestamp": 1.782257840237747
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": -2.184437138856228e-30,
-                    "angularVelocity": -0.012314572246193395,
-                    "velocityX": -0.7765510184161662,
-                    "velocityY": -0.28035948126465987,
-                    "timestamp": 1.8813247742176853
+                    "heading": 0,
+                    "angularVelocity": 4.012487544797503e-17,
+                    "velocityX": -0.7789533333445219,
+                    "velocityY": -0.2735661154121556,
+                    "timestamp": 1.8812721646953996
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
                     "heading": 0,
-                    "angularVelocity": 2.219047153199805e-29,
-                    "velocityX": 0,
-                    "velocityY": -7.552577135588168e-32,
-                    "timestamp": 1.9803418675975635
+                    "angularVelocity": 0,
+                    "velocityX": -5.265297803216144e-35,
+                    "velocityY": 0,
+                    "timestamp": 1.9802864891530523
                 }
             ],
             "constraints": [
@@ -26202,23 +29889,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "C1 To W3": {
             "waypoints": [
@@ -26231,6 +29902,15 @@
                     "headingConstrained": true,
                     "controlIntervalCount": 23
                 },
+                {
+                    "x": 2.279822826385498,
+                    "y": 5.254662990570068,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 40
+                },
                 {
                     "x": 2.3,
                     "y": 4.36,
@@ -26264,16 +29944,6 @@
                     "x": 3.42,
                     "y": 4.04,
                     "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
                 }
             ]
         },
@@ -26302,128 +29972,128 @@
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 1.9214160460800783e-32,
-                    "angularVelocity": -7.67482132813978e-30,
-                    "velocityX": -1.6843046577095525e-31,
-                    "velocityY": 2.6209535651210065e-31,
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": -7.649625564275916e-33,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.6177977502927385,
-                    "y": 7.446201757051976,
-                    "heading": 0.01344244845353824,
-                    "angularVelocity": 0.15144190849275008,
-                    "velocityX": -0.7007672331072132,
-                    "velocityY": -0.23431189760056984,
-                    "timestamp": 0.08876306820393338
-                },
-                {
-                    "x": 7.493368265532485,
-                    "y": 7.404602703026682,
-                    "heading": 0.038702181990287625,
-                    "angularVelocity": 0.2845748129935982,
-                    "velocityX": -1.4018159497887253,
-                    "velocityY": -0.4686527276087507,
-                    "timestamp": 0.17752613640786677
-                },
-                {
-                    "x": 7.306675971092295,
-                    "y": 7.342201042854806,
-                    "heading": 0.0730274998055895,
-                    "angularVelocity": 0.38670720275422266,
-                    "velocityX": -2.1032654483226527,
-                    "velocityY": -0.7030137807724448,
-                    "timestamp": 0.26628920461180017
-                },
-                {
-                    "x": 7.057674447994484,
-                    "y": 7.2590026396848994,
-                    "heading": 0.11075741429874605,
-                    "angularVelocity": 0.42506320764649574,
-                    "velocityX": -2.8052379005867665,
-                    "velocityY": -0.9373087800171416,
-                    "timestamp": 0.35505227281573354
-                },
-                {
-                    "x": 6.746464686770232,
-                    "y": 7.155112270199717,
-                    "heading": 0.1333971305446714,
-                    "angularVelocity": 0.25505783773756635,
-                    "velocityX": -3.506072598902806,
-                    "velocityY": -1.1704233707338827,
-                    "timestamp": 0.4438153410196669
-                },
-                {
-                    "x": 6.428047616602866,
-                    "y": 7.048517672195703,
-                    "heading": 0.13339714147429826,
-                    "angularVelocity": 1.23133576958653e-7,
-                    "velocityX": -3.5872697576863386,
-                    "velocityY": -1.200889065236652,
-                    "timestamp": 0.5325784092236003
-                },
-                {
-                    "x": 6.109630567326096,
-                    "y": 6.9419230117899575,
-                    "heading": 0.13339715240409608,
-                    "angularVelocity": 1.231354889650669e-7,
-                    "velocityX": -3.5872695223333455,
-                    "velocityY": -1.200889768252866,
-                    "timestamp": 0.6213414774275338
-                },
-                {
-                    "x": 5.798381237872348,
-                    "y": 6.8380188367314085,
-                    "heading": 0.15626327537058046,
-                    "angularVelocity": 0.25760852379277716,
-                    "velocityX": -3.506518372457005,
-                    "velocityY": -1.170578903621919,
-                    "timestamp": 0.7101045456314672
-                },
-                {
-                    "x": 5.549363092262374,
-                    "y": 6.7548143649024714,
-                    "heading": 0.19280777245315245,
-                    "angularVelocity": 0.41170835825755314,
-                    "velocityX": -2.8054251689136636,
-                    "velocityY": -0.9373771492347068,
-                    "timestamp": 0.7988676138354006
-                },
-                {
-                    "x": 5.36265388534366,
-                    "y": 6.692406232212667,
-                    "heading": 0.22585001680889633,
-                    "angularVelocity": 0.3722521655212111,
-                    "velocityX": -2.1034559833970325,
-                    "velocityY": -0.7030866998372853,
-                    "timestamp": 0.887630682039334
-                },
-                {
-                    "x": 5.238210362873152,
-                    "y": 6.650801584329758,
-                    "heading": 0.25010700866879837,
-                    "angularVelocity": 0.27327797868904535,
-                    "velocityX": -1.4019740978791027,
-                    "velocityY": -0.46871574772688473,
-                    "timestamp": 0.9763937502432675
+                    "x": 7.617797764168564,
+                    "y": 7.446201717117849,
+                    "heading": 0.013442479376395878,
+                    "angularVelocity": 0.15144225686963014,
+                    "velocityX": -0.7007670767707749,
+                    "velocityY": -0.23431234749607086,
+                    "timestamp": 0.08876306820531243
+                },
+                {
+                    "x": 7.493368305965553,
+                    "y": 7.404602586388856,
+                    "heading": 0.03870227011836747,
+                    "angularVelocity": 0.28457545746724416,
+                    "velocityX": -1.401815650572125,
+                    "velocityY": -0.4686535917478179,
+                    "timestamp": 0.17752613641062487
+                },
+                {
+                    "x": 7.30667604944407,
+                    "y": 7.3422008161122045,
+                    "heading": 0.0730276629215222,
+                    "angularVelocity": 0.38670804756608035,
+                    "velocityX": -2.1032650210965835,
+                    "velocityY": -0.7030150212056434,
+                    "timestamp": 0.2662892046159373
+                },
+                {
+                    "x": 7.057674574154849,
+                    "y": 7.259002273186937,
+                    "heading": 0.11075765125141449,
+                    "angularVelocity": 0.42506403948785515,
+                    "velocityX": -2.8052373619302045,
+                    "velocityY": -0.9373103544914424,
+                    "timestamp": 0.35505227282124974
+                },
+                {
+                    "x": 6.746464870184761,
+                    "y": 7.155111739606062,
+                    "heading": 0.13339737013954753,
+                    "angularVelocity": 0.25505786749611997,
+                    "velocityX": -3.5060719538216505,
+                    "velocityY": -1.170425219424191,
+                    "timestamp": 0.44381534102656217
+                },
+                {
+                    "x": 6.428047828205506,
+                    "y": 7.048517057389171,
+                    "heading": 0.13339738106725169,
+                    "angularVelocity": 1.2311157711164836e-7,
+                    "velocityX": -3.5872694400628884,
+                    "velocityY": -1.2008900139638257,
+                    "timestamp": 0.5325784092318746
+                },
+                {
+                    "x": 6.109630786224927,
+                    "y": 6.941922375176235,
+                    "heading": 0.1333973919949535,
+                    "angularVelocity": 1.23111550991103e-7,
+                    "velocityX": -3.587269440077816,
+                    "velocityY": -1.200890013919235,
+                    "timestamp": 0.621341477437187
+                },
+                {
+                    "x": 5.798381397539494,
+                    "y": 6.838018370585249,
+                    "heading": 0.15626352052401435,
+                    "angularVelocity": 0.2576085864583032,
+                    "velocityX": -3.5065190397107555,
+                    "velocityY": -1.1705769831075976,
+                    "timestamp": 0.7101045456424995
+                },
+                {
+                    "x": 5.5493631968567065,
+                    "y": 6.754814060290535,
+                    "heading": 0.1928079538664876,
+                    "angularVelocity": 0.4117076401528937,
+                    "velocityX": -2.8054257893230465,
+                    "velocityY": -0.9373753293689796,
+                    "timestamp": 0.7988676138478119
+                },
+                {
+                    "x": 5.362653942099421,
+                    "y": 6.692406067264648,
+                    "heading": 0.22585011955614995,
+                    "angularVelocity": 0.37225127926004886,
+                    "velocityX": -2.1034565223155943,
+                    "velocityY": -0.703085126367409,
+                    "timestamp": 0.8876306820531243
+                },
+                {
+                    "x": 5.238210383284219,
+                    "y": 6.650801525067523,
+                    "heading": 0.25010704549903756,
+                    "angularVelocity": 0.2732772360607878,
+                    "velocityX": -1.4019745073178984,
+                    "velocityY": -0.4687145570597535,
+                    "timestamp": 0.9763937502584368
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
                     "heading": 0.263,
-                    "angularVelocity": 0.1452517538281686,
-                    "velocityX": -0.7008586355989793,
-                    "velocityY": -0.23434954143793293,
-                    "timestamp": 1.065156818447201
+                    "angularVelocity": 0.1452513388947291,
+                    "velocityX": -0.7008588655398115,
+                    "velocityY": -0.2343488737838604,
+                    "timestamp": 1.0651568184637492
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
                     "heading": 0.263,
-                    "angularVelocity": 0,
-                    "velocityX": -1.1321901910980418e-33,
+                    "angularVelocity": -2.2743517080702824e-31,
+                    "velocityX": 0,
                     "velocityY": 0,
-                    "timestamp": 1.1539198866511342
+                    "timestamp": 1.1539198866690616
                 }
             ],
             "constraints": [
@@ -26443,23 +30113,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "C1 To S2": {
             "waypoints": [
@@ -26486,146 +30140,146 @@
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": -1.0300263407047986e-28,
-                    "angularVelocity": -5.012733855435028e-29,
+                    "heading": -6.443666081756986e-29,
+                    "angularVelocity": 1.4558689567742243e-28,
                     "velocityX": 0,
-                    "velocityY": -6.93481134012921e-30,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.593939909156862,
-                    "y": 7.445359818001645,
-                    "heading": -0.04618113523184224,
-                    "angularVelocity": -0.44533187598567886,
-                    "velocityX": -0.829890852839506,
-                    "velocityY": -0.20867964370321365,
-                    "timestamp": 0.10370049332232066
-                },
-                {
-                    "x": 7.42157070597782,
-                    "y": 7.402025181316033,
-                    "heading": -0.13159429090032063,
-                    "angularVelocity": -0.8236523562429084,
-                    "velocityX": -1.6621830586985462,
-                    "velocityY": -0.41788264739409464,
-                    "timestamp": 0.20740098664464132
-                },
-                {
-                    "x": 7.162505106155824,
-                    "y": 7.336932536575702,
-                    "heading": -0.24235044061142916,
-                    "angularVelocity": -1.0680387928995545,
-                    "velocityX": -2.4982099074184148,
-                    "velocityY": -0.6276985061006933,
-                    "timestamp": 0.31110147996696197
-                },
-                {
-                    "x": 6.816624706468089,
-                    "y": 7.25023883508299,
-                    "heading": -0.3315040304347048,
-                    "angularVelocity": -0.8597219450650766,
-                    "velocityX": -3.3353785368472084,
-                    "velocityY": -0.8360008589665135,
-                    "timestamp": 0.41480197328928264
-                },
-                {
-                    "x": 6.436297343977517,
-                    "y": 7.154089263427661,
-                    "heading": -0.33150405743314826,
-                    "angularVelocity": -2.603501934813404e-7,
-                    "velocityX": -3.6675559614595192,
-                    "velocityY": -0.9271852869251594,
-                    "timestamp": 0.5185024666116033
-                },
-                {
-                    "x": 6.0559700009320565,
-                    "y": 7.057939614855378,
-                    "heading": -0.3315040844315366,
-                    "angularVelocity": -2.603496620899773e-7,
-                    "velocityX": -3.6675557739472766,
-                    "velocityY": -0.9271860286473393,
-                    "timestamp": 0.6222029599339239
-                },
-                {
-                    "x": 5.675642815380094,
-                    "y": 6.961789343307514,
-                    "heading": -0.3315041114299189,
-                    "angularVelocity": -2.603496031733668e-7,
-                    "velocityX": -3.667554255212956,
-                    "velocityY": -0.927192036097853,
-                    "timestamp": 0.7259034532562446
-                },
-                {
-                    "x": 5.306931757393967,
-                    "y": 6.827827416139461,
-                    "heading": -0.33150413923830896,
-                    "angularVelocity": -2.681606334045615e-7,
-                    "velocityX": -3.5555381288313455,
-                    "velocityY": -1.2918157173241618,
-                    "timestamp": 0.8296039465785653
-                },
-                {
-                    "x": 4.978271181730368,
-                    "y": 6.6136408985726165,
-                    "heading": -0.3315041735834931,
-                    "angularVelocity": -3.311959575473401e-7,
-                    "velocityX": -3.1693250931994936,
-                    "velocityY": -2.0654339309757477,
-                    "timestamp": 0.933304439900886
-                },
-                {
-                    "x": 4.706831855999529,
-                    "y": 6.330419553539566,
-                    "heading": -0.3315041734698381,
-                    "angularVelocity": 1.0959926731432931e-9,
-                    "velocityX": -2.617531672555858,
-                    "velocityY": -2.7311475187754666,
-                    "timestamp": 1.0370049332232065
-                },
-                {
-                    "x": 4.5189225775062924,
-                    "y": 6.028421356099611,
-                    "heading": -0.23258180854056226,
-                    "angularVelocity": 0.9539237641021904,
-                    "velocityX": -1.8120384240525347,
-                    "velocityY": -2.912215629498277,
-                    "timestamp": 1.1407054265455272
-                },
-                {
-                    "x": 4.388098272679404,
-                    "y": 5.795527872027247,
-                    "heading": -0.12496959809450543,
-                    "angularVelocity": 1.0377212971549954,
-                    "velocityX": -1.2615591366596612,
-                    "velocityY": -2.2458281210725595,
-                    "timestamp": 1.2444059198678479
-                },
-                {
-                    "x": 4.304052104796344,
-                    "y": 5.638778152214578,
-                    "heading": -0.04358897284576924,
-                    "angularVelocity": 0.7847660376675182,
-                    "velocityX": -0.8104702802311671,
-                    "velocityY": -1.5115619491362144,
-                    "timestamp": 1.3481064131901686
+                    "x": 7.593939912495806,
+                    "y": 7.445359803187238,
+                    "heading": -0.04618112566514864,
+                    "angularVelocity": -0.44533178373248467,
+                    "velocityX": -0.8298908206418381,
+                    "velocityY": -0.2086797865609081,
+                    "timestamp": 0.10370049332228601
+                },
+                {
+                    "x": 7.421570715628637,
+                    "y": 7.402025139488172,
+                    "heading": -0.13159426902714605,
+                    "angularVelocity": -0.823652237569843,
+                    "velocityX": -1.662182997832739,
+                    "velocityY": -0.41788290788915955,
+                    "timestamp": 0.20740098664457202
+                },
+                {
+                    "x": 7.162505124622784,
+                    "y": 7.336932458231965,
+                    "heading": -0.24235041158328322,
+                    "angularVelocity": -1.0680387239038096,
+                    "velocityX": -2.4982098224038025,
+                    "velocityY": -0.627698858229146,
+                    "timestamp": 0.311101479966858
+                },
+                {
+                    "x": 6.816624735331939,
+                    "y": 7.250238713519174,
+                    "heading": -0.3315039987658666,
+                    "angularVelocity": -0.8597219196007858,
+                    "velocityX": -3.3353784365894983,
+                    "velocityY": -0.8360012757447058,
+                    "timestamp": 0.41480197328914403
+                },
+                {
+                    "x": 6.436297385949381,
+                    "y": 7.154089090014683,
+                    "heading": -0.3315040257643058,
+                    "angularVelocity": -2.603501519346576e-7,
+                    "velocityX": -3.6675558350581356,
+                    "velocityY": -0.927185786914993,
+                    "timestamp": 0.5185024666114301
+                },
+                {
+                    "x": 6.055970043958053,
+                    "y": 7.05793943727301,
+                    "heading": -0.33150405276269507,
+                    "angularVelocity": -2.60349670609762e-7,
+                    "velocityX": -3.6675557637833287,
+                    "velocityY": -0.9271860688537472,
+                    "timestamp": 0.6222029599337161
+                },
+                {
+                    "x": 5.675642817406181,
+                    "y": 6.961789327902759,
+                    "heading": -0.33150407976108204,
+                    "angularVelocity": -2.6034964813703677e-7,
+                    "velocityX": -3.6675546505826837,
+                    "velocityY": -0.9271904721942118,
+                    "timestamp": 0.7259034532560021
+                },
+                {
+                    "x": 5.306931756724127,
+                    "y": 6.827827408155551,
+                    "heading": -0.331504107569482,
+                    "angularVelocity": -2.6816073038858223e-7,
+                    "velocityX": -3.5555381548297533,
+                    "velocityY": -1.2918156457642633,
+                    "timestamp": 0.8296039465782882
+                },
+                {
+                    "x": 4.97827117667767,
+                    "y": 6.613640897314209,
+                    "heading": -0.3315041419146958,
+                    "angularVelocity": -3.311962425493206e-7,
+                    "velocityX": -3.1693251354651193,
+                    "velocityY": -2.065433866121419,
+                    "timestamp": 0.9333044399005742
+                },
+                {
+                    "x": 4.706831845168329,
+                    "y": 6.330419557819514,
+                    "heading": -0.3315041418010325,
+                    "angularVelocity": 1.0960729474265148e-9,
+                    "velocityX": -2.6175317282796997,
+                    "velocityY": -2.7311474653691747,
+                    "timestamp": 1.0370049332228601
+                },
+                {
+                    "x": 4.518922560808715,
+                    "y": 6.028421366315536,
+                    "heading": -0.2325817741817243,
+                    "angularVelocity": 0.9539237900428917,
+                    "velocityX": -1.8120384806234937,
+                    "velocityY": -2.912215572257732,
+                    "timestamp": 1.1407054265451462
+                },
+                {
+                    "x": 4.388098262013575,
+                    "y": 5.795527878516759,
+                    "heading": -0.12496957799500227,
+                    "angularVelocity": 1.0377211596504115,
+                    "velocityX": -1.2615590784949995,
+                    "velocityY": -2.245828157007679,
+                    "timestamp": 1.2444059198674322
+                },
+                {
+                    "x": 4.304052100915554,
+                    "y": 5.638778154553602,
+                    "heading": -0.0435889655527213,
+                    "angularVelocity": 0.7847659141732163,
+                    "velocityX": -0.8104702148022851,
+                    "velocityY": -1.5115619891604988,
+                    "timestamp": 1.3481064131897182
                 },
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 0.42033525057857213,
-                    "velocityX": -0.3958718370677089,
-                    "velocityY": -0.7596699850764065,
-                    "timestamp": 1.4518069065124892
+                    "heading": 7.875266725539043e-28,
+                    "angularVelocity": 0.42033518025077415,
+                    "velocityX": -0.39587179964482383,
+                    "velocityY": -0.7596700076322139,
+                    "timestamp": 1.4518069065120043
                 },
                 {
                     "x": 4.263,
                     "y": 5.56,
                     "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": -5.117426772684828e-28,
-                    "velocityY": 3.5837289853658606e-29,
-                    "timestamp": 1.55550739983481
+                    "angularVelocity": -1.3117957600781845e-26,
+                    "velocityX": -2.8963805377195657e-28,
+                    "velocityY": 0,
+                    "timestamp": 1.5555073998342903
                 }
             ],
             "constraints": [
@@ -26646,16 +30300,6 @@
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
             "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
                 {
                     "x": 5.61,
                     "y": 5.44,
@@ -26672,7 +30316,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 23
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 5.366531848907471,
+                    "y": 4.105798244476318,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 12
                 },
                 {
                     "x": 4.263,
@@ -26684,7 +30337,269 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 0,
+                    "angularVelocity": 3.237981257278863e-31,
+                    "velocityX": 3.6994687797817426e-32,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.663124715083084,
+                    "y": 7.408544235319849,
+                    "heading": 0.027485198347684953,
+                    "angularVelocity": 0.3204918925418427,
+                    "velocityX": -0.19677471240300645,
+                    "velocityY": -0.6816250119548423,
+                    "timestamp": 0.08575941852911824
+                },
+                {
+                    "x": 7.629347887601235,
+                    "y": 7.291532761279695,
+                    "heading": 0.07929730291499706,
+                    "angularVelocity": 0.6041564350126787,
+                    "velocityX": -0.3938556028149893,
+                    "velocityY": -1.3644154315298311,
+                    "timestamp": 0.17151883705823648
+                },
+                {
+                    "x": 7.578636110956943,
+                    "y": 7.115823334781324,
+                    "heading": 0.15015093083574435,
+                    "angularVelocity": 0.8261906288076145,
+                    "velocityX": -0.5913260317532774,
+                    "velocityY": -2.048864480566788,
+                    "timestamp": 0.25727825558735473
+                },
+                {
+                    "x": 7.510961813450331,
+                    "y": 6.881225838812792,
+                    "heading": 0.22908991381513497,
+                    "angularVelocity": 0.9204701283345157,
+                    "velocityX": -0.7891179612374983,
+                    "velocityY": -2.7355303941208287,
+                    "timestamp": 0.34303767411647296
+                },
+                {
+                    "x": 7.426596388399054,
+                    "y": 6.588221528517866,
+                    "heading": 0.2780284674480044,
+                    "angularVelocity": 0.5706493172671544,
+                    "velocityX": -0.9837453016618997,
+                    "velocityY": -3.4165846191627605,
+                    "timestamp": 0.4287970926455912
+                },
+                {
+                    "x": 7.336364964950643,
+                    "y": 6.276599275391096,
+                    "heading": 0.2780284905344079,
+                    "angularVelocity": 2.691996271350766e-7,
+                    "velocityX": -1.0521459333096546,
+                    "velocityY": -3.6336796409243712,
+                    "timestamp": 0.5145565111747095
+                },
+                {
+                    "x": 7.246133490581996,
+                    "y": 5.964977037008454,
+                    "heading": 0.27802851362075814,
+                    "angularVelocity": 2.6919900573505555e-7,
+                    "velocityX": -1.0521465270664079,
+                    "velocityY": -3.633679469000075,
+                    "timestamp": 0.6003159297038277
+                },
+                {
+                    "x": 7.155901958204915,
+                    "y": 5.653354815422387,
+                    "heading": 0.2780285367070952,
+                    "angularVelocity": 2.6919885218329885e-7,
+                    "velocityX": -1.0521472034753196,
+                    "velocityY": -3.633679273143183,
+                    "timestamp": 0.686075348232946
+                },
+                {
+                    "x": 7.06567006903972,
+                    "y": 5.341732697146042,
+                    "heading": 0.27802855979342844,
+                    "angularVelocity": 2.6919880800587287e-7,
+                    "velocityX": -1.052151363812688,
+                    "velocityY": -3.6336780684973866,
+                    "timestamp": 0.7718347667620643
+                },
+                {
+                    "x": 6.941702537617662,
+                    "y": 5.041929112902265,
+                    "heading": 0.2780285835706332,
+                    "angularVelocity": 2.772547333115093e-7,
+                    "velocityX": -1.445526725207054,
+                    "velocityY": -3.495867735413615,
+                    "timestamp": 0.8575941852911826
+                },
+                {
+                    "x": 6.763533066668612,
+                    "y": 4.770809627708675,
+                    "heading": 0.2780286115416881,
+                    "angularVelocity": 3.2615723612466423e-7,
+                    "velocityX": -2.0775498948673006,
+                    "velocityY": -3.1613960290733045,
+                    "timestamp": 0.9433536038203009
+                },
+                {
+                    "x": 6.537527544905599,
+                    "y": 4.538061625407243,
+                    "heading": 0.2780286466069666,
+                    "angularVelocity": 4.0887962009830865e-7,
+                    "velocityX": -2.6353434484432303,
+                    "velocityY": -2.713964323608448,
+                    "timestamp": 1.0291130223494191
+                },
+                {
+                    "x": 6.274440682462666,
+                    "y": 4.354088811409382,
+                    "heading": 0.26884633764727317,
+                    "angularVelocity": -0.10707055991260324,
+                    "velocityX": -3.0677314160380695,
+                    "velocityY": -2.145219932145364,
+                    "timestamp": 1.1148724408785373
+                },
+                {
+                    "x": 5.982323590323029,
+                    "y": 4.22411616098336,
+                    "heading": 0.2547621599051439,
+                    "angularVelocity": -0.16422893232822805,
+                    "velocityX": -3.40623918806603,
+                    "velocityY": -1.5155495764222278,
+                    "timestamp": 1.2006318594076555
+                },
+                {
+                    "x": 5.6751302181547745,
+                    "y": 4.153390532297334,
+                    "heading": 0.22346642432676106,
+                    "angularVelocity": -0.3649247641266729,
+                    "velocityX": -3.582036555716069,
+                    "velocityY": -0.8246980902979518,
+                    "timestamp": 1.2863912779367737
+                },
+                {
+                    "x": 5.366531848907471,
+                    "y": 4.105798244476318,
+                    "heading": 0.18319904977282217,
+                    "angularVelocity": -0.4695388010388101,
+                    "velocityX": -3.5984195618411468,
+                    "velocityY": -0.5549511486584576,
+                    "timestamp": 1.3721506964658918
+                },
+                {
+                    "x": 5.15360011207683,
+                    "y": 4.041515222282991,
+                    "heading": 0.15622154454817708,
+                    "angularVelocity": -0.44121073940112593,
+                    "velocityX": -3.482448368249538,
+                    "velocityY": -1.051333676582719,
+                    "timestamp": 1.4332949588681712
+                },
+                {
+                    "x": 4.961585993321137,
+                    "y": 3.955708543808847,
+                    "heading": 0.09940090208560654,
+                    "angularVelocity": -0.9292882149552651,
+                    "velocityX": -3.1403456548775974,
+                    "velocityY": -1.403348001969601,
+                    "timestamp": 1.4944392212704505
+                },
+                {
+                    "x": 4.792433392014809,
+                    "y": 3.849704139398821,
+                    "heading": 0.01980990286006859,
+                    "angularVelocity": -1.301692032882722,
+                    "velocityX": -2.766450925410501,
+                    "velocityY": -1.7336770490843985,
+                    "timestamp": 1.5555834836727298
+                },
+                {
+                    "x": 4.64716063510186,
+                    "y": 3.7242164869664776,
+                    "heading": -0.07646566712737501,
+                    "angularVelocity": -1.5745642551712946,
+                    "velocityX": -2.3759016987918233,
+                    "velocityY": -2.0523209783239644,
+                    "timestamp": 1.6167277460750091
+                },
+                {
+                    "x": 4.526373969972362,
+                    "y": 3.5797426645365276,
+                    "heading": -0.18496768957846801,
+                    "angularVelocity": -1.7745250034621045,
+                    "velocityX": -1.975437438999914,
+                    "velocityY": -2.362835313629761,
+                    "timestamp": 1.6778720084772885
+                },
+                {
+                    "x": 4.435318488505818,
+                    "y": 3.4263170797911884,
+                    "heading": -0.3033986089581413,
+                    "angularVelocity": -1.936909772506439,
+                    "velocityX": -1.489190937777188,
+                    "velocityY": -2.5092392763841707,
+                    "timestamp": 1.7390162708795678
+                },
+                {
+                    "x": 4.37042278768838,
+                    "y": 3.2883178140362905,
+                    "heading": -0.39683380701899557,
+                    "angularVelocity": -1.5281106417823267,
+                    "velocityX": -1.0613538910728308,
+                    "velocityY": -2.2569454652502787,
+                    "timestamp": 1.800160533281847
+                },
+                {
+                    "x": 4.3241368483863285,
+                    "y": 3.1745112953889034,
+                    "heading": -0.4672868225779202,
+                    "angularVelocity": -1.1522424638210744,
+                    "velocityX": -0.7569956277749857,
+                    "velocityY": -1.8612787884925825,
+                    "timestamp": 1.8613047956841264
+                },
+                {
+                    "x": 4.292271472981756,
+                    "y": 3.08778416242541,
+                    "heading": -0.5177338397629753,
+                    "angularVelocity": -0.8250490757931602,
+                    "velocityX": -0.5211507041318677,
+                    "velocityY": -1.418401818193496,
+                    "timestamp": 1.9224490580864058
+                },
+                {
+                    "x": 4.272405555044274,
+                    "y": 3.0293931910282037,
+                    "heading": -0.5502025950282511,
+                    "angularVelocity": -0.5310188395381721,
+                    "velocityX": -0.3249024055075056,
+                    "velocityY": -0.9549705745576195,
+                    "timestamp": 1.983593320488685
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": -0.566,
+                    "angularVelocity": -0.25836283489389134,
+                    "velocityX": -0.15382563587721435,
+                    "velocityY": -0.48071871134564137,
+                    "timestamp": 2.0447375828909644
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": -0.566,
+                    "angularVelocity": 0,
+                    "velocityX": 3.3381626772503296e-32,
+                    "velocityY": -2.986960154995716e-30,
+                    "timestamp": 2.105881845293244
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -26729,7 +30644,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 25
+                    "controlIntervalCount": 20
+                },
+                {
+                    "x": 6.667655944824219,
+                    "y": 1.738860011100769,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 12
                 },
                 {
                     "x": 5.176,
@@ -26741,7 +30665,305 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 0,
+                    "angularVelocity": 9.776523821034258e-32,
+                    "velocityX": -4.560355515853987e-33,
+                    "velocityY": 2.3850698853192112e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.67836355889096,
+                    "y": 7.400720406813562,
+                    "heading": -0.028945523101477338,
+                    "angularVelocity": -0.3234212568317029,
+                    "velocityX": -0.018284687354286195,
+                    "velocityY": -0.7405714954779761,
+                    "timestamp": 0.0894978993806123
+                },
+                {
+                    "x": 7.675074058245281,
+                    "y": 7.268033905727148,
+                    "heading": -0.0827237859595526,
+                    "angularVelocity": -0.6008885485609575,
+                    "velocityX": -0.03675505982201489,
+                    "velocityY": -1.4825655351097347,
+                    "timestamp": 0.1789957987612246
+                },
+                {
+                    "x": 7.670107822881543,
+                    "y": 7.068767684711926,
+                    "heading": -0.1543450175390465,
+                    "angularVelocity": -0.8002560068466026,
+                    "velocityX": -0.05548996566520156,
+                    "velocityY": -2.226490480718353,
+                    "timestamp": 0.2684936981418369
+                },
+                {
+                    "x": 7.663417838093003,
+                    "y": 6.802742204879413,
+                    "heading": -0.22912917227010834,
+                    "angularVelocity": -0.8355967597967521,
+                    "velocityX": -0.07475018782384167,
+                    "velocityY": -2.9724214945109684,
+                    "timestamp": 0.3579915975224492
+                },
+                {
+                    "x": 7.654753648982881,
+                    "y": 6.471615180012679,
+                    "heading": -0.2502157544796387,
+                    "angularVelocity": -0.23560980040272358,
+                    "velocityX": -0.0968088543986332,
+                    "velocityY": -3.6998301318619107,
+                    "timestamp": 0.4474894969030615
+                },
+                {
+                    "x": 7.646619499365586,
+                    "y": 6.133147678353669,
+                    "heading": -0.250215779137968,
+                    "angularVelocity": -2.7551852588992195e-7,
+                    "velocityX": -0.09088648642693918,
+                    "velocityY": -3.78184855735655,
+                    "timestamp": 0.5369873962836738
+                },
+                {
+                    "x": 7.638485287213415,
+                    "y": 5.7946801781975115,
+                    "heading": -0.25021580379628167,
+                    "angularVelocity": -2.755183512776458e-7,
+                    "velocityX": -0.09088718515702116,
+                    "velocityY": -3.7818485405645093,
+                    "timestamp": 0.626485295664286
+                },
+                {
+                    "x": 7.630351002745582,
+                    "y": 5.456212679779282,
+                    "heading": -0.2502158284545949,
+                    "angularVelocity": -2.7551834628402704e-7,
+                    "velocityX": -0.09088799317222593,
+                    "velocityY": -3.781848521145859,
+                    "timestamp": 0.7159831950448983
+                },
+                {
+                    "x": 7.622216641973054,
+                    "y": 5.117745183194865,
+                    "heading": -0.25021585311291117,
+                    "angularVelocity": -2.7551838019723297e-7,
+                    "velocityX": -0.09088884575864668,
+                    "velocityY": -3.781848500655846,
+                    "timestamp": 0.8054810944255105
+                },
+                {
+                    "x": 7.614082202627753,
+                    "y": 4.779277688498788,
+                    "heading": -0.25021587777122717,
+                    "angularVelocity": -2.755183772950637e-7,
+                    "velocityX": -0.09088972368733664,
+                    "velocityY": -3.7818484795565834,
+                    "timestamp": 0.8949789938061228
+                },
+                {
+                    "x": 7.605947683187911,
+                    "y": 4.440810195727643,
+                    "heading": -0.2502159024295383,
+                    "angularVelocity": -2.755183223713384e-7,
+                    "velocityX": -0.09089061861940764,
+                    "velocityY": -3.7818484580484664,
+                    "timestamp": 0.984476893186735
+                },
+                {
+                    "x": 7.597813082423135,
+                    "y": 4.1023427049110195,
+                    "heading": -0.25021592708784146,
+                    "angularVelocity": -2.755182337798815e-7,
+                    "velocityX": -0.09089152729921718,
+                    "velocityY": -3.781848436209727,
+                    "timestamp": 1.0739747925673473
+                },
+                {
+                    "x": 7.589678399478048,
+                    "y": 3.7638752160694957,
+                    "heading": -0.2502159517461411,
+                    "angularVelocity": -2.755181940361221e-7,
+                    "velocityX": -0.09089244553653594,
+                    "velocityY": -3.781848414141052,
+                    "timestamp": 1.1634726919479597
+                },
+                {
+                    "x": 7.581543633206817,
+                    "y": 3.4254077292306317,
+                    "heading": -0.2502159764044425,
+                    "angularVelocity": -2.7551821391136536e-7,
+                    "velocityX": -0.09089337657676429,
+                    "velocityY": -3.7818483917644334,
+                    "timestamp": 1.252970591328572
+                },
+                {
+                    "x": 7.573407384740196,
+                    "y": 3.0869402780182864,
+                    "heading": -0.2502160010627749,
+                    "angularVelocity": -2.7551856112041726e-7,
+                    "velocityX": -0.09090993780780313,
+                    "velocityY": -3.7818479936934355,
+                    "timestamp": 1.3424684907091844
+                },
+                {
+                    "x": 7.507089727245593,
+                    "y": 2.754933679576005,
+                    "heading": -0.25021602759782946,
+                    "angularVelocity": -2.964880153822211e-7,
+                    "velocityX": -0.7409968049928212,
+                    "velocityY": -3.709658000243552,
+                    "timestamp": 1.4319663900897968
+                },
+                {
+                    "x": 7.3768883522731565,
+                    "y": 2.4424052627551904,
+                    "heading": -0.2502160612558337,
+                    "angularVelocity": -3.7607591242222915e-7,
+                    "velocityX": -1.454798111168213,
+                    "velocityY": -3.492019577931195,
+                    "timestamp": 1.5214642894704091
+                },
+                {
+                    "x": 7.1878697455519145,
+                    "y": 2.1615167326419513,
+                    "heading": -0.2502161148657548,
+                    "angularVelocity": -5.990075908840489e-7,
+                    "velocityX": -2.111989309574672,
+                    "velocityY": -3.1384929932119427,
+                    "timestamp": 1.6109621888510215
+                },
+                {
+                    "x": 6.94738938516625,
+                    "y": 1.9231986374023808,
+                    "heading": -0.25021645217340494,
+                    "angularVelocity": -0.000003768889018580663,
+                    "velocityX": -2.686994466350113,
+                    "velocityY": -2.6628345121941095,
+                    "timestamp": 1.7004600882316339
+                },
+                {
+                    "x": 6.667655944824219,
+                    "y": 1.738860011100769,
+                    "heading": -0.25893161706240975,
+                    "angularVelocity": -0.09737842954215001,
+                    "velocityX": -3.125586659329227,
+                    "velocityY": -2.059697798242895,
+                    "timestamp": 1.7899579876122462
+                },
+                {
+                    "x": 6.480451878153193,
+                    "y": 1.646736643588724,
+                    "heading": -0.268441983388845,
+                    "angularVelocity": -0.16928620019223647,
+                    "velocityX": -3.3322654479868237,
+                    "velocityY": -1.6398122111953808,
+                    "timestamp": 1.846137205502623
+                },
+                {
+                    "x": 6.284980922842322,
+                    "y": 1.5795583069683912,
+                    "heading": -0.28291661109989463,
+                    "angularVelocity": -0.25765092955355146,
+                    "velocityX": -3.47941752575298,
+                    "velocityY": -1.1957862558966221,
+                    "timestamp": 1.9023164233929999
+                },
+                {
+                    "x": 6.086191677834544,
+                    "y": 1.5383458560749974,
+                    "heading": -0.3070406119029933,
+                    "angularVelocity": -0.429411474723066,
+                    "velocityX": -3.53848366838567,
+                    "velocityY": -0.7335889042423714,
+                    "timestamp": 1.9584956412833767
+                },
+                {
+                    "x": 5.891572555778559,
+                    "y": 1.5225106097486452,
+                    "heading": -0.35143250880916643,
+                    "angularVelocity": -0.7901836047765775,
+                    "velocityX": -3.4642547433776456,
+                    "velocityY": -0.2818701811985366,
+                    "timestamp": 2.0146748591737533
+                },
+                {
+                    "x": 5.713348199791213,
+                    "y": 1.5270062964148978,
+                    "heading": -0.407385332387855,
+                    "angularVelocity": -0.9959701412692558,
+                    "velocityX": -3.172425011952234,
+                    "velocityY": 0.08002401662169539,
+                    "timestamp": 2.07085407706413
+                },
+                {
+                    "x": 5.558872974287929,
+                    "y": 1.5428236796222032,
+                    "heading": -0.4653888399418059,
+                    "angularVelocity": -1.0324726781908529,
+                    "velocityX": -2.7496862951120815,
+                    "velocityY": 0.2815522145247918,
+                    "timestamp": 2.1270332949545065
+                },
+                {
+                    "x": 5.4303297125098124,
+                    "y": 1.5629685460150702,
+                    "heading": -0.519265179295127,
+                    "angularVelocity": -0.9590083553396718,
+                    "velocityX": -2.2880927610801525,
+                    "velocityY": 0.35858217948453525,
+                    "timestamp": 2.183212512844883
+                },
+                {
+                    "x": 5.3280145107455015,
+                    "y": 1.583083454211334,
+                    "heading": -0.5654730488836437,
+                    "angularVelocity": -0.8225082392332849,
+                    "velocityX": -1.8212286608183241,
+                    "velocityY": 0.3580489182942135,
+                    "timestamp": 2.2393917307352598
+                },
+                {
+                    "x": 5.251723840078425,
+                    "y": 1.6004232571954151,
+                    "heading": -0.6018676415285119,
+                    "angularVelocity": -0.6478301765590008,
+                    "velocityX": -1.3579874112157035,
+                    "velocityY": 0.3086515554188953,
+                    "timestamp": 2.2955709486256364
+                },
+                {
+                    "x": 5.20115218978415,
+                    "y": 1.613158164546165,
+                    "heading": -0.6270407807035692,
+                    "angularVelocity": -0.44808632302748147,
+                    "velocityX": -0.9001843064628601,
+                    "velocityY": 0.22668359989631479,
+                    "timestamp": 2.351750166516013
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": -0.64,
+                    "angularVelocity": -0.2306763921441399,
+                    "velocityX": -0.44771342016952703,
+                    "velocityY": 0.12178587938311458,
+                    "timestamp": 2.4079293844063896
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": -0.64,
+                    "angularVelocity": 3.4924263364045203e-31,
+                    "velocityX": 0,
+                    "velocityY": 4.738614736798697e-32,
+                    "timestamp": 2.464108602296766
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -26798,64 +31020,98 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
-            "constraints": [
+            "trajectory": [
                 {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": -5.382102664363315e-41,
+                    "angularVelocity": -2.8652892976895567e-45,
+                    "velocityX": 1.592735858156421e-40,
+                    "velocityY": 0,
+                    "timestamp": 0
                 },
                 {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
+                    "x": 7.68,
+                    "y": 7.383499994424153,
+                    "heading": 1.659495816830368e-21,
+                    "angularVelocity": 1.658316243120447e-20,
+                    "velocityX": 2.780383744489643e-33,
+                    "velocityY": -0.8344065481228213,
+                    "timestamp": 0.10007112931184184
+                },
                 {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
+                    "x": 7.68,
+                    "y": 7.216499986370153,
+                    "heading": 4.9512387638524226e-21,
+                    "angularVelocity": 3.289403167370527e-20,
+                    "velocityX": 3.618624574631574e-33,
+                    "velocityY": -1.668813065290736,
+                    "timestamp": 0.20014225862368368
                 },
                 {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
+                    "x": 7.68,
+                    "y": 6.9659999820333836,
+                    "heading": 9.801760970151702e-21,
+                    "angularVelocity": 4.8470744390330394e-20,
+                    "velocityX": 3.439292531132709e-33,
+                    "velocityY": -2.503219520548835,
+                    "timestamp": 0.30021338793552554
                 },
                 {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C1 To C3": {
-            "waypoints": [
+                    "x": 7.68,
+                    "y": 6.632,
+                    "heading": 1.506109654033251e-20,
+                    "angularVelocity": 5.255597191401202e-20,
+                    "velocityX": 4.639675238422859e-33,
+                    "velocityY": -3.3376257900775017,
+                    "timestamp": 0.40028451724736736
+                },
                 {
                     "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 14
+                    "y": 6.298000017966616,
+                    "heading": 5.3586032763760104e-21,
+                    "angularVelocity": -9.69559666912617e-20,
+                    "velocityX": 4.935439546802769e-33,
+                    "velocityY": -3.3376257900775017,
+                    "timestamp": 0.5003556465592092
                 },
                 {
                     "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "y": 6.047500013629846,
+                    "heading": 1.8760590350448333e-21,
+                    "angularVelocity": -3.480068842301423e-20,
+                    "velocityX": 4.701730454770122e-33,
+                    "velocityY": -2.5032195205488352,
+                    "timestamp": 0.600426775871051
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.880500005575846,
+                    "heading": 3.168722428707802e-22,
+                    "angularVelocity": -1.5580785203142817e-20,
+                    "velocityX": 3.49539235617723e-33,
+                    "velocityY": -1.668813065290736,
+                    "timestamp": 0.7004979051828928
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 5.881496870004905e-41,
+                    "angularVelocity": -3.1664700918333573e-21,
+                    "velocityX": 3.364378386644386e-33,
+                    "velocityY": -0.8344065481228213,
+                    "timestamp": 0.8005690344947346
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 9.145026397391945e-42,
+                    "angularVelocity": -4.963805231001231e-40,
+                    "velocityX": -4.870998572760777e-42,
+                    "velocityY": 3.489748854003666e-39,
+                    "timestamp": 0.9006401638065764
                 }
             ],
-            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -26873,25 +31129,9 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
-        "C1 To C4": {
+        "C1 To C3": {
             "waypoints": [
                 {
                     "x": 7.68,
@@ -26900,11 +31140,11 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 18
+                    "controlIntervalCount": 14
                 },
                 {
                     "x": 7.68,
-                    "y": 2.457,
+                    "y": 4.127,
                     "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
@@ -26912,16 +31152,152 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
+            "trajectory": [
                 {
-                    "scope": [
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 0,
+                    "angularVelocity": 1.0127320738607452e-39,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.390554627994461,
+                    "heading": -1.4011773737891353e-18,
+                    "angularVelocity": -1.4633626501290335e-17,
+                    "velocityX": 9.653222160408549e-34,
+                    "velocityY": -0.7983807350946673,
+                    "timestamp": 0.09575052183150974
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.237663887017311,
+                    "heading": -4.2066729097937526e-18,
+                    "angularVelocity": -2.93000547907741e-17,
+                    "velocityX": 5.229262005564601e-34,
+                    "velocityY": -1.5967614385035824,
+                    "timestamp": 0.19150104366301948
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.0083277831364,
+                    "heading": -8.423669065336014e-18,
+                    "angularVelocity": -4.4041495279144365e-17,
+                    "velocityX": -1.080014219031917e-33,
+                    "velocityY": -2.3951420785410225,
+                    "timestamp": 0.28725156549452924
+                },
+                {
+                    "x": 7.68,
+                    "y": 6.702546334555254,
+                    "heading": -1.4072329853209785e-17,
+                    "angularVelocity": -5.899352482411367e-17,
+                    "velocityX": -1.5994734307756055e-33,
+                    "velocityY": -3.1935225284643685,
+                    "timestamp": 0.38300208732603896
+                },
+                {
+                    "x": 7.68,
+                    "y": 6.340327800733179,
+                    "heading": -3.9014262815434015e-17,
+                    "angularVelocity": -2.604887418380216e-16,
+                    "velocityX": 4.89416509473128e-33,
+                    "velocityY": -3.7829405719528495,
+                    "timestamp": 0.4787526091575487
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.978109266911059,
+                    "heading": -3.149786356993182e-17,
+                    "angularVelocity": 7.849982539759592e-17,
+                    "velocityX": -6.0692119772378415e-33,
+                    "velocityY": -3.782940571953311,
+                    "timestamp": 0.5745031309890585
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.61589073308894,
+                    "heading": -3.435394827038493e-17,
+                    "angularVelocity": -2.98283982773363e-17,
+                    "velocityX": 7.480170031140856e-33,
+                    "velocityY": -3.782940571953311,
+                    "timestamp": 0.6702536528205683
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.253672199266821,
+                    "heading": -8.183196831726821e-18,
+                    "angularVelocity": 2.7332228522691796e-16,
+                    "velocityX": -1.1202872015905622e-32,
+                    "velocityY": -3.782940571953311,
+                    "timestamp": 0.766004174652078
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.891453665444746,
+                    "heading": 1.4014689490668664e-17,
+                    "angularVelocity": 2.318304474774222e-16,
+                    "velocityX": 5.155427490211361e-33,
+                    "velocityY": -3.7829405719528495,
+                    "timestamp": 0.8617546964835878
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.5856722168636,
+                    "heading": 8.395813027039913e-18,
+                    "angularVelocity": -5.868246309956073e-17,
+                    "velocityX": -4.168254505442575e-33,
+                    "velocityY": -3.1935225284643685,
+                    "timestamp": 0.9575052183150976
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.356336112982689,
+                    "heading": 4.1945848336017725e-18,
+                    "angularVelocity": -4.387681773072387e-17,
+                    "velocityX": 2.1988846402360483e-33,
+                    "velocityY": -2.3951420785410225,
+                    "timestamp": 1.0532557401466074
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.203445372005539,
+                    "heading": 1.3975677775871127e-18,
+                    "angularVelocity": -2.9211507184809557e-17,
+                    "velocityX": 3.5559383909284404e-33,
+                    "velocityY": -1.5967614385035824,
+                    "timestamp": 1.1490062619781172
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 0,
+                    "angularVelocity": -1.4595928576161535e-17,
+                    "velocityX": -6.471812078129625e-34,
+                    "velocityY": -0.7983807350946673,
+                    "timestamp": 1.244756783809627
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": 8.546727720642512e-43,
+                    "velocityY": 6.233759417908357e-39,
+                    "timestamp": 1.3405073056411367
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
                         "last"
                     ],
                     "type": "StopPoint"
@@ -26930,23 +31306,220 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
+            "circleObstacles": []
+        },
+        "C1 To C4": {
+            "waypoints": [
                 {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 18
                 },
                 {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 3.5626996813915863e-32,
+                    "timestamp": 0
                 },
                 {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
+                    "x": 7.68,
+                    "y": 7.385155846048237,
+                    "heading": 1.781123128781041e-18,
+                    "angularVelocity": 1.7977717932873702e-17,
+                    "velocityX": 2.837123063321612e-19,
+                    "velocityY": -0.8260917454276266,
+                    "timestamp": 0.099073927811004
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.22146754125807,
+                    "heading": 5.344190150368438e-18,
+                    "angularVelocity": 3.5963720225105045e-17,
+                    "velocityX": 5.673750850451309e-19,
+                    "velocityY": -1.65218345943065,
+                    "timestamp": 0.198147855622008
+                },
+                {
+                    "x": 7.68,
+                    "y": 6.975935091856216,
+                    "heading": 1.0690981078424305e-17,
+                    "angularVelocity": 5.396768903659916e-17,
+                    "velocityX": 8.509388117154945e-19,
+                    "velocityY": -2.47827511058448,
+                    "timestamp": 0.29722178343301203
+                },
+                {
+                    "x": 7.68,
+                    "y": 6.648558516522808,
+                    "heading": 1.7824905328944302e-17,
+                    "angularVelocity": 7.200607069613354e-17,
+                    "velocityX": 1.134205375948437e-18,
+                    "velocityY": -3.304366573190878,
+                    "timestamp": 0.396295711244016
+                },
+                {
+                    "x": 7.68,
+                    "y": 6.273767735073314,
+                    "heading": 3.2118763957552995e-17,
+                    "angularVelocity": 1.4427467391699638e-16,
+                    "velocityX": 1.2981920193437937e-18,
+                    "velocityY": -3.782940575087071,
+                    "timestamp": 0.49536963905502
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.898976953623795,
+                    "heading": 3.194072142377472e-17,
+                    "angularVelocity": -1.7970674806511035e-18,
+                    "velocityX": 1.2970624694923187e-18,
+                    "velocityY": -3.7829405750873115,
+                    "timestamp": 0.5944435668660241
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.524186172174278,
+                    "heading": 3.523836351949418e-17,
+                    "angularVelocity": 3.3284660945411024e-17,
+                    "velocityX": 1.2988038949897902e-18,
+                    "velocityY": -3.782940575087312,
+                    "timestamp": 0.6935174946770281
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.149395390724759,
+                    "heading": 4.1591047407718544e-17,
+                    "angularVelocity": 6.412064231794501e-17,
+                    "velocityX": 1.299093645390414e-18,
+                    "velocityY": -3.782940575087312,
+                    "timestamp": 0.792591422488032
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.774604609275242,
+                    "heading": 3.0405238206332753e-17,
+                    "angularVelocity": -1.1290366142265185e-16,
+                    "velocityX": 1.2929821768433727e-18,
+                    "velocityY": -3.782940575087312,
+                    "timestamp": 0.891665350299036
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.3998138278257235,
+                    "heading": 3.8390344063969795e-17,
+                    "angularVelocity": 8.059744913789504e-17,
+                    "velocityX": 1.304793755296235e-18,
+                    "velocityY": -3.782940575087312,
+                    "timestamp": 0.99073927811004
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.025023046376205,
+                    "heading": 1.0979594277826173e-17,
+                    "angularVelocity": -2.766696586253564e-16,
+                    "velocityX": 1.307186742413441e-18,
+                    "velocityY": -3.782940575087312,
+                    "timestamp": 1.0898132059210441
+                },
+                {
+                    "x": 7.68,
+                    "y": 3.650232264926687,
+                    "heading": -2.14668342522941e-17,
+                    "angularVelocity": -3.2749714528425564e-16,
+                    "velocityX": 1.3056923664932481e-18,
+                    "velocityY": -3.7829405750873115,
+                    "timestamp": 1.1888871337320481
+                },
+                {
+                    "x": 7.68,
+                    "y": 3.2754414834771928,
+                    "heading": -1.7689075007663427e-17,
+                    "angularVelocity": 3.813071035032691e-17,
+                    "velocityX": 1.3012288375156972e-18,
+                    "velocityY": -3.782940575087071,
+                    "timestamp": 1.2879610615430521
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.948064908143785,
+                    "heading": -1.0623620375651477e-17,
+                    "angularVelocity": 7.131497447110055e-17,
+                    "velocityX": 1.136256754825581e-18,
+                    "velocityY": -3.304366573190878,
+                    "timestamp": 1.3870349893540561
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.70253245874193,
+                    "heading": -5.31498515459649e-18,
+                    "angularVelocity": 5.35825654501949e-17,
+                    "velocityX": 8.521926280380639e-19,
+                    "velocityY": -2.4782751105844802,
+                    "timestamp": 1.48610891716506
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.538844153951763,
+                    "heading": -1.7723662552803497e-18,
+                    "angularVelocity": 3.575732766130284e-17,
+                    "velocityX": 5.681284366395801e-19,
+                    "velocityY": -1.6521834594306501,
+                    "timestamp": 1.585182844976064
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 0,
+                    "angularVelocity": 1.7889330669293064e-17,
+                    "velocityX": 2.8406422370407584e-19,
+                    "velocityY": -0.8260917454276266,
+                    "timestamp": 1.684256772787068
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": 1.0461754707969188e-34,
+                    "velocityY": 0,
+                    "timestamp": 1.783330700598072
                 }
-            ]
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
         },
         "C1 To C5": {
             "waypoints": [
@@ -26969,7 +31542,224 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": -8.452820466081124e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.3890447295128565,
+                    "heading": -6.111572142469688e-20,
+                    "angularVelocity": -6.320692328199756e-19,
+                    "velocityX": -1.9797111149811777e-21,
+                    "velocityY": -0.8062267267095136,
+                    "timestamp": 0.09669149868710691
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.2331341916331855,
+                    "heading": -1.8226742072400974e-19,
+                    "angularVelocity": -1.2529715733337834e-18,
+                    "velocityX": -3.753564400558873e-21,
+                    "velocityY": -1.6124534214139752,
+                    "timestamp": 0.19338299737421383
+                },
+                {
+                    "x": 7.68,
+                    "y": 6.999268392550218,
+                    "heading": -3.611632829024713e-19,
+                    "angularVelocity": -1.850171571559818e-18,
+                    "velocityX": -5.1157697830729356e-21,
+                    "velocityY": -2.418680052108357,
+                    "timestamp": 0.29007449606132074
+                },
+                {
+                    "x": 7.68,
+                    "y": 6.687447350831621,
+                    "heading": -5.912144198792796e-19,
+                    "angularVelocity": -2.3792281637439373e-18,
+                    "velocityX": -5.242727749103024e-21,
+                    "velocityY": -3.224906490772764,
+                    "timestamp": 0.38676599474842766
+                },
+                {
+                    "x": 7.68,
+                    "y": 6.321669157855709,
+                    "heading": -4.100561450064452e-17,
+                    "angularVelocity": -4.179726307846966e-16,
+                    "velocityX": -1.166132844762965e-20,
+                    "velocityY": -3.782940568121389,
+                    "timestamp": 0.48345749343553457
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.955890964879757,
+                    "heading": -6.040572418063157e-17,
+                    "angularVelocity": -2.0063924898701218e-16,
+                    "velocityX": -1.9392305823460385e-22,
+                    "velocityY": -3.782940568121787,
+                    "timestamp": 0.5801489921226415
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.590112771903805,
+                    "heading": -8.327536807485113e-17,
+                    "angularVelocity": -2.365217646302656e-16,
+                    "velocityX": 3.675624932403311e-22,
+                    "velocityY": -3.782940568121787,
+                    "timestamp": 0.6768404908097484
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.224334578927854,
+                    "heading": -7.575439540555723e-17,
+                    "angularVelocity": 7.778318436801094e-17,
+                    "velocityX": -2.1057162419585226e-21,
+                    "velocityY": -3.782940568121787,
+                    "timestamp": 0.7735319894968553
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.858556385951902,
+                    "heading": -4.8431591576069396e-17,
+                    "angularVelocity": 2.82577105541659e-16,
+                    "velocityX": 1.016892835365518e-21,
+                    "velocityY": -3.782940568121787,
+                    "timestamp": 0.8702234881839622
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.49277819297595,
+                    "heading": -7.892411277004063e-17,
+                    "angularVelocity": -3.1535886409915893e-16,
+                    "velocityX": 1.448731313682566e-20,
+                    "velocityY": -3.782940568121787,
+                    "timestamp": 0.9669149868710691
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.126999999999999,
+                    "heading": -8.668140475632877e-17,
+                    "angularVelocity": -8.022723912254832e-17,
+                    "velocityX": 2.655349226445793e-21,
+                    "velocityY": -3.782940568121787,
+                    "timestamp": 1.063606485558176
+                },
+                {
+                    "x": 7.68,
+                    "y": 3.7612218070240475,
+                    "heading": -4.236036305527668e-17,
+                    "angularVelocity": 4.583757859030679e-16,
+                    "velocityX": 7.500561292003624e-21,
+                    "velocityY": -3.782940568121787,
+                    "timestamp": 1.160297984245283
+                },
+                {
+                    "x": 7.68,
+                    "y": 3.395443614048096,
+                    "heading": -2.060715967072721e-17,
+                    "angularVelocity": 2.249753461257506e-16,
+                    "velocityX": 1.3311646441840309e-21,
+                    "velocityY": -3.782940568121787,
+                    "timestamp": 1.25698948293239
+                },
+                {
+                    "x": 7.68,
+                    "y": 3.0296654210721448,
+                    "heading": -1.7378554757194933e-17,
+                    "angularVelocity": 3.3390783650741015e-17,
+                    "velocityX": 1.5677048108756238e-21,
+                    "velocityY": -3.782940568121787,
+                    "timestamp": 1.353680981619497
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.663887228096194,
+                    "heading": -2.7058170522872628e-18,
+                    "angularVelocity": 1.5174796030816139e-16,
+                    "velocityX": 2.708497475030379e-21,
+                    "velocityY": -3.782940568121787,
+                    "timestamp": 1.450372480306604
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.2981090351202424,
+                    "heading": -9.881529062229235e-18,
+                    "angularVelocity": -7.421243963841227e-17,
+                    "velocityX": 2.3345387345088647e-21,
+                    "velocityY": -3.782940568121787,
+                    "timestamp": 1.547063978993711
+                },
+                {
+                    "x": 7.68,
+                    "y": 1.9323308421442913,
+                    "heading": -6.5997498705085955e-18,
+                    "angularVelocity": 3.39407211211041e-17,
+                    "velocityX": -6.072343928076645e-20,
+                    "velocityY": -3.782940568121787,
+                    "timestamp": 1.643755477680818
+                },
+                {
+                    "x": 7.68,
+                    "y": 1.5665526491683786,
+                    "heading": 6.487893960505722e-19,
+                    "angularVelocity": 7.496563157826373e-17,
+                    "velocityX": 5.228734565126193e-20,
+                    "velocityY": -3.782940568121389,
+                    "timestamp": 1.7404469763679251
+                },
+                {
+                    "x": 7.68,
+                    "y": 1.254731607449781,
+                    "heading": 3.9009642634108723e-19,
+                    "angularVelocity": -2.6754468948553036e-18,
+                    "velocityX": 1.8077268140623735e-21,
+                    "velocityY": -3.224906490772764,
+                    "timestamp": 1.8371384750550321
+                },
+                {
+                    "x": 7.68,
+                    "y": 1.020865808366814,
+                    "heading": 1.9475770164363563e-19,
+                    "angularVelocity": -2.020226466917104e-18,
+                    "velocityX": 1.3557598034014801e-21,
+                    "velocityY": -2.418680052108357,
+                    "timestamp": 1.9338299737421392
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.8649552704871435,
+                    "heading": 6.488190339479676e-20,
+                    "angularVelocity": -1.3431976959152128e-18,
+                    "velocityX": 9.038296351119003e-22,
+                    "velocityY": -1.6124534214139752,
+                    "timestamp": 2.030521472429246
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 0,
+                    "angularVelocity": -6.710197305720325e-19,
+                    "velocityX": 4.519117474728098e-22,
+                    "velocityY": -0.8062267267095136,
+                    "timestamp": 2.127212971116353
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": 1.0656726462878492e-32,
+                    "velocityY": 0,
+                    "timestamp": 2.2239044698034602
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -26987,23 +31777,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "C2 To W1": {
             "waypoints": [
@@ -27031,199 +31805,199 @@
                     "x": 7.68,
                     "y": 5.797,
                     "heading": 0,
-                    "angularVelocity": 5.4178362746118835e-30,
-                    "velocityX": -4.378571579060787e-32,
-                    "velocityY": 1.8329583672175558e-31,
+                    "angularVelocity": 0,
+                    "velocityX": 2.477402495362019e-32,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.617836111630202,
-                    "y": 5.830159453299192,
-                    "heading": -0.009839435811264896,
-                    "angularVelocity": -0.10699973937901981,
-                    "velocityX": -0.6760062245377694,
-                    "velocityY": 0.3605951529154081,
-                    "timestamp": 0.09195756801248979
-                },
-                {
-                    "x": 7.493508339255607,
-                    "y": 5.896478344435856,
-                    "heading": -0.0295193862966414,
-                    "angularVelocity": -0.21401121093919698,
-                    "velocityX": -1.352012401607965,
-                    "velocityY": 0.721190137691066,
-                    "timestamp": 0.18391513602497958
-                },
-                {
-                    "x": 7.3070166954300815,
-                    "y": 5.995956653040542,
-                    "heading": -0.059041701653073396,
-                    "angularVelocity": -0.32104280261546536,
-                    "velocityX": -2.0280184421601457,
-                    "velocityY": 1.0817849009575307,
-                    "timestamp": 0.2758727040374694
-                },
-                {
-                    "x": 7.0583612095128645,
-                    "y": 6.128594352415652,
-                    "heading": -0.09840889768284368,
-                    "angularVelocity": -0.42810175258683864,
-                    "velocityX": -2.7040241634429143,
-                    "velocityY": 1.4423793738987822,
-                    "timestamp": 0.36783027204995916
-                },
-                {
-                    "x": 6.751428460841339,
-                    "y": 6.292319222162274,
-                    "heading": -0.09840890267081659,
-                    "angularVelocity": -5.4242114104221296e-8,
-                    "velocityX": -3.3377649638345908,
-                    "velocityY": 1.780439318756071,
-                    "timestamp": 0.45978784006244894
-                },
-                {
-                    "x": 6.443018724200086,
-                    "y": 6.45324460704626,
-                    "heading": -0.09840890766830068,
-                    "angularVelocity": -5.4345544379257814e-8,
-                    "velocityX": -3.3538265887954406,
-                    "velocityY": 1.7499960945262094,
-                    "timestamp": 0.5517454080749388
-                },
-                {
-                    "x": 6.108495580653603,
-                    "y": 6.54867946433224,
-                    "heading": -0.0984088906849307,
-                    "angularVelocity": 1.846870285822299e-7,
-                    "velocityX": -3.637798941149115,
-                    "velocityY": 1.037814063036315,
-                    "timestamp": 0.6437029760874285
-                },
-                {
-                    "x": 5.762312734581719,
-                    "y": 6.58289903236131,
-                    "heading": -0.09840883650367563,
-                    "angularVelocity": 5.891984340231216e-7,
-                    "velocityX": -3.7645933179188145,
-                    "velocityY": 0.37212345616210646,
-                    "timestamp": 0.7356605440999183
-                },
-                {
-                    "x": 5.416121743024081,
-                    "y": 6.617036096003367,
-                    "heading": -0.09840878231628362,
-                    "angularVelocity": 5.892651705038846e-7,
-                    "velocityX": -3.7646818966614886,
-                    "velocityY": 0.37122625554234295,
-                    "timestamp": 0.8276181121124081
-                },
-                {
-                    "x": 5.0685946034380285,
-                    "y": 6.632477283287523,
-                    "heading": -0.09840873884834453,
-                    "angularVelocity": 4.7269561430407727e-7,
-                    "velocityX": -3.7792119463060407,
-                    "velocityY": 0.16791643817786392,
-                    "timestamp": 0.9195756801248979
-                },
-                {
-                    "x": 4.72106743655186,
-                    "y": 6.647917856129691,
-                    "heading": -0.09840869538060193,
-                    "angularVelocity": 4.726934774100163e-7,
-                    "velocityX": -3.779212243183364,
-                    "velocityY": 0.1679097563788475,
-                    "timestamp": 1.0115332481373878
-                },
-                {
-                    "x": 4.373540264440576,
-                    "y": 6.66335831136751,
-                    "heading": -0.09840865191285005,
-                    "angularVelocity": 4.7269357834441634e-7,
-                    "velocityX": -3.779212300004299,
-                    "velocityY": 0.16790847748084475,
-                    "timestamp": 1.1034908161498775
-                },
-                {
-                    "x": 4.026013088497698,
-                    "y": 6.678798680365008,
-                    "heading": -0.09840860844507962,
-                    "angularVelocity": 4.726937801388588e-7,
-                    "velocityX": -3.7792123416712937,
-                    "velocityY": 0.16790753965352348,
-                    "timestamp": 1.1954483841623673
-                },
-                {
-                    "x": 3.6784859092773967,
-                    "y": 6.694238975594936,
-                    "heading": -0.09840856497728971,
-                    "angularVelocity": 4.7269399197494456e-7,
-                    "velocityX": -3.7792123773118855,
-                    "velocityY": 0.1679067374621128,
-                    "timestamp": 1.287405952174857
-                },
-                {
-                    "x": 3.3309587272270753,
-                    "y": 6.709679207126903,
-                    "heading": -0.09840852150946863,
-                    "angularVelocity": 4.72694330698846e-7,
-                    "velocityX": -3.779212408087172,
-                    "velocityY": 0.16790604477348844,
-                    "timestamp": 1.3793635201873469
-                },
-                {
-                    "x": 2.991492172408929,
-                    "y": 6.7253546024137885,
-                    "heading": -0.07153351873489558,
-                    "angularVelocity": 0.29225438814261445,
-                    "velocityX": -3.691556466261044,
-                    "velocityY": 0.17046335201859075,
-                    "timestamp": 1.4713210881998366
-                },
-                {
-                    "x": 2.7138316831246163,
-                    "y": 6.738400020288083,
-                    "heading": 0.08699256852088375,
-                    "angularVelocity": 1.723904738696962,
-                    "velocityX": -3.019441415051349,
-                    "velocityY": 0.1418634502439354,
-                    "timestamp": 1.5632786562123264
-                },
-                {
-                    "x": 2.5064819973568118,
-                    "y": 6.747857404358727,
-                    "heading": 0.23167973935786532,
-                    "angularVelocity": 1.5734123244465315,
-                    "velocityX": -2.254840903792083,
-                    "velocityY": 0.10284508687050122,
-                    "timestamp": 1.6552362242248162
-                },
-                {
-                    "x": 2.36871059980762,
-                    "y": 6.753996751301636,
-                    "heading": 0.3372042620417382,
-                    "angularVelocity": 1.1475349442640808,
-                    "velocityX": -1.4982061892989513,
-                    "velocityY": 0.0667628241546722,
-                    "timestamp": 1.747193792237306
+                    "x": 7.61783610738437,
+                    "y": 5.830159445563511,
+                    "heading": -0.009839426193567341,
+                    "angularVelocity": -0.10699963479066096,
+                    "velocityX": -0.6760062707098423,
+                    "velocityY": 0.3605950687933442,
+                    "timestamp": 0.09195756801242966
+                },
+                {
+                    "x": 7.493508328314676,
+                    "y": 5.8964783245931205,
+                    "heading": -0.029519357600464063,
+                    "angularVelocity": -0.21401100346886773,
+                    "velocityX": -1.3520124744152573,
+                    "velocityY": 0.7211900060324206,
+                    "timestamp": 0.18391513602485932
+                },
+                {
+                    "x": 7.307016677169599,
+                    "y": 5.995956620127639,
+                    "heading": -0.05904164496194892,
+                    "angularVelocity": -0.32104249818236175,
+                    "velocityX": -2.028018521758536,
+                    "velocityY": 1.0817847588256284,
+                    "timestamp": 0.275872704037289
+                },
+                {
+                    "x": 7.058361185168299,
+                    "y": 6.1285943089313815,
+                    "heading": -0.09840880514573262,
+                    "angularVelocity": -0.42810136277704247,
+                    "velocityX": -2.7040242296065236,
+                    "velocityY": 1.4423792589405482,
+                    "timestamp": 0.36783027204971863
+                },
+                {
+                    "x": 6.751428461707863,
+                    "y": 6.292319225940392,
+                    "heading": -0.09840881013370367,
+                    "angularVelocity": -5.424209394751822e-8,
+                    "velocityX": -3.3377646896767494,
+                    "velocityY": 1.7804398327158815,
+                    "timestamp": 0.4597878400621483
+                },
+                {
+                    "x": 6.443018732830252,
+                    "y": 6.45324462570272,
+                    "heading": -0.09840881513118924,
+                    "angularVelocity": -5.434556026601013e-8,
+                    "velocityX": -3.3538265043712756,
+                    "velocityY": 1.7499962563230937,
+                    "timestamp": 0.551745408074578
+                },
+                {
+                    "x": 6.108495593860041,
+                    "y": 6.548679499028843,
+                    "heading": -0.09840879814777771,
+                    "angularVelocity": 1.8468748020115896e-7,
+                    "velocityX": -3.637798891386455,
+                    "velocityY": 1.0378142374668238,
+                    "timestamp": 0.6437029760870077
+                },
+                {
+                    "x": 5.7623127419169835,
+                    "y": 6.582899007638951,
+                    "heading": -0.09840874396641834,
+                    "angularVelocity": 5.891995683553032e-7,
+                    "velocityX": -3.764593381767812,
+                    "velocityY": 0.37212281000605224,
+                    "timestamp": 0.7356605440994373
+                },
+                {
+                    "x": 5.416121749902788,
+                    "y": 6.617036066627776,
+                    "heading": -0.09840868977891702,
+                    "angularVelocity": 5.892663593006056e-7,
+                    "velocityX": -3.7646819016288298,
+                    "velocityY": 0.37122620494064046,
+                    "timestamp": 0.8276181121118669
+                },
+                {
+                    "x": 5.068594601971295,
+                    "y": 6.632477066079001,
+                    "heading": -0.0984086463109951,
+                    "angularVelocity": 4.72695427552955e-7,
+                    "velocityX": -3.7792120370616824,
+                    "velocityY": 0.1679143955735957,
+                    "timestamp": 0.9195756801242965
+                },
+                {
+                    "x": 4.721067428734796,
+                    "y": 6.647917495985935,
+                    "heading": -0.09840860284326601,
+                    "angularVelocity": 4.7269333042542933e-7,
+                    "velocityX": -3.7792123122430223,
+                    "velocityY": 0.16790820201819795,
+                    "timestamp": 1.011533248136726
+                },
+                {
+                    "x": 4.3735402523552525,
+                    "y": 6.663357855150138,
+                    "heading": -0.09840855937552709,
+                    "angularVelocity": 4.726934374928119e-7,
+                    "velocityX": -3.779212346422309,
+                    "velocityY": 0.16790743272066663,
+                    "timestamp": 1.1034908161491557
+                },
+                {
+                    "x": 4.026013074283842,
+                    "y": 6.678798176234082,
+                    "heading": -0.09840851590777203,
+                    "angularVelocity": 4.7269361285314424e-7,
+                    "velocityX": -3.779212364820652,
+                    "velocityY": 0.1679070186138197,
+                    "timestamp": 1.1954483841615853
+                },
+                {
+                    "x": 3.6784858950907164,
+                    "y": 6.69423847207055,
+                    "heading": -0.09840847244000124,
+                    "angularVelocity": 4.7269378404370134e-7,
+                    "velocityX": -3.7792123770188395,
+                    "velocityY": 0.16790674405809006,
+                    "timestamp": 1.2874059521740149
+                },
+                {
+                    "x": 3.330958715199463,
+                    "y": 6.709678752193432,
+                    "heading": -0.09840842897219951,
+                    "angularVelocity": 4.7269412044437995e-7,
+                    "velocityX": -3.7792123846106827,
+                    "velocityY": 0.16790657317944163,
+                    "timestamp": 1.3793635201864445
+                },
+                {
+                    "x": 2.991492162018093,
+                    "y": 6.7253542779205,
+                    "heading": -0.07153342912076269,
+                    "angularVelocity": 0.2922543563549246,
+                    "velocityX": -3.6915564484642,
+                    "velocityY": 0.17046477050097905,
+                    "timestamp": 1.471321088198874
+                },
+                {
+                    "x": 2.713831676976805,
+                    "y": 6.738399816428541,
+                    "heading": 0.08699262812295999,
+                    "angularVelocity": 1.7239044123295555,
+                    "velocityX": -3.0194413689122035,
+                    "velocityY": 0.14186476208546492,
+                    "timestamp": 1.5632786562113037
+                },
+                {
+                    "x": 2.5064819943185284,
+                    "y": 6.7478572981535265,
+                    "heading": 0.2316797715135602,
+                    "angularVelocity": 1.5734120259796696,
+                    "velocityX": -2.2548408699787488,
+                    "velocityY": 0.10284614882059591,
+                    "timestamp": 1.6552362242237333
+                },
+                {
+                    "x": 2.368710598807467,
+                    "y": 6.7539967145328434,
+                    "heading": 0.3372042735506754,
+                    "angularVelocity": 1.147534719739997,
+                    "velocityX": -1.4982061671361233,
+                    "velocityY": 0.06676357924654308,
+                    "timestamp": 1.747193792236163
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.393,
-                    "angularVelocity": 0.6067552585849546,
-                    "velocityX": -0.7471989667918153,
-                    "velocityY": 0.03265907051778923,
-                    "timestamp": 1.8391513602497958
+                    "angularVelocity": 0.6067551334304849,
+                    "velocityX": -0.7471989559160562,
+                    "velocityY": 0.03265947036300673,
+                    "timestamp": 1.8391513602485925
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.393,
-                    "angularVelocity": -8.637511489261282e-30,
-                    "velocityX": 0,
-                    "velocityY": 5.8539743527830045e-30,
-                    "timestamp": 1.9311089282622855
+                    "angularVelocity": 0,
+                    "velocityX": -4.676461204476467e-32,
+                    "velocityY": 0,
+                    "timestamp": 1.931108928261022
                 }
             ],
             "constraints": [
@@ -27244,16 +32018,6 @@
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
             "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
                 {
                     "x": 5.61,
                     "y": 5.44,
@@ -27282,7 +32046,188 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": -7.048442713490885e-38,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.598761685357721,
+                    "y": 5.839328892781844,
+                    "heading": -0.010877614975927053,
+                    "angularVelocity": -0.10375029848266179,
+                    "velocityX": -0.774848108801196,
+                    "velocityY": 0.4037314494285639,
+                    "timestamp": 0.10484418006512992
+                },
+                {
+                    "x": 7.436285061134455,
+                    "y": 5.923986667743732,
+                    "heading": -0.03263383242489801,
+                    "angularVelocity": -0.20751001567713226,
+                    "velocityX": -1.5496961693280003,
+                    "velocityY": 0.8074627977375359,
+                    "timestamp": 0.20968836013025985
+                },
+                {
+                    "x": 7.192570139480125,
+                    "y": 6.050973310343331,
+                    "heading": -0.06527034306657675,
+                    "angularVelocity": -0.31128585889464805,
+                    "velocityX": -2.3245441139692504,
+                    "velocityY": 1.2111940073422762,
+                    "timestamp": 0.3145325401953898
+                },
+                {
+                    "x": 6.867616948374112,
+                    "y": 6.220288799737083,
+                    "heading": -0.10878948434468085,
+                    "angularVelocity": -0.41508399656585776,
+                    "velocityX": -3.0993917917441864,
+                    "velocityY": 1.6149250181418864,
+                    "timestamp": 0.4193767202605197
+                },
+                {
+                    "x": 6.5158804421759,
+                    "y": 6.403559919066418,
+                    "heading": -0.10878948853448296,
+                    "angularVelocity": -3.996218113987742e-8,
+                    "velocityX": -3.3548500830442913,
+                    "velocityY": 1.7480333120587623,
+                    "timestamp": 0.5242209003256496
+                },
+                {
+                    "x": 6.14377891604402,
+                    "y": 6.540845489176542,
+                    "heading": -0.10878948531130445,
+                    "angularVelocity": 3.074256021150283e-8,
+                    "velocityX": -3.549090907103546,
+                    "velocityY": 1.309424805695852,
+                    "timestamp": 0.6290650803907796
+                },
+                {
+                    "x": 5.750100268474419,
+                    "y": 6.5890532439035745,
+                    "heading": -0.10878941324219087,
+                    "angularVelocity": 6.873926003335433e-7,
+                    "velocityX": -3.754892711498581,
+                    "velocityY": 0.4598038221776824,
+                    "timestamp": 0.7339092604559095
+                },
+                {
+                    "x": 5.35529697720587,
+                    "y": 6.626964014921626,
+                    "heading": -0.10878934177449004,
+                    "angularVelocity": 6.816563474487017e-7,
+                    "velocityX": -3.7656195224503146,
+                    "velocityY": 0.3615915637329853,
+                    "timestamp": 0.8387534405210395
+                },
+                {
+                    "x": 4.9606240931120595,
+                    "y": 6.587718813133039,
+                    "heading": -0.10878931155021784,
+                    "angularVelocity": 2.882780157548741e-7,
+                    "velocityX": -3.7643757035310585,
+                    "velocityY": -0.3743193161910223,
+                    "timestamp": 0.9435976205861695
+                },
+                {
+                    "x": 4.585497747567555,
+                    "y": 6.458927912145357,
+                    "heading": -0.10878930710495487,
+                    "angularVelocity": 4.239875765382173e-8,
+                    "velocityX": -3.5779415253328617,
+                    "velocityY": -1.2284029586351546,
+                    "timestamp": 1.0484418006512994
+                },
+                {
+                    "x": 4.215349929173217,
+                    "y": 6.316458743577884,
+                    "heading": -0.10878930270384918,
+                    "angularVelocity": 4.197758707259389e-8,
+                    "velocityX": -3.530456513317188,
+                    "velocityY": -1.3588657804273945,
+                    "timestamp": 1.1532859807164293
+                },
+                {
+                    "x": 3.8452021463353314,
+                    "y": 6.173989482631574,
+                    "heading": -0.10878929830273087,
+                    "angularVelocity": 4.197770719831182e-8,
+                    "velocityX": -3.530456174181034,
+                    "velocityY": -1.3588666615333942,
+                    "timestamp": 1.2581301607815591
+                },
+                {
+                    "x": 3.475054376591526,
+                    "y": 6.0315201876656985,
+                    "heading": -0.10878929390160265,
+                    "angularVelocity": 4.197780205439048e-8,
+                    "velocityX": -3.530456049290165,
+                    "velocityY": -1.3588669860107905,
+                    "timestamp": 1.362974340846689
+                },
+                {
+                    "x": 3.104906616234201,
+                    "y": 5.889050868312964,
+                    "heading": -0.10878928950045777,
+                    "angularVelocity": 4.19779608468647e-8,
+                    "velocityX": -3.530455959762256,
+                    "velocityY": -1.358867218611777,
+                    "timestamp": 1.4678185209118189
+                },
+                {
+                    "x": 2.7629439773689555,
+                    "y": 5.757430528072475,
+                    "heading": -0.06527490613894259,
+                    "angularVelocity": 0.4150386157294016,
+                    "velocityX": -3.2616272896866185,
+                    "velocityY": -1.2553900479618894,
+                    "timestamp": 1.5726627009769487
+                },
+                {
+                    "x": 2.506471990883214,
+                    "y": 5.658715266844828,
+                    "heading": -0.03263773447849684,
+                    "angularVelocity": 0.311292163667756,
+                    "velocityX": -2.446220537243172,
+                    "velocityY": -0.9415425936501655,
+                    "timestamp": 1.6775068810420786
+                },
+                {
+                    "x": 2.335490664707938,
+                    "y": 5.592905089907167,
+                    "heading": -0.010879217132893882,
+                    "angularVelocity": 0.20753195200808147,
+                    "velocityX": -1.6308137091544919,
+                    "velocityY": -0.6276950890052175,
+                    "timestamp": 1.7823510611072084
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 0,
+                    "angularVelocity": 0.10376557979790346,
+                    "velocityX": -0.8154068700316104,
+                    "velocityY": -0.3138475582214142,
+                    "timestamp": 1.8871952411723383
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": 2.158258486540926e-34,
+                    "velocityY": 0,
+                    "timestamp": 1.9920394212374681
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -27301,16 +32246,6 @@
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
             "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
                 {
                     "x": 5.61,
                     "y": 5.44,
@@ -27329,6 +32264,15 @@
                     "headingConstrained": true,
                     "controlIntervalCount": 22
                 },
+                {
+                    "x": 3.4425296783447266,
+                    "y": 2.5693652629852295,
+                    "heading": 0.5090709736575311,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 15
+                },
                 {
                     "x": 2.3,
                     "y": 4.36,
@@ -27339,7 +32283,350 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 0,
+                    "angularVelocity": -2.981242670977675e-31,
+                    "velocityX": -5.763030958279131e-32,
+                    "velocityY": 3.5928963704249306e-32,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.6464505983184665,
+                    "y": 5.761517456765686,
+                    "heading": 0.04441523610418216,
+                    "angularVelocity": 0.571175712894524,
+                    "velocityX": -0.43144211544178024,
+                    "velocityY": -0.4563021319898165,
+                    "timestamp": 0.07776107264628128
+                },
+                {
+                    "x": 7.57910774134765,
+                    "y": 5.690410071738714,
+                    "heading": 0.1294441379757556,
+                    "angularVelocity": 1.0934635927458562,
+                    "velocityX": -0.8660227370723712,
+                    "velocityY": -0.914434209908928,
+                    "timestamp": 0.15552214529256256
+                },
+                {
+                    "x": 7.4776816969238515,
+                    "y": 5.583411802725407,
+                    "heading": 0.24873158333085876,
+                    "angularVelocity": 1.5340252043296483,
+                    "velocityX": -1.3043292867777738,
+                    "velocityY": -1.3759875651409672,
+                    "timestamp": 0.23328321793884382
+                },
+                {
+                    "x": 7.341761998810456,
+                    "y": 5.440105324158156,
+                    "heading": 0.38885380810835574,
+                    "angularVelocity": 1.8019584865410045,
+                    "velocityX": -1.747914393255167,
+                    "velocityY": -1.842907687489359,
+                    "timestamp": 0.3110442905851251
+                },
+                {
+                    "x": 7.171108690296084,
+                    "y": 5.260470754663091,
+                    "heading": 0.5147184350397563,
+                    "angularVelocity": 1.6186071339824826,
+                    "velocityX": -2.1945853202236263,
+                    "velocityY": -2.3100834824152265,
+                    "timestamp": 0.3888053632314064
+                },
+                {
+                    "x": 6.970033766909185,
+                    "y": 5.052418524467747,
+                    "heading": 0.5282282166004705,
+                    "angularVelocity": 0.17373450623768458,
+                    "velocityX": -2.585804394720051,
+                    "velocityY": -2.67553189681076,
+                    "timestamp": 0.4665664358776877
+                },
+                {
+                    "x": 6.768438732548067,
+                    "y": 4.838192511419719,
+                    "heading": 0.5282282675213732,
+                    "angularVelocity": 6.548379670470849e-7,
+                    "velocityX": -2.592492972391604,
+                    "velocityY": -2.7549261572367425,
+                    "timestamp": 0.5443275085239689
+                },
+                {
+                    "x": 6.5423880710685225,
+                    "y": 4.649952125007829,
+                    "heading": 0.5282283002658353,
+                    "angularVelocity": 4.210906690145718e-7,
+                    "velocityX": -2.9069900116707847,
+                    "velocityY": -2.42075346964613,
+                    "timestamp": 0.6220885811702502
+                },
+                {
+                    "x": 6.292219183977312,
+                    "y": 4.49519788912002,
+                    "heading": 0.5282283052888641,
+                    "angularVelocity": 6.459567356551782e-8,
+                    "velocityX": -3.217148099656167,
+                    "velocityY": -1.9901247580747212,
+                    "timestamp": 0.6998496538165315
+                },
+                {
+                    "x": 6.0420493950115075,
+                    "y": 4.3404451111724525,
+                    "heading": 0.5282283103117955,
+                    "angularVelocity": 6.459441933764687e-8,
+                    "velocityX": -3.217159697677684,
+                    "velocityY": -1.990106009101862,
+                    "timestamp": 0.7776107264628128
+                },
+                {
+                    "x": 5.780250458218128,
+                    "y": 4.206300927357815,
+                    "heading": 0.5282283154384368,
+                    "angularVelocity": 6.592812032299238e-8,
+                    "velocityX": -3.366709432935023,
+                    "velocityY": -1.7250814482051704,
+                    "timestamp": 0.8553717991090941
+                },
+                {
+                    "x": 5.499389832161033,
+                    "y": 4.11883296395985,
+                    "heading": 0.5282284192975907,
+                    "angularVelocity": 0.0000013356188425542589,
+                    "velocityX": -3.611840944307328,
+                    "velocityY": -1.1248296920470104,
+                    "timestamp": 0.9331328717553754
+                },
+                {
+                    "x": 5.2375954046767585,
+                    "y": 3.9846908767107623,
+                    "heading": 0.5282448470390027,
+                    "angularVelocity": 0.0002112591919891304,
+                    "velocityX": -3.3666514436488617,
+                    "velocityY": -1.7250544865716144,
+                    "timestamp": 1.0108939444016567
+                },
+                {
+                    "x": 4.9990709441074195,
+                    "y": 3.8334805845295827,
+                    "heading": 0.56802081206443,
+                    "angularVelocity": 0.5115151279660829,
+                    "velocityX": -3.067401881842984,
+                    "velocityY": -1.944549978997939,
+                    "timestamp": 1.0886550170479379
+                },
+                {
+                    "x": 4.779271759525853,
+                    "y": 3.6735037985529155,
+                    "heading": 0.6417219019727908,
+                    "angularVelocity": 0.9477890080506808,
+                    "velocityX": -2.826596613210087,
+                    "velocityY": -2.057286255609729,
+                    "timestamp": 1.166416089694219
+                },
+                {
+                    "x": 4.5625385904493,
+                    "y": 3.4912294193947493,
+                    "heading": 0.6783171685956464,
+                    "angularVelocity": 0.47061164895862556,
+                    "velocityX": -2.7871679453603333,
+                    "velocityY": -2.3440311836655594,
+                    "timestamp": 1.2441771623405002
+                },
+                {
+                    "x": 4.372028277853454,
+                    "y": 3.267088484433369,
+                    "heading": 0.6783172621445687,
+                    "angularVelocity": 0.0000012030302442626518,
+                    "velocityX": -2.4499445045265382,
+                    "velocityY": -2.8824311102413667,
+                    "timestamp": 1.3219382349867814
+                },
+                {
+                    "x": 4.1999208824099785,
+                    "y": 3.028525386131778,
+                    "heading": 0.678317151805174,
+                    "angularVelocity": -0.0000014189541221835924,
+                    "velocityX": -2.213284739864076,
+                    "velocityY": -3.067898759405809,
+                    "timestamp": 1.3996993076330626
+                },
+                {
+                    "x": 4.015537599692405,
+                    "y": 2.8383590216563426,
+                    "heading": 0.6532286408155417,
+                    "angularVelocity": -0.32263586568249475,
+                    "velocityX": -2.371151482905778,
+                    "velocityY": -2.4455213644037763,
+                    "timestamp": 1.4774603802793438
+                },
+                {
+                    "x": 3.8266113642227695,
+                    "y": 2.6983370404519986,
+                    "heading": 0.6187227841336836,
+                    "angularVelocity": -0.4437420357974964,
+                    "velocityX": -2.429573423311953,
+                    "velocityY": -1.8006693637223163,
+                    "timestamp": 1.555221452925625
+                },
+                {
+                    "x": 3.635277950307217,
+                    "y": 2.6086556112169066,
+                    "heading": 0.5790475746735078,
+                    "angularVelocity": -0.5102194209775099,
+                    "velocityX": -2.460529509229992,
+                    "velocityY": -1.1532946522360021,
+                    "timestamp": 1.6329825255719062
+                },
+                {
+                    "x": 3.4425296783447266,
+                    "y": 2.5693652629852295,
+                    "heading": 0.5361762744054714,
+                    "angularVelocity": -0.5513208448531661,
+                    "velocityX": -2.4787244491760223,
+                    "velocityY": -0.5052701421751292,
+                    "timestamp": 1.7107435982181873
+                },
+                {
+                    "x": 3.273952117174966,
+                    "y": 2.5733990398146904,
+                    "heading": 0.49726118807033537,
+                    "angularVelocity": -0.5743772146540196,
+                    "velocityX": -2.4881638242809143,
+                    "velocityY": 0.05953756545439798,
+                    "timestamp": 1.7784953915385004
+                },
+                {
+                    "x": 3.1059881626789445,
+                    "y": 2.6157002802478813,
+                    "heading": 0.456882179671172,
+                    "angularVelocity": -0.5959843484623638,
+                    "velocityX": -2.4791071389347814,
+                    "velocityY": 0.6243560260198714,
+                    "timestamp": 1.8462471848588136
+                },
+                {
+                    "x": 2.9403803042277463,
+                    "y": 2.6962017103764078,
+                    "heading": 0.41517848324514556,
+                    "angularVelocity": -0.6155364217277942,
+                    "velocityX": -2.444331734043506,
+                    "velocityY": 1.1881815400506988,
+                    "timestamp": 1.9139989781791267
+                },
+                {
+                    "x": 2.7796953039379972,
+                    "y": 2.8146585273254012,
+                    "heading": 0.3723609476398507,
+                    "angularVelocity": -0.6319764172568042,
+                    "velocityX": -2.3716715442508174,
+                    "velocityY": 1.7483938231562413,
+                    "timestamp": 1.9817507714994398
+                },
+                {
+                    "x": 2.628009703912687,
+                    "y": 2.970316258543535,
+                    "heading": 0.3287788631859365,
+                    "angularVelocity": -0.6432609724124856,
+                    "velocityX": -2.23884258396202,
+                    "velocityY": 2.2974702748047298,
+                    "timestamp": 2.0495025648197527
+                },
+                {
+                    "x": 2.4923790399755665,
+                    "y": 3.160718496478581,
+                    "heading": 0.28508218813026037,
+                    "angularVelocity": -0.6449523018392979,
+                    "velocityX": -2.001875630005736,
+                    "velocityY": 2.8102907480968544,
+                    "timestamp": 2.1172543581400656
+                },
+                {
+                    "x": 2.384956097652455,
+                    "y": 3.376988776463332,
+                    "heading": 0.24257394609876834,
+                    "angularVelocity": -0.627411319291935,
+                    "velocityX": -1.5855365158416335,
+                    "velocityY": 3.192096760631575,
+                    "timestamp": 2.1850061514603785
+                },
+                {
+                    "x": 2.3144199991578422,
+                    "y": 3.6018152629752547,
+                    "heading": 0.17996830737750358,
+                    "angularVelocity": -0.9240440090682317,
+                    "velocityX": -1.0410956675513523,
+                    "velocityY": 3.3183842890918167,
+                    "timestamp": 2.2527579447806914
+                },
+                {
+                    "x": 2.277461106698391,
+                    "y": 3.8129334421080285,
+                    "heading": 0.07321465766316056,
+                    "angularVelocity": -1.57565791963083,
+                    "velocityX": -0.5455042685692306,
+                    "velocityY": 3.1160530044520236,
+                    "timestamp": 2.3205097381010042
+                },
+                {
+                    "x": 2.2642620699526455,
+                    "y": 3.9942911908489713,
+                    "heading": -0.04721496632936169,
+                    "angularVelocity": -1.7775119755599884,
+                    "velocityX": -0.1948145738865332,
+                    "velocityY": 2.676796286166625,
+                    "timestamp": 2.388261531421317
+                },
+                {
+                    "x": 2.2685016834700633,
+                    "y": 4.1415779064179725,
+                    "heading": -0.1677701441610935,
+                    "angularVelocity": -1.7793651197065368,
+                    "velocityX": 0.06257566493294611,
+                    "velocityY": 2.173916118687331,
+                    "timestamp": 2.45601332474163
+                },
+                {
+                    "x": 2.2803498467055876,
+                    "y": 4.251629868834139,
+                    "heading": -0.2721018795383186,
+                    "angularVelocity": -1.5399110527447033,
+                    "velocityX": 0.17487600925202312,
+                    "velocityY": 1.624340213341216,
+                    "timestamp": 2.523765118061943
+                },
+                {
+                    "x": 2.2924250535428135,
+                    "y": 4.3241863896986095,
+                    "heading": -0.3487735727111741,
+                    "angularVelocity": -1.1316555535345214,
+                    "velocityX": 0.17822711762236845,
+                    "velocityY": 1.0709166105971757,
+                    "timestamp": 2.591516911382256
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": -0.39,
+                    "angularVelocity": -0.6084920452794201,
+                    "velocityX": 0.1118043683563332,
+                    "velocityY": 0.5286001823165565,
+                    "timestamp": 2.6592687047025687
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": -0.39,
+                    "angularVelocity": -7.720140427747849e-31,
+                    "velocityX": 0,
+                    "velocityY": -1.167304580662352e-31,
+                    "timestamp": 2.7270204980228816
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -27365,12 +32652,12 @@
                 },
                 {
                     "x": 5.61,
-                    "y": 2.81,
+                    "y": 5.44,
                     "radius": 0.5
                 },
                 {
-                    "x": 5.61,
-                    "y": 5.44,
+                    "x": 5.615683555603027,
+                    "y": 2.860041856765747,
                     "radius": 0.5
                 }
             ]
@@ -27400,128 +32687,128 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": -2.09363537243035e-32,
-                    "angularVelocity": -1.4612041776101527e-32,
-                    "velocityX": -1.1521781181914207e-34,
-                    "velocityY": -1.3541198100986985e-32,
+                    "heading": 0,
+                    "angularVelocity": 7.776001513534033e-33,
+                    "velocityX": 0,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.620476774011981,
-                    "y": 5.828095362884936,
-                    "heading": -0.014618875325545894,
-                    "angularVelocity": -0.16274311877436243,
-                    "velocityX": -0.6626361618854291,
-                    "velocityY": 0.346165913767109,
-                    "timestamp": 0.08982791675397632
-                },
-                {
-                    "x": 7.50139830253108,
-                    "y": 5.890300516939614,
-                    "heading": -0.04163561640032371,
-                    "angularVelocity": -0.3007610779705845,
-                    "velocityX": -1.3256287775996987,
-                    "velocityY": 0.692492448923733,
-                    "timestamp": 0.17965583350795264
-                },
-                {
-                    "x": 7.322721643819432,
-                    "y": 5.983633497452849,
-                    "heading": -0.07726722942321354,
-                    "angularVelocity": -0.3966652496292318,
-                    "velocityX": -1.9890994377729274,
-                    "velocityY": 1.0390197600692306,
-                    "timestamp": 0.26948375026192894
-                },
-                {
-                    "x": 7.084404587107726,
-                    "y": 6.108106334427675,
-                    "heading": -0.11365614278993857,
-                    "angularVelocity": -0.4050958174437935,
-                    "velocityX": -2.6530400049732523,
-                    "velocityY": 1.3856809939802615,
-                    "timestamp": 0.3593116670159053
-                },
-                {
-                    "x": 6.786764353262062,
-                    "y": 6.2635080892247865,
-                    "heading": -0.12453213530374094,
-                    "angularVelocity": -0.12107586268075081,
-                    "velocityX": -3.313449143664897,
-                    "velocityY": 1.7299939752886722,
-                    "timestamp": 0.4491395837698816
-                },
-                {
-                    "x": 6.485598483517562,
-                    "y": 6.4209007383980845,
-                    "heading": -0.12453214800780214,
-                    "angularVelocity": -1.4142664848458141e-7,
-                    "velocityX": -3.352697920951937,
-                    "velocityY": 1.752157401182633,
-                    "timestamp": 0.5389675005238579
-                },
-                {
-                    "x": 6.164419225797539,
-                    "y": 6.531883674906018,
-                    "heading": -0.12453214718798315,
-                    "angularVelocity": 9.126550019087707e-9,
-                    "velocityX": -3.575494894306401,
-                    "velocityY": 1.2355060711459842,
-                    "timestamp": 0.6287954172778342
-                },
-                {
-                    "x": 5.83853002564033,
-                    "y": 6.576482035264996,
-                    "heading": -0.08897109874736252,
-                    "angularVelocity": 0.3958796967096147,
-                    "velocityX": -3.6279278417395133,
-                    "velocityY": 0.4964866376799699,
-                    "timestamp": 0.7186233340318104
-                },
-                {
-                    "x": 5.573146790095341,
-                    "y": 6.601508859331819,
-                    "heading": 0.02708371250022136,
-                    "angularVelocity": 1.2919681925323805,
-                    "velocityX": -2.9543514436812472,
-                    "velocityY": 0.278608532527451,
-                    "timestamp": 0.8084512507857867
-                },
-                {
-                    "x": 5.3743330314019,
-                    "y": 6.61692927227141,
-                    "heading": 0.137026184669163,
-                    "angularVelocity": 1.2239232094189134,
-                    "velocityX": -2.213273622252188,
-                    "velocityY": 0.1716661534278421,
-                    "timestamp": 0.898279167539763
-                },
-                {
-                    "x": 5.242037120454666,
-                    "y": 6.625930660217185,
-                    "heading": 0.21902488374006282,
-                    "angularVelocity": 0.912842043253442,
-                    "velocityX": -1.4727705565027283,
-                    "velocityY": 0.10020702105814829,
-                    "timestamp": 0.9881070842937393
+                    "x": 7.620476769374415,
+                    "y": 5.82809535402629,
+                    "heading": -0.014618874829729222,
+                    "angularVelocity": -0.16274311325472623,
+                    "velocityX": -0.6626362135126213,
+                    "velocityY": 0.34616581514913597,
+                    "timestamp": 0.0898279167539809
+                },
+                {
+                    "x": 7.5013982904113785,
+                    "y": 5.890300493780534,
+                    "heading": -0.04163561529349077,
+                    "angularVelocity": -0.30076107116849365,
+                    "velocityX": -1.3256288608937272,
+                    "velocityY": 0.6924922897255835,
+                    "timestamp": 0.1796558335079618
+                },
+                {
+                    "x": 7.3227216232188646,
+                    "y": 5.983633458070523,
+                    "heading": -0.07726722819497155,
+                    "angularVelocity": -0.39666524827763777,
+                    "velocityX": -1.9890995321851956,
+                    "velocityY": 1.0390195794655717,
+                    "timestamp": 0.2694837502619427
+                },
+                {
+                    "x": 7.084404558965627,
+                    "y": 6.108106280619183,
+                    "heading": -0.11365614248079976,
+                    "angularVelocity": -0.40509582767559393,
+                    "velocityX": -2.6530400889284445,
+                    "velocityY": 1.3856808333823862,
+                    "timestamp": 0.3593116670159236
+                },
+                {
+                    "x": 6.786764320803032,
+                    "y": 6.263508027124416,
+                    "heading": -0.12453213515982267,
+                    "angularVelocity": -0.12107586452004557,
+                    "velocityX": -3.313449191722499,
+                    "velocityY": 1.7299938829800985,
+                    "timestamp": 0.4491395837699045
+                },
+                {
+                    "x": 6.485598483527544,
+                    "y": 6.42090073842616,
+                    "heading": -0.1245321478638912,
+                    "angularVelocity": -1.4142672994383305e-7,
+                    "velocityX": -3.3526975594938513,
+                    "velocityY": 1.7521580928210598,
+                    "timestamp": 0.5389675005238854
+                },
+                {
+                    "x": 6.164419225817841,
+                    "y": 6.531883674964022,
+                    "heading": -0.12453214704403566,
+                    "angularVelocity": 9.126957138704023e-9,
+                    "velocityX": -3.575494894191328,
+                    "velocityY": 1.2355060714790946,
+                    "timestamp": 0.6287954172778664
+                },
+                {
+                    "x": 5.838530026083828,
+                    "y": 6.576482035403971,
+                    "heading": -0.08897109734241883,
+                    "angularVelocity": 0.39587971074750417,
+                    "velocityX": -3.62792783702814,
+                    "velocityY": 0.4964866385813549,
+                    "timestamp": 0.7186233340318473
+                },
+                {
+                    "x": 5.57314678987786,
+                    "y": 6.601508855183474,
+                    "heading": 0.02708371106201491,
+                    "angularVelocity": 1.2919681608812392,
+                    "velocityX": -2.9543514510393742,
+                    "velocityY": 0.27860848479928674,
+                    "timestamp": 0.8084512507858282
+                },
+                {
+                    "x": 5.3743330310505755,
+                    "y": 6.616929267325104,
+                    "heading": 0.1370261836602847,
+                    "angularVelocity": 1.2239232141983023,
+                    "velocityX": -2.213273623742076,
+                    "velocityY": 0.17166614454461948,
+                    "timestamp": 0.8982791675398092
+                },
+                {
+                    "x": 5.242037120273562,
+                    "y": 6.625930657483227,
+                    "heading": 0.21902488355780253,
+                    "angularVelocity": 0.9128420524556349,
+                    "velocityX": -1.4727705546076917,
+                    "velocityY": 0.10020704568687104,
+                    "timestamp": 0.9881070842937901
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
                     "heading": 0.263,
-                    "angularVelocity": 0.4895484371565439,
-                    "velocityX": -0.7351514188571259,
-                    "velocityY": 0.04530150458636799,
-                    "timestamp": 1.0779350010477156
+                    "angularVelocity": 0.48954843918551255,
+                    "velocityX": -0.7351514168409599,
+                    "velocityY": 0.0453015350218741,
+                    "timestamp": 1.077935001047771
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
                     "heading": 0.263,
-                    "angularVelocity": 0,
-                    "velocityX": 4.777602762336884e-34,
-                    "velocityY": 2.1640483822800166e-33,
-                    "timestamp": 1.167762917801692
+                    "angularVelocity": -4.780109867389741e-33,
+                    "velocityX": 0,
+                    "velocityY": -5.233682494366388e-34,
+                    "timestamp": 1.167762917801752
                 }
             ],
             "constraints": [
@@ -27542,16 +32829,6 @@
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
             "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
                 {
                     "x": 5.61,
                     "y": 5.44,
@@ -27568,7 +32845,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 14
+                    "controlIntervalCount": 11
+                },
+                {
+                    "x": 5.40805721282959,
+                    "y": 6.638837814331055,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 9
                 },
                 {
                     "x": 4.263,
@@ -27580,7 +32866,197 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 0,
+                    "angularVelocity": 1.3211551091100433e-31,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.6375302942972665,
+                    "y": 5.822387711771769,
+                    "heading": 0.010500881646553203,
+                    "angularVelocity": 0.13619602846032355,
+                    "velocityX": -0.5508304389364825,
+                    "velocityY": 0.32927763890619355,
+                    "timestamp": 0.07710123243140166
+                },
+                {
+                    "x": 7.5525908811480225,
+                    "y": 5.873163180716977,
+                    "heading": 0.031500279151306286,
+                    "angularVelocity": 0.27236137273728567,
+                    "velocityX": -1.1016609004897018,
+                    "velocityY": 0.6585558666702785,
+                    "timestamp": 0.1542024648628033
+                },
+                {
+                    "x": 7.4251817593524025,
+                    "y": 5.949326484276257,
+                    "heading": 0.06299451206522548,
+                    "angularVelocity": 0.40847898173275193,
+                    "velocityX": -1.6524913776051289,
+                    "velocityY": 0.987835098836381,
+                    "timestamp": 0.23130369729420497
+                },
+                {
+                    "x": 7.255302939480135,
+                    "y": 6.050877733979633,
+                    "heading": 0.10497913316622995,
+                    "angularVelocity": 0.5445389104299847,
+                    "velocityX": -2.203321717631576,
+                    "velocityY": 1.3171157775425744,
+                    "timestamp": 0.3084049297256066
+                },
+                {
+                    "x": 7.042954462706059,
+                    "y": 6.177817072431174,
+                    "heading": 0.1574496496087124,
+                    "angularVelocity": 0.6805405670936119,
+                    "velocityX": -2.754151523621958,
+                    "velocityY": 1.6463983058180132,
+                    "timestamp": 0.3855061621570083
+                },
+                {
+                    "x": 6.792607460054783,
+                    "y": 6.3274746833219355,
+                    "heading": 0.15744965721454035,
+                    "angularVelocity": 9.864729365588338e-8,
+                    "velocityX": -3.246990933303352,
+                    "velocityY": 1.941053419916652,
+                    "timestamp": 0.46260739458840994
+                },
+                {
+                    "x": 6.542259846259959,
+                    "y": 6.4771312718889495,
+                    "heading": 0.15744966481969572,
+                    "angularVelocity": 9.863857053631023e-8,
+                    "velocityX": -3.246998859811527,
+                    "velocityY": 1.9410401604172332,
+                    "timestamp": 0.5397086270198116
+                },
+                {
+                    "x": 6.273256747030451,
+                    "y": 6.589862608843688,
+                    "heading": 0.1574496727775284,
+                    "angularVelocity": 1.0321278187268036e-7,
+                    "velocityX": -3.4889597837342676,
+                    "velocityY": 1.4621210764047974,
+                    "timestamp": 0.6168098594512132
+                },
+                {
+                    "x": 5.98904964850956,
+                    "y": 6.655416753006686,
+                    "heading": 0.15744969119167346,
+                    "angularVelocity": 2.3883074855376557e-7,
+                    "velocityX": -3.686155065987494,
+                    "velocityY": 0.8502347121535809,
+                    "timestamp": 0.6939110918826149
+                },
+                {
+                    "x": 5.6978465137603145,
+                    "y": 6.6719003531074685,
+                    "heading": 0.15744999446473232,
+                    "angularVelocity": 0.00000393343983387992,
+                    "velocityX": -3.7768933850484796,
+                    "velocityY": 0.21379165521703997,
+                    "timestamp": 0.7710123243140166
+                },
+                {
+                    "x": 5.40805721282959,
+                    "y": 6.638837814331055,
+                    "heading": 0.15745035498223803,
+                    "angularVelocity": 0.00000467589809324531,
+                    "velocityX": -3.7585560151526085,
+                    "velocityY": -0.428819848059242,
+                    "timestamp": 0.8481135567454182
+                },
+                {
+                    "x": 5.104307335648147,
+                    "y": 6.562247743535093,
+                    "heading": 0.18966206475475966,
+                    "angularVelocity": 0.37702586381684944,
+                    "velocityX": -3.555277277652879,
+                    "velocityY": -0.896457772827513,
+                    "timestamp": 0.9335498964342235
+                },
+                {
+                    "x": 4.836190414730584,
+                    "y": 6.438293970082571,
+                    "heading": 0.27176288986041636,
+                    "angularVelocity": 0.9609590650149814,
+                    "velocityX": -3.1382070193334055,
+                    "velocityY": -1.4508319750590115,
+                    "timestamp": 1.0189862361230289
+                },
+                {
+                    "x": 4.616497381460216,
+                    "y": 6.278221072521664,
+                    "heading": 0.3274582685749672,
+                    "angularVelocity": 0.6518933151562509,
+                    "velocityX": -2.5714237532949347,
+                    "velocityY": -1.8735926438791781,
+                    "timestamp": 1.1044225758118342
+                },
+                {
+                    "x": 4.449006665280017,
+                    "y": 6.09487376587715,
+                    "heading": 0.3096216512620487,
+                    "angularVelocity": -0.2087708506460702,
+                    "velocityX": -1.960415401575849,
+                    "velocityY": -2.1460107878256482,
+                    "timestamp": 1.1898589155006396
+                },
+                {
+                    "x": 4.339814814870372,
+                    "y": 5.903698766708507,
+                    "heading": 0.23521329843339078,
+                    "angularVelocity": -0.8709215902704173,
+                    "velocityX": -1.2780492564097095,
+                    "velocityY": -2.237630964352897,
+                    "timestamp": 1.275295255189445
+                },
+                {
+                    "x": 4.286146359443328,
+                    "y": 5.736442488850756,
+                    "heading": 0.1356278137676922,
+                    "angularVelocity": -1.1656103834554516,
+                    "velocityX": -0.628168945703036,
+                    "velocityY": -1.957671389796991,
+                    "timestamp": 1.3607315948782504
+                },
+                {
+                    "x": 4.266951931468271,
+                    "y": 5.619203136466468,
+                    "heading": 0.049466079264437586,
+                    "angularVelocity": -1.0084904715849423,
+                    "velocityX": -0.2246635102225946,
+                    "velocityY": -1.372242219309977,
+                    "timestamp": 1.4461679345670557
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 4.182976778726496e-34,
+                    "angularVelocity": -0.578981724224301,
+                    "velocityX": -0.046255861178807596,
+                    "velocityY": -0.6929502912005617,
+                    "timestamp": 1.531604274255861
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": -6.30298269203181e-33,
+                    "timestamp": 1.6170406139446665
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -27599,16 +33075,6 @@
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
             "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
                 {
                     "x": 5.61,
                     "y": 5.44,
@@ -27872,7 +33338,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 21
+                    "controlIntervalCount": 12
+                },
+                {
+                    "x": 6.7368645668029785,
+                    "y": 3.4413950443267822,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 15
                 },
                 {
                     "x": 5.176,
@@ -27884,7 +33359,260 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 0,
+                    "angularVelocity": -1.007826409753455e-39,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.661226431831969,
+                    "y": 5.757681823957774,
+                    "heading": -0.0001846118691686639,
+                    "angularVelocity": -0.0025538708536768893,
+                    "velocityX": -0.25970848342391806,
+                    "velocityY": -0.5439170529292338,
+                    "timestamp": 0.07228708096295171
+                },
+                {
+                    "x": 7.623679292701499,
+                    "y": 5.679045474684997,
+                    "heading": -0.0005538375109882563,
+                    "angularVelocity": -0.005107768039614528,
+                    "velocityX": -0.5194170055049473,
+                    "velocityY": -1.0878340669625215,
+                    "timestamp": 0.14457416192590342
+                },
+                {
+                    "x": 7.567358579979895,
+                    "y": 5.561090955652203,
+                    "heading": -0.0011076778331591435,
+                    "angularVelocity": -0.0076616777824344475,
+                    "velocityX": -0.779125563950612,
+                    "velocityY": -1.6317510329853882,
+                    "timestamp": 0.2168612428888551
+                },
+                {
+                    "x": 7.492264291454711,
+                    "y": 5.403818271607319,
+                    "heading": -0.001846132228095339,
+                    "angularVelocity": -0.010215579120073546,
+                    "velocityX": -1.0388341530026832,
+                    "velocityY": -2.175667933326667,
+                    "timestamp": 0.28914832385180683
+                },
+                {
+                    "x": 7.39839642615194,
+                    "y": 5.207227430397753,
+                    "heading": -0.0027691974892554232,
+                    "angularVelocity": -0.012769436099282666,
+                    "velocityX": -1.298542755528879,
+                    "velocityY": -2.719584725109078,
+                    "timestamp": 0.36143540481475855
+                },
+                {
+                    "x": 7.285754988600711,
+                    "y": 4.97131845200793,
+                    "heading": -0.0038768638943260385,
+                    "angularVelocity": -0.015323158582628504,
+                    "velocityX": -1.5582512954003445,
+                    "velocityY": -3.263501240432299,
+                    "timestamp": 0.4337224857777103
+                },
+                {
+                    "x": 7.167927209247298,
+                    "y": 4.7245478329210355,
+                    "heading": -0.0038768640537450113,
+                    "angularVelocity": -2.205359112715029e-9,
+                    "velocityX": -1.629997750411335,
+                    "velocityY": -3.4137582511232942,
+                    "timestamp": 0.506009566740662
+                },
+                {
+                    "x": 7.050099366875978,
+                    "y": 4.477777243923851,
+                    "heading": -0.003876864213154645,
+                    "angularVelocity": -2.2052299170164517e-9,
+                    "velocityX": -1.6299986221840805,
+                    "velocityY": -3.413757834870371,
+                    "timestamp": 0.5782966477036137
+                },
+                {
+                    "x": 6.932271561638543,
+                    "y": 4.2310066371960025,
+                    "heading": -0.0038768643725604667,
+                    "angularVelocity": -2.2051771914474314e-9,
+                    "velocityX": -1.629998108483918,
+                    "velocityY": -3.413758080151585,
+                    "timestamp": 0.6505837286665653
+                },
+                {
+                    "x": 6.8252544845676155,
+                    "y": 3.9793591274718993,
+                    "heading": -0.0038768645333797187,
+                    "angularVelocity": -2.2247301918048757e-9,
+                    "velocityX": -1.4804454080221456,
+                    "velocityY": -3.4812238421008592,
+                    "timestamp": 0.722870809629517
+                },
+                {
+                    "x": 6.7595626994044276,
+                    "y": 3.7139091088268694,
+                    "heading": -0.003876864707631206,
+                    "angularVelocity": -2.410548124556829e-9,
+                    "velocityX": -0.9087624550347577,
+                    "velocityY": -3.6721640313720423,
+                    "timestamp": 0.7951578905924687
+                },
+                {
+                    "x": 6.7368645668029785,
+                    "y": 3.4413950443267822,
+                    "heading": -0.0038768649214938546,
+                    "angularVelocity": -2.9585182521427424e-9,
+                    "velocityX": -0.31399985030634436,
+                    "velocityY": -3.7698861382956523,
+                    "timestamp": 0.8674449715554203
+                },
+                {
+                    "x": 6.763602448674763,
+                    "y": 3.1456190677825386,
+                    "heading": -0.0038769163309766456,
+                    "angularVelocity": -6.548510424231184e-7,
+                    "velocityX": 0.340585605327871,
+                    "velocityY": -3.7675774205236117,
+                    "timestamp": 0.9459505849856912
+                },
+                {
+                    "x": 6.763837716877819,
+                    "y": 2.855181760345028,
+                    "heading": -0.02573615579182971,
+                    "angularVelocity": -0.27844173818562123,
+                    "velocityX": 0.0029968328731749647,
+                    "velocityY": -3.699573759722024,
+                    "timestamp": 1.0244561984159621
+                },
+                {
+                    "x": 6.746691140594586,
+                    "y": 2.576419895706607,
+                    "heading": -0.08408576500639096,
+                    "angularVelocity": -0.7432539746522423,
+                    "velocityX": -0.2184121049950426,
+                    "velocityY": -3.5508526391685153,
+                    "timestamp": 1.102961811846233
+                },
+                {
+                    "x": 6.69086397201793,
+                    "y": 2.322743038831463,
+                    "heading": -0.2046015958190934,
+                    "angularVelocity": -1.5351237388870964,
+                    "velocityX": -0.7111232705192682,
+                    "velocityY": -3.2313212494092722,
+                    "timestamp": 1.1814674252765038
+                },
+                {
+                    "x": 6.586917702656428,
+                    "y": 2.0871040226876985,
+                    "heading": -0.32491541634118276,
+                    "angularVelocity": -1.5325505433946174,
+                    "velocityX": -1.324061615719081,
+                    "velocityY": -3.001556269005664,
+                    "timestamp": 1.2599730387067747
+                },
+                {
+                    "x": 6.439847045017927,
+                    "y": 1.879401635438828,
+                    "heading": -0.448122685606705,
+                    "angularVelocity": -1.5694071274910255,
+                    "velocityX": -1.8733775995410846,
+                    "velocityY": -2.6457010928696563,
+                    "timestamp": 1.3384786521370455
+                },
+                {
+                    "x": 6.25456019704424,
+                    "y": 1.7060438180074393,
+                    "heading": -0.5681346722700569,
+                    "angularVelocity": -1.528705801018258,
+                    "velocityX": -2.3601732395640638,
+                    "velocityY": -2.2082219328859343,
+                    "timestamp": 1.4169842655673164
+                },
+                {
+                    "x": 6.049779045368227,
+                    "y": 1.57972317182915,
+                    "heading": -0.6633309119306381,
+                    "angularVelocity": -1.2126042393788183,
+                    "velocityX": -2.6084905617341763,
+                    "velocityY": -1.6090651439911137,
+                    "timestamp": 1.4954898789975872
+                },
+                {
+                    "x": 5.840944041976222,
+                    "y": 1.503518626439596,
+                    "heading": -0.7208467997653518,
+                    "angularVelocity": -0.7326340795464229,
+                    "velocityX": -2.660128292322611,
+                    "velocityY": -0.9706891273098467,
+                    "timestamp": 1.573995492427858
+                },
+                {
+                    "x": 5.636524773547887,
+                    "y": 1.4770268115818306,
+                    "heading": -0.7348631463149735,
+                    "angularVelocity": -0.17853941822989652,
+                    "velocityX": -2.603880913686534,
+                    "velocityY": -0.33745121782018417,
+                    "timestamp": 1.652501105858129
+                },
+                {
+                    "x": 5.451777868175388,
+                    "y": 1.4973120784666072,
+                    "heading": -0.7197568228935092,
+                    "angularVelocity": 0.19242348109134802,
+                    "velocityX": -2.353295481686686,
+                    "velocityY": 0.25839256581051945,
+                    "timestamp": 1.7310067192883998
+                },
+                {
+                    "x": 5.309771976878378,
+                    "y": 1.5457401851903851,
+                    "heading": -0.687868735789328,
+                    "angularVelocity": 0.4061886241103559,
+                    "velocityX": -1.808862896449324,
+                    "velocityY": 0.6168744451222204,
+                    "timestamp": 1.8095123327186706
+                },
+                {
+                    "x": 5.2191255322417085,
+                    "y": 1.592492981200916,
+                    "heading": -0.6576952711638115,
+                    "angularVelocity": 0.38434786134518467,
+                    "velocityX": -1.154649211386418,
+                    "velocityY": 0.5955344333696231,
+                    "timestamp": 1.8880179461489415
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": -0.64,
+                    "angularVelocity": 0.22540134890517805,
+                    "velocityX": -0.5493305555788384,
+                    "velocityY": 0.35038282738234927,
+                    "timestamp": 1.9665235595792123
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": -0.64,
+                    "angularVelocity": 0,
+                    "velocityX": 6.617232561290069e-35,
+                    "velocityY": 1.0472745338276337e-32,
+                    "timestamp": 2.045029173009483
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -27945,92 +33673,92 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
+                    "heading": -3.817834980565894e-40,
+                    "angularVelocity": -1.9855221195343998e-39,
+                    "velocityX": -9.621145750064654e-40,
+                    "velocityY": 1.4575297691012433e-39,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.680000027621492,
-                    "y": 5.880500005575844,
-                    "heading": 1.812927824526906e-8,
-                    "angularVelocity": 1.811639213454267e-7,
-                    "velocityX": 2.7601859924059844e-7,
-                    "velocityY": 0.8344065481228081,
-                    "timestamp": 0.10007112931184069
+                    "x": 7.68,
+                    "y": 5.880500005575846,
+                    "heading": 3.479114156010015e-22,
+                    "angularVelocity": 3.4766412561552746e-21,
+                    "velocityX": 2.7068848151306367e-33,
+                    "velocityY": 0.8344065481228213,
+                    "timestamp": 0.10007112931184182
                 },
                 {
-                    "x": 7.6800000748220105,
-                    "y": 6.047500013629843,
-                    "heading": 4.634757751496617e-8,
-                    "angularVelocity": 2.819824195265022e-7,
-                    "velocityX": 4.716696889582397e-7,
-                    "velocityY": 1.6688130652907391,
-                    "timestamp": 0.20014225862368137
+                    "x": 7.68,
+                    "y": 6.047500013629847,
+                    "heading": 1.081691491825702e-21,
+                    "angularVelocity": 7.332585052389997e-21,
+                    "velocityX": 5.1589068200097806e-33,
+                    "velocityY": 1.6688130652907358,
+                    "timestamp": 0.20014225862368365
                 },
                 {
-                    "x": 7.68000013381266,
-                    "y": 6.298000017966615,
-                    "heading": 7.748634725130675e-8,
-                    "angularVelocity": 3.1116636502244896e-7,
-                    "velocityX": 5.894871979234934e-7,
-                    "velocityY": 2.5032195205488934,
-                    "timestamp": 0.30021338793552205
+                    "x": 7.68,
+                    "y": 6.298000017966616,
+                    "heading": 2.2511586271387426e-21,
+                    "angularVelocity": 1.1686358860616172e-20,
+                    "velocityX": 5.8498916949463956e-33,
+                    "velocityY": 2.503219520548835,
+                    "timestamp": 0.3002133879355255
                 },
                 {
-                    "x": 7.680000197181065,
-                    "y": 6.63200000000002,
-                    "heading": 1.0565412902376676e-7,
-                    "angularVelocity": 2.814776011952798e-7,
-                    "velocityX": 6.332336214323824e-7,
-                    "velocityY": 3.3376257900777544,
-                    "timestamp": 0.40028451724736275
+                    "x": 7.68,
+                    "y": 6.632,
+                    "heading": 4.481673966100434e-21,
+                    "angularVelocity": 2.228929929167172e-20,
+                    "velocityX": 6.576791251337367e-33,
+                    "velocityY": 3.3376257900775013,
+                    "timestamp": 0.4002845172473673
                 },
                 {
-                    "x": 7.680000129088366,
-                    "y": 6.965999982033409,
-                    "heading": 6.403571463499393e-8,
-                    "angularVelocity": -4.1588832167210794e-7,
-                    "velocityX": -6.804429810396128e-7,
-                    "velocityY": 3.337625790077598,
-                    "timestamp": 0.5003556465592034
+                    "x": 7.68,
+                    "y": 6.9659999820333836,
+                    "heading": 1.4391637904192512e-21,
+                    "angularVelocity": -3.0403475801105514e-20,
+                    "velocityX": 1.0509973010204301e-32,
+                    "velocityY": 3.3376257900775013,
+                    "timestamp": 0.5003556465592092
                 },
                 {
-                    "x": 7.680000069764372,
-                    "y": 7.216499986370169,
-                    "heading": 3.1970340337388056e-8,
-                    "angularVelocity": -3.2042582447411e-7,
-                    "velocityX": -5.92818279593764e-7,
-                    "velocityY": 2.503219520548763,
-                    "timestamp": 0.6004267758710441
+                    "x": 7.68,
+                    "y": 7.216499986370152,
+                    "heading": 4.7457500532831525e-22,
+                    "angularVelocity": -9.63903176962576e-21,
+                    "velocityX": 7.983324746463204e-33,
+                    "velocityY": 2.503219520548835,
+                    "timestamp": 0.600426775871051
                 },
                 {
-                    "x": 7.680000024949458,
-                    "y": 7.3834999944241595,
-                    "heading": 1.0547562280132128e-8,
-                    "angularVelocity": -2.1407550924431962e-7,
-                    "velocityX": -4.47830590750479e-7,
-                    "velocityY": 1.6688130652906517,
-                    "timestamp": 0.7004979051828848
+                    "x": 7.68,
+                    "y": 7.383499994424153,
+                    "heading": 1.0909640932435146e-22,
+                    "angularVelocity": -3.652188228166023e-21,
+                    "velocityX": 7.341551448907972e-33,
+                    "velocityY": 1.6688130652907358,
+                    "timestamp": 0.7004979051828928
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": -4.103497297298223e-32,
-                    "angularVelocity": -1.0540065169911972e-7,
-                    "velocityX": -2.4931725183429086e-7,
-                    "velocityY": 0.8344065481227665,
-                    "timestamp": 0.8005690344947255
+                    "heading": -9.42965601107167e-40,
+                    "angularVelocity": -1.09018866193586e-21,
+                    "velocityX": 7.44470103227488e-33,
+                    "velocityY": 0.8344065481228213,
+                    "timestamp": 0.8005690344947346
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 0,
-                    "angularVelocity": 9.056551022450501e-31,
-                    "velocityX": 0,
-                    "velocityY": 3.73051624872423e-37,
-                    "timestamp": 0.9006401638065662
+                    "heading": 8.249839679439821e-43,
+                    "angularVelocity": 9.431229878048832e-39,
+                    "velocityX": 1.012759498983901e-41,
+                    "velocityY": 5.142787784847508e-39,
+                    "timestamp": 0.9006401638065764
                 }
             ],
             "constraints": [
@@ -28050,23 +33778,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "C2 To C3": {
             "waypoints": [
@@ -28093,92 +33805,92 @@
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": -8.386874849918624e-34,
-                    "velocityY": 2.2609899154097436e-36,
+                    "heading": -5.382102664363315e-41,
+                    "angularVelocity": -2.8652892976895567e-45,
+                    "velocityX": 1.592735858156421e-40,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.6800000367448975,
-                    "y": 5.713499994424246,
-                    "heading": -2.1127327949152964e-9,
-                    "angularVelocity": -2.1112310428149592e-8,
-                    "velocityX": 3.67187799986145e-7,
-                    "velocityY": -0.8344065481223435,
-                    "timestamp": 0.10007112931179547
+                    "x": 7.68,
+                    "y": 5.713499994424153,
+                    "heading": 1.659495816830368e-21,
+                    "angularVelocity": 1.658316243120447e-20,
+                    "velocityX": 2.780383744489643e-33,
+                    "velocityY": -0.8344065481228213,
+                    "timestamp": 0.10007112931184184
                 },
                 {
-                    "x": 7.680000101493379,
-                    "y": 5.546499986370426,
-                    "heading": -1.0421334714876827e-8,
-                    "angularVelocity": -8.302696522536156e-8,
-                    "velocityX": 6.470245866798498e-7,
-                    "velocityY": -1.668813065289814,
-                    "timestamp": 0.20014225862359095
+                    "x": 7.68,
+                    "y": 5.546499986370153,
+                    "heading": 4.9512387638524226e-21,
+                    "angularVelocity": 3.289403167370527e-20,
+                    "velocityX": 3.618624574631574e-33,
+                    "velocityY": -1.668813065290736,
+                    "timestamp": 0.20014225862368368
                 },
                 {
-                    "x": 7.680000185334384,
-                    "y": 5.295999982033924,
-                    "heading": -2.6903389225477584e-8,
-                    "angularVelocity": -1.6470340117310356e-7,
-                    "velocityX": 8.378141164132646e-7,
-                    "velocityY": -2.503219520547496,
-                    "timestamp": 0.30021338793538643
+                    "x": 7.68,
+                    "y": 5.295999982033384,
+                    "heading": 9.801760970151702e-21,
+                    "angularVelocity": 4.8470744390330394e-20,
+                    "velocityX": 3.439292531132709e-33,
+                    "velocityY": -2.503219520548835,
+                    "timestamp": 0.30021338793552554
                 },
                 {
-                    "x": 7.680000279163236,
-                    "y": 4.961999999999876,
-                    "heading": -5.074281043975999e-8,
-                    "angularVelocity": -2.3822478681802103e-7,
-                    "velocityX": 9.376216058628794e-7,
-                    "velocityY": -3.3376257900847826,
-                    "timestamp": 0.4002845172471819
+                    "x": 7.68,
+                    "y": 4.962,
+                    "heading": 1.506109654033251e-20,
+                    "angularVelocity": 5.255597191401202e-20,
+                    "velocityX": 4.639675238422859e-33,
+                    "velocityY": -3.3376257900775017,
+                    "timestamp": 0.40028451724736736
                 },
                 {
-                    "x": 7.680000186570606,
-                    "y": 4.628000017966085,
-                    "heading": -3.213613801692531e-8,
-                    "angularVelocity": 1.8593453255418192e-7,
-                    "velocityX": -9.252681679909059e-7,
-                    "velocityY": -3.3376257900823116,
-                    "timestamp": 0.5003556465589774
+                    "x": 7.68,
+                    "y": 4.628000017966616,
+                    "heading": 5.3586032763760104e-21,
+                    "angularVelocity": -9.69559666912617e-20,
+                    "velocityX": 4.935439546802769e-33,
+                    "velocityY": -3.3376257900775017,
+                    "timestamp": 0.5003556465592092
                 },
                 {
-                    "x": 7.68000010282861,
-                    "y": 4.377500013629577,
-                    "heading": -1.6413785806619038e-8,
-                    "angularVelocity": 1.5711175564873292e-7,
-                    "velocityX": -8.368247227344002e-7,
-                    "velocityY": -2.5032195205475483,
-                    "timestamp": 0.6004267758707729
+                    "x": 7.68,
+                    "y": 4.377500013629846,
+                    "heading": 1.8760590350448333e-21,
+                    "angularVelocity": -3.480068842301423e-20,
+                    "velocityX": 4.701730454770122e-33,
+                    "velocityY": -2.5032195205488352,
+                    "timestamp": 0.600426775871051
                 },
                 {
-                    "x": 7.6800000374582895,
-                    "y": 4.2105000055757555,
-                    "heading": -5.494810627011176e-9,
-                    "angularVelocity": 1.0911213201504113e-7,
-                    "velocityX": -6.532385577861589e-7,
-                    "velocityY": -1.668813065289845,
-                    "timestamp": 0.7004979051825684
+                    "x": 7.68,
+                    "y": 4.210500005575846,
+                    "heading": 3.168722428707802e-22,
+                    "angularVelocity": -1.5580785203142817e-20,
+                    "velocityX": 3.49539235617723e-33,
+                    "velocityY": -1.668813065290736,
+                    "timestamp": 0.7004979051828928
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 0,
-                    "angularVelocity": 5.4909043426679847e-8,
-                    "velocityX": -3.743166510876222e-7,
-                    "velocityY": -0.8344065481223572,
-                    "timestamp": 0.8005690344943639
+                    "heading": 5.881496870004905e-41,
+                    "angularVelocity": -3.1664700918333573e-21,
+                    "velocityX": 3.364378386644386e-33,
+                    "velocityY": -0.8344065481228213,
+                    "timestamp": 0.8005690344947346
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 1.4758845853352304e-34,
-                    "velocityY": 2.7202161180895526e-29,
-                    "timestamp": 0.9006401638061594
+                    "heading": 9.145026397391945e-42,
+                    "angularVelocity": -4.963805231001231e-40,
+                    "velocityX": -4.870998572760777e-42,
+                    "velocityY": 3.489748854003666e-39,
+                    "timestamp": 0.9006401638065764
                 }
             ],
             "constraints": [
@@ -28198,23 +33910,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "C2 To C4": {
             "waypoints": [
@@ -28242,136 +33938,136 @@
                     "x": 7.68,
                     "y": 5.797,
                     "heading": 0,
-                    "angularVelocity": -3.3479093461707414e-32,
+                    "angularVelocity": 7.067561628822357e-40,
                     "velocityX": 0,
-                    "velocityY": 6.414063862178313e-32,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.680000074936813,
-                    "y": 5.720554627994495,
-                    "heading": 3.835783864206662e-9,
-                    "angularVelocity": 4.006018550688398e-8,
-                    "velocityX": 7.826256085285986e-7,
-                    "velocityY": -0.7983807350943762,
-                    "timestamp": 0.09575052183150116
-                },
+                    "x": 7.68,
+                    "y": 5.720554627994461,
+                    "heading": -3.8243496398495774e-19,
+                    "angularVelocity": -3.994077070459528e-18,
+                    "velocityX": -4.592295404090674e-34,
+                    "velocityY": -0.7983807350946677,
+                    "timestamp": 0.09575052183150977
+                },
                 {
-                    "x": 7.680000216991337,
-                    "y": 5.567663887017398,
-                    "heading": 7.581387411185645e-9,
-                    "angularVelocity": 3.911835760311887e-8,
-                    "velocityX": 0.0000014835900057043023,
-                    "velocityY": -1.5967614385031577,
-                    "timestamp": 0.19150104366300233
+                    "x": 7.68,
+                    "y": 5.567663887017311,
+                    "heading": -1.1460790562861162e-18,
+                    "angularVelocity": -7.975351749671857e-18,
+                    "velocityX": 3.216033275420413e-35,
+                    "velocityY": -1.5967614385035829,
+                    "timestamp": 0.19150104366301954
                 },
                 {
-                    "x": 7.68000041820133,
-                    "y": 5.338327783136532,
-                    "heading": 9.040192056984193e-9,
-                    "angularVelocity": 1.523546877140865e-8,
-                    "velocityX": 0.0000021013982858961807,
-                    "velocityY": -2.3951420785407556,
-                    "timestamp": 0.2872515654945035
+                    "x": 7.68,
+                    "y": 5.3383277831364,
+                    "heading": -2.289011643788579e-18,
+                    "angularVelocity": -1.1936567706665239e-17,
+                    "velocityX": -6.99395718741277e-36,
+                    "velocityY": -2.3951420785410233,
+                    "timestamp": 0.2872515654945293
                 },
                 {
-                    "x": 7.680000670435067,
-                    "y": 5.032546334555316,
-                    "heading": 8.315274931561148e-9,
-                    "angularVelocity": -7.570901836021169e-9,
-                    "velocityX": 0.000002634280323205928,
-                    "velocityY": -3.193522528465399,
-                    "timestamp": 0.38300208732600466
+                    "x": 7.68,
+                    "y": 5.032546334555254,
+                    "heading": -3.801510097022925e-18,
+                    "angularVelocity": -1.5796242396621782e-17,
+                    "velocityX": -1.9389720200265424e-34,
+                    "velocityY": -3.1935225284643693,
+                    "timestamp": 0.3830020873260391
                 },
                 {
-                    "x": 7.680000832750872,
-                    "y": 4.67032780073313,
-                    "heading": 8.315279560400229e-9,
-                    "angularVelocity": 4.9454766224662506e-14,
-                    "velocityX": 0.0000016951952366761453,
-                    "velocityY": -3.782940571954316,
-                    "timestamp": 0.47875260915750584
+                    "x": 7.68,
+                    "y": 4.670327800733179,
+                    "heading": -7.244345840319028e-19,
+                    "angularVelocity": 3.2136383741131935e-17,
+                    "velocityX": -8.432373407035536e-35,
+                    "velocityY": -3.7829405719528495,
+                    "timestamp": 0.47875260915754886
                 },
                 {
-                    "x": 7.680000914152323,
-                    "y": 4.308109266911049,
-                    "heading": 8.315282864092028e-9,
-                    "angularVelocity": 3.631070768427552e-14,
-                    "velocityX": 8.501410582319702e-7,
-                    "velocityY": -3.782940571953249,
-                    "timestamp": 0.574503130989007
+                    "x": 7.68,
+                    "y": 4.308109266911059,
+                    "heading": -3.757861819981246e-18,
+                    "angularVelocity": -3.168052954622205e-17,
+                    "velocityX": -1.6004910806259417e-34,
+                    "velocityY": -3.782940571953311,
+                    "timestamp": 0.5745031309890586
                 },
                 {
-                    "x": 7.680000914394957,
-                    "y": 3.9458907330889588,
-                    "heading": 8.315285941661222e-9,
-                    "angularVelocity": 3.388658702074781e-14,
-                    "velocityX": 2.5340105811348506e-9,
-                    "velocityY": -3.7829405719533424,
-                    "timestamp": 0.6702536528205081
+                    "x": 7.68,
+                    "y": 3.9458907330889397,
+                    "heading": -5.3976563030855884e-18,
+                    "angularVelocity": -1.712569761227889e-17,
+                    "velocityX": -2.8603575042124878e-33,
+                    "velocityY": -3.782940571953311,
+                    "timestamp": 0.6702536528205684
                 },
                 {
-                    "x": 7.680000833430653,
-                    "y": 3.583672199266877,
-                    "heading": 8.315288291957849e-9,
-                    "angularVelocity": 2.592873274060122e-14,
-                    "velocityX": -8.455755806508837e-7,
-                    "velocityY": -3.78294057195325,
-                    "timestamp": 0.7660041746520092
+                    "x": 7.68,
+                    "y": 3.5836721992668203,
+                    "heading": -4.35645304230582e-18,
+                    "angularVelocity": 1.0874126227865062e-17,
+                    "velocityX": -1.9073271103237224e-34,
+                    "velocityY": -3.782940571953311,
+                    "timestamp": 0.7660041746520782
                 },
                 {
-                    "x": 7.6800006713921105,
-                    "y": 3.2214536654446886,
-                    "heading": 8.315293718879221e-9,
-                    "angularVelocity": 5.742758811630884e-14,
-                    "velocityX": -0.0000016922995574745742,
-                    "velocityY": -3.782940571954345,
-                    "timestamp": 0.8617546964835103
+                    "x": 7.68,
+                    "y": 3.221453665444745,
+                    "heading": 3.661990694649205e-18,
+                    "angularVelocity": 8.37430813282138e-17,
+                    "velocityX": -3.48109229998139e-33,
+                    "velocityY": -3.7829405719528495,
+                    "timestamp": 0.8617546964835879
                 },
                 {
-                    "x": 7.680000418867919,
-                    "y": 2.915672216863469,
-                    "heading": 6.817552183291689e-10,
-                    "angularVelocity": -7.972319645162058e-8,
-                    "velocityX": -0.000002637313779217548,
-                    "velocityY": -3.1935225284654263,
-                    "timestamp": 0.9575052183150115
+                    "x": 7.68,
+                    "y": 2.915672216863599,
+                    "heading": 2.222559506115206e-18,
+                    "angularVelocity": -1.503314198751682e-17,
+                    "velocityX": 7.233727398507661e-34,
+                    "velocityY": -3.1935225284643693,
+                    "timestamp": 0.9575052183150977
                 },
                 {
-                    "x": 7.680000217372703,
-                    "y": 2.6863361129826027,
-                    "heading": -2.8485300950885154e-9,
-                    "angularVelocity": -3.6869616762863894e-8,
-                    "velocityX": -0.00000210437708878744,
-                    "velocityY": -2.395142078540767,
-                    "timestamp": 1.0532557401465126
+                    "x": 7.68,
+                    "y": 2.686336112982689,
+                    "heading": 1.1174854461234148e-18,
+                    "angularVelocity": -1.1541180553049724e-17,
+                    "velocityX": 7.218959232022663e-34,
+                    "velocityY": -2.3951420785410233,
+                    "timestamp": 1.0532557401466074
                 },
                 {
-                    "x": 7.680000075080448,
-                    "y": 2.533445372005505,
-                    "heading": -2.300788754937178e-9,
-                    "angularVelocity": 5.720505703316253e-9,
-                    "velocityX": -0.0000014860728228146123,
-                    "velocityY": -1.596761438503163,
-                    "timestamp": 1.1490062619780137
+                    "x": 7.68,
+                    "y": 2.5334453720055388,
+                    "heading": 3.7383227049768543e-19,
+                    "angularVelocity": -7.766570474567622e-18,
+                    "velocityX": 1.7872274250399634e-33,
+                    "velocityY": -1.5967614385035829,
+                    "timestamp": 1.1490062619781172
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
                     "heading": 0,
-                    "angularVelocity": 2.4028994457396085e-8,
-                    "velocityX": -7.841257148612366e-7,
-                    "velocityY": -0.7983807350943783,
-                    "timestamp": 1.2447567838095148
+                    "angularVelocity": -3.904232200157263e-18,
+                    "velocityX": 6.089132400706592e-33,
+                    "velocityY": -0.7983807350946677,
+                    "timestamp": 1.244756783809627
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
                     "heading": 0,
                     "angularVelocity": 0,
-                    "velocityX": -1.5050925658054699e-33,
-                    "velocityY": -8.233479718145206e-39,
-                    "timestamp": 1.340507305641016
+                    "velocityX": 3.5763678522980893e-43,
+                    "velocityY": -5.882608067207678e-38,
+                    "timestamp": 1.3405073056411367
                 }
             ],
             "constraints": [
@@ -28391,23 +34087,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "C2 To C5": {
             "waypoints": [
@@ -28435,172 +34115,172 @@
                     "x": 7.68,
                     "y": 5.797,
                     "heading": 0,
-                    "angularVelocity": 1.5719252729433342e-31,
-                    "velocityX": 9.448616775755806e-32,
-                    "velocityY": 5.840312874632397e-38,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": -1.2882449649302714e-32,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.680000115633079,
-                    "y": 5.715155846048254,
-                    "heading": -1.667708661616254e-8,
-                    "angularVelocity": -1.683296946208156e-7,
-                    "velocityX": 0.0000011671392662331341,
-                    "velocityY": -0.8260917454273093,
-                    "timestamp": 0.0990739278110157
-                },
-                {
-                    "x": 7.68000033829167,
-                    "y": 5.551467541258106,
-                    "heading": -5.4122540412155035e-8,
-                    "angularVelocity": -3.7795460569235657e-7,
-                    "velocityX": 0.0000022473982686017914,
-                    "velocityY": -1.6521834594301872,
-                    "timestamp": 0.1981478556220314
-                },
-                {
-                    "x": 7.680000659202128,
-                    "y": 5.305935091856244,
-                    "heading": -1.1439481389604968e-7,
-                    "angularVelocity": -6.083564268701089e-7,
-                    "velocityX": 0.000003239100713146307,
-                    "velocityY": -2.4782751105841556,
-                    "timestamp": 0.2972217834330471
-                },
-                {
-                    "x": 7.680001069396323,
-                    "y": 4.978558516522715,
-                    "heading": -1.9685469205745493e-7,
-                    "angularVelocity": -8.323062817894511e-7,
-                    "velocityX": 0.000004140283484089047,
-                    "velocityY": -3.304366573191614,
-                    "timestamp": 0.3962957112440628
-                },
-                {
-                    "x": 7.680001393540419,
-                    "y": 4.603767735073282,
-                    "heading": -1.9685469402018675e-7,
-                    "angularVelocity": -2.3695928725495928e-14,
-                    "velocityX": 0.0000032717397250257404,
-                    "velocityY": -3.782940575085891,
-                    "timestamp": 0.4953696390550785
-                },
-                {
-                    "x": 7.680001631663456,
-                    "y": 4.228976953623768,
-                    "heading": -1.9685469983118006e-7,
-                    "angularVelocity": -6.145037083886684e-14,
-                    "velocityX": 0.0000024034882723677366,
-                    "velocityY": -3.782940575086774,
-                    "timestamp": 0.5944435668660942
-                },
-                {
-                    "x": 7.680001783604786,
-                    "y": 3.854186172174223,
-                    "heading": -1.9685470647072664e-7,
-                    "angularVelocity": -7.008912946454424e-14,
-                    "velocityX": 0.0000015336156103104506,
-                    "velocityY": -3.78294057508712,
-                    "timestamp": 0.6935174946771099
-                },
-                {
-                    "x": 7.680001849310107,
-                    "y": 3.479395390724661,
-                    "heading": -1.9685471357202238e-7,
-                    "angularVelocity": -7.568241713719523e-14,
-                    "velocityX": 6.631948562175283e-7,
-                    "velocityY": -3.782940575087319,
-                    "timestamp": 0.7925914224881256
-                },
-                {
-                    "x": 7.680001829148679,
-                    "y": 3.104604609275093,
-                    "heading": -1.968547206040393e-7,
-                    "angularVelocity": -7.70206577850091e-14,
-                    "velocityX": -2.0349878104492754e-7,
-                    "velocityY": -3.7829405750873777,
-                    "timestamp": 0.8916653502991414
-                },
-                {
-                    "x": 7.680001725125185,
-                    "y": 2.729813827825532,
-                    "heading": -1.968547288641082e-7,
-                    "angularVelocity": -9.134372064218233e-14,
-                    "velocityX": -0.0000010499582046587897,
-                    "velocityY": -3.7829405750872978,
-                    "timestamp": 0.9907392781101572
-                },
-                {
-                    "x": 7.680001540827678,
-                    "y": 2.3550230463759907,
-                    "heading": -1.9685473554016492e-7,
-                    "angularVelocity": -7.335754268551387e-14,
-                    "velocityX": -0.0000018602017039855543,
-                    "velocityY": -3.7829405750870753,
-                    "timestamp": 1.089813205921173
-                },
-                {
-                    "x": 7.680001281411145,
-                    "y": 1.980232264926482,
-                    "heading": -1.9685473620018866e-7,
-                    "angularVelocity": -6.8770489517299035e-15,
-                    "velocityX": -0.0000026184135239642236,
-                    "velocityY": -3.782940575086706,
-                    "timestamp": 1.1888871337321887
-                },
-                {
-                    "x": 7.680000953971803,
-                    "y": 1.6054414834770692,
-                    "heading": -1.968547355090094e-7,
-                    "angularVelocity": 1.0827757762815176e-14,
-                    "velocityX": -0.000003305000098197799,
-                    "velocityY": -3.7829405750856786,
-                    "timestamp": 1.2879610615432044
-                },
-                {
-                    "x": 7.6800005831051426,
-                    "y": 1.2780649081436681,
-                    "heading": -1.1900708358832985e-7,
-                    "angularVelocity": 7.857529072428817e-7,
-                    "velocityX": -0.0000037433322544868336,
-                    "velocityY": -3.30436657319033,
-                    "timestamp": 1.3870349893542202
-                },
-                {
-                    "x": 7.680000296749478,
-                    "y": 1.0325324587418607,
-                    "heading": -5.959102520162259e-8,
-                    "angularVelocity": 5.997142524637507e-7,
-                    "velocityX": -0.000002890322961269868,
-                    "velocityY": -2.478275110583629,
-                    "timestamp": 1.486108917165236
-                },
+                    "x": 7.68,
+                    "y": 5.715155846048237,
+                    "heading": -2.6675793503802873e-18,
+                    "angularVelocity": -2.692513973443352e-17,
+                    "velocityX": 2.411941793716939e-19,
+                    "velocityY": -0.8260917454276263,
+                    "timestamp": 0.09907392781100396
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.55146754125807,
+                    "heading": -8.002991694377614e-18,
+                    "angularVelocity": -5.385283961043983e-17,
+                    "velocityX": 4.822926900034719e-19,
+                    "velocityY": -1.6521834594306495,
+                    "timestamp": 0.19814785562200793
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.305935091856216,
+                    "heading": -1.6007101079545214e-17,
+                    "angularVelocity": -8.07892607262788e-17,
+                    "velocityX": 7.23199861883135e-19,
+                    "velocityY": -2.478275110584479,
+                    "timestamp": 0.29722178343301187
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.978558516522808,
+                    "heading": -2.6680861930648114e-17,
+                    "angularVelocity": -1.0773531530269989e-16,
+                    "velocityX": 9.635330187135107e-19,
+                    "velocityY": -3.3043665731908765,
+                    "timestamp": 0.39629571124401586
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.603767735073314,
+                    "heading": -3.033567109313287e-17,
+                    "angularVelocity": -3.688971703490728e-17,
+                    "velocityX": 1.1102275506466142e-18,
+                    "velocityY": -3.782940575087071,
+                    "timestamp": 0.49536963905501985
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.228976953623796,
+                    "heading": -3.5529792774719166e-17,
+                    "angularVelocity": -5.2426726146536295e-17,
+                    "velocityX": 1.1087814850277944e-18,
+                    "velocityY": -3.7829405750873115,
+                    "timestamp": 0.5944435668660238
+                },
+                {
+                    "x": 7.68,
+                    "y": 3.8541861721742774,
+                    "heading": -4.8775545061581196e-17,
+                    "angularVelocity": -1.3369564101798844e-16,
+                    "velocityX": 1.1066129122730268e-18,
+                    "velocityY": -3.782940575087312,
+                    "timestamp": 0.6935174946770278
+                },
+                {
+                    "x": 7.68,
+                    "y": 3.4793953907247595,
+                    "heading": -3.3383639047364345e-17,
+                    "angularVelocity": 1.5535778538404392e-16,
+                    "velocityX": 1.113993391664377e-18,
+                    "velocityY": -3.782940575087312,
+                    "timestamp": 0.7925914224880318
+                },
+                {
+                    "x": 7.68,
+                    "y": 3.104604609275241,
+                    "heading": -4.339521377299963e-18,
+                    "angularVelocity": 2.9315601300747485e-16,
+                    "velocityX": 1.114171335036007e-18,
+                    "velocityY": -3.782940575087312,
+                    "timestamp": 0.8916653502990358
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.7298138278257227,
+                    "heading": 1.3174711336089981e-17,
+                    "angularVelocity": 1.7677943229992181e-16,
+                    "velocityX": 1.1158489583207238e-18,
+                    "velocityY": -3.782940575087312,
+                    "timestamp": 0.9907392781100398
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.3550230463762043,
+                    "heading": 2.965421117619106e-17,
+                    "angularVelocity": 1.6633538413393434e-16,
+                    "velocityX": 1.1181357718618657e-18,
+                    "velocityY": -3.782940575087312,
+                    "timestamp": 1.0898132059210437
+                },
+                {
+                    "x": 7.68,
+                    "y": 1.9802322649266861,
+                    "heading": 2.902288280090785e-17,
+                    "angularVelocity": -6.372295813531433e-18,
+                    "velocityX": 1.121394742098521e-18,
+                    "velocityY": -3.7829405750873115,
+                    "timestamp": 1.1888871337320477
+                },
                 {
-                    "x": 7.680000100604838,
-                    "y": 0.8688441539517371,
-                    "heading": -1.9801599569632452e-8,
-                    "angularVelocity": 4.016134309966653e-7,
-                    "velocityX": -0.000001979780510399852,
-                    "velocityY": -1.6521834594299551,
-                    "timestamp": 1.5851828449762517
+                    "x": 7.68,
+                    "y": 1.605441483477192,
+                    "heading": 2.667314494747493e-17,
+                    "angularVelocity": -2.3717015216307205e-17,
+                    "velocityX": 1.1050727300415562e-18,
+                    "velocityY": -3.782940575087071,
+                    "timestamp": 1.2879610615430517
+                },
+                {
+                    "x": 7.68,
+                    "y": 1.2780649081437845,
+                    "heading": 1.6004201006274906e-17,
+                    "angularVelocity": -1.0768669595616384e-16,
+                    "velocityX": 9.663666503471993e-19,
+                    "velocityY": -3.304366573190877,
+                    "timestamp": 1.3870349893540557
+                },
+                {
+                    "x": 7.68,
+                    "y": 1.0325324587419298,
+                    "heading": 8.002125258512226e-18,
+                    "angularVelocity": -8.076873426351199e-17,
+                    "velocityX": 7.247750412790524e-19,
+                    "velocityY": -2.478275110584479,
+                    "timestamp": 1.4861089171650597
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.868844153951763,
+                    "heading": 2.6673738748743342e-18,
+                    "angularVelocity": -5.384616822543564e-17,
+                    "velocityX": 4.831833763651305e-19,
+                    "velocityY": -1.6521834594306495,
+                    "timestamp": 1.5851828449760637
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": -3.727510081119492e-33,
-                    "angularVelocity": 1.998668869584927e-7,
-                    "velocityX": -0.0000010154521677217573,
-                    "velocityY": -0.8260917454272346,
-                    "timestamp": 1.6842567727872675
+                    "heading": 0,
+                    "angularVelocity": -2.6923065773077948e-17,
+                    "velocityX": 2.415916928361511e-19,
+                    "velocityY": -0.8260917454276263,
+                    "timestamp": 1.6842567727870676
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
                     "heading": 0,
-                    "angularVelocity": 1.3621739357478042e-33,
-                    "velocityX": 0,
-                    "velocityY": 1.5851260517441878e-39,
-                    "timestamp": 1.7833307005982832
+                    "angularVelocity": 0,
+                    "velocityX": -1.1487176577652646e-33,
+                    "velocityY": 0,
+                    "timestamp": 1.7833307005980716
                 }
             ],
             "constraints": [
@@ -28620,23 +34300,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "C3 To W1": {
             "waypoints": [
@@ -28659,7 +34323,224 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": 1.832751405105417e-34,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.608571974046609,
+                    "y": 4.129750388677263,
+                    "heading": -0.02440208863451916,
+                    "angularVelocity": -0.26295738388657636,
+                    "velocityX": -0.7697098032139116,
+                    "velocityY": 0.029638242122484983,
+                    "timestamp": 0.09279864392416992
+                },
+                {
+                    "x": 7.46571594445247,
+                    "y": 4.135252523590067,
+                    "heading": -0.07320447592180841,
+                    "angularVelocity": -0.5258954788948677,
+                    "velocityX": -1.5394193659863489,
+                    "velocityY": 0.05929111331948823,
+                    "timestamp": 0.18559728784833984
+                },
+                {
+                    "x": 7.25143160309568,
+                    "y": 4.143508368339934,
+                    "heading": -0.1463869418885076,
+                    "angularVelocity": -0.7886156830752087,
+                    "velocityX": -2.3091322490864354,
+                    "velocityY": 0.08896514432486853,
+                    "timestamp": 0.27839593177250976
+                },
+                {
+                    "x": 6.965717910609067,
+                    "y": 4.154520147774138,
+                    "heading": -0.24389107124776152,
+                    "angularVelocity": -1.050706402983198,
+                    "velocityX": -3.078856332427471,
+                    "velocityY": 0.1186631503279318,
+                    "timestamp": 0.3711945756966797
+                },
+                {
+                    "x": 6.614926201978592,
+                    "y": 4.168029747138803,
+                    "heading": -0.24389108357148506,
+                    "angularVelocity": -1.3280068591073525e-7,
+                    "velocityX": -3.780138305869253,
+                    "velocityY": 0.1455797067002448,
+                    "timestamp": 0.4639932196208496
+                },
+                {
+                    "x": 6.264134491970206,
+                    "y": 4.181539310738243,
+                    "heading": -0.2438910958936344,
+                    "angularVelocity": -1.3278372185354406e-7,
+                    "velocityX": -3.7801383207176356,
+                    "velocityY": 0.14557932129349355,
+                    "timestamp": 0.5567918635450195
+                },
+                {
+                    "x": 5.913342805476888,
+                    "y": 4.195049484934711,
+                    "heading": -0.2438911082157909,
+                    "angularVelocity": -1.32783799697389e-7,
+                    "velocityX": -3.780138067318815,
+                    "velocityY": 0.14558590109896277,
+                    "timestamp": 0.6495905074691894
+                },
+                {
+                    "x": 5.568285864608835,
+                    "y": 4.259648543779433,
+                    "heading": -0.24389114467773698,
+                    "angularVelocity": -3.929146432741106e-7,
+                    "velocityX": -3.718340336417024,
+                    "velocityY": 0.6961207202291138,
+                    "timestamp": 0.7423891513933594
+                },
+                {
+                    "x": 5.223229300660808,
+                    "y": 4.324249615558004,
+                    "heading": -0.2438911811402195,
+                    "angularVelocity": -3.929204239211486e-7,
+                    "velocityX": -3.7183362747195923,
+                    "velocityY": 0.6961424116429128,
+                    "timestamp": 0.8351877953175293
+                },
+                {
+                    "x": 4.889178228675737,
+                    "y": 4.432171862538056,
+                    "heading": -0.2438912192984801,
+                    "angularVelocity": -4.111941617608203e-7,
+                    "velocityX": -3.5997408783046705,
+                    "velocityY": 1.1629722420106654,
+                    "timestamp": 0.9279864392416992
+                },
+                {
+                    "x": 4.584073695522117,
+                    "y": 4.605805251226935,
+                    "heading": -0.24389116821114407,
+                    "angularVelocity": 5.505181312773355e-7,
+                    "velocityX": -3.287812410307864,
+                    "velocityY": 1.8710767889103372,
+                    "timestamp": 1.0207850831658691
+                },
+                {
+                    "x": 4.320679858830311,
+                    "y": 4.837885010533044,
+                    "heading": -0.24389110572504064,
+                    "angularVelocity": 6.733514712028696e-7,
+                    "velocityX": -2.8383371303038154,
+                    "velocityY": 2.5008960205899418,
+                    "timestamp": 1.113583727090039
+                },
+                {
+                    "x": 4.066259556785684,
+                    "y": 5.079768532204951,
+                    "heading": -0.24389104453640137,
+                    "angularVelocity": 6.593699721479525e-7,
+                    "velocityX": -2.7416381456233947,
+                    "velocityY": 2.606541555386963,
+                    "timestamp": 1.206382371014209
+                },
+                {
+                    "x": 3.8118393280605467,
+                    "y": 5.321652130996455,
+                    "heading": -0.243890983347767,
+                    "angularVelocity": 6.593699195322184e-7,
+                    "velocityX": -2.741637355531139,
+                    "velocityY": 2.606542386429263,
+                    "timestamp": 1.299181014938379
+                },
+                {
+                    "x": 3.557419123484207,
+                    "y": 5.563535755188363,
+                    "heading": -0.24389092215913197,
+                    "angularVelocity": 6.593699267467298e-7,
+                    "velocityX": -2.7416370953032203,
+                    "velocityY": 2.606542660144498,
+                    "timestamp": 1.3919796588625488
+                },
+                {
+                    "x": 3.3029989365055474,
+                    "y": 5.805419397890013,
+                    "heading": -0.24389086097049373,
+                    "angularVelocity": 6.593699609263459e-7,
+                    "velocityX": -2.7416369056702816,
+                    "velocityY": 2.606542859605842,
+                    "timestamp": 1.4847783027867187
+                },
+                {
+                    "x": 3.0485787435735707,
+                    "y": 6.04730303432977,
+                    "heading": -0.24389079978182,
+                    "angularVelocity": 6.593703437871394e-7,
+                    "velocityX": -2.741636969823341,
+                    "velocityY": 2.6065427921275584,
+                    "timestamp": 1.5775769467108887
+                },
+                {
+                    "x": 2.7984729856087975,
+                    "y": 6.275491677578651,
+                    "heading": -0.21231080129448254,
+                    "angularVelocity": 0.3403066807004949,
+                    "velocityX": -2.6951445343225506,
+                    "velocityY": 2.45896527793385,
+                    "timestamp": 1.6703755906350586
+                },
+                {
+                    "x": 2.596978431795094,
+                    "y": 6.469397893884434,
+                    "heading": -0.03732218251039644,
+                    "angularVelocity": 1.8856807748945335,
+                    "velocityX": -2.171309248638947,
+                    "velocityY": 2.089537175394848,
+                    "timestamp": 1.7631742345592285
+                },
+                {
+                    "x": 2.4472498512530994,
+                    "y": 6.613867328015145,
+                    "heading": 0.15789573469158452,
+                    "angularVelocity": 2.1036720898801096,
+                    "velocityX": -1.6134781092743464,
+                    "velocityY": 1.5568054447946784,
+                    "timestamp": 1.8559728784833984
+                },
+                {
+                    "x": 2.3487140682465175,
+                    "y": 6.7094536627437265,
+                    "heading": 0.3099829221841124,
+                    "angularVelocity": 1.6388945038552638,
+                    "velocityX": -1.0618235228426263,
+                    "velocityY": 1.0300402105734285,
+                    "timestamp": 1.9487715224075683
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 0.393,
+                    "angularVelocity": 0.8945936525078311,
+                    "velocityX": -0.5249437511858832,
+                    "velocityY": 0.5123602592202229,
+                    "timestamp": 2.0415701663317383
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 0.393,
+                    "angularVelocity": 0,
+                    "velocityX": 1.4564927652554838e-31,
+                    "velocityY": 0,
+                    "timestamp": 2.134368810255908
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -28678,16 +34559,6 @@
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
             "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
                 {
                     "x": 5.61,
                     "y": 5.44,
@@ -29840,7 +35711,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 18
+                    "controlIntervalCount": 12
+                },
+                {
+                    "x": 6.792231559753418,
+                    "y": 1.766543984413147,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 13
                 },
                 {
                     "x": 5.176,
@@ -29852,7 +35732,242 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 0,
+                    "angularVelocity": 3.185769476987751e-31,
+                    "velocityX": 7.333541525795099e-32,
+                    "velocityY": -1.3517842061565195e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.679078910576486,
+                    "y": 4.080614551713116,
+                    "heading": -0.023748331346269917,
+                    "angularVelocity": -0.3167839884646853,
+                    "velocityX": -0.012286605616975374,
+                    "velocityY": -0.6187452541731726,
+                    "timestamp": 0.07496695606800095
+                },
+                {
+                    "x": 7.6772274783441,
+                    "y": 3.987757014244699,
+                    "heading": -0.06889026621284731,
+                    "angularVelocity": -0.6021577670253255,
+                    "velocityX": -0.024696644088144176,
+                    "velocityY": -1.2386462294692595,
+                    "timestamp": 0.1499339121360019
+                },
+                {
+                    "x": 7.674435217742111,
+                    "y": 3.848312298051719,
+                    "heading": -0.1318457680879735,
+                    "angularVelocity": -0.8397766853177616,
+                    "velocityX": -0.037246551660047825,
+                    "velocityY": -1.8600824083946148,
+                    "timestamp": 0.22490086820400285
+                },
+                {
+                    "x": 7.670687411814683,
+                    "y": 3.6621317365631327,
+                    "heading": -0.20653378464827088,
+                    "angularVelocity": -0.996279167218362,
+                    "velocityX": -0.049992771802326974,
+                    "velocityY": -2.483501681990474,
+                    "timestamp": 0.2998678242720038
+                },
+                {
+                    "x": 7.665952301925262,
+                    "y": 3.4291021726627307,
+                    "heading": -0.27999342706200125,
+                    "angularVelocity": -0.9798936260280143,
+                    "velocityX": -0.06316262707958344,
+                    "velocityY": -3.1084303821676644,
+                    "timestamp": 0.3748347803400048
+                },
+                {
+                    "x": 7.659724245559776,
+                    "y": 3.1517739981457797,
+                    "heading": -0.2973184560258276,
+                    "angularVelocity": -0.23110220652567398,
+                    "velocityX": -0.0830773542389579,
+                    "velocityY": -3.699338869586643,
+                    "timestamp": 0.44980173640800575
+                },
+                {
+                    "x": 7.61848609595722,
+                    "y": 2.8711927485616604,
+                    "heading": -0.2973184850947423,
+                    "angularVelocity": -3.8775636846908307e-7,
+                    "velocityX": -0.5500843540339733,
+                    "velocityY": -3.742732322352917,
+                    "timestamp": 0.5247686924760067
+                },
+                {
+                    "x": 7.5316071002697536,
+                    "y": 2.6012326390018865,
+                    "heading": -0.2973185190564447,
+                    "angularVelocity": -4.530222940715824e-7,
+                    "velocityX": -1.1588972027711093,
+                    "velocityY": -3.6010547008857787,
+                    "timestamp": 0.5997356485440076
+                },
+                {
+                    "x": 7.401459139481823,
+                    "y": 2.3492645367371554,
+                    "heading": -0.29731856454052086,
+                    "angularVelocity": -6.067216612944958e-7,
+                    "velocityX": -1.7360710320141561,
+                    "velocityY": -3.3610555300681284,
+                    "timestamp": 0.6747026046120086
+                },
+                {
+                    "x": 7.231595664022054,
+                    "y": 2.1221680676798096,
+                    "heading": -0.2973186386883636,
+                    "angularVelocity": -9.89073675332688e-7,
+                    "velocityX": -2.265844638345654,
+                    "velocityY": -3.0292875817359826,
+                    "timestamp": 0.7496695606800096
+                },
+                {
+                    "x": 7.026654548839687,
+                    "y": 1.9261437915792998,
+                    "heading": -0.2973188151543491,
+                    "angularVelocity": -0.000002353916908579506,
+                    "velocityX": -2.7337526549226907,
+                    "velocityY": -2.614809062311352,
+                    "timestamp": 0.8246365167480105
+                },
+                {
+                    "x": 6.792231559753418,
+                    "y": 1.766543984413147,
+                    "heading": -0.2973195709317279,
+                    "angularVelocity": -0.00001008147347055336,
+                    "velocityX": -3.127017573897922,
+                    "velocityY": -2.1289354075064555,
+                    "timestamp": 0.8996034728160115
+                },
+                {
+                    "x": 6.610935711613446,
+                    "y": 1.6724691195207,
+                    "heading": -0.2973321063250889,
+                    "angularVelocity": -0.00023216320074468077,
+                    "velocityX": -3.3577106919370485,
+                    "velocityY": -1.7423243992220112,
+                    "timestamp": 0.9535973587619662
+                },
+                {
+                    "x": 6.419882816475647,
+                    "y": 1.600658398177247,
+                    "heading": -0.29770902924967546,
+                    "angularVelocity": -0.006980844552986395,
+                    "velocityX": -3.538417207626563,
+                    "velocityY": -1.329978757508455,
+                    "timestamp": 1.007591244707921
+                },
+                {
+                    "x": 6.223144239006287,
+                    "y": 1.5524565272909876,
+                    "heading": -0.30199019992690734,
+                    "angularVelocity": -0.07928991592723345,
+                    "velocityX": -3.6437195438439516,
+                    "velocityY": -0.892728316211746,
+                    "timestamp": 1.0615851306538757
+                },
+                {
+                    "x": 6.027009837481886,
+                    "y": 1.528287215105613,
+                    "heading": -0.31920772077283077,
+                    "angularVelocity": -0.31887908314582397,
+                    "velocityX": -3.63252983348375,
+                    "velocityY": -0.44763053745692566,
+                    "timestamp": 1.1155790165998305
+                },
+                {
+                    "x": 5.840489459577601,
+                    "y": 1.5253276945335545,
+                    "heading": -0.3622266817701244,
+                    "angularVelocity": -0.7967376350788925,
+                    "velocityX": -3.454472198777908,
+                    "velocityY": -0.05481214252704887,
+                    "timestamp": 1.1695729025457853
+                },
+                {
+                    "x": 5.673855769295343,
+                    "y": 1.5359985529910274,
+                    "heading": -0.41631972448131044,
+                    "angularVelocity": -1.0018364443214895,
+                    "velocityX": -3.086158504113808,
+                    "velocityY": 0.19763086635687102,
+                    "timestamp": 1.22356678849174
+                },
+                {
+                    "x": 5.530708201179271,
+                    "y": 1.5529237053226916,
+                    "heading": -0.47196047387840134,
+                    "angularVelocity": -1.0305009247305976,
+                    "velocityX": -2.651181066303626,
+                    "velocityY": 0.3134642383140704,
+                    "timestamp": 1.2775606744376948
+                },
+                {
+                    "x": 5.411784437145291,
+                    "y": 1.57153818466476,
+                    "heading": -0.5236032237572267,
+                    "angularVelocity": -0.956455512954414,
+                    "velocityX": -2.2025413053807186,
+                    "velocityY": 0.34475161429762524,
+                    "timestamp": 1.3315545603836496
+                },
+                {
+                    "x": 5.3170603467926,
+                    "y": 1.5890782395606275,
+                    "heading": -0.5679992830659408,
+                    "angularVelocity": -0.8222423433861833,
+                    "velocityX": -1.754348454332506,
+                    "velocityY": 0.32485261226475975,
+                    "timestamp": 1.3855484463296044
+                },
+                {
+                    "x": 5.246334967071984,
+                    "y": 1.6037618746917708,
+                    "heading": -0.6030801100679523,
+                    "angularVelocity": -0.6497185077054303,
+                    "velocityX": -1.309877562644932,
+                    "velocityY": 0.2719499601462664,
+                    "timestamp": 1.4395423322755592
+                },
+                {
+                    "x": 5.199384048260516,
+                    "y": 1.6143645569232876,
+                    "heading": -0.6274260701015184,
+                    "angularVelocity": -0.45090216432919045,
+                    "velocityX": -0.8695599138477248,
+                    "velocityY": 0.19636820069091412,
+                    "timestamp": 1.493536218221514
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": -0.64,
+                    "angularVelocity": -0.23287692075121041,
+                    "velocityX": -0.4330869662524824,
+                    "velocityY": 0.10437187429616973,
+                    "timestamp": 1.5475301041674687
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": -0.64,
+                    "angularVelocity": 0,
+                    "velocityX": 3.9288115092123874e-33,
+                    "velocityY": 2.0888895254257856e-32,
+                    "timestamp": 1.6015239901134235
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -29871,20 +35986,10 @@
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
             "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
                 {
                     "x": 5.61,
                     "y": 2.81,
                     "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
                 }
             ]
         },
@@ -29914,136 +36019,136 @@
                     "x": 7.68,
                     "y": 4.127,
                     "heading": 0,
-                    "angularVelocity": 1.212112656390561e-31,
-                    "velocityX": -5.041767818814111e-32,
-                    "velocityY": -1.2487171964671538e-38,
+                    "angularVelocity": -2.1749467140472343e-39,
+                    "velocityX": 0,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.6800000701104585,
-                    "y": 4.20344537200552,
-                    "heading": 1.2930383103747041e-8,
-                    "angularVelocity": 1.350424233383134e-7,
-                    "velocityX": 7.322200986357225e-7,
-                    "velocityY": 0.7983807350945226,
-                    "timestamp": 0.09575052183150286
-                },
+                    "x": 7.68,
+                    "y": 4.203445372005539,
+                    "heading": -7.837837605587582e-19,
+                    "angularVelocity": -8.185686568731239e-18,
+                    "velocityX": 1.377912473786286e-33,
+                    "velocityY": 0.7983807350946673,
+                    "timestamp": 0.09575052183150974
+                },
                 {
-                    "x": 7.680000201769031,
-                    "y": 4.356336112982647,
-                    "heading": 3.8758528041627975e-8,
-                    "angularVelocity": 2.6974415299903026e-7,
-                    "velocityX": 0.0000013750167071257275,
-                    "velocityY": 1.596761438503451,
-                    "timestamp": 0.19150104366300572
+                    "x": 7.68,
+                    "y": 4.356336112982689,
+                    "heading": -2.3565356003209568e-18,
+                    "angularVelocity": -1.6425517161035916e-17,
+                    "velocityX": 3.260516050321855e-34,
+                    "velocityY": 1.5967614385035824,
+                    "timestamp": 0.19150104366301948
                 },
                 {
-                    "x": 7.680000386416211,
-                    "y": 4.58567221686356,
-                    "heading": 7.786709578005107e-8,
-                    "angularVelocity": 4.0844231330927475e-7,
-                    "velocityX": 0.000001928419471656301,
-                    "velocityY": 2.395142078541207,
-                    "timestamp": 0.2872515654945086
+                    "x": 7.68,
+                    "y": 4.5856722168636,
+                    "heading": -4.728579560129296e-18,
+                    "angularVelocity": -2.4773170043438095e-17,
+                    "velocityX": -1.693775140889528e-33,
+                    "velocityY": 2.3951420785410225,
+                    "timestamp": 0.28725156549452924
                 },
                 {
-                    "x": 7.6800006155005685,
-                    "y": 4.891453665444849,
-                    "heading": 1.3155130559553437e-7,
-                    "angularVelocity": 5.606674749182053e-7,
-                    "velocityX": 0.0000023925127342176106,
-                    "velocityY": 3.1935225284661,
-                    "timestamp": 0.38300208732601143
+                    "x": 7.68,
+                    "y": 4.891453665444746,
+                    "heading": -7.932188825954499e-18,
+                    "angularVelocity": -3.345787785122934e-17,
+                    "velocityX": -9.607950389092107e-34,
+                    "velocityY": 3.1935225284643685,
+                    "timestamp": 0.38300208732603896
                 },
                 {
-                    "x": 7.68000074582338,
-                    "y": 5.253672199267105,
-                    "heading": 1.3155130364873708e-7,
-                    "angularVelocity": -1.7664134647076396e-14,
-                    "velocityX": 0.000001361066521463048,
-                    "velocityY": 3.7829405719549913,
-                    "timestamp": 0.4787526091575143
+                    "x": 7.68,
+                    "y": 5.253672199266821,
+                    "heading": -4.35728250993897e-17,
+                    "angularVelocity": -3.722239377050204e-16,
+                    "velocityX": -3.660377688643743e-33,
+                    "velocityY": 3.7829405719528495,
+                    "timestamp": 0.4787526091575487
                 },
                 {
-                    "x": 7.680000798333519,
-                    "y": 5.615890733089199,
-                    "heading": 1.315513137062799e-7,
-                    "angularVelocity": 1.0799702172100096e-13,
-                    "velocityX": 5.484057877231364e-7,
-                    "velocityY": 3.7829405719533282,
-                    "timestamp": 0.5745031309890172
+                    "x": 7.68,
+                    "y": 5.61589073308894,
+                    "heading": -3.405116375197899e-17,
+                    "angularVelocity": 9.944239639932065e-17,
+                    "velocityX": -5.791062266320423e-33,
+                    "velocityY": 3.782940571953311,
+                    "timestamp": 0.5745031309890585
                 },
                 {
-                    "x": 7.680000776767743,
-                    "y": 5.9781092669112965,
-                    "heading": 1.3155132085570774e-7,
-                    "angularVelocity": 7.667783798806564e-14,
-                    "velocityX": -2.2522878887411694e-7,
-                    "velocityY": 3.782940571953354,
-                    "timestamp": 0.6702536528205201
+                    "x": 7.68,
+                    "y": 5.978109266911059,
+                    "heading": 5.876472199762529e-18,
+                    "angularVelocity": 4.169965362904391e-16,
+                    "velocityX": -1.6171107443968654e-32,
+                    "velocityY": 3.782940571953311,
+                    "timestamp": 0.6702536528205683
                 },
                 {
-                    "x": 7.680000686239199,
-                    "y": 6.340327800733382,
-                    "heading": 1.315513220333048e-7,
-                    "angularVelocity": 1.2578458101628908e-14,
-                    "velocityX": -9.454626397270524e-7,
-                    "velocityY": 3.78294057195323,
-                    "timestamp": 0.766004174652023
+                    "x": 7.68,
+                    "y": 6.340327800733179,
+                    "heading": 2.547705878984039e-17,
+                    "angularVelocity": 2.047047495424479e-16,
+                    "velocityX": -4.3724642588945734e-33,
+                    "velocityY": 3.782940571953311,
+                    "timestamp": 0.766004174652078
                 },
                 {
-                    "x": 7.680000533602743,
-                    "y": 6.702546334555452,
-                    "heading": 1.3155133191379775e-7,
-                    "angularVelocity": 1.0229175193513549e-13,
-                    "velocityX": -0.0000015941058350287079,
-                    "velocityY": 3.7829405719530396,
-                    "timestamp": 0.8617546964835259
+                    "x": 7.68,
+                    "y": 6.702546334555254,
+                    "heading": 7.687233753276096e-18,
+                    "angularVelocity": -1.8579350479017399e-16,
+                    "velocityX": -2.6103711792186515e-33,
+                    "velocityY": 3.7829405719528495,
+                    "timestamp": 0.8617546964835878
                 },
                 {
-                    "x": 7.680000329705533,
-                    "y": 7.00832778313656,
-                    "heading": 7.923155128573452e-8,
-                    "angularVelocity": -5.46417642222627e-7,
-                    "velocityX": -0.000002129463080315811,
-                    "velocityY": 3.1935225284642037,
-                    "timestamp": 0.9575052183150288
+                    "x": 7.68,
+                    "y": 7.0083277831364,
+                    "heading": 4.613320516549708e-18,
+                    "angularVelocity": -3.210335754994756e-17,
+                    "velocityX": 1.2273832122580678e-32,
+                    "velocityY": 3.1935225284643685,
+                    "timestamp": 0.9575052183150976
                 },
                 {
-                    "x": 7.680000169486885,
-                    "y": 7.2376638870173995,
-                    "heading": 3.947148220681983e-8,
-                    "angularVelocity": -4.152464695661009e-7,
-                    "velocityX": -0.0000016732926513675465,
-                    "velocityY": 2.395142078540454,
-                    "timestamp": 1.0532557401465317
+                    "x": 7.68,
+                    "y": 7.237663887017311,
+                    "heading": 2.3068261479777784e-18,
+                    "angularVelocity": -2.4088582761110343e-17,
+                    "velocityX": 8.179933156696061e-33,
+                    "velocityY": 2.3951420785410225,
+                    "timestamp": 1.0532557401466074
                 },
                 {
-                    "x": 7.680000058003377,
-                    "y": 7.390554627994493,
-                    "heading": 1.303400507527686e-8,
-                    "angularVelocity": -2.7610790251013916e-7,
-                    "velocityX": -0.000001164312243995105,
-                    "velocityY": 1.5967614385030973,
-                    "timestamp": 1.1490062619780346
+                    "x": 7.68,
+                    "y": 7.390554627994461,
+                    "heading": 7.689558058679801e-19,
+                    "angularVelocity": -1.6061221530256843e-17,
+                    "velocityX": 9.843160057308665e-33,
+                    "velocityY": 1.5967614385035824,
+                    "timestamp": 1.1490062619781172
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
                     "heading": 0,
-                    "angularVelocity": -1.3612463214689187e-7,
-                    "velocityX": -6.057760896118273e-7,
-                    "velocityY": 0.7983807350943913,
-                    "timestamp": 1.2447567838095375
+                    "angularVelocity": -8.030826271263815e-18,
+                    "velocityX": 3.1854559448557055e-33,
+                    "velocityY": 0.7983807350946673,
+                    "timestamp": 1.244756783809627
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
                     "heading": 0,
                     "angularVelocity": 0,
-                    "velocityX": 1.2167404646059922e-31,
-                    "velocityY": -2.3530958630073903e-37,
-                    "timestamp": 1.3405073056410404
+                    "velocityX": 2.257351446612169e-43,
+                    "velocityY": -1.1946741683702123e-38,
+                    "timestamp": 1.3405073056411367
                 }
             ],
             "constraints": [
@@ -30063,23 +36168,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "C3 To C2": {
             "waypoints": [
@@ -30102,7 +36191,98 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": -3.817834980565894e-40,
+                    "angularVelocity": -1.9855221195343998e-39,
+                    "velocityX": -9.621145750064654e-40,
+                    "velocityY": 1.4575297691012433e-39,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.210500005575846,
+                    "heading": 3.479114156010015e-22,
+                    "angularVelocity": 3.4766412561552746e-21,
+                    "velocityX": 2.7068848151306367e-33,
+                    "velocityY": 0.8344065481228213,
+                    "timestamp": 0.10007112931184182
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.377500013629847,
+                    "heading": 1.081691491825702e-21,
+                    "angularVelocity": 7.332585052389997e-21,
+                    "velocityX": 5.1589068200097806e-33,
+                    "velocityY": 1.6688130652907358,
+                    "timestamp": 0.20014225862368365
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.628000017966616,
+                    "heading": 2.2511586271387426e-21,
+                    "angularVelocity": 1.1686358860616172e-20,
+                    "velocityX": 5.8498916949463956e-33,
+                    "velocityY": 2.503219520548835,
+                    "timestamp": 0.3002133879355255
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.962,
+                    "heading": 4.481673966100434e-21,
+                    "angularVelocity": 2.228929929167172e-20,
+                    "velocityX": 6.576791251337367e-33,
+                    "velocityY": 3.3376257900775013,
+                    "timestamp": 0.4002845172473673
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.295999982033384,
+                    "heading": 1.4391637904192512e-21,
+                    "angularVelocity": -3.0403475801105514e-20,
+                    "velocityX": 1.0509973010204301e-32,
+                    "velocityY": 3.3376257900775013,
+                    "timestamp": 0.5003556465592092
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.5464999863701525,
+                    "heading": 4.7457500532831525e-22,
+                    "angularVelocity": -9.63903176962576e-21,
+                    "velocityX": 7.983324746463204e-33,
+                    "velocityY": 2.503219520548835,
+                    "timestamp": 0.600426775871051
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.713499994424153,
+                    "heading": 1.0909640932435146e-22,
+                    "angularVelocity": -3.652188228166023e-21,
+                    "velocityX": 7.341551448907972e-33,
+                    "velocityY": 1.6688130652907358,
+                    "timestamp": 0.7004979051828928
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": -9.42965601107167e-40,
+                    "angularVelocity": -1.09018866193586e-21,
+                    "velocityX": 7.44470103227488e-33,
+                    "velocityY": 0.8344065481228213,
+                    "timestamp": 0.8005690344947346
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 8.249839679439821e-43,
+                    "angularVelocity": 9.431229878048832e-39,
+                    "velocityX": 1.012759498983901e-41,
+                    "velocityY": 5.142787784847508e-39,
+                    "timestamp": 0.9006401638065764
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -30120,23 +36300,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "C3 To C4": {
             "waypoints": [
@@ -30159,7 +36323,98 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": -1.6773104267984464e-40,
+                    "angularVelocity": -1.9855220924111835e-39,
+                    "velocityX": -5.3948128499400994e-40,
+                    "velocityY": -2.7550648847397363e-40,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.043499994424153,
+                    "heading": -2.181197470484939e-22,
+                    "angularVelocity": -2.1796471324426728e-21,
+                    "velocityX": 2.2751291494664054e-33,
+                    "velocityY": -0.8344065481228213,
+                    "timestamp": 0.10007112931184182
+                },
+                {
+                    "x": 7.68,
+                    "y": 3.8764999863701526,
+                    "heading": -1.0976856482379068e-21,
+                    "angularVelocity": -8.789407177499062e-21,
+                    "velocityX": 3.967849397509383e-33,
+                    "velocityY": -1.668813065290736,
+                    "timestamp": 0.20014225862368365
+                },
+                {
+                    "x": 7.68,
+                    "y": 3.6259999820333837,
+                    "heading": -3.357689035831156e-21,
+                    "angularVelocity": -2.2583970074080117e-20,
+                    "velocityX": 6.1318806903820435e-33,
+                    "velocityY": -2.503219520548835,
+                    "timestamp": 0.3002133879355255
+                },
+                {
+                    "x": 7.68,
+                    "y": 3.2919999999999994,
+                    "heading": -1.0603568936345678e-20,
+                    "angularVelocity": -7.2407296109284e-20,
+                    "velocityX": 8.195284274358776e-33,
+                    "velocityY": -3.3376257900775017,
+                    "timestamp": 0.4002845172473673
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.9580000179666155,
+                    "heading": -4.785144304302103e-21,
+                    "angularVelocity": 5.814288954429956e-20,
+                    "velocityX": 7.586727405106972e-33,
+                    "velocityY": -3.3376257900775013,
+                    "timestamp": 0.5003556465592092
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.7075000136298466,
+                    "heading": -2.6182090767908737e-21,
+                    "angularVelocity": 2.165395003165498e-20,
+                    "velocityX": 5.326134827450793e-33,
+                    "velocityY": -2.503219520548835,
+                    "timestamp": 0.600426775871051
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.540500005575846,
+                    "heading": -9.668053802454109e-22,
+                    "angularVelocity": 1.6502299039719674e-20,
+                    "velocityX": 3.4348416075821124e-33,
+                    "velocityY": -1.6688130652907358,
+                    "timestamp": 0.7004979051828928
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 1.8181769494337196e-40,
+                    "angularVelocity": 9.661181877631647e-21,
+                    "velocityX": 1.7280403247932335e-33,
+                    "velocityY": -0.8344065481228213,
+                    "timestamp": 0.8005690344947346
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": -1.6883012335149235e-41,
+                    "angularVelocity": -1.9855220810412424e-39,
+                    "velocityX": -1.2101303998889038e-42,
+                    "velocityY": -7.163168700323315e-39,
+                    "timestamp": 0.9006401638065764
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -30177,23 +36432,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "C3 To C5": {
             "waypoints": [
@@ -30221,136 +36460,136 @@
                     "x": 7.68,
                     "y": 4.127,
                     "heading": 0,
-                    "angularVelocity": 2.405138964716482e-32,
-                    "velocityX": 6.733008165659914e-33,
-                    "velocityY": 1.175464991429931e-38,
+                    "angularVelocity": -1.1842498081533988e-39,
+                    "velocityX": 0,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.6800000704332305,
-                    "y": 4.05055462799448,
-                    "heading": -1.2377486895022226e-8,
-                    "angularVelocity": -1.2926808085167386e-7,
-                    "velocityX": 7.355910665177674e-7,
-                    "velocityY": -0.7983807350945193,
-                    "timestamp": 0.09575052183150283
-                },
+                    "x": 7.68,
+                    "y": 4.050554627994461,
+                    "heading": 1.1070575354189048e-18,
+                    "angularVelocity": 1.156189558439314e-17,
+                    "velocityX": 1.1339790734570631e-33,
+                    "velocityY": -0.7983807350946678,
+                    "timestamp": 0.09575052183150978
+                },
                 {
-                    "x": 7.680000202731891,
-                    "y": 3.8976638870173534,
-                    "heading": -3.8235321936944764e-8,
-                    "angularVelocity": -2.700542284308498e-7,
-                    "velocityX": 0.0000013817016646870076,
-                    "velocityY": -1.5967614385034459,
-                    "timestamp": 0.19150104366300566
+                    "x": 7.68,
+                    "y": 3.897663887017311,
+                    "heading": 3.320268139408775e-18,
+                    "angularVelocity": 2.3114345091427606e-17,
+                    "velocityX": 5.50963145740529e-34,
+                    "velocityY": -1.596761438503583,
+                    "timestamp": 0.19150104366301957
                 },
                 {
-                    "x": 7.6800003883277,
-                    "y": 3.6683277831364425,
-                    "heading": -7.893916226933331e-8,
-                    "angularVelocity": -4.251030288838169e-7,
-                    "velocityX": 0.0000019383267807539473,
-                    "velocityY": -2.3951420785412023,
-                    "timestamp": 0.2872515654945085
+                    "x": 7.68,
+                    "y": 3.6683277831364007,
+                    "heading": 6.638014774282681e-18,
+                    "angularVelocity": 3.4649906568297676e-17,
+                    "velocityX": 4.767145470149766e-34,
+                    "velocityY": -2.3951420785410233,
+                    "timestamp": 0.28725156549452935
                 },
                 {
-                    "x": 7.680000618655782,
-                    "y": 3.3625463345551534,
-                    "heading": -1.3615412726424167e-7,
-                    "angularVelocity": -5.975419949298212e-7,
-                    "velocityX": 0.0000024055019376452327,
-                    "velocityY": -3.193522528466104,
-                    "timestamp": 0.3830020873260113
+                    "x": 7.68,
+                    "y": 3.362546334555254,
+                    "heading": 1.1056344671920077e-17,
+                    "angularVelocity": 4.614418609717799e-17,
+                    "velocityX": -5.447572473811757e-34,
+                    "velocityY": -3.19352252846437,
+                    "timestamp": 0.38300208732603913
                 },
                 {
-                    "x": 7.680000750128537,
-                    "y": 3.0003278007328986,
-                    "heading": -1.3615412487460524e-7,
-                    "angularVelocity": 2.2064625199669458e-14,
-                    "velocityX": 0.000001373076316791352,
-                    "velocityY": -3.7829405719549847,
-                    "timestamp": 0.47875260915751416
+                    "x": 7.68,
+                    "y": 3.0003278007331793,
+                    "heading": 2.1759975564527066e-17,
+                    "angularVelocity": 1.11786658577609e-16,
+                    "velocityX": -1.9220305893434716e-33,
+                    "velocityY": -3.7829405719528495,
+                    "timestamp": 0.4787526091575489
                 },
                 {
-                    "x": 7.68000080341356,
-                    "y": 2.638109266910804,
-                    "heading": -1.3615413544372385e-7,
-                    "angularVelocity": -1.1353706918076568e-13,
-                    "velocityX": 5.564985227619036e-7,
-                    "velocityY": -3.7829405719533264,
-                    "timestamp": 0.574503130989017
+                    "x": 7.68,
+                    "y": 2.63810926691106,
+                    "heading": -1.7246492437320545e-17,
+                    "angularVelocity": -4.0737603572006183e-16,
+                    "velocityX": 5.877602560652089e-33,
+                    "velocityY": -3.782940571953311,
+                    "timestamp": 0.5745031309890587
                 },
                 {
-                    "x": 7.680000782147778,
-                    "y": 2.2758907330887066,
-                    "heading": -1.361541430398869e-7,
-                    "angularVelocity": -8.149555996364277e-14,
-                    "velocityX": -2.2209570916932486e-7,
-                    "velocityY": -3.782940571953353,
-                    "timestamp": 0.6702536528205197
+                    "x": 7.68,
+                    "y": 2.27589073308894,
+                    "heading": -9.722837303851897e-18,
+                    "angularVelocity": 7.857560449339153e-17,
+                    "velocityX": 3.350655179860002e-33,
+                    "velocityY": -3.782940571953311,
+                    "timestamp": 0.6702536528205685
                 },
                 {
-                    "x": 7.68000069137085,
-                    "y": 1.9136721992666208,
-                    "heading": -1.3615414447383612e-7,
-                    "angularVelocity": -1.5331626502863808e-14,
-                    "velocityX": -9.48056722512506e-7,
-                    "velocityY": -3.7829405719532305,
-                    "timestamp": 0.7660041746520225
+                    "x": 7.68,
+                    "y": 1.9136721992668206,
+                    "heading": -4.541786403704224e-17,
+                    "angularVelocity": -3.727919811862982e-16,
+                    "velocityX": 1.4553646654154268e-32,
+                    "velocityY": -3.782940571953311,
+                    "timestamp": 0.7660041746520783
                 },
                 {
-                    "x": 7.68000053789062,
-                    "y": 1.55145366544455,
-                    "heading": -1.361541542308917e-7,
-                    "angularVelocity": -1.0101247732712604e-13,
-                    "velocityX": -0.000001602918056057891,
-                    "velocityY": -3.782940571953052,
-                    "timestamp": 0.8617546964835253
+                    "x": 7.68,
+                    "y": 1.5514536654447455,
+                    "heading": -1.11893051502424e-17,
+                    "angularVelocity": 3.5747647356983727e-16,
+                    "velocityX": -5.35720215261278e-33,
+                    "velocityY": -3.7829405719528495,
+                    "timestamp": 0.861754696483588
                 },
                 {
-                    "x": 7.68000033240059,
-                    "y": 1.2456722168634402,
-                    "heading": -8.229196402929451e-8,
-                    "angularVelocity": 5.625262643908853e-7,
-                    "velocityX": -0.0000021460981663034263,
-                    "velocityY": -3.193522528464227,
-                    "timestamp": 0.9575052183150281
+                    "x": 7.68,
+                    "y": 1.2456722168635992,
+                    "heading": -6.7032993083141955e-18,
+                    "angularVelocity": 4.6850980602319425e-17,
+                    "velocityX": -4.3527390380082413e-33,
+                    "velocityY": -3.19352252846437,
+                    "timestamp": 0.9575052183150978
                 },
                 {
-                    "x": 7.680000170894954,
-                    "y": 1.0163361129826005,
-                    "heading": -4.114641273305121e-8,
-                    "angularVelocity": 4.2971617502568956e-7,
-                    "velocityX": -0.000001686733705584938,
-                    "velocityY": -2.395142078540457,
-                    "timestamp": 1.053255740146531
+                    "x": 7.68,
+                    "y": 1.016336112982689,
+                    "heading": -3.348658686079055e-18,
+                    "angularVelocity": 3.5035220254491646e-17,
+                    "velocityX": -6.634490282826358e-33,
+                    "velocityY": -2.395142078541024,
+                    "timestamp": 1.0532557401466076
                 },
                 {
-                    "x": 7.680000058492725,
-                    "y": 0.8634453720055071,
-                    "heading": -1.3639289939958608e-8,
-                    "angularVelocity": 2.872790734007971e-7,
-                    "velocityX": -0.0000011739071958177648,
-                    "velocityY": -1.5967614385030962,
-                    "timestamp": 1.149006261978034
+                    "x": 7.68,
+                    "y": 0.8634453720055388,
+                    "heading": -1.115588951241814e-18,
+                    "angularVelocity": 2.3321750022200153e-17,
+                    "velocityX": -4.820835446954302e-33,
+                    "velocityY": -1.596761438503583,
+                    "timestamp": 1.1490062619781174
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
                     "heading": 0,
-                    "angularVelocity": 1.4244610959089762e-7,
-                    "velocityX": -6.10886739716558e-7,
-                    "velocityY": -0.7983807350943898,
-                    "timestamp": 1.2447567838095368
+                    "angularVelocity": 1.1650996045214628e-17,
+                    "velocityX": -3.709395222464697e-33,
+                    "velocityY": -0.7983807350946678,
+                    "timestamp": 1.2447567838096272
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
                     "heading": 0,
                     "angularVelocity": 0,
-                    "velocityX": 6.08432159346481e-32,
-                    "velocityY": 1.351843649746573e-37,
-                    "timestamp": 1.3405073056410397
+                    "velocityX": 6.978360472267901e-43,
+                    "velocityY": -3.6401965239950315e-38,
+                    "timestamp": 1.340507305641137
                 }
             ],
             "constraints": [
@@ -30363,30 +36602,14 @@
                 {
                     "scope": [
                         "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
+                    ],
+                    "type": "StopPoint"
                 }
-            ]
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
         },
         "C4 To W1": {
             "waypoints": [
@@ -30689,7 +36912,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 22
+                    "controlIntervalCount": 14
+                },
+                {
+                    "x": 4.992805004119873,
+                    "y": 4.258058071136475,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 13
                 },
                 {
                     "x": 2.25,
@@ -30701,7 +36933,260 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 0,
+                    "angularVelocity": 6.426641305668581e-31,
+                    "velocityX": -5.011351780601486e-32,
+                    "velocityY": -1.0551805148640748e-32,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.6491080594765135,
+                    "y": 2.4958059548034544,
+                    "heading": -0.02134777834454554,
+                    "angularVelocity": -0.2757848804434067,
+                    "velocityX": -0.3990827516771383,
+                    "velocityY": 0.5013212819261742,
+                    "timestamp": 0.07740735572675958
+                },
+                {
+                    "x": 7.587308326375472,
+                    "y": 2.5734484495799954,
+                    "heading": -0.06302080337998518,
+                    "angularVelocity": -0.538360013001088,
+                    "velocityX": -0.7983702907923742,
+                    "velocityY": 1.0030376835324535,
+                    "timestamp": 0.15481471145351916
+                },
+                {
+                    "x": 7.4945787588029695,
+                    "y": 2.6899753845455243,
+                    "heading": -0.12336744193035674,
+                    "angularVelocity": -0.7795982433941759,
+                    "velocityX": -1.1979425818371583,
+                    "velocityY": 1.5053728921687748,
+                    "timestamp": 0.23222206718027874
+                },
+                {
+                    "x": 7.370887761034633,
+                    "y": 2.845471450080925,
+                    "heading": -0.19922081386712712,
+                    "angularVelocity": -0.9799245979222482,
+                    "velocityX": -1.5979230475841766,
+                    "velocityY": 2.0088021877957765,
+                    "timestamp": 0.3096294229070383
+                },
+                {
+                    "x": 7.216202117777622,
+                    "y": 3.0401051125017453,
+                    "heading": -0.28194072984640106,
+                    "angularVelocity": -1.0686312069781692,
+                    "velocityX": -1.9983326107022177,
+                    "velocityY": 2.5144078439762083,
+                    "timestamp": 0.38703677863379793
+                },
+                {
+                    "x": 7.032473858656331,
+                    "y": 3.268122248690632,
+                    "heading": -0.2819407809538647,
+                    "angularVelocity": -6.602404015783628e-7,
+                    "velocityX": -2.3735245493959742,
+                    "velocityY": 2.9456778887237176,
+                    "timestamp": 0.46444413436055754
+                },
+                {
+                    "x": 6.834683469529352,
+                    "y": 3.4840547774927524,
+                    "heading": -0.28194080060331267,
+                    "angularVelocity": -2.538447128540298e-7,
+                    "velocityX": -2.5551885511392456,
+                    "velocityY": 2.7895608469606037,
+                    "timestamp": 0.5418514900873171
+                },
+                {
+                    "x": 6.603064880376339,
+                    "y": 3.663221523268011,
+                    "heading": -0.2819408226641663,
+                    "angularVelocity": -2.8499686418440244e-7,
+                    "velocityX": -2.992203867170001,
+                    "velocityY": 2.314595868740169,
+                    "timestamp": 0.6192588458140768
+                },
+                {
+                    "x": 6.344360158554223,
+                    "y": 3.8004066501958644,
+                    "heading": -0.2819407635550332,
+                    "angularVelocity": 7.636113202840995e-7,
+                    "velocityX": -3.342120647233029,
+                    "velocityY": 1.7722492344539278,
+                    "timestamp": 0.6966662015408364
+                },
+                {
+                    "x": 6.0661003337524155,
+                    "y": 3.891617196013079,
+                    "heading": -0.28194070942615473,
+                    "angularVelocity": 6.992730598438293e-7,
+                    "velocityX": -3.59474654817093,
+                    "velocityY": 1.1783188427102327,
+                    "timestamp": 0.774073557267596
+                },
+                {
+                    "x": 5.7830200242104235,
+                    "y": 3.966540038544743,
+                    "heading": -0.2819406560965727,
+                    "angularVelocity": 6.889472132026456e-7,
+                    "velocityX": -3.6570207945254936,
+                    "velocityY": 0.967903396624646,
+                    "timestamp": 0.8514809129943556
+                },
+                {
+                    "x": 5.499939604590639,
+                    "y": 4.041462465169378,
+                    "heading": -0.2819406027672389,
+                    "angularVelocity": 6.889440059098256e-7,
+                    "velocityX": -3.657022216584049,
+                    "velocityY": 0.9678980236594484,
+                    "timestamp": 0.9288882687211152
+                },
+                {
+                    "x": 5.234359644348809,
+                    "y": 4.1279159927443425,
+                    "heading": -0.3272205989423401,
+                    "angularVelocity": -0.5849572789301486,
+                    "velocityX": -3.430939576069496,
+                    "velocityY": 1.1168644990295,
+                    "timestamp": 1.0062956244478747
+                },
+                {
+                    "x": 4.992805004119873,
+                    "y": 4.258058071136475,
+                    "heading": -0.38392632852173914,
+                    "angularVelocity": -0.7325625458589549,
+                    "velocityX": -3.120564421314147,
+                    "velocityY": 1.6812624222886623,
+                    "timestamp": 1.0837029801746343
+                },
+                {
+                    "x": 4.760679474369801,
+                    "y": 4.451318301231298,
+                    "heading": -0.43925940371465827,
+                    "angularVelocity": -0.650567727765224,
+                    "velocityX": -2.7291701738847216,
+                    "velocityY": 2.272219071899485,
+                    "timestamp": 1.1687565000948184
+                },
+                {
+                    "x": 4.558064597648056,
+                    "y": 4.695759350557987,
+                    "heading": -0.45036550730749886,
+                    "angularVelocity": -0.13057782444839394,
+                    "velocityX": -2.3822044862091225,
+                    "velocityY": 2.8739674684372165,
+                    "timestamp": 1.2538100200150024
+                },
+                {
+                    "x": 4.30999794561686,
+                    "y": 4.900667947770545,
+                    "heading": -0.4503653988140509,
+                    "angularVelocity": 0.0000012755903345298267,
+                    "velocityX": -2.9165947777821004,
+                    "velocityY": 2.4091724528843765,
+                    "timestamp": 1.3388635399351865
+                },
+                {
+                    "x": 4.028045524182051,
+                    "y": 5.05567579900462,
+                    "heading": -0.4503653036134663,
+                    "angularVelocity": 0.0000011193021141522574,
+                    "velocityX": -3.3150000340890857,
+                    "velocityY": 1.8224742653747859,
+                    "timestamp": 1.4239170598553705
+                },
+                {
+                    "x": 3.731575050469511,
+                    "y": 5.180695241574213,
+                    "heading": -0.45036521232637566,
+                    "angularVelocity": 0.000001073289978617534,
+                    "velocityX": -3.485693172848094,
+                    "velocityY": 1.4698914599542185,
+                    "timestamp": 1.5089705797755546
+                },
+                {
+                    "x": 3.4348291016010033,
+                    "y": 5.305059402921897,
+                    "heading": -0.4503651210307298,
+                    "angularVelocity": 0.000001073390565931622,
+                    "velocityX": -3.488932017711167,
+                    "velocityY": 1.4621871201127652,
+                    "timestamp": 1.5940240996957387
+                },
+                {
+                    "x": 3.120089238118937,
+                    "y": 5.371868296261872,
+                    "heading": -0.45036508370615375,
+                    "angularVelocity": 4.3883634791364236e-7,
+                    "velocityX": -3.7004919229372337,
+                    "velocityY": 0.785492398232055,
+                    "timestamp": 1.6790776196159227
+                },
+                {
+                    "x": 2.8304866761911076,
+                    "y": 5.435580552385651,
+                    "heading": -0.37567464629819297,
+                    "angularVelocity": 0.8781580994889028,
+                    "velocityX": -3.404945053415936,
+                    "velocityY": 0.7490842963759552,
+                    "timestamp": 1.7641311395361068
+                },
+                {
+                    "x": 2.5976382602327472,
+                    "y": 5.485810295855226,
+                    "heading": -0.2445738789443125,
+                    "angularVelocity": 1.5413914377313083,
+                    "velocityX": -2.7376693660283826,
+                    "velocityY": 0.5905663106789283,
+                    "timestamp": 1.8491846594562908
+                },
+                {
+                    "x": 2.4235493783626687,
+                    "y": 5.523078200691987,
+                    "heading": -0.12850773623159037,
+                    "angularVelocity": 1.3646248012033382,
+                    "velocityX": -2.0468157230112984,
+                    "velocityY": 0.43817004718549557,
+                    "timestamp": 1.934238179376475
+                },
+                {
+                    "x": 2.3077761259926057,
+                    "y": 5.547737838884683,
+                    "heading": -0.04437897835044122,
+                    "angularVelocity": 0.9891272925576834,
+                    "velocityX": -1.3611812007158846,
+                    "velocityY": 0.2899308366756611,
+                    "timestamp": 2.0192916992966587
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 1.7803711242706045e-30,
+                    "angularVelocity": 0.521777092730431,
+                    "velocityX": -0.6792914161204248,
+                    "velocityY": 0.14416994295838573,
+                    "timestamp": 2.104345219216843
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": 1.3562125763475036e-31,
+                    "velocityY": 2.290518308039125e-31,
+                    "timestamp": 2.189398739137027
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -30746,7 +37231,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 22
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 3.8162567615509033,
+                    "y": 2.2371633052825928,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 14
                 },
                 {
                     "x": 2.3,
@@ -30758,7 +37252,278 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 0,
+                    "angularVelocity": -4.62162066936404e-33,
+                    "velocityX": 3.0688151507515204e-31,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.623584376302727,
+                    "y": 2.4297857949616017,
+                    "heading": 0.033539307380936634,
+                    "angularVelocity": 0.38483343876921716,
+                    "velocityX": -0.6473186288895079,
+                    "velocityY": -0.3122585684118786,
+                    "timestamp": 0.08715278871867994
+                },
+                {
+                    "x": 7.510615150776605,
+                    "y": 2.375312721693156,
+                    "heading": 0.0969954791736376,
+                    "angularVelocity": 0.7281025968948721,
+                    "velocityX": -1.2962204329545195,
+                    "velocityY": -0.6250296068468768,
+                    "timestamp": 0.1743055774373599
+                },
+                {
+                    "x": 7.340880293995689,
+                    "y": 2.2935273668964844,
+                    "heading": 0.1838752961952568,
+                    "angularVelocity": 0.9968678948651678,
+                    "velocityX": -1.9475550843106406,
+                    "velocityY": -0.9384135149210906,
+                    "timestamp": 0.26145836615603985
+                },
+                {
+                    "x": 7.114069036249808,
+                    "y": 2.1844157133503646,
+                    "heading": 0.27996244701307155,
+                    "angularVelocity": 1.1025137833279668,
+                    "velocityX": -2.6024555390648842,
+                    "velocityY": -1.251958258023393,
+                    "timestamp": 0.3486111548747198
+                },
+                {
+                    "x": 6.831104073589339,
+                    "y": 2.0490142604857513,
+                    "heading": 0.32912237638568254,
+                    "angularVelocity": 0.5640660510737534,
+                    "velocityX": -3.246768885088134,
+                    "velocityY": -1.5536101007814562,
+                    "timestamp": 0.4357639435933997
+                },
+                {
+                    "x": 6.534528280813305,
+                    "y": 1.9049975621731787,
+                    "heading": 0.3291224030694369,
+                    "angularVelocity": 3.0617212321522066e-7,
+                    "velocityX": -3.4029409401155197,
+                    "velocityY": -1.6524623070575855,
+                    "timestamp": 0.5229167323120797
+                },
+                {
+                    "x": 6.237952262222522,
+                    "y": 1.7609813288852085,
+                    "heading": 0.3291224297529421,
+                    "angularVelocity": 3.0616926449614015e-7,
+                    "velocityX": -3.402943531137019,
+                    "velocityY": -1.6524569713178037,
+                    "timestamp": 0.6100695210307596
+                },
+                {
+                    "x": 5.936787262454918,
+                    "y": 1.6268259970502223,
+                    "heading": 0.32912245667190637,
+                    "angularVelocity": 3.0887094574020004e-7,
+                    "velocityX": -3.4555979699024117,
+                    "velocityY": -1.5393119808021931,
+                    "timestamp": 0.6972223097494396
+                },
+                {
+                    "x": 5.615527521313305,
+                    "y": 1.5527301986214108,
+                    "heading": 0.32912248658360005,
+                    "angularVelocity": 3.4320982850713803e-7,
+                    "velocityX": -3.6861670850098154,
+                    "velocityY": -0.850182759704744,
+                    "timestamp": 0.7843750984681195
+                },
+                {
+                    "x": 5.286027461953728,
+                    "y": 1.5414290964422628,
+                    "heading": 0.32912252585289026,
+                    "angularVelocity": 4.50579847209181e-7,
+                    "velocityX": -3.780717337951944,
+                    "velocityY": -0.12967000075725457,
+                    "timestamp": 0.8715278871867994
+                },
+                {
+                    "x": 4.960446284223111,
+                    "y": 1.593339788205863,
+                    "heading": 0.3291233220408656,
+                    "angularVelocity": 0.000009135542155699871,
+                    "velocityX": -3.7357516898462033,
+                    "velocityY": 0.5956285797252272,
+                    "timestamp": 0.9586806759054793
+                },
+                {
+                    "x": 4.650797848549272,
+                    "y": 1.7065466836899894,
+                    "heading": 0.3291235959637348,
+                    "angularVelocity": 0.0000031430189815062194,
+                    "velocityX": -3.5529377800330906,
+                    "velocityY": 1.2989474823295228,
+                    "timestamp": 1.0458334646241594
+                },
+                {
+                    "x": 4.368508202692925,
+                    "y": 1.8768714467040708,
+                    "heading": 0.32912381701495697,
+                    "angularVelocity": 0.000002536364302217629,
+                    "velocityX": -3.2390202310972316,
+                    "velocityY": 1.9543237286860882,
+                    "timestamp": 1.1329862533428394
+                },
+                {
+                    "x": 4.092384496633659,
+                    "y": 2.0570204638146983,
+                    "heading": 0.3291240376025158,
+                    "angularVelocity": 0.000002531044182467546,
+                    "velocityX": -3.1682716080441704,
+                    "velocityY": 2.067048223690569,
+                    "timestamp": 1.2201390420615195
+                },
+                {
+                    "x": 3.8162567615509033,
+                    "y": 2.2371633052825928,
+                    "heading": 0.32912425818941665,
+                    "angularVelocity": 0.0000025310366317804206,
+                    "velocityX": -3.1683178374712258,
+                    "velocityY": 2.066977363735053,
+                    "timestamp": 1.3072918307801995
+                },
+                {
+                    "x": 3.558387727399027,
+                    "y": 2.3518646644990793,
+                    "heading": 0.32912440943088817,
+                    "angularVelocity": 0.0000020272145178599712,
+                    "velocityX": -3.4564319203160125,
+                    "velocityY": 1.5374371746628512,
+                    "timestamp": 1.3818973893726278
+                },
+                {
+                    "x": 3.300516056227942,
+                    "y": 2.466560095044955,
+                    "heading": 0.3291245606718786,
+                    "angularVelocity": 0.000002027208070545675,
+                    "velocityX": -3.4564672664652627,
+                    "velocityY": 1.5373577077876843,
+                    "timestamp": 1.4565029479650562
+                },
+                {
+                    "x": 3.052458337002912,
+                    "y": 2.601171414843865,
+                    "heading": 0.329124716605593,
+                    "angularVelocity": 0.0000020901085294853097,
+                    "velocityX": -3.3249227524744356,
+                    "velocityY": 1.804306841723335,
+                    "timestamp": 1.5311085065574845
+                },
+                {
+                    "x": 2.829814908557799,
+                    "y": 2.7746151638451657,
+                    "heading": 0.3291249161701291,
+                    "angularVelocity": 0.000002674928516490017,
+                    "velocityX": -2.984273995740964,
+                    "velocityY": 2.3248100044237634,
+                    "timestamp": 1.6057140651499129
+                },
+                {
+                    "x": 2.638606432152533,
+                    "y": 2.982201542308141,
+                    "heading": 0.3291253317978473,
+                    "angularVelocity": 0.000005571002027267265,
+                    "velocityX": -2.562925337103115,
+                    "velocityY": 2.7824519027734036,
+                    "timestamp": 1.6803196237423412
+                },
+                {
+                    "x": 2.4840036736729245,
+                    "y": 3.218316897029485,
+                    "heading": 0.3291273340604081,
+                    "angularVelocity": 0.000026837980957261232,
+                    "velocityX": -2.0722686271167303,
+                    "velocityY": 3.1648493647939433,
+                    "timestamp": 1.7549251823347696
+                },
+                {
+                    "x": 2.3733884856918945,
+                    "y": 3.4659660107751304,
+                    "heading": 0.2954274954004952,
+                    "angularVelocity": -0.4517068070492687,
+                    "velocityX": -1.4826668423638905,
+                    "velocityY": 3.31944587532086,
+                    "timestamp": 1.829530740927198
+                },
+                {
+                    "x": 2.305023098579148,
+                    "y": 3.7064756804968377,
+                    "heading": 0.19598318894217118,
+                    "angularVelocity": -1.3329342790875696,
+                    "velocityX": -0.9163578210871337,
+                    "velocityY": 3.2237500028063106,
+                    "timestamp": 1.9041362995196263
+                },
+                {
+                    "x": 2.272475779606133,
+                    "y": 3.9200958593762563,
+                    "heading": 0.053473340322528436,
+                    "angularVelocity": -1.9101773555262378,
+                    "velocityX": -0.43625863256144176,
+                    "velocityY": 2.8633279196585013,
+                    "timestamp": 1.9787418581120546
+                },
+                {
+                    "x": 2.2677016800416907,
+                    "y": 4.09662450199971,
+                    "heading": -0.09830221598718161,
+                    "angularVelocity": -2.034373298360552,
+                    "velocityX": -0.06399120460344819,
+                    "velocityY": 2.3661593848232187,
+                    "timestamp": 2.053347416704483
+                },
+                {
+                    "x": 2.2778239174156503,
+                    "y": 4.229436050413757,
+                    "heading": -0.23380831521279008,
+                    "angularVelocity": -1.8163003103546322,
+                    "velocityX": 0.13567671853055638,
+                    "velocityY": 1.7801830174558415,
+                    "timestamp": 2.1279529752969113
+                },
+                {
+                    "x": 2.29107059750262,
+                    "y": 4.316936633046431,
+                    "heading": -0.33501872696397506,
+                    "angularVelocity": -1.3566068488823915,
+                    "velocityX": 0.1775562081015563,
+                    "velocityY": 1.1728426712906341,
+                    "timestamp": 2.2025585338893396
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": -0.39,
+                    "angularVelocity": -0.7369594715641588,
+                    "velocityX": 0.11968816621507666,
+                    "velocityY": 0.5772139203303202,
+                    "timestamp": 2.277164092481768
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": -0.39,
+                    "angularVelocity": -8.092865962806098e-32,
+                    "velocityX": 0,
+                    "velocityY": 4.648171279746289e-33,
+                    "timestamp": 2.3517696510741963
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -30803,7 +37568,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 11
+                },
+                {
+                    "x": 5.546474933624268,
+                    "y": 1.3928169012069702,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 13
                 },
                 {
                     "x": 4.263,
@@ -30815,7 +37589,233 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 7.203126847267154e-32,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.6459509070752,
+                    "y": 2.420050479995149,
+                    "heading": -0.026297254997510774,
+                    "angularVelocity": -0.33695614983619343,
+                    "velocityX": -0.4362832264603014,
+                    "velocityY": -0.47344743777665743,
+                    "timestamp": 0.078043552581802
+                },
+                {
+                    "x": 7.57752123433784,
+                    "y": 2.3463325784745406,
+                    "heading": -0.07655763531528098,
+                    "angularVelocity": -0.6440042598660941,
+                    "velocityX": -0.8768139131756897,
+                    "velocityY": -0.9445738831958564,
+                    "timestamp": 0.156087105163604
+                },
+                {
+                    "x": 7.474195561438856,
+                    "y": 2.236153787211583,
+                    "heading": -0.14701004173730176,
+                    "angularVelocity": -0.9027319245646475,
+                    "velocityX": -1.3239488654839289,
+                    "velocityY": -1.4117603263569152,
+                    "timestamp": 0.23413065774540598
+                },
+                {
+                    "x": 7.33507656702923,
+                    "y": 2.0901340736402396,
+                    "heading": -0.23055657416165956,
+                    "angularVelocity": -1.0705116522834195,
+                    "velocityX": -1.7825815177216175,
+                    "velocityY": -1.8710029046700958,
+                    "timestamp": 0.312174210327208
+                },
+                {
+                    "x": 7.158294386463989,
+                    "y": 1.910067298253472,
+                    "heading": -0.30975533817585227,
+                    "angularVelocity": -1.014802137964151,
+                    "velocityX": -2.2651734155741057,
+                    "velocityY": -2.3072601057983104,
+                    "timestamp": 0.39021776290901
+                },
+                {
+                    "x": 6.940360292129115,
+                    "y": 1.7108993896694566,
+                    "heading": -0.30975560484167247,
+                    "angularVelocity": -0.00000341688469228089,
+                    "velocityX": -2.7924676302559823,
+                    "velocityY": -2.552009768843728,
+                    "timestamp": 0.468261315490812
+                },
+                {
+                    "x": 6.6915168927330235,
+                    "y": 1.5520279357314815,
+                    "heading": -0.30975564017638474,
+                    "angularVelocity": -4.5275632793576304e-7,
+                    "velocityX": -3.1885196299240257,
+                    "velocityY": -2.035676858398594,
+                    "timestamp": 0.546304868072614
+                },
+                {
+                    "x": 6.419127595403782,
+                    "y": 1.43815413781978,
+                    "heading": -0.30975568823518523,
+                    "angularVelocity": -6.157946287917162e-7,
+                    "velocityX": -3.490221655962278,
+                    "velocityY": -1.4591057703627237,
+                    "timestamp": 0.624348420654416
+                },
+                {
+                    "x": 6.131252530334066,
+                    "y": 1.37264761019134,
+                    "heading": -0.30975577587401576,
+                    "angularVelocity": -0.0000011229477336220622,
+                    "velocityX": -3.6886463461279972,
+                    "velocityY": -0.8393586076157934,
+                    "timestamp": 0.7023919732362179
+                },
+                {
+                    "x": 5.83657654735769,
+                    "y": 1.3574693553002635,
+                    "heading": -0.31020540722452916,
+                    "angularVelocity": -0.005761287587978277,
+                    "velocityX": -3.7757889438387497,
+                    "velocityY": -0.1944844178532558,
+                    "timestamp": 0.7804355258180199
+                },
+                {
+                    "x": 5.546474933624268,
+                    "y": 1.3928169012069702,
+                    "heading": -0.3189695864044355,
+                    "angularVelocity": -0.11229856778804584,
+                    "velocityX": -3.717175911864151,
+                    "velocityY": 0.4529207697160109,
+                    "timestamp": 0.8584790783998218
+                },
+                {
+                    "x": 5.303276771996363,
+                    "y": 1.4607318697473424,
+                    "heading": -0.3307229091538368,
+                    "angularVelocity": -0.17360472172913416,
+                    "velocityX": -3.5922053767043294,
+                    "velocityY": 1.0031511484968414,
+                    "timestamp": 0.9261807090485242
+                },
+                {
+                    "x": 5.073563214559545,
+                    "y": 1.564404927734292,
+                    "heading": -0.34378229938421223,
+                    "angularVelocity": -0.1928962434919207,
+                    "velocityX": -3.3930284283517023,
+                    "velocityY": 1.5313229089691542,
+                    "timestamp": 0.9938823396972266
+                },
+                {
+                    "x": 4.862974125111252,
+                    "y": 1.7011642845126724,
+                    "heading": -0.35827284529055603,
+                    "angularVelocity": -0.2140354045770704,
+                    "velocityX": -3.1105467834446325,
+                    "velocityY": 2.020030469989471,
+                    "timestamp": 1.0615839703459289
+                },
+                {
+                    "x": 4.677671719496688,
+                    "y": 1.8665243414779629,
+                    "heading": -0.3791415933598452,
+                    "angularVelocity": -0.30824587043792007,
+                    "velocityX": -2.737044940262413,
+                    "velocityY": 2.442482631227878,
+                    "timestamp": 1.1292856009946313
+                },
+                {
+                    "x": 4.523629451068858,
+                    "y": 2.053477766497112,
+                    "heading": -0.41478083919993697,
+                    "angularVelocity": -0.5264163580502632,
+                    "velocityX": -2.27531105162187,
+                    "velocityY": 2.761431640977936,
+                    "timestamp": 1.1969872316433336
+                },
+                {
+                    "x": 4.406611773563404,
+                    "y": 2.2498215105595722,
+                    "heading": -0.45494733767836987,
+                    "angularVelocity": -0.5932870167770028,
+                    "velocityX": -1.7284321867022447,
+                    "velocityY": 2.900133160474012,
+                    "timestamp": 1.264688862292036
+                },
+                {
+                    "x": 4.327688857963163,
+                    "y": 2.4438365235550945,
+                    "heading": -0.48799769437613305,
+                    "angularVelocity": -0.4881766713911416,
+                    "velocityX": -1.1657461547677304,
+                    "velocityY": 2.865736188870776,
+                    "timestamp": 1.3323904929407384
+                },
+                {
+                    "x": 4.28360260298108,
+                    "y": 2.622213717107768,
+                    "heading": -0.5154307830643909,
+                    "angularVelocity": -0.40520573028252516,
+                    "velocityX": -0.6511845366152605,
+                    "velocityY": 2.634754759132393,
+                    "timestamp": 1.4000921235894408
+                },
+                {
+                    "x": 4.264391797811532,
+                    "y": 2.7716303897908317,
+                    "heading": -0.5365689504315967,
+                    "angularVelocity": -0.3122253801676767,
+                    "velocityX": -0.283756904899847,
+                    "velocityY": 2.2069877970647664,
+                    "timestamp": 1.4677937542381432
+                },
+                {
+                    "x": 4.259328393836262,
+                    "y": 2.8855840206662466,
+                    "heading": -0.5517027167443236,
+                    "angularVelocity": -0.22353621571242488,
+                    "velocityX": -0.07478998551086755,
+                    "velocityY": 1.6831740946789628,
+                    "timestamp": 1.5354953848868456
+                },
+                {
+                    "x": 4.260599830778067,
+                    "y": 2.9618797181043535,
+                    "heading": -0.5613571988137664,
+                    "angularVelocity": -0.1426033904496253,
+                    "velocityX": 0.018780004700190214,
+                    "velocityY": 1.1269403219251917,
+                    "timestamp": 1.603197015535548
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": -0.566,
+                    "angularVelocity": -0.0685773908510417,
+                    "velocityX": 0.035452162657366115,
+                    "velocityY": 0.5630629798778553,
+                    "timestamp": 1.6708986461842503
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": -0.566,
+                    "angularVelocity": 0,
+                    "velocityX": 1.1764020723168042e-36,
+                    "velocityY": 0,
+                    "timestamp": 1.7386002768329527
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -30855,24 +37855,442 @@
             "waypoints": [
                 {
                     "x": 7.68,
-                    "y": 2.457,
+                    "y": 2.457,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 9
+                },
+                {
+                    "x": 6.4046630859375,
+                    "y": 1.6973352432250977,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 11
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": -0.64,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 0,
+                    "angularVelocity": 1.0550386775092505e-35,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.6504501923310215,
+                    "y": 2.437078665707293,
+                    "heading": -0.0029944704440250615,
+                    "angularVelocity": -0.0457972069493345,
+                    "velocityX": -0.45193254781641656,
+                    "velocityY": -0.30467539632270557,
+                    "timestamp": 0.0653854381848624
+                },
+                {
+                    "x": 7.5913505770823395,
+                    "y": 2.3972359988448533,
+                    "heading": -0.008983367804687599,
+                    "angularVelocity": -0.09159374819405963,
+                    "velocityX": -0.9038650942674934,
+                    "velocityY": -0.609350766294381,
+                    "timestamp": 0.1307708763697248
+                },
+                {
+                    "x": 7.50270115478166,
+                    "y": 2.337472001372052,
+                    "heading": -0.017966623739921285,
+                    "angularVelocity": -0.13738924422033513,
+                    "velocityX": -1.3557976326478658,
+                    "velocityY": -0.91402610629957,
+                    "timestamp": 0.19615631455458718
+                },
+                {
+                    "x": 7.384501926877228,
+                    "y": 2.2577866758321745,
+                    "heading": -0.02994413949472175,
+                    "angularVelocity": -0.18318322989496186,
+                    "velocityX": -1.8077301488788986,
+                    "velocityY": -1.2187014074079496,
+                    "timestamp": 0.2615417527394496
+                },
+                {
+                    "x": 7.2367528971895485,
+                    "y": 2.1581800263627264,
+                    "heading": -0.044915766222073404,
+                    "angularVelocity": -0.2289749391144676,
+                    "velocityX": -2.25966260667937,
+                    "velocityY": -1.5233766452376303,
+                    "timestamp": 0.326927190924312
+                },
+                {
+                    "x": 7.059454079201559,
+                    "y": 2.0386520636124454,
+                    "heading": -0.06288119646669635,
+                    "angularVelocity": -0.2747619461359239,
+                    "velocityX": -2.711594858272837,
+                    "velocityY": -1.82805172020631,
+                    "timestamp": 0.3923126291091744
+                },
+                {
+                    "x": 6.854358935700705,
+                    "y": 1.900385597577332,
+                    "heading": -0.06288119947800026,
+                    "angularVelocity": -4.6054656889756363e-8,
+                    "velocityX": -3.136709781174733,
+                    "velocityY": -2.114636987584247,
+                    "timestamp": 0.45769806729403684
+                },
+                {
+                    "x": 6.636845838072133,
+                    "y": 1.782615967982618,
+                    "heading": -0.06288120256062167,
+                    "angularVelocity": -4.714538109405926e-8,
+                    "velocityX": -3.3266290425951324,
+                    "velocityY": -1.801159904469057,
+                    "timestamp": 0.5230835054788993
+                },
+                {
+                    "x": 6.4046630859375,
+                    "y": 1.6973352432250977,
+                    "heading": -0.06288120593828476,
+                    "angularVelocity": -5.1657726572043936e-8,
+                    "velocityX": -3.5509856411482486,
+                    "velocityY": -1.3042770244409516,
+                    "timestamp": 0.5884689436637617
+                },
+                {
+                    "x": 6.207746736822867,
+                    "y": 1.6503996785686041,
+                    "heading": -0.06288121580879107,
+                    "angularVelocity": -1.8445407171610623e-7,
+                    "velocityX": -3.679854027466254,
+                    "velocityY": -0.8771035386810692,
+                    "timestamp": 0.6419809437716079
+                },
+                {
+                    "x": 6.009908768236521,
+                    "y": 1.6271742750630454,
+                    "heading": -0.0724170210773865,
+                    "angularVelocity": -0.17819938050114592,
+                    "velocityX": -3.697076696584537,
+                    "velocityY": -0.43402234001253925,
+                    "timestamp": 0.695492943879454
+                },
+                {
+                    "x": 5.8260492909953046,
+                    "y": 1.6186155743000838,
+                    "heading": -0.12620190803666176,
+                    "angularVelocity": -1.0050995449783064,
+                    "velocityX": -3.43585507681777,
+                    "velocityY": -0.1599398405163814,
+                    "timestamp": 0.7490049439873002
+                },
+                {
+                    "x": 5.6625912601386625,
+                    "y": 1.614879890442449,
+                    "heading": -0.22199295256207294,
+                    "angularVelocity": -1.7900852954918034,
+                    "velocityX": -3.0546051451490293,
+                    "velocityY": -0.06981020799271546,
+                    "timestamp": 0.8025169440951464
+                },
+                {
+                    "x": 5.5227239807129855,
+                    "y": 1.6141995251565013,
+                    "heading": -0.32512734843535046,
+                    "angularVelocity": -1.9273134187738115,
+                    "velocityX": -2.61375540334492,
+                    "velocityY": -0.012714256327113658,
+                    "timestamp": 0.8560289442029926
+                },
+                {
+                    "x": 5.406619958232834,
+                    "y": 1.6150039384726416,
+                    "heading": -0.42188556792063076,
+                    "angularVelocity": -1.8081592781110263,
+                    "velocityX": -2.169681982474204,
+                    "velocityY": 0.015032391136922594,
+                    "timestamp": 0.9095409443108388
+                },
+                {
+                    "x": 5.314084878105213,
+                    "y": 1.6164421457440925,
+                    "heading": -0.5051954003859285,
+                    "angularVelocity": -1.5568439284160218,
+                    "velocityX": -1.7292397955809797,
+                    "velocityY": 0.026876350511144828,
+                    "timestamp": 0.9630529444186849
+                },
+                {
+                    "x": 5.244912091070787,
+                    "y": 1.617980616061371,
+                    "heading": -0.5709381839957368,
+                    "angularVelocity": -1.2285615091439794,
+                    "velocityX": -1.2926593454742539,
+                    "velocityY": 0.028750005871169333,
+                    "timestamp": 1.016564944526531
+                },
+                {
+                    "x": 5.198930392622704,
+                    "y": 1.6192554664106382,
+                    "heading": -0.6164892293058035,
+                    "angularVelocity": -0.851230475748707,
+                    "velocityX": -0.8592782619863396,
+                    "velocityY": 0.023823634823929046,
+                    "timestamp": 1.0700769446343772
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": -0.64,
+                    "angularVelocity": -0.4393551100092281,
+                    "velocityX": -0.42850935447171357,
+                    "velocityY": 0.013913394899485362,
+                    "timestamp": 1.1235889447422234
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": -0.64,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 6.2403191565519e-36,
+                    "timestamp": 1.1771009448500696
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": [
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                }
+            ]
+        },
+        "C4 To C1": {
+            "waypoints": [
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 18
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": -2.2542235108498663e-32,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.538844153951763,
+                    "heading": 2.1223723168001845e-18,
+                    "angularVelocity": 2.1422107346760384e-17,
+                    "velocityX": -4.524824041974102e-19,
+                    "velocityY": 0.8260917454276265,
+                    "timestamp": 0.09907392781100399
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.70253245874193,
+                    "heading": 6.368502121517421e-18,
+                    "angularVelocity": 4.285819588010552e-17,
+                    "velocityX": -9.048618887508432e-19,
+                    "velocityY": 1.6521834594306501,
+                    "timestamp": 0.19814785562200798
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.9480649081437846,
+                    "heading": 1.2741259045974704e-17,
+                    "angularVelocity": 6.432324896396774e-17,
+                    "velocityX": -1.3570355356096521e-18,
+                    "velocityY": 2.47827511058448,
+                    "timestamp": 0.297221783433012
+                },
+                {
+                    "x": 7.68,
+                    "y": 3.2754414834771923,
+                    "heading": 2.1247069179422305e-17,
+                    "angularVelocity": 8.585316360490521e-17,
+                    "velocityX": -1.8085916683463265e-18,
+                    "velocityY": 3.3043665731908773,
+                    "timestamp": 0.39629571124401597
+                },
+                {
+                    "x": 7.68,
+                    "y": 3.6502322649266867,
+                    "heading": 5.059469084858907e-18,
+                    "angularVelocity": -1.633891019789118e-16,
+                    "velocityX": -2.0715339797290713e-18,
+                    "velocityY": 3.782940575087071,
+                    "timestamp": 0.49536963905501996
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.025023046376204,
+                    "heading": -4.370025378852898e-19,
+                    "angularVelocity": -5.5478487063848375e-17,
+                    "velocityX": -2.0691441879135133e-18,
+                    "velocityY": 3.7829405750873115,
+                    "timestamp": 0.594443566866024
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.3998138278257235,
+                    "heading": 3.678666355048194e-17,
+                    "angularVelocity": 3.7571606285132846e-16,
+                    "velocityX": -2.072328507072934e-18,
+                    "velocityY": 3.782940575087312,
+                    "timestamp": 0.693517494677028
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.774604609275241,
+                    "heading": 5.708604515927294e-17,
+                    "angularVelocity": 2.04891257037182e-16,
+                    "velocityX": -2.07516702935077e-18,
+                    "velocityY": 3.782940575087312,
+                    "timestamp": 0.7925914224880319
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.14939539072476,
+                    "heading": 3.2711500565517004e-17,
+                    "angularVelocity": -2.460238039643844e-16,
+                    "velocityX": -2.078706211560206e-18,
+                    "velocityY": 3.782940575087312,
+                    "timestamp": 0.8916653502990359
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.524186172174278,
+                    "heading": 3.4723638610714384e-17,
+                    "angularVelocity": 2.0309460720999983e-17,
+                    "velocityX": -2.0667219645048642e-18,
+                    "velocityY": 3.782940575087312,
+                    "timestamp": 0.9907392781100399
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.898976953623796,
+                    "heading": 1.3963468905806427e-18,
+                    "angularVelocity": -3.363881139719203e-16,
+                    "velocityX": -2.078319856885538e-18,
+                    "velocityY": 3.782940575087312,
+                    "timestamp": 1.089813205921044
+                },
+                {
+                    "x": 7.68,
+                    "y": 6.273767735073314,
+                    "heading": -1.338725801529433e-17,
+                    "angularVelocity": -1.492179146704526e-16,
+                    "velocityX": -2.0709974384556896e-18,
+                    "velocityY": 3.7829405750873115,
+                    "timestamp": 1.188887133732048
+                },
+                {
+                    "x": 7.68,
+                    "y": 6.648558516522808,
+                    "heading": -2.1247111946894708e-17,
+                    "angularVelocity": -7.933322222365232e-17,
+                    "velocityX": -2.070808166139798e-18,
+                    "velocityY": 3.782940575087071,
+                    "timestamp": 1.287961061543052
+                },
+                {
+                    "x": 7.68,
+                    "y": 6.975935091856216,
+                    "heading": -1.273985947141283e-17,
+                    "angularVelocity": 8.586772184549593e-17,
+                    "velocityX": -1.8108827209778567e-18,
+                    "velocityY": 3.304366573190878,
+                    "timestamp": 1.387034989354056
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.22146754125807,
+                    "heading": -6.367750753434283e-18,
+                    "angularVelocity": 6.431670631033216e-17,
+                    "velocityX": -1.3581621409711223e-18,
+                    "velocityY": 2.47827511058448,
+                    "timestamp": 1.4861089171650599
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.385155846048237,
+                    "heading": -2.122107432521882e-18,
+                    "angularVelocity": 4.285328556951628e-17,
+                    "velocityX": -9.05441456368543e-19,
+                    "velocityY": 1.65218345943065,
+                    "timestamp": 1.5851828449760639
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
                     "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
+                    "angularVelocity": 2.141943374454498e-17,
+                    "velocityX": -4.527207369006345e-19,
+                    "velocityY": 0.8260917454276265,
+                    "timestamp": 1.6842567727870679
                 },
                 {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": -3.1368402443636883e-34,
+                    "velocityY": 0,
+                    "timestamp": 1.7833307005980719
                 }
             ],
-            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -30890,25 +38308,9 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
-        "C4 To C1": {
+        "C4 To C2": {
             "waypoints": [
                 {
                     "x": 7.68,
@@ -30917,11 +38319,11 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 18
+                    "controlIntervalCount": 14
                 },
                 {
                     "x": 7.68,
-                    "y": 7.467,
+                    "y": 5.797,
                     "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
@@ -30929,64 +38331,143 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
-            "constraints": [
+            "trajectory": [
                 {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 0,
+                    "angularVelocity": -1.7120676318191985e-39,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
                 },
                 {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
+                    "x": 7.68,
+                    "y": 2.5334453720055388,
+                    "heading": -1.6066276958150233e-18,
+                    "angularVelocity": -1.677931007769515e-17,
+                    "velocityX": 1.4398812722194594e-33,
+                    "velocityY": 0.7983807350946674,
+                    "timestamp": 0.09575052183150976
+                },
                 {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
+                    "x": 7.68,
+                    "y": 2.686336112982689,
+                    "heading": -4.816833322743681e-18,
+                    "angularVelocity": -3.3526769001814837e-17,
+                    "velocityX": 1.5865173753469157e-33,
+                    "velocityY": 1.5967614385035827,
+                    "timestamp": 0.1915010436630195
                 },
                 {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
+                    "x": 7.68,
+                    "y": 2.915672216863599,
+                    "heading": -9.624489104750263e-18,
+                    "angularVelocity": -5.0210230614541765e-17,
+                    "velocityX": 1.0182538660036936e-34,
+                    "velocityY": 2.395142078541023,
+                    "timestamp": 0.28725156549452924
                 },
                 {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C4 To C2": {
-            "waypoints": [
+                    "x": 7.68,
+                    "y": 3.221453665444745,
+                    "heading": -1.6009153132358185e-17,
+                    "angularVelocity": -6.66802008672839e-17,
+                    "velocityX": 3.094783239015371e-34,
+                    "velocityY": 3.193522528464369,
+                    "timestamp": 0.383002087326039
+                },
                 {
                     "x": 7.68,
-                    "y": 2.457,
+                    "y": 3.5836721992668203,
+                    "heading": -3.473454086768081e-17,
+                    "angularVelocity": -1.9556434134398854e-16,
+                    "velocityX": 3.082893791022991e-33,
+                    "velocityY": 3.7829405719528495,
+                    "timestamp": 0.4787526091575488
+                },
+                {
+                    "x": 7.68,
+                    "y": 3.9458907330889397,
+                    "heading": -3.402618832360637e-17,
+                    "angularVelocity": 7.397897479044197e-18,
+                    "velocityX": 3.015867889332214e-33,
+                    "velocityY": 3.782940571953311,
+                    "timestamp": 0.5745031309890586
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.308109266911059,
+                    "heading": -1.7375698441954866e-17,
+                    "angularVelocity": 1.7389450796885638e-16,
+                    "velocityX": 5.330140345912211e-33,
+                    "velocityY": 3.782940571953311,
+                    "timestamp": 0.6702536528205684
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.670327800733179,
+                    "heading": 7.579747606314133e-18,
+                    "angularVelocity": 2.6062986990486198e-16,
+                    "velocityX": 7.600730979130334e-33,
+                    "velocityY": 3.782940571953311,
+                    "timestamp": 0.7660041746520782
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.032546334555254,
+                    "heading": 1.6243735534665814e-17,
+                    "angularVelocity": 9.048502047432954e-17,
+                    "velocityX": -2.799411465702586e-33,
+                    "velocityY": 3.7829405719528495,
+                    "timestamp": 0.8617546964835879
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.3383277831364,
+                    "heading": 9.733952786983855e-18,
+                    "angularVelocity": -6.798691664538921e-17,
+                    "velocityX": -8.030123288193386e-33,
+                    "velocityY": 3.193522528464369,
+                    "timestamp": 0.9575052183150977
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.567663887017311,
+                    "heading": 4.864296415465134e-18,
+                    "angularVelocity": -5.08577528116881e-17,
+                    "velocityX": -5.179461626817983e-33,
+                    "velocityY": 2.395142078541023,
+                    "timestamp": 1.0532557401466074
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.720554627994461,
+                    "heading": 1.6208272352196832e-18,
+                    "angularVelocity": -3.3874167136237696e-17,
+                    "velocityX": -4.721587370568867e-33,
+                    "velocityY": 1.5967614385035827,
+                    "timestamp": 1.1490062619781172
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
                     "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 14
+                    "angularVelocity": -1.69276073284528e-17,
+                    "velocityX": -2.4599412486527172e-33,
+                    "velocityY": 0.7983807350946674,
+                    "timestamp": 1.244756783809627
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
                     "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "angularVelocity": 0,
+                    "velocityX": -3.907521125363084e-43,
+                    "velocityY": 8.448865646535191e-39,
+                    "timestamp": 1.3405073056411367
                 }
             ],
-            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -31004,23 +38485,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "C4 To C3": {
             "waypoints": [
@@ -31043,7 +38508,98 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": -2.24426408200091e-39,
+                    "angularVelocity": 3.072610310721988e-47,
+                    "velocityX": -5.239815815905201e-40,
+                    "velocityY": -4.5917748078995606e-40,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.540500005575846,
+                    "heading": -7.160737227180709e-23,
+                    "angularVelocity": -7.15564807370433e-22,
+                    "velocityX": 1.97902852084548e-33,
+                    "velocityY": 0.8344065481228216,
+                    "timestamp": 0.10007112931184187
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.7075000136298466,
+                    "heading": 1.9234971876596208e-22,
+                    "angularVelocity": 2.6376946581601568e-21,
+                    "velocityX": 2.759296119129438e-33,
+                    "velocityY": 1.6688130652907365,
+                    "timestamp": 0.20014225862368373
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.9580000179666155,
+                    "heading": 1.2715262688933367e-21,
+                    "angularVelocity": 1.0784094425251139e-20,
+                    "velocityX": 3.360924516770788e-33,
+                    "velocityY": 2.5032195205488357,
+                    "timestamp": 0.3002133879355256
+                },
+                {
+                    "x": 7.68,
+                    "y": 3.2919999999999994,
+                    "heading": 4.5990005279924634e-21,
+                    "angularVelocity": 3.325109025879841e-20,
+                    "velocityX": 4.212493187097162e-33,
+                    "velocityY": 3.3376257900775026,
+                    "timestamp": 0.40028451724736747
+                },
+                {
+                    "x": 7.68,
+                    "y": 3.6259999820333837,
+                    "heading": 2.8774590551268706e-21,
+                    "angularVelocity": -1.7203177491039364e-20,
+                    "velocityX": -1.8223734723751632e-33,
+                    "velocityY": 3.3376257900775026,
+                    "timestamp": 0.5003556465592094
+                },
+                {
+                    "x": 7.68,
+                    "y": 3.8764999863701526,
+                    "heading": 1.4966782173181084e-21,
+                    "angularVelocity": -1.3797993485663648e-20,
+                    "velocityX": -1.777649344678044e-33,
+                    "velocityY": 2.5032195205488357,
+                    "timestamp": 0.6004267758710513
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.043499994424153,
+                    "heading": 5.147844205579642e-22,
+                    "angularVelocity": -9.81195850164629e-21,
+                    "velocityX": -2.007686184897088e-33,
+                    "velocityY": 1.6688130652907365,
+                    "timestamp": 0.7004979051828932
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": -4.8498876719579466e-42,
+                    "angularVelocity": -5.144185056490002e-21,
+                    "velocityX": -2.233495211598662e-33,
+                    "velocityY": 0.8344065481228216,
+                    "timestamp": 0.8005690344947352
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 9.449822208481667e-41,
+                    "angularVelocity": 9.927610425813228e-40,
+                    "velocityX": 1.9465664275137074e-41,
+                    "velocityY": 5.142787784847508e-39,
+                    "timestamp": 0.9006401638065771
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -31061,23 +38617,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "C4 To C5": {
             "waypoints": [
@@ -31100,7 +38640,98 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": -4.712717929333765e-40,
+                    "angularVelocity": 9.927649781151023e-40,
+                    "velocityX": 1.7772849076267516e-40,
+                    "velocityY": -4.543888050052464e-40,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.3734999944241535,
+                    "heading": 2.8227518572323466e-21,
+                    "angularVelocity": 2.820745460648545e-20,
+                    "velocityX": 2.2465911667965736e-32,
+                    "velocityY": -0.8344065481228212,
+                    "timestamp": 0.10007112931184181
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.206499986370153,
+                    "heading": 8.436925006375396e-21,
+                    "angularVelocity": 5.6101826293588e-20,
+                    "velocityX": 4.4654374072917263e-32,
+                    "velocityY": -1.6688130652907358,
+                    "timestamp": 0.20014225862368362
+                },
+                {
+                    "x": 7.68,
+                    "y": 1.955999982033384,
+                    "heading": 1.682221146414935e-20,
+                    "angularVelocity": 8.379326253074398e-20,
+                    "velocityX": 6.797738711780288e-32,
+                    "velocityY": -2.503219520548835,
+                    "timestamp": 0.30021338793552543
+                },
+                {
+                    "x": 7.68,
+                    "y": 1.6219999999999999,
+                    "heading": 2.78675063552584e-20,
+                    "angularVelocity": 1.1037443968745167e-19,
+                    "velocityX": 8.936946075377416e-32,
+                    "velocityY": -3.3376257900775013,
+                    "timestamp": 0.40028451724736724
+                },
+                {
+                    "x": 7.68,
+                    "y": 1.288000017966616,
+                    "heading": 1.0287640204896551e-20,
+                    "angularVelocity": -1.75673704816788e-19,
+                    "velocityX": 9.060995032099377e-32,
+                    "velocityY": -3.3376257900775013,
+                    "timestamp": 0.500355646559209
+                },
+                {
+                    "x": 7.68,
+                    "y": 1.037500013629847,
+                    "heading": 3.2969803204781124e-21,
+                    "angularVelocity": -6.985690973056607e-20,
+                    "velocityX": 6.916062716279199e-32,
+                    "velocityY": -2.503219520548835,
+                    "timestamp": 0.6004267758710509
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.8705000055758465,
+                    "heading": 6.18120991020044e-22,
+                    "angularVelocity": -2.676955216055553e-20,
+                    "velocityX": 4.34564572466329e-32,
+                    "velocityY": -1.6688130652907358,
+                    "timestamp": 0.7004979051828927
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": -2.2737142324276756e-40,
+                    "angularVelocity": -6.1768164103594875e-21,
+                    "velocityX": 2.131975080632403e-32,
+                    "velocityY": -0.8344065481228212,
+                    "timestamp": 0.8005690344947345
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 2.0993519451368808e-41,
+                    "angularVelocity": 2.4819025971975228e-39,
+                    "velocityX": -8.732390511178075e-43,
+                    "velocityY": -2.571393892423754e-39,
+                    "timestamp": 0.9006401638065763
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -31110,31 +38741,15 @@
                 },
                 {
                     "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
+                        "last"
+                    ],
+                    "type": "StopPoint"
                 }
-            ]
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": []
         },
         "C5 To W1": {
             "waypoints": [
@@ -31145,7 +38760,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 28
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 4.8543877601623535,
+                    "y": 4.064273357391357,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 17
                 },
                 {
                     "x": 2.3,
@@ -31157,7 +38781,314 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 0,
+                    "angularVelocity": -3.516607019090483e-32,
+                    "velocityX": -4.092268936373955e-33,
+                    "velocityY": -4.396806616029776e-33,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.6580594556627535,
+                    "y": 0.8642766421574168,
+                    "heading": -0.06977499787943942,
+                    "angularVelocity": -0.7009716360726537,
+                    "velocityX": -0.22041848409624198,
+                    "velocityY": 0.7763344454252913,
+                    "timestamp": 0.09954040119279155
+                },
+                {
+                    "x": 7.613877684624128,
+                    "y": 1.0194223152597515,
+                    "heading": -0.19807907998906352,
+                    "angularVelocity": -1.2889648883484262,
+                    "velocityX": -0.4438576749661031,
+                    "velocityY": 1.558620130552277,
+                    "timestamp": 0.1990808023855831
+                },
+                {
+                    "x": 7.54706531781781,
+                    "y": 1.253315507091588,
+                    "heading": -0.3641253773176819,
+                    "angularVelocity": -1.6681296773861405,
+                    "velocityX": -0.6712085344815377,
+                    "velocityY": 2.349731255139591,
+                    "timestamp": 0.2986212035783746
+                },
+                {
+                    "x": 7.457648537893544,
+                    "y": 1.5658706931334108,
+                    "heading": -0.49008580394214096,
+                    "angularVelocity": -1.265420122031624,
+                    "velocityX": -0.8982963585919527,
+                    "velocityY": 3.139983185686188,
+                    "timestamp": 0.3981616047711662
+                },
+                {
+                    "x": 7.353528135913809,
+                    "y": 1.9277448806018234,
+                    "heading": -0.490085841920944,
+                    "angularVelocity": -3.8154159059291386e-7,
+                    "velocityX": -1.0460114760646044,
+                    "velocityY": 3.6354503611807685,
+                    "timestamp": 0.49770200596395775
+                },
+                {
+                    "x": 7.249407681423134,
+                    "y": 2.2896190529615703,
+                    "heading": -0.4900858798995876,
+                    "angularVelocity": -3.8153998933914064e-7,
+                    "velocityX": -1.0460120035985523,
+                    "velocityY": 3.635450209396512,
+                    "timestamp": 0.5972424071567493
+                },
+                {
+                    "x": 7.145287117295142,
+                    "y": 2.651493193775847,
+                    "heading": -0.4900859178782245,
+                    "angularVelocity": -3.8153992177326955e-7,
+                    "velocityX": -1.0460131050338848,
+                    "velocityY": 3.6354498924852887,
+                    "timestamp": 0.6967828083495408
+                },
+                {
+                    "x": 7.025437006860337,
+                    "y": 3.00846649318931,
+                    "heading": -0.49008595631381496,
+                    "angularVelocity": -3.8613055604326664e-7,
+                    "velocityX": -1.204034834083884,
+                    "velocityY": 3.586215196401226,
+                    "timestamp": 0.7963232095423323
+                },
+                {
+                    "x": 6.830624533504276,
+                    "y": 3.3307118703307705,
+                    "heading": -0.49008600153372534,
+                    "angularVelocity": -4.542870012894294e-7,
+                    "velocityX": -1.9571196320451347,
+                    "velocityY": 3.237332513029861,
+                    "timestamp": 0.8958636107351238
+                },
+                {
+                    "x": 6.570226686668516,
+                    "y": 3.602717269957059,
+                    "heading": -0.4900860357215963,
+                    "angularVelocity": -3.4345723556551647e-7,
+                    "velocityX": -2.6160015804177474,
+                    "velocityY": 2.7326130532612987,
+                    "timestamp": 0.9954040119279153
+                },
+                {
+                    "x": 6.270670526052627,
+                    "y": 3.8032215967371097,
+                    "heading": -0.4416564414377051,
+                    "angularVelocity": 0.4865320382835506,
+                    "velocityX": -3.009392739292897,
+                    "velocityY": 2.0143009710369753,
+                    "timestamp": 1.094944413120707
+                },
+                {
+                    "x": 5.9498699270968665,
+                    "y": 3.9255658761230467,
+                    "heading": -0.3340733830984328,
+                    "angularVelocity": 1.0807979177309486,
+                    "velocityX": -3.222818022748657,
+                    "velocityY": 1.229091684581206,
+                    "timestamp": 1.1944848143134985
+                },
+                {
+                    "x": 5.637666976812727,
+                    "y": 3.9694128449739723,
+                    "heading": -0.13726885518093873,
+                    "angularVelocity": 1.977132154976145,
+                    "velocityX": -3.136444564649281,
+                    "velocityY": 0.4404941945733328,
+                    "timestamp": 1.2940252155062901
+                },
+                {
+                    "x": 5.333615486534618,
+                    "y": 3.942111372133816,
+                    "heading": 0.08955200005531357,
+                    "angularVelocity": 2.278681344642582,
+                    "velocityX": -3.0545535946677287,
+                    "velocityY": -0.2742752943830166,
+                    "timestamp": 1.3935656166990817
+                },
+                {
+                    "x": 5.078082684497777,
+                    "y": 3.9650753252247246,
+                    "heading": 0.19884455131252843,
+                    "angularVelocity": 1.0979717777662474,
+                    "velocityX": -2.5671265031564414,
+                    "velocityY": 0.2306998245509574,
+                    "timestamp": 1.4931060178918734
+                },
+                {
+                    "x": 4.8543877601623535,
+                    "y": 4.064273357391357,
+                    "heading": 0.3064368289582943,
+                    "angularVelocity": 1.0808905364705061,
+                    "velocityX": -2.2472777048805277,
+                    "velocityY": 0.9965605018459247,
+                    "timestamp": 1.592646419084665
+                },
+                {
+                    "x": 4.700013932982481,
+                    "y": 4.187105651970831,
+                    "heading": 0.3888797273509616,
+                    "angularVelocity": 1.0676410431648378,
+                    "velocityX": -1.99915137751061,
+                    "velocityY": 1.5906864226746236,
+                    "timestamp": 1.6698660978526145
+                },
+                {
+                    "x": 4.564169886311024,
+                    "y": 4.35607150380987,
+                    "heading": 0.4692148602751942,
+                    "angularVelocity": 1.040345339504005,
+                    "velocityX": -1.7591894817340108,
+                    "velocityY": 2.1881190719116157,
+                    "timestamp": 1.747085776620564
+                },
+                {
+                    "x": 4.445019228108457,
+                    "y": 4.571847534724939,
+                    "heading": 0.5443267073647724,
+                    "angularVelocity": 0.9727034389160626,
+                    "velocityX": -1.543008985580261,
+                    "velocityY": 2.7943140188848803,
+                    "timestamp": 1.8243054553885136
+                },
+                {
+                    "x": 4.302573716554072,
+                    "y": 4.826880596020871,
+                    "heading": 0.5443268349443496,
+                    "angularVelocity": 0.0000016521640508021742,
+                    "velocityX": -1.8446788930894609,
+                    "velocityY": 3.302695185541032,
+                    "timestamp": 1.9015251341564632
+                },
+                {
+                    "x": 4.139811862406642,
+                    "y": 5.06945260129317,
+                    "heading": 0.544326895208066,
+                    "angularVelocity": 7.80419154390702e-7,
+                    "velocityX": -2.1077768872432143,
+                    "velocityY": 3.141323677365259,
+                    "timestamp": 1.9787448129244127
+                },
+                {
+                    "x": 3.9382711614032555,
+                    "y": 5.28090933180296,
+                    "heading": 0.5443268879285122,
+                    "angularVelocity": -9.42707078520697e-8,
+                    "velocityX": -2.60996554529876,
+                    "velocityY": 2.738378790013248,
+                    "timestamp": 2.0559644916923623
+                },
+                {
+                    "x": 3.721249786731846,
+                    "y": 5.476445328956613,
+                    "heading": 0.5443268807879367,
+                    "angularVelocity": -9.247093001843253e-8,
+                    "velocityX": -2.810441303745562,
+                    "velocityY": 2.53220422919983,
+                    "timestamp": 2.1331841704603116
+                },
+                {
+                    "x": 3.5042282470745802,
+                    "y": 5.671981142995692,
+                    "heading": 0.5443268736473624,
+                    "angularVelocity": -9.247091428352055e-8,
+                    "velocityX": -2.810443440323428,
+                    "velocityY": 2.53220185785385,
+                    "timestamp": 2.210403849228261
+                },
+                {
+                    "x": 3.287206691377032,
+                    "y": 5.867516939231946,
+                    "heading": 0.5443268665067905,
+                    "angularVelocity": -9.247088327815795e-8,
+                    "velocityX": -2.810443648046155,
+                    "velocityY": 2.5322016273060566,
+                    "timestamp": 2.2876235279962103
+                },
+                {
+                    "x": 3.0701851252220784,
+                    "y": 6.063052723861702,
+                    "heading": 0.5443268593662193,
+                    "angularVelocity": -9.247087442211068e-8,
+                    "velocityX": -2.8104437834702534,
+                    "velocityY": 2.532201477001163,
+                    "timestamp": 2.3648432067641596
+                },
+                {
+                    "x": 2.853163562032648,
+                    "y": 6.258588511782302,
+                    "heading": 0.5443268522252032,
+                    "angularVelocity": -9.247663534512907e-8,
+                    "velocityX": -2.8104437450665345,
+                    "velocityY": 2.5322015196178005,
+                    "timestamp": 2.442062885532109
+                },
+                {
+                    "x": 2.6687757934039245,
+                    "y": 6.424725614600548,
+                    "heading": 0.4938774787934042,
+                    "angularVelocity": -0.6533227570578579,
+                    "velocityX": -2.3878339248576954,
+                    "velocityY": 2.1514865830703513,
+                    "timestamp": 2.5192825643000583
+                },
+                {
+                    "x": 2.521265507868239,
+                    "y": 6.557635351096379,
+                    "heading": 0.4535239115954702,
+                    "angularVelocity": -0.522581391709739,
+                    "velocityX": -1.9102680545844264,
+                    "velocityY": 1.7211899688838848,
+                    "timestamp": 2.5965022430680076
+                },
+                {
+                    "x": 2.4106327602038906,
+                    "y": 6.6573176734827975,
+                    "heading": 0.42326153130587874,
+                    "angularVelocity": -0.391899846935805,
+                    "velocityX": -1.432701474928527,
+                    "velocityY": 1.2908927358526117,
+                    "timestamp": 2.673721921835957
+                },
+                {
+                    "x": 2.336877585524826,
+                    "y": 6.723772559032594,
+                    "heading": 0.40308732310971707,
+                    "angularVelocity": -0.2612573442164476,
+                    "velocityX": -0.9551344405446706,
+                    "velocityY": 0.8605952085024652,
+                    "timestamp": 2.7509416006039062
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 0.393,
+                    "angularVelocity": -0.13063150832354717,
+                    "velocityX": -0.4775671967717696,
+                    "velocityY": 0.43029758084408004,
+                    "timestamp": 2.8281612793718556
+                },
+                {
+                    "x": 2.3,
+                    "y": 6.757,
+                    "heading": 0.393,
+                    "angularVelocity": -5.871044564465464e-32,
+                    "velocityX": 0,
+                    "velocityY": -1.090928833064942e-33,
+                    "timestamp": 2.905380958139805
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -31202,7 +39133,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 17
+                },
+                {
+                    "x": 4.937438011169434,
+                    "y": 4.2165327072143555,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 13
                 },
                 {
                     "x": 2.25,
@@ -31214,7 +39154,287 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 0,
+                    "angularVelocity": -2.906139149989869e-32,
+                    "velocityX": 1.181772084740038e-33,
+                    "velocityY": 2.7502272506846476e-34,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.666043555718406,
+                    "y": 0.8524996120976488,
+                    "heading": 0.004999198621269784,
+                    "angularVelocity": 0.05577597491216818,
+                    "velocityX": -0.15571181404981524,
+                    "velocityY": 0.7307780702234378,
+                    "timestamp": 0.08962996396104499
+                },
+                {
+                    "x": 7.638131101049187,
+                    "y": 0.9835016263770707,
+                    "heading": 0.014524734067527434,
+                    "angularVelocity": 0.10627623871853438,
+                    "velocityX": -0.31141878715192206,
+                    "velocityY": 1.4615872693685117,
+                    "timestamp": 0.17925992792208997
+                },
+                {
+                    "x": 7.596263489657983,
+                    "y": 1.180010140566484,
+                    "heading": 0.02778512041931341,
+                    "angularVelocity": 0.14794590743726296,
+                    "velocityX": -0.46711623592083296,
+                    "velocityY": 2.1924421867983774,
+                    "timestamp": 0.26888989188313495
+                },
+                {
+                    "x": 7.540442918712468,
+                    "y": 1.4420312351509346,
+                    "heading": 0.04318451816280708,
+                    "angularVelocity": 0.17181082154832236,
+                    "velocityX": -0.6227891709269913,
+                    "velocityY": 2.9233649440976053,
+                    "timestamp": 0.35851985584417995
+                },
+                {
+                    "x": 7.470680035396563,
+                    "y": 1.7695653388184471,
+                    "heading": 0.05582613302662815,
+                    "angularVelocity": 0.1410422843561041,
+                    "velocityX": -0.7783433154812639,
+                    "velocityY": 3.654292484261909,
+                    "timestamp": 0.44814981980522495
+                },
+                {
+                    "x": 7.399999368806171,
+                    "y": 2.101181376235394,
+                    "heading": 0.05582613685023459,
+                    "angularVelocity": 4.265991273772723e-8,
+                    "velocityX": -0.7885830080341364,
+                    "velocityY": 3.6998345504308543,
+                    "timestamp": 0.5377797837662699
+                },
+                {
+                    "x": 7.329318638814834,
+                    "y": 2.4327974001390613,
+                    "heading": 0.05582614067384277,
+                    "angularVelocity": 4.265993214747779e-8,
+                    "velocityX": -0.788583715397405,
+                    "velocityY": 3.6998343996634295,
+                    "timestamp": 0.6274097477273148
+                },
+                {
+                    "x": 7.258637432835217,
+                    "y": 2.764413322590103,
+                    "heading": 0.05582614449746126,
+                    "angularVelocity": 4.266004720083137e-8,
+                    "velocityX": -0.7885890259906463,
+                    "velocityY": 3.699833267758185,
+                    "timestamp": 0.7170397116883598
+                },
+                {
+                    "x": 7.145396314308057,
+                    "y": 3.084009006822374,
+                    "heading": 0.05582614848984762,
+                    "angularVelocity": 4.4542987394171874e-8,
+                    "velocityX": -1.2634292542656518,
+                    "velocityY": 3.5657236721770227,
+                    "timestamp": 0.8066696756494047
+                },
+                {
+                    "x": 6.971535677728531,
+                    "y": 3.375105946924151,
+                    "heading": 0.05582615332930866,
+                    "angularVelocity": 5.39937854332775e-8,
+                    "velocityX": -1.9397601973274063,
+                    "velocityY": 3.2477636633692444,
+                    "timestamp": 0.8962996396104497
+                },
+                {
+                    "x": 6.743840876905014,
+                    "y": 3.626342927147427,
+                    "heading": 0.0558261606002303,
+                    "angularVelocity": 8.112155042111421e-8,
+                    "velocityX": -2.5403870621043505,
+                    "velocityY": 2.8030467615993793,
+                    "timestamp": 0.9859296035714946
+                },
+                {
+                    "x": 6.471198532281258,
+                    "y": 3.8279144449501397,
+                    "heading": 0.0558261762788256,
+                    "angularVelocity": 1.7492582391526053e-7,
+                    "velocityX": -3.041866052096724,
+                    "velocityY": 2.2489300329331785,
+                    "timestamp": 1.0755595675325396
+                },
+                {
+                    "x": 6.164249513947577,
+                    "y": 3.9719534129902763,
+                    "heading": 0.05582625958870765,
+                    "angularVelocity": 9.294869524458774e-7,
+                    "velocityX": -3.4246250335109787,
+                    "velocityY": 1.6070403431461682,
+                    "timestamp": 1.1651895314935845
+                },
+                {
+                    "x": 5.836252922691461,
+                    "y": 4.05240893270077,
+                    "heading": 0.05954498226559844,
+                    "angularVelocity": 0.04148972634315725,
+                    "velocityX": -3.6594524505071675,
+                    "velocityY": 0.8976408798452788,
+                    "timestamp": 1.2548194954546295
+                },
+                {
+                    "x": 5.497492188342187,
+                    "y": 4.066758597782368,
+                    "heading": 0.05954602093201425,
+                    "angularVelocity": 0.000011588383726853398,
+                    "velocityX": -3.779547813904135,
+                    "velocityY": 0.16009897190000646,
+                    "timestamp": 1.3444494594156744
+                },
+                {
+                    "x": 5.204246867986104,
+                    "y": 4.110932074419811,
+                    "heading": 0.17401114275805463,
+                    "angularVelocity": 1.2770854384789139,
+                    "velocityX": -3.2717331057226993,
+                    "velocityY": 0.4928427356797903,
+                    "timestamp": 1.4340794233767193
+                },
+                {
+                    "x": 4.937438011169434,
+                    "y": 4.2165327072143555,
+                    "heading": 0.3021760231951433,
+                    "angularVelocity": 1.4299334148208713,
+                    "velocityX": -2.976781926774297,
+                    "velocityY": 1.1781844834885962,
+                    "timestamp": 1.5237093873377643
+                },
+                {
+                    "x": 4.703348327317259,
+                    "y": 4.378813716693983,
+                    "heading": 0.4253911826920849,
+                    "angularVelocity": 1.3960058537628992,
+                    "velocityX": -2.6521945051027034,
+                    "velocityY": 1.8386149895276152,
+                    "timestamp": 1.61197202546698
+                },
+                {
+                    "x": 4.500555108729279,
+                    "y": 4.597666686435954,
+                    "heading": 0.5261840356750895,
+                    "angularVelocity": 1.1419651068603307,
+                    "velocityX": -2.297611117074157,
+                    "velocityY": 2.479565242787912,
+                    "timestamp": 1.7002346635961958
+                },
+                {
+                    "x": 4.326794639818804,
+                    "y": 4.871425359866347,
+                    "heading": 0.5588913487111756,
+                    "angularVelocity": 0.37056804248478087,
+                    "velocityX": -1.968675224233522,
+                    "velocityY": 3.1016371052677254,
+                    "timestamp": 1.7884973017254115
+                },
+                {
+                    "x": 4.0969805858398445,
+                    "y": 5.11364296690146,
+                    "heading": 0.5588923269288569,
+                    "angularVelocity": 0.00001108303243636796,
+                    "velocityX": -2.603752378696331,
+                    "velocityY": 2.744282429905488,
+                    "timestamp": 1.8767599398546273
+                },
+                {
+                    "x": 3.8246168380678105,
+                    "y": 5.306780372295281,
+                    "heading": 0.5588922897097742,
+                    "angularVelocity": -4.2168559122096544e-7,
+                    "velocityX": -3.0858328455274044,
+                    "velocityY": 2.188212470049549,
+                    "timestamp": 1.965022577983843
+                },
+                {
+                    "x": 3.520011793550478,
+                    "y": 5.443527703198532,
+                    "heading": 0.5588922240132788,
+                    "angularVelocity": -7.44329614921164e-7,
+                    "velocityX": -3.451121006278936,
+                    "velocityY": 1.549322950250525,
+                    "timestamp": 2.053285216113059
+                },
+                {
+                    "x": 3.194693864220709,
+                    "y": 5.518709582083228,
+                    "heading": 0.5588921711301718,
+                    "angularVelocity": -5.991562005821763e-7,
+                    "velocityX": -3.6857943091787817,
+                    "velocityY": 0.851797323060201,
+                    "timestamp": 2.1415478542422743
+                },
+                {
+                    "x": 2.8828791952129387,
+                    "y": 5.532174882793972,
+                    "heading": 0.5041280478943673,
+                    "angularVelocity": -0.6204677811196886,
+                    "velocityX": -3.5328047701370098,
+                    "velocityY": 0.15255946339412532,
+                    "timestamp": 2.22981049237149
+                },
+                {
+                    "x": 2.6283322264519793,
+                    "y": 5.543894951157175,
+                    "heading": 0.3346872638422791,
+                    "angularVelocity": -1.9197339626765784,
+                    "velocityX": -2.883971906530878,
+                    "velocityY": 0.132786291137652,
+                    "timestamp": 2.3180731305007054
+                },
+                {
+                    "x": 2.4385492640365714,
+                    "y": 5.5521384887253,
+                    "heading": 0.1780668377028913,
+                    "angularVelocity": -1.7744815865360473,
+                    "velocityX": -2.150207227406541,
+                    "velocityY": 0.09339781523475092,
+                    "timestamp": 2.406335768629921
+                },
+                {
+                    "x": 2.31267915340354,
+                    "y": 5.55742889143524,
+                    "heading": 0.062002293486176877,
+                    "angularVelocity": -1.3149906537667377,
+                    "velocityX": -1.4260859781775244,
+                    "velocityY": 0.0599393222554144,
+                    "timestamp": 2.4945984067591365
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 9.107235017738231e-34,
+                    "angularVelocity": -0.7024749633633888,
+                    "velocityX": -0.7101436659051381,
+                    "velocityY": 0.029130202985730554,
+                    "timestamp": 2.582861044888352
+                },
+                {
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": 0,
+                    "angularVelocity": -2.2232006496445675e-32,
+                    "velocityX": 0,
+                    "velocityY": -2.9660881962078245e-33,
+                    "timestamp": 2.6711236830175675
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -31259,7 +39479,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 19
+                },
+                {
+                    "x": 2.8888590335845947,
+                    "y": 2.6108903884887695,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 12
                 },
                 {
                     "x": 2.3,
@@ -31271,7 +39500,296 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 0,
+                    "angularVelocity": -1.5069359770438582e-30,
+                    "velocityX": 5.69693995403372e-32,
+                    "velocityY": 3.042278119963715e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.627290492953592,
+                    "y": 0.8035142159340071,
+                    "heading": 0.015175150751654183,
+                    "angularVelocity": 0.18617297608634506,
+                    "velocityX": -0.6466549133823012,
+                    "velocityY": 0.20260100070713255,
+                    "timestamp": 0.08151102845675925
+                },
+                {
+                    "x": 7.521871327482097,
+                    "y": 0.8365426259859824,
+                    "heading": 0.045515680376982795,
+                    "angularVelocity": 0.3722260680519286,
+                    "velocityX": -1.2933116839203953,
+                    "velocityY": 0.4052017337690261,
+                    "timestamp": 0.1630220569135185
+                },
+                {
+                    "x": 7.363742244741542,
+                    "y": 0.8860852747673063,
+                    "heading": 0.09100430929463568,
+                    "angularVelocity": 0.5580671693007231,
+                    "velocityX": -1.9399716300284648,
+                    "velocityY": 0.6078030141357551,
+                    "timestamp": 0.24453308537027776
+                },
+                {
+                    "x": 7.152902908699907,
+                    "y": 0.95214234591484,
+                    "heading": 0.15161661460632753,
+                    "angularVelocity": 0.7436086436309897,
+                    "velocityX": -2.5866356986709165,
+                    "velocityY": 0.8104065474106494,
+                    "timestamp": 0.326044113827037
+                },
+                {
+                    "x": 6.889352971860368,
+                    "y": 1.0347142415767305,
+                    "heading": 0.22732297101557483,
+                    "angularVelocity": 0.9287866665726976,
+                    "velocityX": -3.233304030501219,
+                    "velocityY": 1.0130150143510155,
+                    "timestamp": 0.4075551422837963
+                },
+                {
+                    "x": 6.595109187456524,
+                    "y": 1.1269159867790934,
+                    "heading": 0.22732298101704992,
+                    "angularVelocity": 1.22700882310899e-7,
+                    "velocityX": -3.6098647014365244,
+                    "velocityY": 1.1311566906713055,
+                    "timestamp": 0.4890661707405556
+                },
+                {
+                    "x": 6.300865420236581,
+                    "y": 1.219117786820698,
+                    "heading": 0.2273229910184091,
+                    "angularVelocity": 1.2269945998464997e-7,
+                    "velocityX": -3.6098644906196586,
+                    "velocityY": 1.131157363454394,
+                    "timestamp": 0.5705771991973149
+                },
+                {
+                    "x": 6.006621672014901,
+                    "y": 1.3113196474914972,
+                    "heading": 0.2273230010197575,
+                    "angularVelocity": 1.2269932759504928e-7,
+                    "velocityX": -3.6098642575436695,
+                    "velocityY": 1.1311581072702415,
+                    "timestamp": 0.6520882276540741
+                },
+                {
+                    "x": 5.7123779465456614,
+                    "y": 1.4035215807721424,
+                    "heading": 0.22732301102110228,
+                    "angularVelocity": 1.2269928353992546e-7,
+                    "velocityX": -3.6098639784103828,
+                    "velocityY": 1.131158998068056,
+                    "timestamp": 0.7335992561108334
+                },
+                {
+                    "x": 5.418134249112936,
+                    "y": 1.4957236035255936,
+                    "heading": 0.2273230210224565,
+                    "angularVelocity": 1.2269939906393907e-7,
+                    "velocityX": -3.60986363445063,
+                    "velocityY": 1.1311600957453605,
+                    "timestamp": 0.8151102845675927
+                },
+                {
+                    "x": 5.123890658648026,
+                    "y": 1.5879259676440571,
+                    "heading": 0.22732303102385681,
+                    "angularVelocity": 1.2269996504792678e-7,
+                    "velocityX": -3.609862322139673,
+                    "velocityY": 1.1311642837064038,
+                    "timestamp": 0.896621313024352
+                },
+                {
+                    "x": 4.838467298400658,
+                    "y": 1.7046053225590203,
+                    "heading": 0.22732304878104165,
+                    "angularVelocity": 2.1785008908970575e-7,
+                    "velocityX": -3.5016532811726435,
+                    "velocityY": 1.4314548242617389,
+                    "timestamp": 0.9781323414811113
+                },
+                {
+                    "x": 4.5530441892872355,
+                    "y": 1.8212852918009321,
+                    "heading": 0.2273230665381978,
+                    "angularVelocity": 2.1784973753126392e-7,
+                    "velocityX": -3.5016502001914627,
+                    "velocityY": 1.4314623609958343,
+                    "timestamp": 1.0596433699378704
+                },
+                {
+                    "x": 4.267621124217833,
+                    "y": 1.9379653687834795,
+                    "heading": 0.2273230842953322,
+                    "angularVelocity": 2.1784947052459537e-7,
+                    "velocityX": -3.5016496598471556,
+                    "velocityY": 1.4314636827879597,
+                    "timestamp": 1.1411543983946297
+                },
+                {
+                    "x": 3.9821980933555543,
+                    "y": 2.054645529443536,
+                    "heading": 0.22732310205245027,
+                    "angularVelocity": 2.1784927045604472e-7,
+                    "velocityX": -3.5016492401846184,
+                    "velocityY": 1.4314647093669521,
+                    "timestamp": 1.222665426851389
+                },
+                {
+                    "x": 3.6967750938784922,
+                    "y": 2.171325766878081,
+                    "heading": 0.22732311980956,
+                    "angularVelocity": 2.1784916783717796e-7,
+                    "velocityX": -3.501648855142053,
+                    "velocityY": 1.4314656512577588,
+                    "timestamp": 1.3041764553081483
+                },
+                {
+                    "x": 3.411352191599502,
+                    "y": 2.288006242078097,
+                    "heading": 0.2273231375666713,
+                    "angularVelocity": 2.178491871140625e-7,
+                    "velocityX": -3.501647662689034,
+                    "velocityY": 1.4314685682307748,
+                    "timestamp": 1.3856874837649076
+                },
+                {
+                    "x": 3.135544385824121,
+                    "y": 2.425885248705818,
+                    "heading": 0.22732315564878777,
+                    "angularVelocity": 2.218364413735132e-7,
+                    "velocityX": -3.3836869807340535,
+                    "velocityY": 1.6915380561155315,
+                    "timestamp": 1.4671985122216669
+                },
+                {
+                    "x": 2.8888590335845947,
+                    "y": 2.6108903884887695,
+                    "heading": 0.22732317584620773,
+                    "angularVelocity": 2.4778757367040615e-7,
+                    "velocityX": -3.026404609412922,
+                    "velocityY": 2.2696945834893776,
+                    "timestamp": 1.5487095406784261
+                },
+                {
+                    "x": 2.714424659127437,
+                    "y": 2.7861164379371415,
+                    "heading": 0.22732319539367835,
+                    "angularVelocity": 2.990800632496739e-7,
+                    "velocityX": -2.668879509458687,
+                    "velocityY": 2.6809922892290525,
+                    "timestamp": 1.6140681954393303
+                },
+                {
+                    "x": 2.566977650329029,
+                    "y": 2.984587845024715,
+                    "heading": 0.2273232142228265,
+                    "angularVelocity": 2.880895914276183e-7,
+                    "velocityX": -2.2559676195571057,
+                    "velocityY": 3.0366507360596735,
+                    "timestamp": 1.6794268502002345
+                },
+                {
+                    "x": 2.4495779823740462,
+                    "y": 3.20218572489824,
+                    "heading": 0.22732323403642601,
+                    "angularVelocity": 3.0315188694924184e-7,
+                    "velocityX": -1.7962375202558922,
+                    "velocityY": 3.3292894517113587,
+                    "timestamp": 1.7447855049611387
+                },
+                {
+                    "x": 2.3646621591221844,
+                    "y": 3.4343941523127826,
+                    "heading": 0.22732276450933128,
+                    "angularVelocity": -0.000007183855179950579,
+                    "velocityX": -1.2992284428512828,
+                    "velocityY": 3.5528336417572115,
+                    "timestamp": 1.810144159722043
+                },
+                {
+                    "x": 2.3096843159184894,
+                    "y": 3.656196634862642,
+                    "heading": 0.16843360905648927,
+                    "angularVelocity": -0.9010154151467895,
+                    "velocityX": -0.8411715847703609,
+                    "velocityY": 3.3936206820850954,
+                    "timestamp": 1.875502814482947
+                },
+                {
+                    "x": 2.277656929328602,
+                    "y": 3.853541817964372,
+                    "heading": 0.0672675417750508,
+                    "angularVelocity": -1.547860304829062,
+                    "velocityX": -0.49002518039951914,
+                    "velocityY": 3.0194192922676324,
+                    "timestamp": 1.9408614692438513
+                },
+                {
+                    "x": 2.265357919525011,
+                    "y": 4.021569174077037,
+                    "heading": -0.049806740829473055,
+                    "angularVelocity": -1.791259061753831,
+                    "velocityX": -0.188177217670429,
+                    "velocityY": 2.570850895376378,
+                    "timestamp": 2.0062201240047557
+                },
+                {
+                    "x": 2.269649055536998,
+                    "y": 4.158080976089645,
+                    "heading": -0.1685192187929243,
+                    "angularVelocity": -1.8163237661135343,
+                    "velocityX": 0.06565520706762865,
+                    "velocityY": 2.088656850603768,
+                    "timestamp": 2.07157877876566
+                },
+                {
+                    "x": 2.2810300353091644,
+                    "y": 4.259951613943982,
+                    "heading": -0.2719091294332062,
+                    "angularVelocity": -1.581885536330179,
+                    "velocityX": 0.1741311814602323,
+                    "velocityY": 1.5586403702310236,
+                    "timestamp": 2.1369374335265645
+                },
+                {
+                    "x": 2.2926691286250658,
+                    "y": 4.326989538481201,
+                    "heading": -0.34849979264864867,
+                    "angularVelocity": -1.1718518916219969,
+                    "velocityX": 0.1780803683686499,
+                    "velocityY": 1.0256931508529106,
+                    "timestamp": 2.202296088287469
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": -0.39,
+                    "angularVelocity": -0.6349611616573703,
+                    "velocityX": 0.11216374329844404,
+                    "velocityY": 0.5050664160631704,
+                    "timestamp": 2.2676547430483733
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": -0.39,
+                    "angularVelocity": -1.761626207104486e-30,
+                    "velocityX": 0,
+                    "velocityY": -1.839087679439853e-31,
+                    "timestamp": 2.3330133978092777
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -31316,7 +39834,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 19
+                    "controlIntervalCount": 14
+                },
+                {
+                    "x": 4.536026477813721,
+                    "y": 1.628126621246338,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 12
                 },
                 {
                     "x": 4.263,
@@ -31328,7 +39855,251 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 0,
+                    "angularVelocity": -3.982415645335998e-31,
+                    "velocityX": 3.346280928312519e-33,
+                    "velocityY": 3.541078180282768e-38,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.629395539536042,
+                    "y": 0.7881897280541793,
+                    "heading": -0.02207380596023684,
+                    "angularVelocity": -0.2822732032732244,
+                    "velocityX": -0.6471146471374848,
+                    "velocityY": 0.015213885156899113,
+                    "timestamp": 0.0782001468948451
+                },
+                {
+                    "x": 7.528184091504648,
+                    "y": 0.7905692108612611,
+                    "heading": -0.06614402373093474,
+                    "angularVelocity": -0.5635567133951742,
+                    "velocityX": -1.2942616101155584,
+                    "velocityY": 0.030428111729043,
+                    "timestamp": 0.1564002937896902
+                },
+                {
+                    "x": 7.376360973312952,
+                    "y": 0.7941378827255743,
+                    "heading": -0.13206235338942335,
+                    "angularVelocity": -0.8429438086302324,
+                    "velocityX": -1.9414684526853105,
+                    "velocityY": 0.04563510435477082,
+                    "timestamp": 0.23460044068453528
+                },
+                {
+                    "x": 7.173916466766183,
+                    "y": 0.7988917309385671,
+                    "heading": -0.21951772550448787,
+                    "angularVelocity": -1.1183530413716716,
+                    "velocityX": -2.588799568612108,
+                    "velocityY": 0.060790783668257076,
+                    "timestamp": 0.3128005875793804
+                },
+                {
+                    "x": 6.920824289019536,
+                    "y": 0.8048108023879232,
+                    "heading": -0.3276760191057486,
+                    "angularVelocity": -1.383095785572821,
+                    "velocityX": -3.236466781667369,
+                    "velocityY": 0.07569130857196414,
+                    "timestamp": 0.3910007344742255
+                },
+                {
+                    "x": 6.625087122175782,
+                    "y": 0.8120803100358527,
+                    "heading": -0.3276760358371292,
+                    "angularVelocity": -2.1395587179409026e-7,
+                    "velocityX": -3.781798098684327,
+                    "velocityY": 0.09296028123113304,
+                    "timestamp": 0.4692008813690706
+                },
+                {
+                    "x": 6.329343587367458,
+                    "y": 0.8190859886904246,
+                    "heading": -0.3276760513572638,
+                    "angularVelocity": -1.9846682128968708e-7,
+                    "velocityX": -3.78187953030339,
+                    "velocityY": 0.08958651527359804,
+                    "timestamp": 0.5474010282639157
+                },
+                {
+                    "x": 6.033600055760057,
+                    "y": 0.826091802470254,
+                    "heading": -0.3276760668773978,
+                    "angularVelocity": -1.9846681457625392e-7,
+                    "velocityX": -3.7818794893709464,
+                    "velocityY": 0.0895882432148227,
+                    "timestamp": 0.6256011751587608
+                },
+                {
+                    "x": 5.7383102527596765,
+                    "y": 0.8439033174946344,
+                    "heading": -0.3276760825327013,
+                    "angularVelocity": -2.0019532088764418e-7,
+                    "velocityX": -3.7760773441696487,
+                    "velocityY": 0.22776830647924015,
+                    "timestamp": 0.7038013220536059
+                },
+                {
+                    "x": 5.450465299571764,
+                    "y": 0.9121572529445121,
+                    "heading": -0.32767609968005607,
+                    "angularVelocity": -2.1927522433491614e-7,
+                    "velocityX": -3.680874839978332,
+                    "velocityY": 0.8728108342551084,
+                    "timestamp": 0.782001468948451
+                },
+                {
+                    "x": 5.178616904201158,
+                    "y": 1.028826716826592,
+                    "heading": -0.32767612118071976,
+                    "angularVelocity": -2.749440322201668e-7,
+                    "velocityX": -3.4763156613508643,
+                    "velocityY": 1.4919340757663395,
+                    "timestamp": 0.8602016158432961
+                },
+                {
+                    "x": 4.930841529412244,
+                    "y": 1.190445624246428,
+                    "heading": -0.3276761534331376,
+                    "angularVelocity": -4.1243423626599064e-7,
+                    "velocityX": -3.1684771017365216,
+                    "velocityY": 2.0667340642903906,
+                    "timestamp": 0.9384017627381412
+                },
+                {
+                    "x": 4.7145004488669215,
+                    "y": 1.3922123936122235,
+                    "heading": -0.32767621878386327,
+                    "angularVelocity": -8.356854591223944e-7,
+                    "velocityX": -2.7665047846524513,
+                    "velocityY": 2.5801328690255976,
+                    "timestamp": 1.0166019096329864
+                },
+                {
+                    "x": 4.536026477813721,
+                    "y": 1.628126621246338,
+                    "heading": -0.327696539002069,
+                    "angularVelocity": -0.00025984882823803823,
+                    "velocityX": -2.282271557538034,
+                    "velocityY": 3.016800313073996,
+                    "timestamp": 1.0948020565278314
+                },
+                {
+                    "x": 4.4347589269746495,
+                    "y": 1.8009845745755027,
+                    "heading": -0.3311064514524199,
+                    "angularVelocity": -0.06394544615832723,
+                    "velocityX": -1.899051313343304,
+                    "velocityY": 3.241572651564751,
+                    "timestamp": 1.1481273933038434
+                },
+                {
+                    "x": 4.355560152796487,
+                    "y": 1.9823487857199633,
+                    "heading": -0.3404702376957046,
+                    "angularVelocity": -0.17559732033810904,
+                    "velocityX": -1.4851996999249673,
+                    "velocityY": 3.4010889027473166,
+                    "timestamp": 1.2014527300798554
+                },
+                {
+                    "x": 4.29961502910342,
+                    "y": 2.1668323093896307,
+                    "heading": -0.3620606217709223,
+                    "angularVelocity": -0.40488040734269476,
+                    "velocityX": -1.049128370778876,
+                    "velocityY": 3.4595847832064672,
+                    "timestamp": 1.2547780668558675
+                },
+                {
+                    "x": 4.26624261518445,
+                    "y": 2.3445509425061104,
+                    "heading": -0.3930653835662206,
+                    "angularVelocity": -0.5814264600999738,
+                    "velocityX": -0.6258265945712352,
+                    "velocityY": 3.332724064417915,
+                    "timestamp": 1.3081034036318795
+                },
+                {
+                    "x": 4.250245555353669,
+                    "y": 2.506179624728893,
+                    "heading": -0.42827286207411436,
+                    "angularVelocity": -0.6602392152823773,
+                    "velocityX": -0.2999898509389163,
+                    "velocityY": 3.0309922448647635,
+                    "timestamp": 1.3614287404078915
+                },
+                {
+                    "x": 4.245253885327146,
+                    "y": 2.646807044638539,
+                    "heading": -0.46326114250173545,
+                    "angularVelocity": -0.6561286349527023,
+                    "velocityX": -0.09360784813172378,
+                    "velocityY": 2.6371595270071015,
+                    "timestamp": 1.4147540771839036
+                },
+                {
+                    "x": 4.246491895421447,
+                    "y": 2.764573131222025,
+                    "heading": -0.49520995766037457,
+                    "angularVelocity": -0.5991301150594935,
+                    "velocityX": 0.023216170194527277,
+                    "velocityY": 2.2084452476729117,
+                    "timestamp": 1.4680794139599156
+                },
+                {
+                    "x": 4.250760184591696,
+                    "y": 2.858861309855261,
+                    "heading": -0.522398988898447,
+                    "angularVelocity": -0.5098707834110707,
+                    "velocityX": 0.08004242313776105,
+                    "velocityY": 1.7681684605074017,
+                    "timestamp": 1.5214047507359276
+                },
+                {
+                    "x": 4.255888225984497,
+                    "y": 2.929511890736146,
+                    "heading": -0.5437259487715407,
+                    "angularVelocity": -0.3999404628668233,
+                    "velocityX": 0.09616519468543994,
+                    "velocityY": 1.324897040550944,
+                    "timestamp": 1.5747300875119397
+                },
+                {
+                    "x": 4.260343621579396,
+                    "y": 2.9765343465211713,
+                    "heading": -0.5584397105575287,
+                    "angularVelocity": -0.2759244043319095,
+                    "velocityX": 0.08355119468920533,
+                    "velocityY": 0.8818032595380478,
+                    "timestamp": 1.6280554242879517
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": -0.566,
+                    "angularVelocity": -0.14177668439456792,
+                    "velocityX": 0.04981456435476623,
+                    "velocityY": 0.4400469813703916,
+                    "timestamp": 1.6813807610639637
+                },
+                {
+                    "x": 4.263,
+                    "y": 3,
+                    "heading": -0.566,
+                    "angularVelocity": 0,
+                    "velocityX": 3.549235715029611e-32,
+                    "velocityY": -9.598836837089386e-32,
+                    "timestamp": 1.7347060978399758
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -31385,7 +40156,152 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 0,
+                    "angularVelocity": 2.8467808772943154e-31,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.633526252649942,
+                    "y": 0.8024218861992577,
+                    "heading": -0.02675508445787236,
+                    "angularVelocity": -0.3471453590528024,
+                    "velocityX": -0.602993637929559,
+                    "velocityY": 0.20009790028293237,
+                    "timestamp": 0.07707170428800919
+                },
+                {
+                    "x": 7.540488865885011,
+                    "y": 0.8333059401759839,
+                    "heading": -0.07783349665964928,
+                    "angularVelocity": -0.6627388439588938,
+                    "velocityX": -1.2071536191448495,
+                    "velocityY": 0.400718451240102,
+                    "timestamp": 0.15414340857601838
+                },
+                {
+                    "x": 7.400762361365792,
+                    "y": 0.8797032018771439,
+                    "heading": -0.14939031986058435,
+                    "angularVelocity": -0.9284448016555393,
+                    "velocityX": -1.8129416730824437,
+                    "velocityY": 0.6020012419574645,
+                    "timestamp": 0.23121511286402757
+                },
+                {
+                    "x": 7.214174023420098,
+                    "y": 0.9416744881041137,
+                    "heading": -0.23458534610519022,
+                    "angularVelocity": -1.105399537114693,
+                    "velocityX": -2.4209707008480317,
+                    "velocityY": 0.8040731264406625,
+                    "timestamp": 0.30828681715203676
+                },
+                {
+                    "x": 6.98058449689491,
+                    "y": 1.0192605514016468,
+                    "heading": -0.3182722929391017,
+                    "angularVelocity": -1.085832311702643,
+                    "velocityX": -3.030807852026836,
+                    "velocityY": 1.0066737723562156,
+                    "timestamp": 0.38535852144004595
+                },
+                {
+                    "x": 6.7044949607857305,
+                    "y": 1.1109419048147462,
+                    "heading": -0.3204977394277859,
+                    "angularVelocity": -0.02887501333002772,
+                    "velocityX": -3.5822425189594846,
+                    "velocityY": 1.189559180766205,
+                    "timestamp": 0.46243022572805514
+                },
+                {
+                    "x": 6.427835578289735,
+                    "y": 1.202949830409229,
+                    "heading": -0.32049776831244,
+                    "angularVelocity": -3.7477637567079044e-7,
+                    "velocityX": -3.5896362361749183,
+                    "velocityY": 1.1937964320946928,
+                    "timestamp": 0.5395019300160644
+                },
+                {
+                    "x": 6.1511761970625,
+                    "y": 1.294957759818759,
+                    "heading": -0.3204977971970938,
+                    "angularVelocity": -3.7477637370825724e-7,
+                    "velocityX": -3.589636219712834,
+                    "velocityY": 1.1937964815946673,
+                    "timestamp": 0.6165736343040735
+                },
+                {
+                    "x": 5.87524941446386,
+                    "y": 1.3871813457379307,
+                    "heading": -0.32266464886855784,
+                    "angularVelocity": -0.028114749653993878,
+                    "velocityX": -3.5801308034856563,
+                    "velocityY": 1.1965946098005253,
+                    "timestamp": 0.6936453385920827
+                },
+                {
+                    "x": 5.641703675217654,
+                    "y": 1.4649149367510799,
+                    "heading": -0.4060390277887848,
+                    "angularVelocity": -1.081776764773039,
+                    "velocityX": -3.030239715129037,
+                    "velocityY": 1.0085879342004245,
+                    "timestamp": 0.7707170428800918
+                },
+                {
+                    "x": 5.455154020778328,
+                    "y": 1.5270189169325792,
+                    "heading": -0.49098864699750666,
+                    "angularVelocity": -1.1022153979010723,
+                    "velocityX": -2.420468785044752,
+                    "velocityY": 0.8057948212669979,
+                    "timestamp": 0.8477887471681009
+                },
+                {
+                    "x": 5.315462796839212,
+                    "y": 1.573537321073397,
+                    "heading": -0.5623285861822422,
+                    "angularVelocity": -0.9256307466375128,
+                    "velocityX": -1.8124839100106522,
+                    "velocityY": 0.6035730566821683,
+                    "timestamp": 0.9248604514561101
+                },
+                {
+                    "x": 5.222455052636063,
+                    "y": 1.6045200564296747,
+                    "heading": -0.613275527154411,
+                    "angularVelocity": -0.6610330139033284,
+                    "velocityX": -1.2067690089684353,
+                    "velocityY": 0.40199883527290925,
+                    "timestamp": 1.0019321557441192
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": -0.64,
+                    "angularVelocity": -0.34674817551357506,
+                    "velocityX": -0.602751075316368,
+                    "velocityY": 0.2008511906325378,
+                    "timestamp": 1.0790038600321283
+                },
+                {
+                    "x": 5.176,
+                    "y": 1.62,
+                    "heading": -0.64,
+                    "angularVelocity": 8.760742141155177e-29,
+                    "velocityX": 3.313743560366677e-30,
+                    "velocityY": 1.2206105198294578e-30,
+                    "timestamp": 1.1560755643201375
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -31403,46 +40319,247 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
+            "circleObstacles": []
+        },
+        "C5 To C1": {
+            "waypoints": [
                 {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 23
                 },
                 {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": -1.1520076473353784e-31,
+                    "timestamp": 0
                 },
                 {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C5 To C1": {
-            "waypoints": [
+                    "x": 7.68,
+                    "y": 0.8649552704871435,
+                    "heading": -1.2072196888077206e-18,
+                    "angularVelocity": -1.2485272283928241e-17,
+                    "velocityX": -1.2865550388289577e-22,
+                    "velocityY": 0.8062267267095138,
+                    "timestamp": 0.09669149868710694
+                },
+                {
+                    "x": 7.68,
+                    "y": 1.0208658083668138,
+                    "heading": -3.6224298175784965e-18,
+                    "angularVelocity": -2.497851580797786e-17,
+                    "velocityX": -3.7534290948711127e-22,
+                    "velocityY": 1.6124534214139756,
+                    "timestamp": 0.19338299737421388
+                },
+                {
+                    "x": 7.68,
+                    "y": 1.254731607449781,
+                    "heading": -7.246204108405477e-18,
+                    "angularVelocity": -3.7477692864160655e-17,
+                    "velocityX": -8.581224837351263e-22,
+                    "velocityY": 2.4186800521083573,
+                    "timestamp": 0.29007449606132085
+                },
+                {
+                    "x": 7.68,
+                    "y": 1.5665526491683786,
+                    "heading": -1.208276035533523e-17,
+                    "angularVelocity": -5.0020491067317066e-17,
+                    "velocityX": -2.0493771641846554e-21,
+                    "velocityY": 3.2249064907727645,
+                    "timestamp": 0.38676599474842777
+                },
+                {
+                    "x": 7.68,
+                    "y": 1.9323308421442915,
+                    "heading": -4.9593132234185194e-18,
+                    "angularVelocity": 7.367190734076976e-17,
+                    "velocityX": 8.922575847291113e-21,
+                    "velocityY": 3.782940568121389,
+                    "timestamp": 0.4834574934355347
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.298109035120243,
+                    "heading": 4.533704301231546e-17,
+                    "angularVelocity": 5.201735097557347e-16,
+                    "velocityX": 4.097272417616201e-22,
+                    "velocityY": 3.782940568121787,
+                    "timestamp": 0.5801489921226416
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.6638872280961943,
+                    "heading": 2.5191507323029372e-17,
+                    "angularVelocity": -2.083485721374208e-16,
+                    "velocityX": 1.8682331175835897e-21,
+                    "velocityY": 3.782940568121787,
+                    "timestamp": 0.6768404908097485
+                },
+                {
+                    "x": 7.68,
+                    "y": 3.0296654210721456,
+                    "heading": 5.2668856754382746e-17,
+                    "angularVelocity": 2.8417544256148633e-16,
+                    "velocityX": 2.5170076181953607e-21,
+                    "velocityY": 3.782940568121787,
+                    "timestamp": 0.7735319894968554
+                },
+                {
+                    "x": 7.68,
+                    "y": 3.395443614048097,
+                    "heading": 4.929611386988728e-17,
+                    "angularVelocity": -3.48814831737134e-17,
+                    "velocityX": 3.0934195236040766e-21,
+                    "velocityY": 3.782940568121787,
+                    "timestamp": 0.8702234881839623
+                },
+                {
+                    "x": 7.68,
+                    "y": 3.7612218070240484,
+                    "heading": 4.8005098046144365e-17,
+                    "angularVelocity": -1.3351906227498234e-17,
+                    "velocityX": -1.4483318353601114e-20,
+                    "velocityY": 3.782940568121787,
+                    "timestamp": 0.9669149868710692
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 3.8136822799879804e-17,
+                    "angularVelocity": -1.0205938867695289e-16,
+                    "velocityX": -1.0465805642283078e-22,
+                    "velocityY": 3.782940568121787,
+                    "timestamp": 1.0636064855581762
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.492778192975952,
+                    "heading": 4.310486593468924e-17,
+                    "angularVelocity": 5.1380350929111975e-17,
+                    "velocityX": 2.042774398860212e-21,
+                    "velocityY": 3.782940568121787,
+                    "timestamp": 1.1602979842452832
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.8585563859519025,
+                    "heading": 1.5339712420766394e-17,
+                    "angularVelocity": -2.8715196155735124e-16,
+                    "velocityX": 3.895366641644577e-22,
+                    "velocityY": 3.782940568121787,
+                    "timestamp": 1.2569894829323902
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.224334578927854,
+                    "heading": -2.710858238227145e-17,
+                    "angularVelocity": -4.390075175101735e-16,
+                    "velocityX": -3.863371094269421e-21,
+                    "velocityY": 3.782940568121787,
+                    "timestamp": 1.3536809816194972
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.590112771903805,
+                    "heading": -3.378098304971619e-17,
+                    "angularVelocity": -6.900710774001486e-17,
+                    "velocityX": -4.042329982282679e-22,
+                    "velocityY": 3.782940568121787,
+                    "timestamp": 1.4503724803066043
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.955890964879757,
+                    "heading": -3.941496183315801e-17,
+                    "angularVelocity": -5.826757119208287e-17,
+                    "velocityX": 2.6937052538433574e-21,
+                    "velocityY": 3.782940568121787,
+                    "timestamp": 1.5470639789937113
+                },
+                {
+                    "x": 7.68,
+                    "y": 6.321669157855708,
+                    "heading": -1.9923808577496458e-17,
+                    "angularVelocity": 2.0158083720198419e-16,
+                    "velocityX": -4.499746264941967e-21,
+                    "velocityY": 3.782940568121787,
+                    "timestamp": 1.6437554776808183
+                },
+                {
+                    "x": 7.68,
+                    "y": 6.68744735083162,
+                    "heading": 1.1959451428437676e-17,
+                    "angularVelocity": 3.297421225035248e-16,
+                    "velocityX": 5.431014750114918e-21,
+                    "velocityY": 3.782940568121389,
+                    "timestamp": 1.7404469763679253
+                },
+                {
+                    "x": 7.68,
+                    "y": 6.999268392550218,
+                    "heading": 7.18685516064291e-18,
+                    "angularVelocity": -4.935900605111643e-17,
+                    "velocityX": -2.4046856363551417e-22,
+                    "velocityY": 3.2249064907727645,
+                    "timestamp": 1.8371384750550324
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.2331341916331855,
+                    "heading": 3.596188330325507e-18,
+                    "angularVelocity": -3.713528985888002e-17,
+                    "velocityX": -1.8035546521541034e-22,
+                    "velocityY": 2.4186800521083573,
+                    "timestamp": 1.9338299737421394
+                },
                 {
                     "x": 7.68,
-                    "y": 0.787,
+                    "y": 7.3890447295128565,
+                    "heading": 1.199348678010309e-18,
+                    "angularVelocity": -2.4788525206448663e-17,
+                    "velocityX": -1.2023814854884031e-22,
+                    "velocityY": 1.6124534214139756,
+                    "timestamp": 2.030521472429246
+                },
+                {
+                    "x": 7.68,
+                    "y": 7.467,
                     "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 23
+                    "angularVelocity": -1.2403868947555692e-17,
+                    "velocityX": -6.011942579686287e-23,
+                    "velocityY": 0.8062267267095138,
+                    "timestamp": 2.127212971116353
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
                     "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "angularVelocity": 0,
+                    "velocityX": -2.719452927302828e-33,
+                    "velocityY": 0,
+                    "timestamp": 2.2239044698034602
                 }
             ],
-            "trajectory": [],
             "constraints": [
                 {
                     "scope": [
@@ -31460,23 +40577,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "C5 To C2": {
             "waypoints": [
@@ -31499,7 +40600,179 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 2.5195168592304314e-32,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.8688441539517631,
+                    "heading": 1.4487899486085006e-18,
+                    "angularVelocity": 1.4623322004407643e-17,
+                    "velocityX": 2.341988908112997e-19,
+                    "velocityY": 0.8260917454276263,
+                    "timestamp": 0.09907392781100396
+                },
+                {
+                    "x": 7.68,
+                    "y": 1.03253245874193,
+                    "heading": 4.3483011920968684e-18,
+                    "angularVelocity": 2.926613800008521e-17,
+                    "velocityX": 4.684469740721679e-19,
+                    "velocityY": 1.6521834594306495,
+                    "timestamp": 0.19814785562200793
+                },
+                {
+                    "x": 7.68,
+                    "y": 1.2780649081437847,
+                    "heading": 8.701494230539972e-18,
+                    "angularVelocity": 4.393883572228609e-17,
+                    "velocityX": 7.027934476495029e-19,
+                    "velocityY": 2.4782751105844794,
+                    "timestamp": 0.29722178343301187
+                },
+                {
+                    "x": 7.68,
+                    "y": 1.6054414834771922,
+                    "heading": 1.4516368843485863e-17,
+                    "angularVelocity": 5.869227900187472e-17,
+                    "velocityX": 9.37435089599571e-19,
+                    "velocityY": 3.304366573190877,
+                    "timestamp": 0.39629571124401586
+                },
+                {
+                    "x": 7.68,
+                    "y": 1.9802322649266864,
+                    "heading": 6.647154079192576e-18,
+                    "angularVelocity": -7.942770553424421e-17,
+                    "velocityX": 1.0709424285811041e-18,
+                    "velocityY": 3.782940575087071,
+                    "timestamp": 0.49536963905501985
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.3550230463762047,
+                    "heading": -4.11606275908414e-17,
+                    "angularVelocity": -4.825465460619937e-16,
+                    "velocityX": 1.073083844244948e-18,
+                    "velocityY": 3.7829405750873115,
+                    "timestamp": 0.5944435668660238
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.729813827825723,
+                    "heading": -7.635066611173202e-17,
+                    "angularVelocity": -3.5518969822232415e-16,
+                    "velocityX": 1.0738946259920485e-18,
+                    "velocityY": 3.782940575087312,
+                    "timestamp": 0.6935174946770278
+                },
+                {
+                    "x": 7.68,
+                    "y": 3.1046046092752415,
+                    "heading": -5.864574302033697e-17,
+                    "angularVelocity": 1.787041604453991e-16,
+                    "velocityX": 1.0691415340399175e-18,
+                    "velocityY": 3.782940575087312,
+                    "timestamp": 0.7925914224880318
+                },
+                {
+                    "x": 7.68,
+                    "y": 3.4793953907247595,
+                    "heading": -2.6391173603704442e-17,
+                    "angularVelocity": 3.255606205313886e-16,
+                    "velocityX": 1.0732289061667476e-18,
+                    "velocityY": 3.782940575087312,
+                    "timestamp": 0.8916653502990358
+                },
+                {
+                    "x": 7.68,
+                    "y": 3.854186172174278,
+                    "heading": -6.511485011951871e-17,
+                    "angularVelocity": -3.9085637736788184e-16,
+                    "velocityX": 1.0716697150075795e-18,
+                    "velocityY": 3.782940575087312,
+                    "timestamp": 0.9907392781100398
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.228976953623796,
+                    "heading": -6.241329479526944e-17,
+                    "angularVelocity": 2.7268075304360027e-17,
+                    "velocityX": 1.0686111075844087e-18,
+                    "velocityY": 3.782940575087312,
+                    "timestamp": 1.0898132059210437
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.603767735073314,
+                    "heading": -2.2737833863559818e-17,
+                    "angularVelocity": 4.004631875225205e-16,
+                    "velocityX": 1.0710197916722359e-18,
+                    "velocityY": 3.7829405750873115,
+                    "timestamp": 1.1888871337320477
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.978558516522808,
+                    "heading": -1.448944579275074e-17,
+                    "angularVelocity": 8.325488100707829e-17,
+                    "velocityX": 1.0714504382145178e-18,
+                    "velocityY": 3.782940575087071,
+                    "timestamp": 1.2879610615430517
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.305935091856216,
+                    "heading": -8.687037170545135e-18,
+                    "angularVelocity": 5.856645386228716e-17,
+                    "velocityX": 9.36753995288457e-19,
+                    "velocityY": 3.304366573190877,
+                    "timestamp": 1.3870349893540557
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.55146754125807,
+                    "heading": -4.341905072762387e-18,
+                    "angularVelocity": 4.385747283685971e-17,
+                    "velocityX": 7.025655481927048e-19,
+                    "velocityY": 2.4782751105844794,
+                    "timestamp": 1.4861089171650597
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.715155846048237,
+                    "heading": -1.4469357825353783e-18,
+                    "angularVelocity": 2.9220293918225814e-17,
+                    "velocityX": 4.683770471216647e-19,
+                    "velocityY": 1.6521834594306495,
+                    "timestamp": 1.5851828449760637
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 0,
+                    "angularVelocity": 1.460460702963924e-17,
+                    "velocityX": 2.341885280588165e-19,
+                    "velocityY": 0.8260917454276263,
+                    "timestamp": 1.6842567727870676
+                },
+                {
+                    "x": 7.68,
+                    "y": 5.797,
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": 3.1812192470207854e-34,
+                    "velocityY": 0,
+                    "timestamp": 1.7833307005980716
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -31517,23 +40790,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "C5 To C3": {
             "waypoints": [
@@ -31556,7 +40813,143 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 0,
+                    "angularVelocity": 2.417290808823166e-40,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.8634453720055388,
+                    "heading": 1.009810999063182e-18,
+                    "angularVelocity": 1.0546271494620871e-17,
+                    "velocityX": -3.226662245674165e-34,
+                    "velocityY": 0.7983807350946677,
+                    "timestamp": 0.09575052183150978
+                },
+                {
+                    "x": 7.68,
+                    "y": 1.0163361129826893,
+                    "heading": 3.0323418642587253e-18,
+                    "angularVelocity": 2.112292263585315e-17,
+                    "velocityX": -6.037770263383471e-34,
+                    "velocityY": 1.596761438503583,
+                    "timestamp": 0.19150104366301957
+                },
+                {
+                    "x": 7.68,
+                    "y": 1.2456722168635992,
+                    "heading": 6.073900773319643e-18,
+                    "angularVelocity": 3.1765455168164525e-17,
+                    "velocityX": -4.821612934727609e-34,
+                    "velocityY": 2.3951420785410233,
+                    "timestamp": 0.28725156549452935
+                },
+                {
+                    "x": 7.68,
+                    "y": 1.5514536654447455,
+                    "heading": 1.0152788689831222e-17,
+                    "angularVelocity": 4.2599119433487623e-17,
+                    "velocityX": -1.2140491970354773e-33,
+                    "velocityY": 3.19352252846437,
+                    "timestamp": 0.38300208732603913
+                },
+                {
+                    "x": 7.68,
+                    "y": 1.9136721992668209,
+                    "heading": 4.411917253468209e-18,
+                    "angularVelocity": -5.995655508243724e-17,
+                    "velocityX": 4.90795348832912e-33,
+                    "velocityY": 3.7829405719528495,
+                    "timestamp": 0.4787526091575489
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.27589073308894,
+                    "heading": 1.2796107303848674e-17,
+                    "angularVelocity": 8.756286535058141e-17,
+                    "velocityX": -1.5435756690671252e-33,
+                    "velocityY": 3.782940571953311,
+                    "timestamp": 0.5745031309890587
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.63810926691106,
+                    "heading": 6.994895399550348e-18,
+                    "angularVelocity": -6.058673930264937e-17,
+                    "velocityX": 4.776897057793067e-34,
+                    "velocityY": 3.782940571953311,
+                    "timestamp": 0.6702536528205685
+                },
+                {
+                    "x": 7.68,
+                    "y": 3.0003278007331793,
+                    "heading": -9.534020306729642e-18,
+                    "angularVelocity": -1.7262481070720064e-16,
+                    "velocityX": 2.9275045647973183e-33,
+                    "velocityY": 3.782940571953311,
+                    "timestamp": 0.7660041746520783
+                },
+                {
+                    "x": 7.68,
+                    "y": 3.3625463345552546,
+                    "heading": -1.0013819273727212e-17,
+                    "angularVelocity": -5.010927959660929e-18,
+                    "velocityX": 2.237784317663532e-33,
+                    "velocityY": 3.7829405719528495,
+                    "timestamp": 0.861754696483588
+                },
+                {
+                    "x": 7.68,
+                    "y": 3.6683277831364007,
+                    "heading": -6.0064636534747415e-18,
+                    "angularVelocity": 4.185204993640188e-17,
+                    "velocityX": -2.2313398142102424e-33,
+                    "velocityY": 3.19352252846437,
+                    "timestamp": 0.9575052183150978
+                },
+                {
+                    "x": 7.68,
+                    "y": 3.897663887017311,
+                    "heading": -3.002975312877452e-18,
+                    "angularVelocity": 3.136785349810471e-17,
+                    "velocityX": -2.8833027864886262e-33,
+                    "velocityY": 2.3951420785410233,
+                    "timestamp": 1.0532557401466076
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.050554627994461,
+                    "heading": -1.0009557447862449e-18,
+                    "angularVelocity": 2.0908706606963636e-17,
+                    "velocityX": -3.742421273800505e-33,
+                    "velocityY": 1.596761438503583,
+                    "timestamp": 1.1490062619781174
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 0,
+                    "angularVelocity": 1.0453788927770355e-17,
+                    "velocityX": -1.5032681111068507e-33,
+                    "velocityY": 0.7983807350946677,
+                    "timestamp": 1.2447567838096272
+                },
+                {
+                    "x": 7.68,
+                    "y": 4.127,
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": 9.665729080515376e-43,
+                    "velocityY": 2.80463986608038e-38,
+                    "timestamp": 1.340507305641137
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -31574,23 +40967,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "C5 To C4": {
             "waypoints": [
@@ -31613,7 +40990,98 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 9.069206296242399e-40,
+                    "angularVelocity": -9.927610039652183e-40,
+                    "velocityX": -8.253947176298157e-40,
+                    "velocityY": 1.8367099231598242e-40,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.68,
+                    "y": 0.8705000055758465,
+                    "heading": 6.490773345690525e-22,
+                    "angularVelocity": 6.486159683422905e-21,
+                    "velocityX": -7.132918469580153e-33,
+                    "velocityY": 0.8344065481228213,
+                    "timestamp": 0.10007112931184182
+                },
+                {
+                    "x": 7.68,
+                    "y": 1.037500013629847,
+                    "heading": 2.023593010760001e-21,
+                    "angularVelocity": 1.3735386658326633e-20,
+                    "velocityX": -1.5648094960655976e-32,
+                    "velocityY": 1.668813065290736,
+                    "timestamp": 0.20014225862368365
+                },
+                {
+                    "x": 7.68,
+                    "y": 1.2880000179666162,
+                    "heading": 3.787875676222919e-21,
+                    "angularVelocity": 1.7630286060953193e-20,
+                    "velocityX": -2.3089760494009302e-32,
+                    "velocityY": 2.503219520548835,
+                    "timestamp": 0.3002133879355255
+                },
+                {
+                    "x": 7.68,
+                    "y": 1.6219999999999999,
+                    "heading": 5.860880748165694e-21,
+                    "angularVelocity": 2.0715315514205814e-20,
+                    "velocityX": -3.0704903308303665e-32,
+                    "velocityY": 3.3376257900775013,
+                    "timestamp": 0.4002845172473673
+                },
+                {
+                    "x": 7.68,
+                    "y": 1.9559999820333838,
+                    "heading": 3.934658943161884e-21,
+                    "angularVelocity": -1.9248526176341983e-20,
+                    "velocityX": -4.040562356218352e-32,
+                    "velocityY": 3.3376257900775017,
+                    "timestamp": 0.5003556465592092
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.206499986370153,
+                    "heading": 1.6176834885162177e-21,
+                    "angularVelocity": -2.3153285409554772e-20,
+                    "velocityX": -3.08903871620124e-32,
+                    "velocityY": 2.503219520548835,
+                    "timestamp": 0.600426775871051
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.3734999944241535,
+                    "heading": 4.992981261257204e-22,
+                    "angularVelocity": -1.1175904096118944e-20,
+                    "velocityX": -2.3331216062245415e-32,
+                    "velocityY": 1.668813065290736,
+                    "timestamp": 0.7004979051828928
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 1.7562757360140444e-40,
+                    "angularVelocity": -4.9894322348928345e-21,
+                    "velocityX": -1.3647931662662733e-32,
+                    "velocityY": 0.8344065481228213,
+                    "timestamp": 0.8005690344947346
+                },
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": -7.274275970183695e-41,
+                    "angularVelocity": -2.4819026120794767e-39,
+                    "velocityX": -8.637198685981915e-42,
+                    "velocityY": 5.142787784847508e-39,
+                    "timestamp": 0.9006401638065764
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
@@ -31631,25 +41099,9 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         }
     },
     "splitTrajectoriesAtStopPoints": false,
-    "usesObstacles": false
+    "usesObstacles": true
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C2.1.traj
new file mode 100644
index 00000000..50686485
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C2.1.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -5.382102664363315e-41,
+            "angularVelocity": -2.8652892976895567e-45,
+            "velocityX": 1.592735858156421e-40,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 7.383499994424153,
+            "heading": 1.659495816830368e-21,
+            "angularVelocity": 1.658316243120447e-20,
+            "velocityX": 2.780383744489643e-33,
+            "velocityY": -0.8344065481228213,
+            "timestamp": 0.10007112931184184
+        },
+        {
+            "x": 7.68,
+            "y": 7.216499986370153,
+            "heading": 4.9512387638524226e-21,
+            "angularVelocity": 3.289403167370527e-20,
+            "velocityX": 3.618624574631574e-33,
+            "velocityY": -1.668813065290736,
+            "timestamp": 0.20014225862368368
+        },
+        {
+            "x": 7.68,
+            "y": 6.9659999820333836,
+            "heading": 9.801760970151702e-21,
+            "angularVelocity": 4.8470744390330394e-20,
+            "velocityX": 3.439292531132709e-33,
+            "velocityY": -2.503219520548835,
+            "timestamp": 0.30021338793552554
+        },
+        {
+            "x": 7.68,
+            "y": 6.632,
+            "heading": 1.506109654033251e-20,
+            "angularVelocity": 5.255597191401202e-20,
+            "velocityX": 4.639675238422859e-33,
+            "velocityY": -3.3376257900775017,
+            "timestamp": 0.40028451724736736
+        },
+        {
+            "x": 7.68,
+            "y": 6.298000017966616,
+            "heading": 5.3586032763760104e-21,
+            "angularVelocity": -9.69559666912617e-20,
+            "velocityX": 4.935439546802769e-33,
+            "velocityY": -3.3376257900775017,
+            "timestamp": 0.5003556465592092
+        },
+        {
+            "x": 7.68,
+            "y": 6.047500013629846,
+            "heading": 1.8760590350448333e-21,
+            "angularVelocity": -3.480068842301423e-20,
+            "velocityX": 4.701730454770122e-33,
+            "velocityY": -2.5032195205488352,
+            "timestamp": 0.600426775871051
+        },
+        {
+            "x": 7.68,
+            "y": 5.880500005575846,
+            "heading": 3.168722428707802e-22,
+            "angularVelocity": -1.5580785203142817e-20,
+            "velocityX": 3.49539235617723e-33,
+            "velocityY": -1.668813065290736,
+            "timestamp": 0.7004979051828928
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 5.881496870004905e-41,
+            "angularVelocity": -3.1664700918333573e-21,
+            "velocityX": 3.364378386644386e-33,
+            "velocityY": -0.8344065481228213,
+            "timestamp": 0.8005690344947346
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 9.145026397391945e-42,
+            "angularVelocity": -4.963805231001231e-40,
+            "velocityX": -4.870998572760777e-42,
+            "velocityY": 3.489748854003666e-39,
+            "timestamp": 0.9006401638065764
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C2.traj
new file mode 100644
index 00000000..50686485
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C2.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -5.382102664363315e-41,
+            "angularVelocity": -2.8652892976895567e-45,
+            "velocityX": 1.592735858156421e-40,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 7.383499994424153,
+            "heading": 1.659495816830368e-21,
+            "angularVelocity": 1.658316243120447e-20,
+            "velocityX": 2.780383744489643e-33,
+            "velocityY": -0.8344065481228213,
+            "timestamp": 0.10007112931184184
+        },
+        {
+            "x": 7.68,
+            "y": 7.216499986370153,
+            "heading": 4.9512387638524226e-21,
+            "angularVelocity": 3.289403167370527e-20,
+            "velocityX": 3.618624574631574e-33,
+            "velocityY": -1.668813065290736,
+            "timestamp": 0.20014225862368368
+        },
+        {
+            "x": 7.68,
+            "y": 6.9659999820333836,
+            "heading": 9.801760970151702e-21,
+            "angularVelocity": 4.8470744390330394e-20,
+            "velocityX": 3.439292531132709e-33,
+            "velocityY": -2.503219520548835,
+            "timestamp": 0.30021338793552554
+        },
+        {
+            "x": 7.68,
+            "y": 6.632,
+            "heading": 1.506109654033251e-20,
+            "angularVelocity": 5.255597191401202e-20,
+            "velocityX": 4.639675238422859e-33,
+            "velocityY": -3.3376257900775017,
+            "timestamp": 0.40028451724736736
+        },
+        {
+            "x": 7.68,
+            "y": 6.298000017966616,
+            "heading": 5.3586032763760104e-21,
+            "angularVelocity": -9.69559666912617e-20,
+            "velocityX": 4.935439546802769e-33,
+            "velocityY": -3.3376257900775017,
+            "timestamp": 0.5003556465592092
+        },
+        {
+            "x": 7.68,
+            "y": 6.047500013629846,
+            "heading": 1.8760590350448333e-21,
+            "angularVelocity": -3.480068842301423e-20,
+            "velocityX": 4.701730454770122e-33,
+            "velocityY": -2.5032195205488352,
+            "timestamp": 0.600426775871051
+        },
+        {
+            "x": 7.68,
+            "y": 5.880500005575846,
+            "heading": 3.168722428707802e-22,
+            "angularVelocity": -1.5580785203142817e-20,
+            "velocityX": 3.49539235617723e-33,
+            "velocityY": -1.668813065290736,
+            "timestamp": 0.7004979051828928
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 5.881496870004905e-41,
+            "angularVelocity": -3.1664700918333573e-21,
+            "velocityX": 3.364378386644386e-33,
+            "velocityY": -0.8344065481228213,
+            "timestamp": 0.8005690344947346
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 9.145026397391945e-42,
+            "angularVelocity": -4.963805231001231e-40,
+            "velocityX": -4.870998572760777e-42,
+            "velocityY": 3.489748854003666e-39,
+            "timestamp": 0.9006401638065764
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C3.1.traj
new file mode 100644
index 00000000..688fbdaa
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C3.1.traj	
@@ -0,0 +1,139 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 1.0127320738607452e-39,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 7.390554627994461,
+            "heading": -1.4011773737891353e-18,
+            "angularVelocity": -1.4633626501290335e-17,
+            "velocityX": 9.653222160408549e-34,
+            "velocityY": -0.7983807350946673,
+            "timestamp": 0.09575052183150974
+        },
+        {
+            "x": 7.68,
+            "y": 7.237663887017311,
+            "heading": -4.2066729097937526e-18,
+            "angularVelocity": -2.93000547907741e-17,
+            "velocityX": 5.229262005564601e-34,
+            "velocityY": -1.5967614385035824,
+            "timestamp": 0.19150104366301948
+        },
+        {
+            "x": 7.68,
+            "y": 7.0083277831364,
+            "heading": -8.423669065336014e-18,
+            "angularVelocity": -4.4041495279144365e-17,
+            "velocityX": -1.080014219031917e-33,
+            "velocityY": -2.3951420785410225,
+            "timestamp": 0.28725156549452924
+        },
+        {
+            "x": 7.68,
+            "y": 6.702546334555254,
+            "heading": -1.4072329853209785e-17,
+            "angularVelocity": -5.899352482411367e-17,
+            "velocityX": -1.5994734307756055e-33,
+            "velocityY": -3.1935225284643685,
+            "timestamp": 0.38300208732603896
+        },
+        {
+            "x": 7.68,
+            "y": 6.340327800733179,
+            "heading": -3.9014262815434015e-17,
+            "angularVelocity": -2.604887418380216e-16,
+            "velocityX": 4.89416509473128e-33,
+            "velocityY": -3.7829405719528495,
+            "timestamp": 0.4787526091575487
+        },
+        {
+            "x": 7.68,
+            "y": 5.978109266911059,
+            "heading": -3.149786356993182e-17,
+            "angularVelocity": 7.849982539759592e-17,
+            "velocityX": -6.0692119772378415e-33,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.5745031309890585
+        },
+        {
+            "x": 7.68,
+            "y": 5.61589073308894,
+            "heading": -3.435394827038493e-17,
+            "angularVelocity": -2.98283982773363e-17,
+            "velocityX": 7.480170031140856e-33,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.6702536528205683
+        },
+        {
+            "x": 7.68,
+            "y": 5.253672199266821,
+            "heading": -8.183196831726821e-18,
+            "angularVelocity": 2.7332228522691796e-16,
+            "velocityX": -1.1202872015905622e-32,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.766004174652078
+        },
+        {
+            "x": 7.68,
+            "y": 4.891453665444746,
+            "heading": 1.4014689490668664e-17,
+            "angularVelocity": 2.318304474774222e-16,
+            "velocityX": 5.155427490211361e-33,
+            "velocityY": -3.7829405719528495,
+            "timestamp": 0.8617546964835878
+        },
+        {
+            "x": 7.68,
+            "y": 4.5856722168636,
+            "heading": 8.395813027039913e-18,
+            "angularVelocity": -5.868246309956073e-17,
+            "velocityX": -4.168254505442575e-33,
+            "velocityY": -3.1935225284643685,
+            "timestamp": 0.9575052183150976
+        },
+        {
+            "x": 7.68,
+            "y": 4.356336112982689,
+            "heading": 4.1945848336017725e-18,
+            "angularVelocity": -4.387681773072387e-17,
+            "velocityX": 2.1988846402360483e-33,
+            "velocityY": -2.3951420785410225,
+            "timestamp": 1.0532557401466074
+        },
+        {
+            "x": 7.68,
+            "y": 4.203445372005539,
+            "heading": 1.3975677775871127e-18,
+            "angularVelocity": -2.9211507184809557e-17,
+            "velocityX": 3.5559383909284404e-33,
+            "velocityY": -1.5967614385035824,
+            "timestamp": 1.1490062619781172
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": -1.4595928576161535e-17,
+            "velocityX": -6.471812078129625e-34,
+            "velocityY": -0.7983807350946673,
+            "timestamp": 1.244756783809627
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 8.546727720642512e-43,
+            "velocityY": 6.233759417908357e-39,
+            "timestamp": 1.3405073056411367
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C3.traj
new file mode 100644
index 00000000..688fbdaa
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C3.traj	
@@ -0,0 +1,139 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 1.0127320738607452e-39,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 7.390554627994461,
+            "heading": -1.4011773737891353e-18,
+            "angularVelocity": -1.4633626501290335e-17,
+            "velocityX": 9.653222160408549e-34,
+            "velocityY": -0.7983807350946673,
+            "timestamp": 0.09575052183150974
+        },
+        {
+            "x": 7.68,
+            "y": 7.237663887017311,
+            "heading": -4.2066729097937526e-18,
+            "angularVelocity": -2.93000547907741e-17,
+            "velocityX": 5.229262005564601e-34,
+            "velocityY": -1.5967614385035824,
+            "timestamp": 0.19150104366301948
+        },
+        {
+            "x": 7.68,
+            "y": 7.0083277831364,
+            "heading": -8.423669065336014e-18,
+            "angularVelocity": -4.4041495279144365e-17,
+            "velocityX": -1.080014219031917e-33,
+            "velocityY": -2.3951420785410225,
+            "timestamp": 0.28725156549452924
+        },
+        {
+            "x": 7.68,
+            "y": 6.702546334555254,
+            "heading": -1.4072329853209785e-17,
+            "angularVelocity": -5.899352482411367e-17,
+            "velocityX": -1.5994734307756055e-33,
+            "velocityY": -3.1935225284643685,
+            "timestamp": 0.38300208732603896
+        },
+        {
+            "x": 7.68,
+            "y": 6.340327800733179,
+            "heading": -3.9014262815434015e-17,
+            "angularVelocity": -2.604887418380216e-16,
+            "velocityX": 4.89416509473128e-33,
+            "velocityY": -3.7829405719528495,
+            "timestamp": 0.4787526091575487
+        },
+        {
+            "x": 7.68,
+            "y": 5.978109266911059,
+            "heading": -3.149786356993182e-17,
+            "angularVelocity": 7.849982539759592e-17,
+            "velocityX": -6.0692119772378415e-33,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.5745031309890585
+        },
+        {
+            "x": 7.68,
+            "y": 5.61589073308894,
+            "heading": -3.435394827038493e-17,
+            "angularVelocity": -2.98283982773363e-17,
+            "velocityX": 7.480170031140856e-33,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.6702536528205683
+        },
+        {
+            "x": 7.68,
+            "y": 5.253672199266821,
+            "heading": -8.183196831726821e-18,
+            "angularVelocity": 2.7332228522691796e-16,
+            "velocityX": -1.1202872015905622e-32,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.766004174652078
+        },
+        {
+            "x": 7.68,
+            "y": 4.891453665444746,
+            "heading": 1.4014689490668664e-17,
+            "angularVelocity": 2.318304474774222e-16,
+            "velocityX": 5.155427490211361e-33,
+            "velocityY": -3.7829405719528495,
+            "timestamp": 0.8617546964835878
+        },
+        {
+            "x": 7.68,
+            "y": 4.5856722168636,
+            "heading": 8.395813027039913e-18,
+            "angularVelocity": -5.868246309956073e-17,
+            "velocityX": -4.168254505442575e-33,
+            "velocityY": -3.1935225284643685,
+            "timestamp": 0.9575052183150976
+        },
+        {
+            "x": 7.68,
+            "y": 4.356336112982689,
+            "heading": 4.1945848336017725e-18,
+            "angularVelocity": -4.387681773072387e-17,
+            "velocityX": 2.1988846402360483e-33,
+            "velocityY": -2.3951420785410225,
+            "timestamp": 1.0532557401466074
+        },
+        {
+            "x": 7.68,
+            "y": 4.203445372005539,
+            "heading": 1.3975677775871127e-18,
+            "angularVelocity": -2.9211507184809557e-17,
+            "velocityX": 3.5559383909284404e-33,
+            "velocityY": -1.5967614385035824,
+            "timestamp": 1.1490062619781172
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": -1.4595928576161535e-17,
+            "velocityX": -6.471812078129625e-34,
+            "velocityY": -0.7983807350946673,
+            "timestamp": 1.244756783809627
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 8.546727720642512e-43,
+            "velocityY": 6.233759417908357e-39,
+            "timestamp": 1.3405073056411367
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.1.traj
new file mode 100644
index 00000000..d59fe62f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.1.traj	
@@ -0,0 +1,175 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 3.5626996813915863e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 7.385155846048237,
+            "heading": 1.781123128781041e-18,
+            "angularVelocity": 1.7977717932873702e-17,
+            "velocityX": 2.837123063321612e-19,
+            "velocityY": -0.8260917454276266,
+            "timestamp": 0.099073927811004
+        },
+        {
+            "x": 7.68,
+            "y": 7.22146754125807,
+            "heading": 5.344190150368438e-18,
+            "angularVelocity": 3.5963720225105045e-17,
+            "velocityX": 5.673750850451309e-19,
+            "velocityY": -1.65218345943065,
+            "timestamp": 0.198147855622008
+        },
+        {
+            "x": 7.68,
+            "y": 6.975935091856216,
+            "heading": 1.0690981078424305e-17,
+            "angularVelocity": 5.396768903659916e-17,
+            "velocityX": 8.509388117154945e-19,
+            "velocityY": -2.47827511058448,
+            "timestamp": 0.29722178343301203
+        },
+        {
+            "x": 7.68,
+            "y": 6.648558516522808,
+            "heading": 1.7824905328944302e-17,
+            "angularVelocity": 7.200607069613354e-17,
+            "velocityX": 1.134205375948437e-18,
+            "velocityY": -3.304366573190878,
+            "timestamp": 0.396295711244016
+        },
+        {
+            "x": 7.68,
+            "y": 6.273767735073314,
+            "heading": 3.2118763957552995e-17,
+            "angularVelocity": 1.4427467391699638e-16,
+            "velocityX": 1.2981920193437937e-18,
+            "velocityY": -3.782940575087071,
+            "timestamp": 0.49536963905502
+        },
+        {
+            "x": 7.68,
+            "y": 5.898976953623795,
+            "heading": 3.194072142377472e-17,
+            "angularVelocity": -1.7970674806511035e-18,
+            "velocityX": 1.2970624694923187e-18,
+            "velocityY": -3.7829405750873115,
+            "timestamp": 0.5944435668660241
+        },
+        {
+            "x": 7.68,
+            "y": 5.524186172174278,
+            "heading": 3.523836351949418e-17,
+            "angularVelocity": 3.3284660945411024e-17,
+            "velocityX": 1.2988038949897902e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.6935174946770281
+        },
+        {
+            "x": 7.68,
+            "y": 5.149395390724759,
+            "heading": 4.1591047407718544e-17,
+            "angularVelocity": 6.412064231794501e-17,
+            "velocityX": 1.299093645390414e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.792591422488032
+        },
+        {
+            "x": 7.68,
+            "y": 4.774604609275242,
+            "heading": 3.0405238206332753e-17,
+            "angularVelocity": -1.1290366142265185e-16,
+            "velocityX": 1.2929821768433727e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.891665350299036
+        },
+        {
+            "x": 7.68,
+            "y": 4.3998138278257235,
+            "heading": 3.8390344063969795e-17,
+            "angularVelocity": 8.059744913789504e-17,
+            "velocityX": 1.304793755296235e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.99073927811004
+        },
+        {
+            "x": 7.68,
+            "y": 4.025023046376205,
+            "heading": 1.0979594277826173e-17,
+            "angularVelocity": -2.766696586253564e-16,
+            "velocityX": 1.307186742413441e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 1.0898132059210441
+        },
+        {
+            "x": 7.68,
+            "y": 3.650232264926687,
+            "heading": -2.14668342522941e-17,
+            "angularVelocity": -3.2749714528425564e-16,
+            "velocityX": 1.3056923664932481e-18,
+            "velocityY": -3.7829405750873115,
+            "timestamp": 1.1888871337320481
+        },
+        {
+            "x": 7.68,
+            "y": 3.2754414834771928,
+            "heading": -1.7689075007663427e-17,
+            "angularVelocity": 3.813071035032691e-17,
+            "velocityX": 1.3012288375156972e-18,
+            "velocityY": -3.782940575087071,
+            "timestamp": 1.2879610615430521
+        },
+        {
+            "x": 7.68,
+            "y": 2.948064908143785,
+            "heading": -1.0623620375651477e-17,
+            "angularVelocity": 7.131497447110055e-17,
+            "velocityX": 1.136256754825581e-18,
+            "velocityY": -3.304366573190878,
+            "timestamp": 1.3870349893540561
+        },
+        {
+            "x": 7.68,
+            "y": 2.70253245874193,
+            "heading": -5.31498515459649e-18,
+            "angularVelocity": 5.35825654501949e-17,
+            "velocityX": 8.521926280380639e-19,
+            "velocityY": -2.4782751105844802,
+            "timestamp": 1.48610891716506
+        },
+        {
+            "x": 7.68,
+            "y": 2.538844153951763,
+            "heading": -1.7723662552803497e-18,
+            "angularVelocity": 3.575732766130284e-17,
+            "velocityX": 5.681284366395801e-19,
+            "velocityY": -1.6521834594306501,
+            "timestamp": 1.585182844976064
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": 1.7889330669293064e-17,
+            "velocityX": 2.8406422370407584e-19,
+            "velocityY": -0.8260917454276266,
+            "timestamp": 1.684256772787068
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 1.0461754707969188e-34,
+            "velocityY": 0,
+            "timestamp": 1.783330700598072
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.traj
new file mode 100644
index 00000000..d59fe62f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.traj	
@@ -0,0 +1,175 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 3.5626996813915863e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 7.385155846048237,
+            "heading": 1.781123128781041e-18,
+            "angularVelocity": 1.7977717932873702e-17,
+            "velocityX": 2.837123063321612e-19,
+            "velocityY": -0.8260917454276266,
+            "timestamp": 0.099073927811004
+        },
+        {
+            "x": 7.68,
+            "y": 7.22146754125807,
+            "heading": 5.344190150368438e-18,
+            "angularVelocity": 3.5963720225105045e-17,
+            "velocityX": 5.673750850451309e-19,
+            "velocityY": -1.65218345943065,
+            "timestamp": 0.198147855622008
+        },
+        {
+            "x": 7.68,
+            "y": 6.975935091856216,
+            "heading": 1.0690981078424305e-17,
+            "angularVelocity": 5.396768903659916e-17,
+            "velocityX": 8.509388117154945e-19,
+            "velocityY": -2.47827511058448,
+            "timestamp": 0.29722178343301203
+        },
+        {
+            "x": 7.68,
+            "y": 6.648558516522808,
+            "heading": 1.7824905328944302e-17,
+            "angularVelocity": 7.200607069613354e-17,
+            "velocityX": 1.134205375948437e-18,
+            "velocityY": -3.304366573190878,
+            "timestamp": 0.396295711244016
+        },
+        {
+            "x": 7.68,
+            "y": 6.273767735073314,
+            "heading": 3.2118763957552995e-17,
+            "angularVelocity": 1.4427467391699638e-16,
+            "velocityX": 1.2981920193437937e-18,
+            "velocityY": -3.782940575087071,
+            "timestamp": 0.49536963905502
+        },
+        {
+            "x": 7.68,
+            "y": 5.898976953623795,
+            "heading": 3.194072142377472e-17,
+            "angularVelocity": -1.7970674806511035e-18,
+            "velocityX": 1.2970624694923187e-18,
+            "velocityY": -3.7829405750873115,
+            "timestamp": 0.5944435668660241
+        },
+        {
+            "x": 7.68,
+            "y": 5.524186172174278,
+            "heading": 3.523836351949418e-17,
+            "angularVelocity": 3.3284660945411024e-17,
+            "velocityX": 1.2988038949897902e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.6935174946770281
+        },
+        {
+            "x": 7.68,
+            "y": 5.149395390724759,
+            "heading": 4.1591047407718544e-17,
+            "angularVelocity": 6.412064231794501e-17,
+            "velocityX": 1.299093645390414e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.792591422488032
+        },
+        {
+            "x": 7.68,
+            "y": 4.774604609275242,
+            "heading": 3.0405238206332753e-17,
+            "angularVelocity": -1.1290366142265185e-16,
+            "velocityX": 1.2929821768433727e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.891665350299036
+        },
+        {
+            "x": 7.68,
+            "y": 4.3998138278257235,
+            "heading": 3.8390344063969795e-17,
+            "angularVelocity": 8.059744913789504e-17,
+            "velocityX": 1.304793755296235e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.99073927811004
+        },
+        {
+            "x": 7.68,
+            "y": 4.025023046376205,
+            "heading": 1.0979594277826173e-17,
+            "angularVelocity": -2.766696586253564e-16,
+            "velocityX": 1.307186742413441e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 1.0898132059210441
+        },
+        {
+            "x": 7.68,
+            "y": 3.650232264926687,
+            "heading": -2.14668342522941e-17,
+            "angularVelocity": -3.2749714528425564e-16,
+            "velocityX": 1.3056923664932481e-18,
+            "velocityY": -3.7829405750873115,
+            "timestamp": 1.1888871337320481
+        },
+        {
+            "x": 7.68,
+            "y": 3.2754414834771928,
+            "heading": -1.7689075007663427e-17,
+            "angularVelocity": 3.813071035032691e-17,
+            "velocityX": 1.3012288375156972e-18,
+            "velocityY": -3.782940575087071,
+            "timestamp": 1.2879610615430521
+        },
+        {
+            "x": 7.68,
+            "y": 2.948064908143785,
+            "heading": -1.0623620375651477e-17,
+            "angularVelocity": 7.131497447110055e-17,
+            "velocityX": 1.136256754825581e-18,
+            "velocityY": -3.304366573190878,
+            "timestamp": 1.3870349893540561
+        },
+        {
+            "x": 7.68,
+            "y": 2.70253245874193,
+            "heading": -5.31498515459649e-18,
+            "angularVelocity": 5.35825654501949e-17,
+            "velocityX": 8.521926280380639e-19,
+            "velocityY": -2.4782751105844802,
+            "timestamp": 1.48610891716506
+        },
+        {
+            "x": 7.68,
+            "y": 2.538844153951763,
+            "heading": -1.7723662552803497e-18,
+            "angularVelocity": 3.575732766130284e-17,
+            "velocityX": 5.681284366395801e-19,
+            "velocityY": -1.6521834594306501,
+            "timestamp": 1.585182844976064
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": 1.7889330669293064e-17,
+            "velocityX": 2.8406422370407584e-19,
+            "velocityY": -0.8260917454276266,
+            "timestamp": 1.684256772787068
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 1.0461754707969188e-34,
+            "velocityY": 0,
+            "timestamp": 1.783330700598072
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.1.traj
new file mode 100644
index 00000000..6f64999b
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.1.traj	
@@ -0,0 +1,220 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -8.452820466081124e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 7.3890447295128565,
+            "heading": -6.111572142469688e-20,
+            "angularVelocity": -6.320692328199756e-19,
+            "velocityX": -1.9797111149811777e-21,
+            "velocityY": -0.8062267267095136,
+            "timestamp": 0.09669149868710691
+        },
+        {
+            "x": 7.68,
+            "y": 7.2331341916331855,
+            "heading": -1.8226742072400974e-19,
+            "angularVelocity": -1.2529715733337834e-18,
+            "velocityX": -3.753564400558873e-21,
+            "velocityY": -1.6124534214139752,
+            "timestamp": 0.19338299737421383
+        },
+        {
+            "x": 7.68,
+            "y": 6.999268392550218,
+            "heading": -3.611632829024713e-19,
+            "angularVelocity": -1.850171571559818e-18,
+            "velocityX": -5.1157697830729356e-21,
+            "velocityY": -2.418680052108357,
+            "timestamp": 0.29007449606132074
+        },
+        {
+            "x": 7.68,
+            "y": 6.687447350831621,
+            "heading": -5.912144198792796e-19,
+            "angularVelocity": -2.3792281637439373e-18,
+            "velocityX": -5.242727749103024e-21,
+            "velocityY": -3.224906490772764,
+            "timestamp": 0.38676599474842766
+        },
+        {
+            "x": 7.68,
+            "y": 6.321669157855709,
+            "heading": -4.100561450064452e-17,
+            "angularVelocity": -4.179726307846966e-16,
+            "velocityX": -1.166132844762965e-20,
+            "velocityY": -3.782940568121389,
+            "timestamp": 0.48345749343553457
+        },
+        {
+            "x": 7.68,
+            "y": 5.955890964879757,
+            "heading": -6.040572418063157e-17,
+            "angularVelocity": -2.0063924898701218e-16,
+            "velocityX": -1.9392305823460385e-22,
+            "velocityY": -3.782940568121787,
+            "timestamp": 0.5801489921226415
+        },
+        {
+            "x": 7.68,
+            "y": 5.590112771903805,
+            "heading": -8.327536807485113e-17,
+            "angularVelocity": -2.365217646302656e-16,
+            "velocityX": 3.675624932403311e-22,
+            "velocityY": -3.782940568121787,
+            "timestamp": 0.6768404908097484
+        },
+        {
+            "x": 7.68,
+            "y": 5.224334578927854,
+            "heading": -7.575439540555723e-17,
+            "angularVelocity": 7.778318436801094e-17,
+            "velocityX": -2.1057162419585226e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 0.7735319894968553
+        },
+        {
+            "x": 7.68,
+            "y": 4.858556385951902,
+            "heading": -4.8431591576069396e-17,
+            "angularVelocity": 2.82577105541659e-16,
+            "velocityX": 1.016892835365518e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 0.8702234881839622
+        },
+        {
+            "x": 7.68,
+            "y": 4.49277819297595,
+            "heading": -7.892411277004063e-17,
+            "angularVelocity": -3.1535886409915893e-16,
+            "velocityX": 1.448731313682566e-20,
+            "velocityY": -3.782940568121787,
+            "timestamp": 0.9669149868710691
+        },
+        {
+            "x": 7.68,
+            "y": 4.126999999999999,
+            "heading": -8.668140475632877e-17,
+            "angularVelocity": -8.022723912254832e-17,
+            "velocityX": 2.655349226445793e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 1.063606485558176
+        },
+        {
+            "x": 7.68,
+            "y": 3.7612218070240475,
+            "heading": -4.236036305527668e-17,
+            "angularVelocity": 4.583757859030679e-16,
+            "velocityX": 7.500561292003624e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 1.160297984245283
+        },
+        {
+            "x": 7.68,
+            "y": 3.395443614048096,
+            "heading": -2.060715967072721e-17,
+            "angularVelocity": 2.249753461257506e-16,
+            "velocityX": 1.3311646441840309e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 1.25698948293239
+        },
+        {
+            "x": 7.68,
+            "y": 3.0296654210721448,
+            "heading": -1.7378554757194933e-17,
+            "angularVelocity": 3.3390783650741015e-17,
+            "velocityX": 1.5677048108756238e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 1.353680981619497
+        },
+        {
+            "x": 7.68,
+            "y": 2.663887228096194,
+            "heading": -2.7058170522872628e-18,
+            "angularVelocity": 1.5174796030816139e-16,
+            "velocityX": 2.708497475030379e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 1.450372480306604
+        },
+        {
+            "x": 7.68,
+            "y": 2.2981090351202424,
+            "heading": -9.881529062229235e-18,
+            "angularVelocity": -7.421243963841227e-17,
+            "velocityX": 2.3345387345088647e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 1.547063978993711
+        },
+        {
+            "x": 7.68,
+            "y": 1.9323308421442913,
+            "heading": -6.5997498705085955e-18,
+            "angularVelocity": 3.39407211211041e-17,
+            "velocityX": -6.072343928076645e-20,
+            "velocityY": -3.782940568121787,
+            "timestamp": 1.643755477680818
+        },
+        {
+            "x": 7.68,
+            "y": 1.5665526491683786,
+            "heading": 6.487893960505722e-19,
+            "angularVelocity": 7.496563157826373e-17,
+            "velocityX": 5.228734565126193e-20,
+            "velocityY": -3.782940568121389,
+            "timestamp": 1.7404469763679251
+        },
+        {
+            "x": 7.68,
+            "y": 1.254731607449781,
+            "heading": 3.9009642634108723e-19,
+            "angularVelocity": -2.6754468948553036e-18,
+            "velocityX": 1.8077268140623735e-21,
+            "velocityY": -3.224906490772764,
+            "timestamp": 1.8371384750550321
+        },
+        {
+            "x": 7.68,
+            "y": 1.020865808366814,
+            "heading": 1.9475770164363563e-19,
+            "angularVelocity": -2.020226466917104e-18,
+            "velocityX": 1.3557598034014801e-21,
+            "velocityY": -2.418680052108357,
+            "timestamp": 1.9338299737421392
+        },
+        {
+            "x": 7.68,
+            "y": 0.8649552704871435,
+            "heading": 6.488190339479676e-20,
+            "angularVelocity": -1.3431976959152128e-18,
+            "velocityX": 9.038296351119003e-22,
+            "velocityY": -1.6124534214139752,
+            "timestamp": 2.030521472429246
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": -6.710197305720325e-19,
+            "velocityX": 4.519117474728098e-22,
+            "velocityY": -0.8062267267095136,
+            "timestamp": 2.127212971116353
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 1.0656726462878492e-32,
+            "velocityY": 0,
+            "timestamp": 2.2239044698034602
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.traj
new file mode 100644
index 00000000..6f64999b
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.traj	
@@ -0,0 +1,220 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -8.452820466081124e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 7.3890447295128565,
+            "heading": -6.111572142469688e-20,
+            "angularVelocity": -6.320692328199756e-19,
+            "velocityX": -1.9797111149811777e-21,
+            "velocityY": -0.8062267267095136,
+            "timestamp": 0.09669149868710691
+        },
+        {
+            "x": 7.68,
+            "y": 7.2331341916331855,
+            "heading": -1.8226742072400974e-19,
+            "angularVelocity": -1.2529715733337834e-18,
+            "velocityX": -3.753564400558873e-21,
+            "velocityY": -1.6124534214139752,
+            "timestamp": 0.19338299737421383
+        },
+        {
+            "x": 7.68,
+            "y": 6.999268392550218,
+            "heading": -3.611632829024713e-19,
+            "angularVelocity": -1.850171571559818e-18,
+            "velocityX": -5.1157697830729356e-21,
+            "velocityY": -2.418680052108357,
+            "timestamp": 0.29007449606132074
+        },
+        {
+            "x": 7.68,
+            "y": 6.687447350831621,
+            "heading": -5.912144198792796e-19,
+            "angularVelocity": -2.3792281637439373e-18,
+            "velocityX": -5.242727749103024e-21,
+            "velocityY": -3.224906490772764,
+            "timestamp": 0.38676599474842766
+        },
+        {
+            "x": 7.68,
+            "y": 6.321669157855709,
+            "heading": -4.100561450064452e-17,
+            "angularVelocity": -4.179726307846966e-16,
+            "velocityX": -1.166132844762965e-20,
+            "velocityY": -3.782940568121389,
+            "timestamp": 0.48345749343553457
+        },
+        {
+            "x": 7.68,
+            "y": 5.955890964879757,
+            "heading": -6.040572418063157e-17,
+            "angularVelocity": -2.0063924898701218e-16,
+            "velocityX": -1.9392305823460385e-22,
+            "velocityY": -3.782940568121787,
+            "timestamp": 0.5801489921226415
+        },
+        {
+            "x": 7.68,
+            "y": 5.590112771903805,
+            "heading": -8.327536807485113e-17,
+            "angularVelocity": -2.365217646302656e-16,
+            "velocityX": 3.675624932403311e-22,
+            "velocityY": -3.782940568121787,
+            "timestamp": 0.6768404908097484
+        },
+        {
+            "x": 7.68,
+            "y": 5.224334578927854,
+            "heading": -7.575439540555723e-17,
+            "angularVelocity": 7.778318436801094e-17,
+            "velocityX": -2.1057162419585226e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 0.7735319894968553
+        },
+        {
+            "x": 7.68,
+            "y": 4.858556385951902,
+            "heading": -4.8431591576069396e-17,
+            "angularVelocity": 2.82577105541659e-16,
+            "velocityX": 1.016892835365518e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 0.8702234881839622
+        },
+        {
+            "x": 7.68,
+            "y": 4.49277819297595,
+            "heading": -7.892411277004063e-17,
+            "angularVelocity": -3.1535886409915893e-16,
+            "velocityX": 1.448731313682566e-20,
+            "velocityY": -3.782940568121787,
+            "timestamp": 0.9669149868710691
+        },
+        {
+            "x": 7.68,
+            "y": 4.126999999999999,
+            "heading": -8.668140475632877e-17,
+            "angularVelocity": -8.022723912254832e-17,
+            "velocityX": 2.655349226445793e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 1.063606485558176
+        },
+        {
+            "x": 7.68,
+            "y": 3.7612218070240475,
+            "heading": -4.236036305527668e-17,
+            "angularVelocity": 4.583757859030679e-16,
+            "velocityX": 7.500561292003624e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 1.160297984245283
+        },
+        {
+            "x": 7.68,
+            "y": 3.395443614048096,
+            "heading": -2.060715967072721e-17,
+            "angularVelocity": 2.249753461257506e-16,
+            "velocityX": 1.3311646441840309e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 1.25698948293239
+        },
+        {
+            "x": 7.68,
+            "y": 3.0296654210721448,
+            "heading": -1.7378554757194933e-17,
+            "angularVelocity": 3.3390783650741015e-17,
+            "velocityX": 1.5677048108756238e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 1.353680981619497
+        },
+        {
+            "x": 7.68,
+            "y": 2.663887228096194,
+            "heading": -2.7058170522872628e-18,
+            "angularVelocity": 1.5174796030816139e-16,
+            "velocityX": 2.708497475030379e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 1.450372480306604
+        },
+        {
+            "x": 7.68,
+            "y": 2.2981090351202424,
+            "heading": -9.881529062229235e-18,
+            "angularVelocity": -7.421243963841227e-17,
+            "velocityX": 2.3345387345088647e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 1.547063978993711
+        },
+        {
+            "x": 7.68,
+            "y": 1.9323308421442913,
+            "heading": -6.5997498705085955e-18,
+            "angularVelocity": 3.39407211211041e-17,
+            "velocityX": -6.072343928076645e-20,
+            "velocityY": -3.782940568121787,
+            "timestamp": 1.643755477680818
+        },
+        {
+            "x": 7.68,
+            "y": 1.5665526491683786,
+            "heading": 6.487893960505722e-19,
+            "angularVelocity": 7.496563157826373e-17,
+            "velocityX": 5.228734565126193e-20,
+            "velocityY": -3.782940568121389,
+            "timestamp": 1.7404469763679251
+        },
+        {
+            "x": 7.68,
+            "y": 1.254731607449781,
+            "heading": 3.9009642634108723e-19,
+            "angularVelocity": -2.6754468948553036e-18,
+            "velocityX": 1.8077268140623735e-21,
+            "velocityY": -3.224906490772764,
+            "timestamp": 1.8371384750550321
+        },
+        {
+            "x": 7.68,
+            "y": 1.020865808366814,
+            "heading": 1.9475770164363563e-19,
+            "angularVelocity": -2.020226466917104e-18,
+            "velocityX": 1.3557598034014801e-21,
+            "velocityY": -2.418680052108357,
+            "timestamp": 1.9338299737421392
+        },
+        {
+            "x": 7.68,
+            "y": 0.8649552704871435,
+            "heading": 6.488190339479676e-20,
+            "angularVelocity": -1.3431976959152128e-18,
+            "velocityX": 9.038296351119003e-22,
+            "velocityY": -1.6124534214139752,
+            "timestamp": 2.030521472429246
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": -6.710197305720325e-19,
+            "velocityX": 4.519117474728098e-22,
+            "velocityY": -0.8062267267095136,
+            "timestamp": 2.127212971116353
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 1.0656726462878492e-32,
+            "velocityY": 0,
+            "timestamp": 2.2239044698034602
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.1.traj
index 9c633634..ce3947a6 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.1.traj	
@@ -3,128 +3,128 @@
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 1.9214160460800783e-32,
-            "angularVelocity": -7.67482132813978e-30,
-            "velocityX": -1.6843046577095525e-31,
-            "velocityY": 2.6209535651210065e-31,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -7.649625564275916e-33,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.6177977502927385,
-            "y": 7.446201757051976,
-            "heading": 0.01344244845353824,
-            "angularVelocity": 0.15144190849275008,
-            "velocityX": -0.7007672331072132,
-            "velocityY": -0.23431189760056984,
-            "timestamp": 0.08876306820393338
+            "x": 7.617797764168564,
+            "y": 7.446201717117849,
+            "heading": 0.013442479376395878,
+            "angularVelocity": 0.15144225686963014,
+            "velocityX": -0.7007670767707749,
+            "velocityY": -0.23431234749607086,
+            "timestamp": 0.08876306820531243
         },
         {
-            "x": 7.493368265532485,
-            "y": 7.404602703026682,
-            "heading": 0.038702181990287625,
-            "angularVelocity": 0.2845748129935982,
-            "velocityX": -1.4018159497887253,
-            "velocityY": -0.4686527276087507,
-            "timestamp": 0.17752613640786677
+            "x": 7.493368305965553,
+            "y": 7.404602586388856,
+            "heading": 0.03870227011836747,
+            "angularVelocity": 0.28457545746724416,
+            "velocityX": -1.401815650572125,
+            "velocityY": -0.4686535917478179,
+            "timestamp": 0.17752613641062487
         },
         {
-            "x": 7.306675971092295,
-            "y": 7.342201042854806,
-            "heading": 0.0730274998055895,
-            "angularVelocity": 0.38670720275422266,
-            "velocityX": -2.1032654483226527,
-            "velocityY": -0.7030137807724448,
-            "timestamp": 0.26628920461180017
+            "x": 7.30667604944407,
+            "y": 7.3422008161122045,
+            "heading": 0.0730276629215222,
+            "angularVelocity": 0.38670804756608035,
+            "velocityX": -2.1032650210965835,
+            "velocityY": -0.7030150212056434,
+            "timestamp": 0.2662892046159373
         },
         {
-            "x": 7.057674447994484,
-            "y": 7.2590026396848994,
-            "heading": 0.11075741429874605,
-            "angularVelocity": 0.42506320764649574,
-            "velocityX": -2.8052379005867665,
-            "velocityY": -0.9373087800171416,
-            "timestamp": 0.35505227281573354
+            "x": 7.057674574154849,
+            "y": 7.259002273186937,
+            "heading": 0.11075765125141449,
+            "angularVelocity": 0.42506403948785515,
+            "velocityX": -2.8052373619302045,
+            "velocityY": -0.9373103544914424,
+            "timestamp": 0.35505227282124974
         },
         {
-            "x": 6.746464686770232,
-            "y": 7.155112270199717,
-            "heading": 0.1333971305446714,
-            "angularVelocity": 0.25505783773756635,
-            "velocityX": -3.506072598902806,
-            "velocityY": -1.1704233707338827,
-            "timestamp": 0.4438153410196669
+            "x": 6.746464870184761,
+            "y": 7.155111739606062,
+            "heading": 0.13339737013954753,
+            "angularVelocity": 0.25505786749611997,
+            "velocityX": -3.5060719538216505,
+            "velocityY": -1.170425219424191,
+            "timestamp": 0.44381534102656217
         },
         {
-            "x": 6.428047616602866,
-            "y": 7.048517672195703,
-            "heading": 0.13339714147429826,
-            "angularVelocity": 1.23133576958653e-7,
-            "velocityX": -3.5872697576863386,
-            "velocityY": -1.200889065236652,
-            "timestamp": 0.5325784092236003
+            "x": 6.428047828205506,
+            "y": 7.048517057389171,
+            "heading": 0.13339738106725169,
+            "angularVelocity": 1.2311157711164836e-7,
+            "velocityX": -3.5872694400628884,
+            "velocityY": -1.2008900139638257,
+            "timestamp": 0.5325784092318746
         },
         {
-            "x": 6.109630567326096,
-            "y": 6.9419230117899575,
-            "heading": 0.13339715240409608,
-            "angularVelocity": 1.231354889650669e-7,
-            "velocityX": -3.5872695223333455,
-            "velocityY": -1.200889768252866,
-            "timestamp": 0.6213414774275338
+            "x": 6.109630786224927,
+            "y": 6.941922375176235,
+            "heading": 0.1333973919949535,
+            "angularVelocity": 1.23111550991103e-7,
+            "velocityX": -3.587269440077816,
+            "velocityY": -1.200890013919235,
+            "timestamp": 0.621341477437187
         },
         {
-            "x": 5.798381237872348,
-            "y": 6.8380188367314085,
-            "heading": 0.15626327537058046,
-            "angularVelocity": 0.25760852379277716,
-            "velocityX": -3.506518372457005,
-            "velocityY": -1.170578903621919,
-            "timestamp": 0.7101045456314672
+            "x": 5.798381397539494,
+            "y": 6.838018370585249,
+            "heading": 0.15626352052401435,
+            "angularVelocity": 0.2576085864583032,
+            "velocityX": -3.5065190397107555,
+            "velocityY": -1.1705769831075976,
+            "timestamp": 0.7101045456424995
         },
         {
-            "x": 5.549363092262374,
-            "y": 6.7548143649024714,
-            "heading": 0.19280777245315245,
-            "angularVelocity": 0.41170835825755314,
-            "velocityX": -2.8054251689136636,
-            "velocityY": -0.9373771492347068,
-            "timestamp": 0.7988676138354006
+            "x": 5.5493631968567065,
+            "y": 6.754814060290535,
+            "heading": 0.1928079538664876,
+            "angularVelocity": 0.4117076401528937,
+            "velocityX": -2.8054257893230465,
+            "velocityY": -0.9373753293689796,
+            "timestamp": 0.7988676138478119
         },
         {
-            "x": 5.36265388534366,
-            "y": 6.692406232212667,
-            "heading": 0.22585001680889633,
-            "angularVelocity": 0.3722521655212111,
-            "velocityX": -2.1034559833970325,
-            "velocityY": -0.7030866998372853,
-            "timestamp": 0.887630682039334
+            "x": 5.362653942099421,
+            "y": 6.692406067264648,
+            "heading": 0.22585011955614995,
+            "angularVelocity": 0.37225127926004886,
+            "velocityX": -2.1034565223155943,
+            "velocityY": -0.703085126367409,
+            "timestamp": 0.8876306820531243
         },
         {
-            "x": 5.238210362873152,
-            "y": 6.650801584329758,
-            "heading": 0.25010700866879837,
-            "angularVelocity": 0.27327797868904535,
-            "velocityX": -1.4019740978791027,
-            "velocityY": -0.46871574772688473,
-            "timestamp": 0.9763937502432675
+            "x": 5.238210383284219,
+            "y": 6.650801525067523,
+            "heading": 0.25010704549903756,
+            "angularVelocity": 0.2732772360607878,
+            "velocityX": -1.4019745073178984,
+            "velocityY": -0.4687145570597535,
+            "timestamp": 0.9763937502584368
         },
         {
             "x": 5.176,
             "y": 6.63,
             "heading": 0.263,
-            "angularVelocity": 0.1452517538281686,
-            "velocityX": -0.7008586355989793,
-            "velocityY": -0.23434954143793293,
-            "timestamp": 1.065156818447201
+            "angularVelocity": 0.1452513388947291,
+            "velocityX": -0.7008588655398115,
+            "velocityY": -0.2343488737838604,
+            "timestamp": 1.0651568184637492
         },
         {
             "x": 5.176,
             "y": 6.63,
             "heading": 0.263,
-            "angularVelocity": 0,
-            "velocityX": -1.1321901910980418e-33,
+            "angularVelocity": -2.2743517080702824e-31,
+            "velocityX": 0,
             "velocityY": 0,
-            "timestamp": 1.1539198866511342
+            "timestamp": 1.1539198866690616
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.traj
index 9c633634..ce3947a6 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.traj	
@@ -3,128 +3,128 @@
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 1.9214160460800783e-32,
-            "angularVelocity": -7.67482132813978e-30,
-            "velocityX": -1.6843046577095525e-31,
-            "velocityY": 2.6209535651210065e-31,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -7.649625564275916e-33,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.6177977502927385,
-            "y": 7.446201757051976,
-            "heading": 0.01344244845353824,
-            "angularVelocity": 0.15144190849275008,
-            "velocityX": -0.7007672331072132,
-            "velocityY": -0.23431189760056984,
-            "timestamp": 0.08876306820393338
+            "x": 7.617797764168564,
+            "y": 7.446201717117849,
+            "heading": 0.013442479376395878,
+            "angularVelocity": 0.15144225686963014,
+            "velocityX": -0.7007670767707749,
+            "velocityY": -0.23431234749607086,
+            "timestamp": 0.08876306820531243
         },
         {
-            "x": 7.493368265532485,
-            "y": 7.404602703026682,
-            "heading": 0.038702181990287625,
-            "angularVelocity": 0.2845748129935982,
-            "velocityX": -1.4018159497887253,
-            "velocityY": -0.4686527276087507,
-            "timestamp": 0.17752613640786677
+            "x": 7.493368305965553,
+            "y": 7.404602586388856,
+            "heading": 0.03870227011836747,
+            "angularVelocity": 0.28457545746724416,
+            "velocityX": -1.401815650572125,
+            "velocityY": -0.4686535917478179,
+            "timestamp": 0.17752613641062487
         },
         {
-            "x": 7.306675971092295,
-            "y": 7.342201042854806,
-            "heading": 0.0730274998055895,
-            "angularVelocity": 0.38670720275422266,
-            "velocityX": -2.1032654483226527,
-            "velocityY": -0.7030137807724448,
-            "timestamp": 0.26628920461180017
+            "x": 7.30667604944407,
+            "y": 7.3422008161122045,
+            "heading": 0.0730276629215222,
+            "angularVelocity": 0.38670804756608035,
+            "velocityX": -2.1032650210965835,
+            "velocityY": -0.7030150212056434,
+            "timestamp": 0.2662892046159373
         },
         {
-            "x": 7.057674447994484,
-            "y": 7.2590026396848994,
-            "heading": 0.11075741429874605,
-            "angularVelocity": 0.42506320764649574,
-            "velocityX": -2.8052379005867665,
-            "velocityY": -0.9373087800171416,
-            "timestamp": 0.35505227281573354
+            "x": 7.057674574154849,
+            "y": 7.259002273186937,
+            "heading": 0.11075765125141449,
+            "angularVelocity": 0.42506403948785515,
+            "velocityX": -2.8052373619302045,
+            "velocityY": -0.9373103544914424,
+            "timestamp": 0.35505227282124974
         },
         {
-            "x": 6.746464686770232,
-            "y": 7.155112270199717,
-            "heading": 0.1333971305446714,
-            "angularVelocity": 0.25505783773756635,
-            "velocityX": -3.506072598902806,
-            "velocityY": -1.1704233707338827,
-            "timestamp": 0.4438153410196669
+            "x": 6.746464870184761,
+            "y": 7.155111739606062,
+            "heading": 0.13339737013954753,
+            "angularVelocity": 0.25505786749611997,
+            "velocityX": -3.5060719538216505,
+            "velocityY": -1.170425219424191,
+            "timestamp": 0.44381534102656217
         },
         {
-            "x": 6.428047616602866,
-            "y": 7.048517672195703,
-            "heading": 0.13339714147429826,
-            "angularVelocity": 1.23133576958653e-7,
-            "velocityX": -3.5872697576863386,
-            "velocityY": -1.200889065236652,
-            "timestamp": 0.5325784092236003
+            "x": 6.428047828205506,
+            "y": 7.048517057389171,
+            "heading": 0.13339738106725169,
+            "angularVelocity": 1.2311157711164836e-7,
+            "velocityX": -3.5872694400628884,
+            "velocityY": -1.2008900139638257,
+            "timestamp": 0.5325784092318746
         },
         {
-            "x": 6.109630567326096,
-            "y": 6.9419230117899575,
-            "heading": 0.13339715240409608,
-            "angularVelocity": 1.231354889650669e-7,
-            "velocityX": -3.5872695223333455,
-            "velocityY": -1.200889768252866,
-            "timestamp": 0.6213414774275338
+            "x": 6.109630786224927,
+            "y": 6.941922375176235,
+            "heading": 0.1333973919949535,
+            "angularVelocity": 1.23111550991103e-7,
+            "velocityX": -3.587269440077816,
+            "velocityY": -1.200890013919235,
+            "timestamp": 0.621341477437187
         },
         {
-            "x": 5.798381237872348,
-            "y": 6.8380188367314085,
-            "heading": 0.15626327537058046,
-            "angularVelocity": 0.25760852379277716,
-            "velocityX": -3.506518372457005,
-            "velocityY": -1.170578903621919,
-            "timestamp": 0.7101045456314672
+            "x": 5.798381397539494,
+            "y": 6.838018370585249,
+            "heading": 0.15626352052401435,
+            "angularVelocity": 0.2576085864583032,
+            "velocityX": -3.5065190397107555,
+            "velocityY": -1.1705769831075976,
+            "timestamp": 0.7101045456424995
         },
         {
-            "x": 5.549363092262374,
-            "y": 6.7548143649024714,
-            "heading": 0.19280777245315245,
-            "angularVelocity": 0.41170835825755314,
-            "velocityX": -2.8054251689136636,
-            "velocityY": -0.9373771492347068,
-            "timestamp": 0.7988676138354006
+            "x": 5.5493631968567065,
+            "y": 6.754814060290535,
+            "heading": 0.1928079538664876,
+            "angularVelocity": 0.4117076401528937,
+            "velocityX": -2.8054257893230465,
+            "velocityY": -0.9373753293689796,
+            "timestamp": 0.7988676138478119
         },
         {
-            "x": 5.36265388534366,
-            "y": 6.692406232212667,
-            "heading": 0.22585001680889633,
-            "angularVelocity": 0.3722521655212111,
-            "velocityX": -2.1034559833970325,
-            "velocityY": -0.7030866998372853,
-            "timestamp": 0.887630682039334
+            "x": 5.362653942099421,
+            "y": 6.692406067264648,
+            "heading": 0.22585011955614995,
+            "angularVelocity": 0.37225127926004886,
+            "velocityX": -2.1034565223155943,
+            "velocityY": -0.703085126367409,
+            "timestamp": 0.8876306820531243
         },
         {
-            "x": 5.238210362873152,
-            "y": 6.650801584329758,
-            "heading": 0.25010700866879837,
-            "angularVelocity": 0.27327797868904535,
-            "velocityX": -1.4019740978791027,
-            "velocityY": -0.46871574772688473,
-            "timestamp": 0.9763937502432675
+            "x": 5.238210383284219,
+            "y": 6.650801525067523,
+            "heading": 0.25010704549903756,
+            "angularVelocity": 0.2732772360607878,
+            "velocityX": -1.4019745073178984,
+            "velocityY": -0.4687145570597535,
+            "timestamp": 0.9763937502584368
         },
         {
             "x": 5.176,
             "y": 6.63,
             "heading": 0.263,
-            "angularVelocity": 0.1452517538281686,
-            "velocityX": -0.7008586355989793,
-            "velocityY": -0.23434954143793293,
-            "timestamp": 1.065156818447201
+            "angularVelocity": 0.1452513388947291,
+            "velocityX": -0.7008588655398115,
+            "velocityY": -0.2343488737838604,
+            "timestamp": 1.0651568184637492
         },
         {
             "x": 5.176,
             "y": 6.63,
             "heading": 0.263,
-            "angularVelocity": 0,
-            "velocityX": -1.1321901910980418e-33,
+            "angularVelocity": -2.2743517080702824e-31,
+            "velocityX": 0,
             "velocityY": 0,
-            "timestamp": 1.1539198866511342
+            "timestamp": 1.1539198866690616
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.1.traj
index dc1b491d..f95228f2 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.1.traj	
@@ -3,146 +3,146 @@
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": -1.0300263407047986e-28,
-            "angularVelocity": -5.012733855435028e-29,
+            "heading": -6.443666081756986e-29,
+            "angularVelocity": 1.4558689567742243e-28,
             "velocityX": 0,
-            "velocityY": -6.93481134012921e-30,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.593939909156862,
-            "y": 7.445359818001645,
-            "heading": -0.04618113523184224,
-            "angularVelocity": -0.44533187598567886,
-            "velocityX": -0.829890852839506,
-            "velocityY": -0.20867964370321365,
-            "timestamp": 0.10370049332232066
+            "x": 7.593939912495806,
+            "y": 7.445359803187238,
+            "heading": -0.04618112566514864,
+            "angularVelocity": -0.44533178373248467,
+            "velocityX": -0.8298908206418381,
+            "velocityY": -0.2086797865609081,
+            "timestamp": 0.10370049332228601
         },
         {
-            "x": 7.42157070597782,
-            "y": 7.402025181316033,
-            "heading": -0.13159429090032063,
-            "angularVelocity": -0.8236523562429084,
-            "velocityX": -1.6621830586985462,
-            "velocityY": -0.41788264739409464,
-            "timestamp": 0.20740098664464132
+            "x": 7.421570715628637,
+            "y": 7.402025139488172,
+            "heading": -0.13159426902714605,
+            "angularVelocity": -0.823652237569843,
+            "velocityX": -1.662182997832739,
+            "velocityY": -0.41788290788915955,
+            "timestamp": 0.20740098664457202
         },
         {
-            "x": 7.162505106155824,
-            "y": 7.336932536575702,
-            "heading": -0.24235044061142916,
-            "angularVelocity": -1.0680387928995545,
-            "velocityX": -2.4982099074184148,
-            "velocityY": -0.6276985061006933,
-            "timestamp": 0.31110147996696197
+            "x": 7.162505124622784,
+            "y": 7.336932458231965,
+            "heading": -0.24235041158328322,
+            "angularVelocity": -1.0680387239038096,
+            "velocityX": -2.4982098224038025,
+            "velocityY": -0.627698858229146,
+            "timestamp": 0.311101479966858
         },
         {
-            "x": 6.816624706468089,
-            "y": 7.25023883508299,
-            "heading": -0.3315040304347048,
-            "angularVelocity": -0.8597219450650766,
-            "velocityX": -3.3353785368472084,
-            "velocityY": -0.8360008589665135,
-            "timestamp": 0.41480197328928264
+            "x": 6.816624735331939,
+            "y": 7.250238713519174,
+            "heading": -0.3315039987658666,
+            "angularVelocity": -0.8597219196007858,
+            "velocityX": -3.3353784365894983,
+            "velocityY": -0.8360012757447058,
+            "timestamp": 0.41480197328914403
         },
         {
-            "x": 6.436297343977517,
-            "y": 7.154089263427661,
-            "heading": -0.33150405743314826,
-            "angularVelocity": -2.603501934813404e-7,
-            "velocityX": -3.6675559614595192,
-            "velocityY": -0.9271852869251594,
-            "timestamp": 0.5185024666116033
+            "x": 6.436297385949381,
+            "y": 7.154089090014683,
+            "heading": -0.3315040257643058,
+            "angularVelocity": -2.603501519346576e-7,
+            "velocityX": -3.6675558350581356,
+            "velocityY": -0.927185786914993,
+            "timestamp": 0.5185024666114301
         },
         {
-            "x": 6.0559700009320565,
-            "y": 7.057939614855378,
-            "heading": -0.3315040844315366,
-            "angularVelocity": -2.603496620899773e-7,
-            "velocityX": -3.6675557739472766,
-            "velocityY": -0.9271860286473393,
-            "timestamp": 0.6222029599339239
+            "x": 6.055970043958053,
+            "y": 7.05793943727301,
+            "heading": -0.33150405276269507,
+            "angularVelocity": -2.60349670609762e-7,
+            "velocityX": -3.6675557637833287,
+            "velocityY": -0.9271860688537472,
+            "timestamp": 0.6222029599337161
         },
         {
-            "x": 5.675642815380094,
-            "y": 6.961789343307514,
-            "heading": -0.3315041114299189,
-            "angularVelocity": -2.603496031733668e-7,
-            "velocityX": -3.667554255212956,
-            "velocityY": -0.927192036097853,
-            "timestamp": 0.7259034532562446
+            "x": 5.675642817406181,
+            "y": 6.961789327902759,
+            "heading": -0.33150407976108204,
+            "angularVelocity": -2.6034964813703677e-7,
+            "velocityX": -3.6675546505826837,
+            "velocityY": -0.9271904721942118,
+            "timestamp": 0.7259034532560021
         },
         {
-            "x": 5.306931757393967,
-            "y": 6.827827416139461,
-            "heading": -0.33150413923830896,
-            "angularVelocity": -2.681606334045615e-7,
-            "velocityX": -3.5555381288313455,
-            "velocityY": -1.2918157173241618,
-            "timestamp": 0.8296039465785653
+            "x": 5.306931756724127,
+            "y": 6.827827408155551,
+            "heading": -0.331504107569482,
+            "angularVelocity": -2.6816073038858223e-7,
+            "velocityX": -3.5555381548297533,
+            "velocityY": -1.2918156457642633,
+            "timestamp": 0.8296039465782882
         },
         {
-            "x": 4.978271181730368,
-            "y": 6.6136408985726165,
-            "heading": -0.3315041735834931,
-            "angularVelocity": -3.311959575473401e-7,
-            "velocityX": -3.1693250931994936,
-            "velocityY": -2.0654339309757477,
-            "timestamp": 0.933304439900886
+            "x": 4.97827117667767,
+            "y": 6.613640897314209,
+            "heading": -0.3315041419146958,
+            "angularVelocity": -3.311962425493206e-7,
+            "velocityX": -3.1693251354651193,
+            "velocityY": -2.065433866121419,
+            "timestamp": 0.9333044399005742
         },
         {
-            "x": 4.706831855999529,
-            "y": 6.330419553539566,
-            "heading": -0.3315041734698381,
-            "angularVelocity": 1.0959926731432931e-9,
-            "velocityX": -2.617531672555858,
-            "velocityY": -2.7311475187754666,
-            "timestamp": 1.0370049332232065
+            "x": 4.706831845168329,
+            "y": 6.330419557819514,
+            "heading": -0.3315041418010325,
+            "angularVelocity": 1.0960729474265148e-9,
+            "velocityX": -2.6175317282796997,
+            "velocityY": -2.7311474653691747,
+            "timestamp": 1.0370049332228601
         },
         {
-            "x": 4.5189225775062924,
-            "y": 6.028421356099611,
-            "heading": -0.23258180854056226,
-            "angularVelocity": 0.9539237641021904,
-            "velocityX": -1.8120384240525347,
-            "velocityY": -2.912215629498277,
-            "timestamp": 1.1407054265455272
+            "x": 4.518922560808715,
+            "y": 6.028421366315536,
+            "heading": -0.2325817741817243,
+            "angularVelocity": 0.9539237900428917,
+            "velocityX": -1.8120384806234937,
+            "velocityY": -2.912215572257732,
+            "timestamp": 1.1407054265451462
         },
         {
-            "x": 4.388098272679404,
-            "y": 5.795527872027247,
-            "heading": -0.12496959809450543,
-            "angularVelocity": 1.0377212971549954,
-            "velocityX": -1.2615591366596612,
-            "velocityY": -2.2458281210725595,
-            "timestamp": 1.2444059198678479
+            "x": 4.388098262013575,
+            "y": 5.795527878516759,
+            "heading": -0.12496957799500227,
+            "angularVelocity": 1.0377211596504115,
+            "velocityX": -1.2615590784949995,
+            "velocityY": -2.245828157007679,
+            "timestamp": 1.2444059198674322
         },
         {
-            "x": 4.304052104796344,
-            "y": 5.638778152214578,
-            "heading": -0.04358897284576924,
-            "angularVelocity": 0.7847660376675182,
-            "velocityX": -0.8104702802311671,
-            "velocityY": -1.5115619491362144,
-            "timestamp": 1.3481064131901686
+            "x": 4.304052100915554,
+            "y": 5.638778154553602,
+            "heading": -0.0435889655527213,
+            "angularVelocity": 0.7847659141732163,
+            "velocityX": -0.8104702148022851,
+            "velocityY": -1.5115619891604988,
+            "timestamp": 1.3481064131897182
         },
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0.42033525057857213,
-            "velocityX": -0.3958718370677089,
-            "velocityY": -0.7596699850764065,
-            "timestamp": 1.4518069065124892
+            "heading": 7.875266725539043e-28,
+            "angularVelocity": 0.42033518025077415,
+            "velocityX": -0.39587179964482383,
+            "velocityY": -0.7596700076322139,
+            "timestamp": 1.4518069065120043
         },
         {
             "x": 4.263,
             "y": 5.56,
             "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -5.117426772684828e-28,
-            "velocityY": 3.5837289853658606e-29,
-            "timestamp": 1.55550739983481
+            "angularVelocity": -1.3117957600781845e-26,
+            "velocityX": -2.8963805377195657e-28,
+            "velocityY": 0,
+            "timestamp": 1.5555073998342903
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.traj
index dc1b491d..f95228f2 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.traj	
@@ -3,146 +3,146 @@
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": -1.0300263407047986e-28,
-            "angularVelocity": -5.012733855435028e-29,
+            "heading": -6.443666081756986e-29,
+            "angularVelocity": 1.4558689567742243e-28,
             "velocityX": 0,
-            "velocityY": -6.93481134012921e-30,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.593939909156862,
-            "y": 7.445359818001645,
-            "heading": -0.04618113523184224,
-            "angularVelocity": -0.44533187598567886,
-            "velocityX": -0.829890852839506,
-            "velocityY": -0.20867964370321365,
-            "timestamp": 0.10370049332232066
+            "x": 7.593939912495806,
+            "y": 7.445359803187238,
+            "heading": -0.04618112566514864,
+            "angularVelocity": -0.44533178373248467,
+            "velocityX": -0.8298908206418381,
+            "velocityY": -0.2086797865609081,
+            "timestamp": 0.10370049332228601
         },
         {
-            "x": 7.42157070597782,
-            "y": 7.402025181316033,
-            "heading": -0.13159429090032063,
-            "angularVelocity": -0.8236523562429084,
-            "velocityX": -1.6621830586985462,
-            "velocityY": -0.41788264739409464,
-            "timestamp": 0.20740098664464132
+            "x": 7.421570715628637,
+            "y": 7.402025139488172,
+            "heading": -0.13159426902714605,
+            "angularVelocity": -0.823652237569843,
+            "velocityX": -1.662182997832739,
+            "velocityY": -0.41788290788915955,
+            "timestamp": 0.20740098664457202
         },
         {
-            "x": 7.162505106155824,
-            "y": 7.336932536575702,
-            "heading": -0.24235044061142916,
-            "angularVelocity": -1.0680387928995545,
-            "velocityX": -2.4982099074184148,
-            "velocityY": -0.6276985061006933,
-            "timestamp": 0.31110147996696197
+            "x": 7.162505124622784,
+            "y": 7.336932458231965,
+            "heading": -0.24235041158328322,
+            "angularVelocity": -1.0680387239038096,
+            "velocityX": -2.4982098224038025,
+            "velocityY": -0.627698858229146,
+            "timestamp": 0.311101479966858
         },
         {
-            "x": 6.816624706468089,
-            "y": 7.25023883508299,
-            "heading": -0.3315040304347048,
-            "angularVelocity": -0.8597219450650766,
-            "velocityX": -3.3353785368472084,
-            "velocityY": -0.8360008589665135,
-            "timestamp": 0.41480197328928264
+            "x": 6.816624735331939,
+            "y": 7.250238713519174,
+            "heading": -0.3315039987658666,
+            "angularVelocity": -0.8597219196007858,
+            "velocityX": -3.3353784365894983,
+            "velocityY": -0.8360012757447058,
+            "timestamp": 0.41480197328914403
         },
         {
-            "x": 6.436297343977517,
-            "y": 7.154089263427661,
-            "heading": -0.33150405743314826,
-            "angularVelocity": -2.603501934813404e-7,
-            "velocityX": -3.6675559614595192,
-            "velocityY": -0.9271852869251594,
-            "timestamp": 0.5185024666116033
+            "x": 6.436297385949381,
+            "y": 7.154089090014683,
+            "heading": -0.3315040257643058,
+            "angularVelocity": -2.603501519346576e-7,
+            "velocityX": -3.6675558350581356,
+            "velocityY": -0.927185786914993,
+            "timestamp": 0.5185024666114301
         },
         {
-            "x": 6.0559700009320565,
-            "y": 7.057939614855378,
-            "heading": -0.3315040844315366,
-            "angularVelocity": -2.603496620899773e-7,
-            "velocityX": -3.6675557739472766,
-            "velocityY": -0.9271860286473393,
-            "timestamp": 0.6222029599339239
+            "x": 6.055970043958053,
+            "y": 7.05793943727301,
+            "heading": -0.33150405276269507,
+            "angularVelocity": -2.60349670609762e-7,
+            "velocityX": -3.6675557637833287,
+            "velocityY": -0.9271860688537472,
+            "timestamp": 0.6222029599337161
         },
         {
-            "x": 5.675642815380094,
-            "y": 6.961789343307514,
-            "heading": -0.3315041114299189,
-            "angularVelocity": -2.603496031733668e-7,
-            "velocityX": -3.667554255212956,
-            "velocityY": -0.927192036097853,
-            "timestamp": 0.7259034532562446
+            "x": 5.675642817406181,
+            "y": 6.961789327902759,
+            "heading": -0.33150407976108204,
+            "angularVelocity": -2.6034964813703677e-7,
+            "velocityX": -3.6675546505826837,
+            "velocityY": -0.9271904721942118,
+            "timestamp": 0.7259034532560021
         },
         {
-            "x": 5.306931757393967,
-            "y": 6.827827416139461,
-            "heading": -0.33150413923830896,
-            "angularVelocity": -2.681606334045615e-7,
-            "velocityX": -3.5555381288313455,
-            "velocityY": -1.2918157173241618,
-            "timestamp": 0.8296039465785653
+            "x": 5.306931756724127,
+            "y": 6.827827408155551,
+            "heading": -0.331504107569482,
+            "angularVelocity": -2.6816073038858223e-7,
+            "velocityX": -3.5555381548297533,
+            "velocityY": -1.2918156457642633,
+            "timestamp": 0.8296039465782882
         },
         {
-            "x": 4.978271181730368,
-            "y": 6.6136408985726165,
-            "heading": -0.3315041735834931,
-            "angularVelocity": -3.311959575473401e-7,
-            "velocityX": -3.1693250931994936,
-            "velocityY": -2.0654339309757477,
-            "timestamp": 0.933304439900886
+            "x": 4.97827117667767,
+            "y": 6.613640897314209,
+            "heading": -0.3315041419146958,
+            "angularVelocity": -3.311962425493206e-7,
+            "velocityX": -3.1693251354651193,
+            "velocityY": -2.065433866121419,
+            "timestamp": 0.9333044399005742
         },
         {
-            "x": 4.706831855999529,
-            "y": 6.330419553539566,
-            "heading": -0.3315041734698381,
-            "angularVelocity": 1.0959926731432931e-9,
-            "velocityX": -2.617531672555858,
-            "velocityY": -2.7311475187754666,
-            "timestamp": 1.0370049332232065
+            "x": 4.706831845168329,
+            "y": 6.330419557819514,
+            "heading": -0.3315041418010325,
+            "angularVelocity": 1.0960729474265148e-9,
+            "velocityX": -2.6175317282796997,
+            "velocityY": -2.7311474653691747,
+            "timestamp": 1.0370049332228601
         },
         {
-            "x": 4.5189225775062924,
-            "y": 6.028421356099611,
-            "heading": -0.23258180854056226,
-            "angularVelocity": 0.9539237641021904,
-            "velocityX": -1.8120384240525347,
-            "velocityY": -2.912215629498277,
-            "timestamp": 1.1407054265455272
+            "x": 4.518922560808715,
+            "y": 6.028421366315536,
+            "heading": -0.2325817741817243,
+            "angularVelocity": 0.9539237900428917,
+            "velocityX": -1.8120384806234937,
+            "velocityY": -2.912215572257732,
+            "timestamp": 1.1407054265451462
         },
         {
-            "x": 4.388098272679404,
-            "y": 5.795527872027247,
-            "heading": -0.12496959809450543,
-            "angularVelocity": 1.0377212971549954,
-            "velocityX": -1.2615591366596612,
-            "velocityY": -2.2458281210725595,
-            "timestamp": 1.2444059198678479
+            "x": 4.388098262013575,
+            "y": 5.795527878516759,
+            "heading": -0.12496957799500227,
+            "angularVelocity": 1.0377211596504115,
+            "velocityX": -1.2615590784949995,
+            "velocityY": -2.245828157007679,
+            "timestamp": 1.2444059198674322
         },
         {
-            "x": 4.304052104796344,
-            "y": 5.638778152214578,
-            "heading": -0.04358897284576924,
-            "angularVelocity": 0.7847660376675182,
-            "velocityX": -0.8104702802311671,
-            "velocityY": -1.5115619491362144,
-            "timestamp": 1.3481064131901686
+            "x": 4.304052100915554,
+            "y": 5.638778154553602,
+            "heading": -0.0435889655527213,
+            "angularVelocity": 0.7847659141732163,
+            "velocityX": -0.8104702148022851,
+            "velocityY": -1.5115619891604988,
+            "timestamp": 1.3481064131897182
         },
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0.42033525057857213,
-            "velocityX": -0.3958718370677089,
-            "velocityY": -0.7596699850764065,
-            "timestamp": 1.4518069065124892
+            "heading": 7.875266725539043e-28,
+            "angularVelocity": 0.42033518025077415,
+            "velocityX": -0.39587179964482383,
+            "velocityY": -0.7596700076322139,
+            "timestamp": 1.4518069065120043
         },
         {
             "x": 4.263,
             "y": 5.56,
             "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -5.117426772684828e-28,
-            "velocityY": 3.5837289853658606e-29,
-            "timestamp": 1.55550739983481
+            "angularVelocity": -1.3117957600781845e-26,
+            "velocityX": -2.8963805377195657e-28,
+            "velocityY": 0,
+            "timestamp": 1.5555073998342903
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S3.1.traj
new file mode 100644
index 00000000..f3e004b9
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S3.1.traj	
@@ -0,0 +1,265 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 3.237981257278863e-31,
+            "velocityX": 3.6994687797817426e-32,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.663124715083084,
+            "y": 7.408544235319849,
+            "heading": 0.027485198347684953,
+            "angularVelocity": 0.3204918925418427,
+            "velocityX": -0.19677471240300645,
+            "velocityY": -0.6816250119548423,
+            "timestamp": 0.08575941852911824
+        },
+        {
+            "x": 7.629347887601235,
+            "y": 7.291532761279695,
+            "heading": 0.07929730291499706,
+            "angularVelocity": 0.6041564350126787,
+            "velocityX": -0.3938556028149893,
+            "velocityY": -1.3644154315298311,
+            "timestamp": 0.17151883705823648
+        },
+        {
+            "x": 7.578636110956943,
+            "y": 7.115823334781324,
+            "heading": 0.15015093083574435,
+            "angularVelocity": 0.8261906288076145,
+            "velocityX": -0.5913260317532774,
+            "velocityY": -2.048864480566788,
+            "timestamp": 0.25727825558735473
+        },
+        {
+            "x": 7.510961813450331,
+            "y": 6.881225838812792,
+            "heading": 0.22908991381513497,
+            "angularVelocity": 0.9204701283345157,
+            "velocityX": -0.7891179612374983,
+            "velocityY": -2.7355303941208287,
+            "timestamp": 0.34303767411647296
+        },
+        {
+            "x": 7.426596388399054,
+            "y": 6.588221528517866,
+            "heading": 0.2780284674480044,
+            "angularVelocity": 0.5706493172671544,
+            "velocityX": -0.9837453016618997,
+            "velocityY": -3.4165846191627605,
+            "timestamp": 0.4287970926455912
+        },
+        {
+            "x": 7.336364964950643,
+            "y": 6.276599275391096,
+            "heading": 0.2780284905344079,
+            "angularVelocity": 2.691996271350766e-7,
+            "velocityX": -1.0521459333096546,
+            "velocityY": -3.6336796409243712,
+            "timestamp": 0.5145565111747095
+        },
+        {
+            "x": 7.246133490581996,
+            "y": 5.964977037008454,
+            "heading": 0.27802851362075814,
+            "angularVelocity": 2.6919900573505555e-7,
+            "velocityX": -1.0521465270664079,
+            "velocityY": -3.633679469000075,
+            "timestamp": 0.6003159297038277
+        },
+        {
+            "x": 7.155901958204915,
+            "y": 5.653354815422387,
+            "heading": 0.2780285367070952,
+            "angularVelocity": 2.6919885218329885e-7,
+            "velocityX": -1.0521472034753196,
+            "velocityY": -3.633679273143183,
+            "timestamp": 0.686075348232946
+        },
+        {
+            "x": 7.06567006903972,
+            "y": 5.341732697146042,
+            "heading": 0.27802855979342844,
+            "angularVelocity": 2.6919880800587287e-7,
+            "velocityX": -1.052151363812688,
+            "velocityY": -3.6336780684973866,
+            "timestamp": 0.7718347667620643
+        },
+        {
+            "x": 6.941702537617662,
+            "y": 5.041929112902265,
+            "heading": 0.2780285835706332,
+            "angularVelocity": 2.772547333115093e-7,
+            "velocityX": -1.445526725207054,
+            "velocityY": -3.495867735413615,
+            "timestamp": 0.8575941852911826
+        },
+        {
+            "x": 6.763533066668612,
+            "y": 4.770809627708675,
+            "heading": 0.2780286115416881,
+            "angularVelocity": 3.2615723612466423e-7,
+            "velocityX": -2.0775498948673006,
+            "velocityY": -3.1613960290733045,
+            "timestamp": 0.9433536038203009
+        },
+        {
+            "x": 6.537527544905599,
+            "y": 4.538061625407243,
+            "heading": 0.2780286466069666,
+            "angularVelocity": 4.0887962009830865e-7,
+            "velocityX": -2.6353434484432303,
+            "velocityY": -2.713964323608448,
+            "timestamp": 1.0291130223494191
+        },
+        {
+            "x": 6.274440682462666,
+            "y": 4.354088811409382,
+            "heading": 0.26884633764727317,
+            "angularVelocity": -0.10707055991260324,
+            "velocityX": -3.0677314160380695,
+            "velocityY": -2.145219932145364,
+            "timestamp": 1.1148724408785373
+        },
+        {
+            "x": 5.982323590323029,
+            "y": 4.22411616098336,
+            "heading": 0.2547621599051439,
+            "angularVelocity": -0.16422893232822805,
+            "velocityX": -3.40623918806603,
+            "velocityY": -1.5155495764222278,
+            "timestamp": 1.2006318594076555
+        },
+        {
+            "x": 5.6751302181547745,
+            "y": 4.153390532297334,
+            "heading": 0.22346642432676106,
+            "angularVelocity": -0.3649247641266729,
+            "velocityX": -3.582036555716069,
+            "velocityY": -0.8246980902979518,
+            "timestamp": 1.2863912779367737
+        },
+        {
+            "x": 5.366531848907471,
+            "y": 4.105798244476318,
+            "heading": 0.18319904977282217,
+            "angularVelocity": -0.4695388010388101,
+            "velocityX": -3.5984195618411468,
+            "velocityY": -0.5549511486584576,
+            "timestamp": 1.3721506964658918
+        },
+        {
+            "x": 5.15360011207683,
+            "y": 4.041515222282991,
+            "heading": 0.15622154454817708,
+            "angularVelocity": -0.44121073940112593,
+            "velocityX": -3.482448368249538,
+            "velocityY": -1.051333676582719,
+            "timestamp": 1.4332949588681712
+        },
+        {
+            "x": 4.961585993321137,
+            "y": 3.955708543808847,
+            "heading": 0.09940090208560654,
+            "angularVelocity": -0.9292882149552651,
+            "velocityX": -3.1403456548775974,
+            "velocityY": -1.403348001969601,
+            "timestamp": 1.4944392212704505
+        },
+        {
+            "x": 4.792433392014809,
+            "y": 3.849704139398821,
+            "heading": 0.01980990286006859,
+            "angularVelocity": -1.301692032882722,
+            "velocityX": -2.766450925410501,
+            "velocityY": -1.7336770490843985,
+            "timestamp": 1.5555834836727298
+        },
+        {
+            "x": 4.64716063510186,
+            "y": 3.7242164869664776,
+            "heading": -0.07646566712737501,
+            "angularVelocity": -1.5745642551712946,
+            "velocityX": -2.3759016987918233,
+            "velocityY": -2.0523209783239644,
+            "timestamp": 1.6167277460750091
+        },
+        {
+            "x": 4.526373969972362,
+            "y": 3.5797426645365276,
+            "heading": -0.18496768957846801,
+            "angularVelocity": -1.7745250034621045,
+            "velocityX": -1.975437438999914,
+            "velocityY": -2.362835313629761,
+            "timestamp": 1.6778720084772885
+        },
+        {
+            "x": 4.435318488505818,
+            "y": 3.4263170797911884,
+            "heading": -0.3033986089581413,
+            "angularVelocity": -1.936909772506439,
+            "velocityX": -1.489190937777188,
+            "velocityY": -2.5092392763841707,
+            "timestamp": 1.7390162708795678
+        },
+        {
+            "x": 4.37042278768838,
+            "y": 3.2883178140362905,
+            "heading": -0.39683380701899557,
+            "angularVelocity": -1.5281106417823267,
+            "velocityX": -1.0613538910728308,
+            "velocityY": -2.2569454652502787,
+            "timestamp": 1.800160533281847
+        },
+        {
+            "x": 4.3241368483863285,
+            "y": 3.1745112953889034,
+            "heading": -0.4672868225779202,
+            "angularVelocity": -1.1522424638210744,
+            "velocityX": -0.7569956277749857,
+            "velocityY": -1.8612787884925825,
+            "timestamp": 1.8613047956841264
+        },
+        {
+            "x": 4.292271472981756,
+            "y": 3.08778416242541,
+            "heading": -0.5177338397629753,
+            "angularVelocity": -0.8250490757931602,
+            "velocityX": -0.5211507041318677,
+            "velocityY": -1.418401818193496,
+            "timestamp": 1.9224490580864058
+        },
+        {
+            "x": 4.272405555044274,
+            "y": 3.0293931910282037,
+            "heading": -0.5502025950282511,
+            "angularVelocity": -0.5310188395381721,
+            "velocityX": -0.3249024055075056,
+            "velocityY": -0.9549705745576195,
+            "timestamp": 1.983593320488685
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -0.25836283489389134,
+            "velocityX": -0.15382563587721435,
+            "velocityY": -0.48071871134564137,
+            "timestamp": 2.0447375828909644
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 0,
+            "velocityX": 3.3381626772503296e-32,
+            "velocityY": -2.986960154995716e-30,
+            "timestamp": 2.105881845293244
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S3.traj
new file mode 100644
index 00000000..f3e004b9
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S3.traj	
@@ -0,0 +1,265 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 3.237981257278863e-31,
+            "velocityX": 3.6994687797817426e-32,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.663124715083084,
+            "y": 7.408544235319849,
+            "heading": 0.027485198347684953,
+            "angularVelocity": 0.3204918925418427,
+            "velocityX": -0.19677471240300645,
+            "velocityY": -0.6816250119548423,
+            "timestamp": 0.08575941852911824
+        },
+        {
+            "x": 7.629347887601235,
+            "y": 7.291532761279695,
+            "heading": 0.07929730291499706,
+            "angularVelocity": 0.6041564350126787,
+            "velocityX": -0.3938556028149893,
+            "velocityY": -1.3644154315298311,
+            "timestamp": 0.17151883705823648
+        },
+        {
+            "x": 7.578636110956943,
+            "y": 7.115823334781324,
+            "heading": 0.15015093083574435,
+            "angularVelocity": 0.8261906288076145,
+            "velocityX": -0.5913260317532774,
+            "velocityY": -2.048864480566788,
+            "timestamp": 0.25727825558735473
+        },
+        {
+            "x": 7.510961813450331,
+            "y": 6.881225838812792,
+            "heading": 0.22908991381513497,
+            "angularVelocity": 0.9204701283345157,
+            "velocityX": -0.7891179612374983,
+            "velocityY": -2.7355303941208287,
+            "timestamp": 0.34303767411647296
+        },
+        {
+            "x": 7.426596388399054,
+            "y": 6.588221528517866,
+            "heading": 0.2780284674480044,
+            "angularVelocity": 0.5706493172671544,
+            "velocityX": -0.9837453016618997,
+            "velocityY": -3.4165846191627605,
+            "timestamp": 0.4287970926455912
+        },
+        {
+            "x": 7.336364964950643,
+            "y": 6.276599275391096,
+            "heading": 0.2780284905344079,
+            "angularVelocity": 2.691996271350766e-7,
+            "velocityX": -1.0521459333096546,
+            "velocityY": -3.6336796409243712,
+            "timestamp": 0.5145565111747095
+        },
+        {
+            "x": 7.246133490581996,
+            "y": 5.964977037008454,
+            "heading": 0.27802851362075814,
+            "angularVelocity": 2.6919900573505555e-7,
+            "velocityX": -1.0521465270664079,
+            "velocityY": -3.633679469000075,
+            "timestamp": 0.6003159297038277
+        },
+        {
+            "x": 7.155901958204915,
+            "y": 5.653354815422387,
+            "heading": 0.2780285367070952,
+            "angularVelocity": 2.6919885218329885e-7,
+            "velocityX": -1.0521472034753196,
+            "velocityY": -3.633679273143183,
+            "timestamp": 0.686075348232946
+        },
+        {
+            "x": 7.06567006903972,
+            "y": 5.341732697146042,
+            "heading": 0.27802855979342844,
+            "angularVelocity": 2.6919880800587287e-7,
+            "velocityX": -1.052151363812688,
+            "velocityY": -3.6336780684973866,
+            "timestamp": 0.7718347667620643
+        },
+        {
+            "x": 6.941702537617662,
+            "y": 5.041929112902265,
+            "heading": 0.2780285835706332,
+            "angularVelocity": 2.772547333115093e-7,
+            "velocityX": -1.445526725207054,
+            "velocityY": -3.495867735413615,
+            "timestamp": 0.8575941852911826
+        },
+        {
+            "x": 6.763533066668612,
+            "y": 4.770809627708675,
+            "heading": 0.2780286115416881,
+            "angularVelocity": 3.2615723612466423e-7,
+            "velocityX": -2.0775498948673006,
+            "velocityY": -3.1613960290733045,
+            "timestamp": 0.9433536038203009
+        },
+        {
+            "x": 6.537527544905599,
+            "y": 4.538061625407243,
+            "heading": 0.2780286466069666,
+            "angularVelocity": 4.0887962009830865e-7,
+            "velocityX": -2.6353434484432303,
+            "velocityY": -2.713964323608448,
+            "timestamp": 1.0291130223494191
+        },
+        {
+            "x": 6.274440682462666,
+            "y": 4.354088811409382,
+            "heading": 0.26884633764727317,
+            "angularVelocity": -0.10707055991260324,
+            "velocityX": -3.0677314160380695,
+            "velocityY": -2.145219932145364,
+            "timestamp": 1.1148724408785373
+        },
+        {
+            "x": 5.982323590323029,
+            "y": 4.22411616098336,
+            "heading": 0.2547621599051439,
+            "angularVelocity": -0.16422893232822805,
+            "velocityX": -3.40623918806603,
+            "velocityY": -1.5155495764222278,
+            "timestamp": 1.2006318594076555
+        },
+        {
+            "x": 5.6751302181547745,
+            "y": 4.153390532297334,
+            "heading": 0.22346642432676106,
+            "angularVelocity": -0.3649247641266729,
+            "velocityX": -3.582036555716069,
+            "velocityY": -0.8246980902979518,
+            "timestamp": 1.2863912779367737
+        },
+        {
+            "x": 5.366531848907471,
+            "y": 4.105798244476318,
+            "heading": 0.18319904977282217,
+            "angularVelocity": -0.4695388010388101,
+            "velocityX": -3.5984195618411468,
+            "velocityY": -0.5549511486584576,
+            "timestamp": 1.3721506964658918
+        },
+        {
+            "x": 5.15360011207683,
+            "y": 4.041515222282991,
+            "heading": 0.15622154454817708,
+            "angularVelocity": -0.44121073940112593,
+            "velocityX": -3.482448368249538,
+            "velocityY": -1.051333676582719,
+            "timestamp": 1.4332949588681712
+        },
+        {
+            "x": 4.961585993321137,
+            "y": 3.955708543808847,
+            "heading": 0.09940090208560654,
+            "angularVelocity": -0.9292882149552651,
+            "velocityX": -3.1403456548775974,
+            "velocityY": -1.403348001969601,
+            "timestamp": 1.4944392212704505
+        },
+        {
+            "x": 4.792433392014809,
+            "y": 3.849704139398821,
+            "heading": 0.01980990286006859,
+            "angularVelocity": -1.301692032882722,
+            "velocityX": -2.766450925410501,
+            "velocityY": -1.7336770490843985,
+            "timestamp": 1.5555834836727298
+        },
+        {
+            "x": 4.64716063510186,
+            "y": 3.7242164869664776,
+            "heading": -0.07646566712737501,
+            "angularVelocity": -1.5745642551712946,
+            "velocityX": -2.3759016987918233,
+            "velocityY": -2.0523209783239644,
+            "timestamp": 1.6167277460750091
+        },
+        {
+            "x": 4.526373969972362,
+            "y": 3.5797426645365276,
+            "heading": -0.18496768957846801,
+            "angularVelocity": -1.7745250034621045,
+            "velocityX": -1.975437438999914,
+            "velocityY": -2.362835313629761,
+            "timestamp": 1.6778720084772885
+        },
+        {
+            "x": 4.435318488505818,
+            "y": 3.4263170797911884,
+            "heading": -0.3033986089581413,
+            "angularVelocity": -1.936909772506439,
+            "velocityX": -1.489190937777188,
+            "velocityY": -2.5092392763841707,
+            "timestamp": 1.7390162708795678
+        },
+        {
+            "x": 4.37042278768838,
+            "y": 3.2883178140362905,
+            "heading": -0.39683380701899557,
+            "angularVelocity": -1.5281106417823267,
+            "velocityX": -1.0613538910728308,
+            "velocityY": -2.2569454652502787,
+            "timestamp": 1.800160533281847
+        },
+        {
+            "x": 4.3241368483863285,
+            "y": 3.1745112953889034,
+            "heading": -0.4672868225779202,
+            "angularVelocity": -1.1522424638210744,
+            "velocityX": -0.7569956277749857,
+            "velocityY": -1.8612787884925825,
+            "timestamp": 1.8613047956841264
+        },
+        {
+            "x": 4.292271472981756,
+            "y": 3.08778416242541,
+            "heading": -0.5177338397629753,
+            "angularVelocity": -0.8250490757931602,
+            "velocityX": -0.5211507041318677,
+            "velocityY": -1.418401818193496,
+            "timestamp": 1.9224490580864058
+        },
+        {
+            "x": 4.272405555044274,
+            "y": 3.0293931910282037,
+            "heading": -0.5502025950282511,
+            "angularVelocity": -0.5310188395381721,
+            "velocityX": -0.3249024055075056,
+            "velocityY": -0.9549705745576195,
+            "timestamp": 1.983593320488685
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -0.25836283489389134,
+            "velocityX": -0.15382563587721435,
+            "velocityY": -0.48071871134564137,
+            "timestamp": 2.0447375828909644
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 0,
+            "velocityX": 3.3381626772503296e-32,
+            "velocityY": -2.986960154995716e-30,
+            "timestamp": 2.105881845293244
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S4.1.traj
new file mode 100644
index 00000000..6d9a6add
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S4.1.traj	
@@ -0,0 +1,301 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 9.776523821034258e-32,
+            "velocityX": -4.560355515853987e-33,
+            "velocityY": 2.3850698853192112e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 7.67836355889096,
+            "y": 7.400720406813562,
+            "heading": -0.028945523101477338,
+            "angularVelocity": -0.3234212568317029,
+            "velocityX": -0.018284687354286195,
+            "velocityY": -0.7405714954779761,
+            "timestamp": 0.0894978993806123
+        },
+        {
+            "x": 7.675074058245281,
+            "y": 7.268033905727148,
+            "heading": -0.0827237859595526,
+            "angularVelocity": -0.6008885485609575,
+            "velocityX": -0.03675505982201489,
+            "velocityY": -1.4825655351097347,
+            "timestamp": 0.1789957987612246
+        },
+        {
+            "x": 7.670107822881543,
+            "y": 7.068767684711926,
+            "heading": -0.1543450175390465,
+            "angularVelocity": -0.8002560068466026,
+            "velocityX": -0.05548996566520156,
+            "velocityY": -2.226490480718353,
+            "timestamp": 0.2684936981418369
+        },
+        {
+            "x": 7.663417838093003,
+            "y": 6.802742204879413,
+            "heading": -0.22912917227010834,
+            "angularVelocity": -0.8355967597967521,
+            "velocityX": -0.07475018782384167,
+            "velocityY": -2.9724214945109684,
+            "timestamp": 0.3579915975224492
+        },
+        {
+            "x": 7.654753648982881,
+            "y": 6.471615180012679,
+            "heading": -0.2502157544796387,
+            "angularVelocity": -0.23560980040272358,
+            "velocityX": -0.0968088543986332,
+            "velocityY": -3.6998301318619107,
+            "timestamp": 0.4474894969030615
+        },
+        {
+            "x": 7.646619499365586,
+            "y": 6.133147678353669,
+            "heading": -0.250215779137968,
+            "angularVelocity": -2.7551852588992195e-7,
+            "velocityX": -0.09088648642693918,
+            "velocityY": -3.78184855735655,
+            "timestamp": 0.5369873962836738
+        },
+        {
+            "x": 7.638485287213415,
+            "y": 5.7946801781975115,
+            "heading": -0.25021580379628167,
+            "angularVelocity": -2.755183512776458e-7,
+            "velocityX": -0.09088718515702116,
+            "velocityY": -3.7818485405645093,
+            "timestamp": 0.626485295664286
+        },
+        {
+            "x": 7.630351002745582,
+            "y": 5.456212679779282,
+            "heading": -0.2502158284545949,
+            "angularVelocity": -2.7551834628402704e-7,
+            "velocityX": -0.09088799317222593,
+            "velocityY": -3.781848521145859,
+            "timestamp": 0.7159831950448983
+        },
+        {
+            "x": 7.622216641973054,
+            "y": 5.117745183194865,
+            "heading": -0.25021585311291117,
+            "angularVelocity": -2.7551838019723297e-7,
+            "velocityX": -0.09088884575864668,
+            "velocityY": -3.781848500655846,
+            "timestamp": 0.8054810944255105
+        },
+        {
+            "x": 7.614082202627753,
+            "y": 4.779277688498788,
+            "heading": -0.25021587777122717,
+            "angularVelocity": -2.755183772950637e-7,
+            "velocityX": -0.09088972368733664,
+            "velocityY": -3.7818484795565834,
+            "timestamp": 0.8949789938061228
+        },
+        {
+            "x": 7.605947683187911,
+            "y": 4.440810195727643,
+            "heading": -0.2502159024295383,
+            "angularVelocity": -2.755183223713384e-7,
+            "velocityX": -0.09089061861940764,
+            "velocityY": -3.7818484580484664,
+            "timestamp": 0.984476893186735
+        },
+        {
+            "x": 7.597813082423135,
+            "y": 4.1023427049110195,
+            "heading": -0.25021592708784146,
+            "angularVelocity": -2.755182337798815e-7,
+            "velocityX": -0.09089152729921718,
+            "velocityY": -3.781848436209727,
+            "timestamp": 1.0739747925673473
+        },
+        {
+            "x": 7.589678399478048,
+            "y": 3.7638752160694957,
+            "heading": -0.2502159517461411,
+            "angularVelocity": -2.755181940361221e-7,
+            "velocityX": -0.09089244553653594,
+            "velocityY": -3.781848414141052,
+            "timestamp": 1.1634726919479597
+        },
+        {
+            "x": 7.581543633206817,
+            "y": 3.4254077292306317,
+            "heading": -0.2502159764044425,
+            "angularVelocity": -2.7551821391136536e-7,
+            "velocityX": -0.09089337657676429,
+            "velocityY": -3.7818483917644334,
+            "timestamp": 1.252970591328572
+        },
+        {
+            "x": 7.573407384740196,
+            "y": 3.0869402780182864,
+            "heading": -0.2502160010627749,
+            "angularVelocity": -2.7551856112041726e-7,
+            "velocityX": -0.09090993780780313,
+            "velocityY": -3.7818479936934355,
+            "timestamp": 1.3424684907091844
+        },
+        {
+            "x": 7.507089727245593,
+            "y": 2.754933679576005,
+            "heading": -0.25021602759782946,
+            "angularVelocity": -2.964880153822211e-7,
+            "velocityX": -0.7409968049928212,
+            "velocityY": -3.709658000243552,
+            "timestamp": 1.4319663900897968
+        },
+        {
+            "x": 7.3768883522731565,
+            "y": 2.4424052627551904,
+            "heading": -0.2502160612558337,
+            "angularVelocity": -3.7607591242222915e-7,
+            "velocityX": -1.454798111168213,
+            "velocityY": -3.492019577931195,
+            "timestamp": 1.5214642894704091
+        },
+        {
+            "x": 7.1878697455519145,
+            "y": 2.1615167326419513,
+            "heading": -0.2502161148657548,
+            "angularVelocity": -5.990075908840489e-7,
+            "velocityX": -2.111989309574672,
+            "velocityY": -3.1384929932119427,
+            "timestamp": 1.6109621888510215
+        },
+        {
+            "x": 6.94738938516625,
+            "y": 1.9231986374023808,
+            "heading": -0.25021645217340494,
+            "angularVelocity": -0.000003768889018580663,
+            "velocityX": -2.686994466350113,
+            "velocityY": -2.6628345121941095,
+            "timestamp": 1.7004600882316339
+        },
+        {
+            "x": 6.667655944824219,
+            "y": 1.738860011100769,
+            "heading": -0.25893161706240975,
+            "angularVelocity": -0.09737842954215001,
+            "velocityX": -3.125586659329227,
+            "velocityY": -2.059697798242895,
+            "timestamp": 1.7899579876122462
+        },
+        {
+            "x": 6.480451878153193,
+            "y": 1.646736643588724,
+            "heading": -0.268441983388845,
+            "angularVelocity": -0.16928620019223647,
+            "velocityX": -3.3322654479868237,
+            "velocityY": -1.6398122111953808,
+            "timestamp": 1.846137205502623
+        },
+        {
+            "x": 6.284980922842322,
+            "y": 1.5795583069683912,
+            "heading": -0.28291661109989463,
+            "angularVelocity": -0.25765092955355146,
+            "velocityX": -3.47941752575298,
+            "velocityY": -1.1957862558966221,
+            "timestamp": 1.9023164233929999
+        },
+        {
+            "x": 6.086191677834544,
+            "y": 1.5383458560749974,
+            "heading": -0.3070406119029933,
+            "angularVelocity": -0.429411474723066,
+            "velocityX": -3.53848366838567,
+            "velocityY": -0.7335889042423714,
+            "timestamp": 1.9584956412833767
+        },
+        {
+            "x": 5.891572555778559,
+            "y": 1.5225106097486452,
+            "heading": -0.35143250880916643,
+            "angularVelocity": -0.7901836047765775,
+            "velocityX": -3.4642547433776456,
+            "velocityY": -0.2818701811985366,
+            "timestamp": 2.0146748591737533
+        },
+        {
+            "x": 5.713348199791213,
+            "y": 1.5270062964148978,
+            "heading": -0.407385332387855,
+            "angularVelocity": -0.9959701412692558,
+            "velocityX": -3.172425011952234,
+            "velocityY": 0.08002401662169539,
+            "timestamp": 2.07085407706413
+        },
+        {
+            "x": 5.558872974287929,
+            "y": 1.5428236796222032,
+            "heading": -0.4653888399418059,
+            "angularVelocity": -1.0324726781908529,
+            "velocityX": -2.7496862951120815,
+            "velocityY": 0.2815522145247918,
+            "timestamp": 2.1270332949545065
+        },
+        {
+            "x": 5.4303297125098124,
+            "y": 1.5629685460150702,
+            "heading": -0.519265179295127,
+            "angularVelocity": -0.9590083553396718,
+            "velocityX": -2.2880927610801525,
+            "velocityY": 0.35858217948453525,
+            "timestamp": 2.183212512844883
+        },
+        {
+            "x": 5.3280145107455015,
+            "y": 1.583083454211334,
+            "heading": -0.5654730488836437,
+            "angularVelocity": -0.8225082392332849,
+            "velocityX": -1.8212286608183241,
+            "velocityY": 0.3580489182942135,
+            "timestamp": 2.2393917307352598
+        },
+        {
+            "x": 5.251723840078425,
+            "y": 1.6004232571954151,
+            "heading": -0.6018676415285119,
+            "angularVelocity": -0.6478301765590008,
+            "velocityX": -1.3579874112157035,
+            "velocityY": 0.3086515554188953,
+            "timestamp": 2.2955709486256364
+        },
+        {
+            "x": 5.20115218978415,
+            "y": 1.613158164546165,
+            "heading": -0.6270407807035692,
+            "angularVelocity": -0.44808632302748147,
+            "velocityX": -0.9001843064628601,
+            "velocityY": 0.22668359989631479,
+            "timestamp": 2.351750166516013
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -0.2306763921441399,
+            "velocityX": -0.44771342016952703,
+            "velocityY": 0.12178587938311458,
+            "timestamp": 2.4079293844063896
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 3.4924263364045203e-31,
+            "velocityX": 0,
+            "velocityY": 4.738614736798697e-32,
+            "timestamp": 2.464108602296766
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S4.traj
new file mode 100644
index 00000000..6d9a6add
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S4.traj	
@@ -0,0 +1,301 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 9.776523821034258e-32,
+            "velocityX": -4.560355515853987e-33,
+            "velocityY": 2.3850698853192112e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 7.67836355889096,
+            "y": 7.400720406813562,
+            "heading": -0.028945523101477338,
+            "angularVelocity": -0.3234212568317029,
+            "velocityX": -0.018284687354286195,
+            "velocityY": -0.7405714954779761,
+            "timestamp": 0.0894978993806123
+        },
+        {
+            "x": 7.675074058245281,
+            "y": 7.268033905727148,
+            "heading": -0.0827237859595526,
+            "angularVelocity": -0.6008885485609575,
+            "velocityX": -0.03675505982201489,
+            "velocityY": -1.4825655351097347,
+            "timestamp": 0.1789957987612246
+        },
+        {
+            "x": 7.670107822881543,
+            "y": 7.068767684711926,
+            "heading": -0.1543450175390465,
+            "angularVelocity": -0.8002560068466026,
+            "velocityX": -0.05548996566520156,
+            "velocityY": -2.226490480718353,
+            "timestamp": 0.2684936981418369
+        },
+        {
+            "x": 7.663417838093003,
+            "y": 6.802742204879413,
+            "heading": -0.22912917227010834,
+            "angularVelocity": -0.8355967597967521,
+            "velocityX": -0.07475018782384167,
+            "velocityY": -2.9724214945109684,
+            "timestamp": 0.3579915975224492
+        },
+        {
+            "x": 7.654753648982881,
+            "y": 6.471615180012679,
+            "heading": -0.2502157544796387,
+            "angularVelocity": -0.23560980040272358,
+            "velocityX": -0.0968088543986332,
+            "velocityY": -3.6998301318619107,
+            "timestamp": 0.4474894969030615
+        },
+        {
+            "x": 7.646619499365586,
+            "y": 6.133147678353669,
+            "heading": -0.250215779137968,
+            "angularVelocity": -2.7551852588992195e-7,
+            "velocityX": -0.09088648642693918,
+            "velocityY": -3.78184855735655,
+            "timestamp": 0.5369873962836738
+        },
+        {
+            "x": 7.638485287213415,
+            "y": 5.7946801781975115,
+            "heading": -0.25021580379628167,
+            "angularVelocity": -2.755183512776458e-7,
+            "velocityX": -0.09088718515702116,
+            "velocityY": -3.7818485405645093,
+            "timestamp": 0.626485295664286
+        },
+        {
+            "x": 7.630351002745582,
+            "y": 5.456212679779282,
+            "heading": -0.2502158284545949,
+            "angularVelocity": -2.7551834628402704e-7,
+            "velocityX": -0.09088799317222593,
+            "velocityY": -3.781848521145859,
+            "timestamp": 0.7159831950448983
+        },
+        {
+            "x": 7.622216641973054,
+            "y": 5.117745183194865,
+            "heading": -0.25021585311291117,
+            "angularVelocity": -2.7551838019723297e-7,
+            "velocityX": -0.09088884575864668,
+            "velocityY": -3.781848500655846,
+            "timestamp": 0.8054810944255105
+        },
+        {
+            "x": 7.614082202627753,
+            "y": 4.779277688498788,
+            "heading": -0.25021587777122717,
+            "angularVelocity": -2.755183772950637e-7,
+            "velocityX": -0.09088972368733664,
+            "velocityY": -3.7818484795565834,
+            "timestamp": 0.8949789938061228
+        },
+        {
+            "x": 7.605947683187911,
+            "y": 4.440810195727643,
+            "heading": -0.2502159024295383,
+            "angularVelocity": -2.755183223713384e-7,
+            "velocityX": -0.09089061861940764,
+            "velocityY": -3.7818484580484664,
+            "timestamp": 0.984476893186735
+        },
+        {
+            "x": 7.597813082423135,
+            "y": 4.1023427049110195,
+            "heading": -0.25021592708784146,
+            "angularVelocity": -2.755182337798815e-7,
+            "velocityX": -0.09089152729921718,
+            "velocityY": -3.781848436209727,
+            "timestamp": 1.0739747925673473
+        },
+        {
+            "x": 7.589678399478048,
+            "y": 3.7638752160694957,
+            "heading": -0.2502159517461411,
+            "angularVelocity": -2.755181940361221e-7,
+            "velocityX": -0.09089244553653594,
+            "velocityY": -3.781848414141052,
+            "timestamp": 1.1634726919479597
+        },
+        {
+            "x": 7.581543633206817,
+            "y": 3.4254077292306317,
+            "heading": -0.2502159764044425,
+            "angularVelocity": -2.7551821391136536e-7,
+            "velocityX": -0.09089337657676429,
+            "velocityY": -3.7818483917644334,
+            "timestamp": 1.252970591328572
+        },
+        {
+            "x": 7.573407384740196,
+            "y": 3.0869402780182864,
+            "heading": -0.2502160010627749,
+            "angularVelocity": -2.7551856112041726e-7,
+            "velocityX": -0.09090993780780313,
+            "velocityY": -3.7818479936934355,
+            "timestamp": 1.3424684907091844
+        },
+        {
+            "x": 7.507089727245593,
+            "y": 2.754933679576005,
+            "heading": -0.25021602759782946,
+            "angularVelocity": -2.964880153822211e-7,
+            "velocityX": -0.7409968049928212,
+            "velocityY": -3.709658000243552,
+            "timestamp": 1.4319663900897968
+        },
+        {
+            "x": 7.3768883522731565,
+            "y": 2.4424052627551904,
+            "heading": -0.2502160612558337,
+            "angularVelocity": -3.7607591242222915e-7,
+            "velocityX": -1.454798111168213,
+            "velocityY": -3.492019577931195,
+            "timestamp": 1.5214642894704091
+        },
+        {
+            "x": 7.1878697455519145,
+            "y": 2.1615167326419513,
+            "heading": -0.2502161148657548,
+            "angularVelocity": -5.990075908840489e-7,
+            "velocityX": -2.111989309574672,
+            "velocityY": -3.1384929932119427,
+            "timestamp": 1.6109621888510215
+        },
+        {
+            "x": 6.94738938516625,
+            "y": 1.9231986374023808,
+            "heading": -0.25021645217340494,
+            "angularVelocity": -0.000003768889018580663,
+            "velocityX": -2.686994466350113,
+            "velocityY": -2.6628345121941095,
+            "timestamp": 1.7004600882316339
+        },
+        {
+            "x": 6.667655944824219,
+            "y": 1.738860011100769,
+            "heading": -0.25893161706240975,
+            "angularVelocity": -0.09737842954215001,
+            "velocityX": -3.125586659329227,
+            "velocityY": -2.059697798242895,
+            "timestamp": 1.7899579876122462
+        },
+        {
+            "x": 6.480451878153193,
+            "y": 1.646736643588724,
+            "heading": -0.268441983388845,
+            "angularVelocity": -0.16928620019223647,
+            "velocityX": -3.3322654479868237,
+            "velocityY": -1.6398122111953808,
+            "timestamp": 1.846137205502623
+        },
+        {
+            "x": 6.284980922842322,
+            "y": 1.5795583069683912,
+            "heading": -0.28291661109989463,
+            "angularVelocity": -0.25765092955355146,
+            "velocityX": -3.47941752575298,
+            "velocityY": -1.1957862558966221,
+            "timestamp": 1.9023164233929999
+        },
+        {
+            "x": 6.086191677834544,
+            "y": 1.5383458560749974,
+            "heading": -0.3070406119029933,
+            "angularVelocity": -0.429411474723066,
+            "velocityX": -3.53848366838567,
+            "velocityY": -0.7335889042423714,
+            "timestamp": 1.9584956412833767
+        },
+        {
+            "x": 5.891572555778559,
+            "y": 1.5225106097486452,
+            "heading": -0.35143250880916643,
+            "angularVelocity": -0.7901836047765775,
+            "velocityX": -3.4642547433776456,
+            "velocityY": -0.2818701811985366,
+            "timestamp": 2.0146748591737533
+        },
+        {
+            "x": 5.713348199791213,
+            "y": 1.5270062964148978,
+            "heading": -0.407385332387855,
+            "angularVelocity": -0.9959701412692558,
+            "velocityX": -3.172425011952234,
+            "velocityY": 0.08002401662169539,
+            "timestamp": 2.07085407706413
+        },
+        {
+            "x": 5.558872974287929,
+            "y": 1.5428236796222032,
+            "heading": -0.4653888399418059,
+            "angularVelocity": -1.0324726781908529,
+            "velocityX": -2.7496862951120815,
+            "velocityY": 0.2815522145247918,
+            "timestamp": 2.1270332949545065
+        },
+        {
+            "x": 5.4303297125098124,
+            "y": 1.5629685460150702,
+            "heading": -0.519265179295127,
+            "angularVelocity": -0.9590083553396718,
+            "velocityX": -2.2880927610801525,
+            "velocityY": 0.35858217948453525,
+            "timestamp": 2.183212512844883
+        },
+        {
+            "x": 5.3280145107455015,
+            "y": 1.583083454211334,
+            "heading": -0.5654730488836437,
+            "angularVelocity": -0.8225082392332849,
+            "velocityX": -1.8212286608183241,
+            "velocityY": 0.3580489182942135,
+            "timestamp": 2.2393917307352598
+        },
+        {
+            "x": 5.251723840078425,
+            "y": 1.6004232571954151,
+            "heading": -0.6018676415285119,
+            "angularVelocity": -0.6478301765590008,
+            "velocityX": -1.3579874112157035,
+            "velocityY": 0.3086515554188953,
+            "timestamp": 2.2955709486256364
+        },
+        {
+            "x": 5.20115218978415,
+            "y": 1.613158164546165,
+            "heading": -0.6270407807035692,
+            "angularVelocity": -0.44808632302748147,
+            "velocityX": -0.9001843064628601,
+            "velocityY": 0.22668359989631479,
+            "timestamp": 2.351750166516013
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -0.2306763921441399,
+            "velocityX": -0.44771342016952703,
+            "velocityY": 0.12178587938311458,
+            "timestamp": 2.4079293844063896
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 3.4924263364045203e-31,
+            "velocityX": 0,
+            "velocityY": 4.738614736798697e-32,
+            "timestamp": 2.464108602296766
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.1.traj
index c4e0d8b5..1a381a4d 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.1.traj	
@@ -4,199 +4,199 @@
             "x": 7.68,
             "y": 7.467,
             "heading": 0,
-            "angularVelocity": -1.8065144428954335e-34,
-            "velocityX": -4.551747158955398e-35,
-            "velocityY": 1.585845739465702e-34,
+            "angularVelocity": 0,
+            "velocityX": 3.136513796099116e-33,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.613270090763082,
-            "y": 7.458186379338955,
-            "heading": 0.02268667453207945,
-            "angularVelocity": 0.251947675366546,
-            "velocityX": -0.7410713935129049,
-            "velocityY": -0.09787997945546167,
-            "timestamp": 0.09004518298918118
-        },
-        {
-            "x": 7.479732324735008,
-            "y": 7.440537819966174,
-            "heading": 0.06475236187583014,
-            "angularVelocity": 0.4671619952041724,
-            "velocityX": -1.4830084363770935,
-            "velocityY": -0.19599670728529434,
-            "timestamp": 0.18009036597836237
-        },
-        {
-            "x": 7.279281674321286,
-            "y": 7.414023565204846,
-            "heading": 0.12053400112707001,
-            "angularVelocity": 0.6194849896406114,
-            "velocityX": -2.226111866948016,
-            "velocityY": -0.2944550044894082,
-            "timestamp": 0.27013554896754355
-        },
-        {
-            "x": 7.011816314313711,
-            "y": 7.378595756973036,
-            "heading": 0.17807802747436277,
-            "angularVelocity": 0.63905724256462,
-            "velocityX": -2.970346121010285,
-            "velocityY": -0.39344479133399113,
-            "timestamp": 0.36018073195672473
-        },
-        {
-            "x": 6.678420949429204,
-            "y": 7.334257918470772,
-            "heading": 0.1920910018934364,
-            "angularVelocity": 0.15562158856134758,
-            "velocityX": -3.702534148046146,
-            "velocityY": -0.4923954511546947,
-            "timestamp": 0.4502259149459059
-        },
-        {
-            "x": 6.340697263329751,
-            "y": 7.289813527368774,
-            "heading": 0.19209102131154623,
-            "angularVelocity": 2.156485134469417e-7,
-            "velocityX": -3.7506024740938,
-            "velocityY": -0.49357877486170487,
-            "timestamp": 0.5402710979350871
-        },
-        {
-            "x": 6.002973586353306,
-            "y": 7.245369066942952,
-            "heading": 0.1920910407296517,
-            "angularVelocity": 2.1564846490105217e-7,
-            "velocityX": -3.7506023727779296,
-            "velocityY": -0.49357954473991933,
-            "timestamp": 0.6303162809242683
-        },
-        {
-            "x": 5.665249920039304,
-            "y": 7.200924525495619,
-            "heading": 0.1920910601477644,
-            "angularVelocity": 2.156485448418072e-7,
-            "velocityX": -3.750602254365785,
-            "velocityY": -0.4935804445271851,
-            "timestamp": 0.7203614639134495
-        },
-        {
-            "x": 5.327526265186639,
-            "y": 7.156479896956337,
-            "heading": 0.1920910795658876,
-            "angularVelocity": 2.1564866168111011e-7,
-            "velocityX": -3.7506021270814847,
-            "velocityY": -0.4935814117299572,
-            "timestamp": 0.8104066469026306
-        },
-        {
-            "x": 4.989802622149171,
-            "y": 7.1120351786364,
-            "heading": 0.19209109898401447,
-            "angularVelocity": 2.1564870237166035e-7,
-            "velocityX": -3.75060199586739,
-            "velocityY": -0.4935824087922295,
-            "timestamp": 0.9004518298918118
-        },
-        {
-            "x": 4.652078990957916,
-            "y": 7.067590370300322,
-            "heading": 0.19209111840213633,
-            "angularVelocity": 2.156486466006611e-7,
-            "velocityX": -3.75060186430886,
-            "velocityY": -0.49358340846970244,
-            "timestamp": 0.990497012880993
-        },
-        {
-            "x": 4.314355371293917,
-            "y": 7.023145474371938,
-            "heading": 0.19209113782025056,
-            "angularVelocity": 2.156485622267904e-7,
-            "velocityX": -3.7506017362924924,
-            "velocityY": -0.4935843812291933,
-            "timestamp": 1.0805421958701742
-        },
-        {
-            "x": 3.97663176247589,
-            "y": 6.97870049602832,
-            "heading": 0.19209115723836015,
-            "angularVelocity": 2.1564851021035735e-7,
-            "velocityX": -3.7506016158421813,
-            "velocityY": -0.4935852964945219,
-            "timestamp": 1.1705873788593553
-        },
-        {
-            "x": 3.638908163731045,
-            "y": 6.934255441141791,
-            "heading": 0.1920911766564701,
-            "angularVelocity": 2.1564851460850062e-7,
-            "velocityX": -3.7506015039740865,
-            "velocityY": -0.49358614654454386,
-            "timestamp": 1.2606325618485363
-        },
-        {
-            "x": 3.301184574635452,
-            "y": 6.88981031293383,
-            "heading": 0.1920911960745901,
-            "angularVelocity": 2.156486262427343e-7,
-            "velocityX": -3.750601396813981,
-            "velocityY": -0.49358696081834263,
-            "timestamp": 1.3506777448377174
-        },
-        {
-            "x": 2.967975728993463,
-            "y": 6.845441681932465,
-            "heading": 0.20651305517000687,
-            "angularVelocity": 0.16016247195754518,
-            "velocityX": -3.70046275192781,
-            "velocityY": -0.49273741835468604,
-            "timestamp": 1.4407229278268985
-        },
-        {
-            "x": 2.7005739783902083,
-            "y": 6.810001707269663,
-            "heading": 0.26681564809465086,
-            "angularVelocity": 0.6696926023448618,
-            "velocityX": -2.969639704495717,
-            "velocityY": -0.3935799060684999,
-            "timestamp": 1.5307681108160796
-        },
-        {
-            "x": 2.5001863972917366,
-            "y": 6.7834775390625595,
-            "heading": 0.32524314681975514,
-            "angularVelocity": 0.6488686766523009,
-            "velocityX": -2.2254114484119376,
-            "velocityY": -0.2945650986159953,
-            "timestamp": 1.6208132938052606
-        },
-        {
-            "x": 2.3667002159439683,
-            "y": 6.765819938844314,
-            "heading": 0.3692681668658828,
-            "angularVelocity": 0.4889214346026397,
-            "velocityX": -1.482435560865111,
-            "velocityY": -0.1960971107179338,
-            "timestamp": 1.7108584767944417
+            "x": 7.613270105003341,
+            "y": 7.458186274184093,
+            "heading": 0.022686687743237347,
+            "angularVelocity": 0.25194782208363986,
+            "velocityX": -0.7410712353675238,
+            "velocityY": -0.0978811472565877,
+            "timestamp": 0.09004518298914281
+        },
+        {
+            "x": 7.479732366880497,
+            "y": 7.440537508755927,
+            "heading": 0.06475240251532689,
+            "angularVelocity": 0.4671622998107697,
+            "velocityX": -1.483008126475186,
+            "velocityY": -0.19599899564082535,
+            "timestamp": 0.18009036597828562
+        },
+        {
+            "x": 7.2792817573683966,
+            "y": 7.414022951557291,
+            "heading": 0.12053408331695856,
+            "angularVelocity": 0.6194854510802376,
+            "velocityX": -2.226111412714547,
+            "velocityY": -0.2944583632178676,
+            "timestamp": 0.2701355489674284
+        },
+        {
+            "x": 7.01181645042657,
+            "y": 7.378594749595234,
+            "heading": 0.17807816149187164,
+            "angularVelocity": 0.6390578181383835,
+            "velocityX": -2.9703455316879728,
+            "velocityY": -0.39344916391950197,
+            "timestamp": 0.36018073195657124
+        },
+        {
+            "x": 6.678421151155383,
+            "y": 7.334256434894407,
+            "heading": 0.1920911587056688,
+            "angularVelocity": 0.15562184170903165,
+            "velocityX": -3.7025334193766524,
+            "velocityY": -0.49240073959507324,
+            "timestamp": 0.45022591494571407
+        },
+        {
+            "x": 6.340697514229505,
+            "y": 7.289811670135368,
+            "heading": 0.1920911781237931,
+            "angularVelocity": 2.1564867368321945e-7,
+            "velocityX": -3.7506019279965175,
+            "velocityY": -0.49358292452353475,
+            "timestamp": 0.5402710979348568
+        },
+        {
+            "x": 6.002973877037569,
+            "y": 7.245366907398008,
+            "heading": 0.19209119754191162,
+            "angularVelocity": 2.1564861007875057e-7,
+            "velocityX": -3.750601930951228,
+            "velocityY": -0.49358290207171196,
+            "timestamp": 0.6303162809239996
+        },
+        {
+            "x": 5.665250239845632,
+            "y": 7.200922144660658,
+            "heading": 0.19209121696003015,
+            "angularVelocity": 2.1564860966767442e-7,
+            "velocityX": -3.7506019309512406,
+            "velocityY": -0.493582902071613,
+            "timestamp": 0.7203614639131424
+        },
+        {
+            "x": 5.327526602653695,
+            "y": 7.156477381923307,
+            "heading": 0.19209123637814873,
+            "angularVelocity": 2.1564861026621434e-7,
+            "velocityX": -3.7506019309512415,
+            "velocityY": -0.493582902071613,
+            "timestamp": 0.8104066469022851
+        },
+        {
+            "x": 4.989802965461758,
+            "y": 7.112032619185957,
+            "heading": 0.19209125579626718,
+            "angularVelocity": 2.156486089426075e-7,
+            "velocityX": -3.7506019309512415,
+            "velocityY": -0.49358290207161315,
+            "timestamp": 0.9004518298914279
+        },
+        {
+            "x": 4.652079328269822,
+            "y": 7.067587856448606,
+            "heading": 0.19209127521438563,
+            "angularVelocity": 2.156486089612396e-7,
+            "velocityX": -3.750601930951241,
+            "velocityY": -0.4935829020716131,
+            "timestamp": 0.9904970128805707
+        },
+        {
+            "x": 4.314355691077885,
+            "y": 7.023143093711255,
+            "heading": 0.19209129463250418,
+            "angularVelocity": 2.1564860999442945e-7,
+            "velocityX": -3.7506019309512415,
+            "velocityY": -0.4935829020716131,
+            "timestamp": 1.0805421958697135
+        },
+        {
+            "x": 3.976632053885947,
+            "y": 6.978698330973905,
+            "heading": 0.1920913140506227,
+            "angularVelocity": 2.1564860984546372e-7,
+            "velocityX": -3.750601930951241,
+            "velocityY": -0.49358290207161315,
+            "timestamp": 1.1705873788588563
+        },
+        {
+            "x": 3.638908416694012,
+            "y": 6.934253568236545,
+            "heading": 0.19209133346874124,
+            "angularVelocity": 2.1564860987115903e-7,
+            "velocityX": -3.750601930951227,
+            "velocityY": -0.4935829020717237,
+            "timestamp": 1.2606325618479992
+        },
+        {
+            "x": 3.3011847797995815,
+            "y": 6.8898088032385445,
+            "heading": 0.19209135288686577,
+            "angularVelocity": 2.1564867640744468e-7,
+            "velocityX": -3.7506019276472724,
+            "velocityY": -0.4935829271773418,
+            "timestamp": 1.350677744837142
+        },
+        {
+            "x": 2.967975868183031,
+            "y": 6.845440652800829,
+            "heading": 0.2065131865620181,
+            "angularVelocity": 0.16016218965195775,
+            "velocityX": -3.7004634846122446,
+            "velocityY": -0.492732081438091,
+            "timestamp": 1.440722927826285
+        },
+        {
+            "x": 2.7005740636525677,
+            "y": 6.810001078107782,
+            "heading": 0.266815731322117,
+            "angularVelocity": 0.6696920674520688,
+            "velocityX": -2.9696403033875236,
+            "velocityY": -0.39357546419024264,
+            "timestamp": 1.5307681108154279
+        },
+        {
+            "x": 2.5001864407535797,
+            "y": 6.78347721885534,
+            "heading": 0.32524318999567287,
+            "angularVelocity": 0.6488682318586739,
+            "velocityX": -2.2254119126300136,
+            "velocityY": -0.29456166750908425,
+            "timestamp": 1.6208132938045707
+        },
+        {
+            "x": 2.366700230698382,
+            "y": 6.765819830273621,
+            "heading": 0.3692681816828941,
+            "angularVelocity": 0.48892111966199864,
+            "velocityX": -1.4824358796771253,
+            "velocityY": -0.19609476038099796,
+            "timestamp": 1.7108584767937136
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.393,
-            "angularVelocity": 0.2635547215998056,
-            "velocityX": -0.7407416335861364,
-            "velocityY": -0.09795014626572365,
-            "timestamp": 1.8009036597836228
+            "angularVelocity": 0.26355455704906927,
+            "velocityX": -0.740741797442122,
+            "velocityY": -0.09794894053004785,
+            "timestamp": 1.8009036597828565
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.393,
-            "angularVelocity": 9.742765950850105e-34,
-            "velocityX": 0,
-            "velocityY": -8.251938001070805e-34,
-            "timestamp": 1.8909488427728038
+            "angularVelocity": 0,
+            "velocityX": 6.927431243507319e-35,
+            "velocityY": 0,
+            "timestamp": 1.8909488427719994
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.traj
index c4e0d8b5..1a381a4d 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.traj	
@@ -4,199 +4,199 @@
             "x": 7.68,
             "y": 7.467,
             "heading": 0,
-            "angularVelocity": -1.8065144428954335e-34,
-            "velocityX": -4.551747158955398e-35,
-            "velocityY": 1.585845739465702e-34,
+            "angularVelocity": 0,
+            "velocityX": 3.136513796099116e-33,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.613270090763082,
-            "y": 7.458186379338955,
-            "heading": 0.02268667453207945,
-            "angularVelocity": 0.251947675366546,
-            "velocityX": -0.7410713935129049,
-            "velocityY": -0.09787997945546167,
-            "timestamp": 0.09004518298918118
-        },
-        {
-            "x": 7.479732324735008,
-            "y": 7.440537819966174,
-            "heading": 0.06475236187583014,
-            "angularVelocity": 0.4671619952041724,
-            "velocityX": -1.4830084363770935,
-            "velocityY": -0.19599670728529434,
-            "timestamp": 0.18009036597836237
-        },
-        {
-            "x": 7.279281674321286,
-            "y": 7.414023565204846,
-            "heading": 0.12053400112707001,
-            "angularVelocity": 0.6194849896406114,
-            "velocityX": -2.226111866948016,
-            "velocityY": -0.2944550044894082,
-            "timestamp": 0.27013554896754355
-        },
-        {
-            "x": 7.011816314313711,
-            "y": 7.378595756973036,
-            "heading": 0.17807802747436277,
-            "angularVelocity": 0.63905724256462,
-            "velocityX": -2.970346121010285,
-            "velocityY": -0.39344479133399113,
-            "timestamp": 0.36018073195672473
-        },
-        {
-            "x": 6.678420949429204,
-            "y": 7.334257918470772,
-            "heading": 0.1920910018934364,
-            "angularVelocity": 0.15562158856134758,
-            "velocityX": -3.702534148046146,
-            "velocityY": -0.4923954511546947,
-            "timestamp": 0.4502259149459059
-        },
-        {
-            "x": 6.340697263329751,
-            "y": 7.289813527368774,
-            "heading": 0.19209102131154623,
-            "angularVelocity": 2.156485134469417e-7,
-            "velocityX": -3.7506024740938,
-            "velocityY": -0.49357877486170487,
-            "timestamp": 0.5402710979350871
-        },
-        {
-            "x": 6.002973586353306,
-            "y": 7.245369066942952,
-            "heading": 0.1920910407296517,
-            "angularVelocity": 2.1564846490105217e-7,
-            "velocityX": -3.7506023727779296,
-            "velocityY": -0.49357954473991933,
-            "timestamp": 0.6303162809242683
-        },
-        {
-            "x": 5.665249920039304,
-            "y": 7.200924525495619,
-            "heading": 0.1920910601477644,
-            "angularVelocity": 2.156485448418072e-7,
-            "velocityX": -3.750602254365785,
-            "velocityY": -0.4935804445271851,
-            "timestamp": 0.7203614639134495
-        },
-        {
-            "x": 5.327526265186639,
-            "y": 7.156479896956337,
-            "heading": 0.1920910795658876,
-            "angularVelocity": 2.1564866168111011e-7,
-            "velocityX": -3.7506021270814847,
-            "velocityY": -0.4935814117299572,
-            "timestamp": 0.8104066469026306
-        },
-        {
-            "x": 4.989802622149171,
-            "y": 7.1120351786364,
-            "heading": 0.19209109898401447,
-            "angularVelocity": 2.1564870237166035e-7,
-            "velocityX": -3.75060199586739,
-            "velocityY": -0.4935824087922295,
-            "timestamp": 0.9004518298918118
-        },
-        {
-            "x": 4.652078990957916,
-            "y": 7.067590370300322,
-            "heading": 0.19209111840213633,
-            "angularVelocity": 2.156486466006611e-7,
-            "velocityX": -3.75060186430886,
-            "velocityY": -0.49358340846970244,
-            "timestamp": 0.990497012880993
-        },
-        {
-            "x": 4.314355371293917,
-            "y": 7.023145474371938,
-            "heading": 0.19209113782025056,
-            "angularVelocity": 2.156485622267904e-7,
-            "velocityX": -3.7506017362924924,
-            "velocityY": -0.4935843812291933,
-            "timestamp": 1.0805421958701742
-        },
-        {
-            "x": 3.97663176247589,
-            "y": 6.97870049602832,
-            "heading": 0.19209115723836015,
-            "angularVelocity": 2.1564851021035735e-7,
-            "velocityX": -3.7506016158421813,
-            "velocityY": -0.4935852964945219,
-            "timestamp": 1.1705873788593553
-        },
-        {
-            "x": 3.638908163731045,
-            "y": 6.934255441141791,
-            "heading": 0.1920911766564701,
-            "angularVelocity": 2.1564851460850062e-7,
-            "velocityX": -3.7506015039740865,
-            "velocityY": -0.49358614654454386,
-            "timestamp": 1.2606325618485363
-        },
-        {
-            "x": 3.301184574635452,
-            "y": 6.88981031293383,
-            "heading": 0.1920911960745901,
-            "angularVelocity": 2.156486262427343e-7,
-            "velocityX": -3.750601396813981,
-            "velocityY": -0.49358696081834263,
-            "timestamp": 1.3506777448377174
-        },
-        {
-            "x": 2.967975728993463,
-            "y": 6.845441681932465,
-            "heading": 0.20651305517000687,
-            "angularVelocity": 0.16016247195754518,
-            "velocityX": -3.70046275192781,
-            "velocityY": -0.49273741835468604,
-            "timestamp": 1.4407229278268985
-        },
-        {
-            "x": 2.7005739783902083,
-            "y": 6.810001707269663,
-            "heading": 0.26681564809465086,
-            "angularVelocity": 0.6696926023448618,
-            "velocityX": -2.969639704495717,
-            "velocityY": -0.3935799060684999,
-            "timestamp": 1.5307681108160796
-        },
-        {
-            "x": 2.5001863972917366,
-            "y": 6.7834775390625595,
-            "heading": 0.32524314681975514,
-            "angularVelocity": 0.6488686766523009,
-            "velocityX": -2.2254114484119376,
-            "velocityY": -0.2945650986159953,
-            "timestamp": 1.6208132938052606
-        },
-        {
-            "x": 2.3667002159439683,
-            "y": 6.765819938844314,
-            "heading": 0.3692681668658828,
-            "angularVelocity": 0.4889214346026397,
-            "velocityX": -1.482435560865111,
-            "velocityY": -0.1960971107179338,
-            "timestamp": 1.7108584767944417
+            "x": 7.613270105003341,
+            "y": 7.458186274184093,
+            "heading": 0.022686687743237347,
+            "angularVelocity": 0.25194782208363986,
+            "velocityX": -0.7410712353675238,
+            "velocityY": -0.0978811472565877,
+            "timestamp": 0.09004518298914281
+        },
+        {
+            "x": 7.479732366880497,
+            "y": 7.440537508755927,
+            "heading": 0.06475240251532689,
+            "angularVelocity": 0.4671622998107697,
+            "velocityX": -1.483008126475186,
+            "velocityY": -0.19599899564082535,
+            "timestamp": 0.18009036597828562
+        },
+        {
+            "x": 7.2792817573683966,
+            "y": 7.414022951557291,
+            "heading": 0.12053408331695856,
+            "angularVelocity": 0.6194854510802376,
+            "velocityX": -2.226111412714547,
+            "velocityY": -0.2944583632178676,
+            "timestamp": 0.2701355489674284
+        },
+        {
+            "x": 7.01181645042657,
+            "y": 7.378594749595234,
+            "heading": 0.17807816149187164,
+            "angularVelocity": 0.6390578181383835,
+            "velocityX": -2.9703455316879728,
+            "velocityY": -0.39344916391950197,
+            "timestamp": 0.36018073195657124
+        },
+        {
+            "x": 6.678421151155383,
+            "y": 7.334256434894407,
+            "heading": 0.1920911587056688,
+            "angularVelocity": 0.15562184170903165,
+            "velocityX": -3.7025334193766524,
+            "velocityY": -0.49240073959507324,
+            "timestamp": 0.45022591494571407
+        },
+        {
+            "x": 6.340697514229505,
+            "y": 7.289811670135368,
+            "heading": 0.1920911781237931,
+            "angularVelocity": 2.1564867368321945e-7,
+            "velocityX": -3.7506019279965175,
+            "velocityY": -0.49358292452353475,
+            "timestamp": 0.5402710979348568
+        },
+        {
+            "x": 6.002973877037569,
+            "y": 7.245366907398008,
+            "heading": 0.19209119754191162,
+            "angularVelocity": 2.1564861007875057e-7,
+            "velocityX": -3.750601930951228,
+            "velocityY": -0.49358290207171196,
+            "timestamp": 0.6303162809239996
+        },
+        {
+            "x": 5.665250239845632,
+            "y": 7.200922144660658,
+            "heading": 0.19209121696003015,
+            "angularVelocity": 2.1564860966767442e-7,
+            "velocityX": -3.7506019309512406,
+            "velocityY": -0.493582902071613,
+            "timestamp": 0.7203614639131424
+        },
+        {
+            "x": 5.327526602653695,
+            "y": 7.156477381923307,
+            "heading": 0.19209123637814873,
+            "angularVelocity": 2.1564861026621434e-7,
+            "velocityX": -3.7506019309512415,
+            "velocityY": -0.493582902071613,
+            "timestamp": 0.8104066469022851
+        },
+        {
+            "x": 4.989802965461758,
+            "y": 7.112032619185957,
+            "heading": 0.19209125579626718,
+            "angularVelocity": 2.156486089426075e-7,
+            "velocityX": -3.7506019309512415,
+            "velocityY": -0.49358290207161315,
+            "timestamp": 0.9004518298914279
+        },
+        {
+            "x": 4.652079328269822,
+            "y": 7.067587856448606,
+            "heading": 0.19209127521438563,
+            "angularVelocity": 2.156486089612396e-7,
+            "velocityX": -3.750601930951241,
+            "velocityY": -0.4935829020716131,
+            "timestamp": 0.9904970128805707
+        },
+        {
+            "x": 4.314355691077885,
+            "y": 7.023143093711255,
+            "heading": 0.19209129463250418,
+            "angularVelocity": 2.1564860999442945e-7,
+            "velocityX": -3.7506019309512415,
+            "velocityY": -0.4935829020716131,
+            "timestamp": 1.0805421958697135
+        },
+        {
+            "x": 3.976632053885947,
+            "y": 6.978698330973905,
+            "heading": 0.1920913140506227,
+            "angularVelocity": 2.1564860984546372e-7,
+            "velocityX": -3.750601930951241,
+            "velocityY": -0.49358290207161315,
+            "timestamp": 1.1705873788588563
+        },
+        {
+            "x": 3.638908416694012,
+            "y": 6.934253568236545,
+            "heading": 0.19209133346874124,
+            "angularVelocity": 2.1564860987115903e-7,
+            "velocityX": -3.750601930951227,
+            "velocityY": -0.4935829020717237,
+            "timestamp": 1.2606325618479992
+        },
+        {
+            "x": 3.3011847797995815,
+            "y": 6.8898088032385445,
+            "heading": 0.19209135288686577,
+            "angularVelocity": 2.1564867640744468e-7,
+            "velocityX": -3.7506019276472724,
+            "velocityY": -0.4935829271773418,
+            "timestamp": 1.350677744837142
+        },
+        {
+            "x": 2.967975868183031,
+            "y": 6.845440652800829,
+            "heading": 0.2065131865620181,
+            "angularVelocity": 0.16016218965195775,
+            "velocityX": -3.7004634846122446,
+            "velocityY": -0.492732081438091,
+            "timestamp": 1.440722927826285
+        },
+        {
+            "x": 2.7005740636525677,
+            "y": 6.810001078107782,
+            "heading": 0.266815731322117,
+            "angularVelocity": 0.6696920674520688,
+            "velocityX": -2.9696403033875236,
+            "velocityY": -0.39357546419024264,
+            "timestamp": 1.5307681108154279
+        },
+        {
+            "x": 2.5001864407535797,
+            "y": 6.78347721885534,
+            "heading": 0.32524318999567287,
+            "angularVelocity": 0.6488682318586739,
+            "velocityX": -2.2254119126300136,
+            "velocityY": -0.29456166750908425,
+            "timestamp": 1.6208132938045707
+        },
+        {
+            "x": 2.366700230698382,
+            "y": 6.765819830273621,
+            "heading": 0.3692681816828941,
+            "angularVelocity": 0.48892111966199864,
+            "velocityX": -1.4824358796771253,
+            "velocityY": -0.19609476038099796,
+            "timestamp": 1.7108584767937136
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.393,
-            "angularVelocity": 0.2635547215998056,
-            "velocityX": -0.7407416335861364,
-            "velocityY": -0.09795014626572365,
-            "timestamp": 1.8009036597836228
+            "angularVelocity": 0.26355455704906927,
+            "velocityX": -0.740741797442122,
+            "velocityY": -0.09794894053004785,
+            "timestamp": 1.8009036597828565
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.393,
-            "angularVelocity": 9.742765950850105e-34,
-            "velocityX": 0,
-            "velocityY": -8.251938001070805e-34,
-            "timestamp": 1.8909488427728038
+            "angularVelocity": 0,
+            "velocityX": 6.927431243507319e-35,
+            "velocityY": 0,
+            "timestamp": 1.8909488427719994
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.1.traj
index 82d2bcc6..28a53b93 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.1.traj	
@@ -4,190 +4,190 @@
             "x": 7.68,
             "y": 7.467,
             "heading": 0,
-            "angularVelocity": 1.1003498306069892e-28,
-            "velocityX": -2.6796850098783343e-31,
-            "velocityY": 7.1039345927107565e-31,
+            "angularVelocity": 0,
+            "velocityX": -2.4374604803646449e-32,
+            "velocityY": 2.350988701644575e-38,
             "timestamp": 0
         },
         {
-            "x": 7.602663557475489,
-            "y": 7.440503469641943,
-            "heading": 0.0012193469853657303,
-            "angularVelocity": 0.01231450998750236,
-            "velocityX": -0.781041332205243,
-            "velocityY": -0.26759551763859557,
-            "timestamp": 0.09901709337987814
-        },
-        {
-            "x": 7.447990676613656,
-            "y": 7.387510406307137,
-            "heading": 0.00365804177605024,
-            "angularVelocity": 0.024629028255928873,
-            "velocityX": -1.562082622122956,
-            "velocityY": -0.5351910617240387,
-            "timestamp": 0.1980341867597563
-        },
-        {
-            "x": 7.2159813646219675,
-            "y": 7.3080208081596165,
-            "heading": 0.0073160847395553,
-            "angularVelocity": 0.03694355023585351,
-            "velocityX": -2.3431238392505196,
-            "velocityY": -0.8027866243514209,
-            "timestamp": 0.29705128013963444
-        },
-        {
-            "x": 6.906635640594062,
-            "y": 7.202034677060231,
-            "heading": 0.012193473491592185,
-            "angularVelocity": 0.049258048136210826,
-            "velocityX": -3.1241648635463743,
-            "velocityY": -1.0703821681855634,
-            "timestamp": 0.3960683735195126
-        },
-        {
-            "x": 6.5522807849123526,
-            "y": 7.080627711247213,
-            "heading": 0.012193474015757445,
-            "angularVelocity": 5.293684623965923e-9,
-            "velocityX": -3.5787240726429963,
-            "velocityY": -1.2261212854152546,
-            "timestamp": 0.4950854668993907
-        },
-        {
-            "x": 6.197925949682556,
-            "y": 6.959220685740379,
-            "heading": 0.012193474539927869,
-            "angularVelocity": 5.293736792766535e-9,
-            "velocityX": -3.578723866093675,
-            "velocityY": -1.2261218882789995,
-            "timestamp": 0.5941025602792689
-        },
-        {
-            "x": 5.843571141718115,
-            "y": 6.8378135806532265,
-            "heading": 0.012193475064112974,
-            "angularVelocity": 5.2938850654703056e-9,
-            "velocityX": -3.578723590733579,
-            "velocityY": -1.2261226919818409,
-            "timestamp": 0.6931196536591471
-        },
-        {
-            "x": 5.489216369151057,
-            "y": 6.716406372250631,
-            "heading": 0.012193475588313017,
-            "angularVelocity": 5.294035915259789e-9,
-            "velocityX": -3.578723233245997,
-            "velocityY": -1.2261237353920091,
-            "timestamp": 0.7921367470390253
-        },
-        {
-            "x": 5.134861642331667,
-            "y": 6.594999030323008,
-            "heading": 0.012193476112502804,
-            "angularVelocity": 5.293932348218223e-9,
-            "velocityX": -3.5787227712281067,
-            "velocityY": -1.2261250838968347,
-            "timestamp": 0.8911538404189034
-        },
-        {
-            "x": 4.780507205250472,
-            "y": 6.473590842716537,
-            "heading": 0.012193476636274196,
-            "angularVelocity": 5.2897066685502164e-9,
-            "velocityX": -3.578719845084944,
-            "velocityY": -1.2261336246326482,
-            "timestamp": 0.9901709337987816
-        },
-        {
-            "x": 4.428189328838181,
-            "y": 6.346393637776016,
-            "heading": 0.012193476110055806,
-            "angularVelocity": -5.314419742287748e-9,
-            "velocityX": -3.558152076436196,
-            "velocityY": -1.2845984526381864,
-            "timestamp": 1.0891880271786598
-        },
-        {
-            "x": 4.075871503880035,
-            "y": 6.219196290315138,
-            "heading": 0.012193475583846744,
-            "angularVelocity": -5.314325547246037e-9,
-            "velocityX": -3.5581515567870925,
-            "velocityY": -1.2845998919892312,
-            "timestamp": 1.188205120558538
-        },
-        {
-            "x": 3.723553713185669,
-            "y": 6.091998847948706,
-            "heading": 0.012193475057649396,
-            "angularVelocity": -5.314207229499686e-9,
-            "velocityX": -3.5581512107480435,
-            "velocityY": -1.2846008504657016,
-            "timestamp": 1.2872222139384162
-        },
-        {
-            "x": 3.3712359540403063,
-            "y": 5.96480131819632,
-            "heading": 0.012193474531462604,
-            "angularVelocity": -5.314100634839972e-9,
-            "velocityX": -3.5581508921262577,
-            "velocityY": -1.284601732999717,
-            "timestamp": 1.3862393073182944
-        },
-        {
-            "x": 3.018918225427334,
-            "y": 5.837603703873982,
-            "heading": 0.012193474005277722,
-            "angularVelocity": -5.314081331919232e-9,
-            "velocityX": -3.558150583771516,
-            "velocityY": -1.2846025870942077,
-            "timestamp": 1.4852564006981726
-        },
-        {
-            "x": 2.7113509459543246,
-            "y": 5.726562245993832,
-            "heading": 0.0073160988837450616,
-            "angularVelocity": -0.04925791047811804,
-            "velocityX": -3.1062038782842247,
-            "velocityY": -1.121437259869279,
-            "timestamp": 1.5842734940780507
-        },
-        {
-            "x": 2.48067547425723,
-            "y": 5.6432811332300385,
-            "heading": 0.003658054936391331,
-            "angularVelocity": -0.03694356017200323,
-            "velocityX": -2.329653030836908,
-            "velocityY": -0.8410781403599302,
-            "timestamp": 1.683290587457929
-        },
-        {
-            "x": 2.326891824704753,
-            "y": 5.5877603809363166,
-            "heading": 0.0012193531500403563,
-            "angularVelocity": -0.024629098907027813,
-            "velocityX": -1.5531020382762346,
-            "velocityY": -0.560718865789336,
-            "timestamp": 1.782307680837807
+            "x": 7.602872461914855,
+            "y": 7.439913035887961,
+            "heading": 5.3325436298288445e-18,
+            "angularVelocity": 5.3856284215509377e-17,
+            "velocityX": -0.7789533333445217,
+            "velocityY": -0.2735661154121556,
+            "timestamp": 0.09901432445765257
+        },
+        {
+            "x": 7.448617388685645,
+            "y": 7.385739108696781,
+            "heading": -4.4022234029758545e-18,
+            "angularVelocity": -9.831675377471393e-17,
+            "velocityX": -1.557906636985473,
+            "velocityY": -0.5471322203925048,
+            "timestamp": 0.19802864891530514
+        },
+        {
+            "x": 7.217234786194527,
+            "y": 7.304478220492258,
+            "heading": -7.053417683545123e-18,
+            "angularVelocity": -2.6775864730895626e-17,
+            "velocityX": -2.3368598812192953,
+            "velocityY": -0.8206983045092451,
+            "timestamp": 0.2970429733729577
+        },
+        {
+            "x": 6.9087246720879545,
+            "y": 7.196130377471774,
+            "heading": 2.6673179344101e-18,
+            "angularVelocity": 9.817504407722252e-17,
+            "velocityX": -3.1158129472318725,
+            "velocityY": -1.0942643260352094,
+            "timestamp": 0.3960572978306103
+        },
+        {
+            "x": 6.5553201862538,
+            "y": 7.072015763386003,
+            "heading": 8.41895708879283e-18,
+            "angularVelocity": 5.808896021749495e-17,
+            "velocityX": -3.5692258445423475,
+            "velocityY": -1.2535015995473784,
+            "timestamp": 0.49507162228826285
+        },
+        {
+            "x": 6.2019157004196215,
+            "y": 6.9479011493002245,
+            "heading": -3.69857396739816e-19,
+            "angularVelocity": -8.876306064099344e-17,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.2535015995474599,
+            "timestamp": 0.5940859467459154
+        },
+        {
+            "x": 5.848511214585444,
+            "y": 6.823786535214446,
+            "heading": -8.032367976884792e-18,
+            "angularVelocity": -7.738789939855791e-17,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.2535015995474599,
+            "timestamp": 0.693100271203568
+        },
+        {
+            "x": 5.495106728751266,
+            "y": 6.699671921128667,
+            "heading": -3.490709714785344e-17,
+            "angularVelocity": -2.714226382747745e-16,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.2535015995474599,
+            "timestamp": 0.7921145956612206
+        },
+        {
+            "x": 5.141702242917089,
+            "y": 6.575557307042889,
+            "heading": -1.2741337636064699e-17,
+            "angularVelocity": 2.238641694845762e-16,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.2535015995474599,
+            "timestamp": 0.8911289201188731
+        },
+        {
+            "x": 4.788297757082911,
+            "y": 6.4514426929571105,
+            "heading": -2.2645871045802615e-17,
+            "angularVelocity": -1.0003131833516608e-16,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.2535015995474597,
+            "timestamp": 0.9901432445765257
+        },
+        {
+            "x": 4.4348932712487334,
+            "y": 6.327328078871332,
+            "heading": -7.505924377434873e-18,
+            "angularVelocity": 1.529066299375997e-16,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.25350159954746,
+            "timestamp": 1.0891575690341782
+        },
+        {
+            "x": 4.081488785414556,
+            "y": 6.203213464785553,
+            "heading": -3.3249179069580955e-17,
+            "angularVelocity": -2.5999525658031657e-16,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.25350159954746,
+            "timestamp": 1.1881718934918308
+        },
+        {
+            "x": 3.728084299580378,
+            "y": 6.079098850699775,
+            "heading": -3.31250701341244e-17,
+            "angularVelocity": 1.2534442479544336e-18,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.2535015995474597,
+            "timestamp": 1.2871862179494835
+        },
+        {
+            "x": 3.3746798137461993,
+            "y": 5.9549842366139965,
+            "heading": -3.474935359347948e-17,
+            "angularVelocity": -1.6404530025230332e-17,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.2535015995474599,
+            "timestamp": 1.3862005424071362
+        },
+        {
+            "x": 3.021275327912045,
+            "y": 5.830869622528225,
+            "heading": -2.4652412430964293e-17,
+            "angularVelocity": 1.0197454982216833e-16,
+            "velocityX": -3.5692258445423475,
+            "velocityY": -1.2535015995473784,
+            "timestamp": 1.4852148668647889
+        },
+        {
+            "x": 2.7127652138054734,
+            "y": 5.722521779507741,
+            "heading": 1.414690860074665e-18,
+            "angularVelocity": 2.632659789801892e-16,
+            "velocityX": -3.115812947231872,
+            "velocityY": -1.0942643260352094,
+            "timestamp": 1.5842291913224416
+        },
+        {
+            "x": 2.4813826113143547,
+            "y": 5.641260891303218,
+            "heading": 8.307803002216867e-18,
+            "angularVelocity": 6.961732115429119e-17,
+            "velocityX": -2.336859881219295,
+            "velocityY": -0.8206983045092451,
+            "timestamp": 1.6832435157800942
+        },
+        {
+            "x": 2.3271275380851444,
+            "y": 5.587086964112038,
+            "heading": -3.972937436005726e-18,
+            "angularVelocity": -1.2402993582700068e-16,
+            "velocityX": -1.557906636985473,
+            "velocityY": -0.5471322203925048,
+            "timestamp": 1.782257840237747
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": -2.184437138856228e-30,
-            "angularVelocity": -0.012314572246193395,
-            "velocityX": -0.7765510184161662,
-            "velocityY": -0.28035948126465987,
-            "timestamp": 1.8813247742176853
+            "heading": 0,
+            "angularVelocity": 4.012487544797503e-17,
+            "velocityX": -0.7789533333445219,
+            "velocityY": -0.2735661154121556,
+            "timestamp": 1.8812721646953996
         },
         {
             "x": 2.25,
             "y": 5.56,
             "heading": 0,
-            "angularVelocity": 2.219047153199805e-29,
-            "velocityX": 0,
-            "velocityY": -7.552577135588168e-32,
-            "timestamp": 1.9803418675975635
+            "angularVelocity": 0,
+            "velocityX": -5.265297803216144e-35,
+            "velocityY": 0,
+            "timestamp": 1.9802864891530523
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.traj
index 82d2bcc6..28a53b93 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.traj	
@@ -4,190 +4,190 @@
             "x": 7.68,
             "y": 7.467,
             "heading": 0,
-            "angularVelocity": 1.1003498306069892e-28,
-            "velocityX": -2.6796850098783343e-31,
-            "velocityY": 7.1039345927107565e-31,
+            "angularVelocity": 0,
+            "velocityX": -2.4374604803646449e-32,
+            "velocityY": 2.350988701644575e-38,
             "timestamp": 0
         },
         {
-            "x": 7.602663557475489,
-            "y": 7.440503469641943,
-            "heading": 0.0012193469853657303,
-            "angularVelocity": 0.01231450998750236,
-            "velocityX": -0.781041332205243,
-            "velocityY": -0.26759551763859557,
-            "timestamp": 0.09901709337987814
-        },
-        {
-            "x": 7.447990676613656,
-            "y": 7.387510406307137,
-            "heading": 0.00365804177605024,
-            "angularVelocity": 0.024629028255928873,
-            "velocityX": -1.562082622122956,
-            "velocityY": -0.5351910617240387,
-            "timestamp": 0.1980341867597563
-        },
-        {
-            "x": 7.2159813646219675,
-            "y": 7.3080208081596165,
-            "heading": 0.0073160847395553,
-            "angularVelocity": 0.03694355023585351,
-            "velocityX": -2.3431238392505196,
-            "velocityY": -0.8027866243514209,
-            "timestamp": 0.29705128013963444
-        },
-        {
-            "x": 6.906635640594062,
-            "y": 7.202034677060231,
-            "heading": 0.012193473491592185,
-            "angularVelocity": 0.049258048136210826,
-            "velocityX": -3.1241648635463743,
-            "velocityY": -1.0703821681855634,
-            "timestamp": 0.3960683735195126
-        },
-        {
-            "x": 6.5522807849123526,
-            "y": 7.080627711247213,
-            "heading": 0.012193474015757445,
-            "angularVelocity": 5.293684623965923e-9,
-            "velocityX": -3.5787240726429963,
-            "velocityY": -1.2261212854152546,
-            "timestamp": 0.4950854668993907
-        },
-        {
-            "x": 6.197925949682556,
-            "y": 6.959220685740379,
-            "heading": 0.012193474539927869,
-            "angularVelocity": 5.293736792766535e-9,
-            "velocityX": -3.578723866093675,
-            "velocityY": -1.2261218882789995,
-            "timestamp": 0.5941025602792689
-        },
-        {
-            "x": 5.843571141718115,
-            "y": 6.8378135806532265,
-            "heading": 0.012193475064112974,
-            "angularVelocity": 5.2938850654703056e-9,
-            "velocityX": -3.578723590733579,
-            "velocityY": -1.2261226919818409,
-            "timestamp": 0.6931196536591471
-        },
-        {
-            "x": 5.489216369151057,
-            "y": 6.716406372250631,
-            "heading": 0.012193475588313017,
-            "angularVelocity": 5.294035915259789e-9,
-            "velocityX": -3.578723233245997,
-            "velocityY": -1.2261237353920091,
-            "timestamp": 0.7921367470390253
-        },
-        {
-            "x": 5.134861642331667,
-            "y": 6.594999030323008,
-            "heading": 0.012193476112502804,
-            "angularVelocity": 5.293932348218223e-9,
-            "velocityX": -3.5787227712281067,
-            "velocityY": -1.2261250838968347,
-            "timestamp": 0.8911538404189034
-        },
-        {
-            "x": 4.780507205250472,
-            "y": 6.473590842716537,
-            "heading": 0.012193476636274196,
-            "angularVelocity": 5.2897066685502164e-9,
-            "velocityX": -3.578719845084944,
-            "velocityY": -1.2261336246326482,
-            "timestamp": 0.9901709337987816
-        },
-        {
-            "x": 4.428189328838181,
-            "y": 6.346393637776016,
-            "heading": 0.012193476110055806,
-            "angularVelocity": -5.314419742287748e-9,
-            "velocityX": -3.558152076436196,
-            "velocityY": -1.2845984526381864,
-            "timestamp": 1.0891880271786598
-        },
-        {
-            "x": 4.075871503880035,
-            "y": 6.219196290315138,
-            "heading": 0.012193475583846744,
-            "angularVelocity": -5.314325547246037e-9,
-            "velocityX": -3.5581515567870925,
-            "velocityY": -1.2845998919892312,
-            "timestamp": 1.188205120558538
-        },
-        {
-            "x": 3.723553713185669,
-            "y": 6.091998847948706,
-            "heading": 0.012193475057649396,
-            "angularVelocity": -5.314207229499686e-9,
-            "velocityX": -3.5581512107480435,
-            "velocityY": -1.2846008504657016,
-            "timestamp": 1.2872222139384162
-        },
-        {
-            "x": 3.3712359540403063,
-            "y": 5.96480131819632,
-            "heading": 0.012193474531462604,
-            "angularVelocity": -5.314100634839972e-9,
-            "velocityX": -3.5581508921262577,
-            "velocityY": -1.284601732999717,
-            "timestamp": 1.3862393073182944
-        },
-        {
-            "x": 3.018918225427334,
-            "y": 5.837603703873982,
-            "heading": 0.012193474005277722,
-            "angularVelocity": -5.314081331919232e-9,
-            "velocityX": -3.558150583771516,
-            "velocityY": -1.2846025870942077,
-            "timestamp": 1.4852564006981726
-        },
-        {
-            "x": 2.7113509459543246,
-            "y": 5.726562245993832,
-            "heading": 0.0073160988837450616,
-            "angularVelocity": -0.04925791047811804,
-            "velocityX": -3.1062038782842247,
-            "velocityY": -1.121437259869279,
-            "timestamp": 1.5842734940780507
-        },
-        {
-            "x": 2.48067547425723,
-            "y": 5.6432811332300385,
-            "heading": 0.003658054936391331,
-            "angularVelocity": -0.03694356017200323,
-            "velocityX": -2.329653030836908,
-            "velocityY": -0.8410781403599302,
-            "timestamp": 1.683290587457929
-        },
-        {
-            "x": 2.326891824704753,
-            "y": 5.5877603809363166,
-            "heading": 0.0012193531500403563,
-            "angularVelocity": -0.024629098907027813,
-            "velocityX": -1.5531020382762346,
-            "velocityY": -0.560718865789336,
-            "timestamp": 1.782307680837807
+            "x": 7.602872461914855,
+            "y": 7.439913035887961,
+            "heading": 5.3325436298288445e-18,
+            "angularVelocity": 5.3856284215509377e-17,
+            "velocityX": -0.7789533333445217,
+            "velocityY": -0.2735661154121556,
+            "timestamp": 0.09901432445765257
+        },
+        {
+            "x": 7.448617388685645,
+            "y": 7.385739108696781,
+            "heading": -4.4022234029758545e-18,
+            "angularVelocity": -9.831675377471393e-17,
+            "velocityX": -1.557906636985473,
+            "velocityY": -0.5471322203925048,
+            "timestamp": 0.19802864891530514
+        },
+        {
+            "x": 7.217234786194527,
+            "y": 7.304478220492258,
+            "heading": -7.053417683545123e-18,
+            "angularVelocity": -2.6775864730895626e-17,
+            "velocityX": -2.3368598812192953,
+            "velocityY": -0.8206983045092451,
+            "timestamp": 0.2970429733729577
+        },
+        {
+            "x": 6.9087246720879545,
+            "y": 7.196130377471774,
+            "heading": 2.6673179344101e-18,
+            "angularVelocity": 9.817504407722252e-17,
+            "velocityX": -3.1158129472318725,
+            "velocityY": -1.0942643260352094,
+            "timestamp": 0.3960572978306103
+        },
+        {
+            "x": 6.5553201862538,
+            "y": 7.072015763386003,
+            "heading": 8.41895708879283e-18,
+            "angularVelocity": 5.808896021749495e-17,
+            "velocityX": -3.5692258445423475,
+            "velocityY": -1.2535015995473784,
+            "timestamp": 0.49507162228826285
+        },
+        {
+            "x": 6.2019157004196215,
+            "y": 6.9479011493002245,
+            "heading": -3.69857396739816e-19,
+            "angularVelocity": -8.876306064099344e-17,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.2535015995474599,
+            "timestamp": 0.5940859467459154
+        },
+        {
+            "x": 5.848511214585444,
+            "y": 6.823786535214446,
+            "heading": -8.032367976884792e-18,
+            "angularVelocity": -7.738789939855791e-17,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.2535015995474599,
+            "timestamp": 0.693100271203568
+        },
+        {
+            "x": 5.495106728751266,
+            "y": 6.699671921128667,
+            "heading": -3.490709714785344e-17,
+            "angularVelocity": -2.714226382747745e-16,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.2535015995474599,
+            "timestamp": 0.7921145956612206
+        },
+        {
+            "x": 5.141702242917089,
+            "y": 6.575557307042889,
+            "heading": -1.2741337636064699e-17,
+            "angularVelocity": 2.238641694845762e-16,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.2535015995474599,
+            "timestamp": 0.8911289201188731
+        },
+        {
+            "x": 4.788297757082911,
+            "y": 6.4514426929571105,
+            "heading": -2.2645871045802615e-17,
+            "angularVelocity": -1.0003131833516608e-16,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.2535015995474597,
+            "timestamp": 0.9901432445765257
+        },
+        {
+            "x": 4.4348932712487334,
+            "y": 6.327328078871332,
+            "heading": -7.505924377434873e-18,
+            "angularVelocity": 1.529066299375997e-16,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.25350159954746,
+            "timestamp": 1.0891575690341782
+        },
+        {
+            "x": 4.081488785414556,
+            "y": 6.203213464785553,
+            "heading": -3.3249179069580955e-17,
+            "angularVelocity": -2.5999525658031657e-16,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.25350159954746,
+            "timestamp": 1.1881718934918308
+        },
+        {
+            "x": 3.728084299580378,
+            "y": 6.079098850699775,
+            "heading": -3.31250701341244e-17,
+            "angularVelocity": 1.2534442479544336e-18,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.2535015995474597,
+            "timestamp": 1.2871862179494835
+        },
+        {
+            "x": 3.3746798137461993,
+            "y": 5.9549842366139965,
+            "heading": -3.474935359347948e-17,
+            "angularVelocity": -1.6404530025230332e-17,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.2535015995474599,
+            "timestamp": 1.3862005424071362
+        },
+        {
+            "x": 3.021275327912045,
+            "y": 5.830869622528225,
+            "heading": -2.4652412430964293e-17,
+            "angularVelocity": 1.0197454982216833e-16,
+            "velocityX": -3.5692258445423475,
+            "velocityY": -1.2535015995473784,
+            "timestamp": 1.4852148668647889
+        },
+        {
+            "x": 2.7127652138054734,
+            "y": 5.722521779507741,
+            "heading": 1.414690860074665e-18,
+            "angularVelocity": 2.632659789801892e-16,
+            "velocityX": -3.115812947231872,
+            "velocityY": -1.0942643260352094,
+            "timestamp": 1.5842291913224416
+        },
+        {
+            "x": 2.4813826113143547,
+            "y": 5.641260891303218,
+            "heading": 8.307803002216867e-18,
+            "angularVelocity": 6.961732115429119e-17,
+            "velocityX": -2.336859881219295,
+            "velocityY": -0.8206983045092451,
+            "timestamp": 1.6832435157800942
+        },
+        {
+            "x": 2.3271275380851444,
+            "y": 5.587086964112038,
+            "heading": -3.972937436005726e-18,
+            "angularVelocity": -1.2402993582700068e-16,
+            "velocityX": -1.557906636985473,
+            "velocityY": -0.5471322203925048,
+            "timestamp": 1.782257840237747
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": -2.184437138856228e-30,
-            "angularVelocity": -0.012314572246193395,
-            "velocityX": -0.7765510184161662,
-            "velocityY": -0.28035948126465987,
-            "timestamp": 1.8813247742176853
+            "heading": 0,
+            "angularVelocity": 4.012487544797503e-17,
+            "velocityX": -0.7789533333445219,
+            "velocityY": -0.2735661154121556,
+            "timestamp": 1.8812721646953996
         },
         {
             "x": 2.25,
             "y": 5.56,
             "heading": 0,
-            "angularVelocity": 2.219047153199805e-29,
-            "velocityX": 0,
-            "velocityY": -7.552577135588168e-32,
-            "timestamp": 1.9803418675975635
+            "angularVelocity": 0,
+            "velocityX": -5.265297803216144e-35,
+            "velocityY": 0,
+            "timestamp": 1.9802864891530523
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.1.traj
index e08f778f..90a593eb 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.1.traj	
@@ -3,92 +3,92 @@
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
+            "heading": -3.817834980565894e-40,
+            "angularVelocity": -1.9855221195343998e-39,
+            "velocityX": -9.621145750064654e-40,
+            "velocityY": 1.4575297691012433e-39,
             "timestamp": 0
         },
         {
-            "x": 7.680000027621492,
-            "y": 5.880500005575844,
-            "heading": 1.812927824526906e-8,
-            "angularVelocity": 1.811639213454267e-7,
-            "velocityX": 2.7601859924059844e-7,
-            "velocityY": 0.8344065481228081,
-            "timestamp": 0.10007112931184069
+            "x": 7.68,
+            "y": 5.880500005575846,
+            "heading": 3.479114156010015e-22,
+            "angularVelocity": 3.4766412561552746e-21,
+            "velocityX": 2.7068848151306367e-33,
+            "velocityY": 0.8344065481228213,
+            "timestamp": 0.10007112931184182
         },
         {
-            "x": 7.6800000748220105,
-            "y": 6.047500013629843,
-            "heading": 4.634757751496617e-8,
-            "angularVelocity": 2.819824195265022e-7,
-            "velocityX": 4.716696889582397e-7,
-            "velocityY": 1.6688130652907391,
-            "timestamp": 0.20014225862368137
+            "x": 7.68,
+            "y": 6.047500013629847,
+            "heading": 1.081691491825702e-21,
+            "angularVelocity": 7.332585052389997e-21,
+            "velocityX": 5.1589068200097806e-33,
+            "velocityY": 1.6688130652907358,
+            "timestamp": 0.20014225862368365
         },
         {
-            "x": 7.68000013381266,
-            "y": 6.298000017966615,
-            "heading": 7.748634725130675e-8,
-            "angularVelocity": 3.1116636502244896e-7,
-            "velocityX": 5.894871979234934e-7,
-            "velocityY": 2.5032195205488934,
-            "timestamp": 0.30021338793552205
+            "x": 7.68,
+            "y": 6.298000017966616,
+            "heading": 2.2511586271387426e-21,
+            "angularVelocity": 1.1686358860616172e-20,
+            "velocityX": 5.8498916949463956e-33,
+            "velocityY": 2.503219520548835,
+            "timestamp": 0.3002133879355255
         },
         {
-            "x": 7.680000197181065,
-            "y": 6.63200000000002,
-            "heading": 1.0565412902376676e-7,
-            "angularVelocity": 2.814776011952798e-7,
-            "velocityX": 6.332336214323824e-7,
-            "velocityY": 3.3376257900777544,
-            "timestamp": 0.40028451724736275
+            "x": 7.68,
+            "y": 6.632,
+            "heading": 4.481673966100434e-21,
+            "angularVelocity": 2.228929929167172e-20,
+            "velocityX": 6.576791251337367e-33,
+            "velocityY": 3.3376257900775013,
+            "timestamp": 0.4002845172473673
         },
         {
-            "x": 7.680000129088366,
-            "y": 6.965999982033409,
-            "heading": 6.403571463499393e-8,
-            "angularVelocity": -4.1588832167210794e-7,
-            "velocityX": -6.804429810396128e-7,
-            "velocityY": 3.337625790077598,
-            "timestamp": 0.5003556465592034
+            "x": 7.68,
+            "y": 6.9659999820333836,
+            "heading": 1.4391637904192512e-21,
+            "angularVelocity": -3.0403475801105514e-20,
+            "velocityX": 1.0509973010204301e-32,
+            "velocityY": 3.3376257900775013,
+            "timestamp": 0.5003556465592092
         },
         {
-            "x": 7.680000069764372,
-            "y": 7.216499986370169,
-            "heading": 3.1970340337388056e-8,
-            "angularVelocity": -3.2042582447411e-7,
-            "velocityX": -5.92818279593764e-7,
-            "velocityY": 2.503219520548763,
-            "timestamp": 0.6004267758710441
+            "x": 7.68,
+            "y": 7.216499986370152,
+            "heading": 4.7457500532831525e-22,
+            "angularVelocity": -9.63903176962576e-21,
+            "velocityX": 7.983324746463204e-33,
+            "velocityY": 2.503219520548835,
+            "timestamp": 0.600426775871051
         },
         {
-            "x": 7.680000024949458,
-            "y": 7.3834999944241595,
-            "heading": 1.0547562280132128e-8,
-            "angularVelocity": -2.1407550924431962e-7,
-            "velocityX": -4.47830590750479e-7,
-            "velocityY": 1.6688130652906517,
-            "timestamp": 0.7004979051828848
+            "x": 7.68,
+            "y": 7.383499994424153,
+            "heading": 1.0909640932435146e-22,
+            "angularVelocity": -3.652188228166023e-21,
+            "velocityX": 7.341551448907972e-33,
+            "velocityY": 1.6688130652907358,
+            "timestamp": 0.7004979051828928
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": -4.103497297298223e-32,
-            "angularVelocity": -1.0540065169911972e-7,
-            "velocityX": -2.4931725183429086e-7,
-            "velocityY": 0.8344065481227665,
-            "timestamp": 0.8005690344947255
+            "heading": -9.42965601107167e-40,
+            "angularVelocity": -1.09018866193586e-21,
+            "velocityX": 7.44470103227488e-33,
+            "velocityY": 0.8344065481228213,
+            "timestamp": 0.8005690344947346
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 9.056551022450501e-31,
-            "velocityX": 0,
-            "velocityY": 3.73051624872423e-37,
-            "timestamp": 0.9006401638065662
+            "heading": 8.249839679439821e-43,
+            "angularVelocity": 9.431229878048832e-39,
+            "velocityX": 1.012759498983901e-41,
+            "velocityY": 5.142787784847508e-39,
+            "timestamp": 0.9006401638065764
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.traj
index e08f778f..90a593eb 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.traj	
@@ -3,92 +3,92 @@
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
+            "heading": -3.817834980565894e-40,
+            "angularVelocity": -1.9855221195343998e-39,
+            "velocityX": -9.621145750064654e-40,
+            "velocityY": 1.4575297691012433e-39,
             "timestamp": 0
         },
         {
-            "x": 7.680000027621492,
-            "y": 5.880500005575844,
-            "heading": 1.812927824526906e-8,
-            "angularVelocity": 1.811639213454267e-7,
-            "velocityX": 2.7601859924059844e-7,
-            "velocityY": 0.8344065481228081,
-            "timestamp": 0.10007112931184069
+            "x": 7.68,
+            "y": 5.880500005575846,
+            "heading": 3.479114156010015e-22,
+            "angularVelocity": 3.4766412561552746e-21,
+            "velocityX": 2.7068848151306367e-33,
+            "velocityY": 0.8344065481228213,
+            "timestamp": 0.10007112931184182
         },
         {
-            "x": 7.6800000748220105,
-            "y": 6.047500013629843,
-            "heading": 4.634757751496617e-8,
-            "angularVelocity": 2.819824195265022e-7,
-            "velocityX": 4.716696889582397e-7,
-            "velocityY": 1.6688130652907391,
-            "timestamp": 0.20014225862368137
+            "x": 7.68,
+            "y": 6.047500013629847,
+            "heading": 1.081691491825702e-21,
+            "angularVelocity": 7.332585052389997e-21,
+            "velocityX": 5.1589068200097806e-33,
+            "velocityY": 1.6688130652907358,
+            "timestamp": 0.20014225862368365
         },
         {
-            "x": 7.68000013381266,
-            "y": 6.298000017966615,
-            "heading": 7.748634725130675e-8,
-            "angularVelocity": 3.1116636502244896e-7,
-            "velocityX": 5.894871979234934e-7,
-            "velocityY": 2.5032195205488934,
-            "timestamp": 0.30021338793552205
+            "x": 7.68,
+            "y": 6.298000017966616,
+            "heading": 2.2511586271387426e-21,
+            "angularVelocity": 1.1686358860616172e-20,
+            "velocityX": 5.8498916949463956e-33,
+            "velocityY": 2.503219520548835,
+            "timestamp": 0.3002133879355255
         },
         {
-            "x": 7.680000197181065,
-            "y": 6.63200000000002,
-            "heading": 1.0565412902376676e-7,
-            "angularVelocity": 2.814776011952798e-7,
-            "velocityX": 6.332336214323824e-7,
-            "velocityY": 3.3376257900777544,
-            "timestamp": 0.40028451724736275
+            "x": 7.68,
+            "y": 6.632,
+            "heading": 4.481673966100434e-21,
+            "angularVelocity": 2.228929929167172e-20,
+            "velocityX": 6.576791251337367e-33,
+            "velocityY": 3.3376257900775013,
+            "timestamp": 0.4002845172473673
         },
         {
-            "x": 7.680000129088366,
-            "y": 6.965999982033409,
-            "heading": 6.403571463499393e-8,
-            "angularVelocity": -4.1588832167210794e-7,
-            "velocityX": -6.804429810396128e-7,
-            "velocityY": 3.337625790077598,
-            "timestamp": 0.5003556465592034
+            "x": 7.68,
+            "y": 6.9659999820333836,
+            "heading": 1.4391637904192512e-21,
+            "angularVelocity": -3.0403475801105514e-20,
+            "velocityX": 1.0509973010204301e-32,
+            "velocityY": 3.3376257900775013,
+            "timestamp": 0.5003556465592092
         },
         {
-            "x": 7.680000069764372,
-            "y": 7.216499986370169,
-            "heading": 3.1970340337388056e-8,
-            "angularVelocity": -3.2042582447411e-7,
-            "velocityX": -5.92818279593764e-7,
-            "velocityY": 2.503219520548763,
-            "timestamp": 0.6004267758710441
+            "x": 7.68,
+            "y": 7.216499986370152,
+            "heading": 4.7457500532831525e-22,
+            "angularVelocity": -9.63903176962576e-21,
+            "velocityX": 7.983324746463204e-33,
+            "velocityY": 2.503219520548835,
+            "timestamp": 0.600426775871051
         },
         {
-            "x": 7.680000024949458,
-            "y": 7.3834999944241595,
-            "heading": 1.0547562280132128e-8,
-            "angularVelocity": -2.1407550924431962e-7,
-            "velocityX": -4.47830590750479e-7,
-            "velocityY": 1.6688130652906517,
-            "timestamp": 0.7004979051828848
+            "x": 7.68,
+            "y": 7.383499994424153,
+            "heading": 1.0909640932435146e-22,
+            "angularVelocity": -3.652188228166023e-21,
+            "velocityX": 7.341551448907972e-33,
+            "velocityY": 1.6688130652907358,
+            "timestamp": 0.7004979051828928
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": -4.103497297298223e-32,
-            "angularVelocity": -1.0540065169911972e-7,
-            "velocityX": -2.4931725183429086e-7,
-            "velocityY": 0.8344065481227665,
-            "timestamp": 0.8005690344947255
+            "heading": -9.42965601107167e-40,
+            "angularVelocity": -1.09018866193586e-21,
+            "velocityX": 7.44470103227488e-33,
+            "velocityY": 0.8344065481228213,
+            "timestamp": 0.8005690344947346
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 9.056551022450501e-31,
-            "velocityX": 0,
-            "velocityY": 3.73051624872423e-37,
-            "timestamp": 0.9006401638065662
+            "heading": 8.249839679439821e-43,
+            "angularVelocity": 9.431229878048832e-39,
+            "velocityX": 1.012759498983901e-41,
+            "velocityY": 5.142787784847508e-39,
+            "timestamp": 0.9006401638065764
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.1.traj
index d4dafa95..74357de2 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.1.traj	
@@ -3,92 +3,92 @@
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -8.386874849918624e-34,
-            "velocityY": 2.2609899154097436e-36,
+            "heading": -5.382102664363315e-41,
+            "angularVelocity": -2.8652892976895567e-45,
+            "velocityX": 1.592735858156421e-40,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.6800000367448975,
-            "y": 5.713499994424246,
-            "heading": -2.1127327949152964e-9,
-            "angularVelocity": -2.1112310428149592e-8,
-            "velocityX": 3.67187799986145e-7,
-            "velocityY": -0.8344065481223435,
-            "timestamp": 0.10007112931179547
+            "x": 7.68,
+            "y": 5.713499994424153,
+            "heading": 1.659495816830368e-21,
+            "angularVelocity": 1.658316243120447e-20,
+            "velocityX": 2.780383744489643e-33,
+            "velocityY": -0.8344065481228213,
+            "timestamp": 0.10007112931184184
         },
         {
-            "x": 7.680000101493379,
-            "y": 5.546499986370426,
-            "heading": -1.0421334714876827e-8,
-            "angularVelocity": -8.302696522536156e-8,
-            "velocityX": 6.470245866798498e-7,
-            "velocityY": -1.668813065289814,
-            "timestamp": 0.20014225862359095
+            "x": 7.68,
+            "y": 5.546499986370153,
+            "heading": 4.9512387638524226e-21,
+            "angularVelocity": 3.289403167370527e-20,
+            "velocityX": 3.618624574631574e-33,
+            "velocityY": -1.668813065290736,
+            "timestamp": 0.20014225862368368
         },
         {
-            "x": 7.680000185334384,
-            "y": 5.295999982033924,
-            "heading": -2.6903389225477584e-8,
-            "angularVelocity": -1.6470340117310356e-7,
-            "velocityX": 8.378141164132646e-7,
-            "velocityY": -2.503219520547496,
-            "timestamp": 0.30021338793538643
+            "x": 7.68,
+            "y": 5.295999982033384,
+            "heading": 9.801760970151702e-21,
+            "angularVelocity": 4.8470744390330394e-20,
+            "velocityX": 3.439292531132709e-33,
+            "velocityY": -2.503219520548835,
+            "timestamp": 0.30021338793552554
         },
         {
-            "x": 7.680000279163236,
-            "y": 4.961999999999876,
-            "heading": -5.074281043975999e-8,
-            "angularVelocity": -2.3822478681802103e-7,
-            "velocityX": 9.376216058628794e-7,
-            "velocityY": -3.3376257900847826,
-            "timestamp": 0.4002845172471819
+            "x": 7.68,
+            "y": 4.962,
+            "heading": 1.506109654033251e-20,
+            "angularVelocity": 5.255597191401202e-20,
+            "velocityX": 4.639675238422859e-33,
+            "velocityY": -3.3376257900775017,
+            "timestamp": 0.40028451724736736
         },
         {
-            "x": 7.680000186570606,
-            "y": 4.628000017966085,
-            "heading": -3.213613801692531e-8,
-            "angularVelocity": 1.8593453255418192e-7,
-            "velocityX": -9.252681679909059e-7,
-            "velocityY": -3.3376257900823116,
-            "timestamp": 0.5003556465589774
+            "x": 7.68,
+            "y": 4.628000017966616,
+            "heading": 5.3586032763760104e-21,
+            "angularVelocity": -9.69559666912617e-20,
+            "velocityX": 4.935439546802769e-33,
+            "velocityY": -3.3376257900775017,
+            "timestamp": 0.5003556465592092
         },
         {
-            "x": 7.68000010282861,
-            "y": 4.377500013629577,
-            "heading": -1.6413785806619038e-8,
-            "angularVelocity": 1.5711175564873292e-7,
-            "velocityX": -8.368247227344002e-7,
-            "velocityY": -2.5032195205475483,
-            "timestamp": 0.6004267758707729
+            "x": 7.68,
+            "y": 4.377500013629846,
+            "heading": 1.8760590350448333e-21,
+            "angularVelocity": -3.480068842301423e-20,
+            "velocityX": 4.701730454770122e-33,
+            "velocityY": -2.5032195205488352,
+            "timestamp": 0.600426775871051
         },
         {
-            "x": 7.6800000374582895,
-            "y": 4.2105000055757555,
-            "heading": -5.494810627011176e-9,
-            "angularVelocity": 1.0911213201504113e-7,
-            "velocityX": -6.532385577861589e-7,
-            "velocityY": -1.668813065289845,
-            "timestamp": 0.7004979051825684
+            "x": 7.68,
+            "y": 4.210500005575846,
+            "heading": 3.168722428707802e-22,
+            "angularVelocity": -1.5580785203142817e-20,
+            "velocityX": 3.49539235617723e-33,
+            "velocityY": -1.668813065290736,
+            "timestamp": 0.7004979051828928
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 5.4909043426679847e-8,
-            "velocityX": -3.743166510876222e-7,
-            "velocityY": -0.8344065481223572,
-            "timestamp": 0.8005690344943639
+            "heading": 5.881496870004905e-41,
+            "angularVelocity": -3.1664700918333573e-21,
+            "velocityX": 3.364378386644386e-33,
+            "velocityY": -0.8344065481228213,
+            "timestamp": 0.8005690344947346
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 1.4758845853352304e-34,
-            "velocityY": 2.7202161180895526e-29,
-            "timestamp": 0.9006401638061594
+            "heading": 9.145026397391945e-42,
+            "angularVelocity": -4.963805231001231e-40,
+            "velocityX": -4.870998572760777e-42,
+            "velocityY": 3.489748854003666e-39,
+            "timestamp": 0.9006401638065764
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.traj
index d4dafa95..74357de2 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.traj	
@@ -3,92 +3,92 @@
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -8.386874849918624e-34,
-            "velocityY": 2.2609899154097436e-36,
+            "heading": -5.382102664363315e-41,
+            "angularVelocity": -2.8652892976895567e-45,
+            "velocityX": 1.592735858156421e-40,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.6800000367448975,
-            "y": 5.713499994424246,
-            "heading": -2.1127327949152964e-9,
-            "angularVelocity": -2.1112310428149592e-8,
-            "velocityX": 3.67187799986145e-7,
-            "velocityY": -0.8344065481223435,
-            "timestamp": 0.10007112931179547
+            "x": 7.68,
+            "y": 5.713499994424153,
+            "heading": 1.659495816830368e-21,
+            "angularVelocity": 1.658316243120447e-20,
+            "velocityX": 2.780383744489643e-33,
+            "velocityY": -0.8344065481228213,
+            "timestamp": 0.10007112931184184
         },
         {
-            "x": 7.680000101493379,
-            "y": 5.546499986370426,
-            "heading": -1.0421334714876827e-8,
-            "angularVelocity": -8.302696522536156e-8,
-            "velocityX": 6.470245866798498e-7,
-            "velocityY": -1.668813065289814,
-            "timestamp": 0.20014225862359095
+            "x": 7.68,
+            "y": 5.546499986370153,
+            "heading": 4.9512387638524226e-21,
+            "angularVelocity": 3.289403167370527e-20,
+            "velocityX": 3.618624574631574e-33,
+            "velocityY": -1.668813065290736,
+            "timestamp": 0.20014225862368368
         },
         {
-            "x": 7.680000185334384,
-            "y": 5.295999982033924,
-            "heading": -2.6903389225477584e-8,
-            "angularVelocity": -1.6470340117310356e-7,
-            "velocityX": 8.378141164132646e-7,
-            "velocityY": -2.503219520547496,
-            "timestamp": 0.30021338793538643
+            "x": 7.68,
+            "y": 5.295999982033384,
+            "heading": 9.801760970151702e-21,
+            "angularVelocity": 4.8470744390330394e-20,
+            "velocityX": 3.439292531132709e-33,
+            "velocityY": -2.503219520548835,
+            "timestamp": 0.30021338793552554
         },
         {
-            "x": 7.680000279163236,
-            "y": 4.961999999999876,
-            "heading": -5.074281043975999e-8,
-            "angularVelocity": -2.3822478681802103e-7,
-            "velocityX": 9.376216058628794e-7,
-            "velocityY": -3.3376257900847826,
-            "timestamp": 0.4002845172471819
+            "x": 7.68,
+            "y": 4.962,
+            "heading": 1.506109654033251e-20,
+            "angularVelocity": 5.255597191401202e-20,
+            "velocityX": 4.639675238422859e-33,
+            "velocityY": -3.3376257900775017,
+            "timestamp": 0.40028451724736736
         },
         {
-            "x": 7.680000186570606,
-            "y": 4.628000017966085,
-            "heading": -3.213613801692531e-8,
-            "angularVelocity": 1.8593453255418192e-7,
-            "velocityX": -9.252681679909059e-7,
-            "velocityY": -3.3376257900823116,
-            "timestamp": 0.5003556465589774
+            "x": 7.68,
+            "y": 4.628000017966616,
+            "heading": 5.3586032763760104e-21,
+            "angularVelocity": -9.69559666912617e-20,
+            "velocityX": 4.935439546802769e-33,
+            "velocityY": -3.3376257900775017,
+            "timestamp": 0.5003556465592092
         },
         {
-            "x": 7.68000010282861,
-            "y": 4.377500013629577,
-            "heading": -1.6413785806619038e-8,
-            "angularVelocity": 1.5711175564873292e-7,
-            "velocityX": -8.368247227344002e-7,
-            "velocityY": -2.5032195205475483,
-            "timestamp": 0.6004267758707729
+            "x": 7.68,
+            "y": 4.377500013629846,
+            "heading": 1.8760590350448333e-21,
+            "angularVelocity": -3.480068842301423e-20,
+            "velocityX": 4.701730454770122e-33,
+            "velocityY": -2.5032195205488352,
+            "timestamp": 0.600426775871051
         },
         {
-            "x": 7.6800000374582895,
-            "y": 4.2105000055757555,
-            "heading": -5.494810627011176e-9,
-            "angularVelocity": 1.0911213201504113e-7,
-            "velocityX": -6.532385577861589e-7,
-            "velocityY": -1.668813065289845,
-            "timestamp": 0.7004979051825684
+            "x": 7.68,
+            "y": 4.210500005575846,
+            "heading": 3.168722428707802e-22,
+            "angularVelocity": -1.5580785203142817e-20,
+            "velocityX": 3.49539235617723e-33,
+            "velocityY": -1.668813065290736,
+            "timestamp": 0.7004979051828928
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 5.4909043426679847e-8,
-            "velocityX": -3.743166510876222e-7,
-            "velocityY": -0.8344065481223572,
-            "timestamp": 0.8005690344943639
+            "heading": 5.881496870004905e-41,
+            "angularVelocity": -3.1664700918333573e-21,
+            "velocityX": 3.364378386644386e-33,
+            "velocityY": -0.8344065481228213,
+            "timestamp": 0.8005690344947346
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 1.4758845853352304e-34,
-            "velocityY": 2.7202161180895526e-29,
-            "timestamp": 0.9006401638061594
+            "heading": 9.145026397391945e-42,
+            "angularVelocity": -4.963805231001231e-40,
+            "velocityX": -4.870998572760777e-42,
+            "velocityY": 3.489748854003666e-39,
+            "timestamp": 0.9006401638065764
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.1.traj
index be761719..bcede3cb 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.1.traj	
@@ -4,136 +4,136 @@
             "x": 7.68,
             "y": 5.797,
             "heading": 0,
-            "angularVelocity": -3.3479093461707414e-32,
+            "angularVelocity": 7.067561628822357e-40,
             "velocityX": 0,
-            "velocityY": 6.414063862178313e-32,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.680000074936813,
-            "y": 5.720554627994495,
-            "heading": 3.835783864206662e-9,
-            "angularVelocity": 4.006018550688398e-8,
-            "velocityX": 7.826256085285986e-7,
-            "velocityY": -0.7983807350943762,
-            "timestamp": 0.09575052183150116
+            "x": 7.68,
+            "y": 5.720554627994461,
+            "heading": -3.8243496398495774e-19,
+            "angularVelocity": -3.994077070459528e-18,
+            "velocityX": -4.592295404090674e-34,
+            "velocityY": -0.7983807350946677,
+            "timestamp": 0.09575052183150977
         },
         {
-            "x": 7.680000216991337,
-            "y": 5.567663887017398,
-            "heading": 7.581387411185645e-9,
-            "angularVelocity": 3.911835760311887e-8,
-            "velocityX": 0.0000014835900057043023,
-            "velocityY": -1.5967614385031577,
-            "timestamp": 0.19150104366300233
+            "x": 7.68,
+            "y": 5.567663887017311,
+            "heading": -1.1460790562861162e-18,
+            "angularVelocity": -7.975351749671857e-18,
+            "velocityX": 3.216033275420413e-35,
+            "velocityY": -1.5967614385035829,
+            "timestamp": 0.19150104366301954
         },
         {
-            "x": 7.68000041820133,
-            "y": 5.338327783136532,
-            "heading": 9.040192056984193e-9,
-            "angularVelocity": 1.523546877140865e-8,
-            "velocityX": 0.0000021013982858961807,
-            "velocityY": -2.3951420785407556,
-            "timestamp": 0.2872515654945035
+            "x": 7.68,
+            "y": 5.3383277831364,
+            "heading": -2.289011643788579e-18,
+            "angularVelocity": -1.1936567706665239e-17,
+            "velocityX": -6.99395718741277e-36,
+            "velocityY": -2.3951420785410233,
+            "timestamp": 0.2872515654945293
         },
         {
-            "x": 7.680000670435067,
-            "y": 5.032546334555316,
-            "heading": 8.315274931561148e-9,
-            "angularVelocity": -7.570901836021169e-9,
-            "velocityX": 0.000002634280323205928,
-            "velocityY": -3.193522528465399,
-            "timestamp": 0.38300208732600466
+            "x": 7.68,
+            "y": 5.032546334555254,
+            "heading": -3.801510097022925e-18,
+            "angularVelocity": -1.5796242396621782e-17,
+            "velocityX": -1.9389720200265424e-34,
+            "velocityY": -3.1935225284643693,
+            "timestamp": 0.3830020873260391
         },
         {
-            "x": 7.680000832750872,
-            "y": 4.67032780073313,
-            "heading": 8.315279560400229e-9,
-            "angularVelocity": 4.9454766224662506e-14,
-            "velocityX": 0.0000016951952366761453,
-            "velocityY": -3.782940571954316,
-            "timestamp": 0.47875260915750584
+            "x": 7.68,
+            "y": 4.670327800733179,
+            "heading": -7.244345840319028e-19,
+            "angularVelocity": 3.2136383741131935e-17,
+            "velocityX": -8.432373407035536e-35,
+            "velocityY": -3.7829405719528495,
+            "timestamp": 0.47875260915754886
         },
         {
-            "x": 7.680000914152323,
-            "y": 4.308109266911049,
-            "heading": 8.315282864092028e-9,
-            "angularVelocity": 3.631070768427552e-14,
-            "velocityX": 8.501410582319702e-7,
-            "velocityY": -3.782940571953249,
-            "timestamp": 0.574503130989007
+            "x": 7.68,
+            "y": 4.308109266911059,
+            "heading": -3.757861819981246e-18,
+            "angularVelocity": -3.168052954622205e-17,
+            "velocityX": -1.6004910806259417e-34,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.5745031309890586
         },
         {
-            "x": 7.680000914394957,
-            "y": 3.9458907330889588,
-            "heading": 8.315285941661222e-9,
-            "angularVelocity": 3.388658702074781e-14,
-            "velocityX": 2.5340105811348506e-9,
-            "velocityY": -3.7829405719533424,
-            "timestamp": 0.6702536528205081
+            "x": 7.68,
+            "y": 3.9458907330889397,
+            "heading": -5.3976563030855884e-18,
+            "angularVelocity": -1.712569761227889e-17,
+            "velocityX": -2.8603575042124878e-33,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.6702536528205684
         },
         {
-            "x": 7.680000833430653,
-            "y": 3.583672199266877,
-            "heading": 8.315288291957849e-9,
-            "angularVelocity": 2.592873274060122e-14,
-            "velocityX": -8.455755806508837e-7,
-            "velocityY": -3.78294057195325,
-            "timestamp": 0.7660041746520092
+            "x": 7.68,
+            "y": 3.5836721992668203,
+            "heading": -4.35645304230582e-18,
+            "angularVelocity": 1.0874126227865062e-17,
+            "velocityX": -1.9073271103237224e-34,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.7660041746520782
         },
         {
-            "x": 7.6800006713921105,
-            "y": 3.2214536654446886,
-            "heading": 8.315293718879221e-9,
-            "angularVelocity": 5.742758811630884e-14,
-            "velocityX": -0.0000016922995574745742,
-            "velocityY": -3.782940571954345,
-            "timestamp": 0.8617546964835103
+            "x": 7.68,
+            "y": 3.221453665444745,
+            "heading": 3.661990694649205e-18,
+            "angularVelocity": 8.37430813282138e-17,
+            "velocityX": -3.48109229998139e-33,
+            "velocityY": -3.7829405719528495,
+            "timestamp": 0.8617546964835879
         },
         {
-            "x": 7.680000418867919,
-            "y": 2.915672216863469,
-            "heading": 6.817552183291689e-10,
-            "angularVelocity": -7.972319645162058e-8,
-            "velocityX": -0.000002637313779217548,
-            "velocityY": -3.1935225284654263,
-            "timestamp": 0.9575052183150115
+            "x": 7.68,
+            "y": 2.915672216863599,
+            "heading": 2.222559506115206e-18,
+            "angularVelocity": -1.503314198751682e-17,
+            "velocityX": 7.233727398507661e-34,
+            "velocityY": -3.1935225284643693,
+            "timestamp": 0.9575052183150977
         },
         {
-            "x": 7.680000217372703,
-            "y": 2.6863361129826027,
-            "heading": -2.8485300950885154e-9,
-            "angularVelocity": -3.6869616762863894e-8,
-            "velocityX": -0.00000210437708878744,
-            "velocityY": -2.395142078540767,
-            "timestamp": 1.0532557401465126
+            "x": 7.68,
+            "y": 2.686336112982689,
+            "heading": 1.1174854461234148e-18,
+            "angularVelocity": -1.1541180553049724e-17,
+            "velocityX": 7.218959232022663e-34,
+            "velocityY": -2.3951420785410233,
+            "timestamp": 1.0532557401466074
         },
         {
-            "x": 7.680000075080448,
-            "y": 2.533445372005505,
-            "heading": -2.300788754937178e-9,
-            "angularVelocity": 5.720505703316253e-9,
-            "velocityX": -0.0000014860728228146123,
-            "velocityY": -1.596761438503163,
-            "timestamp": 1.1490062619780137
+            "x": 7.68,
+            "y": 2.5334453720055388,
+            "heading": 3.7383227049768543e-19,
+            "angularVelocity": -7.766570474567622e-18,
+            "velocityX": 1.7872274250399634e-33,
+            "velocityY": -1.5967614385035829,
+            "timestamp": 1.1490062619781172
         },
         {
             "x": 7.68,
             "y": 2.457,
             "heading": 0,
-            "angularVelocity": 2.4028994457396085e-8,
-            "velocityX": -7.841257148612366e-7,
-            "velocityY": -0.7983807350943783,
-            "timestamp": 1.2447567838095148
+            "angularVelocity": -3.904232200157263e-18,
+            "velocityX": 6.089132400706592e-33,
+            "velocityY": -0.7983807350946677,
+            "timestamp": 1.244756783809627
         },
         {
             "x": 7.68,
             "y": 2.457,
             "heading": 0,
             "angularVelocity": 0,
-            "velocityX": -1.5050925658054699e-33,
-            "velocityY": -8.233479718145206e-39,
-            "timestamp": 1.340507305641016
+            "velocityX": 3.5763678522980893e-43,
+            "velocityY": -5.882608067207678e-38,
+            "timestamp": 1.3405073056411367
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.traj
index be761719..bcede3cb 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.traj	
@@ -4,136 +4,136 @@
             "x": 7.68,
             "y": 5.797,
             "heading": 0,
-            "angularVelocity": -3.3479093461707414e-32,
+            "angularVelocity": 7.067561628822357e-40,
             "velocityX": 0,
-            "velocityY": 6.414063862178313e-32,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.680000074936813,
-            "y": 5.720554627994495,
-            "heading": 3.835783864206662e-9,
-            "angularVelocity": 4.006018550688398e-8,
-            "velocityX": 7.826256085285986e-7,
-            "velocityY": -0.7983807350943762,
-            "timestamp": 0.09575052183150116
+            "x": 7.68,
+            "y": 5.720554627994461,
+            "heading": -3.8243496398495774e-19,
+            "angularVelocity": -3.994077070459528e-18,
+            "velocityX": -4.592295404090674e-34,
+            "velocityY": -0.7983807350946677,
+            "timestamp": 0.09575052183150977
         },
         {
-            "x": 7.680000216991337,
-            "y": 5.567663887017398,
-            "heading": 7.581387411185645e-9,
-            "angularVelocity": 3.911835760311887e-8,
-            "velocityX": 0.0000014835900057043023,
-            "velocityY": -1.5967614385031577,
-            "timestamp": 0.19150104366300233
+            "x": 7.68,
+            "y": 5.567663887017311,
+            "heading": -1.1460790562861162e-18,
+            "angularVelocity": -7.975351749671857e-18,
+            "velocityX": 3.216033275420413e-35,
+            "velocityY": -1.5967614385035829,
+            "timestamp": 0.19150104366301954
         },
         {
-            "x": 7.68000041820133,
-            "y": 5.338327783136532,
-            "heading": 9.040192056984193e-9,
-            "angularVelocity": 1.523546877140865e-8,
-            "velocityX": 0.0000021013982858961807,
-            "velocityY": -2.3951420785407556,
-            "timestamp": 0.2872515654945035
+            "x": 7.68,
+            "y": 5.3383277831364,
+            "heading": -2.289011643788579e-18,
+            "angularVelocity": -1.1936567706665239e-17,
+            "velocityX": -6.99395718741277e-36,
+            "velocityY": -2.3951420785410233,
+            "timestamp": 0.2872515654945293
         },
         {
-            "x": 7.680000670435067,
-            "y": 5.032546334555316,
-            "heading": 8.315274931561148e-9,
-            "angularVelocity": -7.570901836021169e-9,
-            "velocityX": 0.000002634280323205928,
-            "velocityY": -3.193522528465399,
-            "timestamp": 0.38300208732600466
+            "x": 7.68,
+            "y": 5.032546334555254,
+            "heading": -3.801510097022925e-18,
+            "angularVelocity": -1.5796242396621782e-17,
+            "velocityX": -1.9389720200265424e-34,
+            "velocityY": -3.1935225284643693,
+            "timestamp": 0.3830020873260391
         },
         {
-            "x": 7.680000832750872,
-            "y": 4.67032780073313,
-            "heading": 8.315279560400229e-9,
-            "angularVelocity": 4.9454766224662506e-14,
-            "velocityX": 0.0000016951952366761453,
-            "velocityY": -3.782940571954316,
-            "timestamp": 0.47875260915750584
+            "x": 7.68,
+            "y": 4.670327800733179,
+            "heading": -7.244345840319028e-19,
+            "angularVelocity": 3.2136383741131935e-17,
+            "velocityX": -8.432373407035536e-35,
+            "velocityY": -3.7829405719528495,
+            "timestamp": 0.47875260915754886
         },
         {
-            "x": 7.680000914152323,
-            "y": 4.308109266911049,
-            "heading": 8.315282864092028e-9,
-            "angularVelocity": 3.631070768427552e-14,
-            "velocityX": 8.501410582319702e-7,
-            "velocityY": -3.782940571953249,
-            "timestamp": 0.574503130989007
+            "x": 7.68,
+            "y": 4.308109266911059,
+            "heading": -3.757861819981246e-18,
+            "angularVelocity": -3.168052954622205e-17,
+            "velocityX": -1.6004910806259417e-34,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.5745031309890586
         },
         {
-            "x": 7.680000914394957,
-            "y": 3.9458907330889588,
-            "heading": 8.315285941661222e-9,
-            "angularVelocity": 3.388658702074781e-14,
-            "velocityX": 2.5340105811348506e-9,
-            "velocityY": -3.7829405719533424,
-            "timestamp": 0.6702536528205081
+            "x": 7.68,
+            "y": 3.9458907330889397,
+            "heading": -5.3976563030855884e-18,
+            "angularVelocity": -1.712569761227889e-17,
+            "velocityX": -2.8603575042124878e-33,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.6702536528205684
         },
         {
-            "x": 7.680000833430653,
-            "y": 3.583672199266877,
-            "heading": 8.315288291957849e-9,
-            "angularVelocity": 2.592873274060122e-14,
-            "velocityX": -8.455755806508837e-7,
-            "velocityY": -3.78294057195325,
-            "timestamp": 0.7660041746520092
+            "x": 7.68,
+            "y": 3.5836721992668203,
+            "heading": -4.35645304230582e-18,
+            "angularVelocity": 1.0874126227865062e-17,
+            "velocityX": -1.9073271103237224e-34,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.7660041746520782
         },
         {
-            "x": 7.6800006713921105,
-            "y": 3.2214536654446886,
-            "heading": 8.315293718879221e-9,
-            "angularVelocity": 5.742758811630884e-14,
-            "velocityX": -0.0000016922995574745742,
-            "velocityY": -3.782940571954345,
-            "timestamp": 0.8617546964835103
+            "x": 7.68,
+            "y": 3.221453665444745,
+            "heading": 3.661990694649205e-18,
+            "angularVelocity": 8.37430813282138e-17,
+            "velocityX": -3.48109229998139e-33,
+            "velocityY": -3.7829405719528495,
+            "timestamp": 0.8617546964835879
         },
         {
-            "x": 7.680000418867919,
-            "y": 2.915672216863469,
-            "heading": 6.817552183291689e-10,
-            "angularVelocity": -7.972319645162058e-8,
-            "velocityX": -0.000002637313779217548,
-            "velocityY": -3.1935225284654263,
-            "timestamp": 0.9575052183150115
+            "x": 7.68,
+            "y": 2.915672216863599,
+            "heading": 2.222559506115206e-18,
+            "angularVelocity": -1.503314198751682e-17,
+            "velocityX": 7.233727398507661e-34,
+            "velocityY": -3.1935225284643693,
+            "timestamp": 0.9575052183150977
         },
         {
-            "x": 7.680000217372703,
-            "y": 2.6863361129826027,
-            "heading": -2.8485300950885154e-9,
-            "angularVelocity": -3.6869616762863894e-8,
-            "velocityX": -0.00000210437708878744,
-            "velocityY": -2.395142078540767,
-            "timestamp": 1.0532557401465126
+            "x": 7.68,
+            "y": 2.686336112982689,
+            "heading": 1.1174854461234148e-18,
+            "angularVelocity": -1.1541180553049724e-17,
+            "velocityX": 7.218959232022663e-34,
+            "velocityY": -2.3951420785410233,
+            "timestamp": 1.0532557401466074
         },
         {
-            "x": 7.680000075080448,
-            "y": 2.533445372005505,
-            "heading": -2.300788754937178e-9,
-            "angularVelocity": 5.720505703316253e-9,
-            "velocityX": -0.0000014860728228146123,
-            "velocityY": -1.596761438503163,
-            "timestamp": 1.1490062619780137
+            "x": 7.68,
+            "y": 2.5334453720055388,
+            "heading": 3.7383227049768543e-19,
+            "angularVelocity": -7.766570474567622e-18,
+            "velocityX": 1.7872274250399634e-33,
+            "velocityY": -1.5967614385035829,
+            "timestamp": 1.1490062619781172
         },
         {
             "x": 7.68,
             "y": 2.457,
             "heading": 0,
-            "angularVelocity": 2.4028994457396085e-8,
-            "velocityX": -7.841257148612366e-7,
-            "velocityY": -0.7983807350943783,
-            "timestamp": 1.2447567838095148
+            "angularVelocity": -3.904232200157263e-18,
+            "velocityX": 6.089132400706592e-33,
+            "velocityY": -0.7983807350946677,
+            "timestamp": 1.244756783809627
         },
         {
             "x": 7.68,
             "y": 2.457,
             "heading": 0,
             "angularVelocity": 0,
-            "velocityX": -1.5050925658054699e-33,
-            "velocityY": -8.233479718145206e-39,
-            "timestamp": 1.340507305641016
+            "velocityX": 3.5763678522980893e-43,
+            "velocityY": -5.882608067207678e-38,
+            "timestamp": 1.3405073056411367
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.1.traj
index df939479..3fd40c6e 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.1.traj	
@@ -4,172 +4,172 @@
             "x": 7.68,
             "y": 5.797,
             "heading": 0,
-            "angularVelocity": 1.5719252729433342e-31,
-            "velocityX": 9.448616775755806e-32,
-            "velocityY": 5.840312874632397e-38,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -1.2882449649302714e-32,
             "timestamp": 0
         },
         {
-            "x": 7.680000115633079,
-            "y": 5.715155846048254,
-            "heading": -1.667708661616254e-8,
-            "angularVelocity": -1.683296946208156e-7,
-            "velocityX": 0.0000011671392662331341,
-            "velocityY": -0.8260917454273093,
-            "timestamp": 0.0990739278110157
-        },
-        {
-            "x": 7.68000033829167,
-            "y": 5.551467541258106,
-            "heading": -5.4122540412155035e-8,
-            "angularVelocity": -3.7795460569235657e-7,
-            "velocityX": 0.0000022473982686017914,
-            "velocityY": -1.6521834594301872,
-            "timestamp": 0.1981478556220314
-        },
-        {
-            "x": 7.680000659202128,
-            "y": 5.305935091856244,
-            "heading": -1.1439481389604968e-7,
-            "angularVelocity": -6.083564268701089e-7,
-            "velocityX": 0.000003239100713146307,
-            "velocityY": -2.4782751105841556,
-            "timestamp": 0.2972217834330471
-        },
-        {
-            "x": 7.680001069396323,
-            "y": 4.978558516522715,
-            "heading": -1.9685469205745493e-7,
-            "angularVelocity": -8.323062817894511e-7,
-            "velocityX": 0.000004140283484089047,
-            "velocityY": -3.304366573191614,
-            "timestamp": 0.3962957112440628
-        },
-        {
-            "x": 7.680001393540419,
-            "y": 4.603767735073282,
-            "heading": -1.9685469402018675e-7,
-            "angularVelocity": -2.3695928725495928e-14,
-            "velocityX": 0.0000032717397250257404,
-            "velocityY": -3.782940575085891,
-            "timestamp": 0.4953696390550785
-        },
-        {
-            "x": 7.680001631663456,
-            "y": 4.228976953623768,
-            "heading": -1.9685469983118006e-7,
-            "angularVelocity": -6.145037083886684e-14,
-            "velocityX": 0.0000024034882723677366,
-            "velocityY": -3.782940575086774,
-            "timestamp": 0.5944435668660942
-        },
-        {
-            "x": 7.680001783604786,
-            "y": 3.854186172174223,
-            "heading": -1.9685470647072664e-7,
-            "angularVelocity": -7.008912946454424e-14,
-            "velocityX": 0.0000015336156103104506,
-            "velocityY": -3.78294057508712,
-            "timestamp": 0.6935174946771099
-        },
-        {
-            "x": 7.680001849310107,
-            "y": 3.479395390724661,
-            "heading": -1.9685471357202238e-7,
-            "angularVelocity": -7.568241713719523e-14,
-            "velocityX": 6.631948562175283e-7,
-            "velocityY": -3.782940575087319,
-            "timestamp": 0.7925914224881256
-        },
-        {
-            "x": 7.680001829148679,
-            "y": 3.104604609275093,
-            "heading": -1.968547206040393e-7,
-            "angularVelocity": -7.70206577850091e-14,
-            "velocityX": -2.0349878104492754e-7,
-            "velocityY": -3.7829405750873777,
-            "timestamp": 0.8916653502991414
-        },
-        {
-            "x": 7.680001725125185,
-            "y": 2.729813827825532,
-            "heading": -1.968547288641082e-7,
-            "angularVelocity": -9.134372064218233e-14,
-            "velocityX": -0.0000010499582046587897,
-            "velocityY": -3.7829405750872978,
-            "timestamp": 0.9907392781101572
-        },
-        {
-            "x": 7.680001540827678,
-            "y": 2.3550230463759907,
-            "heading": -1.9685473554016492e-7,
-            "angularVelocity": -7.335754268551387e-14,
-            "velocityX": -0.0000018602017039855543,
-            "velocityY": -3.7829405750870753,
-            "timestamp": 1.089813205921173
-        },
-        {
-            "x": 7.680001281411145,
-            "y": 1.980232264926482,
-            "heading": -1.9685473620018866e-7,
-            "angularVelocity": -6.8770489517299035e-15,
-            "velocityX": -0.0000026184135239642236,
-            "velocityY": -3.782940575086706,
-            "timestamp": 1.1888871337321887
-        },
-        {
-            "x": 7.680000953971803,
-            "y": 1.6054414834770692,
-            "heading": -1.968547355090094e-7,
-            "angularVelocity": 1.0827757762815176e-14,
-            "velocityX": -0.000003305000098197799,
-            "velocityY": -3.7829405750856786,
-            "timestamp": 1.2879610615432044
-        },
-        {
-            "x": 7.6800005831051426,
-            "y": 1.2780649081436681,
-            "heading": -1.1900708358832985e-7,
-            "angularVelocity": 7.857529072428817e-7,
-            "velocityX": -0.0000037433322544868336,
-            "velocityY": -3.30436657319033,
-            "timestamp": 1.3870349893542202
-        },
-        {
-            "x": 7.680000296749478,
-            "y": 1.0325324587418607,
-            "heading": -5.959102520162259e-8,
-            "angularVelocity": 5.997142524637507e-7,
-            "velocityX": -0.000002890322961269868,
-            "velocityY": -2.478275110583629,
-            "timestamp": 1.486108917165236
-        },
+            "x": 7.68,
+            "y": 5.715155846048237,
+            "heading": -2.6675793503802873e-18,
+            "angularVelocity": -2.692513973443352e-17,
+            "velocityX": 2.411941793716939e-19,
+            "velocityY": -0.8260917454276263,
+            "timestamp": 0.09907392781100396
+        },
+        {
+            "x": 7.68,
+            "y": 5.55146754125807,
+            "heading": -8.002991694377614e-18,
+            "angularVelocity": -5.385283961043983e-17,
+            "velocityX": 4.822926900034719e-19,
+            "velocityY": -1.6521834594306495,
+            "timestamp": 0.19814785562200793
+        },
+        {
+            "x": 7.68,
+            "y": 5.305935091856216,
+            "heading": -1.6007101079545214e-17,
+            "angularVelocity": -8.07892607262788e-17,
+            "velocityX": 7.23199861883135e-19,
+            "velocityY": -2.478275110584479,
+            "timestamp": 0.29722178343301187
+        },
+        {
+            "x": 7.68,
+            "y": 4.978558516522808,
+            "heading": -2.6680861930648114e-17,
+            "angularVelocity": -1.0773531530269989e-16,
+            "velocityX": 9.635330187135107e-19,
+            "velocityY": -3.3043665731908765,
+            "timestamp": 0.39629571124401586
+        },
+        {
+            "x": 7.68,
+            "y": 4.603767735073314,
+            "heading": -3.033567109313287e-17,
+            "angularVelocity": -3.688971703490728e-17,
+            "velocityX": 1.1102275506466142e-18,
+            "velocityY": -3.782940575087071,
+            "timestamp": 0.49536963905501985
+        },
+        {
+            "x": 7.68,
+            "y": 4.228976953623796,
+            "heading": -3.5529792774719166e-17,
+            "angularVelocity": -5.2426726146536295e-17,
+            "velocityX": 1.1087814850277944e-18,
+            "velocityY": -3.7829405750873115,
+            "timestamp": 0.5944435668660238
+        },
+        {
+            "x": 7.68,
+            "y": 3.8541861721742774,
+            "heading": -4.8775545061581196e-17,
+            "angularVelocity": -1.3369564101798844e-16,
+            "velocityX": 1.1066129122730268e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.6935174946770278
+        },
+        {
+            "x": 7.68,
+            "y": 3.4793953907247595,
+            "heading": -3.3383639047364345e-17,
+            "angularVelocity": 1.5535778538404392e-16,
+            "velocityX": 1.113993391664377e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.7925914224880318
+        },
+        {
+            "x": 7.68,
+            "y": 3.104604609275241,
+            "heading": -4.339521377299963e-18,
+            "angularVelocity": 2.9315601300747485e-16,
+            "velocityX": 1.114171335036007e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.8916653502990358
+        },
+        {
+            "x": 7.68,
+            "y": 2.7298138278257227,
+            "heading": 1.3174711336089981e-17,
+            "angularVelocity": 1.7677943229992181e-16,
+            "velocityX": 1.1158489583207238e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.9907392781100398
+        },
+        {
+            "x": 7.68,
+            "y": 2.3550230463762043,
+            "heading": 2.965421117619106e-17,
+            "angularVelocity": 1.6633538413393434e-16,
+            "velocityX": 1.1181357718618657e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 1.0898132059210437
+        },
+        {
+            "x": 7.68,
+            "y": 1.9802322649266861,
+            "heading": 2.902288280090785e-17,
+            "angularVelocity": -6.372295813531433e-18,
+            "velocityX": 1.121394742098521e-18,
+            "velocityY": -3.7829405750873115,
+            "timestamp": 1.1888871337320477
+        },
+        {
+            "x": 7.68,
+            "y": 1.605441483477192,
+            "heading": 2.667314494747493e-17,
+            "angularVelocity": -2.3717015216307205e-17,
+            "velocityX": 1.1050727300415562e-18,
+            "velocityY": -3.782940575087071,
+            "timestamp": 1.2879610615430517
+        },
+        {
+            "x": 7.68,
+            "y": 1.2780649081437845,
+            "heading": 1.6004201006274906e-17,
+            "angularVelocity": -1.0768669595616384e-16,
+            "velocityX": 9.663666503471993e-19,
+            "velocityY": -3.304366573190877,
+            "timestamp": 1.3870349893540557
+        },
+        {
+            "x": 7.68,
+            "y": 1.0325324587419298,
+            "heading": 8.002125258512226e-18,
+            "angularVelocity": -8.076873426351199e-17,
+            "velocityX": 7.247750412790524e-19,
+            "velocityY": -2.478275110584479,
+            "timestamp": 1.4861089171650597
+        },
         {
-            "x": 7.680000100604838,
-            "y": 0.8688441539517371,
-            "heading": -1.9801599569632452e-8,
-            "angularVelocity": 4.016134309966653e-7,
-            "velocityX": -0.000001979780510399852,
-            "velocityY": -1.6521834594299551,
-            "timestamp": 1.5851828449762517
+            "x": 7.68,
+            "y": 0.868844153951763,
+            "heading": 2.6673738748743342e-18,
+            "angularVelocity": -5.384616822543564e-17,
+            "velocityX": 4.831833763651305e-19,
+            "velocityY": -1.6521834594306495,
+            "timestamp": 1.5851828449760637
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": -3.727510081119492e-33,
-            "angularVelocity": 1.998668869584927e-7,
-            "velocityX": -0.0000010154521677217573,
-            "velocityY": -0.8260917454272346,
-            "timestamp": 1.6842567727872675
+            "heading": 0,
+            "angularVelocity": -2.6923065773077948e-17,
+            "velocityX": 2.415916928361511e-19,
+            "velocityY": -0.8260917454276263,
+            "timestamp": 1.6842567727870676
         },
         {
             "x": 7.68,
             "y": 0.787,
             "heading": 0,
-            "angularVelocity": 1.3621739357478042e-33,
-            "velocityX": 0,
-            "velocityY": 1.5851260517441878e-39,
-            "timestamp": 1.7833307005982832
+            "angularVelocity": 0,
+            "velocityX": -1.1487176577652646e-33,
+            "velocityY": 0,
+            "timestamp": 1.7833307005980716
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.traj
index df939479..3fd40c6e 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.traj	
@@ -4,172 +4,172 @@
             "x": 7.68,
             "y": 5.797,
             "heading": 0,
-            "angularVelocity": 1.5719252729433342e-31,
-            "velocityX": 9.448616775755806e-32,
-            "velocityY": 5.840312874632397e-38,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -1.2882449649302714e-32,
             "timestamp": 0
         },
         {
-            "x": 7.680000115633079,
-            "y": 5.715155846048254,
-            "heading": -1.667708661616254e-8,
-            "angularVelocity": -1.683296946208156e-7,
-            "velocityX": 0.0000011671392662331341,
-            "velocityY": -0.8260917454273093,
-            "timestamp": 0.0990739278110157
-        },
-        {
-            "x": 7.68000033829167,
-            "y": 5.551467541258106,
-            "heading": -5.4122540412155035e-8,
-            "angularVelocity": -3.7795460569235657e-7,
-            "velocityX": 0.0000022473982686017914,
-            "velocityY": -1.6521834594301872,
-            "timestamp": 0.1981478556220314
-        },
-        {
-            "x": 7.680000659202128,
-            "y": 5.305935091856244,
-            "heading": -1.1439481389604968e-7,
-            "angularVelocity": -6.083564268701089e-7,
-            "velocityX": 0.000003239100713146307,
-            "velocityY": -2.4782751105841556,
-            "timestamp": 0.2972217834330471
-        },
-        {
-            "x": 7.680001069396323,
-            "y": 4.978558516522715,
-            "heading": -1.9685469205745493e-7,
-            "angularVelocity": -8.323062817894511e-7,
-            "velocityX": 0.000004140283484089047,
-            "velocityY": -3.304366573191614,
-            "timestamp": 0.3962957112440628
-        },
-        {
-            "x": 7.680001393540419,
-            "y": 4.603767735073282,
-            "heading": -1.9685469402018675e-7,
-            "angularVelocity": -2.3695928725495928e-14,
-            "velocityX": 0.0000032717397250257404,
-            "velocityY": -3.782940575085891,
-            "timestamp": 0.4953696390550785
-        },
-        {
-            "x": 7.680001631663456,
-            "y": 4.228976953623768,
-            "heading": -1.9685469983118006e-7,
-            "angularVelocity": -6.145037083886684e-14,
-            "velocityX": 0.0000024034882723677366,
-            "velocityY": -3.782940575086774,
-            "timestamp": 0.5944435668660942
-        },
-        {
-            "x": 7.680001783604786,
-            "y": 3.854186172174223,
-            "heading": -1.9685470647072664e-7,
-            "angularVelocity": -7.008912946454424e-14,
-            "velocityX": 0.0000015336156103104506,
-            "velocityY": -3.78294057508712,
-            "timestamp": 0.6935174946771099
-        },
-        {
-            "x": 7.680001849310107,
-            "y": 3.479395390724661,
-            "heading": -1.9685471357202238e-7,
-            "angularVelocity": -7.568241713719523e-14,
-            "velocityX": 6.631948562175283e-7,
-            "velocityY": -3.782940575087319,
-            "timestamp": 0.7925914224881256
-        },
-        {
-            "x": 7.680001829148679,
-            "y": 3.104604609275093,
-            "heading": -1.968547206040393e-7,
-            "angularVelocity": -7.70206577850091e-14,
-            "velocityX": -2.0349878104492754e-7,
-            "velocityY": -3.7829405750873777,
-            "timestamp": 0.8916653502991414
-        },
-        {
-            "x": 7.680001725125185,
-            "y": 2.729813827825532,
-            "heading": -1.968547288641082e-7,
-            "angularVelocity": -9.134372064218233e-14,
-            "velocityX": -0.0000010499582046587897,
-            "velocityY": -3.7829405750872978,
-            "timestamp": 0.9907392781101572
-        },
-        {
-            "x": 7.680001540827678,
-            "y": 2.3550230463759907,
-            "heading": -1.9685473554016492e-7,
-            "angularVelocity": -7.335754268551387e-14,
-            "velocityX": -0.0000018602017039855543,
-            "velocityY": -3.7829405750870753,
-            "timestamp": 1.089813205921173
-        },
-        {
-            "x": 7.680001281411145,
-            "y": 1.980232264926482,
-            "heading": -1.9685473620018866e-7,
-            "angularVelocity": -6.8770489517299035e-15,
-            "velocityX": -0.0000026184135239642236,
-            "velocityY": -3.782940575086706,
-            "timestamp": 1.1888871337321887
-        },
-        {
-            "x": 7.680000953971803,
-            "y": 1.6054414834770692,
-            "heading": -1.968547355090094e-7,
-            "angularVelocity": 1.0827757762815176e-14,
-            "velocityX": -0.000003305000098197799,
-            "velocityY": -3.7829405750856786,
-            "timestamp": 1.2879610615432044
-        },
-        {
-            "x": 7.6800005831051426,
-            "y": 1.2780649081436681,
-            "heading": -1.1900708358832985e-7,
-            "angularVelocity": 7.857529072428817e-7,
-            "velocityX": -0.0000037433322544868336,
-            "velocityY": -3.30436657319033,
-            "timestamp": 1.3870349893542202
-        },
-        {
-            "x": 7.680000296749478,
-            "y": 1.0325324587418607,
-            "heading": -5.959102520162259e-8,
-            "angularVelocity": 5.997142524637507e-7,
-            "velocityX": -0.000002890322961269868,
-            "velocityY": -2.478275110583629,
-            "timestamp": 1.486108917165236
-        },
+            "x": 7.68,
+            "y": 5.715155846048237,
+            "heading": -2.6675793503802873e-18,
+            "angularVelocity": -2.692513973443352e-17,
+            "velocityX": 2.411941793716939e-19,
+            "velocityY": -0.8260917454276263,
+            "timestamp": 0.09907392781100396
+        },
+        {
+            "x": 7.68,
+            "y": 5.55146754125807,
+            "heading": -8.002991694377614e-18,
+            "angularVelocity": -5.385283961043983e-17,
+            "velocityX": 4.822926900034719e-19,
+            "velocityY": -1.6521834594306495,
+            "timestamp": 0.19814785562200793
+        },
+        {
+            "x": 7.68,
+            "y": 5.305935091856216,
+            "heading": -1.6007101079545214e-17,
+            "angularVelocity": -8.07892607262788e-17,
+            "velocityX": 7.23199861883135e-19,
+            "velocityY": -2.478275110584479,
+            "timestamp": 0.29722178343301187
+        },
+        {
+            "x": 7.68,
+            "y": 4.978558516522808,
+            "heading": -2.6680861930648114e-17,
+            "angularVelocity": -1.0773531530269989e-16,
+            "velocityX": 9.635330187135107e-19,
+            "velocityY": -3.3043665731908765,
+            "timestamp": 0.39629571124401586
+        },
+        {
+            "x": 7.68,
+            "y": 4.603767735073314,
+            "heading": -3.033567109313287e-17,
+            "angularVelocity": -3.688971703490728e-17,
+            "velocityX": 1.1102275506466142e-18,
+            "velocityY": -3.782940575087071,
+            "timestamp": 0.49536963905501985
+        },
+        {
+            "x": 7.68,
+            "y": 4.228976953623796,
+            "heading": -3.5529792774719166e-17,
+            "angularVelocity": -5.2426726146536295e-17,
+            "velocityX": 1.1087814850277944e-18,
+            "velocityY": -3.7829405750873115,
+            "timestamp": 0.5944435668660238
+        },
+        {
+            "x": 7.68,
+            "y": 3.8541861721742774,
+            "heading": -4.8775545061581196e-17,
+            "angularVelocity": -1.3369564101798844e-16,
+            "velocityX": 1.1066129122730268e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.6935174946770278
+        },
+        {
+            "x": 7.68,
+            "y": 3.4793953907247595,
+            "heading": -3.3383639047364345e-17,
+            "angularVelocity": 1.5535778538404392e-16,
+            "velocityX": 1.113993391664377e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.7925914224880318
+        },
+        {
+            "x": 7.68,
+            "y": 3.104604609275241,
+            "heading": -4.339521377299963e-18,
+            "angularVelocity": 2.9315601300747485e-16,
+            "velocityX": 1.114171335036007e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.8916653502990358
+        },
+        {
+            "x": 7.68,
+            "y": 2.7298138278257227,
+            "heading": 1.3174711336089981e-17,
+            "angularVelocity": 1.7677943229992181e-16,
+            "velocityX": 1.1158489583207238e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.9907392781100398
+        },
+        {
+            "x": 7.68,
+            "y": 2.3550230463762043,
+            "heading": 2.965421117619106e-17,
+            "angularVelocity": 1.6633538413393434e-16,
+            "velocityX": 1.1181357718618657e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 1.0898132059210437
+        },
+        {
+            "x": 7.68,
+            "y": 1.9802322649266861,
+            "heading": 2.902288280090785e-17,
+            "angularVelocity": -6.372295813531433e-18,
+            "velocityX": 1.121394742098521e-18,
+            "velocityY": -3.7829405750873115,
+            "timestamp": 1.1888871337320477
+        },
+        {
+            "x": 7.68,
+            "y": 1.605441483477192,
+            "heading": 2.667314494747493e-17,
+            "angularVelocity": -2.3717015216307205e-17,
+            "velocityX": 1.1050727300415562e-18,
+            "velocityY": -3.782940575087071,
+            "timestamp": 1.2879610615430517
+        },
+        {
+            "x": 7.68,
+            "y": 1.2780649081437845,
+            "heading": 1.6004201006274906e-17,
+            "angularVelocity": -1.0768669595616384e-16,
+            "velocityX": 9.663666503471993e-19,
+            "velocityY": -3.304366573190877,
+            "timestamp": 1.3870349893540557
+        },
+        {
+            "x": 7.68,
+            "y": 1.0325324587419298,
+            "heading": 8.002125258512226e-18,
+            "angularVelocity": -8.076873426351199e-17,
+            "velocityX": 7.247750412790524e-19,
+            "velocityY": -2.478275110584479,
+            "timestamp": 1.4861089171650597
+        },
         {
-            "x": 7.680000100604838,
-            "y": 0.8688441539517371,
-            "heading": -1.9801599569632452e-8,
-            "angularVelocity": 4.016134309966653e-7,
-            "velocityX": -0.000001979780510399852,
-            "velocityY": -1.6521834594299551,
-            "timestamp": 1.5851828449762517
+            "x": 7.68,
+            "y": 0.868844153951763,
+            "heading": 2.6673738748743342e-18,
+            "angularVelocity": -5.384616822543564e-17,
+            "velocityX": 4.831833763651305e-19,
+            "velocityY": -1.6521834594306495,
+            "timestamp": 1.5851828449760637
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": -3.727510081119492e-33,
-            "angularVelocity": 1.998668869584927e-7,
-            "velocityX": -0.0000010154521677217573,
-            "velocityY": -0.8260917454272346,
-            "timestamp": 1.6842567727872675
+            "heading": 0,
+            "angularVelocity": -2.6923065773077948e-17,
+            "velocityX": 2.415916928361511e-19,
+            "velocityY": -0.8260917454276263,
+            "timestamp": 1.6842567727870676
         },
         {
             "x": 7.68,
             "y": 0.787,
             "heading": 0,
-            "angularVelocity": 1.3621739357478042e-33,
-            "velocityX": 0,
-            "velocityY": 1.5851260517441878e-39,
-            "timestamp": 1.7833307005982832
+            "angularVelocity": 0,
+            "velocityX": -1.1487176577652646e-33,
+            "velocityY": 0,
+            "timestamp": 1.7833307005980716
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.1.traj
index 4ab726d7..6cbc4c1d 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.1.traj	
@@ -3,128 +3,128 @@
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": -2.09363537243035e-32,
-            "angularVelocity": -1.4612041776101527e-32,
-            "velocityX": -1.1521781181914207e-34,
-            "velocityY": -1.3541198100986985e-32,
+            "heading": 0,
+            "angularVelocity": 7.776001513534033e-33,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.620476774011981,
-            "y": 5.828095362884936,
-            "heading": -0.014618875325545894,
-            "angularVelocity": -0.16274311877436243,
-            "velocityX": -0.6626361618854291,
-            "velocityY": 0.346165913767109,
-            "timestamp": 0.08982791675397632
+            "x": 7.620476769374415,
+            "y": 5.82809535402629,
+            "heading": -0.014618874829729222,
+            "angularVelocity": -0.16274311325472623,
+            "velocityX": -0.6626362135126213,
+            "velocityY": 0.34616581514913597,
+            "timestamp": 0.0898279167539809
         },
         {
-            "x": 7.50139830253108,
-            "y": 5.890300516939614,
-            "heading": -0.04163561640032371,
-            "angularVelocity": -0.3007610779705845,
-            "velocityX": -1.3256287775996987,
-            "velocityY": 0.692492448923733,
-            "timestamp": 0.17965583350795264
+            "x": 7.5013982904113785,
+            "y": 5.890300493780534,
+            "heading": -0.04163561529349077,
+            "angularVelocity": -0.30076107116849365,
+            "velocityX": -1.3256288608937272,
+            "velocityY": 0.6924922897255835,
+            "timestamp": 0.1796558335079618
         },
         {
-            "x": 7.322721643819432,
-            "y": 5.983633497452849,
-            "heading": -0.07726722942321354,
-            "angularVelocity": -0.3966652496292318,
-            "velocityX": -1.9890994377729274,
-            "velocityY": 1.0390197600692306,
-            "timestamp": 0.26948375026192894
+            "x": 7.3227216232188646,
+            "y": 5.983633458070523,
+            "heading": -0.07726722819497155,
+            "angularVelocity": -0.39666524827763777,
+            "velocityX": -1.9890995321851956,
+            "velocityY": 1.0390195794655717,
+            "timestamp": 0.2694837502619427
         },
         {
-            "x": 7.084404587107726,
-            "y": 6.108106334427675,
-            "heading": -0.11365614278993857,
-            "angularVelocity": -0.4050958174437935,
-            "velocityX": -2.6530400049732523,
-            "velocityY": 1.3856809939802615,
-            "timestamp": 0.3593116670159053
+            "x": 7.084404558965627,
+            "y": 6.108106280619183,
+            "heading": -0.11365614248079976,
+            "angularVelocity": -0.40509582767559393,
+            "velocityX": -2.6530400889284445,
+            "velocityY": 1.3856808333823862,
+            "timestamp": 0.3593116670159236
         },
         {
-            "x": 6.786764353262062,
-            "y": 6.2635080892247865,
-            "heading": -0.12453213530374094,
-            "angularVelocity": -0.12107586268075081,
-            "velocityX": -3.313449143664897,
-            "velocityY": 1.7299939752886722,
-            "timestamp": 0.4491395837698816
+            "x": 6.786764320803032,
+            "y": 6.263508027124416,
+            "heading": -0.12453213515982267,
+            "angularVelocity": -0.12107586452004557,
+            "velocityX": -3.313449191722499,
+            "velocityY": 1.7299938829800985,
+            "timestamp": 0.4491395837699045
         },
         {
-            "x": 6.485598483517562,
-            "y": 6.4209007383980845,
-            "heading": -0.12453214800780214,
-            "angularVelocity": -1.4142664848458141e-7,
-            "velocityX": -3.352697920951937,
-            "velocityY": 1.752157401182633,
-            "timestamp": 0.5389675005238579
+            "x": 6.485598483527544,
+            "y": 6.42090073842616,
+            "heading": -0.1245321478638912,
+            "angularVelocity": -1.4142672994383305e-7,
+            "velocityX": -3.3526975594938513,
+            "velocityY": 1.7521580928210598,
+            "timestamp": 0.5389675005238854
         },
         {
-            "x": 6.164419225797539,
-            "y": 6.531883674906018,
-            "heading": -0.12453214718798315,
-            "angularVelocity": 9.126550019087707e-9,
-            "velocityX": -3.575494894306401,
-            "velocityY": 1.2355060711459842,
-            "timestamp": 0.6287954172778342
+            "x": 6.164419225817841,
+            "y": 6.531883674964022,
+            "heading": -0.12453214704403566,
+            "angularVelocity": 9.126957138704023e-9,
+            "velocityX": -3.575494894191328,
+            "velocityY": 1.2355060714790946,
+            "timestamp": 0.6287954172778664
         },
         {
-            "x": 5.83853002564033,
-            "y": 6.576482035264996,
-            "heading": -0.08897109874736252,
-            "angularVelocity": 0.3958796967096147,
-            "velocityX": -3.6279278417395133,
-            "velocityY": 0.4964866376799699,
-            "timestamp": 0.7186233340318104
+            "x": 5.838530026083828,
+            "y": 6.576482035403971,
+            "heading": -0.08897109734241883,
+            "angularVelocity": 0.39587971074750417,
+            "velocityX": -3.62792783702814,
+            "velocityY": 0.4964866385813549,
+            "timestamp": 0.7186233340318473
         },
         {
-            "x": 5.573146790095341,
-            "y": 6.601508859331819,
-            "heading": 0.02708371250022136,
-            "angularVelocity": 1.2919681925323805,
-            "velocityX": -2.9543514436812472,
-            "velocityY": 0.278608532527451,
-            "timestamp": 0.8084512507857867
+            "x": 5.57314678987786,
+            "y": 6.601508855183474,
+            "heading": 0.02708371106201491,
+            "angularVelocity": 1.2919681608812392,
+            "velocityX": -2.9543514510393742,
+            "velocityY": 0.27860848479928674,
+            "timestamp": 0.8084512507858282
         },
         {
-            "x": 5.3743330314019,
-            "y": 6.61692927227141,
-            "heading": 0.137026184669163,
-            "angularVelocity": 1.2239232094189134,
-            "velocityX": -2.213273622252188,
-            "velocityY": 0.1716661534278421,
-            "timestamp": 0.898279167539763
+            "x": 5.3743330310505755,
+            "y": 6.616929267325104,
+            "heading": 0.1370261836602847,
+            "angularVelocity": 1.2239232141983023,
+            "velocityX": -2.213273623742076,
+            "velocityY": 0.17166614454461948,
+            "timestamp": 0.8982791675398092
         },
         {
-            "x": 5.242037120454666,
-            "y": 6.625930660217185,
-            "heading": 0.21902488374006282,
-            "angularVelocity": 0.912842043253442,
-            "velocityX": -1.4727705565027283,
-            "velocityY": 0.10020702105814829,
-            "timestamp": 0.9881070842937393
+            "x": 5.242037120273562,
+            "y": 6.625930657483227,
+            "heading": 0.21902488355780253,
+            "angularVelocity": 0.9128420524556349,
+            "velocityX": -1.4727705546076917,
+            "velocityY": 0.10020704568687104,
+            "timestamp": 0.9881070842937901
         },
         {
             "x": 5.176,
             "y": 6.63,
             "heading": 0.263,
-            "angularVelocity": 0.4895484371565439,
-            "velocityX": -0.7351514188571259,
-            "velocityY": 0.04530150458636799,
-            "timestamp": 1.0779350010477156
+            "angularVelocity": 0.48954843918551255,
+            "velocityX": -0.7351514168409599,
+            "velocityY": 0.0453015350218741,
+            "timestamp": 1.077935001047771
         },
         {
             "x": 5.176,
             "y": 6.63,
             "heading": 0.263,
-            "angularVelocity": 0,
-            "velocityX": 4.777602762336884e-34,
-            "velocityY": 2.1640483822800166e-33,
-            "timestamp": 1.167762917801692
+            "angularVelocity": -4.780109867389741e-33,
+            "velocityX": 0,
+            "velocityY": -5.233682494366388e-34,
+            "timestamp": 1.167762917801752
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.traj
index 4ab726d7..6cbc4c1d 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.traj	
@@ -3,128 +3,128 @@
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": -2.09363537243035e-32,
-            "angularVelocity": -1.4612041776101527e-32,
-            "velocityX": -1.1521781181914207e-34,
-            "velocityY": -1.3541198100986985e-32,
+            "heading": 0,
+            "angularVelocity": 7.776001513534033e-33,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.620476774011981,
-            "y": 5.828095362884936,
-            "heading": -0.014618875325545894,
-            "angularVelocity": -0.16274311877436243,
-            "velocityX": -0.6626361618854291,
-            "velocityY": 0.346165913767109,
-            "timestamp": 0.08982791675397632
+            "x": 7.620476769374415,
+            "y": 5.82809535402629,
+            "heading": -0.014618874829729222,
+            "angularVelocity": -0.16274311325472623,
+            "velocityX": -0.6626362135126213,
+            "velocityY": 0.34616581514913597,
+            "timestamp": 0.0898279167539809
         },
         {
-            "x": 7.50139830253108,
-            "y": 5.890300516939614,
-            "heading": -0.04163561640032371,
-            "angularVelocity": -0.3007610779705845,
-            "velocityX": -1.3256287775996987,
-            "velocityY": 0.692492448923733,
-            "timestamp": 0.17965583350795264
+            "x": 7.5013982904113785,
+            "y": 5.890300493780534,
+            "heading": -0.04163561529349077,
+            "angularVelocity": -0.30076107116849365,
+            "velocityX": -1.3256288608937272,
+            "velocityY": 0.6924922897255835,
+            "timestamp": 0.1796558335079618
         },
         {
-            "x": 7.322721643819432,
-            "y": 5.983633497452849,
-            "heading": -0.07726722942321354,
-            "angularVelocity": -0.3966652496292318,
-            "velocityX": -1.9890994377729274,
-            "velocityY": 1.0390197600692306,
-            "timestamp": 0.26948375026192894
+            "x": 7.3227216232188646,
+            "y": 5.983633458070523,
+            "heading": -0.07726722819497155,
+            "angularVelocity": -0.39666524827763777,
+            "velocityX": -1.9890995321851956,
+            "velocityY": 1.0390195794655717,
+            "timestamp": 0.2694837502619427
         },
         {
-            "x": 7.084404587107726,
-            "y": 6.108106334427675,
-            "heading": -0.11365614278993857,
-            "angularVelocity": -0.4050958174437935,
-            "velocityX": -2.6530400049732523,
-            "velocityY": 1.3856809939802615,
-            "timestamp": 0.3593116670159053
+            "x": 7.084404558965627,
+            "y": 6.108106280619183,
+            "heading": -0.11365614248079976,
+            "angularVelocity": -0.40509582767559393,
+            "velocityX": -2.6530400889284445,
+            "velocityY": 1.3856808333823862,
+            "timestamp": 0.3593116670159236
         },
         {
-            "x": 6.786764353262062,
-            "y": 6.2635080892247865,
-            "heading": -0.12453213530374094,
-            "angularVelocity": -0.12107586268075081,
-            "velocityX": -3.313449143664897,
-            "velocityY": 1.7299939752886722,
-            "timestamp": 0.4491395837698816
+            "x": 6.786764320803032,
+            "y": 6.263508027124416,
+            "heading": -0.12453213515982267,
+            "angularVelocity": -0.12107586452004557,
+            "velocityX": -3.313449191722499,
+            "velocityY": 1.7299938829800985,
+            "timestamp": 0.4491395837699045
         },
         {
-            "x": 6.485598483517562,
-            "y": 6.4209007383980845,
-            "heading": -0.12453214800780214,
-            "angularVelocity": -1.4142664848458141e-7,
-            "velocityX": -3.352697920951937,
-            "velocityY": 1.752157401182633,
-            "timestamp": 0.5389675005238579
+            "x": 6.485598483527544,
+            "y": 6.42090073842616,
+            "heading": -0.1245321478638912,
+            "angularVelocity": -1.4142672994383305e-7,
+            "velocityX": -3.3526975594938513,
+            "velocityY": 1.7521580928210598,
+            "timestamp": 0.5389675005238854
         },
         {
-            "x": 6.164419225797539,
-            "y": 6.531883674906018,
-            "heading": -0.12453214718798315,
-            "angularVelocity": 9.126550019087707e-9,
-            "velocityX": -3.575494894306401,
-            "velocityY": 1.2355060711459842,
-            "timestamp": 0.6287954172778342
+            "x": 6.164419225817841,
+            "y": 6.531883674964022,
+            "heading": -0.12453214704403566,
+            "angularVelocity": 9.126957138704023e-9,
+            "velocityX": -3.575494894191328,
+            "velocityY": 1.2355060714790946,
+            "timestamp": 0.6287954172778664
         },
         {
-            "x": 5.83853002564033,
-            "y": 6.576482035264996,
-            "heading": -0.08897109874736252,
-            "angularVelocity": 0.3958796967096147,
-            "velocityX": -3.6279278417395133,
-            "velocityY": 0.4964866376799699,
-            "timestamp": 0.7186233340318104
+            "x": 5.838530026083828,
+            "y": 6.576482035403971,
+            "heading": -0.08897109734241883,
+            "angularVelocity": 0.39587971074750417,
+            "velocityX": -3.62792783702814,
+            "velocityY": 0.4964866385813549,
+            "timestamp": 0.7186233340318473
         },
         {
-            "x": 5.573146790095341,
-            "y": 6.601508859331819,
-            "heading": 0.02708371250022136,
-            "angularVelocity": 1.2919681925323805,
-            "velocityX": -2.9543514436812472,
-            "velocityY": 0.278608532527451,
-            "timestamp": 0.8084512507857867
+            "x": 5.57314678987786,
+            "y": 6.601508855183474,
+            "heading": 0.02708371106201491,
+            "angularVelocity": 1.2919681608812392,
+            "velocityX": -2.9543514510393742,
+            "velocityY": 0.27860848479928674,
+            "timestamp": 0.8084512507858282
         },
         {
-            "x": 5.3743330314019,
-            "y": 6.61692927227141,
-            "heading": 0.137026184669163,
-            "angularVelocity": 1.2239232094189134,
-            "velocityX": -2.213273622252188,
-            "velocityY": 0.1716661534278421,
-            "timestamp": 0.898279167539763
+            "x": 5.3743330310505755,
+            "y": 6.616929267325104,
+            "heading": 0.1370261836602847,
+            "angularVelocity": 1.2239232141983023,
+            "velocityX": -2.213273623742076,
+            "velocityY": 0.17166614454461948,
+            "timestamp": 0.8982791675398092
         },
         {
-            "x": 5.242037120454666,
-            "y": 6.625930660217185,
-            "heading": 0.21902488374006282,
-            "angularVelocity": 0.912842043253442,
-            "velocityX": -1.4727705565027283,
-            "velocityY": 0.10020702105814829,
-            "timestamp": 0.9881070842937393
+            "x": 5.242037120273562,
+            "y": 6.625930657483227,
+            "heading": 0.21902488355780253,
+            "angularVelocity": 0.9128420524556349,
+            "velocityX": -1.4727705546076917,
+            "velocityY": 0.10020704568687104,
+            "timestamp": 0.9881070842937901
         },
         {
             "x": 5.176,
             "y": 6.63,
             "heading": 0.263,
-            "angularVelocity": 0.4895484371565439,
-            "velocityX": -0.7351514188571259,
-            "velocityY": 0.04530150458636799,
-            "timestamp": 1.0779350010477156
+            "angularVelocity": 0.48954843918551255,
+            "velocityX": -0.7351514168409599,
+            "velocityY": 0.0453015350218741,
+            "timestamp": 1.077935001047771
         },
         {
             "x": 5.176,
             "y": 6.63,
             "heading": 0.263,
-            "angularVelocity": 0,
-            "velocityX": 4.777602762336884e-34,
-            "velocityY": 2.1640483822800166e-33,
-            "timestamp": 1.167762917801692
+            "angularVelocity": -4.780109867389741e-33,
+            "velocityX": 0,
+            "velocityY": -5.233682494366388e-34,
+            "timestamp": 1.167762917801752
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.1.traj
new file mode 100644
index 00000000..4e0731ce
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.1.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 1.3211551091100433e-31,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6375302942972665,
+            "y": 5.822387711771769,
+            "heading": 0.010500881646553203,
+            "angularVelocity": 0.13619602846032355,
+            "velocityX": -0.5508304389364825,
+            "velocityY": 0.32927763890619355,
+            "timestamp": 0.07710123243140166
+        },
+        {
+            "x": 7.5525908811480225,
+            "y": 5.873163180716977,
+            "heading": 0.031500279151306286,
+            "angularVelocity": 0.27236137273728567,
+            "velocityX": -1.1016609004897018,
+            "velocityY": 0.6585558666702785,
+            "timestamp": 0.1542024648628033
+        },
+        {
+            "x": 7.4251817593524025,
+            "y": 5.949326484276257,
+            "heading": 0.06299451206522548,
+            "angularVelocity": 0.40847898173275193,
+            "velocityX": -1.6524913776051289,
+            "velocityY": 0.987835098836381,
+            "timestamp": 0.23130369729420497
+        },
+        {
+            "x": 7.255302939480135,
+            "y": 6.050877733979633,
+            "heading": 0.10497913316622995,
+            "angularVelocity": 0.5445389104299847,
+            "velocityX": -2.203321717631576,
+            "velocityY": 1.3171157775425744,
+            "timestamp": 0.3084049297256066
+        },
+        {
+            "x": 7.042954462706059,
+            "y": 6.177817072431174,
+            "heading": 0.1574496496087124,
+            "angularVelocity": 0.6805405670936119,
+            "velocityX": -2.754151523621958,
+            "velocityY": 1.6463983058180132,
+            "timestamp": 0.3855061621570083
+        },
+        {
+            "x": 6.792607460054783,
+            "y": 6.3274746833219355,
+            "heading": 0.15744965721454035,
+            "angularVelocity": 9.864729365588338e-8,
+            "velocityX": -3.246990933303352,
+            "velocityY": 1.941053419916652,
+            "timestamp": 0.46260739458840994
+        },
+        {
+            "x": 6.542259846259959,
+            "y": 6.4771312718889495,
+            "heading": 0.15744966481969572,
+            "angularVelocity": 9.863857053631023e-8,
+            "velocityX": -3.246998859811527,
+            "velocityY": 1.9410401604172332,
+            "timestamp": 0.5397086270198116
+        },
+        {
+            "x": 6.273256747030451,
+            "y": 6.589862608843688,
+            "heading": 0.1574496727775284,
+            "angularVelocity": 1.0321278187268036e-7,
+            "velocityX": -3.4889597837342676,
+            "velocityY": 1.4621210764047974,
+            "timestamp": 0.6168098594512132
+        },
+        {
+            "x": 5.98904964850956,
+            "y": 6.655416753006686,
+            "heading": 0.15744969119167346,
+            "angularVelocity": 2.3883074855376557e-7,
+            "velocityX": -3.686155065987494,
+            "velocityY": 0.8502347121535809,
+            "timestamp": 0.6939110918826149
+        },
+        {
+            "x": 5.6978465137603145,
+            "y": 6.6719003531074685,
+            "heading": 0.15744999446473232,
+            "angularVelocity": 0.00000393343983387992,
+            "velocityX": -3.7768933850484796,
+            "velocityY": 0.21379165521703997,
+            "timestamp": 0.7710123243140166
+        },
+        {
+            "x": 5.40805721282959,
+            "y": 6.638837814331055,
+            "heading": 0.15745035498223803,
+            "angularVelocity": 0.00000467589809324531,
+            "velocityX": -3.7585560151526085,
+            "velocityY": -0.428819848059242,
+            "timestamp": 0.8481135567454182
+        },
+        {
+            "x": 5.104307335648147,
+            "y": 6.562247743535093,
+            "heading": 0.18966206475475966,
+            "angularVelocity": 0.37702586381684944,
+            "velocityX": -3.555277277652879,
+            "velocityY": -0.896457772827513,
+            "timestamp": 0.9335498964342235
+        },
+        {
+            "x": 4.836190414730584,
+            "y": 6.438293970082571,
+            "heading": 0.27176288986041636,
+            "angularVelocity": 0.9609590650149814,
+            "velocityX": -3.1382070193334055,
+            "velocityY": -1.4508319750590115,
+            "timestamp": 1.0189862361230289
+        },
+        {
+            "x": 4.616497381460216,
+            "y": 6.278221072521664,
+            "heading": 0.3274582685749672,
+            "angularVelocity": 0.6518933151562509,
+            "velocityX": -2.5714237532949347,
+            "velocityY": -1.8735926438791781,
+            "timestamp": 1.1044225758118342
+        },
+        {
+            "x": 4.449006665280017,
+            "y": 6.09487376587715,
+            "heading": 0.3096216512620487,
+            "angularVelocity": -0.2087708506460702,
+            "velocityX": -1.960415401575849,
+            "velocityY": -2.1460107878256482,
+            "timestamp": 1.1898589155006396
+        },
+        {
+            "x": 4.339814814870372,
+            "y": 5.903698766708507,
+            "heading": 0.23521329843339078,
+            "angularVelocity": -0.8709215902704173,
+            "velocityX": -1.2780492564097095,
+            "velocityY": -2.237630964352897,
+            "timestamp": 1.275295255189445
+        },
+        {
+            "x": 4.286146359443328,
+            "y": 5.736442488850756,
+            "heading": 0.1356278137676922,
+            "angularVelocity": -1.1656103834554516,
+            "velocityX": -0.628168945703036,
+            "velocityY": -1.957671389796991,
+            "timestamp": 1.3607315948782504
+        },
+        {
+            "x": 4.266951931468271,
+            "y": 5.619203136466468,
+            "heading": 0.049466079264437586,
+            "angularVelocity": -1.0084904715849423,
+            "velocityX": -0.2246635102225946,
+            "velocityY": -1.372242219309977,
+            "timestamp": 1.4461679345670557
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 4.182976778726496e-34,
+            "angularVelocity": -0.578981724224301,
+            "velocityX": -0.046255861178807596,
+            "velocityY": -0.6929502912005617,
+            "timestamp": 1.531604274255861
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -6.30298269203181e-33,
+            "timestamp": 1.6170406139446665
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.traj
new file mode 100644
index 00000000..4e0731ce
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 1.3211551091100433e-31,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6375302942972665,
+            "y": 5.822387711771769,
+            "heading": 0.010500881646553203,
+            "angularVelocity": 0.13619602846032355,
+            "velocityX": -0.5508304389364825,
+            "velocityY": 0.32927763890619355,
+            "timestamp": 0.07710123243140166
+        },
+        {
+            "x": 7.5525908811480225,
+            "y": 5.873163180716977,
+            "heading": 0.031500279151306286,
+            "angularVelocity": 0.27236137273728567,
+            "velocityX": -1.1016609004897018,
+            "velocityY": 0.6585558666702785,
+            "timestamp": 0.1542024648628033
+        },
+        {
+            "x": 7.4251817593524025,
+            "y": 5.949326484276257,
+            "heading": 0.06299451206522548,
+            "angularVelocity": 0.40847898173275193,
+            "velocityX": -1.6524913776051289,
+            "velocityY": 0.987835098836381,
+            "timestamp": 0.23130369729420497
+        },
+        {
+            "x": 7.255302939480135,
+            "y": 6.050877733979633,
+            "heading": 0.10497913316622995,
+            "angularVelocity": 0.5445389104299847,
+            "velocityX": -2.203321717631576,
+            "velocityY": 1.3171157775425744,
+            "timestamp": 0.3084049297256066
+        },
+        {
+            "x": 7.042954462706059,
+            "y": 6.177817072431174,
+            "heading": 0.1574496496087124,
+            "angularVelocity": 0.6805405670936119,
+            "velocityX": -2.754151523621958,
+            "velocityY": 1.6463983058180132,
+            "timestamp": 0.3855061621570083
+        },
+        {
+            "x": 6.792607460054783,
+            "y": 6.3274746833219355,
+            "heading": 0.15744965721454035,
+            "angularVelocity": 9.864729365588338e-8,
+            "velocityX": -3.246990933303352,
+            "velocityY": 1.941053419916652,
+            "timestamp": 0.46260739458840994
+        },
+        {
+            "x": 6.542259846259959,
+            "y": 6.4771312718889495,
+            "heading": 0.15744966481969572,
+            "angularVelocity": 9.863857053631023e-8,
+            "velocityX": -3.246998859811527,
+            "velocityY": 1.9410401604172332,
+            "timestamp": 0.5397086270198116
+        },
+        {
+            "x": 6.273256747030451,
+            "y": 6.589862608843688,
+            "heading": 0.1574496727775284,
+            "angularVelocity": 1.0321278187268036e-7,
+            "velocityX": -3.4889597837342676,
+            "velocityY": 1.4621210764047974,
+            "timestamp": 0.6168098594512132
+        },
+        {
+            "x": 5.98904964850956,
+            "y": 6.655416753006686,
+            "heading": 0.15744969119167346,
+            "angularVelocity": 2.3883074855376557e-7,
+            "velocityX": -3.686155065987494,
+            "velocityY": 0.8502347121535809,
+            "timestamp": 0.6939110918826149
+        },
+        {
+            "x": 5.6978465137603145,
+            "y": 6.6719003531074685,
+            "heading": 0.15744999446473232,
+            "angularVelocity": 0.00000393343983387992,
+            "velocityX": -3.7768933850484796,
+            "velocityY": 0.21379165521703997,
+            "timestamp": 0.7710123243140166
+        },
+        {
+            "x": 5.40805721282959,
+            "y": 6.638837814331055,
+            "heading": 0.15745035498223803,
+            "angularVelocity": 0.00000467589809324531,
+            "velocityX": -3.7585560151526085,
+            "velocityY": -0.428819848059242,
+            "timestamp": 0.8481135567454182
+        },
+        {
+            "x": 5.104307335648147,
+            "y": 6.562247743535093,
+            "heading": 0.18966206475475966,
+            "angularVelocity": 0.37702586381684944,
+            "velocityX": -3.555277277652879,
+            "velocityY": -0.896457772827513,
+            "timestamp": 0.9335498964342235
+        },
+        {
+            "x": 4.836190414730584,
+            "y": 6.438293970082571,
+            "heading": 0.27176288986041636,
+            "angularVelocity": 0.9609590650149814,
+            "velocityX": -3.1382070193334055,
+            "velocityY": -1.4508319750590115,
+            "timestamp": 1.0189862361230289
+        },
+        {
+            "x": 4.616497381460216,
+            "y": 6.278221072521664,
+            "heading": 0.3274582685749672,
+            "angularVelocity": 0.6518933151562509,
+            "velocityX": -2.5714237532949347,
+            "velocityY": -1.8735926438791781,
+            "timestamp": 1.1044225758118342
+        },
+        {
+            "x": 4.449006665280017,
+            "y": 6.09487376587715,
+            "heading": 0.3096216512620487,
+            "angularVelocity": -0.2087708506460702,
+            "velocityX": -1.960415401575849,
+            "velocityY": -2.1460107878256482,
+            "timestamp": 1.1898589155006396
+        },
+        {
+            "x": 4.339814814870372,
+            "y": 5.903698766708507,
+            "heading": 0.23521329843339078,
+            "angularVelocity": -0.8709215902704173,
+            "velocityX": -1.2780492564097095,
+            "velocityY": -2.237630964352897,
+            "timestamp": 1.275295255189445
+        },
+        {
+            "x": 4.286146359443328,
+            "y": 5.736442488850756,
+            "heading": 0.1356278137676922,
+            "angularVelocity": -1.1656103834554516,
+            "velocityX": -0.628168945703036,
+            "velocityY": -1.957671389796991,
+            "timestamp": 1.3607315948782504
+        },
+        {
+            "x": 4.266951931468271,
+            "y": 5.619203136466468,
+            "heading": 0.049466079264437586,
+            "angularVelocity": -1.0084904715849423,
+            "velocityX": -0.2246635102225946,
+            "velocityY": -1.372242219309977,
+            "timestamp": 1.4461679345670557
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 4.182976778726496e-34,
+            "angularVelocity": -0.578981724224301,
+            "velocityX": -0.046255861178807596,
+            "velocityY": -0.6929502912005617,
+            "timestamp": 1.531604274255861
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -6.30298269203181e-33,
+            "timestamp": 1.6170406139446665
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S4.1.traj
new file mode 100644
index 00000000..4efefc66
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S4.1.traj	
@@ -0,0 +1,256 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": -1.007826409753455e-39,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.661226431831969,
+            "y": 5.757681823957774,
+            "heading": -0.0001846118691686639,
+            "angularVelocity": -0.0025538708536768893,
+            "velocityX": -0.25970848342391806,
+            "velocityY": -0.5439170529292338,
+            "timestamp": 0.07228708096295171
+        },
+        {
+            "x": 7.623679292701499,
+            "y": 5.679045474684997,
+            "heading": -0.0005538375109882563,
+            "angularVelocity": -0.005107768039614528,
+            "velocityX": -0.5194170055049473,
+            "velocityY": -1.0878340669625215,
+            "timestamp": 0.14457416192590342
+        },
+        {
+            "x": 7.567358579979895,
+            "y": 5.561090955652203,
+            "heading": -0.0011076778331591435,
+            "angularVelocity": -0.0076616777824344475,
+            "velocityX": -0.779125563950612,
+            "velocityY": -1.6317510329853882,
+            "timestamp": 0.2168612428888551
+        },
+        {
+            "x": 7.492264291454711,
+            "y": 5.403818271607319,
+            "heading": -0.001846132228095339,
+            "angularVelocity": -0.010215579120073546,
+            "velocityX": -1.0388341530026832,
+            "velocityY": -2.175667933326667,
+            "timestamp": 0.28914832385180683
+        },
+        {
+            "x": 7.39839642615194,
+            "y": 5.207227430397753,
+            "heading": -0.0027691974892554232,
+            "angularVelocity": -0.012769436099282666,
+            "velocityX": -1.298542755528879,
+            "velocityY": -2.719584725109078,
+            "timestamp": 0.36143540481475855
+        },
+        {
+            "x": 7.285754988600711,
+            "y": 4.97131845200793,
+            "heading": -0.0038768638943260385,
+            "angularVelocity": -0.015323158582628504,
+            "velocityX": -1.5582512954003445,
+            "velocityY": -3.263501240432299,
+            "timestamp": 0.4337224857777103
+        },
+        {
+            "x": 7.167927209247298,
+            "y": 4.7245478329210355,
+            "heading": -0.0038768640537450113,
+            "angularVelocity": -2.205359112715029e-9,
+            "velocityX": -1.629997750411335,
+            "velocityY": -3.4137582511232942,
+            "timestamp": 0.506009566740662
+        },
+        {
+            "x": 7.050099366875978,
+            "y": 4.477777243923851,
+            "heading": -0.003876864213154645,
+            "angularVelocity": -2.2052299170164517e-9,
+            "velocityX": -1.6299986221840805,
+            "velocityY": -3.413757834870371,
+            "timestamp": 0.5782966477036137
+        },
+        {
+            "x": 6.932271561638543,
+            "y": 4.2310066371960025,
+            "heading": -0.0038768643725604667,
+            "angularVelocity": -2.2051771914474314e-9,
+            "velocityX": -1.629998108483918,
+            "velocityY": -3.413758080151585,
+            "timestamp": 0.6505837286665653
+        },
+        {
+            "x": 6.8252544845676155,
+            "y": 3.9793591274718993,
+            "heading": -0.0038768645333797187,
+            "angularVelocity": -2.2247301918048757e-9,
+            "velocityX": -1.4804454080221456,
+            "velocityY": -3.4812238421008592,
+            "timestamp": 0.722870809629517
+        },
+        {
+            "x": 6.7595626994044276,
+            "y": 3.7139091088268694,
+            "heading": -0.003876864707631206,
+            "angularVelocity": -2.410548124556829e-9,
+            "velocityX": -0.9087624550347577,
+            "velocityY": -3.6721640313720423,
+            "timestamp": 0.7951578905924687
+        },
+        {
+            "x": 6.7368645668029785,
+            "y": 3.4413950443267822,
+            "heading": -0.0038768649214938546,
+            "angularVelocity": -2.9585182521427424e-9,
+            "velocityX": -0.31399985030634436,
+            "velocityY": -3.7698861382956523,
+            "timestamp": 0.8674449715554203
+        },
+        {
+            "x": 6.763602448674763,
+            "y": 3.1456190677825386,
+            "heading": -0.0038769163309766456,
+            "angularVelocity": -6.548510424231184e-7,
+            "velocityX": 0.340585605327871,
+            "velocityY": -3.7675774205236117,
+            "timestamp": 0.9459505849856912
+        },
+        {
+            "x": 6.763837716877819,
+            "y": 2.855181760345028,
+            "heading": -0.02573615579182971,
+            "angularVelocity": -0.27844173818562123,
+            "velocityX": 0.0029968328731749647,
+            "velocityY": -3.699573759722024,
+            "timestamp": 1.0244561984159621
+        },
+        {
+            "x": 6.746691140594586,
+            "y": 2.576419895706607,
+            "heading": -0.08408576500639096,
+            "angularVelocity": -0.7432539746522423,
+            "velocityX": -0.2184121049950426,
+            "velocityY": -3.5508526391685153,
+            "timestamp": 1.102961811846233
+        },
+        {
+            "x": 6.69086397201793,
+            "y": 2.322743038831463,
+            "heading": -0.2046015958190934,
+            "angularVelocity": -1.5351237388870964,
+            "velocityX": -0.7111232705192682,
+            "velocityY": -3.2313212494092722,
+            "timestamp": 1.1814674252765038
+        },
+        {
+            "x": 6.586917702656428,
+            "y": 2.0871040226876985,
+            "heading": -0.32491541634118276,
+            "angularVelocity": -1.5325505433946174,
+            "velocityX": -1.324061615719081,
+            "velocityY": -3.001556269005664,
+            "timestamp": 1.2599730387067747
+        },
+        {
+            "x": 6.439847045017927,
+            "y": 1.879401635438828,
+            "heading": -0.448122685606705,
+            "angularVelocity": -1.5694071274910255,
+            "velocityX": -1.8733775995410846,
+            "velocityY": -2.6457010928696563,
+            "timestamp": 1.3384786521370455
+        },
+        {
+            "x": 6.25456019704424,
+            "y": 1.7060438180074393,
+            "heading": -0.5681346722700569,
+            "angularVelocity": -1.528705801018258,
+            "velocityX": -2.3601732395640638,
+            "velocityY": -2.2082219328859343,
+            "timestamp": 1.4169842655673164
+        },
+        {
+            "x": 6.049779045368227,
+            "y": 1.57972317182915,
+            "heading": -0.6633309119306381,
+            "angularVelocity": -1.2126042393788183,
+            "velocityX": -2.6084905617341763,
+            "velocityY": -1.6090651439911137,
+            "timestamp": 1.4954898789975872
+        },
+        {
+            "x": 5.840944041976222,
+            "y": 1.503518626439596,
+            "heading": -0.7208467997653518,
+            "angularVelocity": -0.7326340795464229,
+            "velocityX": -2.660128292322611,
+            "velocityY": -0.9706891273098467,
+            "timestamp": 1.573995492427858
+        },
+        {
+            "x": 5.636524773547887,
+            "y": 1.4770268115818306,
+            "heading": -0.7348631463149735,
+            "angularVelocity": -0.17853941822989652,
+            "velocityX": -2.603880913686534,
+            "velocityY": -0.33745121782018417,
+            "timestamp": 1.652501105858129
+        },
+        {
+            "x": 5.451777868175388,
+            "y": 1.4973120784666072,
+            "heading": -0.7197568228935092,
+            "angularVelocity": 0.19242348109134802,
+            "velocityX": -2.353295481686686,
+            "velocityY": 0.25839256581051945,
+            "timestamp": 1.7310067192883998
+        },
+        {
+            "x": 5.309771976878378,
+            "y": 1.5457401851903851,
+            "heading": -0.687868735789328,
+            "angularVelocity": 0.4061886241103559,
+            "velocityX": -1.808862896449324,
+            "velocityY": 0.6168744451222204,
+            "timestamp": 1.8095123327186706
+        },
+        {
+            "x": 5.2191255322417085,
+            "y": 1.592492981200916,
+            "heading": -0.6576952711638115,
+            "angularVelocity": 0.38434786134518467,
+            "velocityX": -1.154649211386418,
+            "velocityY": 0.5955344333696231,
+            "timestamp": 1.8880179461489415
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 0.22540134890517805,
+            "velocityX": -0.5493305555788384,
+            "velocityY": 0.35038282738234927,
+            "timestamp": 1.9665235595792123
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 0,
+            "velocityX": 6.617232561290069e-35,
+            "velocityY": 1.0472745338276337e-32,
+            "timestamp": 2.045029173009483
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S4.traj
new file mode 100644
index 00000000..4efefc66
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S4.traj	
@@ -0,0 +1,256 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": -1.007826409753455e-39,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.661226431831969,
+            "y": 5.757681823957774,
+            "heading": -0.0001846118691686639,
+            "angularVelocity": -0.0025538708536768893,
+            "velocityX": -0.25970848342391806,
+            "velocityY": -0.5439170529292338,
+            "timestamp": 0.07228708096295171
+        },
+        {
+            "x": 7.623679292701499,
+            "y": 5.679045474684997,
+            "heading": -0.0005538375109882563,
+            "angularVelocity": -0.005107768039614528,
+            "velocityX": -0.5194170055049473,
+            "velocityY": -1.0878340669625215,
+            "timestamp": 0.14457416192590342
+        },
+        {
+            "x": 7.567358579979895,
+            "y": 5.561090955652203,
+            "heading": -0.0011076778331591435,
+            "angularVelocity": -0.0076616777824344475,
+            "velocityX": -0.779125563950612,
+            "velocityY": -1.6317510329853882,
+            "timestamp": 0.2168612428888551
+        },
+        {
+            "x": 7.492264291454711,
+            "y": 5.403818271607319,
+            "heading": -0.001846132228095339,
+            "angularVelocity": -0.010215579120073546,
+            "velocityX": -1.0388341530026832,
+            "velocityY": -2.175667933326667,
+            "timestamp": 0.28914832385180683
+        },
+        {
+            "x": 7.39839642615194,
+            "y": 5.207227430397753,
+            "heading": -0.0027691974892554232,
+            "angularVelocity": -0.012769436099282666,
+            "velocityX": -1.298542755528879,
+            "velocityY": -2.719584725109078,
+            "timestamp": 0.36143540481475855
+        },
+        {
+            "x": 7.285754988600711,
+            "y": 4.97131845200793,
+            "heading": -0.0038768638943260385,
+            "angularVelocity": -0.015323158582628504,
+            "velocityX": -1.5582512954003445,
+            "velocityY": -3.263501240432299,
+            "timestamp": 0.4337224857777103
+        },
+        {
+            "x": 7.167927209247298,
+            "y": 4.7245478329210355,
+            "heading": -0.0038768640537450113,
+            "angularVelocity": -2.205359112715029e-9,
+            "velocityX": -1.629997750411335,
+            "velocityY": -3.4137582511232942,
+            "timestamp": 0.506009566740662
+        },
+        {
+            "x": 7.050099366875978,
+            "y": 4.477777243923851,
+            "heading": -0.003876864213154645,
+            "angularVelocity": -2.2052299170164517e-9,
+            "velocityX": -1.6299986221840805,
+            "velocityY": -3.413757834870371,
+            "timestamp": 0.5782966477036137
+        },
+        {
+            "x": 6.932271561638543,
+            "y": 4.2310066371960025,
+            "heading": -0.0038768643725604667,
+            "angularVelocity": -2.2051771914474314e-9,
+            "velocityX": -1.629998108483918,
+            "velocityY": -3.413758080151585,
+            "timestamp": 0.6505837286665653
+        },
+        {
+            "x": 6.8252544845676155,
+            "y": 3.9793591274718993,
+            "heading": -0.0038768645333797187,
+            "angularVelocity": -2.2247301918048757e-9,
+            "velocityX": -1.4804454080221456,
+            "velocityY": -3.4812238421008592,
+            "timestamp": 0.722870809629517
+        },
+        {
+            "x": 6.7595626994044276,
+            "y": 3.7139091088268694,
+            "heading": -0.003876864707631206,
+            "angularVelocity": -2.410548124556829e-9,
+            "velocityX": -0.9087624550347577,
+            "velocityY": -3.6721640313720423,
+            "timestamp": 0.7951578905924687
+        },
+        {
+            "x": 6.7368645668029785,
+            "y": 3.4413950443267822,
+            "heading": -0.0038768649214938546,
+            "angularVelocity": -2.9585182521427424e-9,
+            "velocityX": -0.31399985030634436,
+            "velocityY": -3.7698861382956523,
+            "timestamp": 0.8674449715554203
+        },
+        {
+            "x": 6.763602448674763,
+            "y": 3.1456190677825386,
+            "heading": -0.0038769163309766456,
+            "angularVelocity": -6.548510424231184e-7,
+            "velocityX": 0.340585605327871,
+            "velocityY": -3.7675774205236117,
+            "timestamp": 0.9459505849856912
+        },
+        {
+            "x": 6.763837716877819,
+            "y": 2.855181760345028,
+            "heading": -0.02573615579182971,
+            "angularVelocity": -0.27844173818562123,
+            "velocityX": 0.0029968328731749647,
+            "velocityY": -3.699573759722024,
+            "timestamp": 1.0244561984159621
+        },
+        {
+            "x": 6.746691140594586,
+            "y": 2.576419895706607,
+            "heading": -0.08408576500639096,
+            "angularVelocity": -0.7432539746522423,
+            "velocityX": -0.2184121049950426,
+            "velocityY": -3.5508526391685153,
+            "timestamp": 1.102961811846233
+        },
+        {
+            "x": 6.69086397201793,
+            "y": 2.322743038831463,
+            "heading": -0.2046015958190934,
+            "angularVelocity": -1.5351237388870964,
+            "velocityX": -0.7111232705192682,
+            "velocityY": -3.2313212494092722,
+            "timestamp": 1.1814674252765038
+        },
+        {
+            "x": 6.586917702656428,
+            "y": 2.0871040226876985,
+            "heading": -0.32491541634118276,
+            "angularVelocity": -1.5325505433946174,
+            "velocityX": -1.324061615719081,
+            "velocityY": -3.001556269005664,
+            "timestamp": 1.2599730387067747
+        },
+        {
+            "x": 6.439847045017927,
+            "y": 1.879401635438828,
+            "heading": -0.448122685606705,
+            "angularVelocity": -1.5694071274910255,
+            "velocityX": -1.8733775995410846,
+            "velocityY": -2.6457010928696563,
+            "timestamp": 1.3384786521370455
+        },
+        {
+            "x": 6.25456019704424,
+            "y": 1.7060438180074393,
+            "heading": -0.5681346722700569,
+            "angularVelocity": -1.528705801018258,
+            "velocityX": -2.3601732395640638,
+            "velocityY": -2.2082219328859343,
+            "timestamp": 1.4169842655673164
+        },
+        {
+            "x": 6.049779045368227,
+            "y": 1.57972317182915,
+            "heading": -0.6633309119306381,
+            "angularVelocity": -1.2126042393788183,
+            "velocityX": -2.6084905617341763,
+            "velocityY": -1.6090651439911137,
+            "timestamp": 1.4954898789975872
+        },
+        {
+            "x": 5.840944041976222,
+            "y": 1.503518626439596,
+            "heading": -0.7208467997653518,
+            "angularVelocity": -0.7326340795464229,
+            "velocityX": -2.660128292322611,
+            "velocityY": -0.9706891273098467,
+            "timestamp": 1.573995492427858
+        },
+        {
+            "x": 5.636524773547887,
+            "y": 1.4770268115818306,
+            "heading": -0.7348631463149735,
+            "angularVelocity": -0.17853941822989652,
+            "velocityX": -2.603880913686534,
+            "velocityY": -0.33745121782018417,
+            "timestamp": 1.652501105858129
+        },
+        {
+            "x": 5.451777868175388,
+            "y": 1.4973120784666072,
+            "heading": -0.7197568228935092,
+            "angularVelocity": 0.19242348109134802,
+            "velocityX": -2.353295481686686,
+            "velocityY": 0.25839256581051945,
+            "timestamp": 1.7310067192883998
+        },
+        {
+            "x": 5.309771976878378,
+            "y": 1.5457401851903851,
+            "heading": -0.687868735789328,
+            "angularVelocity": 0.4061886241103559,
+            "velocityX": -1.808862896449324,
+            "velocityY": 0.6168744451222204,
+            "timestamp": 1.8095123327186706
+        },
+        {
+            "x": 5.2191255322417085,
+            "y": 1.592492981200916,
+            "heading": -0.6576952711638115,
+            "angularVelocity": 0.38434786134518467,
+            "velocityX": -1.154649211386418,
+            "velocityY": 0.5955344333696231,
+            "timestamp": 1.8880179461489415
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 0.22540134890517805,
+            "velocityX": -0.5493305555788384,
+            "velocityY": 0.35038282738234927,
+            "timestamp": 1.9665235595792123
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 0,
+            "velocityX": 6.617232561290069e-35,
+            "velocityY": 1.0472745338276337e-32,
+            "timestamp": 2.045029173009483
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.1.traj
index a8bed878..0428af29 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.1.traj	
@@ -4,199 +4,199 @@
             "x": 7.68,
             "y": 5.797,
             "heading": 0,
-            "angularVelocity": 5.4178362746118835e-30,
-            "velocityX": -4.378571579060787e-32,
-            "velocityY": 1.8329583672175558e-31,
+            "angularVelocity": 0,
+            "velocityX": 2.477402495362019e-32,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.617836111630202,
-            "y": 5.830159453299192,
-            "heading": -0.009839435811264896,
-            "angularVelocity": -0.10699973937901981,
-            "velocityX": -0.6760062245377694,
-            "velocityY": 0.3605951529154081,
-            "timestamp": 0.09195756801248979
-        },
-        {
-            "x": 7.493508339255607,
-            "y": 5.896478344435856,
-            "heading": -0.0295193862966414,
-            "angularVelocity": -0.21401121093919698,
-            "velocityX": -1.352012401607965,
-            "velocityY": 0.721190137691066,
-            "timestamp": 0.18391513602497958
-        },
-        {
-            "x": 7.3070166954300815,
-            "y": 5.995956653040542,
-            "heading": -0.059041701653073396,
-            "angularVelocity": -0.32104280261546536,
-            "velocityX": -2.0280184421601457,
-            "velocityY": 1.0817849009575307,
-            "timestamp": 0.2758727040374694
-        },
-        {
-            "x": 7.0583612095128645,
-            "y": 6.128594352415652,
-            "heading": -0.09840889768284368,
-            "angularVelocity": -0.42810175258683864,
-            "velocityX": -2.7040241634429143,
-            "velocityY": 1.4423793738987822,
-            "timestamp": 0.36783027204995916
-        },
-        {
-            "x": 6.751428460841339,
-            "y": 6.292319222162274,
-            "heading": -0.09840890267081659,
-            "angularVelocity": -5.4242114104221296e-8,
-            "velocityX": -3.3377649638345908,
-            "velocityY": 1.780439318756071,
-            "timestamp": 0.45978784006244894
-        },
-        {
-            "x": 6.443018724200086,
-            "y": 6.45324460704626,
-            "heading": -0.09840890766830068,
-            "angularVelocity": -5.4345544379257814e-8,
-            "velocityX": -3.3538265887954406,
-            "velocityY": 1.7499960945262094,
-            "timestamp": 0.5517454080749388
-        },
-        {
-            "x": 6.108495580653603,
-            "y": 6.54867946433224,
-            "heading": -0.0984088906849307,
-            "angularVelocity": 1.846870285822299e-7,
-            "velocityX": -3.637798941149115,
-            "velocityY": 1.037814063036315,
-            "timestamp": 0.6437029760874285
-        },
-        {
-            "x": 5.762312734581719,
-            "y": 6.58289903236131,
-            "heading": -0.09840883650367563,
-            "angularVelocity": 5.891984340231216e-7,
-            "velocityX": -3.7645933179188145,
-            "velocityY": 0.37212345616210646,
-            "timestamp": 0.7356605440999183
-        },
-        {
-            "x": 5.416121743024081,
-            "y": 6.617036096003367,
-            "heading": -0.09840878231628362,
-            "angularVelocity": 5.892651705038846e-7,
-            "velocityX": -3.7646818966614886,
-            "velocityY": 0.37122625554234295,
-            "timestamp": 0.8276181121124081
-        },
-        {
-            "x": 5.0685946034380285,
-            "y": 6.632477283287523,
-            "heading": -0.09840873884834453,
-            "angularVelocity": 4.7269561430407727e-7,
-            "velocityX": -3.7792119463060407,
-            "velocityY": 0.16791643817786392,
-            "timestamp": 0.9195756801248979
-        },
-        {
-            "x": 4.72106743655186,
-            "y": 6.647917856129691,
-            "heading": -0.09840869538060193,
-            "angularVelocity": 4.726934774100163e-7,
-            "velocityX": -3.779212243183364,
-            "velocityY": 0.1679097563788475,
-            "timestamp": 1.0115332481373878
-        },
-        {
-            "x": 4.373540264440576,
-            "y": 6.66335831136751,
-            "heading": -0.09840865191285005,
-            "angularVelocity": 4.7269357834441634e-7,
-            "velocityX": -3.779212300004299,
-            "velocityY": 0.16790847748084475,
-            "timestamp": 1.1034908161498775
-        },
-        {
-            "x": 4.026013088497698,
-            "y": 6.678798680365008,
-            "heading": -0.09840860844507962,
-            "angularVelocity": 4.726937801388588e-7,
-            "velocityX": -3.7792123416712937,
-            "velocityY": 0.16790753965352348,
-            "timestamp": 1.1954483841623673
-        },
-        {
-            "x": 3.6784859092773967,
-            "y": 6.694238975594936,
-            "heading": -0.09840856497728971,
-            "angularVelocity": 4.7269399197494456e-7,
-            "velocityX": -3.7792123773118855,
-            "velocityY": 0.1679067374621128,
-            "timestamp": 1.287405952174857
-        },
-        {
-            "x": 3.3309587272270753,
-            "y": 6.709679207126903,
-            "heading": -0.09840852150946863,
-            "angularVelocity": 4.72694330698846e-7,
-            "velocityX": -3.779212408087172,
-            "velocityY": 0.16790604477348844,
-            "timestamp": 1.3793635201873469
-        },
-        {
-            "x": 2.991492172408929,
-            "y": 6.7253546024137885,
-            "heading": -0.07153351873489558,
-            "angularVelocity": 0.29225438814261445,
-            "velocityX": -3.691556466261044,
-            "velocityY": 0.17046335201859075,
-            "timestamp": 1.4713210881998366
-        },
-        {
-            "x": 2.7138316831246163,
-            "y": 6.738400020288083,
-            "heading": 0.08699256852088375,
-            "angularVelocity": 1.723904738696962,
-            "velocityX": -3.019441415051349,
-            "velocityY": 0.1418634502439354,
-            "timestamp": 1.5632786562123264
-        },
-        {
-            "x": 2.5064819973568118,
-            "y": 6.747857404358727,
-            "heading": 0.23167973935786532,
-            "angularVelocity": 1.5734123244465315,
-            "velocityX": -2.254840903792083,
-            "velocityY": 0.10284508687050122,
-            "timestamp": 1.6552362242248162
-        },
-        {
-            "x": 2.36871059980762,
-            "y": 6.753996751301636,
-            "heading": 0.3372042620417382,
-            "angularVelocity": 1.1475349442640808,
-            "velocityX": -1.4982061892989513,
-            "velocityY": 0.0667628241546722,
-            "timestamp": 1.747193792237306
+            "x": 7.61783610738437,
+            "y": 5.830159445563511,
+            "heading": -0.009839426193567341,
+            "angularVelocity": -0.10699963479066096,
+            "velocityX": -0.6760062707098423,
+            "velocityY": 0.3605950687933442,
+            "timestamp": 0.09195756801242966
+        },
+        {
+            "x": 7.493508328314676,
+            "y": 5.8964783245931205,
+            "heading": -0.029519357600464063,
+            "angularVelocity": -0.21401100346886773,
+            "velocityX": -1.3520124744152573,
+            "velocityY": 0.7211900060324206,
+            "timestamp": 0.18391513602485932
+        },
+        {
+            "x": 7.307016677169599,
+            "y": 5.995956620127639,
+            "heading": -0.05904164496194892,
+            "angularVelocity": -0.32104249818236175,
+            "velocityX": -2.028018521758536,
+            "velocityY": 1.0817847588256284,
+            "timestamp": 0.275872704037289
+        },
+        {
+            "x": 7.058361185168299,
+            "y": 6.1285943089313815,
+            "heading": -0.09840880514573262,
+            "angularVelocity": -0.42810136277704247,
+            "velocityX": -2.7040242296065236,
+            "velocityY": 1.4423792589405482,
+            "timestamp": 0.36783027204971863
+        },
+        {
+            "x": 6.751428461707863,
+            "y": 6.292319225940392,
+            "heading": -0.09840881013370367,
+            "angularVelocity": -5.424209394751822e-8,
+            "velocityX": -3.3377646896767494,
+            "velocityY": 1.7804398327158815,
+            "timestamp": 0.4597878400621483
+        },
+        {
+            "x": 6.443018732830252,
+            "y": 6.45324462570272,
+            "heading": -0.09840881513118924,
+            "angularVelocity": -5.434556026601013e-8,
+            "velocityX": -3.3538265043712756,
+            "velocityY": 1.7499962563230937,
+            "timestamp": 0.551745408074578
+        },
+        {
+            "x": 6.108495593860041,
+            "y": 6.548679499028843,
+            "heading": -0.09840879814777771,
+            "angularVelocity": 1.8468748020115896e-7,
+            "velocityX": -3.637798891386455,
+            "velocityY": 1.0378142374668238,
+            "timestamp": 0.6437029760870077
+        },
+        {
+            "x": 5.7623127419169835,
+            "y": 6.582899007638951,
+            "heading": -0.09840874396641834,
+            "angularVelocity": 5.891995683553032e-7,
+            "velocityX": -3.764593381767812,
+            "velocityY": 0.37212281000605224,
+            "timestamp": 0.7356605440994373
+        },
+        {
+            "x": 5.416121749902788,
+            "y": 6.617036066627776,
+            "heading": -0.09840868977891702,
+            "angularVelocity": 5.892663593006056e-7,
+            "velocityX": -3.7646819016288298,
+            "velocityY": 0.37122620494064046,
+            "timestamp": 0.8276181121118669
+        },
+        {
+            "x": 5.068594601971295,
+            "y": 6.632477066079001,
+            "heading": -0.0984086463109951,
+            "angularVelocity": 4.72695427552955e-7,
+            "velocityX": -3.7792120370616824,
+            "velocityY": 0.1679143955735957,
+            "timestamp": 0.9195756801242965
+        },
+        {
+            "x": 4.721067428734796,
+            "y": 6.647917495985935,
+            "heading": -0.09840860284326601,
+            "angularVelocity": 4.7269333042542933e-7,
+            "velocityX": -3.7792123122430223,
+            "velocityY": 0.16790820201819795,
+            "timestamp": 1.011533248136726
+        },
+        {
+            "x": 4.3735402523552525,
+            "y": 6.663357855150138,
+            "heading": -0.09840855937552709,
+            "angularVelocity": 4.726934374928119e-7,
+            "velocityX": -3.779212346422309,
+            "velocityY": 0.16790743272066663,
+            "timestamp": 1.1034908161491557
+        },
+        {
+            "x": 4.026013074283842,
+            "y": 6.678798176234082,
+            "heading": -0.09840851590777203,
+            "angularVelocity": 4.7269361285314424e-7,
+            "velocityX": -3.779212364820652,
+            "velocityY": 0.1679070186138197,
+            "timestamp": 1.1954483841615853
+        },
+        {
+            "x": 3.6784858950907164,
+            "y": 6.69423847207055,
+            "heading": -0.09840847244000124,
+            "angularVelocity": 4.7269378404370134e-7,
+            "velocityX": -3.7792123770188395,
+            "velocityY": 0.16790674405809006,
+            "timestamp": 1.2874059521740149
+        },
+        {
+            "x": 3.330958715199463,
+            "y": 6.709678752193432,
+            "heading": -0.09840842897219951,
+            "angularVelocity": 4.7269412044437995e-7,
+            "velocityX": -3.7792123846106827,
+            "velocityY": 0.16790657317944163,
+            "timestamp": 1.3793635201864445
+        },
+        {
+            "x": 2.991492162018093,
+            "y": 6.7253542779205,
+            "heading": -0.07153342912076269,
+            "angularVelocity": 0.2922543563549246,
+            "velocityX": -3.6915564484642,
+            "velocityY": 0.17046477050097905,
+            "timestamp": 1.471321088198874
+        },
+        {
+            "x": 2.713831676976805,
+            "y": 6.738399816428541,
+            "heading": 0.08699262812295999,
+            "angularVelocity": 1.7239044123295555,
+            "velocityX": -3.0194413689122035,
+            "velocityY": 0.14186476208546492,
+            "timestamp": 1.5632786562113037
+        },
+        {
+            "x": 2.5064819943185284,
+            "y": 6.7478572981535265,
+            "heading": 0.2316797715135602,
+            "angularVelocity": 1.5734120259796696,
+            "velocityX": -2.2548408699787488,
+            "velocityY": 0.10284614882059591,
+            "timestamp": 1.6552362242237333
+        },
+        {
+            "x": 2.368710598807467,
+            "y": 6.7539967145328434,
+            "heading": 0.3372042735506754,
+            "angularVelocity": 1.147534719739997,
+            "velocityX": -1.4982061671361233,
+            "velocityY": 0.06676357924654308,
+            "timestamp": 1.747193792236163
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.393,
-            "angularVelocity": 0.6067552585849546,
-            "velocityX": -0.7471989667918153,
-            "velocityY": 0.03265907051778923,
-            "timestamp": 1.8391513602497958
+            "angularVelocity": 0.6067551334304849,
+            "velocityX": -0.7471989559160562,
+            "velocityY": 0.03265947036300673,
+            "timestamp": 1.8391513602485925
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.393,
-            "angularVelocity": -8.637511489261282e-30,
-            "velocityX": 0,
-            "velocityY": 5.8539743527830045e-30,
-            "timestamp": 1.9311089282622855
+            "angularVelocity": 0,
+            "velocityX": -4.676461204476467e-32,
+            "velocityY": 0,
+            "timestamp": 1.931108928261022
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.traj
index a8bed878..0428af29 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.traj	
@@ -4,199 +4,199 @@
             "x": 7.68,
             "y": 5.797,
             "heading": 0,
-            "angularVelocity": 5.4178362746118835e-30,
-            "velocityX": -4.378571579060787e-32,
-            "velocityY": 1.8329583672175558e-31,
+            "angularVelocity": 0,
+            "velocityX": 2.477402495362019e-32,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.617836111630202,
-            "y": 5.830159453299192,
-            "heading": -0.009839435811264896,
-            "angularVelocity": -0.10699973937901981,
-            "velocityX": -0.6760062245377694,
-            "velocityY": 0.3605951529154081,
-            "timestamp": 0.09195756801248979
-        },
-        {
-            "x": 7.493508339255607,
-            "y": 5.896478344435856,
-            "heading": -0.0295193862966414,
-            "angularVelocity": -0.21401121093919698,
-            "velocityX": -1.352012401607965,
-            "velocityY": 0.721190137691066,
-            "timestamp": 0.18391513602497958
-        },
-        {
-            "x": 7.3070166954300815,
-            "y": 5.995956653040542,
-            "heading": -0.059041701653073396,
-            "angularVelocity": -0.32104280261546536,
-            "velocityX": -2.0280184421601457,
-            "velocityY": 1.0817849009575307,
-            "timestamp": 0.2758727040374694
-        },
-        {
-            "x": 7.0583612095128645,
-            "y": 6.128594352415652,
-            "heading": -0.09840889768284368,
-            "angularVelocity": -0.42810175258683864,
-            "velocityX": -2.7040241634429143,
-            "velocityY": 1.4423793738987822,
-            "timestamp": 0.36783027204995916
-        },
-        {
-            "x": 6.751428460841339,
-            "y": 6.292319222162274,
-            "heading": -0.09840890267081659,
-            "angularVelocity": -5.4242114104221296e-8,
-            "velocityX": -3.3377649638345908,
-            "velocityY": 1.780439318756071,
-            "timestamp": 0.45978784006244894
-        },
-        {
-            "x": 6.443018724200086,
-            "y": 6.45324460704626,
-            "heading": -0.09840890766830068,
-            "angularVelocity": -5.4345544379257814e-8,
-            "velocityX": -3.3538265887954406,
-            "velocityY": 1.7499960945262094,
-            "timestamp": 0.5517454080749388
-        },
-        {
-            "x": 6.108495580653603,
-            "y": 6.54867946433224,
-            "heading": -0.0984088906849307,
-            "angularVelocity": 1.846870285822299e-7,
-            "velocityX": -3.637798941149115,
-            "velocityY": 1.037814063036315,
-            "timestamp": 0.6437029760874285
-        },
-        {
-            "x": 5.762312734581719,
-            "y": 6.58289903236131,
-            "heading": -0.09840883650367563,
-            "angularVelocity": 5.891984340231216e-7,
-            "velocityX": -3.7645933179188145,
-            "velocityY": 0.37212345616210646,
-            "timestamp": 0.7356605440999183
-        },
-        {
-            "x": 5.416121743024081,
-            "y": 6.617036096003367,
-            "heading": -0.09840878231628362,
-            "angularVelocity": 5.892651705038846e-7,
-            "velocityX": -3.7646818966614886,
-            "velocityY": 0.37122625554234295,
-            "timestamp": 0.8276181121124081
-        },
-        {
-            "x": 5.0685946034380285,
-            "y": 6.632477283287523,
-            "heading": -0.09840873884834453,
-            "angularVelocity": 4.7269561430407727e-7,
-            "velocityX": -3.7792119463060407,
-            "velocityY": 0.16791643817786392,
-            "timestamp": 0.9195756801248979
-        },
-        {
-            "x": 4.72106743655186,
-            "y": 6.647917856129691,
-            "heading": -0.09840869538060193,
-            "angularVelocity": 4.726934774100163e-7,
-            "velocityX": -3.779212243183364,
-            "velocityY": 0.1679097563788475,
-            "timestamp": 1.0115332481373878
-        },
-        {
-            "x": 4.373540264440576,
-            "y": 6.66335831136751,
-            "heading": -0.09840865191285005,
-            "angularVelocity": 4.7269357834441634e-7,
-            "velocityX": -3.779212300004299,
-            "velocityY": 0.16790847748084475,
-            "timestamp": 1.1034908161498775
-        },
-        {
-            "x": 4.026013088497698,
-            "y": 6.678798680365008,
-            "heading": -0.09840860844507962,
-            "angularVelocity": 4.726937801388588e-7,
-            "velocityX": -3.7792123416712937,
-            "velocityY": 0.16790753965352348,
-            "timestamp": 1.1954483841623673
-        },
-        {
-            "x": 3.6784859092773967,
-            "y": 6.694238975594936,
-            "heading": -0.09840856497728971,
-            "angularVelocity": 4.7269399197494456e-7,
-            "velocityX": -3.7792123773118855,
-            "velocityY": 0.1679067374621128,
-            "timestamp": 1.287405952174857
-        },
-        {
-            "x": 3.3309587272270753,
-            "y": 6.709679207126903,
-            "heading": -0.09840852150946863,
-            "angularVelocity": 4.72694330698846e-7,
-            "velocityX": -3.779212408087172,
-            "velocityY": 0.16790604477348844,
-            "timestamp": 1.3793635201873469
-        },
-        {
-            "x": 2.991492172408929,
-            "y": 6.7253546024137885,
-            "heading": -0.07153351873489558,
-            "angularVelocity": 0.29225438814261445,
-            "velocityX": -3.691556466261044,
-            "velocityY": 0.17046335201859075,
-            "timestamp": 1.4713210881998366
-        },
-        {
-            "x": 2.7138316831246163,
-            "y": 6.738400020288083,
-            "heading": 0.08699256852088375,
-            "angularVelocity": 1.723904738696962,
-            "velocityX": -3.019441415051349,
-            "velocityY": 0.1418634502439354,
-            "timestamp": 1.5632786562123264
-        },
-        {
-            "x": 2.5064819973568118,
-            "y": 6.747857404358727,
-            "heading": 0.23167973935786532,
-            "angularVelocity": 1.5734123244465315,
-            "velocityX": -2.254840903792083,
-            "velocityY": 0.10284508687050122,
-            "timestamp": 1.6552362242248162
-        },
-        {
-            "x": 2.36871059980762,
-            "y": 6.753996751301636,
-            "heading": 0.3372042620417382,
-            "angularVelocity": 1.1475349442640808,
-            "velocityX": -1.4982061892989513,
-            "velocityY": 0.0667628241546722,
-            "timestamp": 1.747193792237306
+            "x": 7.61783610738437,
+            "y": 5.830159445563511,
+            "heading": -0.009839426193567341,
+            "angularVelocity": -0.10699963479066096,
+            "velocityX": -0.6760062707098423,
+            "velocityY": 0.3605950687933442,
+            "timestamp": 0.09195756801242966
+        },
+        {
+            "x": 7.493508328314676,
+            "y": 5.8964783245931205,
+            "heading": -0.029519357600464063,
+            "angularVelocity": -0.21401100346886773,
+            "velocityX": -1.3520124744152573,
+            "velocityY": 0.7211900060324206,
+            "timestamp": 0.18391513602485932
+        },
+        {
+            "x": 7.307016677169599,
+            "y": 5.995956620127639,
+            "heading": -0.05904164496194892,
+            "angularVelocity": -0.32104249818236175,
+            "velocityX": -2.028018521758536,
+            "velocityY": 1.0817847588256284,
+            "timestamp": 0.275872704037289
+        },
+        {
+            "x": 7.058361185168299,
+            "y": 6.1285943089313815,
+            "heading": -0.09840880514573262,
+            "angularVelocity": -0.42810136277704247,
+            "velocityX": -2.7040242296065236,
+            "velocityY": 1.4423792589405482,
+            "timestamp": 0.36783027204971863
+        },
+        {
+            "x": 6.751428461707863,
+            "y": 6.292319225940392,
+            "heading": -0.09840881013370367,
+            "angularVelocity": -5.424209394751822e-8,
+            "velocityX": -3.3377646896767494,
+            "velocityY": 1.7804398327158815,
+            "timestamp": 0.4597878400621483
+        },
+        {
+            "x": 6.443018732830252,
+            "y": 6.45324462570272,
+            "heading": -0.09840881513118924,
+            "angularVelocity": -5.434556026601013e-8,
+            "velocityX": -3.3538265043712756,
+            "velocityY": 1.7499962563230937,
+            "timestamp": 0.551745408074578
+        },
+        {
+            "x": 6.108495593860041,
+            "y": 6.548679499028843,
+            "heading": -0.09840879814777771,
+            "angularVelocity": 1.8468748020115896e-7,
+            "velocityX": -3.637798891386455,
+            "velocityY": 1.0378142374668238,
+            "timestamp": 0.6437029760870077
+        },
+        {
+            "x": 5.7623127419169835,
+            "y": 6.582899007638951,
+            "heading": -0.09840874396641834,
+            "angularVelocity": 5.891995683553032e-7,
+            "velocityX": -3.764593381767812,
+            "velocityY": 0.37212281000605224,
+            "timestamp": 0.7356605440994373
+        },
+        {
+            "x": 5.416121749902788,
+            "y": 6.617036066627776,
+            "heading": -0.09840868977891702,
+            "angularVelocity": 5.892663593006056e-7,
+            "velocityX": -3.7646819016288298,
+            "velocityY": 0.37122620494064046,
+            "timestamp": 0.8276181121118669
+        },
+        {
+            "x": 5.068594601971295,
+            "y": 6.632477066079001,
+            "heading": -0.0984086463109951,
+            "angularVelocity": 4.72695427552955e-7,
+            "velocityX": -3.7792120370616824,
+            "velocityY": 0.1679143955735957,
+            "timestamp": 0.9195756801242965
+        },
+        {
+            "x": 4.721067428734796,
+            "y": 6.647917495985935,
+            "heading": -0.09840860284326601,
+            "angularVelocity": 4.7269333042542933e-7,
+            "velocityX": -3.7792123122430223,
+            "velocityY": 0.16790820201819795,
+            "timestamp": 1.011533248136726
+        },
+        {
+            "x": 4.3735402523552525,
+            "y": 6.663357855150138,
+            "heading": -0.09840855937552709,
+            "angularVelocity": 4.726934374928119e-7,
+            "velocityX": -3.779212346422309,
+            "velocityY": 0.16790743272066663,
+            "timestamp": 1.1034908161491557
+        },
+        {
+            "x": 4.026013074283842,
+            "y": 6.678798176234082,
+            "heading": -0.09840851590777203,
+            "angularVelocity": 4.7269361285314424e-7,
+            "velocityX": -3.779212364820652,
+            "velocityY": 0.1679070186138197,
+            "timestamp": 1.1954483841615853
+        },
+        {
+            "x": 3.6784858950907164,
+            "y": 6.69423847207055,
+            "heading": -0.09840847244000124,
+            "angularVelocity": 4.7269378404370134e-7,
+            "velocityX": -3.7792123770188395,
+            "velocityY": 0.16790674405809006,
+            "timestamp": 1.2874059521740149
+        },
+        {
+            "x": 3.330958715199463,
+            "y": 6.709678752193432,
+            "heading": -0.09840842897219951,
+            "angularVelocity": 4.7269412044437995e-7,
+            "velocityX": -3.7792123846106827,
+            "velocityY": 0.16790657317944163,
+            "timestamp": 1.3793635201864445
+        },
+        {
+            "x": 2.991492162018093,
+            "y": 6.7253542779205,
+            "heading": -0.07153342912076269,
+            "angularVelocity": 0.2922543563549246,
+            "velocityX": -3.6915564484642,
+            "velocityY": 0.17046477050097905,
+            "timestamp": 1.471321088198874
+        },
+        {
+            "x": 2.713831676976805,
+            "y": 6.738399816428541,
+            "heading": 0.08699262812295999,
+            "angularVelocity": 1.7239044123295555,
+            "velocityX": -3.0194413689122035,
+            "velocityY": 0.14186476208546492,
+            "timestamp": 1.5632786562113037
+        },
+        {
+            "x": 2.5064819943185284,
+            "y": 6.7478572981535265,
+            "heading": 0.2316797715135602,
+            "angularVelocity": 1.5734120259796696,
+            "velocityX": -2.2548408699787488,
+            "velocityY": 0.10284614882059591,
+            "timestamp": 1.6552362242237333
+        },
+        {
+            "x": 2.368710598807467,
+            "y": 6.7539967145328434,
+            "heading": 0.3372042735506754,
+            "angularVelocity": 1.147534719739997,
+            "velocityX": -1.4982061671361233,
+            "velocityY": 0.06676357924654308,
+            "timestamp": 1.747193792236163
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.393,
-            "angularVelocity": 0.6067552585849546,
-            "velocityX": -0.7471989667918153,
-            "velocityY": 0.03265907051778923,
-            "timestamp": 1.8391513602497958
+            "angularVelocity": 0.6067551334304849,
+            "velocityX": -0.7471989559160562,
+            "velocityY": 0.03265947036300673,
+            "timestamp": 1.8391513602485925
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.393,
-            "angularVelocity": -8.637511489261282e-30,
-            "velocityX": 0,
-            "velocityY": 5.8539743527830045e-30,
-            "timestamp": 1.9311089282622855
+            "angularVelocity": 0,
+            "velocityX": -4.676461204476467e-32,
+            "velocityY": 0,
+            "timestamp": 1.931108928261022
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.1.traj
new file mode 100644
index 00000000..5429265f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.1.traj	
@@ -0,0 +1,184 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -7.048442713490885e-38,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.598761685357721,
+            "y": 5.839328892781844,
+            "heading": -0.010877614975927053,
+            "angularVelocity": -0.10375029848266179,
+            "velocityX": -0.774848108801196,
+            "velocityY": 0.4037314494285639,
+            "timestamp": 0.10484418006512992
+        },
+        {
+            "x": 7.436285061134455,
+            "y": 5.923986667743732,
+            "heading": -0.03263383242489801,
+            "angularVelocity": -0.20751001567713226,
+            "velocityX": -1.5496961693280003,
+            "velocityY": 0.8074627977375359,
+            "timestamp": 0.20968836013025985
+        },
+        {
+            "x": 7.192570139480125,
+            "y": 6.050973310343331,
+            "heading": -0.06527034306657675,
+            "angularVelocity": -0.31128585889464805,
+            "velocityX": -2.3245441139692504,
+            "velocityY": 1.2111940073422762,
+            "timestamp": 0.3145325401953898
+        },
+        {
+            "x": 6.867616948374112,
+            "y": 6.220288799737083,
+            "heading": -0.10878948434468085,
+            "angularVelocity": -0.41508399656585776,
+            "velocityX": -3.0993917917441864,
+            "velocityY": 1.6149250181418864,
+            "timestamp": 0.4193767202605197
+        },
+        {
+            "x": 6.5158804421759,
+            "y": 6.403559919066418,
+            "heading": -0.10878948853448296,
+            "angularVelocity": -3.996218113987742e-8,
+            "velocityX": -3.3548500830442913,
+            "velocityY": 1.7480333120587623,
+            "timestamp": 0.5242209003256496
+        },
+        {
+            "x": 6.14377891604402,
+            "y": 6.540845489176542,
+            "heading": -0.10878948531130445,
+            "angularVelocity": 3.074256021150283e-8,
+            "velocityX": -3.549090907103546,
+            "velocityY": 1.309424805695852,
+            "timestamp": 0.6290650803907796
+        },
+        {
+            "x": 5.750100268474419,
+            "y": 6.5890532439035745,
+            "heading": -0.10878941324219087,
+            "angularVelocity": 6.873926003335433e-7,
+            "velocityX": -3.754892711498581,
+            "velocityY": 0.4598038221776824,
+            "timestamp": 0.7339092604559095
+        },
+        {
+            "x": 5.35529697720587,
+            "y": 6.626964014921626,
+            "heading": -0.10878934177449004,
+            "angularVelocity": 6.816563474487017e-7,
+            "velocityX": -3.7656195224503146,
+            "velocityY": 0.3615915637329853,
+            "timestamp": 0.8387534405210395
+        },
+        {
+            "x": 4.9606240931120595,
+            "y": 6.587718813133039,
+            "heading": -0.10878931155021784,
+            "angularVelocity": 2.882780157548741e-7,
+            "velocityX": -3.7643757035310585,
+            "velocityY": -0.3743193161910223,
+            "timestamp": 0.9435976205861695
+        },
+        {
+            "x": 4.585497747567555,
+            "y": 6.458927912145357,
+            "heading": -0.10878930710495487,
+            "angularVelocity": 4.239875765382173e-8,
+            "velocityX": -3.5779415253328617,
+            "velocityY": -1.2284029586351546,
+            "timestamp": 1.0484418006512994
+        },
+        {
+            "x": 4.215349929173217,
+            "y": 6.316458743577884,
+            "heading": -0.10878930270384918,
+            "angularVelocity": 4.197758707259389e-8,
+            "velocityX": -3.530456513317188,
+            "velocityY": -1.3588657804273945,
+            "timestamp": 1.1532859807164293
+        },
+        {
+            "x": 3.8452021463353314,
+            "y": 6.173989482631574,
+            "heading": -0.10878929830273087,
+            "angularVelocity": 4.197770719831182e-8,
+            "velocityX": -3.530456174181034,
+            "velocityY": -1.3588666615333942,
+            "timestamp": 1.2581301607815591
+        },
+        {
+            "x": 3.475054376591526,
+            "y": 6.0315201876656985,
+            "heading": -0.10878929390160265,
+            "angularVelocity": 4.197780205439048e-8,
+            "velocityX": -3.530456049290165,
+            "velocityY": -1.3588669860107905,
+            "timestamp": 1.362974340846689
+        },
+        {
+            "x": 3.104906616234201,
+            "y": 5.889050868312964,
+            "heading": -0.10878928950045777,
+            "angularVelocity": 4.19779608468647e-8,
+            "velocityX": -3.530455959762256,
+            "velocityY": -1.358867218611777,
+            "timestamp": 1.4678185209118189
+        },
+        {
+            "x": 2.7629439773689555,
+            "y": 5.757430528072475,
+            "heading": -0.06527490613894259,
+            "angularVelocity": 0.4150386157294016,
+            "velocityX": -3.2616272896866185,
+            "velocityY": -1.2553900479618894,
+            "timestamp": 1.5726627009769487
+        },
+        {
+            "x": 2.506471990883214,
+            "y": 5.658715266844828,
+            "heading": -0.03263773447849684,
+            "angularVelocity": 0.311292163667756,
+            "velocityX": -2.446220537243172,
+            "velocityY": -0.9415425936501655,
+            "timestamp": 1.6775068810420786
+        },
+        {
+            "x": 2.335490664707938,
+            "y": 5.592905089907167,
+            "heading": -0.010879217132893882,
+            "angularVelocity": 0.20753195200808147,
+            "velocityX": -1.6308137091544919,
+            "velocityY": -0.6276950890052175,
+            "timestamp": 1.7823510611072084
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0.10376557979790346,
+            "velocityX": -0.8154068700316104,
+            "velocityY": -0.3138475582214142,
+            "timestamp": 1.8871952411723383
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 2.158258486540926e-34,
+            "velocityY": 0,
+            "timestamp": 1.9920394212374681
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.traj
new file mode 100644
index 00000000..5429265f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.traj	
@@ -0,0 +1,184 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -7.048442713490885e-38,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.598761685357721,
+            "y": 5.839328892781844,
+            "heading": -0.010877614975927053,
+            "angularVelocity": -0.10375029848266179,
+            "velocityX": -0.774848108801196,
+            "velocityY": 0.4037314494285639,
+            "timestamp": 0.10484418006512992
+        },
+        {
+            "x": 7.436285061134455,
+            "y": 5.923986667743732,
+            "heading": -0.03263383242489801,
+            "angularVelocity": -0.20751001567713226,
+            "velocityX": -1.5496961693280003,
+            "velocityY": 0.8074627977375359,
+            "timestamp": 0.20968836013025985
+        },
+        {
+            "x": 7.192570139480125,
+            "y": 6.050973310343331,
+            "heading": -0.06527034306657675,
+            "angularVelocity": -0.31128585889464805,
+            "velocityX": -2.3245441139692504,
+            "velocityY": 1.2111940073422762,
+            "timestamp": 0.3145325401953898
+        },
+        {
+            "x": 6.867616948374112,
+            "y": 6.220288799737083,
+            "heading": -0.10878948434468085,
+            "angularVelocity": -0.41508399656585776,
+            "velocityX": -3.0993917917441864,
+            "velocityY": 1.6149250181418864,
+            "timestamp": 0.4193767202605197
+        },
+        {
+            "x": 6.5158804421759,
+            "y": 6.403559919066418,
+            "heading": -0.10878948853448296,
+            "angularVelocity": -3.996218113987742e-8,
+            "velocityX": -3.3548500830442913,
+            "velocityY": 1.7480333120587623,
+            "timestamp": 0.5242209003256496
+        },
+        {
+            "x": 6.14377891604402,
+            "y": 6.540845489176542,
+            "heading": -0.10878948531130445,
+            "angularVelocity": 3.074256021150283e-8,
+            "velocityX": -3.549090907103546,
+            "velocityY": 1.309424805695852,
+            "timestamp": 0.6290650803907796
+        },
+        {
+            "x": 5.750100268474419,
+            "y": 6.5890532439035745,
+            "heading": -0.10878941324219087,
+            "angularVelocity": 6.873926003335433e-7,
+            "velocityX": -3.754892711498581,
+            "velocityY": 0.4598038221776824,
+            "timestamp": 0.7339092604559095
+        },
+        {
+            "x": 5.35529697720587,
+            "y": 6.626964014921626,
+            "heading": -0.10878934177449004,
+            "angularVelocity": 6.816563474487017e-7,
+            "velocityX": -3.7656195224503146,
+            "velocityY": 0.3615915637329853,
+            "timestamp": 0.8387534405210395
+        },
+        {
+            "x": 4.9606240931120595,
+            "y": 6.587718813133039,
+            "heading": -0.10878931155021784,
+            "angularVelocity": 2.882780157548741e-7,
+            "velocityX": -3.7643757035310585,
+            "velocityY": -0.3743193161910223,
+            "timestamp": 0.9435976205861695
+        },
+        {
+            "x": 4.585497747567555,
+            "y": 6.458927912145357,
+            "heading": -0.10878930710495487,
+            "angularVelocity": 4.239875765382173e-8,
+            "velocityX": -3.5779415253328617,
+            "velocityY": -1.2284029586351546,
+            "timestamp": 1.0484418006512994
+        },
+        {
+            "x": 4.215349929173217,
+            "y": 6.316458743577884,
+            "heading": -0.10878930270384918,
+            "angularVelocity": 4.197758707259389e-8,
+            "velocityX": -3.530456513317188,
+            "velocityY": -1.3588657804273945,
+            "timestamp": 1.1532859807164293
+        },
+        {
+            "x": 3.8452021463353314,
+            "y": 6.173989482631574,
+            "heading": -0.10878929830273087,
+            "angularVelocity": 4.197770719831182e-8,
+            "velocityX": -3.530456174181034,
+            "velocityY": -1.3588666615333942,
+            "timestamp": 1.2581301607815591
+        },
+        {
+            "x": 3.475054376591526,
+            "y": 6.0315201876656985,
+            "heading": -0.10878929390160265,
+            "angularVelocity": 4.197780205439048e-8,
+            "velocityX": -3.530456049290165,
+            "velocityY": -1.3588669860107905,
+            "timestamp": 1.362974340846689
+        },
+        {
+            "x": 3.104906616234201,
+            "y": 5.889050868312964,
+            "heading": -0.10878928950045777,
+            "angularVelocity": 4.19779608468647e-8,
+            "velocityX": -3.530455959762256,
+            "velocityY": -1.358867218611777,
+            "timestamp": 1.4678185209118189
+        },
+        {
+            "x": 2.7629439773689555,
+            "y": 5.757430528072475,
+            "heading": -0.06527490613894259,
+            "angularVelocity": 0.4150386157294016,
+            "velocityX": -3.2616272896866185,
+            "velocityY": -1.2553900479618894,
+            "timestamp": 1.5726627009769487
+        },
+        {
+            "x": 2.506471990883214,
+            "y": 5.658715266844828,
+            "heading": -0.03263773447849684,
+            "angularVelocity": 0.311292163667756,
+            "velocityX": -2.446220537243172,
+            "velocityY": -0.9415425936501655,
+            "timestamp": 1.6775068810420786
+        },
+        {
+            "x": 2.335490664707938,
+            "y": 5.592905089907167,
+            "heading": -0.010879217132893882,
+            "angularVelocity": 0.20753195200808147,
+            "velocityX": -1.6308137091544919,
+            "velocityY": -0.6276950890052175,
+            "timestamp": 1.7823510611072084
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0.10376557979790346,
+            "velocityX": -0.8154068700316104,
+            "velocityY": -0.3138475582214142,
+            "timestamp": 1.8871952411723383
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 2.158258486540926e-34,
+            "velocityY": 0,
+            "timestamp": 1.9920394212374681
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.1.traj
new file mode 100644
index 00000000..cf63c7e3
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.1.traj	
@@ -0,0 +1,346 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": -2.981242670977675e-31,
+            "velocityX": -5.763030958279131e-32,
+            "velocityY": 3.5928963704249306e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6464505983184665,
+            "y": 5.761517456765686,
+            "heading": 0.04441523610418216,
+            "angularVelocity": 0.571175712894524,
+            "velocityX": -0.43144211544178024,
+            "velocityY": -0.4563021319898165,
+            "timestamp": 0.07776107264628128
+        },
+        {
+            "x": 7.57910774134765,
+            "y": 5.690410071738714,
+            "heading": 0.1294441379757556,
+            "angularVelocity": 1.0934635927458562,
+            "velocityX": -0.8660227370723712,
+            "velocityY": -0.914434209908928,
+            "timestamp": 0.15552214529256256
+        },
+        {
+            "x": 7.4776816969238515,
+            "y": 5.583411802725407,
+            "heading": 0.24873158333085876,
+            "angularVelocity": 1.5340252043296483,
+            "velocityX": -1.3043292867777738,
+            "velocityY": -1.3759875651409672,
+            "timestamp": 0.23328321793884382
+        },
+        {
+            "x": 7.341761998810456,
+            "y": 5.440105324158156,
+            "heading": 0.38885380810835574,
+            "angularVelocity": 1.8019584865410045,
+            "velocityX": -1.747914393255167,
+            "velocityY": -1.842907687489359,
+            "timestamp": 0.3110442905851251
+        },
+        {
+            "x": 7.171108690296084,
+            "y": 5.260470754663091,
+            "heading": 0.5147184350397563,
+            "angularVelocity": 1.6186071339824826,
+            "velocityX": -2.1945853202236263,
+            "velocityY": -2.3100834824152265,
+            "timestamp": 0.3888053632314064
+        },
+        {
+            "x": 6.970033766909185,
+            "y": 5.052418524467747,
+            "heading": 0.5282282166004705,
+            "angularVelocity": 0.17373450623768458,
+            "velocityX": -2.585804394720051,
+            "velocityY": -2.67553189681076,
+            "timestamp": 0.4665664358776877
+        },
+        {
+            "x": 6.768438732548067,
+            "y": 4.838192511419719,
+            "heading": 0.5282282675213732,
+            "angularVelocity": 6.548379670470849e-7,
+            "velocityX": -2.592492972391604,
+            "velocityY": -2.7549261572367425,
+            "timestamp": 0.5443275085239689
+        },
+        {
+            "x": 6.5423880710685225,
+            "y": 4.649952125007829,
+            "heading": 0.5282283002658353,
+            "angularVelocity": 4.210906690145718e-7,
+            "velocityX": -2.9069900116707847,
+            "velocityY": -2.42075346964613,
+            "timestamp": 0.6220885811702502
+        },
+        {
+            "x": 6.292219183977312,
+            "y": 4.49519788912002,
+            "heading": 0.5282283052888641,
+            "angularVelocity": 6.459567356551782e-8,
+            "velocityX": -3.217148099656167,
+            "velocityY": -1.9901247580747212,
+            "timestamp": 0.6998496538165315
+        },
+        {
+            "x": 6.0420493950115075,
+            "y": 4.3404451111724525,
+            "heading": 0.5282283103117955,
+            "angularVelocity": 6.459441933764687e-8,
+            "velocityX": -3.217159697677684,
+            "velocityY": -1.990106009101862,
+            "timestamp": 0.7776107264628128
+        },
+        {
+            "x": 5.780250458218128,
+            "y": 4.206300927357815,
+            "heading": 0.5282283154384368,
+            "angularVelocity": 6.592812032299238e-8,
+            "velocityX": -3.366709432935023,
+            "velocityY": -1.7250814482051704,
+            "timestamp": 0.8553717991090941
+        },
+        {
+            "x": 5.499389832161033,
+            "y": 4.11883296395985,
+            "heading": 0.5282284192975907,
+            "angularVelocity": 0.0000013356188425542589,
+            "velocityX": -3.611840944307328,
+            "velocityY": -1.1248296920470104,
+            "timestamp": 0.9331328717553754
+        },
+        {
+            "x": 5.2375954046767585,
+            "y": 3.9846908767107623,
+            "heading": 0.5282448470390027,
+            "angularVelocity": 0.0002112591919891304,
+            "velocityX": -3.3666514436488617,
+            "velocityY": -1.7250544865716144,
+            "timestamp": 1.0108939444016567
+        },
+        {
+            "x": 4.9990709441074195,
+            "y": 3.8334805845295827,
+            "heading": 0.56802081206443,
+            "angularVelocity": 0.5115151279660829,
+            "velocityX": -3.067401881842984,
+            "velocityY": -1.944549978997939,
+            "timestamp": 1.0886550170479379
+        },
+        {
+            "x": 4.779271759525853,
+            "y": 3.6735037985529155,
+            "heading": 0.6417219019727908,
+            "angularVelocity": 0.9477890080506808,
+            "velocityX": -2.826596613210087,
+            "velocityY": -2.057286255609729,
+            "timestamp": 1.166416089694219
+        },
+        {
+            "x": 4.5625385904493,
+            "y": 3.4912294193947493,
+            "heading": 0.6783171685956464,
+            "angularVelocity": 0.47061164895862556,
+            "velocityX": -2.7871679453603333,
+            "velocityY": -2.3440311836655594,
+            "timestamp": 1.2441771623405002
+        },
+        {
+            "x": 4.372028277853454,
+            "y": 3.267088484433369,
+            "heading": 0.6783172621445687,
+            "angularVelocity": 0.0000012030302442626518,
+            "velocityX": -2.4499445045265382,
+            "velocityY": -2.8824311102413667,
+            "timestamp": 1.3219382349867814
+        },
+        {
+            "x": 4.1999208824099785,
+            "y": 3.028525386131778,
+            "heading": 0.678317151805174,
+            "angularVelocity": -0.0000014189541221835924,
+            "velocityX": -2.213284739864076,
+            "velocityY": -3.067898759405809,
+            "timestamp": 1.3996993076330626
+        },
+        {
+            "x": 4.015537599692405,
+            "y": 2.8383590216563426,
+            "heading": 0.6532286408155417,
+            "angularVelocity": -0.32263586568249475,
+            "velocityX": -2.371151482905778,
+            "velocityY": -2.4455213644037763,
+            "timestamp": 1.4774603802793438
+        },
+        {
+            "x": 3.8266113642227695,
+            "y": 2.6983370404519986,
+            "heading": 0.6187227841336836,
+            "angularVelocity": -0.4437420357974964,
+            "velocityX": -2.429573423311953,
+            "velocityY": -1.8006693637223163,
+            "timestamp": 1.555221452925625
+        },
+        {
+            "x": 3.635277950307217,
+            "y": 2.6086556112169066,
+            "heading": 0.5790475746735078,
+            "angularVelocity": -0.5102194209775099,
+            "velocityX": -2.460529509229992,
+            "velocityY": -1.1532946522360021,
+            "timestamp": 1.6329825255719062
+        },
+        {
+            "x": 3.4425296783447266,
+            "y": 2.5693652629852295,
+            "heading": 0.5361762744054714,
+            "angularVelocity": -0.5513208448531661,
+            "velocityX": -2.4787244491760223,
+            "velocityY": -0.5052701421751292,
+            "timestamp": 1.7107435982181873
+        },
+        {
+            "x": 3.273952117174966,
+            "y": 2.5733990398146904,
+            "heading": 0.49726118807033537,
+            "angularVelocity": -0.5743772146540196,
+            "velocityX": -2.4881638242809143,
+            "velocityY": 0.05953756545439798,
+            "timestamp": 1.7784953915385004
+        },
+        {
+            "x": 3.1059881626789445,
+            "y": 2.6157002802478813,
+            "heading": 0.456882179671172,
+            "angularVelocity": -0.5959843484623638,
+            "velocityX": -2.4791071389347814,
+            "velocityY": 0.6243560260198714,
+            "timestamp": 1.8462471848588136
+        },
+        {
+            "x": 2.9403803042277463,
+            "y": 2.6962017103764078,
+            "heading": 0.41517848324514556,
+            "angularVelocity": -0.6155364217277942,
+            "velocityX": -2.444331734043506,
+            "velocityY": 1.1881815400506988,
+            "timestamp": 1.9139989781791267
+        },
+        {
+            "x": 2.7796953039379972,
+            "y": 2.8146585273254012,
+            "heading": 0.3723609476398507,
+            "angularVelocity": -0.6319764172568042,
+            "velocityX": -2.3716715442508174,
+            "velocityY": 1.7483938231562413,
+            "timestamp": 1.9817507714994398
+        },
+        {
+            "x": 2.628009703912687,
+            "y": 2.970316258543535,
+            "heading": 0.3287788631859365,
+            "angularVelocity": -0.6432609724124856,
+            "velocityX": -2.23884258396202,
+            "velocityY": 2.2974702748047298,
+            "timestamp": 2.0495025648197527
+        },
+        {
+            "x": 2.4923790399755665,
+            "y": 3.160718496478581,
+            "heading": 0.28508218813026037,
+            "angularVelocity": -0.6449523018392979,
+            "velocityX": -2.001875630005736,
+            "velocityY": 2.8102907480968544,
+            "timestamp": 2.1172543581400656
+        },
+        {
+            "x": 2.384956097652455,
+            "y": 3.376988776463332,
+            "heading": 0.24257394609876834,
+            "angularVelocity": -0.627411319291935,
+            "velocityX": -1.5855365158416335,
+            "velocityY": 3.192096760631575,
+            "timestamp": 2.1850061514603785
+        },
+        {
+            "x": 2.3144199991578422,
+            "y": 3.6018152629752547,
+            "heading": 0.17996830737750358,
+            "angularVelocity": -0.9240440090682317,
+            "velocityX": -1.0410956675513523,
+            "velocityY": 3.3183842890918167,
+            "timestamp": 2.2527579447806914
+        },
+        {
+            "x": 2.277461106698391,
+            "y": 3.8129334421080285,
+            "heading": 0.07321465766316056,
+            "angularVelocity": -1.57565791963083,
+            "velocityX": -0.5455042685692306,
+            "velocityY": 3.1160530044520236,
+            "timestamp": 2.3205097381010042
+        },
+        {
+            "x": 2.2642620699526455,
+            "y": 3.9942911908489713,
+            "heading": -0.04721496632936169,
+            "angularVelocity": -1.7775119755599884,
+            "velocityX": -0.1948145738865332,
+            "velocityY": 2.676796286166625,
+            "timestamp": 2.388261531421317
+        },
+        {
+            "x": 2.2685016834700633,
+            "y": 4.1415779064179725,
+            "heading": -0.1677701441610935,
+            "angularVelocity": -1.7793651197065368,
+            "velocityX": 0.06257566493294611,
+            "velocityY": 2.173916118687331,
+            "timestamp": 2.45601332474163
+        },
+        {
+            "x": 2.2803498467055876,
+            "y": 4.251629868834139,
+            "heading": -0.2721018795383186,
+            "angularVelocity": -1.5399110527447033,
+            "velocityX": 0.17487600925202312,
+            "velocityY": 1.624340213341216,
+            "timestamp": 2.523765118061943
+        },
+        {
+            "x": 2.2924250535428135,
+            "y": 4.3241863896986095,
+            "heading": -0.3487735727111741,
+            "angularVelocity": -1.1316555535345214,
+            "velocityX": 0.17822711762236845,
+            "velocityY": 1.0709166105971757,
+            "timestamp": 2.591516911382256
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.6084920452794201,
+            "velocityX": 0.1118043683563332,
+            "velocityY": 0.5286001823165565,
+            "timestamp": 2.6592687047025687
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -7.720140427747849e-31,
+            "velocityX": 0,
+            "velocityY": -1.167304580662352e-31,
+            "timestamp": 2.7270204980228816
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.traj
new file mode 100644
index 00000000..cf63c7e3
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.traj	
@@ -0,0 +1,346 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": -2.981242670977675e-31,
+            "velocityX": -5.763030958279131e-32,
+            "velocityY": 3.5928963704249306e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6464505983184665,
+            "y": 5.761517456765686,
+            "heading": 0.04441523610418216,
+            "angularVelocity": 0.571175712894524,
+            "velocityX": -0.43144211544178024,
+            "velocityY": -0.4563021319898165,
+            "timestamp": 0.07776107264628128
+        },
+        {
+            "x": 7.57910774134765,
+            "y": 5.690410071738714,
+            "heading": 0.1294441379757556,
+            "angularVelocity": 1.0934635927458562,
+            "velocityX": -0.8660227370723712,
+            "velocityY": -0.914434209908928,
+            "timestamp": 0.15552214529256256
+        },
+        {
+            "x": 7.4776816969238515,
+            "y": 5.583411802725407,
+            "heading": 0.24873158333085876,
+            "angularVelocity": 1.5340252043296483,
+            "velocityX": -1.3043292867777738,
+            "velocityY": -1.3759875651409672,
+            "timestamp": 0.23328321793884382
+        },
+        {
+            "x": 7.341761998810456,
+            "y": 5.440105324158156,
+            "heading": 0.38885380810835574,
+            "angularVelocity": 1.8019584865410045,
+            "velocityX": -1.747914393255167,
+            "velocityY": -1.842907687489359,
+            "timestamp": 0.3110442905851251
+        },
+        {
+            "x": 7.171108690296084,
+            "y": 5.260470754663091,
+            "heading": 0.5147184350397563,
+            "angularVelocity": 1.6186071339824826,
+            "velocityX": -2.1945853202236263,
+            "velocityY": -2.3100834824152265,
+            "timestamp": 0.3888053632314064
+        },
+        {
+            "x": 6.970033766909185,
+            "y": 5.052418524467747,
+            "heading": 0.5282282166004705,
+            "angularVelocity": 0.17373450623768458,
+            "velocityX": -2.585804394720051,
+            "velocityY": -2.67553189681076,
+            "timestamp": 0.4665664358776877
+        },
+        {
+            "x": 6.768438732548067,
+            "y": 4.838192511419719,
+            "heading": 0.5282282675213732,
+            "angularVelocity": 6.548379670470849e-7,
+            "velocityX": -2.592492972391604,
+            "velocityY": -2.7549261572367425,
+            "timestamp": 0.5443275085239689
+        },
+        {
+            "x": 6.5423880710685225,
+            "y": 4.649952125007829,
+            "heading": 0.5282283002658353,
+            "angularVelocity": 4.210906690145718e-7,
+            "velocityX": -2.9069900116707847,
+            "velocityY": -2.42075346964613,
+            "timestamp": 0.6220885811702502
+        },
+        {
+            "x": 6.292219183977312,
+            "y": 4.49519788912002,
+            "heading": 0.5282283052888641,
+            "angularVelocity": 6.459567356551782e-8,
+            "velocityX": -3.217148099656167,
+            "velocityY": -1.9901247580747212,
+            "timestamp": 0.6998496538165315
+        },
+        {
+            "x": 6.0420493950115075,
+            "y": 4.3404451111724525,
+            "heading": 0.5282283103117955,
+            "angularVelocity": 6.459441933764687e-8,
+            "velocityX": -3.217159697677684,
+            "velocityY": -1.990106009101862,
+            "timestamp": 0.7776107264628128
+        },
+        {
+            "x": 5.780250458218128,
+            "y": 4.206300927357815,
+            "heading": 0.5282283154384368,
+            "angularVelocity": 6.592812032299238e-8,
+            "velocityX": -3.366709432935023,
+            "velocityY": -1.7250814482051704,
+            "timestamp": 0.8553717991090941
+        },
+        {
+            "x": 5.499389832161033,
+            "y": 4.11883296395985,
+            "heading": 0.5282284192975907,
+            "angularVelocity": 0.0000013356188425542589,
+            "velocityX": -3.611840944307328,
+            "velocityY": -1.1248296920470104,
+            "timestamp": 0.9331328717553754
+        },
+        {
+            "x": 5.2375954046767585,
+            "y": 3.9846908767107623,
+            "heading": 0.5282448470390027,
+            "angularVelocity": 0.0002112591919891304,
+            "velocityX": -3.3666514436488617,
+            "velocityY": -1.7250544865716144,
+            "timestamp": 1.0108939444016567
+        },
+        {
+            "x": 4.9990709441074195,
+            "y": 3.8334805845295827,
+            "heading": 0.56802081206443,
+            "angularVelocity": 0.5115151279660829,
+            "velocityX": -3.067401881842984,
+            "velocityY": -1.944549978997939,
+            "timestamp": 1.0886550170479379
+        },
+        {
+            "x": 4.779271759525853,
+            "y": 3.6735037985529155,
+            "heading": 0.6417219019727908,
+            "angularVelocity": 0.9477890080506808,
+            "velocityX": -2.826596613210087,
+            "velocityY": -2.057286255609729,
+            "timestamp": 1.166416089694219
+        },
+        {
+            "x": 4.5625385904493,
+            "y": 3.4912294193947493,
+            "heading": 0.6783171685956464,
+            "angularVelocity": 0.47061164895862556,
+            "velocityX": -2.7871679453603333,
+            "velocityY": -2.3440311836655594,
+            "timestamp": 1.2441771623405002
+        },
+        {
+            "x": 4.372028277853454,
+            "y": 3.267088484433369,
+            "heading": 0.6783172621445687,
+            "angularVelocity": 0.0000012030302442626518,
+            "velocityX": -2.4499445045265382,
+            "velocityY": -2.8824311102413667,
+            "timestamp": 1.3219382349867814
+        },
+        {
+            "x": 4.1999208824099785,
+            "y": 3.028525386131778,
+            "heading": 0.678317151805174,
+            "angularVelocity": -0.0000014189541221835924,
+            "velocityX": -2.213284739864076,
+            "velocityY": -3.067898759405809,
+            "timestamp": 1.3996993076330626
+        },
+        {
+            "x": 4.015537599692405,
+            "y": 2.8383590216563426,
+            "heading": 0.6532286408155417,
+            "angularVelocity": -0.32263586568249475,
+            "velocityX": -2.371151482905778,
+            "velocityY": -2.4455213644037763,
+            "timestamp": 1.4774603802793438
+        },
+        {
+            "x": 3.8266113642227695,
+            "y": 2.6983370404519986,
+            "heading": 0.6187227841336836,
+            "angularVelocity": -0.4437420357974964,
+            "velocityX": -2.429573423311953,
+            "velocityY": -1.8006693637223163,
+            "timestamp": 1.555221452925625
+        },
+        {
+            "x": 3.635277950307217,
+            "y": 2.6086556112169066,
+            "heading": 0.5790475746735078,
+            "angularVelocity": -0.5102194209775099,
+            "velocityX": -2.460529509229992,
+            "velocityY": -1.1532946522360021,
+            "timestamp": 1.6329825255719062
+        },
+        {
+            "x": 3.4425296783447266,
+            "y": 2.5693652629852295,
+            "heading": 0.5361762744054714,
+            "angularVelocity": -0.5513208448531661,
+            "velocityX": -2.4787244491760223,
+            "velocityY": -0.5052701421751292,
+            "timestamp": 1.7107435982181873
+        },
+        {
+            "x": 3.273952117174966,
+            "y": 2.5733990398146904,
+            "heading": 0.49726118807033537,
+            "angularVelocity": -0.5743772146540196,
+            "velocityX": -2.4881638242809143,
+            "velocityY": 0.05953756545439798,
+            "timestamp": 1.7784953915385004
+        },
+        {
+            "x": 3.1059881626789445,
+            "y": 2.6157002802478813,
+            "heading": 0.456882179671172,
+            "angularVelocity": -0.5959843484623638,
+            "velocityX": -2.4791071389347814,
+            "velocityY": 0.6243560260198714,
+            "timestamp": 1.8462471848588136
+        },
+        {
+            "x": 2.9403803042277463,
+            "y": 2.6962017103764078,
+            "heading": 0.41517848324514556,
+            "angularVelocity": -0.6155364217277942,
+            "velocityX": -2.444331734043506,
+            "velocityY": 1.1881815400506988,
+            "timestamp": 1.9139989781791267
+        },
+        {
+            "x": 2.7796953039379972,
+            "y": 2.8146585273254012,
+            "heading": 0.3723609476398507,
+            "angularVelocity": -0.6319764172568042,
+            "velocityX": -2.3716715442508174,
+            "velocityY": 1.7483938231562413,
+            "timestamp": 1.9817507714994398
+        },
+        {
+            "x": 2.628009703912687,
+            "y": 2.970316258543535,
+            "heading": 0.3287788631859365,
+            "angularVelocity": -0.6432609724124856,
+            "velocityX": -2.23884258396202,
+            "velocityY": 2.2974702748047298,
+            "timestamp": 2.0495025648197527
+        },
+        {
+            "x": 2.4923790399755665,
+            "y": 3.160718496478581,
+            "heading": 0.28508218813026037,
+            "angularVelocity": -0.6449523018392979,
+            "velocityX": -2.001875630005736,
+            "velocityY": 2.8102907480968544,
+            "timestamp": 2.1172543581400656
+        },
+        {
+            "x": 2.384956097652455,
+            "y": 3.376988776463332,
+            "heading": 0.24257394609876834,
+            "angularVelocity": -0.627411319291935,
+            "velocityX": -1.5855365158416335,
+            "velocityY": 3.192096760631575,
+            "timestamp": 2.1850061514603785
+        },
+        {
+            "x": 2.3144199991578422,
+            "y": 3.6018152629752547,
+            "heading": 0.17996830737750358,
+            "angularVelocity": -0.9240440090682317,
+            "velocityX": -1.0410956675513523,
+            "velocityY": 3.3183842890918167,
+            "timestamp": 2.2527579447806914
+        },
+        {
+            "x": 2.277461106698391,
+            "y": 3.8129334421080285,
+            "heading": 0.07321465766316056,
+            "angularVelocity": -1.57565791963083,
+            "velocityX": -0.5455042685692306,
+            "velocityY": 3.1160530044520236,
+            "timestamp": 2.3205097381010042
+        },
+        {
+            "x": 2.2642620699526455,
+            "y": 3.9942911908489713,
+            "heading": -0.04721496632936169,
+            "angularVelocity": -1.7775119755599884,
+            "velocityX": -0.1948145738865332,
+            "velocityY": 2.676796286166625,
+            "timestamp": 2.388261531421317
+        },
+        {
+            "x": 2.2685016834700633,
+            "y": 4.1415779064179725,
+            "heading": -0.1677701441610935,
+            "angularVelocity": -1.7793651197065368,
+            "velocityX": 0.06257566493294611,
+            "velocityY": 2.173916118687331,
+            "timestamp": 2.45601332474163
+        },
+        {
+            "x": 2.2803498467055876,
+            "y": 4.251629868834139,
+            "heading": -0.2721018795383186,
+            "angularVelocity": -1.5399110527447033,
+            "velocityX": 0.17487600925202312,
+            "velocityY": 1.624340213341216,
+            "timestamp": 2.523765118061943
+        },
+        {
+            "x": 2.2924250535428135,
+            "y": 4.3241863896986095,
+            "heading": -0.3487735727111741,
+            "angularVelocity": -1.1316555535345214,
+            "velocityX": 0.17822711762236845,
+            "velocityY": 1.0709166105971757,
+            "timestamp": 2.591516911382256
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.6084920452794201,
+            "velocityX": 0.1118043683563332,
+            "velocityY": 0.5286001823165565,
+            "timestamp": 2.6592687047025687
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -7.720140427747849e-31,
+            "velocityX": 0,
+            "velocityY": -1.167304580662352e-31,
+            "timestamp": 2.7270204980228816
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.1.traj
index 69543755..57334e24 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.1.traj	
@@ -4,136 +4,136 @@
             "x": 7.68,
             "y": 4.127,
             "heading": 0,
-            "angularVelocity": 1.212112656390561e-31,
-            "velocityX": -5.041767818814111e-32,
-            "velocityY": -1.2487171964671538e-38,
+            "angularVelocity": -2.1749467140472343e-39,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.6800000701104585,
-            "y": 4.20344537200552,
-            "heading": 1.2930383103747041e-8,
-            "angularVelocity": 1.350424233383134e-7,
-            "velocityX": 7.322200986357225e-7,
-            "velocityY": 0.7983807350945226,
-            "timestamp": 0.09575052183150286
+            "x": 7.68,
+            "y": 4.203445372005539,
+            "heading": -7.837837605587582e-19,
+            "angularVelocity": -8.185686568731239e-18,
+            "velocityX": 1.377912473786286e-33,
+            "velocityY": 0.7983807350946673,
+            "timestamp": 0.09575052183150974
         },
         {
-            "x": 7.680000201769031,
-            "y": 4.356336112982647,
-            "heading": 3.8758528041627975e-8,
-            "angularVelocity": 2.6974415299903026e-7,
-            "velocityX": 0.0000013750167071257275,
-            "velocityY": 1.596761438503451,
-            "timestamp": 0.19150104366300572
+            "x": 7.68,
+            "y": 4.356336112982689,
+            "heading": -2.3565356003209568e-18,
+            "angularVelocity": -1.6425517161035916e-17,
+            "velocityX": 3.260516050321855e-34,
+            "velocityY": 1.5967614385035824,
+            "timestamp": 0.19150104366301948
         },
         {
-            "x": 7.680000386416211,
-            "y": 4.58567221686356,
-            "heading": 7.786709578005107e-8,
-            "angularVelocity": 4.0844231330927475e-7,
-            "velocityX": 0.000001928419471656301,
-            "velocityY": 2.395142078541207,
-            "timestamp": 0.2872515654945086
+            "x": 7.68,
+            "y": 4.5856722168636,
+            "heading": -4.728579560129296e-18,
+            "angularVelocity": -2.4773170043438095e-17,
+            "velocityX": -1.693775140889528e-33,
+            "velocityY": 2.3951420785410225,
+            "timestamp": 0.28725156549452924
         },
         {
-            "x": 7.6800006155005685,
-            "y": 4.891453665444849,
-            "heading": 1.3155130559553437e-7,
-            "angularVelocity": 5.606674749182053e-7,
-            "velocityX": 0.0000023925127342176106,
-            "velocityY": 3.1935225284661,
-            "timestamp": 0.38300208732601143
+            "x": 7.68,
+            "y": 4.891453665444746,
+            "heading": -7.932188825954499e-18,
+            "angularVelocity": -3.345787785122934e-17,
+            "velocityX": -9.607950389092107e-34,
+            "velocityY": 3.1935225284643685,
+            "timestamp": 0.38300208732603896
         },
         {
-            "x": 7.68000074582338,
-            "y": 5.253672199267105,
-            "heading": 1.3155130364873708e-7,
-            "angularVelocity": -1.7664134647076396e-14,
-            "velocityX": 0.000001361066521463048,
-            "velocityY": 3.7829405719549913,
-            "timestamp": 0.4787526091575143
+            "x": 7.68,
+            "y": 5.253672199266821,
+            "heading": -4.35728250993897e-17,
+            "angularVelocity": -3.722239377050204e-16,
+            "velocityX": -3.660377688643743e-33,
+            "velocityY": 3.7829405719528495,
+            "timestamp": 0.4787526091575487
         },
         {
-            "x": 7.680000798333519,
-            "y": 5.615890733089199,
-            "heading": 1.315513137062799e-7,
-            "angularVelocity": 1.0799702172100096e-13,
-            "velocityX": 5.484057877231364e-7,
-            "velocityY": 3.7829405719533282,
-            "timestamp": 0.5745031309890172
+            "x": 7.68,
+            "y": 5.61589073308894,
+            "heading": -3.405116375197899e-17,
+            "angularVelocity": 9.944239639932065e-17,
+            "velocityX": -5.791062266320423e-33,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.5745031309890585
         },
         {
-            "x": 7.680000776767743,
-            "y": 5.9781092669112965,
-            "heading": 1.3155132085570774e-7,
-            "angularVelocity": 7.667783798806564e-14,
-            "velocityX": -2.2522878887411694e-7,
-            "velocityY": 3.782940571953354,
-            "timestamp": 0.6702536528205201
+            "x": 7.68,
+            "y": 5.978109266911059,
+            "heading": 5.876472199762529e-18,
+            "angularVelocity": 4.169965362904391e-16,
+            "velocityX": -1.6171107443968654e-32,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.6702536528205683
         },
         {
-            "x": 7.680000686239199,
-            "y": 6.340327800733382,
-            "heading": 1.315513220333048e-7,
-            "angularVelocity": 1.2578458101628908e-14,
-            "velocityX": -9.454626397270524e-7,
-            "velocityY": 3.78294057195323,
-            "timestamp": 0.766004174652023
+            "x": 7.68,
+            "y": 6.340327800733179,
+            "heading": 2.547705878984039e-17,
+            "angularVelocity": 2.047047495424479e-16,
+            "velocityX": -4.3724642588945734e-33,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.766004174652078
         },
         {
-            "x": 7.680000533602743,
-            "y": 6.702546334555452,
-            "heading": 1.3155133191379775e-7,
-            "angularVelocity": 1.0229175193513549e-13,
-            "velocityX": -0.0000015941058350287079,
-            "velocityY": 3.7829405719530396,
-            "timestamp": 0.8617546964835259
+            "x": 7.68,
+            "y": 6.702546334555254,
+            "heading": 7.687233753276096e-18,
+            "angularVelocity": -1.8579350479017399e-16,
+            "velocityX": -2.6103711792186515e-33,
+            "velocityY": 3.7829405719528495,
+            "timestamp": 0.8617546964835878
         },
         {
-            "x": 7.680000329705533,
-            "y": 7.00832778313656,
-            "heading": 7.923155128573452e-8,
-            "angularVelocity": -5.46417642222627e-7,
-            "velocityX": -0.000002129463080315811,
-            "velocityY": 3.1935225284642037,
-            "timestamp": 0.9575052183150288
+            "x": 7.68,
+            "y": 7.0083277831364,
+            "heading": 4.613320516549708e-18,
+            "angularVelocity": -3.210335754994756e-17,
+            "velocityX": 1.2273832122580678e-32,
+            "velocityY": 3.1935225284643685,
+            "timestamp": 0.9575052183150976
         },
         {
-            "x": 7.680000169486885,
-            "y": 7.2376638870173995,
-            "heading": 3.947148220681983e-8,
-            "angularVelocity": -4.152464695661009e-7,
-            "velocityX": -0.0000016732926513675465,
-            "velocityY": 2.395142078540454,
-            "timestamp": 1.0532557401465317
+            "x": 7.68,
+            "y": 7.237663887017311,
+            "heading": 2.3068261479777784e-18,
+            "angularVelocity": -2.4088582761110343e-17,
+            "velocityX": 8.179933156696061e-33,
+            "velocityY": 2.3951420785410225,
+            "timestamp": 1.0532557401466074
         },
         {
-            "x": 7.680000058003377,
-            "y": 7.390554627994493,
-            "heading": 1.303400507527686e-8,
-            "angularVelocity": -2.7610790251013916e-7,
-            "velocityX": -0.000001164312243995105,
-            "velocityY": 1.5967614385030973,
-            "timestamp": 1.1490062619780346
+            "x": 7.68,
+            "y": 7.390554627994461,
+            "heading": 7.689558058679801e-19,
+            "angularVelocity": -1.6061221530256843e-17,
+            "velocityX": 9.843160057308665e-33,
+            "velocityY": 1.5967614385035824,
+            "timestamp": 1.1490062619781172
         },
         {
             "x": 7.68,
             "y": 7.467,
             "heading": 0,
-            "angularVelocity": -1.3612463214689187e-7,
-            "velocityX": -6.057760896118273e-7,
-            "velocityY": 0.7983807350943913,
-            "timestamp": 1.2447567838095375
+            "angularVelocity": -8.030826271263815e-18,
+            "velocityX": 3.1854559448557055e-33,
+            "velocityY": 0.7983807350946673,
+            "timestamp": 1.244756783809627
         },
         {
             "x": 7.68,
             "y": 7.467,
             "heading": 0,
             "angularVelocity": 0,
-            "velocityX": 1.2167404646059922e-31,
-            "velocityY": -2.3530958630073903e-37,
-            "timestamp": 1.3405073056410404
+            "velocityX": 2.257351446612169e-43,
+            "velocityY": -1.1946741683702123e-38,
+            "timestamp": 1.3405073056411367
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.traj
index 69543755..57334e24 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.traj	
@@ -4,136 +4,136 @@
             "x": 7.68,
             "y": 4.127,
             "heading": 0,
-            "angularVelocity": 1.212112656390561e-31,
-            "velocityX": -5.041767818814111e-32,
-            "velocityY": -1.2487171964671538e-38,
+            "angularVelocity": -2.1749467140472343e-39,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.6800000701104585,
-            "y": 4.20344537200552,
-            "heading": 1.2930383103747041e-8,
-            "angularVelocity": 1.350424233383134e-7,
-            "velocityX": 7.322200986357225e-7,
-            "velocityY": 0.7983807350945226,
-            "timestamp": 0.09575052183150286
+            "x": 7.68,
+            "y": 4.203445372005539,
+            "heading": -7.837837605587582e-19,
+            "angularVelocity": -8.185686568731239e-18,
+            "velocityX": 1.377912473786286e-33,
+            "velocityY": 0.7983807350946673,
+            "timestamp": 0.09575052183150974
         },
         {
-            "x": 7.680000201769031,
-            "y": 4.356336112982647,
-            "heading": 3.8758528041627975e-8,
-            "angularVelocity": 2.6974415299903026e-7,
-            "velocityX": 0.0000013750167071257275,
-            "velocityY": 1.596761438503451,
-            "timestamp": 0.19150104366300572
+            "x": 7.68,
+            "y": 4.356336112982689,
+            "heading": -2.3565356003209568e-18,
+            "angularVelocity": -1.6425517161035916e-17,
+            "velocityX": 3.260516050321855e-34,
+            "velocityY": 1.5967614385035824,
+            "timestamp": 0.19150104366301948
         },
         {
-            "x": 7.680000386416211,
-            "y": 4.58567221686356,
-            "heading": 7.786709578005107e-8,
-            "angularVelocity": 4.0844231330927475e-7,
-            "velocityX": 0.000001928419471656301,
-            "velocityY": 2.395142078541207,
-            "timestamp": 0.2872515654945086
+            "x": 7.68,
+            "y": 4.5856722168636,
+            "heading": -4.728579560129296e-18,
+            "angularVelocity": -2.4773170043438095e-17,
+            "velocityX": -1.693775140889528e-33,
+            "velocityY": 2.3951420785410225,
+            "timestamp": 0.28725156549452924
         },
         {
-            "x": 7.6800006155005685,
-            "y": 4.891453665444849,
-            "heading": 1.3155130559553437e-7,
-            "angularVelocity": 5.606674749182053e-7,
-            "velocityX": 0.0000023925127342176106,
-            "velocityY": 3.1935225284661,
-            "timestamp": 0.38300208732601143
+            "x": 7.68,
+            "y": 4.891453665444746,
+            "heading": -7.932188825954499e-18,
+            "angularVelocity": -3.345787785122934e-17,
+            "velocityX": -9.607950389092107e-34,
+            "velocityY": 3.1935225284643685,
+            "timestamp": 0.38300208732603896
         },
         {
-            "x": 7.68000074582338,
-            "y": 5.253672199267105,
-            "heading": 1.3155130364873708e-7,
-            "angularVelocity": -1.7664134647076396e-14,
-            "velocityX": 0.000001361066521463048,
-            "velocityY": 3.7829405719549913,
-            "timestamp": 0.4787526091575143
+            "x": 7.68,
+            "y": 5.253672199266821,
+            "heading": -4.35728250993897e-17,
+            "angularVelocity": -3.722239377050204e-16,
+            "velocityX": -3.660377688643743e-33,
+            "velocityY": 3.7829405719528495,
+            "timestamp": 0.4787526091575487
         },
         {
-            "x": 7.680000798333519,
-            "y": 5.615890733089199,
-            "heading": 1.315513137062799e-7,
-            "angularVelocity": 1.0799702172100096e-13,
-            "velocityX": 5.484057877231364e-7,
-            "velocityY": 3.7829405719533282,
-            "timestamp": 0.5745031309890172
+            "x": 7.68,
+            "y": 5.61589073308894,
+            "heading": -3.405116375197899e-17,
+            "angularVelocity": 9.944239639932065e-17,
+            "velocityX": -5.791062266320423e-33,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.5745031309890585
         },
         {
-            "x": 7.680000776767743,
-            "y": 5.9781092669112965,
-            "heading": 1.3155132085570774e-7,
-            "angularVelocity": 7.667783798806564e-14,
-            "velocityX": -2.2522878887411694e-7,
-            "velocityY": 3.782940571953354,
-            "timestamp": 0.6702536528205201
+            "x": 7.68,
+            "y": 5.978109266911059,
+            "heading": 5.876472199762529e-18,
+            "angularVelocity": 4.169965362904391e-16,
+            "velocityX": -1.6171107443968654e-32,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.6702536528205683
         },
         {
-            "x": 7.680000686239199,
-            "y": 6.340327800733382,
-            "heading": 1.315513220333048e-7,
-            "angularVelocity": 1.2578458101628908e-14,
-            "velocityX": -9.454626397270524e-7,
-            "velocityY": 3.78294057195323,
-            "timestamp": 0.766004174652023
+            "x": 7.68,
+            "y": 6.340327800733179,
+            "heading": 2.547705878984039e-17,
+            "angularVelocity": 2.047047495424479e-16,
+            "velocityX": -4.3724642588945734e-33,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.766004174652078
         },
         {
-            "x": 7.680000533602743,
-            "y": 6.702546334555452,
-            "heading": 1.3155133191379775e-7,
-            "angularVelocity": 1.0229175193513549e-13,
-            "velocityX": -0.0000015941058350287079,
-            "velocityY": 3.7829405719530396,
-            "timestamp": 0.8617546964835259
+            "x": 7.68,
+            "y": 6.702546334555254,
+            "heading": 7.687233753276096e-18,
+            "angularVelocity": -1.8579350479017399e-16,
+            "velocityX": -2.6103711792186515e-33,
+            "velocityY": 3.7829405719528495,
+            "timestamp": 0.8617546964835878
         },
         {
-            "x": 7.680000329705533,
-            "y": 7.00832778313656,
-            "heading": 7.923155128573452e-8,
-            "angularVelocity": -5.46417642222627e-7,
-            "velocityX": -0.000002129463080315811,
-            "velocityY": 3.1935225284642037,
-            "timestamp": 0.9575052183150288
+            "x": 7.68,
+            "y": 7.0083277831364,
+            "heading": 4.613320516549708e-18,
+            "angularVelocity": -3.210335754994756e-17,
+            "velocityX": 1.2273832122580678e-32,
+            "velocityY": 3.1935225284643685,
+            "timestamp": 0.9575052183150976
         },
         {
-            "x": 7.680000169486885,
-            "y": 7.2376638870173995,
-            "heading": 3.947148220681983e-8,
-            "angularVelocity": -4.152464695661009e-7,
-            "velocityX": -0.0000016732926513675465,
-            "velocityY": 2.395142078540454,
-            "timestamp": 1.0532557401465317
+            "x": 7.68,
+            "y": 7.237663887017311,
+            "heading": 2.3068261479777784e-18,
+            "angularVelocity": -2.4088582761110343e-17,
+            "velocityX": 8.179933156696061e-33,
+            "velocityY": 2.3951420785410225,
+            "timestamp": 1.0532557401466074
         },
         {
-            "x": 7.680000058003377,
-            "y": 7.390554627994493,
-            "heading": 1.303400507527686e-8,
-            "angularVelocity": -2.7610790251013916e-7,
-            "velocityX": -0.000001164312243995105,
-            "velocityY": 1.5967614385030973,
-            "timestamp": 1.1490062619780346
+            "x": 7.68,
+            "y": 7.390554627994461,
+            "heading": 7.689558058679801e-19,
+            "angularVelocity": -1.6061221530256843e-17,
+            "velocityX": 9.843160057308665e-33,
+            "velocityY": 1.5967614385035824,
+            "timestamp": 1.1490062619781172
         },
         {
             "x": 7.68,
             "y": 7.467,
             "heading": 0,
-            "angularVelocity": -1.3612463214689187e-7,
-            "velocityX": -6.057760896118273e-7,
-            "velocityY": 0.7983807350943913,
-            "timestamp": 1.2447567838095375
+            "angularVelocity": -8.030826271263815e-18,
+            "velocityX": 3.1854559448557055e-33,
+            "velocityY": 0.7983807350946673,
+            "timestamp": 1.244756783809627
         },
         {
             "x": 7.68,
             "y": 7.467,
             "heading": 0,
             "angularVelocity": 0,
-            "velocityX": 1.2167404646059922e-31,
-            "velocityY": -2.3530958630073903e-37,
-            "timestamp": 1.3405073056410404
+            "velocityX": 2.257351446612169e-43,
+            "velocityY": -1.1946741683702123e-38,
+            "timestamp": 1.3405073056411367
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C2.1.traj
new file mode 100644
index 00000000..5ffe4bce
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C2.1.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -3.817834980565894e-40,
+            "angularVelocity": -1.9855221195343998e-39,
+            "velocityX": -9.621145750064654e-40,
+            "velocityY": 1.4575297691012433e-39,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 4.210500005575846,
+            "heading": 3.479114156010015e-22,
+            "angularVelocity": 3.4766412561552746e-21,
+            "velocityX": 2.7068848151306367e-33,
+            "velocityY": 0.8344065481228213,
+            "timestamp": 0.10007112931184182
+        },
+        {
+            "x": 7.68,
+            "y": 4.377500013629847,
+            "heading": 1.081691491825702e-21,
+            "angularVelocity": 7.332585052389997e-21,
+            "velocityX": 5.1589068200097806e-33,
+            "velocityY": 1.6688130652907358,
+            "timestamp": 0.20014225862368365
+        },
+        {
+            "x": 7.68,
+            "y": 4.628000017966616,
+            "heading": 2.2511586271387426e-21,
+            "angularVelocity": 1.1686358860616172e-20,
+            "velocityX": 5.8498916949463956e-33,
+            "velocityY": 2.503219520548835,
+            "timestamp": 0.3002133879355255
+        },
+        {
+            "x": 7.68,
+            "y": 4.962,
+            "heading": 4.481673966100434e-21,
+            "angularVelocity": 2.228929929167172e-20,
+            "velocityX": 6.576791251337367e-33,
+            "velocityY": 3.3376257900775013,
+            "timestamp": 0.4002845172473673
+        },
+        {
+            "x": 7.68,
+            "y": 5.295999982033384,
+            "heading": 1.4391637904192512e-21,
+            "angularVelocity": -3.0403475801105514e-20,
+            "velocityX": 1.0509973010204301e-32,
+            "velocityY": 3.3376257900775013,
+            "timestamp": 0.5003556465592092
+        },
+        {
+            "x": 7.68,
+            "y": 5.5464999863701525,
+            "heading": 4.7457500532831525e-22,
+            "angularVelocity": -9.63903176962576e-21,
+            "velocityX": 7.983324746463204e-33,
+            "velocityY": 2.503219520548835,
+            "timestamp": 0.600426775871051
+        },
+        {
+            "x": 7.68,
+            "y": 5.713499994424153,
+            "heading": 1.0909640932435146e-22,
+            "angularVelocity": -3.652188228166023e-21,
+            "velocityX": 7.341551448907972e-33,
+            "velocityY": 1.6688130652907358,
+            "timestamp": 0.7004979051828928
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -9.42965601107167e-40,
+            "angularVelocity": -1.09018866193586e-21,
+            "velocityX": 7.44470103227488e-33,
+            "velocityY": 0.8344065481228213,
+            "timestamp": 0.8005690344947346
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 8.249839679439821e-43,
+            "angularVelocity": 9.431229878048832e-39,
+            "velocityX": 1.012759498983901e-41,
+            "velocityY": 5.142787784847508e-39,
+            "timestamp": 0.9006401638065764
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C2.traj
new file mode 100644
index 00000000..5ffe4bce
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C2.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -3.817834980565894e-40,
+            "angularVelocity": -1.9855221195343998e-39,
+            "velocityX": -9.621145750064654e-40,
+            "velocityY": 1.4575297691012433e-39,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 4.210500005575846,
+            "heading": 3.479114156010015e-22,
+            "angularVelocity": 3.4766412561552746e-21,
+            "velocityX": 2.7068848151306367e-33,
+            "velocityY": 0.8344065481228213,
+            "timestamp": 0.10007112931184182
+        },
+        {
+            "x": 7.68,
+            "y": 4.377500013629847,
+            "heading": 1.081691491825702e-21,
+            "angularVelocity": 7.332585052389997e-21,
+            "velocityX": 5.1589068200097806e-33,
+            "velocityY": 1.6688130652907358,
+            "timestamp": 0.20014225862368365
+        },
+        {
+            "x": 7.68,
+            "y": 4.628000017966616,
+            "heading": 2.2511586271387426e-21,
+            "angularVelocity": 1.1686358860616172e-20,
+            "velocityX": 5.8498916949463956e-33,
+            "velocityY": 2.503219520548835,
+            "timestamp": 0.3002133879355255
+        },
+        {
+            "x": 7.68,
+            "y": 4.962,
+            "heading": 4.481673966100434e-21,
+            "angularVelocity": 2.228929929167172e-20,
+            "velocityX": 6.576791251337367e-33,
+            "velocityY": 3.3376257900775013,
+            "timestamp": 0.4002845172473673
+        },
+        {
+            "x": 7.68,
+            "y": 5.295999982033384,
+            "heading": 1.4391637904192512e-21,
+            "angularVelocity": -3.0403475801105514e-20,
+            "velocityX": 1.0509973010204301e-32,
+            "velocityY": 3.3376257900775013,
+            "timestamp": 0.5003556465592092
+        },
+        {
+            "x": 7.68,
+            "y": 5.5464999863701525,
+            "heading": 4.7457500532831525e-22,
+            "angularVelocity": -9.63903176962576e-21,
+            "velocityX": 7.983324746463204e-33,
+            "velocityY": 2.503219520548835,
+            "timestamp": 0.600426775871051
+        },
+        {
+            "x": 7.68,
+            "y": 5.713499994424153,
+            "heading": 1.0909640932435146e-22,
+            "angularVelocity": -3.652188228166023e-21,
+            "velocityX": 7.341551448907972e-33,
+            "velocityY": 1.6688130652907358,
+            "timestamp": 0.7004979051828928
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -9.42965601107167e-40,
+            "angularVelocity": -1.09018866193586e-21,
+            "velocityX": 7.44470103227488e-33,
+            "velocityY": 0.8344065481228213,
+            "timestamp": 0.8005690344947346
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 8.249839679439821e-43,
+            "angularVelocity": 9.431229878048832e-39,
+            "velocityX": 1.012759498983901e-41,
+            "velocityY": 5.142787784847508e-39,
+            "timestamp": 0.9006401638065764
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.1.traj
new file mode 100644
index 00000000..1862a6fc
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.1.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -1.6773104267984464e-40,
+            "angularVelocity": -1.9855220924111835e-39,
+            "velocityX": -5.3948128499400994e-40,
+            "velocityY": -2.7550648847397363e-40,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 4.043499994424153,
+            "heading": -2.181197470484939e-22,
+            "angularVelocity": -2.1796471324426728e-21,
+            "velocityX": 2.2751291494664054e-33,
+            "velocityY": -0.8344065481228213,
+            "timestamp": 0.10007112931184182
+        },
+        {
+            "x": 7.68,
+            "y": 3.8764999863701526,
+            "heading": -1.0976856482379068e-21,
+            "angularVelocity": -8.789407177499062e-21,
+            "velocityX": 3.967849397509383e-33,
+            "velocityY": -1.668813065290736,
+            "timestamp": 0.20014225862368365
+        },
+        {
+            "x": 7.68,
+            "y": 3.6259999820333837,
+            "heading": -3.357689035831156e-21,
+            "angularVelocity": -2.2583970074080117e-20,
+            "velocityX": 6.1318806903820435e-33,
+            "velocityY": -2.503219520548835,
+            "timestamp": 0.3002133879355255
+        },
+        {
+            "x": 7.68,
+            "y": 3.2919999999999994,
+            "heading": -1.0603568936345678e-20,
+            "angularVelocity": -7.2407296109284e-20,
+            "velocityX": 8.195284274358776e-33,
+            "velocityY": -3.3376257900775017,
+            "timestamp": 0.4002845172473673
+        },
+        {
+            "x": 7.68,
+            "y": 2.9580000179666155,
+            "heading": -4.785144304302103e-21,
+            "angularVelocity": 5.814288954429956e-20,
+            "velocityX": 7.586727405106972e-33,
+            "velocityY": -3.3376257900775013,
+            "timestamp": 0.5003556465592092
+        },
+        {
+            "x": 7.68,
+            "y": 2.7075000136298466,
+            "heading": -2.6182090767908737e-21,
+            "angularVelocity": 2.165395003165498e-20,
+            "velocityX": 5.326134827450793e-33,
+            "velocityY": -2.503219520548835,
+            "timestamp": 0.600426775871051
+        },
+        {
+            "x": 7.68,
+            "y": 2.540500005575846,
+            "heading": -9.668053802454109e-22,
+            "angularVelocity": 1.6502299039719674e-20,
+            "velocityX": 3.4348416075821124e-33,
+            "velocityY": -1.6688130652907358,
+            "timestamp": 0.7004979051828928
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 1.8181769494337196e-40,
+            "angularVelocity": 9.661181877631647e-21,
+            "velocityX": 1.7280403247932335e-33,
+            "velocityY": -0.8344065481228213,
+            "timestamp": 0.8005690344947346
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -1.6883012335149235e-41,
+            "angularVelocity": -1.9855220810412424e-39,
+            "velocityX": -1.2101303998889038e-42,
+            "velocityY": -7.163168700323315e-39,
+            "timestamp": 0.9006401638065764
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.traj
new file mode 100644
index 00000000..1862a6fc
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -1.6773104267984464e-40,
+            "angularVelocity": -1.9855220924111835e-39,
+            "velocityX": -5.3948128499400994e-40,
+            "velocityY": -2.7550648847397363e-40,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 4.043499994424153,
+            "heading": -2.181197470484939e-22,
+            "angularVelocity": -2.1796471324426728e-21,
+            "velocityX": 2.2751291494664054e-33,
+            "velocityY": -0.8344065481228213,
+            "timestamp": 0.10007112931184182
+        },
+        {
+            "x": 7.68,
+            "y": 3.8764999863701526,
+            "heading": -1.0976856482379068e-21,
+            "angularVelocity": -8.789407177499062e-21,
+            "velocityX": 3.967849397509383e-33,
+            "velocityY": -1.668813065290736,
+            "timestamp": 0.20014225862368365
+        },
+        {
+            "x": 7.68,
+            "y": 3.6259999820333837,
+            "heading": -3.357689035831156e-21,
+            "angularVelocity": -2.2583970074080117e-20,
+            "velocityX": 6.1318806903820435e-33,
+            "velocityY": -2.503219520548835,
+            "timestamp": 0.3002133879355255
+        },
+        {
+            "x": 7.68,
+            "y": 3.2919999999999994,
+            "heading": -1.0603568936345678e-20,
+            "angularVelocity": -7.2407296109284e-20,
+            "velocityX": 8.195284274358776e-33,
+            "velocityY": -3.3376257900775017,
+            "timestamp": 0.4002845172473673
+        },
+        {
+            "x": 7.68,
+            "y": 2.9580000179666155,
+            "heading": -4.785144304302103e-21,
+            "angularVelocity": 5.814288954429956e-20,
+            "velocityX": 7.586727405106972e-33,
+            "velocityY": -3.3376257900775013,
+            "timestamp": 0.5003556465592092
+        },
+        {
+            "x": 7.68,
+            "y": 2.7075000136298466,
+            "heading": -2.6182090767908737e-21,
+            "angularVelocity": 2.165395003165498e-20,
+            "velocityX": 5.326134827450793e-33,
+            "velocityY": -2.503219520548835,
+            "timestamp": 0.600426775871051
+        },
+        {
+            "x": 7.68,
+            "y": 2.540500005575846,
+            "heading": -9.668053802454109e-22,
+            "angularVelocity": 1.6502299039719674e-20,
+            "velocityX": 3.4348416075821124e-33,
+            "velocityY": -1.6688130652907358,
+            "timestamp": 0.7004979051828928
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 1.8181769494337196e-40,
+            "angularVelocity": 9.661181877631647e-21,
+            "velocityX": 1.7280403247932335e-33,
+            "velocityY": -0.8344065481228213,
+            "timestamp": 0.8005690344947346
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -1.6883012335149235e-41,
+            "angularVelocity": -1.9855220810412424e-39,
+            "velocityX": -1.2101303998889038e-42,
+            "velocityY": -7.163168700323315e-39,
+            "timestamp": 0.9006401638065764
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.1.traj
index c126c683..9c555123 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.1.traj	
@@ -4,136 +4,136 @@
             "x": 7.68,
             "y": 4.127,
             "heading": 0,
-            "angularVelocity": 2.405138964716482e-32,
-            "velocityX": 6.733008165659914e-33,
-            "velocityY": 1.175464991429931e-38,
+            "angularVelocity": -1.1842498081533988e-39,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.6800000704332305,
-            "y": 4.05055462799448,
-            "heading": -1.2377486895022226e-8,
-            "angularVelocity": -1.2926808085167386e-7,
-            "velocityX": 7.355910665177674e-7,
-            "velocityY": -0.7983807350945193,
-            "timestamp": 0.09575052183150283
+            "x": 7.68,
+            "y": 4.050554627994461,
+            "heading": 1.1070575354189048e-18,
+            "angularVelocity": 1.156189558439314e-17,
+            "velocityX": 1.1339790734570631e-33,
+            "velocityY": -0.7983807350946678,
+            "timestamp": 0.09575052183150978
         },
         {
-            "x": 7.680000202731891,
-            "y": 3.8976638870173534,
-            "heading": -3.8235321936944764e-8,
-            "angularVelocity": -2.700542284308498e-7,
-            "velocityX": 0.0000013817016646870076,
-            "velocityY": -1.5967614385034459,
-            "timestamp": 0.19150104366300566
+            "x": 7.68,
+            "y": 3.897663887017311,
+            "heading": 3.320268139408775e-18,
+            "angularVelocity": 2.3114345091427606e-17,
+            "velocityX": 5.50963145740529e-34,
+            "velocityY": -1.596761438503583,
+            "timestamp": 0.19150104366301957
         },
         {
-            "x": 7.6800003883277,
-            "y": 3.6683277831364425,
-            "heading": -7.893916226933331e-8,
-            "angularVelocity": -4.251030288838169e-7,
-            "velocityX": 0.0000019383267807539473,
-            "velocityY": -2.3951420785412023,
-            "timestamp": 0.2872515654945085
+            "x": 7.68,
+            "y": 3.6683277831364007,
+            "heading": 6.638014774282681e-18,
+            "angularVelocity": 3.4649906568297676e-17,
+            "velocityX": 4.767145470149766e-34,
+            "velocityY": -2.3951420785410233,
+            "timestamp": 0.28725156549452935
         },
         {
-            "x": 7.680000618655782,
-            "y": 3.3625463345551534,
-            "heading": -1.3615412726424167e-7,
-            "angularVelocity": -5.975419949298212e-7,
-            "velocityX": 0.0000024055019376452327,
-            "velocityY": -3.193522528466104,
-            "timestamp": 0.3830020873260113
+            "x": 7.68,
+            "y": 3.362546334555254,
+            "heading": 1.1056344671920077e-17,
+            "angularVelocity": 4.614418609717799e-17,
+            "velocityX": -5.447572473811757e-34,
+            "velocityY": -3.19352252846437,
+            "timestamp": 0.38300208732603913
         },
         {
-            "x": 7.680000750128537,
-            "y": 3.0003278007328986,
-            "heading": -1.3615412487460524e-7,
-            "angularVelocity": 2.2064625199669458e-14,
-            "velocityX": 0.000001373076316791352,
-            "velocityY": -3.7829405719549847,
-            "timestamp": 0.47875260915751416
+            "x": 7.68,
+            "y": 3.0003278007331793,
+            "heading": 2.1759975564527066e-17,
+            "angularVelocity": 1.11786658577609e-16,
+            "velocityX": -1.9220305893434716e-33,
+            "velocityY": -3.7829405719528495,
+            "timestamp": 0.4787526091575489
         },
         {
-            "x": 7.68000080341356,
-            "y": 2.638109266910804,
-            "heading": -1.3615413544372385e-7,
-            "angularVelocity": -1.1353706918076568e-13,
-            "velocityX": 5.564985227619036e-7,
-            "velocityY": -3.7829405719533264,
-            "timestamp": 0.574503130989017
+            "x": 7.68,
+            "y": 2.63810926691106,
+            "heading": -1.7246492437320545e-17,
+            "angularVelocity": -4.0737603572006183e-16,
+            "velocityX": 5.877602560652089e-33,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.5745031309890587
         },
         {
-            "x": 7.680000782147778,
-            "y": 2.2758907330887066,
-            "heading": -1.361541430398869e-7,
-            "angularVelocity": -8.149555996364277e-14,
-            "velocityX": -2.2209570916932486e-7,
-            "velocityY": -3.782940571953353,
-            "timestamp": 0.6702536528205197
+            "x": 7.68,
+            "y": 2.27589073308894,
+            "heading": -9.722837303851897e-18,
+            "angularVelocity": 7.857560449339153e-17,
+            "velocityX": 3.350655179860002e-33,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.6702536528205685
         },
         {
-            "x": 7.68000069137085,
-            "y": 1.9136721992666208,
-            "heading": -1.3615414447383612e-7,
-            "angularVelocity": -1.5331626502863808e-14,
-            "velocityX": -9.48056722512506e-7,
-            "velocityY": -3.7829405719532305,
-            "timestamp": 0.7660041746520225
+            "x": 7.68,
+            "y": 1.9136721992668206,
+            "heading": -4.541786403704224e-17,
+            "angularVelocity": -3.727919811862982e-16,
+            "velocityX": 1.4553646654154268e-32,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.7660041746520783
         },
         {
-            "x": 7.68000053789062,
-            "y": 1.55145366544455,
-            "heading": -1.361541542308917e-7,
-            "angularVelocity": -1.0101247732712604e-13,
-            "velocityX": -0.000001602918056057891,
-            "velocityY": -3.782940571953052,
-            "timestamp": 0.8617546964835253
+            "x": 7.68,
+            "y": 1.5514536654447455,
+            "heading": -1.11893051502424e-17,
+            "angularVelocity": 3.5747647356983727e-16,
+            "velocityX": -5.35720215261278e-33,
+            "velocityY": -3.7829405719528495,
+            "timestamp": 0.861754696483588
         },
         {
-            "x": 7.68000033240059,
-            "y": 1.2456722168634402,
-            "heading": -8.229196402929451e-8,
-            "angularVelocity": 5.625262643908853e-7,
-            "velocityX": -0.0000021460981663034263,
-            "velocityY": -3.193522528464227,
-            "timestamp": 0.9575052183150281
+            "x": 7.68,
+            "y": 1.2456722168635992,
+            "heading": -6.7032993083141955e-18,
+            "angularVelocity": 4.6850980602319425e-17,
+            "velocityX": -4.3527390380082413e-33,
+            "velocityY": -3.19352252846437,
+            "timestamp": 0.9575052183150978
         },
         {
-            "x": 7.680000170894954,
-            "y": 1.0163361129826005,
-            "heading": -4.114641273305121e-8,
-            "angularVelocity": 4.2971617502568956e-7,
-            "velocityX": -0.000001686733705584938,
-            "velocityY": -2.395142078540457,
-            "timestamp": 1.053255740146531
+            "x": 7.68,
+            "y": 1.016336112982689,
+            "heading": -3.348658686079055e-18,
+            "angularVelocity": 3.5035220254491646e-17,
+            "velocityX": -6.634490282826358e-33,
+            "velocityY": -2.395142078541024,
+            "timestamp": 1.0532557401466076
         },
         {
-            "x": 7.680000058492725,
-            "y": 0.8634453720055071,
-            "heading": -1.3639289939958608e-8,
-            "angularVelocity": 2.872790734007971e-7,
-            "velocityX": -0.0000011739071958177648,
-            "velocityY": -1.5967614385030962,
-            "timestamp": 1.149006261978034
+            "x": 7.68,
+            "y": 0.8634453720055388,
+            "heading": -1.115588951241814e-18,
+            "angularVelocity": 2.3321750022200153e-17,
+            "velocityX": -4.820835446954302e-33,
+            "velocityY": -1.596761438503583,
+            "timestamp": 1.1490062619781174
         },
         {
             "x": 7.68,
             "y": 0.787,
             "heading": 0,
-            "angularVelocity": 1.4244610959089762e-7,
-            "velocityX": -6.10886739716558e-7,
-            "velocityY": -0.7983807350943898,
-            "timestamp": 1.2447567838095368
+            "angularVelocity": 1.1650996045214628e-17,
+            "velocityX": -3.709395222464697e-33,
+            "velocityY": -0.7983807350946678,
+            "timestamp": 1.2447567838096272
         },
         {
             "x": 7.68,
             "y": 0.787,
             "heading": 0,
             "angularVelocity": 0,
-            "velocityX": 6.08432159346481e-32,
-            "velocityY": 1.351843649746573e-37,
-            "timestamp": 1.3405073056410397
+            "velocityX": 6.978360472267901e-43,
+            "velocityY": -3.6401965239950315e-38,
+            "timestamp": 1.340507305641137
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.traj
index c126c683..9c555123 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.traj	
@@ -4,136 +4,136 @@
             "x": 7.68,
             "y": 4.127,
             "heading": 0,
-            "angularVelocity": 2.405138964716482e-32,
-            "velocityX": 6.733008165659914e-33,
-            "velocityY": 1.175464991429931e-38,
+            "angularVelocity": -1.1842498081533988e-39,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.6800000704332305,
-            "y": 4.05055462799448,
-            "heading": -1.2377486895022226e-8,
-            "angularVelocity": -1.2926808085167386e-7,
-            "velocityX": 7.355910665177674e-7,
-            "velocityY": -0.7983807350945193,
-            "timestamp": 0.09575052183150283
+            "x": 7.68,
+            "y": 4.050554627994461,
+            "heading": 1.1070575354189048e-18,
+            "angularVelocity": 1.156189558439314e-17,
+            "velocityX": 1.1339790734570631e-33,
+            "velocityY": -0.7983807350946678,
+            "timestamp": 0.09575052183150978
         },
         {
-            "x": 7.680000202731891,
-            "y": 3.8976638870173534,
-            "heading": -3.8235321936944764e-8,
-            "angularVelocity": -2.700542284308498e-7,
-            "velocityX": 0.0000013817016646870076,
-            "velocityY": -1.5967614385034459,
-            "timestamp": 0.19150104366300566
+            "x": 7.68,
+            "y": 3.897663887017311,
+            "heading": 3.320268139408775e-18,
+            "angularVelocity": 2.3114345091427606e-17,
+            "velocityX": 5.50963145740529e-34,
+            "velocityY": -1.596761438503583,
+            "timestamp": 0.19150104366301957
         },
         {
-            "x": 7.6800003883277,
-            "y": 3.6683277831364425,
-            "heading": -7.893916226933331e-8,
-            "angularVelocity": -4.251030288838169e-7,
-            "velocityX": 0.0000019383267807539473,
-            "velocityY": -2.3951420785412023,
-            "timestamp": 0.2872515654945085
+            "x": 7.68,
+            "y": 3.6683277831364007,
+            "heading": 6.638014774282681e-18,
+            "angularVelocity": 3.4649906568297676e-17,
+            "velocityX": 4.767145470149766e-34,
+            "velocityY": -2.3951420785410233,
+            "timestamp": 0.28725156549452935
         },
         {
-            "x": 7.680000618655782,
-            "y": 3.3625463345551534,
-            "heading": -1.3615412726424167e-7,
-            "angularVelocity": -5.975419949298212e-7,
-            "velocityX": 0.0000024055019376452327,
-            "velocityY": -3.193522528466104,
-            "timestamp": 0.3830020873260113
+            "x": 7.68,
+            "y": 3.362546334555254,
+            "heading": 1.1056344671920077e-17,
+            "angularVelocity": 4.614418609717799e-17,
+            "velocityX": -5.447572473811757e-34,
+            "velocityY": -3.19352252846437,
+            "timestamp": 0.38300208732603913
         },
         {
-            "x": 7.680000750128537,
-            "y": 3.0003278007328986,
-            "heading": -1.3615412487460524e-7,
-            "angularVelocity": 2.2064625199669458e-14,
-            "velocityX": 0.000001373076316791352,
-            "velocityY": -3.7829405719549847,
-            "timestamp": 0.47875260915751416
+            "x": 7.68,
+            "y": 3.0003278007331793,
+            "heading": 2.1759975564527066e-17,
+            "angularVelocity": 1.11786658577609e-16,
+            "velocityX": -1.9220305893434716e-33,
+            "velocityY": -3.7829405719528495,
+            "timestamp": 0.4787526091575489
         },
         {
-            "x": 7.68000080341356,
-            "y": 2.638109266910804,
-            "heading": -1.3615413544372385e-7,
-            "angularVelocity": -1.1353706918076568e-13,
-            "velocityX": 5.564985227619036e-7,
-            "velocityY": -3.7829405719533264,
-            "timestamp": 0.574503130989017
+            "x": 7.68,
+            "y": 2.63810926691106,
+            "heading": -1.7246492437320545e-17,
+            "angularVelocity": -4.0737603572006183e-16,
+            "velocityX": 5.877602560652089e-33,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.5745031309890587
         },
         {
-            "x": 7.680000782147778,
-            "y": 2.2758907330887066,
-            "heading": -1.361541430398869e-7,
-            "angularVelocity": -8.149555996364277e-14,
-            "velocityX": -2.2209570916932486e-7,
-            "velocityY": -3.782940571953353,
-            "timestamp": 0.6702536528205197
+            "x": 7.68,
+            "y": 2.27589073308894,
+            "heading": -9.722837303851897e-18,
+            "angularVelocity": 7.857560449339153e-17,
+            "velocityX": 3.350655179860002e-33,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.6702536528205685
         },
         {
-            "x": 7.68000069137085,
-            "y": 1.9136721992666208,
-            "heading": -1.3615414447383612e-7,
-            "angularVelocity": -1.5331626502863808e-14,
-            "velocityX": -9.48056722512506e-7,
-            "velocityY": -3.7829405719532305,
-            "timestamp": 0.7660041746520225
+            "x": 7.68,
+            "y": 1.9136721992668206,
+            "heading": -4.541786403704224e-17,
+            "angularVelocity": -3.727919811862982e-16,
+            "velocityX": 1.4553646654154268e-32,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.7660041746520783
         },
         {
-            "x": 7.68000053789062,
-            "y": 1.55145366544455,
-            "heading": -1.361541542308917e-7,
-            "angularVelocity": -1.0101247732712604e-13,
-            "velocityX": -0.000001602918056057891,
-            "velocityY": -3.782940571953052,
-            "timestamp": 0.8617546964835253
+            "x": 7.68,
+            "y": 1.5514536654447455,
+            "heading": -1.11893051502424e-17,
+            "angularVelocity": 3.5747647356983727e-16,
+            "velocityX": -5.35720215261278e-33,
+            "velocityY": -3.7829405719528495,
+            "timestamp": 0.861754696483588
         },
         {
-            "x": 7.68000033240059,
-            "y": 1.2456722168634402,
-            "heading": -8.229196402929451e-8,
-            "angularVelocity": 5.625262643908853e-7,
-            "velocityX": -0.0000021460981663034263,
-            "velocityY": -3.193522528464227,
-            "timestamp": 0.9575052183150281
+            "x": 7.68,
+            "y": 1.2456722168635992,
+            "heading": -6.7032993083141955e-18,
+            "angularVelocity": 4.6850980602319425e-17,
+            "velocityX": -4.3527390380082413e-33,
+            "velocityY": -3.19352252846437,
+            "timestamp": 0.9575052183150978
         },
         {
-            "x": 7.680000170894954,
-            "y": 1.0163361129826005,
-            "heading": -4.114641273305121e-8,
-            "angularVelocity": 4.2971617502568956e-7,
-            "velocityX": -0.000001686733705584938,
-            "velocityY": -2.395142078540457,
-            "timestamp": 1.053255740146531
+            "x": 7.68,
+            "y": 1.016336112982689,
+            "heading": -3.348658686079055e-18,
+            "angularVelocity": 3.5035220254491646e-17,
+            "velocityX": -6.634490282826358e-33,
+            "velocityY": -2.395142078541024,
+            "timestamp": 1.0532557401466076
         },
         {
-            "x": 7.680000058492725,
-            "y": 0.8634453720055071,
-            "heading": -1.3639289939958608e-8,
-            "angularVelocity": 2.872790734007971e-7,
-            "velocityX": -0.0000011739071958177648,
-            "velocityY": -1.5967614385030962,
-            "timestamp": 1.149006261978034
+            "x": 7.68,
+            "y": 0.8634453720055388,
+            "heading": -1.115588951241814e-18,
+            "angularVelocity": 2.3321750022200153e-17,
+            "velocityX": -4.820835446954302e-33,
+            "velocityY": -1.596761438503583,
+            "timestamp": 1.1490062619781174
         },
         {
             "x": 7.68,
             "y": 0.787,
             "heading": 0,
-            "angularVelocity": 1.4244610959089762e-7,
-            "velocityX": -6.10886739716558e-7,
-            "velocityY": -0.7983807350943898,
-            "timestamp": 1.2447567838095368
+            "angularVelocity": 1.1650996045214628e-17,
+            "velocityX": -3.709395222464697e-33,
+            "velocityY": -0.7983807350946678,
+            "timestamp": 1.2447567838096272
         },
         {
             "x": 7.68,
             "y": 0.787,
             "heading": 0,
             "angularVelocity": 0,
-            "velocityX": 6.08432159346481e-32,
-            "velocityY": 1.351843649746573e-37,
-            "timestamp": 1.3405073056410397
+            "velocityX": 6.978360472267901e-43,
+            "velocityY": -3.6401965239950315e-38,
+            "timestamp": 1.340507305641137
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S4.1.traj
new file mode 100644
index 00000000..2274eedc
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S4.1.traj	
@@ -0,0 +1,238 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 3.185769476987751e-31,
+            "velocityX": 7.333541525795099e-32,
+            "velocityY": -1.3517842061565195e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 7.679078910576486,
+            "y": 4.080614551713116,
+            "heading": -0.023748331346269917,
+            "angularVelocity": -0.3167839884646853,
+            "velocityX": -0.012286605616975374,
+            "velocityY": -0.6187452541731726,
+            "timestamp": 0.07496695606800095
+        },
+        {
+            "x": 7.6772274783441,
+            "y": 3.987757014244699,
+            "heading": -0.06889026621284731,
+            "angularVelocity": -0.6021577670253255,
+            "velocityX": -0.024696644088144176,
+            "velocityY": -1.2386462294692595,
+            "timestamp": 0.1499339121360019
+        },
+        {
+            "x": 7.674435217742111,
+            "y": 3.848312298051719,
+            "heading": -0.1318457680879735,
+            "angularVelocity": -0.8397766853177616,
+            "velocityX": -0.037246551660047825,
+            "velocityY": -1.8600824083946148,
+            "timestamp": 0.22490086820400285
+        },
+        {
+            "x": 7.670687411814683,
+            "y": 3.6621317365631327,
+            "heading": -0.20653378464827088,
+            "angularVelocity": -0.996279167218362,
+            "velocityX": -0.049992771802326974,
+            "velocityY": -2.483501681990474,
+            "timestamp": 0.2998678242720038
+        },
+        {
+            "x": 7.665952301925262,
+            "y": 3.4291021726627307,
+            "heading": -0.27999342706200125,
+            "angularVelocity": -0.9798936260280143,
+            "velocityX": -0.06316262707958344,
+            "velocityY": -3.1084303821676644,
+            "timestamp": 0.3748347803400048
+        },
+        {
+            "x": 7.659724245559776,
+            "y": 3.1517739981457797,
+            "heading": -0.2973184560258276,
+            "angularVelocity": -0.23110220652567398,
+            "velocityX": -0.0830773542389579,
+            "velocityY": -3.699338869586643,
+            "timestamp": 0.44980173640800575
+        },
+        {
+            "x": 7.61848609595722,
+            "y": 2.8711927485616604,
+            "heading": -0.2973184850947423,
+            "angularVelocity": -3.8775636846908307e-7,
+            "velocityX": -0.5500843540339733,
+            "velocityY": -3.742732322352917,
+            "timestamp": 0.5247686924760067
+        },
+        {
+            "x": 7.5316071002697536,
+            "y": 2.6012326390018865,
+            "heading": -0.2973185190564447,
+            "angularVelocity": -4.530222940715824e-7,
+            "velocityX": -1.1588972027711093,
+            "velocityY": -3.6010547008857787,
+            "timestamp": 0.5997356485440076
+        },
+        {
+            "x": 7.401459139481823,
+            "y": 2.3492645367371554,
+            "heading": -0.29731856454052086,
+            "angularVelocity": -6.067216612944958e-7,
+            "velocityX": -1.7360710320141561,
+            "velocityY": -3.3610555300681284,
+            "timestamp": 0.6747026046120086
+        },
+        {
+            "x": 7.231595664022054,
+            "y": 2.1221680676798096,
+            "heading": -0.2973186386883636,
+            "angularVelocity": -9.89073675332688e-7,
+            "velocityX": -2.265844638345654,
+            "velocityY": -3.0292875817359826,
+            "timestamp": 0.7496695606800096
+        },
+        {
+            "x": 7.026654548839687,
+            "y": 1.9261437915792998,
+            "heading": -0.2973188151543491,
+            "angularVelocity": -0.000002353916908579506,
+            "velocityX": -2.7337526549226907,
+            "velocityY": -2.614809062311352,
+            "timestamp": 0.8246365167480105
+        },
+        {
+            "x": 6.792231559753418,
+            "y": 1.766543984413147,
+            "heading": -0.2973195709317279,
+            "angularVelocity": -0.00001008147347055336,
+            "velocityX": -3.127017573897922,
+            "velocityY": -2.1289354075064555,
+            "timestamp": 0.8996034728160115
+        },
+        {
+            "x": 6.610935711613446,
+            "y": 1.6724691195207,
+            "heading": -0.2973321063250889,
+            "angularVelocity": -0.00023216320074468077,
+            "velocityX": -3.3577106919370485,
+            "velocityY": -1.7423243992220112,
+            "timestamp": 0.9535973587619662
+        },
+        {
+            "x": 6.419882816475647,
+            "y": 1.600658398177247,
+            "heading": -0.29770902924967546,
+            "angularVelocity": -0.006980844552986395,
+            "velocityX": -3.538417207626563,
+            "velocityY": -1.329978757508455,
+            "timestamp": 1.007591244707921
+        },
+        {
+            "x": 6.223144239006287,
+            "y": 1.5524565272909876,
+            "heading": -0.30199019992690734,
+            "angularVelocity": -0.07928991592723345,
+            "velocityX": -3.6437195438439516,
+            "velocityY": -0.892728316211746,
+            "timestamp": 1.0615851306538757
+        },
+        {
+            "x": 6.027009837481886,
+            "y": 1.528287215105613,
+            "heading": -0.31920772077283077,
+            "angularVelocity": -0.31887908314582397,
+            "velocityX": -3.63252983348375,
+            "velocityY": -0.44763053745692566,
+            "timestamp": 1.1155790165998305
+        },
+        {
+            "x": 5.840489459577601,
+            "y": 1.5253276945335545,
+            "heading": -0.3622266817701244,
+            "angularVelocity": -0.7967376350788925,
+            "velocityX": -3.454472198777908,
+            "velocityY": -0.05481214252704887,
+            "timestamp": 1.1695729025457853
+        },
+        {
+            "x": 5.673855769295343,
+            "y": 1.5359985529910274,
+            "heading": -0.41631972448131044,
+            "angularVelocity": -1.0018364443214895,
+            "velocityX": -3.086158504113808,
+            "velocityY": 0.19763086635687102,
+            "timestamp": 1.22356678849174
+        },
+        {
+            "x": 5.530708201179271,
+            "y": 1.5529237053226916,
+            "heading": -0.47196047387840134,
+            "angularVelocity": -1.0305009247305976,
+            "velocityX": -2.651181066303626,
+            "velocityY": 0.3134642383140704,
+            "timestamp": 1.2775606744376948
+        },
+        {
+            "x": 5.411784437145291,
+            "y": 1.57153818466476,
+            "heading": -0.5236032237572267,
+            "angularVelocity": -0.956455512954414,
+            "velocityX": -2.2025413053807186,
+            "velocityY": 0.34475161429762524,
+            "timestamp": 1.3315545603836496
+        },
+        {
+            "x": 5.3170603467926,
+            "y": 1.5890782395606275,
+            "heading": -0.5679992830659408,
+            "angularVelocity": -0.8222423433861833,
+            "velocityX": -1.754348454332506,
+            "velocityY": 0.32485261226475975,
+            "timestamp": 1.3855484463296044
+        },
+        {
+            "x": 5.246334967071984,
+            "y": 1.6037618746917708,
+            "heading": -0.6030801100679523,
+            "angularVelocity": -0.6497185077054303,
+            "velocityX": -1.309877562644932,
+            "velocityY": 0.2719499601462664,
+            "timestamp": 1.4395423322755592
+        },
+        {
+            "x": 5.199384048260516,
+            "y": 1.6143645569232876,
+            "heading": -0.6274260701015184,
+            "angularVelocity": -0.45090216432919045,
+            "velocityX": -0.8695599138477248,
+            "velocityY": 0.19636820069091412,
+            "timestamp": 1.493536218221514
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -0.23287692075121041,
+            "velocityX": -0.4330869662524824,
+            "velocityY": 0.10437187429616973,
+            "timestamp": 1.5475301041674687
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 0,
+            "velocityX": 3.9288115092123874e-33,
+            "velocityY": 2.0888895254257856e-32,
+            "timestamp": 1.6015239901134235
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S4.traj
new file mode 100644
index 00000000..2274eedc
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S4.traj	
@@ -0,0 +1,238 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 3.185769476987751e-31,
+            "velocityX": 7.333541525795099e-32,
+            "velocityY": -1.3517842061565195e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 7.679078910576486,
+            "y": 4.080614551713116,
+            "heading": -0.023748331346269917,
+            "angularVelocity": -0.3167839884646853,
+            "velocityX": -0.012286605616975374,
+            "velocityY": -0.6187452541731726,
+            "timestamp": 0.07496695606800095
+        },
+        {
+            "x": 7.6772274783441,
+            "y": 3.987757014244699,
+            "heading": -0.06889026621284731,
+            "angularVelocity": -0.6021577670253255,
+            "velocityX": -0.024696644088144176,
+            "velocityY": -1.2386462294692595,
+            "timestamp": 0.1499339121360019
+        },
+        {
+            "x": 7.674435217742111,
+            "y": 3.848312298051719,
+            "heading": -0.1318457680879735,
+            "angularVelocity": -0.8397766853177616,
+            "velocityX": -0.037246551660047825,
+            "velocityY": -1.8600824083946148,
+            "timestamp": 0.22490086820400285
+        },
+        {
+            "x": 7.670687411814683,
+            "y": 3.6621317365631327,
+            "heading": -0.20653378464827088,
+            "angularVelocity": -0.996279167218362,
+            "velocityX": -0.049992771802326974,
+            "velocityY": -2.483501681990474,
+            "timestamp": 0.2998678242720038
+        },
+        {
+            "x": 7.665952301925262,
+            "y": 3.4291021726627307,
+            "heading": -0.27999342706200125,
+            "angularVelocity": -0.9798936260280143,
+            "velocityX": -0.06316262707958344,
+            "velocityY": -3.1084303821676644,
+            "timestamp": 0.3748347803400048
+        },
+        {
+            "x": 7.659724245559776,
+            "y": 3.1517739981457797,
+            "heading": -0.2973184560258276,
+            "angularVelocity": -0.23110220652567398,
+            "velocityX": -0.0830773542389579,
+            "velocityY": -3.699338869586643,
+            "timestamp": 0.44980173640800575
+        },
+        {
+            "x": 7.61848609595722,
+            "y": 2.8711927485616604,
+            "heading": -0.2973184850947423,
+            "angularVelocity": -3.8775636846908307e-7,
+            "velocityX": -0.5500843540339733,
+            "velocityY": -3.742732322352917,
+            "timestamp": 0.5247686924760067
+        },
+        {
+            "x": 7.5316071002697536,
+            "y": 2.6012326390018865,
+            "heading": -0.2973185190564447,
+            "angularVelocity": -4.530222940715824e-7,
+            "velocityX": -1.1588972027711093,
+            "velocityY": -3.6010547008857787,
+            "timestamp": 0.5997356485440076
+        },
+        {
+            "x": 7.401459139481823,
+            "y": 2.3492645367371554,
+            "heading": -0.29731856454052086,
+            "angularVelocity": -6.067216612944958e-7,
+            "velocityX": -1.7360710320141561,
+            "velocityY": -3.3610555300681284,
+            "timestamp": 0.6747026046120086
+        },
+        {
+            "x": 7.231595664022054,
+            "y": 2.1221680676798096,
+            "heading": -0.2973186386883636,
+            "angularVelocity": -9.89073675332688e-7,
+            "velocityX": -2.265844638345654,
+            "velocityY": -3.0292875817359826,
+            "timestamp": 0.7496695606800096
+        },
+        {
+            "x": 7.026654548839687,
+            "y": 1.9261437915792998,
+            "heading": -0.2973188151543491,
+            "angularVelocity": -0.000002353916908579506,
+            "velocityX": -2.7337526549226907,
+            "velocityY": -2.614809062311352,
+            "timestamp": 0.8246365167480105
+        },
+        {
+            "x": 6.792231559753418,
+            "y": 1.766543984413147,
+            "heading": -0.2973195709317279,
+            "angularVelocity": -0.00001008147347055336,
+            "velocityX": -3.127017573897922,
+            "velocityY": -2.1289354075064555,
+            "timestamp": 0.8996034728160115
+        },
+        {
+            "x": 6.610935711613446,
+            "y": 1.6724691195207,
+            "heading": -0.2973321063250889,
+            "angularVelocity": -0.00023216320074468077,
+            "velocityX": -3.3577106919370485,
+            "velocityY": -1.7423243992220112,
+            "timestamp": 0.9535973587619662
+        },
+        {
+            "x": 6.419882816475647,
+            "y": 1.600658398177247,
+            "heading": -0.29770902924967546,
+            "angularVelocity": -0.006980844552986395,
+            "velocityX": -3.538417207626563,
+            "velocityY": -1.329978757508455,
+            "timestamp": 1.007591244707921
+        },
+        {
+            "x": 6.223144239006287,
+            "y": 1.5524565272909876,
+            "heading": -0.30199019992690734,
+            "angularVelocity": -0.07928991592723345,
+            "velocityX": -3.6437195438439516,
+            "velocityY": -0.892728316211746,
+            "timestamp": 1.0615851306538757
+        },
+        {
+            "x": 6.027009837481886,
+            "y": 1.528287215105613,
+            "heading": -0.31920772077283077,
+            "angularVelocity": -0.31887908314582397,
+            "velocityX": -3.63252983348375,
+            "velocityY": -0.44763053745692566,
+            "timestamp": 1.1155790165998305
+        },
+        {
+            "x": 5.840489459577601,
+            "y": 1.5253276945335545,
+            "heading": -0.3622266817701244,
+            "angularVelocity": -0.7967376350788925,
+            "velocityX": -3.454472198777908,
+            "velocityY": -0.05481214252704887,
+            "timestamp": 1.1695729025457853
+        },
+        {
+            "x": 5.673855769295343,
+            "y": 1.5359985529910274,
+            "heading": -0.41631972448131044,
+            "angularVelocity": -1.0018364443214895,
+            "velocityX": -3.086158504113808,
+            "velocityY": 0.19763086635687102,
+            "timestamp": 1.22356678849174
+        },
+        {
+            "x": 5.530708201179271,
+            "y": 1.5529237053226916,
+            "heading": -0.47196047387840134,
+            "angularVelocity": -1.0305009247305976,
+            "velocityX": -2.651181066303626,
+            "velocityY": 0.3134642383140704,
+            "timestamp": 1.2775606744376948
+        },
+        {
+            "x": 5.411784437145291,
+            "y": 1.57153818466476,
+            "heading": -0.5236032237572267,
+            "angularVelocity": -0.956455512954414,
+            "velocityX": -2.2025413053807186,
+            "velocityY": 0.34475161429762524,
+            "timestamp": 1.3315545603836496
+        },
+        {
+            "x": 5.3170603467926,
+            "y": 1.5890782395606275,
+            "heading": -0.5679992830659408,
+            "angularVelocity": -0.8222423433861833,
+            "velocityX": -1.754348454332506,
+            "velocityY": 0.32485261226475975,
+            "timestamp": 1.3855484463296044
+        },
+        {
+            "x": 5.246334967071984,
+            "y": 1.6037618746917708,
+            "heading": -0.6030801100679523,
+            "angularVelocity": -0.6497185077054303,
+            "velocityX": -1.309877562644932,
+            "velocityY": 0.2719499601462664,
+            "timestamp": 1.4395423322755592
+        },
+        {
+            "x": 5.199384048260516,
+            "y": 1.6143645569232876,
+            "heading": -0.6274260701015184,
+            "angularVelocity": -0.45090216432919045,
+            "velocityX": -0.8695599138477248,
+            "velocityY": 0.19636820069091412,
+            "timestamp": 1.493536218221514
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -0.23287692075121041,
+            "velocityX": -0.4330869662524824,
+            "velocityY": 0.10437187429616973,
+            "timestamp": 1.5475301041674687
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 0,
+            "velocityX": 3.9288115092123874e-33,
+            "velocityY": 2.0888895254257856e-32,
+            "timestamp": 1.6015239901134235
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.1.traj
new file mode 100644
index 00000000..47660d82
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.1.traj	
@@ -0,0 +1,220 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 1.832751405105417e-34,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.608571974046609,
+            "y": 4.129750388677263,
+            "heading": -0.02440208863451916,
+            "angularVelocity": -0.26295738388657636,
+            "velocityX": -0.7697098032139116,
+            "velocityY": 0.029638242122484983,
+            "timestamp": 0.09279864392416992
+        },
+        {
+            "x": 7.46571594445247,
+            "y": 4.135252523590067,
+            "heading": -0.07320447592180841,
+            "angularVelocity": -0.5258954788948677,
+            "velocityX": -1.5394193659863489,
+            "velocityY": 0.05929111331948823,
+            "timestamp": 0.18559728784833984
+        },
+        {
+            "x": 7.25143160309568,
+            "y": 4.143508368339934,
+            "heading": -0.1463869418885076,
+            "angularVelocity": -0.7886156830752087,
+            "velocityX": -2.3091322490864354,
+            "velocityY": 0.08896514432486853,
+            "timestamp": 0.27839593177250976
+        },
+        {
+            "x": 6.965717910609067,
+            "y": 4.154520147774138,
+            "heading": -0.24389107124776152,
+            "angularVelocity": -1.050706402983198,
+            "velocityX": -3.078856332427471,
+            "velocityY": 0.1186631503279318,
+            "timestamp": 0.3711945756966797
+        },
+        {
+            "x": 6.614926201978592,
+            "y": 4.168029747138803,
+            "heading": -0.24389108357148506,
+            "angularVelocity": -1.3280068591073525e-7,
+            "velocityX": -3.780138305869253,
+            "velocityY": 0.1455797067002448,
+            "timestamp": 0.4639932196208496
+        },
+        {
+            "x": 6.264134491970206,
+            "y": 4.181539310738243,
+            "heading": -0.2438910958936344,
+            "angularVelocity": -1.3278372185354406e-7,
+            "velocityX": -3.7801383207176356,
+            "velocityY": 0.14557932129349355,
+            "timestamp": 0.5567918635450195
+        },
+        {
+            "x": 5.913342805476888,
+            "y": 4.195049484934711,
+            "heading": -0.2438911082157909,
+            "angularVelocity": -1.32783799697389e-7,
+            "velocityX": -3.780138067318815,
+            "velocityY": 0.14558590109896277,
+            "timestamp": 0.6495905074691894
+        },
+        {
+            "x": 5.568285864608835,
+            "y": 4.259648543779433,
+            "heading": -0.24389114467773698,
+            "angularVelocity": -3.929146432741106e-7,
+            "velocityX": -3.718340336417024,
+            "velocityY": 0.6961207202291138,
+            "timestamp": 0.7423891513933594
+        },
+        {
+            "x": 5.223229300660808,
+            "y": 4.324249615558004,
+            "heading": -0.2438911811402195,
+            "angularVelocity": -3.929204239211486e-7,
+            "velocityX": -3.7183362747195923,
+            "velocityY": 0.6961424116429128,
+            "timestamp": 0.8351877953175293
+        },
+        {
+            "x": 4.889178228675737,
+            "y": 4.432171862538056,
+            "heading": -0.2438912192984801,
+            "angularVelocity": -4.111941617608203e-7,
+            "velocityX": -3.5997408783046705,
+            "velocityY": 1.1629722420106654,
+            "timestamp": 0.9279864392416992
+        },
+        {
+            "x": 4.584073695522117,
+            "y": 4.605805251226935,
+            "heading": -0.24389116821114407,
+            "angularVelocity": 5.505181312773355e-7,
+            "velocityX": -3.287812410307864,
+            "velocityY": 1.8710767889103372,
+            "timestamp": 1.0207850831658691
+        },
+        {
+            "x": 4.320679858830311,
+            "y": 4.837885010533044,
+            "heading": -0.24389110572504064,
+            "angularVelocity": 6.733514712028696e-7,
+            "velocityX": -2.8383371303038154,
+            "velocityY": 2.5008960205899418,
+            "timestamp": 1.113583727090039
+        },
+        {
+            "x": 4.066259556785684,
+            "y": 5.079768532204951,
+            "heading": -0.24389104453640137,
+            "angularVelocity": 6.593699721479525e-7,
+            "velocityX": -2.7416381456233947,
+            "velocityY": 2.606541555386963,
+            "timestamp": 1.206382371014209
+        },
+        {
+            "x": 3.8118393280605467,
+            "y": 5.321652130996455,
+            "heading": -0.243890983347767,
+            "angularVelocity": 6.593699195322184e-7,
+            "velocityX": -2.741637355531139,
+            "velocityY": 2.606542386429263,
+            "timestamp": 1.299181014938379
+        },
+        {
+            "x": 3.557419123484207,
+            "y": 5.563535755188363,
+            "heading": -0.24389092215913197,
+            "angularVelocity": 6.593699267467298e-7,
+            "velocityX": -2.7416370953032203,
+            "velocityY": 2.606542660144498,
+            "timestamp": 1.3919796588625488
+        },
+        {
+            "x": 3.3029989365055474,
+            "y": 5.805419397890013,
+            "heading": -0.24389086097049373,
+            "angularVelocity": 6.593699609263459e-7,
+            "velocityX": -2.7416369056702816,
+            "velocityY": 2.606542859605842,
+            "timestamp": 1.4847783027867187
+        },
+        {
+            "x": 3.0485787435735707,
+            "y": 6.04730303432977,
+            "heading": -0.24389079978182,
+            "angularVelocity": 6.593703437871394e-7,
+            "velocityX": -2.741636969823341,
+            "velocityY": 2.6065427921275584,
+            "timestamp": 1.5775769467108887
+        },
+        {
+            "x": 2.7984729856087975,
+            "y": 6.275491677578651,
+            "heading": -0.21231080129448254,
+            "angularVelocity": 0.3403066807004949,
+            "velocityX": -2.6951445343225506,
+            "velocityY": 2.45896527793385,
+            "timestamp": 1.6703755906350586
+        },
+        {
+            "x": 2.596978431795094,
+            "y": 6.469397893884434,
+            "heading": -0.03732218251039644,
+            "angularVelocity": 1.8856807748945335,
+            "velocityX": -2.171309248638947,
+            "velocityY": 2.089537175394848,
+            "timestamp": 1.7631742345592285
+        },
+        {
+            "x": 2.4472498512530994,
+            "y": 6.613867328015145,
+            "heading": 0.15789573469158452,
+            "angularVelocity": 2.1036720898801096,
+            "velocityX": -1.6134781092743464,
+            "velocityY": 1.5568054447946784,
+            "timestamp": 1.8559728784833984
+        },
+        {
+            "x": 2.3487140682465175,
+            "y": 6.7094536627437265,
+            "heading": 0.3099829221841124,
+            "angularVelocity": 1.6388945038552638,
+            "velocityX": -1.0618235228426263,
+            "velocityY": 1.0300402105734285,
+            "timestamp": 1.9487715224075683
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0.8945936525078311,
+            "velocityX": -0.5249437511858832,
+            "velocityY": 0.5123602592202229,
+            "timestamp": 2.0415701663317383
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0,
+            "velocityX": 1.4564927652554838e-31,
+            "velocityY": 0,
+            "timestamp": 2.134368810255908
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.traj
new file mode 100644
index 00000000..47660d82
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.traj	
@@ -0,0 +1,220 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 1.832751405105417e-34,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.608571974046609,
+            "y": 4.129750388677263,
+            "heading": -0.02440208863451916,
+            "angularVelocity": -0.26295738388657636,
+            "velocityX": -0.7697098032139116,
+            "velocityY": 0.029638242122484983,
+            "timestamp": 0.09279864392416992
+        },
+        {
+            "x": 7.46571594445247,
+            "y": 4.135252523590067,
+            "heading": -0.07320447592180841,
+            "angularVelocity": -0.5258954788948677,
+            "velocityX": -1.5394193659863489,
+            "velocityY": 0.05929111331948823,
+            "timestamp": 0.18559728784833984
+        },
+        {
+            "x": 7.25143160309568,
+            "y": 4.143508368339934,
+            "heading": -0.1463869418885076,
+            "angularVelocity": -0.7886156830752087,
+            "velocityX": -2.3091322490864354,
+            "velocityY": 0.08896514432486853,
+            "timestamp": 0.27839593177250976
+        },
+        {
+            "x": 6.965717910609067,
+            "y": 4.154520147774138,
+            "heading": -0.24389107124776152,
+            "angularVelocity": -1.050706402983198,
+            "velocityX": -3.078856332427471,
+            "velocityY": 0.1186631503279318,
+            "timestamp": 0.3711945756966797
+        },
+        {
+            "x": 6.614926201978592,
+            "y": 4.168029747138803,
+            "heading": -0.24389108357148506,
+            "angularVelocity": -1.3280068591073525e-7,
+            "velocityX": -3.780138305869253,
+            "velocityY": 0.1455797067002448,
+            "timestamp": 0.4639932196208496
+        },
+        {
+            "x": 6.264134491970206,
+            "y": 4.181539310738243,
+            "heading": -0.2438910958936344,
+            "angularVelocity": -1.3278372185354406e-7,
+            "velocityX": -3.7801383207176356,
+            "velocityY": 0.14557932129349355,
+            "timestamp": 0.5567918635450195
+        },
+        {
+            "x": 5.913342805476888,
+            "y": 4.195049484934711,
+            "heading": -0.2438911082157909,
+            "angularVelocity": -1.32783799697389e-7,
+            "velocityX": -3.780138067318815,
+            "velocityY": 0.14558590109896277,
+            "timestamp": 0.6495905074691894
+        },
+        {
+            "x": 5.568285864608835,
+            "y": 4.259648543779433,
+            "heading": -0.24389114467773698,
+            "angularVelocity": -3.929146432741106e-7,
+            "velocityX": -3.718340336417024,
+            "velocityY": 0.6961207202291138,
+            "timestamp": 0.7423891513933594
+        },
+        {
+            "x": 5.223229300660808,
+            "y": 4.324249615558004,
+            "heading": -0.2438911811402195,
+            "angularVelocity": -3.929204239211486e-7,
+            "velocityX": -3.7183362747195923,
+            "velocityY": 0.6961424116429128,
+            "timestamp": 0.8351877953175293
+        },
+        {
+            "x": 4.889178228675737,
+            "y": 4.432171862538056,
+            "heading": -0.2438912192984801,
+            "angularVelocity": -4.111941617608203e-7,
+            "velocityX": -3.5997408783046705,
+            "velocityY": 1.1629722420106654,
+            "timestamp": 0.9279864392416992
+        },
+        {
+            "x": 4.584073695522117,
+            "y": 4.605805251226935,
+            "heading": -0.24389116821114407,
+            "angularVelocity": 5.505181312773355e-7,
+            "velocityX": -3.287812410307864,
+            "velocityY": 1.8710767889103372,
+            "timestamp": 1.0207850831658691
+        },
+        {
+            "x": 4.320679858830311,
+            "y": 4.837885010533044,
+            "heading": -0.24389110572504064,
+            "angularVelocity": 6.733514712028696e-7,
+            "velocityX": -2.8383371303038154,
+            "velocityY": 2.5008960205899418,
+            "timestamp": 1.113583727090039
+        },
+        {
+            "x": 4.066259556785684,
+            "y": 5.079768532204951,
+            "heading": -0.24389104453640137,
+            "angularVelocity": 6.593699721479525e-7,
+            "velocityX": -2.7416381456233947,
+            "velocityY": 2.606541555386963,
+            "timestamp": 1.206382371014209
+        },
+        {
+            "x": 3.8118393280605467,
+            "y": 5.321652130996455,
+            "heading": -0.243890983347767,
+            "angularVelocity": 6.593699195322184e-7,
+            "velocityX": -2.741637355531139,
+            "velocityY": 2.606542386429263,
+            "timestamp": 1.299181014938379
+        },
+        {
+            "x": 3.557419123484207,
+            "y": 5.563535755188363,
+            "heading": -0.24389092215913197,
+            "angularVelocity": 6.593699267467298e-7,
+            "velocityX": -2.7416370953032203,
+            "velocityY": 2.606542660144498,
+            "timestamp": 1.3919796588625488
+        },
+        {
+            "x": 3.3029989365055474,
+            "y": 5.805419397890013,
+            "heading": -0.24389086097049373,
+            "angularVelocity": 6.593699609263459e-7,
+            "velocityX": -2.7416369056702816,
+            "velocityY": 2.606542859605842,
+            "timestamp": 1.4847783027867187
+        },
+        {
+            "x": 3.0485787435735707,
+            "y": 6.04730303432977,
+            "heading": -0.24389079978182,
+            "angularVelocity": 6.593703437871394e-7,
+            "velocityX": -2.741636969823341,
+            "velocityY": 2.6065427921275584,
+            "timestamp": 1.5775769467108887
+        },
+        {
+            "x": 2.7984729856087975,
+            "y": 6.275491677578651,
+            "heading": -0.21231080129448254,
+            "angularVelocity": 0.3403066807004949,
+            "velocityX": -2.6951445343225506,
+            "velocityY": 2.45896527793385,
+            "timestamp": 1.6703755906350586
+        },
+        {
+            "x": 2.596978431795094,
+            "y": 6.469397893884434,
+            "heading": -0.03732218251039644,
+            "angularVelocity": 1.8856807748945335,
+            "velocityX": -2.171309248638947,
+            "velocityY": 2.089537175394848,
+            "timestamp": 1.7631742345592285
+        },
+        {
+            "x": 2.4472498512530994,
+            "y": 6.613867328015145,
+            "heading": 0.15789573469158452,
+            "angularVelocity": 2.1036720898801096,
+            "velocityX": -1.6134781092743464,
+            "velocityY": 1.5568054447946784,
+            "timestamp": 1.8559728784833984
+        },
+        {
+            "x": 2.3487140682465175,
+            "y": 6.7094536627437265,
+            "heading": 0.3099829221841124,
+            "angularVelocity": 1.6388945038552638,
+            "velocityX": -1.0618235228426263,
+            "velocityY": 1.0300402105734285,
+            "timestamp": 1.9487715224075683
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0.8945936525078311,
+            "velocityX": -0.5249437511858832,
+            "velocityY": 0.5123602592202229,
+            "timestamp": 2.0415701663317383
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0,
+            "velocityX": 1.4564927652554838e-31,
+            "velocityY": 0,
+            "timestamp": 2.134368810255908
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.1.traj
index 28199c95..1bf739bf 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.1.traj	
@@ -3,173 +3,173 @@
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 1.7868231110584375e-31,
-            "velocityX": 3.500790942409583e-35,
-            "velocityY": -4.724665555223456e-38,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -2.2542235108498663e-32,
             "timestamp": 0
         },
         {
-            "x": 7.6800001156222075,
-            "y": 2.5388441539517457,
-            "heading": 3.140000017932032,
-            "angularVelocity": 1.8099644459911076e-7,
-            "velocityX": 0.0000011670295392084272,
-            "velocityY": 0.8260917454273137,
-            "timestamp": 0.09907392781101554
-        },
-        {
-            "x": 7.680000338204962,
-            "y": 2.7025324587418944,
-            "heading": 3.1400000579849436,
-            "angularVelocity": 4.042728942464055e-7,
-            "velocityX": 0.000002246632826435788,
-            "velocityY": 1.6521834594301972,
-            "timestamp": 0.19814785562203108
-        },
-        {
-            "x": 7.680000658923826,
-            "y": 2.948064908143757,
-            "heading": 3.1400001223157976,
-            "angularVelocity": 6.493215714833696e-7,
-            "velocityX": 0.0000032371668547106284,
-            "velocityY": 2.4782751105841747,
-            "timestamp": 0.2972217834330466
-        },
-        {
-            "x": 7.6800010687648195,
-            "y": 3.2754414834772896,
-            "heading": 3.140000210384731,
-            "angularVelocity": 8.889210892181502e-7,
-            "velocityX": 0.0000041367184645969174,
-            "velocityY": 3.3043665731916554,
-            "timestamp": 0.39629571124406215
-        },
-        {
-            "x": 7.6800013918601255,
-            "y": 3.650232264926723,
-            "heading": 3.140000210384734,
-            "angularVelocity": 3.2741652225293424e-14,
-            "velocityX": 0.000003261153797251531,
-            "velocityY": 3.7829405750859055,
-            "timestamp": 0.4953696390550777
-        },
-        {
-            "x": 7.680001628786808,
-            "y": 4.025023046376236,
-            "heading": 3.140000210384741,
-            "angularVelocity": 7.304626800025654e-14,
-            "velocityX": 0.00000239141289972485,
-            "velocityY": 3.782940575086779,
-            "timestamp": 0.5944435668660932
-        },
-        {
-            "x": 7.6800017795018,
-            "y": 4.399813827825781,
-            "heading": 3.1400002103847475,
-            "angularVelocity": 7.220243449496737e-14,
-            "velocityX": 0.0000015212375955225791,
-            "velocityY": 3.782940575087123,
-            "timestamp": 0.6935174946771088
-        },
-        {
-            "x": 7.680001844060805,
-            "y": 4.774604609275342,
-            "heading": 3.140000210384754,
-            "angularVelocity": 6.886052815923548e-14,
-            "velocityX": 6.516245436906888e-7,
-            "velocityY": 3.782940575087321,
-            "timestamp": 0.7925914224881243
-        },
-        {
-            "x": 7.680001822974652,
-            "y": 5.14939539072491,
-            "heading": 3.1400002103847604,
-            "angularVelocity": 6.934745116017498e-14,
-            "velocityX": -2.1283245913448782e-7,
-            "velocityY": 3.782940575087379,
-            "timestamp": 0.8916653502991398
-        },
-        {
-            "x": 7.680001718396276,
-            "y": 5.5241861721744705,
-            "heading": 3.140000210384768,
-            "angularVelocity": 8.52485669839133e-14,
-            "velocityX": -0.0000010555588768470353,
-            "velocityY": 3.7829405750872986,
-            "timestamp": 0.9907392781101554
-        },
-        {
-            "x": 7.680001534024895,
-            "y": 5.898976953624011,
-            "heading": 3.140000210384774,
-            "angularVelocity": 6.71024495132872e-14,
-            "velocityX": -0.000001860947354605527,
-            "velocityY": 3.7829405750870757,
-            "timestamp": 1.089813205921171
-        },
-        {
-            "x": 7.680001275096042,
-            "y": 6.273767735073519,
-            "heading": 3.140000210384774,
-            "angularVelocity": 2.6424584851850765e-16,
-            "velocityX": -0.000002613491149837329,
-            "velocityY": 3.7829405750867053,
-            "timestamp": 1.1888871337321865
-        },
-        {
-            "x": 7.680000948769078,
-            "y": 6.648558516522932,
-            "heading": 3.1400002103847724,
-            "angularVelocity": -2.22727936711479e-14,
-            "velocityX": -0.000003293772326272547,
-            "velocityY": 3.782940575085685,
-            "timestamp": 1.287961061543202
-        },
-        {
-            "x": 7.680000579847807,
-            "y": 6.975935091856332,
-            "heading": 3.140000126724757,
-            "angularVelocity": -8.444197945421442e-7,
-            "velocityX": -0.0000037236965492526806,
-            "velocityY": 3.3043665731903227,
-            "timestamp": 1.3870349893542175
-        },
-        {
-            "x": 7.680000295053659,
-            "y": 7.221467541258139,
-            "heading": 3.14000006325574,
-            "angularVelocity": -6.40622649068188e-7,
-            "velocityX": -0.000002874561832669146,
-            "velocityY": 2.4782751105836276,
-            "timestamp": 1.486108917165233
-        },
+            "x": 7.68,
+            "y": 2.538844153951763,
+            "heading": 2.1223723168001845e-18,
+            "angularVelocity": 2.1422107346760384e-17,
+            "velocityX": -4.524824041974102e-19,
+            "velocityY": 0.8260917454276265,
+            "timestamp": 0.09907392781100399
+        },
+        {
+            "x": 7.68,
+            "y": 2.70253245874193,
+            "heading": 6.368502121517421e-18,
+            "angularVelocity": 4.285819588010552e-17,
+            "velocityX": -9.048618887508432e-19,
+            "velocityY": 1.6521834594306501,
+            "timestamp": 0.19814785562200798
+        },
+        {
+            "x": 7.68,
+            "y": 2.9480649081437846,
+            "heading": 1.2741259045974704e-17,
+            "angularVelocity": 6.432324896396774e-17,
+            "velocityX": -1.3570355356096521e-18,
+            "velocityY": 2.47827511058448,
+            "timestamp": 0.297221783433012
+        },
+        {
+            "x": 7.68,
+            "y": 3.2754414834771923,
+            "heading": 2.1247069179422305e-17,
+            "angularVelocity": 8.585316360490521e-17,
+            "velocityX": -1.8085916683463265e-18,
+            "velocityY": 3.3043665731908773,
+            "timestamp": 0.39629571124401597
+        },
+        {
+            "x": 7.68,
+            "y": 3.6502322649266867,
+            "heading": 5.059469084858907e-18,
+            "angularVelocity": -1.633891019789118e-16,
+            "velocityX": -2.0715339797290713e-18,
+            "velocityY": 3.782940575087071,
+            "timestamp": 0.49536963905501996
+        },
+        {
+            "x": 7.68,
+            "y": 4.025023046376204,
+            "heading": -4.370025378852898e-19,
+            "angularVelocity": -5.5478487063848375e-17,
+            "velocityX": -2.0691441879135133e-18,
+            "velocityY": 3.7829405750873115,
+            "timestamp": 0.594443566866024
+        },
+        {
+            "x": 7.68,
+            "y": 4.3998138278257235,
+            "heading": 3.678666355048194e-17,
+            "angularVelocity": 3.7571606285132846e-16,
+            "velocityX": -2.072328507072934e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.693517494677028
+        },
         {
-            "x": 7.680000100017398,
-            "y": 7.385155846048263,
-            "heading": 3.14000002096013,
-            "angularVelocity": -4.269095158176857e-7,
-            "velocityX": -0.000001968593124089932,
-            "velocityY": 1.6521834594299558,
-            "timestamp": 1.5851828449762486
+            "x": 7.68,
+            "y": 4.774604609275241,
+            "heading": 5.708604515927294e-17,
+            "angularVelocity": 2.04891257037182e-16,
+            "velocityX": -2.07516702935077e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.7925914224880319
+        },
+        {
+            "x": 7.68,
+            "y": 5.14939539072476,
+            "heading": 3.2711500565517004e-17,
+            "angularVelocity": -2.460238039643844e-16,
+            "velocityX": -2.078706211560206e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.8916653502990359
+        },
+        {
+            "x": 7.68,
+            "y": 5.524186172174278,
+            "heading": 3.4723638610714384e-17,
+            "angularVelocity": 2.0309460720999983e-17,
+            "velocityX": -2.0667219645048642e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.9907392781100399
+        },
+        {
+            "x": 7.68,
+            "y": 5.898976953623796,
+            "heading": 1.3963468905806427e-18,
+            "angularVelocity": -3.363881139719203e-16,
+            "velocityX": -2.078319856885538e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 1.089813205921044
+        },
+        {
+            "x": 7.68,
+            "y": 6.273767735073314,
+            "heading": -1.338725801529433e-17,
+            "angularVelocity": -1.492179146704526e-16,
+            "velocityX": -2.0709974384556896e-18,
+            "velocityY": 3.7829405750873115,
+            "timestamp": 1.188887133732048
+        },
+        {
+            "x": 7.68,
+            "y": 6.648558516522808,
+            "heading": -2.1247111946894708e-17,
+            "angularVelocity": -7.933322222365232e-17,
+            "velocityX": -2.070808166139798e-18,
+            "velocityY": 3.782940575087071,
+            "timestamp": 1.287961061543052
+        },
+        {
+            "x": 7.68,
+            "y": 6.975935091856216,
+            "heading": -1.273985947141283e-17,
+            "angularVelocity": 8.586772184549593e-17,
+            "velocityX": -1.8108827209778567e-18,
+            "velocityY": 3.304366573190878,
+            "timestamp": 1.387034989354056
+        },
+        {
+            "x": 7.68,
+            "y": 7.22146754125807,
+            "heading": -6.367750753434283e-18,
+            "angularVelocity": 6.431670631033216e-17,
+            "velocityX": -1.3581621409711223e-18,
+            "velocityY": 2.47827511058448,
+            "timestamp": 1.4861089171650599
+        },
+        {
+            "x": 7.68,
+            "y": 7.385155846048237,
+            "heading": -2.122107432521882e-18,
+            "angularVelocity": 4.285328556951628e-17,
+            "velocityX": -9.05441456368543e-19,
+            "velocityY": 1.65218345943065,
+            "timestamp": 1.5851828449760639
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": -2.115604748661088e-7,
-            "velocityX": -0.0000010095228578568797,
-            "velocityY": 0.8260917454272356,
-            "timestamp": 1.6842567727872642
+            "heading": 0,
+            "angularVelocity": 2.141943374454498e-17,
+            "velocityX": -4.527207369006345e-19,
+            "velocityY": 0.8260917454276265,
+            "timestamp": 1.6842567727870679
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 8.944588154710766e-32,
-            "velocityX": 0,
-            "velocityY": -4.821363548294539e-38,
-            "timestamp": 1.7833307005982797
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -3.1368402443636883e-34,
+            "velocityY": 0,
+            "timestamp": 1.7833307005980719
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.traj
index 28199c95..1bf739bf 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.traj	
@@ -3,173 +3,173 @@
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 1.7868231110584375e-31,
-            "velocityX": 3.500790942409583e-35,
-            "velocityY": -4.724665555223456e-38,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -2.2542235108498663e-32,
             "timestamp": 0
         },
         {
-            "x": 7.6800001156222075,
-            "y": 2.5388441539517457,
-            "heading": 3.140000017932032,
-            "angularVelocity": 1.8099644459911076e-7,
-            "velocityX": 0.0000011670295392084272,
-            "velocityY": 0.8260917454273137,
-            "timestamp": 0.09907392781101554
-        },
-        {
-            "x": 7.680000338204962,
-            "y": 2.7025324587418944,
-            "heading": 3.1400000579849436,
-            "angularVelocity": 4.042728942464055e-7,
-            "velocityX": 0.000002246632826435788,
-            "velocityY": 1.6521834594301972,
-            "timestamp": 0.19814785562203108
-        },
-        {
-            "x": 7.680000658923826,
-            "y": 2.948064908143757,
-            "heading": 3.1400001223157976,
-            "angularVelocity": 6.493215714833696e-7,
-            "velocityX": 0.0000032371668547106284,
-            "velocityY": 2.4782751105841747,
-            "timestamp": 0.2972217834330466
-        },
-        {
-            "x": 7.6800010687648195,
-            "y": 3.2754414834772896,
-            "heading": 3.140000210384731,
-            "angularVelocity": 8.889210892181502e-7,
-            "velocityX": 0.0000041367184645969174,
-            "velocityY": 3.3043665731916554,
-            "timestamp": 0.39629571124406215
-        },
-        {
-            "x": 7.6800013918601255,
-            "y": 3.650232264926723,
-            "heading": 3.140000210384734,
-            "angularVelocity": 3.2741652225293424e-14,
-            "velocityX": 0.000003261153797251531,
-            "velocityY": 3.7829405750859055,
-            "timestamp": 0.4953696390550777
-        },
-        {
-            "x": 7.680001628786808,
-            "y": 4.025023046376236,
-            "heading": 3.140000210384741,
-            "angularVelocity": 7.304626800025654e-14,
-            "velocityX": 0.00000239141289972485,
-            "velocityY": 3.782940575086779,
-            "timestamp": 0.5944435668660932
-        },
-        {
-            "x": 7.6800017795018,
-            "y": 4.399813827825781,
-            "heading": 3.1400002103847475,
-            "angularVelocity": 7.220243449496737e-14,
-            "velocityX": 0.0000015212375955225791,
-            "velocityY": 3.782940575087123,
-            "timestamp": 0.6935174946771088
-        },
-        {
-            "x": 7.680001844060805,
-            "y": 4.774604609275342,
-            "heading": 3.140000210384754,
-            "angularVelocity": 6.886052815923548e-14,
-            "velocityX": 6.516245436906888e-7,
-            "velocityY": 3.782940575087321,
-            "timestamp": 0.7925914224881243
-        },
-        {
-            "x": 7.680001822974652,
-            "y": 5.14939539072491,
-            "heading": 3.1400002103847604,
-            "angularVelocity": 6.934745116017498e-14,
-            "velocityX": -2.1283245913448782e-7,
-            "velocityY": 3.782940575087379,
-            "timestamp": 0.8916653502991398
-        },
-        {
-            "x": 7.680001718396276,
-            "y": 5.5241861721744705,
-            "heading": 3.140000210384768,
-            "angularVelocity": 8.52485669839133e-14,
-            "velocityX": -0.0000010555588768470353,
-            "velocityY": 3.7829405750872986,
-            "timestamp": 0.9907392781101554
-        },
-        {
-            "x": 7.680001534024895,
-            "y": 5.898976953624011,
-            "heading": 3.140000210384774,
-            "angularVelocity": 6.71024495132872e-14,
-            "velocityX": -0.000001860947354605527,
-            "velocityY": 3.7829405750870757,
-            "timestamp": 1.089813205921171
-        },
-        {
-            "x": 7.680001275096042,
-            "y": 6.273767735073519,
-            "heading": 3.140000210384774,
-            "angularVelocity": 2.6424584851850765e-16,
-            "velocityX": -0.000002613491149837329,
-            "velocityY": 3.7829405750867053,
-            "timestamp": 1.1888871337321865
-        },
-        {
-            "x": 7.680000948769078,
-            "y": 6.648558516522932,
-            "heading": 3.1400002103847724,
-            "angularVelocity": -2.22727936711479e-14,
-            "velocityX": -0.000003293772326272547,
-            "velocityY": 3.782940575085685,
-            "timestamp": 1.287961061543202
-        },
-        {
-            "x": 7.680000579847807,
-            "y": 6.975935091856332,
-            "heading": 3.140000126724757,
-            "angularVelocity": -8.444197945421442e-7,
-            "velocityX": -0.0000037236965492526806,
-            "velocityY": 3.3043665731903227,
-            "timestamp": 1.3870349893542175
-        },
-        {
-            "x": 7.680000295053659,
-            "y": 7.221467541258139,
-            "heading": 3.14000006325574,
-            "angularVelocity": -6.40622649068188e-7,
-            "velocityX": -0.000002874561832669146,
-            "velocityY": 2.4782751105836276,
-            "timestamp": 1.486108917165233
-        },
+            "x": 7.68,
+            "y": 2.538844153951763,
+            "heading": 2.1223723168001845e-18,
+            "angularVelocity": 2.1422107346760384e-17,
+            "velocityX": -4.524824041974102e-19,
+            "velocityY": 0.8260917454276265,
+            "timestamp": 0.09907392781100399
+        },
+        {
+            "x": 7.68,
+            "y": 2.70253245874193,
+            "heading": 6.368502121517421e-18,
+            "angularVelocity": 4.285819588010552e-17,
+            "velocityX": -9.048618887508432e-19,
+            "velocityY": 1.6521834594306501,
+            "timestamp": 0.19814785562200798
+        },
+        {
+            "x": 7.68,
+            "y": 2.9480649081437846,
+            "heading": 1.2741259045974704e-17,
+            "angularVelocity": 6.432324896396774e-17,
+            "velocityX": -1.3570355356096521e-18,
+            "velocityY": 2.47827511058448,
+            "timestamp": 0.297221783433012
+        },
+        {
+            "x": 7.68,
+            "y": 3.2754414834771923,
+            "heading": 2.1247069179422305e-17,
+            "angularVelocity": 8.585316360490521e-17,
+            "velocityX": -1.8085916683463265e-18,
+            "velocityY": 3.3043665731908773,
+            "timestamp": 0.39629571124401597
+        },
+        {
+            "x": 7.68,
+            "y": 3.6502322649266867,
+            "heading": 5.059469084858907e-18,
+            "angularVelocity": -1.633891019789118e-16,
+            "velocityX": -2.0715339797290713e-18,
+            "velocityY": 3.782940575087071,
+            "timestamp": 0.49536963905501996
+        },
+        {
+            "x": 7.68,
+            "y": 4.025023046376204,
+            "heading": -4.370025378852898e-19,
+            "angularVelocity": -5.5478487063848375e-17,
+            "velocityX": -2.0691441879135133e-18,
+            "velocityY": 3.7829405750873115,
+            "timestamp": 0.594443566866024
+        },
+        {
+            "x": 7.68,
+            "y": 4.3998138278257235,
+            "heading": 3.678666355048194e-17,
+            "angularVelocity": 3.7571606285132846e-16,
+            "velocityX": -2.072328507072934e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.693517494677028
+        },
         {
-            "x": 7.680000100017398,
-            "y": 7.385155846048263,
-            "heading": 3.14000002096013,
-            "angularVelocity": -4.269095158176857e-7,
-            "velocityX": -0.000001968593124089932,
-            "velocityY": 1.6521834594299558,
-            "timestamp": 1.5851828449762486
+            "x": 7.68,
+            "y": 4.774604609275241,
+            "heading": 5.708604515927294e-17,
+            "angularVelocity": 2.04891257037182e-16,
+            "velocityX": -2.07516702935077e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.7925914224880319
+        },
+        {
+            "x": 7.68,
+            "y": 5.14939539072476,
+            "heading": 3.2711500565517004e-17,
+            "angularVelocity": -2.460238039643844e-16,
+            "velocityX": -2.078706211560206e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.8916653502990359
+        },
+        {
+            "x": 7.68,
+            "y": 5.524186172174278,
+            "heading": 3.4723638610714384e-17,
+            "angularVelocity": 2.0309460720999983e-17,
+            "velocityX": -2.0667219645048642e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.9907392781100399
+        },
+        {
+            "x": 7.68,
+            "y": 5.898976953623796,
+            "heading": 1.3963468905806427e-18,
+            "angularVelocity": -3.363881139719203e-16,
+            "velocityX": -2.078319856885538e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 1.089813205921044
+        },
+        {
+            "x": 7.68,
+            "y": 6.273767735073314,
+            "heading": -1.338725801529433e-17,
+            "angularVelocity": -1.492179146704526e-16,
+            "velocityX": -2.0709974384556896e-18,
+            "velocityY": 3.7829405750873115,
+            "timestamp": 1.188887133732048
+        },
+        {
+            "x": 7.68,
+            "y": 6.648558516522808,
+            "heading": -2.1247111946894708e-17,
+            "angularVelocity": -7.933322222365232e-17,
+            "velocityX": -2.070808166139798e-18,
+            "velocityY": 3.782940575087071,
+            "timestamp": 1.287961061543052
+        },
+        {
+            "x": 7.68,
+            "y": 6.975935091856216,
+            "heading": -1.273985947141283e-17,
+            "angularVelocity": 8.586772184549593e-17,
+            "velocityX": -1.8108827209778567e-18,
+            "velocityY": 3.304366573190878,
+            "timestamp": 1.387034989354056
+        },
+        {
+            "x": 7.68,
+            "y": 7.22146754125807,
+            "heading": -6.367750753434283e-18,
+            "angularVelocity": 6.431670631033216e-17,
+            "velocityX": -1.3581621409711223e-18,
+            "velocityY": 2.47827511058448,
+            "timestamp": 1.4861089171650599
+        },
+        {
+            "x": 7.68,
+            "y": 7.385155846048237,
+            "heading": -2.122107432521882e-18,
+            "angularVelocity": 4.285328556951628e-17,
+            "velocityX": -9.05441456368543e-19,
+            "velocityY": 1.65218345943065,
+            "timestamp": 1.5851828449760639
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": -2.115604748661088e-7,
-            "velocityX": -0.0000010095228578568797,
-            "velocityY": 0.8260917454272356,
-            "timestamp": 1.6842567727872642
+            "heading": 0,
+            "angularVelocity": 2.141943374454498e-17,
+            "velocityX": -4.527207369006345e-19,
+            "velocityY": 0.8260917454276265,
+            "timestamp": 1.6842567727870679
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 8.944588154710766e-32,
-            "velocityX": 0,
-            "velocityY": -4.821363548294539e-38,
-            "timestamp": 1.7833307005982797
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -3.1368402443636883e-34,
+            "velocityY": 0,
+            "timestamp": 1.7833307005980719
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.1.traj
index da777257..0dd47978 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.1.traj	
@@ -3,137 +3,137 @@
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -9.54519608761012e-33,
-            "velocityX": 3.049131565114831e-33,
-            "velocityY": -1.5169993504833503e-38,
+            "heading": 0,
+            "angularVelocity": -1.7120676318191985e-39,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.68000007508226,
-            "y": 2.533445372005505,
-            "heading": 3.1399999978468434,
-            "angularVelocity": -2.248715497169588e-8,
-            "velocityX": 7.84144633669789e-7,
-            "velocityY": 0.7983807350943783,
-            "timestamp": 0.09575052183150116
+            "x": 7.68,
+            "y": 2.5334453720055388,
+            "heading": -1.6066276958150233e-18,
+            "angularVelocity": -1.677931007769515e-17,
+            "velocityX": 1.4398812722194594e-33,
+            "velocityY": 0.7983807350946674,
+            "timestamp": 0.09575052183150976
         },
         {
-            "x": 7.680000217378094,
-            "y": 2.6863361129826027,
-            "heading": 3.1399999975552166,
-            "angularVelocity": -3.0456922527341925e-9,
-            "velocityX": 0.0000014861101970902313,
-            "velocityY": 1.5967614385031632,
-            "timestamp": 0.19150104366300233
+            "x": 7.68,
+            "y": 2.686336112982689,
+            "heading": -4.816833322743681e-18,
+            "angularVelocity": -3.3526769001814837e-17,
+            "velocityX": 1.5865173753469157e-33,
+            "velocityY": 1.5967614385035827,
+            "timestamp": 0.1915010436630195
         },
         {
-            "x": 7.680000418878438,
-            "y": 2.915672216863469,
-            "heading": 3.1400000014113987,
-            "angularVelocity": 4.027321769334868e-8,
-            "velocityX": 0.000002104430661113429,
-            "velocityY": 2.395142078540767,
-            "timestamp": 0.2872515654945035
+            "x": 7.68,
+            "y": 2.915672216863599,
+            "heading": -9.624489104750263e-18,
+            "angularVelocity": -5.0210230614541765e-17,
+            "velocityX": 1.0182538660036936e-34,
+            "velocityY": 2.395142078541023,
+            "timestamp": 0.28725156549452924
         },
         {
-            "x": 7.680000671408876,
-            "y": 3.2214536654446886,
-            "heading": 3.14000000940269,
-            "angularVelocity": 8.345949630994641e-8,
-            "velocityX": 0.0000026373790047185906,
-            "velocityY": 3.1935225284654267,
-            "timestamp": 0.38300208732600466
+            "x": 7.68,
+            "y": 3.221453665444745,
+            "heading": -1.6009153132358185e-17,
+            "angularVelocity": -6.66802008672839e-17,
+            "velocityX": 3.094783239015371e-34,
+            "velocityY": 3.193522528464369,
+            "timestamp": 0.383002087326039
         },
         {
-            "x": 7.680000833444467,
-            "y": 3.5836721992668776,
-            "heading": 3.1400000094026845,
-            "angularVelocity": -5.801733416228813e-14,
-            "velocityX": 0.0000016922687336334932,
-            "velocityY": 3.7829405719543456,
-            "timestamp": 0.47875260915750584
+            "x": 7.68,
+            "y": 3.5836721992668203,
+            "heading": -3.473454086768081e-17,
+            "angularVelocity": -1.9556434134398854e-16,
+            "velocityX": 3.082893791022991e-33,
+            "velocityY": 3.7829405719528495,
+            "timestamp": 0.4787526091575488
         },
         {
-            "x": 7.680000914400183,
-            "y": 3.9458907330889588,
-            "heading": 3.140000009402682,
-            "angularVelocity": -2.5236519267762366e-14,
-            "velocityX": 8.45485885682039e-7,
-            "velocityY": 3.78294057195325,
-            "timestamp": 0.574503130989007
+            "x": 7.68,
+            "y": 3.9458907330889397,
+            "heading": -3.402618832360637e-17,
+            "angularVelocity": 7.397897479044197e-18,
+            "velocityX": 3.015867889332214e-33,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.5745031309890586
         },
         {
-            "x": 7.68000091414698,
-            "y": 4.308109266911049,
-            "heading": 3.1400000094026788,
-            "angularVelocity": -3.379642410236896e-14,
-            "velocityX": -2.644395591425324e-9,
-            "velocityY": 3.782940571953342,
-            "timestamp": 0.6702536528205081
+            "x": 7.68,
+            "y": 4.308109266911059,
+            "heading": -1.7375698441954866e-17,
+            "angularVelocity": 1.7389450796885638e-16,
+            "velocityX": 5.330140345912211e-33,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.6702536528205684
         },
         {
-            "x": 7.680000832736705,
-            "y": 4.67032780073313,
-            "heading": 3.140000009402675,
-            "angularVelocity": -3.7410067203640955e-14,
-            "velocityX": -8.502332108268853e-7,
-            "velocityY": 3.782940571953249,
-            "timestamp": 0.7660041746520092
+            "x": 7.68,
+            "y": 4.670327800733179,
+            "heading": 7.579747606314133e-18,
+            "angularVelocity": 2.6062986990486198e-16,
+            "velocityX": 7.600730979130334e-33,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.7660041746520782
         },
         {
-            "x": 7.680000670417917,
-            "y": 5.032546334555316,
-            "heading": 3.1400000094026708,
-            "angularVelocity": -4.92147091530942e-14,
-            "velocityX": -0.0000016952263834753921,
-            "velocityY": 3.7829405719543154,
-            "timestamp": 0.8617546964835103
+            "x": 7.68,
+            "y": 5.032546334555254,
+            "heading": 1.6243735534665814e-17,
+            "angularVelocity": 9.048502047432954e-17,
+            "velocityX": -2.799411465702586e-33,
+            "velocityY": 3.7829405719528495,
+            "timestamp": 0.8617546964835879
         },
         {
-            "x": 7.680000418190559,
-            "y": 5.338327783136532,
-            "heading": 3.1400000097695924,
-            "angularVelocity": 3.832069362065189e-9,
-            "velocityX": -0.00000263421371002401,
-            "velocityY": 3.1935225284653987,
-            "timestamp": 0.9575052183150115
+            "x": 7.68,
+            "y": 5.3383277831364,
+            "heading": 9.733952786983855e-18,
+            "angularVelocity": -6.798691664538921e-17,
+            "velocityX": -8.030123288193386e-33,
+            "velocityY": 3.193522528464369,
+            "timestamp": 0.9575052183150977
         },
         {
-            "x": 7.68000021698581,
-            "y": 5.567663887017397,
-            "heading": 3.140000007984905,
-            "angularVelocity": -1.863892342341337e-8,
-            "velocityX": -0.0000021013435063173737,
-            "velocityY": 2.3951420785407556,
-            "timestamp": 1.0532557401465126
+            "x": 7.68,
+            "y": 5.567663887017311,
+            "heading": 4.864296415465134e-18,
+            "angularVelocity": -5.08577528116881e-17,
+            "velocityX": -5.179461626817983e-33,
+            "velocityY": 2.395142078541023,
+            "timestamp": 1.0532557401466074
         },
         {
-            "x": 7.680000074934953,
-            "y": 5.7205546279944945,
-            "heading": 3.140000003983309,
-            "angularVelocity": -4.1791894261559745e-8,
-            "velocityX": -0.0000014835517222041492,
-            "velocityY": 1.5967614385031577,
-            "timestamp": 1.1490062619780137
+            "x": 7.68,
+            "y": 5.720554627994461,
+            "heading": 1.6208272352196832e-18,
+            "angularVelocity": -3.3874167136237696e-17,
+            "velocityX": -4.721587370568867e-33,
+            "velocityY": 1.5967614385035827,
+            "timestamp": 1.1490062619781172
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": -4.160090541448797e-8,
-            "velocityX": -7.826061846711994e-7,
-            "velocityY": 0.7983807350943762,
-            "timestamp": 1.2447567838095148
+            "heading": 0,
+            "angularVelocity": -1.69276073284528e-17,
+            "velocityX": -2.4599412486527172e-33,
+            "velocityY": 0.7983807350946674,
+            "timestamp": 1.244756783809627
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
-            "velocityX": 3.04245898705875e-32,
-            "velocityY": 2.558785218352301e-38,
-            "timestamp": 1.340507305641016
+            "velocityX": -3.907521125363084e-43,
+            "velocityY": 8.448865646535191e-39,
+            "timestamp": 1.3405073056411367
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.traj
index da777257..0dd47978 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.traj	
@@ -3,137 +3,137 @@
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -9.54519608761012e-33,
-            "velocityX": 3.049131565114831e-33,
-            "velocityY": -1.5169993504833503e-38,
+            "heading": 0,
+            "angularVelocity": -1.7120676318191985e-39,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.68000007508226,
-            "y": 2.533445372005505,
-            "heading": 3.1399999978468434,
-            "angularVelocity": -2.248715497169588e-8,
-            "velocityX": 7.84144633669789e-7,
-            "velocityY": 0.7983807350943783,
-            "timestamp": 0.09575052183150116
+            "x": 7.68,
+            "y": 2.5334453720055388,
+            "heading": -1.6066276958150233e-18,
+            "angularVelocity": -1.677931007769515e-17,
+            "velocityX": 1.4398812722194594e-33,
+            "velocityY": 0.7983807350946674,
+            "timestamp": 0.09575052183150976
         },
         {
-            "x": 7.680000217378094,
-            "y": 2.6863361129826027,
-            "heading": 3.1399999975552166,
-            "angularVelocity": -3.0456922527341925e-9,
-            "velocityX": 0.0000014861101970902313,
-            "velocityY": 1.5967614385031632,
-            "timestamp": 0.19150104366300233
+            "x": 7.68,
+            "y": 2.686336112982689,
+            "heading": -4.816833322743681e-18,
+            "angularVelocity": -3.3526769001814837e-17,
+            "velocityX": 1.5865173753469157e-33,
+            "velocityY": 1.5967614385035827,
+            "timestamp": 0.1915010436630195
         },
         {
-            "x": 7.680000418878438,
-            "y": 2.915672216863469,
-            "heading": 3.1400000014113987,
-            "angularVelocity": 4.027321769334868e-8,
-            "velocityX": 0.000002104430661113429,
-            "velocityY": 2.395142078540767,
-            "timestamp": 0.2872515654945035
+            "x": 7.68,
+            "y": 2.915672216863599,
+            "heading": -9.624489104750263e-18,
+            "angularVelocity": -5.0210230614541765e-17,
+            "velocityX": 1.0182538660036936e-34,
+            "velocityY": 2.395142078541023,
+            "timestamp": 0.28725156549452924
         },
         {
-            "x": 7.680000671408876,
-            "y": 3.2214536654446886,
-            "heading": 3.14000000940269,
-            "angularVelocity": 8.345949630994641e-8,
-            "velocityX": 0.0000026373790047185906,
-            "velocityY": 3.1935225284654267,
-            "timestamp": 0.38300208732600466
+            "x": 7.68,
+            "y": 3.221453665444745,
+            "heading": -1.6009153132358185e-17,
+            "angularVelocity": -6.66802008672839e-17,
+            "velocityX": 3.094783239015371e-34,
+            "velocityY": 3.193522528464369,
+            "timestamp": 0.383002087326039
         },
         {
-            "x": 7.680000833444467,
-            "y": 3.5836721992668776,
-            "heading": 3.1400000094026845,
-            "angularVelocity": -5.801733416228813e-14,
-            "velocityX": 0.0000016922687336334932,
-            "velocityY": 3.7829405719543456,
-            "timestamp": 0.47875260915750584
+            "x": 7.68,
+            "y": 3.5836721992668203,
+            "heading": -3.473454086768081e-17,
+            "angularVelocity": -1.9556434134398854e-16,
+            "velocityX": 3.082893791022991e-33,
+            "velocityY": 3.7829405719528495,
+            "timestamp": 0.4787526091575488
         },
         {
-            "x": 7.680000914400183,
-            "y": 3.9458907330889588,
-            "heading": 3.140000009402682,
-            "angularVelocity": -2.5236519267762366e-14,
-            "velocityX": 8.45485885682039e-7,
-            "velocityY": 3.78294057195325,
-            "timestamp": 0.574503130989007
+            "x": 7.68,
+            "y": 3.9458907330889397,
+            "heading": -3.402618832360637e-17,
+            "angularVelocity": 7.397897479044197e-18,
+            "velocityX": 3.015867889332214e-33,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.5745031309890586
         },
         {
-            "x": 7.68000091414698,
-            "y": 4.308109266911049,
-            "heading": 3.1400000094026788,
-            "angularVelocity": -3.379642410236896e-14,
-            "velocityX": -2.644395591425324e-9,
-            "velocityY": 3.782940571953342,
-            "timestamp": 0.6702536528205081
+            "x": 7.68,
+            "y": 4.308109266911059,
+            "heading": -1.7375698441954866e-17,
+            "angularVelocity": 1.7389450796885638e-16,
+            "velocityX": 5.330140345912211e-33,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.6702536528205684
         },
         {
-            "x": 7.680000832736705,
-            "y": 4.67032780073313,
-            "heading": 3.140000009402675,
-            "angularVelocity": -3.7410067203640955e-14,
-            "velocityX": -8.502332108268853e-7,
-            "velocityY": 3.782940571953249,
-            "timestamp": 0.7660041746520092
+            "x": 7.68,
+            "y": 4.670327800733179,
+            "heading": 7.579747606314133e-18,
+            "angularVelocity": 2.6062986990486198e-16,
+            "velocityX": 7.600730979130334e-33,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.7660041746520782
         },
         {
-            "x": 7.680000670417917,
-            "y": 5.032546334555316,
-            "heading": 3.1400000094026708,
-            "angularVelocity": -4.92147091530942e-14,
-            "velocityX": -0.0000016952263834753921,
-            "velocityY": 3.7829405719543154,
-            "timestamp": 0.8617546964835103
+            "x": 7.68,
+            "y": 5.032546334555254,
+            "heading": 1.6243735534665814e-17,
+            "angularVelocity": 9.048502047432954e-17,
+            "velocityX": -2.799411465702586e-33,
+            "velocityY": 3.7829405719528495,
+            "timestamp": 0.8617546964835879
         },
         {
-            "x": 7.680000418190559,
-            "y": 5.338327783136532,
-            "heading": 3.1400000097695924,
-            "angularVelocity": 3.832069362065189e-9,
-            "velocityX": -0.00000263421371002401,
-            "velocityY": 3.1935225284653987,
-            "timestamp": 0.9575052183150115
+            "x": 7.68,
+            "y": 5.3383277831364,
+            "heading": 9.733952786983855e-18,
+            "angularVelocity": -6.798691664538921e-17,
+            "velocityX": -8.030123288193386e-33,
+            "velocityY": 3.193522528464369,
+            "timestamp": 0.9575052183150977
         },
         {
-            "x": 7.68000021698581,
-            "y": 5.567663887017397,
-            "heading": 3.140000007984905,
-            "angularVelocity": -1.863892342341337e-8,
-            "velocityX": -0.0000021013435063173737,
-            "velocityY": 2.3951420785407556,
-            "timestamp": 1.0532557401465126
+            "x": 7.68,
+            "y": 5.567663887017311,
+            "heading": 4.864296415465134e-18,
+            "angularVelocity": -5.08577528116881e-17,
+            "velocityX": -5.179461626817983e-33,
+            "velocityY": 2.395142078541023,
+            "timestamp": 1.0532557401466074
         },
         {
-            "x": 7.680000074934953,
-            "y": 5.7205546279944945,
-            "heading": 3.140000003983309,
-            "angularVelocity": -4.1791894261559745e-8,
-            "velocityX": -0.0000014835517222041492,
-            "velocityY": 1.5967614385031577,
-            "timestamp": 1.1490062619780137
+            "x": 7.68,
+            "y": 5.720554627994461,
+            "heading": 1.6208272352196832e-18,
+            "angularVelocity": -3.3874167136237696e-17,
+            "velocityX": -4.721587370568867e-33,
+            "velocityY": 1.5967614385035827,
+            "timestamp": 1.1490062619781172
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": -4.160090541448797e-8,
-            "velocityX": -7.826061846711994e-7,
-            "velocityY": 0.7983807350943762,
-            "timestamp": 1.2447567838095148
+            "heading": 0,
+            "angularVelocity": -1.69276073284528e-17,
+            "velocityX": -2.4599412486527172e-33,
+            "velocityY": 0.7983807350946674,
+            "timestamp": 1.244756783809627
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
+            "heading": 0,
             "angularVelocity": 0,
-            "velocityX": 3.04245898705875e-32,
-            "velocityY": 2.558785218352301e-38,
-            "timestamp": 1.340507305641016
+            "velocityX": -3.907521125363084e-43,
+            "velocityY": 8.448865646535191e-39,
+            "timestamp": 1.3405073056411367
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.1.traj
index 63f25781..14b92c55 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.1.traj	
@@ -3,92 +3,92 @@
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": -3.7532975982701904e-32,
-            "velocityY": 5.0637303451538295e-36,
+            "heading": -2.24426408200091e-39,
+            "angularVelocity": 3.072610310721988e-47,
+            "velocityX": -5.239815815905201e-40,
+            "velocityY": -4.5917748078995606e-40,
             "timestamp": 0
         },
         {
-            "x": 7.680000036889488,
-            "y": 2.540500005575837,
-            "heading": 3.1400000022500048,
-            "angularVelocity": 2.2484053179234407e-8,
-            "velocityX": 3.6863266642951275e-7,
-            "velocityY": 0.8344065481227464,
-            "timestamp": 0.1000711293118395
+            "x": 7.68,
+            "y": 2.540500005575846,
+            "heading": -7.160737227180709e-23,
+            "angularVelocity": -7.15564807370433e-22,
+            "velocityX": 1.97902852084548e-33,
+            "velocityY": 0.8344065481228216,
+            "timestamp": 0.10007112931184187
         },
         {
-            "x": 7.680000101870814,
-            "y": 2.707500013629824,
-            "heading": 3.140000010932795,
-            "angularVelocity": 8.676618158835649e-8,
-            "velocityX": 6.49351381942043e-7,
-            "velocityY": 1.6688130652906412,
-            "timestamp": 0.200142258623679
+            "x": 7.68,
+            "y": 2.7075000136298466,
+            "heading": 1.9234971876596208e-22,
+            "angularVelocity": 2.6376946581601568e-21,
+            "velocityX": 2.759296119129438e-33,
+            "velocityY": 1.6688130652907365,
+            "timestamp": 0.20014225862368373
         },
         {
-            "x": 7.680000185978468,
-            "y": 2.958000017966584,
-            "heading": 3.1400000281272393,
-            "angularVelocity": 1.7182222560804709e-7,
-            "velocityX": 8.404786987428781e-7,
-            "velocityY": 2.503219520548802,
-            "timestamp": 0.3002133879355185
+            "x": 7.68,
+            "y": 2.9580000179666155,
+            "heading": 1.2715262688933367e-21,
+            "angularVelocity": 1.0784094425251139e-20,
+            "velocityX": 3.360924516770788e-33,
+            "velocityY": 2.5032195205488357,
+            "timestamp": 0.3002133879355256
         },
         {
-            "x": 7.68000028005223,
-            "y": 3.2919999999999923,
-            "heading": 3.1400000531214163,
-            "angularVelocity": 2.497641049380253e-7,
-            "velocityX": 9.400689316139974e-7,
-            "velocityY": 3.3376257900778246,
-            "timestamp": 0.400284517247358
+            "x": 7.68,
+            "y": 3.2919999999999994,
+            "heading": 4.5990005279924634e-21,
+            "angularVelocity": 3.325109025879841e-20,
+            "velocityX": 4.212493187097162e-33,
+            "velocityY": 3.3376257900775026,
+            "timestamp": 0.40028451724736747
         },
         {
-            "x": 7.680000187136683,
-            "y": 3.6259999820334055,
-            "heading": 3.140000035091715,
-            "angularVelocity": -1.801688496007097e-7,
-            "velocityX": -9.284950198395514e-7,
-            "velocityY": 3.337625790077875,
-            "timestamp": 0.5003556465591975
+            "x": 7.68,
+            "y": 3.6259999820333837,
+            "heading": 2.8774590551268706e-21,
+            "angularVelocity": -1.7203177491039364e-20,
+            "velocityX": -1.8223734723751632e-33,
+            "velocityY": 3.3376257900775026,
+            "timestamp": 0.5003556465592094
         },
         {
-            "x": 7.680000103123094,
-            "y": 3.8764999863701703,
-            "heading": 3.1400000189895447,
-            "angularVelocity": -1.609072425985215e-7,
-            "velocityX": -8.395387126580118e-7,
-            "velocityY": 2.5032195205488503,
-            "timestamp": 0.600426775871037
+            "x": 7.68,
+            "y": 3.8764999863701526,
+            "heading": 1.4966782173181084e-21,
+            "angularVelocity": -1.3797993485663648e-20,
+            "velocityX": -1.777649344678044e-33,
+            "velocityY": 2.5032195205488357,
+            "timestamp": 0.6004267758710513
         },
         {
-            "x": 7.680000037559297,
-            "y": 4.043499994424161,
-            "heading": 3.140000006758861,
-            "angularVelocity": -1.222199010476452e-7,
-            "velocityX": -6.55171951368837e-7,
-            "velocityY": 1.6688130652906763,
-            "timestamp": 0.7004979051828765
+            "x": 7.68,
+            "y": 4.043499994424153,
+            "heading": 5.147844205579642e-22,
+            "angularVelocity": -9.81195850164629e-21,
+            "velocityX": -2.007686184897088e-33,
+            "velocityY": 1.6688130652907365,
+            "timestamp": 0.7004979051828932
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": -6.754056559007022e-8,
-            "velocityX": -3.753259946110928e-7,
-            "velocityY": 0.8344065481227645,
-            "timestamp": 0.800569034494716
+            "heading": -4.8498876719579466e-42,
+            "angularVelocity": -5.144185056490002e-21,
+            "velocityX": -2.233495211598662e-33,
+            "velocityY": 0.8344065481228216,
+            "timestamp": 0.8005690344947352
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 3.7078876706224725e-32,
-            "velocityX": 0,
-            "velocityY": -1.600749389553648e-35,
-            "timestamp": 0.9006401638065554
+            "heading": 9.449822208481667e-41,
+            "angularVelocity": 9.927610425813228e-40,
+            "velocityX": 1.9465664275137074e-41,
+            "velocityY": 5.142787784847508e-39,
+            "timestamp": 0.9006401638065771
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.traj
index 63f25781..14b92c55 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.traj	
@@ -3,92 +3,92 @@
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": -3.7532975982701904e-32,
-            "velocityY": 5.0637303451538295e-36,
+            "heading": -2.24426408200091e-39,
+            "angularVelocity": 3.072610310721988e-47,
+            "velocityX": -5.239815815905201e-40,
+            "velocityY": -4.5917748078995606e-40,
             "timestamp": 0
         },
         {
-            "x": 7.680000036889488,
-            "y": 2.540500005575837,
-            "heading": 3.1400000022500048,
-            "angularVelocity": 2.2484053179234407e-8,
-            "velocityX": 3.6863266642951275e-7,
-            "velocityY": 0.8344065481227464,
-            "timestamp": 0.1000711293118395
+            "x": 7.68,
+            "y": 2.540500005575846,
+            "heading": -7.160737227180709e-23,
+            "angularVelocity": -7.15564807370433e-22,
+            "velocityX": 1.97902852084548e-33,
+            "velocityY": 0.8344065481228216,
+            "timestamp": 0.10007112931184187
         },
         {
-            "x": 7.680000101870814,
-            "y": 2.707500013629824,
-            "heading": 3.140000010932795,
-            "angularVelocity": 8.676618158835649e-8,
-            "velocityX": 6.49351381942043e-7,
-            "velocityY": 1.6688130652906412,
-            "timestamp": 0.200142258623679
+            "x": 7.68,
+            "y": 2.7075000136298466,
+            "heading": 1.9234971876596208e-22,
+            "angularVelocity": 2.6376946581601568e-21,
+            "velocityX": 2.759296119129438e-33,
+            "velocityY": 1.6688130652907365,
+            "timestamp": 0.20014225862368373
         },
         {
-            "x": 7.680000185978468,
-            "y": 2.958000017966584,
-            "heading": 3.1400000281272393,
-            "angularVelocity": 1.7182222560804709e-7,
-            "velocityX": 8.404786987428781e-7,
-            "velocityY": 2.503219520548802,
-            "timestamp": 0.3002133879355185
+            "x": 7.68,
+            "y": 2.9580000179666155,
+            "heading": 1.2715262688933367e-21,
+            "angularVelocity": 1.0784094425251139e-20,
+            "velocityX": 3.360924516770788e-33,
+            "velocityY": 2.5032195205488357,
+            "timestamp": 0.3002133879355256
         },
         {
-            "x": 7.68000028005223,
-            "y": 3.2919999999999923,
-            "heading": 3.1400000531214163,
-            "angularVelocity": 2.497641049380253e-7,
-            "velocityX": 9.400689316139974e-7,
-            "velocityY": 3.3376257900778246,
-            "timestamp": 0.400284517247358
+            "x": 7.68,
+            "y": 3.2919999999999994,
+            "heading": 4.5990005279924634e-21,
+            "angularVelocity": 3.325109025879841e-20,
+            "velocityX": 4.212493187097162e-33,
+            "velocityY": 3.3376257900775026,
+            "timestamp": 0.40028451724736747
         },
         {
-            "x": 7.680000187136683,
-            "y": 3.6259999820334055,
-            "heading": 3.140000035091715,
-            "angularVelocity": -1.801688496007097e-7,
-            "velocityX": -9.284950198395514e-7,
-            "velocityY": 3.337625790077875,
-            "timestamp": 0.5003556465591975
+            "x": 7.68,
+            "y": 3.6259999820333837,
+            "heading": 2.8774590551268706e-21,
+            "angularVelocity": -1.7203177491039364e-20,
+            "velocityX": -1.8223734723751632e-33,
+            "velocityY": 3.3376257900775026,
+            "timestamp": 0.5003556465592094
         },
         {
-            "x": 7.680000103123094,
-            "y": 3.8764999863701703,
-            "heading": 3.1400000189895447,
-            "angularVelocity": -1.609072425985215e-7,
-            "velocityX": -8.395387126580118e-7,
-            "velocityY": 2.5032195205488503,
-            "timestamp": 0.600426775871037
+            "x": 7.68,
+            "y": 3.8764999863701526,
+            "heading": 1.4966782173181084e-21,
+            "angularVelocity": -1.3797993485663648e-20,
+            "velocityX": -1.777649344678044e-33,
+            "velocityY": 2.5032195205488357,
+            "timestamp": 0.6004267758710513
         },
         {
-            "x": 7.680000037559297,
-            "y": 4.043499994424161,
-            "heading": 3.140000006758861,
-            "angularVelocity": -1.222199010476452e-7,
-            "velocityX": -6.55171951368837e-7,
-            "velocityY": 1.6688130652906763,
-            "timestamp": 0.7004979051828765
+            "x": 7.68,
+            "y": 4.043499994424153,
+            "heading": 5.147844205579642e-22,
+            "angularVelocity": -9.81195850164629e-21,
+            "velocityX": -2.007686184897088e-33,
+            "velocityY": 1.6688130652907365,
+            "timestamp": 0.7004979051828932
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": -6.754056559007022e-8,
-            "velocityX": -3.753259946110928e-7,
-            "velocityY": 0.8344065481227645,
-            "timestamp": 0.800569034494716
+            "heading": -4.8498876719579466e-42,
+            "angularVelocity": -5.144185056490002e-21,
+            "velocityX": -2.233495211598662e-33,
+            "velocityY": 0.8344065481228216,
+            "timestamp": 0.8005690344947352
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 3.7078876706224725e-32,
-            "velocityX": 0,
-            "velocityY": -1.600749389553648e-35,
-            "timestamp": 0.9006401638065554
+            "heading": 9.449822208481667e-41,
+            "angularVelocity": 9.927610425813228e-40,
+            "velocityX": 1.9465664275137074e-41,
+            "velocityY": 5.142787784847508e-39,
+            "timestamp": 0.9006401638065771
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C5.1.traj
new file mode 100644
index 00000000..27f393e3
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C5.1.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -4.712717929333765e-40,
+            "angularVelocity": 9.927649781151023e-40,
+            "velocityX": 1.7772849076267516e-40,
+            "velocityY": -4.543888050052464e-40,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 2.3734999944241535,
+            "heading": 2.8227518572323466e-21,
+            "angularVelocity": 2.820745460648545e-20,
+            "velocityX": 2.2465911667965736e-32,
+            "velocityY": -0.8344065481228212,
+            "timestamp": 0.10007112931184181
+        },
+        {
+            "x": 7.68,
+            "y": 2.206499986370153,
+            "heading": 8.436925006375396e-21,
+            "angularVelocity": 5.6101826293588e-20,
+            "velocityX": 4.4654374072917263e-32,
+            "velocityY": -1.6688130652907358,
+            "timestamp": 0.20014225862368362
+        },
+        {
+            "x": 7.68,
+            "y": 1.955999982033384,
+            "heading": 1.682221146414935e-20,
+            "angularVelocity": 8.379326253074398e-20,
+            "velocityX": 6.797738711780288e-32,
+            "velocityY": -2.503219520548835,
+            "timestamp": 0.30021338793552543
+        },
+        {
+            "x": 7.68,
+            "y": 1.6219999999999999,
+            "heading": 2.78675063552584e-20,
+            "angularVelocity": 1.1037443968745167e-19,
+            "velocityX": 8.936946075377416e-32,
+            "velocityY": -3.3376257900775013,
+            "timestamp": 0.40028451724736724
+        },
+        {
+            "x": 7.68,
+            "y": 1.288000017966616,
+            "heading": 1.0287640204896551e-20,
+            "angularVelocity": -1.75673704816788e-19,
+            "velocityX": 9.060995032099377e-32,
+            "velocityY": -3.3376257900775013,
+            "timestamp": 0.500355646559209
+        },
+        {
+            "x": 7.68,
+            "y": 1.037500013629847,
+            "heading": 3.2969803204781124e-21,
+            "angularVelocity": -6.985690973056607e-20,
+            "velocityX": 6.916062716279199e-32,
+            "velocityY": -2.503219520548835,
+            "timestamp": 0.6004267758710509
+        },
+        {
+            "x": 7.68,
+            "y": 0.8705000055758465,
+            "heading": 6.18120991020044e-22,
+            "angularVelocity": -2.676955216055553e-20,
+            "velocityX": 4.34564572466329e-32,
+            "velocityY": -1.6688130652907358,
+            "timestamp": 0.7004979051828927
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -2.2737142324276756e-40,
+            "angularVelocity": -6.1768164103594875e-21,
+            "velocityX": 2.131975080632403e-32,
+            "velocityY": -0.8344065481228212,
+            "timestamp": 0.8005690344947345
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 2.0993519451368808e-41,
+            "angularVelocity": 2.4819025971975228e-39,
+            "velocityX": -8.732390511178075e-43,
+            "velocityY": -2.571393892423754e-39,
+            "timestamp": 0.9006401638065763
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C5.traj
new file mode 100644
index 00000000..27f393e3
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C5.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -4.712717929333765e-40,
+            "angularVelocity": 9.927649781151023e-40,
+            "velocityX": 1.7772849076267516e-40,
+            "velocityY": -4.543888050052464e-40,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 2.3734999944241535,
+            "heading": 2.8227518572323466e-21,
+            "angularVelocity": 2.820745460648545e-20,
+            "velocityX": 2.2465911667965736e-32,
+            "velocityY": -0.8344065481228212,
+            "timestamp": 0.10007112931184181
+        },
+        {
+            "x": 7.68,
+            "y": 2.206499986370153,
+            "heading": 8.436925006375396e-21,
+            "angularVelocity": 5.6101826293588e-20,
+            "velocityX": 4.4654374072917263e-32,
+            "velocityY": -1.6688130652907358,
+            "timestamp": 0.20014225862368362
+        },
+        {
+            "x": 7.68,
+            "y": 1.955999982033384,
+            "heading": 1.682221146414935e-20,
+            "angularVelocity": 8.379326253074398e-20,
+            "velocityX": 6.797738711780288e-32,
+            "velocityY": -2.503219520548835,
+            "timestamp": 0.30021338793552543
+        },
+        {
+            "x": 7.68,
+            "y": 1.6219999999999999,
+            "heading": 2.78675063552584e-20,
+            "angularVelocity": 1.1037443968745167e-19,
+            "velocityX": 8.936946075377416e-32,
+            "velocityY": -3.3376257900775013,
+            "timestamp": 0.40028451724736724
+        },
+        {
+            "x": 7.68,
+            "y": 1.288000017966616,
+            "heading": 1.0287640204896551e-20,
+            "angularVelocity": -1.75673704816788e-19,
+            "velocityX": 9.060995032099377e-32,
+            "velocityY": -3.3376257900775013,
+            "timestamp": 0.500355646559209
+        },
+        {
+            "x": 7.68,
+            "y": 1.037500013629847,
+            "heading": 3.2969803204781124e-21,
+            "angularVelocity": -6.985690973056607e-20,
+            "velocityX": 6.916062716279199e-32,
+            "velocityY": -2.503219520548835,
+            "timestamp": 0.6004267758710509
+        },
+        {
+            "x": 7.68,
+            "y": 0.8705000055758465,
+            "heading": 6.18120991020044e-22,
+            "angularVelocity": -2.676955216055553e-20,
+            "velocityX": 4.34564572466329e-32,
+            "velocityY": -1.6688130652907358,
+            "timestamp": 0.7004979051828927
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -2.2737142324276756e-40,
+            "angularVelocity": -6.1768164103594875e-21,
+            "velocityX": 2.131975080632403e-32,
+            "velocityY": -0.8344065481228212,
+            "timestamp": 0.8005690344947345
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 2.0993519451368808e-41,
+            "angularVelocity": 2.4819025971975228e-39,
+            "velocityX": -8.732390511178075e-43,
+            "velocityY": -2.571393892423754e-39,
+            "timestamp": 0.9006401638065763
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S3.1.traj
new file mode 100644
index 00000000..80fc230a
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S3.1.traj	
@@ -0,0 +1,229 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 7.203126847267154e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6459509070752,
+            "y": 2.420050479995149,
+            "heading": -0.026297254997510774,
+            "angularVelocity": -0.33695614983619343,
+            "velocityX": -0.4362832264603014,
+            "velocityY": -0.47344743777665743,
+            "timestamp": 0.078043552581802
+        },
+        {
+            "x": 7.57752123433784,
+            "y": 2.3463325784745406,
+            "heading": -0.07655763531528098,
+            "angularVelocity": -0.6440042598660941,
+            "velocityX": -0.8768139131756897,
+            "velocityY": -0.9445738831958564,
+            "timestamp": 0.156087105163604
+        },
+        {
+            "x": 7.474195561438856,
+            "y": 2.236153787211583,
+            "heading": -0.14701004173730176,
+            "angularVelocity": -0.9027319245646475,
+            "velocityX": -1.3239488654839289,
+            "velocityY": -1.4117603263569152,
+            "timestamp": 0.23413065774540598
+        },
+        {
+            "x": 7.33507656702923,
+            "y": 2.0901340736402396,
+            "heading": -0.23055657416165956,
+            "angularVelocity": -1.0705116522834195,
+            "velocityX": -1.7825815177216175,
+            "velocityY": -1.8710029046700958,
+            "timestamp": 0.312174210327208
+        },
+        {
+            "x": 7.158294386463989,
+            "y": 1.910067298253472,
+            "heading": -0.30975533817585227,
+            "angularVelocity": -1.014802137964151,
+            "velocityX": -2.2651734155741057,
+            "velocityY": -2.3072601057983104,
+            "timestamp": 0.39021776290901
+        },
+        {
+            "x": 6.940360292129115,
+            "y": 1.7108993896694566,
+            "heading": -0.30975560484167247,
+            "angularVelocity": -0.00000341688469228089,
+            "velocityX": -2.7924676302559823,
+            "velocityY": -2.552009768843728,
+            "timestamp": 0.468261315490812
+        },
+        {
+            "x": 6.6915168927330235,
+            "y": 1.5520279357314815,
+            "heading": -0.30975564017638474,
+            "angularVelocity": -4.5275632793576304e-7,
+            "velocityX": -3.1885196299240257,
+            "velocityY": -2.035676858398594,
+            "timestamp": 0.546304868072614
+        },
+        {
+            "x": 6.419127595403782,
+            "y": 1.43815413781978,
+            "heading": -0.30975568823518523,
+            "angularVelocity": -6.157946287917162e-7,
+            "velocityX": -3.490221655962278,
+            "velocityY": -1.4591057703627237,
+            "timestamp": 0.624348420654416
+        },
+        {
+            "x": 6.131252530334066,
+            "y": 1.37264761019134,
+            "heading": -0.30975577587401576,
+            "angularVelocity": -0.0000011229477336220622,
+            "velocityX": -3.6886463461279972,
+            "velocityY": -0.8393586076157934,
+            "timestamp": 0.7023919732362179
+        },
+        {
+            "x": 5.83657654735769,
+            "y": 1.3574693553002635,
+            "heading": -0.31020540722452916,
+            "angularVelocity": -0.005761287587978277,
+            "velocityX": -3.7757889438387497,
+            "velocityY": -0.1944844178532558,
+            "timestamp": 0.7804355258180199
+        },
+        {
+            "x": 5.546474933624268,
+            "y": 1.3928169012069702,
+            "heading": -0.3189695864044355,
+            "angularVelocity": -0.11229856778804584,
+            "velocityX": -3.717175911864151,
+            "velocityY": 0.4529207697160109,
+            "timestamp": 0.8584790783998218
+        },
+        {
+            "x": 5.303276771996363,
+            "y": 1.4607318697473424,
+            "heading": -0.3307229091538368,
+            "angularVelocity": -0.17360472172913416,
+            "velocityX": -3.5922053767043294,
+            "velocityY": 1.0031511484968414,
+            "timestamp": 0.9261807090485242
+        },
+        {
+            "x": 5.073563214559545,
+            "y": 1.564404927734292,
+            "heading": -0.34378229938421223,
+            "angularVelocity": -0.1928962434919207,
+            "velocityX": -3.3930284283517023,
+            "velocityY": 1.5313229089691542,
+            "timestamp": 0.9938823396972266
+        },
+        {
+            "x": 4.862974125111252,
+            "y": 1.7011642845126724,
+            "heading": -0.35827284529055603,
+            "angularVelocity": -0.2140354045770704,
+            "velocityX": -3.1105467834446325,
+            "velocityY": 2.020030469989471,
+            "timestamp": 1.0615839703459289
+        },
+        {
+            "x": 4.677671719496688,
+            "y": 1.8665243414779629,
+            "heading": -0.3791415933598452,
+            "angularVelocity": -0.30824587043792007,
+            "velocityX": -2.737044940262413,
+            "velocityY": 2.442482631227878,
+            "timestamp": 1.1292856009946313
+        },
+        {
+            "x": 4.523629451068858,
+            "y": 2.053477766497112,
+            "heading": -0.41478083919993697,
+            "angularVelocity": -0.5264163580502632,
+            "velocityX": -2.27531105162187,
+            "velocityY": 2.761431640977936,
+            "timestamp": 1.1969872316433336
+        },
+        {
+            "x": 4.406611773563404,
+            "y": 2.2498215105595722,
+            "heading": -0.45494733767836987,
+            "angularVelocity": -0.5932870167770028,
+            "velocityX": -1.7284321867022447,
+            "velocityY": 2.900133160474012,
+            "timestamp": 1.264688862292036
+        },
+        {
+            "x": 4.327688857963163,
+            "y": 2.4438365235550945,
+            "heading": -0.48799769437613305,
+            "angularVelocity": -0.4881766713911416,
+            "velocityX": -1.1657461547677304,
+            "velocityY": 2.865736188870776,
+            "timestamp": 1.3323904929407384
+        },
+        {
+            "x": 4.28360260298108,
+            "y": 2.622213717107768,
+            "heading": -0.5154307830643909,
+            "angularVelocity": -0.40520573028252516,
+            "velocityX": -0.6511845366152605,
+            "velocityY": 2.634754759132393,
+            "timestamp": 1.4000921235894408
+        },
+        {
+            "x": 4.264391797811532,
+            "y": 2.7716303897908317,
+            "heading": -0.5365689504315967,
+            "angularVelocity": -0.3122253801676767,
+            "velocityX": -0.283756904899847,
+            "velocityY": 2.2069877970647664,
+            "timestamp": 1.4677937542381432
+        },
+        {
+            "x": 4.259328393836262,
+            "y": 2.8855840206662466,
+            "heading": -0.5517027167443236,
+            "angularVelocity": -0.22353621571242488,
+            "velocityX": -0.07478998551086755,
+            "velocityY": 1.6831740946789628,
+            "timestamp": 1.5354953848868456
+        },
+        {
+            "x": 4.260599830778067,
+            "y": 2.9618797181043535,
+            "heading": -0.5613571988137664,
+            "angularVelocity": -0.1426033904496253,
+            "velocityX": 0.018780004700190214,
+            "velocityY": 1.1269403219251917,
+            "timestamp": 1.603197015535548
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -0.0685773908510417,
+            "velocityX": 0.035452162657366115,
+            "velocityY": 0.5630629798778553,
+            "timestamp": 1.6708986461842503
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 0,
+            "velocityX": 1.1764020723168042e-36,
+            "velocityY": 0,
+            "timestamp": 1.7386002768329527
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S3.traj
new file mode 100644
index 00000000..80fc230a
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S3.traj	
@@ -0,0 +1,229 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 7.203126847267154e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6459509070752,
+            "y": 2.420050479995149,
+            "heading": -0.026297254997510774,
+            "angularVelocity": -0.33695614983619343,
+            "velocityX": -0.4362832264603014,
+            "velocityY": -0.47344743777665743,
+            "timestamp": 0.078043552581802
+        },
+        {
+            "x": 7.57752123433784,
+            "y": 2.3463325784745406,
+            "heading": -0.07655763531528098,
+            "angularVelocity": -0.6440042598660941,
+            "velocityX": -0.8768139131756897,
+            "velocityY": -0.9445738831958564,
+            "timestamp": 0.156087105163604
+        },
+        {
+            "x": 7.474195561438856,
+            "y": 2.236153787211583,
+            "heading": -0.14701004173730176,
+            "angularVelocity": -0.9027319245646475,
+            "velocityX": -1.3239488654839289,
+            "velocityY": -1.4117603263569152,
+            "timestamp": 0.23413065774540598
+        },
+        {
+            "x": 7.33507656702923,
+            "y": 2.0901340736402396,
+            "heading": -0.23055657416165956,
+            "angularVelocity": -1.0705116522834195,
+            "velocityX": -1.7825815177216175,
+            "velocityY": -1.8710029046700958,
+            "timestamp": 0.312174210327208
+        },
+        {
+            "x": 7.158294386463989,
+            "y": 1.910067298253472,
+            "heading": -0.30975533817585227,
+            "angularVelocity": -1.014802137964151,
+            "velocityX": -2.2651734155741057,
+            "velocityY": -2.3072601057983104,
+            "timestamp": 0.39021776290901
+        },
+        {
+            "x": 6.940360292129115,
+            "y": 1.7108993896694566,
+            "heading": -0.30975560484167247,
+            "angularVelocity": -0.00000341688469228089,
+            "velocityX": -2.7924676302559823,
+            "velocityY": -2.552009768843728,
+            "timestamp": 0.468261315490812
+        },
+        {
+            "x": 6.6915168927330235,
+            "y": 1.5520279357314815,
+            "heading": -0.30975564017638474,
+            "angularVelocity": -4.5275632793576304e-7,
+            "velocityX": -3.1885196299240257,
+            "velocityY": -2.035676858398594,
+            "timestamp": 0.546304868072614
+        },
+        {
+            "x": 6.419127595403782,
+            "y": 1.43815413781978,
+            "heading": -0.30975568823518523,
+            "angularVelocity": -6.157946287917162e-7,
+            "velocityX": -3.490221655962278,
+            "velocityY": -1.4591057703627237,
+            "timestamp": 0.624348420654416
+        },
+        {
+            "x": 6.131252530334066,
+            "y": 1.37264761019134,
+            "heading": -0.30975577587401576,
+            "angularVelocity": -0.0000011229477336220622,
+            "velocityX": -3.6886463461279972,
+            "velocityY": -0.8393586076157934,
+            "timestamp": 0.7023919732362179
+        },
+        {
+            "x": 5.83657654735769,
+            "y": 1.3574693553002635,
+            "heading": -0.31020540722452916,
+            "angularVelocity": -0.005761287587978277,
+            "velocityX": -3.7757889438387497,
+            "velocityY": -0.1944844178532558,
+            "timestamp": 0.7804355258180199
+        },
+        {
+            "x": 5.546474933624268,
+            "y": 1.3928169012069702,
+            "heading": -0.3189695864044355,
+            "angularVelocity": -0.11229856778804584,
+            "velocityX": -3.717175911864151,
+            "velocityY": 0.4529207697160109,
+            "timestamp": 0.8584790783998218
+        },
+        {
+            "x": 5.303276771996363,
+            "y": 1.4607318697473424,
+            "heading": -0.3307229091538368,
+            "angularVelocity": -0.17360472172913416,
+            "velocityX": -3.5922053767043294,
+            "velocityY": 1.0031511484968414,
+            "timestamp": 0.9261807090485242
+        },
+        {
+            "x": 5.073563214559545,
+            "y": 1.564404927734292,
+            "heading": -0.34378229938421223,
+            "angularVelocity": -0.1928962434919207,
+            "velocityX": -3.3930284283517023,
+            "velocityY": 1.5313229089691542,
+            "timestamp": 0.9938823396972266
+        },
+        {
+            "x": 4.862974125111252,
+            "y": 1.7011642845126724,
+            "heading": -0.35827284529055603,
+            "angularVelocity": -0.2140354045770704,
+            "velocityX": -3.1105467834446325,
+            "velocityY": 2.020030469989471,
+            "timestamp": 1.0615839703459289
+        },
+        {
+            "x": 4.677671719496688,
+            "y": 1.8665243414779629,
+            "heading": -0.3791415933598452,
+            "angularVelocity": -0.30824587043792007,
+            "velocityX": -2.737044940262413,
+            "velocityY": 2.442482631227878,
+            "timestamp": 1.1292856009946313
+        },
+        {
+            "x": 4.523629451068858,
+            "y": 2.053477766497112,
+            "heading": -0.41478083919993697,
+            "angularVelocity": -0.5264163580502632,
+            "velocityX": -2.27531105162187,
+            "velocityY": 2.761431640977936,
+            "timestamp": 1.1969872316433336
+        },
+        {
+            "x": 4.406611773563404,
+            "y": 2.2498215105595722,
+            "heading": -0.45494733767836987,
+            "angularVelocity": -0.5932870167770028,
+            "velocityX": -1.7284321867022447,
+            "velocityY": 2.900133160474012,
+            "timestamp": 1.264688862292036
+        },
+        {
+            "x": 4.327688857963163,
+            "y": 2.4438365235550945,
+            "heading": -0.48799769437613305,
+            "angularVelocity": -0.4881766713911416,
+            "velocityX": -1.1657461547677304,
+            "velocityY": 2.865736188870776,
+            "timestamp": 1.3323904929407384
+        },
+        {
+            "x": 4.28360260298108,
+            "y": 2.622213717107768,
+            "heading": -0.5154307830643909,
+            "angularVelocity": -0.40520573028252516,
+            "velocityX": -0.6511845366152605,
+            "velocityY": 2.634754759132393,
+            "timestamp": 1.4000921235894408
+        },
+        {
+            "x": 4.264391797811532,
+            "y": 2.7716303897908317,
+            "heading": -0.5365689504315967,
+            "angularVelocity": -0.3122253801676767,
+            "velocityX": -0.283756904899847,
+            "velocityY": 2.2069877970647664,
+            "timestamp": 1.4677937542381432
+        },
+        {
+            "x": 4.259328393836262,
+            "y": 2.8855840206662466,
+            "heading": -0.5517027167443236,
+            "angularVelocity": -0.22353621571242488,
+            "velocityX": -0.07478998551086755,
+            "velocityY": 1.6831740946789628,
+            "timestamp": 1.5354953848868456
+        },
+        {
+            "x": 4.260599830778067,
+            "y": 2.9618797181043535,
+            "heading": -0.5613571988137664,
+            "angularVelocity": -0.1426033904496253,
+            "velocityX": 0.018780004700190214,
+            "velocityY": 1.1269403219251917,
+            "timestamp": 1.603197015535548
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -0.0685773908510417,
+            "velocityX": 0.035452162657366115,
+            "velocityY": 0.5630629798778553,
+            "timestamp": 1.6708986461842503
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 0,
+            "velocityX": 1.1764020723168042e-36,
+            "velocityY": 0,
+            "timestamp": 1.7386002768329527
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.1.traj
index 12702aa5..e5689e4c 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.1.traj	
@@ -3,146 +3,191 @@
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -1.3041266298020504e-30,
-            "velocityX": -3.538115329170266e-32,
-            "velocityY": -5.678450231579595e-32,
+            "heading": 0,
+            "angularVelocity": 1.0550386775092505e-35,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.634416744984149,
-            "y": 2.432986632964242,
-            "heading": 3.1489798678517915,
-            "angularVelocity": 0.11416974002165144,
-            "velocityX": -0.5795439822048055,
-            "velocityY": -0.3053051466642632,
-            "timestamp": 0.07865365945555033
-        },
-        {
-            "x": 7.543250241722733,
-            "y": 2.384959916525659,
-            "heading": 3.166940966282971,
-            "angularVelocity": 0.22835680571645545,
-            "velocityX": -1.1590878783324414,
-            "velocityY": -0.6106100691440082,
-            "timestamp": 0.15730731891110067
-        },
-        {
-            "x": 7.406500507389113,
-            "y": 2.312919874583363,
-            "heading": 3.193885700427137,
-            "angularVelocity": 0.34257445019952903,
-            "velocityX": -1.7386315561185093,
-            "velocityY": -0.9159146877712426,
-            "timestamp": 0.235960978366651
-        },
-        {
-            "x": 7.224167573256533,
-            "y": 2.216866534550912,
-            "heading": 3.2298175365571806,
-            "angularVelocity": 0.45683616476039074,
-            "velocityX": -2.3181748362976387,
-            "velocityY": -1.2212189578634138,
-            "timestamp": 0.31461463782220134
-        },
-        {
-            "x": 6.996251494378262,
-            "y": 2.0967999260644565,
-            "heading": 3.2747409040255966,
-            "angularVelocity": 0.5711541939609761,
-            "velocityX": -2.8977174165313135,
-            "velocityY": -1.5265228511625537,
-            "timestamp": 0.3932682972777517
-        },
-        {
-            "x": 6.733004702063821,
-            "y": 1.9581188703597256,
-            "heading": 3.2747409100209213,
-            "angularVelocity": 7.622435906772787e-8,
-            "velocityX": -3.3469109274338362,
-            "velocityY": -1.7631863115422364,
-            "timestamp": 0.47192195673330206
-        },
-        {
-            "x": 6.469756571981624,
-            "y": 1.819440354058334,
-            "heading": 3.2747409160161256,
-            "angularVelocity": 7.622282910230258e-8,
-            "velocityX": -3.3469279357684036,
-            "velocityY": -1.7631540256529774,
-            "timestamp": 0.5505756161888524
-        },
-        {
-            "x": 6.187670135263903,
-            "y": 1.7247907391748545,
-            "heading": 3.2747409182109637,
-            "angularVelocity": 2.790510194041111e-8,
-            "velocityX": -3.5864375373041195,
-            "velocityY": -1.2033720431910602,
-            "timestamp": 0.6292292756444028
-        },
-        {
-            "x": 5.905373163009314,
-            "y": 1.6806498188741994,
-            "heading": 3.236681586369982,
-            "angularVelocity": -0.48388507419022697,
-            "velocityX": -3.5891142790899004,
-            "velocityY": -0.561206186796692,
-            "timestamp": 0.7078829350999531
-        },
-        {
-            "x": 5.661530976081841,
-            "y": 1.6531145750479088,
-            "heading": 3.0865007795970714,
-            "angularVelocity": -1.9093937626358353,
-            "velocityX": -3.1002014224814998,
-            "velocityY": -0.3500821705803985,
-            "timestamp": 0.7865365945555035
-        },
-        {
-            "x": 5.4654145585609575,
-            "y": 1.6369706610279315,
-            "heading": 2.879148606187945,
-            "angularVelocity": -2.6362686090442846,
-            "velocityX": -2.4934175838533594,
-            "velocityY": -0.20525318378988838,
-            "timestamp": 0.8651902540110539
-        },
-        {
-            "x": 5.319930503793118,
-            "y": 1.6274302419537983,
-            "heading": 2.697897950588003,
-            "angularVelocity": -2.304414783171942,
-            "velocityX": -1.8496794144722244,
-            "velocityY": -0.12129656954518526,
-            "timestamp": 0.9438439134666042
-        },
-        {
-            "x": 5.223761655113151,
-            "y": 1.6221627992964307,
-            "heading": 2.568061248027121,
-            "angularVelocity": -1.6507395009924064,
-            "velocityX": -1.2226875309509793,
-            "velocityY": -0.0669700900610273,
-            "timestamp": 1.0224975729221546
+            "x": 7.6504501923310215,
+            "y": 2.437078665707293,
+            "heading": -0.0029944704440250615,
+            "angularVelocity": -0.0457972069493345,
+            "velocityX": -0.45193254781641656,
+            "velocityY": -0.30467539632270557,
+            "timestamp": 0.0653854381848624
+        },
+        {
+            "x": 7.5913505770823395,
+            "y": 2.3972359988448533,
+            "heading": -0.008983367804687599,
+            "angularVelocity": -0.09159374819405963,
+            "velocityX": -0.9038650942674934,
+            "velocityY": -0.609350766294381,
+            "timestamp": 0.1307708763697248
+        },
+        {
+            "x": 7.50270115478166,
+            "y": 2.337472001372052,
+            "heading": -0.017966623739921285,
+            "angularVelocity": -0.13738924422033513,
+            "velocityX": -1.3557976326478658,
+            "velocityY": -0.91402610629957,
+            "timestamp": 0.19615631455458718
+        },
+        {
+            "x": 7.384501926877228,
+            "y": 2.2577866758321745,
+            "heading": -0.02994413949472175,
+            "angularVelocity": -0.18318322989496186,
+            "velocityX": -1.8077301488788986,
+            "velocityY": -1.2187014074079496,
+            "timestamp": 0.2615417527394496
+        },
+        {
+            "x": 7.2367528971895485,
+            "y": 2.1581800263627264,
+            "heading": -0.044915766222073404,
+            "angularVelocity": -0.2289749391144676,
+            "velocityX": -2.25966260667937,
+            "velocityY": -1.5233766452376303,
+            "timestamp": 0.326927190924312
+        },
+        {
+            "x": 7.059454079201559,
+            "y": 2.0386520636124454,
+            "heading": -0.06288119646669635,
+            "angularVelocity": -0.2747619461359239,
+            "velocityX": -2.711594858272837,
+            "velocityY": -1.82805172020631,
+            "timestamp": 0.3923126291091744
+        },
+        {
+            "x": 6.854358935700705,
+            "y": 1.900385597577332,
+            "heading": -0.06288119947800026,
+            "angularVelocity": -4.6054656889756363e-8,
+            "velocityX": -3.136709781174733,
+            "velocityY": -2.114636987584247,
+            "timestamp": 0.45769806729403684
+        },
+        {
+            "x": 6.636845838072133,
+            "y": 1.782615967982618,
+            "heading": -0.06288120256062167,
+            "angularVelocity": -4.714538109405926e-8,
+            "velocityX": -3.3266290425951324,
+            "velocityY": -1.801159904469057,
+            "timestamp": 0.5230835054788993
+        },
+        {
+            "x": 6.4046630859375,
+            "y": 1.6973352432250977,
+            "heading": -0.06288120593828476,
+            "angularVelocity": -5.1657726572043936e-8,
+            "velocityX": -3.5509856411482486,
+            "velocityY": -1.3042770244409516,
+            "timestamp": 0.5884689436637617
+        },
+        {
+            "x": 6.207746736822867,
+            "y": 1.6503996785686041,
+            "heading": -0.06288121580879107,
+            "angularVelocity": -1.8445407171610623e-7,
+            "velocityX": -3.679854027466254,
+            "velocityY": -0.8771035386810692,
+            "timestamp": 0.6419809437716079
+        },
+        {
+            "x": 6.009908768236521,
+            "y": 1.6271742750630454,
+            "heading": -0.0724170210773865,
+            "angularVelocity": -0.17819938050114592,
+            "velocityX": -3.697076696584537,
+            "velocityY": -0.43402234001253925,
+            "timestamp": 0.695492943879454
+        },
+        {
+            "x": 5.8260492909953046,
+            "y": 1.6186155743000838,
+            "heading": -0.12620190803666176,
+            "angularVelocity": -1.0050995449783064,
+            "velocityX": -3.43585507681777,
+            "velocityY": -0.1599398405163814,
+            "timestamp": 0.7490049439873002
+        },
+        {
+            "x": 5.6625912601386625,
+            "y": 1.614879890442449,
+            "heading": -0.22199295256207294,
+            "angularVelocity": -1.7900852954918034,
+            "velocityX": -3.0546051451490293,
+            "velocityY": -0.06981020799271546,
+            "timestamp": 0.8025169440951464
+        },
+        {
+            "x": 5.5227239807129855,
+            "y": 1.6141995251565013,
+            "heading": -0.32512734843535046,
+            "angularVelocity": -1.9273134187738115,
+            "velocityX": -2.61375540334492,
+            "velocityY": -0.012714256327113658,
+            "timestamp": 0.8560289442029926
+        },
+        {
+            "x": 5.406619958232834,
+            "y": 1.6150039384726416,
+            "heading": -0.42188556792063076,
+            "angularVelocity": -1.8081592781110263,
+            "velocityX": -2.169681982474204,
+            "velocityY": 0.015032391136922594,
+            "timestamp": 0.9095409443108388
+        },
+        {
+            "x": 5.314084878105213,
+            "y": 1.6164421457440925,
+            "heading": -0.5051954003859285,
+            "angularVelocity": -1.5568439284160218,
+            "velocityX": -1.7292397955809797,
+            "velocityY": 0.026876350511144828,
+            "timestamp": 0.9630529444186849
+        },
+        {
+            "x": 5.244912091070787,
+            "y": 1.617980616061371,
+            "heading": -0.5709381839957368,
+            "angularVelocity": -1.2285615091439794,
+            "velocityX": -1.2926593454742539,
+            "velocityY": 0.028750005871169333,
+            "timestamp": 1.016564944526531
+        },
+        {
+            "x": 5.198930392622704,
+            "y": 1.6192554664106382,
+            "heading": -0.6164892293058035,
+            "angularVelocity": -0.851230475748707,
+            "velocityX": -0.8592782619863396,
+            "velocityY": 0.023823634823929046,
+            "timestamp": 1.0700769446343772
         },
         {
             "x": 5.176,
             "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.8653284347892848,
-            "velocityX": -0.6072400883031053,
-            "velocityY": -0.027497758036964814,
-            "timestamp": 1.1011512323777048
+            "heading": -0.64,
+            "angularVelocity": -0.4393551100092281,
+            "velocityX": -0.42850935447171357,
+            "velocityY": 0.013913394899485362,
+            "timestamp": 1.1235889447422234
         },
         {
             "x": 5.176,
             "y": 1.62,
-            "heading": 2.5,
+            "heading": -0.64,
             "angularVelocity": 0,
-            "velocityX": -1.7743974032793004e-34,
-            "velocityY": 1.752346488498292e-33,
-            "timestamp": 1.179804891833255
+            "velocityX": 0,
+            "velocityY": 6.2403191565519e-36,
+            "timestamp": 1.1771009448500696
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.traj
index 12702aa5..e5689e4c 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.traj	
@@ -3,146 +3,191 @@
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -1.3041266298020504e-30,
-            "velocityX": -3.538115329170266e-32,
-            "velocityY": -5.678450231579595e-32,
+            "heading": 0,
+            "angularVelocity": 1.0550386775092505e-35,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.634416744984149,
-            "y": 2.432986632964242,
-            "heading": 3.1489798678517915,
-            "angularVelocity": 0.11416974002165144,
-            "velocityX": -0.5795439822048055,
-            "velocityY": -0.3053051466642632,
-            "timestamp": 0.07865365945555033
-        },
-        {
-            "x": 7.543250241722733,
-            "y": 2.384959916525659,
-            "heading": 3.166940966282971,
-            "angularVelocity": 0.22835680571645545,
-            "velocityX": -1.1590878783324414,
-            "velocityY": -0.6106100691440082,
-            "timestamp": 0.15730731891110067
-        },
-        {
-            "x": 7.406500507389113,
-            "y": 2.312919874583363,
-            "heading": 3.193885700427137,
-            "angularVelocity": 0.34257445019952903,
-            "velocityX": -1.7386315561185093,
-            "velocityY": -0.9159146877712426,
-            "timestamp": 0.235960978366651
-        },
-        {
-            "x": 7.224167573256533,
-            "y": 2.216866534550912,
-            "heading": 3.2298175365571806,
-            "angularVelocity": 0.45683616476039074,
-            "velocityX": -2.3181748362976387,
-            "velocityY": -1.2212189578634138,
-            "timestamp": 0.31461463782220134
-        },
-        {
-            "x": 6.996251494378262,
-            "y": 2.0967999260644565,
-            "heading": 3.2747409040255966,
-            "angularVelocity": 0.5711541939609761,
-            "velocityX": -2.8977174165313135,
-            "velocityY": -1.5265228511625537,
-            "timestamp": 0.3932682972777517
-        },
-        {
-            "x": 6.733004702063821,
-            "y": 1.9581188703597256,
-            "heading": 3.2747409100209213,
-            "angularVelocity": 7.622435906772787e-8,
-            "velocityX": -3.3469109274338362,
-            "velocityY": -1.7631863115422364,
-            "timestamp": 0.47192195673330206
-        },
-        {
-            "x": 6.469756571981624,
-            "y": 1.819440354058334,
-            "heading": 3.2747409160161256,
-            "angularVelocity": 7.622282910230258e-8,
-            "velocityX": -3.3469279357684036,
-            "velocityY": -1.7631540256529774,
-            "timestamp": 0.5505756161888524
-        },
-        {
-            "x": 6.187670135263903,
-            "y": 1.7247907391748545,
-            "heading": 3.2747409182109637,
-            "angularVelocity": 2.790510194041111e-8,
-            "velocityX": -3.5864375373041195,
-            "velocityY": -1.2033720431910602,
-            "timestamp": 0.6292292756444028
-        },
-        {
-            "x": 5.905373163009314,
-            "y": 1.6806498188741994,
-            "heading": 3.236681586369982,
-            "angularVelocity": -0.48388507419022697,
-            "velocityX": -3.5891142790899004,
-            "velocityY": -0.561206186796692,
-            "timestamp": 0.7078829350999531
-        },
-        {
-            "x": 5.661530976081841,
-            "y": 1.6531145750479088,
-            "heading": 3.0865007795970714,
-            "angularVelocity": -1.9093937626358353,
-            "velocityX": -3.1002014224814998,
-            "velocityY": -0.3500821705803985,
-            "timestamp": 0.7865365945555035
-        },
-        {
-            "x": 5.4654145585609575,
-            "y": 1.6369706610279315,
-            "heading": 2.879148606187945,
-            "angularVelocity": -2.6362686090442846,
-            "velocityX": -2.4934175838533594,
-            "velocityY": -0.20525318378988838,
-            "timestamp": 0.8651902540110539
-        },
-        {
-            "x": 5.319930503793118,
-            "y": 1.6274302419537983,
-            "heading": 2.697897950588003,
-            "angularVelocity": -2.304414783171942,
-            "velocityX": -1.8496794144722244,
-            "velocityY": -0.12129656954518526,
-            "timestamp": 0.9438439134666042
-        },
-        {
-            "x": 5.223761655113151,
-            "y": 1.6221627992964307,
-            "heading": 2.568061248027121,
-            "angularVelocity": -1.6507395009924064,
-            "velocityX": -1.2226875309509793,
-            "velocityY": -0.0669700900610273,
-            "timestamp": 1.0224975729221546
+            "x": 7.6504501923310215,
+            "y": 2.437078665707293,
+            "heading": -0.0029944704440250615,
+            "angularVelocity": -0.0457972069493345,
+            "velocityX": -0.45193254781641656,
+            "velocityY": -0.30467539632270557,
+            "timestamp": 0.0653854381848624
+        },
+        {
+            "x": 7.5913505770823395,
+            "y": 2.3972359988448533,
+            "heading": -0.008983367804687599,
+            "angularVelocity": -0.09159374819405963,
+            "velocityX": -0.9038650942674934,
+            "velocityY": -0.609350766294381,
+            "timestamp": 0.1307708763697248
+        },
+        {
+            "x": 7.50270115478166,
+            "y": 2.337472001372052,
+            "heading": -0.017966623739921285,
+            "angularVelocity": -0.13738924422033513,
+            "velocityX": -1.3557976326478658,
+            "velocityY": -0.91402610629957,
+            "timestamp": 0.19615631455458718
+        },
+        {
+            "x": 7.384501926877228,
+            "y": 2.2577866758321745,
+            "heading": -0.02994413949472175,
+            "angularVelocity": -0.18318322989496186,
+            "velocityX": -1.8077301488788986,
+            "velocityY": -1.2187014074079496,
+            "timestamp": 0.2615417527394496
+        },
+        {
+            "x": 7.2367528971895485,
+            "y": 2.1581800263627264,
+            "heading": -0.044915766222073404,
+            "angularVelocity": -0.2289749391144676,
+            "velocityX": -2.25966260667937,
+            "velocityY": -1.5233766452376303,
+            "timestamp": 0.326927190924312
+        },
+        {
+            "x": 7.059454079201559,
+            "y": 2.0386520636124454,
+            "heading": -0.06288119646669635,
+            "angularVelocity": -0.2747619461359239,
+            "velocityX": -2.711594858272837,
+            "velocityY": -1.82805172020631,
+            "timestamp": 0.3923126291091744
+        },
+        {
+            "x": 6.854358935700705,
+            "y": 1.900385597577332,
+            "heading": -0.06288119947800026,
+            "angularVelocity": -4.6054656889756363e-8,
+            "velocityX": -3.136709781174733,
+            "velocityY": -2.114636987584247,
+            "timestamp": 0.45769806729403684
+        },
+        {
+            "x": 6.636845838072133,
+            "y": 1.782615967982618,
+            "heading": -0.06288120256062167,
+            "angularVelocity": -4.714538109405926e-8,
+            "velocityX": -3.3266290425951324,
+            "velocityY": -1.801159904469057,
+            "timestamp": 0.5230835054788993
+        },
+        {
+            "x": 6.4046630859375,
+            "y": 1.6973352432250977,
+            "heading": -0.06288120593828476,
+            "angularVelocity": -5.1657726572043936e-8,
+            "velocityX": -3.5509856411482486,
+            "velocityY": -1.3042770244409516,
+            "timestamp": 0.5884689436637617
+        },
+        {
+            "x": 6.207746736822867,
+            "y": 1.6503996785686041,
+            "heading": -0.06288121580879107,
+            "angularVelocity": -1.8445407171610623e-7,
+            "velocityX": -3.679854027466254,
+            "velocityY": -0.8771035386810692,
+            "timestamp": 0.6419809437716079
+        },
+        {
+            "x": 6.009908768236521,
+            "y": 1.6271742750630454,
+            "heading": -0.0724170210773865,
+            "angularVelocity": -0.17819938050114592,
+            "velocityX": -3.697076696584537,
+            "velocityY": -0.43402234001253925,
+            "timestamp": 0.695492943879454
+        },
+        {
+            "x": 5.8260492909953046,
+            "y": 1.6186155743000838,
+            "heading": -0.12620190803666176,
+            "angularVelocity": -1.0050995449783064,
+            "velocityX": -3.43585507681777,
+            "velocityY": -0.1599398405163814,
+            "timestamp": 0.7490049439873002
+        },
+        {
+            "x": 5.6625912601386625,
+            "y": 1.614879890442449,
+            "heading": -0.22199295256207294,
+            "angularVelocity": -1.7900852954918034,
+            "velocityX": -3.0546051451490293,
+            "velocityY": -0.06981020799271546,
+            "timestamp": 0.8025169440951464
+        },
+        {
+            "x": 5.5227239807129855,
+            "y": 1.6141995251565013,
+            "heading": -0.32512734843535046,
+            "angularVelocity": -1.9273134187738115,
+            "velocityX": -2.61375540334492,
+            "velocityY": -0.012714256327113658,
+            "timestamp": 0.8560289442029926
+        },
+        {
+            "x": 5.406619958232834,
+            "y": 1.6150039384726416,
+            "heading": -0.42188556792063076,
+            "angularVelocity": -1.8081592781110263,
+            "velocityX": -2.169681982474204,
+            "velocityY": 0.015032391136922594,
+            "timestamp": 0.9095409443108388
+        },
+        {
+            "x": 5.314084878105213,
+            "y": 1.6164421457440925,
+            "heading": -0.5051954003859285,
+            "angularVelocity": -1.5568439284160218,
+            "velocityX": -1.7292397955809797,
+            "velocityY": 0.026876350511144828,
+            "timestamp": 0.9630529444186849
+        },
+        {
+            "x": 5.244912091070787,
+            "y": 1.617980616061371,
+            "heading": -0.5709381839957368,
+            "angularVelocity": -1.2285615091439794,
+            "velocityX": -1.2926593454742539,
+            "velocityY": 0.028750005871169333,
+            "timestamp": 1.016564944526531
+        },
+        {
+            "x": 5.198930392622704,
+            "y": 1.6192554664106382,
+            "heading": -0.6164892293058035,
+            "angularVelocity": -0.851230475748707,
+            "velocityX": -0.8592782619863396,
+            "velocityY": 0.023823634823929046,
+            "timestamp": 1.0700769446343772
         },
         {
             "x": 5.176,
             "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.8653284347892848,
-            "velocityX": -0.6072400883031053,
-            "velocityY": -0.027497758036964814,
-            "timestamp": 1.1011512323777048
+            "heading": -0.64,
+            "angularVelocity": -0.4393551100092281,
+            "velocityX": -0.42850935447171357,
+            "velocityY": 0.013913394899485362,
+            "timestamp": 1.1235889447422234
         },
         {
             "x": 5.176,
             "y": 1.62,
-            "heading": 2.5,
+            "heading": -0.64,
             "angularVelocity": 0,
-            "velocityX": -1.7743974032793004e-34,
-            "velocityY": 1.752346488498292e-33,
-            "timestamp": 1.179804891833255
+            "velocityX": 0,
+            "velocityY": 6.2403191565519e-36,
+            "timestamp": 1.1771009448500696
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.1.traj
new file mode 100644
index 00000000..f49877f8
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.1.traj	
@@ -0,0 +1,256 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": 6.426641305668581e-31,
+            "velocityX": -5.011351780601486e-32,
+            "velocityY": -1.0551805148640748e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6491080594765135,
+            "y": 2.4958059548034544,
+            "heading": -0.02134777834454554,
+            "angularVelocity": -0.2757848804434067,
+            "velocityX": -0.3990827516771383,
+            "velocityY": 0.5013212819261742,
+            "timestamp": 0.07740735572675958
+        },
+        {
+            "x": 7.587308326375472,
+            "y": 2.5734484495799954,
+            "heading": -0.06302080337998518,
+            "angularVelocity": -0.538360013001088,
+            "velocityX": -0.7983702907923742,
+            "velocityY": 1.0030376835324535,
+            "timestamp": 0.15481471145351916
+        },
+        {
+            "x": 7.4945787588029695,
+            "y": 2.6899753845455243,
+            "heading": -0.12336744193035674,
+            "angularVelocity": -0.7795982433941759,
+            "velocityX": -1.1979425818371583,
+            "velocityY": 1.5053728921687748,
+            "timestamp": 0.23222206718027874
+        },
+        {
+            "x": 7.370887761034633,
+            "y": 2.845471450080925,
+            "heading": -0.19922081386712712,
+            "angularVelocity": -0.9799245979222482,
+            "velocityX": -1.5979230475841766,
+            "velocityY": 2.0088021877957765,
+            "timestamp": 0.3096294229070383
+        },
+        {
+            "x": 7.216202117777622,
+            "y": 3.0401051125017453,
+            "heading": -0.28194072984640106,
+            "angularVelocity": -1.0686312069781692,
+            "velocityX": -1.9983326107022177,
+            "velocityY": 2.5144078439762083,
+            "timestamp": 0.38703677863379793
+        },
+        {
+            "x": 7.032473858656331,
+            "y": 3.268122248690632,
+            "heading": -0.2819407809538647,
+            "angularVelocity": -6.602404015783628e-7,
+            "velocityX": -2.3735245493959742,
+            "velocityY": 2.9456778887237176,
+            "timestamp": 0.46444413436055754
+        },
+        {
+            "x": 6.834683469529352,
+            "y": 3.4840547774927524,
+            "heading": -0.28194080060331267,
+            "angularVelocity": -2.538447128540298e-7,
+            "velocityX": -2.5551885511392456,
+            "velocityY": 2.7895608469606037,
+            "timestamp": 0.5418514900873171
+        },
+        {
+            "x": 6.603064880376339,
+            "y": 3.663221523268011,
+            "heading": -0.2819408226641663,
+            "angularVelocity": -2.8499686418440244e-7,
+            "velocityX": -2.992203867170001,
+            "velocityY": 2.314595868740169,
+            "timestamp": 0.6192588458140768
+        },
+        {
+            "x": 6.344360158554223,
+            "y": 3.8004066501958644,
+            "heading": -0.2819407635550332,
+            "angularVelocity": 7.636113202840995e-7,
+            "velocityX": -3.342120647233029,
+            "velocityY": 1.7722492344539278,
+            "timestamp": 0.6966662015408364
+        },
+        {
+            "x": 6.0661003337524155,
+            "y": 3.891617196013079,
+            "heading": -0.28194070942615473,
+            "angularVelocity": 6.992730598438293e-7,
+            "velocityX": -3.59474654817093,
+            "velocityY": 1.1783188427102327,
+            "timestamp": 0.774073557267596
+        },
+        {
+            "x": 5.7830200242104235,
+            "y": 3.966540038544743,
+            "heading": -0.2819406560965727,
+            "angularVelocity": 6.889472132026456e-7,
+            "velocityX": -3.6570207945254936,
+            "velocityY": 0.967903396624646,
+            "timestamp": 0.8514809129943556
+        },
+        {
+            "x": 5.499939604590639,
+            "y": 4.041462465169378,
+            "heading": -0.2819406027672389,
+            "angularVelocity": 6.889440059098256e-7,
+            "velocityX": -3.657022216584049,
+            "velocityY": 0.9678980236594484,
+            "timestamp": 0.9288882687211152
+        },
+        {
+            "x": 5.234359644348809,
+            "y": 4.1279159927443425,
+            "heading": -0.3272205989423401,
+            "angularVelocity": -0.5849572789301486,
+            "velocityX": -3.430939576069496,
+            "velocityY": 1.1168644990295,
+            "timestamp": 1.0062956244478747
+        },
+        {
+            "x": 4.992805004119873,
+            "y": 4.258058071136475,
+            "heading": -0.38392632852173914,
+            "angularVelocity": -0.7325625458589549,
+            "velocityX": -3.120564421314147,
+            "velocityY": 1.6812624222886623,
+            "timestamp": 1.0837029801746343
+        },
+        {
+            "x": 4.760679474369801,
+            "y": 4.451318301231298,
+            "heading": -0.43925940371465827,
+            "angularVelocity": -0.650567727765224,
+            "velocityX": -2.7291701738847216,
+            "velocityY": 2.272219071899485,
+            "timestamp": 1.1687565000948184
+        },
+        {
+            "x": 4.558064597648056,
+            "y": 4.695759350557987,
+            "heading": -0.45036550730749886,
+            "angularVelocity": -0.13057782444839394,
+            "velocityX": -2.3822044862091225,
+            "velocityY": 2.8739674684372165,
+            "timestamp": 1.2538100200150024
+        },
+        {
+            "x": 4.30999794561686,
+            "y": 4.900667947770545,
+            "heading": -0.4503653988140509,
+            "angularVelocity": 0.0000012755903345298267,
+            "velocityX": -2.9165947777821004,
+            "velocityY": 2.4091724528843765,
+            "timestamp": 1.3388635399351865
+        },
+        {
+            "x": 4.028045524182051,
+            "y": 5.05567579900462,
+            "heading": -0.4503653036134663,
+            "angularVelocity": 0.0000011193021141522574,
+            "velocityX": -3.3150000340890857,
+            "velocityY": 1.8224742653747859,
+            "timestamp": 1.4239170598553705
+        },
+        {
+            "x": 3.731575050469511,
+            "y": 5.180695241574213,
+            "heading": -0.45036521232637566,
+            "angularVelocity": 0.000001073289978617534,
+            "velocityX": -3.485693172848094,
+            "velocityY": 1.4698914599542185,
+            "timestamp": 1.5089705797755546
+        },
+        {
+            "x": 3.4348291016010033,
+            "y": 5.305059402921897,
+            "heading": -0.4503651210307298,
+            "angularVelocity": 0.000001073390565931622,
+            "velocityX": -3.488932017711167,
+            "velocityY": 1.4621871201127652,
+            "timestamp": 1.5940240996957387
+        },
+        {
+            "x": 3.120089238118937,
+            "y": 5.371868296261872,
+            "heading": -0.45036508370615375,
+            "angularVelocity": 4.3883634791364236e-7,
+            "velocityX": -3.7004919229372337,
+            "velocityY": 0.785492398232055,
+            "timestamp": 1.6790776196159227
+        },
+        {
+            "x": 2.8304866761911076,
+            "y": 5.435580552385651,
+            "heading": -0.37567464629819297,
+            "angularVelocity": 0.8781580994889028,
+            "velocityX": -3.404945053415936,
+            "velocityY": 0.7490842963759552,
+            "timestamp": 1.7641311395361068
+        },
+        {
+            "x": 2.5976382602327472,
+            "y": 5.485810295855226,
+            "heading": -0.2445738789443125,
+            "angularVelocity": 1.5413914377313083,
+            "velocityX": -2.7376693660283826,
+            "velocityY": 0.5905663106789283,
+            "timestamp": 1.8491846594562908
+        },
+        {
+            "x": 2.4235493783626687,
+            "y": 5.523078200691987,
+            "heading": -0.12850773623159037,
+            "angularVelocity": 1.3646248012033382,
+            "velocityX": -2.0468157230112984,
+            "velocityY": 0.43817004718549557,
+            "timestamp": 1.934238179376475
+        },
+        {
+            "x": 2.3077761259926057,
+            "y": 5.547737838884683,
+            "heading": -0.04437897835044122,
+            "angularVelocity": 0.9891272925576834,
+            "velocityX": -1.3611812007158846,
+            "velocityY": 0.2899308366756611,
+            "timestamp": 2.0192916992966587
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 1.7803711242706045e-30,
+            "angularVelocity": 0.521777092730431,
+            "velocityX": -0.6792914161204248,
+            "velocityY": 0.14416994295838573,
+            "timestamp": 2.104345219216843
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 1.3562125763475036e-31,
+            "velocityY": 2.290518308039125e-31,
+            "timestamp": 2.189398739137027
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.traj
new file mode 100644
index 00000000..f49877f8
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.traj	
@@ -0,0 +1,256 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": 6.426641305668581e-31,
+            "velocityX": -5.011351780601486e-32,
+            "velocityY": -1.0551805148640748e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6491080594765135,
+            "y": 2.4958059548034544,
+            "heading": -0.02134777834454554,
+            "angularVelocity": -0.2757848804434067,
+            "velocityX": -0.3990827516771383,
+            "velocityY": 0.5013212819261742,
+            "timestamp": 0.07740735572675958
+        },
+        {
+            "x": 7.587308326375472,
+            "y": 2.5734484495799954,
+            "heading": -0.06302080337998518,
+            "angularVelocity": -0.538360013001088,
+            "velocityX": -0.7983702907923742,
+            "velocityY": 1.0030376835324535,
+            "timestamp": 0.15481471145351916
+        },
+        {
+            "x": 7.4945787588029695,
+            "y": 2.6899753845455243,
+            "heading": -0.12336744193035674,
+            "angularVelocity": -0.7795982433941759,
+            "velocityX": -1.1979425818371583,
+            "velocityY": 1.5053728921687748,
+            "timestamp": 0.23222206718027874
+        },
+        {
+            "x": 7.370887761034633,
+            "y": 2.845471450080925,
+            "heading": -0.19922081386712712,
+            "angularVelocity": -0.9799245979222482,
+            "velocityX": -1.5979230475841766,
+            "velocityY": 2.0088021877957765,
+            "timestamp": 0.3096294229070383
+        },
+        {
+            "x": 7.216202117777622,
+            "y": 3.0401051125017453,
+            "heading": -0.28194072984640106,
+            "angularVelocity": -1.0686312069781692,
+            "velocityX": -1.9983326107022177,
+            "velocityY": 2.5144078439762083,
+            "timestamp": 0.38703677863379793
+        },
+        {
+            "x": 7.032473858656331,
+            "y": 3.268122248690632,
+            "heading": -0.2819407809538647,
+            "angularVelocity": -6.602404015783628e-7,
+            "velocityX": -2.3735245493959742,
+            "velocityY": 2.9456778887237176,
+            "timestamp": 0.46444413436055754
+        },
+        {
+            "x": 6.834683469529352,
+            "y": 3.4840547774927524,
+            "heading": -0.28194080060331267,
+            "angularVelocity": -2.538447128540298e-7,
+            "velocityX": -2.5551885511392456,
+            "velocityY": 2.7895608469606037,
+            "timestamp": 0.5418514900873171
+        },
+        {
+            "x": 6.603064880376339,
+            "y": 3.663221523268011,
+            "heading": -0.2819408226641663,
+            "angularVelocity": -2.8499686418440244e-7,
+            "velocityX": -2.992203867170001,
+            "velocityY": 2.314595868740169,
+            "timestamp": 0.6192588458140768
+        },
+        {
+            "x": 6.344360158554223,
+            "y": 3.8004066501958644,
+            "heading": -0.2819407635550332,
+            "angularVelocity": 7.636113202840995e-7,
+            "velocityX": -3.342120647233029,
+            "velocityY": 1.7722492344539278,
+            "timestamp": 0.6966662015408364
+        },
+        {
+            "x": 6.0661003337524155,
+            "y": 3.891617196013079,
+            "heading": -0.28194070942615473,
+            "angularVelocity": 6.992730598438293e-7,
+            "velocityX": -3.59474654817093,
+            "velocityY": 1.1783188427102327,
+            "timestamp": 0.774073557267596
+        },
+        {
+            "x": 5.7830200242104235,
+            "y": 3.966540038544743,
+            "heading": -0.2819406560965727,
+            "angularVelocity": 6.889472132026456e-7,
+            "velocityX": -3.6570207945254936,
+            "velocityY": 0.967903396624646,
+            "timestamp": 0.8514809129943556
+        },
+        {
+            "x": 5.499939604590639,
+            "y": 4.041462465169378,
+            "heading": -0.2819406027672389,
+            "angularVelocity": 6.889440059098256e-7,
+            "velocityX": -3.657022216584049,
+            "velocityY": 0.9678980236594484,
+            "timestamp": 0.9288882687211152
+        },
+        {
+            "x": 5.234359644348809,
+            "y": 4.1279159927443425,
+            "heading": -0.3272205989423401,
+            "angularVelocity": -0.5849572789301486,
+            "velocityX": -3.430939576069496,
+            "velocityY": 1.1168644990295,
+            "timestamp": 1.0062956244478747
+        },
+        {
+            "x": 4.992805004119873,
+            "y": 4.258058071136475,
+            "heading": -0.38392632852173914,
+            "angularVelocity": -0.7325625458589549,
+            "velocityX": -3.120564421314147,
+            "velocityY": 1.6812624222886623,
+            "timestamp": 1.0837029801746343
+        },
+        {
+            "x": 4.760679474369801,
+            "y": 4.451318301231298,
+            "heading": -0.43925940371465827,
+            "angularVelocity": -0.650567727765224,
+            "velocityX": -2.7291701738847216,
+            "velocityY": 2.272219071899485,
+            "timestamp": 1.1687565000948184
+        },
+        {
+            "x": 4.558064597648056,
+            "y": 4.695759350557987,
+            "heading": -0.45036550730749886,
+            "angularVelocity": -0.13057782444839394,
+            "velocityX": -2.3822044862091225,
+            "velocityY": 2.8739674684372165,
+            "timestamp": 1.2538100200150024
+        },
+        {
+            "x": 4.30999794561686,
+            "y": 4.900667947770545,
+            "heading": -0.4503653988140509,
+            "angularVelocity": 0.0000012755903345298267,
+            "velocityX": -2.9165947777821004,
+            "velocityY": 2.4091724528843765,
+            "timestamp": 1.3388635399351865
+        },
+        {
+            "x": 4.028045524182051,
+            "y": 5.05567579900462,
+            "heading": -0.4503653036134663,
+            "angularVelocity": 0.0000011193021141522574,
+            "velocityX": -3.3150000340890857,
+            "velocityY": 1.8224742653747859,
+            "timestamp": 1.4239170598553705
+        },
+        {
+            "x": 3.731575050469511,
+            "y": 5.180695241574213,
+            "heading": -0.45036521232637566,
+            "angularVelocity": 0.000001073289978617534,
+            "velocityX": -3.485693172848094,
+            "velocityY": 1.4698914599542185,
+            "timestamp": 1.5089705797755546
+        },
+        {
+            "x": 3.4348291016010033,
+            "y": 5.305059402921897,
+            "heading": -0.4503651210307298,
+            "angularVelocity": 0.000001073390565931622,
+            "velocityX": -3.488932017711167,
+            "velocityY": 1.4621871201127652,
+            "timestamp": 1.5940240996957387
+        },
+        {
+            "x": 3.120089238118937,
+            "y": 5.371868296261872,
+            "heading": -0.45036508370615375,
+            "angularVelocity": 4.3883634791364236e-7,
+            "velocityX": -3.7004919229372337,
+            "velocityY": 0.785492398232055,
+            "timestamp": 1.6790776196159227
+        },
+        {
+            "x": 2.8304866761911076,
+            "y": 5.435580552385651,
+            "heading": -0.37567464629819297,
+            "angularVelocity": 0.8781580994889028,
+            "velocityX": -3.404945053415936,
+            "velocityY": 0.7490842963759552,
+            "timestamp": 1.7641311395361068
+        },
+        {
+            "x": 2.5976382602327472,
+            "y": 5.485810295855226,
+            "heading": -0.2445738789443125,
+            "angularVelocity": 1.5413914377313083,
+            "velocityX": -2.7376693660283826,
+            "velocityY": 0.5905663106789283,
+            "timestamp": 1.8491846594562908
+        },
+        {
+            "x": 2.4235493783626687,
+            "y": 5.523078200691987,
+            "heading": -0.12850773623159037,
+            "angularVelocity": 1.3646248012033382,
+            "velocityX": -2.0468157230112984,
+            "velocityY": 0.43817004718549557,
+            "timestamp": 1.934238179376475
+        },
+        {
+            "x": 2.3077761259926057,
+            "y": 5.547737838884683,
+            "heading": -0.04437897835044122,
+            "angularVelocity": 0.9891272925576834,
+            "velocityX": -1.3611812007158846,
+            "velocityY": 0.2899308366756611,
+            "timestamp": 2.0192916992966587
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 1.7803711242706045e-30,
+            "angularVelocity": 0.521777092730431,
+            "velocityX": -0.6792914161204248,
+            "velocityY": 0.14416994295838573,
+            "timestamp": 2.104345219216843
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 1.3562125763475036e-31,
+            "velocityY": 2.290518308039125e-31,
+            "timestamp": 2.189398739137027
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W3.1.traj
new file mode 100644
index 00000000..15a1cd3f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W3.1.traj	
@@ -0,0 +1,274 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": -4.62162066936404e-33,
+            "velocityX": 3.0688151507515204e-31,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.623584376302727,
+            "y": 2.4297857949616017,
+            "heading": 0.033539307380936634,
+            "angularVelocity": 0.38483343876921716,
+            "velocityX": -0.6473186288895079,
+            "velocityY": -0.3122585684118786,
+            "timestamp": 0.08715278871867994
+        },
+        {
+            "x": 7.510615150776605,
+            "y": 2.375312721693156,
+            "heading": 0.0969954791736376,
+            "angularVelocity": 0.7281025968948721,
+            "velocityX": -1.2962204329545195,
+            "velocityY": -0.6250296068468768,
+            "timestamp": 0.1743055774373599
+        },
+        {
+            "x": 7.340880293995689,
+            "y": 2.2935273668964844,
+            "heading": 0.1838752961952568,
+            "angularVelocity": 0.9968678948651678,
+            "velocityX": -1.9475550843106406,
+            "velocityY": -0.9384135149210906,
+            "timestamp": 0.26145836615603985
+        },
+        {
+            "x": 7.114069036249808,
+            "y": 2.1844157133503646,
+            "heading": 0.27996244701307155,
+            "angularVelocity": 1.1025137833279668,
+            "velocityX": -2.6024555390648842,
+            "velocityY": -1.251958258023393,
+            "timestamp": 0.3486111548747198
+        },
+        {
+            "x": 6.831104073589339,
+            "y": 2.0490142604857513,
+            "heading": 0.32912237638568254,
+            "angularVelocity": 0.5640660510737534,
+            "velocityX": -3.246768885088134,
+            "velocityY": -1.5536101007814562,
+            "timestamp": 0.4357639435933997
+        },
+        {
+            "x": 6.534528280813305,
+            "y": 1.9049975621731787,
+            "heading": 0.3291224030694369,
+            "angularVelocity": 3.0617212321522066e-7,
+            "velocityX": -3.4029409401155197,
+            "velocityY": -1.6524623070575855,
+            "timestamp": 0.5229167323120797
+        },
+        {
+            "x": 6.237952262222522,
+            "y": 1.7609813288852085,
+            "heading": 0.3291224297529421,
+            "angularVelocity": 3.0616926449614015e-7,
+            "velocityX": -3.402943531137019,
+            "velocityY": -1.6524569713178037,
+            "timestamp": 0.6100695210307596
+        },
+        {
+            "x": 5.936787262454918,
+            "y": 1.6268259970502223,
+            "heading": 0.32912245667190637,
+            "angularVelocity": 3.0887094574020004e-7,
+            "velocityX": -3.4555979699024117,
+            "velocityY": -1.5393119808021931,
+            "timestamp": 0.6972223097494396
+        },
+        {
+            "x": 5.615527521313305,
+            "y": 1.5527301986214108,
+            "heading": 0.32912248658360005,
+            "angularVelocity": 3.4320982850713803e-7,
+            "velocityX": -3.6861670850098154,
+            "velocityY": -0.850182759704744,
+            "timestamp": 0.7843750984681195
+        },
+        {
+            "x": 5.286027461953728,
+            "y": 1.5414290964422628,
+            "heading": 0.32912252585289026,
+            "angularVelocity": 4.50579847209181e-7,
+            "velocityX": -3.780717337951944,
+            "velocityY": -0.12967000075725457,
+            "timestamp": 0.8715278871867994
+        },
+        {
+            "x": 4.960446284223111,
+            "y": 1.593339788205863,
+            "heading": 0.3291233220408656,
+            "angularVelocity": 0.000009135542155699871,
+            "velocityX": -3.7357516898462033,
+            "velocityY": 0.5956285797252272,
+            "timestamp": 0.9586806759054793
+        },
+        {
+            "x": 4.650797848549272,
+            "y": 1.7065466836899894,
+            "heading": 0.3291235959637348,
+            "angularVelocity": 0.0000031430189815062194,
+            "velocityX": -3.5529377800330906,
+            "velocityY": 1.2989474823295228,
+            "timestamp": 1.0458334646241594
+        },
+        {
+            "x": 4.368508202692925,
+            "y": 1.8768714467040708,
+            "heading": 0.32912381701495697,
+            "angularVelocity": 0.000002536364302217629,
+            "velocityX": -3.2390202310972316,
+            "velocityY": 1.9543237286860882,
+            "timestamp": 1.1329862533428394
+        },
+        {
+            "x": 4.092384496633659,
+            "y": 2.0570204638146983,
+            "heading": 0.3291240376025158,
+            "angularVelocity": 0.000002531044182467546,
+            "velocityX": -3.1682716080441704,
+            "velocityY": 2.067048223690569,
+            "timestamp": 1.2201390420615195
+        },
+        {
+            "x": 3.8162567615509033,
+            "y": 2.2371633052825928,
+            "heading": 0.32912425818941665,
+            "angularVelocity": 0.0000025310366317804206,
+            "velocityX": -3.1683178374712258,
+            "velocityY": 2.066977363735053,
+            "timestamp": 1.3072918307801995
+        },
+        {
+            "x": 3.558387727399027,
+            "y": 2.3518646644990793,
+            "heading": 0.32912440943088817,
+            "angularVelocity": 0.0000020272145178599712,
+            "velocityX": -3.4564319203160125,
+            "velocityY": 1.5374371746628512,
+            "timestamp": 1.3818973893726278
+        },
+        {
+            "x": 3.300516056227942,
+            "y": 2.466560095044955,
+            "heading": 0.3291245606718786,
+            "angularVelocity": 0.000002027208070545675,
+            "velocityX": -3.4564672664652627,
+            "velocityY": 1.5373577077876843,
+            "timestamp": 1.4565029479650562
+        },
+        {
+            "x": 3.052458337002912,
+            "y": 2.601171414843865,
+            "heading": 0.329124716605593,
+            "angularVelocity": 0.0000020901085294853097,
+            "velocityX": -3.3249227524744356,
+            "velocityY": 1.804306841723335,
+            "timestamp": 1.5311085065574845
+        },
+        {
+            "x": 2.829814908557799,
+            "y": 2.7746151638451657,
+            "heading": 0.3291249161701291,
+            "angularVelocity": 0.000002674928516490017,
+            "velocityX": -2.984273995740964,
+            "velocityY": 2.3248100044237634,
+            "timestamp": 1.6057140651499129
+        },
+        {
+            "x": 2.638606432152533,
+            "y": 2.982201542308141,
+            "heading": 0.3291253317978473,
+            "angularVelocity": 0.000005571002027267265,
+            "velocityX": -2.562925337103115,
+            "velocityY": 2.7824519027734036,
+            "timestamp": 1.6803196237423412
+        },
+        {
+            "x": 2.4840036736729245,
+            "y": 3.218316897029485,
+            "heading": 0.3291273340604081,
+            "angularVelocity": 0.000026837980957261232,
+            "velocityX": -2.0722686271167303,
+            "velocityY": 3.1648493647939433,
+            "timestamp": 1.7549251823347696
+        },
+        {
+            "x": 2.3733884856918945,
+            "y": 3.4659660107751304,
+            "heading": 0.2954274954004952,
+            "angularVelocity": -0.4517068070492687,
+            "velocityX": -1.4826668423638905,
+            "velocityY": 3.31944587532086,
+            "timestamp": 1.829530740927198
+        },
+        {
+            "x": 2.305023098579148,
+            "y": 3.7064756804968377,
+            "heading": 0.19598318894217118,
+            "angularVelocity": -1.3329342790875696,
+            "velocityX": -0.9163578210871337,
+            "velocityY": 3.2237500028063106,
+            "timestamp": 1.9041362995196263
+        },
+        {
+            "x": 2.272475779606133,
+            "y": 3.9200958593762563,
+            "heading": 0.053473340322528436,
+            "angularVelocity": -1.9101773555262378,
+            "velocityX": -0.43625863256144176,
+            "velocityY": 2.8633279196585013,
+            "timestamp": 1.9787418581120546
+        },
+        {
+            "x": 2.2677016800416907,
+            "y": 4.09662450199971,
+            "heading": -0.09830221598718161,
+            "angularVelocity": -2.034373298360552,
+            "velocityX": -0.06399120460344819,
+            "velocityY": 2.3661593848232187,
+            "timestamp": 2.053347416704483
+        },
+        {
+            "x": 2.2778239174156503,
+            "y": 4.229436050413757,
+            "heading": -0.23380831521279008,
+            "angularVelocity": -1.8163003103546322,
+            "velocityX": 0.13567671853055638,
+            "velocityY": 1.7801830174558415,
+            "timestamp": 2.1279529752969113
+        },
+        {
+            "x": 2.29107059750262,
+            "y": 4.316936633046431,
+            "heading": -0.33501872696397506,
+            "angularVelocity": -1.3566068488823915,
+            "velocityX": 0.1775562081015563,
+            "velocityY": 1.1728426712906341,
+            "timestamp": 2.2025585338893396
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.7369594715641588,
+            "velocityX": 0.11968816621507666,
+            "velocityY": 0.5772139203303202,
+            "timestamp": 2.277164092481768
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -8.092865962806098e-32,
+            "velocityX": 0,
+            "velocityY": 4.648171279746289e-33,
+            "timestamp": 2.3517696510741963
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W3.traj
new file mode 100644
index 00000000..15a1cd3f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W3.traj	
@@ -0,0 +1,274 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": -4.62162066936404e-33,
+            "velocityX": 3.0688151507515204e-31,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.623584376302727,
+            "y": 2.4297857949616017,
+            "heading": 0.033539307380936634,
+            "angularVelocity": 0.38483343876921716,
+            "velocityX": -0.6473186288895079,
+            "velocityY": -0.3122585684118786,
+            "timestamp": 0.08715278871867994
+        },
+        {
+            "x": 7.510615150776605,
+            "y": 2.375312721693156,
+            "heading": 0.0969954791736376,
+            "angularVelocity": 0.7281025968948721,
+            "velocityX": -1.2962204329545195,
+            "velocityY": -0.6250296068468768,
+            "timestamp": 0.1743055774373599
+        },
+        {
+            "x": 7.340880293995689,
+            "y": 2.2935273668964844,
+            "heading": 0.1838752961952568,
+            "angularVelocity": 0.9968678948651678,
+            "velocityX": -1.9475550843106406,
+            "velocityY": -0.9384135149210906,
+            "timestamp": 0.26145836615603985
+        },
+        {
+            "x": 7.114069036249808,
+            "y": 2.1844157133503646,
+            "heading": 0.27996244701307155,
+            "angularVelocity": 1.1025137833279668,
+            "velocityX": -2.6024555390648842,
+            "velocityY": -1.251958258023393,
+            "timestamp": 0.3486111548747198
+        },
+        {
+            "x": 6.831104073589339,
+            "y": 2.0490142604857513,
+            "heading": 0.32912237638568254,
+            "angularVelocity": 0.5640660510737534,
+            "velocityX": -3.246768885088134,
+            "velocityY": -1.5536101007814562,
+            "timestamp": 0.4357639435933997
+        },
+        {
+            "x": 6.534528280813305,
+            "y": 1.9049975621731787,
+            "heading": 0.3291224030694369,
+            "angularVelocity": 3.0617212321522066e-7,
+            "velocityX": -3.4029409401155197,
+            "velocityY": -1.6524623070575855,
+            "timestamp": 0.5229167323120797
+        },
+        {
+            "x": 6.237952262222522,
+            "y": 1.7609813288852085,
+            "heading": 0.3291224297529421,
+            "angularVelocity": 3.0616926449614015e-7,
+            "velocityX": -3.402943531137019,
+            "velocityY": -1.6524569713178037,
+            "timestamp": 0.6100695210307596
+        },
+        {
+            "x": 5.936787262454918,
+            "y": 1.6268259970502223,
+            "heading": 0.32912245667190637,
+            "angularVelocity": 3.0887094574020004e-7,
+            "velocityX": -3.4555979699024117,
+            "velocityY": -1.5393119808021931,
+            "timestamp": 0.6972223097494396
+        },
+        {
+            "x": 5.615527521313305,
+            "y": 1.5527301986214108,
+            "heading": 0.32912248658360005,
+            "angularVelocity": 3.4320982850713803e-7,
+            "velocityX": -3.6861670850098154,
+            "velocityY": -0.850182759704744,
+            "timestamp": 0.7843750984681195
+        },
+        {
+            "x": 5.286027461953728,
+            "y": 1.5414290964422628,
+            "heading": 0.32912252585289026,
+            "angularVelocity": 4.50579847209181e-7,
+            "velocityX": -3.780717337951944,
+            "velocityY": -0.12967000075725457,
+            "timestamp": 0.8715278871867994
+        },
+        {
+            "x": 4.960446284223111,
+            "y": 1.593339788205863,
+            "heading": 0.3291233220408656,
+            "angularVelocity": 0.000009135542155699871,
+            "velocityX": -3.7357516898462033,
+            "velocityY": 0.5956285797252272,
+            "timestamp": 0.9586806759054793
+        },
+        {
+            "x": 4.650797848549272,
+            "y": 1.7065466836899894,
+            "heading": 0.3291235959637348,
+            "angularVelocity": 0.0000031430189815062194,
+            "velocityX": -3.5529377800330906,
+            "velocityY": 1.2989474823295228,
+            "timestamp": 1.0458334646241594
+        },
+        {
+            "x": 4.368508202692925,
+            "y": 1.8768714467040708,
+            "heading": 0.32912381701495697,
+            "angularVelocity": 0.000002536364302217629,
+            "velocityX": -3.2390202310972316,
+            "velocityY": 1.9543237286860882,
+            "timestamp": 1.1329862533428394
+        },
+        {
+            "x": 4.092384496633659,
+            "y": 2.0570204638146983,
+            "heading": 0.3291240376025158,
+            "angularVelocity": 0.000002531044182467546,
+            "velocityX": -3.1682716080441704,
+            "velocityY": 2.067048223690569,
+            "timestamp": 1.2201390420615195
+        },
+        {
+            "x": 3.8162567615509033,
+            "y": 2.2371633052825928,
+            "heading": 0.32912425818941665,
+            "angularVelocity": 0.0000025310366317804206,
+            "velocityX": -3.1683178374712258,
+            "velocityY": 2.066977363735053,
+            "timestamp": 1.3072918307801995
+        },
+        {
+            "x": 3.558387727399027,
+            "y": 2.3518646644990793,
+            "heading": 0.32912440943088817,
+            "angularVelocity": 0.0000020272145178599712,
+            "velocityX": -3.4564319203160125,
+            "velocityY": 1.5374371746628512,
+            "timestamp": 1.3818973893726278
+        },
+        {
+            "x": 3.300516056227942,
+            "y": 2.466560095044955,
+            "heading": 0.3291245606718786,
+            "angularVelocity": 0.000002027208070545675,
+            "velocityX": -3.4564672664652627,
+            "velocityY": 1.5373577077876843,
+            "timestamp": 1.4565029479650562
+        },
+        {
+            "x": 3.052458337002912,
+            "y": 2.601171414843865,
+            "heading": 0.329124716605593,
+            "angularVelocity": 0.0000020901085294853097,
+            "velocityX": -3.3249227524744356,
+            "velocityY": 1.804306841723335,
+            "timestamp": 1.5311085065574845
+        },
+        {
+            "x": 2.829814908557799,
+            "y": 2.7746151638451657,
+            "heading": 0.3291249161701291,
+            "angularVelocity": 0.000002674928516490017,
+            "velocityX": -2.984273995740964,
+            "velocityY": 2.3248100044237634,
+            "timestamp": 1.6057140651499129
+        },
+        {
+            "x": 2.638606432152533,
+            "y": 2.982201542308141,
+            "heading": 0.3291253317978473,
+            "angularVelocity": 0.000005571002027267265,
+            "velocityX": -2.562925337103115,
+            "velocityY": 2.7824519027734036,
+            "timestamp": 1.6803196237423412
+        },
+        {
+            "x": 2.4840036736729245,
+            "y": 3.218316897029485,
+            "heading": 0.3291273340604081,
+            "angularVelocity": 0.000026837980957261232,
+            "velocityX": -2.0722686271167303,
+            "velocityY": 3.1648493647939433,
+            "timestamp": 1.7549251823347696
+        },
+        {
+            "x": 2.3733884856918945,
+            "y": 3.4659660107751304,
+            "heading": 0.2954274954004952,
+            "angularVelocity": -0.4517068070492687,
+            "velocityX": -1.4826668423638905,
+            "velocityY": 3.31944587532086,
+            "timestamp": 1.829530740927198
+        },
+        {
+            "x": 2.305023098579148,
+            "y": 3.7064756804968377,
+            "heading": 0.19598318894217118,
+            "angularVelocity": -1.3329342790875696,
+            "velocityX": -0.9163578210871337,
+            "velocityY": 3.2237500028063106,
+            "timestamp": 1.9041362995196263
+        },
+        {
+            "x": 2.272475779606133,
+            "y": 3.9200958593762563,
+            "heading": 0.053473340322528436,
+            "angularVelocity": -1.9101773555262378,
+            "velocityX": -0.43625863256144176,
+            "velocityY": 2.8633279196585013,
+            "timestamp": 1.9787418581120546
+        },
+        {
+            "x": 2.2677016800416907,
+            "y": 4.09662450199971,
+            "heading": -0.09830221598718161,
+            "angularVelocity": -2.034373298360552,
+            "velocityX": -0.06399120460344819,
+            "velocityY": 2.3661593848232187,
+            "timestamp": 2.053347416704483
+        },
+        {
+            "x": 2.2778239174156503,
+            "y": 4.229436050413757,
+            "heading": -0.23380831521279008,
+            "angularVelocity": -1.8163003103546322,
+            "velocityX": 0.13567671853055638,
+            "velocityY": 1.7801830174558415,
+            "timestamp": 2.1279529752969113
+        },
+        {
+            "x": 2.29107059750262,
+            "y": 4.316936633046431,
+            "heading": -0.33501872696397506,
+            "angularVelocity": -1.3566068488823915,
+            "velocityX": 0.1775562081015563,
+            "velocityY": 1.1728426712906341,
+            "timestamp": 2.2025585338893396
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.7369594715641588,
+            "velocityX": 0.11968816621507666,
+            "velocityY": 0.5772139203303202,
+            "timestamp": 2.277164092481768
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -8.092865962806098e-32,
+            "velocityX": 0,
+            "velocityY": 4.648171279746289e-33,
+            "timestamp": 2.3517696510741963
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.1.traj
index 85d8cdac..8293ba75 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.1.traj	
@@ -3,218 +3,218 @@
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 3.6158431875986385e-31,
-            "velocityX": 5.569083161704914e-32,
-            "velocityY": 2.1158895574058437e-37,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -1.1520076473353784e-31,
             "timestamp": 0
         },
         {
-            "x": 7.680000134363673,
-            "y": 0.8649552704871222,
-            "heading": 3.140000000380936,
-            "angularVelocity": 3.939689465162077e-9,
-            "velocityX": 0.0000013896119285392994,
-            "velocityY": 0.8062267267090596,
-            "timestamp": 0.09669149868712118
-        },
-        {
-            "x": 7.680000398315331,
-            "y": 1.0208658083667637,
-            "heading": 3.14000000095338,
-            "angularVelocity": 5.920286837104207e-9,
-            "velocityX": 0.0000027298328198052245,
-            "velocityY": 1.6124534214132247,
-            "timestamp": 0.19338299737424236
-        },
-        {
-            "x": 7.680000786842507,
-            "y": 1.2547316074497201,
-            "heading": 3.1400000020167984,
-            "angularVelocity": 1.0998009739093907e-8,
-            "velocityX": 0.000004018213888590017,
-            "velocityY": 2.4186800521075957,
-            "timestamp": 0.29007449606136354
-        },
-        {
-            "x": 7.680001294600068,
-            "y": 1.566552649168411,
-            "heading": 3.140000004611078,
-            "angularVelocity": 2.6830422686000607e-8,
-            "velocityX": 0.00000525131448771192,
-            "velocityY": 3.2249064907729323,
-            "timestamp": 0.3867659947484847
-        },
-        {
-            "x": 7.680001797129468,
-            "y": 1.9323308421441416,
-            "heading": 3.1400000046110796,
-            "angularVelocity": 2.2904858507221613e-14,
-            "velocityX": 0.000005197244743376452,
-            "velocityY": 3.782940568118395,
-            "timestamp": 0.4834574934356059
-        },
-        {
-            "x": 7.680002234677101,
-            "y": 2.2981090351200577,
-            "heading": 3.1400000046110854,
-            "angularVelocity": 7.187274756998544e-14,
-            "velocityX": 0.00000452519161992871,
-            "velocityY": 3.782940568120504,
-            "timestamp": 0.5801489921227271
-        },
-        {
-            "x": 7.680002600262594,
-            "y": 2.663887228096014,
-            "heading": 3.140000004611095,
-            "angularVelocity": 1.2305801027843294e-13,
-            "velocityX": 0.000003780946939519626,
-            "velocityY": 3.782940568121035,
-            "timestamp": 0.6768404908098482
-        },
-        {
-            "x": 7.680002888746174,
-            "y": 3.029665421071995,
-            "heading": 3.1400000046111063,
-            "angularVelocity": 1.4680337346800463e-13,
-            "velocityX": 0.000002983545883280996,
-            "velocityY": 3.782940568121397,
-            "timestamp": 0.7735319894969694
-        },
-        {
-            "x": 7.680003096449986,
-            "y": 3.395443614047991,
-            "heading": 3.1400000046111183,
-            "angularVelocity": 1.4951605959877598e-13,
-            "velocityX": 0.0000021481078702221435,
-            "velocityY": 3.782940568121633,
-            "timestamp": 0.8702234881840907
-        },
-        {
-            "x": 7.680003221115393,
-            "y": 3.7612218070239956,
-            "heading": 3.1400000046111316,
-            "angularVelocity": 1.5542579572657036e-13,
-            "velocityX": 0.0000012893106851068564,
-            "velocityY": 3.7829405681217785,
-            "timestamp": 0.9669149868712119
-        },
-        {
-            "x": 7.68000326213119,
-            "y": 4.127000000000005,
-            "heading": 3.1400000046111454,
-            "angularVelocity": 1.6157627281901116e-13,
-            "velocityX": 4.241922977706735e-7,
-            "velocityY": 3.782940568121862,
-            "timestamp": 1.0636064855583331
-        },
-        {
-            "x": 7.6800032195335275,
-            "y": 4.492778192976015,
-            "heading": 3.140000004611159,
-            "angularVelocity": 1.571537708778215e-13,
-            "velocityX": -4.4055220975717935e-7,
-            "velocityY": 3.7829405681218624,
-            "timestamp": 1.1602979842454544
-        },
-        {
-            "x": 7.680003093394937,
-            "y": 4.858556385952021,
-            "heading": 3.1400000046111707,
-            "angularVelocity": 1.4279777513481356e-13,
-            "velocityX": -0.00000130454658943142,
-            "velocityY": 3.7829405681217825,
-            "timestamp": 1.2569894829325756
-        },
-        {
-            "x": 7.680002884476087,
-            "y": 5.224334578928017,
-            "heading": 3.1400000046111822,
-            "angularVelocity": 1.3985874807595513e-13,
-            "velocityX": -0.0000021606740038572212,
-            "velocityY": 3.7829405681216364,
-            "timestamp": 1.3536809816196969
-        },
-        {
-            "x": 7.680002595166353,
-            "y": 5.590112771903999,
-            "heading": 3.140000004611193,
-            "angularVelocity": 1.3691025375631502e-13,
-            "velocityX": -0.0000029920901128748315,
-            "velocityY": 3.7829405681213992,
-            "timestamp": 1.450372480306818
-        },
-        {
-            "x": 7.680002229238247,
-            "y": 5.955890964879955,
-            "heading": 3.1400000046112018,
-            "angularVelocity": 1.1312968357611626e-13,
-            "velocityX": -0.000003784490284850132,
-            "velocityY": 3.782940568121032,
-            "timestamp": 1.5470639789939393
-        },
-        {
-            "x": 7.680001791901768,
-            "y": 6.321669157855871,
-            "heading": 3.1400000046112067,
-            "angularVelocity": 6.287792233292802e-14,
-            "velocityX": -0.000004523007841510769,
-            "velocityY": 3.7829405681204946,
-            "timestamp": 1.6437554776810606
-        },
-        {
-            "x": 7.680001290195951,
-            "y": 6.687447350831601,
-            "heading": 3.1400000046112084,
-            "angularVelocity": 2.135816498669109e-14,
-            "velocityX": -0.00000518872709755724,
-            "velocityY": 3.7829405681183994,
-            "timestamp": 1.7404469763681818
-        },
-        {
-            "x": 7.68000078407037,
-            "y": 6.999268392550285,
-            "heading": 3.1400000030761217,
-            "angularVelocity": -1.5876160780832585e-8,
-            "velocityX": -0.000005234436287123649,
-            "velocityY": 3.224906490772867,
-            "timestamp": 1.837138475055303
-        },
-        {
-            "x": 7.68000039686507,
-            "y": 7.233134191633239,
-            "heading": 3.1400000013788225,
-            "angularVelocity": -1.7553792269593633e-8,
-            "velocityX": -0.000004004542818722395,
-            "velocityY": 2.4186800521075664,
-            "timestamp": 1.9338299737424243
-        },
+            "x": 7.68,
+            "y": 0.8649552704871435,
+            "heading": -1.2072196888077206e-18,
+            "angularVelocity": -1.2485272283928241e-17,
+            "velocityX": -1.2865550388289577e-22,
+            "velocityY": 0.8062267267095138,
+            "timestamp": 0.09669149868710694
+        },
+        {
+            "x": 7.68,
+            "y": 1.0208658083668138,
+            "heading": -3.6224298175784965e-18,
+            "angularVelocity": -2.497851580797786e-17,
+            "velocityX": -3.7534290948711127e-22,
+            "velocityY": 1.6124534214139756,
+            "timestamp": 0.19338299737421388
+        },
+        {
+            "x": 7.68,
+            "y": 1.254731607449781,
+            "heading": -7.246204108405477e-18,
+            "angularVelocity": -3.7477692864160655e-17,
+            "velocityX": -8.581224837351263e-22,
+            "velocityY": 2.4186800521083573,
+            "timestamp": 0.29007449606132085
+        },
+        {
+            "x": 7.68,
+            "y": 1.5665526491683786,
+            "heading": -1.208276035533523e-17,
+            "angularVelocity": -5.0020491067317066e-17,
+            "velocityX": -2.0493771641846554e-21,
+            "velocityY": 3.2249064907727645,
+            "timestamp": 0.38676599474842777
+        },
+        {
+            "x": 7.68,
+            "y": 1.9323308421442915,
+            "heading": -4.9593132234185194e-18,
+            "angularVelocity": 7.367190734076976e-17,
+            "velocityX": 8.922575847291113e-21,
+            "velocityY": 3.782940568121389,
+            "timestamp": 0.4834574934355347
+        },
+        {
+            "x": 7.68,
+            "y": 2.298109035120243,
+            "heading": 4.533704301231546e-17,
+            "angularVelocity": 5.201735097557347e-16,
+            "velocityX": 4.097272417616201e-22,
+            "velocityY": 3.782940568121787,
+            "timestamp": 0.5801489921226416
+        },
+        {
+            "x": 7.68,
+            "y": 2.6638872280961943,
+            "heading": 2.5191507323029372e-17,
+            "angularVelocity": -2.083485721374208e-16,
+            "velocityX": 1.8682331175835897e-21,
+            "velocityY": 3.782940568121787,
+            "timestamp": 0.6768404908097485
+        },
+        {
+            "x": 7.68,
+            "y": 3.0296654210721456,
+            "heading": 5.2668856754382746e-17,
+            "angularVelocity": 2.8417544256148633e-16,
+            "velocityX": 2.5170076181953607e-21,
+            "velocityY": 3.782940568121787,
+            "timestamp": 0.7735319894968554
+        },
+        {
+            "x": 7.68,
+            "y": 3.395443614048097,
+            "heading": 4.929611386988728e-17,
+            "angularVelocity": -3.48814831737134e-17,
+            "velocityX": 3.0934195236040766e-21,
+            "velocityY": 3.782940568121787,
+            "timestamp": 0.8702234881839623
+        },
+        {
+            "x": 7.68,
+            "y": 3.7612218070240484,
+            "heading": 4.8005098046144365e-17,
+            "angularVelocity": -1.3351906227498234e-17,
+            "velocityX": -1.4483318353601114e-20,
+            "velocityY": 3.782940568121787,
+            "timestamp": 0.9669149868710692
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.8136822799879804e-17,
+            "angularVelocity": -1.0205938867695289e-16,
+            "velocityX": -1.0465805642283078e-22,
+            "velocityY": 3.782940568121787,
+            "timestamp": 1.0636064855581762
+        },
+        {
+            "x": 7.68,
+            "y": 4.492778192975952,
+            "heading": 4.310486593468924e-17,
+            "angularVelocity": 5.1380350929111975e-17,
+            "velocityX": 2.042774398860212e-21,
+            "velocityY": 3.782940568121787,
+            "timestamp": 1.1602979842452832
+        },
+        {
+            "x": 7.68,
+            "y": 4.8585563859519025,
+            "heading": 1.5339712420766394e-17,
+            "angularVelocity": -2.8715196155735124e-16,
+            "velocityX": 3.895366641644577e-22,
+            "velocityY": 3.782940568121787,
+            "timestamp": 1.2569894829323902
+        },
         {
-            "x": 7.680000133859054,
-            "y": 7.389044729512878,
-            "heading": 3.1400000003266944,
-            "angularVelocity": -1.08813147296025e-8,
-            "velocityX": -0.0000027200528426415456,
-            "velocityY": 1.6124534214132105,
-            "timestamp": 2.0305214724295455
+            "x": 7.68,
+            "y": 5.224334578927854,
+            "heading": -2.710858238227145e-17,
+            "angularVelocity": -4.390075175101735e-16,
+            "velocityX": -3.863371094269421e-21,
+            "velocityY": 3.782940568121787,
+            "timestamp": 1.3536809816194972
+        },
+        {
+            "x": 7.68,
+            "y": 5.590112771903805,
+            "heading": -3.378098304971619e-17,
+            "angularVelocity": -6.900710774001486e-17,
+            "velocityX": -4.042329982282679e-22,
+            "velocityY": 3.782940568121787,
+            "timestamp": 1.4503724803066043
+        },
+        {
+            "x": 7.68,
+            "y": 5.955890964879757,
+            "heading": -3.941496183315801e-17,
+            "angularVelocity": -5.826757119208287e-17,
+            "velocityX": 2.6937052538433574e-21,
+            "velocityY": 3.782940568121787,
+            "timestamp": 1.5470639789937113
+        },
+        {
+            "x": 7.68,
+            "y": 6.321669157855708,
+            "heading": -1.9923808577496458e-17,
+            "angularVelocity": 2.0158083720198419e-16,
+            "velocityX": -4.499746264941967e-21,
+            "velocityY": 3.782940568121787,
+            "timestamp": 1.6437554776808183
+        },
+        {
+            "x": 7.68,
+            "y": 6.68744735083162,
+            "heading": 1.1959451428437676e-17,
+            "angularVelocity": 3.297421225035248e-16,
+            "velocityX": 5.431014750114918e-21,
+            "velocityY": 3.782940568121389,
+            "timestamp": 1.7404469763679253
+        },
+        {
+            "x": 7.68,
+            "y": 6.999268392550218,
+            "heading": 7.18685516064291e-18,
+            "angularVelocity": -4.935900605111643e-17,
+            "velocityX": -2.4046856363551417e-22,
+            "velocityY": 3.2249064907727645,
+            "timestamp": 1.8371384750550324
+        },
+        {
+            "x": 7.68,
+            "y": 7.2331341916331855,
+            "heading": 3.596188330325507e-18,
+            "angularVelocity": -3.713528985888002e-17,
+            "velocityX": -1.8035546521541034e-22,
+            "velocityY": 2.4186800521083573,
+            "timestamp": 1.9338299737421394
+        },
+        {
+            "x": 7.68,
+            "y": 7.3890447295128565,
+            "heading": 1.199348678010309e-18,
+            "angularVelocity": -2.4788525206448663e-17,
+            "velocityX": -1.2023814854884031e-22,
+            "velocityY": 1.6124534214139756,
+            "timestamp": 2.030521472429246
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": -3.378739660348149e-9,
-            "velocityX": -0.000001384393079062506,
-            "velocityY": 0.806226726709054,
-            "timestamp": 2.1272129711166667
+            "heading": 0,
+            "angularVelocity": -1.2403868947555692e-17,
+            "velocityX": -6.011942579686287e-23,
+            "velocityY": 0.8062267267095138,
+            "timestamp": 2.127212971116353
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 1.4001703039309738e-31,
-            "velocityX": 0,
-            "velocityY": 2.3509592524815357e-38,
-            "timestamp": 2.223904469803788
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -2.719452927302828e-33,
+            "velocityY": 0,
+            "timestamp": 2.2239044698034602
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.traj
index 85d8cdac..8293ba75 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.traj	
@@ -3,218 +3,218 @@
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 3.6158431875986385e-31,
-            "velocityX": 5.569083161704914e-32,
-            "velocityY": 2.1158895574058437e-37,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -1.1520076473353784e-31,
             "timestamp": 0
         },
         {
-            "x": 7.680000134363673,
-            "y": 0.8649552704871222,
-            "heading": 3.140000000380936,
-            "angularVelocity": 3.939689465162077e-9,
-            "velocityX": 0.0000013896119285392994,
-            "velocityY": 0.8062267267090596,
-            "timestamp": 0.09669149868712118
-        },
-        {
-            "x": 7.680000398315331,
-            "y": 1.0208658083667637,
-            "heading": 3.14000000095338,
-            "angularVelocity": 5.920286837104207e-9,
-            "velocityX": 0.0000027298328198052245,
-            "velocityY": 1.6124534214132247,
-            "timestamp": 0.19338299737424236
-        },
-        {
-            "x": 7.680000786842507,
-            "y": 1.2547316074497201,
-            "heading": 3.1400000020167984,
-            "angularVelocity": 1.0998009739093907e-8,
-            "velocityX": 0.000004018213888590017,
-            "velocityY": 2.4186800521075957,
-            "timestamp": 0.29007449606136354
-        },
-        {
-            "x": 7.680001294600068,
-            "y": 1.566552649168411,
-            "heading": 3.140000004611078,
-            "angularVelocity": 2.6830422686000607e-8,
-            "velocityX": 0.00000525131448771192,
-            "velocityY": 3.2249064907729323,
-            "timestamp": 0.3867659947484847
-        },
-        {
-            "x": 7.680001797129468,
-            "y": 1.9323308421441416,
-            "heading": 3.1400000046110796,
-            "angularVelocity": 2.2904858507221613e-14,
-            "velocityX": 0.000005197244743376452,
-            "velocityY": 3.782940568118395,
-            "timestamp": 0.4834574934356059
-        },
-        {
-            "x": 7.680002234677101,
-            "y": 2.2981090351200577,
-            "heading": 3.1400000046110854,
-            "angularVelocity": 7.187274756998544e-14,
-            "velocityX": 0.00000452519161992871,
-            "velocityY": 3.782940568120504,
-            "timestamp": 0.5801489921227271
-        },
-        {
-            "x": 7.680002600262594,
-            "y": 2.663887228096014,
-            "heading": 3.140000004611095,
-            "angularVelocity": 1.2305801027843294e-13,
-            "velocityX": 0.000003780946939519626,
-            "velocityY": 3.782940568121035,
-            "timestamp": 0.6768404908098482
-        },
-        {
-            "x": 7.680002888746174,
-            "y": 3.029665421071995,
-            "heading": 3.1400000046111063,
-            "angularVelocity": 1.4680337346800463e-13,
-            "velocityX": 0.000002983545883280996,
-            "velocityY": 3.782940568121397,
-            "timestamp": 0.7735319894969694
-        },
-        {
-            "x": 7.680003096449986,
-            "y": 3.395443614047991,
-            "heading": 3.1400000046111183,
-            "angularVelocity": 1.4951605959877598e-13,
-            "velocityX": 0.0000021481078702221435,
-            "velocityY": 3.782940568121633,
-            "timestamp": 0.8702234881840907
-        },
-        {
-            "x": 7.680003221115393,
-            "y": 3.7612218070239956,
-            "heading": 3.1400000046111316,
-            "angularVelocity": 1.5542579572657036e-13,
-            "velocityX": 0.0000012893106851068564,
-            "velocityY": 3.7829405681217785,
-            "timestamp": 0.9669149868712119
-        },
-        {
-            "x": 7.68000326213119,
-            "y": 4.127000000000005,
-            "heading": 3.1400000046111454,
-            "angularVelocity": 1.6157627281901116e-13,
-            "velocityX": 4.241922977706735e-7,
-            "velocityY": 3.782940568121862,
-            "timestamp": 1.0636064855583331
-        },
-        {
-            "x": 7.6800032195335275,
-            "y": 4.492778192976015,
-            "heading": 3.140000004611159,
-            "angularVelocity": 1.571537708778215e-13,
-            "velocityX": -4.4055220975717935e-7,
-            "velocityY": 3.7829405681218624,
-            "timestamp": 1.1602979842454544
-        },
-        {
-            "x": 7.680003093394937,
-            "y": 4.858556385952021,
-            "heading": 3.1400000046111707,
-            "angularVelocity": 1.4279777513481356e-13,
-            "velocityX": -0.00000130454658943142,
-            "velocityY": 3.7829405681217825,
-            "timestamp": 1.2569894829325756
-        },
-        {
-            "x": 7.680002884476087,
-            "y": 5.224334578928017,
-            "heading": 3.1400000046111822,
-            "angularVelocity": 1.3985874807595513e-13,
-            "velocityX": -0.0000021606740038572212,
-            "velocityY": 3.7829405681216364,
-            "timestamp": 1.3536809816196969
-        },
-        {
-            "x": 7.680002595166353,
-            "y": 5.590112771903999,
-            "heading": 3.140000004611193,
-            "angularVelocity": 1.3691025375631502e-13,
-            "velocityX": -0.0000029920901128748315,
-            "velocityY": 3.7829405681213992,
-            "timestamp": 1.450372480306818
-        },
-        {
-            "x": 7.680002229238247,
-            "y": 5.955890964879955,
-            "heading": 3.1400000046112018,
-            "angularVelocity": 1.1312968357611626e-13,
-            "velocityX": -0.000003784490284850132,
-            "velocityY": 3.782940568121032,
-            "timestamp": 1.5470639789939393
-        },
-        {
-            "x": 7.680001791901768,
-            "y": 6.321669157855871,
-            "heading": 3.1400000046112067,
-            "angularVelocity": 6.287792233292802e-14,
-            "velocityX": -0.000004523007841510769,
-            "velocityY": 3.7829405681204946,
-            "timestamp": 1.6437554776810606
-        },
-        {
-            "x": 7.680001290195951,
-            "y": 6.687447350831601,
-            "heading": 3.1400000046112084,
-            "angularVelocity": 2.135816498669109e-14,
-            "velocityX": -0.00000518872709755724,
-            "velocityY": 3.7829405681183994,
-            "timestamp": 1.7404469763681818
-        },
-        {
-            "x": 7.68000078407037,
-            "y": 6.999268392550285,
-            "heading": 3.1400000030761217,
-            "angularVelocity": -1.5876160780832585e-8,
-            "velocityX": -0.000005234436287123649,
-            "velocityY": 3.224906490772867,
-            "timestamp": 1.837138475055303
-        },
-        {
-            "x": 7.68000039686507,
-            "y": 7.233134191633239,
-            "heading": 3.1400000013788225,
-            "angularVelocity": -1.7553792269593633e-8,
-            "velocityX": -0.000004004542818722395,
-            "velocityY": 2.4186800521075664,
-            "timestamp": 1.9338299737424243
-        },
+            "x": 7.68,
+            "y": 0.8649552704871435,
+            "heading": -1.2072196888077206e-18,
+            "angularVelocity": -1.2485272283928241e-17,
+            "velocityX": -1.2865550388289577e-22,
+            "velocityY": 0.8062267267095138,
+            "timestamp": 0.09669149868710694
+        },
+        {
+            "x": 7.68,
+            "y": 1.0208658083668138,
+            "heading": -3.6224298175784965e-18,
+            "angularVelocity": -2.497851580797786e-17,
+            "velocityX": -3.7534290948711127e-22,
+            "velocityY": 1.6124534214139756,
+            "timestamp": 0.19338299737421388
+        },
+        {
+            "x": 7.68,
+            "y": 1.254731607449781,
+            "heading": -7.246204108405477e-18,
+            "angularVelocity": -3.7477692864160655e-17,
+            "velocityX": -8.581224837351263e-22,
+            "velocityY": 2.4186800521083573,
+            "timestamp": 0.29007449606132085
+        },
+        {
+            "x": 7.68,
+            "y": 1.5665526491683786,
+            "heading": -1.208276035533523e-17,
+            "angularVelocity": -5.0020491067317066e-17,
+            "velocityX": -2.0493771641846554e-21,
+            "velocityY": 3.2249064907727645,
+            "timestamp": 0.38676599474842777
+        },
+        {
+            "x": 7.68,
+            "y": 1.9323308421442915,
+            "heading": -4.9593132234185194e-18,
+            "angularVelocity": 7.367190734076976e-17,
+            "velocityX": 8.922575847291113e-21,
+            "velocityY": 3.782940568121389,
+            "timestamp": 0.4834574934355347
+        },
+        {
+            "x": 7.68,
+            "y": 2.298109035120243,
+            "heading": 4.533704301231546e-17,
+            "angularVelocity": 5.201735097557347e-16,
+            "velocityX": 4.097272417616201e-22,
+            "velocityY": 3.782940568121787,
+            "timestamp": 0.5801489921226416
+        },
+        {
+            "x": 7.68,
+            "y": 2.6638872280961943,
+            "heading": 2.5191507323029372e-17,
+            "angularVelocity": -2.083485721374208e-16,
+            "velocityX": 1.8682331175835897e-21,
+            "velocityY": 3.782940568121787,
+            "timestamp": 0.6768404908097485
+        },
+        {
+            "x": 7.68,
+            "y": 3.0296654210721456,
+            "heading": 5.2668856754382746e-17,
+            "angularVelocity": 2.8417544256148633e-16,
+            "velocityX": 2.5170076181953607e-21,
+            "velocityY": 3.782940568121787,
+            "timestamp": 0.7735319894968554
+        },
+        {
+            "x": 7.68,
+            "y": 3.395443614048097,
+            "heading": 4.929611386988728e-17,
+            "angularVelocity": -3.48814831737134e-17,
+            "velocityX": 3.0934195236040766e-21,
+            "velocityY": 3.782940568121787,
+            "timestamp": 0.8702234881839623
+        },
+        {
+            "x": 7.68,
+            "y": 3.7612218070240484,
+            "heading": 4.8005098046144365e-17,
+            "angularVelocity": -1.3351906227498234e-17,
+            "velocityX": -1.4483318353601114e-20,
+            "velocityY": 3.782940568121787,
+            "timestamp": 0.9669149868710692
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.8136822799879804e-17,
+            "angularVelocity": -1.0205938867695289e-16,
+            "velocityX": -1.0465805642283078e-22,
+            "velocityY": 3.782940568121787,
+            "timestamp": 1.0636064855581762
+        },
+        {
+            "x": 7.68,
+            "y": 4.492778192975952,
+            "heading": 4.310486593468924e-17,
+            "angularVelocity": 5.1380350929111975e-17,
+            "velocityX": 2.042774398860212e-21,
+            "velocityY": 3.782940568121787,
+            "timestamp": 1.1602979842452832
+        },
+        {
+            "x": 7.68,
+            "y": 4.8585563859519025,
+            "heading": 1.5339712420766394e-17,
+            "angularVelocity": -2.8715196155735124e-16,
+            "velocityX": 3.895366641644577e-22,
+            "velocityY": 3.782940568121787,
+            "timestamp": 1.2569894829323902
+        },
         {
-            "x": 7.680000133859054,
-            "y": 7.389044729512878,
-            "heading": 3.1400000003266944,
-            "angularVelocity": -1.08813147296025e-8,
-            "velocityX": -0.0000027200528426415456,
-            "velocityY": 1.6124534214132105,
-            "timestamp": 2.0305214724295455
+            "x": 7.68,
+            "y": 5.224334578927854,
+            "heading": -2.710858238227145e-17,
+            "angularVelocity": -4.390075175101735e-16,
+            "velocityX": -3.863371094269421e-21,
+            "velocityY": 3.782940568121787,
+            "timestamp": 1.3536809816194972
+        },
+        {
+            "x": 7.68,
+            "y": 5.590112771903805,
+            "heading": -3.378098304971619e-17,
+            "angularVelocity": -6.900710774001486e-17,
+            "velocityX": -4.042329982282679e-22,
+            "velocityY": 3.782940568121787,
+            "timestamp": 1.4503724803066043
+        },
+        {
+            "x": 7.68,
+            "y": 5.955890964879757,
+            "heading": -3.941496183315801e-17,
+            "angularVelocity": -5.826757119208287e-17,
+            "velocityX": 2.6937052538433574e-21,
+            "velocityY": 3.782940568121787,
+            "timestamp": 1.5470639789937113
+        },
+        {
+            "x": 7.68,
+            "y": 6.321669157855708,
+            "heading": -1.9923808577496458e-17,
+            "angularVelocity": 2.0158083720198419e-16,
+            "velocityX": -4.499746264941967e-21,
+            "velocityY": 3.782940568121787,
+            "timestamp": 1.6437554776808183
+        },
+        {
+            "x": 7.68,
+            "y": 6.68744735083162,
+            "heading": 1.1959451428437676e-17,
+            "angularVelocity": 3.297421225035248e-16,
+            "velocityX": 5.431014750114918e-21,
+            "velocityY": 3.782940568121389,
+            "timestamp": 1.7404469763679253
+        },
+        {
+            "x": 7.68,
+            "y": 6.999268392550218,
+            "heading": 7.18685516064291e-18,
+            "angularVelocity": -4.935900605111643e-17,
+            "velocityX": -2.4046856363551417e-22,
+            "velocityY": 3.2249064907727645,
+            "timestamp": 1.8371384750550324
+        },
+        {
+            "x": 7.68,
+            "y": 7.2331341916331855,
+            "heading": 3.596188330325507e-18,
+            "angularVelocity": -3.713528985888002e-17,
+            "velocityX": -1.8035546521541034e-22,
+            "velocityY": 2.4186800521083573,
+            "timestamp": 1.9338299737421394
+        },
+        {
+            "x": 7.68,
+            "y": 7.3890447295128565,
+            "heading": 1.199348678010309e-18,
+            "angularVelocity": -2.4788525206448663e-17,
+            "velocityX": -1.2023814854884031e-22,
+            "velocityY": 1.6124534214139756,
+            "timestamp": 2.030521472429246
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": -3.378739660348149e-9,
-            "velocityX": -0.000001384393079062506,
-            "velocityY": 0.806226726709054,
-            "timestamp": 2.1272129711166667
+            "heading": 0,
+            "angularVelocity": -1.2403868947555692e-17,
+            "velocityX": -6.011942579686287e-23,
+            "velocityY": 0.8062267267095138,
+            "timestamp": 2.127212971116353
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 3.14,
-            "angularVelocity": 1.4001703039309738e-31,
-            "velocityX": 0,
-            "velocityY": 2.3509592524815357e-38,
-            "timestamp": 2.223904469803788
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -2.719452927302828e-33,
+            "velocityY": 0,
+            "timestamp": 2.2239044698034602
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.1.traj
index de1957d8..f16ed7e1 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.1.traj	
@@ -3,173 +3,173 @@
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": -9.959658814138394e-32,
-            "velocityX": -4.727301624016019e-32,
-            "velocityY": 3.595241492576121e-38,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.5195168592304314e-32,
             "timestamp": 0
         },
         {
-            "x": 7.680000100608845,
-            "y": 0.8688441539517371,
-            "heading": 3.13999998007937,
-            "angularVelocity": -2.0106831809452372e-7,
-            "velocityX": 0.0000010154926233848571,
-            "velocityY": 0.826091745427235,
-            "timestamp": 0.09907392781101573
-        },
-        {
-            "x": 7.680000296760758,
-            "y": 1.0325324587418605,
-            "heading": 3.139999940084766,
-            "angularVelocity": -4.036843910631592e-7,
-            "velocityX": 0.0000019798539067481827,
-            "velocityY": 1.6521834594299558,
-            "timestamp": 0.19814785562203147
-        },
-        {
-            "x": 7.680000583126213,
-            "y": 1.2780649081436684,
-            "heading": 3.1399998804112537,
-            "angularVelocity": -6.023128542087543e-7,
-            "velocityX": 0.0000028904217813218476,
-            "velocityY": 2.47827511058363,
-            "timestamp": 0.29722178343304717
-        },
-        {
-            "x": 7.680000954004463,
-            "y": 1.6054414834770696,
-            "heading": 3.13999980228898,
-            "angularVelocity": -7.885247914488142e-7,
-            "velocityX": 0.0000037434492396662855,
-            "velocityY": 3.3043665731903316,
-            "timestamp": 0.39629571124406293
-        },
-        {
-            "x": 7.680001281447094,
-            "y": 1.9802322649264825,
-            "heading": 3.1399998022889792,
-            "angularVelocity": -1.0474954246602775e-14,
-            "velocityX": 0.0000033050332881388687,
-            "velocityY": 3.7829405750856777,
-            "timestamp": 0.4953696390550787
-        },
-        {
-            "x": 7.6800015408657325,
-            "y": 2.355023046375991,
-            "heading": 3.13999980228898,
-            "angularVelocity": 7.833153823506449e-15,
-            "velocityX": 0.0000026184347759635413,
-            "velocityY": 3.7829405750867062,
-            "timestamp": 0.5944435668660945
-        },
-        {
-            "x": 7.680001725168594,
-            "y": 2.729813827825533,
-            "heading": 3.139999802288987,
-            "angularVelocity": 7.465414071871633e-14,
-            "velocityX": 0.0000018602557564557753,
-            "velocityY": 3.7829405750870757,
-            "timestamp": 0.6935174946771102
-        },
-        {
-            "x": 7.680001829195369,
-            "y": 3.104604609275094,
-            "heading": 3.1399998022889952,
-            "angularVelocity": 9.203271146236807e-14,
-            "velocityX": 0.0000010499913044241082,
-            "velocityY": 3.7829405750872978,
-            "timestamp": 0.792591422488126
-        },
-        {
-            "x": 7.680001849352207,
-            "y": 3.479395390724662,
-            "heading": 3.1399998022890023,
-            "angularVelocity": 7.71401741801888e-14,
-            "velocityX": 2.034524499813149e-7,
-            "velocityY": 3.7829405750873777,
-            "timestamp": 0.8916653502991417
-        },
-        {
-            "x": 7.6800017836349985,
-            "y": 3.8541861721742245,
-            "heading": 3.1399998022890094,
-            "angularVelocity": 7.688638756481334e-14,
-            "velocityX": -6.633148330797741e-7,
-            "velocityY": 3.7829405750873186,
-            "timestamp": 0.9907392781101575
-        },
-        {
-            "x": 7.68000163167861,
-            "y": 4.22897695362377,
-            "heading": 3.1399998022890157,
-            "angularVelocity": 6.981254883852486e-14,
-            "velocityX": -0.000001533767606062718,
-            "velocityY": 3.7829405750871206,
-            "timestamp": 1.0898132059211731
-        },
-        {
-            "x": 7.680001393541454,
-            "y": 4.603767735073283,
-            "heading": 3.139999802289022,
-            "angularVelocity": 6.191049012077391e-14,
-            "velocityX": -0.00000240363076954954,
-            "velocityY": 3.782940575086774,
-            "timestamp": 1.188887133732189
-        },
-        {
-            "x": 7.680001069388574,
-            "y": 4.978558516522716,
-            "heading": 3.139999802289024,
-            "angularVelocity": 2.5336752196891655e-14,
-            "velocityX": -0.000003271828396587149,
-            "velocityY": 3.782940575085891,
-            "timestamp": 1.2879610615432047
-        },
-        {
-            "x": 7.680000659196884,
-            "y": 5.305935091856245,
-            "heading": 3.139999885175708,
-            "angularVelocity": 8.366142293360243e-7,
-            "velocityX": -0.000004140258200407122,
-            "velocityY": 3.304366573191614,
-            "timestamp": 1.3870349893542204
-        },
-        {
-            "x": 7.6800003382888615,
-            "y": 5.5514675412581065,
-            "heading": 3.1399999457053744,
-            "angularVelocity": 6.10954418213692e-7,
-            "velocityX": -0.0000032390761240661895,
-            "velocityY": 2.478275110584155,
-            "timestamp": 1.4861089171652362
-        },
+            "x": 7.68,
+            "y": 0.8688441539517631,
+            "heading": 1.4487899486085006e-18,
+            "angularVelocity": 1.4623322004407643e-17,
+            "velocityX": 2.341988908112997e-19,
+            "velocityY": 0.8260917454276263,
+            "timestamp": 0.09907392781100396
+        },
+        {
+            "x": 7.68,
+            "y": 1.03253245874193,
+            "heading": 4.3483011920968684e-18,
+            "angularVelocity": 2.926613800008521e-17,
+            "velocityX": 4.684469740721679e-19,
+            "velocityY": 1.6521834594306495,
+            "timestamp": 0.19814785562200793
+        },
+        {
+            "x": 7.68,
+            "y": 1.2780649081437847,
+            "heading": 8.701494230539972e-18,
+            "angularVelocity": 4.393883572228609e-17,
+            "velocityX": 7.027934476495029e-19,
+            "velocityY": 2.4782751105844794,
+            "timestamp": 0.29722178343301187
+        },
+        {
+            "x": 7.68,
+            "y": 1.6054414834771922,
+            "heading": 1.4516368843485863e-17,
+            "angularVelocity": 5.869227900187472e-17,
+            "velocityX": 9.37435089599571e-19,
+            "velocityY": 3.304366573190877,
+            "timestamp": 0.39629571124401586
+        },
+        {
+            "x": 7.68,
+            "y": 1.9802322649266864,
+            "heading": 6.647154079192576e-18,
+            "angularVelocity": -7.942770553424421e-17,
+            "velocityX": 1.0709424285811041e-18,
+            "velocityY": 3.782940575087071,
+            "timestamp": 0.49536963905501985
+        },
+        {
+            "x": 7.68,
+            "y": 2.3550230463762047,
+            "heading": -4.11606275908414e-17,
+            "angularVelocity": -4.825465460619937e-16,
+            "velocityX": 1.073083844244948e-18,
+            "velocityY": 3.7829405750873115,
+            "timestamp": 0.5944435668660238
+        },
+        {
+            "x": 7.68,
+            "y": 2.729813827825723,
+            "heading": -7.635066611173202e-17,
+            "angularVelocity": -3.5518969822232415e-16,
+            "velocityX": 1.0738946259920485e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.6935174946770278
+        },
         {
-            "x": 7.6800001156321125,
-            "y": 5.715155846048254,
-            "heading": 3.139999983279866,
-            "angularVelocity": 3.792570390671995e-7,
-            "velocityX": -0.000002247379691011521,
-            "velocityY": 1.6521834594301872,
-            "timestamp": 1.585182844976252
+            "x": 7.68,
+            "y": 3.1046046092752415,
+            "heading": -5.864574302033697e-17,
+            "angularVelocity": 1.787041604453991e-16,
+            "velocityX": 1.0691415340399175e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.7925914224880318
+        },
+        {
+            "x": 7.68,
+            "y": 3.4793953907247595,
+            "heading": -2.6391173603704442e-17,
+            "angularVelocity": 3.255606205313886e-16,
+            "velocityX": 1.0732289061667476e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.8916653502990358
+        },
+        {
+            "x": 7.68,
+            "y": 3.854186172174278,
+            "heading": -6.511485011951871e-17,
+            "angularVelocity": -3.9085637736788184e-16,
+            "velocityX": 1.0716697150075795e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.9907392781100398
+        },
+        {
+            "x": 7.68,
+            "y": 4.228976953623796,
+            "heading": -6.241329479526944e-17,
+            "angularVelocity": 2.7268075304360027e-17,
+            "velocityX": 1.0686111075844087e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 1.0898132059210437
+        },
+        {
+            "x": 7.68,
+            "y": 4.603767735073314,
+            "heading": -2.2737833863559818e-17,
+            "angularVelocity": 4.004631875225205e-16,
+            "velocityX": 1.0710197916722359e-18,
+            "velocityY": 3.7829405750873115,
+            "timestamp": 1.1888871337320477
+        },
+        {
+            "x": 7.68,
+            "y": 4.978558516522808,
+            "heading": -1.448944579275074e-17,
+            "angularVelocity": 8.325488100707829e-17,
+            "velocityX": 1.0714504382145178e-18,
+            "velocityY": 3.782940575087071,
+            "timestamp": 1.2879610615430517
+        },
+        {
+            "x": 7.68,
+            "y": 5.305935091856216,
+            "heading": -8.687037170545135e-18,
+            "angularVelocity": 5.856645386228716e-17,
+            "velocityX": 9.36753995288457e-19,
+            "velocityY": 3.304366573190877,
+            "timestamp": 1.3870349893540557
+        },
+        {
+            "x": 7.68,
+            "y": 5.55146754125807,
+            "heading": -4.341905072762387e-18,
+            "angularVelocity": 4.385747283685971e-17,
+            "velocityX": 7.025655481927048e-19,
+            "velocityY": 2.4782751105844794,
+            "timestamp": 1.4861089171650597
+        },
+        {
+            "x": 7.68,
+            "y": 5.715155846048237,
+            "heading": -1.4469357825353783e-18,
+            "angularVelocity": 2.9220293918225814e-17,
+            "velocityX": 4.683770471216647e-19,
+            "velocityY": 1.6521834594306495,
+            "timestamp": 1.5851828449760637
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 1.687641935617777e-7,
-            "velocityX": -0.0000011671295149488337,
-            "velocityY": 0.8260917454273095,
-            "timestamp": 1.6842567727872677
+            "heading": 0,
+            "angularVelocity": 1.460460702963924e-17,
+            "velocityX": 2.341885280588165e-19,
+            "velocityY": 0.8260917454276263,
+            "timestamp": 1.6842567727870676
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": -8.926921159411167e-32,
-            "velocityX": 0,
-            "velocityY": 7.102031651059941e-38,
-            "timestamp": 1.7833307005982835
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 3.1812192470207854e-34,
+            "velocityY": 0,
+            "timestamp": 1.7833307005980716
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.traj
index de1957d8..f16ed7e1 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.traj	
@@ -3,173 +3,173 @@
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": -9.959658814138394e-32,
-            "velocityX": -4.727301624016019e-32,
-            "velocityY": 3.595241492576121e-38,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.5195168592304314e-32,
             "timestamp": 0
         },
         {
-            "x": 7.680000100608845,
-            "y": 0.8688441539517371,
-            "heading": 3.13999998007937,
-            "angularVelocity": -2.0106831809452372e-7,
-            "velocityX": 0.0000010154926233848571,
-            "velocityY": 0.826091745427235,
-            "timestamp": 0.09907392781101573
-        },
-        {
-            "x": 7.680000296760758,
-            "y": 1.0325324587418605,
-            "heading": 3.139999940084766,
-            "angularVelocity": -4.036843910631592e-7,
-            "velocityX": 0.0000019798539067481827,
-            "velocityY": 1.6521834594299558,
-            "timestamp": 0.19814785562203147
-        },
-        {
-            "x": 7.680000583126213,
-            "y": 1.2780649081436684,
-            "heading": 3.1399998804112537,
-            "angularVelocity": -6.023128542087543e-7,
-            "velocityX": 0.0000028904217813218476,
-            "velocityY": 2.47827511058363,
-            "timestamp": 0.29722178343304717
-        },
-        {
-            "x": 7.680000954004463,
-            "y": 1.6054414834770696,
-            "heading": 3.13999980228898,
-            "angularVelocity": -7.885247914488142e-7,
-            "velocityX": 0.0000037434492396662855,
-            "velocityY": 3.3043665731903316,
-            "timestamp": 0.39629571124406293
-        },
-        {
-            "x": 7.680001281447094,
-            "y": 1.9802322649264825,
-            "heading": 3.1399998022889792,
-            "angularVelocity": -1.0474954246602775e-14,
-            "velocityX": 0.0000033050332881388687,
-            "velocityY": 3.7829405750856777,
-            "timestamp": 0.4953696390550787
-        },
-        {
-            "x": 7.6800015408657325,
-            "y": 2.355023046375991,
-            "heading": 3.13999980228898,
-            "angularVelocity": 7.833153823506449e-15,
-            "velocityX": 0.0000026184347759635413,
-            "velocityY": 3.7829405750867062,
-            "timestamp": 0.5944435668660945
-        },
-        {
-            "x": 7.680001725168594,
-            "y": 2.729813827825533,
-            "heading": 3.139999802288987,
-            "angularVelocity": 7.465414071871633e-14,
-            "velocityX": 0.0000018602557564557753,
-            "velocityY": 3.7829405750870757,
-            "timestamp": 0.6935174946771102
-        },
-        {
-            "x": 7.680001829195369,
-            "y": 3.104604609275094,
-            "heading": 3.1399998022889952,
-            "angularVelocity": 9.203271146236807e-14,
-            "velocityX": 0.0000010499913044241082,
-            "velocityY": 3.7829405750872978,
-            "timestamp": 0.792591422488126
-        },
-        {
-            "x": 7.680001849352207,
-            "y": 3.479395390724662,
-            "heading": 3.1399998022890023,
-            "angularVelocity": 7.71401741801888e-14,
-            "velocityX": 2.034524499813149e-7,
-            "velocityY": 3.7829405750873777,
-            "timestamp": 0.8916653502991417
-        },
-        {
-            "x": 7.6800017836349985,
-            "y": 3.8541861721742245,
-            "heading": 3.1399998022890094,
-            "angularVelocity": 7.688638756481334e-14,
-            "velocityX": -6.633148330797741e-7,
-            "velocityY": 3.7829405750873186,
-            "timestamp": 0.9907392781101575
-        },
-        {
-            "x": 7.68000163167861,
-            "y": 4.22897695362377,
-            "heading": 3.1399998022890157,
-            "angularVelocity": 6.981254883852486e-14,
-            "velocityX": -0.000001533767606062718,
-            "velocityY": 3.7829405750871206,
-            "timestamp": 1.0898132059211731
-        },
-        {
-            "x": 7.680001393541454,
-            "y": 4.603767735073283,
-            "heading": 3.139999802289022,
-            "angularVelocity": 6.191049012077391e-14,
-            "velocityX": -0.00000240363076954954,
-            "velocityY": 3.782940575086774,
-            "timestamp": 1.188887133732189
-        },
-        {
-            "x": 7.680001069388574,
-            "y": 4.978558516522716,
-            "heading": 3.139999802289024,
-            "angularVelocity": 2.5336752196891655e-14,
-            "velocityX": -0.000003271828396587149,
-            "velocityY": 3.782940575085891,
-            "timestamp": 1.2879610615432047
-        },
-        {
-            "x": 7.680000659196884,
-            "y": 5.305935091856245,
-            "heading": 3.139999885175708,
-            "angularVelocity": 8.366142293360243e-7,
-            "velocityX": -0.000004140258200407122,
-            "velocityY": 3.304366573191614,
-            "timestamp": 1.3870349893542204
-        },
-        {
-            "x": 7.6800003382888615,
-            "y": 5.5514675412581065,
-            "heading": 3.1399999457053744,
-            "angularVelocity": 6.10954418213692e-7,
-            "velocityX": -0.0000032390761240661895,
-            "velocityY": 2.478275110584155,
-            "timestamp": 1.4861089171652362
-        },
+            "x": 7.68,
+            "y": 0.8688441539517631,
+            "heading": 1.4487899486085006e-18,
+            "angularVelocity": 1.4623322004407643e-17,
+            "velocityX": 2.341988908112997e-19,
+            "velocityY": 0.8260917454276263,
+            "timestamp": 0.09907392781100396
+        },
+        {
+            "x": 7.68,
+            "y": 1.03253245874193,
+            "heading": 4.3483011920968684e-18,
+            "angularVelocity": 2.926613800008521e-17,
+            "velocityX": 4.684469740721679e-19,
+            "velocityY": 1.6521834594306495,
+            "timestamp": 0.19814785562200793
+        },
+        {
+            "x": 7.68,
+            "y": 1.2780649081437847,
+            "heading": 8.701494230539972e-18,
+            "angularVelocity": 4.393883572228609e-17,
+            "velocityX": 7.027934476495029e-19,
+            "velocityY": 2.4782751105844794,
+            "timestamp": 0.29722178343301187
+        },
+        {
+            "x": 7.68,
+            "y": 1.6054414834771922,
+            "heading": 1.4516368843485863e-17,
+            "angularVelocity": 5.869227900187472e-17,
+            "velocityX": 9.37435089599571e-19,
+            "velocityY": 3.304366573190877,
+            "timestamp": 0.39629571124401586
+        },
+        {
+            "x": 7.68,
+            "y": 1.9802322649266864,
+            "heading": 6.647154079192576e-18,
+            "angularVelocity": -7.942770553424421e-17,
+            "velocityX": 1.0709424285811041e-18,
+            "velocityY": 3.782940575087071,
+            "timestamp": 0.49536963905501985
+        },
+        {
+            "x": 7.68,
+            "y": 2.3550230463762047,
+            "heading": -4.11606275908414e-17,
+            "angularVelocity": -4.825465460619937e-16,
+            "velocityX": 1.073083844244948e-18,
+            "velocityY": 3.7829405750873115,
+            "timestamp": 0.5944435668660238
+        },
+        {
+            "x": 7.68,
+            "y": 2.729813827825723,
+            "heading": -7.635066611173202e-17,
+            "angularVelocity": -3.5518969822232415e-16,
+            "velocityX": 1.0738946259920485e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.6935174946770278
+        },
         {
-            "x": 7.6800001156321125,
-            "y": 5.715155846048254,
-            "heading": 3.139999983279866,
-            "angularVelocity": 3.792570390671995e-7,
-            "velocityX": -0.000002247379691011521,
-            "velocityY": 1.6521834594301872,
-            "timestamp": 1.585182844976252
+            "x": 7.68,
+            "y": 3.1046046092752415,
+            "heading": -5.864574302033697e-17,
+            "angularVelocity": 1.787041604453991e-16,
+            "velocityX": 1.0691415340399175e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.7925914224880318
+        },
+        {
+            "x": 7.68,
+            "y": 3.4793953907247595,
+            "heading": -2.6391173603704442e-17,
+            "angularVelocity": 3.255606205313886e-16,
+            "velocityX": 1.0732289061667476e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.8916653502990358
+        },
+        {
+            "x": 7.68,
+            "y": 3.854186172174278,
+            "heading": -6.511485011951871e-17,
+            "angularVelocity": -3.9085637736788184e-16,
+            "velocityX": 1.0716697150075795e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.9907392781100398
+        },
+        {
+            "x": 7.68,
+            "y": 4.228976953623796,
+            "heading": -6.241329479526944e-17,
+            "angularVelocity": 2.7268075304360027e-17,
+            "velocityX": 1.0686111075844087e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 1.0898132059210437
+        },
+        {
+            "x": 7.68,
+            "y": 4.603767735073314,
+            "heading": -2.2737833863559818e-17,
+            "angularVelocity": 4.004631875225205e-16,
+            "velocityX": 1.0710197916722359e-18,
+            "velocityY": 3.7829405750873115,
+            "timestamp": 1.1888871337320477
+        },
+        {
+            "x": 7.68,
+            "y": 4.978558516522808,
+            "heading": -1.448944579275074e-17,
+            "angularVelocity": 8.325488100707829e-17,
+            "velocityX": 1.0714504382145178e-18,
+            "velocityY": 3.782940575087071,
+            "timestamp": 1.2879610615430517
+        },
+        {
+            "x": 7.68,
+            "y": 5.305935091856216,
+            "heading": -8.687037170545135e-18,
+            "angularVelocity": 5.856645386228716e-17,
+            "velocityX": 9.36753995288457e-19,
+            "velocityY": 3.304366573190877,
+            "timestamp": 1.3870349893540557
+        },
+        {
+            "x": 7.68,
+            "y": 5.55146754125807,
+            "heading": -4.341905072762387e-18,
+            "angularVelocity": 4.385747283685971e-17,
+            "velocityX": 7.025655481927048e-19,
+            "velocityY": 2.4782751105844794,
+            "timestamp": 1.4861089171650597
+        },
+        {
+            "x": 7.68,
+            "y": 5.715155846048237,
+            "heading": -1.4469357825353783e-18,
+            "angularVelocity": 2.9220293918225814e-17,
+            "velocityX": 4.683770471216647e-19,
+            "velocityY": 1.6521834594306495,
+            "timestamp": 1.5851828449760637
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": 1.687641935617777e-7,
-            "velocityX": -0.0000011671295149488337,
-            "velocityY": 0.8260917454273095,
-            "timestamp": 1.6842567727872677
+            "heading": 0,
+            "angularVelocity": 1.460460702963924e-17,
+            "velocityX": 2.341885280588165e-19,
+            "velocityY": 0.8260917454276263,
+            "timestamp": 1.6842567727870676
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.14,
-            "angularVelocity": -8.926921159411167e-32,
-            "velocityX": 0,
-            "velocityY": 7.102031651059941e-38,
-            "timestamp": 1.7833307005982835
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 3.1812192470207854e-34,
+            "velocityY": 0,
+            "timestamp": 1.7833307005980716
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C3.1.traj
new file mode 100644
index 00000000..864eb2e4
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C3.1.traj	
@@ -0,0 +1,139 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 2.417290808823166e-40,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 0.8634453720055388,
+            "heading": 1.009810999063182e-18,
+            "angularVelocity": 1.0546271494620871e-17,
+            "velocityX": -3.226662245674165e-34,
+            "velocityY": 0.7983807350946677,
+            "timestamp": 0.09575052183150978
+        },
+        {
+            "x": 7.68,
+            "y": 1.0163361129826893,
+            "heading": 3.0323418642587253e-18,
+            "angularVelocity": 2.112292263585315e-17,
+            "velocityX": -6.037770263383471e-34,
+            "velocityY": 1.596761438503583,
+            "timestamp": 0.19150104366301957
+        },
+        {
+            "x": 7.68,
+            "y": 1.2456722168635992,
+            "heading": 6.073900773319643e-18,
+            "angularVelocity": 3.1765455168164525e-17,
+            "velocityX": -4.821612934727609e-34,
+            "velocityY": 2.3951420785410233,
+            "timestamp": 0.28725156549452935
+        },
+        {
+            "x": 7.68,
+            "y": 1.5514536654447455,
+            "heading": 1.0152788689831222e-17,
+            "angularVelocity": 4.2599119433487623e-17,
+            "velocityX": -1.2140491970354773e-33,
+            "velocityY": 3.19352252846437,
+            "timestamp": 0.38300208732603913
+        },
+        {
+            "x": 7.68,
+            "y": 1.9136721992668209,
+            "heading": 4.411917253468209e-18,
+            "angularVelocity": -5.995655508243724e-17,
+            "velocityX": 4.90795348832912e-33,
+            "velocityY": 3.7829405719528495,
+            "timestamp": 0.4787526091575489
+        },
+        {
+            "x": 7.68,
+            "y": 2.27589073308894,
+            "heading": 1.2796107303848674e-17,
+            "angularVelocity": 8.756286535058141e-17,
+            "velocityX": -1.5435756690671252e-33,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.5745031309890587
+        },
+        {
+            "x": 7.68,
+            "y": 2.63810926691106,
+            "heading": 6.994895399550348e-18,
+            "angularVelocity": -6.058673930264937e-17,
+            "velocityX": 4.776897057793067e-34,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.6702536528205685
+        },
+        {
+            "x": 7.68,
+            "y": 3.0003278007331793,
+            "heading": -9.534020306729642e-18,
+            "angularVelocity": -1.7262481070720064e-16,
+            "velocityX": 2.9275045647973183e-33,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.7660041746520783
+        },
+        {
+            "x": 7.68,
+            "y": 3.3625463345552546,
+            "heading": -1.0013819273727212e-17,
+            "angularVelocity": -5.010927959660929e-18,
+            "velocityX": 2.237784317663532e-33,
+            "velocityY": 3.7829405719528495,
+            "timestamp": 0.861754696483588
+        },
+        {
+            "x": 7.68,
+            "y": 3.6683277831364007,
+            "heading": -6.0064636534747415e-18,
+            "angularVelocity": 4.185204993640188e-17,
+            "velocityX": -2.2313398142102424e-33,
+            "velocityY": 3.19352252846437,
+            "timestamp": 0.9575052183150978
+        },
+        {
+            "x": 7.68,
+            "y": 3.897663887017311,
+            "heading": -3.002975312877452e-18,
+            "angularVelocity": 3.136785349810471e-17,
+            "velocityX": -2.8833027864886262e-33,
+            "velocityY": 2.3951420785410233,
+            "timestamp": 1.0532557401466076
+        },
+        {
+            "x": 7.68,
+            "y": 4.050554627994461,
+            "heading": -1.0009557447862449e-18,
+            "angularVelocity": 2.0908706606963636e-17,
+            "velocityX": -3.742421273800505e-33,
+            "velocityY": 1.596761438503583,
+            "timestamp": 1.1490062619781174
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 1.0453788927770355e-17,
+            "velocityX": -1.5032681111068507e-33,
+            "velocityY": 0.7983807350946677,
+            "timestamp": 1.2447567838096272
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 9.665729080515376e-43,
+            "velocityY": 2.80463986608038e-38,
+            "timestamp": 1.340507305641137
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C3.traj
new file mode 100644
index 00000000..864eb2e4
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C3.traj	
@@ -0,0 +1,139 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 2.417290808823166e-40,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 0.8634453720055388,
+            "heading": 1.009810999063182e-18,
+            "angularVelocity": 1.0546271494620871e-17,
+            "velocityX": -3.226662245674165e-34,
+            "velocityY": 0.7983807350946677,
+            "timestamp": 0.09575052183150978
+        },
+        {
+            "x": 7.68,
+            "y": 1.0163361129826893,
+            "heading": 3.0323418642587253e-18,
+            "angularVelocity": 2.112292263585315e-17,
+            "velocityX": -6.037770263383471e-34,
+            "velocityY": 1.596761438503583,
+            "timestamp": 0.19150104366301957
+        },
+        {
+            "x": 7.68,
+            "y": 1.2456722168635992,
+            "heading": 6.073900773319643e-18,
+            "angularVelocity": 3.1765455168164525e-17,
+            "velocityX": -4.821612934727609e-34,
+            "velocityY": 2.3951420785410233,
+            "timestamp": 0.28725156549452935
+        },
+        {
+            "x": 7.68,
+            "y": 1.5514536654447455,
+            "heading": 1.0152788689831222e-17,
+            "angularVelocity": 4.2599119433487623e-17,
+            "velocityX": -1.2140491970354773e-33,
+            "velocityY": 3.19352252846437,
+            "timestamp": 0.38300208732603913
+        },
+        {
+            "x": 7.68,
+            "y": 1.9136721992668209,
+            "heading": 4.411917253468209e-18,
+            "angularVelocity": -5.995655508243724e-17,
+            "velocityX": 4.90795348832912e-33,
+            "velocityY": 3.7829405719528495,
+            "timestamp": 0.4787526091575489
+        },
+        {
+            "x": 7.68,
+            "y": 2.27589073308894,
+            "heading": 1.2796107303848674e-17,
+            "angularVelocity": 8.756286535058141e-17,
+            "velocityX": -1.5435756690671252e-33,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.5745031309890587
+        },
+        {
+            "x": 7.68,
+            "y": 2.63810926691106,
+            "heading": 6.994895399550348e-18,
+            "angularVelocity": -6.058673930264937e-17,
+            "velocityX": 4.776897057793067e-34,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.6702536528205685
+        },
+        {
+            "x": 7.68,
+            "y": 3.0003278007331793,
+            "heading": -9.534020306729642e-18,
+            "angularVelocity": -1.7262481070720064e-16,
+            "velocityX": 2.9275045647973183e-33,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.7660041746520783
+        },
+        {
+            "x": 7.68,
+            "y": 3.3625463345552546,
+            "heading": -1.0013819273727212e-17,
+            "angularVelocity": -5.010927959660929e-18,
+            "velocityX": 2.237784317663532e-33,
+            "velocityY": 3.7829405719528495,
+            "timestamp": 0.861754696483588
+        },
+        {
+            "x": 7.68,
+            "y": 3.6683277831364007,
+            "heading": -6.0064636534747415e-18,
+            "angularVelocity": 4.185204993640188e-17,
+            "velocityX": -2.2313398142102424e-33,
+            "velocityY": 3.19352252846437,
+            "timestamp": 0.9575052183150978
+        },
+        {
+            "x": 7.68,
+            "y": 3.897663887017311,
+            "heading": -3.002975312877452e-18,
+            "angularVelocity": 3.136785349810471e-17,
+            "velocityX": -2.8833027864886262e-33,
+            "velocityY": 2.3951420785410233,
+            "timestamp": 1.0532557401466076
+        },
+        {
+            "x": 7.68,
+            "y": 4.050554627994461,
+            "heading": -1.0009557447862449e-18,
+            "angularVelocity": 2.0908706606963636e-17,
+            "velocityX": -3.742421273800505e-33,
+            "velocityY": 1.596761438503583,
+            "timestamp": 1.1490062619781174
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 1.0453788927770355e-17,
+            "velocityX": -1.5032681111068507e-33,
+            "velocityY": 0.7983807350946677,
+            "timestamp": 1.2447567838096272
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 9.665729080515376e-43,
+            "velocityY": 2.80463986608038e-38,
+            "timestamp": 1.340507305641137
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.1.traj
index b9d634dc..17170fc9 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.1.traj	
@@ -3,92 +3,92 @@
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": -4.029789856147219e-33,
-            "velocityY": -3.379656030870708e-35,
+            "heading": 9.069206296242399e-40,
+            "angularVelocity": -9.927610039652183e-40,
+            "velocityX": -8.253947176298157e-40,
+            "velocityY": 1.8367099231598242e-40,
             "timestamp": 0
         },
         {
-            "x": 7.680000025226674,
-            "y": 0.87050000557584,
-            "heading": 3.139999988557334,
-            "angularVelocity": -1.1434532837257691e-7,
-            "velocityX": 2.5208743257476917e-7,
-            "velocityY": 0.8344065481227664,
-            "timestamp": 0.10007112931184067
+            "x": 7.68,
+            "y": 0.8705000055758465,
+            "heading": 6.490773345690525e-22,
+            "angularVelocity": 6.486159683422905e-21,
+            "velocityX": -7.132918469580153e-33,
+            "velocityY": 0.8344065481228213,
+            "timestamp": 0.10007112931184182
         },
         {
-            "x": 7.680000070528317,
-            "y": 1.03750001362983,
-            "heading": 3.1399999655365143,
-            "angularVelocity": -2.300445661244102e-7,
-            "velocityX": 4.5269443024398455e-7,
-            "velocityY": 1.6688130652906519,
-            "timestamp": 0.20014225862368135
+            "x": 7.68,
+            "y": 1.037500013629847,
+            "heading": 2.023593010760001e-21,
+            "angularVelocity": 1.3735386658326633e-20,
+            "velocityX": -1.5648094960655976e-32,
+            "velocityY": 1.668813065290736,
+            "timestamp": 0.20014225862368365
         },
         {
-            "x": 7.6800001304795185,
-            "y": 1.2880000179665891,
-            "heading": 3.139999931372676,
-            "angularVelocity": -3.413955478564011e-7,
-            "velocityX": 5.990858804609274e-7,
-            "velocityY": 2.5032195205487646,
-            "timestamp": 0.300213387935522
+            "x": 7.68,
+            "y": 1.2880000179666162,
+            "heading": 3.787875676222919e-21,
+            "angularVelocity": 1.7630286060953193e-20,
+            "velocityX": -2.3089760494009302e-32,
+            "velocityY": 2.503219520548835,
+            "timestamp": 0.3002133879355255
         },
         {
-            "x": 7.680000199269358,
-            "y": 1.6219999999999795,
-            "heading": 3.1399998873718746,
-            "angularVelocity": -4.396952561787762e-7,
-            "velocityX": 6.874094371094506e-7,
-            "velocityY": 3.337625790077605,
-            "timestamp": 0.4002845172473627
+            "x": 7.68,
+            "y": 1.6219999999999999,
+            "heading": 5.860880748165694e-21,
+            "angularVelocity": 2.0715315514205814e-20,
+            "velocityX": -3.0704903308303665e-32,
+            "velocityY": 3.3376257900775013,
+            "timestamp": 0.4002845172473673
         },
         {
-            "x": 7.680000135199738,
-            "y": 1.9559999820333853,
-            "heading": 3.1399999178431197,
-            "angularVelocity": 3.0449586057484084e-7,
-            "velocityX": -6.402407947941681e-7,
-            "velocityY": 3.3376257900777593,
-            "timestamp": 0.5003556465592034
+            "x": 7.68,
+            "y": 1.9559999820333838,
+            "heading": 3.934658943161884e-21,
+            "angularVelocity": -1.9248526176341983e-20,
+            "velocityX": -4.040562356218352e-32,
+            "velocityY": 3.3376257900775017,
+            "timestamp": 0.5003556465592092
         },
         {
-            "x": 7.680000075581297,
-            "y": 2.206499986370157,
-            "heading": 3.139999951084868,
-            "angularVelocity": 3.3218120651110665e-7,
-            "velocityX": -5.957606422672805e-7,
-            "velocityY": 2.503219520548894,
-            "timestamp": 0.6004267758710441
+            "x": 7.68,
+            "y": 2.206499986370153,
+            "heading": 1.6176834885162177e-21,
+            "angularVelocity": -2.3153285409554772e-20,
+            "velocityX": -3.08903871620124e-32,
+            "velocityY": 2.503219520548835,
+            "timestamp": 0.600426775871051
         },
         {
-            "x": 7.680000027896081,
-            "y": 2.373499994424156,
-            "heading": 3.13999998094686,
-            "angularVelocity": 2.984076655658666e-7,
-            "velocityX": -4.765132174434968e-7,
-            "velocityY": 1.6688130652907383,
-            "timestamp": 0.7004979051828848
+            "x": 7.68,
+            "y": 2.3734999944241535,
+            "heading": 4.992981261257204e-22,
+            "angularVelocity": -1.1175904096118944e-20,
+            "velocityX": -2.3331216062245415e-32,
+            "velocityY": 1.668813065290736,
+            "timestamp": 0.7004979051828928
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 1.903959699368805e-7,
-            "velocityX": -2.787625301130355e-7,
-            "velocityY": 0.8344065481228072,
-            "timestamp": 0.8005690344947255
+            "heading": 1.7562757360140444e-40,
+            "angularVelocity": -4.9894322348928345e-21,
+            "velocityX": -1.3647931662662733e-32,
+            "velocityY": 0.8344065481228213,
+            "timestamp": 0.8005690344947346
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -2.2348525624284448e-32,
-            "velocityX": 0,
-            "velocityY": 2.3896717289528774e-36,
-            "timestamp": 0.9006401638065662
+            "heading": -7.274275970183695e-41,
+            "angularVelocity": -2.4819026120794767e-39,
+            "velocityX": -8.637198685981915e-42,
+            "velocityY": 5.142787784847508e-39,
+            "timestamp": 0.9006401638065764
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.traj
index b9d634dc..17170fc9 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.traj	
@@ -3,92 +3,92 @@
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": -4.029789856147219e-33,
-            "velocityY": -3.379656030870708e-35,
+            "heading": 9.069206296242399e-40,
+            "angularVelocity": -9.927610039652183e-40,
+            "velocityX": -8.253947176298157e-40,
+            "velocityY": 1.8367099231598242e-40,
             "timestamp": 0
         },
         {
-            "x": 7.680000025226674,
-            "y": 0.87050000557584,
-            "heading": 3.139999988557334,
-            "angularVelocity": -1.1434532837257691e-7,
-            "velocityX": 2.5208743257476917e-7,
-            "velocityY": 0.8344065481227664,
-            "timestamp": 0.10007112931184067
+            "x": 7.68,
+            "y": 0.8705000055758465,
+            "heading": 6.490773345690525e-22,
+            "angularVelocity": 6.486159683422905e-21,
+            "velocityX": -7.132918469580153e-33,
+            "velocityY": 0.8344065481228213,
+            "timestamp": 0.10007112931184182
         },
         {
-            "x": 7.680000070528317,
-            "y": 1.03750001362983,
-            "heading": 3.1399999655365143,
-            "angularVelocity": -2.300445661244102e-7,
-            "velocityX": 4.5269443024398455e-7,
-            "velocityY": 1.6688130652906519,
-            "timestamp": 0.20014225862368135
+            "x": 7.68,
+            "y": 1.037500013629847,
+            "heading": 2.023593010760001e-21,
+            "angularVelocity": 1.3735386658326633e-20,
+            "velocityX": -1.5648094960655976e-32,
+            "velocityY": 1.668813065290736,
+            "timestamp": 0.20014225862368365
         },
         {
-            "x": 7.6800001304795185,
-            "y": 1.2880000179665891,
-            "heading": 3.139999931372676,
-            "angularVelocity": -3.413955478564011e-7,
-            "velocityX": 5.990858804609274e-7,
-            "velocityY": 2.5032195205487646,
-            "timestamp": 0.300213387935522
+            "x": 7.68,
+            "y": 1.2880000179666162,
+            "heading": 3.787875676222919e-21,
+            "angularVelocity": 1.7630286060953193e-20,
+            "velocityX": -2.3089760494009302e-32,
+            "velocityY": 2.503219520548835,
+            "timestamp": 0.3002133879355255
         },
         {
-            "x": 7.680000199269358,
-            "y": 1.6219999999999795,
-            "heading": 3.1399998873718746,
-            "angularVelocity": -4.396952561787762e-7,
-            "velocityX": 6.874094371094506e-7,
-            "velocityY": 3.337625790077605,
-            "timestamp": 0.4002845172473627
+            "x": 7.68,
+            "y": 1.6219999999999999,
+            "heading": 5.860880748165694e-21,
+            "angularVelocity": 2.0715315514205814e-20,
+            "velocityX": -3.0704903308303665e-32,
+            "velocityY": 3.3376257900775013,
+            "timestamp": 0.4002845172473673
         },
         {
-            "x": 7.680000135199738,
-            "y": 1.9559999820333853,
-            "heading": 3.1399999178431197,
-            "angularVelocity": 3.0449586057484084e-7,
-            "velocityX": -6.402407947941681e-7,
-            "velocityY": 3.3376257900777593,
-            "timestamp": 0.5003556465592034
+            "x": 7.68,
+            "y": 1.9559999820333838,
+            "heading": 3.934658943161884e-21,
+            "angularVelocity": -1.9248526176341983e-20,
+            "velocityX": -4.040562356218352e-32,
+            "velocityY": 3.3376257900775017,
+            "timestamp": 0.5003556465592092
         },
         {
-            "x": 7.680000075581297,
-            "y": 2.206499986370157,
-            "heading": 3.139999951084868,
-            "angularVelocity": 3.3218120651110665e-7,
-            "velocityX": -5.957606422672805e-7,
-            "velocityY": 2.503219520548894,
-            "timestamp": 0.6004267758710441
+            "x": 7.68,
+            "y": 2.206499986370153,
+            "heading": 1.6176834885162177e-21,
+            "angularVelocity": -2.3153285409554772e-20,
+            "velocityX": -3.08903871620124e-32,
+            "velocityY": 2.503219520548835,
+            "timestamp": 0.600426775871051
         },
         {
-            "x": 7.680000027896081,
-            "y": 2.373499994424156,
-            "heading": 3.13999998094686,
-            "angularVelocity": 2.984076655658666e-7,
-            "velocityX": -4.765132174434968e-7,
-            "velocityY": 1.6688130652907383,
-            "timestamp": 0.7004979051828848
+            "x": 7.68,
+            "y": 2.3734999944241535,
+            "heading": 4.992981261257204e-22,
+            "angularVelocity": -1.1175904096118944e-20,
+            "velocityX": -2.3331216062245415e-32,
+            "velocityY": 1.668813065290736,
+            "timestamp": 0.7004979051828928
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": 1.903959699368805e-7,
-            "velocityX": -2.787625301130355e-7,
-            "velocityY": 0.8344065481228072,
-            "timestamp": 0.8005690344947255
+            "heading": 1.7562757360140444e-40,
+            "angularVelocity": -4.9894322348928345e-21,
+            "velocityX": -1.3647931662662733e-32,
+            "velocityY": 0.8344065481228213,
+            "timestamp": 0.8005690344947346
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 3.14,
-            "angularVelocity": -2.2348525624284448e-32,
-            "velocityX": 0,
-            "velocityY": 2.3896717289528774e-36,
-            "timestamp": 0.9006401638065662
+            "heading": -7.274275970183695e-41,
+            "angularVelocity": -2.4819026120794767e-39,
+            "velocityX": -8.637198685981915e-42,
+            "velocityY": 5.142787784847508e-39,
+            "timestamp": 0.9006401638065764
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.1.traj
index 9d086628..d9cc7c97 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.1.traj	
@@ -3,182 +3,245 @@
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 9.969319932712997e-31,
-            "velocityX": 3.5015968931749707e-32,
-            "velocityY": -1.3449299836267862e-31,
+            "heading": 0,
+            "angularVelocity": -3.982415645335998e-31,
+            "velocityX": 3.346280928312519e-33,
+            "velocityY": 3.541078180282768e-38,
             "timestamp": 0
         },
         {
-            "x": 7.622998015019161,
-            "y": 0.8057692963239994,
-            "heading": 3.0732063030265673,
-            "angularVelocity": -0.7701918865802806,
-            "velocityX": -0.6572845693610088,
-            "velocityY": 0.21642700435214937,
-            "timestamp": 0.08672344923029876
-        },
-        {
-            "x": 7.508611650111109,
-            "y": 0.8437411084542301,
-            "heading": 2.9459376863662614,
-            "angularVelocity": -1.4675225419406142,
-            "velocityX": -1.318978499163396,
-            "velocityY": 0.43784942212566424,
-            "timestamp": 0.17344689846059752
-        },
-        {
-            "x": 7.336139052754301,
-            "y": 0.9014260823029367,
-            "heading": 2.7711998622663616,
-            "angularVelocity": -2.014885543076985,
-            "velocityX": -1.9887654248944688,
-            "velocityY": 0.6651600502595449,
-            "timestamp": 0.2601703476908963
-        },
-        {
-            "x": 7.104576647516153,
-            "y": 0.9793814888777497,
-            "heading": 2.5768200425700405,
-            "angularVelocity": -2.241375561298711,
-            "velocityX": -2.6701244852844987,
-            "velocityY": 0.8988965183776096,
-            "timestamp": 0.34689379692119504
-        },
-        {
-            "x": 6.822163575098835,
-            "y": 1.078219287033468,
-            "heading": 2.4981665441447336,
-            "angularVelocity": -0.9069461503593836,
-            "velocityX": -3.2564787831184523,
-            "velocityY": 1.1396894269420643,
-            "timestamp": 0.4336172461514938
-        },
-        {
-            "x": 6.510888238397409,
-            "y": 1.1818405008088049,
-            "heading": 2.498166488214478,
-            "angularVelocity": -6.449265521718259e-7,
-            "velocityX": -3.5892868591379226,
-            "velocityY": 1.194846546061207,
-            "timestamp": 0.5203406953817925
-        },
-        {
-            "x": 6.199612902811833,
-            "y": 1.2854617179365646,
-            "heading": 2.498166432284442,
-            "angularVelocity": -6.449240327985389e-7,
-            "velocityX": -3.589286846271159,
-            "velocityY": 1.194846584717682,
-            "timestamp": 0.6070641446120912
-        },
-        {
-            "x": 5.8883375891897005,
-            "y": 1.389083001041933,
-            "heading": 2.4981663763544435,
-            "angularVelocity": -6.449235898174935e-7,
-            "velocityX": -3.5892865930127487,
-            "velocityY": 1.1948473454993283,
-            "timestamp": 0.69378759384239
-        },
-        {
-            "x": 5.577064983818878,
-            "y": 1.4927124193076864,
-            "heading": 2.4981663204239246,
-            "angularVelocity": -6.449295917693127e-7,
-            "velocityX": -3.589255364419621,
-            "velocityY": 1.194941151274556,
-            "timestamp": 0.7805110430726887
-        },
-        {
-            "x": 5.275080475665308,
-            "y": 1.6209116670289339,
-            "heading": 2.4981662550669967,
-            "angularVelocity": -7.536246362770858e-7,
-            "velocityX": -3.4821551821772507,
-            "velocityY": 1.4782535618573864,
-            "timestamp": 0.8672344923029874
-        },
-        {
-            "x": 4.987205237443244,
-            "y": 1.7782569230783876,
-            "heading": 2.49816614132643,
-            "angularVelocity": -0.000001311531862273131,
-            "velocityX": -3.319462507281,
-            "velocityY": 1.8143334639702267,
-            "timestamp": 0.9539579415332862
-        },
-        {
-            "x": 4.728299565339017,
-            "y": 1.9797451797609154,
-            "heading": 2.498165933556362,
-            "angularVelocity": -0.0000023957772582798155,
-            "velocityX": -2.9854171438302592,
-            "velocityY": 2.3233422848238554,
-            "timestamp": 1.040681390763585
-        },
-        {
-            "x": 4.523329416895473,
-            "y": 2.207660623196326,
-            "heading": 2.432888839209935,
-            "angularVelocity": -0.7527040832183707,
-            "velocityX": -2.3634916537882953,
-            "velocityY": 2.6280717090733883,
-            "timestamp": 1.1274048399938839
-        },
-        {
-            "x": 4.381013836981984,
-            "y": 2.433217084695142,
-            "heading": 2.3632823552491717,
-            "angularVelocity": -0.8026258708405342,
-            "velocityX": -1.6410276710231155,
-            "velocityY": 2.600870508503859,
-            "timestamp": 1.2141282892241827
-        },
-        {
-            "x": 4.294783443237854,
-            "y": 2.6401553266407616,
-            "heading": 2.3696134751420326,
-            "angularVelocity": 0.07300355266138053,
-            "velocityX": -0.9943146232011729,
-            "velocityY": 2.386185556297287,
-            "timestamp": 1.3008517384544815
-        },
-        {
-            "x": 4.26053013180991,
-            "y": 2.816849773520389,
-            "heading": 2.4414538337655816,
-            "angularVelocity": 0.8283844711108445,
-            "velocityX": -0.39497173753989284,
-            "velocityY": 2.0374471777570338,
-            "timestamp": 1.3875751876847804
-        },
-        {
-            "x": 4.257469858369506,
-            "y": 2.9392038098412794,
-            "heading": 2.522566294218638,
-            "angularVelocity": 0.9353002120298306,
-            "velocityX": -0.035287727455096035,
-            "velocityY": 1.4108529746778473,
-            "timestamp": 1.4742986369150792
+            "x": 7.629395539536042,
+            "y": 0.7881897280541793,
+            "heading": -0.02207380596023684,
+            "angularVelocity": -0.2822732032732244,
+            "velocityX": -0.6471146471374848,
+            "velocityY": 0.015213885156899113,
+            "timestamp": 0.0782001468948451
+        },
+        {
+            "x": 7.528184091504648,
+            "y": 0.7905692108612611,
+            "heading": -0.06614402373093474,
+            "angularVelocity": -0.5635567133951742,
+            "velocityX": -1.2942616101155584,
+            "velocityY": 0.030428111729043,
+            "timestamp": 0.1564002937896902
+        },
+        {
+            "x": 7.376360973312952,
+            "y": 0.7941378827255743,
+            "heading": -0.13206235338942335,
+            "angularVelocity": -0.8429438086302324,
+            "velocityX": -1.9414684526853105,
+            "velocityY": 0.04563510435477082,
+            "timestamp": 0.23460044068453528
+        },
+        {
+            "x": 7.173916466766183,
+            "y": 0.7988917309385671,
+            "heading": -0.21951772550448787,
+            "angularVelocity": -1.1183530413716716,
+            "velocityX": -2.588799568612108,
+            "velocityY": 0.060790783668257076,
+            "timestamp": 0.3128005875793804
+        },
+        {
+            "x": 6.920824289019536,
+            "y": 0.8048108023879232,
+            "heading": -0.3276760191057486,
+            "angularVelocity": -1.383095785572821,
+            "velocityX": -3.236466781667369,
+            "velocityY": 0.07569130857196414,
+            "timestamp": 0.3910007344742255
+        },
+        {
+            "x": 6.625087122175782,
+            "y": 0.8120803100358527,
+            "heading": -0.3276760358371292,
+            "angularVelocity": -2.1395587179409026e-7,
+            "velocityX": -3.781798098684327,
+            "velocityY": 0.09296028123113304,
+            "timestamp": 0.4692008813690706
+        },
+        {
+            "x": 6.329343587367458,
+            "y": 0.8190859886904246,
+            "heading": -0.3276760513572638,
+            "angularVelocity": -1.9846682128968708e-7,
+            "velocityX": -3.78187953030339,
+            "velocityY": 0.08958651527359804,
+            "timestamp": 0.5474010282639157
+        },
+        {
+            "x": 6.033600055760057,
+            "y": 0.826091802470254,
+            "heading": -0.3276760668773978,
+            "angularVelocity": -1.9846681457625392e-7,
+            "velocityX": -3.7818794893709464,
+            "velocityY": 0.0895882432148227,
+            "timestamp": 0.6256011751587608
+        },
+        {
+            "x": 5.7383102527596765,
+            "y": 0.8439033174946344,
+            "heading": -0.3276760825327013,
+            "angularVelocity": -2.0019532088764418e-7,
+            "velocityX": -3.7760773441696487,
+            "velocityY": 0.22776830647924015,
+            "timestamp": 0.7038013220536059
+        },
+        {
+            "x": 5.450465299571764,
+            "y": 0.9121572529445121,
+            "heading": -0.32767609968005607,
+            "angularVelocity": -2.1927522433491614e-7,
+            "velocityX": -3.680874839978332,
+            "velocityY": 0.8728108342551084,
+            "timestamp": 0.782001468948451
+        },
+        {
+            "x": 5.178616904201158,
+            "y": 1.028826716826592,
+            "heading": -0.32767612118071976,
+            "angularVelocity": -2.749440322201668e-7,
+            "velocityX": -3.4763156613508643,
+            "velocityY": 1.4919340757663395,
+            "timestamp": 0.8602016158432961
+        },
+        {
+            "x": 4.930841529412244,
+            "y": 1.190445624246428,
+            "heading": -0.3276761534331376,
+            "angularVelocity": -4.1243423626599064e-7,
+            "velocityX": -3.1684771017365216,
+            "velocityY": 2.0667340642903906,
+            "timestamp": 0.9384017627381412
+        },
+        {
+            "x": 4.7145004488669215,
+            "y": 1.3922123936122235,
+            "heading": -0.32767621878386327,
+            "angularVelocity": -8.356854591223944e-7,
+            "velocityX": -2.7665047846524513,
+            "velocityY": 2.5801328690255976,
+            "timestamp": 1.0166019096329864
+        },
+        {
+            "x": 4.536026477813721,
+            "y": 1.628126621246338,
+            "heading": -0.327696539002069,
+            "angularVelocity": -0.00025984882823803823,
+            "velocityX": -2.282271557538034,
+            "velocityY": 3.016800313073996,
+            "timestamp": 1.0948020565278314
+        },
+        {
+            "x": 4.4347589269746495,
+            "y": 1.8009845745755027,
+            "heading": -0.3311064514524199,
+            "angularVelocity": -0.06394544615832723,
+            "velocityX": -1.899051313343304,
+            "velocityY": 3.241572651564751,
+            "timestamp": 1.1481273933038434
+        },
+        {
+            "x": 4.355560152796487,
+            "y": 1.9823487857199633,
+            "heading": -0.3404702376957046,
+            "angularVelocity": -0.17559732033810904,
+            "velocityX": -1.4851996999249673,
+            "velocityY": 3.4010889027473166,
+            "timestamp": 1.2014527300798554
+        },
+        {
+            "x": 4.29961502910342,
+            "y": 2.1668323093896307,
+            "heading": -0.3620606217709223,
+            "angularVelocity": -0.40488040734269476,
+            "velocityX": -1.049128370778876,
+            "velocityY": 3.4595847832064672,
+            "timestamp": 1.2547780668558675
+        },
+        {
+            "x": 4.26624261518445,
+            "y": 2.3445509425061104,
+            "heading": -0.3930653835662206,
+            "angularVelocity": -0.5814264600999738,
+            "velocityX": -0.6258265945712352,
+            "velocityY": 3.332724064417915,
+            "timestamp": 1.3081034036318795
+        },
+        {
+            "x": 4.250245555353669,
+            "y": 2.506179624728893,
+            "heading": -0.42827286207411436,
+            "angularVelocity": -0.6602392152823773,
+            "velocityX": -0.2999898509389163,
+            "velocityY": 3.0309922448647635,
+            "timestamp": 1.3614287404078915
+        },
+        {
+            "x": 4.245253885327146,
+            "y": 2.646807044638539,
+            "heading": -0.46326114250173545,
+            "angularVelocity": -0.6561286349527023,
+            "velocityX": -0.09360784813172378,
+            "velocityY": 2.6371595270071015,
+            "timestamp": 1.4147540771839036
+        },
+        {
+            "x": 4.246491895421447,
+            "y": 2.764573131222025,
+            "heading": -0.49520995766037457,
+            "angularVelocity": -0.5991301150594935,
+            "velocityX": 0.023216170194527277,
+            "velocityY": 2.2084452476729117,
+            "timestamp": 1.4680794139599156
+        },
+        {
+            "x": 4.250760184591696,
+            "y": 2.858861309855261,
+            "heading": -0.522398988898447,
+            "angularVelocity": -0.5098707834110707,
+            "velocityX": 0.08004242313776105,
+            "velocityY": 1.7681684605074017,
+            "timestamp": 1.5214047507359276
+        },
+        {
+            "x": 4.255888225984497,
+            "y": 2.929511890736146,
+            "heading": -0.5437259487715407,
+            "angularVelocity": -0.3999404628668233,
+            "velocityX": 0.09616519468543994,
+            "velocityY": 1.324897040550944,
+            "timestamp": 1.5747300875119397
+        },
+        {
+            "x": 4.260343621579396,
+            "y": 2.9765343465211713,
+            "heading": -0.5584397105575287,
+            "angularVelocity": -0.2759244043319095,
+            "velocityX": 0.08355119468920533,
+            "velocityY": 0.8818032595380478,
+            "timestamp": 1.6280554242879517
         },
         {
             "x": 4.263,
             "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0.5930772615463757,
-            "velocityX": 0.06376754706571511,
-            "velocityY": 0.7010351951900964,
-            "timestamp": 1.561022086145378
+            "heading": -0.566,
+            "angularVelocity": -0.14177668439456792,
+            "velocityX": 0.04981456435476623,
+            "velocityY": 0.4400469813703916,
+            "timestamp": 1.6813807610639637
         },
         {
             "x": 4.263,
             "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -5.347216163770528e-32,
-            "velocityX": 0,
-            "velocityY": -5.6953276102603335e-33,
-            "timestamp": 1.647745535375677
+            "heading": -0.566,
+            "angularVelocity": 0,
+            "velocityX": 3.549235715029611e-32,
+            "velocityY": -9.598836837089386e-32,
+            "timestamp": 1.7347060978399758
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.traj
index 9d086628..d9cc7c97 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.traj	
@@ -3,182 +3,245 @@
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 9.969319932712997e-31,
-            "velocityX": 3.5015968931749707e-32,
-            "velocityY": -1.3449299836267862e-31,
+            "heading": 0,
+            "angularVelocity": -3.982415645335998e-31,
+            "velocityX": 3.346280928312519e-33,
+            "velocityY": 3.541078180282768e-38,
             "timestamp": 0
         },
         {
-            "x": 7.622998015019161,
-            "y": 0.8057692963239994,
-            "heading": 3.0732063030265673,
-            "angularVelocity": -0.7701918865802806,
-            "velocityX": -0.6572845693610088,
-            "velocityY": 0.21642700435214937,
-            "timestamp": 0.08672344923029876
-        },
-        {
-            "x": 7.508611650111109,
-            "y": 0.8437411084542301,
-            "heading": 2.9459376863662614,
-            "angularVelocity": -1.4675225419406142,
-            "velocityX": -1.318978499163396,
-            "velocityY": 0.43784942212566424,
-            "timestamp": 0.17344689846059752
-        },
-        {
-            "x": 7.336139052754301,
-            "y": 0.9014260823029367,
-            "heading": 2.7711998622663616,
-            "angularVelocity": -2.014885543076985,
-            "velocityX": -1.9887654248944688,
-            "velocityY": 0.6651600502595449,
-            "timestamp": 0.2601703476908963
-        },
-        {
-            "x": 7.104576647516153,
-            "y": 0.9793814888777497,
-            "heading": 2.5768200425700405,
-            "angularVelocity": -2.241375561298711,
-            "velocityX": -2.6701244852844987,
-            "velocityY": 0.8988965183776096,
-            "timestamp": 0.34689379692119504
-        },
-        {
-            "x": 6.822163575098835,
-            "y": 1.078219287033468,
-            "heading": 2.4981665441447336,
-            "angularVelocity": -0.9069461503593836,
-            "velocityX": -3.2564787831184523,
-            "velocityY": 1.1396894269420643,
-            "timestamp": 0.4336172461514938
-        },
-        {
-            "x": 6.510888238397409,
-            "y": 1.1818405008088049,
-            "heading": 2.498166488214478,
-            "angularVelocity": -6.449265521718259e-7,
-            "velocityX": -3.5892868591379226,
-            "velocityY": 1.194846546061207,
-            "timestamp": 0.5203406953817925
-        },
-        {
-            "x": 6.199612902811833,
-            "y": 1.2854617179365646,
-            "heading": 2.498166432284442,
-            "angularVelocity": -6.449240327985389e-7,
-            "velocityX": -3.589286846271159,
-            "velocityY": 1.194846584717682,
-            "timestamp": 0.6070641446120912
-        },
-        {
-            "x": 5.8883375891897005,
-            "y": 1.389083001041933,
-            "heading": 2.4981663763544435,
-            "angularVelocity": -6.449235898174935e-7,
-            "velocityX": -3.5892865930127487,
-            "velocityY": 1.1948473454993283,
-            "timestamp": 0.69378759384239
-        },
-        {
-            "x": 5.577064983818878,
-            "y": 1.4927124193076864,
-            "heading": 2.4981663204239246,
-            "angularVelocity": -6.449295917693127e-7,
-            "velocityX": -3.589255364419621,
-            "velocityY": 1.194941151274556,
-            "timestamp": 0.7805110430726887
-        },
-        {
-            "x": 5.275080475665308,
-            "y": 1.6209116670289339,
-            "heading": 2.4981662550669967,
-            "angularVelocity": -7.536246362770858e-7,
-            "velocityX": -3.4821551821772507,
-            "velocityY": 1.4782535618573864,
-            "timestamp": 0.8672344923029874
-        },
-        {
-            "x": 4.987205237443244,
-            "y": 1.7782569230783876,
-            "heading": 2.49816614132643,
-            "angularVelocity": -0.000001311531862273131,
-            "velocityX": -3.319462507281,
-            "velocityY": 1.8143334639702267,
-            "timestamp": 0.9539579415332862
-        },
-        {
-            "x": 4.728299565339017,
-            "y": 1.9797451797609154,
-            "heading": 2.498165933556362,
-            "angularVelocity": -0.0000023957772582798155,
-            "velocityX": -2.9854171438302592,
-            "velocityY": 2.3233422848238554,
-            "timestamp": 1.040681390763585
-        },
-        {
-            "x": 4.523329416895473,
-            "y": 2.207660623196326,
-            "heading": 2.432888839209935,
-            "angularVelocity": -0.7527040832183707,
-            "velocityX": -2.3634916537882953,
-            "velocityY": 2.6280717090733883,
-            "timestamp": 1.1274048399938839
-        },
-        {
-            "x": 4.381013836981984,
-            "y": 2.433217084695142,
-            "heading": 2.3632823552491717,
-            "angularVelocity": -0.8026258708405342,
-            "velocityX": -1.6410276710231155,
-            "velocityY": 2.600870508503859,
-            "timestamp": 1.2141282892241827
-        },
-        {
-            "x": 4.294783443237854,
-            "y": 2.6401553266407616,
-            "heading": 2.3696134751420326,
-            "angularVelocity": 0.07300355266138053,
-            "velocityX": -0.9943146232011729,
-            "velocityY": 2.386185556297287,
-            "timestamp": 1.3008517384544815
-        },
-        {
-            "x": 4.26053013180991,
-            "y": 2.816849773520389,
-            "heading": 2.4414538337655816,
-            "angularVelocity": 0.8283844711108445,
-            "velocityX": -0.39497173753989284,
-            "velocityY": 2.0374471777570338,
-            "timestamp": 1.3875751876847804
-        },
-        {
-            "x": 4.257469858369506,
-            "y": 2.9392038098412794,
-            "heading": 2.522566294218638,
-            "angularVelocity": 0.9353002120298306,
-            "velocityX": -0.035287727455096035,
-            "velocityY": 1.4108529746778473,
-            "timestamp": 1.4742986369150792
+            "x": 7.629395539536042,
+            "y": 0.7881897280541793,
+            "heading": -0.02207380596023684,
+            "angularVelocity": -0.2822732032732244,
+            "velocityX": -0.6471146471374848,
+            "velocityY": 0.015213885156899113,
+            "timestamp": 0.0782001468948451
+        },
+        {
+            "x": 7.528184091504648,
+            "y": 0.7905692108612611,
+            "heading": -0.06614402373093474,
+            "angularVelocity": -0.5635567133951742,
+            "velocityX": -1.2942616101155584,
+            "velocityY": 0.030428111729043,
+            "timestamp": 0.1564002937896902
+        },
+        {
+            "x": 7.376360973312952,
+            "y": 0.7941378827255743,
+            "heading": -0.13206235338942335,
+            "angularVelocity": -0.8429438086302324,
+            "velocityX": -1.9414684526853105,
+            "velocityY": 0.04563510435477082,
+            "timestamp": 0.23460044068453528
+        },
+        {
+            "x": 7.173916466766183,
+            "y": 0.7988917309385671,
+            "heading": -0.21951772550448787,
+            "angularVelocity": -1.1183530413716716,
+            "velocityX": -2.588799568612108,
+            "velocityY": 0.060790783668257076,
+            "timestamp": 0.3128005875793804
+        },
+        {
+            "x": 6.920824289019536,
+            "y": 0.8048108023879232,
+            "heading": -0.3276760191057486,
+            "angularVelocity": -1.383095785572821,
+            "velocityX": -3.236466781667369,
+            "velocityY": 0.07569130857196414,
+            "timestamp": 0.3910007344742255
+        },
+        {
+            "x": 6.625087122175782,
+            "y": 0.8120803100358527,
+            "heading": -0.3276760358371292,
+            "angularVelocity": -2.1395587179409026e-7,
+            "velocityX": -3.781798098684327,
+            "velocityY": 0.09296028123113304,
+            "timestamp": 0.4692008813690706
+        },
+        {
+            "x": 6.329343587367458,
+            "y": 0.8190859886904246,
+            "heading": -0.3276760513572638,
+            "angularVelocity": -1.9846682128968708e-7,
+            "velocityX": -3.78187953030339,
+            "velocityY": 0.08958651527359804,
+            "timestamp": 0.5474010282639157
+        },
+        {
+            "x": 6.033600055760057,
+            "y": 0.826091802470254,
+            "heading": -0.3276760668773978,
+            "angularVelocity": -1.9846681457625392e-7,
+            "velocityX": -3.7818794893709464,
+            "velocityY": 0.0895882432148227,
+            "timestamp": 0.6256011751587608
+        },
+        {
+            "x": 5.7383102527596765,
+            "y": 0.8439033174946344,
+            "heading": -0.3276760825327013,
+            "angularVelocity": -2.0019532088764418e-7,
+            "velocityX": -3.7760773441696487,
+            "velocityY": 0.22776830647924015,
+            "timestamp": 0.7038013220536059
+        },
+        {
+            "x": 5.450465299571764,
+            "y": 0.9121572529445121,
+            "heading": -0.32767609968005607,
+            "angularVelocity": -2.1927522433491614e-7,
+            "velocityX": -3.680874839978332,
+            "velocityY": 0.8728108342551084,
+            "timestamp": 0.782001468948451
+        },
+        {
+            "x": 5.178616904201158,
+            "y": 1.028826716826592,
+            "heading": -0.32767612118071976,
+            "angularVelocity": -2.749440322201668e-7,
+            "velocityX": -3.4763156613508643,
+            "velocityY": 1.4919340757663395,
+            "timestamp": 0.8602016158432961
+        },
+        {
+            "x": 4.930841529412244,
+            "y": 1.190445624246428,
+            "heading": -0.3276761534331376,
+            "angularVelocity": -4.1243423626599064e-7,
+            "velocityX": -3.1684771017365216,
+            "velocityY": 2.0667340642903906,
+            "timestamp": 0.9384017627381412
+        },
+        {
+            "x": 4.7145004488669215,
+            "y": 1.3922123936122235,
+            "heading": -0.32767621878386327,
+            "angularVelocity": -8.356854591223944e-7,
+            "velocityX": -2.7665047846524513,
+            "velocityY": 2.5801328690255976,
+            "timestamp": 1.0166019096329864
+        },
+        {
+            "x": 4.536026477813721,
+            "y": 1.628126621246338,
+            "heading": -0.327696539002069,
+            "angularVelocity": -0.00025984882823803823,
+            "velocityX": -2.282271557538034,
+            "velocityY": 3.016800313073996,
+            "timestamp": 1.0948020565278314
+        },
+        {
+            "x": 4.4347589269746495,
+            "y": 1.8009845745755027,
+            "heading": -0.3311064514524199,
+            "angularVelocity": -0.06394544615832723,
+            "velocityX": -1.899051313343304,
+            "velocityY": 3.241572651564751,
+            "timestamp": 1.1481273933038434
+        },
+        {
+            "x": 4.355560152796487,
+            "y": 1.9823487857199633,
+            "heading": -0.3404702376957046,
+            "angularVelocity": -0.17559732033810904,
+            "velocityX": -1.4851996999249673,
+            "velocityY": 3.4010889027473166,
+            "timestamp": 1.2014527300798554
+        },
+        {
+            "x": 4.29961502910342,
+            "y": 2.1668323093896307,
+            "heading": -0.3620606217709223,
+            "angularVelocity": -0.40488040734269476,
+            "velocityX": -1.049128370778876,
+            "velocityY": 3.4595847832064672,
+            "timestamp": 1.2547780668558675
+        },
+        {
+            "x": 4.26624261518445,
+            "y": 2.3445509425061104,
+            "heading": -0.3930653835662206,
+            "angularVelocity": -0.5814264600999738,
+            "velocityX": -0.6258265945712352,
+            "velocityY": 3.332724064417915,
+            "timestamp": 1.3081034036318795
+        },
+        {
+            "x": 4.250245555353669,
+            "y": 2.506179624728893,
+            "heading": -0.42827286207411436,
+            "angularVelocity": -0.6602392152823773,
+            "velocityX": -0.2999898509389163,
+            "velocityY": 3.0309922448647635,
+            "timestamp": 1.3614287404078915
+        },
+        {
+            "x": 4.245253885327146,
+            "y": 2.646807044638539,
+            "heading": -0.46326114250173545,
+            "angularVelocity": -0.6561286349527023,
+            "velocityX": -0.09360784813172378,
+            "velocityY": 2.6371595270071015,
+            "timestamp": 1.4147540771839036
+        },
+        {
+            "x": 4.246491895421447,
+            "y": 2.764573131222025,
+            "heading": -0.49520995766037457,
+            "angularVelocity": -0.5991301150594935,
+            "velocityX": 0.023216170194527277,
+            "velocityY": 2.2084452476729117,
+            "timestamp": 1.4680794139599156
+        },
+        {
+            "x": 4.250760184591696,
+            "y": 2.858861309855261,
+            "heading": -0.522398988898447,
+            "angularVelocity": -0.5098707834110707,
+            "velocityX": 0.08004242313776105,
+            "velocityY": 1.7681684605074017,
+            "timestamp": 1.5214047507359276
+        },
+        {
+            "x": 4.255888225984497,
+            "y": 2.929511890736146,
+            "heading": -0.5437259487715407,
+            "angularVelocity": -0.3999404628668233,
+            "velocityX": 0.09616519468543994,
+            "velocityY": 1.324897040550944,
+            "timestamp": 1.5747300875119397
+        },
+        {
+            "x": 4.260343621579396,
+            "y": 2.9765343465211713,
+            "heading": -0.5584397105575287,
+            "angularVelocity": -0.2759244043319095,
+            "velocityX": 0.08355119468920533,
+            "velocityY": 0.8818032595380478,
+            "timestamp": 1.6280554242879517
         },
         {
             "x": 4.263,
             "y": 3,
-            "heading": 2.574,
-            "angularVelocity": 0.5930772615463757,
-            "velocityX": 0.06376754706571511,
-            "velocityY": 0.7010351951900964,
-            "timestamp": 1.561022086145378
+            "heading": -0.566,
+            "angularVelocity": -0.14177668439456792,
+            "velocityX": 0.04981456435476623,
+            "velocityY": 0.4400469813703916,
+            "timestamp": 1.6813807610639637
         },
         {
             "x": 4.263,
             "y": 3,
-            "heading": 2.574,
-            "angularVelocity": -5.347216163770528e-32,
-            "velocityX": 0,
-            "velocityY": -5.6953276102603335e-33,
-            "timestamp": 1.647745535375677
+            "heading": -0.566,
+            "angularVelocity": 0,
+            "velocityX": 3.549235715029611e-32,
+            "velocityY": -9.598836837089386e-32,
+            "timestamp": 1.7347060978399758
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.1.traj
index 0acd095d..773c6f5e 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.1.traj	
@@ -3,146 +3,146 @@
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 7.117315018424497e-31,
-            "velocityX": 3.987995408855834e-32,
-            "velocityY": -2.0629863005203966e-31,
+            "heading": 0,
+            "angularVelocity": 2.8467808772943154e-31,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.633523057017953,
-            "y": 0.8024232011655344,
-            "heading": 3.1133262335807257,
-            "angularVelocity": -0.3460902136380606,
-            "velocityX": -0.6030350147430711,
-            "velocityY": 0.20011493324412974,
-            "timestamp": 0.07707171531631223
+            "x": 7.633526252649942,
+            "y": 0.8024218861992577,
+            "heading": -0.02675508445787236,
+            "angularVelocity": -0.3471453590528024,
+            "velocityX": -0.602993637929559,
+            "velocityY": 0.20009790028293237,
+            "timestamp": 0.07707170428800919
         },
         {
-            "x": 7.540480034234958,
-            "y": 0.8333108599665054,
-            "heading": 3.0624083977372165,
-            "angularVelocity": -0.6606552823501594,
-            "velocityX": -1.2072265733432221,
-            "velocityY": 0.4007651662377571,
-            "timestamp": 0.15414343063262445
+            "x": 7.540488865885011,
+            "y": 0.8333059401759839,
+            "heading": -0.07783349665964928,
+            "angularVelocity": -0.6627388439588938,
+            "velocityX": -1.2071536191448495,
+            "velocityY": 0.400718451240102,
+            "timestamp": 0.15414340857601838
         },
         {
-            "x": 7.40074665357272,
-            "y": 0.8797155912265175,
-            "heading": 2.9910865272302494,
-            "angularVelocity": -0.9253961743845064,
-            "velocityX": -1.813030631130417,
-            "velocityY": 0.6020980728086949,
-            "timestamp": 0.23121514594893666
+            "x": 7.400762361365792,
+            "y": 0.8797032018771439,
+            "heading": -0.14939031986058435,
+            "angularVelocity": -0.9284448016555393,
+            "velocityX": -1.8129416730824437,
+            "velocityY": 0.6020012419574645,
+            "timestamp": 0.23121511286402757
         },
         {
-            "x": 7.214152290317413,
-            "y": 0.941701209252225,
-            "heading": 2.9061891700546396,
-            "angularVelocity": -1.101537144037597,
-            "velocityX": -2.4210485323896127,
-            "velocityY": 0.804258965449392,
-            "timestamp": 0.3082868612652489
+            "x": 7.214174023420098,
+            "y": 0.9416744881041137,
+            "heading": -0.23458534610519022,
+            "angularVelocity": -1.105399537114693,
+            "velocityX": -2.4209707008480317,
+            "velocityY": 0.8040731264406625,
+            "timestamp": 0.30828681715203676
         },
         {
-            "x": 6.98056152298191,
-            "y": 1.0193157827445936,
-            "heading": 2.8228339071694943,
-            "angularVelocity": -1.0815285807905615,
-            "velocityX": -3.0308235177693503,
-            "velocityY": 1.007043546050951,
-            "timestamp": 0.38535857658156114
+            "x": 6.98058449689491,
+            "y": 1.0192605514016468,
+            "heading": -0.3182722929391017,
+            "angularVelocity": -1.085832311702643,
+            "velocityX": -3.030807852026836,
+            "velocityY": 1.0066737723562156,
+            "timestamp": 0.38535852144004595
         },
         {
-            "x": 6.704462217906169,
-            "y": 1.1110743550526192,
-            "heading": 2.820724227783145,
-            "angularVelocity": -0.027372939316202514,
-            "velocityX": -3.582368758014451,
-            "velocityY": 1.1905609201953868,
-            "timestamp": 0.4624302918978734
+            "x": 6.7044949607857305,
+            "y": 1.1109419048147462,
+            "heading": -0.3204977394277859,
+            "angularVelocity": -0.02887501333002772,
+            "velocityX": -3.5822425189594846,
+            "velocityY": 1.189559180766205,
+            "timestamp": 0.46243022572805514
         },
         {
-            "x": 6.4277914338210325,
-            "y": 1.2030481222509408,
-            "heading": 2.8207241988506935,
-            "angularVelocity": -3.753964944317793e-7,
-            "velocityX": -3.589783657333227,
-            "velocityY": 1.1933530585228214,
-            "timestamp": 0.5395020072141856
+            "x": 6.427835578289735,
+            "y": 1.202949830409229,
+            "heading": -0.32049776831244,
+            "angularVelocity": -3.7477637567079044e-7,
+            "velocityX": -3.5896362361749183,
+            "velocityY": 1.1937964320946928,
+            "timestamp": 0.5395019300160644
         },
         {
-            "x": 6.151120668434435,
-            "y": 1.2950219456972574,
-            "heading": 2.8207241699182544,
-            "angularVelocity": -3.753963293976773e-7,
-            "velocityX": -3.5897834147210324,
-            "velocityY": 1.1933537883365417,
-            "timestamp": 0.6165737225304978
+            "x": 6.1511761970625,
+            "y": 1.294957759818759,
+            "heading": -0.3204977971970938,
+            "angularVelocity": -3.7477637370825724e-7,
+            "velocityX": -3.589636219712834,
+            "velocityY": 1.1937964815946673,
+            "timestamp": 0.6165736343040735
         },
         {
-            "x": 5.875226516165077,
-            "y": 1.387237765393963,
-            "heading": 2.8184428738821614,
-            "angularVelocity": -0.029599653085833715,
-            "velocityX": -3.5797069150083383,
-            "velocityY": 1.1964936723964186,
-            "timestamp": 0.69364543784681
+            "x": 5.87524941446386,
+            "y": 1.3871813457379307,
+            "heading": -0.32266464886855784,
+            "angularVelocity": -0.028114749653993878,
+            "velocityX": -3.5801308034856563,
+            "velocityY": 1.1965946098005253,
+            "timestamp": 0.6936453385920827
         },
         {
-            "x": 5.641686532929837,
-            "y": 1.4649564505690207,
-            "heading": 2.7347434741228605,
-            "angularVelocity": -1.0859937321465907,
-            "velocityX": -3.0301645976966087,
-            "velocityY": 1.0083943876958055,
-            "timestamp": 0.7707171531631222
+            "x": 5.641703675217654,
+            "y": 1.4649149367510799,
+            "heading": -0.4060390277887848,
+            "angularVelocity": -1.081776764773039,
+            "velocityX": -3.030239715129037,
+            "velocityY": 1.0085879342004245,
+            "timestamp": 0.7707170428800918
         },
         {
-            "x": 5.455142642963306,
-            "y": 1.5270452896537086,
-            "heading": 2.6494963329119967,
-            "angularVelocity": -1.1060755669054276,
-            "velocityX": -2.4203936450737,
-            "velocityY": 0.8055982513152498,
-            "timestamp": 0.8477888684794344
+            "x": 5.455154020778328,
+            "y": 1.5270189169325792,
+            "heading": -0.49098864699750666,
+            "angularVelocity": -1.1022153979010723,
+            "velocityX": -2.420468785044752,
+            "velocityY": 0.8057948212669979,
+            "timestamp": 0.8477887471681009
         },
         {
-            "x": 5.315456646631149,
-            "y": 1.5735510161760224,
-            "heading": 2.577919230350482,
-            "angularVelocity": -0.928707792057741,
-            "velocityX": -1.812415822832897,
-            "velocityY": 0.6034084791216642,
-            "timestamp": 0.9248605837957465
+            "x": 5.315462796839212,
+            "y": 1.573537321073397,
+            "heading": -0.5623285861822422,
+            "angularVelocity": -0.9256307466375128,
+            "velocityX": -1.8124839100106522,
+            "velocityY": 0.6035730566821683,
+            "timestamp": 0.9248604514561101
         },
         {
-            "x": 5.222452869215715,
-            "y": 1.6045247487520136,
-            "heading": 2.5268085106209885,
-            "angularVelocity": -0.6631579369906109,
-            "velocityX": -1.2067173675029015,
-            "velocityY": 0.4018819673192824,
-            "timestamp": 1.0019322991120587
+            "x": 5.222455052636063,
+            "y": 1.6045200564296747,
+            "heading": -0.613275527154411,
+            "angularVelocity": -0.6610330139033284,
+            "velocityX": -1.2067690089684353,
+            "velocityY": 0.40199883527290925,
+            "timestamp": 1.0019321557441192
         },
         {
             "x": 5.176,
             "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.3478385100287815,
-            "velocityX": -0.6027226593448243,
-            "velocityY": 0.200790279345336,
-            "timestamp": 1.079004014428371
+            "heading": -0.64,
+            "angularVelocity": -0.34674817551357506,
+            "velocityX": -0.602751075316368,
+            "velocityY": 0.2008511906325378,
+            "timestamp": 1.0790038600321283
         },
         {
             "x": 5.176,
             "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -3.134288783198311e-31,
-            "velocityX": -2.9694883024269136e-32,
-            "velocityY": 4.283237987843841e-31,
-            "timestamp": 1.1560757297446833
+            "heading": -0.64,
+            "angularVelocity": 8.760742141155177e-29,
+            "velocityX": 3.313743560366677e-30,
+            "velocityY": 1.2206105198294578e-30,
+            "timestamp": 1.1560755643201375
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.traj
index 0acd095d..773c6f5e 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.traj	
@@ -3,146 +3,146 @@
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 7.117315018424497e-31,
-            "velocityX": 3.987995408855834e-32,
-            "velocityY": -2.0629863005203966e-31,
+            "heading": 0,
+            "angularVelocity": 2.8467808772943154e-31,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.633523057017953,
-            "y": 0.8024232011655344,
-            "heading": 3.1133262335807257,
-            "angularVelocity": -0.3460902136380606,
-            "velocityX": -0.6030350147430711,
-            "velocityY": 0.20011493324412974,
-            "timestamp": 0.07707171531631223
+            "x": 7.633526252649942,
+            "y": 0.8024218861992577,
+            "heading": -0.02675508445787236,
+            "angularVelocity": -0.3471453590528024,
+            "velocityX": -0.602993637929559,
+            "velocityY": 0.20009790028293237,
+            "timestamp": 0.07707170428800919
         },
         {
-            "x": 7.540480034234958,
-            "y": 0.8333108599665054,
-            "heading": 3.0624083977372165,
-            "angularVelocity": -0.6606552823501594,
-            "velocityX": -1.2072265733432221,
-            "velocityY": 0.4007651662377571,
-            "timestamp": 0.15414343063262445
+            "x": 7.540488865885011,
+            "y": 0.8333059401759839,
+            "heading": -0.07783349665964928,
+            "angularVelocity": -0.6627388439588938,
+            "velocityX": -1.2071536191448495,
+            "velocityY": 0.400718451240102,
+            "timestamp": 0.15414340857601838
         },
         {
-            "x": 7.40074665357272,
-            "y": 0.8797155912265175,
-            "heading": 2.9910865272302494,
-            "angularVelocity": -0.9253961743845064,
-            "velocityX": -1.813030631130417,
-            "velocityY": 0.6020980728086949,
-            "timestamp": 0.23121514594893666
+            "x": 7.400762361365792,
+            "y": 0.8797032018771439,
+            "heading": -0.14939031986058435,
+            "angularVelocity": -0.9284448016555393,
+            "velocityX": -1.8129416730824437,
+            "velocityY": 0.6020012419574645,
+            "timestamp": 0.23121511286402757
         },
         {
-            "x": 7.214152290317413,
-            "y": 0.941701209252225,
-            "heading": 2.9061891700546396,
-            "angularVelocity": -1.101537144037597,
-            "velocityX": -2.4210485323896127,
-            "velocityY": 0.804258965449392,
-            "timestamp": 0.3082868612652489
+            "x": 7.214174023420098,
+            "y": 0.9416744881041137,
+            "heading": -0.23458534610519022,
+            "angularVelocity": -1.105399537114693,
+            "velocityX": -2.4209707008480317,
+            "velocityY": 0.8040731264406625,
+            "timestamp": 0.30828681715203676
         },
         {
-            "x": 6.98056152298191,
-            "y": 1.0193157827445936,
-            "heading": 2.8228339071694943,
-            "angularVelocity": -1.0815285807905615,
-            "velocityX": -3.0308235177693503,
-            "velocityY": 1.007043546050951,
-            "timestamp": 0.38535857658156114
+            "x": 6.98058449689491,
+            "y": 1.0192605514016468,
+            "heading": -0.3182722929391017,
+            "angularVelocity": -1.085832311702643,
+            "velocityX": -3.030807852026836,
+            "velocityY": 1.0066737723562156,
+            "timestamp": 0.38535852144004595
         },
         {
-            "x": 6.704462217906169,
-            "y": 1.1110743550526192,
-            "heading": 2.820724227783145,
-            "angularVelocity": -0.027372939316202514,
-            "velocityX": -3.582368758014451,
-            "velocityY": 1.1905609201953868,
-            "timestamp": 0.4624302918978734
+            "x": 6.7044949607857305,
+            "y": 1.1109419048147462,
+            "heading": -0.3204977394277859,
+            "angularVelocity": -0.02887501333002772,
+            "velocityX": -3.5822425189594846,
+            "velocityY": 1.189559180766205,
+            "timestamp": 0.46243022572805514
         },
         {
-            "x": 6.4277914338210325,
-            "y": 1.2030481222509408,
-            "heading": 2.8207241988506935,
-            "angularVelocity": -3.753964944317793e-7,
-            "velocityX": -3.589783657333227,
-            "velocityY": 1.1933530585228214,
-            "timestamp": 0.5395020072141856
+            "x": 6.427835578289735,
+            "y": 1.202949830409229,
+            "heading": -0.32049776831244,
+            "angularVelocity": -3.7477637567079044e-7,
+            "velocityX": -3.5896362361749183,
+            "velocityY": 1.1937964320946928,
+            "timestamp": 0.5395019300160644
         },
         {
-            "x": 6.151120668434435,
-            "y": 1.2950219456972574,
-            "heading": 2.8207241699182544,
-            "angularVelocity": -3.753963293976773e-7,
-            "velocityX": -3.5897834147210324,
-            "velocityY": 1.1933537883365417,
-            "timestamp": 0.6165737225304978
+            "x": 6.1511761970625,
+            "y": 1.294957759818759,
+            "heading": -0.3204977971970938,
+            "angularVelocity": -3.7477637370825724e-7,
+            "velocityX": -3.589636219712834,
+            "velocityY": 1.1937964815946673,
+            "timestamp": 0.6165736343040735
         },
         {
-            "x": 5.875226516165077,
-            "y": 1.387237765393963,
-            "heading": 2.8184428738821614,
-            "angularVelocity": -0.029599653085833715,
-            "velocityX": -3.5797069150083383,
-            "velocityY": 1.1964936723964186,
-            "timestamp": 0.69364543784681
+            "x": 5.87524941446386,
+            "y": 1.3871813457379307,
+            "heading": -0.32266464886855784,
+            "angularVelocity": -0.028114749653993878,
+            "velocityX": -3.5801308034856563,
+            "velocityY": 1.1965946098005253,
+            "timestamp": 0.6936453385920827
         },
         {
-            "x": 5.641686532929837,
-            "y": 1.4649564505690207,
-            "heading": 2.7347434741228605,
-            "angularVelocity": -1.0859937321465907,
-            "velocityX": -3.0301645976966087,
-            "velocityY": 1.0083943876958055,
-            "timestamp": 0.7707171531631222
+            "x": 5.641703675217654,
+            "y": 1.4649149367510799,
+            "heading": -0.4060390277887848,
+            "angularVelocity": -1.081776764773039,
+            "velocityX": -3.030239715129037,
+            "velocityY": 1.0085879342004245,
+            "timestamp": 0.7707170428800918
         },
         {
-            "x": 5.455142642963306,
-            "y": 1.5270452896537086,
-            "heading": 2.6494963329119967,
-            "angularVelocity": -1.1060755669054276,
-            "velocityX": -2.4203936450737,
-            "velocityY": 0.8055982513152498,
-            "timestamp": 0.8477888684794344
+            "x": 5.455154020778328,
+            "y": 1.5270189169325792,
+            "heading": -0.49098864699750666,
+            "angularVelocity": -1.1022153979010723,
+            "velocityX": -2.420468785044752,
+            "velocityY": 0.8057948212669979,
+            "timestamp": 0.8477887471681009
         },
         {
-            "x": 5.315456646631149,
-            "y": 1.5735510161760224,
-            "heading": 2.577919230350482,
-            "angularVelocity": -0.928707792057741,
-            "velocityX": -1.812415822832897,
-            "velocityY": 0.6034084791216642,
-            "timestamp": 0.9248605837957465
+            "x": 5.315462796839212,
+            "y": 1.573537321073397,
+            "heading": -0.5623285861822422,
+            "angularVelocity": -0.9256307466375128,
+            "velocityX": -1.8124839100106522,
+            "velocityY": 0.6035730566821683,
+            "timestamp": 0.9248604514561101
         },
         {
-            "x": 5.222452869215715,
-            "y": 1.6045247487520136,
-            "heading": 2.5268085106209885,
-            "angularVelocity": -0.6631579369906109,
-            "velocityX": -1.2067173675029015,
-            "velocityY": 0.4018819673192824,
-            "timestamp": 1.0019322991120587
+            "x": 5.222455052636063,
+            "y": 1.6045200564296747,
+            "heading": -0.613275527154411,
+            "angularVelocity": -0.6610330139033284,
+            "velocityX": -1.2067690089684353,
+            "velocityY": 0.40199883527290925,
+            "timestamp": 1.0019321557441192
         },
         {
             "x": 5.176,
             "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -0.3478385100287815,
-            "velocityX": -0.6027226593448243,
-            "velocityY": 0.200790279345336,
-            "timestamp": 1.079004014428371
+            "heading": -0.64,
+            "angularVelocity": -0.34674817551357506,
+            "velocityX": -0.602751075316368,
+            "velocityY": 0.2008511906325378,
+            "timestamp": 1.0790038600321283
         },
         {
             "x": 5.176,
             "y": 1.62,
-            "heading": 2.5,
-            "angularVelocity": -3.134288783198311e-31,
-            "velocityX": -2.9694883024269136e-32,
-            "velocityY": 4.283237987843841e-31,
-            "timestamp": 1.1560757297446833
+            "heading": -0.64,
+            "angularVelocity": 8.760742141155177e-29,
+            "velocityX": 3.313743560366677e-30,
+            "velocityY": 1.2206105198294578e-30,
+            "timestamp": 1.1560755643201375
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.1.traj
index 6f37c8f6..bec33f37 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.1.traj	
@@ -3,263 +3,308 @@
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": -1.8495960722802552e-34,
-            "velocityY": 0,
+            "heading": 0,
+            "angularVelocity": -3.516607019090483e-32,
+            "velocityX": -4.092268936373955e-33,
+            "velocityY": -4.396806616029776e-33,
             "timestamp": 0
         },
         {
-            "x": 7.66217209907118,
-            "y": 0.8633707808076446,
-            "heading": 3.200268562588625,
-            "angularVelocity": 0.6146717037531945,
-            "velocityX": -0.18182458262791865,
-            "velocityY": 0.7788962593386439,
-            "timestamp": 0.09805000331172546
-        },
-        {
-            "x": 7.626250868838738,
-            "y": 1.0164003464766511,
-            "heading": 3.3145470628036264,
-            "angularVelocity": 1.1655124564522605,
-            "velocityX": -0.3663562368094907,
-            "velocityY": 1.5607298368209865,
-            "timestamp": 0.19610000662345092
-        },
-        {
-            "x": 7.571819523432209,
-            "y": 1.2466141084056221,
-            "heading": 3.4692056792172115,
-            "angularVelocity": 1.5773443262606215,
-            "velocityX": -0.555138639143922,
-            "velocityY": 2.347922020941335,
-            "timestamp": 0.29415000993517637
-        },
-        {
-            "x": 7.4978548812850345,
-            "y": 1.55456588878344,
-            "heading": 3.620198528248394,
-            "angularVelocity": 1.5399576127615076,
-            "velocityX": -0.7543563452213444,
-            "velocityY": 3.1407625698773547,
-            "timestamp": 0.39220001324690185
-        },
-        {
-            "x": 7.414564781883039,
-            "y": 1.916010778899352,
-            "heading": 3.6201985877633485,
-            "angularVelocity": 6.069857444827113e-7,
-            "velocityX": -0.8494655439959167,
-            "velocityY": 3.6863322581110816,
-            "timestamp": 0.4902500165586273
-        },
-        {
-            "x": 7.3312746982625,
-            "y": 2.27745567265221,
-            "heading": 3.620198647277482,
-            "angularVelocity": 6.069773758970038e-7,
-            "velocityX": -0.8494653830427515,
-            "velocityY": 3.686332295203854,
-            "timestamp": 0.5883000198703527
-        },
-        {
-            "x": 7.247984475288188,
-            "y": 2.6389005342928713,
-            "heading": 3.6201987067915797,
-            "angularVelocity": 6.069770050551867e-7,
-            "velocityX": -0.8494668042948591,
-            "velocityY": 3.686331967695479,
-            "timestamp": 0.6863500231820782
-        },
-        {
-            "x": 7.16025127777078,
-            "y": 2.999292730685755,
-            "heading": 3.620198766578442,
-            "angularVelocity": 6.097589007693709e-7,
-            "velocityX": -0.8947801586348038,
-            "velocityY": 3.675595963491274,
-            "timestamp": 0.7844000264938036
-        },
-        {
-            "x": 6.997140841431304,
-            "y": 3.3324212817743484,
-            "heading": 3.6201988348555445,
-            "angularVelocity": 6.963498230184926e-7,
-            "velocityX": -1.6635434046943107,
-            "velocityY": 3.3975373772247037,
-            "timestamp": 0.882450029805529
-        },
-        {
-            "x": 6.766266862430273,
-            "y": 3.622726019325249,
-            "heading": 3.620198931073779,
-            "angularVelocity": 9.813180151910862e-7,
-            "velocityX": -2.354655494166845,
-            "velocityY": 2.9607825369260774,
-            "timestamp": 0.9805000331172544
-        },
-        {
-            "x": 6.478412432979381,
-            "y": 3.856647861556034,
-            "heading": 3.62019912006771,
-            "angularVelocity": 0.000001927525998631886,
-            "velocityX": -2.935792144093369,
-            "velocityY": 2.38574027873399,
-            "timestamp": 1.07855003642898
-        },
-        {
-            "x": 6.147022412583985,
-            "y": 4.023260998419484,
-            "heading": 3.6202003231426523,
-            "angularVelocity": 0.000012270014292625344,
-            "velocityX": -3.3798063151698643,
-            "velocityY": 1.6992670192345174,
-            "timestamp": 1.1766000397407055
-        },
-        {
-            "x": 5.787574046048744,
-            "y": 4.114783912760558,
-            "heading": 3.6201999548728017,
-            "angularVelocity": -0.0000037559391933502495,
-            "velocityX": -3.6659699581290637,
-            "velocityY": 0.9334310173361262,
-            "timestamp": 1.274650043052431
-        },
-        {
-            "x": 5.417217638191157,
-            "y": 4.135174501302961,
-            "heading": 3.620199996859582,
-            "angularVelocity": 4.2821804063172526e-7,
-            "velocityX": -3.7772197383831925,
-            "velocityY": 0.2079611203844201,
-            "timestamp": 1.3727000463641565
-        },
-        {
-            "x": 5.066383608065517,
-            "y": 4.23317852630269,
-            "heading": 3.636482368230358,
-            "angularVelocity": 0.16606191556170027,
-            "velocityX": -3.578113393940955,
-            "velocityY": 0.9995310728154599,
-            "timestamp": 1.470750049675882
-        },
-        {
-            "x": 4.742245396435021,
-            "y": 4.406733140775732,
-            "heading": 3.644586480575013,
-            "angularVelocity": 0.08265285130985205,
-            "velocityX": -3.305846003900474,
-            "velocityY": 1.7700623009799354,
-            "timestamp": 1.5688000529876076
-        },
-        {
-            "x": 4.460670694136194,
-            "y": 4.648177372257919,
-            "heading": 3.644586687046199,
-            "angularVelocity": 0.000002105774391224538,
-            "velocityX": -2.87174597438442,
-            "velocityY": 2.4624602073145714,
-            "timestamp": 1.6668500562993331
-        },
-        {
-            "x": 4.237545979665016,
-            "y": 4.944479621179968,
-            "heading": 3.644586791013843,
-            "angularVelocity": 0.0000010603532960842864,
-            "velocityX": -2.2756216923502683,
-            "velocityY": 3.0219504223781635,
-            "timestamp": 1.7649000596110587
-        },
-        {
-            "x": 3.9950087307179025,
-            "y": 5.225113475172281,
-            "heading": 3.6445867953690434,
-            "angularVelocity": 4.441815614419627e-8,
-            "velocityX": -2.473607758849603,
-            "velocityY": 2.862150377497781,
-            "timestamp": 1.8629500629227842
-        },
-        {
-            "x": 3.7198234606165768,
-            "y": 5.473815593691706,
-            "heading": 3.644586791690717,
-            "angularVelocity": -3.7514802268686446e-8,
-            "velocityX": -2.8065809363253424,
-            "velocityY": 2.536482510140642,
-            "timestamp": 1.9610000662345097
-        },
-        {
-            "x": 3.4446380366180587,
-            "y": 5.722517541926013,
-            "heading": 3.6445867880123934,
-            "angularVelocity": -3.751477003592028e-8,
-            "velocityX": -2.8065825059040024,
-            "velocityY": 2.5364807734235555,
-            "timestamp": 2.059050069546235
-        },
-        {
-            "x": 3.1694525996169167,
-            "y": 5.97121947577308,
-            "heading": 3.6445867843340722,
-            "angularVelocity": -3.75147461397837e-8,
-            "velocityX": -2.806582638516174,
-            "velocityY": 2.5364806266898405,
-            "timestamp": 2.1571000728579603
-        },
-        {
-            "x": 2.894267154657697,
-            "y": 6.2199214008146,
-            "heading": 3.6445867806557373,
-            "angularVelocity": -3.751489444763293e-8,
-            "velocityX": -2.806582719679637,
-            "velocityY": 2.53648053688316,
-            "timestamp": 2.2551500761696857
-        },
-        {
-            "x": 2.6565603153930746,
-            "y": 6.434752825744944,
-            "heading": 3.60002452914154,
-            "angularVelocity": -0.45448495674724715,
-            "velocityX": -2.4243430008757167,
-            "velocityY": 2.191039445938014,
-            "timestamp": 2.353200079481411
-        },
-        {
-            "x": 2.478280163108621,
-            "y": 6.595876410073326,
-            "heading": 3.56660458464388,
-            "angularVelocity": -0.3408459293102701,
-            "velocityX": -1.8182574835582244,
-            "velocityY": 1.6432797438683373,
-            "timestamp": 2.4512500827931363
-        },
-        {
-            "x": 2.3594267213059505,
-            "y": 6.703292136714203,
-            "heading": 3.544325120911587,
-            "angularVelocity": -0.2272255275857591,
-            "velocityX": -1.2121717265506466,
-            "velocityY": 1.0955198675452942,
-            "timestamp": 2.5493000861048616
+            "x": 7.6580594556627535,
+            "y": 0.8642766421574168,
+            "heading": -0.06977499787943942,
+            "angularVelocity": -0.7009716360726537,
+            "velocityX": -0.22041848409624198,
+            "velocityY": 0.7763344454252913,
+            "timestamp": 0.09954040119279155
+        },
+        {
+            "x": 7.613877684624128,
+            "y": 1.0194223152597515,
+            "heading": -0.19807907998906352,
+            "angularVelocity": -1.2889648883484262,
+            "velocityX": -0.4438576749661031,
+            "velocityY": 1.558620130552277,
+            "timestamp": 0.1990808023855831
+        },
+        {
+            "x": 7.54706531781781,
+            "y": 1.253315507091588,
+            "heading": -0.3641253773176819,
+            "angularVelocity": -1.6681296773861405,
+            "velocityX": -0.6712085344815377,
+            "velocityY": 2.349731255139591,
+            "timestamp": 0.2986212035783746
+        },
+        {
+            "x": 7.457648537893544,
+            "y": 1.5658706931334108,
+            "heading": -0.49008580394214096,
+            "angularVelocity": -1.265420122031624,
+            "velocityX": -0.8982963585919527,
+            "velocityY": 3.139983185686188,
+            "timestamp": 0.3981616047711662
+        },
+        {
+            "x": 7.353528135913809,
+            "y": 1.9277448806018234,
+            "heading": -0.490085841920944,
+            "angularVelocity": -3.8154159059291386e-7,
+            "velocityX": -1.0460114760646044,
+            "velocityY": 3.6354503611807685,
+            "timestamp": 0.49770200596395775
+        },
+        {
+            "x": 7.249407681423134,
+            "y": 2.2896190529615703,
+            "heading": -0.4900858798995876,
+            "angularVelocity": -3.8153998933914064e-7,
+            "velocityX": -1.0460120035985523,
+            "velocityY": 3.635450209396512,
+            "timestamp": 0.5972424071567493
+        },
+        {
+            "x": 7.145287117295142,
+            "y": 2.651493193775847,
+            "heading": -0.4900859178782245,
+            "angularVelocity": -3.8153992177326955e-7,
+            "velocityX": -1.0460131050338848,
+            "velocityY": 3.6354498924852887,
+            "timestamp": 0.6967828083495408
+        },
+        {
+            "x": 7.025437006860337,
+            "y": 3.00846649318931,
+            "heading": -0.49008595631381496,
+            "angularVelocity": -3.8613055604326664e-7,
+            "velocityX": -1.204034834083884,
+            "velocityY": 3.586215196401226,
+            "timestamp": 0.7963232095423323
+        },
+        {
+            "x": 6.830624533504276,
+            "y": 3.3307118703307705,
+            "heading": -0.49008600153372534,
+            "angularVelocity": -4.542870012894294e-7,
+            "velocityX": -1.9571196320451347,
+            "velocityY": 3.237332513029861,
+            "timestamp": 0.8958636107351238
+        },
+        {
+            "x": 6.570226686668516,
+            "y": 3.602717269957059,
+            "heading": -0.4900860357215963,
+            "angularVelocity": -3.4345723556551647e-7,
+            "velocityX": -2.6160015804177474,
+            "velocityY": 2.7326130532612987,
+            "timestamp": 0.9954040119279153
+        },
+        {
+            "x": 6.270670526052627,
+            "y": 3.8032215967371097,
+            "heading": -0.4416564414377051,
+            "angularVelocity": 0.4865320382835506,
+            "velocityX": -3.009392739292897,
+            "velocityY": 2.0143009710369753,
+            "timestamp": 1.094944413120707
+        },
+        {
+            "x": 5.9498699270968665,
+            "y": 3.9255658761230467,
+            "heading": -0.3340733830984328,
+            "angularVelocity": 1.0807979177309486,
+            "velocityX": -3.222818022748657,
+            "velocityY": 1.229091684581206,
+            "timestamp": 1.1944848143134985
+        },
+        {
+            "x": 5.637666976812727,
+            "y": 3.9694128449739723,
+            "heading": -0.13726885518093873,
+            "angularVelocity": 1.977132154976145,
+            "velocityX": -3.136444564649281,
+            "velocityY": 0.4404941945733328,
+            "timestamp": 1.2940252155062901
+        },
+        {
+            "x": 5.333615486534618,
+            "y": 3.942111372133816,
+            "heading": 0.08955200005531357,
+            "angularVelocity": 2.278681344642582,
+            "velocityX": -3.0545535946677287,
+            "velocityY": -0.2742752943830166,
+            "timestamp": 1.3935656166990817
+        },
+        {
+            "x": 5.078082684497777,
+            "y": 3.9650753252247246,
+            "heading": 0.19884455131252843,
+            "angularVelocity": 1.0979717777662474,
+            "velocityX": -2.5671265031564414,
+            "velocityY": 0.2306998245509574,
+            "timestamp": 1.4931060178918734
+        },
+        {
+            "x": 4.8543877601623535,
+            "y": 4.064273357391357,
+            "heading": 0.3064368289582943,
+            "angularVelocity": 1.0808905364705061,
+            "velocityX": -2.2472777048805277,
+            "velocityY": 0.9965605018459247,
+            "timestamp": 1.592646419084665
+        },
+        {
+            "x": 4.700013932982481,
+            "y": 4.187105651970831,
+            "heading": 0.3888797273509616,
+            "angularVelocity": 1.0676410431648378,
+            "velocityX": -1.99915137751061,
+            "velocityY": 1.5906864226746236,
+            "timestamp": 1.6698660978526145
+        },
+        {
+            "x": 4.564169886311024,
+            "y": 4.35607150380987,
+            "heading": 0.4692148602751942,
+            "angularVelocity": 1.040345339504005,
+            "velocityX": -1.7591894817340108,
+            "velocityY": 2.1881190719116157,
+            "timestamp": 1.747085776620564
+        },
+        {
+            "x": 4.445019228108457,
+            "y": 4.571847534724939,
+            "heading": 0.5443267073647724,
+            "angularVelocity": 0.9727034389160626,
+            "velocityX": -1.543008985580261,
+            "velocityY": 2.7943140188848803,
+            "timestamp": 1.8243054553885136
+        },
+        {
+            "x": 4.302573716554072,
+            "y": 4.826880596020871,
+            "heading": 0.5443268349443496,
+            "angularVelocity": 0.0000016521640508021742,
+            "velocityX": -1.8446788930894609,
+            "velocityY": 3.302695185541032,
+            "timestamp": 1.9015251341564632
+        },
+        {
+            "x": 4.139811862406642,
+            "y": 5.06945260129317,
+            "heading": 0.544326895208066,
+            "angularVelocity": 7.80419154390702e-7,
+            "velocityX": -2.1077768872432143,
+            "velocityY": 3.141323677365259,
+            "timestamp": 1.9787448129244127
+        },
+        {
+            "x": 3.9382711614032555,
+            "y": 5.28090933180296,
+            "heading": 0.5443268879285122,
+            "angularVelocity": -9.42707078520697e-8,
+            "velocityX": -2.60996554529876,
+            "velocityY": 2.738378790013248,
+            "timestamp": 2.0559644916923623
+        },
+        {
+            "x": 3.721249786731846,
+            "y": 5.476445328956613,
+            "heading": 0.5443268807879367,
+            "angularVelocity": -9.247093001843253e-8,
+            "velocityX": -2.810441303745562,
+            "velocityY": 2.53220422919983,
+            "timestamp": 2.1331841704603116
+        },
+        {
+            "x": 3.5042282470745802,
+            "y": 5.671981142995692,
+            "heading": 0.5443268736473624,
+            "angularVelocity": -9.247091428352055e-8,
+            "velocityX": -2.810443440323428,
+            "velocityY": 2.53220185785385,
+            "timestamp": 2.210403849228261
+        },
+        {
+            "x": 3.287206691377032,
+            "y": 5.867516939231946,
+            "heading": 0.5443268665067905,
+            "angularVelocity": -9.247088327815795e-8,
+            "velocityX": -2.810443648046155,
+            "velocityY": 2.5322016273060566,
+            "timestamp": 2.2876235279962103
+        },
+        {
+            "x": 3.0701851252220784,
+            "y": 6.063052723861702,
+            "heading": 0.5443268593662193,
+            "angularVelocity": -9.247087442211068e-8,
+            "velocityX": -2.8104437834702534,
+            "velocityY": 2.532201477001163,
+            "timestamp": 2.3648432067641596
+        },
+        {
+            "x": 2.853163562032648,
+            "y": 6.258588511782302,
+            "heading": 0.5443268522252032,
+            "angularVelocity": -9.247663534512907e-8,
+            "velocityX": -2.8104437450665345,
+            "velocityY": 2.5322015196178005,
+            "timestamp": 2.442062885532109
+        },
+        {
+            "x": 2.6687757934039245,
+            "y": 6.424725614600548,
+            "heading": 0.4938774787934042,
+            "angularVelocity": -0.6533227570578579,
+            "velocityX": -2.3878339248576954,
+            "velocityY": 2.1514865830703513,
+            "timestamp": 2.5192825643000583
+        },
+        {
+            "x": 2.521265507868239,
+            "y": 6.557635351096379,
+            "heading": 0.4535239115954702,
+            "angularVelocity": -0.522581391709739,
+            "velocityX": -1.9102680545844264,
+            "velocityY": 1.7211899688838848,
+            "timestamp": 2.5965022430680076
+        },
+        {
+            "x": 2.4106327602038906,
+            "y": 6.6573176734827975,
+            "heading": 0.42326153130587874,
+            "angularVelocity": -0.391899846935805,
+            "velocityX": -1.432701474928527,
+            "velocityY": 1.2908927358526117,
+            "timestamp": 2.673721921835957
+        },
+        {
+            "x": 2.336877585524826,
+            "y": 6.723772559032594,
+            "heading": 0.40308732310971707,
+            "angularVelocity": -0.2612573442164476,
+            "velocityX": -0.9551344405446706,
+            "velocityY": 0.8605952085024652,
+            "timestamp": 2.7509416006039062
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": -0.1136135987327294,
-            "velocityX": -0.6060858674019433,
-            "velocityY": 0.5477599334193345,
-            "timestamp": 2.647350089416587
+            "heading": 0.393,
+            "angularVelocity": -0.13063150832354717,
+            "velocityX": -0.4775671967717696,
+            "velocityY": 0.43029758084408004,
+            "timestamp": 2.8281612793718556
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 0,
-            "velocityX": -1.9773011985788902e-32,
-            "velocityY": -8.72670829182521e-33,
-            "timestamp": 2.745400092728312
+            "heading": 0.393,
+            "angularVelocity": -5.871044564465464e-32,
+            "velocityX": 0,
+            "velocityY": -1.090928833064942e-33,
+            "timestamp": 2.905380958139805
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.traj
index 6f37c8f6..bec33f37 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.traj	
@@ -3,263 +3,308 @@
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 3.14,
-            "angularVelocity": 0,
-            "velocityX": -1.8495960722802552e-34,
-            "velocityY": 0,
+            "heading": 0,
+            "angularVelocity": -3.516607019090483e-32,
+            "velocityX": -4.092268936373955e-33,
+            "velocityY": -4.396806616029776e-33,
             "timestamp": 0
         },
         {
-            "x": 7.66217209907118,
-            "y": 0.8633707808076446,
-            "heading": 3.200268562588625,
-            "angularVelocity": 0.6146717037531945,
-            "velocityX": -0.18182458262791865,
-            "velocityY": 0.7788962593386439,
-            "timestamp": 0.09805000331172546
-        },
-        {
-            "x": 7.626250868838738,
-            "y": 1.0164003464766511,
-            "heading": 3.3145470628036264,
-            "angularVelocity": 1.1655124564522605,
-            "velocityX": -0.3663562368094907,
-            "velocityY": 1.5607298368209865,
-            "timestamp": 0.19610000662345092
-        },
-        {
-            "x": 7.571819523432209,
-            "y": 1.2466141084056221,
-            "heading": 3.4692056792172115,
-            "angularVelocity": 1.5773443262606215,
-            "velocityX": -0.555138639143922,
-            "velocityY": 2.347922020941335,
-            "timestamp": 0.29415000993517637
-        },
-        {
-            "x": 7.4978548812850345,
-            "y": 1.55456588878344,
-            "heading": 3.620198528248394,
-            "angularVelocity": 1.5399576127615076,
-            "velocityX": -0.7543563452213444,
-            "velocityY": 3.1407625698773547,
-            "timestamp": 0.39220001324690185
-        },
-        {
-            "x": 7.414564781883039,
-            "y": 1.916010778899352,
-            "heading": 3.6201985877633485,
-            "angularVelocity": 6.069857444827113e-7,
-            "velocityX": -0.8494655439959167,
-            "velocityY": 3.6863322581110816,
-            "timestamp": 0.4902500165586273
-        },
-        {
-            "x": 7.3312746982625,
-            "y": 2.27745567265221,
-            "heading": 3.620198647277482,
-            "angularVelocity": 6.069773758970038e-7,
-            "velocityX": -0.8494653830427515,
-            "velocityY": 3.686332295203854,
-            "timestamp": 0.5883000198703527
-        },
-        {
-            "x": 7.247984475288188,
-            "y": 2.6389005342928713,
-            "heading": 3.6201987067915797,
-            "angularVelocity": 6.069770050551867e-7,
-            "velocityX": -0.8494668042948591,
-            "velocityY": 3.686331967695479,
-            "timestamp": 0.6863500231820782
-        },
-        {
-            "x": 7.16025127777078,
-            "y": 2.999292730685755,
-            "heading": 3.620198766578442,
-            "angularVelocity": 6.097589007693709e-7,
-            "velocityX": -0.8947801586348038,
-            "velocityY": 3.675595963491274,
-            "timestamp": 0.7844000264938036
-        },
-        {
-            "x": 6.997140841431304,
-            "y": 3.3324212817743484,
-            "heading": 3.6201988348555445,
-            "angularVelocity": 6.963498230184926e-7,
-            "velocityX": -1.6635434046943107,
-            "velocityY": 3.3975373772247037,
-            "timestamp": 0.882450029805529
-        },
-        {
-            "x": 6.766266862430273,
-            "y": 3.622726019325249,
-            "heading": 3.620198931073779,
-            "angularVelocity": 9.813180151910862e-7,
-            "velocityX": -2.354655494166845,
-            "velocityY": 2.9607825369260774,
-            "timestamp": 0.9805000331172544
-        },
-        {
-            "x": 6.478412432979381,
-            "y": 3.856647861556034,
-            "heading": 3.62019912006771,
-            "angularVelocity": 0.000001927525998631886,
-            "velocityX": -2.935792144093369,
-            "velocityY": 2.38574027873399,
-            "timestamp": 1.07855003642898
-        },
-        {
-            "x": 6.147022412583985,
-            "y": 4.023260998419484,
-            "heading": 3.6202003231426523,
-            "angularVelocity": 0.000012270014292625344,
-            "velocityX": -3.3798063151698643,
-            "velocityY": 1.6992670192345174,
-            "timestamp": 1.1766000397407055
-        },
-        {
-            "x": 5.787574046048744,
-            "y": 4.114783912760558,
-            "heading": 3.6201999548728017,
-            "angularVelocity": -0.0000037559391933502495,
-            "velocityX": -3.6659699581290637,
-            "velocityY": 0.9334310173361262,
-            "timestamp": 1.274650043052431
-        },
-        {
-            "x": 5.417217638191157,
-            "y": 4.135174501302961,
-            "heading": 3.620199996859582,
-            "angularVelocity": 4.2821804063172526e-7,
-            "velocityX": -3.7772197383831925,
-            "velocityY": 0.2079611203844201,
-            "timestamp": 1.3727000463641565
-        },
-        {
-            "x": 5.066383608065517,
-            "y": 4.23317852630269,
-            "heading": 3.636482368230358,
-            "angularVelocity": 0.16606191556170027,
-            "velocityX": -3.578113393940955,
-            "velocityY": 0.9995310728154599,
-            "timestamp": 1.470750049675882
-        },
-        {
-            "x": 4.742245396435021,
-            "y": 4.406733140775732,
-            "heading": 3.644586480575013,
-            "angularVelocity": 0.08265285130985205,
-            "velocityX": -3.305846003900474,
-            "velocityY": 1.7700623009799354,
-            "timestamp": 1.5688000529876076
-        },
-        {
-            "x": 4.460670694136194,
-            "y": 4.648177372257919,
-            "heading": 3.644586687046199,
-            "angularVelocity": 0.000002105774391224538,
-            "velocityX": -2.87174597438442,
-            "velocityY": 2.4624602073145714,
-            "timestamp": 1.6668500562993331
-        },
-        {
-            "x": 4.237545979665016,
-            "y": 4.944479621179968,
-            "heading": 3.644586791013843,
-            "angularVelocity": 0.0000010603532960842864,
-            "velocityX": -2.2756216923502683,
-            "velocityY": 3.0219504223781635,
-            "timestamp": 1.7649000596110587
-        },
-        {
-            "x": 3.9950087307179025,
-            "y": 5.225113475172281,
-            "heading": 3.6445867953690434,
-            "angularVelocity": 4.441815614419627e-8,
-            "velocityX": -2.473607758849603,
-            "velocityY": 2.862150377497781,
-            "timestamp": 1.8629500629227842
-        },
-        {
-            "x": 3.7198234606165768,
-            "y": 5.473815593691706,
-            "heading": 3.644586791690717,
-            "angularVelocity": -3.7514802268686446e-8,
-            "velocityX": -2.8065809363253424,
-            "velocityY": 2.536482510140642,
-            "timestamp": 1.9610000662345097
-        },
-        {
-            "x": 3.4446380366180587,
-            "y": 5.722517541926013,
-            "heading": 3.6445867880123934,
-            "angularVelocity": -3.751477003592028e-8,
-            "velocityX": -2.8065825059040024,
-            "velocityY": 2.5364807734235555,
-            "timestamp": 2.059050069546235
-        },
-        {
-            "x": 3.1694525996169167,
-            "y": 5.97121947577308,
-            "heading": 3.6445867843340722,
-            "angularVelocity": -3.75147461397837e-8,
-            "velocityX": -2.806582638516174,
-            "velocityY": 2.5364806266898405,
-            "timestamp": 2.1571000728579603
-        },
-        {
-            "x": 2.894267154657697,
-            "y": 6.2199214008146,
-            "heading": 3.6445867806557373,
-            "angularVelocity": -3.751489444763293e-8,
-            "velocityX": -2.806582719679637,
-            "velocityY": 2.53648053688316,
-            "timestamp": 2.2551500761696857
-        },
-        {
-            "x": 2.6565603153930746,
-            "y": 6.434752825744944,
-            "heading": 3.60002452914154,
-            "angularVelocity": -0.45448495674724715,
-            "velocityX": -2.4243430008757167,
-            "velocityY": 2.191039445938014,
-            "timestamp": 2.353200079481411
-        },
-        {
-            "x": 2.478280163108621,
-            "y": 6.595876410073326,
-            "heading": 3.56660458464388,
-            "angularVelocity": -0.3408459293102701,
-            "velocityX": -1.8182574835582244,
-            "velocityY": 1.6432797438683373,
-            "timestamp": 2.4512500827931363
-        },
-        {
-            "x": 2.3594267213059505,
-            "y": 6.703292136714203,
-            "heading": 3.544325120911587,
-            "angularVelocity": -0.2272255275857591,
-            "velocityX": -1.2121717265506466,
-            "velocityY": 1.0955198675452942,
-            "timestamp": 2.5493000861048616
+            "x": 7.6580594556627535,
+            "y": 0.8642766421574168,
+            "heading": -0.06977499787943942,
+            "angularVelocity": -0.7009716360726537,
+            "velocityX": -0.22041848409624198,
+            "velocityY": 0.7763344454252913,
+            "timestamp": 0.09954040119279155
+        },
+        {
+            "x": 7.613877684624128,
+            "y": 1.0194223152597515,
+            "heading": -0.19807907998906352,
+            "angularVelocity": -1.2889648883484262,
+            "velocityX": -0.4438576749661031,
+            "velocityY": 1.558620130552277,
+            "timestamp": 0.1990808023855831
+        },
+        {
+            "x": 7.54706531781781,
+            "y": 1.253315507091588,
+            "heading": -0.3641253773176819,
+            "angularVelocity": -1.6681296773861405,
+            "velocityX": -0.6712085344815377,
+            "velocityY": 2.349731255139591,
+            "timestamp": 0.2986212035783746
+        },
+        {
+            "x": 7.457648537893544,
+            "y": 1.5658706931334108,
+            "heading": -0.49008580394214096,
+            "angularVelocity": -1.265420122031624,
+            "velocityX": -0.8982963585919527,
+            "velocityY": 3.139983185686188,
+            "timestamp": 0.3981616047711662
+        },
+        {
+            "x": 7.353528135913809,
+            "y": 1.9277448806018234,
+            "heading": -0.490085841920944,
+            "angularVelocity": -3.8154159059291386e-7,
+            "velocityX": -1.0460114760646044,
+            "velocityY": 3.6354503611807685,
+            "timestamp": 0.49770200596395775
+        },
+        {
+            "x": 7.249407681423134,
+            "y": 2.2896190529615703,
+            "heading": -0.4900858798995876,
+            "angularVelocity": -3.8153998933914064e-7,
+            "velocityX": -1.0460120035985523,
+            "velocityY": 3.635450209396512,
+            "timestamp": 0.5972424071567493
+        },
+        {
+            "x": 7.145287117295142,
+            "y": 2.651493193775847,
+            "heading": -0.4900859178782245,
+            "angularVelocity": -3.8153992177326955e-7,
+            "velocityX": -1.0460131050338848,
+            "velocityY": 3.6354498924852887,
+            "timestamp": 0.6967828083495408
+        },
+        {
+            "x": 7.025437006860337,
+            "y": 3.00846649318931,
+            "heading": -0.49008595631381496,
+            "angularVelocity": -3.8613055604326664e-7,
+            "velocityX": -1.204034834083884,
+            "velocityY": 3.586215196401226,
+            "timestamp": 0.7963232095423323
+        },
+        {
+            "x": 6.830624533504276,
+            "y": 3.3307118703307705,
+            "heading": -0.49008600153372534,
+            "angularVelocity": -4.542870012894294e-7,
+            "velocityX": -1.9571196320451347,
+            "velocityY": 3.237332513029861,
+            "timestamp": 0.8958636107351238
+        },
+        {
+            "x": 6.570226686668516,
+            "y": 3.602717269957059,
+            "heading": -0.4900860357215963,
+            "angularVelocity": -3.4345723556551647e-7,
+            "velocityX": -2.6160015804177474,
+            "velocityY": 2.7326130532612987,
+            "timestamp": 0.9954040119279153
+        },
+        {
+            "x": 6.270670526052627,
+            "y": 3.8032215967371097,
+            "heading": -0.4416564414377051,
+            "angularVelocity": 0.4865320382835506,
+            "velocityX": -3.009392739292897,
+            "velocityY": 2.0143009710369753,
+            "timestamp": 1.094944413120707
+        },
+        {
+            "x": 5.9498699270968665,
+            "y": 3.9255658761230467,
+            "heading": -0.3340733830984328,
+            "angularVelocity": 1.0807979177309486,
+            "velocityX": -3.222818022748657,
+            "velocityY": 1.229091684581206,
+            "timestamp": 1.1944848143134985
+        },
+        {
+            "x": 5.637666976812727,
+            "y": 3.9694128449739723,
+            "heading": -0.13726885518093873,
+            "angularVelocity": 1.977132154976145,
+            "velocityX": -3.136444564649281,
+            "velocityY": 0.4404941945733328,
+            "timestamp": 1.2940252155062901
+        },
+        {
+            "x": 5.333615486534618,
+            "y": 3.942111372133816,
+            "heading": 0.08955200005531357,
+            "angularVelocity": 2.278681344642582,
+            "velocityX": -3.0545535946677287,
+            "velocityY": -0.2742752943830166,
+            "timestamp": 1.3935656166990817
+        },
+        {
+            "x": 5.078082684497777,
+            "y": 3.9650753252247246,
+            "heading": 0.19884455131252843,
+            "angularVelocity": 1.0979717777662474,
+            "velocityX": -2.5671265031564414,
+            "velocityY": 0.2306998245509574,
+            "timestamp": 1.4931060178918734
+        },
+        {
+            "x": 4.8543877601623535,
+            "y": 4.064273357391357,
+            "heading": 0.3064368289582943,
+            "angularVelocity": 1.0808905364705061,
+            "velocityX": -2.2472777048805277,
+            "velocityY": 0.9965605018459247,
+            "timestamp": 1.592646419084665
+        },
+        {
+            "x": 4.700013932982481,
+            "y": 4.187105651970831,
+            "heading": 0.3888797273509616,
+            "angularVelocity": 1.0676410431648378,
+            "velocityX": -1.99915137751061,
+            "velocityY": 1.5906864226746236,
+            "timestamp": 1.6698660978526145
+        },
+        {
+            "x": 4.564169886311024,
+            "y": 4.35607150380987,
+            "heading": 0.4692148602751942,
+            "angularVelocity": 1.040345339504005,
+            "velocityX": -1.7591894817340108,
+            "velocityY": 2.1881190719116157,
+            "timestamp": 1.747085776620564
+        },
+        {
+            "x": 4.445019228108457,
+            "y": 4.571847534724939,
+            "heading": 0.5443267073647724,
+            "angularVelocity": 0.9727034389160626,
+            "velocityX": -1.543008985580261,
+            "velocityY": 2.7943140188848803,
+            "timestamp": 1.8243054553885136
+        },
+        {
+            "x": 4.302573716554072,
+            "y": 4.826880596020871,
+            "heading": 0.5443268349443496,
+            "angularVelocity": 0.0000016521640508021742,
+            "velocityX": -1.8446788930894609,
+            "velocityY": 3.302695185541032,
+            "timestamp": 1.9015251341564632
+        },
+        {
+            "x": 4.139811862406642,
+            "y": 5.06945260129317,
+            "heading": 0.544326895208066,
+            "angularVelocity": 7.80419154390702e-7,
+            "velocityX": -2.1077768872432143,
+            "velocityY": 3.141323677365259,
+            "timestamp": 1.9787448129244127
+        },
+        {
+            "x": 3.9382711614032555,
+            "y": 5.28090933180296,
+            "heading": 0.5443268879285122,
+            "angularVelocity": -9.42707078520697e-8,
+            "velocityX": -2.60996554529876,
+            "velocityY": 2.738378790013248,
+            "timestamp": 2.0559644916923623
+        },
+        {
+            "x": 3.721249786731846,
+            "y": 5.476445328956613,
+            "heading": 0.5443268807879367,
+            "angularVelocity": -9.247093001843253e-8,
+            "velocityX": -2.810441303745562,
+            "velocityY": 2.53220422919983,
+            "timestamp": 2.1331841704603116
+        },
+        {
+            "x": 3.5042282470745802,
+            "y": 5.671981142995692,
+            "heading": 0.5443268736473624,
+            "angularVelocity": -9.247091428352055e-8,
+            "velocityX": -2.810443440323428,
+            "velocityY": 2.53220185785385,
+            "timestamp": 2.210403849228261
+        },
+        {
+            "x": 3.287206691377032,
+            "y": 5.867516939231946,
+            "heading": 0.5443268665067905,
+            "angularVelocity": -9.247088327815795e-8,
+            "velocityX": -2.810443648046155,
+            "velocityY": 2.5322016273060566,
+            "timestamp": 2.2876235279962103
+        },
+        {
+            "x": 3.0701851252220784,
+            "y": 6.063052723861702,
+            "heading": 0.5443268593662193,
+            "angularVelocity": -9.247087442211068e-8,
+            "velocityX": -2.8104437834702534,
+            "velocityY": 2.532201477001163,
+            "timestamp": 2.3648432067641596
+        },
+        {
+            "x": 2.853163562032648,
+            "y": 6.258588511782302,
+            "heading": 0.5443268522252032,
+            "angularVelocity": -9.247663534512907e-8,
+            "velocityX": -2.8104437450665345,
+            "velocityY": 2.5322015196178005,
+            "timestamp": 2.442062885532109
+        },
+        {
+            "x": 2.6687757934039245,
+            "y": 6.424725614600548,
+            "heading": 0.4938774787934042,
+            "angularVelocity": -0.6533227570578579,
+            "velocityX": -2.3878339248576954,
+            "velocityY": 2.1514865830703513,
+            "timestamp": 2.5192825643000583
+        },
+        {
+            "x": 2.521265507868239,
+            "y": 6.557635351096379,
+            "heading": 0.4535239115954702,
+            "angularVelocity": -0.522581391709739,
+            "velocityX": -1.9102680545844264,
+            "velocityY": 1.7211899688838848,
+            "timestamp": 2.5965022430680076
+        },
+        {
+            "x": 2.4106327602038906,
+            "y": 6.6573176734827975,
+            "heading": 0.42326153130587874,
+            "angularVelocity": -0.391899846935805,
+            "velocityX": -1.432701474928527,
+            "velocityY": 1.2908927358526117,
+            "timestamp": 2.673721921835957
+        },
+        {
+            "x": 2.336877585524826,
+            "y": 6.723772559032594,
+            "heading": 0.40308732310971707,
+            "angularVelocity": -0.2612573442164476,
+            "velocityX": -0.9551344405446706,
+            "velocityY": 0.8605952085024652,
+            "timestamp": 2.7509416006039062
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": -0.1136135987327294,
-            "velocityX": -0.6060858674019433,
-            "velocityY": 0.5477599334193345,
-            "timestamp": 2.647350089416587
+            "heading": 0.393,
+            "angularVelocity": -0.13063150832354717,
+            "velocityX": -0.4775671967717696,
+            "velocityY": 0.43029758084408004,
+            "timestamp": 2.8281612793718556
         },
         {
             "x": 2.3,
             "y": 6.757,
-            "heading": 3.5331853071795862,
-            "angularVelocity": 0,
-            "velocityX": -1.9773011985788902e-32,
-            "velocityY": -8.72670829182521e-33,
-            "timestamp": 2.745400092728312
+            "heading": 0.393,
+            "angularVelocity": -5.871044564465464e-32,
+            "velocityX": 0,
+            "velocityY": -1.090928833064942e-33,
+            "timestamp": 2.905380958139805
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.1.traj
new file mode 100644
index 00000000..d94da80a
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.1.traj	
@@ -0,0 +1,283 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": -2.906139149989869e-32,
+            "velocityX": 1.181772084740038e-33,
+            "velocityY": 2.7502272506846476e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 7.666043555718406,
+            "y": 0.8524996120976488,
+            "heading": 0.004999198621269784,
+            "angularVelocity": 0.05577597491216818,
+            "velocityX": -0.15571181404981524,
+            "velocityY": 0.7307780702234378,
+            "timestamp": 0.08962996396104499
+        },
+        {
+            "x": 7.638131101049187,
+            "y": 0.9835016263770707,
+            "heading": 0.014524734067527434,
+            "angularVelocity": 0.10627623871853438,
+            "velocityX": -0.31141878715192206,
+            "velocityY": 1.4615872693685117,
+            "timestamp": 0.17925992792208997
+        },
+        {
+            "x": 7.596263489657983,
+            "y": 1.180010140566484,
+            "heading": 0.02778512041931341,
+            "angularVelocity": 0.14794590743726296,
+            "velocityX": -0.46711623592083296,
+            "velocityY": 2.1924421867983774,
+            "timestamp": 0.26888989188313495
+        },
+        {
+            "x": 7.540442918712468,
+            "y": 1.4420312351509346,
+            "heading": 0.04318451816280708,
+            "angularVelocity": 0.17181082154832236,
+            "velocityX": -0.6227891709269913,
+            "velocityY": 2.9233649440976053,
+            "timestamp": 0.35851985584417995
+        },
+        {
+            "x": 7.470680035396563,
+            "y": 1.7695653388184471,
+            "heading": 0.05582613302662815,
+            "angularVelocity": 0.1410422843561041,
+            "velocityX": -0.7783433154812639,
+            "velocityY": 3.654292484261909,
+            "timestamp": 0.44814981980522495
+        },
+        {
+            "x": 7.399999368806171,
+            "y": 2.101181376235394,
+            "heading": 0.05582613685023459,
+            "angularVelocity": 4.265991273772723e-8,
+            "velocityX": -0.7885830080341364,
+            "velocityY": 3.6998345504308543,
+            "timestamp": 0.5377797837662699
+        },
+        {
+            "x": 7.329318638814834,
+            "y": 2.4327974001390613,
+            "heading": 0.05582614067384277,
+            "angularVelocity": 4.265993214747779e-8,
+            "velocityX": -0.788583715397405,
+            "velocityY": 3.6998343996634295,
+            "timestamp": 0.6274097477273148
+        },
+        {
+            "x": 7.258637432835217,
+            "y": 2.764413322590103,
+            "heading": 0.05582614449746126,
+            "angularVelocity": 4.266004720083137e-8,
+            "velocityX": -0.7885890259906463,
+            "velocityY": 3.699833267758185,
+            "timestamp": 0.7170397116883598
+        },
+        {
+            "x": 7.145396314308057,
+            "y": 3.084009006822374,
+            "heading": 0.05582614848984762,
+            "angularVelocity": 4.4542987394171874e-8,
+            "velocityX": -1.2634292542656518,
+            "velocityY": 3.5657236721770227,
+            "timestamp": 0.8066696756494047
+        },
+        {
+            "x": 6.971535677728531,
+            "y": 3.375105946924151,
+            "heading": 0.05582615332930866,
+            "angularVelocity": 5.39937854332775e-8,
+            "velocityX": -1.9397601973274063,
+            "velocityY": 3.2477636633692444,
+            "timestamp": 0.8962996396104497
+        },
+        {
+            "x": 6.743840876905014,
+            "y": 3.626342927147427,
+            "heading": 0.0558261606002303,
+            "angularVelocity": 8.112155042111421e-8,
+            "velocityX": -2.5403870621043505,
+            "velocityY": 2.8030467615993793,
+            "timestamp": 0.9859296035714946
+        },
+        {
+            "x": 6.471198532281258,
+            "y": 3.8279144449501397,
+            "heading": 0.0558261762788256,
+            "angularVelocity": 1.7492582391526053e-7,
+            "velocityX": -3.041866052096724,
+            "velocityY": 2.2489300329331785,
+            "timestamp": 1.0755595675325396
+        },
+        {
+            "x": 6.164249513947577,
+            "y": 3.9719534129902763,
+            "heading": 0.05582625958870765,
+            "angularVelocity": 9.294869524458774e-7,
+            "velocityX": -3.4246250335109787,
+            "velocityY": 1.6070403431461682,
+            "timestamp": 1.1651895314935845
+        },
+        {
+            "x": 5.836252922691461,
+            "y": 4.05240893270077,
+            "heading": 0.05954498226559844,
+            "angularVelocity": 0.04148972634315725,
+            "velocityX": -3.6594524505071675,
+            "velocityY": 0.8976408798452788,
+            "timestamp": 1.2548194954546295
+        },
+        {
+            "x": 5.497492188342187,
+            "y": 4.066758597782368,
+            "heading": 0.05954602093201425,
+            "angularVelocity": 0.000011588383726853398,
+            "velocityX": -3.779547813904135,
+            "velocityY": 0.16009897190000646,
+            "timestamp": 1.3444494594156744
+        },
+        {
+            "x": 5.204246867986104,
+            "y": 4.110932074419811,
+            "heading": 0.17401114275805463,
+            "angularVelocity": 1.2770854384789139,
+            "velocityX": -3.2717331057226993,
+            "velocityY": 0.4928427356797903,
+            "timestamp": 1.4340794233767193
+        },
+        {
+            "x": 4.937438011169434,
+            "y": 4.2165327072143555,
+            "heading": 0.3021760231951433,
+            "angularVelocity": 1.4299334148208713,
+            "velocityX": -2.976781926774297,
+            "velocityY": 1.1781844834885962,
+            "timestamp": 1.5237093873377643
+        },
+        {
+            "x": 4.703348327317259,
+            "y": 4.378813716693983,
+            "heading": 0.4253911826920849,
+            "angularVelocity": 1.3960058537628992,
+            "velocityX": -2.6521945051027034,
+            "velocityY": 1.8386149895276152,
+            "timestamp": 1.61197202546698
+        },
+        {
+            "x": 4.500555108729279,
+            "y": 4.597666686435954,
+            "heading": 0.5261840356750895,
+            "angularVelocity": 1.1419651068603307,
+            "velocityX": -2.297611117074157,
+            "velocityY": 2.479565242787912,
+            "timestamp": 1.7002346635961958
+        },
+        {
+            "x": 4.326794639818804,
+            "y": 4.871425359866347,
+            "heading": 0.5588913487111756,
+            "angularVelocity": 0.37056804248478087,
+            "velocityX": -1.968675224233522,
+            "velocityY": 3.1016371052677254,
+            "timestamp": 1.7884973017254115
+        },
+        {
+            "x": 4.0969805858398445,
+            "y": 5.11364296690146,
+            "heading": 0.5588923269288569,
+            "angularVelocity": 0.00001108303243636796,
+            "velocityX": -2.603752378696331,
+            "velocityY": 2.744282429905488,
+            "timestamp": 1.8767599398546273
+        },
+        {
+            "x": 3.8246168380678105,
+            "y": 5.306780372295281,
+            "heading": 0.5588922897097742,
+            "angularVelocity": -4.2168559122096544e-7,
+            "velocityX": -3.0858328455274044,
+            "velocityY": 2.188212470049549,
+            "timestamp": 1.965022577983843
+        },
+        {
+            "x": 3.520011793550478,
+            "y": 5.443527703198532,
+            "heading": 0.5588922240132788,
+            "angularVelocity": -7.44329614921164e-7,
+            "velocityX": -3.451121006278936,
+            "velocityY": 1.549322950250525,
+            "timestamp": 2.053285216113059
+        },
+        {
+            "x": 3.194693864220709,
+            "y": 5.518709582083228,
+            "heading": 0.5588921711301718,
+            "angularVelocity": -5.991562005821763e-7,
+            "velocityX": -3.6857943091787817,
+            "velocityY": 0.851797323060201,
+            "timestamp": 2.1415478542422743
+        },
+        {
+            "x": 2.8828791952129387,
+            "y": 5.532174882793972,
+            "heading": 0.5041280478943673,
+            "angularVelocity": -0.6204677811196886,
+            "velocityX": -3.5328047701370098,
+            "velocityY": 0.15255946339412532,
+            "timestamp": 2.22981049237149
+        },
+        {
+            "x": 2.6283322264519793,
+            "y": 5.543894951157175,
+            "heading": 0.3346872638422791,
+            "angularVelocity": -1.9197339626765784,
+            "velocityX": -2.883971906530878,
+            "velocityY": 0.132786291137652,
+            "timestamp": 2.3180731305007054
+        },
+        {
+            "x": 2.4385492640365714,
+            "y": 5.5521384887253,
+            "heading": 0.1780668377028913,
+            "angularVelocity": -1.7744815865360473,
+            "velocityX": -2.150207227406541,
+            "velocityY": 0.09339781523475092,
+            "timestamp": 2.406335768629921
+        },
+        {
+            "x": 2.31267915340354,
+            "y": 5.55742889143524,
+            "heading": 0.062002293486176877,
+            "angularVelocity": -1.3149906537667377,
+            "velocityX": -1.4260859781775244,
+            "velocityY": 0.0599393222554144,
+            "timestamp": 2.4945984067591365
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 9.107235017738231e-34,
+            "angularVelocity": -0.7024749633633888,
+            "velocityX": -0.7101436659051381,
+            "velocityY": 0.029130202985730554,
+            "timestamp": 2.582861044888352
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": -2.2232006496445675e-32,
+            "velocityX": 0,
+            "velocityY": -2.9660881962078245e-33,
+            "timestamp": 2.6711236830175675
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.traj
new file mode 100644
index 00000000..d94da80a
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.traj	
@@ -0,0 +1,283 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": -2.906139149989869e-32,
+            "velocityX": 1.181772084740038e-33,
+            "velocityY": 2.7502272506846476e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 7.666043555718406,
+            "y": 0.8524996120976488,
+            "heading": 0.004999198621269784,
+            "angularVelocity": 0.05577597491216818,
+            "velocityX": -0.15571181404981524,
+            "velocityY": 0.7307780702234378,
+            "timestamp": 0.08962996396104499
+        },
+        {
+            "x": 7.638131101049187,
+            "y": 0.9835016263770707,
+            "heading": 0.014524734067527434,
+            "angularVelocity": 0.10627623871853438,
+            "velocityX": -0.31141878715192206,
+            "velocityY": 1.4615872693685117,
+            "timestamp": 0.17925992792208997
+        },
+        {
+            "x": 7.596263489657983,
+            "y": 1.180010140566484,
+            "heading": 0.02778512041931341,
+            "angularVelocity": 0.14794590743726296,
+            "velocityX": -0.46711623592083296,
+            "velocityY": 2.1924421867983774,
+            "timestamp": 0.26888989188313495
+        },
+        {
+            "x": 7.540442918712468,
+            "y": 1.4420312351509346,
+            "heading": 0.04318451816280708,
+            "angularVelocity": 0.17181082154832236,
+            "velocityX": -0.6227891709269913,
+            "velocityY": 2.9233649440976053,
+            "timestamp": 0.35851985584417995
+        },
+        {
+            "x": 7.470680035396563,
+            "y": 1.7695653388184471,
+            "heading": 0.05582613302662815,
+            "angularVelocity": 0.1410422843561041,
+            "velocityX": -0.7783433154812639,
+            "velocityY": 3.654292484261909,
+            "timestamp": 0.44814981980522495
+        },
+        {
+            "x": 7.399999368806171,
+            "y": 2.101181376235394,
+            "heading": 0.05582613685023459,
+            "angularVelocity": 4.265991273772723e-8,
+            "velocityX": -0.7885830080341364,
+            "velocityY": 3.6998345504308543,
+            "timestamp": 0.5377797837662699
+        },
+        {
+            "x": 7.329318638814834,
+            "y": 2.4327974001390613,
+            "heading": 0.05582614067384277,
+            "angularVelocity": 4.265993214747779e-8,
+            "velocityX": -0.788583715397405,
+            "velocityY": 3.6998343996634295,
+            "timestamp": 0.6274097477273148
+        },
+        {
+            "x": 7.258637432835217,
+            "y": 2.764413322590103,
+            "heading": 0.05582614449746126,
+            "angularVelocity": 4.266004720083137e-8,
+            "velocityX": -0.7885890259906463,
+            "velocityY": 3.699833267758185,
+            "timestamp": 0.7170397116883598
+        },
+        {
+            "x": 7.145396314308057,
+            "y": 3.084009006822374,
+            "heading": 0.05582614848984762,
+            "angularVelocity": 4.4542987394171874e-8,
+            "velocityX": -1.2634292542656518,
+            "velocityY": 3.5657236721770227,
+            "timestamp": 0.8066696756494047
+        },
+        {
+            "x": 6.971535677728531,
+            "y": 3.375105946924151,
+            "heading": 0.05582615332930866,
+            "angularVelocity": 5.39937854332775e-8,
+            "velocityX": -1.9397601973274063,
+            "velocityY": 3.2477636633692444,
+            "timestamp": 0.8962996396104497
+        },
+        {
+            "x": 6.743840876905014,
+            "y": 3.626342927147427,
+            "heading": 0.0558261606002303,
+            "angularVelocity": 8.112155042111421e-8,
+            "velocityX": -2.5403870621043505,
+            "velocityY": 2.8030467615993793,
+            "timestamp": 0.9859296035714946
+        },
+        {
+            "x": 6.471198532281258,
+            "y": 3.8279144449501397,
+            "heading": 0.0558261762788256,
+            "angularVelocity": 1.7492582391526053e-7,
+            "velocityX": -3.041866052096724,
+            "velocityY": 2.2489300329331785,
+            "timestamp": 1.0755595675325396
+        },
+        {
+            "x": 6.164249513947577,
+            "y": 3.9719534129902763,
+            "heading": 0.05582625958870765,
+            "angularVelocity": 9.294869524458774e-7,
+            "velocityX": -3.4246250335109787,
+            "velocityY": 1.6070403431461682,
+            "timestamp": 1.1651895314935845
+        },
+        {
+            "x": 5.836252922691461,
+            "y": 4.05240893270077,
+            "heading": 0.05954498226559844,
+            "angularVelocity": 0.04148972634315725,
+            "velocityX": -3.6594524505071675,
+            "velocityY": 0.8976408798452788,
+            "timestamp": 1.2548194954546295
+        },
+        {
+            "x": 5.497492188342187,
+            "y": 4.066758597782368,
+            "heading": 0.05954602093201425,
+            "angularVelocity": 0.000011588383726853398,
+            "velocityX": -3.779547813904135,
+            "velocityY": 0.16009897190000646,
+            "timestamp": 1.3444494594156744
+        },
+        {
+            "x": 5.204246867986104,
+            "y": 4.110932074419811,
+            "heading": 0.17401114275805463,
+            "angularVelocity": 1.2770854384789139,
+            "velocityX": -3.2717331057226993,
+            "velocityY": 0.4928427356797903,
+            "timestamp": 1.4340794233767193
+        },
+        {
+            "x": 4.937438011169434,
+            "y": 4.2165327072143555,
+            "heading": 0.3021760231951433,
+            "angularVelocity": 1.4299334148208713,
+            "velocityX": -2.976781926774297,
+            "velocityY": 1.1781844834885962,
+            "timestamp": 1.5237093873377643
+        },
+        {
+            "x": 4.703348327317259,
+            "y": 4.378813716693983,
+            "heading": 0.4253911826920849,
+            "angularVelocity": 1.3960058537628992,
+            "velocityX": -2.6521945051027034,
+            "velocityY": 1.8386149895276152,
+            "timestamp": 1.61197202546698
+        },
+        {
+            "x": 4.500555108729279,
+            "y": 4.597666686435954,
+            "heading": 0.5261840356750895,
+            "angularVelocity": 1.1419651068603307,
+            "velocityX": -2.297611117074157,
+            "velocityY": 2.479565242787912,
+            "timestamp": 1.7002346635961958
+        },
+        {
+            "x": 4.326794639818804,
+            "y": 4.871425359866347,
+            "heading": 0.5588913487111756,
+            "angularVelocity": 0.37056804248478087,
+            "velocityX": -1.968675224233522,
+            "velocityY": 3.1016371052677254,
+            "timestamp": 1.7884973017254115
+        },
+        {
+            "x": 4.0969805858398445,
+            "y": 5.11364296690146,
+            "heading": 0.5588923269288569,
+            "angularVelocity": 0.00001108303243636796,
+            "velocityX": -2.603752378696331,
+            "velocityY": 2.744282429905488,
+            "timestamp": 1.8767599398546273
+        },
+        {
+            "x": 3.8246168380678105,
+            "y": 5.306780372295281,
+            "heading": 0.5588922897097742,
+            "angularVelocity": -4.2168559122096544e-7,
+            "velocityX": -3.0858328455274044,
+            "velocityY": 2.188212470049549,
+            "timestamp": 1.965022577983843
+        },
+        {
+            "x": 3.520011793550478,
+            "y": 5.443527703198532,
+            "heading": 0.5588922240132788,
+            "angularVelocity": -7.44329614921164e-7,
+            "velocityX": -3.451121006278936,
+            "velocityY": 1.549322950250525,
+            "timestamp": 2.053285216113059
+        },
+        {
+            "x": 3.194693864220709,
+            "y": 5.518709582083228,
+            "heading": 0.5588921711301718,
+            "angularVelocity": -5.991562005821763e-7,
+            "velocityX": -3.6857943091787817,
+            "velocityY": 0.851797323060201,
+            "timestamp": 2.1415478542422743
+        },
+        {
+            "x": 2.8828791952129387,
+            "y": 5.532174882793972,
+            "heading": 0.5041280478943673,
+            "angularVelocity": -0.6204677811196886,
+            "velocityX": -3.5328047701370098,
+            "velocityY": 0.15255946339412532,
+            "timestamp": 2.22981049237149
+        },
+        {
+            "x": 2.6283322264519793,
+            "y": 5.543894951157175,
+            "heading": 0.3346872638422791,
+            "angularVelocity": -1.9197339626765784,
+            "velocityX": -2.883971906530878,
+            "velocityY": 0.132786291137652,
+            "timestamp": 2.3180731305007054
+        },
+        {
+            "x": 2.4385492640365714,
+            "y": 5.5521384887253,
+            "heading": 0.1780668377028913,
+            "angularVelocity": -1.7744815865360473,
+            "velocityX": -2.150207227406541,
+            "velocityY": 0.09339781523475092,
+            "timestamp": 2.406335768629921
+        },
+        {
+            "x": 2.31267915340354,
+            "y": 5.55742889143524,
+            "heading": 0.062002293486176877,
+            "angularVelocity": -1.3149906537667377,
+            "velocityX": -1.4260859781775244,
+            "velocityY": 0.0599393222554144,
+            "timestamp": 2.4945984067591365
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 9.107235017738231e-34,
+            "angularVelocity": -0.7024749633633888,
+            "velocityX": -0.7101436659051381,
+            "velocityY": 0.029130202985730554,
+            "timestamp": 2.582861044888352
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": -2.2232006496445675e-32,
+            "velocityX": 0,
+            "velocityY": -2.9660881962078245e-33,
+            "timestamp": 2.6711236830175675
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W3.1.traj
new file mode 100644
index 00000000..62c8640c
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W3.1.traj	
@@ -0,0 +1,292 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": -1.5069359770438582e-30,
+            "velocityX": 5.69693995403372e-32,
+            "velocityY": 3.042278119963715e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 7.627290492953592,
+            "y": 0.8035142159340071,
+            "heading": 0.015175150751654183,
+            "angularVelocity": 0.18617297608634506,
+            "velocityX": -0.6466549133823012,
+            "velocityY": 0.20260100070713255,
+            "timestamp": 0.08151102845675925
+        },
+        {
+            "x": 7.521871327482097,
+            "y": 0.8365426259859824,
+            "heading": 0.045515680376982795,
+            "angularVelocity": 0.3722260680519286,
+            "velocityX": -1.2933116839203953,
+            "velocityY": 0.4052017337690261,
+            "timestamp": 0.1630220569135185
+        },
+        {
+            "x": 7.363742244741542,
+            "y": 0.8860852747673063,
+            "heading": 0.09100430929463568,
+            "angularVelocity": 0.5580671693007231,
+            "velocityX": -1.9399716300284648,
+            "velocityY": 0.6078030141357551,
+            "timestamp": 0.24453308537027776
+        },
+        {
+            "x": 7.152902908699907,
+            "y": 0.95214234591484,
+            "heading": 0.15161661460632753,
+            "angularVelocity": 0.7436086436309897,
+            "velocityX": -2.5866356986709165,
+            "velocityY": 0.8104065474106494,
+            "timestamp": 0.326044113827037
+        },
+        {
+            "x": 6.889352971860368,
+            "y": 1.0347142415767305,
+            "heading": 0.22732297101557483,
+            "angularVelocity": 0.9287866665726976,
+            "velocityX": -3.233304030501219,
+            "velocityY": 1.0130150143510155,
+            "timestamp": 0.4075551422837963
+        },
+        {
+            "x": 6.595109187456524,
+            "y": 1.1269159867790934,
+            "heading": 0.22732298101704992,
+            "angularVelocity": 1.22700882310899e-7,
+            "velocityX": -3.6098647014365244,
+            "velocityY": 1.1311566906713055,
+            "timestamp": 0.4890661707405556
+        },
+        {
+            "x": 6.300865420236581,
+            "y": 1.219117786820698,
+            "heading": 0.2273229910184091,
+            "angularVelocity": 1.2269945998464997e-7,
+            "velocityX": -3.6098644906196586,
+            "velocityY": 1.131157363454394,
+            "timestamp": 0.5705771991973149
+        },
+        {
+            "x": 6.006621672014901,
+            "y": 1.3113196474914972,
+            "heading": 0.2273230010197575,
+            "angularVelocity": 1.2269932759504928e-7,
+            "velocityX": -3.6098642575436695,
+            "velocityY": 1.1311581072702415,
+            "timestamp": 0.6520882276540741
+        },
+        {
+            "x": 5.7123779465456614,
+            "y": 1.4035215807721424,
+            "heading": 0.22732301102110228,
+            "angularVelocity": 1.2269928353992546e-7,
+            "velocityX": -3.6098639784103828,
+            "velocityY": 1.131158998068056,
+            "timestamp": 0.7335992561108334
+        },
+        {
+            "x": 5.418134249112936,
+            "y": 1.4957236035255936,
+            "heading": 0.2273230210224565,
+            "angularVelocity": 1.2269939906393907e-7,
+            "velocityX": -3.60986363445063,
+            "velocityY": 1.1311600957453605,
+            "timestamp": 0.8151102845675927
+        },
+        {
+            "x": 5.123890658648026,
+            "y": 1.5879259676440571,
+            "heading": 0.22732303102385681,
+            "angularVelocity": 1.2269996504792678e-7,
+            "velocityX": -3.609862322139673,
+            "velocityY": 1.1311642837064038,
+            "timestamp": 0.896621313024352
+        },
+        {
+            "x": 4.838467298400658,
+            "y": 1.7046053225590203,
+            "heading": 0.22732304878104165,
+            "angularVelocity": 2.1785008908970575e-7,
+            "velocityX": -3.5016532811726435,
+            "velocityY": 1.4314548242617389,
+            "timestamp": 0.9781323414811113
+        },
+        {
+            "x": 4.5530441892872355,
+            "y": 1.8212852918009321,
+            "heading": 0.2273230665381978,
+            "angularVelocity": 2.1784973753126392e-7,
+            "velocityX": -3.5016502001914627,
+            "velocityY": 1.4314623609958343,
+            "timestamp": 1.0596433699378704
+        },
+        {
+            "x": 4.267621124217833,
+            "y": 1.9379653687834795,
+            "heading": 0.2273230842953322,
+            "angularVelocity": 2.1784947052459537e-7,
+            "velocityX": -3.5016496598471556,
+            "velocityY": 1.4314636827879597,
+            "timestamp": 1.1411543983946297
+        },
+        {
+            "x": 3.9821980933555543,
+            "y": 2.054645529443536,
+            "heading": 0.22732310205245027,
+            "angularVelocity": 2.1784927045604472e-7,
+            "velocityX": -3.5016492401846184,
+            "velocityY": 1.4314647093669521,
+            "timestamp": 1.222665426851389
+        },
+        {
+            "x": 3.6967750938784922,
+            "y": 2.171325766878081,
+            "heading": 0.22732311980956,
+            "angularVelocity": 2.1784916783717796e-7,
+            "velocityX": -3.501648855142053,
+            "velocityY": 1.4314656512577588,
+            "timestamp": 1.3041764553081483
+        },
+        {
+            "x": 3.411352191599502,
+            "y": 2.288006242078097,
+            "heading": 0.2273231375666713,
+            "angularVelocity": 2.178491871140625e-7,
+            "velocityX": -3.501647662689034,
+            "velocityY": 1.4314685682307748,
+            "timestamp": 1.3856874837649076
+        },
+        {
+            "x": 3.135544385824121,
+            "y": 2.425885248705818,
+            "heading": 0.22732315564878777,
+            "angularVelocity": 2.218364413735132e-7,
+            "velocityX": -3.3836869807340535,
+            "velocityY": 1.6915380561155315,
+            "timestamp": 1.4671985122216669
+        },
+        {
+            "x": 2.8888590335845947,
+            "y": 2.6108903884887695,
+            "heading": 0.22732317584620773,
+            "angularVelocity": 2.4778757367040615e-7,
+            "velocityX": -3.026404609412922,
+            "velocityY": 2.2696945834893776,
+            "timestamp": 1.5487095406784261
+        },
+        {
+            "x": 2.714424659127437,
+            "y": 2.7861164379371415,
+            "heading": 0.22732319539367835,
+            "angularVelocity": 2.990800632496739e-7,
+            "velocityX": -2.668879509458687,
+            "velocityY": 2.6809922892290525,
+            "timestamp": 1.6140681954393303
+        },
+        {
+            "x": 2.566977650329029,
+            "y": 2.984587845024715,
+            "heading": 0.2273232142228265,
+            "angularVelocity": 2.880895914276183e-7,
+            "velocityX": -2.2559676195571057,
+            "velocityY": 3.0366507360596735,
+            "timestamp": 1.6794268502002345
+        },
+        {
+            "x": 2.4495779823740462,
+            "y": 3.20218572489824,
+            "heading": 0.22732323403642601,
+            "angularVelocity": 3.0315188694924184e-7,
+            "velocityX": -1.7962375202558922,
+            "velocityY": 3.3292894517113587,
+            "timestamp": 1.7447855049611387
+        },
+        {
+            "x": 2.3646621591221844,
+            "y": 3.4343941523127826,
+            "heading": 0.22732276450933128,
+            "angularVelocity": -0.000007183855179950579,
+            "velocityX": -1.2992284428512828,
+            "velocityY": 3.5528336417572115,
+            "timestamp": 1.810144159722043
+        },
+        {
+            "x": 2.3096843159184894,
+            "y": 3.656196634862642,
+            "heading": 0.16843360905648927,
+            "angularVelocity": -0.9010154151467895,
+            "velocityX": -0.8411715847703609,
+            "velocityY": 3.3936206820850954,
+            "timestamp": 1.875502814482947
+        },
+        {
+            "x": 2.277656929328602,
+            "y": 3.853541817964372,
+            "heading": 0.0672675417750508,
+            "angularVelocity": -1.547860304829062,
+            "velocityX": -0.49002518039951914,
+            "velocityY": 3.0194192922676324,
+            "timestamp": 1.9408614692438513
+        },
+        {
+            "x": 2.265357919525011,
+            "y": 4.021569174077037,
+            "heading": -0.049806740829473055,
+            "angularVelocity": -1.791259061753831,
+            "velocityX": -0.188177217670429,
+            "velocityY": 2.570850895376378,
+            "timestamp": 2.0062201240047557
+        },
+        {
+            "x": 2.269649055536998,
+            "y": 4.158080976089645,
+            "heading": -0.1685192187929243,
+            "angularVelocity": -1.8163237661135343,
+            "velocityX": 0.06565520706762865,
+            "velocityY": 2.088656850603768,
+            "timestamp": 2.07157877876566
+        },
+        {
+            "x": 2.2810300353091644,
+            "y": 4.259951613943982,
+            "heading": -0.2719091294332062,
+            "angularVelocity": -1.581885536330179,
+            "velocityX": 0.1741311814602323,
+            "velocityY": 1.5586403702310236,
+            "timestamp": 2.1369374335265645
+        },
+        {
+            "x": 2.2926691286250658,
+            "y": 4.326989538481201,
+            "heading": -0.34849979264864867,
+            "angularVelocity": -1.1718518916219969,
+            "velocityX": 0.1780803683686499,
+            "velocityY": 1.0256931508529106,
+            "timestamp": 2.202296088287469
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.6349611616573703,
+            "velocityX": 0.11216374329844404,
+            "velocityY": 0.5050664160631704,
+            "timestamp": 2.2676547430483733
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -1.761626207104486e-30,
+            "velocityX": 0,
+            "velocityY": -1.839087679439853e-31,
+            "timestamp": 2.3330133978092777
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W3.traj
new file mode 100644
index 00000000..62c8640c
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W3.traj	
@@ -0,0 +1,292 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": -1.5069359770438582e-30,
+            "velocityX": 5.69693995403372e-32,
+            "velocityY": 3.042278119963715e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 7.627290492953592,
+            "y": 0.8035142159340071,
+            "heading": 0.015175150751654183,
+            "angularVelocity": 0.18617297608634506,
+            "velocityX": -0.6466549133823012,
+            "velocityY": 0.20260100070713255,
+            "timestamp": 0.08151102845675925
+        },
+        {
+            "x": 7.521871327482097,
+            "y": 0.8365426259859824,
+            "heading": 0.045515680376982795,
+            "angularVelocity": 0.3722260680519286,
+            "velocityX": -1.2933116839203953,
+            "velocityY": 0.4052017337690261,
+            "timestamp": 0.1630220569135185
+        },
+        {
+            "x": 7.363742244741542,
+            "y": 0.8860852747673063,
+            "heading": 0.09100430929463568,
+            "angularVelocity": 0.5580671693007231,
+            "velocityX": -1.9399716300284648,
+            "velocityY": 0.6078030141357551,
+            "timestamp": 0.24453308537027776
+        },
+        {
+            "x": 7.152902908699907,
+            "y": 0.95214234591484,
+            "heading": 0.15161661460632753,
+            "angularVelocity": 0.7436086436309897,
+            "velocityX": -2.5866356986709165,
+            "velocityY": 0.8104065474106494,
+            "timestamp": 0.326044113827037
+        },
+        {
+            "x": 6.889352971860368,
+            "y": 1.0347142415767305,
+            "heading": 0.22732297101557483,
+            "angularVelocity": 0.9287866665726976,
+            "velocityX": -3.233304030501219,
+            "velocityY": 1.0130150143510155,
+            "timestamp": 0.4075551422837963
+        },
+        {
+            "x": 6.595109187456524,
+            "y": 1.1269159867790934,
+            "heading": 0.22732298101704992,
+            "angularVelocity": 1.22700882310899e-7,
+            "velocityX": -3.6098647014365244,
+            "velocityY": 1.1311566906713055,
+            "timestamp": 0.4890661707405556
+        },
+        {
+            "x": 6.300865420236581,
+            "y": 1.219117786820698,
+            "heading": 0.2273229910184091,
+            "angularVelocity": 1.2269945998464997e-7,
+            "velocityX": -3.6098644906196586,
+            "velocityY": 1.131157363454394,
+            "timestamp": 0.5705771991973149
+        },
+        {
+            "x": 6.006621672014901,
+            "y": 1.3113196474914972,
+            "heading": 0.2273230010197575,
+            "angularVelocity": 1.2269932759504928e-7,
+            "velocityX": -3.6098642575436695,
+            "velocityY": 1.1311581072702415,
+            "timestamp": 0.6520882276540741
+        },
+        {
+            "x": 5.7123779465456614,
+            "y": 1.4035215807721424,
+            "heading": 0.22732301102110228,
+            "angularVelocity": 1.2269928353992546e-7,
+            "velocityX": -3.6098639784103828,
+            "velocityY": 1.131158998068056,
+            "timestamp": 0.7335992561108334
+        },
+        {
+            "x": 5.418134249112936,
+            "y": 1.4957236035255936,
+            "heading": 0.2273230210224565,
+            "angularVelocity": 1.2269939906393907e-7,
+            "velocityX": -3.60986363445063,
+            "velocityY": 1.1311600957453605,
+            "timestamp": 0.8151102845675927
+        },
+        {
+            "x": 5.123890658648026,
+            "y": 1.5879259676440571,
+            "heading": 0.22732303102385681,
+            "angularVelocity": 1.2269996504792678e-7,
+            "velocityX": -3.609862322139673,
+            "velocityY": 1.1311642837064038,
+            "timestamp": 0.896621313024352
+        },
+        {
+            "x": 4.838467298400658,
+            "y": 1.7046053225590203,
+            "heading": 0.22732304878104165,
+            "angularVelocity": 2.1785008908970575e-7,
+            "velocityX": -3.5016532811726435,
+            "velocityY": 1.4314548242617389,
+            "timestamp": 0.9781323414811113
+        },
+        {
+            "x": 4.5530441892872355,
+            "y": 1.8212852918009321,
+            "heading": 0.2273230665381978,
+            "angularVelocity": 2.1784973753126392e-7,
+            "velocityX": -3.5016502001914627,
+            "velocityY": 1.4314623609958343,
+            "timestamp": 1.0596433699378704
+        },
+        {
+            "x": 4.267621124217833,
+            "y": 1.9379653687834795,
+            "heading": 0.2273230842953322,
+            "angularVelocity": 2.1784947052459537e-7,
+            "velocityX": -3.5016496598471556,
+            "velocityY": 1.4314636827879597,
+            "timestamp": 1.1411543983946297
+        },
+        {
+            "x": 3.9821980933555543,
+            "y": 2.054645529443536,
+            "heading": 0.22732310205245027,
+            "angularVelocity": 2.1784927045604472e-7,
+            "velocityX": -3.5016492401846184,
+            "velocityY": 1.4314647093669521,
+            "timestamp": 1.222665426851389
+        },
+        {
+            "x": 3.6967750938784922,
+            "y": 2.171325766878081,
+            "heading": 0.22732311980956,
+            "angularVelocity": 2.1784916783717796e-7,
+            "velocityX": -3.501648855142053,
+            "velocityY": 1.4314656512577588,
+            "timestamp": 1.3041764553081483
+        },
+        {
+            "x": 3.411352191599502,
+            "y": 2.288006242078097,
+            "heading": 0.2273231375666713,
+            "angularVelocity": 2.178491871140625e-7,
+            "velocityX": -3.501647662689034,
+            "velocityY": 1.4314685682307748,
+            "timestamp": 1.3856874837649076
+        },
+        {
+            "x": 3.135544385824121,
+            "y": 2.425885248705818,
+            "heading": 0.22732315564878777,
+            "angularVelocity": 2.218364413735132e-7,
+            "velocityX": -3.3836869807340535,
+            "velocityY": 1.6915380561155315,
+            "timestamp": 1.4671985122216669
+        },
+        {
+            "x": 2.8888590335845947,
+            "y": 2.6108903884887695,
+            "heading": 0.22732317584620773,
+            "angularVelocity": 2.4778757367040615e-7,
+            "velocityX": -3.026404609412922,
+            "velocityY": 2.2696945834893776,
+            "timestamp": 1.5487095406784261
+        },
+        {
+            "x": 2.714424659127437,
+            "y": 2.7861164379371415,
+            "heading": 0.22732319539367835,
+            "angularVelocity": 2.990800632496739e-7,
+            "velocityX": -2.668879509458687,
+            "velocityY": 2.6809922892290525,
+            "timestamp": 1.6140681954393303
+        },
+        {
+            "x": 2.566977650329029,
+            "y": 2.984587845024715,
+            "heading": 0.2273232142228265,
+            "angularVelocity": 2.880895914276183e-7,
+            "velocityX": -2.2559676195571057,
+            "velocityY": 3.0366507360596735,
+            "timestamp": 1.6794268502002345
+        },
+        {
+            "x": 2.4495779823740462,
+            "y": 3.20218572489824,
+            "heading": 0.22732323403642601,
+            "angularVelocity": 3.0315188694924184e-7,
+            "velocityX": -1.7962375202558922,
+            "velocityY": 3.3292894517113587,
+            "timestamp": 1.7447855049611387
+        },
+        {
+            "x": 2.3646621591221844,
+            "y": 3.4343941523127826,
+            "heading": 0.22732276450933128,
+            "angularVelocity": -0.000007183855179950579,
+            "velocityX": -1.2992284428512828,
+            "velocityY": 3.5528336417572115,
+            "timestamp": 1.810144159722043
+        },
+        {
+            "x": 2.3096843159184894,
+            "y": 3.656196634862642,
+            "heading": 0.16843360905648927,
+            "angularVelocity": -0.9010154151467895,
+            "velocityX": -0.8411715847703609,
+            "velocityY": 3.3936206820850954,
+            "timestamp": 1.875502814482947
+        },
+        {
+            "x": 2.277656929328602,
+            "y": 3.853541817964372,
+            "heading": 0.0672675417750508,
+            "angularVelocity": -1.547860304829062,
+            "velocityX": -0.49002518039951914,
+            "velocityY": 3.0194192922676324,
+            "timestamp": 1.9408614692438513
+        },
+        {
+            "x": 2.265357919525011,
+            "y": 4.021569174077037,
+            "heading": -0.049806740829473055,
+            "angularVelocity": -1.791259061753831,
+            "velocityX": -0.188177217670429,
+            "velocityY": 2.570850895376378,
+            "timestamp": 2.0062201240047557
+        },
+        {
+            "x": 2.269649055536998,
+            "y": 4.158080976089645,
+            "heading": -0.1685192187929243,
+            "angularVelocity": -1.8163237661135343,
+            "velocityX": 0.06565520706762865,
+            "velocityY": 2.088656850603768,
+            "timestamp": 2.07157877876566
+        },
+        {
+            "x": 2.2810300353091644,
+            "y": 4.259951613943982,
+            "heading": -0.2719091294332062,
+            "angularVelocity": -1.581885536330179,
+            "velocityX": 0.1741311814602323,
+            "velocityY": 1.5586403702310236,
+            "timestamp": 2.1369374335265645
+        },
+        {
+            "x": 2.2926691286250658,
+            "y": 4.326989538481201,
+            "heading": -0.34849979264864867,
+            "angularVelocity": -1.1718518916219969,
+            "velocityX": 0.1780803683686499,
+            "velocityY": 1.0256931508529106,
+            "timestamp": 2.202296088287469
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.6349611616573703,
+            "velocityX": 0.11216374329844404,
+            "velocityY": 0.5050664160631704,
+            "timestamp": 2.2676547430483733
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -1.761626207104486e-30,
+            "velocityX": 0,
+            "velocityY": -1.839087679439853e-31,
+            "timestamp": 2.3330133978092777
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.1.traj
index 9be2eeb8..4db64f81 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.1.traj	
@@ -3,209 +3,209 @@
         {
             "x": 1.35,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 4.242495082651476e-31,
-            "velocityX": 1.1828951034939025e-32,
-            "velocityY": -1.8669507935571478e-37,
+            "heading": 0,
+            "angularVelocity": 2.6743025901632004e-33,
+            "velocityX": 2.068786142168461e-33,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 1.458096319535533,
-            "y": 5.590985596762502,
-            "heading": 3.1146996746822455,
-            "angularVelocity": -0.21764643682588541,
-            "velocityX": 0.9299002477407399,
-            "velocityY": 0.2665540716802577,
-            "timestamp": 0.11624507015474053
-        },
-        {
-            "x": 1.674289226072187,
-            "y": 5.652956746779397,
-            "heading": 3.0641193580719652,
-            "angularVelocity": -0.4351179498876838,
-            "velocityX": 1.8598027963583075,
-            "velocityY": 0.5331077690813153,
-            "timestamp": 0.23249014030948106
-        },
-        {
-            "x": 1.9985791294926345,
-            "y": 5.745913563807996,
-            "heading": 2.9882935824900567,
-            "angularVelocity": -0.652292398128992,
-            "velocityX": 2.789708870997854,
-            "velocityY": 0.7996624450813951,
-            "timestamp": 0.3487352104642216
-        },
-        {
-            "x": 2.421300960273809,
-            "y": 5.867094149195328,
-            "heading": 2.9882935750965625,
-            "angularVelocity": -6.360264782367115e-8,
-            "velocityX": 3.6364710367369995,
-            "velocityY": 1.0424578455329059,
-            "timestamp": 0.4649802806189621
-        },
-        {
-            "x": 2.844022813063846,
-            "y": 5.988274657808269,
-            "heading": 2.988293567703214,
-            "angularVelocity": -6.360139701488917e-8,
-            "velocityX": 3.636471226068578,
-            "velocityY": 1.0424571850800302,
-            "timestamp": 0.5812253507737026
-        },
-        {
-            "x": 3.266744690508405,
-            "y": 6.109455080417214,
-            "heading": 2.988293560309863,
-            "angularVelocity": -6.360141400691824e-8,
-            "velocityX": 3.636471438159484,
-            "velocityY": 1.0424564452293308,
-            "timestamp": 0.6974704209284431
-        },
-        {
-            "x": 3.6894665932724995,
-            "y": 6.2306354147022835,
-            "heading": 2.9882935529165104,
-            "angularVelocity": -6.360143032479277e-8,
-            "velocityX": 3.6364716559711723,
-            "velocityY": 1.0424556854218385,
-            "timestamp": 0.8137154910831836
-        },
-        {
-            "x": 4.112188522356607,
-            "y": 6.35181565717337,
-            "heading": 2.988293545523163,
-            "angularVelocity": -6.360138139725806e-8,
-            "velocityX": 3.6364718823894826,
-            "velocityY": 1.042454895590637,
-            "timestamp": 0.9299605612379241
-        },
-        {
-            "x": 4.534910480631917,
-            "y": 6.472995797814601,
-            "heading": 2.988293538129828,
-            "angularVelocity": -6.360128257953913e-8,
-            "velocityX": 3.636472133507264,
-            "velocityY": 1.0424540195977474,
-            "timestamp": 1.0462056313926646
-        },
-        {
-            "x": 4.95763256393494,
-            "y": 6.594175502311251,
-            "heading": 2.9882935307365046,
-            "angularVelocity": -6.360117492960731e-8,
-            "velocityX": 3.6364732090600738,
-            "velocityY": 1.0424502676572964,
-            "timestamp": 1.1624507015474053
-        },
-        {
-            "x": 5.395072294009931,
-            "y": 6.63917509969073,
-            "heading": 2.988293514020312,
-            "angularVelocity": -1.4380130211846583e-7,
-            "velocityX": 3.763081991285213,
-            "velocityY": 0.3871097270583396,
-            "timestamp": 1.2786957717021459
-        },
-        {
-            "x": 5.834408437796079,
-            "y": 6.6201427711176,
-            "heading": 2.9882934665190843,
-            "angularVelocity": -4.0863004174605414e-7,
-            "velocityX": 3.7793959193394016,
-            "velocityY": -0.16372589863635184,
-            "timestamp": 1.3949408418568865
-        },
-        {
-            "x": 6.258700069994256,
-            "y": 6.504578567848497,
-            "heading": 2.988293412737735,
-            "angularVelocity": -4.6265488113342876e-7,
-            "velocityX": 3.649975277518237,
-            "velocityY": -0.9941428321671584,
-            "timestamp": 1.511185912011627
-        },
-        {
-            "x": 6.6396986873456,
-            "y": 6.284991393688008,
-            "heading": 2.988293385419437,
-            "angularVelocity": -2.350060774071174e-7,
-            "velocityX": 3.277546452887644,
-            "velocityY": -1.8890020356836066,
-            "timestamp": 1.6274309821663677
-        },
-        {
-            "x": 6.952392117761244,
-            "y": 5.9757968201953435,
-            "heading": 2.988293399279684,
-            "angularVelocity": 1.1923298633596667e-7,
-            "velocityX": 2.689950034004883,
-            "velocityY": -2.6598510636285697,
-            "timestamp": 1.7436760523211083
-        },
-        {
-            "x": 7.176252511109016,
-            "y": 5.597293189983828,
-            "heading": 2.988293408698133,
-            "angularVelocity": 8.102235350708823e-8,
-            "velocityX": 1.9257624693226023,
-            "velocityY": -3.2560832877271055,
-            "timestamp": 1.859921122475849
-        },
-        {
-            "x": 7.318788154943764,
-            "y": 5.181285899414456,
-            "heading": 2.98829341726252,
-            "angularVelocity": 7.367527341392295e-8,
-            "velocityX": 1.226165063559341,
-            "velocityY": -3.5787091015180317,
-            "timestamp": 1.9761661926305896
-        },
-        {
-            "x": 7.461323091390308,
-            "y": 4.765278366475121,
-            "heading": 2.9882934258271017,
-            "angularVelocity": 7.36769447232769e-8,
-            "velocityX": 1.2261589782414601,
-            "velocityY": -3.578711186509359,
-            "timestamp": 2.09241126278533
-        },
-        {
-            "x": 7.570661558741158,
-            "y": 4.44613912293617,
-            "heading": 3.0641409244839792,
-            "angularVelocity": 0.6524792712147914,
-            "velocityX": 0.9405858433850398,
-            "velocityY": -2.745400240320956,
-            "timestamp": 2.2086563329400706
-        },
-        {
-            "x": 7.643553854632525,
-            "y": 4.2333797165247065,
-            "heading": 3.1147139591088293,
-            "angularVelocity": 0.43505530649626106,
-            "velocityX": 0.6270570940714855,
-            "velocityY": -1.8302660588379978,
-            "timestamp": 2.324901403094811
+            "x": 1.4007220754094205,
+            "y": 5.545584664540576,
+            "heading": -0.37232774466736046,
+            "angularVelocity": -3.437582019103784,
+            "velocityX": 0.4683005682394807,
+            "velocityY": -0.13309214444637307,
+            "timestamp": 0.10831094141120465
+        },
+        {
+            "x": 1.5327694511201624,
+            "y": 5.533363500300387,
+            "heading": -0.9362708794209632,
+            "angularVelocity": -5.206705134364786,
+            "velocityX": 1.2191508446909478,
+            "velocityY": -0.11283406903270367,
+            "timestamp": 0.2166218828224093
+        },
+        {
+            "x": 1.760106911270163,
+            "y": 5.516367699163042,
+            "heading": -1.4218044496098983,
+            "angularVelocity": -4.482774905866594,
+            "velocityX": 2.0989334705060814,
+            "velocityY": -0.1569167520465022,
+            "timestamp": 0.3249328242336139
+        },
+        {
+            "x": 2.067387230758427,
+            "y": 5.467280597023701,
+            "heading": -1.7292049865361503,
+            "angularVelocity": -2.838130044121764,
+            "velocityX": 2.83702011527781,
+            "velocityY": -0.453205387191498,
+            "timestamp": 0.4332437656448186
+        },
+        {
+            "x": 2.4488593918735733,
+            "y": 5.393167697716653,
+            "heading": -1.7983473850598053,
+            "angularVelocity": -0.6383694723984937,
+            "velocityX": 3.5220094677866336,
+            "velocityY": -0.6842605035226905,
+            "timestamp": 0.5415547070560233
+        },
+        {
+            "x": 2.8531804118778052,
+            "y": 5.326790341993422,
+            "heading": -1.7983490370676205,
+            "angularVelocity": -0.000015252455509683049,
+            "velocityX": 3.7329656148885193,
+            "velocityY": -0.6128407237384188,
+            "timestamp": 0.6498656484672279
+        },
+        {
+            "x": 3.257501404598323,
+            "y": 5.2604128200678275,
+            "heading": -1.7983506890680847,
+            "angularVelocity": -0.00001525238764202395,
+            "velocityX": 3.732965362986781,
+            "velocityY": -0.6128422582312392,
+            "timestamp": 0.7581765898784326
+        },
+        {
+            "x": 3.6466232677858144,
+            "y": 5.172310745117625,
+            "heading": -1.8337945854134767,
+            "angularVelocity": -0.32724206699329256,
+            "velocityX": 3.5926367005728674,
+            "velocityY": -0.8134180517896293,
+            "timestamp": 0.8664875312896373
+        },
+        {
+            "x": 4.014238623942389,
+            "y": 5.062846847355338,
+            "heading": -1.917513965365452,
+            "angularVelocity": -0.7729540419571551,
+            "velocityX": 3.394074055370967,
+            "velocityY": -1.0106448742487064,
+            "timestamp": 0.974798472700842
+        },
+        {
+            "x": 4.354761888880782,
+            "y": 4.880183720994759,
+            "heading": -1.9914398534549198,
+            "angularVelocity": -0.6825338892476871,
+            "velocityX": 3.143941512294591,
+            "velocityY": -1.6864697507068689,
+            "timestamp": 1.0831094141120465
+        },
+        {
+            "x": 4.661144802240214,
+            "y": 4.608133464126214,
+            "heading": -1.9914398486412812,
+            "angularVelocity": 4.444277523534959e-8,
+            "velocityX": 2.8287346538355926,
+            "velocityY": -2.5117523061285136,
+            "timestamp": 1.1914203555232512
+        },
+        {
+            "x": 5.019380877625839,
+            "y": 4.418922250546549,
+            "heading": -1.9767149939198712,
+            "angularVelocity": 0.13594983599585594,
+            "velocityX": 3.3074781801182462,
+            "velocityY": -1.7469261287400477,
+            "timestamp": 1.2997312969344559
+        },
+        {
+            "x": 5.376950498025801,
+            "y": 4.3069722509438675,
+            "heading": -1.8614424797513431,
+            "angularVelocity": 1.0642739566900585,
+            "velocityX": 3.3013250161167162,
+            "velocityY": -1.0335982509621156,
+            "timestamp": 1.4080422383456606
+        },
+        {
+            "x": 5.749629435463048,
+            "y": 4.240661073830716,
+            "heading": -1.7582912287295662,
+            "angularVelocity": 0.9523622422425545,
+            "velocityX": 3.4408244687151703,
+            "velocityY": -0.6122297179691145,
+            "timestamp": 1.5163531797568652
+        },
+        {
+            "x": 6.158173088993434,
+            "y": 4.209455444526307,
+            "heading": -1.7582906674190417,
+            "angularVelocity": 0.000005182399092709644,
+            "velocityX": 3.771951828756997,
+            "velocityY": -0.2881115139230142,
+            "timestamp": 1.62466412116807
+        },
+        {
+            "x": 6.566716775041419,
+            "y": 4.178250240929697,
+            "heading": -1.7582901061123783,
+            "angularVelocity": 0.000005182363444203767,
+            "velocityX": 3.771952128981517,
+            "velocityY": -0.2881075835001592,
+            "timestamp": 1.7329750625792746
+        },
+        {
+            "x": 6.948239527399369,
+            "y": 4.140315578246664,
+            "heading": -1.6699172621402418,
+            "angularVelocity": 0.8159179748667066,
+            "velocityX": 3.5224765604196056,
+            "velocityY": -0.35023850950582514,
+            "timestamp": 1.8412860039904793
+        },
+        {
+            "x": 7.2588942750640015,
+            "y": 4.14084991195257,
+            "heading": -1.390296709754989,
+            "angularVelocity": 2.5816464037891405,
+            "velocityX": 2.86817512263351,
+            "velocityY": 0.004933330824608595,
+            "timestamp": 1.949596945401684
+        },
+        {
+            "x": 7.4846066646178775,
+            "y": 4.133171401549216,
+            "heading": -0.9384850139510863,
+            "angularVelocity": 4.17143171241205,
+            "velocityX": 2.083929717653862,
+            "velocityY": -0.07089321081793584,
+            "timestamp": 2.0579078868128886
+        },
+        {
+            "x": 7.620450068903259,
+            "y": 4.128683952785512,
+            "heading": -0.34333254233125515,
+            "angularVelocity": 5.494850879010672,
+            "velocityX": 1.2541983525897882,
+            "velocityY": -0.04143116757398245,
+            "timestamp": 2.166218828224093
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.21752355482697977,
-            "velocityX": 0.31352852485666055,
-            "velocityY": -0.9151331439956911,
-            "timestamp": 2.4411464732495514
+            "heading": 9.09513036114211e-34,
+            "angularVelocity": 3.1698786646843584,
+            "velocityX": 0.5498053134877472,
+            "velocityY": -0.015547393121808115,
+            "timestamp": 2.2745297696352975
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": -2.3792206080273036e-31,
+            "heading": 0,
+            "angularVelocity": -7.032634787663221e-33,
             "velocityX": 0,
-            "velocityY": -4.1394011967092386e-32,
-            "timestamp": 2.5573915434042918
+            "velocityY": -1.950631326224301e-32,
+            "timestamp": 2.382840711046502
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.traj
index 9be2eeb8..4db64f81 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.traj	
@@ -3,209 +3,209 @@
         {
             "x": 1.35,
             "y": 5.56,
-            "heading": 3.14,
-            "angularVelocity": 4.242495082651476e-31,
-            "velocityX": 1.1828951034939025e-32,
-            "velocityY": -1.8669507935571478e-37,
+            "heading": 0,
+            "angularVelocity": 2.6743025901632004e-33,
+            "velocityX": 2.068786142168461e-33,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 1.458096319535533,
-            "y": 5.590985596762502,
-            "heading": 3.1146996746822455,
-            "angularVelocity": -0.21764643682588541,
-            "velocityX": 0.9299002477407399,
-            "velocityY": 0.2665540716802577,
-            "timestamp": 0.11624507015474053
-        },
-        {
-            "x": 1.674289226072187,
-            "y": 5.652956746779397,
-            "heading": 3.0641193580719652,
-            "angularVelocity": -0.4351179498876838,
-            "velocityX": 1.8598027963583075,
-            "velocityY": 0.5331077690813153,
-            "timestamp": 0.23249014030948106
-        },
-        {
-            "x": 1.9985791294926345,
-            "y": 5.745913563807996,
-            "heading": 2.9882935824900567,
-            "angularVelocity": -0.652292398128992,
-            "velocityX": 2.789708870997854,
-            "velocityY": 0.7996624450813951,
-            "timestamp": 0.3487352104642216
-        },
-        {
-            "x": 2.421300960273809,
-            "y": 5.867094149195328,
-            "heading": 2.9882935750965625,
-            "angularVelocity": -6.360264782367115e-8,
-            "velocityX": 3.6364710367369995,
-            "velocityY": 1.0424578455329059,
-            "timestamp": 0.4649802806189621
-        },
-        {
-            "x": 2.844022813063846,
-            "y": 5.988274657808269,
-            "heading": 2.988293567703214,
-            "angularVelocity": -6.360139701488917e-8,
-            "velocityX": 3.636471226068578,
-            "velocityY": 1.0424571850800302,
-            "timestamp": 0.5812253507737026
-        },
-        {
-            "x": 3.266744690508405,
-            "y": 6.109455080417214,
-            "heading": 2.988293560309863,
-            "angularVelocity": -6.360141400691824e-8,
-            "velocityX": 3.636471438159484,
-            "velocityY": 1.0424564452293308,
-            "timestamp": 0.6974704209284431
-        },
-        {
-            "x": 3.6894665932724995,
-            "y": 6.2306354147022835,
-            "heading": 2.9882935529165104,
-            "angularVelocity": -6.360143032479277e-8,
-            "velocityX": 3.6364716559711723,
-            "velocityY": 1.0424556854218385,
-            "timestamp": 0.8137154910831836
-        },
-        {
-            "x": 4.112188522356607,
-            "y": 6.35181565717337,
-            "heading": 2.988293545523163,
-            "angularVelocity": -6.360138139725806e-8,
-            "velocityX": 3.6364718823894826,
-            "velocityY": 1.042454895590637,
-            "timestamp": 0.9299605612379241
-        },
-        {
-            "x": 4.534910480631917,
-            "y": 6.472995797814601,
-            "heading": 2.988293538129828,
-            "angularVelocity": -6.360128257953913e-8,
-            "velocityX": 3.636472133507264,
-            "velocityY": 1.0424540195977474,
-            "timestamp": 1.0462056313926646
-        },
-        {
-            "x": 4.95763256393494,
-            "y": 6.594175502311251,
-            "heading": 2.9882935307365046,
-            "angularVelocity": -6.360117492960731e-8,
-            "velocityX": 3.6364732090600738,
-            "velocityY": 1.0424502676572964,
-            "timestamp": 1.1624507015474053
-        },
-        {
-            "x": 5.395072294009931,
-            "y": 6.63917509969073,
-            "heading": 2.988293514020312,
-            "angularVelocity": -1.4380130211846583e-7,
-            "velocityX": 3.763081991285213,
-            "velocityY": 0.3871097270583396,
-            "timestamp": 1.2786957717021459
-        },
-        {
-            "x": 5.834408437796079,
-            "y": 6.6201427711176,
-            "heading": 2.9882934665190843,
-            "angularVelocity": -4.0863004174605414e-7,
-            "velocityX": 3.7793959193394016,
-            "velocityY": -0.16372589863635184,
-            "timestamp": 1.3949408418568865
-        },
-        {
-            "x": 6.258700069994256,
-            "y": 6.504578567848497,
-            "heading": 2.988293412737735,
-            "angularVelocity": -4.6265488113342876e-7,
-            "velocityX": 3.649975277518237,
-            "velocityY": -0.9941428321671584,
-            "timestamp": 1.511185912011627
-        },
-        {
-            "x": 6.6396986873456,
-            "y": 6.284991393688008,
-            "heading": 2.988293385419437,
-            "angularVelocity": -2.350060774071174e-7,
-            "velocityX": 3.277546452887644,
-            "velocityY": -1.8890020356836066,
-            "timestamp": 1.6274309821663677
-        },
-        {
-            "x": 6.952392117761244,
-            "y": 5.9757968201953435,
-            "heading": 2.988293399279684,
-            "angularVelocity": 1.1923298633596667e-7,
-            "velocityX": 2.689950034004883,
-            "velocityY": -2.6598510636285697,
-            "timestamp": 1.7436760523211083
-        },
-        {
-            "x": 7.176252511109016,
-            "y": 5.597293189983828,
-            "heading": 2.988293408698133,
-            "angularVelocity": 8.102235350708823e-8,
-            "velocityX": 1.9257624693226023,
-            "velocityY": -3.2560832877271055,
-            "timestamp": 1.859921122475849
-        },
-        {
-            "x": 7.318788154943764,
-            "y": 5.181285899414456,
-            "heading": 2.98829341726252,
-            "angularVelocity": 7.367527341392295e-8,
-            "velocityX": 1.226165063559341,
-            "velocityY": -3.5787091015180317,
-            "timestamp": 1.9761661926305896
-        },
-        {
-            "x": 7.461323091390308,
-            "y": 4.765278366475121,
-            "heading": 2.9882934258271017,
-            "angularVelocity": 7.36769447232769e-8,
-            "velocityX": 1.2261589782414601,
-            "velocityY": -3.578711186509359,
-            "timestamp": 2.09241126278533
-        },
-        {
-            "x": 7.570661558741158,
-            "y": 4.44613912293617,
-            "heading": 3.0641409244839792,
-            "angularVelocity": 0.6524792712147914,
-            "velocityX": 0.9405858433850398,
-            "velocityY": -2.745400240320956,
-            "timestamp": 2.2086563329400706
-        },
-        {
-            "x": 7.643553854632525,
-            "y": 4.2333797165247065,
-            "heading": 3.1147139591088293,
-            "angularVelocity": 0.43505530649626106,
-            "velocityX": 0.6270570940714855,
-            "velocityY": -1.8302660588379978,
-            "timestamp": 2.324901403094811
+            "x": 1.4007220754094205,
+            "y": 5.545584664540576,
+            "heading": -0.37232774466736046,
+            "angularVelocity": -3.437582019103784,
+            "velocityX": 0.4683005682394807,
+            "velocityY": -0.13309214444637307,
+            "timestamp": 0.10831094141120465
+        },
+        {
+            "x": 1.5327694511201624,
+            "y": 5.533363500300387,
+            "heading": -0.9362708794209632,
+            "angularVelocity": -5.206705134364786,
+            "velocityX": 1.2191508446909478,
+            "velocityY": -0.11283406903270367,
+            "timestamp": 0.2166218828224093
+        },
+        {
+            "x": 1.760106911270163,
+            "y": 5.516367699163042,
+            "heading": -1.4218044496098983,
+            "angularVelocity": -4.482774905866594,
+            "velocityX": 2.0989334705060814,
+            "velocityY": -0.1569167520465022,
+            "timestamp": 0.3249328242336139
+        },
+        {
+            "x": 2.067387230758427,
+            "y": 5.467280597023701,
+            "heading": -1.7292049865361503,
+            "angularVelocity": -2.838130044121764,
+            "velocityX": 2.83702011527781,
+            "velocityY": -0.453205387191498,
+            "timestamp": 0.4332437656448186
+        },
+        {
+            "x": 2.4488593918735733,
+            "y": 5.393167697716653,
+            "heading": -1.7983473850598053,
+            "angularVelocity": -0.6383694723984937,
+            "velocityX": 3.5220094677866336,
+            "velocityY": -0.6842605035226905,
+            "timestamp": 0.5415547070560233
+        },
+        {
+            "x": 2.8531804118778052,
+            "y": 5.326790341993422,
+            "heading": -1.7983490370676205,
+            "angularVelocity": -0.000015252455509683049,
+            "velocityX": 3.7329656148885193,
+            "velocityY": -0.6128407237384188,
+            "timestamp": 0.6498656484672279
+        },
+        {
+            "x": 3.257501404598323,
+            "y": 5.2604128200678275,
+            "heading": -1.7983506890680847,
+            "angularVelocity": -0.00001525238764202395,
+            "velocityX": 3.732965362986781,
+            "velocityY": -0.6128422582312392,
+            "timestamp": 0.7581765898784326
+        },
+        {
+            "x": 3.6466232677858144,
+            "y": 5.172310745117625,
+            "heading": -1.8337945854134767,
+            "angularVelocity": -0.32724206699329256,
+            "velocityX": 3.5926367005728674,
+            "velocityY": -0.8134180517896293,
+            "timestamp": 0.8664875312896373
+        },
+        {
+            "x": 4.014238623942389,
+            "y": 5.062846847355338,
+            "heading": -1.917513965365452,
+            "angularVelocity": -0.7729540419571551,
+            "velocityX": 3.394074055370967,
+            "velocityY": -1.0106448742487064,
+            "timestamp": 0.974798472700842
+        },
+        {
+            "x": 4.354761888880782,
+            "y": 4.880183720994759,
+            "heading": -1.9914398534549198,
+            "angularVelocity": -0.6825338892476871,
+            "velocityX": 3.143941512294591,
+            "velocityY": -1.6864697507068689,
+            "timestamp": 1.0831094141120465
+        },
+        {
+            "x": 4.661144802240214,
+            "y": 4.608133464126214,
+            "heading": -1.9914398486412812,
+            "angularVelocity": 4.444277523534959e-8,
+            "velocityX": 2.8287346538355926,
+            "velocityY": -2.5117523061285136,
+            "timestamp": 1.1914203555232512
+        },
+        {
+            "x": 5.019380877625839,
+            "y": 4.418922250546549,
+            "heading": -1.9767149939198712,
+            "angularVelocity": 0.13594983599585594,
+            "velocityX": 3.3074781801182462,
+            "velocityY": -1.7469261287400477,
+            "timestamp": 1.2997312969344559
+        },
+        {
+            "x": 5.376950498025801,
+            "y": 4.3069722509438675,
+            "heading": -1.8614424797513431,
+            "angularVelocity": 1.0642739566900585,
+            "velocityX": 3.3013250161167162,
+            "velocityY": -1.0335982509621156,
+            "timestamp": 1.4080422383456606
+        },
+        {
+            "x": 5.749629435463048,
+            "y": 4.240661073830716,
+            "heading": -1.7582912287295662,
+            "angularVelocity": 0.9523622422425545,
+            "velocityX": 3.4408244687151703,
+            "velocityY": -0.6122297179691145,
+            "timestamp": 1.5163531797568652
+        },
+        {
+            "x": 6.158173088993434,
+            "y": 4.209455444526307,
+            "heading": -1.7582906674190417,
+            "angularVelocity": 0.000005182399092709644,
+            "velocityX": 3.771951828756997,
+            "velocityY": -0.2881115139230142,
+            "timestamp": 1.62466412116807
+        },
+        {
+            "x": 6.566716775041419,
+            "y": 4.178250240929697,
+            "heading": -1.7582901061123783,
+            "angularVelocity": 0.000005182363444203767,
+            "velocityX": 3.771952128981517,
+            "velocityY": -0.2881075835001592,
+            "timestamp": 1.7329750625792746
+        },
+        {
+            "x": 6.948239527399369,
+            "y": 4.140315578246664,
+            "heading": -1.6699172621402418,
+            "angularVelocity": 0.8159179748667066,
+            "velocityX": 3.5224765604196056,
+            "velocityY": -0.35023850950582514,
+            "timestamp": 1.8412860039904793
+        },
+        {
+            "x": 7.2588942750640015,
+            "y": 4.14084991195257,
+            "heading": -1.390296709754989,
+            "angularVelocity": 2.5816464037891405,
+            "velocityX": 2.86817512263351,
+            "velocityY": 0.004933330824608595,
+            "timestamp": 1.949596945401684
+        },
+        {
+            "x": 7.4846066646178775,
+            "y": 4.133171401549216,
+            "heading": -0.9384850139510863,
+            "angularVelocity": 4.17143171241205,
+            "velocityX": 2.083929717653862,
+            "velocityY": -0.07089321081793584,
+            "timestamp": 2.0579078868128886
+        },
+        {
+            "x": 7.620450068903259,
+            "y": 4.128683952785512,
+            "heading": -0.34333254233125515,
+            "angularVelocity": 5.494850879010672,
+            "velocityX": 1.2541983525897882,
+            "velocityY": -0.04143116757398245,
+            "timestamp": 2.166218828224093
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": 0.21752355482697977,
-            "velocityX": 0.31352852485666055,
-            "velocityY": -0.9151331439956911,
-            "timestamp": 2.4411464732495514
+            "heading": 9.09513036114211e-34,
+            "angularVelocity": 3.1698786646843584,
+            "velocityX": 0.5498053134877472,
+            "velocityY": -0.015547393121808115,
+            "timestamp": 2.2745297696352975
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 3.14,
-            "angularVelocity": -2.3792206080273036e-31,
+            "heading": 0,
+            "angularVelocity": -7.032634787663221e-33,
             "velocityX": 0,
-            "velocityY": -4.1394011967092386e-32,
-            "timestamp": 2.5573915434042918
+            "velocityY": -1.950631326224301e-32,
+            "timestamp": 2.382840711046502
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.1.traj
index 0d03ba96..66955466 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.1.traj	
@@ -5,126 +5,126 @@
             "y": 6.63,
             "heading": 0.26,
             "angularVelocity": 0,
-            "velocityX": -1.5159307977916785e-33,
-            "velocityY": 2.4731304546687864e-31,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 5.238209862421909,
-            "y": 6.650800928993829,
-            "heading": 0.2470076817123004,
-            "angularVelocity": -0.14636981892397002,
-            "velocityX": 0.7008484626165542,
-            "velocityY": 0.23434064212588615,
-            "timestamp": 0.08876364255629132
+            "x": 5.238209882810899,
+            "y": 6.650800869780916,
+            "heading": 0.2470077179050236,
+            "angularVelocity": -0.14636941117769697,
+            "velocityX": 0.7008486923098889,
+            "velocityY": 0.23433997503541223,
+            "timestamp": 0.08876364255722656
         },
         {
-            "x": 5.362652882936083,
-            "y": 6.6924048077649365,
-            "heading": 0.22258636188758224,
-            "angularVelocity": -0.27512750853804663,
-            "velocityX": 1.4019593713167977,
-            "velocityY": 0.4687040501427167,
-            "timestamp": 0.17752728511258264
+            "x": 5.362652939631404,
+            "y": 6.692404642961414,
+            "heading": 0.22258646311132924,
+            "angularVelocity": -0.2751267759001645,
+            "velocityX": 1.401959780326283,
+            "velocityY": 0.4687028605622118,
+            "timestamp": 0.17752728511445312
         },
         {
-            "x": 5.549362119474242,
-            "y": 6.7548129470224065,
-            "heading": 0.1893705540696427,
-            "angularVelocity": -0.37420510089603504,
-            "velocityX": 2.1034427065056938,
-            "velocityY": 0.7030822244423112,
-            "timestamp": 0.26629092766887397
+            "x": 5.549362223961953,
+            "y": 6.754812642679732,
+            "heading": 0.18937073322571307,
+            "angularVelocity": -0.37420422291202055,
+            "velocityX": 2.1034432449090223,
+            "velocityY": 0.7030806523975865,
+            "timestamp": 0.26629092767167967
         },
         {
-            "x": 5.798380890690993,
-            "y": 6.838019653320762,
-            "heading": 0.15275441561528638,
-            "angularVelocity": -0.4125127969134817,
-            "velocityX": 2.80541406419279,
-            "velocityY": 0.9373962571031511,
-            "timestamp": 0.3550545702251653
+            "x": 5.798381050208957,
+            "y": 6.8380191875714225,
+            "heading": 0.15275465831989177,
+            "angularVelocity": -0.4125120809759026,
+            "velocityX": 2.8054146841303256,
+            "velocityY": 0.9373944386978721,
+            "timestamp": 0.35505457022890624
         },
         {
-            "x": 6.109622726468052,
-            "y": 6.941929683454389,
-            "heading": 0.13026224178151366,
-            "angularVelocity": -0.2533939931523947,
-            "velocityX": 3.5064112604341973,
-            "velocityY": 1.17063729182453,
-            "timestamp": 0.4438182127814566
+            "x": 6.109622945204874,
+            "y": 6.941929047341865,
+            "heading": 0.1302624797347834,
+            "angularVelocity": -0.25339404667863696,
+            "velocityX": 3.506411927553799,
+            "velocityY": 1.1706353725121932,
+            "timestamp": 0.4438182127861328
         },
         {
-            "x": 6.428044865529091,
-            "y": 7.048515983686504,
-            "heading": 0.13026223073820456,
-            "angularVelocity": -1.2441305517804787e-7,
-            "velocityX": 3.587303651482315,
-            "velocityY": 1.2007878131408318,
-            "timestamp": 0.5325818553377479
+            "x": 6.4280450770381,
+            "y": 7.048515369175043,
+            "heading": 0.1302624686930749,
+            "angularVelocity": -1.2439475582812342e-7,
+            "velocityX": 3.587303570016191,
+            "velocityY": 1.2007880564873126,
+            "timestamp": 0.5325818553433593
         },
         {
-            "x": 6.746467025502362,
-            "y": 7.155102221444599,
-            "heading": 0.13026221969501808,
-            "angularVelocity": -1.244116833309462e-7,
-            "velocityX": 3.5873038870770317,
-            "velocityY": 1.2007871093161688,
-            "timestamp": 0.6213454978940393
+            "x": 6.746467208870004,
+            "y": 7.1551016910121685,
+            "heading": 0.13026245765136438,
+            "angularVelocity": -1.2439477842907252e-7,
+            "velocityX": 3.5873035700012967,
+            "velocityY": 1.2007880565318134,
+            "timestamp": 0.6213454979005859
         },
         {
-            "x": 7.057671328205741,
-            "y": 7.258999778631647,
-            "heading": 0.10797327486362365,
-            "angularVelocity": -0.25110444085082756,
-            "velocityX": 3.505988417567436,
-            "velocityY": 1.1704967731634406,
-            "timestamp": 0.7101091404503306
+            "x": 7.057671454315644,
+            "y": 7.258999412253756,
+            "heading": 0.10797351007767278,
+            "angularVelocity": -0.2511044717394208,
+            "velocityX": 3.5059877724700192,
+            "velocityY": 1.1704986213770205,
+            "timestamp": 0.7101091404578125
         },
         {
-            "x": 7.306674452232408,
-            "y": 7.342201135769052,
-            "heading": 0.07025598564329198,
-            "angularVelocity": -0.4249182225204012,
-            "velocityX": 2.8052377849284973,
-            "velocityY": 0.9373359941155657,
-            "timestamp": 0.7988727830066219
+            "x": 7.3066745305511045,
+            "y": 7.342200909102172,
+            "heading": 0.07025614763700679,
+            "angularVelocity": -0.4249190474112408,
+            "velocityX": 2.8052372464872497,
+            "velocityY": 0.9373375680789704,
+            "timestamp": 0.7988727830150391
         },
         {
-            "x": 7.493367837286706,
-            "y": 7.404603419315824,
-            "heading": 0.03585067831804618,
-            "angularVelocity": -0.38760585227578265,
-            "velocityX": 2.103264125692411,
-            "velocityY": 0.7030162547322278,
-            "timestamp": 0.8876364255629132
+            "x": 7.49336787770315,
+            "y": 7.4046033027166684,
+            "heading": 0.03585076588111199,
+            "angularVelocity": -0.3876066908033862,
+            "velocityX": 2.103263698666567,
+            "velocityY": 0.7030174947390144,
+            "timestamp": 0.8876364255722656
         },
         {
-            "x": 7.617797724211195,
-            "y": 7.446202169802599,
-            "heading": 0.01050114896385133,
-            "angularVelocity": -0.2855845999919636,
-            "velocityX": 1.401811409955239,
-            "velocityY": 0.46864627552974614,
-            "timestamp": 0.9764000681192045
+            "x": 7.617797738081694,
+            "y": 7.446202129881464,
+            "heading": 0.010501179707511226,
+            "angularVelocity": -0.2855852401136722,
+            "velocityX": 1.4018111108759532,
+            "velocityY": 0.4686471393732002,
+            "timestamp": 0.9764000681294922
         },
         {
             "x": 7.68,
             "y": 7.467,
             "heading": -0.003,
-            "angularVelocity": -0.15210224112604764,
-            "velocityX": 0.7007629925666751,
-            "velocityY": 0.2343057314716256,
-            "timestamp": 1.0651637106754959
+            "angularVelocity": -0.15210258748116404,
+            "velocityX": 0.7007628362953491,
+            "velocityY": 0.23430618121741137,
+            "timestamp": 1.0651637106867187
         },
         {
             "x": 7.68,
             "y": 7.467,
             "heading": -0.003,
-            "angularVelocity": 2.057519635914241e-30,
-            "velocityX": 3.0968464213310355e-31,
-            "velocityY": -5.4576131966501765e-31,
-            "timestamp": 1.1539273532317873
+            "angularVelocity": -3.7910464085672074e-31,
+            "velocityX": 1.2889893239032156e-32,
+            "velocityY": 1.1630630989344696e-31,
+            "timestamp": 1.1539273532439451
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.traj
index 0d03ba96..66955466 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.traj	
@@ -5,126 +5,126 @@
             "y": 6.63,
             "heading": 0.26,
             "angularVelocity": 0,
-            "velocityX": -1.5159307977916785e-33,
-            "velocityY": 2.4731304546687864e-31,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 5.238209862421909,
-            "y": 6.650800928993829,
-            "heading": 0.2470076817123004,
-            "angularVelocity": -0.14636981892397002,
-            "velocityX": 0.7008484626165542,
-            "velocityY": 0.23434064212588615,
-            "timestamp": 0.08876364255629132
+            "x": 5.238209882810899,
+            "y": 6.650800869780916,
+            "heading": 0.2470077179050236,
+            "angularVelocity": -0.14636941117769697,
+            "velocityX": 0.7008486923098889,
+            "velocityY": 0.23433997503541223,
+            "timestamp": 0.08876364255722656
         },
         {
-            "x": 5.362652882936083,
-            "y": 6.6924048077649365,
-            "heading": 0.22258636188758224,
-            "angularVelocity": -0.27512750853804663,
-            "velocityX": 1.4019593713167977,
-            "velocityY": 0.4687040501427167,
-            "timestamp": 0.17752728511258264
+            "x": 5.362652939631404,
+            "y": 6.692404642961414,
+            "heading": 0.22258646311132924,
+            "angularVelocity": -0.2751267759001645,
+            "velocityX": 1.401959780326283,
+            "velocityY": 0.4687028605622118,
+            "timestamp": 0.17752728511445312
         },
         {
-            "x": 5.549362119474242,
-            "y": 6.7548129470224065,
-            "heading": 0.1893705540696427,
-            "angularVelocity": -0.37420510089603504,
-            "velocityX": 2.1034427065056938,
-            "velocityY": 0.7030822244423112,
-            "timestamp": 0.26629092766887397
+            "x": 5.549362223961953,
+            "y": 6.754812642679732,
+            "heading": 0.18937073322571307,
+            "angularVelocity": -0.37420422291202055,
+            "velocityX": 2.1034432449090223,
+            "velocityY": 0.7030806523975865,
+            "timestamp": 0.26629092767167967
         },
         {
-            "x": 5.798380890690993,
-            "y": 6.838019653320762,
-            "heading": 0.15275441561528638,
-            "angularVelocity": -0.4125127969134817,
-            "velocityX": 2.80541406419279,
-            "velocityY": 0.9373962571031511,
-            "timestamp": 0.3550545702251653
+            "x": 5.798381050208957,
+            "y": 6.8380191875714225,
+            "heading": 0.15275465831989177,
+            "angularVelocity": -0.4125120809759026,
+            "velocityX": 2.8054146841303256,
+            "velocityY": 0.9373944386978721,
+            "timestamp": 0.35505457022890624
         },
         {
-            "x": 6.109622726468052,
-            "y": 6.941929683454389,
-            "heading": 0.13026224178151366,
-            "angularVelocity": -0.2533939931523947,
-            "velocityX": 3.5064112604341973,
-            "velocityY": 1.17063729182453,
-            "timestamp": 0.4438182127814566
+            "x": 6.109622945204874,
+            "y": 6.941929047341865,
+            "heading": 0.1302624797347834,
+            "angularVelocity": -0.25339404667863696,
+            "velocityX": 3.506411927553799,
+            "velocityY": 1.1706353725121932,
+            "timestamp": 0.4438182127861328
         },
         {
-            "x": 6.428044865529091,
-            "y": 7.048515983686504,
-            "heading": 0.13026223073820456,
-            "angularVelocity": -1.2441305517804787e-7,
-            "velocityX": 3.587303651482315,
-            "velocityY": 1.2007878131408318,
-            "timestamp": 0.5325818553377479
+            "x": 6.4280450770381,
+            "y": 7.048515369175043,
+            "heading": 0.1302624686930749,
+            "angularVelocity": -1.2439475582812342e-7,
+            "velocityX": 3.587303570016191,
+            "velocityY": 1.2007880564873126,
+            "timestamp": 0.5325818553433593
         },
         {
-            "x": 6.746467025502362,
-            "y": 7.155102221444599,
-            "heading": 0.13026221969501808,
-            "angularVelocity": -1.244116833309462e-7,
-            "velocityX": 3.5873038870770317,
-            "velocityY": 1.2007871093161688,
-            "timestamp": 0.6213454978940393
+            "x": 6.746467208870004,
+            "y": 7.1551016910121685,
+            "heading": 0.13026245765136438,
+            "angularVelocity": -1.2439477842907252e-7,
+            "velocityX": 3.5873035700012967,
+            "velocityY": 1.2007880565318134,
+            "timestamp": 0.6213454979005859
         },
         {
-            "x": 7.057671328205741,
-            "y": 7.258999778631647,
-            "heading": 0.10797327486362365,
-            "angularVelocity": -0.25110444085082756,
-            "velocityX": 3.505988417567436,
-            "velocityY": 1.1704967731634406,
-            "timestamp": 0.7101091404503306
+            "x": 7.057671454315644,
+            "y": 7.258999412253756,
+            "heading": 0.10797351007767278,
+            "angularVelocity": -0.2511044717394208,
+            "velocityX": 3.5059877724700192,
+            "velocityY": 1.1704986213770205,
+            "timestamp": 0.7101091404578125
         },
         {
-            "x": 7.306674452232408,
-            "y": 7.342201135769052,
-            "heading": 0.07025598564329198,
-            "angularVelocity": -0.4249182225204012,
-            "velocityX": 2.8052377849284973,
-            "velocityY": 0.9373359941155657,
-            "timestamp": 0.7988727830066219
+            "x": 7.3066745305511045,
+            "y": 7.342200909102172,
+            "heading": 0.07025614763700679,
+            "angularVelocity": -0.4249190474112408,
+            "velocityX": 2.8052372464872497,
+            "velocityY": 0.9373375680789704,
+            "timestamp": 0.7988727830150391
         },
         {
-            "x": 7.493367837286706,
-            "y": 7.404603419315824,
-            "heading": 0.03585067831804618,
-            "angularVelocity": -0.38760585227578265,
-            "velocityX": 2.103264125692411,
-            "velocityY": 0.7030162547322278,
-            "timestamp": 0.8876364255629132
+            "x": 7.49336787770315,
+            "y": 7.4046033027166684,
+            "heading": 0.03585076588111199,
+            "angularVelocity": -0.3876066908033862,
+            "velocityX": 2.103263698666567,
+            "velocityY": 0.7030174947390144,
+            "timestamp": 0.8876364255722656
         },
         {
-            "x": 7.617797724211195,
-            "y": 7.446202169802599,
-            "heading": 0.01050114896385133,
-            "angularVelocity": -0.2855845999919636,
-            "velocityX": 1.401811409955239,
-            "velocityY": 0.46864627552974614,
-            "timestamp": 0.9764000681192045
+            "x": 7.617797738081694,
+            "y": 7.446202129881464,
+            "heading": 0.010501179707511226,
+            "angularVelocity": -0.2855852401136722,
+            "velocityX": 1.4018111108759532,
+            "velocityY": 0.4686471393732002,
+            "timestamp": 0.9764000681294922
         },
         {
             "x": 7.68,
             "y": 7.467,
             "heading": -0.003,
-            "angularVelocity": -0.15210224112604764,
-            "velocityX": 0.7007629925666751,
-            "velocityY": 0.2343057314716256,
-            "timestamp": 1.0651637106754959
+            "angularVelocity": -0.15210258748116404,
+            "velocityX": 0.7007628362953491,
+            "velocityY": 0.23430618121741137,
+            "timestamp": 1.0651637106867187
         },
         {
             "x": 7.68,
             "y": 7.467,
             "heading": -0.003,
-            "angularVelocity": 2.057519635914241e-30,
-            "velocityX": 3.0968464213310355e-31,
-            "velocityY": -5.4576131966501765e-31,
-            "timestamp": 1.1539273532317873
+            "angularVelocity": -3.7910464085672074e-31,
+            "velocityX": 1.2889893239032156e-32,
+            "velocityY": 1.1630630989344696e-31,
+            "timestamp": 1.1539273532439451
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.1.traj
index 6b7f0e9c..63556f1f 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.1.traj	
@@ -5,126 +5,126 @@
             "y": 6.63,
             "heading": 0.26,
             "angularVelocity": 0,
-            "velocityX": -4.471050480852603e-35,
-            "velocityY": 4.2685150703849235e-34,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 5.242063425720018,
-            "y": 6.625917381735586,
-            "heading": 0.21680591867144897,
-            "angularVelocity": -0.4809077162576206,
-            "velocityX": 0.7355269567955554,
-            "velocityY": -0.04545443647608383,
-            "timestamp": 0.08981781715769374
+            "x": 5.242063425541118,
+            "y": 6.625917379024557,
+            "heading": 0.21680591849920633,
+            "angularVelocity": -0.48090771817524164,
+            "velocityX": 0.735526954803639,
+            "velocityY": -0.04545446665971497,
+            "timestamp": 0.08981781715770647
         },
         {
-            "x": 5.374390186000276,
-            "y": 6.616899298560691,
-            "heading": 0.13592925344567436,
-            "angularVelocity": -0.9004523577296353,
-            "velocityX": 1.473279628338445,
-            "velocityY": -0.10040416768380533,
-            "timestamp": 0.17963563431538748
+            "x": 5.374390185654174,
+            "y": 6.616899293663565,
+            "heading": 0.13592925245739512,
+            "angularVelocity": -0.9004523668149724,
+            "velocityX": 1.473279626476675,
+            "velocityY": -0.10040419202302918,
+            "timestamp": 0.17963563431541293
         },
         {
-            "x": 5.573225936929258,
-            "y": 6.6014853361346235,
-            "heading": 0.027174299165301757,
-            "angularVelocity": -1.2108394272088667,
-            "velocityX": 2.2137673484079947,
-            "velocityY": -0.17161363873946028,
-            "timestamp": 0.2694534514730812
+            "x": 5.573225936717345,
+            "y": 6.601485332040172,
+            "heading": 0.027174297785337913,
+            "angularVelocity": -1.2108394315695736,
+            "velocityX": 2.2137673499017074,
+            "velocityY": -0.1716136298027423,
+            "timestamp": 0.2694534514731194
         },
         {
-            "x": 5.838620178684051,
-            "y": 6.5765367651917135,
-            "heading": -0.08847883132007715,
-            "angularVelocity": -1.287641295961646,
-            "velocityX": 2.9548061860470294,
-            "velocityY": -0.27776861799154184,
-            "timestamp": 0.35927126863077496
+            "x": 5.8386201791233034,
+            "y": 6.576536765329541,
+            "heading": -0.0884788299359431,
+            "angularVelocity": -1.287641265186969,
+            "velocityX": 2.9548061932964313,
+            "velocityY": -0.27776857087079165,
+            "timestamp": 0.35927126863082587
         },
         {
-            "x": 6.164628937797069,
-            "y": 6.53182752325973,
-            "heading": -0.12460373302334064,
-            "angularVelocity": -0.4022019555411679,
-            "velocityX": 3.6296669127534233,
-            "velocityY": -0.4977769817483746,
-            "timestamp": 0.44908908578846873
+            "x": 6.16462893780028,
+            "y": 6.53182752327052,
+            "heading": -0.12460373299787371,
+            "angularVelocity": -0.4022019706680331,
+            "velocityX": 3.6296669078981743,
+            "velocityY": -0.49777698316268804,
+            "timestamp": 0.44908908578853235
         },
         {
-            "x": 6.48574789195848,
-            "y": 6.420787077372237,
-            "heading": -0.12460373465453414,
-            "angularVelocity": -1.816113510684907e-8,
-            "velocityX": 3.5752255434756393,
-            "velocityY": -1.2362852872781145,
-            "timestamp": 0.5389069029461625
+            "x": 6.485747891959683,
+            "y": 6.420787077377064,
+            "heading": -0.12460373462910332,
+            "angularVelocity": -1.816153684215185e-8,
+            "velocityX": 3.5752255434527758,
+            "velocityY": -1.2362852873443355,
+            "timestamp": 0.5389069029462388
         },
         {
-            "x": 6.786885646715688,
-            "y": 6.263423119459949,
-            "heading": -0.12460372224054271,
-            "angularVelocity": 1.3821301643987816e-7,
-            "velocityX": 3.3527618938734416,
-            "velocityY": -1.7520349847291758,
-            "timestamp": 0.6287247201038563
+            "x": 6.7868856142210525,
+            "y": 6.263423057279399,
+            "heading": -0.12460372221510443,
+            "angularVelocity": 1.382130993675135e-7,
+            "velocityX": 3.352761532075727,
+            "velocityY": -1.7520356770790513,
+            "timestamp": 0.6287247201039452
         },
         {
-            "x": 7.084493676309923,
-            "y": 6.108042134366637,
-            "heading": -0.11377963368638179,
-            "angularVelocity": 0.12051159665968046,
-            "velocityX": 3.313463174814436,
-            "velocityY": -1.729957262494017,
-            "timestamp": 0.71854253726155
+            "x": 7.0844936481565455,
+            "y": 6.108042080528118,
+            "heading": -0.11377963349551397,
+            "angularVelocity": 0.12051159850149781,
+            "velocityX": 3.313463223148008,
+            "velocityY": -1.7299571696165401,
+            "timestamp": 0.7185425372616516
         },
         {
-            "x": 7.322771133319765,
-            "y": 5.983596419357133,
-            "heading": -0.07830445075984473,
-            "angularVelocity": 0.3949682150953753,
-            "velocityX": 2.652897437837947,
-            "velocityY": -1.3855348409437966,
-            "timestamp": 0.8083603544192438
+            "x": 7.322771112714566,
+            "y": 5.983596379959823,
+            "heading": -0.07830444960996434,
+            "angularVelocity": 0.39496822577262786,
+            "velocityX": 2.652897521876331,
+            "velocityY": -1.3855346801602506,
+            "timestamp": 0.808360354419358
         },
         {
-            "x": 7.5014211847430525,
-            "y": 5.8902827728941185,
-            "heading": -0.04357887601405372,
-            "angularVelocity": 0.38662234114221544,
-            "velocityX": 1.9890268665695783,
-            "velocityY": -1.0389213344962835,
-            "timestamp": 0.8981781715769376
+            "x": 7.501421172621153,
+            "y": 5.8902827497271355,
+            "heading": -0.04357887494407857,
+            "angularVelocity": 0.3866223402525239,
+            "velocityX": 1.9890269610193678,
+            "velocityY": -1.038921153793384,
+            "timestamp": 0.8981781715770645
         },
         {
-            "x": 7.620483882250166,
-            "y": 5.828089677839219,
-            "heading": -0.01724918141894823,
-            "angularVelocity": 0.293145563189075,
-            "velocityX": 1.3256022165187353,
-            "velocityY": -0.6924360558184839,
-            "timestamp": 0.9879959887346313
+            "x": 7.62048387761172,
+            "y": 5.828089668977501,
+            "heading": -0.01724918093308134,
+            "angularVelocity": 0.29314555668577735,
+            "velocityX": 1.3256022998366965,
+            "velocityY": -0.6924358965486016,
+            "timestamp": 0.9879959887347709
         },
         {
             "x": 7.68,
             "y": 5.797,
             "heading": -0.003,
-            "angularVelocity": 0.1586453764950761,
-            "velocityX": 0.6626315316185003,
-            "velocityY": -0.346141543215585,
-            "timestamp": 1.077813805892325
+            "angularVelocity": 0.15864537108558247,
+            "velocityX": 0.6626315832612425,
+            "velocityY": -0.346141444552285,
+            "timestamp": 1.0778138058924773
         },
         {
             "x": 7.68,
             "y": 5.797,
             "heading": -0.003,
-            "angularVelocity": 2.21164904203514e-32,
-            "velocityX": 6.504499750841485e-34,
-            "velocityY": 5.343957154186367e-33,
-            "timestamp": 1.1676316230500188
+            "angularVelocity": 0,
+            "velocityX": 4.386752331989148e-33,
+            "velocityY": -2.33652586669764e-33,
+            "timestamp": 1.1676316230501838
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.traj
index 6b7f0e9c..63556f1f 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.traj	
@@ -5,126 +5,126 @@
             "y": 6.63,
             "heading": 0.26,
             "angularVelocity": 0,
-            "velocityX": -4.471050480852603e-35,
-            "velocityY": 4.2685150703849235e-34,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 5.242063425720018,
-            "y": 6.625917381735586,
-            "heading": 0.21680591867144897,
-            "angularVelocity": -0.4809077162576206,
-            "velocityX": 0.7355269567955554,
-            "velocityY": -0.04545443647608383,
-            "timestamp": 0.08981781715769374
+            "x": 5.242063425541118,
+            "y": 6.625917379024557,
+            "heading": 0.21680591849920633,
+            "angularVelocity": -0.48090771817524164,
+            "velocityX": 0.735526954803639,
+            "velocityY": -0.04545446665971497,
+            "timestamp": 0.08981781715770647
         },
         {
-            "x": 5.374390186000276,
-            "y": 6.616899298560691,
-            "heading": 0.13592925344567436,
-            "angularVelocity": -0.9004523577296353,
-            "velocityX": 1.473279628338445,
-            "velocityY": -0.10040416768380533,
-            "timestamp": 0.17963563431538748
+            "x": 5.374390185654174,
+            "y": 6.616899293663565,
+            "heading": 0.13592925245739512,
+            "angularVelocity": -0.9004523668149724,
+            "velocityX": 1.473279626476675,
+            "velocityY": -0.10040419202302918,
+            "timestamp": 0.17963563431541293
         },
         {
-            "x": 5.573225936929258,
-            "y": 6.6014853361346235,
-            "heading": 0.027174299165301757,
-            "angularVelocity": -1.2108394272088667,
-            "velocityX": 2.2137673484079947,
-            "velocityY": -0.17161363873946028,
-            "timestamp": 0.2694534514730812
+            "x": 5.573225936717345,
+            "y": 6.601485332040172,
+            "heading": 0.027174297785337913,
+            "angularVelocity": -1.2108394315695736,
+            "velocityX": 2.2137673499017074,
+            "velocityY": -0.1716136298027423,
+            "timestamp": 0.2694534514731194
         },
         {
-            "x": 5.838620178684051,
-            "y": 6.5765367651917135,
-            "heading": -0.08847883132007715,
-            "angularVelocity": -1.287641295961646,
-            "velocityX": 2.9548061860470294,
-            "velocityY": -0.27776861799154184,
-            "timestamp": 0.35927126863077496
+            "x": 5.8386201791233034,
+            "y": 6.576536765329541,
+            "heading": -0.0884788299359431,
+            "angularVelocity": -1.287641265186969,
+            "velocityX": 2.9548061932964313,
+            "velocityY": -0.27776857087079165,
+            "timestamp": 0.35927126863082587
         },
         {
-            "x": 6.164628937797069,
-            "y": 6.53182752325973,
-            "heading": -0.12460373302334064,
-            "angularVelocity": -0.4022019555411679,
-            "velocityX": 3.6296669127534233,
-            "velocityY": -0.4977769817483746,
-            "timestamp": 0.44908908578846873
+            "x": 6.16462893780028,
+            "y": 6.53182752327052,
+            "heading": -0.12460373299787371,
+            "angularVelocity": -0.4022019706680331,
+            "velocityX": 3.6296669078981743,
+            "velocityY": -0.49777698316268804,
+            "timestamp": 0.44908908578853235
         },
         {
-            "x": 6.48574789195848,
-            "y": 6.420787077372237,
-            "heading": -0.12460373465453414,
-            "angularVelocity": -1.816113510684907e-8,
-            "velocityX": 3.5752255434756393,
-            "velocityY": -1.2362852872781145,
-            "timestamp": 0.5389069029461625
+            "x": 6.485747891959683,
+            "y": 6.420787077377064,
+            "heading": -0.12460373462910332,
+            "angularVelocity": -1.816153684215185e-8,
+            "velocityX": 3.5752255434527758,
+            "velocityY": -1.2362852873443355,
+            "timestamp": 0.5389069029462388
         },
         {
-            "x": 6.786885646715688,
-            "y": 6.263423119459949,
-            "heading": -0.12460372224054271,
-            "angularVelocity": 1.3821301643987816e-7,
-            "velocityX": 3.3527618938734416,
-            "velocityY": -1.7520349847291758,
-            "timestamp": 0.6287247201038563
+            "x": 6.7868856142210525,
+            "y": 6.263423057279399,
+            "heading": -0.12460372221510443,
+            "angularVelocity": 1.382130993675135e-7,
+            "velocityX": 3.352761532075727,
+            "velocityY": -1.7520356770790513,
+            "timestamp": 0.6287247201039452
         },
         {
-            "x": 7.084493676309923,
-            "y": 6.108042134366637,
-            "heading": -0.11377963368638179,
-            "angularVelocity": 0.12051159665968046,
-            "velocityX": 3.313463174814436,
-            "velocityY": -1.729957262494017,
-            "timestamp": 0.71854253726155
+            "x": 7.0844936481565455,
+            "y": 6.108042080528118,
+            "heading": -0.11377963349551397,
+            "angularVelocity": 0.12051159850149781,
+            "velocityX": 3.313463223148008,
+            "velocityY": -1.7299571696165401,
+            "timestamp": 0.7185425372616516
         },
         {
-            "x": 7.322771133319765,
-            "y": 5.983596419357133,
-            "heading": -0.07830445075984473,
-            "angularVelocity": 0.3949682150953753,
-            "velocityX": 2.652897437837947,
-            "velocityY": -1.3855348409437966,
-            "timestamp": 0.8083603544192438
+            "x": 7.322771112714566,
+            "y": 5.983596379959823,
+            "heading": -0.07830444960996434,
+            "angularVelocity": 0.39496822577262786,
+            "velocityX": 2.652897521876331,
+            "velocityY": -1.3855346801602506,
+            "timestamp": 0.808360354419358
         },
         {
-            "x": 7.5014211847430525,
-            "y": 5.8902827728941185,
-            "heading": -0.04357887601405372,
-            "angularVelocity": 0.38662234114221544,
-            "velocityX": 1.9890268665695783,
-            "velocityY": -1.0389213344962835,
-            "timestamp": 0.8981781715769376
+            "x": 7.501421172621153,
+            "y": 5.8902827497271355,
+            "heading": -0.04357887494407857,
+            "angularVelocity": 0.3866223402525239,
+            "velocityX": 1.9890269610193678,
+            "velocityY": -1.038921153793384,
+            "timestamp": 0.8981781715770645
         },
         {
-            "x": 7.620483882250166,
-            "y": 5.828089677839219,
-            "heading": -0.01724918141894823,
-            "angularVelocity": 0.293145563189075,
-            "velocityX": 1.3256022165187353,
-            "velocityY": -0.6924360558184839,
-            "timestamp": 0.9879959887346313
+            "x": 7.62048387761172,
+            "y": 5.828089668977501,
+            "heading": -0.01724918093308134,
+            "angularVelocity": 0.29314555668577735,
+            "velocityX": 1.3256022998366965,
+            "velocityY": -0.6924358965486016,
+            "timestamp": 0.9879959887347709
         },
         {
             "x": 7.68,
             "y": 5.797,
             "heading": -0.003,
-            "angularVelocity": 0.1586453764950761,
-            "velocityX": 0.6626315316185003,
-            "velocityY": -0.346141543215585,
-            "timestamp": 1.077813805892325
+            "angularVelocity": 0.15864537108558247,
+            "velocityX": 0.6626315832612425,
+            "velocityY": -0.346141444552285,
+            "timestamp": 1.0778138058924773
         },
         {
             "x": 7.68,
             "y": 5.797,
             "heading": -0.003,
-            "angularVelocity": 2.21164904203514e-32,
-            "velocityX": 6.504499750841485e-34,
-            "velocityY": 5.343957154186367e-33,
-            "timestamp": 1.1676316230500188
+            "angularVelocity": 0,
+            "velocityX": 4.386752331989148e-33,
+            "velocityY": -2.33652586669764e-33,
+            "timestamp": 1.1676316230501838
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3.1.traj
new file mode 100644
index 00000000..b1b40339
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3.1.traj	
@@ -0,0 +1,157 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.246662494261999,
+            "y": 6.628692732737936,
+            "heading": 0.18398762968545185,
+            "angularVelocity": -0.8088463398355284,
+            "velocityX": 0.7519183997414132,
+            "velocityY": -0.013910608703967415,
+            "timestamp": 0.09397628025368156
+        },
+        {
+            "x": 5.3885394117424426,
+            "y": 6.6228569584156665,
+            "heading": 0.04133864379940616,
+            "angularVelocity": -1.5179254328962157,
+            "velocityX": 1.509709866122152,
+            "velocityY": -0.0620983753189278,
+            "timestamp": 0.18795256050736311
+        },
+        {
+            "x": 5.602094427902648,
+            "y": 6.606624752166298,
+            "heading": -0.14949344735311892,
+            "angularVelocity": -2.0306410366252936,
+            "velocityX": 2.272435295201409,
+            "velocityY": -0.172726630651381,
+            "timestamp": 0.2819288407610447
+        },
+        {
+            "x": 5.885007714232348,
+            "y": 6.565697735942791,
+            "heading": -0.3456589598447775,
+            "angularVelocity": -2.087393882393782,
+            "velocityX": 3.010475468554385,
+            "velocityY": -0.4355036836213049,
+            "timestamp": 0.37590512101472623
+        },
+        {
+            "x": 6.196762938930662,
+            "y": 6.464967292979978,
+            "heading": -0.4305176558630451,
+            "angularVelocity": -0.9029799412064233,
+            "velocityX": 3.3173820442430326,
+            "velocityY": -1.0718709305252299,
+            "timestamp": 0.4698814012684078
+        },
+        {
+            "x": 6.507653773495931,
+            "y": 6.294193106326001,
+            "heading": -0.4331106719684992,
+            "angularVelocity": -0.027592240280786797,
+            "velocityX": 3.3081840835372915,
+            "velocityY": -1.8172052159649787,
+            "timestamp": 0.5638576815220894
+        },
+        {
+            "x": 6.777249226823148,
+            "y": 6.0624534109685,
+            "heading": -0.433110646067504,
+            "angularVelocity": 2.7561204856502443e-7,
+            "velocityX": 2.8687606340606973,
+            "velocityY": -2.465938157287546,
+            "timestamp": 0.6578339617757709
+        },
+        {
+            "x": 6.99331744885121,
+            "y": 5.780142381909384,
+            "heading": -0.4331105937998759,
+            "angularVelocity": 5.561789419211182e-7,
+            "velocityX": 2.2991782761011916,
+            "velocityY": -3.004066859180208,
+            "timestamp": 0.7518102420294525
+        },
+        {
+            "x": 7.1465883664001515,
+            "y": 5.459372959434879,
+            "heading": -0.43311054984028735,
+            "angularVelocity": 4.677732342413875e-7,
+            "velocityX": 1.6309532270823943,
+            "velocityY": -3.4133019694822235,
+            "timestamp": 0.845786522283134
+        },
+        {
+            "x": 7.2797482024400875,
+            "y": 5.1297467242247805,
+            "heading": -0.4331105067287741,
+            "angularVelocity": 4.5874887908824285e-7,
+            "velocityX": 1.4169515507581365,
+            "velocityY": -3.507547163180951,
+            "timestamp": 0.9397628025368155
+        },
+        {
+            "x": 7.407925031698652,
+            "y": 4.799268903667217,
+            "heading": -0.4305589051420175,
+            "angularVelocity": 0.027151549091629798,
+            "velocityX": 1.3639274603395894,
+            "velocityY": -3.5166088683810885,
+            "timestamp": 1.033739082790497
+        },
+        {
+            "x": 7.517390810231398,
+            "y": 4.529437032753176,
+            "heading": -0.29559925755962413,
+            "angularVelocity": 1.4361033147734772,
+            "velocityX": 1.164823487770018,
+            "velocityY": -2.871276349581525,
+            "timestamp": 1.1277153630441787
+        },
+        {
+            "x": 7.59899827381295,
+            "y": 4.327770783855267,
+            "heading": -0.16069313013903275,
+            "angularVelocity": 1.4355338076419173,
+            "velocityX": 0.8683836321384404,
+            "velocityY": -2.1459271249460685,
+            "timestamp": 1.2216916432978604
+        },
+        {
+            "x": 7.653087401223331,
+            "y": 4.193799869992724,
+            "heading": -0.05829734753961687,
+            "angularVelocity": 1.0895917812772173,
+            "velocityX": 0.5755614849233384,
+            "velocityY": -1.4255822160751557,
+            "timestamp": 1.315667923551542
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -0.003,
+            "angularVelocity": 0.5884181347713066,
+            "velocityX": 0.28637650590148805,
+            "velocityY": -0.7108162805806236,
+            "timestamp": 1.4096442038052237
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -0.003,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.8051389340682807e-33,
+            "timestamp": 1.5036204840589054
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3.traj
new file mode 100644
index 00000000..b1b40339
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3.traj	
@@ -0,0 +1,157 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.246662494261999,
+            "y": 6.628692732737936,
+            "heading": 0.18398762968545185,
+            "angularVelocity": -0.8088463398355284,
+            "velocityX": 0.7519183997414132,
+            "velocityY": -0.013910608703967415,
+            "timestamp": 0.09397628025368156
+        },
+        {
+            "x": 5.3885394117424426,
+            "y": 6.6228569584156665,
+            "heading": 0.04133864379940616,
+            "angularVelocity": -1.5179254328962157,
+            "velocityX": 1.509709866122152,
+            "velocityY": -0.0620983753189278,
+            "timestamp": 0.18795256050736311
+        },
+        {
+            "x": 5.602094427902648,
+            "y": 6.606624752166298,
+            "heading": -0.14949344735311892,
+            "angularVelocity": -2.0306410366252936,
+            "velocityX": 2.272435295201409,
+            "velocityY": -0.172726630651381,
+            "timestamp": 0.2819288407610447
+        },
+        {
+            "x": 5.885007714232348,
+            "y": 6.565697735942791,
+            "heading": -0.3456589598447775,
+            "angularVelocity": -2.087393882393782,
+            "velocityX": 3.010475468554385,
+            "velocityY": -0.4355036836213049,
+            "timestamp": 0.37590512101472623
+        },
+        {
+            "x": 6.196762938930662,
+            "y": 6.464967292979978,
+            "heading": -0.4305176558630451,
+            "angularVelocity": -0.9029799412064233,
+            "velocityX": 3.3173820442430326,
+            "velocityY": -1.0718709305252299,
+            "timestamp": 0.4698814012684078
+        },
+        {
+            "x": 6.507653773495931,
+            "y": 6.294193106326001,
+            "heading": -0.4331106719684992,
+            "angularVelocity": -0.027592240280786797,
+            "velocityX": 3.3081840835372915,
+            "velocityY": -1.8172052159649787,
+            "timestamp": 0.5638576815220894
+        },
+        {
+            "x": 6.777249226823148,
+            "y": 6.0624534109685,
+            "heading": -0.433110646067504,
+            "angularVelocity": 2.7561204856502443e-7,
+            "velocityX": 2.8687606340606973,
+            "velocityY": -2.465938157287546,
+            "timestamp": 0.6578339617757709
+        },
+        {
+            "x": 6.99331744885121,
+            "y": 5.780142381909384,
+            "heading": -0.4331105937998759,
+            "angularVelocity": 5.561789419211182e-7,
+            "velocityX": 2.2991782761011916,
+            "velocityY": -3.004066859180208,
+            "timestamp": 0.7518102420294525
+        },
+        {
+            "x": 7.1465883664001515,
+            "y": 5.459372959434879,
+            "heading": -0.43311054984028735,
+            "angularVelocity": 4.677732342413875e-7,
+            "velocityX": 1.6309532270823943,
+            "velocityY": -3.4133019694822235,
+            "timestamp": 0.845786522283134
+        },
+        {
+            "x": 7.2797482024400875,
+            "y": 5.1297467242247805,
+            "heading": -0.4331105067287741,
+            "angularVelocity": 4.5874887908824285e-7,
+            "velocityX": 1.4169515507581365,
+            "velocityY": -3.507547163180951,
+            "timestamp": 0.9397628025368155
+        },
+        {
+            "x": 7.407925031698652,
+            "y": 4.799268903667217,
+            "heading": -0.4305589051420175,
+            "angularVelocity": 0.027151549091629798,
+            "velocityX": 1.3639274603395894,
+            "velocityY": -3.5166088683810885,
+            "timestamp": 1.033739082790497
+        },
+        {
+            "x": 7.517390810231398,
+            "y": 4.529437032753176,
+            "heading": -0.29559925755962413,
+            "angularVelocity": 1.4361033147734772,
+            "velocityX": 1.164823487770018,
+            "velocityY": -2.871276349581525,
+            "timestamp": 1.1277153630441787
+        },
+        {
+            "x": 7.59899827381295,
+            "y": 4.327770783855267,
+            "heading": -0.16069313013903275,
+            "angularVelocity": 1.4355338076419173,
+            "velocityX": 0.8683836321384404,
+            "velocityY": -2.1459271249460685,
+            "timestamp": 1.2216916432978604
+        },
+        {
+            "x": 7.653087401223331,
+            "y": 4.193799869992724,
+            "heading": -0.05829734753961687,
+            "angularVelocity": 1.0895917812772173,
+            "velocityX": 0.5755614849233384,
+            "velocityY": -1.4255822160751557,
+            "timestamp": 1.315667923551542
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -0.003,
+            "angularVelocity": 0.5884181347713066,
+            "velocityX": 0.28637650590148805,
+            "velocityY": -0.7108162805806236,
+            "timestamp": 1.4096442038052237
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -0.003,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.8051389340682807e-33,
+            "timestamp": 1.5036204840589054
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C4.1.traj
new file mode 100644
index 00000000..3f479acb
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C4.1.traj	
@@ -0,0 +1,247 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": 0,
+            "velocityX": -1.7123315632321391e-31,
+            "velocityY": 4.571975133912053e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 5.2081944432473115,
+            "y": 6.63354239454729,
+            "heading": 0.2524699257138384,
+            "angularVelocity": -0.12069194203891866,
+            "velocityX": 0.5160121574790498,
+            "velocityY": 0.0567774581143745,
+            "timestamp": 0.062390861883169
+        },
+        {
+            "x": 5.2726441787772975,
+            "y": 6.640106281427696,
+            "heading": 0.23785051964847434,
+            "angularVelocity": -0.2343196683632906,
+            "velocityX": 1.0329996025807835,
+            "velocityY": 0.1052059016703473,
+            "timestamp": 0.124781723766338
+        },
+        {
+            "x": 5.369418980987113,
+            "y": 6.648966513737627,
+            "heading": 0.21675536507543205,
+            "angularVelocity": -0.33811288923278293,
+            "velocityX": 1.551105390898958,
+            "velocityY": 0.14201169918963874,
+            "timestamp": 0.187172585649507
+        },
+        {
+            "x": 5.498590594297912,
+            "y": 6.659046571836944,
+            "heading": 0.19009632617593986,
+            "angularVelocity": -0.42729076173705777,
+            "velocityX": 2.070361097955043,
+            "velocityY": 0.16156305258605722,
+            "timestamp": 0.249563447532676
+        },
+        {
+            "x": 5.6601948909824875,
+            "y": 6.668590441412665,
+            "heading": 0.15936442465931713,
+            "angularVelocity": -0.49257055583183496,
+            "velocityX": 2.5901917653772815,
+            "velocityY": 0.15296902924013772,
+            "timestamp": 0.311954309415845
+        },
+        {
+            "x": 5.854024302844021,
+            "y": 6.674275452040902,
+            "heading": 0.12740102584180835,
+            "angularVelocity": -0.512308980077189,
+            "velocityX": 3.106695532183737,
+            "velocityY": 0.09111928344386595,
+            "timestamp": 0.374345171299014
+        },
+        {
+            "x": 6.078018113514707,
+            "y": 6.668089747759068,
+            "heading": 0.10126004011963459,
+            "angularVelocity": -0.4189874115076864,
+            "velocityX": 3.590170161299073,
+            "velocityY": -0.09914439543117176,
+            "timestamp": 0.436736033182183
+        },
+        {
+            "x": 6.311203816285634,
+            "y": 6.63161682590639,
+            "heading": 0.10125992071184643,
+            "angularVelocity": -0.000001913866624365403,
+            "velocityX": 3.737497699704501,
+            "velocityY": -0.584587562213517,
+            "timestamp": 0.499126895065352
+        },
+        {
+            "x": 6.53718081427167,
+            "y": 6.563497401833266,
+            "heading": 0.10125989664992657,
+            "angularVelocity": -3.856641685769623e-7,
+            "velocityX": 3.621956664249834,
+            "velocityY": -1.0918173273625358,
+            "timestamp": 0.561517756948521
+        },
+        {
+            "x": 6.7516755548198235,
+            "y": 6.465019697743108,
+            "heading": 0.10125985817262735,
+            "angularVelocity": -6.167136938553595e-7,
+            "velocityX": 3.4379191771674527,
+            "velocityY": -1.5783994821960954,
+            "timestamp": 0.62390861883169
+        },
+        {
+            "x": 6.950631618499756,
+            "y": 6.338046073913574,
+            "heading": 0.10125971935519859,
+            "angularVelocity": -0.000002224964167076665,
+            "velocityX": 3.1888654471946203,
+            "velocityY": -2.035131748416977,
+            "timestamp": 0.686299480714859
+        },
+        {
+            "x": 7.196058677643184,
+            "y": 6.101503366378845,
+            "heading": 0.10125966337035743,
+            "angularVelocity": -6.213285755242982e-7,
+            "velocityX": 2.723788105663008,
+            "velocityY": -2.625188174087521,
+            "timestamp": 0.7764045291337008
+        },
+        {
+            "x": 7.389899318639429,
+            "y": 5.821123760827136,
+            "heading": 0.10125964077104323,
+            "angularVelocity": -2.508107435019506e-7,
+            "velocityX": 2.1512739230237647,
+            "velocityY": -3.1116969633976366,
+            "timestamp": 0.8665095775525427
+        },
+        {
+            "x": 7.524507781478929,
+            "y": 5.507966472295818,
+            "heading": 0.101259626680438,
+            "angularVelocity": -1.5637975290365853e-7,
+            "velocityX": 1.4939058931947202,
+            "velocityY": -3.475468844716046,
+            "timestamp": 0.9566146259713845
+        },
+        {
+            "x": 7.594574766806078,
+            "y": 5.174383579102459,
+            "heading": 0.10125961555168732,
+            "angularVelocity": -1.235086255399969e-7,
+            "velocityX": 0.7776144240159958,
+            "velocityY": -3.702155418004344,
+            "timestamp": 1.0467196743902263
+        },
+        {
+            "x": 7.605242469312774,
+            "y": 4.833688510035603,
+            "heading": 0.10125960514477245,
+            "angularVelocity": -1.1549757817967306e-7,
+            "velocityX": 0.11839184034432584,
+            "velocityY": -3.781087464524514,
+            "timestamp": 1.136824722809068
+        },
+        {
+            "x": 7.615908675824772,
+            "y": 4.492993394135096,
+            "heading": 0.10125959473787158,
+            "angularVelocity": -1.1549742264017462e-7,
+            "velocityX": 0.11837523756082047,
+            "velocityY": -3.781087984291716,
+            "timestamp": 1.2269297712279097
+        },
+        {
+            "x": 7.6265748461946865,
+            "y": 4.152298277103123,
+            "heading": 0.10125958433097336,
+            "angularVelocity": -1.1549739343149193e-7,
+            "velocityX": 0.11837483645025074,
+            "velocityY": -3.781087996848918,
+            "timestamp": 1.3170348196467514
+        },
+        {
+            "x": 7.637240986091898,
+            "y": 3.8116031591171406,
+            "heading": 0.10125957392407539,
+            "angularVelocity": -1.1549739036497336e-7,
+            "velocityX": 0.11837449825941278,
+            "velocityY": -3.78108800743665,
+            "timestamp": 1.407139868065593
+        },
+        {
+            "x": 7.647907101492937,
+            "y": 3.470908040364265,
+            "heading": 0.10125956351717343,
+            "angularVelocity": -1.1549743491773867e-7,
+            "velocityX": 0.11837422639702805,
+            "velocityY": -3.781088015947766,
+            "timestamp": 1.4972449164844348
+        },
+        {
+            "x": 7.658649980665503,
+            "y": 3.1330359890940187,
+            "heading": 0.09207038783752043,
+            "angularVelocity": -0.10198291706072676,
+            "velocityX": 0.11922616280763662,
+            "velocityY": -3.7497571689844853,
+            "timestamp": 1.5873499649032765
+        },
+        {
+            "x": 7.667206120655169,
+            "y": 2.862563871388905,
+            "heading": 0.061604548821908564,
+            "angularVelocity": -0.33811467337536444,
+            "velocityX": 0.09495738740298482,
+            "velocityY": -3.0017421049246735,
+            "timestamp": 1.6774550133221182
+        },
+        {
+            "x": 7.673608699461006,
+            "y": 2.659753859006467,
+            "heading": 0.031813701428875164,
+            "angularVelocity": -0.33062351017837394,
+            "velocityX": 0.07105682665061783,
+            "velocityY": -2.250817417462574,
+            "timestamp": 1.76756006174096
+        },
+        {
+            "x": 7.677870998153504,
+            "y": 2.524576540197495,
+            "heading": 0.009226184845951672,
+            "angularVelocity": -0.25067981183391225,
+            "velocityX": 0.04730366130750142,
+            "velocityY": -1.5002191462193974,
+            "timestamp": 1.8576651101598016
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -0.003,
+            "angularVelocity": -0.1356881224803119,
+            "velocityX": 0.023627997363707604,
+            "velocityY": -0.7499750722442785,
+            "timestamp": 1.9477701585786433
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -0.003,
+            "angularVelocity": 0,
+            "velocityX": -1.899719241029588e-32,
+            "velocityY": -4.254972095731821e-34,
+            "timestamp": 2.037875206997485
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C4.traj
new file mode 100644
index 00000000..3f479acb
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C4.traj	
@@ -0,0 +1,247 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": 0,
+            "velocityX": -1.7123315632321391e-31,
+            "velocityY": 4.571975133912053e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 5.2081944432473115,
+            "y": 6.63354239454729,
+            "heading": 0.2524699257138384,
+            "angularVelocity": -0.12069194203891866,
+            "velocityX": 0.5160121574790498,
+            "velocityY": 0.0567774581143745,
+            "timestamp": 0.062390861883169
+        },
+        {
+            "x": 5.2726441787772975,
+            "y": 6.640106281427696,
+            "heading": 0.23785051964847434,
+            "angularVelocity": -0.2343196683632906,
+            "velocityX": 1.0329996025807835,
+            "velocityY": 0.1052059016703473,
+            "timestamp": 0.124781723766338
+        },
+        {
+            "x": 5.369418980987113,
+            "y": 6.648966513737627,
+            "heading": 0.21675536507543205,
+            "angularVelocity": -0.33811288923278293,
+            "velocityX": 1.551105390898958,
+            "velocityY": 0.14201169918963874,
+            "timestamp": 0.187172585649507
+        },
+        {
+            "x": 5.498590594297912,
+            "y": 6.659046571836944,
+            "heading": 0.19009632617593986,
+            "angularVelocity": -0.42729076173705777,
+            "velocityX": 2.070361097955043,
+            "velocityY": 0.16156305258605722,
+            "timestamp": 0.249563447532676
+        },
+        {
+            "x": 5.6601948909824875,
+            "y": 6.668590441412665,
+            "heading": 0.15936442465931713,
+            "angularVelocity": -0.49257055583183496,
+            "velocityX": 2.5901917653772815,
+            "velocityY": 0.15296902924013772,
+            "timestamp": 0.311954309415845
+        },
+        {
+            "x": 5.854024302844021,
+            "y": 6.674275452040902,
+            "heading": 0.12740102584180835,
+            "angularVelocity": -0.512308980077189,
+            "velocityX": 3.106695532183737,
+            "velocityY": 0.09111928344386595,
+            "timestamp": 0.374345171299014
+        },
+        {
+            "x": 6.078018113514707,
+            "y": 6.668089747759068,
+            "heading": 0.10126004011963459,
+            "angularVelocity": -0.4189874115076864,
+            "velocityX": 3.590170161299073,
+            "velocityY": -0.09914439543117176,
+            "timestamp": 0.436736033182183
+        },
+        {
+            "x": 6.311203816285634,
+            "y": 6.63161682590639,
+            "heading": 0.10125992071184643,
+            "angularVelocity": -0.000001913866624365403,
+            "velocityX": 3.737497699704501,
+            "velocityY": -0.584587562213517,
+            "timestamp": 0.499126895065352
+        },
+        {
+            "x": 6.53718081427167,
+            "y": 6.563497401833266,
+            "heading": 0.10125989664992657,
+            "angularVelocity": -3.856641685769623e-7,
+            "velocityX": 3.621956664249834,
+            "velocityY": -1.0918173273625358,
+            "timestamp": 0.561517756948521
+        },
+        {
+            "x": 6.7516755548198235,
+            "y": 6.465019697743108,
+            "heading": 0.10125985817262735,
+            "angularVelocity": -6.167136938553595e-7,
+            "velocityX": 3.4379191771674527,
+            "velocityY": -1.5783994821960954,
+            "timestamp": 0.62390861883169
+        },
+        {
+            "x": 6.950631618499756,
+            "y": 6.338046073913574,
+            "heading": 0.10125971935519859,
+            "angularVelocity": -0.000002224964167076665,
+            "velocityX": 3.1888654471946203,
+            "velocityY": -2.035131748416977,
+            "timestamp": 0.686299480714859
+        },
+        {
+            "x": 7.196058677643184,
+            "y": 6.101503366378845,
+            "heading": 0.10125966337035743,
+            "angularVelocity": -6.213285755242982e-7,
+            "velocityX": 2.723788105663008,
+            "velocityY": -2.625188174087521,
+            "timestamp": 0.7764045291337008
+        },
+        {
+            "x": 7.389899318639429,
+            "y": 5.821123760827136,
+            "heading": 0.10125964077104323,
+            "angularVelocity": -2.508107435019506e-7,
+            "velocityX": 2.1512739230237647,
+            "velocityY": -3.1116969633976366,
+            "timestamp": 0.8665095775525427
+        },
+        {
+            "x": 7.524507781478929,
+            "y": 5.507966472295818,
+            "heading": 0.101259626680438,
+            "angularVelocity": -1.5637975290365853e-7,
+            "velocityX": 1.4939058931947202,
+            "velocityY": -3.475468844716046,
+            "timestamp": 0.9566146259713845
+        },
+        {
+            "x": 7.594574766806078,
+            "y": 5.174383579102459,
+            "heading": 0.10125961555168732,
+            "angularVelocity": -1.235086255399969e-7,
+            "velocityX": 0.7776144240159958,
+            "velocityY": -3.702155418004344,
+            "timestamp": 1.0467196743902263
+        },
+        {
+            "x": 7.605242469312774,
+            "y": 4.833688510035603,
+            "heading": 0.10125960514477245,
+            "angularVelocity": -1.1549757817967306e-7,
+            "velocityX": 0.11839184034432584,
+            "velocityY": -3.781087464524514,
+            "timestamp": 1.136824722809068
+        },
+        {
+            "x": 7.615908675824772,
+            "y": 4.492993394135096,
+            "heading": 0.10125959473787158,
+            "angularVelocity": -1.1549742264017462e-7,
+            "velocityX": 0.11837523756082047,
+            "velocityY": -3.781087984291716,
+            "timestamp": 1.2269297712279097
+        },
+        {
+            "x": 7.6265748461946865,
+            "y": 4.152298277103123,
+            "heading": 0.10125958433097336,
+            "angularVelocity": -1.1549739343149193e-7,
+            "velocityX": 0.11837483645025074,
+            "velocityY": -3.781087996848918,
+            "timestamp": 1.3170348196467514
+        },
+        {
+            "x": 7.637240986091898,
+            "y": 3.8116031591171406,
+            "heading": 0.10125957392407539,
+            "angularVelocity": -1.1549739036497336e-7,
+            "velocityX": 0.11837449825941278,
+            "velocityY": -3.78108800743665,
+            "timestamp": 1.407139868065593
+        },
+        {
+            "x": 7.647907101492937,
+            "y": 3.470908040364265,
+            "heading": 0.10125956351717343,
+            "angularVelocity": -1.1549743491773867e-7,
+            "velocityX": 0.11837422639702805,
+            "velocityY": -3.781088015947766,
+            "timestamp": 1.4972449164844348
+        },
+        {
+            "x": 7.658649980665503,
+            "y": 3.1330359890940187,
+            "heading": 0.09207038783752043,
+            "angularVelocity": -0.10198291706072676,
+            "velocityX": 0.11922616280763662,
+            "velocityY": -3.7497571689844853,
+            "timestamp": 1.5873499649032765
+        },
+        {
+            "x": 7.667206120655169,
+            "y": 2.862563871388905,
+            "heading": 0.061604548821908564,
+            "angularVelocity": -0.33811467337536444,
+            "velocityX": 0.09495738740298482,
+            "velocityY": -3.0017421049246735,
+            "timestamp": 1.6774550133221182
+        },
+        {
+            "x": 7.673608699461006,
+            "y": 2.659753859006467,
+            "heading": 0.031813701428875164,
+            "angularVelocity": -0.33062351017837394,
+            "velocityX": 0.07105682665061783,
+            "velocityY": -2.250817417462574,
+            "timestamp": 1.76756006174096
+        },
+        {
+            "x": 7.677870998153504,
+            "y": 2.524576540197495,
+            "heading": 0.009226184845951672,
+            "angularVelocity": -0.25067981183391225,
+            "velocityX": 0.04730366130750142,
+            "velocityY": -1.5002191462193974,
+            "timestamp": 1.8576651101598016
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -0.003,
+            "angularVelocity": -0.1356881224803119,
+            "velocityX": 0.023627997363707604,
+            "velocityY": -0.7499750722442785,
+            "timestamp": 1.9477701585786433
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -0.003,
+            "angularVelocity": 0,
+            "velocityX": -1.899719241029588e-32,
+            "velocityY": -4.254972095731821e-34,
+            "timestamp": 2.037875206997485
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C5.1.traj
new file mode 100644
index 00000000..7f9f6ab6
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C5.1.traj	
@@ -0,0 +1,292 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": -8.010592837331029e-32,
+            "velocityX": 9.396271853657066e-34,
+            "velocityY": -2.4298772375808502e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 5.207063492876026,
+            "y": 6.6289176408913075,
+            "heading": 0.24374543730705361,
+            "angularVelocity": -0.264834788594711,
+            "velocityX": 0.5061159579768882,
+            "velocityY": -0.01763482359684202,
+            "timestamp": 0.06137623678217253
+        },
+        {
+            "x": 5.269220913885746,
+            "y": 6.626580685647671,
+            "heading": 0.2121773997323976,
+            "angularVelocity": -0.514336479877237,
+            "velocityX": 1.0127277961065038,
+            "velocityY": -0.03807589657100854,
+            "timestamp": 0.12275247356434506
+        },
+        {
+            "x": 5.36250981468421,
+            "y": 6.6227475288112485,
+            "heading": 0.16662318019476377,
+            "angularVelocity": -0.7422126530713462,
+            "velocityX": 1.5199514615005028,
+            "velocityY": -0.06245343535846346,
+            "timestamp": 0.1841287103465176
+        },
+        {
+            "x": 5.486975524936631,
+            "y": 6.617054798693029,
+            "heading": 0.10910232843404971,
+            "angularVelocity": -0.9371844019185364,
+            "velocityX": 2.0279136808950478,
+            "velocityY": -0.0927513711605005,
+            "timestamp": 0.24550494712869012
+        },
+        {
+            "x": 5.642662315095453,
+            "y": 6.608892934358269,
+            "heading": 0.04300979987381644,
+            "angularVelocity": -1.076842309423355,
+            "velocityX": 2.536597196588689,
+            "velocityY": -0.1329808532205177,
+            "timestamp": 0.30688118391086266
+        },
+        {
+            "x": 5.829535978248522,
+            "y": 6.597025931717244,
+            "heading": -0.024847542112422006,
+            "angularVelocity": -1.1055963275666998,
+            "velocityX": 3.0447233807490215,
+            "velocityY": -0.19334848897856358,
+            "timestamp": 0.3682574206930352
+        },
+        {
+            "x": 6.046434546533161,
+            "y": 6.5777851666622515,
+            "heading": -0.07267029178585922,
+            "angularVelocity": -0.7791737027338695,
+            "velocityX": 3.533917679808606,
+            "velocityY": -0.31348883645767445,
+            "timestamp": 0.4296336574752077
+        },
+        {
+            "x": 6.274079716361941,
+            "y": 6.532107529947078,
+            "heading": -0.07267036448034916,
+            "angularVelocity": -0.0000011844077407082938,
+            "velocityX": 3.709011528952235,
+            "velocityY": -0.7442234830605138,
+            "timestamp": 0.49100989425738023
+        },
+        {
+            "x": 6.493476634626264,
+            "y": 6.456122214314626,
+            "heading": -0.07267035803800323,
+            "angularVelocity": 1.049648246960038e-7,
+            "velocityX": 3.5746231728571716,
+            "velocityY": -1.238025001469686,
+            "timestamp": 0.5523861310395528
+        },
+        {
+            "x": 6.700610073343003,
+            "y": 6.351219816335689,
+            "heading": -0.07267035070446132,
+            "angularVelocity": 1.1948503655702063e-7,
+            "velocityX": 3.3748149051866063,
+            "velocityY": -1.7091695985083282,
+            "timestamp": 0.6137623678217253
+        },
+        {
+            "x": 6.891689244621814,
+            "y": 6.21932014936447,
+            "heading": -0.07267034149477326,
+            "angularVelocity": 1.5005299351249212e-7,
+            "velocityX": 3.1132435173071933,
+            "velocityY": -2.1490347712151214,
+            "timestamp": 0.6751386046038979
+        },
+        {
+            "x": 7.0632171630859375,
+            "y": 6.06283712387085,
+            "heading": -0.07267032569899272,
+            "angularVelocity": 2.573598736854518e-7,
+            "velocityX": 2.7946959190881038,
+            "velocityY": -2.549570219643589,
+            "timestamp": 0.7365148413860704
+        },
+        {
+            "x": 7.263317671759933,
+            "y": 5.7907628781558955,
+            "heading": -0.07267031513885969,
+            "angularVelocity": 1.182832965996254e-7,
+            "velocityX": 2.24131152203967,
+            "velocityY": -3.0474842158680824,
+            "timestamp": 0.8257931530827405
+        },
+        {
+            "x": 7.406264443711533,
+            "y": 5.484771313458312,
+            "heading": -0.07267030869611313,
+            "angularVelocity": 7.216474459034548e-8,
+            "velocityX": 1.6011365944875051,
+            "velocityY": -3.4273896860551396,
+            "timestamp": 0.9150714647794106
+        },
+        {
+            "x": 7.486522259331217,
+            "y": 5.1567114044359705,
+            "heading": -0.07267030368970051,
+            "angularVelocity": 5.607647052205946e-8,
+            "velocityX": 0.8989620669840581,
+            "velocityY": -3.6745756364317224,
+            "timestamp": 1.0043497764760807
+        },
+        {
+            "x": 7.50149841248159,
+            "y": 4.81930906784098,
+            "heading": -0.07267029905812865,
+            "angularVelocity": 5.187790600157932e-8,
+            "velocityX": 0.1677468230047605,
+            "velocityY": -3.7792195011633143,
+            "timestamp": 1.093628088172751
+        },
+        {
+            "x": 7.516449925771596,
+            "y": 4.481905638472984,
+            "heading": -0.0726702944266145,
+            "angularVelocity": 5.1877259457701335e-8,
+            "velocityX": 0.16747083368699095,
+            "velocityY": -3.7792317412357557,
+            "timestamp": 1.1829063998694211
+        },
+        {
+            "x": 7.531401402222247,
+            "y": 4.144502207472513,
+            "heading": -0.07267028979509418,
+            "angularVelocity": 5.187732873980297e-8,
+            "velocityX": 0.16747042105201052,
+            "velocityY": -3.779231759520992,
+            "timestamp": 1.2721847115660914
+        },
+        {
+            "x": 7.546352847973806,
+            "y": 3.807098775111667,
+            "heading": -0.07267028516356967,
+            "angularVelocity": 5.1877375603275234e-8,
+            "velocityX": 0.16747007719365256,
+            "velocityY": -3.7792317747584647,
+            "timestamp": 1.3614630232627616
+        },
+        {
+            "x": 7.561304268557873,
+            "y": 3.4696953416355707,
+            "heading": -0.07267028053204275,
+            "angularVelocity": 5.1877402813012475e-8,
+            "velocityX": 0.16746979529438435,
+            "velocityY": -3.7792317872503007,
+            "timestamp": 1.4507413349594318
+        },
+        {
+            "x": 7.576255668582546,
+            "y": 3.132291907248425,
+            "heading": -0.07267027590051456,
+            "angularVelocity": 5.1877416880821484e-8,
+            "velocityX": 0.16746956501007867,
+            "velocityY": -3.779231797454896,
+            "timestamp": 1.540019646656102
+        },
+        {
+            "x": 7.591207051730301,
+            "y": 2.794888472113414,
+            "heading": -0.07267027126898577,
+            "angularVelocity": 5.1877423431260937e-8,
+            "velocityX": 0.16746937597291248,
+            "velocityY": -3.779231805831691,
+            "timestamp": 1.6292979583527722
+        },
+        {
+            "x": 7.60615842090353,
+            "y": 2.457485036359151,
+            "heading": -0.07267026663745696,
+            "angularVelocity": 5.1877423851232326e-8,
+            "velocityX": 0.16746921944524512,
+            "velocityY": -3.779231812767884,
+            "timestamp": 1.7185762700494425
+        },
+        {
+            "x": 7.621109778388665,
+            "y": 2.1200816000869556,
+            "heading": -0.07267026200592838,
+            "angularVelocity": 5.187742117887202e-8,
+            "velocityX": 0.1674690885277524,
+            "velocityY": -3.779231818569208,
+            "timestamp": 1.8078545817461127
+        },
+        {
+            "x": 7.636061125621916,
+            "y": 1.7826781633604734,
+            "heading": -0.07267025737439563,
+            "angularVelocity": 5.1877468094637746e-8,
+            "velocityX": 0.16746897369713376,
+            "velocityY": -3.7792318236576405,
+            "timestamp": 1.897132893442783
+        },
+        {
+            "x": 7.6506850878742485,
+            "y": 1.4507667606163608,
+            "heading": -0.05536549896524321,
+            "angularVelocity": 0.19382936438075055,
+            "velocityX": 0.16380195788222543,
+            "velocityY": -3.7177159428351065,
+            "timestamp": 1.9864112051394531
+        },
+        {
+            "x": 7.662404359721407,
+            "y": 1.1852476637582392,
+            "heading": -0.0353858682020255,
+            "angularVelocity": 0.2237904187872895,
+            "velocityX": 0.13126672788097452,
+            "velocityY": -2.974060461181691,
+            "timestamp": 2.0756895168361233
+        },
+        {
+            "x": 7.6711999602966845,
+            "y": 0.9861189087552604,
+            "heading": -0.018423007440779238,
+            "angularVelocity": 0.18999979321832794,
+            "velocityX": 0.0985188945458782,
+            "velocityY": -2.2304269784976873,
+            "timestamp": 2.1649678285327933
+        },
+        {
+            "x": 7.677066105120016,
+            "y": 0.8533716446291538,
+            "heading": -0.006322956528169105,
+            "angularVelocity": 0.13553180702747777,
+            "velocityX": 0.06570626966225444,
+            "velocityY": -1.4868926349898441,
+            "timestamp": 2.2542461402294633
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -2.444948224573009e-31,
+            "angularVelocity": 0.0708229849781762,
+            "velocityX": 0.03286234724008497,
+            "velocityY": -0.7434240563896022,
+            "timestamp": 2.3435244519261333
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 1.883272219480887e-30,
+            "velocityX": 0,
+            "velocityY": -3.281240907044904e-32,
+            "timestamp": 2.4328027636228033
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C5.traj
new file mode 100644
index 00000000..7f9f6ab6
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C5.traj	
@@ -0,0 +1,292 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": -8.010592837331029e-32,
+            "velocityX": 9.396271853657066e-34,
+            "velocityY": -2.4298772375808502e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 5.207063492876026,
+            "y": 6.6289176408913075,
+            "heading": 0.24374543730705361,
+            "angularVelocity": -0.264834788594711,
+            "velocityX": 0.5061159579768882,
+            "velocityY": -0.01763482359684202,
+            "timestamp": 0.06137623678217253
+        },
+        {
+            "x": 5.269220913885746,
+            "y": 6.626580685647671,
+            "heading": 0.2121773997323976,
+            "angularVelocity": -0.514336479877237,
+            "velocityX": 1.0127277961065038,
+            "velocityY": -0.03807589657100854,
+            "timestamp": 0.12275247356434506
+        },
+        {
+            "x": 5.36250981468421,
+            "y": 6.6227475288112485,
+            "heading": 0.16662318019476377,
+            "angularVelocity": -0.7422126530713462,
+            "velocityX": 1.5199514615005028,
+            "velocityY": -0.06245343535846346,
+            "timestamp": 0.1841287103465176
+        },
+        {
+            "x": 5.486975524936631,
+            "y": 6.617054798693029,
+            "heading": 0.10910232843404971,
+            "angularVelocity": -0.9371844019185364,
+            "velocityX": 2.0279136808950478,
+            "velocityY": -0.0927513711605005,
+            "timestamp": 0.24550494712869012
+        },
+        {
+            "x": 5.642662315095453,
+            "y": 6.608892934358269,
+            "heading": 0.04300979987381644,
+            "angularVelocity": -1.076842309423355,
+            "velocityX": 2.536597196588689,
+            "velocityY": -0.1329808532205177,
+            "timestamp": 0.30688118391086266
+        },
+        {
+            "x": 5.829535978248522,
+            "y": 6.597025931717244,
+            "heading": -0.024847542112422006,
+            "angularVelocity": -1.1055963275666998,
+            "velocityX": 3.0447233807490215,
+            "velocityY": -0.19334848897856358,
+            "timestamp": 0.3682574206930352
+        },
+        {
+            "x": 6.046434546533161,
+            "y": 6.5777851666622515,
+            "heading": -0.07267029178585922,
+            "angularVelocity": -0.7791737027338695,
+            "velocityX": 3.533917679808606,
+            "velocityY": -0.31348883645767445,
+            "timestamp": 0.4296336574752077
+        },
+        {
+            "x": 6.274079716361941,
+            "y": 6.532107529947078,
+            "heading": -0.07267036448034916,
+            "angularVelocity": -0.0000011844077407082938,
+            "velocityX": 3.709011528952235,
+            "velocityY": -0.7442234830605138,
+            "timestamp": 0.49100989425738023
+        },
+        {
+            "x": 6.493476634626264,
+            "y": 6.456122214314626,
+            "heading": -0.07267035803800323,
+            "angularVelocity": 1.049648246960038e-7,
+            "velocityX": 3.5746231728571716,
+            "velocityY": -1.238025001469686,
+            "timestamp": 0.5523861310395528
+        },
+        {
+            "x": 6.700610073343003,
+            "y": 6.351219816335689,
+            "heading": -0.07267035070446132,
+            "angularVelocity": 1.1948503655702063e-7,
+            "velocityX": 3.3748149051866063,
+            "velocityY": -1.7091695985083282,
+            "timestamp": 0.6137623678217253
+        },
+        {
+            "x": 6.891689244621814,
+            "y": 6.21932014936447,
+            "heading": -0.07267034149477326,
+            "angularVelocity": 1.5005299351249212e-7,
+            "velocityX": 3.1132435173071933,
+            "velocityY": -2.1490347712151214,
+            "timestamp": 0.6751386046038979
+        },
+        {
+            "x": 7.0632171630859375,
+            "y": 6.06283712387085,
+            "heading": -0.07267032569899272,
+            "angularVelocity": 2.573598736854518e-7,
+            "velocityX": 2.7946959190881038,
+            "velocityY": -2.549570219643589,
+            "timestamp": 0.7365148413860704
+        },
+        {
+            "x": 7.263317671759933,
+            "y": 5.7907628781558955,
+            "heading": -0.07267031513885969,
+            "angularVelocity": 1.182832965996254e-7,
+            "velocityX": 2.24131152203967,
+            "velocityY": -3.0474842158680824,
+            "timestamp": 0.8257931530827405
+        },
+        {
+            "x": 7.406264443711533,
+            "y": 5.484771313458312,
+            "heading": -0.07267030869611313,
+            "angularVelocity": 7.216474459034548e-8,
+            "velocityX": 1.6011365944875051,
+            "velocityY": -3.4273896860551396,
+            "timestamp": 0.9150714647794106
+        },
+        {
+            "x": 7.486522259331217,
+            "y": 5.1567114044359705,
+            "heading": -0.07267030368970051,
+            "angularVelocity": 5.607647052205946e-8,
+            "velocityX": 0.8989620669840581,
+            "velocityY": -3.6745756364317224,
+            "timestamp": 1.0043497764760807
+        },
+        {
+            "x": 7.50149841248159,
+            "y": 4.81930906784098,
+            "heading": -0.07267029905812865,
+            "angularVelocity": 5.187790600157932e-8,
+            "velocityX": 0.1677468230047605,
+            "velocityY": -3.7792195011633143,
+            "timestamp": 1.093628088172751
+        },
+        {
+            "x": 7.516449925771596,
+            "y": 4.481905638472984,
+            "heading": -0.0726702944266145,
+            "angularVelocity": 5.1877259457701335e-8,
+            "velocityX": 0.16747083368699095,
+            "velocityY": -3.7792317412357557,
+            "timestamp": 1.1829063998694211
+        },
+        {
+            "x": 7.531401402222247,
+            "y": 4.144502207472513,
+            "heading": -0.07267028979509418,
+            "angularVelocity": 5.187732873980297e-8,
+            "velocityX": 0.16747042105201052,
+            "velocityY": -3.779231759520992,
+            "timestamp": 1.2721847115660914
+        },
+        {
+            "x": 7.546352847973806,
+            "y": 3.807098775111667,
+            "heading": -0.07267028516356967,
+            "angularVelocity": 5.1877375603275234e-8,
+            "velocityX": 0.16747007719365256,
+            "velocityY": -3.7792317747584647,
+            "timestamp": 1.3614630232627616
+        },
+        {
+            "x": 7.561304268557873,
+            "y": 3.4696953416355707,
+            "heading": -0.07267028053204275,
+            "angularVelocity": 5.1877402813012475e-8,
+            "velocityX": 0.16746979529438435,
+            "velocityY": -3.7792317872503007,
+            "timestamp": 1.4507413349594318
+        },
+        {
+            "x": 7.576255668582546,
+            "y": 3.132291907248425,
+            "heading": -0.07267027590051456,
+            "angularVelocity": 5.1877416880821484e-8,
+            "velocityX": 0.16746956501007867,
+            "velocityY": -3.779231797454896,
+            "timestamp": 1.540019646656102
+        },
+        {
+            "x": 7.591207051730301,
+            "y": 2.794888472113414,
+            "heading": -0.07267027126898577,
+            "angularVelocity": 5.1877423431260937e-8,
+            "velocityX": 0.16746937597291248,
+            "velocityY": -3.779231805831691,
+            "timestamp": 1.6292979583527722
+        },
+        {
+            "x": 7.60615842090353,
+            "y": 2.457485036359151,
+            "heading": -0.07267026663745696,
+            "angularVelocity": 5.1877423851232326e-8,
+            "velocityX": 0.16746921944524512,
+            "velocityY": -3.779231812767884,
+            "timestamp": 1.7185762700494425
+        },
+        {
+            "x": 7.621109778388665,
+            "y": 2.1200816000869556,
+            "heading": -0.07267026200592838,
+            "angularVelocity": 5.187742117887202e-8,
+            "velocityX": 0.1674690885277524,
+            "velocityY": -3.779231818569208,
+            "timestamp": 1.8078545817461127
+        },
+        {
+            "x": 7.636061125621916,
+            "y": 1.7826781633604734,
+            "heading": -0.07267025737439563,
+            "angularVelocity": 5.1877468094637746e-8,
+            "velocityX": 0.16746897369713376,
+            "velocityY": -3.7792318236576405,
+            "timestamp": 1.897132893442783
+        },
+        {
+            "x": 7.6506850878742485,
+            "y": 1.4507667606163608,
+            "heading": -0.05536549896524321,
+            "angularVelocity": 0.19382936438075055,
+            "velocityX": 0.16380195788222543,
+            "velocityY": -3.7177159428351065,
+            "timestamp": 1.9864112051394531
+        },
+        {
+            "x": 7.662404359721407,
+            "y": 1.1852476637582392,
+            "heading": -0.0353858682020255,
+            "angularVelocity": 0.2237904187872895,
+            "velocityX": 0.13126672788097452,
+            "velocityY": -2.974060461181691,
+            "timestamp": 2.0756895168361233
+        },
+        {
+            "x": 7.6711999602966845,
+            "y": 0.9861189087552604,
+            "heading": -0.018423007440779238,
+            "angularVelocity": 0.18999979321832794,
+            "velocityX": 0.0985188945458782,
+            "velocityY": -2.2304269784976873,
+            "timestamp": 2.1649678285327933
+        },
+        {
+            "x": 7.677066105120016,
+            "y": 0.8533716446291538,
+            "heading": -0.006322956528169105,
+            "angularVelocity": 0.13553180702747777,
+            "velocityX": 0.06570626966225444,
+            "velocityY": -1.4868926349898441,
+            "timestamp": 2.2542461402294633
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -2.444948224573009e-31,
+            "angularVelocity": 0.0708229849781762,
+            "velocityX": 0.03286234724008497,
+            "velocityY": -0.7434240563896022,
+            "timestamp": 2.3435244519261333
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 1.883272219480887e-30,
+            "velocityX": 0,
+            "velocityY": -3.281240907044904e-32,
+            "timestamp": 2.4328027636228033
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj
index cf856573..462787d1 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj	
@@ -4,127 +4,127 @@
             "x": 5.176,
             "y": 6.63,
             "heading": 0.26,
-            "angularVelocity": 2.3848638783294046e-33,
-            "velocityX": 3.9311043555760955e-34,
-            "velocityY": 6.649026992883858e-33,
+            "angularVelocity": -5.95142511939464e-31,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 5.1029790541350515,
-            "y": 6.633224554642693,
-            "heading": 0.2665012951102971,
-            "angularVelocity": 0.06942751851370552,
-            "velocityX": -0.7797927928070008,
-            "velocityY": 0.03443511229011806,
-            "timestamp": 0.09364147314326547
+            "x": 5.1029790507443495,
+            "y": 6.633224483607701,
+            "heading": 0.26650125878935643,
+            "angularVelocity": 0.06942713064122713,
+            "velocityX": -0.7797928290155524,
+            "velocityY": 0.034434353705267144,
+            "timestamp": 0.09364147314336706
         },
         {
-            "x": 4.956937163671988,
-            "y": 6.639673650268821,
-            "heading": 0.2795036636182218,
-            "angularVelocity": 0.13885266935138055,
-            "velocityX": -1.5595855720854526,
-            "velocityY": 0.06887007871263316,
-            "timestamp": 0.18728294628653094
+            "x": 4.9569371541735245,
+            "y": 6.639673450192742,
+            "heading": 0.2795035687162854,
+            "angularVelocity": 0.13885204376302582,
+            "velocityX": -1.559585637308702,
+            "velocityY": 0.06886870067889261,
+            "timestamp": 0.18728294628673411
         },
         {
-            "x": 4.737874332057849,
-            "y": 6.649347273946163,
-            "heading": 0.29900673147988355,
-            "angularVelocity": 0.20827382576349504,
-            "velocityX": -2.3393783145528313,
-            "velocityY": 0.10330490703159452,
-            "timestamp": 0.2809244194297964
+            "x": 4.737874314376445,
+            "y": 6.649346898931416,
+            "heading": 0.2990065710588269,
+            "angularVelocity": 0.20827312608258508,
+            "velocityX": -2.339378401936154,
+            "velocityY": 0.1033030388561254,
+            "timestamp": 0.28092441943010116
         },
         {
-            "x": 4.445790573670186,
-            "y": 6.662245413482842,
-            "heading": 0.325009942942769,
-            "angularVelocity": 0.27768904727835797,
-            "velocityX": -3.1191709034819257,
-            "velocityY": 0.13773960515320766,
-            "timestamp": 0.3745658925730619
+            "x": 4.445790546320906,
+            "y": 6.66224482863892,
+            "heading": 0.3250097249050781,
+            "angularVelocity": 0.2776884319880837,
+            "velocityX": -3.119171006722006,
+            "velocityY": 0.13773736438080994,
+            "timestamp": 0.37456589257346823
         },
         {
-            "x": 4.091895329009691,
-            "y": 6.677873079689744,
-            "heading": 0.32500994593496846,
-            "angularVelocity": 3.195376913904642e-8,
-            "velocityX": -3.7792575531042867,
-            "velocityY": 0.16688829940708347,
-            "timestamp": 0.4682073657163274
+            "x": 4.09189529815297,
+            "y": 6.677872415392626,
+            "heading": 0.32500972789735083,
+            "angularVelocity": 3.195452790688951e-8,
+            "velocityX": -3.7792575905562438,
+            "velocityY": 0.16688745092443205,
+            "timestamp": 0.4682073657168353
         },
         {
-            "x": 3.738000080970794,
-            "y": 6.693500669359476,
-            "heading": 0.3250099489270859,
-            "angularVelocity": 3.1952908210754574e-8,
-            "velocityX": -3.779257589182282,
-            "velocityY": 0.1668874820649196,
-            "timestamp": 0.5618488388595928
+            "x": 3.7380000499890724,
+            "y": 6.693500002240241,
+            "heading": 0.32500973088947493,
+            "angularVelocity": 3.1952970747065184e-8,
+            "velocityX": -3.7792575905130557,
+            "velocityY": 0.16688745192729154,
+            "timestamp": 0.5618488388602023
         },
         {
-            "x": 3.3841048299235403,
-            "y": 6.709128190903334,
-            "heading": 0.3250099519191997,
-            "angularVelocity": 3.195286925539051e-8,
-            "velocityX": -3.779257621308623,
-            "velocityY": 0.16688675454671412,
-            "timestamp": 0.6554903120028582
+            "x": 3.384104801825175,
+            "y": 6.7091275890878554,
+            "heading": 0.3250097338815991,
+            "angularVelocity": 3.195297127919844e-8,
+            "velocityX": -3.7792575905130557,
+            "velocityY": 0.1668874519272919,
+            "timestamp": 0.6554903120035693
         },
         {
-            "x": 3.030209576168151,
-            "y": 6.724755651121081,
-            "heading": 0.32500995491150664,
-            "angularVelocity": 3.195491758844704e-8,
-            "velocityX": -3.7792576502288973,
-            "velocityY": 0.16688609964363924,
-            "timestamp": 0.7491317851461237
+            "x": 3.030209553657211,
+            "y": 6.724755175840924,
+            "heading": 0.32500973687387263,
+            "angularVelocity": 3.195453683514771e-8,
+            "velocityX": -3.7792575905565435,
+            "velocityY": 0.16688745091763021,
+            "timestamp": 0.7491317851469363
         },
         {
-            "x": 2.7381257642201002,
-            "y": 6.737653399625744,
-            "heading": 0.3510061559504441,
-            "angularVelocity": 0.2776141827578367,
-            "velocityX": -3.119171475454889,
-            "velocityY": 0.13773542931034466,
-            "timestamp": 0.8427732582893891
+            "x": 2.7381257502813003,
+            "y": 6.7376531044804615,
+            "heading": 0.35100602630019134,
+            "angularVelocity": 0.2776151266490911,
+            "velocityX": -3.119171383909283,
+            "velocityY": 0.13773735297596457,
+            "timestamp": 0.8427732582903034
         },
         {
-            "x": 2.519062886912394,
-            "y": 6.747326704432178,
-            "heading": 0.37050311197995217,
-            "angularVelocity": 0.20820855733088625,
-            "velocityX": -2.339378802515782,
-            "velocityY": 0.10330150179952581,
-            "timestamp": 0.9364147314326545
+            "x": 2.5190628797280783,
+            "y": 6.747326551971013,
+            "heading": 0.37050304728737704,
+            "angularVelocity": 0.20820925101574836,
+            "velocityX": -2.3393787303818834,
+            "velocityY": 0.10330302552738789,
+            "timestamp": 0.9364147314336704
         },
         {
-            "x": 2.373020963294867,
-            "y": 6.7537755697062725,
-            "heading": 0.383501026964712,
-            "angularVelocity": 0.13880511004851873,
-            "velocityX": -1.5595859261429292,
-            "velocityY": 0.06886761877636173,
-            "timestamp": 1.03005620457592
+            "x": 2.3730209608288053,
+            "y": 6.7537755172838185,
+            "heading": 0.3835010053218344,
+            "angularVelocity": 0.13880556977745576,
+            "velocityX": -1.559585875754842,
+            "velocityY": 0.06886868709264785,
+            "timestamp": 1.0300562045770374
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.39,
-            "angularVelocity": 0.06940272100743458,
-            "velocityX": -0.7797929789416104,
-            "velocityY": 0.034433784363791235,
-            "timestamp": 1.1236976777191856
+            "angularVelocity": 0.06940295213233785,
+            "velocityX": -0.7797929526056125,
+            "velocityY": 0.03443434418469861,
+            "timestamp": 1.1236976777204044
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.0630865476520398e-33,
-            "timestamp": 1.2173391508624511
+            "angularVelocity": -1.6940949553652625e-30,
+            "velocityX": -1.6586267397188983e-32,
+            "velocityY": 0,
+            "timestamp": 1.2173391508637714
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.traj
index cf856573..462787d1 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.traj	
@@ -4,127 +4,127 @@
             "x": 5.176,
             "y": 6.63,
             "heading": 0.26,
-            "angularVelocity": 2.3848638783294046e-33,
-            "velocityX": 3.9311043555760955e-34,
-            "velocityY": 6.649026992883858e-33,
+            "angularVelocity": -5.95142511939464e-31,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 5.1029790541350515,
-            "y": 6.633224554642693,
-            "heading": 0.2665012951102971,
-            "angularVelocity": 0.06942751851370552,
-            "velocityX": -0.7797927928070008,
-            "velocityY": 0.03443511229011806,
-            "timestamp": 0.09364147314326547
+            "x": 5.1029790507443495,
+            "y": 6.633224483607701,
+            "heading": 0.26650125878935643,
+            "angularVelocity": 0.06942713064122713,
+            "velocityX": -0.7797928290155524,
+            "velocityY": 0.034434353705267144,
+            "timestamp": 0.09364147314336706
         },
         {
-            "x": 4.956937163671988,
-            "y": 6.639673650268821,
-            "heading": 0.2795036636182218,
-            "angularVelocity": 0.13885266935138055,
-            "velocityX": -1.5595855720854526,
-            "velocityY": 0.06887007871263316,
-            "timestamp": 0.18728294628653094
+            "x": 4.9569371541735245,
+            "y": 6.639673450192742,
+            "heading": 0.2795035687162854,
+            "angularVelocity": 0.13885204376302582,
+            "velocityX": -1.559585637308702,
+            "velocityY": 0.06886870067889261,
+            "timestamp": 0.18728294628673411
         },
         {
-            "x": 4.737874332057849,
-            "y": 6.649347273946163,
-            "heading": 0.29900673147988355,
-            "angularVelocity": 0.20827382576349504,
-            "velocityX": -2.3393783145528313,
-            "velocityY": 0.10330490703159452,
-            "timestamp": 0.2809244194297964
+            "x": 4.737874314376445,
+            "y": 6.649346898931416,
+            "heading": 0.2990065710588269,
+            "angularVelocity": 0.20827312608258508,
+            "velocityX": -2.339378401936154,
+            "velocityY": 0.1033030388561254,
+            "timestamp": 0.28092441943010116
         },
         {
-            "x": 4.445790573670186,
-            "y": 6.662245413482842,
-            "heading": 0.325009942942769,
-            "angularVelocity": 0.27768904727835797,
-            "velocityX": -3.1191709034819257,
-            "velocityY": 0.13773960515320766,
-            "timestamp": 0.3745658925730619
+            "x": 4.445790546320906,
+            "y": 6.66224482863892,
+            "heading": 0.3250097249050781,
+            "angularVelocity": 0.2776884319880837,
+            "velocityX": -3.119171006722006,
+            "velocityY": 0.13773736438080994,
+            "timestamp": 0.37456589257346823
         },
         {
-            "x": 4.091895329009691,
-            "y": 6.677873079689744,
-            "heading": 0.32500994593496846,
-            "angularVelocity": 3.195376913904642e-8,
-            "velocityX": -3.7792575531042867,
-            "velocityY": 0.16688829940708347,
-            "timestamp": 0.4682073657163274
+            "x": 4.09189529815297,
+            "y": 6.677872415392626,
+            "heading": 0.32500972789735083,
+            "angularVelocity": 3.195452790688951e-8,
+            "velocityX": -3.7792575905562438,
+            "velocityY": 0.16688745092443205,
+            "timestamp": 0.4682073657168353
         },
         {
-            "x": 3.738000080970794,
-            "y": 6.693500669359476,
-            "heading": 0.3250099489270859,
-            "angularVelocity": 3.1952908210754574e-8,
-            "velocityX": -3.779257589182282,
-            "velocityY": 0.1668874820649196,
-            "timestamp": 0.5618488388595928
+            "x": 3.7380000499890724,
+            "y": 6.693500002240241,
+            "heading": 0.32500973088947493,
+            "angularVelocity": 3.1952970747065184e-8,
+            "velocityX": -3.7792575905130557,
+            "velocityY": 0.16688745192729154,
+            "timestamp": 0.5618488388602023
         },
         {
-            "x": 3.3841048299235403,
-            "y": 6.709128190903334,
-            "heading": 0.3250099519191997,
-            "angularVelocity": 3.195286925539051e-8,
-            "velocityX": -3.779257621308623,
-            "velocityY": 0.16688675454671412,
-            "timestamp": 0.6554903120028582
+            "x": 3.384104801825175,
+            "y": 6.7091275890878554,
+            "heading": 0.3250097338815991,
+            "angularVelocity": 3.195297127919844e-8,
+            "velocityX": -3.7792575905130557,
+            "velocityY": 0.1668874519272919,
+            "timestamp": 0.6554903120035693
         },
         {
-            "x": 3.030209576168151,
-            "y": 6.724755651121081,
-            "heading": 0.32500995491150664,
-            "angularVelocity": 3.195491758844704e-8,
-            "velocityX": -3.7792576502288973,
-            "velocityY": 0.16688609964363924,
-            "timestamp": 0.7491317851461237
+            "x": 3.030209553657211,
+            "y": 6.724755175840924,
+            "heading": 0.32500973687387263,
+            "angularVelocity": 3.195453683514771e-8,
+            "velocityX": -3.7792575905565435,
+            "velocityY": 0.16688745091763021,
+            "timestamp": 0.7491317851469363
         },
         {
-            "x": 2.7381257642201002,
-            "y": 6.737653399625744,
-            "heading": 0.3510061559504441,
-            "angularVelocity": 0.2776141827578367,
-            "velocityX": -3.119171475454889,
-            "velocityY": 0.13773542931034466,
-            "timestamp": 0.8427732582893891
+            "x": 2.7381257502813003,
+            "y": 6.7376531044804615,
+            "heading": 0.35100602630019134,
+            "angularVelocity": 0.2776151266490911,
+            "velocityX": -3.119171383909283,
+            "velocityY": 0.13773735297596457,
+            "timestamp": 0.8427732582903034
         },
         {
-            "x": 2.519062886912394,
-            "y": 6.747326704432178,
-            "heading": 0.37050311197995217,
-            "angularVelocity": 0.20820855733088625,
-            "velocityX": -2.339378802515782,
-            "velocityY": 0.10330150179952581,
-            "timestamp": 0.9364147314326545
+            "x": 2.5190628797280783,
+            "y": 6.747326551971013,
+            "heading": 0.37050304728737704,
+            "angularVelocity": 0.20820925101574836,
+            "velocityX": -2.3393787303818834,
+            "velocityY": 0.10330302552738789,
+            "timestamp": 0.9364147314336704
         },
         {
-            "x": 2.373020963294867,
-            "y": 6.7537755697062725,
-            "heading": 0.383501026964712,
-            "angularVelocity": 0.13880511004851873,
-            "velocityX": -1.5595859261429292,
-            "velocityY": 0.06886761877636173,
-            "timestamp": 1.03005620457592
+            "x": 2.3730209608288053,
+            "y": 6.7537755172838185,
+            "heading": 0.3835010053218344,
+            "angularVelocity": 0.13880556977745576,
+            "velocityX": -1.559585875754842,
+            "velocityY": 0.06886868709264785,
+            "timestamp": 1.0300562045770374
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.39,
-            "angularVelocity": 0.06940272100743458,
-            "velocityX": -0.7797929789416104,
-            "velocityY": 0.034433784363791235,
-            "timestamp": 1.1236976777191856
+            "angularVelocity": 0.06940295213233785,
+            "velocityX": -0.7797929526056125,
+            "velocityY": 0.03443434418469861,
+            "timestamp": 1.1236976777204044
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.0630865476520398e-33,
-            "timestamp": 1.2173391508624511
+            "angularVelocity": -1.6940949553652625e-30,
+            "velocityX": -1.6586267397188983e-32,
+            "velocityY": 0,
+            "timestamp": 1.2173391508637714
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj
index 6bf8a7f8..53d96783 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj	
@@ -5,144 +5,144 @@
             "y": 6.63,
             "heading": 0.26,
             "angularVelocity": 0,
-            "velocityX": -1.9284905870694258e-35,
-            "velocityY": -6.933375493218394e-34,
+            "velocityX": 0,
+            "velocityY": 3.363577949177338e-35,
             "timestamp": 0
         },
         {
-            "x": 5.118924935599245,
-            "y": 6.609128491648739,
-            "heading": 0.2512266763512042,
-            "angularVelocity": -0.10272378997697001,
-            "velocityX": -0.6682720441106688,
-            "velocityY": -0.24437721964946355,
-            "timestamp": 0.08540693106010512
+            "x": 5.118924970919473,
+            "y": 6.609128397315466,
+            "heading": 0.25122660793754975,
+            "angularVelocity": -0.10272459100864365,
+            "velocityX": -0.6682716305586017,
+            "velocityY": -0.24437832416494054,
+            "timestamp": 0.08540693106008139
         },
         {
-            "x": 5.004774806713452,
-            "y": 6.567385472222137,
-            "heading": 0.23368042064504363,
-            "angularVelocity": -0.2054429949463047,
-            "velocityX": -1.3365440892081688,
-            "velocityY": -0.4887544711942054,
-            "timestamp": 0.17081386212021024
+            "x": 5.0047749070529814,
+            "y": 6.567385203608531,
+            "heading": 0.23368024533248913,
+            "angularVelocity": -0.20544424658833893,
+            "velocityX": -1.3365433279201937,
+            "velocityY": -0.48875651178205104,
+            "timestamp": 0.17081386212016278
         },
         {
-            "x": 4.833549609463453,
-            "y": 6.504770942290587,
-            "heading": 0.2073620554535925,
-            "angularVelocity": -0.3081525687058067,
-            "velocityX": -2.0048161797255077,
-            "velocityY": -0.7331317160604409,
-            "timestamp": 0.2562207931803154
+            "x": 4.833549798383878,
+            "y": 6.504770434941582,
+            "heading": 0.20736177398264571,
+            "angularVelocity": -0.30815381167752187,
+            "velocityX": -2.004815142563213,
+            "velocityY": -0.7331345113302418,
+            "timestamp": 0.25622079318024416
         },
         {
-            "x": 4.605249336717623,
-            "y": 6.421284904520194,
-            "heading": 0.17227303630641458,
-            "angularVelocity": -0.4108451001767527,
-            "velocityX": -2.673088353744534,
-            "velocityY": -0.9775089297101582,
-            "timestamp": 0.3416277242404205
+            "x": 4.605249631158782,
+            "y": 6.421284110658292,
+            "heading": 0.17227269285622732,
+            "angularVelocity": -0.4108458258702004,
+            "velocityX": -2.6730871182397737,
+            "velocityY": -0.977512284390117,
+            "timestamp": 0.34162772424032556
         },
         {
-            "x": 4.319873982796504,
-            "y": 6.316927367787122,
-            "heading": 0.12841583542221255,
-            "angularVelocity": -0.5135086853002309,
-            "velocityX": -3.341360594262375,
-            "velocityY": -1.22188603943198,
-            "timestamp": 0.4270346553005256
+            "x": 4.319874394275373,
+            "y": 6.316926253811125,
+            "heading": 0.12841549400557886,
+            "angularVelocity": -0.5135086614902031,
+            "velocityX": -3.3413592239095435,
+            "velocityY": -1.2218897875367427,
+            "timestamp": 0.42703465530040696
         },
         {
-            "x": 4.016437213590231,
-            "y": 6.205964096134007,
-            "heading": 0.12841583017477892,
-            "angularVelocity": -6.144037201192911e-8,
-            "velocityX": -3.552835413237479,
-            "velocityY": -1.2992302881721054,
-            "timestamp": 0.5124415863606308
+            "x": 4.016437653324315,
+            "y": 6.205962904892466,
+            "heading": 0.1284154887581796,
+            "angularVelocity": -6.143996998796484e-8,
+            "velocityX": -3.552835082407984,
+            "velocityY": -1.2992311928477889,
+            "timestamp": 0.5124415863604883
         },
         {
-            "x": 3.7130004751736263,
-            "y": 6.095000740284403,
-            "heading": 0.12841582492737386,
-            "angularVelocity": -6.144003768621477e-8,
-            "velocityX": -3.5528350527319725,
-            "velocityY": -1.2992312739994434,
-            "timestamp": 0.5978485174207359
+            "x": 3.713000912388581,
+            "y": 6.094999555931864,
+            "heading": 0.12841548351080107,
+            "angularVelocity": -6.143972697353832e-8,
+            "velocityX": -3.5528350822285746,
+            "velocityY": -1.299231193338901,
+            "timestamp": 0.5978485174205697
         },
         {
-            "x": 3.409563765577162,
-            "y": 5.984037305624233,
-            "heading": 0.12841581967997912,
-            "angularVelocity": -6.143991704551595e-8,
-            "velocityX": -3.5528347152869912,
-            "velocityY": -1.2992321967649234,
-            "timestamp": 0.683255448480841
+            "x": 3.4095641714528466,
+            "y": 5.98403620697126,
+            "heading": 0.12841547826342262,
+            "angularVelocity": -6.143972595662007e-8,
+            "velocityX": -3.5528350822285746,
+            "velocityY": -1.299231193338901,
+            "timestamp": 0.683255448480651
         },
         {
-            "x": 3.1061270838779644,
-            "y": 5.873073794677257,
-            "heading": 0.12841581443257097,
-            "angularVelocity": -6.1440073698458e-8,
-            "velocityX": -3.5528343886476135,
-            "velocityY": -1.2992330899805498,
-            "timestamp": 0.7686623795409461
+            "x": 3.106127430499178,
+            "y": 5.873072858059743,
+            "heading": 0.12841547301602332,
+            "angularVelocity": -6.143997022352915e-8,
+            "velocityX": -3.55283508243856,
+            "velocityY": -1.2992311927641775,
+            "timestamp": 0.7686623795407324
         },
         {
-            "x": 2.8207514220046077,
-            "y": 5.76871589253347,
-            "heading": 0.08460828566897327,
-            "angularVelocity": -0.512927091745846,
-            "velocityX": -3.3413641999678414,
-            "velocityY": -1.2218903178987286,
-            "timestamp": 0.8540693106010512
+            "x": 2.820751661400481,
+            "y": 5.768715245814204,
+            "heading": 0.08460804498823243,
+            "angularVelocity": -0.5129259122655233,
+            "velocityX": -3.3413654554329186,
+            "velocityY": -1.2218869235814813,
+            "timestamp": 0.8540693106008137
         },
         {
-            "x": 2.592450862147759,
-            "y": 5.685229551240001,
-            "heading": 0.0495641189011112,
-            "angularVelocity": -0.4103199392939169,
-            "velocityX": -2.673091715427432,
-            "velocityY": -0.9775124835561102,
-            "timestamp": 0.9394762416611563
+            "x": 2.5924510107246115,
+            "y": 5.685229149936981,
+            "heading": 0.04956396734437731,
+            "angularVelocity": -0.41031889577208425,
+            "velocityX": -2.6730927787964465,
+            "velocityY": -0.977509610063062,
+            "timestamp": 0.939476241660895
         },
         {
-            "x": 2.421225431748781,
-            "y": 5.622614782821321,
-            "heading": 0.02328193601640482,
-            "angularVelocity": -0.30772892268205126,
-            "velocityX": -2.004818909585662,
-            "velocityY": -0.7331345084231428,
-            "timestamp": 1.0248831727212615
+            "x": 2.4212255084791887,
+            "y": 5.622614575599846,
+            "heading": 0.02328185696555881,
+            "angularVelocity": -0.30772807373595673,
+            "velocityX": -2.00481975081121,
+            "velocityY": -0.7331322359901695,
+            "timestamp": 1.0248831727209764
         },
         {
-            "x": 2.3070751430245986,
-            "y": 5.580871596442558,
-            "heading": 0.005760710543037936,
-            "angularVelocity": -0.2051499246710589,
-            "velocityX": -1.3365459607001804,
-            "velocityY": -0.48875642597890145,
-            "timestamp": 1.1102901037813666
+            "x": 2.307075169405903,
+            "y": 5.580871525201533,
+            "heading": 0.005760683198102726,
+            "angularVelocity": -0.20514931926462068,
+            "velocityX": -1.3365465502206675,
+            "velocityY": -0.48875483383130996,
+            "timestamp": 1.1102901037810577
         },
         {
             "x": 2.25,
             "y": 5.56,
             "heading": -0.003,
-            "angularVelocity": -0.10257610751605846,
-            "velocityX": -0.6682729646898584,
-            "velocityY": -0.24437825107975505,
-            "timestamp": 1.1956970348414717
+            "angularVelocity": -0.10257578734376759,
+            "velocityX": -0.6682732735795421,
+            "velocityY": -0.24437741694350015,
+            "timestamp": 1.195697034841139
         },
         {
             "x": 2.25,
             "y": 5.56,
             "heading": -0.003,
-            "angularVelocity": -5.146817856452735e-35,
-            "velocityX": 4.663929716319662e-34,
-            "velocityY": 0,
-            "timestamp": 1.2811039659015768
+            "angularVelocity": 2.4865352537032555e-34,
+            "velocityX": 0,
+            "velocityY": -6.568641934425214e-34,
+            "timestamp": 1.2811039659012204
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.traj
index 6bf8a7f8..53d96783 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.traj	
@@ -5,144 +5,144 @@
             "y": 6.63,
             "heading": 0.26,
             "angularVelocity": 0,
-            "velocityX": -1.9284905870694258e-35,
-            "velocityY": -6.933375493218394e-34,
+            "velocityX": 0,
+            "velocityY": 3.363577949177338e-35,
             "timestamp": 0
         },
         {
-            "x": 5.118924935599245,
-            "y": 6.609128491648739,
-            "heading": 0.2512266763512042,
-            "angularVelocity": -0.10272378997697001,
-            "velocityX": -0.6682720441106688,
-            "velocityY": -0.24437721964946355,
-            "timestamp": 0.08540693106010512
+            "x": 5.118924970919473,
+            "y": 6.609128397315466,
+            "heading": 0.25122660793754975,
+            "angularVelocity": -0.10272459100864365,
+            "velocityX": -0.6682716305586017,
+            "velocityY": -0.24437832416494054,
+            "timestamp": 0.08540693106008139
         },
         {
-            "x": 5.004774806713452,
-            "y": 6.567385472222137,
-            "heading": 0.23368042064504363,
-            "angularVelocity": -0.2054429949463047,
-            "velocityX": -1.3365440892081688,
-            "velocityY": -0.4887544711942054,
-            "timestamp": 0.17081386212021024
+            "x": 5.0047749070529814,
+            "y": 6.567385203608531,
+            "heading": 0.23368024533248913,
+            "angularVelocity": -0.20544424658833893,
+            "velocityX": -1.3365433279201937,
+            "velocityY": -0.48875651178205104,
+            "timestamp": 0.17081386212016278
         },
         {
-            "x": 4.833549609463453,
-            "y": 6.504770942290587,
-            "heading": 0.2073620554535925,
-            "angularVelocity": -0.3081525687058067,
-            "velocityX": -2.0048161797255077,
-            "velocityY": -0.7331317160604409,
-            "timestamp": 0.2562207931803154
+            "x": 4.833549798383878,
+            "y": 6.504770434941582,
+            "heading": 0.20736177398264571,
+            "angularVelocity": -0.30815381167752187,
+            "velocityX": -2.004815142563213,
+            "velocityY": -0.7331345113302418,
+            "timestamp": 0.25622079318024416
         },
         {
-            "x": 4.605249336717623,
-            "y": 6.421284904520194,
-            "heading": 0.17227303630641458,
-            "angularVelocity": -0.4108451001767527,
-            "velocityX": -2.673088353744534,
-            "velocityY": -0.9775089297101582,
-            "timestamp": 0.3416277242404205
+            "x": 4.605249631158782,
+            "y": 6.421284110658292,
+            "heading": 0.17227269285622732,
+            "angularVelocity": -0.4108458258702004,
+            "velocityX": -2.6730871182397737,
+            "velocityY": -0.977512284390117,
+            "timestamp": 0.34162772424032556
         },
         {
-            "x": 4.319873982796504,
-            "y": 6.316927367787122,
-            "heading": 0.12841583542221255,
-            "angularVelocity": -0.5135086853002309,
-            "velocityX": -3.341360594262375,
-            "velocityY": -1.22188603943198,
-            "timestamp": 0.4270346553005256
+            "x": 4.319874394275373,
+            "y": 6.316926253811125,
+            "heading": 0.12841549400557886,
+            "angularVelocity": -0.5135086614902031,
+            "velocityX": -3.3413592239095435,
+            "velocityY": -1.2218897875367427,
+            "timestamp": 0.42703465530040696
         },
         {
-            "x": 4.016437213590231,
-            "y": 6.205964096134007,
-            "heading": 0.12841583017477892,
-            "angularVelocity": -6.144037201192911e-8,
-            "velocityX": -3.552835413237479,
-            "velocityY": -1.2992302881721054,
-            "timestamp": 0.5124415863606308
+            "x": 4.016437653324315,
+            "y": 6.205962904892466,
+            "heading": 0.1284154887581796,
+            "angularVelocity": -6.143996998796484e-8,
+            "velocityX": -3.552835082407984,
+            "velocityY": -1.2992311928477889,
+            "timestamp": 0.5124415863604883
         },
         {
-            "x": 3.7130004751736263,
-            "y": 6.095000740284403,
-            "heading": 0.12841582492737386,
-            "angularVelocity": -6.144003768621477e-8,
-            "velocityX": -3.5528350527319725,
-            "velocityY": -1.2992312739994434,
-            "timestamp": 0.5978485174207359
+            "x": 3.713000912388581,
+            "y": 6.094999555931864,
+            "heading": 0.12841548351080107,
+            "angularVelocity": -6.143972697353832e-8,
+            "velocityX": -3.5528350822285746,
+            "velocityY": -1.299231193338901,
+            "timestamp": 0.5978485174205697
         },
         {
-            "x": 3.409563765577162,
-            "y": 5.984037305624233,
-            "heading": 0.12841581967997912,
-            "angularVelocity": -6.143991704551595e-8,
-            "velocityX": -3.5528347152869912,
-            "velocityY": -1.2992321967649234,
-            "timestamp": 0.683255448480841
+            "x": 3.4095641714528466,
+            "y": 5.98403620697126,
+            "heading": 0.12841547826342262,
+            "angularVelocity": -6.143972595662007e-8,
+            "velocityX": -3.5528350822285746,
+            "velocityY": -1.299231193338901,
+            "timestamp": 0.683255448480651
         },
         {
-            "x": 3.1061270838779644,
-            "y": 5.873073794677257,
-            "heading": 0.12841581443257097,
-            "angularVelocity": -6.1440073698458e-8,
-            "velocityX": -3.5528343886476135,
-            "velocityY": -1.2992330899805498,
-            "timestamp": 0.7686623795409461
+            "x": 3.106127430499178,
+            "y": 5.873072858059743,
+            "heading": 0.12841547301602332,
+            "angularVelocity": -6.143997022352915e-8,
+            "velocityX": -3.55283508243856,
+            "velocityY": -1.2992311927641775,
+            "timestamp": 0.7686623795407324
         },
         {
-            "x": 2.8207514220046077,
-            "y": 5.76871589253347,
-            "heading": 0.08460828566897327,
-            "angularVelocity": -0.512927091745846,
-            "velocityX": -3.3413641999678414,
-            "velocityY": -1.2218903178987286,
-            "timestamp": 0.8540693106010512
+            "x": 2.820751661400481,
+            "y": 5.768715245814204,
+            "heading": 0.08460804498823243,
+            "angularVelocity": -0.5129259122655233,
+            "velocityX": -3.3413654554329186,
+            "velocityY": -1.2218869235814813,
+            "timestamp": 0.8540693106008137
         },
         {
-            "x": 2.592450862147759,
-            "y": 5.685229551240001,
-            "heading": 0.0495641189011112,
-            "angularVelocity": -0.4103199392939169,
-            "velocityX": -2.673091715427432,
-            "velocityY": -0.9775124835561102,
-            "timestamp": 0.9394762416611563
+            "x": 2.5924510107246115,
+            "y": 5.685229149936981,
+            "heading": 0.04956396734437731,
+            "angularVelocity": -0.41031889577208425,
+            "velocityX": -2.6730927787964465,
+            "velocityY": -0.977509610063062,
+            "timestamp": 0.939476241660895
         },
         {
-            "x": 2.421225431748781,
-            "y": 5.622614782821321,
-            "heading": 0.02328193601640482,
-            "angularVelocity": -0.30772892268205126,
-            "velocityX": -2.004818909585662,
-            "velocityY": -0.7331345084231428,
-            "timestamp": 1.0248831727212615
+            "x": 2.4212255084791887,
+            "y": 5.622614575599846,
+            "heading": 0.02328185696555881,
+            "angularVelocity": -0.30772807373595673,
+            "velocityX": -2.00481975081121,
+            "velocityY": -0.7331322359901695,
+            "timestamp": 1.0248831727209764
         },
         {
-            "x": 2.3070751430245986,
-            "y": 5.580871596442558,
-            "heading": 0.005760710543037936,
-            "angularVelocity": -0.2051499246710589,
-            "velocityX": -1.3365459607001804,
-            "velocityY": -0.48875642597890145,
-            "timestamp": 1.1102901037813666
+            "x": 2.307075169405903,
+            "y": 5.580871525201533,
+            "heading": 0.005760683198102726,
+            "angularVelocity": -0.20514931926462068,
+            "velocityX": -1.3365465502206675,
+            "velocityY": -0.48875483383130996,
+            "timestamp": 1.1102901037810577
         },
         {
             "x": 2.25,
             "y": 5.56,
             "heading": -0.003,
-            "angularVelocity": -0.10257610751605846,
-            "velocityX": -0.6682729646898584,
-            "velocityY": -0.24437825107975505,
-            "timestamp": 1.1956970348414717
+            "angularVelocity": -0.10257578734376759,
+            "velocityX": -0.6682732735795421,
+            "velocityY": -0.24437741694350015,
+            "timestamp": 1.195697034841139
         },
         {
             "x": 2.25,
             "y": 5.56,
             "heading": -0.003,
-            "angularVelocity": -5.146817856452735e-35,
-            "velocityX": 4.663929716319662e-34,
-            "velocityY": 0,
-            "timestamp": 1.2811039659015768
+            "angularVelocity": 2.4865352537032555e-34,
+            "velocityX": 0,
+            "velocityY": -6.568641934425214e-34,
+            "timestamp": 1.2811039659012204
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.1.traj
new file mode 100644
index 00000000..359cb1db
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.1.traj	
@@ -0,0 +1,175 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": 2.521971410376041e-31,
+            "velocityX": -3.46085485178236e-32,
+            "velocityY": -2.8734015412732535e-37,
+            "timestamp": 0
+        },
+        {
+            "x": 5.132269964046874,
+            "y": 6.606195610268081,
+            "heading": 0.16914444930535336,
+            "angularVelocity": -1.1148212857556328,
+            "velocityX": -0.5365789380469493,
+            "velocityY": -0.29208606589986635,
+            "timestamp": 0.08149786145594107
+        },
+        {
+            "x": 5.043666795437562,
+            "y": 6.5570487224925476,
+            "heading": 0.005335724612659364,
+            "angularVelocity": -2.0099757437347154,
+            "velocityX": -1.0871839705537814,
+            "velocityY": -0.6030451216453527,
+            "timestamp": 0.16299572291188213
+        },
+        {
+            "x": 4.909391168326261,
+            "y": 6.4806746643853055,
+            "heading": -0.2131663273161798,
+            "angularVelocity": -2.681077123072297,
+            "velocityX": -1.647596939508567,
+            "velocityY": -0.9371295975481627,
+            "timestamp": 0.2444935843678232
+        },
+        {
+            "x": 4.728031239930722,
+            "y": 6.375889843725219,
+            "heading": -0.4550458803770597,
+            "angularVelocity": -2.967925154596154,
+            "velocityX": -2.225333587355345,
+            "velocityY": -1.2857370584715861,
+            "timestamp": 0.32599144582376427
+        },
+        {
+            "x": 4.50844214888531,
+            "y": 6.243100931440797,
+            "heading": -0.5880935326141025,
+            "angularVelocity": -1.6325293677671568,
+            "velocityX": -2.6944153763362864,
+            "velocityY": -1.6293545611158133,
+            "timestamp": 0.40748930727970534
+        },
+        {
+            "x": 4.251218650398233,
+            "y": 6.083818287155582,
+            "heading": -0.6031902279140122,
+            "angularVelocity": -0.1852403858237583,
+            "velocityX": -3.15619936390767,
+            "velocityY": -1.9544395575499292,
+            "timestamp": 0.4889871687356464
+        },
+        {
+            "x": 3.9855391766757995,
+            "y": 5.92740773635831,
+            "heading": -0.6031903133870022,
+            "angularVelocity": -0.0000010487758630839914,
+            "velocityX": -3.259956383837921,
+            "velocityY": -1.9191982219291623,
+            "timestamp": 0.5704850301915875
+        },
+        {
+            "x": 3.719859695364488,
+            "y": 5.770997198451498,
+            "heading": -0.603190398859952,
+            "angularVelocity": -0.0000010487753700599598,
+            "velocityX": -3.259956476955436,
+            "velocityY": -1.919198063759863,
+            "timestamp": 0.6519828916475285
+        },
+        {
+            "x": 3.4541802321431443,
+            "y": 5.614586629816999,
+            "heading": -0.6031904843329001,
+            "angularVelocity": -0.000001048775349610677,
+            "velocityX": -3.259956254986808,
+            "velocityY": -1.9191984407966034,
+            "timestamp": 0.7334807531034696
+        },
+        {
+            "x": 3.1885009063173535,
+            "y": 5.458175827802239,
+            "heading": -0.6031905698058837,
+            "angularVelocity": -0.000001048775783641969,
+            "velocityX": -3.259954569107567,
+            "velocityY": -1.9192013044332228,
+            "timestamp": 0.8149786145594107
+        },
+        {
+            "x": 2.937386380849553,
+            "y": 5.279314604480798,
+            "heading": -0.603190661977047,
+            "angularVelocity": -0.0000011309641953921016,
+            "velocityX": -3.0812406728433848,
+            "velocityY": -2.1946738248847923,
+            "timestamp": 0.8964764760153517
+        },
+        {
+            "x": 2.722322578778117,
+            "y": 5.058413395221267,
+            "heading": -0.6031910253459357,
+            "angularVelocity": -0.00000445863096683889,
+            "velocityX": -2.6388889012468426,
+            "velocityY": -2.710515408787179,
+            "timestamp": 0.9779743374712928
+        },
+        {
+            "x": 2.561151389441887,
+            "y": 4.8410288515668665,
+            "heading": -0.5595876383179096,
+            "angularVelocity": 0.5350249227287813,
+            "velocityX": -1.9776124975176395,
+            "velocityY": -2.667365005300435,
+            "timestamp": 1.0594721989272338
+        },
+        {
+            "x": 2.4444023928247787,
+            "y": 4.656624455699537,
+            "heading": -0.5072145719014195,
+            "angularVelocity": 0.6426311743750899,
+            "velocityX": -1.432540615562337,
+            "velocityY": -2.262689996681954,
+            "timestamp": 1.1409700603831747
+        },
+        {
+            "x": 2.367465069003143,
+            "y": 4.510717874734717,
+            "heading": -0.4552059179368463,
+            "angularVelocity": 0.6381597386170672,
+            "velocityX": -0.9440410146618312,
+            "velocityY": -1.7903117745451358,
+            "timestamp": 1.2224679218391157
+        },
+        {
+            "x": 2.321242179529733,
+            "y": 4.410775659264178,
+            "heading": -0.4150276653385325,
+            "angularVelocity": 0.49299763061923907,
+            "velocityX": -0.5671669004271778,
+            "velocityY": -1.2263170307182782,
+            "timestamp": 1.3039657832950566
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.393,
+            "angularVelocity": 0.27028519454392347,
+            "velocityX": -0.26064707895700284,
+            "velocityY": -0.6230305723006956,
+            "timestamp": 1.3854636447509976
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.393,
+            "angularVelocity": -9.678618240747916e-31,
+            "velocityX": 0,
+            "velocityY": -3.108003047332484e-31,
+            "timestamp": 1.4669615062069385
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.traj
new file mode 100644
index 00000000..359cb1db
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.traj	
@@ -0,0 +1,175 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": 2.521971410376041e-31,
+            "velocityX": -3.46085485178236e-32,
+            "velocityY": -2.8734015412732535e-37,
+            "timestamp": 0
+        },
+        {
+            "x": 5.132269964046874,
+            "y": 6.606195610268081,
+            "heading": 0.16914444930535336,
+            "angularVelocity": -1.1148212857556328,
+            "velocityX": -0.5365789380469493,
+            "velocityY": -0.29208606589986635,
+            "timestamp": 0.08149786145594107
+        },
+        {
+            "x": 5.043666795437562,
+            "y": 6.5570487224925476,
+            "heading": 0.005335724612659364,
+            "angularVelocity": -2.0099757437347154,
+            "velocityX": -1.0871839705537814,
+            "velocityY": -0.6030451216453527,
+            "timestamp": 0.16299572291188213
+        },
+        {
+            "x": 4.909391168326261,
+            "y": 6.4806746643853055,
+            "heading": -0.2131663273161798,
+            "angularVelocity": -2.681077123072297,
+            "velocityX": -1.647596939508567,
+            "velocityY": -0.9371295975481627,
+            "timestamp": 0.2444935843678232
+        },
+        {
+            "x": 4.728031239930722,
+            "y": 6.375889843725219,
+            "heading": -0.4550458803770597,
+            "angularVelocity": -2.967925154596154,
+            "velocityX": -2.225333587355345,
+            "velocityY": -1.2857370584715861,
+            "timestamp": 0.32599144582376427
+        },
+        {
+            "x": 4.50844214888531,
+            "y": 6.243100931440797,
+            "heading": -0.5880935326141025,
+            "angularVelocity": -1.6325293677671568,
+            "velocityX": -2.6944153763362864,
+            "velocityY": -1.6293545611158133,
+            "timestamp": 0.40748930727970534
+        },
+        {
+            "x": 4.251218650398233,
+            "y": 6.083818287155582,
+            "heading": -0.6031902279140122,
+            "angularVelocity": -0.1852403858237583,
+            "velocityX": -3.15619936390767,
+            "velocityY": -1.9544395575499292,
+            "timestamp": 0.4889871687356464
+        },
+        {
+            "x": 3.9855391766757995,
+            "y": 5.92740773635831,
+            "heading": -0.6031903133870022,
+            "angularVelocity": -0.0000010487758630839914,
+            "velocityX": -3.259956383837921,
+            "velocityY": -1.9191982219291623,
+            "timestamp": 0.5704850301915875
+        },
+        {
+            "x": 3.719859695364488,
+            "y": 5.770997198451498,
+            "heading": -0.603190398859952,
+            "angularVelocity": -0.0000010487753700599598,
+            "velocityX": -3.259956476955436,
+            "velocityY": -1.919198063759863,
+            "timestamp": 0.6519828916475285
+        },
+        {
+            "x": 3.4541802321431443,
+            "y": 5.614586629816999,
+            "heading": -0.6031904843329001,
+            "angularVelocity": -0.000001048775349610677,
+            "velocityX": -3.259956254986808,
+            "velocityY": -1.9191984407966034,
+            "timestamp": 0.7334807531034696
+        },
+        {
+            "x": 3.1885009063173535,
+            "y": 5.458175827802239,
+            "heading": -0.6031905698058837,
+            "angularVelocity": -0.000001048775783641969,
+            "velocityX": -3.259954569107567,
+            "velocityY": -1.9192013044332228,
+            "timestamp": 0.8149786145594107
+        },
+        {
+            "x": 2.937386380849553,
+            "y": 5.279314604480798,
+            "heading": -0.603190661977047,
+            "angularVelocity": -0.0000011309641953921016,
+            "velocityX": -3.0812406728433848,
+            "velocityY": -2.1946738248847923,
+            "timestamp": 0.8964764760153517
+        },
+        {
+            "x": 2.722322578778117,
+            "y": 5.058413395221267,
+            "heading": -0.6031910253459357,
+            "angularVelocity": -0.00000445863096683889,
+            "velocityX": -2.6388889012468426,
+            "velocityY": -2.710515408787179,
+            "timestamp": 0.9779743374712928
+        },
+        {
+            "x": 2.561151389441887,
+            "y": 4.8410288515668665,
+            "heading": -0.5595876383179096,
+            "angularVelocity": 0.5350249227287813,
+            "velocityX": -1.9776124975176395,
+            "velocityY": -2.667365005300435,
+            "timestamp": 1.0594721989272338
+        },
+        {
+            "x": 2.4444023928247787,
+            "y": 4.656624455699537,
+            "heading": -0.5072145719014195,
+            "angularVelocity": 0.6426311743750899,
+            "velocityX": -1.432540615562337,
+            "velocityY": -2.262689996681954,
+            "timestamp": 1.1409700603831747
+        },
+        {
+            "x": 2.367465069003143,
+            "y": 4.510717874734717,
+            "heading": -0.4552059179368463,
+            "angularVelocity": 0.6381597386170672,
+            "velocityX": -0.9440410146618312,
+            "velocityY": -1.7903117745451358,
+            "timestamp": 1.2224679218391157
+        },
+        {
+            "x": 2.321242179529733,
+            "y": 4.410775659264178,
+            "heading": -0.4150276653385325,
+            "angularVelocity": 0.49299763061923907,
+            "velocityX": -0.5671669004271778,
+            "velocityY": -1.2263170307182782,
+            "timestamp": 1.3039657832950566
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.393,
+            "angularVelocity": 0.27028519454392347,
+            "velocityX": -0.26064707895700284,
+            "velocityY": -0.6230305723006956,
+            "timestamp": 1.3854636447509976
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.393,
+            "angularVelocity": -9.678618240747916e-31,
+            "velocityX": 0,
+            "velocityY": -3.108003047332484e-31,
+            "timestamp": 1.4669615062069385
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.1.traj
index 60b69798..29c2d4bc 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.1.traj	
@@ -3,146 +3,146 @@
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": -5.481945073939837e-30,
-            "angularVelocity": 8.103891066426341e-30,
-            "velocityX": -1.0115776003849141e-29,
-            "velocityY": 1.8597851705411875e-30,
+            "heading": 0,
+            "angularVelocity": 5.380258045140596e-28,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 4.304265157789358,
-            "y": 5.638698398929109,
-            "heading": -0.04261689056565391,
-            "angularVelocity": -0.41098438981919094,
-            "velocityX": 0.39794868817859114,
-            "velocityY": 0.7589435323492035,
-            "timestamp": 0.10369466972795005
+            "x": 4.304265282414897,
+            "y": 5.638698322322049,
+            "heading": -0.04261698283734242,
+            "angularVelocity": -0.41098530784979936,
+            "velocityX": 0.3979499173257931,
+            "velocityY": 0.7589428456312429,
+            "timestamp": 0.10369466261532896
         },
         {
-            "x": 4.388659791773407,
-            "y": 5.795293123128996,
-            "heading": -0.12280542510902584,
-            "angularVelocity": -0.7733139490558338,
-            "velocityX": 0.8138762986126182,
-            "velocityY": 1.5101521091752137,
-            "timestamp": 0.2073893394559001
+            "x": 4.38866009880019,
+            "y": 5.795292929312062,
+            "heading": -0.1228057028496872,
+            "angularVelocity": -0.7733157907057984,
+            "velocityX": 0.8138781134605525,
+            "velocityY": 1.510151082422871,
+            "timestamp": 0.20738932523065792
         },
         {
-            "x": 4.519821289569113,
-            "y": 6.028001369445331,
-            "heading": -0.2301897087601478,
-            "angularVelocity": -1.035581519598341,
-            "velocityX": 1.2648817739601788,
-            "velocityY": 2.244167872146774,
-            "timestamp": 0.31108400918385015
+            "x": 4.519821720835651,
+            "y": 6.028001091828693,
+            "heading": -0.23019023538729733,
+            "angularVelocity": -1.0355839908170448,
+            "velocityX": 1.264883058851596,
+            "velocityY": 2.2441672179396264,
+            "timestamp": 0.3110839878459869
         },
         {
-            "x": 4.707676223581395,
-            "y": 6.330157885665521,
-            "heading": -0.33387515355278086,
-            "angularVelocity": -0.9999110375168158,
-            "velocityX": 1.811616108187066,
-            "velocityY": 2.9139059607683615,
-            "timestamp": 0.4147786789118002
+            "x": 4.707676475524912,
+            "y": 6.330157785456564,
+            "heading": -0.33387571773962255,
+            "angularVelocity": -0.9999114683169592,
+            "velocityX": 1.8116145031122473,
+            "velocityY": 2.9139078715050806,
+            "timestamp": 0.41477865046131585
         },
         {
-            "x": 4.978690084088412,
-            "y": 6.613755913169546,
-            "heading": -0.33387515567200693,
-            "angularVelocity": -2.0437175255333274e-8,
-            "velocityX": 2.613575617898612,
-            "velocityY": 2.734933514403967,
-            "timestamp": 0.5184733486397503
+            "x": 4.978690211282069,
+            "y": 6.613755936139097,
+            "heading": -0.3338757185080882,
+            "angularVelocity": -7.410850442524386e-9,
+            "velocityX": 2.6135745941189206,
+            "velocityY": 2.7349348898948014,
+            "timestamp": 0.5184733130766448
         },
         {
-            "x": 5.307019069498364,
-            "y": 6.828410126616754,
-            "heading": -0.33387512130937075,
-            "angularVelocity": 3.313828600417541e-7,
-            "velocityX": 3.1663053295925763,
-            "velocityY": 2.0700602452408625,
-            "timestamp": 0.6221680183677003
+            "x": 5.307019127730767,
+            "y": 6.828410259978494,
+            "heading": -0.3338757060420003,
+            "angularVelocity": 1.2021918642580205e-7,
+            "velocityX": 3.166304881734213,
+            "velocityY": 2.070061451819255,
+            "timestamp": 0.6221679756919738
         },
         {
-            "x": 5.675511663962328,
-            "y": 6.962907583051813,
-            "heading": -0.33387509354517536,
-            "angularVelocity": 2.6774949424459703e-7,
-            "velocityX": 3.5536310152752564,
-            "velocityY": 1.2970527490748707,
-            "timestamp": 0.7258626880956504
+            "x": 5.675511606586513,
+            "y": 6.962908031631864,
+            "heading": -0.3338756959696621,
+            "angularVelocity": 9.713458668099172e-8,
+            "velocityX": 3.5536301441350284,
+            "velocityY": 1.2970558779125847,
+            "timestamp": 0.7258626383073028
         },
         {
-            "x": 6.0558437987294225,
-            "y": 7.05894833386441,
-            "heading": -0.33387506661120775,
-            "angularVelocity": 2.597430292697957e-7,
-            "velocityX": 3.6678079573899045,
-            "velocityY": 0.9261879232999765,
-            "timestamp": 0.8295573578236004
+            "x": 6.055844888125957,
+            "y": 7.058944236891981,
+            "heading": -0.3338756861984713,
+            "angularVelocity": 9.423041207795053e-8,
+            "velocityX": 3.6678192680981745,
+            "velocityY": 0.9261441508940896,
+            "timestamp": 0.8295573009226318
         },
         {
-            "x": 6.43617610221841,
-            "y": 7.15498841651687,
-            "heading": -0.333875039677234,
-            "angularVelocity": 2.5974308807396863e-7,
-            "velocityX": 3.667809584492749,
-            "velocityY": 0.9261814797658935,
-            "timestamp": 0.9332520275515505
+            "x": 6.436176952400414,
+            "y": 7.154985262770466,
+            "heading": -0.3338756764272739,
+            "angularVelocity": 9.423047569354886e-8,
+            "velocityX": 3.667807529162388,
+            "velocityY": 0.9261906394812687,
+            "timestamp": 0.9332519635379608
         },
         {
-            "x": 6.8165084241422855,
-            "y": 7.251028426164244,
-            "heading": -0.33387501274320475,
-            "angularVelocity": 2.5974362337794873e-7,
-            "velocityX": 3.667809762273227,
-            "velocityY": 0.9261807757269309,
-            "timestamp": 1.0369466972795005
+            "x": 6.8165090646020285,
+            "y": 7.2510260988513195,
+            "heading": -0.33387566665551793,
+            "angularVelocity": 9.423586238606553e-8,
+            "velocityX": 3.6678079913574164,
+            "velocityY": 0.9261888091302356,
+            "timestamp": 1.0369466261532898
         },
         {
-            "x": 7.162466794959021,
-            "y": 7.337325744518129,
-            "heading": -0.24283225626293373,
-            "angularVelocity": 0.8779887791641197,
-            "velocityX": 3.3363177849390095,
-            "velocityY": 0.8322252106139182,
-            "timestamp": 1.1406413670074507
+            "x": 7.162467192455402,
+            "y": 7.33732437680055,
+            "heading": -0.24283297058924916,
+            "angularVelocity": 0.8779882567727307,
+            "velocityX": 3.336315670718354,
+            "velocityY": 0.8322345217454595,
+            "timestamp": 1.1406412887686188
         },
         {
-            "x": 7.421550082994963,
-            "y": 7.402199028754473,
-            "heading": -0.13158720138626476,
-            "angularVelocity": 1.0728136284004517,
-            "velocityX": 2.498520789117373,
-            "velocityY": 0.6256183119782293,
-            "timestamp": 1.2443360367354006
+            "x": 7.421550289934561,
+            "y": 7.4021983509049765,
+            "heading": -0.1315877257451376,
+            "angularVelocity": 1.0728155339758783,
+            "velocityX": 2.4985191228237835,
+            "velocityY": 0.6256250077700213,
+            "timestamp": 1.2443359513839478
         },
         {
-            "x": 7.5939307756893495,
-            "y": 7.445412774465034,
-            "heading": -0.046073125428300596,
-            "angularVelocity": 0.8246718580841027,
-            "velocityX": 1.6623872099370305,
-            "velocityY": 0.41674028013136566,
-            "timestamp": 1.3480307064633505
+            "x": 7.593930847137365,
+            "y": 7.445412549605325,
+            "heading": -0.046073342510862675,
+            "angularVelocity": 0.8246748779298502,
+            "velocityX": 1.662386017323544,
+            "velocityY": 0.41674467721309977,
+            "timestamp": 1.3480306139992768
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 6.33230436519987e-31,
-            "angularVelocity": 0.444315272416587,
-            "velocityX": 0.8300255407192906,
-            "velocityY": 0.2081806672570287,
-            "timestamp": 1.4517253761913005
+            "heading": 0,
+            "angularVelocity": 0.44431739637147044,
+            "velocityX": 0.8300249086292989,
+            "velocityY": 0.20818285001567757,
+            "timestamp": 1.4517252766146058
         },
         {
             "x": 7.68,
             "y": 7.467,
             "heading": 0,
             "angularVelocity": 0,
-            "velocityX": 2.6078100454164866e-31,
-            "velocityY": 7.556791844119902e-29,
-            "timestamp": 1.5554200459192504
+            "velocityX": 6.677691966309128e-30,
+            "velocityY": 0,
+            "timestamp": 1.5554199392299348
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.traj
index 60b69798..29c2d4bc 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.traj	
@@ -3,146 +3,146 @@
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": -5.481945073939837e-30,
-            "angularVelocity": 8.103891066426341e-30,
-            "velocityX": -1.0115776003849141e-29,
-            "velocityY": 1.8597851705411875e-30,
+            "heading": 0,
+            "angularVelocity": 5.380258045140596e-28,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 4.304265157789358,
-            "y": 5.638698398929109,
-            "heading": -0.04261689056565391,
-            "angularVelocity": -0.41098438981919094,
-            "velocityX": 0.39794868817859114,
-            "velocityY": 0.7589435323492035,
-            "timestamp": 0.10369466972795005
+            "x": 4.304265282414897,
+            "y": 5.638698322322049,
+            "heading": -0.04261698283734242,
+            "angularVelocity": -0.41098530784979936,
+            "velocityX": 0.3979499173257931,
+            "velocityY": 0.7589428456312429,
+            "timestamp": 0.10369466261532896
         },
         {
-            "x": 4.388659791773407,
-            "y": 5.795293123128996,
-            "heading": -0.12280542510902584,
-            "angularVelocity": -0.7733139490558338,
-            "velocityX": 0.8138762986126182,
-            "velocityY": 1.5101521091752137,
-            "timestamp": 0.2073893394559001
+            "x": 4.38866009880019,
+            "y": 5.795292929312062,
+            "heading": -0.1228057028496872,
+            "angularVelocity": -0.7733157907057984,
+            "velocityX": 0.8138781134605525,
+            "velocityY": 1.510151082422871,
+            "timestamp": 0.20738932523065792
         },
         {
-            "x": 4.519821289569113,
-            "y": 6.028001369445331,
-            "heading": -0.2301897087601478,
-            "angularVelocity": -1.035581519598341,
-            "velocityX": 1.2648817739601788,
-            "velocityY": 2.244167872146774,
-            "timestamp": 0.31108400918385015
+            "x": 4.519821720835651,
+            "y": 6.028001091828693,
+            "heading": -0.23019023538729733,
+            "angularVelocity": -1.0355839908170448,
+            "velocityX": 1.264883058851596,
+            "velocityY": 2.2441672179396264,
+            "timestamp": 0.3110839878459869
         },
         {
-            "x": 4.707676223581395,
-            "y": 6.330157885665521,
-            "heading": -0.33387515355278086,
-            "angularVelocity": -0.9999110375168158,
-            "velocityX": 1.811616108187066,
-            "velocityY": 2.9139059607683615,
-            "timestamp": 0.4147786789118002
+            "x": 4.707676475524912,
+            "y": 6.330157785456564,
+            "heading": -0.33387571773962255,
+            "angularVelocity": -0.9999114683169592,
+            "velocityX": 1.8116145031122473,
+            "velocityY": 2.9139078715050806,
+            "timestamp": 0.41477865046131585
         },
         {
-            "x": 4.978690084088412,
-            "y": 6.613755913169546,
-            "heading": -0.33387515567200693,
-            "angularVelocity": -2.0437175255333274e-8,
-            "velocityX": 2.613575617898612,
-            "velocityY": 2.734933514403967,
-            "timestamp": 0.5184733486397503
+            "x": 4.978690211282069,
+            "y": 6.613755936139097,
+            "heading": -0.3338757185080882,
+            "angularVelocity": -7.410850442524386e-9,
+            "velocityX": 2.6135745941189206,
+            "velocityY": 2.7349348898948014,
+            "timestamp": 0.5184733130766448
         },
         {
-            "x": 5.307019069498364,
-            "y": 6.828410126616754,
-            "heading": -0.33387512130937075,
-            "angularVelocity": 3.313828600417541e-7,
-            "velocityX": 3.1663053295925763,
-            "velocityY": 2.0700602452408625,
-            "timestamp": 0.6221680183677003
+            "x": 5.307019127730767,
+            "y": 6.828410259978494,
+            "heading": -0.3338757060420003,
+            "angularVelocity": 1.2021918642580205e-7,
+            "velocityX": 3.166304881734213,
+            "velocityY": 2.070061451819255,
+            "timestamp": 0.6221679756919738
         },
         {
-            "x": 5.675511663962328,
-            "y": 6.962907583051813,
-            "heading": -0.33387509354517536,
-            "angularVelocity": 2.6774949424459703e-7,
-            "velocityX": 3.5536310152752564,
-            "velocityY": 1.2970527490748707,
-            "timestamp": 0.7258626880956504
+            "x": 5.675511606586513,
+            "y": 6.962908031631864,
+            "heading": -0.3338756959696621,
+            "angularVelocity": 9.713458668099172e-8,
+            "velocityX": 3.5536301441350284,
+            "velocityY": 1.2970558779125847,
+            "timestamp": 0.7258626383073028
         },
         {
-            "x": 6.0558437987294225,
-            "y": 7.05894833386441,
-            "heading": -0.33387506661120775,
-            "angularVelocity": 2.597430292697957e-7,
-            "velocityX": 3.6678079573899045,
-            "velocityY": 0.9261879232999765,
-            "timestamp": 0.8295573578236004
+            "x": 6.055844888125957,
+            "y": 7.058944236891981,
+            "heading": -0.3338756861984713,
+            "angularVelocity": 9.423041207795053e-8,
+            "velocityX": 3.6678192680981745,
+            "velocityY": 0.9261441508940896,
+            "timestamp": 0.8295573009226318
         },
         {
-            "x": 6.43617610221841,
-            "y": 7.15498841651687,
-            "heading": -0.333875039677234,
-            "angularVelocity": 2.5974308807396863e-7,
-            "velocityX": 3.667809584492749,
-            "velocityY": 0.9261814797658935,
-            "timestamp": 0.9332520275515505
+            "x": 6.436176952400414,
+            "y": 7.154985262770466,
+            "heading": -0.3338756764272739,
+            "angularVelocity": 9.423047569354886e-8,
+            "velocityX": 3.667807529162388,
+            "velocityY": 0.9261906394812687,
+            "timestamp": 0.9332519635379608
         },
         {
-            "x": 6.8165084241422855,
-            "y": 7.251028426164244,
-            "heading": -0.33387501274320475,
-            "angularVelocity": 2.5974362337794873e-7,
-            "velocityX": 3.667809762273227,
-            "velocityY": 0.9261807757269309,
-            "timestamp": 1.0369466972795005
+            "x": 6.8165090646020285,
+            "y": 7.2510260988513195,
+            "heading": -0.33387566665551793,
+            "angularVelocity": 9.423586238606553e-8,
+            "velocityX": 3.6678079913574164,
+            "velocityY": 0.9261888091302356,
+            "timestamp": 1.0369466261532898
         },
         {
-            "x": 7.162466794959021,
-            "y": 7.337325744518129,
-            "heading": -0.24283225626293373,
-            "angularVelocity": 0.8779887791641197,
-            "velocityX": 3.3363177849390095,
-            "velocityY": 0.8322252106139182,
-            "timestamp": 1.1406413670074507
+            "x": 7.162467192455402,
+            "y": 7.33732437680055,
+            "heading": -0.24283297058924916,
+            "angularVelocity": 0.8779882567727307,
+            "velocityX": 3.336315670718354,
+            "velocityY": 0.8322345217454595,
+            "timestamp": 1.1406412887686188
         },
         {
-            "x": 7.421550082994963,
-            "y": 7.402199028754473,
-            "heading": -0.13158720138626476,
-            "angularVelocity": 1.0728136284004517,
-            "velocityX": 2.498520789117373,
-            "velocityY": 0.6256183119782293,
-            "timestamp": 1.2443360367354006
+            "x": 7.421550289934561,
+            "y": 7.4021983509049765,
+            "heading": -0.1315877257451376,
+            "angularVelocity": 1.0728155339758783,
+            "velocityX": 2.4985191228237835,
+            "velocityY": 0.6256250077700213,
+            "timestamp": 1.2443359513839478
         },
         {
-            "x": 7.5939307756893495,
-            "y": 7.445412774465034,
-            "heading": -0.046073125428300596,
-            "angularVelocity": 0.8246718580841027,
-            "velocityX": 1.6623872099370305,
-            "velocityY": 0.41674028013136566,
-            "timestamp": 1.3480307064633505
+            "x": 7.593930847137365,
+            "y": 7.445412549605325,
+            "heading": -0.046073342510862675,
+            "angularVelocity": 0.8246748779298502,
+            "velocityX": 1.662386017323544,
+            "velocityY": 0.41674467721309977,
+            "timestamp": 1.3480306139992768
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 6.33230436519987e-31,
-            "angularVelocity": 0.444315272416587,
-            "velocityX": 0.8300255407192906,
-            "velocityY": 0.2081806672570287,
-            "timestamp": 1.4517253761913005
+            "heading": 0,
+            "angularVelocity": 0.44431739637147044,
+            "velocityX": 0.8300249086292989,
+            "velocityY": 0.20818285001567757,
+            "timestamp": 1.4517252766146058
         },
         {
             "x": 7.68,
             "y": 7.467,
             "heading": 0,
             "angularVelocity": 0,
-            "velocityX": 2.6078100454164866e-31,
-            "velocityY": 7.556791844119902e-29,
-            "timestamp": 1.5554200459192504
+            "velocityX": 6.677691966309128e-30,
+            "velocityY": 0,
+            "timestamp": 1.5554199392299348
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2.1.traj
new file mode 100644
index 00000000..05a18e04
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2.1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.272659932573731,
+            "y": 5.605321030989923,
+            "heading": -5.269773313916661e-8,
+            "angularVelocity": -7.069117507058631e-7,
+            "velocityX": 0.1295790656325301,
+            "velocityY": 0.6079397351586845,
+            "timestamp": 0.07454855863477708
+        },
+        {
+            "x": 4.294668340488188,
+            "y": 5.695305527660525,
+            "heading": -1.390931476532318e-7,
+            "angularVelocity": -0.0000011589489187991593,
+            "velocityX": 0.2952224471421504,
+            "velocityY": 1.2070588395619357,
+            "timestamp": 0.14909711726955416
+        },
+        {
+            "x": 4.333068049168991,
+            "y": 5.8286332482919585,
+            "heading": -2.3393980900082764e-7,
+            "angularVelocity": -0.0000012723197345504802,
+            "velocityX": 0.5150965944736963,
+            "velocityY": 1.7884681217524945,
+            "timestamp": 0.22364567590433124
+        },
+        {
+            "x": 4.394361213548298,
+            "y": 6.0022499148295365,
+            "heading": -3.0377830712505715e-7,
+            "angularVelocity": -9.368513338342214e-7,
+            "velocityX": 0.8221911395208825,
+            "velocityY": 2.3289070872699873,
+            "timestamp": 0.2981942345391083
+        },
+        {
+            "x": 4.489214393007107,
+            "y": 6.207820209268236,
+            "heading": -3.104589480689307e-7,
+            "angularVelocity": -8.962358439949817e-8,
+            "velocityX": 1.2723677183517785,
+            "velocityY": 2.75753546703125,
+            "timestamp": 0.37274279317388537
+        },
+        {
+            "x": 4.629403012529928,
+            "y": 6.42298249437822,
+            "heading": -2.4199647648725644e-7,
+            "angularVelocity": 9.18393943226279e-7,
+            "velocityX": 1.880500737963715,
+            "velocityY": 2.8862031546566858,
+            "timestamp": 0.4472913518086624
+        },
+        {
+            "x": 4.811531801184116,
+            "y": 6.618438666310206,
+            "heading": -1.472447710538824e-7,
+            "angularVelocity": 0.0000012710752897138218,
+            "velocityX": 2.443089336474201,
+            "velocityY": 2.6218638627509074,
+            "timestamp": 0.5218399104434395
+        },
+        {
+            "x": 5.024847227590037,
+            "y": 6.779620899070013,
+            "heading": -6.254572131926062e-8,
+            "angularVelocity": 0.000001136235548213432,
+            "velocityX": 2.861429252247402,
+            "velocityY": 2.1621106520395927,
+            "timestamp": 0.5963884690782165
+        },
+        {
+            "x": 5.261847496032715,
+            "y": 6.900974273681641,
+            "heading": 1.6289151195668303e-9,
+            "angularVelocity": 8.609140934997053e-7,
+            "velocityX": 3.1791395133448552,
+            "velocityY": 1.6278433384167708,
+            "timestamp": 0.6709370277129936
+        },
+        {
+            "x": 5.5631639462020495,
+            "y": 6.984947323321856,
+            "heading": 4.965855378058324e-8,
+            "angularVelocity": 5.539886921154719e-7,
+            "velocityX": 3.4757171947403775,
+            "velocityY": 0.9686380297960585,
+            "timestamp": 0.7576289054784
+        },
+        {
+            "x": 5.883696640724643,
+            "y": 7.009274337947639,
+            "heading": 7.188189606013307e-8,
+            "angularVelocity": 2.563216327096045e-7,
+            "velocityX": 3.697378610459799,
+            "velocityY": 0.2806146929817695,
+            "timestamp": 0.8443207832438064
+        },
+        {
+            "x": 6.209423466668327,
+            "y": 6.971151903870608,
+            "heading": 7.193737396357795e-8,
+            "angularVelocity": 6.438452889791163e-10,
+            "velocityX": 3.7572934667332714,
+            "velocityY": -0.4397463179458525,
+            "timestamp": 0.9310126610092129
+        },
+        {
+            "x": 6.521952745942683,
+            "y": 6.871769894938554,
+            "heading": 7.193784305655878e-8,
+            "angularVelocity": 5.630908705768223e-12,
+            "velocityX": 3.6050583669434406,
+            "velocityY": -1.1463820079014435,
+            "timestamp": 1.0177045387746193
+        },
+        {
+            "x": 6.809873374118548,
+            "y": 6.714756988695251,
+            "heading": 7.193831945924844e-8,
+            "angularVelocity": 5.91084758847972e-12,
+            "velocityX": 3.321194967875227,
+            "velocityY": -1.8111605177424126,
+            "timestamp": 1.1043964165400257
+        },
+        {
+            "x": 7.062672913559571,
+            "y": 6.505846154651799,
+            "heading": 7.193893641048392e-8,
+            "angularVelocity": 7.48396872358976e-12,
+            "velocityX": 2.9160694861627596,
+            "velocityY": -2.4098086166543093,
+            "timestamp": 1.1910882943054322
+        },
+        {
+            "x": 7.268448732261103,
+            "y": 6.269564240975314,
+            "heading": 6.958123303548702e-8,
+            "angularVelocity": -2.7156604096657526e-8,
+            "velocityX": 2.3736458816624144,
+            "velocityY": -2.7255369226670942,
+            "timestamp": 1.2777801720708386
+        },
+        {
+            "x": 7.433069281193563,
+            "y": 6.080538590855167,
+            "heading": 5.4913746267249155e-8,
+            "angularVelocity": -1.6915879682512025e-7,
+            "velocityX": 1.8989154825033356,
+            "velocityY": -2.180430912690726,
+            "timestamp": 1.364472049836245
+        },
+        {
+            "x": 7.556534655605292,
+            "y": 5.9387693118426395,
+            "heading": 3.4027865091828515e-8,
+            "angularVelocity": -2.408962130800618e-7,
+            "velocityX": 1.424186181835385,
+            "velocityY": -1.635323662267605,
+            "timestamp": 1.4511639276016515
+        },
+        {
+            "x": 7.638844889158882,
+            "y": 5.8442564415645775,
+            "heading": 1.3493455787871544e-8,
+            "angularVelocity": -2.368497714278514e-7,
+            "velocityX": 0.9494572693998051,
+            "velocityY": -1.0902159778725726,
+            "timestamp": 1.537855805367058
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -2.563219583366834e-29,
+            "angularVelocity": -1.5563981959470815e-7,
+            "velocityX": 0.4747285662416774,
+            "velocityY": -0.5451080630467725,
+            "timestamp": 1.6245476831324643
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 4.328374921762355e-28,
+            "velocityX": 0,
+            "velocityY": 6.636862452110489e-29,
+            "timestamp": 1.7112395608978708
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2.traj
new file mode 100644
index 00000000..05a18e04
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.272659932573731,
+            "y": 5.605321030989923,
+            "heading": -5.269773313916661e-8,
+            "angularVelocity": -7.069117507058631e-7,
+            "velocityX": 0.1295790656325301,
+            "velocityY": 0.6079397351586845,
+            "timestamp": 0.07454855863477708
+        },
+        {
+            "x": 4.294668340488188,
+            "y": 5.695305527660525,
+            "heading": -1.390931476532318e-7,
+            "angularVelocity": -0.0000011589489187991593,
+            "velocityX": 0.2952224471421504,
+            "velocityY": 1.2070588395619357,
+            "timestamp": 0.14909711726955416
+        },
+        {
+            "x": 4.333068049168991,
+            "y": 5.8286332482919585,
+            "heading": -2.3393980900082764e-7,
+            "angularVelocity": -0.0000012723197345504802,
+            "velocityX": 0.5150965944736963,
+            "velocityY": 1.7884681217524945,
+            "timestamp": 0.22364567590433124
+        },
+        {
+            "x": 4.394361213548298,
+            "y": 6.0022499148295365,
+            "heading": -3.0377830712505715e-7,
+            "angularVelocity": -9.368513338342214e-7,
+            "velocityX": 0.8221911395208825,
+            "velocityY": 2.3289070872699873,
+            "timestamp": 0.2981942345391083
+        },
+        {
+            "x": 4.489214393007107,
+            "y": 6.207820209268236,
+            "heading": -3.104589480689307e-7,
+            "angularVelocity": -8.962358439949817e-8,
+            "velocityX": 1.2723677183517785,
+            "velocityY": 2.75753546703125,
+            "timestamp": 0.37274279317388537
+        },
+        {
+            "x": 4.629403012529928,
+            "y": 6.42298249437822,
+            "heading": -2.4199647648725644e-7,
+            "angularVelocity": 9.18393943226279e-7,
+            "velocityX": 1.880500737963715,
+            "velocityY": 2.8862031546566858,
+            "timestamp": 0.4472913518086624
+        },
+        {
+            "x": 4.811531801184116,
+            "y": 6.618438666310206,
+            "heading": -1.472447710538824e-7,
+            "angularVelocity": 0.0000012710752897138218,
+            "velocityX": 2.443089336474201,
+            "velocityY": 2.6218638627509074,
+            "timestamp": 0.5218399104434395
+        },
+        {
+            "x": 5.024847227590037,
+            "y": 6.779620899070013,
+            "heading": -6.254572131926062e-8,
+            "angularVelocity": 0.000001136235548213432,
+            "velocityX": 2.861429252247402,
+            "velocityY": 2.1621106520395927,
+            "timestamp": 0.5963884690782165
+        },
+        {
+            "x": 5.261847496032715,
+            "y": 6.900974273681641,
+            "heading": 1.6289151195668303e-9,
+            "angularVelocity": 8.609140934997053e-7,
+            "velocityX": 3.1791395133448552,
+            "velocityY": 1.6278433384167708,
+            "timestamp": 0.6709370277129936
+        },
+        {
+            "x": 5.5631639462020495,
+            "y": 6.984947323321856,
+            "heading": 4.965855378058324e-8,
+            "angularVelocity": 5.539886921154719e-7,
+            "velocityX": 3.4757171947403775,
+            "velocityY": 0.9686380297960585,
+            "timestamp": 0.7576289054784
+        },
+        {
+            "x": 5.883696640724643,
+            "y": 7.009274337947639,
+            "heading": 7.188189606013307e-8,
+            "angularVelocity": 2.563216327096045e-7,
+            "velocityX": 3.697378610459799,
+            "velocityY": 0.2806146929817695,
+            "timestamp": 0.8443207832438064
+        },
+        {
+            "x": 6.209423466668327,
+            "y": 6.971151903870608,
+            "heading": 7.193737396357795e-8,
+            "angularVelocity": 6.438452889791163e-10,
+            "velocityX": 3.7572934667332714,
+            "velocityY": -0.4397463179458525,
+            "timestamp": 0.9310126610092129
+        },
+        {
+            "x": 6.521952745942683,
+            "y": 6.871769894938554,
+            "heading": 7.193784305655878e-8,
+            "angularVelocity": 5.630908705768223e-12,
+            "velocityX": 3.6050583669434406,
+            "velocityY": -1.1463820079014435,
+            "timestamp": 1.0177045387746193
+        },
+        {
+            "x": 6.809873374118548,
+            "y": 6.714756988695251,
+            "heading": 7.193831945924844e-8,
+            "angularVelocity": 5.91084758847972e-12,
+            "velocityX": 3.321194967875227,
+            "velocityY": -1.8111605177424126,
+            "timestamp": 1.1043964165400257
+        },
+        {
+            "x": 7.062672913559571,
+            "y": 6.505846154651799,
+            "heading": 7.193893641048392e-8,
+            "angularVelocity": 7.48396872358976e-12,
+            "velocityX": 2.9160694861627596,
+            "velocityY": -2.4098086166543093,
+            "timestamp": 1.1910882943054322
+        },
+        {
+            "x": 7.268448732261103,
+            "y": 6.269564240975314,
+            "heading": 6.958123303548702e-8,
+            "angularVelocity": -2.7156604096657526e-8,
+            "velocityX": 2.3736458816624144,
+            "velocityY": -2.7255369226670942,
+            "timestamp": 1.2777801720708386
+        },
+        {
+            "x": 7.433069281193563,
+            "y": 6.080538590855167,
+            "heading": 5.4913746267249155e-8,
+            "angularVelocity": -1.6915879682512025e-7,
+            "velocityX": 1.8989154825033356,
+            "velocityY": -2.180430912690726,
+            "timestamp": 1.364472049836245
+        },
+        {
+            "x": 7.556534655605292,
+            "y": 5.9387693118426395,
+            "heading": 3.4027865091828515e-8,
+            "angularVelocity": -2.408962130800618e-7,
+            "velocityX": 1.424186181835385,
+            "velocityY": -1.635323662267605,
+            "timestamp": 1.4511639276016515
+        },
+        {
+            "x": 7.638844889158882,
+            "y": 5.8442564415645775,
+            "heading": 1.3493455787871544e-8,
+            "angularVelocity": -2.368497714278514e-7,
+            "velocityX": 0.9494572693998051,
+            "velocityY": -1.0902159778725726,
+            "timestamp": 1.537855805367058
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -2.563219583366834e-29,
+            "angularVelocity": -1.5563981959470815e-7,
+            "velocityX": 0.4747285662416774,
+            "velocityY": -0.5451080630467725,
+            "timestamp": 1.6245476831324643
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 4.328374921762355e-28,
+            "velocityX": 0,
+            "velocityY": 6.636862452110489e-29,
+            "timestamp": 1.7112395608978708
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj
index c9864950..c913a793 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj	
@@ -3,128 +3,128 @@
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": -7.967068308454128e-24,
-            "angularVelocity": 3.748757739861167e-23,
-            "velocityX": 2.372714537320914e-25,
-            "velocityY": 2.748536601620217e-25,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 8.330960047205791e-35,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 4.215224464845852,
-            "y": 5.589131496345612,
-            "heading": 0.013109636548148297,
-            "angularVelocity": 0.15985714241706056,
-            "velocityX": -0.5825684410958524,
-            "velocityY": 0.3552255429080149,
-            "timestamp": 0.08200845048228003
+            "x": 4.215224456008129,
+            "y": 5.5891314798928855,
+            "heading": 0.013109691961439362,
+            "angularVelocity": 0.15985781811927008,
+            "velocityX": -0.5825685488618567,
+            "velocityY": 0.35522534228568337,
+            "timestamp": 0.08200845048227925
         },
         {
-            "x": 4.119673396532112,
-            "y": 5.647394589751744,
-            "heading": 0.03932466572409392,
-            "angularVelocity": 0.31966253504094716,
-            "velocityX": -1.1651368578703514,
-            "velocityY": 0.7104523139200205,
-            "timestamp": 0.16401690096456006
+            "x": 4.11967337131145,
+            "y": 5.647394543004508,
+            "heading": 0.03932481805987993,
+            "angularVelocity": 0.31966371690079975,
+            "velocityX": -1.1651370576417237,
+            "velocityY": 0.7104519445128596,
+            "timestamp": 0.1640169009645585
         },
         {
-            "x": 3.9763468054990545,
-            "y": 5.734789450682316,
-            "heading": 0.07863876132846631,
-            "angularVelocity": 0.47939078684174374,
-            "velocityX": -1.7477051473375471,
-            "velocityY": 1.06568116354614,
-            "timestamp": 0.2460253514468401
+            "x": 3.976346757700282,
+            "y": 5.734789362523958,
+            "heading": 0.07863903763442207,
+            "angularVelocity": 0.47939229851730114,
+            "velocityX": -1.7477054226519961,
+            "velocityY": 1.0656806585844962,
+            "timestamp": 0.24602535144683776
         },
         {
-            "x": 3.785244738141483,
-            "y": 5.8513163218996445,
-            "heading": 0.131044995583191,
-            "angularVelocity": 0.6390345622497571,
-            "velocityX": -2.330272871072761,
-            "velocityY": 1.4209129733832384,
-            "timestamp": 0.32803380192912013
+            "x": 3.7852446629452063,
+            "y": 5.851316183985278,
+            "heading": 0.13104540832787342,
+            "angularVelocity": 0.6390362259652201,
+            "velocityX": -2.3302732051542674,
+            "velocityY": 1.4209123666652002,
+            "timestamp": 0.328033801929117
         },
         {
-            "x": 3.5463673151125503,
-            "y": 5.996975506896697,
-            "heading": 0.19653801558690542,
-            "angularVelocity": 0.7986130650995023,
-            "velocityX": -2.9128391235796927,
-            "velocityY": 1.776148483972697,
-            "timestamp": 0.41004225241140013
+            "x": 3.5463672091086966,
+            "y": 5.996975313640393,
+            "heading": 0.19653856458379848,
+            "angularVelocity": 0.7986147265406159,
+            "velocityX": -2.912839499243156,
+            "velocityY": 1.77614780914058,
+            "timestamp": 0.41004225241139625
         },
         {
-            "x": 3.2814945553511796,
-            "y": 6.158490181857634,
-            "heading": 0.19653800981726008,
-            "angularVelocity": -7.035427786522906e-8,
-            "velocityX": -3.2298227585534365,
-            "velocityY": 1.9694881931202608,
-            "timestamp": 0.49205070289368014
+            "x": 3.281494446824011,
+            "y": 6.15848998396632,
+            "heading": 0.19653857286419524,
+            "angularVelocity": 1.009700426817188e-7,
+            "velocityX": -3.229822789322431,
+            "velocityY": 1.9694881366015824,
+            "timestamp": 0.4920507028936755
         },
         {
-            "x": 3.016621781154659,
-            "y": 6.320004832717382,
-            "heading": 0.19653801616374372,
-            "angularVelocity": 7.738816679079332e-8,
-            "velocityX": -3.229822934573717,
-            "velocityY": 1.9694878992335856,
-            "timestamp": 0.5740591533759601
+            "x": 3.016621684855716,
+            "y": 6.320004654811106,
+            "heading": 0.1965385811445921,
+            "angularVelocity": 1.0097004416983622e-7,
+            "velocityX": -3.229822785464406,
+            "velocityY": 1.969488142928474,
+            "timestamp": 0.5740591533759547
         },
         {
-            "x": 2.7777476228533557,
-            "y": 6.465669683581835,
-            "heading": 0.2620170313059908,
-            "angularVelocity": 0.7984422917049926,
-            "velocityX": -2.9127993139306705,
-            "velocityY": 1.7762175727966987,
-            "timestamp": 0.6560676038582401
+            "x": 2.77774755563319,
+            "y": 6.465669559960414,
+            "heading": 0.2620174097526663,
+            "angularVelocity": 0.7984400171324189,
+            "velocityX": -2.9127989593480157,
+            "velocityY": 1.7762182347389206,
+            "timestamp": 0.6560676038582339
         },
         {
-            "x": 2.5866484818505064,
-            "y": 6.582201734022544,
-            "heading": 0.31440674809174973,
-            "angularVelocity": 0.6388331504578179,
-            "velocityX": -2.330237187497417,
-            "velocityY": 1.4209761281355728,
-            "timestamp": 0.7380760543405201
+            "x": 2.5866484397575076,
+            "y": 6.582201656937504,
+            "heading": 0.3144069758542471,
+            "angularVelocity": 0.6388313130352502,
+            "velocityX": -2.330236881100143,
+            "velocityY": 1.4209766955939638,
+            "timestamp": 0.7380760543405132
         },
         {
-            "x": 2.443324221748551,
-            "y": 6.669600847656536,
-            "heading": 0.3537026369190334,
-            "angularVelocity": 0.47916877585407597,
-            "velocityX": -1.7476767242776265,
-            "velocityY": 1.0657330204388804,
-            "timestamp": 0.8200845048228002
+            "x": 2.443324199843316,
+            "y": 6.6696008076986475,
+            "heading": 0.35370275100271265,
+            "angularVelocity": 0.4791673896698825,
+            "velocityX": -1.7476764781107736,
+            "velocityY": 1.0657334731623573,
+            "timestamp": 0.8200845048227924
         },
         {
-            "x": 2.347774743561573,
-            "y": 6.727866948074407,
-            "heading": 0.37990101547487837,
-            "angularVelocity": 0.31945949962200243,
-            "velocityX": -1.1651174680787846,
-            "velocityY": 0.7104889810161812,
-            "timestamp": 0.9020929553050802
+            "x": 2.347774735979553,
+            "y": 6.727866934294423,
+            "heading": 0.37990105353306003,
+            "angularVelocity": 0.3194585725773273,
+            "velocityX": -1.1651172934234333,
+            "velocityY": 0.7104893002260324,
+            "timestamp": 0.9020929553050716
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.393,
-            "angularVelocity": 0.15972725308292482,
-            "velocityX": -0.5825587885226076,
-            "velocityY": 0.355244511440778,
-            "timestamp": 0.9841014057873602
+            "angularVelocity": 0.15972678900658524,
+            "velocityX": -0.5825586960684825,
+            "velocityY": 0.3552446794720561,
+            "timestamp": 0.9841014057873508
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.393,
-            "angularVelocity": 1.2555409321863598e-23,
-            "velocityX": -4.0981987257829375e-24,
-            "velocityY": -7.824281389954736e-24,
-            "timestamp": 1.0661098562696403
+            "angularVelocity": -4.59054414969708e-34,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 1.06610985626963
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.traj
index c9864950..c913a793 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.traj	
@@ -3,128 +3,128 @@
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": -7.967068308454128e-24,
-            "angularVelocity": 3.748757739861167e-23,
-            "velocityX": 2.372714537320914e-25,
-            "velocityY": 2.748536601620217e-25,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 8.330960047205791e-35,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 4.215224464845852,
-            "y": 5.589131496345612,
-            "heading": 0.013109636548148297,
-            "angularVelocity": 0.15985714241706056,
-            "velocityX": -0.5825684410958524,
-            "velocityY": 0.3552255429080149,
-            "timestamp": 0.08200845048228003
+            "x": 4.215224456008129,
+            "y": 5.5891314798928855,
+            "heading": 0.013109691961439362,
+            "angularVelocity": 0.15985781811927008,
+            "velocityX": -0.5825685488618567,
+            "velocityY": 0.35522534228568337,
+            "timestamp": 0.08200845048227925
         },
         {
-            "x": 4.119673396532112,
-            "y": 5.647394589751744,
-            "heading": 0.03932466572409392,
-            "angularVelocity": 0.31966253504094716,
-            "velocityX": -1.1651368578703514,
-            "velocityY": 0.7104523139200205,
-            "timestamp": 0.16401690096456006
+            "x": 4.11967337131145,
+            "y": 5.647394543004508,
+            "heading": 0.03932481805987993,
+            "angularVelocity": 0.31966371690079975,
+            "velocityX": -1.1651370576417237,
+            "velocityY": 0.7104519445128596,
+            "timestamp": 0.1640169009645585
         },
         {
-            "x": 3.9763468054990545,
-            "y": 5.734789450682316,
-            "heading": 0.07863876132846631,
-            "angularVelocity": 0.47939078684174374,
-            "velocityX": -1.7477051473375471,
-            "velocityY": 1.06568116354614,
-            "timestamp": 0.2460253514468401
+            "x": 3.976346757700282,
+            "y": 5.734789362523958,
+            "heading": 0.07863903763442207,
+            "angularVelocity": 0.47939229851730114,
+            "velocityX": -1.7477054226519961,
+            "velocityY": 1.0656806585844962,
+            "timestamp": 0.24602535144683776
         },
         {
-            "x": 3.785244738141483,
-            "y": 5.8513163218996445,
-            "heading": 0.131044995583191,
-            "angularVelocity": 0.6390345622497571,
-            "velocityX": -2.330272871072761,
-            "velocityY": 1.4209129733832384,
-            "timestamp": 0.32803380192912013
+            "x": 3.7852446629452063,
+            "y": 5.851316183985278,
+            "heading": 0.13104540832787342,
+            "angularVelocity": 0.6390362259652201,
+            "velocityX": -2.3302732051542674,
+            "velocityY": 1.4209123666652002,
+            "timestamp": 0.328033801929117
         },
         {
-            "x": 3.5463673151125503,
-            "y": 5.996975506896697,
-            "heading": 0.19653801558690542,
-            "angularVelocity": 0.7986130650995023,
-            "velocityX": -2.9128391235796927,
-            "velocityY": 1.776148483972697,
-            "timestamp": 0.41004225241140013
+            "x": 3.5463672091086966,
+            "y": 5.996975313640393,
+            "heading": 0.19653856458379848,
+            "angularVelocity": 0.7986147265406159,
+            "velocityX": -2.912839499243156,
+            "velocityY": 1.77614780914058,
+            "timestamp": 0.41004225241139625
         },
         {
-            "x": 3.2814945553511796,
-            "y": 6.158490181857634,
-            "heading": 0.19653800981726008,
-            "angularVelocity": -7.035427786522906e-8,
-            "velocityX": -3.2298227585534365,
-            "velocityY": 1.9694881931202608,
-            "timestamp": 0.49205070289368014
+            "x": 3.281494446824011,
+            "y": 6.15848998396632,
+            "heading": 0.19653857286419524,
+            "angularVelocity": 1.009700426817188e-7,
+            "velocityX": -3.229822789322431,
+            "velocityY": 1.9694881366015824,
+            "timestamp": 0.4920507028936755
         },
         {
-            "x": 3.016621781154659,
-            "y": 6.320004832717382,
-            "heading": 0.19653801616374372,
-            "angularVelocity": 7.738816679079332e-8,
-            "velocityX": -3.229822934573717,
-            "velocityY": 1.9694878992335856,
-            "timestamp": 0.5740591533759601
+            "x": 3.016621684855716,
+            "y": 6.320004654811106,
+            "heading": 0.1965385811445921,
+            "angularVelocity": 1.0097004416983622e-7,
+            "velocityX": -3.229822785464406,
+            "velocityY": 1.969488142928474,
+            "timestamp": 0.5740591533759547
         },
         {
-            "x": 2.7777476228533557,
-            "y": 6.465669683581835,
-            "heading": 0.2620170313059908,
-            "angularVelocity": 0.7984422917049926,
-            "velocityX": -2.9127993139306705,
-            "velocityY": 1.7762175727966987,
-            "timestamp": 0.6560676038582401
+            "x": 2.77774755563319,
+            "y": 6.465669559960414,
+            "heading": 0.2620174097526663,
+            "angularVelocity": 0.7984400171324189,
+            "velocityX": -2.9127989593480157,
+            "velocityY": 1.7762182347389206,
+            "timestamp": 0.6560676038582339
         },
         {
-            "x": 2.5866484818505064,
-            "y": 6.582201734022544,
-            "heading": 0.31440674809174973,
-            "angularVelocity": 0.6388331504578179,
-            "velocityX": -2.330237187497417,
-            "velocityY": 1.4209761281355728,
-            "timestamp": 0.7380760543405201
+            "x": 2.5866484397575076,
+            "y": 6.582201656937504,
+            "heading": 0.3144069758542471,
+            "angularVelocity": 0.6388313130352502,
+            "velocityX": -2.330236881100143,
+            "velocityY": 1.4209766955939638,
+            "timestamp": 0.7380760543405132
         },
         {
-            "x": 2.443324221748551,
-            "y": 6.669600847656536,
-            "heading": 0.3537026369190334,
-            "angularVelocity": 0.47916877585407597,
-            "velocityX": -1.7476767242776265,
-            "velocityY": 1.0657330204388804,
-            "timestamp": 0.8200845048228002
+            "x": 2.443324199843316,
+            "y": 6.6696008076986475,
+            "heading": 0.35370275100271265,
+            "angularVelocity": 0.4791673896698825,
+            "velocityX": -1.7476764781107736,
+            "velocityY": 1.0657334731623573,
+            "timestamp": 0.8200845048227924
         },
         {
-            "x": 2.347774743561573,
-            "y": 6.727866948074407,
-            "heading": 0.37990101547487837,
-            "angularVelocity": 0.31945949962200243,
-            "velocityX": -1.1651174680787846,
-            "velocityY": 0.7104889810161812,
-            "timestamp": 0.9020929553050802
+            "x": 2.347774735979553,
+            "y": 6.727866934294423,
+            "heading": 0.37990105353306003,
+            "angularVelocity": 0.3194585725773273,
+            "velocityX": -1.1651172934234333,
+            "velocityY": 0.7104893002260324,
+            "timestamp": 0.9020929553050716
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.393,
-            "angularVelocity": 0.15972725308292482,
-            "velocityX": -0.5825587885226076,
-            "velocityY": 0.355244511440778,
-            "timestamp": 0.9841014057873602
+            "angularVelocity": 0.15972678900658524,
+            "velocityX": -0.5825586960684825,
+            "velocityY": 0.3552446794720561,
+            "timestamp": 0.9841014057873508
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.393,
-            "angularVelocity": 1.2555409321863598e-23,
-            "velocityX": -4.0981987257829375e-24,
-            "velocityY": -7.824281389954736e-24,
-            "timestamp": 1.0661098562696403
+            "angularVelocity": -4.59054414969708e-34,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 1.06610985626963
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj
index 5e114a48..a4697851 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj	
@@ -3,101 +3,101 @@
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": -1.042790690974161e-34,
-            "angularVelocity": 1.2672485017804847e-33,
+            "heading": 0,
+            "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": 1.0649309840552648e-32,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 4.181096344253041,
-            "y": 5.560000047617906,
-            "heading": -1.1491589479255403e-8,
-            "angularVelocity": -1.1594790933974305e-7,
-            "velocityX": -0.8263919813282988,
-            "velocityY": 4.80455414778758e-7,
-            "timestamp": 0.09910993523353247
+            "x": 4.181096344253034,
+            "y": 5.56,
+            "heading": 2.5710705899848466e-19,
+            "angularVelocity": 2.5941602968880693e-18,
+            "velocityX": -0.8263919813283321,
+            "velocityY": -1.4479549544654444e-34,
+            "timestamp": 0.099109935233537
         },
         {
-            "x": 4.017289035836251,
-            "y": 5.560000133094268,
-            "heading": -2.0027929206394758e-8,
-            "angularVelocity": -8.613001771881656e-8,
-            "velocityX": -1.6527839316089912,
-            "velocityY": 8.624398858779209e-7,
-            "timestamp": 0.19821987046706493
+            "x": 4.017289035836238,
+            "y": 5.56,
+            "heading": 7.7182898862159e-19,
+            "angularVelocity": 5.193444314378615e-18,
+            "velocityX": -1.6527839316089838,
+            "velocityY": -7.36495654370163e-34,
+            "timestamp": 0.198219870467074
         },
         {
-            "x": 3.771578080903882,
-            "y": 5.560000246435489,
-            "heading": -1.061567052726606e-8,
-            "angularVelocity": 9.49678480632688e-8,
-            "velocityX": -2.4791758197944636,
-            "velocityY": 0.0000011435908848510677,
-            "timestamp": 0.2973298057005974
+            "x": 3.771578080903875,
+            "y": 5.56,
+            "heading": 1.545129672215716e-18,
+            "angularVelocity": 7.802453727507411e-18,
+            "velocityX": -2.479175819794286,
+            "velocityY": -8.54847043233614e-34,
+            "timestamp": 0.297329805700611
         },
         {
-            "x": 3.443963497918667,
-            "y": 5.560000377513817,
-            "heading": 3.109895895304223e-8,
-            "angularVelocity": 4.208924841656569e-7,
-            "velocityX": -3.30556752169455,
-            "velocityY": 0.0000013225548252865442,
-            "timestamp": 0.39643974093412987
+            "x": 3.4439634979187304,
+            "y": 5.56,
+            "heading": 2.5806642382479214e-18,
+            "angularVelocity": 1.0448342676399075e-17,
+            "velocityX": -3.3055675216936846,
+            "velocityY": -5.0910568375180236e-34,
+            "timestamp": 0.396439740934148
         },
         {
-            "x": 3.0690365020812136,
-            "y": 5.5600003676341405,
-            "heading": 3.1098967052884415e-8,
-            "angularVelocity": 8.336617080453033e-14,
-            "velocityX": -3.7829405796100413,
-            "velocityY": -9.968399941565162e-8,
-            "timestamp": 0.4955496761676623
+            "x": 3.0690365020812695,
+            "y": 5.56,
+            "heading": -2.5850068638158606e-18,
+            "angularVelocity": -5.212061828001078e-17,
+            "velocityX": -3.7829405796099502,
+            "velocityY": -4.536187828596351e-34,
+            "timestamp": 0.495549676167685
         },
         {
-            "x": 2.7414219190960516,
-            "y": 5.560000235969226,
-            "heading": 3.331211577627848e-8,
-            "angularVelocity": 2.233025994415073e-8,
-            "velocityX": -3.3055675216940097,
-            "velocityY": -0.0000013284733736467011,
-            "timestamp": 0.5946596114011948
+            "x": 2.741421919096125,
+            "y": 5.56,
+            "heading": -1.5478395588580842e-18,
+            "angularVelocity": 1.0464816695997814e-17,
+            "velocityX": -3.305567521693684,
+            "velocityY": -2.872506388563931e-33,
+            "timestamp": 0.594659611401222
         },
         {
-            "x": 2.495710964163716,
-            "y": 5.560000125328881,
-            "heading": 2.2528078300409453e-8,
-            "angularVelocity": -1.088088286759601e-7,
-            "velocityX": -2.4791758197941256,
-            "velocityY": -0.0000011163395746408495,
-            "timestamp": 0.6937695466347273
+            "x": 2.4957109641637625,
+            "y": 5.56,
+            "heading": -7.728706253244835e-19,
+            "angularVelocity": 7.819286045545264e-18,
+            "velocityX": -2.479175819794286,
+            "velocityY": -1.6595590840772702e-33,
+            "timestamp": 0.6937695466347591
         },
         {
-            "x": 2.3319036557469475,
-            "y": 5.560000044126654,
-            "heading": 9.093200341480335e-9,
-            "angularVelocity": -1.3555529933337065e-7,
-            "velocityX": -1.6527839316087698,
-            "velocityY": -8.193146909757129e-7,
-            "timestamp": 0.7928794818682598
+            "x": 2.3319036557469652,
+            "y": 5.56,
+            "heading": -2.5738443375571516e-19,
+            "angularVelocity": 5.201155568994596e-18,
+            "velocityX": -1.6527839316089838,
+            "velocityY": -1.4347514653062086e-33,
+            "timestamp": 0.792879481868296
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 7.024905960060336e-33,
-            "angularVelocity": -9.17486204713572e-8,
-            "velocityX": -0.826391981328188,
-            "velocityY": -4.4522936458958866e-7,
-            "timestamp": 0.8919894171017924
+            "heading": 0,
+            "angularVelocity": 2.5969589542999405e-18,
+            "velocityX": -0.8263919813283321,
+            "velocityY": 2.9921012122849815e-33,
+            "timestamp": 0.891989417101833
         },
         {
             "x": 2.25,
             "y": 5.56,
             "heading": 0,
-            "angularVelocity": 2.1129142513587384e-31,
-            "velocityX": -1.9174161859426788e-38,
-            "velocityY": -1.530570219467473e-32,
-            "timestamp": 0.9910993523353249
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 1.808288986442481e-43,
+            "timestamp": 0.9910993523353699
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.traj
index 5e114a48..a4697851 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.traj	
@@ -3,101 +3,101 @@
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": -1.042790690974161e-34,
-            "angularVelocity": 1.2672485017804847e-33,
+            "heading": 0,
+            "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": 1.0649309840552648e-32,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 4.181096344253041,
-            "y": 5.560000047617906,
-            "heading": -1.1491589479255403e-8,
-            "angularVelocity": -1.1594790933974305e-7,
-            "velocityX": -0.8263919813282988,
-            "velocityY": 4.80455414778758e-7,
-            "timestamp": 0.09910993523353247
+            "x": 4.181096344253034,
+            "y": 5.56,
+            "heading": 2.5710705899848466e-19,
+            "angularVelocity": 2.5941602968880693e-18,
+            "velocityX": -0.8263919813283321,
+            "velocityY": -1.4479549544654444e-34,
+            "timestamp": 0.099109935233537
         },
         {
-            "x": 4.017289035836251,
-            "y": 5.560000133094268,
-            "heading": -2.0027929206394758e-8,
-            "angularVelocity": -8.613001771881656e-8,
-            "velocityX": -1.6527839316089912,
-            "velocityY": 8.624398858779209e-7,
-            "timestamp": 0.19821987046706493
+            "x": 4.017289035836238,
+            "y": 5.56,
+            "heading": 7.7182898862159e-19,
+            "angularVelocity": 5.193444314378615e-18,
+            "velocityX": -1.6527839316089838,
+            "velocityY": -7.36495654370163e-34,
+            "timestamp": 0.198219870467074
         },
         {
-            "x": 3.771578080903882,
-            "y": 5.560000246435489,
-            "heading": -1.061567052726606e-8,
-            "angularVelocity": 9.49678480632688e-8,
-            "velocityX": -2.4791758197944636,
-            "velocityY": 0.0000011435908848510677,
-            "timestamp": 0.2973298057005974
+            "x": 3.771578080903875,
+            "y": 5.56,
+            "heading": 1.545129672215716e-18,
+            "angularVelocity": 7.802453727507411e-18,
+            "velocityX": -2.479175819794286,
+            "velocityY": -8.54847043233614e-34,
+            "timestamp": 0.297329805700611
         },
         {
-            "x": 3.443963497918667,
-            "y": 5.560000377513817,
-            "heading": 3.109895895304223e-8,
-            "angularVelocity": 4.208924841656569e-7,
-            "velocityX": -3.30556752169455,
-            "velocityY": 0.0000013225548252865442,
-            "timestamp": 0.39643974093412987
+            "x": 3.4439634979187304,
+            "y": 5.56,
+            "heading": 2.5806642382479214e-18,
+            "angularVelocity": 1.0448342676399075e-17,
+            "velocityX": -3.3055675216936846,
+            "velocityY": -5.0910568375180236e-34,
+            "timestamp": 0.396439740934148
         },
         {
-            "x": 3.0690365020812136,
-            "y": 5.5600003676341405,
-            "heading": 3.1098967052884415e-8,
-            "angularVelocity": 8.336617080453033e-14,
-            "velocityX": -3.7829405796100413,
-            "velocityY": -9.968399941565162e-8,
-            "timestamp": 0.4955496761676623
+            "x": 3.0690365020812695,
+            "y": 5.56,
+            "heading": -2.5850068638158606e-18,
+            "angularVelocity": -5.212061828001078e-17,
+            "velocityX": -3.7829405796099502,
+            "velocityY": -4.536187828596351e-34,
+            "timestamp": 0.495549676167685
         },
         {
-            "x": 2.7414219190960516,
-            "y": 5.560000235969226,
-            "heading": 3.331211577627848e-8,
-            "angularVelocity": 2.233025994415073e-8,
-            "velocityX": -3.3055675216940097,
-            "velocityY": -0.0000013284733736467011,
-            "timestamp": 0.5946596114011948
+            "x": 2.741421919096125,
+            "y": 5.56,
+            "heading": -1.5478395588580842e-18,
+            "angularVelocity": 1.0464816695997814e-17,
+            "velocityX": -3.305567521693684,
+            "velocityY": -2.872506388563931e-33,
+            "timestamp": 0.594659611401222
         },
         {
-            "x": 2.495710964163716,
-            "y": 5.560000125328881,
-            "heading": 2.2528078300409453e-8,
-            "angularVelocity": -1.088088286759601e-7,
-            "velocityX": -2.4791758197941256,
-            "velocityY": -0.0000011163395746408495,
-            "timestamp": 0.6937695466347273
+            "x": 2.4957109641637625,
+            "y": 5.56,
+            "heading": -7.728706253244835e-19,
+            "angularVelocity": 7.819286045545264e-18,
+            "velocityX": -2.479175819794286,
+            "velocityY": -1.6595590840772702e-33,
+            "timestamp": 0.6937695466347591
         },
         {
-            "x": 2.3319036557469475,
-            "y": 5.560000044126654,
-            "heading": 9.093200341480335e-9,
-            "angularVelocity": -1.3555529933337065e-7,
-            "velocityX": -1.6527839316087698,
-            "velocityY": -8.193146909757129e-7,
-            "timestamp": 0.7928794818682598
+            "x": 2.3319036557469652,
+            "y": 5.56,
+            "heading": -2.5738443375571516e-19,
+            "angularVelocity": 5.201155568994596e-18,
+            "velocityX": -1.6527839316089838,
+            "velocityY": -1.4347514653062086e-33,
+            "timestamp": 0.792879481868296
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 7.024905960060336e-33,
-            "angularVelocity": -9.17486204713572e-8,
-            "velocityX": -0.826391981328188,
-            "velocityY": -4.4522936458958866e-7,
-            "timestamp": 0.8919894171017924
+            "heading": 0,
+            "angularVelocity": 2.5969589542999405e-18,
+            "velocityX": -0.8263919813283321,
+            "velocityY": 2.9921012122849815e-33,
+            "timestamp": 0.891989417101833
         },
         {
             "x": 2.25,
             "y": 5.56,
             "heading": 0,
-            "angularVelocity": 2.1129142513587384e-31,
-            "velocityX": -1.9174161859426788e-38,
-            "velocityY": -1.530570219467473e-32,
-            "timestamp": 0.9910993523353249
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 1.808288986442481e-43,
+            "timestamp": 0.9910993523353699
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj
index c4008c9d..4a59e123 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj	
@@ -3,128 +3,128 @@
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 3.6296173977413556e-31,
-            "angularVelocity": 2.1802637913498067e-31,
-            "velocityX": -2.5358916422596453e-32,
-            "velocityY": 1.2879635720357984e-31,
+            "heading": 9.014129666558106e-32,
+            "angularVelocity": 9.039417129994225e-31,
+            "velocityX": 0,
+            "velocityY": -2.779554313855136e-31,
             "timestamp": 0
         },
         {
-            "x": 4.206523470432328,
-            "y": 5.554409314419878,
-            "heading": -0.10760113621403576,
-            "angularVelocity": -1.2344151326641672,
-            "velocityX": -0.6479065667114609,
-            "velocityY": -0.06413711903878926,
-            "timestamp": 0.08716770668697735
+            "x": 4.206523478699805,
+            "y": 5.55440930401173,
+            "heading": -0.10760119525493232,
+            "angularVelocity": -1.2344158099898592,
+            "velocityX": -0.6479064718660169,
+            "velocityY": -0.06413723844253004,
+            "timestamp": 0.08716770668695249
         },
         {
-            "x": 4.091196970051195,
-            "y": 5.54119730250816,
-            "heading": -0.2959135822275083,
-            "angularVelocity": -2.1603464536437884,
-            "velocityX": -1.3230415800116804,
-            "velocityY": -0.15157002993279564,
-            "timestamp": 0.1743354133739547
+            "x": 4.091196989898444,
+            "y": 5.541197271810027,
+            "heading": -0.295913717265277,
+            "angularVelocity": -2.1603473254910437,
+            "velocityX": -1.3230414471673093,
+            "velocityY": -0.15157026270235532,
+            "timestamp": 0.17433541337390498
         },
         {
-            "x": 3.9151950635806365,
-            "y": 5.5163708141067165,
-            "heading": -0.5340651676870422,
-            "angularVelocity": -2.732107961894037,
-            "velocityX": -2.0191182395401066,
-            "velocityY": -0.28481291231621586,
-            "timestamp": 0.261503120060932
+            "x": 3.9151950960136452,
+            "y": 5.516370757100726,
+            "heading": -0.5340653762567812,
+            "angularVelocity": -2.7321088054637612,
+            "velocityX": -2.019118095155108,
+            "velocityY": -0.2848132141236808,
+            "timestamp": 0.26150312006085746
         },
         {
-            "x": 3.678758755729211,
-            "y": 5.473177786204715,
-            "heading": -0.7546652286499241,
-            "angularVelocity": -2.5307544427555704,
-            "velocityX": -2.712430059683426,
-            "velocityY": -0.49551639642315154,
-            "timestamp": 0.3486708267479094
+            "x": 3.678758800597895,
+            "y": 5.473177706772957,
+            "heading": -0.7546654655667131,
+            "angularVelocity": -2.530754767957573,
+            "velocityX": -2.712429917020411,
+            "velocityY": -0.4955166536948123,
+            "timestamp": 0.34867082674780997
         },
         {
-            "x": 3.3968844052666176,
-            "y": 5.398848722834353,
-            "heading": -0.8527881698077676,
-            "angularVelocity": -1.1256799666670962,
-            "velocityX": -3.2337015756857643,
-            "velocityY": -0.8527133062853276,
-            "timestamp": 0.43583853343488677
+            "x": 3.3968844521889907,
+            "y": 5.398848640381046,
+            "heading": -0.8527884173152881,
+            "angularVelocity": -1.1256800881657552,
+            "velocityX": -3.233701552126478,
+            "velocityY": -0.8527133409492049,
+            "timestamp": 0.4358385334347625
         },
         {
-            "x": 3.0986154730622166,
-            "y": 5.268356785097819,
-            "heading": -0.8643849747424638,
-            "angularVelocity": -0.1330401518573905,
-            "velocityX": -3.4217824873550513,
-            "velocityY": -1.4970215770977777,
-            "timestamp": 0.5230062401218641
+            "x": 3.098615494079836,
+            "y": 5.268356714792353,
+            "heading": -0.864385170870599,
+            "angularVelocity": -0.1330395624260059,
+            "velocityX": -3.421782784538955,
+            "velocityY": -1.4970214377364781,
+            "timestamp": 0.5230062401217149
         },
         {
-            "x": 2.8320964566491864,
-            "y": 5.083619646782129,
-            "heading": -0.847429861393553,
-            "angularVelocity": 0.19451140787489107,
-            "velocityX": -3.0575430574319302,
-            "velocityY": -2.11933002871221,
-            "timestamp": 0.6101739468088415
+            "x": 2.832096469446971,
+            "y": 5.0836195976119996,
+            "heading": -0.8474300397750907,
+            "angularVelocity": 0.19451161146636953,
+            "velocityX": -3.0575431517318843,
+            "velocityY": -2.1193297862453186,
+            "timestamp": 0.6101739468086674
         },
         {
-            "x": 2.6201643415258493,
-            "y": 4.879059787480057,
-            "heading": -0.7372423358568471,
-            "angularVelocity": 1.2640865490747921,
-            "velocityX": -2.4313145679557007,
-            "velocityY": -2.3467390284415206,
-            "timestamp": 0.6973416534958189
+            "x": 2.620164345186853,
+            "y": 4.879059766080367,
+            "heading": -0.7372424721392209,
+            "angularVelocity": 1.2640870320425897,
+            "velocityX": -2.431314672774813,
+            "velocityY": -2.346738709855847,
+            "timestamp": 0.6973416534956199
         },
         {
-            "x": 2.4692268443526784,
-            "y": 4.691160871442081,
-            "heading": -0.6128307115212047,
-            "angularVelocity": 1.4272673799072706,
-            "velocityX": -1.7315758657640712,
-            "velocityY": -2.1556023805091904,
-            "timestamp": 0.7845093601827963
+            "x": 2.469226841034766,
+            "y": 4.6911608685478585,
+            "heading": -0.6128307853924054,
+            "angularVelocity": 1.427268095897297,
+            "velocityX": -1.7315759458276552,
+            "velocityY": -2.155602168212534,
+            "timestamp": 0.7845093601825724
         },
         {
-            "x": 2.3748885078351467,
-            "y": 4.53130895090854,
-            "heading": -0.5057978375616147,
-            "angularVelocity": 1.227895949401916,
-            "velocityX": -1.0822624582324516,
-            "velocityY": -1.8338433648091068,
-            "timestamp": 0.8716770668697736
+            "x": 2.3748885049182054,
+            "y": 4.5313089505917965,
+            "heading": -0.5057978702393489,
+            "angularVelocity": 1.2278964219794164,
+            "velocityX": -1.0822624536327627,
+            "velocityY": -1.8338433352404204,
+            "timestamp": 0.871677066869525
         },
         {
-            "x": 2.322390449292023,
-            "y": 4.41825305597818,
-            "heading": -0.4294901557678491,
-            "angularVelocity": 0.8754122907900764,
-            "velocityX": -0.6022649962748851,
-            "velocityY": -1.2969928799015857,
-            "timestamp": 0.958844773556751
+            "x": 2.3223904484665,
+            "y": 4.418253055823363,
+            "heading": -0.42949016513376354,
+            "angularVelocity": 0.87541255822675,
+            "velocityX": -0.6022649722820163,
+            "velocityY": -1.2969928780443263,
+            "timestamp": 0.9588447735564775
         },
         {
             "x": 2.3,
             "y": 4.36,
             "heading": -0.39,
-            "angularVelocity": 0.45303653461548066,
-            "velocityX": -0.2568663343688406,
-            "velocityY": -0.6682871236634659,
-            "timestamp": 1.0460124802437283
+            "angularVelocity": 0.4530366420626772,
+            "velocityX": -0.256866324898396,
+            "velocityY": -0.6682871218875669,
+            "timestamp": 1.0460124802434299
         },
         {
             "x": 2.3,
             "y": 4.36,
             "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 3.506966732338881e-31,
-            "velocityY": 5.081769191491498e-30,
-            "timestamp": 1.1331801869307057
+            "angularVelocity": -3.4100744325418914e-31,
+            "velocityX": -9.400788860397695e-33,
+            "velocityY": 0,
+            "timestamp": 1.1331801869303824
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.traj
index c4008c9d..4a59e123 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.traj	
@@ -3,128 +3,128 @@
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 3.6296173977413556e-31,
-            "angularVelocity": 2.1802637913498067e-31,
-            "velocityX": -2.5358916422596453e-32,
-            "velocityY": 1.2879635720357984e-31,
+            "heading": 9.014129666558106e-32,
+            "angularVelocity": 9.039417129994225e-31,
+            "velocityX": 0,
+            "velocityY": -2.779554313855136e-31,
             "timestamp": 0
         },
         {
-            "x": 4.206523470432328,
-            "y": 5.554409314419878,
-            "heading": -0.10760113621403576,
-            "angularVelocity": -1.2344151326641672,
-            "velocityX": -0.6479065667114609,
-            "velocityY": -0.06413711903878926,
-            "timestamp": 0.08716770668697735
+            "x": 4.206523478699805,
+            "y": 5.55440930401173,
+            "heading": -0.10760119525493232,
+            "angularVelocity": -1.2344158099898592,
+            "velocityX": -0.6479064718660169,
+            "velocityY": -0.06413723844253004,
+            "timestamp": 0.08716770668695249
         },
         {
-            "x": 4.091196970051195,
-            "y": 5.54119730250816,
-            "heading": -0.2959135822275083,
-            "angularVelocity": -2.1603464536437884,
-            "velocityX": -1.3230415800116804,
-            "velocityY": -0.15157002993279564,
-            "timestamp": 0.1743354133739547
+            "x": 4.091196989898444,
+            "y": 5.541197271810027,
+            "heading": -0.295913717265277,
+            "angularVelocity": -2.1603473254910437,
+            "velocityX": -1.3230414471673093,
+            "velocityY": -0.15157026270235532,
+            "timestamp": 0.17433541337390498
         },
         {
-            "x": 3.9151950635806365,
-            "y": 5.5163708141067165,
-            "heading": -0.5340651676870422,
-            "angularVelocity": -2.732107961894037,
-            "velocityX": -2.0191182395401066,
-            "velocityY": -0.28481291231621586,
-            "timestamp": 0.261503120060932
+            "x": 3.9151950960136452,
+            "y": 5.516370757100726,
+            "heading": -0.5340653762567812,
+            "angularVelocity": -2.7321088054637612,
+            "velocityX": -2.019118095155108,
+            "velocityY": -0.2848132141236808,
+            "timestamp": 0.26150312006085746
         },
         {
-            "x": 3.678758755729211,
-            "y": 5.473177786204715,
-            "heading": -0.7546652286499241,
-            "angularVelocity": -2.5307544427555704,
-            "velocityX": -2.712430059683426,
-            "velocityY": -0.49551639642315154,
-            "timestamp": 0.3486708267479094
+            "x": 3.678758800597895,
+            "y": 5.473177706772957,
+            "heading": -0.7546654655667131,
+            "angularVelocity": -2.530754767957573,
+            "velocityX": -2.712429917020411,
+            "velocityY": -0.4955166536948123,
+            "timestamp": 0.34867082674780997
         },
         {
-            "x": 3.3968844052666176,
-            "y": 5.398848722834353,
-            "heading": -0.8527881698077676,
-            "angularVelocity": -1.1256799666670962,
-            "velocityX": -3.2337015756857643,
-            "velocityY": -0.8527133062853276,
-            "timestamp": 0.43583853343488677
+            "x": 3.3968844521889907,
+            "y": 5.398848640381046,
+            "heading": -0.8527884173152881,
+            "angularVelocity": -1.1256800881657552,
+            "velocityX": -3.233701552126478,
+            "velocityY": -0.8527133409492049,
+            "timestamp": 0.4358385334347625
         },
         {
-            "x": 3.0986154730622166,
-            "y": 5.268356785097819,
-            "heading": -0.8643849747424638,
-            "angularVelocity": -0.1330401518573905,
-            "velocityX": -3.4217824873550513,
-            "velocityY": -1.4970215770977777,
-            "timestamp": 0.5230062401218641
+            "x": 3.098615494079836,
+            "y": 5.268356714792353,
+            "heading": -0.864385170870599,
+            "angularVelocity": -0.1330395624260059,
+            "velocityX": -3.421782784538955,
+            "velocityY": -1.4970214377364781,
+            "timestamp": 0.5230062401217149
         },
         {
-            "x": 2.8320964566491864,
-            "y": 5.083619646782129,
-            "heading": -0.847429861393553,
-            "angularVelocity": 0.19451140787489107,
-            "velocityX": -3.0575430574319302,
-            "velocityY": -2.11933002871221,
-            "timestamp": 0.6101739468088415
+            "x": 2.832096469446971,
+            "y": 5.0836195976119996,
+            "heading": -0.8474300397750907,
+            "angularVelocity": 0.19451161146636953,
+            "velocityX": -3.0575431517318843,
+            "velocityY": -2.1193297862453186,
+            "timestamp": 0.6101739468086674
         },
         {
-            "x": 2.6201643415258493,
-            "y": 4.879059787480057,
-            "heading": -0.7372423358568471,
-            "angularVelocity": 1.2640865490747921,
-            "velocityX": -2.4313145679557007,
-            "velocityY": -2.3467390284415206,
-            "timestamp": 0.6973416534958189
+            "x": 2.620164345186853,
+            "y": 4.879059766080367,
+            "heading": -0.7372424721392209,
+            "angularVelocity": 1.2640870320425897,
+            "velocityX": -2.431314672774813,
+            "velocityY": -2.346738709855847,
+            "timestamp": 0.6973416534956199
         },
         {
-            "x": 2.4692268443526784,
-            "y": 4.691160871442081,
-            "heading": -0.6128307115212047,
-            "angularVelocity": 1.4272673799072706,
-            "velocityX": -1.7315758657640712,
-            "velocityY": -2.1556023805091904,
-            "timestamp": 0.7845093601827963
+            "x": 2.469226841034766,
+            "y": 4.6911608685478585,
+            "heading": -0.6128307853924054,
+            "angularVelocity": 1.427268095897297,
+            "velocityX": -1.7315759458276552,
+            "velocityY": -2.155602168212534,
+            "timestamp": 0.7845093601825724
         },
         {
-            "x": 2.3748885078351467,
-            "y": 4.53130895090854,
-            "heading": -0.5057978375616147,
-            "angularVelocity": 1.227895949401916,
-            "velocityX": -1.0822624582324516,
-            "velocityY": -1.8338433648091068,
-            "timestamp": 0.8716770668697736
+            "x": 2.3748885049182054,
+            "y": 4.5313089505917965,
+            "heading": -0.5057978702393489,
+            "angularVelocity": 1.2278964219794164,
+            "velocityX": -1.0822624536327627,
+            "velocityY": -1.8338433352404204,
+            "timestamp": 0.871677066869525
         },
         {
-            "x": 2.322390449292023,
-            "y": 4.41825305597818,
-            "heading": -0.4294901557678491,
-            "angularVelocity": 0.8754122907900764,
-            "velocityX": -0.6022649962748851,
-            "velocityY": -1.2969928799015857,
-            "timestamp": 0.958844773556751
+            "x": 2.3223904484665,
+            "y": 4.418253055823363,
+            "heading": -0.42949016513376354,
+            "angularVelocity": 0.87541255822675,
+            "velocityX": -0.6022649722820163,
+            "velocityY": -1.2969928780443263,
+            "timestamp": 0.9588447735564775
         },
         {
             "x": 2.3,
             "y": 4.36,
             "heading": -0.39,
-            "angularVelocity": 0.45303653461548066,
-            "velocityX": -0.2568663343688406,
-            "velocityY": -0.6682871236634659,
-            "timestamp": 1.0460124802437283
+            "angularVelocity": 0.4530366420626772,
+            "velocityX": -0.256866324898396,
+            "velocityY": -0.6682871218875669,
+            "timestamp": 1.0460124802434299
         },
         {
             "x": 2.3,
             "y": 4.36,
             "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 3.506966732338881e-31,
-            "velocityY": 5.081769191491498e-30,
-            "timestamp": 1.1331801869307057
+            "angularVelocity": -3.4100744325418914e-31,
+            "velocityX": -9.400788860397695e-33,
+            "velocityY": 0,
+            "timestamp": 1.1331801869303824
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4.1.traj
new file mode 100644
index 00000000..0b021e4e
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4.1.traj	
@@ -0,0 +1,238 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 0,
+            "velocityX": 5.832812358477626e-33,
+            "velocityY": -2.114400767819492e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 4.2637473414422145,
+            "y": 2.974020167404826,
+            "heading": -0.5570803398431683,
+            "angularVelocity": 0.15939480257853877,
+            "velocityX": 0.013355031418934325,
+            "velocityY": -0.46426099366130524,
+            "timestamp": 0.05595954204614445
+        },
+        {
+            "x": 4.266239342094047,
+            "y": 2.922097369670203,
+            "heading": -0.5396590874905302,
+            "angularVelocity": 0.31131870840312753,
+            "velocityX": 0.04453218451605929,
+            "velocityY": -0.9278631639230981,
+            "timestamp": 0.1119190840922889
+        },
+        {
+            "x": 4.271816064940073,
+            "y": 2.844342168020101,
+            "heading": -0.5143157133438189,
+            "angularVelocity": 0.4528874472527088,
+            "velocityX": 0.0996563345966598,
+            "velocityY": -1.3894895992176792,
+            "timestamp": 0.16787862613843335
+        },
+        {
+            "x": 4.282354343554946,
+            "y": 2.7410289237168692,
+            "heading": -0.48190029722305777,
+            "angularVelocity": 0.5792652143941603,
+            "velocityX": 0.18831960072480589,
+            "velocityY": -1.8462131841257783,
+            "timestamp": 0.2238381681845778
+        },
+        {
+            "x": 4.3006106845058465,
+            "y": 2.6128237251760056,
+            "heading": -0.4437500478548854,
+            "angularVelocity": 0.6817469902936669,
+            "velocityX": 0.32624178617911886,
+            "velocityY": -2.2910337335345985,
+            "timestamp": 0.27979771023072225
+        },
+        {
+            "x": 4.330801306241698,
+            "y": 2.4614150778969766,
+            "heading": -0.40214700400499553,
+            "angularVelocity": 0.7434486117771366,
+            "velocityX": 0.5395080201148661,
+            "velocityY": -2.705680599640681,
+            "timestamp": 0.3357572522768667
+        },
+        {
+            "x": 4.379172513113433,
+            "y": 2.2912661424924936,
+            "heading": -0.3611794534128463,
+            "angularVelocity": 0.7320923133783194,
+            "velocityX": 0.864396045840541,
+            "velocityY": -3.0405705476320635,
+            "timestamp": 0.39171679432301115
+        },
+        {
+            "x": 4.451949891834961,
+            "y": 2.111933259909663,
+            "heading": -0.32494756197390784,
+            "angularVelocity": 0.6474658318159084,
+            "velocityX": 1.3005356380778725,
+            "velocityY": -3.2046881733762564,
+            "timestamp": 0.4476763363691556
+        },
+        {
+            "x": 4.550285732793113,
+            "y": 1.9373903674143325,
+            "heading": -0.2971520965733793,
+            "angularVelocity": 0.49670644869849423,
+            "velocityX": 1.7572667209653445,
+            "velocityY": -3.1190907951212283,
+            "timestamp": 0.5036358784153001
+        },
+        {
+            "x": 4.671000177640236,
+            "y": 1.7762856565305696,
+            "heading": -0.2715476133805437,
+            "angularVelocity": 0.457553479828869,
+            "velocityX": 2.157173565637473,
+            "velocityY": -2.878949773229245,
+            "timestamp": 0.5595954204614446
+        },
+        {
+            "x": 4.810884164899462,
+            "y": 1.6328902215117949,
+            "heading": -0.24301377044973924,
+            "angularVelocity": 0.5099012945331528,
+            "velocityX": 2.4997343106181424,
+            "velocityY": -2.5624840693036233,
+            "timestamp": 0.6155549625075891
+        },
+        {
+            "x": 4.966899091534391,
+            "y": 1.5100232904396305,
+            "heading": -0.21315700918191657,
+            "angularVelocity": 0.5335419157506132,
+            "velocityX": 2.7879950573269263,
+            "velocityY": -2.1956386092446096,
+            "timestamp": 0.6715145045537336
+        },
+        {
+            "x": 5.136752128601074,
+            "y": 1.409298062324524,
+            "heading": -0.1837509654270181,
+            "angularVelocity": 0.5254875697633724,
+            "velocityX": 3.035282828558936,
+            "velocityY": -1.7999651968567865,
+            "timestamp": 0.7274740465998781
+        },
+        {
+            "x": 5.4382274814533025,
+            "y": 1.3074419969375322,
+            "heading": -0.14111678525881038,
+            "angularVelocity": 0.4782734646509064,
+            "velocityX": 3.3819733590888914,
+            "velocityY": -1.142629061849971,
+            "timestamp": 0.8166158939305274
+        },
+        {
+            "x": 5.764139683697637,
+            "y": 1.267066681764717,
+            "heading": -0.11133632809533998,
+            "angularVelocity": 0.3340794257159682,
+            "velocityX": 3.6561077877984602,
+            "velocityY": -0.4529333459183563,
+            "timestamp": 0.9057577412611766
+        },
+        {
+            "x": 6.100462721846291,
+            "y": 1.2915989985388854,
+            "heading": -0.11133120767369757,
+            "angularVelocity": 0.000057441278584387276,
+            "velocityX": 3.7728973340784897,
+            "velocityY": 0.2752053890377148,
+            "timestamp": 0.9948995885918258
+        },
+        {
+            "x": 6.42549869748969,
+            "y": 1.3814198753662668,
+            "heading": -0.1113311879575033,
+            "angularVelocity": 2.2117776172639892e-7,
+            "velocityX": 3.6462782113742715,
+            "velocityY": 1.0076174043625032,
+            "timestamp": 1.084041435922475
+        },
+        {
+            "x": 6.7266979846048365,
+            "y": 1.533061465523067,
+            "heading": -0.11133117504978503,
+            "angularVelocity": 1.4479976182990285e-7,
+            "velocityX": 3.3788764327254777,
+            "velocityY": 1.7011268522889103,
+            "timestamp": 1.1731832832531242
+        },
+        {
+            "x": 6.992432961420095,
+            "y": 1.740669527522903,
+            "heading": -0.11133116451645807,
+            "angularVelocity": 1.1816366034373416e-7,
+            "velocityX": 2.981035111708839,
+            "velocityY": 2.3289629754897407,
+            "timestamp": 1.2623251305837733
+        },
+        {
+            "x": 7.221595592407175,
+            "y": 1.979432543612985,
+            "heading": -0.09587073631794915,
+            "angularVelocity": 0.17343625537800947,
+            "velocityX": 2.5707637641507293,
+            "velocityY": 2.678461611912118,
+            "timestamp": 1.3514669779144224
+        },
+        {
+            "x": 7.404993837760709,
+            "y": 2.170501480308595,
+            "heading": -0.06411284637687509,
+            "angularVelocity": 0.3562624164974272,
+            "velocityX": 2.0573754173309005,
+            "velocityY": 2.1434258142181197,
+            "timestamp": 1.4406088252450715
+        },
+        {
+            "x": 7.54251389523076,
+            "y": 2.3137697288578556,
+            "heading": -0.0342508837604198,
+            "angularVelocity": 0.33499376006509896,
+            "velocityX": 1.5427104282454527,
+            "velocityY": 1.6071940714649837,
+            "timestamp": 1.5297506725757206
+        },
+        {
+            "x": 7.634176122098128,
+            "y": 2.4092619199619465,
+            "heading": -0.011960250388889534,
+            "angularVelocity": 0.25005801471490263,
+            "velocityX": 1.028273808679041,
+            "velocityY": 1.0712386377846093,
+            "timestamp": 1.6188925199063697
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 9.313254165846089e-33,
+            "angularVelocity": 0.1341709954082624,
+            "velocityX": 0.5140557355952123,
+            "velocityY": 0.5355294002487934,
+            "timestamp": 1.7080343672370188
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": -1.1876173054192403e-31,
+            "velocityX": 4.5955148793706025e-32,
+            "velocityY": 0,
+            "timestamp": 1.797176214567668
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4.traj
new file mode 100644
index 00000000..0b021e4e
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4.traj	
@@ -0,0 +1,238 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 0,
+            "velocityX": 5.832812358477626e-33,
+            "velocityY": -2.114400767819492e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 4.2637473414422145,
+            "y": 2.974020167404826,
+            "heading": -0.5570803398431683,
+            "angularVelocity": 0.15939480257853877,
+            "velocityX": 0.013355031418934325,
+            "velocityY": -0.46426099366130524,
+            "timestamp": 0.05595954204614445
+        },
+        {
+            "x": 4.266239342094047,
+            "y": 2.922097369670203,
+            "heading": -0.5396590874905302,
+            "angularVelocity": 0.31131870840312753,
+            "velocityX": 0.04453218451605929,
+            "velocityY": -0.9278631639230981,
+            "timestamp": 0.1119190840922889
+        },
+        {
+            "x": 4.271816064940073,
+            "y": 2.844342168020101,
+            "heading": -0.5143157133438189,
+            "angularVelocity": 0.4528874472527088,
+            "velocityX": 0.0996563345966598,
+            "velocityY": -1.3894895992176792,
+            "timestamp": 0.16787862613843335
+        },
+        {
+            "x": 4.282354343554946,
+            "y": 2.7410289237168692,
+            "heading": -0.48190029722305777,
+            "angularVelocity": 0.5792652143941603,
+            "velocityX": 0.18831960072480589,
+            "velocityY": -1.8462131841257783,
+            "timestamp": 0.2238381681845778
+        },
+        {
+            "x": 4.3006106845058465,
+            "y": 2.6128237251760056,
+            "heading": -0.4437500478548854,
+            "angularVelocity": 0.6817469902936669,
+            "velocityX": 0.32624178617911886,
+            "velocityY": -2.2910337335345985,
+            "timestamp": 0.27979771023072225
+        },
+        {
+            "x": 4.330801306241698,
+            "y": 2.4614150778969766,
+            "heading": -0.40214700400499553,
+            "angularVelocity": 0.7434486117771366,
+            "velocityX": 0.5395080201148661,
+            "velocityY": -2.705680599640681,
+            "timestamp": 0.3357572522768667
+        },
+        {
+            "x": 4.379172513113433,
+            "y": 2.2912661424924936,
+            "heading": -0.3611794534128463,
+            "angularVelocity": 0.7320923133783194,
+            "velocityX": 0.864396045840541,
+            "velocityY": -3.0405705476320635,
+            "timestamp": 0.39171679432301115
+        },
+        {
+            "x": 4.451949891834961,
+            "y": 2.111933259909663,
+            "heading": -0.32494756197390784,
+            "angularVelocity": 0.6474658318159084,
+            "velocityX": 1.3005356380778725,
+            "velocityY": -3.2046881733762564,
+            "timestamp": 0.4476763363691556
+        },
+        {
+            "x": 4.550285732793113,
+            "y": 1.9373903674143325,
+            "heading": -0.2971520965733793,
+            "angularVelocity": 0.49670644869849423,
+            "velocityX": 1.7572667209653445,
+            "velocityY": -3.1190907951212283,
+            "timestamp": 0.5036358784153001
+        },
+        {
+            "x": 4.671000177640236,
+            "y": 1.7762856565305696,
+            "heading": -0.2715476133805437,
+            "angularVelocity": 0.457553479828869,
+            "velocityX": 2.157173565637473,
+            "velocityY": -2.878949773229245,
+            "timestamp": 0.5595954204614446
+        },
+        {
+            "x": 4.810884164899462,
+            "y": 1.6328902215117949,
+            "heading": -0.24301377044973924,
+            "angularVelocity": 0.5099012945331528,
+            "velocityX": 2.4997343106181424,
+            "velocityY": -2.5624840693036233,
+            "timestamp": 0.6155549625075891
+        },
+        {
+            "x": 4.966899091534391,
+            "y": 1.5100232904396305,
+            "heading": -0.21315700918191657,
+            "angularVelocity": 0.5335419157506132,
+            "velocityX": 2.7879950573269263,
+            "velocityY": -2.1956386092446096,
+            "timestamp": 0.6715145045537336
+        },
+        {
+            "x": 5.136752128601074,
+            "y": 1.409298062324524,
+            "heading": -0.1837509654270181,
+            "angularVelocity": 0.5254875697633724,
+            "velocityX": 3.035282828558936,
+            "velocityY": -1.7999651968567865,
+            "timestamp": 0.7274740465998781
+        },
+        {
+            "x": 5.4382274814533025,
+            "y": 1.3074419969375322,
+            "heading": -0.14111678525881038,
+            "angularVelocity": 0.4782734646509064,
+            "velocityX": 3.3819733590888914,
+            "velocityY": -1.142629061849971,
+            "timestamp": 0.8166158939305274
+        },
+        {
+            "x": 5.764139683697637,
+            "y": 1.267066681764717,
+            "heading": -0.11133632809533998,
+            "angularVelocity": 0.3340794257159682,
+            "velocityX": 3.6561077877984602,
+            "velocityY": -0.4529333459183563,
+            "timestamp": 0.9057577412611766
+        },
+        {
+            "x": 6.100462721846291,
+            "y": 1.2915989985388854,
+            "heading": -0.11133120767369757,
+            "angularVelocity": 0.000057441278584387276,
+            "velocityX": 3.7728973340784897,
+            "velocityY": 0.2752053890377148,
+            "timestamp": 0.9948995885918258
+        },
+        {
+            "x": 6.42549869748969,
+            "y": 1.3814198753662668,
+            "heading": -0.1113311879575033,
+            "angularVelocity": 2.2117776172639892e-7,
+            "velocityX": 3.6462782113742715,
+            "velocityY": 1.0076174043625032,
+            "timestamp": 1.084041435922475
+        },
+        {
+            "x": 6.7266979846048365,
+            "y": 1.533061465523067,
+            "heading": -0.11133117504978503,
+            "angularVelocity": 1.4479976182990285e-7,
+            "velocityX": 3.3788764327254777,
+            "velocityY": 1.7011268522889103,
+            "timestamp": 1.1731832832531242
+        },
+        {
+            "x": 6.992432961420095,
+            "y": 1.740669527522903,
+            "heading": -0.11133116451645807,
+            "angularVelocity": 1.1816366034373416e-7,
+            "velocityX": 2.981035111708839,
+            "velocityY": 2.3289629754897407,
+            "timestamp": 1.2623251305837733
+        },
+        {
+            "x": 7.221595592407175,
+            "y": 1.979432543612985,
+            "heading": -0.09587073631794915,
+            "angularVelocity": 0.17343625537800947,
+            "velocityX": 2.5707637641507293,
+            "velocityY": 2.678461611912118,
+            "timestamp": 1.3514669779144224
+        },
+        {
+            "x": 7.404993837760709,
+            "y": 2.170501480308595,
+            "heading": -0.06411284637687509,
+            "angularVelocity": 0.3562624164974272,
+            "velocityX": 2.0573754173309005,
+            "velocityY": 2.1434258142181197,
+            "timestamp": 1.4406088252450715
+        },
+        {
+            "x": 7.54251389523076,
+            "y": 2.3137697288578556,
+            "heading": -0.0342508837604198,
+            "angularVelocity": 0.33499376006509896,
+            "velocityX": 1.5427104282454527,
+            "velocityY": 1.6071940714649837,
+            "timestamp": 1.5297506725757206
+        },
+        {
+            "x": 7.634176122098128,
+            "y": 2.4092619199619465,
+            "heading": -0.011960250388889534,
+            "angularVelocity": 0.25005801471490263,
+            "velocityX": 1.028273808679041,
+            "velocityY": 1.0712386377846093,
+            "timestamp": 1.6188925199063697
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 9.313254165846089e-33,
+            "angularVelocity": 0.1341709954082624,
+            "velocityX": 0.5140557355952123,
+            "velocityY": 0.5355294002487934,
+            "timestamp": 1.7080343672370188
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": -1.1876173054192403e-31,
+            "velocityX": 4.5955148793706025e-32,
+            "velocityY": 0,
+            "timestamp": 1.797176214567668
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C1.1.traj
new file mode 100644
index 00000000..f8bd91fc
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C1.1.traj	
@@ -0,0 +1,292 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 1.1411819845129358e-30,
+            "velocityX": 2.0257584812603577e-32,
+            "velocityY": 3.042151659469263e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 5.128157808100627,
+            "y": 1.6381505638691025,
+            "heading": -0.6351424860741187,
+            "angularVelocity": 0.06199634399134786,
+            "velocityX": -0.6106088487961494,
+            "velocityY": 0.23165524966799825,
+            "timestamp": 0.07835161903352221
+        },
+        {
+            "x": 5.034138141125347,
+            "y": 1.6783506940439978,
+            "heading": -0.625731099901857,
+            "angularVelocity": 0.12011731586854772,
+            "velocityX": -1.199970953185457,
+            "velocityY": 0.5130733821555872,
+            "timestamp": 0.15670323806704442
+        },
+        {
+            "x": 4.897649658060979,
+            "y": 1.7471051706265737,
+            "heading": -0.6123788413002932,
+            "angularVelocity": 0.1704145844880519,
+            "velocityX": -1.74199952404266,
+            "velocityY": 0.8775118808095047,
+            "timestamp": 0.23505485710056662
+        },
+        {
+            "x": 4.7291295198915435,
+            "y": 1.855780594555545,
+            "heading": -0.5966040827715752,
+            "angularVelocity": 0.20133289807283902,
+            "velocityX": -2.150818837544829,
+            "velocityY": 1.3870220586313022,
+            "timestamp": 0.31340647613408884
+        },
+        {
+            "x": 4.559409610033689,
+            "y": 2.0156280635815147,
+            "heading": -0.5822183456142529,
+            "angularVelocity": 0.18360484869071408,
+            "velocityX": -2.1661314973624393,
+            "velocityY": 2.0401297509574126,
+            "timestamp": 0.39175809516761106
+        },
+        {
+            "x": 4.423281260574217,
+            "y": 2.2140742145732073,
+            "heading": -0.5727758831618049,
+            "angularVelocity": 0.1205139417528553,
+            "velocityX": -1.7374031467203144,
+            "velocityY": 2.532763884646578,
+            "timestamp": 0.4701097142011333
+        },
+        {
+            "x": 4.3324586171803325,
+            "y": 2.4362881098121476,
+            "heading": -0.5691499320560302,
+            "angularVelocity": 0.04627793465535804,
+            "velocityX": -1.1591674111421555,
+            "velocityY": 2.8361110846205655,
+            "timestamp": 0.5484613332346555
+        },
+        {
+            "x": 4.290356765831772,
+            "y": 2.674111890908862,
+            "heading": -0.5714790836196856,
+            "angularVelocity": -0.02972691046318988,
+            "velocityX": -0.5373450079001806,
+            "velocityY": 3.0353397164002804,
+            "timestamp": 0.6268129522681777
+        },
+        {
+            "x": 4.298268630722052,
+            "y": 2.9211188280591673,
+            "heading": -0.5947192771376703,
+            "angularVelocity": -0.29661408155511804,
+            "velocityX": 0.1009789585444941,
+            "velocityY": 3.1525441362561364,
+            "timestamp": 0.7051645713016998
+        },
+        {
+            "x": 4.356924477851345,
+            "y": 3.1722484967201265,
+            "heading": -0.6368890523591307,
+            "angularVelocity": -0.5382119188043637,
+            "velocityX": 0.7486232939768359,
+            "velocityY": 3.2051624683532776,
+            "timestamp": 0.783516190335222
+        },
+        {
+            "x": 4.466386404708156,
+            "y": 3.418800428561669,
+            "heading": -0.694172117002664,
+            "angularVelocity": -0.7311025011368958,
+            "velocityX": 1.3970601782967371,
+            "velocityY": 3.1467369134523744,
+            "timestamp": 0.8618678093687442
+        },
+        {
+            "x": 4.622742851027307,
+            "y": 3.644917946871822,
+            "heading": -0.7581397869413499,
+            "angularVelocity": -0.8164179723116849,
+            "velocityX": 1.9955739045067467,
+            "velocityY": 2.8859329404975034,
+            "timestamp": 0.9402194284022664
+        },
+        {
+            "x": 4.8130531116785615,
+            "y": 3.833426042269654,
+            "heading": -0.8482959054101304,
+            "angularVelocity": -1.1506605681014435,
+            "velocityX": 2.4289256941816704,
+            "velocityY": 2.4059246984696085,
+            "timestamp": 1.0185710474357885
+        },
+        {
+            "x": 5.034329891204834,
+            "y": 3.981222629547119,
+            "heading": -0.9454274313666489,
+            "angularVelocity": -1.2396875412997066,
+            "velocityX": 2.824150697276595,
+            "velocityY": 1.8863246107809373,
+            "timestamp": 1.0969226664693108
+        },
+        {
+            "x": 5.3307679727813255,
+            "y": 4.095215339701575,
+            "heading": -1.0325645415374454,
+            "angularVelocity": -0.9491279076997933,
+            "velocityX": 3.2289073573561264,
+            "velocityY": 1.241648503948074,
+            "timestamp": 1.1887302189223545
+        },
+        {
+            "x": 5.6706736122505745,
+            "y": 4.158722341660029,
+            "heading": -1.0366531166779425,
+            "angularVelocity": -0.04453419170049298,
+            "velocityX": 3.7023712144281196,
+            "velocityY": 0.691740496958957,
+            "timestamp": 1.2805377713753983
+        },
+        {
+            "x": 5.985432145120126,
+            "y": 4.2874622186845945,
+            "heading": -1.0150469515971576,
+            "angularVelocity": 0.23534191363870294,
+            "velocityX": 3.4284601262030105,
+            "velocityY": 1.402279807975625,
+            "timestamp": 1.372345323828442
+        },
+        {
+            "x": 6.260473058973373,
+            "y": 4.473603429076733,
+            "heading": -0.9641067919300658,
+            "angularVelocity": 0.5548580515001295,
+            "velocityX": 2.9958419160985765,
+            "velocityY": 2.027515225257119,
+            "timestamp": 1.4641528762814857
+        },
+        {
+            "x": 6.487005993334624,
+            "y": 4.6803674717401,
+            "heading": -0.8309910438969466,
+            "angularVelocity": 1.4499433268434323,
+            "velocityX": 2.4674760224886074,
+            "velocityY": 2.2521463337029823,
+            "timestamp": 1.5559604287345294
+        },
+        {
+            "x": 6.71888754498383,
+            "y": 4.9198952355114685,
+            "heading": -0.7843851972685187,
+            "angularVelocity": 0.507647196588375,
+            "velocityX": 2.5257350343568326,
+            "velocityY": 2.609020253468568,
+            "timestamp": 1.6477679811875732
+        },
+        {
+            "x": 6.906293835583718,
+            "y": 5.2122954621107285,
+            "heading": -0.7843850302000089,
+            "angularVelocity": 0.0000018197686939661552,
+            "velocityX": 2.0412949217411986,
+            "velocityY": 3.1849256274293083,
+            "timestamp": 1.739575533640617
+        },
+        {
+            "x": 7.030998336898649,
+            "y": 5.536437116039629,
+            "heading": -0.7843849409482478,
+            "angularVelocity": 9.721614257680115e-7,
+            "velocityX": 1.3583250830993825,
+            "velocityY": 3.530664365490929,
+            "timestamp": 1.8313830860936606
+        },
+        {
+            "x": 7.139916048661206,
+            "y": 5.86621879705104,
+            "heading": -0.7843848550588985,
+            "angularVelocity": 9.355368597772783e-7,
+            "velocityX": 1.1863698448802928,
+            "velocityY": 3.592097514854049,
+            "timestamp": 1.9231906385467044
+        },
+        {
+            "x": 7.248833621341376,
+            "y": 6.196000523997438,
+            "heading": -0.7843847691695015,
+            "angularVelocity": 9.355373786921671e-7,
+            "velocityX": 1.1863683299462482,
+            "velocityY": 3.592098015194004,
+            "timestamp": 2.0149981909997483
+        },
+        {
+            "x": 7.357751176995467,
+            "y": 6.5257822565670125,
+            "heading": -0.7843846832799689,
+            "angularVelocity": 9.355388566444766e-7,
+            "velocityX": 1.1863681444922285,
+            "velocityY": 3.5920980764436137,
+            "timestamp": 2.1068057434527923
+        },
+        {
+            "x": 7.4669549324183055,
+            "y": 6.834400664448086,
+            "heading": -0.7318192896940902,
+            "angularVelocity": 0.5725606682823186,
+            "velocityX": 1.189485532562175,
+            "velocityY": 3.361579735380924,
+            "timestamp": 2.198613295905836
+        },
+        {
+            "x": 7.555413306559794,
+            "y": 7.089894078912264,
+            "heading": -0.5139853289463429,
+            "angularVelocity": 2.3727237566774426,
+            "velocityX": 0.9635195773978621,
+            "velocityY": 2.782923709842436,
+            "timestamp": 2.29042084835888
+        },
+        {
+            "x": 7.619158448834915,
+            "y": 7.279787689667393,
+            "heading": -0.2782484542424907,
+            "angularVelocity": 2.5677285626846773,
+            "velocityX": 0.6943344046528581,
+            "velocityY": 2.0683876835977384,
+            "timestamp": 2.382228400811924
+        },
+        {
+            "x": 7.660128944224933,
+            "y": 7.404974004983973,
+            "heading": -0.09793823848426357,
+            "angularVelocity": 1.964001990472943,
+            "velocityX": 0.44626497815605787,
+            "velocityY": 1.3635731698719276,
+            "timestamp": 2.474035953264968
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 2.533782828853193e-32,
+            "angularVelocity": 1.0667775783953661,
+            "velocityX": 0.21644249567844834,
+            "velocityY": 0.6756088508922024,
+            "timestamp": 2.565843505718012
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 2.4912680070874024e-31,
+            "velocityX": 0,
+            "velocityY": -3.645397861822831e-32,
+            "timestamp": 2.657651058171056
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C1.traj
new file mode 100644
index 00000000..f8bd91fc
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C1.traj	
@@ -0,0 +1,292 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 1.1411819845129358e-30,
+            "velocityX": 2.0257584812603577e-32,
+            "velocityY": 3.042151659469263e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 5.128157808100627,
+            "y": 1.6381505638691025,
+            "heading": -0.6351424860741187,
+            "angularVelocity": 0.06199634399134786,
+            "velocityX": -0.6106088487961494,
+            "velocityY": 0.23165524966799825,
+            "timestamp": 0.07835161903352221
+        },
+        {
+            "x": 5.034138141125347,
+            "y": 1.6783506940439978,
+            "heading": -0.625731099901857,
+            "angularVelocity": 0.12011731586854772,
+            "velocityX": -1.199970953185457,
+            "velocityY": 0.5130733821555872,
+            "timestamp": 0.15670323806704442
+        },
+        {
+            "x": 4.897649658060979,
+            "y": 1.7471051706265737,
+            "heading": -0.6123788413002932,
+            "angularVelocity": 0.1704145844880519,
+            "velocityX": -1.74199952404266,
+            "velocityY": 0.8775118808095047,
+            "timestamp": 0.23505485710056662
+        },
+        {
+            "x": 4.7291295198915435,
+            "y": 1.855780594555545,
+            "heading": -0.5966040827715752,
+            "angularVelocity": 0.20133289807283902,
+            "velocityX": -2.150818837544829,
+            "velocityY": 1.3870220586313022,
+            "timestamp": 0.31340647613408884
+        },
+        {
+            "x": 4.559409610033689,
+            "y": 2.0156280635815147,
+            "heading": -0.5822183456142529,
+            "angularVelocity": 0.18360484869071408,
+            "velocityX": -2.1661314973624393,
+            "velocityY": 2.0401297509574126,
+            "timestamp": 0.39175809516761106
+        },
+        {
+            "x": 4.423281260574217,
+            "y": 2.2140742145732073,
+            "heading": -0.5727758831618049,
+            "angularVelocity": 0.1205139417528553,
+            "velocityX": -1.7374031467203144,
+            "velocityY": 2.532763884646578,
+            "timestamp": 0.4701097142011333
+        },
+        {
+            "x": 4.3324586171803325,
+            "y": 2.4362881098121476,
+            "heading": -0.5691499320560302,
+            "angularVelocity": 0.04627793465535804,
+            "velocityX": -1.1591674111421555,
+            "velocityY": 2.8361110846205655,
+            "timestamp": 0.5484613332346555
+        },
+        {
+            "x": 4.290356765831772,
+            "y": 2.674111890908862,
+            "heading": -0.5714790836196856,
+            "angularVelocity": -0.02972691046318988,
+            "velocityX": -0.5373450079001806,
+            "velocityY": 3.0353397164002804,
+            "timestamp": 0.6268129522681777
+        },
+        {
+            "x": 4.298268630722052,
+            "y": 2.9211188280591673,
+            "heading": -0.5947192771376703,
+            "angularVelocity": -0.29661408155511804,
+            "velocityX": 0.1009789585444941,
+            "velocityY": 3.1525441362561364,
+            "timestamp": 0.7051645713016998
+        },
+        {
+            "x": 4.356924477851345,
+            "y": 3.1722484967201265,
+            "heading": -0.6368890523591307,
+            "angularVelocity": -0.5382119188043637,
+            "velocityX": 0.7486232939768359,
+            "velocityY": 3.2051624683532776,
+            "timestamp": 0.783516190335222
+        },
+        {
+            "x": 4.466386404708156,
+            "y": 3.418800428561669,
+            "heading": -0.694172117002664,
+            "angularVelocity": -0.7311025011368958,
+            "velocityX": 1.3970601782967371,
+            "velocityY": 3.1467369134523744,
+            "timestamp": 0.8618678093687442
+        },
+        {
+            "x": 4.622742851027307,
+            "y": 3.644917946871822,
+            "heading": -0.7581397869413499,
+            "angularVelocity": -0.8164179723116849,
+            "velocityX": 1.9955739045067467,
+            "velocityY": 2.8859329404975034,
+            "timestamp": 0.9402194284022664
+        },
+        {
+            "x": 4.8130531116785615,
+            "y": 3.833426042269654,
+            "heading": -0.8482959054101304,
+            "angularVelocity": -1.1506605681014435,
+            "velocityX": 2.4289256941816704,
+            "velocityY": 2.4059246984696085,
+            "timestamp": 1.0185710474357885
+        },
+        {
+            "x": 5.034329891204834,
+            "y": 3.981222629547119,
+            "heading": -0.9454274313666489,
+            "angularVelocity": -1.2396875412997066,
+            "velocityX": 2.824150697276595,
+            "velocityY": 1.8863246107809373,
+            "timestamp": 1.0969226664693108
+        },
+        {
+            "x": 5.3307679727813255,
+            "y": 4.095215339701575,
+            "heading": -1.0325645415374454,
+            "angularVelocity": -0.9491279076997933,
+            "velocityX": 3.2289073573561264,
+            "velocityY": 1.241648503948074,
+            "timestamp": 1.1887302189223545
+        },
+        {
+            "x": 5.6706736122505745,
+            "y": 4.158722341660029,
+            "heading": -1.0366531166779425,
+            "angularVelocity": -0.04453419170049298,
+            "velocityX": 3.7023712144281196,
+            "velocityY": 0.691740496958957,
+            "timestamp": 1.2805377713753983
+        },
+        {
+            "x": 5.985432145120126,
+            "y": 4.2874622186845945,
+            "heading": -1.0150469515971576,
+            "angularVelocity": 0.23534191363870294,
+            "velocityX": 3.4284601262030105,
+            "velocityY": 1.402279807975625,
+            "timestamp": 1.372345323828442
+        },
+        {
+            "x": 6.260473058973373,
+            "y": 4.473603429076733,
+            "heading": -0.9641067919300658,
+            "angularVelocity": 0.5548580515001295,
+            "velocityX": 2.9958419160985765,
+            "velocityY": 2.027515225257119,
+            "timestamp": 1.4641528762814857
+        },
+        {
+            "x": 6.487005993334624,
+            "y": 4.6803674717401,
+            "heading": -0.8309910438969466,
+            "angularVelocity": 1.4499433268434323,
+            "velocityX": 2.4674760224886074,
+            "velocityY": 2.2521463337029823,
+            "timestamp": 1.5559604287345294
+        },
+        {
+            "x": 6.71888754498383,
+            "y": 4.9198952355114685,
+            "heading": -0.7843851972685187,
+            "angularVelocity": 0.507647196588375,
+            "velocityX": 2.5257350343568326,
+            "velocityY": 2.609020253468568,
+            "timestamp": 1.6477679811875732
+        },
+        {
+            "x": 6.906293835583718,
+            "y": 5.2122954621107285,
+            "heading": -0.7843850302000089,
+            "angularVelocity": 0.0000018197686939661552,
+            "velocityX": 2.0412949217411986,
+            "velocityY": 3.1849256274293083,
+            "timestamp": 1.739575533640617
+        },
+        {
+            "x": 7.030998336898649,
+            "y": 5.536437116039629,
+            "heading": -0.7843849409482478,
+            "angularVelocity": 9.721614257680115e-7,
+            "velocityX": 1.3583250830993825,
+            "velocityY": 3.530664365490929,
+            "timestamp": 1.8313830860936606
+        },
+        {
+            "x": 7.139916048661206,
+            "y": 5.86621879705104,
+            "heading": -0.7843848550588985,
+            "angularVelocity": 9.355368597772783e-7,
+            "velocityX": 1.1863698448802928,
+            "velocityY": 3.592097514854049,
+            "timestamp": 1.9231906385467044
+        },
+        {
+            "x": 7.248833621341376,
+            "y": 6.196000523997438,
+            "heading": -0.7843847691695015,
+            "angularVelocity": 9.355373786921671e-7,
+            "velocityX": 1.1863683299462482,
+            "velocityY": 3.592098015194004,
+            "timestamp": 2.0149981909997483
+        },
+        {
+            "x": 7.357751176995467,
+            "y": 6.5257822565670125,
+            "heading": -0.7843846832799689,
+            "angularVelocity": 9.355388566444766e-7,
+            "velocityX": 1.1863681444922285,
+            "velocityY": 3.5920980764436137,
+            "timestamp": 2.1068057434527923
+        },
+        {
+            "x": 7.4669549324183055,
+            "y": 6.834400664448086,
+            "heading": -0.7318192896940902,
+            "angularVelocity": 0.5725606682823186,
+            "velocityX": 1.189485532562175,
+            "velocityY": 3.361579735380924,
+            "timestamp": 2.198613295905836
+        },
+        {
+            "x": 7.555413306559794,
+            "y": 7.089894078912264,
+            "heading": -0.5139853289463429,
+            "angularVelocity": 2.3727237566774426,
+            "velocityX": 0.9635195773978621,
+            "velocityY": 2.782923709842436,
+            "timestamp": 2.29042084835888
+        },
+        {
+            "x": 7.619158448834915,
+            "y": 7.279787689667393,
+            "heading": -0.2782484542424907,
+            "angularVelocity": 2.5677285626846773,
+            "velocityX": 0.6943344046528581,
+            "velocityY": 2.0683876835977384,
+            "timestamp": 2.382228400811924
+        },
+        {
+            "x": 7.660128944224933,
+            "y": 7.404974004983973,
+            "heading": -0.09793823848426357,
+            "angularVelocity": 1.964001990472943,
+            "velocityX": 0.44626497815605787,
+            "velocityY": 1.3635731698719276,
+            "timestamp": 2.474035953264968
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 2.533782828853193e-32,
+            "angularVelocity": 1.0667775783953661,
+            "velocityX": 0.21644249567844834,
+            "velocityY": 0.6756088508922024,
+            "timestamp": 2.565843505718012
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 2.4912680070874024e-31,
+            "velocityX": 0,
+            "velocityY": -3.645397861822831e-32,
+            "timestamp": 2.657651058171056
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3.1.traj
new file mode 100644
index 00000000..4d225c1e
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3.1.traj	
@@ -0,0 +1,238 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 0,
+            "velocityX": 2.653642496841617e-31,
+            "velocityY": -1.2761459725684476e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 5.194122324075795,
+            "y": 1.6116215778070426,
+            "heading": -0.6335243646936302,
+            "angularVelocity": 0.13206571622689564,
+            "velocityX": 0.3695911822570569,
+            "velocityY": -0.1708716250073132,
+            "timestamp": 0.049033431926404414
+        },
+        {
+            "x": 5.2306889078335095,
+            "y": 1.5955898422898989,
+            "heading": -0.6207226842720189,
+            "angularVelocity": 0.2610806529090014,
+            "velocityX": 0.745747999295838,
+            "velocityY": -0.3269551994890449,
+            "timestamp": 0.09806686385280883
+        },
+        {
+            "x": 5.286073830286703,
+            "y": 1.5728623577205654,
+            "heading": -0.6018022859553105,
+            "angularVelocity": 0.38586730672362957,
+            "velocityX": 1.1295338767299938,
+            "velocityY": -0.4635099701654054,
+            "timestamp": 0.14710029577921324
+        },
+        {
+            "x": 5.360700050095615,
+            "y": 1.5447484727758143,
+            "heading": -0.577064216904969,
+            "angularVelocity": 0.5045143298894789,
+            "velocityX": 1.5219456782249914,
+            "velocityY": -0.5733615584348081,
+            "timestamp": 0.19613372770561766
+        },
+        {
+            "x": 5.45500431506412,
+            "y": 1.513111103841015,
+            "heading": -0.5469692052395991,
+            "angularVelocity": 0.6137651492712723,
+            "velocityX": 1.9232646230039372,
+            "velocityY": -0.6452203668359155,
+            "timestamp": 0.24516715963202207
+        },
+        {
+            "x": 5.5692993756365,
+            "y": 1.480706991826158,
+            "heading": -0.5122625268421303,
+            "angularVelocity": 0.7078166270271339,
+            "velocityX": 2.330961878090668,
+            "velocityY": -0.6608575158164777,
+            "timestamp": 0.2942005915584265
+        },
+        {
+            "x": 5.703331364670941,
+            "y": 1.4516882815399292,
+            "heading": -0.47420565589215874,
+            "angularVelocity": 0.7761412867722233,
+            "velocityX": 2.733481703578104,
+            "velocityY": -0.5918147913809916,
+            "timestamp": 0.3432340234848309
+        },
+        {
+            "x": 5.855159622936256,
+            "y": 1.4318923943368613,
+            "heading": -0.4349045247507558,
+            "angularVelocity": 0.8015170384282412,
+            "velocityX": 3.096423242273411,
+            "velocityY": -0.40372224470801127,
+            "timestamp": 0.3922674554112353
+        },
+        {
+            "x": 6.019815732483991,
+            "y": 1.4274954608427641,
+            "heading": -0.3972296573283927,
+            "angularVelocity": 0.768350611865634,
+            "velocityX": 3.358037630221609,
+            "velocityY": -0.08967215471871418,
+            "timestamp": 0.4413008873376397
+        },
+        {
+            "x": 6.190507691154086,
+            "y": 1.4421792499476667,
+            "heading": -0.36375877276285506,
+            "angularVelocity": 0.6826135404074718,
+            "velocityX": 3.4811342376834093,
+            "velocityY": 0.2994648452702519,
+            "timestamp": 0.49033431926404414
+        },
+        {
+            "x": 6.361514712702491,
+            "y": 1.4768435219754552,
+            "heading": -0.33601787986089265,
+            "angularVelocity": 0.5657546659914753,
+            "velocityX": 3.487559708345244,
+            "velocityY": 0.7069517809772359,
+            "timestamp": 0.5393677511904486
+        },
+        {
+            "x": 6.529238224029541,
+            "y": 1.531233787536621,
+            "heading": -0.3133159497925571,
+            "angularVelocity": 0.4629888052692109,
+            "velocityX": 3.4205949846286416,
+            "velocityY": 1.1092485968081267,
+            "timestamp": 0.588401183116853
+        },
+        {
+            "x": 6.794330666624461,
+            "y": 1.6764087741891647,
+            "heading": -0.29089151826229404,
+            "angularVelocity": 0.27241251742532613,
+            "velocityX": 3.2203491776466797,
+            "velocityY": 1.7635891250039288,
+            "timestamp": 0.6707191020664587
+        },
+        {
+            "x": 7.034738463760469,
+            "y": 1.8721289187979775,
+            "heading": -0.28740556521383775,
+            "angularVelocity": 0.042347438965477144,
+            "velocityX": 2.9204795286821774,
+            "velocityY": 2.377612883150898,
+            "timestamp": 0.7530370210160644
+        },
+        {
+            "x": 7.236654527584481,
+            "y": 2.109198968129657,
+            "heading": -0.2874055261243408,
+            "angularVelocity": 4.748601214403234e-7,
+            "velocityX": 2.452881054338489,
+            "velocityY": 2.879932490479633,
+            "timestamp": 0.8353549399656701
+        },
+        {
+            "x": 7.392410351066591,
+            "y": 2.3788514084218195,
+            "heading": -0.2874055078665042,
+            "angularVelocity": 2.2179662469221753e-7,
+            "velocityX": 1.8921253776776576,
+            "velocityY": 3.2757441360636306,
+            "timestamp": 0.9176728589152758
+        },
+        {
+            "x": 7.49687844031107,
+            "y": 2.6722091028580075,
+            "heading": -0.28740549372573876,
+            "angularVelocity": 1.7178234776742224e-7,
+            "velocityX": 1.2690807855393922,
+            "velocityY": 3.5637161164844247,
+            "timestamp": 0.9999907778648816
+        },
+        {
+            "x": 7.546620338444338,
+            "y": 2.979614475892834,
+            "heading": -0.28740548096651763,
+            "angularVelocity": 1.5499931641741705e-7,
+            "velocityX": 0.6042657390764874,
+            "velocityY": 3.7343676438541644,
+            "timestamp": 1.0823086968144873
+        },
+        {
+            "x": 7.582493232504898,
+            "y": 3.288945132948743,
+            "heading": -0.2874054683444637,
+            "angularVelocity": 1.5333300623146833e-7,
+            "velocityX": 0.43578475401492967,
+            "velocityY": 3.7577560390622633,
+            "timestamp": 1.164626615764093
+        },
+        {
+            "x": 7.6150468277369106,
+            "y": 3.5683420563080905,
+            "heading": -0.1936159509616472,
+            "angularVelocity": 1.1393572454157397,
+            "velocityX": 0.39546183440153926,
+            "velocityY": 3.3941203437178378,
+            "timestamp": 1.2469445347136987
+        },
+        {
+            "x": 7.641044736890772,
+            "y": 3.791820131624945,
+            "heading": -0.11680359430817472,
+            "angularVelocity": 0.9331183007718732,
+            "velocityX": 0.31582320697043825,
+            "velocityY": 2.714816873027222,
+            "timestamp": 1.3292624536633044
+        },
+        {
+            "x": 7.660527962829294,
+            "y": 3.959415202649684,
+            "heading": -0.05861701943494889,
+            "angularVelocity": 0.7068518691380767,
+            "velocityX": 0.23668268327338735,
+            "velocityY": 2.035948833052459,
+            "timestamp": 1.4115803726129101
+        },
+        {
+            "x": 7.673510674167818,
+            "y": 4.0711397209924485,
+            "heading": -0.019593791766857216,
+            "angularVelocity": 0.4740550801809447,
+            "velocityX": 0.1577142802460941,
+            "velocityY": 1.3572320555281903,
+            "timestamp": 1.4938982915625159
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -2.071552209824535e-33,
+            "angularVelocity": 0.23802583953553896,
+            "velocityX": 0.07883248161483059,
+            "velocityY": 0.6785919726876813,
+            "timestamp": 1.5762162105121216
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": -4.785738309812717e-31,
+            "velocityX": -1.2484093100405624e-30,
+            "velocityY": 4.284452606087597e-31,
+            "timestamp": 1.6585341294617273
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3.traj
new file mode 100644
index 00000000..4d225c1e
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3.traj	
@@ -0,0 +1,238 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 0,
+            "velocityX": 2.653642496841617e-31,
+            "velocityY": -1.2761459725684476e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 5.194122324075795,
+            "y": 1.6116215778070426,
+            "heading": -0.6335243646936302,
+            "angularVelocity": 0.13206571622689564,
+            "velocityX": 0.3695911822570569,
+            "velocityY": -0.1708716250073132,
+            "timestamp": 0.049033431926404414
+        },
+        {
+            "x": 5.2306889078335095,
+            "y": 1.5955898422898989,
+            "heading": -0.6207226842720189,
+            "angularVelocity": 0.2610806529090014,
+            "velocityX": 0.745747999295838,
+            "velocityY": -0.3269551994890449,
+            "timestamp": 0.09806686385280883
+        },
+        {
+            "x": 5.286073830286703,
+            "y": 1.5728623577205654,
+            "heading": -0.6018022859553105,
+            "angularVelocity": 0.38586730672362957,
+            "velocityX": 1.1295338767299938,
+            "velocityY": -0.4635099701654054,
+            "timestamp": 0.14710029577921324
+        },
+        {
+            "x": 5.360700050095615,
+            "y": 1.5447484727758143,
+            "heading": -0.577064216904969,
+            "angularVelocity": 0.5045143298894789,
+            "velocityX": 1.5219456782249914,
+            "velocityY": -0.5733615584348081,
+            "timestamp": 0.19613372770561766
+        },
+        {
+            "x": 5.45500431506412,
+            "y": 1.513111103841015,
+            "heading": -0.5469692052395991,
+            "angularVelocity": 0.6137651492712723,
+            "velocityX": 1.9232646230039372,
+            "velocityY": -0.6452203668359155,
+            "timestamp": 0.24516715963202207
+        },
+        {
+            "x": 5.5692993756365,
+            "y": 1.480706991826158,
+            "heading": -0.5122625268421303,
+            "angularVelocity": 0.7078166270271339,
+            "velocityX": 2.330961878090668,
+            "velocityY": -0.6608575158164777,
+            "timestamp": 0.2942005915584265
+        },
+        {
+            "x": 5.703331364670941,
+            "y": 1.4516882815399292,
+            "heading": -0.47420565589215874,
+            "angularVelocity": 0.7761412867722233,
+            "velocityX": 2.733481703578104,
+            "velocityY": -0.5918147913809916,
+            "timestamp": 0.3432340234848309
+        },
+        {
+            "x": 5.855159622936256,
+            "y": 1.4318923943368613,
+            "heading": -0.4349045247507558,
+            "angularVelocity": 0.8015170384282412,
+            "velocityX": 3.096423242273411,
+            "velocityY": -0.40372224470801127,
+            "timestamp": 0.3922674554112353
+        },
+        {
+            "x": 6.019815732483991,
+            "y": 1.4274954608427641,
+            "heading": -0.3972296573283927,
+            "angularVelocity": 0.768350611865634,
+            "velocityX": 3.358037630221609,
+            "velocityY": -0.08967215471871418,
+            "timestamp": 0.4413008873376397
+        },
+        {
+            "x": 6.190507691154086,
+            "y": 1.4421792499476667,
+            "heading": -0.36375877276285506,
+            "angularVelocity": 0.6826135404074718,
+            "velocityX": 3.4811342376834093,
+            "velocityY": 0.2994648452702519,
+            "timestamp": 0.49033431926404414
+        },
+        {
+            "x": 6.361514712702491,
+            "y": 1.4768435219754552,
+            "heading": -0.33601787986089265,
+            "angularVelocity": 0.5657546659914753,
+            "velocityX": 3.487559708345244,
+            "velocityY": 0.7069517809772359,
+            "timestamp": 0.5393677511904486
+        },
+        {
+            "x": 6.529238224029541,
+            "y": 1.531233787536621,
+            "heading": -0.3133159497925571,
+            "angularVelocity": 0.4629888052692109,
+            "velocityX": 3.4205949846286416,
+            "velocityY": 1.1092485968081267,
+            "timestamp": 0.588401183116853
+        },
+        {
+            "x": 6.794330666624461,
+            "y": 1.6764087741891647,
+            "heading": -0.29089151826229404,
+            "angularVelocity": 0.27241251742532613,
+            "velocityX": 3.2203491776466797,
+            "velocityY": 1.7635891250039288,
+            "timestamp": 0.6707191020664587
+        },
+        {
+            "x": 7.034738463760469,
+            "y": 1.8721289187979775,
+            "heading": -0.28740556521383775,
+            "angularVelocity": 0.042347438965477144,
+            "velocityX": 2.9204795286821774,
+            "velocityY": 2.377612883150898,
+            "timestamp": 0.7530370210160644
+        },
+        {
+            "x": 7.236654527584481,
+            "y": 2.109198968129657,
+            "heading": -0.2874055261243408,
+            "angularVelocity": 4.748601214403234e-7,
+            "velocityX": 2.452881054338489,
+            "velocityY": 2.879932490479633,
+            "timestamp": 0.8353549399656701
+        },
+        {
+            "x": 7.392410351066591,
+            "y": 2.3788514084218195,
+            "heading": -0.2874055078665042,
+            "angularVelocity": 2.2179662469221753e-7,
+            "velocityX": 1.8921253776776576,
+            "velocityY": 3.2757441360636306,
+            "timestamp": 0.9176728589152758
+        },
+        {
+            "x": 7.49687844031107,
+            "y": 2.6722091028580075,
+            "heading": -0.28740549372573876,
+            "angularVelocity": 1.7178234776742224e-7,
+            "velocityX": 1.2690807855393922,
+            "velocityY": 3.5637161164844247,
+            "timestamp": 0.9999907778648816
+        },
+        {
+            "x": 7.546620338444338,
+            "y": 2.979614475892834,
+            "heading": -0.28740548096651763,
+            "angularVelocity": 1.5499931641741705e-7,
+            "velocityX": 0.6042657390764874,
+            "velocityY": 3.7343676438541644,
+            "timestamp": 1.0823086968144873
+        },
+        {
+            "x": 7.582493232504898,
+            "y": 3.288945132948743,
+            "heading": -0.2874054683444637,
+            "angularVelocity": 1.5333300623146833e-7,
+            "velocityX": 0.43578475401492967,
+            "velocityY": 3.7577560390622633,
+            "timestamp": 1.164626615764093
+        },
+        {
+            "x": 7.6150468277369106,
+            "y": 3.5683420563080905,
+            "heading": -0.1936159509616472,
+            "angularVelocity": 1.1393572454157397,
+            "velocityX": 0.39546183440153926,
+            "velocityY": 3.3941203437178378,
+            "timestamp": 1.2469445347136987
+        },
+        {
+            "x": 7.641044736890772,
+            "y": 3.791820131624945,
+            "heading": -0.11680359430817472,
+            "angularVelocity": 0.9331183007718732,
+            "velocityX": 0.31582320697043825,
+            "velocityY": 2.714816873027222,
+            "timestamp": 1.3292624536633044
+        },
+        {
+            "x": 7.660527962829294,
+            "y": 3.959415202649684,
+            "heading": -0.05861701943494889,
+            "angularVelocity": 0.7068518691380767,
+            "velocityX": 0.23668268327338735,
+            "velocityY": 2.035948833052459,
+            "timestamp": 1.4115803726129101
+        },
+        {
+            "x": 7.673510674167818,
+            "y": 4.0711397209924485,
+            "heading": -0.019593791766857216,
+            "angularVelocity": 0.4740550801809447,
+            "velocityX": 0.1577142802460941,
+            "velocityY": 1.3572320555281903,
+            "timestamp": 1.4938982915625159
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -2.071552209824535e-33,
+            "angularVelocity": 0.23802583953553896,
+            "velocityX": 0.07883248161483059,
+            "velocityY": 0.6785919726876813,
+            "timestamp": 1.5762162105121216
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": -4.785738309812717e-31,
+            "velocityX": -1.2484093100405624e-30,
+            "velocityY": 4.284452606087597e-31,
+            "timestamp": 1.6585341294617273
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.1.traj
new file mode 100644
index 00000000..751754fd
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.222401190948388,
+            "y": 1.635609178560311,
+            "heading": -0.6120137040899031,
+            "angularVelocity": 0.36292240482976423,
+            "velocityX": 0.6017242103081577,
+            "velocityY": 0.20241766322785323,
+            "timestamp": 0.07711371780208913
+        },
+        {
+            "x": 5.315327100770923,
+            "y": 1.6668589219810437,
+            "heading": -0.5590312951883875,
+            "angularVelocity": 0.6870685321838068,
+            "velocityX": 1.205050313629373,
+            "velocityY": 0.4052423396435706,
+            "timestamp": 0.15422743560417826
+        },
+        {
+            "x": 5.454938302762681,
+            "y": 1.713800132384211,
+            "heading": -0.4856728473163301,
+            "angularVelocity": 0.9513021802467544,
+            "velocityX": 1.8104587091763231,
+            "velocityY": 0.6087271077195515,
+            "timestamp": 0.23134115340626737
+        },
+        {
+            "x": 5.641435450684747,
+            "y": 1.7765099385015581,
+            "heading": -0.4002091399727602,
+            "angularVelocity": 1.1082815065785667,
+            "velocityX": 2.418469154874715,
+            "velocityY": 0.8132120704942623,
+            "timestamp": 0.3084548712083565
+        },
+        {
+            "x": 5.8748863814192855,
+            "y": 1.8550365409977465,
+            "heading": -0.32154781798640775,
+            "angularVelocity": 1.0200691164734093,
+            "velocityX": 3.0273592998548686,
+            "velocityY": 1.0183220928048784,
+            "timestamp": 0.38556858901044566
+        },
+        {
+            "x": 6.151206632813102,
+            "y": 1.945953456728306,
+            "heading": -0.3195100296755088,
+            "angularVelocity": 0.026425756259478697,
+            "velocityX": 3.583282705976986,
+            "velocityY": 1.178997956808355,
+            "timestamp": 0.4626823068125348
+        },
+        {
+            "x": 6.4277763897122595,
+            "y": 2.0387313847088295,
+            "heading": -0.31950999788997536,
+            "angularVelocity": 4.121903903341971e-7,
+            "velocityX": 3.5865182587742526,
+            "velocityY": 1.2031313056210848,
+            "timestamp": 0.539796024614624
+        },
+        {
+            "x": 6.7043461469693755,
+            "y": 2.1315093116222874,
+            "heading": -0.31950996610444216,
+            "angularVelocity": 4.121903870025697e-7,
+            "velocityX": 3.5865182634161936,
+            "velocityY": 1.2031312917835268,
+            "timestamp": 0.616909742416713
+        },
+        {
+            "x": 6.980831721332989,
+            "y": 2.2223162260189957,
+            "heading": -0.31777519026878787,
+            "angularVelocity": 0.022496332495664188,
+            "velocityX": 3.5854265913259273,
+            "velocityY": 1.1775714747635835,
+            "timestamp": 0.6940234602188021
+        },
+        {
+            "x": 7.214355031558839,
+            "y": 2.3007720927806434,
+            "heading": -0.23870969056825803,
+            "angularVelocity": 1.0253104370281303,
+            "velocityX": 3.028297907062184,
+            "velocityY": 1.0174048016074528,
+            "timestamp": 0.7711371780208912
+        },
+        {
+            "x": 7.400917361819277,
+            "y": 2.3634042679352882,
+            "heading": -0.15373574527352316,
+            "angularVelocity": 1.1019303402387481,
+            "velocityX": 2.419314430400663,
+            "velocityY": 0.8122053629341165,
+            "timestamp": 0.8482508958229803
+        },
+        {
+            "x": 7.540592119323217,
+            "y": 2.4102654434053137,
+            "heading": -0.08076255152981597,
+            "angularVelocity": 0.9463062581289249,
+            "velocityX": 1.8112828882458099,
+            "velocityY": 0.6076892258040765,
+            "timestamp": 0.9253646136250694
+        },
+        {
+            "x": 7.633568850123268,
+            "y": 2.4414404537843537,
+            "heading": -0.02793953661151031,
+            "angularVelocity": 0.6850015331107969,
+            "velocityX": 1.2057093530190521,
+            "velocityY": 0.4042732119212608,
+            "timestamp": 1.0024783314271586
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -3.3818227179407965e-32,
+            "angularVelocity": 0.3623160367292381,
+            "velocityX": 0.6021127135368662,
+            "velocityY": 0.20177403786417583,
+            "timestamp": 1.0795920492292477
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -1.7311819232040874e-32,
+            "angularVelocity": 2.140534191184349e-31,
+            "velocityX": 0,
+            "velocityY": -2.0522160776390996e-31,
+            "timestamp": 1.1567057670313368
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.traj
new file mode 100644
index 00000000..751754fd
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.222401190948388,
+            "y": 1.635609178560311,
+            "heading": -0.6120137040899031,
+            "angularVelocity": 0.36292240482976423,
+            "velocityX": 0.6017242103081577,
+            "velocityY": 0.20241766322785323,
+            "timestamp": 0.07711371780208913
+        },
+        {
+            "x": 5.315327100770923,
+            "y": 1.6668589219810437,
+            "heading": -0.5590312951883875,
+            "angularVelocity": 0.6870685321838068,
+            "velocityX": 1.205050313629373,
+            "velocityY": 0.4052423396435706,
+            "timestamp": 0.15422743560417826
+        },
+        {
+            "x": 5.454938302762681,
+            "y": 1.713800132384211,
+            "heading": -0.4856728473163301,
+            "angularVelocity": 0.9513021802467544,
+            "velocityX": 1.8104587091763231,
+            "velocityY": 0.6087271077195515,
+            "timestamp": 0.23134115340626737
+        },
+        {
+            "x": 5.641435450684747,
+            "y": 1.7765099385015581,
+            "heading": -0.4002091399727602,
+            "angularVelocity": 1.1082815065785667,
+            "velocityX": 2.418469154874715,
+            "velocityY": 0.8132120704942623,
+            "timestamp": 0.3084548712083565
+        },
+        {
+            "x": 5.8748863814192855,
+            "y": 1.8550365409977465,
+            "heading": -0.32154781798640775,
+            "angularVelocity": 1.0200691164734093,
+            "velocityX": 3.0273592998548686,
+            "velocityY": 1.0183220928048784,
+            "timestamp": 0.38556858901044566
+        },
+        {
+            "x": 6.151206632813102,
+            "y": 1.945953456728306,
+            "heading": -0.3195100296755088,
+            "angularVelocity": 0.026425756259478697,
+            "velocityX": 3.583282705976986,
+            "velocityY": 1.178997956808355,
+            "timestamp": 0.4626823068125348
+        },
+        {
+            "x": 6.4277763897122595,
+            "y": 2.0387313847088295,
+            "heading": -0.31950999788997536,
+            "angularVelocity": 4.121903903341971e-7,
+            "velocityX": 3.5865182587742526,
+            "velocityY": 1.2031313056210848,
+            "timestamp": 0.539796024614624
+        },
+        {
+            "x": 6.7043461469693755,
+            "y": 2.1315093116222874,
+            "heading": -0.31950996610444216,
+            "angularVelocity": 4.121903870025697e-7,
+            "velocityX": 3.5865182634161936,
+            "velocityY": 1.2031312917835268,
+            "timestamp": 0.616909742416713
+        },
+        {
+            "x": 6.980831721332989,
+            "y": 2.2223162260189957,
+            "heading": -0.31777519026878787,
+            "angularVelocity": 0.022496332495664188,
+            "velocityX": 3.5854265913259273,
+            "velocityY": 1.1775714747635835,
+            "timestamp": 0.6940234602188021
+        },
+        {
+            "x": 7.214355031558839,
+            "y": 2.3007720927806434,
+            "heading": -0.23870969056825803,
+            "angularVelocity": 1.0253104370281303,
+            "velocityX": 3.028297907062184,
+            "velocityY": 1.0174048016074528,
+            "timestamp": 0.7711371780208912
+        },
+        {
+            "x": 7.400917361819277,
+            "y": 2.3634042679352882,
+            "heading": -0.15373574527352316,
+            "angularVelocity": 1.1019303402387481,
+            "velocityX": 2.419314430400663,
+            "velocityY": 0.8122053629341165,
+            "timestamp": 0.8482508958229803
+        },
+        {
+            "x": 7.540592119323217,
+            "y": 2.4102654434053137,
+            "heading": -0.08076255152981597,
+            "angularVelocity": 0.9463062581289249,
+            "velocityX": 1.8112828882458099,
+            "velocityY": 0.6076892258040765,
+            "timestamp": 0.9253646136250694
+        },
+        {
+            "x": 7.633568850123268,
+            "y": 2.4414404537843537,
+            "heading": -0.02793953661151031,
+            "angularVelocity": 0.6850015331107969,
+            "velocityX": 1.2057093530190521,
+            "velocityY": 0.4042732119212608,
+            "timestamp": 1.0024783314271586
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -3.3818227179407965e-32,
+            "angularVelocity": 0.3623160367292381,
+            "velocityX": 0.6021127135368662,
+            "velocityY": 0.20177403786417583,
+            "timestamp": 1.0795920492292477
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -1.7311819232040874e-32,
+            "angularVelocity": 2.140534191184349e-31,
+            "velocityX": 0,
+            "velocityY": -2.0522160776390996e-31,
+            "timestamp": 1.1567057670313368
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.1.traj
index 10168382..0bd0e944 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.1.traj	
@@ -5,144 +5,144 @@
             "y": 1.62,
             "heading": -0.64,
             "angularVelocity": 0,
-            "velocityX": -3.378617665126574e-35,
-            "velocityY": 1.1970750708579272e-32,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 5.2224556017723405,
-            "y": 1.6045081405552968,
-            "heading": -0.6133597294207822,
-            "angularVelocity": 0.3456555725750699,
-            "velocityX": 0.6027580531582283,
-            "velocityY": -0.20100574919793807,
-            "timestamp": 0.07707172310503405
+            "x": 5.22245562506892,
+            "y": 1.6045082039129626,
+            "heading": -0.6133597798196372,
+            "angularVelocity": 0.3456549186542876,
+            "velocityX": 0.6027583554308628,
+            "velocityY": -0.20100492713723658,
+            "timestamp": 0.07707172310487943
         },
         {
-            "x": 5.3154629271275695,
-            "y": 1.5735046785143454,
-            "heading": -0.5625081883046615,
-            "angularVelocity": 0.6597950463209403,
-            "velocityX": 1.2067632798150456,
-            "velocityY": -0.4022676643507709,
-            "timestamp": 0.1541434462100681
+            "x": 5.31546298986369,
+            "y": 1.573504853521539,
+            "heading": -0.5625082909022232,
+            "angularVelocity": 0.6597943690478437,
+            "velocityX": 1.2067637915426601,
+            "velocityY": -0.4022662157070755,
+            "timestamp": 0.15414344620975887
         },
         {
-            "x": 5.455153059548346,
-            "y": 1.5269582787185583,
-            "heading": -0.4912835813545399,
-            "angularVelocity": 0.9241340932919854,
-            "velocityX": 1.812469305122525,
-            "velocityY": -0.6039361509065161,
-            "timestamp": 0.23121516931510214
+            "x": 5.455153171011431,
+            "y": 1.526958597742617,
+            "heading": -0.4912836919139272,
+            "angularVelocity": 0.924133989989731,
+            "velocityX": 1.8124699373549893,
+            "velocityY": -0.6039342822993798,
+            "timestamp": 0.2312151693146383
         },
         {
-            "x": 5.641700996428857,
-            "y": 1.4648196222358638,
-            "heading": -0.40650985692768693,
-            "angularVelocity": 1.0999329067980297,
-            "velocityX": 2.4204459088877828,
-            "velocityY": -0.8062445470177368,
-            "timestamp": 0.3082868924201362
+            "x": 5.641701159953251,
+            "y": 1.4648201004985182,
+            "heading": -0.406509898431932,
+            "angularVelocity": 1.0999338027856826,
+            "velocityX": 2.420446584384324,
+            "velocityY": -0.8062424809101578,
+            "timestamp": 0.30828689241951773
         },
         {
-            "x": 5.875244676488674,
-            "y": 1.3870501062573488,
-            "heading": -0.3232929818469195,
-            "angularVelocity": 1.0797328997998226,
-            "velocityX": 3.0302122575038553,
-            "velocityY": -1.0090538117660863,
-            "timestamp": 0.38535861552517026
+            "x": 5.875244890448534,
+            "y": 1.3870507382512116,
+            "heading": -0.323292875073049,
+            "angularVelocity": 1.0797348236997513,
+            "velocityX": 3.03021291190643,
+            "velocityY": -1.0090518171168696,
+            "timestamp": 0.38535861552439715
         },
         {
-            "x": 6.151314773692177,
-            "y": 1.2951594028189055,
-            "heading": -0.3212316148216143,
-            "angularVelocity": 0.026746087180327376,
-            "velocityX": 3.581989425969788,
-            "velocityY": -1.1922751916836483,
-            "timestamp": 0.46243033863020433
+            "x": 6.151315012657669,
+            "y": 1.295160148354896,
+            "heading": -0.32123146568205607,
+            "angularVelocity": 0.026746636872095383,
+            "velocityX": 3.5819897504232276,
+            "velocityY": -1.192273718485189,
+            "timestamp": 0.46243033862927657
         },
         {
-            "x": 6.427941619479976,
-            "y": 1.2030534747397732,
-            "heading": -0.3212315858589365,
-            "angularVelocity": 3.7578863628089786e-7,
-            "velocityX": 3.5892131983452513,
-            "velocityY": -1.1950677157381002,
-            "timestamp": 0.5395020617352384
+            "x": 6.427941847872596,
+            "y": 1.2030541885235448,
+            "heading": -0.3212314367193528,
+            "angularVelocity": 3.757889680371681e-7,
+            "velocityX": 3.5892130611702004,
+            "velocityY": -1.1950681277232245,
+            "timestamp": 0.539502061734156
         },
         {
-            "x": 6.704568483880494,
-            "y": 1.1109476025613012,
-            "heading": -0.3212315568962456,
-            "angularVelocity": 3.75788807477423e-7,
-            "velocityX": 3.5892134398439306,
-            "velocityY": -1.195066990431097,
-            "timestamp": 0.6165737848402725
+            "x": 6.704568684105482,
+            "y": 1.1109482317494896,
+            "heading": -0.3212314077566493,
+            "angularVelocity": 3.7578897090221527e-7,
+            "velocityX": 3.589213074378161,
+            "velocityY": -1.1950680880550335,
+            "timestamp": 0.6165737848390355
         },
         {
-            "x": 6.980707555601848,
-            "y": 1.0195121848796085,
-            "heading": -0.31890263703438493,
-            "angularVelocity": 0.03021756576905414,
-            "velocityX": 3.582884365320709,
-            "velocityY": -1.186367892113736,
-            "timestamp": 0.6936455079453065
+            "x": 6.9807077062893965,
+            "y": 1.0195126541212949,
+            "heading": -0.3189025007582472,
+            "angularVelocity": 0.030217398866675224,
+            "velocityX": 3.582883722583232,
+            "velocityY": -1.1863699674103443,
+            "timestamp": 0.6936455079439149
         },
         {
-            "x": 7.214256878757648,
-            "y": 0.9418349206236948,
-            "heading": -0.2350677354758115,
-            "angularVelocity": 1.0877517483853638,
-            "velocityX": 3.030285476263674,
-            "velocityY": -1.0078568523770295,
-            "timestamp": 0.7707172310503406
+            "x": 7.214256981896637,
+            "y": 0.9418352430707998,
+            "heading": -0.2350676331360113,
+            "angularVelocity": 1.0877513080660361,
+            "velocityX": 3.0302848593306506,
+            "velocityY": -1.0078587570280646,
+            "timestamp": 0.7707172310487943
         },
         {
-            "x": 7.4008131329346885,
-            "y": 0.8797967164596704,
-            "heading": -0.14969694055158067,
-            "angularVelocity": 1.1076798530621503,
-            "velocityX": 2.420553825205123,
-            "velocityY": -0.8049411854913137,
-            "timestamp": 0.8477889541553747
+            "x": 7.4008131962231865,
+            "y": 0.8797969152412516,
+            "heading": -0.14969687190170503,
+            "angularVelocity": 1.1076794159400525,
+            "velocityX": 2.4205533081527695,
+            "velocityY": -0.8049427900441033,
+            "timestamp": 0.8477889541536737
         },
         {
-            "x": 7.540515620024687,
-            "y": 0.8333523210527743,
-            "heading": -0.07802156861665294,
-            "angularVelocity": 0.9299827361748209,
-            "velocityX": 1.8126296060568263,
-            "velocityY": -0.6026126513818976,
-            "timestamp": 0.9248606772604088
+            "x": 7.54051565231881,
+            "y": 0.8333524229780926,
+            "heading": -0.0780215306756512,
+            "angularVelocity": 0.9299823377312819,
+            "velocityX": 1.812629203910703,
+            "velocityY": -0.6026139080860716,
+            "timestamp": 0.9248606772585531
         },
         {
-            "x": 7.633536076900545,
-            "y": 0.8024373784171116,
-            "heading": -0.026843164489729465,
-            "angularVelocity": 0.6640360701054664,
-            "velocityX": 1.2069336603398442,
-            "velocityY": -0.40111913150730505,
-            "timestamp": 1.0019324003654428
+            "x": 7.633536087871107,
+            "y": 0.802437413208921,
+            "heading": -0.026843150662094223,
+            "angularVelocity": 0.6640357572376224,
+            "velocityX": 1.2069333836706095,
+            "velocityY": -0.401120002560503,
+            "timestamp": 1.0019324003634327
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 1.4987800218197315e-33,
-            "angularVelocity": 0.3482881063025846,
-            "velocityX": 0.6028660217721296,
-            "velocityY": -0.20029886182865572,
-            "timestamp": 1.0790041234704768
+            "heading": 3.8658238709310755e-33,
+            "angularVelocity": 0.3482879268907221,
+            "velocityX": 0.6028658794310917,
+            "velocityY": -0.20029931325025216,
+            "timestamp": 1.079004123468312
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -8.136191152722898e-32,
-            "velocityX": 4.4028618628466005e-33,
-            "velocityY": 0,
-            "timestamp": 1.1560758465755108
+            "heading": 4.381697144346615e-34,
+            "angularVelocity": -4.4473487408728983e-32,
+            "velocityX": 0,
+            "velocityY": -1.5918634852021244e-32,
+            "timestamp": 1.1560758465731915
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.traj
index 10168382..0bd0e944 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.traj	
@@ -5,144 +5,144 @@
             "y": 1.62,
             "heading": -0.64,
             "angularVelocity": 0,
-            "velocityX": -3.378617665126574e-35,
-            "velocityY": 1.1970750708579272e-32,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 5.2224556017723405,
-            "y": 1.6045081405552968,
-            "heading": -0.6133597294207822,
-            "angularVelocity": 0.3456555725750699,
-            "velocityX": 0.6027580531582283,
-            "velocityY": -0.20100574919793807,
-            "timestamp": 0.07707172310503405
+            "x": 5.22245562506892,
+            "y": 1.6045082039129626,
+            "heading": -0.6133597798196372,
+            "angularVelocity": 0.3456549186542876,
+            "velocityX": 0.6027583554308628,
+            "velocityY": -0.20100492713723658,
+            "timestamp": 0.07707172310487943
         },
         {
-            "x": 5.3154629271275695,
-            "y": 1.5735046785143454,
-            "heading": -0.5625081883046615,
-            "angularVelocity": 0.6597950463209403,
-            "velocityX": 1.2067632798150456,
-            "velocityY": -0.4022676643507709,
-            "timestamp": 0.1541434462100681
+            "x": 5.31546298986369,
+            "y": 1.573504853521539,
+            "heading": -0.5625082909022232,
+            "angularVelocity": 0.6597943690478437,
+            "velocityX": 1.2067637915426601,
+            "velocityY": -0.4022662157070755,
+            "timestamp": 0.15414344620975887
         },
         {
-            "x": 5.455153059548346,
-            "y": 1.5269582787185583,
-            "heading": -0.4912835813545399,
-            "angularVelocity": 0.9241340932919854,
-            "velocityX": 1.812469305122525,
-            "velocityY": -0.6039361509065161,
-            "timestamp": 0.23121516931510214
+            "x": 5.455153171011431,
+            "y": 1.526958597742617,
+            "heading": -0.4912836919139272,
+            "angularVelocity": 0.924133989989731,
+            "velocityX": 1.8124699373549893,
+            "velocityY": -0.6039342822993798,
+            "timestamp": 0.2312151693146383
         },
         {
-            "x": 5.641700996428857,
-            "y": 1.4648196222358638,
-            "heading": -0.40650985692768693,
-            "angularVelocity": 1.0999329067980297,
-            "velocityX": 2.4204459088877828,
-            "velocityY": -0.8062445470177368,
-            "timestamp": 0.3082868924201362
+            "x": 5.641701159953251,
+            "y": 1.4648201004985182,
+            "heading": -0.406509898431932,
+            "angularVelocity": 1.0999338027856826,
+            "velocityX": 2.420446584384324,
+            "velocityY": -0.8062424809101578,
+            "timestamp": 0.30828689241951773
         },
         {
-            "x": 5.875244676488674,
-            "y": 1.3870501062573488,
-            "heading": -0.3232929818469195,
-            "angularVelocity": 1.0797328997998226,
-            "velocityX": 3.0302122575038553,
-            "velocityY": -1.0090538117660863,
-            "timestamp": 0.38535861552517026
+            "x": 5.875244890448534,
+            "y": 1.3870507382512116,
+            "heading": -0.323292875073049,
+            "angularVelocity": 1.0797348236997513,
+            "velocityX": 3.03021291190643,
+            "velocityY": -1.0090518171168696,
+            "timestamp": 0.38535861552439715
         },
         {
-            "x": 6.151314773692177,
-            "y": 1.2951594028189055,
-            "heading": -0.3212316148216143,
-            "angularVelocity": 0.026746087180327376,
-            "velocityX": 3.581989425969788,
-            "velocityY": -1.1922751916836483,
-            "timestamp": 0.46243033863020433
+            "x": 6.151315012657669,
+            "y": 1.295160148354896,
+            "heading": -0.32123146568205607,
+            "angularVelocity": 0.026746636872095383,
+            "velocityX": 3.5819897504232276,
+            "velocityY": -1.192273718485189,
+            "timestamp": 0.46243033862927657
         },
         {
-            "x": 6.427941619479976,
-            "y": 1.2030534747397732,
-            "heading": -0.3212315858589365,
-            "angularVelocity": 3.7578863628089786e-7,
-            "velocityX": 3.5892131983452513,
-            "velocityY": -1.1950677157381002,
-            "timestamp": 0.5395020617352384
+            "x": 6.427941847872596,
+            "y": 1.2030541885235448,
+            "heading": -0.3212314367193528,
+            "angularVelocity": 3.757889680371681e-7,
+            "velocityX": 3.5892130611702004,
+            "velocityY": -1.1950681277232245,
+            "timestamp": 0.539502061734156
         },
         {
-            "x": 6.704568483880494,
-            "y": 1.1109476025613012,
-            "heading": -0.3212315568962456,
-            "angularVelocity": 3.75788807477423e-7,
-            "velocityX": 3.5892134398439306,
-            "velocityY": -1.195066990431097,
-            "timestamp": 0.6165737848402725
+            "x": 6.704568684105482,
+            "y": 1.1109482317494896,
+            "heading": -0.3212314077566493,
+            "angularVelocity": 3.7578897090221527e-7,
+            "velocityX": 3.589213074378161,
+            "velocityY": -1.1950680880550335,
+            "timestamp": 0.6165737848390355
         },
         {
-            "x": 6.980707555601848,
-            "y": 1.0195121848796085,
-            "heading": -0.31890263703438493,
-            "angularVelocity": 0.03021756576905414,
-            "velocityX": 3.582884365320709,
-            "velocityY": -1.186367892113736,
-            "timestamp": 0.6936455079453065
+            "x": 6.9807077062893965,
+            "y": 1.0195126541212949,
+            "heading": -0.3189025007582472,
+            "angularVelocity": 0.030217398866675224,
+            "velocityX": 3.582883722583232,
+            "velocityY": -1.1863699674103443,
+            "timestamp": 0.6936455079439149
         },
         {
-            "x": 7.214256878757648,
-            "y": 0.9418349206236948,
-            "heading": -0.2350677354758115,
-            "angularVelocity": 1.0877517483853638,
-            "velocityX": 3.030285476263674,
-            "velocityY": -1.0078568523770295,
-            "timestamp": 0.7707172310503406
+            "x": 7.214256981896637,
+            "y": 0.9418352430707998,
+            "heading": -0.2350676331360113,
+            "angularVelocity": 1.0877513080660361,
+            "velocityX": 3.0302848593306506,
+            "velocityY": -1.0078587570280646,
+            "timestamp": 0.7707172310487943
         },
         {
-            "x": 7.4008131329346885,
-            "y": 0.8797967164596704,
-            "heading": -0.14969694055158067,
-            "angularVelocity": 1.1076798530621503,
-            "velocityX": 2.420553825205123,
-            "velocityY": -0.8049411854913137,
-            "timestamp": 0.8477889541553747
+            "x": 7.4008131962231865,
+            "y": 0.8797969152412516,
+            "heading": -0.14969687190170503,
+            "angularVelocity": 1.1076794159400525,
+            "velocityX": 2.4205533081527695,
+            "velocityY": -0.8049427900441033,
+            "timestamp": 0.8477889541536737
         },
         {
-            "x": 7.540515620024687,
-            "y": 0.8333523210527743,
-            "heading": -0.07802156861665294,
-            "angularVelocity": 0.9299827361748209,
-            "velocityX": 1.8126296060568263,
-            "velocityY": -0.6026126513818976,
-            "timestamp": 0.9248606772604088
+            "x": 7.54051565231881,
+            "y": 0.8333524229780926,
+            "heading": -0.0780215306756512,
+            "angularVelocity": 0.9299823377312819,
+            "velocityX": 1.812629203910703,
+            "velocityY": -0.6026139080860716,
+            "timestamp": 0.9248606772585531
         },
         {
-            "x": 7.633536076900545,
-            "y": 0.8024373784171116,
-            "heading": -0.026843164489729465,
-            "angularVelocity": 0.6640360701054664,
-            "velocityX": 1.2069336603398442,
-            "velocityY": -0.40111913150730505,
-            "timestamp": 1.0019324003654428
+            "x": 7.633536087871107,
+            "y": 0.802437413208921,
+            "heading": -0.026843150662094223,
+            "angularVelocity": 0.6640357572376224,
+            "velocityX": 1.2069333836706095,
+            "velocityY": -0.401120002560503,
+            "timestamp": 1.0019324003634327
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 1.4987800218197315e-33,
-            "angularVelocity": 0.3482881063025846,
-            "velocityX": 0.6028660217721296,
-            "velocityY": -0.20029886182865572,
-            "timestamp": 1.0790041234704768
+            "heading": 3.8658238709310755e-33,
+            "angularVelocity": 0.3482879268907221,
+            "velocityX": 0.6028658794310917,
+            "velocityY": -0.20029931325025216,
+            "timestamp": 1.079004123468312
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -8.136191152722898e-32,
-            "velocityX": 4.4028618628466005e-33,
-            "velocityY": 0,
-            "timestamp": 1.1560758465755108
+            "heading": 4.381697144346615e-34,
+            "angularVelocity": -4.4473487408728983e-32,
+            "velocityX": 0,
+            "velocityY": -1.5918634852021244e-32,
+            "timestamp": 1.1560758465731915
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.1.traj
new file mode 100644
index 00000000..8a67252f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.1.traj	
@@ -0,0 +1,256 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -2.137980318394988e-34,
+            "velocityX": 1.3258659962604387e-36,
+            "velocityY": 7.61509960712835e-36,
+            "timestamp": 0
+        },
+        {
+            "x": 5.139985955005962,
+            "y": 1.6266018860262446,
+            "heading": -0.6327443563347177,
+            "angularVelocity": 0.10940864810395588,
+            "velocityX": -0.5430597418126536,
+            "velocityY": 0.09955056482775779,
+            "timestamp": 0.06631691178916982
+        },
+        {
+            "x": 5.067957879081044,
+            "y": 1.6398056681484225,
+            "heading": -0.6182319813670383,
+            "angularVelocity": 0.2188336968074761,
+            "velocityX": -1.0861192715653594,
+            "velocityY": 0.19910128149746392,
+            "timestamp": 0.13263382357833964
+        },
+        {
+            "x": 4.959915797050431,
+            "y": 1.6596113536414707,
+            "heading": -0.5964609094580124,
+            "angularVelocity": 0.328288385596709,
+            "velocityX": -1.6291784269764191,
+            "velocityY": 0.2986521078667379,
+            "timestamp": 0.19895073536750946
+        },
+        {
+            "x": 4.815859744494909,
+            "y": 1.686018943789264,
+            "heading": -0.5674282989321845,
+            "angularVelocity": 0.43778592432238833,
+            "velocityX": -2.172237045860861,
+            "velocityY": 0.39820295359570684,
+            "timestamp": 0.2652676471566793
+        },
+        {
+            "x": 4.635789767966758,
+            "y": 1.719028429378711,
+            "heading": -0.5311304969514992,
+            "angularVelocity": 0.5473385445944894,
+            "velocityX": -2.7152949627783816,
+            "velocityY": 0.49775366039945246,
+            "timestamp": 0.3315845589458491
+        },
+        {
+            "x": 4.41970592970577,
+            "y": 1.758639783689655,
+            "heading": -0.48756329690665473,
+            "angularVelocity": 0.6569545967904882,
+            "velocityX": -3.258351941175237,
+            "velocityY": 0.5973039642870227,
+            "timestamp": 0.3979014707350189
+        },
+        {
+            "x": 4.172944378919847,
+            "y": 1.8038720825822125,
+            "heading": -0.4875632898742738,
+            "angularVelocity": 1.0604204507812919e-7,
+            "velocityX": -3.7209445393116334,
+            "velocityY": 0.6820628052819626,
+            "timestamp": 0.46421838252418873
+        },
+        {
+            "x": 3.9261828364502485,
+            "y": 1.849104426845956,
+            "heading": -0.4875632828424199,
+            "angularVelocity": 1.0603409772331237e-7,
+            "velocityX": -3.7209444139088643,
+            "velocityY": 0.6820634894390606,
+            "timestamp": 0.5305352943133586
+        },
+        {
+            "x": 3.6794213056437455,
+            "y": 1.8943368347367484,
+            "heading": -0.487563275810563,
+            "angularVelocity": 1.0603414195839125e-7,
+            "velocityX": -3.720944238039768,
+            "velocityY": 0.6820644488783205,
+            "timestamp": 0.5968522061025283
+        },
+        {
+            "x": 3.4326599282156307,
+            "y": 1.9395700793621615,
+            "heading": -0.4875632687787025,
+            "angularVelocity": 1.0603419804191013e-7,
+            "velocityX": -3.720941925230195,
+            "velocityY": 0.6820770660916139,
+            "timestamp": 0.6631691178916981
+        },
+        {
+            "x": 3.192328571865682,
+            "y": 2.011528836252569,
+            "heading": -0.4875632615410612,
+            "angularVelocity": 1.0913718718412795e-7,
+            "velocityX": -3.6239829308396314,
+            "velocityY": 1.0850740022269671,
+            "timestamp": 0.7294860296808678
+        },
+        {
+            "x": 2.965054771843869,
+            "y": 2.1177542675713954,
+            "heading": -0.48756325344928275,
+            "angularVelocity": 1.2201681646141545e-7,
+            "velocityX": -3.4270866041583203,
+            "velocityY": 1.6017849512735258,
+            "timestamp": 0.7958029414700376
+        },
+        {
+            "x": 2.7556945515371734,
+            "y": 2.2559769801698577,
+            "heading": -0.48756324342419455,
+            "angularVelocity": 1.5116940667572282e-7,
+            "velocityX": -3.156965767228115,
+            "velocityY": 2.084275471660842,
+            "timestamp": 0.8621198532592074
+        },
+        {
+            "x": 2.5687211592217127,
+            "y": 2.4232437322251608,
+            "heading": -0.4875632292213124,
+            "angularVelocity": 2.1416682100483036e-7,
+            "velocityX": -2.8193923280063653,
+            "velocityY": 2.5222337340898235,
+            "timestamp": 0.9284367650483771
+        },
+        {
+            "x": 2.4081295337898796,
+            "y": 2.6159806661875145,
+            "heading": -0.48756314249031313,
+            "angularVelocity": 0.0000013078262679985986,
+            "velocityX": -2.421578766248568,
+            "velocityY": 2.906301405817713,
+            "timestamp": 0.9947536768375469
+        },
+        {
+            "x": 2.2783797783684188,
+            "y": 2.8285106690600896,
+            "heading": -0.4828540771785898,
+            "angularVelocity": 0.07100851328382284,
+            "velocityX": -1.9565108193510647,
+            "velocityY": 3.2047632668456583,
+            "timestamp": 1.0610705886267167
+        },
+        {
+            "x": 2.1829307079315186,
+            "y": 3.053825616836548,
+            "heading": -0.47038335932986286,
+            "angularVelocity": 0.18804732476646344,
+            "velocityX": -1.4392870213912456,
+            "velocityY": 3.3975488559051734,
+            "timestamp": 1.1273875004158864
+        },
+        {
+            "x": 2.124568521000596,
+            "y": 3.2749812318761844,
+            "heading": -0.4550563591461243,
+            "angularVelocity": 0.24322153086635045,
+            "velocityX": -0.9261395106595871,
+            "velocityY": 3.5094804335357668,
+            "timestamp": 1.1904041261609186
+        },
+        {
+            "x": 2.099309698866093,
+            "y": 3.4965263857952165,
+            "heading": -0.4375068558264715,
+            "angularVelocity": 0.2784900510328627,
+            "velocityX": -0.40082790590377965,
+            "velocityY": 3.5156619590425686,
+            "timestamp": 1.2534207519059508
+        },
+        {
+            "x": 2.105558929434475,
+            "y": 3.707892405315209,
+            "heading": -0.4199092504656253,
+            "angularVelocity": 0.2792533740547583,
+            "velocityX": 0.09916796550908585,
+            "velocityY": 3.354131025281934,
+            "timestamp": 1.316437377650983
+        },
+        {
+            "x": 2.135680280502732,
+            "y": 3.8963278332771982,
+            "heading": -0.40525850037923283,
+            "angularVelocity": 0.23249023433387317,
+            "velocityX": 0.4779905415775436,
+            "velocityY": 2.990249410122441,
+            "timestamp": 1.3794540033960152
+        },
+        {
+            "x": 2.177481885950485,
+            "y": 4.0538159629335215,
+            "heading": -0.39590861500997965,
+            "angularVelocity": 0.14837172347315433,
+            "velocityX": 0.6633424902324003,
+            "velocityY": 2.4991520538330105,
+            "timestamp": 1.4424706291410474
+        },
+        {
+            "x": 2.2205433619064863,
+            "y": 4.17824561445827,
+            "heading": -0.39150641146352744,
+            "angularVelocity": 0.06985781124276201,
+            "velocityX": 0.6833351587282585,
+            "velocityY": 1.9745527478446014,
+            "timestamp": 1.5054872548860796
+        },
+        {
+            "x": 2.2579292824582606,
+            "y": 4.270063380418578,
+            "heading": -0.38992694464863337,
+            "angularVelocity": 0.0250642873403699,
+            "velocityX": 0.5932707457717501,
+            "velocityY": 1.4570403425249505,
+            "timestamp": 1.5685038806311118
+        },
+        {
+            "x": 2.285342089257776,
+            "y": 4.3303097045050025,
+            "heading": -0.38974725509942826,
+            "angularVelocity": 0.002851462563739305,
+            "velocityX": 0.43500911823537725,
+            "velocityY": 0.9560385592555154,
+            "timestamp": 1.631520506376144
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.004010765374750144,
+            "velocityX": 0.23260386555018367,
+            "velocityY": 0.47115019479344145,
+            "timestamp": 1.6945371321211762
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -1.6105528525551922e-32,
+            "velocityX": 0,
+            "velocityY": -4.5276793712997926e-36,
+            "timestamp": 1.7575537578662084
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.traj
new file mode 100644
index 00000000..8a67252f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.traj	
@@ -0,0 +1,256 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -2.137980318394988e-34,
+            "velocityX": 1.3258659962604387e-36,
+            "velocityY": 7.61509960712835e-36,
+            "timestamp": 0
+        },
+        {
+            "x": 5.139985955005962,
+            "y": 1.6266018860262446,
+            "heading": -0.6327443563347177,
+            "angularVelocity": 0.10940864810395588,
+            "velocityX": -0.5430597418126536,
+            "velocityY": 0.09955056482775779,
+            "timestamp": 0.06631691178916982
+        },
+        {
+            "x": 5.067957879081044,
+            "y": 1.6398056681484225,
+            "heading": -0.6182319813670383,
+            "angularVelocity": 0.2188336968074761,
+            "velocityX": -1.0861192715653594,
+            "velocityY": 0.19910128149746392,
+            "timestamp": 0.13263382357833964
+        },
+        {
+            "x": 4.959915797050431,
+            "y": 1.6596113536414707,
+            "heading": -0.5964609094580124,
+            "angularVelocity": 0.328288385596709,
+            "velocityX": -1.6291784269764191,
+            "velocityY": 0.2986521078667379,
+            "timestamp": 0.19895073536750946
+        },
+        {
+            "x": 4.815859744494909,
+            "y": 1.686018943789264,
+            "heading": -0.5674282989321845,
+            "angularVelocity": 0.43778592432238833,
+            "velocityX": -2.172237045860861,
+            "velocityY": 0.39820295359570684,
+            "timestamp": 0.2652676471566793
+        },
+        {
+            "x": 4.635789767966758,
+            "y": 1.719028429378711,
+            "heading": -0.5311304969514992,
+            "angularVelocity": 0.5473385445944894,
+            "velocityX": -2.7152949627783816,
+            "velocityY": 0.49775366039945246,
+            "timestamp": 0.3315845589458491
+        },
+        {
+            "x": 4.41970592970577,
+            "y": 1.758639783689655,
+            "heading": -0.48756329690665473,
+            "angularVelocity": 0.6569545967904882,
+            "velocityX": -3.258351941175237,
+            "velocityY": 0.5973039642870227,
+            "timestamp": 0.3979014707350189
+        },
+        {
+            "x": 4.172944378919847,
+            "y": 1.8038720825822125,
+            "heading": -0.4875632898742738,
+            "angularVelocity": 1.0604204507812919e-7,
+            "velocityX": -3.7209445393116334,
+            "velocityY": 0.6820628052819626,
+            "timestamp": 0.46421838252418873
+        },
+        {
+            "x": 3.9261828364502485,
+            "y": 1.849104426845956,
+            "heading": -0.4875632828424199,
+            "angularVelocity": 1.0603409772331237e-7,
+            "velocityX": -3.7209444139088643,
+            "velocityY": 0.6820634894390606,
+            "timestamp": 0.5305352943133586
+        },
+        {
+            "x": 3.6794213056437455,
+            "y": 1.8943368347367484,
+            "heading": -0.487563275810563,
+            "angularVelocity": 1.0603414195839125e-7,
+            "velocityX": -3.720944238039768,
+            "velocityY": 0.6820644488783205,
+            "timestamp": 0.5968522061025283
+        },
+        {
+            "x": 3.4326599282156307,
+            "y": 1.9395700793621615,
+            "heading": -0.4875632687787025,
+            "angularVelocity": 1.0603419804191013e-7,
+            "velocityX": -3.720941925230195,
+            "velocityY": 0.6820770660916139,
+            "timestamp": 0.6631691178916981
+        },
+        {
+            "x": 3.192328571865682,
+            "y": 2.011528836252569,
+            "heading": -0.4875632615410612,
+            "angularVelocity": 1.0913718718412795e-7,
+            "velocityX": -3.6239829308396314,
+            "velocityY": 1.0850740022269671,
+            "timestamp": 0.7294860296808678
+        },
+        {
+            "x": 2.965054771843869,
+            "y": 2.1177542675713954,
+            "heading": -0.48756325344928275,
+            "angularVelocity": 1.2201681646141545e-7,
+            "velocityX": -3.4270866041583203,
+            "velocityY": 1.6017849512735258,
+            "timestamp": 0.7958029414700376
+        },
+        {
+            "x": 2.7556945515371734,
+            "y": 2.2559769801698577,
+            "heading": -0.48756324342419455,
+            "angularVelocity": 1.5116940667572282e-7,
+            "velocityX": -3.156965767228115,
+            "velocityY": 2.084275471660842,
+            "timestamp": 0.8621198532592074
+        },
+        {
+            "x": 2.5687211592217127,
+            "y": 2.4232437322251608,
+            "heading": -0.4875632292213124,
+            "angularVelocity": 2.1416682100483036e-7,
+            "velocityX": -2.8193923280063653,
+            "velocityY": 2.5222337340898235,
+            "timestamp": 0.9284367650483771
+        },
+        {
+            "x": 2.4081295337898796,
+            "y": 2.6159806661875145,
+            "heading": -0.48756314249031313,
+            "angularVelocity": 0.0000013078262679985986,
+            "velocityX": -2.421578766248568,
+            "velocityY": 2.906301405817713,
+            "timestamp": 0.9947536768375469
+        },
+        {
+            "x": 2.2783797783684188,
+            "y": 2.8285106690600896,
+            "heading": -0.4828540771785898,
+            "angularVelocity": 0.07100851328382284,
+            "velocityX": -1.9565108193510647,
+            "velocityY": 3.2047632668456583,
+            "timestamp": 1.0610705886267167
+        },
+        {
+            "x": 2.1829307079315186,
+            "y": 3.053825616836548,
+            "heading": -0.47038335932986286,
+            "angularVelocity": 0.18804732476646344,
+            "velocityX": -1.4392870213912456,
+            "velocityY": 3.3975488559051734,
+            "timestamp": 1.1273875004158864
+        },
+        {
+            "x": 2.124568521000596,
+            "y": 3.2749812318761844,
+            "heading": -0.4550563591461243,
+            "angularVelocity": 0.24322153086635045,
+            "velocityX": -0.9261395106595871,
+            "velocityY": 3.5094804335357668,
+            "timestamp": 1.1904041261609186
+        },
+        {
+            "x": 2.099309698866093,
+            "y": 3.4965263857952165,
+            "heading": -0.4375068558264715,
+            "angularVelocity": 0.2784900510328627,
+            "velocityX": -0.40082790590377965,
+            "velocityY": 3.5156619590425686,
+            "timestamp": 1.2534207519059508
+        },
+        {
+            "x": 2.105558929434475,
+            "y": 3.707892405315209,
+            "heading": -0.4199092504656253,
+            "angularVelocity": 0.2792533740547583,
+            "velocityX": 0.09916796550908585,
+            "velocityY": 3.354131025281934,
+            "timestamp": 1.316437377650983
+        },
+        {
+            "x": 2.135680280502732,
+            "y": 3.8963278332771982,
+            "heading": -0.40525850037923283,
+            "angularVelocity": 0.23249023433387317,
+            "velocityX": 0.4779905415775436,
+            "velocityY": 2.990249410122441,
+            "timestamp": 1.3794540033960152
+        },
+        {
+            "x": 2.177481885950485,
+            "y": 4.0538159629335215,
+            "heading": -0.39590861500997965,
+            "angularVelocity": 0.14837172347315433,
+            "velocityX": 0.6633424902324003,
+            "velocityY": 2.4991520538330105,
+            "timestamp": 1.4424706291410474
+        },
+        {
+            "x": 2.2205433619064863,
+            "y": 4.17824561445827,
+            "heading": -0.39150641146352744,
+            "angularVelocity": 0.06985781124276201,
+            "velocityX": 0.6833351587282585,
+            "velocityY": 1.9745527478446014,
+            "timestamp": 1.5054872548860796
+        },
+        {
+            "x": 2.2579292824582606,
+            "y": 4.270063380418578,
+            "heading": -0.38992694464863337,
+            "angularVelocity": 0.0250642873403699,
+            "velocityX": 0.5932707457717501,
+            "velocityY": 1.4570403425249505,
+            "timestamp": 1.5685038806311118
+        },
+        {
+            "x": 2.285342089257776,
+            "y": 4.3303097045050025,
+            "heading": -0.38974725509942826,
+            "angularVelocity": 0.002851462563739305,
+            "velocityX": 0.43500911823537725,
+            "velocityY": 0.9560385592555154,
+            "timestamp": 1.631520506376144
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.004010765374750144,
+            "velocityX": 0.23260386555018367,
+            "velocityY": 0.47115019479344145,
+            "timestamp": 1.6945371321211762
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -1.6105528525551922e-32,
+            "velocityX": 0,
+            "velocityY": -4.5276793712997926e-36,
+            "timestamp": 1.7575537578662084
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.1.traj
new file mode 100644
index 00000000..a91e184b
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.1.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3668131239789423,
+            "y": 6.6799532352680275,
+            "heading": 0.31688592464549686,
+            "angularVelocity": -0.6546901107445974,
+            "velocityX": 0.5982690928509629,
+            "velocityY": -0.6899048465062367,
+            "timestamp": 0.11167737858654542
+        },
+        {
+            "x": 2.5010623988117353,
+            "y": 6.525329092787392,
+            "heading": 0.1875032700506318,
+            "angularVelocity": -1.1585395022018103,
+            "velocityX": 1.202116995688263,
+            "velocityY": -1.3845609955896643,
+            "timestamp": 0.22335475717309083
+        },
+        {
+            "x": 2.7034647042549174,
+            "y": 6.29238592031536,
+            "heading": 0.03684167640916454,
+            "angularVelocity": -1.3490788873122188,
+            "velocityX": 1.8123841014618285,
+            "velocityY": -2.0858581694904785,
+            "timestamp": 0.33503213575963625
+        },
+        {
+            "x": 2.970402662431944,
+            "y": 5.984552676414473,
+            "heading": 2.692880535790558e-7,
+            "angularVelocity": -0.32989140314179594,
+            "velocityX": 2.39025988571325,
+            "velocityY": -2.7564511971627637,
+            "timestamp": 0.44670951434618167
+        },
+        {
+            "x": 3.247779632307318,
+            "y": 5.665896160181276,
+            "heading": 2.2674846776964554e-7,
+            "angularVelocity": -3.809149744305609e-7,
+            "velocityX": 2.4837346057547505,
+            "velocityY": -2.853366727141142,
+            "timestamp": 0.5583868929327271
+        },
+        {
+            "x": 3.5251566242922663,
+            "y": 5.347239663193555,
+            "heading": 1.842089099097335e-7,
+            "angularVelocity": -3.8091472416520575e-7,
+            "velocityX": 2.4837348037318896,
+            "velocityY": -2.853366554810183,
+            "timestamp": 0.6700642715192726
+        },
+        {
+            "x": 3.8025335437012195,
+            "y": 5.0285831030297485,
+            "heading": 1.4166936288049968e-7,
+            "angularVelocity": -3.8091462718327505e-7,
+            "velocityX": 2.4837341538598574,
+            "velocityY": -2.8533671205119875,
+            "timestamp": 0.7817416501058181
+        },
+        {
+            "x": 4.079910865419093,
+            "y": 4.7099268930580935,
+            "heading": 9.912982095474378e-8,
+            "angularVelocity": -3.809145814844431e-7,
+            "velocityX": 2.4837377562807075,
+            "velocityY": -2.853363984763686,
+            "timestamp": 0.8934190286923636
+        },
+        {
+            "x": 4.382964098552189,
+            "y": 4.415582373772677,
+            "heading": 5.542611481257948e-8,
+            "angularVelocity": -3.9133893269333644e-7,
+            "velocityX": 2.713649236476739,
+            "velocityY": -2.635668234792112,
+            "timestamp": 1.005096407278909
+        },
+        {
+            "x": 4.748738765716553,
+            "y": 4.2041850090026855,
+            "heading": 0,
+            "angularVelocity": -4.963056575446211e-7,
+            "velocityX": 3.27527984443961,
+            "velocityY": -1.8929291450565913,
+            "timestamp": 1.1167737858654545
+        },
+        {
+            "x": 5.133803316447558,
+            "y": 4.089523361787239,
+            "heading": -1.387645596172588e-10,
+            "angularVelocity": -1.3065520492479572e-9,
+            "velocityX": 3.625615065458012,
+            "velocityY": -1.0796085871456598,
+            "timestamp": 1.2229804690844188
+        },
+        {
+            "x": 5.534974194059096,
+            "y": 4.0675249151482635,
+            "heading": -2.526284831735191e-10,
+            "angularVelocity": -1.0720975375429818e-9,
+            "velocityX": 3.7772658504404184,
+            "velocityY": -0.20712864739049783,
+            "timestamp": 1.3291871523033831
+        },
+        {
+            "x": 5.9365934232146325,
+            "y": 4.0786604378791305,
+            "heading": -3.6352969398434843e-10,
+            "angularVelocity": -1.0442018095219078e-9,
+            "velocityX": 3.781487350730336,
+            "velocityY": 0.10484766488641149,
+            "timestamp": 1.4353938355223474
+        },
+        {
+            "x": 6.3382126442540345,
+            "y": 4.089796253283455,
+            "heading": -4.744184411388132e-10,
+            "angularVelocity": -1.044084456790862e-9,
+            "velocityX": 3.781487274312031,
+            "velocityY": 0.1048504205837075,
+            "timestamp": 1.5416005187413118
+        },
+        {
+            "x": 6.739831868633552,
+            "y": 4.100931948201603,
+            "heading": -5.853040774969199e-10,
+            "angularVelocity": -1.0440551668485128e-9,
+            "velocityX": 3.7814873057612264,
+            "velocityY": 0.10484928613381475,
+            "timestamp": 1.647807201960276
+        },
+        {
+            "x": 7.11589910333758,
+            "y": 4.111359168329562,
+            "heading": -0.0011999964635320353,
+            "angularVelocity": -0.011298685184913941,
+            "velocityX": 3.5408999067290075,
+            "velocityY": 0.09817856854143046,
+            "timestamp": 1.7540138851792404
+        },
+        {
+            "x": 7.397949547057075,
+            "y": 4.119179583905487,
+            "heading": -0.002099997192055183,
+            "angularVelocity": -0.008474049854922212,
+            "velocityX": 2.6556750966226486,
+            "velocityY": 0.0736339309252122,
+            "timestamp": 1.8602205683982047
+        },
+        {
+            "x": 7.585983181328134,
+            "y": 4.124393194553852,
+            "heading": -0.0026999988369627403,
+            "angularVelocity": -0.005649377484767236,
+            "velocityX": 1.7704501126675238,
+            "velocityY": 0.049089289773120645,
+            "timestamp": 1.966427251617169
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -0.003,
+            "angularVelocity": -0.0028246919491873987,
+            "velocityX": 0.8852250707992916,
+            "velocityY": 0.024544646034960003,
+            "timestamp": 2.072633934836133
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -0.003,
+            "angularVelocity": 0,
+            "velocityX": 1.6661554323284825e-27,
+            "velocityY": 0,
+            "timestamp": 2.1788406180550974
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.traj
new file mode 100644
index 00000000..a91e184b
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3668131239789423,
+            "y": 6.6799532352680275,
+            "heading": 0.31688592464549686,
+            "angularVelocity": -0.6546901107445974,
+            "velocityX": 0.5982690928509629,
+            "velocityY": -0.6899048465062367,
+            "timestamp": 0.11167737858654542
+        },
+        {
+            "x": 2.5010623988117353,
+            "y": 6.525329092787392,
+            "heading": 0.1875032700506318,
+            "angularVelocity": -1.1585395022018103,
+            "velocityX": 1.202116995688263,
+            "velocityY": -1.3845609955896643,
+            "timestamp": 0.22335475717309083
+        },
+        {
+            "x": 2.7034647042549174,
+            "y": 6.29238592031536,
+            "heading": 0.03684167640916454,
+            "angularVelocity": -1.3490788873122188,
+            "velocityX": 1.8123841014618285,
+            "velocityY": -2.0858581694904785,
+            "timestamp": 0.33503213575963625
+        },
+        {
+            "x": 2.970402662431944,
+            "y": 5.984552676414473,
+            "heading": 2.692880535790558e-7,
+            "angularVelocity": -0.32989140314179594,
+            "velocityX": 2.39025988571325,
+            "velocityY": -2.7564511971627637,
+            "timestamp": 0.44670951434618167
+        },
+        {
+            "x": 3.247779632307318,
+            "y": 5.665896160181276,
+            "heading": 2.2674846776964554e-7,
+            "angularVelocity": -3.809149744305609e-7,
+            "velocityX": 2.4837346057547505,
+            "velocityY": -2.853366727141142,
+            "timestamp": 0.5583868929327271
+        },
+        {
+            "x": 3.5251566242922663,
+            "y": 5.347239663193555,
+            "heading": 1.842089099097335e-7,
+            "angularVelocity": -3.8091472416520575e-7,
+            "velocityX": 2.4837348037318896,
+            "velocityY": -2.853366554810183,
+            "timestamp": 0.6700642715192726
+        },
+        {
+            "x": 3.8025335437012195,
+            "y": 5.0285831030297485,
+            "heading": 1.4166936288049968e-7,
+            "angularVelocity": -3.8091462718327505e-7,
+            "velocityX": 2.4837341538598574,
+            "velocityY": -2.8533671205119875,
+            "timestamp": 0.7817416501058181
+        },
+        {
+            "x": 4.079910865419093,
+            "y": 4.7099268930580935,
+            "heading": 9.912982095474378e-8,
+            "angularVelocity": -3.809145814844431e-7,
+            "velocityX": 2.4837377562807075,
+            "velocityY": -2.853363984763686,
+            "timestamp": 0.8934190286923636
+        },
+        {
+            "x": 4.382964098552189,
+            "y": 4.415582373772677,
+            "heading": 5.542611481257948e-8,
+            "angularVelocity": -3.9133893269333644e-7,
+            "velocityX": 2.713649236476739,
+            "velocityY": -2.635668234792112,
+            "timestamp": 1.005096407278909
+        },
+        {
+            "x": 4.748738765716553,
+            "y": 4.2041850090026855,
+            "heading": 0,
+            "angularVelocity": -4.963056575446211e-7,
+            "velocityX": 3.27527984443961,
+            "velocityY": -1.8929291450565913,
+            "timestamp": 1.1167737858654545
+        },
+        {
+            "x": 5.133803316447558,
+            "y": 4.089523361787239,
+            "heading": -1.387645596172588e-10,
+            "angularVelocity": -1.3065520492479572e-9,
+            "velocityX": 3.625615065458012,
+            "velocityY": -1.0796085871456598,
+            "timestamp": 1.2229804690844188
+        },
+        {
+            "x": 5.534974194059096,
+            "y": 4.0675249151482635,
+            "heading": -2.526284831735191e-10,
+            "angularVelocity": -1.0720975375429818e-9,
+            "velocityX": 3.7772658504404184,
+            "velocityY": -0.20712864739049783,
+            "timestamp": 1.3291871523033831
+        },
+        {
+            "x": 5.9365934232146325,
+            "y": 4.0786604378791305,
+            "heading": -3.6352969398434843e-10,
+            "angularVelocity": -1.0442018095219078e-9,
+            "velocityX": 3.781487350730336,
+            "velocityY": 0.10484766488641149,
+            "timestamp": 1.4353938355223474
+        },
+        {
+            "x": 6.3382126442540345,
+            "y": 4.089796253283455,
+            "heading": -4.744184411388132e-10,
+            "angularVelocity": -1.044084456790862e-9,
+            "velocityX": 3.781487274312031,
+            "velocityY": 0.1048504205837075,
+            "timestamp": 1.5416005187413118
+        },
+        {
+            "x": 6.739831868633552,
+            "y": 4.100931948201603,
+            "heading": -5.853040774969199e-10,
+            "angularVelocity": -1.0440551668485128e-9,
+            "velocityX": 3.7814873057612264,
+            "velocityY": 0.10484928613381475,
+            "timestamp": 1.647807201960276
+        },
+        {
+            "x": 7.11589910333758,
+            "y": 4.111359168329562,
+            "heading": -0.0011999964635320353,
+            "angularVelocity": -0.011298685184913941,
+            "velocityX": 3.5408999067290075,
+            "velocityY": 0.09817856854143046,
+            "timestamp": 1.7540138851792404
+        },
+        {
+            "x": 7.397949547057075,
+            "y": 4.119179583905487,
+            "heading": -0.002099997192055183,
+            "angularVelocity": -0.008474049854922212,
+            "velocityX": 2.6556750966226486,
+            "velocityY": 0.0736339309252122,
+            "timestamp": 1.8602205683982047
+        },
+        {
+            "x": 7.585983181328134,
+            "y": 4.124393194553852,
+            "heading": -0.0026999988369627403,
+            "angularVelocity": -0.005649377484767236,
+            "velocityX": 1.7704501126675238,
+            "velocityY": 0.049089289773120645,
+            "timestamp": 1.966427251617169
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -0.003,
+            "angularVelocity": -0.0028246919491873987,
+            "velocityX": 0.8852250707992916,
+            "velocityY": 0.024544646034960003,
+            "timestamp": 2.072633934836133
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -0.003,
+            "angularVelocity": 0,
+            "velocityX": 1.6661554323284825e-27,
+            "velocityY": 0,
+            "timestamp": 2.1788406180550974
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.1.traj
index 1a6d423b..785ed7b8 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.1.traj	
@@ -4,577 +4,397 @@
             "x": 2.3,
             "y": 6.757,
             "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 1.0547928981710615e-31,
-            "velocityY": 1.4824740100837611e-32,
+            "angularVelocity": 1.592522039385819e-32,
+            "velocityX": 0,
+            "velocityY": -2.1348488243236776e-32,
             "timestamp": 0
         },
         {
-            "x": 2.3072711000078927,
-            "y": 6.749947927475375,
-            "heading": 0.3875624240531776,
-            "angularVelocity": -0.06985747897751753,
-            "velocityX": 0.2083794421285326,
-            "velocityY": -0.20210242424612682,
-            "timestamp": 0.034893557318421635
-        },
-        {
-            "x": 2.321814346754588,
-            "y": 6.735842802544083,
-            "heading": 0.3827646159677583,
-            "angularVelocity": -0.1374983938048157,
-            "velocityX": 0.41678888208448434,
-            "velocityY": -0.40423293052572296,
-            "timestamp": 0.06978711463684327
-        },
-        {
-            "x": 2.343630936460155,
-            "y": 6.714683501867812,
-            "heading": 0.3756983254810237,
-            "angularVelocity": -0.20250989093060998,
-            "velocityX": 0.6252326040156703,
-            "velocityY": -0.6063956300924075,
-            "timestamp": 0.1046806719552649
-        },
-        {
-            "x": 2.372722244735657,
-            "y": 6.6864687287677045,
-            "heading": 0.36647405587954873,
-            "angularVelocity": -0.2643545201567945,
-            "velocityX": 0.8337157490143364,
-            "velocityY": -0.8085954906412398,
-            "timestamp": 0.13957422927368654
-        },
-        {
-            "x": 2.4090898628559576,
-            "y": 6.651196976344128,
-            "heading": 0.3552274150516352,
-            "angularVelocity": -0.3223128191053197,
-            "velocityX": 1.042244497699901,
-            "velocityY": -1.0108385368022033,
-            "timestamp": 0.17446778659210818
-        },
-        {
-            "x": 2.452735639247045,
-            "y": 6.608866484215248,
-            "heading": 0.3421287688368095,
-            "angularVelocity": -0.3753886740550377,
-            "velocityX": 1.2508262196599174,
-            "velocityY": -1.213132033016619,
-            "timestamp": 0.20936134391052982
-        },
-        {
-            "x": 2.5036617186589587,
-            "y": 6.55947519544937,
-            "heading": 0.3273986519293141,
-            "angularVelocity": -0.42214431658760004,
-            "velocityX": 1.459469407122573,
-            "velocityY": -1.4154844779841917,
-            "timestamp": 0.24425490122895147
-        },
-        {
-            "x": 2.5618705496598224,
-            "y": 6.503020741236739,
-            "heading": 0.31133395345849213,
-            "angularVelocity": -0.4603915365872077,
-            "velocityX": 1.6681827670844436,
-            "velocityY": -1.6179048096889317,
-            "timestamp": 0.2791484585473731
-        },
-        {
-            "x": 2.6273647520175327,
-            "y": 6.43950055782557,
-            "heading": 0.294356217256913,
-            "angularVelocity": -0.486557906568497,
-            "velocityX": 1.8769712058889019,
-            "velocityY": -1.8203986148936577,
-            "timestamp": 0.3140420158657947
-        },
-        {
-            "x": 2.7001464092257668,
-            "y": 6.368912559254549,
-            "heading": 0.2771114307132975,
-            "angularVelocity": -0.4942111916606252,
-            "velocityX": 2.0858193546752704,
-            "velocityY": -2.022952200799379,
-            "timestamp": 0.34893557318421636
-        },
-        {
-            "x": 2.7802136587601143,
-            "y": 6.291258423996795,
-            "heading": 0.2607138645949654,
-            "angularVelocity": -0.46993105256354084,
-            "velocityX": 2.294614126146364,
-            "velocityY": -2.225457684039485,
-            "timestamp": 0.383829130502638
-        },
-        {
-            "x": 2.867539127268515,
-            "y": 6.206563552711186,
-            "heading": 0.24753249327015822,
-            "angularVelocity": -0.37775945870237265,
-            "velocityX": 2.5026244160637545,
-            "velocityY": -2.427235220322282,
-            "timestamp": 0.41872268782105965
-        },
-        {
-            "x": 2.961704724093414,
-            "y": 6.1150775036748035,
-            "heading": 0.24574502539338514,
-            "angularVelocity": -0.05122630118968667,
-            "velocityX": 2.6986528190746,
-            "velocityY": -2.6218607693541762,
-            "timestamp": 0.4536162451394813
-        },
-        {
-            "x": 3.0564622015652247,
-            "y": 6.023180384324785,
-            "heading": 0.2457450132145974,
-            "angularVelocity": -3.4902683097432965e-7,
-            "velocityX": 2.7156152812710346,
-            "velocityY": -2.633641463133395,
-            "timestamp": 0.48850980245790293
-        },
-        {
-            "x": 3.1512196852998877,
-            "y": 5.931283271432529,
-            "heading": 0.24574500103583913,
-            "angularVelocity": -3.490259866463667e-7,
-            "velocityX": 2.7156154607555005,
-            "velocityY": -2.6336412780630685,
-            "timestamp": 0.5234033597763246
-        },
-        {
-            "x": 3.2459771690350276,
-            "y": 5.839386158540766,
-            "heading": 0.2457449888570809,
-            "angularVelocity": -3.4902598588384144e-7,
-            "velocityX": 2.7156154607691696,
-            "velocityY": -2.6336412780489744,
-            "timestamp": 0.5582969170947462
-        },
-        {
-            "x": 3.3407346527701676,
-            "y": 5.747489045649002,
-            "heading": 0.24574497667832262,
-            "angularVelocity": -3.4902598639891e-7,
-            "velocityX": 2.7156154607691705,
-            "velocityY": -2.6336412780489735,
-            "timestamp": 0.5931904744131677
-        },
-        {
-            "x": 3.4354921365053075,
-            "y": 5.655591932757238,
-            "heading": 0.24574496449956437,
-            "angularVelocity": -3.4902598640760607e-7,
-            "velocityX": 2.71561546076917,
-            "velocityY": -2.6336412780489735,
-            "timestamp": 0.6280840317315893
-        },
-        {
-            "x": 3.5302496202404474,
-            "y": 5.563694819865473,
-            "heading": 0.2457449523208061,
-            "angularVelocity": -3.490259865548224e-7,
-            "velocityX": 2.7156154607691705,
-            "velocityY": -2.6336412780489735,
-            "timestamp": 0.6629775890500109
-        },
-        {
-            "x": 3.6250071039755873,
-            "y": 5.471797706973709,
-            "heading": 0.2457449401420478,
-            "angularVelocity": -3.4902598644057735e-7,
-            "velocityX": 2.71561546076917,
-            "velocityY": -2.633641278048974,
-            "timestamp": 0.6978711463684325
-        },
-        {
-            "x": 3.719764587710727,
-            "y": 5.379900594081945,
-            "heading": 0.24574492796328956,
-            "angularVelocity": -3.4902598592532335e-7,
-            "velocityX": 2.715615460769166,
-            "velocityY": -2.633641278048979,
-            "timestamp": 0.7327647036868541
-        },
-        {
-            "x": 3.8145220714436685,
-            "y": 5.288003481187913,
-            "heading": 0.24574491578453134,
-            "angularVelocity": -3.490259859254125e-7,
-            "velocityX": 2.7156154607061804,
-            "velocityY": -2.6336412781139247,
-            "timestamp": 0.7676582610052757
-        },
-        {
-            "x": 3.909279526317127,
-            "y": 5.196106338536137,
-            "heading": 0.245744903605773,
-            "angularVelocity": -3.4902598866436484e-7,
-            "velocityX": 2.7156146336342,
-            "velocityY": -2.6336421309288234,
-            "timestamp": 0.8025518183236973
-        },
-        {
-            "x": 4.003658642045134,
-            "y": 5.103820680647278,
-            "heading": 0.24574489142231412,
-            "angularVelocity": -3.4916069991394096e-7,
-            "velocityX": 2.7047719688402356,
-            "velocityY": -2.644776428116158,
-            "timestamp": 0.8374453756421188
-        },
-        {
-            "x": 4.090673374879912,
-            "y": 5.004560939161802,
-            "heading": 0.24574487906407927,
-            "angularVelocity": -3.541695320845138e-7,
-            "velocityX": 2.493719171155964,
-            "velocityY": -2.8446437999909007,
-            "timestamp": 0.8723389329605404
-        },
-        {
-            "x": 4.169802665710449,
-            "y": 4.898907661437988,
-            "heading": 0.24574486610274957,
-            "angularVelocity": -3.7145337638188186e-7,
-            "velocityX": 2.267733556325072,
-            "velocityY": -3.0278735056925665,
-            "timestamp": 0.907232490278962
-        },
-        {
-            "x": 4.2677114826543,
-            "y": 4.731769321945997,
-            "heading": 0.24574485481420902,
-            "angularVelocity": -2.2045907687430814e-7,
-            "velocityX": 1.9121061102333996,
-            "velocityY": -3.264121150398313,
-            "timestamp": 0.9584371888152268
-        },
-        {
-            "x": 4.383830202938794,
-            "y": 4.576728030606808,
-            "heading": 0.24574484074146902,
-            "angularVelocity": -2.7483298164960767e-7,
-            "velocityX": 2.2677356493419163,
-            "velocityY": -3.02787235881067,
-            "timestamp": 1.0096418873514916
-        },
-        {
-            "x": 4.516679781731222,
-            "y": 4.435758724265,
-            "heading": 0.24574482059862868,
-            "angularVelocity": -3.9337875070195686e-7,
-            "velocityX": 2.59448024478339,
-            "velocityY": -2.753054121429295,
-            "timestamp": 1.0608465858877565
-        },
-        {
-            "x": 4.664567883303712,
-            "y": 4.3106571835035465,
-            "heading": 0.24574435877996634,
-            "angularVelocity": -0.000009019068084382453,
-            "velocityX": 2.8881744410183567,
-            "velocityY": -2.4431652629075082,
-            "timestamp": 1.1120512844240213
-        },
-        {
-            "x": 4.823636899736968,
-            "y": 4.204273204304783,
-            "heading": 0.2400133838174815,
-            "angularVelocity": -0.1119228337693714,
-            "velocityX": 3.1065316461261507,
-            "velocityY": -2.0776214339670136,
-            "timestamp": 1.1632559829602862
-        },
-        {
-            "x": 4.9908096959943835,
-            "y": 4.118113363926556,
-            "heading": 0.22784637628221624,
-            "angularVelocity": -0.23761506039622932,
-            "velocityX": 3.26479407234511,
-            "velocityY": -1.6826549680241993,
-            "timestamp": 1.214460681496551
-        },
-        {
-            "x": 5.163718040144699,
-            "y": 4.053028192881653,
-            "heading": 0.21228377101249774,
-            "angularVelocity": -0.3039292431083551,
-            "velocityX": 3.3768062129661107,
-            "velocityY": -1.2710781023114057,
-            "timestamp": 1.265665380032816
-        },
-        {
-            "x": 5.340853559631665,
-            "y": 4.009388363770341,
-            "heading": 0.19524077553782296,
-            "angularVelocity": -0.332840461165967,
-            "velocityX": 3.4593606553803307,
-            "velocityY": -0.8522622016884831,
-            "timestamp": 1.3168700785690808
-        },
-        {
-            "x": 5.521188360604893,
-            "y": 3.987375039650421,
-            "heading": 0.17800934218450973,
-            "angularVelocity": -0.3365205507676074,
-            "velocityX": 3.521840888204999,
-            "velocityY": -0.42990828477058174,
-            "timestamp": 1.3680747771053456
+            "x": 2.311435758812473,
+            "y": 6.743296066265471,
+            "heading": 0.3325883455828011,
+            "angularVelocity": -1.0899722636212008,
+            "velocityX": 0.2171102722189281,
+            "velocityY": -0.26017204737900074,
+            "timestamp": 0.052672582902671385
+        },
+        {
+            "x": 2.3359939039892414,
+            "y": 6.71538911279917,
+            "heading": 0.2289766220305027,
+            "angularVelocity": -1.9670902363712421,
+            "velocityX": 0.46624152117519013,
+            "velocityY": -0.529819346772266,
+            "timestamp": 0.10534516580534277
+        },
+        {
+            "x": 2.3766647023789824,
+            "y": 6.674615688796359,
+            "heading": 0.08902544501665462,
+            "angularVelocity": -2.6570023587499336,
+            "velocityX": 0.7721436115045406,
+            "velocityY": -0.7740919802272123,
+            "timestamp": 0.15801774870801416
+        },
+        {
+            "x": 2.4341603545379895,
+            "y": 6.621640281380937,
+            "heading": -0.08739056422595769,
+            "angularVelocity": -3.349294823240299,
+            "velocityX": 1.0915669783129407,
+            "velocityY": -1.0057491866937662,
+            "timestamp": 0.21069033161068554
+        },
+        {
+            "x": 2.508963105088746,
+            "y": 6.555426903517877,
+            "heading": -0.2922468734649172,
+            "angularVelocity": -3.8892398654057443,
+            "velocityX": 1.4201458601143102,
+            "velocityY": -1.2570748236404126,
+            "timestamp": 0.2633629145133569
+        },
+        {
+            "x": 2.602244928931498,
+            "y": 6.475307627430128,
+            "heading": -0.4990571722352239,
+            "angularVelocity": -3.9263367652285313,
+            "velocityX": 1.7709749304513536,
+            "velocityY": -1.5210812090949455,
+            "timestamp": 0.3160354974160283
+        },
+        {
+            "x": 2.7117676530035184,
+            "y": 6.382297804514516,
+            "heading": -0.6662019146269795,
+            "angularVelocity": -3.173277883497873,
+            "velocityX": 2.0793118172009177,
+            "velocityY": -1.7658109359755114,
+            "timestamp": 0.3687080803186997
+        },
+        {
+            "x": 2.833611348488951,
+            "y": 6.27498915923685,
+            "heading": -0.7793427065081729,
+            "angularVelocity": -2.1480015910792667,
+            "velocityX": 2.3132280357425485,
+            "velocityY": -2.0372770683364947,
+            "timestamp": 0.42138066322137113
+        },
+        {
+            "x": 2.968521622075034,
+            "y": 6.1546184694434425,
+            "heading": -0.8356933529016568,
+            "angularVelocity": -1.0698288044390758,
+            "velocityX": 2.5612997531442825,
+            "velocityY": -2.2852627146807634,
+            "timestamp": 0.47405324612404254
+        },
+        {
+            "x": 3.1177887530073343,
+            "y": 6.022632523021301,
+            "heading": -0.8357129106693775,
+            "angularVelocity": -0.00037130830959681015,
+            "velocityX": 2.8338676918144654,
+            "velocityY": -2.5057807904736045,
+            "timestamp": 0.5267258290267139
+        },
+        {
+            "x": 3.269422043665646,
+            "y": 5.893362401601714,
+            "heading": -0.8357131785361996,
+            "angularVelocity": -0.000005085507627644201,
+            "velocityX": 2.8787897289658386,
+            "velocityY": -2.45422028493365,
+            "timestamp": 0.5793984119293853
+        },
+        {
+            "x": 3.4210553878779164,
+            "y": 5.764092343000596,
+            "heading": -0.8357134464026715,
+            "angularVelocity": -0.000005085500977911915,
+            "velocityX": 2.8787907456989505,
+            "velocityY": -2.4542190923119205,
+            "timestamp": 0.6320709948320568
+        },
+        {
+            "x": 3.572688732091381,
+            "y": 5.634822284400852,
+            "heading": -0.8357137142690544,
+            "angularVelocity": -0.000005085499289329294,
+            "velocityX": 2.878790745721617,
+            "velocityY": -2.4542190922857987,
+            "timestamp": 0.6847435777347282
+        },
+        {
+            "x": 3.7243220763038787,
+            "y": 5.50555222579995,
+            "heading": -0.8357139821353484,
+            "angularVelocity": -0.000005085497600843703,
+            "velocityX": 2.8787907457032484,
+            "velocityY": -2.4542190923078118,
+            "timestamp": 0.7374161606373996
+        },
+        {
+            "x": 3.875955376681643,
+            "y": 5.376282115781124,
+            "heading": -0.8357142500017658,
+            "angularVelocity": -0.000005085499943938472,
+            "velocityX": 2.878789913491687,
+            "velocityY": -2.4542200684878277,
+            "timestamp": 0.790088743540071
+        },
+        {
+            "x": 4.025639474822054,
+            "y": 5.244759916762783,
+            "heading": -0.8357145286285158,
+            "angularVelocity": -0.000005289787108985714,
+            "velocityX": 2.8417839014463553,
+            "velocityY": -2.4969764490450044,
+            "timestamp": 0.8427613264427424
+        },
+        {
+            "x": 4.159073025842643,
+            "y": 5.096777164207917,
+            "heading": -0.835714908739186,
+            "angularVelocity": -0.000007216480551859566,
+            "velocityX": 2.5332638664625207,
+            "velocityY": -2.809483499761346,
+            "timestamp": 0.8954339093454138
+        },
+        {
+            "x": 4.274456024169922,
+            "y": 4.934326648712158,
+            "heading": -0.8357152799414105,
+            "angularVelocity": -0.000007047351849068254,
+            "velocityX": 2.19057034929318,
+            "velocityY": -3.0841570043363045,
+            "timestamp": 0.9481064922480852
+        },
+        {
+            "x": 4.3556906912761315,
+            "y": 4.793345794539918,
+            "heading": -0.8357152378475459,
+            "angularVelocity": 9.786625189030426e-7,
+            "velocityX": 1.8886677315079998,
+            "velocityY": -3.2777384277013035,
+            "timestamp": 0.9911181166598327
+        },
+        {
+            "x": 4.442446525481621,
+            "y": 4.655693706781043,
+            "heading": -0.8357151935315337,
+            "angularVelocity": 0.0000010303263992905167,
+            "velocityX": 2.0170322649286843,
+            "velocityY": -3.200346177143663,
+            "timestamp": 1.0341297410715804
+        },
+        {
+            "x": 4.541847641111602,
+            "y": 4.5268757636785075,
+            "heading": -0.8357148699573956,
+            "angularVelocity": 0.000007522946236810339,
+            "velocityX": 2.311029099445763,
+            "velocityY": -2.9949564766345187,
+            "timestamp": 1.077141365483328
+        },
+        {
+            "x": 4.65067034095477,
+            "y": 4.409944405402937,
+            "heading": -0.8255512517653536,
+            "angularVelocity": 0.23629933375095027,
+            "velocityX": 2.530076492843306,
+            "velocityY": -2.7185989805033612,
+            "timestamp": 1.1201529898950755
+        },
+        {
+            "x": 4.7668457664948924,
+            "y": 4.306168868140065,
+            "heading": -0.803496324956771,
+            "angularVelocity": 0.5127666557638517,
+            "velocityX": 2.701023900608423,
+            "velocityY": -2.4127323411326107,
+            "timestamp": 1.163164614306823
+        },
+        {
+            "x": 4.88912359332006,
+            "y": 4.216234980411927,
+            "heading": -0.7705722789003837,
+            "angularVelocity": 0.7654685566210554,
+            "velocityX": 2.8429018549638743,
+            "velocityY": -2.0909205117948377,
+            "timestamp": 1.2061762387185706
+        },
+        {
+            "x": 5.016560626655114,
+            "y": 4.140615450479534,
+            "heading": -0.7285529163528696,
+            "angularVelocity": 0.9769303792218019,
+            "velocityX": 2.9628509752411754,
+            "velocityY": -1.7581184381341082,
+            "timestamp": 1.2491878631303182
+        },
+        {
+            "x": 5.148349456504816,
+            "y": 4.079665503809577,
+            "heading": -0.6795461936491729,
+            "angularVelocity": 1.1393832103284065,
+            "velocityX": 3.0640281935900804,
+            "velocityY": -1.4170575397591803,
+            "timestamp": 1.2921994875420657
+        },
+        {
+            "x": 5.28369745675312,
+            "y": 4.033673765712218,
+            "heading": -0.6261187752553977,
+            "angularVelocity": 1.2421623020399764,
+            "velocityX": 3.1467772282354725,
+            "velocityY": -1.0692862389265216,
+            "timestamp": 1.3352111119538133
+        },
+        {
+            "x": 5.421794981888129,
+            "y": 4.0028549700404525,
+            "heading": -0.5712230670658915,
+            "angularVelocity": 1.2762993479156466,
+            "velocityX": 3.2107023862435504,
+            "velocityY": -0.7165224771968601,
+            "timestamp": 1.3782227363655608
+        },
+        {
+            "x": 5.5620425748748135,
+            "y": 3.9873099729843906,
+            "heading": -0.5170015662419462,
+            "angularVelocity": 1.2606243443606422,
+            "velocityX": 3.260690450658235,
+            "velocityY": -0.3614138565716755,
+            "timestamp": 1.4212343607773084
         },
         {
             "x": 5.703982353210449,
             "y": 3.987083911895752,
-            "heading": 0.16151347881375258,
-            "angularVelocity": -0.32215526782320836,
-            "velocityX": 3.5698675674478513,
-            "velocityY": -0.005685567203615865,
-            "timestamp": 1.4192794756416105
-        },
-        {
-            "x": 5.829773721796989,
-            "y": 3.97718762682745,
-            "heading": 0.15213932045666498,
-            "angularVelocity": -0.2709697131085786,
-            "velocityX": 3.6361292138465493,
-            "velocityY": -0.2860623240667741,
-            "timestamp": 1.453874332201581
-        },
-        {
-            "x": 5.956799770812804,
-            "y": 3.9574240312185514,
-            "heading": 0.1457481223879076,
-            "angularVelocity": -0.18474417021149397,
-            "velocityX": 3.671818924747189,
-            "velocityY": -0.5712871095342833,
-            "timestamp": 1.4884691887615515
-        },
-        {
-            "x": 6.083503083107889,
-            "y": 3.927762559716917,
-            "heading": 0.14376941436575622,
-            "angularVelocity": -0.05719659564771788,
-            "velocityX": 3.6624898870571543,
-            "velocityY": -0.8573954180216341,
-            "timestamp": 1.523064045321522
-        },
-        {
-            "x": 6.208289204844049,
-            "y": 3.888323576656908,
-            "heading": 0.14376927335150447,
-            "angularVelocity": -0.000004076162347950456,
-            "velocityX": 3.607071517115297,
-            "velocityY": -1.1400244713152792,
-            "timestamp": 1.5576589018814926
-        },
-        {
-            "x": 6.329707486841884,
-            "y": 3.8394909787467797,
-            "heading": 0.14376925272536353,
-            "angularVelocity": -5.962198716671582e-7,
-            "velocityX": 3.5097206368627396,
-            "velocityY": -1.4115565944167625,
-            "timestamp": 1.592253758441463
-        },
-        {
-            "x": 6.447051924752506,
-            "y": 3.781548715234953,
-            "heading": 0.14376923552481305,
-            "angularVelocity": -4.971996473322837e-7,
-            "velocityX": 3.3919619729367447,
-            "velocityY": -1.6748808717094477,
-            "timestamp": 1.6268486150014336
-        },
-        {
-            "x": 6.559640243151509,
-            "y": 3.714833691264027,
-            "heading": 0.14376922059812475,
-            "angularVelocity": -4.3147131718664413e-7,
-            "velocityX": 3.254481434366676,
-            "velocityY": -1.9284665584687428,
-            "timestamp": 1.6614434715614042
-        },
-        {
-            "x": 6.666817827672777,
-            "y": 3.6397338254007683,
-            "heading": 0.14376920720482705,
-            "angularVelocity": -3.8714707984086e-7,
-            "velocityX": 3.0980785925640646,
-            "velocityY": -2.1708390590685647,
-            "timestamp": 1.6960383281213747
-        },
-        {
-            "x": 6.7679615406661,
-            "y": 3.5566858017838987,
-            "heading": 0.14376919483153264,
-            "angularVelocity": -3.5766283306602865e-7,
-            "velocityX": 2.923663314457974,
-            "velocityY": -2.4005887543688678,
-            "timestamp": 1.7306331846813452
-        },
-        {
-            "x": 6.862483380166932,
-            "y": 3.466172559439492,
-            "heading": 0.1437691830947059,
-            "angularVelocity": -3.392650779789794e-7,
-            "velocityX": 2.7322512332715396,
-            "velocityY": -2.6163785991568234,
-            "timestamp": 1.7652280412413157
-        },
-        {
-            "x": 6.949834140776404,
-            "y": 3.3687206741301803,
-            "heading": 0.14376917168351727,
-            "angularVelocity": -3.2985217383486685e-7,
-            "velocityX": 2.5249638037391158,
-            "velocityY": -2.8169472285678507,
-            "timestamp": 1.7998228978012862
-        },
-        {
-            "x": 7.031656177612435,
-            "y": 3.2665826319797073,
-            "heading": 0.14376916034823112,
-            "angularVelocity": -3.2765813438231676e-7,
-            "velocityX": 2.3651503423404003,
-            "velocityY": -2.952405424009072,
-            "timestamp": 1.8344177543612568
-        },
-        {
-            "x": 7.113477155727132,
-            "y": 3.1644437417043827,
-            "heading": 0.14376914901287846,
-            "angularVelocity": -3.2766005667768915e-7,
-            "velocityX": 2.3651197389085663,
-            "velocityY": -2.952429939932413,
-            "timestamp": 1.8690126109212273
-        },
-        {
-            "x": 7.194171820782979,
-            "y": 3.0635723954548135,
-            "heading": 0.13949339206360356,
-            "angularVelocity": -0.12359516339843184,
-            "velocityX": 2.332562498588806,
-            "velocityY": -2.91579027288948,
-            "timestamp": 1.9036074674811978
-        },
-        {
-            "x": 7.268927567281138,
-            "y": 2.9702304150182344,
-            "heading": 0.12535509322542532,
-            "angularVelocity": -0.4086821060717307,
-            "velocityX": 2.160891933995128,
-            "velocityY": -2.6981461904537576,
-            "timestamp": 1.9382023240411683
-        },
-        {
-            "x": 7.337464553197734,
-            "y": 2.884657135542947,
-            "heading": 0.10855528307456563,
-            "angularVelocity": -0.48561583487814164,
-            "velocityX": 1.9811322471531543,
-            "velocityY": -2.473583878775318,
-            "timestamp": 1.9727971806011388
-        },
-        {
-            "x": 7.399764936518732,
-            "y": 2.806872483431395,
-            "heading": 0.0911966728700688,
-            "angularVelocity": -0.5017685266133554,
-            "velocityX": 1.8008568184984473,
-            "velocityY": -2.2484455738878646,
-            "timestamp": 2.0073920371611096
-        },
-        {
-            "x": 7.455827979936063,
-            "y": 2.7368764943083312,
-            "heading": 0.07427285825902008,
-            "angularVelocity": -0.4892003116622579,
-            "velocityX": 1.6205600771936914,
-            "velocityY": -2.023306239230242,
-            "timestamp": 2.0419868937210803
-        },
-        {
-            "x": 7.505655491239628,
-            "y": 2.674666404415114,
-            "heading": 0.05836114831277334,
-            "angularVelocity": -0.4599443827339935,
-            "velocityX": 1.4403155919200792,
-            "velocityY": -1.7982467938658737,
-            "timestamp": 2.076581750281051
-        },
-        {
-            "x": 7.549249543273319,
-            "y": 2.6202393080696202,
-            "heading": 0.04383838388032337,
-            "angularVelocity": -0.4197954805008245,
-            "velocityX": 1.2601310243365538,
-            "velocityY": -1.5732713402393688,
-            "timestamp": 2.111176606841022
-        },
-        {
-            "x": 7.586612019746033,
-            "y": 2.5735926396425732,
-            "heading": 0.030969705386613037,
-            "angularVelocity": -0.3719824208954968,
-            "velocityX": 1.0800009073009074,
-            "velocityY": -1.348370048772548,
-            "timestamp": 2.1457714634009926
-        },
-        {
-            "x": 7.6177445449139505,
-            "y": 2.5347242182058114,
-            "heading": 0.01995171749042388,
-            "angularVelocity": -0.31848630090688007,
-            "velocityX": 0.8999177410650449,
-            "velocityY": -1.1235317992830327,
-            "timestamp": 2.1803663199609633
-        },
-        {
-            "x": 7.642648503297853,
-            "y": 2.503632200129417,
-            "heading": 0.010935986949086615,
-            "angularVelocity": -0.26060898751548167,
-            "velocityX": 0.7198745958298824,
-            "velocityY": -0.8987468418172337,
-            "timestamp": 2.214961176520934
-        },
-        {
-            "x": 7.661325077102299,
-            "y": 2.4803150199755253,
-            "heading": 0.004042922192056018,
-            "angularVelocity": -0.19925114431624774,
-            "velocityX": 0.5398656234365572,
-            "velocityY": -0.6740071349471043,
-            "timestamp": 2.249556033080905
-        },
-        {
-            "x": 7.673775282436928,
-            "y": 2.464771338402636,
-            "heading": -0.0006295042844945549,
-            "angularVelocity": -0.13506130509461317,
-            "velocityX": 0.3598860227400032,
-            "velocityY": -0.44930614312403183,
-            "timestamp": 2.2841508896408755
+            "heading": -0.46507695218435896,
+            "angularVelocity": 1.207222809362328,
+            "velocityX": 3.3000329626441363,
+            "velocityY": -0.005255813788261737,
+            "timestamp": 1.464245985189056
+        },
+        {
+            "x": 5.957415351644791,
+            "y": 3.94389686592464,
+            "heading": -0.40627705964587574,
+            "angularVelocity": 0.8004851872173009,
+            "velocityX": 3.450165509502976,
+            "velocityY": -0.5879362884366123,
+            "timestamp": 1.5377013013865826
+        },
+        {
+            "x": 6.214659603999478,
+            "y": 3.857155103461619,
+            "heading": -0.3855287596647175,
+            "angularVelocity": 0.2824615161326723,
+            "velocityX": 3.502050847660077,
+            "velocityY": -1.180877939858924,
+            "timestamp": 1.6111566175841092
+        },
+        {
+            "x": 6.460339332530576,
+            "y": 3.727320070546642,
+            "heading": -0.38552868222223635,
+            "angularVelocity": 0.0000010542801416145007,
+            "velocityX": 3.3446146752734305,
+            "velocityY": -1.7675375947717866,
+            "timestamp": 1.6846119337816359
+        },
+        {
+            "x": 6.681847040612612,
+            "y": 3.5595404285567858,
+            "heading": -0.3855286464284339,
+            "angularVelocity": 4.872867523834452e-7,
+            "velocityX": 3.0155435923301614,
+            "velocityY": -2.2841048228379357,
+            "timestamp": 1.7580672499791625
+        },
+        {
+            "x": 6.873376982747171,
+            "y": 3.358214932798401,
+            "heading": -0.3855286139444744,
+            "angularVelocity": 4.422274806376815e-7,
+            "velocityX": 2.607434724254966,
+            "velocityY": -2.740788634235891,
+            "timestamp": 1.8315225661766892
+        },
+        {
+            "x": 7.058369006072979,
+            "y": 3.1508658904491242,
+            "heading": -0.3855285817822777,
+            "angularVelocity": 4.3784709334515964e-7,
+            "velocityX": 2.518429337753462,
+            "velocityY": -2.8227915021385286,
+            "timestamp": 1.9049778823742158
+        },
+        {
+            "x": 7.235640342207717,
+            "y": 2.9543486280986375,
+            "heading": -0.3517392740895451,
+            "angularVelocity": 0.4599981245995971,
+            "velocityX": 2.413322075396734,
+            "velocityY": -2.6753306979448217,
+            "timestamp": 1.9784331985717425
+        },
+        {
+            "x": 7.3842205894533075,
+            "y": 2.7881464841241645,
+            "heading": -0.2579536451103976,
+            "angularVelocity": 1.2767711560447332,
+            "velocityX": 2.0227296666458727,
+            "velocityY": -2.2626292088586646,
+            "timestamp": 2.051888514769269
+        },
+        {
+            "x": 7.502781206912582,
+            "y": 2.6554541566341396,
+            "heading": -0.1644602811973769,
+            "angularVelocity": 1.2727923416953275,
+            "velocityX": 1.6140508760517314,
+            "velocityY": -1.806435998903144,
+            "timestamp": 2.125343830966796
+        },
+        {
+            "x": 7.591494122264987,
+            "y": 2.5561305844890163,
+            "heading": -0.0867965293639426,
+            "angularVelocity": 1.0572924582421084,
+            "velocityX": 1.2077126604948574,
+            "velocityY": -1.3521631555983635,
+            "timestamp": 2.1987991471643227
+        },
+        {
+            "x": 7.6505265885867635,
+            "y": 2.4900164938031613,
+            "heading": -0.031723308715129774,
+            "angularVelocity": 0.7497513250193758,
+            "velocityX": 0.8036513812429008,
+            "velocityY": -0.900058622143418,
+            "timestamp": 2.2722544633618496
         },
         {
             "x": 7.68,
             "y": 2.457,
             "heading": -0.003,
-            "angularVelocity": -0.06852162290068065,
-            "velocityX": 0.17993187953478723,
-            "velocityY": -0.22463854964001964,
-            "timestamp": 2.3187457462008463
+            "angularVelocity": 0.39103104039319253,
+            "velocityX": 0.401242727401519,
+            "velocityY": -0.4494772538229548,
+            "timestamp": 2.3457097795593764
         },
         {
             "x": 7.68,
             "y": 2.457,
             "heading": -0.003,
-            "angularVelocity": -8.413636330536319e-34,
-            "velocityX": 0,
-            "velocityY": 4.110332962762654e-35,
-            "timestamp": 2.353340602760817
+            "angularVelocity": -1.5819357203294863e-32,
+            "velocityX": 5.378736393413154e-34,
+            "velocityY": 0,
+            "timestamp": 2.4191650957569033
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.traj
index 1a6d423b..785ed7b8 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.traj	
@@ -4,577 +4,397 @@
             "x": 2.3,
             "y": 6.757,
             "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 1.0547928981710615e-31,
-            "velocityY": 1.4824740100837611e-32,
+            "angularVelocity": 1.592522039385819e-32,
+            "velocityX": 0,
+            "velocityY": -2.1348488243236776e-32,
             "timestamp": 0
         },
         {
-            "x": 2.3072711000078927,
-            "y": 6.749947927475375,
-            "heading": 0.3875624240531776,
-            "angularVelocity": -0.06985747897751753,
-            "velocityX": 0.2083794421285326,
-            "velocityY": -0.20210242424612682,
-            "timestamp": 0.034893557318421635
-        },
-        {
-            "x": 2.321814346754588,
-            "y": 6.735842802544083,
-            "heading": 0.3827646159677583,
-            "angularVelocity": -0.1374983938048157,
-            "velocityX": 0.41678888208448434,
-            "velocityY": -0.40423293052572296,
-            "timestamp": 0.06978711463684327
-        },
-        {
-            "x": 2.343630936460155,
-            "y": 6.714683501867812,
-            "heading": 0.3756983254810237,
-            "angularVelocity": -0.20250989093060998,
-            "velocityX": 0.6252326040156703,
-            "velocityY": -0.6063956300924075,
-            "timestamp": 0.1046806719552649
-        },
-        {
-            "x": 2.372722244735657,
-            "y": 6.6864687287677045,
-            "heading": 0.36647405587954873,
-            "angularVelocity": -0.2643545201567945,
-            "velocityX": 0.8337157490143364,
-            "velocityY": -0.8085954906412398,
-            "timestamp": 0.13957422927368654
-        },
-        {
-            "x": 2.4090898628559576,
-            "y": 6.651196976344128,
-            "heading": 0.3552274150516352,
-            "angularVelocity": -0.3223128191053197,
-            "velocityX": 1.042244497699901,
-            "velocityY": -1.0108385368022033,
-            "timestamp": 0.17446778659210818
-        },
-        {
-            "x": 2.452735639247045,
-            "y": 6.608866484215248,
-            "heading": 0.3421287688368095,
-            "angularVelocity": -0.3753886740550377,
-            "velocityX": 1.2508262196599174,
-            "velocityY": -1.213132033016619,
-            "timestamp": 0.20936134391052982
-        },
-        {
-            "x": 2.5036617186589587,
-            "y": 6.55947519544937,
-            "heading": 0.3273986519293141,
-            "angularVelocity": -0.42214431658760004,
-            "velocityX": 1.459469407122573,
-            "velocityY": -1.4154844779841917,
-            "timestamp": 0.24425490122895147
-        },
-        {
-            "x": 2.5618705496598224,
-            "y": 6.503020741236739,
-            "heading": 0.31133395345849213,
-            "angularVelocity": -0.4603915365872077,
-            "velocityX": 1.6681827670844436,
-            "velocityY": -1.6179048096889317,
-            "timestamp": 0.2791484585473731
-        },
-        {
-            "x": 2.6273647520175327,
-            "y": 6.43950055782557,
-            "heading": 0.294356217256913,
-            "angularVelocity": -0.486557906568497,
-            "velocityX": 1.8769712058889019,
-            "velocityY": -1.8203986148936577,
-            "timestamp": 0.3140420158657947
-        },
-        {
-            "x": 2.7001464092257668,
-            "y": 6.368912559254549,
-            "heading": 0.2771114307132975,
-            "angularVelocity": -0.4942111916606252,
-            "velocityX": 2.0858193546752704,
-            "velocityY": -2.022952200799379,
-            "timestamp": 0.34893557318421636
-        },
-        {
-            "x": 2.7802136587601143,
-            "y": 6.291258423996795,
-            "heading": 0.2607138645949654,
-            "angularVelocity": -0.46993105256354084,
-            "velocityX": 2.294614126146364,
-            "velocityY": -2.225457684039485,
-            "timestamp": 0.383829130502638
-        },
-        {
-            "x": 2.867539127268515,
-            "y": 6.206563552711186,
-            "heading": 0.24753249327015822,
-            "angularVelocity": -0.37775945870237265,
-            "velocityX": 2.5026244160637545,
-            "velocityY": -2.427235220322282,
-            "timestamp": 0.41872268782105965
-        },
-        {
-            "x": 2.961704724093414,
-            "y": 6.1150775036748035,
-            "heading": 0.24574502539338514,
-            "angularVelocity": -0.05122630118968667,
-            "velocityX": 2.6986528190746,
-            "velocityY": -2.6218607693541762,
-            "timestamp": 0.4536162451394813
-        },
-        {
-            "x": 3.0564622015652247,
-            "y": 6.023180384324785,
-            "heading": 0.2457450132145974,
-            "angularVelocity": -3.4902683097432965e-7,
-            "velocityX": 2.7156152812710346,
-            "velocityY": -2.633641463133395,
-            "timestamp": 0.48850980245790293
-        },
-        {
-            "x": 3.1512196852998877,
-            "y": 5.931283271432529,
-            "heading": 0.24574500103583913,
-            "angularVelocity": -3.490259866463667e-7,
-            "velocityX": 2.7156154607555005,
-            "velocityY": -2.6336412780630685,
-            "timestamp": 0.5234033597763246
-        },
-        {
-            "x": 3.2459771690350276,
-            "y": 5.839386158540766,
-            "heading": 0.2457449888570809,
-            "angularVelocity": -3.4902598588384144e-7,
-            "velocityX": 2.7156154607691696,
-            "velocityY": -2.6336412780489744,
-            "timestamp": 0.5582969170947462
-        },
-        {
-            "x": 3.3407346527701676,
-            "y": 5.747489045649002,
-            "heading": 0.24574497667832262,
-            "angularVelocity": -3.4902598639891e-7,
-            "velocityX": 2.7156154607691705,
-            "velocityY": -2.6336412780489735,
-            "timestamp": 0.5931904744131677
-        },
-        {
-            "x": 3.4354921365053075,
-            "y": 5.655591932757238,
-            "heading": 0.24574496449956437,
-            "angularVelocity": -3.4902598640760607e-7,
-            "velocityX": 2.71561546076917,
-            "velocityY": -2.6336412780489735,
-            "timestamp": 0.6280840317315893
-        },
-        {
-            "x": 3.5302496202404474,
-            "y": 5.563694819865473,
-            "heading": 0.2457449523208061,
-            "angularVelocity": -3.490259865548224e-7,
-            "velocityX": 2.7156154607691705,
-            "velocityY": -2.6336412780489735,
-            "timestamp": 0.6629775890500109
-        },
-        {
-            "x": 3.6250071039755873,
-            "y": 5.471797706973709,
-            "heading": 0.2457449401420478,
-            "angularVelocity": -3.4902598644057735e-7,
-            "velocityX": 2.71561546076917,
-            "velocityY": -2.633641278048974,
-            "timestamp": 0.6978711463684325
-        },
-        {
-            "x": 3.719764587710727,
-            "y": 5.379900594081945,
-            "heading": 0.24574492796328956,
-            "angularVelocity": -3.4902598592532335e-7,
-            "velocityX": 2.715615460769166,
-            "velocityY": -2.633641278048979,
-            "timestamp": 0.7327647036868541
-        },
-        {
-            "x": 3.8145220714436685,
-            "y": 5.288003481187913,
-            "heading": 0.24574491578453134,
-            "angularVelocity": -3.490259859254125e-7,
-            "velocityX": 2.7156154607061804,
-            "velocityY": -2.6336412781139247,
-            "timestamp": 0.7676582610052757
-        },
-        {
-            "x": 3.909279526317127,
-            "y": 5.196106338536137,
-            "heading": 0.245744903605773,
-            "angularVelocity": -3.4902598866436484e-7,
-            "velocityX": 2.7156146336342,
-            "velocityY": -2.6336421309288234,
-            "timestamp": 0.8025518183236973
-        },
-        {
-            "x": 4.003658642045134,
-            "y": 5.103820680647278,
-            "heading": 0.24574489142231412,
-            "angularVelocity": -3.4916069991394096e-7,
-            "velocityX": 2.7047719688402356,
-            "velocityY": -2.644776428116158,
-            "timestamp": 0.8374453756421188
-        },
-        {
-            "x": 4.090673374879912,
-            "y": 5.004560939161802,
-            "heading": 0.24574487906407927,
-            "angularVelocity": -3.541695320845138e-7,
-            "velocityX": 2.493719171155964,
-            "velocityY": -2.8446437999909007,
-            "timestamp": 0.8723389329605404
-        },
-        {
-            "x": 4.169802665710449,
-            "y": 4.898907661437988,
-            "heading": 0.24574486610274957,
-            "angularVelocity": -3.7145337638188186e-7,
-            "velocityX": 2.267733556325072,
-            "velocityY": -3.0278735056925665,
-            "timestamp": 0.907232490278962
-        },
-        {
-            "x": 4.2677114826543,
-            "y": 4.731769321945997,
-            "heading": 0.24574485481420902,
-            "angularVelocity": -2.2045907687430814e-7,
-            "velocityX": 1.9121061102333996,
-            "velocityY": -3.264121150398313,
-            "timestamp": 0.9584371888152268
-        },
-        {
-            "x": 4.383830202938794,
-            "y": 4.576728030606808,
-            "heading": 0.24574484074146902,
-            "angularVelocity": -2.7483298164960767e-7,
-            "velocityX": 2.2677356493419163,
-            "velocityY": -3.02787235881067,
-            "timestamp": 1.0096418873514916
-        },
-        {
-            "x": 4.516679781731222,
-            "y": 4.435758724265,
-            "heading": 0.24574482059862868,
-            "angularVelocity": -3.9337875070195686e-7,
-            "velocityX": 2.59448024478339,
-            "velocityY": -2.753054121429295,
-            "timestamp": 1.0608465858877565
-        },
-        {
-            "x": 4.664567883303712,
-            "y": 4.3106571835035465,
-            "heading": 0.24574435877996634,
-            "angularVelocity": -0.000009019068084382453,
-            "velocityX": 2.8881744410183567,
-            "velocityY": -2.4431652629075082,
-            "timestamp": 1.1120512844240213
-        },
-        {
-            "x": 4.823636899736968,
-            "y": 4.204273204304783,
-            "heading": 0.2400133838174815,
-            "angularVelocity": -0.1119228337693714,
-            "velocityX": 3.1065316461261507,
-            "velocityY": -2.0776214339670136,
-            "timestamp": 1.1632559829602862
-        },
-        {
-            "x": 4.9908096959943835,
-            "y": 4.118113363926556,
-            "heading": 0.22784637628221624,
-            "angularVelocity": -0.23761506039622932,
-            "velocityX": 3.26479407234511,
-            "velocityY": -1.6826549680241993,
-            "timestamp": 1.214460681496551
-        },
-        {
-            "x": 5.163718040144699,
-            "y": 4.053028192881653,
-            "heading": 0.21228377101249774,
-            "angularVelocity": -0.3039292431083551,
-            "velocityX": 3.3768062129661107,
-            "velocityY": -1.2710781023114057,
-            "timestamp": 1.265665380032816
-        },
-        {
-            "x": 5.340853559631665,
-            "y": 4.009388363770341,
-            "heading": 0.19524077553782296,
-            "angularVelocity": -0.332840461165967,
-            "velocityX": 3.4593606553803307,
-            "velocityY": -0.8522622016884831,
-            "timestamp": 1.3168700785690808
-        },
-        {
-            "x": 5.521188360604893,
-            "y": 3.987375039650421,
-            "heading": 0.17800934218450973,
-            "angularVelocity": -0.3365205507676074,
-            "velocityX": 3.521840888204999,
-            "velocityY": -0.42990828477058174,
-            "timestamp": 1.3680747771053456
+            "x": 2.311435758812473,
+            "y": 6.743296066265471,
+            "heading": 0.3325883455828011,
+            "angularVelocity": -1.0899722636212008,
+            "velocityX": 0.2171102722189281,
+            "velocityY": -0.26017204737900074,
+            "timestamp": 0.052672582902671385
+        },
+        {
+            "x": 2.3359939039892414,
+            "y": 6.71538911279917,
+            "heading": 0.2289766220305027,
+            "angularVelocity": -1.9670902363712421,
+            "velocityX": 0.46624152117519013,
+            "velocityY": -0.529819346772266,
+            "timestamp": 0.10534516580534277
+        },
+        {
+            "x": 2.3766647023789824,
+            "y": 6.674615688796359,
+            "heading": 0.08902544501665462,
+            "angularVelocity": -2.6570023587499336,
+            "velocityX": 0.7721436115045406,
+            "velocityY": -0.7740919802272123,
+            "timestamp": 0.15801774870801416
+        },
+        {
+            "x": 2.4341603545379895,
+            "y": 6.621640281380937,
+            "heading": -0.08739056422595769,
+            "angularVelocity": -3.349294823240299,
+            "velocityX": 1.0915669783129407,
+            "velocityY": -1.0057491866937662,
+            "timestamp": 0.21069033161068554
+        },
+        {
+            "x": 2.508963105088746,
+            "y": 6.555426903517877,
+            "heading": -0.2922468734649172,
+            "angularVelocity": -3.8892398654057443,
+            "velocityX": 1.4201458601143102,
+            "velocityY": -1.2570748236404126,
+            "timestamp": 0.2633629145133569
+        },
+        {
+            "x": 2.602244928931498,
+            "y": 6.475307627430128,
+            "heading": -0.4990571722352239,
+            "angularVelocity": -3.9263367652285313,
+            "velocityX": 1.7709749304513536,
+            "velocityY": -1.5210812090949455,
+            "timestamp": 0.3160354974160283
+        },
+        {
+            "x": 2.7117676530035184,
+            "y": 6.382297804514516,
+            "heading": -0.6662019146269795,
+            "angularVelocity": -3.173277883497873,
+            "velocityX": 2.0793118172009177,
+            "velocityY": -1.7658109359755114,
+            "timestamp": 0.3687080803186997
+        },
+        {
+            "x": 2.833611348488951,
+            "y": 6.27498915923685,
+            "heading": -0.7793427065081729,
+            "angularVelocity": -2.1480015910792667,
+            "velocityX": 2.3132280357425485,
+            "velocityY": -2.0372770683364947,
+            "timestamp": 0.42138066322137113
+        },
+        {
+            "x": 2.968521622075034,
+            "y": 6.1546184694434425,
+            "heading": -0.8356933529016568,
+            "angularVelocity": -1.0698288044390758,
+            "velocityX": 2.5612997531442825,
+            "velocityY": -2.2852627146807634,
+            "timestamp": 0.47405324612404254
+        },
+        {
+            "x": 3.1177887530073343,
+            "y": 6.022632523021301,
+            "heading": -0.8357129106693775,
+            "angularVelocity": -0.00037130830959681015,
+            "velocityX": 2.8338676918144654,
+            "velocityY": -2.5057807904736045,
+            "timestamp": 0.5267258290267139
+        },
+        {
+            "x": 3.269422043665646,
+            "y": 5.893362401601714,
+            "heading": -0.8357131785361996,
+            "angularVelocity": -0.000005085507627644201,
+            "velocityX": 2.8787897289658386,
+            "velocityY": -2.45422028493365,
+            "timestamp": 0.5793984119293853
+        },
+        {
+            "x": 3.4210553878779164,
+            "y": 5.764092343000596,
+            "heading": -0.8357134464026715,
+            "angularVelocity": -0.000005085500977911915,
+            "velocityX": 2.8787907456989505,
+            "velocityY": -2.4542190923119205,
+            "timestamp": 0.6320709948320568
+        },
+        {
+            "x": 3.572688732091381,
+            "y": 5.634822284400852,
+            "heading": -0.8357137142690544,
+            "angularVelocity": -0.000005085499289329294,
+            "velocityX": 2.878790745721617,
+            "velocityY": -2.4542190922857987,
+            "timestamp": 0.6847435777347282
+        },
+        {
+            "x": 3.7243220763038787,
+            "y": 5.50555222579995,
+            "heading": -0.8357139821353484,
+            "angularVelocity": -0.000005085497600843703,
+            "velocityX": 2.8787907457032484,
+            "velocityY": -2.4542190923078118,
+            "timestamp": 0.7374161606373996
+        },
+        {
+            "x": 3.875955376681643,
+            "y": 5.376282115781124,
+            "heading": -0.8357142500017658,
+            "angularVelocity": -0.000005085499943938472,
+            "velocityX": 2.878789913491687,
+            "velocityY": -2.4542200684878277,
+            "timestamp": 0.790088743540071
+        },
+        {
+            "x": 4.025639474822054,
+            "y": 5.244759916762783,
+            "heading": -0.8357145286285158,
+            "angularVelocity": -0.000005289787108985714,
+            "velocityX": 2.8417839014463553,
+            "velocityY": -2.4969764490450044,
+            "timestamp": 0.8427613264427424
+        },
+        {
+            "x": 4.159073025842643,
+            "y": 5.096777164207917,
+            "heading": -0.835714908739186,
+            "angularVelocity": -0.000007216480551859566,
+            "velocityX": 2.5332638664625207,
+            "velocityY": -2.809483499761346,
+            "timestamp": 0.8954339093454138
+        },
+        {
+            "x": 4.274456024169922,
+            "y": 4.934326648712158,
+            "heading": -0.8357152799414105,
+            "angularVelocity": -0.000007047351849068254,
+            "velocityX": 2.19057034929318,
+            "velocityY": -3.0841570043363045,
+            "timestamp": 0.9481064922480852
+        },
+        {
+            "x": 4.3556906912761315,
+            "y": 4.793345794539918,
+            "heading": -0.8357152378475459,
+            "angularVelocity": 9.786625189030426e-7,
+            "velocityX": 1.8886677315079998,
+            "velocityY": -3.2777384277013035,
+            "timestamp": 0.9911181166598327
+        },
+        {
+            "x": 4.442446525481621,
+            "y": 4.655693706781043,
+            "heading": -0.8357151935315337,
+            "angularVelocity": 0.0000010303263992905167,
+            "velocityX": 2.0170322649286843,
+            "velocityY": -3.200346177143663,
+            "timestamp": 1.0341297410715804
+        },
+        {
+            "x": 4.541847641111602,
+            "y": 4.5268757636785075,
+            "heading": -0.8357148699573956,
+            "angularVelocity": 0.000007522946236810339,
+            "velocityX": 2.311029099445763,
+            "velocityY": -2.9949564766345187,
+            "timestamp": 1.077141365483328
+        },
+        {
+            "x": 4.65067034095477,
+            "y": 4.409944405402937,
+            "heading": -0.8255512517653536,
+            "angularVelocity": 0.23629933375095027,
+            "velocityX": 2.530076492843306,
+            "velocityY": -2.7185989805033612,
+            "timestamp": 1.1201529898950755
+        },
+        {
+            "x": 4.7668457664948924,
+            "y": 4.306168868140065,
+            "heading": -0.803496324956771,
+            "angularVelocity": 0.5127666557638517,
+            "velocityX": 2.701023900608423,
+            "velocityY": -2.4127323411326107,
+            "timestamp": 1.163164614306823
+        },
+        {
+            "x": 4.88912359332006,
+            "y": 4.216234980411927,
+            "heading": -0.7705722789003837,
+            "angularVelocity": 0.7654685566210554,
+            "velocityX": 2.8429018549638743,
+            "velocityY": -2.0909205117948377,
+            "timestamp": 1.2061762387185706
+        },
+        {
+            "x": 5.016560626655114,
+            "y": 4.140615450479534,
+            "heading": -0.7285529163528696,
+            "angularVelocity": 0.9769303792218019,
+            "velocityX": 2.9628509752411754,
+            "velocityY": -1.7581184381341082,
+            "timestamp": 1.2491878631303182
+        },
+        {
+            "x": 5.148349456504816,
+            "y": 4.079665503809577,
+            "heading": -0.6795461936491729,
+            "angularVelocity": 1.1393832103284065,
+            "velocityX": 3.0640281935900804,
+            "velocityY": -1.4170575397591803,
+            "timestamp": 1.2921994875420657
+        },
+        {
+            "x": 5.28369745675312,
+            "y": 4.033673765712218,
+            "heading": -0.6261187752553977,
+            "angularVelocity": 1.2421623020399764,
+            "velocityX": 3.1467772282354725,
+            "velocityY": -1.0692862389265216,
+            "timestamp": 1.3352111119538133
+        },
+        {
+            "x": 5.421794981888129,
+            "y": 4.0028549700404525,
+            "heading": -0.5712230670658915,
+            "angularVelocity": 1.2762993479156466,
+            "velocityX": 3.2107023862435504,
+            "velocityY": -0.7165224771968601,
+            "timestamp": 1.3782227363655608
+        },
+        {
+            "x": 5.5620425748748135,
+            "y": 3.9873099729843906,
+            "heading": -0.5170015662419462,
+            "angularVelocity": 1.2606243443606422,
+            "velocityX": 3.260690450658235,
+            "velocityY": -0.3614138565716755,
+            "timestamp": 1.4212343607773084
         },
         {
             "x": 5.703982353210449,
             "y": 3.987083911895752,
-            "heading": 0.16151347881375258,
-            "angularVelocity": -0.32215526782320836,
-            "velocityX": 3.5698675674478513,
-            "velocityY": -0.005685567203615865,
-            "timestamp": 1.4192794756416105
-        },
-        {
-            "x": 5.829773721796989,
-            "y": 3.97718762682745,
-            "heading": 0.15213932045666498,
-            "angularVelocity": -0.2709697131085786,
-            "velocityX": 3.6361292138465493,
-            "velocityY": -0.2860623240667741,
-            "timestamp": 1.453874332201581
-        },
-        {
-            "x": 5.956799770812804,
-            "y": 3.9574240312185514,
-            "heading": 0.1457481223879076,
-            "angularVelocity": -0.18474417021149397,
-            "velocityX": 3.671818924747189,
-            "velocityY": -0.5712871095342833,
-            "timestamp": 1.4884691887615515
-        },
-        {
-            "x": 6.083503083107889,
-            "y": 3.927762559716917,
-            "heading": 0.14376941436575622,
-            "angularVelocity": -0.05719659564771788,
-            "velocityX": 3.6624898870571543,
-            "velocityY": -0.8573954180216341,
-            "timestamp": 1.523064045321522
-        },
-        {
-            "x": 6.208289204844049,
-            "y": 3.888323576656908,
-            "heading": 0.14376927335150447,
-            "angularVelocity": -0.000004076162347950456,
-            "velocityX": 3.607071517115297,
-            "velocityY": -1.1400244713152792,
-            "timestamp": 1.5576589018814926
-        },
-        {
-            "x": 6.329707486841884,
-            "y": 3.8394909787467797,
-            "heading": 0.14376925272536353,
-            "angularVelocity": -5.962198716671582e-7,
-            "velocityX": 3.5097206368627396,
-            "velocityY": -1.4115565944167625,
-            "timestamp": 1.592253758441463
-        },
-        {
-            "x": 6.447051924752506,
-            "y": 3.781548715234953,
-            "heading": 0.14376923552481305,
-            "angularVelocity": -4.971996473322837e-7,
-            "velocityX": 3.3919619729367447,
-            "velocityY": -1.6748808717094477,
-            "timestamp": 1.6268486150014336
-        },
-        {
-            "x": 6.559640243151509,
-            "y": 3.714833691264027,
-            "heading": 0.14376922059812475,
-            "angularVelocity": -4.3147131718664413e-7,
-            "velocityX": 3.254481434366676,
-            "velocityY": -1.9284665584687428,
-            "timestamp": 1.6614434715614042
-        },
-        {
-            "x": 6.666817827672777,
-            "y": 3.6397338254007683,
-            "heading": 0.14376920720482705,
-            "angularVelocity": -3.8714707984086e-7,
-            "velocityX": 3.0980785925640646,
-            "velocityY": -2.1708390590685647,
-            "timestamp": 1.6960383281213747
-        },
-        {
-            "x": 6.7679615406661,
-            "y": 3.5566858017838987,
-            "heading": 0.14376919483153264,
-            "angularVelocity": -3.5766283306602865e-7,
-            "velocityX": 2.923663314457974,
-            "velocityY": -2.4005887543688678,
-            "timestamp": 1.7306331846813452
-        },
-        {
-            "x": 6.862483380166932,
-            "y": 3.466172559439492,
-            "heading": 0.1437691830947059,
-            "angularVelocity": -3.392650779789794e-7,
-            "velocityX": 2.7322512332715396,
-            "velocityY": -2.6163785991568234,
-            "timestamp": 1.7652280412413157
-        },
-        {
-            "x": 6.949834140776404,
-            "y": 3.3687206741301803,
-            "heading": 0.14376917168351727,
-            "angularVelocity": -3.2985217383486685e-7,
-            "velocityX": 2.5249638037391158,
-            "velocityY": -2.8169472285678507,
-            "timestamp": 1.7998228978012862
-        },
-        {
-            "x": 7.031656177612435,
-            "y": 3.2665826319797073,
-            "heading": 0.14376916034823112,
-            "angularVelocity": -3.2765813438231676e-7,
-            "velocityX": 2.3651503423404003,
-            "velocityY": -2.952405424009072,
-            "timestamp": 1.8344177543612568
-        },
-        {
-            "x": 7.113477155727132,
-            "y": 3.1644437417043827,
-            "heading": 0.14376914901287846,
-            "angularVelocity": -3.2766005667768915e-7,
-            "velocityX": 2.3651197389085663,
-            "velocityY": -2.952429939932413,
-            "timestamp": 1.8690126109212273
-        },
-        {
-            "x": 7.194171820782979,
-            "y": 3.0635723954548135,
-            "heading": 0.13949339206360356,
-            "angularVelocity": -0.12359516339843184,
-            "velocityX": 2.332562498588806,
-            "velocityY": -2.91579027288948,
-            "timestamp": 1.9036074674811978
-        },
-        {
-            "x": 7.268927567281138,
-            "y": 2.9702304150182344,
-            "heading": 0.12535509322542532,
-            "angularVelocity": -0.4086821060717307,
-            "velocityX": 2.160891933995128,
-            "velocityY": -2.6981461904537576,
-            "timestamp": 1.9382023240411683
-        },
-        {
-            "x": 7.337464553197734,
-            "y": 2.884657135542947,
-            "heading": 0.10855528307456563,
-            "angularVelocity": -0.48561583487814164,
-            "velocityX": 1.9811322471531543,
-            "velocityY": -2.473583878775318,
-            "timestamp": 1.9727971806011388
-        },
-        {
-            "x": 7.399764936518732,
-            "y": 2.806872483431395,
-            "heading": 0.0911966728700688,
-            "angularVelocity": -0.5017685266133554,
-            "velocityX": 1.8008568184984473,
-            "velocityY": -2.2484455738878646,
-            "timestamp": 2.0073920371611096
-        },
-        {
-            "x": 7.455827979936063,
-            "y": 2.7368764943083312,
-            "heading": 0.07427285825902008,
-            "angularVelocity": -0.4892003116622579,
-            "velocityX": 1.6205600771936914,
-            "velocityY": -2.023306239230242,
-            "timestamp": 2.0419868937210803
-        },
-        {
-            "x": 7.505655491239628,
-            "y": 2.674666404415114,
-            "heading": 0.05836114831277334,
-            "angularVelocity": -0.4599443827339935,
-            "velocityX": 1.4403155919200792,
-            "velocityY": -1.7982467938658737,
-            "timestamp": 2.076581750281051
-        },
-        {
-            "x": 7.549249543273319,
-            "y": 2.6202393080696202,
-            "heading": 0.04383838388032337,
-            "angularVelocity": -0.4197954805008245,
-            "velocityX": 1.2601310243365538,
-            "velocityY": -1.5732713402393688,
-            "timestamp": 2.111176606841022
-        },
-        {
-            "x": 7.586612019746033,
-            "y": 2.5735926396425732,
-            "heading": 0.030969705386613037,
-            "angularVelocity": -0.3719824208954968,
-            "velocityX": 1.0800009073009074,
-            "velocityY": -1.348370048772548,
-            "timestamp": 2.1457714634009926
-        },
-        {
-            "x": 7.6177445449139505,
-            "y": 2.5347242182058114,
-            "heading": 0.01995171749042388,
-            "angularVelocity": -0.31848630090688007,
-            "velocityX": 0.8999177410650449,
-            "velocityY": -1.1235317992830327,
-            "timestamp": 2.1803663199609633
-        },
-        {
-            "x": 7.642648503297853,
-            "y": 2.503632200129417,
-            "heading": 0.010935986949086615,
-            "angularVelocity": -0.26060898751548167,
-            "velocityX": 0.7198745958298824,
-            "velocityY": -0.8987468418172337,
-            "timestamp": 2.214961176520934
-        },
-        {
-            "x": 7.661325077102299,
-            "y": 2.4803150199755253,
-            "heading": 0.004042922192056018,
-            "angularVelocity": -0.19925114431624774,
-            "velocityX": 0.5398656234365572,
-            "velocityY": -0.6740071349471043,
-            "timestamp": 2.249556033080905
-        },
-        {
-            "x": 7.673775282436928,
-            "y": 2.464771338402636,
-            "heading": -0.0006295042844945549,
-            "angularVelocity": -0.13506130509461317,
-            "velocityX": 0.3598860227400032,
-            "velocityY": -0.44930614312403183,
-            "timestamp": 2.2841508896408755
+            "heading": -0.46507695218435896,
+            "angularVelocity": 1.207222809362328,
+            "velocityX": 3.3000329626441363,
+            "velocityY": -0.005255813788261737,
+            "timestamp": 1.464245985189056
+        },
+        {
+            "x": 5.957415351644791,
+            "y": 3.94389686592464,
+            "heading": -0.40627705964587574,
+            "angularVelocity": 0.8004851872173009,
+            "velocityX": 3.450165509502976,
+            "velocityY": -0.5879362884366123,
+            "timestamp": 1.5377013013865826
+        },
+        {
+            "x": 6.214659603999478,
+            "y": 3.857155103461619,
+            "heading": -0.3855287596647175,
+            "angularVelocity": 0.2824615161326723,
+            "velocityX": 3.502050847660077,
+            "velocityY": -1.180877939858924,
+            "timestamp": 1.6111566175841092
+        },
+        {
+            "x": 6.460339332530576,
+            "y": 3.727320070546642,
+            "heading": -0.38552868222223635,
+            "angularVelocity": 0.0000010542801416145007,
+            "velocityX": 3.3446146752734305,
+            "velocityY": -1.7675375947717866,
+            "timestamp": 1.6846119337816359
+        },
+        {
+            "x": 6.681847040612612,
+            "y": 3.5595404285567858,
+            "heading": -0.3855286464284339,
+            "angularVelocity": 4.872867523834452e-7,
+            "velocityX": 3.0155435923301614,
+            "velocityY": -2.2841048228379357,
+            "timestamp": 1.7580672499791625
+        },
+        {
+            "x": 6.873376982747171,
+            "y": 3.358214932798401,
+            "heading": -0.3855286139444744,
+            "angularVelocity": 4.422274806376815e-7,
+            "velocityX": 2.607434724254966,
+            "velocityY": -2.740788634235891,
+            "timestamp": 1.8315225661766892
+        },
+        {
+            "x": 7.058369006072979,
+            "y": 3.1508658904491242,
+            "heading": -0.3855285817822777,
+            "angularVelocity": 4.3784709334515964e-7,
+            "velocityX": 2.518429337753462,
+            "velocityY": -2.8227915021385286,
+            "timestamp": 1.9049778823742158
+        },
+        {
+            "x": 7.235640342207717,
+            "y": 2.9543486280986375,
+            "heading": -0.3517392740895451,
+            "angularVelocity": 0.4599981245995971,
+            "velocityX": 2.413322075396734,
+            "velocityY": -2.6753306979448217,
+            "timestamp": 1.9784331985717425
+        },
+        {
+            "x": 7.3842205894533075,
+            "y": 2.7881464841241645,
+            "heading": -0.2579536451103976,
+            "angularVelocity": 1.2767711560447332,
+            "velocityX": 2.0227296666458727,
+            "velocityY": -2.2626292088586646,
+            "timestamp": 2.051888514769269
+        },
+        {
+            "x": 7.502781206912582,
+            "y": 2.6554541566341396,
+            "heading": -0.1644602811973769,
+            "angularVelocity": 1.2727923416953275,
+            "velocityX": 1.6140508760517314,
+            "velocityY": -1.806435998903144,
+            "timestamp": 2.125343830966796
+        },
+        {
+            "x": 7.591494122264987,
+            "y": 2.5561305844890163,
+            "heading": -0.0867965293639426,
+            "angularVelocity": 1.0572924582421084,
+            "velocityX": 1.2077126604948574,
+            "velocityY": -1.3521631555983635,
+            "timestamp": 2.1987991471643227
+        },
+        {
+            "x": 7.6505265885867635,
+            "y": 2.4900164938031613,
+            "heading": -0.031723308715129774,
+            "angularVelocity": 0.7497513250193758,
+            "velocityX": 0.8036513812429008,
+            "velocityY": -0.900058622143418,
+            "timestamp": 2.2722544633618496
         },
         {
             "x": 7.68,
             "y": 2.457,
             "heading": -0.003,
-            "angularVelocity": -0.06852162290068065,
-            "velocityX": 0.17993187953478723,
-            "velocityY": -0.22463854964001964,
-            "timestamp": 2.3187457462008463
+            "angularVelocity": 0.39103104039319253,
+            "velocityX": 0.401242727401519,
+            "velocityY": -0.4494772538229548,
+            "timestamp": 2.3457097795593764
         },
         {
             "x": 7.68,
             "y": 2.457,
             "heading": -0.003,
-            "angularVelocity": -8.413636330536319e-34,
-            "velocityX": 0,
-            "velocityY": 4.110332962762654e-35,
-            "timestamp": 2.353340602760817
+            "angularVelocity": -1.5819357203294863e-32,
+            "velocityX": 5.378736393413154e-34,
+            "velocityY": 0,
+            "timestamp": 2.4191650957569033
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.1.traj
new file mode 100644
index 00000000..95620590
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.1.traj	
@@ -0,0 +1,616 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 4.884595084783414e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 2.298507420370097,
+            "y": 6.738728491162364,
+            "heading": 0.38429180377773414,
+            "angularVelocity": -0.12150700609071861,
+            "velocityX": -0.031771662206367984,
+            "velocityY": -0.3889348314553929,
+            "timestamp": 0.04697832994094238
+        },
+        {
+            "x": 2.295523042319471,
+            "y": 6.702182630298916,
+            "heading": 0.3729896107436911,
+            "angularVelocity": -0.2405831166891495,
+            "velocityX": -0.06352669527371438,
+            "velocityY": -0.7779301841804652,
+            "timestamp": 0.09395665988188476
+        },
+        {
+            "x": 2.2910478858134478,
+            "y": 6.647358841243747,
+            "heading": 0.35624034482133005,
+            "angularVelocity": -0.3565317443897418,
+            "velocityX": -0.09526001694076108,
+            "velocityY": -1.1670016606398963,
+            "timestamp": 0.14093498982282715
+        },
+        {
+            "x": 2.285083332518943,
+            "y": 6.574252474793301,
+            "heading": 0.33424119989038353,
+            "angularVelocity": -0.46828282228427764,
+            "velocityX": -0.12696392787914923,
+            "velocityY": -1.556172101953168,
+            "timestamp": 0.18791331976376952
+        },
+        {
+            "x": 2.2776313636635503,
+            "y": 6.48285724279221,
+            "heading": 0.3072714314404645,
+            "angularVelocity": -0.5740895532860236,
+            "velocityX": -0.15862566559434335,
+            "velocityY": -1.9454763955207768,
+            "timestamp": 0.23489164970471188
+        },
+        {
+            "x": 2.268695072443392,
+            "y": 6.373164221156064,
+            "heading": 0.2757564747192681,
+            "angularVelocity": -0.6708402951065854,
+            "velocityX": -0.1902215602681591,
+            "velocityY": -2.334970650809517,
+            "timestamp": 0.28186997964565425
+        },
+        {
+            "x": 2.2582799597491072,
+            "y": 6.245160021300657,
+            "heading": 0.24041955506127732,
+            "angularVelocity": -0.752196165815466,
+            "velocityX": -0.22170036072754723,
+            "velocityY": -2.7247499009931837,
+            "timestamp": 0.3288483095865966
+        },
+        {
+            "x": 2.2463982479830333,
+            "y": 6.098824071496616,
+            "heading": 0.20274918029785605,
+            "angularVelocity": -0.8018670482917891,
+            "velocityX": -0.25291899011757896,
+            "velocityY": -3.114967049446051,
+            "timestamp": 0.375826639527539
+        },
+        {
+            "x": 2.2330953714575887,
+            "y": 5.934154199517091,
+            "heading": 0.1675471140087416,
+            "angularVelocity": -0.7493256216934009,
+            "velocityX": -0.28317048609790474,
+            "velocityY": -3.505230436810659,
+            "timestamp": 0.42280496946848134
+        },
+        {
+            "x": 2.218530566609672,
+            "y": 5.757035827571947,
+            "heading": 0.16754710135001605,
+            "angularVelocity": -2.6945882371217095e-7,
+            "velocityX": -0.3100324099691601,
+            "velocityY": -3.770214313020578,
+            "timestamp": 0.4697832994094237
+        },
+        {
+            "x": 2.203965673260931,
+            "y": 5.579917462904045,
+            "heading": 0.16754708869211127,
+            "angularVelocity": -2.694413527531555e-7,
+            "velocityX": -0.3100342938339967,
+            "velocityY": -3.7702141581142206,
+            "timestamp": 0.5167616293503661
+        },
+        {
+            "x": 2.1894007799094983,
+            "y": 5.402799098236364,
+            "heading": 0.16754707603420654,
+            "angularVelocity": -2.694413521606316e-7,
+            "velocityX": -0.31003429389130516,
+            "velocityY": -3.7702141581095083,
+            "timestamp": 0.5637399592913085
+        },
+        {
+            "x": 2.1748358865580655,
+            "y": 5.225680733568683,
+            "heading": 0.1675470633763018,
+            "angularVelocity": -2.6944135250249435e-7,
+            "velocityX": -0.3100342938913069,
+            "velocityY": -3.770214158109508,
+            "timestamp": 0.6107182892322509
+        },
+        {
+            "x": 2.160270993206632,
+            "y": 5.048562368901002,
+            "heading": 0.16754705071839698,
+            "angularVelocity": -2.694413536359467e-7,
+            "velocityX": -0.31003429389130693,
+            "velocityY": -3.7702141581095083,
+            "timestamp": 0.6576966191731933
+        },
+        {
+            "x": 2.1457060998551993,
+            "y": 4.871444004233322,
+            "heading": 0.1675470380604922,
+            "angularVelocity": -2.6944135247689087e-7,
+            "velocityX": -0.3100342938913068,
+            "velocityY": -3.7702141581095083,
+            "timestamp": 0.7046749491141358
+        },
+        {
+            "x": 2.131141206503766,
+            "y": 4.6943256395656405,
+            "heading": 0.16754702540258745,
+            "angularVelocity": -2.694413528693067e-7,
+            "velocityX": -0.31003429389130693,
+            "velocityY": -3.7702141581095083,
+            "timestamp": 0.7516532790550782
+        },
+        {
+            "x": 2.1165763131523327,
+            "y": 4.51720727489796,
+            "heading": 0.16754701274468267,
+            "angularVelocity": -2.694413529636153e-7,
+            "velocityX": -0.31003429389130693,
+            "velocityY": -3.7702141581095083,
+            "timestamp": 0.7986316089960206
+        },
+        {
+            "x": 2.1020114198009,
+            "y": 4.340088910230279,
+            "heading": 0.16754700008677786,
+            "angularVelocity": -2.694413530501801e-7,
+            "velocityX": -0.3100342938913069,
+            "velocityY": -3.770214158109508,
+            "timestamp": 0.845609938936963
+        },
+        {
+            "x": 2.0874465264494666,
+            "y": 4.162970545562599,
+            "heading": 0.16754698742887306,
+            "angularVelocity": -2.694413534833812e-7,
+            "velocityX": -0.31003429389130704,
+            "velocityY": -3.7702141581095083,
+            "timestamp": 0.8925882688779054
+        },
+        {
+            "x": 2.0728816330980333,
+            "y": 3.9858521808949186,
+            "heading": 0.16754697477096828,
+            "angularVelocity": -2.6944135303696276e-7,
+            "velocityX": -0.31003429389130693,
+            "velocityY": -3.770214158109508,
+            "timestamp": 0.9395665988188479
+        },
+        {
+            "x": 2.0583167397466005,
+            "y": 3.8087338162272375,
+            "heading": 0.16754696211306347,
+            "angularVelocity": -2.694413537071192e-7,
+            "velocityX": -0.31003429389130704,
+            "velocityY": -3.7702141581095083,
+            "timestamp": 0.9865449287597903
+        },
+        {
+            "x": 2.043751846395167,
+            "y": 3.631615451559557,
+            "heading": 0.1675469494551587,
+            "angularVelocity": -2.694413527332354e-7,
+            "velocityX": -0.31003429389130716,
+            "velocityY": -3.770214158109508,
+            "timestamp": 1.0335232587007326
+        },
+        {
+            "x": 2.0291869530437365,
+            "y": 3.4544970868918763,
+            "heading": 0.16754693679725388,
+            "angularVelocity": -2.694413532802898e-7,
+            "velocityX": -0.31003429389125564,
+            "velocityY": -3.7702141581095123,
+            "timestamp": 1.080501588641675
+        },
+        {
+            "x": 2.0146220597715567,
+            "y": 3.2773787222176782,
+            "heading": 0.1675469241393491,
+            "angularVelocity": -2.6944135307750017e-7,
+            "velocityX": -0.31003429220428774,
+            "velocityY": -3.7702141582482356,
+            "timestamp": 1.1274799185826172
+        },
+        {
+            "x": 2.0000597716588953,
+            "y": 3.1002601433334096,
+            "heading": 0.16754691148142445,
+            "angularVelocity": -2.6944177624221033e-7,
+            "velocityX": -0.30997883771037654,
+            "velocityY": -3.770218718011654,
+            "timestamp": 1.1744582485235595
+        },
+        {
+            "x": 2.001608158601577,
+            "y": 2.922550679334096,
+            "heading": 0.16754689857982888,
+            "angularVelocity": -2.7462865480192305e-7,
+            "velocityX": 0.032959599556405594,
+            "velocityY": -3.7827965409310345,
+            "timestamp": 1.2214365784645018
+        },
+        {
+            "x": 2.021524410839774,
+            "y": 2.7459539818451146,
+            "heading": 0.1675468848578902,
+            "angularVelocity": -2.9209081516381327e-7,
+            "velocityX": 0.4239455140962882,
+            "velocityY": -3.7591097365739743,
+            "timestamp": 1.2684149084054441
+        },
+        {
+            "x": 2.0595952756969864,
+            "y": 2.572363489269638,
+            "heading": 0.16754686955661854,
+            "angularVelocity": -3.257091448717022e-7,
+            "velocityX": 0.8103920446953395,
+            "velocityY": -3.6951184257444134,
+            "timestamp": 1.3153932383463864
+        },
+        {
+            "x": 2.1154126290237643,
+            "y": 2.4036404273247642,
+            "heading": 0.16754685154625998,
+            "angularVelocity": -3.8337587986161977e-7,
+            "velocityX": 1.1881510772508834,
+            "velocityY": -3.5915083008906312,
+            "timestamp": 1.3623715682873287
+        },
+        {
+            "x": 2.1883780268761943,
+            "y": 2.2415938375833533,
+            "heading": 0.1675468289010436,
+            "angularVelocity": -4.82035363826859e-7,
+            "velocityX": 1.5531713865554018,
+            "velocityY": -3.449390175111029,
+            "timestamp": 1.409349898228271
+        },
+        {
+            "x": 2.2777091497247044,
+            "y": 2.0879611790897656,
+            "heading": 0.16754679784052873,
+            "angularVelocity": -6.611668595012802e-7,
+            "velocityX": 1.9015389214731706,
+            "velocityY": -3.270287783467881,
+            "timestamp": 1.4563282281692134
+        },
+        {
+            "x": 2.382448196411133,
+            "y": 1.9443897008895872,
+            "heading": 0.16754674836915184,
+            "angularVelocity": -0.0000010530680204285256,
+            "velocityX": 2.2295183080815923,
+            "velocityY": -3.0561213729961265,
+            "timestamp": 1.5033065581101557
+        },
+        {
+            "x": 2.5069799221636964,
+            "y": 1.8074962230648766,
+            "heading": 0.1675467031876573,
+            "angularVelocity": -9.235757728412815e-7,
+            "velocityX": 2.545610454081449,
+            "velocityY": -2.798302730812182,
+            "timestamp": 1.55222673973217
+        },
+        {
+            "x": 2.645527049823341,
+            "y": 1.6848068830394283,
+            "heading": 0.16754667440242366,
+            "angularVelocity": -5.884122397185895e-7,
+            "velocityX": 2.8321057499364835,
+            "velocityY": -2.5079493975189484,
+            "timestamp": 1.6011469213541845
+        },
+        {
+            "x": 2.7964787325047507,
+            "y": 1.577748144471377,
+            "heading": 0.1675466534117406,
+            "angularVelocity": -4.2908023560369677e-7,
+            "velocityX": 3.0856729815058617,
+            "velocityY": -2.1884370625450487,
+            "timestamp": 1.6500671029761989
+        },
+        {
+            "x": 2.958079900807193,
+            "y": 1.4875647290195357,
+            "heading": 0.1675466366326211,
+            "angularVelocity": -3.429897224036844e-7,
+            "velocityX": 3.303364029820377,
+            "velocityY": -1.8434807979383812,
+            "timestamp": 1.6989872845982132
+        },
+        {
+            "x": 3.1284516660647324,
+            "y": 1.4153051353233332,
+            "heading": 0.16754662225394246,
+            "angularVelocity": -2.9392120351406106e-7,
+            "velocityX": 3.4826478481607013,
+            "velocityY": -1.4770916889581902,
+            "timestamp": 1.7479074662202276
+        },
+        {
+            "x": 3.3056131601804433,
+            "y": 1.3618094062641966,
+            "heading": 0.16754660920712522,
+            "angularVelocity": -2.666960100615425e-7,
+            "velocityX": 3.6214398279336546,
+            "velocityY": -1.0935308759169382,
+            "timestamp": 1.796827647842242
+        },
+        {
+            "x": 3.487504529935422,
+            "y": 1.3276990268102373,
+            "heading": 0.16754659675699374,
+            "angularVelocity": -2.544988811687778e-7,
+            "velocityX": 3.7181253978240782,
+            "velocityY": -0.6972660019440657,
+            "timestamp": 1.8457478294642564
+        },
+        {
+            "x": 3.671043389503124,
+            "y": 1.304003553111977,
+            "heading": 0.16754658441216208,
+            "angularVelocity": -2.5234639867690264e-7,
+            "velocityX": 3.751802497092686,
+            "velocityY": -0.48437010887133,
+            "timestamp": 1.8946680110862708
+        },
+        {
+            "x": 3.8545823032662914,
+            "y": 1.2803084992011935,
+            "heading": 0.16754657206733162,
+            "angularVelocity": -2.523463739841797e-7,
+            "velocityX": 3.751803604927206,
+            "velocityY": -0.4843615278018566,
+            "timestamp": 1.9435881927082852
+        },
+        {
+            "x": 4.038121217030901,
+            "y": 1.2566134453015836,
+            "heading": 0.16754655972250113,
+            "angularVelocity": -2.5234637491728997e-7,
+            "velocityX": 3.7518036049566934,
+            "velocityY": -0.48436152757345374,
+            "timestamp": 1.9925083743302996
+        },
+        {
+            "x": 4.221660130795511,
+            "y": 1.232918391401974,
+            "heading": 0.16754654737767066,
+            "angularVelocity": -2.5234637402611237e-7,
+            "velocityX": 3.7518036049566943,
+            "velocityY": -0.4843615275734478,
+            "timestamp": 2.041428555952314
+        },
+        {
+            "x": 4.4051990445601215,
+            "y": 1.2092233375023644,
+            "heading": 0.16754653503284023,
+            "angularVelocity": -2.5234637391390173e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.4843615275734475,
+            "timestamp": 2.090348737574329
+        },
+        {
+            "x": 4.588737958324731,
+            "y": 1.1855282836027548,
+            "heading": 0.16754652268800974,
+            "angularVelocity": -2.523463747404588e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.4843615275734478,
+            "timestamp": 2.1392689191963434
+        },
+        {
+            "x": 4.77227687208934,
+            "y": 1.1618332297031453,
+            "heading": 0.16754651034317924,
+            "angularVelocity": -2.523463747849071e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.48436152757344797,
+            "timestamp": 2.188189100818358
+        },
+        {
+            "x": 4.95581578585395,
+            "y": 1.1381381758035356,
+            "heading": 0.16754649799834875,
+            "angularVelocity": -2.52346374599026e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.4843615275734478,
+            "timestamp": 2.2371092824403727
+        },
+        {
+            "x": 5.1393546996185595,
+            "y": 1.114443121903926,
+            "heading": 0.1675464856535183,
+            "angularVelocity": -2.523463743979587e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.48436152757344764,
+            "timestamp": 2.2860294640623873
+        },
+        {
+            "x": 5.32289361338317,
+            "y": 1.0907480680043165,
+            "heading": 0.16754647330868783,
+            "angularVelocity": -2.523463747332428e-7,
+            "velocityX": 3.7518036049566943,
+            "velocityY": -0.48436152757344797,
+            "timestamp": 2.334949645684402
+        },
+        {
+            "x": 5.506432527147779,
+            "y": 1.0670530141047068,
+            "heading": 0.1675464609638573,
+            "angularVelocity": -2.5234637523589347e-7,
+            "velocityX": 3.7518036049566943,
+            "velocityY": -0.48436152757344764,
+            "timestamp": 2.3838698273064165
+        },
+        {
+            "x": 5.689971440912389,
+            "y": 1.043357960205097,
+            "heading": 0.16754644861902682,
+            "angularVelocity": -2.523463745889681e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.4843615275734477,
+            "timestamp": 2.432790008928431
+        },
+        {
+            "x": 5.8735103546769984,
+            "y": 1.0196629063054876,
+            "heading": 0.16754643627419633,
+            "angularVelocity": -2.5234637515727394e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.4843615275734478,
+            "timestamp": 2.4817101905504457
+        },
+        {
+            "x": 6.057049268441609,
+            "y": 0.995967852405878,
+            "heading": 0.16754642392936583,
+            "angularVelocity": -2.523463750187176e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.4843615275734476,
+            "timestamp": 2.5306303721724603
+        },
+        {
+            "x": 6.240588182206218,
+            "y": 0.9722727985062686,
+            "heading": 0.16754641158453532,
+            "angularVelocity": -2.523463753175626e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.48436152757344764,
+            "timestamp": 2.579550553794475
+        },
+        {
+            "x": 6.424127095970827,
+            "y": 0.948577744606659,
+            "heading": 0.1675463992397048,
+            "angularVelocity": -2.5234637555305e-7,
+            "velocityX": 3.7518036049566934,
+            "velocityY": -0.4843615275734476,
+            "timestamp": 2.6284707354164896
+        },
+        {
+            "x": 6.607666009735459,
+            "y": 0.9248826907072107,
+            "heading": 0.1675463868948743,
+            "angularVelocity": -2.5234637524939476e-7,
+            "velocityX": 3.7518036049571197,
+            "velocityY": -0.48436152757015266,
+            "timestamp": 2.677390917038504
+        },
+        {
+            "x": 6.79120492428193,
+            "y": 0.9011876428641385,
+            "heading": 0.16754637454996243,
+            "angularVelocity": -2.523480385625403e-7,
+            "velocityX": 3.7518036209390857,
+            "velocityY": -0.4843614037689742,
+            "timestamp": 2.726311098660519
+        },
+        {
+            "x": 6.9688913787459,
+            "y": 0.8784350451744124,
+            "heading": 0.1510406339197301,
+            "angularVelocity": -0.33740145851798203,
+            "velocityX": 3.6321707845828914,
+            "velocityY": -0.4650963454209109,
+            "timestamp": 2.7752312802825334
+        },
+        {
+            "x": 7.1269915101358645,
+            "y": 0.8581344466937117,
+            "heading": 0.12402265057850995,
+            "angularVelocity": -0.5522870611964742,
+            "velocityX": 3.2317977192222487,
+            "velocityY": -0.4149738984526803,
+            "timestamp": 2.824151461904548
+        },
+        {
+            "x": 7.265299167873697,
+            "y": 0.8403581253912503,
+            "heading": 0.0960275261585182,
+            "angularVelocity": -0.5722612527545016,
+            "velocityX": 2.8272106347943935,
+            "velocityY": -0.3633739841730647,
+            "timestamp": 2.8730716435265626
+        },
+        {
+            "x": 7.383819928892336,
+            "y": 0.8251162922066029,
+            "heading": 0.06979646830524168,
+            "angularVelocity": -0.5362011542792869,
+            "velocityX": 2.4227375510254454,
+            "velocityY": -0.3115653433671744,
+            "timestamp": 2.9219918251485772
+        },
+        {
+            "x": 7.482566860925546,
+            "y": 0.8124124441531555,
+            "heading": 0.04664460944238867,
+            "angularVelocity": -0.47325782724474763,
+            "velocityX": 2.01853159083065,
+            "velocityY": -0.2596852184974389,
+            "timestamp": 2.970912006770592
+        },
+        {
+            "x": 7.5615507925764724,
+            "y": 0.8022481812485672,
+            "heading": 0.027346506217350703,
+            "angularVelocity": -0.3944814304686405,
+            "velocityX": 1.6145469831081642,
+            "velocityY": -0.20777238692863387,
+            "timestamp": 3.0198321883926065
+        },
+        {
+            "x": 7.620780133649161,
+            "y": 0.7946243691967044,
+            "heading": 0.012413272398069258,
+            "angularVelocity": -0.3052571213791523,
+            "velocityX": 1.2107342840696753,
+            "velocityY": -0.15584185910773468,
+            "timestamp": 3.068752370014621
+        },
+        {
+            "x": 7.660261458521305,
+            "y": 0.7895415301241143,
+            "heading": 0.002207571288741933,
+            "angularVelocity": -0.2086194443876447,
+            "velocityX": 0.8070559749184711,
+            "velocityY": -0.10390065825721975,
+            "timestamp": 3.1176725516366357
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -0.003,
+            "angularVelocity": -0.10645036702804238,
+            "velocityX": 0.40348463199107626,
+            "velocityY": -0.05195258970523994,
+            "timestamp": 3.1665927332586503
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -0.003,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -1.0902826040652685e-32,
+            "timestamp": 3.215512914880665
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.traj
new file mode 100644
index 00000000..95620590
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.traj	
@@ -0,0 +1,616 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 4.884595084783414e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 2.298507420370097,
+            "y": 6.738728491162364,
+            "heading": 0.38429180377773414,
+            "angularVelocity": -0.12150700609071861,
+            "velocityX": -0.031771662206367984,
+            "velocityY": -0.3889348314553929,
+            "timestamp": 0.04697832994094238
+        },
+        {
+            "x": 2.295523042319471,
+            "y": 6.702182630298916,
+            "heading": 0.3729896107436911,
+            "angularVelocity": -0.2405831166891495,
+            "velocityX": -0.06352669527371438,
+            "velocityY": -0.7779301841804652,
+            "timestamp": 0.09395665988188476
+        },
+        {
+            "x": 2.2910478858134478,
+            "y": 6.647358841243747,
+            "heading": 0.35624034482133005,
+            "angularVelocity": -0.3565317443897418,
+            "velocityX": -0.09526001694076108,
+            "velocityY": -1.1670016606398963,
+            "timestamp": 0.14093498982282715
+        },
+        {
+            "x": 2.285083332518943,
+            "y": 6.574252474793301,
+            "heading": 0.33424119989038353,
+            "angularVelocity": -0.46828282228427764,
+            "velocityX": -0.12696392787914923,
+            "velocityY": -1.556172101953168,
+            "timestamp": 0.18791331976376952
+        },
+        {
+            "x": 2.2776313636635503,
+            "y": 6.48285724279221,
+            "heading": 0.3072714314404645,
+            "angularVelocity": -0.5740895532860236,
+            "velocityX": -0.15862566559434335,
+            "velocityY": -1.9454763955207768,
+            "timestamp": 0.23489164970471188
+        },
+        {
+            "x": 2.268695072443392,
+            "y": 6.373164221156064,
+            "heading": 0.2757564747192681,
+            "angularVelocity": -0.6708402951065854,
+            "velocityX": -0.1902215602681591,
+            "velocityY": -2.334970650809517,
+            "timestamp": 0.28186997964565425
+        },
+        {
+            "x": 2.2582799597491072,
+            "y": 6.245160021300657,
+            "heading": 0.24041955506127732,
+            "angularVelocity": -0.752196165815466,
+            "velocityX": -0.22170036072754723,
+            "velocityY": -2.7247499009931837,
+            "timestamp": 0.3288483095865966
+        },
+        {
+            "x": 2.2463982479830333,
+            "y": 6.098824071496616,
+            "heading": 0.20274918029785605,
+            "angularVelocity": -0.8018670482917891,
+            "velocityX": -0.25291899011757896,
+            "velocityY": -3.114967049446051,
+            "timestamp": 0.375826639527539
+        },
+        {
+            "x": 2.2330953714575887,
+            "y": 5.934154199517091,
+            "heading": 0.1675471140087416,
+            "angularVelocity": -0.7493256216934009,
+            "velocityX": -0.28317048609790474,
+            "velocityY": -3.505230436810659,
+            "timestamp": 0.42280496946848134
+        },
+        {
+            "x": 2.218530566609672,
+            "y": 5.757035827571947,
+            "heading": 0.16754710135001605,
+            "angularVelocity": -2.6945882371217095e-7,
+            "velocityX": -0.3100324099691601,
+            "velocityY": -3.770214313020578,
+            "timestamp": 0.4697832994094237
+        },
+        {
+            "x": 2.203965673260931,
+            "y": 5.579917462904045,
+            "heading": 0.16754708869211127,
+            "angularVelocity": -2.694413527531555e-7,
+            "velocityX": -0.3100342938339967,
+            "velocityY": -3.7702141581142206,
+            "timestamp": 0.5167616293503661
+        },
+        {
+            "x": 2.1894007799094983,
+            "y": 5.402799098236364,
+            "heading": 0.16754707603420654,
+            "angularVelocity": -2.694413521606316e-7,
+            "velocityX": -0.31003429389130516,
+            "velocityY": -3.7702141581095083,
+            "timestamp": 0.5637399592913085
+        },
+        {
+            "x": 2.1748358865580655,
+            "y": 5.225680733568683,
+            "heading": 0.1675470633763018,
+            "angularVelocity": -2.6944135250249435e-7,
+            "velocityX": -0.3100342938913069,
+            "velocityY": -3.770214158109508,
+            "timestamp": 0.6107182892322509
+        },
+        {
+            "x": 2.160270993206632,
+            "y": 5.048562368901002,
+            "heading": 0.16754705071839698,
+            "angularVelocity": -2.694413536359467e-7,
+            "velocityX": -0.31003429389130693,
+            "velocityY": -3.7702141581095083,
+            "timestamp": 0.6576966191731933
+        },
+        {
+            "x": 2.1457060998551993,
+            "y": 4.871444004233322,
+            "heading": 0.1675470380604922,
+            "angularVelocity": -2.6944135247689087e-7,
+            "velocityX": -0.3100342938913068,
+            "velocityY": -3.7702141581095083,
+            "timestamp": 0.7046749491141358
+        },
+        {
+            "x": 2.131141206503766,
+            "y": 4.6943256395656405,
+            "heading": 0.16754702540258745,
+            "angularVelocity": -2.694413528693067e-7,
+            "velocityX": -0.31003429389130693,
+            "velocityY": -3.7702141581095083,
+            "timestamp": 0.7516532790550782
+        },
+        {
+            "x": 2.1165763131523327,
+            "y": 4.51720727489796,
+            "heading": 0.16754701274468267,
+            "angularVelocity": -2.694413529636153e-7,
+            "velocityX": -0.31003429389130693,
+            "velocityY": -3.7702141581095083,
+            "timestamp": 0.7986316089960206
+        },
+        {
+            "x": 2.1020114198009,
+            "y": 4.340088910230279,
+            "heading": 0.16754700008677786,
+            "angularVelocity": -2.694413530501801e-7,
+            "velocityX": -0.3100342938913069,
+            "velocityY": -3.770214158109508,
+            "timestamp": 0.845609938936963
+        },
+        {
+            "x": 2.0874465264494666,
+            "y": 4.162970545562599,
+            "heading": 0.16754698742887306,
+            "angularVelocity": -2.694413534833812e-7,
+            "velocityX": -0.31003429389130704,
+            "velocityY": -3.7702141581095083,
+            "timestamp": 0.8925882688779054
+        },
+        {
+            "x": 2.0728816330980333,
+            "y": 3.9858521808949186,
+            "heading": 0.16754697477096828,
+            "angularVelocity": -2.6944135303696276e-7,
+            "velocityX": -0.31003429389130693,
+            "velocityY": -3.770214158109508,
+            "timestamp": 0.9395665988188479
+        },
+        {
+            "x": 2.0583167397466005,
+            "y": 3.8087338162272375,
+            "heading": 0.16754696211306347,
+            "angularVelocity": -2.694413537071192e-7,
+            "velocityX": -0.31003429389130704,
+            "velocityY": -3.7702141581095083,
+            "timestamp": 0.9865449287597903
+        },
+        {
+            "x": 2.043751846395167,
+            "y": 3.631615451559557,
+            "heading": 0.1675469494551587,
+            "angularVelocity": -2.694413527332354e-7,
+            "velocityX": -0.31003429389130716,
+            "velocityY": -3.770214158109508,
+            "timestamp": 1.0335232587007326
+        },
+        {
+            "x": 2.0291869530437365,
+            "y": 3.4544970868918763,
+            "heading": 0.16754693679725388,
+            "angularVelocity": -2.694413532802898e-7,
+            "velocityX": -0.31003429389125564,
+            "velocityY": -3.7702141581095123,
+            "timestamp": 1.080501588641675
+        },
+        {
+            "x": 2.0146220597715567,
+            "y": 3.2773787222176782,
+            "heading": 0.1675469241393491,
+            "angularVelocity": -2.6944135307750017e-7,
+            "velocityX": -0.31003429220428774,
+            "velocityY": -3.7702141582482356,
+            "timestamp": 1.1274799185826172
+        },
+        {
+            "x": 2.0000597716588953,
+            "y": 3.1002601433334096,
+            "heading": 0.16754691148142445,
+            "angularVelocity": -2.6944177624221033e-7,
+            "velocityX": -0.30997883771037654,
+            "velocityY": -3.770218718011654,
+            "timestamp": 1.1744582485235595
+        },
+        {
+            "x": 2.001608158601577,
+            "y": 2.922550679334096,
+            "heading": 0.16754689857982888,
+            "angularVelocity": -2.7462865480192305e-7,
+            "velocityX": 0.032959599556405594,
+            "velocityY": -3.7827965409310345,
+            "timestamp": 1.2214365784645018
+        },
+        {
+            "x": 2.021524410839774,
+            "y": 2.7459539818451146,
+            "heading": 0.1675468848578902,
+            "angularVelocity": -2.9209081516381327e-7,
+            "velocityX": 0.4239455140962882,
+            "velocityY": -3.7591097365739743,
+            "timestamp": 1.2684149084054441
+        },
+        {
+            "x": 2.0595952756969864,
+            "y": 2.572363489269638,
+            "heading": 0.16754686955661854,
+            "angularVelocity": -3.257091448717022e-7,
+            "velocityX": 0.8103920446953395,
+            "velocityY": -3.6951184257444134,
+            "timestamp": 1.3153932383463864
+        },
+        {
+            "x": 2.1154126290237643,
+            "y": 2.4036404273247642,
+            "heading": 0.16754685154625998,
+            "angularVelocity": -3.8337587986161977e-7,
+            "velocityX": 1.1881510772508834,
+            "velocityY": -3.5915083008906312,
+            "timestamp": 1.3623715682873287
+        },
+        {
+            "x": 2.1883780268761943,
+            "y": 2.2415938375833533,
+            "heading": 0.1675468289010436,
+            "angularVelocity": -4.82035363826859e-7,
+            "velocityX": 1.5531713865554018,
+            "velocityY": -3.449390175111029,
+            "timestamp": 1.409349898228271
+        },
+        {
+            "x": 2.2777091497247044,
+            "y": 2.0879611790897656,
+            "heading": 0.16754679784052873,
+            "angularVelocity": -6.611668595012802e-7,
+            "velocityX": 1.9015389214731706,
+            "velocityY": -3.270287783467881,
+            "timestamp": 1.4563282281692134
+        },
+        {
+            "x": 2.382448196411133,
+            "y": 1.9443897008895872,
+            "heading": 0.16754674836915184,
+            "angularVelocity": -0.0000010530680204285256,
+            "velocityX": 2.2295183080815923,
+            "velocityY": -3.0561213729961265,
+            "timestamp": 1.5033065581101557
+        },
+        {
+            "x": 2.5069799221636964,
+            "y": 1.8074962230648766,
+            "heading": 0.1675467031876573,
+            "angularVelocity": -9.235757728412815e-7,
+            "velocityX": 2.545610454081449,
+            "velocityY": -2.798302730812182,
+            "timestamp": 1.55222673973217
+        },
+        {
+            "x": 2.645527049823341,
+            "y": 1.6848068830394283,
+            "heading": 0.16754667440242366,
+            "angularVelocity": -5.884122397185895e-7,
+            "velocityX": 2.8321057499364835,
+            "velocityY": -2.5079493975189484,
+            "timestamp": 1.6011469213541845
+        },
+        {
+            "x": 2.7964787325047507,
+            "y": 1.577748144471377,
+            "heading": 0.1675466534117406,
+            "angularVelocity": -4.2908023560369677e-7,
+            "velocityX": 3.0856729815058617,
+            "velocityY": -2.1884370625450487,
+            "timestamp": 1.6500671029761989
+        },
+        {
+            "x": 2.958079900807193,
+            "y": 1.4875647290195357,
+            "heading": 0.1675466366326211,
+            "angularVelocity": -3.429897224036844e-7,
+            "velocityX": 3.303364029820377,
+            "velocityY": -1.8434807979383812,
+            "timestamp": 1.6989872845982132
+        },
+        {
+            "x": 3.1284516660647324,
+            "y": 1.4153051353233332,
+            "heading": 0.16754662225394246,
+            "angularVelocity": -2.9392120351406106e-7,
+            "velocityX": 3.4826478481607013,
+            "velocityY": -1.4770916889581902,
+            "timestamp": 1.7479074662202276
+        },
+        {
+            "x": 3.3056131601804433,
+            "y": 1.3618094062641966,
+            "heading": 0.16754660920712522,
+            "angularVelocity": -2.666960100615425e-7,
+            "velocityX": 3.6214398279336546,
+            "velocityY": -1.0935308759169382,
+            "timestamp": 1.796827647842242
+        },
+        {
+            "x": 3.487504529935422,
+            "y": 1.3276990268102373,
+            "heading": 0.16754659675699374,
+            "angularVelocity": -2.544988811687778e-7,
+            "velocityX": 3.7181253978240782,
+            "velocityY": -0.6972660019440657,
+            "timestamp": 1.8457478294642564
+        },
+        {
+            "x": 3.671043389503124,
+            "y": 1.304003553111977,
+            "heading": 0.16754658441216208,
+            "angularVelocity": -2.5234639867690264e-7,
+            "velocityX": 3.751802497092686,
+            "velocityY": -0.48437010887133,
+            "timestamp": 1.8946680110862708
+        },
+        {
+            "x": 3.8545823032662914,
+            "y": 1.2803084992011935,
+            "heading": 0.16754657206733162,
+            "angularVelocity": -2.523463739841797e-7,
+            "velocityX": 3.751803604927206,
+            "velocityY": -0.4843615278018566,
+            "timestamp": 1.9435881927082852
+        },
+        {
+            "x": 4.038121217030901,
+            "y": 1.2566134453015836,
+            "heading": 0.16754655972250113,
+            "angularVelocity": -2.5234637491728997e-7,
+            "velocityX": 3.7518036049566934,
+            "velocityY": -0.48436152757345374,
+            "timestamp": 1.9925083743302996
+        },
+        {
+            "x": 4.221660130795511,
+            "y": 1.232918391401974,
+            "heading": 0.16754654737767066,
+            "angularVelocity": -2.5234637402611237e-7,
+            "velocityX": 3.7518036049566943,
+            "velocityY": -0.4843615275734478,
+            "timestamp": 2.041428555952314
+        },
+        {
+            "x": 4.4051990445601215,
+            "y": 1.2092233375023644,
+            "heading": 0.16754653503284023,
+            "angularVelocity": -2.5234637391390173e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.4843615275734475,
+            "timestamp": 2.090348737574329
+        },
+        {
+            "x": 4.588737958324731,
+            "y": 1.1855282836027548,
+            "heading": 0.16754652268800974,
+            "angularVelocity": -2.523463747404588e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.4843615275734478,
+            "timestamp": 2.1392689191963434
+        },
+        {
+            "x": 4.77227687208934,
+            "y": 1.1618332297031453,
+            "heading": 0.16754651034317924,
+            "angularVelocity": -2.523463747849071e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.48436152757344797,
+            "timestamp": 2.188189100818358
+        },
+        {
+            "x": 4.95581578585395,
+            "y": 1.1381381758035356,
+            "heading": 0.16754649799834875,
+            "angularVelocity": -2.52346374599026e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.4843615275734478,
+            "timestamp": 2.2371092824403727
+        },
+        {
+            "x": 5.1393546996185595,
+            "y": 1.114443121903926,
+            "heading": 0.1675464856535183,
+            "angularVelocity": -2.523463743979587e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.48436152757344764,
+            "timestamp": 2.2860294640623873
+        },
+        {
+            "x": 5.32289361338317,
+            "y": 1.0907480680043165,
+            "heading": 0.16754647330868783,
+            "angularVelocity": -2.523463747332428e-7,
+            "velocityX": 3.7518036049566943,
+            "velocityY": -0.48436152757344797,
+            "timestamp": 2.334949645684402
+        },
+        {
+            "x": 5.506432527147779,
+            "y": 1.0670530141047068,
+            "heading": 0.1675464609638573,
+            "angularVelocity": -2.5234637523589347e-7,
+            "velocityX": 3.7518036049566943,
+            "velocityY": -0.48436152757344764,
+            "timestamp": 2.3838698273064165
+        },
+        {
+            "x": 5.689971440912389,
+            "y": 1.043357960205097,
+            "heading": 0.16754644861902682,
+            "angularVelocity": -2.523463745889681e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.4843615275734477,
+            "timestamp": 2.432790008928431
+        },
+        {
+            "x": 5.8735103546769984,
+            "y": 1.0196629063054876,
+            "heading": 0.16754643627419633,
+            "angularVelocity": -2.5234637515727394e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.4843615275734478,
+            "timestamp": 2.4817101905504457
+        },
+        {
+            "x": 6.057049268441609,
+            "y": 0.995967852405878,
+            "heading": 0.16754642392936583,
+            "angularVelocity": -2.523463750187176e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.4843615275734476,
+            "timestamp": 2.5306303721724603
+        },
+        {
+            "x": 6.240588182206218,
+            "y": 0.9722727985062686,
+            "heading": 0.16754641158453532,
+            "angularVelocity": -2.523463753175626e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.48436152757344764,
+            "timestamp": 2.579550553794475
+        },
+        {
+            "x": 6.424127095970827,
+            "y": 0.948577744606659,
+            "heading": 0.1675463992397048,
+            "angularVelocity": -2.5234637555305e-7,
+            "velocityX": 3.7518036049566934,
+            "velocityY": -0.4843615275734476,
+            "timestamp": 2.6284707354164896
+        },
+        {
+            "x": 6.607666009735459,
+            "y": 0.9248826907072107,
+            "heading": 0.1675463868948743,
+            "angularVelocity": -2.5234637524939476e-7,
+            "velocityX": 3.7518036049571197,
+            "velocityY": -0.48436152757015266,
+            "timestamp": 2.677390917038504
+        },
+        {
+            "x": 6.79120492428193,
+            "y": 0.9011876428641385,
+            "heading": 0.16754637454996243,
+            "angularVelocity": -2.523480385625403e-7,
+            "velocityX": 3.7518036209390857,
+            "velocityY": -0.4843614037689742,
+            "timestamp": 2.726311098660519
+        },
+        {
+            "x": 6.9688913787459,
+            "y": 0.8784350451744124,
+            "heading": 0.1510406339197301,
+            "angularVelocity": -0.33740145851798203,
+            "velocityX": 3.6321707845828914,
+            "velocityY": -0.4650963454209109,
+            "timestamp": 2.7752312802825334
+        },
+        {
+            "x": 7.1269915101358645,
+            "y": 0.8581344466937117,
+            "heading": 0.12402265057850995,
+            "angularVelocity": -0.5522870611964742,
+            "velocityX": 3.2317977192222487,
+            "velocityY": -0.4149738984526803,
+            "timestamp": 2.824151461904548
+        },
+        {
+            "x": 7.265299167873697,
+            "y": 0.8403581253912503,
+            "heading": 0.0960275261585182,
+            "angularVelocity": -0.5722612527545016,
+            "velocityX": 2.8272106347943935,
+            "velocityY": -0.3633739841730647,
+            "timestamp": 2.8730716435265626
+        },
+        {
+            "x": 7.383819928892336,
+            "y": 0.8251162922066029,
+            "heading": 0.06979646830524168,
+            "angularVelocity": -0.5362011542792869,
+            "velocityX": 2.4227375510254454,
+            "velocityY": -0.3115653433671744,
+            "timestamp": 2.9219918251485772
+        },
+        {
+            "x": 7.482566860925546,
+            "y": 0.8124124441531555,
+            "heading": 0.04664460944238867,
+            "angularVelocity": -0.47325782724474763,
+            "velocityX": 2.01853159083065,
+            "velocityY": -0.2596852184974389,
+            "timestamp": 2.970912006770592
+        },
+        {
+            "x": 7.5615507925764724,
+            "y": 0.8022481812485672,
+            "heading": 0.027346506217350703,
+            "angularVelocity": -0.3944814304686405,
+            "velocityX": 1.6145469831081642,
+            "velocityY": -0.20777238692863387,
+            "timestamp": 3.0198321883926065
+        },
+        {
+            "x": 7.620780133649161,
+            "y": 0.7946243691967044,
+            "heading": 0.012413272398069258,
+            "angularVelocity": -0.3052571213791523,
+            "velocityX": 1.2107342840696753,
+            "velocityY": -0.15584185910773468,
+            "timestamp": 3.068752370014621
+        },
+        {
+            "x": 7.660261458521305,
+            "y": 0.7895415301241143,
+            "heading": 0.002207571288741933,
+            "angularVelocity": -0.2086194443876447,
+            "velocityX": 0.8070559749184711,
+            "velocityY": -0.10390065825721975,
+            "timestamp": 3.1176725516366357
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -0.003,
+            "angularVelocity": -0.10645036702804238,
+            "velocityX": 0.40348463199107626,
+            "velocityY": -0.05195258970523994,
+            "timestamp": 3.1665927332586503
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -0.003,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -1.0902826040652685e-32,
+            "timestamp": 3.215512914880665
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.1.traj
index 98cdd92f..6d3241e6 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.1.traj	
@@ -4,127 +4,127 @@
             "x": 2.3,
             "y": 6.757,
             "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 1.3865554557202343e-36,
-            "velocityY": 1.1876687369143486e-32,
+            "angularVelocity": -4.966859894029903e-31,
+            "velocityX": 7.6981862107013014e-34,
+            "velocityY": 1.676179535721191e-31,
             "timestamp": 0
         },
         {
-            "x": 2.373020964624076,
-            "y": 6.753775569913306,
-            "heading": 0.3835012481902214,
-            "angularVelocity": -0.06940035874833077,
-            "velocityX": 0.7797929955465333,
-            "velocityY": -0.034433782259311436,
-            "timestamp": 0.09364147285382916
+            "x": 2.3730209621579745,
+            "y": 6.753775517490119,
+            "heading": 0.38350122654498725,
+            "angularVelocity": -0.06940058989840046,
+            "velocityX": 0.7797929692101098,
+            "velocityY": -0.03443434208804683,
+            "timestamp": 0.09364147285393071
         },
         {
-            "x": 2.5190628897747724,
-            "y": 6.747326705309919,
-            "heading": 0.37050354798266344,
-            "angularVelocity": -0.13880281686559404,
-            "velocityX": 1.5595859473362035,
-            "velocityY": -0.06886761182670828,
-            "timestamp": 0.18728294570765833
+            "x": 2.5190628825903247,
+            "y": 6.747326552846718,
+            "heading": 0.37050348328006333,
+            "angularVelocity": -0.13880327667642306,
+            "velocityX": 1.5595858969471437,
+            "velocityY": -0.06886868015693057,
+            "timestamp": 0.18728294570786141
         },
         {
-            "x": 2.738125767671939,
-            "y": 6.737653401682201,
-            "heading": 0.35100657007014807,
-            "angularVelocity": -0.20820879166370637,
-            "velocityX": 2.3393788160414357,
-            "velocityY": -0.10330148953128306,
-            "timestamp": 0.2809244185614875
+            "x": 2.73812575373285,
+            "y": 6.7376531065331395,
+            "heading": 0.35100644039340695,
+            "angularVelocity": -0.2082094855243831,
+            "velocityX": 2.3393787439058484,
+            "velocityY": -0.10330301327774684,
+            "timestamp": 0.2809244185617921
         },
         {
-            "x": 3.0302095781027427,
-            "y": 6.7247556545524185,
-            "heading": 0.3250098803168658,
-            "angularVelocity": -0.27761940261055373,
-            "velocityX": 3.119171468893202,
-            "velocityY": -0.13773541505367973,
-            "timestamp": 0.37456589141531665
+            "x": 3.0302095555912687,
+            "y": 6.7247551792664035,
+            "heading": 0.3250096622235805,
+            "angularVelocity": -0.27762034681324244,
+            "velocityX": 3.1191713773449834,
+            "velocityY": -0.13773733874150373,
+            "timestamp": 0.37456589141572283
         },
         {
-            "x": 3.384104830792732,
-            "y": 6.709128195027275,
-            "heading": 0.3250098773247812,
-            "angularVelocity": -3.195254338233196e-8,
-            "velocityX": 3.77925765053277,
-            "velocityY": -0.16688609276314717,
-            "timestamp": 0.46820736426914583
+            "x": 3.3841048026937948,
+            "y": 6.709127593205063,
+            "heading": 0.3250096592315294,
+            "angularVelocity": -3.195216141250517e-8,
+            "velocityX": 3.779257590860006,
+            "velocityY": -0.16688744404647712,
+            "timestamp": 0.46820736426965354
         },
         {
-            "x": 3.738000080774581,
-            "y": 6.693500674175882,
-            "heading": 0.32500987433288897,
-            "angularVelocity": -3.1950503021671023e-8,
-            "velocityX": 3.7792576216124307,
-            "velocityY": -0.16688674766770242,
-            "timestamp": 0.561848837122975
+            "x": 3.7380000497922845,
+            "y": 6.693500007049851,
+            "heading": 0.3250096562396269,
+            "angularVelocity": -3.195060442591983e-8,
+            "velocityX": 3.7792575908168353,
+            "velocityY": -0.16688744504894126,
+            "timestamp": 0.5618488371235842
         },
         {
-            "x": 4.091895327748041,
-            "y": 6.677873085197927,
-            "heading": 0.32500987134099335,
-            "angularVelocity": -3.1950540402322325e-8,
-            "velocityX": 3.7792575894857667,
-            "velocityY": -0.1668874751932408,
-            "timestamp": 0.6554903099768041
+            "x": 4.091895296890773,
+            "y": 6.677872420894639,
+            "heading": 0.3250096532477245,
+            "angularVelocity": -3.195060369258429e-8,
+            "velocityX": 3.7792575908168353,
+            "velocityY": -0.16688744504893968,
+            "timestamp": 0.6554903099775149
         },
         {
-            "x": 4.445790571342994,
-            "y": 6.662245419680397,
-            "heading": 0.32500986834901585,
-            "angularVelocity": -3.1951399041908766e-8,
-            "velocityX": 3.7792575534066395,
-            "velocityY": -0.166888292561096,
-            "timestamp": 0.7491317828306333
+            "x": 4.445790543993225,
+            "y": 6.6622448348316095,
+            "heading": 0.3250096502556742,
+            "angularVelocity": -3.195215253798021e-8,
+            "velocityX": 3.7792575908592094,
+            "velocityY": -0.1668874440645195,
+            "timestamp": 0.7491317828314457
         },
         {
-            "x": 4.737874328369047,
-            "y": 6.649347277173169,
-            "heading": 0.29900623239629953,
-            "angularVelocity": -0.2776935812758607,
-            "velocityX": 3.119170898582289,
-            "velocityY": -0.13773963730152244,
-            "timestamp": 0.8427732556844625
+            "x": 4.737874310687331,
+            "y": 6.649346902155386,
+            "heading": 0.29900607193864137,
+            "angularVelocity": -0.27769296578215763,
+            "velocityX": 3.119171001824275,
+            "velocityY": -0.1377373965095818,
+            "timestamp": 0.8427732556853764
         },
         {
-            "x": 4.956937160697527,
-            "y": 6.639673651489352,
-            "heading": 0.27950319129562423,
-            "angularVelocity": -0.20827354062589903,
-            "velocityX": 2.339378329412089,
-            "velocityY": -0.10330492877809694,
-            "timestamp": 0.9364147285382917
+            "x": 4.956937151198913,
+            "y": 6.639673451411702,
+            "heading": 0.27950309637569815,
+            "angularVelocity": -0.20827284074623165,
+            "velocityX": 2.339378416797119,
+            "velocityY": -0.10330306058697603,
+            "timestamp": 0.9364147285393072
         },
         {
-            "x": 5.102979052772278,
-            "y": 6.633224554883952,
-            "heading": 0.26650106474792545,
-            "angularVelocity": -0.13885008588030126,
-            "velocityX": 1.5595855941172054,
-            "velocityY": -0.06887008938317173,
-            "timestamp": 1.0300562013921208
+            "x": 5.10297904938153,
+            "y": 6.633224483848421,
+            "heading": 0.2665010284217395,
+            "angularVelocity": -0.1388494601558462,
+            "velocityX": 1.5595856593415998,
+            "velocityY": -0.06886871133840435,
+            "timestamp": 1.030056201393238
         },
         {
             "x": 5.176,
             "y": 6.63,
             "heading": 0.26,
-            "angularVelocity": -0.06942505868186148,
-            "velocityX": 0.7797928097703705,
-            "velocityY": -0.03443511497296222,
-            "timestamp": 1.12369767424595
+            "angularVelocity": -0.06942467075336838,
+            "velocityX": 0.7797928459794067,
+            "velocityY": -0.03443435638235996,
+            "timestamp": 1.1236976742471687
         },
         {
             "x": 5.176,
             "y": 6.63,
             "heading": 0.26,
-            "angularVelocity": 0,
-            "velocityX": -1.6658104354370778e-33,
-            "velocityY": -5.427630465010738e-32,
-            "timestamp": 1.217339147099779
+            "angularVelocity": 2.7813300894409408e-31,
+            "velocityX": -8.985802616555554e-33,
+            "velocityY": -4.9789977648629575e-31,
+            "timestamp": 1.2173391471010995
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.traj
index 98cdd92f..6d3241e6 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.traj	
@@ -4,127 +4,127 @@
             "x": 2.3,
             "y": 6.757,
             "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 1.3865554557202343e-36,
-            "velocityY": 1.1876687369143486e-32,
+            "angularVelocity": -4.966859894029903e-31,
+            "velocityX": 7.6981862107013014e-34,
+            "velocityY": 1.676179535721191e-31,
             "timestamp": 0
         },
         {
-            "x": 2.373020964624076,
-            "y": 6.753775569913306,
-            "heading": 0.3835012481902214,
-            "angularVelocity": -0.06940035874833077,
-            "velocityX": 0.7797929955465333,
-            "velocityY": -0.034433782259311436,
-            "timestamp": 0.09364147285382916
+            "x": 2.3730209621579745,
+            "y": 6.753775517490119,
+            "heading": 0.38350122654498725,
+            "angularVelocity": -0.06940058989840046,
+            "velocityX": 0.7797929692101098,
+            "velocityY": -0.03443434208804683,
+            "timestamp": 0.09364147285393071
         },
         {
-            "x": 2.5190628897747724,
-            "y": 6.747326705309919,
-            "heading": 0.37050354798266344,
-            "angularVelocity": -0.13880281686559404,
-            "velocityX": 1.5595859473362035,
-            "velocityY": -0.06886761182670828,
-            "timestamp": 0.18728294570765833
+            "x": 2.5190628825903247,
+            "y": 6.747326552846718,
+            "heading": 0.37050348328006333,
+            "angularVelocity": -0.13880327667642306,
+            "velocityX": 1.5595858969471437,
+            "velocityY": -0.06886868015693057,
+            "timestamp": 0.18728294570786141
         },
         {
-            "x": 2.738125767671939,
-            "y": 6.737653401682201,
-            "heading": 0.35100657007014807,
-            "angularVelocity": -0.20820879166370637,
-            "velocityX": 2.3393788160414357,
-            "velocityY": -0.10330148953128306,
-            "timestamp": 0.2809244185614875
+            "x": 2.73812575373285,
+            "y": 6.7376531065331395,
+            "heading": 0.35100644039340695,
+            "angularVelocity": -0.2082094855243831,
+            "velocityX": 2.3393787439058484,
+            "velocityY": -0.10330301327774684,
+            "timestamp": 0.2809244185617921
         },
         {
-            "x": 3.0302095781027427,
-            "y": 6.7247556545524185,
-            "heading": 0.3250098803168658,
-            "angularVelocity": -0.27761940261055373,
-            "velocityX": 3.119171468893202,
-            "velocityY": -0.13773541505367973,
-            "timestamp": 0.37456589141531665
+            "x": 3.0302095555912687,
+            "y": 6.7247551792664035,
+            "heading": 0.3250096622235805,
+            "angularVelocity": -0.27762034681324244,
+            "velocityX": 3.1191713773449834,
+            "velocityY": -0.13773733874150373,
+            "timestamp": 0.37456589141572283
         },
         {
-            "x": 3.384104830792732,
-            "y": 6.709128195027275,
-            "heading": 0.3250098773247812,
-            "angularVelocity": -3.195254338233196e-8,
-            "velocityX": 3.77925765053277,
-            "velocityY": -0.16688609276314717,
-            "timestamp": 0.46820736426914583
+            "x": 3.3841048026937948,
+            "y": 6.709127593205063,
+            "heading": 0.3250096592315294,
+            "angularVelocity": -3.195216141250517e-8,
+            "velocityX": 3.779257590860006,
+            "velocityY": -0.16688744404647712,
+            "timestamp": 0.46820736426965354
         },
         {
-            "x": 3.738000080774581,
-            "y": 6.693500674175882,
-            "heading": 0.32500987433288897,
-            "angularVelocity": -3.1950503021671023e-8,
-            "velocityX": 3.7792576216124307,
-            "velocityY": -0.16688674766770242,
-            "timestamp": 0.561848837122975
+            "x": 3.7380000497922845,
+            "y": 6.693500007049851,
+            "heading": 0.3250096562396269,
+            "angularVelocity": -3.195060442591983e-8,
+            "velocityX": 3.7792575908168353,
+            "velocityY": -0.16688744504894126,
+            "timestamp": 0.5618488371235842
         },
         {
-            "x": 4.091895327748041,
-            "y": 6.677873085197927,
-            "heading": 0.32500987134099335,
-            "angularVelocity": -3.1950540402322325e-8,
-            "velocityX": 3.7792575894857667,
-            "velocityY": -0.1668874751932408,
-            "timestamp": 0.6554903099768041
+            "x": 4.091895296890773,
+            "y": 6.677872420894639,
+            "heading": 0.3250096532477245,
+            "angularVelocity": -3.195060369258429e-8,
+            "velocityX": 3.7792575908168353,
+            "velocityY": -0.16688744504893968,
+            "timestamp": 0.6554903099775149
         },
         {
-            "x": 4.445790571342994,
-            "y": 6.662245419680397,
-            "heading": 0.32500986834901585,
-            "angularVelocity": -3.1951399041908766e-8,
-            "velocityX": 3.7792575534066395,
-            "velocityY": -0.166888292561096,
-            "timestamp": 0.7491317828306333
+            "x": 4.445790543993225,
+            "y": 6.6622448348316095,
+            "heading": 0.3250096502556742,
+            "angularVelocity": -3.195215253798021e-8,
+            "velocityX": 3.7792575908592094,
+            "velocityY": -0.1668874440645195,
+            "timestamp": 0.7491317828314457
         },
         {
-            "x": 4.737874328369047,
-            "y": 6.649347277173169,
-            "heading": 0.29900623239629953,
-            "angularVelocity": -0.2776935812758607,
-            "velocityX": 3.119170898582289,
-            "velocityY": -0.13773963730152244,
-            "timestamp": 0.8427732556844625
+            "x": 4.737874310687331,
+            "y": 6.649346902155386,
+            "heading": 0.29900607193864137,
+            "angularVelocity": -0.27769296578215763,
+            "velocityX": 3.119171001824275,
+            "velocityY": -0.1377373965095818,
+            "timestamp": 0.8427732556853764
         },
         {
-            "x": 4.956937160697527,
-            "y": 6.639673651489352,
-            "heading": 0.27950319129562423,
-            "angularVelocity": -0.20827354062589903,
-            "velocityX": 2.339378329412089,
-            "velocityY": -0.10330492877809694,
-            "timestamp": 0.9364147285382917
+            "x": 4.956937151198913,
+            "y": 6.639673451411702,
+            "heading": 0.27950309637569815,
+            "angularVelocity": -0.20827284074623165,
+            "velocityX": 2.339378416797119,
+            "velocityY": -0.10330306058697603,
+            "timestamp": 0.9364147285393072
         },
         {
-            "x": 5.102979052772278,
-            "y": 6.633224554883952,
-            "heading": 0.26650106474792545,
-            "angularVelocity": -0.13885008588030126,
-            "velocityX": 1.5595855941172054,
-            "velocityY": -0.06887008938317173,
-            "timestamp": 1.0300562013921208
+            "x": 5.10297904938153,
+            "y": 6.633224483848421,
+            "heading": 0.2665010284217395,
+            "angularVelocity": -0.1388494601558462,
+            "velocityX": 1.5595856593415998,
+            "velocityY": -0.06886871133840435,
+            "timestamp": 1.030056201393238
         },
         {
             "x": 5.176,
             "y": 6.63,
             "heading": 0.26,
-            "angularVelocity": -0.06942505868186148,
-            "velocityX": 0.7797928097703705,
-            "velocityY": -0.03443511497296222,
-            "timestamp": 1.12369767424595
+            "angularVelocity": -0.06942467075336838,
+            "velocityX": 0.7797928459794067,
+            "velocityY": -0.03443435638235996,
+            "timestamp": 1.1236976742471687
         },
         {
             "x": 5.176,
             "y": 6.63,
             "heading": 0.26,
-            "angularVelocity": 0,
-            "velocityX": -1.6658104354370778e-33,
-            "velocityY": -5.427630465010738e-32,
-            "timestamp": 1.217339147099779
+            "angularVelocity": 2.7813300894409408e-31,
+            "velocityX": -8.985802616555554e-33,
+            "velocityY": -4.9789977648629575e-31,
+            "timestamp": 1.2173391471010995
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.1.traj
index e89d75b6..15f8fbd9 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.1.traj	
@@ -4,127 +4,127 @@
             "x": 2.3,
             "y": 6.757,
             "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 1.1894430825117781e-34,
-            "velocityY": 3.241165026752671e-33,
+            "angularVelocity": -5.274350592003984e-33,
+            "velocityX": 6.842220334639481e-35,
+            "velocityY": 4.754828012658472e-35,
             "timestamp": 0
         },
         {
-            "x": 2.347774772680024,
-            "y": 6.727866824995534,
-            "heading": 0.376906913073687,
-            "angularVelocity": -0.15965534994810393,
-            "velocityX": 0.5825591851522491,
-            "velocityY": -0.35524603759330214,
-            "timestamp": 0.08200844463131937
+            "x": 2.3477747650996332,
+            "y": 6.727866811231813,
+            "heading": 0.37690695075838176,
+            "angularVelocity": -0.1596548904259805,
+            "velocityX": 0.5825590927179829,
+            "velocityY": -0.3552462054262789,
+            "timestamp": 0.08200844463131846
         },
         {
-            "x": 2.443324407414016,
-            "y": 6.669600558032611,
-            "heading": 0.35072353035940795,
-            "angularVelocity": -0.3192766651286972,
-            "velocityX": 1.1651194601183832,
-            "velocityY": -0.7104910625346852,
-            "timestamp": 0.16401688926263874
+            "x": 2.4433243855136606,
+            "y": 6.669600518122756,
+            "heading": 0.35072364334029893,
+            "angularVelocity": -0.3192757469769597,
+            "velocityX": 1.16511928550267,
+            "velocityY": -0.7104913813571495,
+            "timestamp": 0.1640168892626369
         },
         {
-            "x": 2.5866490395090382,
-            "y": 6.582201337136711,
-            "heading": 0.31145340371020835,
-            "angularVelocity": -0.4788546694885386,
-            "velocityX": 1.7476813850007613,
-            "velocityY": -1.0657344044118373,
-            "timestamp": 0.24602533389395811
+            "x": 2.5866489974259967,
+            "y": 6.582201260145896,
+            "heading": 0.3114536293241488,
+            "angularVelocity": -0.47885329605621113,
+            "velocityX": 1.7476811388958082,
+            "velocityY": -1.0657348565721152,
+            "timestamp": 0.24602533389395537
         },
         {
-            "x": 2.7777488318834505,
-            "y": 6.4656693736120285,
-            "heading": 0.25909914961330927,
-            "angularVelocity": -0.6384007687533294,
-            "velocityX": 2.330245296487799,
-            "velocityY": -1.420975169673899,
-            "timestamp": 0.3280337785252775
+            "x": 2.7777487646808345,
+            "y": 6.465669250143918,
+            "heading": 0.2590995246277934,
+            "angularVelocity": -0.638398946983098,
+            "velocityX": 2.330244990183096,
+            "velocityY": -1.4209757364118492,
+            "timestamp": 0.3280337785252738
         },
         {
-            "x": 3.016623938716172,
-            "y": 6.320004968537675,
-            "heading": 0.19366021259536087,
-            "angularVelocity": -0.7979536413858164,
-            "velocityX": 2.9128110880119595,
-            "velocityY": -1.7762122636175968,
-            "timestamp": 0.4100422231565969
+            "x": 3.0166238424465086,
+            "y": 6.320004790854863,
+            "heading": 0.19366077275903337,
+            "angularVelocity": -0.797951383701398,
+            "velocityX": 2.912810733572304,
+            "velocityY": -1.7762129247044165,
+            "timestamp": 0.4100422231565923
         },
         {
-            "x": 3.281496231375059,
-            "y": 6.1584895705745515,
-            "heading": 0.19366020431656966,
-            "angularVelocity": -1.0095047213207674e-7,
-            "velocityX": 3.2298172932026463,
-            "velocityY": -1.9694971498269327,
-            "timestamp": 0.4920506677879163
+            "x": 3.281496122930573,
+            "y": 6.158489372926003,
+            "heading": 0.19366076448026312,
+            "angularVelocity": -1.0095021672660313e-7,
+            "velocityX": 3.2298171447445223,
+            "velocityY": -1.9694973932864837,
+            "timestamp": 0.49205066778791073
         },
         {
-            "x": 3.54636850972689,
-            "y": 5.99697414914899,
-            "heading": 0.1936601960377879,
-            "angularVelocity": -1.0095035712972839e-7,
-            "velocityX": 3.2298171187443208,
-            "velocityY": -1.9694974359247637,
-            "timestamp": 0.5740591124192357
+            "x": 3.546368403715213,
+            "y": 5.996973955490063,
+            "heading": 0.193660756201493,
+            "angularVelocity": -1.0095021513248387e-7,
+            "velocityX": 3.2298171484097047,
+            "velocityY": -1.9694973872758792,
+            "timestamp": 0.5740591124192291
         },
         {
-            "x": 3.7852456869653537,
-            "y": 5.851315832693838,
-            "heading": 0.1281196168762306,
-            "angularVelocity": -0.7991930520838956,
-            "velocityX": 2.91283633426253,
-            "velocityY": -1.7761380198097898,
-            "timestamp": 0.6560675570505551
+            "x": 3.785245611761769,
+            "y": 5.851315694507869,
+            "heading": 0.1281200370686441,
+            "angularVelocity": -0.7991947588751046,
+            "velocityX": 2.912836709932308,
+            "velocityY": -1.7761373433799759,
+            "timestamp": 0.6560675570505476
         },
         {
-            "x": 3.976347420420938,
-            "y": 5.73478940159931,
-            "heading": 0.07567676319198373,
-            "angularVelocity": -0.63948114026563,
-            "velocityX": 2.3302689657718703,
-            "velocityY": -1.420907707960915,
-            "timestamp": 0.7380760016818745
+            "x": 3.9763473726141734,
+            "y": 5.734789313275287,
+            "heading": 0.07567704387560718,
+            "angularVelocity": -0.6394828414171544,
+            "velocityX": 2.3302692998450527,
+            "velocityY": -1.4209070999510438,
+            "timestamp": 0.7380760016818662
         },
         {
-            "x": 4.119673708551739,
-            "y": 5.647394681888429,
-            "heading": 0.03633934856671714,
-            "angularVelocity": -0.47967517006466276,
-            "velocityX": 1.7477015784794376,
-            "velocityY": -1.0656795175640326,
-            "timestamp": 0.8200844463131939
+            "x": 4.119673683324307,
+            "y": 5.647394635056781,
+            "heading": 0.036339503004366135,
+            "angularVelocity": -0.47967670949118707,
+            "velocityX": 1.7477018538088223,
+            "velocityY": -1.0656790116115789,
+            "timestamp": 0.8200844463131847
         },
         {
-            "x": 4.215224566025358,
-            "y": 5.589131563991008,
-            "heading": 0.01011289315165925,
-            "angularVelocity": -0.31980188787828684,
-            "velocityX": 1.1651343700417993,
-            "velocityY": -0.7104526632513426,
-            "timestamp": 0.9020928909445133
+            "x": 4.2152245571841105,
+            "y": 5.589131547509505,
+            "heading": 0.010112949222819228,
+            "angularVelocity": -0.3198030873460947,
+            "velocityX": 1.1651345698527396,
+            "velocityY": -0.710452293165722,
+            "timestamp": 0.9020928909445032
         },
         {
             "x": 4.263,
             "y": 5.56,
             "heading": -0.003,
-            "angularVelocity": -0.15989686440963644,
-            "velocityX": 0.5825672488903126,
-            "velocityY": -0.3552263931107682,
-            "timestamp": 0.9841013355758327
+            "angularVelocity": -0.15989754813385992,
+            "velocityX": 0.5825673566993228,
+            "velocityY": -0.3552261921375451,
+            "timestamp": 0.9841013355758217
         },
         {
             "x": 4.263,
             "y": 5.56,
             "heading": -0.003,
-            "angularVelocity": -2.9357360091407933e-34,
-            "velocityX": 2.8612808764100986e-34,
-            "velocityY": 0,
-            "timestamp": 1.066109780207152
+            "angularVelocity": 2.1439897413450708e-33,
+            "velocityX": 3.9230953244452345e-34,
+            "velocityY": -1.8972025919917558e-33,
+            "timestamp": 1.0661097802071402
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.traj
index e89d75b6..15f8fbd9 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.traj	
@@ -4,127 +4,127 @@
             "x": 2.3,
             "y": 6.757,
             "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 1.1894430825117781e-34,
-            "velocityY": 3.241165026752671e-33,
+            "angularVelocity": -5.274350592003984e-33,
+            "velocityX": 6.842220334639481e-35,
+            "velocityY": 4.754828012658472e-35,
             "timestamp": 0
         },
         {
-            "x": 2.347774772680024,
-            "y": 6.727866824995534,
-            "heading": 0.376906913073687,
-            "angularVelocity": -0.15965534994810393,
-            "velocityX": 0.5825591851522491,
-            "velocityY": -0.35524603759330214,
-            "timestamp": 0.08200844463131937
+            "x": 2.3477747650996332,
+            "y": 6.727866811231813,
+            "heading": 0.37690695075838176,
+            "angularVelocity": -0.1596548904259805,
+            "velocityX": 0.5825590927179829,
+            "velocityY": -0.3552462054262789,
+            "timestamp": 0.08200844463131846
         },
         {
-            "x": 2.443324407414016,
-            "y": 6.669600558032611,
-            "heading": 0.35072353035940795,
-            "angularVelocity": -0.3192766651286972,
-            "velocityX": 1.1651194601183832,
-            "velocityY": -0.7104910625346852,
-            "timestamp": 0.16401688926263874
+            "x": 2.4433243855136606,
+            "y": 6.669600518122756,
+            "heading": 0.35072364334029893,
+            "angularVelocity": -0.3192757469769597,
+            "velocityX": 1.16511928550267,
+            "velocityY": -0.7104913813571495,
+            "timestamp": 0.1640168892626369
         },
         {
-            "x": 2.5866490395090382,
-            "y": 6.582201337136711,
-            "heading": 0.31145340371020835,
-            "angularVelocity": -0.4788546694885386,
-            "velocityX": 1.7476813850007613,
-            "velocityY": -1.0657344044118373,
-            "timestamp": 0.24602533389395811
+            "x": 2.5866489974259967,
+            "y": 6.582201260145896,
+            "heading": 0.3114536293241488,
+            "angularVelocity": -0.47885329605621113,
+            "velocityX": 1.7476811388958082,
+            "velocityY": -1.0657348565721152,
+            "timestamp": 0.24602533389395537
         },
         {
-            "x": 2.7777488318834505,
-            "y": 6.4656693736120285,
-            "heading": 0.25909914961330927,
-            "angularVelocity": -0.6384007687533294,
-            "velocityX": 2.330245296487799,
-            "velocityY": -1.420975169673899,
-            "timestamp": 0.3280337785252775
+            "x": 2.7777487646808345,
+            "y": 6.465669250143918,
+            "heading": 0.2590995246277934,
+            "angularVelocity": -0.638398946983098,
+            "velocityX": 2.330244990183096,
+            "velocityY": -1.4209757364118492,
+            "timestamp": 0.3280337785252738
         },
         {
-            "x": 3.016623938716172,
-            "y": 6.320004968537675,
-            "heading": 0.19366021259536087,
-            "angularVelocity": -0.7979536413858164,
-            "velocityX": 2.9128110880119595,
-            "velocityY": -1.7762122636175968,
-            "timestamp": 0.4100422231565969
+            "x": 3.0166238424465086,
+            "y": 6.320004790854863,
+            "heading": 0.19366077275903337,
+            "angularVelocity": -0.797951383701398,
+            "velocityX": 2.912810733572304,
+            "velocityY": -1.7762129247044165,
+            "timestamp": 0.4100422231565923
         },
         {
-            "x": 3.281496231375059,
-            "y": 6.1584895705745515,
-            "heading": 0.19366020431656966,
-            "angularVelocity": -1.0095047213207674e-7,
-            "velocityX": 3.2298172932026463,
-            "velocityY": -1.9694971498269327,
-            "timestamp": 0.4920506677879163
+            "x": 3.281496122930573,
+            "y": 6.158489372926003,
+            "heading": 0.19366076448026312,
+            "angularVelocity": -1.0095021672660313e-7,
+            "velocityX": 3.2298171447445223,
+            "velocityY": -1.9694973932864837,
+            "timestamp": 0.49205066778791073
         },
         {
-            "x": 3.54636850972689,
-            "y": 5.99697414914899,
-            "heading": 0.1936601960377879,
-            "angularVelocity": -1.0095035712972839e-7,
-            "velocityX": 3.2298171187443208,
-            "velocityY": -1.9694974359247637,
-            "timestamp": 0.5740591124192357
+            "x": 3.546368403715213,
+            "y": 5.996973955490063,
+            "heading": 0.193660756201493,
+            "angularVelocity": -1.0095021513248387e-7,
+            "velocityX": 3.2298171484097047,
+            "velocityY": -1.9694973872758792,
+            "timestamp": 0.5740591124192291
         },
         {
-            "x": 3.7852456869653537,
-            "y": 5.851315832693838,
-            "heading": 0.1281196168762306,
-            "angularVelocity": -0.7991930520838956,
-            "velocityX": 2.91283633426253,
-            "velocityY": -1.7761380198097898,
-            "timestamp": 0.6560675570505551
+            "x": 3.785245611761769,
+            "y": 5.851315694507869,
+            "heading": 0.1281200370686441,
+            "angularVelocity": -0.7991947588751046,
+            "velocityX": 2.912836709932308,
+            "velocityY": -1.7761373433799759,
+            "timestamp": 0.6560675570505476
         },
         {
-            "x": 3.976347420420938,
-            "y": 5.73478940159931,
-            "heading": 0.07567676319198373,
-            "angularVelocity": -0.63948114026563,
-            "velocityX": 2.3302689657718703,
-            "velocityY": -1.420907707960915,
-            "timestamp": 0.7380760016818745
+            "x": 3.9763473726141734,
+            "y": 5.734789313275287,
+            "heading": 0.07567704387560718,
+            "angularVelocity": -0.6394828414171544,
+            "velocityX": 2.3302692998450527,
+            "velocityY": -1.4209070999510438,
+            "timestamp": 0.7380760016818662
         },
         {
-            "x": 4.119673708551739,
-            "y": 5.647394681888429,
-            "heading": 0.03633934856671714,
-            "angularVelocity": -0.47967517006466276,
-            "velocityX": 1.7477015784794376,
-            "velocityY": -1.0656795175640326,
-            "timestamp": 0.8200844463131939
+            "x": 4.119673683324307,
+            "y": 5.647394635056781,
+            "heading": 0.036339503004366135,
+            "angularVelocity": -0.47967670949118707,
+            "velocityX": 1.7477018538088223,
+            "velocityY": -1.0656790116115789,
+            "timestamp": 0.8200844463131847
         },
         {
-            "x": 4.215224566025358,
-            "y": 5.589131563991008,
-            "heading": 0.01011289315165925,
-            "angularVelocity": -0.31980188787828684,
-            "velocityX": 1.1651343700417993,
-            "velocityY": -0.7104526632513426,
-            "timestamp": 0.9020928909445133
+            "x": 4.2152245571841105,
+            "y": 5.589131547509505,
+            "heading": 0.010112949222819228,
+            "angularVelocity": -0.3198030873460947,
+            "velocityX": 1.1651345698527396,
+            "velocityY": -0.710452293165722,
+            "timestamp": 0.9020928909445032
         },
         {
             "x": 4.263,
             "y": 5.56,
             "heading": -0.003,
-            "angularVelocity": -0.15989686440963644,
-            "velocityX": 0.5825672488903126,
-            "velocityY": -0.3552263931107682,
-            "timestamp": 0.9841013355758327
+            "angularVelocity": -0.15989754813385992,
+            "velocityX": 0.5825673566993228,
+            "velocityY": -0.3552261921375451,
+            "timestamp": 0.9841013355758217
         },
         {
             "x": 4.263,
             "y": 5.56,
             "heading": -0.003,
-            "angularVelocity": -2.9357360091407933e-34,
-            "velocityX": 2.8612808764100986e-34,
-            "velocityY": 0,
-            "timestamp": 1.066109780207152
+            "angularVelocity": 2.1439897413450708e-33,
+            "velocityX": 3.9230953244452345e-34,
+            "velocityY": -1.8972025919917558e-33,
+            "timestamp": 1.0661097802071402
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.1.traj
new file mode 100644
index 00000000..8360b86f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.1.traj	
@@ -0,0 +1,544 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.542786541644192e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2937082670823616,
+            "y": 6.738516922494428,
+            "heading": 0.3838170687349535,
+            "angularVelocity": -0.12752172771189255,
+            "velocityX": -0.1297657401618881,
+            "velocityY": -0.3812097977411177,
+            "timestamp": 0.04848531599949176
+        },
+        {
+            "x": 2.281123433600762,
+            "y": 6.70154498788335,
+            "heading": 0.37169259737448246,
+            "angularVelocity": -0.2500648105569625,
+            "velocityX": -0.25955968775719646,
+            "velocityY": -0.7625387985814647,
+            "timestamp": 0.09697063199898352
+        },
+        {
+            "x": 2.262243832879689,
+            "y": 6.646077048114969,
+            "heading": 0.3539392998096368,
+            "angularVelocity": -0.36615823159004923,
+            "velocityX": -0.3893880102034898,
+            "velocityY": -1.1440152265685524,
+            "timestamp": 0.14545594799847528
+        },
+        {
+            "x": 2.2370673940379584,
+            "y": 6.572104077537847,
+            "heading": 0.3309787534387302,
+            "angularVelocity": -0.4735567026240549,
+            "velocityX": -0.5192590441623445,
+            "velocityY": -1.5256778068215682,
+            "timestamp": 0.19394126399796704
+        },
+        {
+            "x": 2.2055915440305824,
+            "y": 6.479614437749851,
+            "heading": 0.303410444299779,
+            "angularVelocity": -0.5685908933466691,
+            "velocityX": -0.6491831466623577,
+            "velocityY": -1.9075804268039651,
+            "timestamp": 0.2424265799974588
+        },
+        {
+            "x": 2.167813255965874,
+            "y": 6.3685929704461826,
+            "heading": 0.2721534840825349,
+            "angularVelocity": -0.6446685882563702,
+            "velocityX": -0.7791696782014628,
+            "velocityY": -2.2897956838082623,
+            "timestamp": 0.29091189599695055
+        },
+        {
+            "x": 2.123730000007902,
+            "y": 6.239021112222755,
+            "heading": 0.23878941688886735,
+            "angularVelocity": -0.6881272505944181,
+            "velocityX": -0.909208387103519,
+            "velocityY": -2.6723938073299895,
+            "timestamp": 0.3393972119964423
+        },
+        {
+            "x": 2.0733472424162183,
+            "y": 6.090890823809695,
+            "heading": 0.2066566550746183,
+            "angularVelocity": -0.6627318220196932,
+            "velocityX": -1.0391343554894152,
+            "velocityY": -3.0551577391940747,
+            "timestamp": 0.38788252799593403
+        },
+        {
+            "x": 2.0168156590767947,
+            "y": 5.924489369921669,
+            "heading": 0.187194468928521,
+            "angularVelocity": -0.4014037187247154,
+            "velocityX": -1.1659526636929207,
+            "velocityY": -3.431996893448088,
+            "timestamp": 0.4363678439954258
+        },
+        {
+            "x": 1.9576664650342146,
+            "y": 5.750871436780747,
+            "heading": 0.18719445417707173,
+            "angularVelocity": -3.042457077435804e-7,
+            "velocityX": -1.2199403638687387,
+            "velocityY": -3.580835342863344,
+            "timestamp": 0.4848531599949175
+        },
+        {
+            "x": 1.8985172819179166,
+            "y": 5.577253499917319,
+            "heading": 0.18719443942575115,
+            "angularVelocity": -3.042430533078605e-7,
+            "velocityX": -1.2199401385163373,
+            "velocityY": -3.5808354196392784,
+            "timestamp": 0.5333384759944093
+        },
+        {
+            "x": 1.8393681174712941,
+            "y": 5.403635556693396,
+            "heading": 0.18719442467442912,
+            "angularVelocity": -3.0424308271607693e-7,
+            "velocityX": -1.219939753457998,
+            "velocityY": -3.5808355508232466,
+            "timestamp": 0.5818237919939011
+        },
+        {
+            "x": 1.780218973506269,
+            "y": 5.230017606491683,
+            "heading": 0.18719440992310651,
+            "angularVelocity": -3.0424309544377536e-7,
+            "velocityX": -1.2199393310291196,
+            "velocityY": -3.580835694738771,
+            "timestamp": 0.6303091079933929
+        },
+        {
+            "x": 1.7210698517072343,
+            "y": 5.056399648738334,
+            "heading": 0.1871943951717844,
+            "angularVelocity": -3.042430844451726e-7,
+            "velocityX": -1.2199388738599835,
+            "velocityY": -3.580835850489736,
+            "timestamp": 0.6787944239928847
+        },
+        {
+            "x": 1.661920753431811,
+            "y": 4.882781682970833,
+            "heading": 0.18719438042046438,
+            "angularVelocity": -3.042430423387178e-7,
+            "velocityX": -1.2199383886901667,
+            "velocityY": -3.580836015780049,
+            "timestamp": 0.7272797399923765
+        },
+        {
+            "x": 1.6027716795006517,
+            "y": 4.709163708909599,
+            "heading": 0.18719436566914785,
+            "angularVelocity": -3.042429697472901e-7,
+            "velocityX": -1.2199378865945683,
+            "velocityY": -3.5808361868366427,
+            "timestamp": 0.7757650559918683
+        },
+        {
+            "x": 1.5436226301529286,
+            "y": 4.5355457264731545,
+            "heading": 0.18719435091783626,
+            "angularVelocity": -3.042428676797686e-7,
+            "velocityX": -1.219937379566087,
+            "velocityY": -3.5808363595737087,
+            "timestamp": 0.8242503719913601
+        },
+        {
+            "x": 1.4844769425124074,
+            "y": 4.361926598741778,
+            "heading": 0.18719433616649686,
+            "angularVelocity": -3.0424344145116126e-7,
+            "velocityX": -1.2198680450801807,
+            "velocityY": -3.58085998103429,
+            "timestamp": 0.8727356879908519
+        },
+        {
+            "x": 1.4424355744034427,
+            "y": 4.183392744566331,
+            "heading": 0.18719432108286724,
+            "angularVelocity": -3.110968611299024e-7,
+            "velocityX": -0.8670948562865489,
+            "velocityY": -3.682225236552195,
+            "timestamp": 0.9212210039903437
+        },
+        {
+            "x": 1.4196862795998648,
+            "y": 4.0013919676043574,
+            "heading": 0.18719430492048,
+            "angularVelocity": -3.3334602243497606e-7,
+            "velocityX": -0.4691996810625877,
+            "velocityY": -3.753729829541186,
+            "timestamp": 0.9697063199898355
+        },
+        {
+            "x": 1.4164891295727136,
+            "y": 3.81800278977475,
+            "heading": 0.18719428668091828,
+            "angularVelocity": -3.761873328628986e-7,
+            "velocityX": -0.06594058347012394,
+            "velocityY": -3.7823653213178425,
+            "timestamp": 1.0181916359893273
+        },
+        {
+            "x": 1.432880701620732,
+            "y": 3.635319656493888,
+            "heading": 0.1871942648241357,
+            "angularVelocity": -4.507917942103899e-7,
+            "velocityX": 0.3380729136345315,
+            "velocityY": -3.767803292913241,
+            "timestamp": 1.066676951988819
+        },
+        {
+            "x": 1.4686738067578264,
+            "y": 3.455429018696199,
+            "heading": 0.18719405074105777,
+            "angularVelocity": -0.0000044154209051021175,
+            "velocityX": 0.7382256751280081,
+            "velocityY": -3.710208628927779,
+            "timestamp": 1.1151622679883109
+        },
+        {
+            "x": 1.5224561218642167,
+            "y": 3.2829543962052377,
+            "heading": 0.17869632281657946,
+            "angularVelocity": -0.17526394846200616,
+            "velocityX": 1.10924955315069,
+            "velocityY": -3.557254788081333,
+            "timestamp": 1.1636475839878027
+        },
+        {
+            "x": 1.5921416539328836,
+            "y": 3.121386413493124,
+            "heading": 0.1576086367549348,
+            "angularVelocity": -0.43492933122045885,
+            "velocityX": 1.4372502402541338,
+            "velocityY": -3.33230751169348,
+            "timestamp": 1.2121328999872945
+        },
+        {
+            "x": 1.6761645416915945,
+            "y": 2.9727322431612,
+            "heading": 0.12423651758368265,
+            "angularVelocity": -0.6882933210462135,
+            "velocityX": 1.7329553500274961,
+            "velocityY": -3.0659626995783453,
+            "timestamp": 1.2606182159867863
+        },
+        {
+            "x": 1.7731411061925635,
+            "y": 2.838565031466778,
+            "heading": 0.08175388007293943,
+            "angularVelocity": -0.8761959499529206,
+            "velocityX": 2.0001223566816617,
+            "velocityY": -2.767172058765521,
+            "timestamp": 1.309103531986278
+        },
+        {
+            "x": 1.8819216264400211,
+            "y": 2.719965378093697,
+            "heading": 0.033593056188374194,
+            "angularVelocity": -0.9933074146707891,
+            "velocityX": 2.2435765964482535,
+            "velocityY": -2.4460942643771193,
+            "timestamp": 1.3575888479857698
+        },
+        {
+            "x": 2.0016674106146493,
+            "y": 2.617587012881804,
+            "heading": -0.0178597683247242,
+            "angularVelocity": -1.0612042729434041,
+            "velocityX": 2.4697329842357973,
+            "velocityY": -2.1115334220519952,
+            "timestamp": 1.4060741639852616
+        },
+        {
+            "x": 2.131748914718628,
+            "y": 2.531857967376709,
+            "heading": -0.07087516069292028,
+            "angularVelocity": -1.0934319242038333,
+            "velocityX": 2.682905152268552,
+            "velocityY": -1.7681445142231829,
+            "timestamp": 1.4545594799847534
+        },
+        {
+            "x": 2.218821194191698,
+            "y": 2.484029060579093,
+            "heading": -0.10483282223316899,
+            "angularVelocity": -1.096901896110445,
+            "velocityX": 2.812612651242981,
+            "velocityY": -1.5449714785171498,
+            "timestamp": 1.4855172727096597
+        },
+        {
+            "x": 2.3097767814437278,
+            "y": 2.4431840829179357,
+            "heading": -0.13859592156415457,
+            "angularVelocity": -1.0906171389530375,
+            "velocityX": 2.938051432149048,
+            "velocityY": -1.3193762883647426,
+            "timestamp": 1.516475065434566
+        },
+        {
+            "x": 2.4044569077078632,
+            "y": 2.4094074987036276,
+            "heading": -0.1718069818405491,
+            "angularVelocity": -1.0727851488087659,
+            "velocityX": 3.0583616572737307,
+            "velocityY": -1.0910527282945428,
+            "timestamp": 1.5474328581594723
+        },
+        {
+            "x": 2.5026677503931767,
+            "y": 2.3827944816766236,
+            "heading": -0.20403633414457553,
+            "angularVelocity": -1.0410739741365005,
+            "velocityX": 3.1724110164298485,
+            "velocityY": -0.859654861819398,
+            "timestamp": 1.5783906508843786
+        },
+        {
+            "x": 2.6041682782784212,
+            "y": 2.363451815109364,
+            "heading": -0.23475758891885987,
+            "angularVelocity": -0.9923593405260972,
+            "velocityX": 3.2786745743372427,
+            "velocityY": -0.6248076773199155,
+            "timestamp": 1.6093484436092849
+        },
+        {
+            "x": 2.7086523080371263,
+            "y": 2.3514976196224144,
+            "heading": -0.26331031649970843,
+            "angularVelocity": -0.9223114784903548,
+            "velocityX": 3.3750477847899405,
+            "velocityY": -0.38614495527174003,
+            "timestamp": 1.6403062363341911
+        },
+        {
+            "x": 2.8157215031828033,
+            "y": 2.3470577985799963,
+            "heading": -0.288841887845313,
+            "angularVelocity": -0.8247219551529203,
+            "velocityX": 3.458553912306755,
+            "velocityY": -0.14341529714262294,
+            "timestamp": 1.6712640290590974
+        },
+        {
+            "x": 2.9248447015312338,
+            "y": 2.3502540561946077,
+            "heading": -0.3102182612745416,
+            "angularVelocity": -0.6905005668986338,
+            "velocityX": 3.524902415281217,
+            "velocityY": 0.10324565588123534,
+            "timestamp": 1.7022218217840037
+        },
+        {
+            "x": 3.0353304312162184,
+            "y": 2.361176073397281,
+            "heading": -0.326082506948615,
+            "angularVelocity": -0.5124475706430824,
+            "velocityX": 3.568914963243171,
+            "velocityY": 0.3528034863374462,
+            "timestamp": 1.73317961450891
+        },
+        {
+            "x": 3.1466093544989446,
+            "y": 2.379927120590619,
+            "heading": -0.3369287204650216,
+            "angularVelocity": -0.35035487229182166,
+            "velocityX": 3.594536738181179,
+            "velocityY": 0.605697161937562,
+            "timestamp": 1.7641374072338163
+        },
+        {
+            "x": 3.2578769593015027,
+            "y": 2.406586601459548,
+            "heading": -0.34366338052401463,
+            "angularVelocity": -0.21754328930658381,
+            "velocityX": 3.5941711281316278,
+            "velocityY": 0.8611557389062998,
+            "timestamp": 1.7950951999587226
+        },
+        {
+            "x": 3.3679824864036325,
+            "y": 2.4411213005139327,
+            "heading": -0.34790283328354016,
+            "angularVelocity": -0.13694299193573645,
+            "velocityX": 3.556633642474382,
+            "velocityY": 1.1155413876329803,
+            "timestamp": 1.8260529926836289
+        },
+        {
+            "x": 3.475304114496225,
+            "y": 2.4831460278092354,
+            "heading": -0.3524511595124565,
+            "angularVelocity": -0.14692023650373393,
+            "velocityX": 3.466708012625811,
+            "velocityY": 1.3574846136083376,
+            "timestamp": 1.8570107854085351
+        },
+        {
+            "x": 3.5778542240444473,
+            "y": 2.531447362567012,
+            "heading": -0.3616437420636369,
+            "angularVelocity": -0.29693921116020117,
+            "velocityX": 3.312578207999158,
+            "velocityY": 1.560231867531513,
+            "timestamp": 1.8879685781334414
+        },
+        {
+            "x": 3.673558919058804,
+            "y": 2.583451056106446,
+            "heading": -0.37745622232401294,
+            "angularVelocity": -0.5107754419475534,
+            "velocityX": 3.091457322713648,
+            "velocityY": 1.6798256258655577,
+            "timestamp": 1.9189263708583477
+        },
+        {
+            "x": 3.7613706261508923,
+            "y": 2.636186152260406,
+            "heading": -0.3968957996596081,
+            "angularVelocity": -0.6279380932955558,
+            "velocityX": 2.836497675157173,
+            "velocityY": 1.7034514257095354,
+            "timestamp": 1.949884163583254
+        },
+        {
+            "x": 3.841306735175595,
+            "y": 2.6876448087218088,
+            "heading": -0.4179459699473055,
+            "angularVelocity": -0.6799635386030257,
+            "velocityX": 2.582099755467769,
+            "velocityY": 1.6622198138851267,
+            "timestamp": 1.9808419563081603
+        },
+        {
+            "x": 3.913645075614716,
+            "y": 2.736629100397619,
+            "heading": -0.43934657697326235,
+            "angularVelocity": -0.6912833617387507,
+            "velocityX": 2.336676296079001,
+            "velocityY": 1.582292772328868,
+            "timestamp": 2.0117997490330666
+        },
+        {
+            "x": 3.9786691618750996,
+            "y": 2.7823974078733573,
+            "heading": -0.46027689817629036,
+            "angularVelocity": -0.6760921681319296,
+            "velocityX": 2.100410931692797,
+            "velocityY": 1.4784099074015198,
+            "timestamp": 2.0427575417579726
+        },
+        {
+            "x": 4.036618763930541,
+            "y": 2.824460782764613,
+            "heading": -0.48017689659950147,
+            "angularVelocity": -0.6428106357887643,
+            "velocityX": 1.8718906276836884,
+            "velocityY": 1.3587330099747585,
+            "timestamp": 2.0737153344828787
+        },
+        {
+            "x": 4.08768908254176,
+            "y": 2.862477844590781,
+            "heading": -0.49864492956893264,
+            "angularVelocity": -0.5965552238942878,
+            "velocityX": 1.6496757073449355,
+            "velocityY": 1.2280288250525657,
+            "timestamp": 2.1046731272077848
+        },
+        {
+            "x": 4.132038835388808,
+            "y": 2.8961988047734253,
+            "heading": -0.5153800748468587,
+            "angularVelocity": -0.5405794084679139,
+            "velocityX": 1.4325876925792385,
+            "velocityY": 1.0892559583398733,
+            "timestamp": 2.135630919932691
+        },
+        {
+            "x": 4.169798307734006,
+            "y": 2.9254338939938918,
+            "heading": -0.5301487101337384,
+            "angularVelocity": -0.47705711511170307,
+            "velocityX": 1.2197081581622198,
+            "velocityY": 0.944353154640467,
+            "timestamp": 2.166588712657597
+        },
+        {
+            "x": 4.201075770562755,
+            "y": 2.9500345091711524,
+            "heading": -0.5427642633485259,
+            "angularVelocity": -0.4075081620800149,
+            "velocityX": 1.0103259979397883,
+            "velocityY": 0.7946501675963605,
+            "timestamp": 2.197546505382503
+        },
+        {
+            "x": 4.225962326087245,
+            "y": 2.9698813733488647,
+            "heading": -0.5530743601373866,
+            "angularVelocity": -0.3330372058810695,
+            "velocityX": 0.8038866254348108,
+            "velocityY": 0.6410942909939014,
+            "timestamp": 2.228504298107409
+        },
+        {
+            "x": 4.24453552816763,
+            "y": 2.984876771487546,
+            "heading": -0.5609523103628415,
+            "angularVelocity": -0.2544738992160421,
+            "velocityX": 0.5999524011761236,
+            "velocityY": 0.48438201883216997,
+            "timestamp": 2.259462090832315
+        },
+        {
+            "x": 4.256862100646349,
+            "y": 2.9949392682905573,
+            "heading": -0.5662912533122896,
+            "angularVelocity": -0.17245877305093363,
+            "velocityX": 0.398173493448939,
+            "velocityY": 0.32503922008988784,
+            "timestamp": 2.290419883557221
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.569,
+            "angularVelocity": -0.08749805620543083,
+            "velocityX": 0.19826669841112912,
+            "velocityY": 0.16347198117160036,
+            "timestamp": 2.321377676282127
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.569,
+            "angularVelocity": 1.3974941712649426e-29,
+            "velocityX": 2.480011116993708e-30,
+            "velocityY": -4.1979337006948246e-30,
+            "timestamp": 2.3523354690070333
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.traj
new file mode 100644
index 00000000..8360b86f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.traj	
@@ -0,0 +1,544 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.542786541644192e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2937082670823616,
+            "y": 6.738516922494428,
+            "heading": 0.3838170687349535,
+            "angularVelocity": -0.12752172771189255,
+            "velocityX": -0.1297657401618881,
+            "velocityY": -0.3812097977411177,
+            "timestamp": 0.04848531599949176
+        },
+        {
+            "x": 2.281123433600762,
+            "y": 6.70154498788335,
+            "heading": 0.37169259737448246,
+            "angularVelocity": -0.2500648105569625,
+            "velocityX": -0.25955968775719646,
+            "velocityY": -0.7625387985814647,
+            "timestamp": 0.09697063199898352
+        },
+        {
+            "x": 2.262243832879689,
+            "y": 6.646077048114969,
+            "heading": 0.3539392998096368,
+            "angularVelocity": -0.36615823159004923,
+            "velocityX": -0.3893880102034898,
+            "velocityY": -1.1440152265685524,
+            "timestamp": 0.14545594799847528
+        },
+        {
+            "x": 2.2370673940379584,
+            "y": 6.572104077537847,
+            "heading": 0.3309787534387302,
+            "angularVelocity": -0.4735567026240549,
+            "velocityX": -0.5192590441623445,
+            "velocityY": -1.5256778068215682,
+            "timestamp": 0.19394126399796704
+        },
+        {
+            "x": 2.2055915440305824,
+            "y": 6.479614437749851,
+            "heading": 0.303410444299779,
+            "angularVelocity": -0.5685908933466691,
+            "velocityX": -0.6491831466623577,
+            "velocityY": -1.9075804268039651,
+            "timestamp": 0.2424265799974588
+        },
+        {
+            "x": 2.167813255965874,
+            "y": 6.3685929704461826,
+            "heading": 0.2721534840825349,
+            "angularVelocity": -0.6446685882563702,
+            "velocityX": -0.7791696782014628,
+            "velocityY": -2.2897956838082623,
+            "timestamp": 0.29091189599695055
+        },
+        {
+            "x": 2.123730000007902,
+            "y": 6.239021112222755,
+            "heading": 0.23878941688886735,
+            "angularVelocity": -0.6881272505944181,
+            "velocityX": -0.909208387103519,
+            "velocityY": -2.6723938073299895,
+            "timestamp": 0.3393972119964423
+        },
+        {
+            "x": 2.0733472424162183,
+            "y": 6.090890823809695,
+            "heading": 0.2066566550746183,
+            "angularVelocity": -0.6627318220196932,
+            "velocityX": -1.0391343554894152,
+            "velocityY": -3.0551577391940747,
+            "timestamp": 0.38788252799593403
+        },
+        {
+            "x": 2.0168156590767947,
+            "y": 5.924489369921669,
+            "heading": 0.187194468928521,
+            "angularVelocity": -0.4014037187247154,
+            "velocityX": -1.1659526636929207,
+            "velocityY": -3.431996893448088,
+            "timestamp": 0.4363678439954258
+        },
+        {
+            "x": 1.9576664650342146,
+            "y": 5.750871436780747,
+            "heading": 0.18719445417707173,
+            "angularVelocity": -3.042457077435804e-7,
+            "velocityX": -1.2199403638687387,
+            "velocityY": -3.580835342863344,
+            "timestamp": 0.4848531599949175
+        },
+        {
+            "x": 1.8985172819179166,
+            "y": 5.577253499917319,
+            "heading": 0.18719443942575115,
+            "angularVelocity": -3.042430533078605e-7,
+            "velocityX": -1.2199401385163373,
+            "velocityY": -3.5808354196392784,
+            "timestamp": 0.5333384759944093
+        },
+        {
+            "x": 1.8393681174712941,
+            "y": 5.403635556693396,
+            "heading": 0.18719442467442912,
+            "angularVelocity": -3.0424308271607693e-7,
+            "velocityX": -1.219939753457998,
+            "velocityY": -3.5808355508232466,
+            "timestamp": 0.5818237919939011
+        },
+        {
+            "x": 1.780218973506269,
+            "y": 5.230017606491683,
+            "heading": 0.18719440992310651,
+            "angularVelocity": -3.0424309544377536e-7,
+            "velocityX": -1.2199393310291196,
+            "velocityY": -3.580835694738771,
+            "timestamp": 0.6303091079933929
+        },
+        {
+            "x": 1.7210698517072343,
+            "y": 5.056399648738334,
+            "heading": 0.1871943951717844,
+            "angularVelocity": -3.042430844451726e-7,
+            "velocityX": -1.2199388738599835,
+            "velocityY": -3.580835850489736,
+            "timestamp": 0.6787944239928847
+        },
+        {
+            "x": 1.661920753431811,
+            "y": 4.882781682970833,
+            "heading": 0.18719438042046438,
+            "angularVelocity": -3.042430423387178e-7,
+            "velocityX": -1.2199383886901667,
+            "velocityY": -3.580836015780049,
+            "timestamp": 0.7272797399923765
+        },
+        {
+            "x": 1.6027716795006517,
+            "y": 4.709163708909599,
+            "heading": 0.18719436566914785,
+            "angularVelocity": -3.042429697472901e-7,
+            "velocityX": -1.2199378865945683,
+            "velocityY": -3.5808361868366427,
+            "timestamp": 0.7757650559918683
+        },
+        {
+            "x": 1.5436226301529286,
+            "y": 4.5355457264731545,
+            "heading": 0.18719435091783626,
+            "angularVelocity": -3.042428676797686e-7,
+            "velocityX": -1.219937379566087,
+            "velocityY": -3.5808363595737087,
+            "timestamp": 0.8242503719913601
+        },
+        {
+            "x": 1.4844769425124074,
+            "y": 4.361926598741778,
+            "heading": 0.18719433616649686,
+            "angularVelocity": -3.0424344145116126e-7,
+            "velocityX": -1.2198680450801807,
+            "velocityY": -3.58085998103429,
+            "timestamp": 0.8727356879908519
+        },
+        {
+            "x": 1.4424355744034427,
+            "y": 4.183392744566331,
+            "heading": 0.18719432108286724,
+            "angularVelocity": -3.110968611299024e-7,
+            "velocityX": -0.8670948562865489,
+            "velocityY": -3.682225236552195,
+            "timestamp": 0.9212210039903437
+        },
+        {
+            "x": 1.4196862795998648,
+            "y": 4.0013919676043574,
+            "heading": 0.18719430492048,
+            "angularVelocity": -3.3334602243497606e-7,
+            "velocityX": -0.4691996810625877,
+            "velocityY": -3.753729829541186,
+            "timestamp": 0.9697063199898355
+        },
+        {
+            "x": 1.4164891295727136,
+            "y": 3.81800278977475,
+            "heading": 0.18719428668091828,
+            "angularVelocity": -3.761873328628986e-7,
+            "velocityX": -0.06594058347012394,
+            "velocityY": -3.7823653213178425,
+            "timestamp": 1.0181916359893273
+        },
+        {
+            "x": 1.432880701620732,
+            "y": 3.635319656493888,
+            "heading": 0.1871942648241357,
+            "angularVelocity": -4.507917942103899e-7,
+            "velocityX": 0.3380729136345315,
+            "velocityY": -3.767803292913241,
+            "timestamp": 1.066676951988819
+        },
+        {
+            "x": 1.4686738067578264,
+            "y": 3.455429018696199,
+            "heading": 0.18719405074105777,
+            "angularVelocity": -0.0000044154209051021175,
+            "velocityX": 0.7382256751280081,
+            "velocityY": -3.710208628927779,
+            "timestamp": 1.1151622679883109
+        },
+        {
+            "x": 1.5224561218642167,
+            "y": 3.2829543962052377,
+            "heading": 0.17869632281657946,
+            "angularVelocity": -0.17526394846200616,
+            "velocityX": 1.10924955315069,
+            "velocityY": -3.557254788081333,
+            "timestamp": 1.1636475839878027
+        },
+        {
+            "x": 1.5921416539328836,
+            "y": 3.121386413493124,
+            "heading": 0.1576086367549348,
+            "angularVelocity": -0.43492933122045885,
+            "velocityX": 1.4372502402541338,
+            "velocityY": -3.33230751169348,
+            "timestamp": 1.2121328999872945
+        },
+        {
+            "x": 1.6761645416915945,
+            "y": 2.9727322431612,
+            "heading": 0.12423651758368265,
+            "angularVelocity": -0.6882933210462135,
+            "velocityX": 1.7329553500274961,
+            "velocityY": -3.0659626995783453,
+            "timestamp": 1.2606182159867863
+        },
+        {
+            "x": 1.7731411061925635,
+            "y": 2.838565031466778,
+            "heading": 0.08175388007293943,
+            "angularVelocity": -0.8761959499529206,
+            "velocityX": 2.0001223566816617,
+            "velocityY": -2.767172058765521,
+            "timestamp": 1.309103531986278
+        },
+        {
+            "x": 1.8819216264400211,
+            "y": 2.719965378093697,
+            "heading": 0.033593056188374194,
+            "angularVelocity": -0.9933074146707891,
+            "velocityX": 2.2435765964482535,
+            "velocityY": -2.4460942643771193,
+            "timestamp": 1.3575888479857698
+        },
+        {
+            "x": 2.0016674106146493,
+            "y": 2.617587012881804,
+            "heading": -0.0178597683247242,
+            "angularVelocity": -1.0612042729434041,
+            "velocityX": 2.4697329842357973,
+            "velocityY": -2.1115334220519952,
+            "timestamp": 1.4060741639852616
+        },
+        {
+            "x": 2.131748914718628,
+            "y": 2.531857967376709,
+            "heading": -0.07087516069292028,
+            "angularVelocity": -1.0934319242038333,
+            "velocityX": 2.682905152268552,
+            "velocityY": -1.7681445142231829,
+            "timestamp": 1.4545594799847534
+        },
+        {
+            "x": 2.218821194191698,
+            "y": 2.484029060579093,
+            "heading": -0.10483282223316899,
+            "angularVelocity": -1.096901896110445,
+            "velocityX": 2.812612651242981,
+            "velocityY": -1.5449714785171498,
+            "timestamp": 1.4855172727096597
+        },
+        {
+            "x": 2.3097767814437278,
+            "y": 2.4431840829179357,
+            "heading": -0.13859592156415457,
+            "angularVelocity": -1.0906171389530375,
+            "velocityX": 2.938051432149048,
+            "velocityY": -1.3193762883647426,
+            "timestamp": 1.516475065434566
+        },
+        {
+            "x": 2.4044569077078632,
+            "y": 2.4094074987036276,
+            "heading": -0.1718069818405491,
+            "angularVelocity": -1.0727851488087659,
+            "velocityX": 3.0583616572737307,
+            "velocityY": -1.0910527282945428,
+            "timestamp": 1.5474328581594723
+        },
+        {
+            "x": 2.5026677503931767,
+            "y": 2.3827944816766236,
+            "heading": -0.20403633414457553,
+            "angularVelocity": -1.0410739741365005,
+            "velocityX": 3.1724110164298485,
+            "velocityY": -0.859654861819398,
+            "timestamp": 1.5783906508843786
+        },
+        {
+            "x": 2.6041682782784212,
+            "y": 2.363451815109364,
+            "heading": -0.23475758891885987,
+            "angularVelocity": -0.9923593405260972,
+            "velocityX": 3.2786745743372427,
+            "velocityY": -0.6248076773199155,
+            "timestamp": 1.6093484436092849
+        },
+        {
+            "x": 2.7086523080371263,
+            "y": 2.3514976196224144,
+            "heading": -0.26331031649970843,
+            "angularVelocity": -0.9223114784903548,
+            "velocityX": 3.3750477847899405,
+            "velocityY": -0.38614495527174003,
+            "timestamp": 1.6403062363341911
+        },
+        {
+            "x": 2.8157215031828033,
+            "y": 2.3470577985799963,
+            "heading": -0.288841887845313,
+            "angularVelocity": -0.8247219551529203,
+            "velocityX": 3.458553912306755,
+            "velocityY": -0.14341529714262294,
+            "timestamp": 1.6712640290590974
+        },
+        {
+            "x": 2.9248447015312338,
+            "y": 2.3502540561946077,
+            "heading": -0.3102182612745416,
+            "angularVelocity": -0.6905005668986338,
+            "velocityX": 3.524902415281217,
+            "velocityY": 0.10324565588123534,
+            "timestamp": 1.7022218217840037
+        },
+        {
+            "x": 3.0353304312162184,
+            "y": 2.361176073397281,
+            "heading": -0.326082506948615,
+            "angularVelocity": -0.5124475706430824,
+            "velocityX": 3.568914963243171,
+            "velocityY": 0.3528034863374462,
+            "timestamp": 1.73317961450891
+        },
+        {
+            "x": 3.1466093544989446,
+            "y": 2.379927120590619,
+            "heading": -0.3369287204650216,
+            "angularVelocity": -0.35035487229182166,
+            "velocityX": 3.594536738181179,
+            "velocityY": 0.605697161937562,
+            "timestamp": 1.7641374072338163
+        },
+        {
+            "x": 3.2578769593015027,
+            "y": 2.406586601459548,
+            "heading": -0.34366338052401463,
+            "angularVelocity": -0.21754328930658381,
+            "velocityX": 3.5941711281316278,
+            "velocityY": 0.8611557389062998,
+            "timestamp": 1.7950951999587226
+        },
+        {
+            "x": 3.3679824864036325,
+            "y": 2.4411213005139327,
+            "heading": -0.34790283328354016,
+            "angularVelocity": -0.13694299193573645,
+            "velocityX": 3.556633642474382,
+            "velocityY": 1.1155413876329803,
+            "timestamp": 1.8260529926836289
+        },
+        {
+            "x": 3.475304114496225,
+            "y": 2.4831460278092354,
+            "heading": -0.3524511595124565,
+            "angularVelocity": -0.14692023650373393,
+            "velocityX": 3.466708012625811,
+            "velocityY": 1.3574846136083376,
+            "timestamp": 1.8570107854085351
+        },
+        {
+            "x": 3.5778542240444473,
+            "y": 2.531447362567012,
+            "heading": -0.3616437420636369,
+            "angularVelocity": -0.29693921116020117,
+            "velocityX": 3.312578207999158,
+            "velocityY": 1.560231867531513,
+            "timestamp": 1.8879685781334414
+        },
+        {
+            "x": 3.673558919058804,
+            "y": 2.583451056106446,
+            "heading": -0.37745622232401294,
+            "angularVelocity": -0.5107754419475534,
+            "velocityX": 3.091457322713648,
+            "velocityY": 1.6798256258655577,
+            "timestamp": 1.9189263708583477
+        },
+        {
+            "x": 3.7613706261508923,
+            "y": 2.636186152260406,
+            "heading": -0.3968957996596081,
+            "angularVelocity": -0.6279380932955558,
+            "velocityX": 2.836497675157173,
+            "velocityY": 1.7034514257095354,
+            "timestamp": 1.949884163583254
+        },
+        {
+            "x": 3.841306735175595,
+            "y": 2.6876448087218088,
+            "heading": -0.4179459699473055,
+            "angularVelocity": -0.6799635386030257,
+            "velocityX": 2.582099755467769,
+            "velocityY": 1.6622198138851267,
+            "timestamp": 1.9808419563081603
+        },
+        {
+            "x": 3.913645075614716,
+            "y": 2.736629100397619,
+            "heading": -0.43934657697326235,
+            "angularVelocity": -0.6912833617387507,
+            "velocityX": 2.336676296079001,
+            "velocityY": 1.582292772328868,
+            "timestamp": 2.0117997490330666
+        },
+        {
+            "x": 3.9786691618750996,
+            "y": 2.7823974078733573,
+            "heading": -0.46027689817629036,
+            "angularVelocity": -0.6760921681319296,
+            "velocityX": 2.100410931692797,
+            "velocityY": 1.4784099074015198,
+            "timestamp": 2.0427575417579726
+        },
+        {
+            "x": 4.036618763930541,
+            "y": 2.824460782764613,
+            "heading": -0.48017689659950147,
+            "angularVelocity": -0.6428106357887643,
+            "velocityX": 1.8718906276836884,
+            "velocityY": 1.3587330099747585,
+            "timestamp": 2.0737153344828787
+        },
+        {
+            "x": 4.08768908254176,
+            "y": 2.862477844590781,
+            "heading": -0.49864492956893264,
+            "angularVelocity": -0.5965552238942878,
+            "velocityX": 1.6496757073449355,
+            "velocityY": 1.2280288250525657,
+            "timestamp": 2.1046731272077848
+        },
+        {
+            "x": 4.132038835388808,
+            "y": 2.8961988047734253,
+            "heading": -0.5153800748468587,
+            "angularVelocity": -0.5405794084679139,
+            "velocityX": 1.4325876925792385,
+            "velocityY": 1.0892559583398733,
+            "timestamp": 2.135630919932691
+        },
+        {
+            "x": 4.169798307734006,
+            "y": 2.9254338939938918,
+            "heading": -0.5301487101337384,
+            "angularVelocity": -0.47705711511170307,
+            "velocityX": 1.2197081581622198,
+            "velocityY": 0.944353154640467,
+            "timestamp": 2.166588712657597
+        },
+        {
+            "x": 4.201075770562755,
+            "y": 2.9500345091711524,
+            "heading": -0.5427642633485259,
+            "angularVelocity": -0.4075081620800149,
+            "velocityX": 1.0103259979397883,
+            "velocityY": 0.7946501675963605,
+            "timestamp": 2.197546505382503
+        },
+        {
+            "x": 4.225962326087245,
+            "y": 2.9698813733488647,
+            "heading": -0.5530743601373866,
+            "angularVelocity": -0.3330372058810695,
+            "velocityX": 0.8038866254348108,
+            "velocityY": 0.6410942909939014,
+            "timestamp": 2.228504298107409
+        },
+        {
+            "x": 4.24453552816763,
+            "y": 2.984876771487546,
+            "heading": -0.5609523103628415,
+            "angularVelocity": -0.2544738992160421,
+            "velocityX": 0.5999524011761236,
+            "velocityY": 0.48438201883216997,
+            "timestamp": 2.259462090832315
+        },
+        {
+            "x": 4.256862100646349,
+            "y": 2.9949392682905573,
+            "heading": -0.5662912533122896,
+            "angularVelocity": -0.17245877305093363,
+            "velocityX": 0.398173493448939,
+            "velocityY": 0.32503922008988784,
+            "timestamp": 2.290419883557221
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.569,
+            "angularVelocity": -0.08749805620543083,
+            "velocityX": 0.19826669841112912,
+            "velocityY": 0.16347198117160036,
+            "timestamp": 2.321377676282127
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.569,
+            "angularVelocity": 1.3974941712649426e-29,
+            "velocityX": 2.480011116993708e-30,
+            "velocityY": -4.1979337006948246e-30,
+            "timestamp": 2.3523354690070333
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.1.traj
new file mode 100644
index 00000000..5aab1ee6
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": -2.4663146867319515e-36,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3001378208847427,
+            "y": 6.713171893956039,
+            "heading": 0.36291265694481545,
+            "angularVelocity": -0.37089801282350154,
+            "velocityX": 0.0018871357066083216,
+            "velocityY": -0.6001237332286558,
+            "timestamp": 0.0730317826428328
+        },
+        {
+            "x": 2.3004082222445,
+            "y": 6.625427179140099,
+            "heading": 0.3106389081779897,
+            "angularVelocity": -0.7157671204942956,
+            "velocityX": 0.0037025162192713427,
+            "velocityY": -1.201459304985902,
+            "timestamp": 0.1460635652856656
+        },
+        {
+            "x": 2.300808308388558,
+            "y": 6.493639269598709,
+            "heading": 0.23621859479744584,
+            "angularVelocity": -1.019012691289514,
+            "velocityX": 0.005478246998500719,
+            "velocityY": -1.804528176258669,
+            "timestamp": 0.2190953479284984
+        },
+        {
+            "x": 2.30134752968001,
+            "y": 6.317621999647275,
+            "heading": 0.14515321533996162,
+            "angularVelocity": -1.2469280655909227,
+            "velocityX": 0.007383378468093947,
+            "velocityY": -2.4101461525629078,
+            "timestamp": 0.2921271305713312
+        },
+        {
+            "x": 2.3020704977057354,
+            "y": 6.097150895386178,
+            "heading": 0.04952404086318575,
+            "angularVelocity": -1.309418598536164,
+            "velocityX": 0.00989936161439861,
+            "velocityY": -3.0188377755932185,
+            "timestamp": 0.36515891321416405
+        },
+        {
+            "x": 2.3031517302727234,
+            "y": 5.834309739739576,
+            "heading": 0.004115026920641748,
+            "angularVelocity": -0.6217705812361183,
+            "velocityX": 0.014804959263771778,
+            "velocityY": -3.598996849522972,
+            "timestamp": 0.4381906958569969
+        },
+        {
+            "x": 2.303521267718626,
+            "y": 5.558035105167069,
+            "heading": 0.00411499765356137,
+            "angularVelocity": -4.007444337013033e-7,
+            "velocityX": 0.005059953797233799,
+            "velocityY": -3.7829370251531125,
+            "timestamp": 0.5112224784998297
+        },
+        {
+            "x": 2.3038907744757537,
+            "y": 5.281760470553516,
+            "heading": 0.004114968386478979,
+            "angularVelocity": -4.0074446127092794e-7,
+            "velocityX": 0.005059533586019425,
+            "velocityY": -3.782937025715139,
+            "timestamp": 0.5842542611426625
+        },
+        {
+            "x": 2.3020058670727765,
+            "y": 5.019317688425813,
+            "heading": -0.04121766182368216,
+            "angularVelocity": -0.6207246840990261,
+            "velocityX": -0.02580941248819387,
+            "velocityY": -3.5935420529332935,
+            "timestamp": 0.6572860437854954
+        },
+        {
+            "x": 2.301170882315387,
+            "y": 4.798980955675817,
+            "heading": -0.139726223563735,
+            "angularVelocity": -1.348845094221732,
+            "velocityX": -0.011433169603334988,
+            "velocityY": -3.016997871016921,
+            "timestamp": 0.7303178264283282
+        },
+        {
+            "x": 2.3006513716532107,
+            "y": 4.623098256624685,
+            "heading": -0.23378951684706242,
+            "angularVelocity": -1.2879775062228829,
+            "velocityX": -0.007113487352719777,
+            "velocityY": -2.4083035178163246,
+            "timestamp": 0.803349609071161
+        },
+        {
+            "x": 2.300310703422249,
+            "y": 4.491429967185545,
+            "heading": -0.31073488166813007,
+            "angularVelocity": -1.0535873839664371,
+            "velocityX": -0.004664657203118652,
+            "velocityY": -1.8028902578357346,
+            "timestamp": 0.8763813917139939
+        },
+        {
+            "x": 2.3001003821722423,
+            "y": 4.403776446553788,
+            "heading": -0.36487144716496317,
+            "angularVelocity": -0.7412740527174575,
+            "velocityX": -0.0028798591845282696,
+            "velocityY": -1.2002106132398922,
+            "timestamp": 0.9494131743568267
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.393,
+            "angularVelocity": -0.3851549533249317,
+            "velocityX": -0.0013744998219966374,
+            "velocityY": -0.5994163769475577,
+            "timestamp": 1.0224449569996594
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.393,
+            "angularVelocity": -7.841220088894993e-36,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 1.0954767396424923
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.traj
new file mode 100644
index 00000000..5aab1ee6
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": -2.4663146867319515e-36,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3001378208847427,
+            "y": 6.713171893956039,
+            "heading": 0.36291265694481545,
+            "angularVelocity": -0.37089801282350154,
+            "velocityX": 0.0018871357066083216,
+            "velocityY": -0.6001237332286558,
+            "timestamp": 0.0730317826428328
+        },
+        {
+            "x": 2.3004082222445,
+            "y": 6.625427179140099,
+            "heading": 0.3106389081779897,
+            "angularVelocity": -0.7157671204942956,
+            "velocityX": 0.0037025162192713427,
+            "velocityY": -1.201459304985902,
+            "timestamp": 0.1460635652856656
+        },
+        {
+            "x": 2.300808308388558,
+            "y": 6.493639269598709,
+            "heading": 0.23621859479744584,
+            "angularVelocity": -1.019012691289514,
+            "velocityX": 0.005478246998500719,
+            "velocityY": -1.804528176258669,
+            "timestamp": 0.2190953479284984
+        },
+        {
+            "x": 2.30134752968001,
+            "y": 6.317621999647275,
+            "heading": 0.14515321533996162,
+            "angularVelocity": -1.2469280655909227,
+            "velocityX": 0.007383378468093947,
+            "velocityY": -2.4101461525629078,
+            "timestamp": 0.2921271305713312
+        },
+        {
+            "x": 2.3020704977057354,
+            "y": 6.097150895386178,
+            "heading": 0.04952404086318575,
+            "angularVelocity": -1.309418598536164,
+            "velocityX": 0.00989936161439861,
+            "velocityY": -3.0188377755932185,
+            "timestamp": 0.36515891321416405
+        },
+        {
+            "x": 2.3031517302727234,
+            "y": 5.834309739739576,
+            "heading": 0.004115026920641748,
+            "angularVelocity": -0.6217705812361183,
+            "velocityX": 0.014804959263771778,
+            "velocityY": -3.598996849522972,
+            "timestamp": 0.4381906958569969
+        },
+        {
+            "x": 2.303521267718626,
+            "y": 5.558035105167069,
+            "heading": 0.00411499765356137,
+            "angularVelocity": -4.007444337013033e-7,
+            "velocityX": 0.005059953797233799,
+            "velocityY": -3.7829370251531125,
+            "timestamp": 0.5112224784998297
+        },
+        {
+            "x": 2.3038907744757537,
+            "y": 5.281760470553516,
+            "heading": 0.004114968386478979,
+            "angularVelocity": -4.0074446127092794e-7,
+            "velocityX": 0.005059533586019425,
+            "velocityY": -3.782937025715139,
+            "timestamp": 0.5842542611426625
+        },
+        {
+            "x": 2.3020058670727765,
+            "y": 5.019317688425813,
+            "heading": -0.04121766182368216,
+            "angularVelocity": -0.6207246840990261,
+            "velocityX": -0.02580941248819387,
+            "velocityY": -3.5935420529332935,
+            "timestamp": 0.6572860437854954
+        },
+        {
+            "x": 2.301170882315387,
+            "y": 4.798980955675817,
+            "heading": -0.139726223563735,
+            "angularVelocity": -1.348845094221732,
+            "velocityX": -0.011433169603334988,
+            "velocityY": -3.016997871016921,
+            "timestamp": 0.7303178264283282
+        },
+        {
+            "x": 2.3006513716532107,
+            "y": 4.623098256624685,
+            "heading": -0.23378951684706242,
+            "angularVelocity": -1.2879775062228829,
+            "velocityX": -0.007113487352719777,
+            "velocityY": -2.4083035178163246,
+            "timestamp": 0.803349609071161
+        },
+        {
+            "x": 2.300310703422249,
+            "y": 4.491429967185545,
+            "heading": -0.31073488166813007,
+            "angularVelocity": -1.0535873839664371,
+            "velocityX": -0.004664657203118652,
+            "velocityY": -1.8028902578357346,
+            "timestamp": 0.8763813917139939
+        },
+        {
+            "x": 2.3001003821722423,
+            "y": 4.403776446553788,
+            "heading": -0.36487144716496317,
+            "angularVelocity": -0.7412740527174575,
+            "velocityX": -0.0028798591845282696,
+            "velocityY": -1.2002106132398922,
+            "timestamp": 0.9494131743568267
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.393,
+            "angularVelocity": -0.3851549533249317,
+            "velocityX": -0.0013744998219966374,
+            "velocityY": -0.5994163769475577,
+            "timestamp": 1.0224449569996594
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.393,
+            "angularVelocity": -7.841220088894993e-36,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 1.0954767396424923
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.1.traj
index 20e2615d..70287ec9 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.1.traj	
@@ -4,190 +4,190 @@
             "x": 2.25,
             "y": 5.56,
             "heading": 0,
-            "angularVelocity": -9.278807284273418e-29,
-            "velocityX": -7.385267906050254e-32,
-            "velocityY": -1.1558173496237395e-34,
+            "angularVelocity": 0,
+            "velocityX": -3.731399929679607e-32,
+            "velocityY": 5.877471754111438e-38,
             "timestamp": 0
         },
         {
-            "x": 2.3268918247049313,
-            "y": 5.587760380936702,
-            "heading": 0.001219352926916483,
-            "angularVelocity": 0.01231456999280599,
-            "velocityX": 0.776551018417967,
-            "velocityY": 0.2803594812685564,
-            "timestamp": 0.09901709337987798
-        },
-        {
-            "x": 2.480675474257635,
-            "y": 5.643281133231001,
-            "heading": 0.0036580544911558616,
-            "angularVelocity": 0.024629096663863686,
-            "velocityX": 1.5531020382785252,
-            "velocityY": 0.5607188657951702,
-            "timestamp": 0.19803418675975595
-        },
-        {
-            "x": 2.7113509459548553,
-            "y": 5.726562245995426,
-            "heading": 0.007316098440537021,
-            "angularVelocity": 0.03694356019247884,
-            "velocityX": 2.3296530308381804,
-            "velocityY": 0.8410781403663149,
-            "timestamp": 0.29705128013963394
-        },
-        {
-            "x": 3.0189182254277274,
-            "y": 5.837603703876164,
-            "heading": 0.01219347400929413,
-            "angularVelocity": 0.04925791499475694,
-            "velocityX": 3.1062038782828476,
-            "velocityY": 1.1214372598752256,
-            "timestamp": 0.3960683735195119
-        },
-        {
-            "x": 3.3712359540410324,
-            "y": 5.964801318197582,
-            "heading": 0.012193474535478832,
-            "angularVelocity": 5.31407952800269e-9,
-            "velocityX": 3.5581505837748795,
-            "velocityY": 1.2846025870848905,
-            "timestamp": 0.49508546689938987
-        },
-        {
-            "x": 3.723553713186701,
-            "y": 6.0919988479491165,
-            "heading": 0.012193475061665435,
-            "angularVelocity": 5.314098710197071e-9,
-            "velocityX": 3.5581508921293548,
-            "velocityY": 1.2846017329911394,
-            "timestamp": 0.5941025602792679
-        },
-        {
-            "x": 4.075871503881281,
-            "y": 6.219196290314954,
-            "heading": 0.012193475587862622,
-            "angularVelocity": 5.314205602691753e-9,
-            "velocityX": 3.558151210750214,
-            "velocityY": 1.2846008504596915,
-            "timestamp": 0.6931196536591459
-        },
-        {
-            "x": 4.428189328839488,
-            "y": 6.346393637775665,
-            "heading": 0.012193476114071436,
-            "angularVelocity": 5.3143230401144125e-9,
-            "velocityX": 3.5581515567877036,
-            "velocityY": 1.2845998919875412,
-            "timestamp": 0.7921367470390238
-        },
-        {
-            "x": 4.780507205251625,
-            "y": 6.473590842716609,
-            "heading": 0.012193476640289547,
-            "angularVelocity": 5.314416922842863e-9,
-            "velocityX": 3.5581520764346486,
-            "velocityY": 1.2845984526424705,
-            "timestamp": 0.8911538404189018
-        },
-        {
-            "x": 5.134861642335281,
-            "y": 6.594999030315895,
-            "heading": 0.012193476116517958,
-            "angularVelocity": -5.289708644023114e-9,
-            "velocityX": 3.5787198451098035,
-            "velocityY": 1.2261336245600862,
-            "timestamp": 0.9901709337987797
-        },
-        {
-            "x": 5.489216369156675,
-            "y": 6.716406372237666,
-            "heading": 0.012193475592327948,
-            "angularVelocity": -5.293934612313601e-9,
-            "velocityX": 3.578722771248356,
-            "velocityY": 1.2261250838377307,
-            "timestamp": 1.0891880271786578
-        },
-        {
-            "x": 5.843571141725175,
-            "y": 6.837813580636051,
-            "heading": 0.012193475068127738,
-            "angularVelocity": -5.2940376086876394e-9,
-            "velocityX": 3.5787232332605625,
-            "velocityY": 1.2261237353494956,
-            "timestamp": 1.1882051205585358
-        },
-        {
-            "x": 6.1979259496904,
-            "y": 6.959220685720912,
-            "heading": 0.012193474543942472,
-            "angularVelocity": -5.293886679384445e-9,
-            "velocityX": 3.5787235907415074,
-            "velocityY": 1.2261226919587003,
-            "timestamp": 1.2872222139384137
-        },
-        {
-            "x": 6.552280784920285,
-            "y": 7.080627711227486,
-            "heading": 0.012193474019771769,
-            "angularVelocity": -5.293739610148028e-9,
-            "velocityX": 3.5787238660945766,
-            "velocityY": 1.2261218882763663,
-            "timestamp": 1.3862393073182917
-        },
-        {
-            "x": 6.906635640601096,
-            "y": 7.202034677043121,
-            "heading": 0.01219347349560639,
-            "angularVelocity": -5.293685828527132e-9,
-            "velocityX": 3.5787240726339373,
-            "velocityY": 1.2261212854416952,
-            "timestamp": 1.4852564006981697
-        },
-        {
-            "x": 7.215981364626797,
-            "y": 7.3080208081486955,
-            "heading": 0.0073160851772923075,
-            "angularVelocity": -0.0492580437559288,
-            "velocityX": 3.124164863524117,
-            "velocityY": 1.0703821682480574,
-            "timestamp": 1.5842734940780476
-        },
-        {
-            "x": 7.447990676616371,
-            "y": 7.387510406301363,
-            "heading": 0.003658042213644346,
-            "angularVelocity": -0.03694355023729677,
-            "velocityX": 2.343123839229173,
-            "velocityY": 0.8027866244033968,
-            "timestamp": 1.6832905874579256
-        },
-        {
-            "x": 7.602663557476493,
-            "y": 7.440503469639918,
-            "heading": 0.0012193472043836597,
-            "angularVelocity": -0.024629030463387942,
-            "velocityX": 1.5620826221056736,
-            "velocityY": 0.5351910617619017,
-            "timestamp": 1.7823076808378036
+            "x": 2.3271275380851444,
+            "y": 5.587086964112038,
+            "heading": 3.953847862678034e-18,
+            "angularVelocity": 3.9932078666702264e-17,
+            "velocityX": 0.7789533333445218,
+            "velocityY": 0.2735661154121556,
+            "timestamp": 0.09901432445765257
+        },
+        {
+            "x": 2.4813826113143547,
+            "y": 5.641260891303218,
+            "heading": 1.1398162294527873e-19,
+            "angularVelocity": -3.8780916638560175e-17,
+            "velocityX": 1.557906636985473,
+            "velocityY": 0.5471322203925046,
+            "timestamp": 0.19802864891530514
+        },
+        {
+            "x": 2.7127652138054734,
+            "y": 5.722521779507741,
+            "heading": 2.1553394674505063e-18,
+            "angularVelocity": 2.0616792512846884e-17,
+            "velocityX": 2.336859881219295,
+            "velocityY": 0.8206983045092451,
+            "timestamp": 0.2970429733729577
+        },
+        {
+            "x": 3.0212753279120443,
+            "y": 5.830869622528225,
+            "heading": -1.0672476097054788e-17,
+            "angularVelocity": -1.295551486240031e-16,
+            "velocityX": 3.1158129472318716,
+            "velocityY": 1.0942643260352094,
+            "timestamp": 0.3960572978306103
+        },
+        {
+            "x": 3.3746798137461993,
+            "y": 5.9549842366139965,
+            "heading": 1.8895880094915182e-18,
+            "angularVelocity": 1.2687117924961395e-16,
+            "velocityX": 3.5692258445423475,
+            "velocityY": 1.2535015995473786,
+            "timestamp": 0.49507162228826285
+        },
+        {
+            "x": 3.7280842995803773,
+            "y": 6.079098850699775,
+            "heading": 1.0049319654091473e-17,
+            "angularVelocity": 8.240960778697987e-17,
+            "velocityX": 3.569225844542579,
+            "velocityY": 1.25350159954746,
+            "timestamp": 0.5940859467459154
+        },
+        {
+            "x": 4.081488785414555,
+            "y": 6.203213464785553,
+            "heading": 8.136834826577652e-18,
+            "angularVelocity": -1.931523381075779e-17,
+            "velocityX": 3.569225844542579,
+            "velocityY": 1.25350159954746,
+            "timestamp": 0.693100271203568
+        },
+        {
+            "x": 4.434893271248733,
+            "y": 6.327328078871332,
+            "heading": -2.4381204969164037e-18,
+            "angularVelocity": -1.0680227715899811e-16,
+            "velocityX": 3.5692258445425793,
+            "velocityY": 1.2535015995474599,
+            "timestamp": 0.7921145956612206
+        },
+        {
+            "x": 4.788297757082911,
+            "y": 6.4514426929571105,
+            "heading": 3.192038150785043e-17,
+            "angularVelocity": 3.47005367081625e-16,
+            "velocityX": 3.569225844542579,
+            "velocityY": 1.25350159954746,
+            "timestamp": 0.8911289201188731
+        },
+        {
+            "x": 5.141702242917089,
+            "y": 6.575557307042889,
+            "heading": 6.016026907559514e-17,
+            "angularVelocity": 2.8521012209839933e-16,
+            "velocityX": 3.569225844542579,
+            "velocityY": 1.2535015995474599,
+            "timestamp": 0.9901432445765257
+        },
+        {
+            "x": 5.495106728751266,
+            "y": 6.699671921128667,
+            "heading": 2.4186102644429564e-17,
+            "angularVelocity": -3.63322848771759e-16,
+            "velocityX": 3.569225844542579,
+            "velocityY": 1.2535015995474599,
+            "timestamp": 1.0891575690341782
+        },
+        {
+            "x": 5.848511214585444,
+            "y": 6.823786535214446,
+            "heading": -1.2421929874703125e-18,
+            "angularVelocity": -2.568143121812034e-16,
+            "velocityX": 3.5692258445425793,
+            "velocityY": 1.2535015995474599,
+            "timestamp": 1.1881718934918308
+        },
+        {
+            "x": 6.2019157004196215,
+            "y": 6.9479011493002245,
+            "heading": 2.3184212364315377e-18,
+            "angularVelocity": 3.596059704901295e-17,
+            "velocityX": 3.569225844542579,
+            "velocityY": 1.2535015995474599,
+            "timestamp": 1.2871862179494835
+        },
+        {
+            "x": 6.5553201862538,
+            "y": 7.072015763386003,
+            "heading": 3.1459219033425486e-17,
+            "angularVelocity": 2.9430890890068574e-16,
+            "velocityX": 3.569225844542579,
+            "velocityY": 1.25350159954746,
+            "timestamp": 1.3862005424071362
+        },
+        {
+            "x": 6.9087246720879545,
+            "y": 7.196130377471773,
+            "heading": 2.9741502939674576e-17,
+            "angularVelocity": -1.7348157482865667e-17,
+            "velocityX": 3.5692258445423475,
+            "velocityY": 1.2535015995473786,
+            "timestamp": 1.4852148668647889
+        },
+        {
+            "x": 7.217234786194527,
+            "y": 7.304478220492258,
+            "heading": 1.2383304928032073e-17,
+            "angularVelocity": -1.753099667759744e-16,
+            "velocityX": 3.115812947231872,
+            "velocityY": 1.0942643260352094,
+            "timestamp": 1.5842291913224416
+        },
+        {
+            "x": 7.448617388685645,
+            "y": 7.385739108696781,
+            "heading": -3.803725574168796e-18,
+            "angularVelocity": -1.6348170333424006e-16,
+            "velocityX": 2.336859881219295,
+            "velocityY": 0.8206983045092451,
+            "timestamp": 1.6832435157800942
+        },
+        {
+            "x": 7.602872461914855,
+            "y": 7.439913035887961,
+            "heading": -2.4779119751079125e-18,
+            "angularVelocity": 1.3390118812558602e-17,
+            "velocityX": 1.557906636985473,
+            "velocityY": 0.5471322203925049,
+            "timestamp": 1.782257840237747
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 1.0919443501059542e-30,
-            "angularVelocity": -0.01231451219942279,
-            "velocityX": 0.781041332195102,
-            "velocityY": 0.26759551765904444,
-            "timestamp": 1.8813247742176815
+            "heading": 0,
+            "angularVelocity": 2.5025792615041696e-17,
+            "velocityX": 0.7789533333445218,
+            "velocityY": 0.27356611541215564,
+            "timestamp": 1.8812721646953996
         },
         {
             "x": 7.68,
             "y": 7.467,
             "heading": 0,
-            "angularVelocity": -8.461898211343436e-30,
-            "velocityX": 0,
-            "velocityY": -1.0908466908101513e-31,
-            "timestamp": 1.9803418675975595
+            "angularVelocity": 0,
+            "velocityX": 1.3533974507170236e-34,
+            "velocityY": 0,
+            "timestamp": 1.9802864891530523
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.traj
index 20e2615d..70287ec9 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.traj	
@@ -4,190 +4,190 @@
             "x": 2.25,
             "y": 5.56,
             "heading": 0,
-            "angularVelocity": -9.278807284273418e-29,
-            "velocityX": -7.385267906050254e-32,
-            "velocityY": -1.1558173496237395e-34,
+            "angularVelocity": 0,
+            "velocityX": -3.731399929679607e-32,
+            "velocityY": 5.877471754111438e-38,
             "timestamp": 0
         },
         {
-            "x": 2.3268918247049313,
-            "y": 5.587760380936702,
-            "heading": 0.001219352926916483,
-            "angularVelocity": 0.01231456999280599,
-            "velocityX": 0.776551018417967,
-            "velocityY": 0.2803594812685564,
-            "timestamp": 0.09901709337987798
-        },
-        {
-            "x": 2.480675474257635,
-            "y": 5.643281133231001,
-            "heading": 0.0036580544911558616,
-            "angularVelocity": 0.024629096663863686,
-            "velocityX": 1.5531020382785252,
-            "velocityY": 0.5607188657951702,
-            "timestamp": 0.19803418675975595
-        },
-        {
-            "x": 2.7113509459548553,
-            "y": 5.726562245995426,
-            "heading": 0.007316098440537021,
-            "angularVelocity": 0.03694356019247884,
-            "velocityX": 2.3296530308381804,
-            "velocityY": 0.8410781403663149,
-            "timestamp": 0.29705128013963394
-        },
-        {
-            "x": 3.0189182254277274,
-            "y": 5.837603703876164,
-            "heading": 0.01219347400929413,
-            "angularVelocity": 0.04925791499475694,
-            "velocityX": 3.1062038782828476,
-            "velocityY": 1.1214372598752256,
-            "timestamp": 0.3960683735195119
-        },
-        {
-            "x": 3.3712359540410324,
-            "y": 5.964801318197582,
-            "heading": 0.012193474535478832,
-            "angularVelocity": 5.31407952800269e-9,
-            "velocityX": 3.5581505837748795,
-            "velocityY": 1.2846025870848905,
-            "timestamp": 0.49508546689938987
-        },
-        {
-            "x": 3.723553713186701,
-            "y": 6.0919988479491165,
-            "heading": 0.012193475061665435,
-            "angularVelocity": 5.314098710197071e-9,
-            "velocityX": 3.5581508921293548,
-            "velocityY": 1.2846017329911394,
-            "timestamp": 0.5941025602792679
-        },
-        {
-            "x": 4.075871503881281,
-            "y": 6.219196290314954,
-            "heading": 0.012193475587862622,
-            "angularVelocity": 5.314205602691753e-9,
-            "velocityX": 3.558151210750214,
-            "velocityY": 1.2846008504596915,
-            "timestamp": 0.6931196536591459
-        },
-        {
-            "x": 4.428189328839488,
-            "y": 6.346393637775665,
-            "heading": 0.012193476114071436,
-            "angularVelocity": 5.3143230401144125e-9,
-            "velocityX": 3.5581515567877036,
-            "velocityY": 1.2845998919875412,
-            "timestamp": 0.7921367470390238
-        },
-        {
-            "x": 4.780507205251625,
-            "y": 6.473590842716609,
-            "heading": 0.012193476640289547,
-            "angularVelocity": 5.314416922842863e-9,
-            "velocityX": 3.5581520764346486,
-            "velocityY": 1.2845984526424705,
-            "timestamp": 0.8911538404189018
-        },
-        {
-            "x": 5.134861642335281,
-            "y": 6.594999030315895,
-            "heading": 0.012193476116517958,
-            "angularVelocity": -5.289708644023114e-9,
-            "velocityX": 3.5787198451098035,
-            "velocityY": 1.2261336245600862,
-            "timestamp": 0.9901709337987797
-        },
-        {
-            "x": 5.489216369156675,
-            "y": 6.716406372237666,
-            "heading": 0.012193475592327948,
-            "angularVelocity": -5.293934612313601e-9,
-            "velocityX": 3.578722771248356,
-            "velocityY": 1.2261250838377307,
-            "timestamp": 1.0891880271786578
-        },
-        {
-            "x": 5.843571141725175,
-            "y": 6.837813580636051,
-            "heading": 0.012193475068127738,
-            "angularVelocity": -5.2940376086876394e-9,
-            "velocityX": 3.5787232332605625,
-            "velocityY": 1.2261237353494956,
-            "timestamp": 1.1882051205585358
-        },
-        {
-            "x": 6.1979259496904,
-            "y": 6.959220685720912,
-            "heading": 0.012193474543942472,
-            "angularVelocity": -5.293886679384445e-9,
-            "velocityX": 3.5787235907415074,
-            "velocityY": 1.2261226919587003,
-            "timestamp": 1.2872222139384137
-        },
-        {
-            "x": 6.552280784920285,
-            "y": 7.080627711227486,
-            "heading": 0.012193474019771769,
-            "angularVelocity": -5.293739610148028e-9,
-            "velocityX": 3.5787238660945766,
-            "velocityY": 1.2261218882763663,
-            "timestamp": 1.3862393073182917
-        },
-        {
-            "x": 6.906635640601096,
-            "y": 7.202034677043121,
-            "heading": 0.01219347349560639,
-            "angularVelocity": -5.293685828527132e-9,
-            "velocityX": 3.5787240726339373,
-            "velocityY": 1.2261212854416952,
-            "timestamp": 1.4852564006981697
-        },
-        {
-            "x": 7.215981364626797,
-            "y": 7.3080208081486955,
-            "heading": 0.0073160851772923075,
-            "angularVelocity": -0.0492580437559288,
-            "velocityX": 3.124164863524117,
-            "velocityY": 1.0703821682480574,
-            "timestamp": 1.5842734940780476
-        },
-        {
-            "x": 7.447990676616371,
-            "y": 7.387510406301363,
-            "heading": 0.003658042213644346,
-            "angularVelocity": -0.03694355023729677,
-            "velocityX": 2.343123839229173,
-            "velocityY": 0.8027866244033968,
-            "timestamp": 1.6832905874579256
-        },
-        {
-            "x": 7.602663557476493,
-            "y": 7.440503469639918,
-            "heading": 0.0012193472043836597,
-            "angularVelocity": -0.024629030463387942,
-            "velocityX": 1.5620826221056736,
-            "velocityY": 0.5351910617619017,
-            "timestamp": 1.7823076808378036
+            "x": 2.3271275380851444,
+            "y": 5.587086964112038,
+            "heading": 3.953847862678034e-18,
+            "angularVelocity": 3.9932078666702264e-17,
+            "velocityX": 0.7789533333445218,
+            "velocityY": 0.2735661154121556,
+            "timestamp": 0.09901432445765257
+        },
+        {
+            "x": 2.4813826113143547,
+            "y": 5.641260891303218,
+            "heading": 1.1398162294527873e-19,
+            "angularVelocity": -3.8780916638560175e-17,
+            "velocityX": 1.557906636985473,
+            "velocityY": 0.5471322203925046,
+            "timestamp": 0.19802864891530514
+        },
+        {
+            "x": 2.7127652138054734,
+            "y": 5.722521779507741,
+            "heading": 2.1553394674505063e-18,
+            "angularVelocity": 2.0616792512846884e-17,
+            "velocityX": 2.336859881219295,
+            "velocityY": 0.8206983045092451,
+            "timestamp": 0.2970429733729577
+        },
+        {
+            "x": 3.0212753279120443,
+            "y": 5.830869622528225,
+            "heading": -1.0672476097054788e-17,
+            "angularVelocity": -1.295551486240031e-16,
+            "velocityX": 3.1158129472318716,
+            "velocityY": 1.0942643260352094,
+            "timestamp": 0.3960572978306103
+        },
+        {
+            "x": 3.3746798137461993,
+            "y": 5.9549842366139965,
+            "heading": 1.8895880094915182e-18,
+            "angularVelocity": 1.2687117924961395e-16,
+            "velocityX": 3.5692258445423475,
+            "velocityY": 1.2535015995473786,
+            "timestamp": 0.49507162228826285
+        },
+        {
+            "x": 3.7280842995803773,
+            "y": 6.079098850699775,
+            "heading": 1.0049319654091473e-17,
+            "angularVelocity": 8.240960778697987e-17,
+            "velocityX": 3.569225844542579,
+            "velocityY": 1.25350159954746,
+            "timestamp": 0.5940859467459154
+        },
+        {
+            "x": 4.081488785414555,
+            "y": 6.203213464785553,
+            "heading": 8.136834826577652e-18,
+            "angularVelocity": -1.931523381075779e-17,
+            "velocityX": 3.569225844542579,
+            "velocityY": 1.25350159954746,
+            "timestamp": 0.693100271203568
+        },
+        {
+            "x": 4.434893271248733,
+            "y": 6.327328078871332,
+            "heading": -2.4381204969164037e-18,
+            "angularVelocity": -1.0680227715899811e-16,
+            "velocityX": 3.5692258445425793,
+            "velocityY": 1.2535015995474599,
+            "timestamp": 0.7921145956612206
+        },
+        {
+            "x": 4.788297757082911,
+            "y": 6.4514426929571105,
+            "heading": 3.192038150785043e-17,
+            "angularVelocity": 3.47005367081625e-16,
+            "velocityX": 3.569225844542579,
+            "velocityY": 1.25350159954746,
+            "timestamp": 0.8911289201188731
+        },
+        {
+            "x": 5.141702242917089,
+            "y": 6.575557307042889,
+            "heading": 6.016026907559514e-17,
+            "angularVelocity": 2.8521012209839933e-16,
+            "velocityX": 3.569225844542579,
+            "velocityY": 1.2535015995474599,
+            "timestamp": 0.9901432445765257
+        },
+        {
+            "x": 5.495106728751266,
+            "y": 6.699671921128667,
+            "heading": 2.4186102644429564e-17,
+            "angularVelocity": -3.63322848771759e-16,
+            "velocityX": 3.569225844542579,
+            "velocityY": 1.2535015995474599,
+            "timestamp": 1.0891575690341782
+        },
+        {
+            "x": 5.848511214585444,
+            "y": 6.823786535214446,
+            "heading": -1.2421929874703125e-18,
+            "angularVelocity": -2.568143121812034e-16,
+            "velocityX": 3.5692258445425793,
+            "velocityY": 1.2535015995474599,
+            "timestamp": 1.1881718934918308
+        },
+        {
+            "x": 6.2019157004196215,
+            "y": 6.9479011493002245,
+            "heading": 2.3184212364315377e-18,
+            "angularVelocity": 3.596059704901295e-17,
+            "velocityX": 3.569225844542579,
+            "velocityY": 1.2535015995474599,
+            "timestamp": 1.2871862179494835
+        },
+        {
+            "x": 6.5553201862538,
+            "y": 7.072015763386003,
+            "heading": 3.1459219033425486e-17,
+            "angularVelocity": 2.9430890890068574e-16,
+            "velocityX": 3.569225844542579,
+            "velocityY": 1.25350159954746,
+            "timestamp": 1.3862005424071362
+        },
+        {
+            "x": 6.9087246720879545,
+            "y": 7.196130377471773,
+            "heading": 2.9741502939674576e-17,
+            "angularVelocity": -1.7348157482865667e-17,
+            "velocityX": 3.5692258445423475,
+            "velocityY": 1.2535015995473786,
+            "timestamp": 1.4852148668647889
+        },
+        {
+            "x": 7.217234786194527,
+            "y": 7.304478220492258,
+            "heading": 1.2383304928032073e-17,
+            "angularVelocity": -1.753099667759744e-16,
+            "velocityX": 3.115812947231872,
+            "velocityY": 1.0942643260352094,
+            "timestamp": 1.5842291913224416
+        },
+        {
+            "x": 7.448617388685645,
+            "y": 7.385739108696781,
+            "heading": -3.803725574168796e-18,
+            "angularVelocity": -1.6348170333424006e-16,
+            "velocityX": 2.336859881219295,
+            "velocityY": 0.8206983045092451,
+            "timestamp": 1.6832435157800942
+        },
+        {
+            "x": 7.602872461914855,
+            "y": 7.439913035887961,
+            "heading": -2.4779119751079125e-18,
+            "angularVelocity": 1.3390118812558602e-17,
+            "velocityX": 1.557906636985473,
+            "velocityY": 0.5471322203925049,
+            "timestamp": 1.782257840237747
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 1.0919443501059542e-30,
-            "angularVelocity": -0.01231451219942279,
-            "velocityX": 0.781041332195102,
-            "velocityY": 0.26759551765904444,
-            "timestamp": 1.8813247742176815
+            "heading": 0,
+            "angularVelocity": 2.5025792615041696e-17,
+            "velocityX": 0.7789533333445218,
+            "velocityY": 0.27356611541215564,
+            "timestamp": 1.8812721646953996
         },
         {
             "x": 7.68,
             "y": 7.467,
             "heading": 0,
-            "angularVelocity": -8.461898211343436e-30,
-            "velocityX": 0,
-            "velocityY": -1.0908466908101513e-31,
-            "timestamp": 1.9803418675975595
+            "angularVelocity": 0,
+            "velocityX": 1.3533974507170236e-34,
+            "velocityY": 0,
+            "timestamp": 1.9802864891530523
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.1.traj
index e1aa1d32..168ae5bd 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.1.traj	
@@ -6,521 +6,521 @@
             "heading": 0,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": -1.3324658216077414e-32,
+            "velocityY": -1.058013890027075e-32,
             "timestamp": 0
         },
         {
-            "x": 2.2604047390881647,
-            "y": 5.564834315872384,
-            "heading": -0.00020682184904158323,
-            "angularVelocity": -0.005575579259585174,
-            "velocityX": 0.2804947723375417,
-            "velocityY": 0.13032526030129518,
-            "timestamp": 0.037094235309470626
-        },
-        {
-            "x": 2.2812142175691745,
-            "y": 5.574502946267583,
-            "heading": -0.0006204643713099006,
-            "angularVelocity": -0.011151126821122416,
-            "velocityX": 0.5609895528887481,
-            "velocityY": 0.2606504842204101,
-            "timestamp": 0.07418847061894125
-        },
-        {
-            "x": 2.3124284356981284,
-            "y": 5.589005889804122,
-            "heading": -0.0012409261115257607,
-            "angularVelocity": -0.01672663515070949,
-            "velocityX": 0.8414843403169981,
-            "velocityY": 0.39097567089719987,
-            "timestamp": 0.11128270592841188
-        },
-        {
-            "x": 2.3540473936632536,
-            "y": 5.608343145059642,
-            "heading": -0.0020682052367836136,
-            "angularVelocity": -0.022302094068156527,
-            "velocityX": 1.121979132819583,
-            "velocityY": 0.5213008192295232,
-            "timestamp": 0.1483769412378825
-        },
-        {
-            "x": 2.4060710915596104,
-            "y": 5.632514710558234,
-            "heading": -0.0031022993856005916,
-            "angularVelocity": -0.027877489323882353,
-            "velocityX": 1.402473927884819,
-            "velocityY": 0.6516259277737647,
-            "timestamp": 0.18547117654735312
-        },
-        {
-            "x": 2.46849952934744,
-            "y": 5.661520584751242,
-            "heading": -0.004343205426988983,
-            "angularVelocity": -0.03345280017329315,
-            "velocityX": 1.6829687218782206,
-            "velocityY": 0.781950994568762,
-            "timestamp": 0.22256541185682374
-        },
-        {
-            "x": 2.5413327067825313,
-            "y": 5.695360765985745,
-            "heading": -0.005790919055957113,
-            "angularVelocity": -0.039027994967159495,
-            "velocityX": 1.9634635092880914,
-            "velocityY": 0.9122760168036823,
-            "timestamp": 0.25965964716629436
-        },
-        {
-            "x": 2.6245706232908623,
-            "y": 5.734035252447861,
-            "heading": -0.007445434065373688,
-            "angularVelocity": -0.044603022426903935,
-            "velocityX": 2.2439582812232635,
-            "velocityY": 1.042600990139337,
-            "timestamp": 0.296753882475765
-        },
-        {
-            "x": 2.7182132777182155,
-            "y": 5.777544042048614,
-            "heading": -0.009306740898831883,
-            "angularVelocity": -0.05017779226152841,
-            "velocityX": 2.5244530220426173,
-            "velocityY": 1.1729259071596103,
-            "timestamp": 0.33384811778523565
-        },
-        {
-            "x": 2.8222606677468502,
-            "y": 5.825887132155531,
-            "heading": -0.011374823315541767,
-            "angularVelocity": -0.05575212427098156,
-            "velocityX": 2.804947700379475,
-            "velocityY": 1.3032507532126736,
-            "timestamp": 0.3709423530947063
-        },
-        {
-            "x": 2.9367127881472834,
-            "y": 5.879064518782887,
-            "heading": -0.013649648601341033,
-            "angularVelocity": -0.0613255743600492,
-            "velocityX": 3.0854422377380093,
-            "velocityY": 1.4335754918171768,
-            "timestamp": 0.40803658840417695
-        },
-        {
-            "x": 3.061569622038521,
-            "y": 5.9370761925311655,
-            "heading": -0.016131121234305167,
-            "angularVelocity": -0.06689644933404303,
-            "velocityX": 3.365936320012513,
-            "velocityY": 1.563899977025988,
-            "timestamp": 0.4451308237136476
-        },
-        {
-            "x": 3.188829425192231,
-            "y": 5.996204205741329,
-            "heading": -0.01613112189285862,
-            "angularVelocity": -1.7753525563113908e-8,
-            "velocityX": 3.430716446693241,
-            "velocityY": 1.5939946656635118,
-            "timestamp": 0.48222505902311824
-        },
-        {
-            "x": 3.3160892428339666,
-            "y": 6.055332187769375,
-            "heading": -0.01613112255138683,
-            "angularVelocity": -1.775284497104555e-8,
-            "velocityX": 3.4307168372667594,
-            "velocityY": 1.5939938250445522,
-            "timestamp": 0.5193192943325888
-        },
-        {
-            "x": 3.443349075020492,
-            "y": 6.114460138492994,
-            "heading": -0.016131123209912376,
-            "angularVelocity": -1.775277322447583e-8,
-            "velocityX": 3.43071722937053,
-            "velocityY": 1.593992981128321,
-            "timestamp": 0.5564135296420595
-        },
-        {
-            "x": 3.5706089218502877,
-            "y": 6.173588057700207,
-            "heading": -0.01613112386843302,
-            "angularVelocity": -1.775264097704561e-8,
-            "velocityX": 3.430717624129179,
-            "velocityY": 1.5939921314975005,
-            "timestamp": 0.5935077649515301
-        },
-        {
-            "x": 3.697868783477107,
-            "y": 6.2327159450600735,
-            "heading": -0.016131124526946636,
-            "angularVelocity": -1.7752451586110053e-8,
-            "velocityX": 3.430718023032762,
-            "velocityY": 1.5939912729450545,
-            "timestamp": 0.6306020002610008
-        },
-        {
-            "x": 3.8251286601281493,
-            "y": 6.291843800083575,
-            "heading": -0.016131125185451407,
-            "angularVelocity": -1.7752213148650806e-8,
-            "velocityX": 3.4307184280612866,
-            "velocityY": 1.5939904012094348,
-            "timestamp": 0.6676962355704714
-        },
-        {
-            "x": 3.95238855212494,
-            "y": 6.350971622078674,
-            "heading": -0.016131125843945708,
-            "angularVelocity": -1.775193091205886e-8,
-            "velocityX": 3.430718841757593,
-            "velocityY": 1.5939895108176958,
-            "timestamp": 0.7047904708799421
-        },
-        {
-            "x": 4.079648459881884,
-            "y": 6.410099410153433,
-            "heading": -0.01613112650242917,
-            "angularVelocity": -1.7751638714463086e-8,
-            "velocityX": 3.430719266625584,
-            "velocityY": 1.5939885963806835,
-            "timestamp": 0.7418847061894127
-        },
-        {
-            "x": 4.2069083839232695,
-            "y": 6.469227163179404,
-            "heading": -0.01613112716090301,
-            "angularVelocity": -1.7751379248608663e-8,
-            "velocityX": 3.430719705627546,
-            "velocityY": 1.5939876515226545,
-            "timestamp": 0.7789789414988834
-        },
-        {
-            "x": 4.334168324938812,
-            "y": 6.528354879672101,
-            "heading": -0.016131127819368877,
-            "angularVelocity": -1.7751164242273923e-8,
-            "velocityX": 3.430720163223074,
-            "velocityY": 1.5939866666451934,
-            "timestamp": 0.816073176808354
-        },
-        {
-            "x": 4.461428283794271,
-            "y": 6.58748255776812,
-            "heading": -0.01613112847782922,
-            "angularVelocity": -1.775101542628313e-8,
-            "velocityX": 3.430720644158123,
-            "velocityY": 1.5939856315335348,
-            "timestamp": 0.8531674121178247
-        },
-        {
-            "x": 4.58868833054201,
-            "y": 6.646610046694028,
-            "heading": -0.016131129136287645,
-            "angularVelocity": -1.7750963701844846e-8,
-            "velocityX": 3.430723013590394,
-            "velocityY": 1.5939805318162525,
-            "timestamp": 0.8902616474272953
-        },
-        {
-            "x": 4.716960790002456,
-            "y": 6.703507810971908,
-            "heading": -0.016131129795891656,
-            "angularVelocity": -1.7781846861521853e-8,
-            "velocityX": 3.4580160068077612,
-            "velocityY": 1.5338707969903813,
-            "timestamp": 0.927355882736766
-        },
-        {
-            "x": 4.849451901724698,
-            "y": 6.749738586923234,
-            "heading": -0.016131130468276335,
-            "angularVelocity": -1.8126392792981438e-8,
-            "velocityX": 3.5717439816966663,
-            "velocityY": 1.2463062135027452,
-            "timestamp": 0.9644501180462366
-        },
-        {
-            "x": 4.985276799458285,
-            "y": 6.784991618369029,
-            "heading": -0.016131131172819703,
-            "angularVelocity": -1.899333848953808e-8,
-            "velocityX": 3.661617407676018,
-            "velocityY": 0.950364150970728,
-            "timestamp": 1.0015443533557071
-        },
-        {
-            "x": 5.12352760039525,
-            "y": 6.80903105836077,
-            "heading": -0.01613113193292308,
-            "angularVelocity": -2.0491145599848975e-8,
-            "velocityX": 3.7270157959468126,
-            "velocityY": 0.6480640399022476,
-            "timestamp": 1.0386385886651777
-        },
-        {
-            "x": 5.263280144021708,
-            "y": 6.821696149590996,
-            "heading": -0.01613113277968929,
-            "angularVelocity": -2.2827434072857776e-8,
-            "velocityX": 3.767500326143083,
-            "velocityY": 0.341430174380559,
-            "timestamp": 1.0757328239746482
+            "x": 2.26040474690827,
+            "y": 5.564834298284281,
+            "heading": -0.00020682462174428096,
+            "angularVelocity": -0.005575654083684234,
+            "velocityX": 0.28049498700644215,
+            "velocityY": 0.1303247879443207,
+            "timestamp": 0.037094234800108185
+        },
+        {
+            "x": 2.2812142405469196,
+            "y": 5.574502894544252,
+            "heading": -0.0006204719876615776,
+            "angularVelocity": -0.011151257551113,
+            "velocityX": 0.5609899692172368,
+            "velocityY": 0.26064956756953994,
+            "timestamp": 0.07418846960021637
+        },
+        {
+            "x": 2.312428480686622,
+            "y": 5.589005788443611,
+            "heading": -0.0012409398807264826,
+            "angularVelocity": -0.016726801251156914,
+            "velocityX": 0.8414849452457597,
+            "velocityY": 0.39097433812857635,
+            "timestamp": 0.11128270440032456
+        },
+        {
+            "x": 2.3540474670292637,
+            "y": 5.608342979609529,
+            "heading": -0.002068225636714691,
+            "angularVelocity": -0.022302273128063925,
+            "velocityX": 1.1219799132376411,
+            "velocityY": 0.5212990986367875,
+            "timestamp": 0.14837693920043274
+        },
+        {
+            "x": 2.406071199181798,
+            "y": 5.632514467619691,
+            "heading": -0.0031023259847413323,
+            "angularVelocity": -0.02787765682725579,
+            "velocityX": 1.4024748706336019,
+            "velocityY": 0.6516238477600773,
+            "timestamp": 0.18547117400054092
+        },
+        {
+            "x": 2.4684996766150022,
+            "y": 5.66152025198224,
+            "heading": -0.004343236799506005,
+            "angularVelocity": -0.03345292931508493,
+            "velocityX": 1.682969813762594,
+            "velocityY": 0.7819485836236739,
+            "timestamp": 0.2225654088006491
+        },
+        {
+            "x": 2.5413328985945514,
+            "y": 5.69536033210275,
+            "heading": -0.005790952692459236,
+            "angularVelocity": -0.03902805653641911,
+            "velocityX": 1.9634647370954057,
+            "velocityY": 0.9122733034625292,
+            "timestamp": 0.2596596436007573
+        },
+        {
+            "x": 2.6245708640566887,
+            "y": 5.7340347072253275,
+            "heading": -0.00744546628275827,
+            "angularVelocity": -0.04460298478228464,
+            "velocityX": 2.2439596317510366,
+            "velocityY": 1.0425980029237354,
+            "timestamp": 0.2967538784008655
+        },
+        {
+            "x": 2.7182135713591755,
+            "y": 5.777543376315584,
+            "heading": -0.009306766753500902,
+            "angularVelocity": -0.05017762142212937,
+            "velocityX": 2.5244544821346007,
+            "velocityY": 1.1729226744995158,
+            "timestamp": 0.3338481132009737
+        },
+        {
+            "x": 2.8222610176995655,
+            "y": 5.8258863377887575,
+            "heading": -0.011374836524793099,
+            "angularVelocity": -0.055751784136736884,
+            "velocityX": 2.8049492569688192,
+            "velocityY": 1.303247303352706,
+            "timestamp": 0.3709423480010819
+        },
+        {
+            "x": 2.9367131973687606,
+            "y": 5.879063588696104,
+            "heading": -0.013649641477510719,
+            "angularVelocity": -0.061325027055469855,
+            "velocityX": 3.0854438778950355,
+            "velocityY": 1.4335718527125696,
+            "timestamp": 0.4080365828011901
+        },
+        {
+            "x": 3.06157009301301,
+            "y": 5.937075120660461,
+            "heading": -0.01613108463550858,
+            "angularVelocity": -0.06689565565567898,
+            "velocityX": 3.3659380309924987,
+            "velocityY": 1.5638961762378367,
+            "timestamp": 0.4451308176012983
+        },
+        {
+            "x": 3.1888299168589427,
+            "y": 5.996203084762226,
+            "heading": -0.01613108529402469,
+            "angularVelocity": -1.7752519083825314e-8,
+            "velocityX": 3.430717051630934,
+            "velocityY": 1.5939933636693306,
+            "timestamp": 0.4822250524014065
+        },
+        {
+            "x": 3.316089744181463,
+            "y": 6.055331041381533,
+            "heading": -0.016131085952509703,
+            "angularVelocity": -1.7751680763659775e-8,
+            "velocityX": 3.4307171453540595,
+            "velocityY": 1.5939931619544851,
+            "timestamp": 0.5193192872015147
+        },
+        {
+            "x": 3.443349575355442,
+            "y": 6.114458989711434,
+            "heading": -0.01613108661098856,
+            "angularVelocity": -1.7751514817327164e-8,
+            "velocityX": 3.430717249183109,
+            "velocityY": 1.5939929384856546,
+            "timestamp": 0.5564135220016229
+        },
+        {
+            "x": 3.5706094108109987,
+            "y": 6.173586928826195,
+            "heading": -0.016131087269461084,
+            "angularVelocity": -1.7751344035751635e-8,
+            "velocityX": 3.4307173646074394,
+            "velocityY": 1.5939926900605101,
+            "timestamp": 0.5935077568017311
+        },
+        {
+            "x": 3.6978692510432434,
+            "y": 6.232714857660196,
+            "heading": -0.01613108792792704,
+            "angularVelocity": -1.7751166882414398e-8,
+            "velocityX": 3.4307174933791567,
+            "velocityY": 1.5939924129080143,
+            "timestamp": 0.6306019916018393
+        },
+        {
+            "x": 3.825129096624906,
+            "y": 6.291842774980762,
+            "heading": -0.016131088586386323,
+            "angularVelocity": -1.775098711225461e-8,
+            "velocityX": 3.430717637590709,
+            "velocityY": 1.5939921025246122,
+            "timestamp": 0.6676962264019475
+        },
+        {
+            "x": 3.952388948222243,
+            "y": 6.3509706793539165,
+            "heading": -0.016131089244838845,
+            "angularVelocity": -1.775080474504649e-8,
+            "velocityX": 3.4307177997634843,
+            "velocityY": 1.5939917534835466,
+            "timestamp": 0.7047904612020557
+        },
+        {
+            "x": 4.07964880661482,
+            "y": 6.410098569101814,
+            "heading": -0.016131089903284688,
+            "angularVelocity": -1.775062484471552e-8,
+            "velocityX": 3.430717982952055,
+            "velocityY": 1.5939913592104822,
+            "timestamp": 0.7418846960021639
+        },
+        {
+            "x": 4.206908672718448,
+            "y": 6.469226442253365,
+            "heading": -0.016131090561724272,
+            "angularVelocity": -1.7750456035932977e-8,
+            "velocityX": 3.4307181908294035,
+            "velocityY": 1.5939909118000826,
+            "timestamp": 0.7789789308022721
+        },
+        {
+            "x": 4.33416854760619,
+            "y": 6.52835429649903,
+            "heading": -0.016131091220158388,
+            "angularVelocity": -1.775030869099235e-8,
+            "velocityX": 3.43071842763476,
+            "velocityY": 1.593990402128264,
+            "timestamp": 0.8160731656023803
+        },
+        {
+            "x": 4.461428432515134,
+            "y": 6.587482129176235,
+            "heading": -0.016131091878588796,
+            "angularVelocity": -1.7750208582663698e-8,
+            "velocityX": 3.430718697790006,
+            "velocityY": 1.5939898206778003,
+            "timestamp": 0.8531674004024885
+        },
+        {
+            "x": 4.588688397859823,
+            "y": 6.646609788732575,
+            "heading": -0.01613109253701839,
+            "angularVelocity": -1.7750186861935105e-8,
+            "velocityX": 3.4307208662063466,
+            "velocityY": 1.5939851536219738,
+            "timestamp": 0.8902616352025967
+        },
+        {
+            "x": 4.7169608374986005,
+            "y": 6.703507592945478,
+            "heading": -0.016131093196594153,
+            "angularVelocity": -1.7781085471466767e-8,
+            "velocityX": 3.4580155199319775,
+            "velocityY": 1.533871894635887,
+            "timestamp": 0.9273558700027049
+        },
+        {
+            "x": 4.84945193271468,
+            "y": 6.749738410352506,
+            "heading": -0.016131093868950192,
+            "angularVelocity": -1.8125621008136676e-8,
+            "velocityX": 3.571743585763167,
+            "velocityY": 1.2463073481945173,
+            "timestamp": 0.9644501048028131
+        },
+        {
+            "x": 4.985276817345946,
+            "y": 6.784991484609654,
+            "heading": -0.0161310945734632,
+            "angularVelocity": -1.8992520332334574e-8,
+            "velocityX": 3.661617104738627,
+            "velocityY": 0.9503653181449264,
+            "timestamp": 1.0015443396029213
+        },
+        {
+            "x": 5.123527608694991,
+            "y": 6.809030968493646,
+            "heading": -0.01613109533353302,
+            "angularVelocity": -2.0490241268499195e-8,
+            "velocityX": 3.7270155886499414,
+            "velocityY": 0.6480652320645928,
+            "timestamp": 1.0386385744030295
+        },
+        {
+            "x": 5.263280146337045,
+            "y": 6.821696104409015,
+            "heading": -0.016131096180260384,
+            "angularVelocity": -2.2826387114294202e-8,
+            "velocityX": 3.767500216547063,
+            "velocityY": 0.34143138370740883,
+            "timestamp": 1.0757328092031377
         },
         {
             "x": 5.403600215911865,
             "y": 6.822902202606201,
-            "heading": -0.016131133746573465,
-            "angularVelocity": -2.6065618224331403e-8,
-            "velocityX": 3.782799961220162,
-            "velocityY": 0.03251321951088522,
-            "timestamp": 1.1128270592841187
-        },
-        {
-            "x": 5.530811600424101,
-            "y": 6.814532053807006,
-            "heading": -0.016131134641902804,
-            "angularVelocity": -2.6567346760379892e-8,
-            "velocityX": 3.7747774237594087,
-            "velocityY": -0.24836966315439601,
-            "timestamp": 1.1465274262810004
-        },
-        {
-            "x": 5.6570507349000225,
-            "y": 6.796742242248266,
-            "heading": -0.016131135427811184,
-            "angularVelocity": -2.332046941486525e-8,
-            "velocityX": 3.7459275884919543,
-            "velocityY": -0.5278818346513846,
-            "timestamp": 1.180227793277882
-        },
-        {
-            "x": 5.781621094465109,
-            "y": 6.769630943607204,
-            "heading": -0.01613113613886582,
-            "angularVelocity": -2.1099314246183972e-8,
-            "velocityX": 3.696409584400467,
-            "velocityY": -0.8044808130300218,
-            "timestamp": 1.2139281602747636
-        },
-        {
-            "x": 5.903835372739644,
-            "y": 6.733347785640362,
-            "heading": -0.016131136799649604,
-            "angularVelocity": -1.9607613962388268e-8,
-            "velocityX": 3.6264969543460657,
-            "velocityY": -1.0766398469844052,
-            "timestamp": 1.2476285272716452
-        },
-        {
-            "x": 6.023019298132563,
-            "y": 6.68809306728661,
-            "heading": -0.01613113742890953,
-            "angularVelocity": -1.8672198003746715e-8,
-            "velocityX": 3.536576483097275,
-            "velocityY": -1.3428553569739705,
-            "timestamp": 1.2813288942685268
-        },
-        {
-            "x": 6.13851552436941,
-            "y": 6.634116966515058,
-            "heading": -0.016131138042081584,
-            "angularVelocity": -1.819481831325098e-8,
-            "velocityX": 3.4271504000990243,
-            "velocityY": -1.601647269198795,
-            "timestamp": 1.3150292612654084
-        },
-        {
-            "x": 6.251225914876305,
-            "y": 6.574542092363786,
-            "heading": -0.016131138651474624,
-            "angularVelocity": -1.808268258780769e-8,
-            "velocityX": 3.344485551665478,
-            "velocityY": -1.7677811685785219,
-            "timestamp": 1.34872962826229
-        },
-        {
-            "x": 6.363935678841957,
-            "y": 6.51496603286697,
-            "heading": -0.016131139260695985,
-            "angularVelocity": -1.807758837477246e-8,
-            "velocityX": 3.344466960139662,
-            "velocityY": -1.7678163416537895,
-            "timestamp": 1.3824299952591717
-        },
-        {
-            "x": 6.475851865782255,
-            "y": 6.4539122462129415,
-            "heading": -0.016131138603390965,
-            "angularVelocity": 1.950438761664722e-8,
-            "velocityX": 3.320918936896271,
-            "velocityY": -1.8116653346735878,
-            "timestamp": 1.4161303622560533
-        },
-        {
-            "x": 6.587767727105175,
-            "y": 6.3928578626815735,
-            "heading": -0.016131137945887282,
-            "angularVelocity": 1.9510282594383425e-8,
-            "velocityX": 3.320909274764734,
-            "velocityY": -1.811683045974467,
-            "timestamp": 1.4498307292529349
-        },
-        {
-            "x": 6.699683536860704,
-            "y": 6.331803384624609,
-            "heading": -0.016131137288379963,
-            "angularVelocity": 1.951039048402864e-8,
-            "velocityX": 3.3209077445917723,
-            "velocityY": -1.811685850857758,
-            "timestamp": 1.4835310962498165
-        },
-        {
-            "x": 6.811599317469464,
-            "y": 6.270748853140253,
-            "heading": -0.0161311366308819,
-            "angularVelocity": 1.951011573787136e-8,
-            "velocityX": 3.320906879711909,
-            "velocityY": -1.811687436223046,
-            "timestamp": 1.517231463246698
-        },
-        {
-            "x": 6.923515076276675,
-            "y": 6.209694281692974,
-            "heading": -0.016131135973346068,
-            "angularVelocity": 1.951123652121546e-8,
-            "velocityX": 3.3209062327887042,
-            "velocityY": -1.8116886220535497,
-            "timestamp": 1.5509318302435797
-        },
-        {
-            "x": 7.031584320380035,
-            "y": 6.150738002364414,
-            "heading": -0.01382673748429001,
-            "angularVelocity": 0.06837903246775291,
-            "velocityX": 3.2067675735804393,
-            "velocityY": -1.7494254390171946,
-            "timestamp": 1.5846321972404613
-        },
-        {
-            "x": 7.1313405673144,
-            "y": 6.0963168023113505,
-            "heading": -0.011699564081675173,
-            "angularVelocity": 0.06312018509503639,
-            "velocityX": 2.9600937860289633,
-            "velocityY": -1.6148548191804175,
-            "timestamp": 1.618332564237343
-        },
-        {
-            "x": 7.222783801332874,
-            "y": 6.046430691545947,
-            "heading": -0.009749641962936324,
-            "angularVelocity": 0.05786056035883372,
-            "velocityX": 2.7134195312156404,
-            "velocityY": -1.480283902250094,
-            "timestamp": 1.6520329312342246
-        },
-        {
-            "x": 7.305914017550448,
-            "y": 6.00107967407256,
-            "heading": -0.007976979919226685,
-            "angularVelocity": 0.05260067476039131,
-            "velocityX": 2.4667451314481808,
-            "velocityY": -1.3457128664973763,
-            "timestamp": 1.6857332982311062
-        },
-        {
-            "x": 7.380731213778514,
-            "y": 5.960263752360125,
-            "heading": -0.006381581900532674,
-            "angularVelocity": 0.047340671952945675,
-            "velocityX": 2.2200706667375205,
-            "velocityY": -1.2111417574833037,
-            "timestamp": 1.7194336652279878
-        },
-        {
-            "x": 7.447235388895749,
-            "y": 5.923982928242947,
-            "heading": -0.004963449854323965,
-            "angularVelocity": 0.042080611357711836,
-            "velocityX": 1.973396168753552,
-            "velocityY": -1.076570594039371,
-            "timestamp": 1.7531340322248694
-        },
-        {
-            "x": 7.505426542307151,
-            "y": 5.892237203224628,
-            "heading": -0.00372258471684395,
-            "angularVelocity": 0.03682052298107744,
-            "velocityX": 1.72672165311393,
-            "velocityY": -0.9419993859786789,
-            "timestamp": 1.786834399221751
-        },
-        {
-            "x": 7.5553046737134695,
-            "y": 5.865026578610625,
-            "heading": -0.002658986847128457,
-            "angularVelocity": 0.03156042395067696,
-            "velocityX": 1.4800471285945758,
-            "velocityY": -0.8074281391808545,
-            "timestamp": 1.8205347662186326
-        },
-        {
-            "x": 7.596869782996782,
-            "y": 5.842351055576096,
-            "heading": -0.0017726562460022867,
-            "angularVelocity": 0.026300324895801946,
-            "velocityX": 1.233372600576068,
-            "velocityY": -0.6728568575121819,
-            "timestamp": 1.8542351332155143
-        },
-        {
-            "x": 7.630121870157635,
-            "y": 5.82421063520489,
-            "heading": -0.0010635926829587166,
-            "angularVelocity": 0.021040232680814546,
-            "velocityX": 0.9866980725738072,
-            "velocityY": -0.5382855436821761,
-            "timestamp": 1.8879355002123959
-        },
-        {
-            "x": 7.655060935277959,
-            "y": 5.8106053185140585,
-            "heading": -0.0005317957806907868,
-            "angularVelocity": 0.015780151661757422,
-            "velocityX": 0.7400235470026807,
-            "velocityY": -0.40371419967297517,
-            "timestamp": 1.9216358672092775
-        },
-        {
-            "x": 7.671686978497787,
-            "y": 5.801535106470048,
-            "heading": -0.00017726506577651584,
-            "angularVelocity": 0.010520084690677335,
-            "velocityX": 0.4933490255867586,
-            "velocityY": -0.2691428269861255,
-            "timestamp": 1.955336234206159
+            "heading": -0.0161310971470982,
+            "angularVelocity": -2.6064368823146806e-8,
+            "velocityX": 3.7827999507462917,
+            "velocityY": 0.032514437989762196,
+            "timestamp": 1.112827044003246
+        },
+        {
+            "x": 5.530811605245593,
+            "y": 6.814532094483614,
+            "heading": -0.016131098042384447,
+            "angularVelocity": -2.6566067685833464e-8,
+            "velocityX": 3.7747775034488793,
+            "velocityY": -0.2483684519763222,
+            "timestamp": 1.1465274115659696
+        },
+        {
+            "x": 5.657050747526861,
+            "y": 6.796742322973506,
+            "heading": -0.016131098828254187,
+            "angularVelocity": -2.3319322572510494e-8,
+            "velocityX": 3.745927757206515,
+            "velocityY": -0.5278806374143462,
+            "timestamp": 1.1802277791286933
+        },
+        {
+            "x": 5.781621117814577,
+            "y": 6.76963106353518,
+            "heading": -0.016131099539274135,
+            "angularVelocity": -2.109828463295381e-8,
+            "velocityX": 3.6964098405118033,
+            "velocityY": -0.8044796362492181,
+            "timestamp": 1.213928146691417
+        },
+        {
+            "x": 5.903835409646495,
+            "y": 6.733347943713289,
+            "heading": -0.016131100200026813,
+            "angularVelocity": -1.96066906061588e-8,
+            "velocityX": 3.626497295747634,
+            "velocityY": -1.0766386970219277,
+            "timestamp": 1.2476285142541406
+        },
+        {
+            "x": 6.023019351333038,
+            "y": 6.68809326224078,
+            "heading": -0.016131100829259,
+            "angularVelocity": -1.86713746487578e-8,
+            "velocityX": 3.5365769072018636,
+            "velocityY": -1.3428542400399708,
+            "timestamp": 1.2813288818168642
+        },
+        {
+            "x": 6.138515596479519,
+            "y": 6.634117196875684,
+            "heading": -0.016131101442406693,
+            "angularVelocity": -1.81940951537302e-8,
+            "velocityX": 3.4271509036665004,
+            "velocityY": -1.601646191681265,
+            "timestamp": 1.3150292493795879
+        },
+        {
+            "x": 6.2512259314426855,
+            "y": 6.5745422130600915,
+            "heading": -0.016131102051778846,
+            "angularVelocity": -1.80820624580199e-8,
+            "velocityX": 3.3444838473464196,
+            "velocityY": -1.7677843929954529,
+            "timestamp": 1.3487296169423115
+        },
+        {
+            "x": 6.363935646775815,
+            "y": 6.514966056976746,
+            "heading": -0.01613110266098242,
+            "angularVelocity": -1.807706019437417e-8,
+            "velocityX": 3.344465460898959,
+            "velocityY": -1.7678191780093255,
+            "timestamp": 1.3824299845050352
+        },
+        {
+            "x": 6.4758517936606514,
+            "y": 6.453912192428435,
+            "heading": -0.016131102003666662,
+            "angularVelocity": 1.950470585863807e-8,
+            "velocityX": 3.320917692560386,
+            "velocityY": -1.811667615632877,
+            "timestamp": 1.4161303520677588
+        },
+        {
+            "x": 6.587767622018337,
+            "y": 6.392857744000494,
+            "heading": -0.016131101346154864,
+            "angularVelocity": 1.951052313957626e-8,
+            "velocityX": 3.32090824081921,
+            "velocityY": -1.8116849412489389,
+            "timestamp": 1.4498307196304825
+        },
+        {
+            "x": 6.699683405881235,
+            "y": 6.331803214011495,
+            "heading": -0.016131100688641712,
+            "angularVelocity": 1.9510563155904532e-8,
+            "velocityX": 3.3209069205135386,
+            "velocityY": -1.811687361432026,
+            "timestamp": 1.4835310871932061
+        },
+        {
+            "x": 6.811599167638959,
+            "y": 6.270748643502715,
+            "heading": -0.016131100031139815,
+            "angularVelocity": 1.9510229240352237e-8,
+            "velocityX": 3.320906264580792,
+            "velocityY": -1.8116885637862563,
+            "timestamp": 1.5172314547559298
+        },
+        {
+            "x": 6.923514914598752,
+            "y": 6.209694045868981,
+            "heading": -0.016131099373601954,
+            "angularVelocity": 1.95112965056638e-8,
+            "velocityX": 3.3209058254778503,
+            "velocityY": -1.8116893686722635,
+            "timestamp": 1.5509318223186535
+        },
+        {
+            "x": 7.031584175346113,
+            "y": 6.1507377884047765,
+            "heading": -0.013826704963615632,
+            "angularVelocity": 0.0683789102803133,
+            "velocityX": 3.2067680136194574,
+            "velocityY": -1.7494247608568367,
+            "timestamp": 1.584632189881377
+        },
+        {
+            "x": 7.131340439149152,
+            "y": 6.096316611291437,
+            "heading": -0.011699535798216875,
+            "angularVelocity": 0.06312005830317652,
+            "velocityX": 2.960094236876553,
+            "velocityY": -1.614854111369865,
+            "timestamp": 1.6183325574441008
+        },
+        {
+            "x": 7.222783690004588,
+            "y": 6.046430524071638,
+            "heading": -0.00974961791861361,
+            "angularVelocity": 0.057860433598327515,
+            "velocityX": 2.713419985263968,
+            "velocityY": -1.4802831787205646,
+            "timestamp": 1.6520329250068244
+        },
+        {
+            "x": 7.305913922772626,
+            "y": 6.001079530283105,
+            "heading": -0.00797695998479322,
+            "angularVelocity": 0.05260055192336173,
+            "velocityX": 2.466745581136908,
+            "velocityY": -1.3457121410953312,
+            "timestamp": 1.685733292569548
+        },
+        {
+            "x": 7.380731135011445,
+            "y": 5.960263631930958,
+            "heading": -0.00638156583911186,
+            "angularVelocity": 0.04734055623313652,
+            "velocityX": 2.2200711045530213,
+            "velocityY": -1.2111410439717167,
+            "timestamp": 1.7194336601322717
+        },
+        {
+            "x": 7.447235325348076,
+            "y": 5.923982830388488,
+            "heading": -0.004963437343393232,
+            "angularVelocity": 0.042080505296519764,
+            "velocityX": 1.9733965872286794,
+            "velocityY": -1.0765699060980205,
+            "timestamp": 1.7531340276949954
+        },
+        {
+            "x": 7.505426492937412,
+            "y": 5.892237126701046,
+            "heading": -0.0037225753686417596,
+            "angularVelocity": 0.0368204285143562,
+            "velocityX": 1.72672204482727,
+            "velocityY": -0.9419987372053683,
+            "timestamp": 1.786834395257719
+        },
+        {
+            "x": 7.555304637231595,
+            "y": 5.8650265217185185,
+            "heading": -0.002658980227466916,
+            "angularVelocity": 0.03156034245604838,
+            "velocityX": 1.4800474861690802,
+            "velocityY": -0.8074275430937743,
+            "timestamp": 1.8205347628204427
+        },
+        {
+            "x": 7.596869757865521,
+            "y": 5.842351016163066,
+            "heading": -0.0017726518914738514,
+            "angularVelocity": 0.02630025724030852,
+            "velocityX": 1.233372916677093,
+            "velocityY": -0.6728563275533557,
+            "timestamp": 1.8542351303831663
+        },
+        {
+            "x": 7.630121854593904,
+            "y": 5.824210610667954,
+            "heading": -0.001063590116536395,
+            "angularVelocity": 0.021040179268589657,
+            "velocityX": 0.9866983399066315,
+            "velocityY": -0.5382850932218669,
+            "timestamp": 1.88793549794589
+        },
+        {
+            "x": 7.655060927254085,
+            "y": 5.810605305801891,
+            "heading": -0.000531794525727047,
+            "angularVelocity": 0.01578011248154109,
+            "velocityX": 0.7400237583095566,
+            "velocityY": -0.4037138420149424,
+            "timestamp": 1.9216358655086136
+        },
+        {
+            "x": 7.671686975742638,
+            "y": 5.8015351020850225,
+            "heading": -0.00017726465841382563,
+            "angularVelocity": 0.010520059362948236,
+            "velocityX": 0.49334917364355607,
+            "velocityY": -0.26914257537363395,
+            "timestamp": 1.9553362330713373
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.986792057163996e-31,
-            "angularVelocity": 0.005260033690221658,
-            "velocityX": 0.24667450959752207,
-            "velocityY": -0.13457142678797143,
-            "timestamp": 1.9890366012030407
+            "heading": 3.6111480760753294e-31,
+            "angularVelocity": 0.005260021514121506,
+            "velocityX": 0.24667458721003926,
+            "velocityY": -0.1345712944104092,
+            "timestamp": 1.989036600634061
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 0,
-            "angularVelocity": -1.0163368054249497e-29,
-            "velocityX": -1.0830525743862323e-33,
-            "velocityY": 5.9205016331337416e-33,
-            "timestamp": 2.022736968199922
+            "heading": -8.452347473573201e-32,
+            "angularVelocity": -1.5098225699671103e-29,
+            "velocityX": 2.1451735090517096e-32,
+            "velocityY": 9.12030277003484e-33,
+            "timestamp": 2.0227369681967846
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.traj
index e1aa1d32..168ae5bd 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.traj	
@@ -6,521 +6,521 @@
             "heading": 0,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": -1.3324658216077414e-32,
+            "velocityY": -1.058013890027075e-32,
             "timestamp": 0
         },
         {
-            "x": 2.2604047390881647,
-            "y": 5.564834315872384,
-            "heading": -0.00020682184904158323,
-            "angularVelocity": -0.005575579259585174,
-            "velocityX": 0.2804947723375417,
-            "velocityY": 0.13032526030129518,
-            "timestamp": 0.037094235309470626
-        },
-        {
-            "x": 2.2812142175691745,
-            "y": 5.574502946267583,
-            "heading": -0.0006204643713099006,
-            "angularVelocity": -0.011151126821122416,
-            "velocityX": 0.5609895528887481,
-            "velocityY": 0.2606504842204101,
-            "timestamp": 0.07418847061894125
-        },
-        {
-            "x": 2.3124284356981284,
-            "y": 5.589005889804122,
-            "heading": -0.0012409261115257607,
-            "angularVelocity": -0.01672663515070949,
-            "velocityX": 0.8414843403169981,
-            "velocityY": 0.39097567089719987,
-            "timestamp": 0.11128270592841188
-        },
-        {
-            "x": 2.3540473936632536,
-            "y": 5.608343145059642,
-            "heading": -0.0020682052367836136,
-            "angularVelocity": -0.022302094068156527,
-            "velocityX": 1.121979132819583,
-            "velocityY": 0.5213008192295232,
-            "timestamp": 0.1483769412378825
-        },
-        {
-            "x": 2.4060710915596104,
-            "y": 5.632514710558234,
-            "heading": -0.0031022993856005916,
-            "angularVelocity": -0.027877489323882353,
-            "velocityX": 1.402473927884819,
-            "velocityY": 0.6516259277737647,
-            "timestamp": 0.18547117654735312
-        },
-        {
-            "x": 2.46849952934744,
-            "y": 5.661520584751242,
-            "heading": -0.004343205426988983,
-            "angularVelocity": -0.03345280017329315,
-            "velocityX": 1.6829687218782206,
-            "velocityY": 0.781950994568762,
-            "timestamp": 0.22256541185682374
-        },
-        {
-            "x": 2.5413327067825313,
-            "y": 5.695360765985745,
-            "heading": -0.005790919055957113,
-            "angularVelocity": -0.039027994967159495,
-            "velocityX": 1.9634635092880914,
-            "velocityY": 0.9122760168036823,
-            "timestamp": 0.25965964716629436
-        },
-        {
-            "x": 2.6245706232908623,
-            "y": 5.734035252447861,
-            "heading": -0.007445434065373688,
-            "angularVelocity": -0.044603022426903935,
-            "velocityX": 2.2439582812232635,
-            "velocityY": 1.042600990139337,
-            "timestamp": 0.296753882475765
-        },
-        {
-            "x": 2.7182132777182155,
-            "y": 5.777544042048614,
-            "heading": -0.009306740898831883,
-            "angularVelocity": -0.05017779226152841,
-            "velocityX": 2.5244530220426173,
-            "velocityY": 1.1729259071596103,
-            "timestamp": 0.33384811778523565
-        },
-        {
-            "x": 2.8222606677468502,
-            "y": 5.825887132155531,
-            "heading": -0.011374823315541767,
-            "angularVelocity": -0.05575212427098156,
-            "velocityX": 2.804947700379475,
-            "velocityY": 1.3032507532126736,
-            "timestamp": 0.3709423530947063
-        },
-        {
-            "x": 2.9367127881472834,
-            "y": 5.879064518782887,
-            "heading": -0.013649648601341033,
-            "angularVelocity": -0.0613255743600492,
-            "velocityX": 3.0854422377380093,
-            "velocityY": 1.4335754918171768,
-            "timestamp": 0.40803658840417695
-        },
-        {
-            "x": 3.061569622038521,
-            "y": 5.9370761925311655,
-            "heading": -0.016131121234305167,
-            "angularVelocity": -0.06689644933404303,
-            "velocityX": 3.365936320012513,
-            "velocityY": 1.563899977025988,
-            "timestamp": 0.4451308237136476
-        },
-        {
-            "x": 3.188829425192231,
-            "y": 5.996204205741329,
-            "heading": -0.01613112189285862,
-            "angularVelocity": -1.7753525563113908e-8,
-            "velocityX": 3.430716446693241,
-            "velocityY": 1.5939946656635118,
-            "timestamp": 0.48222505902311824
-        },
-        {
-            "x": 3.3160892428339666,
-            "y": 6.055332187769375,
-            "heading": -0.01613112255138683,
-            "angularVelocity": -1.775284497104555e-8,
-            "velocityX": 3.4307168372667594,
-            "velocityY": 1.5939938250445522,
-            "timestamp": 0.5193192943325888
-        },
-        {
-            "x": 3.443349075020492,
-            "y": 6.114460138492994,
-            "heading": -0.016131123209912376,
-            "angularVelocity": -1.775277322447583e-8,
-            "velocityX": 3.43071722937053,
-            "velocityY": 1.593992981128321,
-            "timestamp": 0.5564135296420595
-        },
-        {
-            "x": 3.5706089218502877,
-            "y": 6.173588057700207,
-            "heading": -0.01613112386843302,
-            "angularVelocity": -1.775264097704561e-8,
-            "velocityX": 3.430717624129179,
-            "velocityY": 1.5939921314975005,
-            "timestamp": 0.5935077649515301
-        },
-        {
-            "x": 3.697868783477107,
-            "y": 6.2327159450600735,
-            "heading": -0.016131124526946636,
-            "angularVelocity": -1.7752451586110053e-8,
-            "velocityX": 3.430718023032762,
-            "velocityY": 1.5939912729450545,
-            "timestamp": 0.6306020002610008
-        },
-        {
-            "x": 3.8251286601281493,
-            "y": 6.291843800083575,
-            "heading": -0.016131125185451407,
-            "angularVelocity": -1.7752213148650806e-8,
-            "velocityX": 3.4307184280612866,
-            "velocityY": 1.5939904012094348,
-            "timestamp": 0.6676962355704714
-        },
-        {
-            "x": 3.95238855212494,
-            "y": 6.350971622078674,
-            "heading": -0.016131125843945708,
-            "angularVelocity": -1.775193091205886e-8,
-            "velocityX": 3.430718841757593,
-            "velocityY": 1.5939895108176958,
-            "timestamp": 0.7047904708799421
-        },
-        {
-            "x": 4.079648459881884,
-            "y": 6.410099410153433,
-            "heading": -0.01613112650242917,
-            "angularVelocity": -1.7751638714463086e-8,
-            "velocityX": 3.430719266625584,
-            "velocityY": 1.5939885963806835,
-            "timestamp": 0.7418847061894127
-        },
-        {
-            "x": 4.2069083839232695,
-            "y": 6.469227163179404,
-            "heading": -0.01613112716090301,
-            "angularVelocity": -1.7751379248608663e-8,
-            "velocityX": 3.430719705627546,
-            "velocityY": 1.5939876515226545,
-            "timestamp": 0.7789789414988834
-        },
-        {
-            "x": 4.334168324938812,
-            "y": 6.528354879672101,
-            "heading": -0.016131127819368877,
-            "angularVelocity": -1.7751164242273923e-8,
-            "velocityX": 3.430720163223074,
-            "velocityY": 1.5939866666451934,
-            "timestamp": 0.816073176808354
-        },
-        {
-            "x": 4.461428283794271,
-            "y": 6.58748255776812,
-            "heading": -0.01613112847782922,
-            "angularVelocity": -1.775101542628313e-8,
-            "velocityX": 3.430720644158123,
-            "velocityY": 1.5939856315335348,
-            "timestamp": 0.8531674121178247
-        },
-        {
-            "x": 4.58868833054201,
-            "y": 6.646610046694028,
-            "heading": -0.016131129136287645,
-            "angularVelocity": -1.7750963701844846e-8,
-            "velocityX": 3.430723013590394,
-            "velocityY": 1.5939805318162525,
-            "timestamp": 0.8902616474272953
-        },
-        {
-            "x": 4.716960790002456,
-            "y": 6.703507810971908,
-            "heading": -0.016131129795891656,
-            "angularVelocity": -1.7781846861521853e-8,
-            "velocityX": 3.4580160068077612,
-            "velocityY": 1.5338707969903813,
-            "timestamp": 0.927355882736766
-        },
-        {
-            "x": 4.849451901724698,
-            "y": 6.749738586923234,
-            "heading": -0.016131130468276335,
-            "angularVelocity": -1.8126392792981438e-8,
-            "velocityX": 3.5717439816966663,
-            "velocityY": 1.2463062135027452,
-            "timestamp": 0.9644501180462366
-        },
-        {
-            "x": 4.985276799458285,
-            "y": 6.784991618369029,
-            "heading": -0.016131131172819703,
-            "angularVelocity": -1.899333848953808e-8,
-            "velocityX": 3.661617407676018,
-            "velocityY": 0.950364150970728,
-            "timestamp": 1.0015443533557071
-        },
-        {
-            "x": 5.12352760039525,
-            "y": 6.80903105836077,
-            "heading": -0.01613113193292308,
-            "angularVelocity": -2.0491145599848975e-8,
-            "velocityX": 3.7270157959468126,
-            "velocityY": 0.6480640399022476,
-            "timestamp": 1.0386385886651777
-        },
-        {
-            "x": 5.263280144021708,
-            "y": 6.821696149590996,
-            "heading": -0.01613113277968929,
-            "angularVelocity": -2.2827434072857776e-8,
-            "velocityX": 3.767500326143083,
-            "velocityY": 0.341430174380559,
-            "timestamp": 1.0757328239746482
+            "x": 2.26040474690827,
+            "y": 5.564834298284281,
+            "heading": -0.00020682462174428096,
+            "angularVelocity": -0.005575654083684234,
+            "velocityX": 0.28049498700644215,
+            "velocityY": 0.1303247879443207,
+            "timestamp": 0.037094234800108185
+        },
+        {
+            "x": 2.2812142405469196,
+            "y": 5.574502894544252,
+            "heading": -0.0006204719876615776,
+            "angularVelocity": -0.011151257551113,
+            "velocityX": 0.5609899692172368,
+            "velocityY": 0.26064956756953994,
+            "timestamp": 0.07418846960021637
+        },
+        {
+            "x": 2.312428480686622,
+            "y": 5.589005788443611,
+            "heading": -0.0012409398807264826,
+            "angularVelocity": -0.016726801251156914,
+            "velocityX": 0.8414849452457597,
+            "velocityY": 0.39097433812857635,
+            "timestamp": 0.11128270440032456
+        },
+        {
+            "x": 2.3540474670292637,
+            "y": 5.608342979609529,
+            "heading": -0.002068225636714691,
+            "angularVelocity": -0.022302273128063925,
+            "velocityX": 1.1219799132376411,
+            "velocityY": 0.5212990986367875,
+            "timestamp": 0.14837693920043274
+        },
+        {
+            "x": 2.406071199181798,
+            "y": 5.632514467619691,
+            "heading": -0.0031023259847413323,
+            "angularVelocity": -0.02787765682725579,
+            "velocityX": 1.4024748706336019,
+            "velocityY": 0.6516238477600773,
+            "timestamp": 0.18547117400054092
+        },
+        {
+            "x": 2.4684996766150022,
+            "y": 5.66152025198224,
+            "heading": -0.004343236799506005,
+            "angularVelocity": -0.03345292931508493,
+            "velocityX": 1.682969813762594,
+            "velocityY": 0.7819485836236739,
+            "timestamp": 0.2225654088006491
+        },
+        {
+            "x": 2.5413328985945514,
+            "y": 5.69536033210275,
+            "heading": -0.005790952692459236,
+            "angularVelocity": -0.03902805653641911,
+            "velocityX": 1.9634647370954057,
+            "velocityY": 0.9122733034625292,
+            "timestamp": 0.2596596436007573
+        },
+        {
+            "x": 2.6245708640566887,
+            "y": 5.7340347072253275,
+            "heading": -0.00744546628275827,
+            "angularVelocity": -0.04460298478228464,
+            "velocityX": 2.2439596317510366,
+            "velocityY": 1.0425980029237354,
+            "timestamp": 0.2967538784008655
+        },
+        {
+            "x": 2.7182135713591755,
+            "y": 5.777543376315584,
+            "heading": -0.009306766753500902,
+            "angularVelocity": -0.05017762142212937,
+            "velocityX": 2.5244544821346007,
+            "velocityY": 1.1729226744995158,
+            "timestamp": 0.3338481132009737
+        },
+        {
+            "x": 2.8222610176995655,
+            "y": 5.8258863377887575,
+            "heading": -0.011374836524793099,
+            "angularVelocity": -0.055751784136736884,
+            "velocityX": 2.8049492569688192,
+            "velocityY": 1.303247303352706,
+            "timestamp": 0.3709423480010819
+        },
+        {
+            "x": 2.9367131973687606,
+            "y": 5.879063588696104,
+            "heading": -0.013649641477510719,
+            "angularVelocity": -0.061325027055469855,
+            "velocityX": 3.0854438778950355,
+            "velocityY": 1.4335718527125696,
+            "timestamp": 0.4080365828011901
+        },
+        {
+            "x": 3.06157009301301,
+            "y": 5.937075120660461,
+            "heading": -0.01613108463550858,
+            "angularVelocity": -0.06689565565567898,
+            "velocityX": 3.3659380309924987,
+            "velocityY": 1.5638961762378367,
+            "timestamp": 0.4451308176012983
+        },
+        {
+            "x": 3.1888299168589427,
+            "y": 5.996203084762226,
+            "heading": -0.01613108529402469,
+            "angularVelocity": -1.7752519083825314e-8,
+            "velocityX": 3.430717051630934,
+            "velocityY": 1.5939933636693306,
+            "timestamp": 0.4822250524014065
+        },
+        {
+            "x": 3.316089744181463,
+            "y": 6.055331041381533,
+            "heading": -0.016131085952509703,
+            "angularVelocity": -1.7751680763659775e-8,
+            "velocityX": 3.4307171453540595,
+            "velocityY": 1.5939931619544851,
+            "timestamp": 0.5193192872015147
+        },
+        {
+            "x": 3.443349575355442,
+            "y": 6.114458989711434,
+            "heading": -0.01613108661098856,
+            "angularVelocity": -1.7751514817327164e-8,
+            "velocityX": 3.430717249183109,
+            "velocityY": 1.5939929384856546,
+            "timestamp": 0.5564135220016229
+        },
+        {
+            "x": 3.5706094108109987,
+            "y": 6.173586928826195,
+            "heading": -0.016131087269461084,
+            "angularVelocity": -1.7751344035751635e-8,
+            "velocityX": 3.4307173646074394,
+            "velocityY": 1.5939926900605101,
+            "timestamp": 0.5935077568017311
+        },
+        {
+            "x": 3.6978692510432434,
+            "y": 6.232714857660196,
+            "heading": -0.01613108792792704,
+            "angularVelocity": -1.7751166882414398e-8,
+            "velocityX": 3.4307174933791567,
+            "velocityY": 1.5939924129080143,
+            "timestamp": 0.6306019916018393
+        },
+        {
+            "x": 3.825129096624906,
+            "y": 6.291842774980762,
+            "heading": -0.016131088586386323,
+            "angularVelocity": -1.775098711225461e-8,
+            "velocityX": 3.430717637590709,
+            "velocityY": 1.5939921025246122,
+            "timestamp": 0.6676962264019475
+        },
+        {
+            "x": 3.952388948222243,
+            "y": 6.3509706793539165,
+            "heading": -0.016131089244838845,
+            "angularVelocity": -1.775080474504649e-8,
+            "velocityX": 3.4307177997634843,
+            "velocityY": 1.5939917534835466,
+            "timestamp": 0.7047904612020557
+        },
+        {
+            "x": 4.07964880661482,
+            "y": 6.410098569101814,
+            "heading": -0.016131089903284688,
+            "angularVelocity": -1.775062484471552e-8,
+            "velocityX": 3.430717982952055,
+            "velocityY": 1.5939913592104822,
+            "timestamp": 0.7418846960021639
+        },
+        {
+            "x": 4.206908672718448,
+            "y": 6.469226442253365,
+            "heading": -0.016131090561724272,
+            "angularVelocity": -1.7750456035932977e-8,
+            "velocityX": 3.4307181908294035,
+            "velocityY": 1.5939909118000826,
+            "timestamp": 0.7789789308022721
+        },
+        {
+            "x": 4.33416854760619,
+            "y": 6.52835429649903,
+            "heading": -0.016131091220158388,
+            "angularVelocity": -1.775030869099235e-8,
+            "velocityX": 3.43071842763476,
+            "velocityY": 1.593990402128264,
+            "timestamp": 0.8160731656023803
+        },
+        {
+            "x": 4.461428432515134,
+            "y": 6.587482129176235,
+            "heading": -0.016131091878588796,
+            "angularVelocity": -1.7750208582663698e-8,
+            "velocityX": 3.430718697790006,
+            "velocityY": 1.5939898206778003,
+            "timestamp": 0.8531674004024885
+        },
+        {
+            "x": 4.588688397859823,
+            "y": 6.646609788732575,
+            "heading": -0.01613109253701839,
+            "angularVelocity": -1.7750186861935105e-8,
+            "velocityX": 3.4307208662063466,
+            "velocityY": 1.5939851536219738,
+            "timestamp": 0.8902616352025967
+        },
+        {
+            "x": 4.7169608374986005,
+            "y": 6.703507592945478,
+            "heading": -0.016131093196594153,
+            "angularVelocity": -1.7781085471466767e-8,
+            "velocityX": 3.4580155199319775,
+            "velocityY": 1.533871894635887,
+            "timestamp": 0.9273558700027049
+        },
+        {
+            "x": 4.84945193271468,
+            "y": 6.749738410352506,
+            "heading": -0.016131093868950192,
+            "angularVelocity": -1.8125621008136676e-8,
+            "velocityX": 3.571743585763167,
+            "velocityY": 1.2463073481945173,
+            "timestamp": 0.9644501048028131
+        },
+        {
+            "x": 4.985276817345946,
+            "y": 6.784991484609654,
+            "heading": -0.0161310945734632,
+            "angularVelocity": -1.8992520332334574e-8,
+            "velocityX": 3.661617104738627,
+            "velocityY": 0.9503653181449264,
+            "timestamp": 1.0015443396029213
+        },
+        {
+            "x": 5.123527608694991,
+            "y": 6.809030968493646,
+            "heading": -0.01613109533353302,
+            "angularVelocity": -2.0490241268499195e-8,
+            "velocityX": 3.7270155886499414,
+            "velocityY": 0.6480652320645928,
+            "timestamp": 1.0386385744030295
+        },
+        {
+            "x": 5.263280146337045,
+            "y": 6.821696104409015,
+            "heading": -0.016131096180260384,
+            "angularVelocity": -2.2826387114294202e-8,
+            "velocityX": 3.767500216547063,
+            "velocityY": 0.34143138370740883,
+            "timestamp": 1.0757328092031377
         },
         {
             "x": 5.403600215911865,
             "y": 6.822902202606201,
-            "heading": -0.016131133746573465,
-            "angularVelocity": -2.6065618224331403e-8,
-            "velocityX": 3.782799961220162,
-            "velocityY": 0.03251321951088522,
-            "timestamp": 1.1128270592841187
-        },
-        {
-            "x": 5.530811600424101,
-            "y": 6.814532053807006,
-            "heading": -0.016131134641902804,
-            "angularVelocity": -2.6567346760379892e-8,
-            "velocityX": 3.7747774237594087,
-            "velocityY": -0.24836966315439601,
-            "timestamp": 1.1465274262810004
-        },
-        {
-            "x": 5.6570507349000225,
-            "y": 6.796742242248266,
-            "heading": -0.016131135427811184,
-            "angularVelocity": -2.332046941486525e-8,
-            "velocityX": 3.7459275884919543,
-            "velocityY": -0.5278818346513846,
-            "timestamp": 1.180227793277882
-        },
-        {
-            "x": 5.781621094465109,
-            "y": 6.769630943607204,
-            "heading": -0.01613113613886582,
-            "angularVelocity": -2.1099314246183972e-8,
-            "velocityX": 3.696409584400467,
-            "velocityY": -0.8044808130300218,
-            "timestamp": 1.2139281602747636
-        },
-        {
-            "x": 5.903835372739644,
-            "y": 6.733347785640362,
-            "heading": -0.016131136799649604,
-            "angularVelocity": -1.9607613962388268e-8,
-            "velocityX": 3.6264969543460657,
-            "velocityY": -1.0766398469844052,
-            "timestamp": 1.2476285272716452
-        },
-        {
-            "x": 6.023019298132563,
-            "y": 6.68809306728661,
-            "heading": -0.01613113742890953,
-            "angularVelocity": -1.8672198003746715e-8,
-            "velocityX": 3.536576483097275,
-            "velocityY": -1.3428553569739705,
-            "timestamp": 1.2813288942685268
-        },
-        {
-            "x": 6.13851552436941,
-            "y": 6.634116966515058,
-            "heading": -0.016131138042081584,
-            "angularVelocity": -1.819481831325098e-8,
-            "velocityX": 3.4271504000990243,
-            "velocityY": -1.601647269198795,
-            "timestamp": 1.3150292612654084
-        },
-        {
-            "x": 6.251225914876305,
-            "y": 6.574542092363786,
-            "heading": -0.016131138651474624,
-            "angularVelocity": -1.808268258780769e-8,
-            "velocityX": 3.344485551665478,
-            "velocityY": -1.7677811685785219,
-            "timestamp": 1.34872962826229
-        },
-        {
-            "x": 6.363935678841957,
-            "y": 6.51496603286697,
-            "heading": -0.016131139260695985,
-            "angularVelocity": -1.807758837477246e-8,
-            "velocityX": 3.344466960139662,
-            "velocityY": -1.7678163416537895,
-            "timestamp": 1.3824299952591717
-        },
-        {
-            "x": 6.475851865782255,
-            "y": 6.4539122462129415,
-            "heading": -0.016131138603390965,
-            "angularVelocity": 1.950438761664722e-8,
-            "velocityX": 3.320918936896271,
-            "velocityY": -1.8116653346735878,
-            "timestamp": 1.4161303622560533
-        },
-        {
-            "x": 6.587767727105175,
-            "y": 6.3928578626815735,
-            "heading": -0.016131137945887282,
-            "angularVelocity": 1.9510282594383425e-8,
-            "velocityX": 3.320909274764734,
-            "velocityY": -1.811683045974467,
-            "timestamp": 1.4498307292529349
-        },
-        {
-            "x": 6.699683536860704,
-            "y": 6.331803384624609,
-            "heading": -0.016131137288379963,
-            "angularVelocity": 1.951039048402864e-8,
-            "velocityX": 3.3209077445917723,
-            "velocityY": -1.811685850857758,
-            "timestamp": 1.4835310962498165
-        },
-        {
-            "x": 6.811599317469464,
-            "y": 6.270748853140253,
-            "heading": -0.0161311366308819,
-            "angularVelocity": 1.951011573787136e-8,
-            "velocityX": 3.320906879711909,
-            "velocityY": -1.811687436223046,
-            "timestamp": 1.517231463246698
-        },
-        {
-            "x": 6.923515076276675,
-            "y": 6.209694281692974,
-            "heading": -0.016131135973346068,
-            "angularVelocity": 1.951123652121546e-8,
-            "velocityX": 3.3209062327887042,
-            "velocityY": -1.8116886220535497,
-            "timestamp": 1.5509318302435797
-        },
-        {
-            "x": 7.031584320380035,
-            "y": 6.150738002364414,
-            "heading": -0.01382673748429001,
-            "angularVelocity": 0.06837903246775291,
-            "velocityX": 3.2067675735804393,
-            "velocityY": -1.7494254390171946,
-            "timestamp": 1.5846321972404613
-        },
-        {
-            "x": 7.1313405673144,
-            "y": 6.0963168023113505,
-            "heading": -0.011699564081675173,
-            "angularVelocity": 0.06312018509503639,
-            "velocityX": 2.9600937860289633,
-            "velocityY": -1.6148548191804175,
-            "timestamp": 1.618332564237343
-        },
-        {
-            "x": 7.222783801332874,
-            "y": 6.046430691545947,
-            "heading": -0.009749641962936324,
-            "angularVelocity": 0.05786056035883372,
-            "velocityX": 2.7134195312156404,
-            "velocityY": -1.480283902250094,
-            "timestamp": 1.6520329312342246
-        },
-        {
-            "x": 7.305914017550448,
-            "y": 6.00107967407256,
-            "heading": -0.007976979919226685,
-            "angularVelocity": 0.05260067476039131,
-            "velocityX": 2.4667451314481808,
-            "velocityY": -1.3457128664973763,
-            "timestamp": 1.6857332982311062
-        },
-        {
-            "x": 7.380731213778514,
-            "y": 5.960263752360125,
-            "heading": -0.006381581900532674,
-            "angularVelocity": 0.047340671952945675,
-            "velocityX": 2.2200706667375205,
-            "velocityY": -1.2111417574833037,
-            "timestamp": 1.7194336652279878
-        },
-        {
-            "x": 7.447235388895749,
-            "y": 5.923982928242947,
-            "heading": -0.004963449854323965,
-            "angularVelocity": 0.042080611357711836,
-            "velocityX": 1.973396168753552,
-            "velocityY": -1.076570594039371,
-            "timestamp": 1.7531340322248694
-        },
-        {
-            "x": 7.505426542307151,
-            "y": 5.892237203224628,
-            "heading": -0.00372258471684395,
-            "angularVelocity": 0.03682052298107744,
-            "velocityX": 1.72672165311393,
-            "velocityY": -0.9419993859786789,
-            "timestamp": 1.786834399221751
-        },
-        {
-            "x": 7.5553046737134695,
-            "y": 5.865026578610625,
-            "heading": -0.002658986847128457,
-            "angularVelocity": 0.03156042395067696,
-            "velocityX": 1.4800471285945758,
-            "velocityY": -0.8074281391808545,
-            "timestamp": 1.8205347662186326
-        },
-        {
-            "x": 7.596869782996782,
-            "y": 5.842351055576096,
-            "heading": -0.0017726562460022867,
-            "angularVelocity": 0.026300324895801946,
-            "velocityX": 1.233372600576068,
-            "velocityY": -0.6728568575121819,
-            "timestamp": 1.8542351332155143
-        },
-        {
-            "x": 7.630121870157635,
-            "y": 5.82421063520489,
-            "heading": -0.0010635926829587166,
-            "angularVelocity": 0.021040232680814546,
-            "velocityX": 0.9866980725738072,
-            "velocityY": -0.5382855436821761,
-            "timestamp": 1.8879355002123959
-        },
-        {
-            "x": 7.655060935277959,
-            "y": 5.8106053185140585,
-            "heading": -0.0005317957806907868,
-            "angularVelocity": 0.015780151661757422,
-            "velocityX": 0.7400235470026807,
-            "velocityY": -0.40371419967297517,
-            "timestamp": 1.9216358672092775
-        },
-        {
-            "x": 7.671686978497787,
-            "y": 5.801535106470048,
-            "heading": -0.00017726506577651584,
-            "angularVelocity": 0.010520084690677335,
-            "velocityX": 0.4933490255867586,
-            "velocityY": -0.2691428269861255,
-            "timestamp": 1.955336234206159
+            "heading": -0.0161310971470982,
+            "angularVelocity": -2.6064368823146806e-8,
+            "velocityX": 3.7827999507462917,
+            "velocityY": 0.032514437989762196,
+            "timestamp": 1.112827044003246
+        },
+        {
+            "x": 5.530811605245593,
+            "y": 6.814532094483614,
+            "heading": -0.016131098042384447,
+            "angularVelocity": -2.6566067685833464e-8,
+            "velocityX": 3.7747775034488793,
+            "velocityY": -0.2483684519763222,
+            "timestamp": 1.1465274115659696
+        },
+        {
+            "x": 5.657050747526861,
+            "y": 6.796742322973506,
+            "heading": -0.016131098828254187,
+            "angularVelocity": -2.3319322572510494e-8,
+            "velocityX": 3.745927757206515,
+            "velocityY": -0.5278806374143462,
+            "timestamp": 1.1802277791286933
+        },
+        {
+            "x": 5.781621117814577,
+            "y": 6.76963106353518,
+            "heading": -0.016131099539274135,
+            "angularVelocity": -2.109828463295381e-8,
+            "velocityX": 3.6964098405118033,
+            "velocityY": -0.8044796362492181,
+            "timestamp": 1.213928146691417
+        },
+        {
+            "x": 5.903835409646495,
+            "y": 6.733347943713289,
+            "heading": -0.016131100200026813,
+            "angularVelocity": -1.96066906061588e-8,
+            "velocityX": 3.626497295747634,
+            "velocityY": -1.0766386970219277,
+            "timestamp": 1.2476285142541406
+        },
+        {
+            "x": 6.023019351333038,
+            "y": 6.68809326224078,
+            "heading": -0.016131100829259,
+            "angularVelocity": -1.86713746487578e-8,
+            "velocityX": 3.5365769072018636,
+            "velocityY": -1.3428542400399708,
+            "timestamp": 1.2813288818168642
+        },
+        {
+            "x": 6.138515596479519,
+            "y": 6.634117196875684,
+            "heading": -0.016131101442406693,
+            "angularVelocity": -1.81940951537302e-8,
+            "velocityX": 3.4271509036665004,
+            "velocityY": -1.601646191681265,
+            "timestamp": 1.3150292493795879
+        },
+        {
+            "x": 6.2512259314426855,
+            "y": 6.5745422130600915,
+            "heading": -0.016131102051778846,
+            "angularVelocity": -1.80820624580199e-8,
+            "velocityX": 3.3444838473464196,
+            "velocityY": -1.7677843929954529,
+            "timestamp": 1.3487296169423115
+        },
+        {
+            "x": 6.363935646775815,
+            "y": 6.514966056976746,
+            "heading": -0.01613110266098242,
+            "angularVelocity": -1.807706019437417e-8,
+            "velocityX": 3.344465460898959,
+            "velocityY": -1.7678191780093255,
+            "timestamp": 1.3824299845050352
+        },
+        {
+            "x": 6.4758517936606514,
+            "y": 6.453912192428435,
+            "heading": -0.016131102003666662,
+            "angularVelocity": 1.950470585863807e-8,
+            "velocityX": 3.320917692560386,
+            "velocityY": -1.811667615632877,
+            "timestamp": 1.4161303520677588
+        },
+        {
+            "x": 6.587767622018337,
+            "y": 6.392857744000494,
+            "heading": -0.016131101346154864,
+            "angularVelocity": 1.951052313957626e-8,
+            "velocityX": 3.32090824081921,
+            "velocityY": -1.8116849412489389,
+            "timestamp": 1.4498307196304825
+        },
+        {
+            "x": 6.699683405881235,
+            "y": 6.331803214011495,
+            "heading": -0.016131100688641712,
+            "angularVelocity": 1.9510563155904532e-8,
+            "velocityX": 3.3209069205135386,
+            "velocityY": -1.811687361432026,
+            "timestamp": 1.4835310871932061
+        },
+        {
+            "x": 6.811599167638959,
+            "y": 6.270748643502715,
+            "heading": -0.016131100031139815,
+            "angularVelocity": 1.9510229240352237e-8,
+            "velocityX": 3.320906264580792,
+            "velocityY": -1.8116885637862563,
+            "timestamp": 1.5172314547559298
+        },
+        {
+            "x": 6.923514914598752,
+            "y": 6.209694045868981,
+            "heading": -0.016131099373601954,
+            "angularVelocity": 1.95112965056638e-8,
+            "velocityX": 3.3209058254778503,
+            "velocityY": -1.8116893686722635,
+            "timestamp": 1.5509318223186535
+        },
+        {
+            "x": 7.031584175346113,
+            "y": 6.1507377884047765,
+            "heading": -0.013826704963615632,
+            "angularVelocity": 0.0683789102803133,
+            "velocityX": 3.2067680136194574,
+            "velocityY": -1.7494247608568367,
+            "timestamp": 1.584632189881377
+        },
+        {
+            "x": 7.131340439149152,
+            "y": 6.096316611291437,
+            "heading": -0.011699535798216875,
+            "angularVelocity": 0.06312005830317652,
+            "velocityX": 2.960094236876553,
+            "velocityY": -1.614854111369865,
+            "timestamp": 1.6183325574441008
+        },
+        {
+            "x": 7.222783690004588,
+            "y": 6.046430524071638,
+            "heading": -0.00974961791861361,
+            "angularVelocity": 0.057860433598327515,
+            "velocityX": 2.713419985263968,
+            "velocityY": -1.4802831787205646,
+            "timestamp": 1.6520329250068244
+        },
+        {
+            "x": 7.305913922772626,
+            "y": 6.001079530283105,
+            "heading": -0.00797695998479322,
+            "angularVelocity": 0.05260055192336173,
+            "velocityX": 2.466745581136908,
+            "velocityY": -1.3457121410953312,
+            "timestamp": 1.685733292569548
+        },
+        {
+            "x": 7.380731135011445,
+            "y": 5.960263631930958,
+            "heading": -0.00638156583911186,
+            "angularVelocity": 0.04734055623313652,
+            "velocityX": 2.2200711045530213,
+            "velocityY": -1.2111410439717167,
+            "timestamp": 1.7194336601322717
+        },
+        {
+            "x": 7.447235325348076,
+            "y": 5.923982830388488,
+            "heading": -0.004963437343393232,
+            "angularVelocity": 0.042080505296519764,
+            "velocityX": 1.9733965872286794,
+            "velocityY": -1.0765699060980205,
+            "timestamp": 1.7531340276949954
+        },
+        {
+            "x": 7.505426492937412,
+            "y": 5.892237126701046,
+            "heading": -0.0037225753686417596,
+            "angularVelocity": 0.0368204285143562,
+            "velocityX": 1.72672204482727,
+            "velocityY": -0.9419987372053683,
+            "timestamp": 1.786834395257719
+        },
+        {
+            "x": 7.555304637231595,
+            "y": 5.8650265217185185,
+            "heading": -0.002658980227466916,
+            "angularVelocity": 0.03156034245604838,
+            "velocityX": 1.4800474861690802,
+            "velocityY": -0.8074275430937743,
+            "timestamp": 1.8205347628204427
+        },
+        {
+            "x": 7.596869757865521,
+            "y": 5.842351016163066,
+            "heading": -0.0017726518914738514,
+            "angularVelocity": 0.02630025724030852,
+            "velocityX": 1.233372916677093,
+            "velocityY": -0.6728563275533557,
+            "timestamp": 1.8542351303831663
+        },
+        {
+            "x": 7.630121854593904,
+            "y": 5.824210610667954,
+            "heading": -0.001063590116536395,
+            "angularVelocity": 0.021040179268589657,
+            "velocityX": 0.9866983399066315,
+            "velocityY": -0.5382850932218669,
+            "timestamp": 1.88793549794589
+        },
+        {
+            "x": 7.655060927254085,
+            "y": 5.810605305801891,
+            "heading": -0.000531794525727047,
+            "angularVelocity": 0.01578011248154109,
+            "velocityX": 0.7400237583095566,
+            "velocityY": -0.4037138420149424,
+            "timestamp": 1.9216358655086136
+        },
+        {
+            "x": 7.671686975742638,
+            "y": 5.8015351020850225,
+            "heading": -0.00017726465841382563,
+            "angularVelocity": 0.010520059362948236,
+            "velocityX": 0.49334917364355607,
+            "velocityY": -0.26914257537363395,
+            "timestamp": 1.9553362330713373
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.986792057163996e-31,
-            "angularVelocity": 0.005260033690221658,
-            "velocityX": 0.24667450959752207,
-            "velocityY": -0.13457142678797143,
-            "timestamp": 1.9890366012030407
+            "heading": 3.6111480760753294e-31,
+            "angularVelocity": 0.005260021514121506,
+            "velocityX": 0.24667458721003926,
+            "velocityY": -0.1345712944104092,
+            "timestamp": 1.989036600634061
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 0,
-            "angularVelocity": -1.0163368054249497e-29,
-            "velocityX": -1.0830525743862323e-33,
-            "velocityY": 5.9205016331337416e-33,
-            "timestamp": 2.022736968199922
+            "heading": -8.452347473573201e-32,
+            "angularVelocity": -1.5098225699671103e-29,
+            "velocityX": 2.1451735090517096e-32,
+            "velocityY": 9.12030277003484e-33,
+            "timestamp": 2.0227369681967846
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj
index 1bd4bff0..35bba061 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj	
@@ -3,101 +3,101 @@
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -1.6570559277738772e-34,
-            "velocityY": -1.2609465104041817e-35,
+            "heading": -5.451838641793523e-34,
+            "angularVelocity": -2.0131697388730357e-33,
+            "velocityX": -3.0924959567962797e-34,
+            "velocityY": 4.789643826321005e-35,
             "timestamp": 0
         },
         {
-            "x": 2.25200199902585,
-            "y": 5.607878029194235,
-            "heading": 0.015821088838010276,
-            "angularVelocity": 0.20825462300812525,
-            "velocityX": 0.026352519517453685,
-            "velocityY": 0.6302234329322915,
-            "timestamp": 0.0759699285878182
+            "x": 2.2520020156777094,
+            "y": 5.6078780285533805,
+            "heading": 0.01582108665485387,
+            "angularVelocity": 0.208254594271014,
+            "velocityX": 0.026352738707599337,
+            "velocityY": 0.630223424496655,
+            "timestamp": 0.07596992858781765
         },
         {
-            "x": 2.2560055011308777,
-            "y": 5.703634196198671,
-            "heading": 0.04745824894413579,
-            "angularVelocity": 0.4164431992265759,
-            "velocityX": 0.05269851083774254,
-            "velocityY": 1.2604482955877103,
-            "timestamp": 0.1519398571756364
+            "x": 2.2560055471462697,
+            "y": 5.703634194455967,
+            "heading": 0.04745824180691435,
+            "angularVelocity": 0.4164431340157103,
+            "velocityX": 0.05269889735295001,
+            "velocityY": 1.2604482810839739,
+            "timestamp": 0.1519398571756353
         },
         {
-            "x": 2.2620098071430843,
-            "y": 5.847268765721161,
-            "heading": 0.09489872499810467,
-            "angularVelocity": 0.6244638758496344,
-            "velocityX": 0.07903529888495084,
-            "velocityY": 1.8906766426199095,
-            "timestamp": 0.2279097857634546
+            "x": 2.262009891384212,
+            "y": 5.847268762594484,
+            "heading": 0.09489870943903907,
+            "angularVelocity": 0.6244637649920359,
+            "velocityX": 0.07903580205425154,
+            "velocityY": 1.8906766244025437,
+            "timestamp": 0.22790978576345294
         },
         {
-            "x": 2.270014094994434,
-            "y": 6.038782268539179,
-            "heading": 0.1581161373911332,
-            "angularVelocity": 0.8321373149634034,
-            "velocityX": 0.10536126596587263,
-            "velocityY": 2.5209119763306935,
-            "timestamp": 0.3038797143512728
+            "x": 2.2700142224347877,
+            "y": 6.038782263911744,
+            "heading": 0.1581161098401003,
+            "angularVelocity": 0.8321371571119077,
+            "velocityX": 0.1053618346017344,
+            "velocityY": 2.5209119565760743,
+            "timestamp": 0.3038797143512706
         },
         {
-            "x": 2.28000939681014,
-            "y": 6.278187229695624,
-            "heading": 0.23645967758045489,
-            "angularVelocity": 1.031244094151802,
-            "velocityX": 0.13156918798668546,
-            "velocityY": 3.1513121784720814,
-            "timestamp": 0.379849642939091
+            "x": 2.280009491675263,
+            "y": 6.278187226323642,
+            "heading": 0.23645965423367718,
+            "angularVelocity": 1.0312441494928544,
+            "velocityX": 0.13156875919557368,
+            "velocityY": 3.151312194997739,
+            "timestamp": 0.37984964293908824
         },
         {
-            "x": 2.2880055947499205,
-            "y": 6.469712010307998,
-            "heading": 0.29909347070783976,
-            "angularVelocity": 0.824455074417803,
-            "velocityX": 0.10525477762609038,
-            "velocityY": 2.521060427100167,
-            "timestamp": 0.4558195715269092
+            "x": 2.288005657404111,
+            "y": 6.469712008115255,
+            "heading": 0.2990934539478391,
+            "angularVelocity": 0.8244551611202343,
+            "velocityX": 0.10525435363026732,
+            "velocityY": 2.52106044262263,
+            "timestamp": 0.4558195715269059
         },
         {
-            "x": 2.29400278172627,
-            "y": 6.613355953306298,
-            "heading": 0.3460499566805711,
-            "angularVelocity": 0.6180930645268612,
-            "velocityX": 0.07894159028222533,
-            "velocityY": 1.8908000266480898,
-            "timestamp": 0.5317895001147275
+            "x": 2.294002815862431,
+            "y": 6.613355952122642,
+            "heading": 0.3460499471183657,
+            "angularVelocity": 0.6180931592721867,
+            "velocityX": 0.07894121489645621,
+            "velocityY": 1.8908000399308182,
+            "timestamp": 0.5317895001147235
         },
         {
-            "x": 2.298000929157321,
-            "y": 6.709118669792275,
-            "heading": 0.37734923040208523,
-            "angularVelocity": 0.4119955659209739,
-            "velocityX": 0.052628026712297535,
-            "velocityY": 1.2605345070882814,
-            "timestamp": 0.6077594287025456
+            "x": 2.2980009414583966,
+            "y": 6.709118669367686,
+            "heading": 0.3773492268804029,
+            "angularVelocity": 0.4119956454330042,
+            "velocityX": 0.05262773929482027,
+            "velocityY": 1.260534517079955,
+            "timestamp": 0.6077594287025412
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.393,
-            "angularVelocity": 0.20601269329644184,
-            "velocityX": 0.026313975540574065,
-            "velocityY": 0.6302668845130636,
-            "timestamp": 0.6837293572903638
+            "angularVelocity": 0.20601273965271094,
+            "velocityX": 0.026313813620245378,
+            "velocityY": 0.6302668901019914,
+            "timestamp": 0.6837293572903589
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.393,
-            "angularVelocity": 1.653751635278537e-33,
-            "velocityX": 2.0986524182646467e-33,
-            "velocityY": -2.349121999954591e-34,
-            "timestamp": 0.7596992858781819
+            "angularVelocity": 9.82651204832558e-34,
+            "velocityX": 0,
+            "velocityY": 3.8258931367738186e-36,
+            "timestamp": 0.7596992858781766
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.traj
index 1bd4bff0..35bba061 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.traj	
@@ -3,101 +3,101 @@
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -1.6570559277738772e-34,
-            "velocityY": -1.2609465104041817e-35,
+            "heading": -5.451838641793523e-34,
+            "angularVelocity": -2.0131697388730357e-33,
+            "velocityX": -3.0924959567962797e-34,
+            "velocityY": 4.789643826321005e-35,
             "timestamp": 0
         },
         {
-            "x": 2.25200199902585,
-            "y": 5.607878029194235,
-            "heading": 0.015821088838010276,
-            "angularVelocity": 0.20825462300812525,
-            "velocityX": 0.026352519517453685,
-            "velocityY": 0.6302234329322915,
-            "timestamp": 0.0759699285878182
+            "x": 2.2520020156777094,
+            "y": 5.6078780285533805,
+            "heading": 0.01582108665485387,
+            "angularVelocity": 0.208254594271014,
+            "velocityX": 0.026352738707599337,
+            "velocityY": 0.630223424496655,
+            "timestamp": 0.07596992858781765
         },
         {
-            "x": 2.2560055011308777,
-            "y": 5.703634196198671,
-            "heading": 0.04745824894413579,
-            "angularVelocity": 0.4164431992265759,
-            "velocityX": 0.05269851083774254,
-            "velocityY": 1.2604482955877103,
-            "timestamp": 0.1519398571756364
+            "x": 2.2560055471462697,
+            "y": 5.703634194455967,
+            "heading": 0.04745824180691435,
+            "angularVelocity": 0.4164431340157103,
+            "velocityX": 0.05269889735295001,
+            "velocityY": 1.2604482810839739,
+            "timestamp": 0.1519398571756353
         },
         {
-            "x": 2.2620098071430843,
-            "y": 5.847268765721161,
-            "heading": 0.09489872499810467,
-            "angularVelocity": 0.6244638758496344,
-            "velocityX": 0.07903529888495084,
-            "velocityY": 1.8906766426199095,
-            "timestamp": 0.2279097857634546
+            "x": 2.262009891384212,
+            "y": 5.847268762594484,
+            "heading": 0.09489870943903907,
+            "angularVelocity": 0.6244637649920359,
+            "velocityX": 0.07903580205425154,
+            "velocityY": 1.8906766244025437,
+            "timestamp": 0.22790978576345294
         },
         {
-            "x": 2.270014094994434,
-            "y": 6.038782268539179,
-            "heading": 0.1581161373911332,
-            "angularVelocity": 0.8321373149634034,
-            "velocityX": 0.10536126596587263,
-            "velocityY": 2.5209119763306935,
-            "timestamp": 0.3038797143512728
+            "x": 2.2700142224347877,
+            "y": 6.038782263911744,
+            "heading": 0.1581161098401003,
+            "angularVelocity": 0.8321371571119077,
+            "velocityX": 0.1053618346017344,
+            "velocityY": 2.5209119565760743,
+            "timestamp": 0.3038797143512706
         },
         {
-            "x": 2.28000939681014,
-            "y": 6.278187229695624,
-            "heading": 0.23645967758045489,
-            "angularVelocity": 1.031244094151802,
-            "velocityX": 0.13156918798668546,
-            "velocityY": 3.1513121784720814,
-            "timestamp": 0.379849642939091
+            "x": 2.280009491675263,
+            "y": 6.278187226323642,
+            "heading": 0.23645965423367718,
+            "angularVelocity": 1.0312441494928544,
+            "velocityX": 0.13156875919557368,
+            "velocityY": 3.151312194997739,
+            "timestamp": 0.37984964293908824
         },
         {
-            "x": 2.2880055947499205,
-            "y": 6.469712010307998,
-            "heading": 0.29909347070783976,
-            "angularVelocity": 0.824455074417803,
-            "velocityX": 0.10525477762609038,
-            "velocityY": 2.521060427100167,
-            "timestamp": 0.4558195715269092
+            "x": 2.288005657404111,
+            "y": 6.469712008115255,
+            "heading": 0.2990934539478391,
+            "angularVelocity": 0.8244551611202343,
+            "velocityX": 0.10525435363026732,
+            "velocityY": 2.52106044262263,
+            "timestamp": 0.4558195715269059
         },
         {
-            "x": 2.29400278172627,
-            "y": 6.613355953306298,
-            "heading": 0.3460499566805711,
-            "angularVelocity": 0.6180930645268612,
-            "velocityX": 0.07894159028222533,
-            "velocityY": 1.8908000266480898,
-            "timestamp": 0.5317895001147275
+            "x": 2.294002815862431,
+            "y": 6.613355952122642,
+            "heading": 0.3460499471183657,
+            "angularVelocity": 0.6180931592721867,
+            "velocityX": 0.07894121489645621,
+            "velocityY": 1.8908000399308182,
+            "timestamp": 0.5317895001147235
         },
         {
-            "x": 2.298000929157321,
-            "y": 6.709118669792275,
-            "heading": 0.37734923040208523,
-            "angularVelocity": 0.4119955659209739,
-            "velocityX": 0.052628026712297535,
-            "velocityY": 1.2605345070882814,
-            "timestamp": 0.6077594287025456
+            "x": 2.2980009414583966,
+            "y": 6.709118669367686,
+            "heading": 0.3773492268804029,
+            "angularVelocity": 0.4119956454330042,
+            "velocityX": 0.05262773929482027,
+            "velocityY": 1.260534517079955,
+            "timestamp": 0.6077594287025412
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.393,
-            "angularVelocity": 0.20601269329644184,
-            "velocityX": 0.026313975540574065,
-            "velocityY": 0.6302668845130636,
-            "timestamp": 0.6837293572903638
+            "angularVelocity": 0.20601273965271094,
+            "velocityX": 0.026313813620245378,
+            "velocityY": 0.6302668901019914,
+            "timestamp": 0.6837293572903589
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.393,
-            "angularVelocity": 1.653751635278537e-33,
-            "velocityX": 2.0986524182646467e-33,
-            "velocityY": -2.349121999954591e-34,
-            "timestamp": 0.7596992858781819
+            "angularVelocity": 9.82651204832558e-34,
+            "velocityX": 0,
+            "velocityY": 3.8258931367738186e-36,
+            "timestamp": 0.7596992858781766
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj
index a9c39200..f46e6706 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj	
@@ -3,101 +3,101 @@
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": -6.311192632400173e-33,
-            "angularVelocity": 0,
+            "heading": 0,
+            "angularVelocity": 5.7052362935559275e-34,
             "velocityX": 0,
-            "velocityY": 1.0022615189154172e-34,
+            "velocityY": -1.2494033787640354e-35,
             "timestamp": 0
         },
         {
-            "x": 2.2520019144790657,
-            "y": 5.512001876818988,
-            "heading": -0.015697245285858377,
-            "angularVelocity": -0.2063751026737087,
-            "velocityX": 0.02631960567841837,
-            "velocityY": -0.6310417795758498,
-            "timestamp": 0.07606172005484914
+            "x": 2.2520019447146944,
+            "y": 5.512001877264397,
+            "heading": -0.01569721301150974,
+            "angularVelocity": -0.20637467835572687,
+            "velocityX": 0.02632000319280478,
+            "velocityY": -0.6310417737196751,
+            "timestamp": 0.07606172005488261
         },
         {
-            "x": 2.256005269562975,
-            "y": 5.416005526715618,
-            "heading": -0.04708689462727081,
-            "angularVelocity": -0.41268655663817416,
-            "velocityX": 0.05263298122921632,
-            "velocityY": -1.2620849230617632,
-            "timestamp": 0.15212344010969828
+            "x": 2.2560053559202022,
+            "y": 5.416005527882177,
+            "heading": -0.04708679827781808,
+            "angularVelocity": -0.41268571422864314,
+            "velocityX": 0.05263371907208186,
+            "velocityY": -1.262084913580087,
+            "timestamp": 0.15212344010976522
         },
         {
-            "x": 2.26200939656581,
-            "y": 5.272010697248206,
-            "heading": -0.09415666260883829,
-            "angularVelocity": -0.6188364915705936,
-            "velocityX": 0.07893756542062844,
-            "velocityY": -1.8931313854535041,
-            "timestamp": 0.22818516016454743
+            "x": 2.262009560249172,
+            "y": 5.272010699235591,
+            "heading": -0.0941564711571504,
+            "angularVelocity": -0.6188352412405218,
+            "velocityX": 0.07893858204412474,
+            "velocityY": -1.8931313746610776,
+            "timestamp": 0.22818516016464785
         },
         {
-            "x": 2.2700135068749114,
-            "y": 5.080016881567182,
-            "heading": -0.15688110795154597,
-            "angularVelocity": -0.8246519444666291,
-            "velocityX": 0.10523178154963624,
-            "velocityY": -2.5241845115068955,
-            "timestamp": 0.30424688021939655
+            "x": 2.2700137636187,
+            "y": 5.080016884298785,
+            "heading": -0.15688079244309955,
+            "angularVelocity": -0.8246503134650409,
+            "velocityX": 0.10523300503528431,
+            "velocityY": -2.5241845017213995,
+            "timestamp": 0.30424688021953045
         },
         {
-            "x": 2.280008999433411,
-            "y": 4.840012166841925,
-            "heading": -0.23463480916086144,
-            "angularVelocity": -1.0222448447556298,
-            "velocityX": 0.13141291771066904,
-            "velocityY": -3.155394258139129,
-            "timestamp": 0.3803086002742457
+            "x": 2.2800092014745617,
+            "y": 4.840012168782865,
+            "heading": -0.23463455266722952,
+            "angularVelocity": -1.0222456206358,
+            "velocityX": 0.13141219852311892,
+            "velocityY": -3.155394268532757,
+            "timestamp": 0.38030860027441304
         },
         {
-            "x": 2.2880053509029836,
-            "y": 4.648007615057375,
-            "heading": -0.29679802026954394,
-            "angularVelocity": -0.8172732757536348,
-            "velocityX": 0.10512977439645693,
-            "velocityY": -2.524325661398308,
-            "timestamp": 0.4563703203290948
+            "x": 2.2880054914438257,
+            "y": 4.648007616430203,
+            "heading": -0.2967978427641432,
+            "angularVelocity": -0.8172743142287549,
+            "velocityX": 0.10512896583845424,
+            "velocityY": -2.524325668866294,
+            "timestamp": 0.45637032032929564
         },
         {
-            "x": 2.2940026546256975,
-            "y": 4.504003857619058,
-            "heading": -0.34340214900619426,
-            "angularVelocity": -0.6127146310002387,
-            "velocityX": 0.07884785827074504,
-            "velocityY": -1.893248763431504,
-            "timestamp": 0.532432040383944
+            "x": 2.2940027351611283,
+            "y": 4.5040038585170965,
+            "heading": -0.3434020516948695,
+            "angularVelocity": -0.6127156853289512,
+            "velocityX": 0.07884706936650254,
+            "velocityY": -1.8932487696728393,
+            "timestamp": 0.5324320403841782
         },
         {
-            "x": 2.2980008838862798,
-            "y": 4.408001268016137,
-            "heading": -0.37446665790253814,
-            "angularVelocity": -0.4084118643904297,
-            "velocityX": 0.05256559091352319,
-            "velocityY": -1.26216695512135,
-            "timestamp": 0.6084937604387931
+            "x": 2.2980009144821483,
+            "y": 4.408001268437703,
+            "heading": -0.37446662411182524,
+            "angularVelocity": -0.40841269950956927,
+            "velocityX": 0.05256493434720428,
+            "velocityY": -1.2621669613850792,
+            "timestamp": 0.6084937604390609
         },
         {
             "x": 2.3,
             "y": 4.36,
             "heading": -0.39,
-            "angularVelocity": -0.20422023175732293,
-            "velocityX": 0.02628281495972678,
-            "velocityY": -0.6310831254081802,
-            "timestamp": 0.6845554804936422
+            "angularVelocity": -0.20422067601109492,
+            "velocityX": 0.026282412709165117,
+            "velocityY": -0.6310831309503169,
+            "timestamp": 0.6845554804939435
         },
         {
             "x": 2.3,
             "y": 4.36,
             "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": -1.9690468515346402e-33,
-            "velocityY": 7.92544572453684e-31,
-            "timestamp": 0.7606172005484914
+            "angularVelocity": 8.777318885868316e-35,
+            "velocityX": -1.1404320857258046e-34,
+            "velocityY": 1.134511753128188e-36,
+            "timestamp": 0.7606172005488262
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.traj
index a9c39200..f46e6706 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.traj	
@@ -3,101 +3,101 @@
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": -6.311192632400173e-33,
-            "angularVelocity": 0,
+            "heading": 0,
+            "angularVelocity": 5.7052362935559275e-34,
             "velocityX": 0,
-            "velocityY": 1.0022615189154172e-34,
+            "velocityY": -1.2494033787640354e-35,
             "timestamp": 0
         },
         {
-            "x": 2.2520019144790657,
-            "y": 5.512001876818988,
-            "heading": -0.015697245285858377,
-            "angularVelocity": -0.2063751026737087,
-            "velocityX": 0.02631960567841837,
-            "velocityY": -0.6310417795758498,
-            "timestamp": 0.07606172005484914
+            "x": 2.2520019447146944,
+            "y": 5.512001877264397,
+            "heading": -0.01569721301150974,
+            "angularVelocity": -0.20637467835572687,
+            "velocityX": 0.02632000319280478,
+            "velocityY": -0.6310417737196751,
+            "timestamp": 0.07606172005488261
         },
         {
-            "x": 2.256005269562975,
-            "y": 5.416005526715618,
-            "heading": -0.04708689462727081,
-            "angularVelocity": -0.41268655663817416,
-            "velocityX": 0.05263298122921632,
-            "velocityY": -1.2620849230617632,
-            "timestamp": 0.15212344010969828
+            "x": 2.2560053559202022,
+            "y": 5.416005527882177,
+            "heading": -0.04708679827781808,
+            "angularVelocity": -0.41268571422864314,
+            "velocityX": 0.05263371907208186,
+            "velocityY": -1.262084913580087,
+            "timestamp": 0.15212344010976522
         },
         {
-            "x": 2.26200939656581,
-            "y": 5.272010697248206,
-            "heading": -0.09415666260883829,
-            "angularVelocity": -0.6188364915705936,
-            "velocityX": 0.07893756542062844,
-            "velocityY": -1.8931313854535041,
-            "timestamp": 0.22818516016454743
+            "x": 2.262009560249172,
+            "y": 5.272010699235591,
+            "heading": -0.0941564711571504,
+            "angularVelocity": -0.6188352412405218,
+            "velocityX": 0.07893858204412474,
+            "velocityY": -1.8931313746610776,
+            "timestamp": 0.22818516016464785
         },
         {
-            "x": 2.2700135068749114,
-            "y": 5.080016881567182,
-            "heading": -0.15688110795154597,
-            "angularVelocity": -0.8246519444666291,
-            "velocityX": 0.10523178154963624,
-            "velocityY": -2.5241845115068955,
-            "timestamp": 0.30424688021939655
+            "x": 2.2700137636187,
+            "y": 5.080016884298785,
+            "heading": -0.15688079244309955,
+            "angularVelocity": -0.8246503134650409,
+            "velocityX": 0.10523300503528431,
+            "velocityY": -2.5241845017213995,
+            "timestamp": 0.30424688021953045
         },
         {
-            "x": 2.280008999433411,
-            "y": 4.840012166841925,
-            "heading": -0.23463480916086144,
-            "angularVelocity": -1.0222448447556298,
-            "velocityX": 0.13141291771066904,
-            "velocityY": -3.155394258139129,
-            "timestamp": 0.3803086002742457
+            "x": 2.2800092014745617,
+            "y": 4.840012168782865,
+            "heading": -0.23463455266722952,
+            "angularVelocity": -1.0222456206358,
+            "velocityX": 0.13141219852311892,
+            "velocityY": -3.155394268532757,
+            "timestamp": 0.38030860027441304
         },
         {
-            "x": 2.2880053509029836,
-            "y": 4.648007615057375,
-            "heading": -0.29679802026954394,
-            "angularVelocity": -0.8172732757536348,
-            "velocityX": 0.10512977439645693,
-            "velocityY": -2.524325661398308,
-            "timestamp": 0.4563703203290948
+            "x": 2.2880054914438257,
+            "y": 4.648007616430203,
+            "heading": -0.2967978427641432,
+            "angularVelocity": -0.8172743142287549,
+            "velocityX": 0.10512896583845424,
+            "velocityY": -2.524325668866294,
+            "timestamp": 0.45637032032929564
         },
         {
-            "x": 2.2940026546256975,
-            "y": 4.504003857619058,
-            "heading": -0.34340214900619426,
-            "angularVelocity": -0.6127146310002387,
-            "velocityX": 0.07884785827074504,
-            "velocityY": -1.893248763431504,
-            "timestamp": 0.532432040383944
+            "x": 2.2940027351611283,
+            "y": 4.5040038585170965,
+            "heading": -0.3434020516948695,
+            "angularVelocity": -0.6127156853289512,
+            "velocityX": 0.07884706936650254,
+            "velocityY": -1.8932487696728393,
+            "timestamp": 0.5324320403841782
         },
         {
-            "x": 2.2980008838862798,
-            "y": 4.408001268016137,
-            "heading": -0.37446665790253814,
-            "angularVelocity": -0.4084118643904297,
-            "velocityX": 0.05256559091352319,
-            "velocityY": -1.26216695512135,
-            "timestamp": 0.6084937604387931
+            "x": 2.2980009144821483,
+            "y": 4.408001268437703,
+            "heading": -0.37446662411182524,
+            "angularVelocity": -0.40841269950956927,
+            "velocityX": 0.05256493434720428,
+            "velocityY": -1.2621669613850792,
+            "timestamp": 0.6084937604390609
         },
         {
             "x": 2.3,
             "y": 4.36,
             "heading": -0.39,
-            "angularVelocity": -0.20422023175732293,
-            "velocityX": 0.02628281495972678,
-            "velocityY": -0.6310831254081802,
-            "timestamp": 0.6845554804936422
+            "angularVelocity": -0.20422067601109492,
+            "velocityX": 0.026282412709165117,
+            "velocityY": -0.6310831309503169,
+            "timestamp": 0.6845554804939435
         },
         {
             "x": 2.3,
             "y": 4.36,
             "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": -1.9690468515346402e-33,
-            "velocityY": 7.92544572453684e-31,
-            "timestamp": 0.7606172005484914
+            "angularVelocity": 8.777318885868316e-35,
+            "velocityX": -1.1404320857258046e-34,
+            "velocityY": 1.134511753128188e-36,
+            "timestamp": 0.7606172005488262
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.1.traj
index 84a9867e..714e9458 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.1.traj	
@@ -4,172 +4,172 @@
             "x": 2.3,
             "y": 4.36,
             "heading": -0.39,
-            "angularVelocity": 8.834598343952546e-30,
-            "velocityX": -1.1855672444586933e-30,
-            "velocityY": 4.732277944627028e-32,
+            "angularVelocity": 5.020065556819226e-31,
+            "velocityX": -2.911156118414847e-31,
+            "velocityY": 8.635491730351355e-32,
             "timestamp": 0
         },
         {
-            "x": 2.3221690172235006,
-            "y": 4.411229758092553,
-            "heading": -0.4048175852865869,
-            "angularVelocity": -0.18084977247894177,
-            "velocityX": 0.27057456686796094,
-            "velocityY": 0.6252631529349166,
-            "timestamp": 0.08193311544440174
-        },
-        {
-            "x": 2.3687303268224738,
-            "y": 4.512732101891575,
-            "heading": -0.43137596230210584,
-            "angularVelocity": -0.3241470420293335,
-            "velocityX": 0.5682843786231674,
-            "velocityY": 1.2388439429951275,
-            "timestamp": 0.16386623088880348
-        },
-        {
-            "x": 2.4431953900618826,
-            "y": 4.662720774571155,
-            "heading": -0.464675218076173,
-            "angularVelocity": -0.40641998773593985,
-            "velocityX": 0.9088518462346009,
-            "velocityY": 1.8306233305795314,
-            "timestamp": 0.24579934633320522
-        },
-        {
-            "x": 2.551739981424586,
-            "y": 4.8571070042037645,
-            "heading": -0.49532439278782164,
-            "angularVelocity": -0.3740755437574734,
-            "velocityX": 1.3247951182371547,
-            "velocityY": 2.3724989410968234,
-            "timestamp": 0.32773246177760695
-        },
-        {
-            "x": 2.7012931711639805,
-            "y": 5.089437783878959,
-            "heading": -0.5138025644966426,
-            "angularVelocity": -0.22552751239337346,
-            "velocityX": 1.8253082276711035,
-            "velocityY": 2.835615104039969,
-            "timestamp": 0.4096655772220087
-        },
-        {
-            "x": 2.906580390144238,
-            "y": 5.321655181575939,
-            "heading": -0.513802224550241,
-            "angularVelocity": 0.0000041490720786488115,
-            "velocityX": 2.5055463577429817,
-            "velocityY": 2.834231268241715,
-            "timestamp": 0.49159869266641043
-        },
-        {
-            "x": 3.150285117410734,
-            "y": 5.513164499731697,
-            "heading": -0.5138021244683586,
-            "angularVelocity": 0.0000012215071026742433,
-            "velocityX": 2.974435012566654,
-            "velocityY": 2.3373860143974894,
-            "timestamp": 0.5735318081108122
-        },
-        {
-            "x": 3.407534970443545,
-            "y": 5.686052692352735,
-            "heading": -0.513802030650944,
-            "angularVelocity": 0.0000011450487881316094,
-            "velocityX": 3.1397543183546524,
-            "velocityY": 2.110113739521574,
-            "timestamp": 0.6554649235552139
-        },
-        {
-            "x": 3.664784970537928,
-            "y": 5.858940666152602,
-            "heading": -0.5138019368336011,
-            "angularVelocity": 0.0000011450479143905032,
-            "velocityX": 3.1397561132524885,
-            "velocityY": 2.1101110687922637,
-            "timestamp": 0.7373980389996156
-        },
-        {
-            "x": 3.922035018762957,
-            "y": 6.031828568336093,
-            "heading": -0.5138018430162984,
-            "angularVelocity": 0.0000011450474228928023,
-            "velocityX": 3.139756700690747,
-            "velocityY": 2.110110194708886,
-            "timestamp": 0.8193311544440174
-        },
-        {
-            "x": 4.1792852160642955,
-            "y": 6.204716248700005,
-            "heading": -0.5138017491990241,
-            "angularVelocity": 0.0000011450470770765712,
-            "velocityX": 3.1397585201786073,
-            "velocityY": 2.1101074873837833,
-            "timestamp": 0.9012642698884191
-        },
-        {
-            "x": 4.443584051036782,
-            "y": 6.354085007666646,
-            "heading": -0.49783246108316886,
-            "angularVelocity": 0.19490639443210175,
-            "velocityX": 3.225787687175579,
-            "velocityY": 1.8230572358499022,
-            "timestamp": 0.9831973853328209
-        },
-        {
-            "x": 4.674708985410259,
-            "y": 6.469317132198814,
-            "heading": -0.3712043024716245,
-            "angularVelocity": 1.545506452729414,
-            "velocityX": 2.8208976690299576,
-            "velocityY": 1.4064169769104102,
-            "timestamp": 1.0651305007772227
-        },
-        {
-            "x": 4.870816846139177,
-            "y": 6.548974010532027,
-            "heading": -0.15522222327575852,
-            "angularVelocity": 2.636077952417564,
-            "velocityX": 2.393511581553284,
-            "velocityY": 0.9722183503111966,
-            "timestamp": 1.1470636162216246
-        },
-        {
-            "x": 5.022935939261114,
-            "y": 6.594453280726005,
-            "heading": 0.041233837030506995,
-            "angularVelocity": 2.3977613842791428,
-            "velocityX": 1.856625276566729,
-            "velocityY": 0.5550780041415454,
-            "timestamp": 1.2289967316660264
-        },
-        {
-            "x": 5.124959991715153,
-            "y": 6.619307482178149,
-            "heading": 0.1862409016069847,
-            "angularVelocity": 1.7698224190545027,
-            "velocityX": 1.245211437410438,
-            "velocityY": 0.30334744769969435,
-            "timestamp": 1.3109298471104283
+            "x": 2.3221690164859408,
+            "y": 4.411229757983623,
+            "heading": -0.4048176043167742,
+            "angularVelocity": -0.1808500047435309,
+            "velocityX": 0.27057455786553103,
+            "velocityY": 0.6252631516043967,
+            "timestamp": 0.08193311544453755
+        },
+        {
+            "x": 2.3687303275118676,
+            "y": 4.5127321003230465,
+            "heading": -0.4313760168590284,
+            "angularVelocity": -0.32414747563532126,
+            "velocityX": 0.5682843960383053,
+            "velocityY": 1.2388439251785637,
+            "timestamp": 0.1638662308890751
+        },
+        {
+            "x": 2.4431953998066236,
+            "y": 4.662720767025029,
+            "heading": -0.4646753197408115,
+            "angularVelocity": -0.40642056268856935,
+            "velocityX": 0.9088519567543129,
+            "velocityY": 1.830623257619443,
+            "timestamp": 0.24579934633361264
+        },
+        {
+            "x": 2.551740018878988,
+            "y": 4.857106976368847,
+            "heading": -0.4953245435557511,
+            "angularVelocity": -0.3740761430663416,
+            "velocityX": 1.3247954564335083,
+            "velocityY": 2.3724986934666186,
+            "timestamp": 0.3277324617781502
+        },
+        {
+            "x": 2.701293241756868,
+            "y": 5.089437731072216,
+            "heading": -0.5138028115710582,
+            "angularVelocity": -0.2255286878210994,
+            "velocityX": 1.8253086321258618,
+            "velocityY": 2.835614799252222,
+            "timestamp": 0.4096655772226877
+        },
+        {
+            "x": 2.9065804913658706,
+            "y": 5.321655101693131,
+            "heading": -0.5138024716246538,
+            "angularVelocity": 0.000004149072112901325,
+            "velocityX": 2.5055467315650244,
+            "velocityY": 2.8342309377715424,
+            "timestamp": 0.4915986926672252
+        },
+        {
+            "x": 3.1502852438919966,
+            "y": 5.513164387705616,
+            "heading": -0.5138023715427595,
+            "angularVelocity": 0.0000012215072474193347,
+            "velocityX": 2.974435320857471,
+            "velocityY": 2.3373856220824885,
+            "timestamp": 0.5735318081117627
+        },
+        {
+            "x": 3.4075351073224858,
+            "y": 5.686052564856294,
+            "heading": -0.5138022777253332,
+            "angularVelocity": 0.0000011450489327031063,
+            "velocityX": 3.1397544452539083,
+            "velocityY": 2.110113550701131,
+            "timestamp": 0.6554649235563003
+        },
+        {
+            "x": 3.664785106053728,
+            "y": 5.858940540685368,
+            "heading": -0.5138021839079745,
+            "angularVelocity": 0.0000011450481052658978,
+            "velocityX": 3.13975609661006,
+            "velocityY": 2.110111093555414,
+            "timestamp": 0.7373980390008378
+        },
+        {
+            "x": 3.922035141067721,
+            "y": 6.031828462527248,
+            "heading": -0.5138020900906538,
+            "angularVelocity": 0.000001145047641454271,
+            "velocityX": 3.1397565394438236,
+            "velocityY": 2.1101104346375243,
+            "timestamp": 0.8193311544453753
+        },
+        {
+            "x": 4.179285313417939,
+            "y": 6.204716180018366,
+            "heading": -0.5138019962733545,
+            "angularVelocity": 0.00000114504738127865,
+            "velocityX": 3.139758215643051,
+            "velocityY": 2.110107940520708,
+            "timestamp": 0.9012642698899128
+        },
+        {
+            "x": 4.443584109358565,
+            "y": 6.354084975447565,
+            "heading": -0.4978326665645833,
+            "angularVelocity": 0.19490690207652947,
+            "velocityX": 3.225787210783358,
+            "velocityY": 1.8230576808752068,
+            "timestamp": 0.9831973853344503
+        },
+        {
+            "x": 4.674709010100242,
+            "y": 6.4693171293955105,
+            "heading": -0.3712044647358083,
+            "angularVelocity": 1.5455069801964878,
+            "velocityX": 2.8208972585465766,
+            "velocityY": 1.406417335929929,
+            "timestamp": 1.065130500778988
+        },
+        {
+            "x": 4.870816853917273,
+            "y": 6.548974021375514,
+            "heading": -0.15522232897542676,
+            "angularVelocity": 2.636078642787472,
+            "velocityX": 2.3935113751384223,
+            "velocityY": 0.9722185168697003,
+            "timestamp": 1.1470636162235255
+        },
+        {
+            "x": 5.022935942406859,
+            "y": 6.594453289671109,
+            "heading": 0.041233782182786366,
+            "angularVelocity": 2.3977620049271122,
+            "velocityX": 1.856625220025449,
+            "velocityY": 0.5550779809707214,
+            "timestamp": 1.228996731668063
+        },
+        {
+            "x": 5.124959992690689,
+            "y": 6.619307485763033,
+            "heading": 0.18624088374392397,
+            "angularVelocity": 1.769822870452196,
+            "velocityX": 1.2452114109208137,
+            "velocityY": 0.3033473822772884,
+            "timestamp": 1.3109298471126005
         },
         {
             "x": 5.176,
             "y": 6.63,
             "heading": 0.263,
-            "angularVelocity": 0.9368507224054672,
-            "velocityX": 0.6229472418814781,
-            "velocityY": 0.13050300557783806,
-            "timestamp": 1.39286296255483
+            "angularVelocity": 0.9368509404239479,
+            "velocityX": 0.6229472299739557,
+            "velocityY": 0.13050296182384175,
+            "timestamp": 1.392862962557138
         },
         {
             "x": 5.176,
             "y": 6.63,
             "heading": 0.263,
-            "angularVelocity": 1.6488266469177426e-31,
+            "angularVelocity": 1.5668772442975024e-30,
             "velocityX": 0,
-            "velocityY": 1.9617182542723147e-31,
-            "timestamp": 1.474796077999232
+            "velocityY": -2.8584411322391187e-31,
+            "timestamp": 1.4747960780016756
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.traj
index 84a9867e..714e9458 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.traj	
@@ -4,172 +4,172 @@
             "x": 2.3,
             "y": 4.36,
             "heading": -0.39,
-            "angularVelocity": 8.834598343952546e-30,
-            "velocityX": -1.1855672444586933e-30,
-            "velocityY": 4.732277944627028e-32,
+            "angularVelocity": 5.020065556819226e-31,
+            "velocityX": -2.911156118414847e-31,
+            "velocityY": 8.635491730351355e-32,
             "timestamp": 0
         },
         {
-            "x": 2.3221690172235006,
-            "y": 4.411229758092553,
-            "heading": -0.4048175852865869,
-            "angularVelocity": -0.18084977247894177,
-            "velocityX": 0.27057456686796094,
-            "velocityY": 0.6252631529349166,
-            "timestamp": 0.08193311544440174
-        },
-        {
-            "x": 2.3687303268224738,
-            "y": 4.512732101891575,
-            "heading": -0.43137596230210584,
-            "angularVelocity": -0.3241470420293335,
-            "velocityX": 0.5682843786231674,
-            "velocityY": 1.2388439429951275,
-            "timestamp": 0.16386623088880348
-        },
-        {
-            "x": 2.4431953900618826,
-            "y": 4.662720774571155,
-            "heading": -0.464675218076173,
-            "angularVelocity": -0.40641998773593985,
-            "velocityX": 0.9088518462346009,
-            "velocityY": 1.8306233305795314,
-            "timestamp": 0.24579934633320522
-        },
-        {
-            "x": 2.551739981424586,
-            "y": 4.8571070042037645,
-            "heading": -0.49532439278782164,
-            "angularVelocity": -0.3740755437574734,
-            "velocityX": 1.3247951182371547,
-            "velocityY": 2.3724989410968234,
-            "timestamp": 0.32773246177760695
-        },
-        {
-            "x": 2.7012931711639805,
-            "y": 5.089437783878959,
-            "heading": -0.5138025644966426,
-            "angularVelocity": -0.22552751239337346,
-            "velocityX": 1.8253082276711035,
-            "velocityY": 2.835615104039969,
-            "timestamp": 0.4096655772220087
-        },
-        {
-            "x": 2.906580390144238,
-            "y": 5.321655181575939,
-            "heading": -0.513802224550241,
-            "angularVelocity": 0.0000041490720786488115,
-            "velocityX": 2.5055463577429817,
-            "velocityY": 2.834231268241715,
-            "timestamp": 0.49159869266641043
-        },
-        {
-            "x": 3.150285117410734,
-            "y": 5.513164499731697,
-            "heading": -0.5138021244683586,
-            "angularVelocity": 0.0000012215071026742433,
-            "velocityX": 2.974435012566654,
-            "velocityY": 2.3373860143974894,
-            "timestamp": 0.5735318081108122
-        },
-        {
-            "x": 3.407534970443545,
-            "y": 5.686052692352735,
-            "heading": -0.513802030650944,
-            "angularVelocity": 0.0000011450487881316094,
-            "velocityX": 3.1397543183546524,
-            "velocityY": 2.110113739521574,
-            "timestamp": 0.6554649235552139
-        },
-        {
-            "x": 3.664784970537928,
-            "y": 5.858940666152602,
-            "heading": -0.5138019368336011,
-            "angularVelocity": 0.0000011450479143905032,
-            "velocityX": 3.1397561132524885,
-            "velocityY": 2.1101110687922637,
-            "timestamp": 0.7373980389996156
-        },
-        {
-            "x": 3.922035018762957,
-            "y": 6.031828568336093,
-            "heading": -0.5138018430162984,
-            "angularVelocity": 0.0000011450474228928023,
-            "velocityX": 3.139756700690747,
-            "velocityY": 2.110110194708886,
-            "timestamp": 0.8193311544440174
-        },
-        {
-            "x": 4.1792852160642955,
-            "y": 6.204716248700005,
-            "heading": -0.5138017491990241,
-            "angularVelocity": 0.0000011450470770765712,
-            "velocityX": 3.1397585201786073,
-            "velocityY": 2.1101074873837833,
-            "timestamp": 0.9012642698884191
-        },
-        {
-            "x": 4.443584051036782,
-            "y": 6.354085007666646,
-            "heading": -0.49783246108316886,
-            "angularVelocity": 0.19490639443210175,
-            "velocityX": 3.225787687175579,
-            "velocityY": 1.8230572358499022,
-            "timestamp": 0.9831973853328209
-        },
-        {
-            "x": 4.674708985410259,
-            "y": 6.469317132198814,
-            "heading": -0.3712043024716245,
-            "angularVelocity": 1.545506452729414,
-            "velocityX": 2.8208976690299576,
-            "velocityY": 1.4064169769104102,
-            "timestamp": 1.0651305007772227
-        },
-        {
-            "x": 4.870816846139177,
-            "y": 6.548974010532027,
-            "heading": -0.15522222327575852,
-            "angularVelocity": 2.636077952417564,
-            "velocityX": 2.393511581553284,
-            "velocityY": 0.9722183503111966,
-            "timestamp": 1.1470636162216246
-        },
-        {
-            "x": 5.022935939261114,
-            "y": 6.594453280726005,
-            "heading": 0.041233837030506995,
-            "angularVelocity": 2.3977613842791428,
-            "velocityX": 1.856625276566729,
-            "velocityY": 0.5550780041415454,
-            "timestamp": 1.2289967316660264
-        },
-        {
-            "x": 5.124959991715153,
-            "y": 6.619307482178149,
-            "heading": 0.1862409016069847,
-            "angularVelocity": 1.7698224190545027,
-            "velocityX": 1.245211437410438,
-            "velocityY": 0.30334744769969435,
-            "timestamp": 1.3109298471104283
+            "x": 2.3221690164859408,
+            "y": 4.411229757983623,
+            "heading": -0.4048176043167742,
+            "angularVelocity": -0.1808500047435309,
+            "velocityX": 0.27057455786553103,
+            "velocityY": 0.6252631516043967,
+            "timestamp": 0.08193311544453755
+        },
+        {
+            "x": 2.3687303275118676,
+            "y": 4.5127321003230465,
+            "heading": -0.4313760168590284,
+            "angularVelocity": -0.32414747563532126,
+            "velocityX": 0.5682843960383053,
+            "velocityY": 1.2388439251785637,
+            "timestamp": 0.1638662308890751
+        },
+        {
+            "x": 2.4431953998066236,
+            "y": 4.662720767025029,
+            "heading": -0.4646753197408115,
+            "angularVelocity": -0.40642056268856935,
+            "velocityX": 0.9088519567543129,
+            "velocityY": 1.830623257619443,
+            "timestamp": 0.24579934633361264
+        },
+        {
+            "x": 2.551740018878988,
+            "y": 4.857106976368847,
+            "heading": -0.4953245435557511,
+            "angularVelocity": -0.3740761430663416,
+            "velocityX": 1.3247954564335083,
+            "velocityY": 2.3724986934666186,
+            "timestamp": 0.3277324617781502
+        },
+        {
+            "x": 2.701293241756868,
+            "y": 5.089437731072216,
+            "heading": -0.5138028115710582,
+            "angularVelocity": -0.2255286878210994,
+            "velocityX": 1.8253086321258618,
+            "velocityY": 2.835614799252222,
+            "timestamp": 0.4096655772226877
+        },
+        {
+            "x": 2.9065804913658706,
+            "y": 5.321655101693131,
+            "heading": -0.5138024716246538,
+            "angularVelocity": 0.000004149072112901325,
+            "velocityX": 2.5055467315650244,
+            "velocityY": 2.8342309377715424,
+            "timestamp": 0.4915986926672252
+        },
+        {
+            "x": 3.1502852438919966,
+            "y": 5.513164387705616,
+            "heading": -0.5138023715427595,
+            "angularVelocity": 0.0000012215072474193347,
+            "velocityX": 2.974435320857471,
+            "velocityY": 2.3373856220824885,
+            "timestamp": 0.5735318081117627
+        },
+        {
+            "x": 3.4075351073224858,
+            "y": 5.686052564856294,
+            "heading": -0.5138022777253332,
+            "angularVelocity": 0.0000011450489327031063,
+            "velocityX": 3.1397544452539083,
+            "velocityY": 2.110113550701131,
+            "timestamp": 0.6554649235563003
+        },
+        {
+            "x": 3.664785106053728,
+            "y": 5.858940540685368,
+            "heading": -0.5138021839079745,
+            "angularVelocity": 0.0000011450481052658978,
+            "velocityX": 3.13975609661006,
+            "velocityY": 2.110111093555414,
+            "timestamp": 0.7373980390008378
+        },
+        {
+            "x": 3.922035141067721,
+            "y": 6.031828462527248,
+            "heading": -0.5138020900906538,
+            "angularVelocity": 0.000001145047641454271,
+            "velocityX": 3.1397565394438236,
+            "velocityY": 2.1101104346375243,
+            "timestamp": 0.8193311544453753
+        },
+        {
+            "x": 4.179285313417939,
+            "y": 6.204716180018366,
+            "heading": -0.5138019962733545,
+            "angularVelocity": 0.00000114504738127865,
+            "velocityX": 3.139758215643051,
+            "velocityY": 2.110107940520708,
+            "timestamp": 0.9012642698899128
+        },
+        {
+            "x": 4.443584109358565,
+            "y": 6.354084975447565,
+            "heading": -0.4978326665645833,
+            "angularVelocity": 0.19490690207652947,
+            "velocityX": 3.225787210783358,
+            "velocityY": 1.8230576808752068,
+            "timestamp": 0.9831973853344503
+        },
+        {
+            "x": 4.674709010100242,
+            "y": 6.4693171293955105,
+            "heading": -0.3712044647358083,
+            "angularVelocity": 1.5455069801964878,
+            "velocityX": 2.8208972585465766,
+            "velocityY": 1.406417335929929,
+            "timestamp": 1.065130500778988
+        },
+        {
+            "x": 4.870816853917273,
+            "y": 6.548974021375514,
+            "heading": -0.15522232897542676,
+            "angularVelocity": 2.636078642787472,
+            "velocityX": 2.3935113751384223,
+            "velocityY": 0.9722185168697003,
+            "timestamp": 1.1470636162235255
+        },
+        {
+            "x": 5.022935942406859,
+            "y": 6.594453289671109,
+            "heading": 0.041233782182786366,
+            "angularVelocity": 2.3977620049271122,
+            "velocityX": 1.856625220025449,
+            "velocityY": 0.5550779809707214,
+            "timestamp": 1.228996731668063
+        },
+        {
+            "x": 5.124959992690689,
+            "y": 6.619307485763033,
+            "heading": 0.18624088374392397,
+            "angularVelocity": 1.769822870452196,
+            "velocityX": 1.2452114109208137,
+            "velocityY": 0.3033473822772884,
+            "timestamp": 1.3109298471126005
         },
         {
             "x": 5.176,
             "y": 6.63,
             "heading": 0.263,
-            "angularVelocity": 0.9368507224054672,
-            "velocityX": 0.6229472418814781,
-            "velocityY": 0.13050300557783806,
-            "timestamp": 1.39286296255483
+            "angularVelocity": 0.9368509404239479,
+            "velocityX": 0.6229472299739557,
+            "velocityY": 0.13050296182384175,
+            "timestamp": 1.392862962557138
         },
         {
             "x": 5.176,
             "y": 6.63,
             "heading": 0.263,
-            "angularVelocity": 1.6488266469177426e-31,
+            "angularVelocity": 1.5668772442975024e-30,
             "velocityX": 0,
-            "velocityY": 1.9617182542723147e-31,
-            "timestamp": 1.474796077999232
+            "velocityY": -2.8584411322391187e-31,
+            "timestamp": 1.4747960780016756
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.1.traj
index 06a0b427..72f3f88a 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.1.traj	
@@ -5,126 +5,126 @@
             "y": 4.36,
             "heading": -0.39,
             "angularVelocity": 0,
-            "velocityX": -1.8814689011962518e-30,
-            "velocityY": -8.215910483342913e-30,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 2.3223907686912884,
-            "y": 4.418261357303619,
-            "heading": -0.4292195342409252,
-            "angularVelocity": -0.4499474209247965,
-            "velocityX": 0.25687884418209317,
-            "velocityY": 0.6684053741511637,
-            "timestamp": 0.08716470506779908
+            "x": 2.3223907681513127,
+            "y": 4.418261357040706,
+            "heading": -0.429219543622513,
+            "angularVelocity": -0.4499475285553182,
+            "velocityX": 0.2568788379871729,
+            "velocityY": 0.6684053711347925,
+            "timestamp": 0.08716470506781053
         },
         {
-            "x": 2.37490967527786,
-            "y": 4.531319644696227,
-            "heading": -0.5050759172241152,
-            "angularVelocity": -0.8702648959138656,
-            "velocityX": 0.6025249158558101,
-            "velocityY": 1.2970649909807879,
-            "timestamp": 0.17432941013559816
+            "x": 2.374909672838796,
+            "y": 4.531319644218887,
+            "heading": -0.5050759499486299,
+            "angularVelocity": -0.8702651637162607,
+            "velocityX": 0.6025248940683728,
+            "velocityY": 1.297064988520603,
+            "timestamp": 0.17432941013562106
         },
         {
-            "x": 2.4692607215165214,
-            "y": 4.691180973762412,
-            "heading": -0.6116326818995184,
-            "angularVelocity": -1.222476053725161,
-            "velocityX": 1.0824455399151758,
-            "velocityY": 1.8340144550691926,
-            "timestamp": 0.26149411520339727
+            "x": 2.469260718438912,
+            "y": 4.691180971030917,
+            "heading": -0.6116327562753044,
+            "angularVelocity": -1.2224765315706085,
+            "velocityX": 1.082445532589298,
+            "velocityY": 1.834014429208093,
+            "timestamp": 0.2614941152034316
         },
         {
-            "x": 2.6203318015659547,
-            "y": 4.878827771954509,
-            "heading": -0.7357227336999919,
-            "angularVelocity": -1.4236272778523056,
-            "velocityX": 1.7331680286410147,
-            "velocityY": 2.1527841807775245,
-            "timestamp": 0.3486588202711963
+            "x": 2.620331804373909,
+            "y": 4.8788277526908095,
+            "heading": -0.7357228691689549,
+            "angularVelocity": -1.4236279787456398,
+            "velocityX": 1.7331680961631113,
+            "velocityY": 2.1527839911109723,
+            "timestamp": 0.3486588202712421
         },
         {
-            "x": 2.8323811449649123,
-            "y": 5.083303607446834,
-            "heading": -0.8475943342447533,
-            "angularVelocity": -1.2834506863500095,
-            "velocityX": 2.432743198454233,
-            "velocityY": 2.3458558751879917,
-            "timestamp": 0.4358235253389954
+            "x": 2.832381156144981,
+            "y": 5.083303561006889,
+            "heading": -0.84759450212177,
+            "angularVelocity": -1.2834510581523242,
+            "velocityX": 2.432743294503276,
+            "velocityY": 2.3458555634072913,
+            "timestamp": 0.43582352533905266
         },
         {
-            "x": 3.0986117768734247,
-            "y": 5.267387942263234,
-            "heading": -0.8666588377121885,
-            "angularVelocity": -0.2187181549298817,
-            "velocityX": 3.054339846632081,
-            "velocityY": 2.111913700312692,
-            "timestamp": 0.5229882304067944
+            "x": 3.098611796220168,
+            "y": 5.267387874253087,
+            "heading": -0.8666590247194526,
+            "angularVelocity": -0.21871837440225492,
+            "velocityX": 3.0543399403241303,
+            "velocityY": 2.111913452847551,
+            "timestamp": 0.5229882304068632
         },
         {
-            "x": 3.3974594561484373,
-            "y": 5.39777666903363,
-            "heading": -0.8564935513214242,
-            "angularVelocity": 0.11662158878249514,
-            "velocityX": 3.428540015623993,
-            "velocityY": 1.4958890375292995,
-            "timestamp": 0.6101529354745935
+            "x": 3.3974595011437105,
+            "y": 5.397776588765608,
+            "heading": -0.8564937886773937,
+            "angularVelocity": 0.11662101115527058,
+            "velocityX": 3.42854030987716,
+            "velocityY": 1.4958888969002289,
+            "timestamp": 0.6101529354746736
         },
         {
-            "x": 3.678181440932401,
-            "y": 5.472509264787258,
-            "heading": -0.7581228056155188,
-            "angularVelocity": 1.1285616767634317,
-            "velocityX": 3.2205923781375785,
-            "velocityY": 0.8573722092617639,
-            "timestamp": 0.6973176405423925
+            "x": 3.678181484629676,
+            "y": 5.472509187071852,
+            "heading": -0.7581230365630254,
+            "angularVelocity": 1.1285617502845884,
+            "velocityX": 3.220592363245829,
+            "velocityY": 0.8573722385466142,
+            "timestamp": 0.6973176405424841
         },
         {
-            "x": 3.9150331223150032,
-            "y": 5.514676359956983,
-            "heading": -0.5395207483783628,
-            "angularVelocity": 2.507919427560975,
-            "velocityX": 2.7172888521606593,
-            "velocityY": 0.483763412460658,
-            "timestamp": 0.7844823456101916
+            "x": 3.915033154173224,
+            "y": 5.514676302538828,
+            "heading": -0.5395209628894968,
+            "angularVelocity": 2.5079196161274817,
+            "velocityX": 2.7172887163363586,
+            "velocityY": 0.4837636453214852,
+            "timestamp": 0.7844823456102946
         },
         {
-            "x": 4.091189365639749,
-            "y": 5.539762978420037,
-            "heading": -0.2990438366325736,
-            "angularVelocity": 2.758879428992955,
-            "velocityX": 2.0209584049843006,
-            "velocityY": 0.28780707103340436,
-            "timestamp": 0.8716470506779906
+            "x": 4.091189385673594,
+            "y": 5.53976294658506,
+            "heading": -0.29904398170320484,
+            "angularVelocity": 2.7588802256510867,
+            "velocityX": 2.0209582693284847,
+            "velocityY": 0.2878073645372324,
+            "timestamp": 0.8716470506781051
         },
         {
-            "x": 4.206412444969991,
-            "y": 5.553849615471576,
-            "heading": -0.10702573317574342,
-            "angularVelocity": 2.202934126920672,
-            "velocityX": 1.3219006390329475,
-            "velocityY": 0.16160941565260478,
-            "timestamp": 0.9588117557457897
+            "x": 4.2064124532392215,
+            "y": 5.553849604220975,
+            "heading": -0.10702579424844161,
+            "angularVelocity": 2.2029350905894844,
+            "velocityX": 1.3219005040628353,
+            "velocityY": 0.161609651807531,
+            "timestamp": 0.9588117557459156
         },
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 1.2278563105616622,
-            "velocityX": 0.6492026214738336,
-            "velocityY": 0.07056049261727199,
-            "timestamp": 1.0459764608135889
+            "heading": 2.9287791201697906e-29,
+            "angularVelocity": 1.2278570112200775,
+            "velocityX": 0.6492025266047246,
+            "velocityY": 0.0705606216901654,
+            "timestamp": 1.045976460813726
         },
         {
             "x": 4.263,
             "y": 5.56,
             "heading": 0,
             "angularVelocity": 0,
-            "velocityX": 1.8862699039176484e-32,
-            "velocityY": 0,
-            "timestamp": 1.133141165881388
+            "velocityX": 3.0874616220611523e-31,
+            "velocityY": -3.4071713040637054e-30,
+            "timestamp": 1.1331411658815367
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.traj
index 06a0b427..72f3f88a 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.traj	
@@ -5,126 +5,126 @@
             "y": 4.36,
             "heading": -0.39,
             "angularVelocity": 0,
-            "velocityX": -1.8814689011962518e-30,
-            "velocityY": -8.215910483342913e-30,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 2.3223907686912884,
-            "y": 4.418261357303619,
-            "heading": -0.4292195342409252,
-            "angularVelocity": -0.4499474209247965,
-            "velocityX": 0.25687884418209317,
-            "velocityY": 0.6684053741511637,
-            "timestamp": 0.08716470506779908
+            "x": 2.3223907681513127,
+            "y": 4.418261357040706,
+            "heading": -0.429219543622513,
+            "angularVelocity": -0.4499475285553182,
+            "velocityX": 0.2568788379871729,
+            "velocityY": 0.6684053711347925,
+            "timestamp": 0.08716470506781053
         },
         {
-            "x": 2.37490967527786,
-            "y": 4.531319644696227,
-            "heading": -0.5050759172241152,
-            "angularVelocity": -0.8702648959138656,
-            "velocityX": 0.6025249158558101,
-            "velocityY": 1.2970649909807879,
-            "timestamp": 0.17432941013559816
+            "x": 2.374909672838796,
+            "y": 4.531319644218887,
+            "heading": -0.5050759499486299,
+            "angularVelocity": -0.8702651637162607,
+            "velocityX": 0.6025248940683728,
+            "velocityY": 1.297064988520603,
+            "timestamp": 0.17432941013562106
         },
         {
-            "x": 2.4692607215165214,
-            "y": 4.691180973762412,
-            "heading": -0.6116326818995184,
-            "angularVelocity": -1.222476053725161,
-            "velocityX": 1.0824455399151758,
-            "velocityY": 1.8340144550691926,
-            "timestamp": 0.26149411520339727
+            "x": 2.469260718438912,
+            "y": 4.691180971030917,
+            "heading": -0.6116327562753044,
+            "angularVelocity": -1.2224765315706085,
+            "velocityX": 1.082445532589298,
+            "velocityY": 1.834014429208093,
+            "timestamp": 0.2614941152034316
         },
         {
-            "x": 2.6203318015659547,
-            "y": 4.878827771954509,
-            "heading": -0.7357227336999919,
-            "angularVelocity": -1.4236272778523056,
-            "velocityX": 1.7331680286410147,
-            "velocityY": 2.1527841807775245,
-            "timestamp": 0.3486588202711963
+            "x": 2.620331804373909,
+            "y": 4.8788277526908095,
+            "heading": -0.7357228691689549,
+            "angularVelocity": -1.4236279787456398,
+            "velocityX": 1.7331680961631113,
+            "velocityY": 2.1527839911109723,
+            "timestamp": 0.3486588202712421
         },
         {
-            "x": 2.8323811449649123,
-            "y": 5.083303607446834,
-            "heading": -0.8475943342447533,
-            "angularVelocity": -1.2834506863500095,
-            "velocityX": 2.432743198454233,
-            "velocityY": 2.3458558751879917,
-            "timestamp": 0.4358235253389954
+            "x": 2.832381156144981,
+            "y": 5.083303561006889,
+            "heading": -0.84759450212177,
+            "angularVelocity": -1.2834510581523242,
+            "velocityX": 2.432743294503276,
+            "velocityY": 2.3458555634072913,
+            "timestamp": 0.43582352533905266
         },
         {
-            "x": 3.0986117768734247,
-            "y": 5.267387942263234,
-            "heading": -0.8666588377121885,
-            "angularVelocity": -0.2187181549298817,
-            "velocityX": 3.054339846632081,
-            "velocityY": 2.111913700312692,
-            "timestamp": 0.5229882304067944
+            "x": 3.098611796220168,
+            "y": 5.267387874253087,
+            "heading": -0.8666590247194526,
+            "angularVelocity": -0.21871837440225492,
+            "velocityX": 3.0543399403241303,
+            "velocityY": 2.111913452847551,
+            "timestamp": 0.5229882304068632
         },
         {
-            "x": 3.3974594561484373,
-            "y": 5.39777666903363,
-            "heading": -0.8564935513214242,
-            "angularVelocity": 0.11662158878249514,
-            "velocityX": 3.428540015623993,
-            "velocityY": 1.4958890375292995,
-            "timestamp": 0.6101529354745935
+            "x": 3.3974595011437105,
+            "y": 5.397776588765608,
+            "heading": -0.8564937886773937,
+            "angularVelocity": 0.11662101115527058,
+            "velocityX": 3.42854030987716,
+            "velocityY": 1.4958888969002289,
+            "timestamp": 0.6101529354746736
         },
         {
-            "x": 3.678181440932401,
-            "y": 5.472509264787258,
-            "heading": -0.7581228056155188,
-            "angularVelocity": 1.1285616767634317,
-            "velocityX": 3.2205923781375785,
-            "velocityY": 0.8573722092617639,
-            "timestamp": 0.6973176405423925
+            "x": 3.678181484629676,
+            "y": 5.472509187071852,
+            "heading": -0.7581230365630254,
+            "angularVelocity": 1.1285617502845884,
+            "velocityX": 3.220592363245829,
+            "velocityY": 0.8573722385466142,
+            "timestamp": 0.6973176405424841
         },
         {
-            "x": 3.9150331223150032,
-            "y": 5.514676359956983,
-            "heading": -0.5395207483783628,
-            "angularVelocity": 2.507919427560975,
-            "velocityX": 2.7172888521606593,
-            "velocityY": 0.483763412460658,
-            "timestamp": 0.7844823456101916
+            "x": 3.915033154173224,
+            "y": 5.514676302538828,
+            "heading": -0.5395209628894968,
+            "angularVelocity": 2.5079196161274817,
+            "velocityX": 2.7172887163363586,
+            "velocityY": 0.4837636453214852,
+            "timestamp": 0.7844823456102946
         },
         {
-            "x": 4.091189365639749,
-            "y": 5.539762978420037,
-            "heading": -0.2990438366325736,
-            "angularVelocity": 2.758879428992955,
-            "velocityX": 2.0209584049843006,
-            "velocityY": 0.28780707103340436,
-            "timestamp": 0.8716470506779906
+            "x": 4.091189385673594,
+            "y": 5.53976294658506,
+            "heading": -0.29904398170320484,
+            "angularVelocity": 2.7588802256510867,
+            "velocityX": 2.0209582693284847,
+            "velocityY": 0.2878073645372324,
+            "timestamp": 0.8716470506781051
         },
         {
-            "x": 4.206412444969991,
-            "y": 5.553849615471576,
-            "heading": -0.10702573317574342,
-            "angularVelocity": 2.202934126920672,
-            "velocityX": 1.3219006390329475,
-            "velocityY": 0.16160941565260478,
-            "timestamp": 0.9588117557457897
+            "x": 4.2064124532392215,
+            "y": 5.553849604220975,
+            "heading": -0.10702579424844161,
+            "angularVelocity": 2.2029350905894844,
+            "velocityX": 1.3219005040628353,
+            "velocityY": 0.161609651807531,
+            "timestamp": 0.9588117557459156
         },
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 1.2278563105616622,
-            "velocityX": 0.6492026214738336,
-            "velocityY": 0.07056049261727199,
-            "timestamp": 1.0459764608135889
+            "heading": 2.9287791201697906e-29,
+            "angularVelocity": 1.2278570112200775,
+            "velocityX": 0.6492025266047246,
+            "velocityY": 0.0705606216901654,
+            "timestamp": 1.045976460813726
         },
         {
             "x": 4.263,
             "y": 5.56,
             "heading": 0,
             "angularVelocity": 0,
-            "velocityX": 1.8862699039176484e-32,
-            "velocityY": 0,
-            "timestamp": 1.133141165881388
+            "velocityX": 3.0874616220611523e-31,
+            "velocityY": -3.4071713040637054e-30,
+            "timestamp": 1.1331411658815367
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.1.traj
new file mode 100644
index 00000000..2b0725c8
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.1.traj	
@@ -0,0 +1,256 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 9.57151893871228e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2927892107770984,
+            "y": 4.332309394440523,
+            "heading": -0.3722824674321399,
+            "angularVelocity": 0.3002660031020358,
+            "velocityX": -0.12220408518393268,
+            "velocityY": -0.4692835993371687,
+            "timestamp": 0.059006122520770025
+        },
+        {
+            "x": 2.2795380946752672,
+            "y": 4.276530177721837,
+            "heading": -0.3395678297253276,
+            "angularVelocity": 0.5544278510301563,
+            "velocityX": -0.224571883996739,
+            "velocityY": -0.9453123563415277,
+            "timestamp": 0.11801224504154005
+        },
+        {
+            "x": 2.2618545093830753,
+            "y": 4.192237257969867,
+            "heading": -0.2954402922678697,
+            "angularVelocity": 0.7478467584770477,
+            "velocityX": -0.29969068524994186,
+            "velocityY": -1.4285453127732544,
+            "timestamp": 0.17701836756231007
+        },
+        {
+            "x": 2.2420434066604593,
+            "y": 4.079048226765037,
+            "heading": -0.24482967906698166,
+            "angularVelocity": 0.8577179966887506,
+            "velocityX": -0.33574656114103973,
+            "velocityY": -1.9182590953165608,
+            "timestamp": 0.2360244900830801
+        },
+        {
+            "x": 2.2234541825747725,
+            "y": 3.936854095556344,
+            "heading": -0.19463836205748358,
+            "angularVelocity": 0.8506120189787272,
+            "velocityX": -0.3150389026010499,
+            "velocityY": -2.40981994976269,
+            "timestamp": 0.2950306126038501
+        },
+        {
+            "x": 2.211142623620853,
+            "y": 3.7664761504723194,
+            "heading": -0.15521354512815658,
+            "angularVelocity": 0.6681479013546628,
+            "velocityX": -0.2086488389333828,
+            "velocityY": -2.8874621446961175,
+            "timestamp": 0.35403673512462014
+        },
+        {
+            "x": 2.2125400151692816,
+            "y": 3.5715383450180926,
+            "heading": -0.14131017152189063,
+            "angularVelocity": 0.23562594883900537,
+            "velocityX": 0.023682144983125607,
+            "velocityY": -3.303687772156347,
+            "timestamp": 0.41304285764539017
+        },
+        {
+            "x": 2.235363094184648,
+            "y": 3.3576277458759463,
+            "heading": -0.144791073330025,
+            "angularVelocity": -0.058992214018278255,
+            "velocityX": 0.38679170974728244,
+            "velocityY": -3.6252271798888405,
+            "timestamp": 0.4720489801661602
+        },
+        {
+            "x": 2.287018648077521,
+            "y": 3.1404706845748738,
+            "heading": -0.14479216409186968,
+            "angularVelocity": -0.000018485570624385508,
+            "velocityX": 0.8754270181825738,
+            "velocityY": -3.6802462528296687,
+            "timestamp": 0.5310551026869302
+        },
+        {
+            "x": 2.3664206535422005,
+            "y": 2.9318538879811458,
+            "heading": -0.14479229465212481,
+            "angularVelocity": -0.000002212656069584398,
+            "velocityX": 1.3456570618876043,
+            "velocityY": -3.53551102295012,
+            "timestamp": 0.5900612252077002
+        },
+        {
+            "x": 2.4722259044647217,
+            "y": 2.73530650138855,
+            "heading": -0.14479252387880184,
+            "angularVelocity": -0.000003884794784189704,
+            "velocityX": 1.7931232625102267,
+            "velocityY": -3.330965977698533,
+            "timestamp": 0.6490673477284702
+        },
+        {
+            "x": 2.6222628757424284,
+            "y": 2.533912575740545,
+            "heading": -0.14479267276909355,
+            "angularVelocity": -0.0000022427572708026126,
+            "velocityX": 2.260029880862475,
+            "velocityY": -3.033627551346843,
+            "timestamp": 0.7154545103709612
+        },
+        {
+            "x": 2.800083925097921,
+            "y": 2.356570260194387,
+            "heading": -0.14479275333723068,
+            "angularVelocity": -0.0000012136101911737535,
+            "velocityX": 2.678545704884188,
+            "velocityY": -2.671334464182248,
+            "timestamp": 0.7818416730134522
+        },
+        {
+            "x": 3.001881619185789,
+            "y": 2.207076716019015,
+            "heading": -0.14479280967120484,
+            "angularVelocity": -8.485672817028763e-7,
+            "velocityX": 3.0397095772053357,
+            "velocityY": -2.2518441551784094,
+            "timestamp": 0.8482288356559432
+        },
+        {
+            "x": 3.2233351494929763,
+            "y": 2.0886327789398735,
+            "heading": -0.14479285532879432,
+            "angularVelocity": -6.877472642883405e-7,
+            "velocityX": 3.3357884490373766,
+            "velocityY": -1.7841391673415428,
+            "timestamp": 0.9146159982984342
+        },
+        {
+            "x": 3.4597028157497376,
+            "y": 2.003774307484127,
+            "heading": -0.14479289641721121,
+            "angularVelocity": -6.189211176196774e-7,
+            "velocityX": 3.5604423633775646,
+            "velocityY": -1.2782361540698368,
+            "timestamp": 0.9810031609409252
+        },
+        {
+            "x": 3.704835569204283,
+            "y": 1.949179734375487,
+            "heading": -0.14479293648657837,
+            "angularVelocity": -6.035710154638316e-7,
+            "velocityX": 3.692472214464675,
+            "velocityY": -0.8223664174750668,
+            "timestamp": 1.047390323583416
+        },
+        {
+            "x": 3.9499686718209266,
+            "y": 1.8945867290481686,
+            "heading": -0.1447929765559331,
+            "angularVelocity": -6.035708281824289e-7,
+            "velocityX": 3.692477473946853,
+            "velocityY": -0.8223428017448724,
+            "timestamp": 1.113777486225907
+        },
+        {
+            "x": 4.195101784677066,
+            "y": 1.8399937696982192,
+            "heading": -0.14479301662532418,
+            "angularVelocity": -6.035713756667977e-7,
+            "velocityX": 3.692477628185933,
+            "velocityY": -0.8223421091807183,
+            "timestamp": 1.1801646488683981
+        },
+        {
+            "x": 4.435904668374261,
+            "y": 1.7860806487644325,
+            "heading": -0.15898872227240893,
+            "angularVelocity": -0.21383208864538455,
+            "velocityX": 3.627250723064772,
+            "velocityY": -0.8121015989811171,
+            "timestamp": 1.2465518115108891
+        },
+        {
+            "x": 4.648245693564204,
+            "y": 1.7385415440023928,
+            "heading": -0.26462575063007965,
+            "angularVelocity": -1.5912267395211415,
+            "velocityX": 3.1985253886123286,
+            "velocityY": -0.7160888170209653,
+            "timestamp": 1.3129389741533801
+        },
+        {
+            "x": 4.824757883271093,
+            "y": 1.6989793937314068,
+            "heading": -0.37654836939407044,
+            "angularVelocity": -1.6859075506317058,
+            "velocityX": 2.6588301515075172,
+            "velocityY": -0.5959307296206723,
+            "timestamp": 1.3793261367958711
+        },
+        {
+            "x": 4.96555652453039,
+            "y": 1.6673723665346096,
+            "heading": -0.4762340168485147,
+            "angularVelocity": -1.501580177349536,
+            "velocityX": 2.1208715006774397,
+            "velocityY": -0.47610149219673153,
+            "timestamp": 1.4457132994383621
+        },
+        {
+            "x": 5.070906714490224,
+            "y": 1.643682366519874,
+            "heading": -0.5558849180153355,
+            "angularVelocity": -1.1997937251175768,
+            "velocityX": 1.5869060487969315,
+            "velocityY": -0.3568460990313944,
+            "timestamp": 1.5121004620808531
+        },
+        {
+            "x": 5.141006607602348,
+            "y": 1.6278935430132826,
+            "heading": -0.6113350442973207,
+            "angularVelocity": -0.8352537459779052,
+            "velocityX": 1.055925427776252,
+            "velocityY": -0.23782946699526453,
+            "timestamp": 1.5784876247233441
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -0.4317846186174008,
+            "velocityX": 0.5271108299370921,
+            "velocityY": -0.11890164753374041,
+            "timestamp": 1.6448747873658351
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 1.0662498702136072e-32,
+            "velocityX": 0,
+            "velocityY": 8.479039452208929e-33,
+            "timestamp": 1.7112619500083261
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.traj
new file mode 100644
index 00000000..2b0725c8
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.traj	
@@ -0,0 +1,256 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 9.57151893871228e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2927892107770984,
+            "y": 4.332309394440523,
+            "heading": -0.3722824674321399,
+            "angularVelocity": 0.3002660031020358,
+            "velocityX": -0.12220408518393268,
+            "velocityY": -0.4692835993371687,
+            "timestamp": 0.059006122520770025
+        },
+        {
+            "x": 2.2795380946752672,
+            "y": 4.276530177721837,
+            "heading": -0.3395678297253276,
+            "angularVelocity": 0.5544278510301563,
+            "velocityX": -0.224571883996739,
+            "velocityY": -0.9453123563415277,
+            "timestamp": 0.11801224504154005
+        },
+        {
+            "x": 2.2618545093830753,
+            "y": 4.192237257969867,
+            "heading": -0.2954402922678697,
+            "angularVelocity": 0.7478467584770477,
+            "velocityX": -0.29969068524994186,
+            "velocityY": -1.4285453127732544,
+            "timestamp": 0.17701836756231007
+        },
+        {
+            "x": 2.2420434066604593,
+            "y": 4.079048226765037,
+            "heading": -0.24482967906698166,
+            "angularVelocity": 0.8577179966887506,
+            "velocityX": -0.33574656114103973,
+            "velocityY": -1.9182590953165608,
+            "timestamp": 0.2360244900830801
+        },
+        {
+            "x": 2.2234541825747725,
+            "y": 3.936854095556344,
+            "heading": -0.19463836205748358,
+            "angularVelocity": 0.8506120189787272,
+            "velocityX": -0.3150389026010499,
+            "velocityY": -2.40981994976269,
+            "timestamp": 0.2950306126038501
+        },
+        {
+            "x": 2.211142623620853,
+            "y": 3.7664761504723194,
+            "heading": -0.15521354512815658,
+            "angularVelocity": 0.6681479013546628,
+            "velocityX": -0.2086488389333828,
+            "velocityY": -2.8874621446961175,
+            "timestamp": 0.35403673512462014
+        },
+        {
+            "x": 2.2125400151692816,
+            "y": 3.5715383450180926,
+            "heading": -0.14131017152189063,
+            "angularVelocity": 0.23562594883900537,
+            "velocityX": 0.023682144983125607,
+            "velocityY": -3.303687772156347,
+            "timestamp": 0.41304285764539017
+        },
+        {
+            "x": 2.235363094184648,
+            "y": 3.3576277458759463,
+            "heading": -0.144791073330025,
+            "angularVelocity": -0.058992214018278255,
+            "velocityX": 0.38679170974728244,
+            "velocityY": -3.6252271798888405,
+            "timestamp": 0.4720489801661602
+        },
+        {
+            "x": 2.287018648077521,
+            "y": 3.1404706845748738,
+            "heading": -0.14479216409186968,
+            "angularVelocity": -0.000018485570624385508,
+            "velocityX": 0.8754270181825738,
+            "velocityY": -3.6802462528296687,
+            "timestamp": 0.5310551026869302
+        },
+        {
+            "x": 2.3664206535422005,
+            "y": 2.9318538879811458,
+            "heading": -0.14479229465212481,
+            "angularVelocity": -0.000002212656069584398,
+            "velocityX": 1.3456570618876043,
+            "velocityY": -3.53551102295012,
+            "timestamp": 0.5900612252077002
+        },
+        {
+            "x": 2.4722259044647217,
+            "y": 2.73530650138855,
+            "heading": -0.14479252387880184,
+            "angularVelocity": -0.000003884794784189704,
+            "velocityX": 1.7931232625102267,
+            "velocityY": -3.330965977698533,
+            "timestamp": 0.6490673477284702
+        },
+        {
+            "x": 2.6222628757424284,
+            "y": 2.533912575740545,
+            "heading": -0.14479267276909355,
+            "angularVelocity": -0.0000022427572708026126,
+            "velocityX": 2.260029880862475,
+            "velocityY": -3.033627551346843,
+            "timestamp": 0.7154545103709612
+        },
+        {
+            "x": 2.800083925097921,
+            "y": 2.356570260194387,
+            "heading": -0.14479275333723068,
+            "angularVelocity": -0.0000012136101911737535,
+            "velocityX": 2.678545704884188,
+            "velocityY": -2.671334464182248,
+            "timestamp": 0.7818416730134522
+        },
+        {
+            "x": 3.001881619185789,
+            "y": 2.207076716019015,
+            "heading": -0.14479280967120484,
+            "angularVelocity": -8.485672817028763e-7,
+            "velocityX": 3.0397095772053357,
+            "velocityY": -2.2518441551784094,
+            "timestamp": 0.8482288356559432
+        },
+        {
+            "x": 3.2233351494929763,
+            "y": 2.0886327789398735,
+            "heading": -0.14479285532879432,
+            "angularVelocity": -6.877472642883405e-7,
+            "velocityX": 3.3357884490373766,
+            "velocityY": -1.7841391673415428,
+            "timestamp": 0.9146159982984342
+        },
+        {
+            "x": 3.4597028157497376,
+            "y": 2.003774307484127,
+            "heading": -0.14479289641721121,
+            "angularVelocity": -6.189211176196774e-7,
+            "velocityX": 3.5604423633775646,
+            "velocityY": -1.2782361540698368,
+            "timestamp": 0.9810031609409252
+        },
+        {
+            "x": 3.704835569204283,
+            "y": 1.949179734375487,
+            "heading": -0.14479293648657837,
+            "angularVelocity": -6.035710154638316e-7,
+            "velocityX": 3.692472214464675,
+            "velocityY": -0.8223664174750668,
+            "timestamp": 1.047390323583416
+        },
+        {
+            "x": 3.9499686718209266,
+            "y": 1.8945867290481686,
+            "heading": -0.1447929765559331,
+            "angularVelocity": -6.035708281824289e-7,
+            "velocityX": 3.692477473946853,
+            "velocityY": -0.8223428017448724,
+            "timestamp": 1.113777486225907
+        },
+        {
+            "x": 4.195101784677066,
+            "y": 1.8399937696982192,
+            "heading": -0.14479301662532418,
+            "angularVelocity": -6.035713756667977e-7,
+            "velocityX": 3.692477628185933,
+            "velocityY": -0.8223421091807183,
+            "timestamp": 1.1801646488683981
+        },
+        {
+            "x": 4.435904668374261,
+            "y": 1.7860806487644325,
+            "heading": -0.15898872227240893,
+            "angularVelocity": -0.21383208864538455,
+            "velocityX": 3.627250723064772,
+            "velocityY": -0.8121015989811171,
+            "timestamp": 1.2465518115108891
+        },
+        {
+            "x": 4.648245693564204,
+            "y": 1.7385415440023928,
+            "heading": -0.26462575063007965,
+            "angularVelocity": -1.5912267395211415,
+            "velocityX": 3.1985253886123286,
+            "velocityY": -0.7160888170209653,
+            "timestamp": 1.3129389741533801
+        },
+        {
+            "x": 4.824757883271093,
+            "y": 1.6989793937314068,
+            "heading": -0.37654836939407044,
+            "angularVelocity": -1.6859075506317058,
+            "velocityX": 2.6588301515075172,
+            "velocityY": -0.5959307296206723,
+            "timestamp": 1.3793261367958711
+        },
+        {
+            "x": 4.96555652453039,
+            "y": 1.6673723665346096,
+            "heading": -0.4762340168485147,
+            "angularVelocity": -1.501580177349536,
+            "velocityX": 2.1208715006774397,
+            "velocityY": -0.47610149219673153,
+            "timestamp": 1.4457132994383621
+        },
+        {
+            "x": 5.070906714490224,
+            "y": 1.643682366519874,
+            "heading": -0.5558849180153355,
+            "angularVelocity": -1.1997937251175768,
+            "velocityX": 1.5869060487969315,
+            "velocityY": -0.3568460990313944,
+            "timestamp": 1.5121004620808531
+        },
+        {
+            "x": 5.141006607602348,
+            "y": 1.6278935430132826,
+            "heading": -0.6113350442973207,
+            "angularVelocity": -0.8352537459779052,
+            "velocityX": 1.055925427776252,
+            "velocityY": -0.23782946699526453,
+            "timestamp": 1.5784876247233441
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -0.4317846186174008,
+            "velocityX": 0.5271108299370921,
+            "velocityY": -0.11890164753374041,
+            "timestamp": 1.6448747873658351
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 1.0662498702136072e-32,
+            "velocityX": 0,
+            "velocityY": 8.479039452208929e-33,
+            "timestamp": 1.7112619500083261
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.1.traj
index 282ccdeb..087304c1 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.1.traj	
@@ -4,145 +4,145 @@
             "x": 2.3,
             "y": 4.36,
             "heading": -0.39,
-            "angularVelocity": -1.4895643422557431e-33,
-            "velocityX": -9.369180406325922e-35,
+            "angularVelocity": 1.4326217036715317e-32,
+            "velocityX": -1.74824017803789e-33,
             "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 2.300137763138577,
-            "y": 4.403828106227067,
-            "heading": -0.362912658197764,
-            "angularVelocity": 0.3708979956671865,
-            "velocityX": 0.001886345007500309,
-            "velocityY": 0.6001237357356949,
-            "timestamp": 0.07303178264285357
+            "x": 2.3001377838932537,
+            "y": 4.403828106793783,
+            "heading": -0.3629126711329447,
+            "angularVelocity": 0.37089781855009624,
+            "velocityX": 0.0018866291944089578,
+            "velocityY": 0.600123743495633,
+            "timestamp": 0.07303178264284323
         },
         {
-            "x": 2.3004080637884394,
-            "y": 4.491572820860884,
-            "heading": -0.31063890006100675,
-            "angularVelocity": 0.7157672487934603,
-            "velocityX": 0.0037011372320465844,
-            "velocityY": 1.201459302491822,
-            "timestamp": 0.14606356528570713
+            "x": 2.300408122825978,
+            "y": 4.49157282256222,
+            "heading": -0.3106389398567459,
+            "angularVelocity": 0.7157668810008344,
+            "velocityX": 0.0037016614265912673,
+            "velocityY": 1.2014593180279707,
+            "timestamp": 0.14606356528568645
         },
         {
-            "x": 2.3008080200629966,
-            "y": 4.623360729086378,
-            "heading": -0.2362185458103229,
-            "angularVelocity": 1.019013250910508,
-            "velocityX": 0.005476468738453403,
-            "velocityY": 1.8045281582400208,
-            "timestamp": 0.2190953479285607
+            "x": 2.3008081314200264,
+            "y": 4.623360732427042,
+            "heading": -0.2362186265841639,
+            "angularVelocity": 1.019012689811082,
+            "velocityX": 0.005477185131915077,
+            "velocityY": 1.8045281806870437,
+            "timestamp": 0.2190953479285297
         },
         {
-            "x": 2.3013470975241233,
-            "y": 4.799377996239057,
-            "heading": -0.1451530728621028,
-            "angularVelocity": 1.2469293457282353,
-            "velocityX": 0.0073814090471083236,
-            "velocityY": 2.4101461142398,
-            "timestamp": 0.29212713057141426
+            "x": 2.301347271135313,
+            "y": 4.79937800153835,
+            "heading": -0.14515320669009546,
+            "angularVelocity": 1.2469286192754925,
+            "velocityX": 0.007382261472700921,
+            "velocityY": 2.410146141058999,
+            "timestamp": 0.2921271305713729
         },
         {
-            "x": 2.30206992781241,
-            "y": 5.019849097252827,
-            "heading": -0.0495237523149135,
-            "angularVelocity": 1.3094205986295608,
-            "velocityX": 0.009897475621284645,
-            "velocityY": 3.0188377311277894,
-            "timestamp": 0.36515891321426786
+            "x": 2.302070167703843,
+            "y": 5.019849104537176,
+            "heading": -0.04952394454697364,
+            "angularVelocity": 1.3094197989222025,
+            "velocityX": 0.009898383174695338,
+            "velocityY": 3.018837758308937,
+            "timestamp": 0.3651589132142161
         },
         {
-            "x": 2.303151073626671,
-            "y": 5.282690240833408,
-            "heading": -0.004114697333369339,
-            "angularVelocity": 0.621771143169372,
-            "velocityX": 0.014803771387434566,
-            "velocityY": 3.598996684305928,
-            "timestamp": 0.43819069585712145
+            "x": 2.3031513688730865,
+            "y": 5.282690253403799,
+            "heading": -0.004114907781753178,
+            "angularVelocity": 0.6217708937393756,
+            "velocityX": 0.014804529344862738,
+            "velocityY": 3.598996756686451,
+            "timestamp": 0.43819069585705933
         },
         {
-            "x": 2.3035206151085394,
-            "y": 5.5589648754006,
-            "heading": -0.004114668066309664,
-            "angularVelocity": 4.007441502253877e-7,
-            "velocityX": 0.005060009060371044,
-            "velocityY": 3.7829370250792667,
-            "timestamp": 0.511222478499975
+            "x": 2.303520931398292,
+            "y": 5.5589648879428,
+            "heading": -0.0041148785146688655,
+            "angularVelocity": 4.00744487575221e-7,
+            "velocityX": 0.005060297199824665,
+            "velocityY": 3.7829370246937892,
+            "timestamp": 0.5112224784999025
         },
         {
-            "x": 2.3038901811674446,
-            "y": 5.835239509934909,
-            "heading": -0.0041146387992405135,
-            "angularVelocity": 4.0074427997802584e-7,
-            "velocityX": 0.0050603455856026045,
-            "velocityY": 3.7829370246290135,
-            "timestamp": 0.5842542611428286
+            "x": 2.3038904865521834,
+            "y": 5.835239522491655,
+            "heading": -0.004114849247579104,
+            "angularVelocity": 4.0074456218901027e-7,
+            "velocityX": 0.005060196266858216,
+            "velocityY": 3.7829370248287195,
+            "timestamp": 0.5842542611427458
         },
         {
-            "x": 2.3020054253248463,
-            "y": 6.097682298903223,
-            "heading": 0.0412179398919569,
-            "angularVelocity": 0.6207239786667508,
-            "velocityX": -0.02580733722214711,
-            "velocityY": 3.593542146598477,
-            "timestamp": 0.6572860437856822
+            "x": 2.302005656251481,
+            "y": 6.097682306820458,
+            "heading": 0.0412177605138735,
+            "angularVelocity": 0.6207244041015477,
+            "velocityX": -0.025808356752290516,
+            "velocityY": 3.5935420830717044,
+            "timestamp": 0.6572860437855891
         },
         {
-            "x": 2.301170577421719,
-            "y": 6.318019035246094,
-            "heading": 0.13972641476186656,
-            "angularVelocity": 1.3488439047372631,
-            "velocityX": -0.011431295703265835,
-            "velocityY": 3.016997920212089,
-            "timestamp": 0.7303178264285358
+            "x": 2.3011707390613565,
+            "y": 6.318019040898657,
+            "heading": 0.13972629361592653,
+            "angularVelocity": 1.3488447020908976,
+            "velocityX": -0.01143224442716972,
+            "velocityY": 3.016997889203128,
+            "timestamp": 0.7303178264284323
         },
         {
-            "x": 2.3006511827054728,
-            "y": 6.4939017376421555,
-            "heading": 0.2337896338628955,
-            "angularVelocity": 1.2879764904688844,
-            "velocityX": -0.007111899743518039,
-            "velocityY": 2.4083035636166743,
-            "timestamp": 0.8033496090713894
+            "x": 2.3006512841511224,
+            "y": 6.493901741171369,
+            "heading": 0.23378956116370217,
+            "angularVelocity": 1.2879771538343168,
+            "velocityX": -0.007112723959845375,
+            "velocityY": 2.408303534542668,
+            "timestamp": 0.8033496090712756
         },
         {
-            "x": 2.300310606012852,
-            "y": 6.62557002983581,
-            "heading": 0.3107349410237032,
-            "angularVelocity": 1.0535865944433043,
-            "velocityX": -0.004663403798945386,
-            "velocityY": 1.8028902955518677,
-            "timestamp": 0.876381391714243
+            "x": 2.300310658917655,
+            "y": 6.625570031648912,
+            "heading": 0.310734904704345,
+            "angularVelocity": 1.053587092580481,
+            "velocityX": -0.00466406845267762,
+            "velocityY": 1.802890272053981,
+            "timestamp": 0.8763813917141189
         },
         {
-            "x": 2.30010034874232,
-            "y": 6.713223552423264,
-            "heading": 0.36487146716810653,
-            "angularVelocity": 0.741273513877466,
-            "velocityX": -0.002878983134789396,
-            "velocityY": 1.2002106400182577,
-            "timestamp": 0.9494131743570966
+            "x": 2.3001003670918414,
+            "y": 6.7132235530415985,
+            "heading": 0.3648714550351714,
+            "angularVelocity": 0.7412738450542471,
+            "velocityX": -0.0028794562888088465,
+            "velocityY": 1.2002106236588805,
+            "timestamp": 0.9494131743569622
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.393,
-            "angularVelocity": 0.38515467942840836,
-            "velocityX": -0.0013740420771443632,
-            "velocityY": 0.5994163909542732,
-            "timestamp": 1.02244495699995
+            "angularVelocity": 0.38515484556072405,
+            "velocityX": -0.0013742933310585549,
+            "velocityY": 0.5994163824877027,
+            "timestamp": 1.0224449569998053
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.393,
-            "angularVelocity": 8.681406116042979e-33,
-            "velocityX": 1.0196837521674476e-33,
+            "angularVelocity": 2.8585488767662063e-32,
+            "velocityX": -1.0732467562704116e-31,
             "velocityY": 0,
-            "timestamp": 1.0954767396428036
+            "timestamp": 1.0954767396426486
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.traj
index 282ccdeb..087304c1 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.traj	
@@ -4,145 +4,145 @@
             "x": 2.3,
             "y": 4.36,
             "heading": -0.39,
-            "angularVelocity": -1.4895643422557431e-33,
-            "velocityX": -9.369180406325922e-35,
+            "angularVelocity": 1.4326217036715317e-32,
+            "velocityX": -1.74824017803789e-33,
             "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 2.300137763138577,
-            "y": 4.403828106227067,
-            "heading": -0.362912658197764,
-            "angularVelocity": 0.3708979956671865,
-            "velocityX": 0.001886345007500309,
-            "velocityY": 0.6001237357356949,
-            "timestamp": 0.07303178264285357
+            "x": 2.3001377838932537,
+            "y": 4.403828106793783,
+            "heading": -0.3629126711329447,
+            "angularVelocity": 0.37089781855009624,
+            "velocityX": 0.0018866291944089578,
+            "velocityY": 0.600123743495633,
+            "timestamp": 0.07303178264284323
         },
         {
-            "x": 2.3004080637884394,
-            "y": 4.491572820860884,
-            "heading": -0.31063890006100675,
-            "angularVelocity": 0.7157672487934603,
-            "velocityX": 0.0037011372320465844,
-            "velocityY": 1.201459302491822,
-            "timestamp": 0.14606356528570713
+            "x": 2.300408122825978,
+            "y": 4.49157282256222,
+            "heading": -0.3106389398567459,
+            "angularVelocity": 0.7157668810008344,
+            "velocityX": 0.0037016614265912673,
+            "velocityY": 1.2014593180279707,
+            "timestamp": 0.14606356528568645
         },
         {
-            "x": 2.3008080200629966,
-            "y": 4.623360729086378,
-            "heading": -0.2362185458103229,
-            "angularVelocity": 1.019013250910508,
-            "velocityX": 0.005476468738453403,
-            "velocityY": 1.8045281582400208,
-            "timestamp": 0.2190953479285607
+            "x": 2.3008081314200264,
+            "y": 4.623360732427042,
+            "heading": -0.2362186265841639,
+            "angularVelocity": 1.019012689811082,
+            "velocityX": 0.005477185131915077,
+            "velocityY": 1.8045281806870437,
+            "timestamp": 0.2190953479285297
         },
         {
-            "x": 2.3013470975241233,
-            "y": 4.799377996239057,
-            "heading": -0.1451530728621028,
-            "angularVelocity": 1.2469293457282353,
-            "velocityX": 0.0073814090471083236,
-            "velocityY": 2.4101461142398,
-            "timestamp": 0.29212713057141426
+            "x": 2.301347271135313,
+            "y": 4.79937800153835,
+            "heading": -0.14515320669009546,
+            "angularVelocity": 1.2469286192754925,
+            "velocityX": 0.007382261472700921,
+            "velocityY": 2.410146141058999,
+            "timestamp": 0.2921271305713729
         },
         {
-            "x": 2.30206992781241,
-            "y": 5.019849097252827,
-            "heading": -0.0495237523149135,
-            "angularVelocity": 1.3094205986295608,
-            "velocityX": 0.009897475621284645,
-            "velocityY": 3.0188377311277894,
-            "timestamp": 0.36515891321426786
+            "x": 2.302070167703843,
+            "y": 5.019849104537176,
+            "heading": -0.04952394454697364,
+            "angularVelocity": 1.3094197989222025,
+            "velocityX": 0.009898383174695338,
+            "velocityY": 3.018837758308937,
+            "timestamp": 0.3651589132142161
         },
         {
-            "x": 2.303151073626671,
-            "y": 5.282690240833408,
-            "heading": -0.004114697333369339,
-            "angularVelocity": 0.621771143169372,
-            "velocityX": 0.014803771387434566,
-            "velocityY": 3.598996684305928,
-            "timestamp": 0.43819069585712145
+            "x": 2.3031513688730865,
+            "y": 5.282690253403799,
+            "heading": -0.004114907781753178,
+            "angularVelocity": 0.6217708937393756,
+            "velocityX": 0.014804529344862738,
+            "velocityY": 3.598996756686451,
+            "timestamp": 0.43819069585705933
         },
         {
-            "x": 2.3035206151085394,
-            "y": 5.5589648754006,
-            "heading": -0.004114668066309664,
-            "angularVelocity": 4.007441502253877e-7,
-            "velocityX": 0.005060009060371044,
-            "velocityY": 3.7829370250792667,
-            "timestamp": 0.511222478499975
+            "x": 2.303520931398292,
+            "y": 5.5589648879428,
+            "heading": -0.0041148785146688655,
+            "angularVelocity": 4.00744487575221e-7,
+            "velocityX": 0.005060297199824665,
+            "velocityY": 3.7829370246937892,
+            "timestamp": 0.5112224784999025
         },
         {
-            "x": 2.3038901811674446,
-            "y": 5.835239509934909,
-            "heading": -0.0041146387992405135,
-            "angularVelocity": 4.0074427997802584e-7,
-            "velocityX": 0.0050603455856026045,
-            "velocityY": 3.7829370246290135,
-            "timestamp": 0.5842542611428286
+            "x": 2.3038904865521834,
+            "y": 5.835239522491655,
+            "heading": -0.004114849247579104,
+            "angularVelocity": 4.0074456218901027e-7,
+            "velocityX": 0.005060196266858216,
+            "velocityY": 3.7829370248287195,
+            "timestamp": 0.5842542611427458
         },
         {
-            "x": 2.3020054253248463,
-            "y": 6.097682298903223,
-            "heading": 0.0412179398919569,
-            "angularVelocity": 0.6207239786667508,
-            "velocityX": -0.02580733722214711,
-            "velocityY": 3.593542146598477,
-            "timestamp": 0.6572860437856822
+            "x": 2.302005656251481,
+            "y": 6.097682306820458,
+            "heading": 0.0412177605138735,
+            "angularVelocity": 0.6207244041015477,
+            "velocityX": -0.025808356752290516,
+            "velocityY": 3.5935420830717044,
+            "timestamp": 0.6572860437855891
         },
         {
-            "x": 2.301170577421719,
-            "y": 6.318019035246094,
-            "heading": 0.13972641476186656,
-            "angularVelocity": 1.3488439047372631,
-            "velocityX": -0.011431295703265835,
-            "velocityY": 3.016997920212089,
-            "timestamp": 0.7303178264285358
+            "x": 2.3011707390613565,
+            "y": 6.318019040898657,
+            "heading": 0.13972629361592653,
+            "angularVelocity": 1.3488447020908976,
+            "velocityX": -0.01143224442716972,
+            "velocityY": 3.016997889203128,
+            "timestamp": 0.7303178264284323
         },
         {
-            "x": 2.3006511827054728,
-            "y": 6.4939017376421555,
-            "heading": 0.2337896338628955,
-            "angularVelocity": 1.2879764904688844,
-            "velocityX": -0.007111899743518039,
-            "velocityY": 2.4083035636166743,
-            "timestamp": 0.8033496090713894
+            "x": 2.3006512841511224,
+            "y": 6.493901741171369,
+            "heading": 0.23378956116370217,
+            "angularVelocity": 1.2879771538343168,
+            "velocityX": -0.007112723959845375,
+            "velocityY": 2.408303534542668,
+            "timestamp": 0.8033496090712756
         },
         {
-            "x": 2.300310606012852,
-            "y": 6.62557002983581,
-            "heading": 0.3107349410237032,
-            "angularVelocity": 1.0535865944433043,
-            "velocityX": -0.004663403798945386,
-            "velocityY": 1.8028902955518677,
-            "timestamp": 0.876381391714243
+            "x": 2.300310658917655,
+            "y": 6.625570031648912,
+            "heading": 0.310734904704345,
+            "angularVelocity": 1.053587092580481,
+            "velocityX": -0.00466406845267762,
+            "velocityY": 1.802890272053981,
+            "timestamp": 0.8763813917141189
         },
         {
-            "x": 2.30010034874232,
-            "y": 6.713223552423264,
-            "heading": 0.36487146716810653,
-            "angularVelocity": 0.741273513877466,
-            "velocityX": -0.002878983134789396,
-            "velocityY": 1.2002106400182577,
-            "timestamp": 0.9494131743570966
+            "x": 2.3001003670918414,
+            "y": 6.7132235530415985,
+            "heading": 0.3648714550351714,
+            "angularVelocity": 0.7412738450542471,
+            "velocityX": -0.0028794562888088465,
+            "velocityY": 1.2002106236588805,
+            "timestamp": 0.9494131743569622
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.393,
-            "angularVelocity": 0.38515467942840836,
-            "velocityX": -0.0013740420771443632,
-            "velocityY": 0.5994163909542732,
-            "timestamp": 1.02244495699995
+            "angularVelocity": 0.38515484556072405,
+            "velocityX": -0.0013742933310585549,
+            "velocityY": 0.5994163824877027,
+            "timestamp": 1.0224449569998053
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.393,
-            "angularVelocity": 8.681406116042979e-33,
-            "velocityX": 1.0196837521674476e-33,
+            "angularVelocity": 2.8585488767662063e-32,
+            "velocityX": -1.0732467562704116e-31,
             "velocityY": 0,
-            "timestamp": 1.0954767396428036
+            "timestamp": 1.0954767396426486
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj
index efed994e..4345e451 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj	
@@ -5,99 +5,99 @@
             "y": 4.36,
             "heading": -0.39,
             "angularVelocity": 0,
-            "velocityX": 1.7319348109741582e-34,
-            "velocityY": -4.404349434317184e-36,
+            "velocityX": 2.0150450292109907e-34,
+            "velocityY": 3.039389955204995e-35,
             "timestamp": 0
         },
         {
-            "x": 2.298001242299744,
-            "y": 4.408001442881322,
-            "heading": -0.3744767722715256,
-            "angularVelocity": 0.20408758634762672,
-            "velocityX": -0.02627814536217338,
-            "velocityY": 0.6310864460799332,
-            "timestamp": 0.07606159691669512
+            "x": 2.2980012520823867,
+            "y": 4.4080014432651184,
+            "heading": -0.3744767713085023,
+            "angularVelocity": 0.2040875990087458,
+            "velocityX": -0.02627801674742958,
+            "velocityY": 0.6310864511258738,
+            "timestamp": 0.07606159691668693
         },
         {
-            "x": 2.2940037533773614,
-            "y": 4.504004077466311,
-            "heading": -0.3434155460751892,
-            "angularVelocity": 0.4083693671374729,
-            "velocityX": -0.05255594261004741,
-            "velocityY": 1.26216958986719,
-            "timestamp": 0.15212319383339024
+            "x": 2.294003780157748,
+            "y": 4.504004078538053,
+            "heading": -0.34341554427294535,
+            "angularVelocity": 0.40836937817095076,
+            "velocityX": -0.05255571913664827,
+            "velocityY": 1.262169598911913,
+            "timestamp": 0.15212319383337386
         },
         {
-            "x": 2.2880074570982525,
-            "y": 4.648007443871442,
-            "heading": -0.29679113248213845,
-            "angularVelocity": 0.6129823127972874,
-            "velocityX": -0.07883474081771351,
-            "velocityY": 1.8932466874557927,
-            "timestamp": 0.22818479075008535
+            "x": 2.2880075055108158,
+            "y": 4.648007445856594,
+            "heading": -0.296791131111893,
+            "angularVelocity": 0.6129823071177671,
+            "velocityX": -0.07883445641432706,
+            "velocityY": 1.8932466994648223,
+            "timestamp": 0.2281847907500608
         },
         {
-            "x": 2.2800120385493474,
-            "y": 4.8400108934587145,
-            "heading": -0.2345694550660327,
-            "angularVelocity": 0.8180432693814297,
-            "velocityX": -0.10511767926279252,
-            "velocityY": 2.524315257245527,
-            "timestamp": 0.30424638766678047
+            "x": 2.2800121106149036,
+            "y": 4.840010896492559,
+            "heading": -0.2345694561462187,
+            "angularVelocity": 0.8180432371651087,
+            "velocityX": -0.10511736829125365,
+            "velocityY": 2.5243152710332057,
+            "timestamp": 0.3042463876667477
         },
         {
-            "x": 2.270016765601229,
-            "y": 5.0800136328565335,
-            "heading": -0.15671111441894314,
-            "angularVelocity": 1.0236222194014972,
-            "velocityX": -0.1314102431883619,
-            "velocityY": 3.1553733963892037,
-            "timestamp": 0.3803079845834756
+            "x": 2.270016860554186,
+            "y": 5.080013636963748,
+            "heading": -0.15671112019223,
+            "angularVelocity": 1.023622157700289,
+            "velocityX": -0.13140994228224737,
+            "velocityY": 3.1553734105013973,
+            "timestamp": 0.3803079845834346
         },
         {
-            "x": 2.2620105775052464,
-            "y": 5.272008323301556,
-            "heading": -0.09403333232435188,
-            "angularVelocity": 0.8240397866381615,
-            "velocityX": -0.10525926907308407,
-            "velocityY": 2.524200098708164,
-            "timestamp": 0.4563695815001707
+            "x": 2.2620106394348882,
+            "y": 5.2720083260038075,
+            "heading": -0.09403333701953483,
+            "angularVelocity": 0.82403980081234,
+            "velocityX": -0.10525970323851294,
+            "velocityY": 2.524200080237073,
+            "timestamp": 0.4563695815001215
         },
         {
-            "x": 2.256005419001405,
-            "y": 5.416004186972581,
-            "heading": -0.04701785122811225,
-            "angularVelocity": 0.6181237707608528,
-            "velocityX": -0.07895125460510179,
-            "velocityY": 1.8931480472166962,
-            "timestamp": 0.5324311784168658
+            "x": 2.256005452365101,
+            "y": 5.416004188429854,
+            "heading": -0.0470178538159679,
+            "angularVelocity": 0.6181237984664553,
+            "velocityX": -0.07895163016843816,
+            "velocityY": 1.8931480308488688,
+            "timestamp": 0.5324311784168084
         },
         {
-            "x": 2.252001808264489,
-            "y": 5.512001394535746,
-            "heading": -0.015672604394819867,
-            "angularVelocity": 0.4121034543571656,
-            "velocityX": -0.05263642756936803,
-            "velocityY": 1.262098239513749,
-            "timestamp": 0.6084927753335609
+            "x": 2.2520018201736485,
+            "y": 5.5120013950535185,
+            "heading": -0.01567260522346991,
+            "angularVelocity": 0.4121034774859067,
+            "velocityX": -0.05263670963729669,
+            "velocityY": 1.262098227162051,
+            "timestamp": 0.6084927753334953
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": -1.7090620600656545e-34,
-            "angularVelocity": 0.2060514770940842,
-            "velocityX": -0.026318251859498508,
-            "velocityY": 0.6310491418793501,
-            "timestamp": 0.6845543722502561
+            "heading": 1.3656663814986182e-35,
+            "angularVelocity": 0.20605148798856665,
+            "velocityX": -0.026318408432062,
+            "velocityY": 0.6310491350721452,
+            "timestamp": 0.6845543722501822
         },
         {
             "x": 2.25,
             "y": 5.56,
             "heading": 0,
-            "angularVelocity": 2.0052459094930362e-33,
-            "velocityX": -1.9224144442861376e-33,
-            "velocityY": 1.2850315433355203e-34,
-            "timestamp": 0.7606159691669512
+            "angularVelocity": 6.6551444751621536e-34,
+            "velocityX": 3.9479513180483775e-34,
+            "velocityY": 0,
+            "timestamp": 0.7606159691668691
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.traj
index efed994e..4345e451 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.traj	
@@ -5,99 +5,99 @@
             "y": 4.36,
             "heading": -0.39,
             "angularVelocity": 0,
-            "velocityX": 1.7319348109741582e-34,
-            "velocityY": -4.404349434317184e-36,
+            "velocityX": 2.0150450292109907e-34,
+            "velocityY": 3.039389955204995e-35,
             "timestamp": 0
         },
         {
-            "x": 2.298001242299744,
-            "y": 4.408001442881322,
-            "heading": -0.3744767722715256,
-            "angularVelocity": 0.20408758634762672,
-            "velocityX": -0.02627814536217338,
-            "velocityY": 0.6310864460799332,
-            "timestamp": 0.07606159691669512
+            "x": 2.2980012520823867,
+            "y": 4.4080014432651184,
+            "heading": -0.3744767713085023,
+            "angularVelocity": 0.2040875990087458,
+            "velocityX": -0.02627801674742958,
+            "velocityY": 0.6310864511258738,
+            "timestamp": 0.07606159691668693
         },
         {
-            "x": 2.2940037533773614,
-            "y": 4.504004077466311,
-            "heading": -0.3434155460751892,
-            "angularVelocity": 0.4083693671374729,
-            "velocityX": -0.05255594261004741,
-            "velocityY": 1.26216958986719,
-            "timestamp": 0.15212319383339024
+            "x": 2.294003780157748,
+            "y": 4.504004078538053,
+            "heading": -0.34341554427294535,
+            "angularVelocity": 0.40836937817095076,
+            "velocityX": -0.05255571913664827,
+            "velocityY": 1.262169598911913,
+            "timestamp": 0.15212319383337386
         },
         {
-            "x": 2.2880074570982525,
-            "y": 4.648007443871442,
-            "heading": -0.29679113248213845,
-            "angularVelocity": 0.6129823127972874,
-            "velocityX": -0.07883474081771351,
-            "velocityY": 1.8932466874557927,
-            "timestamp": 0.22818479075008535
+            "x": 2.2880075055108158,
+            "y": 4.648007445856594,
+            "heading": -0.296791131111893,
+            "angularVelocity": 0.6129823071177671,
+            "velocityX": -0.07883445641432706,
+            "velocityY": 1.8932466994648223,
+            "timestamp": 0.2281847907500608
         },
         {
-            "x": 2.2800120385493474,
-            "y": 4.8400108934587145,
-            "heading": -0.2345694550660327,
-            "angularVelocity": 0.8180432693814297,
-            "velocityX": -0.10511767926279252,
-            "velocityY": 2.524315257245527,
-            "timestamp": 0.30424638766678047
+            "x": 2.2800121106149036,
+            "y": 4.840010896492559,
+            "heading": -0.2345694561462187,
+            "angularVelocity": 0.8180432371651087,
+            "velocityX": -0.10511736829125365,
+            "velocityY": 2.5243152710332057,
+            "timestamp": 0.3042463876667477
         },
         {
-            "x": 2.270016765601229,
-            "y": 5.0800136328565335,
-            "heading": -0.15671111441894314,
-            "angularVelocity": 1.0236222194014972,
-            "velocityX": -0.1314102431883619,
-            "velocityY": 3.1553733963892037,
-            "timestamp": 0.3803079845834756
+            "x": 2.270016860554186,
+            "y": 5.080013636963748,
+            "heading": -0.15671112019223,
+            "angularVelocity": 1.023622157700289,
+            "velocityX": -0.13140994228224737,
+            "velocityY": 3.1553734105013973,
+            "timestamp": 0.3803079845834346
         },
         {
-            "x": 2.2620105775052464,
-            "y": 5.272008323301556,
-            "heading": -0.09403333232435188,
-            "angularVelocity": 0.8240397866381615,
-            "velocityX": -0.10525926907308407,
-            "velocityY": 2.524200098708164,
-            "timestamp": 0.4563695815001707
+            "x": 2.2620106394348882,
+            "y": 5.2720083260038075,
+            "heading": -0.09403333701953483,
+            "angularVelocity": 0.82403980081234,
+            "velocityX": -0.10525970323851294,
+            "velocityY": 2.524200080237073,
+            "timestamp": 0.4563695815001215
         },
         {
-            "x": 2.256005419001405,
-            "y": 5.416004186972581,
-            "heading": -0.04701785122811225,
-            "angularVelocity": 0.6181237707608528,
-            "velocityX": -0.07895125460510179,
-            "velocityY": 1.8931480472166962,
-            "timestamp": 0.5324311784168658
+            "x": 2.256005452365101,
+            "y": 5.416004188429854,
+            "heading": -0.0470178538159679,
+            "angularVelocity": 0.6181237984664553,
+            "velocityX": -0.07895163016843816,
+            "velocityY": 1.8931480308488688,
+            "timestamp": 0.5324311784168084
         },
         {
-            "x": 2.252001808264489,
-            "y": 5.512001394535746,
-            "heading": -0.015672604394819867,
-            "angularVelocity": 0.4121034543571656,
-            "velocityX": -0.05263642756936803,
-            "velocityY": 1.262098239513749,
-            "timestamp": 0.6084927753335609
+            "x": 2.2520018201736485,
+            "y": 5.5120013950535185,
+            "heading": -0.01567260522346991,
+            "angularVelocity": 0.4121034774859067,
+            "velocityX": -0.05263670963729669,
+            "velocityY": 1.262098227162051,
+            "timestamp": 0.6084927753334953
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": -1.7090620600656545e-34,
-            "angularVelocity": 0.2060514770940842,
-            "velocityX": -0.026318251859498508,
-            "velocityY": 0.6310491418793501,
-            "timestamp": 0.6845543722502561
+            "heading": 1.3656663814986182e-35,
+            "angularVelocity": 0.20605148798856665,
+            "velocityX": -0.026318408432062,
+            "velocityY": 0.6310491350721452,
+            "timestamp": 0.6845543722501822
         },
         {
             "x": 2.25,
             "y": 5.56,
             "heading": 0,
-            "angularVelocity": 2.0052459094930362e-33,
-            "velocityX": -1.9224144442861376e-33,
-            "velocityY": 1.2850315433355203e-34,
-            "timestamp": 0.7606159691669512
+            "angularVelocity": 6.6551444751621536e-34,
+            "velocityX": 3.9479513180483775e-34,
+            "velocityY": 0,
+            "timestamp": 0.7606159691668691
         }
     ]
 }
\ No newline at end of file

From 36ced9b19211b5d74fcd58380fafe90f3dde8d78 Mon Sep 17 00:00:00 2001
From: Awesomeplayer165 <dev@jacob2.dev>
Date: Sun, 18 Feb 2024 21:46:50 -0800
Subject: [PATCH 39/62] second pass at fine tuning

---
 .../frc/robot/utils/dynamicauto/data.chor     | 9675 +++++++++--------
 .../deploy/choreo/Amp Side To C1.1.traj       |  394 +-
 .../deploy/choreo/Amp Side To C1.traj         |  394 +-
 .../deploy/choreo/Amp Side To C2.1.traj       |  545 +-
 .../deploy/choreo/Amp Side To C2.traj         |  545 +-
 .../deploy/choreo/Amp Side To C5.1.traj       | 1394 +--
 .../deploy/choreo/Amp Side To C5.traj         | 1394 +--
 .../deploy/choreo/Amp Side To W1.1.traj       |  162 +-
 .../deploy/choreo/Amp Side To W1.traj         |  162 +-
 .../deploy/choreo/Amp Side To W2.1.traj       |  204 +-
 .../deploy/choreo/Amp Side To W2.traj         |  204 +-
 .../deploy/choreo/Amp Side To W3.1.traj       |  342 +-
 .../deploy/choreo/Amp Side To W3.traj         |  342 +-
 .../dynamicauto/deploy/choreo/C1 To W3.1.traj |  274 +
 .../dynamicauto/deploy/choreo/C1 To W3.traj   |  274 +
 .../deploy/choreo/Center To C1.1.traj         |  398 +-
 .../deploy/choreo/Center To C1.traj           |  398 +-
 .../deploy/choreo/Center To C4.1.traj         | 2130 ++--
 .../deploy/choreo/Center To C4.traj           | 2130 ++--
 .../deploy/choreo/Center To W2.1.traj         |   92 +-
 .../deploy/choreo/Center To W2.traj           |   92 +-
 .../deploy/choreo/Center To W3.1.traj         |  150 +-
 .../deploy/choreo/Center To W3.traj           |  150 +-
 .../deploy/choreo/Stage Side To C5.1.traj     |  587 +-
 .../deploy/choreo/Stage Side To C5.traj       |  587 +-
 .../deploy/choreo/Stage Side To W1.1.traj     |  342 +-
 .../deploy/choreo/Stage Side To W1.traj       |  342 +-
 .../deploy/choreo/Stage Side To W2.1.traj     |  208 +-
 .../deploy/choreo/Stage Side To W2.traj       |  208 +-
 .../deploy/choreo/Stage Side To W3.1.traj     |  158 +-
 .../deploy/choreo/Stage Side To W3.traj       |  158 +-
 .../dynamicauto/deploy/choreo/W1 To C2.1.traj |  412 +-
 .../dynamicauto/deploy/choreo/W1 To C2.traj   |  412 +-
 .../dynamicauto/deploy/choreo/W2 To S4.1.traj | 1050 +-
 .../dynamicauto/deploy/choreo/W2 To S4.traj   | 1050 +-
 .../dynamicauto/deploy/choreo/W3 To S3.1.traj |  306 +-
 .../dynamicauto/deploy/choreo/W3 To S3.traj   |  306 +-
 37 files changed, 14912 insertions(+), 13059 deletions(-)
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.traj

diff --git a/src/main/java/frc/robot/utils/dynamicauto/data.chor b/src/main/java/frc/robot/utils/dynamicauto/data.chor
index cc81043a..9d822e94 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/data.chor
+++ b/src/main/java/frc/robot/utils/dynamicauto/data.chor
@@ -39,118 +39,118 @@
                     "x": 0.71,
                     "y": 6.72,
                     "heading": 1.04,
-                    "angularVelocity": 6.421381184701714e-32,
-                    "velocityX": 9.791473054297313e-34,
-                    "velocityY": 6.656933252086786e-33,
+                    "angularVelocity": -3.0520704892108665e-32,
+                    "velocityX": 0,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7541195074128881,
-                    "y": 6.720987207910964,
-                    "heading": 1.0162961446571532,
-                    "angularVelocity": -0.3240022105828226,
-                    "velocityX": 0.6030587735562275,
-                    "velocityY": 0.01349390387478939,
-                    "timestamp": 0.0731595482024364
-                },
-                {
-                    "x": 0.842413628953284,
-                    "y": 6.722972522190638,
-                    "heading": 0.9702543638919431,
-                    "angularVelocity": -0.6293338586206134,
-                    "velocityX": 1.2068707873384146,
-                    "velocityY": 0.027136776107259192,
-                    "timestamp": 0.1463190964048728
-                },
-                {
-                    "x": 0.9749626727340511,
-                    "y": 6.725969397195605,
-                    "heading": 0.9040114408742106,
-                    "angularVelocity": -0.9054583392778368,
-                    "velocityX": 1.811780513105384,
-                    "velocityY": 0.04096355265456321,
-                    "timestamp": 0.2194786446073092
-                },
-                {
-                    "x": 1.1518931623979327,
-                    "y": 6.729992809371002,
-                    "heading": 0.821470963167313,
-                    "angularVelocity": -1.1282256347246662,
-                    "velocityX": 2.418419659649969,
-                    "velocityY": 0.05499503857340178,
-                    "timestamp": 0.2926381928097456
-                },
-                {
-                    "x": 1.3734034405206448,
-                    "y": 6.735064554387548,
-                    "heading": 0.7325177503671355,
-                    "angularVelocity": -1.215879744828927,
-                    "velocityX": 3.027769902429938,
-                    "velocityY": 0.06932444419316391,
-                    "timestamp": 0.36579774101218204
-                },
-                {
-                    "x": 1.6366130242279704,
-                    "y": 6.741790766846534,
-                    "heading": 0.6993734477778637,
-                    "angularVelocity": -0.45304137879066375,
-                    "velocityX": 3.5977475281693874,
-                    "velocityY": 0.09193895566910278,
-                    "timestamp": 0.43895728921461846
-                },
-                {
-                    "x": 1.8581205064725355,
-                    "y": 6.746838576232807,
-                    "heading": 0.6098795976632514,
-                    "angularVelocity": -1.223269584265702,
-                    "velocityX": 3.0277316862542305,
-                    "velocityY": 0.06899727390750988,
-                    "timestamp": 0.5121168374170548
-                },
-                {
-                    "x": 2.0350481932432154,
-                    "y": 6.750890564225749,
-                    "heading": 0.5269052400796844,
-                    "angularVelocity": -1.134156232813029,
-                    "velocityX": 2.4183813475871814,
-                    "velocityY": 0.05538563444554272,
-                    "timestamp": 0.5852763856194912
-                },
-                {
-                    "x": 2.167592642728435,
-                    "y": 6.753939698452488,
-                    "heading": 0.4602292040948866,
-                    "angularVelocity": -0.9113784546659414,
-                    "velocityX": 1.8117177148013692,
-                    "velocityY": 0.04167787119598535,
-                    "timestamp": 0.6584359338219277
-                },
-                {
-                    "x": 2.255882514523324,
-                    "y": 6.755978249842539,
-                    "heading": 0.41386063175705684,
-                    "angularVelocity": -0.633800692829844,
-                    "velocityX": 1.2068126986047898,
-                    "velocityY": 0.02786446116929368,
-                    "timestamp": 0.7315954820243641
+                    "x": 0.7541195079597249,
+                    "y": 6.720987197392724,
+                    "heading": 1.0162961519009226,
+                    "angularVelocity": -0.3240021115694771,
+                    "velocityX": 0.6030587810307456,
+                    "velocityY": 0.013493760103527496,
+                    "timestamp": 0.07315954820244353
+                },
+                {
+                    "x": 0.8424136303815098,
+                    "y": 6.722972492793459,
+                    "heading": 0.9702543819568796,
+                    "angularVelocity": -0.6293337107086672,
+                    "velocityX": 1.2068707993857855,
+                    "velocityY": 0.027136518055622637,
+                    "timestamp": 0.14631909640488705
+                },
+                {
+                    "x": 0.9749626751701804,
+                    "y": 6.7259693430054925,
+                    "heading": 0.9040114694947134,
+                    "angularVelocity": -0.9054581949963141,
+                    "velocityX": 1.8117805268819878,
+                    "velocityY": 0.0409632137659495,
+                    "timestamp": 0.21947864460733058
+                },
+                {
+                    "x": 1.1518931657803464,
+                    "y": 6.729992727527654,
+                    "heading": 0.8214709979365993,
+                    "angularVelocity": -1.1282255506783914,
+                    "velocityX": 2.4184196725842533,
+                    "velocityY": 0.0549946605880228,
+                    "timestamp": 0.2926381928097741
+                },
+                {
+                    "x": 1.3734034446931793,
+                    "y": 6.735064446892539,
+                    "heading": 0.7325177816386138,
+                    "angularVelocity": -1.215879792639604,
+                    "velocityX": 3.0277699132296103,
+                    "velocityY": 0.06932409356680827,
+                    "timestamp": 0.3657977410122176
+                },
+                {
+                    "x": 1.6366130282089124,
+                    "y": 6.741790657706068,
+                    "heading": 0.6993734786052862,
+                    "angularVelocity": -0.4530413848603092,
+                    "velocityX": 3.597747525550208,
+                    "velocityY": 0.09193893317775545,
+                    "timestamp": 0.43895728921466115
+                },
+                {
+                    "x": 1.858120509684905,
+                    "y": 6.746838490569188,
+                    "heading": 0.6098796286126051,
+                    "angularVelocity": -1.2232695825988174,
+                    "velocityX": 3.0277316757485147,
+                    "velocityY": 0.0689975948068392,
+                    "timestamp": 0.5121168374171047
+                },
+                {
+                    "x": 2.035048195511774,
+                    "y": 6.750890505964207,
+                    "heading": 0.5269052635268205,
+                    "angularVelocity": -1.1341563353588457,
+                    "velocityX": 2.418381334686234,
+                    "velocityY": 0.0553860089978706,
+                    "timestamp": 0.5852763856195482
+                },
+                {
+                    "x": 2.1675926440263233,
+                    "y": 6.753939666061267,
+                    "heading": 0.4602292179018529,
+                    "angularVelocity": -0.9113785864349585,
+                    "velocityX": 1.811717701533345,
+                    "velocityY": 0.041678224811154865,
+                    "timestamp": 0.6584359338219917
+                },
+                {
+                    "x": 2.2558825150080364,
+                    "y": 6.755978237986949,
+                    "heading": 0.41386063697230147,
+                    "angularVelocity": -0.6338008102679221,
+                    "velocityX": 1.206812687489562,
+                    "velocityY": 0.027864741865846625,
+                    "timestamp": 0.7315954820244353
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.39,
-                    "angularVelocity": -0.3261451491065467,
-                    "velocityX": 0.6030311362038534,
-                    "velocityY": 0.013966053407445108,
-                    "timestamp": 0.8047550302268005
+                    "angularVelocity": -0.3261452203924221,
+                    "velocityX": 0.6030311295783809,
+                    "velocityY": 0.013966215458624026,
+                    "timestamp": 0.8047550302268788
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.39,
-                    "angularVelocity": -1.0168243354874858e-31,
-                    "velocityX": -4.086489815881427e-33,
-                    "velocityY": 0,
-                    "timestamp": 0.8779145784292369
+                    "angularVelocity": 9.371559495672325e-33,
+                    "velocityX": 2.5226214414524116e-33,
+                    "velocityY": -5.278780681595662e-33,
+                    "timestamp": 0.8779145784293223
                 }
             ],
             "constraints": [
@@ -170,23 +170,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "Amp Side To W2": {
             "waypoints": [
@@ -214,145 +198,145 @@
                     "x": 0.71,
                     "y": 6.72,
                     "heading": 1.04,
-                    "angularVelocity": 1.094777100362113e-30,
-                    "velocityX": 3.4815734426203924e-31,
-                    "velocityY": -2.2714927632651006e-31,
+                    "angularVelocity": 3.547724392998225e-32,
+                    "velocityX": 0,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7374353381803705,
-                    "y": 6.699514089758397,
-                    "heading": 1.0076617651006685,
-                    "angularVelocity": -0.49661202140938077,
-                    "velocityX": 0.42131918437160226,
-                    "velocityY": -0.3145981630464246,
-                    "timestamp": 0.06511770457661528
-                },
-                {
-                    "x": 0.7924238459364756,
-                    "y": 6.658398285396173,
-                    "heading": 0.9457835104036431,
-                    "angularVelocity": -0.9502523944808582,
-                    "velocityX": 0.8444478827015074,
-                    "velocityY": -0.6314074586865741,
-                    "timestamp": 0.13023540915323056
-                },
-                {
-                    "x": 0.8751312050328514,
-                    "y": 6.596482140589978,
-                    "heading": 0.858451871788505,
-                    "angularVelocity": -1.3411350904174961,
-                    "velocityX": 1.270120923858195,
-                    "velocityY": -0.9508342655621027,
-                    "timestamp": 0.19535311372984585
-                },
-                {
-                    "x": 0.9857818852205986,
-                    "y": 6.513571276882533,
-                    "heading": 0.7519056244975073,
-                    "angularVelocity": -1.6362101210990878,
-                    "velocityX": 1.6992411035858814,
-                    "velocityY": -1.2732461048269026,
-                    "timestamp": 0.2604708183064611
-                },
-                {
-                    "x": 1.1246151400642461,
-                    "y": 6.409448224375173,
-                    "heading": 0.63671471411989,
-                    "angularVelocity": -1.7689645408506078,
-                    "velocityX": 2.1320354540492286,
-                    "velocityY": -1.5989975872821531,
-                    "timestamp": 0.3255885228830764
-                },
-                {
-                    "x": 1.2913898302664586,
-                    "y": 6.284148790385846,
-                    "heading": 0.5378900355869111,
-                    "angularVelocity": -1.5176314824903545,
-                    "velocityX": 2.5611266749427672,
-                    "velocityY": -1.9241991836782961,
-                    "timestamp": 0.39070622745969164
-                },
-                {
-                    "x": 1.4799020803988825,
-                    "y": 6.139172930172668,
-                    "heading": 0.5154397477236047,
-                    "angularVelocity": -0.3447647304104786,
-                    "velocityX": 2.8949461802761682,
-                    "velocityY": -2.2263662571613483,
-                    "timestamp": 0.4558239320363069
-                },
-                {
-                    "x": 1.668209892858901,
-                    "y": 5.994133297119725,
-                    "heading": 0.4928531841519364,
-                    "angularVelocity": -0.3468574901176696,
-                    "velocityX": 2.891806670464904,
-                    "velocityY": -2.2273456043324518,
-                    "timestamp": 0.5209416366129221
-                },
-                {
-                    "x": 1.8350205833080036,
-                    "y": 5.869338176931447,
-                    "heading": 0.3972353467498499,
-                    "angularVelocity": -1.4683846432207315,
-                    "velocityX": 2.5616795237743473,
-                    "velocityY": -1.9164545341343717,
-                    "timestamp": 0.5860593411895374
-                },
-                {
-                    "x": 1.9739536541090807,
-                    "y": 5.765582598648054,
-                    "heading": 0.2820330218741829,
-                    "angularVelocity": -1.7691398310903903,
-                    "velocityX": 2.1335683084100903,
-                    "velocityY": -1.5933543566683568,
-                    "timestamp": 0.6511770457661527
-                },
-                {
-                    "x": 2.084680900392731,
-                    "y": 5.683000749842365,
-                    "heading": 0.17568947614699124,
-                    "angularVelocity": -1.6330972723719295,
-                    "velocityX": 1.7004169143181675,
-                    "velocityY": -1.2681934865889841,
-                    "timestamp": 0.7162947503427679
-                },
-                {
-                    "x": 2.1674678561101657,
-                    "y": 5.621347760780931,
-                    "heading": 0.08927495882603552,
-                    "angularVelocity": -1.3270510360094632,
-                    "velocityX": 1.271343273779417,
-                    "velocityY": -0.9467930336656067,
-                    "timestamp": 0.7814124549193832
-                },
-                {
-                    "x": 2.222525102580128,
-                    "y": 5.580406412641355,
-                    "heading": 0.02852690376362466,
-                    "angularVelocity": -0.9328961371931729,
-                    "velocityX": 0.8455034898409743,
-                    "velocityY": -0.6287283682029188,
-                    "timestamp": 0.8465301594959984
+                    "x": 0.737435339105297,
+                    "y": 6.6995140898970185,
+                    "heading": 1.00766177496316,
+                    "angularVelocity": -0.49661186995303425,
+                    "velocityX": 0.4213191985755434,
+                    "velocityY": -0.3145981609176562,
+                    "timestamp": 0.06511770457661178
+                },
+                {
+                    "x": 0.7924238483684921,
+                    "y": 6.658398285939555,
+                    "heading": 0.945783535120445,
+                    "angularVelocity": -0.95025216636614,
+                    "velocityX": 0.8444479058456402,
+                    "velocityY": -0.6314074524707902,
+                    "timestamp": 0.13023540915322357
+                },
+                {
+                    "x": 0.8751312092590968,
+                    "y": 6.596482141852294,
+                    "heading": 0.8584519121155169,
+                    "angularVelocity": -1.3411348506945964,
+                    "velocityX": 1.2701209514118896,
+                    "velocityY": -0.9508342545216032,
+                    "timestamp": 0.19535311372983535
+                },
+                {
+                    "x": 0.9857818913354864,
+                    "y": 6.513571279138628,
+                    "heading": 0.7519056782658311,
+                    "angularVelocity": -1.6362099146835547,
+                    "velocityX": 1.6992411325894867,
+                    "velocityY": -1.273246089565702,
+                    "timestamp": 0.26047081830644714
+                },
+                {
+                    "x": 1.1246151480908613,
+                    "y": 6.409448227792029,
+                    "heading": 0.6367147770436085,
+                    "angularVelocity": -1.7689644002530829,
+                    "velocityX": 2.132035483407367,
+                    "velocityY": -1.5989975694566836,
+                    "timestamp": 0.32558852288305895
+                },
+                {
+                    "x": 1.2913898404503776,
+                    "y": 6.284148794801154,
+                    "heading": 0.5378900995711877,
+                    "angularVelocity": -1.5176314662036392,
+                    "velocityX": 2.5611267080722016,
+                    "velocityY": -1.9241991683453519,
+                    "timestamp": 0.39070622745967076
+                },
+                {
+                    "x": 1.479902092849603,
+                    "y": 6.139172935751937,
+                    "heading": 0.5154398140678585,
+                    "angularVelocity": -0.3447646941687853,
+                    "velocityX": 2.8949462150871557,
+                    "velocityY": -2.2263662392867607,
+                    "timestamp": 0.4558239320362826
+                },
+                {
+                    "x": 1.6682099030328927,
+                    "y": 5.994133301373275,
+                    "heading": 0.4928532472983225,
+                    "angularVelocity": -0.34685753922672635,
+                    "velocityX": 2.8918066355017875,
+                    "velocityY": -2.2273456246913312,
+                    "timestamp": 0.5209416366128944
+                },
+                {
+                    "x": 1.8350205910276742,
+                    "y": 5.8693381796986746,
+                    "heading": 0.39723540571375665,
+                    "angularVelocity": -1.4683847074503305,
+                    "velocityX": 2.561679486083952,
+                    "velocityY": -1.9164545569596636,
+                    "timestamp": 0.5860593411895062
+                },
+                {
+                    "x": 1.9739536597530634,
+                    "y": 5.765582600106914,
+                    "heading": 0.2820330722411117,
+                    "angularVelocity": -1.7691399631126123,
+                    "velocityX": 2.1335682765342634,
+                    "velocityY": -1.5933543767607987,
+                    "timestamp": 0.651177045766118
+                },
+                {
+                    "x": 2.08468090414567,
+                    "y": 5.683000750382208,
+                    "heading": 0.1756895135649168,
+                    "angularVelocity": -1.6330974712273538,
+                    "velocityX": 1.7004168852778658,
+                    "velocityY": -1.2681935007022147,
+                    "timestamp": 0.7162947503427298
+                },
+                {
+                    "x": 2.167467858188685,
+                    "y": 5.6213477608369935,
+                    "heading": 0.08927498136102419,
+                    "angularVelocity": -1.3270512645639152,
+                    "velocityX": 1.2713432480657338,
+                    "velocityY": -0.946793041094972,
+                    "timestamp": 0.7814124549193416
+                },
+                {
+                    "x": 2.2225251033432207,
+                    "y": 5.5804064125777195,
+                    "heading": 0.028526912579098198,
+                    "angularVelocity": -0.9328963478811614,
+                    "velocityX": 0.8455034696402836,
+                    "velocityY": -0.628728370041152,
+                    "timestamp": 0.8465301594959534
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
                     "heading": -0.003,
-                    "angularVelocity": -0.4841525660126885,
-                    "velocityX": 0.42192668796465593,
-                    "velocityY": -0.3133773337686722,
-                    "timestamp": 0.9116478640726137
+                    "angularVelocity": -0.4841527013902356,
+                    "velocityX": 0.42192667624600166,
+                    "velocityY": -0.31337733279145685,
+                    "timestamp": 0.9116478640725653
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
                     "heading": -0.003,
-                    "angularVelocity": 5.587549482799738e-30,
-                    "velocityX": -2.981373275826657e-31,
-                    "velocityY": -1.0757932056005479e-30,
-                    "timestamp": 0.9767655686492289
+                    "angularVelocity": -3.2592320489843035e-28,
+                    "velocityX": -3.6408651267401044e-33,
+                    "velocityY": 2.9208085569078165e-32,
+                    "timestamp": 0.9767655686491771
                 }
             ],
             "constraints": [
@@ -372,23 +356,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "Amp Side To W3": {
             "waypoints": [
@@ -416,190 +384,190 @@
                     "x": 0.71,
                     "y": 6.72,
                     "heading": 1.04,
-                    "angularVelocity": 1.9442039286246259e-31,
-                    "velocityX": 1.5017390020853034e-32,
-                    "velocityY": 2.127944688535994e-32,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7269018921676205,
-                    "y": 6.6955064720122275,
-                    "heading": 1.0019343004430092,
-                    "angularVelocity": -0.6189082678684702,
-                    "velocityX": 0.2748070028110285,
-                    "velocityY": -0.3982390224617833,
-                    "timestamp": 0.06150459047524045
-                },
-                {
-                    "x": 0.7606735160812093,
-                    "y": 6.646281374376347,
-                    "heading": 0.9275074281520226,
-                    "angularVelocity": -1.2101027210472746,
-                    "velocityX": 0.5490911109664911,
-                    "velocityY": -0.8003483521396102,
-                    "timestamp": 0.1230091809504809
-                },
-                {
-                    "x": 0.8112864226090604,
-                    "y": 6.571983368952451,
-                    "heading": 0.8196845896852679,
-                    "angularVelocity": -1.7530860320118746,
-                    "velocityX": 0.8229126661403596,
-                    "velocityY": -1.2080074812270452,
-                    "timestamp": 0.18451377142572134
-                },
-                {
-                    "x": 0.8787769249457482,
-                    "y": 6.472119220055144,
-                    "heading": 0.6843882308491542,
-                    "angularVelocity": -2.199776598635823,
-                    "velocityX": 1.0973246356929591,
-                    "velocityY": -1.6236861041698774,
-                    "timestamp": 0.2460183619009618
-                },
-                {
-                    "x": 0.9632583450726888,
-                    "y": 6.346050730942796,
-                    "heading": 0.5331821377651369,
-                    "angularVelocity": -2.458452156427049,
-                    "velocityX": 1.373579101562342,
-                    "velocityY": -2.0497411353888846,
-                    "timestamp": 0.30752295237620225
-                },
-                {
-                    "x": 1.064336906516991,
-                    "y": 6.193349961542198,
-                    "heading": 0.3933800445651471,
-                    "angularVelocity": -2.273035103880077,
-                    "velocityX": 1.6434311758403277,
-                    "velocityY": -2.4827540224346163,
-                    "timestamp": 0.3690275428514427
-                },
-                {
-                    "x": 1.1776989215840556,
-                    "y": 6.018028084645807,
-                    "heading": 0.32650139843607295,
-                    "angularVelocity": -1.0873764968160713,
-                    "velocityX": 1.8431472218760627,
-                    "velocityY": -2.8505494556047704,
-                    "timestamp": 0.4305321333266831
-                },
-                {
-                    "x": 1.3085726451154573,
-                    "y": 5.825657208090679,
-                    "heading": 0.3265013287499931,
-                    "angularVelocity": -0.0000011330224187482564,
-                    "velocityX": 2.1278691967567314,
-                    "velocityY": -3.1277482716118437,
-                    "timestamp": 0.49203672380192354
-                },
-                {
-                    "x": 1.4394478126055321,
-                    "y": 5.633287313894505,
-                    "heading": 0.3265012590706379,
-                    "angularVelocity": -0.000001132913083706052,
-                    "velocityX": 2.1278926740071626,
-                    "velocityY": -3.127732299487714,
-                    "timestamp": 0.553541314277164
-                },
-                {
-                    "x": 1.570322997049186,
-                    "y": 5.440917431232361,
-                    "heading": 0.3265011893913128,
-                    "angularVelocity": -0.0000011329125928412157,
-                    "velocityX": 2.1278929496545396,
-                    "velocityY": -3.1277321119565196,
-                    "timestamp": 0.6150459047524045
-                },
-                {
-                    "x": 1.7011982004807327,
-                    "y": 5.248547561488253,
-                    "heading": 0.32650111971202045,
-                    "angularVelocity": -0.0000011329120610367875,
-                    "velocityX": 2.1278932583777177,
-                    "velocityY": -3.12773190192281,
-                    "timestamp": 0.676550495227645
-                },
-                {
-                    "x": 1.8320716066458687,
-                    "y": 5.056176469023738,
-                    "heading": 0.3265010500238972,
-                    "angularVelocity": -0.0000011330556420647947,
-                    "velocityX": 2.1278640367148025,
-                    "velocityY": -3.127751782071573,
-                    "timestamp": 0.7380550857028855
-                },
-                {
-                    "x": 1.9453996822937256,
-                    "y": 4.881259566133339,
-                    "heading": 0.2607759086629828,
-                    "angularVelocity": -1.0686217216156084,
-                    "velocityX": 1.8425954025899711,
-                    "velocityY": -2.843965003893719,
-                    "timestamp": 0.799559676178126
-                },
-                {
-                    "x": 2.0464428806975716,
-                    "y": 4.730058134054239,
-                    "heading": 0.1310608851796909,
-                    "angularVelocity": -2.109029951764503,
-                    "velocityX": 1.6428562099689532,
-                    "velocityY": -2.458376373385783,
-                    "timestamp": 0.8610642666533664
-                },
-                {
-                    "x": 2.1311236772079067,
-                    "y": 4.60496312733231,
-                    "heading": -0.022718365147026284,
-                    "angularVelocity": -2.500288988813334,
-                    "velocityX": 1.3768207520124016,
-                    "velocityY": -2.0339133348475604,
-                    "timestamp": 0.9225688571286069
-                },
-                {
-                    "x": 2.1986971663058994,
-                    "y": 4.506052833762169,
-                    "heading": -0.16200277924985632,
-                    "angularVelocity": -2.264618185839331,
-                    "velocityX": 1.0986739132129535,
-                    "velocityY": -1.6081774190490494,
-                    "timestamp": 0.9840734476038474
-                },
-                {
-                    "x": 2.2493424732278595,
-                    "y": 4.432626695040595,
-                    "heading": -0.27409854656488936,
-                    "angularVelocity": -1.822559364250361,
-                    "velocityX": 0.823439462495856,
-                    "velocityY": -1.1938318449763972,
-                    "timestamp": 1.0455780380790878
-                },
-                {
-                    "x": 2.2831120594312195,
-                    "y": 4.384102039370989,
-                    "heading": -0.35252594149925776,
-                    "angularVelocity": -1.2751470146921873,
-                    "velocityX": 0.5490579799398,
-                    "velocityY": -0.7889599019302513,
-                    "timestamp": 1.1070826285543283
+                    "x": 0.7269019070910594,
+                    "y": 6.695506477136938,
+                    "heading": 1.0019343442516249,
+                    "angularVelocity": -0.6189075555866055,
+                    "velocityX": 0.2748072454505401,
+                    "velocityY": -0.3982389391394925,
+                    "timestamp": 0.06150459047522247
+                },
+                {
+                    "x": 0.7606735605009036,
+                    "y": 6.646281390208111,
+                    "heading": 0.9275075561119867,
+                    "angularVelocity": -1.2101013528351434,
+                    "velocityX": 0.5490915905447641,
+                    "velocityY": -0.8003481780543988,
+                    "timestamp": 0.12300918095044494
+                },
+                {
+                    "x": 0.8112865108119746,
+                    "y": 6.571983401826325,
+                    "heading": 0.8196848361236891,
+                    "angularVelocity": -1.7530841056771946,
+                    "velocityX": 0.8229133780097379,
+                    "velocityY": -1.2080072041406207,
+                    "timestamp": 0.1845137714256674
+                },
+                {
+                    "x": 0.8787770707149783,
+                    "y": 6.472119277485466,
+                    "heading": 0.6843886163577808,
+                    "angularVelocity": -2.199774337501089,
+                    "velocityX": 1.0973255716610744,
+                    "velocityY": -1.6236857049083067,
+                    "timestamp": 0.24601836190088988
+                },
+                {
+                    "x": 0.9632585614164454,
+                    "y": 6.3460508217508655,
+                    "heading": 0.5331826539280757,
+                    "angularVelocity": -2.4584500321259677,
+                    "velocityX": 1.373580249030372,
+                    "velocityY": -2.0497405927024026,
+                    "timestamp": 0.3075229523761124
+                },
+                {
+                    "x": 1.0643372089754837,
+                    "y": 6.193350092274746,
+                    "heading": 0.39338059647753487,
+                    "angularVelocity": -2.273034522632274,
+                    "velocityX": 1.6434325759759127,
+                    "velocityY": -2.4827533733053033,
+                    "timestamp": 0.3690275428513349
+                },
+                {
+                    "x": 1.1776993188507112,
+                    "y": 6.018028259717655,
+                    "heading": 0.3265019863738018,
+                    "angularVelocity": -1.087375911082207,
+                    "velocityX": 1.843148763357691,
+                    "velocityY": -2.85054873469518,
+                    "timestamp": 0.4305321333265574
+                },
+                {
+                    "x": 1.3085730997897271,
+                    "y": 5.825657422218107,
+                    "heading": 0.3265019166878518,
+                    "angularVelocity": -0.0000011330203075824657,
+                    "velocityX": 2.1278701301448275,
+                    "velocityY": -3.1277476366100623,
+                    "timestamp": 0.4920367238017799
+                },
+                {
+                    "x": 1.4394483094166801,
+                    "y": 5.633287556689,
+                    "heading": 0.32650184700859936,
+                    "angularVelocity": -0.00000113291141146818,
+                    "velocityX": 2.1278933591091382,
+                    "velocityY": -3.127731833392264,
+                    "timestamp": 0.5535413142770024
+                },
+                {
+                    "x": 1.5703235190623746,
+                    "y": 5.4409176911726425,
+                    "heading": 0.32650177732934843,
+                    "angularVelocity": -0.0000011329113877746253,
+                    "velocityX": 2.127893359413853,
+                    "velocityY": -3.1277318331849595,
+                    "timestamp": 0.6150459047522249
+                },
+                {
+                    "x": 1.7011987286842072,
+                    "y": 5.24854782564005,
+                    "heading": 0.32650170765009895,
+                    "angularVelocity": -0.0000011329113643360664,
+                    "velocityX": 2.127893359025885,
+                    "velocityY": -3.1277318334489093,
+                    "timestamp": 0.6765504952274474
+                },
+                {
+                    "x": 1.8320721192807423,
+                    "y": 5.056176722584032,
+                    "heading": 0.32650163796198384,
+                    "angularVelocity": -0.000001133055510645955,
+                    "velocityX": 2.1278637835863385,
+                    "velocityY": -3.1277519542791743,
+                    "timestamp": 0.7380550857026699
+                },
+                {
+                    "x": 1.9454000953072241,
+                    "y": 4.881259770298511,
+                    "heading": 0.26077643177319376,
+                    "angularVelocity": -1.0686227756490447,
+                    "velocityX": 1.8425937828516514,
+                    "velocityY": -2.843965807007333,
+                    "timestamp": 0.7995596761778924
+                },
+                {
+                    "x": 2.046443192812682,
+                    "y": 4.7300582879948045,
+                    "heading": 0.13106133632258532,
+                    "angularVelocity": -2.1090311218780498,
+                    "velocityX": 1.6428545694676804,
+                    "velocityY": -2.4583771899858244,
+                    "timestamp": 0.8610642666531149
+                },
+                {
+                    "x": 2.131123897414369,
+                    "y": 4.604963233646066,
+                    "heading": -0.022718028385538325,
+                    "angularVelocity": -2.50029084853552,
+                    "velocityX": 1.376819257674782,
+                    "velocityY": -2.0339141092100017,
+                    "timestamp": 0.9225688571283374
+                },
+                {
+                    "x": 2.198697305455373,
+                    "y": 4.506052899677577,
+                    "heading": -0.16200255999945734,
+                    "angularVelocity": -2.264620096446796,
+                    "velocityX": 1.0986725953118257,
+                    "velocityY": -1.6081780758842066,
+                    "timestamp": 0.9840734476035599
+                },
+                {
+                    "x": 2.249342546290196,
+                    "y": 4.432626729162034,
+                    "heading": -0.2740984297313893,
+                    "angularVelocity": -1.822561029442031,
+                    "velocityX": 0.8234383879887088,
+                    "velocityY": -1.1938323619132651,
+                    "timestamp": 1.0455780380787822
+                },
+                {
+                    "x": 2.283112085041758,
+                    "y": 4.38410205128396,
+                    "heading": -0.35252590091377134,
+                    "angularVelocity": -1.2751482544051254,
+                    "velocityX": 0.549057208423595,
+                    "velocityY": -0.788960263016835,
+                    "timestamp": 1.1070826285540047
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
                     "heading": -0.393,
-                    "angularVelocity": -0.6580656531163417,
-                    "velocityX": 0.2745801644769678,
-                    "velocityY": -0.3918738290061591,
-                    "timestamp": 1.1685872190295687
+                    "angularVelocity": -0.6580663129938888,
+                    "velocityX": 0.2745797480766085,
+                    "velocityY": -0.39187402269867344,
+                    "timestamp": 1.1685872190292272
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
                     "heading": -0.393,
-                    "angularVelocity": -1.5278905371426354e-30,
-                    "velocityX": 0,
-                    "velocityY": -1.0222462264365078e-31,
-                    "timestamp": 1.2300918095048092
+                    "angularVelocity": 0,
+                    "velocityX": 1.1335977805694889e-32,
+                    "velocityY": 0,
+                    "timestamp": 1.2300918095044497
                 }
             ],
             "constraints": [
@@ -619,23 +587,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "Amp Side To C1": {
             "waypoints": [
@@ -664,270 +616,270 @@
                     "y": 6.72,
                     "heading": 1.04,
                     "angularVelocity": 0,
-                    "velocityX": -1.8357629106361847e-33,
+                    "velocityX": -1.8098951472235817e-30,
                     "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7613816250456615,
-                    "y": 6.725489542887092,
-                    "heading": 0.9931061774124983,
-                    "angularVelocity": -0.5863053943309177,
-                    "velocityX": 0.6424156162050443,
-                    "velocityY": 0.06863481007775876,
-                    "timestamp": 0.07998190540446273
-                },
-                {
-                    "x": 0.8643774278996095,
-                    "y": 6.736651981527731,
-                    "heading": 0.9029653992770575,
-                    "angularVelocity": -1.1270146376184127,
-                    "velocityX": 1.2877387995835512,
-                    "velocityY": 0.13956204949346018,
-                    "timestamp": 0.15996381080892547
-                },
-                {
-                    "x": 1.019343252130834,
-                    "y": 6.753670139482374,
-                    "heading": 0.7755395431364547,
-                    "angularVelocity": -1.5931835519073916,
-                    "velocityX": 1.9375110338716452,
-                    "velocityY": 0.21277510042531103,
-                    "timestamp": 0.2399457162133882
-                },
-                {
-                    "x": 1.2269177512246652,
-                    "y": 6.77676689021641,
-                    "heading": 0.6253872743553933,
-                    "angularVelocity": -1.8773279783940144,
-                    "velocityX": 2.5952682427874385,
-                    "velocityY": 0.2887746999429258,
-                    "timestamp": 0.31992762161785093
-                },
-                {
-                    "x": 1.4861215960194483,
-                    "y": 6.806717977426302,
-                    "heading": 0.5166968690593423,
-                    "angularVelocity": -1.358937433990995,
-                    "velocityX": 3.240781067717853,
-                    "velocityY": 0.37447328940753305,
-                    "timestamp": 0.39990952702231364
-                },
-                {
-                    "x": 1.7870123176597594,
-                    "y": 6.838521008932437,
-                    "heading": 0.5166968273679649,
-                    "angularVelocity": -5.212601175914625e-7,
-                    "velocityX": 3.7619849154471687,
-                    "velocityY": 0.397627830261236,
-                    "timestamp": 0.47989143242677634
-                },
-                {
-                    "x": 2.087903104978946,
-                    "y": 6.870323419047386,
-                    "heading": 0.5166967856787098,
-                    "angularVelocity": -5.212335818457529e-7,
-                    "velocityX": 3.7619857366188425,
-                    "velocityY": 0.3976200611141572,
-                    "timestamp": 0.559873337831239
-                },
-                {
-                    "x": 2.3887938966166886,
-                    "y": 6.9021257883033815,
-                    "heading": 0.5166967439894641,
-                    "angularVelocity": -5.212334661065162e-7,
-                    "velocityX": 3.761985790613012,
-                    "velocityY": 0.3976195502616874,
-                    "timestamp": 0.6398552432357018
-                },
-                {
-                    "x": 2.689684692935055,
-                    "y": 6.933928113274756,
-                    "heading": 0.5166967023002289,
-                    "angularVelocity": -5.212333328622992e-7,
-                    "velocityX": 3.76198584913404,
-                    "velocityY": 0.3976189965786884,
-                    "timestamp": 0.7198371486401645
-                },
-                {
-                    "x": 2.990575494322058,
-                    "y": 6.965730390290324,
-                    "heading": 0.5166966606110028,
-                    "angularVelocity": -5.212332194068621e-7,
-                    "velocityX": 3.761985912506342,
-                    "velocityY": 0.3976183969955037,
-                    "timestamp": 0.7998190540446272
-                },
-                {
-                    "x": 3.291466301153434,
-                    "y": 6.997532615795069,
-                    "heading": 0.5166966189217854,
-                    "angularVelocity": -5.21233112050001e-7,
-                    "velocityX": 3.76198598057639,
-                    "velocityY": 0.39761775296452984,
-                    "timestamp": 0.8798009594490899
-                },
-                {
-                    "x": 3.592357113798132,
-                    "y": 7.029334786298146,
-                    "heading": 0.5166965772325762,
-                    "angularVelocity": -5.212330086999819e-7,
-                    "velocityX": 3.7619860532593528,
-                    "velocityY": 0.39761706528816065,
-                    "timestamp": 0.9597828648535526
-                },
-                {
-                    "x": 3.8932479326255467,
-                    "y": 7.061136898304478,
-                    "heading": 0.5166965355433745,
-                    "angularVelocity": -5.212329156376395e-7,
-                    "velocityX": 3.7619861305608038,
-                    "velocityY": 0.3976163339134084,
-                    "timestamp": 1.0397647702580153
-                },
-                {
-                    "x": 4.194138758019097,
-                    "y": 7.092938948186284,
-                    "heading": 0.5166964938541775,
-                    "angularVelocity": -5.212328561470798e-7,
-                    "velocityX": 3.7619862126560695,
-                    "velocityY": 0.3976155571811608,
-                    "timestamp": 1.119746675662478
-                },
-                {
-                    "x": 4.495029590361164,
-                    "y": 7.1247409323255795,
-                    "heading": 0.516696452164982,
-                    "angularVelocity": -5.212328389753682e-7,
-                    "velocityX": 3.7619862995321793,
-                    "velocityY": 0.397614735213847,
-                    "timestamp": 1.199728581066941
-                },
-                {
-                    "x": 4.79592042996658,
-                    "y": 7.15654284774347,
-                    "heading": 0.5166964104757901,
-                    "angularVelocity": -5.212327914819509e-7,
-                    "velocityX": 3.7619863903445805,
-                    "velocityY": 0.39761387600196396,
-                    "timestamp": 1.2797104864714037
-                },
-                {
-                    "x": 5.096811277131865,
-                    "y": 7.188344691634321,
-                    "heading": 0.5166963687866055,
-                    "angularVelocity": -5.212326996843497e-7,
-                    "velocityX": 3.7619864848642997,
-                    "velocityY": 0.39761298171169474,
-                    "timestamp": 1.3596923918758665
-                },
-                {
-                    "x": 5.397702132107731,
-                    "y": 7.220146461625841,
-                    "heading": 0.5166963270974331,
-                    "angularVelocity": -5.212325496997691e-7,
-                    "velocityX": 3.761986582518669,
-                    "velocityY": 0.3976120577610798,
-                    "timestamp": 1.4396742972803294
-                },
-                {
-                    "x": 5.698592995103496,
-                    "y": 7.251948155737401,
-                    "heading": 0.516696285408269,
-                    "angularVelocity": -5.212324447299826e-7,
-                    "velocityX": 3.7619866827900856,
-                    "velocityY": 0.3976111090469843,
-                    "timestamp": 1.5196562026847922
-                },
-                {
-                    "x": 5.99948386596862,
-                    "y": 7.283749775393132,
-                    "heading": 0.5166962437191063,
-                    "angularVelocity": -5.212324282706134e-7,
-                    "velocityX": 3.7619867811793206,
-                    "velocityY": 0.3976101781385786,
-                    "timestamp": 1.599638108089255
-                },
-                {
-                    "x": 6.300374744112002,
-                    "y": 7.315551326185539,
-                    "heading": 0.5166962020299369,
-                    "angularVelocity": -5.212325087463018e-7,
-                    "velocityX": 3.761986872178135,
-                    "velocityY": 0.3976093171522739,
-                    "timestamp": 1.6796200134937178
-                },
-                {
-                    "x": 6.6012656287075195,
-                    "y": 7.347352815930844,
-                    "heading": 0.5166961603407556,
-                    "angularVelocity": -5.21232660707814e-7,
-                    "velocityX": 3.7619869528480785,
-                    "velocityY": 0.39760855389086563,
-                    "timestamp": 1.7596019188981806
-                },
-                {
-                    "x": 6.9021564720244495,
-                    "y": 7.37915469622686,
-                    "heading": 0.5166961186499832,
-                    "angularVelocity": -5.212525529907653e-7,
-                    "velocityX": 3.7619864367489835,
-                    "velocityY": 0.3976134368792068,
-                    "timestamp": 1.8395838243026434
-                },
-                {
-                    "x": 7.162090895853125,
-                    "y": 7.409612003973996,
-                    "heading": 0.405130711689902,
-                    "angularVelocity": -1.3948830850665912,
-                    "velocityX": 3.2499153716606957,
-                    "velocityY": 0.3808024776743751,
-                    "timestamp": 1.9195657297071063
-                },
-                {
-                    "x": 7.370036531434433,
-                    "y": 7.432975355945854,
-                    "heading": 0.2588526808330054,
-                    "angularVelocity": -1.8288890483063531,
-                    "velocityX": 2.5999084984252714,
-                    "velocityY": 0.2921079693427033,
-                    "timestamp": 1.999547635111569
-                },
-                {
-                    "x": 7.52532693851905,
-                    "y": 7.450135904743354,
-                    "heading": 0.13353023499549588,
-                    "angularVelocity": -1.5668849748422837,
-                    "velocityX": 1.9415692374334488,
-                    "velocityY": 0.21455538863097798,
-                    "timestamp": 2.079529540516032
-                },
+                    "x": 0.7613816362208097,
+                    "y": 6.725489442192221,
+                    "heading": 0.9931061799728784,
+                    "angularVelocity": -0.5863053623193526,
+                    "velocityX": 0.6424157559264662,
+                    "velocityY": 0.06863355110715653,
+                    "timestamp": 0.07998190540440472
+                },
+                {
+                    "x": 0.8643774617687672,
+                    "y": 6.736651681634022,
+                    "heading": 0.9029654144342619,
+                    "angularVelocity": -1.127014480123304,
+                    "velocityX": 1.287739083323781,
+                    "velocityY": 0.1395595589447693,
+                    "timestamp": 0.15996381080880945
+                },
+                {
+                    "x": 1.019343320389449,
+                    "y": 6.753669544326843,
+                    "heading": 0.7755395878520636,
+                    "angularVelocity": -1.5931831823449016,
+                    "velocityX": 1.9375114638385158,
+                    "velocityY": 0.21277140881771536,
+                    "timestamp": 0.23994571621321417
+                },
+                {
+                    "x": 1.2269178654194646,
+                    "y": 6.7767659053404685,
+                    "heading": 0.62538736801047,
+                    "angularVelocity": -1.8773273665136312,
+                    "velocityX": 2.5952688171215317,
+                    "velocityY": 0.2887698273359004,
+                    "timestamp": 0.3199276216176189
+                },
+                {
+                    "x": 1.4861217663901574,
+                    "y": 6.806716511012647,
+                    "heading": 0.5166970392894589,
+                    "angularVelocity": -1.3589364765874345,
+                    "velocityX": 3.240781770077933,
+                    "velocityY": 0.37446726882464465,
+                    "timestamp": 0.39990952702202365
+                },
+                {
+                    "x": 1.7870125383049598,
+                    "y": 6.838519066862702,
+                    "heading": 0.5166969975980924,
+                    "angularVelocity": -5.212599803241117e-7,
+                    "velocityX": 3.76198554402321,
+                    "velocityY": 0.39762188321538583,
+                    "timestamp": 0.4798914324264284
+                },
+                {
+                    "x": 2.0879033719171214,
+                    "y": 6.870321038982823,
+                    "heading": 0.5166969559088411,
+                    "angularVelocity": -5.212335340785354e-7,
+                    "velocityX": 3.761986315414672,
+                    "velocityY": 0.39761458494048757,
+                    "timestamp": 0.5598733378308332
+                },
+                {
+                    "x": 2.3887942055296523,
+                    "y": 6.902123011099449,
+                    "heading": 0.5166969142195897,
+                    "angularVelocity": -5.212335365150009e-7,
+                    "velocityX": 3.7619863154192914,
+                    "velocityY": 0.3976145848967806,
+                    "timestamp": 0.6398552432352379
+                },
+                {
+                    "x": 2.689685039142183,
+                    "y": 6.933924983216074,
+                    "heading": 0.516696872530338,
+                    "angularVelocity": -5.212335398189914e-7,
+                    "velocityX": 3.7619863154192914,
+                    "velocityY": 0.3976145848967804,
+                    "timestamp": 0.7198371486396427
+                },
+                {
+                    "x": 2.990575872754714,
+                    "y": 6.9657269553327,
+                    "heading": 0.5166968308410861,
+                    "angularVelocity": -5.212335433606452e-7,
+                    "velocityX": 3.7619863154192914,
+                    "velocityY": 0.39761458489678053,
+                    "timestamp": 0.7998190540440474
+                },
+                {
+                    "x": 3.291466706367245,
+                    "y": 6.997528927449325,
+                    "heading": 0.5166967891518339,
+                    "angularVelocity": -5.212335460884143e-7,
+                    "velocityX": 3.761986315419291,
+                    "velocityY": 0.3976145848967808,
+                    "timestamp": 0.8798009594484522
+                },
+                {
+                    "x": 3.5923575399797762,
+                    "y": 7.0293308995659505,
+                    "heading": 0.5166967474625817,
+                    "angularVelocity": -5.212335479348436e-7,
+                    "velocityX": 3.761986315419291,
+                    "velocityY": 0.3976145848967805,
+                    "timestamp": 0.9597828648528569
+                },
+                {
+                    "x": 3.893248373592307,
+                    "y": 7.0611328716825765,
+                    "heading": 0.516696705773329,
+                    "angularVelocity": -5.212335516712931e-7,
+                    "velocityX": 3.7619863154192905,
+                    "velocityY": 0.3976145848967809,
+                    "timestamp": 1.0397647702572617
+                },
+                {
+                    "x": 4.194139207204837,
+                    "y": 7.092934843799203,
+                    "heading": 0.5166966640840763,
+                    "angularVelocity": -5.212335535473894e-7,
+                    "velocityX": 3.7619863154192905,
+                    "velocityY": 0.3976145848967809,
+                    "timestamp": 1.1197466756616663
+                },
+                {
+                    "x": 4.495030040817368,
+                    "y": 7.1247368159158295,
+                    "heading": 0.5166966223948233,
+                    "angularVelocity": -5.212335568545237e-7,
+                    "velocityX": 3.7619863154192905,
+                    "velocityY": 0.3976145848967808,
+                    "timestamp": 1.199728581066071
+                },
+                {
+                    "x": 4.7959208744299,
+                    "y": 7.156538788032456,
+                    "heading": 0.51669658070557,
+                    "angularVelocity": -5.212335599678648e-7,
+                    "velocityX": 3.7619863154192905,
+                    "velocityY": 0.397614584896781,
+                    "timestamp": 1.2797104864704756
+                },
+                {
+                    "x": 5.096811708042431,
+                    "y": 7.1883407601490825,
+                    "heading": 0.5166965390163165,
+                    "angularVelocity": -5.212335628762146e-7,
+                    "velocityX": 3.7619863154192905,
+                    "velocityY": 0.3976145848967809,
+                    "timestamp": 1.3596923918748802
+                },
+                {
+                    "x": 5.3977025416549615,
+                    "y": 7.220142732265709,
+                    "heading": 0.5166964973270628,
+                    "angularVelocity": -5.212335655841188e-7,
+                    "velocityX": 3.76198631541929,
+                    "velocityY": 0.3976145848967813,
+                    "timestamp": 1.4396742972792849
+                },
+                {
+                    "x": 5.698593375267492,
+                    "y": 7.251944704382335,
+                    "heading": 0.5166964556378089,
+                    "angularVelocity": -5.212335680260379e-7,
+                    "velocityX": 3.7619863154192896,
+                    "velocityY": 0.3976145848967809,
+                    "timestamp": 1.5196562026836895
+                },
+                {
+                    "x": 5.999484208880023,
+                    "y": 7.283746676498962,
+                    "heading": 0.5166964139485547,
+                    "angularVelocity": -5.212335712969897e-7,
+                    "velocityX": 3.7619863154192896,
+                    "velocityY": 0.3976145848967812,
+                    "timestamp": 1.5996381080880941
+                },
+                {
+                    "x": 6.300375042492554,
+                    "y": 7.315548648615588,
+                    "heading": 0.5166963722593003,
+                    "angularVelocity": -5.21233573548864e-7,
+                    "velocityX": 3.7619863154192896,
+                    "velocityY": 0.39761458489678125,
+                    "timestamp": 1.6796200134924988
+                },
+                {
+                    "x": 6.6012658761048035,
+                    "y": 7.347350620734872,
+                    "heading": 0.5166963305700458,
+                    "angularVelocity": -5.212335762453642e-7,
+                    "velocityX": 3.7619863154157773,
+                    "velocityY": 0.3976145849300086,
+                    "timestamp": 1.7596019188969034
+                },
+                {
+                    "x": 6.902156662813089,
+                    "y": 7.3791530366221005,
+                    "heading": 0.5166962888792155,
+                    "angularVelocity": -5.212532766918364e-7,
+                    "velocityX": 3.761985728983584,
+                    "velocityY": 0.3976201332842598,
+                    "timestamp": 1.839583824301308
+                },
+                {
+                    "x": 7.162091022778527,
+                    "y": 7.409610892063227,
+                    "heading": 0.4051307797997224,
+                    "angularVelocity": -1.3948843618490365,
+                    "velocityX": 3.249914573191993,
+                    "velocityY": 0.3808093253983604,
+                    "timestamp": 1.9195657297057127
+                },
+                {
+                    "x": 7.370036606479622,
+                    "y": 7.432974685540162,
+                    "heading": 0.25885269617169443,
+                    "angularVelocity": -1.8288897080960538,
+                    "velocityX": 2.5999078497777726,
+                    "velocityY": 0.2921134894049214,
+                    "timestamp": 1.9995476351101173
+                },
+                {
+                    "x": 7.525326975450784,
+                    "y": 7.45013556763307,
+                    "heading": 0.1335302309738367,
+                    "angularVelocity": -1.5668852169025227,
+                    "velocityX": 1.9415687609088876,
+                    "velocityY": 0.21455955576625652,
+                    "timestamp": 2.079529540514522
+                },
                 {
-                    "x": 7.628525628712187,
-                    "y": 7.461415184148821,
-                    "heading": 0.04391070884179766,
-                    "angularVelocity": -1.120497513787634,
-                    "velocityX": 1.2902754650726174,
-                    "velocityY": 0.14102288947017258,
-                    "timestamp": 2.1595114459204945
+                    "x": 7.628525640862077,
+                    "y": 7.461415071089343,
+                    "heading": 0.043910704401711645,
+                    "angularVelocity": -1.1204975190199653,
+                    "velocityX": 1.2902751552304177,
+                    "velocityY": 0.14102569073894916,
+                    "timestamp": 2.159511445918927
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
                     "heading": -0.003,
-                    "angularVelocity": -0.5865165202625967,
-                    "velocityX": 0.6435752065109067,
-                    "velocityY": 0.06982599155316412,
-                    "timestamp": 2.239493351324957
+                    "angularVelocity": -0.5865164647493907,
+                    "velocityX": 0.6435750546033896,
+                    "velocityY": 0.0698274051164111,
+                    "timestamp": 2.2394933513233317
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
                     "heading": -0.003,
-                    "angularVelocity": -3.4580717798993225e-32,
-                    "velocityX": 0,
+                    "angularVelocity": 0,
+                    "velocityX": 4.312091780947568e-36,
                     "velocityY": 0,
-                    "timestamp": 2.3194752567294197
+                    "timestamp": 2.3194752567277366
                 }
             ],
             "constraints": [
@@ -947,23 +899,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "Amp Side To C2": {
             "waypoints": [
@@ -974,7 +910,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 29
+                    "controlIntervalCount": 24
+                },
+                {
+                    "x": 5.9063591957092285,
+                    "y": 6.791097164154053,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 10
                 },
                 {
                     "x": 7.68,
@@ -992,270 +937,315 @@
                     "y": 6.72,
                     "heading": 1.04,
                     "angularVelocity": 0,
-                    "velocityX": 8.672453699203877e-33,
-                    "velocityY": 0,
+                    "velocityX": 2.674890564447798e-33,
+                    "velocityY": 3.559444452002106e-32,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7586383226811528,
-                    "y": 6.716006460523898,
-                    "heading": 0.9415437629009579,
-                    "angularVelocity": -1.201643840291405,
-                    "velocityX": 0.5936235486342963,
-                    "velocityY": -0.048740559804155884,
-                    "timestamp": 0.08193462471805792
-                },
-                {
-                    "x": 0.8580797410236602,
-                    "y": 6.713039346170398,
-                    "heading": 0.7612656224060018,
-                    "angularVelocity": -2.2002680931938623,
-                    "velocityX": 1.2136678319415195,
-                    "velocityY": -0.03621319269735464,
-                    "timestamp": 0.16386924943611583
-                },
-                {
-                    "x": 1.0092416285400778,
-                    "y": 6.712497099188053,
-                    "heading": 0.5037500418751092,
-                    "angularVelocity": -3.142939647518002,
-                    "velocityX": 1.8449085235524658,
-                    "velocityY": -0.006618044376361366,
-                    "timestamp": 0.24580387415417376
-                },
-                {
-                    "x": 1.2158692368887885,
-                    "y": 6.714801206948958,
-                    "heading": 0.22114949007633808,
-                    "angularVelocity": -3.4490980189537326,
-                    "velocityX": 2.5218594588030294,
-                    "velocityY": 0.028121295103663166,
-                    "timestamp": 0.32773849887223166
-                },
-                {
-                    "x": 1.4708450332398146,
-                    "y": 6.7208733735889465,
-                    "heading": 0.04860948021713714,
-                    "angularVelocity": -2.105825351039585,
-                    "velocityX": 3.1119419565100066,
-                    "velocityY": 0.07410989750528489,
-                    "timestamp": 0.40967312359028957
-                },
-                {
-                    "x": 1.7704680006957136,
-                    "y": 6.724924507610315,
-                    "heading": 0.013522063773945665,
-                    "angularVelocity": -0.42823673830116915,
-                    "velocityX": 3.6568540893049817,
-                    "velocityY": 0.049443492727383076,
-                    "timestamp": 0.49160774830834747
-                },
-                {
-                    "x": 2.080344251704432,
-                    "y": 6.7179919202849705,
-                    "heading": 0.013521948546161235,
-                    "angularVelocity": -0.000001406338099756076,
-                    "velocityX": 3.78199390154067,
-                    "velocityY": -0.08461120496004566,
-                    "timestamp": 0.5735423730264054
-                },
-                {
-                    "x": 2.3902205002379273,
-                    "y": 6.711059222319389,
-                    "heading": 0.013521833318547862,
-                    "angularVelocity": -0.000001406336012025862,
-                    "velocityX": 3.781993871330935,
-                    "velocityY": -0.08461255530781114,
-                    "timestamp": 0.6554769977444633
-                },
-                {
-                    "x": 2.7000967477974154,
-                    "y": 6.704126480817366,
-                    "heading": 0.013521718090975673,
-                    "angularVelocity": -0.000001406335509399091,
-                    "velocityX": 3.7819938594433236,
-                    "velocityY": -0.08461308666366031,
-                    "timestamp": 0.7374116224625212
-                },
-                {
-                    "x": 3.009972994287386,
-                    "y": 6.6971936915101855,
-                    "heading": 0.013521602863440677,
-                    "angularVelocity": -0.0000014063350554339165,
-                    "velocityX": 3.7819938463900247,
-                    "velocityY": -0.08461367011854067,
-                    "timestamp": 0.8193462471805791
-                },
-                {
-                    "x": 3.319849239613809,
-                    "y": 6.690260850195287,
-                    "heading": 0.013521487635941729,
-                    "angularVelocity": -0.0000014063346155021318,
-                    "velocityX": 3.781993832189087,
-                    "velocityY": -0.08461430486506759,
-                    "timestamp": 0.901280871898637
-                },
-                {
-                    "x": 3.629725483679546,
-                    "y": 6.683327952531275,
-                    "heading": 0.013521372408489362,
-                    "angularVelocity": -0.0000014063340469636637,
-                    "velocityX": 3.7819938168026104,
-                    "velocityY": -0.0846149925976696,
-                    "timestamp": 0.9832154966166949
-                },
-                {
-                    "x": 3.939601726375346,
-                    "y": 6.6763949936353555,
-                    "heading": 0.013521257181093756,
-                    "angularVelocity": -0.0000014063333542144944,
-                    "velocityX": 3.7819938000827285,
-                    "velocityY": -0.08461573992406812,
-                    "timestamp": 1.065150121334753
-                },
-                {
-                    "x": 4.249477967564444,
-                    "y": 6.66946196739526,
-                    "heading": 0.013521141953758897,
-                    "angularVelocity": -0.0000014063326128127892,
-                    "velocityX": 3.781993781693661,
-                    "velocityY": -0.0846165618498062,
-                    "timestamp": 1.1470847460528109
-                },
-                {
-                    "x": 4.559354207050928,
-                    "y": 6.662528865055332,
-                    "heading": 0.013521026726476763,
-                    "angularVelocity": -0.0000014063319692972842,
-                    "velocityX": 3.7819937609134957,
-                    "velocityY": -0.08461749063701171,
-                    "timestamp": 1.2290193707708688
-                },
-                {
-                    "x": 4.869230444482983,
-                    "y": 6.655595670892424,
-                    "heading": 0.01352091149922989,
-                    "angularVelocity": -0.0000014063315389464776,
-                    "velocityX": 3.7819937358394955,
-                    "velocityY": -0.08461861132293771,
-                    "timestamp": 1.3109539954889267
-                },
-                {
-                    "x": 5.1791066790320945,
-                    "y": 6.648662347878877,
-                    "heading": 0.013520796271957977,
-                    "angularVelocity": -0.0000014063318445457383,
-                    "velocityX": 3.7819937006535858,
-                    "velocityY": -0.08462018392597198,
-                    "timestamp": 1.3928886202069846
-                },
-                {
-                    "x": 5.488982909387702,
-                    "y": 6.641728837446599,
-                    "heading": 0.013520681044501265,
-                    "angularVelocity": -0.000001406334100009416,
-                    "velocityX": 3.781993649472511,
-                    "velocityY": -0.08462247134390694,
-                    "timestamp": 1.4748232449250425
-                },
-                {
-                    "x": 5.798859128679326,
-                    "y": 6.634794832555163,
-                    "heading": 0.013520565816988437,
-                    "angularVelocity": -0.0000014063347848865157,
-                    "velocityX": 3.781993514438203,
-                    "velocityY": -0.08462850614495548,
-                    "timestamp": 1.5567578696431004
-                },
-                {
-                    "x": 6.106285584248806,
-                    "y": 6.595293875173847,
-                    "heading": 0.013520451149743993,
-                    "angularVelocity": -0.000001399496791973984,
-                    "velocityX": 3.7520945098284613,
-                    "velocityY": -0.48210335395128207,
-                    "timestamp": 1.6386924943611583
-                },
-                {
-                    "x": 6.401594301701064,
-                    "y": 6.501144389493677,
-                    "heading": 0.013520442621996222,
-                    "angularVelocity": -1.0407990272609098e-7,
-                    "velocityX": 3.604199304853526,
-                    "velocityY": -1.1490805749601392,
-                    "timestamp": 1.7206271190792162
-                },
-                {
-                    "x": 6.675154779128766,
-                    "y": 6.355418517983256,
-                    "heading": 0.013520441934185553,
-                    "angularVelocity": -8.394627700401504e-9,
-                    "velocityX": 3.3387652456948387,
-                    "velocityY": -1.7785627506304358,
-                    "timestamp": 1.802561743797274
-                },
-                {
-                    "x": 6.946083635654423,
-                    "y": 6.204856506903887,
-                    "heading": 0.013520441248790115,
-                    "angularVelocity": -8.365150168101951e-9,
-                    "velocityX": 3.3066467010490315,
-                    "velocityY": -1.8375871201883405,
-                    "timestamp": 1.884496368515332
-                },
-                {
-                    "x": 7.190722413199009,
-                    "y": 6.068904365786755,
-                    "heading": 0.008013663922991063,
-                    "angularVelocity": -0.06720940438487652,
-                    "velocityX": 2.985780167864364,
-                    "velocityY": -1.6592758139181236,
-                    "timestamp": 1.9664309932333899
-                },
-                {
-                    "x": 7.38643344699072,
-                    "y": 5.96014263272892,
-                    "heading": 0.0036082164005303732,
-                    "angularVelocity": -0.053767836706643944,
-                    "velocityX": 2.3886242777722417,
-                    "velocityY": -1.3274208972347252,
-                    "timestamp": 2.048365617951448
-                },
-                {
-                    "x": 7.533216723981145,
-                    "y": 5.878571322292618,
-                    "heading": 0.0003041162966361105,
-                    "angularVelocity": -0.04032605403715309,
-                    "velocityX": 1.7914682284261954,
-                    "velocityY": -0.9955658028213874,
-                    "timestamp": 2.130300242669506
-                },
+                    "x": 0.744719766454751,
+                    "y": 6.720759968199705,
+                    "heading": 1.006760857121406,
+                    "angularVelocity": -0.5061419234074507,
+                    "velocityX": 0.5286878015432446,
+                    "velocityY": 0.011572252862695013,
+                    "timestamp": 0.06567158605400746
+                },
+                {
+                    "x": 0.8143576636925598,
+                    "y": 6.722372451876338,
+                    "heading": 0.9431499891720047,
+                    "angularVelocity": -0.9686208567749168,
+                    "velocityX": 1.0603961533765867,
+                    "velocityY": 0.024553749551892837,
+                    "timestamp": 0.1313431721080149
+                },
+                {
+                    "x": 0.9191612228651693,
+                    "y": 6.724923861576486,
+                    "heading": 0.8530685118915011,
+                    "angularVelocity": -1.3716963864162066,
+                    "velocityX": 1.5958737327650472,
+                    "velocityY": 0.03885104431723037,
+                    "timestamp": 0.19701475816202235
+                },
+                {
+                    "x": 1.059454644842809,
+                    "y": 6.7284663342548425,
+                    "heading": 0.7426650921374723,
+                    "angularVelocity": -1.6811444094442096,
+                    "velocityX": 2.1362880114128,
+                    "velocityY": 0.05394224338464699,
+                    "timestamp": 0.2626863442160298
+                },
+                {
+                    "x": 1.2356244745863587,
+                    "y": 6.733039509062333,
+                    "heading": 0.6243118436370656,
+                    "angularVelocity": -1.8021987226420155,
+                    "velocityX": 2.682588320596835,
+                    "velocityY": 0.06963703912619325,
+                    "timestamp": 0.3283579302700373
+                },
+                {
+                    "x": 1.4470569399809903,
+                    "y": 6.738777367450543,
+                    "heading": 0.5308220213971302,
+                    "angularVelocity": -1.4235962287107038,
+                    "velocityX": 3.219542546463745,
+                    "velocityY": 0.08737200870294055,
+                    "timestamp": 0.39402951632404476
+                },
+                {
+                    "x": 1.688226065456379,
+                    "y": 6.748371696739962,
+                    "heading": 0.5126606987274956,
+                    "angularVelocity": -0.2765476480908373,
+                    "velocityX": 3.6723511638207493,
+                    "velocityY": 0.14609559272058092,
+                    "timestamp": 0.45970110237805223
+                },
+                {
+                    "x": 1.9365780121804386,
+                    "y": 6.754665679712547,
+                    "heading": 0.5126606479364623,
+                    "angularVelocity": -7.734095747362075e-7,
+                    "velocityX": 3.7817260347544686,
+                    "velocityY": 0.09584027660688858,
+                    "timestamp": 0.5253726884320596
+                },
+                {
+                    "x": 2.1849299738494317,
+                    "y": 6.760959072962077,
+                    "heading": 0.5126605971462416,
+                    "angularVelocity": -7.733971982996713e-7,
+                    "velocityX": 3.7817262623252414,
+                    "velocityY": 0.09583129672545204,
+                    "timestamp": 0.5910442744860671
+                },
+                {
+                    "x": 2.433281935680659,
+                    "y": 6.767252459809602,
+                    "heading": 0.5126605463560266,
+                    "angularVelocity": -7.733971099299294e-7,
+                    "velocityX": 3.7817262647956325,
+                    "velocityY": 0.09583119924030911,
+                    "timestamp": 0.6567158605400746
+                },
+                {
+                    "x": 2.6816338977021794,
+                    "y": 6.773545839147862,
+                    "heading": 0.5126604955658177,
+                    "angularVelocity": -7.733970196193196e-7,
+                    "velocityX": 3.78172626769327,
+                    "velocityY": 0.09583108489457716,
+                    "timestamp": 0.722387446594082
+                },
+                {
+                    "x": 2.9299858599438156,
+                    "y": 6.779839209799928,
+                    "heading": 0.5126604447756148,
+                    "angularVelocity": -7.733969286509421e-7,
+                    "velocityX": 3.7817262710450463,
+                    "velocityY": 0.09583095262742326,
+                    "timestamp": 0.7880590326480895
+                },
+                {
+                    "x": 3.1783378224424883,
+                    "y": 6.786132570308856,
+                    "heading": 0.5126603939854177,
+                    "angularVelocity": -7.733968385113031e-7,
+                    "velocityX": 3.781726274959007,
+                    "velocityY": 0.09583079817497217,
+                    "timestamp": 0.853730618702097
+                },
+                {
+                    "x": 3.426689785244194,
+                    "y": 6.792425918859452,
+                    "heading": 0.5126603431952265,
+                    "angularVelocity": -7.733967515033378e-7,
+                    "velocityX": 3.7817262795733857,
+                    "velocityY": 0.09583061608201786,
+                    "timestamp": 0.9194022047561045
+                },
+                {
+                    "x": 3.6750417484067377,
+                    "y": 6.798719253170605,
+                    "heading": 0.5126602924050404,
+                    "angularVelocity": -7.733966712943809e-7,
+                    "velocityX": 3.781726285067957,
+                    "velocityY": 0.09583039925395942,
+                    "timestamp": 0.9850737908101119
+                },
+                {
+                    "x": 3.9233937120031053,
+                    "y": 6.805012570362013,
+                    "heading": 0.5126602416148589,
+                    "angularVelocity": -7.733966025007139e-7,
+                    "velocityX": 3.7817262916739214,
+                    "velocityY": 0.095830138566711,
+                    "timestamp": 1.0507453768641193
+                },
+                {
+                    "x": 4.171745676125189,
+                    "y": 6.811305866807266,
+                    "heading": 0.5126601908246804,
+                    "angularVelocity": -7.733965561950186e-7,
+                    "velocityX": 3.7817262996791623,
+                    "velocityY": 0.09582982265909266,
+                    "timestamp": 1.1164169629181266
+                },
+                {
+                    "x": 4.420097640886595,
+                    "y": 6.817599138023107,
+                    "heading": 0.5126601400345029,
+                    "angularVelocity": -7.733965406200526e-7,
+                    "velocityX": 3.781726309414286,
+                    "velocityY": 0.09582943848362173,
+                    "timestamp": 1.182088548972134
+                },
+                {
+                    "x": 4.6684496064150185,
+                    "y": 6.8238923789700765,
+                    "heading": 0.5126600892443262,
+                    "angularVelocity": -7.73396529455525e-7,
+                    "velocityX": 3.7817263210938923,
+                    "velocityY": 0.09582897757087808,
+                    "timestamp": 1.2477601350261414
+                },
+                {
+                    "x": 4.916801572841165,
+                    "y": 6.830185584489968,
+                    "heading": 0.5126600384541553,
+                    "angularVelocity": -7.733964406232834e-7,
+                    "velocityX": 3.7817263347637717,
+                    "velocityY": 0.09582843811261686,
+                    "timestamp": 1.3134317210801487
+                },
+                {
+                    "x": 5.165153539254683,
+                    "y": 6.8364787905062485,
+                    "heading": 0.5126599876639959,
+                    "angularVelocity": -7.733962639482034e-7,
+                    "velocityX": 3.781726334571479,
+                    "velocityY": 0.09582844567126618,
+                    "timestamp": 1.379103307134156
+                },
+                {
+                    "x": 5.413504500375102,
+                    "y": 6.842811545538402,
+                    "heading": 0.5126599368726894,
+                    "angularVelocity": -7.734137320837285e-7,
+                    "velocityX": 3.781711026686433,
+                    "velocityY": 0.09643066982787928,
+                    "timestamp": 1.4447748931881634
+                },
+                {
+                    "x": 5.661808131063952,
+                    "y": 6.834835924305975,
+                    "heading": 0.5126598860199024,
+                    "angularVelocity": -7.743499133059843e-7,
+                    "velocityX": 3.780990312684674,
+                    "velocityY": -0.12144706275568991,
+                    "timestamp": 1.5104464792421708
+                },
+                {
+                    "x": 5.9063591957092285,
+                    "y": 6.791097164154053,
+                    "heading": 0.5126598314406843,
+                    "angularVelocity": -8.310933457001734e-7,
+                    "velocityX": 3.723848917614642,
+                    "velocityY": -0.6660225948568639,
+                    "timestamp": 1.5761180652961782
+                },
+                {
+                    "x": 6.194622556481831,
+                    "y": 6.683762426219015,
+                    "heading": 0.5126597887806125,
+                    "angularVelocity": -5.246475178274693e-7,
+                    "velocityX": 3.5451571072929595,
+                    "velocityY": -1.3200377183874616,
+                    "timestamp": 1.65742993725234
+                },
+                {
+                    "x": 6.459547388261047,
+                    "y": 6.527457130068993,
+                    "heading": 0.5126597482300577,
+                    "angularVelocity": -4.987039864896594e-7,
+                    "velocityX": 3.2581322432449182,
+                    "velocityY": -1.9222936625379825,
+                    "timestamp": 1.7387418092085019
+                },
+                {
+                    "x": 6.723521508223802,
+                    "y": 6.3695515377198895,
+                    "heading": 0.5126597076877473,
+                    "angularVelocity": -4.98602596336278e-7,
+                    "velocityX": 3.2464400783305223,
+                    "velocityY": -1.9419746286809643,
+                    "timestamp": 1.8200536811646637
+                },
+                {
+                    "x": 6.987495332330779,
+                    "y": 6.211645450805702,
+                    "heading": 0.5126596671480953,
+                    "angularVelocity": -4.985699012832353e-7,
+                    "velocityX": 3.246436439802837,
+                    "velocityY": -1.9419807109984206,
+                    "timestamp": 1.9013655531208256
+                },
+                {
+                    "x": 7.218138260463332,
+                    "y": 6.072816355413413,
+                    "heading": 0.4174347514218684,
+                    "angularVelocity": -1.1711071635093024,
+                    "velocityX": 2.8365221779297074,
+                    "velocityY": -1.7073656288101424,
+                    "timestamp": 1.9826774250769874
+                },
+                {
+                    "x": 7.40359391202109,
+                    "y": 5.961763553999033,
+                    "heading": 0.26911254592387807,
+                    "angularVelocity": -1.8241150022712385,
+                    "velocityX": 2.2807942689821186,
+                    "velocityY": -1.365763679308134,
+                    "timestamp": 2.063989297033149
+                },
+                {
+                    "x": 7.542069024042313,
+                    "y": 5.879081457057784,
+                    "heading": 0.13934312775190755,
+                    "angularVelocity": -1.595946754760921,
+                    "velocityX": 1.7030122255201257,
+                    "velocityY": -1.0168514750927211,
+                    "timestamp": 2.145301168989311
+                },
                 {
-                    "x": 7.631072241649754,
-                    "y": 5.82419044260697,
-                    "heading": -0.0018986254325586427,
-                    "angularVelocity": -0.026884137649675257,
-                    "velocityX": 1.1943121483174686,
-                    "velocityY": -0.66371060919331,
-                    "timestamp": 2.212234867387564
+                    "x": 7.63409233729861,
+                    "y": 5.824280880315082,
+                    "heading": 0.04592275308495019,
+                    "angularVelocity": -1.1489143272623823,
+                    "velocityX": 1.1317328090287153,
+                    "velocityY": -0.6739554191077514,
+                    "timestamp": 2.2266130409454727
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
                     "heading": -0.003,
-                    "angularVelocity": -0.013442114017501823,
-                    "velocityX": 0.5971560682508651,
-                    "velocityY": -0.3318553383326643,
-                    "timestamp": 2.294169492105622
+                    "angularVelocity": -0.6016680210157614,
+                    "velocityX": 0.5645874531846384,
+                    "velocityY": -0.3355091902172136,
+                    "timestamp": 2.3079249129016346
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
                     "heading": -0.003,
-                    "angularVelocity": -2.3555736468302963e-30,
+                    "angularVelocity": 3.440382093642737e-30,
                     "velocityX": 0,
-                    "velocityY": 5.757868192156219e-33,
-                    "timestamp": 2.3761041168236803
+                    "velocityY": 2.1122019835026996e-31,
+                    "timestamp": 2.3892367848577964
                 }
             ],
             "constraints": [
@@ -1276,16 +1266,6 @@
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
             "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
                 {
                     "x": 5.61,
                     "y": 5.44,
@@ -2338,16 +2318,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 26
+                    "controlIntervalCount": 23
                 },
                 {
                     "x": 4.092995643615723,
                     "y": 5.023272514343262,
-                    "heading": 3.14,
+                    "heading": -0.5720542830492642,
                     "isInitialGuess": false,
                     "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 10
+                    "headingConstrained": true,
+                    "controlIntervalCount": 29
                 },
                 {
                     "x": 5.7832255363464355,
@@ -2383,657 +2363,801 @@
                     "y": 6.72,
                     "heading": 1.04,
                     "angularVelocity": 0,
-                    "velocityX": -2.70270630963226e-31,
-                    "velocityY": -3.181766396851851e-31,
+                    "velocityX": 0,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7256414720586181,
-                    "y": 6.712155337793633,
-                    "heading": 1.0237015987984905,
-                    "angularVelocity": -0.3502584296953155,
-                    "velocityX": 0.33614078912617174,
-                    "velocityY": -0.16858457660462928,
-                    "timestamp": 0.046532502346056014
-                },
-                {
-                    "x": 0.756979104040484,
-                    "y": 6.696435827024302,
-                    "heading": 0.9920873730999193,
-                    "angularVelocity": -0.6794009370802031,
-                    "velocityX": 0.6734568398839507,
-                    "velocityY": -0.33781786873231273,
-                    "timestamp": 0.09306500469211203
-                },
-                {
-                    "x": 0.8040789517219918,
-                    "y": 6.672807532790365,
-                    "heading": 0.9464055818688567,
-                    "angularVelocity": -0.9817179160357762,
-                    "velocityX": 1.0121924527341073,
-                    "velocityY": -0.5077804339471543,
-                    "timestamp": 0.13959750703816803
-                },
-                {
-                    "x": 0.8670206146686451,
-                    "y": 6.641234227942618,
-                    "heading": 0.8882606987188267,
-                    "angularVelocity": -1.2495541872573583,
-                    "velocityX": 1.352638688514273,
-                    "velocityY": -0.6785215334633973,
-                    "timestamp": 0.18613000938422405
-                },
-                {
-                    "x": 0.9458974559747272,
-                    "y": 6.601677384330224,
-                    "heading": 0.8197602775960398,
-                    "angularVelocity": -1.4720983757420398,
-                    "velocityX": 1.6950913303456163,
-                    "velocityY": -0.8500906166232347,
-                    "timestamp": 0.23266251173028007
-                },
-                {
-                    "x": 1.04081251945038,
-                    "y": 6.554094078416389,
-                    "heading": 0.743830819371293,
-                    "angularVelocity": -1.6317510212566968,
-                    "velocityX": 2.039758420249741,
-                    "velocityY": -1.0225821418316814,
-                    "timestamp": 0.27919501407633607
-                },
-                {
-                    "x": 1.15185810993152,
-                    "y": 6.498440545281296,
-                    "heading": 0.6650543509456113,
-                    "angularVelocity": -1.6929342815026889,
-                    "velocityX": 2.3864091738569346,
-                    "velocityY": -1.1960141907087913,
-                    "timestamp": 0.32572751642239206
-                },
-                {
-                    "x": 1.2789797685079123,
-                    "y": 6.43473905676892,
-                    "heading": 0.5921954143923897,
-                    "angularVelocity": -1.5657644201343583,
-                    "velocityX": 2.7318895861437724,
-                    "velocityY": -1.3689676097497174,
-                    "timestamp": 0.37226001876844805
-                },
-                {
-                    "x": 1.420223914518488,
-                    "y": 6.363052522149474,
-                    "heading": 0.5479689699773799,
-                    "angularVelocity": -0.9504419961367557,
-                    "velocityX": 3.035386856270174,
-                    "velocityY": -1.5405690862335149,
-                    "timestamp": 0.41879252111450405
-                },
-                {
-                    "x": 1.5740028534513755,
-                    "y": 6.283114192872816,
-                    "heading": 0.5411402298477218,
-                    "angularVelocity": -0.14675205040326236,
-                    "velocityX": 3.304764007515849,
-                    "velocityY": -1.717903083787275,
-                    "timestamp": 0.46532502346056004
-                },
-                {
-                    "x": 1.7314538549626566,
-                    "y": 6.20440189931456,
-                    "heading": 0.5411401828527235,
-                    "angularVelocity": -0.0000010099392058430412,
-                    "velocityX": 3.383677936345371,
-                    "velocityY": -1.6915551408103213,
-                    "timestamp": 0.511857525806616
-                },
-                {
-                    "x": 1.888910012901083,
-                    "y": 6.125699921176338,
-                    "heading": 0.5411401358667378,
-                    "angularVelocity": -0.0000010097455172737006,
-                    "velocityX": 3.383788749796141,
-                    "velocityY": -1.6913334587700644,
-                    "timestamp": 0.558390028152672
-                },
-                {
-                    "x": 2.0463658388325006,
-                    "y": 6.046997278808062,
-                    "heading": 0.5411400888807237,
-                    "angularVelocity": -0.0000010097461271838877,
-                    "velocityX": 3.3837816148475093,
-                    "velocityY": -1.691347733310028,
-                    "timestamp": 0.604922530498728
-                },
-                {
-                    "x": 2.2038216598445532,
-                    "y": 5.9682946265981345,
-                    "heading": 0.5411400418947097,
-                    "angularVelocity": -0.0000010097461301639238,
-                    "velocityX": 3.3837815091285663,
-                    "velocityY": -1.6913479448107236,
-                    "timestamp": 0.651455032844784
-                },
-                {
-                    "x": 2.3612775122085363,
-                    "y": 5.8895920371121315,
-                    "heading": 0.5411399949086985,
-                    "angularVelocity": -0.0000010097460671631127,
-                    "velocityX": 3.3837821828926944,
-                    "velocityY": -1.6913465968512624,
-                    "timestamp": 0.69798753519084
-                },
-                {
-                    "x": 2.5187333978232895,
-                    "y": 5.810889514148883,
-                    "heading": 0.5411399479226903,
-                    "angularVelocity": -0.0000010097460006294838,
-                    "velocityX": 3.383782897463604,
-                    "velocityY": -1.6913451672535464,
-                    "timestamp": 0.744520037536896
-                },
-                {
-                    "x": 2.676189337500118,
-                    "y": 5.732187099344657,
-                    "heading": 0.541139900936687,
-                    "angularVelocity": -0.0000010097458968738495,
-                    "velocityX": 3.3837840592768114,
-                    "velocityY": -1.6913428428777157,
-                    "timestamp": 0.791052539882952
-                },
-                {
-                    "x": 2.833645305906935,
-                    "y": 5.653484742019454,
-                    "heading": 0.5411398539506863,
-                    "angularVelocity": -0.0000010097458381135432,
-                    "velocityX": 3.3837846766944186,
-                    "velocityY": -1.6913416076332204,
-                    "timestamp": 0.837585042229008
-                },
-                {
-                    "x": 2.991101010217502,
-                    "y": 5.57478185633263,
-                    "heading": 0.5411398069646632,
-                    "angularVelocity": -0.0000010097463224464638,
-                    "velocityX": 3.383779001172013,
-                    "velocityY": -1.6913529623124643,
-                    "timestamp": 0.884117544575064
-                },
-                {
-                    "x": 3.14855589978306,
-                    "y": 5.496077340659936,
-                    "heading": 0.5411397599785701,
-                    "angularVelocity": -0.0000010097478275290572,
-                    "velocityX": 3.3837614920124643,
-                    "velocityY": -1.6913879912875969,
-                    "timestamp": 0.93065004692112
-                },
-                {
-                    "x": 3.306010013541662,
-                    "y": 5.417371272923184,
-                    "heading": 0.5411397129924104,
-                    "angularVelocity": -0.0000010097492581811278,
-                    "velocityX": 3.3837448196468105,
-                    "velocityY": -1.6914213456927205,
-                    "timestamp": 0.977182549267176
-                },
-                {
-                    "x": 3.4634680562159517,
-                    "y": 5.338673065406879,
-                    "heading": 0.5411396660065909,
-                    "angularVelocity": -0.0000010097419454671228,
-                    "velocityX": 3.383829253443873,
-                    "velocityY": -1.6912524267565827,
-                    "timestamp": 1.023715051613232
-                },
-                {
-                    "x": 3.6209395624979943,
-                    "y": 5.2600018013447745,
-                    "heading": 0.5411396190219294,
-                    "angularVelocity": -0.0000010097170609305348,
-                    "velocityX": 3.3841185911564384,
-                    "velocityY": -1.690673402356174,
-                    "timestamp": 1.0702475539592882
-                },
-                {
-                    "x": 3.778386684395729,
-                    "y": 5.181281747313384,
-                    "heading": 0.5411395720351826,
-                    "angularVelocity": -0.0000010097618775946008,
-                    "velocityX": 3.3835945620651766,
-                    "velocityY": -1.6917219161486032,
-                    "timestamp": 1.1167800563053443
-                },
-                {
-                    "x": 3.9357449151310644,
-                    "y": 5.10238415335025,
-                    "heading": 0.5411395250405994,
-                    "angularVelocity": -0.000001009930280464059,
-                    "velocityX": 3.3816842594175633,
-                    "velocityY": -1.6955373123309678,
-                    "timestamp": 1.1633125586514004
+                    "x": 0.7237404107060486,
+                    "y": 6.712071337633356,
+                    "heading": 0.9558489426883627,
+                    "angularVelocity": -1.537881157714612,
+                    "velocityX": 0.25110936688338653,
+                    "velocityY": -0.14489824428928566,
+                    "timestamp": 0.05471882979351295
+                },
+                {
+                    "x": 0.7514918072858734,
+                    "y": 6.694799584586779,
+                    "heading": 0.7935491608961526,
+                    "angularVelocity": -2.966068214628546,
+                    "velocityX": 0.5071635611460984,
+                    "velocityY": -0.31564551200661234,
+                    "timestamp": 0.1094376595870259
+                },
+                {
+                    "x": 0.7959632333479747,
+                    "y": 6.666655428800684,
+                    "heading": 0.574279005326375,
+                    "angularVelocity": -4.007215731718184,
+                    "velocityX": 0.8127261900504715,
+                    "velocityY": -0.514341331718894,
+                    "timestamp": 0.16415648938053887
+                },
+                {
+                    "x": 0.8617224500002636,
+                    "y": 6.63045184710829,
+                    "heading": 0.31905346805964874,
+                    "angularVelocity": -4.664309127769047,
+                    "velocityX": 1.2017657705846174,
+                    "velocityY": -0.6616293116832328,
+                    "timestamp": 0.2188753191740518
+                },
+                {
+                    "x": 0.9507709996198539,
+                    "y": 6.585947725507511,
+                    "heading": 0.05142997722620149,
+                    "angularVelocity": -4.890884762034389,
+                    "velocityX": 1.627384027685255,
+                    "velocityY": -0.8133237090178916,
+                    "timestamp": 0.2735941489675648
+                },
+                {
+                    "x": 1.0614190010918925,
+                    "y": 6.535018660742887,
+                    "heading": -0.17495854287380355,
+                    "angularVelocity": -4.137305584828904,
+                    "velocityX": 2.0221192940269503,
+                    "velocityY": -0.9307411170306262,
+                    "timestamp": 0.32831297876107773
+                },
+                {
+                    "x": 1.1921384640832198,
+                    "y": 6.474205616338843,
+                    "heading": -0.35862575739678126,
+                    "angularVelocity": -3.356563274764148,
+                    "velocityX": 2.388930163247485,
+                    "velocityY": -1.111373262796896,
+                    "timestamp": 0.3830318085545907
+                },
+                {
+                    "x": 1.3384031857193457,
+                    "y": 6.398894346098027,
+                    "heading": -0.49152044440666937,
+                    "angularVelocity": -2.4286829142980233,
+                    "velocityX": 2.6730235677201417,
+                    "velocityY": -1.376331886573771,
+                    "timestamp": 0.4377506383481036
+                },
+                {
+                    "x": 1.500957547301863,
+                    "y": 6.312259668262383,
+                    "heading": -0.5624501430724521,
+                    "angularVelocity": -1.2962575942036991,
+                    "velocityX": 2.9707207225727275,
+                    "velocityY": -1.5832699303433067,
+                    "timestamp": 0.4924694681416166
+                },
+                {
+                    "x": 1.6817069101290123,
+                    "y": 6.217739107445247,
+                    "heading": -0.5720464000476684,
+                    "angularVelocity": -0.17537394369443057,
+                    "velocityX": 3.3032388212471107,
+                    "velocityY": -1.7273863709040396,
+                    "timestamp": 0.5471882979351296
+                },
+                {
+                    "x": 1.8673643551319996,
+                    "y": 6.126199810501224,
+                    "heading": -0.5720469984173726,
+                    "angularVelocity": -0.000010935352720012522,
+                    "velocityX": 3.392935223643913,
+                    "velocityY": -1.6729030443351731,
+                    "timestamp": 0.6019071277286425
+                },
+                {
+                    "x": 2.0530162492759167,
+                    "y": 6.034649256644459,
+                    "heading": -0.5720475972653951,
+                    "angularVelocity": -0.000010944094102204816,
+                    "velocityX": 3.3928337803365705,
+                    "velocityY": -1.6731087671691436,
+                    "timestamp": 0.6566259575221555
+                },
+                {
+                    "x": 2.23866813267496,
+                    "y": 5.943098680998449,
+                    "heading": -0.5720481961130315,
+                    "angularVelocity": -0.000010944087046546937,
+                    "velocityX": 3.3928335839713664,
+                    "velocityY": -1.673109165372953,
+                    "timestamp": 0.7113447873156684
+                },
+                {
+                    "x": 2.4243200161477754,
+                    "y": 5.851548105501653,
+                    "heading": -0.5720487949598734,
+                    "angularVelocity": -0.000010944072528266654,
+                    "velocityX": 3.392833585319575,
+                    "velocityY": -1.673109162646019,
+                    "timestamp": 0.7660636171091814
+                },
+                {
+                    "x": 2.6099718996232633,
+                    "y": 5.759997530009892,
+                    "heading": -0.5720493938059237,
+                    "angularVelocity": -0.000010944058060277418,
+                    "velocityX": 3.3928335853684146,
+                    "velocityY": -1.673109162553994,
+                    "timestamp": 0.8207824469026943
+                },
+                {
+                    "x": 2.7956237830994004,
+                    "y": 5.668446954519065,
+                    "heading": -0.5720499926511823,
+                    "angularVelocity": -0.000010944043595406558,
+                    "velocityX": 3.3928335853802802,
+                    "velocityY": -1.6731091625369476,
+                    "timestamp": 0.8755012766962073
+                },
+                {
+                    "x": 2.9812756665756717,
+                    "y": 5.576896379028125,
+                    "heading": -0.5720505914956495,
+                    "angularVelocity": -0.000010944029129446314,
+                    "velocityX": 3.3928335853827263,
+                    "velocityY": -1.6731091625390042,
+                    "timestamp": 0.9302201064897202
+                },
+                {
+                    "x": 3.166927550036217,
+                    "y": 5.485345803504912,
+                    "heading": -0.5720511903393257,
+                    "angularVelocity": -0.000010944014673786385,
+                    "velocityX": 3.392833585095338,
+                    "velocityY": -1.673109163128796,
+                    "timestamp": 0.9849389362832331
+                },
+                {
+                    "x": 3.3525794334560626,
+                    "y": 5.393795227898782,
+                    "heading": -0.5720517891822119,
+                    "angularVelocity": -0.000010944000236842356,
+                    "velocityX": 3.392833584351539,
+                    "velocityY": -1.6731091646441256,
+                    "timestamp": 1.039657766076746
+                },
+                {
+                    "x": 3.538231313226632,
+                    "y": 5.302244644892125,
+                    "heading": -0.5720523880244444,
+                    "angularVelocity": -0.000010943988288632917,
+                    "velocityX": 3.392833517660122,
+                    "velocityY": -1.673109299890567,
+                    "timestamp": 1.0943765958702592
+                },
+                {
+                    "x": 3.723883292814953,
+                    "y": 5.21069426429933,
+                    "heading": -0.5720529868621156,
+                    "angularVelocity": -0.000010943904932797782,
+                    "velocityX": 3.3928353418539645,
+                    "velocityY": -1.673105600727685,
+                    "timestamp": 1.1490954256637722
+                },
+                {
+                    "x": 3.909492929164672,
+                    "y": 5.1190580682844296,
+                    "heading": -0.5720535873273735,
+                    "angularVelocity": -0.000010973649477031932,
+                    "velocityX": 3.3920615088102064,
+                    "velocityY": -1.6746738985594591,
+                    "timestamp": 1.2038142554572853
                 },
                 {
                     "x": 4.092995643615723,
                     "y": 5.023272514343262,
-                    "heading": 0.5411394780180799,
-                    "angularVelocity": -0.0000010105306414990813,
-                    "velocityX": 3.379373997880459,
-                    "velocityY": -1.7001372163227824,
-                    "timestamp": 1.2098450609974565
-                },
-                {
-                    "x": 4.2612300154414715,
-                    "y": 4.916069066513277,
-                    "heading": 0.541139432730096,
-                    "angularVelocity": -8.588080638060829e-7,
-                    "velocityX": 3.1902730598822937,
-                    "velocityY": -2.0329274441782497,
-                    "timestamp": 1.2625785946693153
-                },
-                {
-                    "x": 4.429297905294805,
-                    "y": 4.808604804974877,
-                    "heading": 0.5411393874448718,
-                    "angularVelocity": -8.58755730533247e-7,
-                    "velocityX": 3.1871160180389406,
-                    "velocityY": -2.0378733237761697,
-                    "timestamp": 1.3153121283411742
-                },
-                {
-                    "x": 4.5972191269209945,
-                    "y": 4.700911507061467,
-                    "heading": 0.5411393421481548,
-                    "angularVelocity": -8.589736709635531e-7,
-                    "velocityX": 3.1843347095055985,
-                    "velocityY": -2.042216601367096,
-                    "timestamp": 1.368045662013033
-                },
-                {
-                    "x": 4.765178956046694,
-                    "y": 4.593278431809082,
-                    "heading": 0.5411392968544698,
-                    "angularVelocity": -8.589161734970966e-7,
-                    "velocityX": 3.185066833769243,
-                    "velocityY": -2.0410745830567376,
-                    "timestamp": 1.4207791956848919
-                },
-                {
-                    "x": 4.9331339491528,
-                    "y": 4.485637810244685,
-                    "heading": 0.5411392515604048,
-                    "angularVelocity": -8.58923380695705e-7,
-                    "velocityX": 3.1849751270448214,
-                    "velocityY": -2.041217685776372,
-                    "timestamp": 1.4735127293567507
-                },
-                {
-                    "x": 5.1010877481253685,
-                    "y": 4.377995325600118,
-                    "heading": 0.5411392062662455,
-                    "angularVelocity": -8.589251678490757e-7,
-                    "velocityX": 3.1849524823742743,
-                    "velocityY": -2.041253015859115,
-                    "timestamp": 1.5262462630286096
-                },
-                {
-                    "x": 5.269016703536996,
-                    "y": 4.270314087571009,
-                    "heading": 0.5411391609701367,
-                    "angularVelocity": -8.589621362197366e-7,
-                    "velocityX": 3.184481367332135,
-                    "velocityY": -2.0419879065793034,
-                    "timestamp": 1.5789797967004684
-                },
-                {
-                    "x": 5.437056059940434,
-                    "y": 4.162805214274757,
-                    "heading": 0.5411391156827058,
-                    "angularVelocity": -8.587975760545075e-7,
-                    "velocityX": 3.18657493065327,
-                    "velocityY": -2.038719308384206,
-                    "timestamp": 1.6317133303723272
-                },
-                {
-                    "x": 5.605213513282879,
-                    "y": 4.0554811537322735,
-                    "heading": 0.5411390703937885,
-                    "angularVelocity": -8.588257618591655e-7,
-                    "velocityX": 3.188814434261697,
-                    "velocityY": -2.03521465506778,
-                    "timestamp": 1.684446864044186
+                    "heading": -0.5720542830492642,
+                    "angularVelocity": -0.000012714487741704976,
+                    "velocityX": 3.353556995709023,
+                    "velocityY": -1.7505044296932655,
+                    "timestamp": 1.2585330852507983
+                },
+                {
+                    "x": 4.152699494738863,
+                    "y": 4.989124335806784,
+                    "heading": -0.5720542274621303,
+                    "angularVelocity": 0.0000030573365885939726,
+                    "velocityX": 3.283759307267904,
+                    "velocityY": -1.8781769850015708,
+                    "timestamp": 1.2767146404552234
+                },
+                {
+                    "x": 4.211236123155136,
+                    "y": 4.953011857528808,
+                    "heading": -0.5720541720789936,
+                    "angularVelocity": 0.000003046116576982166,
+                    "velocityX": 3.2195611298433677,
+                    "velocityY": -1.9862150334199096,
+                    "timestamp": 1.2948961956596485
+                },
+                {
+                    "x": 4.269217408443118,
+                    "y": 4.916014310130808,
+                    "heading": -0.5720541167402081,
+                    "angularVelocity": 0.000003043677217870583,
+                    "velocityX": 3.1890168160017764,
+                    "velocityY": -2.034894539109796,
+                    "timestamp": 1.3130777508640736
+                },
+                {
+                    "x": 4.327172181408397,
+                    "y": 4.8789752463128,
+                    "heading": -0.5720540614037476,
+                    "angularVelocity": 0.000003043549349895456,
+                    "velocityX": 3.187558617163689,
+                    "velocityY": -2.0371779752347874,
+                    "timestamp": 1.3312593060684987
+                },
+                {
+                    "x": 4.385108415981365,
+                    "y": 4.841907191568928,
+                    "heading": -0.5720540060672449,
+                    "angularVelocity": 0.0000030435516711711228,
+                    "velocityX": 3.186538990839725,
+                    "velocityY": -2.0387724992190503,
+                    "timestamp": 1.3494408612729238
+                },
+                {
+                    "x": 4.443034277698237,
+                    "y": 4.804822929374826,
+                    "heading": -0.5720539507307192,
+                    "angularVelocity": 0.000003043552934036031,
+                    "velocityX": 3.1859684755020403,
+                    "velocityY": -2.0396639218758956,
+                    "timestamp": 1.367622416477349
+                },
+                {
+                    "x": 4.500957639641057,
+                    "y": 4.767734762799275,
+                    "heading": -0.5720538953941279,
+                    "angularVelocity": 0.0000030435565398491907,
+                    "velocityX": 3.1858309859372986,
+                    "velocityY": -2.039878665966735,
+                    "timestamp": 1.385803971681774
+                },
+                {
+                    "x": 4.558883202891902,
+                    "y": 4.7306500344023386,
+                    "heading": -0.5720538400575734,
+                    "angularVelocity": 0.000003043554516316428,
+                    "velocityX": 3.1859520596302042,
+                    "velocityY": -2.03968956340728,
+                    "timestamp": 1.4039855268861992
+                },
+                {
+                    "x": 4.616809360440233,
+                    "y": 4.693566234312182,
+                    "heading": -0.572053784721022,
+                    "angularVelocity": 0.000003043554346667464,
+                    "velocityX": 3.1859847464654854,
+                    "velocityY": -2.039638505794633,
+                    "timestamp": 1.4221670820906243
+                },
+                {
+                    "x": 4.674735447300298,
+                    "y": 4.656482323806888,
+                    "heading": -0.5720537293844703,
+                    "angularVelocity": 0.00000304355436453364,
+                    "velocityX": 3.185980858554995,
+                    "velocityY": -2.039644578714451,
+                    "timestamp": 1.4403486372950494
+                },
+                {
+                    "x": 4.732661331177625,
+                    "y": 4.619398096238942,
+                    "heading": -0.5720536740479176,
+                    "angularVelocity": 0.0000030435544255368,
+                    "velocityX": 3.1859696943429383,
+                    "velocityY": -2.0396620174125277,
+                    "timestamp": 1.4585301924994745
+                },
+                {
+                    "x": 4.790587117727679,
+                    "y": 4.582313716645222,
+                    "heading": -0.5720536187113643,
+                    "angularVelocity": 0.0000030435544545803464,
+                    "velocityX": 3.1859643412654055,
+                    "velocityY": -2.039670378951063,
+                    "timestamp": 1.4767117477038996
+                },
+                {
+                    "x": 4.848512993160408,
+                    "y": 4.5452294758867104,
+                    "heading": -0.5720535633748114,
+                    "angularVelocity": 0.0000030435544295946876,
+                    "velocityX": 3.1859692298833355,
+                    "velocityY": -2.0396627429041696,
+                    "timestamp": 1.4948933029083247
+                },
+                {
+                    "x": 4.906438819485911,
+                    "y": 4.508145158422396,
+                    "heading": -0.5720535080382583,
+                    "angularVelocity": 0.0000030435544442083865,
+                    "velocityX": 3.1859665289468815,
+                    "velocityY": -2.039666961784329,
+                    "timestamp": 1.5130748581127498
+                },
+                {
+                    "x": 4.9643646433421065,
+                    "y": 4.471060837101057,
+                    "heading": -0.5720534527017052,
+                    "angularVelocity": 0.0000030435544458805193,
+                    "velocityX": 3.185966393132936,
+                    "velocityY": -2.039667173923745,
+                    "timestamp": 1.5312564133171749
+                },
+                {
+                    "x": 5.022290480818567,
+                    "y": 4.433976537054754,
+                    "heading": -0.572053397365152,
+                    "angularVelocity": 0.0000030435544415454932,
+                    "velocityX": 3.185967142258403,
+                    "velocityY": -2.039666003779759,
+                    "timestamp": 1.5494379685216
+                },
+                {
+                    "x": 5.080216347467466,
+                    "y": 4.396892282576102,
+                    "heading": -0.572053342028599,
+                    "angularVelocity": 0.000003043554433808661,
+                    "velocityX": 3.1859687467658464,
+                    "velocityY": -2.039663497522643,
+                    "timestamp": 1.567619523726025
+                },
+                {
+                    "x": 5.138142382220086,
+                    "y": 4.359808290677983,
+                    "heading": -0.5720532866920471,
+                    "angularVelocity": 0.000003043554377469469,
+                    "velocityX": 3.185977992604117,
+                    "velocityY": -2.039649055384386,
+                    "timestamp": 1.5858010789304502
+                },
+                {
+                    "x": 5.196068138494407,
+                    "y": 4.322723863792993,
+                    "heading": -0.5720532313554916,
+                    "angularVelocity": 0.000003043554576021048,
+                    "velocityX": 3.1859626760759787,
+                    "velocityY": -2.0396729800088242,
+                    "timestamp": 1.6039826341348753
+                },
+                {
+                    "x": 5.253993690845546,
+                    "y": 4.285639118381674,
+                    "heading": -0.5720531760189423,
+                    "angularVelocity": 0.0000030435542278177066,
+                    "velocityX": 3.1859514601384813,
+                    "velocityY": -2.0396904992104243,
+                    "timestamp": 1.6221641893393004
+                },
+                {
+                    "x": 5.3119198680466875,
+                    "y": 4.248555348991913,
+                    "heading": -0.5720531206823879,
+                    "angularVelocity": 0.000003043554518420827,
+                    "velocityX": 3.1859858273860655,
+                    "velocityY": -2.0396368172479113,
+                    "timestamp": 1.6403457445437255
+                },
+                {
+                    "x": 5.369845451022558,
+                    "y": 4.211470651416335,
+                    "heading": -0.5720530653458327,
+                    "angularVelocity": 0.0000030435545594837867,
+                    "velocityX": 3.1859531445232334,
+                    "velocityY": -2.039687868206526,
+                    "timestamp": 1.6585272997481506
+                },
+                {
+                    "x": 5.427769955136907,
+                    "y": 4.17438426873464,
+                    "heading": -0.5720530100092767,
+                    "angularVelocity": 0.0000030435545998300077,
+                    "velocityX": 3.185893806281465,
+                    "velocityY": -2.039780550383344,
+                    "timestamp": 1.6767088549525757
+                },
+                {
+                    "x": 5.485698211873678,
+                    "y": 4.137303747848898,
+                    "heading": -0.5720529546727399,
+                    "angularVelocity": 0.000003043553546723704,
+                    "velocityX": 3.1861002035013715,
+                    "velocityY": -2.0394581469404574,
+                    "timestamp": 1.6948904101570008
+                },
+                {
+                    "x": 5.5436372749681775,
+                    "y": 4.100240114388813,
+                    "heading": -0.5720528993360909,
+                    "angularVelocity": 0.0000030435597092212927,
+                    "velocityX": 3.186694561772871,
+                    "velocityY": -2.0385293250953618,
+                    "timestamp": 1.713071965361426
+                },
+                {
+                    "x": 5.601833960036281,
+                    "y": 4.063582321171135,
+                    "heading": -0.5720528439855975,
+                    "angularVelocity": 0.0000030443211669066955,
+                    "velocityX": 3.2008639752640304,
+                    "velocityY": -2.0162077889097265,
+                    "timestamp": 1.731253520565851
+                },
+                {
+                    "x": 5.661045954546234,
+                    "y": 4.0285882123488195,
+                    "heading": -0.5720527885077281,
+                    "angularVelocity": 0.000003051326955110969,
+                    "velocityX": 3.2567068022627845,
+                    "velocityY": -1.9247038236737246,
+                    "timestamp": 1.7494350757702761
+                },
+                {
+                    "x": 5.721517200862813,
+                    "y": 3.9958181607132874,
+                    "heading": -0.5720527326566293,
+                    "angularVelocity": 0.0000030718548612359243,
+                    "velocityX": 3.3259666533847816,
+                    "velocityY": -1.8023789091228484,
+                    "timestamp": 1.7676166309747012
                 },
                 {
                     "x": 5.7832255363464355,
                     "y": 3.9654417037963863,
-                    "heading": 0.5411390248741499,
-                    "angularVelocity": -8.632009947472993e-7,
-                    "velocityX": 3.3756892563138687,
-                    "velocityY": -1.7074419949962474,
-                    "timestamp": 1.737180397716045
-                },
-                {
-                    "x": 5.9452002659111205,
-                    "y": 3.9031260890359523,
-                    "heading": 0.54113898049095,
-                    "angularVelocity": -9.674475759968245e-7,
-                    "velocityX": 3.530661595583403,
-                    "velocityY": -1.358331317676469,
-                    "timestamp": 1.7830569919972126
-                },
-                {
-                    "x": 6.112434032055133,
-                    "y": 3.8567373346649623,
-                    "heading": 0.5411389373525121,
-                    "angularVelocity": -9.403147416321739e-7,
-                    "velocityX": 3.6452960112747332,
-                    "velocityY": -1.0111638646647567,
-                    "timestamp": 1.8289335862783802
-                },
-                {
-                    "x": 6.280097272848707,
-                    "y": 3.8119256534794217,
-                    "heading": 0.5411388942859083,
-                    "angularVelocity": -9.387489265836176e-7,
-                    "velocityX": 3.65465753115864,
-                    "velocityY": -0.9767874422169035,
-                    "timestamp": 1.8748101805595478
-                },
-                {
-                    "x": 6.4446058180798245,
-                    "y": 3.756644666326502,
-                    "heading": 0.5411388506931323,
-                    "angularVelocity": -9.502182266145116e-7,
-                    "velocityX": 3.5858927152021427,
-                    "velocityY": -1.204993265500441,
-                    "timestamp": 1.9206867748407155
-                },
-                {
-                    "x": 6.602719007178768,
-                    "y": 3.68509560621824,
-                    "heading": 0.5411388045872843,
-                    "angularVelocity": -0.0000010049971826881646,
-                    "velocityX": 3.4464892517938135,
-                    "velocityY": -1.5595983361306547,
-                    "timestamp": 1.9665633691218831
-                },
-                {
-                    "x": 6.75280187814694,
-                    "y": 3.5979510164257396,
-                    "heading": 0.5411387531507533,
-                    "angularVelocity": -0.000001121193317368561,
-                    "velocityX": 3.271447528304838,
-                    "velocityY": -1.899543572446005,
-                    "timestamp": 2.0124399634030508
-                },
-                {
-                    "x": 6.893316834439751,
-                    "y": 3.4960952692408354,
-                    "heading": 0.5411386921189019,
-                    "angularVelocity": -0.0000013303483490867941,
-                    "velocityX": 3.0628898786954792,
-                    "velocityY": -2.2202116085744548,
-                    "timestamp": 2.0583165576842184
-                },
-                {
-                    "x": 7.022827105769335,
-                    "y": 3.380569803374169,
-                    "heading": 0.5411386138405815,
-                    "angularVelocity": -0.0000017062801116380342,
-                    "velocityX": 2.8230140741451093,
-                    "velocityY": -2.5181787723526305,
-                    "timestamp": 2.104193151965386
+                    "heading": -0.572052675208486,
+                    "angularVelocity": 0.000003159693577578397,
+                    "velocityX": 3.3940075416988695,
+                    "velocityY": -1.670729295450284,
+                    "timestamp": 1.7857981861791263
+                },
+                {
+                    "x": 5.945861586676733,
+                    "y": 3.9048707731215235,
+                    "heading": -0.5720526214657033,
+                    "angularVelocity": 0.0000011714587578241761,
+                    "velocityX": 3.5450606755342,
+                    "velocityY": -1.3202953710429959,
+                    "timestamp": 1.831674989206145
+                },
+                {
+                    "x": 6.113092819895856,
+                    "y": 3.858469939627616,
+                    "heading": -0.5720525698257588,
+                    "angularVelocity": 0.0000011256221281562495,
+                    "velocityX": 3.6452242132183756,
+                    "velocityY": -1.0114225585101817,
+                    "timestamp": 1.8775517922331635
+                },
+                {
+                    "x": 6.280428909416468,
+                    "y": 3.8124486851902244,
+                    "heading": -0.5720525182264563,
+                    "angularVelocity": 0.0000011247362300949913,
+                    "velocityX": 3.6475098193329205,
+                    "velocityY": -1.0031486808299002,
+                    "timestamp": 1.923428595260182
+                },
+                {
+                    "x": 6.444900242470224,
+                    "y": 3.7570546132296903,
+                    "heading": -0.5720524653781475,
+                    "angularVelocity": 0.000001151961455361842,
+                    "velocityX": 3.585065266142057,
+                    "velocityY": -1.2074527496557126,
+                    "timestamp": 1.9693053982872004
+                },
+                {
+                    "x": 6.602974533610246,
+                    "y": 3.685417746961916,
+                    "heading": -0.5720524086130027,
+                    "angularVelocity": 0.0000012373387213911858,
+                    "velocityX": 3.445625691199315,
+                    "velocityY": -1.561505195243581,
+                    "timestamp": 2.015182201314219
+                },
+                {
+                    "x": 6.753006171084989,
+                    "y": 3.5981834135738024,
+                    "heading": -0.5720523449049553,
+                    "angularVelocity": 0.0000013886766999770178,
+                    "velocityX": 3.27031588025751,
+                    "velocityY": -1.9014911160397099,
+                    "timestamp": 2.0610590043412373
+                },
+                {
+                    "x": 6.893461047058093,
+                    "y": 3.4962434932531963,
+                    "heading": -0.5720522700606003,
+                    "angularVelocity": 0.0000016314204553722347,
+                    "velocityX": 3.0615663408441725,
+                    "velocityY": -2.2220362709370285,
+                    "timestamp": 2.106935807368256
+                },
+                {
+                    "x": 7.0229029918463715,
+                    "y": 3.380640293435665,
+                    "heading": -0.5720521774325067,
+                    "angularVelocity": 0.000002019061650871652,
+                    "velocityX": 2.8215118806773165,
+                    "velocityY": -2.519861720737319,
+                    "timestamp": 2.1528126103952743
                 },
                 {
                     "x": 7.140008449554443,
                     "y": 3.2525558471679688,
-                    "heading": 0.541138510449912,
-                    "angularVelocity": -0.0000022536692406151377,
-                    "velocityX": 2.5542729494461978,
-                    "velocityY": -2.790397984245173,
-                    "timestamp": 2.1500697462465537
-                },
-                {
-                    "x": 7.220411568965462,
-                    "y": 3.1500924469852043,
-                    "heading": 0.5411383954402629,
-                    "angularVelocity": -0.0000033404657713126234,
-                    "velocityX": 2.3353159519018676,
-                    "velocityY": -2.97605882316215,
-                    "timestamp": 2.1844989716991257
-                },
-                {
-                    "x": 7.292813165778826,
-                    "y": 3.0418269330928753,
-                    "heading": 0.5411383062644725,
-                    "angularVelocity": -0.000002590118983726798,
-                    "velocityX": 2.102910996737314,
-                    "velocityY": -3.1445817461525896,
-                    "timestamp": 2.2189281971516976
-                },
-                {
-                    "x": 7.3567962971772936,
-                    "y": 2.928382790909777,
-                    "heading": 0.5411382336043034,
-                    "angularVelocity": -0.000002110421251450144,
-                    "velocityX": 1.8583958993386591,
-                    "velocityY": -3.2949954781694024,
-                    "timestamp": 2.2533574226042696
-                },
-                {
-                    "x": 7.411992506551136,
-                    "y": 2.810413328004449,
-                    "heading": 0.5411381719390687,
-                    "angularVelocity": -0.00000179107237982498,
-                    "velocityX": 1.603178946034542,
-                    "velocityY": -3.426433832147572,
-                    "timestamp": 2.2877866480568416
-                },
-                {
-                    "x": 7.458083940034777,
-                    "y": 2.688597909034967,
-                    "heading": 0.5411381177395469,
-                    "angularVelocity": -0.0000015742300651441758,
-                    "velocityX": 1.3387298981538813,
-                    "velocityY": -3.5381399775400135,
-                    "timestamp": 2.3222158735094136
-                },
-                {
-                    "x": 7.4948097486817264,
-                    "y": 2.563639387590104,
-                    "heading": 0.5411380685927316,
-                    "angularVelocity": -0.0000014274737405214791,
-                    "velocityX": 1.0667044687814087,
-                    "velocityY": -3.6294316762078314,
-                    "timestamp": 2.3566450989619856
-                },
-                {
-                    "x": 7.521973664613143,
-                    "y": 2.436259879309271,
-                    "heading": 0.5411380227355477,
-                    "angularVelocity": -0.0000013319260973088958,
-                    "velocityX": 0.7889784209297904,
-                    "velocityY": -3.6997494601296452,
-                    "timestamp": 2.3910743244145576
-                },
-                {
-                    "x": 7.539529818670366,
-                    "y": 2.3072048557852676,
-                    "heading": 0.5411379787715688,
-                    "angularVelocity": -0.0000012769377868531526,
-                    "velocityX": 0.5099200991725963,
-                    "velocityY": -3.748414953504679,
-                    "timestamp": 2.4255035498671296
-                },
-                {
-                    "x": 7.551115587366525,
-                    "y": 2.177477496871536,
-                    "heading": 0.5411379354425048,
-                    "angularVelocity": -0.0000012584966258013745,
-                    "velocityX": 0.336509710684848,
-                    "velocityY": -3.7679429963488027,
-                    "timestamp": 2.4599327753197016
-                },
-                {
-                    "x": 7.56271584344406,
-                    "y": 2.0477514326184285,
-                    "heading": 0.5411378921115911,
-                    "angularVelocity": -0.0000012585503448264627,
-                    "velocityX": 0.3369304980023622,
-                    "velocityY": -3.76790539281253,
-                    "timestamp": 2.4943620007722735
-                },
-                {
-                    "x": 7.574376716473137,
-                    "y": 1.9180308030778739,
-                    "heading": 0.5411378487769694,
-                    "angularVelocity": -0.0000012586580504181092,
-                    "velocityX": 0.3386911228976962,
-                    "velocityY": -3.767747541089928,
-                    "timestamp": 2.5287912262248455
-                },
-                {
-                    "x": 7.58611108718596,
-                    "y": 1.788316801391074,
-                    "heading": 0.541137805437783,
-                    "angularVelocity": -0.0000012587906282413006,
-                    "velocityX": 0.34082586983899804,
-                    "velocityY": -3.7675550344719086,
-                    "timestamp": 2.5632204516774175
-                },
-                {
-                    "x": 7.597929211011114,
-                    "y": 1.6586104040760536,
-                    "heading": 0.541137760738121,
-                    "angularVelocity": -0.0000012983057658513822,
-                    "velocityX": 0.3432584866444477,
-                    "velocityY": -3.7673341647982834,
-                    "timestamp": 2.5976496771299895
-                },
-                {
-                    "x": 7.610687440254598,
-                    "y": 1.5360368797463901,
-                    "heading": 0.5228402741207003,
-                    "angularVelocity": -0.5314521711397164,
-                    "velocityX": 0.37056393444221536,
-                    "velocityY": -3.560159217014913,
-                    "timestamp": 2.6320789025825615
-                },
-                {
-                    "x": 7.6219652115761445,
-                    "y": 1.4215043504075173,
-                    "heading": 0.4829676933785713,
-                    "angularVelocity": -1.1581027518921772,
-                    "velocityX": 0.3275638987899306,
-                    "velocityY": -3.3266077825840545,
-                    "timestamp": 2.6665081280351335
-                },
-                {
-                    "x": 7.631828440236478,
-                    "y": 1.3151733520485158,
-                    "heading": 0.42230189460591644,
-                    "angularVelocity": -1.7620436700286786,
-                    "velocityX": 0.2864783779098223,
-                    "velocityY": -3.0883935656779764,
-                    "timestamp": 2.7009373534877055
-                },
-                {
-                    "x": 7.640727843462309,
-                    "y": 1.2186121100176637,
-                    "heading": 0.3575309788065375,
-                    "angularVelocity": -1.8812771692652814,
-                    "velocityX": 0.2584839800738238,
-                    "velocityY": -2.804630100199854,
-                    "timestamp": 2.7353665789402775
-                },
-                {
-                    "x": 7.648686345492233,
-                    "y": 1.1318869979940107,
-                    "heading": 0.2935915741251458,
-                    "angularVelocity": -1.8571258528454382,
-                    "velocityX": 0.23115541884308086,
-                    "velocityY": -2.518938805147378,
-                    "timestamp": 2.7697958043928494
-                },
-                {
-                    "x": 7.655721476158885,
-                    "y": 1.054956995812666,
-                    "heading": 0.2330360616068616,
-                    "angularVelocity": -1.7588403956899903,
-                    "velocityX": 0.20433601320340974,
-                    "velocityY": -2.234438944533275,
-                    "timestamp": 2.8042250298454214
-                },
-                {
-                    "x": 7.661845751852373,
-                    "y": 0.9877699914439897,
-                    "heading": 0.17748768247912494,
-                    "angularVelocity": -1.6134077487241032,
-                    "velocityX": 0.17788014725831017,
-                    "velocityY": -1.9514526825829648,
-                    "timestamp": 2.8386542552979934
-                },
-                {
-                    "x": 7.667069516180101,
-                    "y": 0.9302779311076093,
-                    "heading": 0.1280864585154153,
-                    "angularVelocity": -1.4348630651528973,
-                    "velocityX": 0.15172471233573487,
-                    "velocityY": -1.6698621470755346,
-                    "timestamp": 2.8730834807505654
-                },
-                {
-                    "x": 7.6714024480246685,
-                    "y": 0.88243945985842,
-                    "heading": 0.08567598072175592,
-                    "angularVelocity": -1.2318162037100373,
-                    "velocityX": 0.1258504014428196,
-                    "velocityY": -1.3894727697283988,
-                    "timestamp": 2.9075127062031374
-                },
-                {
-                    "x": 7.674853841393155,
-                    "y": 0.8442197139951643,
-                    "heading": 0.050901182135293016,
-                    "angularVelocity": -1.0100372032583045,
-                    "velocityX": 0.10024603583499626,
-                    "velocityY": -1.110096011770723,
-                    "timestamp": 2.9419419316557094
-                },
-                {
-                    "x": 7.6774323573426795,
-                    "y": 0.8155893376478138,
-                    "heading": 0.02426863930506587,
-                    "angularVelocity": -0.7735446406401408,
-                    "velocityX": 0.07489323142279661,
-                    "velocityY": -0.8315718977410657,
-                    "timestamp": 2.9763711571082814
-                },
+                    "heading": -0.5720520639012779,
+                    "angularVelocity": 0.0000024746979161572956,
+                    "velocityX": 2.5526071997441684,
+                    "velocityY": -2.7919217952549285,
+                    "timestamp": 2.1986894134222927
+                },
+                {
+                    "x": 7.220415929770595,
+                    "y": 3.149946383889573,
+                    "heading": -0.5720519473102574,
+                    "angularVelocity": 0.000003383341158681086,
+                    "velocityX": 2.333335244140948,
+                    "velocityY": -2.977611988413324,
+                    "timestamp": 2.2331497338662096
+                },
+                {
+                    "x": 7.2928033634314815,
+                    "y": 3.0415299314010706,
+                    "heading": -0.5720518591979534,
+                    "angularVelocity": 0.000002556920622107498,
+                    "velocityX": 2.100602453152653,
+                    "velocityY": -3.1461243276865574,
+                    "timestamp": 2.2676100543101265
+                },
+                {
+                    "x": 7.356753135957797,
+                    "y": 2.927931973099431,
+                    "heading": -0.5720517877574303,
+                    "angularVelocity": 0.000002073124165462267,
+                    "velocityX": 1.855750953634494,
+                    "velocityY": -3.2964858375736252,
+                    "timestamp": 2.3020703747540434
+                },
+                {
+                    "x": 7.411896317838347,
+                    "y": 2.8098078848104033,
+                    "heading": -0.5720517267503814,
+                    "angularVelocity": 0.0000017703564026272917,
+                    "velocityX": 1.6001935318717442,
+                    "velocityY": -3.427829073187373,
+                    "timestamp": 2.3365306951979603
+                },
+                {
+                    "x": 7.457914797691639,
+                    "y": 2.687839154921363,
+                    "heading": -0.572051672451037,
+                    "angularVelocity": 0.0000015757063075533039,
+                    "velocityX": 1.3354048732140973,
+                    "velocityY": -3.5393962771625227,
+                    "timestamp": 2.370991015641877
+                },
+                {
+                    "x": 7.494543133171347,
+                    "y": 2.5627294544923394,
+                    "heading": -0.5720516223974168,
+                    "angularVelocity": 0.000001452500141635371,
+                    "velocityX": 1.0629133742199746,
+                    "velocityY": -3.630543733121263,
+                    "timestamp": 2.405451336085794
+                },
+                {
+                    "x": 7.521592976236692,
+                    "y": 2.435205423722826,
+                    "heading": -0.572051574793162,
+                    "angularVelocity": 0.0000013814222897397843,
+                    "velocityX": 0.7849562255098341,
+                    "velocityY": -3.70060490229679,
+                    "timestamp": 2.439911656529711
+                },
+                {
+                    "x": 7.539069360193098,
+                    "y": 2.3060208712744585,
+                    "heading": -0.572051528188319,
+                    "angularVelocity": 0.000001352420480159191,
+                    "velocityX": 0.5071451376970121,
+                    "velocityY": -3.748791386272309,
+                    "timestamp": 2.474371976973628
+                },
+                {
+                    "x": 7.551504596507269,
+                    "y": 2.176254012981795,
+                    "heading": -0.5720514816592456,
+                    "angularVelocity": 0.0000013502217280486013,
+                    "velocityX": 0.3608566651205708,
+                    "velocityY": -3.7656892513183533,
+                    "timestamp": 2.5088322974175448
+                },
+                {
+                    "x": 7.5639699142109125,
+                    "y": 2.0464900408376128,
+                    "heading": -0.5720514351306527,
+                    "angularVelocity": 0.0000013502077822692335,
+                    "velocityX": 0.36172959342888805,
+                    "velocityY": -3.765605498514506,
+                    "timestamp": 2.5432926178614617
+                },
+                {
+                    "x": 7.576441327813496,
+                    "y": 1.916726654418564,
+                    "heading": -0.5720513886022516,
+                    "angularVelocity": 0.00000135020222116771,
+                    "velocityX": 0.36190648960876737,
+                    "velocityY": -3.7655885014252446,
+                    "timestamp": 2.5777529383053785
+                },
+                {
+                    "x": 7.58884833483585,
+                    "y": 1.786957094038731,
+                    "heading": -0.5720513420716767,
+                    "angularVelocity": 0.000001350265295572762,
+                    "velocityX": 0.36003748258495927,
+                    "velocityY": -3.765767662871606,
+                    "timestamp": 2.6122132587492954
+                },
+                {
+                    "x": 7.601019357724818,
+                    "y": 1.6571651890693204,
+                    "heading": -0.5720512933827582,
+                    "angularVelocity": 0.0000014128980208836598,
+                    "velocityX": 0.3531894866947264,
+                    "velocityY": -3.7664160778959355,
+                    "timestamp": 2.6466735791932123
+                },
+                {
+                    "x": 7.610550391987499,
+                    "y": 1.5345810963054864,
+                    "heading": -0.5537181215211259,
+                    "angularVelocity": 0.5320081654919462,
+                    "velocityX": 0.2765799661741953,
+                    "velocityY": -3.5572534203020942,
+                    "timestamp": 2.6811338996371292
+                },
+                {
+                    "x": 7.620334086621719,
+                    "y": 1.4202071871878734,
+                    "heading": -0.5150322166693104,
+                    "angularVelocity": 1.122621738667962,
+                    "velocityX": 0.2839118878810709,
+                    "velocityY": -3.319003063356803,
+                    "timestamp": 2.715594220081046
+                },
+                {
+                    "x": 7.630352820350644,
+                    "y": 1.314046573370508,
+                    "heading": -0.4556179078059868,
+                    "angularVelocity": 1.7241368651811046,
+                    "velocityX": 0.290732459821803,
+                    "velocityY": -3.0806624096878554,
+                    "timestamp": 2.750054540524963
+                },
+                {
+                    "x": 7.63947235322732,
+                    "y": 1.2175511973082003,
+                    "heading": -0.3892109145347841,
+                    "angularVelocity": 1.9270567544269157,
+                    "velocityX": 0.2646386556821465,
+                    "velocityY": -2.8001880080992603,
+                    "timestamp": 2.78451486096888
+                },
+                {
+                    "x": 7.647652812069227,
+                    "y": 1.1309118710627388,
+                    "heading": -0.32231407882554747,
+                    "angularVelocity": 1.9412714347239703,
+                    "velocityX": 0.23738777633239685,
+                    "velocityY": -2.5141764536539966,
+                    "timestamp": 2.818975181412797
+                },
+                {
+                    "x": 7.654895533140534,
+                    "y": 1.0540989766744202,
+                    "heading": -0.25819483301023066,
+                    "angularVelocity": 1.8606688791433752,
+                    "velocityX": 0.21017567387602423,
+                    "velocityY": -2.22902437930995,
+                    "timestamp": 2.8534355018567137
+                },
+                {
+                    "x": 7.66120815675186,
+                    "y": 0.9870533783479266,
+                    "heading": -0.19885832997172487,
+                    "angularVelocity": 1.7218790270703905,
+                    "velocityX": 0.18318528469800885,
+                    "velocityY": -1.9455883596791401,
+                    "timestamp": 2.8878958223006306
+                },
+                {
+                    "x": 7.666600355589264,
+                    "y": 0.929716153410391,
+                    "heading": -0.14570825222338094,
+                    "angularVelocity": 1.542355876662632,
+                    "velocityX": 0.15647558606309142,
+                    "velocityY": -1.663862210186161,
+                    "timestamp": 2.9223561427445475
+                },
+                {
+                    "x": 7.671080940668327,
+                    "y": 0.8820338579186632,
+                    "heading": -0.09980369912266952,
+                    "angularVelocity": 1.3320988461333123,
+                    "velocityX": 0.1300215732566484,
+                    "velocityY": -1.383686944215496,
+                    "timestamp": 2.9568164631884644
+                },
+                {
+                    "x": 7.674656709905022,
+                    "y": 0.8439593502971015,
+                    "heading": -0.06197555143316066,
+                    "angularVelocity": 1.0977305841102087,
+                    "velocityX": 0.10376482837700403,
+                    "velocityY": -1.1048796741031013,
+                    "timestamp": 2.9912767836323813
+                },
+                {
+                    "x": 7.67733258738128,
+                    "y": 0.8154516156506164,
+                    "heading": -0.032889080662387146,
+                    "angularVelocity": 0.8440568861838474,
+                    "velocityX": 0.07765097485389878,
+                    "velocityY": -0.8272626104241091,
+                    "timestamp": 3.025737104076298
+                },
                 {
-                    "x": 7.6791456483845035,
-                    "y": 0.7965233362757584,
-                    "heading": 0.006187731070844568,
-                    "angularVelocity": -0.5251616322048499,
-                    "velocityX": 0.04976269489958636,
-                    "velocityY": -0.5537737524278505,
-                    "timestamp": 3.0108003825608534
+                    "x": 7.679112422858347,
+                    "y": 0.7964752743679322,
+                    "heading": -0.013083588528854375,
+                    "angularVelocity": 0.5747332548975859,
+                    "velocityX": 0.05164883710108163,
+                    "velocityY": -0.5506722235380279,
+                    "timestamp": 3.060197424520215
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
                     "heading": -0.003,
-                    "angularVelocity": -0.26685848868430057,
-                    "velocityX": 0.024814720757314154,
-                    "velocityY": -0.2766061725343536,
-                    "timestamp": 3.0452296080134253
+                    "angularVelocity": 0.2926144736603049,
+                    "velocityX": 0.025756497043992084,
+                    "velocityY": -0.27496187632245256,
+                    "timestamp": 3.094657744964132
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
                     "heading": -0.003,
-                    "angularVelocity": -2.388488097950269e-30,
+                    "angularVelocity": 9.445490920898867e-33,
                     "velocityX": 0,
-                    "velocityY": 9.563090319474696e-31,
-                    "timestamp": 3.0796588334659973
+                    "velocityY": 3.8255947334103544e-33,
+                    "timestamp": 3.129118065408049
                 }
             ],
             "constraints": [
@@ -3230,73 +3354,73 @@
                 {
                     "x": 1.35,
                     "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": -1.1658790467474463e-32,
-                    "velocityY": 2.7665844766493316e-30,
+                    "heading": 7.039472547405692e-42,
+                    "angularVelocity": 1.0705191064262213e-40,
+                    "velocityX": 0,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 1.4250000045295077,
-                    "y": 5.560000008699219,
-                    "heading": -5.237332222253062e-9,
-                    "angularVelocity": -5.5222228614505115e-8,
-                    "velocityX": 0.7907971475606554,
-                    "velocityY": 9.172423930862072e-8,
-                    "timestamp": 0.09484101550044491
+                    "x": 1.4250000045295101,
+                    "y": 5.56,
+                    "heading": 8.194631828276944e-23,
+                    "angularVelocity": 8.6403876700238e-22,
+                    "velocityX": 0.7907971475606789,
+                    "velocityY": 8.424071056371489e-35,
+                    "timestamp": 0.0948410155004449
                 },
                 {
-                    "x": 1.575000009059013,
-                    "y": 5.560000023677869,
-                    "heading": -1.3214169082837694e-8,
-                    "angularVelocity": -8.41074599041897e-8,
-                    "velocityX": 1.5815942473623306,
-                    "velocityY": 1.579343082639882e-7,
-                    "timestamp": 0.18968203100088982
+                    "x": 1.5750000090590195,
+                    "y": 5.56,
+                    "heading": 2.6954329855279996e-22,
+                    "angularVelocity": 1.978015178210688e-21,
+                    "velocityX": 1.5815942473623736,
+                    "velocityY": 1.2488550168110521e-34,
+                    "timestamp": 0.1896820310008898
                 },
                 {
-                    "x": 1.7999999999999883,
-                    "y": 5.560000042361517,
-                    "heading": -2.1609400534628168e-8,
-                    "angularVelocity": -8.851899686654573e-8,
-                    "velocityX": 2.372391203887095,
-                    "velocityY": 1.9699965815991792e-7,
-                    "timestamp": 0.28452304650133475
+                    "x": 1.7999999999999998,
+                    "y": 5.56,
+                    "heading": 5.87848829010012e-22,
+                    "angularVelocity": 3.3562006983676933e-21,
+                    "velocityX": 2.3723912038871497,
+                    "velocityY": 2.135553914770072e-34,
+                    "timestamp": 0.2845230465013347
                 },
                 {
-                    "x": 2.0249999909409753,
-                    "y": 5.560000024806983,
-                    "heading": -1.1174048484706425e-8,
-                    "angularVelocity": 1.1002994951426502e-7,
-                    "velocityX": 2.372391203887216,
-                    "velocityY": -1.8509433303171823e-7,
-                    "timestamp": 0.37936406200177963
+                    "x": 2.0249999909409806,
+                    "y": 5.56,
+                    "heading": 4.475372520266276e-22,
+                    "angularVelocity": -1.4794393741169351e-21,
+                    "velocityX": 2.37239120388715,
+                    "velocityY": 2.583104828416982e-34,
+                    "timestamp": 0.3793640620017796
                 },
                 {
-                    "x": 2.1749999954704884,
-                    "y": 5.560000009521861,
-                    "heading": -3.737396801552092e-9,
-                    "angularVelocity": 7.841176829375941e-8,
-                    "velocityX": 1.581594247362411,
-                    "velocityY": -1.6116572709153717e-7,
-                    "timestamp": 0.4742050775022245
+                    "x": 2.17499999547049,
+                    "y": 5.56,
+                    "heading": 1.6184867431561565e-22,
+                    "angularVelocity": -3.012289189189882e-21,
+                    "velocityX": 1.5815942473623736,
+                    "velocityY": 1.9272829246954695e-34,
+                    "timestamp": 0.47420507750222446
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 2.347281627650706e-31,
-                    "angularVelocity": 3.940696757721575e-8,
-                    "velocityX": 0.7907971475606952,
-                    "velocityY": -1.0039814487748917e-7,
+                    "heading": 1.246024226890136e-41,
+                    "angularVelocity": -1.7065260802739357e-21,
+                    "velocityX": 0.7907971475606789,
+                    "velocityY": -3.1542839688982834e-34,
                     "timestamp": 0.5690460930026694
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": -9.04607706846012e-36,
+                    "heading": 7.131086050293936e-41,
+                    "angularVelocity": 6.207915462550707e-40,
                     "velocityX": 0,
-                    "velocityY": -2.7063013184679635e-30,
+                    "velocityY": 0,
                     "timestamp": 0.6638871085031143
                 }
             ],
@@ -3317,23 +3441,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "Center To W3": {
             "waypoints": [
@@ -3360,110 +3468,110 @@
                 {
                     "x": 1.35,
                     "y": 5.56,
-                    "heading": -1.4926991504029801e-31,
-                    "angularVelocity": 3.0130798970250824e-30,
-                    "velocityX": -6.359546450907426e-32,
-                    "velocityY": -1.6682795346112646e-37,
+                    "heading": 0,
+                    "angularVelocity": 5.56203910119815e-35,
+                    "velocityX": 0,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 1.381665864471773,
-                    "y": 5.519999051141232,
-                    "heading": -0.0129840210078559,
-                    "angularVelocity": -0.16577982445307443,
-                    "velocityX": 0.40430937766578573,
-                    "velocityY": -0.5107316351191263,
-                    "timestamp": 0.07832087559925201
+                    "x": 1.3816658774685813,
+                    "y": 5.519999060819193,
+                    "heading": -0.012983997100427386,
+                    "angularVelocity": -0.16577951920330813,
+                    "velocityX": 0.4043095436088268,
+                    "velocityY": -0.5107315115509736,
+                    "timestamp": 0.07832087559926142
                 },
                 {
-                    "x": 1.4449977408289922,
-                    "y": 5.439997231153384,
-                    "heading": -0.03894969123495126,
-                    "angularVelocity": -0.3315293659375504,
-                    "velocityX": 0.8086206375075861,
-                    "velocityY": -1.0214622777865288,
-                    "timestamp": 0.15664175119850401
+                    "x": 1.4449977779940397,
+                    "y": 5.439997258819368,
+                    "heading": -0.038949622534069785,
+                    "angularVelocity": -0.33152879401526647,
+                    "velocityX": 0.8086209460872757,
+                    "velocityY": -1.0214620481155445,
+                    "timestamp": 0.15664175119852283
                 },
                 {
-                    "x": 1.5399958493613461,
-                    "y": 5.319994671739442,
-                    "heading": -0.07789445681060216,
-                    "angularVelocity": -0.4972462996317751,
-                    "velocityX": 1.212934710005481,
-                    "velocityY": -1.532191238871801,
-                    "timestamp": 0.23496262679775604
+                    "x": 1.5399959199593471,
+                    "y": 5.3199947242673264,
+                    "heading": -0.07789432529768456,
+                    "angularVelocity": -0.497245497648426,
+                    "velocityX": 1.2129351368768808,
+                    "velocityY": -1.5321909214351659,
+                    "timestamp": 0.23496262679778424
                 },
                 {
-                    "x": 1.6666604646819347,
-                    "y": 5.1599915852165825,
-                    "heading": -0.12981761618342352,
-                    "angularVelocity": -0.6629542759263352,
-                    "velocityX": 1.6172522887601493,
-                    "velocityY": -2.042917489093896,
-                    "timestamp": 0.31328350239700803
+                    "x": 1.6666605759505058,
+                    "y": 5.159991667949077,
+                    "heading": -0.1298174067167751,
+                    "angularVelocity": -0.662953280613991,
+                    "velocityX": 1.6172528080412967,
+                    "velocityY": -2.042917103441561,
+                    "timestamp": 0.31328350239704567
                 },
                 {
-                    "x": 1.8249918742420412,
-                    "y": 4.95998829498017,
-                    "heading": -0.19472302000870664,
-                    "angularVelocity": -0.8287114173414627,
-                    "velocityX": 2.0215735376893504,
-                    "velocityY": -2.5536396102078434,
-                    "timestamp": 0.39160437799626
+                    "x": 1.8249920311985446,
+                    "y": 4.959988411604181,
+                    "heading": -0.19472272142711608,
+                    "angularVelocity": -0.828710279522869,
+                    "velocityX": 2.0215741210320677,
+                    "velocityY": -2.5536391774810774,
+                    "timestamp": 0.3916043779963071
                 },
                 {
-                    "x": 1.9833278418494473,
-                    "y": 4.759991864222915,
-                    "heading": -0.25979913954005024,
-                    "angularVelocity": -0.8308911134284025,
-                    "velocityX": 2.0216317347825643,
-                    "velocityY": -2.5535520284602864,
-                    "timestamp": 0.469925253595512
+                    "x": 1.9833279596837488,
+                    "y": 4.759991951944026,
+                    "heading": -0.259798921868918,
+                    "angularVelocity": -0.8308921464919682,
+                    "velocityX": 2.021631235270526,
+                    "velocityY": -2.553552397491847,
+                    "timestamp": 0.4699252535955685
                 },
                 {
-                    "x": 2.1099966770232617,
-                    "y": 4.599994992856252,
-                    "heading": -0.31187323961880925,
-                    "angularVelocity": -0.6648814850489074,
-                    "velocityX": 1.6173061677954905,
-                    "velocityY": -2.0428381340541457,
-                    "timestamp": 0.548246129194764
+                    "x": 2.1099967561163506,
+                    "y": 4.599995051908685,
+                    "heading": -0.3118731002379449,
+                    "angularVelocity": -0.6648824846579983,
+                    "velocityX": 1.6173056731479674,
+                    "velocityY": -2.042838500095239,
+                    "timestamp": 0.54824612919483
                 },
                 {
-                    "x": 2.204998325241535,
-                    "y": 4.479997471163016,
-                    "heading": -0.3509354488627182,
-                    "angularVelocity": -0.49874581897910997,
-                    "velocityX": 1.212979904672827,
-                    "velocityY": -1.5321269173142713,
-                    "timestamp": 0.6265670047940161
+                    "x": 2.2049983692603425,
+                    "y": 4.4799975041635465,
+                    "heading": -0.3509353765811358,
+                    "angularVelocity": -0.4987466757018699,
+                    "velocityX": 1.2129794568446812,
+                    "velocityY": -1.532127249944463,
+                    "timestamp": 0.6265670047940913
                 },
                 {
-                    "x": 2.2683327695206965,
-                    "y": 4.399999160989759,
-                    "heading": -0.37697877943637265,
-                    "angularVelocity": -0.33252093231071334,
-                    "velocityX": 0.8086534247041309,
-                    "velocityY": -1.0214174645159662,
-                    "timestamp": 0.7048878803932681
+                    "x": 2.268332785802464,
+                    "y": 4.399999173265512,
+                    "heading": -0.3769787554180828,
+                    "angularVelocity": -0.33252154853581545,
+                    "velocityX": 0.808653070557838,
+                    "velocityY": -1.021417729129522,
+                    "timestamp": 0.7048878803933527
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
                     "heading": -0.39,
-                    "angularVelocity": -0.16625478793487553,
-                    "velocityX": 0.40432681883354604,
-                    "velocityY": -0.5107088076290647,
-                    "timestamp": 0.7832087559925202
+                    "angularVelocity": -0.16625509460009477,
+                    "velocityX": 0.40432661094808764,
+                    "velocityY": -0.5107089643656818,
+                    "timestamp": 0.7832087559926141
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
                     "heading": -0.39,
-                    "angularVelocity": 0,
-                    "velocityX": 7.259744325812885e-33,
-                    "velocityY": -1.1119528318776295e-29,
-                    "timestamp": 0.8615296315917722
+                    "angularVelocity": -5.703180031220666e-32,
+                    "velocityX": 1.0257416304055705e-32,
+                    "velocityY": 0,
+                    "timestamp": 0.8615296315918755
                 }
             ],
             "constraints": [
@@ -3483,23 +3591,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "Center To C1": {
             "waypoints": [
@@ -3527,217 +3619,217 @@
                     "x": 1.35,
                     "y": 5.56,
                     "heading": 0,
-                    "angularVelocity": -1.7848043602710886e-30,
-                    "velocityX": 4.033392918765628e-32,
-                    "velocityY": -2.9619524123271102e-31,
+                    "angularVelocity": 0,
+                    "velocityX": -2.1086675817598662e-32,
+                    "velocityY": -7.052966104933725e-38,
                     "timestamp": 0
                 },
                 {
-                    "x": 1.4233949207458116,
-                    "y": 5.582111402652435,
-                    "heading": -4.931478664026919e-9,
-                    "angularVelocity": -5.143711867197603e-8,
-                    "velocityX": 0.7654820738778157,
-                    "velocityY": 0.2306138106917608,
-                    "timestamp": 0.09588065253314748
-                },
-                {
-                    "x": 1.570184760818058,
-                    "y": 5.626334201974914,
-                    "heading": -1.1368240805751946e-8,
-                    "angularVelocity": -6.714026800936027e-8,
-                    "velocityX": 1.530964132952106,
-                    "velocityY": 0.4612275589893128,
-                    "timestamp": 0.19176130506629496
-                },
-                {
-                    "x": 1.7903695159222153,
-                    "y": 5.69266839083479,
-                    "heading": -1.6115736339739044e-8,
-                    "angularVelocity": -4.952541704566216e-8,
-                    "velocityX": 2.2964461472361695,
-                    "velocityY": 0.6918412328958446,
-                    "timestamp": 0.28764195759944244
-                },
-                {
-                    "x": 2.0839491700576396,
-                    "y": 5.781113958180788,
-                    "heading": -1.639772094621516e-8,
-                    "angularVelocity": -2.9553274583871895e-9,
-                    "velocityX": 3.0619279946395954,
-                    "velocityY": 0.9224547915413995,
-                    "timestamp": 0.3835226101325899
-                },
-                {
-                    "x": 2.431241903149783,
-                    "y": 5.885741340897451,
-                    "heading": -1.6397499367976412e-8,
-                    "angularVelocity": 2.3168753954621105e-12,
-                    "velocityX": 3.6221356855297673,
-                    "velocityY": 1.0912251841409724,
-                    "timestamp": 0.4794032626657374
-                },
-                {
-                    "x": 2.7785346566980107,
-                    "y": 5.9903686557177505,
-                    "heading": -1.6397564221401302e-8,
-                    "angularVelocity": -6.713496881725287e-13,
-                    "velocityX": 3.622135898879975,
-                    "velocityY": 1.091224476005724,
-                    "timestamp": 0.5752839151988849
-                },
-                {
-                    "x": 3.125827432121608,
-                    "y": 6.094995897925851,
-                    "heading": -1.6397647407961563e-8,
-                    "angularVelocity": -8.684642063246534e-13,
-                    "velocityX": 3.6221361270322743,
-                    "velocityY": 1.0912237186873281,
-                    "timestamp": 0.6711645677320324
-                },
-                {
-                    "x": 3.4731202302782154,
-                    "y": 6.1996230646748005,
-                    "heading": -1.6397748264247437e-8,
-                    "angularVelocity": -1.0543921850153982e-12,
-                    "velocityX": 3.6221363641294033,
-                    "velocityY": 1.0912229316763022,
-                    "timestamp": 0.7670452202651799
-                },
-                {
-                    "x": 3.8204130521112987,
-                    "y": 6.304250152832848,
-                    "heading": -1.639786614425739e-8,
-                    "angularVelocity": -1.2203887930777277e-12,
-                    "velocityX": 3.622136611066492,
-                    "velocityY": 1.091222112002269,
-                    "timestamp": 0.8629258727983273
-                },
-                {
-                    "x": 4.167705899087819,
-                    "y": 6.408877157530555,
-                    "heading": -1.6398004207896958e-8,
-                    "angularVelocity": -1.408074947723948e-12,
-                    "velocityX": 3.6221368733034063,
-                    "velocityY": 1.0912212415417761,
-                    "timestamp": 0.9588065253314748
-                },
-                {
-                    "x": 4.514998774010408,
-                    "y": 6.51350406946466,
-                    "heading": -1.6398168919050745e-8,
-                    "angularVelocity": -1.6668913936591836e-12,
-                    "velocityX": 3.6221371647706677,
-                    "velocityY": 1.091220274051783,
-                    "timestamp": 1.0546871778646223
-                },
-                {
-                    "x": 4.862291683961819,
-                    "y": 6.618130865121593,
-                    "heading": -1.6398370762014746e-8,
-                    "angularVelocity": -2.0513887673195245e-12,
-                    "velocityX": 3.6221375301080334,
-                    "velocityY": 1.0912190613250203,
-                    "timestamp": 1.1505678303977698
-                },
-                {
-                    "x": 5.20958464417323,
-                    "y": 6.722757493946074,
-                    "heading": -1.639835180923073e-8,
-                    "angularVelocity": 1.902358989315915e-13,
-                    "velocityX": 3.6221380542985027,
-                    "velocityY": 1.091217321333091,
-                    "timestamp": 1.2464484829309173
-                },
-                {
-                    "x": 5.556877646148189,
-                    "y": 6.8273839841549115,
-                    "heading": -1.6398206125528607e-8,
-                    "angularVelocity": 1.4352640935347951e-12,
-                    "velocityX": 3.62213848987558,
-                    "velocityY": 1.09121587562664,
-                    "timestamp": 1.3423291354640647
-                },
-                {
-                    "x": 5.904170680592406,
-                    "y": 6.932010366591235,
-                    "heading": -1.6398102572596455e-8,
-                    "angularVelocity": 9.899977906647045e-13,
-                    "velocityX": 3.6221388285174303,
-                    "velocityY": 1.091214751599812,
-                    "timestamp": 1.4382097879972122
-                },
-                {
-                    "x": 6.2514637423704675,
-                    "y": 7.036636658299249,
-                    "heading": -1.6398041778119566e-8,
-                    "angularVelocity": 5.750349690087699e-13,
-                    "velocityX": 3.6221391135989474,
-                    "velocityY": 1.0912138053372793,
-                    "timestamp": 1.5340904405303597
-                },
-                {
-                    "x": 6.5987568262419405,
-                    "y": 7.141262876671409,
-                    "heading": -1.639800860171266e-8,
-                    "angularVelocity": 3.175769618903374e-13,
-                    "velocityX": 3.6221393440248217,
-                    "velocityY": 1.0912130404711837,
-                    "timestamp": 1.6299710930635072
-                },
-                {
-                    "x": 6.946049927457118,
-                    "y": 7.2458890374680776,
-                    "heading": -1.6398270316090924e-8,
-                    "angularVelocity": -2.740394345840068e-12,
-                    "velocityX": 3.6221395249132713,
-                    "velocityY": 1.0912124399786907,
-                    "timestamp": 1.7258517455966547
-                },
-                {
-                    "x": 7.239629937032664,
-                    "y": 7.334333424990155,
-                    "heading": -1.4134914023195805e-8,
-                    "angularVelocity": 2.36203418937418e-8,
-                    "velocityX": 3.061931701744924,
-                    "velocityY": 0.9224424864262022,
-                    "timestamp": 1.8217323981298021
-                },
-                {
-                    "x": 7.45981496297016,
-                    "y": 7.4006667148640615,
-                    "heading": -8.953329207671134e-9,
-                    "angularVelocity": 5.4052849582725206e-8,
-                    "velocityX": 2.2964489719251726,
-                    "velocityY": 0.6918318568130147,
-                    "timestamp": 1.9176130506629496
-                },
+                    "x": 1.4233949672121562,
+                    "y": 5.582111248416047,
+                    "heading": 6.989955400155978e-19,
+                    "angularVelocity": 7.290266672073875e-18,
+                    "velocityX": 0.7654825584964474,
+                    "velocityY": 0.23061220206204183,
+                    "timestamp": 0.09588065253415788
+                },
+                {
+                    "x": 1.5701848986828006,
+                    "y": 5.626333744358309,
+                    "heading": -5.64955528358279e-19,
+                    "angularVelocity": -1.3182545571253806e-17,
+                    "velocityX": 1.530965086187225,
+                    "velocityY": 0.4612243948434498,
+                    "timestamp": 0.19176130506831576
+                },
+                {
+                    "x": 1.7903697885046006,
+                    "y": 5.69266748604712,
+                    "heading": -1.1328157855510893e-18,
+                    "angularVelocity": -5.922574643212496e-18,
+                    "velocityX": 2.2964475522666925,
+                    "velocityY": 0.6918365690635988,
+                    "timestamp": 0.28764195760247363
+                },
+                {
+                    "x": 2.08394961895559,
+                    "y": 5.781112468143492,
+                    "heading": -1.2347885191323161e-17,
+                    "angularVelocity": -1.1696905584501664e-16,
+                    "velocityX": 3.061929833512548,
+                    "velocityY": 0.9224486876000673,
+                    "timestamp": 0.3835226101366315
+                },
+                {
+                    "x": 2.4312425305333245,
+                    "y": 5.885739258408696,
+                    "heading": -1.8571615119787707e-17,
+                    "angularVelocity": -6.491121789400923e-17,
+                    "velocityX": 3.6221375470302584,
+                    "velocityY": 1.0912190050847865,
+                    "timestamp": 0.4794032626707894
+                },
+                {
+                    "x": 2.7785354421111035,
+                    "y": 5.990366048673914,
+                    "heading": -2.5105020741934312e-17,
+                    "angularVelocity": -6.814102166319416e-17,
+                    "velocityX": 3.6221375470307158,
+                    "velocityY": 1.0912190050849244,
+                    "timestamp": 0.5752839152049473
+                },
+                {
+                    "x": 3.125828353688883,
+                    "y": 6.094992838939131,
+                    "heading": -3.974939977960392e-17,
+                    "angularVelocity": -1.527354961675144e-16,
+                    "velocityX": 3.622137547030716,
+                    "velocityY": 1.0912190050849244,
+                    "timestamp": 0.6711645677391052
+                },
+                {
+                    "x": 3.473121265266662,
+                    "y": 6.199619629204348,
+                    "heading": -4.6160884572564577e-17,
+                    "angularVelocity": -6.68694321901595e-17,
+                    "velocityX": 3.6221375470307158,
+                    "velocityY": 1.0912190050849242,
+                    "timestamp": 0.767045220273263
+                },
+                {
+                    "x": 3.8204141768444413,
+                    "y": 6.304246419469565,
+                    "heading": -5.207468497060529e-17,
+                    "angularVelocity": -6.167876669314405e-17,
+                    "velocityX": 3.6221375470307158,
+                    "velocityY": 1.0912190050849244,
+                    "timestamp": 0.862925872807421
+                },
+                {
+                    "x": 4.16770708842222,
+                    "y": 6.408873209734782,
+                    "heading": -3.0597254964935646e-17,
+                    "angularVelocity": 2.24001708769928e-16,
+                    "velocityX": 3.6221375470307158,
+                    "velocityY": 1.0912190050849242,
+                    "timestamp": 0.9588065253415788
+                },
+                {
+                    "x": 4.515,
+                    "y": 6.5135,
+                    "heading": -2.0783137065340813e-18,
+                    "angularVelocity": 2.974420855995074e-16,
+                    "velocityX": 3.6221375470307158,
+                    "velocityY": 1.0912190050849244,
+                    "timestamp": 1.0546871778757367
+                },
+                {
+                    "x": 4.862292911577779,
+                    "y": 6.618126790265217,
+                    "heading": -2.7789619178473824e-17,
+                    "angularVelocity": -2.681594752696605e-16,
+                    "velocityX": 3.6221375470307158,
+                    "velocityY": 1.0912190050849244,
+                    "timestamp": 1.1505678304098945
+                },
+                {
+                    "x": 5.209585823155558,
+                    "y": 6.722753580530434,
+                    "heading": 2.2712942266126744e-18,
+                    "angularVelocity": 3.135242889004865e-16,
+                    "velocityX": 3.6221375470307158,
+                    "velocityY": 1.0912190050849242,
+                    "timestamp": 1.2464484829440523
+                },
+                {
+                    "x": 5.5568787347333375,
+                    "y": 6.827380370795651,
+                    "heading": 3.7041174502192387e-17,
+                    "angularVelocity": 3.6263708429820527e-16,
+                    "velocityX": 3.6221375470307158,
+                    "velocityY": 1.0912190050849244,
+                    "timestamp": 1.34232913547821
+                },
+                {
+                    "x": 5.9041716463111165,
+                    "y": 6.932007161060868,
+                    "heading": 6.494346998398645e-17,
+                    "angularVelocity": 2.910106965725311e-16,
+                    "velocityX": 3.6221375470307158,
+                    "velocityY": 1.0912190050849244,
+                    "timestamp": 1.4382097880123679
+                },
+                {
+                    "x": 6.251464557888895,
+                    "y": 7.036633951326086,
+                    "heading": 5.759102638783417e-17,
+                    "angularVelocity": -7.668328700140211e-17,
+                    "velocityX": 3.6221375470307158,
+                    "velocityY": 1.0912190050849244,
+                    "timestamp": 1.5340904405465257
+                },
+                {
+                    "x": 6.598757469466675,
+                    "y": 7.141260741591303,
+                    "heading": 2.5440135069531877e-17,
+                    "angularVelocity": -3.353219911269207e-16,
+                    "velocityX": 3.6221375470307158,
+                    "velocityY": 1.0912190050849244,
+                    "timestamp": 1.6299710930806834
+                },
+                {
+                    "x": 6.94605038104441,
+                    "y": 7.245887531856507,
+                    "heading": 2.2711688502269004e-17,
+                    "angularVelocity": -2.84566958514003e-17,
+                    "velocityX": 3.6221375470302584,
+                    "velocityY": 1.0912190050847868,
+                    "timestamp": 1.7258517456148412
+                },
+                {
+                    "x": 7.239630211495399,
+                    "y": 7.334332513952879,
+                    "heading": 1.1340524791180054e-17,
+                    "angularVelocity": -1.1859706178712186e-16,
+                    "velocityX": 3.061929833512548,
+                    "velocityY": 0.9224486876000674,
+                    "timestamp": 1.821732398148999
+                },
+                {
+                    "x": 7.4598151013172,
+                    "y": 7.40066625564169,
+                    "heading": 5.896173994762203e-18,
+                    "angularVelocity": -5.67825796795303e-17,
+                    "velocityX": 2.2964475522666925,
+                    "velocityY": 0.6918365690635988,
+                    "timestamp": 1.9176130506831568
+                },
                 {
-                    "x": 7.606604986310546,
-                    "y": 7.444888905858524,
-                    "heading": -3.5476510780655253e-9,
-                    "angularVelocity": 5.638647309242438e-8,
-                    "velocityX": 1.5309660443693864,
-                    "velocityY": 0.4612212143584205,
-                    "timestamp": 2.0134937031960973
+                    "x": 7.606605032787844,
+                    "y": 7.444888751583952,
+                    "heading": 1.9652682638036787e-18,
+                    "angularVelocity": -4.099790316709085e-17,
+                    "velocityX": 1.530965086187225,
+                    "velocityY": 0.46122439484344985,
+                    "timestamp": 2.0134937032173146
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 1.0344705803481413e-31,
-                    "angularVelocity": 3.7004322975715074e-8,
-                    "velocityX": 0.7654830432448523,
-                    "velocityY": 0.23061059303913045,
-                    "timestamp": 2.109374355729245
+                    "heading": 0,
+                    "angularVelocity": -2.0497026255693862e-17,
+                    "velocityX": 0.7654825584964475,
+                    "velocityY": 0.23061220206204183,
+                    "timestamp": 2.1093743557514726
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
                     "heading": 0,
-                    "angularVelocity": -1.5335768309775317e-33,
-                    "velocityX": 0,
-                    "velocityY": -2.5688294662733772e-30,
-                    "timestamp": 2.2052550082623927
+                    "angularVelocity": 0,
+                    "velocityX": 2.8084467268312453e-34,
+                    "velocityY": 0,
+                    "timestamp": 2.2052550082856306
                 }
             ],
             "constraints": [
@@ -3757,23 +3849,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "Center To C2": {
             "waypoints": [
@@ -4304,10 +4380,10 @@
                 {
                     "x": 4.018431663513184,
                     "y": 5.088562488555908,
-                    "heading": 3.14,
+                    "heading": -0.4829717875114574,
                     "isInitialGuess": false,
                     "translationConstrained": true,
-                    "headingConstrained": false,
+                    "headingConstrained": true,
                     "controlIntervalCount": 40
                 },
                 {
@@ -4333,1091 +4409,1091 @@
                 {
                     "x": 1.35,
                     "y": 5.56,
-                    "heading": 4.408218114222724e-33,
+                    "heading": -1.9296664556128306e-33,
                     "angularVelocity": 0,
-                    "velocityX": 1.8935205228973059e-34,
-                    "velocityY": 9.513896878368867e-32,
+                    "velocityX": -7.813211441980168e-34,
+                    "velocityY": -2.2407271587906947e-32,
                     "timestamp": 0
                 },
                 {
-                    "x": 1.3544641547566925,
-                    "y": 5.559252486403593,
-                    "heading": 5.298729726015063e-12,
-                    "angularVelocity": 2.274228720676412e-10,
-                    "velocityX": 0.1916026983161006,
-                    "velocityY": -0.0320834805032552,
-                    "timestamp": 0.02329901820759936
-                },
-                {
-                    "x": 1.3633924655059346,
-                    "y": 5.557757467272512,
-                    "heading": 1.605830471024488e-11,
-                    "angularVelocity": 4.618037851350714e-10,
-                    "velocityX": 0.38320544967553505,
-                    "velocityY": -0.06416661499484601,
-                    "timestamp": 0.04659803641519872
-                },
-                {
-                    "x": 1.3767849336296958,
-                    "y": 5.555514951621415,
-                    "heading": 3.246020157611451e-11,
-                    "angularVelocity": 7.039737222967939e-10,
-                    "velocityX": 0.574808260349448,
-                    "velocityY": -0.09624936257493934,
-                    "timestamp": 0.06989705462279808
-                },
-                {
-                    "x": 1.3946415606835925,
-                    "y": 5.552524949597437,
-                    "heading": 5.470894313107332e-11,
-                    "angularVelocity": 9.549218317932479e-10,
-                    "velocityX": 0.7664111377908356,
-                    "velocityY": -0.12833167463728912,
-                    "timestamp": 0.09319607283039744
-                },
-                {
-                    "x": 1.416962348431807,
-                    "y": 5.548787472707871,
-                    "heading": 8.303679095756625e-11,
-                    "angularVelocity": 1.2158386690956822e-9,
-                    "velocityX": 0.9580140909513821,
-                    "velocityY": -0.1604134928036672,
-                    "timestamp": 0.1164950910379968
-                },
-                {
-                    "x": 1.4437472988920377,
-                    "y": 5.544302534113244,
-                    "heading": 1.1770979097345793e-10,
-                    "angularVelocity": 1.4881742667086445e-9,
-                    "velocityX": 1.1496171307120806,
-                    "velocityY": -0.19249474611664116,
-                    "timestamp": 0.13979410924559615
-                },
-                {
-                    "x": 1.4749964143943655,
-                    "y": 5.539070149011117,
-                    "heading": 1.59035725627389e-10,
-                    "angularVelocity": 1.7737199674396097e-9,
-                    "velocityX": 1.3412202704804594,
-                    "velocityY": -0.22457534714647368,
-                    "timestamp": 0.1630931274531955
-                },
-                {
-                    "x": 1.5107096976598688,
-                    "y": 5.533090335148591,
-                    "heading": 2.073747689126765e-10,
-                    "angularVelocity": 2.0747244300773254e-9,
-                    "velocityX": 1.532823527038234,
-                    "velocityY": -0.25665518646593166,
-                    "timestamp": 0.18639214566079487
-                },
-                {
-                    "x": 1.5508871519080267,
-                    "y": 5.526363113522473,
-                    "heading": 2.631540997774015e-10,
-                    "angularVelocity": 2.3940635247907603e-9,
-                    "velocityX": 1.7244269217769022,
-                    "velocityY": -0.28873412459698383,
-                    "timestamp": 0.20969116386839423
-                },
-                {
-                    "x": 1.595528781007412,
-                    "y": 5.518888509361551,
-                    "heading": 3.268885139183836e-10,
-                    "angularVelocity": 2.7354977853334202e-9,
-                    "velocityX": 1.9160304825557395,
-                    "velocityY": -0.3208119799018042,
-                    "timestamp": 0.2329901820759936
-                },
-                {
-                    "x": 1.6446345896939198,
-                    "y": 5.5106665535489805,
-                    "heading": 3.9921044507968937e-10,
-                    "angularVelocity": 3.104076267437521e-9,
-                    "velocityX": 2.1076342466004383,
-                    "velocityY": -0.35288850969337676,
-                    "timestamp": 0.2562892002835929
-                },
-                {
-                    "x": 1.698204583899122,
-                    "y": 5.501697284762255,
-                    "heading": 4.809154717853132e-10,
-                    "angularVelocity": 3.5068011876967e-9,
-                    "velocityX": 2.299238265229739,
-                    "velocityY": -0.38496337943692166,
-                    "timestamp": 0.2795882184911923
-                },
-                {
-                    "x": 1.7562387712682006,
-                    "y": 5.491980752848314,
-                    "heading": 5.730347406124241e-10,
-                    "angularVelocity": 3.953783188523521e-9,
-                    "velocityX": 2.4908426119922917,
-                    "velocityY": -0.41703610973659105,
-                    "timestamp": 0.3028872366987917
-                },
-                {
-                    "x": 1.8187371620272614,
-                    "y": 5.48151702447463,
-                    "heading": 6.769576861119728e-10,
-                    "angularVelocity": 4.460400083403711e-9,
-                    "velocityX": 2.6824473976619876,
-                    "velocityY": -0.449105978650929,
-                    "timestamp": 0.3261862549063911
-                },
-                {
-                    "x": 1.8856997705540457,
-                    "y": 5.4703061933617745,
-                    "heading": 7.946577963057737e-10,
-                    "angularVelocity": 5.051719599980361e-9,
-                    "velocityX": 2.8740528004281045,
-                    "velocityY": -0.4811718250535651,
-                    "timestamp": 0.34948527311399047
-                },
-                {
-                    "x": 1.957126618545275,
-                    "y": 5.4583484009138274,
-                    "heading": 9.291581669995643e-10,
-                    "angularVelocity": 5.772791178735968e-9,
-                    "velocityX": 3.0656591344232145,
-                    "velocityY": -0.5132316023559669,
-                    "timestamp": 0.37278429132158986
-                },
-                {
-                    "x": 2.0330177424946343,
-                    "y": 5.445643884917638,
-                    "heading": 1.0856695619650843e-9,
-                    "angularVelocity": 6.717510100758224e-9,
-                    "velocityX": 3.257267034736916,
-                    "velocityY": -0.5452811737842305,
-                    "timestamp": 0.39608330952918924
-                },
-                {
-                    "x": 2.1133732165489003,
-                    "y": 5.432193128386492,
-                    "heading": 1.2752962310893515e-9,
-                    "angularVelocity": 8.13882633127911e-9,
-                    "velocityX": 3.44887811744974,
-                    "velocityY": -0.5773100141552516,
-                    "timestamp": 0.41938232773678863
-                },
-                {
-                    "x": 2.1981932628587333,
-                    "y": 5.417997633484574,
-                    "heading": 1.5377676317044546e-9,
-                    "angularVelocity": 1.1265341261395982e-8,
-                    "velocityX": 3.640498734927221,
-                    "velocityY": -0.6092743812441802,
-                    "timestamp": 0.442681345944388
-                },
-                {
-                    "x": 2.285115669591906,
-                    "y": 5.403405223128801,
-                    "heading": 1.537767230961928e-9,
-                    "angularVelocity": -1.719997404273206e-14,
-                    "velocityX": 3.730732598201487,
-                    "velocityY": -0.6263100970908951,
-                    "timestamp": 0.4659803641519874
-                },
-                {
-                    "x": 2.3720410537379144,
-                    "y": 5.388830564421114,
-                    "heading": 1.5377672475870007e-9,
-                    "angularVelocity": 7.135525631450618e-16,
-                    "velocityX": 3.7308603895453194,
-                    "velocityY": -0.6255481916751834,
-                    "timestamp": 0.4892793823595868
-                },
-                {
-                    "x": 2.45896811239221,
-                    "y": 5.374265896231001,
-                    "heading": 1.5377672778413637e-9,
-                    "angularVelocity": 1.298525326828493e-15,
-                    "velocityX": 3.730932259882311,
-                    "velocityY": -0.625119396028046,
-                    "timestamp": 0.5125784005671862
-                },
-                {
-                    "x": 2.5458958805121723,
-                    "y": 5.359705463035692,
-                    "heading": 1.5377672869369513e-9,
-                    "angularVelocity": 3.903849318113422e-16,
-                    "velocityX": 3.730962710335439,
-                    "velocityY": -0.6249376289364498,
-                    "timestamp": 0.5358774187747856
-                },
-                {
-                    "x": 2.632823878731898,
-                    "y": 5.345146403658019,
-                    "heading": 1.5377672987189865e-9,
-                    "angularVelocity": 5.056879723770718e-16,
-                    "velocityX": 3.7309725862775656,
-                    "velocityY": -0.6248786643237555,
-                    "timestamp": 0.559176436982385
-                },
-                {
-                    "x": 2.7197519358547084,
-                    "y": 5.330587695987482,
-                    "heading": 1.5377673268477785e-9,
-                    "angularVelocity": 1.2072952571647457e-15,
-                    "velocityX": 3.7309751144130874,
-                    "velocityY": -0.6248635689629851,
-                    "timestamp": 0.5824754551899843
-                },
-                {
-                    "x": 2.806680023647356,
-                    "y": 5.316029171436071,
-                    "heading": 1.5377673567071691e-9,
-                    "angularVelocity": 1.2815730065587283e-15,
-                    "velocityX": 3.7309764307702067,
-                    "velocityY": -0.6248557094441328,
-                    "timestamp": 0.6057744733975837
-                },
-                {
-                    "x": 2.8936082747189524,
-                    "y": 5.301471621825431,
-                    "heading": 1.5377673928791505e-9,
-                    "angularVelocity": 1.5525110632783364e-15,
-                    "velocityX": 3.7309834387453797,
-                    "velocityY": -0.6248138647313558,
-                    "timestamp": 0.6290734916051831
-                },
-                {
-                    "x": 2.980536964716405,
-                    "y": 5.286916693414742,
-                    "heading": 1.5377674058328616e-9,
-                    "angularVelocity": 5.559766105183297e-16,
-                    "velocityX": 3.7310022775585137,
-                    "velocityY": -0.6247013621403136,
-                    "timestamp": 0.6523725098127825
-                },
-                {
-                    "x": 3.0674663067511823,
-                    "y": 5.272365659809418,
-                    "heading": 1.5377674408459054e-9,
-                    "angularVelocity": 1.5027691524839091e-15,
-                    "velocityX": 3.73103026317236,
-                    "velocityY": -0.6245341960676912,
-                    "timestamp": 0.6756715280203819
-                },
-                {
-                    "x": 3.154396100803892,
-                    "y": 5.257817326905748,
-                    "heading": 1.5377674636831172e-9,
-                    "angularVelocity": 9.801792492345066e-16,
-                    "velocityX": 3.7310496639016963,
-                    "velocityY": -0.624418281237505,
-                    "timestamp": 0.6989705462279813
-                },
-                {
-                    "x": 3.2413249978571956,
-                    "y": 5.243263635140824,
-                    "heading": 1.5377674638426843e-9,
-                    "angularVelocity": 6.848580307945622e-18,
-                    "velocityX": 3.731011164452385,
-                    "velocityY": -0.6246482849651879,
-                    "timestamp": 0.7222695644355807
-                },
-                {
-                    "x": 3.3282506523054423,
-                    "y": 5.228690588454777,
-                    "heading": 1.5377674749707248e-9,
-                    "angularVelocity": 4.776183371381179e-16,
-                    "velocityX": 3.7308719909897503,
-                    "velocityY": -0.6254790032748808,
-                    "timestamp": 0.74556858264318
-                },
-                {
-                    "x": 3.415171009638962,
-                    "y": 5.214085980519485,
-                    "heading": 1.5377674915535578e-9,
-                    "angularVelocity": 7.117396517888937e-16,
-                    "velocityX": 3.7306446374302076,
-                    "velocityY": -0.626833620407003,
-                    "timestamp": 0.7688676008507794
-                },
-                {
-                    "x": 3.5020842171218036,
-                    "y": 5.1994388833661285,
-                    "heading": 1.53776751807092e-9,
-                    "angularVelocity": 1.13813226337154e-15,
-                    "velocityX": 3.730337763955042,
-                    "velocityY": -0.6286572688425895,
-                    "timestamp": 0.7921666190583788
-                },
-                {
-                    "x": 3.5889883305639807,
-                    "y": 5.184737925794629,
-                    "heading": 1.5377675551515876e-9,
-                    "angularVelocity": 1.5915121086693097e-15,
-                    "velocityX": 3.729947445333141,
-                    "velocityY": -0.6309689721834142,
-                    "timestamp": 0.8154656372659782
-                },
-                {
-                    "x": 3.675881697311449,
-                    "y": 5.1699735802440445,
-                    "heading": 1.5377675887419511e-9,
-                    "angularVelocity": 1.441707334130238e-15,
-                    "velocityX": 3.7294861943638535,
-                    "velocityY": -0.6336896009521162,
-                    "timestamp": 0.8387646554735776
-                },
-                {
-                    "x": 3.7627625314341118,
-                    "y": 5.155135664387593,
-                    "heading": 1.5377676077743658e-9,
-                    "angularVelocity": 8.168762038570203e-16,
-                    "velocityX": 3.728948290804007,
-                    "velocityY": -0.636847257870781,
-                    "timestamp": 0.862063673681177
-                },
-                {
-                    "x": 3.8490591061764308,
-                    "y": 5.137209586747657,
-                    "heading": 1.5377676390897132e-9,
-                    "angularVelocity": 1.344063051824819e-15,
-                    "velocityX": 3.703871724266177,
-                    "velocityY": -0.7693919752476525,
-                    "timestamp": 0.8853626918887764
-                },
-                {
-                    "x": 3.934365266987087,
-                    "y": 5.115044254457407,
-                    "heading": 1.5377676702466937e-9,
-                    "angularVelocity": 1.337265902944942e-15,
-                    "velocityX": 3.661362897377225,
-                    "velocityY": -0.9513419017440721,
-                    "timestamp": 0.9086617100963758
+                    "x": 1.35439418502164,
+                    "y": 5.559271848784049,
+                    "heading": -0.0037613483747314606,
+                    "angularVelocity": -0.16062668985124567,
+                    "velocityX": 0.18765169410035007,
+                    "velocityY": -0.031095370031394967,
+                    "timestamp": 0.023416708507251784
+                },
+                {
+                    "x": 1.3631871179840578,
+                    "y": 5.557813607446842,
+                    "heading": -0.011205273313316288,
+                    "angularVelocity": -0.317889464963847,
+                    "velocityX": 0.37549824560930783,
+                    "velocityY": -0.06227354014150002,
+                    "timestamp": 0.04683341701450357
+                },
+                {
+                    "x": 1.3763837173940232,
+                    "y": 5.5556231238892515,
+                    "heading": -0.02224504855076525,
+                    "angularVelocity": -0.47144863395424863,
+                    "velocityX": 0.563554839736692,
+                    "velocityY": -0.09354361467639355,
+                    "timestamp": 0.07025012552175536
+                },
+                {
+                    "x": 1.3939893513502628,
+                    "y": 5.552698044632534,
+                    "heading": -0.03678367986427699,
+                    "angularVelocity": -0.6208657083043512,
+                    "velocityX": 0.7518406761047259,
+                    "velocityY": -0.12491419346204678,
+                    "timestamp": 0.09366683402900713
+                },
+                {
+                    "x": 1.4160099408355828,
+                    "y": 5.549035829570074,
+                    "heading": -0.0547111148835232,
+                    "angularVelocity": -0.7655830456997818,
+                    "velocityX": 0.9403793653792271,
+                    "velocityY": -0.15639324635770782,
+                    "timestamp": 0.11708354253625891
+                },
+                {
+                    "x": 1.4424520698548373,
+                    "y": 5.544633771060564,
+                    "heading": -0.07590088077413953,
+                    "angularVelocity": -0.9048994176125892,
+                    "velocityX": 1.129199221618411,
+                    "velocityY": -0.18798792785714832,
+                    "timestamp": 0.1405002510435107
+                },
+                {
+                    "x": 1.4733230980825243,
+                    "y": 5.539489016814175,
+                    "heading": -0.10020600379509263,
+                    "angularVelocity": -1.0379393420482537,
+                    "velocityX": 1.3183333694454236,
+                    "velocityY": -0.2197044151102235,
+                    "timestamp": 0.16391695955076246
+                },
+                {
+                    "x": 1.5086312686123933,
+                    "y": 5.533598592872994,
+                    "heading": -0.12745397379473564,
+                    "angularVelocity": -1.163612298082128,
+                    "velocityX": 1.5078195348818708,
+                    "velocityY": -0.25154790389774145,
+                    "timestamp": 0.18733366805801424
+                },
+                {
+                    "x": 1.5483857981367395,
+                    "y": 5.526959418437362,
+                    "heading": -0.15744034724886685,
+                    "angularVelocity": -1.2805545854082991,
+                    "velocityX": 1.6976992950155392,
+                    "velocityY": -0.28352295684851286,
+                    "timestamp": 0.21075037656526602
+                },
+                {
+                    "x": 1.5925969268012463,
+                    "y": 5.5195682985391485,
+                    "heading": -0.18992026053497643,
+                    "angularVelocity": -1.3870400819163256,
+                    "velocityX": 1.888016355962865,
+                    "velocityY": -0.31563444947560276,
+                    "timestamp": 0.2341670850725178
+                },
+                {
+                    "x": 1.6412758830317984,
+                    "y": 5.511421875068327,
+                    "heading": -0.22459650029504458,
+                    "angularVelocity": -1.4808332157070316,
+                    "velocityX": 2.0788129217851563,
+                    "velocityY": -0.34788934868040844,
+                    "timestamp": 0.25758379357976957
+                },
+                {
+                    "x": 1.6944346649736404,
+                    "y": 5.502516516620167,
+                    "heading": -0.26110152083126115,
+                    "angularVelocity": -1.5589304758570732,
+                    "velocityX": 2.2701218630013136,
+                    "velocityY": -0.38029932539011047,
+                    "timestamp": 0.2810005020870214
+                },
+                {
+                    "x": 1.7520853949904163,
+                    "y": 5.492848149517658,
+                    "heading": -0.298968117565775,
+                    "angularVelocity": -1.6170759747377448,
+                    "velocityX": 2.4619484843022064,
+                    "velocityY": -0.4128832666435571,
+                    "timestamp": 0.3044172105942732
+                },
+                {
+                    "x": 1.8142385663960479,
+                    "y": 5.482412126535505,
+                    "heading": -0.337577206769387,
+                    "angularVelocity": -1.6487837815317001,
+                    "velocityX": 2.654223217852448,
+                    "velocityY": -0.44566566556215986,
+                    "timestamp": 0.327833919101525
+                },
+                {
+                    "x": 1.8808979849725118,
+                    "y": 5.471203531590798,
+                    "heading": -0.3760544549024592,
+                    "angularVelocity": -1.6431535679387552,
+                    "velocityX": 2.8466604756095246,
+                    "velocityY": -0.47865800358925525,
+                    "timestamp": 0.3512506276087768
+                },
+                {
+                    "x": 1.9520436894250033,
+                    "y": 5.459218834920382,
+                    "heading": -0.41303307424029884,
+                    "angularVelocity": -1.5791552995754587,
+                    "velocityX": 3.0382452952539354,
+                    "velocityY": -0.5118010785637118,
+                    "timestamp": 0.3746673361160286
+                },
+                {
+                    "x": 2.027554046757936,
+                    "y": 5.446452385802885,
+                    "heading": -0.44595627669268334,
+                    "angularVelocity": -1.4059705462956014,
+                    "velocityX": 3.2246358325529645,
+                    "velocityY": -0.5451854650516427,
+                    "timestamp": 0.3980840446232804
+                },
+                {
+                    "x": 2.106704107988529,
+                    "y": 5.4327632732947455,
+                    "heading": -0.46854805770485686,
+                    "angularVelocity": -0.9647718425148408,
+                    "velocityX": 3.3800677497471567,
+                    "velocityY": -0.5845873899786794,
+                    "timestamp": 0.4215007531305322
+                },
+                {
+                    "x": 2.1895060473348913,
+                    "y": 5.418170740153337,
+                    "heading": -0.4808901513114338,
+                    "angularVelocity": -0.5270635539044761,
+                    "velocityX": 3.5360195614476866,
+                    "velocityY": -0.6231675616108724,
+                    "timestamp": 0.444917461637784
+                },
+                {
+                    "x": 2.275961949822614,
+                    "y": 5.402702256569038,
+                    "heading": -0.4829709400280361,
+                    "angularVelocity": -0.0888591458512548,
+                    "velocityX": 3.6920604132278863,
+                    "velocityY": -0.6605746311236733,
+                    "timestamp": 0.4683341701450358
+                },
+                {
+                    "x": 2.3633379318433296,
+                    "y": 5.388122823546896,
+                    "heading": -0.48297098226531077,
+                    "angularVelocity": -0.0000018037238101032338,
+                    "velocityX": 3.7313519956767345,
+                    "velocityY": -0.6226081269122786,
+                    "timestamp": 0.4917508786522876
+                },
+                {
+                    "x": 2.4507217300981603,
+                    "y": 5.373590311340196,
+                    "heading": -0.48297102439627604,
+                    "angularVelocity": -0.0000017991839149984119,
+                    "velocityX": 3.7316857844376363,
+                    "velocityY": -0.6206043946012527,
+                    "timestamp": 0.5151675871595394
+                },
+                {
+                    "x": 2.5381084152434026,
+                    "y": 5.359075168632344,
+                    "heading": -0.4829710665257657,
+                    "angularVelocity": -0.0000017991208983915868,
+                    "velocityX": 3.7318090677936033,
+                    "velocityY": -0.6198626379688947,
+                    "timestamp": 0.5385842956667911
+                },
+                {
+                    "x": 2.6254950830921073,
+                    "y": 5.344559921791865,
+                    "heading": -0.482971108655264,
+                    "angularVelocity": -0.0000017991212673817243,
+                    "velocityX": 3.7318083291527238,
+                    "velocityY": -0.6198670849061145,
+                    "timestamp": 0.5620010041740429
+                },
+                {
+                    "x": 2.712881673840345,
+                    "y": 5.330044210788612,
+                    "heading": -0.4829711507848006,
+                    "angularVelocity": -0.0000017991229009417428,
+                    "velocityX": 3.731805036611997,
+                    "velocityY": -0.6198869067684731,
+                    "timestamp": 0.5854177126812946
+                },
+                {
+                    "x": 2.800268245782709,
+                    "y": 5.315528386572539,
+                    "heading": -0.4829711929143463,
+                    "angularVelocity": -0.0000017991232935293638,
+                    "velocityX": 3.7318042335155903,
+                    "velocityY": -0.6198917414709255,
+                    "timestamp": 0.6088344211885464
+                },
+                {
+                    "x": 2.8876548130761686,
+                    "y": 5.301012534370082,
+                    "heading": -0.4829712350438941,
+                    "angularVelocity": -0.0000017991233825733017,
+                    "velocityX": 3.731804034986268,
+                    "velocityY": -0.6198929366167053,
+                    "timestamp": 0.6322511296957981
+                },
+                {
+                    "x": 2.975041345179212,
+                    "y": 5.286496470320027,
+                    "heading": -0.48297127717345933,
+                    "angularVelocity": -0.000001799124124149535,
+                    "velocityX": 3.7318025321954105,
+                    "velocityY": -0.6199019834729332,
+                    "timestamp": 0.6556678382030499
+                },
+                {
+                    "x": 3.0624277772479402,
+                    "y": 5.271979804077107,
+                    "heading": -0.4829713193030743,
+                    "angularVelocity": -0.0000017991262516148556,
+                    "velocityX": 3.7317982602749975,
+                    "velocityY": -0.6199276998481766,
+                    "timestamp": 0.6790845467103016
+                },
+                {
+                    "x": 3.149814115388955,
+                    "y": 5.257462572426413,
+                    "heading": -0.482971361432736,
+                    "angularVelocity": -0.0000017991282469783394,
+                    "velocityX": 3.731794249134225,
+                    "velocityY": -0.619951845332987,
+                    "timestamp": 0.7025012552175534
+                },
+                {
+                    "x": 3.2372009926614522,
+                    "y": 5.242948586436876,
+                    "heading": -0.48297140356212753,
+                    "angularVelocity": -0.0000017991167072996656,
+                    "velocityX": 3.7318172725014778,
+                    "velocityY": -0.6198132408343549,
+                    "timestamp": 0.7259179637248051
+                },
+                {
+                    "x": 3.3245908005140246,
+                    "y": 5.228452256133779,
+                    "heading": -0.4829714456900571,
+                    "angularVelocity": -0.0000017990542795775989,
+                    "velocityX": 3.7319424216049217,
+                    "velocityY": -0.6190592626894866,
+                    "timestamp": 0.7493346722320569
+                },
+                {
+                    "x": 3.4119833565424647,
+                    "y": 5.213972502683899,
+                    "heading": -0.48297148781659605,
+                    "angularVelocity": -0.0000017989948869691892,
+                    "velocityX": 3.7320597812190472,
+                    "velocityY": -0.6183513556300411,
+                    "timestamp": 0.7727513807393086
+                },
+                {
+                    "x": 3.4993677183765524,
+                    "y": 5.199443379550061,
+                    "heading": -0.48297152994719206,
+                    "angularVelocity": -0.0000017991681461203675,
+                    "velocityX": 3.731709851836638,
+                    "velocityY": -0.6204596657670539,
+                    "timestamp": 0.7961680892465604
+                },
+                {
+                    "x": 3.5867330233447126,
+                    "y": 5.184800101210533,
+                    "heading": -0.48297157208742414,
+                    "angularVelocity": -0.0000017995796480013664,
+                    "velocityX": 3.7308960369522555,
+                    "velocityY": -0.6253346124593939,
+                    "timestamp": 0.8195847977538121
+                },
+                {
+                    "x": 3.6740864894013328,
+                    "y": 5.170086363489935,
+                    "heading": -0.48297161423379187,
+                    "angularVelocity": -0.0000017998416699478457,
+                    "velocityX": 3.7303904615618975,
+                    "velocityY": -0.6283435486246598,
+                    "timestamp": 0.8430015062610638
+                },
+                {
+                    "x": 3.7614250146254347,
+                    "y": 5.155284197469098,
+                    "heading": -0.482971656398251,
+                    "angularVelocity": -0.0000018006142553635733,
+                    "velocityX": 3.729752420031712,
+                    "velocityY": -0.6321198393988197,
+                    "timestamp": 0.8664182147683156
+                },
+                {
+                    "x": 3.848175799548237,
+                    "y": 5.137355912505346,
+                    "heading": -0.48297169899264486,
+                    "angularVelocity": -0.000001818974422860047,
+                    "velocityX": 3.704653234929829,
+                    "velocityY": -0.7656193421980864,
+                    "timestamp": 0.8898349232755673
+                },
+                {
+                    "x": 3.9339293985898425,
+                    "y": 5.115142351916783,
+                    "heading": -0.48297174253890224,
+                    "angularVelocity": -0.0000018596233270255789,
+                    "velocityX": 3.6620688605764276,
+                    "velocityY": -0.9486201095127581,
+                    "timestamp": 0.9132516317828191
                 },
                 {
                     "x": 4.018431663513184,
                     "y": 5.088562488555908,
-                    "heading": 1.5377676866838871e-9,
-                    "angularVelocity": 7.054887729794734e-16,
-                    "velocityX": 3.608151887646215,
-                    "velocityY": -1.1366043695780652,
-                    "timestamp": 0.9319607283039751
-                },
-                {
-                    "x": 4.095480434320656,
-                    "y": 5.06018496604887,
-                    "heading": 1.5377676994335126e-9,
-                    "angularVelocity": 5.874067965982521e-16,
-                    "velocityX": 3.549827430213289,
-                    "velocityY": -1.3074226459526814,
-                    "timestamp": 0.9536656625077448
-                },
-                {
-                    "x": 4.171095847536846,
-                    "y": 5.028183306370306,
-                    "heading": 1.5377677188417825e-9,
-                    "angularVelocity": 8.941870233905506e-16,
-                    "velocityX": 3.4837891009642465,
-                    "velocityY": -1.4743956087592989,
-                    "timestamp": 0.9753705967115144
-                },
-                {
-                    "x": 4.24514985906727,
-                    "y": 4.992717825181937,
-                    "heading": 1.5377677281143787e-9,
-                    "angularVelocity": 4.2721152582009774e-16,
-                    "velocityX": 3.4118514636002706,
-                    "velocityY": -1.6339824325334937,
-                    "timestamp": 0.997075530915284
-                },
-                {
-                    "x": 4.317807248978416,
-                    "y": 4.95447255507026,
-                    "heading": 1.5377677290979519e-9,
-                    "angularVelocity": 4.531574211416763e-17,
-                    "velocityX": 3.3475056514378028,
-                    "velocityY": -1.7620541832797834,
-                    "timestamp": 1.0187804651190535
-                },
-                {
-                    "x": 4.3901722659206355,
-                    "y": 4.9156769197600605,
-                    "heading": 1.5377677449553673e-9,
-                    "angularVelocity": 7.305902735413318e-16,
-                    "velocityX": 3.334035305654692,
-                    "velocityY": -1.7874108691538961,
-                    "timestamp": 1.0404853993228231
-                },
-                {
-                    "x": 4.4625185516483485,
-                    "y": 4.876846365558822,
-                    "heading": 1.5377677746336623e-9,
-                    "angularVelocity": 1.3673523815751288e-15,
-                    "velocityX": 3.333172312277143,
-                    "velocityY": -1.7890196688239732,
-                    "timestamp": 1.0621903335265928
-                },
-                {
-                    "x": 4.534844691324942,
-                    "y": 4.837978300028894,
-                    "heading": 1.5377677779199108e-9,
-                    "angularVelocity": 1.5140568225814148e-16,
-                    "velocityX": 3.3322441338764652,
-                    "velocityY": -1.7907479085103777,
-                    "timestamp": 1.0838952677303624
-                },
-                {
-                    "x": 4.6071619879089205,
-                    "y": 4.799093783673277,
-                    "heading": 1.537767801979053e-9,
-                    "angularVelocity": 1.1084641137098069e-15,
-                    "velocityX": 3.331836710728115,
-                    "velocityY": -1.79150583873765,
-                    "timestamp": 1.105600201934132
-                },
-                {
-                    "x": 4.6794740379059565,
-                    "y": 4.7601995113399385,
-                    "heading": 1.5377678257958516e-9,
-                    "angularVelocity": 1.0972987247710634e-15,
-                    "velocityX": 3.331594987489656,
-                    "velocityY": -1.791955320763837,
-                    "timestamp": 1.1273051361379016
-                },
-                {
-                    "x": 4.751784687579244,
-                    "y": 4.721302635647572,
-                    "heading": 1.5377678370559733e-9,
-                    "angularVelocity": 5.187816897475249e-16,
-                    "velocityX": 3.331530471108496,
-                    "velocityY": -1.7920752639579989,
-                    "timestamp": 1.1490100703416712
-                },
-                {
-                    "x": 4.824095678023389,
-                    "y": 4.682406393453542,
-                    "heading": 1.5377678370330874e-9,
-                    "angularVelocity": -1.0543217693671843e-18,
-                    "velocityX": 3.3315461712634193,
-                    "velocityY": -1.792046077125237,
-                    "timestamp": 1.1707150045454409
-                },
-                {
-                    "x": 4.896406945309153,
-                    "y": 4.643510665929771,
-                    "heading": 1.5377678640109226e-9,
-                    "angularVelocity": 1.242935520614447e-15,
-                    "velocityX": 3.331558926041247,
-                    "velocityY": -1.7920223649940337,
-                    "timestamp": 1.1924199387492105
-                },
-                {
-                    "x": 4.968718824670649,
-                    "y": 4.604616076350799,
-                    "heading": 1.5377678830932924e-9,
-                    "angularVelocity": 8.791718726461835e-16,
-                    "velocityX": 3.33158712588479,
-                    "velocityY": -1.7919699370612494,
-                    "timestamp": 1.21412487295298
-                },
-                {
-                    "x": 5.041033201559798,
-                    "y": 4.565726130481367,
-                    "heading": 1.5377679126898002e-9,
-                    "angularVelocity": 1.3635842497565112e-15,
-                    "velocityX": 3.331702193162082,
-                    "velocityY": -1.7917559898752364,
-                    "timestamp": 1.2358298071567497
-                },
-                {
-                    "x": 5.113354228854967,
-                    "y": 4.526848553325412,
-                    "heading": 1.5377679256265375e-9,
-                    "angularVelocity": 5.960275144059911e-16,
-                    "velocityX": 3.332008593816099,
-                    "velocityY": -1.7911861326534588,
-                    "timestamp": 1.2575347413605193
-                },
-                {
-                    "x": 5.1856663573422805,
-                    "y": 4.487954426928371,
-                    "heading": 1.537767949104212e-9,
-                    "angularVelocity": 1.0816744331209405e-15,
-                    "velocityX": 3.3315986037289687,
-                    "velocityY": -1.791948597121158,
-                    "timestamp": 1.279239675564289
-                },
-                {
-                    "x": 5.257977857015597,
-                    "y": 4.449059131458437,
-                    "heading": 1.5377679798803482e-9,
-                    "angularVelocity": 1.4179326501228622e-15,
-                    "velocityX": 3.3315696327123336,
-                    "velocityY": -1.7920024592017922,
-                    "timestamp": 1.3009446097680586
-                },
-                {
-                    "x": 5.330290964048228,
-                    "y": 4.410166824431303,
-                    "heading": 1.5377679920779735e-9,
-                    "angularVelocity": 5.619748235100741e-16,
-                    "velocityX": 3.3316436877316113,
-                    "velocityY": -1.7918647742503244,
-                    "timestamp": 1.3226495439718282
-                },
-                {
-                    "x": 5.402605809232356,
-                    "y": 4.371277749351831,
-                    "heading": 1.5377680157791104e-9,
-                    "angularVelocity": 1.0919698963856968e-15,
-                    "velocityX": 3.3317237686705528,
-                    "velocityY": -1.7917158704270917,
-                    "timestamp": 1.3443544781755978
-                },
-                {
-                    "x": 5.474921974691154,
-                    "y": 4.332391129443428,
-                    "heading": 1.5377680240202577e-9,
-                    "angularVelocity": 3.7969007641948726e-16,
-                    "velocityX": 3.331784596990179,
-                    "velocityY": -1.7916027546232325,
-                    "timestamp": 1.3660594123793675
-                },
-                {
-                    "x": 5.54723622900274,
-                    "y": 4.29350095564713,
-                    "heading": 1.537768038636468e-9,
-                    "angularVelocity": 6.734049902652557e-16,
-                    "velocityX": 3.331696545710963,
-                    "velocityY": -1.7917664910290456,
-                    "timestamp": 1.387764346583137
-                },
-                {
-                    "x": 5.619548385303401,
-                    "y": 4.254606880959325,
-                    "heading": 1.5377680494372407e-9,
-                    "angularVelocity": 4.976183803494664e-16,
-                    "velocityX": 3.331599885158129,
-                    "velocityY": -1.7919462147497014,
-                    "timestamp": 1.4094692807869067
-                },
-                {
-                    "x": 5.6918607364808045,
-                    "y": 4.215713168590034,
-                    "heading": 1.5377680578875737e-9,
-                    "angularVelocity": 3.893277832205449e-16,
-                    "velocityX": 3.3316088636117085,
-                    "velocityY": -1.7919295218396705,
-                    "timestamp": 1.4311742149906763
-                },
-                {
-                    "x": 5.764173499508255,
-                    "y": 4.176820221955942,
-                    "heading": 1.5377680813491443e-9,
-                    "angularVelocity": 1.0809325075471762e-15,
-                    "velocityX": 3.331627838562525,
-                    "velocityY": -1.7918942425240987,
-                    "timestamp": 1.452879149194446
-                },
-                {
-                    "x": 5.8364876254997204,
-                    "y": 4.137929809554028,
-                    "heading": 1.5377680862134134e-9,
-                    "angularVelocity": 2.241089873857745e-16,
-                    "velocityX": 3.331690633686676,
-                    "velocityY": -1.7917774841780256,
-                    "timestamp": 1.4745840833982156
-                },
-                {
-                    "x": 5.90880309835116,
-                    "y": 4.099041901645272,
-                    "heading": 1.537768099475084e-9,
-                    "angularVelocity": 6.109980238433844e-16,
-                    "velocityX": 3.3317526868576484,
-                    "velocityY": -1.7916620959822775,
-                    "timestamp": 1.4962890176019852
-                },
-                {
-                    "x": 5.981120307623631,
-                    "y": 4.060157222932383,
-                    "heading": 1.5377681005691863e-9,
-                    "angularVelocity": 5.040806501517701e-17,
-                    "velocityX": 3.331832688069564,
-                    "velocityY": -1.7915133189414525,
-                    "timestamp": 1.5179939518057548
-                },
-                {
-                    "x": 6.053439067813782,
-                    "y": 4.0212754287411325,
-                    "heading": 1.5377681034134081e-9,
-                    "angularVelocity": 1.3104042715425496e-16,
-                    "velocityX": 3.3319041426820193,
-                    "velocityY": -1.7913804218969325,
-                    "timestamp": 1.5396988860095244
-                },
-                {
-                    "x": 6.1257574594958575,
-                    "y": 3.9823929491454755,
-                    "heading": 1.5377681141371771e-9,
-                    "angularVelocity": 4.940705905043031e-16,
-                    "velocityX": 3.3318871646008015,
-                    "velocityY": -1.7914120001783713,
-                    "timestamp": 1.561403820213294
-                },
-                {
-                    "x": 6.198071250397711,
-                    "y": 3.943501913620472,
-                    "heading": 1.5377681379511677e-9,
-                    "angularVelocity": 1.09716939803524e-15,
-                    "velocityX": 3.3316751952742036,
-                    "velocityY": -1.7918061930096516,
-                    "timestamp": 1.5831087544170637
-                },
-                {
-                    "x": 6.270375940604977,
-                    "y": 3.9045939610161593,
-                    "heading": 1.5377681515073034e-9,
-                    "angularVelocity": 6.245647694184294e-16,
-                    "velocityX": 3.331255903765101,
-                    "velocityY": -1.7925856046957434,
-                    "timestamp": 1.6048136886208333
-                },
-                {
-                    "x": 6.342673755736347,
-                    "y": 3.865673234790299,
-                    "heading": 1.5377699302540744e-9,
-                    "angularVelocity": 8.19513085832153e-14,
-                    "velocityX": 3.3309391520146527,
-                    "velocityY": -1.7931741170331514,
-                    "timestamp": 1.626518622824603
-                },
-                {
-                    "x": 6.414965746925861,
-                    "y": 3.826741692219575,
-                    "heading": 1.5377690472977265e-9,
-                    "angularVelocity": -4.068002902168065e-14,
-                    "velocityX": 3.3306708286121007,
-                    "velocityY": -1.7936724527872456,
-                    "timestamp": 1.6482235570283725
-                },
-                {
-                    "x": 6.487241720022193,
-                    "y": 3.7877804203169507,
-                    "heading": 1.5377684535437603e-9,
-                    "angularVelocity": -2.7355713192172554e-14,
-                    "velocityX": 3.3299328354443367,
-                    "velocityY": -1.7950421566324242,
-                    "timestamp": 1.6699284912321422
-                },
-                {
-                    "x": 6.559496923985796,
-                    "y": 3.748780644748349,
-                    "heading": 1.5377682143039968e-9,
-                    "angularVelocity": -1.1022367481483692e-14,
-                    "velocityX": 3.3289759501347236,
-                    "velocityY": -1.796816115748624,
-                    "timestamp": 1.6916334254359118
-                },
-                {
-                    "x": 6.630585509558441,
-                    "y": 3.7076928121475503,
-                    "heading": 1.5377682448018014e-9,
-                    "angularVelocity": 1.4051092264620289e-15,
-                    "velocityX": 3.2752269555510436,
-                    "velocityY": -1.8930180674612387,
-                    "timestamp": 1.7133383596396814
-                },
-                {
-                    "x": 6.6997616539527085,
-                    "y": 3.6634609457782683,
-                    "heading": 1.5377682789348178e-9,
-                    "angularVelocity": 1.5725923809489335e-15,
-                    "velocityX": 3.1871160605618085,
-                    "velocityY": -2.037871479084932,
-                    "timestamp": 1.735043293843451
-                },
-                {
-                    "x": 6.7667761400423245,
-                    "y": 3.6160177634442383,
-                    "heading": 1.5377682945212807e-9,
-                    "angularVelocity": 7.181069480230158e-16,
-                    "velocityX": 3.0875231161945336,
-                    "velocityY": -2.185824747894625,
-                    "timestamp": 1.7567482280472206
-                },
-                {
-                    "x": 6.831456555555867,
-                    "y": 3.5654387099344507,
-                    "heading": 1.5377683209709385e-9,
-                    "angularVelocity": 1.2186011773240001e-15,
-                    "velocityX": 2.979986712068234,
-                    "velocityY": -2.330302088683456,
-                    "timestamp": 1.7784531622509903
+                    "heading": -0.4829717875114574,
+                    "angularVelocity": -0.000001920532733427299,
+                    "velocityX": 3.6086312001179417,
+                    "velocityY": -1.135081104701801,
+                    "timestamp": 0.9366683402900708
+                },
+                {
+                    "x": 4.095489617333613,
+                    "y": 5.060208566496142,
+                    "heading": -0.48297174072333765,
+                    "angularVelocity": 0.00000215563208981735,
+                    "velocityX": 3.550230245224857,
+                    "velocityY": -1.306327856847248,
+                    "timestamp": 0.9583733984124503
+                },
+                {
+                    "x": 4.1711135466589555,
+                    "y": 5.028225843813575,
+                    "heading": -0.482971694875483,
+                    "angularVelocity": 0.0000021123119922022846,
+                    "velocityX": 3.4841615672696387,
+                    "velocityY": -1.4735147218783384,
+                    "timestamp": 0.9800784565348297
+                },
+                {
+                    "x": 4.245175973739862,
+                    "y": 4.9927768637698655,
+                    "heading": -0.48297164958118555,
+                    "angularVelocity": 0.0000020868083942836762,
+                    "velocityX": 3.4122197076519116,
+                    "velocityY": -1.633212859593547,
+                    "timestamp": 1.0017835146572092
+                },
+                {
+                    "x": 4.317835043195099,
+                    "y": 4.954533786792328,
+                    "heading": -0.4829716044969262,
+                    "angularVelocity": 0.0000020771314744606885,
+                    "velocityX": 3.3475639201504173,
+                    "velocityY": -1.7619430808199938,
+                    "timestamp": 1.0234885727795886
+                },
+                {
+                    "x": 4.39018173013391,
+                    "y": 4.915702997439957,
+                    "heading": -0.48297155943721365,
+                    "angularVelocity": 0.000002076000546478681,
+                    "velocityX": 3.333171767193961,
+                    "velocityY": -1.789020288884605,
+                    "timestamp": 1.045193630901968
+                },
+                {
+                    "x": 4.462524157787397,
+                    "y": 4.876864273483723,
+                    "heading": -0.4829715143782692,
+                    "angularVelocity": 0.000002075965162802355,
+                    "velocityX": 3.3329755325063237,
+                    "velocityY": -1.7893858536194671,
+                    "timestamp": 1.0668986890243475
+                },
+                {
+                    "x": 4.534839835738717,
+                    "y": 4.8379757656949245,
+                    "heading": -0.48297146931947993,
+                    "angularVelocity": 0.0000020759580074306272,
+                    "velocityX": 3.331743114605303,
+                    "velocityY": -1.7916795048180625,
+                    "timestamp": 1.088603747146727
+                },
+                {
+                    "x": 4.60713879668486,
+                    "y": 4.7990561876016065,
+                    "heading": -0.48297142426091105,
+                    "angularVelocity": 0.000002075947856864141,
+                    "velocityX": 3.3309729252282607,
+                    "velocityY": -1.793110982419974,
+                    "timestamp": 1.1103088052691064
+                },
+                {
+                    "x": 4.679445820973998,
+                    "y": 4.760151592099993,
+                    "heading": -0.48297137920221006,
+                    "angularVelocity": 0.0000020759539417382525,
+                    "velocityX": 3.331344421260875,
+                    "velocityY": -1.7924207013070022,
+                    "timestamp": 1.1320138633914858
+                },
+                {
+                    "x": 4.75175835900604,
+                    "y": 4.72125724602898,
+                    "heading": -0.4829713341434503,
+                    "angularVelocity": 0.000002075956651977989,
+                    "velocityX": 3.3315984515826855,
+                    "velocityY": -1.791948487386363,
+                    "timestamp": 1.1537189215138652
+                },
+                {
+                    "x": 4.824072564767644,
+                    "y": 4.68236600077068,
+                    "heading": -0.48297128908467135,
+                    "angularVelocity": 0.0000020759575350386475,
+                    "velocityX": 3.331675287569862,
+                    "velocityY": -1.7918056260909452,
+                    "timestamp": 1.1754239796362447
+                },
+                {
+                    "x": 4.8963869623773055,
+                    "y": 4.643475112237639,
+                    "heading": -0.48297124402589003,
+                    "angularVelocity": 0.000002075957644932215,
+                    "velocityX": 3.3316841264339563,
+                    "velocityY": -1.79178919096958,
+                    "timestamp": 1.1971290377586241
+                },
+                {
+                    "x": 4.968701267365325,
+                    "y": 4.604584051483172,
+                    "heading": -0.4829711989671097,
+                    "angularVelocity": 0.000002075957594698055,
+                    "velocityX": 3.331679859150302,
+                    "velocityY": -1.7917971255911598,
+                    "timestamp": 1.2188340958810036
+                },
+                {
+                    "x": 5.0410154847403765,
+                    "y": 4.565692827821048,
+                    "heading": -0.4829711539083305,
+                    "angularVelocity": 0.0000020759575472868184,
+                    "velocityX": 3.331675822627313,
+                    "velocityY": -1.7918046311069282,
+                    "timestamp": 1.240539154003383
+                },
+                {
+                    "x": 5.113329654064444,
+                    "y": 4.526801514813212,
+                    "heading": -0.4829711088495519,
+                    "angularVelocity": 0.000002075957520958662,
+                    "velocityX": 3.331673608812237,
+                    "velocityY": -1.7918087474613025,
+                    "timestamp": 1.2622442121257624
+                },
+                {
+                    "x": 5.185643801945598,
+                    "y": 4.487910161934636,
+                    "heading": -0.48297106379077354,
+                    "angularVelocity": 0.0000020759575090327586,
+                    "velocityX": 3.3316726208897984,
+                    "velocityY": -1.7918105843944965,
+                    "timestamp": 1.2839492702481419
+                },
+                {
+                    "x": 5.257957938797454,
+                    "y": 4.4490187885483135,
+                    "heading": -0.48297101873199527,
+                    "angularVelocity": 0.0000020759575026231074,
+                    "velocityX": 3.331672112745739,
+                    "velocityY": -1.7918115292316654,
+                    "timestamp": 1.3056543283705213
+                },
+                {
+                    "x": 5.330272065176824,
+                    "y": 4.410127395689581,
+                    "heading": -0.4829709736732172,
+                    "angularVelocity": 0.000002075957497099591,
+                    "velocityX": 3.3316716302550873,
+                    "velocityY": -1.7918124263686461,
+                    "timestamp": 1.3273593864929008
+                },
+                {
+                    "x": 5.402586187963667,
+                    "y": 4.371235996150952,
+                    "heading": -0.4829709286144391,
+                    "angularVelocity": 0.00000207595749402331,
+                    "velocityX": 3.3316714647394585,
+                    "velocityY": -1.7918127341261798,
+                    "timestamp": 1.3490644446152802
+                },
+                {
+                    "x": 5.474900315939471,
+                    "y": 4.332344606260608,
+                    "heading": -0.482970883555661,
+                    "angularVelocity": 0.0000020759574957870433,
+                    "velocityX": 3.331671703806323,
+                    "velocityY": -1.7918122896084423,
+                    "timestamp": 1.3707695027376596
+                },
+                {
+                    "x": 5.547214451711641,
+                    "y": 4.293453230866729,
+                    "heading": -0.4829708384968828,
+                    "angularVelocity": 0.0000020759574997718594,
+                    "velocityX": 3.331672063002153,
+                    "velocityY": -1.7918116217242377,
+                    "timestamp": 1.392474560860039
+                },
+                {
+                    "x": 5.619528591620017,
+                    "y": 4.254561863163663,
+                    "heading": -0.4829707934381046,
+                    "angularVelocity": 0.0000020759575011693265,
+                    "velocityX": 3.3316722535663335,
+                    "velocityY": -1.7918112673914555,
+                    "timestamp": 1.4141796189824185
+                },
+                {
+                    "x": 5.6918427353771435,
+                    "y": 4.215670502616919,
+                    "heading": -0.4829707483793263,
+                    "angularVelocity": 0.000002075957502624911,
+                    "velocityX": 3.3316724308867816,
+                    "velocityY": -1.7918109376838844,
+                    "timestamp": 1.435884677104798
+                },
+                {
+                    "x": 5.764156898177578,
+                    "y": 4.176779177479102,
+                    "heading": -0.4829707033205479,
+                    "angularVelocity": 0.0000020759575111736717,
+                    "velocityX": 3.3316733082540817,
+                    "velocityY": -1.7918093063163627,
+                    "timestamp": 1.4575897352271774
+                },
+                {
+                    "x": 5.836471080177112,
+                    "y": 4.137887888039928,
+                    "heading": -0.48297065826176927,
+                    "angularVelocity": 0.0000020759575207599675,
+                    "velocityX": 3.331674192798964,
+                    "velocityY": -1.7918076616010294,
+                    "timestamp": 1.4792947933495568
+                },
+                {
+                    "x": 5.908785251268908,
+                    "y": 4.098996578318978,
+                    "heading": -0.48297061320299073,
+                    "angularVelocity": 0.0000020759575149169034,
+                    "velocityX": 3.331673690255448,
+                    "velocityY": -1.7918085960272527,
+                    "timestamp": 1.5009998514719363
+                },
+                {
+                    "x": 5.981099339043632,
+                    "y": 4.0601051136793025,
+                    "heading": -0.48297056814421313,
+                    "angularVelocity": 0.000002075957470564626,
+                    "velocityX": 3.3316698516536873,
+                    "velocityY": -1.7918157334753977,
+                    "timestamp": 1.5227049095943157
+                },
+                {
+                    "x": 6.053413282760847,
+                    "y": 4.021213381182831,
+                    "heading": -0.48297052308543725,
+                    "angularVelocity": 0.00000207595739406405,
+                    "velocityX": 3.3316632146058507,
+                    "velocityY": -1.7918280742300665,
+                    "timestamp": 1.5444099677166951
+                },
+                {
+                    "x": 6.125727369888532,
+                    "y": 3.982321915340118,
+                    "heading": -0.4829704780266597,
+                    "angularVelocity": 0.0000020759574679524,
+                    "velocityX": 3.3316698218431826,
+                    "velocityY": -1.7918157889019397,
+                    "timestamp": 1.5661150258390746
+                },
+                {
+                    "x": 6.198042995964267,
+                    "y": 3.9434333111252817,
+                    "heading": -0.4829704329678644,
+                    "angularVelocity": 0.000002075958288103883,
+                    "velocityX": 3.3317407245813113,
+                    "velocityY": -1.7916839473808401,
+                    "timestamp": 1.587820083961454
+                },
+                {
+                    "x": 6.270359121166566,
+                    "y": 3.9045456350763237,
+                    "heading": -0.4829703879090624,
+                    "angularVelocity": 0.0000020759585995923127,
+                    "velocityX": 3.331763720444871,
+                    "velocityY": -1.791641184728093,
+                    "timestamp": 1.6095251420838335
+                },
+                {
+                    "x": 6.342677550055512,
+                    "y": 3.8656622432711547,
+                    "heading": -0.48297034285023976,
+                    "angularVelocity": 0.0000020759595452668008,
+                    "velocityX": 3.331869856379567,
+                    "velocityY": -1.791443800149147,
+                    "timestamp": 1.631230200206213
+                },
+                {
+                    "x": 6.414977737835259,
+                    "y": 3.8267449442462484,
+                    "heading": -0.4829702977916256,
+                    "angularVelocity": 0.0000020759499429522823,
+                    "velocityX": 3.3310294481622025,
+                    "velocityY": -1.7930059807042389,
+                    "timestamp": 1.6529352583285923
+                },
+                {
+                    "x": 6.487268603977522,
+                    "y": 3.787810332568674,
+                    "heading": -0.4829702527330872,
+                    "angularVelocity": 0.00000207594645249869,
+                    "velocityX": 3.330599979721225,
+                    "velocityY": -1.793803612876013,
+                    "timestamp": 1.6746403164509718
+                },
+                {
+                    "x": 6.559543297837492,
+                    "y": 3.7488457083598847,
+                    "heading": -0.4829702076720725,
+                    "angularVelocity": 0.000002076060537305802,
+                    "velocityX": 3.3298548869330626,
+                    "velocityY": -1.7951863565208734,
+                    "timestamp": 1.6963453745733512
+                },
+                {
+                    "x": 6.630628961599716,
+                    "y": 3.7077518923991635,
+                    "heading": -0.48297016252441444,
+                    "angularVelocity": 0.000002080052392839484,
+                    "velocityX": 3.2750736423474676,
+                    "velocityY": -1.893282926450661,
+                    "timestamp": 1.7180504326957307
+                },
+                {
+                    "x": 6.699799614538253,
+                    "y": 3.6635105764693066,
+                    "heading": -0.4829701170038858,
+                    "angularVelocity": 0.0000020972313632639956,
+                    "velocityX": 3.186844861162469,
+                    "velocityY": -2.0382952066016227,
+                    "timestamp": 1.73975549081811
+                },
+                {
+                    "x": 6.766806665010948,
+                    "y": 3.616056089090901,
+                    "heading": -0.48297007073382403,
+                    "angularVelocity": 0.000002131764013783056,
+                    "velocityX": 3.0871629135886995,
+                    "velocityY": -2.1863331169555953,
+                    "timestamp": 1.7614605489404895
+                },
+                {
+                    "x": 6.831474779993965,
+                    "y": 3.565460555976799,
+                    "heading": -0.48297002329923955,
+                    "angularVelocity": 0.0000021854161466442962,
+                    "velocityX": 2.9794029860873867,
+                    "velocityY": -2.331048036306895,
+                    "timestamp": 1.783165607062869
                 },
                 {
                     "x": 6.893649101257324,
                     "y": 3.5118300914764404,
-                    "heading": 1.5377683531646926e-9,
-                    "angularVelocity": 1.4832457732424108e-15,
-                    "velocityX": 2.8653643967583453,
-                    "velocityY": -2.4698816386502185,
-                    "timestamp": 1.8001580964547599
-                },
-                {
-                    "x": 6.934219508494461,
-                    "y": 3.4745268434152994,
-                    "heading": 1.5377683787984002e-9,
-                    "angularVelocity": 1.7594754613333528e-15,
-                    "velocityX": 2.784717658151537,
-                    "velocityY": -2.56046267850036,
-                    "timestamp": 1.8147270446274342
-                },
-                {
-                    "x": 6.973574535692003,
-                    "y": 3.4359435289599864,
-                    "heading": 1.5377684019331704e-9,
-                    "angularVelocity": 1.5879505990145664e-15,
-                    "velocityX": 2.701295023573416,
-                    "velocityY": -2.648325328500865,
-                    "timestamp": 1.8292959928001085
-                },
-                {
-                    "x": 7.011676190859291,
-                    "y": 3.3961219990562412,
-                    "heading": 1.5377684253031837e-9,
-                    "angularVelocity": 1.6040974952296623e-15,
-                    "velocityX": 2.6152646516208327,
-                    "velocityY": -2.733315365788167,
-                    "timestamp": 1.8438649409727828
-                },
-                {
-                    "x": 7.048490476455465,
-                    "y": 3.3551073823105697,
-                    "heading": 1.5377684383832216e-9,
-                    "angularVelocity": 8.97802495662944e-16,
-                    "velocityX": 2.5269007178724983,
-                    "velocityY": -2.815207814562461,
-                    "timestamp": 1.858433889145457
-                },
-                {
-                    "x": 7.083991390306391,
-                    "y": 3.3129508222340074,
-                    "heading": 1.5377684557448877e-9,
-                    "angularVelocity": 1.191689740876308e-15,
-                    "velocityX": 2.4367520173841823,
-                    "velocityY": -2.893589816979968,
-                    "timestamp": 1.8730028373181313
-                },
-                {
-                    "x": 7.118172550776329,
-                    "y": 3.269717285105602,
-                    "heading": 1.5377684739509254e-9,
-                    "angularVelocity": 1.2496467044902449e-15,
-                    "velocityX": 2.346165286939037,
-                    "velocityY": -2.9675125902013777,
-                    "timestamp": 1.8875717854908056
-                },
-                {
-                    "x": 7.15108313208807,
-                    "y": 3.2255088683196913,
-                    "heading": 1.5377684882032233e-9,
-                    "angularVelocity": 9.782653941580851e-16,
-                    "velocityX": 2.2589538326090746,
-                    "velocityY": -3.0344274865927217,
-                    "timestamp": 1.90214073366348
-                },
-                {
-                    "x": 7.182924606349684,
-                    "y": 3.1805243042550315,
-                    "heading": 1.5377697666399164e-9,
-                    "angularVelocity": 8.775078931151136e-14,
-                    "velocityX": 2.185571249497722,
-                    "velocityY": -3.0877015644156707,
-                    "timestamp": 1.9167096818361542
-                },
-                {
-                    "x": 7.2139811373084415,
-                    "y": 3.1352968072431113,
-                    "heading": 1.2915519070501347e-9,
-                    "angularVelocity": -1.6900181039231953e-8,
-                    "velocityX": 2.131693420188762,
-                    "velocityY": -3.1043762717714163,
-                    "timestamp": 1.9312786300088285
-                },
-                {
-                    "x": 7.244040263059187,
-                    "y": 3.0915312465009275,
-                    "heading": 1.1211375399666565e-9,
-                    "angularVelocity": -1.169709485282711e-8,
-                    "velocityX": 2.063232389495776,
-                    "velocityY": -3.0040302308350935,
-                    "timestamp": 1.9458475781815028
-                },
-                {
-                    "x": 7.2730994599044765,
-                    "y": 3.0492259199621516,
-                    "heading": 9.847245150980228e-10,
-                    "angularVelocity": -9.363272118309568e-9,
-                    "velocityX": 1.9945981343934114,
-                    "velocityY": -2.903801018259091,
-                    "timestamp": 1.960416526354177
-                },
-                {
-                    "x": 7.301157878524098,
-                    "y": 3.0083802533354898,
-                    "heading": 8.698146081553592e-10,
-                    "angularVelocity": -7.887316693614981e-9,
-                    "velocityX": 1.9259055826862927,
-                    "velocityY": -2.803611224540764,
-                    "timestamp": 1.9749854745268514
-                },
-                {
-                    "x": 7.328215092540324,
-                    "y": 2.968993958016925,
-                    "heading": 7.704962597777701e-10,
-                    "angularVelocity": -6.8171255926008325e-9,
-                    "velocityX": 1.857183764780944,
-                    "velocityY": -2.7034412403524777,
-                    "timestamp": 1.9895544226995256
-                },
-                {
-                    "x": 7.354270845617658,
-                    "y": 2.9310668604072108,
-                    "heading": 6.833248169018587e-10,
-                    "angularVelocity": -5.983372499761897e-9,
-                    "velocityX": 1.7884443522287568,
-                    "velocityY": -2.6032831718663054,
-                    "timestamp": 2.0041233708722
-                },
-                {
-                    "x": 7.379324966656107,
-                    "y": 2.894598844592948,
-                    "heading": 6.060446454340774e-10,
-                    "angularVelocity": -5.304444180973637e-9,
-                    "velocityX": 1.719693195521089,
-                    "velocityY": -2.5031330595756027,
-                    "timestamp": 2.0186923190448742
-                },
-                {
-                    "x": 7.403377333338156,
-                    "y": 2.8595898276899736,
-                    "heading": 5.370639718880826e-10,
-                    "angularVelocity": -4.734773818100124e-9,
-                    "velocityX": 1.6509336430450512,
-                    "velocityY": -2.4029886363820543,
-                    "timestamp": 2.0332612672175485
-                },
-                {
-                    "x": 7.426427853868939,
-                    "y": 2.8260397474863512,
-                    "heading": 4.75199340983144e-10,
-                    "angularVelocity": -4.246334802521844e-9,
-                    "velocityX": 1.582167789848791,
-                    "velocityY": -2.3028484833621037,
-                    "timestamp": 2.047830215390223
-                },
-                {
-                    "x": 7.44847645681879,
-                    "y": 2.79394855556605,
-                    "heading": 4.195363288341646e-10,
-                    "angularVelocity": -3.820661051835768e-9,
-                    "velocityX": 1.5133970337819913,
-                    "velocityY": -2.202711653576578,
-                    "timestamp": 2.062399163562897
-                },
-                {
-                    "x": 7.469523085022814,
-                    "y": 2.7633162131780225,
-                    "heading": 3.6934715040819367e-10,
-                    "angularVelocity": -3.4449418365911004e-9,
-                    "velocityX": 1.4446223539663485,
-                    "velocityY": -2.1025774836294286,
-                    "timestamp": 2.0769681117355714
-                },
-                {
-                    "x": 7.489567691695811,
-                    "y": 2.7341426886048597,
-                    "heading": 3.2403879933557077e-10,
-                    "angularVelocity": -3.1099260658641154e-9,
-                    "velocityX": 1.3758444628549362,
-                    "velocityY": -2.002445490737794,
-                    "timestamp": 2.0915370599082457
-                },
-                {
-                    "x": 7.5086102378406,
-                    "y": 2.7064279554071735,
-                    "heading": 2.831187733993615e-10,
-                    "angularVelocity": -2.8087152215039624e-9,
-                    "velocityX": 1.3070638950107798,
-                    "velocityY": -1.9023153126229018,
-                    "timestamp": 2.10610600808092
-                },
-                {
-                    "x": 7.52665069045286,
-                    "y": 2.680171991207375,
-                    "heading": 2.4617151569169827e-10,
-                    "angularVelocity": -2.536027844020545e-9,
-                    "velocityX": 1.2382810617787536,
-                    "velocityY": -1.8021866704863527,
-                    "timestamp": 2.1206749562535943
-                },
-                {
-                    "x": 7.543689021238326,
-                    "y": 2.65537477682047,
-                    "heading": 2.128416865029733e-10,
-                    "angularVelocity": -2.287730676987115e-9,
-                    "velocityX": 1.169496286452695,
-                    "velocityY": -1.7020593451912265,
-                    "timestamp": 2.1352439044262685
-                },
-                {
-                    "x": 7.559725205671708,
-                    "y": 2.6320362956163437,
-                    "heading": 1.8282195651887185e-10,
-                    "angularVelocity": -2.0605283283789865e-9,
-                    "velocityX": 1.1007098277319451,
-                    "velocityY": -1.6019331613726957,
-                    "timestamp": 2.149812852598943
-                },
-                {
-                    "x": 7.5747592222906635,
-                    "y": 2.6101565330412897,
-                    "heading": 1.558438913168689e-10,
-                    "angularVelocity": -1.8517510864510685e-9,
-                    "velocityX": 1.0319218958546759,
-                    "velocityY": -1.5018079765081174,
-                    "timestamp": 2.164381800771617
-                },
-                {
-                    "x": 7.588791052155751,
-                    "y": 2.5897354762519833,
-                    "heading": 1.3167100643601026e-10,
-                    "angularVelocity": -1.6592059334641093e-9,
-                    "velocityX": 0.9631326639903722,
-                    "velocityY": -1.4016836731981617,
-                    "timestamp": 2.1789507489442914
-                },
-                {
-                    "x": 7.601820678430282,
-                    "y": 2.570773113830717,
-                    "heading": 1.100933830131512e-10,
-                    "angularVelocity": -1.4810694297523211e-9,
-                    "velocityX": 0.894342276470416,
-                    "velocityY": -1.3015601535897774,
-                    "timestamp": 2.1935196971169657
-                },
-                {
-                    "x": 7.613848086048565,
-                    "y": 2.5532694355605217,
-                    "heading": 9.092342750630814e-11,
-                    "angularVelocity": -1.315809184573053e-9,
-                    "velocityX": 0.8255508548544661,
-                    "velocityY": -1.201437335265309,
-                    "timestamp": 2.20808864528964
-                },
-                {
-                    "x": 7.624873261450447,
-                    "y": 2.537224432245223,
-                    "heading": 7.399249133993465e-11,
-                    "angularVelocity": -1.1621248318093622e-9,
-                    "velocityX": 0.7567585024813989,
-                    "velocityY": -1.101315148158345,
-                    "timestamp": 2.2226575934623143
-                },
-                {
-                    "x": 7.634896192366388,
-                    "y": 2.5226380955637384,
-                    "heading": 5.914813867727886e-11,
-                    "angularVelocity": -1.0189035430874983e-9,
-                    "velocityX": 0.6879653079374735,
-                    "velocityY": -1.0011935322032168,
-                    "timestamp": 2.2372265416349886
-                },
-                {
-                    "x": 7.643916867641572,
-                    "y": 2.5095104179508487,
-                    "heading": 4.6251914688462445e-11,
-                    "angularVelocity": -8.851856741832805e-10,
-                    "velocityX": 0.6191713477368228,
-                    "velocityY": -0.9010724355182365,
-                    "timestamp": 2.251795489807663
-                },
-                {
-                    "x": 7.651935277090606,
-                    "y": 2.4978413924986826,
-                    "heading": 3.517750680608747e-11,
-                    "angularVelocity": -7.601377906979457e-10,
-                    "velocityX": 0.5503766884196106,
-                    "velocityY": -0.8009518129835539,
-                    "timestamp": 2.266364437980337
-                },
-                {
-                    "x": 7.658951411376412,
-                    "y": 2.487631012874611,
-                    "heading": 2.580921448386634e-11,
-                    "angularVelocity": -6.43031492677376e-10,
-                    "velocityX": 0.48158138821334623,
-                    "velocityY": -0.7008316251150604,
-                    "timestamp": 2.2809333861530114
-                },
-                {
-                    "x": 7.664965261908482,
-                    "y": 2.4788792732522666,
-                    "heading": 1.804066569533592e-11,
-                    "angularVelocity": -5.332264785860361e-10,
-                    "velocityX": 0.41278549836213546,
-                    "velocityY": -0.6007118371633381,
-                    "timestamp": 2.2955023343256857
-                },
-                {
-                    "x": 7.669976820756786,
-                    "y": 2.4715861682531717,
-                    "heading": 1.1773732592627752e-11,
-                    "angularVelocity": -4.3015687513290903e-10,
-                    "velocityX": 0.34398906420045333,
-                    "velocityY": -0.500592418385764,
-                    "timestamp": 2.31007128249836
-                },
-                {
-                    "x": 7.67398608057842,
-                    "y": 2.465751692897001,
-                    "heading": 6.917607828867898e-12,
-                    "angularVelocity": -3.3332020945886676e-10,
-                    "velocityX": 0.27519212602817883,
-                    "velocityY": -0.40047334145331454,
-                    "timestamp": 2.3246402306710343
-                },
-                {
-                    "x": 7.676993034554729,
-                    "y": 2.461375842558954,
-                    "heading": 3.3880167119093778e-12,
-                    "angularVelocity": -2.4226808579143967e-10,
-                    "velocityX": 0.20639471982945568,
-                    "velocityY": -0.30035458196322756,
-                    "timestamp": 2.3392091788437086
-                },
+                    "heading": -0.48296997476148124,
+                    "angularVelocity": 0.00000223624180421772,
+                    "velocityX": 2.8645083976649595,
+                    "velocityY": -2.470874033046681,
+                    "timestamp": 1.8048706651852484
+                },
+                {
+                    "x": 6.934561334359113,
+                    "y": 3.474160479757131,
+                    "heading": -0.4829699264500687,
+                    "angularVelocity": 0.0000032862628672599958,
+                    "velocityX": 2.782952215912443,
+                    "velocityY": -2.5623810156238895,
+                    "timestamp": 1.8195716840772946
+                },
+                {
+                    "x": 6.974231870495054,
+                    "y": 3.435185370851295,
+                    "heading": -0.4829698798888691,
+                    "angularVelocity": 0.0000031672090196575476,
+                    "velocityX": 2.6984888889168737,
+                    "velocityY": -2.6511841928806597,
+                    "timestamp": 1.8342727029693409
+                },
+                {
+                    "x": 7.012622614846585,
+                    "y": 3.394949051793438,
+                    "heading": -0.482969834780297,
+                    "angularVelocity": 0.0000030683976682796836,
+                    "velocityX": 2.611434257274787,
+                    "velocityY": -2.736974855506367,
+                    "timestamp": 1.848973721861387
+                },
+                {
+                    "x": 7.049699862488227,
+                    "y": 3.3534992220248374,
+                    "heading": -0.4829697908553284,
+                    "angularVelocity": 0.0000029878860039287864,
+                    "velocityX": 2.5220869324714577,
+                    "velocityY": -2.8195208830745364,
+                    "timestamp": 1.8636747407534333
+                },
+                {
+                    "x": 7.085438739774718,
+                    "y": 3.310890028476691,
+                    "heading": -0.48296974786836877,
+                    "angularVelocity": 0.0000029240802913465185,
+                    "velocityX": 2.4310476402306533,
+                    "velocityY": -2.8983837012274143,
+                    "timestamp": 1.8783757596454795
+                },
+                {
+                    "x": 7.119836677140904,
+                    "y": 3.2671911422114968,
+                    "heading": -0.4829697055890564,
+                    "angularVelocity": 0.0000028759443619428334,
+                    "velocityX": 2.339833559754191,
+                    "velocityY": -2.9725073198044742,
+                    "timestamp": 1.8930767785375258
+                },
+                {
+                    "x": 7.1529563800127525,
+                    "y": 3.222515689615774,
+                    "heading": -0.48296966398118896,
+                    "angularVelocity": 0.000002830270996857658,
+                    "velocityX": 2.2528848588700137,
+                    "velocityY": -3.0389357991976094,
+                    "timestamp": 1.907777797429572
+                },
+                {
+                    "x": 7.184894346745796,
+                    "y": 3.177381185858669,
+                    "heading": -0.4821430167851757,
+                    "angularVelocity": 0.056230605652804695,
+                    "velocityX": 2.17250021699707,
+                    "velocityY": -3.0701616050246474,
+                    "timestamp": 1.9224788163216182
+                },
+                {
+                    "x": 7.2158791345376,
+                    "y": 3.1333943550888805,
+                    "heading": -0.47750834116458535,
+                    "angularVelocity": 0.31526220424747137,
+                    "velocityX": 2.107662606199986,
+                    "velocityY": -2.992094023740475,
+                    "timestamp": 1.9371798352136644
+                },
+                {
+                    "x": 7.245895346179036,
+                    "y": 3.090550131820991,
+                    "heading": -0.46908945700490695,
+                    "angularVelocity": 0.5726735147747635,
+                    "velocityX": 2.041777638805421,
+                    "velocityY": -2.9143710094182476,
+                    "timestamp": 1.9518808541057107
+                },
+                {
+                    "x": 7.274933319615603,
+                    "y": 3.048846636162261,
+                    "heading": -0.45692051225089847,
+                    "angularVelocity": 0.827761996863506,
+                    "velocityX": 1.9752354343465999,
+                    "velocityY": -2.8367758700924766,
+                    "timestamp": 1.9665818729977569
+                },
+                {
+                    "x": 7.3029845213490745,
+                    "y": 3.008282422680816,
+                    "heading": -0.4410407652781448,
+                    "angularVelocity": 1.08018002625281,
+                    "velocityX": 1.90811276003785,
+                    "velocityY": -2.7592790526506894,
+                    "timestamp": 1.981282891889803
+                },
+                {
+                    "x": 7.330040747407425,
+                    "y": 2.9688555797343485,
+                    "heading": -0.42149121219843927,
+                    "angularVelocity": 1.3298093977882255,
+                    "velocityX": 1.8404320310743014,
+                    "velocityY": -2.6819122698903146,
+                    "timestamp": 1.9959839107818493
+                },
+                {
+                    "x": 7.356112777651364,
+                    "y": 2.9307983654229495,
+                    "heading": -0.3996724931243264,
+                    "angularVelocity": 1.4841637327544344,
+                    "velocityX": 1.7734845751435842,
+                    "velocityY": -2.5887467114262686,
+                    "timestamp": 2.0106849296738956
+                },
+                {
+                    "x": 7.38118332940277,
+                    "y": 2.8941879380599844,
+                    "heading": -0.37646864699164706,
+                    "angularVelocity": 1.578383532670194,
+                    "velocityX": 1.7053615083080305,
+                    "velocityY": -2.4903326518935414,
+                    "timestamp": 2.025385948565942
+                },
+                {
+                    "x": 7.40523963577506,
+                    "y": 2.8590504496296347,
+                    "heading": -0.3524751392807844,
+                    "angularVelocity": 1.6320982842790503,
+                    "velocityX": 1.6363700059799433,
+                    "velocityY": -2.390139669119207,
+                    "timestamp": 2.040086967457988
+                },
+                {
+                    "x": 7.428276340738969,
+                    "y": 2.825395406477394,
+                    "heading": -0.3281233875567818,
+                    "angularVelocity": 1.656466936259613,
+                    "velocityX": 1.5670141731720877,
+                    "velocityY": -2.289300040995877,
+                    "timestamp": 2.0547879863500342
+                },
+                {
+                    "x": 7.450292281624529,
+                    "y": 2.793225377698084,
+                    "heading": -0.3037369697425905,
+                    "angularVelocity": 1.658825010243686,
+                    "velocityX": 1.497579252650967,
+                    "velocityY": -2.1882856566299744,
+                    "timestamp": 2.0694890052420805
+                },
+                {
+                    "x": 7.471288319158757,
+                    "y": 2.7625397684205146,
+                    "heading": -0.27956516086770095,
+                    "angularVelocity": 1.6442267745106667,
+                    "velocityX": 1.4282028809299567,
+                    "velocityY": -2.0873117368873455,
+                    "timestamp": 2.0841900241341267
+                },
+                {
+                    "x": 7.491266209557209,
+                    "y": 2.733336527235843,
+                    "heading": -0.2558043926700512,
+                    "angularVelocity": 1.6162667616531794,
+                    "velocityX": 1.3589459713748013,
+                    "velocityY": -1.9864773590946931,
+                    "timestamp": 2.098891043026173
+                },
+                {
+                    "x": 7.510228057377649,
+                    "y": 2.7056129838220437,
+                    "heading": -0.2326125564520332,
+                    "angularVelocity": 1.577566588297197,
+                    "velocityX": 1.28983221909181,
+                    "velocityY": -1.885824623271541,
+                    "timestamp": 2.113592061918219
+                },
+                {
+                    "x": 7.528176060430696,
+                    "y": 2.679366275566135,
+                    "heading": -0.21011884554685348,
+                    "angularVelocity": 1.5300783619392286,
+                    "velocityX": 1.2208679673730278,
+                    "velocityY": -1.7853666095287006,
+                    "timestamp": 2.1282930808102654
+                },
+                {
+                    "x": 7.545112396890263,
+                    "y": 2.654593565371254,
+                    "heading": -0.18843070859149175,
+                    "angularVelocity": 1.4752812110931899,
+                    "velocityX": 1.152051880480808,
+                    "velocityY": -1.6851015821961506,
+                    "timestamp": 2.1429940997023116
+                },
+                {
+                    "x": 7.561039180049459,
+                    "y": 2.631292148556631,
+                    "heading": -0.1676388680105614,
+                    "angularVelocity": 1.4143128944742427,
+                    "velocityX": 1.0833795450607198,
+                    "velocityY": -1.5850205340005987,
+                    "timestamp": 2.157695118594358
+                },
+                {
+                    "x": 7.575958444025175,
+                    "y": 2.6094594995019675,
+                    "heading": -0.14782100448684538,
+                    "angularVelocity": 1.3480605439149753,
+                    "velocityX": 1.0148455753489192,
+                    "velocityY": -1.485111284801969,
+                    "timestamp": 2.172396137486404
+                },
+                {
+                    "x": 7.589872142500143,
+                    "y": 2.5890932855015207,
+                    "heading": -0.12904449811203259,
+                    "angularVelocity": 1.2772248313327248,
+                    "velocityX": 0.9464445000131985,
+                    "velocityY": -1.3853607120705473,
+                    "timestamp": 2.1870971563784503
+                },
+                {
+                    "x": 7.602782151943804,
+                    "y": 2.570191363211945,
+                    "heading": -0.11136848807718236,
+                    "angularVelocity": 1.2023663233582889,
+                    "velocityX": 0.8781710668125251,
+                    "velocityY": -1.2857559349035097,
+                    "timestamp": 2.2017981752704965
+                },
+                {
+                    "x": 7.614690275426409,
+                    "y": 2.552751766479238,
+                    "heading": -0.09484543137092857,
+                    "angularVelocity": 1.1239395600799826,
+                    "velocityX": 0.8100202829510676,
+                    "velocityY": -1.186284900439311,
+                    "timestamp": 2.2164991941625427
+                },
+                {
+                    "x": 7.62559824546286,
+                    "y": 2.536772690588083,
+                    "heading": -0.07952228805027751,
+                    "angularVelocity": 1.042318456507909,
+                    "velocityX": 0.7419873490775946,
+                    "velocityY": -1.086936627215761,
+                    "timestamp": 2.231200213054589
+                },
+                {
+                    "x": 7.635507725447758,
+                    "y": 2.522252475791086,
+                    "heading": -0.06544142530276546,
+                    "angularVelocity": 0.9578154310876013,
+                    "velocityX": 0.6740675634570735,
+                    "velocityY": -0.9877012541528096,
+                    "timestamp": 2.245901231946635
+                },
+                {
+                    "x": 7.644420309753795,
+                    "y": 2.509189591670685,
+                    "heading": -0.05264130826624494,
+                    "angularVelocity": 0.8706959109783889,
+                    "velocityX": 0.6062562310465265,
+                    "velocityY": -0.8885699839124228,
+                    "timestamp": 2.2606022508386814
+                },
+                {
+                    "x": 7.652337522761704,
+                    "y": 2.497582623097395,
+                    "heading": -0.04115702850586921,
+                    "angularVelocity": 0.7811893750159932,
+                    "velocityX": 0.538548590818527,
+                    "velocityY": -0.7895349743118973,
+                    "timestamp": 2.2753032697307276
+                },
+                {
+                    "x": 7.6592608171290975,
+                    "y": 2.487430258069545,
+                    "heading": -0.031020708755951136,
+                    "angularVelocity": 0.6894977704846257,
+                    "velocityX": 0.4709397639872257,
+                    "velocityY": -0.6905892103399419,
+                    "timestamp": 2.290004288622774
+                },
+                {
+                    "x": 7.665191571570965,
+                    "y": 2.4787312774326344,
+                    "heading": -0.022261813501037925,
+                    "angularVelocity": 0.5958019181685587,
+                    "velocityX": 0.40342472079099084,
+                    "velocityY": -0.5917263763001924,
+                    "timestamp": 2.30470530751482
+                },
+                {
+                    "x": 7.670131088361328,
+                    "y": 2.4714845463094046,
+                    "heading": -0.014907388223763604,
+                    "angularVelocity": 0.5002663646159557,
+                    "velocityX": 0.33599826152420176,
+                    "velocityY": -0.49294073944433947,
+                    "timestamp": 2.3194063264068663
+                },
+                {
+                    "x": 7.674080590697013,
+                    "y": 2.465689006980458,
+                    "heading": -0.008982245050778671,
+                    "angularVelocity": 0.40304302827545885,
+                    "velocityX": 0.26865500715889973,
+                    "velocityY": -0.3942270513020354,
+                    "timestamp": 2.3341073452989125
+                },
+                {
+                    "x": 7.677041220003002,
+                    "y": 2.461343672911727,
+                    "heading": -0.0045091086563273375,
+                    "angularVelocity": 0.304273902870198,
+                    "velocityX": 0.20138939536970257,
+                    "velocityY": -0.29558046966947404,
+                    "timestamp": 2.3488083641909587
+                },
                 {
-                    "x": 7.678997676337105,
-                    "y": 2.4584586129329957,
-                    "heading": 1.1065347655527578e-12,
-                    "angularVelocity": -1.56598950365953e-10,
-                    "velocityX": 0.13759687786767488,
-                    "velocityY": -0.2002361180356331,
-                    "timestamp": 2.353778127016383
+                    "x": 7.679014033209882,
+                    "y": 2.458447623611078,
+                    "heading": -0.00150873334272897,
+                    "angularVelocity": 0.20409301801671295,
+                    "velocityX": 0.1341956786374654,
+                    "velocityY": -0.1969965022095263,
+                    "timestamp": 2.363509383083005
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": -3.441813048880529e-34,
-                    "angularVelocity": -7.595159066564751e-11,
-                    "velocityX": 0.06879862918134139,
-                    "velocityY": -0.10011792997737116,
-                    "timestamp": 2.368347075189057
+                    "heading": -1.1328442579469643e-34,
+                    "angularVelocity": 0.10262780789604178,
+                    "velocityX": 0.06706792211866815,
+                    "velocityY": -0.0984709714141921,
+                    "timestamp": 2.378210401975051
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": -3.481182114535467e-34,
-                    "angularVelocity": -2.7023024138211624e-34,
-                    "velocityX": -3.7878173595367446e-32,
-                    "velocityY": -1.5253102288533056e-31,
-                    "timestamp": 2.3829160233617315
+                    "heading": -2.155134012002005e-33,
+                    "angularVelocity": -1.3889170145055665e-31,
+                    "velocityX": 1.4892305241424507e-32,
+                    "velocityY": 6.576507695252416e-33,
+                    "timestamp": 2.3929114208670974
                 }
             ],
             "constraints": [
@@ -6146,190 +6222,190 @@
                     "x": 0.71,
                     "y": 4.36,
                     "heading": -1.04,
-                    "angularVelocity": -9.2124979355083e-31,
-                    "velocityX": -2.0292335982741827e-31,
-                    "velocityY": 8.51805082704341e-32,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7269596629981327,
-                    "y": 4.384958058102961,
-                    "heading": -1.0015712045971708,
-                    "angularVelocity": 0.6206072078812727,
-                    "velocityX": 0.27389068508516395,
-                    "velocityY": 0.40306105333388875,
-                    "timestamp": 0.06192128437248337
-                },
-                {
-                    "x": 0.7608459539039729,
-                    "y": 4.435118328610172,
-                    "heading": -0.9264796986002697,
-                    "angularVelocity": 1.212692965882186,
-                    "velocityX": 0.5472478687941834,
-                    "velocityY": 0.8100650853021081,
-                    "timestamp": 0.12384256874496674
-                },
-                {
-                    "x": 0.8116291734017863,
-                    "y": 4.510832058036491,
-                    "heading": -0.8178101616266298,
-                    "angularVelocity": 1.754962579909447,
-                    "velocityX": 0.8201254223399239,
-                    "velocityY": 1.2227415854436723,
-                    "timestamp": 0.1857638531174501
-                },
-                {
-                    "x": 0.8793445718640878,
-                    "y": 4.61260707850531,
-                    "heading": -0.6817312766556918,
-                    "angularVelocity": 2.1976108271973906,
-                    "velocityX": 1.0935722530392615,
-                    "velocityY": 1.6436193386525904,
-                    "timestamp": 0.24768513748993348
-                },
-                {
-                    "x": 0.9640980850122779,
-                    "y": 4.741096013070982,
-                    "heading": -0.5303097082973756,
-                    "angularVelocity": 2.445388042138302,
-                    "velocityX": 1.368729896465987,
-                    "velocityY": 2.0750366512547602,
-                    "timestamp": 0.3096064218624168
-                },
-                {
-                    "x": 1.0654131930810256,
-                    "y": 4.896663967052664,
-                    "heading": -0.3925803189619664,
-                    "angularVelocity": 2.2242657065526474,
-                    "velocityX": 1.6361919668735143,
-                    "velocityY": 2.512350245286791,
-                    "timestamp": 0.37152770623490017
-                },
-                {
-                    "x": 1.1790960248711007,
-                    "y": 5.075186526688609,
-                    "heading": -0.32893475345024337,
-                    "angularVelocity": 1.0278463400220732,
-                    "velocityX": 1.8359249641241828,
-                    "velocityY": 2.8830564715365803,
-                    "timestamp": 0.4334489906073835
-                },
-                {
-                    "x": 1.3095059591702463,
-                    "y": 5.269772606119823,
-                    "heading": -0.32893468266066545,
-                    "angularVelocity": 0.0000011432188240302224,
-                    "velocityX": 2.106059905260899,
-                    "velocityY": 3.1424748598671592,
-                    "timestamp": 0.49537027497986685
-                },
-                {
-                    "x": 1.439916398287199,
-                    "y": 5.464358347226451,
-                    "heading": -0.3289346118734761,
-                    "angularVelocity": 0.0000011431802504043458,
-                    "velocityX": 2.106068057834158,
-                    "velocityY": 3.14246939608213,
-                    "timestamp": 0.5572915593523502
-                },
-                {
-                    "x": 1.5703268751378339,
-                    "y": 5.65894406304414,
-                    "heading": -0.32893454108629183,
-                    "angularVelocity": 0.0000011431801680911917,
-                    "velocityX": 2.1060686672156135,
-                    "velocityY": 3.142468987677516,
-                    "timestamp": 0.6192128437248335
-                },
-                {
-                    "x": 1.700737390180202,
-                    "y": 5.853529753265897,
-                    "heading": -0.3289344702991123,
-                    "angularVelocity": 0.0000011431800901048107,
-                    "velocityX": 2.1060692839943727,
-                    "velocityY": 3.1424685743150773,
-                    "timestamp": 0.6811341280973169
-                },
-                {
-                    "x": 1.8311473976991637,
-                    "y": 6.048115783625596,
-                    "heading": -0.32893439950904224,
-                    "angularVelocity": 0.000001143226773133006,
-                    "velocityX": 2.1060610877269452,
-                    "velocityY": 3.1424740673849674,
-                    "timestamp": 0.7430554124698002
-                },
-                {
-                    "x": 1.9446375646824816,
-                    "y": 6.226114798697167,
-                    "heading": -0.26580646349628556,
-                    "angularVelocity": 1.0194868638869756,
-                    "velocityX": 1.8328135169261883,
-                    "velocityY": 2.8746014698408153,
-                    "timestamp": 0.8049766968422836
-                },
-                {
-                    "x": 2.0457805429701614,
-                    "y": 6.3800476604210195,
-                    "heading": -0.13750031114094655,
-                    "angularVelocity": 2.072084803401717,
-                    "velocityX": 1.6334121508084516,
-                    "velocityY": 2.485944264299801,
-                    "timestamp": 0.8668979812147669
-                },
-                {
-                    "x": 2.130677229255867,
-                    "y": 6.507493779703991,
-                    "heading": 0.017637122988449796,
-                    "angularVelocity": 2.505397549511044,
-                    "velocityX": 1.3710420761787727,
-                    "velocityY": 2.0581956684930476,
-                    "timestamp": 0.9288192655872503
-                },
-                {
-                    "x": 2.198423808533576,
-                    "y": 6.608249828696692,
-                    "heading": 0.1586985563433061,
-                    "angularVelocity": 2.2780766707988582,
-                    "velocityX": 1.0940758087345794,
-                    "velocityY": 1.6271634222993538,
-                    "timestamp": 0.9907405499597336
-                },
-                {
-                    "x": 2.2492018276993044,
-                    "y": 6.683037485807007,
-                    "heading": 0.27235601520130115,
-                    "angularVelocity": 1.8355152030486992,
-                    "velocityX": 0.8200414393906406,
-                    "velocityY": 1.2077859474043515,
-                    "timestamp": 1.052661834332217
-                },
-                {
-                    "x": 2.2830642005186585,
-                    "y": 6.732456177807481,
-                    "heading": 0.3519224237627513,
-                    "angularVelocity": 1.2849605651398268,
-                    "velocityX": 0.5468616027997278,
-                    "velocityY": 0.7980889366441329,
-                    "timestamp": 1.1145831187047004
+                    "x": 0.7269596910839872,
+                    "y": 4.384958038291164,
+                    "heading": -1.001571192673364,
+                    "angularVelocity": 0.6206074004453302,
+                    "velocityX": 0.27389113865872694,
+                    "velocityY": 0.40306073338263126,
+                    "timestamp": 0.06192128437247234
+                },
+                {
+                    "x": 0.760846036232504,
+                    "y": 4.4351182711466235,
+                    "heading": -0.9264796681168371,
+                    "angularVelocity": 1.2126932656117428,
+                    "velocityX": 0.5472487447883301,
+                    "velocityY": 0.8100644772439347,
+                    "timestamp": 0.12384256874494468
+                },
+                {
+                    "x": 0.8116293344011996,
+                    "y": 4.510831947021006,
+                    "heading": -0.817810113065978,
+                    "angularVelocity": 1.7549628718484593,
+                    "velocityX": 0.8201266928382989,
+                    "velocityY": 1.2227407206049843,
+                    "timestamp": 0.185763853117417
+                },
+                {
+                    "x": 0.8793448344180163,
+                    "y": 4.612606899775036,
+                    "heading": -0.6817312187907495,
+                    "angularVelocity": 2.197610977457753,
+                    "velocityX": 1.0935738930977383,
+                    "velocityY": 1.6436182450904597,
+                    "timestamp": 0.24768513748988935
+                },
+                {
+                    "x": 0.9640984700059907,
+                    "y": 4.74109575406918,
+                    "heading": -0.5303096586797065,
+                    "angularVelocity": 2.4453879089491037,
+                    "velocityX": 1.3687318738119136,
+                    "velocityY": 2.075035354907217,
+                    "timestamp": 0.3096064218623617
+                },
+                {
+                    "x": 1.0654137163163762,
+                    "y": 4.896663616218439,
+                    "heading": -0.3925803264267699,
+                    "angularVelocity": 2.224264784697608,
+                    "velocityX": 1.6361941994120859,
+                    "velocityY": 2.5123487622362273,
+                    "timestamp": 0.37152770623483405
+                },
+                {
+                    "x": 1.179096696888173,
+                    "y": 5.075186082347611,
+                    "heading": -0.3289347906925715,
+                    "angularVelocity": 1.0278458591290565,
+                    "velocityX": 1.8359273668802587,
+                    "velocityY": 2.88305496144612,
+                    "timestamp": 0.4334489906073064
+                },
+                {
+                    "x": 1.3095067112863388,
+                    "y": 5.269772108097061,
+                    "heading": -0.32893471990297235,
+                    "angularVelocity": 0.0000011432191669124052,
+                    "velocityX": 2.1060611988232667,
+                    "velocityY": 3.142473992932148,
+                    "timestamp": 0.49537027497977876
+                },
+                {
+                    "x": 1.4399171936865056,
+                    "y": 5.464357820195416,
+                    "heading": -0.32893464911575754,
+                    "angularVelocity": 0.0000011431806608935866,
+                    "velocityX": 2.106068756838354,
+                    "velocityY": 3.1424689276125553,
+                    "timestamp": 0.557291559352251
+                },
+                {
+                    "x": 1.570327676092705,
+                    "y": 5.6589435322897295,
+                    "heading": -0.3289345783285442,
+                    "angularVelocity": 0.0000011431806365404781,
+                    "velocityX": 2.106068756935774,
+                    "velocityY": 3.1424689275472675,
+                    "timestamp": 0.6192128437247234
+                },
+                {
+                    "x": 1.7007381584918697,
+                    "y": 5.853529244388756,
+                    "heading": -0.32893450754133247,
+                    "angularVelocity": 0.000001143180612539941,
+                    "velocityX": 2.1060687568221708,
+                    "velocityY": 3.1424689276234057,
+                    "timestamp": 0.6811341280971958
+                },
+                {
+                    "x": 1.8311480951447636,
+                    "y": 6.048115322242248,
+                    "heading": -0.3289344367512267,
+                    "angularVelocity": 0.0000011432273482693713,
+                    "velocityX": 2.1060599432732174,
+                    "velocityY": 3.1424748343882203,
+                    "timestamp": 0.7430554124696681
+                },
+                {
+                    "x": 1.9446381116074218,
+                    "y": 6.226114434087632,
+                    "heading": -0.2658064774598036,
+                    "angularVelocity": 1.0194872398268149,
+                    "velocityX": 1.8328110860877296,
+                    "velocityY": 2.8746030326934724,
+                    "timestamp": 0.8049766968421405
+                },
+                {
+                    "x": 2.0457809471284243,
+                    "y": 6.380047389358405,
+                    "heading": -0.1375002927378408,
+                    "angularVelocity": 2.0720853261080365,
+                    "velocityX": 1.6334098451931771,
+                    "velocityY": 2.485945775039604,
+                    "timestamp": 0.8668979812146128
+                },
+                {
+                    "x": 2.1306775060879315,
+                    "y": 6.50749359169558,
+                    "heading": 0.01763716177237307,
+                    "angularVelocity": 2.5053978786522335,
+                    "velocityX": 1.3710400199200075,
+                    "velocityY": 2.0581970097802293,
+                    "timestamp": 0.9288192655870852
+                },
+                {
+                    "x": 2.198423978678441,
+                    "y": 6.608249711273454,
+                    "heading": 0.15869859646801654,
+                    "angularVelocity": 2.278076692452355,
+                    "velocityX": 1.0940740857860296,
+                    "velocityY": 1.6271645622174213,
+                    "timestamp": 0.9907405499595575
+                },
+                {
+                    "x": 2.24920191473134,
+                    "y": 6.683037424747583,
+                    "heading": 0.27235604329703667,
+                    "angularVelocity": 1.8355150087866652,
+                    "velocityX": 0.8200400971571714,
+                    "velocityY": 1.2077868576540292,
+                    "timestamp": 1.0526618343320298
+                },
+                {
+                    "x": 2.2830642302273354,
+                    "y": 6.732456156660043,
+                    "heading": 0.35192243545091534,
+                    "angularVelocity": 1.2849603001653922,
+                    "velocityX": 0.5468606770541943,
+                    "velocityY": 0.7980895812043076,
+                    "timestamp": 1.114583118704502
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.393,
-                    "angularVelocity": 0.6633837888463243,
-                    "velocityX": 0.27350530036594517,
-                    "velocityY": 0.39637133566025207,
-                    "timestamp": 1.1765044030771838
+                    "angularVelocity": 0.6633836000880191,
+                    "velocityX": 0.2735048205846554,
+                    "velocityY": 0.3963716771816175,
+                    "timestamp": 1.1765044030769745
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.393,
-                    "angularVelocity": 4.204601117767945e-31,
-                    "velocityX": 0,
-                    "velocityY": -1.1377088902550092e-31,
-                    "timestamp": 1.2384256874496673
+                    "angularVelocity": 0,
+                    "velocityX": 2.8187230537991962e-33,
+                    "velocityY": 0,
+                    "timestamp": 1.2384256874494468
                 }
             ],
             "constraints": [
@@ -6349,23 +6425,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "Stage Side To W2": {
             "waypoints": [
@@ -6393,145 +6453,145 @@
                     "x": 0.71,
                     "y": 4.36,
                     "heading": -1.04,
-                    "angularVelocity": 1.681543952112751e-30,
-                    "velocityX": -3.802997916124736e-32,
-                    "velocityY": 9.397294493952762e-34,
+                    "angularVelocity": 2.3377521162047767e-32,
+                    "velocityX": 0,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7374413965489649,
-                    "y": 4.381188295193423,
-                    "heading": -1.0071421527847422,
-                    "angularVelocity": 0.5013988353179083,
-                    "velocityX": 0.41874576197914515,
-                    "velocityY": 0.3233257024648049,
-                    "timestamp": 0.06553235648109366
-                },
-                {
-                    "x": 0.7924436098605977,
-                    "y": 4.423719284711624,
-                    "heading": -0.9443396170050365,
-                    "angularVelocity": 0.9583439258410564,
-                    "velocityX": 0.8393138331215114,
-                    "velocityY": 0.6490074796939781,
-                    "timestamp": 0.13106471296218733
-                },
-                {
-                    "x": 0.8751753442261428,
-                    "y": 4.487775925735343,
-                    "heading": -0.8558629917892655,
-                    "angularVelocity": 1.350121222045466,
-                    "velocityX": 1.2624562705815747,
-                    "velocityY": 0.9774811171669098,
-                    "timestamp": 0.19659706944328098
-                },
-                {
-                    "x": 0.9858650513706745,
-                    "y": 4.5735667081068,
-                    "heading": -0.748264583846062,
-                    "angularVelocity": 1.641912693529484,
-                    "velocityX": 1.6890847985371011,
-                    "velocityY": 1.3091362340405182,
-                    "timestamp": 0.26212942592437466
-                },
-                {
-                    "x": 1.1247485645977424,
-                    "y": 4.681320324271238,
-                    "heading": -0.6327125157113742,
-                    "angularVelocity": 1.763282664312989,
-                    "velocityX": 2.1193120571993105,
-                    "velocityY": 1.6442811147120544,
-                    "timestamp": 0.32766178240546834
-                },
-                {
-                    "x": 1.291488910381981,
-                    "y": 4.810947401273882,
-                    "heading": -0.5360470717928266,
-                    "angularVelocity": 1.4750796264503891,
-                    "velocityX": 2.544397222040073,
-                    "velocityY": 1.9780621964975005,
-                    "timestamp": 0.393194138886562
-                },
-                {
-                    "x": 1.4799946113507723,
-                    "y": 4.960741770751019,
-                    "heading": -0.5171262553925533,
-                    "angularVelocity": 0.2887247981954086,
-                    "velocityX": 2.8765286507463124,
-                    "velocityY": 2.2858077676538255,
-                    "timestamp": 0.4587264953676557
-                },
-                {
-                    "x": 1.6682616674549375,
-                    "y": 5.11061398659558,
-                    "heading": -0.49798039258281773,
-                    "angularVelocity": 0.29215892480933153,
-                    "velocityX": 2.8728870166371596,
-                    "velocityY": 2.2869956749959233,
-                    "timestamp": 0.5242588518487493
-                },
-                {
-                    "x": 1.8349961429002288,
-                    "y": 5.239757672124722,
-                    "heading": -0.4043635195675931,
-                    "angularVelocity": 1.4285595397783832,
-                    "velocityX": 2.5443076427962366,
-                    "velocityY": 1.970685817873752,
-                    "timestamp": 0.589791208329843
-                },
-                {
-                    "x": 1.9739512346286359,
-                    "y": 5.34717518731749,
-                    "heading": -0.2886497176580553,
-                    "angularVelocity": 1.7657506630777153,
-                    "velocityX": 2.120404319177778,
-                    "velocityY": 1.6391523357435742,
-                    "timestamp": 0.6553235648109367
-                },
-                {
-                    "x": 2.084688896220359,
-                    "y": 5.432668536845291,
-                    "heading": -0.1811622358281448,
-                    "angularVelocity": 1.6402200012587402,
-                    "velocityX": 1.6898165660145774,
-                    "velocityY": 1.304597516685743,
-                    "timestamp": 0.7208559212920304
-                },
-                {
-                    "x": 2.167475787005346,
-                    "y": 5.496492971314002,
-                    "heading": -0.09361417394502371,
-                    "angularVelocity": 1.3359516822560165,
-                    "velocityX": 1.2632979375443678,
-                    "velocityY": 0.97393772932798,
-                    "timestamp": 0.786388277773124
-                },
-                {
-                    "x": 2.222528989324329,
-                    "y": 5.538875463156123,
-                    "heading": -0.0320000732344696,
-                    "angularVelocity": 0.9402088375736564,
-                    "velocityX": 0.8400919068867495,
-                    "velocityY": 0.6467414589974029,
-                    "timestamp": 0.8519206342542177
+                    "x": 0.737441410859622,
+                    "y": 4.3811882786622816,
+                    "heading": -1.0071421705955907,
+                    "angularVelocity": 0.501398563530153,
+                    "velocityX": 0.41874598035403643,
+                    "velocityY": 0.32332545020514236,
+                    "timestamp": 0.06553235648117978
+                },
+                {
+                    "x": 0.7924436512054522,
+                    "y": 4.423719236654154,
+                    "heading": -0.9443396668116766,
+                    "angularVelocity": 0.9583434375956188,
+                    "velocityX": 0.8393142456524665,
+                    "velocityY": 0.6490069986127681,
+                    "timestamp": 0.13106471296235955
+                },
+                {
+                    "x": 0.8751754235694627,
+                    "y": 4.487775832809834,
+                    "heading": -0.8558630826468019,
+                    "angularVelocity": 1.350120595621899,
+                    "velocityX": 1.2624568504227318,
+                    "velocityY": 0.9774804324956131,
+                    "timestamp": 0.19659706944353933
+                },
+                {
+                    "x": 0.9858651777004729,
+                    "y": 4.5735665588886505,
+                    "heading": -0.748264717894899,
+                    "angularVelocity": 1.641912034443748,
+                    "velocityX": 1.6890855155315267,
+                    "velocityY": 1.3091353750334698,
+                    "timestamp": 0.2621294259247191
+                },
+                {
+                    "x": 1.1247487447497322,
+                    "y": 4.6813201098081265,
+                    "heading": -0.6327126866941822,
+                    "angularVelocity": 1.7632821007116781,
+                    "velocityX": 2.1193128785036808,
+                    "velocityY": 1.644280119095407,
+                    "timestamp": 0.3276617824058989
+                },
+                {
+                    "x": 1.291489149115039,
+                    "y": 4.810947116825512,
+                    "heading": -0.5360472598717524,
+                    "angularVelocity": 1.4750793655678647,
+                    "velocityX": 2.5443981159626317,
+                    "velocityY": 1.9780611285451524,
+                    "timestamp": 0.39319413888707866
+                },
+                {
+                    "x": 1.4799949106081978,
+                    "y": 4.9607414161097925,
+                    "heading": -0.5171264547525753,
+                    "angularVelocity": 0.288724626049585,
+                    "velocityX": 2.876529574322522,
+                    "velocityY": 2.285806696533185,
+                    "timestamp": 0.45872649536825844
+                },
+                {
+                    "x": 1.6682619137790426,
+                    "y": 5.11061369240764,
+                    "heading": -0.49798057816746544,
+                    "angularVelocity": 0.29215913501614493,
+                    "velocityX": 2.8728862088900566,
+                    "velocityY": 2.2869965974882214,
+                    "timestamp": 0.5242588518494382
+                },
+                {
+                    "x": 1.8349963336194852,
+                    "y": 5.239757443274396,
+                    "heading": -0.40436368242247644,
+                    "angularVelocity": 1.4285598866244509,
+                    "velocityX": 2.54430679428301,
+                    "velocityY": 1.970686814899508,
+                    "timestamp": 0.589791208330618
+                },
+                {
+                    "x": 1.9739513715778099,
+                    "y": 5.347175023297594,
+                    "heading": -0.28864983484418905,
+                    "angularVelocity": 1.7657513599639834,
+                    "velocityX": 2.120403498662996,
+                    "velocityY": 1.6391533250303247,
+                    "timestamp": 0.6553235648117979
+                },
+                {
+                    "x": 2.0846889840994356,
+                    "y": 5.432668431655269,
+                    "heading": -0.18116230899252247,
+                    "angularVelocity": 1.6402206730126385,
+                    "velocityX": 1.6898158172204303,
+                    "velocityY": 1.304598414406612,
+                    "timestamp": 0.7208559212929777
+                },
+                {
+                    "x": 2.167475833751633,
+                    "y": 5.496492915332372,
+                    "heading": -0.09361421148490373,
+                    "angularVelocity": 1.3359522258711838,
+                    "velocityX": 1.2632973098712375,
+                    "velocityY": 0.973938480228949,
+                    "timestamp": 0.7863882777741575
+                },
+                {
+                    "x": 2.2225290058379836,
+                    "y": 5.538875443360028,
+                    "heading": -0.032000086006940494,
+                    "angularVelocity": 0.9402092155141033,
+                    "velocityX": 0.8400914455466154,
+                    "velocityY": 0.6467420111747162,
+                    "timestamp": 0.8519206342553374
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 0.48830951537200207,
-                    "velocityX": 0.41919766281556964,
-                    "velocityY": 0.32235277316741867,
-                    "timestamp": 0.9174529907353114
+                    "heading": 2.4308347188047162e-30,
+                    "angularVelocity": 0.48830971027463277,
+                    "velocityX": 0.41919741082263007,
+                    "velocityY": 0.3223530752482305,
+                    "timestamp": 0.9174529907365172
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 1.4283065738497147e-31,
-                    "velocityY": -2.9738366407041548e-31,
-                    "timestamp": 0.9829853472164051
+                    "heading": 1.7485008040489027e-29,
+                    "angularVelocity": 1.1123032826758857e-30,
+                    "velocityX": 0,
+                    "velocityY": 1.931278949788811e-31,
+                    "timestamp": 0.982985347217697
                 }
             ],
             "constraints": [
@@ -6551,23 +6611,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "Stage Side To W3": {
             "waypoints": [
@@ -6595,118 +6639,118 @@
                     "x": 0.71,
                     "y": 4.36,
                     "heading": -1.04,
-                    "angularVelocity": -7.700576656700109e-31,
+                    "angularVelocity": -2.204776300650663e-32,
                     "velocityX": 0,
-                    "velocityY": -1.136023856217491e-30,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7541176222366596,
-                    "y": 4.360036691224542,
-                    "heading": -1.0162545462543013,
-                    "angularVelocity": 0.3246127495796489,
-                    "velocityX": 0.6031109286246202,
-                    "velocityY": 0.0005015881949855901,
-                    "timestamp": 0.07315009584931663
-                },
-                {
-                    "x": 0.8424088840100864,
-                    "y": 4.360100335716613,
-                    "heading": -0.970147925779941,
-                    "angularVelocity": 0.630301572936589,
-                    "velocityX": 1.2069876429868645,
-                    "velocityY": 0.0008700534337042256,
-                    "timestamp": 0.14630019169863326
-                },
-                {
-                    "x": 0.9749552173528473,
-                    "y": 4.360178948568722,
-                    "heading": -0.9038422068087609,
-                    "angularVelocity": 0.9064337948053328,
-                    "velocityX": 1.8119775757478125,
-                    "velocityY": 0.0010746787299806282,
-                    "timestamp": 0.21945028754794987
-                },
-                {
-                    "x": 1.151884255871623,
-                    "y": 4.36025970565234,
-                    "heading": -0.8212723355896727,
-                    "angularVelocity": 1.1287732471196155,
-                    "velocityX": 2.4187123265462183,
-                    "velocityY": 0.0011039914941527397,
-                    "timestamp": 0.2926003833972665
-                },
-                {
-                    "x": 1.3733948183653495,
-                    "y": 4.360324587725535,
-                    "heading": -0.7323747736104,
-                    "angularVelocity": 1.2152760833319434,
-                    "velocityX": 3.028165034124097,
-                    "velocityY": 0.000886971813693119,
-                    "timestamp": 0.36575047924658316
-                },
-                {
-                    "x": 1.6366160137298507,
-                    "y": 4.3597898231256424,
-                    "heading": -0.6992108014826405,
-                    "angularVelocity": 0.45336881302434434,
-                    "velocityX": 3.598371161490932,
-                    "velocityY": -0.007310511266700298,
-                    "timestamp": 0.4389005750958998
-                },
-                {
-                    "x": 1.858123270171759,
-                    "y": 4.359896131611675,
-                    "heading": -0.6098312657316373,
-                    "angularVelocity": 1.2218649164225759,
-                    "velocityX": 3.02811983866963,
-                    "velocityY": 0.001453292504923468,
-                    "timestamp": 0.5120506709452164
-                },
-                {
-                    "x": 2.0350502388104363,
-                    "y": 4.359956556106789,
-                    "heading": -0.5268889356205209,
-                    "angularVelocity": 1.1338649546283055,
-                    "velocityX": 2.4186840302045134,
-                    "velocityY": 0.000826034394207285,
-                    "timestamp": 0.585200766794533
-                },
-                {
-                    "x": 2.167593784990145,
-                    "y": 4.359986293011168,
-                    "heading": -0.460224121535325,
-                    "angularVelocity": 0.9113428124904321,
-                    "velocityX": 1.8119394737736823,
-                    "velocityY": 0.0004065190076972891,
-                    "timestamp": 0.6583508626438497
-                },
-                {
-                    "x": 2.2558829179021025,
-                    "y": 4.359997710834851,
-                    "heading": -0.41385943895752736,
-                    "angularVelocity": 0.6338294166193973,
-                    "velocityX": 1.206958540339091,
-                    "velocityY": 0.00015608761065604354,
-                    "timestamp": 0.7315009584931663
+                    "x": 0.7541176231747116,
+                    "y": 4.360036687345491,
+                    "heading": -1.0162545684059647,
+                    "angularVelocity": 0.32461244675544165,
+                    "velocityX": 0.6031109414489596,
+                    "velocityY": 0.0005015351663374791,
+                    "timestamp": 0.07315009584923313
+                },
+                {
+                    "x": 0.8424088865359771,
+                    "y": 4.360100324606402,
+                    "heading": -0.9701479863262087,
+                    "angularVelocity": 0.630301048063541,
+                    "velocityX": 1.2069876646948012,
+                    "velocityY": 0.0008699545799604022,
+                    "timestamp": 0.14630019169846625
+                },
+                {
+                    "x": 0.974955221752511,
+                    "y": 4.3601789275184695,
+                    "heading": -0.9038423136459668,
+                    "angularVelocity": 0.9064331619859869,
+                    "velocityX": 1.8119776013652948,
+                    "velocityY": 0.0010745428444254071,
+                    "timestamp": 0.21945028754769938
+                },
+                {
+                    "x": 1.151884261990258,
+                    "y": 4.360259672825357,
+                    "heading": -0.8212724833638784,
+                    "angularVelocity": 1.128772687492223,
+                    "velocityX": 2.4187123500481573,
+                    "velocityY": 0.0011038305000959348,
+                    "timestamp": 0.2926003833969325
+                },
+                {
+                    "x": 1.3733948258568207,
+                    "y": 4.360324542882412,
+                    "heading": -0.7323749312717828,
+                    "angularVelocity": 1.215275948172809,
+                    "velocityX": 3.028165052894887,
+                    "velocityY": 0.0008868075468144582,
+                    "timestamp": 0.36575047924616566
+                },
+                {
+                    "x": 1.6366160230983884,
+                    "y": 4.359789775146019,
+                    "heading": -0.6992109641912426,
+                    "angularVelocity": 0.4533687440268879,
+                    "velocityX": 3.5983711871554758,
+                    "velocityY": -0.007310554144235059,
+                    "timestamp": 0.4389005750953988
+                },
+                {
+                    "x": 1.8581232786849116,
+                    "y": 4.3598960912826,
+                    "heading": -0.6098314600860953,
+                    "angularVelocity": 1.2218644838062247,
+                    "velocityX": 3.0281198269795637,
+                    "velocityY": 0.001453397091854858,
+                    "timestamp": 0.512050670944632
+                },
+                {
+                    "x": 2.035050245808528,
+                    "y": 4.359956526837273,
+                    "heading": -0.5268891037054299,
+                    "angularVelocity": 1.1338653137464598,
+                    "velocityX": 2.4186840094956525,
+                    "velocityY": 0.000826185584123381,
+                    "timestamp": 0.5852007667938651
+                },
+                {
+                    "x": 2.1675937895751547,
+                    "y": 4.359986275638159,
+                    "heading": -0.46022423203591045,
+                    "angularVelocity": 0.9113435996982586,
+                    "velocityX": 1.8119394407877087,
+                    "velocityY": 0.00040668163915013244,
+                    "timestamp": 0.6583508626430983
+                },
+                {
+                    "x": 2.255882919836075,
+                    "y": 4.359997704013322,
+                    "heading": -0.41385948514831483,
+                    "angularVelocity": 0.6338302957681125,
+                    "velocityX": 1.2069585040994306,
+                    "velocityY": 0.0001562318549097459,
+                    "timestamp": 0.7315009584923314
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
                     "heading": -0.39,
-                    "angularVelocity": 0.32617098693462643,
-                    "velocityX": 0.6031035446457226,
-                    "velocityY": 0.00003129408268678862,
-                    "timestamp": 0.804651054342483
+                    "angularVelocity": 0.32617161838698483,
+                    "velocityX": 0.6031035182080025,
+                    "velocityY": 0.00003138733655167869,
+                    "timestamp": 0.8046510543415646
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
                     "heading": -0.39,
-                    "angularVelocity": 3.953163438305433e-30,
-                    "velocityX": 7.397518409454568e-32,
+                    "angularVelocity": -2.2755735462869925e-32,
+                    "velocityX": -9.780003655586715e-35,
                     "velocityY": 0,
-                    "timestamp": 0.8778011501917996
+                    "timestamp": 0.8778011501907977
                 }
             ],
             "constraints": [
@@ -6726,23 +6770,7 @@
             "usesControlIntervalGuessing": true,
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
+            "circleObstacles": []
         },
         "Stage Side To C1": {
             "waypoints": [
@@ -9133,7 +9161,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 31
+                    "controlIntervalCount": 19
+                },
+                {
+                    "x": 3.4563703536987305,
+                    "y": 2.4171059131622314,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 17
                 },
                 {
                     "x": 7.68,
@@ -9151,288 +9188,333 @@
                     "y": 4.36,
                     "heading": -1.04,
                     "angularVelocity": 0,
-                    "velocityX": -3.7161803829578067e-31,
-                    "velocityY": -1.7109886686245282e-31,
+                    "velocityX": 0,
+                    "velocityY": 1.4815405069278332e-31,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.755738680236623,
-                    "y": 4.330590345332975,
-                    "heading": -0.988927755482241,
-                    "angularVelocity": 0.6221231913092291,
-                    "velocityX": 0.5571537727343439,
-                    "velocityY": -0.35824601776392057,
-                    "timestamp": 0.08209345870916634
-                },
-                {
-                    "x": 0.8475293838257123,
-                    "y": 4.27179448486036,
-                    "heading": -0.8901763080939228,
-                    "angularVelocity": 1.2029149330663034,
-                    "velocityX": 1.1181244526957685,
-                    "velocityY": -0.7162063993540811,
-                    "timestamp": 0.1641869174183327
-                },
-                {
-                    "x": 0.9859043696516769,
-                    "y": 4.183626202129818,
-                    "heading": -0.7509222083756354,
-                    "angularVelocity": 1.6962874010659592,
-                    "velocityX": 1.6855787050730526,
-                    "velocityY": -1.0739988802627505,
-                    "timestamp": 0.24628037612749903
-                },
-                {
-                    "x": 1.1717528122715652,
-                    "y": 4.066081969426692,
-                    "heading": -0.5886701024002182,
-                    "angularVelocity": 1.9764316003572233,
-                    "velocityX": 2.263864204799754,
-                    "velocityY": -1.4318343331050452,
-                    "timestamp": 0.3283738348366654
-                },
-                {
-                    "x": 1.404824377392605,
-                    "y": 3.921350877318508,
-                    "heading": -0.4767670307510258,
-                    "angularVelocity": 1.3631180048782332,
-                    "velocityX": 2.8391003228009395,
-                    "velocityY": -1.7630039516415732,
-                    "timestamp": 0.4104672935458317
-                },
-                {
-                    "x": 1.6657642530728491,
-                    "y": 3.752962692607369,
-                    "heading": -0.4767669793872945,
-                    "angularVelocity": 6.256738607298855e-7,
-                    "velocityX": 3.1785708603736573,
-                    "velocityY": -2.051176638904815,
-                    "timestamp": 0.49256075225499807
-                },
-                {
-                    "x": 1.926704086710966,
-                    "y": 3.584574442746028,
-                    "heading": -0.476766928024127,
-                    "angularVelocity": 6.256669936190949e-7,
-                    "velocityX": 3.178570348248477,
-                    "velocityY": -2.051177432514957,
-                    "timestamp": 0.5746542109641644
-                },
-                {
-                    "x": 2.187643960670788,
-                    "y": 3.416186255368488,
-                    "heading": -0.47676687666098916,
-                    "angularVelocity": 6.256666327585045e-7,
-                    "velocityX": 3.178570839416784,
-                    "velocityY": -2.0511766713848885,
-                    "timestamp": 0.6567476696733308
-                },
-                {
-                    "x": 2.448583988107283,
-                    "y": 3.2477983058236712,
-                    "heading": -0.4767668252978414,
-                    "angularVelocity": 6.256667527167073e-7,
-                    "velocityX": 3.1785727089527405,
-                    "velocityY": -2.051173774287752,
-                    "timestamp": 0.7388411283824972
-                },
-                {
-                    "x": 2.7194719503347105,
-                    "y": 3.095930240616485,
-                    "heading": -0.47676675593417933,
-                    "angularVelocity": 8.449353114908744e-7,
-                    "velocityX": 3.2997508752445928,
-                    "velocityY": -1.8499411231437015,
-                    "timestamp": 0.8209345870916636
-                },
-                {
-                    "x": 2.9903602222167747,
-                    "y": 2.9440627277435274,
-                    "heading": -0.4767666865702561,
-                    "angularVelocity": 8.449384925343815e-7,
-                    "velocityX": 3.2997546472215804,
-                    "velocityY": -1.8499343950287315,
-                    "timestamp": 0.90302804580083
-                },
-                {
-                    "x": 3.2612486310597473,
-                    "y": 2.7921954591699243,
-                    "heading": -0.4767666172064318,
-                    "angularVelocity": 8.44937286881618e-7,
-                    "velocityX": 3.299756315575062,
-                    "velocityY": -1.8499314191600278,
-                    "timestamp": 0.9851215045099964
-                },
-                {
-                    "x": 3.5321371803805834,
-                    "y": 2.6403284411694066,
-                    "heading": -0.4767665478428475,
-                    "angularVelocity": 8.449343642301634e-7,
-                    "velocityX": 3.2997580267694278,
-                    "velocityY": -1.84992836686951,
-                    "timestamp": 1.0672149632191628
-                },
-                {
-                    "x": 3.8030257968728702,
-                    "y": 2.488461542984137,
-                    "heading": -0.47676647847929754,
-                    "angularVelocity": 8.449339449505073e-7,
-                    "velocityX": 3.2997588450008877,
-                    "velocityY": -1.849926907371401,
-                    "timestamp": 1.1493084219283292
-                },
-                {
-                    "x": 4.07391446115854,
-                    "y": 2.336594730049097,
-                    "heading": -0.4767664091157508,
-                    "angularVelocity": 8.449339066268147e-7,
-                    "velocityX": 3.2997594271834845,
-                    "velocityY": -1.8499258689180111,
-                    "timestamp": 1.2314018806374956
-                },
-                {
-                    "x": 4.344803172855204,
-                    "y": 2.184728001682272,
-                    "heading": -0.4767663397522045,
-                    "angularVelocity": 8.449339005791607e-7,
-                    "velocityX": 3.2997600047081037,
-                    "velocityY": -1.8499248387724243,
-                    "timestamp": 1.313495339346662
-                },
-                {
-                    "x": 4.6156919675948584,
-                    "y": 2.0328614214415324,
-                    "heading": -0.47676627038863556,
-                    "angularVelocity": 8.449341769958558e-7,
-                    "velocityX": 3.2997610162745823,
-                    "velocityY": -1.8499230344132471,
-                    "timestamp": 1.3955887980558284
-                },
-                {
-                    "x": 4.88903699417146,
-                    "y": 1.8854616483024196,
-                    "heading": -0.4767661960128138,
-                    "angularVelocity": 9.059896238747082e-7,
-                    "velocityX": 3.3296809620969254,
-                    "velocityY": -1.7955117917653842,
-                    "timestamp": 1.4776822567649948
-                },
-                {
-                    "x": 5.164941930672226,
-                    "y": 1.7429110878363532,
-                    "heading": -0.47676611821696785,
-                    "angularVelocity": 9.476497536578291e-7,
-                    "velocityX": 3.3608638354271223,
-                    "velocityY": -1.736442375647519,
-                    "timestamp": 1.5597757154741612
-                },
-                {
-                    "x": 5.4408826630316725,
-                    "y": 1.6004298311654348,
-                    "heading": -0.4767660404421832,
-                    "angularVelocity": 9.473932014254074e-7,
-                    "velocityX": 3.3612998733190906,
-                    "velocityY": -1.7355981695897218,
-                    "timestamp": 1.6418691741833276
-                },
-                {
-                    "x": 5.73313461063715,
-                    "y": 1.4953917422787115,
-                    "heading": -0.4767660015440003,
-                    "angularVelocity": 4.738280431565268e-7,
-                    "velocityX": 3.55999067649034,
-                    "velocityY": -1.2794940125356749,
-                    "timestamp": 1.723962632892494
-                },
-                {
-                    "x": 6.025386917456382,
-                    "y": 1.3903546528530497,
-                    "heading": -0.476765962645991,
-                    "angularVelocity": 4.738259283996548e-7,
-                    "velocityX": 3.5599950521587655,
-                    "velocityY": -1.2794818378621156,
-                    "timestamp": 1.8060560916016604
-                },
-                {
-                    "x": 6.31763925599377,
-                    "y": 1.2853176516791878,
-                    "heading": -0.4767659237479627,
-                    "angularVelocity": 4.7382616041897537e-7,
-                    "velocityX": 3.5599954385251906,
-                    "velocityY": -1.2794807628458942,
-                    "timestamp": 1.8881495503108268
-                },
-                {
-                    "x": 6.609891614856598,
-                    "y": 1.1802807070583716,
-                    "heading": -0.4767658848499098,
-                    "angularVelocity": 4.7382646005084667e-7,
-                    "velocityX": 3.5599956861142137,
-                    "velocityY": -1.279480073959752,
-                    "timestamp": 1.9702430090199932
-                },
-                {
-                    "x": 6.9021439611938415,
-                    "y": 1.0752437275870097,
-                    "heading": -0.4767658459515333,
-                    "angularVelocity": 4.738304011500936e-7,
-                    "velocityX": 3.559995533537077,
-                    "velocityY": -1.2794804984825638,
-                    "timestamp": 2.0523364677291593
-                },
-                {
-                    "x": 7.1615588383732955,
-                    "y": 0.9774297585720934,
-                    "heading": -0.3677133060860067,
-                    "angularVelocity": 1.3283949973635365,
-                    "velocityX": 3.159994489920166,
-                    "velocityY": -1.1914952854093204,
-                    "timestamp": 2.1344299264383255
-                },
-                {
-                    "x": 7.369316955835238,
-                    "y": 0.9006256752646921,
-                    "heading": -0.23381405774557934,
-                    "angularVelocity": 1.6310586792888595,
-                    "velocityX": 2.5307511795546396,
-                    "velocityY": -0.9355688567038716,
-                    "timestamp": 2.2165233851474917
-                },
-                {
-                    "x": 7.5248080438305305,
-                    "y": 0.8435895899389048,
-                    "heading": -0.12113401657570753,
-                    "angularVelocity": 1.3725824559185034,
-                    "velocityX": 1.894074027824225,
-                    "velocityY": -0.6947701585804769,
-                    "timestamp": 2.298616843856658
-                },
+                    "x": 0.7317173879428727,
+                    "y": 4.344479531295635,
+                    "heading": -1.013582317544266,
+                    "angularVelocity": 0.4585462516411977,
+                    "velocityX": 0.37696065327942924,
+                    "velocityY": -0.26939731598432753,
+                    "timestamp": 0.05761181638969176
+                },
+                {
+                    "x": 0.7752730705207751,
+                    "y": 4.313424298338961,
+                    "heading": -0.9621480921974644,
+                    "angularVelocity": 0.892772152832256,
+                    "velocityX": 0.7560199505477795,
+                    "velocityY": -0.5390427676609577,
+                    "timestamp": 0.11522363277938352
+                },
+                {
+                    "x": 0.8408250898493527,
+                    "y": 4.266812487548848,
+                    "heading": -0.8877439224939486,
+                    "angularVelocity": 1.2914741170498636,
+                    "velocityX": 1.1378224717856051,
+                    "velocityY": -0.8090668496689294,
+                    "timestamp": 0.17283544916907528
+                },
+                {
+                    "x": 0.9285847241631874,
+                    "y": 4.204600540366139,
+                    "heading": -0.7936992773737622,
+                    "angularVelocity": 1.6323846567179892,
+                    "velocityX": 1.5232922656043415,
+                    "velocityY": -1.0798470015578314,
+                    "timestamp": 0.23044726555876704
+                },
+                {
+                    "x": 1.0388213735909644,
+                    "y": 4.1267152282410935,
+                    "heading": -0.6856850144743787,
+                    "angularVelocity": 1.8748629997840256,
+                    "velocityX": 1.9134381857035365,
+                    "velocityY": -1.3518982216811457,
+                    "timestamp": 0.2880590819484588
+                },
+                {
+                    "x": 1.1718232312823271,
+                    "y": 4.033123229014291,
+                    "heading": -0.5741627817643435,
+                    "angularVelocity": 1.935752762171023,
+                    "velocityX": 2.3085864328894954,
+                    "velocityY": -1.6245278328622093,
+                    "timestamp": 0.34567089833815057
+                },
+                {
+                    "x": 1.3270687303513053,
+                    "y": 3.9246703032584405,
+                    "heading": -0.488682735559766,
+                    "angularVelocity": 1.4837242003685942,
+                    "velocityX": 2.6946815566251776,
+                    "velocityY": -1.882477112373353,
+                    "timestamp": 0.40328271472784233
+                },
+                {
+                    "x": 1.5010681185276094,
+                    "y": 3.804139759424889,
+                    "heading": -0.46956462349945394,
+                    "angularVelocity": 0.3318435914430377,
+                    "velocityX": 3.020203129846763,
+                    "velocityY": -2.0921149754812625,
+                    "timestamp": 0.4608945311175341
+                },
+                {
+                    "x": 1.6785494425409546,
+                    "y": 3.677652137140934,
+                    "heading": -0.4695645649556455,
+                    "angularVelocity": 0.0000010161770978802793,
+                    "velocityX": 3.080641006227699,
+                    "velocityY": -2.195515264236428,
+                    "timestamp": 0.5185063475072258
+                },
+                {
+                    "x": 1.85603075289391,
+                    "y": 3.551164495689284,
+                    "heading": -0.4695645064119953,
+                    "angularVelocity": 0.000001016174352223477,
+                    "velocityX": 3.080640769116793,
+                    "velocityY": -2.1955155969406546,
+                    "timestamp": 0.5761181638969175
+                },
+                {
+                    "x": 2.033512086622271,
+                    "y": 3.4246768870368545,
+                    "heading": -0.4695644478683731,
+                    "angularVelocity": 0.0000010161738659585955,
+                    "velocityX": 3.0806411748565776,
+                    "velocityY": -2.195515027626543,
+                    "timestamp": 0.6337299802866092
+                },
+                {
+                    "x": 2.2109934521039283,
+                    "y": 3.298189322939119,
+                    "heading": -0.46956438932476746,
+                    "angularVelocity": 0.0000010161735782211743,
+                    "velocityX": 3.080641726016013,
+                    "velocityY": -2.1955142542661994,
+                    "timestamp": 0.6913417966763009
+                },
+                {
+                    "x": 2.3884748618363862,
+                    "y": 3.17170182093203,
+                    "heading": -0.4695643307811488,
+                    "angularVelocity": 0.000001016173805422874,
+                    "velocityX": 3.0806424941015065,
+                    "velocityY": -2.1955131765246763,
+                    "timestamp": 0.7489536130659926
+                },
+                {
+                    "x": 2.5659563234551954,
+                    "y": 3.045214391729511,
+                    "heading": -0.4695642722374779,
+                    "angularVelocity": 0.0000010161747112988211,
+                    "velocityX": 3.0806433947214558,
+                    "velocityY": -2.1955119128156926,
+                    "timestamp": 0.8065654294556843
+                },
+                {
+                    "x": 2.743437831089148,
+                    "y": 2.918727027093402,
+                    "heading": -0.46956421369376844,
+                    "angularVelocity": 0.000001016175381373278,
+                    "velocityX": 3.08064419343162,
+                    "velocityY": -2.1955107921009867,
+                    "timestamp": 0.864177245845376
+                },
+                {
+                    "x": 2.920919379832659,
+                    "y": 2.7922397201404365,
+                    "heading": -0.4695641551500532,
+                    "angularVelocity": 0.0000010161754817816058,
+                    "velocityX": 3.080644906992851,
+                    "velocityY": -2.195509790862922,
+                    "timestamp": 0.9217890622350677
+                },
+                {
+                    "x": 3.0984009676669872,
+                    "y": 2.6657524680380327,
+                    "heading": -0.4695640966063568,
+                    "angularVelocity": 0.000001016175153333812,
+                    "velocityX": 3.080645585513673,
+                    "velocityY": -2.195508838791537,
+                    "timestamp": 0.9794008786247594
+                },
+                {
+                    "x": 3.275882647398041,
+                    "y": 2.5392653448813247,
+                    "heading": -0.4695640380626867,
+                    "angularVelocity": 0.0000010161746978103879,
+                    "velocityX": 3.080647180615714,
+                    "velocityY": -2.1955066006101345,
+                    "timestamp": 1.0370126950144511
+                },
+                {
+                    "x": 3.4563703536987305,
+                    "y": 2.417105913162231,
+                    "heading": -0.4695639788949644,
+                    "angularVelocity": 0.0000010270067149252654,
+                    "velocityX": 3.1328244379564545,
+                    "velocityY": -2.1203884788634437,
+                    "timestamp": 1.0946245114041429
+                },
+                {
+                    "x": 3.7578524899325245,
+                    "y": 2.288322056940197,
+                    "heading": -0.4695639274563231,
+                    "angularVelocity": 5.9355601779034e-7,
+                    "velocityX": 3.4788348155609183,
+                    "velocityY": -1.4860507766867785,
+                    "timestamp": 1.1812863255510104
+                },
+                {
+                    "x": 4.061243078218464,
+                    "y": 2.1641008385808727,
+                    "heading": -0.46956387627292245,
+                    "angularVelocity": 5.906107685245515e-7,
+                    "velocityX": 3.5008566491785738,
+                    "velocityY": -1.43340200735705,
+                    "timestamp": 1.267948139697878
+                },
+                {
+                    "x": 4.364633703659485,
+                    "y": 2.039879710966971,
+                    "heading": -0.46956382508952316,
+                    "angularVelocity": 5.906107531304183e-7,
+                    "velocityX": 3.5008570779150574,
+                    "velocityY": -1.433400960235855,
+                    "timestamp": 1.3546099538447456
+                },
+                {
+                    "x": 4.668024366282006,
+                    "y": 1.9156586741630988,
+                    "heading": -0.46956377390610504,
+                    "angularVelocity": 5.906109701114576e-7,
+                    "velocityX": 3.5008575069563967,
+                    "velocityY": -1.4333999123691519,
+                    "timestamp": 1.4412717679916132
+                },
+                {
+                    "x": 4.971415087165123,
+                    "y": 1.7914377796518506,
+                    "heading": -0.4695637227226054,
+                    "angularVelocity": 5.906119107964098e-7,
+                    "velocityX": 3.5008581792316793,
+                    "velocityY": -1.4333982704392505,
+                    "timestamp": 1.5279335821384807
+                },
+                {
+                    "x": 5.2748058711699946,
+                    "y": 1.6672170393060612,
+                    "heading": -0.4695636715390663,
+                    "angularVelocity": 5.906123657897232e-7,
+                    "velocityX": 3.5008589076003997,
+                    "velocityY": -1.4333964915074275,
+                    "timestamp": 1.6145953962853483
+                },
+                {
+                    "x": 5.5781967938554144,
+                    "y": 1.5429966376674569,
+                    "heading": -0.4695636203556594,
+                    "angularVelocity": 5.906108405690599e-7,
+                    "velocityX": 3.5008605078501667,
+                    "velocityY": -1.4333925831287677,
+                    "timestamp": 1.701257210432216
+                },
+                {
+                    "x": 5.886988948403778,
+                    "y": 1.4328868446307816,
+                    "heading": -0.4695635793301605,
+                    "angularVelocity": 4.733976467078746e-7,
+                    "velocityX": 3.5631859035981908,
+                    "velocityY": -1.2705687518849984,
+                    "timestamp": 1.7879190245790835
+                },
+                {
+                    "x": 6.195781169913944,
+                    "y": 1.3227772393821355,
+                    "heading": -0.4695635383046595,
+                    "angularVelocity": 4.7339767079441426e-7,
+                    "velocityX": 3.5631866762776085,
+                    "velocityY": -1.2705665849787193,
+                    "timestamp": 1.874580838725951
+                },
+                {
+                    "x": 6.5045734104803365,
+                    "y": 1.212667687574965,
+                    "heading": -0.46956349727913566,
+                    "angularVelocity": 4.733979354519185e-7,
+                    "velocityX": 3.5631868961694693,
+                    "velocityY": -1.270565968311784,
+                    "timestamp": 1.9612426528728186
+                },
+                {
+                    "x": 6.813365656566466,
+                    "y": 1.1025581512474965,
+                    "heading": -0.46956345625352064,
+                    "angularVelocity": 4.733989867793667e-7,
+                    "velocityX": 3.563186959862305,
+                    "velocityY": -1.270565789689833,
+                    "timestamp": 2.0479044670196864
+                },
+                {
+                    "x": 7.101065618447083,
+                    "y": 0.9964425714529646,
+                    "heading": -0.4012732100130825,
+                    "angularVelocity": 0.7880085007764233,
+                    "velocityX": 3.3198008224596522,
+                    "velocityY": -1.2244790954259896,
+                    "timestamp": 2.134566281166554
+                },
+                {
+                    "x": 7.333266238045498,
+                    "y": 0.911992130350644,
+                    "heading": -0.2613611242014092,
+                    "angularVelocity": 1.6144606155435655,
+                    "velocityX": 2.6793879390165927,
+                    "velocityY": -0.9744827284506232,
+                    "timestamp": 2.2212280953134216
+                },
+                {
+                    "x": 7.506890250099573,
+                    "y": 0.8492417427348896,
+                    "heading": -0.1371754084070899,
+                    "angularVelocity": 1.43299233943868,
+                    "velocityX": 2.0034661605379225,
+                    "velocityY": -0.7240834759056616,
+                    "timestamp": 2.307889909460289
+                },
                 {
-                    "x": 7.628310039444481,
-                    "y": 0.8058059657857352,
-                    "heading": -0.04143578092949631,
-                    "angularVelocity": 0.9708232166043739,
-                    "velocityX": 1.2607824940185441,
-                    "velocityY": -0.4602513372840855,
-                    "timestamp": 2.380710302565824
+                    "x": 7.622366585478705,
+                    "y": 0.8076799304753898,
+                    "heading": -0.04732610702177306,
+                    "angularVelocity": 1.0367807582823931,
+                    "velocityX": 1.332493861522807,
+                    "velocityY": -0.4795862245517285,
+                    "timestamp": 2.3945517236071567
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 6.308726444888931e-32,
-                    "angularVelocity": 0.5047391300236409,
-                    "velocityX": 0.6296477376917574,
-                    "velocityY": -0.22907995449868002,
-                    "timestamp": 2.4628037612749902
+                    "heading": 2.9489608951695904e-31,
+                    "angularVelocity": 0.5461010421679925,
+                    "velocityX": 0.6650381726791765,
+                    "velocityY": -0.2386279433331854,
+                    "timestamp": 2.4812135377540243
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
-                    "heading": 0,
-                    "angularVelocity": -2.9272372134325455e-30,
-                    "velocityX": -7.795387702917406e-31,
-                    "velocityY": 4.863651986550052e-31,
-                    "timestamp": 2.5448972199841564
+                    "heading": -7.162012659830636e-32,
+                    "angularVelocity": -4.0878892657023166e-30,
+                    "velocityX": 2.3668785600931567e-32,
+                    "velocityY": 6.81692291678628e-32,
+                    "timestamp": 2.567875351900892
                 }
             ],
             "constraints": [
@@ -9462,11 +9544,6 @@
                     "x": 5.61,
                     "y": 2.81,
                     "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
                 }
             ]
         },
@@ -11615,7 +11692,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 21
+                    "controlIntervalCount": 17
+                },
+                {
+                    "x": 6.113985538482666,
+                    "y": 7.0125651359558105,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 10
                 },
                 {
                     "x": 7.68,
@@ -11632,199 +11718,253 @@
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.39,
-                    "angularVelocity": 5.971193698047002e-30,
-                    "velocityX": 3.860312986446214e-32,
-                    "velocityY": -2.276118500298611e-31,
+                    "angularVelocity": 2.274971502954598e-31,
+                    "velocityX": 3.487335890979115e-33,
+                    "velocityY": 2.862188318424697e-32,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.3687225561485863,
-                    "y": 6.754047320431167,
-                    "heading": 0.3342984160631402,
-                    "angularVelocity": -0.6057639861694455,
-                    "velocityX": 0.7473692238179469,
-                    "velocityY": -0.03211088121883033,
-                    "timestamp": 0.09195261720507575
-                },
-                {
-                    "x": 2.5065025886783423,
-                    "y": 6.748008642736996,
-                    "heading": 0.22854226308747308,
-                    "angularVelocity": -1.1501157464589211,
-                    "velocityX": 1.498380760848541,
-                    "velocityY": -0.06567162390498478,
-                    "timestamp": 0.1839052344101515
-                },
-                {
-                    "x": 2.713881673600013,
-                    "y": 6.738717029343825,
-                    "heading": 0.08399021423974613,
-                    "angularVelocity": -1.5720275642110566,
-                    "velocityX": 2.2552820270375413,
-                    "velocityY": -0.10104784045947955,
-                    "timestamp": 0.27585785161522725
-                },
-                {
-                    "x": 2.991587720371644,
-                    "y": 6.725981822834066,
-                    "heading": -0.07328714089763647,
-                    "angularVelocity": -1.710417385800096,
-                    "velocityX": 3.0200994296038606,
-                    "velocityY": -0.13849748812866708,
-                    "timestamp": 0.367810468820303
-                },
-                {
-                    "x": 3.331126465665418,
-                    "y": 6.709290658495849,
-                    "heading": -0.09912948961150339,
-                    "angularVelocity": -0.2810398387707929,
-                    "velocityX": 3.692540306237542,
-                    "velocityY": -0.18151918722434224,
-                    "timestamp": 0.45976308602537874
-                },
-                {
-                    "x": 3.678639360618228,
-                    "y": 6.693951135251218,
-                    "heading": -0.09912953281628478,
-                    "angularVelocity": -4.6985918076152147e-7,
-                    "velocityX": 3.7792605095489114,
-                    "velocityY": -0.16681986560992548,
-                    "timestamp": 0.5517157032304545
-                },
-                {
-                    "x": 4.0261522529573766,
-                    "y": 6.678611552794569,
-                    "heading": -0.09912957602103574,
-                    "angularVelocity": -4.698588497287581e-7,
-                    "velocityX": 3.77926048112489,
-                    "velocityY": -0.16682050955045719,
-                    "timestamp": 0.6436683204355302
-                },
-                {
-                    "x": 4.373665142037621,
-                    "y": 6.663271896508641,
-                    "heading": -0.0991296192257673,
-                    "angularVelocity": -4.698586389884745e-7,
-                    "velocityX": 3.779260445683786,
-                    "velocityY": -0.1668213124561534,
-                    "timestamp": 0.735620937640606
-                },
-                {
-                    "x": 4.721178027308032,
-                    "y": 6.647932153912761,
-                    "heading": -0.09912966243048027,
-                    "angularVelocity": -4.6985843677462385e-7,
-                    "velocityX": 3.779260404251215,
-                    "velocityY": -0.1668222510912166,
-                    "timestamp": 0.8275735548456817
-                },
-                {
-                    "x": 5.068690907382649,
-                    "y": 6.632592293609503,
-                    "heading": -0.09912970563518403,
-                    "angularVelocity": -4.698583364707967e-7,
-                    "velocityX": 3.7792603477460833,
-                    "velocityY": -0.16682353117852486,
-                    "timestamp": 0.9195261720507575
-                },
-                {
-                    "x": 5.416203760083376,
-                    "y": 6.617251813184553,
-                    "heading": -0.09912974884008513,
-                    "angularVelocity": -4.698604826805718e-7,
-                    "velocityX": 3.779260050050473,
-                    "velocityY": -0.16683027510502133,
-                    "timestamp": 1.0114787892558332
-                },
-                {
-                    "x": 5.762351413678364,
-                    "y": 6.582867023594091,
-                    "heading": -0.09912980297819443,
-                    "angularVelocity": -5.887609395849737e-7,
-                    "velocityX": 3.7644132827997527,
-                    "velocityY": -0.3739403035563012,
-                    "timestamp": 1.103431406460909
-                },
-                {
-                    "x": 6.108490861918874,
-                    "y": 6.5483997316058735,
-                    "heading": -0.0991298571102857,
-                    "angularVelocity": -5.886954924717416e-7,
-                    "velocityX": 3.7643240482055917,
-                    "velocityY": -0.3748375308486051,
-                    "timestamp": 1.1953840236659847
-                },
-                {
-                    "x": 6.4430213978276925,
-                    "y": 6.453059099313297,
-                    "heading": -0.09912987433352956,
-                    "angularVelocity": -1.8730564053245824e-7,
-                    "velocityX": 3.6380751965192784,
-                    "velocityY": -1.0368452273625233,
-                    "timestamp": 1.2873366408710605
-                },
-                {
-                    "x": 6.751459095644277,
-                    "y": 6.29222781566083,
-                    "heading": -0.09912986944515287,
-                    "angularVelocity": 5.316190922150303e-8,
-                    "velocityX": 3.3543112441127847,
-                    "velocityY": -1.7490669492721016,
-                    "timestamp": 1.3792892580761362
-                },
-                {
-                    "x": 7.058385657911462,
-                    "y": 6.1285311430110365,
-                    "heading": -0.09912986456630993,
-                    "angularVelocity": 5.305822810376802e-8,
-                    "velocityX": 3.337877393774089,
-                    "velocityY": -1.7802285310130133,
-                    "timestamp": 1.471241875281212
-                },
-                {
-                    "x": 7.307031381410103,
-                    "y": 5.995918713976455,
-                    "heading": -0.06067697672181489,
-                    "angularVelocity": 0.4181815484244036,
-                    "velocityX": 2.7040635824873065,
-                    "velocityY": -1.442182213681043,
-                    "timestamp": 1.5631944924862877
-                },
-                {
-                    "x": 7.4935156893705,
-                    "y": 5.896459366852187,
-                    "heading": -0.03183830930275791,
-                    "angularVelocity": 0.3136253028528817,
-                    "velocityX": 2.028047853651562,
-                    "velocityY": -1.0816369359281015,
-                    "timestamp": 1.6551471096913635
-                },
+                    "x": 2.3417817158351957,
+                    "y": 6.761449131523058,
+                    "heading": 0.37953631669999827,
+                    "angularVelocity": -0.14722299547472617,
+                    "velocityX": 0.5878646347531417,
+                    "velocityY": 0.06259884318982863,
+                    "timestamp": 0.07107370194626751
+                },
+                {
+                    "x": 2.42534939307215,
+                    "y": 6.770350816518497,
+                    "heading": 0.3588464620969251,
+                    "angularVelocity": -0.2911042204995314,
+                    "velocityX": 1.1757890042104941,
+                    "velocityY": 0.12524583287024788,
+                    "timestamp": 0.14214740389253502
+                },
+                {
+                    "x": 2.5507092244237364,
+                    "y": 6.783710205716013,
+                    "heading": 0.3282884162089386,
+                    "angularVelocity": -0.42994870185747563,
+                    "velocityX": 1.7638005045292426,
+                    "velocityY": 0.18796529281121122,
+                    "timestamp": 0.21322110583880255
+                },
+                {
+                    "x": 2.717871083967313,
+                    "y": 6.80153596037414,
+                    "heading": 0.2884653946252666,
+                    "angularVelocity": -0.5603059991696991,
+                    "velocityX": 2.351950932145827,
+                    "velocityY": 0.25080661580833774,
+                    "timestamp": 0.28429480778507005
+                },
+                {
+                    "x": 2.926852904271386,
+                    "y": 6.82384561239607,
+                    "heading": 0.2406022074409234,
+                    "angularVelocity": -0.673430338841082,
+                    "velocityX": 2.9403536692385535,
+                    "velocityY": 0.31389461096009597,
+                    "timestamp": 0.35536850973133755
+                },
+                {
+                    "x": 3.1776924244019935,
+                    "y": 6.85069218229951,
+                    "heading": 0.1884744580400949,
+                    "angularVelocity": -0.7334323100298853,
+                    "velocityX": 3.529287391280776,
+                    "velocityY": 0.3777285995842137,
+                    "timestamp": 0.42644221167760504
+                },
+                {
+                    "x": 3.4450596021465354,
+                    "y": 6.879057072545224,
+                    "heading": 0.1884744481252109,
+                    "angularVelocity": -1.3950144320454384e-7,
+                    "velocityX": 3.7618299092775445,
+                    "velocityY": 0.39909121755204374,
+                    "timestamp": 0.49751591362387254
+                },
+                {
+                    "x": 3.712426777772062,
+                    "y": 6.907421982754425,
+                    "heading": 0.18847443820909762,
+                    "angularVelocity": -1.3951873937625683e-7,
+                    "velocityX": 3.7618298794631864,
+                    "velocityY": 0.39909149843660235,
+                    "timestamp": 0.5685896155701401
+                },
+                {
+                    "x": 3.979793955045072,
+                    "y": 6.93578687743453,
+                    "heading": 0.188474428292985,
+                    "angularVelocity": -1.3951872987555026e-7,
+                    "velocityX": 3.7618299026431106,
+                    "velocityY": 0.3990912799437423,
+                    "timestamp": 0.6396633175164076
+                },
+                {
+                    "x": 4.247161134956238,
+                    "y": 6.964151747247427,
+                    "heading": 0.18847441837687146,
+                    "angularVelocity": -1.395187425580786e-7,
+                    "velocityX": 3.761829939761708,
+                    "velocityY": 0.399090930064572,
+                    "timestamp": 0.7107370194626751
+                },
+                {
+                    "x": 4.514528318048531,
+                    "y": 6.992516587075052,
+                    "heading": 0.18847440846075478,
+                    "angularVelocity": -1.3951878716159625e-7,
+                    "velocityX": 3.7618299845198404,
+                    "velocityY": 0.39909050817474234,
+                    "timestamp": 0.7818107214089426
+                },
+                {
+                    "x": 4.781895504744899,
+                    "y": 7.020881392930694,
+                    "heading": 0.18847439854463183,
+                    "angularVelocity": -1.395188750932112e-7,
+                    "velocityX": 3.761830035228834,
+                    "velocityY": 0.3990900301922755,
+                    "timestamp": 0.8528844233552101
+                },
+                {
+                    "x": 5.049262689335958,
+                    "y": 7.049246218630208,
+                    "heading": 0.18847438862849922,
+                    "angularVelocity": -1.3951901124406535e-7,
+                    "velocityX": 3.7618300056073353,
+                    "velocityY": 0.3990903093936624,
+                    "timestamp": 0.9239581253014776
+                },
+                {
+                    "x": 5.3166271708653765,
+                    "y": 7.077636511969196,
+                    "heading": 0.18847437871238637,
+                    "angularVelocity": -1.3951873300413244e-7,
+                    "velocityX": 3.761791973795463,
+                    "velocityY": 0.3994486365804798,
+                    "timestamp": 0.9950318272477451
+                },
+                {
+                    "x": 5.584732592713635,
+                    "y": 7.097866624574175,
+                    "heading": 0.18847436874075538,
+                    "angularVelocity": -1.4029986815741496e-7,
+                    "velocityX": 3.772216931248832,
+                    "velocityY": 0.28463569578063386,
+                    "timestamp": 1.0661055291940127
+                },
+                {
+                    "x": 5.85271010689652,
+                    "y": 7.076007397472714,
+                    "heading": 0.188474358062239,
+                    "angularVelocity": -1.5024567569917294e-7,
+                    "velocityX": 3.7704172829702145,
+                    "velocityY": -0.3075571766057785,
+                    "timestamp": 1.1371792311402802
+                },
+                {
+                    "x": 6.113985538482666,
+                    "y": 7.0125651359558105,
+                    "heading": 0.18847434515977443,
+                    "angularVelocity": -1.8153640819270364e-7,
+                    "velocityX": 3.6761196396335167,
+                    "velocityY": -0.8926263833128883,
+                    "timestamp": 1.2082529330865477
+                },
+                {
+                    "x": 6.38971197835489,
+                    "y": 6.891444245730791,
+                    "heading": 0.18847433517810044,
+                    "angularVelocity": -1.2538350348338093e-7,
+                    "velocityX": 3.4635019217336325,
+                    "velocityY": -1.521444357133088,
+                    "timestamp": 1.2878620818911364
+                },
+                {
+                    "x": 6.640022696468843,
+                    "y": 6.723993100546061,
+                    "heading": 0.18847432674418188,
+                    "angularVelocity": -1.0594157434839283e-7,
+                    "velocityX": 3.1442456284571,
+                    "velocityY": -2.103415847287377,
+                    "timestamp": 1.367471230695725
+                },
+                {
+                    "x": 6.8648306229514935,
+                    "y": 6.523601637914116,
+                    "heading": 0.18847431867026868,
+                    "angularVelocity": -1.014194138550125e-7,
+                    "velocityX": 2.8238956182605124,
+                    "velocityY": -2.5171913736164258,
+                    "timestamp": 1.4470803795003138
+                },
+                {
+                    "x": 7.089642217529903,
+                    "y": 6.3232142903896476,
+                    "heading": 0.1884743105959038,
+                    "angularVelocity": -1.0142508708190059e-7,
+                    "velocityX": 2.823941694568014,
+                    "velocityY": -2.517139682229462,
+                    "timestamp": 1.5266895283049025
+                },
+                {
+                    "x": 7.2864279243188,
+                    "y": 6.147809259471006,
+                    "heading": 0.12465186300240168,
+                    "angularVelocity": -0.8016973997575559,
+                    "velocityX": 2.471898139143281,
+                    "velocityY": -2.203327551576542,
+                    "timestamp": 1.6062986771094911
+                },
+                {
+                    "x": 7.443856673448633,
+                    "y": 6.007485445692029,
+                    "heading": 0.07359246308164451,
+                    "angularVelocity": -0.6413760313670541,
+                    "velocityX": 1.9775208188222257,
+                    "velocityY": -1.7626593913656032,
+                    "timestamp": 1.6859078259140798
+                },
+                {
+                    "x": 7.561928317357885,
+                    "y": 5.902242696623479,
+                    "heading": 0.0352966131369852,
+                    "angularVelocity": -0.4810483533581169,
+                    "velocityX": 1.483141644926293,
+                    "velocityY": -1.3219931458747511,
+                    "timestamp": 1.7655169747186685
+                },
                 {
-                    "x": 7.617838563683624,
-                    "y": 5.830153124497745,
-                    "heading": -0.012612817121248107,
-                    "angularVelocity": 0.2090804238734443,
-                    "velocityX": 1.3520319278771131,
-                    "velocityY": -0.7210914095741754,
-                    "timestamp": 1.7470997268964392
+                    "x": 7.640642772199918,
+                    "y": 5.832080900256126,
+                    "heading": 0.009765510491971057,
+                    "angularVelocity": -0.320705635324694,
+                    "velocityX": 0.9887614178017027,
+                    "velocityY": -0.8813283073733266,
+                    "timestamp": 1.8451261235232572
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
                     "heading": -0.003,
-                    "angularVelocity": 0.10454098440514517,
-                    "velocityX": 0.6760159548013833,
-                    "velocityY": -0.36054574089833413,
-                    "timestamp": 1.839052344101515
+                    "angularVelocity": -0.1603523047748632,
+                    "velocityX": 0.4943807136624207,
+                    "velocityY": -0.44066417971947147,
+                    "timestamp": 1.924735272327846
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
                     "heading": -0.003,
-                    "angularVelocity": -2.423215560944852e-29,
+                    "angularVelocity": 9.952815155734959e-29,
                     "velocityX": 0,
-                    "velocityY": -2.0134519400533763e-31,
-                    "timestamp": 1.9310049613065907
+                    "velocityY": 1.270429811278963e-30,
+                    "timestamp": 2.004344421132435
                 }
             ],
             "constraints": [
@@ -11845,16 +11985,6 @@
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
             "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
                 {
                     "x": 5.61,
                     "y": 5.44,
@@ -14469,548 +14599,548 @@
                     "heading": 0,
                     "angularVelocity": 0,
                     "velocityX": 0,
-                    "velocityY": 6.585905017096791e-33,
+                    "velocityY": 1.4317938010297452e-32,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.24734688866176,
-                    "y": 5.551784954914585,
-                    "heading": -0.0040460391001905256,
-                    "angularVelocity": -0.1252124844254258,
-                    "velocityX": -0.08210564799104332,
-                    "velocityY": -0.2542304163009863,
-                    "timestamp": 0.03231338407473799
-                },
-                {
-                    "x": 2.242039203719346,
-                    "y": 5.535352462397274,
-                    "heading": -0.012062241013264058,
-                    "angularVelocity": -0.24807683078112674,
-                    "velocityX": -0.16425654862202216,
-                    "velocityY": -0.5085351778477981,
-                    "timestamp": 0.06462676814947597
-                },
-                {
-                    "x": 2.2340752644376702,
-                    "y": 5.510699787230797,
-                    "heading": -0.023960373976211733,
-                    "angularVelocity": -0.3682106750388487,
-                    "velocityX": -0.24645946284226605,
-                    "velocityY": -0.7629245859689895,
-                    "timestamp": 0.09694015222421395
-                },
-                {
-                    "x": 2.223453118891146,
-                    "y": 5.477823766232339,
-                    "heading": -0.039635722986674345,
-                    "angularVelocity": -0.4851039115621088,
-                    "velocityX": -0.32872278316489917,
-                    "velocityY": -1.0174118848839497,
-                    "timestamp": 0.12925353629895195
-                },
-                {
-                    "x": 2.210170475892355,
-                    "y": 5.4367206851300205,
-                    "heading": -0.05896132885919571,
-                    "angularVelocity": -0.5980681511978081,
-                    "velocityX": -0.4110570087017019,
-                    "velocityY": -1.2720141291067089,
-                    "timestamp": 0.16156692037368994
-                },
-                {
-                    "x": 2.194224615527819,
-                    "y": 5.387386118415271,
-                    "heading": -0.08177969695024358,
-                    "angularVelocity": -0.706158415295445,
-                    "velocityX": -0.4934754072075649,
-                    "velocityY": -1.5267533292286215,
-                    "timestamp": 0.19388030444842794
-                },
-                {
-                    "x": 2.1756122677867826,
-                    "y": 5.329814722150547,
-                    "heading": -0.10789038891803496,
-                    "angularVelocity": -0.8080457282778595,
-                    "velocityX": -0.575995002503841,
-                    "velocityY": -1.7816579078058363,
-                    "timestamp": 0.22619368852316593
-                },
-                {
-                    "x": 2.1543294408518685,
-                    "y": 5.263999973874467,
-                    "heading": -0.13703048715481858,
-                    "angularVelocity": -0.901796548742399,
-                    "velocityX": -0.6586381322887348,
-                    "velocityY": -2.0367643365317547,
-                    "timestamp": 0.2585070725979039
-                },
-                {
-                    "x": 2.1303711674850674,
-                    "y": 5.189933880722087,
-                    "heading": -0.16884170077604577,
-                    "angularVelocity": -0.984459366671516,
-                    "velocityX": -0.7414349828352131,
-                    "velocityY": -2.2921181198809735,
-                    "timestamp": 0.2908204566726419
-                },
-                {
-                    "x": 2.103731122650155,
-                    "y": 5.107606803578156,
-                    "heading": -0.20280968351747503,
-                    "angularVelocity": -1.0512047473228812,
-                    "velocityX": -0.8244275738281073,
-                    "velocityY": -2.547770204244666,
-                    "timestamp": 0.3231338407473799
-                },
-                {
-                    "x": 2.0744011006982923,
-                    "y": 5.0170081629947525,
-                    "heading": -0.23813652479783928,
-                    "angularVelocity": -1.0932572459343544,
-                    "velocityX": -0.9076741044523434,
-                    "velocityY": -2.8037496900311267,
-                    "timestamp": 0.3554472248221179
-                },
-                {
-                    "x": 2.0423709680329383,
-                    "y": 4.918132526359071,
-                    "heading": -0.27341203074981035,
-                    "angularVelocity": -1.0916685751756738,
-                    "velocityX": -0.991234238768389,
-                    "velocityY": -3.05989729849995,
-                    "timestamp": 0.3877606088968559
-                },
-                {
-                    "x": 2.007638970172759,
-                    "y": 4.811032750261161,
-                    "heading": -0.30535515788271084,
-                    "angularVelocity": -0.9885416847402974,
-                    "velocityX": -1.0748486688935617,
-                    "velocityY": -3.3144091578337234,
-                    "timestamp": 0.42007399297159387
-                },
-                {
-                    "x": 1.9707337326379286,
-                    "y": 4.697468787945422,
-                    "heading": -0.31502999189110337,
-                    "angularVelocity": -0.2994064003329394,
-                    "velocityX": -1.1421037626226502,
-                    "velocityY": -3.5144558692173984,
-                    "timestamp": 0.45238737704633186
-                },
-                {
-                    "x": 1.9331993823421831,
-                    "y": 4.581134405528862,
-                    "heading": -0.31503001351145116,
-                    "angularVelocity": -6.690833652147531e-7,
-                    "velocityX": -1.1615728705149717,
-                    "velocityY": -3.6001918631452843,
-                    "timestamp": 0.48470076112106986
-                },
-                {
-                    "x": 1.8956658919440634,
-                    "y": 4.464799745676637,
-                    "heading": -0.31503003513146777,
-                    "angularVelocity": -6.690731172580015e-7,
-                    "velocityX": -1.1615462593242614,
-                    "velocityY": -3.600200448927068,
-                    "timestamp": 0.5170141451958078
-                },
-                {
-                    "x": 1.8630741103436357,
-                    "y": 4.346985095772027,
-                    "heading": -0.31503005685343444,
-                    "angularVelocity": -6.722281571161063e-7,
-                    "velocityX": -1.008615548437953,
-                    "velocityY": -3.646001595874795,
-                    "timestamp": 0.5493275292705457
-                },
-                {
-                    "x": 1.8389499213812899,
-                    "y": 4.227149627930832,
-                    "heading": -0.3150300790516974,
-                    "angularVelocity": -6.869680669187691e-7,
-                    "velocityX": -0.746569560976628,
-                    "velocityY": -3.708539704910713,
-                    "timestamp": 0.5816409133452837
-                },
-                {
-                    "x": 1.8234163358184028,
-                    "y": 4.105901026539528,
-                    "heading": -0.3150301022065626,
-                    "angularVelocity": -7.165719666567231e-7,
-                    "velocityX": -0.48071676822704484,
-                    "velocityY": -3.752271848434857,
-                    "timestamp": 0.6139542974200216
-                },
-                {
-                    "x": 1.8165522814590422,
-                    "y": 3.983854314068792,
-                    "heading": -0.3150301268891266,
-                    "angularVelocity": -7.638495563130961e-7,
-                    "velocityX": -0.21242140233547732,
-                    "velocityY": -3.7769709352772667,
-                    "timestamp": 0.6462676814947595
-                },
-                {
-                    "x": 1.818392625605847,
-                    "y": 3.861628588886844,
-                    "heading": -0.3150301538292836,
-                    "angularVelocity": -8.337151223773892e-7,
-                    "velocityX": 0.05695299949234183,
-                    "velocityY": -3.7825108289262035,
-                    "timestamp": 0.6785810655694975
-                },
-                {
-                    "x": 1.828928055424025,
-                    "y": 3.7398438657688415,
-                    "heading": -0.315030184023291,
-                    "angularVelocity": -9.344118022266017e-7,
-                    "velocityX": 0.32603919768385625,
-                    "velocityY": -3.768863169401422,
-                    "timestamp": 0.7108944496442354
-                },
-                {
-                    "x": 1.848105139864153,
-                    "y": 3.6191179259506723,
-                    "heading": -0.31503021891945676,
-                    "angularVelocity": -0.0000010799291597080048,
-                    "velocityX": 0.593471868986944,
-                    "velocityY": -3.7360970778839238,
-                    "timestamp": 0.7432078337189734
-                },
-                {
-                    "x": 1.8758266057917121,
-                    "y": 3.5000631817385077,
-                    "heading": -0.3150302607662258,
-                    "angularVelocity": -0.0000012950289872625492,
-                    "velocityX": 0.8578942355106304,
-                    "velocityY": -3.6843787062599658,
-                    "timestamp": 0.7755212177937113
-                },
-                {
-                    "x": 1.911951833448642,
-                    "y": 3.3832835694780297,
-                    "heading": -0.3150303133273049,
-                    "angularVelocity": -0.000001626603978371328,
-                    "velocityX": 1.1179648523774262,
-                    "velocityY": -3.6139703594763453,
-                    "timestamp": 0.8078346018684492
-                },
-                {
-                    "x": 1.9562975704379837,
-                    "y": 3.2693714862411047,
-                    "heading": -0.315030383530948,
-                    "angularVelocity": -0.000002172587152219365,
-                    "velocityX": 1.3723643703418302,
-                    "velocityY": -3.525229142619559,
-                    "timestamp": 0.8401479859431872
+                    "x": 2.2473468901462774,
+                    "y": 5.551784954334613,
+                    "heading": -0.004046039178700474,
+                    "angularVelocity": -0.12521248615625988,
+                    "velocityX": -0.08210560159156222,
+                    "velocityY": -0.2542304328304919,
+                    "timestamp": 0.032313384255078216
+                },
+                {
+                    "x": 2.2420392078855844,
+                    "y": 5.535352460743813,
+                    "heading": -0.012062241078351911,
+                    "angularVelocity": -0.24807682898124367,
+                    "velocityX": -0.16425646471427996,
+                    "velocityY": -0.5085352082308542,
+                    "timestamp": 0.06462676851015643
+                },
+                {
+                    "x": 2.234075272195187,
+                    "y": 5.510699784095841,
+                    "heading": -0.023960373728225737,
+                    "angularVelocity": -0.36821066329520014,
+                    "velocityX": -0.24645935032774877,
+                    "velocityY": -0.7629246275588465,
+                    "timestamp": 0.09694015276523466
+                },
+                {
+                    "x": 2.2234531308615284,
+                    "y": 5.477823761292619,
+                    "heading": -0.03963572188039383,
+                    "angularVelocity": -0.4851038822931769,
+                    "velocityX": -0.32872265095504705,
+                    "velocityY": -1.017411935057708,
+                    "timestamp": 0.12925353702031286
+                },
+                {
+                    "x": 2.2101704924088987,
+                    "y": 5.436720678146679,
+                    "heading": -0.05896132607089341,
+                    "angularVelocity": -0.5980680958065965,
+                    "velocityX": -0.4110568657178767,
+                    "velocityY": -1.27201418525146,
+                    "timestamp": 0.16156692127539107
+                },
+                {
+                    "x": 2.1942246366351648,
+                    "y": 5.38738610923438,
+                    "heading": -0.08177969135834087,
+                    "angularVelocity": -0.7061583245915709,
+                    "velocityX": -0.4934752623822669,
+                    "velocityY": -1.5267533887152571,
+                    "timestamp": 0.19388030553046928
+                },
+                {
+                    "x": 2.1756122932404036,
+                    "y": 5.329814710705053,
+                    "heading": -0.10789037911156549,
+                    "angularVelocity": -0.8080455933402793,
+                    "velocityX": -0.5759948647853634,
+                    "velocityY": -1.7816579679449487,
+                    "timestamp": 0.2261936897855475
+                },
+                {
+                    "x": 2.1543294701171383,
+                    "y": 5.263999960188385,
+                    "heading": -0.1370304714941401,
+                    "angularVelocity": -0.9017963625397062,
+                    "velocityX": -0.6586380106540685,
+                    "velocityY": -2.0367643945039555,
+                    "timestamp": 0.25850707404062573
+                },
+                {
+                    "x": 2.1303711997350336,
+                    "y": 5.1899338649176165,
+                    "heading": -0.1688416775308267,
+                    "angularVelocity": -0.9844591264590365,
+                    "velocityX": -0.7414348863300811,
+                    "velocityY": -2.2921181726463207,
+                    "timestamp": 0.29082045829570397
+                },
+                {
+                    "x": 2.1037311567595207,
+                    "y": 5.107606785885568,
+                    "heading": -0.20280965108227242,
+                    "angularVelocity": -1.051204457054369,
+                    "velocityX": -0.8244275116843016,
+                    "velocityY": -2.5477702484570597,
+                    "timestamp": 0.3231338425507822
+                },
+                {
+                    "x": 2.074401135228672,
+                    "y": 5.017008143765364,
+                    "heading": -0.23813648207052604,
+                    "angularVelocity": -1.0932569213236978,
+                    "velocityX": -0.9076740863575303,
+                    "velocityY": -2.8037497219427316,
+                    "timestamp": 0.35544722680586044
+                },
+                {
+                    "x": 2.0423710011934597,
+                    "y": 4.918132506074763,
+                    "heading": -0.27341197795784133,
+                    "angularVelocity": -1.0916682576129122,
+                    "velocityX": -0.9912342756292476,
+                    "velocityY": -3.0598973140692034,
+                    "timestamp": 0.3877606110609387
+                },
+                {
+                    "x": 2.00763899967256,
+                    "y": 4.811032729430715,
+                    "heading": -0.3053550995537523,
+                    "angularVelocity": -0.9885415078704507,
+                    "velocityX": -1.0748487761829064,
+                    "velocityY": -3.3144091562374,
+                    "timestamp": 0.4200739953160169
+                },
+                {
+                    "x": 1.9707337568018743,
+                    "y": 4.697468766706309,
+                    "heading": -0.3150299299334663,
+                    "angularVelocity": -0.2994062863655037,
+                    "velocityX": -1.1421039213769328,
+                    "velocityY": -3.514455862250325,
+                    "timestamp": 0.45238737957109515
+                },
+                {
+                    "x": 1.9331993710024595,
+                    "y": 4.581134395027877,
+                    "heading": -0.3150299515537849,
+                    "angularVelocity": -6.690824588937528e-7,
+                    "velocityX": -1.1615739627617825,
+                    "velocityY": -3.6001915107405935,
+                    "timestamp": 0.4847007638261734
+                },
+                {
+                    "x": 1.8956658385726595,
+                    "y": 4.464799748019651,
+                    "heading": -0.3150299731737692,
+                    "angularVelocity": -6.690721139443934e-7,
+                    "velocityX": -1.1615475535931208,
+                    "velocityY": -3.600200031352131,
+                    "timestamp": 0.5170141480812516
+                },
+                {
+                    "x": 1.8630740613066095,
+                    "y": 4.346985096208152,
+                    "heading": -0.31502999489570194,
+                    "angularVelocity": -6.722271053919432e-7,
+                    "velocityX": -1.0086154086732113,
+                    "velocityY": -3.64600163453891,
+                    "timestamp": 0.5493275323363298
+                },
+                {
+                    "x": 1.8389498768606893,
+                    "y": 4.227149626761845,
+                    "heading": -0.3150300170939308,
+                    "angularVelocity": -6.869670074324573e-7,
+                    "velocityX": -0.7465694170405304,
+                    "velocityY": -3.708539733886721,
+                    "timestamp": 0.581640916591408
+                },
+                {
+                    "x": 1.8234162959667812,
+                    "y": 4.10590102408459,
+                    "heading": -0.31503004024876186,
+                    "angularVelocity": -7.165709048863888e-7,
+                    "velocityX": -0.4807166210536207,
+                    "velocityY": -3.7522718672898008,
+                    "timestamp": 0.6139543008464863
+                },
+                {
+                    "x": 1.816552246405149,
+                    "y": 3.98385431066073,
+                    "heading": -0.315030064931292,
+                    "angularVelocity": -7.63848502164674e-7,
+                    "velocityX": -0.21242125267501,
+                    "velocityY": -3.776970943694307,
+                    "timestamp": 0.6462676851015645
+                },
+                {
+                    "x": 1.818392595454208,
+                    "y": 3.861628584870307,
+                    "heading": -0.315030091871416,
+                    "angularVelocity": -8.337140982566696e-7,
+                    "velocityX": 0.05695315088421205,
+                    "velocityY": -3.782510826646546,
+                    "timestamp": 0.6785810693566428
+                },
+                {
+                    "x": 1.828928030254088,
+                    "y": 3.739843861498511,
+                    "heading": -0.3150301220653929,
+                    "angularVelocity": -9.344108498281474e-7,
+                    "velocityX": 0.32603935003261814,
+                    "velocityY": -3.7688631562216113,
+                    "timestamp": 0.710894453611721
+                },
+                {
+                    "x": 1.8481051197294465,
+                    "y": 3.619117921789427,
+                    "heading": -0.3150301569615331,
+                    "angularVelocity": -0.0000010799283629257,
+                    "velocityX": 0.593472021499706,
+                    "velocityY": -3.7360970536569806,
+                    "timestamp": 0.7432078378667992
+                },
+                {
+                    "x": 1.875826590719106,
+                    "y": 3.500063178055521,
+                    "heading": -0.31503019880828736,
+                    "angularVelocity": -0.0000012950285239246719,
+                    "velocityX": 0.8578943873792161,
+                    "velocityY": -3.684378670896923,
+                    "timestamp": 0.7755212221218775
+                },
+                {
+                    "x": 1.9119518234377388,
+                    "y": 3.3832835666467993,
+                    "heading": -0.31503025136937535,
+                    "angularVelocity": -0.0000016266042432729645,
+                    "velocityX": 1.1179650027822445,
+                    "velocityY": -3.61397031294762,
+                    "timestamp": 0.8078346063769557
+                },
+                {
+                    "x": 1.9562975654606443,
+                    "y": 3.269371484637439,
+                    "heading": -0.31503032157308203,
+                    "angularVelocity": -0.0000021725891085409373,
+                    "velocityX": 1.3723645184560365,
+                    "velocityY": -3.5252290849559675,
+                    "timestamp": 0.8401479906320339
                 },
                 {
                     "x": 2.008638858795166,
                     "y": 3.158904790878296,
-                    "heading": -0.3150304952316807,
-                    "angularVelocity": -0.00000345679463513026,
-                    "velocityX": 1.6198021301675205,
-                    "velocityY": -3.418604968990847,
-                    "timestamp": 0.8724613700179251
-                },
-                {
-                    "x": 2.0775165593610554,
-                    "y": 3.0394044439018932,
-                    "heading": -0.31503058092883834,
-                    "angularVelocity": -0.0000023503872475309846,
-                    "velocityX": 1.8890856295919138,
-                    "velocityY": -3.277496001603852,
-                    "timestamp": 0.9089222373745671
-                },
-                {
-                    "x": 2.155767716832464,
-                    "y": 2.9258208551273497,
-                    "heading": -0.31503063899444206,
-                    "angularVelocity": -0.0000015925458689218539,
-                    "velocityX": 2.146168293419776,
-                    "velocityY": -3.11521905563918,
-                    "timestamp": 0.9453831047312091
-                },
-                {
-                    "x": 2.242886939864493,
-                    "y": 2.818887609111427,
-                    "heading": -0.31503068232040304,
-                    "angularVelocity": -0.000001188286624986991,
-                    "velocityX": 2.38938975806231,
-                    "velocityY": -2.9328223316783686,
-                    "timestamp": 0.9818439720878511
-                },
-                {
-                    "x": 2.3383115633038396,
-                    "y": 2.719295334161189,
-                    "heading": -0.31503071689328305,
-                    "angularVelocity": -9.482188022909844e-7,
-                    "velocityX": 2.6171791939547426,
-                    "velocityY": -2.7314839763979357,
-                    "timestamp": 1.0183048394444931
-                },
-                {
-                    "x": 2.4414252810789625,
-                    "y": 2.627687242403576,
-                    "heading": -0.3150307459166021,
-                    "angularVelocity": -7.96012851101892e-7,
-                    "velocityX": 2.8280654095942417,
-                    "velocityY": -2.5125044574926996,
-                    "timestamp": 1.0547657068011351
-                },
-                {
-                    "x": 2.5515621254678895,
-                    "y": 2.5446549735283583,
-                    "heading": -0.3150307712925926,
-                    "angularVelocity": -6.959787951410741e-7,
-                    "velocityX": 3.020686351523773,
-                    "velocityY": -2.2772982349278084,
-                    "timestamp": 1.0912265741577771
-                },
-                {
-                    "x": 2.668010765906386,
-                    "y": 2.470734767010798,
-                    "heading": -0.31503079425241676,
-                    "angularVelocity": -6.297114091058936e-7,
-                    "velocityX": 3.1937978682584367,
-                    "velocityY": -2.027384751835701,
-                    "timestamp": 1.1276874415144191
-                },
-                {
-                    "x": 2.790019095895906,
-                    "y": 2.406403976394343,
-                    "heading": -0.31503081565982244,
-                    "angularVelocity": -5.871337458164478e-7,
-                    "velocityX": 3.3462816119017478,
-                    "velocityY": -1.7643790529502215,
-                    "timestamp": 1.1641483088710611
-                },
-                {
-                    "x": 2.916799054801627,
-                    "y": 2.352077873335088,
-                    "heading": -0.31503083617399774,
-                    "angularVelocity": -5.626354173508468e-7,
-                    "velocityX": 3.477151480397404,
-                    "velocityY": -1.4899838374074656,
-                    "timestamp": 1.2006091762277031
-                },
-                {
-                    "x": 3.0475310715630033,
-                    "y": 2.308104889106193,
-                    "heading": -0.31503085634737477,
-                    "angularVelocity": -5.532884563345105e-7,
-                    "velocityX": 3.585543247850392,
-                    "velocityY": -1.2060323140087088,
-                    "timestamp": 1.2370700435843451
-                },
-                {
-                    "x": 3.1788176786437843,
-                    "y": 2.2658164704041117,
-                    "heading": -0.3150308764961414,
-                    "angularVelocity": -5.526134756982608e-7,
-                    "velocityX": 3.600753810835078,
-                    "velocityY": -1.1598302993847416,
-                    "timestamp": 1.2735309109409871
-                },
-                {
-                    "x": 3.3101043321458232,
-                    "y": 2.2235281958195148,
-                    "heading": -0.3150308966449121,
-                    "angularVelocity": -5.526135855693721e-7,
-                    "velocityX": 3.600755084015385,
-                    "velocityY": -1.159826346722746,
-                    "timestamp": 1.3099917782976291
-                },
-                {
-                    "x": 3.441390996396766,
-                    "y": 2.181239954605585,
-                    "heading": -0.31503091679368933,
-                    "angularVelocity": -5.526137662420421e-7,
-                    "velocityX": 3.6007553788219733,
-                    "velocityY": -1.1598254314766572,
-                    "timestamp": 1.3464526456542711
-                },
-                {
-                    "x": 3.5726776715245805,
-                    "y": 2.1389517471596347,
-                    "heading": -0.31503093694247225,
-                    "angularVelocity": -5.526139217103117e-7,
-                    "velocityX": 3.600755677138282,
-                    "velocityY": -1.1598245053335783,
-                    "timestamp": 1.3829135130109131
-                },
-                {
-                    "x": 3.7039643576361168,
-                    "y": 2.0966635738134185,
-                    "heading": -0.3150309570912594,
-                    "angularVelocity": -5.526140383605173e-7,
-                    "velocityX": 3.6007559783851373,
-                    "velocityY": -1.159823570091575,
-                    "timestamp": 1.4193743803675551
-                },
-                {
-                    "x": 3.835251054850112,
-                    "y": 2.0543754349355976,
-                    "heading": -0.31503097724004914,
-                    "angularVelocity": -5.526141098258176e-7,
-                    "velocityX": 3.600756282888565,
-                    "velocityY": -1.1598226247384584,
-                    "timestamp": 1.4558352477241971
-                },
-                {
-                    "x": 3.966537763342614,
-                    "y": 2.012087331072757,
-                    "heading": -0.3150309973888395,
-                    "angularVelocity": -5.526141250500724e-7,
-                    "velocityX": 3.6007565922204043,
-                    "velocityY": -1.1598216643943178,
-                    "timestamp": 1.4922961150808391
-                },
-                {
-                    "x": 4.097824483400825,
-                    "y": 1.9697992631165677,
-                    "heading": -0.315031017537628,
-                    "angularVelocity": -5.526140761706961e-7,
-                    "velocityX": 3.6007569094292164,
-                    "velocityY": -1.1598206795946073,
-                    "timestamp": 1.5287569824374811
-                },
-                {
-                    "x": 4.2291112154854185,
-                    "y": 1.9275112324972625,
-                    "heading": -0.3150310376864126,
-                    "angularVelocity": -5.526139663753558e-7,
-                    "velocityX": 3.600757239272763,
-                    "velocityY": -1.1598196555683948,
-                    "timestamp": 1.5652178497941232
-                },
-                {
-                    "x": 4.360397911943487,
-                    "y": 1.8852230912740195,
-                    "heading": -0.3150310578362786,
-                    "angularVelocity": -5.526436278671969e-7,
-                    "velocityX": 3.600756262156026,
-                    "velocityY": -1.159822689065559,
-                    "timestamp": 1.6016787171507652
-                },
-                {
-                    "x": 4.485161433583451,
-                    "y": 1.8443297112343198,
-                    "heading": -0.3370639022790847,
-                    "angularVelocity": -0.6042874467931063,
-                    "velocityX": 3.4218473307173203,
-                    "velocityY": -1.121569041122948,
-                    "timestamp": 1.6381395845074072
-                },
-                {
-                    "x": 4.600444501590908,
-                    "y": 1.8067758967734413,
-                    "heading": -0.37703885598165165,
-                    "angularVelocity": -1.0963796695112746,
-                    "velocityX": 3.1618301034864644,
-                    "velocityY": -1.029975894252485,
-                    "timestamp": 1.6746004518640492
-                },
-                {
-                    "x": 4.705219738357152,
-                    "y": 1.7727109130585983,
-                    "heading": -0.41891394403468696,
-                    "angularVelocity": -1.1484940180779766,
-                    "velocityX": 2.873635334600916,
-                    "velocityY": -0.9342889016225668,
-                    "timestamp": 1.7110613192206912
-                },
-                {
-                    "x": 4.799468915908326,
-                    "y": 1.742101106125887,
-                    "heading": -0.4594847392760727,
-                    "angularVelocity": -1.112721615877786,
-                    "velocityX": 2.5849406331801226,
-                    "velocityY": -0.8395249249915333,
-                    "timestamp": 1.7475221865773332
-                },
-                {
-                    "x": 4.883206605537126,
-                    "y": 1.7149241083644653,
-                    "heading": -0.4972998071219025,
-                    "angularVelocity": -1.0371412033604122,
-                    "velocityX": 2.2966455737248266,
-                    "velocityY": -0.7453744173332496,
-                    "timestamp": 1.7839830539339752
-                },
-                {
-                    "x": 4.956447647832475,
-                    "y": 1.6911654776329053,
-                    "heading": -0.531527000493052,
-                    "angularVelocity": -0.9387377715499247,
-                    "velocityX": 2.008757542132538,
-                    "velocityY": -0.6516200094519161,
-                    "timestamp": 1.8204439212906172
-                },
-                {
-                    "x": 5.019204190370849,
-                    "y": 1.670815407546972,
-                    "heading": -0.5616288284566417,
-                    "angularVelocity": -0.8255927559032131,
-                    "velocityX": 1.721202678052634,
-                    "velocityY": -0.5581345579873002,
-                    "timestamp": 1.8569047886472592
-                },
-                {
-                    "x": 5.071485904769321,
-                    "y": 1.6538669030565063,
-                    "heading": -0.5872312038868318,
-                    "angularVelocity": -0.7021877779198676,
-                    "velocityX": 1.433913074175601,
-                    "velocityY": -0.46484095742112364,
-                    "timestamp": 1.8933656560039012
-                },
-                {
-                    "x": 5.113300541419085,
-                    "y": 1.6403147551859525,
-                    "heading": -0.6080594891340769,
-                    "angularVelocity": -0.5712504050853902,
-                    "velocityX": 1.14683603768266,
-                    "velocityY": -0.3716902216832496,
-                    "timestamp": 1.9298265233605432
-                },
-                {
-                    "x": 5.144654410764751,
-                    "y": 1.6301549374248205,
-                    "heading": -0.6239033656036709,
-                    "angularVelocity": -0.43454469457947725,
-                    "velocityX": 0.8599320756409368,
-                    "velocityY": -0.2786499197003079,
-                    "timestamp": 1.9662873907171852
-                },
-                {
-                    "x": 5.165552750765901,
-                    "y": 1.6233842339153202,
-                    "heading": -0.634595855168354,
-                    "angularVelocity": -0.29325933089016243,
-                    "velocityX": 0.57317177336273,
-                    "velocityY": -0.18569781797215093,
-                    "timestamp": 2.002748258073827
+                    "heading": -0.3150304332740777,
+                    "angularVelocity": -0.0000034568027529519608,
+                    "velocityX": 1.6198022751608165,
+                    "velocityY": -3.4186049002831713,
+                    "timestamp": 0.8724613748871122
+                },
+                {
+                    "x": 2.0775165641117233,
+                    "y": 3.0394044472847606,
+                    "heading": -0.3150305189714284,
+                    "angularVelocity": -0.000002350392551393865,
+                    "velocityX": 1.8890857675343722,
+                    "velocityY": -3.277495922091173,
+                    "timestamp": 0.9089222420961518
+                },
+                {
+                    "x": 2.155767726009908,
+                    "y": 2.9258208622381168,
+                    "heading": -0.3150305770370975,
+                    "angularVelocity": -0.00000159254767063084,
+                    "velocityX": 2.1461684235196876,
+                    "velocityY": -3.1152189660064753,
+                    "timestamp": 0.9453831093051914
+                },
+                {
+                    "x": 2.242886953120307,
+                    "y": 2.818887620265158,
+                    "heading": -0.3150306203630764,
+                    "angularVelocity": -0.0000011882871183876306,
+                    "velocityX": 2.389389879591231,
+                    "velocityY": -2.932822232666113,
+                    "timestamp": 0.981843976514231
+                },
+                {
+                    "x": 2.338311580267833,
+                    "y": 2.719295349641276,
+                    "heading": -0.3150306549359537,
+                    "angularVelocity": -9.482187337248934e-7,
+                    "velocityX": 2.617179306252704,
+                    "velocityY": -2.731483868798106,
+                    "timestamp": 1.0183048437232707
+                },
+                {
+                    "x": 2.4414253013619946,
+                    "y": 2.6276872624602983,
+                    "heading": -0.31503068395926054,
+                    "angularVelocity": -7.960125215618009e-7,
+                    "velocityX": 2.8280655120730875,
+                    "velocityY": -2.512504342142078,
+                    "timestamp": 1.0547657109323103
+                },
+                {
+                    "x": 2.5515621486647997,
+                    "y": 2.5446549983777076,
+                    "heading": -0.3150307093352344,
+                    "angularVelocity": -6.959783403374032e-7,
+                    "velocityX": 3.020686443670201,
+                    "velocityY": -2.2772981127010774,
+                    "timestamp": 1.09122657814135
+                },
+                {
+                    "x": 2.6680107915990137,
+                    "y": 2.4707347968336117,
+                    "heading": -0.31503073229503964,
+                    "angularVelocity": -6.297108928988803e-7,
+                    "velocityX": 3.1937979496369038,
+                    "velocityY": -2.0273846236374995,
+                    "timestamp": 1.1276874453503896
+                },
+                {
+                    "x": 2.7900191236562386,
+                    "y": 2.4064040113359084,
+                    "heading": -0.3150307537024253,
+                    "angularVelocity": -5.871332010760787e-7,
+                    "velocityX": 3.3462816821585726,
+                    "velocityY": -1.764378919702536,
+                    "timestamp": 1.1641483125594292
+                },
+                {
+                    "x": 2.9167990841955884,
+                    "y": 2.352077913506683,
+                    "heading": -0.3150307742165802,
+                    "angularVelocity": -5.626348607586924e-7,
+                    "velocityX": 3.477151539278742,
+                    "velocityY": -1.4899836999970275,
+                    "timestamp": 1.2006091797684688
+                },
+                {
+                    "x": 3.0475311021850517,
+                    "y": 2.308104934680324,
+                    "heading": -0.31503079438993686,
+                    "angularVelocity": -5.532878997498012e-7,
+                    "velocityX": 3.585543296047864,
+                    "velocityY": -1.2060321707174542,
+                    "timestamp": 1.2370700469775084
+                },
+                {
+                    "x": 3.178817728061225,
+                    "y": 2.2658165761507623,
+                    "heading": -0.3150308145386853,
+                    "angularVelocity": -5.526129783451472e-7,
+                    "velocityX": 3.600754340906743,
+                    "velocityY": -1.159828653748452,
+                    "timestamp": 1.273530914186548
+                },
+                {
+                    "x": 3.3101043979960063,
+                    "y": 2.2235283544039026,
+                    "heading": -0.3150308346874411,
+                    "angularVelocity": -5.526131795163358e-7,
+                    "velocityX": 3.600755549287413,
+                    "velocityY": -1.1598249022550946,
+                    "timestamp": 1.3099917813955877
+                },
+                {
+                    "x": 3.4413910762638236,
+                    "y": 2.1812401585275256,
+                    "heading": -0.31503085483620696,
+                    "angularVelocity": -5.52613457009699e-7,
+                    "velocityX": 3.60075577783481,
+                    "velocityY": -1.159824192714005,
+                    "timestamp": 1.3464526486046273
+                },
+                {
+                    "x": 3.572677762945226,
+                    "y": 2.138951988771722,
+                    "heading": -0.31503087498498206,
+                    "angularVelocity": -5.52613710159953e-7,
+                    "velocityX": 3.6007560085914068,
+                    "velocityY": -1.1598234763137998,
+                    "timestamp": 1.382913515813667
+                },
+                {
+                    "x": 3.7039644581047573,
+                    "y": 2.0966638453368867,
+                    "heading": -0.31503089513376514,
+                    "angularVelocity": -5.526139295643368e-7,
+                    "velocityX": 3.6007562411181984,
+                    "velocityY": -1.1598227544173942,
+                    "timestamp": 1.4193743830227066
+                },
+                {
+                    "x": 3.8352511618245644,
+                    "y": 2.05437572847807,
+                    "heading": -0.3150309152825546,
+                    "angularVelocity": -5.526141044747151e-7,
+                    "velocityX": 3.600756475898009,
+                    "velocityY": -1.1598220255258234,
+                    "timestamp": 1.4558352502317462
+                },
+                {
+                    "x": 3.966537874250263,
+                    "y": 2.01208763864737,
+                    "heading": -0.3150309354313486,
+                    "angularVelocity": -5.526142273042023e-7,
+                    "velocityX": 3.6007567146716046,
+                    "velocityY": -1.159821284234734,
+                    "timestamp": 1.4922961174407858
+                },
+                {
+                    "x": 4.097824595645338,
+                    "y": 1.9697995766628096,
+                    "heading": -0.3150309555801447,
+                    "angularVelocity": -5.526142864672618e-7,
+                    "velocityX": 3.600756960671674,
+                    "velocityY": -1.159820520507965,
+                    "timestamp": 1.5287569846498255
+                },
+                {
+                    "x": 4.229111326453571,
+                    "y": 1.9275115439021684,
+                    "heading": -0.3150309757289402,
+                    "angularVelocity": -5.5261426880692e-7,
+                    "velocityX": 3.600757218843208,
+                    "velocityY": -1.1598197189933317,
+                    "timestamp": 1.565217851858865
+                },
+                {
+                    "x": 4.360398019008411,
+                    "y": 1.885223392382287,
+                    "heading": -0.31503099587881916,
+                    "angularVelocity": -5.52643987126892e-7,
+                    "velocityX": 3.6007561696802117,
+                    "velocityY": -1.1598229761632481,
+                    "timestamp": 1.6016787190679047
+                },
+                {
+                    "x": 4.485161526388635,
+                    "y": 1.844329974236374,
+                    "heading": -0.3370638509456068,
+                    "angularVelocity": -0.6042877406197901,
+                    "velocityX": 3.4218469534726492,
+                    "velocityY": -1.1215700907896822,
+                    "timestamp": 1.6381395862769443
+                },
+                {
+                    "x": 4.60044458089581,
+                    "y": 1.8067761222092775,
+                    "heading": -0.3770388194107599,
+                    "angularVelocity": -1.0963800788381923,
+                    "velocityX": 3.1618297460185825,
+                    "velocityY": -1.0299769287381584,
+                    "timestamp": 1.674600453485984
+                },
+                {
+                    "x": 4.705219804759057,
+                    "y": 1.772711102552083,
+                    "heading": -0.4189139196427851,
+                    "angularVelocity": -1.148494356756437,
+                    "velocityX": 2.8736349923479074,
+                    "velocityY": -0.9342898911836356,
+                    "timestamp": 1.7110613206950236
+                },
+                {
+                    "x": 4.799468970200479,
+                    "y": 1.7421012617230478,
+                    "heading": -0.45948472427125614,
+                    "angularVelocity": -1.1127218778387968,
+                    "velocityX": 2.5849403115144614,
+                    "velocityY": -0.8395258580532651,
+                    "timestamp": 1.7475221879040632
+                },
+                {
+                    "x": 4.883206648654635,
+                    "y": 1.7149242324789875,
+                    "heading": -0.49729979894596704,
+                    "angularVelocity": -1.037141394852447,
+                    "velocityX": 2.296645276538986,
+                    "velocityY": -0.7453752838144925,
+                    "timestamp": 1.7839830551131028
+                },
+                {
+                    "x": 4.956447680828415,
+                    "y": 1.6911655730288653,
+                    "heading": -0.5315269969459382,
+                    "angularVelocity": -0.9387379023033031,
+                    "velocityX": 2.0087572726635705,
+                    "velocityY": -0.6516207997442213,
+                    "timestamp": 1.8204439223221425
+                },
+                {
+                    "x": 5.019204214404415,
+                    "y": 1.6708154773305386,
+                    "heading": -0.5616288277349688,
+                    "angularVelocity": -0.825592836737831,
+                    "velocityX": 1.7212024392124468,
+                    "velocityY": -0.5581352627093101,
+                    "timestamp": 1.856904789531182
+                },
+                {
+                    "x": 5.071485921099441,
+                    "y": 1.6538669506708767,
+                    "heading": -0.587231204588931,
+                    "angularVelocity": -0.7021878198118158,
+                    "velocityX": 1.4339128687006073,
+                    "velocityY": -0.46484156733002446,
+                    "timestamp": 1.8933656567402217
+                },
+                {
+                    "x": 5.11330055140079,
+                    "y": 1.6403147844077772,
+                    "heading": -0.6080594902506735,
+                    "angularVelocity": -0.5712504187662296,
+                    "velocityX": 1.1468358682094448,
+                    "velocityY": -0.37169072763413913,
+                    "timestamp": 1.9298265239492614
+                },
+                {
+                    "x": 5.1446544158470235,
+                    "y": 1.6301549523614358,
+                    "heading": -0.6239033665029361,
+                    "angularVelocity": -0.43454469037794713,
+                    "velocityX": 0.859931944747064,
+                    "velocityY": -0.2786503126240163,
+                    "timestamp": 1.966287391158301
+                },
+                {
+                    "x": 5.165552752490355,
+                    "y": 1.6233842390024875,
+                    "heading": -0.6345958555798376,
+                    "angularVelocity": -0.29325931869912014,
+                    "velocityX": 0.5731716835893133,
+                    "velocityY": -0.18569808886140712,
+                    "timestamp": 2.0027482583673404
                 },
                 {
                     "x": 5.176,
                     "y": 1.62,
                     "heading": -0.64,
-                    "angularVelocity": -0.14821767070941108,
-                    "velocityX": 0.28653320646240676,
-                    "velocityY": -0.09281825037833577,
-                    "timestamp": 2.039209125430469
+                    "angularVelocity": -0.14821766002380998,
+                    "velocityX": 0.2865331603263514,
+                    "velocityY": -0.09281839027812194,
+                    "timestamp": 2.03920912557638
                 },
                 {
                     "x": 5.176,
                     "y": 1.62,
                     "heading": -0.64,
-                    "angularVelocity": 2.131559152063316e-30,
-                    "velocityX": 2.0336113095175017e-32,
-                    "velocityY": 7.340802046004562e-32,
-                    "timestamp": 2.075669992787111
+                    "angularVelocity": 2.046404654845657e-31,
+                    "velocityX": 2.793064044552828e-33,
+                    "velocityY": 5.221456528298987e-32,
+                    "timestamp": 2.0756699927854196
                 }
             ],
             "constraints": [
@@ -15040,11 +15170,6 @@
                     "x": 5.61,
                     "y": 2.81,
                     "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
                 }
             ]
         },
@@ -18584,7 +18709,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 12
+                    "controlIntervalCount": 11
+                },
+                {
+                    "x": 2.2382969856262207,
+                    "y": 2.9569339752197266,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 13
                 },
                 {
                     "x": 4.263,
@@ -18601,118 +18735,226 @@
                     "x": 2.3,
                     "y": 4.36,
                     "heading": -0.39,
-                    "angularVelocity": 1.6493730429010574e-29,
-                    "velocityX": 1.2833975714085328e-29,
-                    "velocityY": -2.2715148176984883e-30,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.2589715401081683,
-                    "y": 4.250531667444243,
-                    "heading": -0.30389257282100673,
-                    "angularVelocity": 0.7199270532350494,
-                    "velocityX": -0.3430307837128689,
-                    "velocityY": -0.9152429315492105,
-                    "timestamp": 0.11960576671208706
-                },
-                {
-                    "x": 2.20035382258247,
-                    "y": 4.024377658967842,
-                    "heading": -0.15502297939398071,
-                    "angularVelocity": 1.244669028252838,
-                    "velocityX": -0.49009106447848483,
-                    "velocityY": -1.890828633880168,
-                    "timestamp": 0.23921153342417412
-                },
-                {
-                    "x": 2.196580641751606,
-                    "y": 3.6923673074709873,
-                    "heading": -0.01977955961101222,
-                    "angularVelocity": 1.1307433036110668,
-                    "velocityX": -0.031546813624408795,
-                    "velocityY": -2.7758724401313977,
-                    "timestamp": 0.3588173001362612
-                },
-                {
-                    "x": 2.3070960866935795,
-                    "y": 3.359465621426977,
-                    "heading": -0.010919467382492536,
-                    "angularVelocity": 0.07407746693291484,
-                    "velocityX": 0.9239976297129873,
-                    "velocityY": -2.7833247108007275,
-                    "timestamp": 0.47842306684834823
-                },
-                {
-                    "x": 2.522207133394474,
-                    "y": 3.0732241883923836,
-                    "heading": -0.12115396742434953,
-                    "angularVelocity": -0.9216487053427286,
-                    "velocityX": 1.7985006293108312,
-                    "velocityY": -2.3932076262144095,
-                    "timestamp": 0.5980288335604353
-                },
-                {
-                    "x": 2.8293982195436245,
-                    "y": 2.8628067869682225,
-                    "heading": -0.23050381325401667,
-                    "angularVelocity": -0.9142522876239637,
-                    "velocityX": 2.5683635044839206,
-                    "velocityY": -1.759257995734114,
-                    "timestamp": 0.7176346002725225
-                },
-                {
-                    "x": 3.2122552089453738,
-                    "y": 2.74459738527869,
-                    "heading": -0.337506021977117,
-                    "angularVelocity": -0.8946241612299765,
-                    "velocityX": 3.200991055250252,
-                    "velocityY": -0.9883252700855956,
-                    "timestamp": 0.8372403669846096
-                },
-                {
-                    "x": 3.6197114657100724,
-                    "y": 2.743095325026798,
-                    "heading": -0.4390431608485269,
-                    "angularVelocity": -0.8489318003875522,
-                    "velocityX": 3.406660631552456,
-                    "velocityY": -0.012558426681036643,
-                    "timestamp": 0.9568461336966967
-                },
-                {
-                    "x": 3.949401767099036,
-                    "y": 2.832014285774074,
-                    "heading": -0.5329543188084961,
-                    "angularVelocity": -0.7851724924441524,
-                    "velocityX": 2.7564749631394143,
-                    "velocityY": 0.7434337255770672,
-                    "timestamp": 1.0764519004087838
-                },
-                {
-                    "x": 4.162198776267108,
-                    "y": 2.936236025183353,
-                    "heading": -0.5618569264034092,
-                    "angularVelocity": -0.24164894711549667,
-                    "velocityX": 1.7791534222619447,
-                    "velocityY": 0.8713772109346234,
-                    "timestamp": 1.196057667120871
+                    "x": 2.284359298751307,
+                    "y": 4.3328670624584875,
+                    "heading": -0.3913603360985968,
+                    "angularVelocity": -0.02219557362709146,
+                    "velocityX": -0.2551974739939649,
+                    "velocityY": -0.44270758788444614,
+                    "timestamp": 0.061288620940906494
+                },
+                {
+                    "x": 2.2541133789559487,
+                    "y": 4.2780305351291235,
+                    "heading": -0.3941996942145126,
+                    "angularVelocity": -0.04632765548197678,
+                    "velocityX": -0.4934997611468709,
+                    "velocityY": -0.89472607618691,
+                    "timestamp": 0.12257724188181299
+                },
+                {
+                    "x": 2.2108693669713233,
+                    "y": 4.194701771021148,
+                    "heading": -0.39869373638946143,
+                    "angularVelocity": -0.0733258818024626,
+                    "velocityX": -0.7055797849705345,
+                    "velocityY": -1.3596123200148318,
+                    "timestamp": 0.1838658628227195
+                },
+                {
+                    "x": 2.157413012714945,
+                    "y": 4.081769184110331,
+                    "heading": -0.40512443208979565,
+                    "angularVelocity": -0.1049247903054391,
+                    "velocityX": -0.8722068376757797,
+                    "velocityY": -1.8426354709417345,
+                    "timestamp": 0.24515448376362597
+                },
+                {
+                    "x": 2.099439422754663,
+                    "y": 3.9378510886177804,
+                    "heading": -0.41398926531008917,
+                    "angularVelocity": -0.1446407682894496,
+                    "velocityX": -0.9459111507204496,
+                    "velocityY": -2.348202542056109,
+                    "timestamp": 0.30644310470453245
+                },
+                {
+                    "x": 2.05084134036617,
+                    "y": 3.764062564560248,
+                    "heading": -0.42610260644735526,
+                    "angularVelocity": -0.1976442111325286,
+                    "velocityX": -0.7929380958881483,
+                    "velocityY": -2.835575697242347,
+                    "timestamp": 0.36773172564543893
+                },
+                {
+                    "x": 2.0313297442556637,
+                    "y": 3.578686605657539,
+                    "heading": -0.44115755081157715,
+                    "angularVelocity": -0.24564012263773397,
+                    "velocityX": -0.3183559331400068,
+                    "velocityY": -3.024639093795978,
+                    "timestamp": 0.4290203465863454
+                },
+                {
+                    "x": 2.0424292488902682,
+                    "y": 3.3999266361315956,
+                    "heading": -0.45764648760363913,
+                    "angularVelocity": -0.2690374907923018,
+                    "velocityX": 0.18110220892890863,
+                    "velocityY": -2.9166910069375023,
+                    "timestamp": 0.4903089675272519
+                },
+                {
+                    "x": 2.0816987250314285,
+                    "y": 3.234855796713624,
+                    "heading": -0.4747853916237668,
+                    "angularVelocity": -0.27964251368378285,
+                    "velocityX": 0.6407302944379066,
+                    "velocityY": -2.6933358408754287,
+                    "timestamp": 0.5515975884681584
+                },
+                {
+                    "x": 2.1473776050382467,
+                    "y": 3.086622748034336,
+                    "heading": -0.49218081575104855,
+                    "angularVelocity": -0.2838279579508591,
+                    "velocityX": 1.071632531431,
+                    "velocityY": -2.418606364502998,
+                    "timestamp": 0.6128862094090649
+                },
+                {
+                    "x": 2.2382969856262207,
+                    "y": 2.9569339752197266,
+                    "heading": -0.5096053711064815,
+                    "angularVelocity": -0.2843032701328595,
+                    "velocityX": 1.4834626590087072,
+                    "velocityY": -2.116033463041253,
+                    "timestamp": 0.6741748303499714
+                },
+                {
+                    "x": 2.3750291535583634,
+                    "y": 2.833789120602945,
+                    "heading": -0.5294729435704262,
+                    "angularVelocity": -0.282105779182567,
+                    "velocityX": 1.9415021560293924,
+                    "velocityY": -1.7485717103605045,
+                    "timestamp": 0.7446007979480589
+                },
+                {
+                    "x": 2.5431654959507464,
+                    "y": 2.737552434720476,
+                    "heading": -0.5488979650801319,
+                    "angularVelocity": -0.2758218619098283,
+                    "velocityX": 2.3874196993915153,
+                    "velocityY": -1.366494336743516,
+                    "timestamp": 0.8150267655461465
+                },
+                {
+                    "x": 2.7413669689925393,
+                    "y": 2.669711436345154,
+                    "heading": -0.5674506234763109,
+                    "angularVelocity": -0.2634349094364851,
+                    "velocityX": 2.8143237473555907,
+                    "velocityY": -0.9632952260234809,
+                    "timestamp": 0.8854527331442341
+                },
+                {
+                    "x": 2.9672720655352354,
+                    "y": 2.6325722400915463,
+                    "heading": -0.5844267226661307,
+                    "angularVelocity": -0.24104885979984425,
+                    "velocityX": 3.207696028145596,
+                    "velocityY": -0.5273508837756647,
+                    "timestamp": 0.9558787007423217
+                },
+                {
+                    "x": 3.21586763898004,
+                    "y": 2.629998478445332,
+                    "heading": -0.5985057428976699,
+                    "angularVelocity": -0.19991234358165255,
+                    "velocityX": 3.529885096694866,
+                    "velocityY": -0.03654563414595668,
+                    "timestamp": 1.0263046683404093
+                },
+                {
+                    "x": 3.4730987079905056,
+                    "y": 2.6678365706373897,
+                    "heading": -0.6068355688677516,
+                    "angularVelocity": -0.11827776392962061,
+                    "velocityX": 3.652503157336117,
+                    "velocityY": 0.5372747224148254,
+                    "timestamp": 1.096730635938497
+                },
+                {
+                    "x": 3.704150591962856,
+                    "y": 2.737591563426048,
+                    "heading": -0.6062385008661115,
+                    "angularVelocity": 0.00847795240879764,
+                    "velocityX": 3.28077684769537,
+                    "velocityY": 0.9904726220694814,
+                    "timestamp": 1.1671566035365846
+                },
+                {
+                    "x": 3.894448488053554,
+                    "y": 2.8141322684658854,
+                    "heading": -0.5991077413907381,
+                    "angularVelocity": 0.10125184954600595,
+                    "velocityX": 2.702098424500253,
+                    "velocityY": 1.086825039829707,
+                    "timestamp": 1.2375825711346722
+                },
+                {
+                    "x": 4.043960756386648,
+                    "y": 2.883897596305393,
+                    "heading": -0.5885679211534333,
+                    "angularVelocity": 0.1496581530473843,
+                    "velocityX": 2.122970736963697,
+                    "velocityY": 0.9906193726389452,
+                    "timestamp": 1.3080085387327598
+                },
+                {
+                    "x": 4.154363809487208,
+                    "y": 2.940218619761556,
+                    "heading": -0.5783208756256007,
+                    "angularVelocity": 0.14550095479427785,
+                    "velocityX": 1.5676469470837475,
+                    "velocityY": 0.7997195548321091,
+                    "timestamp": 1.3784345063308474
+                },
+                {
+                    "x": 4.227038806897989,
+                    "y": 2.9796144762337837,
+                    "heading": -0.5703846528919979,
+                    "angularVelocity": 0.11268887037369305,
+                    "velocityX": 1.0319346668480025,
+                    "velocityY": 0.5593938971070241,
+                    "timestamp": 1.448860473928935
                 },
                 {
                     "x": 4.263,
                     "y": 3,
                     "heading": -0.566,
-                    "angularVelocity": -0.03463941338677448,
-                    "velocityX": 0.8427789604454271,
-                    "velocityY": 0.5331178969835093,
-                    "timestamp": 1.315663433832958
+                    "angularVelocity": 0.06225903656760019,
+                    "velocityX": 0.5106240542868632,
+                    "velocityY": 0.2894603292148455,
+                    "timestamp": 1.5192864415270226
                 },
                 {
                     "x": 4.263,
                     "y": 3,
                     "heading": -0.566,
-                    "angularVelocity": 0,
-                    "velocityX": 1.4642265257925552e-30,
-                    "velocityY": 5.326911647277941e-29,
-                    "timestamp": 1.4352692005450451
+                    "angularVelocity": 9.24329495349137e-32,
+                    "velocityX": 0,
+                    "velocityY": 1.2740379664097394e-33,
+                    "timestamp": 1.5897124091251102
                 }
             ],
             "constraints": [
@@ -29900,7 +30142,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 23
+                    "controlIntervalCount": 20
                 },
                 {
                     "x": 2.279822826385498,
@@ -29909,7 +30151,7 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 9
                 },
                 {
                     "x": 2.3,
@@ -29921,7 +30163,278 @@
                     "controlIntervalCount": 40
                 }
             ],
-            "trajectory": [],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 0,
+                    "angularVelocity": -1.8418609272007183e-28,
+                    "velocityX": -1.1316944174618986e-32,
+                    "velocityY": -1.0903166764521522e-29,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.615058283165484,
+                    "y": 7.4500526040017725,
+                    "heading": -0.011528934363875379,
+                    "angularVelocity": -0.12842736126317786,
+                    "velocityX": -0.7234227436435784,
+                    "velocityY": -0.18878668917675853,
+                    "timestamp": 0.08977007898235294
+                },
+                {
+                    "x": 7.485153650546927,
+                    "y": 7.41615410854214,
+                    "heading": -0.0328638632229319,
+                    "angularVelocity": -0.23766191476385995,
+                    "velocityX": -1.4470816344507456,
+                    "velocityY": -0.3776146333375848,
+                    "timestamp": 0.17954015796470588
+                },
+                {
+                    "x": 7.290257872847477,
+                    "y": 7.365300387326372,
+                    "heading": -0.06109799257845879,
+                    "angularVelocity": -0.3145160355888266,
+                    "velocityX": -2.17105498746146,
+                    "velocityY": -0.5664885426438214,
+                    "timestamp": 0.2693102369470588
+                },
+                {
+                    "x": 7.030342832843462,
+                    "y": 7.297491054945299,
+                    "heading": -0.0902650461777,
+                    "angularVelocity": -0.32490840968750134,
+                    "velocityX": -2.895341554228826,
+                    "velocityY": -0.7553667452421856,
+                    "timestamp": 0.35908031592941175
+                },
+                {
+                    "x": 6.705594231461252,
+                    "y": 7.212800171241254,
+                    "heading": -0.10097705422378679,
+                    "angularVelocity": -0.11932715407194204,
+                    "velocityX": -3.617559492690666,
+                    "velocityY": -0.9434199531077018,
+                    "timestamp": 0.4488503949117647
+                },
+                {
+                    "x": 6.377014064882571,
+                    "y": 7.1270111914036525,
+                    "heading": -0.10097706428057987,
+                    "angularVelocity": -1.120283753063034e-7,
+                    "velocityX": -3.6602414780458985,
+                    "velocityY": -0.9556522708907337,
+                    "timestamp": 0.5386204738941176
+                },
+                {
+                    "x": 6.048433901032778,
+                    "y": 7.0412222011141505,
+                    "heading": -0.10097707433737516,
+                    "angularVelocity": -1.1202840770590317e-7,
+                    "velocityX": -3.660241447647229,
+                    "velocityY": -0.9556523873204973,
+                    "timestamp": 0.6283905528764705
+                },
+                {
+                    "x": 5.719853740215168,
+                    "y": 6.955433199211206,
+                    "heading": -0.10097708439415136,
+                    "angularVelocity": -1.1202819821797078e-7,
+                    "velocityX": -3.6602414138700015,
+                    "velocityY": -0.9556525166893554,
+                    "timestamp": 0.7181606318588234
+                },
+                {
+                    "x": 5.391273582926602,
+                    "y": 6.86964418379178,
+                    "heading": -0.100977094450907,
+                    "angularVelocity": -1.1202797265919916e-7,
+                    "velocityX": -3.660241374557919,
+                    "velocityY": -0.9556526672572584,
+                    "timestamp": 0.8079307108411763
+                },
+                {
+                    "x": 5.062693429765362,
+                    "y": 6.783855152564382,
+                    "heading": -0.10097710450764096,
+                    "angularVelocity": -1.1202773594839044e-7,
+                    "velocityX": -3.6602413285812334,
+                    "velocityY": -0.9556528433514181,
+                    "timestamp": 0.8977007898235292
+                },
+                {
+                    "x": 4.734113281444647,
+                    "y": 6.698066102797359,
+                    "heading": -0.10097711456435234,
+                    "angularVelocity": -1.1202748993058552e-7,
+                    "velocityX": -3.6602412746598207,
+                    "velocityY": -0.9556530498750426,
+                    "timestamp": 0.9874708688058821
+                },
+                {
+                    "x": 4.4055331387963745,
+                    "y": 6.612277031304317,
+                    "heading": -0.10097712462104143,
+                    "angularVelocity": -1.1202724880866421e-7,
+                    "velocityX": -3.660241211471189,
+                    "velocityY": -0.9556532918937597,
+                    "timestamp": 1.077240947788235
+                },
+                {
+                    "x": 4.076953002755506,
+                    "y": 6.526487934504167,
+                    "heading": -0.10097713467770983,
+                    "angularVelocity": -1.1202702659530083e-7,
+                    "velocityX": -3.6602411378675055,
+                    "velocityY": -0.9556535738043043,
+                    "timestamp": 1.167011026770588
+                },
+                {
+                    "x": 3.748372874246301,
+                    "y": 6.440698808855463,
+                    "heading": -0.10097714473442117,
+                    "angularVelocity": -1.1202751392033635e-7,
+                    "velocityX": -3.6602410539684915,
+                    "velocityY": -0.9556538951629009,
+                    "timestamp": 1.256781105752941
+                },
+                {
+                    "x": 3.4197928479820465,
+                    "y": 6.354909291415878,
+                    "heading": -0.10097715479114391,
+                    "angularVelocity": -1.120276421337113e-7,
+                    "velocityX": -3.6602399150000573,
+                    "velocityY": -0.9556582595606669,
+                    "timestamp": 1.346551184735294
+                },
+                {
+                    "x": 3.1047619528127512,
+                    "y": 6.228101525660344,
+                    "heading": -0.10097716526412484,
+                    "angularVelocity": -1.1666457370126234e-7,
+                    "velocityX": -3.5093084326123236,
+                    "velocityY": -1.412583871971558,
+                    "timestamp": 1.436321263717647
+                },
+                {
+                    "x": 2.8208656132222933,
+                    "y": 6.041748259045348,
+                    "heading": -0.1009771779112871,
+                    "angularVelocity": -1.4088397962304153e-7,
+                    "velocityX": -3.1624828986324123,
+                    "velocityY": -2.0758950947495576,
+                    "timestamp": 1.5260913427
+                },
+                {
+                    "x": 2.5792187192315943,
+                    "y": 5.803145197833315,
+                    "heading": -0.10097728674036179,
+                    "angularVelocity": -0.0000012123108078932896,
+                    "velocityX": -2.691842278966871,
+                    "velocityY": -2.65793529332427,
+                    "timestamp": 1.615861421682353
+                },
+                {
+                    "x": 2.3963290188287294,
+                    "y": 5.533479989641919,
+                    "heading": -0.13644335482170092,
+                    "angularVelocity": -0.3950767169047361,
+                    "velocityX": -2.037312459521214,
+                    "velocityY": -3.003954226707092,
+                    "timestamp": 1.705631500664706
+                },
+                {
+                    "x": 2.279822826385498,
+                    "y": 5.254662990570068,
+                    "heading": -0.18962986506951118,
+                    "angularVelocity": -0.5924748073105779,
+                    "velocityX": -1.2978287839789375,
+                    "velocityY": -3.105901233821009,
+                    "timestamp": 1.795401579647059
+                },
+                {
+                    "x": 2.2328990354823257,
+                    "y": 5.0773593431323425,
+                    "heading": -0.225508532252886,
+                    "angularVelocity": -0.6284306472549296,
+                    "velocityX": -0.8218908506157174,
+                    "velocityY": -3.1055514229095325,
+                    "timestamp": 1.8524940644563022
+                },
+                {
+                    "x": 2.2122650091837905,
+                    "y": 4.906948206481987,
+                    "heading": -0.2618449581428327,
+                    "angularVelocity": -0.636448492486558,
+                    "velocityX": -0.3614140524419242,
+                    "velocityY": -2.9848260628208765,
+                    "timestamp": 1.9095865492655453
+                },
+                {
+                    "x": 2.2138710973194917,
+                    "y": 4.752151212947765,
+                    "heading": -0.29639527524079207,
+                    "angularVelocity": -0.6051640108688915,
+                    "velocityX": 0.02813134059521416,
+                    "velocityY": -2.7113374737719043,
+                    "timestamp": 1.9666790340747884
+                },
+                {
+                    "x": 2.229883400142308,
+                    "y": 4.620372877512837,
+                    "heading": -0.32691110500805654,
+                    "angularVelocity": -0.5344981895462751,
+                    "velocityX": 0.280462531565201,
+                    "velocityY": -2.308155545781648,
+                    "timestamp": 2.0237715188840317
+                },
+                {
+                    "x": 2.251883940293385,
+                    "y": 4.51506115985361,
+                    "heading": -0.3519685302004407,
+                    "angularVelocity": -0.4388918309689961,
+                    "velocityX": 0.38534914402167997,
+                    "velocityY": -1.844581086479138,
+                    "timestamp": 2.080864003693275
+                },
+                {
+                    "x": 2.273515445313811,
+                    "y": 4.436872238113893,
+                    "heading": -0.37095335253129014,
+                    "angularVelocity": -0.33252751906728534,
+                    "velocityX": 0.3788853312819565,
+                    "velocityY": -1.3695133781784317,
+                    "timestamp": 2.1379564885025184
+                },
+                {
+                    "x": 2.2905088224909966,
+                    "y": 4.385405157743103,
+                    "heading": -0.3836505931421081,
+                    "angularVelocity": -0.22239775783770355,
+                    "velocityX": 0.2976464806908316,
+                    "velocityY": -0.9014685653065743,
+                    "timestamp": 2.1950489733117617
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": -0.39,
+                    "angularVelocity": -0.11121265573212626,
+                    "velocityX": 0.16624215149881386,
+                    "velocityY": -0.4449825196428863,
+                    "timestamp": 2.252141458121005
+                },
+                {
+                    "x": 2.3,
+                    "y": 4.36,
+                    "heading": -0.39,
+                    "angularVelocity": -5.463060715454085e-29,
+                    "velocityX": 0,
+                    "velocityY": -5.448481045128469e-30,
+                    "timestamp": 2.3092339429302484
+                }
+            ],
             "constraints": [
                 {
                     "scope": [
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.1.traj
index 137c9bbd..b2886244 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.1.traj	
@@ -5,270 +5,270 @@
             "y": 6.72,
             "heading": 1.04,
             "angularVelocity": 0,
-            "velocityX": -1.8357629106361847e-33,
+            "velocityX": -1.8098951472235817e-30,
             "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.7613816250456615,
-            "y": 6.725489542887092,
-            "heading": 0.9931061774124983,
-            "angularVelocity": -0.5863053943309177,
-            "velocityX": 0.6424156162050443,
-            "velocityY": 0.06863481007775876,
-            "timestamp": 0.07998190540446273
+            "x": 0.7613816362208097,
+            "y": 6.725489442192221,
+            "heading": 0.9931061799728784,
+            "angularVelocity": -0.5863053623193526,
+            "velocityX": 0.6424157559264662,
+            "velocityY": 0.06863355110715653,
+            "timestamp": 0.07998190540440472
         },
         {
-            "x": 0.8643774278996095,
-            "y": 6.736651981527731,
-            "heading": 0.9029653992770575,
-            "angularVelocity": -1.1270146376184127,
-            "velocityX": 1.2877387995835512,
-            "velocityY": 0.13956204949346018,
-            "timestamp": 0.15996381080892547
+            "x": 0.8643774617687672,
+            "y": 6.736651681634022,
+            "heading": 0.9029654144342619,
+            "angularVelocity": -1.127014480123304,
+            "velocityX": 1.287739083323781,
+            "velocityY": 0.1395595589447693,
+            "timestamp": 0.15996381080880945
         },
         {
-            "x": 1.019343252130834,
-            "y": 6.753670139482374,
-            "heading": 0.7755395431364547,
-            "angularVelocity": -1.5931835519073916,
-            "velocityX": 1.9375110338716452,
-            "velocityY": 0.21277510042531103,
-            "timestamp": 0.2399457162133882
+            "x": 1.019343320389449,
+            "y": 6.753669544326843,
+            "heading": 0.7755395878520636,
+            "angularVelocity": -1.5931831823449016,
+            "velocityX": 1.9375114638385158,
+            "velocityY": 0.21277140881771536,
+            "timestamp": 0.23994571621321417
         },
         {
-            "x": 1.2269177512246652,
-            "y": 6.77676689021641,
-            "heading": 0.6253872743553933,
-            "angularVelocity": -1.8773279783940144,
-            "velocityX": 2.5952682427874385,
-            "velocityY": 0.2887746999429258,
-            "timestamp": 0.31992762161785093
+            "x": 1.2269178654194646,
+            "y": 6.7767659053404685,
+            "heading": 0.62538736801047,
+            "angularVelocity": -1.8773273665136312,
+            "velocityX": 2.5952688171215317,
+            "velocityY": 0.2887698273359004,
+            "timestamp": 0.3199276216176189
         },
         {
-            "x": 1.4861215960194483,
-            "y": 6.806717977426302,
-            "heading": 0.5166968690593423,
-            "angularVelocity": -1.358937433990995,
-            "velocityX": 3.240781067717853,
-            "velocityY": 0.37447328940753305,
-            "timestamp": 0.39990952702231364
+            "x": 1.4861217663901574,
+            "y": 6.806716511012647,
+            "heading": 0.5166970392894589,
+            "angularVelocity": -1.3589364765874345,
+            "velocityX": 3.240781770077933,
+            "velocityY": 0.37446726882464465,
+            "timestamp": 0.39990952702202365
         },
         {
-            "x": 1.7870123176597594,
-            "y": 6.838521008932437,
-            "heading": 0.5166968273679649,
-            "angularVelocity": -5.212601175914625e-7,
-            "velocityX": 3.7619849154471687,
-            "velocityY": 0.397627830261236,
-            "timestamp": 0.47989143242677634
+            "x": 1.7870125383049598,
+            "y": 6.838519066862702,
+            "heading": 0.5166969975980924,
+            "angularVelocity": -5.212599803241117e-7,
+            "velocityX": 3.76198554402321,
+            "velocityY": 0.39762188321538583,
+            "timestamp": 0.4798914324264284
         },
         {
-            "x": 2.087903104978946,
-            "y": 6.870323419047386,
-            "heading": 0.5166967856787098,
-            "angularVelocity": -5.212335818457529e-7,
-            "velocityX": 3.7619857366188425,
-            "velocityY": 0.3976200611141572,
-            "timestamp": 0.559873337831239
+            "x": 2.0879033719171214,
+            "y": 6.870321038982823,
+            "heading": 0.5166969559088411,
+            "angularVelocity": -5.212335340785354e-7,
+            "velocityX": 3.761986315414672,
+            "velocityY": 0.39761458494048757,
+            "timestamp": 0.5598733378308332
         },
         {
-            "x": 2.3887938966166886,
-            "y": 6.9021257883033815,
-            "heading": 0.5166967439894641,
-            "angularVelocity": -5.212334661065162e-7,
-            "velocityX": 3.761985790613012,
-            "velocityY": 0.3976195502616874,
-            "timestamp": 0.6398552432357018
+            "x": 2.3887942055296523,
+            "y": 6.902123011099449,
+            "heading": 0.5166969142195897,
+            "angularVelocity": -5.212335365150009e-7,
+            "velocityX": 3.7619863154192914,
+            "velocityY": 0.3976145848967806,
+            "timestamp": 0.6398552432352379
         },
         {
-            "x": 2.689684692935055,
-            "y": 6.933928113274756,
-            "heading": 0.5166967023002289,
-            "angularVelocity": -5.212333328622992e-7,
-            "velocityX": 3.76198584913404,
-            "velocityY": 0.3976189965786884,
-            "timestamp": 0.7198371486401645
+            "x": 2.689685039142183,
+            "y": 6.933924983216074,
+            "heading": 0.516696872530338,
+            "angularVelocity": -5.212335398189914e-7,
+            "velocityX": 3.7619863154192914,
+            "velocityY": 0.3976145848967804,
+            "timestamp": 0.7198371486396427
         },
         {
-            "x": 2.990575494322058,
-            "y": 6.965730390290324,
-            "heading": 0.5166966606110028,
-            "angularVelocity": -5.212332194068621e-7,
-            "velocityX": 3.761985912506342,
-            "velocityY": 0.3976183969955037,
-            "timestamp": 0.7998190540446272
+            "x": 2.990575872754714,
+            "y": 6.9657269553327,
+            "heading": 0.5166968308410861,
+            "angularVelocity": -5.212335433606452e-7,
+            "velocityX": 3.7619863154192914,
+            "velocityY": 0.39761458489678053,
+            "timestamp": 0.7998190540440474
         },
         {
-            "x": 3.291466301153434,
-            "y": 6.997532615795069,
-            "heading": 0.5166966189217854,
-            "angularVelocity": -5.21233112050001e-7,
-            "velocityX": 3.76198598057639,
-            "velocityY": 0.39761775296452984,
-            "timestamp": 0.8798009594490899
+            "x": 3.291466706367245,
+            "y": 6.997528927449325,
+            "heading": 0.5166967891518339,
+            "angularVelocity": -5.212335460884143e-7,
+            "velocityX": 3.761986315419291,
+            "velocityY": 0.3976145848967808,
+            "timestamp": 0.8798009594484522
         },
         {
-            "x": 3.592357113798132,
-            "y": 7.029334786298146,
-            "heading": 0.5166965772325762,
-            "angularVelocity": -5.212330086999819e-7,
-            "velocityX": 3.7619860532593528,
-            "velocityY": 0.39761706528816065,
-            "timestamp": 0.9597828648535526
+            "x": 3.5923575399797762,
+            "y": 7.0293308995659505,
+            "heading": 0.5166967474625817,
+            "angularVelocity": -5.212335479348436e-7,
+            "velocityX": 3.761986315419291,
+            "velocityY": 0.3976145848967805,
+            "timestamp": 0.9597828648528569
         },
         {
-            "x": 3.8932479326255467,
-            "y": 7.061136898304478,
-            "heading": 0.5166965355433745,
-            "angularVelocity": -5.212329156376395e-7,
-            "velocityX": 3.7619861305608038,
-            "velocityY": 0.3976163339134084,
-            "timestamp": 1.0397647702580153
+            "x": 3.893248373592307,
+            "y": 7.0611328716825765,
+            "heading": 0.516696705773329,
+            "angularVelocity": -5.212335516712931e-7,
+            "velocityX": 3.7619863154192905,
+            "velocityY": 0.3976145848967809,
+            "timestamp": 1.0397647702572617
         },
         {
-            "x": 4.194138758019097,
-            "y": 7.092938948186284,
-            "heading": 0.5166964938541775,
-            "angularVelocity": -5.212328561470798e-7,
-            "velocityX": 3.7619862126560695,
-            "velocityY": 0.3976155571811608,
-            "timestamp": 1.119746675662478
+            "x": 4.194139207204837,
+            "y": 7.092934843799203,
+            "heading": 0.5166966640840763,
+            "angularVelocity": -5.212335535473894e-7,
+            "velocityX": 3.7619863154192905,
+            "velocityY": 0.3976145848967809,
+            "timestamp": 1.1197466756616663
         },
         {
-            "x": 4.495029590361164,
-            "y": 7.1247409323255795,
-            "heading": 0.516696452164982,
-            "angularVelocity": -5.212328389753682e-7,
-            "velocityX": 3.7619862995321793,
-            "velocityY": 0.397614735213847,
-            "timestamp": 1.199728581066941
+            "x": 4.495030040817368,
+            "y": 7.1247368159158295,
+            "heading": 0.5166966223948233,
+            "angularVelocity": -5.212335568545237e-7,
+            "velocityX": 3.7619863154192905,
+            "velocityY": 0.3976145848967808,
+            "timestamp": 1.199728581066071
         },
         {
-            "x": 4.79592042996658,
-            "y": 7.15654284774347,
-            "heading": 0.5166964104757901,
-            "angularVelocity": -5.212327914819509e-7,
-            "velocityX": 3.7619863903445805,
-            "velocityY": 0.39761387600196396,
-            "timestamp": 1.2797104864714037
+            "x": 4.7959208744299,
+            "y": 7.156538788032456,
+            "heading": 0.51669658070557,
+            "angularVelocity": -5.212335599678648e-7,
+            "velocityX": 3.7619863154192905,
+            "velocityY": 0.397614584896781,
+            "timestamp": 1.2797104864704756
         },
         {
-            "x": 5.096811277131865,
-            "y": 7.188344691634321,
-            "heading": 0.5166963687866055,
-            "angularVelocity": -5.212326996843497e-7,
-            "velocityX": 3.7619864848642997,
-            "velocityY": 0.39761298171169474,
-            "timestamp": 1.3596923918758665
+            "x": 5.096811708042431,
+            "y": 7.1883407601490825,
+            "heading": 0.5166965390163165,
+            "angularVelocity": -5.212335628762146e-7,
+            "velocityX": 3.7619863154192905,
+            "velocityY": 0.3976145848967809,
+            "timestamp": 1.3596923918748802
         },
         {
-            "x": 5.397702132107731,
-            "y": 7.220146461625841,
-            "heading": 0.5166963270974331,
-            "angularVelocity": -5.212325496997691e-7,
-            "velocityX": 3.761986582518669,
-            "velocityY": 0.3976120577610798,
-            "timestamp": 1.4396742972803294
+            "x": 5.3977025416549615,
+            "y": 7.220142732265709,
+            "heading": 0.5166964973270628,
+            "angularVelocity": -5.212335655841188e-7,
+            "velocityX": 3.76198631541929,
+            "velocityY": 0.3976145848967813,
+            "timestamp": 1.4396742972792849
         },
         {
-            "x": 5.698592995103496,
-            "y": 7.251948155737401,
-            "heading": 0.516696285408269,
-            "angularVelocity": -5.212324447299826e-7,
-            "velocityX": 3.7619866827900856,
-            "velocityY": 0.3976111090469843,
-            "timestamp": 1.5196562026847922
+            "x": 5.698593375267492,
+            "y": 7.251944704382335,
+            "heading": 0.5166964556378089,
+            "angularVelocity": -5.212335680260379e-7,
+            "velocityX": 3.7619863154192896,
+            "velocityY": 0.3976145848967809,
+            "timestamp": 1.5196562026836895
         },
         {
-            "x": 5.99948386596862,
-            "y": 7.283749775393132,
-            "heading": 0.5166962437191063,
-            "angularVelocity": -5.212324282706134e-7,
-            "velocityX": 3.7619867811793206,
-            "velocityY": 0.3976101781385786,
-            "timestamp": 1.599638108089255
+            "x": 5.999484208880023,
+            "y": 7.283746676498962,
+            "heading": 0.5166964139485547,
+            "angularVelocity": -5.212335712969897e-7,
+            "velocityX": 3.7619863154192896,
+            "velocityY": 0.3976145848967812,
+            "timestamp": 1.5996381080880941
         },
         {
-            "x": 6.300374744112002,
-            "y": 7.315551326185539,
-            "heading": 0.5166962020299369,
-            "angularVelocity": -5.212325087463018e-7,
-            "velocityX": 3.761986872178135,
-            "velocityY": 0.3976093171522739,
-            "timestamp": 1.6796200134937178
+            "x": 6.300375042492554,
+            "y": 7.315548648615588,
+            "heading": 0.5166963722593003,
+            "angularVelocity": -5.21233573548864e-7,
+            "velocityX": 3.7619863154192896,
+            "velocityY": 0.39761458489678125,
+            "timestamp": 1.6796200134924988
         },
         {
-            "x": 6.6012656287075195,
-            "y": 7.347352815930844,
-            "heading": 0.5166961603407556,
-            "angularVelocity": -5.21232660707814e-7,
-            "velocityX": 3.7619869528480785,
-            "velocityY": 0.39760855389086563,
-            "timestamp": 1.7596019188981806
+            "x": 6.6012658761048035,
+            "y": 7.347350620734872,
+            "heading": 0.5166963305700458,
+            "angularVelocity": -5.212335762453642e-7,
+            "velocityX": 3.7619863154157773,
+            "velocityY": 0.3976145849300086,
+            "timestamp": 1.7596019188969034
         },
         {
-            "x": 6.9021564720244495,
-            "y": 7.37915469622686,
-            "heading": 0.5166961186499832,
-            "angularVelocity": -5.212525529907653e-7,
-            "velocityX": 3.7619864367489835,
-            "velocityY": 0.3976134368792068,
-            "timestamp": 1.8395838243026434
+            "x": 6.902156662813089,
+            "y": 7.3791530366221005,
+            "heading": 0.5166962888792155,
+            "angularVelocity": -5.212532766918364e-7,
+            "velocityX": 3.761985728983584,
+            "velocityY": 0.3976201332842598,
+            "timestamp": 1.839583824301308
         },
         {
-            "x": 7.162090895853125,
-            "y": 7.409612003973996,
-            "heading": 0.405130711689902,
-            "angularVelocity": -1.3948830850665912,
-            "velocityX": 3.2499153716606957,
-            "velocityY": 0.3808024776743751,
-            "timestamp": 1.9195657297071063
+            "x": 7.162091022778527,
+            "y": 7.409610892063227,
+            "heading": 0.4051307797997224,
+            "angularVelocity": -1.3948843618490365,
+            "velocityX": 3.249914573191993,
+            "velocityY": 0.3808093253983604,
+            "timestamp": 1.9195657297057127
         },
         {
-            "x": 7.370036531434433,
-            "y": 7.432975355945854,
-            "heading": 0.2588526808330054,
-            "angularVelocity": -1.8288890483063531,
-            "velocityX": 2.5999084984252714,
-            "velocityY": 0.2921079693427033,
-            "timestamp": 1.999547635111569
+            "x": 7.370036606479622,
+            "y": 7.432974685540162,
+            "heading": 0.25885269617169443,
+            "angularVelocity": -1.8288897080960538,
+            "velocityX": 2.5999078497777726,
+            "velocityY": 0.2921134894049214,
+            "timestamp": 1.9995476351101173
         },
         {
-            "x": 7.52532693851905,
-            "y": 7.450135904743354,
-            "heading": 0.13353023499549588,
-            "angularVelocity": -1.5668849748422837,
-            "velocityX": 1.9415692374334488,
-            "velocityY": 0.21455538863097798,
-            "timestamp": 2.079529540516032
+            "x": 7.525326975450784,
+            "y": 7.45013556763307,
+            "heading": 0.1335302309738367,
+            "angularVelocity": -1.5668852169025227,
+            "velocityX": 1.9415687609088876,
+            "velocityY": 0.21455955576625652,
+            "timestamp": 2.079529540514522
         },
         {
-            "x": 7.628525628712187,
-            "y": 7.461415184148821,
-            "heading": 0.04391070884179766,
-            "angularVelocity": -1.120497513787634,
-            "velocityX": 1.2902754650726174,
-            "velocityY": 0.14102288947017258,
-            "timestamp": 2.1595114459204945
+            "x": 7.628525640862077,
+            "y": 7.461415071089343,
+            "heading": 0.043910704401711645,
+            "angularVelocity": -1.1204975190199653,
+            "velocityX": 1.2902751552304177,
+            "velocityY": 0.14102569073894916,
+            "timestamp": 2.159511445918927
         },
         {
             "x": 7.68,
             "y": 7.467,
             "heading": -0.003,
-            "angularVelocity": -0.5865165202625967,
-            "velocityX": 0.6435752065109067,
-            "velocityY": 0.06982599155316412,
-            "timestamp": 2.239493351324957
+            "angularVelocity": -0.5865164647493907,
+            "velocityX": 0.6435750546033896,
+            "velocityY": 0.0698274051164111,
+            "timestamp": 2.2394933513233317
         },
         {
             "x": 7.68,
             "y": 7.467,
             "heading": -0.003,
-            "angularVelocity": -3.4580717798993225e-32,
-            "velocityX": 0,
+            "angularVelocity": 0,
+            "velocityX": 4.312091780947568e-36,
             "velocityY": 0,
-            "timestamp": 2.3194752567294197
+            "timestamp": 2.3194752567277366
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.traj
index 137c9bbd..b2886244 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.traj	
@@ -5,270 +5,270 @@
             "y": 6.72,
             "heading": 1.04,
             "angularVelocity": 0,
-            "velocityX": -1.8357629106361847e-33,
+            "velocityX": -1.8098951472235817e-30,
             "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.7613816250456615,
-            "y": 6.725489542887092,
-            "heading": 0.9931061774124983,
-            "angularVelocity": -0.5863053943309177,
-            "velocityX": 0.6424156162050443,
-            "velocityY": 0.06863481007775876,
-            "timestamp": 0.07998190540446273
+            "x": 0.7613816362208097,
+            "y": 6.725489442192221,
+            "heading": 0.9931061799728784,
+            "angularVelocity": -0.5863053623193526,
+            "velocityX": 0.6424157559264662,
+            "velocityY": 0.06863355110715653,
+            "timestamp": 0.07998190540440472
         },
         {
-            "x": 0.8643774278996095,
-            "y": 6.736651981527731,
-            "heading": 0.9029653992770575,
-            "angularVelocity": -1.1270146376184127,
-            "velocityX": 1.2877387995835512,
-            "velocityY": 0.13956204949346018,
-            "timestamp": 0.15996381080892547
+            "x": 0.8643774617687672,
+            "y": 6.736651681634022,
+            "heading": 0.9029654144342619,
+            "angularVelocity": -1.127014480123304,
+            "velocityX": 1.287739083323781,
+            "velocityY": 0.1395595589447693,
+            "timestamp": 0.15996381080880945
         },
         {
-            "x": 1.019343252130834,
-            "y": 6.753670139482374,
-            "heading": 0.7755395431364547,
-            "angularVelocity": -1.5931835519073916,
-            "velocityX": 1.9375110338716452,
-            "velocityY": 0.21277510042531103,
-            "timestamp": 0.2399457162133882
+            "x": 1.019343320389449,
+            "y": 6.753669544326843,
+            "heading": 0.7755395878520636,
+            "angularVelocity": -1.5931831823449016,
+            "velocityX": 1.9375114638385158,
+            "velocityY": 0.21277140881771536,
+            "timestamp": 0.23994571621321417
         },
         {
-            "x": 1.2269177512246652,
-            "y": 6.77676689021641,
-            "heading": 0.6253872743553933,
-            "angularVelocity": -1.8773279783940144,
-            "velocityX": 2.5952682427874385,
-            "velocityY": 0.2887746999429258,
-            "timestamp": 0.31992762161785093
+            "x": 1.2269178654194646,
+            "y": 6.7767659053404685,
+            "heading": 0.62538736801047,
+            "angularVelocity": -1.8773273665136312,
+            "velocityX": 2.5952688171215317,
+            "velocityY": 0.2887698273359004,
+            "timestamp": 0.3199276216176189
         },
         {
-            "x": 1.4861215960194483,
-            "y": 6.806717977426302,
-            "heading": 0.5166968690593423,
-            "angularVelocity": -1.358937433990995,
-            "velocityX": 3.240781067717853,
-            "velocityY": 0.37447328940753305,
-            "timestamp": 0.39990952702231364
+            "x": 1.4861217663901574,
+            "y": 6.806716511012647,
+            "heading": 0.5166970392894589,
+            "angularVelocity": -1.3589364765874345,
+            "velocityX": 3.240781770077933,
+            "velocityY": 0.37446726882464465,
+            "timestamp": 0.39990952702202365
         },
         {
-            "x": 1.7870123176597594,
-            "y": 6.838521008932437,
-            "heading": 0.5166968273679649,
-            "angularVelocity": -5.212601175914625e-7,
-            "velocityX": 3.7619849154471687,
-            "velocityY": 0.397627830261236,
-            "timestamp": 0.47989143242677634
+            "x": 1.7870125383049598,
+            "y": 6.838519066862702,
+            "heading": 0.5166969975980924,
+            "angularVelocity": -5.212599803241117e-7,
+            "velocityX": 3.76198554402321,
+            "velocityY": 0.39762188321538583,
+            "timestamp": 0.4798914324264284
         },
         {
-            "x": 2.087903104978946,
-            "y": 6.870323419047386,
-            "heading": 0.5166967856787098,
-            "angularVelocity": -5.212335818457529e-7,
-            "velocityX": 3.7619857366188425,
-            "velocityY": 0.3976200611141572,
-            "timestamp": 0.559873337831239
+            "x": 2.0879033719171214,
+            "y": 6.870321038982823,
+            "heading": 0.5166969559088411,
+            "angularVelocity": -5.212335340785354e-7,
+            "velocityX": 3.761986315414672,
+            "velocityY": 0.39761458494048757,
+            "timestamp": 0.5598733378308332
         },
         {
-            "x": 2.3887938966166886,
-            "y": 6.9021257883033815,
-            "heading": 0.5166967439894641,
-            "angularVelocity": -5.212334661065162e-7,
-            "velocityX": 3.761985790613012,
-            "velocityY": 0.3976195502616874,
-            "timestamp": 0.6398552432357018
+            "x": 2.3887942055296523,
+            "y": 6.902123011099449,
+            "heading": 0.5166969142195897,
+            "angularVelocity": -5.212335365150009e-7,
+            "velocityX": 3.7619863154192914,
+            "velocityY": 0.3976145848967806,
+            "timestamp": 0.6398552432352379
         },
         {
-            "x": 2.689684692935055,
-            "y": 6.933928113274756,
-            "heading": 0.5166967023002289,
-            "angularVelocity": -5.212333328622992e-7,
-            "velocityX": 3.76198584913404,
-            "velocityY": 0.3976189965786884,
-            "timestamp": 0.7198371486401645
+            "x": 2.689685039142183,
+            "y": 6.933924983216074,
+            "heading": 0.516696872530338,
+            "angularVelocity": -5.212335398189914e-7,
+            "velocityX": 3.7619863154192914,
+            "velocityY": 0.3976145848967804,
+            "timestamp": 0.7198371486396427
         },
         {
-            "x": 2.990575494322058,
-            "y": 6.965730390290324,
-            "heading": 0.5166966606110028,
-            "angularVelocity": -5.212332194068621e-7,
-            "velocityX": 3.761985912506342,
-            "velocityY": 0.3976183969955037,
-            "timestamp": 0.7998190540446272
+            "x": 2.990575872754714,
+            "y": 6.9657269553327,
+            "heading": 0.5166968308410861,
+            "angularVelocity": -5.212335433606452e-7,
+            "velocityX": 3.7619863154192914,
+            "velocityY": 0.39761458489678053,
+            "timestamp": 0.7998190540440474
         },
         {
-            "x": 3.291466301153434,
-            "y": 6.997532615795069,
-            "heading": 0.5166966189217854,
-            "angularVelocity": -5.21233112050001e-7,
-            "velocityX": 3.76198598057639,
-            "velocityY": 0.39761775296452984,
-            "timestamp": 0.8798009594490899
+            "x": 3.291466706367245,
+            "y": 6.997528927449325,
+            "heading": 0.5166967891518339,
+            "angularVelocity": -5.212335460884143e-7,
+            "velocityX": 3.761986315419291,
+            "velocityY": 0.3976145848967808,
+            "timestamp": 0.8798009594484522
         },
         {
-            "x": 3.592357113798132,
-            "y": 7.029334786298146,
-            "heading": 0.5166965772325762,
-            "angularVelocity": -5.212330086999819e-7,
-            "velocityX": 3.7619860532593528,
-            "velocityY": 0.39761706528816065,
-            "timestamp": 0.9597828648535526
+            "x": 3.5923575399797762,
+            "y": 7.0293308995659505,
+            "heading": 0.5166967474625817,
+            "angularVelocity": -5.212335479348436e-7,
+            "velocityX": 3.761986315419291,
+            "velocityY": 0.3976145848967805,
+            "timestamp": 0.9597828648528569
         },
         {
-            "x": 3.8932479326255467,
-            "y": 7.061136898304478,
-            "heading": 0.5166965355433745,
-            "angularVelocity": -5.212329156376395e-7,
-            "velocityX": 3.7619861305608038,
-            "velocityY": 0.3976163339134084,
-            "timestamp": 1.0397647702580153
+            "x": 3.893248373592307,
+            "y": 7.0611328716825765,
+            "heading": 0.516696705773329,
+            "angularVelocity": -5.212335516712931e-7,
+            "velocityX": 3.7619863154192905,
+            "velocityY": 0.3976145848967809,
+            "timestamp": 1.0397647702572617
         },
         {
-            "x": 4.194138758019097,
-            "y": 7.092938948186284,
-            "heading": 0.5166964938541775,
-            "angularVelocity": -5.212328561470798e-7,
-            "velocityX": 3.7619862126560695,
-            "velocityY": 0.3976155571811608,
-            "timestamp": 1.119746675662478
+            "x": 4.194139207204837,
+            "y": 7.092934843799203,
+            "heading": 0.5166966640840763,
+            "angularVelocity": -5.212335535473894e-7,
+            "velocityX": 3.7619863154192905,
+            "velocityY": 0.3976145848967809,
+            "timestamp": 1.1197466756616663
         },
         {
-            "x": 4.495029590361164,
-            "y": 7.1247409323255795,
-            "heading": 0.516696452164982,
-            "angularVelocity": -5.212328389753682e-7,
-            "velocityX": 3.7619862995321793,
-            "velocityY": 0.397614735213847,
-            "timestamp": 1.199728581066941
+            "x": 4.495030040817368,
+            "y": 7.1247368159158295,
+            "heading": 0.5166966223948233,
+            "angularVelocity": -5.212335568545237e-7,
+            "velocityX": 3.7619863154192905,
+            "velocityY": 0.3976145848967808,
+            "timestamp": 1.199728581066071
         },
         {
-            "x": 4.79592042996658,
-            "y": 7.15654284774347,
-            "heading": 0.5166964104757901,
-            "angularVelocity": -5.212327914819509e-7,
-            "velocityX": 3.7619863903445805,
-            "velocityY": 0.39761387600196396,
-            "timestamp": 1.2797104864714037
+            "x": 4.7959208744299,
+            "y": 7.156538788032456,
+            "heading": 0.51669658070557,
+            "angularVelocity": -5.212335599678648e-7,
+            "velocityX": 3.7619863154192905,
+            "velocityY": 0.397614584896781,
+            "timestamp": 1.2797104864704756
         },
         {
-            "x": 5.096811277131865,
-            "y": 7.188344691634321,
-            "heading": 0.5166963687866055,
-            "angularVelocity": -5.212326996843497e-7,
-            "velocityX": 3.7619864848642997,
-            "velocityY": 0.39761298171169474,
-            "timestamp": 1.3596923918758665
+            "x": 5.096811708042431,
+            "y": 7.1883407601490825,
+            "heading": 0.5166965390163165,
+            "angularVelocity": -5.212335628762146e-7,
+            "velocityX": 3.7619863154192905,
+            "velocityY": 0.3976145848967809,
+            "timestamp": 1.3596923918748802
         },
         {
-            "x": 5.397702132107731,
-            "y": 7.220146461625841,
-            "heading": 0.5166963270974331,
-            "angularVelocity": -5.212325496997691e-7,
-            "velocityX": 3.761986582518669,
-            "velocityY": 0.3976120577610798,
-            "timestamp": 1.4396742972803294
+            "x": 5.3977025416549615,
+            "y": 7.220142732265709,
+            "heading": 0.5166964973270628,
+            "angularVelocity": -5.212335655841188e-7,
+            "velocityX": 3.76198631541929,
+            "velocityY": 0.3976145848967813,
+            "timestamp": 1.4396742972792849
         },
         {
-            "x": 5.698592995103496,
-            "y": 7.251948155737401,
-            "heading": 0.516696285408269,
-            "angularVelocity": -5.212324447299826e-7,
-            "velocityX": 3.7619866827900856,
-            "velocityY": 0.3976111090469843,
-            "timestamp": 1.5196562026847922
+            "x": 5.698593375267492,
+            "y": 7.251944704382335,
+            "heading": 0.5166964556378089,
+            "angularVelocity": -5.212335680260379e-7,
+            "velocityX": 3.7619863154192896,
+            "velocityY": 0.3976145848967809,
+            "timestamp": 1.5196562026836895
         },
         {
-            "x": 5.99948386596862,
-            "y": 7.283749775393132,
-            "heading": 0.5166962437191063,
-            "angularVelocity": -5.212324282706134e-7,
-            "velocityX": 3.7619867811793206,
-            "velocityY": 0.3976101781385786,
-            "timestamp": 1.599638108089255
+            "x": 5.999484208880023,
+            "y": 7.283746676498962,
+            "heading": 0.5166964139485547,
+            "angularVelocity": -5.212335712969897e-7,
+            "velocityX": 3.7619863154192896,
+            "velocityY": 0.3976145848967812,
+            "timestamp": 1.5996381080880941
         },
         {
-            "x": 6.300374744112002,
-            "y": 7.315551326185539,
-            "heading": 0.5166962020299369,
-            "angularVelocity": -5.212325087463018e-7,
-            "velocityX": 3.761986872178135,
-            "velocityY": 0.3976093171522739,
-            "timestamp": 1.6796200134937178
+            "x": 6.300375042492554,
+            "y": 7.315548648615588,
+            "heading": 0.5166963722593003,
+            "angularVelocity": -5.21233573548864e-7,
+            "velocityX": 3.7619863154192896,
+            "velocityY": 0.39761458489678125,
+            "timestamp": 1.6796200134924988
         },
         {
-            "x": 6.6012656287075195,
-            "y": 7.347352815930844,
-            "heading": 0.5166961603407556,
-            "angularVelocity": -5.21232660707814e-7,
-            "velocityX": 3.7619869528480785,
-            "velocityY": 0.39760855389086563,
-            "timestamp": 1.7596019188981806
+            "x": 6.6012658761048035,
+            "y": 7.347350620734872,
+            "heading": 0.5166963305700458,
+            "angularVelocity": -5.212335762453642e-7,
+            "velocityX": 3.7619863154157773,
+            "velocityY": 0.3976145849300086,
+            "timestamp": 1.7596019188969034
         },
         {
-            "x": 6.9021564720244495,
-            "y": 7.37915469622686,
-            "heading": 0.5166961186499832,
-            "angularVelocity": -5.212525529907653e-7,
-            "velocityX": 3.7619864367489835,
-            "velocityY": 0.3976134368792068,
-            "timestamp": 1.8395838243026434
+            "x": 6.902156662813089,
+            "y": 7.3791530366221005,
+            "heading": 0.5166962888792155,
+            "angularVelocity": -5.212532766918364e-7,
+            "velocityX": 3.761985728983584,
+            "velocityY": 0.3976201332842598,
+            "timestamp": 1.839583824301308
         },
         {
-            "x": 7.162090895853125,
-            "y": 7.409612003973996,
-            "heading": 0.405130711689902,
-            "angularVelocity": -1.3948830850665912,
-            "velocityX": 3.2499153716606957,
-            "velocityY": 0.3808024776743751,
-            "timestamp": 1.9195657297071063
+            "x": 7.162091022778527,
+            "y": 7.409610892063227,
+            "heading": 0.4051307797997224,
+            "angularVelocity": -1.3948843618490365,
+            "velocityX": 3.249914573191993,
+            "velocityY": 0.3808093253983604,
+            "timestamp": 1.9195657297057127
         },
         {
-            "x": 7.370036531434433,
-            "y": 7.432975355945854,
-            "heading": 0.2588526808330054,
-            "angularVelocity": -1.8288890483063531,
-            "velocityX": 2.5999084984252714,
-            "velocityY": 0.2921079693427033,
-            "timestamp": 1.999547635111569
+            "x": 7.370036606479622,
+            "y": 7.432974685540162,
+            "heading": 0.25885269617169443,
+            "angularVelocity": -1.8288897080960538,
+            "velocityX": 2.5999078497777726,
+            "velocityY": 0.2921134894049214,
+            "timestamp": 1.9995476351101173
         },
         {
-            "x": 7.52532693851905,
-            "y": 7.450135904743354,
-            "heading": 0.13353023499549588,
-            "angularVelocity": -1.5668849748422837,
-            "velocityX": 1.9415692374334488,
-            "velocityY": 0.21455538863097798,
-            "timestamp": 2.079529540516032
+            "x": 7.525326975450784,
+            "y": 7.45013556763307,
+            "heading": 0.1335302309738367,
+            "angularVelocity": -1.5668852169025227,
+            "velocityX": 1.9415687609088876,
+            "velocityY": 0.21455955576625652,
+            "timestamp": 2.079529540514522
         },
         {
-            "x": 7.628525628712187,
-            "y": 7.461415184148821,
-            "heading": 0.04391070884179766,
-            "angularVelocity": -1.120497513787634,
-            "velocityX": 1.2902754650726174,
-            "velocityY": 0.14102288947017258,
-            "timestamp": 2.1595114459204945
+            "x": 7.628525640862077,
+            "y": 7.461415071089343,
+            "heading": 0.043910704401711645,
+            "angularVelocity": -1.1204975190199653,
+            "velocityX": 1.2902751552304177,
+            "velocityY": 0.14102569073894916,
+            "timestamp": 2.159511445918927
         },
         {
             "x": 7.68,
             "y": 7.467,
             "heading": -0.003,
-            "angularVelocity": -0.5865165202625967,
-            "velocityX": 0.6435752065109067,
-            "velocityY": 0.06982599155316412,
-            "timestamp": 2.239493351324957
+            "angularVelocity": -0.5865164647493907,
+            "velocityX": 0.6435750546033896,
+            "velocityY": 0.0698274051164111,
+            "timestamp": 2.2394933513233317
         },
         {
             "x": 7.68,
             "y": 7.467,
             "heading": -0.003,
-            "angularVelocity": -3.4580717798993225e-32,
-            "velocityX": 0,
+            "angularVelocity": 0,
+            "velocityX": 4.312091780947568e-36,
             "velocityY": 0,
-            "timestamp": 2.3194752567294197
+            "timestamp": 2.3194752567277366
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.1.traj
index 1088f323..8c7890ab 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.1.traj	
@@ -5,270 +5,315 @@
             "y": 6.72,
             "heading": 1.04,
             "angularVelocity": 0,
-            "velocityX": 8.672453699203877e-33,
-            "velocityY": 0,
+            "velocityX": 2.674890564447798e-33,
+            "velocityY": 3.559444452002106e-32,
             "timestamp": 0
         },
         {
-            "x": 0.7586383226811528,
-            "y": 6.716006460523898,
-            "heading": 0.9415437629009579,
-            "angularVelocity": -1.201643840291405,
-            "velocityX": 0.5936235486342963,
-            "velocityY": -0.048740559804155884,
-            "timestamp": 0.08193462471805792
-        },
-        {
-            "x": 0.8580797410236602,
-            "y": 6.713039346170398,
-            "heading": 0.7612656224060018,
-            "angularVelocity": -2.2002680931938623,
-            "velocityX": 1.2136678319415195,
-            "velocityY": -0.03621319269735464,
-            "timestamp": 0.16386924943611583
-        },
-        {
-            "x": 1.0092416285400778,
-            "y": 6.712497099188053,
-            "heading": 0.5037500418751092,
-            "angularVelocity": -3.142939647518002,
-            "velocityX": 1.8449085235524658,
-            "velocityY": -0.006618044376361366,
-            "timestamp": 0.24580387415417376
-        },
-        {
-            "x": 1.2158692368887885,
-            "y": 6.714801206948958,
-            "heading": 0.22114949007633808,
-            "angularVelocity": -3.4490980189537326,
-            "velocityX": 2.5218594588030294,
-            "velocityY": 0.028121295103663166,
-            "timestamp": 0.32773849887223166
-        },
-        {
-            "x": 1.4708450332398146,
-            "y": 6.7208733735889465,
-            "heading": 0.04860948021713714,
-            "angularVelocity": -2.105825351039585,
-            "velocityX": 3.1119419565100066,
-            "velocityY": 0.07410989750528489,
-            "timestamp": 0.40967312359028957
-        },
-        {
-            "x": 1.7704680006957136,
-            "y": 6.724924507610315,
-            "heading": 0.013522063773945665,
-            "angularVelocity": -0.42823673830116915,
-            "velocityX": 3.6568540893049817,
-            "velocityY": 0.049443492727383076,
-            "timestamp": 0.49160774830834747
-        },
-        {
-            "x": 2.080344251704432,
-            "y": 6.7179919202849705,
-            "heading": 0.013521948546161235,
-            "angularVelocity": -0.000001406338099756076,
-            "velocityX": 3.78199390154067,
-            "velocityY": -0.08461120496004566,
-            "timestamp": 0.5735423730264054
-        },
-        {
-            "x": 2.3902205002379273,
-            "y": 6.711059222319389,
-            "heading": 0.013521833318547862,
-            "angularVelocity": -0.000001406336012025862,
-            "velocityX": 3.781993871330935,
-            "velocityY": -0.08461255530781114,
-            "timestamp": 0.6554769977444633
-        },
-        {
-            "x": 2.7000967477974154,
-            "y": 6.704126480817366,
-            "heading": 0.013521718090975673,
-            "angularVelocity": -0.000001406335509399091,
-            "velocityX": 3.7819938594433236,
-            "velocityY": -0.08461308666366031,
-            "timestamp": 0.7374116224625212
-        },
-        {
-            "x": 3.009972994287386,
-            "y": 6.6971936915101855,
-            "heading": 0.013521602863440677,
-            "angularVelocity": -0.0000014063350554339165,
-            "velocityX": 3.7819938463900247,
-            "velocityY": -0.08461367011854067,
-            "timestamp": 0.8193462471805791
-        },
-        {
-            "x": 3.319849239613809,
-            "y": 6.690260850195287,
-            "heading": 0.013521487635941729,
-            "angularVelocity": -0.0000014063346155021318,
-            "velocityX": 3.781993832189087,
-            "velocityY": -0.08461430486506759,
-            "timestamp": 0.901280871898637
-        },
-        {
-            "x": 3.629725483679546,
-            "y": 6.683327952531275,
-            "heading": 0.013521372408489362,
-            "angularVelocity": -0.0000014063340469636637,
-            "velocityX": 3.7819938168026104,
-            "velocityY": -0.0846149925976696,
-            "timestamp": 0.9832154966166949
-        },
-        {
-            "x": 3.939601726375346,
-            "y": 6.6763949936353555,
-            "heading": 0.013521257181093756,
-            "angularVelocity": -0.0000014063333542144944,
-            "velocityX": 3.7819938000827285,
-            "velocityY": -0.08461573992406812,
-            "timestamp": 1.065150121334753
-        },
-        {
-            "x": 4.249477967564444,
-            "y": 6.66946196739526,
-            "heading": 0.013521141953758897,
-            "angularVelocity": -0.0000014063326128127892,
-            "velocityX": 3.781993781693661,
-            "velocityY": -0.0846165618498062,
-            "timestamp": 1.1470847460528109
-        },
-        {
-            "x": 4.559354207050928,
-            "y": 6.662528865055332,
-            "heading": 0.013521026726476763,
-            "angularVelocity": -0.0000014063319692972842,
-            "velocityX": 3.7819937609134957,
-            "velocityY": -0.08461749063701171,
-            "timestamp": 1.2290193707708688
-        },
-        {
-            "x": 4.869230444482983,
-            "y": 6.655595670892424,
-            "heading": 0.01352091149922989,
-            "angularVelocity": -0.0000014063315389464776,
-            "velocityX": 3.7819937358394955,
-            "velocityY": -0.08461861132293771,
-            "timestamp": 1.3109539954889267
-        },
-        {
-            "x": 5.1791066790320945,
-            "y": 6.648662347878877,
-            "heading": 0.013520796271957977,
-            "angularVelocity": -0.0000014063318445457383,
-            "velocityX": 3.7819937006535858,
-            "velocityY": -0.08462018392597198,
-            "timestamp": 1.3928886202069846
-        },
-        {
-            "x": 5.488982909387702,
-            "y": 6.641728837446599,
-            "heading": 0.013520681044501265,
-            "angularVelocity": -0.000001406334100009416,
-            "velocityX": 3.781993649472511,
-            "velocityY": -0.08462247134390694,
-            "timestamp": 1.4748232449250425
-        },
-        {
-            "x": 5.798859128679326,
-            "y": 6.634794832555163,
-            "heading": 0.013520565816988437,
-            "angularVelocity": -0.0000014063347848865157,
-            "velocityX": 3.781993514438203,
-            "velocityY": -0.08462850614495548,
-            "timestamp": 1.5567578696431004
-        },
-        {
-            "x": 6.106285584248806,
-            "y": 6.595293875173847,
-            "heading": 0.013520451149743993,
-            "angularVelocity": -0.000001399496791973984,
-            "velocityX": 3.7520945098284613,
-            "velocityY": -0.48210335395128207,
-            "timestamp": 1.6386924943611583
-        },
-        {
-            "x": 6.401594301701064,
-            "y": 6.501144389493677,
-            "heading": 0.013520442621996222,
-            "angularVelocity": -1.0407990272609098e-7,
-            "velocityX": 3.604199304853526,
-            "velocityY": -1.1490805749601392,
-            "timestamp": 1.7206271190792162
-        },
-        {
-            "x": 6.675154779128766,
-            "y": 6.355418517983256,
-            "heading": 0.013520441934185553,
-            "angularVelocity": -8.394627700401504e-9,
-            "velocityX": 3.3387652456948387,
-            "velocityY": -1.7785627506304358,
-            "timestamp": 1.802561743797274
-        },
-        {
-            "x": 6.946083635654423,
-            "y": 6.204856506903887,
-            "heading": 0.013520441248790115,
-            "angularVelocity": -8.365150168101951e-9,
-            "velocityX": 3.3066467010490315,
-            "velocityY": -1.8375871201883405,
-            "timestamp": 1.884496368515332
-        },
-        {
-            "x": 7.190722413199009,
-            "y": 6.068904365786755,
-            "heading": 0.008013663922991063,
-            "angularVelocity": -0.06720940438487652,
-            "velocityX": 2.985780167864364,
-            "velocityY": -1.6592758139181236,
-            "timestamp": 1.9664309932333899
-        },
-        {
-            "x": 7.38643344699072,
-            "y": 5.96014263272892,
-            "heading": 0.0036082164005303732,
-            "angularVelocity": -0.053767836706643944,
-            "velocityX": 2.3886242777722417,
-            "velocityY": -1.3274208972347252,
-            "timestamp": 2.048365617951448
-        },
-        {
-            "x": 7.533216723981145,
-            "y": 5.878571322292618,
-            "heading": 0.0003041162966361105,
-            "angularVelocity": -0.04032605403715309,
-            "velocityX": 1.7914682284261954,
-            "velocityY": -0.9955658028213874,
-            "timestamp": 2.130300242669506
-        },
-        {
-            "x": 7.631072241649754,
-            "y": 5.82419044260697,
-            "heading": -0.0018986254325586427,
-            "angularVelocity": -0.026884137649675257,
-            "velocityX": 1.1943121483174686,
-            "velocityY": -0.66371060919331,
-            "timestamp": 2.212234867387564
+            "x": 0.744719766454751,
+            "y": 6.720759968199705,
+            "heading": 1.006760857121406,
+            "angularVelocity": -0.5061419234074507,
+            "velocityX": 0.5286878015432446,
+            "velocityY": 0.011572252862695013,
+            "timestamp": 0.06567158605400746
+        },
+        {
+            "x": 0.8143576636925598,
+            "y": 6.722372451876338,
+            "heading": 0.9431499891720047,
+            "angularVelocity": -0.9686208567749168,
+            "velocityX": 1.0603961533765867,
+            "velocityY": 0.024553749551892837,
+            "timestamp": 0.1313431721080149
+        },
+        {
+            "x": 0.9191612228651693,
+            "y": 6.724923861576486,
+            "heading": 0.8530685118915011,
+            "angularVelocity": -1.3716963864162066,
+            "velocityX": 1.5958737327650472,
+            "velocityY": 0.03885104431723037,
+            "timestamp": 0.19701475816202235
+        },
+        {
+            "x": 1.059454644842809,
+            "y": 6.7284663342548425,
+            "heading": 0.7426650921374723,
+            "angularVelocity": -1.6811444094442096,
+            "velocityX": 2.1362880114128,
+            "velocityY": 0.05394224338464699,
+            "timestamp": 0.2626863442160298
+        },
+        {
+            "x": 1.2356244745863587,
+            "y": 6.733039509062333,
+            "heading": 0.6243118436370656,
+            "angularVelocity": -1.8021987226420155,
+            "velocityX": 2.682588320596835,
+            "velocityY": 0.06963703912619325,
+            "timestamp": 0.3283579302700373
+        },
+        {
+            "x": 1.4470569399809903,
+            "y": 6.738777367450543,
+            "heading": 0.5308220213971302,
+            "angularVelocity": -1.4235962287107038,
+            "velocityX": 3.219542546463745,
+            "velocityY": 0.08737200870294055,
+            "timestamp": 0.39402951632404476
+        },
+        {
+            "x": 1.688226065456379,
+            "y": 6.748371696739962,
+            "heading": 0.5126606987274956,
+            "angularVelocity": -0.2765476480908373,
+            "velocityX": 3.6723511638207493,
+            "velocityY": 0.14609559272058092,
+            "timestamp": 0.45970110237805223
+        },
+        {
+            "x": 1.9365780121804386,
+            "y": 6.754665679712547,
+            "heading": 0.5126606479364623,
+            "angularVelocity": -7.734095747362075e-7,
+            "velocityX": 3.7817260347544686,
+            "velocityY": 0.09584027660688858,
+            "timestamp": 0.5253726884320596
+        },
+        {
+            "x": 2.1849299738494317,
+            "y": 6.760959072962077,
+            "heading": 0.5126605971462416,
+            "angularVelocity": -7.733971982996713e-7,
+            "velocityX": 3.7817262623252414,
+            "velocityY": 0.09583129672545204,
+            "timestamp": 0.5910442744860671
+        },
+        {
+            "x": 2.433281935680659,
+            "y": 6.767252459809602,
+            "heading": 0.5126605463560266,
+            "angularVelocity": -7.733971099299294e-7,
+            "velocityX": 3.7817262647956325,
+            "velocityY": 0.09583119924030911,
+            "timestamp": 0.6567158605400746
+        },
+        {
+            "x": 2.6816338977021794,
+            "y": 6.773545839147862,
+            "heading": 0.5126604955658177,
+            "angularVelocity": -7.733970196193196e-7,
+            "velocityX": 3.78172626769327,
+            "velocityY": 0.09583108489457716,
+            "timestamp": 0.722387446594082
+        },
+        {
+            "x": 2.9299858599438156,
+            "y": 6.779839209799928,
+            "heading": 0.5126604447756148,
+            "angularVelocity": -7.733969286509421e-7,
+            "velocityX": 3.7817262710450463,
+            "velocityY": 0.09583095262742326,
+            "timestamp": 0.7880590326480895
+        },
+        {
+            "x": 3.1783378224424883,
+            "y": 6.786132570308856,
+            "heading": 0.5126603939854177,
+            "angularVelocity": -7.733968385113031e-7,
+            "velocityX": 3.781726274959007,
+            "velocityY": 0.09583079817497217,
+            "timestamp": 0.853730618702097
+        },
+        {
+            "x": 3.426689785244194,
+            "y": 6.792425918859452,
+            "heading": 0.5126603431952265,
+            "angularVelocity": -7.733967515033378e-7,
+            "velocityX": 3.7817262795733857,
+            "velocityY": 0.09583061608201786,
+            "timestamp": 0.9194022047561045
+        },
+        {
+            "x": 3.6750417484067377,
+            "y": 6.798719253170605,
+            "heading": 0.5126602924050404,
+            "angularVelocity": -7.733966712943809e-7,
+            "velocityX": 3.781726285067957,
+            "velocityY": 0.09583039925395942,
+            "timestamp": 0.9850737908101119
+        },
+        {
+            "x": 3.9233937120031053,
+            "y": 6.805012570362013,
+            "heading": 0.5126602416148589,
+            "angularVelocity": -7.733966025007139e-7,
+            "velocityX": 3.7817262916739214,
+            "velocityY": 0.095830138566711,
+            "timestamp": 1.0507453768641193
+        },
+        {
+            "x": 4.171745676125189,
+            "y": 6.811305866807266,
+            "heading": 0.5126601908246804,
+            "angularVelocity": -7.733965561950186e-7,
+            "velocityX": 3.7817262996791623,
+            "velocityY": 0.09582982265909266,
+            "timestamp": 1.1164169629181266
+        },
+        {
+            "x": 4.420097640886595,
+            "y": 6.817599138023107,
+            "heading": 0.5126601400345029,
+            "angularVelocity": -7.733965406200526e-7,
+            "velocityX": 3.781726309414286,
+            "velocityY": 0.09582943848362173,
+            "timestamp": 1.182088548972134
+        },
+        {
+            "x": 4.6684496064150185,
+            "y": 6.8238923789700765,
+            "heading": 0.5126600892443262,
+            "angularVelocity": -7.73396529455525e-7,
+            "velocityX": 3.7817263210938923,
+            "velocityY": 0.09582897757087808,
+            "timestamp": 1.2477601350261414
+        },
+        {
+            "x": 4.916801572841165,
+            "y": 6.830185584489968,
+            "heading": 0.5126600384541553,
+            "angularVelocity": -7.733964406232834e-7,
+            "velocityX": 3.7817263347637717,
+            "velocityY": 0.09582843811261686,
+            "timestamp": 1.3134317210801487
+        },
+        {
+            "x": 5.165153539254683,
+            "y": 6.8364787905062485,
+            "heading": 0.5126599876639959,
+            "angularVelocity": -7.733962639482034e-7,
+            "velocityX": 3.781726334571479,
+            "velocityY": 0.09582844567126618,
+            "timestamp": 1.379103307134156
+        },
+        {
+            "x": 5.413504500375102,
+            "y": 6.842811545538402,
+            "heading": 0.5126599368726894,
+            "angularVelocity": -7.734137320837285e-7,
+            "velocityX": 3.781711026686433,
+            "velocityY": 0.09643066982787928,
+            "timestamp": 1.4447748931881634
+        },
+        {
+            "x": 5.661808131063952,
+            "y": 6.834835924305975,
+            "heading": 0.5126598860199024,
+            "angularVelocity": -7.743499133059843e-7,
+            "velocityX": 3.780990312684674,
+            "velocityY": -0.12144706275568991,
+            "timestamp": 1.5104464792421708
+        },
+        {
+            "x": 5.9063591957092285,
+            "y": 6.791097164154053,
+            "heading": 0.5126598314406843,
+            "angularVelocity": -8.310933457001734e-7,
+            "velocityX": 3.723848917614642,
+            "velocityY": -0.6660225948568639,
+            "timestamp": 1.5761180652961782
+        },
+        {
+            "x": 6.194622556481831,
+            "y": 6.683762426219015,
+            "heading": 0.5126597887806125,
+            "angularVelocity": -5.246475178274693e-7,
+            "velocityX": 3.5451571072929595,
+            "velocityY": -1.3200377183874616,
+            "timestamp": 1.65742993725234
+        },
+        {
+            "x": 6.459547388261047,
+            "y": 6.527457130068993,
+            "heading": 0.5126597482300577,
+            "angularVelocity": -4.987039864896594e-7,
+            "velocityX": 3.2581322432449182,
+            "velocityY": -1.9222936625379825,
+            "timestamp": 1.7387418092085019
+        },
+        {
+            "x": 6.723521508223802,
+            "y": 6.3695515377198895,
+            "heading": 0.5126597076877473,
+            "angularVelocity": -4.98602596336278e-7,
+            "velocityX": 3.2464400783305223,
+            "velocityY": -1.9419746286809643,
+            "timestamp": 1.8200536811646637
+        },
+        {
+            "x": 6.987495332330779,
+            "y": 6.211645450805702,
+            "heading": 0.5126596671480953,
+            "angularVelocity": -4.985699012832353e-7,
+            "velocityX": 3.246436439802837,
+            "velocityY": -1.9419807109984206,
+            "timestamp": 1.9013655531208256
+        },
+        {
+            "x": 7.218138260463332,
+            "y": 6.072816355413413,
+            "heading": 0.4174347514218684,
+            "angularVelocity": -1.1711071635093024,
+            "velocityX": 2.8365221779297074,
+            "velocityY": -1.7073656288101424,
+            "timestamp": 1.9826774250769874
+        },
+        {
+            "x": 7.40359391202109,
+            "y": 5.961763553999033,
+            "heading": 0.26911254592387807,
+            "angularVelocity": -1.8241150022712385,
+            "velocityX": 2.2807942689821186,
+            "velocityY": -1.365763679308134,
+            "timestamp": 2.063989297033149
+        },
+        {
+            "x": 7.542069024042313,
+            "y": 5.879081457057784,
+            "heading": 0.13934312775190755,
+            "angularVelocity": -1.595946754760921,
+            "velocityX": 1.7030122255201257,
+            "velocityY": -1.0168514750927211,
+            "timestamp": 2.145301168989311
+        },
+        {
+            "x": 7.63409233729861,
+            "y": 5.824280880315082,
+            "heading": 0.04592275308495019,
+            "angularVelocity": -1.1489143272623823,
+            "velocityX": 1.1317328090287153,
+            "velocityY": -0.6739554191077514,
+            "timestamp": 2.2266130409454727
         },
         {
             "x": 7.68,
             "y": 5.797,
             "heading": -0.003,
-            "angularVelocity": -0.013442114017501823,
-            "velocityX": 0.5971560682508651,
-            "velocityY": -0.3318553383326643,
-            "timestamp": 2.294169492105622
+            "angularVelocity": -0.6016680210157614,
+            "velocityX": 0.5645874531846384,
+            "velocityY": -0.3355091902172136,
+            "timestamp": 2.3079249129016346
         },
         {
             "x": 7.68,
             "y": 5.797,
             "heading": -0.003,
-            "angularVelocity": -2.3555736468302963e-30,
+            "angularVelocity": 3.440382093642737e-30,
             "velocityX": 0,
-            "velocityY": 5.757868192156219e-33,
-            "timestamp": 2.3761041168236803
+            "velocityY": 2.1122019835026996e-31,
+            "timestamp": 2.3892367848577964
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.traj
index 1088f323..8c7890ab 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.traj	
@@ -5,270 +5,315 @@
             "y": 6.72,
             "heading": 1.04,
             "angularVelocity": 0,
-            "velocityX": 8.672453699203877e-33,
-            "velocityY": 0,
+            "velocityX": 2.674890564447798e-33,
+            "velocityY": 3.559444452002106e-32,
             "timestamp": 0
         },
         {
-            "x": 0.7586383226811528,
-            "y": 6.716006460523898,
-            "heading": 0.9415437629009579,
-            "angularVelocity": -1.201643840291405,
-            "velocityX": 0.5936235486342963,
-            "velocityY": -0.048740559804155884,
-            "timestamp": 0.08193462471805792
-        },
-        {
-            "x": 0.8580797410236602,
-            "y": 6.713039346170398,
-            "heading": 0.7612656224060018,
-            "angularVelocity": -2.2002680931938623,
-            "velocityX": 1.2136678319415195,
-            "velocityY": -0.03621319269735464,
-            "timestamp": 0.16386924943611583
-        },
-        {
-            "x": 1.0092416285400778,
-            "y": 6.712497099188053,
-            "heading": 0.5037500418751092,
-            "angularVelocity": -3.142939647518002,
-            "velocityX": 1.8449085235524658,
-            "velocityY": -0.006618044376361366,
-            "timestamp": 0.24580387415417376
-        },
-        {
-            "x": 1.2158692368887885,
-            "y": 6.714801206948958,
-            "heading": 0.22114949007633808,
-            "angularVelocity": -3.4490980189537326,
-            "velocityX": 2.5218594588030294,
-            "velocityY": 0.028121295103663166,
-            "timestamp": 0.32773849887223166
-        },
-        {
-            "x": 1.4708450332398146,
-            "y": 6.7208733735889465,
-            "heading": 0.04860948021713714,
-            "angularVelocity": -2.105825351039585,
-            "velocityX": 3.1119419565100066,
-            "velocityY": 0.07410989750528489,
-            "timestamp": 0.40967312359028957
-        },
-        {
-            "x": 1.7704680006957136,
-            "y": 6.724924507610315,
-            "heading": 0.013522063773945665,
-            "angularVelocity": -0.42823673830116915,
-            "velocityX": 3.6568540893049817,
-            "velocityY": 0.049443492727383076,
-            "timestamp": 0.49160774830834747
-        },
-        {
-            "x": 2.080344251704432,
-            "y": 6.7179919202849705,
-            "heading": 0.013521948546161235,
-            "angularVelocity": -0.000001406338099756076,
-            "velocityX": 3.78199390154067,
-            "velocityY": -0.08461120496004566,
-            "timestamp": 0.5735423730264054
-        },
-        {
-            "x": 2.3902205002379273,
-            "y": 6.711059222319389,
-            "heading": 0.013521833318547862,
-            "angularVelocity": -0.000001406336012025862,
-            "velocityX": 3.781993871330935,
-            "velocityY": -0.08461255530781114,
-            "timestamp": 0.6554769977444633
-        },
-        {
-            "x": 2.7000967477974154,
-            "y": 6.704126480817366,
-            "heading": 0.013521718090975673,
-            "angularVelocity": -0.000001406335509399091,
-            "velocityX": 3.7819938594433236,
-            "velocityY": -0.08461308666366031,
-            "timestamp": 0.7374116224625212
-        },
-        {
-            "x": 3.009972994287386,
-            "y": 6.6971936915101855,
-            "heading": 0.013521602863440677,
-            "angularVelocity": -0.0000014063350554339165,
-            "velocityX": 3.7819938463900247,
-            "velocityY": -0.08461367011854067,
-            "timestamp": 0.8193462471805791
-        },
-        {
-            "x": 3.319849239613809,
-            "y": 6.690260850195287,
-            "heading": 0.013521487635941729,
-            "angularVelocity": -0.0000014063346155021318,
-            "velocityX": 3.781993832189087,
-            "velocityY": -0.08461430486506759,
-            "timestamp": 0.901280871898637
-        },
-        {
-            "x": 3.629725483679546,
-            "y": 6.683327952531275,
-            "heading": 0.013521372408489362,
-            "angularVelocity": -0.0000014063340469636637,
-            "velocityX": 3.7819938168026104,
-            "velocityY": -0.0846149925976696,
-            "timestamp": 0.9832154966166949
-        },
-        {
-            "x": 3.939601726375346,
-            "y": 6.6763949936353555,
-            "heading": 0.013521257181093756,
-            "angularVelocity": -0.0000014063333542144944,
-            "velocityX": 3.7819938000827285,
-            "velocityY": -0.08461573992406812,
-            "timestamp": 1.065150121334753
-        },
-        {
-            "x": 4.249477967564444,
-            "y": 6.66946196739526,
-            "heading": 0.013521141953758897,
-            "angularVelocity": -0.0000014063326128127892,
-            "velocityX": 3.781993781693661,
-            "velocityY": -0.0846165618498062,
-            "timestamp": 1.1470847460528109
-        },
-        {
-            "x": 4.559354207050928,
-            "y": 6.662528865055332,
-            "heading": 0.013521026726476763,
-            "angularVelocity": -0.0000014063319692972842,
-            "velocityX": 3.7819937609134957,
-            "velocityY": -0.08461749063701171,
-            "timestamp": 1.2290193707708688
-        },
-        {
-            "x": 4.869230444482983,
-            "y": 6.655595670892424,
-            "heading": 0.01352091149922989,
-            "angularVelocity": -0.0000014063315389464776,
-            "velocityX": 3.7819937358394955,
-            "velocityY": -0.08461861132293771,
-            "timestamp": 1.3109539954889267
-        },
-        {
-            "x": 5.1791066790320945,
-            "y": 6.648662347878877,
-            "heading": 0.013520796271957977,
-            "angularVelocity": -0.0000014063318445457383,
-            "velocityX": 3.7819937006535858,
-            "velocityY": -0.08462018392597198,
-            "timestamp": 1.3928886202069846
-        },
-        {
-            "x": 5.488982909387702,
-            "y": 6.641728837446599,
-            "heading": 0.013520681044501265,
-            "angularVelocity": -0.000001406334100009416,
-            "velocityX": 3.781993649472511,
-            "velocityY": -0.08462247134390694,
-            "timestamp": 1.4748232449250425
-        },
-        {
-            "x": 5.798859128679326,
-            "y": 6.634794832555163,
-            "heading": 0.013520565816988437,
-            "angularVelocity": -0.0000014063347848865157,
-            "velocityX": 3.781993514438203,
-            "velocityY": -0.08462850614495548,
-            "timestamp": 1.5567578696431004
-        },
-        {
-            "x": 6.106285584248806,
-            "y": 6.595293875173847,
-            "heading": 0.013520451149743993,
-            "angularVelocity": -0.000001399496791973984,
-            "velocityX": 3.7520945098284613,
-            "velocityY": -0.48210335395128207,
-            "timestamp": 1.6386924943611583
-        },
-        {
-            "x": 6.401594301701064,
-            "y": 6.501144389493677,
-            "heading": 0.013520442621996222,
-            "angularVelocity": -1.0407990272609098e-7,
-            "velocityX": 3.604199304853526,
-            "velocityY": -1.1490805749601392,
-            "timestamp": 1.7206271190792162
-        },
-        {
-            "x": 6.675154779128766,
-            "y": 6.355418517983256,
-            "heading": 0.013520441934185553,
-            "angularVelocity": -8.394627700401504e-9,
-            "velocityX": 3.3387652456948387,
-            "velocityY": -1.7785627506304358,
-            "timestamp": 1.802561743797274
-        },
-        {
-            "x": 6.946083635654423,
-            "y": 6.204856506903887,
-            "heading": 0.013520441248790115,
-            "angularVelocity": -8.365150168101951e-9,
-            "velocityX": 3.3066467010490315,
-            "velocityY": -1.8375871201883405,
-            "timestamp": 1.884496368515332
-        },
-        {
-            "x": 7.190722413199009,
-            "y": 6.068904365786755,
-            "heading": 0.008013663922991063,
-            "angularVelocity": -0.06720940438487652,
-            "velocityX": 2.985780167864364,
-            "velocityY": -1.6592758139181236,
-            "timestamp": 1.9664309932333899
-        },
-        {
-            "x": 7.38643344699072,
-            "y": 5.96014263272892,
-            "heading": 0.0036082164005303732,
-            "angularVelocity": -0.053767836706643944,
-            "velocityX": 2.3886242777722417,
-            "velocityY": -1.3274208972347252,
-            "timestamp": 2.048365617951448
-        },
-        {
-            "x": 7.533216723981145,
-            "y": 5.878571322292618,
-            "heading": 0.0003041162966361105,
-            "angularVelocity": -0.04032605403715309,
-            "velocityX": 1.7914682284261954,
-            "velocityY": -0.9955658028213874,
-            "timestamp": 2.130300242669506
-        },
-        {
-            "x": 7.631072241649754,
-            "y": 5.82419044260697,
-            "heading": -0.0018986254325586427,
-            "angularVelocity": -0.026884137649675257,
-            "velocityX": 1.1943121483174686,
-            "velocityY": -0.66371060919331,
-            "timestamp": 2.212234867387564
+            "x": 0.744719766454751,
+            "y": 6.720759968199705,
+            "heading": 1.006760857121406,
+            "angularVelocity": -0.5061419234074507,
+            "velocityX": 0.5286878015432446,
+            "velocityY": 0.011572252862695013,
+            "timestamp": 0.06567158605400746
+        },
+        {
+            "x": 0.8143576636925598,
+            "y": 6.722372451876338,
+            "heading": 0.9431499891720047,
+            "angularVelocity": -0.9686208567749168,
+            "velocityX": 1.0603961533765867,
+            "velocityY": 0.024553749551892837,
+            "timestamp": 0.1313431721080149
+        },
+        {
+            "x": 0.9191612228651693,
+            "y": 6.724923861576486,
+            "heading": 0.8530685118915011,
+            "angularVelocity": -1.3716963864162066,
+            "velocityX": 1.5958737327650472,
+            "velocityY": 0.03885104431723037,
+            "timestamp": 0.19701475816202235
+        },
+        {
+            "x": 1.059454644842809,
+            "y": 6.7284663342548425,
+            "heading": 0.7426650921374723,
+            "angularVelocity": -1.6811444094442096,
+            "velocityX": 2.1362880114128,
+            "velocityY": 0.05394224338464699,
+            "timestamp": 0.2626863442160298
+        },
+        {
+            "x": 1.2356244745863587,
+            "y": 6.733039509062333,
+            "heading": 0.6243118436370656,
+            "angularVelocity": -1.8021987226420155,
+            "velocityX": 2.682588320596835,
+            "velocityY": 0.06963703912619325,
+            "timestamp": 0.3283579302700373
+        },
+        {
+            "x": 1.4470569399809903,
+            "y": 6.738777367450543,
+            "heading": 0.5308220213971302,
+            "angularVelocity": -1.4235962287107038,
+            "velocityX": 3.219542546463745,
+            "velocityY": 0.08737200870294055,
+            "timestamp": 0.39402951632404476
+        },
+        {
+            "x": 1.688226065456379,
+            "y": 6.748371696739962,
+            "heading": 0.5126606987274956,
+            "angularVelocity": -0.2765476480908373,
+            "velocityX": 3.6723511638207493,
+            "velocityY": 0.14609559272058092,
+            "timestamp": 0.45970110237805223
+        },
+        {
+            "x": 1.9365780121804386,
+            "y": 6.754665679712547,
+            "heading": 0.5126606479364623,
+            "angularVelocity": -7.734095747362075e-7,
+            "velocityX": 3.7817260347544686,
+            "velocityY": 0.09584027660688858,
+            "timestamp": 0.5253726884320596
+        },
+        {
+            "x": 2.1849299738494317,
+            "y": 6.760959072962077,
+            "heading": 0.5126605971462416,
+            "angularVelocity": -7.733971982996713e-7,
+            "velocityX": 3.7817262623252414,
+            "velocityY": 0.09583129672545204,
+            "timestamp": 0.5910442744860671
+        },
+        {
+            "x": 2.433281935680659,
+            "y": 6.767252459809602,
+            "heading": 0.5126605463560266,
+            "angularVelocity": -7.733971099299294e-7,
+            "velocityX": 3.7817262647956325,
+            "velocityY": 0.09583119924030911,
+            "timestamp": 0.6567158605400746
+        },
+        {
+            "x": 2.6816338977021794,
+            "y": 6.773545839147862,
+            "heading": 0.5126604955658177,
+            "angularVelocity": -7.733970196193196e-7,
+            "velocityX": 3.78172626769327,
+            "velocityY": 0.09583108489457716,
+            "timestamp": 0.722387446594082
+        },
+        {
+            "x": 2.9299858599438156,
+            "y": 6.779839209799928,
+            "heading": 0.5126604447756148,
+            "angularVelocity": -7.733969286509421e-7,
+            "velocityX": 3.7817262710450463,
+            "velocityY": 0.09583095262742326,
+            "timestamp": 0.7880590326480895
+        },
+        {
+            "x": 3.1783378224424883,
+            "y": 6.786132570308856,
+            "heading": 0.5126603939854177,
+            "angularVelocity": -7.733968385113031e-7,
+            "velocityX": 3.781726274959007,
+            "velocityY": 0.09583079817497217,
+            "timestamp": 0.853730618702097
+        },
+        {
+            "x": 3.426689785244194,
+            "y": 6.792425918859452,
+            "heading": 0.5126603431952265,
+            "angularVelocity": -7.733967515033378e-7,
+            "velocityX": 3.7817262795733857,
+            "velocityY": 0.09583061608201786,
+            "timestamp": 0.9194022047561045
+        },
+        {
+            "x": 3.6750417484067377,
+            "y": 6.798719253170605,
+            "heading": 0.5126602924050404,
+            "angularVelocity": -7.733966712943809e-7,
+            "velocityX": 3.781726285067957,
+            "velocityY": 0.09583039925395942,
+            "timestamp": 0.9850737908101119
+        },
+        {
+            "x": 3.9233937120031053,
+            "y": 6.805012570362013,
+            "heading": 0.5126602416148589,
+            "angularVelocity": -7.733966025007139e-7,
+            "velocityX": 3.7817262916739214,
+            "velocityY": 0.095830138566711,
+            "timestamp": 1.0507453768641193
+        },
+        {
+            "x": 4.171745676125189,
+            "y": 6.811305866807266,
+            "heading": 0.5126601908246804,
+            "angularVelocity": -7.733965561950186e-7,
+            "velocityX": 3.7817262996791623,
+            "velocityY": 0.09582982265909266,
+            "timestamp": 1.1164169629181266
+        },
+        {
+            "x": 4.420097640886595,
+            "y": 6.817599138023107,
+            "heading": 0.5126601400345029,
+            "angularVelocity": -7.733965406200526e-7,
+            "velocityX": 3.781726309414286,
+            "velocityY": 0.09582943848362173,
+            "timestamp": 1.182088548972134
+        },
+        {
+            "x": 4.6684496064150185,
+            "y": 6.8238923789700765,
+            "heading": 0.5126600892443262,
+            "angularVelocity": -7.73396529455525e-7,
+            "velocityX": 3.7817263210938923,
+            "velocityY": 0.09582897757087808,
+            "timestamp": 1.2477601350261414
+        },
+        {
+            "x": 4.916801572841165,
+            "y": 6.830185584489968,
+            "heading": 0.5126600384541553,
+            "angularVelocity": -7.733964406232834e-7,
+            "velocityX": 3.7817263347637717,
+            "velocityY": 0.09582843811261686,
+            "timestamp": 1.3134317210801487
+        },
+        {
+            "x": 5.165153539254683,
+            "y": 6.8364787905062485,
+            "heading": 0.5126599876639959,
+            "angularVelocity": -7.733962639482034e-7,
+            "velocityX": 3.781726334571479,
+            "velocityY": 0.09582844567126618,
+            "timestamp": 1.379103307134156
+        },
+        {
+            "x": 5.413504500375102,
+            "y": 6.842811545538402,
+            "heading": 0.5126599368726894,
+            "angularVelocity": -7.734137320837285e-7,
+            "velocityX": 3.781711026686433,
+            "velocityY": 0.09643066982787928,
+            "timestamp": 1.4447748931881634
+        },
+        {
+            "x": 5.661808131063952,
+            "y": 6.834835924305975,
+            "heading": 0.5126598860199024,
+            "angularVelocity": -7.743499133059843e-7,
+            "velocityX": 3.780990312684674,
+            "velocityY": -0.12144706275568991,
+            "timestamp": 1.5104464792421708
+        },
+        {
+            "x": 5.9063591957092285,
+            "y": 6.791097164154053,
+            "heading": 0.5126598314406843,
+            "angularVelocity": -8.310933457001734e-7,
+            "velocityX": 3.723848917614642,
+            "velocityY": -0.6660225948568639,
+            "timestamp": 1.5761180652961782
+        },
+        {
+            "x": 6.194622556481831,
+            "y": 6.683762426219015,
+            "heading": 0.5126597887806125,
+            "angularVelocity": -5.246475178274693e-7,
+            "velocityX": 3.5451571072929595,
+            "velocityY": -1.3200377183874616,
+            "timestamp": 1.65742993725234
+        },
+        {
+            "x": 6.459547388261047,
+            "y": 6.527457130068993,
+            "heading": 0.5126597482300577,
+            "angularVelocity": -4.987039864896594e-7,
+            "velocityX": 3.2581322432449182,
+            "velocityY": -1.9222936625379825,
+            "timestamp": 1.7387418092085019
+        },
+        {
+            "x": 6.723521508223802,
+            "y": 6.3695515377198895,
+            "heading": 0.5126597076877473,
+            "angularVelocity": -4.98602596336278e-7,
+            "velocityX": 3.2464400783305223,
+            "velocityY": -1.9419746286809643,
+            "timestamp": 1.8200536811646637
+        },
+        {
+            "x": 6.987495332330779,
+            "y": 6.211645450805702,
+            "heading": 0.5126596671480953,
+            "angularVelocity": -4.985699012832353e-7,
+            "velocityX": 3.246436439802837,
+            "velocityY": -1.9419807109984206,
+            "timestamp": 1.9013655531208256
+        },
+        {
+            "x": 7.218138260463332,
+            "y": 6.072816355413413,
+            "heading": 0.4174347514218684,
+            "angularVelocity": -1.1711071635093024,
+            "velocityX": 2.8365221779297074,
+            "velocityY": -1.7073656288101424,
+            "timestamp": 1.9826774250769874
+        },
+        {
+            "x": 7.40359391202109,
+            "y": 5.961763553999033,
+            "heading": 0.26911254592387807,
+            "angularVelocity": -1.8241150022712385,
+            "velocityX": 2.2807942689821186,
+            "velocityY": -1.365763679308134,
+            "timestamp": 2.063989297033149
+        },
+        {
+            "x": 7.542069024042313,
+            "y": 5.879081457057784,
+            "heading": 0.13934312775190755,
+            "angularVelocity": -1.595946754760921,
+            "velocityX": 1.7030122255201257,
+            "velocityY": -1.0168514750927211,
+            "timestamp": 2.145301168989311
+        },
+        {
+            "x": 7.63409233729861,
+            "y": 5.824280880315082,
+            "heading": 0.04592275308495019,
+            "angularVelocity": -1.1489143272623823,
+            "velocityX": 1.1317328090287153,
+            "velocityY": -0.6739554191077514,
+            "timestamp": 2.2266130409454727
         },
         {
             "x": 7.68,
             "y": 5.797,
             "heading": -0.003,
-            "angularVelocity": -0.013442114017501823,
-            "velocityX": 0.5971560682508651,
-            "velocityY": -0.3318553383326643,
-            "timestamp": 2.294169492105622
+            "angularVelocity": -0.6016680210157614,
+            "velocityX": 0.5645874531846384,
+            "velocityY": -0.3355091902172136,
+            "timestamp": 2.3079249129016346
         },
         {
             "x": 7.68,
             "y": 5.797,
             "heading": -0.003,
-            "angularVelocity": -2.3555736468302963e-30,
+            "angularVelocity": 3.440382093642737e-30,
             "velocityX": 0,
-            "velocityY": 5.757868192156219e-33,
-            "timestamp": 2.3761041168236803
+            "velocityY": 2.1122019835026996e-31,
+            "timestamp": 2.3892367848577964
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.1.traj
index 17baf1d1..6231149d 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.1.traj	
@@ -5,657 +5,801 @@
             "y": 6.72,
             "heading": 1.04,
             "angularVelocity": 0,
-            "velocityX": -2.70270630963226e-31,
-            "velocityY": -3.181766396851851e-31,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.7256414720586181,
-            "y": 6.712155337793633,
-            "heading": 1.0237015987984905,
-            "angularVelocity": -0.3502584296953155,
-            "velocityX": 0.33614078912617174,
-            "velocityY": -0.16858457660462928,
-            "timestamp": 0.046532502346056014
-        },
-        {
-            "x": 0.756979104040484,
-            "y": 6.696435827024302,
-            "heading": 0.9920873730999193,
-            "angularVelocity": -0.6794009370802031,
-            "velocityX": 0.6734568398839507,
-            "velocityY": -0.33781786873231273,
-            "timestamp": 0.09306500469211203
-        },
-        {
-            "x": 0.8040789517219918,
-            "y": 6.672807532790365,
-            "heading": 0.9464055818688567,
-            "angularVelocity": -0.9817179160357762,
-            "velocityX": 1.0121924527341073,
-            "velocityY": -0.5077804339471543,
-            "timestamp": 0.13959750703816803
-        },
-        {
-            "x": 0.8670206146686451,
-            "y": 6.641234227942618,
-            "heading": 0.8882606987188267,
-            "angularVelocity": -1.2495541872573583,
-            "velocityX": 1.352638688514273,
-            "velocityY": -0.6785215334633973,
-            "timestamp": 0.18613000938422405
-        },
-        {
-            "x": 0.9458974559747272,
-            "y": 6.601677384330224,
-            "heading": 0.8197602775960398,
-            "angularVelocity": -1.4720983757420398,
-            "velocityX": 1.6950913303456163,
-            "velocityY": -0.8500906166232347,
-            "timestamp": 0.23266251173028007
-        },
-        {
-            "x": 1.04081251945038,
-            "y": 6.554094078416389,
-            "heading": 0.743830819371293,
-            "angularVelocity": -1.6317510212566968,
-            "velocityX": 2.039758420249741,
-            "velocityY": -1.0225821418316814,
-            "timestamp": 0.27919501407633607
-        },
-        {
-            "x": 1.15185810993152,
-            "y": 6.498440545281296,
-            "heading": 0.6650543509456113,
-            "angularVelocity": -1.6929342815026889,
-            "velocityX": 2.3864091738569346,
-            "velocityY": -1.1960141907087913,
-            "timestamp": 0.32572751642239206
-        },
-        {
-            "x": 1.2789797685079123,
-            "y": 6.43473905676892,
-            "heading": 0.5921954143923897,
-            "angularVelocity": -1.5657644201343583,
-            "velocityX": 2.7318895861437724,
-            "velocityY": -1.3689676097497174,
-            "timestamp": 0.37226001876844805
-        },
-        {
-            "x": 1.420223914518488,
-            "y": 6.363052522149474,
-            "heading": 0.5479689699773799,
-            "angularVelocity": -0.9504419961367557,
-            "velocityX": 3.035386856270174,
-            "velocityY": -1.5405690862335149,
-            "timestamp": 0.41879252111450405
-        },
-        {
-            "x": 1.5740028534513755,
-            "y": 6.283114192872816,
-            "heading": 0.5411402298477218,
-            "angularVelocity": -0.14675205040326236,
-            "velocityX": 3.304764007515849,
-            "velocityY": -1.717903083787275,
-            "timestamp": 0.46532502346056004
-        },
-        {
-            "x": 1.7314538549626566,
-            "y": 6.20440189931456,
-            "heading": 0.5411401828527235,
-            "angularVelocity": -0.0000010099392058430412,
-            "velocityX": 3.383677936345371,
-            "velocityY": -1.6915551408103213,
-            "timestamp": 0.511857525806616
-        },
-        {
-            "x": 1.888910012901083,
-            "y": 6.125699921176338,
-            "heading": 0.5411401358667378,
-            "angularVelocity": -0.0000010097455172737006,
-            "velocityX": 3.383788749796141,
-            "velocityY": -1.6913334587700644,
-            "timestamp": 0.558390028152672
-        },
-        {
-            "x": 2.0463658388325006,
-            "y": 6.046997278808062,
-            "heading": 0.5411400888807237,
-            "angularVelocity": -0.0000010097461271838877,
-            "velocityX": 3.3837816148475093,
-            "velocityY": -1.691347733310028,
-            "timestamp": 0.604922530498728
-        },
-        {
-            "x": 2.2038216598445532,
-            "y": 5.9682946265981345,
-            "heading": 0.5411400418947097,
-            "angularVelocity": -0.0000010097461301639238,
-            "velocityX": 3.3837815091285663,
-            "velocityY": -1.6913479448107236,
-            "timestamp": 0.651455032844784
-        },
-        {
-            "x": 2.3612775122085363,
-            "y": 5.8895920371121315,
-            "heading": 0.5411399949086985,
-            "angularVelocity": -0.0000010097460671631127,
-            "velocityX": 3.3837821828926944,
-            "velocityY": -1.6913465968512624,
-            "timestamp": 0.69798753519084
-        },
-        {
-            "x": 2.5187333978232895,
-            "y": 5.810889514148883,
-            "heading": 0.5411399479226903,
-            "angularVelocity": -0.0000010097460006294838,
-            "velocityX": 3.383782897463604,
-            "velocityY": -1.6913451672535464,
-            "timestamp": 0.744520037536896
-        },
-        {
-            "x": 2.676189337500118,
-            "y": 5.732187099344657,
-            "heading": 0.541139900936687,
-            "angularVelocity": -0.0000010097458968738495,
-            "velocityX": 3.3837840592768114,
-            "velocityY": -1.6913428428777157,
-            "timestamp": 0.791052539882952
-        },
-        {
-            "x": 2.833645305906935,
-            "y": 5.653484742019454,
-            "heading": 0.5411398539506863,
-            "angularVelocity": -0.0000010097458381135432,
-            "velocityX": 3.3837846766944186,
-            "velocityY": -1.6913416076332204,
-            "timestamp": 0.837585042229008
-        },
-        {
-            "x": 2.991101010217502,
-            "y": 5.57478185633263,
-            "heading": 0.5411398069646632,
-            "angularVelocity": -0.0000010097463224464638,
-            "velocityX": 3.383779001172013,
-            "velocityY": -1.6913529623124643,
-            "timestamp": 0.884117544575064
-        },
-        {
-            "x": 3.14855589978306,
-            "y": 5.496077340659936,
-            "heading": 0.5411397599785701,
-            "angularVelocity": -0.0000010097478275290572,
-            "velocityX": 3.3837614920124643,
-            "velocityY": -1.6913879912875969,
-            "timestamp": 0.93065004692112
-        },
-        {
-            "x": 3.306010013541662,
-            "y": 5.417371272923184,
-            "heading": 0.5411397129924104,
-            "angularVelocity": -0.0000010097492581811278,
-            "velocityX": 3.3837448196468105,
-            "velocityY": -1.6914213456927205,
-            "timestamp": 0.977182549267176
-        },
-        {
-            "x": 3.4634680562159517,
-            "y": 5.338673065406879,
-            "heading": 0.5411396660065909,
-            "angularVelocity": -0.0000010097419454671228,
-            "velocityX": 3.383829253443873,
-            "velocityY": -1.6912524267565827,
-            "timestamp": 1.023715051613232
-        },
-        {
-            "x": 3.6209395624979943,
-            "y": 5.2600018013447745,
-            "heading": 0.5411396190219294,
-            "angularVelocity": -0.0000010097170609305348,
-            "velocityX": 3.3841185911564384,
-            "velocityY": -1.690673402356174,
-            "timestamp": 1.0702475539592882
-        },
-        {
-            "x": 3.778386684395729,
-            "y": 5.181281747313384,
-            "heading": 0.5411395720351826,
-            "angularVelocity": -0.0000010097618775946008,
-            "velocityX": 3.3835945620651766,
-            "velocityY": -1.6917219161486032,
-            "timestamp": 1.1167800563053443
-        },
-        {
-            "x": 3.9357449151310644,
-            "y": 5.10238415335025,
-            "heading": 0.5411395250405994,
-            "angularVelocity": -0.000001009930280464059,
-            "velocityX": 3.3816842594175633,
-            "velocityY": -1.6955373123309678,
-            "timestamp": 1.1633125586514004
+            "x": 0.7237404107060486,
+            "y": 6.712071337633356,
+            "heading": 0.9558489426883627,
+            "angularVelocity": -1.537881157714612,
+            "velocityX": 0.25110936688338653,
+            "velocityY": -0.14489824428928566,
+            "timestamp": 0.05471882979351295
+        },
+        {
+            "x": 0.7514918072858734,
+            "y": 6.694799584586779,
+            "heading": 0.7935491608961526,
+            "angularVelocity": -2.966068214628546,
+            "velocityX": 0.5071635611460984,
+            "velocityY": -0.31564551200661234,
+            "timestamp": 0.1094376595870259
+        },
+        {
+            "x": 0.7959632333479747,
+            "y": 6.666655428800684,
+            "heading": 0.574279005326375,
+            "angularVelocity": -4.007215731718184,
+            "velocityX": 0.8127261900504715,
+            "velocityY": -0.514341331718894,
+            "timestamp": 0.16415648938053887
+        },
+        {
+            "x": 0.8617224500002636,
+            "y": 6.63045184710829,
+            "heading": 0.31905346805964874,
+            "angularVelocity": -4.664309127769047,
+            "velocityX": 1.2017657705846174,
+            "velocityY": -0.6616293116832328,
+            "timestamp": 0.2188753191740518
+        },
+        {
+            "x": 0.9507709996198539,
+            "y": 6.585947725507511,
+            "heading": 0.05142997722620149,
+            "angularVelocity": -4.890884762034389,
+            "velocityX": 1.627384027685255,
+            "velocityY": -0.8133237090178916,
+            "timestamp": 0.2735941489675648
+        },
+        {
+            "x": 1.0614190010918925,
+            "y": 6.535018660742887,
+            "heading": -0.17495854287380355,
+            "angularVelocity": -4.137305584828904,
+            "velocityX": 2.0221192940269503,
+            "velocityY": -0.9307411170306262,
+            "timestamp": 0.32831297876107773
+        },
+        {
+            "x": 1.1921384640832198,
+            "y": 6.474205616338843,
+            "heading": -0.35862575739678126,
+            "angularVelocity": -3.356563274764148,
+            "velocityX": 2.388930163247485,
+            "velocityY": -1.111373262796896,
+            "timestamp": 0.3830318085545907
+        },
+        {
+            "x": 1.3384031857193457,
+            "y": 6.398894346098027,
+            "heading": -0.49152044440666937,
+            "angularVelocity": -2.4286829142980233,
+            "velocityX": 2.6730235677201417,
+            "velocityY": -1.376331886573771,
+            "timestamp": 0.4377506383481036
+        },
+        {
+            "x": 1.500957547301863,
+            "y": 6.312259668262383,
+            "heading": -0.5624501430724521,
+            "angularVelocity": -1.2962575942036991,
+            "velocityX": 2.9707207225727275,
+            "velocityY": -1.5832699303433067,
+            "timestamp": 0.4924694681416166
+        },
+        {
+            "x": 1.6817069101290123,
+            "y": 6.217739107445247,
+            "heading": -0.5720464000476684,
+            "angularVelocity": -0.17537394369443057,
+            "velocityX": 3.3032388212471107,
+            "velocityY": -1.7273863709040396,
+            "timestamp": 0.5471882979351296
+        },
+        {
+            "x": 1.8673643551319996,
+            "y": 6.126199810501224,
+            "heading": -0.5720469984173726,
+            "angularVelocity": -0.000010935352720012522,
+            "velocityX": 3.392935223643913,
+            "velocityY": -1.6729030443351731,
+            "timestamp": 0.6019071277286425
+        },
+        {
+            "x": 2.0530162492759167,
+            "y": 6.034649256644459,
+            "heading": -0.5720475972653951,
+            "angularVelocity": -0.000010944094102204816,
+            "velocityX": 3.3928337803365705,
+            "velocityY": -1.6731087671691436,
+            "timestamp": 0.6566259575221555
+        },
+        {
+            "x": 2.23866813267496,
+            "y": 5.943098680998449,
+            "heading": -0.5720481961130315,
+            "angularVelocity": -0.000010944087046546937,
+            "velocityX": 3.3928335839713664,
+            "velocityY": -1.673109165372953,
+            "timestamp": 0.7113447873156684
+        },
+        {
+            "x": 2.4243200161477754,
+            "y": 5.851548105501653,
+            "heading": -0.5720487949598734,
+            "angularVelocity": -0.000010944072528266654,
+            "velocityX": 3.392833585319575,
+            "velocityY": -1.673109162646019,
+            "timestamp": 0.7660636171091814
+        },
+        {
+            "x": 2.6099718996232633,
+            "y": 5.759997530009892,
+            "heading": -0.5720493938059237,
+            "angularVelocity": -0.000010944058060277418,
+            "velocityX": 3.3928335853684146,
+            "velocityY": -1.673109162553994,
+            "timestamp": 0.8207824469026943
+        },
+        {
+            "x": 2.7956237830994004,
+            "y": 5.668446954519065,
+            "heading": -0.5720499926511823,
+            "angularVelocity": -0.000010944043595406558,
+            "velocityX": 3.3928335853802802,
+            "velocityY": -1.6731091625369476,
+            "timestamp": 0.8755012766962073
+        },
+        {
+            "x": 2.9812756665756717,
+            "y": 5.576896379028125,
+            "heading": -0.5720505914956495,
+            "angularVelocity": -0.000010944029129446314,
+            "velocityX": 3.3928335853827263,
+            "velocityY": -1.6731091625390042,
+            "timestamp": 0.9302201064897202
+        },
+        {
+            "x": 3.166927550036217,
+            "y": 5.485345803504912,
+            "heading": -0.5720511903393257,
+            "angularVelocity": -0.000010944014673786385,
+            "velocityX": 3.392833585095338,
+            "velocityY": -1.673109163128796,
+            "timestamp": 0.9849389362832331
+        },
+        {
+            "x": 3.3525794334560626,
+            "y": 5.393795227898782,
+            "heading": -0.5720517891822119,
+            "angularVelocity": -0.000010944000236842356,
+            "velocityX": 3.392833584351539,
+            "velocityY": -1.6731091646441256,
+            "timestamp": 1.039657766076746
+        },
+        {
+            "x": 3.538231313226632,
+            "y": 5.302244644892125,
+            "heading": -0.5720523880244444,
+            "angularVelocity": -0.000010943988288632917,
+            "velocityX": 3.392833517660122,
+            "velocityY": -1.673109299890567,
+            "timestamp": 1.0943765958702592
+        },
+        {
+            "x": 3.723883292814953,
+            "y": 5.21069426429933,
+            "heading": -0.5720529868621156,
+            "angularVelocity": -0.000010943904932797782,
+            "velocityX": 3.3928353418539645,
+            "velocityY": -1.673105600727685,
+            "timestamp": 1.1490954256637722
+        },
+        {
+            "x": 3.909492929164672,
+            "y": 5.1190580682844296,
+            "heading": -0.5720535873273735,
+            "angularVelocity": -0.000010973649477031932,
+            "velocityX": 3.3920615088102064,
+            "velocityY": -1.6746738985594591,
+            "timestamp": 1.2038142554572853
         },
         {
             "x": 4.092995643615723,
             "y": 5.023272514343262,
-            "heading": 0.5411394780180799,
-            "angularVelocity": -0.0000010105306414990813,
-            "velocityX": 3.379373997880459,
-            "velocityY": -1.7001372163227824,
-            "timestamp": 1.2098450609974565
-        },
-        {
-            "x": 4.2612300154414715,
-            "y": 4.916069066513277,
-            "heading": 0.541139432730096,
-            "angularVelocity": -8.588080638060829e-7,
-            "velocityX": 3.1902730598822937,
-            "velocityY": -2.0329274441782497,
-            "timestamp": 1.2625785946693153
-        },
-        {
-            "x": 4.429297905294805,
-            "y": 4.808604804974877,
-            "heading": 0.5411393874448718,
-            "angularVelocity": -8.58755730533247e-7,
-            "velocityX": 3.1871160180389406,
-            "velocityY": -2.0378733237761697,
-            "timestamp": 1.3153121283411742
-        },
-        {
-            "x": 4.5972191269209945,
-            "y": 4.700911507061467,
-            "heading": 0.5411393421481548,
-            "angularVelocity": -8.589736709635531e-7,
-            "velocityX": 3.1843347095055985,
-            "velocityY": -2.042216601367096,
-            "timestamp": 1.368045662013033
-        },
-        {
-            "x": 4.765178956046694,
-            "y": 4.593278431809082,
-            "heading": 0.5411392968544698,
-            "angularVelocity": -8.589161734970966e-7,
-            "velocityX": 3.185066833769243,
-            "velocityY": -2.0410745830567376,
-            "timestamp": 1.4207791956848919
-        },
-        {
-            "x": 4.9331339491528,
-            "y": 4.485637810244685,
-            "heading": 0.5411392515604048,
-            "angularVelocity": -8.58923380695705e-7,
-            "velocityX": 3.1849751270448214,
-            "velocityY": -2.041217685776372,
-            "timestamp": 1.4735127293567507
-        },
-        {
-            "x": 5.1010877481253685,
-            "y": 4.377995325600118,
-            "heading": 0.5411392062662455,
-            "angularVelocity": -8.589251678490757e-7,
-            "velocityX": 3.1849524823742743,
-            "velocityY": -2.041253015859115,
-            "timestamp": 1.5262462630286096
-        },
-        {
-            "x": 5.269016703536996,
-            "y": 4.270314087571009,
-            "heading": 0.5411391609701367,
-            "angularVelocity": -8.589621362197366e-7,
-            "velocityX": 3.184481367332135,
-            "velocityY": -2.0419879065793034,
-            "timestamp": 1.5789797967004684
-        },
-        {
-            "x": 5.437056059940434,
-            "y": 4.162805214274757,
-            "heading": 0.5411391156827058,
-            "angularVelocity": -8.587975760545075e-7,
-            "velocityX": 3.18657493065327,
-            "velocityY": -2.038719308384206,
-            "timestamp": 1.6317133303723272
-        },
-        {
-            "x": 5.605213513282879,
-            "y": 4.0554811537322735,
-            "heading": 0.5411390703937885,
-            "angularVelocity": -8.588257618591655e-7,
-            "velocityX": 3.188814434261697,
-            "velocityY": -2.03521465506778,
-            "timestamp": 1.684446864044186
+            "heading": -0.5720542830492642,
+            "angularVelocity": -0.000012714487741704976,
+            "velocityX": 3.353556995709023,
+            "velocityY": -1.7505044296932655,
+            "timestamp": 1.2585330852507983
+        },
+        {
+            "x": 4.152699494738863,
+            "y": 4.989124335806784,
+            "heading": -0.5720542274621303,
+            "angularVelocity": 0.0000030573365885939726,
+            "velocityX": 3.283759307267904,
+            "velocityY": -1.8781769850015708,
+            "timestamp": 1.2767146404552234
+        },
+        {
+            "x": 4.211236123155136,
+            "y": 4.953011857528808,
+            "heading": -0.5720541720789936,
+            "angularVelocity": 0.000003046116576982166,
+            "velocityX": 3.2195611298433677,
+            "velocityY": -1.9862150334199096,
+            "timestamp": 1.2948961956596485
+        },
+        {
+            "x": 4.269217408443118,
+            "y": 4.916014310130808,
+            "heading": -0.5720541167402081,
+            "angularVelocity": 0.000003043677217870583,
+            "velocityX": 3.1890168160017764,
+            "velocityY": -2.034894539109796,
+            "timestamp": 1.3130777508640736
+        },
+        {
+            "x": 4.327172181408397,
+            "y": 4.8789752463128,
+            "heading": -0.5720540614037476,
+            "angularVelocity": 0.000003043549349895456,
+            "velocityX": 3.187558617163689,
+            "velocityY": -2.0371779752347874,
+            "timestamp": 1.3312593060684987
+        },
+        {
+            "x": 4.385108415981365,
+            "y": 4.841907191568928,
+            "heading": -0.5720540060672449,
+            "angularVelocity": 0.0000030435516711711228,
+            "velocityX": 3.186538990839725,
+            "velocityY": -2.0387724992190503,
+            "timestamp": 1.3494408612729238
+        },
+        {
+            "x": 4.443034277698237,
+            "y": 4.804822929374826,
+            "heading": -0.5720539507307192,
+            "angularVelocity": 0.000003043552934036031,
+            "velocityX": 3.1859684755020403,
+            "velocityY": -2.0396639218758956,
+            "timestamp": 1.367622416477349
+        },
+        {
+            "x": 4.500957639641057,
+            "y": 4.767734762799275,
+            "heading": -0.5720538953941279,
+            "angularVelocity": 0.0000030435565398491907,
+            "velocityX": 3.1858309859372986,
+            "velocityY": -2.039878665966735,
+            "timestamp": 1.385803971681774
+        },
+        {
+            "x": 4.558883202891902,
+            "y": 4.7306500344023386,
+            "heading": -0.5720538400575734,
+            "angularVelocity": 0.000003043554516316428,
+            "velocityX": 3.1859520596302042,
+            "velocityY": -2.03968956340728,
+            "timestamp": 1.4039855268861992
+        },
+        {
+            "x": 4.616809360440233,
+            "y": 4.693566234312182,
+            "heading": -0.572053784721022,
+            "angularVelocity": 0.000003043554346667464,
+            "velocityX": 3.1859847464654854,
+            "velocityY": -2.039638505794633,
+            "timestamp": 1.4221670820906243
+        },
+        {
+            "x": 4.674735447300298,
+            "y": 4.656482323806888,
+            "heading": -0.5720537293844703,
+            "angularVelocity": 0.00000304355436453364,
+            "velocityX": 3.185980858554995,
+            "velocityY": -2.039644578714451,
+            "timestamp": 1.4403486372950494
+        },
+        {
+            "x": 4.732661331177625,
+            "y": 4.619398096238942,
+            "heading": -0.5720536740479176,
+            "angularVelocity": 0.0000030435544255368,
+            "velocityX": 3.1859696943429383,
+            "velocityY": -2.0396620174125277,
+            "timestamp": 1.4585301924994745
+        },
+        {
+            "x": 4.790587117727679,
+            "y": 4.582313716645222,
+            "heading": -0.5720536187113643,
+            "angularVelocity": 0.0000030435544545803464,
+            "velocityX": 3.1859643412654055,
+            "velocityY": -2.039670378951063,
+            "timestamp": 1.4767117477038996
+        },
+        {
+            "x": 4.848512993160408,
+            "y": 4.5452294758867104,
+            "heading": -0.5720535633748114,
+            "angularVelocity": 0.0000030435544295946876,
+            "velocityX": 3.1859692298833355,
+            "velocityY": -2.0396627429041696,
+            "timestamp": 1.4948933029083247
+        },
+        {
+            "x": 4.906438819485911,
+            "y": 4.508145158422396,
+            "heading": -0.5720535080382583,
+            "angularVelocity": 0.0000030435544442083865,
+            "velocityX": 3.1859665289468815,
+            "velocityY": -2.039666961784329,
+            "timestamp": 1.5130748581127498
+        },
+        {
+            "x": 4.9643646433421065,
+            "y": 4.471060837101057,
+            "heading": -0.5720534527017052,
+            "angularVelocity": 0.0000030435544458805193,
+            "velocityX": 3.185966393132936,
+            "velocityY": -2.039667173923745,
+            "timestamp": 1.5312564133171749
+        },
+        {
+            "x": 5.022290480818567,
+            "y": 4.433976537054754,
+            "heading": -0.572053397365152,
+            "angularVelocity": 0.0000030435544415454932,
+            "velocityX": 3.185967142258403,
+            "velocityY": -2.039666003779759,
+            "timestamp": 1.5494379685216
+        },
+        {
+            "x": 5.080216347467466,
+            "y": 4.396892282576102,
+            "heading": -0.572053342028599,
+            "angularVelocity": 0.000003043554433808661,
+            "velocityX": 3.1859687467658464,
+            "velocityY": -2.039663497522643,
+            "timestamp": 1.567619523726025
+        },
+        {
+            "x": 5.138142382220086,
+            "y": 4.359808290677983,
+            "heading": -0.5720532866920471,
+            "angularVelocity": 0.000003043554377469469,
+            "velocityX": 3.185977992604117,
+            "velocityY": -2.039649055384386,
+            "timestamp": 1.5858010789304502
+        },
+        {
+            "x": 5.196068138494407,
+            "y": 4.322723863792993,
+            "heading": -0.5720532313554916,
+            "angularVelocity": 0.000003043554576021048,
+            "velocityX": 3.1859626760759787,
+            "velocityY": -2.0396729800088242,
+            "timestamp": 1.6039826341348753
+        },
+        {
+            "x": 5.253993690845546,
+            "y": 4.285639118381674,
+            "heading": -0.5720531760189423,
+            "angularVelocity": 0.0000030435542278177066,
+            "velocityX": 3.1859514601384813,
+            "velocityY": -2.0396904992104243,
+            "timestamp": 1.6221641893393004
+        },
+        {
+            "x": 5.3119198680466875,
+            "y": 4.248555348991913,
+            "heading": -0.5720531206823879,
+            "angularVelocity": 0.000003043554518420827,
+            "velocityX": 3.1859858273860655,
+            "velocityY": -2.0396368172479113,
+            "timestamp": 1.6403457445437255
+        },
+        {
+            "x": 5.369845451022558,
+            "y": 4.211470651416335,
+            "heading": -0.5720530653458327,
+            "angularVelocity": 0.0000030435545594837867,
+            "velocityX": 3.1859531445232334,
+            "velocityY": -2.039687868206526,
+            "timestamp": 1.6585272997481506
+        },
+        {
+            "x": 5.427769955136907,
+            "y": 4.17438426873464,
+            "heading": -0.5720530100092767,
+            "angularVelocity": 0.0000030435545998300077,
+            "velocityX": 3.185893806281465,
+            "velocityY": -2.039780550383344,
+            "timestamp": 1.6767088549525757
+        },
+        {
+            "x": 5.485698211873678,
+            "y": 4.137303747848898,
+            "heading": -0.5720529546727399,
+            "angularVelocity": 0.000003043553546723704,
+            "velocityX": 3.1861002035013715,
+            "velocityY": -2.0394581469404574,
+            "timestamp": 1.6948904101570008
+        },
+        {
+            "x": 5.5436372749681775,
+            "y": 4.100240114388813,
+            "heading": -0.5720528993360909,
+            "angularVelocity": 0.0000030435597092212927,
+            "velocityX": 3.186694561772871,
+            "velocityY": -2.0385293250953618,
+            "timestamp": 1.713071965361426
+        },
+        {
+            "x": 5.601833960036281,
+            "y": 4.063582321171135,
+            "heading": -0.5720528439855975,
+            "angularVelocity": 0.0000030443211669066955,
+            "velocityX": 3.2008639752640304,
+            "velocityY": -2.0162077889097265,
+            "timestamp": 1.731253520565851
+        },
+        {
+            "x": 5.661045954546234,
+            "y": 4.0285882123488195,
+            "heading": -0.5720527885077281,
+            "angularVelocity": 0.000003051326955110969,
+            "velocityX": 3.2567068022627845,
+            "velocityY": -1.9247038236737246,
+            "timestamp": 1.7494350757702761
+        },
+        {
+            "x": 5.721517200862813,
+            "y": 3.9958181607132874,
+            "heading": -0.5720527326566293,
+            "angularVelocity": 0.0000030718548612359243,
+            "velocityX": 3.3259666533847816,
+            "velocityY": -1.8023789091228484,
+            "timestamp": 1.7676166309747012
         },
         {
             "x": 5.7832255363464355,
             "y": 3.9654417037963863,
-            "heading": 0.5411390248741499,
-            "angularVelocity": -8.632009947472993e-7,
-            "velocityX": 3.3756892563138687,
-            "velocityY": -1.7074419949962474,
-            "timestamp": 1.737180397716045
-        },
-        {
-            "x": 5.9452002659111205,
-            "y": 3.9031260890359523,
-            "heading": 0.54113898049095,
-            "angularVelocity": -9.674475759968245e-7,
-            "velocityX": 3.530661595583403,
-            "velocityY": -1.358331317676469,
-            "timestamp": 1.7830569919972126
-        },
-        {
-            "x": 6.112434032055133,
-            "y": 3.8567373346649623,
-            "heading": 0.5411389373525121,
-            "angularVelocity": -9.403147416321739e-7,
-            "velocityX": 3.6452960112747332,
-            "velocityY": -1.0111638646647567,
-            "timestamp": 1.8289335862783802
-        },
-        {
-            "x": 6.280097272848707,
-            "y": 3.8119256534794217,
-            "heading": 0.5411388942859083,
-            "angularVelocity": -9.387489265836176e-7,
-            "velocityX": 3.65465753115864,
-            "velocityY": -0.9767874422169035,
-            "timestamp": 1.8748101805595478
-        },
-        {
-            "x": 6.4446058180798245,
-            "y": 3.756644666326502,
-            "heading": 0.5411388506931323,
-            "angularVelocity": -9.502182266145116e-7,
-            "velocityX": 3.5858927152021427,
-            "velocityY": -1.204993265500441,
-            "timestamp": 1.9206867748407155
-        },
-        {
-            "x": 6.602719007178768,
-            "y": 3.68509560621824,
-            "heading": 0.5411388045872843,
-            "angularVelocity": -0.0000010049971826881646,
-            "velocityX": 3.4464892517938135,
-            "velocityY": -1.5595983361306547,
-            "timestamp": 1.9665633691218831
-        },
-        {
-            "x": 6.75280187814694,
-            "y": 3.5979510164257396,
-            "heading": 0.5411387531507533,
-            "angularVelocity": -0.000001121193317368561,
-            "velocityX": 3.271447528304838,
-            "velocityY": -1.899543572446005,
-            "timestamp": 2.0124399634030508
-        },
-        {
-            "x": 6.893316834439751,
-            "y": 3.4960952692408354,
-            "heading": 0.5411386921189019,
-            "angularVelocity": -0.0000013303483490867941,
-            "velocityX": 3.0628898786954792,
-            "velocityY": -2.2202116085744548,
-            "timestamp": 2.0583165576842184
-        },
-        {
-            "x": 7.022827105769335,
-            "y": 3.380569803374169,
-            "heading": 0.5411386138405815,
-            "angularVelocity": -0.0000017062801116380342,
-            "velocityX": 2.8230140741451093,
-            "velocityY": -2.5181787723526305,
-            "timestamp": 2.104193151965386
+            "heading": -0.572052675208486,
+            "angularVelocity": 0.000003159693577578397,
+            "velocityX": 3.3940075416988695,
+            "velocityY": -1.670729295450284,
+            "timestamp": 1.7857981861791263
+        },
+        {
+            "x": 5.945861586676733,
+            "y": 3.9048707731215235,
+            "heading": -0.5720526214657033,
+            "angularVelocity": 0.0000011714587578241761,
+            "velocityX": 3.5450606755342,
+            "velocityY": -1.3202953710429959,
+            "timestamp": 1.831674989206145
+        },
+        {
+            "x": 6.113092819895856,
+            "y": 3.858469939627616,
+            "heading": -0.5720525698257588,
+            "angularVelocity": 0.0000011256221281562495,
+            "velocityX": 3.6452242132183756,
+            "velocityY": -1.0114225585101817,
+            "timestamp": 1.8775517922331635
+        },
+        {
+            "x": 6.280428909416468,
+            "y": 3.8124486851902244,
+            "heading": -0.5720525182264563,
+            "angularVelocity": 0.0000011247362300949913,
+            "velocityX": 3.6475098193329205,
+            "velocityY": -1.0031486808299002,
+            "timestamp": 1.923428595260182
+        },
+        {
+            "x": 6.444900242470224,
+            "y": 3.7570546132296903,
+            "heading": -0.5720524653781475,
+            "angularVelocity": 0.000001151961455361842,
+            "velocityX": 3.585065266142057,
+            "velocityY": -1.2074527496557126,
+            "timestamp": 1.9693053982872004
+        },
+        {
+            "x": 6.602974533610246,
+            "y": 3.685417746961916,
+            "heading": -0.5720524086130027,
+            "angularVelocity": 0.0000012373387213911858,
+            "velocityX": 3.445625691199315,
+            "velocityY": -1.561505195243581,
+            "timestamp": 2.015182201314219
+        },
+        {
+            "x": 6.753006171084989,
+            "y": 3.5981834135738024,
+            "heading": -0.5720523449049553,
+            "angularVelocity": 0.0000013886766999770178,
+            "velocityX": 3.27031588025751,
+            "velocityY": -1.9014911160397099,
+            "timestamp": 2.0610590043412373
+        },
+        {
+            "x": 6.893461047058093,
+            "y": 3.4962434932531963,
+            "heading": -0.5720522700606003,
+            "angularVelocity": 0.0000016314204553722347,
+            "velocityX": 3.0615663408441725,
+            "velocityY": -2.2220362709370285,
+            "timestamp": 2.106935807368256
+        },
+        {
+            "x": 7.0229029918463715,
+            "y": 3.380640293435665,
+            "heading": -0.5720521774325067,
+            "angularVelocity": 0.000002019061650871652,
+            "velocityX": 2.8215118806773165,
+            "velocityY": -2.519861720737319,
+            "timestamp": 2.1528126103952743
         },
         {
             "x": 7.140008449554443,
             "y": 3.2525558471679688,
-            "heading": 0.541138510449912,
-            "angularVelocity": -0.0000022536692406151377,
-            "velocityX": 2.5542729494461978,
-            "velocityY": -2.790397984245173,
-            "timestamp": 2.1500697462465537
-        },
-        {
-            "x": 7.220411568965462,
-            "y": 3.1500924469852043,
-            "heading": 0.5411383954402629,
-            "angularVelocity": -0.0000033404657713126234,
-            "velocityX": 2.3353159519018676,
-            "velocityY": -2.97605882316215,
-            "timestamp": 2.1844989716991257
-        },
-        {
-            "x": 7.292813165778826,
-            "y": 3.0418269330928753,
-            "heading": 0.5411383062644725,
-            "angularVelocity": -0.000002590118983726798,
-            "velocityX": 2.102910996737314,
-            "velocityY": -3.1445817461525896,
-            "timestamp": 2.2189281971516976
-        },
-        {
-            "x": 7.3567962971772936,
-            "y": 2.928382790909777,
-            "heading": 0.5411382336043034,
-            "angularVelocity": -0.000002110421251450144,
-            "velocityX": 1.8583958993386591,
-            "velocityY": -3.2949954781694024,
-            "timestamp": 2.2533574226042696
-        },
-        {
-            "x": 7.411992506551136,
-            "y": 2.810413328004449,
-            "heading": 0.5411381719390687,
-            "angularVelocity": -0.00000179107237982498,
-            "velocityX": 1.603178946034542,
-            "velocityY": -3.426433832147572,
-            "timestamp": 2.2877866480568416
-        },
-        {
-            "x": 7.458083940034777,
-            "y": 2.688597909034967,
-            "heading": 0.5411381177395469,
-            "angularVelocity": -0.0000015742300651441758,
-            "velocityX": 1.3387298981538813,
-            "velocityY": -3.5381399775400135,
-            "timestamp": 2.3222158735094136
-        },
-        {
-            "x": 7.4948097486817264,
-            "y": 2.563639387590104,
-            "heading": 0.5411380685927316,
-            "angularVelocity": -0.0000014274737405214791,
-            "velocityX": 1.0667044687814087,
-            "velocityY": -3.6294316762078314,
-            "timestamp": 2.3566450989619856
-        },
-        {
-            "x": 7.521973664613143,
-            "y": 2.436259879309271,
-            "heading": 0.5411380227355477,
-            "angularVelocity": -0.0000013319260973088958,
-            "velocityX": 0.7889784209297904,
-            "velocityY": -3.6997494601296452,
-            "timestamp": 2.3910743244145576
-        },
-        {
-            "x": 7.539529818670366,
-            "y": 2.3072048557852676,
-            "heading": 0.5411379787715688,
-            "angularVelocity": -0.0000012769377868531526,
-            "velocityX": 0.5099200991725963,
-            "velocityY": -3.748414953504679,
-            "timestamp": 2.4255035498671296
-        },
-        {
-            "x": 7.551115587366525,
-            "y": 2.177477496871536,
-            "heading": 0.5411379354425048,
-            "angularVelocity": -0.0000012584966258013745,
-            "velocityX": 0.336509710684848,
-            "velocityY": -3.7679429963488027,
-            "timestamp": 2.4599327753197016
-        },
-        {
-            "x": 7.56271584344406,
-            "y": 2.0477514326184285,
-            "heading": 0.5411378921115911,
-            "angularVelocity": -0.0000012585503448264627,
-            "velocityX": 0.3369304980023622,
-            "velocityY": -3.76790539281253,
-            "timestamp": 2.4943620007722735
-        },
-        {
-            "x": 7.574376716473137,
-            "y": 1.9180308030778739,
-            "heading": 0.5411378487769694,
-            "angularVelocity": -0.0000012586580504181092,
-            "velocityX": 0.3386911228976962,
-            "velocityY": -3.767747541089928,
-            "timestamp": 2.5287912262248455
-        },
-        {
-            "x": 7.58611108718596,
-            "y": 1.788316801391074,
-            "heading": 0.541137805437783,
-            "angularVelocity": -0.0000012587906282413006,
-            "velocityX": 0.34082586983899804,
-            "velocityY": -3.7675550344719086,
-            "timestamp": 2.5632204516774175
-        },
-        {
-            "x": 7.597929211011114,
-            "y": 1.6586104040760536,
-            "heading": 0.541137760738121,
-            "angularVelocity": -0.0000012983057658513822,
-            "velocityX": 0.3432584866444477,
-            "velocityY": -3.7673341647982834,
-            "timestamp": 2.5976496771299895
-        },
-        {
-            "x": 7.610687440254598,
-            "y": 1.5360368797463901,
-            "heading": 0.5228402741207003,
-            "angularVelocity": -0.5314521711397164,
-            "velocityX": 0.37056393444221536,
-            "velocityY": -3.560159217014913,
-            "timestamp": 2.6320789025825615
-        },
-        {
-            "x": 7.6219652115761445,
-            "y": 1.4215043504075173,
-            "heading": 0.4829676933785713,
-            "angularVelocity": -1.1581027518921772,
-            "velocityX": 0.3275638987899306,
-            "velocityY": -3.3266077825840545,
-            "timestamp": 2.6665081280351335
-        },
-        {
-            "x": 7.631828440236478,
-            "y": 1.3151733520485158,
-            "heading": 0.42230189460591644,
-            "angularVelocity": -1.7620436700286786,
-            "velocityX": 0.2864783779098223,
-            "velocityY": -3.0883935656779764,
-            "timestamp": 2.7009373534877055
-        },
-        {
-            "x": 7.640727843462309,
-            "y": 1.2186121100176637,
-            "heading": 0.3575309788065375,
-            "angularVelocity": -1.8812771692652814,
-            "velocityX": 0.2584839800738238,
-            "velocityY": -2.804630100199854,
-            "timestamp": 2.7353665789402775
-        },
-        {
-            "x": 7.648686345492233,
-            "y": 1.1318869979940107,
-            "heading": 0.2935915741251458,
-            "angularVelocity": -1.8571258528454382,
-            "velocityX": 0.23115541884308086,
-            "velocityY": -2.518938805147378,
-            "timestamp": 2.7697958043928494
-        },
-        {
-            "x": 7.655721476158885,
-            "y": 1.054956995812666,
-            "heading": 0.2330360616068616,
-            "angularVelocity": -1.7588403956899903,
-            "velocityX": 0.20433601320340974,
-            "velocityY": -2.234438944533275,
-            "timestamp": 2.8042250298454214
-        },
-        {
-            "x": 7.661845751852373,
-            "y": 0.9877699914439897,
-            "heading": 0.17748768247912494,
-            "angularVelocity": -1.6134077487241032,
-            "velocityX": 0.17788014725831017,
-            "velocityY": -1.9514526825829648,
-            "timestamp": 2.8386542552979934
-        },
-        {
-            "x": 7.667069516180101,
-            "y": 0.9302779311076093,
-            "heading": 0.1280864585154153,
-            "angularVelocity": -1.4348630651528973,
-            "velocityX": 0.15172471233573487,
-            "velocityY": -1.6698621470755346,
-            "timestamp": 2.8730834807505654
-        },
-        {
-            "x": 7.6714024480246685,
-            "y": 0.88243945985842,
-            "heading": 0.08567598072175592,
-            "angularVelocity": -1.2318162037100373,
-            "velocityX": 0.1258504014428196,
-            "velocityY": -1.3894727697283988,
-            "timestamp": 2.9075127062031374
-        },
-        {
-            "x": 7.674853841393155,
-            "y": 0.8442197139951643,
-            "heading": 0.050901182135293016,
-            "angularVelocity": -1.0100372032583045,
-            "velocityX": 0.10024603583499626,
-            "velocityY": -1.110096011770723,
-            "timestamp": 2.9419419316557094
-        },
-        {
-            "x": 7.6774323573426795,
-            "y": 0.8155893376478138,
-            "heading": 0.02426863930506587,
-            "angularVelocity": -0.7735446406401408,
-            "velocityX": 0.07489323142279661,
-            "velocityY": -0.8315718977410657,
-            "timestamp": 2.9763711571082814
-        },
-        {
-            "x": 7.6791456483845035,
-            "y": 0.7965233362757584,
-            "heading": 0.006187731070844568,
-            "angularVelocity": -0.5251616322048499,
-            "velocityX": 0.04976269489958636,
-            "velocityY": -0.5537737524278505,
-            "timestamp": 3.0108003825608534
+            "heading": -0.5720520639012779,
+            "angularVelocity": 0.0000024746979161572956,
+            "velocityX": 2.5526071997441684,
+            "velocityY": -2.7919217952549285,
+            "timestamp": 2.1986894134222927
+        },
+        {
+            "x": 7.220415929770595,
+            "y": 3.149946383889573,
+            "heading": -0.5720519473102574,
+            "angularVelocity": 0.000003383341158681086,
+            "velocityX": 2.333335244140948,
+            "velocityY": -2.977611988413324,
+            "timestamp": 2.2331497338662096
+        },
+        {
+            "x": 7.2928033634314815,
+            "y": 3.0415299314010706,
+            "heading": -0.5720518591979534,
+            "angularVelocity": 0.000002556920622107498,
+            "velocityX": 2.100602453152653,
+            "velocityY": -3.1461243276865574,
+            "timestamp": 2.2676100543101265
+        },
+        {
+            "x": 7.356753135957797,
+            "y": 2.927931973099431,
+            "heading": -0.5720517877574303,
+            "angularVelocity": 0.000002073124165462267,
+            "velocityX": 1.855750953634494,
+            "velocityY": -3.2964858375736252,
+            "timestamp": 2.3020703747540434
+        },
+        {
+            "x": 7.411896317838347,
+            "y": 2.8098078848104033,
+            "heading": -0.5720517267503814,
+            "angularVelocity": 0.0000017703564026272917,
+            "velocityX": 1.6001935318717442,
+            "velocityY": -3.427829073187373,
+            "timestamp": 2.3365306951979603
+        },
+        {
+            "x": 7.457914797691639,
+            "y": 2.687839154921363,
+            "heading": -0.572051672451037,
+            "angularVelocity": 0.0000015757063075533039,
+            "velocityX": 1.3354048732140973,
+            "velocityY": -3.5393962771625227,
+            "timestamp": 2.370991015641877
+        },
+        {
+            "x": 7.494543133171347,
+            "y": 2.5627294544923394,
+            "heading": -0.5720516223974168,
+            "angularVelocity": 0.000001452500141635371,
+            "velocityX": 1.0629133742199746,
+            "velocityY": -3.630543733121263,
+            "timestamp": 2.405451336085794
+        },
+        {
+            "x": 7.521592976236692,
+            "y": 2.435205423722826,
+            "heading": -0.572051574793162,
+            "angularVelocity": 0.0000013814222897397843,
+            "velocityX": 0.7849562255098341,
+            "velocityY": -3.70060490229679,
+            "timestamp": 2.439911656529711
+        },
+        {
+            "x": 7.539069360193098,
+            "y": 2.3060208712744585,
+            "heading": -0.572051528188319,
+            "angularVelocity": 0.000001352420480159191,
+            "velocityX": 0.5071451376970121,
+            "velocityY": -3.748791386272309,
+            "timestamp": 2.474371976973628
+        },
+        {
+            "x": 7.551504596507269,
+            "y": 2.176254012981795,
+            "heading": -0.5720514816592456,
+            "angularVelocity": 0.0000013502217280486013,
+            "velocityX": 0.3608566651205708,
+            "velocityY": -3.7656892513183533,
+            "timestamp": 2.5088322974175448
+        },
+        {
+            "x": 7.5639699142109125,
+            "y": 2.0464900408376128,
+            "heading": -0.5720514351306527,
+            "angularVelocity": 0.0000013502077822692335,
+            "velocityX": 0.36172959342888805,
+            "velocityY": -3.765605498514506,
+            "timestamp": 2.5432926178614617
+        },
+        {
+            "x": 7.576441327813496,
+            "y": 1.916726654418564,
+            "heading": -0.5720513886022516,
+            "angularVelocity": 0.00000135020222116771,
+            "velocityX": 0.36190648960876737,
+            "velocityY": -3.7655885014252446,
+            "timestamp": 2.5777529383053785
+        },
+        {
+            "x": 7.58884833483585,
+            "y": 1.786957094038731,
+            "heading": -0.5720513420716767,
+            "angularVelocity": 0.000001350265295572762,
+            "velocityX": 0.36003748258495927,
+            "velocityY": -3.765767662871606,
+            "timestamp": 2.6122132587492954
+        },
+        {
+            "x": 7.601019357724818,
+            "y": 1.6571651890693204,
+            "heading": -0.5720512933827582,
+            "angularVelocity": 0.0000014128980208836598,
+            "velocityX": 0.3531894866947264,
+            "velocityY": -3.7664160778959355,
+            "timestamp": 2.6466735791932123
+        },
+        {
+            "x": 7.610550391987499,
+            "y": 1.5345810963054864,
+            "heading": -0.5537181215211259,
+            "angularVelocity": 0.5320081654919462,
+            "velocityX": 0.2765799661741953,
+            "velocityY": -3.5572534203020942,
+            "timestamp": 2.6811338996371292
+        },
+        {
+            "x": 7.620334086621719,
+            "y": 1.4202071871878734,
+            "heading": -0.5150322166693104,
+            "angularVelocity": 1.122621738667962,
+            "velocityX": 0.2839118878810709,
+            "velocityY": -3.319003063356803,
+            "timestamp": 2.715594220081046
+        },
+        {
+            "x": 7.630352820350644,
+            "y": 1.314046573370508,
+            "heading": -0.4556179078059868,
+            "angularVelocity": 1.7241368651811046,
+            "velocityX": 0.290732459821803,
+            "velocityY": -3.0806624096878554,
+            "timestamp": 2.750054540524963
+        },
+        {
+            "x": 7.63947235322732,
+            "y": 1.2175511973082003,
+            "heading": -0.3892109145347841,
+            "angularVelocity": 1.9270567544269157,
+            "velocityX": 0.2646386556821465,
+            "velocityY": -2.8001880080992603,
+            "timestamp": 2.78451486096888
+        },
+        {
+            "x": 7.647652812069227,
+            "y": 1.1309118710627388,
+            "heading": -0.32231407882554747,
+            "angularVelocity": 1.9412714347239703,
+            "velocityX": 0.23738777633239685,
+            "velocityY": -2.5141764536539966,
+            "timestamp": 2.818975181412797
+        },
+        {
+            "x": 7.654895533140534,
+            "y": 1.0540989766744202,
+            "heading": -0.25819483301023066,
+            "angularVelocity": 1.8606688791433752,
+            "velocityX": 0.21017567387602423,
+            "velocityY": -2.22902437930995,
+            "timestamp": 2.8534355018567137
+        },
+        {
+            "x": 7.66120815675186,
+            "y": 0.9870533783479266,
+            "heading": -0.19885832997172487,
+            "angularVelocity": 1.7218790270703905,
+            "velocityX": 0.18318528469800885,
+            "velocityY": -1.9455883596791401,
+            "timestamp": 2.8878958223006306
+        },
+        {
+            "x": 7.666600355589264,
+            "y": 0.929716153410391,
+            "heading": -0.14570825222338094,
+            "angularVelocity": 1.542355876662632,
+            "velocityX": 0.15647558606309142,
+            "velocityY": -1.663862210186161,
+            "timestamp": 2.9223561427445475
+        },
+        {
+            "x": 7.671080940668327,
+            "y": 0.8820338579186632,
+            "heading": -0.09980369912266952,
+            "angularVelocity": 1.3320988461333123,
+            "velocityX": 0.1300215732566484,
+            "velocityY": -1.383686944215496,
+            "timestamp": 2.9568164631884644
+        },
+        {
+            "x": 7.674656709905022,
+            "y": 0.8439593502971015,
+            "heading": -0.06197555143316066,
+            "angularVelocity": 1.0977305841102087,
+            "velocityX": 0.10376482837700403,
+            "velocityY": -1.1048796741031013,
+            "timestamp": 2.9912767836323813
+        },
+        {
+            "x": 7.67733258738128,
+            "y": 0.8154516156506164,
+            "heading": -0.032889080662387146,
+            "angularVelocity": 0.8440568861838474,
+            "velocityX": 0.07765097485389878,
+            "velocityY": -0.8272626104241091,
+            "timestamp": 3.025737104076298
+        },
+        {
+            "x": 7.679112422858347,
+            "y": 0.7964752743679322,
+            "heading": -0.013083588528854375,
+            "angularVelocity": 0.5747332548975859,
+            "velocityX": 0.05164883710108163,
+            "velocityY": -0.5506722235380279,
+            "timestamp": 3.060197424520215
         },
         {
             "x": 7.68,
             "y": 0.787,
             "heading": -0.003,
-            "angularVelocity": -0.26685848868430057,
-            "velocityX": 0.024814720757314154,
-            "velocityY": -0.2766061725343536,
-            "timestamp": 3.0452296080134253
+            "angularVelocity": 0.2926144736603049,
+            "velocityX": 0.025756497043992084,
+            "velocityY": -0.27496187632245256,
+            "timestamp": 3.094657744964132
         },
         {
             "x": 7.68,
             "y": 0.787,
             "heading": -0.003,
-            "angularVelocity": -2.388488097950269e-30,
+            "angularVelocity": 9.445490920898867e-33,
             "velocityX": 0,
-            "velocityY": 9.563090319474696e-31,
-            "timestamp": 3.0796588334659973
+            "velocityY": 3.8255947334103544e-33,
+            "timestamp": 3.129118065408049
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.traj
index 17baf1d1..6231149d 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.traj	
@@ -5,657 +5,801 @@
             "y": 6.72,
             "heading": 1.04,
             "angularVelocity": 0,
-            "velocityX": -2.70270630963226e-31,
-            "velocityY": -3.181766396851851e-31,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.7256414720586181,
-            "y": 6.712155337793633,
-            "heading": 1.0237015987984905,
-            "angularVelocity": -0.3502584296953155,
-            "velocityX": 0.33614078912617174,
-            "velocityY": -0.16858457660462928,
-            "timestamp": 0.046532502346056014
-        },
-        {
-            "x": 0.756979104040484,
-            "y": 6.696435827024302,
-            "heading": 0.9920873730999193,
-            "angularVelocity": -0.6794009370802031,
-            "velocityX": 0.6734568398839507,
-            "velocityY": -0.33781786873231273,
-            "timestamp": 0.09306500469211203
-        },
-        {
-            "x": 0.8040789517219918,
-            "y": 6.672807532790365,
-            "heading": 0.9464055818688567,
-            "angularVelocity": -0.9817179160357762,
-            "velocityX": 1.0121924527341073,
-            "velocityY": -0.5077804339471543,
-            "timestamp": 0.13959750703816803
-        },
-        {
-            "x": 0.8670206146686451,
-            "y": 6.641234227942618,
-            "heading": 0.8882606987188267,
-            "angularVelocity": -1.2495541872573583,
-            "velocityX": 1.352638688514273,
-            "velocityY": -0.6785215334633973,
-            "timestamp": 0.18613000938422405
-        },
-        {
-            "x": 0.9458974559747272,
-            "y": 6.601677384330224,
-            "heading": 0.8197602775960398,
-            "angularVelocity": -1.4720983757420398,
-            "velocityX": 1.6950913303456163,
-            "velocityY": -0.8500906166232347,
-            "timestamp": 0.23266251173028007
-        },
-        {
-            "x": 1.04081251945038,
-            "y": 6.554094078416389,
-            "heading": 0.743830819371293,
-            "angularVelocity": -1.6317510212566968,
-            "velocityX": 2.039758420249741,
-            "velocityY": -1.0225821418316814,
-            "timestamp": 0.27919501407633607
-        },
-        {
-            "x": 1.15185810993152,
-            "y": 6.498440545281296,
-            "heading": 0.6650543509456113,
-            "angularVelocity": -1.6929342815026889,
-            "velocityX": 2.3864091738569346,
-            "velocityY": -1.1960141907087913,
-            "timestamp": 0.32572751642239206
-        },
-        {
-            "x": 1.2789797685079123,
-            "y": 6.43473905676892,
-            "heading": 0.5921954143923897,
-            "angularVelocity": -1.5657644201343583,
-            "velocityX": 2.7318895861437724,
-            "velocityY": -1.3689676097497174,
-            "timestamp": 0.37226001876844805
-        },
-        {
-            "x": 1.420223914518488,
-            "y": 6.363052522149474,
-            "heading": 0.5479689699773799,
-            "angularVelocity": -0.9504419961367557,
-            "velocityX": 3.035386856270174,
-            "velocityY": -1.5405690862335149,
-            "timestamp": 0.41879252111450405
-        },
-        {
-            "x": 1.5740028534513755,
-            "y": 6.283114192872816,
-            "heading": 0.5411402298477218,
-            "angularVelocity": -0.14675205040326236,
-            "velocityX": 3.304764007515849,
-            "velocityY": -1.717903083787275,
-            "timestamp": 0.46532502346056004
-        },
-        {
-            "x": 1.7314538549626566,
-            "y": 6.20440189931456,
-            "heading": 0.5411401828527235,
-            "angularVelocity": -0.0000010099392058430412,
-            "velocityX": 3.383677936345371,
-            "velocityY": -1.6915551408103213,
-            "timestamp": 0.511857525806616
-        },
-        {
-            "x": 1.888910012901083,
-            "y": 6.125699921176338,
-            "heading": 0.5411401358667378,
-            "angularVelocity": -0.0000010097455172737006,
-            "velocityX": 3.383788749796141,
-            "velocityY": -1.6913334587700644,
-            "timestamp": 0.558390028152672
-        },
-        {
-            "x": 2.0463658388325006,
-            "y": 6.046997278808062,
-            "heading": 0.5411400888807237,
-            "angularVelocity": -0.0000010097461271838877,
-            "velocityX": 3.3837816148475093,
-            "velocityY": -1.691347733310028,
-            "timestamp": 0.604922530498728
-        },
-        {
-            "x": 2.2038216598445532,
-            "y": 5.9682946265981345,
-            "heading": 0.5411400418947097,
-            "angularVelocity": -0.0000010097461301639238,
-            "velocityX": 3.3837815091285663,
-            "velocityY": -1.6913479448107236,
-            "timestamp": 0.651455032844784
-        },
-        {
-            "x": 2.3612775122085363,
-            "y": 5.8895920371121315,
-            "heading": 0.5411399949086985,
-            "angularVelocity": -0.0000010097460671631127,
-            "velocityX": 3.3837821828926944,
-            "velocityY": -1.6913465968512624,
-            "timestamp": 0.69798753519084
-        },
-        {
-            "x": 2.5187333978232895,
-            "y": 5.810889514148883,
-            "heading": 0.5411399479226903,
-            "angularVelocity": -0.0000010097460006294838,
-            "velocityX": 3.383782897463604,
-            "velocityY": -1.6913451672535464,
-            "timestamp": 0.744520037536896
-        },
-        {
-            "x": 2.676189337500118,
-            "y": 5.732187099344657,
-            "heading": 0.541139900936687,
-            "angularVelocity": -0.0000010097458968738495,
-            "velocityX": 3.3837840592768114,
-            "velocityY": -1.6913428428777157,
-            "timestamp": 0.791052539882952
-        },
-        {
-            "x": 2.833645305906935,
-            "y": 5.653484742019454,
-            "heading": 0.5411398539506863,
-            "angularVelocity": -0.0000010097458381135432,
-            "velocityX": 3.3837846766944186,
-            "velocityY": -1.6913416076332204,
-            "timestamp": 0.837585042229008
-        },
-        {
-            "x": 2.991101010217502,
-            "y": 5.57478185633263,
-            "heading": 0.5411398069646632,
-            "angularVelocity": -0.0000010097463224464638,
-            "velocityX": 3.383779001172013,
-            "velocityY": -1.6913529623124643,
-            "timestamp": 0.884117544575064
-        },
-        {
-            "x": 3.14855589978306,
-            "y": 5.496077340659936,
-            "heading": 0.5411397599785701,
-            "angularVelocity": -0.0000010097478275290572,
-            "velocityX": 3.3837614920124643,
-            "velocityY": -1.6913879912875969,
-            "timestamp": 0.93065004692112
-        },
-        {
-            "x": 3.306010013541662,
-            "y": 5.417371272923184,
-            "heading": 0.5411397129924104,
-            "angularVelocity": -0.0000010097492581811278,
-            "velocityX": 3.3837448196468105,
-            "velocityY": -1.6914213456927205,
-            "timestamp": 0.977182549267176
-        },
-        {
-            "x": 3.4634680562159517,
-            "y": 5.338673065406879,
-            "heading": 0.5411396660065909,
-            "angularVelocity": -0.0000010097419454671228,
-            "velocityX": 3.383829253443873,
-            "velocityY": -1.6912524267565827,
-            "timestamp": 1.023715051613232
-        },
-        {
-            "x": 3.6209395624979943,
-            "y": 5.2600018013447745,
-            "heading": 0.5411396190219294,
-            "angularVelocity": -0.0000010097170609305348,
-            "velocityX": 3.3841185911564384,
-            "velocityY": -1.690673402356174,
-            "timestamp": 1.0702475539592882
-        },
-        {
-            "x": 3.778386684395729,
-            "y": 5.181281747313384,
-            "heading": 0.5411395720351826,
-            "angularVelocity": -0.0000010097618775946008,
-            "velocityX": 3.3835945620651766,
-            "velocityY": -1.6917219161486032,
-            "timestamp": 1.1167800563053443
-        },
-        {
-            "x": 3.9357449151310644,
-            "y": 5.10238415335025,
-            "heading": 0.5411395250405994,
-            "angularVelocity": -0.000001009930280464059,
-            "velocityX": 3.3816842594175633,
-            "velocityY": -1.6955373123309678,
-            "timestamp": 1.1633125586514004
+            "x": 0.7237404107060486,
+            "y": 6.712071337633356,
+            "heading": 0.9558489426883627,
+            "angularVelocity": -1.537881157714612,
+            "velocityX": 0.25110936688338653,
+            "velocityY": -0.14489824428928566,
+            "timestamp": 0.05471882979351295
+        },
+        {
+            "x": 0.7514918072858734,
+            "y": 6.694799584586779,
+            "heading": 0.7935491608961526,
+            "angularVelocity": -2.966068214628546,
+            "velocityX": 0.5071635611460984,
+            "velocityY": -0.31564551200661234,
+            "timestamp": 0.1094376595870259
+        },
+        {
+            "x": 0.7959632333479747,
+            "y": 6.666655428800684,
+            "heading": 0.574279005326375,
+            "angularVelocity": -4.007215731718184,
+            "velocityX": 0.8127261900504715,
+            "velocityY": -0.514341331718894,
+            "timestamp": 0.16415648938053887
+        },
+        {
+            "x": 0.8617224500002636,
+            "y": 6.63045184710829,
+            "heading": 0.31905346805964874,
+            "angularVelocity": -4.664309127769047,
+            "velocityX": 1.2017657705846174,
+            "velocityY": -0.6616293116832328,
+            "timestamp": 0.2188753191740518
+        },
+        {
+            "x": 0.9507709996198539,
+            "y": 6.585947725507511,
+            "heading": 0.05142997722620149,
+            "angularVelocity": -4.890884762034389,
+            "velocityX": 1.627384027685255,
+            "velocityY": -0.8133237090178916,
+            "timestamp": 0.2735941489675648
+        },
+        {
+            "x": 1.0614190010918925,
+            "y": 6.535018660742887,
+            "heading": -0.17495854287380355,
+            "angularVelocity": -4.137305584828904,
+            "velocityX": 2.0221192940269503,
+            "velocityY": -0.9307411170306262,
+            "timestamp": 0.32831297876107773
+        },
+        {
+            "x": 1.1921384640832198,
+            "y": 6.474205616338843,
+            "heading": -0.35862575739678126,
+            "angularVelocity": -3.356563274764148,
+            "velocityX": 2.388930163247485,
+            "velocityY": -1.111373262796896,
+            "timestamp": 0.3830318085545907
+        },
+        {
+            "x": 1.3384031857193457,
+            "y": 6.398894346098027,
+            "heading": -0.49152044440666937,
+            "angularVelocity": -2.4286829142980233,
+            "velocityX": 2.6730235677201417,
+            "velocityY": -1.376331886573771,
+            "timestamp": 0.4377506383481036
+        },
+        {
+            "x": 1.500957547301863,
+            "y": 6.312259668262383,
+            "heading": -0.5624501430724521,
+            "angularVelocity": -1.2962575942036991,
+            "velocityX": 2.9707207225727275,
+            "velocityY": -1.5832699303433067,
+            "timestamp": 0.4924694681416166
+        },
+        {
+            "x": 1.6817069101290123,
+            "y": 6.217739107445247,
+            "heading": -0.5720464000476684,
+            "angularVelocity": -0.17537394369443057,
+            "velocityX": 3.3032388212471107,
+            "velocityY": -1.7273863709040396,
+            "timestamp": 0.5471882979351296
+        },
+        {
+            "x": 1.8673643551319996,
+            "y": 6.126199810501224,
+            "heading": -0.5720469984173726,
+            "angularVelocity": -0.000010935352720012522,
+            "velocityX": 3.392935223643913,
+            "velocityY": -1.6729030443351731,
+            "timestamp": 0.6019071277286425
+        },
+        {
+            "x": 2.0530162492759167,
+            "y": 6.034649256644459,
+            "heading": -0.5720475972653951,
+            "angularVelocity": -0.000010944094102204816,
+            "velocityX": 3.3928337803365705,
+            "velocityY": -1.6731087671691436,
+            "timestamp": 0.6566259575221555
+        },
+        {
+            "x": 2.23866813267496,
+            "y": 5.943098680998449,
+            "heading": -0.5720481961130315,
+            "angularVelocity": -0.000010944087046546937,
+            "velocityX": 3.3928335839713664,
+            "velocityY": -1.673109165372953,
+            "timestamp": 0.7113447873156684
+        },
+        {
+            "x": 2.4243200161477754,
+            "y": 5.851548105501653,
+            "heading": -0.5720487949598734,
+            "angularVelocity": -0.000010944072528266654,
+            "velocityX": 3.392833585319575,
+            "velocityY": -1.673109162646019,
+            "timestamp": 0.7660636171091814
+        },
+        {
+            "x": 2.6099718996232633,
+            "y": 5.759997530009892,
+            "heading": -0.5720493938059237,
+            "angularVelocity": -0.000010944058060277418,
+            "velocityX": 3.3928335853684146,
+            "velocityY": -1.673109162553994,
+            "timestamp": 0.8207824469026943
+        },
+        {
+            "x": 2.7956237830994004,
+            "y": 5.668446954519065,
+            "heading": -0.5720499926511823,
+            "angularVelocity": -0.000010944043595406558,
+            "velocityX": 3.3928335853802802,
+            "velocityY": -1.6731091625369476,
+            "timestamp": 0.8755012766962073
+        },
+        {
+            "x": 2.9812756665756717,
+            "y": 5.576896379028125,
+            "heading": -0.5720505914956495,
+            "angularVelocity": -0.000010944029129446314,
+            "velocityX": 3.3928335853827263,
+            "velocityY": -1.6731091625390042,
+            "timestamp": 0.9302201064897202
+        },
+        {
+            "x": 3.166927550036217,
+            "y": 5.485345803504912,
+            "heading": -0.5720511903393257,
+            "angularVelocity": -0.000010944014673786385,
+            "velocityX": 3.392833585095338,
+            "velocityY": -1.673109163128796,
+            "timestamp": 0.9849389362832331
+        },
+        {
+            "x": 3.3525794334560626,
+            "y": 5.393795227898782,
+            "heading": -0.5720517891822119,
+            "angularVelocity": -0.000010944000236842356,
+            "velocityX": 3.392833584351539,
+            "velocityY": -1.6731091646441256,
+            "timestamp": 1.039657766076746
+        },
+        {
+            "x": 3.538231313226632,
+            "y": 5.302244644892125,
+            "heading": -0.5720523880244444,
+            "angularVelocity": -0.000010943988288632917,
+            "velocityX": 3.392833517660122,
+            "velocityY": -1.673109299890567,
+            "timestamp": 1.0943765958702592
+        },
+        {
+            "x": 3.723883292814953,
+            "y": 5.21069426429933,
+            "heading": -0.5720529868621156,
+            "angularVelocity": -0.000010943904932797782,
+            "velocityX": 3.3928353418539645,
+            "velocityY": -1.673105600727685,
+            "timestamp": 1.1490954256637722
+        },
+        {
+            "x": 3.909492929164672,
+            "y": 5.1190580682844296,
+            "heading": -0.5720535873273735,
+            "angularVelocity": -0.000010973649477031932,
+            "velocityX": 3.3920615088102064,
+            "velocityY": -1.6746738985594591,
+            "timestamp": 1.2038142554572853
         },
         {
             "x": 4.092995643615723,
             "y": 5.023272514343262,
-            "heading": 0.5411394780180799,
-            "angularVelocity": -0.0000010105306414990813,
-            "velocityX": 3.379373997880459,
-            "velocityY": -1.7001372163227824,
-            "timestamp": 1.2098450609974565
-        },
-        {
-            "x": 4.2612300154414715,
-            "y": 4.916069066513277,
-            "heading": 0.541139432730096,
-            "angularVelocity": -8.588080638060829e-7,
-            "velocityX": 3.1902730598822937,
-            "velocityY": -2.0329274441782497,
-            "timestamp": 1.2625785946693153
-        },
-        {
-            "x": 4.429297905294805,
-            "y": 4.808604804974877,
-            "heading": 0.5411393874448718,
-            "angularVelocity": -8.58755730533247e-7,
-            "velocityX": 3.1871160180389406,
-            "velocityY": -2.0378733237761697,
-            "timestamp": 1.3153121283411742
-        },
-        {
-            "x": 4.5972191269209945,
-            "y": 4.700911507061467,
-            "heading": 0.5411393421481548,
-            "angularVelocity": -8.589736709635531e-7,
-            "velocityX": 3.1843347095055985,
-            "velocityY": -2.042216601367096,
-            "timestamp": 1.368045662013033
-        },
-        {
-            "x": 4.765178956046694,
-            "y": 4.593278431809082,
-            "heading": 0.5411392968544698,
-            "angularVelocity": -8.589161734970966e-7,
-            "velocityX": 3.185066833769243,
-            "velocityY": -2.0410745830567376,
-            "timestamp": 1.4207791956848919
-        },
-        {
-            "x": 4.9331339491528,
-            "y": 4.485637810244685,
-            "heading": 0.5411392515604048,
-            "angularVelocity": -8.58923380695705e-7,
-            "velocityX": 3.1849751270448214,
-            "velocityY": -2.041217685776372,
-            "timestamp": 1.4735127293567507
-        },
-        {
-            "x": 5.1010877481253685,
-            "y": 4.377995325600118,
-            "heading": 0.5411392062662455,
-            "angularVelocity": -8.589251678490757e-7,
-            "velocityX": 3.1849524823742743,
-            "velocityY": -2.041253015859115,
-            "timestamp": 1.5262462630286096
-        },
-        {
-            "x": 5.269016703536996,
-            "y": 4.270314087571009,
-            "heading": 0.5411391609701367,
-            "angularVelocity": -8.589621362197366e-7,
-            "velocityX": 3.184481367332135,
-            "velocityY": -2.0419879065793034,
-            "timestamp": 1.5789797967004684
-        },
-        {
-            "x": 5.437056059940434,
-            "y": 4.162805214274757,
-            "heading": 0.5411391156827058,
-            "angularVelocity": -8.587975760545075e-7,
-            "velocityX": 3.18657493065327,
-            "velocityY": -2.038719308384206,
-            "timestamp": 1.6317133303723272
-        },
-        {
-            "x": 5.605213513282879,
-            "y": 4.0554811537322735,
-            "heading": 0.5411390703937885,
-            "angularVelocity": -8.588257618591655e-7,
-            "velocityX": 3.188814434261697,
-            "velocityY": -2.03521465506778,
-            "timestamp": 1.684446864044186
+            "heading": -0.5720542830492642,
+            "angularVelocity": -0.000012714487741704976,
+            "velocityX": 3.353556995709023,
+            "velocityY": -1.7505044296932655,
+            "timestamp": 1.2585330852507983
+        },
+        {
+            "x": 4.152699494738863,
+            "y": 4.989124335806784,
+            "heading": -0.5720542274621303,
+            "angularVelocity": 0.0000030573365885939726,
+            "velocityX": 3.283759307267904,
+            "velocityY": -1.8781769850015708,
+            "timestamp": 1.2767146404552234
+        },
+        {
+            "x": 4.211236123155136,
+            "y": 4.953011857528808,
+            "heading": -0.5720541720789936,
+            "angularVelocity": 0.000003046116576982166,
+            "velocityX": 3.2195611298433677,
+            "velocityY": -1.9862150334199096,
+            "timestamp": 1.2948961956596485
+        },
+        {
+            "x": 4.269217408443118,
+            "y": 4.916014310130808,
+            "heading": -0.5720541167402081,
+            "angularVelocity": 0.000003043677217870583,
+            "velocityX": 3.1890168160017764,
+            "velocityY": -2.034894539109796,
+            "timestamp": 1.3130777508640736
+        },
+        {
+            "x": 4.327172181408397,
+            "y": 4.8789752463128,
+            "heading": -0.5720540614037476,
+            "angularVelocity": 0.000003043549349895456,
+            "velocityX": 3.187558617163689,
+            "velocityY": -2.0371779752347874,
+            "timestamp": 1.3312593060684987
+        },
+        {
+            "x": 4.385108415981365,
+            "y": 4.841907191568928,
+            "heading": -0.5720540060672449,
+            "angularVelocity": 0.0000030435516711711228,
+            "velocityX": 3.186538990839725,
+            "velocityY": -2.0387724992190503,
+            "timestamp": 1.3494408612729238
+        },
+        {
+            "x": 4.443034277698237,
+            "y": 4.804822929374826,
+            "heading": -0.5720539507307192,
+            "angularVelocity": 0.000003043552934036031,
+            "velocityX": 3.1859684755020403,
+            "velocityY": -2.0396639218758956,
+            "timestamp": 1.367622416477349
+        },
+        {
+            "x": 4.500957639641057,
+            "y": 4.767734762799275,
+            "heading": -0.5720538953941279,
+            "angularVelocity": 0.0000030435565398491907,
+            "velocityX": 3.1858309859372986,
+            "velocityY": -2.039878665966735,
+            "timestamp": 1.385803971681774
+        },
+        {
+            "x": 4.558883202891902,
+            "y": 4.7306500344023386,
+            "heading": -0.5720538400575734,
+            "angularVelocity": 0.000003043554516316428,
+            "velocityX": 3.1859520596302042,
+            "velocityY": -2.03968956340728,
+            "timestamp": 1.4039855268861992
+        },
+        {
+            "x": 4.616809360440233,
+            "y": 4.693566234312182,
+            "heading": -0.572053784721022,
+            "angularVelocity": 0.000003043554346667464,
+            "velocityX": 3.1859847464654854,
+            "velocityY": -2.039638505794633,
+            "timestamp": 1.4221670820906243
+        },
+        {
+            "x": 4.674735447300298,
+            "y": 4.656482323806888,
+            "heading": -0.5720537293844703,
+            "angularVelocity": 0.00000304355436453364,
+            "velocityX": 3.185980858554995,
+            "velocityY": -2.039644578714451,
+            "timestamp": 1.4403486372950494
+        },
+        {
+            "x": 4.732661331177625,
+            "y": 4.619398096238942,
+            "heading": -0.5720536740479176,
+            "angularVelocity": 0.0000030435544255368,
+            "velocityX": 3.1859696943429383,
+            "velocityY": -2.0396620174125277,
+            "timestamp": 1.4585301924994745
+        },
+        {
+            "x": 4.790587117727679,
+            "y": 4.582313716645222,
+            "heading": -0.5720536187113643,
+            "angularVelocity": 0.0000030435544545803464,
+            "velocityX": 3.1859643412654055,
+            "velocityY": -2.039670378951063,
+            "timestamp": 1.4767117477038996
+        },
+        {
+            "x": 4.848512993160408,
+            "y": 4.5452294758867104,
+            "heading": -0.5720535633748114,
+            "angularVelocity": 0.0000030435544295946876,
+            "velocityX": 3.1859692298833355,
+            "velocityY": -2.0396627429041696,
+            "timestamp": 1.4948933029083247
+        },
+        {
+            "x": 4.906438819485911,
+            "y": 4.508145158422396,
+            "heading": -0.5720535080382583,
+            "angularVelocity": 0.0000030435544442083865,
+            "velocityX": 3.1859665289468815,
+            "velocityY": -2.039666961784329,
+            "timestamp": 1.5130748581127498
+        },
+        {
+            "x": 4.9643646433421065,
+            "y": 4.471060837101057,
+            "heading": -0.5720534527017052,
+            "angularVelocity": 0.0000030435544458805193,
+            "velocityX": 3.185966393132936,
+            "velocityY": -2.039667173923745,
+            "timestamp": 1.5312564133171749
+        },
+        {
+            "x": 5.022290480818567,
+            "y": 4.433976537054754,
+            "heading": -0.572053397365152,
+            "angularVelocity": 0.0000030435544415454932,
+            "velocityX": 3.185967142258403,
+            "velocityY": -2.039666003779759,
+            "timestamp": 1.5494379685216
+        },
+        {
+            "x": 5.080216347467466,
+            "y": 4.396892282576102,
+            "heading": -0.572053342028599,
+            "angularVelocity": 0.000003043554433808661,
+            "velocityX": 3.1859687467658464,
+            "velocityY": -2.039663497522643,
+            "timestamp": 1.567619523726025
+        },
+        {
+            "x": 5.138142382220086,
+            "y": 4.359808290677983,
+            "heading": -0.5720532866920471,
+            "angularVelocity": 0.000003043554377469469,
+            "velocityX": 3.185977992604117,
+            "velocityY": -2.039649055384386,
+            "timestamp": 1.5858010789304502
+        },
+        {
+            "x": 5.196068138494407,
+            "y": 4.322723863792993,
+            "heading": -0.5720532313554916,
+            "angularVelocity": 0.000003043554576021048,
+            "velocityX": 3.1859626760759787,
+            "velocityY": -2.0396729800088242,
+            "timestamp": 1.6039826341348753
+        },
+        {
+            "x": 5.253993690845546,
+            "y": 4.285639118381674,
+            "heading": -0.5720531760189423,
+            "angularVelocity": 0.0000030435542278177066,
+            "velocityX": 3.1859514601384813,
+            "velocityY": -2.0396904992104243,
+            "timestamp": 1.6221641893393004
+        },
+        {
+            "x": 5.3119198680466875,
+            "y": 4.248555348991913,
+            "heading": -0.5720531206823879,
+            "angularVelocity": 0.000003043554518420827,
+            "velocityX": 3.1859858273860655,
+            "velocityY": -2.0396368172479113,
+            "timestamp": 1.6403457445437255
+        },
+        {
+            "x": 5.369845451022558,
+            "y": 4.211470651416335,
+            "heading": -0.5720530653458327,
+            "angularVelocity": 0.0000030435545594837867,
+            "velocityX": 3.1859531445232334,
+            "velocityY": -2.039687868206526,
+            "timestamp": 1.6585272997481506
+        },
+        {
+            "x": 5.427769955136907,
+            "y": 4.17438426873464,
+            "heading": -0.5720530100092767,
+            "angularVelocity": 0.0000030435545998300077,
+            "velocityX": 3.185893806281465,
+            "velocityY": -2.039780550383344,
+            "timestamp": 1.6767088549525757
+        },
+        {
+            "x": 5.485698211873678,
+            "y": 4.137303747848898,
+            "heading": -0.5720529546727399,
+            "angularVelocity": 0.000003043553546723704,
+            "velocityX": 3.1861002035013715,
+            "velocityY": -2.0394581469404574,
+            "timestamp": 1.6948904101570008
+        },
+        {
+            "x": 5.5436372749681775,
+            "y": 4.100240114388813,
+            "heading": -0.5720528993360909,
+            "angularVelocity": 0.0000030435597092212927,
+            "velocityX": 3.186694561772871,
+            "velocityY": -2.0385293250953618,
+            "timestamp": 1.713071965361426
+        },
+        {
+            "x": 5.601833960036281,
+            "y": 4.063582321171135,
+            "heading": -0.5720528439855975,
+            "angularVelocity": 0.0000030443211669066955,
+            "velocityX": 3.2008639752640304,
+            "velocityY": -2.0162077889097265,
+            "timestamp": 1.731253520565851
+        },
+        {
+            "x": 5.661045954546234,
+            "y": 4.0285882123488195,
+            "heading": -0.5720527885077281,
+            "angularVelocity": 0.000003051326955110969,
+            "velocityX": 3.2567068022627845,
+            "velocityY": -1.9247038236737246,
+            "timestamp": 1.7494350757702761
+        },
+        {
+            "x": 5.721517200862813,
+            "y": 3.9958181607132874,
+            "heading": -0.5720527326566293,
+            "angularVelocity": 0.0000030718548612359243,
+            "velocityX": 3.3259666533847816,
+            "velocityY": -1.8023789091228484,
+            "timestamp": 1.7676166309747012
         },
         {
             "x": 5.7832255363464355,
             "y": 3.9654417037963863,
-            "heading": 0.5411390248741499,
-            "angularVelocity": -8.632009947472993e-7,
-            "velocityX": 3.3756892563138687,
-            "velocityY": -1.7074419949962474,
-            "timestamp": 1.737180397716045
-        },
-        {
-            "x": 5.9452002659111205,
-            "y": 3.9031260890359523,
-            "heading": 0.54113898049095,
-            "angularVelocity": -9.674475759968245e-7,
-            "velocityX": 3.530661595583403,
-            "velocityY": -1.358331317676469,
-            "timestamp": 1.7830569919972126
-        },
-        {
-            "x": 6.112434032055133,
-            "y": 3.8567373346649623,
-            "heading": 0.5411389373525121,
-            "angularVelocity": -9.403147416321739e-7,
-            "velocityX": 3.6452960112747332,
-            "velocityY": -1.0111638646647567,
-            "timestamp": 1.8289335862783802
-        },
-        {
-            "x": 6.280097272848707,
-            "y": 3.8119256534794217,
-            "heading": 0.5411388942859083,
-            "angularVelocity": -9.387489265836176e-7,
-            "velocityX": 3.65465753115864,
-            "velocityY": -0.9767874422169035,
-            "timestamp": 1.8748101805595478
-        },
-        {
-            "x": 6.4446058180798245,
-            "y": 3.756644666326502,
-            "heading": 0.5411388506931323,
-            "angularVelocity": -9.502182266145116e-7,
-            "velocityX": 3.5858927152021427,
-            "velocityY": -1.204993265500441,
-            "timestamp": 1.9206867748407155
-        },
-        {
-            "x": 6.602719007178768,
-            "y": 3.68509560621824,
-            "heading": 0.5411388045872843,
-            "angularVelocity": -0.0000010049971826881646,
-            "velocityX": 3.4464892517938135,
-            "velocityY": -1.5595983361306547,
-            "timestamp": 1.9665633691218831
-        },
-        {
-            "x": 6.75280187814694,
-            "y": 3.5979510164257396,
-            "heading": 0.5411387531507533,
-            "angularVelocity": -0.000001121193317368561,
-            "velocityX": 3.271447528304838,
-            "velocityY": -1.899543572446005,
-            "timestamp": 2.0124399634030508
-        },
-        {
-            "x": 6.893316834439751,
-            "y": 3.4960952692408354,
-            "heading": 0.5411386921189019,
-            "angularVelocity": -0.0000013303483490867941,
-            "velocityX": 3.0628898786954792,
-            "velocityY": -2.2202116085744548,
-            "timestamp": 2.0583165576842184
-        },
-        {
-            "x": 7.022827105769335,
-            "y": 3.380569803374169,
-            "heading": 0.5411386138405815,
-            "angularVelocity": -0.0000017062801116380342,
-            "velocityX": 2.8230140741451093,
-            "velocityY": -2.5181787723526305,
-            "timestamp": 2.104193151965386
+            "heading": -0.572052675208486,
+            "angularVelocity": 0.000003159693577578397,
+            "velocityX": 3.3940075416988695,
+            "velocityY": -1.670729295450284,
+            "timestamp": 1.7857981861791263
+        },
+        {
+            "x": 5.945861586676733,
+            "y": 3.9048707731215235,
+            "heading": -0.5720526214657033,
+            "angularVelocity": 0.0000011714587578241761,
+            "velocityX": 3.5450606755342,
+            "velocityY": -1.3202953710429959,
+            "timestamp": 1.831674989206145
+        },
+        {
+            "x": 6.113092819895856,
+            "y": 3.858469939627616,
+            "heading": -0.5720525698257588,
+            "angularVelocity": 0.0000011256221281562495,
+            "velocityX": 3.6452242132183756,
+            "velocityY": -1.0114225585101817,
+            "timestamp": 1.8775517922331635
+        },
+        {
+            "x": 6.280428909416468,
+            "y": 3.8124486851902244,
+            "heading": -0.5720525182264563,
+            "angularVelocity": 0.0000011247362300949913,
+            "velocityX": 3.6475098193329205,
+            "velocityY": -1.0031486808299002,
+            "timestamp": 1.923428595260182
+        },
+        {
+            "x": 6.444900242470224,
+            "y": 3.7570546132296903,
+            "heading": -0.5720524653781475,
+            "angularVelocity": 0.000001151961455361842,
+            "velocityX": 3.585065266142057,
+            "velocityY": -1.2074527496557126,
+            "timestamp": 1.9693053982872004
+        },
+        {
+            "x": 6.602974533610246,
+            "y": 3.685417746961916,
+            "heading": -0.5720524086130027,
+            "angularVelocity": 0.0000012373387213911858,
+            "velocityX": 3.445625691199315,
+            "velocityY": -1.561505195243581,
+            "timestamp": 2.015182201314219
+        },
+        {
+            "x": 6.753006171084989,
+            "y": 3.5981834135738024,
+            "heading": -0.5720523449049553,
+            "angularVelocity": 0.0000013886766999770178,
+            "velocityX": 3.27031588025751,
+            "velocityY": -1.9014911160397099,
+            "timestamp": 2.0610590043412373
+        },
+        {
+            "x": 6.893461047058093,
+            "y": 3.4962434932531963,
+            "heading": -0.5720522700606003,
+            "angularVelocity": 0.0000016314204553722347,
+            "velocityX": 3.0615663408441725,
+            "velocityY": -2.2220362709370285,
+            "timestamp": 2.106935807368256
+        },
+        {
+            "x": 7.0229029918463715,
+            "y": 3.380640293435665,
+            "heading": -0.5720521774325067,
+            "angularVelocity": 0.000002019061650871652,
+            "velocityX": 2.8215118806773165,
+            "velocityY": -2.519861720737319,
+            "timestamp": 2.1528126103952743
         },
         {
             "x": 7.140008449554443,
             "y": 3.2525558471679688,
-            "heading": 0.541138510449912,
-            "angularVelocity": -0.0000022536692406151377,
-            "velocityX": 2.5542729494461978,
-            "velocityY": -2.790397984245173,
-            "timestamp": 2.1500697462465537
-        },
-        {
-            "x": 7.220411568965462,
-            "y": 3.1500924469852043,
-            "heading": 0.5411383954402629,
-            "angularVelocity": -0.0000033404657713126234,
-            "velocityX": 2.3353159519018676,
-            "velocityY": -2.97605882316215,
-            "timestamp": 2.1844989716991257
-        },
-        {
-            "x": 7.292813165778826,
-            "y": 3.0418269330928753,
-            "heading": 0.5411383062644725,
-            "angularVelocity": -0.000002590118983726798,
-            "velocityX": 2.102910996737314,
-            "velocityY": -3.1445817461525896,
-            "timestamp": 2.2189281971516976
-        },
-        {
-            "x": 7.3567962971772936,
-            "y": 2.928382790909777,
-            "heading": 0.5411382336043034,
-            "angularVelocity": -0.000002110421251450144,
-            "velocityX": 1.8583958993386591,
-            "velocityY": -3.2949954781694024,
-            "timestamp": 2.2533574226042696
-        },
-        {
-            "x": 7.411992506551136,
-            "y": 2.810413328004449,
-            "heading": 0.5411381719390687,
-            "angularVelocity": -0.00000179107237982498,
-            "velocityX": 1.603178946034542,
-            "velocityY": -3.426433832147572,
-            "timestamp": 2.2877866480568416
-        },
-        {
-            "x": 7.458083940034777,
-            "y": 2.688597909034967,
-            "heading": 0.5411381177395469,
-            "angularVelocity": -0.0000015742300651441758,
-            "velocityX": 1.3387298981538813,
-            "velocityY": -3.5381399775400135,
-            "timestamp": 2.3222158735094136
-        },
-        {
-            "x": 7.4948097486817264,
-            "y": 2.563639387590104,
-            "heading": 0.5411380685927316,
-            "angularVelocity": -0.0000014274737405214791,
-            "velocityX": 1.0667044687814087,
-            "velocityY": -3.6294316762078314,
-            "timestamp": 2.3566450989619856
-        },
-        {
-            "x": 7.521973664613143,
-            "y": 2.436259879309271,
-            "heading": 0.5411380227355477,
-            "angularVelocity": -0.0000013319260973088958,
-            "velocityX": 0.7889784209297904,
-            "velocityY": -3.6997494601296452,
-            "timestamp": 2.3910743244145576
-        },
-        {
-            "x": 7.539529818670366,
-            "y": 2.3072048557852676,
-            "heading": 0.5411379787715688,
-            "angularVelocity": -0.0000012769377868531526,
-            "velocityX": 0.5099200991725963,
-            "velocityY": -3.748414953504679,
-            "timestamp": 2.4255035498671296
-        },
-        {
-            "x": 7.551115587366525,
-            "y": 2.177477496871536,
-            "heading": 0.5411379354425048,
-            "angularVelocity": -0.0000012584966258013745,
-            "velocityX": 0.336509710684848,
-            "velocityY": -3.7679429963488027,
-            "timestamp": 2.4599327753197016
-        },
-        {
-            "x": 7.56271584344406,
-            "y": 2.0477514326184285,
-            "heading": 0.5411378921115911,
-            "angularVelocity": -0.0000012585503448264627,
-            "velocityX": 0.3369304980023622,
-            "velocityY": -3.76790539281253,
-            "timestamp": 2.4943620007722735
-        },
-        {
-            "x": 7.574376716473137,
-            "y": 1.9180308030778739,
-            "heading": 0.5411378487769694,
-            "angularVelocity": -0.0000012586580504181092,
-            "velocityX": 0.3386911228976962,
-            "velocityY": -3.767747541089928,
-            "timestamp": 2.5287912262248455
-        },
-        {
-            "x": 7.58611108718596,
-            "y": 1.788316801391074,
-            "heading": 0.541137805437783,
-            "angularVelocity": -0.0000012587906282413006,
-            "velocityX": 0.34082586983899804,
-            "velocityY": -3.7675550344719086,
-            "timestamp": 2.5632204516774175
-        },
-        {
-            "x": 7.597929211011114,
-            "y": 1.6586104040760536,
-            "heading": 0.541137760738121,
-            "angularVelocity": -0.0000012983057658513822,
-            "velocityX": 0.3432584866444477,
-            "velocityY": -3.7673341647982834,
-            "timestamp": 2.5976496771299895
-        },
-        {
-            "x": 7.610687440254598,
-            "y": 1.5360368797463901,
-            "heading": 0.5228402741207003,
-            "angularVelocity": -0.5314521711397164,
-            "velocityX": 0.37056393444221536,
-            "velocityY": -3.560159217014913,
-            "timestamp": 2.6320789025825615
-        },
-        {
-            "x": 7.6219652115761445,
-            "y": 1.4215043504075173,
-            "heading": 0.4829676933785713,
-            "angularVelocity": -1.1581027518921772,
-            "velocityX": 0.3275638987899306,
-            "velocityY": -3.3266077825840545,
-            "timestamp": 2.6665081280351335
-        },
-        {
-            "x": 7.631828440236478,
-            "y": 1.3151733520485158,
-            "heading": 0.42230189460591644,
-            "angularVelocity": -1.7620436700286786,
-            "velocityX": 0.2864783779098223,
-            "velocityY": -3.0883935656779764,
-            "timestamp": 2.7009373534877055
-        },
-        {
-            "x": 7.640727843462309,
-            "y": 1.2186121100176637,
-            "heading": 0.3575309788065375,
-            "angularVelocity": -1.8812771692652814,
-            "velocityX": 0.2584839800738238,
-            "velocityY": -2.804630100199854,
-            "timestamp": 2.7353665789402775
-        },
-        {
-            "x": 7.648686345492233,
-            "y": 1.1318869979940107,
-            "heading": 0.2935915741251458,
-            "angularVelocity": -1.8571258528454382,
-            "velocityX": 0.23115541884308086,
-            "velocityY": -2.518938805147378,
-            "timestamp": 2.7697958043928494
-        },
-        {
-            "x": 7.655721476158885,
-            "y": 1.054956995812666,
-            "heading": 0.2330360616068616,
-            "angularVelocity": -1.7588403956899903,
-            "velocityX": 0.20433601320340974,
-            "velocityY": -2.234438944533275,
-            "timestamp": 2.8042250298454214
-        },
-        {
-            "x": 7.661845751852373,
-            "y": 0.9877699914439897,
-            "heading": 0.17748768247912494,
-            "angularVelocity": -1.6134077487241032,
-            "velocityX": 0.17788014725831017,
-            "velocityY": -1.9514526825829648,
-            "timestamp": 2.8386542552979934
-        },
-        {
-            "x": 7.667069516180101,
-            "y": 0.9302779311076093,
-            "heading": 0.1280864585154153,
-            "angularVelocity": -1.4348630651528973,
-            "velocityX": 0.15172471233573487,
-            "velocityY": -1.6698621470755346,
-            "timestamp": 2.8730834807505654
-        },
-        {
-            "x": 7.6714024480246685,
-            "y": 0.88243945985842,
-            "heading": 0.08567598072175592,
-            "angularVelocity": -1.2318162037100373,
-            "velocityX": 0.1258504014428196,
-            "velocityY": -1.3894727697283988,
-            "timestamp": 2.9075127062031374
-        },
-        {
-            "x": 7.674853841393155,
-            "y": 0.8442197139951643,
-            "heading": 0.050901182135293016,
-            "angularVelocity": -1.0100372032583045,
-            "velocityX": 0.10024603583499626,
-            "velocityY": -1.110096011770723,
-            "timestamp": 2.9419419316557094
-        },
-        {
-            "x": 7.6774323573426795,
-            "y": 0.8155893376478138,
-            "heading": 0.02426863930506587,
-            "angularVelocity": -0.7735446406401408,
-            "velocityX": 0.07489323142279661,
-            "velocityY": -0.8315718977410657,
-            "timestamp": 2.9763711571082814
-        },
-        {
-            "x": 7.6791456483845035,
-            "y": 0.7965233362757584,
-            "heading": 0.006187731070844568,
-            "angularVelocity": -0.5251616322048499,
-            "velocityX": 0.04976269489958636,
-            "velocityY": -0.5537737524278505,
-            "timestamp": 3.0108003825608534
+            "heading": -0.5720520639012779,
+            "angularVelocity": 0.0000024746979161572956,
+            "velocityX": 2.5526071997441684,
+            "velocityY": -2.7919217952549285,
+            "timestamp": 2.1986894134222927
+        },
+        {
+            "x": 7.220415929770595,
+            "y": 3.149946383889573,
+            "heading": -0.5720519473102574,
+            "angularVelocity": 0.000003383341158681086,
+            "velocityX": 2.333335244140948,
+            "velocityY": -2.977611988413324,
+            "timestamp": 2.2331497338662096
+        },
+        {
+            "x": 7.2928033634314815,
+            "y": 3.0415299314010706,
+            "heading": -0.5720518591979534,
+            "angularVelocity": 0.000002556920622107498,
+            "velocityX": 2.100602453152653,
+            "velocityY": -3.1461243276865574,
+            "timestamp": 2.2676100543101265
+        },
+        {
+            "x": 7.356753135957797,
+            "y": 2.927931973099431,
+            "heading": -0.5720517877574303,
+            "angularVelocity": 0.000002073124165462267,
+            "velocityX": 1.855750953634494,
+            "velocityY": -3.2964858375736252,
+            "timestamp": 2.3020703747540434
+        },
+        {
+            "x": 7.411896317838347,
+            "y": 2.8098078848104033,
+            "heading": -0.5720517267503814,
+            "angularVelocity": 0.0000017703564026272917,
+            "velocityX": 1.6001935318717442,
+            "velocityY": -3.427829073187373,
+            "timestamp": 2.3365306951979603
+        },
+        {
+            "x": 7.457914797691639,
+            "y": 2.687839154921363,
+            "heading": -0.572051672451037,
+            "angularVelocity": 0.0000015757063075533039,
+            "velocityX": 1.3354048732140973,
+            "velocityY": -3.5393962771625227,
+            "timestamp": 2.370991015641877
+        },
+        {
+            "x": 7.494543133171347,
+            "y": 2.5627294544923394,
+            "heading": -0.5720516223974168,
+            "angularVelocity": 0.000001452500141635371,
+            "velocityX": 1.0629133742199746,
+            "velocityY": -3.630543733121263,
+            "timestamp": 2.405451336085794
+        },
+        {
+            "x": 7.521592976236692,
+            "y": 2.435205423722826,
+            "heading": -0.572051574793162,
+            "angularVelocity": 0.0000013814222897397843,
+            "velocityX": 0.7849562255098341,
+            "velocityY": -3.70060490229679,
+            "timestamp": 2.439911656529711
+        },
+        {
+            "x": 7.539069360193098,
+            "y": 2.3060208712744585,
+            "heading": -0.572051528188319,
+            "angularVelocity": 0.000001352420480159191,
+            "velocityX": 0.5071451376970121,
+            "velocityY": -3.748791386272309,
+            "timestamp": 2.474371976973628
+        },
+        {
+            "x": 7.551504596507269,
+            "y": 2.176254012981795,
+            "heading": -0.5720514816592456,
+            "angularVelocity": 0.0000013502217280486013,
+            "velocityX": 0.3608566651205708,
+            "velocityY": -3.7656892513183533,
+            "timestamp": 2.5088322974175448
+        },
+        {
+            "x": 7.5639699142109125,
+            "y": 2.0464900408376128,
+            "heading": -0.5720514351306527,
+            "angularVelocity": 0.0000013502077822692335,
+            "velocityX": 0.36172959342888805,
+            "velocityY": -3.765605498514506,
+            "timestamp": 2.5432926178614617
+        },
+        {
+            "x": 7.576441327813496,
+            "y": 1.916726654418564,
+            "heading": -0.5720513886022516,
+            "angularVelocity": 0.00000135020222116771,
+            "velocityX": 0.36190648960876737,
+            "velocityY": -3.7655885014252446,
+            "timestamp": 2.5777529383053785
+        },
+        {
+            "x": 7.58884833483585,
+            "y": 1.786957094038731,
+            "heading": -0.5720513420716767,
+            "angularVelocity": 0.000001350265295572762,
+            "velocityX": 0.36003748258495927,
+            "velocityY": -3.765767662871606,
+            "timestamp": 2.6122132587492954
+        },
+        {
+            "x": 7.601019357724818,
+            "y": 1.6571651890693204,
+            "heading": -0.5720512933827582,
+            "angularVelocity": 0.0000014128980208836598,
+            "velocityX": 0.3531894866947264,
+            "velocityY": -3.7664160778959355,
+            "timestamp": 2.6466735791932123
+        },
+        {
+            "x": 7.610550391987499,
+            "y": 1.5345810963054864,
+            "heading": -0.5537181215211259,
+            "angularVelocity": 0.5320081654919462,
+            "velocityX": 0.2765799661741953,
+            "velocityY": -3.5572534203020942,
+            "timestamp": 2.6811338996371292
+        },
+        {
+            "x": 7.620334086621719,
+            "y": 1.4202071871878734,
+            "heading": -0.5150322166693104,
+            "angularVelocity": 1.122621738667962,
+            "velocityX": 0.2839118878810709,
+            "velocityY": -3.319003063356803,
+            "timestamp": 2.715594220081046
+        },
+        {
+            "x": 7.630352820350644,
+            "y": 1.314046573370508,
+            "heading": -0.4556179078059868,
+            "angularVelocity": 1.7241368651811046,
+            "velocityX": 0.290732459821803,
+            "velocityY": -3.0806624096878554,
+            "timestamp": 2.750054540524963
+        },
+        {
+            "x": 7.63947235322732,
+            "y": 1.2175511973082003,
+            "heading": -0.3892109145347841,
+            "angularVelocity": 1.9270567544269157,
+            "velocityX": 0.2646386556821465,
+            "velocityY": -2.8001880080992603,
+            "timestamp": 2.78451486096888
+        },
+        {
+            "x": 7.647652812069227,
+            "y": 1.1309118710627388,
+            "heading": -0.32231407882554747,
+            "angularVelocity": 1.9412714347239703,
+            "velocityX": 0.23738777633239685,
+            "velocityY": -2.5141764536539966,
+            "timestamp": 2.818975181412797
+        },
+        {
+            "x": 7.654895533140534,
+            "y": 1.0540989766744202,
+            "heading": -0.25819483301023066,
+            "angularVelocity": 1.8606688791433752,
+            "velocityX": 0.21017567387602423,
+            "velocityY": -2.22902437930995,
+            "timestamp": 2.8534355018567137
+        },
+        {
+            "x": 7.66120815675186,
+            "y": 0.9870533783479266,
+            "heading": -0.19885832997172487,
+            "angularVelocity": 1.7218790270703905,
+            "velocityX": 0.18318528469800885,
+            "velocityY": -1.9455883596791401,
+            "timestamp": 2.8878958223006306
+        },
+        {
+            "x": 7.666600355589264,
+            "y": 0.929716153410391,
+            "heading": -0.14570825222338094,
+            "angularVelocity": 1.542355876662632,
+            "velocityX": 0.15647558606309142,
+            "velocityY": -1.663862210186161,
+            "timestamp": 2.9223561427445475
+        },
+        {
+            "x": 7.671080940668327,
+            "y": 0.8820338579186632,
+            "heading": -0.09980369912266952,
+            "angularVelocity": 1.3320988461333123,
+            "velocityX": 0.1300215732566484,
+            "velocityY": -1.383686944215496,
+            "timestamp": 2.9568164631884644
+        },
+        {
+            "x": 7.674656709905022,
+            "y": 0.8439593502971015,
+            "heading": -0.06197555143316066,
+            "angularVelocity": 1.0977305841102087,
+            "velocityX": 0.10376482837700403,
+            "velocityY": -1.1048796741031013,
+            "timestamp": 2.9912767836323813
+        },
+        {
+            "x": 7.67733258738128,
+            "y": 0.8154516156506164,
+            "heading": -0.032889080662387146,
+            "angularVelocity": 0.8440568861838474,
+            "velocityX": 0.07765097485389878,
+            "velocityY": -0.8272626104241091,
+            "timestamp": 3.025737104076298
+        },
+        {
+            "x": 7.679112422858347,
+            "y": 0.7964752743679322,
+            "heading": -0.013083588528854375,
+            "angularVelocity": 0.5747332548975859,
+            "velocityX": 0.05164883710108163,
+            "velocityY": -0.5506722235380279,
+            "timestamp": 3.060197424520215
         },
         {
             "x": 7.68,
             "y": 0.787,
             "heading": -0.003,
-            "angularVelocity": -0.26685848868430057,
-            "velocityX": 0.024814720757314154,
-            "velocityY": -0.2766061725343536,
-            "timestamp": 3.0452296080134253
+            "angularVelocity": 0.2926144736603049,
+            "velocityX": 0.025756497043992084,
+            "velocityY": -0.27496187632245256,
+            "timestamp": 3.094657744964132
         },
         {
             "x": 7.68,
             "y": 0.787,
             "heading": -0.003,
-            "angularVelocity": -2.388488097950269e-30,
+            "angularVelocity": 9.445490920898867e-33,
             "velocityX": 0,
-            "velocityY": 9.563090319474696e-31,
-            "timestamp": 3.0796588334659973
+            "velocityY": 3.8255947334103544e-33,
+            "timestamp": 3.129118065408049
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj
index 7714b3ff..59066a5b 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj	
@@ -4,118 +4,118 @@
             "x": 0.71,
             "y": 6.72,
             "heading": 1.04,
-            "angularVelocity": 6.421381184701714e-32,
-            "velocityX": 9.791473054297313e-34,
-            "velocityY": 6.656933252086786e-33,
+            "angularVelocity": -3.0520704892108665e-32,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.7541195074128881,
-            "y": 6.720987207910964,
-            "heading": 1.0162961446571532,
-            "angularVelocity": -0.3240022105828226,
-            "velocityX": 0.6030587735562275,
-            "velocityY": 0.01349390387478939,
-            "timestamp": 0.0731595482024364
+            "x": 0.7541195079597249,
+            "y": 6.720987197392724,
+            "heading": 1.0162961519009226,
+            "angularVelocity": -0.3240021115694771,
+            "velocityX": 0.6030587810307456,
+            "velocityY": 0.013493760103527496,
+            "timestamp": 0.07315954820244353
         },
         {
-            "x": 0.842413628953284,
-            "y": 6.722972522190638,
-            "heading": 0.9702543638919431,
-            "angularVelocity": -0.6293338586206134,
-            "velocityX": 1.2068707873384146,
-            "velocityY": 0.027136776107259192,
-            "timestamp": 0.1463190964048728
+            "x": 0.8424136303815098,
+            "y": 6.722972492793459,
+            "heading": 0.9702543819568796,
+            "angularVelocity": -0.6293337107086672,
+            "velocityX": 1.2068707993857855,
+            "velocityY": 0.027136518055622637,
+            "timestamp": 0.14631909640488705
         },
         {
-            "x": 0.9749626727340511,
-            "y": 6.725969397195605,
-            "heading": 0.9040114408742106,
-            "angularVelocity": -0.9054583392778368,
-            "velocityX": 1.811780513105384,
-            "velocityY": 0.04096355265456321,
-            "timestamp": 0.2194786446073092
+            "x": 0.9749626751701804,
+            "y": 6.7259693430054925,
+            "heading": 0.9040114694947134,
+            "angularVelocity": -0.9054581949963141,
+            "velocityX": 1.8117805268819878,
+            "velocityY": 0.0409632137659495,
+            "timestamp": 0.21947864460733058
         },
         {
-            "x": 1.1518931623979327,
-            "y": 6.729992809371002,
-            "heading": 0.821470963167313,
-            "angularVelocity": -1.1282256347246662,
-            "velocityX": 2.418419659649969,
-            "velocityY": 0.05499503857340178,
-            "timestamp": 0.2926381928097456
+            "x": 1.1518931657803464,
+            "y": 6.729992727527654,
+            "heading": 0.8214709979365993,
+            "angularVelocity": -1.1282255506783914,
+            "velocityX": 2.4184196725842533,
+            "velocityY": 0.0549946605880228,
+            "timestamp": 0.2926381928097741
         },
         {
-            "x": 1.3734034405206448,
-            "y": 6.735064554387548,
-            "heading": 0.7325177503671355,
-            "angularVelocity": -1.215879744828927,
-            "velocityX": 3.027769902429938,
-            "velocityY": 0.06932444419316391,
-            "timestamp": 0.36579774101218204
+            "x": 1.3734034446931793,
+            "y": 6.735064446892539,
+            "heading": 0.7325177816386138,
+            "angularVelocity": -1.215879792639604,
+            "velocityX": 3.0277699132296103,
+            "velocityY": 0.06932409356680827,
+            "timestamp": 0.3657977410122176
         },
         {
-            "x": 1.6366130242279704,
-            "y": 6.741790766846534,
-            "heading": 0.6993734477778637,
-            "angularVelocity": -0.45304137879066375,
-            "velocityX": 3.5977475281693874,
-            "velocityY": 0.09193895566910278,
-            "timestamp": 0.43895728921461846
+            "x": 1.6366130282089124,
+            "y": 6.741790657706068,
+            "heading": 0.6993734786052862,
+            "angularVelocity": -0.4530413848603092,
+            "velocityX": 3.597747525550208,
+            "velocityY": 0.09193893317775545,
+            "timestamp": 0.43895728921466115
         },
         {
-            "x": 1.8581205064725355,
-            "y": 6.746838576232807,
-            "heading": 0.6098795976632514,
-            "angularVelocity": -1.223269584265702,
-            "velocityX": 3.0277316862542305,
-            "velocityY": 0.06899727390750988,
-            "timestamp": 0.5121168374170548
+            "x": 1.858120509684905,
+            "y": 6.746838490569188,
+            "heading": 0.6098796286126051,
+            "angularVelocity": -1.2232695825988174,
+            "velocityX": 3.0277316757485147,
+            "velocityY": 0.0689975948068392,
+            "timestamp": 0.5121168374171047
         },
         {
-            "x": 2.0350481932432154,
-            "y": 6.750890564225749,
-            "heading": 0.5269052400796844,
-            "angularVelocity": -1.134156232813029,
-            "velocityX": 2.4183813475871814,
-            "velocityY": 0.05538563444554272,
-            "timestamp": 0.5852763856194912
+            "x": 2.035048195511774,
+            "y": 6.750890505964207,
+            "heading": 0.5269052635268205,
+            "angularVelocity": -1.1341563353588457,
+            "velocityX": 2.418381334686234,
+            "velocityY": 0.0553860089978706,
+            "timestamp": 0.5852763856195482
         },
         {
-            "x": 2.167592642728435,
-            "y": 6.753939698452488,
-            "heading": 0.4602292040948866,
-            "angularVelocity": -0.9113784546659414,
-            "velocityX": 1.8117177148013692,
-            "velocityY": 0.04167787119598535,
-            "timestamp": 0.6584359338219277
+            "x": 2.1675926440263233,
+            "y": 6.753939666061267,
+            "heading": 0.4602292179018529,
+            "angularVelocity": -0.9113785864349585,
+            "velocityX": 1.811717701533345,
+            "velocityY": 0.041678224811154865,
+            "timestamp": 0.6584359338219917
         },
         {
-            "x": 2.255882514523324,
-            "y": 6.755978249842539,
-            "heading": 0.41386063175705684,
-            "angularVelocity": -0.633800692829844,
-            "velocityX": 1.2068126986047898,
-            "velocityY": 0.02786446116929368,
-            "timestamp": 0.7315954820243641
+            "x": 2.2558825150080364,
+            "y": 6.755978237986949,
+            "heading": 0.41386063697230147,
+            "angularVelocity": -0.6338008102679221,
+            "velocityX": 1.206812687489562,
+            "velocityY": 0.027864741865846625,
+            "timestamp": 0.7315954820244353
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.39,
-            "angularVelocity": -0.3261451491065467,
-            "velocityX": 0.6030311362038534,
-            "velocityY": 0.013966053407445108,
-            "timestamp": 0.8047550302268005
+            "angularVelocity": -0.3261452203924221,
+            "velocityX": 0.6030311295783809,
+            "velocityY": 0.013966215458624026,
+            "timestamp": 0.8047550302268788
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.39,
-            "angularVelocity": -1.0168243354874858e-31,
-            "velocityX": -4.086489815881427e-33,
-            "velocityY": 0,
-            "timestamp": 0.8779145784292369
+            "angularVelocity": 9.371559495672325e-33,
+            "velocityX": 2.5226214414524116e-33,
+            "velocityY": -5.278780681595662e-33,
+            "timestamp": 0.8779145784293223
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.traj
index 7714b3ff..59066a5b 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.traj	
@@ -4,118 +4,118 @@
             "x": 0.71,
             "y": 6.72,
             "heading": 1.04,
-            "angularVelocity": 6.421381184701714e-32,
-            "velocityX": 9.791473054297313e-34,
-            "velocityY": 6.656933252086786e-33,
+            "angularVelocity": -3.0520704892108665e-32,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.7541195074128881,
-            "y": 6.720987207910964,
-            "heading": 1.0162961446571532,
-            "angularVelocity": -0.3240022105828226,
-            "velocityX": 0.6030587735562275,
-            "velocityY": 0.01349390387478939,
-            "timestamp": 0.0731595482024364
+            "x": 0.7541195079597249,
+            "y": 6.720987197392724,
+            "heading": 1.0162961519009226,
+            "angularVelocity": -0.3240021115694771,
+            "velocityX": 0.6030587810307456,
+            "velocityY": 0.013493760103527496,
+            "timestamp": 0.07315954820244353
         },
         {
-            "x": 0.842413628953284,
-            "y": 6.722972522190638,
-            "heading": 0.9702543638919431,
-            "angularVelocity": -0.6293338586206134,
-            "velocityX": 1.2068707873384146,
-            "velocityY": 0.027136776107259192,
-            "timestamp": 0.1463190964048728
+            "x": 0.8424136303815098,
+            "y": 6.722972492793459,
+            "heading": 0.9702543819568796,
+            "angularVelocity": -0.6293337107086672,
+            "velocityX": 1.2068707993857855,
+            "velocityY": 0.027136518055622637,
+            "timestamp": 0.14631909640488705
         },
         {
-            "x": 0.9749626727340511,
-            "y": 6.725969397195605,
-            "heading": 0.9040114408742106,
-            "angularVelocity": -0.9054583392778368,
-            "velocityX": 1.811780513105384,
-            "velocityY": 0.04096355265456321,
-            "timestamp": 0.2194786446073092
+            "x": 0.9749626751701804,
+            "y": 6.7259693430054925,
+            "heading": 0.9040114694947134,
+            "angularVelocity": -0.9054581949963141,
+            "velocityX": 1.8117805268819878,
+            "velocityY": 0.0409632137659495,
+            "timestamp": 0.21947864460733058
         },
         {
-            "x": 1.1518931623979327,
-            "y": 6.729992809371002,
-            "heading": 0.821470963167313,
-            "angularVelocity": -1.1282256347246662,
-            "velocityX": 2.418419659649969,
-            "velocityY": 0.05499503857340178,
-            "timestamp": 0.2926381928097456
+            "x": 1.1518931657803464,
+            "y": 6.729992727527654,
+            "heading": 0.8214709979365993,
+            "angularVelocity": -1.1282255506783914,
+            "velocityX": 2.4184196725842533,
+            "velocityY": 0.0549946605880228,
+            "timestamp": 0.2926381928097741
         },
         {
-            "x": 1.3734034405206448,
-            "y": 6.735064554387548,
-            "heading": 0.7325177503671355,
-            "angularVelocity": -1.215879744828927,
-            "velocityX": 3.027769902429938,
-            "velocityY": 0.06932444419316391,
-            "timestamp": 0.36579774101218204
+            "x": 1.3734034446931793,
+            "y": 6.735064446892539,
+            "heading": 0.7325177816386138,
+            "angularVelocity": -1.215879792639604,
+            "velocityX": 3.0277699132296103,
+            "velocityY": 0.06932409356680827,
+            "timestamp": 0.3657977410122176
         },
         {
-            "x": 1.6366130242279704,
-            "y": 6.741790766846534,
-            "heading": 0.6993734477778637,
-            "angularVelocity": -0.45304137879066375,
-            "velocityX": 3.5977475281693874,
-            "velocityY": 0.09193895566910278,
-            "timestamp": 0.43895728921461846
+            "x": 1.6366130282089124,
+            "y": 6.741790657706068,
+            "heading": 0.6993734786052862,
+            "angularVelocity": -0.4530413848603092,
+            "velocityX": 3.597747525550208,
+            "velocityY": 0.09193893317775545,
+            "timestamp": 0.43895728921466115
         },
         {
-            "x": 1.8581205064725355,
-            "y": 6.746838576232807,
-            "heading": 0.6098795976632514,
-            "angularVelocity": -1.223269584265702,
-            "velocityX": 3.0277316862542305,
-            "velocityY": 0.06899727390750988,
-            "timestamp": 0.5121168374170548
+            "x": 1.858120509684905,
+            "y": 6.746838490569188,
+            "heading": 0.6098796286126051,
+            "angularVelocity": -1.2232695825988174,
+            "velocityX": 3.0277316757485147,
+            "velocityY": 0.0689975948068392,
+            "timestamp": 0.5121168374171047
         },
         {
-            "x": 2.0350481932432154,
-            "y": 6.750890564225749,
-            "heading": 0.5269052400796844,
-            "angularVelocity": -1.134156232813029,
-            "velocityX": 2.4183813475871814,
-            "velocityY": 0.05538563444554272,
-            "timestamp": 0.5852763856194912
+            "x": 2.035048195511774,
+            "y": 6.750890505964207,
+            "heading": 0.5269052635268205,
+            "angularVelocity": -1.1341563353588457,
+            "velocityX": 2.418381334686234,
+            "velocityY": 0.0553860089978706,
+            "timestamp": 0.5852763856195482
         },
         {
-            "x": 2.167592642728435,
-            "y": 6.753939698452488,
-            "heading": 0.4602292040948866,
-            "angularVelocity": -0.9113784546659414,
-            "velocityX": 1.8117177148013692,
-            "velocityY": 0.04167787119598535,
-            "timestamp": 0.6584359338219277
+            "x": 2.1675926440263233,
+            "y": 6.753939666061267,
+            "heading": 0.4602292179018529,
+            "angularVelocity": -0.9113785864349585,
+            "velocityX": 1.811717701533345,
+            "velocityY": 0.041678224811154865,
+            "timestamp": 0.6584359338219917
         },
         {
-            "x": 2.255882514523324,
-            "y": 6.755978249842539,
-            "heading": 0.41386063175705684,
-            "angularVelocity": -0.633800692829844,
-            "velocityX": 1.2068126986047898,
-            "velocityY": 0.02786446116929368,
-            "timestamp": 0.7315954820243641
+            "x": 2.2558825150080364,
+            "y": 6.755978237986949,
+            "heading": 0.41386063697230147,
+            "angularVelocity": -0.6338008102679221,
+            "velocityX": 1.206812687489562,
+            "velocityY": 0.027864741865846625,
+            "timestamp": 0.7315954820244353
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.39,
-            "angularVelocity": -0.3261451491065467,
-            "velocityX": 0.6030311362038534,
-            "velocityY": 0.013966053407445108,
-            "timestamp": 0.8047550302268005
+            "angularVelocity": -0.3261452203924221,
+            "velocityX": 0.6030311295783809,
+            "velocityY": 0.013966215458624026,
+            "timestamp": 0.8047550302268788
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.39,
-            "angularVelocity": -1.0168243354874858e-31,
-            "velocityX": -4.086489815881427e-33,
-            "velocityY": 0,
-            "timestamp": 0.8779145784292369
+            "angularVelocity": 9.371559495672325e-33,
+            "velocityX": 2.5226214414524116e-33,
+            "velocityY": -5.278780681595662e-33,
+            "timestamp": 0.8779145784293223
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.1.traj
index 886a091a..fb2e2531 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.1.traj	
@@ -4,145 +4,145 @@
             "x": 0.71,
             "y": 6.72,
             "heading": 1.04,
-            "angularVelocity": 1.094777100362113e-30,
-            "velocityX": 3.4815734426203924e-31,
-            "velocityY": -2.2714927632651006e-31,
+            "angularVelocity": 3.547724392998225e-32,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.7374353381803705,
-            "y": 6.699514089758397,
-            "heading": 1.0076617651006685,
-            "angularVelocity": -0.49661202140938077,
-            "velocityX": 0.42131918437160226,
-            "velocityY": -0.3145981630464246,
-            "timestamp": 0.06511770457661528
+            "x": 0.737435339105297,
+            "y": 6.6995140898970185,
+            "heading": 1.00766177496316,
+            "angularVelocity": -0.49661186995303425,
+            "velocityX": 0.4213191985755434,
+            "velocityY": -0.3145981609176562,
+            "timestamp": 0.06511770457661178
         },
         {
-            "x": 0.7924238459364756,
-            "y": 6.658398285396173,
-            "heading": 0.9457835104036431,
-            "angularVelocity": -0.9502523944808582,
-            "velocityX": 0.8444478827015074,
-            "velocityY": -0.6314074586865741,
-            "timestamp": 0.13023540915323056
+            "x": 0.7924238483684921,
+            "y": 6.658398285939555,
+            "heading": 0.945783535120445,
+            "angularVelocity": -0.95025216636614,
+            "velocityX": 0.8444479058456402,
+            "velocityY": -0.6314074524707902,
+            "timestamp": 0.13023540915322357
         },
         {
-            "x": 0.8751312050328514,
-            "y": 6.596482140589978,
-            "heading": 0.858451871788505,
-            "angularVelocity": -1.3411350904174961,
-            "velocityX": 1.270120923858195,
-            "velocityY": -0.9508342655621027,
-            "timestamp": 0.19535311372984585
+            "x": 0.8751312092590968,
+            "y": 6.596482141852294,
+            "heading": 0.8584519121155169,
+            "angularVelocity": -1.3411348506945964,
+            "velocityX": 1.2701209514118896,
+            "velocityY": -0.9508342545216032,
+            "timestamp": 0.19535311372983535
         },
         {
-            "x": 0.9857818852205986,
-            "y": 6.513571276882533,
-            "heading": 0.7519056244975073,
-            "angularVelocity": -1.6362101210990878,
-            "velocityX": 1.6992411035858814,
-            "velocityY": -1.2732461048269026,
-            "timestamp": 0.2604708183064611
+            "x": 0.9857818913354864,
+            "y": 6.513571279138628,
+            "heading": 0.7519056782658311,
+            "angularVelocity": -1.6362099146835547,
+            "velocityX": 1.6992411325894867,
+            "velocityY": -1.273246089565702,
+            "timestamp": 0.26047081830644714
         },
         {
-            "x": 1.1246151400642461,
-            "y": 6.409448224375173,
-            "heading": 0.63671471411989,
-            "angularVelocity": -1.7689645408506078,
-            "velocityX": 2.1320354540492286,
-            "velocityY": -1.5989975872821531,
-            "timestamp": 0.3255885228830764
+            "x": 1.1246151480908613,
+            "y": 6.409448227792029,
+            "heading": 0.6367147770436085,
+            "angularVelocity": -1.7689644002530829,
+            "velocityX": 2.132035483407367,
+            "velocityY": -1.5989975694566836,
+            "timestamp": 0.32558852288305895
         },
         {
-            "x": 1.2913898302664586,
-            "y": 6.284148790385846,
-            "heading": 0.5378900355869111,
-            "angularVelocity": -1.5176314824903545,
-            "velocityX": 2.5611266749427672,
-            "velocityY": -1.9241991836782961,
-            "timestamp": 0.39070622745969164
+            "x": 1.2913898404503776,
+            "y": 6.284148794801154,
+            "heading": 0.5378900995711877,
+            "angularVelocity": -1.5176314662036392,
+            "velocityX": 2.5611267080722016,
+            "velocityY": -1.9241991683453519,
+            "timestamp": 0.39070622745967076
         },
         {
-            "x": 1.4799020803988825,
-            "y": 6.139172930172668,
-            "heading": 0.5154397477236047,
-            "angularVelocity": -0.3447647304104786,
-            "velocityX": 2.8949461802761682,
-            "velocityY": -2.2263662571613483,
-            "timestamp": 0.4558239320363069
+            "x": 1.479902092849603,
+            "y": 6.139172935751937,
+            "heading": 0.5154398140678585,
+            "angularVelocity": -0.3447646941687853,
+            "velocityX": 2.8949462150871557,
+            "velocityY": -2.2263662392867607,
+            "timestamp": 0.4558239320362826
         },
         {
-            "x": 1.668209892858901,
-            "y": 5.994133297119725,
-            "heading": 0.4928531841519364,
-            "angularVelocity": -0.3468574901176696,
-            "velocityX": 2.891806670464904,
-            "velocityY": -2.2273456043324518,
-            "timestamp": 0.5209416366129221
+            "x": 1.6682099030328927,
+            "y": 5.994133301373275,
+            "heading": 0.4928532472983225,
+            "angularVelocity": -0.34685753922672635,
+            "velocityX": 2.8918066355017875,
+            "velocityY": -2.2273456246913312,
+            "timestamp": 0.5209416366128944
         },
         {
-            "x": 1.8350205833080036,
-            "y": 5.869338176931447,
-            "heading": 0.3972353467498499,
-            "angularVelocity": -1.4683846432207315,
-            "velocityX": 2.5616795237743473,
-            "velocityY": -1.9164545341343717,
-            "timestamp": 0.5860593411895374
+            "x": 1.8350205910276742,
+            "y": 5.8693381796986746,
+            "heading": 0.39723540571375665,
+            "angularVelocity": -1.4683847074503305,
+            "velocityX": 2.561679486083952,
+            "velocityY": -1.9164545569596636,
+            "timestamp": 0.5860593411895062
         },
         {
-            "x": 1.9739536541090807,
-            "y": 5.765582598648054,
-            "heading": 0.2820330218741829,
-            "angularVelocity": -1.7691398310903903,
-            "velocityX": 2.1335683084100903,
-            "velocityY": -1.5933543566683568,
-            "timestamp": 0.6511770457661527
+            "x": 1.9739536597530634,
+            "y": 5.765582600106914,
+            "heading": 0.2820330722411117,
+            "angularVelocity": -1.7691399631126123,
+            "velocityX": 2.1335682765342634,
+            "velocityY": -1.5933543767607987,
+            "timestamp": 0.651177045766118
         },
         {
-            "x": 2.084680900392731,
-            "y": 5.683000749842365,
-            "heading": 0.17568947614699124,
-            "angularVelocity": -1.6330972723719295,
-            "velocityX": 1.7004169143181675,
-            "velocityY": -1.2681934865889841,
-            "timestamp": 0.7162947503427679
+            "x": 2.08468090414567,
+            "y": 5.683000750382208,
+            "heading": 0.1756895135649168,
+            "angularVelocity": -1.6330974712273538,
+            "velocityX": 1.7004168852778658,
+            "velocityY": -1.2681935007022147,
+            "timestamp": 0.7162947503427298
         },
         {
-            "x": 2.1674678561101657,
-            "y": 5.621347760780931,
-            "heading": 0.08927495882603552,
-            "angularVelocity": -1.3270510360094632,
-            "velocityX": 1.271343273779417,
-            "velocityY": -0.9467930336656067,
-            "timestamp": 0.7814124549193832
+            "x": 2.167467858188685,
+            "y": 5.6213477608369935,
+            "heading": 0.08927498136102419,
+            "angularVelocity": -1.3270512645639152,
+            "velocityX": 1.2713432480657338,
+            "velocityY": -0.946793041094972,
+            "timestamp": 0.7814124549193416
         },
         {
-            "x": 2.222525102580128,
-            "y": 5.580406412641355,
-            "heading": 0.02852690376362466,
-            "angularVelocity": -0.9328961371931729,
-            "velocityX": 0.8455034898409743,
-            "velocityY": -0.6287283682029188,
-            "timestamp": 0.8465301594959984
+            "x": 2.2225251033432207,
+            "y": 5.5804064125777195,
+            "heading": 0.028526912579098198,
+            "angularVelocity": -0.9328963478811614,
+            "velocityX": 0.8455034696402836,
+            "velocityY": -0.628728370041152,
+            "timestamp": 0.8465301594959534
         },
         {
             "x": 2.25,
             "y": 5.56,
             "heading": -0.003,
-            "angularVelocity": -0.4841525660126885,
-            "velocityX": 0.42192668796465593,
-            "velocityY": -0.3133773337686722,
-            "timestamp": 0.9116478640726137
+            "angularVelocity": -0.4841527013902356,
+            "velocityX": 0.42192667624600166,
+            "velocityY": -0.31337733279145685,
+            "timestamp": 0.9116478640725653
         },
         {
             "x": 2.25,
             "y": 5.56,
             "heading": -0.003,
-            "angularVelocity": 5.587549482799738e-30,
-            "velocityX": -2.981373275826657e-31,
-            "velocityY": -1.0757932056005479e-30,
-            "timestamp": 0.9767655686492289
+            "angularVelocity": -3.2592320489843035e-28,
+            "velocityX": -3.6408651267401044e-33,
+            "velocityY": 2.9208085569078165e-32,
+            "timestamp": 0.9767655686491771
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.traj
index 886a091a..fb2e2531 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.traj	
@@ -4,145 +4,145 @@
             "x": 0.71,
             "y": 6.72,
             "heading": 1.04,
-            "angularVelocity": 1.094777100362113e-30,
-            "velocityX": 3.4815734426203924e-31,
-            "velocityY": -2.2714927632651006e-31,
+            "angularVelocity": 3.547724392998225e-32,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.7374353381803705,
-            "y": 6.699514089758397,
-            "heading": 1.0076617651006685,
-            "angularVelocity": -0.49661202140938077,
-            "velocityX": 0.42131918437160226,
-            "velocityY": -0.3145981630464246,
-            "timestamp": 0.06511770457661528
+            "x": 0.737435339105297,
+            "y": 6.6995140898970185,
+            "heading": 1.00766177496316,
+            "angularVelocity": -0.49661186995303425,
+            "velocityX": 0.4213191985755434,
+            "velocityY": -0.3145981609176562,
+            "timestamp": 0.06511770457661178
         },
         {
-            "x": 0.7924238459364756,
-            "y": 6.658398285396173,
-            "heading": 0.9457835104036431,
-            "angularVelocity": -0.9502523944808582,
-            "velocityX": 0.8444478827015074,
-            "velocityY": -0.6314074586865741,
-            "timestamp": 0.13023540915323056
+            "x": 0.7924238483684921,
+            "y": 6.658398285939555,
+            "heading": 0.945783535120445,
+            "angularVelocity": -0.95025216636614,
+            "velocityX": 0.8444479058456402,
+            "velocityY": -0.6314074524707902,
+            "timestamp": 0.13023540915322357
         },
         {
-            "x": 0.8751312050328514,
-            "y": 6.596482140589978,
-            "heading": 0.858451871788505,
-            "angularVelocity": -1.3411350904174961,
-            "velocityX": 1.270120923858195,
-            "velocityY": -0.9508342655621027,
-            "timestamp": 0.19535311372984585
+            "x": 0.8751312092590968,
+            "y": 6.596482141852294,
+            "heading": 0.8584519121155169,
+            "angularVelocity": -1.3411348506945964,
+            "velocityX": 1.2701209514118896,
+            "velocityY": -0.9508342545216032,
+            "timestamp": 0.19535311372983535
         },
         {
-            "x": 0.9857818852205986,
-            "y": 6.513571276882533,
-            "heading": 0.7519056244975073,
-            "angularVelocity": -1.6362101210990878,
-            "velocityX": 1.6992411035858814,
-            "velocityY": -1.2732461048269026,
-            "timestamp": 0.2604708183064611
+            "x": 0.9857818913354864,
+            "y": 6.513571279138628,
+            "heading": 0.7519056782658311,
+            "angularVelocity": -1.6362099146835547,
+            "velocityX": 1.6992411325894867,
+            "velocityY": -1.273246089565702,
+            "timestamp": 0.26047081830644714
         },
         {
-            "x": 1.1246151400642461,
-            "y": 6.409448224375173,
-            "heading": 0.63671471411989,
-            "angularVelocity": -1.7689645408506078,
-            "velocityX": 2.1320354540492286,
-            "velocityY": -1.5989975872821531,
-            "timestamp": 0.3255885228830764
+            "x": 1.1246151480908613,
+            "y": 6.409448227792029,
+            "heading": 0.6367147770436085,
+            "angularVelocity": -1.7689644002530829,
+            "velocityX": 2.132035483407367,
+            "velocityY": -1.5989975694566836,
+            "timestamp": 0.32558852288305895
         },
         {
-            "x": 1.2913898302664586,
-            "y": 6.284148790385846,
-            "heading": 0.5378900355869111,
-            "angularVelocity": -1.5176314824903545,
-            "velocityX": 2.5611266749427672,
-            "velocityY": -1.9241991836782961,
-            "timestamp": 0.39070622745969164
+            "x": 1.2913898404503776,
+            "y": 6.284148794801154,
+            "heading": 0.5378900995711877,
+            "angularVelocity": -1.5176314662036392,
+            "velocityX": 2.5611267080722016,
+            "velocityY": -1.9241991683453519,
+            "timestamp": 0.39070622745967076
         },
         {
-            "x": 1.4799020803988825,
-            "y": 6.139172930172668,
-            "heading": 0.5154397477236047,
-            "angularVelocity": -0.3447647304104786,
-            "velocityX": 2.8949461802761682,
-            "velocityY": -2.2263662571613483,
-            "timestamp": 0.4558239320363069
+            "x": 1.479902092849603,
+            "y": 6.139172935751937,
+            "heading": 0.5154398140678585,
+            "angularVelocity": -0.3447646941687853,
+            "velocityX": 2.8949462150871557,
+            "velocityY": -2.2263662392867607,
+            "timestamp": 0.4558239320362826
         },
         {
-            "x": 1.668209892858901,
-            "y": 5.994133297119725,
-            "heading": 0.4928531841519364,
-            "angularVelocity": -0.3468574901176696,
-            "velocityX": 2.891806670464904,
-            "velocityY": -2.2273456043324518,
-            "timestamp": 0.5209416366129221
+            "x": 1.6682099030328927,
+            "y": 5.994133301373275,
+            "heading": 0.4928532472983225,
+            "angularVelocity": -0.34685753922672635,
+            "velocityX": 2.8918066355017875,
+            "velocityY": -2.2273456246913312,
+            "timestamp": 0.5209416366128944
         },
         {
-            "x": 1.8350205833080036,
-            "y": 5.869338176931447,
-            "heading": 0.3972353467498499,
-            "angularVelocity": -1.4683846432207315,
-            "velocityX": 2.5616795237743473,
-            "velocityY": -1.9164545341343717,
-            "timestamp": 0.5860593411895374
+            "x": 1.8350205910276742,
+            "y": 5.8693381796986746,
+            "heading": 0.39723540571375665,
+            "angularVelocity": -1.4683847074503305,
+            "velocityX": 2.561679486083952,
+            "velocityY": -1.9164545569596636,
+            "timestamp": 0.5860593411895062
         },
         {
-            "x": 1.9739536541090807,
-            "y": 5.765582598648054,
-            "heading": 0.2820330218741829,
-            "angularVelocity": -1.7691398310903903,
-            "velocityX": 2.1335683084100903,
-            "velocityY": -1.5933543566683568,
-            "timestamp": 0.6511770457661527
+            "x": 1.9739536597530634,
+            "y": 5.765582600106914,
+            "heading": 0.2820330722411117,
+            "angularVelocity": -1.7691399631126123,
+            "velocityX": 2.1335682765342634,
+            "velocityY": -1.5933543767607987,
+            "timestamp": 0.651177045766118
         },
         {
-            "x": 2.084680900392731,
-            "y": 5.683000749842365,
-            "heading": 0.17568947614699124,
-            "angularVelocity": -1.6330972723719295,
-            "velocityX": 1.7004169143181675,
-            "velocityY": -1.2681934865889841,
-            "timestamp": 0.7162947503427679
+            "x": 2.08468090414567,
+            "y": 5.683000750382208,
+            "heading": 0.1756895135649168,
+            "angularVelocity": -1.6330974712273538,
+            "velocityX": 1.7004168852778658,
+            "velocityY": -1.2681935007022147,
+            "timestamp": 0.7162947503427298
         },
         {
-            "x": 2.1674678561101657,
-            "y": 5.621347760780931,
-            "heading": 0.08927495882603552,
-            "angularVelocity": -1.3270510360094632,
-            "velocityX": 1.271343273779417,
-            "velocityY": -0.9467930336656067,
-            "timestamp": 0.7814124549193832
+            "x": 2.167467858188685,
+            "y": 5.6213477608369935,
+            "heading": 0.08927498136102419,
+            "angularVelocity": -1.3270512645639152,
+            "velocityX": 1.2713432480657338,
+            "velocityY": -0.946793041094972,
+            "timestamp": 0.7814124549193416
         },
         {
-            "x": 2.222525102580128,
-            "y": 5.580406412641355,
-            "heading": 0.02852690376362466,
-            "angularVelocity": -0.9328961371931729,
-            "velocityX": 0.8455034898409743,
-            "velocityY": -0.6287283682029188,
-            "timestamp": 0.8465301594959984
+            "x": 2.2225251033432207,
+            "y": 5.5804064125777195,
+            "heading": 0.028526912579098198,
+            "angularVelocity": -0.9328963478811614,
+            "velocityX": 0.8455034696402836,
+            "velocityY": -0.628728370041152,
+            "timestamp": 0.8465301594959534
         },
         {
             "x": 2.25,
             "y": 5.56,
             "heading": -0.003,
-            "angularVelocity": -0.4841525660126885,
-            "velocityX": 0.42192668796465593,
-            "velocityY": -0.3133773337686722,
-            "timestamp": 0.9116478640726137
+            "angularVelocity": -0.4841527013902356,
+            "velocityX": 0.42192667624600166,
+            "velocityY": -0.31337733279145685,
+            "timestamp": 0.9116478640725653
         },
         {
             "x": 2.25,
             "y": 5.56,
             "heading": -0.003,
-            "angularVelocity": 5.587549482799738e-30,
-            "velocityX": -2.981373275826657e-31,
-            "velocityY": -1.0757932056005479e-30,
-            "timestamp": 0.9767655686492289
+            "angularVelocity": -3.2592320489843035e-28,
+            "velocityX": -3.6408651267401044e-33,
+            "velocityY": 2.9208085569078165e-32,
+            "timestamp": 0.9767655686491771
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.1.traj
index f63af9ed..dd55da0b 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.1.traj	
@@ -4,190 +4,190 @@
             "x": 0.71,
             "y": 6.72,
             "heading": 1.04,
-            "angularVelocity": 1.9442039286246259e-31,
-            "velocityX": 1.5017390020853034e-32,
-            "velocityY": 2.127944688535994e-32,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.7269018921676205,
-            "y": 6.6955064720122275,
-            "heading": 1.0019343004430092,
-            "angularVelocity": -0.6189082678684702,
-            "velocityX": 0.2748070028110285,
-            "velocityY": -0.3982390224617833,
-            "timestamp": 0.06150459047524045
-        },
-        {
-            "x": 0.7606735160812093,
-            "y": 6.646281374376347,
-            "heading": 0.9275074281520226,
-            "angularVelocity": -1.2101027210472746,
-            "velocityX": 0.5490911109664911,
-            "velocityY": -0.8003483521396102,
-            "timestamp": 0.1230091809504809
-        },
-        {
-            "x": 0.8112864226090604,
-            "y": 6.571983368952451,
-            "heading": 0.8196845896852679,
-            "angularVelocity": -1.7530860320118746,
-            "velocityX": 0.8229126661403596,
-            "velocityY": -1.2080074812270452,
-            "timestamp": 0.18451377142572134
-        },
-        {
-            "x": 0.8787769249457482,
-            "y": 6.472119220055144,
-            "heading": 0.6843882308491542,
-            "angularVelocity": -2.199776598635823,
-            "velocityX": 1.0973246356929591,
-            "velocityY": -1.6236861041698774,
-            "timestamp": 0.2460183619009618
-        },
-        {
-            "x": 0.9632583450726888,
-            "y": 6.346050730942796,
-            "heading": 0.5331821377651369,
-            "angularVelocity": -2.458452156427049,
-            "velocityX": 1.373579101562342,
-            "velocityY": -2.0497411353888846,
-            "timestamp": 0.30752295237620225
-        },
-        {
-            "x": 1.064336906516991,
-            "y": 6.193349961542198,
-            "heading": 0.3933800445651471,
-            "angularVelocity": -2.273035103880077,
-            "velocityX": 1.6434311758403277,
-            "velocityY": -2.4827540224346163,
-            "timestamp": 0.3690275428514427
-        },
-        {
-            "x": 1.1776989215840556,
-            "y": 6.018028084645807,
-            "heading": 0.32650139843607295,
-            "angularVelocity": -1.0873764968160713,
-            "velocityX": 1.8431472218760627,
-            "velocityY": -2.8505494556047704,
-            "timestamp": 0.4305321333266831
-        },
-        {
-            "x": 1.3085726451154573,
-            "y": 5.825657208090679,
-            "heading": 0.3265013287499931,
-            "angularVelocity": -0.0000011330224187482564,
-            "velocityX": 2.1278691967567314,
-            "velocityY": -3.1277482716118437,
-            "timestamp": 0.49203672380192354
-        },
-        {
-            "x": 1.4394478126055321,
-            "y": 5.633287313894505,
-            "heading": 0.3265012590706379,
-            "angularVelocity": -0.000001132913083706052,
-            "velocityX": 2.1278926740071626,
-            "velocityY": -3.127732299487714,
-            "timestamp": 0.553541314277164
-        },
-        {
-            "x": 1.570322997049186,
-            "y": 5.440917431232361,
-            "heading": 0.3265011893913128,
-            "angularVelocity": -0.0000011329125928412157,
-            "velocityX": 2.1278929496545396,
-            "velocityY": -3.1277321119565196,
-            "timestamp": 0.6150459047524045
-        },
-        {
-            "x": 1.7011982004807327,
-            "y": 5.248547561488253,
-            "heading": 0.32650111971202045,
-            "angularVelocity": -0.0000011329120610367875,
-            "velocityX": 2.1278932583777177,
-            "velocityY": -3.12773190192281,
-            "timestamp": 0.676550495227645
-        },
-        {
-            "x": 1.8320716066458687,
-            "y": 5.056176469023738,
-            "heading": 0.3265010500238972,
-            "angularVelocity": -0.0000011330556420647947,
-            "velocityX": 2.1278640367148025,
-            "velocityY": -3.127751782071573,
-            "timestamp": 0.7380550857028855
-        },
-        {
-            "x": 1.9453996822937256,
-            "y": 4.881259566133339,
-            "heading": 0.2607759086629828,
-            "angularVelocity": -1.0686217216156084,
-            "velocityX": 1.8425954025899711,
-            "velocityY": -2.843965003893719,
-            "timestamp": 0.799559676178126
-        },
-        {
-            "x": 2.0464428806975716,
-            "y": 4.730058134054239,
-            "heading": 0.1310608851796909,
-            "angularVelocity": -2.109029951764503,
-            "velocityX": 1.6428562099689532,
-            "velocityY": -2.458376373385783,
-            "timestamp": 0.8610642666533664
-        },
-        {
-            "x": 2.1311236772079067,
-            "y": 4.60496312733231,
-            "heading": -0.022718365147026284,
-            "angularVelocity": -2.500288988813334,
-            "velocityX": 1.3768207520124016,
-            "velocityY": -2.0339133348475604,
-            "timestamp": 0.9225688571286069
-        },
-        {
-            "x": 2.1986971663058994,
-            "y": 4.506052833762169,
-            "heading": -0.16200277924985632,
-            "angularVelocity": -2.264618185839331,
-            "velocityX": 1.0986739132129535,
-            "velocityY": -1.6081774190490494,
-            "timestamp": 0.9840734476038474
-        },
-        {
-            "x": 2.2493424732278595,
-            "y": 4.432626695040595,
-            "heading": -0.27409854656488936,
-            "angularVelocity": -1.822559364250361,
-            "velocityX": 0.823439462495856,
-            "velocityY": -1.1938318449763972,
-            "timestamp": 1.0455780380790878
-        },
-        {
-            "x": 2.2831120594312195,
-            "y": 4.384102039370989,
-            "heading": -0.35252594149925776,
-            "angularVelocity": -1.2751470146921873,
-            "velocityX": 0.5490579799398,
-            "velocityY": -0.7889599019302513,
-            "timestamp": 1.1070826285543283
+            "x": 0.7269019070910594,
+            "y": 6.695506477136938,
+            "heading": 1.0019343442516249,
+            "angularVelocity": -0.6189075555866055,
+            "velocityX": 0.2748072454505401,
+            "velocityY": -0.3982389391394925,
+            "timestamp": 0.06150459047522247
+        },
+        {
+            "x": 0.7606735605009036,
+            "y": 6.646281390208111,
+            "heading": 0.9275075561119867,
+            "angularVelocity": -1.2101013528351434,
+            "velocityX": 0.5490915905447641,
+            "velocityY": -0.8003481780543988,
+            "timestamp": 0.12300918095044494
+        },
+        {
+            "x": 0.8112865108119746,
+            "y": 6.571983401826325,
+            "heading": 0.8196848361236891,
+            "angularVelocity": -1.7530841056771946,
+            "velocityX": 0.8229133780097379,
+            "velocityY": -1.2080072041406207,
+            "timestamp": 0.1845137714256674
+        },
+        {
+            "x": 0.8787770707149783,
+            "y": 6.472119277485466,
+            "heading": 0.6843886163577808,
+            "angularVelocity": -2.199774337501089,
+            "velocityX": 1.0973255716610744,
+            "velocityY": -1.6236857049083067,
+            "timestamp": 0.24601836190088988
+        },
+        {
+            "x": 0.9632585614164454,
+            "y": 6.3460508217508655,
+            "heading": 0.5331826539280757,
+            "angularVelocity": -2.4584500321259677,
+            "velocityX": 1.373580249030372,
+            "velocityY": -2.0497405927024026,
+            "timestamp": 0.3075229523761124
+        },
+        {
+            "x": 1.0643372089754837,
+            "y": 6.193350092274746,
+            "heading": 0.39338059647753487,
+            "angularVelocity": -2.273034522632274,
+            "velocityX": 1.6434325759759127,
+            "velocityY": -2.4827533733053033,
+            "timestamp": 0.3690275428513349
+        },
+        {
+            "x": 1.1776993188507112,
+            "y": 6.018028259717655,
+            "heading": 0.3265019863738018,
+            "angularVelocity": -1.087375911082207,
+            "velocityX": 1.843148763357691,
+            "velocityY": -2.85054873469518,
+            "timestamp": 0.4305321333265574
+        },
+        {
+            "x": 1.3085730997897271,
+            "y": 5.825657422218107,
+            "heading": 0.3265019166878518,
+            "angularVelocity": -0.0000011330203075824657,
+            "velocityX": 2.1278701301448275,
+            "velocityY": -3.1277476366100623,
+            "timestamp": 0.4920367238017799
+        },
+        {
+            "x": 1.4394483094166801,
+            "y": 5.633287556689,
+            "heading": 0.32650184700859936,
+            "angularVelocity": -0.00000113291141146818,
+            "velocityX": 2.1278933591091382,
+            "velocityY": -3.127731833392264,
+            "timestamp": 0.5535413142770024
+        },
+        {
+            "x": 1.5703235190623746,
+            "y": 5.4409176911726425,
+            "heading": 0.32650177732934843,
+            "angularVelocity": -0.0000011329113877746253,
+            "velocityX": 2.127893359413853,
+            "velocityY": -3.1277318331849595,
+            "timestamp": 0.6150459047522249
+        },
+        {
+            "x": 1.7011987286842072,
+            "y": 5.24854782564005,
+            "heading": 0.32650170765009895,
+            "angularVelocity": -0.0000011329113643360664,
+            "velocityX": 2.127893359025885,
+            "velocityY": -3.1277318334489093,
+            "timestamp": 0.6765504952274474
+        },
+        {
+            "x": 1.8320721192807423,
+            "y": 5.056176722584032,
+            "heading": 0.32650163796198384,
+            "angularVelocity": -0.000001133055510645955,
+            "velocityX": 2.1278637835863385,
+            "velocityY": -3.1277519542791743,
+            "timestamp": 0.7380550857026699
+        },
+        {
+            "x": 1.9454000953072241,
+            "y": 4.881259770298511,
+            "heading": 0.26077643177319376,
+            "angularVelocity": -1.0686227756490447,
+            "velocityX": 1.8425937828516514,
+            "velocityY": -2.843965807007333,
+            "timestamp": 0.7995596761778924
+        },
+        {
+            "x": 2.046443192812682,
+            "y": 4.7300582879948045,
+            "heading": 0.13106133632258532,
+            "angularVelocity": -2.1090311218780498,
+            "velocityX": 1.6428545694676804,
+            "velocityY": -2.4583771899858244,
+            "timestamp": 0.8610642666531149
+        },
+        {
+            "x": 2.131123897414369,
+            "y": 4.604963233646066,
+            "heading": -0.022718028385538325,
+            "angularVelocity": -2.50029084853552,
+            "velocityX": 1.376819257674782,
+            "velocityY": -2.0339141092100017,
+            "timestamp": 0.9225688571283374
+        },
+        {
+            "x": 2.198697305455373,
+            "y": 4.506052899677577,
+            "heading": -0.16200255999945734,
+            "angularVelocity": -2.264620096446796,
+            "velocityX": 1.0986725953118257,
+            "velocityY": -1.6081780758842066,
+            "timestamp": 0.9840734476035599
+        },
+        {
+            "x": 2.249342546290196,
+            "y": 4.432626729162034,
+            "heading": -0.2740984297313893,
+            "angularVelocity": -1.822561029442031,
+            "velocityX": 0.8234383879887088,
+            "velocityY": -1.1938323619132651,
+            "timestamp": 1.0455780380787822
+        },
+        {
+            "x": 2.283112085041758,
+            "y": 4.38410205128396,
+            "heading": -0.35252590091377134,
+            "angularVelocity": -1.2751482544051254,
+            "velocityX": 0.549057208423595,
+            "velocityY": -0.788960263016835,
+            "timestamp": 1.1070826285540047
         },
         {
             "x": 2.3,
             "y": 4.36,
             "heading": -0.393,
-            "angularVelocity": -0.6580656531163417,
-            "velocityX": 0.2745801644769678,
-            "velocityY": -0.3918738290061591,
-            "timestamp": 1.1685872190295687
+            "angularVelocity": -0.6580663129938888,
+            "velocityX": 0.2745797480766085,
+            "velocityY": -0.39187402269867344,
+            "timestamp": 1.1685872190292272
         },
         {
             "x": 2.3,
             "y": 4.36,
             "heading": -0.393,
-            "angularVelocity": -1.5278905371426354e-30,
-            "velocityX": 0,
-            "velocityY": -1.0222462264365078e-31,
-            "timestamp": 1.2300918095048092
+            "angularVelocity": 0,
+            "velocityX": 1.1335977805694889e-32,
+            "velocityY": 0,
+            "timestamp": 1.2300918095044497
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.traj
index f63af9ed..dd55da0b 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.traj	
@@ -4,190 +4,190 @@
             "x": 0.71,
             "y": 6.72,
             "heading": 1.04,
-            "angularVelocity": 1.9442039286246259e-31,
-            "velocityX": 1.5017390020853034e-32,
-            "velocityY": 2.127944688535994e-32,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.7269018921676205,
-            "y": 6.6955064720122275,
-            "heading": 1.0019343004430092,
-            "angularVelocity": -0.6189082678684702,
-            "velocityX": 0.2748070028110285,
-            "velocityY": -0.3982390224617833,
-            "timestamp": 0.06150459047524045
-        },
-        {
-            "x": 0.7606735160812093,
-            "y": 6.646281374376347,
-            "heading": 0.9275074281520226,
-            "angularVelocity": -1.2101027210472746,
-            "velocityX": 0.5490911109664911,
-            "velocityY": -0.8003483521396102,
-            "timestamp": 0.1230091809504809
-        },
-        {
-            "x": 0.8112864226090604,
-            "y": 6.571983368952451,
-            "heading": 0.8196845896852679,
-            "angularVelocity": -1.7530860320118746,
-            "velocityX": 0.8229126661403596,
-            "velocityY": -1.2080074812270452,
-            "timestamp": 0.18451377142572134
-        },
-        {
-            "x": 0.8787769249457482,
-            "y": 6.472119220055144,
-            "heading": 0.6843882308491542,
-            "angularVelocity": -2.199776598635823,
-            "velocityX": 1.0973246356929591,
-            "velocityY": -1.6236861041698774,
-            "timestamp": 0.2460183619009618
-        },
-        {
-            "x": 0.9632583450726888,
-            "y": 6.346050730942796,
-            "heading": 0.5331821377651369,
-            "angularVelocity": -2.458452156427049,
-            "velocityX": 1.373579101562342,
-            "velocityY": -2.0497411353888846,
-            "timestamp": 0.30752295237620225
-        },
-        {
-            "x": 1.064336906516991,
-            "y": 6.193349961542198,
-            "heading": 0.3933800445651471,
-            "angularVelocity": -2.273035103880077,
-            "velocityX": 1.6434311758403277,
-            "velocityY": -2.4827540224346163,
-            "timestamp": 0.3690275428514427
-        },
-        {
-            "x": 1.1776989215840556,
-            "y": 6.018028084645807,
-            "heading": 0.32650139843607295,
-            "angularVelocity": -1.0873764968160713,
-            "velocityX": 1.8431472218760627,
-            "velocityY": -2.8505494556047704,
-            "timestamp": 0.4305321333266831
-        },
-        {
-            "x": 1.3085726451154573,
-            "y": 5.825657208090679,
-            "heading": 0.3265013287499931,
-            "angularVelocity": -0.0000011330224187482564,
-            "velocityX": 2.1278691967567314,
-            "velocityY": -3.1277482716118437,
-            "timestamp": 0.49203672380192354
-        },
-        {
-            "x": 1.4394478126055321,
-            "y": 5.633287313894505,
-            "heading": 0.3265012590706379,
-            "angularVelocity": -0.000001132913083706052,
-            "velocityX": 2.1278926740071626,
-            "velocityY": -3.127732299487714,
-            "timestamp": 0.553541314277164
-        },
-        {
-            "x": 1.570322997049186,
-            "y": 5.440917431232361,
-            "heading": 0.3265011893913128,
-            "angularVelocity": -0.0000011329125928412157,
-            "velocityX": 2.1278929496545396,
-            "velocityY": -3.1277321119565196,
-            "timestamp": 0.6150459047524045
-        },
-        {
-            "x": 1.7011982004807327,
-            "y": 5.248547561488253,
-            "heading": 0.32650111971202045,
-            "angularVelocity": -0.0000011329120610367875,
-            "velocityX": 2.1278932583777177,
-            "velocityY": -3.12773190192281,
-            "timestamp": 0.676550495227645
-        },
-        {
-            "x": 1.8320716066458687,
-            "y": 5.056176469023738,
-            "heading": 0.3265010500238972,
-            "angularVelocity": -0.0000011330556420647947,
-            "velocityX": 2.1278640367148025,
-            "velocityY": -3.127751782071573,
-            "timestamp": 0.7380550857028855
-        },
-        {
-            "x": 1.9453996822937256,
-            "y": 4.881259566133339,
-            "heading": 0.2607759086629828,
-            "angularVelocity": -1.0686217216156084,
-            "velocityX": 1.8425954025899711,
-            "velocityY": -2.843965003893719,
-            "timestamp": 0.799559676178126
-        },
-        {
-            "x": 2.0464428806975716,
-            "y": 4.730058134054239,
-            "heading": 0.1310608851796909,
-            "angularVelocity": -2.109029951764503,
-            "velocityX": 1.6428562099689532,
-            "velocityY": -2.458376373385783,
-            "timestamp": 0.8610642666533664
-        },
-        {
-            "x": 2.1311236772079067,
-            "y": 4.60496312733231,
-            "heading": -0.022718365147026284,
-            "angularVelocity": -2.500288988813334,
-            "velocityX": 1.3768207520124016,
-            "velocityY": -2.0339133348475604,
-            "timestamp": 0.9225688571286069
-        },
-        {
-            "x": 2.1986971663058994,
-            "y": 4.506052833762169,
-            "heading": -0.16200277924985632,
-            "angularVelocity": -2.264618185839331,
-            "velocityX": 1.0986739132129535,
-            "velocityY": -1.6081774190490494,
-            "timestamp": 0.9840734476038474
-        },
-        {
-            "x": 2.2493424732278595,
-            "y": 4.432626695040595,
-            "heading": -0.27409854656488936,
-            "angularVelocity": -1.822559364250361,
-            "velocityX": 0.823439462495856,
-            "velocityY": -1.1938318449763972,
-            "timestamp": 1.0455780380790878
-        },
-        {
-            "x": 2.2831120594312195,
-            "y": 4.384102039370989,
-            "heading": -0.35252594149925776,
-            "angularVelocity": -1.2751470146921873,
-            "velocityX": 0.5490579799398,
-            "velocityY": -0.7889599019302513,
-            "timestamp": 1.1070826285543283
+            "x": 0.7269019070910594,
+            "y": 6.695506477136938,
+            "heading": 1.0019343442516249,
+            "angularVelocity": -0.6189075555866055,
+            "velocityX": 0.2748072454505401,
+            "velocityY": -0.3982389391394925,
+            "timestamp": 0.06150459047522247
+        },
+        {
+            "x": 0.7606735605009036,
+            "y": 6.646281390208111,
+            "heading": 0.9275075561119867,
+            "angularVelocity": -1.2101013528351434,
+            "velocityX": 0.5490915905447641,
+            "velocityY": -0.8003481780543988,
+            "timestamp": 0.12300918095044494
+        },
+        {
+            "x": 0.8112865108119746,
+            "y": 6.571983401826325,
+            "heading": 0.8196848361236891,
+            "angularVelocity": -1.7530841056771946,
+            "velocityX": 0.8229133780097379,
+            "velocityY": -1.2080072041406207,
+            "timestamp": 0.1845137714256674
+        },
+        {
+            "x": 0.8787770707149783,
+            "y": 6.472119277485466,
+            "heading": 0.6843886163577808,
+            "angularVelocity": -2.199774337501089,
+            "velocityX": 1.0973255716610744,
+            "velocityY": -1.6236857049083067,
+            "timestamp": 0.24601836190088988
+        },
+        {
+            "x": 0.9632585614164454,
+            "y": 6.3460508217508655,
+            "heading": 0.5331826539280757,
+            "angularVelocity": -2.4584500321259677,
+            "velocityX": 1.373580249030372,
+            "velocityY": -2.0497405927024026,
+            "timestamp": 0.3075229523761124
+        },
+        {
+            "x": 1.0643372089754837,
+            "y": 6.193350092274746,
+            "heading": 0.39338059647753487,
+            "angularVelocity": -2.273034522632274,
+            "velocityX": 1.6434325759759127,
+            "velocityY": -2.4827533733053033,
+            "timestamp": 0.3690275428513349
+        },
+        {
+            "x": 1.1776993188507112,
+            "y": 6.018028259717655,
+            "heading": 0.3265019863738018,
+            "angularVelocity": -1.087375911082207,
+            "velocityX": 1.843148763357691,
+            "velocityY": -2.85054873469518,
+            "timestamp": 0.4305321333265574
+        },
+        {
+            "x": 1.3085730997897271,
+            "y": 5.825657422218107,
+            "heading": 0.3265019166878518,
+            "angularVelocity": -0.0000011330203075824657,
+            "velocityX": 2.1278701301448275,
+            "velocityY": -3.1277476366100623,
+            "timestamp": 0.4920367238017799
+        },
+        {
+            "x": 1.4394483094166801,
+            "y": 5.633287556689,
+            "heading": 0.32650184700859936,
+            "angularVelocity": -0.00000113291141146818,
+            "velocityX": 2.1278933591091382,
+            "velocityY": -3.127731833392264,
+            "timestamp": 0.5535413142770024
+        },
+        {
+            "x": 1.5703235190623746,
+            "y": 5.4409176911726425,
+            "heading": 0.32650177732934843,
+            "angularVelocity": -0.0000011329113877746253,
+            "velocityX": 2.127893359413853,
+            "velocityY": -3.1277318331849595,
+            "timestamp": 0.6150459047522249
+        },
+        {
+            "x": 1.7011987286842072,
+            "y": 5.24854782564005,
+            "heading": 0.32650170765009895,
+            "angularVelocity": -0.0000011329113643360664,
+            "velocityX": 2.127893359025885,
+            "velocityY": -3.1277318334489093,
+            "timestamp": 0.6765504952274474
+        },
+        {
+            "x": 1.8320721192807423,
+            "y": 5.056176722584032,
+            "heading": 0.32650163796198384,
+            "angularVelocity": -0.000001133055510645955,
+            "velocityX": 2.1278637835863385,
+            "velocityY": -3.1277519542791743,
+            "timestamp": 0.7380550857026699
+        },
+        {
+            "x": 1.9454000953072241,
+            "y": 4.881259770298511,
+            "heading": 0.26077643177319376,
+            "angularVelocity": -1.0686227756490447,
+            "velocityX": 1.8425937828516514,
+            "velocityY": -2.843965807007333,
+            "timestamp": 0.7995596761778924
+        },
+        {
+            "x": 2.046443192812682,
+            "y": 4.7300582879948045,
+            "heading": 0.13106133632258532,
+            "angularVelocity": -2.1090311218780498,
+            "velocityX": 1.6428545694676804,
+            "velocityY": -2.4583771899858244,
+            "timestamp": 0.8610642666531149
+        },
+        {
+            "x": 2.131123897414369,
+            "y": 4.604963233646066,
+            "heading": -0.022718028385538325,
+            "angularVelocity": -2.50029084853552,
+            "velocityX": 1.376819257674782,
+            "velocityY": -2.0339141092100017,
+            "timestamp": 0.9225688571283374
+        },
+        {
+            "x": 2.198697305455373,
+            "y": 4.506052899677577,
+            "heading": -0.16200255999945734,
+            "angularVelocity": -2.264620096446796,
+            "velocityX": 1.0986725953118257,
+            "velocityY": -1.6081780758842066,
+            "timestamp": 0.9840734476035599
+        },
+        {
+            "x": 2.249342546290196,
+            "y": 4.432626729162034,
+            "heading": -0.2740984297313893,
+            "angularVelocity": -1.822561029442031,
+            "velocityX": 0.8234383879887088,
+            "velocityY": -1.1938323619132651,
+            "timestamp": 1.0455780380787822
+        },
+        {
+            "x": 2.283112085041758,
+            "y": 4.38410205128396,
+            "heading": -0.35252590091377134,
+            "angularVelocity": -1.2751482544051254,
+            "velocityX": 0.549057208423595,
+            "velocityY": -0.788960263016835,
+            "timestamp": 1.1070826285540047
         },
         {
             "x": 2.3,
             "y": 4.36,
             "heading": -0.393,
-            "angularVelocity": -0.6580656531163417,
-            "velocityX": 0.2745801644769678,
-            "velocityY": -0.3918738290061591,
-            "timestamp": 1.1685872190295687
+            "angularVelocity": -0.6580663129938888,
+            "velocityX": 0.2745797480766085,
+            "velocityY": -0.39187402269867344,
+            "timestamp": 1.1685872190292272
         },
         {
             "x": 2.3,
             "y": 4.36,
             "heading": -0.393,
-            "angularVelocity": -1.5278905371426354e-30,
-            "velocityX": 0,
-            "velocityY": -1.0222462264365078e-31,
-            "timestamp": 1.2300918095048092
+            "angularVelocity": 0,
+            "velocityX": 1.1335977805694889e-32,
+            "velocityY": 0,
+            "timestamp": 1.2300918095044497
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.1.traj
new file mode 100644
index 00000000..96b5b24b
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.1.traj	
@@ -0,0 +1,274 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": -1.8418609272007183e-28,
+            "velocityX": -1.1316944174618986e-32,
+            "velocityY": -1.0903166764521522e-29,
+            "timestamp": 0
+        },
+        {
+            "x": 7.615058283165484,
+            "y": 7.4500526040017725,
+            "heading": -0.011528934363875379,
+            "angularVelocity": -0.12842736126317786,
+            "velocityX": -0.7234227436435784,
+            "velocityY": -0.18878668917675853,
+            "timestamp": 0.08977007898235294
+        },
+        {
+            "x": 7.485153650546927,
+            "y": 7.41615410854214,
+            "heading": -0.0328638632229319,
+            "angularVelocity": -0.23766191476385995,
+            "velocityX": -1.4470816344507456,
+            "velocityY": -0.3776146333375848,
+            "timestamp": 0.17954015796470588
+        },
+        {
+            "x": 7.290257872847477,
+            "y": 7.365300387326372,
+            "heading": -0.06109799257845879,
+            "angularVelocity": -0.3145160355888266,
+            "velocityX": -2.17105498746146,
+            "velocityY": -0.5664885426438214,
+            "timestamp": 0.2693102369470588
+        },
+        {
+            "x": 7.030342832843462,
+            "y": 7.297491054945299,
+            "heading": -0.0902650461777,
+            "angularVelocity": -0.32490840968750134,
+            "velocityX": -2.895341554228826,
+            "velocityY": -0.7553667452421856,
+            "timestamp": 0.35908031592941175
+        },
+        {
+            "x": 6.705594231461252,
+            "y": 7.212800171241254,
+            "heading": -0.10097705422378679,
+            "angularVelocity": -0.11932715407194204,
+            "velocityX": -3.617559492690666,
+            "velocityY": -0.9434199531077018,
+            "timestamp": 0.4488503949117647
+        },
+        {
+            "x": 6.377014064882571,
+            "y": 7.1270111914036525,
+            "heading": -0.10097706428057987,
+            "angularVelocity": -1.120283753063034e-7,
+            "velocityX": -3.6602414780458985,
+            "velocityY": -0.9556522708907337,
+            "timestamp": 0.5386204738941176
+        },
+        {
+            "x": 6.048433901032778,
+            "y": 7.0412222011141505,
+            "heading": -0.10097707433737516,
+            "angularVelocity": -1.1202840770590317e-7,
+            "velocityX": -3.660241447647229,
+            "velocityY": -0.9556523873204973,
+            "timestamp": 0.6283905528764705
+        },
+        {
+            "x": 5.719853740215168,
+            "y": 6.955433199211206,
+            "heading": -0.10097708439415136,
+            "angularVelocity": -1.1202819821797078e-7,
+            "velocityX": -3.6602414138700015,
+            "velocityY": -0.9556525166893554,
+            "timestamp": 0.7181606318588234
+        },
+        {
+            "x": 5.391273582926602,
+            "y": 6.86964418379178,
+            "heading": -0.100977094450907,
+            "angularVelocity": -1.1202797265919916e-7,
+            "velocityX": -3.660241374557919,
+            "velocityY": -0.9556526672572584,
+            "timestamp": 0.8079307108411763
+        },
+        {
+            "x": 5.062693429765362,
+            "y": 6.783855152564382,
+            "heading": -0.10097710450764096,
+            "angularVelocity": -1.1202773594839044e-7,
+            "velocityX": -3.6602413285812334,
+            "velocityY": -0.9556528433514181,
+            "timestamp": 0.8977007898235292
+        },
+        {
+            "x": 4.734113281444647,
+            "y": 6.698066102797359,
+            "heading": -0.10097711456435234,
+            "angularVelocity": -1.1202748993058552e-7,
+            "velocityX": -3.6602412746598207,
+            "velocityY": -0.9556530498750426,
+            "timestamp": 0.9874708688058821
+        },
+        {
+            "x": 4.4055331387963745,
+            "y": 6.612277031304317,
+            "heading": -0.10097712462104143,
+            "angularVelocity": -1.1202724880866421e-7,
+            "velocityX": -3.660241211471189,
+            "velocityY": -0.9556532918937597,
+            "timestamp": 1.077240947788235
+        },
+        {
+            "x": 4.076953002755506,
+            "y": 6.526487934504167,
+            "heading": -0.10097713467770983,
+            "angularVelocity": -1.1202702659530083e-7,
+            "velocityX": -3.6602411378675055,
+            "velocityY": -0.9556535738043043,
+            "timestamp": 1.167011026770588
+        },
+        {
+            "x": 3.748372874246301,
+            "y": 6.440698808855463,
+            "heading": -0.10097714473442117,
+            "angularVelocity": -1.1202751392033635e-7,
+            "velocityX": -3.6602410539684915,
+            "velocityY": -0.9556538951629009,
+            "timestamp": 1.256781105752941
+        },
+        {
+            "x": 3.4197928479820465,
+            "y": 6.354909291415878,
+            "heading": -0.10097715479114391,
+            "angularVelocity": -1.120276421337113e-7,
+            "velocityX": -3.6602399150000573,
+            "velocityY": -0.9556582595606669,
+            "timestamp": 1.346551184735294
+        },
+        {
+            "x": 3.1047619528127512,
+            "y": 6.228101525660344,
+            "heading": -0.10097716526412484,
+            "angularVelocity": -1.1666457370126234e-7,
+            "velocityX": -3.5093084326123236,
+            "velocityY": -1.412583871971558,
+            "timestamp": 1.436321263717647
+        },
+        {
+            "x": 2.8208656132222933,
+            "y": 6.041748259045348,
+            "heading": -0.1009771779112871,
+            "angularVelocity": -1.4088397962304153e-7,
+            "velocityX": -3.1624828986324123,
+            "velocityY": -2.0758950947495576,
+            "timestamp": 1.5260913427
+        },
+        {
+            "x": 2.5792187192315943,
+            "y": 5.803145197833315,
+            "heading": -0.10097728674036179,
+            "angularVelocity": -0.0000012123108078932896,
+            "velocityX": -2.691842278966871,
+            "velocityY": -2.65793529332427,
+            "timestamp": 1.615861421682353
+        },
+        {
+            "x": 2.3963290188287294,
+            "y": 5.533479989641919,
+            "heading": -0.13644335482170092,
+            "angularVelocity": -0.3950767169047361,
+            "velocityX": -2.037312459521214,
+            "velocityY": -3.003954226707092,
+            "timestamp": 1.705631500664706
+        },
+        {
+            "x": 2.279822826385498,
+            "y": 5.254662990570068,
+            "heading": -0.18962986506951118,
+            "angularVelocity": -0.5924748073105779,
+            "velocityX": -1.2978287839789375,
+            "velocityY": -3.105901233821009,
+            "timestamp": 1.795401579647059
+        },
+        {
+            "x": 2.2328990354823257,
+            "y": 5.0773593431323425,
+            "heading": -0.225508532252886,
+            "angularVelocity": -0.6284306472549296,
+            "velocityX": -0.8218908506157174,
+            "velocityY": -3.1055514229095325,
+            "timestamp": 1.8524940644563022
+        },
+        {
+            "x": 2.2122650091837905,
+            "y": 4.906948206481987,
+            "heading": -0.2618449581428327,
+            "angularVelocity": -0.636448492486558,
+            "velocityX": -0.3614140524419242,
+            "velocityY": -2.9848260628208765,
+            "timestamp": 1.9095865492655453
+        },
+        {
+            "x": 2.2138710973194917,
+            "y": 4.752151212947765,
+            "heading": -0.29639527524079207,
+            "angularVelocity": -0.6051640108688915,
+            "velocityX": 0.02813134059521416,
+            "velocityY": -2.7113374737719043,
+            "timestamp": 1.9666790340747884
+        },
+        {
+            "x": 2.229883400142308,
+            "y": 4.620372877512837,
+            "heading": -0.32691110500805654,
+            "angularVelocity": -0.5344981895462751,
+            "velocityX": 0.280462531565201,
+            "velocityY": -2.308155545781648,
+            "timestamp": 2.0237715188840317
+        },
+        {
+            "x": 2.251883940293385,
+            "y": 4.51506115985361,
+            "heading": -0.3519685302004407,
+            "angularVelocity": -0.4388918309689961,
+            "velocityX": 0.38534914402167997,
+            "velocityY": -1.844581086479138,
+            "timestamp": 2.080864003693275
+        },
+        {
+            "x": 2.273515445313811,
+            "y": 4.436872238113893,
+            "heading": -0.37095335253129014,
+            "angularVelocity": -0.33252751906728534,
+            "velocityX": 0.3788853312819565,
+            "velocityY": -1.3695133781784317,
+            "timestamp": 2.1379564885025184
+        },
+        {
+            "x": 2.2905088224909966,
+            "y": 4.385405157743103,
+            "heading": -0.3836505931421081,
+            "angularVelocity": -0.22239775783770355,
+            "velocityX": 0.2976464806908316,
+            "velocityY": -0.9014685653065743,
+            "timestamp": 2.1950489733117617
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.11121265573212626,
+            "velocityX": 0.16624215149881386,
+            "velocityY": -0.4449825196428863,
+            "timestamp": 2.252141458121005
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -5.463060715454085e-29,
+            "velocityX": 0,
+            "velocityY": -5.448481045128469e-30,
+            "timestamp": 2.3092339429302484
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.traj
new file mode 100644
index 00000000..96b5b24b
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.traj	
@@ -0,0 +1,274 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": -1.8418609272007183e-28,
+            "velocityX": -1.1316944174618986e-32,
+            "velocityY": -1.0903166764521522e-29,
+            "timestamp": 0
+        },
+        {
+            "x": 7.615058283165484,
+            "y": 7.4500526040017725,
+            "heading": -0.011528934363875379,
+            "angularVelocity": -0.12842736126317786,
+            "velocityX": -0.7234227436435784,
+            "velocityY": -0.18878668917675853,
+            "timestamp": 0.08977007898235294
+        },
+        {
+            "x": 7.485153650546927,
+            "y": 7.41615410854214,
+            "heading": -0.0328638632229319,
+            "angularVelocity": -0.23766191476385995,
+            "velocityX": -1.4470816344507456,
+            "velocityY": -0.3776146333375848,
+            "timestamp": 0.17954015796470588
+        },
+        {
+            "x": 7.290257872847477,
+            "y": 7.365300387326372,
+            "heading": -0.06109799257845879,
+            "angularVelocity": -0.3145160355888266,
+            "velocityX": -2.17105498746146,
+            "velocityY": -0.5664885426438214,
+            "timestamp": 0.2693102369470588
+        },
+        {
+            "x": 7.030342832843462,
+            "y": 7.297491054945299,
+            "heading": -0.0902650461777,
+            "angularVelocity": -0.32490840968750134,
+            "velocityX": -2.895341554228826,
+            "velocityY": -0.7553667452421856,
+            "timestamp": 0.35908031592941175
+        },
+        {
+            "x": 6.705594231461252,
+            "y": 7.212800171241254,
+            "heading": -0.10097705422378679,
+            "angularVelocity": -0.11932715407194204,
+            "velocityX": -3.617559492690666,
+            "velocityY": -0.9434199531077018,
+            "timestamp": 0.4488503949117647
+        },
+        {
+            "x": 6.377014064882571,
+            "y": 7.1270111914036525,
+            "heading": -0.10097706428057987,
+            "angularVelocity": -1.120283753063034e-7,
+            "velocityX": -3.6602414780458985,
+            "velocityY": -0.9556522708907337,
+            "timestamp": 0.5386204738941176
+        },
+        {
+            "x": 6.048433901032778,
+            "y": 7.0412222011141505,
+            "heading": -0.10097707433737516,
+            "angularVelocity": -1.1202840770590317e-7,
+            "velocityX": -3.660241447647229,
+            "velocityY": -0.9556523873204973,
+            "timestamp": 0.6283905528764705
+        },
+        {
+            "x": 5.719853740215168,
+            "y": 6.955433199211206,
+            "heading": -0.10097708439415136,
+            "angularVelocity": -1.1202819821797078e-7,
+            "velocityX": -3.6602414138700015,
+            "velocityY": -0.9556525166893554,
+            "timestamp": 0.7181606318588234
+        },
+        {
+            "x": 5.391273582926602,
+            "y": 6.86964418379178,
+            "heading": -0.100977094450907,
+            "angularVelocity": -1.1202797265919916e-7,
+            "velocityX": -3.660241374557919,
+            "velocityY": -0.9556526672572584,
+            "timestamp": 0.8079307108411763
+        },
+        {
+            "x": 5.062693429765362,
+            "y": 6.783855152564382,
+            "heading": -0.10097710450764096,
+            "angularVelocity": -1.1202773594839044e-7,
+            "velocityX": -3.6602413285812334,
+            "velocityY": -0.9556528433514181,
+            "timestamp": 0.8977007898235292
+        },
+        {
+            "x": 4.734113281444647,
+            "y": 6.698066102797359,
+            "heading": -0.10097711456435234,
+            "angularVelocity": -1.1202748993058552e-7,
+            "velocityX": -3.6602412746598207,
+            "velocityY": -0.9556530498750426,
+            "timestamp": 0.9874708688058821
+        },
+        {
+            "x": 4.4055331387963745,
+            "y": 6.612277031304317,
+            "heading": -0.10097712462104143,
+            "angularVelocity": -1.1202724880866421e-7,
+            "velocityX": -3.660241211471189,
+            "velocityY": -0.9556532918937597,
+            "timestamp": 1.077240947788235
+        },
+        {
+            "x": 4.076953002755506,
+            "y": 6.526487934504167,
+            "heading": -0.10097713467770983,
+            "angularVelocity": -1.1202702659530083e-7,
+            "velocityX": -3.6602411378675055,
+            "velocityY": -0.9556535738043043,
+            "timestamp": 1.167011026770588
+        },
+        {
+            "x": 3.748372874246301,
+            "y": 6.440698808855463,
+            "heading": -0.10097714473442117,
+            "angularVelocity": -1.1202751392033635e-7,
+            "velocityX": -3.6602410539684915,
+            "velocityY": -0.9556538951629009,
+            "timestamp": 1.256781105752941
+        },
+        {
+            "x": 3.4197928479820465,
+            "y": 6.354909291415878,
+            "heading": -0.10097715479114391,
+            "angularVelocity": -1.120276421337113e-7,
+            "velocityX": -3.6602399150000573,
+            "velocityY": -0.9556582595606669,
+            "timestamp": 1.346551184735294
+        },
+        {
+            "x": 3.1047619528127512,
+            "y": 6.228101525660344,
+            "heading": -0.10097716526412484,
+            "angularVelocity": -1.1666457370126234e-7,
+            "velocityX": -3.5093084326123236,
+            "velocityY": -1.412583871971558,
+            "timestamp": 1.436321263717647
+        },
+        {
+            "x": 2.8208656132222933,
+            "y": 6.041748259045348,
+            "heading": -0.1009771779112871,
+            "angularVelocity": -1.4088397962304153e-7,
+            "velocityX": -3.1624828986324123,
+            "velocityY": -2.0758950947495576,
+            "timestamp": 1.5260913427
+        },
+        {
+            "x": 2.5792187192315943,
+            "y": 5.803145197833315,
+            "heading": -0.10097728674036179,
+            "angularVelocity": -0.0000012123108078932896,
+            "velocityX": -2.691842278966871,
+            "velocityY": -2.65793529332427,
+            "timestamp": 1.615861421682353
+        },
+        {
+            "x": 2.3963290188287294,
+            "y": 5.533479989641919,
+            "heading": -0.13644335482170092,
+            "angularVelocity": -0.3950767169047361,
+            "velocityX": -2.037312459521214,
+            "velocityY": -3.003954226707092,
+            "timestamp": 1.705631500664706
+        },
+        {
+            "x": 2.279822826385498,
+            "y": 5.254662990570068,
+            "heading": -0.18962986506951118,
+            "angularVelocity": -0.5924748073105779,
+            "velocityX": -1.2978287839789375,
+            "velocityY": -3.105901233821009,
+            "timestamp": 1.795401579647059
+        },
+        {
+            "x": 2.2328990354823257,
+            "y": 5.0773593431323425,
+            "heading": -0.225508532252886,
+            "angularVelocity": -0.6284306472549296,
+            "velocityX": -0.8218908506157174,
+            "velocityY": -3.1055514229095325,
+            "timestamp": 1.8524940644563022
+        },
+        {
+            "x": 2.2122650091837905,
+            "y": 4.906948206481987,
+            "heading": -0.2618449581428327,
+            "angularVelocity": -0.636448492486558,
+            "velocityX": -0.3614140524419242,
+            "velocityY": -2.9848260628208765,
+            "timestamp": 1.9095865492655453
+        },
+        {
+            "x": 2.2138710973194917,
+            "y": 4.752151212947765,
+            "heading": -0.29639527524079207,
+            "angularVelocity": -0.6051640108688915,
+            "velocityX": 0.02813134059521416,
+            "velocityY": -2.7113374737719043,
+            "timestamp": 1.9666790340747884
+        },
+        {
+            "x": 2.229883400142308,
+            "y": 4.620372877512837,
+            "heading": -0.32691110500805654,
+            "angularVelocity": -0.5344981895462751,
+            "velocityX": 0.280462531565201,
+            "velocityY": -2.308155545781648,
+            "timestamp": 2.0237715188840317
+        },
+        {
+            "x": 2.251883940293385,
+            "y": 4.51506115985361,
+            "heading": -0.3519685302004407,
+            "angularVelocity": -0.4388918309689961,
+            "velocityX": 0.38534914402167997,
+            "velocityY": -1.844581086479138,
+            "timestamp": 2.080864003693275
+        },
+        {
+            "x": 2.273515445313811,
+            "y": 4.436872238113893,
+            "heading": -0.37095335253129014,
+            "angularVelocity": -0.33252751906728534,
+            "velocityX": 0.3788853312819565,
+            "velocityY": -1.3695133781784317,
+            "timestamp": 2.1379564885025184
+        },
+        {
+            "x": 2.2905088224909966,
+            "y": 4.385405157743103,
+            "heading": -0.3836505931421081,
+            "angularVelocity": -0.22239775783770355,
+            "velocityX": 0.2976464806908316,
+            "velocityY": -0.9014685653065743,
+            "timestamp": 2.1950489733117617
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.11121265573212626,
+            "velocityX": 0.16624215149881386,
+            "velocityY": -0.4449825196428863,
+            "timestamp": 2.252141458121005
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -5.463060715454085e-29,
+            "velocityX": 0,
+            "velocityY": -5.448481045128469e-30,
+            "timestamp": 2.3092339429302484
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.1.traj
index 98bf1cf8..1fe0cdb8 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.1.traj	
@@ -4,217 +4,217 @@
             "x": 1.35,
             "y": 5.56,
             "heading": 0,
-            "angularVelocity": -1.7848043602710886e-30,
-            "velocityX": 4.033392918765628e-32,
-            "velocityY": -2.9619524123271102e-31,
+            "angularVelocity": 0,
+            "velocityX": -2.1086675817598662e-32,
+            "velocityY": -7.052966104933725e-38,
             "timestamp": 0
         },
         {
-            "x": 1.4233949207458116,
-            "y": 5.582111402652435,
-            "heading": -4.931478664026919e-9,
-            "angularVelocity": -5.143711867197603e-8,
-            "velocityX": 0.7654820738778157,
-            "velocityY": 0.2306138106917608,
-            "timestamp": 0.09588065253314748
-        },
-        {
-            "x": 1.570184760818058,
-            "y": 5.626334201974914,
-            "heading": -1.1368240805751946e-8,
-            "angularVelocity": -6.714026800936027e-8,
-            "velocityX": 1.530964132952106,
-            "velocityY": 0.4612275589893128,
-            "timestamp": 0.19176130506629496
-        },
-        {
-            "x": 1.7903695159222153,
-            "y": 5.69266839083479,
-            "heading": -1.6115736339739044e-8,
-            "angularVelocity": -4.952541704566216e-8,
-            "velocityX": 2.2964461472361695,
-            "velocityY": 0.6918412328958446,
-            "timestamp": 0.28764195759944244
-        },
-        {
-            "x": 2.0839491700576396,
-            "y": 5.781113958180788,
-            "heading": -1.639772094621516e-8,
-            "angularVelocity": -2.9553274583871895e-9,
-            "velocityX": 3.0619279946395954,
-            "velocityY": 0.9224547915413995,
-            "timestamp": 0.3835226101325899
-        },
-        {
-            "x": 2.431241903149783,
-            "y": 5.885741340897451,
-            "heading": -1.6397499367976412e-8,
-            "angularVelocity": 2.3168753954621105e-12,
-            "velocityX": 3.6221356855297673,
-            "velocityY": 1.0912251841409724,
-            "timestamp": 0.4794032626657374
-        },
-        {
-            "x": 2.7785346566980107,
-            "y": 5.9903686557177505,
-            "heading": -1.6397564221401302e-8,
-            "angularVelocity": -6.713496881725287e-13,
-            "velocityX": 3.622135898879975,
-            "velocityY": 1.091224476005724,
-            "timestamp": 0.5752839151988849
-        },
-        {
-            "x": 3.125827432121608,
-            "y": 6.094995897925851,
-            "heading": -1.6397647407961563e-8,
-            "angularVelocity": -8.684642063246534e-13,
-            "velocityX": 3.6221361270322743,
-            "velocityY": 1.0912237186873281,
-            "timestamp": 0.6711645677320324
-        },
-        {
-            "x": 3.4731202302782154,
-            "y": 6.1996230646748005,
-            "heading": -1.6397748264247437e-8,
-            "angularVelocity": -1.0543921850153982e-12,
-            "velocityX": 3.6221363641294033,
-            "velocityY": 1.0912229316763022,
-            "timestamp": 0.7670452202651799
-        },
-        {
-            "x": 3.8204130521112987,
-            "y": 6.304250152832848,
-            "heading": -1.639786614425739e-8,
-            "angularVelocity": -1.2203887930777277e-12,
-            "velocityX": 3.622136611066492,
-            "velocityY": 1.091222112002269,
-            "timestamp": 0.8629258727983273
-        },
-        {
-            "x": 4.167705899087819,
-            "y": 6.408877157530555,
-            "heading": -1.6398004207896958e-8,
-            "angularVelocity": -1.408074947723948e-12,
-            "velocityX": 3.6221368733034063,
-            "velocityY": 1.0912212415417761,
-            "timestamp": 0.9588065253314748
-        },
-        {
-            "x": 4.514998774010408,
-            "y": 6.51350406946466,
-            "heading": -1.6398168919050745e-8,
-            "angularVelocity": -1.6668913936591836e-12,
-            "velocityX": 3.6221371647706677,
-            "velocityY": 1.091220274051783,
-            "timestamp": 1.0546871778646223
-        },
-        {
-            "x": 4.862291683961819,
-            "y": 6.618130865121593,
-            "heading": -1.6398370762014746e-8,
-            "angularVelocity": -2.0513887673195245e-12,
-            "velocityX": 3.6221375301080334,
-            "velocityY": 1.0912190613250203,
-            "timestamp": 1.1505678303977698
-        },
-        {
-            "x": 5.20958464417323,
-            "y": 6.722757493946074,
-            "heading": -1.639835180923073e-8,
-            "angularVelocity": 1.902358989315915e-13,
-            "velocityX": 3.6221380542985027,
-            "velocityY": 1.091217321333091,
-            "timestamp": 1.2464484829309173
-        },
-        {
-            "x": 5.556877646148189,
-            "y": 6.8273839841549115,
-            "heading": -1.6398206125528607e-8,
-            "angularVelocity": 1.4352640935347951e-12,
-            "velocityX": 3.62213848987558,
-            "velocityY": 1.09121587562664,
-            "timestamp": 1.3423291354640647
-        },
-        {
-            "x": 5.904170680592406,
-            "y": 6.932010366591235,
-            "heading": -1.6398102572596455e-8,
-            "angularVelocity": 9.899977906647045e-13,
-            "velocityX": 3.6221388285174303,
-            "velocityY": 1.091214751599812,
-            "timestamp": 1.4382097879972122
-        },
-        {
-            "x": 6.2514637423704675,
-            "y": 7.036636658299249,
-            "heading": -1.6398041778119566e-8,
-            "angularVelocity": 5.750349690087699e-13,
-            "velocityX": 3.6221391135989474,
-            "velocityY": 1.0912138053372793,
-            "timestamp": 1.5340904405303597
-        },
-        {
-            "x": 6.5987568262419405,
-            "y": 7.141262876671409,
-            "heading": -1.639800860171266e-8,
-            "angularVelocity": 3.175769618903374e-13,
-            "velocityX": 3.6221393440248217,
-            "velocityY": 1.0912130404711837,
-            "timestamp": 1.6299710930635072
-        },
-        {
-            "x": 6.946049927457118,
-            "y": 7.2458890374680776,
-            "heading": -1.6398270316090924e-8,
-            "angularVelocity": -2.740394345840068e-12,
-            "velocityX": 3.6221395249132713,
-            "velocityY": 1.0912124399786907,
-            "timestamp": 1.7258517455966547
-        },
-        {
-            "x": 7.239629937032664,
-            "y": 7.334333424990155,
-            "heading": -1.4134914023195805e-8,
-            "angularVelocity": 2.36203418937418e-8,
-            "velocityX": 3.061931701744924,
-            "velocityY": 0.9224424864262022,
-            "timestamp": 1.8217323981298021
-        },
-        {
-            "x": 7.45981496297016,
-            "y": 7.4006667148640615,
-            "heading": -8.953329207671134e-9,
-            "angularVelocity": 5.4052849582725206e-8,
-            "velocityX": 2.2964489719251726,
-            "velocityY": 0.6918318568130147,
-            "timestamp": 1.9176130506629496
-        },
-        {
-            "x": 7.606604986310546,
-            "y": 7.444888905858524,
-            "heading": -3.5476510780655253e-9,
-            "angularVelocity": 5.638647309242438e-8,
-            "velocityX": 1.5309660443693864,
-            "velocityY": 0.4612212143584205,
-            "timestamp": 2.0134937031960973
+            "x": 1.4233949672121562,
+            "y": 5.582111248416047,
+            "heading": 6.989955400155978e-19,
+            "angularVelocity": 7.290266672073875e-18,
+            "velocityX": 0.7654825584964474,
+            "velocityY": 0.23061220206204183,
+            "timestamp": 0.09588065253415788
+        },
+        {
+            "x": 1.5701848986828006,
+            "y": 5.626333744358309,
+            "heading": -5.64955528358279e-19,
+            "angularVelocity": -1.3182545571253806e-17,
+            "velocityX": 1.530965086187225,
+            "velocityY": 0.4612243948434498,
+            "timestamp": 0.19176130506831576
+        },
+        {
+            "x": 1.7903697885046006,
+            "y": 5.69266748604712,
+            "heading": -1.1328157855510893e-18,
+            "angularVelocity": -5.922574643212496e-18,
+            "velocityX": 2.2964475522666925,
+            "velocityY": 0.6918365690635988,
+            "timestamp": 0.28764195760247363
+        },
+        {
+            "x": 2.08394961895559,
+            "y": 5.781112468143492,
+            "heading": -1.2347885191323161e-17,
+            "angularVelocity": -1.1696905584501664e-16,
+            "velocityX": 3.061929833512548,
+            "velocityY": 0.9224486876000673,
+            "timestamp": 0.3835226101366315
+        },
+        {
+            "x": 2.4312425305333245,
+            "y": 5.885739258408696,
+            "heading": -1.8571615119787707e-17,
+            "angularVelocity": -6.491121789400923e-17,
+            "velocityX": 3.6221375470302584,
+            "velocityY": 1.0912190050847865,
+            "timestamp": 0.4794032626707894
+        },
+        {
+            "x": 2.7785354421111035,
+            "y": 5.990366048673914,
+            "heading": -2.5105020741934312e-17,
+            "angularVelocity": -6.814102166319416e-17,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849244,
+            "timestamp": 0.5752839152049473
+        },
+        {
+            "x": 3.125828353688883,
+            "y": 6.094992838939131,
+            "heading": -3.974939977960392e-17,
+            "angularVelocity": -1.527354961675144e-16,
+            "velocityX": 3.622137547030716,
+            "velocityY": 1.0912190050849244,
+            "timestamp": 0.6711645677391052
+        },
+        {
+            "x": 3.473121265266662,
+            "y": 6.199619629204348,
+            "heading": -4.6160884572564577e-17,
+            "angularVelocity": -6.68694321901595e-17,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849242,
+            "timestamp": 0.767045220273263
+        },
+        {
+            "x": 3.8204141768444413,
+            "y": 6.304246419469565,
+            "heading": -5.207468497060529e-17,
+            "angularVelocity": -6.167876669314405e-17,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849244,
+            "timestamp": 0.862925872807421
+        },
+        {
+            "x": 4.16770708842222,
+            "y": 6.408873209734782,
+            "heading": -3.0597254964935646e-17,
+            "angularVelocity": 2.24001708769928e-16,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849242,
+            "timestamp": 0.9588065253415788
+        },
+        {
+            "x": 4.515,
+            "y": 6.5135,
+            "heading": -2.0783137065340813e-18,
+            "angularVelocity": 2.974420855995074e-16,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849244,
+            "timestamp": 1.0546871778757367
+        },
+        {
+            "x": 4.862292911577779,
+            "y": 6.618126790265217,
+            "heading": -2.7789619178473824e-17,
+            "angularVelocity": -2.681594752696605e-16,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849244,
+            "timestamp": 1.1505678304098945
+        },
+        {
+            "x": 5.209585823155558,
+            "y": 6.722753580530434,
+            "heading": 2.2712942266126744e-18,
+            "angularVelocity": 3.135242889004865e-16,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849242,
+            "timestamp": 1.2464484829440523
+        },
+        {
+            "x": 5.5568787347333375,
+            "y": 6.827380370795651,
+            "heading": 3.7041174502192387e-17,
+            "angularVelocity": 3.6263708429820527e-16,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849244,
+            "timestamp": 1.34232913547821
+        },
+        {
+            "x": 5.9041716463111165,
+            "y": 6.932007161060868,
+            "heading": 6.494346998398645e-17,
+            "angularVelocity": 2.910106965725311e-16,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849244,
+            "timestamp": 1.4382097880123679
+        },
+        {
+            "x": 6.251464557888895,
+            "y": 7.036633951326086,
+            "heading": 5.759102638783417e-17,
+            "angularVelocity": -7.668328700140211e-17,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849244,
+            "timestamp": 1.5340904405465257
+        },
+        {
+            "x": 6.598757469466675,
+            "y": 7.141260741591303,
+            "heading": 2.5440135069531877e-17,
+            "angularVelocity": -3.353219911269207e-16,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849244,
+            "timestamp": 1.6299710930806834
+        },
+        {
+            "x": 6.94605038104441,
+            "y": 7.245887531856507,
+            "heading": 2.2711688502269004e-17,
+            "angularVelocity": -2.84566958514003e-17,
+            "velocityX": 3.6221375470302584,
+            "velocityY": 1.0912190050847868,
+            "timestamp": 1.7258517456148412
+        },
+        {
+            "x": 7.239630211495399,
+            "y": 7.334332513952879,
+            "heading": 1.1340524791180054e-17,
+            "angularVelocity": -1.1859706178712186e-16,
+            "velocityX": 3.061929833512548,
+            "velocityY": 0.9224486876000674,
+            "timestamp": 1.821732398148999
+        },
+        {
+            "x": 7.4598151013172,
+            "y": 7.40066625564169,
+            "heading": 5.896173994762203e-18,
+            "angularVelocity": -5.67825796795303e-17,
+            "velocityX": 2.2964475522666925,
+            "velocityY": 0.6918365690635988,
+            "timestamp": 1.9176130506831568
+        },
+        {
+            "x": 7.606605032787844,
+            "y": 7.444888751583952,
+            "heading": 1.9652682638036787e-18,
+            "angularVelocity": -4.099790316709085e-17,
+            "velocityX": 1.530965086187225,
+            "velocityY": 0.46122439484344985,
+            "timestamp": 2.0134937032173146
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 1.0344705803481413e-31,
-            "angularVelocity": 3.7004322975715074e-8,
-            "velocityX": 0.7654830432448523,
-            "velocityY": 0.23061059303913045,
-            "timestamp": 2.109374355729245
+            "heading": 0,
+            "angularVelocity": -2.0497026255693862e-17,
+            "velocityX": 0.7654825584964475,
+            "velocityY": 0.23061220206204183,
+            "timestamp": 2.1093743557514726
         },
         {
             "x": 7.68,
             "y": 7.467,
             "heading": 0,
-            "angularVelocity": -1.5335768309775317e-33,
-            "velocityX": 0,
-            "velocityY": -2.5688294662733772e-30,
-            "timestamp": 2.2052550082623927
+            "angularVelocity": 0,
+            "velocityX": 2.8084467268312453e-34,
+            "velocityY": 0,
+            "timestamp": 2.2052550082856306
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.traj
index 98bf1cf8..1fe0cdb8 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.traj	
@@ -4,217 +4,217 @@
             "x": 1.35,
             "y": 5.56,
             "heading": 0,
-            "angularVelocity": -1.7848043602710886e-30,
-            "velocityX": 4.033392918765628e-32,
-            "velocityY": -2.9619524123271102e-31,
+            "angularVelocity": 0,
+            "velocityX": -2.1086675817598662e-32,
+            "velocityY": -7.052966104933725e-38,
             "timestamp": 0
         },
         {
-            "x": 1.4233949207458116,
-            "y": 5.582111402652435,
-            "heading": -4.931478664026919e-9,
-            "angularVelocity": -5.143711867197603e-8,
-            "velocityX": 0.7654820738778157,
-            "velocityY": 0.2306138106917608,
-            "timestamp": 0.09588065253314748
-        },
-        {
-            "x": 1.570184760818058,
-            "y": 5.626334201974914,
-            "heading": -1.1368240805751946e-8,
-            "angularVelocity": -6.714026800936027e-8,
-            "velocityX": 1.530964132952106,
-            "velocityY": 0.4612275589893128,
-            "timestamp": 0.19176130506629496
-        },
-        {
-            "x": 1.7903695159222153,
-            "y": 5.69266839083479,
-            "heading": -1.6115736339739044e-8,
-            "angularVelocity": -4.952541704566216e-8,
-            "velocityX": 2.2964461472361695,
-            "velocityY": 0.6918412328958446,
-            "timestamp": 0.28764195759944244
-        },
-        {
-            "x": 2.0839491700576396,
-            "y": 5.781113958180788,
-            "heading": -1.639772094621516e-8,
-            "angularVelocity": -2.9553274583871895e-9,
-            "velocityX": 3.0619279946395954,
-            "velocityY": 0.9224547915413995,
-            "timestamp": 0.3835226101325899
-        },
-        {
-            "x": 2.431241903149783,
-            "y": 5.885741340897451,
-            "heading": -1.6397499367976412e-8,
-            "angularVelocity": 2.3168753954621105e-12,
-            "velocityX": 3.6221356855297673,
-            "velocityY": 1.0912251841409724,
-            "timestamp": 0.4794032626657374
-        },
-        {
-            "x": 2.7785346566980107,
-            "y": 5.9903686557177505,
-            "heading": -1.6397564221401302e-8,
-            "angularVelocity": -6.713496881725287e-13,
-            "velocityX": 3.622135898879975,
-            "velocityY": 1.091224476005724,
-            "timestamp": 0.5752839151988849
-        },
-        {
-            "x": 3.125827432121608,
-            "y": 6.094995897925851,
-            "heading": -1.6397647407961563e-8,
-            "angularVelocity": -8.684642063246534e-13,
-            "velocityX": 3.6221361270322743,
-            "velocityY": 1.0912237186873281,
-            "timestamp": 0.6711645677320324
-        },
-        {
-            "x": 3.4731202302782154,
-            "y": 6.1996230646748005,
-            "heading": -1.6397748264247437e-8,
-            "angularVelocity": -1.0543921850153982e-12,
-            "velocityX": 3.6221363641294033,
-            "velocityY": 1.0912229316763022,
-            "timestamp": 0.7670452202651799
-        },
-        {
-            "x": 3.8204130521112987,
-            "y": 6.304250152832848,
-            "heading": -1.639786614425739e-8,
-            "angularVelocity": -1.2203887930777277e-12,
-            "velocityX": 3.622136611066492,
-            "velocityY": 1.091222112002269,
-            "timestamp": 0.8629258727983273
-        },
-        {
-            "x": 4.167705899087819,
-            "y": 6.408877157530555,
-            "heading": -1.6398004207896958e-8,
-            "angularVelocity": -1.408074947723948e-12,
-            "velocityX": 3.6221368733034063,
-            "velocityY": 1.0912212415417761,
-            "timestamp": 0.9588065253314748
-        },
-        {
-            "x": 4.514998774010408,
-            "y": 6.51350406946466,
-            "heading": -1.6398168919050745e-8,
-            "angularVelocity": -1.6668913936591836e-12,
-            "velocityX": 3.6221371647706677,
-            "velocityY": 1.091220274051783,
-            "timestamp": 1.0546871778646223
-        },
-        {
-            "x": 4.862291683961819,
-            "y": 6.618130865121593,
-            "heading": -1.6398370762014746e-8,
-            "angularVelocity": -2.0513887673195245e-12,
-            "velocityX": 3.6221375301080334,
-            "velocityY": 1.0912190613250203,
-            "timestamp": 1.1505678303977698
-        },
-        {
-            "x": 5.20958464417323,
-            "y": 6.722757493946074,
-            "heading": -1.639835180923073e-8,
-            "angularVelocity": 1.902358989315915e-13,
-            "velocityX": 3.6221380542985027,
-            "velocityY": 1.091217321333091,
-            "timestamp": 1.2464484829309173
-        },
-        {
-            "x": 5.556877646148189,
-            "y": 6.8273839841549115,
-            "heading": -1.6398206125528607e-8,
-            "angularVelocity": 1.4352640935347951e-12,
-            "velocityX": 3.62213848987558,
-            "velocityY": 1.09121587562664,
-            "timestamp": 1.3423291354640647
-        },
-        {
-            "x": 5.904170680592406,
-            "y": 6.932010366591235,
-            "heading": -1.6398102572596455e-8,
-            "angularVelocity": 9.899977906647045e-13,
-            "velocityX": 3.6221388285174303,
-            "velocityY": 1.091214751599812,
-            "timestamp": 1.4382097879972122
-        },
-        {
-            "x": 6.2514637423704675,
-            "y": 7.036636658299249,
-            "heading": -1.6398041778119566e-8,
-            "angularVelocity": 5.750349690087699e-13,
-            "velocityX": 3.6221391135989474,
-            "velocityY": 1.0912138053372793,
-            "timestamp": 1.5340904405303597
-        },
-        {
-            "x": 6.5987568262419405,
-            "y": 7.141262876671409,
-            "heading": -1.639800860171266e-8,
-            "angularVelocity": 3.175769618903374e-13,
-            "velocityX": 3.6221393440248217,
-            "velocityY": 1.0912130404711837,
-            "timestamp": 1.6299710930635072
-        },
-        {
-            "x": 6.946049927457118,
-            "y": 7.2458890374680776,
-            "heading": -1.6398270316090924e-8,
-            "angularVelocity": -2.740394345840068e-12,
-            "velocityX": 3.6221395249132713,
-            "velocityY": 1.0912124399786907,
-            "timestamp": 1.7258517455966547
-        },
-        {
-            "x": 7.239629937032664,
-            "y": 7.334333424990155,
-            "heading": -1.4134914023195805e-8,
-            "angularVelocity": 2.36203418937418e-8,
-            "velocityX": 3.061931701744924,
-            "velocityY": 0.9224424864262022,
-            "timestamp": 1.8217323981298021
-        },
-        {
-            "x": 7.45981496297016,
-            "y": 7.4006667148640615,
-            "heading": -8.953329207671134e-9,
-            "angularVelocity": 5.4052849582725206e-8,
-            "velocityX": 2.2964489719251726,
-            "velocityY": 0.6918318568130147,
-            "timestamp": 1.9176130506629496
-        },
-        {
-            "x": 7.606604986310546,
-            "y": 7.444888905858524,
-            "heading": -3.5476510780655253e-9,
-            "angularVelocity": 5.638647309242438e-8,
-            "velocityX": 1.5309660443693864,
-            "velocityY": 0.4612212143584205,
-            "timestamp": 2.0134937031960973
+            "x": 1.4233949672121562,
+            "y": 5.582111248416047,
+            "heading": 6.989955400155978e-19,
+            "angularVelocity": 7.290266672073875e-18,
+            "velocityX": 0.7654825584964474,
+            "velocityY": 0.23061220206204183,
+            "timestamp": 0.09588065253415788
+        },
+        {
+            "x": 1.5701848986828006,
+            "y": 5.626333744358309,
+            "heading": -5.64955528358279e-19,
+            "angularVelocity": -1.3182545571253806e-17,
+            "velocityX": 1.530965086187225,
+            "velocityY": 0.4612243948434498,
+            "timestamp": 0.19176130506831576
+        },
+        {
+            "x": 1.7903697885046006,
+            "y": 5.69266748604712,
+            "heading": -1.1328157855510893e-18,
+            "angularVelocity": -5.922574643212496e-18,
+            "velocityX": 2.2964475522666925,
+            "velocityY": 0.6918365690635988,
+            "timestamp": 0.28764195760247363
+        },
+        {
+            "x": 2.08394961895559,
+            "y": 5.781112468143492,
+            "heading": -1.2347885191323161e-17,
+            "angularVelocity": -1.1696905584501664e-16,
+            "velocityX": 3.061929833512548,
+            "velocityY": 0.9224486876000673,
+            "timestamp": 0.3835226101366315
+        },
+        {
+            "x": 2.4312425305333245,
+            "y": 5.885739258408696,
+            "heading": -1.8571615119787707e-17,
+            "angularVelocity": -6.491121789400923e-17,
+            "velocityX": 3.6221375470302584,
+            "velocityY": 1.0912190050847865,
+            "timestamp": 0.4794032626707894
+        },
+        {
+            "x": 2.7785354421111035,
+            "y": 5.990366048673914,
+            "heading": -2.5105020741934312e-17,
+            "angularVelocity": -6.814102166319416e-17,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849244,
+            "timestamp": 0.5752839152049473
+        },
+        {
+            "x": 3.125828353688883,
+            "y": 6.094992838939131,
+            "heading": -3.974939977960392e-17,
+            "angularVelocity": -1.527354961675144e-16,
+            "velocityX": 3.622137547030716,
+            "velocityY": 1.0912190050849244,
+            "timestamp": 0.6711645677391052
+        },
+        {
+            "x": 3.473121265266662,
+            "y": 6.199619629204348,
+            "heading": -4.6160884572564577e-17,
+            "angularVelocity": -6.68694321901595e-17,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849242,
+            "timestamp": 0.767045220273263
+        },
+        {
+            "x": 3.8204141768444413,
+            "y": 6.304246419469565,
+            "heading": -5.207468497060529e-17,
+            "angularVelocity": -6.167876669314405e-17,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849244,
+            "timestamp": 0.862925872807421
+        },
+        {
+            "x": 4.16770708842222,
+            "y": 6.408873209734782,
+            "heading": -3.0597254964935646e-17,
+            "angularVelocity": 2.24001708769928e-16,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849242,
+            "timestamp": 0.9588065253415788
+        },
+        {
+            "x": 4.515,
+            "y": 6.5135,
+            "heading": -2.0783137065340813e-18,
+            "angularVelocity": 2.974420855995074e-16,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849244,
+            "timestamp": 1.0546871778757367
+        },
+        {
+            "x": 4.862292911577779,
+            "y": 6.618126790265217,
+            "heading": -2.7789619178473824e-17,
+            "angularVelocity": -2.681594752696605e-16,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849244,
+            "timestamp": 1.1505678304098945
+        },
+        {
+            "x": 5.209585823155558,
+            "y": 6.722753580530434,
+            "heading": 2.2712942266126744e-18,
+            "angularVelocity": 3.135242889004865e-16,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849242,
+            "timestamp": 1.2464484829440523
+        },
+        {
+            "x": 5.5568787347333375,
+            "y": 6.827380370795651,
+            "heading": 3.7041174502192387e-17,
+            "angularVelocity": 3.6263708429820527e-16,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849244,
+            "timestamp": 1.34232913547821
+        },
+        {
+            "x": 5.9041716463111165,
+            "y": 6.932007161060868,
+            "heading": 6.494346998398645e-17,
+            "angularVelocity": 2.910106965725311e-16,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849244,
+            "timestamp": 1.4382097880123679
+        },
+        {
+            "x": 6.251464557888895,
+            "y": 7.036633951326086,
+            "heading": 5.759102638783417e-17,
+            "angularVelocity": -7.668328700140211e-17,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849244,
+            "timestamp": 1.5340904405465257
+        },
+        {
+            "x": 6.598757469466675,
+            "y": 7.141260741591303,
+            "heading": 2.5440135069531877e-17,
+            "angularVelocity": -3.353219911269207e-16,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849244,
+            "timestamp": 1.6299710930806834
+        },
+        {
+            "x": 6.94605038104441,
+            "y": 7.245887531856507,
+            "heading": 2.2711688502269004e-17,
+            "angularVelocity": -2.84566958514003e-17,
+            "velocityX": 3.6221375470302584,
+            "velocityY": 1.0912190050847868,
+            "timestamp": 1.7258517456148412
+        },
+        {
+            "x": 7.239630211495399,
+            "y": 7.334332513952879,
+            "heading": 1.1340524791180054e-17,
+            "angularVelocity": -1.1859706178712186e-16,
+            "velocityX": 3.061929833512548,
+            "velocityY": 0.9224486876000674,
+            "timestamp": 1.821732398148999
+        },
+        {
+            "x": 7.4598151013172,
+            "y": 7.40066625564169,
+            "heading": 5.896173994762203e-18,
+            "angularVelocity": -5.67825796795303e-17,
+            "velocityX": 2.2964475522666925,
+            "velocityY": 0.6918365690635988,
+            "timestamp": 1.9176130506831568
+        },
+        {
+            "x": 7.606605032787844,
+            "y": 7.444888751583952,
+            "heading": 1.9652682638036787e-18,
+            "angularVelocity": -4.099790316709085e-17,
+            "velocityX": 1.530965086187225,
+            "velocityY": 0.46122439484344985,
+            "timestamp": 2.0134937032173146
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 1.0344705803481413e-31,
-            "angularVelocity": 3.7004322975715074e-8,
-            "velocityX": 0.7654830432448523,
-            "velocityY": 0.23061059303913045,
-            "timestamp": 2.109374355729245
+            "heading": 0,
+            "angularVelocity": -2.0497026255693862e-17,
+            "velocityX": 0.7654825584964475,
+            "velocityY": 0.23061220206204183,
+            "timestamp": 2.1093743557514726
         },
         {
             "x": 7.68,
             "y": 7.467,
             "heading": 0,
-            "angularVelocity": -1.5335768309775317e-33,
-            "velocityX": 0,
-            "velocityY": -2.5688294662733772e-30,
-            "timestamp": 2.2052550082623927
+            "angularVelocity": 0,
+            "velocityX": 2.8084467268312453e-34,
+            "velocityY": 0,
+            "timestamp": 2.2052550082856306
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.1.traj
index f95c4186..1f520266 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.1.traj	
@@ -3,1091 +3,1091 @@
         {
             "x": 1.35,
             "y": 5.56,
-            "heading": 4.408218114222724e-33,
+            "heading": -1.9296664556128306e-33,
             "angularVelocity": 0,
-            "velocityX": 1.8935205228973059e-34,
-            "velocityY": 9.513896878368867e-32,
+            "velocityX": -7.813211441980168e-34,
+            "velocityY": -2.2407271587906947e-32,
             "timestamp": 0
         },
         {
-            "x": 1.3544641547566925,
-            "y": 5.559252486403593,
-            "heading": 5.298729726015063e-12,
-            "angularVelocity": 2.274228720676412e-10,
-            "velocityX": 0.1916026983161006,
-            "velocityY": -0.0320834805032552,
-            "timestamp": 0.02329901820759936
-        },
-        {
-            "x": 1.3633924655059346,
-            "y": 5.557757467272512,
-            "heading": 1.605830471024488e-11,
-            "angularVelocity": 4.618037851350714e-10,
-            "velocityX": 0.38320544967553505,
-            "velocityY": -0.06416661499484601,
-            "timestamp": 0.04659803641519872
-        },
-        {
-            "x": 1.3767849336296958,
-            "y": 5.555514951621415,
-            "heading": 3.246020157611451e-11,
-            "angularVelocity": 7.039737222967939e-10,
-            "velocityX": 0.574808260349448,
-            "velocityY": -0.09624936257493934,
-            "timestamp": 0.06989705462279808
-        },
-        {
-            "x": 1.3946415606835925,
-            "y": 5.552524949597437,
-            "heading": 5.470894313107332e-11,
-            "angularVelocity": 9.549218317932479e-10,
-            "velocityX": 0.7664111377908356,
-            "velocityY": -0.12833167463728912,
-            "timestamp": 0.09319607283039744
-        },
-        {
-            "x": 1.416962348431807,
-            "y": 5.548787472707871,
-            "heading": 8.303679095756625e-11,
-            "angularVelocity": 1.2158386690956822e-9,
-            "velocityX": 0.9580140909513821,
-            "velocityY": -0.1604134928036672,
-            "timestamp": 0.1164950910379968
-        },
-        {
-            "x": 1.4437472988920377,
-            "y": 5.544302534113244,
-            "heading": 1.1770979097345793e-10,
-            "angularVelocity": 1.4881742667086445e-9,
-            "velocityX": 1.1496171307120806,
-            "velocityY": -0.19249474611664116,
-            "timestamp": 0.13979410924559615
-        },
-        {
-            "x": 1.4749964143943655,
-            "y": 5.539070149011117,
-            "heading": 1.59035725627389e-10,
-            "angularVelocity": 1.7737199674396097e-9,
-            "velocityX": 1.3412202704804594,
-            "velocityY": -0.22457534714647368,
-            "timestamp": 0.1630931274531955
-        },
-        {
-            "x": 1.5107096976598688,
-            "y": 5.533090335148591,
-            "heading": 2.073747689126765e-10,
-            "angularVelocity": 2.0747244300773254e-9,
-            "velocityX": 1.532823527038234,
-            "velocityY": -0.25665518646593166,
-            "timestamp": 0.18639214566079487
-        },
-        {
-            "x": 1.5508871519080267,
-            "y": 5.526363113522473,
-            "heading": 2.631540997774015e-10,
-            "angularVelocity": 2.3940635247907603e-9,
-            "velocityX": 1.7244269217769022,
-            "velocityY": -0.28873412459698383,
-            "timestamp": 0.20969116386839423
-        },
-        {
-            "x": 1.595528781007412,
-            "y": 5.518888509361551,
-            "heading": 3.268885139183836e-10,
-            "angularVelocity": 2.7354977853334202e-9,
-            "velocityX": 1.9160304825557395,
-            "velocityY": -0.3208119799018042,
-            "timestamp": 0.2329901820759936
-        },
-        {
-            "x": 1.6446345896939198,
-            "y": 5.5106665535489805,
-            "heading": 3.9921044507968937e-10,
-            "angularVelocity": 3.104076267437521e-9,
-            "velocityX": 2.1076342466004383,
-            "velocityY": -0.35288850969337676,
-            "timestamp": 0.2562892002835929
-        },
-        {
-            "x": 1.698204583899122,
-            "y": 5.501697284762255,
-            "heading": 4.809154717853132e-10,
-            "angularVelocity": 3.5068011876967e-9,
-            "velocityX": 2.299238265229739,
-            "velocityY": -0.38496337943692166,
-            "timestamp": 0.2795882184911923
-        },
-        {
-            "x": 1.7562387712682006,
-            "y": 5.491980752848314,
-            "heading": 5.730347406124241e-10,
-            "angularVelocity": 3.953783188523521e-9,
-            "velocityX": 2.4908426119922917,
-            "velocityY": -0.41703610973659105,
-            "timestamp": 0.3028872366987917
-        },
-        {
-            "x": 1.8187371620272614,
-            "y": 5.48151702447463,
-            "heading": 6.769576861119728e-10,
-            "angularVelocity": 4.460400083403711e-9,
-            "velocityX": 2.6824473976619876,
-            "velocityY": -0.449105978650929,
-            "timestamp": 0.3261862549063911
-        },
-        {
-            "x": 1.8856997705540457,
-            "y": 5.4703061933617745,
-            "heading": 7.946577963057737e-10,
-            "angularVelocity": 5.051719599980361e-9,
-            "velocityX": 2.8740528004281045,
-            "velocityY": -0.4811718250535651,
-            "timestamp": 0.34948527311399047
-        },
-        {
-            "x": 1.957126618545275,
-            "y": 5.4583484009138274,
-            "heading": 9.291581669995643e-10,
-            "angularVelocity": 5.772791178735968e-9,
-            "velocityX": 3.0656591344232145,
-            "velocityY": -0.5132316023559669,
-            "timestamp": 0.37278429132158986
-        },
-        {
-            "x": 2.0330177424946343,
-            "y": 5.445643884917638,
-            "heading": 1.0856695619650843e-9,
-            "angularVelocity": 6.717510100758224e-9,
-            "velocityX": 3.257267034736916,
-            "velocityY": -0.5452811737842305,
-            "timestamp": 0.39608330952918924
-        },
-        {
-            "x": 2.1133732165489003,
-            "y": 5.432193128386492,
-            "heading": 1.2752962310893515e-9,
-            "angularVelocity": 8.13882633127911e-9,
-            "velocityX": 3.44887811744974,
-            "velocityY": -0.5773100141552516,
-            "timestamp": 0.41938232773678863
-        },
-        {
-            "x": 2.1981932628587333,
-            "y": 5.417997633484574,
-            "heading": 1.5377676317044546e-9,
-            "angularVelocity": 1.1265341261395982e-8,
-            "velocityX": 3.640498734927221,
-            "velocityY": -0.6092743812441802,
-            "timestamp": 0.442681345944388
-        },
-        {
-            "x": 2.285115669591906,
-            "y": 5.403405223128801,
-            "heading": 1.537767230961928e-9,
-            "angularVelocity": -1.719997404273206e-14,
-            "velocityX": 3.730732598201487,
-            "velocityY": -0.6263100970908951,
-            "timestamp": 0.4659803641519874
-        },
-        {
-            "x": 2.3720410537379144,
-            "y": 5.388830564421114,
-            "heading": 1.5377672475870007e-9,
-            "angularVelocity": 7.135525631450618e-16,
-            "velocityX": 3.7308603895453194,
-            "velocityY": -0.6255481916751834,
-            "timestamp": 0.4892793823595868
-        },
-        {
-            "x": 2.45896811239221,
-            "y": 5.374265896231001,
-            "heading": 1.5377672778413637e-9,
-            "angularVelocity": 1.298525326828493e-15,
-            "velocityX": 3.730932259882311,
-            "velocityY": -0.625119396028046,
-            "timestamp": 0.5125784005671862
-        },
-        {
-            "x": 2.5458958805121723,
-            "y": 5.359705463035692,
-            "heading": 1.5377672869369513e-9,
-            "angularVelocity": 3.903849318113422e-16,
-            "velocityX": 3.730962710335439,
-            "velocityY": -0.6249376289364498,
-            "timestamp": 0.5358774187747856
-        },
-        {
-            "x": 2.632823878731898,
-            "y": 5.345146403658019,
-            "heading": 1.5377672987189865e-9,
-            "angularVelocity": 5.056879723770718e-16,
-            "velocityX": 3.7309725862775656,
-            "velocityY": -0.6248786643237555,
-            "timestamp": 0.559176436982385
-        },
-        {
-            "x": 2.7197519358547084,
-            "y": 5.330587695987482,
-            "heading": 1.5377673268477785e-9,
-            "angularVelocity": 1.2072952571647457e-15,
-            "velocityX": 3.7309751144130874,
-            "velocityY": -0.6248635689629851,
-            "timestamp": 0.5824754551899843
-        },
-        {
-            "x": 2.806680023647356,
-            "y": 5.316029171436071,
-            "heading": 1.5377673567071691e-9,
-            "angularVelocity": 1.2815730065587283e-15,
-            "velocityX": 3.7309764307702067,
-            "velocityY": -0.6248557094441328,
-            "timestamp": 0.6057744733975837
-        },
-        {
-            "x": 2.8936082747189524,
-            "y": 5.301471621825431,
-            "heading": 1.5377673928791505e-9,
-            "angularVelocity": 1.5525110632783364e-15,
-            "velocityX": 3.7309834387453797,
-            "velocityY": -0.6248138647313558,
-            "timestamp": 0.6290734916051831
-        },
-        {
-            "x": 2.980536964716405,
-            "y": 5.286916693414742,
-            "heading": 1.5377674058328616e-9,
-            "angularVelocity": 5.559766105183297e-16,
-            "velocityX": 3.7310022775585137,
-            "velocityY": -0.6247013621403136,
-            "timestamp": 0.6523725098127825
-        },
-        {
-            "x": 3.0674663067511823,
-            "y": 5.272365659809418,
-            "heading": 1.5377674408459054e-9,
-            "angularVelocity": 1.5027691524839091e-15,
-            "velocityX": 3.73103026317236,
-            "velocityY": -0.6245341960676912,
-            "timestamp": 0.6756715280203819
-        },
-        {
-            "x": 3.154396100803892,
-            "y": 5.257817326905748,
-            "heading": 1.5377674636831172e-9,
-            "angularVelocity": 9.801792492345066e-16,
-            "velocityX": 3.7310496639016963,
-            "velocityY": -0.624418281237505,
-            "timestamp": 0.6989705462279813
-        },
-        {
-            "x": 3.2413249978571956,
-            "y": 5.243263635140824,
-            "heading": 1.5377674638426843e-9,
-            "angularVelocity": 6.848580307945622e-18,
-            "velocityX": 3.731011164452385,
-            "velocityY": -0.6246482849651879,
-            "timestamp": 0.7222695644355807
-        },
-        {
-            "x": 3.3282506523054423,
-            "y": 5.228690588454777,
-            "heading": 1.5377674749707248e-9,
-            "angularVelocity": 4.776183371381179e-16,
-            "velocityX": 3.7308719909897503,
-            "velocityY": -0.6254790032748808,
-            "timestamp": 0.74556858264318
-        },
-        {
-            "x": 3.415171009638962,
-            "y": 5.214085980519485,
-            "heading": 1.5377674915535578e-9,
-            "angularVelocity": 7.117396517888937e-16,
-            "velocityX": 3.7306446374302076,
-            "velocityY": -0.626833620407003,
-            "timestamp": 0.7688676008507794
-        },
-        {
-            "x": 3.5020842171218036,
-            "y": 5.1994388833661285,
-            "heading": 1.53776751807092e-9,
-            "angularVelocity": 1.13813226337154e-15,
-            "velocityX": 3.730337763955042,
-            "velocityY": -0.6286572688425895,
-            "timestamp": 0.7921666190583788
-        },
-        {
-            "x": 3.5889883305639807,
-            "y": 5.184737925794629,
-            "heading": 1.5377675551515876e-9,
-            "angularVelocity": 1.5915121086693097e-15,
-            "velocityX": 3.729947445333141,
-            "velocityY": -0.6309689721834142,
-            "timestamp": 0.8154656372659782
-        },
-        {
-            "x": 3.675881697311449,
-            "y": 5.1699735802440445,
-            "heading": 1.5377675887419511e-9,
-            "angularVelocity": 1.441707334130238e-15,
-            "velocityX": 3.7294861943638535,
-            "velocityY": -0.6336896009521162,
-            "timestamp": 0.8387646554735776
-        },
-        {
-            "x": 3.7627625314341118,
-            "y": 5.155135664387593,
-            "heading": 1.5377676077743658e-9,
-            "angularVelocity": 8.168762038570203e-16,
-            "velocityX": 3.728948290804007,
-            "velocityY": -0.636847257870781,
-            "timestamp": 0.862063673681177
-        },
-        {
-            "x": 3.8490591061764308,
-            "y": 5.137209586747657,
-            "heading": 1.5377676390897132e-9,
-            "angularVelocity": 1.344063051824819e-15,
-            "velocityX": 3.703871724266177,
-            "velocityY": -0.7693919752476525,
-            "timestamp": 0.8853626918887764
-        },
-        {
-            "x": 3.934365266987087,
-            "y": 5.115044254457407,
-            "heading": 1.5377676702466937e-9,
-            "angularVelocity": 1.337265902944942e-15,
-            "velocityX": 3.661362897377225,
-            "velocityY": -0.9513419017440721,
-            "timestamp": 0.9086617100963758
+            "x": 1.35439418502164,
+            "y": 5.559271848784049,
+            "heading": -0.0037613483747314606,
+            "angularVelocity": -0.16062668985124567,
+            "velocityX": 0.18765169410035007,
+            "velocityY": -0.031095370031394967,
+            "timestamp": 0.023416708507251784
+        },
+        {
+            "x": 1.3631871179840578,
+            "y": 5.557813607446842,
+            "heading": -0.011205273313316288,
+            "angularVelocity": -0.317889464963847,
+            "velocityX": 0.37549824560930783,
+            "velocityY": -0.06227354014150002,
+            "timestamp": 0.04683341701450357
+        },
+        {
+            "x": 1.3763837173940232,
+            "y": 5.5556231238892515,
+            "heading": -0.02224504855076525,
+            "angularVelocity": -0.47144863395424863,
+            "velocityX": 0.563554839736692,
+            "velocityY": -0.09354361467639355,
+            "timestamp": 0.07025012552175536
+        },
+        {
+            "x": 1.3939893513502628,
+            "y": 5.552698044632534,
+            "heading": -0.03678367986427699,
+            "angularVelocity": -0.6208657083043512,
+            "velocityX": 0.7518406761047259,
+            "velocityY": -0.12491419346204678,
+            "timestamp": 0.09366683402900713
+        },
+        {
+            "x": 1.4160099408355828,
+            "y": 5.549035829570074,
+            "heading": -0.0547111148835232,
+            "angularVelocity": -0.7655830456997818,
+            "velocityX": 0.9403793653792271,
+            "velocityY": -0.15639324635770782,
+            "timestamp": 0.11708354253625891
+        },
+        {
+            "x": 1.4424520698548373,
+            "y": 5.544633771060564,
+            "heading": -0.07590088077413953,
+            "angularVelocity": -0.9048994176125892,
+            "velocityX": 1.129199221618411,
+            "velocityY": -0.18798792785714832,
+            "timestamp": 0.1405002510435107
+        },
+        {
+            "x": 1.4733230980825243,
+            "y": 5.539489016814175,
+            "heading": -0.10020600379509263,
+            "angularVelocity": -1.0379393420482537,
+            "velocityX": 1.3183333694454236,
+            "velocityY": -0.2197044151102235,
+            "timestamp": 0.16391695955076246
+        },
+        {
+            "x": 1.5086312686123933,
+            "y": 5.533598592872994,
+            "heading": -0.12745397379473564,
+            "angularVelocity": -1.163612298082128,
+            "velocityX": 1.5078195348818708,
+            "velocityY": -0.25154790389774145,
+            "timestamp": 0.18733366805801424
+        },
+        {
+            "x": 1.5483857981367395,
+            "y": 5.526959418437362,
+            "heading": -0.15744034724886685,
+            "angularVelocity": -1.2805545854082991,
+            "velocityX": 1.6976992950155392,
+            "velocityY": -0.28352295684851286,
+            "timestamp": 0.21075037656526602
+        },
+        {
+            "x": 1.5925969268012463,
+            "y": 5.5195682985391485,
+            "heading": -0.18992026053497643,
+            "angularVelocity": -1.3870400819163256,
+            "velocityX": 1.888016355962865,
+            "velocityY": -0.31563444947560276,
+            "timestamp": 0.2341670850725178
+        },
+        {
+            "x": 1.6412758830317984,
+            "y": 5.511421875068327,
+            "heading": -0.22459650029504458,
+            "angularVelocity": -1.4808332157070316,
+            "velocityX": 2.0788129217851563,
+            "velocityY": -0.34788934868040844,
+            "timestamp": 0.25758379357976957
+        },
+        {
+            "x": 1.6944346649736404,
+            "y": 5.502516516620167,
+            "heading": -0.26110152083126115,
+            "angularVelocity": -1.5589304758570732,
+            "velocityX": 2.2701218630013136,
+            "velocityY": -0.38029932539011047,
+            "timestamp": 0.2810005020870214
+        },
+        {
+            "x": 1.7520853949904163,
+            "y": 5.492848149517658,
+            "heading": -0.298968117565775,
+            "angularVelocity": -1.6170759747377448,
+            "velocityX": 2.4619484843022064,
+            "velocityY": -0.4128832666435571,
+            "timestamp": 0.3044172105942732
+        },
+        {
+            "x": 1.8142385663960479,
+            "y": 5.482412126535505,
+            "heading": -0.337577206769387,
+            "angularVelocity": -1.6487837815317001,
+            "velocityX": 2.654223217852448,
+            "velocityY": -0.44566566556215986,
+            "timestamp": 0.327833919101525
+        },
+        {
+            "x": 1.8808979849725118,
+            "y": 5.471203531590798,
+            "heading": -0.3760544549024592,
+            "angularVelocity": -1.6431535679387552,
+            "velocityX": 2.8466604756095246,
+            "velocityY": -0.47865800358925525,
+            "timestamp": 0.3512506276087768
+        },
+        {
+            "x": 1.9520436894250033,
+            "y": 5.459218834920382,
+            "heading": -0.41303307424029884,
+            "angularVelocity": -1.5791552995754587,
+            "velocityX": 3.0382452952539354,
+            "velocityY": -0.5118010785637118,
+            "timestamp": 0.3746673361160286
+        },
+        {
+            "x": 2.027554046757936,
+            "y": 5.446452385802885,
+            "heading": -0.44595627669268334,
+            "angularVelocity": -1.4059705462956014,
+            "velocityX": 3.2246358325529645,
+            "velocityY": -0.5451854650516427,
+            "timestamp": 0.3980840446232804
+        },
+        {
+            "x": 2.106704107988529,
+            "y": 5.4327632732947455,
+            "heading": -0.46854805770485686,
+            "angularVelocity": -0.9647718425148408,
+            "velocityX": 3.3800677497471567,
+            "velocityY": -0.5845873899786794,
+            "timestamp": 0.4215007531305322
+        },
+        {
+            "x": 2.1895060473348913,
+            "y": 5.418170740153337,
+            "heading": -0.4808901513114338,
+            "angularVelocity": -0.5270635539044761,
+            "velocityX": 3.5360195614476866,
+            "velocityY": -0.6231675616108724,
+            "timestamp": 0.444917461637784
+        },
+        {
+            "x": 2.275961949822614,
+            "y": 5.402702256569038,
+            "heading": -0.4829709400280361,
+            "angularVelocity": -0.0888591458512548,
+            "velocityX": 3.6920604132278863,
+            "velocityY": -0.6605746311236733,
+            "timestamp": 0.4683341701450358
+        },
+        {
+            "x": 2.3633379318433296,
+            "y": 5.388122823546896,
+            "heading": -0.48297098226531077,
+            "angularVelocity": -0.0000018037238101032338,
+            "velocityX": 3.7313519956767345,
+            "velocityY": -0.6226081269122786,
+            "timestamp": 0.4917508786522876
+        },
+        {
+            "x": 2.4507217300981603,
+            "y": 5.373590311340196,
+            "heading": -0.48297102439627604,
+            "angularVelocity": -0.0000017991839149984119,
+            "velocityX": 3.7316857844376363,
+            "velocityY": -0.6206043946012527,
+            "timestamp": 0.5151675871595394
+        },
+        {
+            "x": 2.5381084152434026,
+            "y": 5.359075168632344,
+            "heading": -0.4829710665257657,
+            "angularVelocity": -0.0000017991208983915868,
+            "velocityX": 3.7318090677936033,
+            "velocityY": -0.6198626379688947,
+            "timestamp": 0.5385842956667911
+        },
+        {
+            "x": 2.6254950830921073,
+            "y": 5.344559921791865,
+            "heading": -0.482971108655264,
+            "angularVelocity": -0.0000017991212673817243,
+            "velocityX": 3.7318083291527238,
+            "velocityY": -0.6198670849061145,
+            "timestamp": 0.5620010041740429
+        },
+        {
+            "x": 2.712881673840345,
+            "y": 5.330044210788612,
+            "heading": -0.4829711507848006,
+            "angularVelocity": -0.0000017991229009417428,
+            "velocityX": 3.731805036611997,
+            "velocityY": -0.6198869067684731,
+            "timestamp": 0.5854177126812946
+        },
+        {
+            "x": 2.800268245782709,
+            "y": 5.315528386572539,
+            "heading": -0.4829711929143463,
+            "angularVelocity": -0.0000017991232935293638,
+            "velocityX": 3.7318042335155903,
+            "velocityY": -0.6198917414709255,
+            "timestamp": 0.6088344211885464
+        },
+        {
+            "x": 2.8876548130761686,
+            "y": 5.301012534370082,
+            "heading": -0.4829712350438941,
+            "angularVelocity": -0.0000017991233825733017,
+            "velocityX": 3.731804034986268,
+            "velocityY": -0.6198929366167053,
+            "timestamp": 0.6322511296957981
+        },
+        {
+            "x": 2.975041345179212,
+            "y": 5.286496470320027,
+            "heading": -0.48297127717345933,
+            "angularVelocity": -0.000001799124124149535,
+            "velocityX": 3.7318025321954105,
+            "velocityY": -0.6199019834729332,
+            "timestamp": 0.6556678382030499
+        },
+        {
+            "x": 3.0624277772479402,
+            "y": 5.271979804077107,
+            "heading": -0.4829713193030743,
+            "angularVelocity": -0.0000017991262516148556,
+            "velocityX": 3.7317982602749975,
+            "velocityY": -0.6199276998481766,
+            "timestamp": 0.6790845467103016
+        },
+        {
+            "x": 3.149814115388955,
+            "y": 5.257462572426413,
+            "heading": -0.482971361432736,
+            "angularVelocity": -0.0000017991282469783394,
+            "velocityX": 3.731794249134225,
+            "velocityY": -0.619951845332987,
+            "timestamp": 0.7025012552175534
+        },
+        {
+            "x": 3.2372009926614522,
+            "y": 5.242948586436876,
+            "heading": -0.48297140356212753,
+            "angularVelocity": -0.0000017991167072996656,
+            "velocityX": 3.7318172725014778,
+            "velocityY": -0.6198132408343549,
+            "timestamp": 0.7259179637248051
+        },
+        {
+            "x": 3.3245908005140246,
+            "y": 5.228452256133779,
+            "heading": -0.4829714456900571,
+            "angularVelocity": -0.0000017990542795775989,
+            "velocityX": 3.7319424216049217,
+            "velocityY": -0.6190592626894866,
+            "timestamp": 0.7493346722320569
+        },
+        {
+            "x": 3.4119833565424647,
+            "y": 5.213972502683899,
+            "heading": -0.48297148781659605,
+            "angularVelocity": -0.0000017989948869691892,
+            "velocityX": 3.7320597812190472,
+            "velocityY": -0.6183513556300411,
+            "timestamp": 0.7727513807393086
+        },
+        {
+            "x": 3.4993677183765524,
+            "y": 5.199443379550061,
+            "heading": -0.48297152994719206,
+            "angularVelocity": -0.0000017991681461203675,
+            "velocityX": 3.731709851836638,
+            "velocityY": -0.6204596657670539,
+            "timestamp": 0.7961680892465604
+        },
+        {
+            "x": 3.5867330233447126,
+            "y": 5.184800101210533,
+            "heading": -0.48297157208742414,
+            "angularVelocity": -0.0000017995796480013664,
+            "velocityX": 3.7308960369522555,
+            "velocityY": -0.6253346124593939,
+            "timestamp": 0.8195847977538121
+        },
+        {
+            "x": 3.6740864894013328,
+            "y": 5.170086363489935,
+            "heading": -0.48297161423379187,
+            "angularVelocity": -0.0000017998416699478457,
+            "velocityX": 3.7303904615618975,
+            "velocityY": -0.6283435486246598,
+            "timestamp": 0.8430015062610638
+        },
+        {
+            "x": 3.7614250146254347,
+            "y": 5.155284197469098,
+            "heading": -0.482971656398251,
+            "angularVelocity": -0.0000018006142553635733,
+            "velocityX": 3.729752420031712,
+            "velocityY": -0.6321198393988197,
+            "timestamp": 0.8664182147683156
+        },
+        {
+            "x": 3.848175799548237,
+            "y": 5.137355912505346,
+            "heading": -0.48297169899264486,
+            "angularVelocity": -0.000001818974422860047,
+            "velocityX": 3.704653234929829,
+            "velocityY": -0.7656193421980864,
+            "timestamp": 0.8898349232755673
+        },
+        {
+            "x": 3.9339293985898425,
+            "y": 5.115142351916783,
+            "heading": -0.48297174253890224,
+            "angularVelocity": -0.0000018596233270255789,
+            "velocityX": 3.6620688605764276,
+            "velocityY": -0.9486201095127581,
+            "timestamp": 0.9132516317828191
         },
         {
             "x": 4.018431663513184,
             "y": 5.088562488555908,
-            "heading": 1.5377676866838871e-9,
-            "angularVelocity": 7.054887729794734e-16,
-            "velocityX": 3.608151887646215,
-            "velocityY": -1.1366043695780652,
-            "timestamp": 0.9319607283039751
-        },
-        {
-            "x": 4.095480434320656,
-            "y": 5.06018496604887,
-            "heading": 1.5377676994335126e-9,
-            "angularVelocity": 5.874067965982521e-16,
-            "velocityX": 3.549827430213289,
-            "velocityY": -1.3074226459526814,
-            "timestamp": 0.9536656625077448
-        },
-        {
-            "x": 4.171095847536846,
-            "y": 5.028183306370306,
-            "heading": 1.5377677188417825e-9,
-            "angularVelocity": 8.941870233905506e-16,
-            "velocityX": 3.4837891009642465,
-            "velocityY": -1.4743956087592989,
-            "timestamp": 0.9753705967115144
-        },
-        {
-            "x": 4.24514985906727,
-            "y": 4.992717825181937,
-            "heading": 1.5377677281143787e-9,
-            "angularVelocity": 4.2721152582009774e-16,
-            "velocityX": 3.4118514636002706,
-            "velocityY": -1.6339824325334937,
-            "timestamp": 0.997075530915284
-        },
-        {
-            "x": 4.317807248978416,
-            "y": 4.95447255507026,
-            "heading": 1.5377677290979519e-9,
-            "angularVelocity": 4.531574211416763e-17,
-            "velocityX": 3.3475056514378028,
-            "velocityY": -1.7620541832797834,
-            "timestamp": 1.0187804651190535
-        },
-        {
-            "x": 4.3901722659206355,
-            "y": 4.9156769197600605,
-            "heading": 1.5377677449553673e-9,
-            "angularVelocity": 7.305902735413318e-16,
-            "velocityX": 3.334035305654692,
-            "velocityY": -1.7874108691538961,
-            "timestamp": 1.0404853993228231
-        },
-        {
-            "x": 4.4625185516483485,
-            "y": 4.876846365558822,
-            "heading": 1.5377677746336623e-9,
-            "angularVelocity": 1.3673523815751288e-15,
-            "velocityX": 3.333172312277143,
-            "velocityY": -1.7890196688239732,
-            "timestamp": 1.0621903335265928
-        },
-        {
-            "x": 4.534844691324942,
-            "y": 4.837978300028894,
-            "heading": 1.5377677779199108e-9,
-            "angularVelocity": 1.5140568225814148e-16,
-            "velocityX": 3.3322441338764652,
-            "velocityY": -1.7907479085103777,
-            "timestamp": 1.0838952677303624
-        },
-        {
-            "x": 4.6071619879089205,
-            "y": 4.799093783673277,
-            "heading": 1.537767801979053e-9,
-            "angularVelocity": 1.1084641137098069e-15,
-            "velocityX": 3.331836710728115,
-            "velocityY": -1.79150583873765,
-            "timestamp": 1.105600201934132
-        },
-        {
-            "x": 4.6794740379059565,
-            "y": 4.7601995113399385,
-            "heading": 1.5377678257958516e-9,
-            "angularVelocity": 1.0972987247710634e-15,
-            "velocityX": 3.331594987489656,
-            "velocityY": -1.791955320763837,
-            "timestamp": 1.1273051361379016
-        },
-        {
-            "x": 4.751784687579244,
-            "y": 4.721302635647572,
-            "heading": 1.5377678370559733e-9,
-            "angularVelocity": 5.187816897475249e-16,
-            "velocityX": 3.331530471108496,
-            "velocityY": -1.7920752639579989,
-            "timestamp": 1.1490100703416712
-        },
-        {
-            "x": 4.824095678023389,
-            "y": 4.682406393453542,
-            "heading": 1.5377678370330874e-9,
-            "angularVelocity": -1.0543217693671843e-18,
-            "velocityX": 3.3315461712634193,
-            "velocityY": -1.792046077125237,
-            "timestamp": 1.1707150045454409
-        },
-        {
-            "x": 4.896406945309153,
-            "y": 4.643510665929771,
-            "heading": 1.5377678640109226e-9,
-            "angularVelocity": 1.242935520614447e-15,
-            "velocityX": 3.331558926041247,
-            "velocityY": -1.7920223649940337,
-            "timestamp": 1.1924199387492105
-        },
-        {
-            "x": 4.968718824670649,
-            "y": 4.604616076350799,
-            "heading": 1.5377678830932924e-9,
-            "angularVelocity": 8.791718726461835e-16,
-            "velocityX": 3.33158712588479,
-            "velocityY": -1.7919699370612494,
-            "timestamp": 1.21412487295298
-        },
-        {
-            "x": 5.041033201559798,
-            "y": 4.565726130481367,
-            "heading": 1.5377679126898002e-9,
-            "angularVelocity": 1.3635842497565112e-15,
-            "velocityX": 3.331702193162082,
-            "velocityY": -1.7917559898752364,
-            "timestamp": 1.2358298071567497
-        },
-        {
-            "x": 5.113354228854967,
-            "y": 4.526848553325412,
-            "heading": 1.5377679256265375e-9,
-            "angularVelocity": 5.960275144059911e-16,
-            "velocityX": 3.332008593816099,
-            "velocityY": -1.7911861326534588,
-            "timestamp": 1.2575347413605193
-        },
-        {
-            "x": 5.1856663573422805,
-            "y": 4.487954426928371,
-            "heading": 1.537767949104212e-9,
-            "angularVelocity": 1.0816744331209405e-15,
-            "velocityX": 3.3315986037289687,
-            "velocityY": -1.791948597121158,
-            "timestamp": 1.279239675564289
-        },
-        {
-            "x": 5.257977857015597,
-            "y": 4.449059131458437,
-            "heading": 1.5377679798803482e-9,
-            "angularVelocity": 1.4179326501228622e-15,
-            "velocityX": 3.3315696327123336,
-            "velocityY": -1.7920024592017922,
-            "timestamp": 1.3009446097680586
-        },
-        {
-            "x": 5.330290964048228,
-            "y": 4.410166824431303,
-            "heading": 1.5377679920779735e-9,
-            "angularVelocity": 5.619748235100741e-16,
-            "velocityX": 3.3316436877316113,
-            "velocityY": -1.7918647742503244,
-            "timestamp": 1.3226495439718282
-        },
-        {
-            "x": 5.402605809232356,
-            "y": 4.371277749351831,
-            "heading": 1.5377680157791104e-9,
-            "angularVelocity": 1.0919698963856968e-15,
-            "velocityX": 3.3317237686705528,
-            "velocityY": -1.7917158704270917,
-            "timestamp": 1.3443544781755978
-        },
-        {
-            "x": 5.474921974691154,
-            "y": 4.332391129443428,
-            "heading": 1.5377680240202577e-9,
-            "angularVelocity": 3.7969007641948726e-16,
-            "velocityX": 3.331784596990179,
-            "velocityY": -1.7916027546232325,
-            "timestamp": 1.3660594123793675
-        },
-        {
-            "x": 5.54723622900274,
-            "y": 4.29350095564713,
-            "heading": 1.537768038636468e-9,
-            "angularVelocity": 6.734049902652557e-16,
-            "velocityX": 3.331696545710963,
-            "velocityY": -1.7917664910290456,
-            "timestamp": 1.387764346583137
-        },
-        {
-            "x": 5.619548385303401,
-            "y": 4.254606880959325,
-            "heading": 1.5377680494372407e-9,
-            "angularVelocity": 4.976183803494664e-16,
-            "velocityX": 3.331599885158129,
-            "velocityY": -1.7919462147497014,
-            "timestamp": 1.4094692807869067
-        },
-        {
-            "x": 5.6918607364808045,
-            "y": 4.215713168590034,
-            "heading": 1.5377680578875737e-9,
-            "angularVelocity": 3.893277832205449e-16,
-            "velocityX": 3.3316088636117085,
-            "velocityY": -1.7919295218396705,
-            "timestamp": 1.4311742149906763
-        },
-        {
-            "x": 5.764173499508255,
-            "y": 4.176820221955942,
-            "heading": 1.5377680813491443e-9,
-            "angularVelocity": 1.0809325075471762e-15,
-            "velocityX": 3.331627838562525,
-            "velocityY": -1.7918942425240987,
-            "timestamp": 1.452879149194446
-        },
-        {
-            "x": 5.8364876254997204,
-            "y": 4.137929809554028,
-            "heading": 1.5377680862134134e-9,
-            "angularVelocity": 2.241089873857745e-16,
-            "velocityX": 3.331690633686676,
-            "velocityY": -1.7917774841780256,
-            "timestamp": 1.4745840833982156
-        },
-        {
-            "x": 5.90880309835116,
-            "y": 4.099041901645272,
-            "heading": 1.537768099475084e-9,
-            "angularVelocity": 6.109980238433844e-16,
-            "velocityX": 3.3317526868576484,
-            "velocityY": -1.7916620959822775,
-            "timestamp": 1.4962890176019852
-        },
-        {
-            "x": 5.981120307623631,
-            "y": 4.060157222932383,
-            "heading": 1.5377681005691863e-9,
-            "angularVelocity": 5.040806501517701e-17,
-            "velocityX": 3.331832688069564,
-            "velocityY": -1.7915133189414525,
-            "timestamp": 1.5179939518057548
-        },
-        {
-            "x": 6.053439067813782,
-            "y": 4.0212754287411325,
-            "heading": 1.5377681034134081e-9,
-            "angularVelocity": 1.3104042715425496e-16,
-            "velocityX": 3.3319041426820193,
-            "velocityY": -1.7913804218969325,
-            "timestamp": 1.5396988860095244
-        },
-        {
-            "x": 6.1257574594958575,
-            "y": 3.9823929491454755,
-            "heading": 1.5377681141371771e-9,
-            "angularVelocity": 4.940705905043031e-16,
-            "velocityX": 3.3318871646008015,
-            "velocityY": -1.7914120001783713,
-            "timestamp": 1.561403820213294
-        },
-        {
-            "x": 6.198071250397711,
-            "y": 3.943501913620472,
-            "heading": 1.5377681379511677e-9,
-            "angularVelocity": 1.09716939803524e-15,
-            "velocityX": 3.3316751952742036,
-            "velocityY": -1.7918061930096516,
-            "timestamp": 1.5831087544170637
-        },
-        {
-            "x": 6.270375940604977,
-            "y": 3.9045939610161593,
-            "heading": 1.5377681515073034e-9,
-            "angularVelocity": 6.245647694184294e-16,
-            "velocityX": 3.331255903765101,
-            "velocityY": -1.7925856046957434,
-            "timestamp": 1.6048136886208333
-        },
-        {
-            "x": 6.342673755736347,
-            "y": 3.865673234790299,
-            "heading": 1.5377699302540744e-9,
-            "angularVelocity": 8.19513085832153e-14,
-            "velocityX": 3.3309391520146527,
-            "velocityY": -1.7931741170331514,
-            "timestamp": 1.626518622824603
-        },
-        {
-            "x": 6.414965746925861,
-            "y": 3.826741692219575,
-            "heading": 1.5377690472977265e-9,
-            "angularVelocity": -4.068002902168065e-14,
-            "velocityX": 3.3306708286121007,
-            "velocityY": -1.7936724527872456,
-            "timestamp": 1.6482235570283725
-        },
-        {
-            "x": 6.487241720022193,
-            "y": 3.7877804203169507,
-            "heading": 1.5377684535437603e-9,
-            "angularVelocity": -2.7355713192172554e-14,
-            "velocityX": 3.3299328354443367,
-            "velocityY": -1.7950421566324242,
-            "timestamp": 1.6699284912321422
-        },
-        {
-            "x": 6.559496923985796,
-            "y": 3.748780644748349,
-            "heading": 1.5377682143039968e-9,
-            "angularVelocity": -1.1022367481483692e-14,
-            "velocityX": 3.3289759501347236,
-            "velocityY": -1.796816115748624,
-            "timestamp": 1.6916334254359118
-        },
-        {
-            "x": 6.630585509558441,
-            "y": 3.7076928121475503,
-            "heading": 1.5377682448018014e-9,
-            "angularVelocity": 1.4051092264620289e-15,
-            "velocityX": 3.2752269555510436,
-            "velocityY": -1.8930180674612387,
-            "timestamp": 1.7133383596396814
-        },
-        {
-            "x": 6.6997616539527085,
-            "y": 3.6634609457782683,
-            "heading": 1.5377682789348178e-9,
-            "angularVelocity": 1.5725923809489335e-15,
-            "velocityX": 3.1871160605618085,
-            "velocityY": -2.037871479084932,
-            "timestamp": 1.735043293843451
-        },
-        {
-            "x": 6.7667761400423245,
-            "y": 3.6160177634442383,
-            "heading": 1.5377682945212807e-9,
-            "angularVelocity": 7.181069480230158e-16,
-            "velocityX": 3.0875231161945336,
-            "velocityY": -2.185824747894625,
-            "timestamp": 1.7567482280472206
-        },
-        {
-            "x": 6.831456555555867,
-            "y": 3.5654387099344507,
-            "heading": 1.5377683209709385e-9,
-            "angularVelocity": 1.2186011773240001e-15,
-            "velocityX": 2.979986712068234,
-            "velocityY": -2.330302088683456,
-            "timestamp": 1.7784531622509903
+            "heading": -0.4829717875114574,
+            "angularVelocity": -0.000001920532733427299,
+            "velocityX": 3.6086312001179417,
+            "velocityY": -1.135081104701801,
+            "timestamp": 0.9366683402900708
+        },
+        {
+            "x": 4.095489617333613,
+            "y": 5.060208566496142,
+            "heading": -0.48297174072333765,
+            "angularVelocity": 0.00000215563208981735,
+            "velocityX": 3.550230245224857,
+            "velocityY": -1.306327856847248,
+            "timestamp": 0.9583733984124503
+        },
+        {
+            "x": 4.1711135466589555,
+            "y": 5.028225843813575,
+            "heading": -0.482971694875483,
+            "angularVelocity": 0.0000021123119922022846,
+            "velocityX": 3.4841615672696387,
+            "velocityY": -1.4735147218783384,
+            "timestamp": 0.9800784565348297
+        },
+        {
+            "x": 4.245175973739862,
+            "y": 4.9927768637698655,
+            "heading": -0.48297164958118555,
+            "angularVelocity": 0.0000020868083942836762,
+            "velocityX": 3.4122197076519116,
+            "velocityY": -1.633212859593547,
+            "timestamp": 1.0017835146572092
+        },
+        {
+            "x": 4.317835043195099,
+            "y": 4.954533786792328,
+            "heading": -0.4829716044969262,
+            "angularVelocity": 0.0000020771314744606885,
+            "velocityX": 3.3475639201504173,
+            "velocityY": -1.7619430808199938,
+            "timestamp": 1.0234885727795886
+        },
+        {
+            "x": 4.39018173013391,
+            "y": 4.915702997439957,
+            "heading": -0.48297155943721365,
+            "angularVelocity": 0.000002076000546478681,
+            "velocityX": 3.333171767193961,
+            "velocityY": -1.789020288884605,
+            "timestamp": 1.045193630901968
+        },
+        {
+            "x": 4.462524157787397,
+            "y": 4.876864273483723,
+            "heading": -0.4829715143782692,
+            "angularVelocity": 0.000002075965162802355,
+            "velocityX": 3.3329755325063237,
+            "velocityY": -1.7893858536194671,
+            "timestamp": 1.0668986890243475
+        },
+        {
+            "x": 4.534839835738717,
+            "y": 4.8379757656949245,
+            "heading": -0.48297146931947993,
+            "angularVelocity": 0.0000020759580074306272,
+            "velocityX": 3.331743114605303,
+            "velocityY": -1.7916795048180625,
+            "timestamp": 1.088603747146727
+        },
+        {
+            "x": 4.60713879668486,
+            "y": 4.7990561876016065,
+            "heading": -0.48297142426091105,
+            "angularVelocity": 0.000002075947856864141,
+            "velocityX": 3.3309729252282607,
+            "velocityY": -1.793110982419974,
+            "timestamp": 1.1103088052691064
+        },
+        {
+            "x": 4.679445820973998,
+            "y": 4.760151592099993,
+            "heading": -0.48297137920221006,
+            "angularVelocity": 0.0000020759539417382525,
+            "velocityX": 3.331344421260875,
+            "velocityY": -1.7924207013070022,
+            "timestamp": 1.1320138633914858
+        },
+        {
+            "x": 4.75175835900604,
+            "y": 4.72125724602898,
+            "heading": -0.4829713341434503,
+            "angularVelocity": 0.000002075956651977989,
+            "velocityX": 3.3315984515826855,
+            "velocityY": -1.791948487386363,
+            "timestamp": 1.1537189215138652
+        },
+        {
+            "x": 4.824072564767644,
+            "y": 4.68236600077068,
+            "heading": -0.48297128908467135,
+            "angularVelocity": 0.0000020759575350386475,
+            "velocityX": 3.331675287569862,
+            "velocityY": -1.7918056260909452,
+            "timestamp": 1.1754239796362447
+        },
+        {
+            "x": 4.8963869623773055,
+            "y": 4.643475112237639,
+            "heading": -0.48297124402589003,
+            "angularVelocity": 0.000002075957644932215,
+            "velocityX": 3.3316841264339563,
+            "velocityY": -1.79178919096958,
+            "timestamp": 1.1971290377586241
+        },
+        {
+            "x": 4.968701267365325,
+            "y": 4.604584051483172,
+            "heading": -0.4829711989671097,
+            "angularVelocity": 0.000002075957594698055,
+            "velocityX": 3.331679859150302,
+            "velocityY": -1.7917971255911598,
+            "timestamp": 1.2188340958810036
+        },
+        {
+            "x": 5.0410154847403765,
+            "y": 4.565692827821048,
+            "heading": -0.4829711539083305,
+            "angularVelocity": 0.0000020759575472868184,
+            "velocityX": 3.331675822627313,
+            "velocityY": -1.7918046311069282,
+            "timestamp": 1.240539154003383
+        },
+        {
+            "x": 5.113329654064444,
+            "y": 4.526801514813212,
+            "heading": -0.4829711088495519,
+            "angularVelocity": 0.000002075957520958662,
+            "velocityX": 3.331673608812237,
+            "velocityY": -1.7918087474613025,
+            "timestamp": 1.2622442121257624
+        },
+        {
+            "x": 5.185643801945598,
+            "y": 4.487910161934636,
+            "heading": -0.48297106379077354,
+            "angularVelocity": 0.0000020759575090327586,
+            "velocityX": 3.3316726208897984,
+            "velocityY": -1.7918105843944965,
+            "timestamp": 1.2839492702481419
+        },
+        {
+            "x": 5.257957938797454,
+            "y": 4.4490187885483135,
+            "heading": -0.48297101873199527,
+            "angularVelocity": 0.0000020759575026231074,
+            "velocityX": 3.331672112745739,
+            "velocityY": -1.7918115292316654,
+            "timestamp": 1.3056543283705213
+        },
+        {
+            "x": 5.330272065176824,
+            "y": 4.410127395689581,
+            "heading": -0.4829709736732172,
+            "angularVelocity": 0.000002075957497099591,
+            "velocityX": 3.3316716302550873,
+            "velocityY": -1.7918124263686461,
+            "timestamp": 1.3273593864929008
+        },
+        {
+            "x": 5.402586187963667,
+            "y": 4.371235996150952,
+            "heading": -0.4829709286144391,
+            "angularVelocity": 0.00000207595749402331,
+            "velocityX": 3.3316714647394585,
+            "velocityY": -1.7918127341261798,
+            "timestamp": 1.3490644446152802
+        },
+        {
+            "x": 5.474900315939471,
+            "y": 4.332344606260608,
+            "heading": -0.482970883555661,
+            "angularVelocity": 0.0000020759574957870433,
+            "velocityX": 3.331671703806323,
+            "velocityY": -1.7918122896084423,
+            "timestamp": 1.3707695027376596
+        },
+        {
+            "x": 5.547214451711641,
+            "y": 4.293453230866729,
+            "heading": -0.4829708384968828,
+            "angularVelocity": 0.0000020759574997718594,
+            "velocityX": 3.331672063002153,
+            "velocityY": -1.7918116217242377,
+            "timestamp": 1.392474560860039
+        },
+        {
+            "x": 5.619528591620017,
+            "y": 4.254561863163663,
+            "heading": -0.4829707934381046,
+            "angularVelocity": 0.0000020759575011693265,
+            "velocityX": 3.3316722535663335,
+            "velocityY": -1.7918112673914555,
+            "timestamp": 1.4141796189824185
+        },
+        {
+            "x": 5.6918427353771435,
+            "y": 4.215670502616919,
+            "heading": -0.4829707483793263,
+            "angularVelocity": 0.000002075957502624911,
+            "velocityX": 3.3316724308867816,
+            "velocityY": -1.7918109376838844,
+            "timestamp": 1.435884677104798
+        },
+        {
+            "x": 5.764156898177578,
+            "y": 4.176779177479102,
+            "heading": -0.4829707033205479,
+            "angularVelocity": 0.0000020759575111736717,
+            "velocityX": 3.3316733082540817,
+            "velocityY": -1.7918093063163627,
+            "timestamp": 1.4575897352271774
+        },
+        {
+            "x": 5.836471080177112,
+            "y": 4.137887888039928,
+            "heading": -0.48297065826176927,
+            "angularVelocity": 0.0000020759575207599675,
+            "velocityX": 3.331674192798964,
+            "velocityY": -1.7918076616010294,
+            "timestamp": 1.4792947933495568
+        },
+        {
+            "x": 5.908785251268908,
+            "y": 4.098996578318978,
+            "heading": -0.48297061320299073,
+            "angularVelocity": 0.0000020759575149169034,
+            "velocityX": 3.331673690255448,
+            "velocityY": -1.7918085960272527,
+            "timestamp": 1.5009998514719363
+        },
+        {
+            "x": 5.981099339043632,
+            "y": 4.0601051136793025,
+            "heading": -0.48297056814421313,
+            "angularVelocity": 0.000002075957470564626,
+            "velocityX": 3.3316698516536873,
+            "velocityY": -1.7918157334753977,
+            "timestamp": 1.5227049095943157
+        },
+        {
+            "x": 6.053413282760847,
+            "y": 4.021213381182831,
+            "heading": -0.48297052308543725,
+            "angularVelocity": 0.00000207595739406405,
+            "velocityX": 3.3316632146058507,
+            "velocityY": -1.7918280742300665,
+            "timestamp": 1.5444099677166951
+        },
+        {
+            "x": 6.125727369888532,
+            "y": 3.982321915340118,
+            "heading": -0.4829704780266597,
+            "angularVelocity": 0.0000020759574679524,
+            "velocityX": 3.3316698218431826,
+            "velocityY": -1.7918157889019397,
+            "timestamp": 1.5661150258390746
+        },
+        {
+            "x": 6.198042995964267,
+            "y": 3.9434333111252817,
+            "heading": -0.4829704329678644,
+            "angularVelocity": 0.000002075958288103883,
+            "velocityX": 3.3317407245813113,
+            "velocityY": -1.7916839473808401,
+            "timestamp": 1.587820083961454
+        },
+        {
+            "x": 6.270359121166566,
+            "y": 3.9045456350763237,
+            "heading": -0.4829703879090624,
+            "angularVelocity": 0.0000020759585995923127,
+            "velocityX": 3.331763720444871,
+            "velocityY": -1.791641184728093,
+            "timestamp": 1.6095251420838335
+        },
+        {
+            "x": 6.342677550055512,
+            "y": 3.8656622432711547,
+            "heading": -0.48297034285023976,
+            "angularVelocity": 0.0000020759595452668008,
+            "velocityX": 3.331869856379567,
+            "velocityY": -1.791443800149147,
+            "timestamp": 1.631230200206213
+        },
+        {
+            "x": 6.414977737835259,
+            "y": 3.8267449442462484,
+            "heading": -0.4829702977916256,
+            "angularVelocity": 0.0000020759499429522823,
+            "velocityX": 3.3310294481622025,
+            "velocityY": -1.7930059807042389,
+            "timestamp": 1.6529352583285923
+        },
+        {
+            "x": 6.487268603977522,
+            "y": 3.787810332568674,
+            "heading": -0.4829702527330872,
+            "angularVelocity": 0.00000207594645249869,
+            "velocityX": 3.330599979721225,
+            "velocityY": -1.793803612876013,
+            "timestamp": 1.6746403164509718
+        },
+        {
+            "x": 6.559543297837492,
+            "y": 3.7488457083598847,
+            "heading": -0.4829702076720725,
+            "angularVelocity": 0.000002076060537305802,
+            "velocityX": 3.3298548869330626,
+            "velocityY": -1.7951863565208734,
+            "timestamp": 1.6963453745733512
+        },
+        {
+            "x": 6.630628961599716,
+            "y": 3.7077518923991635,
+            "heading": -0.48297016252441444,
+            "angularVelocity": 0.000002080052392839484,
+            "velocityX": 3.2750736423474676,
+            "velocityY": -1.893282926450661,
+            "timestamp": 1.7180504326957307
+        },
+        {
+            "x": 6.699799614538253,
+            "y": 3.6635105764693066,
+            "heading": -0.4829701170038858,
+            "angularVelocity": 0.0000020972313632639956,
+            "velocityX": 3.186844861162469,
+            "velocityY": -2.0382952066016227,
+            "timestamp": 1.73975549081811
+        },
+        {
+            "x": 6.766806665010948,
+            "y": 3.616056089090901,
+            "heading": -0.48297007073382403,
+            "angularVelocity": 0.000002131764013783056,
+            "velocityX": 3.0871629135886995,
+            "velocityY": -2.1863331169555953,
+            "timestamp": 1.7614605489404895
+        },
+        {
+            "x": 6.831474779993965,
+            "y": 3.565460555976799,
+            "heading": -0.48297002329923955,
+            "angularVelocity": 0.0000021854161466442962,
+            "velocityX": 2.9794029860873867,
+            "velocityY": -2.331048036306895,
+            "timestamp": 1.783165607062869
         },
         {
             "x": 6.893649101257324,
             "y": 3.5118300914764404,
-            "heading": 1.5377683531646926e-9,
-            "angularVelocity": 1.4832457732424108e-15,
-            "velocityX": 2.8653643967583453,
-            "velocityY": -2.4698816386502185,
-            "timestamp": 1.8001580964547599
-        },
-        {
-            "x": 6.934219508494461,
-            "y": 3.4745268434152994,
-            "heading": 1.5377683787984002e-9,
-            "angularVelocity": 1.7594754613333528e-15,
-            "velocityX": 2.784717658151537,
-            "velocityY": -2.56046267850036,
-            "timestamp": 1.8147270446274342
-        },
-        {
-            "x": 6.973574535692003,
-            "y": 3.4359435289599864,
-            "heading": 1.5377684019331704e-9,
-            "angularVelocity": 1.5879505990145664e-15,
-            "velocityX": 2.701295023573416,
-            "velocityY": -2.648325328500865,
-            "timestamp": 1.8292959928001085
-        },
-        {
-            "x": 7.011676190859291,
-            "y": 3.3961219990562412,
-            "heading": 1.5377684253031837e-9,
-            "angularVelocity": 1.6040974952296623e-15,
-            "velocityX": 2.6152646516208327,
-            "velocityY": -2.733315365788167,
-            "timestamp": 1.8438649409727828
-        },
-        {
-            "x": 7.048490476455465,
-            "y": 3.3551073823105697,
-            "heading": 1.5377684383832216e-9,
-            "angularVelocity": 8.97802495662944e-16,
-            "velocityX": 2.5269007178724983,
-            "velocityY": -2.815207814562461,
-            "timestamp": 1.858433889145457
-        },
-        {
-            "x": 7.083991390306391,
-            "y": 3.3129508222340074,
-            "heading": 1.5377684557448877e-9,
-            "angularVelocity": 1.191689740876308e-15,
-            "velocityX": 2.4367520173841823,
-            "velocityY": -2.893589816979968,
-            "timestamp": 1.8730028373181313
-        },
-        {
-            "x": 7.118172550776329,
-            "y": 3.269717285105602,
-            "heading": 1.5377684739509254e-9,
-            "angularVelocity": 1.2496467044902449e-15,
-            "velocityX": 2.346165286939037,
-            "velocityY": -2.9675125902013777,
-            "timestamp": 1.8875717854908056
-        },
-        {
-            "x": 7.15108313208807,
-            "y": 3.2255088683196913,
-            "heading": 1.5377684882032233e-9,
-            "angularVelocity": 9.782653941580851e-16,
-            "velocityX": 2.2589538326090746,
-            "velocityY": -3.0344274865927217,
-            "timestamp": 1.90214073366348
-        },
-        {
-            "x": 7.182924606349684,
-            "y": 3.1805243042550315,
-            "heading": 1.5377697666399164e-9,
-            "angularVelocity": 8.775078931151136e-14,
-            "velocityX": 2.185571249497722,
-            "velocityY": -3.0877015644156707,
-            "timestamp": 1.9167096818361542
-        },
-        {
-            "x": 7.2139811373084415,
-            "y": 3.1352968072431113,
-            "heading": 1.2915519070501347e-9,
-            "angularVelocity": -1.6900181039231953e-8,
-            "velocityX": 2.131693420188762,
-            "velocityY": -3.1043762717714163,
-            "timestamp": 1.9312786300088285
-        },
-        {
-            "x": 7.244040263059187,
-            "y": 3.0915312465009275,
-            "heading": 1.1211375399666565e-9,
-            "angularVelocity": -1.169709485282711e-8,
-            "velocityX": 2.063232389495776,
-            "velocityY": -3.0040302308350935,
-            "timestamp": 1.9458475781815028
-        },
-        {
-            "x": 7.2730994599044765,
-            "y": 3.0492259199621516,
-            "heading": 9.847245150980228e-10,
-            "angularVelocity": -9.363272118309568e-9,
-            "velocityX": 1.9945981343934114,
-            "velocityY": -2.903801018259091,
-            "timestamp": 1.960416526354177
-        },
-        {
-            "x": 7.301157878524098,
-            "y": 3.0083802533354898,
-            "heading": 8.698146081553592e-10,
-            "angularVelocity": -7.887316693614981e-9,
-            "velocityX": 1.9259055826862927,
-            "velocityY": -2.803611224540764,
-            "timestamp": 1.9749854745268514
-        },
-        {
-            "x": 7.328215092540324,
-            "y": 2.968993958016925,
-            "heading": 7.704962597777701e-10,
-            "angularVelocity": -6.8171255926008325e-9,
-            "velocityX": 1.857183764780944,
-            "velocityY": -2.7034412403524777,
-            "timestamp": 1.9895544226995256
-        },
-        {
-            "x": 7.354270845617658,
-            "y": 2.9310668604072108,
-            "heading": 6.833248169018587e-10,
-            "angularVelocity": -5.983372499761897e-9,
-            "velocityX": 1.7884443522287568,
-            "velocityY": -2.6032831718663054,
-            "timestamp": 2.0041233708722
-        },
-        {
-            "x": 7.379324966656107,
-            "y": 2.894598844592948,
-            "heading": 6.060446454340774e-10,
-            "angularVelocity": -5.304444180973637e-9,
-            "velocityX": 1.719693195521089,
-            "velocityY": -2.5031330595756027,
-            "timestamp": 2.0186923190448742
-        },
-        {
-            "x": 7.403377333338156,
-            "y": 2.8595898276899736,
-            "heading": 5.370639718880826e-10,
-            "angularVelocity": -4.734773818100124e-9,
-            "velocityX": 1.6509336430450512,
-            "velocityY": -2.4029886363820543,
-            "timestamp": 2.0332612672175485
-        },
-        {
-            "x": 7.426427853868939,
-            "y": 2.8260397474863512,
-            "heading": 4.75199340983144e-10,
-            "angularVelocity": -4.246334802521844e-9,
-            "velocityX": 1.582167789848791,
-            "velocityY": -2.3028484833621037,
-            "timestamp": 2.047830215390223
-        },
-        {
-            "x": 7.44847645681879,
-            "y": 2.79394855556605,
-            "heading": 4.195363288341646e-10,
-            "angularVelocity": -3.820661051835768e-9,
-            "velocityX": 1.5133970337819913,
-            "velocityY": -2.202711653576578,
-            "timestamp": 2.062399163562897
-        },
-        {
-            "x": 7.469523085022814,
-            "y": 2.7633162131780225,
-            "heading": 3.6934715040819367e-10,
-            "angularVelocity": -3.4449418365911004e-9,
-            "velocityX": 1.4446223539663485,
-            "velocityY": -2.1025774836294286,
-            "timestamp": 2.0769681117355714
-        },
-        {
-            "x": 7.489567691695811,
-            "y": 2.7341426886048597,
-            "heading": 3.2403879933557077e-10,
-            "angularVelocity": -3.1099260658641154e-9,
-            "velocityX": 1.3758444628549362,
-            "velocityY": -2.002445490737794,
-            "timestamp": 2.0915370599082457
-        },
-        {
-            "x": 7.5086102378406,
-            "y": 2.7064279554071735,
-            "heading": 2.831187733993615e-10,
-            "angularVelocity": -2.8087152215039624e-9,
-            "velocityX": 1.3070638950107798,
-            "velocityY": -1.9023153126229018,
-            "timestamp": 2.10610600808092
-        },
-        {
-            "x": 7.52665069045286,
-            "y": 2.680171991207375,
-            "heading": 2.4617151569169827e-10,
-            "angularVelocity": -2.536027844020545e-9,
-            "velocityX": 1.2382810617787536,
-            "velocityY": -1.8021866704863527,
-            "timestamp": 2.1206749562535943
-        },
-        {
-            "x": 7.543689021238326,
-            "y": 2.65537477682047,
-            "heading": 2.128416865029733e-10,
-            "angularVelocity": -2.287730676987115e-9,
-            "velocityX": 1.169496286452695,
-            "velocityY": -1.7020593451912265,
-            "timestamp": 2.1352439044262685
-        },
-        {
-            "x": 7.559725205671708,
-            "y": 2.6320362956163437,
-            "heading": 1.8282195651887185e-10,
-            "angularVelocity": -2.0605283283789865e-9,
-            "velocityX": 1.1007098277319451,
-            "velocityY": -1.6019331613726957,
-            "timestamp": 2.149812852598943
-        },
-        {
-            "x": 7.5747592222906635,
-            "y": 2.6101565330412897,
-            "heading": 1.558438913168689e-10,
-            "angularVelocity": -1.8517510864510685e-9,
-            "velocityX": 1.0319218958546759,
-            "velocityY": -1.5018079765081174,
-            "timestamp": 2.164381800771617
-        },
-        {
-            "x": 7.588791052155751,
-            "y": 2.5897354762519833,
-            "heading": 1.3167100643601026e-10,
-            "angularVelocity": -1.6592059334641093e-9,
-            "velocityX": 0.9631326639903722,
-            "velocityY": -1.4016836731981617,
-            "timestamp": 2.1789507489442914
-        },
-        {
-            "x": 7.601820678430282,
-            "y": 2.570773113830717,
-            "heading": 1.100933830131512e-10,
-            "angularVelocity": -1.4810694297523211e-9,
-            "velocityX": 0.894342276470416,
-            "velocityY": -1.3015601535897774,
-            "timestamp": 2.1935196971169657
-        },
-        {
-            "x": 7.613848086048565,
-            "y": 2.5532694355605217,
-            "heading": 9.092342750630814e-11,
-            "angularVelocity": -1.315809184573053e-9,
-            "velocityX": 0.8255508548544661,
-            "velocityY": -1.201437335265309,
-            "timestamp": 2.20808864528964
-        },
-        {
-            "x": 7.624873261450447,
-            "y": 2.537224432245223,
-            "heading": 7.399249133993465e-11,
-            "angularVelocity": -1.1621248318093622e-9,
-            "velocityX": 0.7567585024813989,
-            "velocityY": -1.101315148158345,
-            "timestamp": 2.2226575934623143
-        },
-        {
-            "x": 7.634896192366388,
-            "y": 2.5226380955637384,
-            "heading": 5.914813867727886e-11,
-            "angularVelocity": -1.0189035430874983e-9,
-            "velocityX": 0.6879653079374735,
-            "velocityY": -1.0011935322032168,
-            "timestamp": 2.2372265416349886
-        },
-        {
-            "x": 7.643916867641572,
-            "y": 2.5095104179508487,
-            "heading": 4.6251914688462445e-11,
-            "angularVelocity": -8.851856741832805e-10,
-            "velocityX": 0.6191713477368228,
-            "velocityY": -0.9010724355182365,
-            "timestamp": 2.251795489807663
-        },
-        {
-            "x": 7.651935277090606,
-            "y": 2.4978413924986826,
-            "heading": 3.517750680608747e-11,
-            "angularVelocity": -7.601377906979457e-10,
-            "velocityX": 0.5503766884196106,
-            "velocityY": -0.8009518129835539,
-            "timestamp": 2.266364437980337
-        },
-        {
-            "x": 7.658951411376412,
-            "y": 2.487631012874611,
-            "heading": 2.580921448386634e-11,
-            "angularVelocity": -6.43031492677376e-10,
-            "velocityX": 0.48158138821334623,
-            "velocityY": -0.7008316251150604,
-            "timestamp": 2.2809333861530114
-        },
-        {
-            "x": 7.664965261908482,
-            "y": 2.4788792732522666,
-            "heading": 1.804066569533592e-11,
-            "angularVelocity": -5.332264785860361e-10,
-            "velocityX": 0.41278549836213546,
-            "velocityY": -0.6007118371633381,
-            "timestamp": 2.2955023343256857
-        },
-        {
-            "x": 7.669976820756786,
-            "y": 2.4715861682531717,
-            "heading": 1.1773732592627752e-11,
-            "angularVelocity": -4.3015687513290903e-10,
-            "velocityX": 0.34398906420045333,
-            "velocityY": -0.500592418385764,
-            "timestamp": 2.31007128249836
-        },
-        {
-            "x": 7.67398608057842,
-            "y": 2.465751692897001,
-            "heading": 6.917607828867898e-12,
-            "angularVelocity": -3.3332020945886676e-10,
-            "velocityX": 0.27519212602817883,
-            "velocityY": -0.40047334145331454,
-            "timestamp": 2.3246402306710343
-        },
-        {
-            "x": 7.676993034554729,
-            "y": 2.461375842558954,
-            "heading": 3.3880167119093778e-12,
-            "angularVelocity": -2.4226808579143967e-10,
-            "velocityX": 0.20639471982945568,
-            "velocityY": -0.30035458196322756,
-            "timestamp": 2.3392091788437086
-        },
-        {
-            "x": 7.678997676337105,
-            "y": 2.4584586129329957,
-            "heading": 1.1065347655527578e-12,
-            "angularVelocity": -1.56598950365953e-10,
-            "velocityX": 0.13759687786767488,
-            "velocityY": -0.2002361180356331,
-            "timestamp": 2.353778127016383
+            "heading": -0.48296997476148124,
+            "angularVelocity": 0.00000223624180421772,
+            "velocityX": 2.8645083976649595,
+            "velocityY": -2.470874033046681,
+            "timestamp": 1.8048706651852484
+        },
+        {
+            "x": 6.934561334359113,
+            "y": 3.474160479757131,
+            "heading": -0.4829699264500687,
+            "angularVelocity": 0.0000032862628672599958,
+            "velocityX": 2.782952215912443,
+            "velocityY": -2.5623810156238895,
+            "timestamp": 1.8195716840772946
+        },
+        {
+            "x": 6.974231870495054,
+            "y": 3.435185370851295,
+            "heading": -0.4829698798888691,
+            "angularVelocity": 0.0000031672090196575476,
+            "velocityX": 2.6984888889168737,
+            "velocityY": -2.6511841928806597,
+            "timestamp": 1.8342727029693409
+        },
+        {
+            "x": 7.012622614846585,
+            "y": 3.394949051793438,
+            "heading": -0.482969834780297,
+            "angularVelocity": 0.0000030683976682796836,
+            "velocityX": 2.611434257274787,
+            "velocityY": -2.736974855506367,
+            "timestamp": 1.848973721861387
+        },
+        {
+            "x": 7.049699862488227,
+            "y": 3.3534992220248374,
+            "heading": -0.4829697908553284,
+            "angularVelocity": 0.0000029878860039287864,
+            "velocityX": 2.5220869324714577,
+            "velocityY": -2.8195208830745364,
+            "timestamp": 1.8636747407534333
+        },
+        {
+            "x": 7.085438739774718,
+            "y": 3.310890028476691,
+            "heading": -0.48296974786836877,
+            "angularVelocity": 0.0000029240802913465185,
+            "velocityX": 2.4310476402306533,
+            "velocityY": -2.8983837012274143,
+            "timestamp": 1.8783757596454795
+        },
+        {
+            "x": 7.119836677140904,
+            "y": 3.2671911422114968,
+            "heading": -0.4829697055890564,
+            "angularVelocity": 0.0000028759443619428334,
+            "velocityX": 2.339833559754191,
+            "velocityY": -2.9725073198044742,
+            "timestamp": 1.8930767785375258
+        },
+        {
+            "x": 7.1529563800127525,
+            "y": 3.222515689615774,
+            "heading": -0.48296966398118896,
+            "angularVelocity": 0.000002830270996857658,
+            "velocityX": 2.2528848588700137,
+            "velocityY": -3.0389357991976094,
+            "timestamp": 1.907777797429572
+        },
+        {
+            "x": 7.184894346745796,
+            "y": 3.177381185858669,
+            "heading": -0.4821430167851757,
+            "angularVelocity": 0.056230605652804695,
+            "velocityX": 2.17250021699707,
+            "velocityY": -3.0701616050246474,
+            "timestamp": 1.9224788163216182
+        },
+        {
+            "x": 7.2158791345376,
+            "y": 3.1333943550888805,
+            "heading": -0.47750834116458535,
+            "angularVelocity": 0.31526220424747137,
+            "velocityX": 2.107662606199986,
+            "velocityY": -2.992094023740475,
+            "timestamp": 1.9371798352136644
+        },
+        {
+            "x": 7.245895346179036,
+            "y": 3.090550131820991,
+            "heading": -0.46908945700490695,
+            "angularVelocity": 0.5726735147747635,
+            "velocityX": 2.041777638805421,
+            "velocityY": -2.9143710094182476,
+            "timestamp": 1.9518808541057107
+        },
+        {
+            "x": 7.274933319615603,
+            "y": 3.048846636162261,
+            "heading": -0.45692051225089847,
+            "angularVelocity": 0.827761996863506,
+            "velocityX": 1.9752354343465999,
+            "velocityY": -2.8367758700924766,
+            "timestamp": 1.9665818729977569
+        },
+        {
+            "x": 7.3029845213490745,
+            "y": 3.008282422680816,
+            "heading": -0.4410407652781448,
+            "angularVelocity": 1.08018002625281,
+            "velocityX": 1.90811276003785,
+            "velocityY": -2.7592790526506894,
+            "timestamp": 1.981282891889803
+        },
+        {
+            "x": 7.330040747407425,
+            "y": 2.9688555797343485,
+            "heading": -0.42149121219843927,
+            "angularVelocity": 1.3298093977882255,
+            "velocityX": 1.8404320310743014,
+            "velocityY": -2.6819122698903146,
+            "timestamp": 1.9959839107818493
+        },
+        {
+            "x": 7.356112777651364,
+            "y": 2.9307983654229495,
+            "heading": -0.3996724931243264,
+            "angularVelocity": 1.4841637327544344,
+            "velocityX": 1.7734845751435842,
+            "velocityY": -2.5887467114262686,
+            "timestamp": 2.0106849296738956
+        },
+        {
+            "x": 7.38118332940277,
+            "y": 2.8941879380599844,
+            "heading": -0.37646864699164706,
+            "angularVelocity": 1.578383532670194,
+            "velocityX": 1.7053615083080305,
+            "velocityY": -2.4903326518935414,
+            "timestamp": 2.025385948565942
+        },
+        {
+            "x": 7.40523963577506,
+            "y": 2.8590504496296347,
+            "heading": -0.3524751392807844,
+            "angularVelocity": 1.6320982842790503,
+            "velocityX": 1.6363700059799433,
+            "velocityY": -2.390139669119207,
+            "timestamp": 2.040086967457988
+        },
+        {
+            "x": 7.428276340738969,
+            "y": 2.825395406477394,
+            "heading": -0.3281233875567818,
+            "angularVelocity": 1.656466936259613,
+            "velocityX": 1.5670141731720877,
+            "velocityY": -2.289300040995877,
+            "timestamp": 2.0547879863500342
+        },
+        {
+            "x": 7.450292281624529,
+            "y": 2.793225377698084,
+            "heading": -0.3037369697425905,
+            "angularVelocity": 1.658825010243686,
+            "velocityX": 1.497579252650967,
+            "velocityY": -2.1882856566299744,
+            "timestamp": 2.0694890052420805
+        },
+        {
+            "x": 7.471288319158757,
+            "y": 2.7625397684205146,
+            "heading": -0.27956516086770095,
+            "angularVelocity": 1.6442267745106667,
+            "velocityX": 1.4282028809299567,
+            "velocityY": -2.0873117368873455,
+            "timestamp": 2.0841900241341267
+        },
+        {
+            "x": 7.491266209557209,
+            "y": 2.733336527235843,
+            "heading": -0.2558043926700512,
+            "angularVelocity": 1.6162667616531794,
+            "velocityX": 1.3589459713748013,
+            "velocityY": -1.9864773590946931,
+            "timestamp": 2.098891043026173
+        },
+        {
+            "x": 7.510228057377649,
+            "y": 2.7056129838220437,
+            "heading": -0.2326125564520332,
+            "angularVelocity": 1.577566588297197,
+            "velocityX": 1.28983221909181,
+            "velocityY": -1.885824623271541,
+            "timestamp": 2.113592061918219
+        },
+        {
+            "x": 7.528176060430696,
+            "y": 2.679366275566135,
+            "heading": -0.21011884554685348,
+            "angularVelocity": 1.5300783619392286,
+            "velocityX": 1.2208679673730278,
+            "velocityY": -1.7853666095287006,
+            "timestamp": 2.1282930808102654
+        },
+        {
+            "x": 7.545112396890263,
+            "y": 2.654593565371254,
+            "heading": -0.18843070859149175,
+            "angularVelocity": 1.4752812110931899,
+            "velocityX": 1.152051880480808,
+            "velocityY": -1.6851015821961506,
+            "timestamp": 2.1429940997023116
+        },
+        {
+            "x": 7.561039180049459,
+            "y": 2.631292148556631,
+            "heading": -0.1676388680105614,
+            "angularVelocity": 1.4143128944742427,
+            "velocityX": 1.0833795450607198,
+            "velocityY": -1.5850205340005987,
+            "timestamp": 2.157695118594358
+        },
+        {
+            "x": 7.575958444025175,
+            "y": 2.6094594995019675,
+            "heading": -0.14782100448684538,
+            "angularVelocity": 1.3480605439149753,
+            "velocityX": 1.0148455753489192,
+            "velocityY": -1.485111284801969,
+            "timestamp": 2.172396137486404
+        },
+        {
+            "x": 7.589872142500143,
+            "y": 2.5890932855015207,
+            "heading": -0.12904449811203259,
+            "angularVelocity": 1.2772248313327248,
+            "velocityX": 0.9464445000131985,
+            "velocityY": -1.3853607120705473,
+            "timestamp": 2.1870971563784503
+        },
+        {
+            "x": 7.602782151943804,
+            "y": 2.570191363211945,
+            "heading": -0.11136848807718236,
+            "angularVelocity": 1.2023663233582889,
+            "velocityX": 0.8781710668125251,
+            "velocityY": -1.2857559349035097,
+            "timestamp": 2.2017981752704965
+        },
+        {
+            "x": 7.614690275426409,
+            "y": 2.552751766479238,
+            "heading": -0.09484543137092857,
+            "angularVelocity": 1.1239395600799826,
+            "velocityX": 0.8100202829510676,
+            "velocityY": -1.186284900439311,
+            "timestamp": 2.2164991941625427
+        },
+        {
+            "x": 7.62559824546286,
+            "y": 2.536772690588083,
+            "heading": -0.07952228805027751,
+            "angularVelocity": 1.042318456507909,
+            "velocityX": 0.7419873490775946,
+            "velocityY": -1.086936627215761,
+            "timestamp": 2.231200213054589
+        },
+        {
+            "x": 7.635507725447758,
+            "y": 2.522252475791086,
+            "heading": -0.06544142530276546,
+            "angularVelocity": 0.9578154310876013,
+            "velocityX": 0.6740675634570735,
+            "velocityY": -0.9877012541528096,
+            "timestamp": 2.245901231946635
+        },
+        {
+            "x": 7.644420309753795,
+            "y": 2.509189591670685,
+            "heading": -0.05264130826624494,
+            "angularVelocity": 0.8706959109783889,
+            "velocityX": 0.6062562310465265,
+            "velocityY": -0.8885699839124228,
+            "timestamp": 2.2606022508386814
+        },
+        {
+            "x": 7.652337522761704,
+            "y": 2.497582623097395,
+            "heading": -0.04115702850586921,
+            "angularVelocity": 0.7811893750159932,
+            "velocityX": 0.538548590818527,
+            "velocityY": -0.7895349743118973,
+            "timestamp": 2.2753032697307276
+        },
+        {
+            "x": 7.6592608171290975,
+            "y": 2.487430258069545,
+            "heading": -0.031020708755951136,
+            "angularVelocity": 0.6894977704846257,
+            "velocityX": 0.4709397639872257,
+            "velocityY": -0.6905892103399419,
+            "timestamp": 2.290004288622774
+        },
+        {
+            "x": 7.665191571570965,
+            "y": 2.4787312774326344,
+            "heading": -0.022261813501037925,
+            "angularVelocity": 0.5958019181685587,
+            "velocityX": 0.40342472079099084,
+            "velocityY": -0.5917263763001924,
+            "timestamp": 2.30470530751482
+        },
+        {
+            "x": 7.670131088361328,
+            "y": 2.4714845463094046,
+            "heading": -0.014907388223763604,
+            "angularVelocity": 0.5002663646159557,
+            "velocityX": 0.33599826152420176,
+            "velocityY": -0.49294073944433947,
+            "timestamp": 2.3194063264068663
+        },
+        {
+            "x": 7.674080590697013,
+            "y": 2.465689006980458,
+            "heading": -0.008982245050778671,
+            "angularVelocity": 0.40304302827545885,
+            "velocityX": 0.26865500715889973,
+            "velocityY": -0.3942270513020354,
+            "timestamp": 2.3341073452989125
+        },
+        {
+            "x": 7.677041220003002,
+            "y": 2.461343672911727,
+            "heading": -0.0045091086563273375,
+            "angularVelocity": 0.304273902870198,
+            "velocityX": 0.20138939536970257,
+            "velocityY": -0.29558046966947404,
+            "timestamp": 2.3488083641909587
+        },
+        {
+            "x": 7.679014033209882,
+            "y": 2.458447623611078,
+            "heading": -0.00150873334272897,
+            "angularVelocity": 0.20409301801671295,
+            "velocityX": 0.1341956786374654,
+            "velocityY": -0.1969965022095263,
+            "timestamp": 2.363509383083005
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": -3.441813048880529e-34,
-            "angularVelocity": -7.595159066564751e-11,
-            "velocityX": 0.06879862918134139,
-            "velocityY": -0.10011792997737116,
-            "timestamp": 2.368347075189057
+            "heading": -1.1328442579469643e-34,
+            "angularVelocity": 0.10262780789604178,
+            "velocityX": 0.06706792211866815,
+            "velocityY": -0.0984709714141921,
+            "timestamp": 2.378210401975051
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": -3.481182114535467e-34,
-            "angularVelocity": -2.7023024138211624e-34,
-            "velocityX": -3.7878173595367446e-32,
-            "velocityY": -1.5253102288533056e-31,
-            "timestamp": 2.3829160233617315
+            "heading": -2.155134012002005e-33,
+            "angularVelocity": -1.3889170145055665e-31,
+            "velocityX": 1.4892305241424507e-32,
+            "velocityY": 6.576507695252416e-33,
+            "timestamp": 2.3929114208670974
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.traj
index f95c4186..1f520266 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.traj	
@@ -3,1091 +3,1091 @@
         {
             "x": 1.35,
             "y": 5.56,
-            "heading": 4.408218114222724e-33,
+            "heading": -1.9296664556128306e-33,
             "angularVelocity": 0,
-            "velocityX": 1.8935205228973059e-34,
-            "velocityY": 9.513896878368867e-32,
+            "velocityX": -7.813211441980168e-34,
+            "velocityY": -2.2407271587906947e-32,
             "timestamp": 0
         },
         {
-            "x": 1.3544641547566925,
-            "y": 5.559252486403593,
-            "heading": 5.298729726015063e-12,
-            "angularVelocity": 2.274228720676412e-10,
-            "velocityX": 0.1916026983161006,
-            "velocityY": -0.0320834805032552,
-            "timestamp": 0.02329901820759936
-        },
-        {
-            "x": 1.3633924655059346,
-            "y": 5.557757467272512,
-            "heading": 1.605830471024488e-11,
-            "angularVelocity": 4.618037851350714e-10,
-            "velocityX": 0.38320544967553505,
-            "velocityY": -0.06416661499484601,
-            "timestamp": 0.04659803641519872
-        },
-        {
-            "x": 1.3767849336296958,
-            "y": 5.555514951621415,
-            "heading": 3.246020157611451e-11,
-            "angularVelocity": 7.039737222967939e-10,
-            "velocityX": 0.574808260349448,
-            "velocityY": -0.09624936257493934,
-            "timestamp": 0.06989705462279808
-        },
-        {
-            "x": 1.3946415606835925,
-            "y": 5.552524949597437,
-            "heading": 5.470894313107332e-11,
-            "angularVelocity": 9.549218317932479e-10,
-            "velocityX": 0.7664111377908356,
-            "velocityY": -0.12833167463728912,
-            "timestamp": 0.09319607283039744
-        },
-        {
-            "x": 1.416962348431807,
-            "y": 5.548787472707871,
-            "heading": 8.303679095756625e-11,
-            "angularVelocity": 1.2158386690956822e-9,
-            "velocityX": 0.9580140909513821,
-            "velocityY": -0.1604134928036672,
-            "timestamp": 0.1164950910379968
-        },
-        {
-            "x": 1.4437472988920377,
-            "y": 5.544302534113244,
-            "heading": 1.1770979097345793e-10,
-            "angularVelocity": 1.4881742667086445e-9,
-            "velocityX": 1.1496171307120806,
-            "velocityY": -0.19249474611664116,
-            "timestamp": 0.13979410924559615
-        },
-        {
-            "x": 1.4749964143943655,
-            "y": 5.539070149011117,
-            "heading": 1.59035725627389e-10,
-            "angularVelocity": 1.7737199674396097e-9,
-            "velocityX": 1.3412202704804594,
-            "velocityY": -0.22457534714647368,
-            "timestamp": 0.1630931274531955
-        },
-        {
-            "x": 1.5107096976598688,
-            "y": 5.533090335148591,
-            "heading": 2.073747689126765e-10,
-            "angularVelocity": 2.0747244300773254e-9,
-            "velocityX": 1.532823527038234,
-            "velocityY": -0.25665518646593166,
-            "timestamp": 0.18639214566079487
-        },
-        {
-            "x": 1.5508871519080267,
-            "y": 5.526363113522473,
-            "heading": 2.631540997774015e-10,
-            "angularVelocity": 2.3940635247907603e-9,
-            "velocityX": 1.7244269217769022,
-            "velocityY": -0.28873412459698383,
-            "timestamp": 0.20969116386839423
-        },
-        {
-            "x": 1.595528781007412,
-            "y": 5.518888509361551,
-            "heading": 3.268885139183836e-10,
-            "angularVelocity": 2.7354977853334202e-9,
-            "velocityX": 1.9160304825557395,
-            "velocityY": -0.3208119799018042,
-            "timestamp": 0.2329901820759936
-        },
-        {
-            "x": 1.6446345896939198,
-            "y": 5.5106665535489805,
-            "heading": 3.9921044507968937e-10,
-            "angularVelocity": 3.104076267437521e-9,
-            "velocityX": 2.1076342466004383,
-            "velocityY": -0.35288850969337676,
-            "timestamp": 0.2562892002835929
-        },
-        {
-            "x": 1.698204583899122,
-            "y": 5.501697284762255,
-            "heading": 4.809154717853132e-10,
-            "angularVelocity": 3.5068011876967e-9,
-            "velocityX": 2.299238265229739,
-            "velocityY": -0.38496337943692166,
-            "timestamp": 0.2795882184911923
-        },
-        {
-            "x": 1.7562387712682006,
-            "y": 5.491980752848314,
-            "heading": 5.730347406124241e-10,
-            "angularVelocity": 3.953783188523521e-9,
-            "velocityX": 2.4908426119922917,
-            "velocityY": -0.41703610973659105,
-            "timestamp": 0.3028872366987917
-        },
-        {
-            "x": 1.8187371620272614,
-            "y": 5.48151702447463,
-            "heading": 6.769576861119728e-10,
-            "angularVelocity": 4.460400083403711e-9,
-            "velocityX": 2.6824473976619876,
-            "velocityY": -0.449105978650929,
-            "timestamp": 0.3261862549063911
-        },
-        {
-            "x": 1.8856997705540457,
-            "y": 5.4703061933617745,
-            "heading": 7.946577963057737e-10,
-            "angularVelocity": 5.051719599980361e-9,
-            "velocityX": 2.8740528004281045,
-            "velocityY": -0.4811718250535651,
-            "timestamp": 0.34948527311399047
-        },
-        {
-            "x": 1.957126618545275,
-            "y": 5.4583484009138274,
-            "heading": 9.291581669995643e-10,
-            "angularVelocity": 5.772791178735968e-9,
-            "velocityX": 3.0656591344232145,
-            "velocityY": -0.5132316023559669,
-            "timestamp": 0.37278429132158986
-        },
-        {
-            "x": 2.0330177424946343,
-            "y": 5.445643884917638,
-            "heading": 1.0856695619650843e-9,
-            "angularVelocity": 6.717510100758224e-9,
-            "velocityX": 3.257267034736916,
-            "velocityY": -0.5452811737842305,
-            "timestamp": 0.39608330952918924
-        },
-        {
-            "x": 2.1133732165489003,
-            "y": 5.432193128386492,
-            "heading": 1.2752962310893515e-9,
-            "angularVelocity": 8.13882633127911e-9,
-            "velocityX": 3.44887811744974,
-            "velocityY": -0.5773100141552516,
-            "timestamp": 0.41938232773678863
-        },
-        {
-            "x": 2.1981932628587333,
-            "y": 5.417997633484574,
-            "heading": 1.5377676317044546e-9,
-            "angularVelocity": 1.1265341261395982e-8,
-            "velocityX": 3.640498734927221,
-            "velocityY": -0.6092743812441802,
-            "timestamp": 0.442681345944388
-        },
-        {
-            "x": 2.285115669591906,
-            "y": 5.403405223128801,
-            "heading": 1.537767230961928e-9,
-            "angularVelocity": -1.719997404273206e-14,
-            "velocityX": 3.730732598201487,
-            "velocityY": -0.6263100970908951,
-            "timestamp": 0.4659803641519874
-        },
-        {
-            "x": 2.3720410537379144,
-            "y": 5.388830564421114,
-            "heading": 1.5377672475870007e-9,
-            "angularVelocity": 7.135525631450618e-16,
-            "velocityX": 3.7308603895453194,
-            "velocityY": -0.6255481916751834,
-            "timestamp": 0.4892793823595868
-        },
-        {
-            "x": 2.45896811239221,
-            "y": 5.374265896231001,
-            "heading": 1.5377672778413637e-9,
-            "angularVelocity": 1.298525326828493e-15,
-            "velocityX": 3.730932259882311,
-            "velocityY": -0.625119396028046,
-            "timestamp": 0.5125784005671862
-        },
-        {
-            "x": 2.5458958805121723,
-            "y": 5.359705463035692,
-            "heading": 1.5377672869369513e-9,
-            "angularVelocity": 3.903849318113422e-16,
-            "velocityX": 3.730962710335439,
-            "velocityY": -0.6249376289364498,
-            "timestamp": 0.5358774187747856
-        },
-        {
-            "x": 2.632823878731898,
-            "y": 5.345146403658019,
-            "heading": 1.5377672987189865e-9,
-            "angularVelocity": 5.056879723770718e-16,
-            "velocityX": 3.7309725862775656,
-            "velocityY": -0.6248786643237555,
-            "timestamp": 0.559176436982385
-        },
-        {
-            "x": 2.7197519358547084,
-            "y": 5.330587695987482,
-            "heading": 1.5377673268477785e-9,
-            "angularVelocity": 1.2072952571647457e-15,
-            "velocityX": 3.7309751144130874,
-            "velocityY": -0.6248635689629851,
-            "timestamp": 0.5824754551899843
-        },
-        {
-            "x": 2.806680023647356,
-            "y": 5.316029171436071,
-            "heading": 1.5377673567071691e-9,
-            "angularVelocity": 1.2815730065587283e-15,
-            "velocityX": 3.7309764307702067,
-            "velocityY": -0.6248557094441328,
-            "timestamp": 0.6057744733975837
-        },
-        {
-            "x": 2.8936082747189524,
-            "y": 5.301471621825431,
-            "heading": 1.5377673928791505e-9,
-            "angularVelocity": 1.5525110632783364e-15,
-            "velocityX": 3.7309834387453797,
-            "velocityY": -0.6248138647313558,
-            "timestamp": 0.6290734916051831
-        },
-        {
-            "x": 2.980536964716405,
-            "y": 5.286916693414742,
-            "heading": 1.5377674058328616e-9,
-            "angularVelocity": 5.559766105183297e-16,
-            "velocityX": 3.7310022775585137,
-            "velocityY": -0.6247013621403136,
-            "timestamp": 0.6523725098127825
-        },
-        {
-            "x": 3.0674663067511823,
-            "y": 5.272365659809418,
-            "heading": 1.5377674408459054e-9,
-            "angularVelocity": 1.5027691524839091e-15,
-            "velocityX": 3.73103026317236,
-            "velocityY": -0.6245341960676912,
-            "timestamp": 0.6756715280203819
-        },
-        {
-            "x": 3.154396100803892,
-            "y": 5.257817326905748,
-            "heading": 1.5377674636831172e-9,
-            "angularVelocity": 9.801792492345066e-16,
-            "velocityX": 3.7310496639016963,
-            "velocityY": -0.624418281237505,
-            "timestamp": 0.6989705462279813
-        },
-        {
-            "x": 3.2413249978571956,
-            "y": 5.243263635140824,
-            "heading": 1.5377674638426843e-9,
-            "angularVelocity": 6.848580307945622e-18,
-            "velocityX": 3.731011164452385,
-            "velocityY": -0.6246482849651879,
-            "timestamp": 0.7222695644355807
-        },
-        {
-            "x": 3.3282506523054423,
-            "y": 5.228690588454777,
-            "heading": 1.5377674749707248e-9,
-            "angularVelocity": 4.776183371381179e-16,
-            "velocityX": 3.7308719909897503,
-            "velocityY": -0.6254790032748808,
-            "timestamp": 0.74556858264318
-        },
-        {
-            "x": 3.415171009638962,
-            "y": 5.214085980519485,
-            "heading": 1.5377674915535578e-9,
-            "angularVelocity": 7.117396517888937e-16,
-            "velocityX": 3.7306446374302076,
-            "velocityY": -0.626833620407003,
-            "timestamp": 0.7688676008507794
-        },
-        {
-            "x": 3.5020842171218036,
-            "y": 5.1994388833661285,
-            "heading": 1.53776751807092e-9,
-            "angularVelocity": 1.13813226337154e-15,
-            "velocityX": 3.730337763955042,
-            "velocityY": -0.6286572688425895,
-            "timestamp": 0.7921666190583788
-        },
-        {
-            "x": 3.5889883305639807,
-            "y": 5.184737925794629,
-            "heading": 1.5377675551515876e-9,
-            "angularVelocity": 1.5915121086693097e-15,
-            "velocityX": 3.729947445333141,
-            "velocityY": -0.6309689721834142,
-            "timestamp": 0.8154656372659782
-        },
-        {
-            "x": 3.675881697311449,
-            "y": 5.1699735802440445,
-            "heading": 1.5377675887419511e-9,
-            "angularVelocity": 1.441707334130238e-15,
-            "velocityX": 3.7294861943638535,
-            "velocityY": -0.6336896009521162,
-            "timestamp": 0.8387646554735776
-        },
-        {
-            "x": 3.7627625314341118,
-            "y": 5.155135664387593,
-            "heading": 1.5377676077743658e-9,
-            "angularVelocity": 8.168762038570203e-16,
-            "velocityX": 3.728948290804007,
-            "velocityY": -0.636847257870781,
-            "timestamp": 0.862063673681177
-        },
-        {
-            "x": 3.8490591061764308,
-            "y": 5.137209586747657,
-            "heading": 1.5377676390897132e-9,
-            "angularVelocity": 1.344063051824819e-15,
-            "velocityX": 3.703871724266177,
-            "velocityY": -0.7693919752476525,
-            "timestamp": 0.8853626918887764
-        },
-        {
-            "x": 3.934365266987087,
-            "y": 5.115044254457407,
-            "heading": 1.5377676702466937e-9,
-            "angularVelocity": 1.337265902944942e-15,
-            "velocityX": 3.661362897377225,
-            "velocityY": -0.9513419017440721,
-            "timestamp": 0.9086617100963758
+            "x": 1.35439418502164,
+            "y": 5.559271848784049,
+            "heading": -0.0037613483747314606,
+            "angularVelocity": -0.16062668985124567,
+            "velocityX": 0.18765169410035007,
+            "velocityY": -0.031095370031394967,
+            "timestamp": 0.023416708507251784
+        },
+        {
+            "x": 1.3631871179840578,
+            "y": 5.557813607446842,
+            "heading": -0.011205273313316288,
+            "angularVelocity": -0.317889464963847,
+            "velocityX": 0.37549824560930783,
+            "velocityY": -0.06227354014150002,
+            "timestamp": 0.04683341701450357
+        },
+        {
+            "x": 1.3763837173940232,
+            "y": 5.5556231238892515,
+            "heading": -0.02224504855076525,
+            "angularVelocity": -0.47144863395424863,
+            "velocityX": 0.563554839736692,
+            "velocityY": -0.09354361467639355,
+            "timestamp": 0.07025012552175536
+        },
+        {
+            "x": 1.3939893513502628,
+            "y": 5.552698044632534,
+            "heading": -0.03678367986427699,
+            "angularVelocity": -0.6208657083043512,
+            "velocityX": 0.7518406761047259,
+            "velocityY": -0.12491419346204678,
+            "timestamp": 0.09366683402900713
+        },
+        {
+            "x": 1.4160099408355828,
+            "y": 5.549035829570074,
+            "heading": -0.0547111148835232,
+            "angularVelocity": -0.7655830456997818,
+            "velocityX": 0.9403793653792271,
+            "velocityY": -0.15639324635770782,
+            "timestamp": 0.11708354253625891
+        },
+        {
+            "x": 1.4424520698548373,
+            "y": 5.544633771060564,
+            "heading": -0.07590088077413953,
+            "angularVelocity": -0.9048994176125892,
+            "velocityX": 1.129199221618411,
+            "velocityY": -0.18798792785714832,
+            "timestamp": 0.1405002510435107
+        },
+        {
+            "x": 1.4733230980825243,
+            "y": 5.539489016814175,
+            "heading": -0.10020600379509263,
+            "angularVelocity": -1.0379393420482537,
+            "velocityX": 1.3183333694454236,
+            "velocityY": -0.2197044151102235,
+            "timestamp": 0.16391695955076246
+        },
+        {
+            "x": 1.5086312686123933,
+            "y": 5.533598592872994,
+            "heading": -0.12745397379473564,
+            "angularVelocity": -1.163612298082128,
+            "velocityX": 1.5078195348818708,
+            "velocityY": -0.25154790389774145,
+            "timestamp": 0.18733366805801424
+        },
+        {
+            "x": 1.5483857981367395,
+            "y": 5.526959418437362,
+            "heading": -0.15744034724886685,
+            "angularVelocity": -1.2805545854082991,
+            "velocityX": 1.6976992950155392,
+            "velocityY": -0.28352295684851286,
+            "timestamp": 0.21075037656526602
+        },
+        {
+            "x": 1.5925969268012463,
+            "y": 5.5195682985391485,
+            "heading": -0.18992026053497643,
+            "angularVelocity": -1.3870400819163256,
+            "velocityX": 1.888016355962865,
+            "velocityY": -0.31563444947560276,
+            "timestamp": 0.2341670850725178
+        },
+        {
+            "x": 1.6412758830317984,
+            "y": 5.511421875068327,
+            "heading": -0.22459650029504458,
+            "angularVelocity": -1.4808332157070316,
+            "velocityX": 2.0788129217851563,
+            "velocityY": -0.34788934868040844,
+            "timestamp": 0.25758379357976957
+        },
+        {
+            "x": 1.6944346649736404,
+            "y": 5.502516516620167,
+            "heading": -0.26110152083126115,
+            "angularVelocity": -1.5589304758570732,
+            "velocityX": 2.2701218630013136,
+            "velocityY": -0.38029932539011047,
+            "timestamp": 0.2810005020870214
+        },
+        {
+            "x": 1.7520853949904163,
+            "y": 5.492848149517658,
+            "heading": -0.298968117565775,
+            "angularVelocity": -1.6170759747377448,
+            "velocityX": 2.4619484843022064,
+            "velocityY": -0.4128832666435571,
+            "timestamp": 0.3044172105942732
+        },
+        {
+            "x": 1.8142385663960479,
+            "y": 5.482412126535505,
+            "heading": -0.337577206769387,
+            "angularVelocity": -1.6487837815317001,
+            "velocityX": 2.654223217852448,
+            "velocityY": -0.44566566556215986,
+            "timestamp": 0.327833919101525
+        },
+        {
+            "x": 1.8808979849725118,
+            "y": 5.471203531590798,
+            "heading": -0.3760544549024592,
+            "angularVelocity": -1.6431535679387552,
+            "velocityX": 2.8466604756095246,
+            "velocityY": -0.47865800358925525,
+            "timestamp": 0.3512506276087768
+        },
+        {
+            "x": 1.9520436894250033,
+            "y": 5.459218834920382,
+            "heading": -0.41303307424029884,
+            "angularVelocity": -1.5791552995754587,
+            "velocityX": 3.0382452952539354,
+            "velocityY": -0.5118010785637118,
+            "timestamp": 0.3746673361160286
+        },
+        {
+            "x": 2.027554046757936,
+            "y": 5.446452385802885,
+            "heading": -0.44595627669268334,
+            "angularVelocity": -1.4059705462956014,
+            "velocityX": 3.2246358325529645,
+            "velocityY": -0.5451854650516427,
+            "timestamp": 0.3980840446232804
+        },
+        {
+            "x": 2.106704107988529,
+            "y": 5.4327632732947455,
+            "heading": -0.46854805770485686,
+            "angularVelocity": -0.9647718425148408,
+            "velocityX": 3.3800677497471567,
+            "velocityY": -0.5845873899786794,
+            "timestamp": 0.4215007531305322
+        },
+        {
+            "x": 2.1895060473348913,
+            "y": 5.418170740153337,
+            "heading": -0.4808901513114338,
+            "angularVelocity": -0.5270635539044761,
+            "velocityX": 3.5360195614476866,
+            "velocityY": -0.6231675616108724,
+            "timestamp": 0.444917461637784
+        },
+        {
+            "x": 2.275961949822614,
+            "y": 5.402702256569038,
+            "heading": -0.4829709400280361,
+            "angularVelocity": -0.0888591458512548,
+            "velocityX": 3.6920604132278863,
+            "velocityY": -0.6605746311236733,
+            "timestamp": 0.4683341701450358
+        },
+        {
+            "x": 2.3633379318433296,
+            "y": 5.388122823546896,
+            "heading": -0.48297098226531077,
+            "angularVelocity": -0.0000018037238101032338,
+            "velocityX": 3.7313519956767345,
+            "velocityY": -0.6226081269122786,
+            "timestamp": 0.4917508786522876
+        },
+        {
+            "x": 2.4507217300981603,
+            "y": 5.373590311340196,
+            "heading": -0.48297102439627604,
+            "angularVelocity": -0.0000017991839149984119,
+            "velocityX": 3.7316857844376363,
+            "velocityY": -0.6206043946012527,
+            "timestamp": 0.5151675871595394
+        },
+        {
+            "x": 2.5381084152434026,
+            "y": 5.359075168632344,
+            "heading": -0.4829710665257657,
+            "angularVelocity": -0.0000017991208983915868,
+            "velocityX": 3.7318090677936033,
+            "velocityY": -0.6198626379688947,
+            "timestamp": 0.5385842956667911
+        },
+        {
+            "x": 2.6254950830921073,
+            "y": 5.344559921791865,
+            "heading": -0.482971108655264,
+            "angularVelocity": -0.0000017991212673817243,
+            "velocityX": 3.7318083291527238,
+            "velocityY": -0.6198670849061145,
+            "timestamp": 0.5620010041740429
+        },
+        {
+            "x": 2.712881673840345,
+            "y": 5.330044210788612,
+            "heading": -0.4829711507848006,
+            "angularVelocity": -0.0000017991229009417428,
+            "velocityX": 3.731805036611997,
+            "velocityY": -0.6198869067684731,
+            "timestamp": 0.5854177126812946
+        },
+        {
+            "x": 2.800268245782709,
+            "y": 5.315528386572539,
+            "heading": -0.4829711929143463,
+            "angularVelocity": -0.0000017991232935293638,
+            "velocityX": 3.7318042335155903,
+            "velocityY": -0.6198917414709255,
+            "timestamp": 0.6088344211885464
+        },
+        {
+            "x": 2.8876548130761686,
+            "y": 5.301012534370082,
+            "heading": -0.4829712350438941,
+            "angularVelocity": -0.0000017991233825733017,
+            "velocityX": 3.731804034986268,
+            "velocityY": -0.6198929366167053,
+            "timestamp": 0.6322511296957981
+        },
+        {
+            "x": 2.975041345179212,
+            "y": 5.286496470320027,
+            "heading": -0.48297127717345933,
+            "angularVelocity": -0.000001799124124149535,
+            "velocityX": 3.7318025321954105,
+            "velocityY": -0.6199019834729332,
+            "timestamp": 0.6556678382030499
+        },
+        {
+            "x": 3.0624277772479402,
+            "y": 5.271979804077107,
+            "heading": -0.4829713193030743,
+            "angularVelocity": -0.0000017991262516148556,
+            "velocityX": 3.7317982602749975,
+            "velocityY": -0.6199276998481766,
+            "timestamp": 0.6790845467103016
+        },
+        {
+            "x": 3.149814115388955,
+            "y": 5.257462572426413,
+            "heading": -0.482971361432736,
+            "angularVelocity": -0.0000017991282469783394,
+            "velocityX": 3.731794249134225,
+            "velocityY": -0.619951845332987,
+            "timestamp": 0.7025012552175534
+        },
+        {
+            "x": 3.2372009926614522,
+            "y": 5.242948586436876,
+            "heading": -0.48297140356212753,
+            "angularVelocity": -0.0000017991167072996656,
+            "velocityX": 3.7318172725014778,
+            "velocityY": -0.6198132408343549,
+            "timestamp": 0.7259179637248051
+        },
+        {
+            "x": 3.3245908005140246,
+            "y": 5.228452256133779,
+            "heading": -0.4829714456900571,
+            "angularVelocity": -0.0000017990542795775989,
+            "velocityX": 3.7319424216049217,
+            "velocityY": -0.6190592626894866,
+            "timestamp": 0.7493346722320569
+        },
+        {
+            "x": 3.4119833565424647,
+            "y": 5.213972502683899,
+            "heading": -0.48297148781659605,
+            "angularVelocity": -0.0000017989948869691892,
+            "velocityX": 3.7320597812190472,
+            "velocityY": -0.6183513556300411,
+            "timestamp": 0.7727513807393086
+        },
+        {
+            "x": 3.4993677183765524,
+            "y": 5.199443379550061,
+            "heading": -0.48297152994719206,
+            "angularVelocity": -0.0000017991681461203675,
+            "velocityX": 3.731709851836638,
+            "velocityY": -0.6204596657670539,
+            "timestamp": 0.7961680892465604
+        },
+        {
+            "x": 3.5867330233447126,
+            "y": 5.184800101210533,
+            "heading": -0.48297157208742414,
+            "angularVelocity": -0.0000017995796480013664,
+            "velocityX": 3.7308960369522555,
+            "velocityY": -0.6253346124593939,
+            "timestamp": 0.8195847977538121
+        },
+        {
+            "x": 3.6740864894013328,
+            "y": 5.170086363489935,
+            "heading": -0.48297161423379187,
+            "angularVelocity": -0.0000017998416699478457,
+            "velocityX": 3.7303904615618975,
+            "velocityY": -0.6283435486246598,
+            "timestamp": 0.8430015062610638
+        },
+        {
+            "x": 3.7614250146254347,
+            "y": 5.155284197469098,
+            "heading": -0.482971656398251,
+            "angularVelocity": -0.0000018006142553635733,
+            "velocityX": 3.729752420031712,
+            "velocityY": -0.6321198393988197,
+            "timestamp": 0.8664182147683156
+        },
+        {
+            "x": 3.848175799548237,
+            "y": 5.137355912505346,
+            "heading": -0.48297169899264486,
+            "angularVelocity": -0.000001818974422860047,
+            "velocityX": 3.704653234929829,
+            "velocityY": -0.7656193421980864,
+            "timestamp": 0.8898349232755673
+        },
+        {
+            "x": 3.9339293985898425,
+            "y": 5.115142351916783,
+            "heading": -0.48297174253890224,
+            "angularVelocity": -0.0000018596233270255789,
+            "velocityX": 3.6620688605764276,
+            "velocityY": -0.9486201095127581,
+            "timestamp": 0.9132516317828191
         },
         {
             "x": 4.018431663513184,
             "y": 5.088562488555908,
-            "heading": 1.5377676866838871e-9,
-            "angularVelocity": 7.054887729794734e-16,
-            "velocityX": 3.608151887646215,
-            "velocityY": -1.1366043695780652,
-            "timestamp": 0.9319607283039751
-        },
-        {
-            "x": 4.095480434320656,
-            "y": 5.06018496604887,
-            "heading": 1.5377676994335126e-9,
-            "angularVelocity": 5.874067965982521e-16,
-            "velocityX": 3.549827430213289,
-            "velocityY": -1.3074226459526814,
-            "timestamp": 0.9536656625077448
-        },
-        {
-            "x": 4.171095847536846,
-            "y": 5.028183306370306,
-            "heading": 1.5377677188417825e-9,
-            "angularVelocity": 8.941870233905506e-16,
-            "velocityX": 3.4837891009642465,
-            "velocityY": -1.4743956087592989,
-            "timestamp": 0.9753705967115144
-        },
-        {
-            "x": 4.24514985906727,
-            "y": 4.992717825181937,
-            "heading": 1.5377677281143787e-9,
-            "angularVelocity": 4.2721152582009774e-16,
-            "velocityX": 3.4118514636002706,
-            "velocityY": -1.6339824325334937,
-            "timestamp": 0.997075530915284
-        },
-        {
-            "x": 4.317807248978416,
-            "y": 4.95447255507026,
-            "heading": 1.5377677290979519e-9,
-            "angularVelocity": 4.531574211416763e-17,
-            "velocityX": 3.3475056514378028,
-            "velocityY": -1.7620541832797834,
-            "timestamp": 1.0187804651190535
-        },
-        {
-            "x": 4.3901722659206355,
-            "y": 4.9156769197600605,
-            "heading": 1.5377677449553673e-9,
-            "angularVelocity": 7.305902735413318e-16,
-            "velocityX": 3.334035305654692,
-            "velocityY": -1.7874108691538961,
-            "timestamp": 1.0404853993228231
-        },
-        {
-            "x": 4.4625185516483485,
-            "y": 4.876846365558822,
-            "heading": 1.5377677746336623e-9,
-            "angularVelocity": 1.3673523815751288e-15,
-            "velocityX": 3.333172312277143,
-            "velocityY": -1.7890196688239732,
-            "timestamp": 1.0621903335265928
-        },
-        {
-            "x": 4.534844691324942,
-            "y": 4.837978300028894,
-            "heading": 1.5377677779199108e-9,
-            "angularVelocity": 1.5140568225814148e-16,
-            "velocityX": 3.3322441338764652,
-            "velocityY": -1.7907479085103777,
-            "timestamp": 1.0838952677303624
-        },
-        {
-            "x": 4.6071619879089205,
-            "y": 4.799093783673277,
-            "heading": 1.537767801979053e-9,
-            "angularVelocity": 1.1084641137098069e-15,
-            "velocityX": 3.331836710728115,
-            "velocityY": -1.79150583873765,
-            "timestamp": 1.105600201934132
-        },
-        {
-            "x": 4.6794740379059565,
-            "y": 4.7601995113399385,
-            "heading": 1.5377678257958516e-9,
-            "angularVelocity": 1.0972987247710634e-15,
-            "velocityX": 3.331594987489656,
-            "velocityY": -1.791955320763837,
-            "timestamp": 1.1273051361379016
-        },
-        {
-            "x": 4.751784687579244,
-            "y": 4.721302635647572,
-            "heading": 1.5377678370559733e-9,
-            "angularVelocity": 5.187816897475249e-16,
-            "velocityX": 3.331530471108496,
-            "velocityY": -1.7920752639579989,
-            "timestamp": 1.1490100703416712
-        },
-        {
-            "x": 4.824095678023389,
-            "y": 4.682406393453542,
-            "heading": 1.5377678370330874e-9,
-            "angularVelocity": -1.0543217693671843e-18,
-            "velocityX": 3.3315461712634193,
-            "velocityY": -1.792046077125237,
-            "timestamp": 1.1707150045454409
-        },
-        {
-            "x": 4.896406945309153,
-            "y": 4.643510665929771,
-            "heading": 1.5377678640109226e-9,
-            "angularVelocity": 1.242935520614447e-15,
-            "velocityX": 3.331558926041247,
-            "velocityY": -1.7920223649940337,
-            "timestamp": 1.1924199387492105
-        },
-        {
-            "x": 4.968718824670649,
-            "y": 4.604616076350799,
-            "heading": 1.5377678830932924e-9,
-            "angularVelocity": 8.791718726461835e-16,
-            "velocityX": 3.33158712588479,
-            "velocityY": -1.7919699370612494,
-            "timestamp": 1.21412487295298
-        },
-        {
-            "x": 5.041033201559798,
-            "y": 4.565726130481367,
-            "heading": 1.5377679126898002e-9,
-            "angularVelocity": 1.3635842497565112e-15,
-            "velocityX": 3.331702193162082,
-            "velocityY": -1.7917559898752364,
-            "timestamp": 1.2358298071567497
-        },
-        {
-            "x": 5.113354228854967,
-            "y": 4.526848553325412,
-            "heading": 1.5377679256265375e-9,
-            "angularVelocity": 5.960275144059911e-16,
-            "velocityX": 3.332008593816099,
-            "velocityY": -1.7911861326534588,
-            "timestamp": 1.2575347413605193
-        },
-        {
-            "x": 5.1856663573422805,
-            "y": 4.487954426928371,
-            "heading": 1.537767949104212e-9,
-            "angularVelocity": 1.0816744331209405e-15,
-            "velocityX": 3.3315986037289687,
-            "velocityY": -1.791948597121158,
-            "timestamp": 1.279239675564289
-        },
-        {
-            "x": 5.257977857015597,
-            "y": 4.449059131458437,
-            "heading": 1.5377679798803482e-9,
-            "angularVelocity": 1.4179326501228622e-15,
-            "velocityX": 3.3315696327123336,
-            "velocityY": -1.7920024592017922,
-            "timestamp": 1.3009446097680586
-        },
-        {
-            "x": 5.330290964048228,
-            "y": 4.410166824431303,
-            "heading": 1.5377679920779735e-9,
-            "angularVelocity": 5.619748235100741e-16,
-            "velocityX": 3.3316436877316113,
-            "velocityY": -1.7918647742503244,
-            "timestamp": 1.3226495439718282
-        },
-        {
-            "x": 5.402605809232356,
-            "y": 4.371277749351831,
-            "heading": 1.5377680157791104e-9,
-            "angularVelocity": 1.0919698963856968e-15,
-            "velocityX": 3.3317237686705528,
-            "velocityY": -1.7917158704270917,
-            "timestamp": 1.3443544781755978
-        },
-        {
-            "x": 5.474921974691154,
-            "y": 4.332391129443428,
-            "heading": 1.5377680240202577e-9,
-            "angularVelocity": 3.7969007641948726e-16,
-            "velocityX": 3.331784596990179,
-            "velocityY": -1.7916027546232325,
-            "timestamp": 1.3660594123793675
-        },
-        {
-            "x": 5.54723622900274,
-            "y": 4.29350095564713,
-            "heading": 1.537768038636468e-9,
-            "angularVelocity": 6.734049902652557e-16,
-            "velocityX": 3.331696545710963,
-            "velocityY": -1.7917664910290456,
-            "timestamp": 1.387764346583137
-        },
-        {
-            "x": 5.619548385303401,
-            "y": 4.254606880959325,
-            "heading": 1.5377680494372407e-9,
-            "angularVelocity": 4.976183803494664e-16,
-            "velocityX": 3.331599885158129,
-            "velocityY": -1.7919462147497014,
-            "timestamp": 1.4094692807869067
-        },
-        {
-            "x": 5.6918607364808045,
-            "y": 4.215713168590034,
-            "heading": 1.5377680578875737e-9,
-            "angularVelocity": 3.893277832205449e-16,
-            "velocityX": 3.3316088636117085,
-            "velocityY": -1.7919295218396705,
-            "timestamp": 1.4311742149906763
-        },
-        {
-            "x": 5.764173499508255,
-            "y": 4.176820221955942,
-            "heading": 1.5377680813491443e-9,
-            "angularVelocity": 1.0809325075471762e-15,
-            "velocityX": 3.331627838562525,
-            "velocityY": -1.7918942425240987,
-            "timestamp": 1.452879149194446
-        },
-        {
-            "x": 5.8364876254997204,
-            "y": 4.137929809554028,
-            "heading": 1.5377680862134134e-9,
-            "angularVelocity": 2.241089873857745e-16,
-            "velocityX": 3.331690633686676,
-            "velocityY": -1.7917774841780256,
-            "timestamp": 1.4745840833982156
-        },
-        {
-            "x": 5.90880309835116,
-            "y": 4.099041901645272,
-            "heading": 1.537768099475084e-9,
-            "angularVelocity": 6.109980238433844e-16,
-            "velocityX": 3.3317526868576484,
-            "velocityY": -1.7916620959822775,
-            "timestamp": 1.4962890176019852
-        },
-        {
-            "x": 5.981120307623631,
-            "y": 4.060157222932383,
-            "heading": 1.5377681005691863e-9,
-            "angularVelocity": 5.040806501517701e-17,
-            "velocityX": 3.331832688069564,
-            "velocityY": -1.7915133189414525,
-            "timestamp": 1.5179939518057548
-        },
-        {
-            "x": 6.053439067813782,
-            "y": 4.0212754287411325,
-            "heading": 1.5377681034134081e-9,
-            "angularVelocity": 1.3104042715425496e-16,
-            "velocityX": 3.3319041426820193,
-            "velocityY": -1.7913804218969325,
-            "timestamp": 1.5396988860095244
-        },
-        {
-            "x": 6.1257574594958575,
-            "y": 3.9823929491454755,
-            "heading": 1.5377681141371771e-9,
-            "angularVelocity": 4.940705905043031e-16,
-            "velocityX": 3.3318871646008015,
-            "velocityY": -1.7914120001783713,
-            "timestamp": 1.561403820213294
-        },
-        {
-            "x": 6.198071250397711,
-            "y": 3.943501913620472,
-            "heading": 1.5377681379511677e-9,
-            "angularVelocity": 1.09716939803524e-15,
-            "velocityX": 3.3316751952742036,
-            "velocityY": -1.7918061930096516,
-            "timestamp": 1.5831087544170637
-        },
-        {
-            "x": 6.270375940604977,
-            "y": 3.9045939610161593,
-            "heading": 1.5377681515073034e-9,
-            "angularVelocity": 6.245647694184294e-16,
-            "velocityX": 3.331255903765101,
-            "velocityY": -1.7925856046957434,
-            "timestamp": 1.6048136886208333
-        },
-        {
-            "x": 6.342673755736347,
-            "y": 3.865673234790299,
-            "heading": 1.5377699302540744e-9,
-            "angularVelocity": 8.19513085832153e-14,
-            "velocityX": 3.3309391520146527,
-            "velocityY": -1.7931741170331514,
-            "timestamp": 1.626518622824603
-        },
-        {
-            "x": 6.414965746925861,
-            "y": 3.826741692219575,
-            "heading": 1.5377690472977265e-9,
-            "angularVelocity": -4.068002902168065e-14,
-            "velocityX": 3.3306708286121007,
-            "velocityY": -1.7936724527872456,
-            "timestamp": 1.6482235570283725
-        },
-        {
-            "x": 6.487241720022193,
-            "y": 3.7877804203169507,
-            "heading": 1.5377684535437603e-9,
-            "angularVelocity": -2.7355713192172554e-14,
-            "velocityX": 3.3299328354443367,
-            "velocityY": -1.7950421566324242,
-            "timestamp": 1.6699284912321422
-        },
-        {
-            "x": 6.559496923985796,
-            "y": 3.748780644748349,
-            "heading": 1.5377682143039968e-9,
-            "angularVelocity": -1.1022367481483692e-14,
-            "velocityX": 3.3289759501347236,
-            "velocityY": -1.796816115748624,
-            "timestamp": 1.6916334254359118
-        },
-        {
-            "x": 6.630585509558441,
-            "y": 3.7076928121475503,
-            "heading": 1.5377682448018014e-9,
-            "angularVelocity": 1.4051092264620289e-15,
-            "velocityX": 3.2752269555510436,
-            "velocityY": -1.8930180674612387,
-            "timestamp": 1.7133383596396814
-        },
-        {
-            "x": 6.6997616539527085,
-            "y": 3.6634609457782683,
-            "heading": 1.5377682789348178e-9,
-            "angularVelocity": 1.5725923809489335e-15,
-            "velocityX": 3.1871160605618085,
-            "velocityY": -2.037871479084932,
-            "timestamp": 1.735043293843451
-        },
-        {
-            "x": 6.7667761400423245,
-            "y": 3.6160177634442383,
-            "heading": 1.5377682945212807e-9,
-            "angularVelocity": 7.181069480230158e-16,
-            "velocityX": 3.0875231161945336,
-            "velocityY": -2.185824747894625,
-            "timestamp": 1.7567482280472206
-        },
-        {
-            "x": 6.831456555555867,
-            "y": 3.5654387099344507,
-            "heading": 1.5377683209709385e-9,
-            "angularVelocity": 1.2186011773240001e-15,
-            "velocityX": 2.979986712068234,
-            "velocityY": -2.330302088683456,
-            "timestamp": 1.7784531622509903
+            "heading": -0.4829717875114574,
+            "angularVelocity": -0.000001920532733427299,
+            "velocityX": 3.6086312001179417,
+            "velocityY": -1.135081104701801,
+            "timestamp": 0.9366683402900708
+        },
+        {
+            "x": 4.095489617333613,
+            "y": 5.060208566496142,
+            "heading": -0.48297174072333765,
+            "angularVelocity": 0.00000215563208981735,
+            "velocityX": 3.550230245224857,
+            "velocityY": -1.306327856847248,
+            "timestamp": 0.9583733984124503
+        },
+        {
+            "x": 4.1711135466589555,
+            "y": 5.028225843813575,
+            "heading": -0.482971694875483,
+            "angularVelocity": 0.0000021123119922022846,
+            "velocityX": 3.4841615672696387,
+            "velocityY": -1.4735147218783384,
+            "timestamp": 0.9800784565348297
+        },
+        {
+            "x": 4.245175973739862,
+            "y": 4.9927768637698655,
+            "heading": -0.48297164958118555,
+            "angularVelocity": 0.0000020868083942836762,
+            "velocityX": 3.4122197076519116,
+            "velocityY": -1.633212859593547,
+            "timestamp": 1.0017835146572092
+        },
+        {
+            "x": 4.317835043195099,
+            "y": 4.954533786792328,
+            "heading": -0.4829716044969262,
+            "angularVelocity": 0.0000020771314744606885,
+            "velocityX": 3.3475639201504173,
+            "velocityY": -1.7619430808199938,
+            "timestamp": 1.0234885727795886
+        },
+        {
+            "x": 4.39018173013391,
+            "y": 4.915702997439957,
+            "heading": -0.48297155943721365,
+            "angularVelocity": 0.000002076000546478681,
+            "velocityX": 3.333171767193961,
+            "velocityY": -1.789020288884605,
+            "timestamp": 1.045193630901968
+        },
+        {
+            "x": 4.462524157787397,
+            "y": 4.876864273483723,
+            "heading": -0.4829715143782692,
+            "angularVelocity": 0.000002075965162802355,
+            "velocityX": 3.3329755325063237,
+            "velocityY": -1.7893858536194671,
+            "timestamp": 1.0668986890243475
+        },
+        {
+            "x": 4.534839835738717,
+            "y": 4.8379757656949245,
+            "heading": -0.48297146931947993,
+            "angularVelocity": 0.0000020759580074306272,
+            "velocityX": 3.331743114605303,
+            "velocityY": -1.7916795048180625,
+            "timestamp": 1.088603747146727
+        },
+        {
+            "x": 4.60713879668486,
+            "y": 4.7990561876016065,
+            "heading": -0.48297142426091105,
+            "angularVelocity": 0.000002075947856864141,
+            "velocityX": 3.3309729252282607,
+            "velocityY": -1.793110982419974,
+            "timestamp": 1.1103088052691064
+        },
+        {
+            "x": 4.679445820973998,
+            "y": 4.760151592099993,
+            "heading": -0.48297137920221006,
+            "angularVelocity": 0.0000020759539417382525,
+            "velocityX": 3.331344421260875,
+            "velocityY": -1.7924207013070022,
+            "timestamp": 1.1320138633914858
+        },
+        {
+            "x": 4.75175835900604,
+            "y": 4.72125724602898,
+            "heading": -0.4829713341434503,
+            "angularVelocity": 0.000002075956651977989,
+            "velocityX": 3.3315984515826855,
+            "velocityY": -1.791948487386363,
+            "timestamp": 1.1537189215138652
+        },
+        {
+            "x": 4.824072564767644,
+            "y": 4.68236600077068,
+            "heading": -0.48297128908467135,
+            "angularVelocity": 0.0000020759575350386475,
+            "velocityX": 3.331675287569862,
+            "velocityY": -1.7918056260909452,
+            "timestamp": 1.1754239796362447
+        },
+        {
+            "x": 4.8963869623773055,
+            "y": 4.643475112237639,
+            "heading": -0.48297124402589003,
+            "angularVelocity": 0.000002075957644932215,
+            "velocityX": 3.3316841264339563,
+            "velocityY": -1.79178919096958,
+            "timestamp": 1.1971290377586241
+        },
+        {
+            "x": 4.968701267365325,
+            "y": 4.604584051483172,
+            "heading": -0.4829711989671097,
+            "angularVelocity": 0.000002075957594698055,
+            "velocityX": 3.331679859150302,
+            "velocityY": -1.7917971255911598,
+            "timestamp": 1.2188340958810036
+        },
+        {
+            "x": 5.0410154847403765,
+            "y": 4.565692827821048,
+            "heading": -0.4829711539083305,
+            "angularVelocity": 0.0000020759575472868184,
+            "velocityX": 3.331675822627313,
+            "velocityY": -1.7918046311069282,
+            "timestamp": 1.240539154003383
+        },
+        {
+            "x": 5.113329654064444,
+            "y": 4.526801514813212,
+            "heading": -0.4829711088495519,
+            "angularVelocity": 0.000002075957520958662,
+            "velocityX": 3.331673608812237,
+            "velocityY": -1.7918087474613025,
+            "timestamp": 1.2622442121257624
+        },
+        {
+            "x": 5.185643801945598,
+            "y": 4.487910161934636,
+            "heading": -0.48297106379077354,
+            "angularVelocity": 0.0000020759575090327586,
+            "velocityX": 3.3316726208897984,
+            "velocityY": -1.7918105843944965,
+            "timestamp": 1.2839492702481419
+        },
+        {
+            "x": 5.257957938797454,
+            "y": 4.4490187885483135,
+            "heading": -0.48297101873199527,
+            "angularVelocity": 0.0000020759575026231074,
+            "velocityX": 3.331672112745739,
+            "velocityY": -1.7918115292316654,
+            "timestamp": 1.3056543283705213
+        },
+        {
+            "x": 5.330272065176824,
+            "y": 4.410127395689581,
+            "heading": -0.4829709736732172,
+            "angularVelocity": 0.000002075957497099591,
+            "velocityX": 3.3316716302550873,
+            "velocityY": -1.7918124263686461,
+            "timestamp": 1.3273593864929008
+        },
+        {
+            "x": 5.402586187963667,
+            "y": 4.371235996150952,
+            "heading": -0.4829709286144391,
+            "angularVelocity": 0.00000207595749402331,
+            "velocityX": 3.3316714647394585,
+            "velocityY": -1.7918127341261798,
+            "timestamp": 1.3490644446152802
+        },
+        {
+            "x": 5.474900315939471,
+            "y": 4.332344606260608,
+            "heading": -0.482970883555661,
+            "angularVelocity": 0.0000020759574957870433,
+            "velocityX": 3.331671703806323,
+            "velocityY": -1.7918122896084423,
+            "timestamp": 1.3707695027376596
+        },
+        {
+            "x": 5.547214451711641,
+            "y": 4.293453230866729,
+            "heading": -0.4829708384968828,
+            "angularVelocity": 0.0000020759574997718594,
+            "velocityX": 3.331672063002153,
+            "velocityY": -1.7918116217242377,
+            "timestamp": 1.392474560860039
+        },
+        {
+            "x": 5.619528591620017,
+            "y": 4.254561863163663,
+            "heading": -0.4829707934381046,
+            "angularVelocity": 0.0000020759575011693265,
+            "velocityX": 3.3316722535663335,
+            "velocityY": -1.7918112673914555,
+            "timestamp": 1.4141796189824185
+        },
+        {
+            "x": 5.6918427353771435,
+            "y": 4.215670502616919,
+            "heading": -0.4829707483793263,
+            "angularVelocity": 0.000002075957502624911,
+            "velocityX": 3.3316724308867816,
+            "velocityY": -1.7918109376838844,
+            "timestamp": 1.435884677104798
+        },
+        {
+            "x": 5.764156898177578,
+            "y": 4.176779177479102,
+            "heading": -0.4829707033205479,
+            "angularVelocity": 0.0000020759575111736717,
+            "velocityX": 3.3316733082540817,
+            "velocityY": -1.7918093063163627,
+            "timestamp": 1.4575897352271774
+        },
+        {
+            "x": 5.836471080177112,
+            "y": 4.137887888039928,
+            "heading": -0.48297065826176927,
+            "angularVelocity": 0.0000020759575207599675,
+            "velocityX": 3.331674192798964,
+            "velocityY": -1.7918076616010294,
+            "timestamp": 1.4792947933495568
+        },
+        {
+            "x": 5.908785251268908,
+            "y": 4.098996578318978,
+            "heading": -0.48297061320299073,
+            "angularVelocity": 0.0000020759575149169034,
+            "velocityX": 3.331673690255448,
+            "velocityY": -1.7918085960272527,
+            "timestamp": 1.5009998514719363
+        },
+        {
+            "x": 5.981099339043632,
+            "y": 4.0601051136793025,
+            "heading": -0.48297056814421313,
+            "angularVelocity": 0.000002075957470564626,
+            "velocityX": 3.3316698516536873,
+            "velocityY": -1.7918157334753977,
+            "timestamp": 1.5227049095943157
+        },
+        {
+            "x": 6.053413282760847,
+            "y": 4.021213381182831,
+            "heading": -0.48297052308543725,
+            "angularVelocity": 0.00000207595739406405,
+            "velocityX": 3.3316632146058507,
+            "velocityY": -1.7918280742300665,
+            "timestamp": 1.5444099677166951
+        },
+        {
+            "x": 6.125727369888532,
+            "y": 3.982321915340118,
+            "heading": -0.4829704780266597,
+            "angularVelocity": 0.0000020759574679524,
+            "velocityX": 3.3316698218431826,
+            "velocityY": -1.7918157889019397,
+            "timestamp": 1.5661150258390746
+        },
+        {
+            "x": 6.198042995964267,
+            "y": 3.9434333111252817,
+            "heading": -0.4829704329678644,
+            "angularVelocity": 0.000002075958288103883,
+            "velocityX": 3.3317407245813113,
+            "velocityY": -1.7916839473808401,
+            "timestamp": 1.587820083961454
+        },
+        {
+            "x": 6.270359121166566,
+            "y": 3.9045456350763237,
+            "heading": -0.4829703879090624,
+            "angularVelocity": 0.0000020759585995923127,
+            "velocityX": 3.331763720444871,
+            "velocityY": -1.791641184728093,
+            "timestamp": 1.6095251420838335
+        },
+        {
+            "x": 6.342677550055512,
+            "y": 3.8656622432711547,
+            "heading": -0.48297034285023976,
+            "angularVelocity": 0.0000020759595452668008,
+            "velocityX": 3.331869856379567,
+            "velocityY": -1.791443800149147,
+            "timestamp": 1.631230200206213
+        },
+        {
+            "x": 6.414977737835259,
+            "y": 3.8267449442462484,
+            "heading": -0.4829702977916256,
+            "angularVelocity": 0.0000020759499429522823,
+            "velocityX": 3.3310294481622025,
+            "velocityY": -1.7930059807042389,
+            "timestamp": 1.6529352583285923
+        },
+        {
+            "x": 6.487268603977522,
+            "y": 3.787810332568674,
+            "heading": -0.4829702527330872,
+            "angularVelocity": 0.00000207594645249869,
+            "velocityX": 3.330599979721225,
+            "velocityY": -1.793803612876013,
+            "timestamp": 1.6746403164509718
+        },
+        {
+            "x": 6.559543297837492,
+            "y": 3.7488457083598847,
+            "heading": -0.4829702076720725,
+            "angularVelocity": 0.000002076060537305802,
+            "velocityX": 3.3298548869330626,
+            "velocityY": -1.7951863565208734,
+            "timestamp": 1.6963453745733512
+        },
+        {
+            "x": 6.630628961599716,
+            "y": 3.7077518923991635,
+            "heading": -0.48297016252441444,
+            "angularVelocity": 0.000002080052392839484,
+            "velocityX": 3.2750736423474676,
+            "velocityY": -1.893282926450661,
+            "timestamp": 1.7180504326957307
+        },
+        {
+            "x": 6.699799614538253,
+            "y": 3.6635105764693066,
+            "heading": -0.4829701170038858,
+            "angularVelocity": 0.0000020972313632639956,
+            "velocityX": 3.186844861162469,
+            "velocityY": -2.0382952066016227,
+            "timestamp": 1.73975549081811
+        },
+        {
+            "x": 6.766806665010948,
+            "y": 3.616056089090901,
+            "heading": -0.48297007073382403,
+            "angularVelocity": 0.000002131764013783056,
+            "velocityX": 3.0871629135886995,
+            "velocityY": -2.1863331169555953,
+            "timestamp": 1.7614605489404895
+        },
+        {
+            "x": 6.831474779993965,
+            "y": 3.565460555976799,
+            "heading": -0.48297002329923955,
+            "angularVelocity": 0.0000021854161466442962,
+            "velocityX": 2.9794029860873867,
+            "velocityY": -2.331048036306895,
+            "timestamp": 1.783165607062869
         },
         {
             "x": 6.893649101257324,
             "y": 3.5118300914764404,
-            "heading": 1.5377683531646926e-9,
-            "angularVelocity": 1.4832457732424108e-15,
-            "velocityX": 2.8653643967583453,
-            "velocityY": -2.4698816386502185,
-            "timestamp": 1.8001580964547599
-        },
-        {
-            "x": 6.934219508494461,
-            "y": 3.4745268434152994,
-            "heading": 1.5377683787984002e-9,
-            "angularVelocity": 1.7594754613333528e-15,
-            "velocityX": 2.784717658151537,
-            "velocityY": -2.56046267850036,
-            "timestamp": 1.8147270446274342
-        },
-        {
-            "x": 6.973574535692003,
-            "y": 3.4359435289599864,
-            "heading": 1.5377684019331704e-9,
-            "angularVelocity": 1.5879505990145664e-15,
-            "velocityX": 2.701295023573416,
-            "velocityY": -2.648325328500865,
-            "timestamp": 1.8292959928001085
-        },
-        {
-            "x": 7.011676190859291,
-            "y": 3.3961219990562412,
-            "heading": 1.5377684253031837e-9,
-            "angularVelocity": 1.6040974952296623e-15,
-            "velocityX": 2.6152646516208327,
-            "velocityY": -2.733315365788167,
-            "timestamp": 1.8438649409727828
-        },
-        {
-            "x": 7.048490476455465,
-            "y": 3.3551073823105697,
-            "heading": 1.5377684383832216e-9,
-            "angularVelocity": 8.97802495662944e-16,
-            "velocityX": 2.5269007178724983,
-            "velocityY": -2.815207814562461,
-            "timestamp": 1.858433889145457
-        },
-        {
-            "x": 7.083991390306391,
-            "y": 3.3129508222340074,
-            "heading": 1.5377684557448877e-9,
-            "angularVelocity": 1.191689740876308e-15,
-            "velocityX": 2.4367520173841823,
-            "velocityY": -2.893589816979968,
-            "timestamp": 1.8730028373181313
-        },
-        {
-            "x": 7.118172550776329,
-            "y": 3.269717285105602,
-            "heading": 1.5377684739509254e-9,
-            "angularVelocity": 1.2496467044902449e-15,
-            "velocityX": 2.346165286939037,
-            "velocityY": -2.9675125902013777,
-            "timestamp": 1.8875717854908056
-        },
-        {
-            "x": 7.15108313208807,
-            "y": 3.2255088683196913,
-            "heading": 1.5377684882032233e-9,
-            "angularVelocity": 9.782653941580851e-16,
-            "velocityX": 2.2589538326090746,
-            "velocityY": -3.0344274865927217,
-            "timestamp": 1.90214073366348
-        },
-        {
-            "x": 7.182924606349684,
-            "y": 3.1805243042550315,
-            "heading": 1.5377697666399164e-9,
-            "angularVelocity": 8.775078931151136e-14,
-            "velocityX": 2.185571249497722,
-            "velocityY": -3.0877015644156707,
-            "timestamp": 1.9167096818361542
-        },
-        {
-            "x": 7.2139811373084415,
-            "y": 3.1352968072431113,
-            "heading": 1.2915519070501347e-9,
-            "angularVelocity": -1.6900181039231953e-8,
-            "velocityX": 2.131693420188762,
-            "velocityY": -3.1043762717714163,
-            "timestamp": 1.9312786300088285
-        },
-        {
-            "x": 7.244040263059187,
-            "y": 3.0915312465009275,
-            "heading": 1.1211375399666565e-9,
-            "angularVelocity": -1.169709485282711e-8,
-            "velocityX": 2.063232389495776,
-            "velocityY": -3.0040302308350935,
-            "timestamp": 1.9458475781815028
-        },
-        {
-            "x": 7.2730994599044765,
-            "y": 3.0492259199621516,
-            "heading": 9.847245150980228e-10,
-            "angularVelocity": -9.363272118309568e-9,
-            "velocityX": 1.9945981343934114,
-            "velocityY": -2.903801018259091,
-            "timestamp": 1.960416526354177
-        },
-        {
-            "x": 7.301157878524098,
-            "y": 3.0083802533354898,
-            "heading": 8.698146081553592e-10,
-            "angularVelocity": -7.887316693614981e-9,
-            "velocityX": 1.9259055826862927,
-            "velocityY": -2.803611224540764,
-            "timestamp": 1.9749854745268514
-        },
-        {
-            "x": 7.328215092540324,
-            "y": 2.968993958016925,
-            "heading": 7.704962597777701e-10,
-            "angularVelocity": -6.8171255926008325e-9,
-            "velocityX": 1.857183764780944,
-            "velocityY": -2.7034412403524777,
-            "timestamp": 1.9895544226995256
-        },
-        {
-            "x": 7.354270845617658,
-            "y": 2.9310668604072108,
-            "heading": 6.833248169018587e-10,
-            "angularVelocity": -5.983372499761897e-9,
-            "velocityX": 1.7884443522287568,
-            "velocityY": -2.6032831718663054,
-            "timestamp": 2.0041233708722
-        },
-        {
-            "x": 7.379324966656107,
-            "y": 2.894598844592948,
-            "heading": 6.060446454340774e-10,
-            "angularVelocity": -5.304444180973637e-9,
-            "velocityX": 1.719693195521089,
-            "velocityY": -2.5031330595756027,
-            "timestamp": 2.0186923190448742
-        },
-        {
-            "x": 7.403377333338156,
-            "y": 2.8595898276899736,
-            "heading": 5.370639718880826e-10,
-            "angularVelocity": -4.734773818100124e-9,
-            "velocityX": 1.6509336430450512,
-            "velocityY": -2.4029886363820543,
-            "timestamp": 2.0332612672175485
-        },
-        {
-            "x": 7.426427853868939,
-            "y": 2.8260397474863512,
-            "heading": 4.75199340983144e-10,
-            "angularVelocity": -4.246334802521844e-9,
-            "velocityX": 1.582167789848791,
-            "velocityY": -2.3028484833621037,
-            "timestamp": 2.047830215390223
-        },
-        {
-            "x": 7.44847645681879,
-            "y": 2.79394855556605,
-            "heading": 4.195363288341646e-10,
-            "angularVelocity": -3.820661051835768e-9,
-            "velocityX": 1.5133970337819913,
-            "velocityY": -2.202711653576578,
-            "timestamp": 2.062399163562897
-        },
-        {
-            "x": 7.469523085022814,
-            "y": 2.7633162131780225,
-            "heading": 3.6934715040819367e-10,
-            "angularVelocity": -3.4449418365911004e-9,
-            "velocityX": 1.4446223539663485,
-            "velocityY": -2.1025774836294286,
-            "timestamp": 2.0769681117355714
-        },
-        {
-            "x": 7.489567691695811,
-            "y": 2.7341426886048597,
-            "heading": 3.2403879933557077e-10,
-            "angularVelocity": -3.1099260658641154e-9,
-            "velocityX": 1.3758444628549362,
-            "velocityY": -2.002445490737794,
-            "timestamp": 2.0915370599082457
-        },
-        {
-            "x": 7.5086102378406,
-            "y": 2.7064279554071735,
-            "heading": 2.831187733993615e-10,
-            "angularVelocity": -2.8087152215039624e-9,
-            "velocityX": 1.3070638950107798,
-            "velocityY": -1.9023153126229018,
-            "timestamp": 2.10610600808092
-        },
-        {
-            "x": 7.52665069045286,
-            "y": 2.680171991207375,
-            "heading": 2.4617151569169827e-10,
-            "angularVelocity": -2.536027844020545e-9,
-            "velocityX": 1.2382810617787536,
-            "velocityY": -1.8021866704863527,
-            "timestamp": 2.1206749562535943
-        },
-        {
-            "x": 7.543689021238326,
-            "y": 2.65537477682047,
-            "heading": 2.128416865029733e-10,
-            "angularVelocity": -2.287730676987115e-9,
-            "velocityX": 1.169496286452695,
-            "velocityY": -1.7020593451912265,
-            "timestamp": 2.1352439044262685
-        },
-        {
-            "x": 7.559725205671708,
-            "y": 2.6320362956163437,
-            "heading": 1.8282195651887185e-10,
-            "angularVelocity": -2.0605283283789865e-9,
-            "velocityX": 1.1007098277319451,
-            "velocityY": -1.6019331613726957,
-            "timestamp": 2.149812852598943
-        },
-        {
-            "x": 7.5747592222906635,
-            "y": 2.6101565330412897,
-            "heading": 1.558438913168689e-10,
-            "angularVelocity": -1.8517510864510685e-9,
-            "velocityX": 1.0319218958546759,
-            "velocityY": -1.5018079765081174,
-            "timestamp": 2.164381800771617
-        },
-        {
-            "x": 7.588791052155751,
-            "y": 2.5897354762519833,
-            "heading": 1.3167100643601026e-10,
-            "angularVelocity": -1.6592059334641093e-9,
-            "velocityX": 0.9631326639903722,
-            "velocityY": -1.4016836731981617,
-            "timestamp": 2.1789507489442914
-        },
-        {
-            "x": 7.601820678430282,
-            "y": 2.570773113830717,
-            "heading": 1.100933830131512e-10,
-            "angularVelocity": -1.4810694297523211e-9,
-            "velocityX": 0.894342276470416,
-            "velocityY": -1.3015601535897774,
-            "timestamp": 2.1935196971169657
-        },
-        {
-            "x": 7.613848086048565,
-            "y": 2.5532694355605217,
-            "heading": 9.092342750630814e-11,
-            "angularVelocity": -1.315809184573053e-9,
-            "velocityX": 0.8255508548544661,
-            "velocityY": -1.201437335265309,
-            "timestamp": 2.20808864528964
-        },
-        {
-            "x": 7.624873261450447,
-            "y": 2.537224432245223,
-            "heading": 7.399249133993465e-11,
-            "angularVelocity": -1.1621248318093622e-9,
-            "velocityX": 0.7567585024813989,
-            "velocityY": -1.101315148158345,
-            "timestamp": 2.2226575934623143
-        },
-        {
-            "x": 7.634896192366388,
-            "y": 2.5226380955637384,
-            "heading": 5.914813867727886e-11,
-            "angularVelocity": -1.0189035430874983e-9,
-            "velocityX": 0.6879653079374735,
-            "velocityY": -1.0011935322032168,
-            "timestamp": 2.2372265416349886
-        },
-        {
-            "x": 7.643916867641572,
-            "y": 2.5095104179508487,
-            "heading": 4.6251914688462445e-11,
-            "angularVelocity": -8.851856741832805e-10,
-            "velocityX": 0.6191713477368228,
-            "velocityY": -0.9010724355182365,
-            "timestamp": 2.251795489807663
-        },
-        {
-            "x": 7.651935277090606,
-            "y": 2.4978413924986826,
-            "heading": 3.517750680608747e-11,
-            "angularVelocity": -7.601377906979457e-10,
-            "velocityX": 0.5503766884196106,
-            "velocityY": -0.8009518129835539,
-            "timestamp": 2.266364437980337
-        },
-        {
-            "x": 7.658951411376412,
-            "y": 2.487631012874611,
-            "heading": 2.580921448386634e-11,
-            "angularVelocity": -6.43031492677376e-10,
-            "velocityX": 0.48158138821334623,
-            "velocityY": -0.7008316251150604,
-            "timestamp": 2.2809333861530114
-        },
-        {
-            "x": 7.664965261908482,
-            "y": 2.4788792732522666,
-            "heading": 1.804066569533592e-11,
-            "angularVelocity": -5.332264785860361e-10,
-            "velocityX": 0.41278549836213546,
-            "velocityY": -0.6007118371633381,
-            "timestamp": 2.2955023343256857
-        },
-        {
-            "x": 7.669976820756786,
-            "y": 2.4715861682531717,
-            "heading": 1.1773732592627752e-11,
-            "angularVelocity": -4.3015687513290903e-10,
-            "velocityX": 0.34398906420045333,
-            "velocityY": -0.500592418385764,
-            "timestamp": 2.31007128249836
-        },
-        {
-            "x": 7.67398608057842,
-            "y": 2.465751692897001,
-            "heading": 6.917607828867898e-12,
-            "angularVelocity": -3.3332020945886676e-10,
-            "velocityX": 0.27519212602817883,
-            "velocityY": -0.40047334145331454,
-            "timestamp": 2.3246402306710343
-        },
-        {
-            "x": 7.676993034554729,
-            "y": 2.461375842558954,
-            "heading": 3.3880167119093778e-12,
-            "angularVelocity": -2.4226808579143967e-10,
-            "velocityX": 0.20639471982945568,
-            "velocityY": -0.30035458196322756,
-            "timestamp": 2.3392091788437086
-        },
-        {
-            "x": 7.678997676337105,
-            "y": 2.4584586129329957,
-            "heading": 1.1065347655527578e-12,
-            "angularVelocity": -1.56598950365953e-10,
-            "velocityX": 0.13759687786767488,
-            "velocityY": -0.2002361180356331,
-            "timestamp": 2.353778127016383
+            "heading": -0.48296997476148124,
+            "angularVelocity": 0.00000223624180421772,
+            "velocityX": 2.8645083976649595,
+            "velocityY": -2.470874033046681,
+            "timestamp": 1.8048706651852484
+        },
+        {
+            "x": 6.934561334359113,
+            "y": 3.474160479757131,
+            "heading": -0.4829699264500687,
+            "angularVelocity": 0.0000032862628672599958,
+            "velocityX": 2.782952215912443,
+            "velocityY": -2.5623810156238895,
+            "timestamp": 1.8195716840772946
+        },
+        {
+            "x": 6.974231870495054,
+            "y": 3.435185370851295,
+            "heading": -0.4829698798888691,
+            "angularVelocity": 0.0000031672090196575476,
+            "velocityX": 2.6984888889168737,
+            "velocityY": -2.6511841928806597,
+            "timestamp": 1.8342727029693409
+        },
+        {
+            "x": 7.012622614846585,
+            "y": 3.394949051793438,
+            "heading": -0.482969834780297,
+            "angularVelocity": 0.0000030683976682796836,
+            "velocityX": 2.611434257274787,
+            "velocityY": -2.736974855506367,
+            "timestamp": 1.848973721861387
+        },
+        {
+            "x": 7.049699862488227,
+            "y": 3.3534992220248374,
+            "heading": -0.4829697908553284,
+            "angularVelocity": 0.0000029878860039287864,
+            "velocityX": 2.5220869324714577,
+            "velocityY": -2.8195208830745364,
+            "timestamp": 1.8636747407534333
+        },
+        {
+            "x": 7.085438739774718,
+            "y": 3.310890028476691,
+            "heading": -0.48296974786836877,
+            "angularVelocity": 0.0000029240802913465185,
+            "velocityX": 2.4310476402306533,
+            "velocityY": -2.8983837012274143,
+            "timestamp": 1.8783757596454795
+        },
+        {
+            "x": 7.119836677140904,
+            "y": 3.2671911422114968,
+            "heading": -0.4829697055890564,
+            "angularVelocity": 0.0000028759443619428334,
+            "velocityX": 2.339833559754191,
+            "velocityY": -2.9725073198044742,
+            "timestamp": 1.8930767785375258
+        },
+        {
+            "x": 7.1529563800127525,
+            "y": 3.222515689615774,
+            "heading": -0.48296966398118896,
+            "angularVelocity": 0.000002830270996857658,
+            "velocityX": 2.2528848588700137,
+            "velocityY": -3.0389357991976094,
+            "timestamp": 1.907777797429572
+        },
+        {
+            "x": 7.184894346745796,
+            "y": 3.177381185858669,
+            "heading": -0.4821430167851757,
+            "angularVelocity": 0.056230605652804695,
+            "velocityX": 2.17250021699707,
+            "velocityY": -3.0701616050246474,
+            "timestamp": 1.9224788163216182
+        },
+        {
+            "x": 7.2158791345376,
+            "y": 3.1333943550888805,
+            "heading": -0.47750834116458535,
+            "angularVelocity": 0.31526220424747137,
+            "velocityX": 2.107662606199986,
+            "velocityY": -2.992094023740475,
+            "timestamp": 1.9371798352136644
+        },
+        {
+            "x": 7.245895346179036,
+            "y": 3.090550131820991,
+            "heading": -0.46908945700490695,
+            "angularVelocity": 0.5726735147747635,
+            "velocityX": 2.041777638805421,
+            "velocityY": -2.9143710094182476,
+            "timestamp": 1.9518808541057107
+        },
+        {
+            "x": 7.274933319615603,
+            "y": 3.048846636162261,
+            "heading": -0.45692051225089847,
+            "angularVelocity": 0.827761996863506,
+            "velocityX": 1.9752354343465999,
+            "velocityY": -2.8367758700924766,
+            "timestamp": 1.9665818729977569
+        },
+        {
+            "x": 7.3029845213490745,
+            "y": 3.008282422680816,
+            "heading": -0.4410407652781448,
+            "angularVelocity": 1.08018002625281,
+            "velocityX": 1.90811276003785,
+            "velocityY": -2.7592790526506894,
+            "timestamp": 1.981282891889803
+        },
+        {
+            "x": 7.330040747407425,
+            "y": 2.9688555797343485,
+            "heading": -0.42149121219843927,
+            "angularVelocity": 1.3298093977882255,
+            "velocityX": 1.8404320310743014,
+            "velocityY": -2.6819122698903146,
+            "timestamp": 1.9959839107818493
+        },
+        {
+            "x": 7.356112777651364,
+            "y": 2.9307983654229495,
+            "heading": -0.3996724931243264,
+            "angularVelocity": 1.4841637327544344,
+            "velocityX": 1.7734845751435842,
+            "velocityY": -2.5887467114262686,
+            "timestamp": 2.0106849296738956
+        },
+        {
+            "x": 7.38118332940277,
+            "y": 2.8941879380599844,
+            "heading": -0.37646864699164706,
+            "angularVelocity": 1.578383532670194,
+            "velocityX": 1.7053615083080305,
+            "velocityY": -2.4903326518935414,
+            "timestamp": 2.025385948565942
+        },
+        {
+            "x": 7.40523963577506,
+            "y": 2.8590504496296347,
+            "heading": -0.3524751392807844,
+            "angularVelocity": 1.6320982842790503,
+            "velocityX": 1.6363700059799433,
+            "velocityY": -2.390139669119207,
+            "timestamp": 2.040086967457988
+        },
+        {
+            "x": 7.428276340738969,
+            "y": 2.825395406477394,
+            "heading": -0.3281233875567818,
+            "angularVelocity": 1.656466936259613,
+            "velocityX": 1.5670141731720877,
+            "velocityY": -2.289300040995877,
+            "timestamp": 2.0547879863500342
+        },
+        {
+            "x": 7.450292281624529,
+            "y": 2.793225377698084,
+            "heading": -0.3037369697425905,
+            "angularVelocity": 1.658825010243686,
+            "velocityX": 1.497579252650967,
+            "velocityY": -2.1882856566299744,
+            "timestamp": 2.0694890052420805
+        },
+        {
+            "x": 7.471288319158757,
+            "y": 2.7625397684205146,
+            "heading": -0.27956516086770095,
+            "angularVelocity": 1.6442267745106667,
+            "velocityX": 1.4282028809299567,
+            "velocityY": -2.0873117368873455,
+            "timestamp": 2.0841900241341267
+        },
+        {
+            "x": 7.491266209557209,
+            "y": 2.733336527235843,
+            "heading": -0.2558043926700512,
+            "angularVelocity": 1.6162667616531794,
+            "velocityX": 1.3589459713748013,
+            "velocityY": -1.9864773590946931,
+            "timestamp": 2.098891043026173
+        },
+        {
+            "x": 7.510228057377649,
+            "y": 2.7056129838220437,
+            "heading": -0.2326125564520332,
+            "angularVelocity": 1.577566588297197,
+            "velocityX": 1.28983221909181,
+            "velocityY": -1.885824623271541,
+            "timestamp": 2.113592061918219
+        },
+        {
+            "x": 7.528176060430696,
+            "y": 2.679366275566135,
+            "heading": -0.21011884554685348,
+            "angularVelocity": 1.5300783619392286,
+            "velocityX": 1.2208679673730278,
+            "velocityY": -1.7853666095287006,
+            "timestamp": 2.1282930808102654
+        },
+        {
+            "x": 7.545112396890263,
+            "y": 2.654593565371254,
+            "heading": -0.18843070859149175,
+            "angularVelocity": 1.4752812110931899,
+            "velocityX": 1.152051880480808,
+            "velocityY": -1.6851015821961506,
+            "timestamp": 2.1429940997023116
+        },
+        {
+            "x": 7.561039180049459,
+            "y": 2.631292148556631,
+            "heading": -0.1676388680105614,
+            "angularVelocity": 1.4143128944742427,
+            "velocityX": 1.0833795450607198,
+            "velocityY": -1.5850205340005987,
+            "timestamp": 2.157695118594358
+        },
+        {
+            "x": 7.575958444025175,
+            "y": 2.6094594995019675,
+            "heading": -0.14782100448684538,
+            "angularVelocity": 1.3480605439149753,
+            "velocityX": 1.0148455753489192,
+            "velocityY": -1.485111284801969,
+            "timestamp": 2.172396137486404
+        },
+        {
+            "x": 7.589872142500143,
+            "y": 2.5890932855015207,
+            "heading": -0.12904449811203259,
+            "angularVelocity": 1.2772248313327248,
+            "velocityX": 0.9464445000131985,
+            "velocityY": -1.3853607120705473,
+            "timestamp": 2.1870971563784503
+        },
+        {
+            "x": 7.602782151943804,
+            "y": 2.570191363211945,
+            "heading": -0.11136848807718236,
+            "angularVelocity": 1.2023663233582889,
+            "velocityX": 0.8781710668125251,
+            "velocityY": -1.2857559349035097,
+            "timestamp": 2.2017981752704965
+        },
+        {
+            "x": 7.614690275426409,
+            "y": 2.552751766479238,
+            "heading": -0.09484543137092857,
+            "angularVelocity": 1.1239395600799826,
+            "velocityX": 0.8100202829510676,
+            "velocityY": -1.186284900439311,
+            "timestamp": 2.2164991941625427
+        },
+        {
+            "x": 7.62559824546286,
+            "y": 2.536772690588083,
+            "heading": -0.07952228805027751,
+            "angularVelocity": 1.042318456507909,
+            "velocityX": 0.7419873490775946,
+            "velocityY": -1.086936627215761,
+            "timestamp": 2.231200213054589
+        },
+        {
+            "x": 7.635507725447758,
+            "y": 2.522252475791086,
+            "heading": -0.06544142530276546,
+            "angularVelocity": 0.9578154310876013,
+            "velocityX": 0.6740675634570735,
+            "velocityY": -0.9877012541528096,
+            "timestamp": 2.245901231946635
+        },
+        {
+            "x": 7.644420309753795,
+            "y": 2.509189591670685,
+            "heading": -0.05264130826624494,
+            "angularVelocity": 0.8706959109783889,
+            "velocityX": 0.6062562310465265,
+            "velocityY": -0.8885699839124228,
+            "timestamp": 2.2606022508386814
+        },
+        {
+            "x": 7.652337522761704,
+            "y": 2.497582623097395,
+            "heading": -0.04115702850586921,
+            "angularVelocity": 0.7811893750159932,
+            "velocityX": 0.538548590818527,
+            "velocityY": -0.7895349743118973,
+            "timestamp": 2.2753032697307276
+        },
+        {
+            "x": 7.6592608171290975,
+            "y": 2.487430258069545,
+            "heading": -0.031020708755951136,
+            "angularVelocity": 0.6894977704846257,
+            "velocityX": 0.4709397639872257,
+            "velocityY": -0.6905892103399419,
+            "timestamp": 2.290004288622774
+        },
+        {
+            "x": 7.665191571570965,
+            "y": 2.4787312774326344,
+            "heading": -0.022261813501037925,
+            "angularVelocity": 0.5958019181685587,
+            "velocityX": 0.40342472079099084,
+            "velocityY": -0.5917263763001924,
+            "timestamp": 2.30470530751482
+        },
+        {
+            "x": 7.670131088361328,
+            "y": 2.4714845463094046,
+            "heading": -0.014907388223763604,
+            "angularVelocity": 0.5002663646159557,
+            "velocityX": 0.33599826152420176,
+            "velocityY": -0.49294073944433947,
+            "timestamp": 2.3194063264068663
+        },
+        {
+            "x": 7.674080590697013,
+            "y": 2.465689006980458,
+            "heading": -0.008982245050778671,
+            "angularVelocity": 0.40304302827545885,
+            "velocityX": 0.26865500715889973,
+            "velocityY": -0.3942270513020354,
+            "timestamp": 2.3341073452989125
+        },
+        {
+            "x": 7.677041220003002,
+            "y": 2.461343672911727,
+            "heading": -0.0045091086563273375,
+            "angularVelocity": 0.304273902870198,
+            "velocityX": 0.20138939536970257,
+            "velocityY": -0.29558046966947404,
+            "timestamp": 2.3488083641909587
+        },
+        {
+            "x": 7.679014033209882,
+            "y": 2.458447623611078,
+            "heading": -0.00150873334272897,
+            "angularVelocity": 0.20409301801671295,
+            "velocityX": 0.1341956786374654,
+            "velocityY": -0.1969965022095263,
+            "timestamp": 2.363509383083005
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": -3.441813048880529e-34,
-            "angularVelocity": -7.595159066564751e-11,
-            "velocityX": 0.06879862918134139,
-            "velocityY": -0.10011792997737116,
-            "timestamp": 2.368347075189057
+            "heading": -1.1328442579469643e-34,
+            "angularVelocity": 0.10262780789604178,
+            "velocityX": 0.06706792211866815,
+            "velocityY": -0.0984709714141921,
+            "timestamp": 2.378210401975051
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": -3.481182114535467e-34,
-            "angularVelocity": -2.7023024138211624e-34,
-            "velocityX": -3.7878173595367446e-32,
-            "velocityY": -1.5253102288533056e-31,
-            "timestamp": 2.3829160233617315
+            "heading": -2.155134012002005e-33,
+            "angularVelocity": -1.3889170145055665e-31,
+            "velocityX": 1.4892305241424507e-32,
+            "velocityY": 6.576507695252416e-33,
+            "timestamp": 2.3929114208670974
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.1.traj
index 63887ddf..bb0e9680 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.1.traj	
@@ -3,73 +3,73 @@
         {
             "x": 1.35,
             "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -1.1658790467474463e-32,
-            "velocityY": 2.7665844766493316e-30,
+            "heading": 7.039472547405692e-42,
+            "angularVelocity": 1.0705191064262213e-40,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 1.4250000045295077,
-            "y": 5.560000008699219,
-            "heading": -5.237332222253062e-9,
-            "angularVelocity": -5.5222228614505115e-8,
-            "velocityX": 0.7907971475606554,
-            "velocityY": 9.172423930862072e-8,
-            "timestamp": 0.09484101550044491
+            "x": 1.4250000045295101,
+            "y": 5.56,
+            "heading": 8.194631828276944e-23,
+            "angularVelocity": 8.6403876700238e-22,
+            "velocityX": 0.7907971475606789,
+            "velocityY": 8.424071056371489e-35,
+            "timestamp": 0.0948410155004449
         },
         {
-            "x": 1.575000009059013,
-            "y": 5.560000023677869,
-            "heading": -1.3214169082837694e-8,
-            "angularVelocity": -8.41074599041897e-8,
-            "velocityX": 1.5815942473623306,
-            "velocityY": 1.579343082639882e-7,
-            "timestamp": 0.18968203100088982
+            "x": 1.5750000090590195,
+            "y": 5.56,
+            "heading": 2.6954329855279996e-22,
+            "angularVelocity": 1.978015178210688e-21,
+            "velocityX": 1.5815942473623736,
+            "velocityY": 1.2488550168110521e-34,
+            "timestamp": 0.1896820310008898
         },
         {
-            "x": 1.7999999999999883,
-            "y": 5.560000042361517,
-            "heading": -2.1609400534628168e-8,
-            "angularVelocity": -8.851899686654573e-8,
-            "velocityX": 2.372391203887095,
-            "velocityY": 1.9699965815991792e-7,
-            "timestamp": 0.28452304650133475
+            "x": 1.7999999999999998,
+            "y": 5.56,
+            "heading": 5.87848829010012e-22,
+            "angularVelocity": 3.3562006983676933e-21,
+            "velocityX": 2.3723912038871497,
+            "velocityY": 2.135553914770072e-34,
+            "timestamp": 0.2845230465013347
         },
         {
-            "x": 2.0249999909409753,
-            "y": 5.560000024806983,
-            "heading": -1.1174048484706425e-8,
-            "angularVelocity": 1.1002994951426502e-7,
-            "velocityX": 2.372391203887216,
-            "velocityY": -1.8509433303171823e-7,
-            "timestamp": 0.37936406200177963
+            "x": 2.0249999909409806,
+            "y": 5.56,
+            "heading": 4.475372520266276e-22,
+            "angularVelocity": -1.4794393741169351e-21,
+            "velocityX": 2.37239120388715,
+            "velocityY": 2.583104828416982e-34,
+            "timestamp": 0.3793640620017796
         },
         {
-            "x": 2.1749999954704884,
-            "y": 5.560000009521861,
-            "heading": -3.737396801552092e-9,
-            "angularVelocity": 7.841176829375941e-8,
-            "velocityX": 1.581594247362411,
-            "velocityY": -1.6116572709153717e-7,
-            "timestamp": 0.4742050775022245
+            "x": 2.17499999547049,
+            "y": 5.56,
+            "heading": 1.6184867431561565e-22,
+            "angularVelocity": -3.012289189189882e-21,
+            "velocityX": 1.5815942473623736,
+            "velocityY": 1.9272829246954695e-34,
+            "timestamp": 0.47420507750222446
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 2.347281627650706e-31,
-            "angularVelocity": 3.940696757721575e-8,
-            "velocityX": 0.7907971475606952,
-            "velocityY": -1.0039814487748917e-7,
+            "heading": 1.246024226890136e-41,
+            "angularVelocity": -1.7065260802739357e-21,
+            "velocityX": 0.7907971475606789,
+            "velocityY": -3.1542839688982834e-34,
             "timestamp": 0.5690460930026694
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -9.04607706846012e-36,
+            "heading": 7.131086050293936e-41,
+            "angularVelocity": 6.207915462550707e-40,
             "velocityX": 0,
-            "velocityY": -2.7063013184679635e-30,
+            "velocityY": 0,
             "timestamp": 0.6638871085031143
         }
     ]
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.traj
index 63887ddf..bb0e9680 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.traj	
@@ -3,73 +3,73 @@
         {
             "x": 1.35,
             "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -1.1658790467474463e-32,
-            "velocityY": 2.7665844766493316e-30,
+            "heading": 7.039472547405692e-42,
+            "angularVelocity": 1.0705191064262213e-40,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 1.4250000045295077,
-            "y": 5.560000008699219,
-            "heading": -5.237332222253062e-9,
-            "angularVelocity": -5.5222228614505115e-8,
-            "velocityX": 0.7907971475606554,
-            "velocityY": 9.172423930862072e-8,
-            "timestamp": 0.09484101550044491
+            "x": 1.4250000045295101,
+            "y": 5.56,
+            "heading": 8.194631828276944e-23,
+            "angularVelocity": 8.6403876700238e-22,
+            "velocityX": 0.7907971475606789,
+            "velocityY": 8.424071056371489e-35,
+            "timestamp": 0.0948410155004449
         },
         {
-            "x": 1.575000009059013,
-            "y": 5.560000023677869,
-            "heading": -1.3214169082837694e-8,
-            "angularVelocity": -8.41074599041897e-8,
-            "velocityX": 1.5815942473623306,
-            "velocityY": 1.579343082639882e-7,
-            "timestamp": 0.18968203100088982
+            "x": 1.5750000090590195,
+            "y": 5.56,
+            "heading": 2.6954329855279996e-22,
+            "angularVelocity": 1.978015178210688e-21,
+            "velocityX": 1.5815942473623736,
+            "velocityY": 1.2488550168110521e-34,
+            "timestamp": 0.1896820310008898
         },
         {
-            "x": 1.7999999999999883,
-            "y": 5.560000042361517,
-            "heading": -2.1609400534628168e-8,
-            "angularVelocity": -8.851899686654573e-8,
-            "velocityX": 2.372391203887095,
-            "velocityY": 1.9699965815991792e-7,
-            "timestamp": 0.28452304650133475
+            "x": 1.7999999999999998,
+            "y": 5.56,
+            "heading": 5.87848829010012e-22,
+            "angularVelocity": 3.3562006983676933e-21,
+            "velocityX": 2.3723912038871497,
+            "velocityY": 2.135553914770072e-34,
+            "timestamp": 0.2845230465013347
         },
         {
-            "x": 2.0249999909409753,
-            "y": 5.560000024806983,
-            "heading": -1.1174048484706425e-8,
-            "angularVelocity": 1.1002994951426502e-7,
-            "velocityX": 2.372391203887216,
-            "velocityY": -1.8509433303171823e-7,
-            "timestamp": 0.37936406200177963
+            "x": 2.0249999909409806,
+            "y": 5.56,
+            "heading": 4.475372520266276e-22,
+            "angularVelocity": -1.4794393741169351e-21,
+            "velocityX": 2.37239120388715,
+            "velocityY": 2.583104828416982e-34,
+            "timestamp": 0.3793640620017796
         },
         {
-            "x": 2.1749999954704884,
-            "y": 5.560000009521861,
-            "heading": -3.737396801552092e-9,
-            "angularVelocity": 7.841176829375941e-8,
-            "velocityX": 1.581594247362411,
-            "velocityY": -1.6116572709153717e-7,
-            "timestamp": 0.4742050775022245
+            "x": 2.17499999547049,
+            "y": 5.56,
+            "heading": 1.6184867431561565e-22,
+            "angularVelocity": -3.012289189189882e-21,
+            "velocityX": 1.5815942473623736,
+            "velocityY": 1.9272829246954695e-34,
+            "timestamp": 0.47420507750222446
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 2.347281627650706e-31,
-            "angularVelocity": 3.940696757721575e-8,
-            "velocityX": 0.7907971475606952,
-            "velocityY": -1.0039814487748917e-7,
+            "heading": 1.246024226890136e-41,
+            "angularVelocity": -1.7065260802739357e-21,
+            "velocityX": 0.7907971475606789,
+            "velocityY": -3.1542839688982834e-34,
             "timestamp": 0.5690460930026694
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -9.04607706846012e-36,
+            "heading": 7.131086050293936e-41,
+            "angularVelocity": 6.207915462550707e-40,
             "velocityX": 0,
-            "velocityY": -2.7063013184679635e-30,
+            "velocityY": 0,
             "timestamp": 0.6638871085031143
         }
     ]
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.1.traj
index fcfc6794..aceaac81 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.1.traj	
@@ -3,110 +3,110 @@
         {
             "x": 1.35,
             "y": 5.56,
-            "heading": -1.4926991504029801e-31,
-            "angularVelocity": 3.0130798970250824e-30,
-            "velocityX": -6.359546450907426e-32,
-            "velocityY": -1.6682795346112646e-37,
+            "heading": 0,
+            "angularVelocity": 5.56203910119815e-35,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 1.381665864471773,
-            "y": 5.519999051141232,
-            "heading": -0.0129840210078559,
-            "angularVelocity": -0.16577982445307443,
-            "velocityX": 0.40430937766578573,
-            "velocityY": -0.5107316351191263,
-            "timestamp": 0.07832087559925201
+            "x": 1.3816658774685813,
+            "y": 5.519999060819193,
+            "heading": -0.012983997100427386,
+            "angularVelocity": -0.16577951920330813,
+            "velocityX": 0.4043095436088268,
+            "velocityY": -0.5107315115509736,
+            "timestamp": 0.07832087559926142
         },
         {
-            "x": 1.4449977408289922,
-            "y": 5.439997231153384,
-            "heading": -0.03894969123495126,
-            "angularVelocity": -0.3315293659375504,
-            "velocityX": 0.8086206375075861,
-            "velocityY": -1.0214622777865288,
-            "timestamp": 0.15664175119850401
+            "x": 1.4449977779940397,
+            "y": 5.439997258819368,
+            "heading": -0.038949622534069785,
+            "angularVelocity": -0.33152879401526647,
+            "velocityX": 0.8086209460872757,
+            "velocityY": -1.0214620481155445,
+            "timestamp": 0.15664175119852283
         },
         {
-            "x": 1.5399958493613461,
-            "y": 5.319994671739442,
-            "heading": -0.07789445681060216,
-            "angularVelocity": -0.4972462996317751,
-            "velocityX": 1.212934710005481,
-            "velocityY": -1.532191238871801,
-            "timestamp": 0.23496262679775604
+            "x": 1.5399959199593471,
+            "y": 5.3199947242673264,
+            "heading": -0.07789432529768456,
+            "angularVelocity": -0.497245497648426,
+            "velocityX": 1.2129351368768808,
+            "velocityY": -1.5321909214351659,
+            "timestamp": 0.23496262679778424
         },
         {
-            "x": 1.6666604646819347,
-            "y": 5.1599915852165825,
-            "heading": -0.12981761618342352,
-            "angularVelocity": -0.6629542759263352,
-            "velocityX": 1.6172522887601493,
-            "velocityY": -2.042917489093896,
-            "timestamp": 0.31328350239700803
+            "x": 1.6666605759505058,
+            "y": 5.159991667949077,
+            "heading": -0.1298174067167751,
+            "angularVelocity": -0.662953280613991,
+            "velocityX": 1.6172528080412967,
+            "velocityY": -2.042917103441561,
+            "timestamp": 0.31328350239704567
         },
         {
-            "x": 1.8249918742420412,
-            "y": 4.95998829498017,
-            "heading": -0.19472302000870664,
-            "angularVelocity": -0.8287114173414627,
-            "velocityX": 2.0215735376893504,
-            "velocityY": -2.5536396102078434,
-            "timestamp": 0.39160437799626
+            "x": 1.8249920311985446,
+            "y": 4.959988411604181,
+            "heading": -0.19472272142711608,
+            "angularVelocity": -0.828710279522869,
+            "velocityX": 2.0215741210320677,
+            "velocityY": -2.5536391774810774,
+            "timestamp": 0.3916043779963071
         },
         {
-            "x": 1.9833278418494473,
-            "y": 4.759991864222915,
-            "heading": -0.25979913954005024,
-            "angularVelocity": -0.8308911134284025,
-            "velocityX": 2.0216317347825643,
-            "velocityY": -2.5535520284602864,
-            "timestamp": 0.469925253595512
+            "x": 1.9833279596837488,
+            "y": 4.759991951944026,
+            "heading": -0.259798921868918,
+            "angularVelocity": -0.8308921464919682,
+            "velocityX": 2.021631235270526,
+            "velocityY": -2.553552397491847,
+            "timestamp": 0.4699252535955685
         },
         {
-            "x": 2.1099966770232617,
-            "y": 4.599994992856252,
-            "heading": -0.31187323961880925,
-            "angularVelocity": -0.6648814850489074,
-            "velocityX": 1.6173061677954905,
-            "velocityY": -2.0428381340541457,
-            "timestamp": 0.548246129194764
+            "x": 2.1099967561163506,
+            "y": 4.599995051908685,
+            "heading": -0.3118731002379449,
+            "angularVelocity": -0.6648824846579983,
+            "velocityX": 1.6173056731479674,
+            "velocityY": -2.042838500095239,
+            "timestamp": 0.54824612919483
         },
         {
-            "x": 2.204998325241535,
-            "y": 4.479997471163016,
-            "heading": -0.3509354488627182,
-            "angularVelocity": -0.49874581897910997,
-            "velocityX": 1.212979904672827,
-            "velocityY": -1.5321269173142713,
-            "timestamp": 0.6265670047940161
+            "x": 2.2049983692603425,
+            "y": 4.4799975041635465,
+            "heading": -0.3509353765811358,
+            "angularVelocity": -0.4987466757018699,
+            "velocityX": 1.2129794568446812,
+            "velocityY": -1.532127249944463,
+            "timestamp": 0.6265670047940913
         },
         {
-            "x": 2.2683327695206965,
-            "y": 4.399999160989759,
-            "heading": -0.37697877943637265,
-            "angularVelocity": -0.33252093231071334,
-            "velocityX": 0.8086534247041309,
-            "velocityY": -1.0214174645159662,
-            "timestamp": 0.7048878803932681
+            "x": 2.268332785802464,
+            "y": 4.399999173265512,
+            "heading": -0.3769787554180828,
+            "angularVelocity": -0.33252154853581545,
+            "velocityX": 0.808653070557838,
+            "velocityY": -1.021417729129522,
+            "timestamp": 0.7048878803933527
         },
         {
             "x": 2.3,
             "y": 4.36,
             "heading": -0.39,
-            "angularVelocity": -0.16625478793487553,
-            "velocityX": 0.40432681883354604,
-            "velocityY": -0.5107088076290647,
-            "timestamp": 0.7832087559925202
+            "angularVelocity": -0.16625509460009477,
+            "velocityX": 0.40432661094808764,
+            "velocityY": -0.5107089643656818,
+            "timestamp": 0.7832087559926141
         },
         {
             "x": 2.3,
             "y": 4.36,
             "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 7.259744325812885e-33,
-            "velocityY": -1.1119528318776295e-29,
-            "timestamp": 0.8615296315917722
+            "angularVelocity": -5.703180031220666e-32,
+            "velocityX": 1.0257416304055705e-32,
+            "velocityY": 0,
+            "timestamp": 0.8615296315918755
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.traj
index fcfc6794..aceaac81 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.traj	
@@ -3,110 +3,110 @@
         {
             "x": 1.35,
             "y": 5.56,
-            "heading": -1.4926991504029801e-31,
-            "angularVelocity": 3.0130798970250824e-30,
-            "velocityX": -6.359546450907426e-32,
-            "velocityY": -1.6682795346112646e-37,
+            "heading": 0,
+            "angularVelocity": 5.56203910119815e-35,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 1.381665864471773,
-            "y": 5.519999051141232,
-            "heading": -0.0129840210078559,
-            "angularVelocity": -0.16577982445307443,
-            "velocityX": 0.40430937766578573,
-            "velocityY": -0.5107316351191263,
-            "timestamp": 0.07832087559925201
+            "x": 1.3816658774685813,
+            "y": 5.519999060819193,
+            "heading": -0.012983997100427386,
+            "angularVelocity": -0.16577951920330813,
+            "velocityX": 0.4043095436088268,
+            "velocityY": -0.5107315115509736,
+            "timestamp": 0.07832087559926142
         },
         {
-            "x": 1.4449977408289922,
-            "y": 5.439997231153384,
-            "heading": -0.03894969123495126,
-            "angularVelocity": -0.3315293659375504,
-            "velocityX": 0.8086206375075861,
-            "velocityY": -1.0214622777865288,
-            "timestamp": 0.15664175119850401
+            "x": 1.4449977779940397,
+            "y": 5.439997258819368,
+            "heading": -0.038949622534069785,
+            "angularVelocity": -0.33152879401526647,
+            "velocityX": 0.8086209460872757,
+            "velocityY": -1.0214620481155445,
+            "timestamp": 0.15664175119852283
         },
         {
-            "x": 1.5399958493613461,
-            "y": 5.319994671739442,
-            "heading": -0.07789445681060216,
-            "angularVelocity": -0.4972462996317751,
-            "velocityX": 1.212934710005481,
-            "velocityY": -1.532191238871801,
-            "timestamp": 0.23496262679775604
+            "x": 1.5399959199593471,
+            "y": 5.3199947242673264,
+            "heading": -0.07789432529768456,
+            "angularVelocity": -0.497245497648426,
+            "velocityX": 1.2129351368768808,
+            "velocityY": -1.5321909214351659,
+            "timestamp": 0.23496262679778424
         },
         {
-            "x": 1.6666604646819347,
-            "y": 5.1599915852165825,
-            "heading": -0.12981761618342352,
-            "angularVelocity": -0.6629542759263352,
-            "velocityX": 1.6172522887601493,
-            "velocityY": -2.042917489093896,
-            "timestamp": 0.31328350239700803
+            "x": 1.6666605759505058,
+            "y": 5.159991667949077,
+            "heading": -0.1298174067167751,
+            "angularVelocity": -0.662953280613991,
+            "velocityX": 1.6172528080412967,
+            "velocityY": -2.042917103441561,
+            "timestamp": 0.31328350239704567
         },
         {
-            "x": 1.8249918742420412,
-            "y": 4.95998829498017,
-            "heading": -0.19472302000870664,
-            "angularVelocity": -0.8287114173414627,
-            "velocityX": 2.0215735376893504,
-            "velocityY": -2.5536396102078434,
-            "timestamp": 0.39160437799626
+            "x": 1.8249920311985446,
+            "y": 4.959988411604181,
+            "heading": -0.19472272142711608,
+            "angularVelocity": -0.828710279522869,
+            "velocityX": 2.0215741210320677,
+            "velocityY": -2.5536391774810774,
+            "timestamp": 0.3916043779963071
         },
         {
-            "x": 1.9833278418494473,
-            "y": 4.759991864222915,
-            "heading": -0.25979913954005024,
-            "angularVelocity": -0.8308911134284025,
-            "velocityX": 2.0216317347825643,
-            "velocityY": -2.5535520284602864,
-            "timestamp": 0.469925253595512
+            "x": 1.9833279596837488,
+            "y": 4.759991951944026,
+            "heading": -0.259798921868918,
+            "angularVelocity": -0.8308921464919682,
+            "velocityX": 2.021631235270526,
+            "velocityY": -2.553552397491847,
+            "timestamp": 0.4699252535955685
         },
         {
-            "x": 2.1099966770232617,
-            "y": 4.599994992856252,
-            "heading": -0.31187323961880925,
-            "angularVelocity": -0.6648814850489074,
-            "velocityX": 1.6173061677954905,
-            "velocityY": -2.0428381340541457,
-            "timestamp": 0.548246129194764
+            "x": 2.1099967561163506,
+            "y": 4.599995051908685,
+            "heading": -0.3118731002379449,
+            "angularVelocity": -0.6648824846579983,
+            "velocityX": 1.6173056731479674,
+            "velocityY": -2.042838500095239,
+            "timestamp": 0.54824612919483
         },
         {
-            "x": 2.204998325241535,
-            "y": 4.479997471163016,
-            "heading": -0.3509354488627182,
-            "angularVelocity": -0.49874581897910997,
-            "velocityX": 1.212979904672827,
-            "velocityY": -1.5321269173142713,
-            "timestamp": 0.6265670047940161
+            "x": 2.2049983692603425,
+            "y": 4.4799975041635465,
+            "heading": -0.3509353765811358,
+            "angularVelocity": -0.4987466757018699,
+            "velocityX": 1.2129794568446812,
+            "velocityY": -1.532127249944463,
+            "timestamp": 0.6265670047940913
         },
         {
-            "x": 2.2683327695206965,
-            "y": 4.399999160989759,
-            "heading": -0.37697877943637265,
-            "angularVelocity": -0.33252093231071334,
-            "velocityX": 0.8086534247041309,
-            "velocityY": -1.0214174645159662,
-            "timestamp": 0.7048878803932681
+            "x": 2.268332785802464,
+            "y": 4.399999173265512,
+            "heading": -0.3769787554180828,
+            "angularVelocity": -0.33252154853581545,
+            "velocityX": 0.808653070557838,
+            "velocityY": -1.021417729129522,
+            "timestamp": 0.7048878803933527
         },
         {
             "x": 2.3,
             "y": 4.36,
             "heading": -0.39,
-            "angularVelocity": -0.16625478793487553,
-            "velocityX": 0.40432681883354604,
-            "velocityY": -0.5107088076290647,
-            "timestamp": 0.7832087559925202
+            "angularVelocity": -0.16625509460009477,
+            "velocityX": 0.40432661094808764,
+            "velocityY": -0.5107089643656818,
+            "timestamp": 0.7832087559926141
         },
         {
             "x": 2.3,
             "y": 4.36,
             "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 7.259744325812885e-33,
-            "velocityY": -1.1119528318776295e-29,
-            "timestamp": 0.8615296315917722
+            "angularVelocity": -5.703180031220666e-32,
+            "velocityX": 1.0257416304055705e-32,
+            "velocityY": 0,
+            "timestamp": 0.8615296315918755
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.1.traj
index f3a7c6d5..818f5ad4 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.1.traj	
@@ -5,288 +5,333 @@
             "y": 4.36,
             "heading": -1.04,
             "angularVelocity": 0,
-            "velocityX": -3.7161803829578067e-31,
-            "velocityY": -1.7109886686245282e-31,
+            "velocityX": 0,
+            "velocityY": 1.4815405069278332e-31,
             "timestamp": 0
         },
         {
-            "x": 0.755738680236623,
-            "y": 4.330590345332975,
-            "heading": -0.988927755482241,
-            "angularVelocity": 0.6221231913092291,
-            "velocityX": 0.5571537727343439,
-            "velocityY": -0.35824601776392057,
-            "timestamp": 0.08209345870916634
-        },
-        {
-            "x": 0.8475293838257123,
-            "y": 4.27179448486036,
-            "heading": -0.8901763080939228,
-            "angularVelocity": 1.2029149330663034,
-            "velocityX": 1.1181244526957685,
-            "velocityY": -0.7162063993540811,
-            "timestamp": 0.1641869174183327
-        },
-        {
-            "x": 0.9859043696516769,
-            "y": 4.183626202129818,
-            "heading": -0.7509222083756354,
-            "angularVelocity": 1.6962874010659592,
-            "velocityX": 1.6855787050730526,
-            "velocityY": -1.0739988802627505,
-            "timestamp": 0.24628037612749903
-        },
-        {
-            "x": 1.1717528122715652,
-            "y": 4.066081969426692,
-            "heading": -0.5886701024002182,
-            "angularVelocity": 1.9764316003572233,
-            "velocityX": 2.263864204799754,
-            "velocityY": -1.4318343331050452,
-            "timestamp": 0.3283738348366654
-        },
-        {
-            "x": 1.404824377392605,
-            "y": 3.921350877318508,
-            "heading": -0.4767670307510258,
-            "angularVelocity": 1.3631180048782332,
-            "velocityX": 2.8391003228009395,
-            "velocityY": -1.7630039516415732,
-            "timestamp": 0.4104672935458317
-        },
-        {
-            "x": 1.6657642530728491,
-            "y": 3.752962692607369,
-            "heading": -0.4767669793872945,
-            "angularVelocity": 6.256738607298855e-7,
-            "velocityX": 3.1785708603736573,
-            "velocityY": -2.051176638904815,
-            "timestamp": 0.49256075225499807
-        },
-        {
-            "x": 1.926704086710966,
-            "y": 3.584574442746028,
-            "heading": -0.476766928024127,
-            "angularVelocity": 6.256669936190949e-7,
-            "velocityX": 3.178570348248477,
-            "velocityY": -2.051177432514957,
-            "timestamp": 0.5746542109641644
-        },
-        {
-            "x": 2.187643960670788,
-            "y": 3.416186255368488,
-            "heading": -0.47676687666098916,
-            "angularVelocity": 6.256666327585045e-7,
-            "velocityX": 3.178570839416784,
-            "velocityY": -2.0511766713848885,
-            "timestamp": 0.6567476696733308
-        },
-        {
-            "x": 2.448583988107283,
-            "y": 3.2477983058236712,
-            "heading": -0.4767668252978414,
-            "angularVelocity": 6.256667527167073e-7,
-            "velocityX": 3.1785727089527405,
-            "velocityY": -2.051173774287752,
-            "timestamp": 0.7388411283824972
-        },
-        {
-            "x": 2.7194719503347105,
-            "y": 3.095930240616485,
-            "heading": -0.47676675593417933,
-            "angularVelocity": 8.449353114908744e-7,
-            "velocityX": 3.2997508752445928,
-            "velocityY": -1.8499411231437015,
-            "timestamp": 0.8209345870916636
-        },
-        {
-            "x": 2.9903602222167747,
-            "y": 2.9440627277435274,
-            "heading": -0.4767666865702561,
-            "angularVelocity": 8.449384925343815e-7,
-            "velocityX": 3.2997546472215804,
-            "velocityY": -1.8499343950287315,
-            "timestamp": 0.90302804580083
-        },
-        {
-            "x": 3.2612486310597473,
-            "y": 2.7921954591699243,
-            "heading": -0.4767666172064318,
-            "angularVelocity": 8.44937286881618e-7,
-            "velocityX": 3.299756315575062,
-            "velocityY": -1.8499314191600278,
-            "timestamp": 0.9851215045099964
-        },
-        {
-            "x": 3.5321371803805834,
-            "y": 2.6403284411694066,
-            "heading": -0.4767665478428475,
-            "angularVelocity": 8.449343642301634e-7,
-            "velocityX": 3.2997580267694278,
-            "velocityY": -1.84992836686951,
-            "timestamp": 1.0672149632191628
-        },
-        {
-            "x": 3.8030257968728702,
-            "y": 2.488461542984137,
-            "heading": -0.47676647847929754,
-            "angularVelocity": 8.449339449505073e-7,
-            "velocityX": 3.2997588450008877,
-            "velocityY": -1.849926907371401,
-            "timestamp": 1.1493084219283292
-        },
-        {
-            "x": 4.07391446115854,
-            "y": 2.336594730049097,
-            "heading": -0.4767664091157508,
-            "angularVelocity": 8.449339066268147e-7,
-            "velocityX": 3.2997594271834845,
-            "velocityY": -1.8499258689180111,
-            "timestamp": 1.2314018806374956
-        },
-        {
-            "x": 4.344803172855204,
-            "y": 2.184728001682272,
-            "heading": -0.4767663397522045,
-            "angularVelocity": 8.449339005791607e-7,
-            "velocityX": 3.2997600047081037,
-            "velocityY": -1.8499248387724243,
-            "timestamp": 1.313495339346662
-        },
-        {
-            "x": 4.6156919675948584,
-            "y": 2.0328614214415324,
-            "heading": -0.47676627038863556,
-            "angularVelocity": 8.449341769958558e-7,
-            "velocityX": 3.2997610162745823,
-            "velocityY": -1.8499230344132471,
-            "timestamp": 1.3955887980558284
-        },
-        {
-            "x": 4.88903699417146,
-            "y": 1.8854616483024196,
-            "heading": -0.4767661960128138,
-            "angularVelocity": 9.059896238747082e-7,
-            "velocityX": 3.3296809620969254,
-            "velocityY": -1.7955117917653842,
-            "timestamp": 1.4776822567649948
-        },
-        {
-            "x": 5.164941930672226,
-            "y": 1.7429110878363532,
-            "heading": -0.47676611821696785,
-            "angularVelocity": 9.476497536578291e-7,
-            "velocityX": 3.3608638354271223,
-            "velocityY": -1.736442375647519,
-            "timestamp": 1.5597757154741612
-        },
-        {
-            "x": 5.4408826630316725,
-            "y": 1.6004298311654348,
-            "heading": -0.4767660404421832,
-            "angularVelocity": 9.473932014254074e-7,
-            "velocityX": 3.3612998733190906,
-            "velocityY": -1.7355981695897218,
-            "timestamp": 1.6418691741833276
-        },
-        {
-            "x": 5.73313461063715,
-            "y": 1.4953917422787115,
-            "heading": -0.4767660015440003,
-            "angularVelocity": 4.738280431565268e-7,
-            "velocityX": 3.55999067649034,
-            "velocityY": -1.2794940125356749,
-            "timestamp": 1.723962632892494
-        },
-        {
-            "x": 6.025386917456382,
-            "y": 1.3903546528530497,
-            "heading": -0.476765962645991,
-            "angularVelocity": 4.738259283996548e-7,
-            "velocityX": 3.5599950521587655,
-            "velocityY": -1.2794818378621156,
-            "timestamp": 1.8060560916016604
-        },
-        {
-            "x": 6.31763925599377,
-            "y": 1.2853176516791878,
-            "heading": -0.4767659237479627,
-            "angularVelocity": 4.7382616041897537e-7,
-            "velocityX": 3.5599954385251906,
-            "velocityY": -1.2794807628458942,
-            "timestamp": 1.8881495503108268
-        },
-        {
-            "x": 6.609891614856598,
-            "y": 1.1802807070583716,
-            "heading": -0.4767658848499098,
-            "angularVelocity": 4.7382646005084667e-7,
-            "velocityX": 3.5599956861142137,
-            "velocityY": -1.279480073959752,
-            "timestamp": 1.9702430090199932
-        },
-        {
-            "x": 6.9021439611938415,
-            "y": 1.0752437275870097,
-            "heading": -0.4767658459515333,
-            "angularVelocity": 4.738304011500936e-7,
-            "velocityX": 3.559995533537077,
-            "velocityY": -1.2794804984825638,
-            "timestamp": 2.0523364677291593
-        },
-        {
-            "x": 7.1615588383732955,
-            "y": 0.9774297585720934,
-            "heading": -0.3677133060860067,
-            "angularVelocity": 1.3283949973635365,
-            "velocityX": 3.159994489920166,
-            "velocityY": -1.1914952854093204,
-            "timestamp": 2.1344299264383255
-        },
-        {
-            "x": 7.369316955835238,
-            "y": 0.9006256752646921,
-            "heading": -0.23381405774557934,
-            "angularVelocity": 1.6310586792888595,
-            "velocityX": 2.5307511795546396,
-            "velocityY": -0.9355688567038716,
-            "timestamp": 2.2165233851474917
-        },
-        {
-            "x": 7.5248080438305305,
-            "y": 0.8435895899389048,
-            "heading": -0.12113401657570753,
-            "angularVelocity": 1.3725824559185034,
-            "velocityX": 1.894074027824225,
-            "velocityY": -0.6947701585804769,
-            "timestamp": 2.298616843856658
-        },
-        {
-            "x": 7.628310039444481,
-            "y": 0.8058059657857352,
-            "heading": -0.04143578092949631,
-            "angularVelocity": 0.9708232166043739,
-            "velocityX": 1.2607824940185441,
-            "velocityY": -0.4602513372840855,
-            "timestamp": 2.380710302565824
+            "x": 0.7317173879428727,
+            "y": 4.344479531295635,
+            "heading": -1.013582317544266,
+            "angularVelocity": 0.4585462516411977,
+            "velocityX": 0.37696065327942924,
+            "velocityY": -0.26939731598432753,
+            "timestamp": 0.05761181638969176
+        },
+        {
+            "x": 0.7752730705207751,
+            "y": 4.313424298338961,
+            "heading": -0.9621480921974644,
+            "angularVelocity": 0.892772152832256,
+            "velocityX": 0.7560199505477795,
+            "velocityY": -0.5390427676609577,
+            "timestamp": 0.11522363277938352
+        },
+        {
+            "x": 0.8408250898493527,
+            "y": 4.266812487548848,
+            "heading": -0.8877439224939486,
+            "angularVelocity": 1.2914741170498636,
+            "velocityX": 1.1378224717856051,
+            "velocityY": -0.8090668496689294,
+            "timestamp": 0.17283544916907528
+        },
+        {
+            "x": 0.9285847241631874,
+            "y": 4.204600540366139,
+            "heading": -0.7936992773737622,
+            "angularVelocity": 1.6323846567179892,
+            "velocityX": 1.5232922656043415,
+            "velocityY": -1.0798470015578314,
+            "timestamp": 0.23044726555876704
+        },
+        {
+            "x": 1.0388213735909644,
+            "y": 4.1267152282410935,
+            "heading": -0.6856850144743787,
+            "angularVelocity": 1.8748629997840256,
+            "velocityX": 1.9134381857035365,
+            "velocityY": -1.3518982216811457,
+            "timestamp": 0.2880590819484588
+        },
+        {
+            "x": 1.1718232312823271,
+            "y": 4.033123229014291,
+            "heading": -0.5741627817643435,
+            "angularVelocity": 1.935752762171023,
+            "velocityX": 2.3085864328894954,
+            "velocityY": -1.6245278328622093,
+            "timestamp": 0.34567089833815057
+        },
+        {
+            "x": 1.3270687303513053,
+            "y": 3.9246703032584405,
+            "heading": -0.488682735559766,
+            "angularVelocity": 1.4837242003685942,
+            "velocityX": 2.6946815566251776,
+            "velocityY": -1.882477112373353,
+            "timestamp": 0.40328271472784233
+        },
+        {
+            "x": 1.5010681185276094,
+            "y": 3.804139759424889,
+            "heading": -0.46956462349945394,
+            "angularVelocity": 0.3318435914430377,
+            "velocityX": 3.020203129846763,
+            "velocityY": -2.0921149754812625,
+            "timestamp": 0.4608945311175341
+        },
+        {
+            "x": 1.6785494425409546,
+            "y": 3.677652137140934,
+            "heading": -0.4695645649556455,
+            "angularVelocity": 0.0000010161770978802793,
+            "velocityX": 3.080641006227699,
+            "velocityY": -2.195515264236428,
+            "timestamp": 0.5185063475072258
+        },
+        {
+            "x": 1.85603075289391,
+            "y": 3.551164495689284,
+            "heading": -0.4695645064119953,
+            "angularVelocity": 0.000001016174352223477,
+            "velocityX": 3.080640769116793,
+            "velocityY": -2.1955155969406546,
+            "timestamp": 0.5761181638969175
+        },
+        {
+            "x": 2.033512086622271,
+            "y": 3.4246768870368545,
+            "heading": -0.4695644478683731,
+            "angularVelocity": 0.0000010161738659585955,
+            "velocityX": 3.0806411748565776,
+            "velocityY": -2.195515027626543,
+            "timestamp": 0.6337299802866092
+        },
+        {
+            "x": 2.2109934521039283,
+            "y": 3.298189322939119,
+            "heading": -0.46956438932476746,
+            "angularVelocity": 0.0000010161735782211743,
+            "velocityX": 3.080641726016013,
+            "velocityY": -2.1955142542661994,
+            "timestamp": 0.6913417966763009
+        },
+        {
+            "x": 2.3884748618363862,
+            "y": 3.17170182093203,
+            "heading": -0.4695643307811488,
+            "angularVelocity": 0.000001016173805422874,
+            "velocityX": 3.0806424941015065,
+            "velocityY": -2.1955131765246763,
+            "timestamp": 0.7489536130659926
+        },
+        {
+            "x": 2.5659563234551954,
+            "y": 3.045214391729511,
+            "heading": -0.4695642722374779,
+            "angularVelocity": 0.0000010161747112988211,
+            "velocityX": 3.0806433947214558,
+            "velocityY": -2.1955119128156926,
+            "timestamp": 0.8065654294556843
+        },
+        {
+            "x": 2.743437831089148,
+            "y": 2.918727027093402,
+            "heading": -0.46956421369376844,
+            "angularVelocity": 0.000001016175381373278,
+            "velocityX": 3.08064419343162,
+            "velocityY": -2.1955107921009867,
+            "timestamp": 0.864177245845376
+        },
+        {
+            "x": 2.920919379832659,
+            "y": 2.7922397201404365,
+            "heading": -0.4695641551500532,
+            "angularVelocity": 0.0000010161754817816058,
+            "velocityX": 3.080644906992851,
+            "velocityY": -2.195509790862922,
+            "timestamp": 0.9217890622350677
+        },
+        {
+            "x": 3.0984009676669872,
+            "y": 2.6657524680380327,
+            "heading": -0.4695640966063568,
+            "angularVelocity": 0.000001016175153333812,
+            "velocityX": 3.080645585513673,
+            "velocityY": -2.195508838791537,
+            "timestamp": 0.9794008786247594
+        },
+        {
+            "x": 3.275882647398041,
+            "y": 2.5392653448813247,
+            "heading": -0.4695640380626867,
+            "angularVelocity": 0.0000010161746978103879,
+            "velocityX": 3.080647180615714,
+            "velocityY": -2.1955066006101345,
+            "timestamp": 1.0370126950144511
+        },
+        {
+            "x": 3.4563703536987305,
+            "y": 2.417105913162231,
+            "heading": -0.4695639788949644,
+            "angularVelocity": 0.0000010270067149252654,
+            "velocityX": 3.1328244379564545,
+            "velocityY": -2.1203884788634437,
+            "timestamp": 1.0946245114041429
+        },
+        {
+            "x": 3.7578524899325245,
+            "y": 2.288322056940197,
+            "heading": -0.4695639274563231,
+            "angularVelocity": 5.9355601779034e-7,
+            "velocityX": 3.4788348155609183,
+            "velocityY": -1.4860507766867785,
+            "timestamp": 1.1812863255510104
+        },
+        {
+            "x": 4.061243078218464,
+            "y": 2.1641008385808727,
+            "heading": -0.46956387627292245,
+            "angularVelocity": 5.906107685245515e-7,
+            "velocityX": 3.5008566491785738,
+            "velocityY": -1.43340200735705,
+            "timestamp": 1.267948139697878
+        },
+        {
+            "x": 4.364633703659485,
+            "y": 2.039879710966971,
+            "heading": -0.46956382508952316,
+            "angularVelocity": 5.906107531304183e-7,
+            "velocityX": 3.5008570779150574,
+            "velocityY": -1.433400960235855,
+            "timestamp": 1.3546099538447456
+        },
+        {
+            "x": 4.668024366282006,
+            "y": 1.9156586741630988,
+            "heading": -0.46956377390610504,
+            "angularVelocity": 5.906109701114576e-7,
+            "velocityX": 3.5008575069563967,
+            "velocityY": -1.4333999123691519,
+            "timestamp": 1.4412717679916132
+        },
+        {
+            "x": 4.971415087165123,
+            "y": 1.7914377796518506,
+            "heading": -0.4695637227226054,
+            "angularVelocity": 5.906119107964098e-7,
+            "velocityX": 3.5008581792316793,
+            "velocityY": -1.4333982704392505,
+            "timestamp": 1.5279335821384807
+        },
+        {
+            "x": 5.2748058711699946,
+            "y": 1.6672170393060612,
+            "heading": -0.4695636715390663,
+            "angularVelocity": 5.906123657897232e-7,
+            "velocityX": 3.5008589076003997,
+            "velocityY": -1.4333964915074275,
+            "timestamp": 1.6145953962853483
+        },
+        {
+            "x": 5.5781967938554144,
+            "y": 1.5429966376674569,
+            "heading": -0.4695636203556594,
+            "angularVelocity": 5.906108405690599e-7,
+            "velocityX": 3.5008605078501667,
+            "velocityY": -1.4333925831287677,
+            "timestamp": 1.701257210432216
+        },
+        {
+            "x": 5.886988948403778,
+            "y": 1.4328868446307816,
+            "heading": -0.4695635793301605,
+            "angularVelocity": 4.733976467078746e-7,
+            "velocityX": 3.5631859035981908,
+            "velocityY": -1.2705687518849984,
+            "timestamp": 1.7879190245790835
+        },
+        {
+            "x": 6.195781169913944,
+            "y": 1.3227772393821355,
+            "heading": -0.4695635383046595,
+            "angularVelocity": 4.7339767079441426e-7,
+            "velocityX": 3.5631866762776085,
+            "velocityY": -1.2705665849787193,
+            "timestamp": 1.874580838725951
+        },
+        {
+            "x": 6.5045734104803365,
+            "y": 1.212667687574965,
+            "heading": -0.46956349727913566,
+            "angularVelocity": 4.733979354519185e-7,
+            "velocityX": 3.5631868961694693,
+            "velocityY": -1.270565968311784,
+            "timestamp": 1.9612426528728186
+        },
+        {
+            "x": 6.813365656566466,
+            "y": 1.1025581512474965,
+            "heading": -0.46956345625352064,
+            "angularVelocity": 4.733989867793667e-7,
+            "velocityX": 3.563186959862305,
+            "velocityY": -1.270565789689833,
+            "timestamp": 2.0479044670196864
+        },
+        {
+            "x": 7.101065618447083,
+            "y": 0.9964425714529646,
+            "heading": -0.4012732100130825,
+            "angularVelocity": 0.7880085007764233,
+            "velocityX": 3.3198008224596522,
+            "velocityY": -1.2244790954259896,
+            "timestamp": 2.134566281166554
+        },
+        {
+            "x": 7.333266238045498,
+            "y": 0.911992130350644,
+            "heading": -0.2613611242014092,
+            "angularVelocity": 1.6144606155435655,
+            "velocityX": 2.6793879390165927,
+            "velocityY": -0.9744827284506232,
+            "timestamp": 2.2212280953134216
+        },
+        {
+            "x": 7.506890250099573,
+            "y": 0.8492417427348896,
+            "heading": -0.1371754084070899,
+            "angularVelocity": 1.43299233943868,
+            "velocityX": 2.0034661605379225,
+            "velocityY": -0.7240834759056616,
+            "timestamp": 2.307889909460289
+        },
+        {
+            "x": 7.622366585478705,
+            "y": 0.8076799304753898,
+            "heading": -0.04732610702177306,
+            "angularVelocity": 1.0367807582823931,
+            "velocityX": 1.332493861522807,
+            "velocityY": -0.4795862245517285,
+            "timestamp": 2.3945517236071567
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 6.308726444888931e-32,
-            "angularVelocity": 0.5047391300236409,
-            "velocityX": 0.6296477376917574,
-            "velocityY": -0.22907995449868002,
-            "timestamp": 2.4628037612749902
+            "heading": 2.9489608951695904e-31,
+            "angularVelocity": 0.5461010421679925,
+            "velocityX": 0.6650381726791765,
+            "velocityY": -0.2386279433331854,
+            "timestamp": 2.4812135377540243
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -2.9272372134325455e-30,
-            "velocityX": -7.795387702917406e-31,
-            "velocityY": 4.863651986550052e-31,
-            "timestamp": 2.5448972199841564
+            "heading": -7.162012659830636e-32,
+            "angularVelocity": -4.0878892657023166e-30,
+            "velocityX": 2.3668785600931567e-32,
+            "velocityY": 6.81692291678628e-32,
+            "timestamp": 2.567875351900892
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.traj
index f3a7c6d5..818f5ad4 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.traj	
@@ -5,288 +5,333 @@
             "y": 4.36,
             "heading": -1.04,
             "angularVelocity": 0,
-            "velocityX": -3.7161803829578067e-31,
-            "velocityY": -1.7109886686245282e-31,
+            "velocityX": 0,
+            "velocityY": 1.4815405069278332e-31,
             "timestamp": 0
         },
         {
-            "x": 0.755738680236623,
-            "y": 4.330590345332975,
-            "heading": -0.988927755482241,
-            "angularVelocity": 0.6221231913092291,
-            "velocityX": 0.5571537727343439,
-            "velocityY": -0.35824601776392057,
-            "timestamp": 0.08209345870916634
-        },
-        {
-            "x": 0.8475293838257123,
-            "y": 4.27179448486036,
-            "heading": -0.8901763080939228,
-            "angularVelocity": 1.2029149330663034,
-            "velocityX": 1.1181244526957685,
-            "velocityY": -0.7162063993540811,
-            "timestamp": 0.1641869174183327
-        },
-        {
-            "x": 0.9859043696516769,
-            "y": 4.183626202129818,
-            "heading": -0.7509222083756354,
-            "angularVelocity": 1.6962874010659592,
-            "velocityX": 1.6855787050730526,
-            "velocityY": -1.0739988802627505,
-            "timestamp": 0.24628037612749903
-        },
-        {
-            "x": 1.1717528122715652,
-            "y": 4.066081969426692,
-            "heading": -0.5886701024002182,
-            "angularVelocity": 1.9764316003572233,
-            "velocityX": 2.263864204799754,
-            "velocityY": -1.4318343331050452,
-            "timestamp": 0.3283738348366654
-        },
-        {
-            "x": 1.404824377392605,
-            "y": 3.921350877318508,
-            "heading": -0.4767670307510258,
-            "angularVelocity": 1.3631180048782332,
-            "velocityX": 2.8391003228009395,
-            "velocityY": -1.7630039516415732,
-            "timestamp": 0.4104672935458317
-        },
-        {
-            "x": 1.6657642530728491,
-            "y": 3.752962692607369,
-            "heading": -0.4767669793872945,
-            "angularVelocity": 6.256738607298855e-7,
-            "velocityX": 3.1785708603736573,
-            "velocityY": -2.051176638904815,
-            "timestamp": 0.49256075225499807
-        },
-        {
-            "x": 1.926704086710966,
-            "y": 3.584574442746028,
-            "heading": -0.476766928024127,
-            "angularVelocity": 6.256669936190949e-7,
-            "velocityX": 3.178570348248477,
-            "velocityY": -2.051177432514957,
-            "timestamp": 0.5746542109641644
-        },
-        {
-            "x": 2.187643960670788,
-            "y": 3.416186255368488,
-            "heading": -0.47676687666098916,
-            "angularVelocity": 6.256666327585045e-7,
-            "velocityX": 3.178570839416784,
-            "velocityY": -2.0511766713848885,
-            "timestamp": 0.6567476696733308
-        },
-        {
-            "x": 2.448583988107283,
-            "y": 3.2477983058236712,
-            "heading": -0.4767668252978414,
-            "angularVelocity": 6.256667527167073e-7,
-            "velocityX": 3.1785727089527405,
-            "velocityY": -2.051173774287752,
-            "timestamp": 0.7388411283824972
-        },
-        {
-            "x": 2.7194719503347105,
-            "y": 3.095930240616485,
-            "heading": -0.47676675593417933,
-            "angularVelocity": 8.449353114908744e-7,
-            "velocityX": 3.2997508752445928,
-            "velocityY": -1.8499411231437015,
-            "timestamp": 0.8209345870916636
-        },
-        {
-            "x": 2.9903602222167747,
-            "y": 2.9440627277435274,
-            "heading": -0.4767666865702561,
-            "angularVelocity": 8.449384925343815e-7,
-            "velocityX": 3.2997546472215804,
-            "velocityY": -1.8499343950287315,
-            "timestamp": 0.90302804580083
-        },
-        {
-            "x": 3.2612486310597473,
-            "y": 2.7921954591699243,
-            "heading": -0.4767666172064318,
-            "angularVelocity": 8.44937286881618e-7,
-            "velocityX": 3.299756315575062,
-            "velocityY": -1.8499314191600278,
-            "timestamp": 0.9851215045099964
-        },
-        {
-            "x": 3.5321371803805834,
-            "y": 2.6403284411694066,
-            "heading": -0.4767665478428475,
-            "angularVelocity": 8.449343642301634e-7,
-            "velocityX": 3.2997580267694278,
-            "velocityY": -1.84992836686951,
-            "timestamp": 1.0672149632191628
-        },
-        {
-            "x": 3.8030257968728702,
-            "y": 2.488461542984137,
-            "heading": -0.47676647847929754,
-            "angularVelocity": 8.449339449505073e-7,
-            "velocityX": 3.2997588450008877,
-            "velocityY": -1.849926907371401,
-            "timestamp": 1.1493084219283292
-        },
-        {
-            "x": 4.07391446115854,
-            "y": 2.336594730049097,
-            "heading": -0.4767664091157508,
-            "angularVelocity": 8.449339066268147e-7,
-            "velocityX": 3.2997594271834845,
-            "velocityY": -1.8499258689180111,
-            "timestamp": 1.2314018806374956
-        },
-        {
-            "x": 4.344803172855204,
-            "y": 2.184728001682272,
-            "heading": -0.4767663397522045,
-            "angularVelocity": 8.449339005791607e-7,
-            "velocityX": 3.2997600047081037,
-            "velocityY": -1.8499248387724243,
-            "timestamp": 1.313495339346662
-        },
-        {
-            "x": 4.6156919675948584,
-            "y": 2.0328614214415324,
-            "heading": -0.47676627038863556,
-            "angularVelocity": 8.449341769958558e-7,
-            "velocityX": 3.2997610162745823,
-            "velocityY": -1.8499230344132471,
-            "timestamp": 1.3955887980558284
-        },
-        {
-            "x": 4.88903699417146,
-            "y": 1.8854616483024196,
-            "heading": -0.4767661960128138,
-            "angularVelocity": 9.059896238747082e-7,
-            "velocityX": 3.3296809620969254,
-            "velocityY": -1.7955117917653842,
-            "timestamp": 1.4776822567649948
-        },
-        {
-            "x": 5.164941930672226,
-            "y": 1.7429110878363532,
-            "heading": -0.47676611821696785,
-            "angularVelocity": 9.476497536578291e-7,
-            "velocityX": 3.3608638354271223,
-            "velocityY": -1.736442375647519,
-            "timestamp": 1.5597757154741612
-        },
-        {
-            "x": 5.4408826630316725,
-            "y": 1.6004298311654348,
-            "heading": -0.4767660404421832,
-            "angularVelocity": 9.473932014254074e-7,
-            "velocityX": 3.3612998733190906,
-            "velocityY": -1.7355981695897218,
-            "timestamp": 1.6418691741833276
-        },
-        {
-            "x": 5.73313461063715,
-            "y": 1.4953917422787115,
-            "heading": -0.4767660015440003,
-            "angularVelocity": 4.738280431565268e-7,
-            "velocityX": 3.55999067649034,
-            "velocityY": -1.2794940125356749,
-            "timestamp": 1.723962632892494
-        },
-        {
-            "x": 6.025386917456382,
-            "y": 1.3903546528530497,
-            "heading": -0.476765962645991,
-            "angularVelocity": 4.738259283996548e-7,
-            "velocityX": 3.5599950521587655,
-            "velocityY": -1.2794818378621156,
-            "timestamp": 1.8060560916016604
-        },
-        {
-            "x": 6.31763925599377,
-            "y": 1.2853176516791878,
-            "heading": -0.4767659237479627,
-            "angularVelocity": 4.7382616041897537e-7,
-            "velocityX": 3.5599954385251906,
-            "velocityY": -1.2794807628458942,
-            "timestamp": 1.8881495503108268
-        },
-        {
-            "x": 6.609891614856598,
-            "y": 1.1802807070583716,
-            "heading": -0.4767658848499098,
-            "angularVelocity": 4.7382646005084667e-7,
-            "velocityX": 3.5599956861142137,
-            "velocityY": -1.279480073959752,
-            "timestamp": 1.9702430090199932
-        },
-        {
-            "x": 6.9021439611938415,
-            "y": 1.0752437275870097,
-            "heading": -0.4767658459515333,
-            "angularVelocity": 4.738304011500936e-7,
-            "velocityX": 3.559995533537077,
-            "velocityY": -1.2794804984825638,
-            "timestamp": 2.0523364677291593
-        },
-        {
-            "x": 7.1615588383732955,
-            "y": 0.9774297585720934,
-            "heading": -0.3677133060860067,
-            "angularVelocity": 1.3283949973635365,
-            "velocityX": 3.159994489920166,
-            "velocityY": -1.1914952854093204,
-            "timestamp": 2.1344299264383255
-        },
-        {
-            "x": 7.369316955835238,
-            "y": 0.9006256752646921,
-            "heading": -0.23381405774557934,
-            "angularVelocity": 1.6310586792888595,
-            "velocityX": 2.5307511795546396,
-            "velocityY": -0.9355688567038716,
-            "timestamp": 2.2165233851474917
-        },
-        {
-            "x": 7.5248080438305305,
-            "y": 0.8435895899389048,
-            "heading": -0.12113401657570753,
-            "angularVelocity": 1.3725824559185034,
-            "velocityX": 1.894074027824225,
-            "velocityY": -0.6947701585804769,
-            "timestamp": 2.298616843856658
-        },
-        {
-            "x": 7.628310039444481,
-            "y": 0.8058059657857352,
-            "heading": -0.04143578092949631,
-            "angularVelocity": 0.9708232166043739,
-            "velocityX": 1.2607824940185441,
-            "velocityY": -0.4602513372840855,
-            "timestamp": 2.380710302565824
+            "x": 0.7317173879428727,
+            "y": 4.344479531295635,
+            "heading": -1.013582317544266,
+            "angularVelocity": 0.4585462516411977,
+            "velocityX": 0.37696065327942924,
+            "velocityY": -0.26939731598432753,
+            "timestamp": 0.05761181638969176
+        },
+        {
+            "x": 0.7752730705207751,
+            "y": 4.313424298338961,
+            "heading": -0.9621480921974644,
+            "angularVelocity": 0.892772152832256,
+            "velocityX": 0.7560199505477795,
+            "velocityY": -0.5390427676609577,
+            "timestamp": 0.11522363277938352
+        },
+        {
+            "x": 0.8408250898493527,
+            "y": 4.266812487548848,
+            "heading": -0.8877439224939486,
+            "angularVelocity": 1.2914741170498636,
+            "velocityX": 1.1378224717856051,
+            "velocityY": -0.8090668496689294,
+            "timestamp": 0.17283544916907528
+        },
+        {
+            "x": 0.9285847241631874,
+            "y": 4.204600540366139,
+            "heading": -0.7936992773737622,
+            "angularVelocity": 1.6323846567179892,
+            "velocityX": 1.5232922656043415,
+            "velocityY": -1.0798470015578314,
+            "timestamp": 0.23044726555876704
+        },
+        {
+            "x": 1.0388213735909644,
+            "y": 4.1267152282410935,
+            "heading": -0.6856850144743787,
+            "angularVelocity": 1.8748629997840256,
+            "velocityX": 1.9134381857035365,
+            "velocityY": -1.3518982216811457,
+            "timestamp": 0.2880590819484588
+        },
+        {
+            "x": 1.1718232312823271,
+            "y": 4.033123229014291,
+            "heading": -0.5741627817643435,
+            "angularVelocity": 1.935752762171023,
+            "velocityX": 2.3085864328894954,
+            "velocityY": -1.6245278328622093,
+            "timestamp": 0.34567089833815057
+        },
+        {
+            "x": 1.3270687303513053,
+            "y": 3.9246703032584405,
+            "heading": -0.488682735559766,
+            "angularVelocity": 1.4837242003685942,
+            "velocityX": 2.6946815566251776,
+            "velocityY": -1.882477112373353,
+            "timestamp": 0.40328271472784233
+        },
+        {
+            "x": 1.5010681185276094,
+            "y": 3.804139759424889,
+            "heading": -0.46956462349945394,
+            "angularVelocity": 0.3318435914430377,
+            "velocityX": 3.020203129846763,
+            "velocityY": -2.0921149754812625,
+            "timestamp": 0.4608945311175341
+        },
+        {
+            "x": 1.6785494425409546,
+            "y": 3.677652137140934,
+            "heading": -0.4695645649556455,
+            "angularVelocity": 0.0000010161770978802793,
+            "velocityX": 3.080641006227699,
+            "velocityY": -2.195515264236428,
+            "timestamp": 0.5185063475072258
+        },
+        {
+            "x": 1.85603075289391,
+            "y": 3.551164495689284,
+            "heading": -0.4695645064119953,
+            "angularVelocity": 0.000001016174352223477,
+            "velocityX": 3.080640769116793,
+            "velocityY": -2.1955155969406546,
+            "timestamp": 0.5761181638969175
+        },
+        {
+            "x": 2.033512086622271,
+            "y": 3.4246768870368545,
+            "heading": -0.4695644478683731,
+            "angularVelocity": 0.0000010161738659585955,
+            "velocityX": 3.0806411748565776,
+            "velocityY": -2.195515027626543,
+            "timestamp": 0.6337299802866092
+        },
+        {
+            "x": 2.2109934521039283,
+            "y": 3.298189322939119,
+            "heading": -0.46956438932476746,
+            "angularVelocity": 0.0000010161735782211743,
+            "velocityX": 3.080641726016013,
+            "velocityY": -2.1955142542661994,
+            "timestamp": 0.6913417966763009
+        },
+        {
+            "x": 2.3884748618363862,
+            "y": 3.17170182093203,
+            "heading": -0.4695643307811488,
+            "angularVelocity": 0.000001016173805422874,
+            "velocityX": 3.0806424941015065,
+            "velocityY": -2.1955131765246763,
+            "timestamp": 0.7489536130659926
+        },
+        {
+            "x": 2.5659563234551954,
+            "y": 3.045214391729511,
+            "heading": -0.4695642722374779,
+            "angularVelocity": 0.0000010161747112988211,
+            "velocityX": 3.0806433947214558,
+            "velocityY": -2.1955119128156926,
+            "timestamp": 0.8065654294556843
+        },
+        {
+            "x": 2.743437831089148,
+            "y": 2.918727027093402,
+            "heading": -0.46956421369376844,
+            "angularVelocity": 0.000001016175381373278,
+            "velocityX": 3.08064419343162,
+            "velocityY": -2.1955107921009867,
+            "timestamp": 0.864177245845376
+        },
+        {
+            "x": 2.920919379832659,
+            "y": 2.7922397201404365,
+            "heading": -0.4695641551500532,
+            "angularVelocity": 0.0000010161754817816058,
+            "velocityX": 3.080644906992851,
+            "velocityY": -2.195509790862922,
+            "timestamp": 0.9217890622350677
+        },
+        {
+            "x": 3.0984009676669872,
+            "y": 2.6657524680380327,
+            "heading": -0.4695640966063568,
+            "angularVelocity": 0.000001016175153333812,
+            "velocityX": 3.080645585513673,
+            "velocityY": -2.195508838791537,
+            "timestamp": 0.9794008786247594
+        },
+        {
+            "x": 3.275882647398041,
+            "y": 2.5392653448813247,
+            "heading": -0.4695640380626867,
+            "angularVelocity": 0.0000010161746978103879,
+            "velocityX": 3.080647180615714,
+            "velocityY": -2.1955066006101345,
+            "timestamp": 1.0370126950144511
+        },
+        {
+            "x": 3.4563703536987305,
+            "y": 2.417105913162231,
+            "heading": -0.4695639788949644,
+            "angularVelocity": 0.0000010270067149252654,
+            "velocityX": 3.1328244379564545,
+            "velocityY": -2.1203884788634437,
+            "timestamp": 1.0946245114041429
+        },
+        {
+            "x": 3.7578524899325245,
+            "y": 2.288322056940197,
+            "heading": -0.4695639274563231,
+            "angularVelocity": 5.9355601779034e-7,
+            "velocityX": 3.4788348155609183,
+            "velocityY": -1.4860507766867785,
+            "timestamp": 1.1812863255510104
+        },
+        {
+            "x": 4.061243078218464,
+            "y": 2.1641008385808727,
+            "heading": -0.46956387627292245,
+            "angularVelocity": 5.906107685245515e-7,
+            "velocityX": 3.5008566491785738,
+            "velocityY": -1.43340200735705,
+            "timestamp": 1.267948139697878
+        },
+        {
+            "x": 4.364633703659485,
+            "y": 2.039879710966971,
+            "heading": -0.46956382508952316,
+            "angularVelocity": 5.906107531304183e-7,
+            "velocityX": 3.5008570779150574,
+            "velocityY": -1.433400960235855,
+            "timestamp": 1.3546099538447456
+        },
+        {
+            "x": 4.668024366282006,
+            "y": 1.9156586741630988,
+            "heading": -0.46956377390610504,
+            "angularVelocity": 5.906109701114576e-7,
+            "velocityX": 3.5008575069563967,
+            "velocityY": -1.4333999123691519,
+            "timestamp": 1.4412717679916132
+        },
+        {
+            "x": 4.971415087165123,
+            "y": 1.7914377796518506,
+            "heading": -0.4695637227226054,
+            "angularVelocity": 5.906119107964098e-7,
+            "velocityX": 3.5008581792316793,
+            "velocityY": -1.4333982704392505,
+            "timestamp": 1.5279335821384807
+        },
+        {
+            "x": 5.2748058711699946,
+            "y": 1.6672170393060612,
+            "heading": -0.4695636715390663,
+            "angularVelocity": 5.906123657897232e-7,
+            "velocityX": 3.5008589076003997,
+            "velocityY": -1.4333964915074275,
+            "timestamp": 1.6145953962853483
+        },
+        {
+            "x": 5.5781967938554144,
+            "y": 1.5429966376674569,
+            "heading": -0.4695636203556594,
+            "angularVelocity": 5.906108405690599e-7,
+            "velocityX": 3.5008605078501667,
+            "velocityY": -1.4333925831287677,
+            "timestamp": 1.701257210432216
+        },
+        {
+            "x": 5.886988948403778,
+            "y": 1.4328868446307816,
+            "heading": -0.4695635793301605,
+            "angularVelocity": 4.733976467078746e-7,
+            "velocityX": 3.5631859035981908,
+            "velocityY": -1.2705687518849984,
+            "timestamp": 1.7879190245790835
+        },
+        {
+            "x": 6.195781169913944,
+            "y": 1.3227772393821355,
+            "heading": -0.4695635383046595,
+            "angularVelocity": 4.7339767079441426e-7,
+            "velocityX": 3.5631866762776085,
+            "velocityY": -1.2705665849787193,
+            "timestamp": 1.874580838725951
+        },
+        {
+            "x": 6.5045734104803365,
+            "y": 1.212667687574965,
+            "heading": -0.46956349727913566,
+            "angularVelocity": 4.733979354519185e-7,
+            "velocityX": 3.5631868961694693,
+            "velocityY": -1.270565968311784,
+            "timestamp": 1.9612426528728186
+        },
+        {
+            "x": 6.813365656566466,
+            "y": 1.1025581512474965,
+            "heading": -0.46956345625352064,
+            "angularVelocity": 4.733989867793667e-7,
+            "velocityX": 3.563186959862305,
+            "velocityY": -1.270565789689833,
+            "timestamp": 2.0479044670196864
+        },
+        {
+            "x": 7.101065618447083,
+            "y": 0.9964425714529646,
+            "heading": -0.4012732100130825,
+            "angularVelocity": 0.7880085007764233,
+            "velocityX": 3.3198008224596522,
+            "velocityY": -1.2244790954259896,
+            "timestamp": 2.134566281166554
+        },
+        {
+            "x": 7.333266238045498,
+            "y": 0.911992130350644,
+            "heading": -0.2613611242014092,
+            "angularVelocity": 1.6144606155435655,
+            "velocityX": 2.6793879390165927,
+            "velocityY": -0.9744827284506232,
+            "timestamp": 2.2212280953134216
+        },
+        {
+            "x": 7.506890250099573,
+            "y": 0.8492417427348896,
+            "heading": -0.1371754084070899,
+            "angularVelocity": 1.43299233943868,
+            "velocityX": 2.0034661605379225,
+            "velocityY": -0.7240834759056616,
+            "timestamp": 2.307889909460289
+        },
+        {
+            "x": 7.622366585478705,
+            "y": 0.8076799304753898,
+            "heading": -0.04732610702177306,
+            "angularVelocity": 1.0367807582823931,
+            "velocityX": 1.332493861522807,
+            "velocityY": -0.4795862245517285,
+            "timestamp": 2.3945517236071567
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 6.308726444888931e-32,
-            "angularVelocity": 0.5047391300236409,
-            "velocityX": 0.6296477376917574,
-            "velocityY": -0.22907995449868002,
-            "timestamp": 2.4628037612749902
+            "heading": 2.9489608951695904e-31,
+            "angularVelocity": 0.5461010421679925,
+            "velocityX": 0.6650381726791765,
+            "velocityY": -0.2386279433331854,
+            "timestamp": 2.4812135377540243
         },
         {
             "x": 7.68,
             "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -2.9272372134325455e-30,
-            "velocityX": -7.795387702917406e-31,
-            "velocityY": 4.863651986550052e-31,
-            "timestamp": 2.5448972199841564
+            "heading": -7.162012659830636e-32,
+            "angularVelocity": -4.0878892657023166e-30,
+            "velocityX": 2.3668785600931567e-32,
+            "velocityY": 6.81692291678628e-32,
+            "timestamp": 2.567875351900892
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.1.traj
index 83dd207f..9649eb8c 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.1.traj	
@@ -4,190 +4,190 @@
             "x": 0.71,
             "y": 4.36,
             "heading": -1.04,
-            "angularVelocity": -9.2124979355083e-31,
-            "velocityX": -2.0292335982741827e-31,
-            "velocityY": 8.51805082704341e-32,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.7269596629981327,
-            "y": 4.384958058102961,
-            "heading": -1.0015712045971708,
-            "angularVelocity": 0.6206072078812727,
-            "velocityX": 0.27389068508516395,
-            "velocityY": 0.40306105333388875,
-            "timestamp": 0.06192128437248337
-        },
-        {
-            "x": 0.7608459539039729,
-            "y": 4.435118328610172,
-            "heading": -0.9264796986002697,
-            "angularVelocity": 1.212692965882186,
-            "velocityX": 0.5472478687941834,
-            "velocityY": 0.8100650853021081,
-            "timestamp": 0.12384256874496674
-        },
-        {
-            "x": 0.8116291734017863,
-            "y": 4.510832058036491,
-            "heading": -0.8178101616266298,
-            "angularVelocity": 1.754962579909447,
-            "velocityX": 0.8201254223399239,
-            "velocityY": 1.2227415854436723,
-            "timestamp": 0.1857638531174501
-        },
-        {
-            "x": 0.8793445718640878,
-            "y": 4.61260707850531,
-            "heading": -0.6817312766556918,
-            "angularVelocity": 2.1976108271973906,
-            "velocityX": 1.0935722530392615,
-            "velocityY": 1.6436193386525904,
-            "timestamp": 0.24768513748993348
-        },
-        {
-            "x": 0.9640980850122779,
-            "y": 4.741096013070982,
-            "heading": -0.5303097082973756,
-            "angularVelocity": 2.445388042138302,
-            "velocityX": 1.368729896465987,
-            "velocityY": 2.0750366512547602,
-            "timestamp": 0.3096064218624168
-        },
-        {
-            "x": 1.0654131930810256,
-            "y": 4.896663967052664,
-            "heading": -0.3925803189619664,
-            "angularVelocity": 2.2242657065526474,
-            "velocityX": 1.6361919668735143,
-            "velocityY": 2.512350245286791,
-            "timestamp": 0.37152770623490017
-        },
-        {
-            "x": 1.1790960248711007,
-            "y": 5.075186526688609,
-            "heading": -0.32893475345024337,
-            "angularVelocity": 1.0278463400220732,
-            "velocityX": 1.8359249641241828,
-            "velocityY": 2.8830564715365803,
-            "timestamp": 0.4334489906073835
-        },
-        {
-            "x": 1.3095059591702463,
-            "y": 5.269772606119823,
-            "heading": -0.32893468266066545,
-            "angularVelocity": 0.0000011432188240302224,
-            "velocityX": 2.106059905260899,
-            "velocityY": 3.1424748598671592,
-            "timestamp": 0.49537027497986685
-        },
-        {
-            "x": 1.439916398287199,
-            "y": 5.464358347226451,
-            "heading": -0.3289346118734761,
-            "angularVelocity": 0.0000011431802504043458,
-            "velocityX": 2.106068057834158,
-            "velocityY": 3.14246939608213,
-            "timestamp": 0.5572915593523502
-        },
-        {
-            "x": 1.5703268751378339,
-            "y": 5.65894406304414,
-            "heading": -0.32893454108629183,
-            "angularVelocity": 0.0000011431801680911917,
-            "velocityX": 2.1060686672156135,
-            "velocityY": 3.142468987677516,
-            "timestamp": 0.6192128437248335
-        },
-        {
-            "x": 1.700737390180202,
-            "y": 5.853529753265897,
-            "heading": -0.3289344702991123,
-            "angularVelocity": 0.0000011431800901048107,
-            "velocityX": 2.1060692839943727,
-            "velocityY": 3.1424685743150773,
-            "timestamp": 0.6811341280973169
-        },
-        {
-            "x": 1.8311473976991637,
-            "y": 6.048115783625596,
-            "heading": -0.32893439950904224,
-            "angularVelocity": 0.000001143226773133006,
-            "velocityX": 2.1060610877269452,
-            "velocityY": 3.1424740673849674,
-            "timestamp": 0.7430554124698002
-        },
-        {
-            "x": 1.9446375646824816,
-            "y": 6.226114798697167,
-            "heading": -0.26580646349628556,
-            "angularVelocity": 1.0194868638869756,
-            "velocityX": 1.8328135169261883,
-            "velocityY": 2.8746014698408153,
-            "timestamp": 0.8049766968422836
-        },
-        {
-            "x": 2.0457805429701614,
-            "y": 6.3800476604210195,
-            "heading": -0.13750031114094655,
-            "angularVelocity": 2.072084803401717,
-            "velocityX": 1.6334121508084516,
-            "velocityY": 2.485944264299801,
-            "timestamp": 0.8668979812147669
-        },
-        {
-            "x": 2.130677229255867,
-            "y": 6.507493779703991,
-            "heading": 0.017637122988449796,
-            "angularVelocity": 2.505397549511044,
-            "velocityX": 1.3710420761787727,
-            "velocityY": 2.0581956684930476,
-            "timestamp": 0.9288192655872503
-        },
-        {
-            "x": 2.198423808533576,
-            "y": 6.608249828696692,
-            "heading": 0.1586985563433061,
-            "angularVelocity": 2.2780766707988582,
-            "velocityX": 1.0940758087345794,
-            "velocityY": 1.6271634222993538,
-            "timestamp": 0.9907405499597336
-        },
-        {
-            "x": 2.2492018276993044,
-            "y": 6.683037485807007,
-            "heading": 0.27235601520130115,
-            "angularVelocity": 1.8355152030486992,
-            "velocityX": 0.8200414393906406,
-            "velocityY": 1.2077859474043515,
-            "timestamp": 1.052661834332217
-        },
-        {
-            "x": 2.2830642005186585,
-            "y": 6.732456177807481,
-            "heading": 0.3519224237627513,
-            "angularVelocity": 1.2849605651398268,
-            "velocityX": 0.5468616027997278,
-            "velocityY": 0.7980889366441329,
-            "timestamp": 1.1145831187047004
+            "x": 0.7269596910839872,
+            "y": 4.384958038291164,
+            "heading": -1.001571192673364,
+            "angularVelocity": 0.6206074004453302,
+            "velocityX": 0.27389113865872694,
+            "velocityY": 0.40306073338263126,
+            "timestamp": 0.06192128437247234
+        },
+        {
+            "x": 0.760846036232504,
+            "y": 4.4351182711466235,
+            "heading": -0.9264796681168371,
+            "angularVelocity": 1.2126932656117428,
+            "velocityX": 0.5472487447883301,
+            "velocityY": 0.8100644772439347,
+            "timestamp": 0.12384256874494468
+        },
+        {
+            "x": 0.8116293344011996,
+            "y": 4.510831947021006,
+            "heading": -0.817810113065978,
+            "angularVelocity": 1.7549628718484593,
+            "velocityX": 0.8201266928382989,
+            "velocityY": 1.2227407206049843,
+            "timestamp": 0.185763853117417
+        },
+        {
+            "x": 0.8793448344180163,
+            "y": 4.612606899775036,
+            "heading": -0.6817312187907495,
+            "angularVelocity": 2.197610977457753,
+            "velocityX": 1.0935738930977383,
+            "velocityY": 1.6436182450904597,
+            "timestamp": 0.24768513748988935
+        },
+        {
+            "x": 0.9640984700059907,
+            "y": 4.74109575406918,
+            "heading": -0.5303096586797065,
+            "angularVelocity": 2.4453879089491037,
+            "velocityX": 1.3687318738119136,
+            "velocityY": 2.075035354907217,
+            "timestamp": 0.3096064218623617
+        },
+        {
+            "x": 1.0654137163163762,
+            "y": 4.896663616218439,
+            "heading": -0.3925803264267699,
+            "angularVelocity": 2.224264784697608,
+            "velocityX": 1.6361941994120859,
+            "velocityY": 2.5123487622362273,
+            "timestamp": 0.37152770623483405
+        },
+        {
+            "x": 1.179096696888173,
+            "y": 5.075186082347611,
+            "heading": -0.3289347906925715,
+            "angularVelocity": 1.0278458591290565,
+            "velocityX": 1.8359273668802587,
+            "velocityY": 2.88305496144612,
+            "timestamp": 0.4334489906073064
+        },
+        {
+            "x": 1.3095067112863388,
+            "y": 5.269772108097061,
+            "heading": -0.32893471990297235,
+            "angularVelocity": 0.0000011432191669124052,
+            "velocityX": 2.1060611988232667,
+            "velocityY": 3.142473992932148,
+            "timestamp": 0.49537027497977876
+        },
+        {
+            "x": 1.4399171936865056,
+            "y": 5.464357820195416,
+            "heading": -0.32893464911575754,
+            "angularVelocity": 0.0000011431806608935866,
+            "velocityX": 2.106068756838354,
+            "velocityY": 3.1424689276125553,
+            "timestamp": 0.557291559352251
+        },
+        {
+            "x": 1.570327676092705,
+            "y": 5.6589435322897295,
+            "heading": -0.3289345783285442,
+            "angularVelocity": 0.0000011431806365404781,
+            "velocityX": 2.106068756935774,
+            "velocityY": 3.1424689275472675,
+            "timestamp": 0.6192128437247234
+        },
+        {
+            "x": 1.7007381584918697,
+            "y": 5.853529244388756,
+            "heading": -0.32893450754133247,
+            "angularVelocity": 0.000001143180612539941,
+            "velocityX": 2.1060687568221708,
+            "velocityY": 3.1424689276234057,
+            "timestamp": 0.6811341280971958
+        },
+        {
+            "x": 1.8311480951447636,
+            "y": 6.048115322242248,
+            "heading": -0.3289344367512267,
+            "angularVelocity": 0.0000011432273482693713,
+            "velocityX": 2.1060599432732174,
+            "velocityY": 3.1424748343882203,
+            "timestamp": 0.7430554124696681
+        },
+        {
+            "x": 1.9446381116074218,
+            "y": 6.226114434087632,
+            "heading": -0.2658064774598036,
+            "angularVelocity": 1.0194872398268149,
+            "velocityX": 1.8328110860877296,
+            "velocityY": 2.8746030326934724,
+            "timestamp": 0.8049766968421405
+        },
+        {
+            "x": 2.0457809471284243,
+            "y": 6.380047389358405,
+            "heading": -0.1375002927378408,
+            "angularVelocity": 2.0720853261080365,
+            "velocityX": 1.6334098451931771,
+            "velocityY": 2.485945775039604,
+            "timestamp": 0.8668979812146128
+        },
+        {
+            "x": 2.1306775060879315,
+            "y": 6.50749359169558,
+            "heading": 0.01763716177237307,
+            "angularVelocity": 2.5053978786522335,
+            "velocityX": 1.3710400199200075,
+            "velocityY": 2.0581970097802293,
+            "timestamp": 0.9288192655870852
+        },
+        {
+            "x": 2.198423978678441,
+            "y": 6.608249711273454,
+            "heading": 0.15869859646801654,
+            "angularVelocity": 2.278076692452355,
+            "velocityX": 1.0940740857860296,
+            "velocityY": 1.6271645622174213,
+            "timestamp": 0.9907405499595575
+        },
+        {
+            "x": 2.24920191473134,
+            "y": 6.683037424747583,
+            "heading": 0.27235604329703667,
+            "angularVelocity": 1.8355150087866652,
+            "velocityX": 0.8200400971571714,
+            "velocityY": 1.2077868576540292,
+            "timestamp": 1.0526618343320298
+        },
+        {
+            "x": 2.2830642302273354,
+            "y": 6.732456156660043,
+            "heading": 0.35192243545091534,
+            "angularVelocity": 1.2849603001653922,
+            "velocityX": 0.5468606770541943,
+            "velocityY": 0.7980895812043076,
+            "timestamp": 1.114583118704502
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.393,
-            "angularVelocity": 0.6633837888463243,
-            "velocityX": 0.27350530036594517,
-            "velocityY": 0.39637133566025207,
-            "timestamp": 1.1765044030771838
+            "angularVelocity": 0.6633836000880191,
+            "velocityX": 0.2735048205846554,
+            "velocityY": 0.3963716771816175,
+            "timestamp": 1.1765044030769745
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.393,
-            "angularVelocity": 4.204601117767945e-31,
-            "velocityX": 0,
-            "velocityY": -1.1377088902550092e-31,
-            "timestamp": 1.2384256874496673
+            "angularVelocity": 0,
+            "velocityX": 2.8187230537991962e-33,
+            "velocityY": 0,
+            "timestamp": 1.2384256874494468
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.traj
index 83dd207f..9649eb8c 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.traj	
@@ -4,190 +4,190 @@
             "x": 0.71,
             "y": 4.36,
             "heading": -1.04,
-            "angularVelocity": -9.2124979355083e-31,
-            "velocityX": -2.0292335982741827e-31,
-            "velocityY": 8.51805082704341e-32,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.7269596629981327,
-            "y": 4.384958058102961,
-            "heading": -1.0015712045971708,
-            "angularVelocity": 0.6206072078812727,
-            "velocityX": 0.27389068508516395,
-            "velocityY": 0.40306105333388875,
-            "timestamp": 0.06192128437248337
-        },
-        {
-            "x": 0.7608459539039729,
-            "y": 4.435118328610172,
-            "heading": -0.9264796986002697,
-            "angularVelocity": 1.212692965882186,
-            "velocityX": 0.5472478687941834,
-            "velocityY": 0.8100650853021081,
-            "timestamp": 0.12384256874496674
-        },
-        {
-            "x": 0.8116291734017863,
-            "y": 4.510832058036491,
-            "heading": -0.8178101616266298,
-            "angularVelocity": 1.754962579909447,
-            "velocityX": 0.8201254223399239,
-            "velocityY": 1.2227415854436723,
-            "timestamp": 0.1857638531174501
-        },
-        {
-            "x": 0.8793445718640878,
-            "y": 4.61260707850531,
-            "heading": -0.6817312766556918,
-            "angularVelocity": 2.1976108271973906,
-            "velocityX": 1.0935722530392615,
-            "velocityY": 1.6436193386525904,
-            "timestamp": 0.24768513748993348
-        },
-        {
-            "x": 0.9640980850122779,
-            "y": 4.741096013070982,
-            "heading": -0.5303097082973756,
-            "angularVelocity": 2.445388042138302,
-            "velocityX": 1.368729896465987,
-            "velocityY": 2.0750366512547602,
-            "timestamp": 0.3096064218624168
-        },
-        {
-            "x": 1.0654131930810256,
-            "y": 4.896663967052664,
-            "heading": -0.3925803189619664,
-            "angularVelocity": 2.2242657065526474,
-            "velocityX": 1.6361919668735143,
-            "velocityY": 2.512350245286791,
-            "timestamp": 0.37152770623490017
-        },
-        {
-            "x": 1.1790960248711007,
-            "y": 5.075186526688609,
-            "heading": -0.32893475345024337,
-            "angularVelocity": 1.0278463400220732,
-            "velocityX": 1.8359249641241828,
-            "velocityY": 2.8830564715365803,
-            "timestamp": 0.4334489906073835
-        },
-        {
-            "x": 1.3095059591702463,
-            "y": 5.269772606119823,
-            "heading": -0.32893468266066545,
-            "angularVelocity": 0.0000011432188240302224,
-            "velocityX": 2.106059905260899,
-            "velocityY": 3.1424748598671592,
-            "timestamp": 0.49537027497986685
-        },
-        {
-            "x": 1.439916398287199,
-            "y": 5.464358347226451,
-            "heading": -0.3289346118734761,
-            "angularVelocity": 0.0000011431802504043458,
-            "velocityX": 2.106068057834158,
-            "velocityY": 3.14246939608213,
-            "timestamp": 0.5572915593523502
-        },
-        {
-            "x": 1.5703268751378339,
-            "y": 5.65894406304414,
-            "heading": -0.32893454108629183,
-            "angularVelocity": 0.0000011431801680911917,
-            "velocityX": 2.1060686672156135,
-            "velocityY": 3.142468987677516,
-            "timestamp": 0.6192128437248335
-        },
-        {
-            "x": 1.700737390180202,
-            "y": 5.853529753265897,
-            "heading": -0.3289344702991123,
-            "angularVelocity": 0.0000011431800901048107,
-            "velocityX": 2.1060692839943727,
-            "velocityY": 3.1424685743150773,
-            "timestamp": 0.6811341280973169
-        },
-        {
-            "x": 1.8311473976991637,
-            "y": 6.048115783625596,
-            "heading": -0.32893439950904224,
-            "angularVelocity": 0.000001143226773133006,
-            "velocityX": 2.1060610877269452,
-            "velocityY": 3.1424740673849674,
-            "timestamp": 0.7430554124698002
-        },
-        {
-            "x": 1.9446375646824816,
-            "y": 6.226114798697167,
-            "heading": -0.26580646349628556,
-            "angularVelocity": 1.0194868638869756,
-            "velocityX": 1.8328135169261883,
-            "velocityY": 2.8746014698408153,
-            "timestamp": 0.8049766968422836
-        },
-        {
-            "x": 2.0457805429701614,
-            "y": 6.3800476604210195,
-            "heading": -0.13750031114094655,
-            "angularVelocity": 2.072084803401717,
-            "velocityX": 1.6334121508084516,
-            "velocityY": 2.485944264299801,
-            "timestamp": 0.8668979812147669
-        },
-        {
-            "x": 2.130677229255867,
-            "y": 6.507493779703991,
-            "heading": 0.017637122988449796,
-            "angularVelocity": 2.505397549511044,
-            "velocityX": 1.3710420761787727,
-            "velocityY": 2.0581956684930476,
-            "timestamp": 0.9288192655872503
-        },
-        {
-            "x": 2.198423808533576,
-            "y": 6.608249828696692,
-            "heading": 0.1586985563433061,
-            "angularVelocity": 2.2780766707988582,
-            "velocityX": 1.0940758087345794,
-            "velocityY": 1.6271634222993538,
-            "timestamp": 0.9907405499597336
-        },
-        {
-            "x": 2.2492018276993044,
-            "y": 6.683037485807007,
-            "heading": 0.27235601520130115,
-            "angularVelocity": 1.8355152030486992,
-            "velocityX": 0.8200414393906406,
-            "velocityY": 1.2077859474043515,
-            "timestamp": 1.052661834332217
-        },
-        {
-            "x": 2.2830642005186585,
-            "y": 6.732456177807481,
-            "heading": 0.3519224237627513,
-            "angularVelocity": 1.2849605651398268,
-            "velocityX": 0.5468616027997278,
-            "velocityY": 0.7980889366441329,
-            "timestamp": 1.1145831187047004
+            "x": 0.7269596910839872,
+            "y": 4.384958038291164,
+            "heading": -1.001571192673364,
+            "angularVelocity": 0.6206074004453302,
+            "velocityX": 0.27389113865872694,
+            "velocityY": 0.40306073338263126,
+            "timestamp": 0.06192128437247234
+        },
+        {
+            "x": 0.760846036232504,
+            "y": 4.4351182711466235,
+            "heading": -0.9264796681168371,
+            "angularVelocity": 1.2126932656117428,
+            "velocityX": 0.5472487447883301,
+            "velocityY": 0.8100644772439347,
+            "timestamp": 0.12384256874494468
+        },
+        {
+            "x": 0.8116293344011996,
+            "y": 4.510831947021006,
+            "heading": -0.817810113065978,
+            "angularVelocity": 1.7549628718484593,
+            "velocityX": 0.8201266928382989,
+            "velocityY": 1.2227407206049843,
+            "timestamp": 0.185763853117417
+        },
+        {
+            "x": 0.8793448344180163,
+            "y": 4.612606899775036,
+            "heading": -0.6817312187907495,
+            "angularVelocity": 2.197610977457753,
+            "velocityX": 1.0935738930977383,
+            "velocityY": 1.6436182450904597,
+            "timestamp": 0.24768513748988935
+        },
+        {
+            "x": 0.9640984700059907,
+            "y": 4.74109575406918,
+            "heading": -0.5303096586797065,
+            "angularVelocity": 2.4453879089491037,
+            "velocityX": 1.3687318738119136,
+            "velocityY": 2.075035354907217,
+            "timestamp": 0.3096064218623617
+        },
+        {
+            "x": 1.0654137163163762,
+            "y": 4.896663616218439,
+            "heading": -0.3925803264267699,
+            "angularVelocity": 2.224264784697608,
+            "velocityX": 1.6361941994120859,
+            "velocityY": 2.5123487622362273,
+            "timestamp": 0.37152770623483405
+        },
+        {
+            "x": 1.179096696888173,
+            "y": 5.075186082347611,
+            "heading": -0.3289347906925715,
+            "angularVelocity": 1.0278458591290565,
+            "velocityX": 1.8359273668802587,
+            "velocityY": 2.88305496144612,
+            "timestamp": 0.4334489906073064
+        },
+        {
+            "x": 1.3095067112863388,
+            "y": 5.269772108097061,
+            "heading": -0.32893471990297235,
+            "angularVelocity": 0.0000011432191669124052,
+            "velocityX": 2.1060611988232667,
+            "velocityY": 3.142473992932148,
+            "timestamp": 0.49537027497977876
+        },
+        {
+            "x": 1.4399171936865056,
+            "y": 5.464357820195416,
+            "heading": -0.32893464911575754,
+            "angularVelocity": 0.0000011431806608935866,
+            "velocityX": 2.106068756838354,
+            "velocityY": 3.1424689276125553,
+            "timestamp": 0.557291559352251
+        },
+        {
+            "x": 1.570327676092705,
+            "y": 5.6589435322897295,
+            "heading": -0.3289345783285442,
+            "angularVelocity": 0.0000011431806365404781,
+            "velocityX": 2.106068756935774,
+            "velocityY": 3.1424689275472675,
+            "timestamp": 0.6192128437247234
+        },
+        {
+            "x": 1.7007381584918697,
+            "y": 5.853529244388756,
+            "heading": -0.32893450754133247,
+            "angularVelocity": 0.000001143180612539941,
+            "velocityX": 2.1060687568221708,
+            "velocityY": 3.1424689276234057,
+            "timestamp": 0.6811341280971958
+        },
+        {
+            "x": 1.8311480951447636,
+            "y": 6.048115322242248,
+            "heading": -0.3289344367512267,
+            "angularVelocity": 0.0000011432273482693713,
+            "velocityX": 2.1060599432732174,
+            "velocityY": 3.1424748343882203,
+            "timestamp": 0.7430554124696681
+        },
+        {
+            "x": 1.9446381116074218,
+            "y": 6.226114434087632,
+            "heading": -0.2658064774598036,
+            "angularVelocity": 1.0194872398268149,
+            "velocityX": 1.8328110860877296,
+            "velocityY": 2.8746030326934724,
+            "timestamp": 0.8049766968421405
+        },
+        {
+            "x": 2.0457809471284243,
+            "y": 6.380047389358405,
+            "heading": -0.1375002927378408,
+            "angularVelocity": 2.0720853261080365,
+            "velocityX": 1.6334098451931771,
+            "velocityY": 2.485945775039604,
+            "timestamp": 0.8668979812146128
+        },
+        {
+            "x": 2.1306775060879315,
+            "y": 6.50749359169558,
+            "heading": 0.01763716177237307,
+            "angularVelocity": 2.5053978786522335,
+            "velocityX": 1.3710400199200075,
+            "velocityY": 2.0581970097802293,
+            "timestamp": 0.9288192655870852
+        },
+        {
+            "x": 2.198423978678441,
+            "y": 6.608249711273454,
+            "heading": 0.15869859646801654,
+            "angularVelocity": 2.278076692452355,
+            "velocityX": 1.0940740857860296,
+            "velocityY": 1.6271645622174213,
+            "timestamp": 0.9907405499595575
+        },
+        {
+            "x": 2.24920191473134,
+            "y": 6.683037424747583,
+            "heading": 0.27235604329703667,
+            "angularVelocity": 1.8355150087866652,
+            "velocityX": 0.8200400971571714,
+            "velocityY": 1.2077868576540292,
+            "timestamp": 1.0526618343320298
+        },
+        {
+            "x": 2.2830642302273354,
+            "y": 6.732456156660043,
+            "heading": 0.35192243545091534,
+            "angularVelocity": 1.2849603001653922,
+            "velocityX": 0.5468606770541943,
+            "velocityY": 0.7980895812043076,
+            "timestamp": 1.114583118704502
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.393,
-            "angularVelocity": 0.6633837888463243,
-            "velocityX": 0.27350530036594517,
-            "velocityY": 0.39637133566025207,
-            "timestamp": 1.1765044030771838
+            "angularVelocity": 0.6633836000880191,
+            "velocityX": 0.2735048205846554,
+            "velocityY": 0.3963716771816175,
+            "timestamp": 1.1765044030769745
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.393,
-            "angularVelocity": 4.204601117767945e-31,
-            "velocityX": 0,
-            "velocityY": -1.1377088902550092e-31,
-            "timestamp": 1.2384256874496673
+            "angularVelocity": 0,
+            "velocityX": 2.8187230537991962e-33,
+            "velocityY": 0,
+            "timestamp": 1.2384256874494468
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.1.traj
index 856b6994..f9120604 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.1.traj	
@@ -4,145 +4,145 @@
             "x": 0.71,
             "y": 4.36,
             "heading": -1.04,
-            "angularVelocity": 1.681543952112751e-30,
-            "velocityX": -3.802997916124736e-32,
-            "velocityY": 9.397294493952762e-34,
+            "angularVelocity": 2.3377521162047767e-32,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.7374413965489649,
-            "y": 4.381188295193423,
-            "heading": -1.0071421527847422,
-            "angularVelocity": 0.5013988353179083,
-            "velocityX": 0.41874576197914515,
-            "velocityY": 0.3233257024648049,
-            "timestamp": 0.06553235648109366
+            "x": 0.737441410859622,
+            "y": 4.3811882786622816,
+            "heading": -1.0071421705955907,
+            "angularVelocity": 0.501398563530153,
+            "velocityX": 0.41874598035403643,
+            "velocityY": 0.32332545020514236,
+            "timestamp": 0.06553235648117978
         },
         {
-            "x": 0.7924436098605977,
-            "y": 4.423719284711624,
-            "heading": -0.9443396170050365,
-            "angularVelocity": 0.9583439258410564,
-            "velocityX": 0.8393138331215114,
-            "velocityY": 0.6490074796939781,
-            "timestamp": 0.13106471296218733
+            "x": 0.7924436512054522,
+            "y": 4.423719236654154,
+            "heading": -0.9443396668116766,
+            "angularVelocity": 0.9583434375956188,
+            "velocityX": 0.8393142456524665,
+            "velocityY": 0.6490069986127681,
+            "timestamp": 0.13106471296235955
         },
         {
-            "x": 0.8751753442261428,
-            "y": 4.487775925735343,
-            "heading": -0.8558629917892655,
-            "angularVelocity": 1.350121222045466,
-            "velocityX": 1.2624562705815747,
-            "velocityY": 0.9774811171669098,
-            "timestamp": 0.19659706944328098
+            "x": 0.8751754235694627,
+            "y": 4.487775832809834,
+            "heading": -0.8558630826468019,
+            "angularVelocity": 1.350120595621899,
+            "velocityX": 1.2624568504227318,
+            "velocityY": 0.9774804324956131,
+            "timestamp": 0.19659706944353933
         },
         {
-            "x": 0.9858650513706745,
-            "y": 4.5735667081068,
-            "heading": -0.748264583846062,
-            "angularVelocity": 1.641912693529484,
-            "velocityX": 1.6890847985371011,
-            "velocityY": 1.3091362340405182,
-            "timestamp": 0.26212942592437466
+            "x": 0.9858651777004729,
+            "y": 4.5735665588886505,
+            "heading": -0.748264717894899,
+            "angularVelocity": 1.641912034443748,
+            "velocityX": 1.6890855155315267,
+            "velocityY": 1.3091353750334698,
+            "timestamp": 0.2621294259247191
         },
         {
-            "x": 1.1247485645977424,
-            "y": 4.681320324271238,
-            "heading": -0.6327125157113742,
-            "angularVelocity": 1.763282664312989,
-            "velocityX": 2.1193120571993105,
-            "velocityY": 1.6442811147120544,
-            "timestamp": 0.32766178240546834
+            "x": 1.1247487447497322,
+            "y": 4.6813201098081265,
+            "heading": -0.6327126866941822,
+            "angularVelocity": 1.7632821007116781,
+            "velocityX": 2.1193128785036808,
+            "velocityY": 1.644280119095407,
+            "timestamp": 0.3276617824058989
         },
         {
-            "x": 1.291488910381981,
-            "y": 4.810947401273882,
-            "heading": -0.5360470717928266,
-            "angularVelocity": 1.4750796264503891,
-            "velocityX": 2.544397222040073,
-            "velocityY": 1.9780621964975005,
-            "timestamp": 0.393194138886562
+            "x": 1.291489149115039,
+            "y": 4.810947116825512,
+            "heading": -0.5360472598717524,
+            "angularVelocity": 1.4750793655678647,
+            "velocityX": 2.5443981159626317,
+            "velocityY": 1.9780611285451524,
+            "timestamp": 0.39319413888707866
         },
         {
-            "x": 1.4799946113507723,
-            "y": 4.960741770751019,
-            "heading": -0.5171262553925533,
-            "angularVelocity": 0.2887247981954086,
-            "velocityX": 2.8765286507463124,
-            "velocityY": 2.2858077676538255,
-            "timestamp": 0.4587264953676557
+            "x": 1.4799949106081978,
+            "y": 4.9607414161097925,
+            "heading": -0.5171264547525753,
+            "angularVelocity": 0.288724626049585,
+            "velocityX": 2.876529574322522,
+            "velocityY": 2.285806696533185,
+            "timestamp": 0.45872649536825844
         },
         {
-            "x": 1.6682616674549375,
-            "y": 5.11061398659558,
-            "heading": -0.49798039258281773,
-            "angularVelocity": 0.29215892480933153,
-            "velocityX": 2.8728870166371596,
-            "velocityY": 2.2869956749959233,
-            "timestamp": 0.5242588518487493
+            "x": 1.6682619137790426,
+            "y": 5.11061369240764,
+            "heading": -0.49798057816746544,
+            "angularVelocity": 0.29215913501614493,
+            "velocityX": 2.8728862088900566,
+            "velocityY": 2.2869965974882214,
+            "timestamp": 0.5242588518494382
         },
         {
-            "x": 1.8349961429002288,
-            "y": 5.239757672124722,
-            "heading": -0.4043635195675931,
-            "angularVelocity": 1.4285595397783832,
-            "velocityX": 2.5443076427962366,
-            "velocityY": 1.970685817873752,
-            "timestamp": 0.589791208329843
+            "x": 1.8349963336194852,
+            "y": 5.239757443274396,
+            "heading": -0.40436368242247644,
+            "angularVelocity": 1.4285598866244509,
+            "velocityX": 2.54430679428301,
+            "velocityY": 1.970686814899508,
+            "timestamp": 0.589791208330618
         },
         {
-            "x": 1.9739512346286359,
-            "y": 5.34717518731749,
-            "heading": -0.2886497176580553,
-            "angularVelocity": 1.7657506630777153,
-            "velocityX": 2.120404319177778,
-            "velocityY": 1.6391523357435742,
-            "timestamp": 0.6553235648109367
+            "x": 1.9739513715778099,
+            "y": 5.347175023297594,
+            "heading": -0.28864983484418905,
+            "angularVelocity": 1.7657513599639834,
+            "velocityX": 2.120403498662996,
+            "velocityY": 1.6391533250303247,
+            "timestamp": 0.6553235648117979
         },
         {
-            "x": 2.084688896220359,
-            "y": 5.432668536845291,
-            "heading": -0.1811622358281448,
-            "angularVelocity": 1.6402200012587402,
-            "velocityX": 1.6898165660145774,
-            "velocityY": 1.304597516685743,
-            "timestamp": 0.7208559212920304
+            "x": 2.0846889840994356,
+            "y": 5.432668431655269,
+            "heading": -0.18116230899252247,
+            "angularVelocity": 1.6402206730126385,
+            "velocityX": 1.6898158172204303,
+            "velocityY": 1.304598414406612,
+            "timestamp": 0.7208559212929777
         },
         {
-            "x": 2.167475787005346,
-            "y": 5.496492971314002,
-            "heading": -0.09361417394502371,
-            "angularVelocity": 1.3359516822560165,
-            "velocityX": 1.2632979375443678,
-            "velocityY": 0.97393772932798,
-            "timestamp": 0.786388277773124
+            "x": 2.167475833751633,
+            "y": 5.496492915332372,
+            "heading": -0.09361421148490373,
+            "angularVelocity": 1.3359522258711838,
+            "velocityX": 1.2632973098712375,
+            "velocityY": 0.973938480228949,
+            "timestamp": 0.7863882777741575
         },
         {
-            "x": 2.222528989324329,
-            "y": 5.538875463156123,
-            "heading": -0.0320000732344696,
-            "angularVelocity": 0.9402088375736564,
-            "velocityX": 0.8400919068867495,
-            "velocityY": 0.6467414589974029,
-            "timestamp": 0.8519206342542177
+            "x": 2.2225290058379836,
+            "y": 5.538875443360028,
+            "heading": -0.032000086006940494,
+            "angularVelocity": 0.9402092155141033,
+            "velocityX": 0.8400914455466154,
+            "velocityY": 0.6467420111747162,
+            "timestamp": 0.8519206342553374
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0.48830951537200207,
-            "velocityX": 0.41919766281556964,
-            "velocityY": 0.32235277316741867,
-            "timestamp": 0.9174529907353114
+            "heading": 2.4308347188047162e-30,
+            "angularVelocity": 0.48830971027463277,
+            "velocityX": 0.41919741082263007,
+            "velocityY": 0.3223530752482305,
+            "timestamp": 0.9174529907365172
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 1.4283065738497147e-31,
-            "velocityY": -2.9738366407041548e-31,
-            "timestamp": 0.9829853472164051
+            "heading": 1.7485008040489027e-29,
+            "angularVelocity": 1.1123032826758857e-30,
+            "velocityX": 0,
+            "velocityY": 1.931278949788811e-31,
+            "timestamp": 0.982985347217697
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.traj
index 856b6994..f9120604 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.traj	
@@ -4,145 +4,145 @@
             "x": 0.71,
             "y": 4.36,
             "heading": -1.04,
-            "angularVelocity": 1.681543952112751e-30,
-            "velocityX": -3.802997916124736e-32,
-            "velocityY": 9.397294493952762e-34,
+            "angularVelocity": 2.3377521162047767e-32,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.7374413965489649,
-            "y": 4.381188295193423,
-            "heading": -1.0071421527847422,
-            "angularVelocity": 0.5013988353179083,
-            "velocityX": 0.41874576197914515,
-            "velocityY": 0.3233257024648049,
-            "timestamp": 0.06553235648109366
+            "x": 0.737441410859622,
+            "y": 4.3811882786622816,
+            "heading": -1.0071421705955907,
+            "angularVelocity": 0.501398563530153,
+            "velocityX": 0.41874598035403643,
+            "velocityY": 0.32332545020514236,
+            "timestamp": 0.06553235648117978
         },
         {
-            "x": 0.7924436098605977,
-            "y": 4.423719284711624,
-            "heading": -0.9443396170050365,
-            "angularVelocity": 0.9583439258410564,
-            "velocityX": 0.8393138331215114,
-            "velocityY": 0.6490074796939781,
-            "timestamp": 0.13106471296218733
+            "x": 0.7924436512054522,
+            "y": 4.423719236654154,
+            "heading": -0.9443396668116766,
+            "angularVelocity": 0.9583434375956188,
+            "velocityX": 0.8393142456524665,
+            "velocityY": 0.6490069986127681,
+            "timestamp": 0.13106471296235955
         },
         {
-            "x": 0.8751753442261428,
-            "y": 4.487775925735343,
-            "heading": -0.8558629917892655,
-            "angularVelocity": 1.350121222045466,
-            "velocityX": 1.2624562705815747,
-            "velocityY": 0.9774811171669098,
-            "timestamp": 0.19659706944328098
+            "x": 0.8751754235694627,
+            "y": 4.487775832809834,
+            "heading": -0.8558630826468019,
+            "angularVelocity": 1.350120595621899,
+            "velocityX": 1.2624568504227318,
+            "velocityY": 0.9774804324956131,
+            "timestamp": 0.19659706944353933
         },
         {
-            "x": 0.9858650513706745,
-            "y": 4.5735667081068,
-            "heading": -0.748264583846062,
-            "angularVelocity": 1.641912693529484,
-            "velocityX": 1.6890847985371011,
-            "velocityY": 1.3091362340405182,
-            "timestamp": 0.26212942592437466
+            "x": 0.9858651777004729,
+            "y": 4.5735665588886505,
+            "heading": -0.748264717894899,
+            "angularVelocity": 1.641912034443748,
+            "velocityX": 1.6890855155315267,
+            "velocityY": 1.3091353750334698,
+            "timestamp": 0.2621294259247191
         },
         {
-            "x": 1.1247485645977424,
-            "y": 4.681320324271238,
-            "heading": -0.6327125157113742,
-            "angularVelocity": 1.763282664312989,
-            "velocityX": 2.1193120571993105,
-            "velocityY": 1.6442811147120544,
-            "timestamp": 0.32766178240546834
+            "x": 1.1247487447497322,
+            "y": 4.6813201098081265,
+            "heading": -0.6327126866941822,
+            "angularVelocity": 1.7632821007116781,
+            "velocityX": 2.1193128785036808,
+            "velocityY": 1.644280119095407,
+            "timestamp": 0.3276617824058989
         },
         {
-            "x": 1.291488910381981,
-            "y": 4.810947401273882,
-            "heading": -0.5360470717928266,
-            "angularVelocity": 1.4750796264503891,
-            "velocityX": 2.544397222040073,
-            "velocityY": 1.9780621964975005,
-            "timestamp": 0.393194138886562
+            "x": 1.291489149115039,
+            "y": 4.810947116825512,
+            "heading": -0.5360472598717524,
+            "angularVelocity": 1.4750793655678647,
+            "velocityX": 2.5443981159626317,
+            "velocityY": 1.9780611285451524,
+            "timestamp": 0.39319413888707866
         },
         {
-            "x": 1.4799946113507723,
-            "y": 4.960741770751019,
-            "heading": -0.5171262553925533,
-            "angularVelocity": 0.2887247981954086,
-            "velocityX": 2.8765286507463124,
-            "velocityY": 2.2858077676538255,
-            "timestamp": 0.4587264953676557
+            "x": 1.4799949106081978,
+            "y": 4.9607414161097925,
+            "heading": -0.5171264547525753,
+            "angularVelocity": 0.288724626049585,
+            "velocityX": 2.876529574322522,
+            "velocityY": 2.285806696533185,
+            "timestamp": 0.45872649536825844
         },
         {
-            "x": 1.6682616674549375,
-            "y": 5.11061398659558,
-            "heading": -0.49798039258281773,
-            "angularVelocity": 0.29215892480933153,
-            "velocityX": 2.8728870166371596,
-            "velocityY": 2.2869956749959233,
-            "timestamp": 0.5242588518487493
+            "x": 1.6682619137790426,
+            "y": 5.11061369240764,
+            "heading": -0.49798057816746544,
+            "angularVelocity": 0.29215913501614493,
+            "velocityX": 2.8728862088900566,
+            "velocityY": 2.2869965974882214,
+            "timestamp": 0.5242588518494382
         },
         {
-            "x": 1.8349961429002288,
-            "y": 5.239757672124722,
-            "heading": -0.4043635195675931,
-            "angularVelocity": 1.4285595397783832,
-            "velocityX": 2.5443076427962366,
-            "velocityY": 1.970685817873752,
-            "timestamp": 0.589791208329843
+            "x": 1.8349963336194852,
+            "y": 5.239757443274396,
+            "heading": -0.40436368242247644,
+            "angularVelocity": 1.4285598866244509,
+            "velocityX": 2.54430679428301,
+            "velocityY": 1.970686814899508,
+            "timestamp": 0.589791208330618
         },
         {
-            "x": 1.9739512346286359,
-            "y": 5.34717518731749,
-            "heading": -0.2886497176580553,
-            "angularVelocity": 1.7657506630777153,
-            "velocityX": 2.120404319177778,
-            "velocityY": 1.6391523357435742,
-            "timestamp": 0.6553235648109367
+            "x": 1.9739513715778099,
+            "y": 5.347175023297594,
+            "heading": -0.28864983484418905,
+            "angularVelocity": 1.7657513599639834,
+            "velocityX": 2.120403498662996,
+            "velocityY": 1.6391533250303247,
+            "timestamp": 0.6553235648117979
         },
         {
-            "x": 2.084688896220359,
-            "y": 5.432668536845291,
-            "heading": -0.1811622358281448,
-            "angularVelocity": 1.6402200012587402,
-            "velocityX": 1.6898165660145774,
-            "velocityY": 1.304597516685743,
-            "timestamp": 0.7208559212920304
+            "x": 2.0846889840994356,
+            "y": 5.432668431655269,
+            "heading": -0.18116230899252247,
+            "angularVelocity": 1.6402206730126385,
+            "velocityX": 1.6898158172204303,
+            "velocityY": 1.304598414406612,
+            "timestamp": 0.7208559212929777
         },
         {
-            "x": 2.167475787005346,
-            "y": 5.496492971314002,
-            "heading": -0.09361417394502371,
-            "angularVelocity": 1.3359516822560165,
-            "velocityX": 1.2632979375443678,
-            "velocityY": 0.97393772932798,
-            "timestamp": 0.786388277773124
+            "x": 2.167475833751633,
+            "y": 5.496492915332372,
+            "heading": -0.09361421148490373,
+            "angularVelocity": 1.3359522258711838,
+            "velocityX": 1.2632973098712375,
+            "velocityY": 0.973938480228949,
+            "timestamp": 0.7863882777741575
         },
         {
-            "x": 2.222528989324329,
-            "y": 5.538875463156123,
-            "heading": -0.0320000732344696,
-            "angularVelocity": 0.9402088375736564,
-            "velocityX": 0.8400919068867495,
-            "velocityY": 0.6467414589974029,
-            "timestamp": 0.8519206342542177
+            "x": 2.2225290058379836,
+            "y": 5.538875443360028,
+            "heading": -0.032000086006940494,
+            "angularVelocity": 0.9402092155141033,
+            "velocityX": 0.8400914455466154,
+            "velocityY": 0.6467420111747162,
+            "timestamp": 0.8519206342553374
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0.48830951537200207,
-            "velocityX": 0.41919766281556964,
-            "velocityY": 0.32235277316741867,
-            "timestamp": 0.9174529907353114
+            "heading": 2.4308347188047162e-30,
+            "angularVelocity": 0.48830971027463277,
+            "velocityX": 0.41919741082263007,
+            "velocityY": 0.3223530752482305,
+            "timestamp": 0.9174529907365172
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 1.4283065738497147e-31,
-            "velocityY": -2.9738366407041548e-31,
-            "timestamp": 0.9829853472164051
+            "heading": 1.7485008040489027e-29,
+            "angularVelocity": 1.1123032826758857e-30,
+            "velocityX": 0,
+            "velocityY": 1.931278949788811e-31,
+            "timestamp": 0.982985347217697
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.1.traj
index 0f3d2b08..ff6d2947 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.1.traj	
@@ -4,118 +4,118 @@
             "x": 0.71,
             "y": 4.36,
             "heading": -1.04,
-            "angularVelocity": -7.700576656700109e-31,
+            "angularVelocity": -2.204776300650663e-32,
             "velocityX": 0,
-            "velocityY": -1.136023856217491e-30,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.7541176222366596,
-            "y": 4.360036691224542,
-            "heading": -1.0162545462543013,
-            "angularVelocity": 0.3246127495796489,
-            "velocityX": 0.6031109286246202,
-            "velocityY": 0.0005015881949855901,
-            "timestamp": 0.07315009584931663
+            "x": 0.7541176231747116,
+            "y": 4.360036687345491,
+            "heading": -1.0162545684059647,
+            "angularVelocity": 0.32461244675544165,
+            "velocityX": 0.6031109414489596,
+            "velocityY": 0.0005015351663374791,
+            "timestamp": 0.07315009584923313
         },
         {
-            "x": 0.8424088840100864,
-            "y": 4.360100335716613,
-            "heading": -0.970147925779941,
-            "angularVelocity": 0.630301572936589,
-            "velocityX": 1.2069876429868645,
-            "velocityY": 0.0008700534337042256,
-            "timestamp": 0.14630019169863326
+            "x": 0.8424088865359771,
+            "y": 4.360100324606402,
+            "heading": -0.9701479863262087,
+            "angularVelocity": 0.630301048063541,
+            "velocityX": 1.2069876646948012,
+            "velocityY": 0.0008699545799604022,
+            "timestamp": 0.14630019169846625
         },
         {
-            "x": 0.9749552173528473,
-            "y": 4.360178948568722,
-            "heading": -0.9038422068087609,
-            "angularVelocity": 0.9064337948053328,
-            "velocityX": 1.8119775757478125,
-            "velocityY": 0.0010746787299806282,
-            "timestamp": 0.21945028754794987
+            "x": 0.974955221752511,
+            "y": 4.3601789275184695,
+            "heading": -0.9038423136459668,
+            "angularVelocity": 0.9064331619859869,
+            "velocityX": 1.8119776013652948,
+            "velocityY": 0.0010745428444254071,
+            "timestamp": 0.21945028754769938
         },
         {
-            "x": 1.151884255871623,
-            "y": 4.36025970565234,
-            "heading": -0.8212723355896727,
-            "angularVelocity": 1.1287732471196155,
-            "velocityX": 2.4187123265462183,
-            "velocityY": 0.0011039914941527397,
-            "timestamp": 0.2926003833972665
+            "x": 1.151884261990258,
+            "y": 4.360259672825357,
+            "heading": -0.8212724833638784,
+            "angularVelocity": 1.128772687492223,
+            "velocityX": 2.4187123500481573,
+            "velocityY": 0.0011038305000959348,
+            "timestamp": 0.2926003833969325
         },
         {
-            "x": 1.3733948183653495,
-            "y": 4.360324587725535,
-            "heading": -0.7323747736104,
-            "angularVelocity": 1.2152760833319434,
-            "velocityX": 3.028165034124097,
-            "velocityY": 0.000886971813693119,
-            "timestamp": 0.36575047924658316
+            "x": 1.3733948258568207,
+            "y": 4.360324542882412,
+            "heading": -0.7323749312717828,
+            "angularVelocity": 1.215275948172809,
+            "velocityX": 3.028165052894887,
+            "velocityY": 0.0008868075468144582,
+            "timestamp": 0.36575047924616566
         },
         {
-            "x": 1.6366160137298507,
-            "y": 4.3597898231256424,
-            "heading": -0.6992108014826405,
-            "angularVelocity": 0.45336881302434434,
-            "velocityX": 3.598371161490932,
-            "velocityY": -0.007310511266700298,
-            "timestamp": 0.4389005750958998
+            "x": 1.6366160230983884,
+            "y": 4.359789775146019,
+            "heading": -0.6992109641912426,
+            "angularVelocity": 0.4533687440268879,
+            "velocityX": 3.5983711871554758,
+            "velocityY": -0.007310554144235059,
+            "timestamp": 0.4389005750953988
         },
         {
-            "x": 1.858123270171759,
-            "y": 4.359896131611675,
-            "heading": -0.6098312657316373,
-            "angularVelocity": 1.2218649164225759,
-            "velocityX": 3.02811983866963,
-            "velocityY": 0.001453292504923468,
-            "timestamp": 0.5120506709452164
+            "x": 1.8581232786849116,
+            "y": 4.3598960912826,
+            "heading": -0.6098314600860953,
+            "angularVelocity": 1.2218644838062247,
+            "velocityX": 3.0281198269795637,
+            "velocityY": 0.001453397091854858,
+            "timestamp": 0.512050670944632
         },
         {
-            "x": 2.0350502388104363,
-            "y": 4.359956556106789,
-            "heading": -0.5268889356205209,
-            "angularVelocity": 1.1338649546283055,
-            "velocityX": 2.4186840302045134,
-            "velocityY": 0.000826034394207285,
-            "timestamp": 0.585200766794533
+            "x": 2.035050245808528,
+            "y": 4.359956526837273,
+            "heading": -0.5268891037054299,
+            "angularVelocity": 1.1338653137464598,
+            "velocityX": 2.4186840094956525,
+            "velocityY": 0.000826185584123381,
+            "timestamp": 0.5852007667938651
         },
         {
-            "x": 2.167593784990145,
-            "y": 4.359986293011168,
-            "heading": -0.460224121535325,
-            "angularVelocity": 0.9113428124904321,
-            "velocityX": 1.8119394737736823,
-            "velocityY": 0.0004065190076972891,
-            "timestamp": 0.6583508626438497
+            "x": 2.1675937895751547,
+            "y": 4.359986275638159,
+            "heading": -0.46022423203591045,
+            "angularVelocity": 0.9113435996982586,
+            "velocityX": 1.8119394407877087,
+            "velocityY": 0.00040668163915013244,
+            "timestamp": 0.6583508626430983
         },
         {
-            "x": 2.2558829179021025,
-            "y": 4.359997710834851,
-            "heading": -0.41385943895752736,
-            "angularVelocity": 0.6338294166193973,
-            "velocityX": 1.206958540339091,
-            "velocityY": 0.00015608761065604354,
-            "timestamp": 0.7315009584931663
+            "x": 2.255882919836075,
+            "y": 4.359997704013322,
+            "heading": -0.41385948514831483,
+            "angularVelocity": 0.6338302957681125,
+            "velocityX": 1.2069585040994306,
+            "velocityY": 0.0001562318549097459,
+            "timestamp": 0.7315009584923314
         },
         {
             "x": 2.3,
             "y": 4.36,
             "heading": -0.39,
-            "angularVelocity": 0.32617098693462643,
-            "velocityX": 0.6031035446457226,
-            "velocityY": 0.00003129408268678862,
-            "timestamp": 0.804651054342483
+            "angularVelocity": 0.32617161838698483,
+            "velocityX": 0.6031035182080025,
+            "velocityY": 0.00003138733655167869,
+            "timestamp": 0.8046510543415646
         },
         {
             "x": 2.3,
             "y": 4.36,
             "heading": -0.39,
-            "angularVelocity": 3.953163438305433e-30,
-            "velocityX": 7.397518409454568e-32,
+            "angularVelocity": -2.2755735462869925e-32,
+            "velocityX": -9.780003655586715e-35,
             "velocityY": 0,
-            "timestamp": 0.8778011501917996
+            "timestamp": 0.8778011501907977
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.traj
index 0f3d2b08..ff6d2947 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.traj	
@@ -4,118 +4,118 @@
             "x": 0.71,
             "y": 4.36,
             "heading": -1.04,
-            "angularVelocity": -7.700576656700109e-31,
+            "angularVelocity": -2.204776300650663e-32,
             "velocityX": 0,
-            "velocityY": -1.136023856217491e-30,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.7541176222366596,
-            "y": 4.360036691224542,
-            "heading": -1.0162545462543013,
-            "angularVelocity": 0.3246127495796489,
-            "velocityX": 0.6031109286246202,
-            "velocityY": 0.0005015881949855901,
-            "timestamp": 0.07315009584931663
+            "x": 0.7541176231747116,
+            "y": 4.360036687345491,
+            "heading": -1.0162545684059647,
+            "angularVelocity": 0.32461244675544165,
+            "velocityX": 0.6031109414489596,
+            "velocityY": 0.0005015351663374791,
+            "timestamp": 0.07315009584923313
         },
         {
-            "x": 0.8424088840100864,
-            "y": 4.360100335716613,
-            "heading": -0.970147925779941,
-            "angularVelocity": 0.630301572936589,
-            "velocityX": 1.2069876429868645,
-            "velocityY": 0.0008700534337042256,
-            "timestamp": 0.14630019169863326
+            "x": 0.8424088865359771,
+            "y": 4.360100324606402,
+            "heading": -0.9701479863262087,
+            "angularVelocity": 0.630301048063541,
+            "velocityX": 1.2069876646948012,
+            "velocityY": 0.0008699545799604022,
+            "timestamp": 0.14630019169846625
         },
         {
-            "x": 0.9749552173528473,
-            "y": 4.360178948568722,
-            "heading": -0.9038422068087609,
-            "angularVelocity": 0.9064337948053328,
-            "velocityX": 1.8119775757478125,
-            "velocityY": 0.0010746787299806282,
-            "timestamp": 0.21945028754794987
+            "x": 0.974955221752511,
+            "y": 4.3601789275184695,
+            "heading": -0.9038423136459668,
+            "angularVelocity": 0.9064331619859869,
+            "velocityX": 1.8119776013652948,
+            "velocityY": 0.0010745428444254071,
+            "timestamp": 0.21945028754769938
         },
         {
-            "x": 1.151884255871623,
-            "y": 4.36025970565234,
-            "heading": -0.8212723355896727,
-            "angularVelocity": 1.1287732471196155,
-            "velocityX": 2.4187123265462183,
-            "velocityY": 0.0011039914941527397,
-            "timestamp": 0.2926003833972665
+            "x": 1.151884261990258,
+            "y": 4.360259672825357,
+            "heading": -0.8212724833638784,
+            "angularVelocity": 1.128772687492223,
+            "velocityX": 2.4187123500481573,
+            "velocityY": 0.0011038305000959348,
+            "timestamp": 0.2926003833969325
         },
         {
-            "x": 1.3733948183653495,
-            "y": 4.360324587725535,
-            "heading": -0.7323747736104,
-            "angularVelocity": 1.2152760833319434,
-            "velocityX": 3.028165034124097,
-            "velocityY": 0.000886971813693119,
-            "timestamp": 0.36575047924658316
+            "x": 1.3733948258568207,
+            "y": 4.360324542882412,
+            "heading": -0.7323749312717828,
+            "angularVelocity": 1.215275948172809,
+            "velocityX": 3.028165052894887,
+            "velocityY": 0.0008868075468144582,
+            "timestamp": 0.36575047924616566
         },
         {
-            "x": 1.6366160137298507,
-            "y": 4.3597898231256424,
-            "heading": -0.6992108014826405,
-            "angularVelocity": 0.45336881302434434,
-            "velocityX": 3.598371161490932,
-            "velocityY": -0.007310511266700298,
-            "timestamp": 0.4389005750958998
+            "x": 1.6366160230983884,
+            "y": 4.359789775146019,
+            "heading": -0.6992109641912426,
+            "angularVelocity": 0.4533687440268879,
+            "velocityX": 3.5983711871554758,
+            "velocityY": -0.007310554144235059,
+            "timestamp": 0.4389005750953988
         },
         {
-            "x": 1.858123270171759,
-            "y": 4.359896131611675,
-            "heading": -0.6098312657316373,
-            "angularVelocity": 1.2218649164225759,
-            "velocityX": 3.02811983866963,
-            "velocityY": 0.001453292504923468,
-            "timestamp": 0.5120506709452164
+            "x": 1.8581232786849116,
+            "y": 4.3598960912826,
+            "heading": -0.6098314600860953,
+            "angularVelocity": 1.2218644838062247,
+            "velocityX": 3.0281198269795637,
+            "velocityY": 0.001453397091854858,
+            "timestamp": 0.512050670944632
         },
         {
-            "x": 2.0350502388104363,
-            "y": 4.359956556106789,
-            "heading": -0.5268889356205209,
-            "angularVelocity": 1.1338649546283055,
-            "velocityX": 2.4186840302045134,
-            "velocityY": 0.000826034394207285,
-            "timestamp": 0.585200766794533
+            "x": 2.035050245808528,
+            "y": 4.359956526837273,
+            "heading": -0.5268891037054299,
+            "angularVelocity": 1.1338653137464598,
+            "velocityX": 2.4186840094956525,
+            "velocityY": 0.000826185584123381,
+            "timestamp": 0.5852007667938651
         },
         {
-            "x": 2.167593784990145,
-            "y": 4.359986293011168,
-            "heading": -0.460224121535325,
-            "angularVelocity": 0.9113428124904321,
-            "velocityX": 1.8119394737736823,
-            "velocityY": 0.0004065190076972891,
-            "timestamp": 0.6583508626438497
+            "x": 2.1675937895751547,
+            "y": 4.359986275638159,
+            "heading": -0.46022423203591045,
+            "angularVelocity": 0.9113435996982586,
+            "velocityX": 1.8119394407877087,
+            "velocityY": 0.00040668163915013244,
+            "timestamp": 0.6583508626430983
         },
         {
-            "x": 2.2558829179021025,
-            "y": 4.359997710834851,
-            "heading": -0.41385943895752736,
-            "angularVelocity": 0.6338294166193973,
-            "velocityX": 1.206958540339091,
-            "velocityY": 0.00015608761065604354,
-            "timestamp": 0.7315009584931663
+            "x": 2.255882919836075,
+            "y": 4.359997704013322,
+            "heading": -0.41385948514831483,
+            "angularVelocity": 0.6338302957681125,
+            "velocityX": 1.2069585040994306,
+            "velocityY": 0.0001562318549097459,
+            "timestamp": 0.7315009584923314
         },
         {
             "x": 2.3,
             "y": 4.36,
             "heading": -0.39,
-            "angularVelocity": 0.32617098693462643,
-            "velocityX": 0.6031035446457226,
-            "velocityY": 0.00003129408268678862,
-            "timestamp": 0.804651054342483
+            "angularVelocity": 0.32617161838698483,
+            "velocityX": 0.6031035182080025,
+            "velocityY": 0.00003138733655167869,
+            "timestamp": 0.8046510543415646
         },
         {
             "x": 2.3,
             "y": 4.36,
             "heading": -0.39,
-            "angularVelocity": 3.953163438305433e-30,
-            "velocityX": 7.397518409454568e-32,
+            "angularVelocity": -2.2755735462869925e-32,
+            "velocityX": -9.780003655586715e-35,
             "velocityY": 0,
-            "timestamp": 0.8778011501917996
+            "timestamp": 0.8778011501907977
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.1.traj
index 5089d19a..cd2bebdf 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.1.traj	
@@ -4,199 +4,253 @@
             "x": 2.3,
             "y": 6.757,
             "heading": 0.39,
-            "angularVelocity": 5.971193698047002e-30,
-            "velocityX": 3.860312986446214e-32,
-            "velocityY": -2.276118500298611e-31,
+            "angularVelocity": 2.274971502954598e-31,
+            "velocityX": 3.487335890979115e-33,
+            "velocityY": 2.862188318424697e-32,
             "timestamp": 0
         },
         {
-            "x": 2.3687225561485863,
-            "y": 6.754047320431167,
-            "heading": 0.3342984160631402,
-            "angularVelocity": -0.6057639861694455,
-            "velocityX": 0.7473692238179469,
-            "velocityY": -0.03211088121883033,
-            "timestamp": 0.09195261720507575
-        },
-        {
-            "x": 2.5065025886783423,
-            "y": 6.748008642736996,
-            "heading": 0.22854226308747308,
-            "angularVelocity": -1.1501157464589211,
-            "velocityX": 1.498380760848541,
-            "velocityY": -0.06567162390498478,
-            "timestamp": 0.1839052344101515
-        },
-        {
-            "x": 2.713881673600013,
-            "y": 6.738717029343825,
-            "heading": 0.08399021423974613,
-            "angularVelocity": -1.5720275642110566,
-            "velocityX": 2.2552820270375413,
-            "velocityY": -0.10104784045947955,
-            "timestamp": 0.27585785161522725
-        },
-        {
-            "x": 2.991587720371644,
-            "y": 6.725981822834066,
-            "heading": -0.07328714089763647,
-            "angularVelocity": -1.710417385800096,
-            "velocityX": 3.0200994296038606,
-            "velocityY": -0.13849748812866708,
-            "timestamp": 0.367810468820303
-        },
-        {
-            "x": 3.331126465665418,
-            "y": 6.709290658495849,
-            "heading": -0.09912948961150339,
-            "angularVelocity": -0.2810398387707929,
-            "velocityX": 3.692540306237542,
-            "velocityY": -0.18151918722434224,
-            "timestamp": 0.45976308602537874
-        },
-        {
-            "x": 3.678639360618228,
-            "y": 6.693951135251218,
-            "heading": -0.09912953281628478,
-            "angularVelocity": -4.6985918076152147e-7,
-            "velocityX": 3.7792605095489114,
-            "velocityY": -0.16681986560992548,
-            "timestamp": 0.5517157032304545
-        },
-        {
-            "x": 4.0261522529573766,
-            "y": 6.678611552794569,
-            "heading": -0.09912957602103574,
-            "angularVelocity": -4.698588497287581e-7,
-            "velocityX": 3.77926048112489,
-            "velocityY": -0.16682050955045719,
-            "timestamp": 0.6436683204355302
-        },
-        {
-            "x": 4.373665142037621,
-            "y": 6.663271896508641,
-            "heading": -0.0991296192257673,
-            "angularVelocity": -4.698586389884745e-7,
-            "velocityX": 3.779260445683786,
-            "velocityY": -0.1668213124561534,
-            "timestamp": 0.735620937640606
-        },
-        {
-            "x": 4.721178027308032,
-            "y": 6.647932153912761,
-            "heading": -0.09912966243048027,
-            "angularVelocity": -4.6985843677462385e-7,
-            "velocityX": 3.779260404251215,
-            "velocityY": -0.1668222510912166,
-            "timestamp": 0.8275735548456817
-        },
-        {
-            "x": 5.068690907382649,
-            "y": 6.632592293609503,
-            "heading": -0.09912970563518403,
-            "angularVelocity": -4.698583364707967e-7,
-            "velocityX": 3.7792603477460833,
-            "velocityY": -0.16682353117852486,
-            "timestamp": 0.9195261720507575
-        },
-        {
-            "x": 5.416203760083376,
-            "y": 6.617251813184553,
-            "heading": -0.09912974884008513,
-            "angularVelocity": -4.698604826805718e-7,
-            "velocityX": 3.779260050050473,
-            "velocityY": -0.16683027510502133,
-            "timestamp": 1.0114787892558332
-        },
-        {
-            "x": 5.762351413678364,
-            "y": 6.582867023594091,
-            "heading": -0.09912980297819443,
-            "angularVelocity": -5.887609395849737e-7,
-            "velocityX": 3.7644132827997527,
-            "velocityY": -0.3739403035563012,
-            "timestamp": 1.103431406460909
-        },
-        {
-            "x": 6.108490861918874,
-            "y": 6.5483997316058735,
-            "heading": -0.0991298571102857,
-            "angularVelocity": -5.886954924717416e-7,
-            "velocityX": 3.7643240482055917,
-            "velocityY": -0.3748375308486051,
-            "timestamp": 1.1953840236659847
-        },
-        {
-            "x": 6.4430213978276925,
-            "y": 6.453059099313297,
-            "heading": -0.09912987433352956,
-            "angularVelocity": -1.8730564053245824e-7,
-            "velocityX": 3.6380751965192784,
-            "velocityY": -1.0368452273625233,
-            "timestamp": 1.2873366408710605
-        },
-        {
-            "x": 6.751459095644277,
-            "y": 6.29222781566083,
-            "heading": -0.09912986944515287,
-            "angularVelocity": 5.316190922150303e-8,
-            "velocityX": 3.3543112441127847,
-            "velocityY": -1.7490669492721016,
-            "timestamp": 1.3792892580761362
-        },
-        {
-            "x": 7.058385657911462,
-            "y": 6.1285311430110365,
-            "heading": -0.09912986456630993,
-            "angularVelocity": 5.305822810376802e-8,
-            "velocityX": 3.337877393774089,
-            "velocityY": -1.7802285310130133,
-            "timestamp": 1.471241875281212
-        },
-        {
-            "x": 7.307031381410103,
-            "y": 5.995918713976455,
-            "heading": -0.06067697672181489,
-            "angularVelocity": 0.4181815484244036,
-            "velocityX": 2.7040635824873065,
-            "velocityY": -1.442182213681043,
-            "timestamp": 1.5631944924862877
-        },
-        {
-            "x": 7.4935156893705,
-            "y": 5.896459366852187,
-            "heading": -0.03183830930275791,
-            "angularVelocity": 0.3136253028528817,
-            "velocityX": 2.028047853651562,
-            "velocityY": -1.0816369359281015,
-            "timestamp": 1.6551471096913635
-        },
-        {
-            "x": 7.617838563683624,
-            "y": 5.830153124497745,
-            "heading": -0.012612817121248107,
-            "angularVelocity": 0.2090804238734443,
-            "velocityX": 1.3520319278771131,
-            "velocityY": -0.7210914095741754,
-            "timestamp": 1.7470997268964392
+            "x": 2.3417817158351957,
+            "y": 6.761449131523058,
+            "heading": 0.37953631669999827,
+            "angularVelocity": -0.14722299547472617,
+            "velocityX": 0.5878646347531417,
+            "velocityY": 0.06259884318982863,
+            "timestamp": 0.07107370194626751
+        },
+        {
+            "x": 2.42534939307215,
+            "y": 6.770350816518497,
+            "heading": 0.3588464620969251,
+            "angularVelocity": -0.2911042204995314,
+            "velocityX": 1.1757890042104941,
+            "velocityY": 0.12524583287024788,
+            "timestamp": 0.14214740389253502
+        },
+        {
+            "x": 2.5507092244237364,
+            "y": 6.783710205716013,
+            "heading": 0.3282884162089386,
+            "angularVelocity": -0.42994870185747563,
+            "velocityX": 1.7638005045292426,
+            "velocityY": 0.18796529281121122,
+            "timestamp": 0.21322110583880255
+        },
+        {
+            "x": 2.717871083967313,
+            "y": 6.80153596037414,
+            "heading": 0.2884653946252666,
+            "angularVelocity": -0.5603059991696991,
+            "velocityX": 2.351950932145827,
+            "velocityY": 0.25080661580833774,
+            "timestamp": 0.28429480778507005
+        },
+        {
+            "x": 2.926852904271386,
+            "y": 6.82384561239607,
+            "heading": 0.2406022074409234,
+            "angularVelocity": -0.673430338841082,
+            "velocityX": 2.9403536692385535,
+            "velocityY": 0.31389461096009597,
+            "timestamp": 0.35536850973133755
+        },
+        {
+            "x": 3.1776924244019935,
+            "y": 6.85069218229951,
+            "heading": 0.1884744580400949,
+            "angularVelocity": -0.7334323100298853,
+            "velocityX": 3.529287391280776,
+            "velocityY": 0.3777285995842137,
+            "timestamp": 0.42644221167760504
+        },
+        {
+            "x": 3.4450596021465354,
+            "y": 6.879057072545224,
+            "heading": 0.1884744481252109,
+            "angularVelocity": -1.3950144320454384e-7,
+            "velocityX": 3.7618299092775445,
+            "velocityY": 0.39909121755204374,
+            "timestamp": 0.49751591362387254
+        },
+        {
+            "x": 3.712426777772062,
+            "y": 6.907421982754425,
+            "heading": 0.18847443820909762,
+            "angularVelocity": -1.3951873937625683e-7,
+            "velocityX": 3.7618298794631864,
+            "velocityY": 0.39909149843660235,
+            "timestamp": 0.5685896155701401
+        },
+        {
+            "x": 3.979793955045072,
+            "y": 6.93578687743453,
+            "heading": 0.188474428292985,
+            "angularVelocity": -1.3951872987555026e-7,
+            "velocityX": 3.7618299026431106,
+            "velocityY": 0.3990912799437423,
+            "timestamp": 0.6396633175164076
+        },
+        {
+            "x": 4.247161134956238,
+            "y": 6.964151747247427,
+            "heading": 0.18847441837687146,
+            "angularVelocity": -1.395187425580786e-7,
+            "velocityX": 3.761829939761708,
+            "velocityY": 0.399090930064572,
+            "timestamp": 0.7107370194626751
+        },
+        {
+            "x": 4.514528318048531,
+            "y": 6.992516587075052,
+            "heading": 0.18847440846075478,
+            "angularVelocity": -1.3951878716159625e-7,
+            "velocityX": 3.7618299845198404,
+            "velocityY": 0.39909050817474234,
+            "timestamp": 0.7818107214089426
+        },
+        {
+            "x": 4.781895504744899,
+            "y": 7.020881392930694,
+            "heading": 0.18847439854463183,
+            "angularVelocity": -1.395188750932112e-7,
+            "velocityX": 3.761830035228834,
+            "velocityY": 0.3990900301922755,
+            "timestamp": 0.8528844233552101
+        },
+        {
+            "x": 5.049262689335958,
+            "y": 7.049246218630208,
+            "heading": 0.18847438862849922,
+            "angularVelocity": -1.3951901124406535e-7,
+            "velocityX": 3.7618300056073353,
+            "velocityY": 0.3990903093936624,
+            "timestamp": 0.9239581253014776
+        },
+        {
+            "x": 5.3166271708653765,
+            "y": 7.077636511969196,
+            "heading": 0.18847437871238637,
+            "angularVelocity": -1.3951873300413244e-7,
+            "velocityX": 3.761791973795463,
+            "velocityY": 0.3994486365804798,
+            "timestamp": 0.9950318272477451
+        },
+        {
+            "x": 5.584732592713635,
+            "y": 7.097866624574175,
+            "heading": 0.18847436874075538,
+            "angularVelocity": -1.4029986815741496e-7,
+            "velocityX": 3.772216931248832,
+            "velocityY": 0.28463569578063386,
+            "timestamp": 1.0661055291940127
+        },
+        {
+            "x": 5.85271010689652,
+            "y": 7.076007397472714,
+            "heading": 0.188474358062239,
+            "angularVelocity": -1.5024567569917294e-7,
+            "velocityX": 3.7704172829702145,
+            "velocityY": -0.3075571766057785,
+            "timestamp": 1.1371792311402802
+        },
+        {
+            "x": 6.113985538482666,
+            "y": 7.0125651359558105,
+            "heading": 0.18847434515977443,
+            "angularVelocity": -1.8153640819270364e-7,
+            "velocityX": 3.6761196396335167,
+            "velocityY": -0.8926263833128883,
+            "timestamp": 1.2082529330865477
+        },
+        {
+            "x": 6.38971197835489,
+            "y": 6.891444245730791,
+            "heading": 0.18847433517810044,
+            "angularVelocity": -1.2538350348338093e-7,
+            "velocityX": 3.4635019217336325,
+            "velocityY": -1.521444357133088,
+            "timestamp": 1.2878620818911364
+        },
+        {
+            "x": 6.640022696468843,
+            "y": 6.723993100546061,
+            "heading": 0.18847432674418188,
+            "angularVelocity": -1.0594157434839283e-7,
+            "velocityX": 3.1442456284571,
+            "velocityY": -2.103415847287377,
+            "timestamp": 1.367471230695725
+        },
+        {
+            "x": 6.8648306229514935,
+            "y": 6.523601637914116,
+            "heading": 0.18847431867026868,
+            "angularVelocity": -1.014194138550125e-7,
+            "velocityX": 2.8238956182605124,
+            "velocityY": -2.5171913736164258,
+            "timestamp": 1.4470803795003138
+        },
+        {
+            "x": 7.089642217529903,
+            "y": 6.3232142903896476,
+            "heading": 0.1884743105959038,
+            "angularVelocity": -1.0142508708190059e-7,
+            "velocityX": 2.823941694568014,
+            "velocityY": -2.517139682229462,
+            "timestamp": 1.5266895283049025
+        },
+        {
+            "x": 7.2864279243188,
+            "y": 6.147809259471006,
+            "heading": 0.12465186300240168,
+            "angularVelocity": -0.8016973997575559,
+            "velocityX": 2.471898139143281,
+            "velocityY": -2.203327551576542,
+            "timestamp": 1.6062986771094911
+        },
+        {
+            "x": 7.443856673448633,
+            "y": 6.007485445692029,
+            "heading": 0.07359246308164451,
+            "angularVelocity": -0.6413760313670541,
+            "velocityX": 1.9775208188222257,
+            "velocityY": -1.7626593913656032,
+            "timestamp": 1.6859078259140798
+        },
+        {
+            "x": 7.561928317357885,
+            "y": 5.902242696623479,
+            "heading": 0.0352966131369852,
+            "angularVelocity": -0.4810483533581169,
+            "velocityX": 1.483141644926293,
+            "velocityY": -1.3219931458747511,
+            "timestamp": 1.7655169747186685
+        },
+        {
+            "x": 7.640642772199918,
+            "y": 5.832080900256126,
+            "heading": 0.009765510491971057,
+            "angularVelocity": -0.320705635324694,
+            "velocityX": 0.9887614178017027,
+            "velocityY": -0.8813283073733266,
+            "timestamp": 1.8451261235232572
         },
         {
             "x": 7.68,
             "y": 5.797,
             "heading": -0.003,
-            "angularVelocity": 0.10454098440514517,
-            "velocityX": 0.6760159548013833,
-            "velocityY": -0.36054574089833413,
-            "timestamp": 1.839052344101515
+            "angularVelocity": -0.1603523047748632,
+            "velocityX": 0.4943807136624207,
+            "velocityY": -0.44066417971947147,
+            "timestamp": 1.924735272327846
         },
         {
             "x": 7.68,
             "y": 5.797,
             "heading": -0.003,
-            "angularVelocity": -2.423215560944852e-29,
+            "angularVelocity": 9.952815155734959e-29,
             "velocityX": 0,
-            "velocityY": -2.0134519400533763e-31,
-            "timestamp": 1.9310049613065907
+            "velocityY": 1.270429811278963e-30,
+            "timestamp": 2.004344421132435
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.traj
index 5089d19a..cd2bebdf 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.traj	
@@ -4,199 +4,253 @@
             "x": 2.3,
             "y": 6.757,
             "heading": 0.39,
-            "angularVelocity": 5.971193698047002e-30,
-            "velocityX": 3.860312986446214e-32,
-            "velocityY": -2.276118500298611e-31,
+            "angularVelocity": 2.274971502954598e-31,
+            "velocityX": 3.487335890979115e-33,
+            "velocityY": 2.862188318424697e-32,
             "timestamp": 0
         },
         {
-            "x": 2.3687225561485863,
-            "y": 6.754047320431167,
-            "heading": 0.3342984160631402,
-            "angularVelocity": -0.6057639861694455,
-            "velocityX": 0.7473692238179469,
-            "velocityY": -0.03211088121883033,
-            "timestamp": 0.09195261720507575
-        },
-        {
-            "x": 2.5065025886783423,
-            "y": 6.748008642736996,
-            "heading": 0.22854226308747308,
-            "angularVelocity": -1.1501157464589211,
-            "velocityX": 1.498380760848541,
-            "velocityY": -0.06567162390498478,
-            "timestamp": 0.1839052344101515
-        },
-        {
-            "x": 2.713881673600013,
-            "y": 6.738717029343825,
-            "heading": 0.08399021423974613,
-            "angularVelocity": -1.5720275642110566,
-            "velocityX": 2.2552820270375413,
-            "velocityY": -0.10104784045947955,
-            "timestamp": 0.27585785161522725
-        },
-        {
-            "x": 2.991587720371644,
-            "y": 6.725981822834066,
-            "heading": -0.07328714089763647,
-            "angularVelocity": -1.710417385800096,
-            "velocityX": 3.0200994296038606,
-            "velocityY": -0.13849748812866708,
-            "timestamp": 0.367810468820303
-        },
-        {
-            "x": 3.331126465665418,
-            "y": 6.709290658495849,
-            "heading": -0.09912948961150339,
-            "angularVelocity": -0.2810398387707929,
-            "velocityX": 3.692540306237542,
-            "velocityY": -0.18151918722434224,
-            "timestamp": 0.45976308602537874
-        },
-        {
-            "x": 3.678639360618228,
-            "y": 6.693951135251218,
-            "heading": -0.09912953281628478,
-            "angularVelocity": -4.6985918076152147e-7,
-            "velocityX": 3.7792605095489114,
-            "velocityY": -0.16681986560992548,
-            "timestamp": 0.5517157032304545
-        },
-        {
-            "x": 4.0261522529573766,
-            "y": 6.678611552794569,
-            "heading": -0.09912957602103574,
-            "angularVelocity": -4.698588497287581e-7,
-            "velocityX": 3.77926048112489,
-            "velocityY": -0.16682050955045719,
-            "timestamp": 0.6436683204355302
-        },
-        {
-            "x": 4.373665142037621,
-            "y": 6.663271896508641,
-            "heading": -0.0991296192257673,
-            "angularVelocity": -4.698586389884745e-7,
-            "velocityX": 3.779260445683786,
-            "velocityY": -0.1668213124561534,
-            "timestamp": 0.735620937640606
-        },
-        {
-            "x": 4.721178027308032,
-            "y": 6.647932153912761,
-            "heading": -0.09912966243048027,
-            "angularVelocity": -4.6985843677462385e-7,
-            "velocityX": 3.779260404251215,
-            "velocityY": -0.1668222510912166,
-            "timestamp": 0.8275735548456817
-        },
-        {
-            "x": 5.068690907382649,
-            "y": 6.632592293609503,
-            "heading": -0.09912970563518403,
-            "angularVelocity": -4.698583364707967e-7,
-            "velocityX": 3.7792603477460833,
-            "velocityY": -0.16682353117852486,
-            "timestamp": 0.9195261720507575
-        },
-        {
-            "x": 5.416203760083376,
-            "y": 6.617251813184553,
-            "heading": -0.09912974884008513,
-            "angularVelocity": -4.698604826805718e-7,
-            "velocityX": 3.779260050050473,
-            "velocityY": -0.16683027510502133,
-            "timestamp": 1.0114787892558332
-        },
-        {
-            "x": 5.762351413678364,
-            "y": 6.582867023594091,
-            "heading": -0.09912980297819443,
-            "angularVelocity": -5.887609395849737e-7,
-            "velocityX": 3.7644132827997527,
-            "velocityY": -0.3739403035563012,
-            "timestamp": 1.103431406460909
-        },
-        {
-            "x": 6.108490861918874,
-            "y": 6.5483997316058735,
-            "heading": -0.0991298571102857,
-            "angularVelocity": -5.886954924717416e-7,
-            "velocityX": 3.7643240482055917,
-            "velocityY": -0.3748375308486051,
-            "timestamp": 1.1953840236659847
-        },
-        {
-            "x": 6.4430213978276925,
-            "y": 6.453059099313297,
-            "heading": -0.09912987433352956,
-            "angularVelocity": -1.8730564053245824e-7,
-            "velocityX": 3.6380751965192784,
-            "velocityY": -1.0368452273625233,
-            "timestamp": 1.2873366408710605
-        },
-        {
-            "x": 6.751459095644277,
-            "y": 6.29222781566083,
-            "heading": -0.09912986944515287,
-            "angularVelocity": 5.316190922150303e-8,
-            "velocityX": 3.3543112441127847,
-            "velocityY": -1.7490669492721016,
-            "timestamp": 1.3792892580761362
-        },
-        {
-            "x": 7.058385657911462,
-            "y": 6.1285311430110365,
-            "heading": -0.09912986456630993,
-            "angularVelocity": 5.305822810376802e-8,
-            "velocityX": 3.337877393774089,
-            "velocityY": -1.7802285310130133,
-            "timestamp": 1.471241875281212
-        },
-        {
-            "x": 7.307031381410103,
-            "y": 5.995918713976455,
-            "heading": -0.06067697672181489,
-            "angularVelocity": 0.4181815484244036,
-            "velocityX": 2.7040635824873065,
-            "velocityY": -1.442182213681043,
-            "timestamp": 1.5631944924862877
-        },
-        {
-            "x": 7.4935156893705,
-            "y": 5.896459366852187,
-            "heading": -0.03183830930275791,
-            "angularVelocity": 0.3136253028528817,
-            "velocityX": 2.028047853651562,
-            "velocityY": -1.0816369359281015,
-            "timestamp": 1.6551471096913635
-        },
-        {
-            "x": 7.617838563683624,
-            "y": 5.830153124497745,
-            "heading": -0.012612817121248107,
-            "angularVelocity": 0.2090804238734443,
-            "velocityX": 1.3520319278771131,
-            "velocityY": -0.7210914095741754,
-            "timestamp": 1.7470997268964392
+            "x": 2.3417817158351957,
+            "y": 6.761449131523058,
+            "heading": 0.37953631669999827,
+            "angularVelocity": -0.14722299547472617,
+            "velocityX": 0.5878646347531417,
+            "velocityY": 0.06259884318982863,
+            "timestamp": 0.07107370194626751
+        },
+        {
+            "x": 2.42534939307215,
+            "y": 6.770350816518497,
+            "heading": 0.3588464620969251,
+            "angularVelocity": -0.2911042204995314,
+            "velocityX": 1.1757890042104941,
+            "velocityY": 0.12524583287024788,
+            "timestamp": 0.14214740389253502
+        },
+        {
+            "x": 2.5507092244237364,
+            "y": 6.783710205716013,
+            "heading": 0.3282884162089386,
+            "angularVelocity": -0.42994870185747563,
+            "velocityX": 1.7638005045292426,
+            "velocityY": 0.18796529281121122,
+            "timestamp": 0.21322110583880255
+        },
+        {
+            "x": 2.717871083967313,
+            "y": 6.80153596037414,
+            "heading": 0.2884653946252666,
+            "angularVelocity": -0.5603059991696991,
+            "velocityX": 2.351950932145827,
+            "velocityY": 0.25080661580833774,
+            "timestamp": 0.28429480778507005
+        },
+        {
+            "x": 2.926852904271386,
+            "y": 6.82384561239607,
+            "heading": 0.2406022074409234,
+            "angularVelocity": -0.673430338841082,
+            "velocityX": 2.9403536692385535,
+            "velocityY": 0.31389461096009597,
+            "timestamp": 0.35536850973133755
+        },
+        {
+            "x": 3.1776924244019935,
+            "y": 6.85069218229951,
+            "heading": 0.1884744580400949,
+            "angularVelocity": -0.7334323100298853,
+            "velocityX": 3.529287391280776,
+            "velocityY": 0.3777285995842137,
+            "timestamp": 0.42644221167760504
+        },
+        {
+            "x": 3.4450596021465354,
+            "y": 6.879057072545224,
+            "heading": 0.1884744481252109,
+            "angularVelocity": -1.3950144320454384e-7,
+            "velocityX": 3.7618299092775445,
+            "velocityY": 0.39909121755204374,
+            "timestamp": 0.49751591362387254
+        },
+        {
+            "x": 3.712426777772062,
+            "y": 6.907421982754425,
+            "heading": 0.18847443820909762,
+            "angularVelocity": -1.3951873937625683e-7,
+            "velocityX": 3.7618298794631864,
+            "velocityY": 0.39909149843660235,
+            "timestamp": 0.5685896155701401
+        },
+        {
+            "x": 3.979793955045072,
+            "y": 6.93578687743453,
+            "heading": 0.188474428292985,
+            "angularVelocity": -1.3951872987555026e-7,
+            "velocityX": 3.7618299026431106,
+            "velocityY": 0.3990912799437423,
+            "timestamp": 0.6396633175164076
+        },
+        {
+            "x": 4.247161134956238,
+            "y": 6.964151747247427,
+            "heading": 0.18847441837687146,
+            "angularVelocity": -1.395187425580786e-7,
+            "velocityX": 3.761829939761708,
+            "velocityY": 0.399090930064572,
+            "timestamp": 0.7107370194626751
+        },
+        {
+            "x": 4.514528318048531,
+            "y": 6.992516587075052,
+            "heading": 0.18847440846075478,
+            "angularVelocity": -1.3951878716159625e-7,
+            "velocityX": 3.7618299845198404,
+            "velocityY": 0.39909050817474234,
+            "timestamp": 0.7818107214089426
+        },
+        {
+            "x": 4.781895504744899,
+            "y": 7.020881392930694,
+            "heading": 0.18847439854463183,
+            "angularVelocity": -1.395188750932112e-7,
+            "velocityX": 3.761830035228834,
+            "velocityY": 0.3990900301922755,
+            "timestamp": 0.8528844233552101
+        },
+        {
+            "x": 5.049262689335958,
+            "y": 7.049246218630208,
+            "heading": 0.18847438862849922,
+            "angularVelocity": -1.3951901124406535e-7,
+            "velocityX": 3.7618300056073353,
+            "velocityY": 0.3990903093936624,
+            "timestamp": 0.9239581253014776
+        },
+        {
+            "x": 5.3166271708653765,
+            "y": 7.077636511969196,
+            "heading": 0.18847437871238637,
+            "angularVelocity": -1.3951873300413244e-7,
+            "velocityX": 3.761791973795463,
+            "velocityY": 0.3994486365804798,
+            "timestamp": 0.9950318272477451
+        },
+        {
+            "x": 5.584732592713635,
+            "y": 7.097866624574175,
+            "heading": 0.18847436874075538,
+            "angularVelocity": -1.4029986815741496e-7,
+            "velocityX": 3.772216931248832,
+            "velocityY": 0.28463569578063386,
+            "timestamp": 1.0661055291940127
+        },
+        {
+            "x": 5.85271010689652,
+            "y": 7.076007397472714,
+            "heading": 0.188474358062239,
+            "angularVelocity": -1.5024567569917294e-7,
+            "velocityX": 3.7704172829702145,
+            "velocityY": -0.3075571766057785,
+            "timestamp": 1.1371792311402802
+        },
+        {
+            "x": 6.113985538482666,
+            "y": 7.0125651359558105,
+            "heading": 0.18847434515977443,
+            "angularVelocity": -1.8153640819270364e-7,
+            "velocityX": 3.6761196396335167,
+            "velocityY": -0.8926263833128883,
+            "timestamp": 1.2082529330865477
+        },
+        {
+            "x": 6.38971197835489,
+            "y": 6.891444245730791,
+            "heading": 0.18847433517810044,
+            "angularVelocity": -1.2538350348338093e-7,
+            "velocityX": 3.4635019217336325,
+            "velocityY": -1.521444357133088,
+            "timestamp": 1.2878620818911364
+        },
+        {
+            "x": 6.640022696468843,
+            "y": 6.723993100546061,
+            "heading": 0.18847432674418188,
+            "angularVelocity": -1.0594157434839283e-7,
+            "velocityX": 3.1442456284571,
+            "velocityY": -2.103415847287377,
+            "timestamp": 1.367471230695725
+        },
+        {
+            "x": 6.8648306229514935,
+            "y": 6.523601637914116,
+            "heading": 0.18847431867026868,
+            "angularVelocity": -1.014194138550125e-7,
+            "velocityX": 2.8238956182605124,
+            "velocityY": -2.5171913736164258,
+            "timestamp": 1.4470803795003138
+        },
+        {
+            "x": 7.089642217529903,
+            "y": 6.3232142903896476,
+            "heading": 0.1884743105959038,
+            "angularVelocity": -1.0142508708190059e-7,
+            "velocityX": 2.823941694568014,
+            "velocityY": -2.517139682229462,
+            "timestamp": 1.5266895283049025
+        },
+        {
+            "x": 7.2864279243188,
+            "y": 6.147809259471006,
+            "heading": 0.12465186300240168,
+            "angularVelocity": -0.8016973997575559,
+            "velocityX": 2.471898139143281,
+            "velocityY": -2.203327551576542,
+            "timestamp": 1.6062986771094911
+        },
+        {
+            "x": 7.443856673448633,
+            "y": 6.007485445692029,
+            "heading": 0.07359246308164451,
+            "angularVelocity": -0.6413760313670541,
+            "velocityX": 1.9775208188222257,
+            "velocityY": -1.7626593913656032,
+            "timestamp": 1.6859078259140798
+        },
+        {
+            "x": 7.561928317357885,
+            "y": 5.902242696623479,
+            "heading": 0.0352966131369852,
+            "angularVelocity": -0.4810483533581169,
+            "velocityX": 1.483141644926293,
+            "velocityY": -1.3219931458747511,
+            "timestamp": 1.7655169747186685
+        },
+        {
+            "x": 7.640642772199918,
+            "y": 5.832080900256126,
+            "heading": 0.009765510491971057,
+            "angularVelocity": -0.320705635324694,
+            "velocityX": 0.9887614178017027,
+            "velocityY": -0.8813283073733266,
+            "timestamp": 1.8451261235232572
         },
         {
             "x": 7.68,
             "y": 5.797,
             "heading": -0.003,
-            "angularVelocity": 0.10454098440514517,
-            "velocityX": 0.6760159548013833,
-            "velocityY": -0.36054574089833413,
-            "timestamp": 1.839052344101515
+            "angularVelocity": -0.1603523047748632,
+            "velocityX": 0.4943807136624207,
+            "velocityY": -0.44066417971947147,
+            "timestamp": 1.924735272327846
         },
         {
             "x": 7.68,
             "y": 5.797,
             "heading": -0.003,
-            "angularVelocity": -2.423215560944852e-29,
+            "angularVelocity": 9.952815155734959e-29,
             "velocityX": 0,
-            "velocityY": -2.0134519400533763e-31,
-            "timestamp": 1.9310049613065907
+            "velocityY": 1.270429811278963e-30,
+            "timestamp": 2.004344421132435
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.1.traj
index 0ce351ef..848b8926 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.1.traj	
@@ -6,548 +6,548 @@
             "heading": 0,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": 6.585905017096791e-33,
+            "velocityY": 1.4317938010297452e-32,
             "timestamp": 0
         },
         {
-            "x": 2.24734688866176,
-            "y": 5.551784954914585,
-            "heading": -0.0040460391001905256,
-            "angularVelocity": -0.1252124844254258,
-            "velocityX": -0.08210564799104332,
-            "velocityY": -0.2542304163009863,
-            "timestamp": 0.03231338407473799
-        },
-        {
-            "x": 2.242039203719346,
-            "y": 5.535352462397274,
-            "heading": -0.012062241013264058,
-            "angularVelocity": -0.24807683078112674,
-            "velocityX": -0.16425654862202216,
-            "velocityY": -0.5085351778477981,
-            "timestamp": 0.06462676814947597
-        },
-        {
-            "x": 2.2340752644376702,
-            "y": 5.510699787230797,
-            "heading": -0.023960373976211733,
-            "angularVelocity": -0.3682106750388487,
-            "velocityX": -0.24645946284226605,
-            "velocityY": -0.7629245859689895,
-            "timestamp": 0.09694015222421395
-        },
-        {
-            "x": 2.223453118891146,
-            "y": 5.477823766232339,
-            "heading": -0.039635722986674345,
-            "angularVelocity": -0.4851039115621088,
-            "velocityX": -0.32872278316489917,
-            "velocityY": -1.0174118848839497,
-            "timestamp": 0.12925353629895195
-        },
-        {
-            "x": 2.210170475892355,
-            "y": 5.4367206851300205,
-            "heading": -0.05896132885919571,
-            "angularVelocity": -0.5980681511978081,
-            "velocityX": -0.4110570087017019,
-            "velocityY": -1.2720141291067089,
-            "timestamp": 0.16156692037368994
-        },
-        {
-            "x": 2.194224615527819,
-            "y": 5.387386118415271,
-            "heading": -0.08177969695024358,
-            "angularVelocity": -0.706158415295445,
-            "velocityX": -0.4934754072075649,
-            "velocityY": -1.5267533292286215,
-            "timestamp": 0.19388030444842794
-        },
-        {
-            "x": 2.1756122677867826,
-            "y": 5.329814722150547,
-            "heading": -0.10789038891803496,
-            "angularVelocity": -0.8080457282778595,
-            "velocityX": -0.575995002503841,
-            "velocityY": -1.7816579078058363,
-            "timestamp": 0.22619368852316593
-        },
-        {
-            "x": 2.1543294408518685,
-            "y": 5.263999973874467,
-            "heading": -0.13703048715481858,
-            "angularVelocity": -0.901796548742399,
-            "velocityX": -0.6586381322887348,
-            "velocityY": -2.0367643365317547,
-            "timestamp": 0.2585070725979039
-        },
-        {
-            "x": 2.1303711674850674,
-            "y": 5.189933880722087,
-            "heading": -0.16884170077604577,
-            "angularVelocity": -0.984459366671516,
-            "velocityX": -0.7414349828352131,
-            "velocityY": -2.2921181198809735,
-            "timestamp": 0.2908204566726419
-        },
-        {
-            "x": 2.103731122650155,
-            "y": 5.107606803578156,
-            "heading": -0.20280968351747503,
-            "angularVelocity": -1.0512047473228812,
-            "velocityX": -0.8244275738281073,
-            "velocityY": -2.547770204244666,
-            "timestamp": 0.3231338407473799
-        },
-        {
-            "x": 2.0744011006982923,
-            "y": 5.0170081629947525,
-            "heading": -0.23813652479783928,
-            "angularVelocity": -1.0932572459343544,
-            "velocityX": -0.9076741044523434,
-            "velocityY": -2.8037496900311267,
-            "timestamp": 0.3554472248221179
-        },
-        {
-            "x": 2.0423709680329383,
-            "y": 4.918132526359071,
-            "heading": -0.27341203074981035,
-            "angularVelocity": -1.0916685751756738,
-            "velocityX": -0.991234238768389,
-            "velocityY": -3.05989729849995,
-            "timestamp": 0.3877606088968559
-        },
-        {
-            "x": 2.007638970172759,
-            "y": 4.811032750261161,
-            "heading": -0.30535515788271084,
-            "angularVelocity": -0.9885416847402974,
-            "velocityX": -1.0748486688935617,
-            "velocityY": -3.3144091578337234,
-            "timestamp": 0.42007399297159387
-        },
-        {
-            "x": 1.9707337326379286,
-            "y": 4.697468787945422,
-            "heading": -0.31502999189110337,
-            "angularVelocity": -0.2994064003329394,
-            "velocityX": -1.1421037626226502,
-            "velocityY": -3.5144558692173984,
-            "timestamp": 0.45238737704633186
-        },
-        {
-            "x": 1.9331993823421831,
-            "y": 4.581134405528862,
-            "heading": -0.31503001351145116,
-            "angularVelocity": -6.690833652147531e-7,
-            "velocityX": -1.1615728705149717,
-            "velocityY": -3.6001918631452843,
-            "timestamp": 0.48470076112106986
-        },
-        {
-            "x": 1.8956658919440634,
-            "y": 4.464799745676637,
-            "heading": -0.31503003513146777,
-            "angularVelocity": -6.690731172580015e-7,
-            "velocityX": -1.1615462593242614,
-            "velocityY": -3.600200448927068,
-            "timestamp": 0.5170141451958078
-        },
-        {
-            "x": 1.8630741103436357,
-            "y": 4.346985095772027,
-            "heading": -0.31503005685343444,
-            "angularVelocity": -6.722281571161063e-7,
-            "velocityX": -1.008615548437953,
-            "velocityY": -3.646001595874795,
-            "timestamp": 0.5493275292705457
-        },
-        {
-            "x": 1.8389499213812899,
-            "y": 4.227149627930832,
-            "heading": -0.3150300790516974,
-            "angularVelocity": -6.869680669187691e-7,
-            "velocityX": -0.746569560976628,
-            "velocityY": -3.708539704910713,
-            "timestamp": 0.5816409133452837
-        },
-        {
-            "x": 1.8234163358184028,
-            "y": 4.105901026539528,
-            "heading": -0.3150301022065626,
-            "angularVelocity": -7.165719666567231e-7,
-            "velocityX": -0.48071676822704484,
-            "velocityY": -3.752271848434857,
-            "timestamp": 0.6139542974200216
-        },
-        {
-            "x": 1.8165522814590422,
-            "y": 3.983854314068792,
-            "heading": -0.3150301268891266,
-            "angularVelocity": -7.638495563130961e-7,
-            "velocityX": -0.21242140233547732,
-            "velocityY": -3.7769709352772667,
-            "timestamp": 0.6462676814947595
-        },
-        {
-            "x": 1.818392625605847,
-            "y": 3.861628588886844,
-            "heading": -0.3150301538292836,
-            "angularVelocity": -8.337151223773892e-7,
-            "velocityX": 0.05695299949234183,
-            "velocityY": -3.7825108289262035,
-            "timestamp": 0.6785810655694975
-        },
-        {
-            "x": 1.828928055424025,
-            "y": 3.7398438657688415,
-            "heading": -0.315030184023291,
-            "angularVelocity": -9.344118022266017e-7,
-            "velocityX": 0.32603919768385625,
-            "velocityY": -3.768863169401422,
-            "timestamp": 0.7108944496442354
-        },
-        {
-            "x": 1.848105139864153,
-            "y": 3.6191179259506723,
-            "heading": -0.31503021891945676,
-            "angularVelocity": -0.0000010799291597080048,
-            "velocityX": 0.593471868986944,
-            "velocityY": -3.7360970778839238,
-            "timestamp": 0.7432078337189734
-        },
-        {
-            "x": 1.8758266057917121,
-            "y": 3.5000631817385077,
-            "heading": -0.3150302607662258,
-            "angularVelocity": -0.0000012950289872625492,
-            "velocityX": 0.8578942355106304,
-            "velocityY": -3.6843787062599658,
-            "timestamp": 0.7755212177937113
-        },
-        {
-            "x": 1.911951833448642,
-            "y": 3.3832835694780297,
-            "heading": -0.3150303133273049,
-            "angularVelocity": -0.000001626603978371328,
-            "velocityX": 1.1179648523774262,
-            "velocityY": -3.6139703594763453,
-            "timestamp": 0.8078346018684492
-        },
-        {
-            "x": 1.9562975704379837,
-            "y": 3.2693714862411047,
-            "heading": -0.315030383530948,
-            "angularVelocity": -0.000002172587152219365,
-            "velocityX": 1.3723643703418302,
-            "velocityY": -3.525229142619559,
-            "timestamp": 0.8401479859431872
+            "x": 2.2473468901462774,
+            "y": 5.551784954334613,
+            "heading": -0.004046039178700474,
+            "angularVelocity": -0.12521248615625988,
+            "velocityX": -0.08210560159156222,
+            "velocityY": -0.2542304328304919,
+            "timestamp": 0.032313384255078216
+        },
+        {
+            "x": 2.2420392078855844,
+            "y": 5.535352460743813,
+            "heading": -0.012062241078351911,
+            "angularVelocity": -0.24807682898124367,
+            "velocityX": -0.16425646471427996,
+            "velocityY": -0.5085352082308542,
+            "timestamp": 0.06462676851015643
+        },
+        {
+            "x": 2.234075272195187,
+            "y": 5.510699784095841,
+            "heading": -0.023960373728225737,
+            "angularVelocity": -0.36821066329520014,
+            "velocityX": -0.24645935032774877,
+            "velocityY": -0.7629246275588465,
+            "timestamp": 0.09694015276523466
+        },
+        {
+            "x": 2.2234531308615284,
+            "y": 5.477823761292619,
+            "heading": -0.03963572188039383,
+            "angularVelocity": -0.4851038822931769,
+            "velocityX": -0.32872265095504705,
+            "velocityY": -1.017411935057708,
+            "timestamp": 0.12925353702031286
+        },
+        {
+            "x": 2.2101704924088987,
+            "y": 5.436720678146679,
+            "heading": -0.05896132607089341,
+            "angularVelocity": -0.5980680958065965,
+            "velocityX": -0.4110568657178767,
+            "velocityY": -1.27201418525146,
+            "timestamp": 0.16156692127539107
+        },
+        {
+            "x": 2.1942246366351648,
+            "y": 5.38738610923438,
+            "heading": -0.08177969135834087,
+            "angularVelocity": -0.7061583245915709,
+            "velocityX": -0.4934752623822669,
+            "velocityY": -1.5267533887152571,
+            "timestamp": 0.19388030553046928
+        },
+        {
+            "x": 2.1756122932404036,
+            "y": 5.329814710705053,
+            "heading": -0.10789037911156549,
+            "angularVelocity": -0.8080455933402793,
+            "velocityX": -0.5759948647853634,
+            "velocityY": -1.7816579679449487,
+            "timestamp": 0.2261936897855475
+        },
+        {
+            "x": 2.1543294701171383,
+            "y": 5.263999960188385,
+            "heading": -0.1370304714941401,
+            "angularVelocity": -0.9017963625397062,
+            "velocityX": -0.6586380106540685,
+            "velocityY": -2.0367643945039555,
+            "timestamp": 0.25850707404062573
+        },
+        {
+            "x": 2.1303711997350336,
+            "y": 5.1899338649176165,
+            "heading": -0.1688416775308267,
+            "angularVelocity": -0.9844591264590365,
+            "velocityX": -0.7414348863300811,
+            "velocityY": -2.2921181726463207,
+            "timestamp": 0.29082045829570397
+        },
+        {
+            "x": 2.1037311567595207,
+            "y": 5.107606785885568,
+            "heading": -0.20280965108227242,
+            "angularVelocity": -1.051204457054369,
+            "velocityX": -0.8244275116843016,
+            "velocityY": -2.5477702484570597,
+            "timestamp": 0.3231338425507822
+        },
+        {
+            "x": 2.074401135228672,
+            "y": 5.017008143765364,
+            "heading": -0.23813648207052604,
+            "angularVelocity": -1.0932569213236978,
+            "velocityX": -0.9076740863575303,
+            "velocityY": -2.8037497219427316,
+            "timestamp": 0.35544722680586044
+        },
+        {
+            "x": 2.0423710011934597,
+            "y": 4.918132506074763,
+            "heading": -0.27341197795784133,
+            "angularVelocity": -1.0916682576129122,
+            "velocityX": -0.9912342756292476,
+            "velocityY": -3.0598973140692034,
+            "timestamp": 0.3877606110609387
+        },
+        {
+            "x": 2.00763899967256,
+            "y": 4.811032729430715,
+            "heading": -0.3053550995537523,
+            "angularVelocity": -0.9885415078704507,
+            "velocityX": -1.0748487761829064,
+            "velocityY": -3.3144091562374,
+            "timestamp": 0.4200739953160169
+        },
+        {
+            "x": 1.9707337568018743,
+            "y": 4.697468766706309,
+            "heading": -0.3150299299334663,
+            "angularVelocity": -0.2994062863655037,
+            "velocityX": -1.1421039213769328,
+            "velocityY": -3.514455862250325,
+            "timestamp": 0.45238737957109515
+        },
+        {
+            "x": 1.9331993710024595,
+            "y": 4.581134395027877,
+            "heading": -0.3150299515537849,
+            "angularVelocity": -6.690824588937528e-7,
+            "velocityX": -1.1615739627617825,
+            "velocityY": -3.6001915107405935,
+            "timestamp": 0.4847007638261734
+        },
+        {
+            "x": 1.8956658385726595,
+            "y": 4.464799748019651,
+            "heading": -0.3150299731737692,
+            "angularVelocity": -6.690721139443934e-7,
+            "velocityX": -1.1615475535931208,
+            "velocityY": -3.600200031352131,
+            "timestamp": 0.5170141480812516
+        },
+        {
+            "x": 1.8630740613066095,
+            "y": 4.346985096208152,
+            "heading": -0.31502999489570194,
+            "angularVelocity": -6.722271053919432e-7,
+            "velocityX": -1.0086154086732113,
+            "velocityY": -3.64600163453891,
+            "timestamp": 0.5493275323363298
+        },
+        {
+            "x": 1.8389498768606893,
+            "y": 4.227149626761845,
+            "heading": -0.3150300170939308,
+            "angularVelocity": -6.869670074324573e-7,
+            "velocityX": -0.7465694170405304,
+            "velocityY": -3.708539733886721,
+            "timestamp": 0.581640916591408
+        },
+        {
+            "x": 1.8234162959667812,
+            "y": 4.10590102408459,
+            "heading": -0.31503004024876186,
+            "angularVelocity": -7.165709048863888e-7,
+            "velocityX": -0.4807166210536207,
+            "velocityY": -3.7522718672898008,
+            "timestamp": 0.6139543008464863
+        },
+        {
+            "x": 1.816552246405149,
+            "y": 3.98385431066073,
+            "heading": -0.315030064931292,
+            "angularVelocity": -7.63848502164674e-7,
+            "velocityX": -0.21242125267501,
+            "velocityY": -3.776970943694307,
+            "timestamp": 0.6462676851015645
+        },
+        {
+            "x": 1.818392595454208,
+            "y": 3.861628584870307,
+            "heading": -0.315030091871416,
+            "angularVelocity": -8.337140982566696e-7,
+            "velocityX": 0.05695315088421205,
+            "velocityY": -3.782510826646546,
+            "timestamp": 0.6785810693566428
+        },
+        {
+            "x": 1.828928030254088,
+            "y": 3.739843861498511,
+            "heading": -0.3150301220653929,
+            "angularVelocity": -9.344108498281474e-7,
+            "velocityX": 0.32603935003261814,
+            "velocityY": -3.7688631562216113,
+            "timestamp": 0.710894453611721
+        },
+        {
+            "x": 1.8481051197294465,
+            "y": 3.619117921789427,
+            "heading": -0.3150301569615331,
+            "angularVelocity": -0.0000010799283629257,
+            "velocityX": 0.593472021499706,
+            "velocityY": -3.7360970536569806,
+            "timestamp": 0.7432078378667992
+        },
+        {
+            "x": 1.875826590719106,
+            "y": 3.500063178055521,
+            "heading": -0.31503019880828736,
+            "angularVelocity": -0.0000012950285239246719,
+            "velocityX": 0.8578943873792161,
+            "velocityY": -3.684378670896923,
+            "timestamp": 0.7755212221218775
+        },
+        {
+            "x": 1.9119518234377388,
+            "y": 3.3832835666467993,
+            "heading": -0.31503025136937535,
+            "angularVelocity": -0.0000016266042432729645,
+            "velocityX": 1.1179650027822445,
+            "velocityY": -3.61397031294762,
+            "timestamp": 0.8078346063769557
+        },
+        {
+            "x": 1.9562975654606443,
+            "y": 3.269371484637439,
+            "heading": -0.31503032157308203,
+            "angularVelocity": -0.0000021725891085409373,
+            "velocityX": 1.3723645184560365,
+            "velocityY": -3.5252290849559675,
+            "timestamp": 0.8401479906320339
         },
         {
             "x": 2.008638858795166,
             "y": 3.158904790878296,
-            "heading": -0.3150304952316807,
-            "angularVelocity": -0.00000345679463513026,
-            "velocityX": 1.6198021301675205,
-            "velocityY": -3.418604968990847,
-            "timestamp": 0.8724613700179251
-        },
-        {
-            "x": 2.0775165593610554,
-            "y": 3.0394044439018932,
-            "heading": -0.31503058092883834,
-            "angularVelocity": -0.0000023503872475309846,
-            "velocityX": 1.8890856295919138,
-            "velocityY": -3.277496001603852,
-            "timestamp": 0.9089222373745671
-        },
-        {
-            "x": 2.155767716832464,
-            "y": 2.9258208551273497,
-            "heading": -0.31503063899444206,
-            "angularVelocity": -0.0000015925458689218539,
-            "velocityX": 2.146168293419776,
-            "velocityY": -3.11521905563918,
-            "timestamp": 0.9453831047312091
-        },
-        {
-            "x": 2.242886939864493,
-            "y": 2.818887609111427,
-            "heading": -0.31503068232040304,
-            "angularVelocity": -0.000001188286624986991,
-            "velocityX": 2.38938975806231,
-            "velocityY": -2.9328223316783686,
-            "timestamp": 0.9818439720878511
-        },
-        {
-            "x": 2.3383115633038396,
-            "y": 2.719295334161189,
-            "heading": -0.31503071689328305,
-            "angularVelocity": -9.482188022909844e-7,
-            "velocityX": 2.6171791939547426,
-            "velocityY": -2.7314839763979357,
-            "timestamp": 1.0183048394444931
-        },
-        {
-            "x": 2.4414252810789625,
-            "y": 2.627687242403576,
-            "heading": -0.3150307459166021,
-            "angularVelocity": -7.96012851101892e-7,
-            "velocityX": 2.8280654095942417,
-            "velocityY": -2.5125044574926996,
-            "timestamp": 1.0547657068011351
-        },
-        {
-            "x": 2.5515621254678895,
-            "y": 2.5446549735283583,
-            "heading": -0.3150307712925926,
-            "angularVelocity": -6.959787951410741e-7,
-            "velocityX": 3.020686351523773,
-            "velocityY": -2.2772982349278084,
-            "timestamp": 1.0912265741577771
-        },
-        {
-            "x": 2.668010765906386,
-            "y": 2.470734767010798,
-            "heading": -0.31503079425241676,
-            "angularVelocity": -6.297114091058936e-7,
-            "velocityX": 3.1937978682584367,
-            "velocityY": -2.027384751835701,
-            "timestamp": 1.1276874415144191
-        },
-        {
-            "x": 2.790019095895906,
-            "y": 2.406403976394343,
-            "heading": -0.31503081565982244,
-            "angularVelocity": -5.871337458164478e-7,
-            "velocityX": 3.3462816119017478,
-            "velocityY": -1.7643790529502215,
-            "timestamp": 1.1641483088710611
-        },
-        {
-            "x": 2.916799054801627,
-            "y": 2.352077873335088,
-            "heading": -0.31503083617399774,
-            "angularVelocity": -5.626354173508468e-7,
-            "velocityX": 3.477151480397404,
-            "velocityY": -1.4899838374074656,
-            "timestamp": 1.2006091762277031
-        },
-        {
-            "x": 3.0475310715630033,
-            "y": 2.308104889106193,
-            "heading": -0.31503085634737477,
-            "angularVelocity": -5.532884563345105e-7,
-            "velocityX": 3.585543247850392,
-            "velocityY": -1.2060323140087088,
-            "timestamp": 1.2370700435843451
-        },
-        {
-            "x": 3.1788176786437843,
-            "y": 2.2658164704041117,
-            "heading": -0.3150308764961414,
-            "angularVelocity": -5.526134756982608e-7,
-            "velocityX": 3.600753810835078,
-            "velocityY": -1.1598302993847416,
-            "timestamp": 1.2735309109409871
-        },
-        {
-            "x": 3.3101043321458232,
-            "y": 2.2235281958195148,
-            "heading": -0.3150308966449121,
-            "angularVelocity": -5.526135855693721e-7,
-            "velocityX": 3.600755084015385,
-            "velocityY": -1.159826346722746,
-            "timestamp": 1.3099917782976291
-        },
-        {
-            "x": 3.441390996396766,
-            "y": 2.181239954605585,
-            "heading": -0.31503091679368933,
-            "angularVelocity": -5.526137662420421e-7,
-            "velocityX": 3.6007553788219733,
-            "velocityY": -1.1598254314766572,
-            "timestamp": 1.3464526456542711
-        },
-        {
-            "x": 3.5726776715245805,
-            "y": 2.1389517471596347,
-            "heading": -0.31503093694247225,
-            "angularVelocity": -5.526139217103117e-7,
-            "velocityX": 3.600755677138282,
-            "velocityY": -1.1598245053335783,
-            "timestamp": 1.3829135130109131
-        },
-        {
-            "x": 3.7039643576361168,
-            "y": 2.0966635738134185,
-            "heading": -0.3150309570912594,
-            "angularVelocity": -5.526140383605173e-7,
-            "velocityX": 3.6007559783851373,
-            "velocityY": -1.159823570091575,
-            "timestamp": 1.4193743803675551
-        },
-        {
-            "x": 3.835251054850112,
-            "y": 2.0543754349355976,
-            "heading": -0.31503097724004914,
-            "angularVelocity": -5.526141098258176e-7,
-            "velocityX": 3.600756282888565,
-            "velocityY": -1.1598226247384584,
-            "timestamp": 1.4558352477241971
-        },
-        {
-            "x": 3.966537763342614,
-            "y": 2.012087331072757,
-            "heading": -0.3150309973888395,
-            "angularVelocity": -5.526141250500724e-7,
-            "velocityX": 3.6007565922204043,
-            "velocityY": -1.1598216643943178,
-            "timestamp": 1.4922961150808391
-        },
-        {
-            "x": 4.097824483400825,
-            "y": 1.9697992631165677,
-            "heading": -0.315031017537628,
-            "angularVelocity": -5.526140761706961e-7,
-            "velocityX": 3.6007569094292164,
-            "velocityY": -1.1598206795946073,
-            "timestamp": 1.5287569824374811
-        },
-        {
-            "x": 4.2291112154854185,
-            "y": 1.9275112324972625,
-            "heading": -0.3150310376864126,
-            "angularVelocity": -5.526139663753558e-7,
-            "velocityX": 3.600757239272763,
-            "velocityY": -1.1598196555683948,
-            "timestamp": 1.5652178497941232
-        },
-        {
-            "x": 4.360397911943487,
-            "y": 1.8852230912740195,
-            "heading": -0.3150310578362786,
-            "angularVelocity": -5.526436278671969e-7,
-            "velocityX": 3.600756262156026,
-            "velocityY": -1.159822689065559,
-            "timestamp": 1.6016787171507652
-        },
-        {
-            "x": 4.485161433583451,
-            "y": 1.8443297112343198,
-            "heading": -0.3370639022790847,
-            "angularVelocity": -0.6042874467931063,
-            "velocityX": 3.4218473307173203,
-            "velocityY": -1.121569041122948,
-            "timestamp": 1.6381395845074072
-        },
-        {
-            "x": 4.600444501590908,
-            "y": 1.8067758967734413,
-            "heading": -0.37703885598165165,
-            "angularVelocity": -1.0963796695112746,
-            "velocityX": 3.1618301034864644,
-            "velocityY": -1.029975894252485,
-            "timestamp": 1.6746004518640492
-        },
-        {
-            "x": 4.705219738357152,
-            "y": 1.7727109130585983,
-            "heading": -0.41891394403468696,
-            "angularVelocity": -1.1484940180779766,
-            "velocityX": 2.873635334600916,
-            "velocityY": -0.9342889016225668,
-            "timestamp": 1.7110613192206912
-        },
-        {
-            "x": 4.799468915908326,
-            "y": 1.742101106125887,
-            "heading": -0.4594847392760727,
-            "angularVelocity": -1.112721615877786,
-            "velocityX": 2.5849406331801226,
-            "velocityY": -0.8395249249915333,
-            "timestamp": 1.7475221865773332
-        },
-        {
-            "x": 4.883206605537126,
-            "y": 1.7149241083644653,
-            "heading": -0.4972998071219025,
-            "angularVelocity": -1.0371412033604122,
-            "velocityX": 2.2966455737248266,
-            "velocityY": -0.7453744173332496,
-            "timestamp": 1.7839830539339752
-        },
-        {
-            "x": 4.956447647832475,
-            "y": 1.6911654776329053,
-            "heading": -0.531527000493052,
-            "angularVelocity": -0.9387377715499247,
-            "velocityX": 2.008757542132538,
-            "velocityY": -0.6516200094519161,
-            "timestamp": 1.8204439212906172
-        },
-        {
-            "x": 5.019204190370849,
-            "y": 1.670815407546972,
-            "heading": -0.5616288284566417,
-            "angularVelocity": -0.8255927559032131,
-            "velocityX": 1.721202678052634,
-            "velocityY": -0.5581345579873002,
-            "timestamp": 1.8569047886472592
-        },
-        {
-            "x": 5.071485904769321,
-            "y": 1.6538669030565063,
-            "heading": -0.5872312038868318,
-            "angularVelocity": -0.7021877779198676,
-            "velocityX": 1.433913074175601,
-            "velocityY": -0.46484095742112364,
-            "timestamp": 1.8933656560039012
-        },
-        {
-            "x": 5.113300541419085,
-            "y": 1.6403147551859525,
-            "heading": -0.6080594891340769,
-            "angularVelocity": -0.5712504050853902,
-            "velocityX": 1.14683603768266,
-            "velocityY": -0.3716902216832496,
-            "timestamp": 1.9298265233605432
-        },
-        {
-            "x": 5.144654410764751,
-            "y": 1.6301549374248205,
-            "heading": -0.6239033656036709,
-            "angularVelocity": -0.43454469457947725,
-            "velocityX": 0.8599320756409368,
-            "velocityY": -0.2786499197003079,
-            "timestamp": 1.9662873907171852
-        },
-        {
-            "x": 5.165552750765901,
-            "y": 1.6233842339153202,
-            "heading": -0.634595855168354,
-            "angularVelocity": -0.29325933089016243,
-            "velocityX": 0.57317177336273,
-            "velocityY": -0.18569781797215093,
-            "timestamp": 2.002748258073827
+            "heading": -0.3150304332740777,
+            "angularVelocity": -0.0000034568027529519608,
+            "velocityX": 1.6198022751608165,
+            "velocityY": -3.4186049002831713,
+            "timestamp": 0.8724613748871122
+        },
+        {
+            "x": 2.0775165641117233,
+            "y": 3.0394044472847606,
+            "heading": -0.3150305189714284,
+            "angularVelocity": -0.000002350392551393865,
+            "velocityX": 1.8890857675343722,
+            "velocityY": -3.277495922091173,
+            "timestamp": 0.9089222420961518
+        },
+        {
+            "x": 2.155767726009908,
+            "y": 2.9258208622381168,
+            "heading": -0.3150305770370975,
+            "angularVelocity": -0.00000159254767063084,
+            "velocityX": 2.1461684235196876,
+            "velocityY": -3.1152189660064753,
+            "timestamp": 0.9453831093051914
+        },
+        {
+            "x": 2.242886953120307,
+            "y": 2.818887620265158,
+            "heading": -0.3150306203630764,
+            "angularVelocity": -0.0000011882871183876306,
+            "velocityX": 2.389389879591231,
+            "velocityY": -2.932822232666113,
+            "timestamp": 0.981843976514231
+        },
+        {
+            "x": 2.338311580267833,
+            "y": 2.719295349641276,
+            "heading": -0.3150306549359537,
+            "angularVelocity": -9.482187337248934e-7,
+            "velocityX": 2.617179306252704,
+            "velocityY": -2.731483868798106,
+            "timestamp": 1.0183048437232707
+        },
+        {
+            "x": 2.4414253013619946,
+            "y": 2.6276872624602983,
+            "heading": -0.31503068395926054,
+            "angularVelocity": -7.960125215618009e-7,
+            "velocityX": 2.8280655120730875,
+            "velocityY": -2.512504342142078,
+            "timestamp": 1.0547657109323103
+        },
+        {
+            "x": 2.5515621486647997,
+            "y": 2.5446549983777076,
+            "heading": -0.3150307093352344,
+            "angularVelocity": -6.959783403374032e-7,
+            "velocityX": 3.020686443670201,
+            "velocityY": -2.2772981127010774,
+            "timestamp": 1.09122657814135
+        },
+        {
+            "x": 2.6680107915990137,
+            "y": 2.4707347968336117,
+            "heading": -0.31503073229503964,
+            "angularVelocity": -6.297108928988803e-7,
+            "velocityX": 3.1937979496369038,
+            "velocityY": -2.0273846236374995,
+            "timestamp": 1.1276874453503896
+        },
+        {
+            "x": 2.7900191236562386,
+            "y": 2.4064040113359084,
+            "heading": -0.3150307537024253,
+            "angularVelocity": -5.871332010760787e-7,
+            "velocityX": 3.3462816821585726,
+            "velocityY": -1.764378919702536,
+            "timestamp": 1.1641483125594292
+        },
+        {
+            "x": 2.9167990841955884,
+            "y": 2.352077913506683,
+            "heading": -0.3150307742165802,
+            "angularVelocity": -5.626348607586924e-7,
+            "velocityX": 3.477151539278742,
+            "velocityY": -1.4899836999970275,
+            "timestamp": 1.2006091797684688
+        },
+        {
+            "x": 3.0475311021850517,
+            "y": 2.308104934680324,
+            "heading": -0.31503079438993686,
+            "angularVelocity": -5.532878997498012e-7,
+            "velocityX": 3.585543296047864,
+            "velocityY": -1.2060321707174542,
+            "timestamp": 1.2370700469775084
+        },
+        {
+            "x": 3.178817728061225,
+            "y": 2.2658165761507623,
+            "heading": -0.3150308145386853,
+            "angularVelocity": -5.526129783451472e-7,
+            "velocityX": 3.600754340906743,
+            "velocityY": -1.159828653748452,
+            "timestamp": 1.273530914186548
+        },
+        {
+            "x": 3.3101043979960063,
+            "y": 2.2235283544039026,
+            "heading": -0.3150308346874411,
+            "angularVelocity": -5.526131795163358e-7,
+            "velocityX": 3.600755549287413,
+            "velocityY": -1.1598249022550946,
+            "timestamp": 1.3099917813955877
+        },
+        {
+            "x": 3.4413910762638236,
+            "y": 2.1812401585275256,
+            "heading": -0.31503085483620696,
+            "angularVelocity": -5.52613457009699e-7,
+            "velocityX": 3.60075577783481,
+            "velocityY": -1.159824192714005,
+            "timestamp": 1.3464526486046273
+        },
+        {
+            "x": 3.572677762945226,
+            "y": 2.138951988771722,
+            "heading": -0.31503087498498206,
+            "angularVelocity": -5.52613710159953e-7,
+            "velocityX": 3.6007560085914068,
+            "velocityY": -1.1598234763137998,
+            "timestamp": 1.382913515813667
+        },
+        {
+            "x": 3.7039644581047573,
+            "y": 2.0966638453368867,
+            "heading": -0.31503089513376514,
+            "angularVelocity": -5.526139295643368e-7,
+            "velocityX": 3.6007562411181984,
+            "velocityY": -1.1598227544173942,
+            "timestamp": 1.4193743830227066
+        },
+        {
+            "x": 3.8352511618245644,
+            "y": 2.05437572847807,
+            "heading": -0.3150309152825546,
+            "angularVelocity": -5.526141044747151e-7,
+            "velocityX": 3.600756475898009,
+            "velocityY": -1.1598220255258234,
+            "timestamp": 1.4558352502317462
+        },
+        {
+            "x": 3.966537874250263,
+            "y": 2.01208763864737,
+            "heading": -0.3150309354313486,
+            "angularVelocity": -5.526142273042023e-7,
+            "velocityX": 3.6007567146716046,
+            "velocityY": -1.159821284234734,
+            "timestamp": 1.4922961174407858
+        },
+        {
+            "x": 4.097824595645338,
+            "y": 1.9697995766628096,
+            "heading": -0.3150309555801447,
+            "angularVelocity": -5.526142864672618e-7,
+            "velocityX": 3.600756960671674,
+            "velocityY": -1.159820520507965,
+            "timestamp": 1.5287569846498255
+        },
+        {
+            "x": 4.229111326453571,
+            "y": 1.9275115439021684,
+            "heading": -0.3150309757289402,
+            "angularVelocity": -5.5261426880692e-7,
+            "velocityX": 3.600757218843208,
+            "velocityY": -1.1598197189933317,
+            "timestamp": 1.565217851858865
+        },
+        {
+            "x": 4.360398019008411,
+            "y": 1.885223392382287,
+            "heading": -0.31503099587881916,
+            "angularVelocity": -5.52643987126892e-7,
+            "velocityX": 3.6007561696802117,
+            "velocityY": -1.1598229761632481,
+            "timestamp": 1.6016787190679047
+        },
+        {
+            "x": 4.485161526388635,
+            "y": 1.844329974236374,
+            "heading": -0.3370638509456068,
+            "angularVelocity": -0.6042877406197901,
+            "velocityX": 3.4218469534726492,
+            "velocityY": -1.1215700907896822,
+            "timestamp": 1.6381395862769443
+        },
+        {
+            "x": 4.60044458089581,
+            "y": 1.8067761222092775,
+            "heading": -0.3770388194107599,
+            "angularVelocity": -1.0963800788381923,
+            "velocityX": 3.1618297460185825,
+            "velocityY": -1.0299769287381584,
+            "timestamp": 1.674600453485984
+        },
+        {
+            "x": 4.705219804759057,
+            "y": 1.772711102552083,
+            "heading": -0.4189139196427851,
+            "angularVelocity": -1.148494356756437,
+            "velocityX": 2.8736349923479074,
+            "velocityY": -0.9342898911836356,
+            "timestamp": 1.7110613206950236
+        },
+        {
+            "x": 4.799468970200479,
+            "y": 1.7421012617230478,
+            "heading": -0.45948472427125614,
+            "angularVelocity": -1.1127218778387968,
+            "velocityX": 2.5849403115144614,
+            "velocityY": -0.8395258580532651,
+            "timestamp": 1.7475221879040632
+        },
+        {
+            "x": 4.883206648654635,
+            "y": 1.7149242324789875,
+            "heading": -0.49729979894596704,
+            "angularVelocity": -1.037141394852447,
+            "velocityX": 2.296645276538986,
+            "velocityY": -0.7453752838144925,
+            "timestamp": 1.7839830551131028
+        },
+        {
+            "x": 4.956447680828415,
+            "y": 1.6911655730288653,
+            "heading": -0.5315269969459382,
+            "angularVelocity": -0.9387379023033031,
+            "velocityX": 2.0087572726635705,
+            "velocityY": -0.6516207997442213,
+            "timestamp": 1.8204439223221425
+        },
+        {
+            "x": 5.019204214404415,
+            "y": 1.6708154773305386,
+            "heading": -0.5616288277349688,
+            "angularVelocity": -0.825592836737831,
+            "velocityX": 1.7212024392124468,
+            "velocityY": -0.5581352627093101,
+            "timestamp": 1.856904789531182
+        },
+        {
+            "x": 5.071485921099441,
+            "y": 1.6538669506708767,
+            "heading": -0.587231204588931,
+            "angularVelocity": -0.7021878198118158,
+            "velocityX": 1.4339128687006073,
+            "velocityY": -0.46484156733002446,
+            "timestamp": 1.8933656567402217
+        },
+        {
+            "x": 5.11330055140079,
+            "y": 1.6403147844077772,
+            "heading": -0.6080594902506735,
+            "angularVelocity": -0.5712504187662296,
+            "velocityX": 1.1468358682094448,
+            "velocityY": -0.37169072763413913,
+            "timestamp": 1.9298265239492614
+        },
+        {
+            "x": 5.1446544158470235,
+            "y": 1.6301549523614358,
+            "heading": -0.6239033665029361,
+            "angularVelocity": -0.43454469037794713,
+            "velocityX": 0.859931944747064,
+            "velocityY": -0.2786503126240163,
+            "timestamp": 1.966287391158301
+        },
+        {
+            "x": 5.165552752490355,
+            "y": 1.6233842390024875,
+            "heading": -0.6345958555798376,
+            "angularVelocity": -0.29325931869912014,
+            "velocityX": 0.5731716835893133,
+            "velocityY": -0.18569808886140712,
+            "timestamp": 2.0027482583673404
         },
         {
             "x": 5.176,
             "y": 1.62,
             "heading": -0.64,
-            "angularVelocity": -0.14821767070941108,
-            "velocityX": 0.28653320646240676,
-            "velocityY": -0.09281825037833577,
-            "timestamp": 2.039209125430469
+            "angularVelocity": -0.14821766002380998,
+            "velocityX": 0.2865331603263514,
+            "velocityY": -0.09281839027812194,
+            "timestamp": 2.03920912557638
         },
         {
             "x": 5.176,
             "y": 1.62,
             "heading": -0.64,
-            "angularVelocity": 2.131559152063316e-30,
-            "velocityX": 2.0336113095175017e-32,
-            "velocityY": 7.340802046004562e-32,
-            "timestamp": 2.075669992787111
+            "angularVelocity": 2.046404654845657e-31,
+            "velocityX": 2.793064044552828e-33,
+            "velocityY": 5.221456528298987e-32,
+            "timestamp": 2.0756699927854196
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.traj
index 0ce351ef..848b8926 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.traj	
@@ -6,548 +6,548 @@
             "heading": 0,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": 6.585905017096791e-33,
+            "velocityY": 1.4317938010297452e-32,
             "timestamp": 0
         },
         {
-            "x": 2.24734688866176,
-            "y": 5.551784954914585,
-            "heading": -0.0040460391001905256,
-            "angularVelocity": -0.1252124844254258,
-            "velocityX": -0.08210564799104332,
-            "velocityY": -0.2542304163009863,
-            "timestamp": 0.03231338407473799
-        },
-        {
-            "x": 2.242039203719346,
-            "y": 5.535352462397274,
-            "heading": -0.012062241013264058,
-            "angularVelocity": -0.24807683078112674,
-            "velocityX": -0.16425654862202216,
-            "velocityY": -0.5085351778477981,
-            "timestamp": 0.06462676814947597
-        },
-        {
-            "x": 2.2340752644376702,
-            "y": 5.510699787230797,
-            "heading": -0.023960373976211733,
-            "angularVelocity": -0.3682106750388487,
-            "velocityX": -0.24645946284226605,
-            "velocityY": -0.7629245859689895,
-            "timestamp": 0.09694015222421395
-        },
-        {
-            "x": 2.223453118891146,
-            "y": 5.477823766232339,
-            "heading": -0.039635722986674345,
-            "angularVelocity": -0.4851039115621088,
-            "velocityX": -0.32872278316489917,
-            "velocityY": -1.0174118848839497,
-            "timestamp": 0.12925353629895195
-        },
-        {
-            "x": 2.210170475892355,
-            "y": 5.4367206851300205,
-            "heading": -0.05896132885919571,
-            "angularVelocity": -0.5980681511978081,
-            "velocityX": -0.4110570087017019,
-            "velocityY": -1.2720141291067089,
-            "timestamp": 0.16156692037368994
-        },
-        {
-            "x": 2.194224615527819,
-            "y": 5.387386118415271,
-            "heading": -0.08177969695024358,
-            "angularVelocity": -0.706158415295445,
-            "velocityX": -0.4934754072075649,
-            "velocityY": -1.5267533292286215,
-            "timestamp": 0.19388030444842794
-        },
-        {
-            "x": 2.1756122677867826,
-            "y": 5.329814722150547,
-            "heading": -0.10789038891803496,
-            "angularVelocity": -0.8080457282778595,
-            "velocityX": -0.575995002503841,
-            "velocityY": -1.7816579078058363,
-            "timestamp": 0.22619368852316593
-        },
-        {
-            "x": 2.1543294408518685,
-            "y": 5.263999973874467,
-            "heading": -0.13703048715481858,
-            "angularVelocity": -0.901796548742399,
-            "velocityX": -0.6586381322887348,
-            "velocityY": -2.0367643365317547,
-            "timestamp": 0.2585070725979039
-        },
-        {
-            "x": 2.1303711674850674,
-            "y": 5.189933880722087,
-            "heading": -0.16884170077604577,
-            "angularVelocity": -0.984459366671516,
-            "velocityX": -0.7414349828352131,
-            "velocityY": -2.2921181198809735,
-            "timestamp": 0.2908204566726419
-        },
-        {
-            "x": 2.103731122650155,
-            "y": 5.107606803578156,
-            "heading": -0.20280968351747503,
-            "angularVelocity": -1.0512047473228812,
-            "velocityX": -0.8244275738281073,
-            "velocityY": -2.547770204244666,
-            "timestamp": 0.3231338407473799
-        },
-        {
-            "x": 2.0744011006982923,
-            "y": 5.0170081629947525,
-            "heading": -0.23813652479783928,
-            "angularVelocity": -1.0932572459343544,
-            "velocityX": -0.9076741044523434,
-            "velocityY": -2.8037496900311267,
-            "timestamp": 0.3554472248221179
-        },
-        {
-            "x": 2.0423709680329383,
-            "y": 4.918132526359071,
-            "heading": -0.27341203074981035,
-            "angularVelocity": -1.0916685751756738,
-            "velocityX": -0.991234238768389,
-            "velocityY": -3.05989729849995,
-            "timestamp": 0.3877606088968559
-        },
-        {
-            "x": 2.007638970172759,
-            "y": 4.811032750261161,
-            "heading": -0.30535515788271084,
-            "angularVelocity": -0.9885416847402974,
-            "velocityX": -1.0748486688935617,
-            "velocityY": -3.3144091578337234,
-            "timestamp": 0.42007399297159387
-        },
-        {
-            "x": 1.9707337326379286,
-            "y": 4.697468787945422,
-            "heading": -0.31502999189110337,
-            "angularVelocity": -0.2994064003329394,
-            "velocityX": -1.1421037626226502,
-            "velocityY": -3.5144558692173984,
-            "timestamp": 0.45238737704633186
-        },
-        {
-            "x": 1.9331993823421831,
-            "y": 4.581134405528862,
-            "heading": -0.31503001351145116,
-            "angularVelocity": -6.690833652147531e-7,
-            "velocityX": -1.1615728705149717,
-            "velocityY": -3.6001918631452843,
-            "timestamp": 0.48470076112106986
-        },
-        {
-            "x": 1.8956658919440634,
-            "y": 4.464799745676637,
-            "heading": -0.31503003513146777,
-            "angularVelocity": -6.690731172580015e-7,
-            "velocityX": -1.1615462593242614,
-            "velocityY": -3.600200448927068,
-            "timestamp": 0.5170141451958078
-        },
-        {
-            "x": 1.8630741103436357,
-            "y": 4.346985095772027,
-            "heading": -0.31503005685343444,
-            "angularVelocity": -6.722281571161063e-7,
-            "velocityX": -1.008615548437953,
-            "velocityY": -3.646001595874795,
-            "timestamp": 0.5493275292705457
-        },
-        {
-            "x": 1.8389499213812899,
-            "y": 4.227149627930832,
-            "heading": -0.3150300790516974,
-            "angularVelocity": -6.869680669187691e-7,
-            "velocityX": -0.746569560976628,
-            "velocityY": -3.708539704910713,
-            "timestamp": 0.5816409133452837
-        },
-        {
-            "x": 1.8234163358184028,
-            "y": 4.105901026539528,
-            "heading": -0.3150301022065626,
-            "angularVelocity": -7.165719666567231e-7,
-            "velocityX": -0.48071676822704484,
-            "velocityY": -3.752271848434857,
-            "timestamp": 0.6139542974200216
-        },
-        {
-            "x": 1.8165522814590422,
-            "y": 3.983854314068792,
-            "heading": -0.3150301268891266,
-            "angularVelocity": -7.638495563130961e-7,
-            "velocityX": -0.21242140233547732,
-            "velocityY": -3.7769709352772667,
-            "timestamp": 0.6462676814947595
-        },
-        {
-            "x": 1.818392625605847,
-            "y": 3.861628588886844,
-            "heading": -0.3150301538292836,
-            "angularVelocity": -8.337151223773892e-7,
-            "velocityX": 0.05695299949234183,
-            "velocityY": -3.7825108289262035,
-            "timestamp": 0.6785810655694975
-        },
-        {
-            "x": 1.828928055424025,
-            "y": 3.7398438657688415,
-            "heading": -0.315030184023291,
-            "angularVelocity": -9.344118022266017e-7,
-            "velocityX": 0.32603919768385625,
-            "velocityY": -3.768863169401422,
-            "timestamp": 0.7108944496442354
-        },
-        {
-            "x": 1.848105139864153,
-            "y": 3.6191179259506723,
-            "heading": -0.31503021891945676,
-            "angularVelocity": -0.0000010799291597080048,
-            "velocityX": 0.593471868986944,
-            "velocityY": -3.7360970778839238,
-            "timestamp": 0.7432078337189734
-        },
-        {
-            "x": 1.8758266057917121,
-            "y": 3.5000631817385077,
-            "heading": -0.3150302607662258,
-            "angularVelocity": -0.0000012950289872625492,
-            "velocityX": 0.8578942355106304,
-            "velocityY": -3.6843787062599658,
-            "timestamp": 0.7755212177937113
-        },
-        {
-            "x": 1.911951833448642,
-            "y": 3.3832835694780297,
-            "heading": -0.3150303133273049,
-            "angularVelocity": -0.000001626603978371328,
-            "velocityX": 1.1179648523774262,
-            "velocityY": -3.6139703594763453,
-            "timestamp": 0.8078346018684492
-        },
-        {
-            "x": 1.9562975704379837,
-            "y": 3.2693714862411047,
-            "heading": -0.315030383530948,
-            "angularVelocity": -0.000002172587152219365,
-            "velocityX": 1.3723643703418302,
-            "velocityY": -3.525229142619559,
-            "timestamp": 0.8401479859431872
+            "x": 2.2473468901462774,
+            "y": 5.551784954334613,
+            "heading": -0.004046039178700474,
+            "angularVelocity": -0.12521248615625988,
+            "velocityX": -0.08210560159156222,
+            "velocityY": -0.2542304328304919,
+            "timestamp": 0.032313384255078216
+        },
+        {
+            "x": 2.2420392078855844,
+            "y": 5.535352460743813,
+            "heading": -0.012062241078351911,
+            "angularVelocity": -0.24807682898124367,
+            "velocityX": -0.16425646471427996,
+            "velocityY": -0.5085352082308542,
+            "timestamp": 0.06462676851015643
+        },
+        {
+            "x": 2.234075272195187,
+            "y": 5.510699784095841,
+            "heading": -0.023960373728225737,
+            "angularVelocity": -0.36821066329520014,
+            "velocityX": -0.24645935032774877,
+            "velocityY": -0.7629246275588465,
+            "timestamp": 0.09694015276523466
+        },
+        {
+            "x": 2.2234531308615284,
+            "y": 5.477823761292619,
+            "heading": -0.03963572188039383,
+            "angularVelocity": -0.4851038822931769,
+            "velocityX": -0.32872265095504705,
+            "velocityY": -1.017411935057708,
+            "timestamp": 0.12925353702031286
+        },
+        {
+            "x": 2.2101704924088987,
+            "y": 5.436720678146679,
+            "heading": -0.05896132607089341,
+            "angularVelocity": -0.5980680958065965,
+            "velocityX": -0.4110568657178767,
+            "velocityY": -1.27201418525146,
+            "timestamp": 0.16156692127539107
+        },
+        {
+            "x": 2.1942246366351648,
+            "y": 5.38738610923438,
+            "heading": -0.08177969135834087,
+            "angularVelocity": -0.7061583245915709,
+            "velocityX": -0.4934752623822669,
+            "velocityY": -1.5267533887152571,
+            "timestamp": 0.19388030553046928
+        },
+        {
+            "x": 2.1756122932404036,
+            "y": 5.329814710705053,
+            "heading": -0.10789037911156549,
+            "angularVelocity": -0.8080455933402793,
+            "velocityX": -0.5759948647853634,
+            "velocityY": -1.7816579679449487,
+            "timestamp": 0.2261936897855475
+        },
+        {
+            "x": 2.1543294701171383,
+            "y": 5.263999960188385,
+            "heading": -0.1370304714941401,
+            "angularVelocity": -0.9017963625397062,
+            "velocityX": -0.6586380106540685,
+            "velocityY": -2.0367643945039555,
+            "timestamp": 0.25850707404062573
+        },
+        {
+            "x": 2.1303711997350336,
+            "y": 5.1899338649176165,
+            "heading": -0.1688416775308267,
+            "angularVelocity": -0.9844591264590365,
+            "velocityX": -0.7414348863300811,
+            "velocityY": -2.2921181726463207,
+            "timestamp": 0.29082045829570397
+        },
+        {
+            "x": 2.1037311567595207,
+            "y": 5.107606785885568,
+            "heading": -0.20280965108227242,
+            "angularVelocity": -1.051204457054369,
+            "velocityX": -0.8244275116843016,
+            "velocityY": -2.5477702484570597,
+            "timestamp": 0.3231338425507822
+        },
+        {
+            "x": 2.074401135228672,
+            "y": 5.017008143765364,
+            "heading": -0.23813648207052604,
+            "angularVelocity": -1.0932569213236978,
+            "velocityX": -0.9076740863575303,
+            "velocityY": -2.8037497219427316,
+            "timestamp": 0.35544722680586044
+        },
+        {
+            "x": 2.0423710011934597,
+            "y": 4.918132506074763,
+            "heading": -0.27341197795784133,
+            "angularVelocity": -1.0916682576129122,
+            "velocityX": -0.9912342756292476,
+            "velocityY": -3.0598973140692034,
+            "timestamp": 0.3877606110609387
+        },
+        {
+            "x": 2.00763899967256,
+            "y": 4.811032729430715,
+            "heading": -0.3053550995537523,
+            "angularVelocity": -0.9885415078704507,
+            "velocityX": -1.0748487761829064,
+            "velocityY": -3.3144091562374,
+            "timestamp": 0.4200739953160169
+        },
+        {
+            "x": 1.9707337568018743,
+            "y": 4.697468766706309,
+            "heading": -0.3150299299334663,
+            "angularVelocity": -0.2994062863655037,
+            "velocityX": -1.1421039213769328,
+            "velocityY": -3.514455862250325,
+            "timestamp": 0.45238737957109515
+        },
+        {
+            "x": 1.9331993710024595,
+            "y": 4.581134395027877,
+            "heading": -0.3150299515537849,
+            "angularVelocity": -6.690824588937528e-7,
+            "velocityX": -1.1615739627617825,
+            "velocityY": -3.6001915107405935,
+            "timestamp": 0.4847007638261734
+        },
+        {
+            "x": 1.8956658385726595,
+            "y": 4.464799748019651,
+            "heading": -0.3150299731737692,
+            "angularVelocity": -6.690721139443934e-7,
+            "velocityX": -1.1615475535931208,
+            "velocityY": -3.600200031352131,
+            "timestamp": 0.5170141480812516
+        },
+        {
+            "x": 1.8630740613066095,
+            "y": 4.346985096208152,
+            "heading": -0.31502999489570194,
+            "angularVelocity": -6.722271053919432e-7,
+            "velocityX": -1.0086154086732113,
+            "velocityY": -3.64600163453891,
+            "timestamp": 0.5493275323363298
+        },
+        {
+            "x": 1.8389498768606893,
+            "y": 4.227149626761845,
+            "heading": -0.3150300170939308,
+            "angularVelocity": -6.869670074324573e-7,
+            "velocityX": -0.7465694170405304,
+            "velocityY": -3.708539733886721,
+            "timestamp": 0.581640916591408
+        },
+        {
+            "x": 1.8234162959667812,
+            "y": 4.10590102408459,
+            "heading": -0.31503004024876186,
+            "angularVelocity": -7.165709048863888e-7,
+            "velocityX": -0.4807166210536207,
+            "velocityY": -3.7522718672898008,
+            "timestamp": 0.6139543008464863
+        },
+        {
+            "x": 1.816552246405149,
+            "y": 3.98385431066073,
+            "heading": -0.315030064931292,
+            "angularVelocity": -7.63848502164674e-7,
+            "velocityX": -0.21242125267501,
+            "velocityY": -3.776970943694307,
+            "timestamp": 0.6462676851015645
+        },
+        {
+            "x": 1.818392595454208,
+            "y": 3.861628584870307,
+            "heading": -0.315030091871416,
+            "angularVelocity": -8.337140982566696e-7,
+            "velocityX": 0.05695315088421205,
+            "velocityY": -3.782510826646546,
+            "timestamp": 0.6785810693566428
+        },
+        {
+            "x": 1.828928030254088,
+            "y": 3.739843861498511,
+            "heading": -0.3150301220653929,
+            "angularVelocity": -9.344108498281474e-7,
+            "velocityX": 0.32603935003261814,
+            "velocityY": -3.7688631562216113,
+            "timestamp": 0.710894453611721
+        },
+        {
+            "x": 1.8481051197294465,
+            "y": 3.619117921789427,
+            "heading": -0.3150301569615331,
+            "angularVelocity": -0.0000010799283629257,
+            "velocityX": 0.593472021499706,
+            "velocityY": -3.7360970536569806,
+            "timestamp": 0.7432078378667992
+        },
+        {
+            "x": 1.875826590719106,
+            "y": 3.500063178055521,
+            "heading": -0.31503019880828736,
+            "angularVelocity": -0.0000012950285239246719,
+            "velocityX": 0.8578943873792161,
+            "velocityY": -3.684378670896923,
+            "timestamp": 0.7755212221218775
+        },
+        {
+            "x": 1.9119518234377388,
+            "y": 3.3832835666467993,
+            "heading": -0.31503025136937535,
+            "angularVelocity": -0.0000016266042432729645,
+            "velocityX": 1.1179650027822445,
+            "velocityY": -3.61397031294762,
+            "timestamp": 0.8078346063769557
+        },
+        {
+            "x": 1.9562975654606443,
+            "y": 3.269371484637439,
+            "heading": -0.31503032157308203,
+            "angularVelocity": -0.0000021725891085409373,
+            "velocityX": 1.3723645184560365,
+            "velocityY": -3.5252290849559675,
+            "timestamp": 0.8401479906320339
         },
         {
             "x": 2.008638858795166,
             "y": 3.158904790878296,
-            "heading": -0.3150304952316807,
-            "angularVelocity": -0.00000345679463513026,
-            "velocityX": 1.6198021301675205,
-            "velocityY": -3.418604968990847,
-            "timestamp": 0.8724613700179251
-        },
-        {
-            "x": 2.0775165593610554,
-            "y": 3.0394044439018932,
-            "heading": -0.31503058092883834,
-            "angularVelocity": -0.0000023503872475309846,
-            "velocityX": 1.8890856295919138,
-            "velocityY": -3.277496001603852,
-            "timestamp": 0.9089222373745671
-        },
-        {
-            "x": 2.155767716832464,
-            "y": 2.9258208551273497,
-            "heading": -0.31503063899444206,
-            "angularVelocity": -0.0000015925458689218539,
-            "velocityX": 2.146168293419776,
-            "velocityY": -3.11521905563918,
-            "timestamp": 0.9453831047312091
-        },
-        {
-            "x": 2.242886939864493,
-            "y": 2.818887609111427,
-            "heading": -0.31503068232040304,
-            "angularVelocity": -0.000001188286624986991,
-            "velocityX": 2.38938975806231,
-            "velocityY": -2.9328223316783686,
-            "timestamp": 0.9818439720878511
-        },
-        {
-            "x": 2.3383115633038396,
-            "y": 2.719295334161189,
-            "heading": -0.31503071689328305,
-            "angularVelocity": -9.482188022909844e-7,
-            "velocityX": 2.6171791939547426,
-            "velocityY": -2.7314839763979357,
-            "timestamp": 1.0183048394444931
-        },
-        {
-            "x": 2.4414252810789625,
-            "y": 2.627687242403576,
-            "heading": -0.3150307459166021,
-            "angularVelocity": -7.96012851101892e-7,
-            "velocityX": 2.8280654095942417,
-            "velocityY": -2.5125044574926996,
-            "timestamp": 1.0547657068011351
-        },
-        {
-            "x": 2.5515621254678895,
-            "y": 2.5446549735283583,
-            "heading": -0.3150307712925926,
-            "angularVelocity": -6.959787951410741e-7,
-            "velocityX": 3.020686351523773,
-            "velocityY": -2.2772982349278084,
-            "timestamp": 1.0912265741577771
-        },
-        {
-            "x": 2.668010765906386,
-            "y": 2.470734767010798,
-            "heading": -0.31503079425241676,
-            "angularVelocity": -6.297114091058936e-7,
-            "velocityX": 3.1937978682584367,
-            "velocityY": -2.027384751835701,
-            "timestamp": 1.1276874415144191
-        },
-        {
-            "x": 2.790019095895906,
-            "y": 2.406403976394343,
-            "heading": -0.31503081565982244,
-            "angularVelocity": -5.871337458164478e-7,
-            "velocityX": 3.3462816119017478,
-            "velocityY": -1.7643790529502215,
-            "timestamp": 1.1641483088710611
-        },
-        {
-            "x": 2.916799054801627,
-            "y": 2.352077873335088,
-            "heading": -0.31503083617399774,
-            "angularVelocity": -5.626354173508468e-7,
-            "velocityX": 3.477151480397404,
-            "velocityY": -1.4899838374074656,
-            "timestamp": 1.2006091762277031
-        },
-        {
-            "x": 3.0475310715630033,
-            "y": 2.308104889106193,
-            "heading": -0.31503085634737477,
-            "angularVelocity": -5.532884563345105e-7,
-            "velocityX": 3.585543247850392,
-            "velocityY": -1.2060323140087088,
-            "timestamp": 1.2370700435843451
-        },
-        {
-            "x": 3.1788176786437843,
-            "y": 2.2658164704041117,
-            "heading": -0.3150308764961414,
-            "angularVelocity": -5.526134756982608e-7,
-            "velocityX": 3.600753810835078,
-            "velocityY": -1.1598302993847416,
-            "timestamp": 1.2735309109409871
-        },
-        {
-            "x": 3.3101043321458232,
-            "y": 2.2235281958195148,
-            "heading": -0.3150308966449121,
-            "angularVelocity": -5.526135855693721e-7,
-            "velocityX": 3.600755084015385,
-            "velocityY": -1.159826346722746,
-            "timestamp": 1.3099917782976291
-        },
-        {
-            "x": 3.441390996396766,
-            "y": 2.181239954605585,
-            "heading": -0.31503091679368933,
-            "angularVelocity": -5.526137662420421e-7,
-            "velocityX": 3.6007553788219733,
-            "velocityY": -1.1598254314766572,
-            "timestamp": 1.3464526456542711
-        },
-        {
-            "x": 3.5726776715245805,
-            "y": 2.1389517471596347,
-            "heading": -0.31503093694247225,
-            "angularVelocity": -5.526139217103117e-7,
-            "velocityX": 3.600755677138282,
-            "velocityY": -1.1598245053335783,
-            "timestamp": 1.3829135130109131
-        },
-        {
-            "x": 3.7039643576361168,
-            "y": 2.0966635738134185,
-            "heading": -0.3150309570912594,
-            "angularVelocity": -5.526140383605173e-7,
-            "velocityX": 3.6007559783851373,
-            "velocityY": -1.159823570091575,
-            "timestamp": 1.4193743803675551
-        },
-        {
-            "x": 3.835251054850112,
-            "y": 2.0543754349355976,
-            "heading": -0.31503097724004914,
-            "angularVelocity": -5.526141098258176e-7,
-            "velocityX": 3.600756282888565,
-            "velocityY": -1.1598226247384584,
-            "timestamp": 1.4558352477241971
-        },
-        {
-            "x": 3.966537763342614,
-            "y": 2.012087331072757,
-            "heading": -0.3150309973888395,
-            "angularVelocity": -5.526141250500724e-7,
-            "velocityX": 3.6007565922204043,
-            "velocityY": -1.1598216643943178,
-            "timestamp": 1.4922961150808391
-        },
-        {
-            "x": 4.097824483400825,
-            "y": 1.9697992631165677,
-            "heading": -0.315031017537628,
-            "angularVelocity": -5.526140761706961e-7,
-            "velocityX": 3.6007569094292164,
-            "velocityY": -1.1598206795946073,
-            "timestamp": 1.5287569824374811
-        },
-        {
-            "x": 4.2291112154854185,
-            "y": 1.9275112324972625,
-            "heading": -0.3150310376864126,
-            "angularVelocity": -5.526139663753558e-7,
-            "velocityX": 3.600757239272763,
-            "velocityY": -1.1598196555683948,
-            "timestamp": 1.5652178497941232
-        },
-        {
-            "x": 4.360397911943487,
-            "y": 1.8852230912740195,
-            "heading": -0.3150310578362786,
-            "angularVelocity": -5.526436278671969e-7,
-            "velocityX": 3.600756262156026,
-            "velocityY": -1.159822689065559,
-            "timestamp": 1.6016787171507652
-        },
-        {
-            "x": 4.485161433583451,
-            "y": 1.8443297112343198,
-            "heading": -0.3370639022790847,
-            "angularVelocity": -0.6042874467931063,
-            "velocityX": 3.4218473307173203,
-            "velocityY": -1.121569041122948,
-            "timestamp": 1.6381395845074072
-        },
-        {
-            "x": 4.600444501590908,
-            "y": 1.8067758967734413,
-            "heading": -0.37703885598165165,
-            "angularVelocity": -1.0963796695112746,
-            "velocityX": 3.1618301034864644,
-            "velocityY": -1.029975894252485,
-            "timestamp": 1.6746004518640492
-        },
-        {
-            "x": 4.705219738357152,
-            "y": 1.7727109130585983,
-            "heading": -0.41891394403468696,
-            "angularVelocity": -1.1484940180779766,
-            "velocityX": 2.873635334600916,
-            "velocityY": -0.9342889016225668,
-            "timestamp": 1.7110613192206912
-        },
-        {
-            "x": 4.799468915908326,
-            "y": 1.742101106125887,
-            "heading": -0.4594847392760727,
-            "angularVelocity": -1.112721615877786,
-            "velocityX": 2.5849406331801226,
-            "velocityY": -0.8395249249915333,
-            "timestamp": 1.7475221865773332
-        },
-        {
-            "x": 4.883206605537126,
-            "y": 1.7149241083644653,
-            "heading": -0.4972998071219025,
-            "angularVelocity": -1.0371412033604122,
-            "velocityX": 2.2966455737248266,
-            "velocityY": -0.7453744173332496,
-            "timestamp": 1.7839830539339752
-        },
-        {
-            "x": 4.956447647832475,
-            "y": 1.6911654776329053,
-            "heading": -0.531527000493052,
-            "angularVelocity": -0.9387377715499247,
-            "velocityX": 2.008757542132538,
-            "velocityY": -0.6516200094519161,
-            "timestamp": 1.8204439212906172
-        },
-        {
-            "x": 5.019204190370849,
-            "y": 1.670815407546972,
-            "heading": -0.5616288284566417,
-            "angularVelocity": -0.8255927559032131,
-            "velocityX": 1.721202678052634,
-            "velocityY": -0.5581345579873002,
-            "timestamp": 1.8569047886472592
-        },
-        {
-            "x": 5.071485904769321,
-            "y": 1.6538669030565063,
-            "heading": -0.5872312038868318,
-            "angularVelocity": -0.7021877779198676,
-            "velocityX": 1.433913074175601,
-            "velocityY": -0.46484095742112364,
-            "timestamp": 1.8933656560039012
-        },
-        {
-            "x": 5.113300541419085,
-            "y": 1.6403147551859525,
-            "heading": -0.6080594891340769,
-            "angularVelocity": -0.5712504050853902,
-            "velocityX": 1.14683603768266,
-            "velocityY": -0.3716902216832496,
-            "timestamp": 1.9298265233605432
-        },
-        {
-            "x": 5.144654410764751,
-            "y": 1.6301549374248205,
-            "heading": -0.6239033656036709,
-            "angularVelocity": -0.43454469457947725,
-            "velocityX": 0.8599320756409368,
-            "velocityY": -0.2786499197003079,
-            "timestamp": 1.9662873907171852
-        },
-        {
-            "x": 5.165552750765901,
-            "y": 1.6233842339153202,
-            "heading": -0.634595855168354,
-            "angularVelocity": -0.29325933089016243,
-            "velocityX": 0.57317177336273,
-            "velocityY": -0.18569781797215093,
-            "timestamp": 2.002748258073827
+            "heading": -0.3150304332740777,
+            "angularVelocity": -0.0000034568027529519608,
+            "velocityX": 1.6198022751608165,
+            "velocityY": -3.4186049002831713,
+            "timestamp": 0.8724613748871122
+        },
+        {
+            "x": 2.0775165641117233,
+            "y": 3.0394044472847606,
+            "heading": -0.3150305189714284,
+            "angularVelocity": -0.000002350392551393865,
+            "velocityX": 1.8890857675343722,
+            "velocityY": -3.277495922091173,
+            "timestamp": 0.9089222420961518
+        },
+        {
+            "x": 2.155767726009908,
+            "y": 2.9258208622381168,
+            "heading": -0.3150305770370975,
+            "angularVelocity": -0.00000159254767063084,
+            "velocityX": 2.1461684235196876,
+            "velocityY": -3.1152189660064753,
+            "timestamp": 0.9453831093051914
+        },
+        {
+            "x": 2.242886953120307,
+            "y": 2.818887620265158,
+            "heading": -0.3150306203630764,
+            "angularVelocity": -0.0000011882871183876306,
+            "velocityX": 2.389389879591231,
+            "velocityY": -2.932822232666113,
+            "timestamp": 0.981843976514231
+        },
+        {
+            "x": 2.338311580267833,
+            "y": 2.719295349641276,
+            "heading": -0.3150306549359537,
+            "angularVelocity": -9.482187337248934e-7,
+            "velocityX": 2.617179306252704,
+            "velocityY": -2.731483868798106,
+            "timestamp": 1.0183048437232707
+        },
+        {
+            "x": 2.4414253013619946,
+            "y": 2.6276872624602983,
+            "heading": -0.31503068395926054,
+            "angularVelocity": -7.960125215618009e-7,
+            "velocityX": 2.8280655120730875,
+            "velocityY": -2.512504342142078,
+            "timestamp": 1.0547657109323103
+        },
+        {
+            "x": 2.5515621486647997,
+            "y": 2.5446549983777076,
+            "heading": -0.3150307093352344,
+            "angularVelocity": -6.959783403374032e-7,
+            "velocityX": 3.020686443670201,
+            "velocityY": -2.2772981127010774,
+            "timestamp": 1.09122657814135
+        },
+        {
+            "x": 2.6680107915990137,
+            "y": 2.4707347968336117,
+            "heading": -0.31503073229503964,
+            "angularVelocity": -6.297108928988803e-7,
+            "velocityX": 3.1937979496369038,
+            "velocityY": -2.0273846236374995,
+            "timestamp": 1.1276874453503896
+        },
+        {
+            "x": 2.7900191236562386,
+            "y": 2.4064040113359084,
+            "heading": -0.3150307537024253,
+            "angularVelocity": -5.871332010760787e-7,
+            "velocityX": 3.3462816821585726,
+            "velocityY": -1.764378919702536,
+            "timestamp": 1.1641483125594292
+        },
+        {
+            "x": 2.9167990841955884,
+            "y": 2.352077913506683,
+            "heading": -0.3150307742165802,
+            "angularVelocity": -5.626348607586924e-7,
+            "velocityX": 3.477151539278742,
+            "velocityY": -1.4899836999970275,
+            "timestamp": 1.2006091797684688
+        },
+        {
+            "x": 3.0475311021850517,
+            "y": 2.308104934680324,
+            "heading": -0.31503079438993686,
+            "angularVelocity": -5.532878997498012e-7,
+            "velocityX": 3.585543296047864,
+            "velocityY": -1.2060321707174542,
+            "timestamp": 1.2370700469775084
+        },
+        {
+            "x": 3.178817728061225,
+            "y": 2.2658165761507623,
+            "heading": -0.3150308145386853,
+            "angularVelocity": -5.526129783451472e-7,
+            "velocityX": 3.600754340906743,
+            "velocityY": -1.159828653748452,
+            "timestamp": 1.273530914186548
+        },
+        {
+            "x": 3.3101043979960063,
+            "y": 2.2235283544039026,
+            "heading": -0.3150308346874411,
+            "angularVelocity": -5.526131795163358e-7,
+            "velocityX": 3.600755549287413,
+            "velocityY": -1.1598249022550946,
+            "timestamp": 1.3099917813955877
+        },
+        {
+            "x": 3.4413910762638236,
+            "y": 2.1812401585275256,
+            "heading": -0.31503085483620696,
+            "angularVelocity": -5.52613457009699e-7,
+            "velocityX": 3.60075577783481,
+            "velocityY": -1.159824192714005,
+            "timestamp": 1.3464526486046273
+        },
+        {
+            "x": 3.572677762945226,
+            "y": 2.138951988771722,
+            "heading": -0.31503087498498206,
+            "angularVelocity": -5.52613710159953e-7,
+            "velocityX": 3.6007560085914068,
+            "velocityY": -1.1598234763137998,
+            "timestamp": 1.382913515813667
+        },
+        {
+            "x": 3.7039644581047573,
+            "y": 2.0966638453368867,
+            "heading": -0.31503089513376514,
+            "angularVelocity": -5.526139295643368e-7,
+            "velocityX": 3.6007562411181984,
+            "velocityY": -1.1598227544173942,
+            "timestamp": 1.4193743830227066
+        },
+        {
+            "x": 3.8352511618245644,
+            "y": 2.05437572847807,
+            "heading": -0.3150309152825546,
+            "angularVelocity": -5.526141044747151e-7,
+            "velocityX": 3.600756475898009,
+            "velocityY": -1.1598220255258234,
+            "timestamp": 1.4558352502317462
+        },
+        {
+            "x": 3.966537874250263,
+            "y": 2.01208763864737,
+            "heading": -0.3150309354313486,
+            "angularVelocity": -5.526142273042023e-7,
+            "velocityX": 3.6007567146716046,
+            "velocityY": -1.159821284234734,
+            "timestamp": 1.4922961174407858
+        },
+        {
+            "x": 4.097824595645338,
+            "y": 1.9697995766628096,
+            "heading": -0.3150309555801447,
+            "angularVelocity": -5.526142864672618e-7,
+            "velocityX": 3.600756960671674,
+            "velocityY": -1.159820520507965,
+            "timestamp": 1.5287569846498255
+        },
+        {
+            "x": 4.229111326453571,
+            "y": 1.9275115439021684,
+            "heading": -0.3150309757289402,
+            "angularVelocity": -5.5261426880692e-7,
+            "velocityX": 3.600757218843208,
+            "velocityY": -1.1598197189933317,
+            "timestamp": 1.565217851858865
+        },
+        {
+            "x": 4.360398019008411,
+            "y": 1.885223392382287,
+            "heading": -0.31503099587881916,
+            "angularVelocity": -5.52643987126892e-7,
+            "velocityX": 3.6007561696802117,
+            "velocityY": -1.1598229761632481,
+            "timestamp": 1.6016787190679047
+        },
+        {
+            "x": 4.485161526388635,
+            "y": 1.844329974236374,
+            "heading": -0.3370638509456068,
+            "angularVelocity": -0.6042877406197901,
+            "velocityX": 3.4218469534726492,
+            "velocityY": -1.1215700907896822,
+            "timestamp": 1.6381395862769443
+        },
+        {
+            "x": 4.60044458089581,
+            "y": 1.8067761222092775,
+            "heading": -0.3770388194107599,
+            "angularVelocity": -1.0963800788381923,
+            "velocityX": 3.1618297460185825,
+            "velocityY": -1.0299769287381584,
+            "timestamp": 1.674600453485984
+        },
+        {
+            "x": 4.705219804759057,
+            "y": 1.772711102552083,
+            "heading": -0.4189139196427851,
+            "angularVelocity": -1.148494356756437,
+            "velocityX": 2.8736349923479074,
+            "velocityY": -0.9342898911836356,
+            "timestamp": 1.7110613206950236
+        },
+        {
+            "x": 4.799468970200479,
+            "y": 1.7421012617230478,
+            "heading": -0.45948472427125614,
+            "angularVelocity": -1.1127218778387968,
+            "velocityX": 2.5849403115144614,
+            "velocityY": -0.8395258580532651,
+            "timestamp": 1.7475221879040632
+        },
+        {
+            "x": 4.883206648654635,
+            "y": 1.7149242324789875,
+            "heading": -0.49729979894596704,
+            "angularVelocity": -1.037141394852447,
+            "velocityX": 2.296645276538986,
+            "velocityY": -0.7453752838144925,
+            "timestamp": 1.7839830551131028
+        },
+        {
+            "x": 4.956447680828415,
+            "y": 1.6911655730288653,
+            "heading": -0.5315269969459382,
+            "angularVelocity": -0.9387379023033031,
+            "velocityX": 2.0087572726635705,
+            "velocityY": -0.6516207997442213,
+            "timestamp": 1.8204439223221425
+        },
+        {
+            "x": 5.019204214404415,
+            "y": 1.6708154773305386,
+            "heading": -0.5616288277349688,
+            "angularVelocity": -0.825592836737831,
+            "velocityX": 1.7212024392124468,
+            "velocityY": -0.5581352627093101,
+            "timestamp": 1.856904789531182
+        },
+        {
+            "x": 5.071485921099441,
+            "y": 1.6538669506708767,
+            "heading": -0.587231204588931,
+            "angularVelocity": -0.7021878198118158,
+            "velocityX": 1.4339128687006073,
+            "velocityY": -0.46484156733002446,
+            "timestamp": 1.8933656567402217
+        },
+        {
+            "x": 5.11330055140079,
+            "y": 1.6403147844077772,
+            "heading": -0.6080594902506735,
+            "angularVelocity": -0.5712504187662296,
+            "velocityX": 1.1468358682094448,
+            "velocityY": -0.37169072763413913,
+            "timestamp": 1.9298265239492614
+        },
+        {
+            "x": 5.1446544158470235,
+            "y": 1.6301549523614358,
+            "heading": -0.6239033665029361,
+            "angularVelocity": -0.43454469037794713,
+            "velocityX": 0.859931944747064,
+            "velocityY": -0.2786503126240163,
+            "timestamp": 1.966287391158301
+        },
+        {
+            "x": 5.165552752490355,
+            "y": 1.6233842390024875,
+            "heading": -0.6345958555798376,
+            "angularVelocity": -0.29325931869912014,
+            "velocityX": 0.5731716835893133,
+            "velocityY": -0.18569808886140712,
+            "timestamp": 2.0027482583673404
         },
         {
             "x": 5.176,
             "y": 1.62,
             "heading": -0.64,
-            "angularVelocity": -0.14821767070941108,
-            "velocityX": 0.28653320646240676,
-            "velocityY": -0.09281825037833577,
-            "timestamp": 2.039209125430469
+            "angularVelocity": -0.14821766002380998,
+            "velocityX": 0.2865331603263514,
+            "velocityY": -0.09281839027812194,
+            "timestamp": 2.03920912557638
         },
         {
             "x": 5.176,
             "y": 1.62,
             "heading": -0.64,
-            "angularVelocity": 2.131559152063316e-30,
-            "velocityX": 2.0336113095175017e-32,
-            "velocityY": 7.340802046004562e-32,
-            "timestamp": 2.075669992787111
+            "angularVelocity": 2.046404654845657e-31,
+            "velocityX": 2.793064044552828e-33,
+            "velocityY": 5.221456528298987e-32,
+            "timestamp": 2.0756699927854196
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.1.traj
index b542317e..4ff6b6c8 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.1.traj	
@@ -4,118 +4,226 @@
             "x": 2.3,
             "y": 4.36,
             "heading": -0.39,
-            "angularVelocity": 1.6493730429010574e-29,
-            "velocityX": 1.2833975714085328e-29,
-            "velocityY": -2.2715148176984883e-30,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 2.2589715401081683,
-            "y": 4.250531667444243,
-            "heading": -0.30389257282100673,
-            "angularVelocity": 0.7199270532350494,
-            "velocityX": -0.3430307837128689,
-            "velocityY": -0.9152429315492105,
-            "timestamp": 0.11960576671208706
-        },
-        {
-            "x": 2.20035382258247,
-            "y": 4.024377658967842,
-            "heading": -0.15502297939398071,
-            "angularVelocity": 1.244669028252838,
-            "velocityX": -0.49009106447848483,
-            "velocityY": -1.890828633880168,
-            "timestamp": 0.23921153342417412
-        },
-        {
-            "x": 2.196580641751606,
-            "y": 3.6923673074709873,
-            "heading": -0.01977955961101222,
-            "angularVelocity": 1.1307433036110668,
-            "velocityX": -0.031546813624408795,
-            "velocityY": -2.7758724401313977,
-            "timestamp": 0.3588173001362612
-        },
-        {
-            "x": 2.3070960866935795,
-            "y": 3.359465621426977,
-            "heading": -0.010919467382492536,
-            "angularVelocity": 0.07407746693291484,
-            "velocityX": 0.9239976297129873,
-            "velocityY": -2.7833247108007275,
-            "timestamp": 0.47842306684834823
-        },
-        {
-            "x": 2.522207133394474,
-            "y": 3.0732241883923836,
-            "heading": -0.12115396742434953,
-            "angularVelocity": -0.9216487053427286,
-            "velocityX": 1.7985006293108312,
-            "velocityY": -2.3932076262144095,
-            "timestamp": 0.5980288335604353
-        },
-        {
-            "x": 2.8293982195436245,
-            "y": 2.8628067869682225,
-            "heading": -0.23050381325401667,
-            "angularVelocity": -0.9142522876239637,
-            "velocityX": 2.5683635044839206,
-            "velocityY": -1.759257995734114,
-            "timestamp": 0.7176346002725225
-        },
-        {
-            "x": 3.2122552089453738,
-            "y": 2.74459738527869,
-            "heading": -0.337506021977117,
-            "angularVelocity": -0.8946241612299765,
-            "velocityX": 3.200991055250252,
-            "velocityY": -0.9883252700855956,
-            "timestamp": 0.8372403669846096
-        },
-        {
-            "x": 3.6197114657100724,
-            "y": 2.743095325026798,
-            "heading": -0.4390431608485269,
-            "angularVelocity": -0.8489318003875522,
-            "velocityX": 3.406660631552456,
-            "velocityY": -0.012558426681036643,
-            "timestamp": 0.9568461336966967
-        },
-        {
-            "x": 3.949401767099036,
-            "y": 2.832014285774074,
-            "heading": -0.5329543188084961,
-            "angularVelocity": -0.7851724924441524,
-            "velocityX": 2.7564749631394143,
-            "velocityY": 0.7434337255770672,
-            "timestamp": 1.0764519004087838
-        },
-        {
-            "x": 4.162198776267108,
-            "y": 2.936236025183353,
-            "heading": -0.5618569264034092,
-            "angularVelocity": -0.24164894711549667,
-            "velocityX": 1.7791534222619447,
-            "velocityY": 0.8713772109346234,
-            "timestamp": 1.196057667120871
+            "x": 2.284359298751307,
+            "y": 4.3328670624584875,
+            "heading": -0.3913603360985968,
+            "angularVelocity": -0.02219557362709146,
+            "velocityX": -0.2551974739939649,
+            "velocityY": -0.44270758788444614,
+            "timestamp": 0.061288620940906494
+        },
+        {
+            "x": 2.2541133789559487,
+            "y": 4.2780305351291235,
+            "heading": -0.3941996942145126,
+            "angularVelocity": -0.04632765548197678,
+            "velocityX": -0.4934997611468709,
+            "velocityY": -0.89472607618691,
+            "timestamp": 0.12257724188181299
+        },
+        {
+            "x": 2.2108693669713233,
+            "y": 4.194701771021148,
+            "heading": -0.39869373638946143,
+            "angularVelocity": -0.0733258818024626,
+            "velocityX": -0.7055797849705345,
+            "velocityY": -1.3596123200148318,
+            "timestamp": 0.1838658628227195
+        },
+        {
+            "x": 2.157413012714945,
+            "y": 4.081769184110331,
+            "heading": -0.40512443208979565,
+            "angularVelocity": -0.1049247903054391,
+            "velocityX": -0.8722068376757797,
+            "velocityY": -1.8426354709417345,
+            "timestamp": 0.24515448376362597
+        },
+        {
+            "x": 2.099439422754663,
+            "y": 3.9378510886177804,
+            "heading": -0.41398926531008917,
+            "angularVelocity": -0.1446407682894496,
+            "velocityX": -0.9459111507204496,
+            "velocityY": -2.348202542056109,
+            "timestamp": 0.30644310470453245
+        },
+        {
+            "x": 2.05084134036617,
+            "y": 3.764062564560248,
+            "heading": -0.42610260644735526,
+            "angularVelocity": -0.1976442111325286,
+            "velocityX": -0.7929380958881483,
+            "velocityY": -2.835575697242347,
+            "timestamp": 0.36773172564543893
+        },
+        {
+            "x": 2.0313297442556637,
+            "y": 3.578686605657539,
+            "heading": -0.44115755081157715,
+            "angularVelocity": -0.24564012263773397,
+            "velocityX": -0.3183559331400068,
+            "velocityY": -3.024639093795978,
+            "timestamp": 0.4290203465863454
+        },
+        {
+            "x": 2.0424292488902682,
+            "y": 3.3999266361315956,
+            "heading": -0.45764648760363913,
+            "angularVelocity": -0.2690374907923018,
+            "velocityX": 0.18110220892890863,
+            "velocityY": -2.9166910069375023,
+            "timestamp": 0.4903089675272519
+        },
+        {
+            "x": 2.0816987250314285,
+            "y": 3.234855796713624,
+            "heading": -0.4747853916237668,
+            "angularVelocity": -0.27964251368378285,
+            "velocityX": 0.6407302944379066,
+            "velocityY": -2.6933358408754287,
+            "timestamp": 0.5515975884681584
+        },
+        {
+            "x": 2.1473776050382467,
+            "y": 3.086622748034336,
+            "heading": -0.49218081575104855,
+            "angularVelocity": -0.2838279579508591,
+            "velocityX": 1.071632531431,
+            "velocityY": -2.418606364502998,
+            "timestamp": 0.6128862094090649
+        },
+        {
+            "x": 2.2382969856262207,
+            "y": 2.9569339752197266,
+            "heading": -0.5096053711064815,
+            "angularVelocity": -0.2843032701328595,
+            "velocityX": 1.4834626590087072,
+            "velocityY": -2.116033463041253,
+            "timestamp": 0.6741748303499714
+        },
+        {
+            "x": 2.3750291535583634,
+            "y": 2.833789120602945,
+            "heading": -0.5294729435704262,
+            "angularVelocity": -0.282105779182567,
+            "velocityX": 1.9415021560293924,
+            "velocityY": -1.7485717103605045,
+            "timestamp": 0.7446007979480589
+        },
+        {
+            "x": 2.5431654959507464,
+            "y": 2.737552434720476,
+            "heading": -0.5488979650801319,
+            "angularVelocity": -0.2758218619098283,
+            "velocityX": 2.3874196993915153,
+            "velocityY": -1.366494336743516,
+            "timestamp": 0.8150267655461465
+        },
+        {
+            "x": 2.7413669689925393,
+            "y": 2.669711436345154,
+            "heading": -0.5674506234763109,
+            "angularVelocity": -0.2634349094364851,
+            "velocityX": 2.8143237473555907,
+            "velocityY": -0.9632952260234809,
+            "timestamp": 0.8854527331442341
+        },
+        {
+            "x": 2.9672720655352354,
+            "y": 2.6325722400915463,
+            "heading": -0.5844267226661307,
+            "angularVelocity": -0.24104885979984425,
+            "velocityX": 3.207696028145596,
+            "velocityY": -0.5273508837756647,
+            "timestamp": 0.9558787007423217
+        },
+        {
+            "x": 3.21586763898004,
+            "y": 2.629998478445332,
+            "heading": -0.5985057428976699,
+            "angularVelocity": -0.19991234358165255,
+            "velocityX": 3.529885096694866,
+            "velocityY": -0.03654563414595668,
+            "timestamp": 1.0263046683404093
+        },
+        {
+            "x": 3.4730987079905056,
+            "y": 2.6678365706373897,
+            "heading": -0.6068355688677516,
+            "angularVelocity": -0.11827776392962061,
+            "velocityX": 3.652503157336117,
+            "velocityY": 0.5372747224148254,
+            "timestamp": 1.096730635938497
+        },
+        {
+            "x": 3.704150591962856,
+            "y": 2.737591563426048,
+            "heading": -0.6062385008661115,
+            "angularVelocity": 0.00847795240879764,
+            "velocityX": 3.28077684769537,
+            "velocityY": 0.9904726220694814,
+            "timestamp": 1.1671566035365846
+        },
+        {
+            "x": 3.894448488053554,
+            "y": 2.8141322684658854,
+            "heading": -0.5991077413907381,
+            "angularVelocity": 0.10125184954600595,
+            "velocityX": 2.702098424500253,
+            "velocityY": 1.086825039829707,
+            "timestamp": 1.2375825711346722
+        },
+        {
+            "x": 4.043960756386648,
+            "y": 2.883897596305393,
+            "heading": -0.5885679211534333,
+            "angularVelocity": 0.1496581530473843,
+            "velocityX": 2.122970736963697,
+            "velocityY": 0.9906193726389452,
+            "timestamp": 1.3080085387327598
+        },
+        {
+            "x": 4.154363809487208,
+            "y": 2.940218619761556,
+            "heading": -0.5783208756256007,
+            "angularVelocity": 0.14550095479427785,
+            "velocityX": 1.5676469470837475,
+            "velocityY": 0.7997195548321091,
+            "timestamp": 1.3784345063308474
+        },
+        {
+            "x": 4.227038806897989,
+            "y": 2.9796144762337837,
+            "heading": -0.5703846528919979,
+            "angularVelocity": 0.11268887037369305,
+            "velocityX": 1.0319346668480025,
+            "velocityY": 0.5593938971070241,
+            "timestamp": 1.448860473928935
         },
         {
             "x": 4.263,
             "y": 3,
             "heading": -0.566,
-            "angularVelocity": -0.03463941338677448,
-            "velocityX": 0.8427789604454271,
-            "velocityY": 0.5331178969835093,
-            "timestamp": 1.315663433832958
+            "angularVelocity": 0.06225903656760019,
+            "velocityX": 0.5106240542868632,
+            "velocityY": 0.2894603292148455,
+            "timestamp": 1.5192864415270226
         },
         {
             "x": 4.263,
             "y": 3,
             "heading": -0.566,
-            "angularVelocity": 0,
-            "velocityX": 1.4642265257925552e-30,
-            "velocityY": 5.326911647277941e-29,
-            "timestamp": 1.4352692005450451
+            "angularVelocity": 9.24329495349137e-32,
+            "velocityX": 0,
+            "velocityY": 1.2740379664097394e-33,
+            "timestamp": 1.5897124091251102
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.traj
index b542317e..4ff6b6c8 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.traj	
@@ -4,118 +4,226 @@
             "x": 2.3,
             "y": 4.36,
             "heading": -0.39,
-            "angularVelocity": 1.6493730429010574e-29,
-            "velocityX": 1.2833975714085328e-29,
-            "velocityY": -2.2715148176984883e-30,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 2.2589715401081683,
-            "y": 4.250531667444243,
-            "heading": -0.30389257282100673,
-            "angularVelocity": 0.7199270532350494,
-            "velocityX": -0.3430307837128689,
-            "velocityY": -0.9152429315492105,
-            "timestamp": 0.11960576671208706
-        },
-        {
-            "x": 2.20035382258247,
-            "y": 4.024377658967842,
-            "heading": -0.15502297939398071,
-            "angularVelocity": 1.244669028252838,
-            "velocityX": -0.49009106447848483,
-            "velocityY": -1.890828633880168,
-            "timestamp": 0.23921153342417412
-        },
-        {
-            "x": 2.196580641751606,
-            "y": 3.6923673074709873,
-            "heading": -0.01977955961101222,
-            "angularVelocity": 1.1307433036110668,
-            "velocityX": -0.031546813624408795,
-            "velocityY": -2.7758724401313977,
-            "timestamp": 0.3588173001362612
-        },
-        {
-            "x": 2.3070960866935795,
-            "y": 3.359465621426977,
-            "heading": -0.010919467382492536,
-            "angularVelocity": 0.07407746693291484,
-            "velocityX": 0.9239976297129873,
-            "velocityY": -2.7833247108007275,
-            "timestamp": 0.47842306684834823
-        },
-        {
-            "x": 2.522207133394474,
-            "y": 3.0732241883923836,
-            "heading": -0.12115396742434953,
-            "angularVelocity": -0.9216487053427286,
-            "velocityX": 1.7985006293108312,
-            "velocityY": -2.3932076262144095,
-            "timestamp": 0.5980288335604353
-        },
-        {
-            "x": 2.8293982195436245,
-            "y": 2.8628067869682225,
-            "heading": -0.23050381325401667,
-            "angularVelocity": -0.9142522876239637,
-            "velocityX": 2.5683635044839206,
-            "velocityY": -1.759257995734114,
-            "timestamp": 0.7176346002725225
-        },
-        {
-            "x": 3.2122552089453738,
-            "y": 2.74459738527869,
-            "heading": -0.337506021977117,
-            "angularVelocity": -0.8946241612299765,
-            "velocityX": 3.200991055250252,
-            "velocityY": -0.9883252700855956,
-            "timestamp": 0.8372403669846096
-        },
-        {
-            "x": 3.6197114657100724,
-            "y": 2.743095325026798,
-            "heading": -0.4390431608485269,
-            "angularVelocity": -0.8489318003875522,
-            "velocityX": 3.406660631552456,
-            "velocityY": -0.012558426681036643,
-            "timestamp": 0.9568461336966967
-        },
-        {
-            "x": 3.949401767099036,
-            "y": 2.832014285774074,
-            "heading": -0.5329543188084961,
-            "angularVelocity": -0.7851724924441524,
-            "velocityX": 2.7564749631394143,
-            "velocityY": 0.7434337255770672,
-            "timestamp": 1.0764519004087838
-        },
-        {
-            "x": 4.162198776267108,
-            "y": 2.936236025183353,
-            "heading": -0.5618569264034092,
-            "angularVelocity": -0.24164894711549667,
-            "velocityX": 1.7791534222619447,
-            "velocityY": 0.8713772109346234,
-            "timestamp": 1.196057667120871
+            "x": 2.284359298751307,
+            "y": 4.3328670624584875,
+            "heading": -0.3913603360985968,
+            "angularVelocity": -0.02219557362709146,
+            "velocityX": -0.2551974739939649,
+            "velocityY": -0.44270758788444614,
+            "timestamp": 0.061288620940906494
+        },
+        {
+            "x": 2.2541133789559487,
+            "y": 4.2780305351291235,
+            "heading": -0.3941996942145126,
+            "angularVelocity": -0.04632765548197678,
+            "velocityX": -0.4934997611468709,
+            "velocityY": -0.89472607618691,
+            "timestamp": 0.12257724188181299
+        },
+        {
+            "x": 2.2108693669713233,
+            "y": 4.194701771021148,
+            "heading": -0.39869373638946143,
+            "angularVelocity": -0.0733258818024626,
+            "velocityX": -0.7055797849705345,
+            "velocityY": -1.3596123200148318,
+            "timestamp": 0.1838658628227195
+        },
+        {
+            "x": 2.157413012714945,
+            "y": 4.081769184110331,
+            "heading": -0.40512443208979565,
+            "angularVelocity": -0.1049247903054391,
+            "velocityX": -0.8722068376757797,
+            "velocityY": -1.8426354709417345,
+            "timestamp": 0.24515448376362597
+        },
+        {
+            "x": 2.099439422754663,
+            "y": 3.9378510886177804,
+            "heading": -0.41398926531008917,
+            "angularVelocity": -0.1446407682894496,
+            "velocityX": -0.9459111507204496,
+            "velocityY": -2.348202542056109,
+            "timestamp": 0.30644310470453245
+        },
+        {
+            "x": 2.05084134036617,
+            "y": 3.764062564560248,
+            "heading": -0.42610260644735526,
+            "angularVelocity": -0.1976442111325286,
+            "velocityX": -0.7929380958881483,
+            "velocityY": -2.835575697242347,
+            "timestamp": 0.36773172564543893
+        },
+        {
+            "x": 2.0313297442556637,
+            "y": 3.578686605657539,
+            "heading": -0.44115755081157715,
+            "angularVelocity": -0.24564012263773397,
+            "velocityX": -0.3183559331400068,
+            "velocityY": -3.024639093795978,
+            "timestamp": 0.4290203465863454
+        },
+        {
+            "x": 2.0424292488902682,
+            "y": 3.3999266361315956,
+            "heading": -0.45764648760363913,
+            "angularVelocity": -0.2690374907923018,
+            "velocityX": 0.18110220892890863,
+            "velocityY": -2.9166910069375023,
+            "timestamp": 0.4903089675272519
+        },
+        {
+            "x": 2.0816987250314285,
+            "y": 3.234855796713624,
+            "heading": -0.4747853916237668,
+            "angularVelocity": -0.27964251368378285,
+            "velocityX": 0.6407302944379066,
+            "velocityY": -2.6933358408754287,
+            "timestamp": 0.5515975884681584
+        },
+        {
+            "x": 2.1473776050382467,
+            "y": 3.086622748034336,
+            "heading": -0.49218081575104855,
+            "angularVelocity": -0.2838279579508591,
+            "velocityX": 1.071632531431,
+            "velocityY": -2.418606364502998,
+            "timestamp": 0.6128862094090649
+        },
+        {
+            "x": 2.2382969856262207,
+            "y": 2.9569339752197266,
+            "heading": -0.5096053711064815,
+            "angularVelocity": -0.2843032701328595,
+            "velocityX": 1.4834626590087072,
+            "velocityY": -2.116033463041253,
+            "timestamp": 0.6741748303499714
+        },
+        {
+            "x": 2.3750291535583634,
+            "y": 2.833789120602945,
+            "heading": -0.5294729435704262,
+            "angularVelocity": -0.282105779182567,
+            "velocityX": 1.9415021560293924,
+            "velocityY": -1.7485717103605045,
+            "timestamp": 0.7446007979480589
+        },
+        {
+            "x": 2.5431654959507464,
+            "y": 2.737552434720476,
+            "heading": -0.5488979650801319,
+            "angularVelocity": -0.2758218619098283,
+            "velocityX": 2.3874196993915153,
+            "velocityY": -1.366494336743516,
+            "timestamp": 0.8150267655461465
+        },
+        {
+            "x": 2.7413669689925393,
+            "y": 2.669711436345154,
+            "heading": -0.5674506234763109,
+            "angularVelocity": -0.2634349094364851,
+            "velocityX": 2.8143237473555907,
+            "velocityY": -0.9632952260234809,
+            "timestamp": 0.8854527331442341
+        },
+        {
+            "x": 2.9672720655352354,
+            "y": 2.6325722400915463,
+            "heading": -0.5844267226661307,
+            "angularVelocity": -0.24104885979984425,
+            "velocityX": 3.207696028145596,
+            "velocityY": -0.5273508837756647,
+            "timestamp": 0.9558787007423217
+        },
+        {
+            "x": 3.21586763898004,
+            "y": 2.629998478445332,
+            "heading": -0.5985057428976699,
+            "angularVelocity": -0.19991234358165255,
+            "velocityX": 3.529885096694866,
+            "velocityY": -0.03654563414595668,
+            "timestamp": 1.0263046683404093
+        },
+        {
+            "x": 3.4730987079905056,
+            "y": 2.6678365706373897,
+            "heading": -0.6068355688677516,
+            "angularVelocity": -0.11827776392962061,
+            "velocityX": 3.652503157336117,
+            "velocityY": 0.5372747224148254,
+            "timestamp": 1.096730635938497
+        },
+        {
+            "x": 3.704150591962856,
+            "y": 2.737591563426048,
+            "heading": -0.6062385008661115,
+            "angularVelocity": 0.00847795240879764,
+            "velocityX": 3.28077684769537,
+            "velocityY": 0.9904726220694814,
+            "timestamp": 1.1671566035365846
+        },
+        {
+            "x": 3.894448488053554,
+            "y": 2.8141322684658854,
+            "heading": -0.5991077413907381,
+            "angularVelocity": 0.10125184954600595,
+            "velocityX": 2.702098424500253,
+            "velocityY": 1.086825039829707,
+            "timestamp": 1.2375825711346722
+        },
+        {
+            "x": 4.043960756386648,
+            "y": 2.883897596305393,
+            "heading": -0.5885679211534333,
+            "angularVelocity": 0.1496581530473843,
+            "velocityX": 2.122970736963697,
+            "velocityY": 0.9906193726389452,
+            "timestamp": 1.3080085387327598
+        },
+        {
+            "x": 4.154363809487208,
+            "y": 2.940218619761556,
+            "heading": -0.5783208756256007,
+            "angularVelocity": 0.14550095479427785,
+            "velocityX": 1.5676469470837475,
+            "velocityY": 0.7997195548321091,
+            "timestamp": 1.3784345063308474
+        },
+        {
+            "x": 4.227038806897989,
+            "y": 2.9796144762337837,
+            "heading": -0.5703846528919979,
+            "angularVelocity": 0.11268887037369305,
+            "velocityX": 1.0319346668480025,
+            "velocityY": 0.5593938971070241,
+            "timestamp": 1.448860473928935
         },
         {
             "x": 4.263,
             "y": 3,
             "heading": -0.566,
-            "angularVelocity": -0.03463941338677448,
-            "velocityX": 0.8427789604454271,
-            "velocityY": 0.5331178969835093,
-            "timestamp": 1.315663433832958
+            "angularVelocity": 0.06225903656760019,
+            "velocityX": 0.5106240542868632,
+            "velocityY": 0.2894603292148455,
+            "timestamp": 1.5192864415270226
         },
         {
             "x": 4.263,
             "y": 3,
             "heading": -0.566,
-            "angularVelocity": 0,
-            "velocityX": 1.4642265257925552e-30,
-            "velocityY": 5.326911647277941e-29,
-            "timestamp": 1.4352692005450451
+            "angularVelocity": 9.24329495349137e-32,
+            "velocityX": 0,
+            "velocityY": 1.2740379664097394e-33,
+            "timestamp": 1.5897124091251102
         }
     ]
 }
\ No newline at end of file

From d8cb5f00ae43c3cea93c1b1e0975103b2a2562d1 Mon Sep 17 00:00:00 2001
From: Awesomeplayer165 <dev@jacob2.dev>
Date: Sun, 18 Feb 2024 22:36:58 -0800
Subject: [PATCH 40/62] remove new path from traj folder from created py script

---
 .../utils/dynamicauto/FindMissingPath.py      |  45 ++
 .../dynamicauto/deploy/choreo/New Path.1.traj | 625 ------------------
 .../dynamicauto/deploy/choreo/New Path.traj   | 625 ------------------
 .../java/frc/robot/utils/dynamicauto/dos.txt  |   1 +
 .../java/frc/robot/utils/dynamicauto/uno.txt  |   1 +
 5 files changed, 47 insertions(+), 1250 deletions(-)
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/FindMissingPath.py
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/dos.txt
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/uno.txt

diff --git a/src/main/java/frc/robot/utils/dynamicauto/FindMissingPath.py b/src/main/java/frc/robot/utils/dynamicauto/FindMissingPath.py
new file mode 100644
index 00000000..f3b2a456
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/FindMissingPath.py
@@ -0,0 +1,45 @@
+import json
+import os
+
+# we know that the file missing must be missing from the deploy/choreo folder
+
+# open file in read mode
+
+paths = {}
+
+with open('src/main/java/frc/robot/utils/dynamicauto/data.chor', 'r') as file:
+    data = json.load(file)
+
+    paths = data["paths"]
+
+print("Number of paths in data.chor:")
+print(len(paths))
+
+folder_path = "src/main/java/frc/robot/utils/dynamicauto/deploy/choreo"
+files_in_folder = os.listdir(folder_path)
+
+print("Files in deploy/choreo folder:")
+print(len(files_in_folder) / 2)
+
+originalFiles = []
+
+for file in files_in_folder:
+    if not file.endswith(".1.traj"):
+        originalFiles.append(file[:-5])
+
+print("Files that end with '.1.traj':")
+print(len(originalFiles))
+
+print("Finding missing files...")
+
+pathNames = [value for value in paths.keys()]
+
+missingFiles = []
+
+print(originalFiles)
+
+print(pathNames)
+
+print("Missing files:")
+
+print(list(set(originalFiles) ^ set(pathNames)))
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.1.traj
deleted file mode 100644
index 79c52dac..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.1.traj	
+++ /dev/null
@@ -1,625 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.7328571677207947,
-            "y": 6.758700847625732,
-            "heading": -2.1130989969635645,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7582517080124825,
-            "y": 6.75986365233427,
-            "heading": -2.1364061905138043,
-            "angularVelocity": -0.4153761780165196,
-            "velocityX": 0.4525764573976041,
-            "velocityY": 0.020723274750874442,
-            "timestamp": 0.05611105013661352
-        },
-        {
-            "x": 0.8091370757597145,
-            "y": 6.7622371099463585,
-            "heading": -2.1815821515780387,
-            "angularVelocity": -0.8051170126783445,
-            "velocityX": 0.9068689255207545,
-            "velocityY": 0.04229929053754837,
-            "timestamp": 0.11222210027322704
-        },
-        {
-            "x": 0.8856327770155913,
-            "y": 6.7658721035419465,
-            "heading": -2.246734552492055,
-            "angularVelocity": -1.1611331592509972,
-            "velocityX": 1.3632912068056597,
-            "velocityY": 0.06478213447706996,
-            "timestamp": 0.16833315040984057
-        },
-        {
-            "x": 0.9878912539433133,
-            "y": 6.770811955024787,
-            "heading": -2.329199596325353,
-            "angularVelocity": -1.4696756455728817,
-            "velocityX": 1.8224302820701674,
-            "velocityY": 0.08803705278779066,
-            "timestamp": 0.2244442005464541
-        },
-        {
-            "x": 1.116115460670533,
-            "y": 6.777089415314351,
-            "heading": -2.4245891589263606,
-            "angularVelocity": -1.7000138541118799,
-            "velocityX": 2.285186365520377,
-            "velocityY": 0.1118756514857201,
-            "timestamp": 0.28055525068306764
-        },
-        {
-            "x": 1.270529175155293,
-            "y": 6.784749415960288,
-            "heading": -2.524162862737573,
-            "angularVelocity": -1.774582788395185,
-            "velocityX": 2.751930575328903,
-            "velocityY": 0.13651501134463712,
-            "timestamp": 0.3366663008196812
-        },
-        {
-            "x": 1.4506438070602947,
-            "y": 6.793909267096616,
-            "heading": -2.6053474913460852,
-            "angularVelocity": -1.4468563395419025,
-            "velocityX": 3.2099672251094336,
-            "velocityY": 0.16324504913117607,
-            "timestamp": 0.39277735095629474
-        },
-        {
-            "x": 1.6524388072374525,
-            "y": 6.806354930258429,
-            "heading": -2.6314264655034,
-            "angularVelocity": -0.46477430192125535,
-            "velocityX": 3.5963504458720204,
-            "velocityY": 0.22180413896261764,
-            "timestamp": 0.4488884010929083
-        },
-        {
-            "x": 1.8644608843714687,
-            "y": 6.816501923570639,
-            "heading": -2.6314265116901563,
-            "angularVelocity": -8.231311961492043e-7,
-            "velocityX": 3.7786153817796047,
-            "velocityY": 0.18083770108638533,
-            "timestamp": 0.5049994512295218
-        },
-        {
-            "x": 2.0764830467529034,
-            "y": 6.826647135513074,
-            "heading": -2.631426557872052,
-            "angularVelocity": -8.230445785018772e-7,
-            "velocityX": 3.7786169010421835,
-            "velocityY": 0.1808059538676366,
-            "timestamp": 0.5611105013661354
-        },
-        {
-            "x": 2.288505209106376,
-            "y": 6.836792348039794,
-            "heading": -2.631426604053948,
-            "angularVelocity": -8.230445836057129e-7,
-            "velocityX": 3.778616900543848,
-            "velocityY": 0.18080596428064635,
-            "timestamp": 0.6172215515027489
-        },
-        {
-            "x": 2.5005273714588676,
-            "y": 6.846937560587016,
-            "heading": -2.631426650235844,
-            "angularVelocity": -8.230445885587366e-7,
-            "velocityX": 3.7786169005263646,
-            "velocityY": 0.18080596464604262,
-            "timestamp": 0.6733326016393625
-        },
-        {
-            "x": 2.7125495338113623,
-            "y": 6.857082773134173,
-            "heading": -2.6314266964177406,
-            "angularVelocity": -8.230445944915301e-7,
-            "velocityX": 3.778616900526421,
-            "velocityY": 0.18080596464485496,
-            "timestamp": 0.729443651775976
-        },
-        {
-            "x": 2.9245716961638615,
-            "y": 6.867227985681239,
-            "heading": -2.631426742599637,
-            "angularVelocity": -8.230445986306635e-7,
-            "velocityX": 3.7786169005264982,
-            "velocityY": 0.18080596464325174,
-            "timestamp": 0.7855547019125896
-        },
-        {
-            "x": 3.1365938585163606,
-            "y": 6.877373198228301,
-            "heading": -2.631426788781534,
-            "angularVelocity": -8.230446041948432e-7,
-            "velocityX": 3.7786169005265,
-            "velocityY": 0.18080596464320411,
-            "timestamp": 0.8416657520492031
-        },
-        {
-            "x": 3.34861602086886,
-            "y": 6.887518410775364,
-            "heading": -2.6314268349634315,
-            "angularVelocity": -8.230446102544191e-7,
-            "velocityX": 3.7786169005265,
-            "velocityY": 0.18080596464319879,
-            "timestamp": 0.8977768021858167
-        },
-        {
-            "x": 3.560638183221359,
-            "y": 6.897663623322427,
-            "heading": -2.631426881145329,
-            "angularVelocity": -8.230446143262805e-7,
-            "velocityX": 3.7786169005264996,
-            "velocityY": 0.1808059646431973,
-            "timestamp": 0.9538878523224302
-        },
-        {
-            "x": 3.772660345573858,
-            "y": 6.90780883586949,
-            "heading": -2.631426927327227,
-            "angularVelocity": -8.230446193740569e-7,
-            "velocityX": 3.778616900526499,
-            "velocityY": 0.1808059646432043,
-            "timestamp": 1.0099989024590437
-        },
-        {
-            "x": 3.984682507926357,
-            "y": 6.917954048416554,
-            "heading": -2.631426973509125,
-            "angularVelocity": -8.230446249324534e-7,
-            "velocityX": 3.778616900526498,
-            "velocityY": 0.18080596464321827,
-            "timestamp": 1.0661099525956572
-        },
-        {
-            "x": 4.196704670278859,
-            "y": 6.928099260963566,
-            "heading": -2.6314270196910234,
-            "angularVelocity": -8.230446298795515e-7,
-            "velocityX": 3.7786169005265413,
-            "velocityY": 0.18080596464229606,
-            "timestamp": 1.1222210027322708
-        },
-        {
-            "x": 4.40872683263124,
-            "y": 6.938244473513073,
-            "heading": -2.631427065872922,
-            "angularVelocity": -8.230446358036557e-7,
-            "velocityX": 3.778616900524414,
-            "velocityY": 0.18080596468676038,
-            "timestamp": 1.1783320528688843
-        },
-        {
-            "x": 4.620748994963782,
-            "y": 6.948389686477181,
-            "heading": -2.6314271120548214,
-            "angularVelocity": -8.230446409621988e-7,
-            "velocityX": 3.778616900170846,
-            "velocityY": 0.18080597207568838,
-            "timestamp": 1.2344431030054979
-        },
-        {
-            "x": 4.832771157387297,
-            "y": 6.958534897538403,
-            "heading": -2.631427158236721,
-            "angularVelocity": -8.230446451889797e-7,
-            "velocityX": 3.778616901792118,
-            "velocityY": 0.18080593816277588,
-            "timestamp": 1.2905541531421114
-        },
-        {
-            "x": 5.044793317425079,
-            "y": 6.968680158463288,
-            "heading": -2.6314272044186224,
-            "angularVelocity": -8.230446869472616e-7,
-            "velocityX": 3.7786168592740705,
-            "velocityY": 0.18080682682325677,
-            "timestamp": 1.346665203278725
-        },
-        {
-            "x": 5.256813933209308,
-            "y": 6.978857641482405,
-            "heading": -2.6314272506013574,
-            "angularVelocity": -8.230595289243604e-7,
-            "velocityX": 3.778589337893981,
-            "velocityY": 0.18138108258631616,
-            "timestamp": 1.4027762534153385
-        },
-        {
-            "x": 5.469038653323493,
-            "y": 6.974735659993926,
-            "heading": -2.6314272968552426,
-            "angularVelocity": -8.243275644517052e-7,
-            "velocityX": 3.7822268447565213,
-            "velocityY": -0.07346113606385067,
-            "timestamp": 1.458887303551952
-        },
-        {
-            "x": 5.679131625937882,
-            "y": 6.944449281942863,
-            "heading": -2.631427345537303,
-            "angularVelocity": -8.676020260335946e-7,
-            "velocityX": 3.7442352638718064,
-            "velocityY": -0.5397578191342853,
-            "timestamp": 1.5149983536885656
-        },
-        {
-            "x": 5.8838793004435885,
-            "y": 6.888460800908395,
-            "heading": -2.6314274009810084,
-            "angularVelocity": -9.881067120494687e-7,
-            "velocityX": 3.648972421781526,
-            "velocityY": -0.9978155977862404,
-            "timestamp": 1.5711094038251792
-        },
-        {
-            "x": 6.080149873036162,
-            "y": 6.807626528914084,
-            "heading": -2.6314274705096907,
-            "angularVelocity": -0.0000012391263740452789,
-            "velocityX": 3.497895193811386,
-            "velocityY": -1.440612353493913,
-            "timestamp": 1.6272204539617927
-        },
-        {
-            "x": 6.264941198917061,
-            "y": 6.703182873189846,
-            "heading": -2.631427570246947,
-            "angularVelocity": -0.0000017774975926495223,
-            "velocityX": 3.2933143370331135,
-            "velocityY": -1.8613741049215575,
-            "timestamp": 1.6833315040984063
-        },
-        {
-            "x": 6.435426712036133,
-            "y": 6.576727390289307,
-            "heading": -2.6314277398800354,
-            "angularVelocity": -0.0000030231672442365436,
-            "velocityX": 3.0383589810560285,
-            "velocityY": -2.253664520494037,
-            "timestamp": 1.7394425542350198
-        },
-        {
-            "x": 6.574927462247643,
-            "y": 6.446962020265696,
-            "heading": -2.631427921819672,
-            "angularVelocity": -0.0000036124868348171132,
-            "velocityX": 2.7698451702830824,
-            "velocityY": -2.5765451647028184,
-            "timestamp": 1.7898066510113226
-        },
-        {
-            "x": 6.6991857061041244,
-            "y": 6.302534147821866,
-            "heading": -2.631428035463349,
-            "angularVelocity": -0.0000022564422659644148,
-            "velocityX": 2.4671988938546927,
-            "velocityY": -2.867675222794474,
-            "timestamp": 1.8401707477876255
-        },
-        {
-            "x": 6.806670192415463,
-            "y": 6.145223588911882,
-            "heading": -2.631428117085094,
-            "angularVelocity": -0.0000016206335538188846,
-            "velocityX": 2.134148990872305,
-            "velocityY": -3.123466297999793,
-            "timestamp": 1.8905348445639283
-        },
-        {
-            "x": 6.896056392277072,
-            "y": 5.976968910136802,
-            "heading": -2.631428181121382,
-            "angularVelocity": -0.000001271467013700371,
-            "velocityX": 1.7748000179300458,
-            "velocityY": -3.340766330475413,
-            "timestamp": 1.940898941340231
-        },
-        {
-            "x": 6.966242833371282,
-            "y": 5.799843545392681,
-            "heading": -2.631428234939726,
-            "angularVelocity": -0.0000010685855124139772,
-            "velocityX": 1.393580855940955,
-            "velocityY": -3.5168974742233234,
-            "timestamp": 1.991263038116534
-        },
-        {
-            "x": 7.016364754971619,
-            "y": 5.616030256280146,
-            "heading": -2.6314282829523696,
-            "angularVelocity": -9.53310925320319e-7,
-            "velocityX": 0.9951915115833975,
-            "velocityY": -3.6496889823907637,
-            "timestamp": 2.041627134892837
-        },
-        {
-            "x": 7.046069338416312,
-            "y": 5.427835754816808,
-            "heading": -2.631428328230437,
-            "angularVelocity": -8.990147867657557e-7,
-            "velocityX": 0.5897968065726167,
-            "velocityY": -3.7366797681141786,
-            "timestamp": 2.09199123166914
-        },
-        {
-            "x": 7.072076786928083,
-            "y": 5.239094807157974,
-            "heading": -2.6314283732011274,
-            "angularVelocity": -8.929116824307271e-7,
-            "velocityX": 0.5163886612943162,
-            "velocityY": -3.747529683638862,
-            "timestamp": 2.142355328445443
-        },
-        {
-            "x": 7.098039162505348,
-            "y": 5.0503476542439945,
-            "heading": -2.631428418169392,
-            "angularVelocity": -8.928635140632744e-7,
-            "velocityX": 0.5154937195157807,
-            "velocityY": -3.747652891549277,
-            "timestamp": 2.192719425221746
-        },
-        {
-            "x": 7.124001613673454,
-            "y": 4.861600511727296,
-            "heading": -2.6314284631376603,
-            "angularVelocity": -8.928635936943803e-7,
-            "velocityX": 0.5154952204033956,
-            "velocityY": -3.7476526851069147,
-            "timestamp": 2.243083521998049
-        },
-        {
-            "x": 7.149964068909211,
-            "y": 4.67285336977021,
-            "heading": -2.631428508105929,
-            "angularVelocity": -8.928636032719525e-7,
-            "velocityX": 0.5154953011682646,
-            "velocityY": -3.747652673995615,
-            "timestamp": 2.293447618774352
-        },
-        {
-            "x": 7.175926523495577,
-            "y": 4.484106227723796,
-            "heading": -2.6314285530741985,
-            "angularVelocity": -8.928636081094573e-7,
-            "velocityX": 0.5154952882743342,
-            "velocityY": -3.7476526757692454,
-            "timestamp": 2.3438117155506553
-        },
-        {
-            "x": 7.2018889780951465,
-            "y": 4.295359085679197,
-            "heading": -2.6314285980424676,
-            "angularVelocity": -8.928636141069267e-7,
-            "velocityX": 0.5154952885364941,
-            "velocityY": -3.747652675733191,
-            "timestamp": 2.3941758123269583
-        },
-        {
-            "x": 7.227851432696769,
-            "y": 4.106611943634882,
-            "heading": -2.6314286430107376,
-            "angularVelocity": -8.928636206980889e-7,
-            "velocityX": 0.5154952885772718,
-            "velocityY": -3.7476526757275823,
-            "timestamp": 2.4445399091032614
-        },
-        {
-            "x": 7.253813887298451,
-            "y": 3.9178648015905746,
-            "heading": -2.6314286879790076,
-            "angularVelocity": -8.928636265842637e-7,
-            "velocityX": 0.5154952885784285,
-            "velocityY": -3.747652675727423,
-            "timestamp": 2.4949040058795644
-        },
-        {
-            "x": 7.2797763419001384,
-            "y": 3.729117659546268,
-            "heading": -2.631428732947278,
-            "angularVelocity": -8.928636327298174e-7,
-            "velocityX": 0.5154952885785545,
-            "velocityY": -3.747652675727405,
-            "timestamp": 2.5452681026558674
-        },
-        {
-            "x": 7.30573879650183,
-            "y": 3.5403705175019615,
-            "heading": -2.6314287779155485,
-            "angularVelocity": -8.928636380137632e-7,
-            "velocityX": 0.5154952885786028,
-            "velocityY": -3.747652675727398,
-            "timestamp": 2.5956321994321705
-        },
-        {
-            "x": 7.33170125110352,
-            "y": 3.351623375457655,
-            "heading": -2.63142882288382,
-            "angularVelocity": -8.928636442498648e-7,
-            "velocityX": 0.5154952885786169,
-            "velocityY": -3.7476526757273954,
-            "timestamp": 2.6459962962084735
-        },
-        {
-            "x": 7.357663705705211,
-            "y": 3.162876233413349,
-            "heading": -2.631428867852091,
-            "angularVelocity": -8.92863650023284e-7,
-            "velocityX": 0.5154952885786185,
-            "velocityY": -3.747652675727395,
-            "timestamp": 2.6963603929847766
-        },
-        {
-            "x": 7.3836261603069016,
-            "y": 2.974129091369042,
-            "heading": -2.6314289128203625,
-            "angularVelocity": -8.928636550825091e-7,
-            "velocityX": 0.5154952885786133,
-            "velocityY": -3.7476526757273954,
-            "timestamp": 2.7467244897610796
-        },
-        {
-            "x": 7.409588614908591,
-            "y": 2.785381949324736,
-            "heading": -2.6314289577886343,
-            "angularVelocity": -8.928636615837126e-7,
-            "velocityX": 0.5154952885785878,
-            "velocityY": -3.7476526757273985,
-            "timestamp": 2.7970885865373827
-        },
-        {
-            "x": 7.435551069510267,
-            "y": 2.5966348072804273,
-            "heading": -2.6314290027569065,
-            "angularVelocity": -8.928636688471597e-7,
-            "velocityX": 0.5154952885783288,
-            "velocityY": -3.7476526757274335,
-            "timestamp": 2.8474526833136857
-        },
-        {
-            "x": 7.461513524111708,
-            "y": 2.4078876652360863,
-            "heading": -2.631429047725179,
-            "angularVelocity": -8.928636741117398e-7,
-            "velocityX": 0.5154952885736368,
-            "velocityY": -3.747652675728079,
-            "timestamp": 2.8978167800899888
-        },
-        {
-            "x": 7.487475978714219,
-            "y": 2.219140523191893,
-            "heading": -2.631429092693452,
-            "angularVelocity": -8.928636797177715e-7,
-            "velocityX": 0.5154952885949088,
-            "velocityY": -3.7476526757251514,
-            "timestamp": 2.948180876866292
-        },
-        {
-            "x": 7.513438433344531,
-            "y": 2.030393381151524,
-            "heading": -2.631429137661725,
-            "angularVelocity": -8.928636858438467e-7,
-            "velocityX": 0.5154952891469065,
-            "velocityY": -3.747652675649225,
-            "timestamp": 2.998544973642595
-        },
-        {
-            "x": 7.539400885537171,
-            "y": 1.8416462387758443,
-            "heading": -2.631429182629998,
-            "angularVelocity": -8.928636898911743e-7,
-            "velocityX": 0.5154952407459272,
-            "velocityY": -3.747652682306939,
-            "timestamp": 3.048909070418898
-        },
-        {
-            "x": 7.565366905970307,
-            "y": 1.6528995872554086,
-            "heading": -2.631429227608021,
-            "angularVelocity": -8.930572773269882e-7,
-            "velocityX": 0.5155660896386092,
-            "velocityY": -3.747642936172811,
-            "timestamp": 3.099273167195201
-        },
-        {
-            "x": 7.5925510306015145,
-            "y": 1.4714803587358116,
-            "heading": -2.650528472115119,
-            "angularVelocity": -0.3792234097224001,
-            "velocityX": 0.5397520529745963,
-            "velocityY": -3.6021539178074944,
-            "timestamp": 3.149637263971504
-        },
-        {
-            "x": 7.6161137082008885,
-            "y": 1.3070490604011487,
-            "heading": -2.717467314284565,
-            "angularVelocity": -1.3290984342827263,
-            "velocityX": 0.46784672231967417,
-            "velocityY": -3.2648515283615525,
-            "timestamp": 3.200001360747807
-        },
-        {
-            "x": 7.636512760583371,
-            "y": 1.162828952012704,
-            "heading": -2.804054032355297,
-            "angularVelocity": -1.7192151475547452,
-            "velocityX": 0.4050316334093296,
-            "velocityY": -2.863549981428464,
-            "timestamp": 3.25036545752411
-        },
-        {
-            "x": 7.65384671106135,
-            "y": 1.039535414639525,
-            "heading": -2.8903428318832396,
-            "angularVelocity": -1.7132998515034186,
-            "velocityX": 0.344172765670314,
-            "velocityY": -2.4480442470913055,
-            "timestamp": 3.300729554300413
-        },
-        {
-            "x": 7.668186632114738,
-            "y": 0.9370698417317626,
-            "heading": -2.968979200240837,
-            "angularVelocity": -1.561357661329035,
-            "velocityX": 0.28472507145489134,
-            "velocityY": -2.034496386639724,
-            "timestamp": 3.351093651076716
-        },
-        {
-            "x": 7.6795847479840615,
-            "y": 0.8552933598147935,
-            "heading": -3.035849646004894,
-            "angularVelocity": -1.3277403953270235,
-            "velocityX": 0.22631431116405423,
-            "velocityY": -1.6237059165418328,
-            "timestamp": 3.4014577478530192
-        },
-        {
-            "x": 7.688084380345824,
-            "y": 0.7940870720973938,
-            "heading": -3.088311607549879,
-            "angularVelocity": -1.0416539738218624,
-            "velocityX": 0.16876372070203405,
-            "velocityY": -1.2152761914753087,
-            "timestamp": 3.4518218446293223
-        },
-        {
-            "x": 7.693722705516469,
-            "y": 0.7533557615174019,
-            "heading": -3.124538922216378,
-            "angularVelocity": -0.7193083364009395,
-            "velocityX": 0.11195128139975212,
-            "velocityY": -0.8087370406125588,
-            "timestamp": 3.5021859414056253
-        },
-        {
-            "x": 7.696530342102051,
-            "y": 0.7330225110054016,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.37023169592473243,
-            "velocityX": 0.05574678720146357,
-            "velocityY": -0.40372511001859746,
-            "timestamp": 3.5525500381819284
-        },
-        {
-            "x": 7.696530342102051,
-            "y": 0.7330225110054016,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0,
-            "velocityX": -4.6862983235773636e-32,
-            "velocityY": -2.8797435391634997e-33,
-            "timestamp": 3.6029141349582314
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.traj
deleted file mode 100644
index 79c52dac..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/New Path.traj	
+++ /dev/null
@@ -1,625 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.7328571677207947,
-            "y": 6.758700847625732,
-            "heading": -2.1130989969635645,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7582517080124825,
-            "y": 6.75986365233427,
-            "heading": -2.1364061905138043,
-            "angularVelocity": -0.4153761780165196,
-            "velocityX": 0.4525764573976041,
-            "velocityY": 0.020723274750874442,
-            "timestamp": 0.05611105013661352
-        },
-        {
-            "x": 0.8091370757597145,
-            "y": 6.7622371099463585,
-            "heading": -2.1815821515780387,
-            "angularVelocity": -0.8051170126783445,
-            "velocityX": 0.9068689255207545,
-            "velocityY": 0.04229929053754837,
-            "timestamp": 0.11222210027322704
-        },
-        {
-            "x": 0.8856327770155913,
-            "y": 6.7658721035419465,
-            "heading": -2.246734552492055,
-            "angularVelocity": -1.1611331592509972,
-            "velocityX": 1.3632912068056597,
-            "velocityY": 0.06478213447706996,
-            "timestamp": 0.16833315040984057
-        },
-        {
-            "x": 0.9878912539433133,
-            "y": 6.770811955024787,
-            "heading": -2.329199596325353,
-            "angularVelocity": -1.4696756455728817,
-            "velocityX": 1.8224302820701674,
-            "velocityY": 0.08803705278779066,
-            "timestamp": 0.2244442005464541
-        },
-        {
-            "x": 1.116115460670533,
-            "y": 6.777089415314351,
-            "heading": -2.4245891589263606,
-            "angularVelocity": -1.7000138541118799,
-            "velocityX": 2.285186365520377,
-            "velocityY": 0.1118756514857201,
-            "timestamp": 0.28055525068306764
-        },
-        {
-            "x": 1.270529175155293,
-            "y": 6.784749415960288,
-            "heading": -2.524162862737573,
-            "angularVelocity": -1.774582788395185,
-            "velocityX": 2.751930575328903,
-            "velocityY": 0.13651501134463712,
-            "timestamp": 0.3366663008196812
-        },
-        {
-            "x": 1.4506438070602947,
-            "y": 6.793909267096616,
-            "heading": -2.6053474913460852,
-            "angularVelocity": -1.4468563395419025,
-            "velocityX": 3.2099672251094336,
-            "velocityY": 0.16324504913117607,
-            "timestamp": 0.39277735095629474
-        },
-        {
-            "x": 1.6524388072374525,
-            "y": 6.806354930258429,
-            "heading": -2.6314264655034,
-            "angularVelocity": -0.46477430192125535,
-            "velocityX": 3.5963504458720204,
-            "velocityY": 0.22180413896261764,
-            "timestamp": 0.4488884010929083
-        },
-        {
-            "x": 1.8644608843714687,
-            "y": 6.816501923570639,
-            "heading": -2.6314265116901563,
-            "angularVelocity": -8.231311961492043e-7,
-            "velocityX": 3.7786153817796047,
-            "velocityY": 0.18083770108638533,
-            "timestamp": 0.5049994512295218
-        },
-        {
-            "x": 2.0764830467529034,
-            "y": 6.826647135513074,
-            "heading": -2.631426557872052,
-            "angularVelocity": -8.230445785018772e-7,
-            "velocityX": 3.7786169010421835,
-            "velocityY": 0.1808059538676366,
-            "timestamp": 0.5611105013661354
-        },
-        {
-            "x": 2.288505209106376,
-            "y": 6.836792348039794,
-            "heading": -2.631426604053948,
-            "angularVelocity": -8.230445836057129e-7,
-            "velocityX": 3.778616900543848,
-            "velocityY": 0.18080596428064635,
-            "timestamp": 0.6172215515027489
-        },
-        {
-            "x": 2.5005273714588676,
-            "y": 6.846937560587016,
-            "heading": -2.631426650235844,
-            "angularVelocity": -8.230445885587366e-7,
-            "velocityX": 3.7786169005263646,
-            "velocityY": 0.18080596464604262,
-            "timestamp": 0.6733326016393625
-        },
-        {
-            "x": 2.7125495338113623,
-            "y": 6.857082773134173,
-            "heading": -2.6314266964177406,
-            "angularVelocity": -8.230445944915301e-7,
-            "velocityX": 3.778616900526421,
-            "velocityY": 0.18080596464485496,
-            "timestamp": 0.729443651775976
-        },
-        {
-            "x": 2.9245716961638615,
-            "y": 6.867227985681239,
-            "heading": -2.631426742599637,
-            "angularVelocity": -8.230445986306635e-7,
-            "velocityX": 3.7786169005264982,
-            "velocityY": 0.18080596464325174,
-            "timestamp": 0.7855547019125896
-        },
-        {
-            "x": 3.1365938585163606,
-            "y": 6.877373198228301,
-            "heading": -2.631426788781534,
-            "angularVelocity": -8.230446041948432e-7,
-            "velocityX": 3.7786169005265,
-            "velocityY": 0.18080596464320411,
-            "timestamp": 0.8416657520492031
-        },
-        {
-            "x": 3.34861602086886,
-            "y": 6.887518410775364,
-            "heading": -2.6314268349634315,
-            "angularVelocity": -8.230446102544191e-7,
-            "velocityX": 3.7786169005265,
-            "velocityY": 0.18080596464319879,
-            "timestamp": 0.8977768021858167
-        },
-        {
-            "x": 3.560638183221359,
-            "y": 6.897663623322427,
-            "heading": -2.631426881145329,
-            "angularVelocity": -8.230446143262805e-7,
-            "velocityX": 3.7786169005264996,
-            "velocityY": 0.1808059646431973,
-            "timestamp": 0.9538878523224302
-        },
-        {
-            "x": 3.772660345573858,
-            "y": 6.90780883586949,
-            "heading": -2.631426927327227,
-            "angularVelocity": -8.230446193740569e-7,
-            "velocityX": 3.778616900526499,
-            "velocityY": 0.1808059646432043,
-            "timestamp": 1.0099989024590437
-        },
-        {
-            "x": 3.984682507926357,
-            "y": 6.917954048416554,
-            "heading": -2.631426973509125,
-            "angularVelocity": -8.230446249324534e-7,
-            "velocityX": 3.778616900526498,
-            "velocityY": 0.18080596464321827,
-            "timestamp": 1.0661099525956572
-        },
-        {
-            "x": 4.196704670278859,
-            "y": 6.928099260963566,
-            "heading": -2.6314270196910234,
-            "angularVelocity": -8.230446298795515e-7,
-            "velocityX": 3.7786169005265413,
-            "velocityY": 0.18080596464229606,
-            "timestamp": 1.1222210027322708
-        },
-        {
-            "x": 4.40872683263124,
-            "y": 6.938244473513073,
-            "heading": -2.631427065872922,
-            "angularVelocity": -8.230446358036557e-7,
-            "velocityX": 3.778616900524414,
-            "velocityY": 0.18080596468676038,
-            "timestamp": 1.1783320528688843
-        },
-        {
-            "x": 4.620748994963782,
-            "y": 6.948389686477181,
-            "heading": -2.6314271120548214,
-            "angularVelocity": -8.230446409621988e-7,
-            "velocityX": 3.778616900170846,
-            "velocityY": 0.18080597207568838,
-            "timestamp": 1.2344431030054979
-        },
-        {
-            "x": 4.832771157387297,
-            "y": 6.958534897538403,
-            "heading": -2.631427158236721,
-            "angularVelocity": -8.230446451889797e-7,
-            "velocityX": 3.778616901792118,
-            "velocityY": 0.18080593816277588,
-            "timestamp": 1.2905541531421114
-        },
-        {
-            "x": 5.044793317425079,
-            "y": 6.968680158463288,
-            "heading": -2.6314272044186224,
-            "angularVelocity": -8.230446869472616e-7,
-            "velocityX": 3.7786168592740705,
-            "velocityY": 0.18080682682325677,
-            "timestamp": 1.346665203278725
-        },
-        {
-            "x": 5.256813933209308,
-            "y": 6.978857641482405,
-            "heading": -2.6314272506013574,
-            "angularVelocity": -8.230595289243604e-7,
-            "velocityX": 3.778589337893981,
-            "velocityY": 0.18138108258631616,
-            "timestamp": 1.4027762534153385
-        },
-        {
-            "x": 5.469038653323493,
-            "y": 6.974735659993926,
-            "heading": -2.6314272968552426,
-            "angularVelocity": -8.243275644517052e-7,
-            "velocityX": 3.7822268447565213,
-            "velocityY": -0.07346113606385067,
-            "timestamp": 1.458887303551952
-        },
-        {
-            "x": 5.679131625937882,
-            "y": 6.944449281942863,
-            "heading": -2.631427345537303,
-            "angularVelocity": -8.676020260335946e-7,
-            "velocityX": 3.7442352638718064,
-            "velocityY": -0.5397578191342853,
-            "timestamp": 1.5149983536885656
-        },
-        {
-            "x": 5.8838793004435885,
-            "y": 6.888460800908395,
-            "heading": -2.6314274009810084,
-            "angularVelocity": -9.881067120494687e-7,
-            "velocityX": 3.648972421781526,
-            "velocityY": -0.9978155977862404,
-            "timestamp": 1.5711094038251792
-        },
-        {
-            "x": 6.080149873036162,
-            "y": 6.807626528914084,
-            "heading": -2.6314274705096907,
-            "angularVelocity": -0.0000012391263740452789,
-            "velocityX": 3.497895193811386,
-            "velocityY": -1.440612353493913,
-            "timestamp": 1.6272204539617927
-        },
-        {
-            "x": 6.264941198917061,
-            "y": 6.703182873189846,
-            "heading": -2.631427570246947,
-            "angularVelocity": -0.0000017774975926495223,
-            "velocityX": 3.2933143370331135,
-            "velocityY": -1.8613741049215575,
-            "timestamp": 1.6833315040984063
-        },
-        {
-            "x": 6.435426712036133,
-            "y": 6.576727390289307,
-            "heading": -2.6314277398800354,
-            "angularVelocity": -0.0000030231672442365436,
-            "velocityX": 3.0383589810560285,
-            "velocityY": -2.253664520494037,
-            "timestamp": 1.7394425542350198
-        },
-        {
-            "x": 6.574927462247643,
-            "y": 6.446962020265696,
-            "heading": -2.631427921819672,
-            "angularVelocity": -0.0000036124868348171132,
-            "velocityX": 2.7698451702830824,
-            "velocityY": -2.5765451647028184,
-            "timestamp": 1.7898066510113226
-        },
-        {
-            "x": 6.6991857061041244,
-            "y": 6.302534147821866,
-            "heading": -2.631428035463349,
-            "angularVelocity": -0.0000022564422659644148,
-            "velocityX": 2.4671988938546927,
-            "velocityY": -2.867675222794474,
-            "timestamp": 1.8401707477876255
-        },
-        {
-            "x": 6.806670192415463,
-            "y": 6.145223588911882,
-            "heading": -2.631428117085094,
-            "angularVelocity": -0.0000016206335538188846,
-            "velocityX": 2.134148990872305,
-            "velocityY": -3.123466297999793,
-            "timestamp": 1.8905348445639283
-        },
-        {
-            "x": 6.896056392277072,
-            "y": 5.976968910136802,
-            "heading": -2.631428181121382,
-            "angularVelocity": -0.000001271467013700371,
-            "velocityX": 1.7748000179300458,
-            "velocityY": -3.340766330475413,
-            "timestamp": 1.940898941340231
-        },
-        {
-            "x": 6.966242833371282,
-            "y": 5.799843545392681,
-            "heading": -2.631428234939726,
-            "angularVelocity": -0.0000010685855124139772,
-            "velocityX": 1.393580855940955,
-            "velocityY": -3.5168974742233234,
-            "timestamp": 1.991263038116534
-        },
-        {
-            "x": 7.016364754971619,
-            "y": 5.616030256280146,
-            "heading": -2.6314282829523696,
-            "angularVelocity": -9.53310925320319e-7,
-            "velocityX": 0.9951915115833975,
-            "velocityY": -3.6496889823907637,
-            "timestamp": 2.041627134892837
-        },
-        {
-            "x": 7.046069338416312,
-            "y": 5.427835754816808,
-            "heading": -2.631428328230437,
-            "angularVelocity": -8.990147867657557e-7,
-            "velocityX": 0.5897968065726167,
-            "velocityY": -3.7366797681141786,
-            "timestamp": 2.09199123166914
-        },
-        {
-            "x": 7.072076786928083,
-            "y": 5.239094807157974,
-            "heading": -2.6314283732011274,
-            "angularVelocity": -8.929116824307271e-7,
-            "velocityX": 0.5163886612943162,
-            "velocityY": -3.747529683638862,
-            "timestamp": 2.142355328445443
-        },
-        {
-            "x": 7.098039162505348,
-            "y": 5.0503476542439945,
-            "heading": -2.631428418169392,
-            "angularVelocity": -8.928635140632744e-7,
-            "velocityX": 0.5154937195157807,
-            "velocityY": -3.747652891549277,
-            "timestamp": 2.192719425221746
-        },
-        {
-            "x": 7.124001613673454,
-            "y": 4.861600511727296,
-            "heading": -2.6314284631376603,
-            "angularVelocity": -8.928635936943803e-7,
-            "velocityX": 0.5154952204033956,
-            "velocityY": -3.7476526851069147,
-            "timestamp": 2.243083521998049
-        },
-        {
-            "x": 7.149964068909211,
-            "y": 4.67285336977021,
-            "heading": -2.631428508105929,
-            "angularVelocity": -8.928636032719525e-7,
-            "velocityX": 0.5154953011682646,
-            "velocityY": -3.747652673995615,
-            "timestamp": 2.293447618774352
-        },
-        {
-            "x": 7.175926523495577,
-            "y": 4.484106227723796,
-            "heading": -2.6314285530741985,
-            "angularVelocity": -8.928636081094573e-7,
-            "velocityX": 0.5154952882743342,
-            "velocityY": -3.7476526757692454,
-            "timestamp": 2.3438117155506553
-        },
-        {
-            "x": 7.2018889780951465,
-            "y": 4.295359085679197,
-            "heading": -2.6314285980424676,
-            "angularVelocity": -8.928636141069267e-7,
-            "velocityX": 0.5154952885364941,
-            "velocityY": -3.747652675733191,
-            "timestamp": 2.3941758123269583
-        },
-        {
-            "x": 7.227851432696769,
-            "y": 4.106611943634882,
-            "heading": -2.6314286430107376,
-            "angularVelocity": -8.928636206980889e-7,
-            "velocityX": 0.5154952885772718,
-            "velocityY": -3.7476526757275823,
-            "timestamp": 2.4445399091032614
-        },
-        {
-            "x": 7.253813887298451,
-            "y": 3.9178648015905746,
-            "heading": -2.6314286879790076,
-            "angularVelocity": -8.928636265842637e-7,
-            "velocityX": 0.5154952885784285,
-            "velocityY": -3.747652675727423,
-            "timestamp": 2.4949040058795644
-        },
-        {
-            "x": 7.2797763419001384,
-            "y": 3.729117659546268,
-            "heading": -2.631428732947278,
-            "angularVelocity": -8.928636327298174e-7,
-            "velocityX": 0.5154952885785545,
-            "velocityY": -3.747652675727405,
-            "timestamp": 2.5452681026558674
-        },
-        {
-            "x": 7.30573879650183,
-            "y": 3.5403705175019615,
-            "heading": -2.6314287779155485,
-            "angularVelocity": -8.928636380137632e-7,
-            "velocityX": 0.5154952885786028,
-            "velocityY": -3.747652675727398,
-            "timestamp": 2.5956321994321705
-        },
-        {
-            "x": 7.33170125110352,
-            "y": 3.351623375457655,
-            "heading": -2.63142882288382,
-            "angularVelocity": -8.928636442498648e-7,
-            "velocityX": 0.5154952885786169,
-            "velocityY": -3.7476526757273954,
-            "timestamp": 2.6459962962084735
-        },
-        {
-            "x": 7.357663705705211,
-            "y": 3.162876233413349,
-            "heading": -2.631428867852091,
-            "angularVelocity": -8.92863650023284e-7,
-            "velocityX": 0.5154952885786185,
-            "velocityY": -3.747652675727395,
-            "timestamp": 2.6963603929847766
-        },
-        {
-            "x": 7.3836261603069016,
-            "y": 2.974129091369042,
-            "heading": -2.6314289128203625,
-            "angularVelocity": -8.928636550825091e-7,
-            "velocityX": 0.5154952885786133,
-            "velocityY": -3.7476526757273954,
-            "timestamp": 2.7467244897610796
-        },
-        {
-            "x": 7.409588614908591,
-            "y": 2.785381949324736,
-            "heading": -2.6314289577886343,
-            "angularVelocity": -8.928636615837126e-7,
-            "velocityX": 0.5154952885785878,
-            "velocityY": -3.7476526757273985,
-            "timestamp": 2.7970885865373827
-        },
-        {
-            "x": 7.435551069510267,
-            "y": 2.5966348072804273,
-            "heading": -2.6314290027569065,
-            "angularVelocity": -8.928636688471597e-7,
-            "velocityX": 0.5154952885783288,
-            "velocityY": -3.7476526757274335,
-            "timestamp": 2.8474526833136857
-        },
-        {
-            "x": 7.461513524111708,
-            "y": 2.4078876652360863,
-            "heading": -2.631429047725179,
-            "angularVelocity": -8.928636741117398e-7,
-            "velocityX": 0.5154952885736368,
-            "velocityY": -3.747652675728079,
-            "timestamp": 2.8978167800899888
-        },
-        {
-            "x": 7.487475978714219,
-            "y": 2.219140523191893,
-            "heading": -2.631429092693452,
-            "angularVelocity": -8.928636797177715e-7,
-            "velocityX": 0.5154952885949088,
-            "velocityY": -3.7476526757251514,
-            "timestamp": 2.948180876866292
-        },
-        {
-            "x": 7.513438433344531,
-            "y": 2.030393381151524,
-            "heading": -2.631429137661725,
-            "angularVelocity": -8.928636858438467e-7,
-            "velocityX": 0.5154952891469065,
-            "velocityY": -3.747652675649225,
-            "timestamp": 2.998544973642595
-        },
-        {
-            "x": 7.539400885537171,
-            "y": 1.8416462387758443,
-            "heading": -2.631429182629998,
-            "angularVelocity": -8.928636898911743e-7,
-            "velocityX": 0.5154952407459272,
-            "velocityY": -3.747652682306939,
-            "timestamp": 3.048909070418898
-        },
-        {
-            "x": 7.565366905970307,
-            "y": 1.6528995872554086,
-            "heading": -2.631429227608021,
-            "angularVelocity": -8.930572773269882e-7,
-            "velocityX": 0.5155660896386092,
-            "velocityY": -3.747642936172811,
-            "timestamp": 3.099273167195201
-        },
-        {
-            "x": 7.5925510306015145,
-            "y": 1.4714803587358116,
-            "heading": -2.650528472115119,
-            "angularVelocity": -0.3792234097224001,
-            "velocityX": 0.5397520529745963,
-            "velocityY": -3.6021539178074944,
-            "timestamp": 3.149637263971504
-        },
-        {
-            "x": 7.6161137082008885,
-            "y": 1.3070490604011487,
-            "heading": -2.717467314284565,
-            "angularVelocity": -1.3290984342827263,
-            "velocityX": 0.46784672231967417,
-            "velocityY": -3.2648515283615525,
-            "timestamp": 3.200001360747807
-        },
-        {
-            "x": 7.636512760583371,
-            "y": 1.162828952012704,
-            "heading": -2.804054032355297,
-            "angularVelocity": -1.7192151475547452,
-            "velocityX": 0.4050316334093296,
-            "velocityY": -2.863549981428464,
-            "timestamp": 3.25036545752411
-        },
-        {
-            "x": 7.65384671106135,
-            "y": 1.039535414639525,
-            "heading": -2.8903428318832396,
-            "angularVelocity": -1.7132998515034186,
-            "velocityX": 0.344172765670314,
-            "velocityY": -2.4480442470913055,
-            "timestamp": 3.300729554300413
-        },
-        {
-            "x": 7.668186632114738,
-            "y": 0.9370698417317626,
-            "heading": -2.968979200240837,
-            "angularVelocity": -1.561357661329035,
-            "velocityX": 0.28472507145489134,
-            "velocityY": -2.034496386639724,
-            "timestamp": 3.351093651076716
-        },
-        {
-            "x": 7.6795847479840615,
-            "y": 0.8552933598147935,
-            "heading": -3.035849646004894,
-            "angularVelocity": -1.3277403953270235,
-            "velocityX": 0.22631431116405423,
-            "velocityY": -1.6237059165418328,
-            "timestamp": 3.4014577478530192
-        },
-        {
-            "x": 7.688084380345824,
-            "y": 0.7940870720973938,
-            "heading": -3.088311607549879,
-            "angularVelocity": -1.0416539738218624,
-            "velocityX": 0.16876372070203405,
-            "velocityY": -1.2152761914753087,
-            "timestamp": 3.4518218446293223
-        },
-        {
-            "x": 7.693722705516469,
-            "y": 0.7533557615174019,
-            "heading": -3.124538922216378,
-            "angularVelocity": -0.7193083364009395,
-            "velocityX": 0.11195128139975212,
-            "velocityY": -0.8087370406125588,
-            "timestamp": 3.5021859414056253
-        },
-        {
-            "x": 7.696530342102051,
-            "y": 0.7330225110054016,
-            "heading": -3.143185307179586,
-            "angularVelocity": -0.37023169592473243,
-            "velocityX": 0.05574678720146357,
-            "velocityY": -0.40372511001859746,
-            "timestamp": 3.5525500381819284
-        },
-        {
-            "x": 7.696530342102051,
-            "y": 0.7330225110054016,
-            "heading": -3.143185307179586,
-            "angularVelocity": 0,
-            "velocityX": -4.6862983235773636e-32,
-            "velocityY": -2.8797435391634997e-33,
-            "timestamp": 3.6029141349582314
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/dos.txt b/src/main/java/frc/robot/utils/dynamicauto/dos.txt
new file mode 100644
index 00000000..38dd1058
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/dos.txt
@@ -0,0 +1 @@
+['Amp Side To W1', 'Amp Side To W2', 'Amp Side To W3', 'Amp Side To C1', 'Amp Side To C2', 'Amp Side To C3', 'Amp Side To C4', 'Amp Side To C5', 'Center To W1', 'Center To W2', 'Center To W3', 'Center To C1', 'Center To C2', 'Center To C3', 'Center To C4', 'Center To C5', 'Stage Side To W1', 'Stage Side To W2', 'Stage Side To W3', 'Stage Side To C1', 'Stage Side To C2', 'Stage Side To C3', 'Stage Side To C4', 'Stage Side To C5', 'W1 To W2', 'W1 To W3', 'W1 To S1', 'W1 To S2', 'W1 To S3', 'W1 To S4', 'W1 To C1', 'W1 To C2', 'W1 To C3', 'W1 To C4', 'W1 To C5', 'W2 To W1', 'W2 To W3', 'W2 To S1', 'W2 To S2', 'W2 To S3', 'W2 To S4', 'W2 To C1', 'W2 To C2', 'W2 To C3', 'W2 To C4', 'W2 To C5', 'W3 To W1', 'W3 To W2', 'W3 To S1', 'W3 To S2', 'W3 To S3', 'W3 To S4', 'W3 To C1', 'W3 To C2', 'W3 To C3', 'W3 To C4', 'W3 To C5', 'S1 To W1', 'S1 To W2', 'S1 To W3', 'S1 To C1', 'S1 To C2', 'S1 To C3', 'S1 To C4', 'S1 To C5', 'S2 To W1', 'S2 To W2', 'S2 To W3', 'S2 To C1', 'S2 To C2', 'S2 To C3', 'S2 To C4', 'S2 To C5', 'S3 To W1', 'S3 To W2', 'S3 To W3', 'S3 To C1', 'S3 To C2', 'S3 To C3', 'S3 To C4', 'S3 To C5', 'S4 To W1', 'S4 To W2', 'S4 To W3', 'S4 To C1', 'S4 To C2', 'S4 To C3', 'S4 To C4', 'S4 To C5', 'C1 To W1', 'C1 To W2', 'C1 To W3', 'C1 To S1', 'C1 To S2', 'C1 To S3', 'C1 To S4', 'C1 To C2', 'C1 To C3', 'C1 To C4', 'C1 To C5', 'C2 To W1', 'C2 To W2', 'C2 To W3', 'C2 To S1', 'C2 To S2', 'C2 To S3', 'C2 To S4', 'C2 To C1', 'C2 To C3', 'C2 To C4', 'C2 To C5', 'C3 To W1', 'C3 To W2', 'C3 To W3', 'C3 To S1', 'C3 To S2', 'C3 To S3', 'C3 To S4', 'C3 To C1', 'C3 To C2', 'C3 To C4', 'C3 To C5', 'C4 To W1', 'C4 To W2', 'C4 To W3', 'C4 To S3', 'C4 To S4', 'C4 To C1', 'C4 To C2', 'C4 To C3', 'C4 To C5', 'C5 To W1', 'C5 To W2', 'C5 To W3', 'C5 To S3', 'C5 To S4', 'C5 To C1', 'C5 To C2', 'C5 To C3', 'C5 To C4']
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/uno.txt b/src/main/java/frc/robot/utils/dynamicauto/uno.txt
new file mode 100644
index 00000000..d501ae87
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/uno.txt
@@ -0,0 +1 @@
+['S1 To W1', 'C5 To W3', 'W3 To S2', 'S3 To C2', 'C3 To C4', 'C2 To C1', 'W1 To S4', 'C1 To C2', 'S1 To C4', 'C4 To C3', 'S4 To C5', 'S2 To W2', 'W2 To C3', 'Center To C3', 'C3 To W1', 'C2 To S4', 'Center To C2', 'W2 To C2', 'S2 To W3', 'C3 To S1', 'Amp Side To C1', 'C4 To C2', 'S4 To C4', 'Stage Side To C1', 'C1 To C3', 'S1 To C5', 'W3 To W2', 'C5 To S3', 'S4 To W1', 'S3 To C3', 'C3 To C5', 'W3 To S3', 'C5 To W2', 'W1 To C1', 'W1 To S2', 'S1 To C2', 'C1 To C4', 'W1 To W3', 'S4 To C3', 'C4 To C5', 'C2 To W2', 'W2 To C5', 'C2 To S3', 'Center To C5', 'S3 To W1', 'Stage Side To W3', 'C1 To W1', 'W3 To S4', 'C3 To C2', 'S3 To C4', 'C5 To S4', 'W2 To S1', 'S2 To C1', 'Amp Side To W3', 'Amp Side To W2', 'C4 To W1', 'Center To W1', 'S3 To C5', 'W2 To W1', 'C1 To S1', 'Stage Side To W2', 'C2 To S2', 'Center To C4', 'W2 To C4', 'C5 To C1', 'C2 To W3', 'W3 To C1', 'S4 To C2', 'W1 To W2', 'S1 To C3', 'C1 To C5', 'W1 To S3', 'S3 To W3', 'C2 To S1', 'C5 To C2', 'C3 To S4', 'Amp Side To C4', 'W3 To C2', 'S4 To C1', 'New Path', 'Stage Side To C4', 'Amp Side To W1', 'C2 To C5', 'S2 To C3', 'W2 To S3', 'C4 To W2', 'Center To W2', 'C4 To S3', 'C1 To W3', 'Stage Side To W1', 'C1 To S2', 'W1 To C4', 'W1 To C5', 'C1 To S3', 'C1 To W2', 'W2 To W3', 'C3 To C1', 'Center To W3', 'C4 To W3', 'W2 To S2', 'C2 To C4', 'S2 To C2', 'W1 To S1', 'S1 To C1', 'Stage Side To C5', 'W3 To C3', 'Amp Side To C5', 'C2 To W1', 'C5 To C3', 'S3 To W2', 'S2 To C5', 'C2 To C3', 'W3 To W1', 'S4 To W2', 'C5 To W1', 'S1 To W3', 'C1 To S4', 'W1 To C2', 'C3 To W3', 'Center To C1', 'W2 To C1', 'C5 To C4', 'Amp Side To C2', 'C3 To S2', 'W3 To C4', 'C4 To C1', 'Stage Side To C2', 'Stage Side To C3', 'W3 To C5', 'Amp Side To C3', 'C3 To S3', 'S2 To W1', 'C3 To W2', 'W1 To C3', 'S1 To W2', 'C4 To S4', 'W3 To S1', 'S3 To C1', 'S4 To W3', 'W2 To S4', 'S2 To C4']
\ No newline at end of file

From 75fb24ef890f8ff83f9c9d4fd52e21a2a61feabb Mon Sep 17 00:00:00 2001
From: Awesomeplayer165 <dev@jacob2.dev>
Date: Sun, 18 Feb 2024 22:37:26 -0800
Subject: [PATCH 41/62] remove uno and dos debug artifact files

---
 src/main/java/frc/robot/utils/dynamicauto/dos.txt | 1 -
 src/main/java/frc/robot/utils/dynamicauto/uno.txt | 1 -
 2 files changed, 2 deletions(-)
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/dos.txt
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/uno.txt

diff --git a/src/main/java/frc/robot/utils/dynamicauto/dos.txt b/src/main/java/frc/robot/utils/dynamicauto/dos.txt
deleted file mode 100644
index 38dd1058..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/dos.txt
+++ /dev/null
@@ -1 +0,0 @@
-['Amp Side To W1', 'Amp Side To W2', 'Amp Side To W3', 'Amp Side To C1', 'Amp Side To C2', 'Amp Side To C3', 'Amp Side To C4', 'Amp Side To C5', 'Center To W1', 'Center To W2', 'Center To W3', 'Center To C1', 'Center To C2', 'Center To C3', 'Center To C4', 'Center To C5', 'Stage Side To W1', 'Stage Side To W2', 'Stage Side To W3', 'Stage Side To C1', 'Stage Side To C2', 'Stage Side To C3', 'Stage Side To C4', 'Stage Side To C5', 'W1 To W2', 'W1 To W3', 'W1 To S1', 'W1 To S2', 'W1 To S3', 'W1 To S4', 'W1 To C1', 'W1 To C2', 'W1 To C3', 'W1 To C4', 'W1 To C5', 'W2 To W1', 'W2 To W3', 'W2 To S1', 'W2 To S2', 'W2 To S3', 'W2 To S4', 'W2 To C1', 'W2 To C2', 'W2 To C3', 'W2 To C4', 'W2 To C5', 'W3 To W1', 'W3 To W2', 'W3 To S1', 'W3 To S2', 'W3 To S3', 'W3 To S4', 'W3 To C1', 'W3 To C2', 'W3 To C3', 'W3 To C4', 'W3 To C5', 'S1 To W1', 'S1 To W2', 'S1 To W3', 'S1 To C1', 'S1 To C2', 'S1 To C3', 'S1 To C4', 'S1 To C5', 'S2 To W1', 'S2 To W2', 'S2 To W3', 'S2 To C1', 'S2 To C2', 'S2 To C3', 'S2 To C4', 'S2 To C5', 'S3 To W1', 'S3 To W2', 'S3 To W3', 'S3 To C1', 'S3 To C2', 'S3 To C3', 'S3 To C4', 'S3 To C5', 'S4 To W1', 'S4 To W2', 'S4 To W3', 'S4 To C1', 'S4 To C2', 'S4 To C3', 'S4 To C4', 'S4 To C5', 'C1 To W1', 'C1 To W2', 'C1 To W3', 'C1 To S1', 'C1 To S2', 'C1 To S3', 'C1 To S4', 'C1 To C2', 'C1 To C3', 'C1 To C4', 'C1 To C5', 'C2 To W1', 'C2 To W2', 'C2 To W3', 'C2 To S1', 'C2 To S2', 'C2 To S3', 'C2 To S4', 'C2 To C1', 'C2 To C3', 'C2 To C4', 'C2 To C5', 'C3 To W1', 'C3 To W2', 'C3 To W3', 'C3 To S1', 'C3 To S2', 'C3 To S3', 'C3 To S4', 'C3 To C1', 'C3 To C2', 'C3 To C4', 'C3 To C5', 'C4 To W1', 'C4 To W2', 'C4 To W3', 'C4 To S3', 'C4 To S4', 'C4 To C1', 'C4 To C2', 'C4 To C3', 'C4 To C5', 'C5 To W1', 'C5 To W2', 'C5 To W3', 'C5 To S3', 'C5 To S4', 'C5 To C1', 'C5 To C2', 'C5 To C3', 'C5 To C4']
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/uno.txt b/src/main/java/frc/robot/utils/dynamicauto/uno.txt
deleted file mode 100644
index d501ae87..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/uno.txt
+++ /dev/null
@@ -1 +0,0 @@
-['S1 To W1', 'C5 To W3', 'W3 To S2', 'S3 To C2', 'C3 To C4', 'C2 To C1', 'W1 To S4', 'C1 To C2', 'S1 To C4', 'C4 To C3', 'S4 To C5', 'S2 To W2', 'W2 To C3', 'Center To C3', 'C3 To W1', 'C2 To S4', 'Center To C2', 'W2 To C2', 'S2 To W3', 'C3 To S1', 'Amp Side To C1', 'C4 To C2', 'S4 To C4', 'Stage Side To C1', 'C1 To C3', 'S1 To C5', 'W3 To W2', 'C5 To S3', 'S4 To W1', 'S3 To C3', 'C3 To C5', 'W3 To S3', 'C5 To W2', 'W1 To C1', 'W1 To S2', 'S1 To C2', 'C1 To C4', 'W1 To W3', 'S4 To C3', 'C4 To C5', 'C2 To W2', 'W2 To C5', 'C2 To S3', 'Center To C5', 'S3 To W1', 'Stage Side To W3', 'C1 To W1', 'W3 To S4', 'C3 To C2', 'S3 To C4', 'C5 To S4', 'W2 To S1', 'S2 To C1', 'Amp Side To W3', 'Amp Side To W2', 'C4 To W1', 'Center To W1', 'S3 To C5', 'W2 To W1', 'C1 To S1', 'Stage Side To W2', 'C2 To S2', 'Center To C4', 'W2 To C4', 'C5 To C1', 'C2 To W3', 'W3 To C1', 'S4 To C2', 'W1 To W2', 'S1 To C3', 'C1 To C5', 'W1 To S3', 'S3 To W3', 'C2 To S1', 'C5 To C2', 'C3 To S4', 'Amp Side To C4', 'W3 To C2', 'S4 To C1', 'New Path', 'Stage Side To C4', 'Amp Side To W1', 'C2 To C5', 'S2 To C3', 'W2 To S3', 'C4 To W2', 'Center To W2', 'C4 To S3', 'C1 To W3', 'Stage Side To W1', 'C1 To S2', 'W1 To C4', 'W1 To C5', 'C1 To S3', 'C1 To W2', 'W2 To W3', 'C3 To C1', 'Center To W3', 'C4 To W3', 'W2 To S2', 'C2 To C4', 'S2 To C2', 'W1 To S1', 'S1 To C1', 'Stage Side To C5', 'W3 To C3', 'Amp Side To C5', 'C2 To W1', 'C5 To C3', 'S3 To W2', 'S2 To C5', 'C2 To C3', 'W3 To W1', 'S4 To W2', 'C5 To W1', 'S1 To W3', 'C1 To S4', 'W1 To C2', 'C3 To W3', 'Center To C1', 'W2 To C1', 'C5 To C4', 'Amp Side To C2', 'C3 To S2', 'W3 To C4', 'C4 To C1', 'Stage Side To C2', 'Stage Side To C3', 'W3 To C5', 'Amp Side To C3', 'C3 To S3', 'S2 To W1', 'C3 To W2', 'W1 To C3', 'S1 To W2', 'C4 To S4', 'W3 To S1', 'S3 To C1', 'S4 To W3', 'W2 To S4', 'S2 To C4']
\ No newline at end of file

From d9e07dfd85d5932abec73d73eb2642fef0b8ae9b Mon Sep 17 00:00:00 2001
From: Awesomeplayer165 <dev@jacob2.dev>
Date: Sun, 18 Feb 2024 22:52:35 -0800
Subject: [PATCH 42/62] removed segment .1.traj ending files (140/280)

---
 .../utils/dynamicauto/RemoveTrajSplits.py     |   22 +
 .../deploy/choreo/Amp Side To C1.1.traj       |  274 -----
 .../deploy/choreo/Amp Side To C2.1.traj       |  319 -----
 .../deploy/choreo/Amp Side To C3.1.traj       |  526 --------
 .../deploy/choreo/Amp Side To C4.1.traj       |  400 ------
 .../deploy/choreo/Amp Side To C5.1.traj       |  805 ------------
 .../deploy/choreo/Amp Side To W1.1.traj       |  121 --
 .../deploy/choreo/Amp Side To W2.1.traj       |  148 ---
 .../deploy/choreo/Amp Side To W3.1.traj       |  193 ---
 .../dynamicauto/deploy/choreo/C1 To C2.1.traj |   94 --
 .../dynamicauto/deploy/choreo/C1 To C3.1.traj |  139 ---
 .../dynamicauto/deploy/choreo/C1 To C4.1.traj |  175 ---
 .../dynamicauto/deploy/choreo/C1 To C5.1.traj |  220 ----
 .../dynamicauto/deploy/choreo/C1 To S1.1.traj |  130 --
 .../dynamicauto/deploy/choreo/C1 To S2.1.traj |  148 ---
 .../dynamicauto/deploy/choreo/C1 To S3.1.traj |  265 ----
 .../dynamicauto/deploy/choreo/C1 To S4.1.traj |  301 -----
 .../dynamicauto/deploy/choreo/C1 To W1.1.traj |  202 ---
 .../dynamicauto/deploy/choreo/C1 To W2.1.traj |  193 ---
 .../dynamicauto/deploy/choreo/C1 To W3.1.traj |  274 -----
 .../dynamicauto/deploy/choreo/C2 To C1.1.traj |   94 --
 .../dynamicauto/deploy/choreo/C2 To C3.1.traj |   94 --
 .../dynamicauto/deploy/choreo/C2 To C4.1.traj |  139 ---
 .../dynamicauto/deploy/choreo/C2 To C5.1.traj |  175 ---
 .../dynamicauto/deploy/choreo/C2 To S1.1.traj |  130 --
 .../dynamicauto/deploy/choreo/C2 To S2.1.traj |  193 ---
 .../dynamicauto/deploy/choreo/C2 To S3.1.traj |  193 ---
 .../dynamicauto/deploy/choreo/C2 To S4.1.traj |  256 ----
 .../dynamicauto/deploy/choreo/C2 To W1.1.traj |  202 ---
 .../dynamicauto/deploy/choreo/C2 To W2.1.traj |  184 ---
 .../dynamicauto/deploy/choreo/C2 To W3.1.traj |  346 ------
 .../dynamicauto/deploy/choreo/C3 To C1.1.traj |  139 ---
 .../dynamicauto/deploy/choreo/C3 To C2.1.traj |   94 --
 .../dynamicauto/deploy/choreo/C3 To C4.1.traj |   94 --
 .../dynamicauto/deploy/choreo/C3 To C5.1.traj |  139 ---
 .../dynamicauto/deploy/choreo/C3 To S1.1.traj |  157 ---
 .../dynamicauto/deploy/choreo/C3 To S2.1.traj |  148 ---
 .../dynamicauto/deploy/choreo/C3 To S3.1.traj |  166 ---
 .../dynamicauto/deploy/choreo/C3 To S4.1.traj |  238 ----
 .../dynamicauto/deploy/choreo/C3 To W1.1.traj |  220 ----
 .../dynamicauto/deploy/choreo/C3 To W2.1.traj |  193 ---
 .../dynamicauto/deploy/choreo/C3 To W3.1.traj |  202 ---
 .../dynamicauto/deploy/choreo/C4 To C1.1.traj |  175 ---
 .../dynamicauto/deploy/choreo/C4 To C2.1.traj |  139 ---
 .../dynamicauto/deploy/choreo/C4 To C3.1.traj |   94 --
 .../dynamicauto/deploy/choreo/C4 To C5.1.traj |   94 --
 .../dynamicauto/deploy/choreo/C4 To S3.1.traj |  229 ----
 .../dynamicauto/deploy/choreo/C4 To S4.1.traj |  193 ---
 .../dynamicauto/deploy/choreo/C4 To W1.1.traj |  238 ----
 .../dynamicauto/deploy/choreo/C4 To W2.1.traj |  256 ----
 .../dynamicauto/deploy/choreo/C4 To W3.1.traj |  274 -----
 .../dynamicauto/deploy/choreo/C5 To C1.1.traj |  220 ----
 .../dynamicauto/deploy/choreo/C5 To C2.1.traj |  175 ---
 .../dynamicauto/deploy/choreo/C5 To C3.1.traj |  139 ---
 .../dynamicauto/deploy/choreo/C5 To C4.1.traj |   94 --
 .../dynamicauto/deploy/choreo/C5 To S3.1.traj |  247 ----
 .../dynamicauto/deploy/choreo/C5 To S4.1.traj |  148 ---
 .../dynamicauto/deploy/choreo/C5 To W1.1.traj |  310 -----
 .../dynamicauto/deploy/choreo/C5 To W2.1.traj |  283 -----
 .../dynamicauto/deploy/choreo/C5 To W3.1.traj |  292 -----
 .../deploy/choreo/Center To C1.1.traj         |  220 ----
 .../deploy/choreo/Center To C2.1.traj         |  211 ----
 .../deploy/choreo/Center To C3.1.traj         |  211 ----
 .../deploy/choreo/Center To C4.1.traj         | 1093 -----------------
 .../deploy/choreo/Center To C5.1.traj         |  625 ----------
 .../deploy/choreo/Center To W1.1.traj         |  112 --
 .../deploy/choreo/Center To W2.1.traj         |   76 --
 .../deploy/choreo/Center To W3.1.traj         |  112 --
 .../dynamicauto/deploy/choreo/S1 To C1.1.traj |  130 --
 .../dynamicauto/deploy/choreo/S1 To C2.1.traj |  130 --
 .../dynamicauto/deploy/choreo/S1 To C3.1.traj |  157 ---
 .../dynamicauto/deploy/choreo/S1 To C4.1.traj |  247 ----
 .../dynamicauto/deploy/choreo/S1 To C5.1.traj |  292 -----
 .../dynamicauto/deploy/choreo/S1 To W1.1.traj |  130 --
 .../dynamicauto/deploy/choreo/S1 To W2.1.traj |  148 ---
 .../dynamicauto/deploy/choreo/S1 To W3.1.traj |  175 ---
 .../dynamicauto/deploy/choreo/S2 To C1.1.traj |  148 ---
 .../dynamicauto/deploy/choreo/S2 To C2.1.traj |  202 ---
 .../dynamicauto/deploy/choreo/S2 To C3.1.traj |  148 ---
 .../dynamicauto/deploy/choreo/S2 To C4.1.traj |  166 ---
 .../dynamicauto/deploy/choreo/S2 To C5.1.traj |  202 ---
 .../dynamicauto/deploy/choreo/S2 To W1.1.traj |  130 --
 .../dynamicauto/deploy/choreo/S2 To W2.1.traj |  103 --
 .../dynamicauto/deploy/choreo/S2 To W3.1.traj |  130 --
 .../dynamicauto/deploy/choreo/S3 To C1.1.traj |  220 ----
 .../dynamicauto/deploy/choreo/S3 To C2.1.traj |  193 ---
 .../dynamicauto/deploy/choreo/S3 To C3.1.traj |  166 ---
 .../dynamicauto/deploy/choreo/S3 To C4.1.traj |  238 ----
 .../dynamicauto/deploy/choreo/S3 To C5.1.traj |  184 ---
 .../dynamicauto/deploy/choreo/S3 To W1.1.traj |  202 ---
 .../dynamicauto/deploy/choreo/S3 To W2.1.traj |  157 ---
 .../dynamicauto/deploy/choreo/S3 To W3.1.traj |  121 --
 .../dynamicauto/deploy/choreo/S4 To C1.1.traj |  292 -----
 .../dynamicauto/deploy/choreo/S4 To C2.1.traj |  202 ---
 .../dynamicauto/deploy/choreo/S4 To C3.1.traj |  238 ----
 .../dynamicauto/deploy/choreo/S4 To C4.1.traj |  148 ---
 .../dynamicauto/deploy/choreo/S4 To C5.1.traj |  148 ---
 .../dynamicauto/deploy/choreo/S4 To W1.1.traj |  247 ----
 .../dynamicauto/deploy/choreo/S4 To W2.1.traj |  202 ---
 .../dynamicauto/deploy/choreo/S4 To W3.1.traj |  256 ----
 .../deploy/choreo/Stage Side To C1.1.traj     |  616 ----------
 .../deploy/choreo/Stage Side To C2.1.traj     |  607 ---------
 .../deploy/choreo/Stage Side To C3.1.traj     |  274 -----
 .../deploy/choreo/Stage Side To C4.1.traj     |  679 ----------
 .../deploy/choreo/Stage Side To C5.1.traj     |  337 -----
 .../deploy/choreo/Stage Side To W1.1.traj     |  193 ---
 .../deploy/choreo/Stage Side To W2.1.traj     |  148 ---
 .../deploy/choreo/Stage Side To W3.1.traj     |  121 --
 .../dynamicauto/deploy/choreo/W1 To C1.1.traj |  202 ---
 .../dynamicauto/deploy/choreo/W1 To C2.1.traj |  256 ----
 .../dynamicauto/deploy/choreo/W1 To C3.1.traj |  193 ---
 .../dynamicauto/deploy/choreo/W1 To C4.1.traj |  400 ------
 .../dynamicauto/deploy/choreo/W1 To C5.1.traj |  616 ----------
 .../dynamicauto/deploy/choreo/W1 To S1.1.traj |  130 --
 .../dynamicauto/deploy/choreo/W1 To S2.1.traj |  130 --
 .../dynamicauto/deploy/choreo/W1 To S3.1.traj |  544 --------
 .../dynamicauto/deploy/choreo/W1 To S4.1.traj |  562 ---------
 .../dynamicauto/deploy/choreo/W1 To W2.1.traj |  103 --
 .../dynamicauto/deploy/choreo/W1 To W3.1.traj |  148 ---
 .../dynamicauto/deploy/choreo/W2 To C1.1.traj |  193 ---
 .../dynamicauto/deploy/choreo/W2 To C2.1.traj |  526 --------
 .../dynamicauto/deploy/choreo/W2 To C3.1.traj |  571 ---------
 .../dynamicauto/deploy/choreo/W2 To C4.1.traj |  589 ---------
 .../dynamicauto/deploy/choreo/W2 To C5.1.traj |  625 ----------
 .../dynamicauto/deploy/choreo/W2 To S1.1.traj |  148 ---
 .../dynamicauto/deploy/choreo/W2 To S2.1.traj |  103 --
 .../dynamicauto/deploy/choreo/W2 To S3.1.traj |  526 --------
 .../dynamicauto/deploy/choreo/W2 To S4.1.traj |  553 ---------
 .../dynamicauto/deploy/choreo/W2 To W1.1.traj |  103 --
 .../dynamicauto/deploy/choreo/W2 To W3.1.traj |  103 --
 .../dynamicauto/deploy/choreo/W3 To C1.1.traj |  553 ---------
 .../dynamicauto/deploy/choreo/W3 To C2.1.traj |  553 ---------
 .../dynamicauto/deploy/choreo/W3 To C3.1.traj |  310 -----
 .../dynamicauto/deploy/choreo/W3 To C4.1.traj |  625 ----------
 .../dynamicauto/deploy/choreo/W3 To C5.1.traj |  571 ---------
 .../dynamicauto/deploy/choreo/W3 To S1.1.traj |  175 ---
 .../dynamicauto/deploy/choreo/W3 To S2.1.traj |  130 --
 .../dynamicauto/deploy/choreo/W3 To S3.1.traj |  229 ----
 .../dynamicauto/deploy/choreo/W3 To S4.1.traj |  256 ----
 .../dynamicauto/deploy/choreo/W3 To W1.1.traj |  148 ---
 .../dynamicauto/deploy/choreo/W3 To W2.1.traj |  103 --
 141 files changed, 22 insertions(+), 34787 deletions(-)
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/RemoveTrajSplits.py
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj

diff --git a/src/main/java/frc/robot/utils/dynamicauto/RemoveTrajSplits.py b/src/main/java/frc/robot/utils/dynamicauto/RemoveTrajSplits.py
new file mode 100644
index 00000000..f32c0193
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/RemoveTrajSplits.py
@@ -0,0 +1,22 @@
+import json
+import os
+import urllib.parse
+
+folder_path = "src/main/java/frc/robot/utils/dynamicauto/deploy/choreo"
+files_in_folder = os.listdir(folder_path)
+
+print("Files in deploy/choreo folder:")
+print(len(files_in_folder) / 2)
+
+repeatFiles = []
+
+for file in files_in_folder:
+    if file.endswith(".1.traj"):
+        repeatFiles.append(file)
+
+print(f"Removing {len(repeatFiles)} files...")
+
+for file in repeatFiles:
+    os.remove(os.path.join(folder_path, file))
+
+print("Files removed.")
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.1.traj
deleted file mode 100644
index b2886244..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.1.traj	
+++ /dev/null
@@ -1,274 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": 0,
-            "velocityX": -1.8098951472235817e-30,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7613816362208097,
-            "y": 6.725489442192221,
-            "heading": 0.9931061799728784,
-            "angularVelocity": -0.5863053623193526,
-            "velocityX": 0.6424157559264662,
-            "velocityY": 0.06863355110715653,
-            "timestamp": 0.07998190540440472
-        },
-        {
-            "x": 0.8643774617687672,
-            "y": 6.736651681634022,
-            "heading": 0.9029654144342619,
-            "angularVelocity": -1.127014480123304,
-            "velocityX": 1.287739083323781,
-            "velocityY": 0.1395595589447693,
-            "timestamp": 0.15996381080880945
-        },
-        {
-            "x": 1.019343320389449,
-            "y": 6.753669544326843,
-            "heading": 0.7755395878520636,
-            "angularVelocity": -1.5931831823449016,
-            "velocityX": 1.9375114638385158,
-            "velocityY": 0.21277140881771536,
-            "timestamp": 0.23994571621321417
-        },
-        {
-            "x": 1.2269178654194646,
-            "y": 6.7767659053404685,
-            "heading": 0.62538736801047,
-            "angularVelocity": -1.8773273665136312,
-            "velocityX": 2.5952688171215317,
-            "velocityY": 0.2887698273359004,
-            "timestamp": 0.3199276216176189
-        },
-        {
-            "x": 1.4861217663901574,
-            "y": 6.806716511012647,
-            "heading": 0.5166970392894589,
-            "angularVelocity": -1.3589364765874345,
-            "velocityX": 3.240781770077933,
-            "velocityY": 0.37446726882464465,
-            "timestamp": 0.39990952702202365
-        },
-        {
-            "x": 1.7870125383049598,
-            "y": 6.838519066862702,
-            "heading": 0.5166969975980924,
-            "angularVelocity": -5.212599803241117e-7,
-            "velocityX": 3.76198554402321,
-            "velocityY": 0.39762188321538583,
-            "timestamp": 0.4798914324264284
-        },
-        {
-            "x": 2.0879033719171214,
-            "y": 6.870321038982823,
-            "heading": 0.5166969559088411,
-            "angularVelocity": -5.212335340785354e-7,
-            "velocityX": 3.761986315414672,
-            "velocityY": 0.39761458494048757,
-            "timestamp": 0.5598733378308332
-        },
-        {
-            "x": 2.3887942055296523,
-            "y": 6.902123011099449,
-            "heading": 0.5166969142195897,
-            "angularVelocity": -5.212335365150009e-7,
-            "velocityX": 3.7619863154192914,
-            "velocityY": 0.3976145848967806,
-            "timestamp": 0.6398552432352379
-        },
-        {
-            "x": 2.689685039142183,
-            "y": 6.933924983216074,
-            "heading": 0.516696872530338,
-            "angularVelocity": -5.212335398189914e-7,
-            "velocityX": 3.7619863154192914,
-            "velocityY": 0.3976145848967804,
-            "timestamp": 0.7198371486396427
-        },
-        {
-            "x": 2.990575872754714,
-            "y": 6.9657269553327,
-            "heading": 0.5166968308410861,
-            "angularVelocity": -5.212335433606452e-7,
-            "velocityX": 3.7619863154192914,
-            "velocityY": 0.39761458489678053,
-            "timestamp": 0.7998190540440474
-        },
-        {
-            "x": 3.291466706367245,
-            "y": 6.997528927449325,
-            "heading": 0.5166967891518339,
-            "angularVelocity": -5.212335460884143e-7,
-            "velocityX": 3.761986315419291,
-            "velocityY": 0.3976145848967808,
-            "timestamp": 0.8798009594484522
-        },
-        {
-            "x": 3.5923575399797762,
-            "y": 7.0293308995659505,
-            "heading": 0.5166967474625817,
-            "angularVelocity": -5.212335479348436e-7,
-            "velocityX": 3.761986315419291,
-            "velocityY": 0.3976145848967805,
-            "timestamp": 0.9597828648528569
-        },
-        {
-            "x": 3.893248373592307,
-            "y": 7.0611328716825765,
-            "heading": 0.516696705773329,
-            "angularVelocity": -5.212335516712931e-7,
-            "velocityX": 3.7619863154192905,
-            "velocityY": 0.3976145848967809,
-            "timestamp": 1.0397647702572617
-        },
-        {
-            "x": 4.194139207204837,
-            "y": 7.092934843799203,
-            "heading": 0.5166966640840763,
-            "angularVelocity": -5.212335535473894e-7,
-            "velocityX": 3.7619863154192905,
-            "velocityY": 0.3976145848967809,
-            "timestamp": 1.1197466756616663
-        },
-        {
-            "x": 4.495030040817368,
-            "y": 7.1247368159158295,
-            "heading": 0.5166966223948233,
-            "angularVelocity": -5.212335568545237e-7,
-            "velocityX": 3.7619863154192905,
-            "velocityY": 0.3976145848967808,
-            "timestamp": 1.199728581066071
-        },
-        {
-            "x": 4.7959208744299,
-            "y": 7.156538788032456,
-            "heading": 0.51669658070557,
-            "angularVelocity": -5.212335599678648e-7,
-            "velocityX": 3.7619863154192905,
-            "velocityY": 0.397614584896781,
-            "timestamp": 1.2797104864704756
-        },
-        {
-            "x": 5.096811708042431,
-            "y": 7.1883407601490825,
-            "heading": 0.5166965390163165,
-            "angularVelocity": -5.212335628762146e-7,
-            "velocityX": 3.7619863154192905,
-            "velocityY": 0.3976145848967809,
-            "timestamp": 1.3596923918748802
-        },
-        {
-            "x": 5.3977025416549615,
-            "y": 7.220142732265709,
-            "heading": 0.5166964973270628,
-            "angularVelocity": -5.212335655841188e-7,
-            "velocityX": 3.76198631541929,
-            "velocityY": 0.3976145848967813,
-            "timestamp": 1.4396742972792849
-        },
-        {
-            "x": 5.698593375267492,
-            "y": 7.251944704382335,
-            "heading": 0.5166964556378089,
-            "angularVelocity": -5.212335680260379e-7,
-            "velocityX": 3.7619863154192896,
-            "velocityY": 0.3976145848967809,
-            "timestamp": 1.5196562026836895
-        },
-        {
-            "x": 5.999484208880023,
-            "y": 7.283746676498962,
-            "heading": 0.5166964139485547,
-            "angularVelocity": -5.212335712969897e-7,
-            "velocityX": 3.7619863154192896,
-            "velocityY": 0.3976145848967812,
-            "timestamp": 1.5996381080880941
-        },
-        {
-            "x": 6.300375042492554,
-            "y": 7.315548648615588,
-            "heading": 0.5166963722593003,
-            "angularVelocity": -5.21233573548864e-7,
-            "velocityX": 3.7619863154192896,
-            "velocityY": 0.39761458489678125,
-            "timestamp": 1.6796200134924988
-        },
-        {
-            "x": 6.6012658761048035,
-            "y": 7.347350620734872,
-            "heading": 0.5166963305700458,
-            "angularVelocity": -5.212335762453642e-7,
-            "velocityX": 3.7619863154157773,
-            "velocityY": 0.3976145849300086,
-            "timestamp": 1.7596019188969034
-        },
-        {
-            "x": 6.902156662813089,
-            "y": 7.3791530366221005,
-            "heading": 0.5166962888792155,
-            "angularVelocity": -5.212532766918364e-7,
-            "velocityX": 3.761985728983584,
-            "velocityY": 0.3976201332842598,
-            "timestamp": 1.839583824301308
-        },
-        {
-            "x": 7.162091022778527,
-            "y": 7.409610892063227,
-            "heading": 0.4051307797997224,
-            "angularVelocity": -1.3948843618490365,
-            "velocityX": 3.249914573191993,
-            "velocityY": 0.3808093253983604,
-            "timestamp": 1.9195657297057127
-        },
-        {
-            "x": 7.370036606479622,
-            "y": 7.432974685540162,
-            "heading": 0.25885269617169443,
-            "angularVelocity": -1.8288897080960538,
-            "velocityX": 2.5999078497777726,
-            "velocityY": 0.2921134894049214,
-            "timestamp": 1.9995476351101173
-        },
-        {
-            "x": 7.525326975450784,
-            "y": 7.45013556763307,
-            "heading": 0.1335302309738367,
-            "angularVelocity": -1.5668852169025227,
-            "velocityX": 1.9415687609088876,
-            "velocityY": 0.21455955576625652,
-            "timestamp": 2.079529540514522
-        },
-        {
-            "x": 7.628525640862077,
-            "y": 7.461415071089343,
-            "heading": 0.043910704401711645,
-            "angularVelocity": -1.1204975190199653,
-            "velocityX": 1.2902751552304177,
-            "velocityY": 0.14102569073894916,
-            "timestamp": 2.159511445918927
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -0.003,
-            "angularVelocity": -0.5865164647493907,
-            "velocityX": 0.6435750546033896,
-            "velocityY": 0.0698274051164111,
-            "timestamp": 2.2394933513233317
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -0.003,
-            "angularVelocity": 0,
-            "velocityX": 4.312091780947568e-36,
-            "velocityY": 0,
-            "timestamp": 2.3194752567277366
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.1.traj
deleted file mode 100644
index 8c7890ab..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.1.traj	
+++ /dev/null
@@ -1,319 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": 0,
-            "velocityX": 2.674890564447798e-33,
-            "velocityY": 3.559444452002106e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 0.744719766454751,
-            "y": 6.720759968199705,
-            "heading": 1.006760857121406,
-            "angularVelocity": -0.5061419234074507,
-            "velocityX": 0.5286878015432446,
-            "velocityY": 0.011572252862695013,
-            "timestamp": 0.06567158605400746
-        },
-        {
-            "x": 0.8143576636925598,
-            "y": 6.722372451876338,
-            "heading": 0.9431499891720047,
-            "angularVelocity": -0.9686208567749168,
-            "velocityX": 1.0603961533765867,
-            "velocityY": 0.024553749551892837,
-            "timestamp": 0.1313431721080149
-        },
-        {
-            "x": 0.9191612228651693,
-            "y": 6.724923861576486,
-            "heading": 0.8530685118915011,
-            "angularVelocity": -1.3716963864162066,
-            "velocityX": 1.5958737327650472,
-            "velocityY": 0.03885104431723037,
-            "timestamp": 0.19701475816202235
-        },
-        {
-            "x": 1.059454644842809,
-            "y": 6.7284663342548425,
-            "heading": 0.7426650921374723,
-            "angularVelocity": -1.6811444094442096,
-            "velocityX": 2.1362880114128,
-            "velocityY": 0.05394224338464699,
-            "timestamp": 0.2626863442160298
-        },
-        {
-            "x": 1.2356244745863587,
-            "y": 6.733039509062333,
-            "heading": 0.6243118436370656,
-            "angularVelocity": -1.8021987226420155,
-            "velocityX": 2.682588320596835,
-            "velocityY": 0.06963703912619325,
-            "timestamp": 0.3283579302700373
-        },
-        {
-            "x": 1.4470569399809903,
-            "y": 6.738777367450543,
-            "heading": 0.5308220213971302,
-            "angularVelocity": -1.4235962287107038,
-            "velocityX": 3.219542546463745,
-            "velocityY": 0.08737200870294055,
-            "timestamp": 0.39402951632404476
-        },
-        {
-            "x": 1.688226065456379,
-            "y": 6.748371696739962,
-            "heading": 0.5126606987274956,
-            "angularVelocity": -0.2765476480908373,
-            "velocityX": 3.6723511638207493,
-            "velocityY": 0.14609559272058092,
-            "timestamp": 0.45970110237805223
-        },
-        {
-            "x": 1.9365780121804386,
-            "y": 6.754665679712547,
-            "heading": 0.5126606479364623,
-            "angularVelocity": -7.734095747362075e-7,
-            "velocityX": 3.7817260347544686,
-            "velocityY": 0.09584027660688858,
-            "timestamp": 0.5253726884320596
-        },
-        {
-            "x": 2.1849299738494317,
-            "y": 6.760959072962077,
-            "heading": 0.5126605971462416,
-            "angularVelocity": -7.733971982996713e-7,
-            "velocityX": 3.7817262623252414,
-            "velocityY": 0.09583129672545204,
-            "timestamp": 0.5910442744860671
-        },
-        {
-            "x": 2.433281935680659,
-            "y": 6.767252459809602,
-            "heading": 0.5126605463560266,
-            "angularVelocity": -7.733971099299294e-7,
-            "velocityX": 3.7817262647956325,
-            "velocityY": 0.09583119924030911,
-            "timestamp": 0.6567158605400746
-        },
-        {
-            "x": 2.6816338977021794,
-            "y": 6.773545839147862,
-            "heading": 0.5126604955658177,
-            "angularVelocity": -7.733970196193196e-7,
-            "velocityX": 3.78172626769327,
-            "velocityY": 0.09583108489457716,
-            "timestamp": 0.722387446594082
-        },
-        {
-            "x": 2.9299858599438156,
-            "y": 6.779839209799928,
-            "heading": 0.5126604447756148,
-            "angularVelocity": -7.733969286509421e-7,
-            "velocityX": 3.7817262710450463,
-            "velocityY": 0.09583095262742326,
-            "timestamp": 0.7880590326480895
-        },
-        {
-            "x": 3.1783378224424883,
-            "y": 6.786132570308856,
-            "heading": 0.5126603939854177,
-            "angularVelocity": -7.733968385113031e-7,
-            "velocityX": 3.781726274959007,
-            "velocityY": 0.09583079817497217,
-            "timestamp": 0.853730618702097
-        },
-        {
-            "x": 3.426689785244194,
-            "y": 6.792425918859452,
-            "heading": 0.5126603431952265,
-            "angularVelocity": -7.733967515033378e-7,
-            "velocityX": 3.7817262795733857,
-            "velocityY": 0.09583061608201786,
-            "timestamp": 0.9194022047561045
-        },
-        {
-            "x": 3.6750417484067377,
-            "y": 6.798719253170605,
-            "heading": 0.5126602924050404,
-            "angularVelocity": -7.733966712943809e-7,
-            "velocityX": 3.781726285067957,
-            "velocityY": 0.09583039925395942,
-            "timestamp": 0.9850737908101119
-        },
-        {
-            "x": 3.9233937120031053,
-            "y": 6.805012570362013,
-            "heading": 0.5126602416148589,
-            "angularVelocity": -7.733966025007139e-7,
-            "velocityX": 3.7817262916739214,
-            "velocityY": 0.095830138566711,
-            "timestamp": 1.0507453768641193
-        },
-        {
-            "x": 4.171745676125189,
-            "y": 6.811305866807266,
-            "heading": 0.5126601908246804,
-            "angularVelocity": -7.733965561950186e-7,
-            "velocityX": 3.7817262996791623,
-            "velocityY": 0.09582982265909266,
-            "timestamp": 1.1164169629181266
-        },
-        {
-            "x": 4.420097640886595,
-            "y": 6.817599138023107,
-            "heading": 0.5126601400345029,
-            "angularVelocity": -7.733965406200526e-7,
-            "velocityX": 3.781726309414286,
-            "velocityY": 0.09582943848362173,
-            "timestamp": 1.182088548972134
-        },
-        {
-            "x": 4.6684496064150185,
-            "y": 6.8238923789700765,
-            "heading": 0.5126600892443262,
-            "angularVelocity": -7.73396529455525e-7,
-            "velocityX": 3.7817263210938923,
-            "velocityY": 0.09582897757087808,
-            "timestamp": 1.2477601350261414
-        },
-        {
-            "x": 4.916801572841165,
-            "y": 6.830185584489968,
-            "heading": 0.5126600384541553,
-            "angularVelocity": -7.733964406232834e-7,
-            "velocityX": 3.7817263347637717,
-            "velocityY": 0.09582843811261686,
-            "timestamp": 1.3134317210801487
-        },
-        {
-            "x": 5.165153539254683,
-            "y": 6.8364787905062485,
-            "heading": 0.5126599876639959,
-            "angularVelocity": -7.733962639482034e-7,
-            "velocityX": 3.781726334571479,
-            "velocityY": 0.09582844567126618,
-            "timestamp": 1.379103307134156
-        },
-        {
-            "x": 5.413504500375102,
-            "y": 6.842811545538402,
-            "heading": 0.5126599368726894,
-            "angularVelocity": -7.734137320837285e-7,
-            "velocityX": 3.781711026686433,
-            "velocityY": 0.09643066982787928,
-            "timestamp": 1.4447748931881634
-        },
-        {
-            "x": 5.661808131063952,
-            "y": 6.834835924305975,
-            "heading": 0.5126598860199024,
-            "angularVelocity": -7.743499133059843e-7,
-            "velocityX": 3.780990312684674,
-            "velocityY": -0.12144706275568991,
-            "timestamp": 1.5104464792421708
-        },
-        {
-            "x": 5.9063591957092285,
-            "y": 6.791097164154053,
-            "heading": 0.5126598314406843,
-            "angularVelocity": -8.310933457001734e-7,
-            "velocityX": 3.723848917614642,
-            "velocityY": -0.6660225948568639,
-            "timestamp": 1.5761180652961782
-        },
-        {
-            "x": 6.194622556481831,
-            "y": 6.683762426219015,
-            "heading": 0.5126597887806125,
-            "angularVelocity": -5.246475178274693e-7,
-            "velocityX": 3.5451571072929595,
-            "velocityY": -1.3200377183874616,
-            "timestamp": 1.65742993725234
-        },
-        {
-            "x": 6.459547388261047,
-            "y": 6.527457130068993,
-            "heading": 0.5126597482300577,
-            "angularVelocity": -4.987039864896594e-7,
-            "velocityX": 3.2581322432449182,
-            "velocityY": -1.9222936625379825,
-            "timestamp": 1.7387418092085019
-        },
-        {
-            "x": 6.723521508223802,
-            "y": 6.3695515377198895,
-            "heading": 0.5126597076877473,
-            "angularVelocity": -4.98602596336278e-7,
-            "velocityX": 3.2464400783305223,
-            "velocityY": -1.9419746286809643,
-            "timestamp": 1.8200536811646637
-        },
-        {
-            "x": 6.987495332330779,
-            "y": 6.211645450805702,
-            "heading": 0.5126596671480953,
-            "angularVelocity": -4.985699012832353e-7,
-            "velocityX": 3.246436439802837,
-            "velocityY": -1.9419807109984206,
-            "timestamp": 1.9013655531208256
-        },
-        {
-            "x": 7.218138260463332,
-            "y": 6.072816355413413,
-            "heading": 0.4174347514218684,
-            "angularVelocity": -1.1711071635093024,
-            "velocityX": 2.8365221779297074,
-            "velocityY": -1.7073656288101424,
-            "timestamp": 1.9826774250769874
-        },
-        {
-            "x": 7.40359391202109,
-            "y": 5.961763553999033,
-            "heading": 0.26911254592387807,
-            "angularVelocity": -1.8241150022712385,
-            "velocityX": 2.2807942689821186,
-            "velocityY": -1.365763679308134,
-            "timestamp": 2.063989297033149
-        },
-        {
-            "x": 7.542069024042313,
-            "y": 5.879081457057784,
-            "heading": 0.13934312775190755,
-            "angularVelocity": -1.595946754760921,
-            "velocityX": 1.7030122255201257,
-            "velocityY": -1.0168514750927211,
-            "timestamp": 2.145301168989311
-        },
-        {
-            "x": 7.63409233729861,
-            "y": 5.824280880315082,
-            "heading": 0.04592275308495019,
-            "angularVelocity": -1.1489143272623823,
-            "velocityX": 1.1317328090287153,
-            "velocityY": -0.6739554191077514,
-            "timestamp": 2.2266130409454727
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -0.003,
-            "angularVelocity": -0.6016680210157614,
-            "velocityX": 0.5645874531846384,
-            "velocityY": -0.3355091902172136,
-            "timestamp": 2.3079249129016346
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -0.003,
-            "angularVelocity": 3.440382093642737e-30,
-            "velocityX": 0,
-            "velocityY": 2.1122019835026996e-31,
-            "timestamp": 2.3892367848577964
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.1.traj
deleted file mode 100644
index 463b42ef..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.1.traj	
+++ /dev/null
@@ -1,526 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 1.3928609100313025e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 0.729279758485921,
-            "y": 6.711628327586588,
-            "heading": 1.093428142295198,
-            "angularVelocity": 0.9770378814200095,
-            "velocityX": 0.352568020824986,
-            "velocityY": -0.15309237280887605,
-            "timestamp": 0.05468379815278649
-        },
-        {
-            "x": 0.7686493967347447,
-            "y": 6.69501140584797,
-            "heading": 1.1949690599508662,
-            "angularVelocity": 1.8568739020644303,
-            "velocityX": 0.7199506906748694,
-            "velocityY": -0.30387285265355396,
-            "timestamp": 0.10936759630557298
-        },
-        {
-            "x": 0.8290790315170126,
-            "y": 6.670059182269693,
-            "heading": 1.3362954219692313,
-            "angularVelocity": 2.584428419245852,
-            "velocityX": 1.1050738394839994,
-            "velocityY": -0.4563001185206737,
-            "timestamp": 0.16405139445835948
-        },
-        {
-            "x": 0.9115429372168562,
-            "y": 6.636285478537955,
-            "heading": 1.5055888656223098,
-            "angularVelocity": 3.0958611027725738,
-            "velocityX": 1.508013497333151,
-            "velocityY": -0.6176181039468854,
-            "timestamp": 0.21873519261114596
-        },
-        {
-            "x": 1.0168329273736771,
-            "y": 6.593191021214006,
-            "heading": 1.6881423696003794,
-            "angularVelocity": 3.3383471913932383,
-            "velocityX": 1.9254330114861617,
-            "velocityY": -0.7880662788554578,
-            "timestamp": 0.27341899076393245
-        },
-        {
-            "x": 1.1449422871140338,
-            "y": 6.540878714080721,
-            "heading": 1.8564615478628756,
-            "angularVelocity": 3.078044758197163,
-            "velocityX": 2.3427297310698694,
-            "velocityY": -0.9566326572109004,
-            "timestamp": 0.32810278891671896
-        },
-        {
-            "x": 1.2912409780613738,
-            "y": 6.479735868066328,
-            "heading": 1.974688906048384,
-            "angularVelocity": 2.1620180415263293,
-            "velocityX": 2.675357160425865,
-            "velocityY": -1.1181162991560991,
-            "timestamp": 0.38278658706950547
-        },
-        {
-            "x": 1.4553681499836075,
-            "y": 6.408144093279595,
-            "heading": 2.0430197361851268,
-            "angularVelocity": 1.2495626208301445,
-            "velocityX": 3.001385738855639,
-            "velocityY": -1.3091953595963746,
-            "timestamp": 0.437470385222292
-        },
-        {
-            "x": 1.6374055328463037,
-            "y": 6.326368400043195,
-            "heading": 2.0610350683831213,
-            "angularVelocity": 0.3294455178051113,
-            "velocityX": 3.328908909254691,
-            "velocityY": -1.4954281889476506,
-            "timestamp": 0.4921541833750785
-        },
-        {
-            "x": 1.8283093127689483,
-            "y": 6.246687216839916,
-            "heading": 2.061035169590908,
-            "angularVelocity": 0.000001850781943801841,
-            "velocityX": 3.4910482880003957,
-            "velocityY": -1.45712598420193,
-            "timestamp": 0.546837981527865
-        },
-        {
-            "x": 2.01921313853121,
-            "y": 6.167006143460992,
-            "heading": 2.0610352707983473,
-            "angularVelocity": 0.0000018507755948234292,
-            "velocityX": 3.491049126267287,
-            "velocityY": -1.4571239758492094,
-            "timestamp": 0.6015217796806515
-        },
-        {
-            "x": 2.210116959480156,
-            "y": 6.087325058550032,
-            "heading": 2.061035372005735,
-            "angularVelocity": 0.000001850774661522582,
-            "velocityX": 3.4910490382464,
-            "velocityY": -1.4571241867349893,
-            "timestamp": 0.6562055778334379
-        },
-        {
-            "x": 2.40102077504018,
-            "y": 6.007643960727976,
-            "heading": 2.0610354732130722,
-            "angularVelocity": 0.0000018507737318277825,
-            "velocityX": 3.49104893969945,
-            "velocityY": -1.4571244228395928,
-            "timestamp": 0.7108893759862244
-        },
-        {
-            "x": 2.5919245845305627,
-            "y": 5.927962848363926,
-            "heading": 2.0610355744203592,
-            "angularVelocity": 0.0000018507728139380304,
-            "velocityX": 3.491048828704212,
-            "velocityY": -1.457124688768356,
-            "timestamp": 0.7655731741390108
-        },
-        {
-            "x": 2.78282838714305,
-            "y": 5.848281719521431,
-            "heading": 2.0610356756275974,
-            "angularVelocity": 0.0000018507719179726602,
-            "velocityX": 3.4910487029284663,
-            "velocityY": -1.457124990108895,
-            "timestamp": 0.8202569722917973
-        },
-        {
-            "x": 2.973732181912291,
-            "y": 5.768600571887663,
-            "heading": 2.061035776834788,
-            "angularVelocity": 0.0000018507710613284672,
-            "velocityX": 3.491048559499401,
-            "velocityY": -1.4571253337439838,
-            "timestamp": 0.8749407704445837
-        },
-        {
-            "x": 3.16463596766188,
-            "y": 5.688919402644143,
-            "heading": 2.061035878041936,
-            "angularVelocity": 0.000001850770272196717,
-            "velocityX": 3.4910483945574473,
-            "velocityY": -1.4571257289205004,
-            "timestamp": 0.9296245685973702
-        },
-        {
-            "x": 3.3555397425941185,
-            "y": 5.609238207483871,
-            "heading": 2.061035979249042,
-            "angularVelocity": 0.000001850769506382635,
-            "velocityX": 3.4910481967410822,
-            "velocityY": -1.457126202858873,
-            "timestamp": 0.9843083667501566
-        },
-        {
-            "x": 3.546443239175115,
-            "y": 5.529556345440524,
-            "heading": 2.0610360804562036,
-            "angularVelocity": 0.0000018507705174022697,
-            "velocityX": 3.491043106545231,
-            "velocityY": -1.4571383981177848,
-            "timestamp": 1.0389921649029432
-        },
-        {
-            "x": 3.7301891812097523,
-            "y": 5.434525969364101,
-            "heading": 2.0610361860102304,
-            "angularVelocity": 0.0000019302614304565207,
-            "velocityX": 3.360153249071168,
-            "velocityY": -1.7378159397580357,
-            "timestamp": 1.0936759630557298
-        },
-        {
-            "x": 3.9011674048607876,
-            "y": 5.318079465137689,
-            "heading": 2.0610363082632275,
-            "angularVelocity": 0.0000022356347107367405,
-            "velocityX": 3.126670593972313,
-            "velocityY": -2.129451650396711,
-            "timestamp": 1.1483597612085163
-        },
-        {
-            "x": 4.05689386248419,
-            "y": 5.181908226701039,
-            "heading": 2.0610364709885136,
-            "angularVelocity": 0.0000029757495174652245,
-            "velocityX": 2.8477622784778513,
-            "velocityY": -2.490156920998575,
-            "timestamp": 1.203043559361303
-        },
-        {
-            "x": 4.195106185477333,
-            "y": 5.027990436285506,
-            "heading": 2.061036659879246,
-            "angularVelocity": 0.0000034542357869786317,
-            "velocityX": 2.52748213660978,
-            "velocityY": -2.814687267798894,
-            "timestamp": 1.2577273575140895
-        },
-        {
-            "x": 4.313711649853664,
-            "y": 4.858669951905228,
-            "heading": 2.0606108116890636,
-            "angularVelocity": -0.007787465475470693,
-            "velocityX": 2.168932451344132,
-            "velocityY": -3.096355595256869,
-            "timestamp": 1.312411155666876
-        },
-        {
-            "x": 4.40996698448611,
-            "y": 4.67856050613378,
-            "heading": 2.051588761211157,
-            "angularVelocity": -0.16498580535132465,
-            "velocityX": 1.7602166982532683,
-            "velocityY": -3.293652815925874,
-            "timestamp": 1.3670949538196626
-        },
-        {
-            "x": 4.510742037442817,
-            "y": 4.51918682060045,
-            "heading": 1.996778499947209,
-            "angularVelocity": -1.002312624862085,
-            "velocityX": 1.8428685709639585,
-            "velocityY": -2.9144589607334965,
-            "timestamp": 1.4217787519724492
-        },
-        {
-            "x": 4.62612450472694,
-            "y": 4.379617585881877,
-            "heading": 1.9276644376664414,
-            "angularVelocity": -1.263885549567404,
-            "velocityX": 2.1099936577511538,
-            "velocityY": -2.5522959163995464,
-            "timestamp": 1.4764625501252358
-        },
-        {
-            "x": 4.756154634809633,
-            "y": 4.26013511834241,
-            "heading": 1.8517335763640235,
-            "angularVelocity": -1.3885440270675238,
-            "velocityX": 2.3778547664042544,
-            "velocityY": -2.1849701662205963,
-            "timestamp": 1.5311463482780223
-        },
-        {
-            "x": 4.900439739227295,
-            "y": 4.161101341247559,
-            "heading": 1.7737408289368037,
-            "angularVelocity": -1.4262496399629772,
-            "velocityX": 2.638534799915143,
-            "velocityY": -1.8110259426046127,
-            "timestamp": 1.5858301464308089
-        },
-        {
-            "x": 5.008276114908647,
-            "y": 4.1019129343639005,
-            "heading": 1.719841693932476,
-            "angularVelocity": -1.4074809230288232,
-            "velocityX": 2.815956908545699,
-            "velocityY": -1.5456009367595303,
-            "timestamp": 1.6241248999315097
-        },
-        {
-            "x": 5.122676890234505,
-            "y": 4.053024888243358,
-            "heading": 1.6679900254477509,
-            "angularVelocity": -1.354014943163857,
-            "velocityX": 2.987374636678728,
-            "velocityY": -1.276625167978901,
-            "timestamp": 1.6624196534322104
-        },
-        {
-            "x": 5.243382451295884,
-            "y": 4.014566134854959,
-            "heading": 1.6197010617895011,
-            "angularVelocity": -1.260981185252083,
-            "velocityX": 3.1520130051018556,
-            "velocityY": -1.0042825680467018,
-            "timestamp": 1.7007144069329112
-        },
-        {
-            "x": 5.370099790784461,
-            "y": 3.9866453379407707,
-            "heading": 1.576858310766692,
-            "angularVelocity": -1.1187629402556432,
-            "velocityX": 3.3090000040412137,
-            "velocityY": -0.7291024059908604,
-            "timestamp": 1.739009160433612
-        },
-        {
-            "x": 5.502536097274536,
-            "y": 3.9692960416426737,
-            "heading": 1.5420636410872666,
-            "angularVelocity": -0.9086014792806747,
-            "velocityX": 3.4583407486263615,
-            "velocityY": -0.45304629778540073,
-            "timestamp": 1.7773039139343128
-        },
-        {
-            "x": 5.640613600815518,
-            "y": 3.9622331858505495,
-            "heading": 1.5196895147171037,
-            "angularVelocity": -0.5842608797508935,
-            "velocityX": 3.6056506680074274,
-            "velocityY": -0.18443403198808295,
-            "timestamp": 1.8155986674350135
-        },
-        {
-            "x": 5.785301546169388,
-            "y": 3.963377277975263,
-            "heading": 1.519119424890785,
-            "angularVelocity": -0.014886891132695255,
-            "velocityX": 3.778270706226711,
-            "velocityY": 0.029875949578638192,
-            "timestamp": 1.8538934209357143
-        },
-        {
-            "x": 5.929550760809323,
-            "y": 3.9767385598159253,
-            "heading": 1.5191191822149777,
-            "angularVelocity": -0.000006337051042848807,
-            "velocityX": 3.7668140268168893,
-            "velocityY": 0.3489063283934661,
-            "timestamp": 1.892188174436415
-        },
-        {
-            "x": 6.073462686179593,
-            "y": 3.9933433315789473,
-            "heading": 1.5191189525644635,
-            "angularVelocity": -0.0000059969184602345635,
-            "velocityX": 3.7580063119517533,
-            "velocityY": 0.4336043516435729,
-            "timestamp": 1.9304829279371158
-        },
-        {
-            "x": 6.217374593429904,
-            "y": 4.009948260386706,
-            "heading": 1.519118722914699,
-            "angularVelocity": -0.000005996898884562842,
-            "velocityX": 3.7580058387809343,
-            "velocityY": 0.43360845259010783,
-            "timestamp": 1.9687776814378166
-        },
-        {
-            "x": 6.361286503020876,
-            "y": 4.026553168908082,
-            "heading": 1.5191184932648618,
-            "angularVelocity": -0.000005996900776462565,
-            "velocityX": 3.7580058999032113,
-            "velocityY": 0.4336079228469906,
-            "timestamp": 2.0070724349385176
-        },
-        {
-            "x": 6.5051984133944005,
-            "y": 4.04315807064667,
-            "heading": 1.5191182636148604,
-            "angularVelocity": -0.000005996905070522029,
-            "velocityX": 3.7580059203381437,
-            "velocityY": 0.4336077457264339,
-            "timestamp": 2.0453671884392186
-        },
-        {
-            "x": 6.6491104352583505,
-            "y": 4.059762006067292,
-            "heading": 1.5191180339567425,
-            "angularVelocity": -0.000005997117020238007,
-            "velocityX": 3.7580088317141747,
-            "velocityY": 0.4335825120357026,
-            "timestamp": 2.0836619419399196
-        },
-        {
-            "x": 6.790280334982046,
-            "y": 4.068588148423426,
-            "heading": 1.5085875786821552,
-            "angularVelocity": -0.27498428144717973,
-            "velocityX": 3.6864031445224414,
-            "velocityY": 0.23047915312922457,
-            "timestamp": 2.1219566954406206
-        },
-        {
-            "x": 6.921716704437578,
-            "y": 4.075833761978748,
-            "heading": 1.4694677109965053,
-            "angularVelocity": -1.0215464028234083,
-            "velocityX": 3.43222915517987,
-            "velocityY": 0.18920642889606504,
-            "timestamp": 2.1602514489413216
-        },
-        {
-            "x": 7.043039125371844,
-            "y": 4.08211395301362,
-            "heading": 1.4037305732475935,
-            "angularVelocity": -1.716609502335841,
-            "velocityX": 3.1681212135768497,
-            "velocityY": 0.16399612115943957,
-            "timestamp": 2.1985462024420226
-        },
-        {
-            "x": 7.153946697571625,
-            "y": 4.087967374487544,
-            "heading": 1.3134036899027293,
-            "angularVelocity": -2.3587273735346383,
-            "velocityX": 2.896155793188521,
-            "velocityY": 0.1528517861805002,
-            "timestamp": 2.2368409559427236
-        },
-        {
-            "x": 7.254278338500056,
-            "y": 4.0938696715910385,
-            "heading": 1.199938125135736,
-            "angularVelocity": -2.9629532610757465,
-            "velocityX": 2.619983986229206,
-            "velocityY": 0.1541280871121784,
-            "timestamp": 2.2751357094434246
-        },
-        {
-            "x": 7.34404947628579,
-            "y": 4.100268813517142,
-            "heading": 1.0638507127340207,
-            "angularVelocity": -3.5536829450860603,
-            "velocityX": 2.344215057660344,
-            "velocityY": 0.16710231405423262,
-            "timestamp": 2.3134304629441256
-        },
-        {
-            "x": 7.4235066081317465,
-            "y": 4.107648882181887,
-            "heading": 0.9045749783818139,
-            "angularVelocity": -4.159205107542786,
-            "velocityX": 2.0748829691383666,
-            "velocityY": 0.192717487125464,
-            "timestamp": 2.3517252164448266
-        },
-        {
-            "x": 7.492760083799764,
-            "y": 4.113297423743801,
-            "heading": 0.7219171880631509,
-            "angularVelocity": -4.769786292404788,
-            "velocityX": 1.808432470175071,
-            "velocityY": 0.1475017083426549,
-            "timestamp": 2.3900199699455276
-        },
-        {
-            "x": 7.549820937161571,
-            "y": 4.1180620764150975,
-            "heading": 0.5402485968001275,
-            "angularVelocity": -4.7439551023536115,
-            "velocityX": 1.4900436259698675,
-            "velocityY": 0.1244205076606485,
-            "timestamp": 2.4283147234462286
-        },
-        {
-            "x": 7.595294548959434,
-            "y": 4.121747510359626,
-            "heading": 0.3723680212509228,
-            "angularVelocity": -4.383905370904984,
-            "velocityX": 1.1874632329734431,
-            "velocityY": 0.09623861254154589,
-            "timestamp": 2.4666094769469296
-        },
-        {
-            "x": 7.630234770559726,
-            "y": 4.124236647711556,
-            "heading": 0.22836852122657442,
-            "angularVelocity": -3.7602931697083015,
-            "velocityX": 0.912402311184801,
-            "velocityY": 0.06499943528517171,
-            "timestamp": 2.5049042304476306
-        },
-        {
-            "x": 7.655576852148959,
-            "y": 4.125750923706981,
-            "heading": 0.11519495737029227,
-            "angularVelocity": -2.9553281718920323,
-            "velocityX": 0.6617637998053468,
-            "velocityY": 0.03954264897925492,
-            "timestamp": 2.5431989839483315
-        },
-        {
-            "x": 7.671995118301485,
-            "y": 4.1266005721732935,
-            "heading": 0.03711275718074835,
-            "angularVelocity": -2.0389790520029063,
-            "velocityX": 0.4287340863083462,
-            "velocityY": 0.02218707234390751,
-            "timestamp": 2.5814937374490325
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -0.003,
-            "angularVelocity": -1.047473962197309,
-            "velocityX": 0.20903337838088354,
-            "velocityY": 0.010430353774151994,
-            "timestamp": 2.6197884909497335
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -0.003,
-            "angularVelocity": 7.250240493925158e-33,
-            "velocityX": 3.887711813813661e-34,
-            "velocityY": -4.900942603355191e-34,
-            "timestamp": 2.6580832444504345
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.1.traj
deleted file mode 100644
index ca6ee59b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.1.traj	
+++ /dev/null
@@ -1,400 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7271139423991175,
-            "y": 6.709732017673653,
-            "heading": 0.9305052411067223,
-            "angularVelocity": -1.767895408627961,
-            "velocityX": 0.2763206248110281,
-            "velocityY": -0.16578618916650492,
-            "timestamp": 0.06193508867035021
-        },
-        {
-            "x": 0.7615532933104725,
-            "y": 6.686914726598375,
-            "heading": 0.7206788937202419,
-            "angularVelocity": -3.387842851138587,
-            "velocityX": 0.5560555680263672,
-            "velocityY": -0.36840652956392367,
-            "timestamp": 0.12387017734070042
-        },
-        {
-            "x": 0.8202948153873781,
-            "y": 6.650989300229197,
-            "heading": 0.4540271404715134,
-            "angularVelocity": -4.305342237709284,
-            "velocityX": 0.9484368770271357,
-            "velocityY": -0.5800496477915925,
-            "timestamp": 0.18580526601105063
-        },
-        {
-            "x": 0.9068549514543394,
-            "y": 6.604915057765644,
-            "heading": 0.1437481054753499,
-            "angularVelocity": -5.009745552276917,
-            "velocityX": 1.3975944480789861,
-            "velocityY": -0.7439117865606645,
-            "timestamp": 0.24774035468140085
-        },
-        {
-            "x": 1.022793030858839,
-            "y": 6.550181878656492,
-            "heading": -0.13251218552677876,
-            "angularVelocity": -4.460481076769346,
-            "velocityX": 1.871928851536495,
-            "velocityY": -0.8837184265686551,
-            "timestamp": 0.30967544335175107
-        },
-        {
-            "x": 1.1650239086947571,
-            "y": 6.483494014593417,
-            "heading": -0.35594040398018906,
-            "angularVelocity": -3.6074577957352734,
-            "velocityX": 2.296450701684509,
-            "velocityY": -1.0767380090149254,
-            "timestamp": 0.37161053202210126
-        },
-        {
-            "x": 1.3271753983931303,
-            "y": 6.398307258999271,
-            "heading": -0.5136957877872723,
-            "angularVelocity": -2.5471083870846947,
-            "velocityX": 2.61808763302859,
-            "velocityY": -1.3754199343695408,
-            "timestamp": 0.43354562069245145
-        },
-        {
-            "x": 1.510544798478812,
-            "y": 6.2989823057862875,
-            "heading": -0.5925289533905876,
-            "angularVelocity": -1.2728352747326335,
-            "velocityX": 2.96067066379232,
-            "velocityY": -1.6036943733404823,
-            "timestamp": 0.49548070936280164
-        },
-        {
-            "x": 1.7178378905511122,
-            "y": 6.190936000456915,
-            "heading": -0.5942013489806535,
-            "angularVelocity": -0.027002392762643195,
-            "velocityX": 3.3469410720572084,
-            "velocityY": -1.7445087695675872,
-            "timestamp": 0.5574157980331519
-        },
-        {
-            "x": 1.927701821836241,
-            "y": 6.086763021965953,
-            "heading": -0.5942019920614379,
-            "angularVelocity": -0.000010383141417040622,
-            "velocityX": 3.388449678374247,
-            "velocityY": -1.6819702809408126,
-            "timestamp": 0.6193508867035021
-        },
-        {
-            "x": 2.1375657815463693,
-            "y": 5.982590100737573,
-            "heading": -0.594202635139582,
-            "angularVelocity": -0.0000103830987882079,
-            "velocityX": 3.3884501373224802,
-            "velocityY": -1.6819693563827811,
-            "timestamp": 0.6812859753738524
-        },
-        {
-            "x": 2.347429741257157,
-            "y": 5.878417179509917,
-            "heading": -0.5942032782166262,
-            "angularVelocity": -0.000010383081029176535,
-            "velocityX": 3.3884501373331317,
-            "velocityY": -1.6819693563710962,
-            "timestamp": 0.7432210640442026
-        },
-        {
-            "x": 2.5572937009682177,
-            "y": 5.774244258282216,
-            "heading": -0.5942039212925836,
-            "angularVelocity": -0.000010383063479788062,
-            "velocityX": 3.3884501373375415,
-            "velocityY": -1.6819693563718265,
-            "timestamp": 0.8051561527145529
-        },
-        {
-            "x": 2.7671576606795463,
-            "y": 5.670071337054474,
-            "heading": -0.594204564367474,
-            "angularVelocity": -0.00001038304625561267,
-            "velocityX": 3.388450137341862,
-            "velocityY": -1.68196935637249,
-            "timestamp": 0.8670912413849031
-        },
-        {
-            "x": 2.977021620391137,
-            "y": 5.5658984158266955,
-            "heading": -0.5942052074413191,
-            "angularVelocity": -0.000010383029375253952,
-            "velocityX": 3.388450137346089,
-            "velocityY": -1.6819693563730826,
-            "timestamp": 0.9290263300552534
-        },
-        {
-            "x": 3.186885580102983,
-            "y": 5.461725494598884,
-            "heading": -0.5942058505141404,
-            "angularVelocity": -0.000010383012846871076,
-            "velocityX": 3.3884501373502207,
-            "velocityY": -1.6819693563736025,
-            "timestamp": 0.9909614187256036
-        },
-        {
-            "x": 3.3967495398150795,
-            "y": 5.357552573371046,
-            "heading": -0.594206493585961,
-            "angularVelocity": -0.000010382996688205053,
-            "velocityX": 3.3884501373542517,
-            "velocityY": -1.681969356374048,
-            "timestamp": 1.0528965073959538
-        },
-        {
-            "x": 3.6066134995274184,
-            "y": 5.253379652143185,
-            "heading": -0.5942071366568044,
-            "angularVelocity": -0.000010382980910469912,
-            "velocityX": 3.38845013735818,
-            "velocityY": -1.6819693563744145,
-            "timestamp": 1.114831596066304
-        },
-        {
-            "x": 3.8164774592399944,
-            "y": 5.149206730915306,
-            "heading": -0.5942077797266946,
-            "angularVelocity": -0.000010382965523044611,
-            "velocityX": 3.388450137362003,
-            "velocityY": -1.681969356374701,
-            "timestamp": 1.1767666847366542
-        },
-        {
-            "x": 4.026341418952801,
-            "y": 5.045033809687414,
-            "heading": -0.594208422795657,
-            "angularVelocity": -0.00001038295054018878,
-            "velocityX": 3.388450137365716,
-            "velocityY": -1.6819693563749047,
-            "timestamp": 1.2387017734070045
-        },
-        {
-            "x": 4.236205378665829,
-            "y": 4.940860888459515,
-            "heading": -0.594209065863717,
-            "angularVelocity": -0.000010382935971395078,
-            "velocityX": 3.388450137369317,
-            "velocityY": -1.681969356375023,
-            "timestamp": 1.3006368620773547
-        },
-        {
-            "x": 4.446069338379075,
-            "y": 4.836687967231613,
-            "heading": -0.5942097089309012,
-            "angularVelocity": -0.000010382921828372961,
-            "velocityX": 3.3884501373728035,
-            "velocityY": -1.6819693563750544,
-            "timestamp": 1.362571950747705
-        },
-        {
-            "x": 4.6559332980925285,
-            "y": 4.732515046003717,
-            "heading": -0.5942103519972364,
-            "angularVelocity": -0.000010382908121631514,
-            "velocityX": 3.388450137376173,
-            "velocityY": -1.6819693563749973,
-            "timestamp": 1.4245070394180552
-        },
-        {
-            "x": 4.865797257806183,
-            "y": 4.628342124775828,
-            "heading": -0.5942109950627501,
-            "angularVelocity": -0.00001038289485891648,
-            "velocityX": 3.388450137379422,
-            "velocityY": -1.6819693563748486,
-            "timestamp": 1.4864421280884055
-        },
-        {
-            "x": 5.075661217518705,
-            "y": 4.524169203545277,
-            "heading": -0.594211638127464,
-            "angularVelocity": -0.000010382881945026341,
-            "velocityX": 3.3884501373611275,
-            "velocityY": -1.6819693564178315,
-            "timestamp": 1.5483772167587557
-        },
-        {
-            "x": 5.285525078653758,
-            "y": 4.419996083725492,
-            "heading": -0.5942122811951368,
-            "angularVelocity": -0.000010382929719206097,
-            "velocityX": 3.3884485457355824,
-            "velocityY": -1.6819725628269917,
-            "timestamp": 1.610312305429106
-        },
-        {
-            "x": 5.489266395568848,
-            "y": 4.304305553436279,
-            "heading": -0.5942133396987443,
-            "angularVelocity": -0.000017090531880803138,
-            "velocityX": 3.2895943364108793,
-            "velocityY": -1.8679319392755818,
-            "timestamp": 1.6722473940994562
-        },
-        {
-            "x": 5.681024172067428,
-            "y": 4.156179263559471,
-            "heading": -0.5942132880529166,
-            "angularVelocity": 8.063055616728172e-7,
-            "velocityX": 2.9937628707653796,
-            "velocityY": -2.312578894659331,
-            "timestamp": 1.7362998206864544
-        },
-        {
-            "x": 5.864967854996084,
-            "y": 3.998454299605836,
-            "heading": -0.5942132372714615,
-            "angularVelocity": 7.928107921043856e-7,
-            "velocityX": 2.871767593048443,
-            "velocityY": -2.4624354198261003,
-            "timestamp": 1.8003522472734526
-        },
-        {
-            "x": 6.0489114264021975,
-            "y": 3.840729205591288,
-            "heading": -0.5942131864900138,
-            "angularVelocity": 7.928106775338953e-7,
-            "velocityX": 2.871765851934987,
-            "velocityY": -2.4624374503645132,
-            "timestamp": 1.8644046738604507
-        },
-        {
-            "x": 6.232854997807022,
-            "y": 3.683004111575237,
-            "heading": -0.5942131357085663,
-            "angularVelocity": 7.928106743629292e-7,
-            "velocityX": 2.87176585191486,
-            "velocityY": -2.462437450387986,
-            "timestamp": 1.928457100447449
-        },
-        {
-            "x": 6.4167985692118465,
-            "y": 3.5252790175591864,
-            "heading": -0.594213084927119,
-            "angularVelocity": 7.928106702567426e-7,
-            "velocityX": 2.871765851914861,
-            "velocityY": -2.462437450387986,
-            "timestamp": 1.992509527034447
-        },
-        {
-            "x": 6.6007421406166715,
-            "y": 3.367553923543135,
-            "heading": -0.5942130341456721,
-            "angularVelocity": 7.928106666471698e-7,
-            "velocityX": 2.8717658519148612,
-            "velocityY": -2.462437450387985,
-            "timestamp": 2.0565619536214452
-        },
-        {
-            "x": 6.784685712022088,
-            "y": 3.2098288295277735,
-            "heading": -0.5942129833642253,
-            "angularVelocity": 7.928106614555171e-7,
-            "velocityX": 2.8717658519240943,
-            "velocityY": -2.4624374503772177,
-            "timestamp": 2.120614380208443
-        },
-        {
-            "x": 6.968629334581004,
-            "y": 3.052103795169223,
-            "heading": -0.594212932582592,
-            "angularVelocity": 7.928135744203515e-7,
-            "velocityX": 2.871766650543336,
-            "velocityY": -2.462436519002493,
-            "timestamp": 2.184666806795441
-        },
-        {
-            "x": 7.14554518368725,
-            "y": 2.9064126516878526,
-            "heading": -0.554455950225117,
-            "angularVelocity": 0.6206943979472176,
-            "velocityX": 2.7620475684235433,
-            "velocityY": -2.2745608752775093,
-            "timestamp": 2.248719233382439
-        },
-        {
-            "x": 7.299470766691596,
-            "y": 2.7775737815312107,
-            "heading": -0.43989859569345574,
-            "angularVelocity": 1.7884935924490803,
-            "velocityX": 2.4031186827134454,
-            "velocityY": -2.0114596280228136,
-            "timestamp": 2.312771659969437
-        },
-        {
-            "x": 7.4271138269599435,
-            "y": 2.670230084319628,
-            "heading": -0.3103960824325961,
-            "angularVelocity": 2.021820564205559,
-            "velocityX": 1.9927903917110543,
-            "velocityY": -1.6758724521667427,
-            "timestamp": 2.376824086556435
-        },
-        {
-            "x": 7.52865057013146,
-            "y": 2.5846878922538004,
-            "heading": -0.1939655985882249,
-            "angularVelocity": 1.8177372825404448,
-            "velocityX": 1.5852130603296692,
-            "velocityY": -1.3355027533522315,
-            "timestamp": 2.440876513143433
-        },
-        {
-            "x": 7.604480145165485,
-            "y": 2.520737422138354,
-            "heading": -0.10094853997619462,
-            "angularVelocity": 1.4522019471922978,
-            "velocityX": 1.1838673267285131,
-            "velocityY": -0.9984082340516305,
-            "timestamp": 2.504928939730431
-        },
-        {
-            "x": 7.654868393442648,
-            "y": 2.4782123742697575,
-            "heading": -0.03632146129528005,
-            "angularVelocity": 1.008971589751342,
-            "velocityX": 0.7866719648587251,
-            "velocityY": -0.6639100208145539,
-            "timestamp": 2.568981366317429
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -0.003,
-            "angularVelocity": 0.5202216851226045,
-            "velocityX": 0.39235994475896174,
-            "velocityY": -0.3311720632620588,
-            "timestamp": 2.6330337929044267
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -0.003,
-            "angularVelocity": -9.133242422473854e-34,
-            "velocityX": 1.8081868884139647e-34,
-            "velocityY": 0,
-            "timestamp": 2.6970862194914247
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.1.traj
deleted file mode 100644
index 6231149d..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.1.traj	
+++ /dev/null
@@ -1,805 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7237404107060486,
-            "y": 6.712071337633356,
-            "heading": 0.9558489426883627,
-            "angularVelocity": -1.537881157714612,
-            "velocityX": 0.25110936688338653,
-            "velocityY": -0.14489824428928566,
-            "timestamp": 0.05471882979351295
-        },
-        {
-            "x": 0.7514918072858734,
-            "y": 6.694799584586779,
-            "heading": 0.7935491608961526,
-            "angularVelocity": -2.966068214628546,
-            "velocityX": 0.5071635611460984,
-            "velocityY": -0.31564551200661234,
-            "timestamp": 0.1094376595870259
-        },
-        {
-            "x": 0.7959632333479747,
-            "y": 6.666655428800684,
-            "heading": 0.574279005326375,
-            "angularVelocity": -4.007215731718184,
-            "velocityX": 0.8127261900504715,
-            "velocityY": -0.514341331718894,
-            "timestamp": 0.16415648938053887
-        },
-        {
-            "x": 0.8617224500002636,
-            "y": 6.63045184710829,
-            "heading": 0.31905346805964874,
-            "angularVelocity": -4.664309127769047,
-            "velocityX": 1.2017657705846174,
-            "velocityY": -0.6616293116832328,
-            "timestamp": 0.2188753191740518
-        },
-        {
-            "x": 0.9507709996198539,
-            "y": 6.585947725507511,
-            "heading": 0.05142997722620149,
-            "angularVelocity": -4.890884762034389,
-            "velocityX": 1.627384027685255,
-            "velocityY": -0.8133237090178916,
-            "timestamp": 0.2735941489675648
-        },
-        {
-            "x": 1.0614190010918925,
-            "y": 6.535018660742887,
-            "heading": -0.17495854287380355,
-            "angularVelocity": -4.137305584828904,
-            "velocityX": 2.0221192940269503,
-            "velocityY": -0.9307411170306262,
-            "timestamp": 0.32831297876107773
-        },
-        {
-            "x": 1.1921384640832198,
-            "y": 6.474205616338843,
-            "heading": -0.35862575739678126,
-            "angularVelocity": -3.356563274764148,
-            "velocityX": 2.388930163247485,
-            "velocityY": -1.111373262796896,
-            "timestamp": 0.3830318085545907
-        },
-        {
-            "x": 1.3384031857193457,
-            "y": 6.398894346098027,
-            "heading": -0.49152044440666937,
-            "angularVelocity": -2.4286829142980233,
-            "velocityX": 2.6730235677201417,
-            "velocityY": -1.376331886573771,
-            "timestamp": 0.4377506383481036
-        },
-        {
-            "x": 1.500957547301863,
-            "y": 6.312259668262383,
-            "heading": -0.5624501430724521,
-            "angularVelocity": -1.2962575942036991,
-            "velocityX": 2.9707207225727275,
-            "velocityY": -1.5832699303433067,
-            "timestamp": 0.4924694681416166
-        },
-        {
-            "x": 1.6817069101290123,
-            "y": 6.217739107445247,
-            "heading": -0.5720464000476684,
-            "angularVelocity": -0.17537394369443057,
-            "velocityX": 3.3032388212471107,
-            "velocityY": -1.7273863709040396,
-            "timestamp": 0.5471882979351296
-        },
-        {
-            "x": 1.8673643551319996,
-            "y": 6.126199810501224,
-            "heading": -0.5720469984173726,
-            "angularVelocity": -0.000010935352720012522,
-            "velocityX": 3.392935223643913,
-            "velocityY": -1.6729030443351731,
-            "timestamp": 0.6019071277286425
-        },
-        {
-            "x": 2.0530162492759167,
-            "y": 6.034649256644459,
-            "heading": -0.5720475972653951,
-            "angularVelocity": -0.000010944094102204816,
-            "velocityX": 3.3928337803365705,
-            "velocityY": -1.6731087671691436,
-            "timestamp": 0.6566259575221555
-        },
-        {
-            "x": 2.23866813267496,
-            "y": 5.943098680998449,
-            "heading": -0.5720481961130315,
-            "angularVelocity": -0.000010944087046546937,
-            "velocityX": 3.3928335839713664,
-            "velocityY": -1.673109165372953,
-            "timestamp": 0.7113447873156684
-        },
-        {
-            "x": 2.4243200161477754,
-            "y": 5.851548105501653,
-            "heading": -0.5720487949598734,
-            "angularVelocity": -0.000010944072528266654,
-            "velocityX": 3.392833585319575,
-            "velocityY": -1.673109162646019,
-            "timestamp": 0.7660636171091814
-        },
-        {
-            "x": 2.6099718996232633,
-            "y": 5.759997530009892,
-            "heading": -0.5720493938059237,
-            "angularVelocity": -0.000010944058060277418,
-            "velocityX": 3.3928335853684146,
-            "velocityY": -1.673109162553994,
-            "timestamp": 0.8207824469026943
-        },
-        {
-            "x": 2.7956237830994004,
-            "y": 5.668446954519065,
-            "heading": -0.5720499926511823,
-            "angularVelocity": -0.000010944043595406558,
-            "velocityX": 3.3928335853802802,
-            "velocityY": -1.6731091625369476,
-            "timestamp": 0.8755012766962073
-        },
-        {
-            "x": 2.9812756665756717,
-            "y": 5.576896379028125,
-            "heading": -0.5720505914956495,
-            "angularVelocity": -0.000010944029129446314,
-            "velocityX": 3.3928335853827263,
-            "velocityY": -1.6731091625390042,
-            "timestamp": 0.9302201064897202
-        },
-        {
-            "x": 3.166927550036217,
-            "y": 5.485345803504912,
-            "heading": -0.5720511903393257,
-            "angularVelocity": -0.000010944014673786385,
-            "velocityX": 3.392833585095338,
-            "velocityY": -1.673109163128796,
-            "timestamp": 0.9849389362832331
-        },
-        {
-            "x": 3.3525794334560626,
-            "y": 5.393795227898782,
-            "heading": -0.5720517891822119,
-            "angularVelocity": -0.000010944000236842356,
-            "velocityX": 3.392833584351539,
-            "velocityY": -1.6731091646441256,
-            "timestamp": 1.039657766076746
-        },
-        {
-            "x": 3.538231313226632,
-            "y": 5.302244644892125,
-            "heading": -0.5720523880244444,
-            "angularVelocity": -0.000010943988288632917,
-            "velocityX": 3.392833517660122,
-            "velocityY": -1.673109299890567,
-            "timestamp": 1.0943765958702592
-        },
-        {
-            "x": 3.723883292814953,
-            "y": 5.21069426429933,
-            "heading": -0.5720529868621156,
-            "angularVelocity": -0.000010943904932797782,
-            "velocityX": 3.3928353418539645,
-            "velocityY": -1.673105600727685,
-            "timestamp": 1.1490954256637722
-        },
-        {
-            "x": 3.909492929164672,
-            "y": 5.1190580682844296,
-            "heading": -0.5720535873273735,
-            "angularVelocity": -0.000010973649477031932,
-            "velocityX": 3.3920615088102064,
-            "velocityY": -1.6746738985594591,
-            "timestamp": 1.2038142554572853
-        },
-        {
-            "x": 4.092995643615723,
-            "y": 5.023272514343262,
-            "heading": -0.5720542830492642,
-            "angularVelocity": -0.000012714487741704976,
-            "velocityX": 3.353556995709023,
-            "velocityY": -1.7505044296932655,
-            "timestamp": 1.2585330852507983
-        },
-        {
-            "x": 4.152699494738863,
-            "y": 4.989124335806784,
-            "heading": -0.5720542274621303,
-            "angularVelocity": 0.0000030573365885939726,
-            "velocityX": 3.283759307267904,
-            "velocityY": -1.8781769850015708,
-            "timestamp": 1.2767146404552234
-        },
-        {
-            "x": 4.211236123155136,
-            "y": 4.953011857528808,
-            "heading": -0.5720541720789936,
-            "angularVelocity": 0.000003046116576982166,
-            "velocityX": 3.2195611298433677,
-            "velocityY": -1.9862150334199096,
-            "timestamp": 1.2948961956596485
-        },
-        {
-            "x": 4.269217408443118,
-            "y": 4.916014310130808,
-            "heading": -0.5720541167402081,
-            "angularVelocity": 0.000003043677217870583,
-            "velocityX": 3.1890168160017764,
-            "velocityY": -2.034894539109796,
-            "timestamp": 1.3130777508640736
-        },
-        {
-            "x": 4.327172181408397,
-            "y": 4.8789752463128,
-            "heading": -0.5720540614037476,
-            "angularVelocity": 0.000003043549349895456,
-            "velocityX": 3.187558617163689,
-            "velocityY": -2.0371779752347874,
-            "timestamp": 1.3312593060684987
-        },
-        {
-            "x": 4.385108415981365,
-            "y": 4.841907191568928,
-            "heading": -0.5720540060672449,
-            "angularVelocity": 0.0000030435516711711228,
-            "velocityX": 3.186538990839725,
-            "velocityY": -2.0387724992190503,
-            "timestamp": 1.3494408612729238
-        },
-        {
-            "x": 4.443034277698237,
-            "y": 4.804822929374826,
-            "heading": -0.5720539507307192,
-            "angularVelocity": 0.000003043552934036031,
-            "velocityX": 3.1859684755020403,
-            "velocityY": -2.0396639218758956,
-            "timestamp": 1.367622416477349
-        },
-        {
-            "x": 4.500957639641057,
-            "y": 4.767734762799275,
-            "heading": -0.5720538953941279,
-            "angularVelocity": 0.0000030435565398491907,
-            "velocityX": 3.1858309859372986,
-            "velocityY": -2.039878665966735,
-            "timestamp": 1.385803971681774
-        },
-        {
-            "x": 4.558883202891902,
-            "y": 4.7306500344023386,
-            "heading": -0.5720538400575734,
-            "angularVelocity": 0.000003043554516316428,
-            "velocityX": 3.1859520596302042,
-            "velocityY": -2.03968956340728,
-            "timestamp": 1.4039855268861992
-        },
-        {
-            "x": 4.616809360440233,
-            "y": 4.693566234312182,
-            "heading": -0.572053784721022,
-            "angularVelocity": 0.000003043554346667464,
-            "velocityX": 3.1859847464654854,
-            "velocityY": -2.039638505794633,
-            "timestamp": 1.4221670820906243
-        },
-        {
-            "x": 4.674735447300298,
-            "y": 4.656482323806888,
-            "heading": -0.5720537293844703,
-            "angularVelocity": 0.00000304355436453364,
-            "velocityX": 3.185980858554995,
-            "velocityY": -2.039644578714451,
-            "timestamp": 1.4403486372950494
-        },
-        {
-            "x": 4.732661331177625,
-            "y": 4.619398096238942,
-            "heading": -0.5720536740479176,
-            "angularVelocity": 0.0000030435544255368,
-            "velocityX": 3.1859696943429383,
-            "velocityY": -2.0396620174125277,
-            "timestamp": 1.4585301924994745
-        },
-        {
-            "x": 4.790587117727679,
-            "y": 4.582313716645222,
-            "heading": -0.5720536187113643,
-            "angularVelocity": 0.0000030435544545803464,
-            "velocityX": 3.1859643412654055,
-            "velocityY": -2.039670378951063,
-            "timestamp": 1.4767117477038996
-        },
-        {
-            "x": 4.848512993160408,
-            "y": 4.5452294758867104,
-            "heading": -0.5720535633748114,
-            "angularVelocity": 0.0000030435544295946876,
-            "velocityX": 3.1859692298833355,
-            "velocityY": -2.0396627429041696,
-            "timestamp": 1.4948933029083247
-        },
-        {
-            "x": 4.906438819485911,
-            "y": 4.508145158422396,
-            "heading": -0.5720535080382583,
-            "angularVelocity": 0.0000030435544442083865,
-            "velocityX": 3.1859665289468815,
-            "velocityY": -2.039666961784329,
-            "timestamp": 1.5130748581127498
-        },
-        {
-            "x": 4.9643646433421065,
-            "y": 4.471060837101057,
-            "heading": -0.5720534527017052,
-            "angularVelocity": 0.0000030435544458805193,
-            "velocityX": 3.185966393132936,
-            "velocityY": -2.039667173923745,
-            "timestamp": 1.5312564133171749
-        },
-        {
-            "x": 5.022290480818567,
-            "y": 4.433976537054754,
-            "heading": -0.572053397365152,
-            "angularVelocity": 0.0000030435544415454932,
-            "velocityX": 3.185967142258403,
-            "velocityY": -2.039666003779759,
-            "timestamp": 1.5494379685216
-        },
-        {
-            "x": 5.080216347467466,
-            "y": 4.396892282576102,
-            "heading": -0.572053342028599,
-            "angularVelocity": 0.000003043554433808661,
-            "velocityX": 3.1859687467658464,
-            "velocityY": -2.039663497522643,
-            "timestamp": 1.567619523726025
-        },
-        {
-            "x": 5.138142382220086,
-            "y": 4.359808290677983,
-            "heading": -0.5720532866920471,
-            "angularVelocity": 0.000003043554377469469,
-            "velocityX": 3.185977992604117,
-            "velocityY": -2.039649055384386,
-            "timestamp": 1.5858010789304502
-        },
-        {
-            "x": 5.196068138494407,
-            "y": 4.322723863792993,
-            "heading": -0.5720532313554916,
-            "angularVelocity": 0.000003043554576021048,
-            "velocityX": 3.1859626760759787,
-            "velocityY": -2.0396729800088242,
-            "timestamp": 1.6039826341348753
-        },
-        {
-            "x": 5.253993690845546,
-            "y": 4.285639118381674,
-            "heading": -0.5720531760189423,
-            "angularVelocity": 0.0000030435542278177066,
-            "velocityX": 3.1859514601384813,
-            "velocityY": -2.0396904992104243,
-            "timestamp": 1.6221641893393004
-        },
-        {
-            "x": 5.3119198680466875,
-            "y": 4.248555348991913,
-            "heading": -0.5720531206823879,
-            "angularVelocity": 0.000003043554518420827,
-            "velocityX": 3.1859858273860655,
-            "velocityY": -2.0396368172479113,
-            "timestamp": 1.6403457445437255
-        },
-        {
-            "x": 5.369845451022558,
-            "y": 4.211470651416335,
-            "heading": -0.5720530653458327,
-            "angularVelocity": 0.0000030435545594837867,
-            "velocityX": 3.1859531445232334,
-            "velocityY": -2.039687868206526,
-            "timestamp": 1.6585272997481506
-        },
-        {
-            "x": 5.427769955136907,
-            "y": 4.17438426873464,
-            "heading": -0.5720530100092767,
-            "angularVelocity": 0.0000030435545998300077,
-            "velocityX": 3.185893806281465,
-            "velocityY": -2.039780550383344,
-            "timestamp": 1.6767088549525757
-        },
-        {
-            "x": 5.485698211873678,
-            "y": 4.137303747848898,
-            "heading": -0.5720529546727399,
-            "angularVelocity": 0.000003043553546723704,
-            "velocityX": 3.1861002035013715,
-            "velocityY": -2.0394581469404574,
-            "timestamp": 1.6948904101570008
-        },
-        {
-            "x": 5.5436372749681775,
-            "y": 4.100240114388813,
-            "heading": -0.5720528993360909,
-            "angularVelocity": 0.0000030435597092212927,
-            "velocityX": 3.186694561772871,
-            "velocityY": -2.0385293250953618,
-            "timestamp": 1.713071965361426
-        },
-        {
-            "x": 5.601833960036281,
-            "y": 4.063582321171135,
-            "heading": -0.5720528439855975,
-            "angularVelocity": 0.0000030443211669066955,
-            "velocityX": 3.2008639752640304,
-            "velocityY": -2.0162077889097265,
-            "timestamp": 1.731253520565851
-        },
-        {
-            "x": 5.661045954546234,
-            "y": 4.0285882123488195,
-            "heading": -0.5720527885077281,
-            "angularVelocity": 0.000003051326955110969,
-            "velocityX": 3.2567068022627845,
-            "velocityY": -1.9247038236737246,
-            "timestamp": 1.7494350757702761
-        },
-        {
-            "x": 5.721517200862813,
-            "y": 3.9958181607132874,
-            "heading": -0.5720527326566293,
-            "angularVelocity": 0.0000030718548612359243,
-            "velocityX": 3.3259666533847816,
-            "velocityY": -1.8023789091228484,
-            "timestamp": 1.7676166309747012
-        },
-        {
-            "x": 5.7832255363464355,
-            "y": 3.9654417037963863,
-            "heading": -0.572052675208486,
-            "angularVelocity": 0.000003159693577578397,
-            "velocityX": 3.3940075416988695,
-            "velocityY": -1.670729295450284,
-            "timestamp": 1.7857981861791263
-        },
-        {
-            "x": 5.945861586676733,
-            "y": 3.9048707731215235,
-            "heading": -0.5720526214657033,
-            "angularVelocity": 0.0000011714587578241761,
-            "velocityX": 3.5450606755342,
-            "velocityY": -1.3202953710429959,
-            "timestamp": 1.831674989206145
-        },
-        {
-            "x": 6.113092819895856,
-            "y": 3.858469939627616,
-            "heading": -0.5720525698257588,
-            "angularVelocity": 0.0000011256221281562495,
-            "velocityX": 3.6452242132183756,
-            "velocityY": -1.0114225585101817,
-            "timestamp": 1.8775517922331635
-        },
-        {
-            "x": 6.280428909416468,
-            "y": 3.8124486851902244,
-            "heading": -0.5720525182264563,
-            "angularVelocity": 0.0000011247362300949913,
-            "velocityX": 3.6475098193329205,
-            "velocityY": -1.0031486808299002,
-            "timestamp": 1.923428595260182
-        },
-        {
-            "x": 6.444900242470224,
-            "y": 3.7570546132296903,
-            "heading": -0.5720524653781475,
-            "angularVelocity": 0.000001151961455361842,
-            "velocityX": 3.585065266142057,
-            "velocityY": -1.2074527496557126,
-            "timestamp": 1.9693053982872004
-        },
-        {
-            "x": 6.602974533610246,
-            "y": 3.685417746961916,
-            "heading": -0.5720524086130027,
-            "angularVelocity": 0.0000012373387213911858,
-            "velocityX": 3.445625691199315,
-            "velocityY": -1.561505195243581,
-            "timestamp": 2.015182201314219
-        },
-        {
-            "x": 6.753006171084989,
-            "y": 3.5981834135738024,
-            "heading": -0.5720523449049553,
-            "angularVelocity": 0.0000013886766999770178,
-            "velocityX": 3.27031588025751,
-            "velocityY": -1.9014911160397099,
-            "timestamp": 2.0610590043412373
-        },
-        {
-            "x": 6.893461047058093,
-            "y": 3.4962434932531963,
-            "heading": -0.5720522700606003,
-            "angularVelocity": 0.0000016314204553722347,
-            "velocityX": 3.0615663408441725,
-            "velocityY": -2.2220362709370285,
-            "timestamp": 2.106935807368256
-        },
-        {
-            "x": 7.0229029918463715,
-            "y": 3.380640293435665,
-            "heading": -0.5720521774325067,
-            "angularVelocity": 0.000002019061650871652,
-            "velocityX": 2.8215118806773165,
-            "velocityY": -2.519861720737319,
-            "timestamp": 2.1528126103952743
-        },
-        {
-            "x": 7.140008449554443,
-            "y": 3.2525558471679688,
-            "heading": -0.5720520639012779,
-            "angularVelocity": 0.0000024746979161572956,
-            "velocityX": 2.5526071997441684,
-            "velocityY": -2.7919217952549285,
-            "timestamp": 2.1986894134222927
-        },
-        {
-            "x": 7.220415929770595,
-            "y": 3.149946383889573,
-            "heading": -0.5720519473102574,
-            "angularVelocity": 0.000003383341158681086,
-            "velocityX": 2.333335244140948,
-            "velocityY": -2.977611988413324,
-            "timestamp": 2.2331497338662096
-        },
-        {
-            "x": 7.2928033634314815,
-            "y": 3.0415299314010706,
-            "heading": -0.5720518591979534,
-            "angularVelocity": 0.000002556920622107498,
-            "velocityX": 2.100602453152653,
-            "velocityY": -3.1461243276865574,
-            "timestamp": 2.2676100543101265
-        },
-        {
-            "x": 7.356753135957797,
-            "y": 2.927931973099431,
-            "heading": -0.5720517877574303,
-            "angularVelocity": 0.000002073124165462267,
-            "velocityX": 1.855750953634494,
-            "velocityY": -3.2964858375736252,
-            "timestamp": 2.3020703747540434
-        },
-        {
-            "x": 7.411896317838347,
-            "y": 2.8098078848104033,
-            "heading": -0.5720517267503814,
-            "angularVelocity": 0.0000017703564026272917,
-            "velocityX": 1.6001935318717442,
-            "velocityY": -3.427829073187373,
-            "timestamp": 2.3365306951979603
-        },
-        {
-            "x": 7.457914797691639,
-            "y": 2.687839154921363,
-            "heading": -0.572051672451037,
-            "angularVelocity": 0.0000015757063075533039,
-            "velocityX": 1.3354048732140973,
-            "velocityY": -3.5393962771625227,
-            "timestamp": 2.370991015641877
-        },
-        {
-            "x": 7.494543133171347,
-            "y": 2.5627294544923394,
-            "heading": -0.5720516223974168,
-            "angularVelocity": 0.000001452500141635371,
-            "velocityX": 1.0629133742199746,
-            "velocityY": -3.630543733121263,
-            "timestamp": 2.405451336085794
-        },
-        {
-            "x": 7.521592976236692,
-            "y": 2.435205423722826,
-            "heading": -0.572051574793162,
-            "angularVelocity": 0.0000013814222897397843,
-            "velocityX": 0.7849562255098341,
-            "velocityY": -3.70060490229679,
-            "timestamp": 2.439911656529711
-        },
-        {
-            "x": 7.539069360193098,
-            "y": 2.3060208712744585,
-            "heading": -0.572051528188319,
-            "angularVelocity": 0.000001352420480159191,
-            "velocityX": 0.5071451376970121,
-            "velocityY": -3.748791386272309,
-            "timestamp": 2.474371976973628
-        },
-        {
-            "x": 7.551504596507269,
-            "y": 2.176254012981795,
-            "heading": -0.5720514816592456,
-            "angularVelocity": 0.0000013502217280486013,
-            "velocityX": 0.3608566651205708,
-            "velocityY": -3.7656892513183533,
-            "timestamp": 2.5088322974175448
-        },
-        {
-            "x": 7.5639699142109125,
-            "y": 2.0464900408376128,
-            "heading": -0.5720514351306527,
-            "angularVelocity": 0.0000013502077822692335,
-            "velocityX": 0.36172959342888805,
-            "velocityY": -3.765605498514506,
-            "timestamp": 2.5432926178614617
-        },
-        {
-            "x": 7.576441327813496,
-            "y": 1.916726654418564,
-            "heading": -0.5720513886022516,
-            "angularVelocity": 0.00000135020222116771,
-            "velocityX": 0.36190648960876737,
-            "velocityY": -3.7655885014252446,
-            "timestamp": 2.5777529383053785
-        },
-        {
-            "x": 7.58884833483585,
-            "y": 1.786957094038731,
-            "heading": -0.5720513420716767,
-            "angularVelocity": 0.000001350265295572762,
-            "velocityX": 0.36003748258495927,
-            "velocityY": -3.765767662871606,
-            "timestamp": 2.6122132587492954
-        },
-        {
-            "x": 7.601019357724818,
-            "y": 1.6571651890693204,
-            "heading": -0.5720512933827582,
-            "angularVelocity": 0.0000014128980208836598,
-            "velocityX": 0.3531894866947264,
-            "velocityY": -3.7664160778959355,
-            "timestamp": 2.6466735791932123
-        },
-        {
-            "x": 7.610550391987499,
-            "y": 1.5345810963054864,
-            "heading": -0.5537181215211259,
-            "angularVelocity": 0.5320081654919462,
-            "velocityX": 0.2765799661741953,
-            "velocityY": -3.5572534203020942,
-            "timestamp": 2.6811338996371292
-        },
-        {
-            "x": 7.620334086621719,
-            "y": 1.4202071871878734,
-            "heading": -0.5150322166693104,
-            "angularVelocity": 1.122621738667962,
-            "velocityX": 0.2839118878810709,
-            "velocityY": -3.319003063356803,
-            "timestamp": 2.715594220081046
-        },
-        {
-            "x": 7.630352820350644,
-            "y": 1.314046573370508,
-            "heading": -0.4556179078059868,
-            "angularVelocity": 1.7241368651811046,
-            "velocityX": 0.290732459821803,
-            "velocityY": -3.0806624096878554,
-            "timestamp": 2.750054540524963
-        },
-        {
-            "x": 7.63947235322732,
-            "y": 1.2175511973082003,
-            "heading": -0.3892109145347841,
-            "angularVelocity": 1.9270567544269157,
-            "velocityX": 0.2646386556821465,
-            "velocityY": -2.8001880080992603,
-            "timestamp": 2.78451486096888
-        },
-        {
-            "x": 7.647652812069227,
-            "y": 1.1309118710627388,
-            "heading": -0.32231407882554747,
-            "angularVelocity": 1.9412714347239703,
-            "velocityX": 0.23738777633239685,
-            "velocityY": -2.5141764536539966,
-            "timestamp": 2.818975181412797
-        },
-        {
-            "x": 7.654895533140534,
-            "y": 1.0540989766744202,
-            "heading": -0.25819483301023066,
-            "angularVelocity": 1.8606688791433752,
-            "velocityX": 0.21017567387602423,
-            "velocityY": -2.22902437930995,
-            "timestamp": 2.8534355018567137
-        },
-        {
-            "x": 7.66120815675186,
-            "y": 0.9870533783479266,
-            "heading": -0.19885832997172487,
-            "angularVelocity": 1.7218790270703905,
-            "velocityX": 0.18318528469800885,
-            "velocityY": -1.9455883596791401,
-            "timestamp": 2.8878958223006306
-        },
-        {
-            "x": 7.666600355589264,
-            "y": 0.929716153410391,
-            "heading": -0.14570825222338094,
-            "angularVelocity": 1.542355876662632,
-            "velocityX": 0.15647558606309142,
-            "velocityY": -1.663862210186161,
-            "timestamp": 2.9223561427445475
-        },
-        {
-            "x": 7.671080940668327,
-            "y": 0.8820338579186632,
-            "heading": -0.09980369912266952,
-            "angularVelocity": 1.3320988461333123,
-            "velocityX": 0.1300215732566484,
-            "velocityY": -1.383686944215496,
-            "timestamp": 2.9568164631884644
-        },
-        {
-            "x": 7.674656709905022,
-            "y": 0.8439593502971015,
-            "heading": -0.06197555143316066,
-            "angularVelocity": 1.0977305841102087,
-            "velocityX": 0.10376482837700403,
-            "velocityY": -1.1048796741031013,
-            "timestamp": 2.9912767836323813
-        },
-        {
-            "x": 7.67733258738128,
-            "y": 0.8154516156506164,
-            "heading": -0.032889080662387146,
-            "angularVelocity": 0.8440568861838474,
-            "velocityX": 0.07765097485389878,
-            "velocityY": -0.8272626104241091,
-            "timestamp": 3.025737104076298
-        },
-        {
-            "x": 7.679112422858347,
-            "y": 0.7964752743679322,
-            "heading": -0.013083588528854375,
-            "angularVelocity": 0.5747332548975859,
-            "velocityX": 0.05164883710108163,
-            "velocityY": -0.5506722235380279,
-            "timestamp": 3.060197424520215
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -0.003,
-            "angularVelocity": 0.2926144736603049,
-            "velocityX": 0.025756497043992084,
-            "velocityY": -0.27496187632245256,
-            "timestamp": 3.094657744964132
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -0.003,
-            "angularVelocity": 9.445490920898867e-33,
-            "velocityX": 0,
-            "velocityY": 3.8255947334103544e-33,
-            "timestamp": 3.129118065408049
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj
deleted file mode 100644
index 59066a5b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj	
+++ /dev/null
@@ -1,121 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": -3.0520704892108665e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7541195079597249,
-            "y": 6.720987197392724,
-            "heading": 1.0162961519009226,
-            "angularVelocity": -0.3240021115694771,
-            "velocityX": 0.6030587810307456,
-            "velocityY": 0.013493760103527496,
-            "timestamp": 0.07315954820244353
-        },
-        {
-            "x": 0.8424136303815098,
-            "y": 6.722972492793459,
-            "heading": 0.9702543819568796,
-            "angularVelocity": -0.6293337107086672,
-            "velocityX": 1.2068707993857855,
-            "velocityY": 0.027136518055622637,
-            "timestamp": 0.14631909640488705
-        },
-        {
-            "x": 0.9749626751701804,
-            "y": 6.7259693430054925,
-            "heading": 0.9040114694947134,
-            "angularVelocity": -0.9054581949963141,
-            "velocityX": 1.8117805268819878,
-            "velocityY": 0.0409632137659495,
-            "timestamp": 0.21947864460733058
-        },
-        {
-            "x": 1.1518931657803464,
-            "y": 6.729992727527654,
-            "heading": 0.8214709979365993,
-            "angularVelocity": -1.1282255506783914,
-            "velocityX": 2.4184196725842533,
-            "velocityY": 0.0549946605880228,
-            "timestamp": 0.2926381928097741
-        },
-        {
-            "x": 1.3734034446931793,
-            "y": 6.735064446892539,
-            "heading": 0.7325177816386138,
-            "angularVelocity": -1.215879792639604,
-            "velocityX": 3.0277699132296103,
-            "velocityY": 0.06932409356680827,
-            "timestamp": 0.3657977410122176
-        },
-        {
-            "x": 1.6366130282089124,
-            "y": 6.741790657706068,
-            "heading": 0.6993734786052862,
-            "angularVelocity": -0.4530413848603092,
-            "velocityX": 3.597747525550208,
-            "velocityY": 0.09193893317775545,
-            "timestamp": 0.43895728921466115
-        },
-        {
-            "x": 1.858120509684905,
-            "y": 6.746838490569188,
-            "heading": 0.6098796286126051,
-            "angularVelocity": -1.2232695825988174,
-            "velocityX": 3.0277316757485147,
-            "velocityY": 0.0689975948068392,
-            "timestamp": 0.5121168374171047
-        },
-        {
-            "x": 2.035048195511774,
-            "y": 6.750890505964207,
-            "heading": 0.5269052635268205,
-            "angularVelocity": -1.1341563353588457,
-            "velocityX": 2.418381334686234,
-            "velocityY": 0.0553860089978706,
-            "timestamp": 0.5852763856195482
-        },
-        {
-            "x": 2.1675926440263233,
-            "y": 6.753939666061267,
-            "heading": 0.4602292179018529,
-            "angularVelocity": -0.9113785864349585,
-            "velocityX": 1.811717701533345,
-            "velocityY": 0.041678224811154865,
-            "timestamp": 0.6584359338219917
-        },
-        {
-            "x": 2.2558825150080364,
-            "y": 6.755978237986949,
-            "heading": 0.41386063697230147,
-            "angularVelocity": -0.6338008102679221,
-            "velocityX": 1.206812687489562,
-            "velocityY": 0.027864741865846625,
-            "timestamp": 0.7315954820244353
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": -0.3261452203924221,
-            "velocityX": 0.6030311295783809,
-            "velocityY": 0.013966215458624026,
-            "timestamp": 0.8047550302268788
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 9.371559495672325e-33,
-            "velocityX": 2.5226214414524116e-33,
-            "velocityY": -5.278780681595662e-33,
-            "timestamp": 0.8779145784293223
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.1.traj
deleted file mode 100644
index fb2e2531..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": 3.547724392998225e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.737435339105297,
-            "y": 6.6995140898970185,
-            "heading": 1.00766177496316,
-            "angularVelocity": -0.49661186995303425,
-            "velocityX": 0.4213191985755434,
-            "velocityY": -0.3145981609176562,
-            "timestamp": 0.06511770457661178
-        },
-        {
-            "x": 0.7924238483684921,
-            "y": 6.658398285939555,
-            "heading": 0.945783535120445,
-            "angularVelocity": -0.95025216636614,
-            "velocityX": 0.8444479058456402,
-            "velocityY": -0.6314074524707902,
-            "timestamp": 0.13023540915322357
-        },
-        {
-            "x": 0.8751312092590968,
-            "y": 6.596482141852294,
-            "heading": 0.8584519121155169,
-            "angularVelocity": -1.3411348506945964,
-            "velocityX": 1.2701209514118896,
-            "velocityY": -0.9508342545216032,
-            "timestamp": 0.19535311372983535
-        },
-        {
-            "x": 0.9857818913354864,
-            "y": 6.513571279138628,
-            "heading": 0.7519056782658311,
-            "angularVelocity": -1.6362099146835547,
-            "velocityX": 1.6992411325894867,
-            "velocityY": -1.273246089565702,
-            "timestamp": 0.26047081830644714
-        },
-        {
-            "x": 1.1246151480908613,
-            "y": 6.409448227792029,
-            "heading": 0.6367147770436085,
-            "angularVelocity": -1.7689644002530829,
-            "velocityX": 2.132035483407367,
-            "velocityY": -1.5989975694566836,
-            "timestamp": 0.32558852288305895
-        },
-        {
-            "x": 1.2913898404503776,
-            "y": 6.284148794801154,
-            "heading": 0.5378900995711877,
-            "angularVelocity": -1.5176314662036392,
-            "velocityX": 2.5611267080722016,
-            "velocityY": -1.9241991683453519,
-            "timestamp": 0.39070622745967076
-        },
-        {
-            "x": 1.479902092849603,
-            "y": 6.139172935751937,
-            "heading": 0.5154398140678585,
-            "angularVelocity": -0.3447646941687853,
-            "velocityX": 2.8949462150871557,
-            "velocityY": -2.2263662392867607,
-            "timestamp": 0.4558239320362826
-        },
-        {
-            "x": 1.6682099030328927,
-            "y": 5.994133301373275,
-            "heading": 0.4928532472983225,
-            "angularVelocity": -0.34685753922672635,
-            "velocityX": 2.8918066355017875,
-            "velocityY": -2.2273456246913312,
-            "timestamp": 0.5209416366128944
-        },
-        {
-            "x": 1.8350205910276742,
-            "y": 5.8693381796986746,
-            "heading": 0.39723540571375665,
-            "angularVelocity": -1.4683847074503305,
-            "velocityX": 2.561679486083952,
-            "velocityY": -1.9164545569596636,
-            "timestamp": 0.5860593411895062
-        },
-        {
-            "x": 1.9739536597530634,
-            "y": 5.765582600106914,
-            "heading": 0.2820330722411117,
-            "angularVelocity": -1.7691399631126123,
-            "velocityX": 2.1335682765342634,
-            "velocityY": -1.5933543767607987,
-            "timestamp": 0.651177045766118
-        },
-        {
-            "x": 2.08468090414567,
-            "y": 5.683000750382208,
-            "heading": 0.1756895135649168,
-            "angularVelocity": -1.6330974712273538,
-            "velocityX": 1.7004168852778658,
-            "velocityY": -1.2681935007022147,
-            "timestamp": 0.7162947503427298
-        },
-        {
-            "x": 2.167467858188685,
-            "y": 5.6213477608369935,
-            "heading": 0.08927498136102419,
-            "angularVelocity": -1.3270512645639152,
-            "velocityX": 1.2713432480657338,
-            "velocityY": -0.946793041094972,
-            "timestamp": 0.7814124549193416
-        },
-        {
-            "x": 2.2225251033432207,
-            "y": 5.5804064125777195,
-            "heading": 0.028526912579098198,
-            "angularVelocity": -0.9328963478811614,
-            "velocityX": 0.8455034696402836,
-            "velocityY": -0.628728370041152,
-            "timestamp": 0.8465301594959534
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": -0.4841527013902356,
-            "velocityX": 0.42192667624600166,
-            "velocityY": -0.31337733279145685,
-            "timestamp": 0.9116478640725653
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": -3.2592320489843035e-28,
-            "velocityX": -3.6408651267401044e-33,
-            "velocityY": 2.9208085569078165e-32,
-            "timestamp": 0.9767655686491771
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.1.traj
deleted file mode 100644
index dd55da0b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.1.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7269019070910594,
-            "y": 6.695506477136938,
-            "heading": 1.0019343442516249,
-            "angularVelocity": -0.6189075555866055,
-            "velocityX": 0.2748072454505401,
-            "velocityY": -0.3982389391394925,
-            "timestamp": 0.06150459047522247
-        },
-        {
-            "x": 0.7606735605009036,
-            "y": 6.646281390208111,
-            "heading": 0.9275075561119867,
-            "angularVelocity": -1.2101013528351434,
-            "velocityX": 0.5490915905447641,
-            "velocityY": -0.8003481780543988,
-            "timestamp": 0.12300918095044494
-        },
-        {
-            "x": 0.8112865108119746,
-            "y": 6.571983401826325,
-            "heading": 0.8196848361236891,
-            "angularVelocity": -1.7530841056771946,
-            "velocityX": 0.8229133780097379,
-            "velocityY": -1.2080072041406207,
-            "timestamp": 0.1845137714256674
-        },
-        {
-            "x": 0.8787770707149783,
-            "y": 6.472119277485466,
-            "heading": 0.6843886163577808,
-            "angularVelocity": -2.199774337501089,
-            "velocityX": 1.0973255716610744,
-            "velocityY": -1.6236857049083067,
-            "timestamp": 0.24601836190088988
-        },
-        {
-            "x": 0.9632585614164454,
-            "y": 6.3460508217508655,
-            "heading": 0.5331826539280757,
-            "angularVelocity": -2.4584500321259677,
-            "velocityX": 1.373580249030372,
-            "velocityY": -2.0497405927024026,
-            "timestamp": 0.3075229523761124
-        },
-        {
-            "x": 1.0643372089754837,
-            "y": 6.193350092274746,
-            "heading": 0.39338059647753487,
-            "angularVelocity": -2.273034522632274,
-            "velocityX": 1.6434325759759127,
-            "velocityY": -2.4827533733053033,
-            "timestamp": 0.3690275428513349
-        },
-        {
-            "x": 1.1776993188507112,
-            "y": 6.018028259717655,
-            "heading": 0.3265019863738018,
-            "angularVelocity": -1.087375911082207,
-            "velocityX": 1.843148763357691,
-            "velocityY": -2.85054873469518,
-            "timestamp": 0.4305321333265574
-        },
-        {
-            "x": 1.3085730997897271,
-            "y": 5.825657422218107,
-            "heading": 0.3265019166878518,
-            "angularVelocity": -0.0000011330203075824657,
-            "velocityX": 2.1278701301448275,
-            "velocityY": -3.1277476366100623,
-            "timestamp": 0.4920367238017799
-        },
-        {
-            "x": 1.4394483094166801,
-            "y": 5.633287556689,
-            "heading": 0.32650184700859936,
-            "angularVelocity": -0.00000113291141146818,
-            "velocityX": 2.1278933591091382,
-            "velocityY": -3.127731833392264,
-            "timestamp": 0.5535413142770024
-        },
-        {
-            "x": 1.5703235190623746,
-            "y": 5.4409176911726425,
-            "heading": 0.32650177732934843,
-            "angularVelocity": -0.0000011329113877746253,
-            "velocityX": 2.127893359413853,
-            "velocityY": -3.1277318331849595,
-            "timestamp": 0.6150459047522249
-        },
-        {
-            "x": 1.7011987286842072,
-            "y": 5.24854782564005,
-            "heading": 0.32650170765009895,
-            "angularVelocity": -0.0000011329113643360664,
-            "velocityX": 2.127893359025885,
-            "velocityY": -3.1277318334489093,
-            "timestamp": 0.6765504952274474
-        },
-        {
-            "x": 1.8320721192807423,
-            "y": 5.056176722584032,
-            "heading": 0.32650163796198384,
-            "angularVelocity": -0.000001133055510645955,
-            "velocityX": 2.1278637835863385,
-            "velocityY": -3.1277519542791743,
-            "timestamp": 0.7380550857026699
-        },
-        {
-            "x": 1.9454000953072241,
-            "y": 4.881259770298511,
-            "heading": 0.26077643177319376,
-            "angularVelocity": -1.0686227756490447,
-            "velocityX": 1.8425937828516514,
-            "velocityY": -2.843965807007333,
-            "timestamp": 0.7995596761778924
-        },
-        {
-            "x": 2.046443192812682,
-            "y": 4.7300582879948045,
-            "heading": 0.13106133632258532,
-            "angularVelocity": -2.1090311218780498,
-            "velocityX": 1.6428545694676804,
-            "velocityY": -2.4583771899858244,
-            "timestamp": 0.8610642666531149
-        },
-        {
-            "x": 2.131123897414369,
-            "y": 4.604963233646066,
-            "heading": -0.022718028385538325,
-            "angularVelocity": -2.50029084853552,
-            "velocityX": 1.376819257674782,
-            "velocityY": -2.0339141092100017,
-            "timestamp": 0.9225688571283374
-        },
-        {
-            "x": 2.198697305455373,
-            "y": 4.506052899677577,
-            "heading": -0.16200255999945734,
-            "angularVelocity": -2.264620096446796,
-            "velocityX": 1.0986725953118257,
-            "velocityY": -1.6081780758842066,
-            "timestamp": 0.9840734476035599
-        },
-        {
-            "x": 2.249342546290196,
-            "y": 4.432626729162034,
-            "heading": -0.2740984297313893,
-            "angularVelocity": -1.822561029442031,
-            "velocityX": 0.8234383879887088,
-            "velocityY": -1.1938323619132651,
-            "timestamp": 1.0455780380787822
-        },
-        {
-            "x": 2.283112085041758,
-            "y": 4.38410205128396,
-            "heading": -0.35252590091377134,
-            "angularVelocity": -1.2751482544051254,
-            "velocityX": 0.549057208423595,
-            "velocityY": -0.788960263016835,
-            "timestamp": 1.1070826285540047
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.393,
-            "angularVelocity": -0.6580663129938888,
-            "velocityX": 0.2745797480766085,
-            "velocityY": -0.39187402269867344,
-            "timestamp": 1.1685872190292272
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.393,
-            "angularVelocity": 0,
-            "velocityX": 1.1335977805694889e-32,
-            "velocityY": 0,
-            "timestamp": 1.2300918095044497
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C2.1.traj
deleted file mode 100644
index 50686485..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C2.1.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -5.382102664363315e-41,
-            "angularVelocity": -2.8652892976895567e-45,
-            "velocityX": 1.592735858156421e-40,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 7.383499994424153,
-            "heading": 1.659495816830368e-21,
-            "angularVelocity": 1.658316243120447e-20,
-            "velocityX": 2.780383744489643e-33,
-            "velocityY": -0.8344065481228213,
-            "timestamp": 0.10007112931184184
-        },
-        {
-            "x": 7.68,
-            "y": 7.216499986370153,
-            "heading": 4.9512387638524226e-21,
-            "angularVelocity": 3.289403167370527e-20,
-            "velocityX": 3.618624574631574e-33,
-            "velocityY": -1.668813065290736,
-            "timestamp": 0.20014225862368368
-        },
-        {
-            "x": 7.68,
-            "y": 6.9659999820333836,
-            "heading": 9.801760970151702e-21,
-            "angularVelocity": 4.8470744390330394e-20,
-            "velocityX": 3.439292531132709e-33,
-            "velocityY": -2.503219520548835,
-            "timestamp": 0.30021338793552554
-        },
-        {
-            "x": 7.68,
-            "y": 6.632,
-            "heading": 1.506109654033251e-20,
-            "angularVelocity": 5.255597191401202e-20,
-            "velocityX": 4.639675238422859e-33,
-            "velocityY": -3.3376257900775017,
-            "timestamp": 0.40028451724736736
-        },
-        {
-            "x": 7.68,
-            "y": 6.298000017966616,
-            "heading": 5.3586032763760104e-21,
-            "angularVelocity": -9.69559666912617e-20,
-            "velocityX": 4.935439546802769e-33,
-            "velocityY": -3.3376257900775017,
-            "timestamp": 0.5003556465592092
-        },
-        {
-            "x": 7.68,
-            "y": 6.047500013629846,
-            "heading": 1.8760590350448333e-21,
-            "angularVelocity": -3.480068842301423e-20,
-            "velocityX": 4.701730454770122e-33,
-            "velocityY": -2.5032195205488352,
-            "timestamp": 0.600426775871051
-        },
-        {
-            "x": 7.68,
-            "y": 5.880500005575846,
-            "heading": 3.168722428707802e-22,
-            "angularVelocity": -1.5580785203142817e-20,
-            "velocityX": 3.49539235617723e-33,
-            "velocityY": -1.668813065290736,
-            "timestamp": 0.7004979051828928
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 5.881496870004905e-41,
-            "angularVelocity": -3.1664700918333573e-21,
-            "velocityX": 3.364378386644386e-33,
-            "velocityY": -0.8344065481228213,
-            "timestamp": 0.8005690344947346
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 9.145026397391945e-42,
-            "angularVelocity": -4.963805231001231e-40,
-            "velocityX": -4.870998572760777e-42,
-            "velocityY": 3.489748854003666e-39,
-            "timestamp": 0.9006401638065764
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C3.1.traj
deleted file mode 100644
index 688fbdaa..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C3.1.traj	
+++ /dev/null
@@ -1,139 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 1.0127320738607452e-39,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 7.390554627994461,
-            "heading": -1.4011773737891353e-18,
-            "angularVelocity": -1.4633626501290335e-17,
-            "velocityX": 9.653222160408549e-34,
-            "velocityY": -0.7983807350946673,
-            "timestamp": 0.09575052183150974
-        },
-        {
-            "x": 7.68,
-            "y": 7.237663887017311,
-            "heading": -4.2066729097937526e-18,
-            "angularVelocity": -2.93000547907741e-17,
-            "velocityX": 5.229262005564601e-34,
-            "velocityY": -1.5967614385035824,
-            "timestamp": 0.19150104366301948
-        },
-        {
-            "x": 7.68,
-            "y": 7.0083277831364,
-            "heading": -8.423669065336014e-18,
-            "angularVelocity": -4.4041495279144365e-17,
-            "velocityX": -1.080014219031917e-33,
-            "velocityY": -2.3951420785410225,
-            "timestamp": 0.28725156549452924
-        },
-        {
-            "x": 7.68,
-            "y": 6.702546334555254,
-            "heading": -1.4072329853209785e-17,
-            "angularVelocity": -5.899352482411367e-17,
-            "velocityX": -1.5994734307756055e-33,
-            "velocityY": -3.1935225284643685,
-            "timestamp": 0.38300208732603896
-        },
-        {
-            "x": 7.68,
-            "y": 6.340327800733179,
-            "heading": -3.9014262815434015e-17,
-            "angularVelocity": -2.604887418380216e-16,
-            "velocityX": 4.89416509473128e-33,
-            "velocityY": -3.7829405719528495,
-            "timestamp": 0.4787526091575487
-        },
-        {
-            "x": 7.68,
-            "y": 5.978109266911059,
-            "heading": -3.149786356993182e-17,
-            "angularVelocity": 7.849982539759592e-17,
-            "velocityX": -6.0692119772378415e-33,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.5745031309890585
-        },
-        {
-            "x": 7.68,
-            "y": 5.61589073308894,
-            "heading": -3.435394827038493e-17,
-            "angularVelocity": -2.98283982773363e-17,
-            "velocityX": 7.480170031140856e-33,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.6702536528205683
-        },
-        {
-            "x": 7.68,
-            "y": 5.253672199266821,
-            "heading": -8.183196831726821e-18,
-            "angularVelocity": 2.7332228522691796e-16,
-            "velocityX": -1.1202872015905622e-32,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.766004174652078
-        },
-        {
-            "x": 7.68,
-            "y": 4.891453665444746,
-            "heading": 1.4014689490668664e-17,
-            "angularVelocity": 2.318304474774222e-16,
-            "velocityX": 5.155427490211361e-33,
-            "velocityY": -3.7829405719528495,
-            "timestamp": 0.8617546964835878
-        },
-        {
-            "x": 7.68,
-            "y": 4.5856722168636,
-            "heading": 8.395813027039913e-18,
-            "angularVelocity": -5.868246309956073e-17,
-            "velocityX": -4.168254505442575e-33,
-            "velocityY": -3.1935225284643685,
-            "timestamp": 0.9575052183150976
-        },
-        {
-            "x": 7.68,
-            "y": 4.356336112982689,
-            "heading": 4.1945848336017725e-18,
-            "angularVelocity": -4.387681773072387e-17,
-            "velocityX": 2.1988846402360483e-33,
-            "velocityY": -2.3951420785410225,
-            "timestamp": 1.0532557401466074
-        },
-        {
-            "x": 7.68,
-            "y": 4.203445372005539,
-            "heading": 1.3975677775871127e-18,
-            "angularVelocity": -2.9211507184809557e-17,
-            "velocityX": 3.5559383909284404e-33,
-            "velocityY": -1.5967614385035824,
-            "timestamp": 1.1490062619781172
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -1.4595928576161535e-17,
-            "velocityX": -6.471812078129625e-34,
-            "velocityY": -0.7983807350946673,
-            "timestamp": 1.244756783809627
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 8.546727720642512e-43,
-            "velocityY": 6.233759417908357e-39,
-            "timestamp": 1.3405073056411367
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.1.traj
deleted file mode 100644
index d59fe62f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.1.traj	
+++ /dev/null
@@ -1,175 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 3.5626996813915863e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 7.385155846048237,
-            "heading": 1.781123128781041e-18,
-            "angularVelocity": 1.7977717932873702e-17,
-            "velocityX": 2.837123063321612e-19,
-            "velocityY": -0.8260917454276266,
-            "timestamp": 0.099073927811004
-        },
-        {
-            "x": 7.68,
-            "y": 7.22146754125807,
-            "heading": 5.344190150368438e-18,
-            "angularVelocity": 3.5963720225105045e-17,
-            "velocityX": 5.673750850451309e-19,
-            "velocityY": -1.65218345943065,
-            "timestamp": 0.198147855622008
-        },
-        {
-            "x": 7.68,
-            "y": 6.975935091856216,
-            "heading": 1.0690981078424305e-17,
-            "angularVelocity": 5.396768903659916e-17,
-            "velocityX": 8.509388117154945e-19,
-            "velocityY": -2.47827511058448,
-            "timestamp": 0.29722178343301203
-        },
-        {
-            "x": 7.68,
-            "y": 6.648558516522808,
-            "heading": 1.7824905328944302e-17,
-            "angularVelocity": 7.200607069613354e-17,
-            "velocityX": 1.134205375948437e-18,
-            "velocityY": -3.304366573190878,
-            "timestamp": 0.396295711244016
-        },
-        {
-            "x": 7.68,
-            "y": 6.273767735073314,
-            "heading": 3.2118763957552995e-17,
-            "angularVelocity": 1.4427467391699638e-16,
-            "velocityX": 1.2981920193437937e-18,
-            "velocityY": -3.782940575087071,
-            "timestamp": 0.49536963905502
-        },
-        {
-            "x": 7.68,
-            "y": 5.898976953623795,
-            "heading": 3.194072142377472e-17,
-            "angularVelocity": -1.7970674806511035e-18,
-            "velocityX": 1.2970624694923187e-18,
-            "velocityY": -3.7829405750873115,
-            "timestamp": 0.5944435668660241
-        },
-        {
-            "x": 7.68,
-            "y": 5.524186172174278,
-            "heading": 3.523836351949418e-17,
-            "angularVelocity": 3.3284660945411024e-17,
-            "velocityX": 1.2988038949897902e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.6935174946770281
-        },
-        {
-            "x": 7.68,
-            "y": 5.149395390724759,
-            "heading": 4.1591047407718544e-17,
-            "angularVelocity": 6.412064231794501e-17,
-            "velocityX": 1.299093645390414e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.792591422488032
-        },
-        {
-            "x": 7.68,
-            "y": 4.774604609275242,
-            "heading": 3.0405238206332753e-17,
-            "angularVelocity": -1.1290366142265185e-16,
-            "velocityX": 1.2929821768433727e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.891665350299036
-        },
-        {
-            "x": 7.68,
-            "y": 4.3998138278257235,
-            "heading": 3.8390344063969795e-17,
-            "angularVelocity": 8.059744913789504e-17,
-            "velocityX": 1.304793755296235e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.99073927811004
-        },
-        {
-            "x": 7.68,
-            "y": 4.025023046376205,
-            "heading": 1.0979594277826173e-17,
-            "angularVelocity": -2.766696586253564e-16,
-            "velocityX": 1.307186742413441e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 1.0898132059210441
-        },
-        {
-            "x": 7.68,
-            "y": 3.650232264926687,
-            "heading": -2.14668342522941e-17,
-            "angularVelocity": -3.2749714528425564e-16,
-            "velocityX": 1.3056923664932481e-18,
-            "velocityY": -3.7829405750873115,
-            "timestamp": 1.1888871337320481
-        },
-        {
-            "x": 7.68,
-            "y": 3.2754414834771928,
-            "heading": -1.7689075007663427e-17,
-            "angularVelocity": 3.813071035032691e-17,
-            "velocityX": 1.3012288375156972e-18,
-            "velocityY": -3.782940575087071,
-            "timestamp": 1.2879610615430521
-        },
-        {
-            "x": 7.68,
-            "y": 2.948064908143785,
-            "heading": -1.0623620375651477e-17,
-            "angularVelocity": 7.131497447110055e-17,
-            "velocityX": 1.136256754825581e-18,
-            "velocityY": -3.304366573190878,
-            "timestamp": 1.3870349893540561
-        },
-        {
-            "x": 7.68,
-            "y": 2.70253245874193,
-            "heading": -5.31498515459649e-18,
-            "angularVelocity": 5.35825654501949e-17,
-            "velocityX": 8.521926280380639e-19,
-            "velocityY": -2.4782751105844802,
-            "timestamp": 1.48610891716506
-        },
-        {
-            "x": 7.68,
-            "y": 2.538844153951763,
-            "heading": -1.7723662552803497e-18,
-            "angularVelocity": 3.575732766130284e-17,
-            "velocityX": 5.681284366395801e-19,
-            "velocityY": -1.6521834594306501,
-            "timestamp": 1.585182844976064
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 1.7889330669293064e-17,
-            "velocityX": 2.8406422370407584e-19,
-            "velocityY": -0.8260917454276266,
-            "timestamp": 1.684256772787068
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 1.0461754707969188e-34,
-            "velocityY": 0,
-            "timestamp": 1.783330700598072
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.1.traj
deleted file mode 100644
index 6f64999b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.1.traj	
+++ /dev/null
@@ -1,220 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -8.452820466081124e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 7.3890447295128565,
-            "heading": -6.111572142469688e-20,
-            "angularVelocity": -6.320692328199756e-19,
-            "velocityX": -1.9797111149811777e-21,
-            "velocityY": -0.8062267267095136,
-            "timestamp": 0.09669149868710691
-        },
-        {
-            "x": 7.68,
-            "y": 7.2331341916331855,
-            "heading": -1.8226742072400974e-19,
-            "angularVelocity": -1.2529715733337834e-18,
-            "velocityX": -3.753564400558873e-21,
-            "velocityY": -1.6124534214139752,
-            "timestamp": 0.19338299737421383
-        },
-        {
-            "x": 7.68,
-            "y": 6.999268392550218,
-            "heading": -3.611632829024713e-19,
-            "angularVelocity": -1.850171571559818e-18,
-            "velocityX": -5.1157697830729356e-21,
-            "velocityY": -2.418680052108357,
-            "timestamp": 0.29007449606132074
-        },
-        {
-            "x": 7.68,
-            "y": 6.687447350831621,
-            "heading": -5.912144198792796e-19,
-            "angularVelocity": -2.3792281637439373e-18,
-            "velocityX": -5.242727749103024e-21,
-            "velocityY": -3.224906490772764,
-            "timestamp": 0.38676599474842766
-        },
-        {
-            "x": 7.68,
-            "y": 6.321669157855709,
-            "heading": -4.100561450064452e-17,
-            "angularVelocity": -4.179726307846966e-16,
-            "velocityX": -1.166132844762965e-20,
-            "velocityY": -3.782940568121389,
-            "timestamp": 0.48345749343553457
-        },
-        {
-            "x": 7.68,
-            "y": 5.955890964879757,
-            "heading": -6.040572418063157e-17,
-            "angularVelocity": -2.0063924898701218e-16,
-            "velocityX": -1.9392305823460385e-22,
-            "velocityY": -3.782940568121787,
-            "timestamp": 0.5801489921226415
-        },
-        {
-            "x": 7.68,
-            "y": 5.590112771903805,
-            "heading": -8.327536807485113e-17,
-            "angularVelocity": -2.365217646302656e-16,
-            "velocityX": 3.675624932403311e-22,
-            "velocityY": -3.782940568121787,
-            "timestamp": 0.6768404908097484
-        },
-        {
-            "x": 7.68,
-            "y": 5.224334578927854,
-            "heading": -7.575439540555723e-17,
-            "angularVelocity": 7.778318436801094e-17,
-            "velocityX": -2.1057162419585226e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 0.7735319894968553
-        },
-        {
-            "x": 7.68,
-            "y": 4.858556385951902,
-            "heading": -4.8431591576069396e-17,
-            "angularVelocity": 2.82577105541659e-16,
-            "velocityX": 1.016892835365518e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 0.8702234881839622
-        },
-        {
-            "x": 7.68,
-            "y": 4.49277819297595,
-            "heading": -7.892411277004063e-17,
-            "angularVelocity": -3.1535886409915893e-16,
-            "velocityX": 1.448731313682566e-20,
-            "velocityY": -3.782940568121787,
-            "timestamp": 0.9669149868710691
-        },
-        {
-            "x": 7.68,
-            "y": 4.126999999999999,
-            "heading": -8.668140475632877e-17,
-            "angularVelocity": -8.022723912254832e-17,
-            "velocityX": 2.655349226445793e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 1.063606485558176
-        },
-        {
-            "x": 7.68,
-            "y": 3.7612218070240475,
-            "heading": -4.236036305527668e-17,
-            "angularVelocity": 4.583757859030679e-16,
-            "velocityX": 7.500561292003624e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 1.160297984245283
-        },
-        {
-            "x": 7.68,
-            "y": 3.395443614048096,
-            "heading": -2.060715967072721e-17,
-            "angularVelocity": 2.249753461257506e-16,
-            "velocityX": 1.3311646441840309e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 1.25698948293239
-        },
-        {
-            "x": 7.68,
-            "y": 3.0296654210721448,
-            "heading": -1.7378554757194933e-17,
-            "angularVelocity": 3.3390783650741015e-17,
-            "velocityX": 1.5677048108756238e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 1.353680981619497
-        },
-        {
-            "x": 7.68,
-            "y": 2.663887228096194,
-            "heading": -2.7058170522872628e-18,
-            "angularVelocity": 1.5174796030816139e-16,
-            "velocityX": 2.708497475030379e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 1.450372480306604
-        },
-        {
-            "x": 7.68,
-            "y": 2.2981090351202424,
-            "heading": -9.881529062229235e-18,
-            "angularVelocity": -7.421243963841227e-17,
-            "velocityX": 2.3345387345088647e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 1.547063978993711
-        },
-        {
-            "x": 7.68,
-            "y": 1.9323308421442913,
-            "heading": -6.5997498705085955e-18,
-            "angularVelocity": 3.39407211211041e-17,
-            "velocityX": -6.072343928076645e-20,
-            "velocityY": -3.782940568121787,
-            "timestamp": 1.643755477680818
-        },
-        {
-            "x": 7.68,
-            "y": 1.5665526491683786,
-            "heading": 6.487893960505722e-19,
-            "angularVelocity": 7.496563157826373e-17,
-            "velocityX": 5.228734565126193e-20,
-            "velocityY": -3.782940568121389,
-            "timestamp": 1.7404469763679251
-        },
-        {
-            "x": 7.68,
-            "y": 1.254731607449781,
-            "heading": 3.9009642634108723e-19,
-            "angularVelocity": -2.6754468948553036e-18,
-            "velocityX": 1.8077268140623735e-21,
-            "velocityY": -3.224906490772764,
-            "timestamp": 1.8371384750550321
-        },
-        {
-            "x": 7.68,
-            "y": 1.020865808366814,
-            "heading": 1.9475770164363563e-19,
-            "angularVelocity": -2.020226466917104e-18,
-            "velocityX": 1.3557598034014801e-21,
-            "velocityY": -2.418680052108357,
-            "timestamp": 1.9338299737421392
-        },
-        {
-            "x": 7.68,
-            "y": 0.8649552704871435,
-            "heading": 6.488190339479676e-20,
-            "angularVelocity": -1.3431976959152128e-18,
-            "velocityX": 9.038296351119003e-22,
-            "velocityY": -1.6124534214139752,
-            "timestamp": 2.030521472429246
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -6.710197305720325e-19,
-            "velocityX": 4.519117474728098e-22,
-            "velocityY": -0.8062267267095136,
-            "timestamp": 2.127212971116353
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 1.0656726462878492e-32,
-            "velocityY": 0,
-            "timestamp": 2.2239044698034602
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.1.traj
deleted file mode 100644
index ce3947a6..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -7.649625564275916e-33,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.617797764168564,
-            "y": 7.446201717117849,
-            "heading": 0.013442479376395878,
-            "angularVelocity": 0.15144225686963014,
-            "velocityX": -0.7007670767707749,
-            "velocityY": -0.23431234749607086,
-            "timestamp": 0.08876306820531243
-        },
-        {
-            "x": 7.493368305965553,
-            "y": 7.404602586388856,
-            "heading": 0.03870227011836747,
-            "angularVelocity": 0.28457545746724416,
-            "velocityX": -1.401815650572125,
-            "velocityY": -0.4686535917478179,
-            "timestamp": 0.17752613641062487
-        },
-        {
-            "x": 7.30667604944407,
-            "y": 7.3422008161122045,
-            "heading": 0.0730276629215222,
-            "angularVelocity": 0.38670804756608035,
-            "velocityX": -2.1032650210965835,
-            "velocityY": -0.7030150212056434,
-            "timestamp": 0.2662892046159373
-        },
-        {
-            "x": 7.057674574154849,
-            "y": 7.259002273186937,
-            "heading": 0.11075765125141449,
-            "angularVelocity": 0.42506403948785515,
-            "velocityX": -2.8052373619302045,
-            "velocityY": -0.9373103544914424,
-            "timestamp": 0.35505227282124974
-        },
-        {
-            "x": 6.746464870184761,
-            "y": 7.155111739606062,
-            "heading": 0.13339737013954753,
-            "angularVelocity": 0.25505786749611997,
-            "velocityX": -3.5060719538216505,
-            "velocityY": -1.170425219424191,
-            "timestamp": 0.44381534102656217
-        },
-        {
-            "x": 6.428047828205506,
-            "y": 7.048517057389171,
-            "heading": 0.13339738106725169,
-            "angularVelocity": 1.2311157711164836e-7,
-            "velocityX": -3.5872694400628884,
-            "velocityY": -1.2008900139638257,
-            "timestamp": 0.5325784092318746
-        },
-        {
-            "x": 6.109630786224927,
-            "y": 6.941922375176235,
-            "heading": 0.1333973919949535,
-            "angularVelocity": 1.23111550991103e-7,
-            "velocityX": -3.587269440077816,
-            "velocityY": -1.200890013919235,
-            "timestamp": 0.621341477437187
-        },
-        {
-            "x": 5.798381397539494,
-            "y": 6.838018370585249,
-            "heading": 0.15626352052401435,
-            "angularVelocity": 0.2576085864583032,
-            "velocityX": -3.5065190397107555,
-            "velocityY": -1.1705769831075976,
-            "timestamp": 0.7101045456424995
-        },
-        {
-            "x": 5.5493631968567065,
-            "y": 6.754814060290535,
-            "heading": 0.1928079538664876,
-            "angularVelocity": 0.4117076401528937,
-            "velocityX": -2.8054257893230465,
-            "velocityY": -0.9373753293689796,
-            "timestamp": 0.7988676138478119
-        },
-        {
-            "x": 5.362653942099421,
-            "y": 6.692406067264648,
-            "heading": 0.22585011955614995,
-            "angularVelocity": 0.37225127926004886,
-            "velocityX": -2.1034565223155943,
-            "velocityY": -0.703085126367409,
-            "timestamp": 0.8876306820531243
-        },
-        {
-            "x": 5.238210383284219,
-            "y": 6.650801525067523,
-            "heading": 0.25010704549903756,
-            "angularVelocity": 0.2732772360607878,
-            "velocityX": -1.4019745073178984,
-            "velocityY": -0.4687145570597535,
-            "timestamp": 0.9763937502584368
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": 0.1452513388947291,
-            "velocityX": -0.7008588655398115,
-            "velocityY": -0.2343488737838604,
-            "timestamp": 1.0651568184637492
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": -2.2743517080702824e-31,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 1.1539198866690616
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.1.traj
deleted file mode 100644
index f95228f2..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -6.443666081756986e-29,
-            "angularVelocity": 1.4558689567742243e-28,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.593939912495806,
-            "y": 7.445359803187238,
-            "heading": -0.04618112566514864,
-            "angularVelocity": -0.44533178373248467,
-            "velocityX": -0.8298908206418381,
-            "velocityY": -0.2086797865609081,
-            "timestamp": 0.10370049332228601
-        },
-        {
-            "x": 7.421570715628637,
-            "y": 7.402025139488172,
-            "heading": -0.13159426902714605,
-            "angularVelocity": -0.823652237569843,
-            "velocityX": -1.662182997832739,
-            "velocityY": -0.41788290788915955,
-            "timestamp": 0.20740098664457202
-        },
-        {
-            "x": 7.162505124622784,
-            "y": 7.336932458231965,
-            "heading": -0.24235041158328322,
-            "angularVelocity": -1.0680387239038096,
-            "velocityX": -2.4982098224038025,
-            "velocityY": -0.627698858229146,
-            "timestamp": 0.311101479966858
-        },
-        {
-            "x": 6.816624735331939,
-            "y": 7.250238713519174,
-            "heading": -0.3315039987658666,
-            "angularVelocity": -0.8597219196007858,
-            "velocityX": -3.3353784365894983,
-            "velocityY": -0.8360012757447058,
-            "timestamp": 0.41480197328914403
-        },
-        {
-            "x": 6.436297385949381,
-            "y": 7.154089090014683,
-            "heading": -0.3315040257643058,
-            "angularVelocity": -2.603501519346576e-7,
-            "velocityX": -3.6675558350581356,
-            "velocityY": -0.927185786914993,
-            "timestamp": 0.5185024666114301
-        },
-        {
-            "x": 6.055970043958053,
-            "y": 7.05793943727301,
-            "heading": -0.33150405276269507,
-            "angularVelocity": -2.60349670609762e-7,
-            "velocityX": -3.6675557637833287,
-            "velocityY": -0.9271860688537472,
-            "timestamp": 0.6222029599337161
-        },
-        {
-            "x": 5.675642817406181,
-            "y": 6.961789327902759,
-            "heading": -0.33150407976108204,
-            "angularVelocity": -2.6034964813703677e-7,
-            "velocityX": -3.6675546505826837,
-            "velocityY": -0.9271904721942118,
-            "timestamp": 0.7259034532560021
-        },
-        {
-            "x": 5.306931756724127,
-            "y": 6.827827408155551,
-            "heading": -0.331504107569482,
-            "angularVelocity": -2.6816073038858223e-7,
-            "velocityX": -3.5555381548297533,
-            "velocityY": -1.2918156457642633,
-            "timestamp": 0.8296039465782882
-        },
-        {
-            "x": 4.97827117667767,
-            "y": 6.613640897314209,
-            "heading": -0.3315041419146958,
-            "angularVelocity": -3.311962425493206e-7,
-            "velocityX": -3.1693251354651193,
-            "velocityY": -2.065433866121419,
-            "timestamp": 0.9333044399005742
-        },
-        {
-            "x": 4.706831845168329,
-            "y": 6.330419557819514,
-            "heading": -0.3315041418010325,
-            "angularVelocity": 1.0960729474265148e-9,
-            "velocityX": -2.6175317282796997,
-            "velocityY": -2.7311474653691747,
-            "timestamp": 1.0370049332228601
-        },
-        {
-            "x": 4.518922560808715,
-            "y": 6.028421366315536,
-            "heading": -0.2325817741817243,
-            "angularVelocity": 0.9539237900428917,
-            "velocityX": -1.8120384806234937,
-            "velocityY": -2.912215572257732,
-            "timestamp": 1.1407054265451462
-        },
-        {
-            "x": 4.388098262013575,
-            "y": 5.795527878516759,
-            "heading": -0.12496957799500227,
-            "angularVelocity": 1.0377211596504115,
-            "velocityX": -1.2615590784949995,
-            "velocityY": -2.245828157007679,
-            "timestamp": 1.2444059198674322
-        },
-        {
-            "x": 4.304052100915554,
-            "y": 5.638778154553602,
-            "heading": -0.0435889655527213,
-            "angularVelocity": 0.7847659141732163,
-            "velocityX": -0.8104702148022851,
-            "velocityY": -1.5115619891604988,
-            "timestamp": 1.3481064131897182
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 7.875266725539043e-28,
-            "angularVelocity": 0.42033518025077415,
-            "velocityX": -0.39587179964482383,
-            "velocityY": -0.7596700076322139,
-            "timestamp": 1.4518069065120043
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -1.3117957600781845e-26,
-            "velocityX": -2.8963805377195657e-28,
-            "velocityY": 0,
-            "timestamp": 1.5555073998342903
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S3.1.traj
deleted file mode 100644
index f3e004b9..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S3.1.traj	
+++ /dev/null
@@ -1,265 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 3.237981257278863e-31,
-            "velocityX": 3.6994687797817426e-32,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.663124715083084,
-            "y": 7.408544235319849,
-            "heading": 0.027485198347684953,
-            "angularVelocity": 0.3204918925418427,
-            "velocityX": -0.19677471240300645,
-            "velocityY": -0.6816250119548423,
-            "timestamp": 0.08575941852911824
-        },
-        {
-            "x": 7.629347887601235,
-            "y": 7.291532761279695,
-            "heading": 0.07929730291499706,
-            "angularVelocity": 0.6041564350126787,
-            "velocityX": -0.3938556028149893,
-            "velocityY": -1.3644154315298311,
-            "timestamp": 0.17151883705823648
-        },
-        {
-            "x": 7.578636110956943,
-            "y": 7.115823334781324,
-            "heading": 0.15015093083574435,
-            "angularVelocity": 0.8261906288076145,
-            "velocityX": -0.5913260317532774,
-            "velocityY": -2.048864480566788,
-            "timestamp": 0.25727825558735473
-        },
-        {
-            "x": 7.510961813450331,
-            "y": 6.881225838812792,
-            "heading": 0.22908991381513497,
-            "angularVelocity": 0.9204701283345157,
-            "velocityX": -0.7891179612374983,
-            "velocityY": -2.7355303941208287,
-            "timestamp": 0.34303767411647296
-        },
-        {
-            "x": 7.426596388399054,
-            "y": 6.588221528517866,
-            "heading": 0.2780284674480044,
-            "angularVelocity": 0.5706493172671544,
-            "velocityX": -0.9837453016618997,
-            "velocityY": -3.4165846191627605,
-            "timestamp": 0.4287970926455912
-        },
-        {
-            "x": 7.336364964950643,
-            "y": 6.276599275391096,
-            "heading": 0.2780284905344079,
-            "angularVelocity": 2.691996271350766e-7,
-            "velocityX": -1.0521459333096546,
-            "velocityY": -3.6336796409243712,
-            "timestamp": 0.5145565111747095
-        },
-        {
-            "x": 7.246133490581996,
-            "y": 5.964977037008454,
-            "heading": 0.27802851362075814,
-            "angularVelocity": 2.6919900573505555e-7,
-            "velocityX": -1.0521465270664079,
-            "velocityY": -3.633679469000075,
-            "timestamp": 0.6003159297038277
-        },
-        {
-            "x": 7.155901958204915,
-            "y": 5.653354815422387,
-            "heading": 0.2780285367070952,
-            "angularVelocity": 2.6919885218329885e-7,
-            "velocityX": -1.0521472034753196,
-            "velocityY": -3.633679273143183,
-            "timestamp": 0.686075348232946
-        },
-        {
-            "x": 7.06567006903972,
-            "y": 5.341732697146042,
-            "heading": 0.27802855979342844,
-            "angularVelocity": 2.6919880800587287e-7,
-            "velocityX": -1.052151363812688,
-            "velocityY": -3.6336780684973866,
-            "timestamp": 0.7718347667620643
-        },
-        {
-            "x": 6.941702537617662,
-            "y": 5.041929112902265,
-            "heading": 0.2780285835706332,
-            "angularVelocity": 2.772547333115093e-7,
-            "velocityX": -1.445526725207054,
-            "velocityY": -3.495867735413615,
-            "timestamp": 0.8575941852911826
-        },
-        {
-            "x": 6.763533066668612,
-            "y": 4.770809627708675,
-            "heading": 0.2780286115416881,
-            "angularVelocity": 3.2615723612466423e-7,
-            "velocityX": -2.0775498948673006,
-            "velocityY": -3.1613960290733045,
-            "timestamp": 0.9433536038203009
-        },
-        {
-            "x": 6.537527544905599,
-            "y": 4.538061625407243,
-            "heading": 0.2780286466069666,
-            "angularVelocity": 4.0887962009830865e-7,
-            "velocityX": -2.6353434484432303,
-            "velocityY": -2.713964323608448,
-            "timestamp": 1.0291130223494191
-        },
-        {
-            "x": 6.274440682462666,
-            "y": 4.354088811409382,
-            "heading": 0.26884633764727317,
-            "angularVelocity": -0.10707055991260324,
-            "velocityX": -3.0677314160380695,
-            "velocityY": -2.145219932145364,
-            "timestamp": 1.1148724408785373
-        },
-        {
-            "x": 5.982323590323029,
-            "y": 4.22411616098336,
-            "heading": 0.2547621599051439,
-            "angularVelocity": -0.16422893232822805,
-            "velocityX": -3.40623918806603,
-            "velocityY": -1.5155495764222278,
-            "timestamp": 1.2006318594076555
-        },
-        {
-            "x": 5.6751302181547745,
-            "y": 4.153390532297334,
-            "heading": 0.22346642432676106,
-            "angularVelocity": -0.3649247641266729,
-            "velocityX": -3.582036555716069,
-            "velocityY": -0.8246980902979518,
-            "timestamp": 1.2863912779367737
-        },
-        {
-            "x": 5.366531848907471,
-            "y": 4.105798244476318,
-            "heading": 0.18319904977282217,
-            "angularVelocity": -0.4695388010388101,
-            "velocityX": -3.5984195618411468,
-            "velocityY": -0.5549511486584576,
-            "timestamp": 1.3721506964658918
-        },
-        {
-            "x": 5.15360011207683,
-            "y": 4.041515222282991,
-            "heading": 0.15622154454817708,
-            "angularVelocity": -0.44121073940112593,
-            "velocityX": -3.482448368249538,
-            "velocityY": -1.051333676582719,
-            "timestamp": 1.4332949588681712
-        },
-        {
-            "x": 4.961585993321137,
-            "y": 3.955708543808847,
-            "heading": 0.09940090208560654,
-            "angularVelocity": -0.9292882149552651,
-            "velocityX": -3.1403456548775974,
-            "velocityY": -1.403348001969601,
-            "timestamp": 1.4944392212704505
-        },
-        {
-            "x": 4.792433392014809,
-            "y": 3.849704139398821,
-            "heading": 0.01980990286006859,
-            "angularVelocity": -1.301692032882722,
-            "velocityX": -2.766450925410501,
-            "velocityY": -1.7336770490843985,
-            "timestamp": 1.5555834836727298
-        },
-        {
-            "x": 4.64716063510186,
-            "y": 3.7242164869664776,
-            "heading": -0.07646566712737501,
-            "angularVelocity": -1.5745642551712946,
-            "velocityX": -2.3759016987918233,
-            "velocityY": -2.0523209783239644,
-            "timestamp": 1.6167277460750091
-        },
-        {
-            "x": 4.526373969972362,
-            "y": 3.5797426645365276,
-            "heading": -0.18496768957846801,
-            "angularVelocity": -1.7745250034621045,
-            "velocityX": -1.975437438999914,
-            "velocityY": -2.362835313629761,
-            "timestamp": 1.6778720084772885
-        },
-        {
-            "x": 4.435318488505818,
-            "y": 3.4263170797911884,
-            "heading": -0.3033986089581413,
-            "angularVelocity": -1.936909772506439,
-            "velocityX": -1.489190937777188,
-            "velocityY": -2.5092392763841707,
-            "timestamp": 1.7390162708795678
-        },
-        {
-            "x": 4.37042278768838,
-            "y": 3.2883178140362905,
-            "heading": -0.39683380701899557,
-            "angularVelocity": -1.5281106417823267,
-            "velocityX": -1.0613538910728308,
-            "velocityY": -2.2569454652502787,
-            "timestamp": 1.800160533281847
-        },
-        {
-            "x": 4.3241368483863285,
-            "y": 3.1745112953889034,
-            "heading": -0.4672868225779202,
-            "angularVelocity": -1.1522424638210744,
-            "velocityX": -0.7569956277749857,
-            "velocityY": -1.8612787884925825,
-            "timestamp": 1.8613047956841264
-        },
-        {
-            "x": 4.292271472981756,
-            "y": 3.08778416242541,
-            "heading": -0.5177338397629753,
-            "angularVelocity": -0.8250490757931602,
-            "velocityX": -0.5211507041318677,
-            "velocityY": -1.418401818193496,
-            "timestamp": 1.9224490580864058
-        },
-        {
-            "x": 4.272405555044274,
-            "y": 3.0293931910282037,
-            "heading": -0.5502025950282511,
-            "angularVelocity": -0.5310188395381721,
-            "velocityX": -0.3249024055075056,
-            "velocityY": -0.9549705745576195,
-            "timestamp": 1.983593320488685
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -0.25836283489389134,
-            "velocityX": -0.15382563587721435,
-            "velocityY": -0.48071871134564137,
-            "timestamp": 2.0447375828909644
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 0,
-            "velocityX": 3.3381626772503296e-32,
-            "velocityY": -2.986960154995716e-30,
-            "timestamp": 2.105881845293244
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S4.1.traj
deleted file mode 100644
index 6d9a6add..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S4.1.traj	
+++ /dev/null
@@ -1,301 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 9.776523821034258e-32,
-            "velocityX": -4.560355515853987e-33,
-            "velocityY": 2.3850698853192112e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 7.67836355889096,
-            "y": 7.400720406813562,
-            "heading": -0.028945523101477338,
-            "angularVelocity": -0.3234212568317029,
-            "velocityX": -0.018284687354286195,
-            "velocityY": -0.7405714954779761,
-            "timestamp": 0.0894978993806123
-        },
-        {
-            "x": 7.675074058245281,
-            "y": 7.268033905727148,
-            "heading": -0.0827237859595526,
-            "angularVelocity": -0.6008885485609575,
-            "velocityX": -0.03675505982201489,
-            "velocityY": -1.4825655351097347,
-            "timestamp": 0.1789957987612246
-        },
-        {
-            "x": 7.670107822881543,
-            "y": 7.068767684711926,
-            "heading": -0.1543450175390465,
-            "angularVelocity": -0.8002560068466026,
-            "velocityX": -0.05548996566520156,
-            "velocityY": -2.226490480718353,
-            "timestamp": 0.2684936981418369
-        },
-        {
-            "x": 7.663417838093003,
-            "y": 6.802742204879413,
-            "heading": -0.22912917227010834,
-            "angularVelocity": -0.8355967597967521,
-            "velocityX": -0.07475018782384167,
-            "velocityY": -2.9724214945109684,
-            "timestamp": 0.3579915975224492
-        },
-        {
-            "x": 7.654753648982881,
-            "y": 6.471615180012679,
-            "heading": -0.2502157544796387,
-            "angularVelocity": -0.23560980040272358,
-            "velocityX": -0.0968088543986332,
-            "velocityY": -3.6998301318619107,
-            "timestamp": 0.4474894969030615
-        },
-        {
-            "x": 7.646619499365586,
-            "y": 6.133147678353669,
-            "heading": -0.250215779137968,
-            "angularVelocity": -2.7551852588992195e-7,
-            "velocityX": -0.09088648642693918,
-            "velocityY": -3.78184855735655,
-            "timestamp": 0.5369873962836738
-        },
-        {
-            "x": 7.638485287213415,
-            "y": 5.7946801781975115,
-            "heading": -0.25021580379628167,
-            "angularVelocity": -2.755183512776458e-7,
-            "velocityX": -0.09088718515702116,
-            "velocityY": -3.7818485405645093,
-            "timestamp": 0.626485295664286
-        },
-        {
-            "x": 7.630351002745582,
-            "y": 5.456212679779282,
-            "heading": -0.2502158284545949,
-            "angularVelocity": -2.7551834628402704e-7,
-            "velocityX": -0.09088799317222593,
-            "velocityY": -3.781848521145859,
-            "timestamp": 0.7159831950448983
-        },
-        {
-            "x": 7.622216641973054,
-            "y": 5.117745183194865,
-            "heading": -0.25021585311291117,
-            "angularVelocity": -2.7551838019723297e-7,
-            "velocityX": -0.09088884575864668,
-            "velocityY": -3.781848500655846,
-            "timestamp": 0.8054810944255105
-        },
-        {
-            "x": 7.614082202627753,
-            "y": 4.779277688498788,
-            "heading": -0.25021587777122717,
-            "angularVelocity": -2.755183772950637e-7,
-            "velocityX": -0.09088972368733664,
-            "velocityY": -3.7818484795565834,
-            "timestamp": 0.8949789938061228
-        },
-        {
-            "x": 7.605947683187911,
-            "y": 4.440810195727643,
-            "heading": -0.2502159024295383,
-            "angularVelocity": -2.755183223713384e-7,
-            "velocityX": -0.09089061861940764,
-            "velocityY": -3.7818484580484664,
-            "timestamp": 0.984476893186735
-        },
-        {
-            "x": 7.597813082423135,
-            "y": 4.1023427049110195,
-            "heading": -0.25021592708784146,
-            "angularVelocity": -2.755182337798815e-7,
-            "velocityX": -0.09089152729921718,
-            "velocityY": -3.781848436209727,
-            "timestamp": 1.0739747925673473
-        },
-        {
-            "x": 7.589678399478048,
-            "y": 3.7638752160694957,
-            "heading": -0.2502159517461411,
-            "angularVelocity": -2.755181940361221e-7,
-            "velocityX": -0.09089244553653594,
-            "velocityY": -3.781848414141052,
-            "timestamp": 1.1634726919479597
-        },
-        {
-            "x": 7.581543633206817,
-            "y": 3.4254077292306317,
-            "heading": -0.2502159764044425,
-            "angularVelocity": -2.7551821391136536e-7,
-            "velocityX": -0.09089337657676429,
-            "velocityY": -3.7818483917644334,
-            "timestamp": 1.252970591328572
-        },
-        {
-            "x": 7.573407384740196,
-            "y": 3.0869402780182864,
-            "heading": -0.2502160010627749,
-            "angularVelocity": -2.7551856112041726e-7,
-            "velocityX": -0.09090993780780313,
-            "velocityY": -3.7818479936934355,
-            "timestamp": 1.3424684907091844
-        },
-        {
-            "x": 7.507089727245593,
-            "y": 2.754933679576005,
-            "heading": -0.25021602759782946,
-            "angularVelocity": -2.964880153822211e-7,
-            "velocityX": -0.7409968049928212,
-            "velocityY": -3.709658000243552,
-            "timestamp": 1.4319663900897968
-        },
-        {
-            "x": 7.3768883522731565,
-            "y": 2.4424052627551904,
-            "heading": -0.2502160612558337,
-            "angularVelocity": -3.7607591242222915e-7,
-            "velocityX": -1.454798111168213,
-            "velocityY": -3.492019577931195,
-            "timestamp": 1.5214642894704091
-        },
-        {
-            "x": 7.1878697455519145,
-            "y": 2.1615167326419513,
-            "heading": -0.2502161148657548,
-            "angularVelocity": -5.990075908840489e-7,
-            "velocityX": -2.111989309574672,
-            "velocityY": -3.1384929932119427,
-            "timestamp": 1.6109621888510215
-        },
-        {
-            "x": 6.94738938516625,
-            "y": 1.9231986374023808,
-            "heading": -0.25021645217340494,
-            "angularVelocity": -0.000003768889018580663,
-            "velocityX": -2.686994466350113,
-            "velocityY": -2.6628345121941095,
-            "timestamp": 1.7004600882316339
-        },
-        {
-            "x": 6.667655944824219,
-            "y": 1.738860011100769,
-            "heading": -0.25893161706240975,
-            "angularVelocity": -0.09737842954215001,
-            "velocityX": -3.125586659329227,
-            "velocityY": -2.059697798242895,
-            "timestamp": 1.7899579876122462
-        },
-        {
-            "x": 6.480451878153193,
-            "y": 1.646736643588724,
-            "heading": -0.268441983388845,
-            "angularVelocity": -0.16928620019223647,
-            "velocityX": -3.3322654479868237,
-            "velocityY": -1.6398122111953808,
-            "timestamp": 1.846137205502623
-        },
-        {
-            "x": 6.284980922842322,
-            "y": 1.5795583069683912,
-            "heading": -0.28291661109989463,
-            "angularVelocity": -0.25765092955355146,
-            "velocityX": -3.47941752575298,
-            "velocityY": -1.1957862558966221,
-            "timestamp": 1.9023164233929999
-        },
-        {
-            "x": 6.086191677834544,
-            "y": 1.5383458560749974,
-            "heading": -0.3070406119029933,
-            "angularVelocity": -0.429411474723066,
-            "velocityX": -3.53848366838567,
-            "velocityY": -0.7335889042423714,
-            "timestamp": 1.9584956412833767
-        },
-        {
-            "x": 5.891572555778559,
-            "y": 1.5225106097486452,
-            "heading": -0.35143250880916643,
-            "angularVelocity": -0.7901836047765775,
-            "velocityX": -3.4642547433776456,
-            "velocityY": -0.2818701811985366,
-            "timestamp": 2.0146748591737533
-        },
-        {
-            "x": 5.713348199791213,
-            "y": 1.5270062964148978,
-            "heading": -0.407385332387855,
-            "angularVelocity": -0.9959701412692558,
-            "velocityX": -3.172425011952234,
-            "velocityY": 0.08002401662169539,
-            "timestamp": 2.07085407706413
-        },
-        {
-            "x": 5.558872974287929,
-            "y": 1.5428236796222032,
-            "heading": -0.4653888399418059,
-            "angularVelocity": -1.0324726781908529,
-            "velocityX": -2.7496862951120815,
-            "velocityY": 0.2815522145247918,
-            "timestamp": 2.1270332949545065
-        },
-        {
-            "x": 5.4303297125098124,
-            "y": 1.5629685460150702,
-            "heading": -0.519265179295127,
-            "angularVelocity": -0.9590083553396718,
-            "velocityX": -2.2880927610801525,
-            "velocityY": 0.35858217948453525,
-            "timestamp": 2.183212512844883
-        },
-        {
-            "x": 5.3280145107455015,
-            "y": 1.583083454211334,
-            "heading": -0.5654730488836437,
-            "angularVelocity": -0.8225082392332849,
-            "velocityX": -1.8212286608183241,
-            "velocityY": 0.3580489182942135,
-            "timestamp": 2.2393917307352598
-        },
-        {
-            "x": 5.251723840078425,
-            "y": 1.6004232571954151,
-            "heading": -0.6018676415285119,
-            "angularVelocity": -0.6478301765590008,
-            "velocityX": -1.3579874112157035,
-            "velocityY": 0.3086515554188953,
-            "timestamp": 2.2955709486256364
-        },
-        {
-            "x": 5.20115218978415,
-            "y": 1.613158164546165,
-            "heading": -0.6270407807035692,
-            "angularVelocity": -0.44808632302748147,
-            "velocityX": -0.9001843064628601,
-            "velocityY": 0.22668359989631479,
-            "timestamp": 2.351750166516013
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -0.2306763921441399,
-            "velocityX": -0.44771342016952703,
-            "velocityY": 0.12178587938311458,
-            "timestamp": 2.4079293844063896
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 3.4924263364045203e-31,
-            "velocityX": 0,
-            "velocityY": 4.738614736798697e-32,
-            "timestamp": 2.464108602296766
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.1.traj
deleted file mode 100644
index 1a381a4d..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 3.136513796099116e-33,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.613270105003341,
-            "y": 7.458186274184093,
-            "heading": 0.022686687743237347,
-            "angularVelocity": 0.25194782208363986,
-            "velocityX": -0.7410712353675238,
-            "velocityY": -0.0978811472565877,
-            "timestamp": 0.09004518298914281
-        },
-        {
-            "x": 7.479732366880497,
-            "y": 7.440537508755927,
-            "heading": 0.06475240251532689,
-            "angularVelocity": 0.4671622998107697,
-            "velocityX": -1.483008126475186,
-            "velocityY": -0.19599899564082535,
-            "timestamp": 0.18009036597828562
-        },
-        {
-            "x": 7.2792817573683966,
-            "y": 7.414022951557291,
-            "heading": 0.12053408331695856,
-            "angularVelocity": 0.6194854510802376,
-            "velocityX": -2.226111412714547,
-            "velocityY": -0.2944583632178676,
-            "timestamp": 0.2701355489674284
-        },
-        {
-            "x": 7.01181645042657,
-            "y": 7.378594749595234,
-            "heading": 0.17807816149187164,
-            "angularVelocity": 0.6390578181383835,
-            "velocityX": -2.9703455316879728,
-            "velocityY": -0.39344916391950197,
-            "timestamp": 0.36018073195657124
-        },
-        {
-            "x": 6.678421151155383,
-            "y": 7.334256434894407,
-            "heading": 0.1920911587056688,
-            "angularVelocity": 0.15562184170903165,
-            "velocityX": -3.7025334193766524,
-            "velocityY": -0.49240073959507324,
-            "timestamp": 0.45022591494571407
-        },
-        {
-            "x": 6.340697514229505,
-            "y": 7.289811670135368,
-            "heading": 0.1920911781237931,
-            "angularVelocity": 2.1564867368321945e-7,
-            "velocityX": -3.7506019279965175,
-            "velocityY": -0.49358292452353475,
-            "timestamp": 0.5402710979348568
-        },
-        {
-            "x": 6.002973877037569,
-            "y": 7.245366907398008,
-            "heading": 0.19209119754191162,
-            "angularVelocity": 2.1564861007875057e-7,
-            "velocityX": -3.750601930951228,
-            "velocityY": -0.49358290207171196,
-            "timestamp": 0.6303162809239996
-        },
-        {
-            "x": 5.665250239845632,
-            "y": 7.200922144660658,
-            "heading": 0.19209121696003015,
-            "angularVelocity": 2.1564860966767442e-7,
-            "velocityX": -3.7506019309512406,
-            "velocityY": -0.493582902071613,
-            "timestamp": 0.7203614639131424
-        },
-        {
-            "x": 5.327526602653695,
-            "y": 7.156477381923307,
-            "heading": 0.19209123637814873,
-            "angularVelocity": 2.1564861026621434e-7,
-            "velocityX": -3.7506019309512415,
-            "velocityY": -0.493582902071613,
-            "timestamp": 0.8104066469022851
-        },
-        {
-            "x": 4.989802965461758,
-            "y": 7.112032619185957,
-            "heading": 0.19209125579626718,
-            "angularVelocity": 2.156486089426075e-7,
-            "velocityX": -3.7506019309512415,
-            "velocityY": -0.49358290207161315,
-            "timestamp": 0.9004518298914279
-        },
-        {
-            "x": 4.652079328269822,
-            "y": 7.067587856448606,
-            "heading": 0.19209127521438563,
-            "angularVelocity": 2.156486089612396e-7,
-            "velocityX": -3.750601930951241,
-            "velocityY": -0.4935829020716131,
-            "timestamp": 0.9904970128805707
-        },
-        {
-            "x": 4.314355691077885,
-            "y": 7.023143093711255,
-            "heading": 0.19209129463250418,
-            "angularVelocity": 2.1564860999442945e-7,
-            "velocityX": -3.7506019309512415,
-            "velocityY": -0.4935829020716131,
-            "timestamp": 1.0805421958697135
-        },
-        {
-            "x": 3.976632053885947,
-            "y": 6.978698330973905,
-            "heading": 0.1920913140506227,
-            "angularVelocity": 2.1564860984546372e-7,
-            "velocityX": -3.750601930951241,
-            "velocityY": -0.49358290207161315,
-            "timestamp": 1.1705873788588563
-        },
-        {
-            "x": 3.638908416694012,
-            "y": 6.934253568236545,
-            "heading": 0.19209133346874124,
-            "angularVelocity": 2.1564860987115903e-7,
-            "velocityX": -3.750601930951227,
-            "velocityY": -0.4935829020717237,
-            "timestamp": 1.2606325618479992
-        },
-        {
-            "x": 3.3011847797995815,
-            "y": 6.8898088032385445,
-            "heading": 0.19209135288686577,
-            "angularVelocity": 2.1564867640744468e-7,
-            "velocityX": -3.7506019276472724,
-            "velocityY": -0.4935829271773418,
-            "timestamp": 1.350677744837142
-        },
-        {
-            "x": 2.967975868183031,
-            "y": 6.845440652800829,
-            "heading": 0.2065131865620181,
-            "angularVelocity": 0.16016218965195775,
-            "velocityX": -3.7004634846122446,
-            "velocityY": -0.492732081438091,
-            "timestamp": 1.440722927826285
-        },
-        {
-            "x": 2.7005740636525677,
-            "y": 6.810001078107782,
-            "heading": 0.266815731322117,
-            "angularVelocity": 0.6696920674520688,
-            "velocityX": -2.9696403033875236,
-            "velocityY": -0.39357546419024264,
-            "timestamp": 1.5307681108154279
-        },
-        {
-            "x": 2.5001864407535797,
-            "y": 6.78347721885534,
-            "heading": 0.32524318999567287,
-            "angularVelocity": 0.6488682318586739,
-            "velocityX": -2.2254119126300136,
-            "velocityY": -0.29456166750908425,
-            "timestamp": 1.6208132938045707
-        },
-        {
-            "x": 2.366700230698382,
-            "y": 6.765819830273621,
-            "heading": 0.3692681816828941,
-            "angularVelocity": 0.48892111966199864,
-            "velocityX": -1.4824358796771253,
-            "velocityY": -0.19609476038099796,
-            "timestamp": 1.7108584767937136
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.26355455704906927,
-            "velocityX": -0.740741797442122,
-            "velocityY": -0.09794894053004785,
-            "timestamp": 1.8009036597828565
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0,
-            "velocityX": 6.927431243507319e-35,
-            "velocityY": 0,
-            "timestamp": 1.8909488427719994
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.1.traj
deleted file mode 100644
index 28a53b93..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.1.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -2.4374604803646449e-32,
-            "velocityY": 2.350988701644575e-38,
-            "timestamp": 0
-        },
-        {
-            "x": 7.602872461914855,
-            "y": 7.439913035887961,
-            "heading": 5.3325436298288445e-18,
-            "angularVelocity": 5.3856284215509377e-17,
-            "velocityX": -0.7789533333445217,
-            "velocityY": -0.2735661154121556,
-            "timestamp": 0.09901432445765257
-        },
-        {
-            "x": 7.448617388685645,
-            "y": 7.385739108696781,
-            "heading": -4.4022234029758545e-18,
-            "angularVelocity": -9.831675377471393e-17,
-            "velocityX": -1.557906636985473,
-            "velocityY": -0.5471322203925048,
-            "timestamp": 0.19802864891530514
-        },
-        {
-            "x": 7.217234786194527,
-            "y": 7.304478220492258,
-            "heading": -7.053417683545123e-18,
-            "angularVelocity": -2.6775864730895626e-17,
-            "velocityX": -2.3368598812192953,
-            "velocityY": -0.8206983045092451,
-            "timestamp": 0.2970429733729577
-        },
-        {
-            "x": 6.9087246720879545,
-            "y": 7.196130377471774,
-            "heading": 2.6673179344101e-18,
-            "angularVelocity": 9.817504407722252e-17,
-            "velocityX": -3.1158129472318725,
-            "velocityY": -1.0942643260352094,
-            "timestamp": 0.3960572978306103
-        },
-        {
-            "x": 6.5553201862538,
-            "y": 7.072015763386003,
-            "heading": 8.41895708879283e-18,
-            "angularVelocity": 5.808896021749495e-17,
-            "velocityX": -3.5692258445423475,
-            "velocityY": -1.2535015995473784,
-            "timestamp": 0.49507162228826285
-        },
-        {
-            "x": 6.2019157004196215,
-            "y": 6.9479011493002245,
-            "heading": -3.69857396739816e-19,
-            "angularVelocity": -8.876306064099344e-17,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.2535015995474599,
-            "timestamp": 0.5940859467459154
-        },
-        {
-            "x": 5.848511214585444,
-            "y": 6.823786535214446,
-            "heading": -8.032367976884792e-18,
-            "angularVelocity": -7.738789939855791e-17,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.2535015995474599,
-            "timestamp": 0.693100271203568
-        },
-        {
-            "x": 5.495106728751266,
-            "y": 6.699671921128667,
-            "heading": -3.490709714785344e-17,
-            "angularVelocity": -2.714226382747745e-16,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.2535015995474599,
-            "timestamp": 0.7921145956612206
-        },
-        {
-            "x": 5.141702242917089,
-            "y": 6.575557307042889,
-            "heading": -1.2741337636064699e-17,
-            "angularVelocity": 2.238641694845762e-16,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.2535015995474599,
-            "timestamp": 0.8911289201188731
-        },
-        {
-            "x": 4.788297757082911,
-            "y": 6.4514426929571105,
-            "heading": -2.2645871045802615e-17,
-            "angularVelocity": -1.0003131833516608e-16,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.2535015995474597,
-            "timestamp": 0.9901432445765257
-        },
-        {
-            "x": 4.4348932712487334,
-            "y": 6.327328078871332,
-            "heading": -7.505924377434873e-18,
-            "angularVelocity": 1.529066299375997e-16,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.25350159954746,
-            "timestamp": 1.0891575690341782
-        },
-        {
-            "x": 4.081488785414556,
-            "y": 6.203213464785553,
-            "heading": -3.3249179069580955e-17,
-            "angularVelocity": -2.5999525658031657e-16,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.25350159954746,
-            "timestamp": 1.1881718934918308
-        },
-        {
-            "x": 3.728084299580378,
-            "y": 6.079098850699775,
-            "heading": -3.31250701341244e-17,
-            "angularVelocity": 1.2534442479544336e-18,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.2535015995474597,
-            "timestamp": 1.2871862179494835
-        },
-        {
-            "x": 3.3746798137461993,
-            "y": 5.9549842366139965,
-            "heading": -3.474935359347948e-17,
-            "angularVelocity": -1.6404530025230332e-17,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.2535015995474599,
-            "timestamp": 1.3862005424071362
-        },
-        {
-            "x": 3.021275327912045,
-            "y": 5.830869622528225,
-            "heading": -2.4652412430964293e-17,
-            "angularVelocity": 1.0197454982216833e-16,
-            "velocityX": -3.5692258445423475,
-            "velocityY": -1.2535015995473784,
-            "timestamp": 1.4852148668647889
-        },
-        {
-            "x": 2.7127652138054734,
-            "y": 5.722521779507741,
-            "heading": 1.414690860074665e-18,
-            "angularVelocity": 2.632659789801892e-16,
-            "velocityX": -3.115812947231872,
-            "velocityY": -1.0942643260352094,
-            "timestamp": 1.5842291913224416
-        },
-        {
-            "x": 2.4813826113143547,
-            "y": 5.641260891303218,
-            "heading": 8.307803002216867e-18,
-            "angularVelocity": 6.961732115429119e-17,
-            "velocityX": -2.336859881219295,
-            "velocityY": -0.8206983045092451,
-            "timestamp": 1.6832435157800942
-        },
-        {
-            "x": 2.3271275380851444,
-            "y": 5.587086964112038,
-            "heading": -3.972937436005726e-18,
-            "angularVelocity": -1.2402993582700068e-16,
-            "velocityX": -1.557906636985473,
-            "velocityY": -0.5471322203925048,
-            "timestamp": 1.782257840237747
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 4.012487544797503e-17,
-            "velocityX": -0.7789533333445219,
-            "velocityY": -0.2735661154121556,
-            "timestamp": 1.8812721646953996
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -5.265297803216144e-35,
-            "velocityY": 0,
-            "timestamp": 1.9802864891530523
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.1.traj
deleted file mode 100644
index 96b5b24b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.1.traj	
+++ /dev/null
@@ -1,274 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": -1.8418609272007183e-28,
-            "velocityX": -1.1316944174618986e-32,
-            "velocityY": -1.0903166764521522e-29,
-            "timestamp": 0
-        },
-        {
-            "x": 7.615058283165484,
-            "y": 7.4500526040017725,
-            "heading": -0.011528934363875379,
-            "angularVelocity": -0.12842736126317786,
-            "velocityX": -0.7234227436435784,
-            "velocityY": -0.18878668917675853,
-            "timestamp": 0.08977007898235294
-        },
-        {
-            "x": 7.485153650546927,
-            "y": 7.41615410854214,
-            "heading": -0.0328638632229319,
-            "angularVelocity": -0.23766191476385995,
-            "velocityX": -1.4470816344507456,
-            "velocityY": -0.3776146333375848,
-            "timestamp": 0.17954015796470588
-        },
-        {
-            "x": 7.290257872847477,
-            "y": 7.365300387326372,
-            "heading": -0.06109799257845879,
-            "angularVelocity": -0.3145160355888266,
-            "velocityX": -2.17105498746146,
-            "velocityY": -0.5664885426438214,
-            "timestamp": 0.2693102369470588
-        },
-        {
-            "x": 7.030342832843462,
-            "y": 7.297491054945299,
-            "heading": -0.0902650461777,
-            "angularVelocity": -0.32490840968750134,
-            "velocityX": -2.895341554228826,
-            "velocityY": -0.7553667452421856,
-            "timestamp": 0.35908031592941175
-        },
-        {
-            "x": 6.705594231461252,
-            "y": 7.212800171241254,
-            "heading": -0.10097705422378679,
-            "angularVelocity": -0.11932715407194204,
-            "velocityX": -3.617559492690666,
-            "velocityY": -0.9434199531077018,
-            "timestamp": 0.4488503949117647
-        },
-        {
-            "x": 6.377014064882571,
-            "y": 7.1270111914036525,
-            "heading": -0.10097706428057987,
-            "angularVelocity": -1.120283753063034e-7,
-            "velocityX": -3.6602414780458985,
-            "velocityY": -0.9556522708907337,
-            "timestamp": 0.5386204738941176
-        },
-        {
-            "x": 6.048433901032778,
-            "y": 7.0412222011141505,
-            "heading": -0.10097707433737516,
-            "angularVelocity": -1.1202840770590317e-7,
-            "velocityX": -3.660241447647229,
-            "velocityY": -0.9556523873204973,
-            "timestamp": 0.6283905528764705
-        },
-        {
-            "x": 5.719853740215168,
-            "y": 6.955433199211206,
-            "heading": -0.10097708439415136,
-            "angularVelocity": -1.1202819821797078e-7,
-            "velocityX": -3.6602414138700015,
-            "velocityY": -0.9556525166893554,
-            "timestamp": 0.7181606318588234
-        },
-        {
-            "x": 5.391273582926602,
-            "y": 6.86964418379178,
-            "heading": -0.100977094450907,
-            "angularVelocity": -1.1202797265919916e-7,
-            "velocityX": -3.660241374557919,
-            "velocityY": -0.9556526672572584,
-            "timestamp": 0.8079307108411763
-        },
-        {
-            "x": 5.062693429765362,
-            "y": 6.783855152564382,
-            "heading": -0.10097710450764096,
-            "angularVelocity": -1.1202773594839044e-7,
-            "velocityX": -3.6602413285812334,
-            "velocityY": -0.9556528433514181,
-            "timestamp": 0.8977007898235292
-        },
-        {
-            "x": 4.734113281444647,
-            "y": 6.698066102797359,
-            "heading": -0.10097711456435234,
-            "angularVelocity": -1.1202748993058552e-7,
-            "velocityX": -3.6602412746598207,
-            "velocityY": -0.9556530498750426,
-            "timestamp": 0.9874708688058821
-        },
-        {
-            "x": 4.4055331387963745,
-            "y": 6.612277031304317,
-            "heading": -0.10097712462104143,
-            "angularVelocity": -1.1202724880866421e-7,
-            "velocityX": -3.660241211471189,
-            "velocityY": -0.9556532918937597,
-            "timestamp": 1.077240947788235
-        },
-        {
-            "x": 4.076953002755506,
-            "y": 6.526487934504167,
-            "heading": -0.10097713467770983,
-            "angularVelocity": -1.1202702659530083e-7,
-            "velocityX": -3.6602411378675055,
-            "velocityY": -0.9556535738043043,
-            "timestamp": 1.167011026770588
-        },
-        {
-            "x": 3.748372874246301,
-            "y": 6.440698808855463,
-            "heading": -0.10097714473442117,
-            "angularVelocity": -1.1202751392033635e-7,
-            "velocityX": -3.6602410539684915,
-            "velocityY": -0.9556538951629009,
-            "timestamp": 1.256781105752941
-        },
-        {
-            "x": 3.4197928479820465,
-            "y": 6.354909291415878,
-            "heading": -0.10097715479114391,
-            "angularVelocity": -1.120276421337113e-7,
-            "velocityX": -3.6602399150000573,
-            "velocityY": -0.9556582595606669,
-            "timestamp": 1.346551184735294
-        },
-        {
-            "x": 3.1047619528127512,
-            "y": 6.228101525660344,
-            "heading": -0.10097716526412484,
-            "angularVelocity": -1.1666457370126234e-7,
-            "velocityX": -3.5093084326123236,
-            "velocityY": -1.412583871971558,
-            "timestamp": 1.436321263717647
-        },
-        {
-            "x": 2.8208656132222933,
-            "y": 6.041748259045348,
-            "heading": -0.1009771779112871,
-            "angularVelocity": -1.4088397962304153e-7,
-            "velocityX": -3.1624828986324123,
-            "velocityY": -2.0758950947495576,
-            "timestamp": 1.5260913427
-        },
-        {
-            "x": 2.5792187192315943,
-            "y": 5.803145197833315,
-            "heading": -0.10097728674036179,
-            "angularVelocity": -0.0000012123108078932896,
-            "velocityX": -2.691842278966871,
-            "velocityY": -2.65793529332427,
-            "timestamp": 1.615861421682353
-        },
-        {
-            "x": 2.3963290188287294,
-            "y": 5.533479989641919,
-            "heading": -0.13644335482170092,
-            "angularVelocity": -0.3950767169047361,
-            "velocityX": -2.037312459521214,
-            "velocityY": -3.003954226707092,
-            "timestamp": 1.705631500664706
-        },
-        {
-            "x": 2.279822826385498,
-            "y": 5.254662990570068,
-            "heading": -0.18962986506951118,
-            "angularVelocity": -0.5924748073105779,
-            "velocityX": -1.2978287839789375,
-            "velocityY": -3.105901233821009,
-            "timestamp": 1.795401579647059
-        },
-        {
-            "x": 2.2328990354823257,
-            "y": 5.0773593431323425,
-            "heading": -0.225508532252886,
-            "angularVelocity": -0.6284306472549296,
-            "velocityX": -0.8218908506157174,
-            "velocityY": -3.1055514229095325,
-            "timestamp": 1.8524940644563022
-        },
-        {
-            "x": 2.2122650091837905,
-            "y": 4.906948206481987,
-            "heading": -0.2618449581428327,
-            "angularVelocity": -0.636448492486558,
-            "velocityX": -0.3614140524419242,
-            "velocityY": -2.9848260628208765,
-            "timestamp": 1.9095865492655453
-        },
-        {
-            "x": 2.2138710973194917,
-            "y": 4.752151212947765,
-            "heading": -0.29639527524079207,
-            "angularVelocity": -0.6051640108688915,
-            "velocityX": 0.02813134059521416,
-            "velocityY": -2.7113374737719043,
-            "timestamp": 1.9666790340747884
-        },
-        {
-            "x": 2.229883400142308,
-            "y": 4.620372877512837,
-            "heading": -0.32691110500805654,
-            "angularVelocity": -0.5344981895462751,
-            "velocityX": 0.280462531565201,
-            "velocityY": -2.308155545781648,
-            "timestamp": 2.0237715188840317
-        },
-        {
-            "x": 2.251883940293385,
-            "y": 4.51506115985361,
-            "heading": -0.3519685302004407,
-            "angularVelocity": -0.4388918309689961,
-            "velocityX": 0.38534914402167997,
-            "velocityY": -1.844581086479138,
-            "timestamp": 2.080864003693275
-        },
-        {
-            "x": 2.273515445313811,
-            "y": 4.436872238113893,
-            "heading": -0.37095335253129014,
-            "angularVelocity": -0.33252751906728534,
-            "velocityX": 0.3788853312819565,
-            "velocityY": -1.3695133781784317,
-            "timestamp": 2.1379564885025184
-        },
-        {
-            "x": 2.2905088224909966,
-            "y": 4.385405157743103,
-            "heading": -0.3836505931421081,
-            "angularVelocity": -0.22239775783770355,
-            "velocityX": 0.2976464806908316,
-            "velocityY": -0.9014685653065743,
-            "timestamp": 2.1950489733117617
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.11121265573212626,
-            "velocityX": 0.16624215149881386,
-            "velocityY": -0.4449825196428863,
-            "timestamp": 2.252141458121005
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -5.463060715454085e-29,
-            "velocityX": 0,
-            "velocityY": -5.448481045128469e-30,
-            "timestamp": 2.3092339429302484
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.1.traj
deleted file mode 100644
index 90a593eb..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.1.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -3.817834980565894e-40,
-            "angularVelocity": -1.9855221195343998e-39,
-            "velocityX": -9.621145750064654e-40,
-            "velocityY": 1.4575297691012433e-39,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 5.880500005575846,
-            "heading": 3.479114156010015e-22,
-            "angularVelocity": 3.4766412561552746e-21,
-            "velocityX": 2.7068848151306367e-33,
-            "velocityY": 0.8344065481228213,
-            "timestamp": 0.10007112931184182
-        },
-        {
-            "x": 7.68,
-            "y": 6.047500013629847,
-            "heading": 1.081691491825702e-21,
-            "angularVelocity": 7.332585052389997e-21,
-            "velocityX": 5.1589068200097806e-33,
-            "velocityY": 1.6688130652907358,
-            "timestamp": 0.20014225862368365
-        },
-        {
-            "x": 7.68,
-            "y": 6.298000017966616,
-            "heading": 2.2511586271387426e-21,
-            "angularVelocity": 1.1686358860616172e-20,
-            "velocityX": 5.8498916949463956e-33,
-            "velocityY": 2.503219520548835,
-            "timestamp": 0.3002133879355255
-        },
-        {
-            "x": 7.68,
-            "y": 6.632,
-            "heading": 4.481673966100434e-21,
-            "angularVelocity": 2.228929929167172e-20,
-            "velocityX": 6.576791251337367e-33,
-            "velocityY": 3.3376257900775013,
-            "timestamp": 0.4002845172473673
-        },
-        {
-            "x": 7.68,
-            "y": 6.9659999820333836,
-            "heading": 1.4391637904192512e-21,
-            "angularVelocity": -3.0403475801105514e-20,
-            "velocityX": 1.0509973010204301e-32,
-            "velocityY": 3.3376257900775013,
-            "timestamp": 0.5003556465592092
-        },
-        {
-            "x": 7.68,
-            "y": 7.216499986370152,
-            "heading": 4.7457500532831525e-22,
-            "angularVelocity": -9.63903176962576e-21,
-            "velocityX": 7.983324746463204e-33,
-            "velocityY": 2.503219520548835,
-            "timestamp": 0.600426775871051
-        },
-        {
-            "x": 7.68,
-            "y": 7.383499994424153,
-            "heading": 1.0909640932435146e-22,
-            "angularVelocity": -3.652188228166023e-21,
-            "velocityX": 7.341551448907972e-33,
-            "velocityY": 1.6688130652907358,
-            "timestamp": 0.7004979051828928
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -9.42965601107167e-40,
-            "angularVelocity": -1.09018866193586e-21,
-            "velocityX": 7.44470103227488e-33,
-            "velocityY": 0.8344065481228213,
-            "timestamp": 0.8005690344947346
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 8.249839679439821e-43,
-            "angularVelocity": 9.431229878048832e-39,
-            "velocityX": 1.012759498983901e-41,
-            "velocityY": 5.142787784847508e-39,
-            "timestamp": 0.9006401638065764
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.1.traj
deleted file mode 100644
index 74357de2..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.1.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -5.382102664363315e-41,
-            "angularVelocity": -2.8652892976895567e-45,
-            "velocityX": 1.592735858156421e-40,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 5.713499994424153,
-            "heading": 1.659495816830368e-21,
-            "angularVelocity": 1.658316243120447e-20,
-            "velocityX": 2.780383744489643e-33,
-            "velocityY": -0.8344065481228213,
-            "timestamp": 0.10007112931184184
-        },
-        {
-            "x": 7.68,
-            "y": 5.546499986370153,
-            "heading": 4.9512387638524226e-21,
-            "angularVelocity": 3.289403167370527e-20,
-            "velocityX": 3.618624574631574e-33,
-            "velocityY": -1.668813065290736,
-            "timestamp": 0.20014225862368368
-        },
-        {
-            "x": 7.68,
-            "y": 5.295999982033384,
-            "heading": 9.801760970151702e-21,
-            "angularVelocity": 4.8470744390330394e-20,
-            "velocityX": 3.439292531132709e-33,
-            "velocityY": -2.503219520548835,
-            "timestamp": 0.30021338793552554
-        },
-        {
-            "x": 7.68,
-            "y": 4.962,
-            "heading": 1.506109654033251e-20,
-            "angularVelocity": 5.255597191401202e-20,
-            "velocityX": 4.639675238422859e-33,
-            "velocityY": -3.3376257900775017,
-            "timestamp": 0.40028451724736736
-        },
-        {
-            "x": 7.68,
-            "y": 4.628000017966616,
-            "heading": 5.3586032763760104e-21,
-            "angularVelocity": -9.69559666912617e-20,
-            "velocityX": 4.935439546802769e-33,
-            "velocityY": -3.3376257900775017,
-            "timestamp": 0.5003556465592092
-        },
-        {
-            "x": 7.68,
-            "y": 4.377500013629846,
-            "heading": 1.8760590350448333e-21,
-            "angularVelocity": -3.480068842301423e-20,
-            "velocityX": 4.701730454770122e-33,
-            "velocityY": -2.5032195205488352,
-            "timestamp": 0.600426775871051
-        },
-        {
-            "x": 7.68,
-            "y": 4.210500005575846,
-            "heading": 3.168722428707802e-22,
-            "angularVelocity": -1.5580785203142817e-20,
-            "velocityX": 3.49539235617723e-33,
-            "velocityY": -1.668813065290736,
-            "timestamp": 0.7004979051828928
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 5.881496870004905e-41,
-            "angularVelocity": -3.1664700918333573e-21,
-            "velocityX": 3.364378386644386e-33,
-            "velocityY": -0.8344065481228213,
-            "timestamp": 0.8005690344947346
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 9.145026397391945e-42,
-            "angularVelocity": -4.963805231001231e-40,
-            "velocityX": -4.870998572760777e-42,
-            "velocityY": 3.489748854003666e-39,
-            "timestamp": 0.9006401638065764
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.1.traj
deleted file mode 100644
index bcede3cb..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.1.traj	
+++ /dev/null
@@ -1,139 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 7.067561628822357e-40,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 5.720554627994461,
-            "heading": -3.8243496398495774e-19,
-            "angularVelocity": -3.994077070459528e-18,
-            "velocityX": -4.592295404090674e-34,
-            "velocityY": -0.7983807350946677,
-            "timestamp": 0.09575052183150977
-        },
-        {
-            "x": 7.68,
-            "y": 5.567663887017311,
-            "heading": -1.1460790562861162e-18,
-            "angularVelocity": -7.975351749671857e-18,
-            "velocityX": 3.216033275420413e-35,
-            "velocityY": -1.5967614385035829,
-            "timestamp": 0.19150104366301954
-        },
-        {
-            "x": 7.68,
-            "y": 5.3383277831364,
-            "heading": -2.289011643788579e-18,
-            "angularVelocity": -1.1936567706665239e-17,
-            "velocityX": -6.99395718741277e-36,
-            "velocityY": -2.3951420785410233,
-            "timestamp": 0.2872515654945293
-        },
-        {
-            "x": 7.68,
-            "y": 5.032546334555254,
-            "heading": -3.801510097022925e-18,
-            "angularVelocity": -1.5796242396621782e-17,
-            "velocityX": -1.9389720200265424e-34,
-            "velocityY": -3.1935225284643693,
-            "timestamp": 0.3830020873260391
-        },
-        {
-            "x": 7.68,
-            "y": 4.670327800733179,
-            "heading": -7.244345840319028e-19,
-            "angularVelocity": 3.2136383741131935e-17,
-            "velocityX": -8.432373407035536e-35,
-            "velocityY": -3.7829405719528495,
-            "timestamp": 0.47875260915754886
-        },
-        {
-            "x": 7.68,
-            "y": 4.308109266911059,
-            "heading": -3.757861819981246e-18,
-            "angularVelocity": -3.168052954622205e-17,
-            "velocityX": -1.6004910806259417e-34,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.5745031309890586
-        },
-        {
-            "x": 7.68,
-            "y": 3.9458907330889397,
-            "heading": -5.3976563030855884e-18,
-            "angularVelocity": -1.712569761227889e-17,
-            "velocityX": -2.8603575042124878e-33,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.6702536528205684
-        },
-        {
-            "x": 7.68,
-            "y": 3.5836721992668203,
-            "heading": -4.35645304230582e-18,
-            "angularVelocity": 1.0874126227865062e-17,
-            "velocityX": -1.9073271103237224e-34,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.7660041746520782
-        },
-        {
-            "x": 7.68,
-            "y": 3.221453665444745,
-            "heading": 3.661990694649205e-18,
-            "angularVelocity": 8.37430813282138e-17,
-            "velocityX": -3.48109229998139e-33,
-            "velocityY": -3.7829405719528495,
-            "timestamp": 0.8617546964835879
-        },
-        {
-            "x": 7.68,
-            "y": 2.915672216863599,
-            "heading": 2.222559506115206e-18,
-            "angularVelocity": -1.503314198751682e-17,
-            "velocityX": 7.233727398507661e-34,
-            "velocityY": -3.1935225284643693,
-            "timestamp": 0.9575052183150977
-        },
-        {
-            "x": 7.68,
-            "y": 2.686336112982689,
-            "heading": 1.1174854461234148e-18,
-            "angularVelocity": -1.1541180553049724e-17,
-            "velocityX": 7.218959232022663e-34,
-            "velocityY": -2.3951420785410233,
-            "timestamp": 1.0532557401466074
-        },
-        {
-            "x": 7.68,
-            "y": 2.5334453720055388,
-            "heading": 3.7383227049768543e-19,
-            "angularVelocity": -7.766570474567622e-18,
-            "velocityX": 1.7872274250399634e-33,
-            "velocityY": -1.5967614385035829,
-            "timestamp": 1.1490062619781172
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -3.904232200157263e-18,
-            "velocityX": 6.089132400706592e-33,
-            "velocityY": -0.7983807350946677,
-            "timestamp": 1.244756783809627
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 3.5763678522980893e-43,
-            "velocityY": -5.882608067207678e-38,
-            "timestamp": 1.3405073056411367
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.1.traj
deleted file mode 100644
index 3fd40c6e..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.1.traj	
+++ /dev/null
@@ -1,175 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -1.2882449649302714e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 5.715155846048237,
-            "heading": -2.6675793503802873e-18,
-            "angularVelocity": -2.692513973443352e-17,
-            "velocityX": 2.411941793716939e-19,
-            "velocityY": -0.8260917454276263,
-            "timestamp": 0.09907392781100396
-        },
-        {
-            "x": 7.68,
-            "y": 5.55146754125807,
-            "heading": -8.002991694377614e-18,
-            "angularVelocity": -5.385283961043983e-17,
-            "velocityX": 4.822926900034719e-19,
-            "velocityY": -1.6521834594306495,
-            "timestamp": 0.19814785562200793
-        },
-        {
-            "x": 7.68,
-            "y": 5.305935091856216,
-            "heading": -1.6007101079545214e-17,
-            "angularVelocity": -8.07892607262788e-17,
-            "velocityX": 7.23199861883135e-19,
-            "velocityY": -2.478275110584479,
-            "timestamp": 0.29722178343301187
-        },
-        {
-            "x": 7.68,
-            "y": 4.978558516522808,
-            "heading": -2.6680861930648114e-17,
-            "angularVelocity": -1.0773531530269989e-16,
-            "velocityX": 9.635330187135107e-19,
-            "velocityY": -3.3043665731908765,
-            "timestamp": 0.39629571124401586
-        },
-        {
-            "x": 7.68,
-            "y": 4.603767735073314,
-            "heading": -3.033567109313287e-17,
-            "angularVelocity": -3.688971703490728e-17,
-            "velocityX": 1.1102275506466142e-18,
-            "velocityY": -3.782940575087071,
-            "timestamp": 0.49536963905501985
-        },
-        {
-            "x": 7.68,
-            "y": 4.228976953623796,
-            "heading": -3.5529792774719166e-17,
-            "angularVelocity": -5.2426726146536295e-17,
-            "velocityX": 1.1087814850277944e-18,
-            "velocityY": -3.7829405750873115,
-            "timestamp": 0.5944435668660238
-        },
-        {
-            "x": 7.68,
-            "y": 3.8541861721742774,
-            "heading": -4.8775545061581196e-17,
-            "angularVelocity": -1.3369564101798844e-16,
-            "velocityX": 1.1066129122730268e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.6935174946770278
-        },
-        {
-            "x": 7.68,
-            "y": 3.4793953907247595,
-            "heading": -3.3383639047364345e-17,
-            "angularVelocity": 1.5535778538404392e-16,
-            "velocityX": 1.113993391664377e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.7925914224880318
-        },
-        {
-            "x": 7.68,
-            "y": 3.104604609275241,
-            "heading": -4.339521377299963e-18,
-            "angularVelocity": 2.9315601300747485e-16,
-            "velocityX": 1.114171335036007e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.8916653502990358
-        },
-        {
-            "x": 7.68,
-            "y": 2.7298138278257227,
-            "heading": 1.3174711336089981e-17,
-            "angularVelocity": 1.7677943229992181e-16,
-            "velocityX": 1.1158489583207238e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.9907392781100398
-        },
-        {
-            "x": 7.68,
-            "y": 2.3550230463762043,
-            "heading": 2.965421117619106e-17,
-            "angularVelocity": 1.6633538413393434e-16,
-            "velocityX": 1.1181357718618657e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 1.0898132059210437
-        },
-        {
-            "x": 7.68,
-            "y": 1.9802322649266861,
-            "heading": 2.902288280090785e-17,
-            "angularVelocity": -6.372295813531433e-18,
-            "velocityX": 1.121394742098521e-18,
-            "velocityY": -3.7829405750873115,
-            "timestamp": 1.1888871337320477
-        },
-        {
-            "x": 7.68,
-            "y": 1.605441483477192,
-            "heading": 2.667314494747493e-17,
-            "angularVelocity": -2.3717015216307205e-17,
-            "velocityX": 1.1050727300415562e-18,
-            "velocityY": -3.782940575087071,
-            "timestamp": 1.2879610615430517
-        },
-        {
-            "x": 7.68,
-            "y": 1.2780649081437845,
-            "heading": 1.6004201006274906e-17,
-            "angularVelocity": -1.0768669595616384e-16,
-            "velocityX": 9.663666503471993e-19,
-            "velocityY": -3.304366573190877,
-            "timestamp": 1.3870349893540557
-        },
-        {
-            "x": 7.68,
-            "y": 1.0325324587419298,
-            "heading": 8.002125258512226e-18,
-            "angularVelocity": -8.076873426351199e-17,
-            "velocityX": 7.247750412790524e-19,
-            "velocityY": -2.478275110584479,
-            "timestamp": 1.4861089171650597
-        },
-        {
-            "x": 7.68,
-            "y": 0.868844153951763,
-            "heading": 2.6673738748743342e-18,
-            "angularVelocity": -5.384616822543564e-17,
-            "velocityX": 4.831833763651305e-19,
-            "velocityY": -1.6521834594306495,
-            "timestamp": 1.5851828449760637
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -2.6923065773077948e-17,
-            "velocityX": 2.415916928361511e-19,
-            "velocityY": -0.8260917454276263,
-            "timestamp": 1.6842567727870676
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -1.1487176577652646e-33,
-            "velocityY": 0,
-            "timestamp": 1.7833307005980716
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.1.traj
deleted file mode 100644
index 6cbc4c1d..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 7.776001513534033e-33,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.620476769374415,
-            "y": 5.82809535402629,
-            "heading": -0.014618874829729222,
-            "angularVelocity": -0.16274311325472623,
-            "velocityX": -0.6626362135126213,
-            "velocityY": 0.34616581514913597,
-            "timestamp": 0.0898279167539809
-        },
-        {
-            "x": 7.5013982904113785,
-            "y": 5.890300493780534,
-            "heading": -0.04163561529349077,
-            "angularVelocity": -0.30076107116849365,
-            "velocityX": -1.3256288608937272,
-            "velocityY": 0.6924922897255835,
-            "timestamp": 0.1796558335079618
-        },
-        {
-            "x": 7.3227216232188646,
-            "y": 5.983633458070523,
-            "heading": -0.07726722819497155,
-            "angularVelocity": -0.39666524827763777,
-            "velocityX": -1.9890995321851956,
-            "velocityY": 1.0390195794655717,
-            "timestamp": 0.2694837502619427
-        },
-        {
-            "x": 7.084404558965627,
-            "y": 6.108106280619183,
-            "heading": -0.11365614248079976,
-            "angularVelocity": -0.40509582767559393,
-            "velocityX": -2.6530400889284445,
-            "velocityY": 1.3856808333823862,
-            "timestamp": 0.3593116670159236
-        },
-        {
-            "x": 6.786764320803032,
-            "y": 6.263508027124416,
-            "heading": -0.12453213515982267,
-            "angularVelocity": -0.12107586452004557,
-            "velocityX": -3.313449191722499,
-            "velocityY": 1.7299938829800985,
-            "timestamp": 0.4491395837699045
-        },
-        {
-            "x": 6.485598483527544,
-            "y": 6.42090073842616,
-            "heading": -0.1245321478638912,
-            "angularVelocity": -1.4142672994383305e-7,
-            "velocityX": -3.3526975594938513,
-            "velocityY": 1.7521580928210598,
-            "timestamp": 0.5389675005238854
-        },
-        {
-            "x": 6.164419225817841,
-            "y": 6.531883674964022,
-            "heading": -0.12453214704403566,
-            "angularVelocity": 9.126957138704023e-9,
-            "velocityX": -3.575494894191328,
-            "velocityY": 1.2355060714790946,
-            "timestamp": 0.6287954172778664
-        },
-        {
-            "x": 5.838530026083828,
-            "y": 6.576482035403971,
-            "heading": -0.08897109734241883,
-            "angularVelocity": 0.39587971074750417,
-            "velocityX": -3.62792783702814,
-            "velocityY": 0.4964866385813549,
-            "timestamp": 0.7186233340318473
-        },
-        {
-            "x": 5.57314678987786,
-            "y": 6.601508855183474,
-            "heading": 0.02708371106201491,
-            "angularVelocity": 1.2919681608812392,
-            "velocityX": -2.9543514510393742,
-            "velocityY": 0.27860848479928674,
-            "timestamp": 0.8084512507858282
-        },
-        {
-            "x": 5.3743330310505755,
-            "y": 6.616929267325104,
-            "heading": 0.1370261836602847,
-            "angularVelocity": 1.2239232141983023,
-            "velocityX": -2.213273623742076,
-            "velocityY": 0.17166614454461948,
-            "timestamp": 0.8982791675398092
-        },
-        {
-            "x": 5.242037120273562,
-            "y": 6.625930657483227,
-            "heading": 0.21902488355780253,
-            "angularVelocity": 0.9128420524556349,
-            "velocityX": -1.4727705546076917,
-            "velocityY": 0.10020704568687104,
-            "timestamp": 0.9881070842937901
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": 0.48954843918551255,
-            "velocityX": -0.7351514168409599,
-            "velocityY": 0.0453015350218741,
-            "timestamp": 1.077935001047771
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": -4.780109867389741e-33,
-            "velocityX": 0,
-            "velocityY": -5.233682494366388e-34,
-            "timestamp": 1.167762917801752
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.1.traj
deleted file mode 100644
index 4e0731ce..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.1.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 1.3211551091100433e-31,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.6375302942972665,
-            "y": 5.822387711771769,
-            "heading": 0.010500881646553203,
-            "angularVelocity": 0.13619602846032355,
-            "velocityX": -0.5508304389364825,
-            "velocityY": 0.32927763890619355,
-            "timestamp": 0.07710123243140166
-        },
-        {
-            "x": 7.5525908811480225,
-            "y": 5.873163180716977,
-            "heading": 0.031500279151306286,
-            "angularVelocity": 0.27236137273728567,
-            "velocityX": -1.1016609004897018,
-            "velocityY": 0.6585558666702785,
-            "timestamp": 0.1542024648628033
-        },
-        {
-            "x": 7.4251817593524025,
-            "y": 5.949326484276257,
-            "heading": 0.06299451206522548,
-            "angularVelocity": 0.40847898173275193,
-            "velocityX": -1.6524913776051289,
-            "velocityY": 0.987835098836381,
-            "timestamp": 0.23130369729420497
-        },
-        {
-            "x": 7.255302939480135,
-            "y": 6.050877733979633,
-            "heading": 0.10497913316622995,
-            "angularVelocity": 0.5445389104299847,
-            "velocityX": -2.203321717631576,
-            "velocityY": 1.3171157775425744,
-            "timestamp": 0.3084049297256066
-        },
-        {
-            "x": 7.042954462706059,
-            "y": 6.177817072431174,
-            "heading": 0.1574496496087124,
-            "angularVelocity": 0.6805405670936119,
-            "velocityX": -2.754151523621958,
-            "velocityY": 1.6463983058180132,
-            "timestamp": 0.3855061621570083
-        },
-        {
-            "x": 6.792607460054783,
-            "y": 6.3274746833219355,
-            "heading": 0.15744965721454035,
-            "angularVelocity": 9.864729365588338e-8,
-            "velocityX": -3.246990933303352,
-            "velocityY": 1.941053419916652,
-            "timestamp": 0.46260739458840994
-        },
-        {
-            "x": 6.542259846259959,
-            "y": 6.4771312718889495,
-            "heading": 0.15744966481969572,
-            "angularVelocity": 9.863857053631023e-8,
-            "velocityX": -3.246998859811527,
-            "velocityY": 1.9410401604172332,
-            "timestamp": 0.5397086270198116
-        },
-        {
-            "x": 6.273256747030451,
-            "y": 6.589862608843688,
-            "heading": 0.1574496727775284,
-            "angularVelocity": 1.0321278187268036e-7,
-            "velocityX": -3.4889597837342676,
-            "velocityY": 1.4621210764047974,
-            "timestamp": 0.6168098594512132
-        },
-        {
-            "x": 5.98904964850956,
-            "y": 6.655416753006686,
-            "heading": 0.15744969119167346,
-            "angularVelocity": 2.3883074855376557e-7,
-            "velocityX": -3.686155065987494,
-            "velocityY": 0.8502347121535809,
-            "timestamp": 0.6939110918826149
-        },
-        {
-            "x": 5.6978465137603145,
-            "y": 6.6719003531074685,
-            "heading": 0.15744999446473232,
-            "angularVelocity": 0.00000393343983387992,
-            "velocityX": -3.7768933850484796,
-            "velocityY": 0.21379165521703997,
-            "timestamp": 0.7710123243140166
-        },
-        {
-            "x": 5.40805721282959,
-            "y": 6.638837814331055,
-            "heading": 0.15745035498223803,
-            "angularVelocity": 0.00000467589809324531,
-            "velocityX": -3.7585560151526085,
-            "velocityY": -0.428819848059242,
-            "timestamp": 0.8481135567454182
-        },
-        {
-            "x": 5.104307335648147,
-            "y": 6.562247743535093,
-            "heading": 0.18966206475475966,
-            "angularVelocity": 0.37702586381684944,
-            "velocityX": -3.555277277652879,
-            "velocityY": -0.896457772827513,
-            "timestamp": 0.9335498964342235
-        },
-        {
-            "x": 4.836190414730584,
-            "y": 6.438293970082571,
-            "heading": 0.27176288986041636,
-            "angularVelocity": 0.9609590650149814,
-            "velocityX": -3.1382070193334055,
-            "velocityY": -1.4508319750590115,
-            "timestamp": 1.0189862361230289
-        },
-        {
-            "x": 4.616497381460216,
-            "y": 6.278221072521664,
-            "heading": 0.3274582685749672,
-            "angularVelocity": 0.6518933151562509,
-            "velocityX": -2.5714237532949347,
-            "velocityY": -1.8735926438791781,
-            "timestamp": 1.1044225758118342
-        },
-        {
-            "x": 4.449006665280017,
-            "y": 6.09487376587715,
-            "heading": 0.3096216512620487,
-            "angularVelocity": -0.2087708506460702,
-            "velocityX": -1.960415401575849,
-            "velocityY": -2.1460107878256482,
-            "timestamp": 1.1898589155006396
-        },
-        {
-            "x": 4.339814814870372,
-            "y": 5.903698766708507,
-            "heading": 0.23521329843339078,
-            "angularVelocity": -0.8709215902704173,
-            "velocityX": -1.2780492564097095,
-            "velocityY": -2.237630964352897,
-            "timestamp": 1.275295255189445
-        },
-        {
-            "x": 4.286146359443328,
-            "y": 5.736442488850756,
-            "heading": 0.1356278137676922,
-            "angularVelocity": -1.1656103834554516,
-            "velocityX": -0.628168945703036,
-            "velocityY": -1.957671389796991,
-            "timestamp": 1.3607315948782504
-        },
-        {
-            "x": 4.266951931468271,
-            "y": 5.619203136466468,
-            "heading": 0.049466079264437586,
-            "angularVelocity": -1.0084904715849423,
-            "velocityX": -0.2246635102225946,
-            "velocityY": -1.372242219309977,
-            "timestamp": 1.4461679345670557
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 4.182976778726496e-34,
-            "angularVelocity": -0.578981724224301,
-            "velocityX": -0.046255861178807596,
-            "velocityY": -0.6929502912005617,
-            "timestamp": 1.531604274255861
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -6.30298269203181e-33,
-            "timestamp": 1.6170406139446665
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.1.traj
deleted file mode 100644
index 90749c58..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.1.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 7.446350164213638e-30,
-            "velocityX": -3.476389258222187e-32,
-            "velocityY": 2.4606016306403766e-37,
-            "timestamp": 0
-        },
-        {
-            "x": 7.6436015698643995,
-            "y": 5.74806653588022,
-            "heading": -0.0038807165751560974,
-            "angularVelocity": -0.045372809336175934,
-            "velocityX": -0.4255654848002544,
-            "velocityY": -0.572123393880186,
-            "timestamp": 0.08552956345292874
-        },
-        {
-            "x": 7.570804707716712,
-            "y": 5.6501996114987145,
-            "heading": -0.011642164214821673,
-            "angularVelocity": -0.09074578807989701,
-            "velocityX": -0.8511309915401587,
-            "velocityY": -1.1442467426525054,
-            "timestamp": 0.17105912690585748
-        },
-        {
-            "x": 7.461609412342726,
-            "y": 5.503399231866818,
-            "heading": -0.02328436718357624,
-            "angularVelocity": -0.136119050521775,
-            "velocityX": -1.2766965124764296,
-            "velocityY": -1.7163700328330225,
-            "timestamp": 0.25658869035878623
-        },
-        {
-            "x": 7.3160156842382476,
-            "y": 5.307665404737857,
-            "heading": -0.038807352623976644,
-            "angularVelocity": -0.18149263031072865,
-            "velocityX": -1.7022620276158245,
-            "velocityY": -2.288493232362672,
-            "timestamp": 0.34211825381171496
-        },
-        {
-            "x": 7.134023531165023,
-            "y": 5.062998148299065,
-            "heading": -0.05821111114579378,
-            "angularVelocity": -0.22686610031040333,
-            "velocityX": -2.127827452005934,
-            "velocityY": -2.8606162192496707,
-            "timestamp": 0.4276478172646437
-        },
-        {
-            "x": 6.9409164317403365,
-            "y": 4.803390071813602,
-            "heading": -0.058211113467083825,
-            "angularVelocity": -2.714020682501112e-8,
-            "velocityX": -2.2577818900123794,
-            "velocityY": -3.0353022511138943,
-            "timestamp": 0.5131773807175725
-        },
-        {
-            "x": 6.70968017178257,
-            "y": 4.577079452070077,
-            "heading": -0.05821111591888997,
-            "angularVelocity": -2.866618337367041e-8,
-            "velocityX": -2.7035828387575864,
-            "velocityY": -2.645992924634468,
-            "timestamp": 0.5987069441705012
-        },
-        {
-            "x": 6.4400791107327935,
-            "y": 4.398188508585333,
-            "heading": -0.05821111892081074,
-            "angularVelocity": -3.509804859460385e-8,
-            "velocityX": -3.152138864804934,
-            "velocityY": -2.091568532127445,
-            "timestamp": 0.6842365076234299
-        },
-        {
-            "x": 6.141694589328426,
-            "y": 4.273075039260189,
-            "heading": -0.058211144902897714,
-            "angularVelocity": -3.0377902004237926e-7,
-            "velocityX": -3.488671160686832,
-            "velocityY": -1.462809632999017,
-            "timestamp": 0.7697660710763586
-        },
-        {
-            "x": 5.825131041705898,
-            "y": 4.206185466847777,
-            "heading": -0.05821124348692034,
-            "angularVelocity": -0.000001152630957627144,
-            "velocityX": -3.7012178578082944,
-            "velocityY": -0.7820637649954203,
-            "timestamp": 0.8552956345292874
-        },
-        {
-            "x": 5.503318785454965,
-            "y": 4.1726658015863975,
-            "heading": -0.058211338856237596,
-            "angularVelocity": -0.0000011150450605169934,
-            "velocityX": -3.76258504380235,
-            "velocityY": -0.39190735820634004,
-            "timestamp": 0.9408251979822161
-        },
-        {
-            "x": 5.1935160355708865,
-            "y": 4.079344267897855,
-            "heading": -0.0582116136415855,
-            "angularVelocity": -0.0000032127528402629677,
-            "velocityX": -3.622171532005757,
-            "velocityY": -1.091102654112135,
-            "timestamp": 1.026354761435145
-        },
-        {
-            "x": 4.918641828284195,
-            "y": 3.9369980738928456,
-            "heading": -0.09326111746178506,
-            "angularVelocity": -0.4097940221510551,
-            "velocityX": -3.2137917719873403,
-            "velocityY": -1.664292301495839,
-            "timestamp": 1.1118843248880736
-        },
-        {
-            "x": 4.689025118769517,
-            "y": 3.755353610578967,
-            "heading": -0.16867561276178636,
-            "angularVelocity": -0.8817360016283226,
-            "velocityX": -2.6846472756878708,
-            "velocityY": -2.1237623107224977,
-            "timestamp": 1.1974138883410024
-        },
-        {
-            "x": 4.510985991586203,
-            "y": 3.542062267326439,
-            "heading": -0.2715725395002121,
-            "angularVelocity": -1.2030568447254515,
-            "velocityX": -2.0816092120158958,
-            "velocityY": -2.493773318156982,
-            "timestamp": 1.282943451793931
-        },
-        {
-            "x": 4.39344009542189,
-            "y": 3.3364721307805043,
-            "heading": -0.3792171957634167,
-            "angularVelocity": -1.2585666513128753,
-            "velocityX": -1.3743305989045989,
-            "velocityY": -2.4037318588569803,
-            "timestamp": 1.3684730152468598
-        },
-        {
-            "x": 4.321460732883776,
-            "y": 3.1711461674458645,
-            "heading": -0.4697306425856333,
-            "angularVelocity": -1.0582708851545903,
-            "velocityX": -0.8415728975131295,
-            "velocityY": -1.932968632835679,
-            "timestamp": 1.4540025786997886
-        },
-        {
-            "x": 4.280782976343733,
-            "y": 3.057632956228743,
-            "heading": -0.5333100471714295,
-            "angularVelocity": -0.7433617338733072,
-            "velocityX": -0.475598785938263,
-            "velocityY": -1.3271809960727023,
-            "timestamp": 1.5395321421527173
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -0.38220647351440606,
-            "velocityX": -0.20791613596297567,
-            "velocityY": -0.6738366700592523,
-            "timestamp": 1.625061705605646
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -1.9453350650223036e-32,
-            "velocityX": 0,
-            "velocityY": 3.675541041669649e-33,
-            "timestamp": 1.7105912690585747
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S4.1.traj
deleted file mode 100644
index 4efefc66..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S4.1.traj	
+++ /dev/null
@@ -1,256 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": -1.007826409753455e-39,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.661226431831969,
-            "y": 5.757681823957774,
-            "heading": -0.0001846118691686639,
-            "angularVelocity": -0.0025538708536768893,
-            "velocityX": -0.25970848342391806,
-            "velocityY": -0.5439170529292338,
-            "timestamp": 0.07228708096295171
-        },
-        {
-            "x": 7.623679292701499,
-            "y": 5.679045474684997,
-            "heading": -0.0005538375109882563,
-            "angularVelocity": -0.005107768039614528,
-            "velocityX": -0.5194170055049473,
-            "velocityY": -1.0878340669625215,
-            "timestamp": 0.14457416192590342
-        },
-        {
-            "x": 7.567358579979895,
-            "y": 5.561090955652203,
-            "heading": -0.0011076778331591435,
-            "angularVelocity": -0.0076616777824344475,
-            "velocityX": -0.779125563950612,
-            "velocityY": -1.6317510329853882,
-            "timestamp": 0.2168612428888551
-        },
-        {
-            "x": 7.492264291454711,
-            "y": 5.403818271607319,
-            "heading": -0.001846132228095339,
-            "angularVelocity": -0.010215579120073546,
-            "velocityX": -1.0388341530026832,
-            "velocityY": -2.175667933326667,
-            "timestamp": 0.28914832385180683
-        },
-        {
-            "x": 7.39839642615194,
-            "y": 5.207227430397753,
-            "heading": -0.0027691974892554232,
-            "angularVelocity": -0.012769436099282666,
-            "velocityX": -1.298542755528879,
-            "velocityY": -2.719584725109078,
-            "timestamp": 0.36143540481475855
-        },
-        {
-            "x": 7.285754988600711,
-            "y": 4.97131845200793,
-            "heading": -0.0038768638943260385,
-            "angularVelocity": -0.015323158582628504,
-            "velocityX": -1.5582512954003445,
-            "velocityY": -3.263501240432299,
-            "timestamp": 0.4337224857777103
-        },
-        {
-            "x": 7.167927209247298,
-            "y": 4.7245478329210355,
-            "heading": -0.0038768640537450113,
-            "angularVelocity": -2.205359112715029e-9,
-            "velocityX": -1.629997750411335,
-            "velocityY": -3.4137582511232942,
-            "timestamp": 0.506009566740662
-        },
-        {
-            "x": 7.050099366875978,
-            "y": 4.477777243923851,
-            "heading": -0.003876864213154645,
-            "angularVelocity": -2.2052299170164517e-9,
-            "velocityX": -1.6299986221840805,
-            "velocityY": -3.413757834870371,
-            "timestamp": 0.5782966477036137
-        },
-        {
-            "x": 6.932271561638543,
-            "y": 4.2310066371960025,
-            "heading": -0.0038768643725604667,
-            "angularVelocity": -2.2051771914474314e-9,
-            "velocityX": -1.629998108483918,
-            "velocityY": -3.413758080151585,
-            "timestamp": 0.6505837286665653
-        },
-        {
-            "x": 6.8252544845676155,
-            "y": 3.9793591274718993,
-            "heading": -0.0038768645333797187,
-            "angularVelocity": -2.2247301918048757e-9,
-            "velocityX": -1.4804454080221456,
-            "velocityY": -3.4812238421008592,
-            "timestamp": 0.722870809629517
-        },
-        {
-            "x": 6.7595626994044276,
-            "y": 3.7139091088268694,
-            "heading": -0.003876864707631206,
-            "angularVelocity": -2.410548124556829e-9,
-            "velocityX": -0.9087624550347577,
-            "velocityY": -3.6721640313720423,
-            "timestamp": 0.7951578905924687
-        },
-        {
-            "x": 6.7368645668029785,
-            "y": 3.4413950443267822,
-            "heading": -0.0038768649214938546,
-            "angularVelocity": -2.9585182521427424e-9,
-            "velocityX": -0.31399985030634436,
-            "velocityY": -3.7698861382956523,
-            "timestamp": 0.8674449715554203
-        },
-        {
-            "x": 6.763602448674763,
-            "y": 3.1456190677825386,
-            "heading": -0.0038769163309766456,
-            "angularVelocity": -6.548510424231184e-7,
-            "velocityX": 0.340585605327871,
-            "velocityY": -3.7675774205236117,
-            "timestamp": 0.9459505849856912
-        },
-        {
-            "x": 6.763837716877819,
-            "y": 2.855181760345028,
-            "heading": -0.02573615579182971,
-            "angularVelocity": -0.27844173818562123,
-            "velocityX": 0.0029968328731749647,
-            "velocityY": -3.699573759722024,
-            "timestamp": 1.0244561984159621
-        },
-        {
-            "x": 6.746691140594586,
-            "y": 2.576419895706607,
-            "heading": -0.08408576500639096,
-            "angularVelocity": -0.7432539746522423,
-            "velocityX": -0.2184121049950426,
-            "velocityY": -3.5508526391685153,
-            "timestamp": 1.102961811846233
-        },
-        {
-            "x": 6.69086397201793,
-            "y": 2.322743038831463,
-            "heading": -0.2046015958190934,
-            "angularVelocity": -1.5351237388870964,
-            "velocityX": -0.7111232705192682,
-            "velocityY": -3.2313212494092722,
-            "timestamp": 1.1814674252765038
-        },
-        {
-            "x": 6.586917702656428,
-            "y": 2.0871040226876985,
-            "heading": -0.32491541634118276,
-            "angularVelocity": -1.5325505433946174,
-            "velocityX": -1.324061615719081,
-            "velocityY": -3.001556269005664,
-            "timestamp": 1.2599730387067747
-        },
-        {
-            "x": 6.439847045017927,
-            "y": 1.879401635438828,
-            "heading": -0.448122685606705,
-            "angularVelocity": -1.5694071274910255,
-            "velocityX": -1.8733775995410846,
-            "velocityY": -2.6457010928696563,
-            "timestamp": 1.3384786521370455
-        },
-        {
-            "x": 6.25456019704424,
-            "y": 1.7060438180074393,
-            "heading": -0.5681346722700569,
-            "angularVelocity": -1.528705801018258,
-            "velocityX": -2.3601732395640638,
-            "velocityY": -2.2082219328859343,
-            "timestamp": 1.4169842655673164
-        },
-        {
-            "x": 6.049779045368227,
-            "y": 1.57972317182915,
-            "heading": -0.6633309119306381,
-            "angularVelocity": -1.2126042393788183,
-            "velocityX": -2.6084905617341763,
-            "velocityY": -1.6090651439911137,
-            "timestamp": 1.4954898789975872
-        },
-        {
-            "x": 5.840944041976222,
-            "y": 1.503518626439596,
-            "heading": -0.7208467997653518,
-            "angularVelocity": -0.7326340795464229,
-            "velocityX": -2.660128292322611,
-            "velocityY": -0.9706891273098467,
-            "timestamp": 1.573995492427858
-        },
-        {
-            "x": 5.636524773547887,
-            "y": 1.4770268115818306,
-            "heading": -0.7348631463149735,
-            "angularVelocity": -0.17853941822989652,
-            "velocityX": -2.603880913686534,
-            "velocityY": -0.33745121782018417,
-            "timestamp": 1.652501105858129
-        },
-        {
-            "x": 5.451777868175388,
-            "y": 1.4973120784666072,
-            "heading": -0.7197568228935092,
-            "angularVelocity": 0.19242348109134802,
-            "velocityX": -2.353295481686686,
-            "velocityY": 0.25839256581051945,
-            "timestamp": 1.7310067192883998
-        },
-        {
-            "x": 5.309771976878378,
-            "y": 1.5457401851903851,
-            "heading": -0.687868735789328,
-            "angularVelocity": 0.4061886241103559,
-            "velocityX": -1.808862896449324,
-            "velocityY": 0.6168744451222204,
-            "timestamp": 1.8095123327186706
-        },
-        {
-            "x": 5.2191255322417085,
-            "y": 1.592492981200916,
-            "heading": -0.6576952711638115,
-            "angularVelocity": 0.38434786134518467,
-            "velocityX": -1.154649211386418,
-            "velocityY": 0.5955344333696231,
-            "timestamp": 1.8880179461489415
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0.22540134890517805,
-            "velocityX": -0.5493305555788384,
-            "velocityY": 0.35038282738234927,
-            "timestamp": 1.9665235595792123
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0,
-            "velocityX": 6.617232561290069e-35,
-            "velocityY": 1.0472745338276337e-32,
-            "timestamp": 2.045029173009483
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.1.traj
deleted file mode 100644
index 0428af29..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 2.477402495362019e-32,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.61783610738437,
-            "y": 5.830159445563511,
-            "heading": -0.009839426193567341,
-            "angularVelocity": -0.10699963479066096,
-            "velocityX": -0.6760062707098423,
-            "velocityY": 0.3605950687933442,
-            "timestamp": 0.09195756801242966
-        },
-        {
-            "x": 7.493508328314676,
-            "y": 5.8964783245931205,
-            "heading": -0.029519357600464063,
-            "angularVelocity": -0.21401100346886773,
-            "velocityX": -1.3520124744152573,
-            "velocityY": 0.7211900060324206,
-            "timestamp": 0.18391513602485932
-        },
-        {
-            "x": 7.307016677169599,
-            "y": 5.995956620127639,
-            "heading": -0.05904164496194892,
-            "angularVelocity": -0.32104249818236175,
-            "velocityX": -2.028018521758536,
-            "velocityY": 1.0817847588256284,
-            "timestamp": 0.275872704037289
-        },
-        {
-            "x": 7.058361185168299,
-            "y": 6.1285943089313815,
-            "heading": -0.09840880514573262,
-            "angularVelocity": -0.42810136277704247,
-            "velocityX": -2.7040242296065236,
-            "velocityY": 1.4423792589405482,
-            "timestamp": 0.36783027204971863
-        },
-        {
-            "x": 6.751428461707863,
-            "y": 6.292319225940392,
-            "heading": -0.09840881013370367,
-            "angularVelocity": -5.424209394751822e-8,
-            "velocityX": -3.3377646896767494,
-            "velocityY": 1.7804398327158815,
-            "timestamp": 0.4597878400621483
-        },
-        {
-            "x": 6.443018732830252,
-            "y": 6.45324462570272,
-            "heading": -0.09840881513118924,
-            "angularVelocity": -5.434556026601013e-8,
-            "velocityX": -3.3538265043712756,
-            "velocityY": 1.7499962563230937,
-            "timestamp": 0.551745408074578
-        },
-        {
-            "x": 6.108495593860041,
-            "y": 6.548679499028843,
-            "heading": -0.09840879814777771,
-            "angularVelocity": 1.8468748020115896e-7,
-            "velocityX": -3.637798891386455,
-            "velocityY": 1.0378142374668238,
-            "timestamp": 0.6437029760870077
-        },
-        {
-            "x": 5.7623127419169835,
-            "y": 6.582899007638951,
-            "heading": -0.09840874396641834,
-            "angularVelocity": 5.891995683553032e-7,
-            "velocityX": -3.764593381767812,
-            "velocityY": 0.37212281000605224,
-            "timestamp": 0.7356605440994373
-        },
-        {
-            "x": 5.416121749902788,
-            "y": 6.617036066627776,
-            "heading": -0.09840868977891702,
-            "angularVelocity": 5.892663593006056e-7,
-            "velocityX": -3.7646819016288298,
-            "velocityY": 0.37122620494064046,
-            "timestamp": 0.8276181121118669
-        },
-        {
-            "x": 5.068594601971295,
-            "y": 6.632477066079001,
-            "heading": -0.0984086463109951,
-            "angularVelocity": 4.72695427552955e-7,
-            "velocityX": -3.7792120370616824,
-            "velocityY": 0.1679143955735957,
-            "timestamp": 0.9195756801242965
-        },
-        {
-            "x": 4.721067428734796,
-            "y": 6.647917495985935,
-            "heading": -0.09840860284326601,
-            "angularVelocity": 4.7269333042542933e-7,
-            "velocityX": -3.7792123122430223,
-            "velocityY": 0.16790820201819795,
-            "timestamp": 1.011533248136726
-        },
-        {
-            "x": 4.3735402523552525,
-            "y": 6.663357855150138,
-            "heading": -0.09840855937552709,
-            "angularVelocity": 4.726934374928119e-7,
-            "velocityX": -3.779212346422309,
-            "velocityY": 0.16790743272066663,
-            "timestamp": 1.1034908161491557
-        },
-        {
-            "x": 4.026013074283842,
-            "y": 6.678798176234082,
-            "heading": -0.09840851590777203,
-            "angularVelocity": 4.7269361285314424e-7,
-            "velocityX": -3.779212364820652,
-            "velocityY": 0.1679070186138197,
-            "timestamp": 1.1954483841615853
-        },
-        {
-            "x": 3.6784858950907164,
-            "y": 6.69423847207055,
-            "heading": -0.09840847244000124,
-            "angularVelocity": 4.7269378404370134e-7,
-            "velocityX": -3.7792123770188395,
-            "velocityY": 0.16790674405809006,
-            "timestamp": 1.2874059521740149
-        },
-        {
-            "x": 3.330958715199463,
-            "y": 6.709678752193432,
-            "heading": -0.09840842897219951,
-            "angularVelocity": 4.7269412044437995e-7,
-            "velocityX": -3.7792123846106827,
-            "velocityY": 0.16790657317944163,
-            "timestamp": 1.3793635201864445
-        },
-        {
-            "x": 2.991492162018093,
-            "y": 6.7253542779205,
-            "heading": -0.07153342912076269,
-            "angularVelocity": 0.2922543563549246,
-            "velocityX": -3.6915564484642,
-            "velocityY": 0.17046477050097905,
-            "timestamp": 1.471321088198874
-        },
-        {
-            "x": 2.713831676976805,
-            "y": 6.738399816428541,
-            "heading": 0.08699262812295999,
-            "angularVelocity": 1.7239044123295555,
-            "velocityX": -3.0194413689122035,
-            "velocityY": 0.14186476208546492,
-            "timestamp": 1.5632786562113037
-        },
-        {
-            "x": 2.5064819943185284,
-            "y": 6.7478572981535265,
-            "heading": 0.2316797715135602,
-            "angularVelocity": 1.5734120259796696,
-            "velocityX": -2.2548408699787488,
-            "velocityY": 0.10284614882059591,
-            "timestamp": 1.6552362242237333
-        },
-        {
-            "x": 2.368710598807467,
-            "y": 6.7539967145328434,
-            "heading": 0.3372042735506754,
-            "angularVelocity": 1.147534719739997,
-            "velocityX": -1.4982061671361233,
-            "velocityY": 0.06676357924654308,
-            "timestamp": 1.747193792236163
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.6067551334304849,
-            "velocityX": -0.7471989559160562,
-            "velocityY": 0.03265947036300673,
-            "timestamp": 1.8391513602485925
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0,
-            "velocityX": -4.676461204476467e-32,
-            "velocityY": 0,
-            "timestamp": 1.931108928261022
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.1.traj
deleted file mode 100644
index 5429265f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.1.traj	
+++ /dev/null
@@ -1,184 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -7.048442713490885e-38,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.598761685357721,
-            "y": 5.839328892781844,
-            "heading": -0.010877614975927053,
-            "angularVelocity": -0.10375029848266179,
-            "velocityX": -0.774848108801196,
-            "velocityY": 0.4037314494285639,
-            "timestamp": 0.10484418006512992
-        },
-        {
-            "x": 7.436285061134455,
-            "y": 5.923986667743732,
-            "heading": -0.03263383242489801,
-            "angularVelocity": -0.20751001567713226,
-            "velocityX": -1.5496961693280003,
-            "velocityY": 0.8074627977375359,
-            "timestamp": 0.20968836013025985
-        },
-        {
-            "x": 7.192570139480125,
-            "y": 6.050973310343331,
-            "heading": -0.06527034306657675,
-            "angularVelocity": -0.31128585889464805,
-            "velocityX": -2.3245441139692504,
-            "velocityY": 1.2111940073422762,
-            "timestamp": 0.3145325401953898
-        },
-        {
-            "x": 6.867616948374112,
-            "y": 6.220288799737083,
-            "heading": -0.10878948434468085,
-            "angularVelocity": -0.41508399656585776,
-            "velocityX": -3.0993917917441864,
-            "velocityY": 1.6149250181418864,
-            "timestamp": 0.4193767202605197
-        },
-        {
-            "x": 6.5158804421759,
-            "y": 6.403559919066418,
-            "heading": -0.10878948853448296,
-            "angularVelocity": -3.996218113987742e-8,
-            "velocityX": -3.3548500830442913,
-            "velocityY": 1.7480333120587623,
-            "timestamp": 0.5242209003256496
-        },
-        {
-            "x": 6.14377891604402,
-            "y": 6.540845489176542,
-            "heading": -0.10878948531130445,
-            "angularVelocity": 3.074256021150283e-8,
-            "velocityX": -3.549090907103546,
-            "velocityY": 1.309424805695852,
-            "timestamp": 0.6290650803907796
-        },
-        {
-            "x": 5.750100268474419,
-            "y": 6.5890532439035745,
-            "heading": -0.10878941324219087,
-            "angularVelocity": 6.873926003335433e-7,
-            "velocityX": -3.754892711498581,
-            "velocityY": 0.4598038221776824,
-            "timestamp": 0.7339092604559095
-        },
-        {
-            "x": 5.35529697720587,
-            "y": 6.626964014921626,
-            "heading": -0.10878934177449004,
-            "angularVelocity": 6.816563474487017e-7,
-            "velocityX": -3.7656195224503146,
-            "velocityY": 0.3615915637329853,
-            "timestamp": 0.8387534405210395
-        },
-        {
-            "x": 4.9606240931120595,
-            "y": 6.587718813133039,
-            "heading": -0.10878931155021784,
-            "angularVelocity": 2.882780157548741e-7,
-            "velocityX": -3.7643757035310585,
-            "velocityY": -0.3743193161910223,
-            "timestamp": 0.9435976205861695
-        },
-        {
-            "x": 4.585497747567555,
-            "y": 6.458927912145357,
-            "heading": -0.10878930710495487,
-            "angularVelocity": 4.239875765382173e-8,
-            "velocityX": -3.5779415253328617,
-            "velocityY": -1.2284029586351546,
-            "timestamp": 1.0484418006512994
-        },
-        {
-            "x": 4.215349929173217,
-            "y": 6.316458743577884,
-            "heading": -0.10878930270384918,
-            "angularVelocity": 4.197758707259389e-8,
-            "velocityX": -3.530456513317188,
-            "velocityY": -1.3588657804273945,
-            "timestamp": 1.1532859807164293
-        },
-        {
-            "x": 3.8452021463353314,
-            "y": 6.173989482631574,
-            "heading": -0.10878929830273087,
-            "angularVelocity": 4.197770719831182e-8,
-            "velocityX": -3.530456174181034,
-            "velocityY": -1.3588666615333942,
-            "timestamp": 1.2581301607815591
-        },
-        {
-            "x": 3.475054376591526,
-            "y": 6.0315201876656985,
-            "heading": -0.10878929390160265,
-            "angularVelocity": 4.197780205439048e-8,
-            "velocityX": -3.530456049290165,
-            "velocityY": -1.3588669860107905,
-            "timestamp": 1.362974340846689
-        },
-        {
-            "x": 3.104906616234201,
-            "y": 5.889050868312964,
-            "heading": -0.10878928950045777,
-            "angularVelocity": 4.19779608468647e-8,
-            "velocityX": -3.530455959762256,
-            "velocityY": -1.358867218611777,
-            "timestamp": 1.4678185209118189
-        },
-        {
-            "x": 2.7629439773689555,
-            "y": 5.757430528072475,
-            "heading": -0.06527490613894259,
-            "angularVelocity": 0.4150386157294016,
-            "velocityX": -3.2616272896866185,
-            "velocityY": -1.2553900479618894,
-            "timestamp": 1.5726627009769487
-        },
-        {
-            "x": 2.506471990883214,
-            "y": 5.658715266844828,
-            "heading": -0.03263773447849684,
-            "angularVelocity": 0.311292163667756,
-            "velocityX": -2.446220537243172,
-            "velocityY": -0.9415425936501655,
-            "timestamp": 1.6775068810420786
-        },
-        {
-            "x": 2.335490664707938,
-            "y": 5.592905089907167,
-            "heading": -0.010879217132893882,
-            "angularVelocity": 0.20753195200808147,
-            "velocityX": -1.6308137091544919,
-            "velocityY": -0.6276950890052175,
-            "timestamp": 1.7823510611072084
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0.10376557979790346,
-            "velocityX": -0.8154068700316104,
-            "velocityY": -0.3138475582214142,
-            "timestamp": 1.8871952411723383
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 2.158258486540926e-34,
-            "velocityY": 0,
-            "timestamp": 1.9920394212374681
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.1.traj
deleted file mode 100644
index cf63c7e3..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.1.traj	
+++ /dev/null
@@ -1,346 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": -2.981242670977675e-31,
-            "velocityX": -5.763030958279131e-32,
-            "velocityY": 3.5928963704249306e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.6464505983184665,
-            "y": 5.761517456765686,
-            "heading": 0.04441523610418216,
-            "angularVelocity": 0.571175712894524,
-            "velocityX": -0.43144211544178024,
-            "velocityY": -0.4563021319898165,
-            "timestamp": 0.07776107264628128
-        },
-        {
-            "x": 7.57910774134765,
-            "y": 5.690410071738714,
-            "heading": 0.1294441379757556,
-            "angularVelocity": 1.0934635927458562,
-            "velocityX": -0.8660227370723712,
-            "velocityY": -0.914434209908928,
-            "timestamp": 0.15552214529256256
-        },
-        {
-            "x": 7.4776816969238515,
-            "y": 5.583411802725407,
-            "heading": 0.24873158333085876,
-            "angularVelocity": 1.5340252043296483,
-            "velocityX": -1.3043292867777738,
-            "velocityY": -1.3759875651409672,
-            "timestamp": 0.23328321793884382
-        },
-        {
-            "x": 7.341761998810456,
-            "y": 5.440105324158156,
-            "heading": 0.38885380810835574,
-            "angularVelocity": 1.8019584865410045,
-            "velocityX": -1.747914393255167,
-            "velocityY": -1.842907687489359,
-            "timestamp": 0.3110442905851251
-        },
-        {
-            "x": 7.171108690296084,
-            "y": 5.260470754663091,
-            "heading": 0.5147184350397563,
-            "angularVelocity": 1.6186071339824826,
-            "velocityX": -2.1945853202236263,
-            "velocityY": -2.3100834824152265,
-            "timestamp": 0.3888053632314064
-        },
-        {
-            "x": 6.970033766909185,
-            "y": 5.052418524467747,
-            "heading": 0.5282282166004705,
-            "angularVelocity": 0.17373450623768458,
-            "velocityX": -2.585804394720051,
-            "velocityY": -2.67553189681076,
-            "timestamp": 0.4665664358776877
-        },
-        {
-            "x": 6.768438732548067,
-            "y": 4.838192511419719,
-            "heading": 0.5282282675213732,
-            "angularVelocity": 6.548379670470849e-7,
-            "velocityX": -2.592492972391604,
-            "velocityY": -2.7549261572367425,
-            "timestamp": 0.5443275085239689
-        },
-        {
-            "x": 6.5423880710685225,
-            "y": 4.649952125007829,
-            "heading": 0.5282283002658353,
-            "angularVelocity": 4.210906690145718e-7,
-            "velocityX": -2.9069900116707847,
-            "velocityY": -2.42075346964613,
-            "timestamp": 0.6220885811702502
-        },
-        {
-            "x": 6.292219183977312,
-            "y": 4.49519788912002,
-            "heading": 0.5282283052888641,
-            "angularVelocity": 6.459567356551782e-8,
-            "velocityX": -3.217148099656167,
-            "velocityY": -1.9901247580747212,
-            "timestamp": 0.6998496538165315
-        },
-        {
-            "x": 6.0420493950115075,
-            "y": 4.3404451111724525,
-            "heading": 0.5282283103117955,
-            "angularVelocity": 6.459441933764687e-8,
-            "velocityX": -3.217159697677684,
-            "velocityY": -1.990106009101862,
-            "timestamp": 0.7776107264628128
-        },
-        {
-            "x": 5.780250458218128,
-            "y": 4.206300927357815,
-            "heading": 0.5282283154384368,
-            "angularVelocity": 6.592812032299238e-8,
-            "velocityX": -3.366709432935023,
-            "velocityY": -1.7250814482051704,
-            "timestamp": 0.8553717991090941
-        },
-        {
-            "x": 5.499389832161033,
-            "y": 4.11883296395985,
-            "heading": 0.5282284192975907,
-            "angularVelocity": 0.0000013356188425542589,
-            "velocityX": -3.611840944307328,
-            "velocityY": -1.1248296920470104,
-            "timestamp": 0.9331328717553754
-        },
-        {
-            "x": 5.2375954046767585,
-            "y": 3.9846908767107623,
-            "heading": 0.5282448470390027,
-            "angularVelocity": 0.0002112591919891304,
-            "velocityX": -3.3666514436488617,
-            "velocityY": -1.7250544865716144,
-            "timestamp": 1.0108939444016567
-        },
-        {
-            "x": 4.9990709441074195,
-            "y": 3.8334805845295827,
-            "heading": 0.56802081206443,
-            "angularVelocity": 0.5115151279660829,
-            "velocityX": -3.067401881842984,
-            "velocityY": -1.944549978997939,
-            "timestamp": 1.0886550170479379
-        },
-        {
-            "x": 4.779271759525853,
-            "y": 3.6735037985529155,
-            "heading": 0.6417219019727908,
-            "angularVelocity": 0.9477890080506808,
-            "velocityX": -2.826596613210087,
-            "velocityY": -2.057286255609729,
-            "timestamp": 1.166416089694219
-        },
-        {
-            "x": 4.5625385904493,
-            "y": 3.4912294193947493,
-            "heading": 0.6783171685956464,
-            "angularVelocity": 0.47061164895862556,
-            "velocityX": -2.7871679453603333,
-            "velocityY": -2.3440311836655594,
-            "timestamp": 1.2441771623405002
-        },
-        {
-            "x": 4.372028277853454,
-            "y": 3.267088484433369,
-            "heading": 0.6783172621445687,
-            "angularVelocity": 0.0000012030302442626518,
-            "velocityX": -2.4499445045265382,
-            "velocityY": -2.8824311102413667,
-            "timestamp": 1.3219382349867814
-        },
-        {
-            "x": 4.1999208824099785,
-            "y": 3.028525386131778,
-            "heading": 0.678317151805174,
-            "angularVelocity": -0.0000014189541221835924,
-            "velocityX": -2.213284739864076,
-            "velocityY": -3.067898759405809,
-            "timestamp": 1.3996993076330626
-        },
-        {
-            "x": 4.015537599692405,
-            "y": 2.8383590216563426,
-            "heading": 0.6532286408155417,
-            "angularVelocity": -0.32263586568249475,
-            "velocityX": -2.371151482905778,
-            "velocityY": -2.4455213644037763,
-            "timestamp": 1.4774603802793438
-        },
-        {
-            "x": 3.8266113642227695,
-            "y": 2.6983370404519986,
-            "heading": 0.6187227841336836,
-            "angularVelocity": -0.4437420357974964,
-            "velocityX": -2.429573423311953,
-            "velocityY": -1.8006693637223163,
-            "timestamp": 1.555221452925625
-        },
-        {
-            "x": 3.635277950307217,
-            "y": 2.6086556112169066,
-            "heading": 0.5790475746735078,
-            "angularVelocity": -0.5102194209775099,
-            "velocityX": -2.460529509229992,
-            "velocityY": -1.1532946522360021,
-            "timestamp": 1.6329825255719062
-        },
-        {
-            "x": 3.4425296783447266,
-            "y": 2.5693652629852295,
-            "heading": 0.5361762744054714,
-            "angularVelocity": -0.5513208448531661,
-            "velocityX": -2.4787244491760223,
-            "velocityY": -0.5052701421751292,
-            "timestamp": 1.7107435982181873
-        },
-        {
-            "x": 3.273952117174966,
-            "y": 2.5733990398146904,
-            "heading": 0.49726118807033537,
-            "angularVelocity": -0.5743772146540196,
-            "velocityX": -2.4881638242809143,
-            "velocityY": 0.05953756545439798,
-            "timestamp": 1.7784953915385004
-        },
-        {
-            "x": 3.1059881626789445,
-            "y": 2.6157002802478813,
-            "heading": 0.456882179671172,
-            "angularVelocity": -0.5959843484623638,
-            "velocityX": -2.4791071389347814,
-            "velocityY": 0.6243560260198714,
-            "timestamp": 1.8462471848588136
-        },
-        {
-            "x": 2.9403803042277463,
-            "y": 2.6962017103764078,
-            "heading": 0.41517848324514556,
-            "angularVelocity": -0.6155364217277942,
-            "velocityX": -2.444331734043506,
-            "velocityY": 1.1881815400506988,
-            "timestamp": 1.9139989781791267
-        },
-        {
-            "x": 2.7796953039379972,
-            "y": 2.8146585273254012,
-            "heading": 0.3723609476398507,
-            "angularVelocity": -0.6319764172568042,
-            "velocityX": -2.3716715442508174,
-            "velocityY": 1.7483938231562413,
-            "timestamp": 1.9817507714994398
-        },
-        {
-            "x": 2.628009703912687,
-            "y": 2.970316258543535,
-            "heading": 0.3287788631859365,
-            "angularVelocity": -0.6432609724124856,
-            "velocityX": -2.23884258396202,
-            "velocityY": 2.2974702748047298,
-            "timestamp": 2.0495025648197527
-        },
-        {
-            "x": 2.4923790399755665,
-            "y": 3.160718496478581,
-            "heading": 0.28508218813026037,
-            "angularVelocity": -0.6449523018392979,
-            "velocityX": -2.001875630005736,
-            "velocityY": 2.8102907480968544,
-            "timestamp": 2.1172543581400656
-        },
-        {
-            "x": 2.384956097652455,
-            "y": 3.376988776463332,
-            "heading": 0.24257394609876834,
-            "angularVelocity": -0.627411319291935,
-            "velocityX": -1.5855365158416335,
-            "velocityY": 3.192096760631575,
-            "timestamp": 2.1850061514603785
-        },
-        {
-            "x": 2.3144199991578422,
-            "y": 3.6018152629752547,
-            "heading": 0.17996830737750358,
-            "angularVelocity": -0.9240440090682317,
-            "velocityX": -1.0410956675513523,
-            "velocityY": 3.3183842890918167,
-            "timestamp": 2.2527579447806914
-        },
-        {
-            "x": 2.277461106698391,
-            "y": 3.8129334421080285,
-            "heading": 0.07321465766316056,
-            "angularVelocity": -1.57565791963083,
-            "velocityX": -0.5455042685692306,
-            "velocityY": 3.1160530044520236,
-            "timestamp": 2.3205097381010042
-        },
-        {
-            "x": 2.2642620699526455,
-            "y": 3.9942911908489713,
-            "heading": -0.04721496632936169,
-            "angularVelocity": -1.7775119755599884,
-            "velocityX": -0.1948145738865332,
-            "velocityY": 2.676796286166625,
-            "timestamp": 2.388261531421317
-        },
-        {
-            "x": 2.2685016834700633,
-            "y": 4.1415779064179725,
-            "heading": -0.1677701441610935,
-            "angularVelocity": -1.7793651197065368,
-            "velocityX": 0.06257566493294611,
-            "velocityY": 2.173916118687331,
-            "timestamp": 2.45601332474163
-        },
-        {
-            "x": 2.2803498467055876,
-            "y": 4.251629868834139,
-            "heading": -0.2721018795383186,
-            "angularVelocity": -1.5399110527447033,
-            "velocityX": 0.17487600925202312,
-            "velocityY": 1.624340213341216,
-            "timestamp": 2.523765118061943
-        },
-        {
-            "x": 2.2924250535428135,
-            "y": 4.3241863896986095,
-            "heading": -0.3487735727111741,
-            "angularVelocity": -1.1316555535345214,
-            "velocityX": 0.17822711762236845,
-            "velocityY": 1.0709166105971757,
-            "timestamp": 2.591516911382256
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.6084920452794201,
-            "velocityX": 0.1118043683563332,
-            "velocityY": 0.5286001823165565,
-            "timestamp": 2.6592687047025687
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -7.720140427747849e-31,
-            "velocityX": 0,
-            "velocityY": -1.167304580662352e-31,
-            "timestamp": 2.7270204980228816
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.1.traj
deleted file mode 100644
index 57334e24..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.1.traj	
+++ /dev/null
@@ -1,139 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -2.1749467140472343e-39,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 4.203445372005539,
-            "heading": -7.837837605587582e-19,
-            "angularVelocity": -8.185686568731239e-18,
-            "velocityX": 1.377912473786286e-33,
-            "velocityY": 0.7983807350946673,
-            "timestamp": 0.09575052183150974
-        },
-        {
-            "x": 7.68,
-            "y": 4.356336112982689,
-            "heading": -2.3565356003209568e-18,
-            "angularVelocity": -1.6425517161035916e-17,
-            "velocityX": 3.260516050321855e-34,
-            "velocityY": 1.5967614385035824,
-            "timestamp": 0.19150104366301948
-        },
-        {
-            "x": 7.68,
-            "y": 4.5856722168636,
-            "heading": -4.728579560129296e-18,
-            "angularVelocity": -2.4773170043438095e-17,
-            "velocityX": -1.693775140889528e-33,
-            "velocityY": 2.3951420785410225,
-            "timestamp": 0.28725156549452924
-        },
-        {
-            "x": 7.68,
-            "y": 4.891453665444746,
-            "heading": -7.932188825954499e-18,
-            "angularVelocity": -3.345787785122934e-17,
-            "velocityX": -9.607950389092107e-34,
-            "velocityY": 3.1935225284643685,
-            "timestamp": 0.38300208732603896
-        },
-        {
-            "x": 7.68,
-            "y": 5.253672199266821,
-            "heading": -4.35728250993897e-17,
-            "angularVelocity": -3.722239377050204e-16,
-            "velocityX": -3.660377688643743e-33,
-            "velocityY": 3.7829405719528495,
-            "timestamp": 0.4787526091575487
-        },
-        {
-            "x": 7.68,
-            "y": 5.61589073308894,
-            "heading": -3.405116375197899e-17,
-            "angularVelocity": 9.944239639932065e-17,
-            "velocityX": -5.791062266320423e-33,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.5745031309890585
-        },
-        {
-            "x": 7.68,
-            "y": 5.978109266911059,
-            "heading": 5.876472199762529e-18,
-            "angularVelocity": 4.169965362904391e-16,
-            "velocityX": -1.6171107443968654e-32,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.6702536528205683
-        },
-        {
-            "x": 7.68,
-            "y": 6.340327800733179,
-            "heading": 2.547705878984039e-17,
-            "angularVelocity": 2.047047495424479e-16,
-            "velocityX": -4.3724642588945734e-33,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.766004174652078
-        },
-        {
-            "x": 7.68,
-            "y": 6.702546334555254,
-            "heading": 7.687233753276096e-18,
-            "angularVelocity": -1.8579350479017399e-16,
-            "velocityX": -2.6103711792186515e-33,
-            "velocityY": 3.7829405719528495,
-            "timestamp": 0.8617546964835878
-        },
-        {
-            "x": 7.68,
-            "y": 7.0083277831364,
-            "heading": 4.613320516549708e-18,
-            "angularVelocity": -3.210335754994756e-17,
-            "velocityX": 1.2273832122580678e-32,
-            "velocityY": 3.1935225284643685,
-            "timestamp": 0.9575052183150976
-        },
-        {
-            "x": 7.68,
-            "y": 7.237663887017311,
-            "heading": 2.3068261479777784e-18,
-            "angularVelocity": -2.4088582761110343e-17,
-            "velocityX": 8.179933156696061e-33,
-            "velocityY": 2.3951420785410225,
-            "timestamp": 1.0532557401466074
-        },
-        {
-            "x": 7.68,
-            "y": 7.390554627994461,
-            "heading": 7.689558058679801e-19,
-            "angularVelocity": -1.6061221530256843e-17,
-            "velocityX": 9.843160057308665e-33,
-            "velocityY": 1.5967614385035824,
-            "timestamp": 1.1490062619781172
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": -8.030826271263815e-18,
-            "velocityX": 3.1854559448557055e-33,
-            "velocityY": 0.7983807350946673,
-            "timestamp": 1.244756783809627
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 2.257351446612169e-43,
-            "velocityY": -1.1946741683702123e-38,
-            "timestamp": 1.3405073056411367
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C2.1.traj
deleted file mode 100644
index 5ffe4bce..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C2.1.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -3.817834980565894e-40,
-            "angularVelocity": -1.9855221195343998e-39,
-            "velocityX": -9.621145750064654e-40,
-            "velocityY": 1.4575297691012433e-39,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 4.210500005575846,
-            "heading": 3.479114156010015e-22,
-            "angularVelocity": 3.4766412561552746e-21,
-            "velocityX": 2.7068848151306367e-33,
-            "velocityY": 0.8344065481228213,
-            "timestamp": 0.10007112931184182
-        },
-        {
-            "x": 7.68,
-            "y": 4.377500013629847,
-            "heading": 1.081691491825702e-21,
-            "angularVelocity": 7.332585052389997e-21,
-            "velocityX": 5.1589068200097806e-33,
-            "velocityY": 1.6688130652907358,
-            "timestamp": 0.20014225862368365
-        },
-        {
-            "x": 7.68,
-            "y": 4.628000017966616,
-            "heading": 2.2511586271387426e-21,
-            "angularVelocity": 1.1686358860616172e-20,
-            "velocityX": 5.8498916949463956e-33,
-            "velocityY": 2.503219520548835,
-            "timestamp": 0.3002133879355255
-        },
-        {
-            "x": 7.68,
-            "y": 4.962,
-            "heading": 4.481673966100434e-21,
-            "angularVelocity": 2.228929929167172e-20,
-            "velocityX": 6.576791251337367e-33,
-            "velocityY": 3.3376257900775013,
-            "timestamp": 0.4002845172473673
-        },
-        {
-            "x": 7.68,
-            "y": 5.295999982033384,
-            "heading": 1.4391637904192512e-21,
-            "angularVelocity": -3.0403475801105514e-20,
-            "velocityX": 1.0509973010204301e-32,
-            "velocityY": 3.3376257900775013,
-            "timestamp": 0.5003556465592092
-        },
-        {
-            "x": 7.68,
-            "y": 5.5464999863701525,
-            "heading": 4.7457500532831525e-22,
-            "angularVelocity": -9.63903176962576e-21,
-            "velocityX": 7.983324746463204e-33,
-            "velocityY": 2.503219520548835,
-            "timestamp": 0.600426775871051
-        },
-        {
-            "x": 7.68,
-            "y": 5.713499994424153,
-            "heading": 1.0909640932435146e-22,
-            "angularVelocity": -3.652188228166023e-21,
-            "velocityX": 7.341551448907972e-33,
-            "velocityY": 1.6688130652907358,
-            "timestamp": 0.7004979051828928
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -9.42965601107167e-40,
-            "angularVelocity": -1.09018866193586e-21,
-            "velocityX": 7.44470103227488e-33,
-            "velocityY": 0.8344065481228213,
-            "timestamp": 0.8005690344947346
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 8.249839679439821e-43,
-            "angularVelocity": 9.431229878048832e-39,
-            "velocityX": 1.012759498983901e-41,
-            "velocityY": 5.142787784847508e-39,
-            "timestamp": 0.9006401638065764
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.1.traj
deleted file mode 100644
index 1862a6fc..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.1.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -1.6773104267984464e-40,
-            "angularVelocity": -1.9855220924111835e-39,
-            "velocityX": -5.3948128499400994e-40,
-            "velocityY": -2.7550648847397363e-40,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 4.043499994424153,
-            "heading": -2.181197470484939e-22,
-            "angularVelocity": -2.1796471324426728e-21,
-            "velocityX": 2.2751291494664054e-33,
-            "velocityY": -0.8344065481228213,
-            "timestamp": 0.10007112931184182
-        },
-        {
-            "x": 7.68,
-            "y": 3.8764999863701526,
-            "heading": -1.0976856482379068e-21,
-            "angularVelocity": -8.789407177499062e-21,
-            "velocityX": 3.967849397509383e-33,
-            "velocityY": -1.668813065290736,
-            "timestamp": 0.20014225862368365
-        },
-        {
-            "x": 7.68,
-            "y": 3.6259999820333837,
-            "heading": -3.357689035831156e-21,
-            "angularVelocity": -2.2583970074080117e-20,
-            "velocityX": 6.1318806903820435e-33,
-            "velocityY": -2.503219520548835,
-            "timestamp": 0.3002133879355255
-        },
-        {
-            "x": 7.68,
-            "y": 3.2919999999999994,
-            "heading": -1.0603568936345678e-20,
-            "angularVelocity": -7.2407296109284e-20,
-            "velocityX": 8.195284274358776e-33,
-            "velocityY": -3.3376257900775017,
-            "timestamp": 0.4002845172473673
-        },
-        {
-            "x": 7.68,
-            "y": 2.9580000179666155,
-            "heading": -4.785144304302103e-21,
-            "angularVelocity": 5.814288954429956e-20,
-            "velocityX": 7.586727405106972e-33,
-            "velocityY": -3.3376257900775013,
-            "timestamp": 0.5003556465592092
-        },
-        {
-            "x": 7.68,
-            "y": 2.7075000136298466,
-            "heading": -2.6182090767908737e-21,
-            "angularVelocity": 2.165395003165498e-20,
-            "velocityX": 5.326134827450793e-33,
-            "velocityY": -2.503219520548835,
-            "timestamp": 0.600426775871051
-        },
-        {
-            "x": 7.68,
-            "y": 2.540500005575846,
-            "heading": -9.668053802454109e-22,
-            "angularVelocity": 1.6502299039719674e-20,
-            "velocityX": 3.4348416075821124e-33,
-            "velocityY": -1.6688130652907358,
-            "timestamp": 0.7004979051828928
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 1.8181769494337196e-40,
-            "angularVelocity": 9.661181877631647e-21,
-            "velocityX": 1.7280403247932335e-33,
-            "velocityY": -0.8344065481228213,
-            "timestamp": 0.8005690344947346
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -1.6883012335149235e-41,
-            "angularVelocity": -1.9855220810412424e-39,
-            "velocityX": -1.2101303998889038e-42,
-            "velocityY": -7.163168700323315e-39,
-            "timestamp": 0.9006401638065764
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.1.traj
deleted file mode 100644
index 9c555123..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.1.traj	
+++ /dev/null
@@ -1,139 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -1.1842498081533988e-39,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 4.050554627994461,
-            "heading": 1.1070575354189048e-18,
-            "angularVelocity": 1.156189558439314e-17,
-            "velocityX": 1.1339790734570631e-33,
-            "velocityY": -0.7983807350946678,
-            "timestamp": 0.09575052183150978
-        },
-        {
-            "x": 7.68,
-            "y": 3.897663887017311,
-            "heading": 3.320268139408775e-18,
-            "angularVelocity": 2.3114345091427606e-17,
-            "velocityX": 5.50963145740529e-34,
-            "velocityY": -1.596761438503583,
-            "timestamp": 0.19150104366301957
-        },
-        {
-            "x": 7.68,
-            "y": 3.6683277831364007,
-            "heading": 6.638014774282681e-18,
-            "angularVelocity": 3.4649906568297676e-17,
-            "velocityX": 4.767145470149766e-34,
-            "velocityY": -2.3951420785410233,
-            "timestamp": 0.28725156549452935
-        },
-        {
-            "x": 7.68,
-            "y": 3.362546334555254,
-            "heading": 1.1056344671920077e-17,
-            "angularVelocity": 4.614418609717799e-17,
-            "velocityX": -5.447572473811757e-34,
-            "velocityY": -3.19352252846437,
-            "timestamp": 0.38300208732603913
-        },
-        {
-            "x": 7.68,
-            "y": 3.0003278007331793,
-            "heading": 2.1759975564527066e-17,
-            "angularVelocity": 1.11786658577609e-16,
-            "velocityX": -1.9220305893434716e-33,
-            "velocityY": -3.7829405719528495,
-            "timestamp": 0.4787526091575489
-        },
-        {
-            "x": 7.68,
-            "y": 2.63810926691106,
-            "heading": -1.7246492437320545e-17,
-            "angularVelocity": -4.0737603572006183e-16,
-            "velocityX": 5.877602560652089e-33,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.5745031309890587
-        },
-        {
-            "x": 7.68,
-            "y": 2.27589073308894,
-            "heading": -9.722837303851897e-18,
-            "angularVelocity": 7.857560449339153e-17,
-            "velocityX": 3.350655179860002e-33,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.6702536528205685
-        },
-        {
-            "x": 7.68,
-            "y": 1.9136721992668206,
-            "heading": -4.541786403704224e-17,
-            "angularVelocity": -3.727919811862982e-16,
-            "velocityX": 1.4553646654154268e-32,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.7660041746520783
-        },
-        {
-            "x": 7.68,
-            "y": 1.5514536654447455,
-            "heading": -1.11893051502424e-17,
-            "angularVelocity": 3.5747647356983727e-16,
-            "velocityX": -5.35720215261278e-33,
-            "velocityY": -3.7829405719528495,
-            "timestamp": 0.861754696483588
-        },
-        {
-            "x": 7.68,
-            "y": 1.2456722168635992,
-            "heading": -6.7032993083141955e-18,
-            "angularVelocity": 4.6850980602319425e-17,
-            "velocityX": -4.3527390380082413e-33,
-            "velocityY": -3.19352252846437,
-            "timestamp": 0.9575052183150978
-        },
-        {
-            "x": 7.68,
-            "y": 1.016336112982689,
-            "heading": -3.348658686079055e-18,
-            "angularVelocity": 3.5035220254491646e-17,
-            "velocityX": -6.634490282826358e-33,
-            "velocityY": -2.395142078541024,
-            "timestamp": 1.0532557401466076
-        },
-        {
-            "x": 7.68,
-            "y": 0.8634453720055388,
-            "heading": -1.115588951241814e-18,
-            "angularVelocity": 2.3321750022200153e-17,
-            "velocityX": -4.820835446954302e-33,
-            "velocityY": -1.596761438503583,
-            "timestamp": 1.1490062619781174
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 1.1650996045214628e-17,
-            "velocityX": -3.709395222464697e-33,
-            "velocityY": -0.7983807350946678,
-            "timestamp": 1.2447567838096272
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 6.978360472267901e-43,
-            "velocityY": -3.6401965239950315e-38,
-            "timestamp": 1.340507305641137
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.1.traj
deleted file mode 100644
index e393ae58..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.1.traj	
+++ /dev/null
@@ -1,157 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -7.709535672280055e-33,
-            "angularVelocity": 3.0776034223095e-32,
-            "velocityX": -2.0787724059290825e-32,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.653625318689537,
-            "y": 4.193887618013532,
-            "heading": -0.05699658004084171,
-            "angularVelocity": -0.6065329552716214,
-            "velocityX": -0.28066795214939116,
-            "velocityY": 0.7117891037630026,
-            "timestamp": 0.0939711182145365
-        },
-        {
-            "x": 7.600609429037226,
-            "y": 4.328102182971276,
-            "heading": -0.1617251039431606,
-            "angularVelocity": -1.1144756590341216,
-            "velocityX": -0.5641721697008566,
-            "velocityY": 1.4282533560080801,
-            "timestamp": 0.187942236429073
-        },
-        {
-            "x": 7.520673672603614,
-            "y": 4.5302373715153195,
-            "heading": -0.299017155202882,
-            "angularVelocity": -1.4610026343017766,
-            "velocityX": -0.8506417498525306,
-            "velocityY": 2.151035258328725,
-            "timestamp": 0.2819133546436095
-        },
-        {
-            "x": 7.413680530437971,
-            "y": 4.80084724043498,
-            "heading": -0.43363333911195256,
-            "angularVelocity": -1.4325272111984575,
-            "velocityX": -1.1385747472045327,
-            "velocityY": 2.8797131933862543,
-            "timestamp": 0.375884472858146
-        },
-        {
-            "x": 7.279380160390009,
-            "y": 5.1284214711267,
-            "heading": -0.4371879316114584,
-            "angularVelocity": -0.037826436111898676,
-            "velocityX": -1.4291664566697349,
-            "velocityY": 3.4859032957750506,
-            "timestamp": 0.46985559107268254
-        },
-        {
-            "x": 7.147763767829931,
-            "y": 5.458645997615663,
-            "heading": -0.43718797458387054,
-            "angularVelocity": -4.5729382513446123e-7,
-            "velocityX": -1.4006047289934083,
-            "velocityY": 3.514106597466034,
-            "timestamp": 0.563826709287219
-        },
-        {
-            "x": 6.993957305230812,
-            "y": 5.779137314807096,
-            "heading": -0.43718801851214967,
-            "angularVelocity": -4.6746574877776706e-7,
-            "velocityX": -1.6367418577267254,
-            "velocityY": 3.410529993478958,
-            "timestamp": 0.6577978275017555
-        },
-        {
-            "x": 6.777425503833482,
-            "y": 6.0610683140345785,
-            "heading": -0.43718807107725144,
-            "angularVelocity": -5.593750798607861e-7,
-            "velocityX": -2.3042377861566687,
-            "velocityY": 3.000187766030755,
-            "timestamp": 0.751768945716292
-        },
-        {
-            "x": 6.507457353311083,
-            "y": 6.292343710685516,
-            "heading": -0.4371880918313372,
-            "angularVelocity": -2.2085600557820128e-7,
-            "velocityX": -2.87288430372895,
-            "velocityY": 2.4611327506280603,
-            "timestamp": 0.8457400639308286
-        },
-        {
-            "x": 6.1957042493791805,
-            "y": 6.462993619393473,
-            "heading": -0.4369141825473297,
-            "angularVelocity": 0.0029148241418374306,
-            "velocityX": -3.3175417070186306,
-            "velocityY": 1.8159825268692036,
-            "timestamp": 0.9397111821453651
-        },
-        {
-            "x": 5.885878112264826,
-            "y": 6.564806359719212,
-            "heading": -0.3410861416211598,
-            "angularVelocity": 1.0197605684269313,
-            "velocityX": -3.297035759508787,
-            "velocityY": 1.083447151211916,
-            "timestamp": 1.0336823003599016
-        },
-        {
-            "x": 5.602663899704114,
-            "y": 6.605182844765102,
-            "heading": -0.13545254769192583,
-            "angularVelocity": 2.1882637754695176,
-            "velocityX": -3.0138431673669435,
-            "velocityY": 0.4296690920896744,
-            "timestamp": 1.127653418574438
-        },
-        {
-            "x": 5.388992479168701,
-            "y": 6.621862905384601,
-            "heading": 0.0527880784098822,
-            "angularVelocity": 2.0031753338515554,
-            "velocityX": -2.2737988500636965,
-            "velocityY": 0.17750199142483714,
-            "timestamp": 1.2216245367889744
-        },
-        {
-            "x": 5.246852509457665,
-            "y": 6.628278045699216,
-            "heading": 0.19017469868317557,
-            "angularVelocity": 1.4620089968455965,
-            "velocityX": -1.5125920858633375,
-            "velocityY": 0.06826714884852915,
-            "timestamp": 1.3155956550035108
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": 0.7749753615846516,
-            "velocityX": -0.7539817638001085,
-            "velocityY": 0.01832429296896703,
-            "timestamp": 1.4095667732180472
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": 6.971645129510918e-32,
-            "velocityX": 3.634932162806737e-32,
-            "velocityY": 0,
-            "timestamp": 1.5035378914325837
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S2.1.traj
deleted file mode 100644
index 67cbd06a..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S2.1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 1.358270377512338e-32,
-            "angularVelocity": -4.2989327725546786e-32,
-            "velocityX": 9.28706448380456e-34,
-            "velocityY": 4.1113328098198935e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 7.585313636391529,
-            "y": 4.127746947940637,
-            "heading": -0.08924586799649764,
-            "angularVelocity": -0.8241960155421162,
-            "velocityX": -0.8744396280098535,
-            "velocityY": 0.006898151480975746,
-            "timestamp": 0.10828233370892486
-        },
-        {
-            "x": 7.394793136195202,
-            "y": 4.129317895600586,
-            "heading": -0.2483761167773939,
-            "angularVelocity": -1.4695864351119023,
-            "velocityX": -1.7594790735529262,
-            "velocityY": 0.014507885138232623,
-            "timestamp": 0.21656466741784972
-        },
-        {
-            "x": 7.106938899563738,
-            "y": 4.131715387581193,
-            "heading": -0.440143741107867,
-            "angularVelocity": -1.7709964106053178,
-            "velocityX": -2.6583674988502586,
-            "velocityY": 0.022141118486160887,
-            "timestamp": 0.32484700112677456
-        },
-        {
-            "x": 6.726884682874397,
-            "y": 4.136782702937484,
-            "heading": -0.5152103460374668,
-            "angularVelocity": -0.6932488648738155,
-            "velocityX": -3.509845084341914,
-            "velocityY": 0.04679724921622947,
-            "timestamp": 0.43312933483569943
-        },
-        {
-            "x": 6.317264615210477,
-            "y": 4.138917325587368,
-            "heading": -0.515210397277194,
-            "angularVelocity": -4.7320486478177106e-7,
-            "velocityX": -3.782889171607859,
-            "velocityY": 0.019713489511795114,
-            "timestamp": 0.5414116685446243
-        },
-        {
-            "x": 5.907644548289307,
-            "y": 4.14105209076814,
-            "heading": -0.5152104485168384,
-            "angularVelocity": -4.732041015646636e-7,
-            "velocityX": -3.7828891647484775,
-            "velocityY": 0.019714805801191258,
-            "timestamp": 0.6496940022535491
-        },
-        {
-            "x": 5.500258277815471,
-            "y": 4.183825709430579,
-            "heading": -0.5152105024899405,
-            "angularVelocity": -4.984479029228616e-7,
-            "velocityX": -3.762259793632967,
-            "velocityY": 0.3950193646307994,
-            "timestamp": 0.7579763359624739
-        },
-        {
-            "x": 5.114610765355458,
-            "y": 4.3219164843017905,
-            "heading": -0.5152109227554116,
-            "angularVelocity": -0.000003881200715936794,
-            "velocityX": -3.5614998241234512,
-            "velocityY": 1.2752844359857862,
-            "timestamp": 0.8662586696713988
-        },
-        {
-            "x": 4.790273412388094,
-            "y": 4.533322369220829,
-            "heading": -0.5901314422767667,
-            "angularVelocity": -0.6918997490647923,
-            "velocityX": -2.9952933397170605,
-            "velocityY": 1.952358041038544,
-            "timestamp": 0.9745410033803236
-        },
-        {
-            "x": 4.547771251602889,
-            "y": 4.790487049727204,
-            "heading": -0.7662492650374113,
-            "angularVelocity": -1.6264686650923976,
-            "velocityX": -2.239535780943542,
-            "velocityY": 2.3749458632620883,
-            "timestamp": 1.0828233370892484
-        },
-        {
-            "x": 4.394005608234175,
-            "y": 5.057835455573767,
-            "heading": -0.8024780004938983,
-            "angularVelocity": -0.3345766037318139,
-            "velocityX": -1.4200436775037824,
-            "velocityY": 2.468993756315091,
-            "timestamp": 1.1911056707981733
-        },
-        {
-            "x": 4.299277084870805,
-            "y": 5.304155354811625,
-            "heading": -0.5153879845736841,
-            "angularVelocity": 2.651309831315093,
-            "velocityX": -0.8748289782709269,
-            "velocityY": 2.2747930414945974,
-            "timestamp": 1.2993880045070982
-        },
-        {
-            "x": 4.268346904630433,
-            "y": 5.476762087961931,
-            "heading": -0.20205360473699635,
-            "angularVelocity": 2.893679597624539,
-            "velocityX": -0.2856438273995415,
-            "velocityY": 1.5940433424190128,
-            "timestamp": 1.4076703382160232
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 2.864290108945775e-30,
-            "angularVelocity": 1.8659886411401074,
-            "velocityX": -0.04937928882107947,
-            "velocityY": 0.7687118404911842,
-            "timestamp": 1.515952671924948
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -1.339793214188515e-31,
-            "velocityX": -1.755749143439243e-31,
-            "velocityY": 0,
-            "timestamp": 1.624235005633873
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S3.1.traj
deleted file mode 100644
index c2db4e34..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S3.1.traj	
+++ /dev/null
@@ -1,166 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 4.8594393424661863e-32,
-            "velocityX": 8.070887902586468e-34,
-            "velocityY": -1.5484627527783442e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.613090966537703,
-            "y": 4.130415229492922,
-            "heading": -0.02261774188189381,
-            "angularVelocity": -0.25176724339812206,
-            "velocityX": -0.7447915447554241,
-            "velocityY": 0.038016302406854975,
-            "timestamp": 0.08983591977315897
-        },
-        {
-            "x": 7.479193313628693,
-            "y": 4.137259099498979,
-            "heading": -0.06454119971556918,
-            "angularVelocity": -0.46666698504702897,
-            "velocityX": -1.4904689933291897,
-            "velocityY": 0.0761818883097586,
-            "timestamp": 0.17967183954631794
-        },
-        {
-            "x": 7.278200146895047,
-            "y": 4.147551727639681,
-            "heading": -0.12012980810986089,
-            "angularVelocity": -0.6187793093216658,
-            "velocityX": -2.237336326507015,
-            "velocityY": 0.11457141160316474,
-            "timestamp": 0.2695077593194769
-        },
-        {
-            "x": 7.010006404514947,
-            "y": 4.161329433657577,
-            "heading": -0.1775477987284744,
-            "angularVelocity": -0.6391429036434741,
-            "velocityX": -2.985373145364283,
-            "velocityY": 0.15336522465649807,
-            "timestamp": 0.35934367909263587
-        },
-        {
-            "x": 6.675632733992502,
-            "y": 4.178685588756415,
-            "heading": -0.19285230682653456,
-            "angularVelocity": -0.17036067687364617,
-            "velocityX": -3.7220487235639528,
-            "velocityY": 0.19319839038333309,
-            "timestamp": 0.44917959886579484
-        },
-        {
-            "x": 6.3362226416697665,
-            "y": 4.195852203162663,
-            "heading": -0.19285232652882817,
-            "angularVelocity": -2.193200640663355e-7,
-            "velocityX": -3.7781111740035236,
-            "velocityY": 0.1910885362012055,
-            "timestamp": 0.5390155186389538
-        },
-        {
-            "x": 5.996812544590127,
-            "y": 4.213018723498308,
-            "heading": -0.19285234623213549,
-            "angularVelocity": -2.1933156264800868e-7,
-            "velocityX": -3.7781112269558137,
-            "velocityY": 0.1910874890356692,
-            "timestamp": 0.6288514384121128
-        },
-        {
-            "x": 5.65703115405788,
-            "y": 4.206499194085455,
-            "heading": -0.1928524148916534,
-            "angularVelocity": -7.642946895727943e-7,
-            "velocityX": -3.782244244720454,
-            "velocityY": -0.07257152185432766,
-            "timestamp": 0.7186873581852717
-        },
-        {
-            "x": 5.325195518902672,
-            "y": 4.133157682156235,
-            "heading": -0.192852601151229,
-            "angularVelocity": -0.0000020733763948999505,
-            "velocityX": -3.6937968241559735,
-            "velocityY": -0.8163940672507154,
-            "timestamp": 0.8085232779584307
-        },
-        {
-            "x": 5.022921580906399,
-            "y": 3.999719932132448,
-            "heading": -0.21619143682617745,
-            "angularVelocity": -0.2597940304978627,
-            "velocityX": -3.364733602738329,
-            "velocityY": -1.485349628067107,
-            "timestamp": 0.8983591977315897
-        },
-        {
-            "x": 4.76405537852928,
-            "y": 3.8156128777716756,
-            "heading": -0.2707600447886353,
-            "angularVelocity": -0.6074252715762222,
-            "velocityX": -2.881544520603651,
-            "velocityY": -2.0493701720293274,
-            "timestamp": 0.9881951175047486
-        },
-        {
-            "x": 4.5584097754694755,
-            "y": 3.590752248351908,
-            "heading": -0.3464948618413025,
-            "angularVelocity": -0.8430349156856024,
-            "velocityX": -2.2891244791432657,
-            "velocityY": -2.5030147182477354,
-            "timestamp": 1.0780310372779076
-        },
-        {
-            "x": 4.420003796304514,
-            "y": 3.3682412112408735,
-            "heading": -0.42679529224450924,
-            "angularVelocity": -0.8938566066393836,
-            "velocityX": -1.5406529984228516,
-            "velocityY": -2.476860454851527,
-            "timestamp": 1.1678669570510665
-        },
-        {
-            "x": 4.3340794956684014,
-            "y": 3.187695881574611,
-            "heading": -0.4942860320434666,
-            "angularVelocity": -0.7512667535241839,
-            "velocityX": -0.9564581834687845,
-            "velocityY": -2.009723172222817,
-            "timestamp": 1.2577028768242253
-        },
-        {
-            "x": 4.284833149556787,
-            "y": 3.063288277698013,
-            "heading": -0.5416494132844137,
-            "angularVelocity": -0.5272209752980105,
-            "velocityX": -0.5481810197605272,
-            "velocityY": -1.3848314147664058,
-            "timestamp": 1.3475387965973842
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -0.27105624093568903,
-            "velocityX": -0.24303362855860308,
-            "velocityY": -0.7044874461979944,
-            "timestamp": 1.437374716370543
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 6.221864662981848e-31,
-            "velocityX": 0,
-            "velocityY": 2.6923638006152617e-32,
-            "timestamp": 1.527210636143702
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S4.1.traj
deleted file mode 100644
index 2274eedc..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S4.1.traj	
+++ /dev/null
@@ -1,238 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 3.185769476987751e-31,
-            "velocityX": 7.333541525795099e-32,
-            "velocityY": -1.3517842061565195e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 7.679078910576486,
-            "y": 4.080614551713116,
-            "heading": -0.023748331346269917,
-            "angularVelocity": -0.3167839884646853,
-            "velocityX": -0.012286605616975374,
-            "velocityY": -0.6187452541731726,
-            "timestamp": 0.07496695606800095
-        },
-        {
-            "x": 7.6772274783441,
-            "y": 3.987757014244699,
-            "heading": -0.06889026621284731,
-            "angularVelocity": -0.6021577670253255,
-            "velocityX": -0.024696644088144176,
-            "velocityY": -1.2386462294692595,
-            "timestamp": 0.1499339121360019
-        },
-        {
-            "x": 7.674435217742111,
-            "y": 3.848312298051719,
-            "heading": -0.1318457680879735,
-            "angularVelocity": -0.8397766853177616,
-            "velocityX": -0.037246551660047825,
-            "velocityY": -1.8600824083946148,
-            "timestamp": 0.22490086820400285
-        },
-        {
-            "x": 7.670687411814683,
-            "y": 3.6621317365631327,
-            "heading": -0.20653378464827088,
-            "angularVelocity": -0.996279167218362,
-            "velocityX": -0.049992771802326974,
-            "velocityY": -2.483501681990474,
-            "timestamp": 0.2998678242720038
-        },
-        {
-            "x": 7.665952301925262,
-            "y": 3.4291021726627307,
-            "heading": -0.27999342706200125,
-            "angularVelocity": -0.9798936260280143,
-            "velocityX": -0.06316262707958344,
-            "velocityY": -3.1084303821676644,
-            "timestamp": 0.3748347803400048
-        },
-        {
-            "x": 7.659724245559776,
-            "y": 3.1517739981457797,
-            "heading": -0.2973184560258276,
-            "angularVelocity": -0.23110220652567398,
-            "velocityX": -0.0830773542389579,
-            "velocityY": -3.699338869586643,
-            "timestamp": 0.44980173640800575
-        },
-        {
-            "x": 7.61848609595722,
-            "y": 2.8711927485616604,
-            "heading": -0.2973184850947423,
-            "angularVelocity": -3.8775636846908307e-7,
-            "velocityX": -0.5500843540339733,
-            "velocityY": -3.742732322352917,
-            "timestamp": 0.5247686924760067
-        },
-        {
-            "x": 7.5316071002697536,
-            "y": 2.6012326390018865,
-            "heading": -0.2973185190564447,
-            "angularVelocity": -4.530222940715824e-7,
-            "velocityX": -1.1588972027711093,
-            "velocityY": -3.6010547008857787,
-            "timestamp": 0.5997356485440076
-        },
-        {
-            "x": 7.401459139481823,
-            "y": 2.3492645367371554,
-            "heading": -0.29731856454052086,
-            "angularVelocity": -6.067216612944958e-7,
-            "velocityX": -1.7360710320141561,
-            "velocityY": -3.3610555300681284,
-            "timestamp": 0.6747026046120086
-        },
-        {
-            "x": 7.231595664022054,
-            "y": 2.1221680676798096,
-            "heading": -0.2973186386883636,
-            "angularVelocity": -9.89073675332688e-7,
-            "velocityX": -2.265844638345654,
-            "velocityY": -3.0292875817359826,
-            "timestamp": 0.7496695606800096
-        },
-        {
-            "x": 7.026654548839687,
-            "y": 1.9261437915792998,
-            "heading": -0.2973188151543491,
-            "angularVelocity": -0.000002353916908579506,
-            "velocityX": -2.7337526549226907,
-            "velocityY": -2.614809062311352,
-            "timestamp": 0.8246365167480105
-        },
-        {
-            "x": 6.792231559753418,
-            "y": 1.766543984413147,
-            "heading": -0.2973195709317279,
-            "angularVelocity": -0.00001008147347055336,
-            "velocityX": -3.127017573897922,
-            "velocityY": -2.1289354075064555,
-            "timestamp": 0.8996034728160115
-        },
-        {
-            "x": 6.610935711613446,
-            "y": 1.6724691195207,
-            "heading": -0.2973321063250889,
-            "angularVelocity": -0.00023216320074468077,
-            "velocityX": -3.3577106919370485,
-            "velocityY": -1.7423243992220112,
-            "timestamp": 0.9535973587619662
-        },
-        {
-            "x": 6.419882816475647,
-            "y": 1.600658398177247,
-            "heading": -0.29770902924967546,
-            "angularVelocity": -0.006980844552986395,
-            "velocityX": -3.538417207626563,
-            "velocityY": -1.329978757508455,
-            "timestamp": 1.007591244707921
-        },
-        {
-            "x": 6.223144239006287,
-            "y": 1.5524565272909876,
-            "heading": -0.30199019992690734,
-            "angularVelocity": -0.07928991592723345,
-            "velocityX": -3.6437195438439516,
-            "velocityY": -0.892728316211746,
-            "timestamp": 1.0615851306538757
-        },
-        {
-            "x": 6.027009837481886,
-            "y": 1.528287215105613,
-            "heading": -0.31920772077283077,
-            "angularVelocity": -0.31887908314582397,
-            "velocityX": -3.63252983348375,
-            "velocityY": -0.44763053745692566,
-            "timestamp": 1.1155790165998305
-        },
-        {
-            "x": 5.840489459577601,
-            "y": 1.5253276945335545,
-            "heading": -0.3622266817701244,
-            "angularVelocity": -0.7967376350788925,
-            "velocityX": -3.454472198777908,
-            "velocityY": -0.05481214252704887,
-            "timestamp": 1.1695729025457853
-        },
-        {
-            "x": 5.673855769295343,
-            "y": 1.5359985529910274,
-            "heading": -0.41631972448131044,
-            "angularVelocity": -1.0018364443214895,
-            "velocityX": -3.086158504113808,
-            "velocityY": 0.19763086635687102,
-            "timestamp": 1.22356678849174
-        },
-        {
-            "x": 5.530708201179271,
-            "y": 1.5529237053226916,
-            "heading": -0.47196047387840134,
-            "angularVelocity": -1.0305009247305976,
-            "velocityX": -2.651181066303626,
-            "velocityY": 0.3134642383140704,
-            "timestamp": 1.2775606744376948
-        },
-        {
-            "x": 5.411784437145291,
-            "y": 1.57153818466476,
-            "heading": -0.5236032237572267,
-            "angularVelocity": -0.956455512954414,
-            "velocityX": -2.2025413053807186,
-            "velocityY": 0.34475161429762524,
-            "timestamp": 1.3315545603836496
-        },
-        {
-            "x": 5.3170603467926,
-            "y": 1.5890782395606275,
-            "heading": -0.5679992830659408,
-            "angularVelocity": -0.8222423433861833,
-            "velocityX": -1.754348454332506,
-            "velocityY": 0.32485261226475975,
-            "timestamp": 1.3855484463296044
-        },
-        {
-            "x": 5.246334967071984,
-            "y": 1.6037618746917708,
-            "heading": -0.6030801100679523,
-            "angularVelocity": -0.6497185077054303,
-            "velocityX": -1.309877562644932,
-            "velocityY": 0.2719499601462664,
-            "timestamp": 1.4395423322755592
-        },
-        {
-            "x": 5.199384048260516,
-            "y": 1.6143645569232876,
-            "heading": -0.6274260701015184,
-            "angularVelocity": -0.45090216432919045,
-            "velocityX": -0.8695599138477248,
-            "velocityY": 0.19636820069091412,
-            "timestamp": 1.493536218221514
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -0.23287692075121041,
-            "velocityX": -0.4330869662524824,
-            "velocityY": 0.10437187429616973,
-            "timestamp": 1.5475301041674687
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0,
-            "velocityX": 3.9288115092123874e-33,
-            "velocityY": 2.0888895254257856e-32,
-            "timestamp": 1.6015239901134235
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.1.traj
deleted file mode 100644
index 47660d82..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.1.traj	
+++ /dev/null
@@ -1,220 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 1.832751405105417e-34,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.608571974046609,
-            "y": 4.129750388677263,
-            "heading": -0.02440208863451916,
-            "angularVelocity": -0.26295738388657636,
-            "velocityX": -0.7697098032139116,
-            "velocityY": 0.029638242122484983,
-            "timestamp": 0.09279864392416992
-        },
-        {
-            "x": 7.46571594445247,
-            "y": 4.135252523590067,
-            "heading": -0.07320447592180841,
-            "angularVelocity": -0.5258954788948677,
-            "velocityX": -1.5394193659863489,
-            "velocityY": 0.05929111331948823,
-            "timestamp": 0.18559728784833984
-        },
-        {
-            "x": 7.25143160309568,
-            "y": 4.143508368339934,
-            "heading": -0.1463869418885076,
-            "angularVelocity": -0.7886156830752087,
-            "velocityX": -2.3091322490864354,
-            "velocityY": 0.08896514432486853,
-            "timestamp": 0.27839593177250976
-        },
-        {
-            "x": 6.965717910609067,
-            "y": 4.154520147774138,
-            "heading": -0.24389107124776152,
-            "angularVelocity": -1.050706402983198,
-            "velocityX": -3.078856332427471,
-            "velocityY": 0.1186631503279318,
-            "timestamp": 0.3711945756966797
-        },
-        {
-            "x": 6.614926201978592,
-            "y": 4.168029747138803,
-            "heading": -0.24389108357148506,
-            "angularVelocity": -1.3280068591073525e-7,
-            "velocityX": -3.780138305869253,
-            "velocityY": 0.1455797067002448,
-            "timestamp": 0.4639932196208496
-        },
-        {
-            "x": 6.264134491970206,
-            "y": 4.181539310738243,
-            "heading": -0.2438910958936344,
-            "angularVelocity": -1.3278372185354406e-7,
-            "velocityX": -3.7801383207176356,
-            "velocityY": 0.14557932129349355,
-            "timestamp": 0.5567918635450195
-        },
-        {
-            "x": 5.913342805476888,
-            "y": 4.195049484934711,
-            "heading": -0.2438911082157909,
-            "angularVelocity": -1.32783799697389e-7,
-            "velocityX": -3.780138067318815,
-            "velocityY": 0.14558590109896277,
-            "timestamp": 0.6495905074691894
-        },
-        {
-            "x": 5.568285864608835,
-            "y": 4.259648543779433,
-            "heading": -0.24389114467773698,
-            "angularVelocity": -3.929146432741106e-7,
-            "velocityX": -3.718340336417024,
-            "velocityY": 0.6961207202291138,
-            "timestamp": 0.7423891513933594
-        },
-        {
-            "x": 5.223229300660808,
-            "y": 4.324249615558004,
-            "heading": -0.2438911811402195,
-            "angularVelocity": -3.929204239211486e-7,
-            "velocityX": -3.7183362747195923,
-            "velocityY": 0.6961424116429128,
-            "timestamp": 0.8351877953175293
-        },
-        {
-            "x": 4.889178228675737,
-            "y": 4.432171862538056,
-            "heading": -0.2438912192984801,
-            "angularVelocity": -4.111941617608203e-7,
-            "velocityX": -3.5997408783046705,
-            "velocityY": 1.1629722420106654,
-            "timestamp": 0.9279864392416992
-        },
-        {
-            "x": 4.584073695522117,
-            "y": 4.605805251226935,
-            "heading": -0.24389116821114407,
-            "angularVelocity": 5.505181312773355e-7,
-            "velocityX": -3.287812410307864,
-            "velocityY": 1.8710767889103372,
-            "timestamp": 1.0207850831658691
-        },
-        {
-            "x": 4.320679858830311,
-            "y": 4.837885010533044,
-            "heading": -0.24389110572504064,
-            "angularVelocity": 6.733514712028696e-7,
-            "velocityX": -2.8383371303038154,
-            "velocityY": 2.5008960205899418,
-            "timestamp": 1.113583727090039
-        },
-        {
-            "x": 4.066259556785684,
-            "y": 5.079768532204951,
-            "heading": -0.24389104453640137,
-            "angularVelocity": 6.593699721479525e-7,
-            "velocityX": -2.7416381456233947,
-            "velocityY": 2.606541555386963,
-            "timestamp": 1.206382371014209
-        },
-        {
-            "x": 3.8118393280605467,
-            "y": 5.321652130996455,
-            "heading": -0.243890983347767,
-            "angularVelocity": 6.593699195322184e-7,
-            "velocityX": -2.741637355531139,
-            "velocityY": 2.606542386429263,
-            "timestamp": 1.299181014938379
-        },
-        {
-            "x": 3.557419123484207,
-            "y": 5.563535755188363,
-            "heading": -0.24389092215913197,
-            "angularVelocity": 6.593699267467298e-7,
-            "velocityX": -2.7416370953032203,
-            "velocityY": 2.606542660144498,
-            "timestamp": 1.3919796588625488
-        },
-        {
-            "x": 3.3029989365055474,
-            "y": 5.805419397890013,
-            "heading": -0.24389086097049373,
-            "angularVelocity": 6.593699609263459e-7,
-            "velocityX": -2.7416369056702816,
-            "velocityY": 2.606542859605842,
-            "timestamp": 1.4847783027867187
-        },
-        {
-            "x": 3.0485787435735707,
-            "y": 6.04730303432977,
-            "heading": -0.24389079978182,
-            "angularVelocity": 6.593703437871394e-7,
-            "velocityX": -2.741636969823341,
-            "velocityY": 2.6065427921275584,
-            "timestamp": 1.5775769467108887
-        },
-        {
-            "x": 2.7984729856087975,
-            "y": 6.275491677578651,
-            "heading": -0.21231080129448254,
-            "angularVelocity": 0.3403066807004949,
-            "velocityX": -2.6951445343225506,
-            "velocityY": 2.45896527793385,
-            "timestamp": 1.6703755906350586
-        },
-        {
-            "x": 2.596978431795094,
-            "y": 6.469397893884434,
-            "heading": -0.03732218251039644,
-            "angularVelocity": 1.8856807748945335,
-            "velocityX": -2.171309248638947,
-            "velocityY": 2.089537175394848,
-            "timestamp": 1.7631742345592285
-        },
-        {
-            "x": 2.4472498512530994,
-            "y": 6.613867328015145,
-            "heading": 0.15789573469158452,
-            "angularVelocity": 2.1036720898801096,
-            "velocityX": -1.6134781092743464,
-            "velocityY": 1.5568054447946784,
-            "timestamp": 1.8559728784833984
-        },
-        {
-            "x": 2.3487140682465175,
-            "y": 6.7094536627437265,
-            "heading": 0.3099829221841124,
-            "angularVelocity": 1.6388945038552638,
-            "velocityX": -1.0618235228426263,
-            "velocityY": 1.0300402105734285,
-            "timestamp": 1.9487715224075683
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.8945936525078311,
-            "velocityX": -0.5249437511858832,
-            "velocityY": 0.5123602592202229,
-            "timestamp": 2.0415701663317383
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0,
-            "velocityX": 1.4564927652554838e-31,
-            "velocityY": 0,
-            "timestamp": 2.134368810255908
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.1.traj
deleted file mode 100644
index 2f31d71f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.1.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -8.248091288877123e-33,
-            "velocityX": -1.7499589770527321e-34,
-            "velocityY": 1.243057151683689e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 7.599512207946833,
-            "y": 4.126338330595212,
-            "heading": -0.05672955044148203,
-            "angularVelocity": -0.5720722032058021,
-            "velocityX": -0.8116550928518662,
-            "velocityY": -0.0066724074357217035,
-            "timestamp": 0.09916501819801522
-        },
-        {
-            "x": 7.438184902121127,
-            "y": 4.125091465076226,
-            "heading": -0.16291927611759752,
-            "angularVelocity": -1.070838563898341,
-            "velocityX": -1.6268570183042166,
-            "velocityY": -0.01257364281924666,
-            "timestamp": 0.19833003639603045
-        },
-        {
-            "x": 7.19545269041534,
-            "y": 4.123364615129754,
-            "heading": -0.30423484322627714,
-            "angularVelocity": -1.4250546178138856,
-            "velocityX": -2.4477604715515007,
-            "velocityY": -0.017413902380612675,
-            "timestamp": 0.2974950545940457
-        },
-        {
-            "x": 6.870785634071806,
-            "y": 4.1216134281783505,
-            "heading": -0.4343749203987102,
-            "angularVelocity": -1.3123587282822462,
-            "velocityX": -3.274007933878773,
-            "velocityY": -0.01765932163604032,
-            "timestamp": 0.3966600727920609
-        },
-        {
-            "x": 6.495672194218458,
-            "y": 4.117557524267507,
-            "heading": -0.4343749552288927,
-            "angularVelocity": -3.5123456983963544e-7,
-            "velocityX": -3.782719417287976,
-            "velocityY": -0.040900551268437134,
-            "timestamp": 0.4958250909900761
-        },
-        {
-            "x": 6.120558752684864,
-            "y": 4.113501775750749,
-            "heading": -0.43437499005887026,
-            "angularVelocity": -3.512325030859258e-7,
-            "velocityX": -3.782719434231919,
-            "velocityY": -0.04089898424321494,
-            "timestamp": 0.5949901091880914
-        },
-        {
-            "x": 5.747019040696225,
-            "y": 4.148065138700183,
-            "heading": -0.43437502656757854,
-            "angularVelocity": -3.681611615448704e-7,
-            "velocityX": -3.766849628794958,
-            "velocityY": 0.3485439076955189,
-            "timestamp": 0.6941551273861066
-        },
-        {
-            "x": 5.389911454629062,
-            "y": 4.262959433025878,
-            "heading": -0.434375162279621,
-            "angularVelocity": -0.0000013685475476133038,
-            "velocityX": -3.601144764115114,
-            "velocityY": 1.1586171859140066,
-            "timestamp": 0.7933201455841219
-        },
-        {
-            "x": 5.045913588511523,
-            "y": 4.412598985545015,
-            "heading": -0.4343752985502662,
-            "angularVelocity": -0.0000013741806101348962,
-            "velocityX": -3.468943709873945,
-            "velocityY": 1.5089953618556682,
-            "timestamp": 0.8924851637821372
-        },
-        {
-            "x": 4.713243052034498,
-            "y": 4.585968073714987,
-            "heading": -0.43437543856751193,
-            "angularVelocity": -0.0000014119620841874337,
-            "velocityX": -3.354716638207433,
-            "velocityY": 1.7482887748155678,
-            "timestamp": 0.9916501819801524
-        },
-        {
-            "x": 4.426185676495707,
-            "y": 4.82747296096862,
-            "heading": -0.4343754384530987,
-            "angularVelocity": 1.153766536466057e-9,
-            "velocityX": -2.894744343873237,
-            "velocityY": 2.4353838847827403,
-            "timestamp": 1.0908152001781677
-        },
-        {
-            "x": 4.09351517988242,
-            "y": 5.000842166204109,
-            "heading": -0.4343753660253311,
-            "angularVelocity": 7.303761838407394e-7,
-            "velocityX": -3.354716236213479,
-            "velocityY": 1.748289955327798,
-            "timestamp": 1.189980218376183
-        },
-        {
-            "x": 3.752063829141965,
-            "y": 5.156204697333757,
-            "heading": -0.43437529472941433,
-            "angularVelocity": 7.189623727917814e-7,
-            "velocityX": -3.4432641363372296,
-            "velocityY": 1.5667070298864525,
-            "timestamp": 1.2891452365741982
-        },
-        {
-            "x": 3.4085469650969755,
-            "y": 5.306945220863998,
-            "heading": -0.43437522535251666,
-            "angularVelocity": 6.996105981774619e-7,
-            "velocityX": -3.46409318817495,
-            "velocityY": 1.5200977750968405,
-            "timestamp": 1.3883102547722135
-        },
-        {
-            "x": 3.0417944650952866,
-            "y": 5.3858068126004595,
-            "heading": -0.43437520041032157,
-            "angularVelocity": 2.5152211488411685e-7,
-            "velocityX": -3.6984060172242224,
-            "velocityY": 0.7952561615930772,
-            "timestamp": 1.4874752729702287
-        },
-        {
-            "x": 2.7246409461958954,
-            "y": 5.456187357907523,
-            "heading": -0.2777696543737322,
-            "angularVelocity": 1.5792418423589194,
-            "velocityX": -3.1982399102281316,
-            "velocityY": 0.7097315826285274,
-            "timestamp": 1.586640291168244
-        },
-        {
-            "x": 2.4871805723397817,
-            "y": 5.508279050924135,
-            "heading": -0.14311381126711797,
-            "angularVelocity": 1.3578966207390795,
-            "velocityX": -2.394598197742947,
-            "velocityY": 0.5253031155865293,
-            "timestamp": 1.6858053093662593
-        },
-        {
-            "x": 2.3290265259060328,
-            "y": 5.5428007138640085,
-            "heading": -0.04864902301750531,
-            "angularVelocity": 0.9526019353012469,
-            "velocityX": -1.594857231992267,
-            "velocityY": 0.3481233964071807,
-            "timestamp": 1.7849703275642745
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -2.5681051863102055e-32,
-            "angularVelocity": 0.490586538494469,
-            "velocityX": -0.7969193909512572,
-            "velocityY": 0.1734410626703791,
-            "timestamp": 1.8841353457622898
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 1.14717044306562e-31,
-            "velocityX": 0,
-            "velocityY": 5.846493116157662e-32,
-            "timestamp": 1.983300363960305
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W3.1.traj
deleted file mode 100644
index 3874f8b2..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W3.1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 5.58164160972529e-32,
-            "velocityX": 1.1273756142039836e-32,
-            "velocityY": 2.4507602066878393e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.588395006557631,
-            "y": 4.128528104169273,
-            "heading": -0.06094869084906696,
-            "angularVelocity": -0.5766457987157873,
-            "velocityX": -0.8666902254019087,
-            "velocityY": 0.01445765014696692,
-            "timestamp": 0.10569519622756653
-        },
-        {
-            "x": 7.4046836501246664,
-            "y": 4.131675342578391,
-            "heading": -0.17161815397097052,
-            "angularVelocity": -1.0470623743734504,
-            "velocityX": -1.7381239922903071,
-            "velocityY": 0.029776551077511915,
-            "timestamp": 0.21139039245513305
-        },
-        {
-            "x": 7.128148507793846,
-            "y": 4.136585421276787,
-            "heading": -0.30937735862178317,
-            "angularVelocity": -1.3033629679272352,
-            "velocityX": -2.6163454177749763,
-            "velocityY": 0.0464550790730766,
-            "timestamp": 0.3170855886826996
-        },
-        {
-            "x": 6.759745872373483,
-            "y": 4.143898882898015,
-            "heading": -0.3925495634756188,
-            "angularVelocity": -0.7869061965196801,
-            "velocityX": -3.485519196418107,
-            "velocityY": 0.06919388848555069,
-            "timestamp": 0.4227807849102661
-        },
-        {
-            "x": 6.3599483672427946,
-            "y": 4.149634642956697,
-            "heading": -0.39254959749689916,
-            "angularVelocity": -3.2188104690533836e-7,
-            "velocityX": -3.782551330619671,
-            "velocityY": 0.05426698907234567,
-            "timestamp": 0.5284759811378327
-        },
-        {
-            "x": 5.960150865134662,
-            "y": 4.155370613693523,
-            "heading": -0.39254963151811834,
-            "angularVelocity": -3.2188046698591794e-7,
-            "velocityX": -3.7825513020227493,
-            "velocityY": 0.05426898233365955,
-            "timestamp": 0.6341711773653992
-        },
-        {
-            "x": 5.56430885680334,
-            "y": 4.211762521698523,
-            "heading": -0.3925496691964927,
-            "angularVelocity": -3.5648142721372326e-7,
-            "velocityX": -3.745127711188092,
-            "velocityY": 0.5335333110464675,
-            "timestamp": 0.7398663735929657
-        },
-        {
-            "x": 5.196451370697568,
-            "y": 4.356905683947299,
-            "heading": -0.407192768864793,
-            "angularVelocity": -0.13854082485237101,
-            "velocityX": -3.4803614472105,
-            "velocityY": 1.3732238306864724,
-            "timestamp": 0.8455615698205322
-        },
-        {
-            "x": 4.853119399386178,
-            "y": 4.529141900436451,
-            "heading": -0.46036689242459117,
-            "angularVelocity": -0.5030893120753747,
-            "velocityX": -3.2483214333807724,
-            "velocityY": 1.6295557663597022,
-            "timestamp": 0.9512567660480987
-        },
-        {
-            "x": 4.514017884763102,
-            "y": 4.740993233583884,
-            "heading": -0.4603674462778229,
-            "angularVelocity": -0.000005240098429403888,
-            "velocityX": -3.208296372268186,
-            "velocityY": 2.0043610372916896,
-            "timestamp": 1.0569519622756653
-        },
-        {
-            "x": 4.21750261375463,
-            "y": 5.009227519029098,
-            "heading": -0.46036738913342323,
-            "angularVelocity": 5.406527608893175e-7,
-            "velocityX": -2.8053807702864813,
-            "velocityY": 2.537809616888297,
-            "timestamp": 1.1626471585032319
-        },
-        {
-            "x": 3.866969646487487,
-            "y": 5.201574981603133,
-            "heading": -0.46036721071098435,
-            "angularVelocity": 0.0000016880846552508654,
-            "velocityX": -3.3164512653198517,
-            "velocityY": 1.8198316426784653,
-            "timestamp": 1.2683423547307984
-        },
-        {
-            "x": 3.5045677865422378,
-            "y": 5.303672838764994,
-            "heading": -0.3887465759877037,
-            "angularVelocity": 0.6776148517580518,
-            "velocityX": -3.4287448519891264,
-            "velocityY": 0.9659649710289621,
-            "timestamp": 1.3740375509583649
-        },
-        {
-            "x": 3.16557460185915,
-            "y": 5.315860378892371,
-            "heading": -0.29320626763004903,
-            "angularVelocity": 0.9039229006392311,
-            "velocityX": -3.207271444515043,
-            "velocityY": 0.1153083636945626,
-            "timestamp": 1.4797327471859314
-        },
-        {
-            "x": 2.8681536233597122,
-            "y": 5.2454821587095966,
-            "heading": -0.23849853505250618,
-            "angularVelocity": 0.5175990445181124,
-            "velocityX": -2.8139498209462337,
-            "velocityY": -0.6658601591622668,
-            "timestamp": 1.585427943413498
-        },
-        {
-            "x": 2.618332286986499,
-            "y": 5.0970676603891505,
-            "heading": -0.24744806192055374,
-            "angularVelocity": -0.08467297651615932,
-            "velocityX": -2.3636016137889215,
-            "velocityY": -1.4041744905880433,
-            "timestamp": 1.6911231396410644
-        },
-        {
-            "x": 2.4325478975720265,
-            "y": 4.8813111498736355,
-            "heading": -0.27927376577641677,
-            "angularVelocity": -0.3011083283987745,
-            "velocityX": -1.7577373054350616,
-            "velocityY": -2.0413085761341616,
-            "timestamp": 1.796818335868631
-        },
-        {
-            "x": 2.3348070320385017,
-            "y": 4.635496280912222,
-            "heading": -0.32770640318337707,
-            "angularVelocity": -0.4582293153861289,
-            "velocityX": -0.9247427416008974,
-            "velocityY": -2.3256957528340565,
-            "timestamp": 1.9025135320961974
-        },
-        {
-            "x": 2.305548232287739,
-            "y": 4.452786448669473,
-            "heading": -0.3683698320727586,
-            "angularVelocity": -0.3847235289845278,
-            "velocityX": -0.2768224176221525,
-            "velocityY": -1.72864840374928,
-            "timestamp": 2.008208728323764
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.2046466509288718,
-            "velocityX": -0.05249275734153,
-            "velocityY": -0.8778681716972112,
-            "timestamp": 2.1139039245513307
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 5.518495272475637e-32,
-            "velocityX": 0,
-            "velocityY": 3.7393231194786592e-34,
-            "timestamp": 2.219599120778897
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.1.traj
deleted file mode 100644
index 1bf739bf..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.1.traj	
+++ /dev/null
@@ -1,175 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -2.2542235108498663e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 2.538844153951763,
-            "heading": 2.1223723168001845e-18,
-            "angularVelocity": 2.1422107346760384e-17,
-            "velocityX": -4.524824041974102e-19,
-            "velocityY": 0.8260917454276265,
-            "timestamp": 0.09907392781100399
-        },
-        {
-            "x": 7.68,
-            "y": 2.70253245874193,
-            "heading": 6.368502121517421e-18,
-            "angularVelocity": 4.285819588010552e-17,
-            "velocityX": -9.048618887508432e-19,
-            "velocityY": 1.6521834594306501,
-            "timestamp": 0.19814785562200798
-        },
-        {
-            "x": 7.68,
-            "y": 2.9480649081437846,
-            "heading": 1.2741259045974704e-17,
-            "angularVelocity": 6.432324896396774e-17,
-            "velocityX": -1.3570355356096521e-18,
-            "velocityY": 2.47827511058448,
-            "timestamp": 0.297221783433012
-        },
-        {
-            "x": 7.68,
-            "y": 3.2754414834771923,
-            "heading": 2.1247069179422305e-17,
-            "angularVelocity": 8.585316360490521e-17,
-            "velocityX": -1.8085916683463265e-18,
-            "velocityY": 3.3043665731908773,
-            "timestamp": 0.39629571124401597
-        },
-        {
-            "x": 7.68,
-            "y": 3.6502322649266867,
-            "heading": 5.059469084858907e-18,
-            "angularVelocity": -1.633891019789118e-16,
-            "velocityX": -2.0715339797290713e-18,
-            "velocityY": 3.782940575087071,
-            "timestamp": 0.49536963905501996
-        },
-        {
-            "x": 7.68,
-            "y": 4.025023046376204,
-            "heading": -4.370025378852898e-19,
-            "angularVelocity": -5.5478487063848375e-17,
-            "velocityX": -2.0691441879135133e-18,
-            "velocityY": 3.7829405750873115,
-            "timestamp": 0.594443566866024
-        },
-        {
-            "x": 7.68,
-            "y": 4.3998138278257235,
-            "heading": 3.678666355048194e-17,
-            "angularVelocity": 3.7571606285132846e-16,
-            "velocityX": -2.072328507072934e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.693517494677028
-        },
-        {
-            "x": 7.68,
-            "y": 4.774604609275241,
-            "heading": 5.708604515927294e-17,
-            "angularVelocity": 2.04891257037182e-16,
-            "velocityX": -2.07516702935077e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.7925914224880319
-        },
-        {
-            "x": 7.68,
-            "y": 5.14939539072476,
-            "heading": 3.2711500565517004e-17,
-            "angularVelocity": -2.460238039643844e-16,
-            "velocityX": -2.078706211560206e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.8916653502990359
-        },
-        {
-            "x": 7.68,
-            "y": 5.524186172174278,
-            "heading": 3.4723638610714384e-17,
-            "angularVelocity": 2.0309460720999983e-17,
-            "velocityX": -2.0667219645048642e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.9907392781100399
-        },
-        {
-            "x": 7.68,
-            "y": 5.898976953623796,
-            "heading": 1.3963468905806427e-18,
-            "angularVelocity": -3.363881139719203e-16,
-            "velocityX": -2.078319856885538e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 1.089813205921044
-        },
-        {
-            "x": 7.68,
-            "y": 6.273767735073314,
-            "heading": -1.338725801529433e-17,
-            "angularVelocity": -1.492179146704526e-16,
-            "velocityX": -2.0709974384556896e-18,
-            "velocityY": 3.7829405750873115,
-            "timestamp": 1.188887133732048
-        },
-        {
-            "x": 7.68,
-            "y": 6.648558516522808,
-            "heading": -2.1247111946894708e-17,
-            "angularVelocity": -7.933322222365232e-17,
-            "velocityX": -2.070808166139798e-18,
-            "velocityY": 3.782940575087071,
-            "timestamp": 1.287961061543052
-        },
-        {
-            "x": 7.68,
-            "y": 6.975935091856216,
-            "heading": -1.273985947141283e-17,
-            "angularVelocity": 8.586772184549593e-17,
-            "velocityX": -1.8108827209778567e-18,
-            "velocityY": 3.304366573190878,
-            "timestamp": 1.387034989354056
-        },
-        {
-            "x": 7.68,
-            "y": 7.22146754125807,
-            "heading": -6.367750753434283e-18,
-            "angularVelocity": 6.431670631033216e-17,
-            "velocityX": -1.3581621409711223e-18,
-            "velocityY": 2.47827511058448,
-            "timestamp": 1.4861089171650599
-        },
-        {
-            "x": 7.68,
-            "y": 7.385155846048237,
-            "heading": -2.122107432521882e-18,
-            "angularVelocity": 4.285328556951628e-17,
-            "velocityX": -9.05441456368543e-19,
-            "velocityY": 1.65218345943065,
-            "timestamp": 1.5851828449760639
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 2.141943374454498e-17,
-            "velocityX": -4.527207369006345e-19,
-            "velocityY": 0.8260917454276265,
-            "timestamp": 1.6842567727870679
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -3.1368402443636883e-34,
-            "velocityY": 0,
-            "timestamp": 1.7833307005980719
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.1.traj
deleted file mode 100644
index 0dd47978..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.1.traj	
+++ /dev/null
@@ -1,139 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -1.7120676318191985e-39,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 2.5334453720055388,
-            "heading": -1.6066276958150233e-18,
-            "angularVelocity": -1.677931007769515e-17,
-            "velocityX": 1.4398812722194594e-33,
-            "velocityY": 0.7983807350946674,
-            "timestamp": 0.09575052183150976
-        },
-        {
-            "x": 7.68,
-            "y": 2.686336112982689,
-            "heading": -4.816833322743681e-18,
-            "angularVelocity": -3.3526769001814837e-17,
-            "velocityX": 1.5865173753469157e-33,
-            "velocityY": 1.5967614385035827,
-            "timestamp": 0.1915010436630195
-        },
-        {
-            "x": 7.68,
-            "y": 2.915672216863599,
-            "heading": -9.624489104750263e-18,
-            "angularVelocity": -5.0210230614541765e-17,
-            "velocityX": 1.0182538660036936e-34,
-            "velocityY": 2.395142078541023,
-            "timestamp": 0.28725156549452924
-        },
-        {
-            "x": 7.68,
-            "y": 3.221453665444745,
-            "heading": -1.6009153132358185e-17,
-            "angularVelocity": -6.66802008672839e-17,
-            "velocityX": 3.094783239015371e-34,
-            "velocityY": 3.193522528464369,
-            "timestamp": 0.383002087326039
-        },
-        {
-            "x": 7.68,
-            "y": 3.5836721992668203,
-            "heading": -3.473454086768081e-17,
-            "angularVelocity": -1.9556434134398854e-16,
-            "velocityX": 3.082893791022991e-33,
-            "velocityY": 3.7829405719528495,
-            "timestamp": 0.4787526091575488
-        },
-        {
-            "x": 7.68,
-            "y": 3.9458907330889397,
-            "heading": -3.402618832360637e-17,
-            "angularVelocity": 7.397897479044197e-18,
-            "velocityX": 3.015867889332214e-33,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.5745031309890586
-        },
-        {
-            "x": 7.68,
-            "y": 4.308109266911059,
-            "heading": -1.7375698441954866e-17,
-            "angularVelocity": 1.7389450796885638e-16,
-            "velocityX": 5.330140345912211e-33,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.6702536528205684
-        },
-        {
-            "x": 7.68,
-            "y": 4.670327800733179,
-            "heading": 7.579747606314133e-18,
-            "angularVelocity": 2.6062986990486198e-16,
-            "velocityX": 7.600730979130334e-33,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.7660041746520782
-        },
-        {
-            "x": 7.68,
-            "y": 5.032546334555254,
-            "heading": 1.6243735534665814e-17,
-            "angularVelocity": 9.048502047432954e-17,
-            "velocityX": -2.799411465702586e-33,
-            "velocityY": 3.7829405719528495,
-            "timestamp": 0.8617546964835879
-        },
-        {
-            "x": 7.68,
-            "y": 5.3383277831364,
-            "heading": 9.733952786983855e-18,
-            "angularVelocity": -6.798691664538921e-17,
-            "velocityX": -8.030123288193386e-33,
-            "velocityY": 3.193522528464369,
-            "timestamp": 0.9575052183150977
-        },
-        {
-            "x": 7.68,
-            "y": 5.567663887017311,
-            "heading": 4.864296415465134e-18,
-            "angularVelocity": -5.08577528116881e-17,
-            "velocityX": -5.179461626817983e-33,
-            "velocityY": 2.395142078541023,
-            "timestamp": 1.0532557401466074
-        },
-        {
-            "x": 7.68,
-            "y": 5.720554627994461,
-            "heading": 1.6208272352196832e-18,
-            "angularVelocity": -3.3874167136237696e-17,
-            "velocityX": -4.721587370568867e-33,
-            "velocityY": 1.5967614385035827,
-            "timestamp": 1.1490062619781172
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": -1.69276073284528e-17,
-            "velocityX": -2.4599412486527172e-33,
-            "velocityY": 0.7983807350946674,
-            "timestamp": 1.244756783809627
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -3.907521125363084e-43,
-            "velocityY": 8.448865646535191e-39,
-            "timestamp": 1.3405073056411367
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.1.traj
deleted file mode 100644
index 14b92c55..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.1.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -2.24426408200091e-39,
-            "angularVelocity": 3.072610310721988e-47,
-            "velocityX": -5.239815815905201e-40,
-            "velocityY": -4.5917748078995606e-40,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 2.540500005575846,
-            "heading": -7.160737227180709e-23,
-            "angularVelocity": -7.15564807370433e-22,
-            "velocityX": 1.97902852084548e-33,
-            "velocityY": 0.8344065481228216,
-            "timestamp": 0.10007112931184187
-        },
-        {
-            "x": 7.68,
-            "y": 2.7075000136298466,
-            "heading": 1.9234971876596208e-22,
-            "angularVelocity": 2.6376946581601568e-21,
-            "velocityX": 2.759296119129438e-33,
-            "velocityY": 1.6688130652907365,
-            "timestamp": 0.20014225862368373
-        },
-        {
-            "x": 7.68,
-            "y": 2.9580000179666155,
-            "heading": 1.2715262688933367e-21,
-            "angularVelocity": 1.0784094425251139e-20,
-            "velocityX": 3.360924516770788e-33,
-            "velocityY": 2.5032195205488357,
-            "timestamp": 0.3002133879355256
-        },
-        {
-            "x": 7.68,
-            "y": 3.2919999999999994,
-            "heading": 4.5990005279924634e-21,
-            "angularVelocity": 3.325109025879841e-20,
-            "velocityX": 4.212493187097162e-33,
-            "velocityY": 3.3376257900775026,
-            "timestamp": 0.40028451724736747
-        },
-        {
-            "x": 7.68,
-            "y": 3.6259999820333837,
-            "heading": 2.8774590551268706e-21,
-            "angularVelocity": -1.7203177491039364e-20,
-            "velocityX": -1.8223734723751632e-33,
-            "velocityY": 3.3376257900775026,
-            "timestamp": 0.5003556465592094
-        },
-        {
-            "x": 7.68,
-            "y": 3.8764999863701526,
-            "heading": 1.4966782173181084e-21,
-            "angularVelocity": -1.3797993485663648e-20,
-            "velocityX": -1.777649344678044e-33,
-            "velocityY": 2.5032195205488357,
-            "timestamp": 0.6004267758710513
-        },
-        {
-            "x": 7.68,
-            "y": 4.043499994424153,
-            "heading": 5.147844205579642e-22,
-            "angularVelocity": -9.81195850164629e-21,
-            "velocityX": -2.007686184897088e-33,
-            "velocityY": 1.6688130652907365,
-            "timestamp": 0.7004979051828932
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -4.8498876719579466e-42,
-            "angularVelocity": -5.144185056490002e-21,
-            "velocityX": -2.233495211598662e-33,
-            "velocityY": 0.8344065481228216,
-            "timestamp": 0.8005690344947352
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 9.449822208481667e-41,
-            "angularVelocity": 9.927610425813228e-40,
-            "velocityX": 1.9465664275137074e-41,
-            "velocityY": 5.142787784847508e-39,
-            "timestamp": 0.9006401638065771
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C5.1.traj
deleted file mode 100644
index 27f393e3..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C5.1.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -4.712717929333765e-40,
-            "angularVelocity": 9.927649781151023e-40,
-            "velocityX": 1.7772849076267516e-40,
-            "velocityY": -4.543888050052464e-40,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 2.3734999944241535,
-            "heading": 2.8227518572323466e-21,
-            "angularVelocity": 2.820745460648545e-20,
-            "velocityX": 2.2465911667965736e-32,
-            "velocityY": -0.8344065481228212,
-            "timestamp": 0.10007112931184181
-        },
-        {
-            "x": 7.68,
-            "y": 2.206499986370153,
-            "heading": 8.436925006375396e-21,
-            "angularVelocity": 5.6101826293588e-20,
-            "velocityX": 4.4654374072917263e-32,
-            "velocityY": -1.6688130652907358,
-            "timestamp": 0.20014225862368362
-        },
-        {
-            "x": 7.68,
-            "y": 1.955999982033384,
-            "heading": 1.682221146414935e-20,
-            "angularVelocity": 8.379326253074398e-20,
-            "velocityX": 6.797738711780288e-32,
-            "velocityY": -2.503219520548835,
-            "timestamp": 0.30021338793552543
-        },
-        {
-            "x": 7.68,
-            "y": 1.6219999999999999,
-            "heading": 2.78675063552584e-20,
-            "angularVelocity": 1.1037443968745167e-19,
-            "velocityX": 8.936946075377416e-32,
-            "velocityY": -3.3376257900775013,
-            "timestamp": 0.40028451724736724
-        },
-        {
-            "x": 7.68,
-            "y": 1.288000017966616,
-            "heading": 1.0287640204896551e-20,
-            "angularVelocity": -1.75673704816788e-19,
-            "velocityX": 9.060995032099377e-32,
-            "velocityY": -3.3376257900775013,
-            "timestamp": 0.500355646559209
-        },
-        {
-            "x": 7.68,
-            "y": 1.037500013629847,
-            "heading": 3.2969803204781124e-21,
-            "angularVelocity": -6.985690973056607e-20,
-            "velocityX": 6.916062716279199e-32,
-            "velocityY": -2.503219520548835,
-            "timestamp": 0.6004267758710509
-        },
-        {
-            "x": 7.68,
-            "y": 0.8705000055758465,
-            "heading": 6.18120991020044e-22,
-            "angularVelocity": -2.676955216055553e-20,
-            "velocityX": 4.34564572466329e-32,
-            "velocityY": -1.6688130652907358,
-            "timestamp": 0.7004979051828927
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -2.2737142324276756e-40,
-            "angularVelocity": -6.1768164103594875e-21,
-            "velocityX": 2.131975080632403e-32,
-            "velocityY": -0.8344065481228212,
-            "timestamp": 0.8005690344947345
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 2.0993519451368808e-41,
-            "angularVelocity": 2.4819025971975228e-39,
-            "velocityX": -8.732390511178075e-43,
-            "velocityY": -2.571393892423754e-39,
-            "timestamp": 0.9006401638065763
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S3.1.traj
deleted file mode 100644
index 80fc230a..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S3.1.traj	
+++ /dev/null
@@ -1,229 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 7.203126847267154e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.6459509070752,
-            "y": 2.420050479995149,
-            "heading": -0.026297254997510774,
-            "angularVelocity": -0.33695614983619343,
-            "velocityX": -0.4362832264603014,
-            "velocityY": -0.47344743777665743,
-            "timestamp": 0.078043552581802
-        },
-        {
-            "x": 7.57752123433784,
-            "y": 2.3463325784745406,
-            "heading": -0.07655763531528098,
-            "angularVelocity": -0.6440042598660941,
-            "velocityX": -0.8768139131756897,
-            "velocityY": -0.9445738831958564,
-            "timestamp": 0.156087105163604
-        },
-        {
-            "x": 7.474195561438856,
-            "y": 2.236153787211583,
-            "heading": -0.14701004173730176,
-            "angularVelocity": -0.9027319245646475,
-            "velocityX": -1.3239488654839289,
-            "velocityY": -1.4117603263569152,
-            "timestamp": 0.23413065774540598
-        },
-        {
-            "x": 7.33507656702923,
-            "y": 2.0901340736402396,
-            "heading": -0.23055657416165956,
-            "angularVelocity": -1.0705116522834195,
-            "velocityX": -1.7825815177216175,
-            "velocityY": -1.8710029046700958,
-            "timestamp": 0.312174210327208
-        },
-        {
-            "x": 7.158294386463989,
-            "y": 1.910067298253472,
-            "heading": -0.30975533817585227,
-            "angularVelocity": -1.014802137964151,
-            "velocityX": -2.2651734155741057,
-            "velocityY": -2.3072601057983104,
-            "timestamp": 0.39021776290901
-        },
-        {
-            "x": 6.940360292129115,
-            "y": 1.7108993896694566,
-            "heading": -0.30975560484167247,
-            "angularVelocity": -0.00000341688469228089,
-            "velocityX": -2.7924676302559823,
-            "velocityY": -2.552009768843728,
-            "timestamp": 0.468261315490812
-        },
-        {
-            "x": 6.6915168927330235,
-            "y": 1.5520279357314815,
-            "heading": -0.30975564017638474,
-            "angularVelocity": -4.5275632793576304e-7,
-            "velocityX": -3.1885196299240257,
-            "velocityY": -2.035676858398594,
-            "timestamp": 0.546304868072614
-        },
-        {
-            "x": 6.419127595403782,
-            "y": 1.43815413781978,
-            "heading": -0.30975568823518523,
-            "angularVelocity": -6.157946287917162e-7,
-            "velocityX": -3.490221655962278,
-            "velocityY": -1.4591057703627237,
-            "timestamp": 0.624348420654416
-        },
-        {
-            "x": 6.131252530334066,
-            "y": 1.37264761019134,
-            "heading": -0.30975577587401576,
-            "angularVelocity": -0.0000011229477336220622,
-            "velocityX": -3.6886463461279972,
-            "velocityY": -0.8393586076157934,
-            "timestamp": 0.7023919732362179
-        },
-        {
-            "x": 5.83657654735769,
-            "y": 1.3574693553002635,
-            "heading": -0.31020540722452916,
-            "angularVelocity": -0.005761287587978277,
-            "velocityX": -3.7757889438387497,
-            "velocityY": -0.1944844178532558,
-            "timestamp": 0.7804355258180199
-        },
-        {
-            "x": 5.546474933624268,
-            "y": 1.3928169012069702,
-            "heading": -0.3189695864044355,
-            "angularVelocity": -0.11229856778804584,
-            "velocityX": -3.717175911864151,
-            "velocityY": 0.4529207697160109,
-            "timestamp": 0.8584790783998218
-        },
-        {
-            "x": 5.303276771996363,
-            "y": 1.4607318697473424,
-            "heading": -0.3307229091538368,
-            "angularVelocity": -0.17360472172913416,
-            "velocityX": -3.5922053767043294,
-            "velocityY": 1.0031511484968414,
-            "timestamp": 0.9261807090485242
-        },
-        {
-            "x": 5.073563214559545,
-            "y": 1.564404927734292,
-            "heading": -0.34378229938421223,
-            "angularVelocity": -0.1928962434919207,
-            "velocityX": -3.3930284283517023,
-            "velocityY": 1.5313229089691542,
-            "timestamp": 0.9938823396972266
-        },
-        {
-            "x": 4.862974125111252,
-            "y": 1.7011642845126724,
-            "heading": -0.35827284529055603,
-            "angularVelocity": -0.2140354045770704,
-            "velocityX": -3.1105467834446325,
-            "velocityY": 2.020030469989471,
-            "timestamp": 1.0615839703459289
-        },
-        {
-            "x": 4.677671719496688,
-            "y": 1.8665243414779629,
-            "heading": -0.3791415933598452,
-            "angularVelocity": -0.30824587043792007,
-            "velocityX": -2.737044940262413,
-            "velocityY": 2.442482631227878,
-            "timestamp": 1.1292856009946313
-        },
-        {
-            "x": 4.523629451068858,
-            "y": 2.053477766497112,
-            "heading": -0.41478083919993697,
-            "angularVelocity": -0.5264163580502632,
-            "velocityX": -2.27531105162187,
-            "velocityY": 2.761431640977936,
-            "timestamp": 1.1969872316433336
-        },
-        {
-            "x": 4.406611773563404,
-            "y": 2.2498215105595722,
-            "heading": -0.45494733767836987,
-            "angularVelocity": -0.5932870167770028,
-            "velocityX": -1.7284321867022447,
-            "velocityY": 2.900133160474012,
-            "timestamp": 1.264688862292036
-        },
-        {
-            "x": 4.327688857963163,
-            "y": 2.4438365235550945,
-            "heading": -0.48799769437613305,
-            "angularVelocity": -0.4881766713911416,
-            "velocityX": -1.1657461547677304,
-            "velocityY": 2.865736188870776,
-            "timestamp": 1.3323904929407384
-        },
-        {
-            "x": 4.28360260298108,
-            "y": 2.622213717107768,
-            "heading": -0.5154307830643909,
-            "angularVelocity": -0.40520573028252516,
-            "velocityX": -0.6511845366152605,
-            "velocityY": 2.634754759132393,
-            "timestamp": 1.4000921235894408
-        },
-        {
-            "x": 4.264391797811532,
-            "y": 2.7716303897908317,
-            "heading": -0.5365689504315967,
-            "angularVelocity": -0.3122253801676767,
-            "velocityX": -0.283756904899847,
-            "velocityY": 2.2069877970647664,
-            "timestamp": 1.4677937542381432
-        },
-        {
-            "x": 4.259328393836262,
-            "y": 2.8855840206662466,
-            "heading": -0.5517027167443236,
-            "angularVelocity": -0.22353621571242488,
-            "velocityX": -0.07478998551086755,
-            "velocityY": 1.6831740946789628,
-            "timestamp": 1.5354953848868456
-        },
-        {
-            "x": 4.260599830778067,
-            "y": 2.9618797181043535,
-            "heading": -0.5613571988137664,
-            "angularVelocity": -0.1426033904496253,
-            "velocityX": 0.018780004700190214,
-            "velocityY": 1.1269403219251917,
-            "timestamp": 1.603197015535548
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -0.0685773908510417,
-            "velocityX": 0.035452162657366115,
-            "velocityY": 0.5630629798778553,
-            "timestamp": 1.6708986461842503
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 0,
-            "velocityX": 1.1764020723168042e-36,
-            "velocityY": 0,
-            "timestamp": 1.7386002768329527
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.1.traj
deleted file mode 100644
index e5689e4c..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.1.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 1.0550386775092505e-35,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.6504501923310215,
-            "y": 2.437078665707293,
-            "heading": -0.0029944704440250615,
-            "angularVelocity": -0.0457972069493345,
-            "velocityX": -0.45193254781641656,
-            "velocityY": -0.30467539632270557,
-            "timestamp": 0.0653854381848624
-        },
-        {
-            "x": 7.5913505770823395,
-            "y": 2.3972359988448533,
-            "heading": -0.008983367804687599,
-            "angularVelocity": -0.09159374819405963,
-            "velocityX": -0.9038650942674934,
-            "velocityY": -0.609350766294381,
-            "timestamp": 0.1307708763697248
-        },
-        {
-            "x": 7.50270115478166,
-            "y": 2.337472001372052,
-            "heading": -0.017966623739921285,
-            "angularVelocity": -0.13738924422033513,
-            "velocityX": -1.3557976326478658,
-            "velocityY": -0.91402610629957,
-            "timestamp": 0.19615631455458718
-        },
-        {
-            "x": 7.384501926877228,
-            "y": 2.2577866758321745,
-            "heading": -0.02994413949472175,
-            "angularVelocity": -0.18318322989496186,
-            "velocityX": -1.8077301488788986,
-            "velocityY": -1.2187014074079496,
-            "timestamp": 0.2615417527394496
-        },
-        {
-            "x": 7.2367528971895485,
-            "y": 2.1581800263627264,
-            "heading": -0.044915766222073404,
-            "angularVelocity": -0.2289749391144676,
-            "velocityX": -2.25966260667937,
-            "velocityY": -1.5233766452376303,
-            "timestamp": 0.326927190924312
-        },
-        {
-            "x": 7.059454079201559,
-            "y": 2.0386520636124454,
-            "heading": -0.06288119646669635,
-            "angularVelocity": -0.2747619461359239,
-            "velocityX": -2.711594858272837,
-            "velocityY": -1.82805172020631,
-            "timestamp": 0.3923126291091744
-        },
-        {
-            "x": 6.854358935700705,
-            "y": 1.900385597577332,
-            "heading": -0.06288119947800026,
-            "angularVelocity": -4.6054656889756363e-8,
-            "velocityX": -3.136709781174733,
-            "velocityY": -2.114636987584247,
-            "timestamp": 0.45769806729403684
-        },
-        {
-            "x": 6.636845838072133,
-            "y": 1.782615967982618,
-            "heading": -0.06288120256062167,
-            "angularVelocity": -4.714538109405926e-8,
-            "velocityX": -3.3266290425951324,
-            "velocityY": -1.801159904469057,
-            "timestamp": 0.5230835054788993
-        },
-        {
-            "x": 6.4046630859375,
-            "y": 1.6973352432250977,
-            "heading": -0.06288120593828476,
-            "angularVelocity": -5.1657726572043936e-8,
-            "velocityX": -3.5509856411482486,
-            "velocityY": -1.3042770244409516,
-            "timestamp": 0.5884689436637617
-        },
-        {
-            "x": 6.207746736822867,
-            "y": 1.6503996785686041,
-            "heading": -0.06288121580879107,
-            "angularVelocity": -1.8445407171610623e-7,
-            "velocityX": -3.679854027466254,
-            "velocityY": -0.8771035386810692,
-            "timestamp": 0.6419809437716079
-        },
-        {
-            "x": 6.009908768236521,
-            "y": 1.6271742750630454,
-            "heading": -0.0724170210773865,
-            "angularVelocity": -0.17819938050114592,
-            "velocityX": -3.697076696584537,
-            "velocityY": -0.43402234001253925,
-            "timestamp": 0.695492943879454
-        },
-        {
-            "x": 5.8260492909953046,
-            "y": 1.6186155743000838,
-            "heading": -0.12620190803666176,
-            "angularVelocity": -1.0050995449783064,
-            "velocityX": -3.43585507681777,
-            "velocityY": -0.1599398405163814,
-            "timestamp": 0.7490049439873002
-        },
-        {
-            "x": 5.6625912601386625,
-            "y": 1.614879890442449,
-            "heading": -0.22199295256207294,
-            "angularVelocity": -1.7900852954918034,
-            "velocityX": -3.0546051451490293,
-            "velocityY": -0.06981020799271546,
-            "timestamp": 0.8025169440951464
-        },
-        {
-            "x": 5.5227239807129855,
-            "y": 1.6141995251565013,
-            "heading": -0.32512734843535046,
-            "angularVelocity": -1.9273134187738115,
-            "velocityX": -2.61375540334492,
-            "velocityY": -0.012714256327113658,
-            "timestamp": 0.8560289442029926
-        },
-        {
-            "x": 5.406619958232834,
-            "y": 1.6150039384726416,
-            "heading": -0.42188556792063076,
-            "angularVelocity": -1.8081592781110263,
-            "velocityX": -2.169681982474204,
-            "velocityY": 0.015032391136922594,
-            "timestamp": 0.9095409443108388
-        },
-        {
-            "x": 5.314084878105213,
-            "y": 1.6164421457440925,
-            "heading": -0.5051954003859285,
-            "angularVelocity": -1.5568439284160218,
-            "velocityX": -1.7292397955809797,
-            "velocityY": 0.026876350511144828,
-            "timestamp": 0.9630529444186849
-        },
-        {
-            "x": 5.244912091070787,
-            "y": 1.617980616061371,
-            "heading": -0.5709381839957368,
-            "angularVelocity": -1.2285615091439794,
-            "velocityX": -1.2926593454742539,
-            "velocityY": 0.028750005871169333,
-            "timestamp": 1.016564944526531
-        },
-        {
-            "x": 5.198930392622704,
-            "y": 1.6192554664106382,
-            "heading": -0.6164892293058035,
-            "angularVelocity": -0.851230475748707,
-            "velocityX": -0.8592782619863396,
-            "velocityY": 0.023823634823929046,
-            "timestamp": 1.0700769446343772
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -0.4393551100092281,
-            "velocityX": -0.42850935447171357,
-            "velocityY": 0.013913394899485362,
-            "timestamp": 1.1235889447422234
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 6.2403191565519e-36,
-            "timestamp": 1.1771009448500696
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.1.traj
deleted file mode 100644
index f850aede..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.1.traj	
+++ /dev/null
@@ -1,238 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 2.322866633708497e-28,
-            "velocityX": -2.1618763976557105e-31,
-            "velocityY": -1.247476705840714e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 7.635666877682845,
-            "y": 2.515362381859407,
-            "heading": -0.0008300252680408195,
-            "angularVelocity": -0.00885317162168204,
-            "velocityX": -0.4728636048938783,
-            "velocityY": 0.6225017511467659,
-            "timestamp": 0.09375456655646779
-        },
-        {
-            "x": 7.547000631163484,
-            "y": 2.632087139923421,
-            "heading": -0.0024900693866490363,
-            "angularVelocity": -0.01770627479364842,
-            "velocityX": -0.9457272298940119,
-            "velocityY": 1.2450034419786018,
-            "timestamp": 0.18750913311293557
-        },
-        {
-            "x": 7.4140012603329,
-            "y": 2.8071742656560237,
-            "heading": -0.004980126198969091,
-            "angularVelocity": -0.026559312295687615,
-            "velocityX": -1.418590856056911,
-            "velocityY": 1.867505041764004,
-            "timestamp": 0.28126369966940334
-        },
-        {
-            "x": 7.236668772838196,
-            "y": 3.0406237394955027,
-            "heading": -0.008300187115435396,
-            "angularVelocity": -0.035412258180156565,
-            "velocityX": -1.891454400654691,
-            "velocityY": 2.4900064329013105,
-            "timestamp": 0.37501826622587114
-        },
-        {
-            "x": 7.022136423676778,
-            "y": 3.3230511197393278,
-            "heading": -0.008300187546286333,
-            "angularVelocity": -4.59551947810532e-9,
-            "velocityX": -2.2882335980104607,
-            "velocityY": 3.012411988206685,
-            "timestamp": 0.46877283278233894
-        },
-        {
-            "x": 6.786560921928144,
-            "y": 3.5881804025568163,
-            "heading": -0.008300187986713265,
-            "angularVelocity": -4.6976584720676315e-9,
-            "velocityX": -2.51268295936015,
-            "velocityY": 2.827907936172931,
-            "timestamp": 0.5625273993388067
-        },
-        {
-            "x": 6.5015204114211205,
-            "y": 3.799228403383322,
-            "heading": -0.008300188501414158,
-            "angularVelocity": -5.489875460815635e-9,
-            "velocityX": -3.0402840200359327,
-            "velocityY": 2.251069026055312,
-            "timestamp": 0.6562819658952744
-        },
-        {
-            "x": 6.179186778264032,
-            "y": 3.9471823835437703,
-            "heading": -0.008300169798343478,
-            "angularVelocity": 1.9948970346527163e-7,
-            "velocityX": -3.4380579527606097,
-            "velocityY": 1.5780989192812942,
-            "timestamp": 0.7500365324517422
-        },
-        {
-            "x": 5.833324835964247,
-            "y": 4.025724182042927,
-            "heading": -0.008299866211032016,
-            "angularVelocity": 0.0000032381069276099122,
-            "velocityX": -3.689014359545628,
-            "velocityY": 0.8377383778085268,
-            "timestamp": 0.8437910990082099
-        },
-        {
-            "x": 5.481092764022396,
-            "y": 4.0672194252777,
-            "heading": -0.008299498284594501,
-            "angularVelocity": 0.000003924357511610969,
-            "velocityX": -3.7569590994770814,
-            "velocityY": 0.44259436909433375,
-            "timestamp": 0.9375456655646777
-        },
-        {
-            "x": 5.14904862351455,
-            "y": 4.179095043376677,
-            "heading": 0.0033651913425959347,
-            "angularVelocity": 0.12441729566489874,
-            "velocityX": -3.5416316527670983,
-            "velocityY": 1.1932818017093059,
-            "timestamp": 1.0313002321211455
-        },
-        {
-            "x": 4.858987953002521,
-            "y": 4.350333701259038,
-            "heading": 0.047887100206889445,
-            "angularVelocity": 0.474877229979814,
-            "velocityX": -3.093829785211866,
-            "velocityY": 1.8264567174893411,
-            "timestamp": 1.1250547986776134
-        },
-        {
-            "x": 4.6181973815778665,
-            "y": 4.575631660267378,
-            "heading": 0.10901822024185409,
-            "angularVelocity": 0.652033519862158,
-            "velocityX": -2.5683076597621346,
-            "velocityY": 2.403061176467013,
-            "timestamp": 1.2188093652340812
-        },
-        {
-            "x": 4.427445933507109,
-            "y": 4.854263962903063,
-            "heading": 0.15298068479753676,
-            "angularVelocity": 0.46891011467909954,
-            "velocityX": -2.0345830083473238,
-            "velocityY": 2.971933132108998,
-            "timestamp": 1.312563931790549
-        },
-        {
-            "x": 4.19709169118396,
-            "y": 5.123942070734021,
-            "heading": 0.15298069409509907,
-            "angularVelocity": 9.916916750047786e-8,
-            "velocityX": -2.4569922381797515,
-            "velocityY": 2.8764263729866686,
-            "timestamp": 1.406318498347017
-        },
-        {
-            "x": 3.9282946631899107,
-            "y": 5.355323824233057,
-            "heading": 0.15298070467968702,
-            "angularVelocity": 1.1289677246612154e-7,
-            "velocityX": -2.8670286458223346,
-            "velocityY": 2.4679518235485323,
-            "timestamp": 1.5000730649034848
-        },
-        {
-            "x": 3.6594937496643456,
-            "y": 5.586701063819283,
-            "heading": 0.1529807152642756,
-            "angularVelocity": 1.1289677885329184e-7,
-            "velocityX": -2.867070089473112,
-            "velocityY": 2.4679036774904017,
-            "timestamp": 1.5938276314599527
-        },
-        {
-            "x": 3.3906928188108107,
-            "y": 5.818078283274842,
-            "heading": 0.1529807258488866,
-            "angularVelocity": 1.1289701863908675e-7,
-            "velocityX": -2.8670702742957928,
-            "velocityY": 2.467903462773746,
-            "timestamp": 1.6875821980164205
-        },
-        {
-            "x": 3.1218918754104785,
-            "y": 6.049455488154229,
-            "heading": 0.15298073643351034,
-            "angularVelocity": 1.1289715379702627e-7,
-            "velocityX": -2.867070408121775,
-            "velocityY": 2.467903307302158,
-            "timestamp": 1.7813367645728884
-        },
-        {
-            "x": 2.853090944613356,
-            "y": 6.280832707674931,
-            "heading": 0.15298074701856604,
-            "angularVelocity": 1.1290176153813175e-7,
-            "velocityX": -2.8670702736940825,
-            "velocityY": 2.4679034634685753,
-            "timestamp": 1.8750913311293562
-        },
-        {
-            "x": 2.631853660621228,
-            "y": 6.471299719512446,
-            "heading": 0.24894777935018825,
-            "angularVelocity": 1.0235984854542721,
-            "velocityX": -2.3597494193403232,
-            "velocityY": 2.0315491696374846,
-            "timestamp": 1.968845897685824
-        },
-        {
-            "x": 2.465926649664366,
-            "y": 6.614149879341798,
-            "heading": 0.32096506390227886,
-            "angularVelocity": 0.7681469521670183,
-            "velocityX": -1.7698019099360354,
-            "velocityY": 1.52366082075921,
-            "timestamp": 2.062600464242292
-        },
-        {
-            "x": 2.355308907973467,
-            "y": 6.709383273555383,
-            "heading": 0.3689895410149908,
-            "angularVelocity": 0.5122361382129277,
-            "velocityX": -1.1798651068828128,
-            "velocityY": 1.0157733933549364,
-            "timestamp": 2.1563550307987596
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.25609908793666974,
-            "velocityX": -0.5899329494543092,
-            "velocityY": 0.5078870095990298,
-            "timestamp": 2.250109597355227
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0,
-            "velocityX": -3.330818616870831e-32,
-            "velocityY": 5.736512740310497e-33,
-            "timestamp": 2.343864163911695
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.1.traj
deleted file mode 100644
index f49877f8..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.1.traj	
+++ /dev/null
@@ -1,256 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 6.426641305668581e-31,
-            "velocityX": -5.011351780601486e-32,
-            "velocityY": -1.0551805148640748e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.6491080594765135,
-            "y": 2.4958059548034544,
-            "heading": -0.02134777834454554,
-            "angularVelocity": -0.2757848804434067,
-            "velocityX": -0.3990827516771383,
-            "velocityY": 0.5013212819261742,
-            "timestamp": 0.07740735572675958
-        },
-        {
-            "x": 7.587308326375472,
-            "y": 2.5734484495799954,
-            "heading": -0.06302080337998518,
-            "angularVelocity": -0.538360013001088,
-            "velocityX": -0.7983702907923742,
-            "velocityY": 1.0030376835324535,
-            "timestamp": 0.15481471145351916
-        },
-        {
-            "x": 7.4945787588029695,
-            "y": 2.6899753845455243,
-            "heading": -0.12336744193035674,
-            "angularVelocity": -0.7795982433941759,
-            "velocityX": -1.1979425818371583,
-            "velocityY": 1.5053728921687748,
-            "timestamp": 0.23222206718027874
-        },
-        {
-            "x": 7.370887761034633,
-            "y": 2.845471450080925,
-            "heading": -0.19922081386712712,
-            "angularVelocity": -0.9799245979222482,
-            "velocityX": -1.5979230475841766,
-            "velocityY": 2.0088021877957765,
-            "timestamp": 0.3096294229070383
-        },
-        {
-            "x": 7.216202117777622,
-            "y": 3.0401051125017453,
-            "heading": -0.28194072984640106,
-            "angularVelocity": -1.0686312069781692,
-            "velocityX": -1.9983326107022177,
-            "velocityY": 2.5144078439762083,
-            "timestamp": 0.38703677863379793
-        },
-        {
-            "x": 7.032473858656331,
-            "y": 3.268122248690632,
-            "heading": -0.2819407809538647,
-            "angularVelocity": -6.602404015783628e-7,
-            "velocityX": -2.3735245493959742,
-            "velocityY": 2.9456778887237176,
-            "timestamp": 0.46444413436055754
-        },
-        {
-            "x": 6.834683469529352,
-            "y": 3.4840547774927524,
-            "heading": -0.28194080060331267,
-            "angularVelocity": -2.538447128540298e-7,
-            "velocityX": -2.5551885511392456,
-            "velocityY": 2.7895608469606037,
-            "timestamp": 0.5418514900873171
-        },
-        {
-            "x": 6.603064880376339,
-            "y": 3.663221523268011,
-            "heading": -0.2819408226641663,
-            "angularVelocity": -2.8499686418440244e-7,
-            "velocityX": -2.992203867170001,
-            "velocityY": 2.314595868740169,
-            "timestamp": 0.6192588458140768
-        },
-        {
-            "x": 6.344360158554223,
-            "y": 3.8004066501958644,
-            "heading": -0.2819407635550332,
-            "angularVelocity": 7.636113202840995e-7,
-            "velocityX": -3.342120647233029,
-            "velocityY": 1.7722492344539278,
-            "timestamp": 0.6966662015408364
-        },
-        {
-            "x": 6.0661003337524155,
-            "y": 3.891617196013079,
-            "heading": -0.28194070942615473,
-            "angularVelocity": 6.992730598438293e-7,
-            "velocityX": -3.59474654817093,
-            "velocityY": 1.1783188427102327,
-            "timestamp": 0.774073557267596
-        },
-        {
-            "x": 5.7830200242104235,
-            "y": 3.966540038544743,
-            "heading": -0.2819406560965727,
-            "angularVelocity": 6.889472132026456e-7,
-            "velocityX": -3.6570207945254936,
-            "velocityY": 0.967903396624646,
-            "timestamp": 0.8514809129943556
-        },
-        {
-            "x": 5.499939604590639,
-            "y": 4.041462465169378,
-            "heading": -0.2819406027672389,
-            "angularVelocity": 6.889440059098256e-7,
-            "velocityX": -3.657022216584049,
-            "velocityY": 0.9678980236594484,
-            "timestamp": 0.9288882687211152
-        },
-        {
-            "x": 5.234359644348809,
-            "y": 4.1279159927443425,
-            "heading": -0.3272205989423401,
-            "angularVelocity": -0.5849572789301486,
-            "velocityX": -3.430939576069496,
-            "velocityY": 1.1168644990295,
-            "timestamp": 1.0062956244478747
-        },
-        {
-            "x": 4.992805004119873,
-            "y": 4.258058071136475,
-            "heading": -0.38392632852173914,
-            "angularVelocity": -0.7325625458589549,
-            "velocityX": -3.120564421314147,
-            "velocityY": 1.6812624222886623,
-            "timestamp": 1.0837029801746343
-        },
-        {
-            "x": 4.760679474369801,
-            "y": 4.451318301231298,
-            "heading": -0.43925940371465827,
-            "angularVelocity": -0.650567727765224,
-            "velocityX": -2.7291701738847216,
-            "velocityY": 2.272219071899485,
-            "timestamp": 1.1687565000948184
-        },
-        {
-            "x": 4.558064597648056,
-            "y": 4.695759350557987,
-            "heading": -0.45036550730749886,
-            "angularVelocity": -0.13057782444839394,
-            "velocityX": -2.3822044862091225,
-            "velocityY": 2.8739674684372165,
-            "timestamp": 1.2538100200150024
-        },
-        {
-            "x": 4.30999794561686,
-            "y": 4.900667947770545,
-            "heading": -0.4503653988140509,
-            "angularVelocity": 0.0000012755903345298267,
-            "velocityX": -2.9165947777821004,
-            "velocityY": 2.4091724528843765,
-            "timestamp": 1.3388635399351865
-        },
-        {
-            "x": 4.028045524182051,
-            "y": 5.05567579900462,
-            "heading": -0.4503653036134663,
-            "angularVelocity": 0.0000011193021141522574,
-            "velocityX": -3.3150000340890857,
-            "velocityY": 1.8224742653747859,
-            "timestamp": 1.4239170598553705
-        },
-        {
-            "x": 3.731575050469511,
-            "y": 5.180695241574213,
-            "heading": -0.45036521232637566,
-            "angularVelocity": 0.000001073289978617534,
-            "velocityX": -3.485693172848094,
-            "velocityY": 1.4698914599542185,
-            "timestamp": 1.5089705797755546
-        },
-        {
-            "x": 3.4348291016010033,
-            "y": 5.305059402921897,
-            "heading": -0.4503651210307298,
-            "angularVelocity": 0.000001073390565931622,
-            "velocityX": -3.488932017711167,
-            "velocityY": 1.4621871201127652,
-            "timestamp": 1.5940240996957387
-        },
-        {
-            "x": 3.120089238118937,
-            "y": 5.371868296261872,
-            "heading": -0.45036508370615375,
-            "angularVelocity": 4.3883634791364236e-7,
-            "velocityX": -3.7004919229372337,
-            "velocityY": 0.785492398232055,
-            "timestamp": 1.6790776196159227
-        },
-        {
-            "x": 2.8304866761911076,
-            "y": 5.435580552385651,
-            "heading": -0.37567464629819297,
-            "angularVelocity": 0.8781580994889028,
-            "velocityX": -3.404945053415936,
-            "velocityY": 0.7490842963759552,
-            "timestamp": 1.7641311395361068
-        },
-        {
-            "x": 2.5976382602327472,
-            "y": 5.485810295855226,
-            "heading": -0.2445738789443125,
-            "angularVelocity": 1.5413914377313083,
-            "velocityX": -2.7376693660283826,
-            "velocityY": 0.5905663106789283,
-            "timestamp": 1.8491846594562908
-        },
-        {
-            "x": 2.4235493783626687,
-            "y": 5.523078200691987,
-            "heading": -0.12850773623159037,
-            "angularVelocity": 1.3646248012033382,
-            "velocityX": -2.0468157230112984,
-            "velocityY": 0.43817004718549557,
-            "timestamp": 1.934238179376475
-        },
-        {
-            "x": 2.3077761259926057,
-            "y": 5.547737838884683,
-            "heading": -0.04437897835044122,
-            "angularVelocity": 0.9891272925576834,
-            "velocityX": -1.3611812007158846,
-            "velocityY": 0.2899308366756611,
-            "timestamp": 2.0192916992966587
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 1.7803711242706045e-30,
-            "angularVelocity": 0.521777092730431,
-            "velocityX": -0.6792914161204248,
-            "velocityY": 0.14416994295838573,
-            "timestamp": 2.104345219216843
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 1.3562125763475036e-31,
-            "velocityY": 2.290518308039125e-31,
-            "timestamp": 2.189398739137027
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W3.1.traj
deleted file mode 100644
index 15a1cd3f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W3.1.traj	
+++ /dev/null
@@ -1,274 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -4.62162066936404e-33,
-            "velocityX": 3.0688151507515204e-31,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.623584376302727,
-            "y": 2.4297857949616017,
-            "heading": 0.033539307380936634,
-            "angularVelocity": 0.38483343876921716,
-            "velocityX": -0.6473186288895079,
-            "velocityY": -0.3122585684118786,
-            "timestamp": 0.08715278871867994
-        },
-        {
-            "x": 7.510615150776605,
-            "y": 2.375312721693156,
-            "heading": 0.0969954791736376,
-            "angularVelocity": 0.7281025968948721,
-            "velocityX": -1.2962204329545195,
-            "velocityY": -0.6250296068468768,
-            "timestamp": 0.1743055774373599
-        },
-        {
-            "x": 7.340880293995689,
-            "y": 2.2935273668964844,
-            "heading": 0.1838752961952568,
-            "angularVelocity": 0.9968678948651678,
-            "velocityX": -1.9475550843106406,
-            "velocityY": -0.9384135149210906,
-            "timestamp": 0.26145836615603985
-        },
-        {
-            "x": 7.114069036249808,
-            "y": 2.1844157133503646,
-            "heading": 0.27996244701307155,
-            "angularVelocity": 1.1025137833279668,
-            "velocityX": -2.6024555390648842,
-            "velocityY": -1.251958258023393,
-            "timestamp": 0.3486111548747198
-        },
-        {
-            "x": 6.831104073589339,
-            "y": 2.0490142604857513,
-            "heading": 0.32912237638568254,
-            "angularVelocity": 0.5640660510737534,
-            "velocityX": -3.246768885088134,
-            "velocityY": -1.5536101007814562,
-            "timestamp": 0.4357639435933997
-        },
-        {
-            "x": 6.534528280813305,
-            "y": 1.9049975621731787,
-            "heading": 0.3291224030694369,
-            "angularVelocity": 3.0617212321522066e-7,
-            "velocityX": -3.4029409401155197,
-            "velocityY": -1.6524623070575855,
-            "timestamp": 0.5229167323120797
-        },
-        {
-            "x": 6.237952262222522,
-            "y": 1.7609813288852085,
-            "heading": 0.3291224297529421,
-            "angularVelocity": 3.0616926449614015e-7,
-            "velocityX": -3.402943531137019,
-            "velocityY": -1.6524569713178037,
-            "timestamp": 0.6100695210307596
-        },
-        {
-            "x": 5.936787262454918,
-            "y": 1.6268259970502223,
-            "heading": 0.32912245667190637,
-            "angularVelocity": 3.0887094574020004e-7,
-            "velocityX": -3.4555979699024117,
-            "velocityY": -1.5393119808021931,
-            "timestamp": 0.6972223097494396
-        },
-        {
-            "x": 5.615527521313305,
-            "y": 1.5527301986214108,
-            "heading": 0.32912248658360005,
-            "angularVelocity": 3.4320982850713803e-7,
-            "velocityX": -3.6861670850098154,
-            "velocityY": -0.850182759704744,
-            "timestamp": 0.7843750984681195
-        },
-        {
-            "x": 5.286027461953728,
-            "y": 1.5414290964422628,
-            "heading": 0.32912252585289026,
-            "angularVelocity": 4.50579847209181e-7,
-            "velocityX": -3.780717337951944,
-            "velocityY": -0.12967000075725457,
-            "timestamp": 0.8715278871867994
-        },
-        {
-            "x": 4.960446284223111,
-            "y": 1.593339788205863,
-            "heading": 0.3291233220408656,
-            "angularVelocity": 0.000009135542155699871,
-            "velocityX": -3.7357516898462033,
-            "velocityY": 0.5956285797252272,
-            "timestamp": 0.9586806759054793
-        },
-        {
-            "x": 4.650797848549272,
-            "y": 1.7065466836899894,
-            "heading": 0.3291235959637348,
-            "angularVelocity": 0.0000031430189815062194,
-            "velocityX": -3.5529377800330906,
-            "velocityY": 1.2989474823295228,
-            "timestamp": 1.0458334646241594
-        },
-        {
-            "x": 4.368508202692925,
-            "y": 1.8768714467040708,
-            "heading": 0.32912381701495697,
-            "angularVelocity": 0.000002536364302217629,
-            "velocityX": -3.2390202310972316,
-            "velocityY": 1.9543237286860882,
-            "timestamp": 1.1329862533428394
-        },
-        {
-            "x": 4.092384496633659,
-            "y": 2.0570204638146983,
-            "heading": 0.3291240376025158,
-            "angularVelocity": 0.000002531044182467546,
-            "velocityX": -3.1682716080441704,
-            "velocityY": 2.067048223690569,
-            "timestamp": 1.2201390420615195
-        },
-        {
-            "x": 3.8162567615509033,
-            "y": 2.2371633052825928,
-            "heading": 0.32912425818941665,
-            "angularVelocity": 0.0000025310366317804206,
-            "velocityX": -3.1683178374712258,
-            "velocityY": 2.066977363735053,
-            "timestamp": 1.3072918307801995
-        },
-        {
-            "x": 3.558387727399027,
-            "y": 2.3518646644990793,
-            "heading": 0.32912440943088817,
-            "angularVelocity": 0.0000020272145178599712,
-            "velocityX": -3.4564319203160125,
-            "velocityY": 1.5374371746628512,
-            "timestamp": 1.3818973893726278
-        },
-        {
-            "x": 3.300516056227942,
-            "y": 2.466560095044955,
-            "heading": 0.3291245606718786,
-            "angularVelocity": 0.000002027208070545675,
-            "velocityX": -3.4564672664652627,
-            "velocityY": 1.5373577077876843,
-            "timestamp": 1.4565029479650562
-        },
-        {
-            "x": 3.052458337002912,
-            "y": 2.601171414843865,
-            "heading": 0.329124716605593,
-            "angularVelocity": 0.0000020901085294853097,
-            "velocityX": -3.3249227524744356,
-            "velocityY": 1.804306841723335,
-            "timestamp": 1.5311085065574845
-        },
-        {
-            "x": 2.829814908557799,
-            "y": 2.7746151638451657,
-            "heading": 0.3291249161701291,
-            "angularVelocity": 0.000002674928516490017,
-            "velocityX": -2.984273995740964,
-            "velocityY": 2.3248100044237634,
-            "timestamp": 1.6057140651499129
-        },
-        {
-            "x": 2.638606432152533,
-            "y": 2.982201542308141,
-            "heading": 0.3291253317978473,
-            "angularVelocity": 0.000005571002027267265,
-            "velocityX": -2.562925337103115,
-            "velocityY": 2.7824519027734036,
-            "timestamp": 1.6803196237423412
-        },
-        {
-            "x": 2.4840036736729245,
-            "y": 3.218316897029485,
-            "heading": 0.3291273340604081,
-            "angularVelocity": 0.000026837980957261232,
-            "velocityX": -2.0722686271167303,
-            "velocityY": 3.1648493647939433,
-            "timestamp": 1.7549251823347696
-        },
-        {
-            "x": 2.3733884856918945,
-            "y": 3.4659660107751304,
-            "heading": 0.2954274954004952,
-            "angularVelocity": -0.4517068070492687,
-            "velocityX": -1.4826668423638905,
-            "velocityY": 3.31944587532086,
-            "timestamp": 1.829530740927198
-        },
-        {
-            "x": 2.305023098579148,
-            "y": 3.7064756804968377,
-            "heading": 0.19598318894217118,
-            "angularVelocity": -1.3329342790875696,
-            "velocityX": -0.9163578210871337,
-            "velocityY": 3.2237500028063106,
-            "timestamp": 1.9041362995196263
-        },
-        {
-            "x": 2.272475779606133,
-            "y": 3.9200958593762563,
-            "heading": 0.053473340322528436,
-            "angularVelocity": -1.9101773555262378,
-            "velocityX": -0.43625863256144176,
-            "velocityY": 2.8633279196585013,
-            "timestamp": 1.9787418581120546
-        },
-        {
-            "x": 2.2677016800416907,
-            "y": 4.09662450199971,
-            "heading": -0.09830221598718161,
-            "angularVelocity": -2.034373298360552,
-            "velocityX": -0.06399120460344819,
-            "velocityY": 2.3661593848232187,
-            "timestamp": 2.053347416704483
-        },
-        {
-            "x": 2.2778239174156503,
-            "y": 4.229436050413757,
-            "heading": -0.23380831521279008,
-            "angularVelocity": -1.8163003103546322,
-            "velocityX": 0.13567671853055638,
-            "velocityY": 1.7801830174558415,
-            "timestamp": 2.1279529752969113
-        },
-        {
-            "x": 2.29107059750262,
-            "y": 4.316936633046431,
-            "heading": -0.33501872696397506,
-            "angularVelocity": -1.3566068488823915,
-            "velocityX": 0.1775562081015563,
-            "velocityY": 1.1728426712906341,
-            "timestamp": 2.2025585338893396
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.7369594715641588,
-            "velocityX": 0.11968816621507666,
-            "velocityY": 0.5772139203303202,
-            "timestamp": 2.277164092481768
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -8.092865962806098e-32,
-            "velocityX": 0,
-            "velocityY": 4.648171279746289e-33,
-            "timestamp": 2.3517696510741963
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.1.traj
deleted file mode 100644
index 8293ba75..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.1.traj	
+++ /dev/null
@@ -1,220 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -1.1520076473353784e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 0.8649552704871435,
-            "heading": -1.2072196888077206e-18,
-            "angularVelocity": -1.2485272283928241e-17,
-            "velocityX": -1.2865550388289577e-22,
-            "velocityY": 0.8062267267095138,
-            "timestamp": 0.09669149868710694
-        },
-        {
-            "x": 7.68,
-            "y": 1.0208658083668138,
-            "heading": -3.6224298175784965e-18,
-            "angularVelocity": -2.497851580797786e-17,
-            "velocityX": -3.7534290948711127e-22,
-            "velocityY": 1.6124534214139756,
-            "timestamp": 0.19338299737421388
-        },
-        {
-            "x": 7.68,
-            "y": 1.254731607449781,
-            "heading": -7.246204108405477e-18,
-            "angularVelocity": -3.7477692864160655e-17,
-            "velocityX": -8.581224837351263e-22,
-            "velocityY": 2.4186800521083573,
-            "timestamp": 0.29007449606132085
-        },
-        {
-            "x": 7.68,
-            "y": 1.5665526491683786,
-            "heading": -1.208276035533523e-17,
-            "angularVelocity": -5.0020491067317066e-17,
-            "velocityX": -2.0493771641846554e-21,
-            "velocityY": 3.2249064907727645,
-            "timestamp": 0.38676599474842777
-        },
-        {
-            "x": 7.68,
-            "y": 1.9323308421442915,
-            "heading": -4.9593132234185194e-18,
-            "angularVelocity": 7.367190734076976e-17,
-            "velocityX": 8.922575847291113e-21,
-            "velocityY": 3.782940568121389,
-            "timestamp": 0.4834574934355347
-        },
-        {
-            "x": 7.68,
-            "y": 2.298109035120243,
-            "heading": 4.533704301231546e-17,
-            "angularVelocity": 5.201735097557347e-16,
-            "velocityX": 4.097272417616201e-22,
-            "velocityY": 3.782940568121787,
-            "timestamp": 0.5801489921226416
-        },
-        {
-            "x": 7.68,
-            "y": 2.6638872280961943,
-            "heading": 2.5191507323029372e-17,
-            "angularVelocity": -2.083485721374208e-16,
-            "velocityX": 1.8682331175835897e-21,
-            "velocityY": 3.782940568121787,
-            "timestamp": 0.6768404908097485
-        },
-        {
-            "x": 7.68,
-            "y": 3.0296654210721456,
-            "heading": 5.2668856754382746e-17,
-            "angularVelocity": 2.8417544256148633e-16,
-            "velocityX": 2.5170076181953607e-21,
-            "velocityY": 3.782940568121787,
-            "timestamp": 0.7735319894968554
-        },
-        {
-            "x": 7.68,
-            "y": 3.395443614048097,
-            "heading": 4.929611386988728e-17,
-            "angularVelocity": -3.48814831737134e-17,
-            "velocityX": 3.0934195236040766e-21,
-            "velocityY": 3.782940568121787,
-            "timestamp": 0.8702234881839623
-        },
-        {
-            "x": 7.68,
-            "y": 3.7612218070240484,
-            "heading": 4.8005098046144365e-17,
-            "angularVelocity": -1.3351906227498234e-17,
-            "velocityX": -1.4483318353601114e-20,
-            "velocityY": 3.782940568121787,
-            "timestamp": 0.9669149868710692
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.8136822799879804e-17,
-            "angularVelocity": -1.0205938867695289e-16,
-            "velocityX": -1.0465805642283078e-22,
-            "velocityY": 3.782940568121787,
-            "timestamp": 1.0636064855581762
-        },
-        {
-            "x": 7.68,
-            "y": 4.492778192975952,
-            "heading": 4.310486593468924e-17,
-            "angularVelocity": 5.1380350929111975e-17,
-            "velocityX": 2.042774398860212e-21,
-            "velocityY": 3.782940568121787,
-            "timestamp": 1.1602979842452832
-        },
-        {
-            "x": 7.68,
-            "y": 4.8585563859519025,
-            "heading": 1.5339712420766394e-17,
-            "angularVelocity": -2.8715196155735124e-16,
-            "velocityX": 3.895366641644577e-22,
-            "velocityY": 3.782940568121787,
-            "timestamp": 1.2569894829323902
-        },
-        {
-            "x": 7.68,
-            "y": 5.224334578927854,
-            "heading": -2.710858238227145e-17,
-            "angularVelocity": -4.390075175101735e-16,
-            "velocityX": -3.863371094269421e-21,
-            "velocityY": 3.782940568121787,
-            "timestamp": 1.3536809816194972
-        },
-        {
-            "x": 7.68,
-            "y": 5.590112771903805,
-            "heading": -3.378098304971619e-17,
-            "angularVelocity": -6.900710774001486e-17,
-            "velocityX": -4.042329982282679e-22,
-            "velocityY": 3.782940568121787,
-            "timestamp": 1.4503724803066043
-        },
-        {
-            "x": 7.68,
-            "y": 5.955890964879757,
-            "heading": -3.941496183315801e-17,
-            "angularVelocity": -5.826757119208287e-17,
-            "velocityX": 2.6937052538433574e-21,
-            "velocityY": 3.782940568121787,
-            "timestamp": 1.5470639789937113
-        },
-        {
-            "x": 7.68,
-            "y": 6.321669157855708,
-            "heading": -1.9923808577496458e-17,
-            "angularVelocity": 2.0158083720198419e-16,
-            "velocityX": -4.499746264941967e-21,
-            "velocityY": 3.782940568121787,
-            "timestamp": 1.6437554776808183
-        },
-        {
-            "x": 7.68,
-            "y": 6.68744735083162,
-            "heading": 1.1959451428437676e-17,
-            "angularVelocity": 3.297421225035248e-16,
-            "velocityX": 5.431014750114918e-21,
-            "velocityY": 3.782940568121389,
-            "timestamp": 1.7404469763679253
-        },
-        {
-            "x": 7.68,
-            "y": 6.999268392550218,
-            "heading": 7.18685516064291e-18,
-            "angularVelocity": -4.935900605111643e-17,
-            "velocityX": -2.4046856363551417e-22,
-            "velocityY": 3.2249064907727645,
-            "timestamp": 1.8371384750550324
-        },
-        {
-            "x": 7.68,
-            "y": 7.2331341916331855,
-            "heading": 3.596188330325507e-18,
-            "angularVelocity": -3.713528985888002e-17,
-            "velocityX": -1.8035546521541034e-22,
-            "velocityY": 2.4186800521083573,
-            "timestamp": 1.9338299737421394
-        },
-        {
-            "x": 7.68,
-            "y": 7.3890447295128565,
-            "heading": 1.199348678010309e-18,
-            "angularVelocity": -2.4788525206448663e-17,
-            "velocityX": -1.2023814854884031e-22,
-            "velocityY": 1.6124534214139756,
-            "timestamp": 2.030521472429246
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": -1.2403868947555692e-17,
-            "velocityX": -6.011942579686287e-23,
-            "velocityY": 0.8062267267095138,
-            "timestamp": 2.127212971116353
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -2.719452927302828e-33,
-            "velocityY": 0,
-            "timestamp": 2.2239044698034602
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.1.traj
deleted file mode 100644
index f16ed7e1..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.1.traj	
+++ /dev/null
@@ -1,175 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.5195168592304314e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 0.8688441539517631,
-            "heading": 1.4487899486085006e-18,
-            "angularVelocity": 1.4623322004407643e-17,
-            "velocityX": 2.341988908112997e-19,
-            "velocityY": 0.8260917454276263,
-            "timestamp": 0.09907392781100396
-        },
-        {
-            "x": 7.68,
-            "y": 1.03253245874193,
-            "heading": 4.3483011920968684e-18,
-            "angularVelocity": 2.926613800008521e-17,
-            "velocityX": 4.684469740721679e-19,
-            "velocityY": 1.6521834594306495,
-            "timestamp": 0.19814785562200793
-        },
-        {
-            "x": 7.68,
-            "y": 1.2780649081437847,
-            "heading": 8.701494230539972e-18,
-            "angularVelocity": 4.393883572228609e-17,
-            "velocityX": 7.027934476495029e-19,
-            "velocityY": 2.4782751105844794,
-            "timestamp": 0.29722178343301187
-        },
-        {
-            "x": 7.68,
-            "y": 1.6054414834771922,
-            "heading": 1.4516368843485863e-17,
-            "angularVelocity": 5.869227900187472e-17,
-            "velocityX": 9.37435089599571e-19,
-            "velocityY": 3.304366573190877,
-            "timestamp": 0.39629571124401586
-        },
-        {
-            "x": 7.68,
-            "y": 1.9802322649266864,
-            "heading": 6.647154079192576e-18,
-            "angularVelocity": -7.942770553424421e-17,
-            "velocityX": 1.0709424285811041e-18,
-            "velocityY": 3.782940575087071,
-            "timestamp": 0.49536963905501985
-        },
-        {
-            "x": 7.68,
-            "y": 2.3550230463762047,
-            "heading": -4.11606275908414e-17,
-            "angularVelocity": -4.825465460619937e-16,
-            "velocityX": 1.073083844244948e-18,
-            "velocityY": 3.7829405750873115,
-            "timestamp": 0.5944435668660238
-        },
-        {
-            "x": 7.68,
-            "y": 2.729813827825723,
-            "heading": -7.635066611173202e-17,
-            "angularVelocity": -3.5518969822232415e-16,
-            "velocityX": 1.0738946259920485e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.6935174946770278
-        },
-        {
-            "x": 7.68,
-            "y": 3.1046046092752415,
-            "heading": -5.864574302033697e-17,
-            "angularVelocity": 1.787041604453991e-16,
-            "velocityX": 1.0691415340399175e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.7925914224880318
-        },
-        {
-            "x": 7.68,
-            "y": 3.4793953907247595,
-            "heading": -2.6391173603704442e-17,
-            "angularVelocity": 3.255606205313886e-16,
-            "velocityX": 1.0732289061667476e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.8916653502990358
-        },
-        {
-            "x": 7.68,
-            "y": 3.854186172174278,
-            "heading": -6.511485011951871e-17,
-            "angularVelocity": -3.9085637736788184e-16,
-            "velocityX": 1.0716697150075795e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.9907392781100398
-        },
-        {
-            "x": 7.68,
-            "y": 4.228976953623796,
-            "heading": -6.241329479526944e-17,
-            "angularVelocity": 2.7268075304360027e-17,
-            "velocityX": 1.0686111075844087e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 1.0898132059210437
-        },
-        {
-            "x": 7.68,
-            "y": 4.603767735073314,
-            "heading": -2.2737833863559818e-17,
-            "angularVelocity": 4.004631875225205e-16,
-            "velocityX": 1.0710197916722359e-18,
-            "velocityY": 3.7829405750873115,
-            "timestamp": 1.1888871337320477
-        },
-        {
-            "x": 7.68,
-            "y": 4.978558516522808,
-            "heading": -1.448944579275074e-17,
-            "angularVelocity": 8.325488100707829e-17,
-            "velocityX": 1.0714504382145178e-18,
-            "velocityY": 3.782940575087071,
-            "timestamp": 1.2879610615430517
-        },
-        {
-            "x": 7.68,
-            "y": 5.305935091856216,
-            "heading": -8.687037170545135e-18,
-            "angularVelocity": 5.856645386228716e-17,
-            "velocityX": 9.36753995288457e-19,
-            "velocityY": 3.304366573190877,
-            "timestamp": 1.3870349893540557
-        },
-        {
-            "x": 7.68,
-            "y": 5.55146754125807,
-            "heading": -4.341905072762387e-18,
-            "angularVelocity": 4.385747283685971e-17,
-            "velocityX": 7.025655481927048e-19,
-            "velocityY": 2.4782751105844794,
-            "timestamp": 1.4861089171650597
-        },
-        {
-            "x": 7.68,
-            "y": 5.715155846048237,
-            "heading": -1.4469357825353783e-18,
-            "angularVelocity": 2.9220293918225814e-17,
-            "velocityX": 4.683770471216647e-19,
-            "velocityY": 1.6521834594306495,
-            "timestamp": 1.5851828449760637
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 1.460460702963924e-17,
-            "velocityX": 2.341885280588165e-19,
-            "velocityY": 0.8260917454276263,
-            "timestamp": 1.6842567727870676
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 3.1812192470207854e-34,
-            "velocityY": 0,
-            "timestamp": 1.7833307005980716
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C3.1.traj
deleted file mode 100644
index 864eb2e4..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C3.1.traj	
+++ /dev/null
@@ -1,139 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 2.417290808823166e-40,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 0.8634453720055388,
-            "heading": 1.009810999063182e-18,
-            "angularVelocity": 1.0546271494620871e-17,
-            "velocityX": -3.226662245674165e-34,
-            "velocityY": 0.7983807350946677,
-            "timestamp": 0.09575052183150978
-        },
-        {
-            "x": 7.68,
-            "y": 1.0163361129826893,
-            "heading": 3.0323418642587253e-18,
-            "angularVelocity": 2.112292263585315e-17,
-            "velocityX": -6.037770263383471e-34,
-            "velocityY": 1.596761438503583,
-            "timestamp": 0.19150104366301957
-        },
-        {
-            "x": 7.68,
-            "y": 1.2456722168635992,
-            "heading": 6.073900773319643e-18,
-            "angularVelocity": 3.1765455168164525e-17,
-            "velocityX": -4.821612934727609e-34,
-            "velocityY": 2.3951420785410233,
-            "timestamp": 0.28725156549452935
-        },
-        {
-            "x": 7.68,
-            "y": 1.5514536654447455,
-            "heading": 1.0152788689831222e-17,
-            "angularVelocity": 4.2599119433487623e-17,
-            "velocityX": -1.2140491970354773e-33,
-            "velocityY": 3.19352252846437,
-            "timestamp": 0.38300208732603913
-        },
-        {
-            "x": 7.68,
-            "y": 1.9136721992668209,
-            "heading": 4.411917253468209e-18,
-            "angularVelocity": -5.995655508243724e-17,
-            "velocityX": 4.90795348832912e-33,
-            "velocityY": 3.7829405719528495,
-            "timestamp": 0.4787526091575489
-        },
-        {
-            "x": 7.68,
-            "y": 2.27589073308894,
-            "heading": 1.2796107303848674e-17,
-            "angularVelocity": 8.756286535058141e-17,
-            "velocityX": -1.5435756690671252e-33,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.5745031309890587
-        },
-        {
-            "x": 7.68,
-            "y": 2.63810926691106,
-            "heading": 6.994895399550348e-18,
-            "angularVelocity": -6.058673930264937e-17,
-            "velocityX": 4.776897057793067e-34,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.6702536528205685
-        },
-        {
-            "x": 7.68,
-            "y": 3.0003278007331793,
-            "heading": -9.534020306729642e-18,
-            "angularVelocity": -1.7262481070720064e-16,
-            "velocityX": 2.9275045647973183e-33,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.7660041746520783
-        },
-        {
-            "x": 7.68,
-            "y": 3.3625463345552546,
-            "heading": -1.0013819273727212e-17,
-            "angularVelocity": -5.010927959660929e-18,
-            "velocityX": 2.237784317663532e-33,
-            "velocityY": 3.7829405719528495,
-            "timestamp": 0.861754696483588
-        },
-        {
-            "x": 7.68,
-            "y": 3.6683277831364007,
-            "heading": -6.0064636534747415e-18,
-            "angularVelocity": 4.185204993640188e-17,
-            "velocityX": -2.2313398142102424e-33,
-            "velocityY": 3.19352252846437,
-            "timestamp": 0.9575052183150978
-        },
-        {
-            "x": 7.68,
-            "y": 3.897663887017311,
-            "heading": -3.002975312877452e-18,
-            "angularVelocity": 3.136785349810471e-17,
-            "velocityX": -2.8833027864886262e-33,
-            "velocityY": 2.3951420785410233,
-            "timestamp": 1.0532557401466076
-        },
-        {
-            "x": 7.68,
-            "y": 4.050554627994461,
-            "heading": -1.0009557447862449e-18,
-            "angularVelocity": 2.0908706606963636e-17,
-            "velocityX": -3.742421273800505e-33,
-            "velocityY": 1.596761438503583,
-            "timestamp": 1.1490062619781174
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 1.0453788927770355e-17,
-            "velocityX": -1.5032681111068507e-33,
-            "velocityY": 0.7983807350946677,
-            "timestamp": 1.2447567838096272
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 9.665729080515376e-43,
-            "velocityY": 2.80463986608038e-38,
-            "timestamp": 1.340507305641137
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.1.traj
deleted file mode 100644
index 17170fc9..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.1.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 9.069206296242399e-40,
-            "angularVelocity": -9.927610039652183e-40,
-            "velocityX": -8.253947176298157e-40,
-            "velocityY": 1.8367099231598242e-40,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 0.8705000055758465,
-            "heading": 6.490773345690525e-22,
-            "angularVelocity": 6.486159683422905e-21,
-            "velocityX": -7.132918469580153e-33,
-            "velocityY": 0.8344065481228213,
-            "timestamp": 0.10007112931184182
-        },
-        {
-            "x": 7.68,
-            "y": 1.037500013629847,
-            "heading": 2.023593010760001e-21,
-            "angularVelocity": 1.3735386658326633e-20,
-            "velocityX": -1.5648094960655976e-32,
-            "velocityY": 1.668813065290736,
-            "timestamp": 0.20014225862368365
-        },
-        {
-            "x": 7.68,
-            "y": 1.2880000179666162,
-            "heading": 3.787875676222919e-21,
-            "angularVelocity": 1.7630286060953193e-20,
-            "velocityX": -2.3089760494009302e-32,
-            "velocityY": 2.503219520548835,
-            "timestamp": 0.3002133879355255
-        },
-        {
-            "x": 7.68,
-            "y": 1.6219999999999999,
-            "heading": 5.860880748165694e-21,
-            "angularVelocity": 2.0715315514205814e-20,
-            "velocityX": -3.0704903308303665e-32,
-            "velocityY": 3.3376257900775013,
-            "timestamp": 0.4002845172473673
-        },
-        {
-            "x": 7.68,
-            "y": 1.9559999820333838,
-            "heading": 3.934658943161884e-21,
-            "angularVelocity": -1.9248526176341983e-20,
-            "velocityX": -4.040562356218352e-32,
-            "velocityY": 3.3376257900775017,
-            "timestamp": 0.5003556465592092
-        },
-        {
-            "x": 7.68,
-            "y": 2.206499986370153,
-            "heading": 1.6176834885162177e-21,
-            "angularVelocity": -2.3153285409554772e-20,
-            "velocityX": -3.08903871620124e-32,
-            "velocityY": 2.503219520548835,
-            "timestamp": 0.600426775871051
-        },
-        {
-            "x": 7.68,
-            "y": 2.3734999944241535,
-            "heading": 4.992981261257204e-22,
-            "angularVelocity": -1.1175904096118944e-20,
-            "velocityX": -2.3331216062245415e-32,
-            "velocityY": 1.668813065290736,
-            "timestamp": 0.7004979051828928
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 1.7562757360140444e-40,
-            "angularVelocity": -4.9894322348928345e-21,
-            "velocityX": -1.3647931662662733e-32,
-            "velocityY": 0.8344065481228213,
-            "timestamp": 0.8005690344947346
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -7.274275970183695e-41,
-            "angularVelocity": -2.4819026120794767e-39,
-            "velocityX": -8.637198685981915e-42,
-            "velocityY": 5.142787784847508e-39,
-            "timestamp": 0.9006401638065764
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.1.traj
deleted file mode 100644
index d9cc7c97..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.1.traj	
+++ /dev/null
@@ -1,247 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -3.982415645335998e-31,
-            "velocityX": 3.346280928312519e-33,
-            "velocityY": 3.541078180282768e-38,
-            "timestamp": 0
-        },
-        {
-            "x": 7.629395539536042,
-            "y": 0.7881897280541793,
-            "heading": -0.02207380596023684,
-            "angularVelocity": -0.2822732032732244,
-            "velocityX": -0.6471146471374848,
-            "velocityY": 0.015213885156899113,
-            "timestamp": 0.0782001468948451
-        },
-        {
-            "x": 7.528184091504648,
-            "y": 0.7905692108612611,
-            "heading": -0.06614402373093474,
-            "angularVelocity": -0.5635567133951742,
-            "velocityX": -1.2942616101155584,
-            "velocityY": 0.030428111729043,
-            "timestamp": 0.1564002937896902
-        },
-        {
-            "x": 7.376360973312952,
-            "y": 0.7941378827255743,
-            "heading": -0.13206235338942335,
-            "angularVelocity": -0.8429438086302324,
-            "velocityX": -1.9414684526853105,
-            "velocityY": 0.04563510435477082,
-            "timestamp": 0.23460044068453528
-        },
-        {
-            "x": 7.173916466766183,
-            "y": 0.7988917309385671,
-            "heading": -0.21951772550448787,
-            "angularVelocity": -1.1183530413716716,
-            "velocityX": -2.588799568612108,
-            "velocityY": 0.060790783668257076,
-            "timestamp": 0.3128005875793804
-        },
-        {
-            "x": 6.920824289019536,
-            "y": 0.8048108023879232,
-            "heading": -0.3276760191057486,
-            "angularVelocity": -1.383095785572821,
-            "velocityX": -3.236466781667369,
-            "velocityY": 0.07569130857196414,
-            "timestamp": 0.3910007344742255
-        },
-        {
-            "x": 6.625087122175782,
-            "y": 0.8120803100358527,
-            "heading": -0.3276760358371292,
-            "angularVelocity": -2.1395587179409026e-7,
-            "velocityX": -3.781798098684327,
-            "velocityY": 0.09296028123113304,
-            "timestamp": 0.4692008813690706
-        },
-        {
-            "x": 6.329343587367458,
-            "y": 0.8190859886904246,
-            "heading": -0.3276760513572638,
-            "angularVelocity": -1.9846682128968708e-7,
-            "velocityX": -3.78187953030339,
-            "velocityY": 0.08958651527359804,
-            "timestamp": 0.5474010282639157
-        },
-        {
-            "x": 6.033600055760057,
-            "y": 0.826091802470254,
-            "heading": -0.3276760668773978,
-            "angularVelocity": -1.9846681457625392e-7,
-            "velocityX": -3.7818794893709464,
-            "velocityY": 0.0895882432148227,
-            "timestamp": 0.6256011751587608
-        },
-        {
-            "x": 5.7383102527596765,
-            "y": 0.8439033174946344,
-            "heading": -0.3276760825327013,
-            "angularVelocity": -2.0019532088764418e-7,
-            "velocityX": -3.7760773441696487,
-            "velocityY": 0.22776830647924015,
-            "timestamp": 0.7038013220536059
-        },
-        {
-            "x": 5.450465299571764,
-            "y": 0.9121572529445121,
-            "heading": -0.32767609968005607,
-            "angularVelocity": -2.1927522433491614e-7,
-            "velocityX": -3.680874839978332,
-            "velocityY": 0.8728108342551084,
-            "timestamp": 0.782001468948451
-        },
-        {
-            "x": 5.178616904201158,
-            "y": 1.028826716826592,
-            "heading": -0.32767612118071976,
-            "angularVelocity": -2.749440322201668e-7,
-            "velocityX": -3.4763156613508643,
-            "velocityY": 1.4919340757663395,
-            "timestamp": 0.8602016158432961
-        },
-        {
-            "x": 4.930841529412244,
-            "y": 1.190445624246428,
-            "heading": -0.3276761534331376,
-            "angularVelocity": -4.1243423626599064e-7,
-            "velocityX": -3.1684771017365216,
-            "velocityY": 2.0667340642903906,
-            "timestamp": 0.9384017627381412
-        },
-        {
-            "x": 4.7145004488669215,
-            "y": 1.3922123936122235,
-            "heading": -0.32767621878386327,
-            "angularVelocity": -8.356854591223944e-7,
-            "velocityX": -2.7665047846524513,
-            "velocityY": 2.5801328690255976,
-            "timestamp": 1.0166019096329864
-        },
-        {
-            "x": 4.536026477813721,
-            "y": 1.628126621246338,
-            "heading": -0.327696539002069,
-            "angularVelocity": -0.00025984882823803823,
-            "velocityX": -2.282271557538034,
-            "velocityY": 3.016800313073996,
-            "timestamp": 1.0948020565278314
-        },
-        {
-            "x": 4.4347589269746495,
-            "y": 1.8009845745755027,
-            "heading": -0.3311064514524199,
-            "angularVelocity": -0.06394544615832723,
-            "velocityX": -1.899051313343304,
-            "velocityY": 3.241572651564751,
-            "timestamp": 1.1481273933038434
-        },
-        {
-            "x": 4.355560152796487,
-            "y": 1.9823487857199633,
-            "heading": -0.3404702376957046,
-            "angularVelocity": -0.17559732033810904,
-            "velocityX": -1.4851996999249673,
-            "velocityY": 3.4010889027473166,
-            "timestamp": 1.2014527300798554
-        },
-        {
-            "x": 4.29961502910342,
-            "y": 2.1668323093896307,
-            "heading": -0.3620606217709223,
-            "angularVelocity": -0.40488040734269476,
-            "velocityX": -1.049128370778876,
-            "velocityY": 3.4595847832064672,
-            "timestamp": 1.2547780668558675
-        },
-        {
-            "x": 4.26624261518445,
-            "y": 2.3445509425061104,
-            "heading": -0.3930653835662206,
-            "angularVelocity": -0.5814264600999738,
-            "velocityX": -0.6258265945712352,
-            "velocityY": 3.332724064417915,
-            "timestamp": 1.3081034036318795
-        },
-        {
-            "x": 4.250245555353669,
-            "y": 2.506179624728893,
-            "heading": -0.42827286207411436,
-            "angularVelocity": -0.6602392152823773,
-            "velocityX": -0.2999898509389163,
-            "velocityY": 3.0309922448647635,
-            "timestamp": 1.3614287404078915
-        },
-        {
-            "x": 4.245253885327146,
-            "y": 2.646807044638539,
-            "heading": -0.46326114250173545,
-            "angularVelocity": -0.6561286349527023,
-            "velocityX": -0.09360784813172378,
-            "velocityY": 2.6371595270071015,
-            "timestamp": 1.4147540771839036
-        },
-        {
-            "x": 4.246491895421447,
-            "y": 2.764573131222025,
-            "heading": -0.49520995766037457,
-            "angularVelocity": -0.5991301150594935,
-            "velocityX": 0.023216170194527277,
-            "velocityY": 2.2084452476729117,
-            "timestamp": 1.4680794139599156
-        },
-        {
-            "x": 4.250760184591696,
-            "y": 2.858861309855261,
-            "heading": -0.522398988898447,
-            "angularVelocity": -0.5098707834110707,
-            "velocityX": 0.08004242313776105,
-            "velocityY": 1.7681684605074017,
-            "timestamp": 1.5214047507359276
-        },
-        {
-            "x": 4.255888225984497,
-            "y": 2.929511890736146,
-            "heading": -0.5437259487715407,
-            "angularVelocity": -0.3999404628668233,
-            "velocityX": 0.09616519468543994,
-            "velocityY": 1.324897040550944,
-            "timestamp": 1.5747300875119397
-        },
-        {
-            "x": 4.260343621579396,
-            "y": 2.9765343465211713,
-            "heading": -0.5584397105575287,
-            "angularVelocity": -0.2759244043319095,
-            "velocityX": 0.08355119468920533,
-            "velocityY": 0.8818032595380478,
-            "timestamp": 1.6280554242879517
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -0.14177668439456792,
-            "velocityX": 0.04981456435476623,
-            "velocityY": 0.4400469813703916,
-            "timestamp": 1.6813807610639637
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 0,
-            "velocityX": 3.549235715029611e-32,
-            "velocityY": -9.598836837089386e-32,
-            "timestamp": 1.7347060978399758
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.1.traj
deleted file mode 100644
index 773c6f5e..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 2.8467808772943154e-31,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.633526252649942,
-            "y": 0.8024218861992577,
-            "heading": -0.02675508445787236,
-            "angularVelocity": -0.3471453590528024,
-            "velocityX": -0.602993637929559,
-            "velocityY": 0.20009790028293237,
-            "timestamp": 0.07707170428800919
-        },
-        {
-            "x": 7.540488865885011,
-            "y": 0.8333059401759839,
-            "heading": -0.07783349665964928,
-            "angularVelocity": -0.6627388439588938,
-            "velocityX": -1.2071536191448495,
-            "velocityY": 0.400718451240102,
-            "timestamp": 0.15414340857601838
-        },
-        {
-            "x": 7.400762361365792,
-            "y": 0.8797032018771439,
-            "heading": -0.14939031986058435,
-            "angularVelocity": -0.9284448016555393,
-            "velocityX": -1.8129416730824437,
-            "velocityY": 0.6020012419574645,
-            "timestamp": 0.23121511286402757
-        },
-        {
-            "x": 7.214174023420098,
-            "y": 0.9416744881041137,
-            "heading": -0.23458534610519022,
-            "angularVelocity": -1.105399537114693,
-            "velocityX": -2.4209707008480317,
-            "velocityY": 0.8040731264406625,
-            "timestamp": 0.30828681715203676
-        },
-        {
-            "x": 6.98058449689491,
-            "y": 1.0192605514016468,
-            "heading": -0.3182722929391017,
-            "angularVelocity": -1.085832311702643,
-            "velocityX": -3.030807852026836,
-            "velocityY": 1.0066737723562156,
-            "timestamp": 0.38535852144004595
-        },
-        {
-            "x": 6.7044949607857305,
-            "y": 1.1109419048147462,
-            "heading": -0.3204977394277859,
-            "angularVelocity": -0.02887501333002772,
-            "velocityX": -3.5822425189594846,
-            "velocityY": 1.189559180766205,
-            "timestamp": 0.46243022572805514
-        },
-        {
-            "x": 6.427835578289735,
-            "y": 1.202949830409229,
-            "heading": -0.32049776831244,
-            "angularVelocity": -3.7477637567079044e-7,
-            "velocityX": -3.5896362361749183,
-            "velocityY": 1.1937964320946928,
-            "timestamp": 0.5395019300160644
-        },
-        {
-            "x": 6.1511761970625,
-            "y": 1.294957759818759,
-            "heading": -0.3204977971970938,
-            "angularVelocity": -3.7477637370825724e-7,
-            "velocityX": -3.589636219712834,
-            "velocityY": 1.1937964815946673,
-            "timestamp": 0.6165736343040735
-        },
-        {
-            "x": 5.87524941446386,
-            "y": 1.3871813457379307,
-            "heading": -0.32266464886855784,
-            "angularVelocity": -0.028114749653993878,
-            "velocityX": -3.5801308034856563,
-            "velocityY": 1.1965946098005253,
-            "timestamp": 0.6936453385920827
-        },
-        {
-            "x": 5.641703675217654,
-            "y": 1.4649149367510799,
-            "heading": -0.4060390277887848,
-            "angularVelocity": -1.081776764773039,
-            "velocityX": -3.030239715129037,
-            "velocityY": 1.0085879342004245,
-            "timestamp": 0.7707170428800918
-        },
-        {
-            "x": 5.455154020778328,
-            "y": 1.5270189169325792,
-            "heading": -0.49098864699750666,
-            "angularVelocity": -1.1022153979010723,
-            "velocityX": -2.420468785044752,
-            "velocityY": 0.8057948212669979,
-            "timestamp": 0.8477887471681009
-        },
-        {
-            "x": 5.315462796839212,
-            "y": 1.573537321073397,
-            "heading": -0.5623285861822422,
-            "angularVelocity": -0.9256307466375128,
-            "velocityX": -1.8124839100106522,
-            "velocityY": 0.6035730566821683,
-            "timestamp": 0.9248604514561101
-        },
-        {
-            "x": 5.222455052636063,
-            "y": 1.6045200564296747,
-            "heading": -0.613275527154411,
-            "angularVelocity": -0.6610330139033284,
-            "velocityX": -1.2067690089684353,
-            "velocityY": 0.40199883527290925,
-            "timestamp": 1.0019321557441192
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -0.34674817551357506,
-            "velocityX": -0.602751075316368,
-            "velocityY": 0.2008511906325378,
-            "timestamp": 1.0790038600321283
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 8.760742141155177e-29,
-            "velocityX": 3.313743560366677e-30,
-            "velocityY": 1.2206105198294578e-30,
-            "timestamp": 1.1560755643201375
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.1.traj
deleted file mode 100644
index bec33f37..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.1.traj	
+++ /dev/null
@@ -1,310 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -3.516607019090483e-32,
-            "velocityX": -4.092268936373955e-33,
-            "velocityY": -4.396806616029776e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 7.6580594556627535,
-            "y": 0.8642766421574168,
-            "heading": -0.06977499787943942,
-            "angularVelocity": -0.7009716360726537,
-            "velocityX": -0.22041848409624198,
-            "velocityY": 0.7763344454252913,
-            "timestamp": 0.09954040119279155
-        },
-        {
-            "x": 7.613877684624128,
-            "y": 1.0194223152597515,
-            "heading": -0.19807907998906352,
-            "angularVelocity": -1.2889648883484262,
-            "velocityX": -0.4438576749661031,
-            "velocityY": 1.558620130552277,
-            "timestamp": 0.1990808023855831
-        },
-        {
-            "x": 7.54706531781781,
-            "y": 1.253315507091588,
-            "heading": -0.3641253773176819,
-            "angularVelocity": -1.6681296773861405,
-            "velocityX": -0.6712085344815377,
-            "velocityY": 2.349731255139591,
-            "timestamp": 0.2986212035783746
-        },
-        {
-            "x": 7.457648537893544,
-            "y": 1.5658706931334108,
-            "heading": -0.49008580394214096,
-            "angularVelocity": -1.265420122031624,
-            "velocityX": -0.8982963585919527,
-            "velocityY": 3.139983185686188,
-            "timestamp": 0.3981616047711662
-        },
-        {
-            "x": 7.353528135913809,
-            "y": 1.9277448806018234,
-            "heading": -0.490085841920944,
-            "angularVelocity": -3.8154159059291386e-7,
-            "velocityX": -1.0460114760646044,
-            "velocityY": 3.6354503611807685,
-            "timestamp": 0.49770200596395775
-        },
-        {
-            "x": 7.249407681423134,
-            "y": 2.2896190529615703,
-            "heading": -0.4900858798995876,
-            "angularVelocity": -3.8153998933914064e-7,
-            "velocityX": -1.0460120035985523,
-            "velocityY": 3.635450209396512,
-            "timestamp": 0.5972424071567493
-        },
-        {
-            "x": 7.145287117295142,
-            "y": 2.651493193775847,
-            "heading": -0.4900859178782245,
-            "angularVelocity": -3.8153992177326955e-7,
-            "velocityX": -1.0460131050338848,
-            "velocityY": 3.6354498924852887,
-            "timestamp": 0.6967828083495408
-        },
-        {
-            "x": 7.025437006860337,
-            "y": 3.00846649318931,
-            "heading": -0.49008595631381496,
-            "angularVelocity": -3.8613055604326664e-7,
-            "velocityX": -1.204034834083884,
-            "velocityY": 3.586215196401226,
-            "timestamp": 0.7963232095423323
-        },
-        {
-            "x": 6.830624533504276,
-            "y": 3.3307118703307705,
-            "heading": -0.49008600153372534,
-            "angularVelocity": -4.542870012894294e-7,
-            "velocityX": -1.9571196320451347,
-            "velocityY": 3.237332513029861,
-            "timestamp": 0.8958636107351238
-        },
-        {
-            "x": 6.570226686668516,
-            "y": 3.602717269957059,
-            "heading": -0.4900860357215963,
-            "angularVelocity": -3.4345723556551647e-7,
-            "velocityX": -2.6160015804177474,
-            "velocityY": 2.7326130532612987,
-            "timestamp": 0.9954040119279153
-        },
-        {
-            "x": 6.270670526052627,
-            "y": 3.8032215967371097,
-            "heading": -0.4416564414377051,
-            "angularVelocity": 0.4865320382835506,
-            "velocityX": -3.009392739292897,
-            "velocityY": 2.0143009710369753,
-            "timestamp": 1.094944413120707
-        },
-        {
-            "x": 5.9498699270968665,
-            "y": 3.9255658761230467,
-            "heading": -0.3340733830984328,
-            "angularVelocity": 1.0807979177309486,
-            "velocityX": -3.222818022748657,
-            "velocityY": 1.229091684581206,
-            "timestamp": 1.1944848143134985
-        },
-        {
-            "x": 5.637666976812727,
-            "y": 3.9694128449739723,
-            "heading": -0.13726885518093873,
-            "angularVelocity": 1.977132154976145,
-            "velocityX": -3.136444564649281,
-            "velocityY": 0.4404941945733328,
-            "timestamp": 1.2940252155062901
-        },
-        {
-            "x": 5.333615486534618,
-            "y": 3.942111372133816,
-            "heading": 0.08955200005531357,
-            "angularVelocity": 2.278681344642582,
-            "velocityX": -3.0545535946677287,
-            "velocityY": -0.2742752943830166,
-            "timestamp": 1.3935656166990817
-        },
-        {
-            "x": 5.078082684497777,
-            "y": 3.9650753252247246,
-            "heading": 0.19884455131252843,
-            "angularVelocity": 1.0979717777662474,
-            "velocityX": -2.5671265031564414,
-            "velocityY": 0.2306998245509574,
-            "timestamp": 1.4931060178918734
-        },
-        {
-            "x": 4.8543877601623535,
-            "y": 4.064273357391357,
-            "heading": 0.3064368289582943,
-            "angularVelocity": 1.0808905364705061,
-            "velocityX": -2.2472777048805277,
-            "velocityY": 0.9965605018459247,
-            "timestamp": 1.592646419084665
-        },
-        {
-            "x": 4.700013932982481,
-            "y": 4.187105651970831,
-            "heading": 0.3888797273509616,
-            "angularVelocity": 1.0676410431648378,
-            "velocityX": -1.99915137751061,
-            "velocityY": 1.5906864226746236,
-            "timestamp": 1.6698660978526145
-        },
-        {
-            "x": 4.564169886311024,
-            "y": 4.35607150380987,
-            "heading": 0.4692148602751942,
-            "angularVelocity": 1.040345339504005,
-            "velocityX": -1.7591894817340108,
-            "velocityY": 2.1881190719116157,
-            "timestamp": 1.747085776620564
-        },
-        {
-            "x": 4.445019228108457,
-            "y": 4.571847534724939,
-            "heading": 0.5443267073647724,
-            "angularVelocity": 0.9727034389160626,
-            "velocityX": -1.543008985580261,
-            "velocityY": 2.7943140188848803,
-            "timestamp": 1.8243054553885136
-        },
-        {
-            "x": 4.302573716554072,
-            "y": 4.826880596020871,
-            "heading": 0.5443268349443496,
-            "angularVelocity": 0.0000016521640508021742,
-            "velocityX": -1.8446788930894609,
-            "velocityY": 3.302695185541032,
-            "timestamp": 1.9015251341564632
-        },
-        {
-            "x": 4.139811862406642,
-            "y": 5.06945260129317,
-            "heading": 0.544326895208066,
-            "angularVelocity": 7.80419154390702e-7,
-            "velocityX": -2.1077768872432143,
-            "velocityY": 3.141323677365259,
-            "timestamp": 1.9787448129244127
-        },
-        {
-            "x": 3.9382711614032555,
-            "y": 5.28090933180296,
-            "heading": 0.5443268879285122,
-            "angularVelocity": -9.42707078520697e-8,
-            "velocityX": -2.60996554529876,
-            "velocityY": 2.738378790013248,
-            "timestamp": 2.0559644916923623
-        },
-        {
-            "x": 3.721249786731846,
-            "y": 5.476445328956613,
-            "heading": 0.5443268807879367,
-            "angularVelocity": -9.247093001843253e-8,
-            "velocityX": -2.810441303745562,
-            "velocityY": 2.53220422919983,
-            "timestamp": 2.1331841704603116
-        },
-        {
-            "x": 3.5042282470745802,
-            "y": 5.671981142995692,
-            "heading": 0.5443268736473624,
-            "angularVelocity": -9.247091428352055e-8,
-            "velocityX": -2.810443440323428,
-            "velocityY": 2.53220185785385,
-            "timestamp": 2.210403849228261
-        },
-        {
-            "x": 3.287206691377032,
-            "y": 5.867516939231946,
-            "heading": 0.5443268665067905,
-            "angularVelocity": -9.247088327815795e-8,
-            "velocityX": -2.810443648046155,
-            "velocityY": 2.5322016273060566,
-            "timestamp": 2.2876235279962103
-        },
-        {
-            "x": 3.0701851252220784,
-            "y": 6.063052723861702,
-            "heading": 0.5443268593662193,
-            "angularVelocity": -9.247087442211068e-8,
-            "velocityX": -2.8104437834702534,
-            "velocityY": 2.532201477001163,
-            "timestamp": 2.3648432067641596
-        },
-        {
-            "x": 2.853163562032648,
-            "y": 6.258588511782302,
-            "heading": 0.5443268522252032,
-            "angularVelocity": -9.247663534512907e-8,
-            "velocityX": -2.8104437450665345,
-            "velocityY": 2.5322015196178005,
-            "timestamp": 2.442062885532109
-        },
-        {
-            "x": 2.6687757934039245,
-            "y": 6.424725614600548,
-            "heading": 0.4938774787934042,
-            "angularVelocity": -0.6533227570578579,
-            "velocityX": -2.3878339248576954,
-            "velocityY": 2.1514865830703513,
-            "timestamp": 2.5192825643000583
-        },
-        {
-            "x": 2.521265507868239,
-            "y": 6.557635351096379,
-            "heading": 0.4535239115954702,
-            "angularVelocity": -0.522581391709739,
-            "velocityX": -1.9102680545844264,
-            "velocityY": 1.7211899688838848,
-            "timestamp": 2.5965022430680076
-        },
-        {
-            "x": 2.4106327602038906,
-            "y": 6.6573176734827975,
-            "heading": 0.42326153130587874,
-            "angularVelocity": -0.391899846935805,
-            "velocityX": -1.432701474928527,
-            "velocityY": 1.2908927358526117,
-            "timestamp": 2.673721921835957
-        },
-        {
-            "x": 2.336877585524826,
-            "y": 6.723772559032594,
-            "heading": 0.40308732310971707,
-            "angularVelocity": -0.2612573442164476,
-            "velocityX": -0.9551344405446706,
-            "velocityY": 0.8605952085024652,
-            "timestamp": 2.7509416006039062
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": -0.13063150832354717,
-            "velocityX": -0.4775671967717696,
-            "velocityY": 0.43029758084408004,
-            "timestamp": 2.8281612793718556
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": -5.871044564465464e-32,
-            "velocityX": 0,
-            "velocityY": -1.090928833064942e-33,
-            "timestamp": 2.905380958139805
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.1.traj
deleted file mode 100644
index d94da80a..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.1.traj	
+++ /dev/null
@@ -1,283 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -2.906139149989869e-32,
-            "velocityX": 1.181772084740038e-33,
-            "velocityY": 2.7502272506846476e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 7.666043555718406,
-            "y": 0.8524996120976488,
-            "heading": 0.004999198621269784,
-            "angularVelocity": 0.05577597491216818,
-            "velocityX": -0.15571181404981524,
-            "velocityY": 0.7307780702234378,
-            "timestamp": 0.08962996396104499
-        },
-        {
-            "x": 7.638131101049187,
-            "y": 0.9835016263770707,
-            "heading": 0.014524734067527434,
-            "angularVelocity": 0.10627623871853438,
-            "velocityX": -0.31141878715192206,
-            "velocityY": 1.4615872693685117,
-            "timestamp": 0.17925992792208997
-        },
-        {
-            "x": 7.596263489657983,
-            "y": 1.180010140566484,
-            "heading": 0.02778512041931341,
-            "angularVelocity": 0.14794590743726296,
-            "velocityX": -0.46711623592083296,
-            "velocityY": 2.1924421867983774,
-            "timestamp": 0.26888989188313495
-        },
-        {
-            "x": 7.540442918712468,
-            "y": 1.4420312351509346,
-            "heading": 0.04318451816280708,
-            "angularVelocity": 0.17181082154832236,
-            "velocityX": -0.6227891709269913,
-            "velocityY": 2.9233649440976053,
-            "timestamp": 0.35851985584417995
-        },
-        {
-            "x": 7.470680035396563,
-            "y": 1.7695653388184471,
-            "heading": 0.05582613302662815,
-            "angularVelocity": 0.1410422843561041,
-            "velocityX": -0.7783433154812639,
-            "velocityY": 3.654292484261909,
-            "timestamp": 0.44814981980522495
-        },
-        {
-            "x": 7.399999368806171,
-            "y": 2.101181376235394,
-            "heading": 0.05582613685023459,
-            "angularVelocity": 4.265991273772723e-8,
-            "velocityX": -0.7885830080341364,
-            "velocityY": 3.6998345504308543,
-            "timestamp": 0.5377797837662699
-        },
-        {
-            "x": 7.329318638814834,
-            "y": 2.4327974001390613,
-            "heading": 0.05582614067384277,
-            "angularVelocity": 4.265993214747779e-8,
-            "velocityX": -0.788583715397405,
-            "velocityY": 3.6998343996634295,
-            "timestamp": 0.6274097477273148
-        },
-        {
-            "x": 7.258637432835217,
-            "y": 2.764413322590103,
-            "heading": 0.05582614449746126,
-            "angularVelocity": 4.266004720083137e-8,
-            "velocityX": -0.7885890259906463,
-            "velocityY": 3.699833267758185,
-            "timestamp": 0.7170397116883598
-        },
-        {
-            "x": 7.145396314308057,
-            "y": 3.084009006822374,
-            "heading": 0.05582614848984762,
-            "angularVelocity": 4.4542987394171874e-8,
-            "velocityX": -1.2634292542656518,
-            "velocityY": 3.5657236721770227,
-            "timestamp": 0.8066696756494047
-        },
-        {
-            "x": 6.971535677728531,
-            "y": 3.375105946924151,
-            "heading": 0.05582615332930866,
-            "angularVelocity": 5.39937854332775e-8,
-            "velocityX": -1.9397601973274063,
-            "velocityY": 3.2477636633692444,
-            "timestamp": 0.8962996396104497
-        },
-        {
-            "x": 6.743840876905014,
-            "y": 3.626342927147427,
-            "heading": 0.0558261606002303,
-            "angularVelocity": 8.112155042111421e-8,
-            "velocityX": -2.5403870621043505,
-            "velocityY": 2.8030467615993793,
-            "timestamp": 0.9859296035714946
-        },
-        {
-            "x": 6.471198532281258,
-            "y": 3.8279144449501397,
-            "heading": 0.0558261762788256,
-            "angularVelocity": 1.7492582391526053e-7,
-            "velocityX": -3.041866052096724,
-            "velocityY": 2.2489300329331785,
-            "timestamp": 1.0755595675325396
-        },
-        {
-            "x": 6.164249513947577,
-            "y": 3.9719534129902763,
-            "heading": 0.05582625958870765,
-            "angularVelocity": 9.294869524458774e-7,
-            "velocityX": -3.4246250335109787,
-            "velocityY": 1.6070403431461682,
-            "timestamp": 1.1651895314935845
-        },
-        {
-            "x": 5.836252922691461,
-            "y": 4.05240893270077,
-            "heading": 0.05954498226559844,
-            "angularVelocity": 0.04148972634315725,
-            "velocityX": -3.6594524505071675,
-            "velocityY": 0.8976408798452788,
-            "timestamp": 1.2548194954546295
-        },
-        {
-            "x": 5.497492188342187,
-            "y": 4.066758597782368,
-            "heading": 0.05954602093201425,
-            "angularVelocity": 0.000011588383726853398,
-            "velocityX": -3.779547813904135,
-            "velocityY": 0.16009897190000646,
-            "timestamp": 1.3444494594156744
-        },
-        {
-            "x": 5.204246867986104,
-            "y": 4.110932074419811,
-            "heading": 0.17401114275805463,
-            "angularVelocity": 1.2770854384789139,
-            "velocityX": -3.2717331057226993,
-            "velocityY": 0.4928427356797903,
-            "timestamp": 1.4340794233767193
-        },
-        {
-            "x": 4.937438011169434,
-            "y": 4.2165327072143555,
-            "heading": 0.3021760231951433,
-            "angularVelocity": 1.4299334148208713,
-            "velocityX": -2.976781926774297,
-            "velocityY": 1.1781844834885962,
-            "timestamp": 1.5237093873377643
-        },
-        {
-            "x": 4.703348327317259,
-            "y": 4.378813716693983,
-            "heading": 0.4253911826920849,
-            "angularVelocity": 1.3960058537628992,
-            "velocityX": -2.6521945051027034,
-            "velocityY": 1.8386149895276152,
-            "timestamp": 1.61197202546698
-        },
-        {
-            "x": 4.500555108729279,
-            "y": 4.597666686435954,
-            "heading": 0.5261840356750895,
-            "angularVelocity": 1.1419651068603307,
-            "velocityX": -2.297611117074157,
-            "velocityY": 2.479565242787912,
-            "timestamp": 1.7002346635961958
-        },
-        {
-            "x": 4.326794639818804,
-            "y": 4.871425359866347,
-            "heading": 0.5588913487111756,
-            "angularVelocity": 0.37056804248478087,
-            "velocityX": -1.968675224233522,
-            "velocityY": 3.1016371052677254,
-            "timestamp": 1.7884973017254115
-        },
-        {
-            "x": 4.0969805858398445,
-            "y": 5.11364296690146,
-            "heading": 0.5588923269288569,
-            "angularVelocity": 0.00001108303243636796,
-            "velocityX": -2.603752378696331,
-            "velocityY": 2.744282429905488,
-            "timestamp": 1.8767599398546273
-        },
-        {
-            "x": 3.8246168380678105,
-            "y": 5.306780372295281,
-            "heading": 0.5588922897097742,
-            "angularVelocity": -4.2168559122096544e-7,
-            "velocityX": -3.0858328455274044,
-            "velocityY": 2.188212470049549,
-            "timestamp": 1.965022577983843
-        },
-        {
-            "x": 3.520011793550478,
-            "y": 5.443527703198532,
-            "heading": 0.5588922240132788,
-            "angularVelocity": -7.44329614921164e-7,
-            "velocityX": -3.451121006278936,
-            "velocityY": 1.549322950250525,
-            "timestamp": 2.053285216113059
-        },
-        {
-            "x": 3.194693864220709,
-            "y": 5.518709582083228,
-            "heading": 0.5588921711301718,
-            "angularVelocity": -5.991562005821763e-7,
-            "velocityX": -3.6857943091787817,
-            "velocityY": 0.851797323060201,
-            "timestamp": 2.1415478542422743
-        },
-        {
-            "x": 2.8828791952129387,
-            "y": 5.532174882793972,
-            "heading": 0.5041280478943673,
-            "angularVelocity": -0.6204677811196886,
-            "velocityX": -3.5328047701370098,
-            "velocityY": 0.15255946339412532,
-            "timestamp": 2.22981049237149
-        },
-        {
-            "x": 2.6283322264519793,
-            "y": 5.543894951157175,
-            "heading": 0.3346872638422791,
-            "angularVelocity": -1.9197339626765784,
-            "velocityX": -2.883971906530878,
-            "velocityY": 0.132786291137652,
-            "timestamp": 2.3180731305007054
-        },
-        {
-            "x": 2.4385492640365714,
-            "y": 5.5521384887253,
-            "heading": 0.1780668377028913,
-            "angularVelocity": -1.7744815865360473,
-            "velocityX": -2.150207227406541,
-            "velocityY": 0.09339781523475092,
-            "timestamp": 2.406335768629921
-        },
-        {
-            "x": 2.31267915340354,
-            "y": 5.55742889143524,
-            "heading": 0.062002293486176877,
-            "angularVelocity": -1.3149906537667377,
-            "velocityX": -1.4260859781775244,
-            "velocityY": 0.0599393222554144,
-            "timestamp": 2.4945984067591365
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 9.107235017738231e-34,
-            "angularVelocity": -0.7024749633633888,
-            "velocityX": -0.7101436659051381,
-            "velocityY": 0.029130202985730554,
-            "timestamp": 2.582861044888352
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -2.2232006496445675e-32,
-            "velocityX": 0,
-            "velocityY": -2.9660881962078245e-33,
-            "timestamp": 2.6711236830175675
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W3.1.traj
deleted file mode 100644
index 62c8640c..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W3.1.traj	
+++ /dev/null
@@ -1,292 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -1.5069359770438582e-30,
-            "velocityX": 5.69693995403372e-32,
-            "velocityY": 3.042278119963715e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 7.627290492953592,
-            "y": 0.8035142159340071,
-            "heading": 0.015175150751654183,
-            "angularVelocity": 0.18617297608634506,
-            "velocityX": -0.6466549133823012,
-            "velocityY": 0.20260100070713255,
-            "timestamp": 0.08151102845675925
-        },
-        {
-            "x": 7.521871327482097,
-            "y": 0.8365426259859824,
-            "heading": 0.045515680376982795,
-            "angularVelocity": 0.3722260680519286,
-            "velocityX": -1.2933116839203953,
-            "velocityY": 0.4052017337690261,
-            "timestamp": 0.1630220569135185
-        },
-        {
-            "x": 7.363742244741542,
-            "y": 0.8860852747673063,
-            "heading": 0.09100430929463568,
-            "angularVelocity": 0.5580671693007231,
-            "velocityX": -1.9399716300284648,
-            "velocityY": 0.6078030141357551,
-            "timestamp": 0.24453308537027776
-        },
-        {
-            "x": 7.152902908699907,
-            "y": 0.95214234591484,
-            "heading": 0.15161661460632753,
-            "angularVelocity": 0.7436086436309897,
-            "velocityX": -2.5866356986709165,
-            "velocityY": 0.8104065474106494,
-            "timestamp": 0.326044113827037
-        },
-        {
-            "x": 6.889352971860368,
-            "y": 1.0347142415767305,
-            "heading": 0.22732297101557483,
-            "angularVelocity": 0.9287866665726976,
-            "velocityX": -3.233304030501219,
-            "velocityY": 1.0130150143510155,
-            "timestamp": 0.4075551422837963
-        },
-        {
-            "x": 6.595109187456524,
-            "y": 1.1269159867790934,
-            "heading": 0.22732298101704992,
-            "angularVelocity": 1.22700882310899e-7,
-            "velocityX": -3.6098647014365244,
-            "velocityY": 1.1311566906713055,
-            "timestamp": 0.4890661707405556
-        },
-        {
-            "x": 6.300865420236581,
-            "y": 1.219117786820698,
-            "heading": 0.2273229910184091,
-            "angularVelocity": 1.2269945998464997e-7,
-            "velocityX": -3.6098644906196586,
-            "velocityY": 1.131157363454394,
-            "timestamp": 0.5705771991973149
-        },
-        {
-            "x": 6.006621672014901,
-            "y": 1.3113196474914972,
-            "heading": 0.2273230010197575,
-            "angularVelocity": 1.2269932759504928e-7,
-            "velocityX": -3.6098642575436695,
-            "velocityY": 1.1311581072702415,
-            "timestamp": 0.6520882276540741
-        },
-        {
-            "x": 5.7123779465456614,
-            "y": 1.4035215807721424,
-            "heading": 0.22732301102110228,
-            "angularVelocity": 1.2269928353992546e-7,
-            "velocityX": -3.6098639784103828,
-            "velocityY": 1.131158998068056,
-            "timestamp": 0.7335992561108334
-        },
-        {
-            "x": 5.418134249112936,
-            "y": 1.4957236035255936,
-            "heading": 0.2273230210224565,
-            "angularVelocity": 1.2269939906393907e-7,
-            "velocityX": -3.60986363445063,
-            "velocityY": 1.1311600957453605,
-            "timestamp": 0.8151102845675927
-        },
-        {
-            "x": 5.123890658648026,
-            "y": 1.5879259676440571,
-            "heading": 0.22732303102385681,
-            "angularVelocity": 1.2269996504792678e-7,
-            "velocityX": -3.609862322139673,
-            "velocityY": 1.1311642837064038,
-            "timestamp": 0.896621313024352
-        },
-        {
-            "x": 4.838467298400658,
-            "y": 1.7046053225590203,
-            "heading": 0.22732304878104165,
-            "angularVelocity": 2.1785008908970575e-7,
-            "velocityX": -3.5016532811726435,
-            "velocityY": 1.4314548242617389,
-            "timestamp": 0.9781323414811113
-        },
-        {
-            "x": 4.5530441892872355,
-            "y": 1.8212852918009321,
-            "heading": 0.2273230665381978,
-            "angularVelocity": 2.1784973753126392e-7,
-            "velocityX": -3.5016502001914627,
-            "velocityY": 1.4314623609958343,
-            "timestamp": 1.0596433699378704
-        },
-        {
-            "x": 4.267621124217833,
-            "y": 1.9379653687834795,
-            "heading": 0.2273230842953322,
-            "angularVelocity": 2.1784947052459537e-7,
-            "velocityX": -3.5016496598471556,
-            "velocityY": 1.4314636827879597,
-            "timestamp": 1.1411543983946297
-        },
-        {
-            "x": 3.9821980933555543,
-            "y": 2.054645529443536,
-            "heading": 0.22732310205245027,
-            "angularVelocity": 2.1784927045604472e-7,
-            "velocityX": -3.5016492401846184,
-            "velocityY": 1.4314647093669521,
-            "timestamp": 1.222665426851389
-        },
-        {
-            "x": 3.6967750938784922,
-            "y": 2.171325766878081,
-            "heading": 0.22732311980956,
-            "angularVelocity": 2.1784916783717796e-7,
-            "velocityX": -3.501648855142053,
-            "velocityY": 1.4314656512577588,
-            "timestamp": 1.3041764553081483
-        },
-        {
-            "x": 3.411352191599502,
-            "y": 2.288006242078097,
-            "heading": 0.2273231375666713,
-            "angularVelocity": 2.178491871140625e-7,
-            "velocityX": -3.501647662689034,
-            "velocityY": 1.4314685682307748,
-            "timestamp": 1.3856874837649076
-        },
-        {
-            "x": 3.135544385824121,
-            "y": 2.425885248705818,
-            "heading": 0.22732315564878777,
-            "angularVelocity": 2.218364413735132e-7,
-            "velocityX": -3.3836869807340535,
-            "velocityY": 1.6915380561155315,
-            "timestamp": 1.4671985122216669
-        },
-        {
-            "x": 2.8888590335845947,
-            "y": 2.6108903884887695,
-            "heading": 0.22732317584620773,
-            "angularVelocity": 2.4778757367040615e-7,
-            "velocityX": -3.026404609412922,
-            "velocityY": 2.2696945834893776,
-            "timestamp": 1.5487095406784261
-        },
-        {
-            "x": 2.714424659127437,
-            "y": 2.7861164379371415,
-            "heading": 0.22732319539367835,
-            "angularVelocity": 2.990800632496739e-7,
-            "velocityX": -2.668879509458687,
-            "velocityY": 2.6809922892290525,
-            "timestamp": 1.6140681954393303
-        },
-        {
-            "x": 2.566977650329029,
-            "y": 2.984587845024715,
-            "heading": 0.2273232142228265,
-            "angularVelocity": 2.880895914276183e-7,
-            "velocityX": -2.2559676195571057,
-            "velocityY": 3.0366507360596735,
-            "timestamp": 1.6794268502002345
-        },
-        {
-            "x": 2.4495779823740462,
-            "y": 3.20218572489824,
-            "heading": 0.22732323403642601,
-            "angularVelocity": 3.0315188694924184e-7,
-            "velocityX": -1.7962375202558922,
-            "velocityY": 3.3292894517113587,
-            "timestamp": 1.7447855049611387
-        },
-        {
-            "x": 2.3646621591221844,
-            "y": 3.4343941523127826,
-            "heading": 0.22732276450933128,
-            "angularVelocity": -0.000007183855179950579,
-            "velocityX": -1.2992284428512828,
-            "velocityY": 3.5528336417572115,
-            "timestamp": 1.810144159722043
-        },
-        {
-            "x": 2.3096843159184894,
-            "y": 3.656196634862642,
-            "heading": 0.16843360905648927,
-            "angularVelocity": -0.9010154151467895,
-            "velocityX": -0.8411715847703609,
-            "velocityY": 3.3936206820850954,
-            "timestamp": 1.875502814482947
-        },
-        {
-            "x": 2.277656929328602,
-            "y": 3.853541817964372,
-            "heading": 0.0672675417750508,
-            "angularVelocity": -1.547860304829062,
-            "velocityX": -0.49002518039951914,
-            "velocityY": 3.0194192922676324,
-            "timestamp": 1.9408614692438513
-        },
-        {
-            "x": 2.265357919525011,
-            "y": 4.021569174077037,
-            "heading": -0.049806740829473055,
-            "angularVelocity": -1.791259061753831,
-            "velocityX": -0.188177217670429,
-            "velocityY": 2.570850895376378,
-            "timestamp": 2.0062201240047557
-        },
-        {
-            "x": 2.269649055536998,
-            "y": 4.158080976089645,
-            "heading": -0.1685192187929243,
-            "angularVelocity": -1.8163237661135343,
-            "velocityX": 0.06565520706762865,
-            "velocityY": 2.088656850603768,
-            "timestamp": 2.07157877876566
-        },
-        {
-            "x": 2.2810300353091644,
-            "y": 4.259951613943982,
-            "heading": -0.2719091294332062,
-            "angularVelocity": -1.581885536330179,
-            "velocityX": 0.1741311814602323,
-            "velocityY": 1.5586403702310236,
-            "timestamp": 2.1369374335265645
-        },
-        {
-            "x": 2.2926691286250658,
-            "y": 4.326989538481201,
-            "heading": -0.34849979264864867,
-            "angularVelocity": -1.1718518916219969,
-            "velocityX": 0.1780803683686499,
-            "velocityY": 1.0256931508529106,
-            "timestamp": 2.202296088287469
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.6349611616573703,
-            "velocityX": 0.11216374329844404,
-            "velocityY": 0.5050664160631704,
-            "timestamp": 2.2676547430483733
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -1.761626207104486e-30,
-            "velocityX": 0,
-            "velocityY": -1.839087679439853e-31,
-            "timestamp": 2.3330133978092777
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.1.traj
deleted file mode 100644
index 1fe0cdb8..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.1.traj	
+++ /dev/null
@@ -1,220 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -2.1086675817598662e-32,
-            "velocityY": -7.052966104933725e-38,
-            "timestamp": 0
-        },
-        {
-            "x": 1.4233949672121562,
-            "y": 5.582111248416047,
-            "heading": 6.989955400155978e-19,
-            "angularVelocity": 7.290266672073875e-18,
-            "velocityX": 0.7654825584964474,
-            "velocityY": 0.23061220206204183,
-            "timestamp": 0.09588065253415788
-        },
-        {
-            "x": 1.5701848986828006,
-            "y": 5.626333744358309,
-            "heading": -5.64955528358279e-19,
-            "angularVelocity": -1.3182545571253806e-17,
-            "velocityX": 1.530965086187225,
-            "velocityY": 0.4612243948434498,
-            "timestamp": 0.19176130506831576
-        },
-        {
-            "x": 1.7903697885046006,
-            "y": 5.69266748604712,
-            "heading": -1.1328157855510893e-18,
-            "angularVelocity": -5.922574643212496e-18,
-            "velocityX": 2.2964475522666925,
-            "velocityY": 0.6918365690635988,
-            "timestamp": 0.28764195760247363
-        },
-        {
-            "x": 2.08394961895559,
-            "y": 5.781112468143492,
-            "heading": -1.2347885191323161e-17,
-            "angularVelocity": -1.1696905584501664e-16,
-            "velocityX": 3.061929833512548,
-            "velocityY": 0.9224486876000673,
-            "timestamp": 0.3835226101366315
-        },
-        {
-            "x": 2.4312425305333245,
-            "y": 5.885739258408696,
-            "heading": -1.8571615119787707e-17,
-            "angularVelocity": -6.491121789400923e-17,
-            "velocityX": 3.6221375470302584,
-            "velocityY": 1.0912190050847865,
-            "timestamp": 0.4794032626707894
-        },
-        {
-            "x": 2.7785354421111035,
-            "y": 5.990366048673914,
-            "heading": -2.5105020741934312e-17,
-            "angularVelocity": -6.814102166319416e-17,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849244,
-            "timestamp": 0.5752839152049473
-        },
-        {
-            "x": 3.125828353688883,
-            "y": 6.094992838939131,
-            "heading": -3.974939977960392e-17,
-            "angularVelocity": -1.527354961675144e-16,
-            "velocityX": 3.622137547030716,
-            "velocityY": 1.0912190050849244,
-            "timestamp": 0.6711645677391052
-        },
-        {
-            "x": 3.473121265266662,
-            "y": 6.199619629204348,
-            "heading": -4.6160884572564577e-17,
-            "angularVelocity": -6.68694321901595e-17,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849242,
-            "timestamp": 0.767045220273263
-        },
-        {
-            "x": 3.8204141768444413,
-            "y": 6.304246419469565,
-            "heading": -5.207468497060529e-17,
-            "angularVelocity": -6.167876669314405e-17,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849244,
-            "timestamp": 0.862925872807421
-        },
-        {
-            "x": 4.16770708842222,
-            "y": 6.408873209734782,
-            "heading": -3.0597254964935646e-17,
-            "angularVelocity": 2.24001708769928e-16,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849242,
-            "timestamp": 0.9588065253415788
-        },
-        {
-            "x": 4.515,
-            "y": 6.5135,
-            "heading": -2.0783137065340813e-18,
-            "angularVelocity": 2.974420855995074e-16,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849244,
-            "timestamp": 1.0546871778757367
-        },
-        {
-            "x": 4.862292911577779,
-            "y": 6.618126790265217,
-            "heading": -2.7789619178473824e-17,
-            "angularVelocity": -2.681594752696605e-16,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849244,
-            "timestamp": 1.1505678304098945
-        },
-        {
-            "x": 5.209585823155558,
-            "y": 6.722753580530434,
-            "heading": 2.2712942266126744e-18,
-            "angularVelocity": 3.135242889004865e-16,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849242,
-            "timestamp": 1.2464484829440523
-        },
-        {
-            "x": 5.5568787347333375,
-            "y": 6.827380370795651,
-            "heading": 3.7041174502192387e-17,
-            "angularVelocity": 3.6263708429820527e-16,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849244,
-            "timestamp": 1.34232913547821
-        },
-        {
-            "x": 5.9041716463111165,
-            "y": 6.932007161060868,
-            "heading": 6.494346998398645e-17,
-            "angularVelocity": 2.910106965725311e-16,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849244,
-            "timestamp": 1.4382097880123679
-        },
-        {
-            "x": 6.251464557888895,
-            "y": 7.036633951326086,
-            "heading": 5.759102638783417e-17,
-            "angularVelocity": -7.668328700140211e-17,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849244,
-            "timestamp": 1.5340904405465257
-        },
-        {
-            "x": 6.598757469466675,
-            "y": 7.141260741591303,
-            "heading": 2.5440135069531877e-17,
-            "angularVelocity": -3.353219911269207e-16,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849244,
-            "timestamp": 1.6299710930806834
-        },
-        {
-            "x": 6.94605038104441,
-            "y": 7.245887531856507,
-            "heading": 2.2711688502269004e-17,
-            "angularVelocity": -2.84566958514003e-17,
-            "velocityX": 3.6221375470302584,
-            "velocityY": 1.0912190050847868,
-            "timestamp": 1.7258517456148412
-        },
-        {
-            "x": 7.239630211495399,
-            "y": 7.334332513952879,
-            "heading": 1.1340524791180054e-17,
-            "angularVelocity": -1.1859706178712186e-16,
-            "velocityX": 3.061929833512548,
-            "velocityY": 0.9224486876000674,
-            "timestamp": 1.821732398148999
-        },
-        {
-            "x": 7.4598151013172,
-            "y": 7.40066625564169,
-            "heading": 5.896173994762203e-18,
-            "angularVelocity": -5.67825796795303e-17,
-            "velocityX": 2.2964475522666925,
-            "velocityY": 0.6918365690635988,
-            "timestamp": 1.9176130506831568
-        },
-        {
-            "x": 7.606605032787844,
-            "y": 7.444888751583952,
-            "heading": 1.9652682638036787e-18,
-            "angularVelocity": -4.099790316709085e-17,
-            "velocityX": 1.530965086187225,
-            "velocityY": 0.46122439484344985,
-            "timestamp": 2.0134937032173146
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": -2.0497026255693862e-17,
-            "velocityX": 0.7654825584964475,
-            "velocityY": 0.23061220206204183,
-            "timestamp": 2.1093743557514726
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 2.8084467268312453e-34,
-            "velocityY": 0,
-            "timestamp": 2.2052550082856306
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.1.traj
deleted file mode 100644
index a76ce3f5..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.1.traj	
+++ /dev/null
@@ -1,211 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 4.006684802425094e-34,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 1.4307759237486215,
-            "y": 5.581186236587336,
-            "heading": -0.002104963011526108,
-            "angularVelocity": -0.021033383553861286,
-            "velocityX": 0.8071357913745165,
-            "velocityY": 0.21169884590096585,
-            "timestamp": 0.1000772417873624
-        },
-        {
-            "x": 1.592327768448065,
-            "y": 5.623558708255768,
-            "heading": -0.006314883423264977,
-            "angularVelocity": -0.04206671103789844,
-            "velocityX": 1.6142715547926287,
-            "velocityY": 0.42339767675114676,
-            "timestamp": 0.2001544835747248
-        },
-        {
-            "x": 1.8346555283201378,
-            "y": 5.6871174126709505,
-            "heading": -0.012629752552513055,
-            "angularVelocity": -0.06309995176191506,
-            "velocityX": 2.4214072604734147,
-            "velocityY": 0.6350964842758995,
-            "timestamp": 0.3002317253620872
-        },
-        {
-            "x": 2.1577591856288034,
-            "y": 5.77186234429474,
-            "heading": -0.02104955396970995,
-            "angularVelocity": -0.08413302831713466,
-            "velocityX": 3.228542788930726,
-            "velocityY": 0.8467952364619561,
-            "timestamp": 0.4003089671494496
-        },
-        {
-            "x": 2.5239589562380984,
-            "y": 5.867910684343787,
-            "heading": -0.021049554851793276,
-            "angularVelocity": -8.814025154574047e-9,
-            "velocityX": 3.6591712967806616,
-            "velocityY": 0.9597420785549242,
-            "timestamp": 0.500386208936812
-        },
-        {
-            "x": 2.8901587297210503,
-            "y": 5.963959013436644,
-            "heading": -0.021049555733868682,
-            "angularVelocity": -8.813945991956408e-9,
-            "velocityX": 3.659171325495056,
-            "velocityY": 0.9597419690775807,
-            "timestamp": 0.6004634507241744
-        },
-        {
-            "x": 3.2563585069426413,
-            "y": 6.060007328275347,
-            "heading": -0.02104955661593894,
-            "angularVelocity": -8.813894597525165e-9,
-            "velocityX": 3.659171362852588,
-            "velocityY": 0.9597418266460623,
-            "timestamp": 0.7005406925115368
-        },
-        {
-            "x": 3.622558289188601,
-            "y": 6.15605562395784,
-            "heading": -0.021049557498003604,
-            "angularVelocity": -8.813838677794314e-9,
-            "velocityX": 3.6591714130574977,
-            "velocityY": 0.9597416352318217,
-            "timestamp": 0.8006179342988993
-        },
-        {
-            "x": 3.9887580784892815,
-            "y": 6.252103892743073,
-            "heading": -0.02104955838006166,
-            "angularVelocity": -8.81377261667878e-9,
-            "velocityX": 3.659171483550255,
-            "velocityY": 0.9597413664668089,
-            "timestamp": 0.9006951760862617
-        },
-        {
-            "x": 4.354957878489299,
-            "y": 6.3481521207353335,
-            "heading": -0.021049559262112345,
-            "angularVelocity": -8.813699036774153e-9,
-            "velocityX": 3.6591715904610522,
-            "velocityY": 0.9597409588519333,
-            "timestamp": 1.0007724178736241
-        },
-        {
-            "x": 4.721157716449094,
-            "y": 6.444200203999558,
-            "heading": -0.021049560144156942,
-            "angularVelocity": -8.81363814954091e-9,
-            "velocityX": 3.659171969765827,
-            "velocityY": 0.9597395126886197,
-            "timestamp": 1.1008496596609865
-        },
-        {
-            "x": 5.092065894328832,
-            "y": 6.520059807846628,
-            "heading": -0.02104956519242719,
-            "angularVelocity": -5.0443738888274115e-8,
-            "velocityX": 3.70621902897584,
-            "velocityY": 0.7580105375830706,
-            "timestamp": 1.2009269014483488
-        },
-        {
-            "x": 5.47058189966855,
-            "y": 6.512767035208857,
-            "heading": -0.021049620221549888,
-            "angularVelocity": -5.498665002090286e-7,
-            "velocityX": 3.7822385847120277,
-            "velocityY": -0.07287143917560433,
-            "timestamp": 1.3010041432357111
-        },
-        {
-            "x": 5.849069453983326,
-            "y": 6.50412284194313,
-            "heading": -0.02104967520004647,
-            "angularVelocity": -5.493606298291521e-7,
-            "velocityX": 3.7819542940538056,
-            "velocityY": -0.08637521489745259,
-            "timestamp": 1.4010813850230734
-        },
-        {
-            "x": 6.2213754396920145,
-            "y": 6.435451104158958,
-            "heading": -0.021049699334872447,
-            "angularVelocity": -2.4116198195152956e-7,
-            "velocityX": 3.720186318681132,
-            "velocityY": -0.6861873544644725,
-            "timestamp": 1.5011586268104358
-        },
-        {
-            "x": 6.5695694546238315,
-            "y": 6.286829159405189,
-            "heading": -0.02104969841623417,
-            "angularVelocity": 9.179292491243133e-9,
-            "velocityX": 3.4792527123362977,
-            "velocityY": -1.4850723511100814,
-            "timestamp": 1.601235868597798
-        },
-        {
-            "x": 6.915952468622566,
-            "y": 6.134033995374984,
-            "heading": -0.021049697500180466,
-            "angularVelocity": 9.153466759383175e-9,
-            "velocityX": 3.4611566807037675,
-            "velocityY": -1.5267723340622583,
-            "timestamp": 1.7013131103851604
-        },
-        {
-            "x": 7.221571466882516,
-            "y": 5.999220411113471,
-            "heading": -0.012629808861323527,
-            "angularVelocity": 0.0841338998605394,
-            "velocityX": 3.053831148837094,
-            "velocityY": -1.347095322110863,
-            "timestamp": 1.8013903521725227
-        },
-        {
-            "x": 7.450785730315982,
-            "y": 5.898110210460681,
-            "heading": -0.0063148981970481905,
-            "angularVelocity": 0.0631003667916113,
-            "velocityX": 2.290373508899117,
-            "velocityY": -1.0103216160535444,
-            "timestamp": 1.901467593959885
-        },
-        {
-            "x": 7.603595242863294,
-            "y": 5.830703404850431,
-            "heading": -0.0021049639599730564,
-            "angularVelocity": 0.04206684918455414,
-            "velocityX": 1.5269157084883698,
-            "velocityY": -0.673547795746317,
-            "timestamp": 2.0015448357472474
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0.02103339303101045,
-            "velocityX": 0.7634578628680123,
-            "velocityY": -0.33677391831044023,
-            "timestamp": 2.1016220775346097
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 1.7450911582816108e-33,
-            "velocityY": 0,
-            "timestamp": 2.201699319321972
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.1.traj
deleted file mode 100644
index 4db64f81..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.1.traj	
+++ /dev/null
@@ -1,211 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 2.6743025901632004e-33,
-            "velocityX": 2.068786142168461e-33,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 1.4007220754094205,
-            "y": 5.545584664540576,
-            "heading": -0.37232774466736046,
-            "angularVelocity": -3.437582019103784,
-            "velocityX": 0.4683005682394807,
-            "velocityY": -0.13309214444637307,
-            "timestamp": 0.10831094141120465
-        },
-        {
-            "x": 1.5327694511201624,
-            "y": 5.533363500300387,
-            "heading": -0.9362708794209632,
-            "angularVelocity": -5.206705134364786,
-            "velocityX": 1.2191508446909478,
-            "velocityY": -0.11283406903270367,
-            "timestamp": 0.2166218828224093
-        },
-        {
-            "x": 1.760106911270163,
-            "y": 5.516367699163042,
-            "heading": -1.4218044496098983,
-            "angularVelocity": -4.482774905866594,
-            "velocityX": 2.0989334705060814,
-            "velocityY": -0.1569167520465022,
-            "timestamp": 0.3249328242336139
-        },
-        {
-            "x": 2.067387230758427,
-            "y": 5.467280597023701,
-            "heading": -1.7292049865361503,
-            "angularVelocity": -2.838130044121764,
-            "velocityX": 2.83702011527781,
-            "velocityY": -0.453205387191498,
-            "timestamp": 0.4332437656448186
-        },
-        {
-            "x": 2.4488593918735733,
-            "y": 5.393167697716653,
-            "heading": -1.7983473850598053,
-            "angularVelocity": -0.6383694723984937,
-            "velocityX": 3.5220094677866336,
-            "velocityY": -0.6842605035226905,
-            "timestamp": 0.5415547070560233
-        },
-        {
-            "x": 2.8531804118778052,
-            "y": 5.326790341993422,
-            "heading": -1.7983490370676205,
-            "angularVelocity": -0.000015252455509683049,
-            "velocityX": 3.7329656148885193,
-            "velocityY": -0.6128407237384188,
-            "timestamp": 0.6498656484672279
-        },
-        {
-            "x": 3.257501404598323,
-            "y": 5.2604128200678275,
-            "heading": -1.7983506890680847,
-            "angularVelocity": -0.00001525238764202395,
-            "velocityX": 3.732965362986781,
-            "velocityY": -0.6128422582312392,
-            "timestamp": 0.7581765898784326
-        },
-        {
-            "x": 3.6466232677858144,
-            "y": 5.172310745117625,
-            "heading": -1.8337945854134767,
-            "angularVelocity": -0.32724206699329256,
-            "velocityX": 3.5926367005728674,
-            "velocityY": -0.8134180517896293,
-            "timestamp": 0.8664875312896373
-        },
-        {
-            "x": 4.014238623942389,
-            "y": 5.062846847355338,
-            "heading": -1.917513965365452,
-            "angularVelocity": -0.7729540419571551,
-            "velocityX": 3.394074055370967,
-            "velocityY": -1.0106448742487064,
-            "timestamp": 0.974798472700842
-        },
-        {
-            "x": 4.354761888880782,
-            "y": 4.880183720994759,
-            "heading": -1.9914398534549198,
-            "angularVelocity": -0.6825338892476871,
-            "velocityX": 3.143941512294591,
-            "velocityY": -1.6864697507068689,
-            "timestamp": 1.0831094141120465
-        },
-        {
-            "x": 4.661144802240214,
-            "y": 4.608133464126214,
-            "heading": -1.9914398486412812,
-            "angularVelocity": 4.444277523534959e-8,
-            "velocityX": 2.8287346538355926,
-            "velocityY": -2.5117523061285136,
-            "timestamp": 1.1914203555232512
-        },
-        {
-            "x": 5.019380877625839,
-            "y": 4.418922250546549,
-            "heading": -1.9767149939198712,
-            "angularVelocity": 0.13594983599585594,
-            "velocityX": 3.3074781801182462,
-            "velocityY": -1.7469261287400477,
-            "timestamp": 1.2997312969344559
-        },
-        {
-            "x": 5.376950498025801,
-            "y": 4.3069722509438675,
-            "heading": -1.8614424797513431,
-            "angularVelocity": 1.0642739566900585,
-            "velocityX": 3.3013250161167162,
-            "velocityY": -1.0335982509621156,
-            "timestamp": 1.4080422383456606
-        },
-        {
-            "x": 5.749629435463048,
-            "y": 4.240661073830716,
-            "heading": -1.7582912287295662,
-            "angularVelocity": 0.9523622422425545,
-            "velocityX": 3.4408244687151703,
-            "velocityY": -0.6122297179691145,
-            "timestamp": 1.5163531797568652
-        },
-        {
-            "x": 6.158173088993434,
-            "y": 4.209455444526307,
-            "heading": -1.7582906674190417,
-            "angularVelocity": 0.000005182399092709644,
-            "velocityX": 3.771951828756997,
-            "velocityY": -0.2881115139230142,
-            "timestamp": 1.62466412116807
-        },
-        {
-            "x": 6.566716775041419,
-            "y": 4.178250240929697,
-            "heading": -1.7582901061123783,
-            "angularVelocity": 0.000005182363444203767,
-            "velocityX": 3.771952128981517,
-            "velocityY": -0.2881075835001592,
-            "timestamp": 1.7329750625792746
-        },
-        {
-            "x": 6.948239527399369,
-            "y": 4.140315578246664,
-            "heading": -1.6699172621402418,
-            "angularVelocity": 0.8159179748667066,
-            "velocityX": 3.5224765604196056,
-            "velocityY": -0.35023850950582514,
-            "timestamp": 1.8412860039904793
-        },
-        {
-            "x": 7.2588942750640015,
-            "y": 4.14084991195257,
-            "heading": -1.390296709754989,
-            "angularVelocity": 2.5816464037891405,
-            "velocityX": 2.86817512263351,
-            "velocityY": 0.004933330824608595,
-            "timestamp": 1.949596945401684
-        },
-        {
-            "x": 7.4846066646178775,
-            "y": 4.133171401549216,
-            "heading": -0.9384850139510863,
-            "angularVelocity": 4.17143171241205,
-            "velocityX": 2.083929717653862,
-            "velocityY": -0.07089321081793584,
-            "timestamp": 2.0579078868128886
-        },
-        {
-            "x": 7.620450068903259,
-            "y": 4.128683952785512,
-            "heading": -0.34333254233125515,
-            "angularVelocity": 5.494850879010672,
-            "velocityX": 1.2541983525897882,
-            "velocityY": -0.04143116757398245,
-            "timestamp": 2.166218828224093
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 9.09513036114211e-34,
-            "angularVelocity": 3.1698786646843584,
-            "velocityX": 0.5498053134877472,
-            "velocityY": -0.015547393121808115,
-            "timestamp": 2.2745297696352975
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -7.032634787663221e-33,
-            "velocityX": 0,
-            "velocityY": -1.950631326224301e-32,
-            "timestamp": 2.382840711046502
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.1.traj
deleted file mode 100644
index 1f520266..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.1.traj	
+++ /dev/null
@@ -1,1093 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": -1.9296664556128306e-33,
-            "angularVelocity": 0,
-            "velocityX": -7.813211441980168e-34,
-            "velocityY": -2.2407271587906947e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 1.35439418502164,
-            "y": 5.559271848784049,
-            "heading": -0.0037613483747314606,
-            "angularVelocity": -0.16062668985124567,
-            "velocityX": 0.18765169410035007,
-            "velocityY": -0.031095370031394967,
-            "timestamp": 0.023416708507251784
-        },
-        {
-            "x": 1.3631871179840578,
-            "y": 5.557813607446842,
-            "heading": -0.011205273313316288,
-            "angularVelocity": -0.317889464963847,
-            "velocityX": 0.37549824560930783,
-            "velocityY": -0.06227354014150002,
-            "timestamp": 0.04683341701450357
-        },
-        {
-            "x": 1.3763837173940232,
-            "y": 5.5556231238892515,
-            "heading": -0.02224504855076525,
-            "angularVelocity": -0.47144863395424863,
-            "velocityX": 0.563554839736692,
-            "velocityY": -0.09354361467639355,
-            "timestamp": 0.07025012552175536
-        },
-        {
-            "x": 1.3939893513502628,
-            "y": 5.552698044632534,
-            "heading": -0.03678367986427699,
-            "angularVelocity": -0.6208657083043512,
-            "velocityX": 0.7518406761047259,
-            "velocityY": -0.12491419346204678,
-            "timestamp": 0.09366683402900713
-        },
-        {
-            "x": 1.4160099408355828,
-            "y": 5.549035829570074,
-            "heading": -0.0547111148835232,
-            "angularVelocity": -0.7655830456997818,
-            "velocityX": 0.9403793653792271,
-            "velocityY": -0.15639324635770782,
-            "timestamp": 0.11708354253625891
-        },
-        {
-            "x": 1.4424520698548373,
-            "y": 5.544633771060564,
-            "heading": -0.07590088077413953,
-            "angularVelocity": -0.9048994176125892,
-            "velocityX": 1.129199221618411,
-            "velocityY": -0.18798792785714832,
-            "timestamp": 0.1405002510435107
-        },
-        {
-            "x": 1.4733230980825243,
-            "y": 5.539489016814175,
-            "heading": -0.10020600379509263,
-            "angularVelocity": -1.0379393420482537,
-            "velocityX": 1.3183333694454236,
-            "velocityY": -0.2197044151102235,
-            "timestamp": 0.16391695955076246
-        },
-        {
-            "x": 1.5086312686123933,
-            "y": 5.533598592872994,
-            "heading": -0.12745397379473564,
-            "angularVelocity": -1.163612298082128,
-            "velocityX": 1.5078195348818708,
-            "velocityY": -0.25154790389774145,
-            "timestamp": 0.18733366805801424
-        },
-        {
-            "x": 1.5483857981367395,
-            "y": 5.526959418437362,
-            "heading": -0.15744034724886685,
-            "angularVelocity": -1.2805545854082991,
-            "velocityX": 1.6976992950155392,
-            "velocityY": -0.28352295684851286,
-            "timestamp": 0.21075037656526602
-        },
-        {
-            "x": 1.5925969268012463,
-            "y": 5.5195682985391485,
-            "heading": -0.18992026053497643,
-            "angularVelocity": -1.3870400819163256,
-            "velocityX": 1.888016355962865,
-            "velocityY": -0.31563444947560276,
-            "timestamp": 0.2341670850725178
-        },
-        {
-            "x": 1.6412758830317984,
-            "y": 5.511421875068327,
-            "heading": -0.22459650029504458,
-            "angularVelocity": -1.4808332157070316,
-            "velocityX": 2.0788129217851563,
-            "velocityY": -0.34788934868040844,
-            "timestamp": 0.25758379357976957
-        },
-        {
-            "x": 1.6944346649736404,
-            "y": 5.502516516620167,
-            "heading": -0.26110152083126115,
-            "angularVelocity": -1.5589304758570732,
-            "velocityX": 2.2701218630013136,
-            "velocityY": -0.38029932539011047,
-            "timestamp": 0.2810005020870214
-        },
-        {
-            "x": 1.7520853949904163,
-            "y": 5.492848149517658,
-            "heading": -0.298968117565775,
-            "angularVelocity": -1.6170759747377448,
-            "velocityX": 2.4619484843022064,
-            "velocityY": -0.4128832666435571,
-            "timestamp": 0.3044172105942732
-        },
-        {
-            "x": 1.8142385663960479,
-            "y": 5.482412126535505,
-            "heading": -0.337577206769387,
-            "angularVelocity": -1.6487837815317001,
-            "velocityX": 2.654223217852448,
-            "velocityY": -0.44566566556215986,
-            "timestamp": 0.327833919101525
-        },
-        {
-            "x": 1.8808979849725118,
-            "y": 5.471203531590798,
-            "heading": -0.3760544549024592,
-            "angularVelocity": -1.6431535679387552,
-            "velocityX": 2.8466604756095246,
-            "velocityY": -0.47865800358925525,
-            "timestamp": 0.3512506276087768
-        },
-        {
-            "x": 1.9520436894250033,
-            "y": 5.459218834920382,
-            "heading": -0.41303307424029884,
-            "angularVelocity": -1.5791552995754587,
-            "velocityX": 3.0382452952539354,
-            "velocityY": -0.5118010785637118,
-            "timestamp": 0.3746673361160286
-        },
-        {
-            "x": 2.027554046757936,
-            "y": 5.446452385802885,
-            "heading": -0.44595627669268334,
-            "angularVelocity": -1.4059705462956014,
-            "velocityX": 3.2246358325529645,
-            "velocityY": -0.5451854650516427,
-            "timestamp": 0.3980840446232804
-        },
-        {
-            "x": 2.106704107988529,
-            "y": 5.4327632732947455,
-            "heading": -0.46854805770485686,
-            "angularVelocity": -0.9647718425148408,
-            "velocityX": 3.3800677497471567,
-            "velocityY": -0.5845873899786794,
-            "timestamp": 0.4215007531305322
-        },
-        {
-            "x": 2.1895060473348913,
-            "y": 5.418170740153337,
-            "heading": -0.4808901513114338,
-            "angularVelocity": -0.5270635539044761,
-            "velocityX": 3.5360195614476866,
-            "velocityY": -0.6231675616108724,
-            "timestamp": 0.444917461637784
-        },
-        {
-            "x": 2.275961949822614,
-            "y": 5.402702256569038,
-            "heading": -0.4829709400280361,
-            "angularVelocity": -0.0888591458512548,
-            "velocityX": 3.6920604132278863,
-            "velocityY": -0.6605746311236733,
-            "timestamp": 0.4683341701450358
-        },
-        {
-            "x": 2.3633379318433296,
-            "y": 5.388122823546896,
-            "heading": -0.48297098226531077,
-            "angularVelocity": -0.0000018037238101032338,
-            "velocityX": 3.7313519956767345,
-            "velocityY": -0.6226081269122786,
-            "timestamp": 0.4917508786522876
-        },
-        {
-            "x": 2.4507217300981603,
-            "y": 5.373590311340196,
-            "heading": -0.48297102439627604,
-            "angularVelocity": -0.0000017991839149984119,
-            "velocityX": 3.7316857844376363,
-            "velocityY": -0.6206043946012527,
-            "timestamp": 0.5151675871595394
-        },
-        {
-            "x": 2.5381084152434026,
-            "y": 5.359075168632344,
-            "heading": -0.4829710665257657,
-            "angularVelocity": -0.0000017991208983915868,
-            "velocityX": 3.7318090677936033,
-            "velocityY": -0.6198626379688947,
-            "timestamp": 0.5385842956667911
-        },
-        {
-            "x": 2.6254950830921073,
-            "y": 5.344559921791865,
-            "heading": -0.482971108655264,
-            "angularVelocity": -0.0000017991212673817243,
-            "velocityX": 3.7318083291527238,
-            "velocityY": -0.6198670849061145,
-            "timestamp": 0.5620010041740429
-        },
-        {
-            "x": 2.712881673840345,
-            "y": 5.330044210788612,
-            "heading": -0.4829711507848006,
-            "angularVelocity": -0.0000017991229009417428,
-            "velocityX": 3.731805036611997,
-            "velocityY": -0.6198869067684731,
-            "timestamp": 0.5854177126812946
-        },
-        {
-            "x": 2.800268245782709,
-            "y": 5.315528386572539,
-            "heading": -0.4829711929143463,
-            "angularVelocity": -0.0000017991232935293638,
-            "velocityX": 3.7318042335155903,
-            "velocityY": -0.6198917414709255,
-            "timestamp": 0.6088344211885464
-        },
-        {
-            "x": 2.8876548130761686,
-            "y": 5.301012534370082,
-            "heading": -0.4829712350438941,
-            "angularVelocity": -0.0000017991233825733017,
-            "velocityX": 3.731804034986268,
-            "velocityY": -0.6198929366167053,
-            "timestamp": 0.6322511296957981
-        },
-        {
-            "x": 2.975041345179212,
-            "y": 5.286496470320027,
-            "heading": -0.48297127717345933,
-            "angularVelocity": -0.000001799124124149535,
-            "velocityX": 3.7318025321954105,
-            "velocityY": -0.6199019834729332,
-            "timestamp": 0.6556678382030499
-        },
-        {
-            "x": 3.0624277772479402,
-            "y": 5.271979804077107,
-            "heading": -0.4829713193030743,
-            "angularVelocity": -0.0000017991262516148556,
-            "velocityX": 3.7317982602749975,
-            "velocityY": -0.6199276998481766,
-            "timestamp": 0.6790845467103016
-        },
-        {
-            "x": 3.149814115388955,
-            "y": 5.257462572426413,
-            "heading": -0.482971361432736,
-            "angularVelocity": -0.0000017991282469783394,
-            "velocityX": 3.731794249134225,
-            "velocityY": -0.619951845332987,
-            "timestamp": 0.7025012552175534
-        },
-        {
-            "x": 3.2372009926614522,
-            "y": 5.242948586436876,
-            "heading": -0.48297140356212753,
-            "angularVelocity": -0.0000017991167072996656,
-            "velocityX": 3.7318172725014778,
-            "velocityY": -0.6198132408343549,
-            "timestamp": 0.7259179637248051
-        },
-        {
-            "x": 3.3245908005140246,
-            "y": 5.228452256133779,
-            "heading": -0.4829714456900571,
-            "angularVelocity": -0.0000017990542795775989,
-            "velocityX": 3.7319424216049217,
-            "velocityY": -0.6190592626894866,
-            "timestamp": 0.7493346722320569
-        },
-        {
-            "x": 3.4119833565424647,
-            "y": 5.213972502683899,
-            "heading": -0.48297148781659605,
-            "angularVelocity": -0.0000017989948869691892,
-            "velocityX": 3.7320597812190472,
-            "velocityY": -0.6183513556300411,
-            "timestamp": 0.7727513807393086
-        },
-        {
-            "x": 3.4993677183765524,
-            "y": 5.199443379550061,
-            "heading": -0.48297152994719206,
-            "angularVelocity": -0.0000017991681461203675,
-            "velocityX": 3.731709851836638,
-            "velocityY": -0.6204596657670539,
-            "timestamp": 0.7961680892465604
-        },
-        {
-            "x": 3.5867330233447126,
-            "y": 5.184800101210533,
-            "heading": -0.48297157208742414,
-            "angularVelocity": -0.0000017995796480013664,
-            "velocityX": 3.7308960369522555,
-            "velocityY": -0.6253346124593939,
-            "timestamp": 0.8195847977538121
-        },
-        {
-            "x": 3.6740864894013328,
-            "y": 5.170086363489935,
-            "heading": -0.48297161423379187,
-            "angularVelocity": -0.0000017998416699478457,
-            "velocityX": 3.7303904615618975,
-            "velocityY": -0.6283435486246598,
-            "timestamp": 0.8430015062610638
-        },
-        {
-            "x": 3.7614250146254347,
-            "y": 5.155284197469098,
-            "heading": -0.482971656398251,
-            "angularVelocity": -0.0000018006142553635733,
-            "velocityX": 3.729752420031712,
-            "velocityY": -0.6321198393988197,
-            "timestamp": 0.8664182147683156
-        },
-        {
-            "x": 3.848175799548237,
-            "y": 5.137355912505346,
-            "heading": -0.48297169899264486,
-            "angularVelocity": -0.000001818974422860047,
-            "velocityX": 3.704653234929829,
-            "velocityY": -0.7656193421980864,
-            "timestamp": 0.8898349232755673
-        },
-        {
-            "x": 3.9339293985898425,
-            "y": 5.115142351916783,
-            "heading": -0.48297174253890224,
-            "angularVelocity": -0.0000018596233270255789,
-            "velocityX": 3.6620688605764276,
-            "velocityY": -0.9486201095127581,
-            "timestamp": 0.9132516317828191
-        },
-        {
-            "x": 4.018431663513184,
-            "y": 5.088562488555908,
-            "heading": -0.4829717875114574,
-            "angularVelocity": -0.000001920532733427299,
-            "velocityX": 3.6086312001179417,
-            "velocityY": -1.135081104701801,
-            "timestamp": 0.9366683402900708
-        },
-        {
-            "x": 4.095489617333613,
-            "y": 5.060208566496142,
-            "heading": -0.48297174072333765,
-            "angularVelocity": 0.00000215563208981735,
-            "velocityX": 3.550230245224857,
-            "velocityY": -1.306327856847248,
-            "timestamp": 0.9583733984124503
-        },
-        {
-            "x": 4.1711135466589555,
-            "y": 5.028225843813575,
-            "heading": -0.482971694875483,
-            "angularVelocity": 0.0000021123119922022846,
-            "velocityX": 3.4841615672696387,
-            "velocityY": -1.4735147218783384,
-            "timestamp": 0.9800784565348297
-        },
-        {
-            "x": 4.245175973739862,
-            "y": 4.9927768637698655,
-            "heading": -0.48297164958118555,
-            "angularVelocity": 0.0000020868083942836762,
-            "velocityX": 3.4122197076519116,
-            "velocityY": -1.633212859593547,
-            "timestamp": 1.0017835146572092
-        },
-        {
-            "x": 4.317835043195099,
-            "y": 4.954533786792328,
-            "heading": -0.4829716044969262,
-            "angularVelocity": 0.0000020771314744606885,
-            "velocityX": 3.3475639201504173,
-            "velocityY": -1.7619430808199938,
-            "timestamp": 1.0234885727795886
-        },
-        {
-            "x": 4.39018173013391,
-            "y": 4.915702997439957,
-            "heading": -0.48297155943721365,
-            "angularVelocity": 0.000002076000546478681,
-            "velocityX": 3.333171767193961,
-            "velocityY": -1.789020288884605,
-            "timestamp": 1.045193630901968
-        },
-        {
-            "x": 4.462524157787397,
-            "y": 4.876864273483723,
-            "heading": -0.4829715143782692,
-            "angularVelocity": 0.000002075965162802355,
-            "velocityX": 3.3329755325063237,
-            "velocityY": -1.7893858536194671,
-            "timestamp": 1.0668986890243475
-        },
-        {
-            "x": 4.534839835738717,
-            "y": 4.8379757656949245,
-            "heading": -0.48297146931947993,
-            "angularVelocity": 0.0000020759580074306272,
-            "velocityX": 3.331743114605303,
-            "velocityY": -1.7916795048180625,
-            "timestamp": 1.088603747146727
-        },
-        {
-            "x": 4.60713879668486,
-            "y": 4.7990561876016065,
-            "heading": -0.48297142426091105,
-            "angularVelocity": 0.000002075947856864141,
-            "velocityX": 3.3309729252282607,
-            "velocityY": -1.793110982419974,
-            "timestamp": 1.1103088052691064
-        },
-        {
-            "x": 4.679445820973998,
-            "y": 4.760151592099993,
-            "heading": -0.48297137920221006,
-            "angularVelocity": 0.0000020759539417382525,
-            "velocityX": 3.331344421260875,
-            "velocityY": -1.7924207013070022,
-            "timestamp": 1.1320138633914858
-        },
-        {
-            "x": 4.75175835900604,
-            "y": 4.72125724602898,
-            "heading": -0.4829713341434503,
-            "angularVelocity": 0.000002075956651977989,
-            "velocityX": 3.3315984515826855,
-            "velocityY": -1.791948487386363,
-            "timestamp": 1.1537189215138652
-        },
-        {
-            "x": 4.824072564767644,
-            "y": 4.68236600077068,
-            "heading": -0.48297128908467135,
-            "angularVelocity": 0.0000020759575350386475,
-            "velocityX": 3.331675287569862,
-            "velocityY": -1.7918056260909452,
-            "timestamp": 1.1754239796362447
-        },
-        {
-            "x": 4.8963869623773055,
-            "y": 4.643475112237639,
-            "heading": -0.48297124402589003,
-            "angularVelocity": 0.000002075957644932215,
-            "velocityX": 3.3316841264339563,
-            "velocityY": -1.79178919096958,
-            "timestamp": 1.1971290377586241
-        },
-        {
-            "x": 4.968701267365325,
-            "y": 4.604584051483172,
-            "heading": -0.4829711989671097,
-            "angularVelocity": 0.000002075957594698055,
-            "velocityX": 3.331679859150302,
-            "velocityY": -1.7917971255911598,
-            "timestamp": 1.2188340958810036
-        },
-        {
-            "x": 5.0410154847403765,
-            "y": 4.565692827821048,
-            "heading": -0.4829711539083305,
-            "angularVelocity": 0.0000020759575472868184,
-            "velocityX": 3.331675822627313,
-            "velocityY": -1.7918046311069282,
-            "timestamp": 1.240539154003383
-        },
-        {
-            "x": 5.113329654064444,
-            "y": 4.526801514813212,
-            "heading": -0.4829711088495519,
-            "angularVelocity": 0.000002075957520958662,
-            "velocityX": 3.331673608812237,
-            "velocityY": -1.7918087474613025,
-            "timestamp": 1.2622442121257624
-        },
-        {
-            "x": 5.185643801945598,
-            "y": 4.487910161934636,
-            "heading": -0.48297106379077354,
-            "angularVelocity": 0.0000020759575090327586,
-            "velocityX": 3.3316726208897984,
-            "velocityY": -1.7918105843944965,
-            "timestamp": 1.2839492702481419
-        },
-        {
-            "x": 5.257957938797454,
-            "y": 4.4490187885483135,
-            "heading": -0.48297101873199527,
-            "angularVelocity": 0.0000020759575026231074,
-            "velocityX": 3.331672112745739,
-            "velocityY": -1.7918115292316654,
-            "timestamp": 1.3056543283705213
-        },
-        {
-            "x": 5.330272065176824,
-            "y": 4.410127395689581,
-            "heading": -0.4829709736732172,
-            "angularVelocity": 0.000002075957497099591,
-            "velocityX": 3.3316716302550873,
-            "velocityY": -1.7918124263686461,
-            "timestamp": 1.3273593864929008
-        },
-        {
-            "x": 5.402586187963667,
-            "y": 4.371235996150952,
-            "heading": -0.4829709286144391,
-            "angularVelocity": 0.00000207595749402331,
-            "velocityX": 3.3316714647394585,
-            "velocityY": -1.7918127341261798,
-            "timestamp": 1.3490644446152802
-        },
-        {
-            "x": 5.474900315939471,
-            "y": 4.332344606260608,
-            "heading": -0.482970883555661,
-            "angularVelocity": 0.0000020759574957870433,
-            "velocityX": 3.331671703806323,
-            "velocityY": -1.7918122896084423,
-            "timestamp": 1.3707695027376596
-        },
-        {
-            "x": 5.547214451711641,
-            "y": 4.293453230866729,
-            "heading": -0.4829708384968828,
-            "angularVelocity": 0.0000020759574997718594,
-            "velocityX": 3.331672063002153,
-            "velocityY": -1.7918116217242377,
-            "timestamp": 1.392474560860039
-        },
-        {
-            "x": 5.619528591620017,
-            "y": 4.254561863163663,
-            "heading": -0.4829707934381046,
-            "angularVelocity": 0.0000020759575011693265,
-            "velocityX": 3.3316722535663335,
-            "velocityY": -1.7918112673914555,
-            "timestamp": 1.4141796189824185
-        },
-        {
-            "x": 5.6918427353771435,
-            "y": 4.215670502616919,
-            "heading": -0.4829707483793263,
-            "angularVelocity": 0.000002075957502624911,
-            "velocityX": 3.3316724308867816,
-            "velocityY": -1.7918109376838844,
-            "timestamp": 1.435884677104798
-        },
-        {
-            "x": 5.764156898177578,
-            "y": 4.176779177479102,
-            "heading": -0.4829707033205479,
-            "angularVelocity": 0.0000020759575111736717,
-            "velocityX": 3.3316733082540817,
-            "velocityY": -1.7918093063163627,
-            "timestamp": 1.4575897352271774
-        },
-        {
-            "x": 5.836471080177112,
-            "y": 4.137887888039928,
-            "heading": -0.48297065826176927,
-            "angularVelocity": 0.0000020759575207599675,
-            "velocityX": 3.331674192798964,
-            "velocityY": -1.7918076616010294,
-            "timestamp": 1.4792947933495568
-        },
-        {
-            "x": 5.908785251268908,
-            "y": 4.098996578318978,
-            "heading": -0.48297061320299073,
-            "angularVelocity": 0.0000020759575149169034,
-            "velocityX": 3.331673690255448,
-            "velocityY": -1.7918085960272527,
-            "timestamp": 1.5009998514719363
-        },
-        {
-            "x": 5.981099339043632,
-            "y": 4.0601051136793025,
-            "heading": -0.48297056814421313,
-            "angularVelocity": 0.000002075957470564626,
-            "velocityX": 3.3316698516536873,
-            "velocityY": -1.7918157334753977,
-            "timestamp": 1.5227049095943157
-        },
-        {
-            "x": 6.053413282760847,
-            "y": 4.021213381182831,
-            "heading": -0.48297052308543725,
-            "angularVelocity": 0.00000207595739406405,
-            "velocityX": 3.3316632146058507,
-            "velocityY": -1.7918280742300665,
-            "timestamp": 1.5444099677166951
-        },
-        {
-            "x": 6.125727369888532,
-            "y": 3.982321915340118,
-            "heading": -0.4829704780266597,
-            "angularVelocity": 0.0000020759574679524,
-            "velocityX": 3.3316698218431826,
-            "velocityY": -1.7918157889019397,
-            "timestamp": 1.5661150258390746
-        },
-        {
-            "x": 6.198042995964267,
-            "y": 3.9434333111252817,
-            "heading": -0.4829704329678644,
-            "angularVelocity": 0.000002075958288103883,
-            "velocityX": 3.3317407245813113,
-            "velocityY": -1.7916839473808401,
-            "timestamp": 1.587820083961454
-        },
-        {
-            "x": 6.270359121166566,
-            "y": 3.9045456350763237,
-            "heading": -0.4829703879090624,
-            "angularVelocity": 0.0000020759585995923127,
-            "velocityX": 3.331763720444871,
-            "velocityY": -1.791641184728093,
-            "timestamp": 1.6095251420838335
-        },
-        {
-            "x": 6.342677550055512,
-            "y": 3.8656622432711547,
-            "heading": -0.48297034285023976,
-            "angularVelocity": 0.0000020759595452668008,
-            "velocityX": 3.331869856379567,
-            "velocityY": -1.791443800149147,
-            "timestamp": 1.631230200206213
-        },
-        {
-            "x": 6.414977737835259,
-            "y": 3.8267449442462484,
-            "heading": -0.4829702977916256,
-            "angularVelocity": 0.0000020759499429522823,
-            "velocityX": 3.3310294481622025,
-            "velocityY": -1.7930059807042389,
-            "timestamp": 1.6529352583285923
-        },
-        {
-            "x": 6.487268603977522,
-            "y": 3.787810332568674,
-            "heading": -0.4829702527330872,
-            "angularVelocity": 0.00000207594645249869,
-            "velocityX": 3.330599979721225,
-            "velocityY": -1.793803612876013,
-            "timestamp": 1.6746403164509718
-        },
-        {
-            "x": 6.559543297837492,
-            "y": 3.7488457083598847,
-            "heading": -0.4829702076720725,
-            "angularVelocity": 0.000002076060537305802,
-            "velocityX": 3.3298548869330626,
-            "velocityY": -1.7951863565208734,
-            "timestamp": 1.6963453745733512
-        },
-        {
-            "x": 6.630628961599716,
-            "y": 3.7077518923991635,
-            "heading": -0.48297016252441444,
-            "angularVelocity": 0.000002080052392839484,
-            "velocityX": 3.2750736423474676,
-            "velocityY": -1.893282926450661,
-            "timestamp": 1.7180504326957307
-        },
-        {
-            "x": 6.699799614538253,
-            "y": 3.6635105764693066,
-            "heading": -0.4829701170038858,
-            "angularVelocity": 0.0000020972313632639956,
-            "velocityX": 3.186844861162469,
-            "velocityY": -2.0382952066016227,
-            "timestamp": 1.73975549081811
-        },
-        {
-            "x": 6.766806665010948,
-            "y": 3.616056089090901,
-            "heading": -0.48297007073382403,
-            "angularVelocity": 0.000002131764013783056,
-            "velocityX": 3.0871629135886995,
-            "velocityY": -2.1863331169555953,
-            "timestamp": 1.7614605489404895
-        },
-        {
-            "x": 6.831474779993965,
-            "y": 3.565460555976799,
-            "heading": -0.48297002329923955,
-            "angularVelocity": 0.0000021854161466442962,
-            "velocityX": 2.9794029860873867,
-            "velocityY": -2.331048036306895,
-            "timestamp": 1.783165607062869
-        },
-        {
-            "x": 6.893649101257324,
-            "y": 3.5118300914764404,
-            "heading": -0.48296997476148124,
-            "angularVelocity": 0.00000223624180421772,
-            "velocityX": 2.8645083976649595,
-            "velocityY": -2.470874033046681,
-            "timestamp": 1.8048706651852484
-        },
-        {
-            "x": 6.934561334359113,
-            "y": 3.474160479757131,
-            "heading": -0.4829699264500687,
-            "angularVelocity": 0.0000032862628672599958,
-            "velocityX": 2.782952215912443,
-            "velocityY": -2.5623810156238895,
-            "timestamp": 1.8195716840772946
-        },
-        {
-            "x": 6.974231870495054,
-            "y": 3.435185370851295,
-            "heading": -0.4829698798888691,
-            "angularVelocity": 0.0000031672090196575476,
-            "velocityX": 2.6984888889168737,
-            "velocityY": -2.6511841928806597,
-            "timestamp": 1.8342727029693409
-        },
-        {
-            "x": 7.012622614846585,
-            "y": 3.394949051793438,
-            "heading": -0.482969834780297,
-            "angularVelocity": 0.0000030683976682796836,
-            "velocityX": 2.611434257274787,
-            "velocityY": -2.736974855506367,
-            "timestamp": 1.848973721861387
-        },
-        {
-            "x": 7.049699862488227,
-            "y": 3.3534992220248374,
-            "heading": -0.4829697908553284,
-            "angularVelocity": 0.0000029878860039287864,
-            "velocityX": 2.5220869324714577,
-            "velocityY": -2.8195208830745364,
-            "timestamp": 1.8636747407534333
-        },
-        {
-            "x": 7.085438739774718,
-            "y": 3.310890028476691,
-            "heading": -0.48296974786836877,
-            "angularVelocity": 0.0000029240802913465185,
-            "velocityX": 2.4310476402306533,
-            "velocityY": -2.8983837012274143,
-            "timestamp": 1.8783757596454795
-        },
-        {
-            "x": 7.119836677140904,
-            "y": 3.2671911422114968,
-            "heading": -0.4829697055890564,
-            "angularVelocity": 0.0000028759443619428334,
-            "velocityX": 2.339833559754191,
-            "velocityY": -2.9725073198044742,
-            "timestamp": 1.8930767785375258
-        },
-        {
-            "x": 7.1529563800127525,
-            "y": 3.222515689615774,
-            "heading": -0.48296966398118896,
-            "angularVelocity": 0.000002830270996857658,
-            "velocityX": 2.2528848588700137,
-            "velocityY": -3.0389357991976094,
-            "timestamp": 1.907777797429572
-        },
-        {
-            "x": 7.184894346745796,
-            "y": 3.177381185858669,
-            "heading": -0.4821430167851757,
-            "angularVelocity": 0.056230605652804695,
-            "velocityX": 2.17250021699707,
-            "velocityY": -3.0701616050246474,
-            "timestamp": 1.9224788163216182
-        },
-        {
-            "x": 7.2158791345376,
-            "y": 3.1333943550888805,
-            "heading": -0.47750834116458535,
-            "angularVelocity": 0.31526220424747137,
-            "velocityX": 2.107662606199986,
-            "velocityY": -2.992094023740475,
-            "timestamp": 1.9371798352136644
-        },
-        {
-            "x": 7.245895346179036,
-            "y": 3.090550131820991,
-            "heading": -0.46908945700490695,
-            "angularVelocity": 0.5726735147747635,
-            "velocityX": 2.041777638805421,
-            "velocityY": -2.9143710094182476,
-            "timestamp": 1.9518808541057107
-        },
-        {
-            "x": 7.274933319615603,
-            "y": 3.048846636162261,
-            "heading": -0.45692051225089847,
-            "angularVelocity": 0.827761996863506,
-            "velocityX": 1.9752354343465999,
-            "velocityY": -2.8367758700924766,
-            "timestamp": 1.9665818729977569
-        },
-        {
-            "x": 7.3029845213490745,
-            "y": 3.008282422680816,
-            "heading": -0.4410407652781448,
-            "angularVelocity": 1.08018002625281,
-            "velocityX": 1.90811276003785,
-            "velocityY": -2.7592790526506894,
-            "timestamp": 1.981282891889803
-        },
-        {
-            "x": 7.330040747407425,
-            "y": 2.9688555797343485,
-            "heading": -0.42149121219843927,
-            "angularVelocity": 1.3298093977882255,
-            "velocityX": 1.8404320310743014,
-            "velocityY": -2.6819122698903146,
-            "timestamp": 1.9959839107818493
-        },
-        {
-            "x": 7.356112777651364,
-            "y": 2.9307983654229495,
-            "heading": -0.3996724931243264,
-            "angularVelocity": 1.4841637327544344,
-            "velocityX": 1.7734845751435842,
-            "velocityY": -2.5887467114262686,
-            "timestamp": 2.0106849296738956
-        },
-        {
-            "x": 7.38118332940277,
-            "y": 2.8941879380599844,
-            "heading": -0.37646864699164706,
-            "angularVelocity": 1.578383532670194,
-            "velocityX": 1.7053615083080305,
-            "velocityY": -2.4903326518935414,
-            "timestamp": 2.025385948565942
-        },
-        {
-            "x": 7.40523963577506,
-            "y": 2.8590504496296347,
-            "heading": -0.3524751392807844,
-            "angularVelocity": 1.6320982842790503,
-            "velocityX": 1.6363700059799433,
-            "velocityY": -2.390139669119207,
-            "timestamp": 2.040086967457988
-        },
-        {
-            "x": 7.428276340738969,
-            "y": 2.825395406477394,
-            "heading": -0.3281233875567818,
-            "angularVelocity": 1.656466936259613,
-            "velocityX": 1.5670141731720877,
-            "velocityY": -2.289300040995877,
-            "timestamp": 2.0547879863500342
-        },
-        {
-            "x": 7.450292281624529,
-            "y": 2.793225377698084,
-            "heading": -0.3037369697425905,
-            "angularVelocity": 1.658825010243686,
-            "velocityX": 1.497579252650967,
-            "velocityY": -2.1882856566299744,
-            "timestamp": 2.0694890052420805
-        },
-        {
-            "x": 7.471288319158757,
-            "y": 2.7625397684205146,
-            "heading": -0.27956516086770095,
-            "angularVelocity": 1.6442267745106667,
-            "velocityX": 1.4282028809299567,
-            "velocityY": -2.0873117368873455,
-            "timestamp": 2.0841900241341267
-        },
-        {
-            "x": 7.491266209557209,
-            "y": 2.733336527235843,
-            "heading": -0.2558043926700512,
-            "angularVelocity": 1.6162667616531794,
-            "velocityX": 1.3589459713748013,
-            "velocityY": -1.9864773590946931,
-            "timestamp": 2.098891043026173
-        },
-        {
-            "x": 7.510228057377649,
-            "y": 2.7056129838220437,
-            "heading": -0.2326125564520332,
-            "angularVelocity": 1.577566588297197,
-            "velocityX": 1.28983221909181,
-            "velocityY": -1.885824623271541,
-            "timestamp": 2.113592061918219
-        },
-        {
-            "x": 7.528176060430696,
-            "y": 2.679366275566135,
-            "heading": -0.21011884554685348,
-            "angularVelocity": 1.5300783619392286,
-            "velocityX": 1.2208679673730278,
-            "velocityY": -1.7853666095287006,
-            "timestamp": 2.1282930808102654
-        },
-        {
-            "x": 7.545112396890263,
-            "y": 2.654593565371254,
-            "heading": -0.18843070859149175,
-            "angularVelocity": 1.4752812110931899,
-            "velocityX": 1.152051880480808,
-            "velocityY": -1.6851015821961506,
-            "timestamp": 2.1429940997023116
-        },
-        {
-            "x": 7.561039180049459,
-            "y": 2.631292148556631,
-            "heading": -0.1676388680105614,
-            "angularVelocity": 1.4143128944742427,
-            "velocityX": 1.0833795450607198,
-            "velocityY": -1.5850205340005987,
-            "timestamp": 2.157695118594358
-        },
-        {
-            "x": 7.575958444025175,
-            "y": 2.6094594995019675,
-            "heading": -0.14782100448684538,
-            "angularVelocity": 1.3480605439149753,
-            "velocityX": 1.0148455753489192,
-            "velocityY": -1.485111284801969,
-            "timestamp": 2.172396137486404
-        },
-        {
-            "x": 7.589872142500143,
-            "y": 2.5890932855015207,
-            "heading": -0.12904449811203259,
-            "angularVelocity": 1.2772248313327248,
-            "velocityX": 0.9464445000131985,
-            "velocityY": -1.3853607120705473,
-            "timestamp": 2.1870971563784503
-        },
-        {
-            "x": 7.602782151943804,
-            "y": 2.570191363211945,
-            "heading": -0.11136848807718236,
-            "angularVelocity": 1.2023663233582889,
-            "velocityX": 0.8781710668125251,
-            "velocityY": -1.2857559349035097,
-            "timestamp": 2.2017981752704965
-        },
-        {
-            "x": 7.614690275426409,
-            "y": 2.552751766479238,
-            "heading": -0.09484543137092857,
-            "angularVelocity": 1.1239395600799826,
-            "velocityX": 0.8100202829510676,
-            "velocityY": -1.186284900439311,
-            "timestamp": 2.2164991941625427
-        },
-        {
-            "x": 7.62559824546286,
-            "y": 2.536772690588083,
-            "heading": -0.07952228805027751,
-            "angularVelocity": 1.042318456507909,
-            "velocityX": 0.7419873490775946,
-            "velocityY": -1.086936627215761,
-            "timestamp": 2.231200213054589
-        },
-        {
-            "x": 7.635507725447758,
-            "y": 2.522252475791086,
-            "heading": -0.06544142530276546,
-            "angularVelocity": 0.9578154310876013,
-            "velocityX": 0.6740675634570735,
-            "velocityY": -0.9877012541528096,
-            "timestamp": 2.245901231946635
-        },
-        {
-            "x": 7.644420309753795,
-            "y": 2.509189591670685,
-            "heading": -0.05264130826624494,
-            "angularVelocity": 0.8706959109783889,
-            "velocityX": 0.6062562310465265,
-            "velocityY": -0.8885699839124228,
-            "timestamp": 2.2606022508386814
-        },
-        {
-            "x": 7.652337522761704,
-            "y": 2.497582623097395,
-            "heading": -0.04115702850586921,
-            "angularVelocity": 0.7811893750159932,
-            "velocityX": 0.538548590818527,
-            "velocityY": -0.7895349743118973,
-            "timestamp": 2.2753032697307276
-        },
-        {
-            "x": 7.6592608171290975,
-            "y": 2.487430258069545,
-            "heading": -0.031020708755951136,
-            "angularVelocity": 0.6894977704846257,
-            "velocityX": 0.4709397639872257,
-            "velocityY": -0.6905892103399419,
-            "timestamp": 2.290004288622774
-        },
-        {
-            "x": 7.665191571570965,
-            "y": 2.4787312774326344,
-            "heading": -0.022261813501037925,
-            "angularVelocity": 0.5958019181685587,
-            "velocityX": 0.40342472079099084,
-            "velocityY": -0.5917263763001924,
-            "timestamp": 2.30470530751482
-        },
-        {
-            "x": 7.670131088361328,
-            "y": 2.4714845463094046,
-            "heading": -0.014907388223763604,
-            "angularVelocity": 0.5002663646159557,
-            "velocityX": 0.33599826152420176,
-            "velocityY": -0.49294073944433947,
-            "timestamp": 2.3194063264068663
-        },
-        {
-            "x": 7.674080590697013,
-            "y": 2.465689006980458,
-            "heading": -0.008982245050778671,
-            "angularVelocity": 0.40304302827545885,
-            "velocityX": 0.26865500715889973,
-            "velocityY": -0.3942270513020354,
-            "timestamp": 2.3341073452989125
-        },
-        {
-            "x": 7.677041220003002,
-            "y": 2.461343672911727,
-            "heading": -0.0045091086563273375,
-            "angularVelocity": 0.304273902870198,
-            "velocityX": 0.20138939536970257,
-            "velocityY": -0.29558046966947404,
-            "timestamp": 2.3488083641909587
-        },
-        {
-            "x": 7.679014033209882,
-            "y": 2.458447623611078,
-            "heading": -0.00150873334272897,
-            "angularVelocity": 0.20409301801671295,
-            "velocityX": 0.1341956786374654,
-            "velocityY": -0.1969965022095263,
-            "timestamp": 2.363509383083005
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -1.1328442579469643e-34,
-            "angularVelocity": 0.10262780789604178,
-            "velocityX": 0.06706792211866815,
-            "velocityY": -0.0984709714141921,
-            "timestamp": 2.378210401975051
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -2.155134012002005e-33,
-            "angularVelocity": -1.3889170145055665e-31,
-            "velocityX": 1.4892305241424507e-32,
-            "velocityY": 6.576507695252416e-33,
-            "timestamp": 2.3929114208670974
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.1.traj
deleted file mode 100644
index 39d8ef61..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.1.traj	
+++ /dev/null
@@ -1,625 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -4.1566893564125267e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 1.3546830312136449,
-            "y": 5.550921973415328,
-            "heading": -1.4050458772244498e-18,
-            "angularVelocity": -4.014308801297789e-17,
-            "velocityX": 0.13379730014070312,
-            "velocityY": -0.25936522568857817,
-            "timestamp": 0.03500093954603885
-        },
-        {
-            "x": 1.3640490935091651,
-            "y": 5.532765920501419,
-            "heading": -3.603876179363921e-18,
-            "angularVelocity": -6.28220327965604e-17,
-            "velocityX": 0.26759459651667833,
-            "velocityY": -0.5187304440792354,
-            "timestamp": 0.0700018790920777
-        },
-        {
-            "x": 1.3780981867284385,
-            "y": 5.505531841564791,
-            "heading": -4.091508575199503e-18,
-            "angularVelocity": -1.3931950355515604e-17,
-            "velocityX": 0.4013918883749871,
-            "velocityY": -0.7780956537124009,
-            "timestamp": 0.10500281863811654
-        },
-        {
-            "x": 1.3968303106782045,
-            "y": 5.469219736980081,
-            "heading": -4.985304542720085e-18,
-            "angularVelocity": -2.5536326104216816e-17,
-            "velocityX": 0.5351891747117141,
-            "velocityY": -1.0374608526419855,
-            "timestamp": 0.1400037581841554
-        },
-        {
-            "x": 1.4202454651168877,
-            "y": 5.423829607215581,
-            "heading": -4.3424315250592746e-18,
-            "angularVelocity": 1.8367331153854942e-17,
-            "velocityX": 0.6689864541464801,
-            "velocityY": -1.2968260381921248,
-            "timestamp": 0.17500469773019423
-        },
-        {
-            "x": 1.4483436497338926,
-            "y": 5.369361452873384,
-            "heading": -2.0560193041515625e-18,
-            "angularVelocity": 6.532433512064817e-17,
-            "velocityX": 0.8027837247073224,
-            "velocityY": -1.5561912065401715,
-            "timestamp": 0.21000563727623306
-        },
-        {
-            "x": 1.4811248641150936,
-            "y": 5.305815274756268,
-            "heading": 2.1208651292681738e-19,
-            "angularVelocity": 6.480130833433249e-17,
-            "velocityX": 0.9365809834363137,
-            "velocityY": -1.815556351952184,
-            "timestamp": 0.2450065768222719
-        },
-        {
-            "x": 1.518589107680717,
-            "y": 5.233191073988126,
-            "heading": 3.0112738874394488e-18,
-            "angularVelocity": 7.99746695569e-17,
-            "velocityX": 1.0703782256008032,
-            "velocityY": -2.074921465253921,
-            "timestamp": 0.2800075163683107
-        },
-        {
-            "x": 1.5607363795611104,
-            "y": 5.151488852254779,
-            "heading": 5.4707987602321704e-18,
-            "angularVelocity": 7.027028770373186e-17,
-            "velocityX": 1.2041754429187383,
-            "velocityY": -2.334286530390624,
-            "timestamp": 0.31500845591434956
-        },
-        {
-            "x": 1.6075666783068712,
-            "y": 5.060708612365892,
-            "heading": 7.441304754324509e-18,
-            "angularVelocity": 5.629869559433992e-17,
-            "velocityX": 1.3379726188262824,
-            "velocityY": -2.5936515152532893,
-            "timestamp": 0.3500093954603884
-        },
-        {
-            "x": 1.6590800010192646,
-            "y": 4.9608503599406655,
-            "heading": 8.015191331788108e-18,
-            "angularVelocity": 1.639633984973558e-17,
-            "velocityX": 1.4717697119150586,
-            "velocityY": -2.853016339571725,
-            "timestamp": 0.38501033500642723
-        },
-        {
-            "x": 1.7152763390026586,
-            "y": 4.851914111835609,
-            "heading": 8.871725964928404e-18,
-            "angularVelocity": 2.4471795525975083e-17,
-            "velocityX": 1.6055665565638684,
-            "velocityY": -3.112380682288518,
-            "timestamp": 0.42001127455246606
-        },
-        {
-            "x": 1.775979033805623,
-            "y": 4.734242319347404,
-            "heading": 1.526334137039101e-17,
-            "angularVelocity": 1.8261268092684144e-16,
-            "velocityX": 1.7343161523752855,
-            "velocityY": -3.361960964887902,
-            "timestamp": 0.4550122140985049
-        },
-        {
-            "x": 1.8366817286107127,
-            "y": 4.616570526856247,
-            "heading": 2.6738432547349554e-17,
-            "angularVelocity": 3.2785094702569765e-16,
-            "velocityX": 1.734316152436003,
-            "velocityY": -3.3619609649722197,
-            "timestamp": 0.49001315364454373
-        },
-        {
-            "x": 1.8973844234157844,
-            "y": 4.498898734365081,
-            "heading": 1.9852147320466475e-17,
-            "angularVelocity": -1.967457250062152e-16,
-            "velocityX": 1.7343161524354893,
-            "velocityY": -3.3619609649724835,
-            "timestamp": 0.5250140931905826
-        },
-        {
-            "x": 1.9580871182208561,
-            "y": 4.381226941873916,
-            "heading": 2.777024505719557e-17,
-            "angularVelocity": 2.2622529107579697e-16,
-            "velocityX": 1.7343161524354895,
-            "velocityY": -3.361960964972483,
-            "timestamp": 0.5600150327366215
-        },
-        {
-            "x": 2.018789813025928,
-            "y": 4.26355514938275,
-            "heading": 1.7414652311267274e-17,
-            "angularVelocity": -2.9586613617347484e-16,
-            "velocityX": 1.7343161524354893,
-            "velocityY": -3.361960964972483,
-            "timestamp": 0.5950159722826603
-        },
-        {
-            "x": 2.0794925078309996,
-            "y": 4.145883356891583,
-            "heading": 6.771469247853252e-18,
-            "angularVelocity": -3.040827818183264e-16,
-            "velocityX": 1.7343161524354895,
-            "velocityY": -3.361960964972483,
-            "timestamp": 0.6300169118286992
-        },
-        {
-            "x": 2.140195202636071,
-            "y": 4.028211564400419,
-            "heading": 1.7156471914154247e-17,
-            "angularVelocity": 2.9670636674740876e-16,
-            "velocityX": 1.7343161524354882,
-            "velocityY": -3.361960964972482,
-            "timestamp": 0.6650178513747381
-        },
-        {
-            "x": 2.2008978974411426,
-            "y": 3.910539771909253,
-            "heading": 9.427453062324153e-18,
-            "angularVelocity": -2.208231822366884e-16,
-            "velocityX": 1.734316152435471,
-            "velocityY": -3.361960964972468,
-            "timestamp": 0.700018790920777
-        },
-        {
-            "x": 2.261600592246207,
-            "y": 3.7928679794180944,
-            "heading": -2.0403441270686145e-18,
-            "angularVelocity": -3.2764255297515643e-16,
-            "velocityX": 1.7343161524352513,
-            "velocityY": -3.3619609649722837,
-            "timestamp": 0.7350197304668159
-        },
-        {
-            "x": 2.322303287051213,
-            "y": 3.6751961869269927,
-            "heading": 3.357966916114392e-18,
-            "angularVelocity": 1.5423331811082687e-16,
-            "velocityX": 1.7343161524334347,
-            "velocityY": -3.3619609649707387,
-            "timestamp": 0.7700206700128548
-        },
-        {
-            "x": 2.383005981879539,
-            "y": 3.55752439444761,
-            "heading": 1.6083387472754582e-17,
-            "angularVelocity": 3.6357368464068287e-16,
-            "velocityX": 1.7343161531001825,
-            "velocityY": -3.361960964635663,
-            "timestamp": 0.8050216095588937
-        },
-        {
-            "x": 2.4437089810285184,
-            "y": 3.439852758955193,
-            "heading": 2.2946423291224054e-17,
-            "angularVelocity": 1.9608147402560587e-16,
-            "velocityX": 1.7343248477441577,
-            "velocityY": -3.3619564794134043,
-            "timestamp": 0.8400225491049326
-        },
-        {
-            "x": 2.507799181067132,
-            "y": 3.323991142238629,
-            "heading": 9.669545465348722e-18,
-            "angularVelocity": -3.793291836756151e-16,
-            "velocityX": 1.8310994181462568,
-            "velocityY": -3.3102430454713967,
-            "timestamp": 0.8750234886509715
-        },
-        {
-            "x": 2.5806292795037855,
-            "y": 3.213414270581727,
-            "heading": 3.38196516760506e-17,
-            "angularVelocity": 6.899845125281262e-16,
-            "velocityX": 2.0808040978720626,
-            "velocityY": -3.159254382625257,
-            "timestamp": 0.9100244281970103
-        },
-        {
-            "x": 2.661766753531331,
-            "y": 3.1087807297901673,
-            "heading": 5.146705636243318e-17,
-            "angularVelocity": 5.041980276892245e-16,
-            "velocityX": 2.3181513147947848,
-            "velocityY": -2.9894494876003863,
-            "timestamp": 0.9450253677430492
-        },
-        {
-            "x": 2.7507288455963135,
-            "y": 3.0107133388519287,
-            "heading": 6.422105835490463e-17,
-            "angularVelocity": 3.643902751722035e-16,
-            "velocityX": 2.5417058290140595,
-            "velocityY": -2.8018502420229328,
-            "timestamp": 0.9800263072890881
-        },
-        {
-            "x": 2.9215337593923705,
-            "y": 2.8658352980219064,
-            "heading": 5.1318206888106e-17,
-            "angularVelocity": -2.1793112103404205e-16,
-            "velocityX": 2.884920936848368,
-            "velocityY": -2.4470121145413257,
-            "timestamp": 1.0392324060123015
-        },
-        {
-            "x": 3.109750180151373,
-            "y": 2.744432888397421,
-            "heading": 6.648964720130561e-17,
-            "angularVelocity": 2.5624793121609224e-16,
-            "velocityX": 3.179003934013426,
-            "velocityY": -2.0505051378665997,
-            "timestamp": 1.098438504735515
-        },
-        {
-            "x": 3.309758578030313,
-            "y": 2.6436330516151556,
-            "heading": 6.809268296964168e-17,
-            "angularVelocity": 2.7075517621497836e-17,
-            "velocityX": 3.3781722186064402,
-            "velocityY": -1.702524553319066,
-            "timestamp": 1.1576446034587284
-        },
-        {
-            "x": 3.5097675494851996,
-            "y": 2.542834352942729,
-            "heading": 6.982732716570642e-17,
-            "angularVelocity": 2.929840393934814e-17,
-            "velocityX": 3.378181906393955,
-            "velocityY": -1.702505330465446,
-            "timestamp": 1.216850702181942
-        },
-        {
-            "x": 3.709776520948374,
-            "y": 2.4420356542872104,
-            "heading": 8.871000937094426e-17,
-            "angularVelocity": 3.189313704575333e-16,
-            "velocityX": 3.3781819065336705,
-            "velocityY": -1.702505330180368,
-            "timestamp": 1.2760568009051554
-        },
-        {
-            "x": 3.9097854924115545,
-            "y": 2.341236955631689,
-            "heading": 7.04137580064196e-17,
-            "angularVelocity": -3.090264644873572e-16,
-            "velocityX": 3.3781819065337895,
-            "velocityY": -1.7025053301803954,
-            "timestamp": 1.3352628996283689
-        },
-        {
-            "x": 4.109794463874735,
-            "y": 2.240438256976168,
-            "heading": 6.635408667627181e-17,
-            "angularVelocity": -6.856846534504647e-17,
-            "velocityX": 3.3781819065337926,
-            "velocityY": -1.7025053301803963,
-            "timestamp": 1.3944689983515823
-        },
-        {
-            "x": 4.3098034353379155,
-            "y": 2.1396395583206465,
-            "heading": 8.756379552984881e-17,
-            "angularVelocity": 3.5823520399025277e-16,
-            "velocityX": 3.3781819065337926,
-            "velocityY": -1.7025053301803963,
-            "timestamp": 1.4536750970747958
-        },
-        {
-            "x": 4.509812406801097,
-            "y": 2.0388408596651253,
-            "heading": 7.076319345079811e-17,
-            "angularVelocity": -2.837647208878057e-16,
-            "velocityX": 3.3781819065337926,
-            "velocityY": -1.7025053301803963,
-            "timestamp": 1.5128811957980093
-        },
-        {
-            "x": 4.709821378264278,
-            "y": 1.9380421610096037,
-            "heading": 6.652684249010322e-17,
-            "angularVelocity": -7.15526111686399e-17,
-            "velocityX": 3.378181906533802,
-            "velocityY": -1.7025053301803985,
-            "timestamp": 1.5720872945212228
-        },
-        {
-            "x": 4.9098303497278835,
-            "y": 1.837243462354927,
-            "heading": 7.855425123263822e-17,
-            "angularVelocity": 2.0314476045367986e-16,
-            "velocityX": 3.378181906540968,
-            "velocityY": -1.7025053301661324,
-            "timestamp": 1.6312933932444362
-        },
-        {
-            "x": 5.109839349236117,
-            "y": 1.7364448193478421,
-            "heading": 7.31685607290898e-17,
-            "angularVelocity": -9.096513061477316e-17,
-            "velocityX": 3.378182380218786,
-            "velocityY": -1.7025043902702668,
-            "timestamp": 1.6904994919676497
-        },
-        {
-            "x": 5.311619758605957,
-            "y": 1.6392407417297363,
-            "heading": 6.073866217998324e-17,
-            "angularVelocity": -2.0994287441931112e-16,
-            "velocityX": 3.4081017618384997,
-            "velocityY": -1.6417916348765775,
-            "timestamp": 1.7497055906908632
-        },
-        {
-            "x": 5.430192834880249,
-            "y": 1.5926107881863976,
-            "heading": 7.355026472308393e-17,
-            "angularVelocity": 3.8038299911394555e-16,
-            "velocityX": 3.5204949744296554,
-            "velocityY": -1.3844670499027307,
-            "timestamp": 1.78338638760593
-        },
-        {
-            "x": 5.550146685206584,
-            "y": 1.5496573158737061,
-            "heading": 8.480161924468705e-17,
-            "angularVelocity": 3.3405844137167443e-16,
-            "velocityX": 3.5614908586879754,
-            "velocityY": -1.2753104512640399,
-            "timestamp": 1.8170671845209967
-        },
-        {
-            "x": 5.670100671705041,
-            "y": 1.506704223843596,
-            "heading": 8.467696633181045e-17,
-            "angularVelocity": -3.701008417078351e-18,
-            "velocityX": 3.5614949017075612,
-            "velocityY": -1.2752991604806918,
-            "timestamp": 1.8507479814360634
-        },
-        {
-            "x": 5.790054658213967,
-            "y": 1.4637511318444054,
-            "heading": 8.225246246818014e-17,
-            "angularVelocity": -7.198475350046388e-17,
-            "velocityX": 3.561494902018399,
-            "velocityY": -1.275299159562779,
-            "timestamp": 1.8844287783511302
-        },
-        {
-            "x": 5.910008644722922,
-            "y": 1.4207980398452125,
-            "heading": 9.443730259968232e-17,
-            "angularVelocity": 3.617741041647218e-16,
-            "velocityX": 3.5614949020192235,
-            "velocityY": -1.2752991595628402,
-            "timestamp": 1.918109575266197
-        },
-        {
-            "x": 6.02996263123188,
-            "y": 1.3778449478460182,
-            "heading": 8.582959689044548e-17,
-            "angularVelocity": -2.555671628240573e-16,
-            "velocityX": 3.5614949020193487,
-            "velocityY": -1.2752991595628858,
-            "timestamp": 1.9517903721812637
-        },
-        {
-            "x": 6.149916617740839,
-            "y": 1.3348918558468235,
-            "heading": 7.701354643967334e-17,
-            "angularVelocity": -2.6175302422337863e-16,
-            "velocityX": 3.56149490201936,
-            "velocityY": -1.2752991595628895,
-            "timestamp": 1.9854711690963305
-        },
-        {
-            "x": 6.269870604249798,
-            "y": 1.291938763847629,
-            "heading": 8.367010625720842e-17,
-            "angularVelocity": 1.9763664839406868e-16,
-            "velocityX": 3.5614949020193607,
-            "velocityY": -1.27529915956289,
-            "timestamp": 2.019151966011397
-        },
-        {
-            "x": 6.3898245907587565,
-            "y": 1.2489856718484342,
-            "heading": 7.968637324735258e-17,
-            "angularVelocity": -1.1827906031741492e-16,
-            "velocityX": 3.5614949020193607,
-            "velocityY": -1.2752991595628902,
-            "timestamp": 2.0528327629264638
-        },
-        {
-            "x": 6.509778577267715,
-            "y": 1.2060325798492397,
-            "heading": 7.914041276645377e-17,
-            "angularVelocity": -1.6209844508000374e-17,
-            "velocityX": 3.561494902019361,
-            "velocityY": -1.27529915956289,
-            "timestamp": 2.0865135598415305
-        },
-        {
-            "x": 6.629732563776674,
-            "y": 1.1630794878500452,
-            "heading": 7.575912442206911e-17,
-            "angularVelocity": -1.0039217162560839e-16,
-            "velocityX": 3.561494902019361,
-            "velocityY": -1.27529915956289,
-            "timestamp": 2.1201943567565973
-        },
-        {
-            "x": 6.749686550285633,
-            "y": 1.1201263958508503,
-            "heading": 6.889954024340196e-17,
-            "angularVelocity": -2.0366454499176497e-16,
-            "velocityX": 3.5614949020193576,
-            "velocityY": -1.2752991595628989,
-            "timestamp": 2.153875153671664
-        },
-        {
-            "x": 6.869640536794461,
-            "y": 1.077173303851719,
-            "heading": 8.054846416919904e-17,
-            "angularVelocity": 3.458624792985843e-16,
-            "velocityX": 3.5614949020154905,
-            "velocityY": -1.275299159561009,
-            "timestamp": 2.187555950586731
-        },
-        {
-            "x": 6.985406128017608,
-            "y": 1.0357199913369377,
-            "heading": 6.97905590140722e-17,
-            "angularVelocity": -3.1940767538205783e-16,
-            "velocityX": 3.437139314579595,
-            "velocityY": -1.2307699434580792,
-            "timestamp": 2.2212367475017976
-        },
-        {
-            "x": 7.092266700241795,
-            "y": 0.9974553856875888,
-            "heading": 5.947830002893604e-17,
-            "angularVelocity": -3.0617621445548207e-16,
-            "velocityX": 3.172744768885924,
-            "velocityY": -1.1360956139440452,
-            "timestamp": 2.2549175444168643
-        },
-        {
-            "x": 7.190222235939473,
-            "y": 0.9623794931799317,
-            "heading": 5.044588900647289e-17,
-            "angularVelocity": -2.6817687744625345e-16,
-            "velocityX": 2.9083497027904013,
-            "velocityY": -1.0414210980846788,
-            "timestamp": 2.288598341331931
-        },
-        {
-            "x": 7.279272729268112,
-            "y": 0.930492315906057,
-            "heading": 4.27627289839913e-17,
-            "angularVelocity": -2.281169282784766e-16,
-            "velocityX": 2.6439544632272356,
-            "velocityY": -0.9467465201100714,
-            "timestamp": 2.322279138246998
-        },
-        {
-            "x": 7.35941817730645,
-            "y": 0.9017938549120106,
-            "heading": 3.626946964953948e-17,
-            "angularVelocity": -1.9278817213060433e-16,
-            "velocityX": 2.379559136930222,
-            "velocityY": -0.8520719110778341,
-            "timestamp": 2.3559599351620646
-        },
-        {
-            "x": 7.430658578301727,
-            "y": 0.8762841108254199,
-            "heading": 2.9442071838197796e-17,
-            "angularVelocity": -2.0270891340137855e-16,
-            "velocityX": 2.1151637585928986,
-            "velocityY": -0.7573972834110186,
-            "timestamp": 2.3896407320771313
-        },
-        {
-            "x": 7.492993931085436,
-            "y": 0.8539630840647031,
-            "heading": 2.3413209096667504e-17,
-            "angularVelocity": -1.790000012026981e-16,
-            "velocityX": 1.850768345562039,
-            "velocityY": -0.662722643321152,
-            "timestamp": 2.423321528992198
-        },
-        {
-            "x": 7.546424234822929,
-            "y": 0.8348307749287305,
-            "heading": 1.7731266545962487e-17,
-            "angularVelocity": -1.6869976443484408e-16,
-            "velocityX": 1.5863729077500839,
-            "velocityY": -0.5680479943576784,
-            "timestamp": 2.457002325907265
-        },
-        {
-            "x": 7.590949488888224,
-            "y": 0.8188871836416546,
-            "heading": 1.2363765500283435e-17,
-            "angularVelocity": -1.593638362048541e-16,
-            "velocityX": 1.321977451352309,
-            "velocityY": -0.4733733387389999,
-            "timestamp": 2.4906831228223316
-        },
-        {
-            "x": 7.626569692794441,
-            "y": 0.8061323103778162,
-            "heading": 7.88828312817125e-18,
-            "angularVelocity": -1.3287934794211324e-16,
-            "velocityX": 1.057581980498898,
-            "velocityY": -0.37869867794405154,
-            "timestamp": 2.5243639197373984
-        },
-        {
-            "x": 7.653284846152079,
-            "y": 0.7965661552766882,
-            "heading": 4.236479358871306e-18,
-            "angularVelocity": -1.0842391112822455e-16,
-            "velocityX": 0.7931864980809786,
-            "velocityY": -0.28402401300808766,
-            "timestamp": 2.558044716652465
-        },
-        {
-            "x": 7.671094948642455,
-            "y": 0.7901887184523844,
-            "heading": 1.249730071610374e-18,
-            "angularVelocity": -8.867810508445172e-17,
-            "velocityX": 0.5287910062011898,
-            "velocityY": -0.18934934468402032,
-            "timestamp": 2.591725513567532
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -2.906959217388984e-34,
-            "angularVelocity": -3.7105121777105e-17,
-            "velocityX": 0.26439550643651005,
-            "velocityY": -0.09467467353653367,
-            "timestamp": 2.6254063104825986
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -4.078160951605287e-34,
-            "angularVelocity": -3.477050502256718e-33,
-            "velocityX": 7.685636324015334e-33,
-            "velocityY": -2.3747808746440768e-32,
-            "timestamp": 2.6590871073976654
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.1.traj
deleted file mode 100644
index 641af2c0..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.1.traj	
+++ /dev/null
@@ -1,112 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -1.3982338019676807e-32,
-            "velocityX": 1.9225496754980614e-32,
-            "velocityY": -8.567103847838605e-38,
-            "timestamp": 0
-        },
-        {
-            "x": 1.3816658535274804,
-            "y": 5.599900984360149,
-            "heading": 0.013082876743969601,
-            "angularVelocity": 0.16716651866961724,
-            "velocityX": 0.4046105912700521,
-            "velocityY": 0.509835013927756,
-            "timestamp": 0.07826254233257365
-        },
-        {
-            "x": 1.4449977124682947,
-            "y": 5.679702872674784,
-            "heading": 0.039246215534929935,
-            "angularVelocity": 0.3343021835373132,
-            "velocityX": 0.8092231232623148,
-            "velocityY": 1.0196690004717623,
-            "timestamp": 0.1565250846651473
-        },
-        {
-            "x": 1.5399958043003432,
-            "y": 5.799405527723277,
-            "heading": 0.07848745324353076,
-            "angularVelocity": 0.501405098007769,
-            "velocityX": 1.2138385618545031,
-            "velocityY": 1.5295012336785618,
-            "timestamp": 0.23478762699772096
-        },
-        {
-            "x": 1.6666604122853452,
-            "y": 5.959008727776544,
-            "heading": 0.1308059995871385,
-            "angularVelocity": 0.6685004701391144,
-            "velocityX": 1.6184576198246376,
-            "velocityY": 2.039330633740969,
-            "timestamp": 0.3130501693302946
-        },
-        {
-            "x": 1.8249918316073899,
-            "y": 6.158512135429991,
-            "heading": 0.19620604644996878,
-            "angularVelocity": 0.8356494040905814,
-            "velocityX": 2.023080449510844,
-            "velocityY": 2.5491557226145924,
-            "timestamp": 0.3913127116628683
-        },
-        {
-            "x": 1.9833278311922586,
-            "y": 6.358008426099132,
-            "heading": 0.26178707056088246,
-            "angularVelocity": 0.8379618417228061,
-            "velocityX": 2.023138973840461,
-            "velocityY": 2.549064785314409,
-            "timestamp": 0.469575253995442
-        },
-        {
-            "x": 2.109996681019138,
-            "y": 6.517605179473628,
-            "heading": 0.31426571552794874,
-            "angularVelocity": 0.6705461310477281,
-            "velocityX": 1.618511819979031,
-            "velocityY": 2.039248261272889,
-            "timestamp": 0.5478377963280157
-        },
-        {
-            "x": 2.204998332006287,
-            "y": 6.637302612224517,
-            "heading": 0.35363163522076196,
-            "angularVelocity": 0.5029982226430836,
-            "velocityX": 1.2138840389754155,
-            "velocityY": 1.5294345057465644,
-            "timestamp": 0.6261003386605893
-        },
-        {
-            "x": 2.268332773183591,
-            "y": 6.717100865272008,
-            "heading": 0.3798775217934225,
-            "angularVelocity": 0.3353569382033373,
-            "velocityX": 0.8092561178011136,
-            "velocityY": 1.0196225508288703,
-            "timestamp": 0.704362880993163
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.1676725265429579,
-            "velocityX": 0.40462813847574725,
-            "velocityY": 0.509811380244222,
-            "timestamp": 0.7826254233257367
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 9.092584352802723e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0.8608879656583104
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.1.traj
deleted file mode 100644
index bb0e9680..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.1.traj	
+++ /dev/null
@@ -1,76 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 7.039472547405692e-42,
-            "angularVelocity": 1.0705191064262213e-40,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 1.4250000045295101,
-            "y": 5.56,
-            "heading": 8.194631828276944e-23,
-            "angularVelocity": 8.6403876700238e-22,
-            "velocityX": 0.7907971475606789,
-            "velocityY": 8.424071056371489e-35,
-            "timestamp": 0.0948410155004449
-        },
-        {
-            "x": 1.5750000090590195,
-            "y": 5.56,
-            "heading": 2.6954329855279996e-22,
-            "angularVelocity": 1.978015178210688e-21,
-            "velocityX": 1.5815942473623736,
-            "velocityY": 1.2488550168110521e-34,
-            "timestamp": 0.1896820310008898
-        },
-        {
-            "x": 1.7999999999999998,
-            "y": 5.56,
-            "heading": 5.87848829010012e-22,
-            "angularVelocity": 3.3562006983676933e-21,
-            "velocityX": 2.3723912038871497,
-            "velocityY": 2.135553914770072e-34,
-            "timestamp": 0.2845230465013347
-        },
-        {
-            "x": 2.0249999909409806,
-            "y": 5.56,
-            "heading": 4.475372520266276e-22,
-            "angularVelocity": -1.4794393741169351e-21,
-            "velocityX": 2.37239120388715,
-            "velocityY": 2.583104828416982e-34,
-            "timestamp": 0.3793640620017796
-        },
-        {
-            "x": 2.17499999547049,
-            "y": 5.56,
-            "heading": 1.6184867431561565e-22,
-            "angularVelocity": -3.012289189189882e-21,
-            "velocityX": 1.5815942473623736,
-            "velocityY": 1.9272829246954695e-34,
-            "timestamp": 0.47420507750222446
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 1.246024226890136e-41,
-            "angularVelocity": -1.7065260802739357e-21,
-            "velocityX": 0.7907971475606789,
-            "velocityY": -3.1542839688982834e-34,
-            "timestamp": 0.5690460930026694
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 7.131086050293936e-41,
-            "angularVelocity": 6.207915462550707e-40,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0.6638871085031143
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.1.traj
deleted file mode 100644
index aceaac81..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.1.traj	
+++ /dev/null
@@ -1,112 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 5.56203910119815e-35,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 1.3816658774685813,
-            "y": 5.519999060819193,
-            "heading": -0.012983997100427386,
-            "angularVelocity": -0.16577951920330813,
-            "velocityX": 0.4043095436088268,
-            "velocityY": -0.5107315115509736,
-            "timestamp": 0.07832087559926142
-        },
-        {
-            "x": 1.4449977779940397,
-            "y": 5.439997258819368,
-            "heading": -0.038949622534069785,
-            "angularVelocity": -0.33152879401526647,
-            "velocityX": 0.8086209460872757,
-            "velocityY": -1.0214620481155445,
-            "timestamp": 0.15664175119852283
-        },
-        {
-            "x": 1.5399959199593471,
-            "y": 5.3199947242673264,
-            "heading": -0.07789432529768456,
-            "angularVelocity": -0.497245497648426,
-            "velocityX": 1.2129351368768808,
-            "velocityY": -1.5321909214351659,
-            "timestamp": 0.23496262679778424
-        },
-        {
-            "x": 1.6666605759505058,
-            "y": 5.159991667949077,
-            "heading": -0.1298174067167751,
-            "angularVelocity": -0.662953280613991,
-            "velocityX": 1.6172528080412967,
-            "velocityY": -2.042917103441561,
-            "timestamp": 0.31328350239704567
-        },
-        {
-            "x": 1.8249920311985446,
-            "y": 4.959988411604181,
-            "heading": -0.19472272142711608,
-            "angularVelocity": -0.828710279522869,
-            "velocityX": 2.0215741210320677,
-            "velocityY": -2.5536391774810774,
-            "timestamp": 0.3916043779963071
-        },
-        {
-            "x": 1.9833279596837488,
-            "y": 4.759991951944026,
-            "heading": -0.259798921868918,
-            "angularVelocity": -0.8308921464919682,
-            "velocityX": 2.021631235270526,
-            "velocityY": -2.553552397491847,
-            "timestamp": 0.4699252535955685
-        },
-        {
-            "x": 2.1099967561163506,
-            "y": 4.599995051908685,
-            "heading": -0.3118731002379449,
-            "angularVelocity": -0.6648824846579983,
-            "velocityX": 1.6173056731479674,
-            "velocityY": -2.042838500095239,
-            "timestamp": 0.54824612919483
-        },
-        {
-            "x": 2.2049983692603425,
-            "y": 4.4799975041635465,
-            "heading": -0.3509353765811358,
-            "angularVelocity": -0.4987466757018699,
-            "velocityX": 1.2129794568446812,
-            "velocityY": -1.532127249944463,
-            "timestamp": 0.6265670047940913
-        },
-        {
-            "x": 2.268332785802464,
-            "y": 4.399999173265512,
-            "heading": -0.3769787554180828,
-            "angularVelocity": -0.33252154853581545,
-            "velocityX": 0.808653070557838,
-            "velocityY": -1.021417729129522,
-            "timestamp": 0.7048878803933527
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.16625509460009477,
-            "velocityX": 0.40432661094808764,
-            "velocityY": -0.5107089643656818,
-            "timestamp": 0.7832087559926141
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -5.703180031220666e-32,
-            "velocityX": 1.0257416304055705e-32,
-            "velocityY": 0,
-            "timestamp": 0.8615296315918755
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.1.traj
deleted file mode 100644
index 66955466..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.238209882810899,
-            "y": 6.650800869780916,
-            "heading": 0.2470077179050236,
-            "angularVelocity": -0.14636941117769697,
-            "velocityX": 0.7008486923098889,
-            "velocityY": 0.23433997503541223,
-            "timestamp": 0.08876364255722656
-        },
-        {
-            "x": 5.362652939631404,
-            "y": 6.692404642961414,
-            "heading": 0.22258646311132924,
-            "angularVelocity": -0.2751267759001645,
-            "velocityX": 1.401959780326283,
-            "velocityY": 0.4687028605622118,
-            "timestamp": 0.17752728511445312
-        },
-        {
-            "x": 5.549362223961953,
-            "y": 6.754812642679732,
-            "heading": 0.18937073322571307,
-            "angularVelocity": -0.37420422291202055,
-            "velocityX": 2.1034432449090223,
-            "velocityY": 0.7030806523975865,
-            "timestamp": 0.26629092767167967
-        },
-        {
-            "x": 5.798381050208957,
-            "y": 6.8380191875714225,
-            "heading": 0.15275465831989177,
-            "angularVelocity": -0.4125120809759026,
-            "velocityX": 2.8054146841303256,
-            "velocityY": 0.9373944386978721,
-            "timestamp": 0.35505457022890624
-        },
-        {
-            "x": 6.109622945204874,
-            "y": 6.941929047341865,
-            "heading": 0.1302624797347834,
-            "angularVelocity": -0.25339404667863696,
-            "velocityX": 3.506411927553799,
-            "velocityY": 1.1706353725121932,
-            "timestamp": 0.4438182127861328
-        },
-        {
-            "x": 6.4280450770381,
-            "y": 7.048515369175043,
-            "heading": 0.1302624686930749,
-            "angularVelocity": -1.2439475582812342e-7,
-            "velocityX": 3.587303570016191,
-            "velocityY": 1.2007880564873126,
-            "timestamp": 0.5325818553433593
-        },
-        {
-            "x": 6.746467208870004,
-            "y": 7.1551016910121685,
-            "heading": 0.13026245765136438,
-            "angularVelocity": -1.2439477842907252e-7,
-            "velocityX": 3.5873035700012967,
-            "velocityY": 1.2007880565318134,
-            "timestamp": 0.6213454979005859
-        },
-        {
-            "x": 7.057671454315644,
-            "y": 7.258999412253756,
-            "heading": 0.10797351007767278,
-            "angularVelocity": -0.2511044717394208,
-            "velocityX": 3.5059877724700192,
-            "velocityY": 1.1704986213770205,
-            "timestamp": 0.7101091404578125
-        },
-        {
-            "x": 7.3066745305511045,
-            "y": 7.342200909102172,
-            "heading": 0.07025614763700679,
-            "angularVelocity": -0.4249190474112408,
-            "velocityX": 2.8052372464872497,
-            "velocityY": 0.9373375680789704,
-            "timestamp": 0.7988727830150391
-        },
-        {
-            "x": 7.49336787770315,
-            "y": 7.4046033027166684,
-            "heading": 0.03585076588111199,
-            "angularVelocity": -0.3876066908033862,
-            "velocityX": 2.103263698666567,
-            "velocityY": 0.7030174947390144,
-            "timestamp": 0.8876364255722656
-        },
-        {
-            "x": 7.617797738081694,
-            "y": 7.446202129881464,
-            "heading": 0.010501179707511226,
-            "angularVelocity": -0.2855852401136722,
-            "velocityX": 1.4018111108759532,
-            "velocityY": 0.4686471393732002,
-            "timestamp": 0.9764000681294922
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -0.003,
-            "angularVelocity": -0.15210258748116404,
-            "velocityX": 0.7007628362953491,
-            "velocityY": 0.23430618121741137,
-            "timestamp": 1.0651637106867187
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -0.003,
-            "angularVelocity": -3.7910464085672074e-31,
-            "velocityX": 1.2889893239032156e-32,
-            "velocityY": 1.1630630989344696e-31,
-            "timestamp": 1.1539273532439451
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.1.traj
deleted file mode 100644
index 63556f1f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.242063425541118,
-            "y": 6.625917379024557,
-            "heading": 0.21680591849920633,
-            "angularVelocity": -0.48090771817524164,
-            "velocityX": 0.735526954803639,
-            "velocityY": -0.04545446665971497,
-            "timestamp": 0.08981781715770647
-        },
-        {
-            "x": 5.374390185654174,
-            "y": 6.616899293663565,
-            "heading": 0.13592925245739512,
-            "angularVelocity": -0.9004523668149724,
-            "velocityX": 1.473279626476675,
-            "velocityY": -0.10040419202302918,
-            "timestamp": 0.17963563431541293
-        },
-        {
-            "x": 5.573225936717345,
-            "y": 6.601485332040172,
-            "heading": 0.027174297785337913,
-            "angularVelocity": -1.2108394315695736,
-            "velocityX": 2.2137673499017074,
-            "velocityY": -0.1716136298027423,
-            "timestamp": 0.2694534514731194
-        },
-        {
-            "x": 5.8386201791233034,
-            "y": 6.576536765329541,
-            "heading": -0.0884788299359431,
-            "angularVelocity": -1.287641265186969,
-            "velocityX": 2.9548061932964313,
-            "velocityY": -0.27776857087079165,
-            "timestamp": 0.35927126863082587
-        },
-        {
-            "x": 6.16462893780028,
-            "y": 6.53182752327052,
-            "heading": -0.12460373299787371,
-            "angularVelocity": -0.4022019706680331,
-            "velocityX": 3.6296669078981743,
-            "velocityY": -0.49777698316268804,
-            "timestamp": 0.44908908578853235
-        },
-        {
-            "x": 6.485747891959683,
-            "y": 6.420787077377064,
-            "heading": -0.12460373462910332,
-            "angularVelocity": -1.816153684215185e-8,
-            "velocityX": 3.5752255434527758,
-            "velocityY": -1.2362852873443355,
-            "timestamp": 0.5389069029462388
-        },
-        {
-            "x": 6.7868856142210525,
-            "y": 6.263423057279399,
-            "heading": -0.12460372221510443,
-            "angularVelocity": 1.382130993675135e-7,
-            "velocityX": 3.352761532075727,
-            "velocityY": -1.7520356770790513,
-            "timestamp": 0.6287247201039452
-        },
-        {
-            "x": 7.0844936481565455,
-            "y": 6.108042080528118,
-            "heading": -0.11377963349551397,
-            "angularVelocity": 0.12051159850149781,
-            "velocityX": 3.313463223148008,
-            "velocityY": -1.7299571696165401,
-            "timestamp": 0.7185425372616516
-        },
-        {
-            "x": 7.322771112714566,
-            "y": 5.983596379959823,
-            "heading": -0.07830444960996434,
-            "angularVelocity": 0.39496822577262786,
-            "velocityX": 2.652897521876331,
-            "velocityY": -1.3855346801602506,
-            "timestamp": 0.808360354419358
-        },
-        {
-            "x": 7.501421172621153,
-            "y": 5.8902827497271355,
-            "heading": -0.04357887494407857,
-            "angularVelocity": 0.3866223402525239,
-            "velocityX": 1.9890269610193678,
-            "velocityY": -1.038921153793384,
-            "timestamp": 0.8981781715770645
-        },
-        {
-            "x": 7.62048387761172,
-            "y": 5.828089668977501,
-            "heading": -0.01724918093308134,
-            "angularVelocity": 0.29314555668577735,
-            "velocityX": 1.3256022998366965,
-            "velocityY": -0.6924358965486016,
-            "timestamp": 0.9879959887347709
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -0.003,
-            "angularVelocity": 0.15864537108558247,
-            "velocityX": 0.6626315832612425,
-            "velocityY": -0.346141444552285,
-            "timestamp": 1.0778138058924773
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -0.003,
-            "angularVelocity": 0,
-            "velocityX": 4.386752331989148e-33,
-            "velocityY": -2.33652586669764e-33,
-            "timestamp": 1.1676316230501838
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3.1.traj
deleted file mode 100644
index b1b40339..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3.1.traj	
+++ /dev/null
@@ -1,157 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.246662494261999,
-            "y": 6.628692732737936,
-            "heading": 0.18398762968545185,
-            "angularVelocity": -0.8088463398355284,
-            "velocityX": 0.7519183997414132,
-            "velocityY": -0.013910608703967415,
-            "timestamp": 0.09397628025368156
-        },
-        {
-            "x": 5.3885394117424426,
-            "y": 6.6228569584156665,
-            "heading": 0.04133864379940616,
-            "angularVelocity": -1.5179254328962157,
-            "velocityX": 1.509709866122152,
-            "velocityY": -0.0620983753189278,
-            "timestamp": 0.18795256050736311
-        },
-        {
-            "x": 5.602094427902648,
-            "y": 6.606624752166298,
-            "heading": -0.14949344735311892,
-            "angularVelocity": -2.0306410366252936,
-            "velocityX": 2.272435295201409,
-            "velocityY": -0.172726630651381,
-            "timestamp": 0.2819288407610447
-        },
-        {
-            "x": 5.885007714232348,
-            "y": 6.565697735942791,
-            "heading": -0.3456589598447775,
-            "angularVelocity": -2.087393882393782,
-            "velocityX": 3.010475468554385,
-            "velocityY": -0.4355036836213049,
-            "timestamp": 0.37590512101472623
-        },
-        {
-            "x": 6.196762938930662,
-            "y": 6.464967292979978,
-            "heading": -0.4305176558630451,
-            "angularVelocity": -0.9029799412064233,
-            "velocityX": 3.3173820442430326,
-            "velocityY": -1.0718709305252299,
-            "timestamp": 0.4698814012684078
-        },
-        {
-            "x": 6.507653773495931,
-            "y": 6.294193106326001,
-            "heading": -0.4331106719684992,
-            "angularVelocity": -0.027592240280786797,
-            "velocityX": 3.3081840835372915,
-            "velocityY": -1.8172052159649787,
-            "timestamp": 0.5638576815220894
-        },
-        {
-            "x": 6.777249226823148,
-            "y": 6.0624534109685,
-            "heading": -0.433110646067504,
-            "angularVelocity": 2.7561204856502443e-7,
-            "velocityX": 2.8687606340606973,
-            "velocityY": -2.465938157287546,
-            "timestamp": 0.6578339617757709
-        },
-        {
-            "x": 6.99331744885121,
-            "y": 5.780142381909384,
-            "heading": -0.4331105937998759,
-            "angularVelocity": 5.561789419211182e-7,
-            "velocityX": 2.2991782761011916,
-            "velocityY": -3.004066859180208,
-            "timestamp": 0.7518102420294525
-        },
-        {
-            "x": 7.1465883664001515,
-            "y": 5.459372959434879,
-            "heading": -0.43311054984028735,
-            "angularVelocity": 4.677732342413875e-7,
-            "velocityX": 1.6309532270823943,
-            "velocityY": -3.4133019694822235,
-            "timestamp": 0.845786522283134
-        },
-        {
-            "x": 7.2797482024400875,
-            "y": 5.1297467242247805,
-            "heading": -0.4331105067287741,
-            "angularVelocity": 4.5874887908824285e-7,
-            "velocityX": 1.4169515507581365,
-            "velocityY": -3.507547163180951,
-            "timestamp": 0.9397628025368155
-        },
-        {
-            "x": 7.407925031698652,
-            "y": 4.799268903667217,
-            "heading": -0.4305589051420175,
-            "angularVelocity": 0.027151549091629798,
-            "velocityX": 1.3639274603395894,
-            "velocityY": -3.5166088683810885,
-            "timestamp": 1.033739082790497
-        },
-        {
-            "x": 7.517390810231398,
-            "y": 4.529437032753176,
-            "heading": -0.29559925755962413,
-            "angularVelocity": 1.4361033147734772,
-            "velocityX": 1.164823487770018,
-            "velocityY": -2.871276349581525,
-            "timestamp": 1.1277153630441787
-        },
-        {
-            "x": 7.59899827381295,
-            "y": 4.327770783855267,
-            "heading": -0.16069313013903275,
-            "angularVelocity": 1.4355338076419173,
-            "velocityX": 0.8683836321384404,
-            "velocityY": -2.1459271249460685,
-            "timestamp": 1.2216916432978604
-        },
-        {
-            "x": 7.653087401223331,
-            "y": 4.193799869992724,
-            "heading": -0.05829734753961687,
-            "angularVelocity": 1.0895917812772173,
-            "velocityX": 0.5755614849233384,
-            "velocityY": -1.4255822160751557,
-            "timestamp": 1.315667923551542
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -0.003,
-            "angularVelocity": 0.5884181347713066,
-            "velocityX": 0.28637650590148805,
-            "velocityY": -0.7108162805806236,
-            "timestamp": 1.4096442038052237
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -0.003,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.8051389340682807e-33,
-            "timestamp": 1.5036204840589054
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C4.1.traj
deleted file mode 100644
index 3f479acb..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C4.1.traj	
+++ /dev/null
@@ -1,247 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 0,
-            "velocityX": -1.7123315632321391e-31,
-            "velocityY": 4.571975133912053e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 5.2081944432473115,
-            "y": 6.63354239454729,
-            "heading": 0.2524699257138384,
-            "angularVelocity": -0.12069194203891866,
-            "velocityX": 0.5160121574790498,
-            "velocityY": 0.0567774581143745,
-            "timestamp": 0.062390861883169
-        },
-        {
-            "x": 5.2726441787772975,
-            "y": 6.640106281427696,
-            "heading": 0.23785051964847434,
-            "angularVelocity": -0.2343196683632906,
-            "velocityX": 1.0329996025807835,
-            "velocityY": 0.1052059016703473,
-            "timestamp": 0.124781723766338
-        },
-        {
-            "x": 5.369418980987113,
-            "y": 6.648966513737627,
-            "heading": 0.21675536507543205,
-            "angularVelocity": -0.33811288923278293,
-            "velocityX": 1.551105390898958,
-            "velocityY": 0.14201169918963874,
-            "timestamp": 0.187172585649507
-        },
-        {
-            "x": 5.498590594297912,
-            "y": 6.659046571836944,
-            "heading": 0.19009632617593986,
-            "angularVelocity": -0.42729076173705777,
-            "velocityX": 2.070361097955043,
-            "velocityY": 0.16156305258605722,
-            "timestamp": 0.249563447532676
-        },
-        {
-            "x": 5.6601948909824875,
-            "y": 6.668590441412665,
-            "heading": 0.15936442465931713,
-            "angularVelocity": -0.49257055583183496,
-            "velocityX": 2.5901917653772815,
-            "velocityY": 0.15296902924013772,
-            "timestamp": 0.311954309415845
-        },
-        {
-            "x": 5.854024302844021,
-            "y": 6.674275452040902,
-            "heading": 0.12740102584180835,
-            "angularVelocity": -0.512308980077189,
-            "velocityX": 3.106695532183737,
-            "velocityY": 0.09111928344386595,
-            "timestamp": 0.374345171299014
-        },
-        {
-            "x": 6.078018113514707,
-            "y": 6.668089747759068,
-            "heading": 0.10126004011963459,
-            "angularVelocity": -0.4189874115076864,
-            "velocityX": 3.590170161299073,
-            "velocityY": -0.09914439543117176,
-            "timestamp": 0.436736033182183
-        },
-        {
-            "x": 6.311203816285634,
-            "y": 6.63161682590639,
-            "heading": 0.10125992071184643,
-            "angularVelocity": -0.000001913866624365403,
-            "velocityX": 3.737497699704501,
-            "velocityY": -0.584587562213517,
-            "timestamp": 0.499126895065352
-        },
-        {
-            "x": 6.53718081427167,
-            "y": 6.563497401833266,
-            "heading": 0.10125989664992657,
-            "angularVelocity": -3.856641685769623e-7,
-            "velocityX": 3.621956664249834,
-            "velocityY": -1.0918173273625358,
-            "timestamp": 0.561517756948521
-        },
-        {
-            "x": 6.7516755548198235,
-            "y": 6.465019697743108,
-            "heading": 0.10125985817262735,
-            "angularVelocity": -6.167136938553595e-7,
-            "velocityX": 3.4379191771674527,
-            "velocityY": -1.5783994821960954,
-            "timestamp": 0.62390861883169
-        },
-        {
-            "x": 6.950631618499756,
-            "y": 6.338046073913574,
-            "heading": 0.10125971935519859,
-            "angularVelocity": -0.000002224964167076665,
-            "velocityX": 3.1888654471946203,
-            "velocityY": -2.035131748416977,
-            "timestamp": 0.686299480714859
-        },
-        {
-            "x": 7.196058677643184,
-            "y": 6.101503366378845,
-            "heading": 0.10125966337035743,
-            "angularVelocity": -6.213285755242982e-7,
-            "velocityX": 2.723788105663008,
-            "velocityY": -2.625188174087521,
-            "timestamp": 0.7764045291337008
-        },
-        {
-            "x": 7.389899318639429,
-            "y": 5.821123760827136,
-            "heading": 0.10125964077104323,
-            "angularVelocity": -2.508107435019506e-7,
-            "velocityX": 2.1512739230237647,
-            "velocityY": -3.1116969633976366,
-            "timestamp": 0.8665095775525427
-        },
-        {
-            "x": 7.524507781478929,
-            "y": 5.507966472295818,
-            "heading": 0.101259626680438,
-            "angularVelocity": -1.5637975290365853e-7,
-            "velocityX": 1.4939058931947202,
-            "velocityY": -3.475468844716046,
-            "timestamp": 0.9566146259713845
-        },
-        {
-            "x": 7.594574766806078,
-            "y": 5.174383579102459,
-            "heading": 0.10125961555168732,
-            "angularVelocity": -1.235086255399969e-7,
-            "velocityX": 0.7776144240159958,
-            "velocityY": -3.702155418004344,
-            "timestamp": 1.0467196743902263
-        },
-        {
-            "x": 7.605242469312774,
-            "y": 4.833688510035603,
-            "heading": 0.10125960514477245,
-            "angularVelocity": -1.1549757817967306e-7,
-            "velocityX": 0.11839184034432584,
-            "velocityY": -3.781087464524514,
-            "timestamp": 1.136824722809068
-        },
-        {
-            "x": 7.615908675824772,
-            "y": 4.492993394135096,
-            "heading": 0.10125959473787158,
-            "angularVelocity": -1.1549742264017462e-7,
-            "velocityX": 0.11837523756082047,
-            "velocityY": -3.781087984291716,
-            "timestamp": 1.2269297712279097
-        },
-        {
-            "x": 7.6265748461946865,
-            "y": 4.152298277103123,
-            "heading": 0.10125958433097336,
-            "angularVelocity": -1.1549739343149193e-7,
-            "velocityX": 0.11837483645025074,
-            "velocityY": -3.781087996848918,
-            "timestamp": 1.3170348196467514
-        },
-        {
-            "x": 7.637240986091898,
-            "y": 3.8116031591171406,
-            "heading": 0.10125957392407539,
-            "angularVelocity": -1.1549739036497336e-7,
-            "velocityX": 0.11837449825941278,
-            "velocityY": -3.78108800743665,
-            "timestamp": 1.407139868065593
-        },
-        {
-            "x": 7.647907101492937,
-            "y": 3.470908040364265,
-            "heading": 0.10125956351717343,
-            "angularVelocity": -1.1549743491773867e-7,
-            "velocityX": 0.11837422639702805,
-            "velocityY": -3.781088015947766,
-            "timestamp": 1.4972449164844348
-        },
-        {
-            "x": 7.658649980665503,
-            "y": 3.1330359890940187,
-            "heading": 0.09207038783752043,
-            "angularVelocity": -0.10198291706072676,
-            "velocityX": 0.11922616280763662,
-            "velocityY": -3.7497571689844853,
-            "timestamp": 1.5873499649032765
-        },
-        {
-            "x": 7.667206120655169,
-            "y": 2.862563871388905,
-            "heading": 0.061604548821908564,
-            "angularVelocity": -0.33811467337536444,
-            "velocityX": 0.09495738740298482,
-            "velocityY": -3.0017421049246735,
-            "timestamp": 1.6774550133221182
-        },
-        {
-            "x": 7.673608699461006,
-            "y": 2.659753859006467,
-            "heading": 0.031813701428875164,
-            "angularVelocity": -0.33062351017837394,
-            "velocityX": 0.07105682665061783,
-            "velocityY": -2.250817417462574,
-            "timestamp": 1.76756006174096
-        },
-        {
-            "x": 7.677870998153504,
-            "y": 2.524576540197495,
-            "heading": 0.009226184845951672,
-            "angularVelocity": -0.25067981183391225,
-            "velocityX": 0.04730366130750142,
-            "velocityY": -1.5002191462193974,
-            "timestamp": 1.8576651101598016
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -0.003,
-            "angularVelocity": -0.1356881224803119,
-            "velocityX": 0.023627997363707604,
-            "velocityY": -0.7499750722442785,
-            "timestamp": 1.9477701585786433
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -0.003,
-            "angularVelocity": 0,
-            "velocityX": -1.899719241029588e-32,
-            "velocityY": -4.254972095731821e-34,
-            "timestamp": 2.037875206997485
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C5.1.traj
deleted file mode 100644
index 7f9f6ab6..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C5.1.traj	
+++ /dev/null
@@ -1,292 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": -8.010592837331029e-32,
-            "velocityX": 9.396271853657066e-34,
-            "velocityY": -2.4298772375808502e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 5.207063492876026,
-            "y": 6.6289176408913075,
-            "heading": 0.24374543730705361,
-            "angularVelocity": -0.264834788594711,
-            "velocityX": 0.5061159579768882,
-            "velocityY": -0.01763482359684202,
-            "timestamp": 0.06137623678217253
-        },
-        {
-            "x": 5.269220913885746,
-            "y": 6.626580685647671,
-            "heading": 0.2121773997323976,
-            "angularVelocity": -0.514336479877237,
-            "velocityX": 1.0127277961065038,
-            "velocityY": -0.03807589657100854,
-            "timestamp": 0.12275247356434506
-        },
-        {
-            "x": 5.36250981468421,
-            "y": 6.6227475288112485,
-            "heading": 0.16662318019476377,
-            "angularVelocity": -0.7422126530713462,
-            "velocityX": 1.5199514615005028,
-            "velocityY": -0.06245343535846346,
-            "timestamp": 0.1841287103465176
-        },
-        {
-            "x": 5.486975524936631,
-            "y": 6.617054798693029,
-            "heading": 0.10910232843404971,
-            "angularVelocity": -0.9371844019185364,
-            "velocityX": 2.0279136808950478,
-            "velocityY": -0.0927513711605005,
-            "timestamp": 0.24550494712869012
-        },
-        {
-            "x": 5.642662315095453,
-            "y": 6.608892934358269,
-            "heading": 0.04300979987381644,
-            "angularVelocity": -1.076842309423355,
-            "velocityX": 2.536597196588689,
-            "velocityY": -0.1329808532205177,
-            "timestamp": 0.30688118391086266
-        },
-        {
-            "x": 5.829535978248522,
-            "y": 6.597025931717244,
-            "heading": -0.024847542112422006,
-            "angularVelocity": -1.1055963275666998,
-            "velocityX": 3.0447233807490215,
-            "velocityY": -0.19334848897856358,
-            "timestamp": 0.3682574206930352
-        },
-        {
-            "x": 6.046434546533161,
-            "y": 6.5777851666622515,
-            "heading": -0.07267029178585922,
-            "angularVelocity": -0.7791737027338695,
-            "velocityX": 3.533917679808606,
-            "velocityY": -0.31348883645767445,
-            "timestamp": 0.4296336574752077
-        },
-        {
-            "x": 6.274079716361941,
-            "y": 6.532107529947078,
-            "heading": -0.07267036448034916,
-            "angularVelocity": -0.0000011844077407082938,
-            "velocityX": 3.709011528952235,
-            "velocityY": -0.7442234830605138,
-            "timestamp": 0.49100989425738023
-        },
-        {
-            "x": 6.493476634626264,
-            "y": 6.456122214314626,
-            "heading": -0.07267035803800323,
-            "angularVelocity": 1.049648246960038e-7,
-            "velocityX": 3.5746231728571716,
-            "velocityY": -1.238025001469686,
-            "timestamp": 0.5523861310395528
-        },
-        {
-            "x": 6.700610073343003,
-            "y": 6.351219816335689,
-            "heading": -0.07267035070446132,
-            "angularVelocity": 1.1948503655702063e-7,
-            "velocityX": 3.3748149051866063,
-            "velocityY": -1.7091695985083282,
-            "timestamp": 0.6137623678217253
-        },
-        {
-            "x": 6.891689244621814,
-            "y": 6.21932014936447,
-            "heading": -0.07267034149477326,
-            "angularVelocity": 1.5005299351249212e-7,
-            "velocityX": 3.1132435173071933,
-            "velocityY": -2.1490347712151214,
-            "timestamp": 0.6751386046038979
-        },
-        {
-            "x": 7.0632171630859375,
-            "y": 6.06283712387085,
-            "heading": -0.07267032569899272,
-            "angularVelocity": 2.573598736854518e-7,
-            "velocityX": 2.7946959190881038,
-            "velocityY": -2.549570219643589,
-            "timestamp": 0.7365148413860704
-        },
-        {
-            "x": 7.263317671759933,
-            "y": 5.7907628781558955,
-            "heading": -0.07267031513885969,
-            "angularVelocity": 1.182832965996254e-7,
-            "velocityX": 2.24131152203967,
-            "velocityY": -3.0474842158680824,
-            "timestamp": 0.8257931530827405
-        },
-        {
-            "x": 7.406264443711533,
-            "y": 5.484771313458312,
-            "heading": -0.07267030869611313,
-            "angularVelocity": 7.216474459034548e-8,
-            "velocityX": 1.6011365944875051,
-            "velocityY": -3.4273896860551396,
-            "timestamp": 0.9150714647794106
-        },
-        {
-            "x": 7.486522259331217,
-            "y": 5.1567114044359705,
-            "heading": -0.07267030368970051,
-            "angularVelocity": 5.607647052205946e-8,
-            "velocityX": 0.8989620669840581,
-            "velocityY": -3.6745756364317224,
-            "timestamp": 1.0043497764760807
-        },
-        {
-            "x": 7.50149841248159,
-            "y": 4.81930906784098,
-            "heading": -0.07267029905812865,
-            "angularVelocity": 5.187790600157932e-8,
-            "velocityX": 0.1677468230047605,
-            "velocityY": -3.7792195011633143,
-            "timestamp": 1.093628088172751
-        },
-        {
-            "x": 7.516449925771596,
-            "y": 4.481905638472984,
-            "heading": -0.0726702944266145,
-            "angularVelocity": 5.1877259457701335e-8,
-            "velocityX": 0.16747083368699095,
-            "velocityY": -3.7792317412357557,
-            "timestamp": 1.1829063998694211
-        },
-        {
-            "x": 7.531401402222247,
-            "y": 4.144502207472513,
-            "heading": -0.07267028979509418,
-            "angularVelocity": 5.187732873980297e-8,
-            "velocityX": 0.16747042105201052,
-            "velocityY": -3.779231759520992,
-            "timestamp": 1.2721847115660914
-        },
-        {
-            "x": 7.546352847973806,
-            "y": 3.807098775111667,
-            "heading": -0.07267028516356967,
-            "angularVelocity": 5.1877375603275234e-8,
-            "velocityX": 0.16747007719365256,
-            "velocityY": -3.7792317747584647,
-            "timestamp": 1.3614630232627616
-        },
-        {
-            "x": 7.561304268557873,
-            "y": 3.4696953416355707,
-            "heading": -0.07267028053204275,
-            "angularVelocity": 5.1877402813012475e-8,
-            "velocityX": 0.16746979529438435,
-            "velocityY": -3.7792317872503007,
-            "timestamp": 1.4507413349594318
-        },
-        {
-            "x": 7.576255668582546,
-            "y": 3.132291907248425,
-            "heading": -0.07267027590051456,
-            "angularVelocity": 5.1877416880821484e-8,
-            "velocityX": 0.16746956501007867,
-            "velocityY": -3.779231797454896,
-            "timestamp": 1.540019646656102
-        },
-        {
-            "x": 7.591207051730301,
-            "y": 2.794888472113414,
-            "heading": -0.07267027126898577,
-            "angularVelocity": 5.1877423431260937e-8,
-            "velocityX": 0.16746937597291248,
-            "velocityY": -3.779231805831691,
-            "timestamp": 1.6292979583527722
-        },
-        {
-            "x": 7.60615842090353,
-            "y": 2.457485036359151,
-            "heading": -0.07267026663745696,
-            "angularVelocity": 5.1877423851232326e-8,
-            "velocityX": 0.16746921944524512,
-            "velocityY": -3.779231812767884,
-            "timestamp": 1.7185762700494425
-        },
-        {
-            "x": 7.621109778388665,
-            "y": 2.1200816000869556,
-            "heading": -0.07267026200592838,
-            "angularVelocity": 5.187742117887202e-8,
-            "velocityX": 0.1674690885277524,
-            "velocityY": -3.779231818569208,
-            "timestamp": 1.8078545817461127
-        },
-        {
-            "x": 7.636061125621916,
-            "y": 1.7826781633604734,
-            "heading": -0.07267025737439563,
-            "angularVelocity": 5.1877468094637746e-8,
-            "velocityX": 0.16746897369713376,
-            "velocityY": -3.7792318236576405,
-            "timestamp": 1.897132893442783
-        },
-        {
-            "x": 7.6506850878742485,
-            "y": 1.4507667606163608,
-            "heading": -0.05536549896524321,
-            "angularVelocity": 0.19382936438075055,
-            "velocityX": 0.16380195788222543,
-            "velocityY": -3.7177159428351065,
-            "timestamp": 1.9864112051394531
-        },
-        {
-            "x": 7.662404359721407,
-            "y": 1.1852476637582392,
-            "heading": -0.0353858682020255,
-            "angularVelocity": 0.2237904187872895,
-            "velocityX": 0.13126672788097452,
-            "velocityY": -2.974060461181691,
-            "timestamp": 2.0756895168361233
-        },
-        {
-            "x": 7.6711999602966845,
-            "y": 0.9861189087552604,
-            "heading": -0.018423007440779238,
-            "angularVelocity": 0.18999979321832794,
-            "velocityX": 0.0985188945458782,
-            "velocityY": -2.2304269784976873,
-            "timestamp": 2.1649678285327933
-        },
-        {
-            "x": 7.677066105120016,
-            "y": 0.8533716446291538,
-            "heading": -0.006322956528169105,
-            "angularVelocity": 0.13553180702747777,
-            "velocityX": 0.06570626966225444,
-            "velocityY": -1.4868926349898441,
-            "timestamp": 2.2542461402294633
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -2.444948224573009e-31,
-            "angularVelocity": 0.0708229849781762,
-            "velocityX": 0.03286234724008497,
-            "velocityY": -0.7434240563896022,
-            "timestamp": 2.3435244519261333
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 1.883272219480887e-30,
-            "velocityX": 0,
-            "velocityY": -3.281240907044904e-32,
-            "timestamp": 2.4328027636228033
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj
deleted file mode 100644
index 462787d1..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": -5.95142511939464e-31,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.1029790507443495,
-            "y": 6.633224483607701,
-            "heading": 0.26650125878935643,
-            "angularVelocity": 0.06942713064122713,
-            "velocityX": -0.7797928290155524,
-            "velocityY": 0.034434353705267144,
-            "timestamp": 0.09364147314336706
-        },
-        {
-            "x": 4.9569371541735245,
-            "y": 6.639673450192742,
-            "heading": 0.2795035687162854,
-            "angularVelocity": 0.13885204376302582,
-            "velocityX": -1.559585637308702,
-            "velocityY": 0.06886870067889261,
-            "timestamp": 0.18728294628673411
-        },
-        {
-            "x": 4.737874314376445,
-            "y": 6.649346898931416,
-            "heading": 0.2990065710588269,
-            "angularVelocity": 0.20827312608258508,
-            "velocityX": -2.339378401936154,
-            "velocityY": 0.1033030388561254,
-            "timestamp": 0.28092441943010116
-        },
-        {
-            "x": 4.445790546320906,
-            "y": 6.66224482863892,
-            "heading": 0.3250097249050781,
-            "angularVelocity": 0.2776884319880837,
-            "velocityX": -3.119171006722006,
-            "velocityY": 0.13773736438080994,
-            "timestamp": 0.37456589257346823
-        },
-        {
-            "x": 4.09189529815297,
-            "y": 6.677872415392626,
-            "heading": 0.32500972789735083,
-            "angularVelocity": 3.195452790688951e-8,
-            "velocityX": -3.7792575905562438,
-            "velocityY": 0.16688745092443205,
-            "timestamp": 0.4682073657168353
-        },
-        {
-            "x": 3.7380000499890724,
-            "y": 6.693500002240241,
-            "heading": 0.32500973088947493,
-            "angularVelocity": 3.1952970747065184e-8,
-            "velocityX": -3.7792575905130557,
-            "velocityY": 0.16688745192729154,
-            "timestamp": 0.5618488388602023
-        },
-        {
-            "x": 3.384104801825175,
-            "y": 6.7091275890878554,
-            "heading": 0.3250097338815991,
-            "angularVelocity": 3.195297127919844e-8,
-            "velocityX": -3.7792575905130557,
-            "velocityY": 0.1668874519272919,
-            "timestamp": 0.6554903120035693
-        },
-        {
-            "x": 3.030209553657211,
-            "y": 6.724755175840924,
-            "heading": 0.32500973687387263,
-            "angularVelocity": 3.195453683514771e-8,
-            "velocityX": -3.7792575905565435,
-            "velocityY": 0.16688745091763021,
-            "timestamp": 0.7491317851469363
-        },
-        {
-            "x": 2.7381257502813003,
-            "y": 6.7376531044804615,
-            "heading": 0.35100602630019134,
-            "angularVelocity": 0.2776151266490911,
-            "velocityX": -3.119171383909283,
-            "velocityY": 0.13773735297596457,
-            "timestamp": 0.8427732582903034
-        },
-        {
-            "x": 2.5190628797280783,
-            "y": 6.747326551971013,
-            "heading": 0.37050304728737704,
-            "angularVelocity": 0.20820925101574836,
-            "velocityX": -2.3393787303818834,
-            "velocityY": 0.10330302552738789,
-            "timestamp": 0.9364147314336704
-        },
-        {
-            "x": 2.3730209608288053,
-            "y": 6.7537755172838185,
-            "heading": 0.3835010053218344,
-            "angularVelocity": 0.13880556977745576,
-            "velocityX": -1.559585875754842,
-            "velocityY": 0.06886868709264785,
-            "timestamp": 1.0300562045770374
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0.06940295213233785,
-            "velocityX": -0.7797929526056125,
-            "velocityY": 0.03443434418469861,
-            "timestamp": 1.1236976777204044
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": -1.6940949553652625e-30,
-            "velocityX": -1.6586267397188983e-32,
-            "velocityY": 0,
-            "timestamp": 1.2173391508637714
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj
deleted file mode 100644
index 53d96783..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 3.363577949177338e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 5.118924970919473,
-            "y": 6.609128397315466,
-            "heading": 0.25122660793754975,
-            "angularVelocity": -0.10272459100864365,
-            "velocityX": -0.6682716305586017,
-            "velocityY": -0.24437832416494054,
-            "timestamp": 0.08540693106008139
-        },
-        {
-            "x": 5.0047749070529814,
-            "y": 6.567385203608531,
-            "heading": 0.23368024533248913,
-            "angularVelocity": -0.20544424658833893,
-            "velocityX": -1.3365433279201937,
-            "velocityY": -0.48875651178205104,
-            "timestamp": 0.17081386212016278
-        },
-        {
-            "x": 4.833549798383878,
-            "y": 6.504770434941582,
-            "heading": 0.20736177398264571,
-            "angularVelocity": -0.30815381167752187,
-            "velocityX": -2.004815142563213,
-            "velocityY": -0.7331345113302418,
-            "timestamp": 0.25622079318024416
-        },
-        {
-            "x": 4.605249631158782,
-            "y": 6.421284110658292,
-            "heading": 0.17227269285622732,
-            "angularVelocity": -0.4108458258702004,
-            "velocityX": -2.6730871182397737,
-            "velocityY": -0.977512284390117,
-            "timestamp": 0.34162772424032556
-        },
-        {
-            "x": 4.319874394275373,
-            "y": 6.316926253811125,
-            "heading": 0.12841549400557886,
-            "angularVelocity": -0.5135086614902031,
-            "velocityX": -3.3413592239095435,
-            "velocityY": -1.2218897875367427,
-            "timestamp": 0.42703465530040696
-        },
-        {
-            "x": 4.016437653324315,
-            "y": 6.205962904892466,
-            "heading": 0.1284154887581796,
-            "angularVelocity": -6.143996998796484e-8,
-            "velocityX": -3.552835082407984,
-            "velocityY": -1.2992311928477889,
-            "timestamp": 0.5124415863604883
-        },
-        {
-            "x": 3.713000912388581,
-            "y": 6.094999555931864,
-            "heading": 0.12841548351080107,
-            "angularVelocity": -6.143972697353832e-8,
-            "velocityX": -3.5528350822285746,
-            "velocityY": -1.299231193338901,
-            "timestamp": 0.5978485174205697
-        },
-        {
-            "x": 3.4095641714528466,
-            "y": 5.98403620697126,
-            "heading": 0.12841547826342262,
-            "angularVelocity": -6.143972595662007e-8,
-            "velocityX": -3.5528350822285746,
-            "velocityY": -1.299231193338901,
-            "timestamp": 0.683255448480651
-        },
-        {
-            "x": 3.106127430499178,
-            "y": 5.873072858059743,
-            "heading": 0.12841547301602332,
-            "angularVelocity": -6.143997022352915e-8,
-            "velocityX": -3.55283508243856,
-            "velocityY": -1.2992311927641775,
-            "timestamp": 0.7686623795407324
-        },
-        {
-            "x": 2.820751661400481,
-            "y": 5.768715245814204,
-            "heading": 0.08460804498823243,
-            "angularVelocity": -0.5129259122655233,
-            "velocityX": -3.3413654554329186,
-            "velocityY": -1.2218869235814813,
-            "timestamp": 0.8540693106008137
-        },
-        {
-            "x": 2.5924510107246115,
-            "y": 5.685229149936981,
-            "heading": 0.04956396734437731,
-            "angularVelocity": -0.41031889577208425,
-            "velocityX": -2.6730927787964465,
-            "velocityY": -0.977509610063062,
-            "timestamp": 0.939476241660895
-        },
-        {
-            "x": 2.4212255084791887,
-            "y": 5.622614575599846,
-            "heading": 0.02328185696555881,
-            "angularVelocity": -0.30772807373595673,
-            "velocityX": -2.00481975081121,
-            "velocityY": -0.7331322359901695,
-            "timestamp": 1.0248831727209764
-        },
-        {
-            "x": 2.307075169405903,
-            "y": 5.580871525201533,
-            "heading": 0.005760683198102726,
-            "angularVelocity": -0.20514931926462068,
-            "velocityX": -1.3365465502206675,
-            "velocityY": -0.48875483383130996,
-            "timestamp": 1.1102901037810577
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": -0.10257578734376759,
-            "velocityX": -0.6682732735795421,
-            "velocityY": -0.24437741694350015,
-            "timestamp": 1.195697034841139
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": 2.4865352537032555e-34,
-            "velocityX": 0,
-            "velocityY": -6.568641934425214e-34,
-            "timestamp": 1.2811039659012204
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.1.traj
deleted file mode 100644
index 359cb1db..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.1.traj	
+++ /dev/null
@@ -1,175 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 2.521971410376041e-31,
-            "velocityX": -3.46085485178236e-32,
-            "velocityY": -2.8734015412732535e-37,
-            "timestamp": 0
-        },
-        {
-            "x": 5.132269964046874,
-            "y": 6.606195610268081,
-            "heading": 0.16914444930535336,
-            "angularVelocity": -1.1148212857556328,
-            "velocityX": -0.5365789380469493,
-            "velocityY": -0.29208606589986635,
-            "timestamp": 0.08149786145594107
-        },
-        {
-            "x": 5.043666795437562,
-            "y": 6.5570487224925476,
-            "heading": 0.005335724612659364,
-            "angularVelocity": -2.0099757437347154,
-            "velocityX": -1.0871839705537814,
-            "velocityY": -0.6030451216453527,
-            "timestamp": 0.16299572291188213
-        },
-        {
-            "x": 4.909391168326261,
-            "y": 6.4806746643853055,
-            "heading": -0.2131663273161798,
-            "angularVelocity": -2.681077123072297,
-            "velocityX": -1.647596939508567,
-            "velocityY": -0.9371295975481627,
-            "timestamp": 0.2444935843678232
-        },
-        {
-            "x": 4.728031239930722,
-            "y": 6.375889843725219,
-            "heading": -0.4550458803770597,
-            "angularVelocity": -2.967925154596154,
-            "velocityX": -2.225333587355345,
-            "velocityY": -1.2857370584715861,
-            "timestamp": 0.32599144582376427
-        },
-        {
-            "x": 4.50844214888531,
-            "y": 6.243100931440797,
-            "heading": -0.5880935326141025,
-            "angularVelocity": -1.6325293677671568,
-            "velocityX": -2.6944153763362864,
-            "velocityY": -1.6293545611158133,
-            "timestamp": 0.40748930727970534
-        },
-        {
-            "x": 4.251218650398233,
-            "y": 6.083818287155582,
-            "heading": -0.6031902279140122,
-            "angularVelocity": -0.1852403858237583,
-            "velocityX": -3.15619936390767,
-            "velocityY": -1.9544395575499292,
-            "timestamp": 0.4889871687356464
-        },
-        {
-            "x": 3.9855391766757995,
-            "y": 5.92740773635831,
-            "heading": -0.6031903133870022,
-            "angularVelocity": -0.0000010487758630839914,
-            "velocityX": -3.259956383837921,
-            "velocityY": -1.9191982219291623,
-            "timestamp": 0.5704850301915875
-        },
-        {
-            "x": 3.719859695364488,
-            "y": 5.770997198451498,
-            "heading": -0.603190398859952,
-            "angularVelocity": -0.0000010487753700599598,
-            "velocityX": -3.259956476955436,
-            "velocityY": -1.919198063759863,
-            "timestamp": 0.6519828916475285
-        },
-        {
-            "x": 3.4541802321431443,
-            "y": 5.614586629816999,
-            "heading": -0.6031904843329001,
-            "angularVelocity": -0.000001048775349610677,
-            "velocityX": -3.259956254986808,
-            "velocityY": -1.9191984407966034,
-            "timestamp": 0.7334807531034696
-        },
-        {
-            "x": 3.1885009063173535,
-            "y": 5.458175827802239,
-            "heading": -0.6031905698058837,
-            "angularVelocity": -0.000001048775783641969,
-            "velocityX": -3.259954569107567,
-            "velocityY": -1.9192013044332228,
-            "timestamp": 0.8149786145594107
-        },
-        {
-            "x": 2.937386380849553,
-            "y": 5.279314604480798,
-            "heading": -0.603190661977047,
-            "angularVelocity": -0.0000011309641953921016,
-            "velocityX": -3.0812406728433848,
-            "velocityY": -2.1946738248847923,
-            "timestamp": 0.8964764760153517
-        },
-        {
-            "x": 2.722322578778117,
-            "y": 5.058413395221267,
-            "heading": -0.6031910253459357,
-            "angularVelocity": -0.00000445863096683889,
-            "velocityX": -2.6388889012468426,
-            "velocityY": -2.710515408787179,
-            "timestamp": 0.9779743374712928
-        },
-        {
-            "x": 2.561151389441887,
-            "y": 4.8410288515668665,
-            "heading": -0.5595876383179096,
-            "angularVelocity": 0.5350249227287813,
-            "velocityX": -1.9776124975176395,
-            "velocityY": -2.667365005300435,
-            "timestamp": 1.0594721989272338
-        },
-        {
-            "x": 2.4444023928247787,
-            "y": 4.656624455699537,
-            "heading": -0.5072145719014195,
-            "angularVelocity": 0.6426311743750899,
-            "velocityX": -1.432540615562337,
-            "velocityY": -2.262689996681954,
-            "timestamp": 1.1409700603831747
-        },
-        {
-            "x": 2.367465069003143,
-            "y": 4.510717874734717,
-            "heading": -0.4552059179368463,
-            "angularVelocity": 0.6381597386170672,
-            "velocityX": -0.9440410146618312,
-            "velocityY": -1.7903117745451358,
-            "timestamp": 1.2224679218391157
-        },
-        {
-            "x": 2.321242179529733,
-            "y": 4.410775659264178,
-            "heading": -0.4150276653385325,
-            "angularVelocity": 0.49299763061923907,
-            "velocityX": -0.5671669004271778,
-            "velocityY": -1.2263170307182782,
-            "timestamp": 1.3039657832950566
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.393,
-            "angularVelocity": 0.27028519454392347,
-            "velocityX": -0.26064707895700284,
-            "velocityY": -0.6230305723006956,
-            "timestamp": 1.3854636447509976
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.393,
-            "angularVelocity": -9.678618240747916e-31,
-            "velocityX": 0,
-            "velocityY": -3.108003047332484e-31,
-            "timestamp": 1.4669615062069385
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.1.traj
deleted file mode 100644
index 29c2d4bc..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 5.380258045140596e-28,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.304265282414897,
-            "y": 5.638698322322049,
-            "heading": -0.04261698283734242,
-            "angularVelocity": -0.41098530784979936,
-            "velocityX": 0.3979499173257931,
-            "velocityY": 0.7589428456312429,
-            "timestamp": 0.10369466261532896
-        },
-        {
-            "x": 4.38866009880019,
-            "y": 5.795292929312062,
-            "heading": -0.1228057028496872,
-            "angularVelocity": -0.7733157907057984,
-            "velocityX": 0.8138781134605525,
-            "velocityY": 1.510151082422871,
-            "timestamp": 0.20738932523065792
-        },
-        {
-            "x": 4.519821720835651,
-            "y": 6.028001091828693,
-            "heading": -0.23019023538729733,
-            "angularVelocity": -1.0355839908170448,
-            "velocityX": 1.264883058851596,
-            "velocityY": 2.2441672179396264,
-            "timestamp": 0.3110839878459869
-        },
-        {
-            "x": 4.707676475524912,
-            "y": 6.330157785456564,
-            "heading": -0.33387571773962255,
-            "angularVelocity": -0.9999114683169592,
-            "velocityX": 1.8116145031122473,
-            "velocityY": 2.9139078715050806,
-            "timestamp": 0.41477865046131585
-        },
-        {
-            "x": 4.978690211282069,
-            "y": 6.613755936139097,
-            "heading": -0.3338757185080882,
-            "angularVelocity": -7.410850442524386e-9,
-            "velocityX": 2.6135745941189206,
-            "velocityY": 2.7349348898948014,
-            "timestamp": 0.5184733130766448
-        },
-        {
-            "x": 5.307019127730767,
-            "y": 6.828410259978494,
-            "heading": -0.3338757060420003,
-            "angularVelocity": 1.2021918642580205e-7,
-            "velocityX": 3.166304881734213,
-            "velocityY": 2.070061451819255,
-            "timestamp": 0.6221679756919738
-        },
-        {
-            "x": 5.675511606586513,
-            "y": 6.962908031631864,
-            "heading": -0.3338756959696621,
-            "angularVelocity": 9.713458668099172e-8,
-            "velocityX": 3.5536301441350284,
-            "velocityY": 1.2970558779125847,
-            "timestamp": 0.7258626383073028
-        },
-        {
-            "x": 6.055844888125957,
-            "y": 7.058944236891981,
-            "heading": -0.3338756861984713,
-            "angularVelocity": 9.423041207795053e-8,
-            "velocityX": 3.6678192680981745,
-            "velocityY": 0.9261441508940896,
-            "timestamp": 0.8295573009226318
-        },
-        {
-            "x": 6.436176952400414,
-            "y": 7.154985262770466,
-            "heading": -0.3338756764272739,
-            "angularVelocity": 9.423047569354886e-8,
-            "velocityX": 3.667807529162388,
-            "velocityY": 0.9261906394812687,
-            "timestamp": 0.9332519635379608
-        },
-        {
-            "x": 6.8165090646020285,
-            "y": 7.2510260988513195,
-            "heading": -0.33387566665551793,
-            "angularVelocity": 9.423586238606553e-8,
-            "velocityX": 3.6678079913574164,
-            "velocityY": 0.9261888091302356,
-            "timestamp": 1.0369466261532898
-        },
-        {
-            "x": 7.162467192455402,
-            "y": 7.33732437680055,
-            "heading": -0.24283297058924916,
-            "angularVelocity": 0.8779882567727307,
-            "velocityX": 3.336315670718354,
-            "velocityY": 0.8322345217454595,
-            "timestamp": 1.1406412887686188
-        },
-        {
-            "x": 7.421550289934561,
-            "y": 7.4021983509049765,
-            "heading": -0.1315877257451376,
-            "angularVelocity": 1.0728155339758783,
-            "velocityX": 2.4985191228237835,
-            "velocityY": 0.6256250077700213,
-            "timestamp": 1.2443359513839478
-        },
-        {
-            "x": 7.593930847137365,
-            "y": 7.445412549605325,
-            "heading": -0.046073342510862675,
-            "angularVelocity": 0.8246748779298502,
-            "velocityX": 1.662386017323544,
-            "velocityY": 0.41674467721309977,
-            "timestamp": 1.3480306139992768
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0.44431739637147044,
-            "velocityX": 0.8300249086292989,
-            "velocityY": 0.20818285001567757,
-            "timestamp": 1.4517252766146058
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 6.677691966309128e-30,
-            "velocityY": 0,
-            "timestamp": 1.5554199392299348
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2.1.traj
deleted file mode 100644
index 05a18e04..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2.1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.272659932573731,
-            "y": 5.605321030989923,
-            "heading": -5.269773313916661e-8,
-            "angularVelocity": -7.069117507058631e-7,
-            "velocityX": 0.1295790656325301,
-            "velocityY": 0.6079397351586845,
-            "timestamp": 0.07454855863477708
-        },
-        {
-            "x": 4.294668340488188,
-            "y": 5.695305527660525,
-            "heading": -1.390931476532318e-7,
-            "angularVelocity": -0.0000011589489187991593,
-            "velocityX": 0.2952224471421504,
-            "velocityY": 1.2070588395619357,
-            "timestamp": 0.14909711726955416
-        },
-        {
-            "x": 4.333068049168991,
-            "y": 5.8286332482919585,
-            "heading": -2.3393980900082764e-7,
-            "angularVelocity": -0.0000012723197345504802,
-            "velocityX": 0.5150965944736963,
-            "velocityY": 1.7884681217524945,
-            "timestamp": 0.22364567590433124
-        },
-        {
-            "x": 4.394361213548298,
-            "y": 6.0022499148295365,
-            "heading": -3.0377830712505715e-7,
-            "angularVelocity": -9.368513338342214e-7,
-            "velocityX": 0.8221911395208825,
-            "velocityY": 2.3289070872699873,
-            "timestamp": 0.2981942345391083
-        },
-        {
-            "x": 4.489214393007107,
-            "y": 6.207820209268236,
-            "heading": -3.104589480689307e-7,
-            "angularVelocity": -8.962358439949817e-8,
-            "velocityX": 1.2723677183517785,
-            "velocityY": 2.75753546703125,
-            "timestamp": 0.37274279317388537
-        },
-        {
-            "x": 4.629403012529928,
-            "y": 6.42298249437822,
-            "heading": -2.4199647648725644e-7,
-            "angularVelocity": 9.18393943226279e-7,
-            "velocityX": 1.880500737963715,
-            "velocityY": 2.8862031546566858,
-            "timestamp": 0.4472913518086624
-        },
-        {
-            "x": 4.811531801184116,
-            "y": 6.618438666310206,
-            "heading": -1.472447710538824e-7,
-            "angularVelocity": 0.0000012710752897138218,
-            "velocityX": 2.443089336474201,
-            "velocityY": 2.6218638627509074,
-            "timestamp": 0.5218399104434395
-        },
-        {
-            "x": 5.024847227590037,
-            "y": 6.779620899070013,
-            "heading": -6.254572131926062e-8,
-            "angularVelocity": 0.000001136235548213432,
-            "velocityX": 2.861429252247402,
-            "velocityY": 2.1621106520395927,
-            "timestamp": 0.5963884690782165
-        },
-        {
-            "x": 5.261847496032715,
-            "y": 6.900974273681641,
-            "heading": 1.6289151195668303e-9,
-            "angularVelocity": 8.609140934997053e-7,
-            "velocityX": 3.1791395133448552,
-            "velocityY": 1.6278433384167708,
-            "timestamp": 0.6709370277129936
-        },
-        {
-            "x": 5.5631639462020495,
-            "y": 6.984947323321856,
-            "heading": 4.965855378058324e-8,
-            "angularVelocity": 5.539886921154719e-7,
-            "velocityX": 3.4757171947403775,
-            "velocityY": 0.9686380297960585,
-            "timestamp": 0.7576289054784
-        },
-        {
-            "x": 5.883696640724643,
-            "y": 7.009274337947639,
-            "heading": 7.188189606013307e-8,
-            "angularVelocity": 2.563216327096045e-7,
-            "velocityX": 3.697378610459799,
-            "velocityY": 0.2806146929817695,
-            "timestamp": 0.8443207832438064
-        },
-        {
-            "x": 6.209423466668327,
-            "y": 6.971151903870608,
-            "heading": 7.193737396357795e-8,
-            "angularVelocity": 6.438452889791163e-10,
-            "velocityX": 3.7572934667332714,
-            "velocityY": -0.4397463179458525,
-            "timestamp": 0.9310126610092129
-        },
-        {
-            "x": 6.521952745942683,
-            "y": 6.871769894938554,
-            "heading": 7.193784305655878e-8,
-            "angularVelocity": 5.630908705768223e-12,
-            "velocityX": 3.6050583669434406,
-            "velocityY": -1.1463820079014435,
-            "timestamp": 1.0177045387746193
-        },
-        {
-            "x": 6.809873374118548,
-            "y": 6.714756988695251,
-            "heading": 7.193831945924844e-8,
-            "angularVelocity": 5.91084758847972e-12,
-            "velocityX": 3.321194967875227,
-            "velocityY": -1.8111605177424126,
-            "timestamp": 1.1043964165400257
-        },
-        {
-            "x": 7.062672913559571,
-            "y": 6.505846154651799,
-            "heading": 7.193893641048392e-8,
-            "angularVelocity": 7.48396872358976e-12,
-            "velocityX": 2.9160694861627596,
-            "velocityY": -2.4098086166543093,
-            "timestamp": 1.1910882943054322
-        },
-        {
-            "x": 7.268448732261103,
-            "y": 6.269564240975314,
-            "heading": 6.958123303548702e-8,
-            "angularVelocity": -2.7156604096657526e-8,
-            "velocityX": 2.3736458816624144,
-            "velocityY": -2.7255369226670942,
-            "timestamp": 1.2777801720708386
-        },
-        {
-            "x": 7.433069281193563,
-            "y": 6.080538590855167,
-            "heading": 5.4913746267249155e-8,
-            "angularVelocity": -1.6915879682512025e-7,
-            "velocityX": 1.8989154825033356,
-            "velocityY": -2.180430912690726,
-            "timestamp": 1.364472049836245
-        },
-        {
-            "x": 7.556534655605292,
-            "y": 5.9387693118426395,
-            "heading": 3.4027865091828515e-8,
-            "angularVelocity": -2.408962130800618e-7,
-            "velocityX": 1.424186181835385,
-            "velocityY": -1.635323662267605,
-            "timestamp": 1.4511639276016515
-        },
-        {
-            "x": 7.638844889158882,
-            "y": 5.8442564415645775,
-            "heading": 1.3493455787871544e-8,
-            "angularVelocity": -2.368497714278514e-7,
-            "velocityX": 0.9494572693998051,
-            "velocityY": -1.0902159778725726,
-            "timestamp": 1.537855805367058
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -2.563219583366834e-29,
-            "angularVelocity": -1.5563981959470815e-7,
-            "velocityX": 0.4747285662416774,
-            "velocityY": -0.5451080630467725,
-            "timestamp": 1.6245476831324643
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 4.328374921762355e-28,
-            "velocityX": 0,
-            "velocityY": 6.636862452110489e-29,
-            "timestamp": 1.7112395608978708
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.1.traj
deleted file mode 100644
index 2b663e69..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -9.522541691773037e-34,
-            "velocityX": 0,
-            "velocityY": -4.2308696063129985e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 4.271262994735721,
-            "y": 5.464085078742271,
-            "heading": 0.013066573413208545,
-            "angularVelocity": 0.12156077000957308,
-            "velocityX": 0.0768721814736735,
-            "velocityY": -0.8923144052220247,
-            "timestamp": 0.10749005137249076
-        },
-        {
-            "x": 4.301424317332477,
-            "y": 5.27437671657762,
-            "heading": 0.033784324267673776,
-            "angularVelocity": 0.192741101059399,
-            "velocityX": 0.2805964106597774,
-            "velocityY": -1.7648922829820315,
-            "timestamp": 0.21498010274498153
-        },
-        {
-            "x": 4.379013311524821,
-            "y": 5.000817118183451,
-            "heading": 0.051063906614390905,
-            "angularVelocity": 0.16075517804747627,
-            "velocityX": 0.7218248870629933,
-            "velocityY": -2.544975975927198,
-            "timestamp": 0.32247015411747226
-        },
-        {
-            "x": 4.5426840546799285,
-            "y": 4.684552935799795,
-            "heading": 0.043724640930798246,
-            "angularVelocity": -0.06827855778168279,
-            "velocityX": 1.5226594560638123,
-            "velocityY": -2.9422646872470954,
-            "timestamp": 0.42996020548996305
-        },
-        {
-            "x": 4.7987371937718075,
-            "y": 4.3933637164950845,
-            "heading": -0.0025480008668042047,
-            "angularVelocity": -0.4304830187237655,
-            "velocityX": 2.382110119238521,
-            "velocityY": -2.7089876280330105,
-            "timestamp": 0.5374502568624538
-        },
-        {
-            "x": 5.129552682758233,
-            "y": 4.161939062753506,
-            "heading": -0.00976861092561694,
-            "angularVelocity": -0.06717468236935505,
-            "velocityX": 3.0776382070935524,
-            "velocityY": -2.152986725623672,
-            "timestamp": 0.6449403082349446
-        },
-        {
-            "x": 5.508331065778466,
-            "y": 4.0140416634822165,
-            "heading": -0.009768639154040644,
-            "angularVelocity": -2.6261429169311795e-7,
-            "velocityX": 3.5238459576843355,
-            "velocityY": -1.3759170954228361,
-            "timestamp": 0.7524303596074354
-        },
-        {
-            "x": 5.911272135069668,
-            "y": 3.959404581326429,
-            "heading": -0.009768652411608816,
-            "angularVelocity": -1.233376298753571e-7,
-            "velocityX": 3.7486359355701584,
-            "velocityY": -0.5082989677477323,
-            "timestamp": 0.8599204109799262
-        },
-        {
-            "x": 6.316087697151271,
-            "y": 3.997759236961094,
-            "heading": -0.009768652034230711,
-            "angularVelocity": 3.51081891151765e-9,
-            "velocityX": 3.7660746916826353,
-            "velocityY": 0.3568205163634492,
-            "timestamp": 0.967410462352417
-        },
-        {
-            "x": 6.720902827559142,
-            "y": 4.036118448432054,
-            "heading": -0.009768651656829377,
-            "angularVelocity": 3.5110350109919825e-9,
-            "velocityX": 3.766070675741356,
-            "velocityY": 0.35686290015837574,
-            "timestamp": 1.0749005137249077
-        },
-        {
-            "x": 7.104541677751467,
-            "y": 4.072471064803872,
-            "heading": -0.005861206189315246,
-            "angularVelocity": 0.03635169411142489,
-            "velocityX": 3.569063790497984,
-            "velocityY": 0.3381951716242375,
-            "timestamp": 1.1823905650973985
-        },
-        {
-            "x": 7.392270834070994,
-            "y": 4.099735530737146,
-            "heading": -0.0029306066828158664,
-            "angularVelocity": 0.027263913907193366,
-            "velocityX": 2.6767980166131413,
-            "velocityY": 0.2536464127158502,
-            "timestamp": 1.2898806164698893
-        },
-        {
-            "x": 7.584090276967534,
-            "y": 4.117911843086064,
-            "heading": -0.0009768696053991616,
-            "angularVelocity": 0.018175980497453854,
-            "velocityX": 1.7845320608492279,
-            "velocityY": 0.16909762454137037,
-            "timestamp": 1.39737066784238
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -6.809535330810639e-32,
-            "angularVelocity": 0.009088000172350516,
-            "velocityX": 0.8922660451626774,
-            "velocityY": 0.08454881915017429,
-            "timestamp": 1.5048607192148709
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -9.489213882041181e-31,
-            "velocityX": 2.198140684032685e-33,
-            "velocityY": -6.756921472052438e-32,
-            "timestamp": 1.6123507705873616
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C4.1.traj
deleted file mode 100644
index c13783bc..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C4.1.traj	
+++ /dev/null
@@ -1,166 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 7.441191049306378e-32,
-            "velocityX": -6.15829104129807e-34,
-            "velocityY": 2.4252394047543234e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 4.262142393092874,
-            "y": 5.471176724349191,
-            "heading": -0.06580713819401311,
-            "angularVelocity": -0.6311237133024392,
-            "velocityX": -0.008224883662067025,
-            "velocityY": -0.8518601035521746,
-            "timestamp": 0.10426979181255681
-        },
-        {
-            "x": 4.27936690849282,
-            "y": 5.294863623901166,
-            "heading": -0.18680392528158013,
-            "angularVelocity": -1.1604203382805245,
-            "velocityX": 0.16519180771848577,
-            "velocityY": -1.6909317395107013,
-            "timestamp": 0.20853958362511363
-        },
-        {
-            "x": 4.348116425232891,
-            "y": 5.0445566747334585,
-            "heading": -0.3412830267774945,
-            "angularVelocity": -1.4815326549574157,
-            "velocityX": 0.6593426105967491,
-            "velocityY": -2.400570144205118,
-            "timestamp": 0.31280937543767046
-        },
-        {
-            "x": 4.502267029635194,
-            "y": 4.763921050939977,
-            "heading": -0.4878370803946758,
-            "angularVelocity": -1.4055274405902505,
-            "velocityX": 1.4783822018117698,
-            "velocityY": -2.6914374615609895,
-            "timestamp": 0.41707916725022726
-        },
-        {
-            "x": 4.740029978369904,
-            "y": 4.496898841850153,
-            "heading": -0.5207249369577782,
-            "angularVelocity": -0.31541116551017917,
-            "velocityX": 2.280266840487519,
-            "velocityY": -2.5608779345205184,
-            "timestamp": 0.5213489590627841
-        },
-        {
-            "x": 5.0582868126043,
-            "y": 4.2697391199027495,
-            "heading": -0.509978725166827,
-            "angularVelocity": 0.10306160206274768,
-            "velocityX": 3.0522438829312937,
-            "velocityY": -2.1785765368723764,
-            "timestamp": 0.6256187508753409
-        },
-        {
-            "x": 5.423286105863135,
-            "y": 4.1201949004052105,
-            "heading": -0.5099785687835147,
-            "angularVelocity": 0.0000014997949991595825,
-            "velocityX": 3.5005276879710774,
-            "velocityY": -1.4342046425715584,
-            "timestamp": 0.7298885426878977
-        },
-        {
-            "x": 5.805099761015269,
-            "y": 4.021168099470067,
-            "heading": -0.5099784851647622,
-            "angularVelocity": 8.019460959727046e-7,
-            "velocityX": 3.6617859162748747,
-            "velocityY": -0.9497170677501874,
-            "timestamp": 0.8341583345004545
-        },
-        {
-            "x": 6.154222032422552,
-            "y": 3.8375880828272497,
-            "heading": -0.5099784796190555,
-            "angularVelocity": 5.318612994948929e-8,
-            "velocityX": 3.348259024386382,
-            "velocityY": -1.7606251384181746,
-            "timestamp": 0.9384281263130113
-        },
-        {
-            "x": 6.493379431179238,
-            "y": 3.636192348012897,
-            "heading": -0.5099784741544249,
-            "angularVelocity": 5.240856867670856e-8,
-            "velocityX": 3.2526908595576858,
-            "velocityY": -1.9314868795019449,
-            "timestamp": 1.0426979181255682
-        },
-        {
-            "x": 6.795285003697281,
-            "y": 3.3823400148372036,
-            "heading": -0.5099784394461939,
-            "angularVelocity": 3.32869476219209e-7,
-            "velocityX": 2.895427019368865,
-            "velocityY": -2.4345721686299853,
-            "timestamp": 1.146967709938125
-        },
-        {
-            "x": 7.066681771760518,
-            "y": 3.0961027469536373,
-            "heading": -0.509978390292543,
-            "angularVelocity": 4.714083530965781e-7,
-            "velocityX": 2.6028321659174227,
-            "velocityY": -2.7451600593787298,
-            "timestamp": 1.2512375017506818
-        },
-        {
-            "x": 7.312943361341716,
-            "y": 2.8410544331524363,
-            "heading": -0.40533862017886096,
-            "angularVelocity": 1.0035482788897323,
-            "velocityX": 2.361773101301441,
-            "velocityY": -2.446042227260748,
-            "timestamp": 1.3555072935632386
-        },
-        {
-            "x": 7.49731350893092,
-            "y": 2.6484381177603322,
-            "heading": -0.22178416093118458,
-            "angularVelocity": 1.7603800300824186,
-            "velocityX": 1.768202893515326,
-            "velocityY": -1.8472878102448496,
-            "timestamp": 1.4597770853757954
-        },
-        {
-            "x": 7.619307085639959,
-            "y": 2.520663921373434,
-            "heading": -0.0776901780416505,
-            "angularVelocity": 1.3819341190262298,
-            "velocityX": 1.1699800545142,
-            "velocityY": -1.225419118670484,
-            "timestamp": 1.5640468771883522
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 1.429761379305419e-34,
-            "angularVelocity": 0.7450880709660586,
-            "velocityX": 0.5820757220763062,
-            "velocityY": -0.6105691808408042,
-            "timestamp": 1.668316669000909
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -5.61453765178046e-32,
-            "velocityX": 0,
-            "velocityY": 2.318867528557263e-33,
-            "timestamp": 1.7725864608134658
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C5.1.traj
deleted file mode 100644
index 004abebc..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C5.1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -1.2613927548149136e-32,
-            "velocityX": 4.983015637334932e-33,
-            "velocityY": -3.6905945238681026e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 4.258006222511338,
-            "y": 5.473607841813564,
-            "heading": -0.06530003091818118,
-            "angularVelocity": -0.6342708718414432,
-            "velocityX": -0.04850545332028736,
-            "velocityY": -0.8391424739420397,
-            "timestamp": 0.10295290831912149
-        },
-        {
-            "x": 4.271377859079598,
-            "y": 5.302453682559796,
-            "heading": -0.1916286288786279,
-            "angularVelocity": -1.2270522515873779,
-            "velocityX": 0.12988109599401854,
-            "velocityY": -1.662450940416803,
-            "timestamp": 0.20590581663824298
-        },
-        {
-            "x": 4.340766271943036,
-            "y": 5.0642424556348775,
-            "heading": -0.3653686640748466,
-            "angularVelocity": -1.6875680156375907,
-            "velocityX": 0.673982056420939,
-            "velocityY": -2.3137882242873524,
-            "timestamp": 0.30885872495736444
-        },
-        {
-            "x": 4.494848034080926,
-            "y": 4.801255485663876,
-            "heading": -0.5565822720790902,
-            "angularVelocity": -1.8572919514963226,
-            "velocityX": 1.4966236957608356,
-            "velocityY": -2.5544394448365244,
-            "timestamp": 0.41181163327648596
-        },
-        {
-            "x": 4.73233691115851,
-            "y": 4.546498868370588,
-            "heading": -0.6490495118957932,
-            "angularVelocity": -0.8981508276588281,
-            "velocityX": 2.3067719111095206,
-            "velocityY": -2.4744965582091383,
-            "timestamp": 0.5147645415956075
-        },
-        {
-            "x": 5.049349589860016,
-            "y": 4.3202563768587865,
-            "heading": -0.6490493910522949,
-            "angularVelocity": 0.0000011737744981731646,
-            "velocityX": 3.079200810130272,
-            "velocityY": -2.1975337579636096,
-            "timestamp": 0.617717449914729
-        },
-        {
-            "x": 5.4092082222704905,
-            "y": 4.171311637049865,
-            "heading": -0.6490493274377749,
-            "angularVelocity": 6.178992034513071e-7,
-            "velocityX": 3.4953712166637025,
-            "velocityY": -1.446726879703489,
-            "timestamp": 0.7206703582338505
-        },
-        {
-            "x": 5.789554256648214,
-            "y": 4.087528164867666,
-            "heading": -0.6490492895940018,
-            "angularVelocity": 3.675833320529158e-7,
-            "velocityX": 3.6943690138288297,
-            "velocityY": -0.8138038405141147,
-            "timestamp": 0.823623266552972
-        },
-        {
-            "x": 6.141218018459777,
-            "y": 3.9201502365347105,
-            "heading": -0.6490493503243893,
-            "angularVelocity": -5.898851085689342e-7,
-            "velocityX": 3.4157729738097102,
-            "velocityY": -1.6257717345306835,
-            "timestamp": 0.9265761748720935
-        },
-        {
-            "x": 6.456138339829479,
-            "y": 3.69100407104571,
-            "heading": -0.6490494074378677,
-            "angularVelocity": -5.547534235510974e-7,
-            "velocityX": 3.0588773693847333,
-            "velocityY": -2.2257376622981755,
-            "timestamp": 1.029529083191215
-        },
-        {
-            "x": 6.721775326533507,
-            "y": 3.406188756476768,
-            "heading": -0.6490494689481024,
-            "angularVelocity": -5.974599048336077e-7,
-            "velocityX": 2.580179531020508,
-            "velocityY": -2.7664620574496457,
-            "timestamp": 1.1324819915103363
-        },
-        {
-            "x": 6.916359430113237,
-            "y": 3.0688168532177102,
-            "heading": -0.6490494077682868,
-            "angularVelocity": 5.942504852606566e-7,
-            "velocityX": 1.8900301774533879,
-            "velocityY": -3.276953597204961,
-            "timestamp": 1.2354348998294578
-        },
-        {
-            "x": 7.040594979350531,
-            "y": 2.699698573462511,
-            "heading": -0.6490493528706274,
-            "angularVelocity": 5.332307776860117e-7,
-            "velocityX": 1.206722095234099,
-            "velocityY": -3.585311826364818,
-            "timestamp": 1.3383878081485792
-        },
-        {
-            "x": 7.164812386541997,
-            "y": 2.330574188065911,
-            "heading": -0.649049297974002,
-            "angularVelocity": 5.332207349098274e-7,
-            "velocityX": 1.2065458783003051,
-            "velocityY": -3.585371131551044,
-            "timestamp": 1.4413407164677006
-        },
-        {
-            "x": 7.289029734328084,
-            "y": 1.9614497826782673,
-            "heading": -0.6490492430773671,
-            "angularVelocity": 5.332208261000816e-7,
-            "velocityX": 1.206545301285243,
-            "velocityY": -3.5853713257276327,
-            "timestamp": 1.544293624786822
-        },
-        {
-            "x": 7.413246520819429,
-            "y": 1.5923251884092744,
-            "heading": -0.6490491881699392,
-            "angularVelocity": 5.333256623024379e-7,
-            "velocityX": 1.2065398493290926,
-            "velocityY": -3.585373160365936,
-            "timestamp": 1.6472465331059434
-        },
-        {
-            "x": 7.520074458284658,
-            "y": 1.2704783284902474,
-            "heading": -0.5256216946651037,
-            "angularVelocity": 1.1988733054752465,
-            "velocityX": 1.037638850707321,
-            "velocityY": -3.1261560763432104,
-            "timestamp": 1.7501994414250648
-        },
-        {
-            "x": 7.601104601009551,
-            "y": 1.0271731610238788,
-            "heading": -0.2913266412644364,
-            "angularVelocity": 2.275749730881093,
-            "velocityX": 0.7870602593733868,
-            "velocityY": -2.3632665792422265,
-            "timestamp": 1.8531523497441862
-        },
-        {
-            "x": 7.654009357663848,
-            "y": 0.8666565360101991,
-            "heading": -0.10310334462657153,
-            "angularVelocity": 1.828246522715339,
-            "velocityX": 0.5138733574219042,
-            "velocityY": -1.5591266690216163,
-            "timestamp": 1.9561052580633076
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -1.429149737287701e-32,
-            "angularVelocity": 1.0014612147428026,
-            "velocityX": 0.2524517545010862,
-            "velocityY": -0.7737181718391956,
-            "timestamp": 2.059058166382429
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -8.032741852653566e-32,
-            "velocityX": 0,
-            "velocityY": 2.733638748586157e-32,
-            "timestamp": 2.1620110747015504
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj
deleted file mode 100644
index c913a793..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 8.330960047205791e-35,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.215224456008129,
-            "y": 5.5891314798928855,
-            "heading": 0.013109691961439362,
-            "angularVelocity": 0.15985781811927008,
-            "velocityX": -0.5825685488618567,
-            "velocityY": 0.35522534228568337,
-            "timestamp": 0.08200845048227925
-        },
-        {
-            "x": 4.11967337131145,
-            "y": 5.647394543004508,
-            "heading": 0.03932481805987993,
-            "angularVelocity": 0.31966371690079975,
-            "velocityX": -1.1651370576417237,
-            "velocityY": 0.7104519445128596,
-            "timestamp": 0.1640169009645585
-        },
-        {
-            "x": 3.976346757700282,
-            "y": 5.734789362523958,
-            "heading": 0.07863903763442207,
-            "angularVelocity": 0.47939229851730114,
-            "velocityX": -1.7477054226519961,
-            "velocityY": 1.0656806585844962,
-            "timestamp": 0.24602535144683776
-        },
-        {
-            "x": 3.7852446629452063,
-            "y": 5.851316183985278,
-            "heading": 0.13104540832787342,
-            "angularVelocity": 0.6390362259652201,
-            "velocityX": -2.3302732051542674,
-            "velocityY": 1.4209123666652002,
-            "timestamp": 0.328033801929117
-        },
-        {
-            "x": 3.5463672091086966,
-            "y": 5.996975313640393,
-            "heading": 0.19653856458379848,
-            "angularVelocity": 0.7986147265406159,
-            "velocityX": -2.912839499243156,
-            "velocityY": 1.77614780914058,
-            "timestamp": 0.41004225241139625
-        },
-        {
-            "x": 3.281494446824011,
-            "y": 6.15848998396632,
-            "heading": 0.19653857286419524,
-            "angularVelocity": 1.009700426817188e-7,
-            "velocityX": -3.229822789322431,
-            "velocityY": 1.9694881366015824,
-            "timestamp": 0.4920507028936755
-        },
-        {
-            "x": 3.016621684855716,
-            "y": 6.320004654811106,
-            "heading": 0.1965385811445921,
-            "angularVelocity": 1.0097004416983622e-7,
-            "velocityX": -3.229822785464406,
-            "velocityY": 1.969488142928474,
-            "timestamp": 0.5740591533759547
-        },
-        {
-            "x": 2.77774755563319,
-            "y": 6.465669559960414,
-            "heading": 0.2620174097526663,
-            "angularVelocity": 0.7984400171324189,
-            "velocityX": -2.9127989593480157,
-            "velocityY": 1.7762182347389206,
-            "timestamp": 0.6560676038582339
-        },
-        {
-            "x": 2.5866484397575076,
-            "y": 6.582201656937504,
-            "heading": 0.3144069758542471,
-            "angularVelocity": 0.6388313130352502,
-            "velocityX": -2.330236881100143,
-            "velocityY": 1.4209766955939638,
-            "timestamp": 0.7380760543405132
-        },
-        {
-            "x": 2.443324199843316,
-            "y": 6.6696008076986475,
-            "heading": 0.35370275100271265,
-            "angularVelocity": 0.4791673896698825,
-            "velocityX": -1.7476764781107736,
-            "velocityY": 1.0657334731623573,
-            "timestamp": 0.8200845048227924
-        },
-        {
-            "x": 2.347774735979553,
-            "y": 6.727866934294423,
-            "heading": 0.37990105353306003,
-            "angularVelocity": 0.3194585725773273,
-            "velocityX": -1.1651172934234333,
-            "velocityY": 0.7104893002260324,
-            "timestamp": 0.9020929553050716
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.15972678900658524,
-            "velocityX": -0.5825586960684825,
-            "velocityY": 0.3552446794720561,
-            "timestamp": 0.9841014057873508
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": -4.59054414969708e-34,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 1.06610985626963
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj
deleted file mode 100644
index a4697851..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.181096344253034,
-            "y": 5.56,
-            "heading": 2.5710705899848466e-19,
-            "angularVelocity": 2.5941602968880693e-18,
-            "velocityX": -0.8263919813283321,
-            "velocityY": -1.4479549544654444e-34,
-            "timestamp": 0.099109935233537
-        },
-        {
-            "x": 4.017289035836238,
-            "y": 5.56,
-            "heading": 7.7182898862159e-19,
-            "angularVelocity": 5.193444314378615e-18,
-            "velocityX": -1.6527839316089838,
-            "velocityY": -7.36495654370163e-34,
-            "timestamp": 0.198219870467074
-        },
-        {
-            "x": 3.771578080903875,
-            "y": 5.56,
-            "heading": 1.545129672215716e-18,
-            "angularVelocity": 7.802453727507411e-18,
-            "velocityX": -2.479175819794286,
-            "velocityY": -8.54847043233614e-34,
-            "timestamp": 0.297329805700611
-        },
-        {
-            "x": 3.4439634979187304,
-            "y": 5.56,
-            "heading": 2.5806642382479214e-18,
-            "angularVelocity": 1.0448342676399075e-17,
-            "velocityX": -3.3055675216936846,
-            "velocityY": -5.0910568375180236e-34,
-            "timestamp": 0.396439740934148
-        },
-        {
-            "x": 3.0690365020812695,
-            "y": 5.56,
-            "heading": -2.5850068638158606e-18,
-            "angularVelocity": -5.212061828001078e-17,
-            "velocityX": -3.7829405796099502,
-            "velocityY": -4.536187828596351e-34,
-            "timestamp": 0.495549676167685
-        },
-        {
-            "x": 2.741421919096125,
-            "y": 5.56,
-            "heading": -1.5478395588580842e-18,
-            "angularVelocity": 1.0464816695997814e-17,
-            "velocityX": -3.305567521693684,
-            "velocityY": -2.872506388563931e-33,
-            "timestamp": 0.594659611401222
-        },
-        {
-            "x": 2.4957109641637625,
-            "y": 5.56,
-            "heading": -7.728706253244835e-19,
-            "angularVelocity": 7.819286045545264e-18,
-            "velocityX": -2.479175819794286,
-            "velocityY": -1.6595590840772702e-33,
-            "timestamp": 0.6937695466347591
-        },
-        {
-            "x": 2.3319036557469652,
-            "y": 5.56,
-            "heading": -2.5738443375571516e-19,
-            "angularVelocity": 5.201155568994596e-18,
-            "velocityX": -1.6527839316089838,
-            "velocityY": -1.4347514653062086e-33,
-            "timestamp": 0.792879481868296
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 2.5969589542999405e-18,
-            "velocityX": -0.8263919813283321,
-            "velocityY": 2.9921012122849815e-33,
-            "timestamp": 0.891989417101833
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 1.808288986442481e-43,
-            "timestamp": 0.9910993523353699
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj
deleted file mode 100644
index 4a59e123..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 9.014129666558106e-32,
-            "angularVelocity": 9.039417129994225e-31,
-            "velocityX": 0,
-            "velocityY": -2.779554313855136e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 4.206523478699805,
-            "y": 5.55440930401173,
-            "heading": -0.10760119525493232,
-            "angularVelocity": -1.2344158099898592,
-            "velocityX": -0.6479064718660169,
-            "velocityY": -0.06413723844253004,
-            "timestamp": 0.08716770668695249
-        },
-        {
-            "x": 4.091196989898444,
-            "y": 5.541197271810027,
-            "heading": -0.295913717265277,
-            "angularVelocity": -2.1603473254910437,
-            "velocityX": -1.3230414471673093,
-            "velocityY": -0.15157026270235532,
-            "timestamp": 0.17433541337390498
-        },
-        {
-            "x": 3.9151950960136452,
-            "y": 5.516370757100726,
-            "heading": -0.5340653762567812,
-            "angularVelocity": -2.7321088054637612,
-            "velocityX": -2.019118095155108,
-            "velocityY": -0.2848132141236808,
-            "timestamp": 0.26150312006085746
-        },
-        {
-            "x": 3.678758800597895,
-            "y": 5.473177706772957,
-            "heading": -0.7546654655667131,
-            "angularVelocity": -2.530754767957573,
-            "velocityX": -2.712429917020411,
-            "velocityY": -0.4955166536948123,
-            "timestamp": 0.34867082674780997
-        },
-        {
-            "x": 3.3968844521889907,
-            "y": 5.398848640381046,
-            "heading": -0.8527884173152881,
-            "angularVelocity": -1.1256800881657552,
-            "velocityX": -3.233701552126478,
-            "velocityY": -0.8527133409492049,
-            "timestamp": 0.4358385334347625
-        },
-        {
-            "x": 3.098615494079836,
-            "y": 5.268356714792353,
-            "heading": -0.864385170870599,
-            "angularVelocity": -0.1330395624260059,
-            "velocityX": -3.421782784538955,
-            "velocityY": -1.4970214377364781,
-            "timestamp": 0.5230062401217149
-        },
-        {
-            "x": 2.832096469446971,
-            "y": 5.0836195976119996,
-            "heading": -0.8474300397750907,
-            "angularVelocity": 0.19451161146636953,
-            "velocityX": -3.0575431517318843,
-            "velocityY": -2.1193297862453186,
-            "timestamp": 0.6101739468086674
-        },
-        {
-            "x": 2.620164345186853,
-            "y": 4.879059766080367,
-            "heading": -0.7372424721392209,
-            "angularVelocity": 1.2640870320425897,
-            "velocityX": -2.431314672774813,
-            "velocityY": -2.346738709855847,
-            "timestamp": 0.6973416534956199
-        },
-        {
-            "x": 2.469226841034766,
-            "y": 4.6911608685478585,
-            "heading": -0.6128307853924054,
-            "angularVelocity": 1.427268095897297,
-            "velocityX": -1.7315759458276552,
-            "velocityY": -2.155602168212534,
-            "timestamp": 0.7845093601825724
-        },
-        {
-            "x": 2.3748885049182054,
-            "y": 4.5313089505917965,
-            "heading": -0.5057978702393489,
-            "angularVelocity": 1.2278964219794164,
-            "velocityX": -1.0822624536327627,
-            "velocityY": -1.8338433352404204,
-            "timestamp": 0.871677066869525
-        },
-        {
-            "x": 2.3223904484665,
-            "y": 4.418253055823363,
-            "heading": -0.42949016513376354,
-            "angularVelocity": 0.87541255822675,
-            "velocityX": -0.6022649722820163,
-            "velocityY": -1.2969928780443263,
-            "timestamp": 0.9588447735564775
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0.4530366420626772,
-            "velocityX": -0.256866324898396,
-            "velocityY": -0.6682871218875669,
-            "timestamp": 1.0460124802434299
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -3.4100744325418914e-31,
-            "velocityX": -9.400788860397695e-33,
-            "velocityY": 0,
-            "timestamp": 1.1331801869303824
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C1.1.traj
deleted file mode 100644
index 97fb7eaf..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C1.1.traj	
+++ /dev/null
@@ -1,220 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 3.2688003829398086e-32,
-            "velocityX": -2.078713781970587e-32,
-            "velocityY": -4.638967376158192e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 4.29154268151456,
-            "y": 3.0623077457400183,
-            "heading": -0.5127665183876435,
-            "angularVelocity": 0.580669777278592,
-            "velocityX": 0.31134301225465,
-            "velocityY": 0.6796516730776717,
-            "timestamp": 0.09167599846825866
-        },
-        {
-            "x": 4.3492977760941525,
-            "y": 3.186661868715021,
-            "heading": -0.4070750475472326,
-            "angularVelocity": 1.1528804987818582,
-            "velocityX": 0.6299914431757085,
-            "velocityY": 1.3564523436094174,
-            "timestamp": 0.18335199693651733
-        },
-        {
-            "x": 4.438053987492854,
-            "y": 3.3723523918382017,
-            "heading": -0.25179906449139605,
-            "angularVelocity": 1.6937473891773143,
-            "velocityX": 0.968151019696088,
-            "velocityY": 2.025508597950785,
-            "timestamp": 0.27502799540477596
-        },
-        {
-            "x": 4.5709698511757795,
-            "y": 3.6117104026755453,
-            "heading": -0.06452556421817643,
-            "angularVelocity": 2.042775681773022,
-            "velocityX": 1.449843643960376,
-            "velocityY": 2.6109125053076734,
-            "timestamp": 0.36670399387303465
-        },
-        {
-            "x": 4.761013106791701,
-            "y": 3.810602792551275,
-            "heading": 0.11153690871840848,
-            "angularVelocity": 1.9204860146414893,
-            "velocityX": 2.0729881189319217,
-            "velocityY": 2.1695143025313537,
-            "timestamp": 0.45837999234129334
-        },
-        {
-            "x": 5.007328053154023,
-            "y": 3.968591564774131,
-            "heading": 0.25752334504560864,
-            "angularVelocity": 1.5924171949733021,
-            "velocityX": 2.6867986220799525,
-            "velocityY": 1.7233384404049508,
-            "timestamp": 0.550055990809552
-        },
-        {
-            "x": 5.305296422655045,
-            "y": 4.081628168088871,
-            "heading": 0.35020186087766364,
-            "angularVelocity": 1.0109354398157564,
-            "velocityX": 3.2502331524012615,
-            "velocityY": 1.2330010602925316,
-            "timestamp": 0.6417319892778107
-        },
-        {
-            "x": 5.6446965706540295,
-            "y": 4.152909996485894,
-            "heading": 0.3502021264888923,
-            "angularVelocity": 0.0000028972820926514052,
-            "velocityX": 3.702170182705961,
-            "velocityY": 0.7775407913523397,
-            "timestamp": 0.7334079877460694
-        },
-        {
-            "x": 5.981889969698284,
-            "y": 4.233991234618027,
-            "heading": 0.3502024148432213,
-            "angularVelocity": 0.0000031453633856654694,
-            "velocityX": 3.6780990082262504,
-            "velocityY": 0.884432561268565,
-            "timestamp": 0.8250839862143281
-        },
-        {
-            "x": 6.295899946950055,
-            "y": 4.381202864314355,
-            "heading": 0.350202827556894,
-            "angularVelocity": 0.000004501872678764248,
-            "velocityX": 3.425214696303447,
-            "velocityY": 1.6057815803042044,
-            "timestamp": 0.9167599846825868
-        },
-        {
-            "x": 6.57390491063703,
-            "y": 4.588535293884794,
-            "heading": 0.350203324900352,
-            "angularVelocity": 0.0000054250127210279085,
-            "velocityX": 3.0324727118541346,
-            "velocityY": 2.2615780906081357,
-            "timestamp": 1.0084359831508454
-        },
-        {
-            "x": 6.804553755441848,
-            "y": 4.847523149382792,
-            "heading": 0.35020327996165895,
-            "angularVelocity": -4.901903857536901e-7,
-            "velocityX": 2.5159130923965525,
-            "velocityY": 2.825034467311176,
-            "timestamp": 1.100111981619104
-        },
-        {
-            "x": 6.978428828519116,
-            "y": 5.147591577808381,
-            "heading": 0.35020324373344786,
-            "angularVelocity": -3.9517661894346874e-7,
-            "velocityX": 1.8966258997164847,
-            "velocityY": 3.273140554116584,
-            "timestamp": 1.1917879800873628
-        },
-        {
-            "x": 7.088431556948234,
-            "y": 5.476488224562697,
-            "heading": 0.3502032113343976,
-            "angularVelocity": -3.5340820734555857e-7,
-            "velocityX": 1.1999076123202526,
-            "velocityY": 3.587598196360961,
-            "timestamp": 1.2834639785556214
-        },
-        {
-            "x": 7.187536215850895,
-            "y": 5.808831226900728,
-            "heading": 0.3502031791295722,
-            "angularVelocity": -3.5128960565806065e-7,
-            "velocityX": 1.0810316828670656,
-            "velocityY": 3.6251909757285055,
-            "timestamp": 1.3751399770238801
-        },
-        {
-            "x": 7.286640767886556,
-            "y": 6.141174261106461,
-            "heading": 0.3502031469247365,
-            "angularVelocity": -3.512897194087527e-7,
-            "velocityX": 1.0810305171639119,
-            "velocityY": 3.6251913233407715,
-            "timestamp": 1.4668159754921388
-        },
-        {
-            "x": 7.3857452620780215,
-            "y": 6.473517312561292,
-            "heading": 0.350203114719879,
-            "angularVelocity": -3.512899557892109e-7,
-            "velocityX": 1.0810298862005645,
-            "velocityY": 3.625191511493593,
-            "timestamp": 1.5584919739603975
-        },
-        {
-            "x": 7.481976228665169,
-            "y": 6.80241098377653,
-            "heading": 0.33998449018149474,
-            "angularVelocity": -0.11146455679915658,
-            "velocityX": 1.0496855032395944,
-            "velocityY": 3.5875657392388547,
-            "timestamp": 1.6501679724286562
-        },
-        {
-            "x": 7.561455250393006,
-            "y": 7.068914029375232,
-            "heading": 0.2316909043200923,
-            "angularVelocity": -1.1812643185871456,
-            "velocityX": 0.8669556160367877,
-            "velocityY": 2.907010014087551,
-            "timestamp": 1.741843970896915
-        },
-        {
-            "x": 7.620860593043084,
-            "y": 7.268277067392774,
-            "heading": 0.12481105079738766,
-            "angularVelocity": -1.1658433538600752,
-            "velocityX": 0.6479923168837385,
-            "velocityY": 2.1746481232660675,
-            "timestamp": 1.8335199693651736
-        },
-        {
-            "x": 7.66032628893014,
-            "y": 7.4008489817464715,
-            "heading": 0.04379045785830094,
-            "angularVelocity": -0.8837710446877722,
-            "velocityX": 0.430491039600946,
-            "velocityY": 1.4460918513977026,
-            "timestamp": 1.9251959678334323
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 6.52409657127002e-31,
-            "angularVelocity": -0.4776654586801488,
-            "velocityX": 0.21460045593800534,
-            "velocityY": 0.7215740145599008,
-            "timestamp": 2.0168719663016907
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": -6.745238901014272e-30,
-            "velocityX": 0,
-            "velocityY": 3.373152313643601e-31,
-            "timestamp": 2.1085479647699494
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.1.traj
deleted file mode 100644
index 69777c27..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.1.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -2.8196964588027846e-31,
-            "velocityX": -1.564828148658428e-31,
-            "velocityY": 3.07478616058615e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 4.294807569207289,
-            "y": 3.0477321348748183,
-            "heading": -0.5960373953696199,
-            "angularVelocity": -0.36026847684168845,
-            "velocityX": 0.381499939303545,
-            "velocityY": 0.5724991570056517,
-            "timestamp": 0.08337503084628527
-        },
-        {
-            "x": 4.359939756522268,
-            "y": 3.142193816557181,
-            "heading": -0.6551934033575421,
-            "angularVelocity": -0.7095170746861289,
-            "velocityX": 0.7811953609355742,
-            "velocityY": 1.1329732741750633,
-            "timestamp": 0.16675006169257053
-        },
-        {
-            "x": 4.461050205756915,
-            "y": 3.2814565261988764,
-            "heading": -0.7417418377173162,
-            "angularVelocity": -1.038061797174498,
-            "velocityX": 1.2127185826300881,
-            "velocityY": 1.6703167390540123,
-            "timestamp": 0.2501250925388558
-        },
-        {
-            "x": 4.603825208189836,
-            "y": 3.4604941356958814,
-            "heading": -0.8514332359801334,
-            "angularVelocity": -1.315638472926628,
-            "velocityX": 1.7124431737380506,
-            "velocityY": 2.147376830685539,
-            "timestamp": 0.33350012338514107
-        },
-        {
-            "x": 4.8030511460470855,
-            "y": 3.6526461815261504,
-            "heading": -0.9644207302174959,
-            "angularVelocity": -1.3551718433024906,
-            "velocityX": 2.3895156119888465,
-            "velocityY": 2.3046713611960277,
-            "timestamp": 0.41687515423142635
-        },
-        {
-            "x": 5.051097476442208,
-            "y": 3.8467657037250893,
-            "heading": -0.9658198239131095,
-            "angularVelocity": -0.016780727772000164,
-            "velocityX": 2.975067329839247,
-            "velocityY": 2.328269269930804,
-            "timestamp": 0.5002501850777116
-        },
-        {
-            "x": 5.320526470824812,
-            "y": 4.010737925204951,
-            "heading": -0.965820109061322,
-            "angularVelocity": -0.000003420067250854742,
-            "velocityX": 3.2315309709370754,
-            "velocityY": 1.9666825884858703,
-            "timestamp": 0.5836252159239969
-        },
-        {
-            "x": 5.614243645591117,
-            "y": 4.1256688963704224,
-            "heading": -0.9658201941489747,
-            "angularVelocity": -0.000001020541184209393,
-            "velocityX": 3.5228433715103167,
-            "velocityY": 1.37848190278171,
-            "timestamp": 0.6670002467702821
-        },
-        {
-            "x": 5.906557788271019,
-            "y": 4.244122886725936,
-            "heading": -0.965820105112684,
-            "angularVelocity": 0.0000010679011417361832,
-            "velocityX": 3.506015406684877,
-            "velocityY": 1.4207369898777267,
-            "timestamp": 0.7503752776165674
-        },
-        {
-            "x": 6.172253111887642,
-            "y": 4.41406398245588,
-            "heading": -0.9657938677546691,
-            "angularVelocity": 0.0003146908342779248,
-            "velocityX": 3.1867493291423483,
-            "velocityY": 2.0382732576526004,
-            "timestamp": 0.8337503084628527
-        },
-        {
-            "x": 6.426785390452526,
-            "y": 4.5994304003649855,
-            "heading": -0.9640144900160125,
-            "angularVelocity": 0.021341854037057975,
-            "velocityX": 3.0528597828545716,
-            "velocityY": 2.2232845496735987,
-            "timestamp": 0.917125339309138
-        },
-        {
-            "x": 6.681598941757368,
-            "y": 4.785246665794811,
-            "heading": -0.9639002296367303,
-            "angularVelocity": 0.0013704388246982366,
-            "velocityX": 3.0562333676929114,
-            "velocityY": 2.2286800201898096,
-            "timestamp": 1.0005003701554231
-        },
-        {
-            "x": 6.9014822245071255,
-            "y": 5.01136660335564,
-            "heading": -0.9639000238890051,
-            "angularVelocity": 0.000002467737919950576,
-            "velocityX": 2.6372797769052228,
-            "velocityY": 2.7120822057351384,
-            "timestamp": 1.0838754010017084
-        },
-        {
-            "x": 7.122012116423131,
-            "y": 5.23107273201418,
-            "heading": -0.9515772318649645,
-            "angularVelocity": 0.1477995498047812,
-            "velocityX": 2.645035206314798,
-            "velocityY": 2.635154990990089,
-            "timestamp": 1.1672504318479937
-        },
-        {
-            "x": 7.311148990719613,
-            "y": 5.417840497645029,
-            "heading": -0.7917020160601519,
-            "angularVelocity": 1.9175431083146124,
-            "velocityX": 2.2685073981583956,
-            "velocityY": 2.2400923122317544,
-            "timestamp": 1.250625462694279
-        },
-        {
-            "x": 7.462007390054419,
-            "y": 5.572027096128993,
-            "heading": -0.5225549044290918,
-            "angularVelocity": 3.228150069620655,
-            "velocityX": 1.8093954245478587,
-            "velocityY": 1.8493138403538476,
-            "timestamp": 1.3340004935405643
-        },
-        {
-            "x": 7.571928087470151,
-            "y": 5.685474744918936,
-            "heading": -0.27088873612536774,
-            "angularVelocity": 3.018483660506339,
-            "velocityX": 1.318388686636741,
-            "velocityY": 1.3606909363440005,
-            "timestamp": 1.4173755243868496
-        },
-        {
-            "x": 7.6442805622690475,
-            "y": 5.7599789636443175,
-            "heading": -0.0924144798498188,
-            "angularVelocity": 2.1406199729580164,
-            "velocityX": 0.8677954786282445,
-            "velocityY": 0.8936034921863096,
-            "timestamp": 1.5007505552331348
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -2.5332548087748344e-31,
-            "angularVelocity": 1.1084191383412998,
-            "velocityX": 0.42841888474747514,
-            "velocityY": 0.4440302567795869,
-            "timestamp": 1.5841255860794201
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 5.737670210968143e-33,
-            "velocityX": 0,
-            "velocityY": -3.2710687085311585e-31,
-            "timestamp": 1.6675006169257054
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.1.traj
deleted file mode 100644
index 9d039264..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.1.traj	
+++ /dev/null
@@ -1,166 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -1.0027901855857707e-30,
-            "velocityX": -5.17158696002297e-32,
-            "velocityY": 1.9371271312239428e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 4.300239460670815,
-            "y": 3.0530479185441526,
-            "heading": -0.6028935811234369,
-            "angularVelocity": -0.4158951640704737,
-            "velocityX": 0.4197942063083121,
-            "velocityY": 0.5980003056006388,
-            "timestamp": 0.08870884855296232
-        },
-        {
-            "x": 4.375862012794379,
-            "y": 3.1584586937478076,
-            "heading": -0.6740120078870278,
-            "angularVelocity": -0.8017061197804932,
-            "velocityX": 0.8524803709791626,
-            "velocityY": 1.1882780232540273,
-            "timestamp": 0.17741769710592464
-        },
-        {
-            "x": 4.491898262382256,
-            "y": 3.3149103934712074,
-            "heading": -0.7745169367898204,
-            "angularVelocity": -1.1329752391362358,
-            "velocityX": 1.3080572172977813,
-            "velocityY": 1.763653821185524,
-            "timestamp": 0.26612654565888694
-        },
-        {
-            "x": 4.65291407862912,
-            "y": 3.5188640922932937,
-            "heading": -0.893163244153065,
-            "angularVelocity": -1.3374799616794542,
-            "velocityX": 1.8151043427277864,
-            "velocityY": 2.299135905256609,
-            "timestamp": 0.3548353942118493
-        },
-        {
-            "x": 4.870914712338585,
-            "y": 3.7498942741357597,
-            "heading": -0.9455687285938831,
-            "angularVelocity": -0.5907582534963245,
-            "velocityX": 2.457484650804715,
-            "velocityY": 2.6043645657798487,
-            "timestamp": 0.44354424276481164
-        },
-        {
-            "x": 5.142859309500119,
-            "y": 3.9465163862068953,
-            "heading": -0.9455688623878027,
-            "angularVelocity": -0.0000015082364580335284,
-            "velocityX": 3.065585920655626,
-            "velocityY": 2.216488155110533,
-            "timestamp": 0.532253091317774
-        },
-        {
-            "x": 5.447866241414174,
-            "y": 4.086462405550872,
-            "heading": -0.9455689961292948,
-            "angularVelocity": -0.0000015076454528851402,
-            "velocityX": 3.438292085732067,
-            "velocityY": 1.5775880492961702,
-            "timestamp": 0.6209619398707363
-        },
-        {
-            "x": 5.774274975125892,
-            "y": 4.16438201701327,
-            "heading": -0.9455689173593601,
-            "angularVelocity": 8.879602884106285e-7,
-            "velocityX": 3.6795510147653454,
-            "velocityY": 0.8783747363813094,
-            "timestamp": 0.7096707884236987
-        },
-        {
-            "x": 6.109606649873876,
-            "y": 4.1772967927030695,
-            "heading": -0.9455688167433821,
-            "angularVelocity": 0.0000011342270768412302,
-            "velocityX": 3.780137835379285,
-            "velocityY": 0.14558610443566755,
-            "timestamp": 0.798379636976661
-        },
-        {
-            "x": 6.445060320326861,
-            "y": 4.168079453807152,
-            "heading": -0.9455687201101868,
-            "angularVelocity": 0.0000010893298350239914,
-            "velocityX": 3.781513072539862,
-            "velocityY": -0.10390551840399499,
-            "timestamp": 0.8870884855296234
-        },
-        {
-            "x": 6.780513985890507,
-            "y": 4.158861936977066,
-            "heading": -0.9455686234766241,
-            "angularVelocity": 0.0000010893339756447378,
-            "velocityX": 3.781513017423149,
-            "velocityY": -0.10390752422611307,
-            "timestamp": 0.9757973340825857
-        },
-        {
-            "x": 7.079210544299498,
-            "y": 4.144050975293544,
-            "heading": -0.856180476499644,
-            "angularVelocity": 1.0076576174203435,
-            "velocityX": 3.3671563015572095,
-            "velocityY": -0.16696149172400718,
-            "timestamp": 1.064506182635548
-        },
-        {
-            "x": 7.324091725395792,
-            "y": 4.136558502839443,
-            "heading": -0.6317167360096363,
-            "angularVelocity": 2.5303421716267107,
-            "velocityX": 2.760504561730298,
-            "velocityY": -0.08446138774564337,
-            "timestamp": 1.1532150311885103
-        },
-        {
-            "x": 7.50528856962614,
-            "y": 4.13147246408345,
-            "heading": -0.35065072299807204,
-            "angularVelocity": 3.1684101146207286,
-            "velocityX": 2.042601692909661,
-            "velocityY": -0.05733406350050504,
-            "timestamp": 1.2419238797414727
-        },
-        {
-            "x": 7.622685008969739,
-            "y": 4.128418008534293,
-            "heading": -0.12509027443555182,
-            "angularVelocity": 2.5427051781407424,
-            "velocityX": 1.323390408720159,
-            "velocityY": -0.03443236609404116,
-            "timestamp": 1.330632728294435
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.405276849475189e-32,
-            "angularVelocity": 1.4101217237744719,
-            "velocityX": 0.6461022994345517,
-            "velocityY": -0.01598497283444213,
-            "timestamp": 1.4193415768473974
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -2.3485646679922316e-31,
-            "velocityX": 0,
-            "velocityY": -2.6996723431981715e-31,
-            "timestamp": 1.5080504254003597
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4.1.traj
deleted file mode 100644
index 0b021e4e..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4.1.traj	
+++ /dev/null
@@ -1,238 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 0,
-            "velocityX": 5.832812358477626e-33,
-            "velocityY": -2.114400767819492e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 4.2637473414422145,
-            "y": 2.974020167404826,
-            "heading": -0.5570803398431683,
-            "angularVelocity": 0.15939480257853877,
-            "velocityX": 0.013355031418934325,
-            "velocityY": -0.46426099366130524,
-            "timestamp": 0.05595954204614445
-        },
-        {
-            "x": 4.266239342094047,
-            "y": 2.922097369670203,
-            "heading": -0.5396590874905302,
-            "angularVelocity": 0.31131870840312753,
-            "velocityX": 0.04453218451605929,
-            "velocityY": -0.9278631639230981,
-            "timestamp": 0.1119190840922889
-        },
-        {
-            "x": 4.271816064940073,
-            "y": 2.844342168020101,
-            "heading": -0.5143157133438189,
-            "angularVelocity": 0.4528874472527088,
-            "velocityX": 0.0996563345966598,
-            "velocityY": -1.3894895992176792,
-            "timestamp": 0.16787862613843335
-        },
-        {
-            "x": 4.282354343554946,
-            "y": 2.7410289237168692,
-            "heading": -0.48190029722305777,
-            "angularVelocity": 0.5792652143941603,
-            "velocityX": 0.18831960072480589,
-            "velocityY": -1.8462131841257783,
-            "timestamp": 0.2238381681845778
-        },
-        {
-            "x": 4.3006106845058465,
-            "y": 2.6128237251760056,
-            "heading": -0.4437500478548854,
-            "angularVelocity": 0.6817469902936669,
-            "velocityX": 0.32624178617911886,
-            "velocityY": -2.2910337335345985,
-            "timestamp": 0.27979771023072225
-        },
-        {
-            "x": 4.330801306241698,
-            "y": 2.4614150778969766,
-            "heading": -0.40214700400499553,
-            "angularVelocity": 0.7434486117771366,
-            "velocityX": 0.5395080201148661,
-            "velocityY": -2.705680599640681,
-            "timestamp": 0.3357572522768667
-        },
-        {
-            "x": 4.379172513113433,
-            "y": 2.2912661424924936,
-            "heading": -0.3611794534128463,
-            "angularVelocity": 0.7320923133783194,
-            "velocityX": 0.864396045840541,
-            "velocityY": -3.0405705476320635,
-            "timestamp": 0.39171679432301115
-        },
-        {
-            "x": 4.451949891834961,
-            "y": 2.111933259909663,
-            "heading": -0.32494756197390784,
-            "angularVelocity": 0.6474658318159084,
-            "velocityX": 1.3005356380778725,
-            "velocityY": -3.2046881733762564,
-            "timestamp": 0.4476763363691556
-        },
-        {
-            "x": 4.550285732793113,
-            "y": 1.9373903674143325,
-            "heading": -0.2971520965733793,
-            "angularVelocity": 0.49670644869849423,
-            "velocityX": 1.7572667209653445,
-            "velocityY": -3.1190907951212283,
-            "timestamp": 0.5036358784153001
-        },
-        {
-            "x": 4.671000177640236,
-            "y": 1.7762856565305696,
-            "heading": -0.2715476133805437,
-            "angularVelocity": 0.457553479828869,
-            "velocityX": 2.157173565637473,
-            "velocityY": -2.878949773229245,
-            "timestamp": 0.5595954204614446
-        },
-        {
-            "x": 4.810884164899462,
-            "y": 1.6328902215117949,
-            "heading": -0.24301377044973924,
-            "angularVelocity": 0.5099012945331528,
-            "velocityX": 2.4997343106181424,
-            "velocityY": -2.5624840693036233,
-            "timestamp": 0.6155549625075891
-        },
-        {
-            "x": 4.966899091534391,
-            "y": 1.5100232904396305,
-            "heading": -0.21315700918191657,
-            "angularVelocity": 0.5335419157506132,
-            "velocityX": 2.7879950573269263,
-            "velocityY": -2.1956386092446096,
-            "timestamp": 0.6715145045537336
-        },
-        {
-            "x": 5.136752128601074,
-            "y": 1.409298062324524,
-            "heading": -0.1837509654270181,
-            "angularVelocity": 0.5254875697633724,
-            "velocityX": 3.035282828558936,
-            "velocityY": -1.7999651968567865,
-            "timestamp": 0.7274740465998781
-        },
-        {
-            "x": 5.4382274814533025,
-            "y": 1.3074419969375322,
-            "heading": -0.14111678525881038,
-            "angularVelocity": 0.4782734646509064,
-            "velocityX": 3.3819733590888914,
-            "velocityY": -1.142629061849971,
-            "timestamp": 0.8166158939305274
-        },
-        {
-            "x": 5.764139683697637,
-            "y": 1.267066681764717,
-            "heading": -0.11133632809533998,
-            "angularVelocity": 0.3340794257159682,
-            "velocityX": 3.6561077877984602,
-            "velocityY": -0.4529333459183563,
-            "timestamp": 0.9057577412611766
-        },
-        {
-            "x": 6.100462721846291,
-            "y": 1.2915989985388854,
-            "heading": -0.11133120767369757,
-            "angularVelocity": 0.000057441278584387276,
-            "velocityX": 3.7728973340784897,
-            "velocityY": 0.2752053890377148,
-            "timestamp": 0.9948995885918258
-        },
-        {
-            "x": 6.42549869748969,
-            "y": 1.3814198753662668,
-            "heading": -0.1113311879575033,
-            "angularVelocity": 2.2117776172639892e-7,
-            "velocityX": 3.6462782113742715,
-            "velocityY": 1.0076174043625032,
-            "timestamp": 1.084041435922475
-        },
-        {
-            "x": 6.7266979846048365,
-            "y": 1.533061465523067,
-            "heading": -0.11133117504978503,
-            "angularVelocity": 1.4479976182990285e-7,
-            "velocityX": 3.3788764327254777,
-            "velocityY": 1.7011268522889103,
-            "timestamp": 1.1731832832531242
-        },
-        {
-            "x": 6.992432961420095,
-            "y": 1.740669527522903,
-            "heading": -0.11133116451645807,
-            "angularVelocity": 1.1816366034373416e-7,
-            "velocityX": 2.981035111708839,
-            "velocityY": 2.3289629754897407,
-            "timestamp": 1.2623251305837733
-        },
-        {
-            "x": 7.221595592407175,
-            "y": 1.979432543612985,
-            "heading": -0.09587073631794915,
-            "angularVelocity": 0.17343625537800947,
-            "velocityX": 2.5707637641507293,
-            "velocityY": 2.678461611912118,
-            "timestamp": 1.3514669779144224
-        },
-        {
-            "x": 7.404993837760709,
-            "y": 2.170501480308595,
-            "heading": -0.06411284637687509,
-            "angularVelocity": 0.3562624164974272,
-            "velocityX": 2.0573754173309005,
-            "velocityY": 2.1434258142181197,
-            "timestamp": 1.4406088252450715
-        },
-        {
-            "x": 7.54251389523076,
-            "y": 2.3137697288578556,
-            "heading": -0.0342508837604198,
-            "angularVelocity": 0.33499376006509896,
-            "velocityX": 1.5427104282454527,
-            "velocityY": 1.6071940714649837,
-            "timestamp": 1.5297506725757206
-        },
-        {
-            "x": 7.634176122098128,
-            "y": 2.4092619199619465,
-            "heading": -0.011960250388889534,
-            "angularVelocity": 0.25005801471490263,
-            "velocityX": 1.028273808679041,
-            "velocityY": 1.0712386377846093,
-            "timestamp": 1.6188925199063697
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 9.313254165846089e-33,
-            "angularVelocity": 0.1341709954082624,
-            "velocityX": 0.5140557355952123,
-            "velocityY": 0.5355294002487934,
-            "timestamp": 1.7080343672370188
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -1.1876173054192403e-31,
-            "velocityX": 4.5955148793706025e-32,
-            "velocityY": 0,
-            "timestamp": 1.797176214567668
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.1.traj
deleted file mode 100644
index d933d22a..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.1.traj	
+++ /dev/null
@@ -1,184 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -2.2118032332823513e-27,
-            "velocityX": -2.2398078489200873e-28,
-            "velocityY": 7.32316842173409e-29,
-            "timestamp": 0
-        },
-        {
-            "x": 4.257275610783411,
-            "y": 2.9390850995701614,
-            "heading": -0.6153074756658753,
-            "angularVelocity": -0.5685220144659363,
-            "velocityX": -0.06600299944485946,
-            "velocityY": -0.7023572274929988,
-            "timestamp": 0.0867292284401611
-        },
-        {
-            "x": 4.259999029271463,
-            "y": 2.8165594202827866,
-            "heading": -0.6936339270516045,
-            "angularVelocity": -0.9031148183195571,
-            "velocityX": 0.03140139186100546,
-            "velocityY": -1.4127380295088343,
-            "timestamp": 0.1734584568803222
-        },
-        {
-            "x": 4.293815085802935,
-            "y": 2.6396142169455077,
-            "heading": -0.7636289676296801,
-            "angularVelocity": -0.8070524993355632,
-            "velocityX": 0.3899038091270867,
-            "velocityY": -2.0402026689233352,
-            "timestamp": 0.2601876853204833
-        },
-        {
-            "x": 4.379794167011224,
-            "y": 2.432035667571809,
-            "heading": -0.7730870281638328,
-            "angularVelocity": -0.10905274616477362,
-            "velocityX": 0.9913506986587586,
-            "velocityY": -2.3934093858210397,
-            "timestamp": 0.3469169137606444
-        },
-        {
-            "x": 4.522000800858502,
-            "y": 2.2059673134468256,
-            "heading": -0.7069342437539652,
-            "angularVelocity": 0.7627507542686203,
-            "velocityX": 1.6396621577856372,
-            "velocityY": -2.6065993920487904,
-            "timestamp": 0.43364614220080544
-        },
-        {
-            "x": 4.726803922559788,
-            "y": 1.9764479224139355,
-            "heading": -0.6474090008701758,
-            "angularVelocity": 0.6863342837743838,
-            "velocityX": 2.3614083208706247,
-            "velocityY": -2.6463903249323897,
-            "timestamp": 0.5203753706409665
-        },
-        {
-            "x": 4.985426543182091,
-            "y": 1.774560944536545,
-            "heading": -0.6474088034070749,
-            "angularVelocity": 0.000002276776866191791,
-            "velocityX": 2.9819545875556583,
-            "velocityY": -2.3277847792302735,
-            "timestamp": 0.6071045990811276
-        },
-        {
-            "x": 5.274431548584119,
-            "y": 1.6192543598542732,
-            "heading": -0.6474086941993331,
-            "angularVelocity": 0.000001259180368575944,
-            "velocityX": 3.3322676864516145,
-            "velocityY": -1.7907064028526984,
-            "timestamp": 0.6938338275212886
-        },
-        {
-            "x": 5.576835908979959,
-            "y": 1.4919922741488194,
-            "heading": -0.6474086295363626,
-            "angularVelocity": 7.455729944345581e-7,
-            "velocityX": 3.4867641028824017,
-            "velocityY": -1.4673494506325018,
-            "timestamp": 0.7805630559614497
-        },
-        {
-            "x": 5.888110144336631,
-            "y": 1.388298557512795,
-            "heading": -0.6474085736154427,
-            "angularVelocity": 6.447759407143774e-7,
-            "velocityX": 3.589034988031027,
-            "velocityY": -1.1956028953671916,
-            "timestamp": 0.8672922844016108
-        },
-        {
-            "x": 6.199390629137113,
-            "y": 1.2846236026664921,
-            "heading": -0.6474085176958322,
-            "angularVelocity": 6.447608435198244e-7,
-            "velocityX": 3.5891070449826046,
-            "velocityY": -1.1953865693365275,
-            "timestamp": 0.9540215128417718
-        },
-        {
-            "x": 6.510671944712167,
-            "y": 1.1809511422971397,
-            "heading": -0.6474084617763125,
-            "angularVelocity": 6.447597973785494e-7,
-            "velocityX": 3.589116623928252,
-            "velocityY": -1.1953578076724478,
-            "timestamp": 1.040750741281933
-        },
-        {
-            "x": 6.8219538378473334,
-            "y": 1.0772804160640193,
-            "heading": -0.6474084058668956,
-            "angularVelocity": 6.446433092317229e-7,
-            "velocityX": 3.5891232832762197,
-            "velocityY": -1.1953378128419065,
-            "timestamp": 1.127479969722094
-        },
-        {
-            "x": 7.104843996542823,
-            "y": 0.9795715918943764,
-            "heading": -0.5659567926753784,
-            "angularVelocity": 0.9391483662017565,
-            "velocityX": 3.2617626581409067,
-            "velocityY": -1.126596257420401,
-            "timestamp": 1.2142091981622551
-        },
-        {
-            "x": 7.336171547926519,
-            "y": 0.9014041069183023,
-            "heading": -0.36771202193482067,
-            "angularVelocity": 2.2857896271651064,
-            "velocityX": 2.6672386638755765,
-            "velocityY": -0.9012819136285574,
-            "timestamp": 1.3009384266024162
-        },
-        {
-            "x": 7.508580909570239,
-            "y": 0.8437450323938049,
-            "heading": -0.19262457792893875,
-            "angularVelocity": 2.018782446873579,
-            "velocityX": 1.9879037868147627,
-            "velocityY": -0.6648171044699364,
-            "timestamp": 1.3876676550425773
-        },
-        {
-            "x": 7.622996353926532,
-            "y": 0.8057914476404057,
-            "heading": -0.06643070405801102,
-            "angularVelocity": 1.4550328204291798,
-            "velocityX": 1.3192258989739947,
-            "velocityY": -0.4376100818144245,
-            "timestamp": 1.4743968834827383
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -1.2156690964354266e-28,
-            "angularVelocity": 0.7659552062526378,
-            "velocityX": 0.6572599237729437,
-            "velocityY": -0.21666799046150395,
-            "timestamp": 1.5611261119228994
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 1.25023472254267e-27,
-            "velocityX": 0,
-            "velocityY": -4.9030509543456705e-29,
-            "timestamp": 1.6478553403630605
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W1.1.traj
deleted file mode 100644
index 44ee68f9..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W1.1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -1.6794679447375943e-30,
-            "velocityX": 6.742046899995187e-33,
-            "velocityY": 9.708842875589211e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 4.290023963257714,
-            "y": 3.0497874441329906,
-            "heading": -0.5200869306801483,
-            "angularVelocity": 0.5503538158132694,
-            "velocityX": 0.3239326300245928,
-            "velocityY": 0.5967954280576505,
-            "timestamp": 0.08342464065958187
-        },
-        {
-            "x": 4.342730369898429,
-            "y": 3.150178003649595,
-            "heading": -0.42997936088427535,
-            "angularVelocity": 1.0801073769506666,
-            "velocityX": 0.6317846408926776,
-            "velocityY": 1.2033681982072046,
-            "timestamp": 0.16684928131916374
-        },
-        {
-            "x": 4.418537982231504,
-            "y": 3.3026029150734924,
-            "heading": -0.2990998156933407,
-            "angularVelocity": 1.5688355880967437,
-            "velocityX": 0.9086957010988046,
-            "velocityY": 1.8270970089745318,
-            "timestamp": 0.2502739219787456
-        },
-        {
-            "x": 4.5104462205151705,
-            "y": 3.5100667276192694,
-            "heading": -0.13617594937576347,
-            "angularVelocity": 1.9529465758491624,
-            "velocityX": 1.1016917490685052,
-            "velocityY": 2.4868409489750483,
-            "timestamp": 0.3336985626383275
-        },
-        {
-            "x": 4.573472263152817,
-            "y": 3.7674681923433693,
-            "heading": 0.004287761033867021,
-            "angularVelocity": 1.6837196935950753,
-            "velocityX": 0.7554847361563879,
-            "velocityY": 3.0854369007645883,
-            "timestamp": 0.41712320329790936
-        },
-        {
-            "x": 4.6071871310155705,
-            "y": 4.066840423815047,
-            "heading": 0.04705620405427594,
-            "angularVelocity": 0.5126596013152507,
-            "velocityX": 0.40413560785150865,
-            "velocityY": 3.5885348633778387,
-            "timestamp": 0.5005478439574912
-        },
-        {
-            "x": 4.587417601167403,
-            "y": 4.370555626705338,
-            "heading": 0.08458578048595149,
-            "angularVelocity": 0.4498620088136386,
-            "velocityX": -0.23697470785444333,
-            "velocityY": 3.6405934804036506,
-            "timestamp": 0.5839724846170731
-        },
-        {
-            "x": 4.510022832963655,
-            "y": 4.67116829443229,
-            "heading": 0.1002321163817716,
-            "angularVelocity": 0.1875505338964035,
-            "velocityX": -0.9277207260569706,
-            "velocityY": 3.603403806719601,
-            "timestamp": 0.667397125276655
-        },
-        {
-            "x": 4.376634060333898,
-            "y": 4.957183607465761,
-            "heading": 0.10023219176740035,
-            "angularVelocity": 9.036374403612271e-7,
-            "velocityX": -1.5989133615097586,
-            "velocityY": 3.4284272700744434,
-            "timestamp": 0.7508217659362368
-        },
-        {
-            "x": 4.19313075849175,
-            "y": 5.213940045953902,
-            "heading": 0.10023221692184411,
-            "angularVelocity": 3.015229499928316e-7,
-            "velocityX": -2.1996295146291525,
-            "velocityY": 3.0777050576201734,
-            "timestamp": 0.8342464065958187
-        },
-        {
-            "x": 3.9657176811920825,
-            "y": 5.432756463781841,
-            "heading": 0.10023223856547658,
-            "angularVelocity": 2.5943932502769966e-7,
-            "velocityX": -2.725970115084308,
-            "velocityY": 2.6229231087830502,
-            "timestamp": 0.9176710472554006
-        },
-        {
-            "x": 3.7186426713834653,
-            "y": 5.629101242954768,
-            "heading": 0.10023225962798245,
-            "angularVelocity": 2.524734383899783e-7,
-            "velocityX": -2.961655068037016,
-            "velocityY": 2.3535585843770224,
-            "timestamp": 1.0010956879149824
-        },
-        {
-            "x": 3.4715770348675625,
-            "y": 5.825457816649942,
-            "heading": 0.10023228069061084,
-            "angularVelocity": 2.5247490699416256e-7,
-            "velocityX": -2.961542711631996,
-            "velocityY": 2.353699963736333,
-            "timestamp": 1.0845203285745642
-        },
-        {
-            "x": 3.224511384705754,
-            "y": 6.021814373175139,
-            "heading": 0.10023230175325984,
-            "angularVelocity": 2.5247515406639525e-7,
-            "velocityX": -2.96154287520364,
-            "velocityY": 2.353699757922098,
-            "timestamp": 1.167944969234146
-        },
-        {
-            "x": 2.9774457295747583,
-            "y": 6.218170923447359,
-            "heading": 0.10023232281654228,
-            "angularVelocity": 2.524827469327523e-7,
-            "velocityX": -2.9615429347686173,
-            "velocityY": 2.3536996829685104,
-            "timestamp": 1.2513696098937277
-        },
-        {
-            "x": 2.751412346836958,
-            "y": 6.39797212959724,
-            "heading": 0.1658210049038553,
-            "angularVelocity": 0.7862027521934517,
-            "velocityX": -2.7094319010631414,
-            "velocityY": 2.155252989144637,
-            "timestamp": 1.3347942505533095
-        },
-        {
-            "x": 2.5706626829115913,
-            "y": 6.54172747182469,
-            "heading": 0.24638520673123038,
-            "angularVelocity": 0.9657123026291593,
-            "velocityX": -2.166622025534686,
-            "velocityY": 1.723176043563091,
-            "timestamp": 1.4182188912128912
-        },
-        {
-            "x": 2.435257350873027,
-            "y": 6.649419264379792,
-            "heading": 0.31518856404188306,
-            "angularVelocity": 0.8247366337651717,
-            "velocityX": -1.6230855891977036,
-            "velocityY": 1.2908871012647558,
-            "timestamp": 1.501643531872473
-        },
-        {
-            "x": 2.3450659538089185,
-            "y": 6.721154353305962,
-            "heading": 0.3643262231924431,
-            "angularVelocity": 0.5890065424563048,
-            "velocityX": -1.0811122031935256,
-            "velocityY": 0.8598789081859941,
-            "timestamp": 1.5850681725320548
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0.3077481257883978,
-            "velocityX": -0.5401995555822935,
-            "velocityY": 0.42967696846676384,
-            "timestamp": 1.6684928131916366
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": -2.8369790280137563e-29,
-            "velocityX": 0,
-            "velocityY": -1.9155196850548007e-31,
-            "timestamp": 1.7519174538512183
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.1.traj
deleted file mode 100644
index 01699583..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.1.traj	
+++ /dev/null
@@ -1,157 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.1175745999315144e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 4.331197427717942,
-            "y": 3.064203342806604,
-            "heading": -0.5918486780870994,
-            "angularVelocity": -0.24352382070717674,
-            "velocityX": 0.642497001367488,
-            "velocityY": 0.604868198278966,
-            "timestamp": 0.10614435176007272
-        },
-        {
-            "x": 4.457228812945604,
-            "y": 3.20188389088458,
-            "heading": -0.6500618235234895,
-            "angularVelocity": -0.5484337552691863,
-            "velocityX": 1.1873583769444531,
-            "velocityY": 1.2971066834454588,
-            "timestamp": 0.21228870352014545
-        },
-        {
-            "x": 4.610677951553499,
-            "y": 3.4285671398637003,
-            "heading": -0.7523465196948876,
-            "angularVelocity": -0.9636376733695737,
-            "velocityX": 1.4456646638603075,
-            "velocityY": 2.1356129197671536,
-            "timestamp": 0.31843305528021815
-        },
-        {
-            "x": 4.708912918877337,
-            "y": 3.730071149255109,
-            "heading": -0.9027593729452231,
-            "angularVelocity": -1.4170594172577993,
-            "velocityX": 0.9254846413861598,
-            "velocityY": 2.840509215911215,
-            "timestamp": 0.4245774070402909
-        },
-        {
-            "x": 4.717930123488916,
-            "y": 4.059451821332611,
-            "heading": -1.0196863730682848,
-            "angularVelocity": -1.1015847587195386,
-            "velocityX": 0.08495227925044571,
-            "velocityY": 3.10313894819415,
-            "timestamp": 0.5307217588003637
-        },
-        {
-            "x": 4.639893034223715,
-            "y": 4.389369622514019,
-            "heading": -1.0106655427651627,
-            "angularVelocity": 0.08498643737080536,
-            "velocityX": -0.7351977563685651,
-            "velocityY": 3.1081993126412395,
-            "timestamp": 0.6368661105604364
-        },
-        {
-            "x": 4.471904603111427,
-            "y": 4.710090421849215,
-            "heading": -0.9107146957571205,
-            "angularVelocity": 0.9416501712118399,
-            "velocityX": -1.5826412647185075,
-            "velocityY": 3.021553139823678,
-            "timestamp": 0.7430104623205092
-        },
-        {
-            "x": 4.218561716713538,
-            "y": 4.992188206454831,
-            "heading": -0.8355436069817835,
-            "angularVelocity": 0.7081967860640641,
-            "velocityX": -2.3867768957743736,
-            "velocityY": 2.657680601255776,
-            "timestamp": 0.8491548140805819
-        },
-        {
-            "x": 3.8896224481808446,
-            "y": 5.222473200709689,
-            "heading": -0.83554222765338,
-            "angularVelocity": 0.000012994835623464205,
-            "velocityX": -3.098980426921093,
-            "velocityY": 2.169545439171289,
-            "timestamp": 0.9552991658406547
-        },
-        {
-            "x": 3.516178298161353,
-            "y": 5.370026719708121,
-            "heading": -0.8355421446101063,
-            "angularVelocity": 7.823616840140413e-7,
-            "velocityX": -3.518266811442025,
-            "velocityY": 1.390121250464274,
-            "timestamp": 1.0614435176007273
-        },
-        {
-            "x": 3.1189612059977283,
-            "y": 5.42877328608909,
-            "heading": -0.8355420737846555,
-            "angularVelocity": 6.672559542292647e-7,
-            "velocityX": -3.742234848835754,
-            "velocityY": 0.55345918465599,
-            "timestamp": 1.1675878693608
-        },
-        {
-            "x": 2.774416222135212,
-            "y": 5.484170620762628,
-            "heading": -0.7035735462776811,
-            "angularVelocity": 1.243292980914089,
-            "velocityX": -3.2460039384980304,
-            "velocityY": 0.5219056290320314,
-            "timestamp": 1.2737322211208726
-        },
-        {
-            "x": 2.5095166350816602,
-            "y": 5.523131311595933,
-            "heading": -0.3884677902217159,
-            "angularVelocity": 2.9686530732056857,
-            "velocityX": -2.4956541036901063,
-            "velocityY": 0.3670538298766209,
-            "timestamp": 1.3798765728809452
-        },
-        {
-            "x": 2.335800273937377,
-            "y": 5.547984865708467,
-            "heading": -0.13742924633332027,
-            "angularVelocity": 2.365067379711733,
-            "velocityX": -1.6366048523895966,
-            "velocityY": 0.23414862590816604,
-            "timestamp": 1.4860209246410179
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 1.2947391364164484,
-            "velocityX": -0.8083357476365635,
-            "velocityY": 0.11319617193282665,
-            "timestamp": 1.5921652764010905
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -1.986468774957569e-34,
-            "velocityY": 0,
-            "timestamp": 1.6983096281611632
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.1.traj
deleted file mode 100644
index 26f24eb2..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.1.traj	
+++ /dev/null
@@ -1,121 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 0,
-            "velocityX": -3.809622996347125e-33,
-            "velocityY": 4.7029981856297106e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 4.1621681366579715,
-            "y": 2.9362991956112916,
-            "heading": -0.5618109581410129,
-            "angularVelocity": 0.03502487973446616,
-            "velocityX": -0.8430624486077395,
-            "velocityY": -0.5326069988815002,
-            "timestamp": 0.11960189130537771
-        },
-        {
-            "x": 3.949371568032143,
-            "y": 2.832059073063443,
-            "heading": -0.5332490097615643,
-            "angularVelocity": 0.2388085010004113,
-            "velocityX": -1.7792073879709687,
-            "velocityY": -0.8715591485229383,
-            "timestamp": 0.23920378261075542
-        },
-        {
-            "x": 3.6196334559490815,
-            "y": 2.7428786025637635,
-            "heading": -0.4400400521459035,
-            "angularVelocity": 0.7793267865446122,
-            "velocityX": -2.756964028613438,
-            "velocityY": -0.7456443165432319,
-            "timestamp": 0.35880567391613316
-        },
-        {
-            "x": 3.2124362068162218,
-            "y": 2.7443781288173086,
-            "heading": -0.34020495702705544,
-            "angularVelocity": 0.8347283979314292,
-            "velocityX": -3.4046054346513173,
-            "velocityY": 0.012537646664080927,
-            "timestamp": 0.47840756522151084
-        },
-        {
-            "x": 2.829910158877178,
-            "y": 2.8625567119312745,
-            "heading": -0.23363822858373923,
-            "angularVelocity": 0.891012067453128,
-            "velocityX": -3.1983277502054186,
-            "velocityY": 0.9880996180254248,
-            "timestamp": 0.5980094565268885
-        },
-        {
-            "x": 2.5228974587582895,
-            "y": 3.073051960500077,
-            "heading": -0.12255766699698727,
-            "angularVelocity": 0.9287525504349555,
-            "velocityX": -2.566955227614215,
-            "velocityY": 1.7599658857512914,
-            "timestamp": 0.7176113478322662
-        },
-        {
-            "x": 2.307806503132395,
-            "y": 3.359545759981809,
-            "heading": -0.008191563913849259,
-            "angularVelocity": 0.9562231987710198,
-            "velocityX": -1.7983909224036065,
-            "velocityY": 2.395395226236259,
-            "timestamp": 0.8372132391376439
-        },
-        {
-            "x": 2.197079056424326,
-            "y": 3.692587049160117,
-            "heading": -0.017161158826022753,
-            "angularVelocity": -0.07499542703130056,
-            "velocityX": -0.9258001315827844,
-            "velocityY": 2.7845821294578066,
-            "timestamp": 0.9568151304430216
-        },
-        {
-            "x": 2.2005818293568207,
-            "y": 4.024472502975606,
-            "heading": -0.15274865723996348,
-            "angularVelocity": -1.133656808718376,
-            "velocityX": 0.029286935969546842,
-            "velocityY": 2.774918107006309,
-            "timestamp": 1.0764170217483993
-        },
-        {
-            "x": 2.2590478504225837,
-            "y": 4.250579625353832,
-            "heading": -0.3027008584759083,
-            "angularVelocity": -1.2537611203242134,
-            "velocityX": 0.48883859968803833,
-            "velocityY": 1.8904978835235247,
-            "timestamp": 1.196018913053777
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.729914389908579,
-            "velocityX": 0.3424038627687911,
-            "velocityY": 0.914871608232253,
-            "timestamp": 1.3156208043591548
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 3.1982065543283105e-28,
-            "velocityX": 4.1170744495691044e-30,
-            "velocityY": 1.8090480101607624e-30,
-            "timestamp": 1.4352226956645326
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C1.1.traj
deleted file mode 100644
index f8bd91fc..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C1.1.traj	
+++ /dev/null
@@ -1,292 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 1.1411819845129358e-30,
-            "velocityX": 2.0257584812603577e-32,
-            "velocityY": 3.042151659469263e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 5.128157808100627,
-            "y": 1.6381505638691025,
-            "heading": -0.6351424860741187,
-            "angularVelocity": 0.06199634399134786,
-            "velocityX": -0.6106088487961494,
-            "velocityY": 0.23165524966799825,
-            "timestamp": 0.07835161903352221
-        },
-        {
-            "x": 5.034138141125347,
-            "y": 1.6783506940439978,
-            "heading": -0.625731099901857,
-            "angularVelocity": 0.12011731586854772,
-            "velocityX": -1.199970953185457,
-            "velocityY": 0.5130733821555872,
-            "timestamp": 0.15670323806704442
-        },
-        {
-            "x": 4.897649658060979,
-            "y": 1.7471051706265737,
-            "heading": -0.6123788413002932,
-            "angularVelocity": 0.1704145844880519,
-            "velocityX": -1.74199952404266,
-            "velocityY": 0.8775118808095047,
-            "timestamp": 0.23505485710056662
-        },
-        {
-            "x": 4.7291295198915435,
-            "y": 1.855780594555545,
-            "heading": -0.5966040827715752,
-            "angularVelocity": 0.20133289807283902,
-            "velocityX": -2.150818837544829,
-            "velocityY": 1.3870220586313022,
-            "timestamp": 0.31340647613408884
-        },
-        {
-            "x": 4.559409610033689,
-            "y": 2.0156280635815147,
-            "heading": -0.5822183456142529,
-            "angularVelocity": 0.18360484869071408,
-            "velocityX": -2.1661314973624393,
-            "velocityY": 2.0401297509574126,
-            "timestamp": 0.39175809516761106
-        },
-        {
-            "x": 4.423281260574217,
-            "y": 2.2140742145732073,
-            "heading": -0.5727758831618049,
-            "angularVelocity": 0.1205139417528553,
-            "velocityX": -1.7374031467203144,
-            "velocityY": 2.532763884646578,
-            "timestamp": 0.4701097142011333
-        },
-        {
-            "x": 4.3324586171803325,
-            "y": 2.4362881098121476,
-            "heading": -0.5691499320560302,
-            "angularVelocity": 0.04627793465535804,
-            "velocityX": -1.1591674111421555,
-            "velocityY": 2.8361110846205655,
-            "timestamp": 0.5484613332346555
-        },
-        {
-            "x": 4.290356765831772,
-            "y": 2.674111890908862,
-            "heading": -0.5714790836196856,
-            "angularVelocity": -0.02972691046318988,
-            "velocityX": -0.5373450079001806,
-            "velocityY": 3.0353397164002804,
-            "timestamp": 0.6268129522681777
-        },
-        {
-            "x": 4.298268630722052,
-            "y": 2.9211188280591673,
-            "heading": -0.5947192771376703,
-            "angularVelocity": -0.29661408155511804,
-            "velocityX": 0.1009789585444941,
-            "velocityY": 3.1525441362561364,
-            "timestamp": 0.7051645713016998
-        },
-        {
-            "x": 4.356924477851345,
-            "y": 3.1722484967201265,
-            "heading": -0.6368890523591307,
-            "angularVelocity": -0.5382119188043637,
-            "velocityX": 0.7486232939768359,
-            "velocityY": 3.2051624683532776,
-            "timestamp": 0.783516190335222
-        },
-        {
-            "x": 4.466386404708156,
-            "y": 3.418800428561669,
-            "heading": -0.694172117002664,
-            "angularVelocity": -0.7311025011368958,
-            "velocityX": 1.3970601782967371,
-            "velocityY": 3.1467369134523744,
-            "timestamp": 0.8618678093687442
-        },
-        {
-            "x": 4.622742851027307,
-            "y": 3.644917946871822,
-            "heading": -0.7581397869413499,
-            "angularVelocity": -0.8164179723116849,
-            "velocityX": 1.9955739045067467,
-            "velocityY": 2.8859329404975034,
-            "timestamp": 0.9402194284022664
-        },
-        {
-            "x": 4.8130531116785615,
-            "y": 3.833426042269654,
-            "heading": -0.8482959054101304,
-            "angularVelocity": -1.1506605681014435,
-            "velocityX": 2.4289256941816704,
-            "velocityY": 2.4059246984696085,
-            "timestamp": 1.0185710474357885
-        },
-        {
-            "x": 5.034329891204834,
-            "y": 3.981222629547119,
-            "heading": -0.9454274313666489,
-            "angularVelocity": -1.2396875412997066,
-            "velocityX": 2.824150697276595,
-            "velocityY": 1.8863246107809373,
-            "timestamp": 1.0969226664693108
-        },
-        {
-            "x": 5.3307679727813255,
-            "y": 4.095215339701575,
-            "heading": -1.0325645415374454,
-            "angularVelocity": -0.9491279076997933,
-            "velocityX": 3.2289073573561264,
-            "velocityY": 1.241648503948074,
-            "timestamp": 1.1887302189223545
-        },
-        {
-            "x": 5.6706736122505745,
-            "y": 4.158722341660029,
-            "heading": -1.0366531166779425,
-            "angularVelocity": -0.04453419170049298,
-            "velocityX": 3.7023712144281196,
-            "velocityY": 0.691740496958957,
-            "timestamp": 1.2805377713753983
-        },
-        {
-            "x": 5.985432145120126,
-            "y": 4.2874622186845945,
-            "heading": -1.0150469515971576,
-            "angularVelocity": 0.23534191363870294,
-            "velocityX": 3.4284601262030105,
-            "velocityY": 1.402279807975625,
-            "timestamp": 1.372345323828442
-        },
-        {
-            "x": 6.260473058973373,
-            "y": 4.473603429076733,
-            "heading": -0.9641067919300658,
-            "angularVelocity": 0.5548580515001295,
-            "velocityX": 2.9958419160985765,
-            "velocityY": 2.027515225257119,
-            "timestamp": 1.4641528762814857
-        },
-        {
-            "x": 6.487005993334624,
-            "y": 4.6803674717401,
-            "heading": -0.8309910438969466,
-            "angularVelocity": 1.4499433268434323,
-            "velocityX": 2.4674760224886074,
-            "velocityY": 2.2521463337029823,
-            "timestamp": 1.5559604287345294
-        },
-        {
-            "x": 6.71888754498383,
-            "y": 4.9198952355114685,
-            "heading": -0.7843851972685187,
-            "angularVelocity": 0.507647196588375,
-            "velocityX": 2.5257350343568326,
-            "velocityY": 2.609020253468568,
-            "timestamp": 1.6477679811875732
-        },
-        {
-            "x": 6.906293835583718,
-            "y": 5.2122954621107285,
-            "heading": -0.7843850302000089,
-            "angularVelocity": 0.0000018197686939661552,
-            "velocityX": 2.0412949217411986,
-            "velocityY": 3.1849256274293083,
-            "timestamp": 1.739575533640617
-        },
-        {
-            "x": 7.030998336898649,
-            "y": 5.536437116039629,
-            "heading": -0.7843849409482478,
-            "angularVelocity": 9.721614257680115e-7,
-            "velocityX": 1.3583250830993825,
-            "velocityY": 3.530664365490929,
-            "timestamp": 1.8313830860936606
-        },
-        {
-            "x": 7.139916048661206,
-            "y": 5.86621879705104,
-            "heading": -0.7843848550588985,
-            "angularVelocity": 9.355368597772783e-7,
-            "velocityX": 1.1863698448802928,
-            "velocityY": 3.592097514854049,
-            "timestamp": 1.9231906385467044
-        },
-        {
-            "x": 7.248833621341376,
-            "y": 6.196000523997438,
-            "heading": -0.7843847691695015,
-            "angularVelocity": 9.355373786921671e-7,
-            "velocityX": 1.1863683299462482,
-            "velocityY": 3.592098015194004,
-            "timestamp": 2.0149981909997483
-        },
-        {
-            "x": 7.357751176995467,
-            "y": 6.5257822565670125,
-            "heading": -0.7843846832799689,
-            "angularVelocity": 9.355388566444766e-7,
-            "velocityX": 1.1863681444922285,
-            "velocityY": 3.5920980764436137,
-            "timestamp": 2.1068057434527923
-        },
-        {
-            "x": 7.4669549324183055,
-            "y": 6.834400664448086,
-            "heading": -0.7318192896940902,
-            "angularVelocity": 0.5725606682823186,
-            "velocityX": 1.189485532562175,
-            "velocityY": 3.361579735380924,
-            "timestamp": 2.198613295905836
-        },
-        {
-            "x": 7.555413306559794,
-            "y": 7.089894078912264,
-            "heading": -0.5139853289463429,
-            "angularVelocity": 2.3727237566774426,
-            "velocityX": 0.9635195773978621,
-            "velocityY": 2.782923709842436,
-            "timestamp": 2.29042084835888
-        },
-        {
-            "x": 7.619158448834915,
-            "y": 7.279787689667393,
-            "heading": -0.2782484542424907,
-            "angularVelocity": 2.5677285626846773,
-            "velocityX": 0.6943344046528581,
-            "velocityY": 2.0683876835977384,
-            "timestamp": 2.382228400811924
-        },
-        {
-            "x": 7.660128944224933,
-            "y": 7.404974004983973,
-            "heading": -0.09793823848426357,
-            "angularVelocity": 1.964001990472943,
-            "velocityX": 0.44626497815605787,
-            "velocityY": 1.3635731698719276,
-            "timestamp": 2.474035953264968
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 2.533782828853193e-32,
-            "angularVelocity": 1.0667775783953661,
-            "velocityX": 0.21644249567844834,
-            "velocityY": 0.6756088508922024,
-            "timestamp": 2.565843505718012
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 2.4912680070874024e-31,
-            "velocityX": 0,
-            "velocityY": -3.645397861822831e-32,
-            "timestamp": 2.657651058171056
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C2.1.traj
deleted file mode 100644
index 0e0e8d38..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C2.1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -2.491480567253294e-33,
-            "velocityX": -2.0103104784295923e-34,
-            "velocityY": 1.605618083193578e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 5.240158813928264,
-            "y": 1.6204190327819517,
-            "heading": -0.5237338108165747,
-            "angularVelocity": 1.2526389382742513,
-            "velocityX": 0.6912398963489377,
-            "velocityY": 0.004514612397400031,
-            "timestamp": 0.0928170006782674
-        },
-        {
-            "x": 5.369626464101249,
-            "y": 1.6247166229091008,
-            "heading": -0.2974184918320366,
-            "angularVelocity": 2.4382959730515035,
-            "velocityX": 1.394870004707003,
-            "velocityY": 0.046301756097956205,
-            "timestamp": 0.1856340013565348
-        },
-        {
-            "x": 5.5674551093283755,
-            "y": 1.639197763374398,
-            "heading": 0.0009013327241277373,
-            "angularVelocity": 3.214064475001015,
-            "velocityX": 2.1313837312289596,
-            "velocityY": 0.15601819019657054,
-            "timestamp": 0.2784510020348022
-        },
-        {
-            "x": 5.831585704851482,
-            "y": 1.6774487923340957,
-            "heading": 0.2480008567342784,
-            "angularVelocity": 2.6622226769282764,
-            "velocityX": 2.8457135394696165,
-            "velocityY": 0.41211231434085943,
-            "timestamp": 0.3712680027130696
-        },
-        {
-            "x": 6.141513989428359,
-            "y": 1.75469627200855,
-            "heading": 0.3453177753240257,
-            "angularVelocity": 1.0484816130514494,
-            "velocityX": 3.339132726893265,
-            "velocityY": 0.8322557194259916,
-            "timestamp": 0.46408500339133696
-        },
-        {
-            "x": 6.452443961209774,
-            "y": 1.9008832260175597,
-            "heading": 0.36957097138497697,
-            "angularVelocity": 0.2613012258930921,
-            "velocityX": 3.3499247929718696,
-            "velocityY": 1.5750019171136467,
-            "timestamp": 0.5569020040696043
-        },
-        {
-            "x": 6.733109637262523,
-            "y": 2.111863899724239,
-            "heading": 0.36957109822659895,
-            "angularVelocity": 0.0000013665774702958542,
-            "velocityX": 3.023860650546378,
-            "velocityY": 2.273082217319261,
-            "timestamp": 0.6497190047478717
-        },
-        {
-            "x": 6.964965499466835,
-            "y": 2.3755472934997988,
-            "heading": 0.36957101579118784,
-            "angularVelocity": -8.881499138395027e-7,
-            "velocityX": 2.4979891669630336,
-            "velocityY": 2.840895437782658,
-            "timestamp": 0.7425360054261391
-        },
-        {
-            "x": 7.138307566970259,
-            "y": 2.680897277378083,
-            "heading": 0.36957096673523665,
-            "angularVelocity": -5.285233395413412e-7,
-            "velocityX": 1.867568077364202,
-            "velocityY": 3.289806626446832,
-            "timestamp": 0.8353530061044064
-        },
-        {
-            "x": 7.245881008413397,
-            "y": 3.0151337865477195,
-            "heading": 0.3695709277774119,
-            "angularVelocity": -4.197272531605919e-7,
-            "velocityX": 1.1589842448800958,
-            "velocityY": 3.6010268240427665,
-            "timestamp": 0.9281700067826738
-        },
-        {
-            "x": 7.300266060372569,
-            "y": 3.36201756600148,
-            "heading": 0.3695708905369613,
-            "angularVelocity": -4.012244559973519e-7,
-            "velocityX": 0.5859384763755527,
-            "velocityY": 3.7372871017042186,
-            "timestamp": 1.0209870074609413
-        },
-        {
-            "x": 7.354650396010165,
-            "y": 3.708901457760651,
-            "heading": 0.3695708532965109,
-            "angularVelocity": -4.0122445392101915e-7,
-            "velocityX": 0.5859307588068667,
-            "velocityY": 3.7372883116701696,
-            "timestamp": 1.1138040081392087
-        },
-        {
-            "x": 7.409034640641628,
-            "y": 4.055785363787717,
-            "heading": 0.36957081605605097,
-            "angularVelocity": -4.012245569815441e-7,
-            "velocityX": 0.5859297783169686,
-            "velocityY": 3.737288465390881,
-            "timestamp": 1.206621008817476
-        },
-        {
-            "x": 7.463418795891099,
-            "y": 4.402669283828022,
-            "heading": 0.36957077881557854,
-            "angularVelocity": -4.0122469066112833e-7,
-            "velocityX": 0.5859288153253637,
-            "velocityY": 3.7372886163679513,
-            "timestamp": 1.2994380094957434
-        },
-        {
-            "x": 7.517802854177964,
-            "y": 4.7495532190700205,
-            "heading": 0.3695707415750949,
-            "angularVelocity": -4.0122481249990114e-7,
-            "velocityX": 0.5859277706610763,
-            "velocityY": 3.737288780149305,
-            "timestamp": 1.3922550101740108
-        },
-        {
-            "x": 7.56989445358868,
-            "y": 5.095450165697103,
-            "heading": 0.36620989603022114,
-            "angularVelocity": -0.036209374579162214,
-            "velocityX": 0.5612290747390308,
-            "velocityY": 3.7266550750338174,
-            "timestamp": 1.4850720108522781
-        },
-        {
-            "x": 7.614129631330889,
-            "y": 5.376850109081837,
-            "heading": 0.24917441522367106,
-            "angularVelocity": -1.2609272003114111,
-            "velocityX": 0.4765848650458117,
-            "velocityY": 3.0317715647821197,
-            "timestamp": 1.5778890115305455
-        },
-        {
-            "x": 7.6471559939174325,
-            "y": 5.587299345919279,
-            "heading": 0.1342077337923999,
-            "angularVelocity": -1.2386381868746383,
-            "velocityX": 0.35582234229936965,
-            "velocityY": 2.267356575838128,
-            "timestamp": 1.6707060122088129
-        },
-        {
-            "x": 7.669079055841979,
-            "y": 5.72720580380319,
-            "heading": 0.04709984861251457,
-            "angularVelocity": -0.9384906271840042,
-            "velocityX": 0.23619662092442378,
-            "velocityY": 1.5073365532341503,
-            "timestamp": 1.7635230128870802
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -9.59663427624948e-34,
-            "angularVelocity": -0.5074485090913172,
-            "velocityX": 0.11766103276571077,
-            "velocityY": 0.751954875580805,
-            "timestamp": 1.8563400135653476
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 1.9250732473517817e-32,
-            "velocityX": 0,
-            "velocityY": 2.1529996497623123e-34,
-            "timestamp": 1.949157014243615
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3.1.traj
deleted file mode 100644
index 4d225c1e..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3.1.traj	
+++ /dev/null
@@ -1,238 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0,
-            "velocityX": 2.653642496841617e-31,
-            "velocityY": -1.2761459725684476e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 5.194122324075795,
-            "y": 1.6116215778070426,
-            "heading": -0.6335243646936302,
-            "angularVelocity": 0.13206571622689564,
-            "velocityX": 0.3695911822570569,
-            "velocityY": -0.1708716250073132,
-            "timestamp": 0.049033431926404414
-        },
-        {
-            "x": 5.2306889078335095,
-            "y": 1.5955898422898989,
-            "heading": -0.6207226842720189,
-            "angularVelocity": 0.2610806529090014,
-            "velocityX": 0.745747999295838,
-            "velocityY": -0.3269551994890449,
-            "timestamp": 0.09806686385280883
-        },
-        {
-            "x": 5.286073830286703,
-            "y": 1.5728623577205654,
-            "heading": -0.6018022859553105,
-            "angularVelocity": 0.38586730672362957,
-            "velocityX": 1.1295338767299938,
-            "velocityY": -0.4635099701654054,
-            "timestamp": 0.14710029577921324
-        },
-        {
-            "x": 5.360700050095615,
-            "y": 1.5447484727758143,
-            "heading": -0.577064216904969,
-            "angularVelocity": 0.5045143298894789,
-            "velocityX": 1.5219456782249914,
-            "velocityY": -0.5733615584348081,
-            "timestamp": 0.19613372770561766
-        },
-        {
-            "x": 5.45500431506412,
-            "y": 1.513111103841015,
-            "heading": -0.5469692052395991,
-            "angularVelocity": 0.6137651492712723,
-            "velocityX": 1.9232646230039372,
-            "velocityY": -0.6452203668359155,
-            "timestamp": 0.24516715963202207
-        },
-        {
-            "x": 5.5692993756365,
-            "y": 1.480706991826158,
-            "heading": -0.5122625268421303,
-            "angularVelocity": 0.7078166270271339,
-            "velocityX": 2.330961878090668,
-            "velocityY": -0.6608575158164777,
-            "timestamp": 0.2942005915584265
-        },
-        {
-            "x": 5.703331364670941,
-            "y": 1.4516882815399292,
-            "heading": -0.47420565589215874,
-            "angularVelocity": 0.7761412867722233,
-            "velocityX": 2.733481703578104,
-            "velocityY": -0.5918147913809916,
-            "timestamp": 0.3432340234848309
-        },
-        {
-            "x": 5.855159622936256,
-            "y": 1.4318923943368613,
-            "heading": -0.4349045247507558,
-            "angularVelocity": 0.8015170384282412,
-            "velocityX": 3.096423242273411,
-            "velocityY": -0.40372224470801127,
-            "timestamp": 0.3922674554112353
-        },
-        {
-            "x": 6.019815732483991,
-            "y": 1.4274954608427641,
-            "heading": -0.3972296573283927,
-            "angularVelocity": 0.768350611865634,
-            "velocityX": 3.358037630221609,
-            "velocityY": -0.08967215471871418,
-            "timestamp": 0.4413008873376397
-        },
-        {
-            "x": 6.190507691154086,
-            "y": 1.4421792499476667,
-            "heading": -0.36375877276285506,
-            "angularVelocity": 0.6826135404074718,
-            "velocityX": 3.4811342376834093,
-            "velocityY": 0.2994648452702519,
-            "timestamp": 0.49033431926404414
-        },
-        {
-            "x": 6.361514712702491,
-            "y": 1.4768435219754552,
-            "heading": -0.33601787986089265,
-            "angularVelocity": 0.5657546659914753,
-            "velocityX": 3.487559708345244,
-            "velocityY": 0.7069517809772359,
-            "timestamp": 0.5393677511904486
-        },
-        {
-            "x": 6.529238224029541,
-            "y": 1.531233787536621,
-            "heading": -0.3133159497925571,
-            "angularVelocity": 0.4629888052692109,
-            "velocityX": 3.4205949846286416,
-            "velocityY": 1.1092485968081267,
-            "timestamp": 0.588401183116853
-        },
-        {
-            "x": 6.794330666624461,
-            "y": 1.6764087741891647,
-            "heading": -0.29089151826229404,
-            "angularVelocity": 0.27241251742532613,
-            "velocityX": 3.2203491776466797,
-            "velocityY": 1.7635891250039288,
-            "timestamp": 0.6707191020664587
-        },
-        {
-            "x": 7.034738463760469,
-            "y": 1.8721289187979775,
-            "heading": -0.28740556521383775,
-            "angularVelocity": 0.042347438965477144,
-            "velocityX": 2.9204795286821774,
-            "velocityY": 2.377612883150898,
-            "timestamp": 0.7530370210160644
-        },
-        {
-            "x": 7.236654527584481,
-            "y": 2.109198968129657,
-            "heading": -0.2874055261243408,
-            "angularVelocity": 4.748601214403234e-7,
-            "velocityX": 2.452881054338489,
-            "velocityY": 2.879932490479633,
-            "timestamp": 0.8353549399656701
-        },
-        {
-            "x": 7.392410351066591,
-            "y": 2.3788514084218195,
-            "heading": -0.2874055078665042,
-            "angularVelocity": 2.2179662469221753e-7,
-            "velocityX": 1.8921253776776576,
-            "velocityY": 3.2757441360636306,
-            "timestamp": 0.9176728589152758
-        },
-        {
-            "x": 7.49687844031107,
-            "y": 2.6722091028580075,
-            "heading": -0.28740549372573876,
-            "angularVelocity": 1.7178234776742224e-7,
-            "velocityX": 1.2690807855393922,
-            "velocityY": 3.5637161164844247,
-            "timestamp": 0.9999907778648816
-        },
-        {
-            "x": 7.546620338444338,
-            "y": 2.979614475892834,
-            "heading": -0.28740548096651763,
-            "angularVelocity": 1.5499931641741705e-7,
-            "velocityX": 0.6042657390764874,
-            "velocityY": 3.7343676438541644,
-            "timestamp": 1.0823086968144873
-        },
-        {
-            "x": 7.582493232504898,
-            "y": 3.288945132948743,
-            "heading": -0.2874054683444637,
-            "angularVelocity": 1.5333300623146833e-7,
-            "velocityX": 0.43578475401492967,
-            "velocityY": 3.7577560390622633,
-            "timestamp": 1.164626615764093
-        },
-        {
-            "x": 7.6150468277369106,
-            "y": 3.5683420563080905,
-            "heading": -0.1936159509616472,
-            "angularVelocity": 1.1393572454157397,
-            "velocityX": 0.39546183440153926,
-            "velocityY": 3.3941203437178378,
-            "timestamp": 1.2469445347136987
-        },
-        {
-            "x": 7.641044736890772,
-            "y": 3.791820131624945,
-            "heading": -0.11680359430817472,
-            "angularVelocity": 0.9331183007718732,
-            "velocityX": 0.31582320697043825,
-            "velocityY": 2.714816873027222,
-            "timestamp": 1.3292624536633044
-        },
-        {
-            "x": 7.660527962829294,
-            "y": 3.959415202649684,
-            "heading": -0.05861701943494889,
-            "angularVelocity": 0.7068518691380767,
-            "velocityX": 0.23668268327338735,
-            "velocityY": 2.035948833052459,
-            "timestamp": 1.4115803726129101
-        },
-        {
-            "x": 7.673510674167818,
-            "y": 4.0711397209924485,
-            "heading": -0.019593791766857216,
-            "angularVelocity": 0.4740550801809447,
-            "velocityX": 0.1577142802460941,
-            "velocityY": 1.3572320555281903,
-            "timestamp": 1.4938982915625159
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -2.071552209824535e-33,
-            "angularVelocity": 0.23802583953553896,
-            "velocityX": 0.07883248161483059,
-            "velocityY": 0.6785919726876813,
-            "timestamp": 1.5762162105121216
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -4.785738309812717e-31,
-            "velocityX": -1.2484093100405624e-30,
-            "velocityY": 4.284452606087597e-31,
-            "timestamp": 1.6585341294617273
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.1.traj
deleted file mode 100644
index 751754fd..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.222401190948388,
-            "y": 1.635609178560311,
-            "heading": -0.6120137040899031,
-            "angularVelocity": 0.36292240482976423,
-            "velocityX": 0.6017242103081577,
-            "velocityY": 0.20241766322785323,
-            "timestamp": 0.07711371780208913
-        },
-        {
-            "x": 5.315327100770923,
-            "y": 1.6668589219810437,
-            "heading": -0.5590312951883875,
-            "angularVelocity": 0.6870685321838068,
-            "velocityX": 1.205050313629373,
-            "velocityY": 0.4052423396435706,
-            "timestamp": 0.15422743560417826
-        },
-        {
-            "x": 5.454938302762681,
-            "y": 1.713800132384211,
-            "heading": -0.4856728473163301,
-            "angularVelocity": 0.9513021802467544,
-            "velocityX": 1.8104587091763231,
-            "velocityY": 0.6087271077195515,
-            "timestamp": 0.23134115340626737
-        },
-        {
-            "x": 5.641435450684747,
-            "y": 1.7765099385015581,
-            "heading": -0.4002091399727602,
-            "angularVelocity": 1.1082815065785667,
-            "velocityX": 2.418469154874715,
-            "velocityY": 0.8132120704942623,
-            "timestamp": 0.3084548712083565
-        },
-        {
-            "x": 5.8748863814192855,
-            "y": 1.8550365409977465,
-            "heading": -0.32154781798640775,
-            "angularVelocity": 1.0200691164734093,
-            "velocityX": 3.0273592998548686,
-            "velocityY": 1.0183220928048784,
-            "timestamp": 0.38556858901044566
-        },
-        {
-            "x": 6.151206632813102,
-            "y": 1.945953456728306,
-            "heading": -0.3195100296755088,
-            "angularVelocity": 0.026425756259478697,
-            "velocityX": 3.583282705976986,
-            "velocityY": 1.178997956808355,
-            "timestamp": 0.4626823068125348
-        },
-        {
-            "x": 6.4277763897122595,
-            "y": 2.0387313847088295,
-            "heading": -0.31950999788997536,
-            "angularVelocity": 4.121903903341971e-7,
-            "velocityX": 3.5865182587742526,
-            "velocityY": 1.2031313056210848,
-            "timestamp": 0.539796024614624
-        },
-        {
-            "x": 6.7043461469693755,
-            "y": 2.1315093116222874,
-            "heading": -0.31950996610444216,
-            "angularVelocity": 4.121903870025697e-7,
-            "velocityX": 3.5865182634161936,
-            "velocityY": 1.2031312917835268,
-            "timestamp": 0.616909742416713
-        },
-        {
-            "x": 6.980831721332989,
-            "y": 2.2223162260189957,
-            "heading": -0.31777519026878787,
-            "angularVelocity": 0.022496332495664188,
-            "velocityX": 3.5854265913259273,
-            "velocityY": 1.1775714747635835,
-            "timestamp": 0.6940234602188021
-        },
-        {
-            "x": 7.214355031558839,
-            "y": 2.3007720927806434,
-            "heading": -0.23870969056825803,
-            "angularVelocity": 1.0253104370281303,
-            "velocityX": 3.028297907062184,
-            "velocityY": 1.0174048016074528,
-            "timestamp": 0.7711371780208912
-        },
-        {
-            "x": 7.400917361819277,
-            "y": 2.3634042679352882,
-            "heading": -0.15373574527352316,
-            "angularVelocity": 1.1019303402387481,
-            "velocityX": 2.419314430400663,
-            "velocityY": 0.8122053629341165,
-            "timestamp": 0.8482508958229803
-        },
-        {
-            "x": 7.540592119323217,
-            "y": 2.4102654434053137,
-            "heading": -0.08076255152981597,
-            "angularVelocity": 0.9463062581289249,
-            "velocityX": 1.8112828882458099,
-            "velocityY": 0.6076892258040765,
-            "timestamp": 0.9253646136250694
-        },
-        {
-            "x": 7.633568850123268,
-            "y": 2.4414404537843537,
-            "heading": -0.02793953661151031,
-            "angularVelocity": 0.6850015331107969,
-            "velocityX": 1.2057093530190521,
-            "velocityY": 0.4042732119212608,
-            "timestamp": 1.0024783314271586
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -3.3818227179407965e-32,
-            "angularVelocity": 0.3623160367292381,
-            "velocityX": 0.6021127135368662,
-            "velocityY": 0.20177403786417583,
-            "timestamp": 1.0795920492292477
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -1.7311819232040874e-32,
-            "angularVelocity": 2.140534191184349e-31,
-            "velocityX": 0,
-            "velocityY": -2.0522160776390996e-31,
-            "timestamp": 1.1567057670313368
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.1.traj
deleted file mode 100644
index 0bd0e944..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.22245562506892,
-            "y": 1.6045082039129626,
-            "heading": -0.6133597798196372,
-            "angularVelocity": 0.3456549186542876,
-            "velocityX": 0.6027583554308628,
-            "velocityY": -0.20100492713723658,
-            "timestamp": 0.07707172310487943
-        },
-        {
-            "x": 5.31546298986369,
-            "y": 1.573504853521539,
-            "heading": -0.5625082909022232,
-            "angularVelocity": 0.6597943690478437,
-            "velocityX": 1.2067637915426601,
-            "velocityY": -0.4022662157070755,
-            "timestamp": 0.15414344620975887
-        },
-        {
-            "x": 5.455153171011431,
-            "y": 1.526958597742617,
-            "heading": -0.4912836919139272,
-            "angularVelocity": 0.924133989989731,
-            "velocityX": 1.8124699373549893,
-            "velocityY": -0.6039342822993798,
-            "timestamp": 0.2312151693146383
-        },
-        {
-            "x": 5.641701159953251,
-            "y": 1.4648201004985182,
-            "heading": -0.406509898431932,
-            "angularVelocity": 1.0999338027856826,
-            "velocityX": 2.420446584384324,
-            "velocityY": -0.8062424809101578,
-            "timestamp": 0.30828689241951773
-        },
-        {
-            "x": 5.875244890448534,
-            "y": 1.3870507382512116,
-            "heading": -0.323292875073049,
-            "angularVelocity": 1.0797348236997513,
-            "velocityX": 3.03021291190643,
-            "velocityY": -1.0090518171168696,
-            "timestamp": 0.38535861552439715
-        },
-        {
-            "x": 6.151315012657669,
-            "y": 1.295160148354896,
-            "heading": -0.32123146568205607,
-            "angularVelocity": 0.026746636872095383,
-            "velocityX": 3.5819897504232276,
-            "velocityY": -1.192273718485189,
-            "timestamp": 0.46243033862927657
-        },
-        {
-            "x": 6.427941847872596,
-            "y": 1.2030541885235448,
-            "heading": -0.3212314367193528,
-            "angularVelocity": 3.757889680371681e-7,
-            "velocityX": 3.5892130611702004,
-            "velocityY": -1.1950681277232245,
-            "timestamp": 0.539502061734156
-        },
-        {
-            "x": 6.704568684105482,
-            "y": 1.1109482317494896,
-            "heading": -0.3212314077566493,
-            "angularVelocity": 3.7578897090221527e-7,
-            "velocityX": 3.589213074378161,
-            "velocityY": -1.1950680880550335,
-            "timestamp": 0.6165737848390355
-        },
-        {
-            "x": 6.9807077062893965,
-            "y": 1.0195126541212949,
-            "heading": -0.3189025007582472,
-            "angularVelocity": 0.030217398866675224,
-            "velocityX": 3.582883722583232,
-            "velocityY": -1.1863699674103443,
-            "timestamp": 0.6936455079439149
-        },
-        {
-            "x": 7.214256981896637,
-            "y": 0.9418352430707998,
-            "heading": -0.2350676331360113,
-            "angularVelocity": 1.0877513080660361,
-            "velocityX": 3.0302848593306506,
-            "velocityY": -1.0078587570280646,
-            "timestamp": 0.7707172310487943
-        },
-        {
-            "x": 7.4008131962231865,
-            "y": 0.8797969152412516,
-            "heading": -0.14969687190170503,
-            "angularVelocity": 1.1076794159400525,
-            "velocityX": 2.4205533081527695,
-            "velocityY": -0.8049427900441033,
-            "timestamp": 0.8477889541536737
-        },
-        {
-            "x": 7.54051565231881,
-            "y": 0.8333524229780926,
-            "heading": -0.0780215306756512,
-            "angularVelocity": 0.9299823377312819,
-            "velocityX": 1.812629203910703,
-            "velocityY": -0.6026139080860716,
-            "timestamp": 0.9248606772585531
-        },
-        {
-            "x": 7.633536087871107,
-            "y": 0.802437413208921,
-            "heading": -0.026843150662094223,
-            "angularVelocity": 0.6640357572376224,
-            "velocityX": 1.2069333836706095,
-            "velocityY": -0.401120002560503,
-            "timestamp": 1.0019324003634327
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 3.8658238709310755e-33,
-            "angularVelocity": 0.3482879268907221,
-            "velocityX": 0.6028658794310917,
-            "velocityY": -0.20029931325025216,
-            "timestamp": 1.079004123468312
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 4.381697144346615e-34,
-            "angularVelocity": -4.4473487408728983e-32,
-            "velocityX": 0,
-            "velocityY": -1.5918634852021244e-32,
-            "timestamp": 1.1560758465731915
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W1.1.traj
deleted file mode 100644
index 90e7d73f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W1.1.traj	
+++ /dev/null
@@ -1,247 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0,
-            "velocityX": -4.2485019622555225e-35,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.123544064624381,
-            "y": 1.640479835564366,
-            "heading": -0.5261105113915723,
-            "angularVelocity": 1.3044458865146935,
-            "velocityX": -0.6008098724480858,
-            "velocityY": 0.23456806756139406,
-            "timestamp": 0.08730871076049408
-        },
-        {
-            "x": 5.016418569113309,
-            "y": 1.6815339921073649,
-            "heading": -0.31680443246750034,
-            "angularVelocity": 2.397310384049101,
-            "velocityX": -1.2269737415426907,
-            "velocityY": 0.4702183342922002,
-            "timestamp": 0.17461742152098816
-        },
-        {
-            "x": 4.8522086939147036,
-            "y": 1.7459427773647251,
-            "heading": -0.051891218250848345,
-            "angularVelocity": 3.0342128741697256,
-            "velocityX": -1.8807960141464901,
-            "velocityY": 0.7377131639710823,
-            "timestamp": 0.26192613228148226
-        },
-        {
-            "x": 4.632595451769563,
-            "y": 1.839668047804236,
-            "heading": 0.17554509517769362,
-            "angularVelocity": 2.6049670353333583,
-            "velocityX": -2.515364620920639,
-            "velocityY": 1.073492777789593,
-            "timestamp": 0.34923484304197633
-        },
-        {
-            "x": 4.3704901470738005,
-            "y": 1.9627849470607086,
-            "heading": 0.27538033292006053,
-            "angularVelocity": 1.1434739658020574,
-            "velocityX": -3.002052171114658,
-            "velocityY": 1.4101330575617794,
-            "timestamp": 0.4365435538024704
-        },
-        {
-            "x": 4.075718453010027,
-            "y": 2.1117712847634245,
-            "heading": 0.2753804447713452,
-            "angularVelocity": 0.0000012811010915301473,
-            "velocityX": -3.3762002839830365,
-            "velocityY": 1.7064315393617047,
-            "timestamp": 0.5238522645629645
-        },
-        {
-            "x": 3.780946849777969,
-            "y": 2.260757802178556,
-            "heading": 0.2753805566219007,
-            "angularVelocity": 0.0000012810927397041138,
-            "velocityX": -3.3761992436319233,
-            "velocityY": 1.7064335977177825,
-            "timestamp": 0.6111609753234586
-        },
-        {
-            "x": 3.4861752863833098,
-            "y": 2.409744398412427,
-            "heading": 0.27538066847245973,
-            "angularVelocity": 0.0000012810927810381003,
-            "velocityX": -3.3761987873498507,
-            "velocityY": 1.7064345004769594,
-            "timestamp": 0.6984696860839527
-        },
-        {
-            "x": 3.191403823869153,
-            "y": 2.5587311942392557,
-            "heading": 0.2753807803230405,
-            "angularVelocity": 0.000001281093028963251,
-            "velocityX": -3.3761976319038296,
-            "velocityY": 1.7064367865370367,
-            "timestamp": 0.7857783968444467
-        },
-        {
-            "x": 2.907963654667952,
-            "y": 2.72828634801538,
-            "heading": 0.27538089480545036,
-            "angularVelocity": 0.0000013112369766496227,
-            "velocityX": -3.246413407463273,
-            "velocityY": 1.9420187550501031,
-            "timestamp": 0.8730871076049408
-        },
-        {
-            "x": 2.662249448997808,
-            "y": 2.9489938499602837,
-            "heading": 0.27538103946019826,
-            "angularVelocity": 0.000001656819194794751,
-            "velocityX": -2.81431490088302,
-            "velocityY": 2.527897846875211,
-            "timestamp": 0.9603958183654349
-        },
-        {
-            "x": 2.463361015566044,
-            "y": 3.212680537293485,
-            "heading": 0.27538116270778873,
-            "angularVelocity": 0.0000014116299433562534,
-            "velocityX": -2.277990726232991,
-            "velocityY": 3.0201647125055904,
-            "timestamp": 1.047704529125929
-        },
-        {
-            "x": 2.3186638703555946,
-            "y": 3.509581240155329,
-            "heading": 0.2753811333053821,
-            "angularVelocity": -3.3676372418890626e-7,
-            "velocityX": -1.657304797540575,
-            "velocityY": 3.4005851223287915,
-            "timestamp": 1.1350132398864232
-        },
-        {
-            "x": 2.233516432992889,
-            "y": 3.8287006658490608,
-            "heading": 0.2753811088610493,
-            "angularVelocity": -2.799758765995743e-7,
-            "velocityX": -0.9752456154836949,
-            "velocityY": 3.655069728026828,
-            "timestamp": 1.2223219506469174
-        },
-        {
-            "x": 2.1898375161467403,
-            "y": 4.15608338798651,
-            "heading": 0.27538108549661844,
-            "angularVelocity": -2.6760709935894875e-7,
-            "velocityX": -0.5002813174732252,
-            "velocityY": 3.749714310127982,
-            "timestamp": 1.3096306614074116
-        },
-        {
-            "x": 2.1796859113101297,
-            "y": 4.486211000706444,
-            "heading": 0.2753810837562719,
-            "angularVelocity": -1.9933252293602923e-8,
-            "velocityX": -0.11627253166592438,
-            "velocityY": 3.78115321878412,
-            "timestamp": 1.3969393721679058
-        },
-        {
-            "x": 2.197179274626327,
-            "y": 4.816031069152981,
-            "heading": 0.2753810893961792,
-            "angularVelocity": 6.459730400705482e-8,
-            "velocityX": 0.20036217650934227,
-            "velocityY": 3.7776307263464504,
-            "timestamp": 1.4842480829284
-        },
-        {
-            "x": 2.214672942788018,
-            "y": 5.1458511214306375,
-            "heading": 0.27538109503608554,
-            "angularVelocity": 6.459729260879602e-8,
-            "velocityX": 0.20036566809100828,
-            "velocityY": 3.777630541154376,
-            "timestamp": 1.5715567936888941
-        },
-        {
-            "x": 2.2321666974024836,
-            "y": 5.475671169122818,
-            "heading": 0.2753811006759881,
-            "angularVelocity": 6.459724966098765e-8,
-            "velocityX": 0.20036665828744704,
-            "velocityY": 3.7776304886341237,
-            "timestamp": 1.6588655044493883
-        },
-        {
-            "x": 2.2496605217353887,
-            "y": 5.805491213117091,
-            "heading": 0.2753811063158993,
-            "angularVelocity": 6.459734855412882e-8,
-            "velocityX": 0.20036745681532722,
-            "velocityY": 3.7776304462797263,
-            "timestamp": 1.7461742152098825
-        },
-        {
-            "x": 2.2664262161021513,
-            "y": 6.122693971906954,
-            "heading": 0.30917552472065174,
-            "angularVelocity": 0.3870681185232194,
-            "velocityX": 0.19202773950877436,
-            "velocityY": 3.633116971112041,
-            "timestamp": 1.8334829259703767
-        },
-        {
-            "x": 2.279851444819284,
-            "y": 6.376429629998848,
-            "heading": 0.34011357351362637,
-            "angularVelocity": 0.35435237244361656,
-            "velocityX": 0.15376734578020115,
-            "velocityY": 2.9061894956614607,
-            "timestamp": 1.9207916367308708
-        },
-        {
-            "x": 2.289924296428913,
-            "y": 6.566719611746641,
-            "heading": 0.3645867540299274,
-            "angularVelocity": 0.2803062867740199,
-            "velocityX": 0.11537052284807026,
-            "velocityY": 2.179507406423607,
-            "timestamp": 2.008100347491365
-        },
-        {
-            "x": 2.296641079194263,
-            "y": 6.6935744437684805,
-            "heading": 0.3814115840440414,
-            "angularVelocity": 0.192705056203017,
-            "velocityX": 0.07693141619941274,
-            "velocityY": 1.4529458849739352,
-            "timestamp": 2.095409058251859
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0.09836837448577633,
-            "velocityX": 0.03847177190545204,
-            "velocityY": 0.7264516412973858,
-            "timestamp": 2.182717769012353
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 2.6936232860428826e-33,
-            "velocityX": 0,
-            "velocityY": 5.62663175256514e-35,
-            "timestamp": 2.2700264797728473
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W2.1.traj
deleted file mode 100644
index 97118e90..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W2.1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -2.256840111784074e-31,
-            "velocityX": -2.1308644711626517e-33,
-            "velocityY": -1.0217798620245829e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 5.1132615869041915,
-            "y": 1.6556053336408048,
-            "heading": -0.6461329738139044,
-            "angularVelocity": -0.06592692141286781,
-            "velocityX": -0.6744118848768044,
-            "velocityY": 0.3827425493802767,
-            "timestamp": 0.09302684976743651
-        },
-        {
-            "x": 4.987784771636742,
-            "y": 1.7268160147391884,
-            "heading": -0.6583988506620269,
-            "angularVelocity": -0.131853081973501,
-            "velocityX": -1.348823652323362,
-            "velocityY": 0.7654852472851404,
-            "timestamp": 0.18605369953487302
-        },
-        {
-            "x": 4.799569566809376,
-            "y": 1.8336320552005005,
-            "heading": -0.6767974581881981,
-            "angularVelocity": -0.19777738977689827,
-            "velocityX": -2.02323528419909,
-            "velocityY": 1.148228073167565,
-            "timestamp": 0.2790805493023095
-        },
-        {
-            "x": 4.54861599428641,
-            "y": 1.9760534609244509,
-            "heading": -0.7013284716868837,
-            "angularVelocity": -0.26369820713065356,
-            "velocityX": -2.697646681042515,
-            "velocityY": 1.5309709624694186,
-            "timestamp": 0.37210739906974605
-        },
-        {
-            "x": 4.242554383646119,
-            "y": 2.1497500028424636,
-            "heading": -0.7013284745367188,
-            "angularVelocity": -3.063454187141868e-8,
-            "velocityX": -3.29003520387323,
-            "velocityY": 1.8671656876724017,
-            "timestamp": 0.4651342488371826
-        },
-        {
-            "x": 3.9364928269084327,
-            "y": 2.3234466397396827,
-            "heading": -0.7013284773864732,
-            "angularVelocity": -3.0633677229161635e-8,
-            "velocityX": -3.290034624442601,
-            "velocityY": 1.8671667086594281,
-            "timestamp": 0.558161098604619
-        },
-        {
-            "x": 3.630431318940711,
-            "y": 2.497143362571822,
-            "heading": -0.7013284802362207,
-            "angularVelocity": -3.063360291364155e-8,
-            "velocityX": -3.2900341001857365,
-            "velocityY": 1.867167632424124,
-            "timestamp": 0.6511879483720555
-        },
-        {
-            "x": 3.3243698648507793,
-            "y": 2.6708401803390394,
-            "heading": -0.7013284830859674,
-            "angularVelocity": -3.063359264364299e-8,
-            "velocityX": -3.2900335210218676,
-            "velocityY": 1.8671686529367946,
-            "timestamp": 0.744214798139492
-        },
-        {
-            "x": 3.0183085209148324,
-            "y": 2.844537192202297,
-            "heading": -0.7013284859357423,
-            "angularVelocity": -3.063389607722874e-8,
-            "velocityX": -3.290032336912287,
-            "velocityY": 1.8671707393886097,
-            "timestamp": 0.8372416479069285
-        },
-        {
-            "x": 2.714360997870643,
-            "y": 3.021907148464231,
-            "heading": -0.7013284887932081,
-            "angularVelocity": -3.071657002046057e-8,
-            "velocityX": -3.2673096402172703,
-            "velocityY": 1.9066533662630898,
-            "timestamp": 0.9302684976743649
-        },
-        {
-            "x": 2.4529780195268613,
-            "y": 3.2575406255854364,
-            "heading": -0.7013284919399162,
-            "angularVelocity": -3.382580562568125e-8,
-            "velocityX": -2.8097584621776335,
-            "velocityY": 2.532962018065539,
-            "timestamp": 1.0232953474418014
-        },
-        {
-            "x": 2.2451503238556145,
-            "y": 3.5415333045009083,
-            "heading": -0.7013284961513817,
-            "angularVelocity": -4.527150531202637e-8,
-            "velocityX": -2.2340614155032408,
-            "velocityY": 3.0528033532839474,
-            "timestamp": 1.1163221972092379
-        },
-        {
-            "x": 2.099615704405981,
-            "y": 3.861945316168419,
-            "heading": -0.7013284332934493,
-            "angularVelocity": 6.756966674200994e-7,
-            "velocityX": -1.564436717071087,
-            "velocityY": 3.444296055047848,
-            "timestamp": 1.2093490469766743
-        },
-        {
-            "x": 2.0224929199626733,
-            "y": 4.205305507968674,
-            "heading": -0.7013282219960775,
-            "angularVelocity": 0.0000022713589924444385,
-            "velocityX": -0.8290379028862299,
-            "velocityY": 3.6909794608614943,
-            "timestamp": 1.3023758967441108
-        },
-        {
-            "x": 2.0170244087221176,
-            "y": 4.5571780060792255,
-            "heading": -0.7013280555552984,
-            "angularVelocity": 0.0000017891692504432307,
-            "velocityX": -0.05878422470745493,
-            "velocityY": 3.782483218449498,
-            "timestamp": 1.3954027465115473
-        },
-        {
-            "x": 2.079587370397571,
-            "y": 4.887709205686539,
-            "heading": -0.661535006789308,
-            "angularVelocity": 0.4277587477752017,
-            "velocityX": 0.6725258549747536,
-            "velocityY": 3.5530731227987227,
-            "timestamp": 1.4884295962789837
-        },
-        {
-            "x": 2.143441139356313,
-            "y": 5.160361503306635,
-            "heading": -0.462273406431507,
-            "angularVelocity": 2.1419794484704924,
-            "velocityX": 0.6864014971846709,
-            "velocityY": 2.9308989641347156,
-            "timestamp": 1.5814564460464202
-        },
-        {
-            "x": 2.1958862911911914,
-            "y": 5.3618445595482385,
-            "heading": -0.25085794958853325,
-            "angularVelocity": 2.272628358065485,
-            "velocityX": 0.5637636012182422,
-            "velocityY": 2.165859176628076,
-            "timestamp": 1.6744832958138567
-        },
-        {
-            "x": 2.231834886718389,
-            "y": 5.494391410801975,
-            "heading": -0.08836645960984135,
-            "angularVelocity": 1.7467160328971074,
-            "velocityX": 0.38643247209883785,
-            "velocityY": 1.4248236029178551,
-            "timestamp": 1.7675101455812932
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -3.8128513304390615e-33,
-            "angularVelocity": 0.9499027413134385,
-            "velocityX": 0.19526742362042082,
-            "velocityY": 0.7052650859622128,
-            "timestamp": 1.8605369953487296
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 5.82518331797089e-32,
-            "velocityX": 0,
-            "velocityY": -4.1086986258215814e-33,
-            "timestamp": 1.953563845116166
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.1.traj
deleted file mode 100644
index 8a67252f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.1.traj	
+++ /dev/null
@@ -1,256 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -2.137980318394988e-34,
-            "velocityX": 1.3258659962604387e-36,
-            "velocityY": 7.61509960712835e-36,
-            "timestamp": 0
-        },
-        {
-            "x": 5.139985955005962,
-            "y": 1.6266018860262446,
-            "heading": -0.6327443563347177,
-            "angularVelocity": 0.10940864810395588,
-            "velocityX": -0.5430597418126536,
-            "velocityY": 0.09955056482775779,
-            "timestamp": 0.06631691178916982
-        },
-        {
-            "x": 5.067957879081044,
-            "y": 1.6398056681484225,
-            "heading": -0.6182319813670383,
-            "angularVelocity": 0.2188336968074761,
-            "velocityX": -1.0861192715653594,
-            "velocityY": 0.19910128149746392,
-            "timestamp": 0.13263382357833964
-        },
-        {
-            "x": 4.959915797050431,
-            "y": 1.6596113536414707,
-            "heading": -0.5964609094580124,
-            "angularVelocity": 0.328288385596709,
-            "velocityX": -1.6291784269764191,
-            "velocityY": 0.2986521078667379,
-            "timestamp": 0.19895073536750946
-        },
-        {
-            "x": 4.815859744494909,
-            "y": 1.686018943789264,
-            "heading": -0.5674282989321845,
-            "angularVelocity": 0.43778592432238833,
-            "velocityX": -2.172237045860861,
-            "velocityY": 0.39820295359570684,
-            "timestamp": 0.2652676471566793
-        },
-        {
-            "x": 4.635789767966758,
-            "y": 1.719028429378711,
-            "heading": -0.5311304969514992,
-            "angularVelocity": 0.5473385445944894,
-            "velocityX": -2.7152949627783816,
-            "velocityY": 0.49775366039945246,
-            "timestamp": 0.3315845589458491
-        },
-        {
-            "x": 4.41970592970577,
-            "y": 1.758639783689655,
-            "heading": -0.48756329690665473,
-            "angularVelocity": 0.6569545967904882,
-            "velocityX": -3.258351941175237,
-            "velocityY": 0.5973039642870227,
-            "timestamp": 0.3979014707350189
-        },
-        {
-            "x": 4.172944378919847,
-            "y": 1.8038720825822125,
-            "heading": -0.4875632898742738,
-            "angularVelocity": 1.0604204507812919e-7,
-            "velocityX": -3.7209445393116334,
-            "velocityY": 0.6820628052819626,
-            "timestamp": 0.46421838252418873
-        },
-        {
-            "x": 3.9261828364502485,
-            "y": 1.849104426845956,
-            "heading": -0.4875632828424199,
-            "angularVelocity": 1.0603409772331237e-7,
-            "velocityX": -3.7209444139088643,
-            "velocityY": 0.6820634894390606,
-            "timestamp": 0.5305352943133586
-        },
-        {
-            "x": 3.6794213056437455,
-            "y": 1.8943368347367484,
-            "heading": -0.487563275810563,
-            "angularVelocity": 1.0603414195839125e-7,
-            "velocityX": -3.720944238039768,
-            "velocityY": 0.6820644488783205,
-            "timestamp": 0.5968522061025283
-        },
-        {
-            "x": 3.4326599282156307,
-            "y": 1.9395700793621615,
-            "heading": -0.4875632687787025,
-            "angularVelocity": 1.0603419804191013e-7,
-            "velocityX": -3.720941925230195,
-            "velocityY": 0.6820770660916139,
-            "timestamp": 0.6631691178916981
-        },
-        {
-            "x": 3.192328571865682,
-            "y": 2.011528836252569,
-            "heading": -0.4875632615410612,
-            "angularVelocity": 1.0913718718412795e-7,
-            "velocityX": -3.6239829308396314,
-            "velocityY": 1.0850740022269671,
-            "timestamp": 0.7294860296808678
-        },
-        {
-            "x": 2.965054771843869,
-            "y": 2.1177542675713954,
-            "heading": -0.48756325344928275,
-            "angularVelocity": 1.2201681646141545e-7,
-            "velocityX": -3.4270866041583203,
-            "velocityY": 1.6017849512735258,
-            "timestamp": 0.7958029414700376
-        },
-        {
-            "x": 2.7556945515371734,
-            "y": 2.2559769801698577,
-            "heading": -0.48756324342419455,
-            "angularVelocity": 1.5116940667572282e-7,
-            "velocityX": -3.156965767228115,
-            "velocityY": 2.084275471660842,
-            "timestamp": 0.8621198532592074
-        },
-        {
-            "x": 2.5687211592217127,
-            "y": 2.4232437322251608,
-            "heading": -0.4875632292213124,
-            "angularVelocity": 2.1416682100483036e-7,
-            "velocityX": -2.8193923280063653,
-            "velocityY": 2.5222337340898235,
-            "timestamp": 0.9284367650483771
-        },
-        {
-            "x": 2.4081295337898796,
-            "y": 2.6159806661875145,
-            "heading": -0.48756314249031313,
-            "angularVelocity": 0.0000013078262679985986,
-            "velocityX": -2.421578766248568,
-            "velocityY": 2.906301405817713,
-            "timestamp": 0.9947536768375469
-        },
-        {
-            "x": 2.2783797783684188,
-            "y": 2.8285106690600896,
-            "heading": -0.4828540771785898,
-            "angularVelocity": 0.07100851328382284,
-            "velocityX": -1.9565108193510647,
-            "velocityY": 3.2047632668456583,
-            "timestamp": 1.0610705886267167
-        },
-        {
-            "x": 2.1829307079315186,
-            "y": 3.053825616836548,
-            "heading": -0.47038335932986286,
-            "angularVelocity": 0.18804732476646344,
-            "velocityX": -1.4392870213912456,
-            "velocityY": 3.3975488559051734,
-            "timestamp": 1.1273875004158864
-        },
-        {
-            "x": 2.124568521000596,
-            "y": 3.2749812318761844,
-            "heading": -0.4550563591461243,
-            "angularVelocity": 0.24322153086635045,
-            "velocityX": -0.9261395106595871,
-            "velocityY": 3.5094804335357668,
-            "timestamp": 1.1904041261609186
-        },
-        {
-            "x": 2.099309698866093,
-            "y": 3.4965263857952165,
-            "heading": -0.4375068558264715,
-            "angularVelocity": 0.2784900510328627,
-            "velocityX": -0.40082790590377965,
-            "velocityY": 3.5156619590425686,
-            "timestamp": 1.2534207519059508
-        },
-        {
-            "x": 2.105558929434475,
-            "y": 3.707892405315209,
-            "heading": -0.4199092504656253,
-            "angularVelocity": 0.2792533740547583,
-            "velocityX": 0.09916796550908585,
-            "velocityY": 3.354131025281934,
-            "timestamp": 1.316437377650983
-        },
-        {
-            "x": 2.135680280502732,
-            "y": 3.8963278332771982,
-            "heading": -0.40525850037923283,
-            "angularVelocity": 0.23249023433387317,
-            "velocityX": 0.4779905415775436,
-            "velocityY": 2.990249410122441,
-            "timestamp": 1.3794540033960152
-        },
-        {
-            "x": 2.177481885950485,
-            "y": 4.0538159629335215,
-            "heading": -0.39590861500997965,
-            "angularVelocity": 0.14837172347315433,
-            "velocityX": 0.6633424902324003,
-            "velocityY": 2.4991520538330105,
-            "timestamp": 1.4424706291410474
-        },
-        {
-            "x": 2.2205433619064863,
-            "y": 4.17824561445827,
-            "heading": -0.39150641146352744,
-            "angularVelocity": 0.06985781124276201,
-            "velocityX": 0.6833351587282585,
-            "velocityY": 1.9745527478446014,
-            "timestamp": 1.5054872548860796
-        },
-        {
-            "x": 2.2579292824582606,
-            "y": 4.270063380418578,
-            "heading": -0.38992694464863337,
-            "angularVelocity": 0.0250642873403699,
-            "velocityX": 0.5932707457717501,
-            "velocityY": 1.4570403425249505,
-            "timestamp": 1.5685038806311118
-        },
-        {
-            "x": 2.285342089257776,
-            "y": 4.3303097045050025,
-            "heading": -0.38974725509942826,
-            "angularVelocity": 0.002851462563739305,
-            "velocityX": 0.43500911823537725,
-            "velocityY": 0.9560385592555154,
-            "timestamp": 1.631520506376144
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.004010765374750144,
-            "velocityX": 0.23260386555018367,
-            "velocityY": 0.47115019479344145,
-            "timestamp": 1.6945371321211762
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -1.6105528525551922e-32,
-            "velocityX": 0,
-            "velocityY": -4.5276793712997926e-36,
-            "timestamp": 1.7575537578662084
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.1.traj
deleted file mode 100644
index a684ff7a..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.1.traj	
+++ /dev/null
@@ -1,616 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 5.951430910881942e-24,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7172662637575132,
-            "y": 4.364294720467062,
-            "heading": -1.0315470131742317,
-            "angularVelocity": 0.26098153509522004,
-            "velocityX": 0.22434208273715575,
-            "velocityY": 0.13259724206381326,
-            "timestamp": 0.03238921413610362
-        },
-        {
-            "x": 0.7318144654254066,
-            "y": 4.3728967855708385,
-            "heading": -1.0149312072454106,
-            "angularVelocity": 0.5130042939294357,
-            "velocityX": 0.4491680967238058,
-            "velocityY": 0.2655842487449578,
-            "timestamp": 0.06477842827220724
-        },
-        {
-            "x": 0.7536624687467911,
-            "y": 4.385820612299006,
-            "heading": -0.9904958605622504,
-            "angularVelocity": 0.7544285137786853,
-            "velocityX": 0.6745456444104029,
-            "velocityY": 0.39901637235962667,
-            "timestamp": 0.09716764240831086
-        },
-        {
-            "x": 0.7828310955502448,
-            "y": 4.4030821899572965,
-            "heading": -0.9586530378385134,
-            "angularVelocity": 0.9831304516969581,
-            "velocityX": 0.9005660551343861,
-            "velocityY": 0.5329421574032179,
-            "timestamp": 0.12955685654441448
-        },
-        {
-            "x": 0.8193448564336872,
-            "y": 4.424698840047832,
-            "heading": -0.919902879162077,
-            "angularVelocity": 1.1963908266993857,
-            "velocityX": 1.1273432177146,
-            "velocityY": 0.6674027347406944,
-            "timestamp": 0.1619460706805181
-        },
-        {
-            "x": 0.8632324943492647,
-            "y": 4.450689040734006,
-            "heading": -0.8748582540701377,
-            "angularVelocity": 1.3907291761589549,
-            "velocityX": 1.3550078038681697,
-            "velocityY": 0.8024338156819394,
-            "timestamp": 0.19433528481662174
-        },
-        {
-            "x": 0.9145271224696989,
-            "y": 4.481072488182425,
-            "heading": -0.8242801160685745,
-            "angularVelocity": 1.5615734852049024,
-            "velocityX": 1.5836947418633758,
-            "velocityY": 0.9380730054376618,
-            "timestamp": 0.22672449895272537
-        },
-        {
-            "x": 0.9732655893831887,
-            "y": 4.515870568727518,
-            "heading": -0.7691375276578237,
-            "angularVelocity": 1.7024984977725797,
-            "velocityX": 1.8135193606940647,
-            "velocityY": 1.0743724870528544,
-            "timestamp": 0.25911371308882897
-        },
-        {
-            "x": 1.0394861119792222,
-            "y": 4.555106942812012,
-            "heading": -0.7107271690719036,
-            "angularVelocity": 1.8033891881560444,
-            "velocityX": 2.044523905944935,
-            "velocityY": 1.2114024724285466,
-            "timestamp": 0.2915029272249326
-        },
-        {
-            "x": 1.1132202582415271,
-            "y": 4.598805444215395,
-            "heading": -0.6509416110612904,
-            "angularVelocity": 1.8458477491731227,
-            "velocityX": 2.276503096137642,
-            "velocityY": 1.3491683132463759,
-            "timestamp": 0.3238921413610362
-        },
-        {
-            "x": 1.194458181637227,
-            "y": 4.646971254862031,
-            "heading": -0.5929406477835183,
-            "angularVelocity": 1.790749322723445,
-            "velocityX": 2.508178279791774,
-            "velocityY": 1.48709414326129,
-            "timestamp": 0.35628135549713985
-        },
-        {
-            "x": 1.2829200414273145,
-            "y": 4.699514481647374,
-            "heading": -0.5430956567390244,
-            "angularVelocity": 1.5389379574026985,
-            "velocityX": 2.7312135273909255,
-            "velocityY": 1.6222445708176134,
-            "timestamp": 0.3886705696332435
-        },
-        {
-            "x": 1.3771379621947135,
-            "y": 4.756486240363935,
-            "heading": -0.5116699283377882,
-            "angularVelocity": 0.9702528832339478,
-            "velocityX": 2.908928891311882,
-            "velocityY": 1.758973171660129,
-            "timestamp": 0.4210597837693471
-        },
-        {
-            "x": 1.4771406301349097,
-            "y": 4.8178582459332,
-            "heading": -0.4985254849792972,
-            "angularVelocity": 0.40582779511896505,
-            "velocityX": 3.087529926474048,
-            "velocityY": 1.894828485537579,
-            "timestamp": 0.45344899790545073
-        },
-        {
-            "x": 1.5826858211522454,
-            "y": 4.880091207833619,
-            "heading": -0.49852543793275184,
-            "angularVelocity": 0.000001452537415626245,
-            "velocityX": 3.2586524197166784,
-            "velocityY": 1.9214100607352578,
-            "timestamp": 0.48583821204155436
-        },
-        {
-            "x": 1.6882312828144117,
-            "y": 4.9423237107273525,
-            "heading": -0.4985253908851185,
-            "angularVelocity": 0.0000014525710071923091,
-            "velocityX": 3.2586607757338757,
-            "velocityY": 1.9213958891446032,
-            "timestamp": 0.5182274261776579
-        },
-        {
-            "x": 1.7937767445018398,
-            "y": 5.004556213578243,
-            "heading": -0.49852534383749064,
-            "angularVelocity": 0.0000014525708350435827,
-            "velocityX": 3.2586607765138327,
-            "velocityY": 1.921395887821837,
-            "timestamp": 0.5506166403137616
-        },
-        {
-            "x": 1.8993222061892705,
-            "y": 5.06678871642913,
-            "heading": -0.49852529678986307,
-            "angularVelocity": 0.0000014525708270038968,
-            "velocityX": 3.2586607765139086,
-            "velocityY": 1.9213958878217097,
-            "timestamp": 0.5830058544498652
-        },
-        {
-            "x": 2.0048676678767015,
-            "y": 5.129021219280016,
-            "heading": -0.4985252497422358,
-            "angularVelocity": 0.0000014525708183379938,
-            "velocityX": 3.258660776513909,
-            "velocityY": 1.9213958878217101,
-            "timestamp": 0.6153950685859688
-        },
-        {
-            "x": 2.1104131295641317,
-            "y": 5.191253722130902,
-            "heading": -0.49852520269460865,
-            "angularVelocity": 0.000001452570810915054,
-            "velocityX": 3.2586607765139104,
-            "velocityY": 1.9213958878217086,
-            "timestamp": 0.6477842827220724
-        },
-        {
-            "x": 2.2159585912515625,
-            "y": 5.253486224981789,
-            "heading": -0.4985251556469819,
-            "angularVelocity": 0.0000014525708028649706,
-            "velocityX": 3.2586607765139086,
-            "velocityY": 1.9213958878217123,
-            "timestamp": 0.6801734968581761
-        },
-        {
-            "x": 2.321504052938993,
-            "y": 5.315718727832675,
-            "heading": -0.4985251085993554,
-            "angularVelocity": 0.000001452570794356237,
-            "velocityX": 3.2586607765139117,
-            "velocityY": 1.9213958878217081,
-            "timestamp": 0.7125627109942797
-        },
-        {
-            "x": 2.427049514626424,
-            "y": 5.3779512306835615,
-            "heading": -0.49852506155172915,
-            "angularVelocity": 0.0000014525707870212498,
-            "velocityX": 3.258660776513912,
-            "velocityY": 1.9213958878217077,
-            "timestamp": 0.7449519251303833
-        },
-        {
-            "x": 2.5325949763138547,
-            "y": 5.440183733534448,
-            "heading": -0.4985250145041031,
-            "angularVelocity": 0.000001452570779107594,
-            "velocityX": 3.258660776513913,
-            "velocityY": 1.9213958878217068,
-            "timestamp": 0.777341139266487
-        },
-        {
-            "x": 2.6381404380012854,
-            "y": 5.502416236385335,
-            "heading": -0.49852496745647734,
-            "angularVelocity": 0.0000014525707705186945,
-            "velocityX": 3.258660776513915,
-            "velocityY": 1.9213958878217055,
-            "timestamp": 0.8097303534025906
-        },
-        {
-            "x": 2.743685899688716,
-            "y": 5.564648739236221,
-            "heading": -0.4985249204088519,
-            "angularVelocity": 0.00000145257076360687,
-            "velocityX": 3.2586607765139153,
-            "velocityY": 1.9213958878217066,
-            "timestamp": 0.8421195675386942
-        },
-        {
-            "x": 2.849231361376147,
-            "y": 5.626881242087108,
-            "heading": -0.4985248733612266,
-            "angularVelocity": 0.0000014525707544682094,
-            "velocityX": 3.2586607765139153,
-            "velocityY": 1.9213958878217066,
-            "timestamp": 0.8745087816747978
-        },
-        {
-            "x": 2.954776823063578,
-            "y": 5.689113744937994,
-            "heading": -0.4985248263136016,
-            "angularVelocity": 0.0000014525707471414822,
-            "velocityX": 3.2586607765139166,
-            "velocityY": 1.921395887821705,
-            "timestamp": 0.9068979958109015
-        },
-        {
-            "x": 3.0603222847510096,
-            "y": 5.751346247788881,
-            "heading": -0.49852477926597694,
-            "angularVelocity": 0.0000014525707385370494,
-            "velocityX": 3.258660776513917,
-            "velocityY": 1.9213958878217057,
-            "timestamp": 0.9392872099470051
-        },
-        {
-            "x": 3.1658677464384404,
-            "y": 5.813578750639766,
-            "heading": -0.4985247322183525,
-            "angularVelocity": 0.0000014525707310188908,
-            "velocityX": 3.2586607765139175,
-            "velocityY": 1.9213958878217063,
-            "timestamp": 0.9716764240831087
-        },
-        {
-            "x": 3.271413208125871,
-            "y": 5.875811253490653,
-            "heading": -0.4985246851707283,
-            "angularVelocity": 0.0000014525707228586851,
-            "velocityX": 3.258660776513917,
-            "velocityY": 1.921395887821708,
-            "timestamp": 1.0040656382192124
-        },
-        {
-            "x": 3.376958669813326,
-            "y": 5.938043756341499,
-            "heading": -0.4985246381231044,
-            "angularVelocity": 0.000001452570714543944,
-            "velocityX": 3.258660776514645,
-            "velocityY": 1.921395887820474,
-            "timestamp": 1.0364548523553159
-        },
-        {
-            "x": 3.482504131751534,
-            "y": 6.000276258767071,
-            "heading": -0.49852459107548075,
-            "angularVelocity": 0.0000014525707064344823,
-            "velocityX": 3.258660784256519,
-            "velocityY": 1.9213958746903632,
-            "timestamp": 1.0688440664914194
-        },
-        {
-            "x": 3.588052280044538,
-            "y": 6.0625042049472775,
-            "heading": -0.4985245440280508,
-            "angularVelocity": 0.0000014525647243024202,
-            "velocityX": 3.258743724052004,
-            "velocityY": 1.921255203004168,
-            "timestamp": 1.101233280627523
-        },
-        {
-            "x": 3.69740891456604,
-            "y": 6.1177659034729,
-            "heading": -0.4985244969178424,
-            "angularVelocity": 0.0000014545029788824699,
-            "velocityX": 3.3763287390046313,
-            "velocityY": 1.7061759601022248,
-            "timestamp": 1.1336224947636264
-        },
-        {
-            "x": 3.8477077097405896,
-            "y": 6.176720729441604,
-            "heading": -0.4985244519972541,
-            "angularVelocity": 0.0000010525498975792195,
-            "velocityX": 3.5217032391581933,
-            "velocityY": 1.3813909907718895,
-            "timestamp": 1.1763003653755966
-        },
-        {
-            "x": 4.000682189533232,
-            "y": 6.228336695265556,
-            "heading": -0.49852440748602983,
-            "angularVelocity": 0.000001042957945295385,
-            "velocityX": 3.58439813418752,
-            "velocityY": 1.2094316113670969,
-            "timestamp": 1.2189782359875667
-        },
-        {
-            "x": 4.153656816760498,
-            "y": 6.279952224134777,
-            "heading": -0.4985243629748153,
-            "angularVelocity": 0.00000104295771772215,
-            "velocityX": 3.5844015887793454,
-            "velocityY": 1.2094213729291292,
-            "timestamp": 1.2616561065995369
-        },
-        {
-            "x": 4.306631443993679,
-            "y": 6.331567752986466,
-            "heading": -0.4985243184636008,
-            "angularVelocity": 0.0000010429577168479619,
-            "velocityX": 3.584401588917953,
-            "velocityY": 1.209421372518333,
-            "timestamp": 1.304333977211507
-        },
-        {
-            "x": 4.459606071226857,
-            "y": 6.38318328183816,
-            "heading": -0.4985242739523863,
-            "angularVelocity": 0.000001042957716433214,
-            "velocityX": 3.584401588917909,
-            "velocityY": 1.2094213725184633,
-            "timestamp": 1.3470118478234772
-        },
-        {
-            "x": 4.612580698460036,
-            "y": 6.434798810689855,
-            "heading": -0.4985242294411719,
-            "angularVelocity": 0.0000010429577140470852,
-            "velocityX": 3.5844015889179057,
-            "velocityY": 1.209421372518471,
-            "timestamp": 1.3896897184354473
-        },
-        {
-            "x": 4.765555325693214,
-            "y": 6.4864143395415494,
-            "heading": -0.49852418492995754,
-            "angularVelocity": 0.0000010429577143791187,
-            "velocityX": 3.584401588917906,
-            "velocityY": 1.2094213725184708,
-            "timestamp": 1.4323675890474175
-        },
-        {
-            "x": 4.918529952926393,
-            "y": 6.5380298683932425,
-            "heading": -0.4985241404187432,
-            "angularVelocity": 0.0000010429577134079309,
-            "velocityX": 3.584401588917911,
-            "velocityY": 1.2094213725184557,
-            "timestamp": 1.4750454596593876
-        },
-        {
-            "x": 5.071504580159572,
-            "y": 6.589645397244938,
-            "heading": -0.49852409590752894,
-            "angularVelocity": 0.000001042957711441133,
-            "velocityX": 3.584401588917906,
-            "velocityY": 1.20942137251847,
-            "timestamp": 1.5177233302713578
-        },
-        {
-            "x": 5.224479207392751,
-            "y": 6.641260926096632,
-            "heading": -0.49852405139631467,
-            "angularVelocity": 0.0000010429577109856096,
-            "velocityX": 3.5844015889179066,
-            "velocityY": 1.2094213725184684,
-            "timestamp": 1.560401200883328
-        },
-        {
-            "x": 5.377453834625929,
-            "y": 6.692876454948327,
-            "heading": -0.4985240068851004,
-            "angularVelocity": 0.000001042957710628888,
-            "velocityX": 3.584401588917908,
-            "velocityY": 1.209421372518467,
-            "timestamp": 1.603079071495298
-        },
-        {
-            "x": 5.530428461859109,
-            "y": 6.744491983800022,
-            "heading": -0.4985239623738862,
-            "angularVelocity": 0.0000010429577097054993,
-            "velocityX": 3.5844015889179075,
-            "velocityY": 1.2094213725184686,
-            "timestamp": 1.6457569421072682
-        },
-        {
-            "x": 5.683403089092287,
-            "y": 6.796107512651717,
-            "heading": -0.4985239178626721,
-            "angularVelocity": 0.0000010429577084041104,
-            "velocityX": 3.584401588917908,
-            "velocityY": 1.2094213725184642,
-            "timestamp": 1.6884348127192383
-        },
-        {
-            "x": 5.836377716325465,
-            "y": 6.847723041503412,
-            "heading": -0.4985238733514579,
-            "angularVelocity": 0.000001042957708437487,
-            "velocityX": 3.5844015889179084,
-            "velocityY": 1.209421372518465,
-            "timestamp": 1.7311126833312085
-        },
-        {
-            "x": 5.989352343558645,
-            "y": 6.899338570355106,
-            "heading": -0.4985238288402439,
-            "angularVelocity": 0.0000010429577061050693,
-            "velocityX": 3.5844015889179084,
-            "velocityY": 1.2094213725184642,
-            "timestamp": 1.7737905539431786
-        },
-        {
-            "x": 6.142326970791823,
-            "y": 6.9509540992067995,
-            "heading": -0.49852378432902983,
-            "angularVelocity": 0.0000010429577060929704,
-            "velocityX": 3.584401588917909,
-            "velocityY": 1.2094213725184646,
-            "timestamp": 1.8164684245551488
-        },
-        {
-            "x": 6.295301598025002,
-            "y": 7.002569628058494,
-            "heading": -0.4985237398178158,
-            "angularVelocity": 0.0000010429577057751362,
-            "velocityX": 3.58440158891791,
-            "velocityY": 1.2094213725184613,
-            "timestamp": 1.859146295167119
-        },
-        {
-            "x": 6.44827622525818,
-            "y": 7.054185156910189,
-            "heading": -0.49852369530660184,
-            "angularVelocity": 0.0000010429577050460837,
-            "velocityX": 3.5844015889179097,
-            "velocityY": 1.2094213725184615,
-            "timestamp": 1.901824165779089
-        },
-        {
-            "x": 6.60125085249139,
-            "y": 7.105800685761796,
-            "heading": -0.4985236507953879,
-            "angularVelocity": 0.0000010429577031279755,
-            "velocityX": 3.584401588918602,
-            "velocityY": 1.2094213725164094,
-            "timestamp": 1.9445020363910592
-        },
-        {
-            "x": 6.754225480461122,
-            "y": 7.15741621243045,
-            "heading": -0.49852360628410947,
-            "angularVelocity": 0.000001042959214800434,
-            "velocityX": 3.584401606176385,
-            "velocityY": 1.209421321366894,
-            "timestamp": 1.9871799070030294
-        },
-        {
-            "x": 6.905473027890085,
-            "y": 7.208390081717835,
-            "heading": -0.49401401466016337,
-            "angularVelocity": 0.10566580664128356,
-            "velocityX": 3.543933782547739,
-            "velocityY": 1.1943864245440503,
-            "timestamp": 2.0298577776149997
-        },
-        {
-            "x": 7.045508663452743,
-            "y": 7.253974234854833,
-            "heading": -0.45930885159823864,
-            "angularVelocity": 0.8131887220303519,
-            "velocityX": 3.2812235839006783,
-            "velocityY": 1.0680981146283737,
-            "timestamp": 2.07253564822697
-        },
-        {
-            "x": 7.17296615196941,
-            "y": 7.296903697598188,
-            "heading": -0.39602280772923965,
-            "angularVelocity": 1.4828772607799376,
-            "velocityX": 2.986500654531669,
-            "velocityY": 1.0058951425592673,
-            "timestamp": 2.1152135188389405
-        },
-        {
-            "x": 7.286216905364156,
-            "y": 7.335009151903716,
-            "heading": -0.3243524220820172,
-            "angularVelocity": 1.6793336832302157,
-            "velocityX": 2.6536177126649325,
-            "velocityY": 0.8928621264164087,
-            "timestamp": 2.157891389450911
-        },
-        {
-            "x": 7.38508554947564,
-            "y": 7.368236895639727,
-            "heading": -0.2530399098538711,
-            "angularVelocity": 1.6709482269282783,
-            "velocityX": 2.31662551795061,
-            "velocityY": 0.7785707969856192,
-            "timestamp": 2.2005692600628812
-        },
-        {
-            "x": 7.469627579006154,
-            "y": 7.396613050645165,
-            "heading": -0.18659143584410423,
-            "angularVelocity": 1.5569772591027433,
-            "velocityX": 1.9809336388681473,
-            "velocityY": 0.6648915374301301,
-            "timestamp": 2.2432471306748516
-        },
-        {
-            "x": 7.539923583097788,
-            "y": 7.420174905803342,
-            "heading": -0.12774543015301906,
-            "angularVelocity": 1.3788411850749698,
-            "velocityX": 1.6471300719468636,
-            "velocityY": 0.5520860066427349,
-            "timestamp": 2.285925001286822
-        },
-        {
-            "x": 7.596048224439389,
-            "y": 7.4389607760624,
-            "heading": -0.07839593087005978,
-            "angularVelocity": 1.1563252471438406,
-            "velocityX": 1.315075952403783,
-            "velocityY": 0.4401782467044836,
-            "timestamp": 2.3286028718987923
-        },
-        {
-            "x": 7.638066692552635,
-            "y": 7.453006402993039,
-            "heading": -0.03996068961729096,
-            "angularVelocity": 0.9005894788478175,
-            "velocityX": 0.9845493111706484,
-            "velocityY": 0.3291079599154316,
-            "timestamp": 2.3712807425107627
-        },
-        {
-            "x": 7.666034942577791,
-            "y": 7.46234355914255,
-            "heading": -0.013541291231896178,
-            "angularVelocity": 0.6190420938664342,
-            "velocityX": 0.655333774251397,
-            "velocityY": 0.2187821467102802,
-            "timestamp": 2.413958613122733
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0.31729069510085045,
-            "velocityX": 0.3272201077973249,
-            "velocityY": 0.10910668200355174,
-            "timestamp": 2.4566364837347034
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 5.5875375349932e-23,
-            "velocityY": -3.709612464642882e-23,
-            "timestamp": 2.499314354346674
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.1.traj
deleted file mode 100644
index a6fd22bb..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.1.traj	
+++ /dev/null
@@ -1,607 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7226799898554153,
-            "y": 4.366381680411806,
-            "heading": -1.026991220057009,
-            "angularVelocity": 0.3105337423596819,
-            "velocityX": 0.3026851649532591,
-            "velocityY": 0.15233766037290783,
-            "timestamp": 0.04189167928786116
-        },
-        {
-            "x": 0.7480755502425919,
-            "y": 4.379165077654378,
-            "heading": -1.001621000893582,
-            "angularVelocity": 0.6056147567896238,
-            "velocityX": 0.6062196793943158,
-            "velocityY": 0.3051536118838673,
-            "timestamp": 0.08378335857572232
-        },
-        {
-            "x": 0.7862289943958503,
-            "y": 4.398373013543058,
-            "heading": -0.964694126725153,
-            "angularVelocity": 0.8814846956762871,
-            "velocityX": 0.9107642568130221,
-            "velocityY": 0.4585143449774818,
-            "timestamp": 0.12567503786358347
-        },
-        {
-            "x": 0.8371909372554502,
-            "y": 4.424030289360705,
-            "heading": -0.9172220550063085,
-            "angularVelocity": 1.1332100437568369,
-            "velocityX": 1.2165170679697972,
-            "velocityY": 0.6124671116987632,
-            "timestamp": 0.16756671715144464
-        },
-        {
-            "x": 0.9010212440299602,
-            "y": 4.456163358075127,
-            "heading": -0.8604944995891576,
-            "angularVelocity": 1.354148517832005,
-            "velocityX": 1.523698926841682,
-            "velocityY": 0.767051339565964,
-            "timestamp": 0.2094583964393058
-        },
-        {
-            "x": 0.9777886193671033,
-            "y": 4.494801080149917,
-            "heading": -0.796206518536166,
-            "angularVelocity": 1.5346241102256366,
-            "velocityX": 1.8325208404664681,
-            "velocityY": 0.9223244981249834,
-            "timestamp": 0.25135007572716694
-        },
-        {
-            "x": 1.0675667159355893,
-            "y": 4.539976112871383,
-            "heading": -0.7267376359019178,
-            "angularVelocity": 1.658297872398206,
-            "velocityX": 2.143100923492954,
-            "velocityY": 1.07837722166836,
-            "timestamp": 0.2932417550150281
-        },
-        {
-            "x": 1.1704147696438687,
-            "y": 4.591719023811467,
-            "heading": -0.6558650979752687,
-            "angularVelocity": 1.6918046526529587,
-            "velocityX": 2.4550950321555027,
-            "velocityY": 1.23515962643868,
-            "timestamp": 0.3351334343028892
-        },
-        {
-            "x": 1.286251945776662,
-            "y": 4.64999734523705,
-            "heading": -0.5908433350340152,
-            "angularVelocity": 1.5521402829056499,
-            "velocityX": 2.765159528144268,
-            "velocityY": 1.3911669910657207,
-            "timestamp": 0.37702511359075036
-        },
-        {
-            "x": 1.4132089603720126,
-            "y": 4.714842752574763,
-            "heading": -0.5508840698501896,
-            "angularVelocity": 0.9538711711517488,
-            "velocityX": 3.0306021805179513,
-            "velocityY": 1.547930482617412,
-            "timestamp": 0.4189167928786115
-        },
-        {
-            "x": 1.550306465812279,
-            "y": 4.786404286835006,
-            "heading": -0.541265046364201,
-            "angularVelocity": 0.22961656466170327,
-            "velocityX": 3.272666738857444,
-            "velocityY": 1.7082517453765305,
-            "timestamp": 0.46080847216647264
-        },
-        {
-            "x": 1.6919561664396436,
-            "y": 4.857462558344818,
-            "heading": -0.5412650016128938,
-            "angularVelocity": 0.000001068262432444083,
-            "velocityX": 3.3813325948098276,
-            "velocityY": 1.6962383155263616,
-            "timestamp": 0.5027001514543338
-        },
-        {
-            "x": 1.8336059249814771,
-            "y": 4.928520714406238,
-            "heading": -0.541264956861776,
-            "angularVelocity": 0.0000010682579123299043,
-            "velocityX": 3.3813339772912605,
-            "velocityY": 1.6962355596475316,
-            "timestamp": 0.544591830742195
-        },
-        {
-            "x": 1.9752556835258195,
-            "y": 4.999578870462656,
-            "heading": -0.5412649121106583,
-            "angularVelocity": 0.0000010682579080478101,
-            "velocityX": 3.3813339773511535,
-            "velocityY": 1.696235559528139,
-            "timestamp": 0.5864835100300562
-        },
-        {
-            "x": 2.1169054420701623,
-            "y": 5.0706370265190746,
-            "heading": -0.5412648673595408,
-            "angularVelocity": 0.0000010682579032833847,
-            "velocityX": 3.381333977351157,
-            "velocityY": 1.6962355595281329,
-            "timestamp": 0.6283751893179174
-        },
-        {
-            "x": 2.258555200614505,
-            "y": 5.141695182575493,
-            "heading": -0.5412648226084235,
-            "angularVelocity": 0.0000010682578992307337,
-            "velocityX": 3.381333977351157,
-            "velocityY": 1.6962355595281327,
-            "timestamp": 0.6702668686057786
-        },
-        {
-            "x": 2.400204959158848,
-            "y": 5.212753338631911,
-            "heading": -0.5412647778573064,
-            "angularVelocity": 0.0000010682578937479676,
-            "velocityX": 3.3813339773511575,
-            "velocityY": 1.696235559528132,
-            "timestamp": 0.7121585478936397
-        },
-        {
-            "x": 2.5418547177031905,
-            "y": 5.2838114946883294,
-            "heading": -0.5412647331061895,
-            "angularVelocity": 0.0000010682578893581349,
-            "velocityX": 3.3813339773511584,
-            "velocityY": 1.696235559528132,
-            "timestamp": 0.7540502271815009
-        },
-        {
-            "x": 2.6835044762475335,
-            "y": 5.354869650744748,
-            "heading": -0.5412646883550728,
-            "angularVelocity": 0.0000010682578845109594,
-            "velocityX": 3.3813339773511584,
-            "velocityY": 1.6962355595281313,
-            "timestamp": 0.7959419064693621
-        },
-        {
-            "x": 2.8251542347918766,
-            "y": 5.425927806801167,
-            "heading": -0.5412646436039563,
-            "angularVelocity": 0.0000010682578796288815,
-            "velocityX": 3.3813339773511593,
-            "velocityY": 1.6962355595281313,
-            "timestamp": 0.8378335857572233
-        },
-        {
-            "x": 2.9668039933362196,
-            "y": 5.496985962857585,
-            "heading": -0.5412645988528401,
-            "angularVelocity": 0.0000010682578747880459,
-            "velocityX": 3.38133397735116,
-            "velocityY": 1.6962355595281309,
-            "timestamp": 0.8797252650450845
-        },
-        {
-            "x": 3.108453751880562,
-            "y": 5.5680441189140035,
-            "heading": -0.541264554101724,
-            "angularVelocity": 0.0000010682578699288912,
-            "velocityX": 3.38133397735116,
-            "velocityY": 1.69623555952813,
-            "timestamp": 0.9216169443329457
-        },
-        {
-            "x": 3.250103510424905,
-            "y": 5.639102274970423,
-            "heading": -0.5412645093506081,
-            "angularVelocity": 0.000001068257865104235,
-            "velocityX": 3.3813339773511606,
-            "velocityY": 1.69623555952813,
-            "timestamp": 0.9635086236208069
-        },
-        {
-            "x": 3.3917532689692482,
-            "y": 5.71016043102684,
-            "heading": -0.5412644645994924,
-            "angularVelocity": 0.0000010682578613220535,
-            "velocityX": 3.381333977351161,
-            "velocityY": 1.6962355595281293,
-            "timestamp": 1.005400302908668
-        },
-        {
-            "x": 3.5334030275135913,
-            "y": 5.7812185870832575,
-            "heading": -0.5412644198483769,
-            "angularVelocity": 0.0000010682578558700584,
-            "velocityX": 3.381333977351162,
-            "velocityY": 1.6962355595281287,
-            "timestamp": 1.0472919821965292
-        },
-        {
-            "x": 3.675052786057934,
-            "y": 5.852276743139676,
-            "heading": -0.5412643750972616,
-            "angularVelocity": 0.0000010682578513840427,
-            "velocityX": 3.381333977351162,
-            "velocityY": 1.6962355595281287,
-            "timestamp": 1.0891836614843904
-        },
-        {
-            "x": 3.816702544602277,
-            "y": 5.923334899196093,
-            "heading": -0.5412643303461464,
-            "angularVelocity": 0.0000010682578474926016,
-            "velocityX": 3.381333977351163,
-            "velocityY": 1.6962355595281278,
-            "timestamp": 1.1310753407722516
-        },
-        {
-            "x": 3.95835230314662,
-            "y": 5.994393055252511,
-            "heading": -0.5412642855950315,
-            "angularVelocity": 0.000001068257842976485,
-            "velocityX": 3.3813339773511637,
-            "velocityY": 1.6962355595281275,
-            "timestamp": 1.1729670200601128
-        },
-        {
-            "x": 4.100002061690963,
-            "y": 6.065451211308929,
-            "heading": -0.5412642408439168,
-            "angularVelocity": 0.0000010682578369198447,
-            "velocityX": 3.3813339773511637,
-            "velocityY": 1.6962355595281269,
-            "timestamp": 1.214858699347974
-        },
-        {
-            "x": 4.2416518202353055,
-            "y": 6.136509367365346,
-            "heading": -0.5412641960928023,
-            "angularVelocity": 0.0000010682578319796932,
-            "velocityX": 3.3813339773511646,
-            "velocityY": 1.696235559528127,
-            "timestamp": 1.2567503786358352
-        },
-        {
-            "x": 4.383301578779649,
-            "y": 6.207567523421765,
-            "heading": -0.5412641513416879,
-            "angularVelocity": 0.0000010682578277344439,
-            "velocityX": 3.381333977351165,
-            "velocityY": 1.6962355595281264,
-            "timestamp": 1.2986420579236964
-        },
-        {
-            "x": 4.524951337323992,
-            "y": 6.278625679478183,
-            "heading": -0.5412641065905739,
-            "angularVelocity": 0.0000010682578220378627,
-            "velocityX": 3.3813339773511655,
-            "velocityY": 1.6962355595281264,
-            "timestamp": 1.3405337372115576
-        },
-        {
-            "x": 4.666601095868337,
-            "y": 6.349683835534597,
-            "heading": -0.54126406183946,
-            "angularVelocity": 0.0000010682578180389232,
-            "velocityX": 3.381333977351213,
-            "velocityY": 1.6962355595280314,
-            "timestamp": 1.3824254164994187
-        },
-        {
-            "x": 4.8082508544580715,
-            "y": 6.420741991500528,
-            "heading": -0.5412640170883463,
-            "angularVelocity": 0.000001068257814314864,
-            "velocityX": 3.3813339784347316,
-            "velocityY": 1.6962355573681103,
-            "timestamp": 1.42431709578728
-        },
-        {
-            "x": 4.949901660762853,
-            "y": 6.491798058877407,
-            "heading": -0.54126397233727,
-            "angularVelocity": 0.0000010682569195340074,
-            "velocityX": 3.3813589885337056,
-            "velocityY": 1.6961857004731962,
-            "timestamp": 1.4662087750751411
-        },
-        {
-            "x": 5.0966757016987385,
-            "y": 6.551558383908352,
-            "heading": -0.5412639273749705,
-            "angularVelocity": 0.000001073299046263079,
-            "velocityX": 3.503656177813225,
-            "velocityY": 1.4265440308634512,
-            "timestamp": 1.5081004543630023
-        },
-        {
-            "x": 5.2483360143420885,
-            "y": 6.597526467706988,
-            "heading": -0.5412638804954443,
-            "angularVelocity": 0.000001119065339105663,
-            "velocityX": 3.6202968040790893,
-            "velocityY": 1.097308214425216,
-            "timestamp": 1.5499921336508635
-        },
-        {
-            "x": 5.403589728009299,
-            "y": 6.629310092724924,
-            "heading": -0.5412638294474256,
-            "angularVelocity": 0.0000012185717939310704,
-            "velocityX": 3.7060751993343666,
-            "velocityY": 0.7587097380253649,
-            "timestamp": 1.5918838129387247
-        },
-        {
-            "x": 5.561113217492597,
-            "y": 6.646638205385644,
-            "heading": -0.54126377105557,
-            "angularVelocity": 0.0000013938771748607603,
-            "velocityX": 3.760257219598817,
-            "velocityY": 0.4136409176067717,
-            "timestamp": 1.633775492226586
-        },
-        {
-            "x": 5.7195634841918945,
-            "y": 6.649363040924072,
-            "heading": -0.5412637028328066,
-            "angularVelocity": 0.000001628551649307522,
-            "velocityX": 3.7823804009024578,
-            "velocityY": 0.06504479134637085,
-            "timestamp": 1.675667171514447
-        },
-        {
-            "x": 5.8386728592662305,
-            "y": 6.64312635201416,
-            "heading": -0.5412636336326876,
-            "angularVelocity": 0.000002194803906563273,
-            "velocityX": 3.777764046962115,
-            "velocityY": -0.19780759592812253,
-            "timestamp": 1.7071962383364254
-        },
-        {
-            "x": 5.95706146956484,
-            "y": 6.62863229973747,
-            "heading": -0.5412635727602775,
-            "angularVelocity": 0.0000019306759164775456,
-            "velocityX": 3.7549037200201067,
-            "velocityY": -0.459704448549871,
-            "timestamp": 1.7387253051584037
-        },
-        {
-            "x": 6.0741575637122,
-            "y": 6.605950895283392,
-            "heading": -0.5412635175985968,
-            "angularVelocity": 0.000001749550058024173,
-            "velocityX": 3.713909289117771,
-            "velocityY": -0.7193807727372066,
-            "timestamp": 1.770254371980382
-        },
-        {
-            "x": 6.189395638406918,
-            "y": 6.575191700510336,
-            "heading": -0.5412634663439824,
-            "angularVelocity": 0.0000016256305579593937,
-            "velocityX": 3.654978922953296,
-            "velocityY": -0.9755821492190131,
-            "timestamp": 1.8017834388023604
-        },
-        {
-            "x": 6.302219177732501,
-            "y": 6.536503313694644,
-            "heading": -0.5412634176727114,
-            "angularVelocity": 0.0000015436952600896132,
-            "velocityX": 3.578397672300798,
-            "velocityY": -1.2270704691051513,
-            "timestamp": 1.8333125056243387
-        },
-        {
-            "x": 6.412083374588637,
-            "y": 6.49007271186475,
-            "heading": -0.5412633705487488,
-            "angularVelocity": 0.0000014946196396789507,
-            "velocityX": 3.484536902930231,
-            "velocityY": -1.4726284825381617,
-            "timestamp": 1.864841572446317
-        },
-        {
-            "x": 6.518458035998333,
-            "y": 6.436124829047777,
-            "heading": -0.5412633241017745,
-            "angularVelocity": 0.0000014731477658308475,
-            "velocityX": 3.3738601275551834,
-            "velocityY": -1.7110523163142606,
-            "timestamp": 1.8963706392682953
-        },
-        {
-            "x": 6.622870515934444,
-            "y": 6.378470889852871,
-            "heading": -0.5412632777559726,
-            "angularVelocity": 0.0000014699389024112294,
-            "velocityX": 3.3116260790606757,
-            "velocityY": -1.8285964351699866,
-            "timestamp": 1.9278997060902736
-        },
-        {
-            "x": 6.7272827038996414,
-            "y": 6.320816421895741,
-            "heading": -0.5412632314101712,
-            "angularVelocity": 0.0000014699388892769531,
-            "velocityX": 3.311616818687876,
-            "velocityY": -1.8286132057971307,
-            "timestamp": 1.959428772912252
-        },
-        {
-            "x": 6.831694893880165,
-            "y": 6.263161957588452,
-            "heading": -0.5412631850642508,
-            "angularVelocity": 0.0000014699426583817394,
-            "velocityX": 3.311616882607505,
-            "velocityY": -1.828613090035986,
-            "timestamp": 1.9909578397342302
-        },
-        {
-            "x": 6.935462774507961,
-            "y": 6.205910954727142,
-            "heading": -0.5387292297574777,
-            "angularVelocity": 0.08036886473933494,
-            "velocityX": 3.2911814743423053,
-            "velocityY": -1.815816598206477,
-            "timestamp": 2.0224869065562086
-        },
-        {
-            "x": 7.033610221214887,
-            "y": 6.151825220973909,
-            "heading": -0.514724354544779,
-            "angularVelocity": 0.7613569836442236,
-            "velocityX": 3.11291949302188,
-            "velocityY": -1.715424502051262,
-            "timestamp": 2.054015973378187
-        },
-        {
-            "x": 7.126006777010342,
-            "y": 6.101041958255997,
-            "heading": -0.47044950446923295,
-            "angularVelocity": 1.4042550109565184,
-            "velocityX": 2.930519838000589,
-            "velocityY": -1.610680804625417,
-            "timestamp": 2.0855450402001656
-        },
-        {
-            "x": 7.2116589199714145,
-            "y": 6.053937431211348,
-            "heading": -0.4144026036835486,
-            "angularVelocity": 1.7776263757548019,
-            "velocityX": 2.7166088817245364,
-            "velocityY": -1.4940032101366643,
-            "timestamp": 2.117074107022144
-        },
-        {
-            "x": 7.290084520639544,
-            "y": 6.01082680132011,
-            "heading": -0.35575031779267857,
-            "angularVelocity": 1.8602607626174494,
-            "velocityX": 2.4874063387585092,
-            "velocityY": -1.3673297130756235,
-            "timestamp": 2.1486031738441227
-        },
-        {
-            "x": 7.3612581622075055,
-            "y": 5.971723406014521,
-            "heading": -0.2979353118382045,
-            "angularVelocity": 1.8337049517168786,
-            "velocityX": 2.2573976568931324,
-            "velocityY": -1.2402331958119186,
-            "timestamp": 2.1801322406661012
-        },
-        {
-            "x": 7.425208576949436,
-            "y": 5.936609973900717,
-            "heading": -0.2429131400794928,
-            "angularVelocity": 1.7451252861171487,
-            "velocityX": 2.028300269811719,
-            "velocityY": -1.1136844712868785,
-            "timestamp": 2.2116613074880798
-        },
-        {
-            "x": 7.481970224994158,
-            "y": 5.90546408447042,
-            "heading": -0.1919554556036592,
-            "angularVelocity": 1.616212898515343,
-            "velocityX": 1.8002958465346786,
-            "velocityY": -0.9878468527519235,
-            "timestamp": 2.2431903743100583
-        },
-        {
-            "x": 7.531573575573894,
-            "y": 5.8782638409384305,
-            "heading": -0.14593987362659777,
-            "angularVelocity": 1.4594653954358323,
-            "velocityX": 1.5732578087328088,
-            "velocityY": -0.8627037294052898,
-            "timestamp": 2.274719441132037
-        },
-        {
-            "x": 7.574043708984396,
-            "y": 5.854989674792864,
-            "heading": -0.10549518757833008,
-            "angularVelocity": 1.2827745989638484,
-            "velocityX": 1.347015236774987,
-            "velocityY": -0.7381812559495933,
-            "timestamp": 2.3062485079540154
-        },
-        {
-            "x": 7.60940100035129,
-            "y": 5.835624772967359,
-            "heading": -0.07108683169331761,
-            "angularVelocity": 1.091321734299697,
-            "velocityX": 1.1214188978865451,
-            "velocityY": -0.614192038566993,
-            "timestamp": 2.337777574775994
-        },
-        {
-            "x": 7.637662176953836,
-            "y": 5.820154874880602,
-            "heading": -0.043071374084194446,
-            "angularVelocity": 0.8885596826352652,
-            "velocityX": 0.8963530941818176,
-            "velocityY": -0.4906551206892414,
-            "timestamp": 2.3693066415979724
-        },
-        {
-            "x": 7.65884126375683,
-            "y": 5.808567782468078,
-            "heading": -0.021732750340195603,
-            "angularVelocity": 0.6767921126395045,
-            "velocityX": 0.6717321169883477,
-            "velocityY": -0.3675050859559891,
-            "timestamp": 2.400835708419951
-        },
-        {
-            "x": 7.672950320288622,
-            "y": 5.8008527701995645,
-            "heading": -0.007307045765720019,
-            "angularVelocity": 0.45753668054709684,
-            "velocityX": 0.44749362902029066,
-            "velocityY": -0.24469523034329732,
-            "timestamp": 2.4323647752419295
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 5.476349034092433e-35,
-            "angularVelocity": 0.23175585268595406,
-            "velocityX": 0.22359303404641942,
-            "velocityY": -0.12219740664444498,
-            "timestamp": 2.463893842063908
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 1.465348958332225e-34,
-            "angularVelocity": 2.9106988947740147e-33,
-            "velocityX": 3.969530612508356e-33,
-            "velocityY": 4.7510565840732025e-33,
-            "timestamp": 2.4954229088858866
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.1.traj
deleted file mode 100644
index 49c665b8..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.1.traj	
+++ /dev/null
@@ -1,274 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": 0,
-            "velocityX": -1.418990828065155e-30,
-            "velocityY": 1.8798559266210403e-30,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7612279700458028,
-            "y": 4.384337560333447,
-            "heading": -0.9750502376080931,
-            "angularVelocity": 0.7696859194569483,
-            "velocityX": 0.6070760811828038,
-            "velocityY": 0.28841179417367857,
-            "timestamp": 0.0843847610434429
-        },
-        {
-            "x": 0.8642667385584091,
-            "y": 4.433290258850185,
-            "heading": -0.8542284973914237,
-            "angularVelocity": 1.4317957261788106,
-            "velocityX": 1.2210589594435701,
-            "velocityY": 0.5801130193585202,
-            "timestamp": 0.1687695220868858
-        },
-        {
-            "x": 1.0199495140663002,
-            "y": 4.507092326164681,
-            "heading": -0.6915219404263555,
-            "angularVelocity": 1.9281509475538106,
-            "velocityX": 1.8449157594667183,
-            "velocityY": 0.8745899899690757,
-            "timestamp": 0.25315428313032873
-        },
-        {
-            "x": 1.2292345273519185,
-            "y": 4.606077015104397,
-            "heading": -0.5137189126301165,
-            "angularVelocity": 2.1070513869757743,
-            "velocityX": 2.480128055086372,
-            "velocityY": 1.173016166850306,
-            "timestamp": 0.3375390441737716
-        },
-        {
-            "x": 1.485641598676529,
-            "y": 4.729632973105035,
-            "heading": -0.42871970592241276,
-            "angularVelocity": 1.007281476615537,
-            "velocityX": 3.038547104406527,
-            "velocityY": 1.4641975218379706,
-            "timestamp": 0.4219238052172145
-        },
-        {
-            "x": 1.7745745246623985,
-            "y": 4.865356167675212,
-            "heading": -0.42871965586223265,
-            "angularVelocity": 5.932372081830595e-7,
-            "velocityX": 3.4239941242128182,
-            "velocityY": 1.6083851265555307,
-            "timestamp": 0.5063085662606575
-        },
-        {
-            "x": 2.063508085893233,
-            "y": 5.001078009918069,
-            "heading": -0.4287196058112515,
-            "angularVelocity": 5.931281972756146e-7,
-            "velocityX": 3.4240016521710777,
-            "velocityY": 1.6083691008265977,
-            "timestamp": 0.5906933273041004
-        },
-        {
-            "x": 2.3524294773436862,
-            "y": 5.136825756955775,
-            "heading": -0.4287195557601797,
-            "angularVelocity": 5.931292703362374e-7,
-            "velocityX": 3.423857434421327,
-            "velocityY": 1.6086760851027126,
-            "timestamp": 0.6750780883475433
-        },
-        {
-            "x": 2.647360543237261,
-            "y": 5.258967835966316,
-            "heading": -0.4287195052637089,
-            "angularVelocity": 5.984074633873495e-7,
-            "velocityX": 3.4950749666965533,
-            "velocityY": 1.447442375852424,
-            "timestamp": 0.7594628493909863
-        },
-        {
-            "x": 2.959809428031792,
-            "y": 5.324379711821066,
-            "heading": -0.42871944828896014,
-            "angularVelocity": 6.751781731100131e-7,
-            "velocityX": 3.702669545200082,
-            "velocityY": 0.7751621862272086,
-            "timestamp": 0.8438476104344292
-        },
-        {
-            "x": 3.2789674137401184,
-            "y": 5.330797896945005,
-            "heading": -0.4287193686010616,
-            "angularVelocity": 9.443399202542765e-7,
-            "velocityX": 3.782175617514861,
-            "velocityY": 0.0760585802995927,
-            "timestamp": 0.9282323714778722
-        },
-        {
-            "x": 3.593793413911528,
-            "y": 5.278000284107237,
-            "heading": -0.42871953037751775,
-            "angularVelocity": -0.000001917128805011847,
-            "velocityX": 3.7308395055989028,
-            "velocityY": -0.625677103128148,
-            "timestamp": 1.0126171325213151
-        },
-        {
-            "x": 3.893396265271842,
-            "y": 5.167813441772834,
-            "heading": -0.42871965571467335,
-            "angularVelocity": -0.0000014853055701792625,
-            "velocityX": 3.550437871193987,
-            "velocityY": -1.3057670718262655,
-            "timestamp": 1.097001893564758
-        },
-        {
-            "x": 4.167411681517661,
-            "y": 5.0040496950416875,
-            "heading": -0.4287197602752934,
-            "angularVelocity": -0.0000012390936318239587,
-            "velocityX": 3.247214459785327,
-            "velocityY": -1.9406791546980178,
-            "timestamp": 1.181386654608201
-        },
-        {
-            "x": 4.4320204493985145,
-            "y": 4.825485382292427,
-            "heading": -0.42871985986303984,
-            "angularVelocity": -0.0000011801626881351292,
-            "velocityX": 3.1357411528921184,
-            "velocityY": -2.1160729797797915,
-            "timestamp": 1.265771415651644
-        },
-        {
-            "x": 4.666313927400726,
-            "y": 4.608668432226026,
-            "heading": -0.4287198793836171,
-            "angularVelocity": -2.3132822841838536e-7,
-            "velocityX": 2.77649038884734,
-            "velocityY": -2.5693851281356843,
-            "timestamp": 1.350156176695087
-        },
-        {
-            "x": 4.936706954544898,
-            "y": 4.438990462685779,
-            "heading": -0.4287197198154701,
-            "angularVelocity": 0.0000018909592816311697,
-            "velocityX": 3.204287406881873,
-            "velocityY": -2.010765539204243,
-            "timestamp": 1.4345409377385299
-        },
-        {
-            "x": 5.233776210787073,
-            "y": 4.322144997944839,
-            "heading": -0.42871957094251306,
-            "angularVelocity": 0.0000017642161351814778,
-            "velocityX": 3.5204135506067917,
-            "velocityY": -1.384674949557486,
-            "timestamp": 1.5189256987819728
-        },
-        {
-            "x": 5.532138329985476,
-            "y": 4.208641466951728,
-            "heading": -0.42871942351676046,
-            "angularVelocity": 0.0000017470660671471722,
-            "velocityX": 3.5357345983929718,
-            "velocityY": -1.3450714274656952,
-            "timestamp": 1.6033104598254158
-        },
-        {
-            "x": 5.845046534221127,
-            "y": 4.145463344127181,
-            "heading": -0.42871936362457447,
-            "angularVelocity": 7.097512069029325e-7,
-            "velocityX": 3.708112701469523,
-            "velocityY": -0.7486911385809435,
-            "timestamp": 1.6876952208688587
-        },
-        {
-            "x": 6.164234411105391,
-            "y": 4.140760073731674,
-            "heading": -0.4287193263126739,
-            "angularVelocity": 4.4216396519651743e-7,
-            "velocityX": 3.7825298423246796,
-            "velocityY": -0.055736016049057806,
-            "timestamp": 1.7720799819123016
-        },
-        {
-            "x": 6.483450051427475,
-            "y": 4.138663317552119,
-            "heading": -0.42871928910258295,
-            "angularVelocity": 4.4095747260310005e-7,
-            "velocityX": 3.782858852414673,
-            "velocityY": -0.024847569082867782,
-            "timestamp": 1.8564647429557446
-        },
-        {
-            "x": 6.802665691714121,
-            "y": 4.136566553568004,
-            "heading": -0.4287192518958351,
-            "angularVelocity": 4.4091785477775863e-7,
-            "velocityX": 3.7828588519947024,
-            "velocityY": -0.024847661569458286,
-            "timestamp": 1.9408495039991875
-        },
-        {
-            "x": 7.0950029322992965,
-            "y": 4.132696093446151,
-            "heading": -0.35666839777179793,
-            "angularVelocity": 0.853837271482519,
-            "velocityX": 3.4643368893901423,
-            "velocityY": -0.04586681379319509,
-            "timestamp": 2.0252342650426303
-        },
-        {
-            "x": 7.329695147990892,
-            "y": 4.13025195968354,
-            "heading": -0.23302792235716066,
-            "angularVelocity": 1.465199093839195,
-            "velocityX": 2.7812156222231823,
-            "velocityY": -0.02896416049909081,
-            "timestamp": 2.109619026086073
-        },
-        {
-            "x": 7.505137224924894,
-            "y": 4.1285705338055685,
-            "heading": -0.12281843303988041,
-            "angularVelocity": 1.3060354494646,
-            "velocityX": 2.0790729838492856,
-            "velocityY": -0.019925705271038107,
-            "timestamp": 2.194003787129516
-        },
-        {
-            "x": 7.621791455543556,
-            "y": 4.127510522704197,
-            "heading": -0.04250049300917318,
-            "angularVelocity": 0.9518062152170125,
-            "velocityX": 1.3824087332380297,
-            "velocityY": -0.012561641322782502,
-            "timestamp": 2.278388548172959
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -1.5179160305718205e-31,
-            "angularVelocity": 0.5036512811515541,
-            "velocityX": 0.6897992449901723,
-            "velocityY": -0.0060499395609808024,
-            "timestamp": 2.362773309216402
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 2.3304076579629275e-30,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 2.447158070259845
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.1.traj
deleted file mode 100644
index 00bde21f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.1.traj	
+++ /dev/null
@@ -1,679 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.687708804701003e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7140616140612069,
-            "y": 4.355992841184312,
-            "heading": -1.0347167301054676,
-            "angularVelocity": 0.1988304134727778,
-            "velocityX": 0.15285465616518656,
-            "velocityY": -0.15080528916361516,
-            "timestamp": 0.026571739213607736
-        },
-        {
-            "x": 0.7221925258884458,
-            "y": 4.3479754290630535,
-            "heading": -1.0242624006876555,
-            "angularVelocity": 0.39343790535390527,
-            "velocityX": 0.30599848063671325,
-            "velocityY": -0.3017270362623706,
-            "timestamp": 0.05314347842721547
-        },
-        {
-            "x": 0.7344011981721603,
-            "y": 4.335944497876898,
-            "heading": -1.0087613580778494,
-            "angularVelocity": 0.5833657513042195,
-            "velocityX": 0.45946078973491633,
-            "velocityY": -0.4527716868451825,
-            "timestamp": 0.0797152176408232
-        },
-        {
-            "x": 0.7506970627866689,
-            "y": 4.31989642008046,
-            "heading": -0.9883558514269565,
-            "angularVelocity": 0.7679401971717026,
-            "velocityX": 0.6132780576953482,
-            "velocityY": -0.6039528563572185,
-            "timestamp": 0.10628695685443094
-        },
-        {
-            "x": 0.7710907256657638,
-            "y": 4.29982697901967,
-            "heading": -0.963213241169254,
-            "angularVelocity": 0.9462162057057438,
-            "velocityX": 0.7674944690353961,
-            "velocityY": -0.7552927152962536,
-            "timestamp": 0.13285869606803868
-        },
-        {
-            "x": 0.7955941760833964,
-            "y": 4.27573110299105,
-            "heading": -0.9335350310204893,
-            "angularVelocity": 1.1169088297226009,
-            "velocityX": 0.9221620843352277,
-            "velocityY": -0.9068234425649082,
-            "timestamp": 0.1594304352816464
-        },
-        {
-            "x": 0.8242209914920048,
-            "y": 4.247602572989807,
-            "heading": -0.8995682913456332,
-            "angularVelocity": 1.2783032153748224,
-            "velocityX": 1.0773406730541775,
-            "velocityY": -1.0585882156647894,
-            "timestamp": 0.18600217449525414
-        },
-        {
-            "x": 0.856986533327333,
-            "y": 4.215433742997202,
-            "heading": -0.8616204330063739,
-            "angularVelocity": 1.4281285103018635,
-            "velocityX": 1.2330973735640383,
-            "velocityY": -1.210640738794065,
-            "timestamp": 0.21257391370886186
-        },
-        {
-            "x": 0.893908134971617,
-            "y": 4.179215360896215,
-            "heading": -0.8200791656995261,
-            "angularVelocity": 1.5633627506615773,
-            "velocityX": 1.3895063980372095,
-            "velocityY": -1.3630414558050108,
-            "timestamp": 0.2391456529224696
-        },
-        {
-            "x": 0.9350052770321623,
-            "y": 4.138936674970777,
-            "heading": -0.7754414952973147,
-            "angularVelocity": 1.6798926876172218,
-            "velocityX": 1.546648555074586,
-            "velocityY": -1.5158468025612346,
-            "timestamp": 0.2657173921360773
-        },
-        {
-            "x": 0.9802996637151041,
-            "y": 4.094586228803994,
-            "heading": -0.7283605055619151,
-            "angularVelocity": 1.7718444907546276,
-            "velocityX": 1.7046075275248067,
-            "velocityY": -1.669083299751435,
-            "timestamp": 0.29228913134968504
-        },
-        {
-            "x": 1.0298146765069804,
-            "y": 4.046154350498487,
-            "heading": -0.6797316150493184,
-            "angularVelocity": 1.8300981400454652,
-            "velocityX": 1.8634464381059026,
-            "velocityY": -1.8226837888242364,
-            "timestamp": 0.31886087056329276
-        },
-        {
-            "x": 1.0835715343165668,
-            "y": 3.9936405744754793,
-            "heading": -0.6308802721211105,
-            "angularVelocity": 1.8384699072761677,
-            "velocityX": 2.0230839004342207,
-            "velocityY": -1.9763017994740086,
-            "timestamp": 0.3454326097769005
-        },
-        {
-            "x": 1.1415677463510772,
-            "y": 3.937082152581159,
-            "heading": -0.5840793547040092,
-            "angularVelocity": 1.7613042579137594,
-            "velocityX": 2.1826276243449603,
-            "velocityY": -2.12851787531304,
-            "timestamp": 0.3720043489905082
-        },
-        {
-            "x": 1.203636670620202,
-            "y": 3.876777756204273,
-            "heading": -0.5447644235662668,
-            "angularVelocity": 1.4795768851144226,
-            "velocityX": 2.33589994882,
-            "velocityY": -2.2694937614773476,
-            "timestamp": 0.39857608820411594
-        },
-        {
-            "x": 1.2693974629594884,
-            "y": 3.8133907541919347,
-            "heading": -0.5190150912676422,
-            "angularVelocity": 0.969049563960718,
-            "velocityX": 2.4748395959572558,
-            "velocityY": -2.3855044452595306,
-            "timestamp": 0.42514782741772367
-        },
-        {
-            "x": 1.3388335264794184,
-            "y": 3.7468848835898942,
-            "heading": -0.5067181615747146,
-            "angularVelocity": 0.4627822663045784,
-            "velocityX": 2.613154636275026,
-            "velocityY": -2.5028798479243823,
-            "timestamp": 0.4517195666313314
-        },
-        {
-            "x": 1.410439403600934,
-            "y": 3.676338986398213,
-            "heading": -0.5067181155488927,
-            "angularVelocity": 0.0000017321343363764108,
-            "velocityX": 2.6948133332892708,
-            "velocityY": -2.6549220818618244,
-            "timestamp": 0.4782913058449391
-        },
-        {
-            "x": 1.482040293717765,
-            "y": 3.605788027611728,
-            "heading": -0.5067180695571892,
-            "angularVelocity": 0.0000017308503249172533,
-            "velocityX": 2.694625652511417,
-            "velocityY": -2.655112569761896,
-            "timestamp": 0.5048630450585468
-        },
-        {
-            "x": 1.5536411829944505,
-            "y": 3.5352370679725946,
-            "heading": -0.5067180235654862,
-            "angularVelocity": 0.0000017308503100877725,
-            "velocityX": 2.6946256208934063,
-            "velocityY": -2.6551126018504427,
-            "timestamp": 0.5314347842721546
-        },
-        {
-            "x": 1.6252420722709946,
-            "y": 3.4646861083333174,
-            "heading": -0.5067179775737835,
-            "angularVelocity": 0.0000017308502987237163,
-            "velocityX": 2.6946256208880808,
-            "velocityY": -2.6551126018558486,
-            "timestamp": 0.5580065234857624
-        },
-        {
-            "x": 1.6968429615475384,
-            "y": 3.39413514869404,
-            "heading": -0.506717931582081,
-            "angularVelocity": 0.0000017308502871423956,
-            "velocityX": 2.6946256208880808,
-            "velocityY": -2.65511260185585,
-            "timestamp": 0.5845782626993702
-        },
-        {
-            "x": 1.7684438508240825,
-            "y": 3.323584189054763,
-            "heading": -0.5067178855903789,
-            "angularVelocity": 0.0000017308502762001758,
-            "velocityX": 2.694625620888081,
-            "velocityY": -2.65511260185585,
-            "timestamp": 0.611150001912978
-        },
-        {
-            "x": 1.8400447401006266,
-            "y": 3.2530332294154856,
-            "heading": -0.506717839598677,
-            "angularVelocity": 0.000001730850265567339,
-            "velocityX": 2.6946256208880826,
-            "velocityY": -2.6551126018558495,
-            "timestamp": 0.6377217411265858
-        },
-        {
-            "x": 1.9116456293771706,
-            "y": 3.1824822697762087,
-            "heading": -0.5067177936069754,
-            "angularVelocity": 0.0000017308502535085818,
-            "velocityX": 2.694625620888084,
-            "velocityY": -2.65511260185585,
-            "timestamp": 0.6642934803401935
-        },
-        {
-            "x": 1.9832465186537145,
-            "y": 3.1119313101369315,
-            "heading": -0.5067177476152741,
-            "angularVelocity": 0.0000017308502432657092,
-            "velocityX": 2.6946256208880843,
-            "velocityY": -2.65511260185585,
-            "timestamp": 0.6908652195538013
-        },
-        {
-            "x": 2.0548474079302586,
-            "y": 3.041380350497654,
-            "heading": -0.5067177016235731,
-            "angularVelocity": 0.000001730850231529751,
-            "velocityX": 2.694625620888085,
-            "velocityY": -2.6551126018558504,
-            "timestamp": 0.7174369587674091
-        },
-        {
-            "x": 2.1264482972068026,
-            "y": 2.970829390858377,
-            "heading": -0.5067176556318724,
-            "angularVelocity": 0.0000017308502204807436,
-            "velocityX": 2.6946256208880865,
-            "velocityY": -2.6551126018558495,
-            "timestamp": 0.7440086979810169
-        },
-        {
-            "x": 2.1980491864833867,
-            "y": 2.9002784312191405,
-            "heading": -0.5067176096401721,
-            "angularVelocity": 0.0000017308502087562552,
-            "velocityX": 2.694625620889591,
-            "velocityY": -2.6551126018543236,
-            "timestamp": 0.7705804371946247
-        },
-        {
-            "x": 2.2696500759972245,
-            "y": 2.829727471820689,
-            "heading": -0.506717563648472,
-            "angularVelocity": 0.0000017308501985869009,
-            "velocityX": 2.6946256298184115,
-            "velocityY": -2.6551125927926265,
-            "timestamp": 0.7971521764082324
-        },
-        {
-            "x": 2.3412523738653572,
-            "y": 2.7591779417640674,
-            "heading": -0.5067175176565489,
-            "angularVelocity": 0.0000017308585893832803,
-            "velocityX": 2.694678631779742,
-            "velocityY": -2.6550588009870313,
-            "timestamp": 0.8237239156218402
-        },
-        {
-            "x": 2.4158866145337003,
-            "y": 2.6918440324740667,
-            "heading": -0.5067174710515008,
-            "angularVelocity": 0.000001753932919872258,
-            "velocityX": 2.808782672009746,
-            "velocityY": -2.5340422299311864,
-            "timestamp": 0.850295654835448
-        },
-        {
-            "x": 2.4943342208862305,
-            "y": 2.6289942264556885,
-            "heading": -0.5067174224716422,
-            "angularVelocity": 0.000001828252870098291,
-            "velocityX": 2.952294756541798,
-            "velocityY": -2.365287628074865,
-            "timestamp": 0.8768673940490558
-        },
-        {
-            "x": 2.693828949223235,
-            "y": 2.509453130751911,
-            "heading": -0.5067173754009504,
-            "angularVelocity": 7.656472375459629e-7,
-            "velocityX": 3.244961599231293,
-            "velocityY": -1.9444436869204162,
-            "timestamp": 0.9383456957905196
-        },
-        {
-            "x": 2.900420237054028,
-            "y": 2.4026436735913763,
-            "heading": -0.5067173289743672,
-            "angularVelocity": 7.551702291921459e-7,
-            "velocityX": 3.3603935368868174,
-            "velocityY": -1.7373521085488381,
-            "timestamp": 0.9998239975319835
-        },
-        {
-            "x": 3.1070116472391316,
-            "y": 2.295834453089378,
-            "heading": -0.5067172825477892,
-            "angularVelocity": 7.55170142432061e-7,
-            "velocityX": 3.3603955270899863,
-            "velocityY": -1.7373482590844154,
-            "timestamp": 1.0613022992734473
-        },
-        {
-            "x": 3.3136030574258837,
-            "y": 2.1890252325905677,
-            "heading": -0.5067172361212112,
-            "angularVelocity": 7.551701448333524e-7,
-            "velocityX": 3.3603955271167996,
-            "velocityY": -1.737348259032553,
-            "timestamp": 1.1227806010149113
-        },
-        {
-            "x": 3.5201944676126358,
-            "y": 2.0822160120917577,
-            "heading": -0.5067171896946332,
-            "angularVelocity": 7.551701452492318e-7,
-            "velocityX": 3.3603955271167996,
-            "velocityY": -1.737348259032552,
-            "timestamp": 1.1842589027563752
-        },
-        {
-            "x": 3.726785877799388,
-            "y": 1.9754067915929476,
-            "heading": -0.5067171432680552,
-            "angularVelocity": 7.5517014486464e-7,
-            "velocityX": 3.3603955271168005,
-            "velocityY": -1.7373482590325509,
-            "timestamp": 1.2457372044978392
-        },
-        {
-            "x": 3.93337728798614,
-            "y": 1.8685975710941374,
-            "heading": -0.506717096841477,
-            "angularVelocity": 7.551701472516521e-7,
-            "velocityX": 3.360395527116801,
-            "velocityY": -1.7373482590325502,
-            "timestamp": 1.3072155062393032
-        },
-        {
-            "x": 4.139968698173285,
-            "y": 1.761788350596088,
-            "heading": -0.5067170504148988,
-            "angularVelocity": 7.551701467640916e-7,
-            "velocityX": 3.360395527123197,
-            "velocityY": -1.7373482590201774,
-            "timestamp": 1.3686938079807671
-        },
-        {
-            "x": 4.346560137549085,
-            "y": 1.6549791865550314,
-            "heading": -0.5067170039883208,
-            "angularVelocity": 7.55170145558408e-7,
-            "velocityX": 3.36039600190298,
-            "velocityY": -1.737347340696294,
-            "timestamp": 1.430172109722231
-        },
-        {
-            "x": 4.5552180589852185,
-            "y": 1.552265545137979,
-            "heading": -0.5067169574392034,
-            "angularVelocity": 7.571633584219393e-7,
-            "velocityX": 3.3940091955305984,
-            "velocityY": -1.6707299731374539,
-            "timestamp": 1.491650411463695
-        },
-        {
-            "x": 4.775846094749007,
-            "y": 1.478702717632789,
-            "heading": -0.5067169090858323,
-            "angularVelocity": 7.865111701229533e-7,
-            "velocityX": 3.5887138960279197,
-            "velocityY": -1.1965657056459655,
-            "timestamp": 1.553128713205159
-        },
-        {
-            "x": 5.004393851392972,
-            "y": 1.4356429788646896,
-            "heading": -0.5067168550876733,
-            "angularVelocity": 8.783287361409535e-7,
-            "velocityX": 3.717535295706156,
-            "velocityY": -0.7004054690576753,
-            "timestamp": 1.614607014946623
-        },
-        {
-            "x": 5.236664772033691,
-            "y": 1.4238771200180054,
-            "heading": -0.5067167933216484,
-            "angularVelocity": 0.000001004680076164895,
-            "velocityX": 3.7780959145145716,
-            "velocityY": -0.19138230096471237,
-            "timestamp": 1.676085316688087
-        },
-        {
-            "x": 5.366036550697627,
-            "y": 1.4270871399969536,
-            "heading": -0.5067167282581706,
-            "angularVelocity": 0.000001901925327561791,
-            "velocityX": 3.781775437099809,
-            "velocityY": 0.09383479793163368,
-            "timestamp": 1.7102945871261934
-        },
-        {
-            "x": 5.494798697340742,
-            "y": 1.4400359689986706,
-            "heading": -0.5067166730622293,
-            "angularVelocity": 0.0000016134790500794753,
-            "velocityX": 3.76395476998197,
-            "velocityY": 0.37851812786083194,
-            "timestamp": 1.7445038575643
-        },
-        {
-            "x": 5.6222191444596294,
-            "y": 1.4626499710733982,
-            "heading": -0.5067166242756445,
-            "angularVelocity": 0.0000014261217576278581,
-            "velocityX": 3.724734421022641,
-            "velocityY": 0.6610489433161765,
-            "timestamp": 1.7787131280024064
-        },
-        {
-            "x": 5.747573461036382,
-            "y": 1.4948005438980272,
-            "heading": -0.5067165796372217,
-            "angularVelocity": 0.0000013048633380814483,
-            "velocityX": 3.6643376187618593,
-            "velocityY": 0.9398204759378852,
-            "timestamp": 1.812922398440513
-        },
-        {
-            "x": 5.870148986660447,
-            "y": 1.5363048197489622,
-            "heading": -0.5067165375293654,
-            "angularVelocity": 0.0000012308902156872765,
-            "velocityX": 3.583108439738183,
-            "velocityY": 1.2132464480944425,
-            "timestamp": 1.8471316688786195
-        },
-        {
-            "x": 5.9892489774115445,
-            "y": 1.5869265226922225,
-            "heading": -0.5067164966802475,
-            "angularVelocity": 0.0000011940949728764683,
-            "velocityX": 3.481512152285735,
-            "velocityY": 1.4797656394002383,
-            "timestamp": 1.881340939316726
-        },
-        {
-            "x": 6.104355729464833,
-            "y": 1.6460687267313168,
-            "heading": -0.5067164560018657,
-            "angularVelocity": 0.0000011891040447074693,
-            "velocityX": 3.3647824282469623,
-            "velocityY": 1.7288355840881788,
-            "timestamp": 1.9155502097548325
-        },
-        {
-            "x": 6.219456425834801,
-            "y": 1.705222715286976,
-            "heading": -0.5067164153233215,
-            "angularVelocity": 0.0000011891087921450935,
-            "velocityX": 3.3646054094668667,
-            "velocityY": 1.7291800672184492,
-            "timestamp": 1.949759480192939
-        },
-        {
-            "x": 6.334557121741728,
-            "y": 1.764376704743614,
-            "heading": -0.5067163746447773,
-            "angularVelocity": 0.0000011891087898511233,
-            "velocityX": 3.3646053959312843,
-            "velocityY": 1.7291800935557256,
-            "timestamp": 1.9839687506310455
-        },
-        {
-            "x": 6.449657817648618,
-            "y": 1.8235306942003213,
-            "heading": -0.5067163339662332,
-            "angularVelocity": 0.000001189108787218646,
-            "velocityX": 3.3646053959302495,
-            "velocityY": 1.7291800935577397,
-            "timestamp": 2.018178021069152
-        },
-        {
-            "x": 6.564758513555509,
-            "y": 1.8826846836570283,
-            "heading": -0.5067162932876892,
-            "angularVelocity": 0.0000011891087819705439,
-            "velocityX": 3.3646053959302504,
-            "velocityY": 1.7291800935577386,
-            "timestamp": 2.0523872915072587
-        },
-        {
-            "x": 6.679859209462396,
-            "y": 1.9418386731137416,
-            "heading": -0.5067162526091454,
-            "angularVelocity": 0.0000011891087784797324,
-            "velocityX": 3.3646053959301545,
-            "velocityY": 1.7291800935579253,
-            "timestamp": 2.0865965619453655
-        },
-        {
-            "x": 6.7949599053264,
-            "y": 2.0009926626538963,
-            "heading": -0.5067162119306018,
-            "angularVelocity": 0.0000011891087739781964,
-            "velocityX": 3.3646053946765995,
-            "velocityY": 1.7291800959970696,
-            "timestamp": 2.120805832383472
-        },
-        {
-            "x": 6.910060040297267,
-            "y": 2.06014774355027,
-            "heading": -0.5067161712446878,
-            "angularVelocity": 0.0000011893242211155864,
-            "velocityX": 3.3645889987368625,
-            "velocityY": 1.7292119983499907,
-            "timestamp": 2.155015102821579
-        },
-        {
-            "x": 7.017868923227757,
-            "y": 2.1174564154331827,
-            "heading": -0.4884934235175169,
-            "angularVelocity": 0.5326844885552473,
-            "velocityX": 3.151452268634164,
-            "velocityY": 1.6752380611740627,
-            "timestamp": 2.1892243732596857
-        },
-        {
-            "x": 7.119036711180674,
-            "y": 2.1700871649460596,
-            "heading": -0.45052101262699384,
-            "angularVelocity": 1.1100035284068692,
-            "velocityX": 2.9573208272872957,
-            "velocityY": 1.5384937719762113,
-            "timestamp": 2.2234336436977924
-        },
-        {
-            "x": 7.212945348428852,
-            "y": 2.2182419084801723,
-            "heading": -0.3958429177265348,
-            "angularVelocity": 1.5983414495607555,
-            "velocityX": 2.745122478366898,
-            "velocityY": 1.4076518708938115,
-            "timestamp": 2.257642914135899
-        },
-        {
-            "x": 7.29827375889861,
-            "y": 2.2619654936466005,
-            "heading": -0.3361817714679226,
-            "angularVelocity": 1.7440052212324946,
-            "velocityX": 2.494306641941948,
-            "velocityY": 1.2781209481077944,
-            "timestamp": 2.291852184574006
-        },
-        {
-            "x": 7.374929402835313,
-            "y": 2.3012175879094494,
-            "heading": -0.2767819040095215,
-            "angularVelocity": 1.7363675605380382,
-            "velocityX": 2.2407856979994336,
-            "velocityY": 1.1474110309913224,
-            "timestamp": 2.3260614550121126
-        },
-        {
-            "x": 7.442942536538503,
-            "y": 2.336017519822898,
-            "heading": -0.22036844778019443,
-            "angularVelocity": 1.6490692583284896,
-            "velocityX": 1.9881492014348112,
-            "velocityY": 1.0172661231232827,
-            "timestamp": 2.3602707254502193
-        },
-        {
-            "x": 7.502356073864926,
-            "y": 2.3663915967666074,
-            "heading": -0.16856655782281169,
-            "angularVelocity": 1.5142646801283255,
-            "velocityX": 1.7367671559648723,
-            "velocityY": 0.8878902284298598,
-            "timestamp": 2.394479995888326
-        },
-        {
-            "x": 7.553208169092694,
-            "y": 2.392365594084153,
-            "heading": -0.12246425110299042,
-            "angularVelocity": 1.3476553615264124,
-            "velocityX": 1.4865004303372251,
-            "velocityY": 0.7592677945160818,
-            "timestamp": 2.428689266326433
-        },
-        {
-            "x": 7.595530700913324,
-            "y": 2.4139635888667987,
-            "heading": -0.08285639859822795,
-            "angularVelocity": 1.1578104998299616,
-            "velocityX": 1.2371655775940182,
-            "velocityY": 0.6313491783381474,
-            "timestamp": 2.4628985367645395
-        },
-        {
-            "x": 7.629350339345417,
-            "y": 2.431207580669044,
-            "heading": -0.050358801794747596,
-            "angularVelocity": 0.9499646261757312,
-            "velocityX": 0.988610338629752,
-            "velocityY": 0.5040736496688569,
-            "timestamp": 2.4971078072026462
-        },
-        {
-            "x": 7.654689602970606,
-            "y": 2.444117248020803,
-            "heading": -0.025464280349729525,
-            "angularVelocity": 0.7277127260009457,
-            "velocityX": 0.7407133592934628,
-            "velocityY": 0.3773733606834889,
-            "timestamp": 2.531317077640753
-        },
-        {
-            "x": 7.671567523131761,
-            "y": 2.4527098140869614,
-            "heading": -0.008571412442380029,
-            "angularVelocity": 0.49380965133159116,
-            "velocityX": 0.4933727011714593,
-            "velocityY": 0.2511765365386793,
-            "timestamp": 2.5655263480788597
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 5.0379367595035626e-36,
-            "angularVelocity": 0.2505581771434718,
-            "velocityX": 0.24649683434481964,
-            "velocityY": 0.12541003821758284,
-            "timestamp": 2.5997356185169664
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 2.854280403338002e-36,
-            "angularVelocity": -6.383230494440131e-35,
-            "velocityX": -1.2082929677962258e-35,
-            "velocityY": 1.8766665143888344e-34,
-            "timestamp": 2.633944888955073
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.1.traj
deleted file mode 100644
index 818f5ad4..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.1.traj	
+++ /dev/null
@@ -1,337 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 1.4815405069278332e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7317173879428727,
-            "y": 4.344479531295635,
-            "heading": -1.013582317544266,
-            "angularVelocity": 0.4585462516411977,
-            "velocityX": 0.37696065327942924,
-            "velocityY": -0.26939731598432753,
-            "timestamp": 0.05761181638969176
-        },
-        {
-            "x": 0.7752730705207751,
-            "y": 4.313424298338961,
-            "heading": -0.9621480921974644,
-            "angularVelocity": 0.892772152832256,
-            "velocityX": 0.7560199505477795,
-            "velocityY": -0.5390427676609577,
-            "timestamp": 0.11522363277938352
-        },
-        {
-            "x": 0.8408250898493527,
-            "y": 4.266812487548848,
-            "heading": -0.8877439224939486,
-            "angularVelocity": 1.2914741170498636,
-            "velocityX": 1.1378224717856051,
-            "velocityY": -0.8090668496689294,
-            "timestamp": 0.17283544916907528
-        },
-        {
-            "x": 0.9285847241631874,
-            "y": 4.204600540366139,
-            "heading": -0.7936992773737622,
-            "angularVelocity": 1.6323846567179892,
-            "velocityX": 1.5232922656043415,
-            "velocityY": -1.0798470015578314,
-            "timestamp": 0.23044726555876704
-        },
-        {
-            "x": 1.0388213735909644,
-            "y": 4.1267152282410935,
-            "heading": -0.6856850144743787,
-            "angularVelocity": 1.8748629997840256,
-            "velocityX": 1.9134381857035365,
-            "velocityY": -1.3518982216811457,
-            "timestamp": 0.2880590819484588
-        },
-        {
-            "x": 1.1718232312823271,
-            "y": 4.033123229014291,
-            "heading": -0.5741627817643435,
-            "angularVelocity": 1.935752762171023,
-            "velocityX": 2.3085864328894954,
-            "velocityY": -1.6245278328622093,
-            "timestamp": 0.34567089833815057
-        },
-        {
-            "x": 1.3270687303513053,
-            "y": 3.9246703032584405,
-            "heading": -0.488682735559766,
-            "angularVelocity": 1.4837242003685942,
-            "velocityX": 2.6946815566251776,
-            "velocityY": -1.882477112373353,
-            "timestamp": 0.40328271472784233
-        },
-        {
-            "x": 1.5010681185276094,
-            "y": 3.804139759424889,
-            "heading": -0.46956462349945394,
-            "angularVelocity": 0.3318435914430377,
-            "velocityX": 3.020203129846763,
-            "velocityY": -2.0921149754812625,
-            "timestamp": 0.4608945311175341
-        },
-        {
-            "x": 1.6785494425409546,
-            "y": 3.677652137140934,
-            "heading": -0.4695645649556455,
-            "angularVelocity": 0.0000010161770978802793,
-            "velocityX": 3.080641006227699,
-            "velocityY": -2.195515264236428,
-            "timestamp": 0.5185063475072258
-        },
-        {
-            "x": 1.85603075289391,
-            "y": 3.551164495689284,
-            "heading": -0.4695645064119953,
-            "angularVelocity": 0.000001016174352223477,
-            "velocityX": 3.080640769116793,
-            "velocityY": -2.1955155969406546,
-            "timestamp": 0.5761181638969175
-        },
-        {
-            "x": 2.033512086622271,
-            "y": 3.4246768870368545,
-            "heading": -0.4695644478683731,
-            "angularVelocity": 0.0000010161738659585955,
-            "velocityX": 3.0806411748565776,
-            "velocityY": -2.195515027626543,
-            "timestamp": 0.6337299802866092
-        },
-        {
-            "x": 2.2109934521039283,
-            "y": 3.298189322939119,
-            "heading": -0.46956438932476746,
-            "angularVelocity": 0.0000010161735782211743,
-            "velocityX": 3.080641726016013,
-            "velocityY": -2.1955142542661994,
-            "timestamp": 0.6913417966763009
-        },
-        {
-            "x": 2.3884748618363862,
-            "y": 3.17170182093203,
-            "heading": -0.4695643307811488,
-            "angularVelocity": 0.000001016173805422874,
-            "velocityX": 3.0806424941015065,
-            "velocityY": -2.1955131765246763,
-            "timestamp": 0.7489536130659926
-        },
-        {
-            "x": 2.5659563234551954,
-            "y": 3.045214391729511,
-            "heading": -0.4695642722374779,
-            "angularVelocity": 0.0000010161747112988211,
-            "velocityX": 3.0806433947214558,
-            "velocityY": -2.1955119128156926,
-            "timestamp": 0.8065654294556843
-        },
-        {
-            "x": 2.743437831089148,
-            "y": 2.918727027093402,
-            "heading": -0.46956421369376844,
-            "angularVelocity": 0.000001016175381373278,
-            "velocityX": 3.08064419343162,
-            "velocityY": -2.1955107921009867,
-            "timestamp": 0.864177245845376
-        },
-        {
-            "x": 2.920919379832659,
-            "y": 2.7922397201404365,
-            "heading": -0.4695641551500532,
-            "angularVelocity": 0.0000010161754817816058,
-            "velocityX": 3.080644906992851,
-            "velocityY": -2.195509790862922,
-            "timestamp": 0.9217890622350677
-        },
-        {
-            "x": 3.0984009676669872,
-            "y": 2.6657524680380327,
-            "heading": -0.4695640966063568,
-            "angularVelocity": 0.000001016175153333812,
-            "velocityX": 3.080645585513673,
-            "velocityY": -2.195508838791537,
-            "timestamp": 0.9794008786247594
-        },
-        {
-            "x": 3.275882647398041,
-            "y": 2.5392653448813247,
-            "heading": -0.4695640380626867,
-            "angularVelocity": 0.0000010161746978103879,
-            "velocityX": 3.080647180615714,
-            "velocityY": -2.1955066006101345,
-            "timestamp": 1.0370126950144511
-        },
-        {
-            "x": 3.4563703536987305,
-            "y": 2.417105913162231,
-            "heading": -0.4695639788949644,
-            "angularVelocity": 0.0000010270067149252654,
-            "velocityX": 3.1328244379564545,
-            "velocityY": -2.1203884788634437,
-            "timestamp": 1.0946245114041429
-        },
-        {
-            "x": 3.7578524899325245,
-            "y": 2.288322056940197,
-            "heading": -0.4695639274563231,
-            "angularVelocity": 5.9355601779034e-7,
-            "velocityX": 3.4788348155609183,
-            "velocityY": -1.4860507766867785,
-            "timestamp": 1.1812863255510104
-        },
-        {
-            "x": 4.061243078218464,
-            "y": 2.1641008385808727,
-            "heading": -0.46956387627292245,
-            "angularVelocity": 5.906107685245515e-7,
-            "velocityX": 3.5008566491785738,
-            "velocityY": -1.43340200735705,
-            "timestamp": 1.267948139697878
-        },
-        {
-            "x": 4.364633703659485,
-            "y": 2.039879710966971,
-            "heading": -0.46956382508952316,
-            "angularVelocity": 5.906107531304183e-7,
-            "velocityX": 3.5008570779150574,
-            "velocityY": -1.433400960235855,
-            "timestamp": 1.3546099538447456
-        },
-        {
-            "x": 4.668024366282006,
-            "y": 1.9156586741630988,
-            "heading": -0.46956377390610504,
-            "angularVelocity": 5.906109701114576e-7,
-            "velocityX": 3.5008575069563967,
-            "velocityY": -1.4333999123691519,
-            "timestamp": 1.4412717679916132
-        },
-        {
-            "x": 4.971415087165123,
-            "y": 1.7914377796518506,
-            "heading": -0.4695637227226054,
-            "angularVelocity": 5.906119107964098e-7,
-            "velocityX": 3.5008581792316793,
-            "velocityY": -1.4333982704392505,
-            "timestamp": 1.5279335821384807
-        },
-        {
-            "x": 5.2748058711699946,
-            "y": 1.6672170393060612,
-            "heading": -0.4695636715390663,
-            "angularVelocity": 5.906123657897232e-7,
-            "velocityX": 3.5008589076003997,
-            "velocityY": -1.4333964915074275,
-            "timestamp": 1.6145953962853483
-        },
-        {
-            "x": 5.5781967938554144,
-            "y": 1.5429966376674569,
-            "heading": -0.4695636203556594,
-            "angularVelocity": 5.906108405690599e-7,
-            "velocityX": 3.5008605078501667,
-            "velocityY": -1.4333925831287677,
-            "timestamp": 1.701257210432216
-        },
-        {
-            "x": 5.886988948403778,
-            "y": 1.4328868446307816,
-            "heading": -0.4695635793301605,
-            "angularVelocity": 4.733976467078746e-7,
-            "velocityX": 3.5631859035981908,
-            "velocityY": -1.2705687518849984,
-            "timestamp": 1.7879190245790835
-        },
-        {
-            "x": 6.195781169913944,
-            "y": 1.3227772393821355,
-            "heading": -0.4695635383046595,
-            "angularVelocity": 4.7339767079441426e-7,
-            "velocityX": 3.5631866762776085,
-            "velocityY": -1.2705665849787193,
-            "timestamp": 1.874580838725951
-        },
-        {
-            "x": 6.5045734104803365,
-            "y": 1.212667687574965,
-            "heading": -0.46956349727913566,
-            "angularVelocity": 4.733979354519185e-7,
-            "velocityX": 3.5631868961694693,
-            "velocityY": -1.270565968311784,
-            "timestamp": 1.9612426528728186
-        },
-        {
-            "x": 6.813365656566466,
-            "y": 1.1025581512474965,
-            "heading": -0.46956345625352064,
-            "angularVelocity": 4.733989867793667e-7,
-            "velocityX": 3.563186959862305,
-            "velocityY": -1.270565789689833,
-            "timestamp": 2.0479044670196864
-        },
-        {
-            "x": 7.101065618447083,
-            "y": 0.9964425714529646,
-            "heading": -0.4012732100130825,
-            "angularVelocity": 0.7880085007764233,
-            "velocityX": 3.3198008224596522,
-            "velocityY": -1.2244790954259896,
-            "timestamp": 2.134566281166554
-        },
-        {
-            "x": 7.333266238045498,
-            "y": 0.911992130350644,
-            "heading": -0.2613611242014092,
-            "angularVelocity": 1.6144606155435655,
-            "velocityX": 2.6793879390165927,
-            "velocityY": -0.9744827284506232,
-            "timestamp": 2.2212280953134216
-        },
-        {
-            "x": 7.506890250099573,
-            "y": 0.8492417427348896,
-            "heading": -0.1371754084070899,
-            "angularVelocity": 1.43299233943868,
-            "velocityX": 2.0034661605379225,
-            "velocityY": -0.7240834759056616,
-            "timestamp": 2.307889909460289
-        },
-        {
-            "x": 7.622366585478705,
-            "y": 0.8076799304753898,
-            "heading": -0.04732610702177306,
-            "angularVelocity": 1.0367807582823931,
-            "velocityX": 1.332493861522807,
-            "velocityY": -0.4795862245517285,
-            "timestamp": 2.3945517236071567
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 2.9489608951695904e-31,
-            "angularVelocity": 0.5461010421679925,
-            "velocityX": 0.6650381726791765,
-            "velocityY": -0.2386279433331854,
-            "timestamp": 2.4812135377540243
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -7.162012659830636e-32,
-            "angularVelocity": -4.0878892657023166e-30,
-            "velocityX": 2.3668785600931567e-32,
-            "velocityY": 6.81692291678628e-32,
-            "timestamp": 2.567875351900892
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.1.traj
deleted file mode 100644
index 9649eb8c..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.1.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7269596910839872,
-            "y": 4.384958038291164,
-            "heading": -1.001571192673364,
-            "angularVelocity": 0.6206074004453302,
-            "velocityX": 0.27389113865872694,
-            "velocityY": 0.40306073338263126,
-            "timestamp": 0.06192128437247234
-        },
-        {
-            "x": 0.760846036232504,
-            "y": 4.4351182711466235,
-            "heading": -0.9264796681168371,
-            "angularVelocity": 1.2126932656117428,
-            "velocityX": 0.5472487447883301,
-            "velocityY": 0.8100644772439347,
-            "timestamp": 0.12384256874494468
-        },
-        {
-            "x": 0.8116293344011996,
-            "y": 4.510831947021006,
-            "heading": -0.817810113065978,
-            "angularVelocity": 1.7549628718484593,
-            "velocityX": 0.8201266928382989,
-            "velocityY": 1.2227407206049843,
-            "timestamp": 0.185763853117417
-        },
-        {
-            "x": 0.8793448344180163,
-            "y": 4.612606899775036,
-            "heading": -0.6817312187907495,
-            "angularVelocity": 2.197610977457753,
-            "velocityX": 1.0935738930977383,
-            "velocityY": 1.6436182450904597,
-            "timestamp": 0.24768513748988935
-        },
-        {
-            "x": 0.9640984700059907,
-            "y": 4.74109575406918,
-            "heading": -0.5303096586797065,
-            "angularVelocity": 2.4453879089491037,
-            "velocityX": 1.3687318738119136,
-            "velocityY": 2.075035354907217,
-            "timestamp": 0.3096064218623617
-        },
-        {
-            "x": 1.0654137163163762,
-            "y": 4.896663616218439,
-            "heading": -0.3925803264267699,
-            "angularVelocity": 2.224264784697608,
-            "velocityX": 1.6361941994120859,
-            "velocityY": 2.5123487622362273,
-            "timestamp": 0.37152770623483405
-        },
-        {
-            "x": 1.179096696888173,
-            "y": 5.075186082347611,
-            "heading": -0.3289347906925715,
-            "angularVelocity": 1.0278458591290565,
-            "velocityX": 1.8359273668802587,
-            "velocityY": 2.88305496144612,
-            "timestamp": 0.4334489906073064
-        },
-        {
-            "x": 1.3095067112863388,
-            "y": 5.269772108097061,
-            "heading": -0.32893471990297235,
-            "angularVelocity": 0.0000011432191669124052,
-            "velocityX": 2.1060611988232667,
-            "velocityY": 3.142473992932148,
-            "timestamp": 0.49537027497977876
-        },
-        {
-            "x": 1.4399171936865056,
-            "y": 5.464357820195416,
-            "heading": -0.32893464911575754,
-            "angularVelocity": 0.0000011431806608935866,
-            "velocityX": 2.106068756838354,
-            "velocityY": 3.1424689276125553,
-            "timestamp": 0.557291559352251
-        },
-        {
-            "x": 1.570327676092705,
-            "y": 5.6589435322897295,
-            "heading": -0.3289345783285442,
-            "angularVelocity": 0.0000011431806365404781,
-            "velocityX": 2.106068756935774,
-            "velocityY": 3.1424689275472675,
-            "timestamp": 0.6192128437247234
-        },
-        {
-            "x": 1.7007381584918697,
-            "y": 5.853529244388756,
-            "heading": -0.32893450754133247,
-            "angularVelocity": 0.000001143180612539941,
-            "velocityX": 2.1060687568221708,
-            "velocityY": 3.1424689276234057,
-            "timestamp": 0.6811341280971958
-        },
-        {
-            "x": 1.8311480951447636,
-            "y": 6.048115322242248,
-            "heading": -0.3289344367512267,
-            "angularVelocity": 0.0000011432273482693713,
-            "velocityX": 2.1060599432732174,
-            "velocityY": 3.1424748343882203,
-            "timestamp": 0.7430554124696681
-        },
-        {
-            "x": 1.9446381116074218,
-            "y": 6.226114434087632,
-            "heading": -0.2658064774598036,
-            "angularVelocity": 1.0194872398268149,
-            "velocityX": 1.8328110860877296,
-            "velocityY": 2.8746030326934724,
-            "timestamp": 0.8049766968421405
-        },
-        {
-            "x": 2.0457809471284243,
-            "y": 6.380047389358405,
-            "heading": -0.1375002927378408,
-            "angularVelocity": 2.0720853261080365,
-            "velocityX": 1.6334098451931771,
-            "velocityY": 2.485945775039604,
-            "timestamp": 0.8668979812146128
-        },
-        {
-            "x": 2.1306775060879315,
-            "y": 6.50749359169558,
-            "heading": 0.01763716177237307,
-            "angularVelocity": 2.5053978786522335,
-            "velocityX": 1.3710400199200075,
-            "velocityY": 2.0581970097802293,
-            "timestamp": 0.9288192655870852
-        },
-        {
-            "x": 2.198423978678441,
-            "y": 6.608249711273454,
-            "heading": 0.15869859646801654,
-            "angularVelocity": 2.278076692452355,
-            "velocityX": 1.0940740857860296,
-            "velocityY": 1.6271645622174213,
-            "timestamp": 0.9907405499595575
-        },
-        {
-            "x": 2.24920191473134,
-            "y": 6.683037424747583,
-            "heading": 0.27235604329703667,
-            "angularVelocity": 1.8355150087866652,
-            "velocityX": 0.8200400971571714,
-            "velocityY": 1.2077868576540292,
-            "timestamp": 1.0526618343320298
-        },
-        {
-            "x": 2.2830642302273354,
-            "y": 6.732456156660043,
-            "heading": 0.35192243545091534,
-            "angularVelocity": 1.2849603001653922,
-            "velocityX": 0.5468606770541943,
-            "velocityY": 0.7980895812043076,
-            "timestamp": 1.114583118704502
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.6633836000880191,
-            "velocityX": 0.2735048205846554,
-            "velocityY": 0.3963716771816175,
-            "timestamp": 1.1765044030769745
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0,
-            "velocityX": 2.8187230537991962e-33,
-            "velocityY": 0,
-            "timestamp": 1.2384256874494468
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.1.traj
deleted file mode 100644
index f9120604..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": 2.3377521162047767e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.737441410859622,
-            "y": 4.3811882786622816,
-            "heading": -1.0071421705955907,
-            "angularVelocity": 0.501398563530153,
-            "velocityX": 0.41874598035403643,
-            "velocityY": 0.32332545020514236,
-            "timestamp": 0.06553235648117978
-        },
-        {
-            "x": 0.7924436512054522,
-            "y": 4.423719236654154,
-            "heading": -0.9443396668116766,
-            "angularVelocity": 0.9583434375956188,
-            "velocityX": 0.8393142456524665,
-            "velocityY": 0.6490069986127681,
-            "timestamp": 0.13106471296235955
-        },
-        {
-            "x": 0.8751754235694627,
-            "y": 4.487775832809834,
-            "heading": -0.8558630826468019,
-            "angularVelocity": 1.350120595621899,
-            "velocityX": 1.2624568504227318,
-            "velocityY": 0.9774804324956131,
-            "timestamp": 0.19659706944353933
-        },
-        {
-            "x": 0.9858651777004729,
-            "y": 4.5735665588886505,
-            "heading": -0.748264717894899,
-            "angularVelocity": 1.641912034443748,
-            "velocityX": 1.6890855155315267,
-            "velocityY": 1.3091353750334698,
-            "timestamp": 0.2621294259247191
-        },
-        {
-            "x": 1.1247487447497322,
-            "y": 4.6813201098081265,
-            "heading": -0.6327126866941822,
-            "angularVelocity": 1.7632821007116781,
-            "velocityX": 2.1193128785036808,
-            "velocityY": 1.644280119095407,
-            "timestamp": 0.3276617824058989
-        },
-        {
-            "x": 1.291489149115039,
-            "y": 4.810947116825512,
-            "heading": -0.5360472598717524,
-            "angularVelocity": 1.4750793655678647,
-            "velocityX": 2.5443981159626317,
-            "velocityY": 1.9780611285451524,
-            "timestamp": 0.39319413888707866
-        },
-        {
-            "x": 1.4799949106081978,
-            "y": 4.9607414161097925,
-            "heading": -0.5171264547525753,
-            "angularVelocity": 0.288724626049585,
-            "velocityX": 2.876529574322522,
-            "velocityY": 2.285806696533185,
-            "timestamp": 0.45872649536825844
-        },
-        {
-            "x": 1.6682619137790426,
-            "y": 5.11061369240764,
-            "heading": -0.49798057816746544,
-            "angularVelocity": 0.29215913501614493,
-            "velocityX": 2.8728862088900566,
-            "velocityY": 2.2869965974882214,
-            "timestamp": 0.5242588518494382
-        },
-        {
-            "x": 1.8349963336194852,
-            "y": 5.239757443274396,
-            "heading": -0.40436368242247644,
-            "angularVelocity": 1.4285598866244509,
-            "velocityX": 2.54430679428301,
-            "velocityY": 1.970686814899508,
-            "timestamp": 0.589791208330618
-        },
-        {
-            "x": 1.9739513715778099,
-            "y": 5.347175023297594,
-            "heading": -0.28864983484418905,
-            "angularVelocity": 1.7657513599639834,
-            "velocityX": 2.120403498662996,
-            "velocityY": 1.6391533250303247,
-            "timestamp": 0.6553235648117979
-        },
-        {
-            "x": 2.0846889840994356,
-            "y": 5.432668431655269,
-            "heading": -0.18116230899252247,
-            "angularVelocity": 1.6402206730126385,
-            "velocityX": 1.6898158172204303,
-            "velocityY": 1.304598414406612,
-            "timestamp": 0.7208559212929777
-        },
-        {
-            "x": 2.167475833751633,
-            "y": 5.496492915332372,
-            "heading": -0.09361421148490373,
-            "angularVelocity": 1.3359522258711838,
-            "velocityX": 1.2632973098712375,
-            "velocityY": 0.973938480228949,
-            "timestamp": 0.7863882777741575
-        },
-        {
-            "x": 2.2225290058379836,
-            "y": 5.538875443360028,
-            "heading": -0.032000086006940494,
-            "angularVelocity": 0.9402092155141033,
-            "velocityX": 0.8400914455466154,
-            "velocityY": 0.6467420111747162,
-            "timestamp": 0.8519206342553374
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 2.4308347188047162e-30,
-            "angularVelocity": 0.48830971027463277,
-            "velocityX": 0.41919741082263007,
-            "velocityY": 0.3223530752482305,
-            "timestamp": 0.9174529907365172
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 1.7485008040489027e-29,
-            "angularVelocity": 1.1123032826758857e-30,
-            "velocityX": 0,
-            "velocityY": 1.931278949788811e-31,
-            "timestamp": 0.982985347217697
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.1.traj
deleted file mode 100644
index ff6d2947..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.1.traj	
+++ /dev/null
@@ -1,121 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": -2.204776300650663e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7541176231747116,
-            "y": 4.360036687345491,
-            "heading": -1.0162545684059647,
-            "angularVelocity": 0.32461244675544165,
-            "velocityX": 0.6031109414489596,
-            "velocityY": 0.0005015351663374791,
-            "timestamp": 0.07315009584923313
-        },
-        {
-            "x": 0.8424088865359771,
-            "y": 4.360100324606402,
-            "heading": -0.9701479863262087,
-            "angularVelocity": 0.630301048063541,
-            "velocityX": 1.2069876646948012,
-            "velocityY": 0.0008699545799604022,
-            "timestamp": 0.14630019169846625
-        },
-        {
-            "x": 0.974955221752511,
-            "y": 4.3601789275184695,
-            "heading": -0.9038423136459668,
-            "angularVelocity": 0.9064331619859869,
-            "velocityX": 1.8119776013652948,
-            "velocityY": 0.0010745428444254071,
-            "timestamp": 0.21945028754769938
-        },
-        {
-            "x": 1.151884261990258,
-            "y": 4.360259672825357,
-            "heading": -0.8212724833638784,
-            "angularVelocity": 1.128772687492223,
-            "velocityX": 2.4187123500481573,
-            "velocityY": 0.0011038305000959348,
-            "timestamp": 0.2926003833969325
-        },
-        {
-            "x": 1.3733948258568207,
-            "y": 4.360324542882412,
-            "heading": -0.7323749312717828,
-            "angularVelocity": 1.215275948172809,
-            "velocityX": 3.028165052894887,
-            "velocityY": 0.0008868075468144582,
-            "timestamp": 0.36575047924616566
-        },
-        {
-            "x": 1.6366160230983884,
-            "y": 4.359789775146019,
-            "heading": -0.6992109641912426,
-            "angularVelocity": 0.4533687440268879,
-            "velocityX": 3.5983711871554758,
-            "velocityY": -0.007310554144235059,
-            "timestamp": 0.4389005750953988
-        },
-        {
-            "x": 1.8581232786849116,
-            "y": 4.3598960912826,
-            "heading": -0.6098314600860953,
-            "angularVelocity": 1.2218644838062247,
-            "velocityX": 3.0281198269795637,
-            "velocityY": 0.001453397091854858,
-            "timestamp": 0.512050670944632
-        },
-        {
-            "x": 2.035050245808528,
-            "y": 4.359956526837273,
-            "heading": -0.5268891037054299,
-            "angularVelocity": 1.1338653137464598,
-            "velocityX": 2.4186840094956525,
-            "velocityY": 0.000826185584123381,
-            "timestamp": 0.5852007667938651
-        },
-        {
-            "x": 2.1675937895751547,
-            "y": 4.359986275638159,
-            "heading": -0.46022423203591045,
-            "angularVelocity": 0.9113435996982586,
-            "velocityX": 1.8119394407877087,
-            "velocityY": 0.00040668163915013244,
-            "timestamp": 0.6583508626430983
-        },
-        {
-            "x": 2.255882919836075,
-            "y": 4.359997704013322,
-            "heading": -0.41385948514831483,
-            "angularVelocity": 0.6338302957681125,
-            "velocityX": 1.2069585040994306,
-            "velocityY": 0.0001562318549097459,
-            "timestamp": 0.7315009584923314
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0.32617161838698483,
-            "velocityX": 0.6031035182080025,
-            "velocityY": 0.00003138733655167869,
-            "timestamp": 0.8046510543415646
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -2.2755735462869925e-32,
-            "velocityX": -9.780003655586715e-35,
-            "velocityY": 0,
-            "timestamp": 0.8778011501907977
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.1.traj
deleted file mode 100644
index b6c78aaa..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 6.154725095706714e-32,
-            "velocityX": 1.2115062621827067e-33,
-            "velocityY": 6.540799795634554e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3666999539229265,
-            "y": 6.765820977804648,
-            "heading": 0.36628259128993745,
-            "angularVelocity": -0.26339586361920003,
-            "velocityX": 0.7407424724032508,
-            "velocityY": 0.09796218023747214,
-            "timestamp": 0.09004472729439511
-        },
-        {
-            "x": 2.5001846245776895,
-            "y": 6.783480891306791,
-            "heading": 0.3222458902940237,
-            "angularVelocity": -0.4890536327789505,
-            "velocityX": 1.4824262859761226,
-            "velocityY": 0.1961237935054848,
-            "timestamp": 0.18008945458879022
-        },
-        {
-            "x": 2.70056880915549,
-            "y": 6.810010034997764,
-            "heading": 0.26375227569196347,
-            "angularVelocity": -0.6496062163731077,
-            "velocityX": 2.225384990313294,
-            "velocityY": 0.2946218450330504,
-            "timestamp": 0.27013418188318533
-        },
-        {
-            "x": 2.967965472322839,
-            "y": 6.8454603445891635,
-            "heading": 0.20325713210858826,
-            "angularVelocity": -0.6718343805472418,
-            "velocityX": 2.9695982341432847,
-            "velocityY": 0.3936966733820705,
-            "timestamp": 0.36017890917758044
-        },
-        {
-            "x": 3.3011740475809854,
-            "y": 6.889827568786931,
-            "heading": 0.1886164627156386,
-            "angularVelocity": -0.16259330038374148,
-            "velocityX": 3.700478476310374,
-            "velocityY": 0.4927242885939573,
-            "timestamp": 0.45022363647197555
-        },
-        {
-            "x": 3.6388963576772766,
-            "y": 6.934269203744266,
-            "heading": 0.18861644342798295,
-            "angularVelocity": -2.1420083336262225e-7,
-            "velocityX": 3.7506061736644574,
-            "velocityY": 0.4935506641275844,
-            "timestamp": 0.5402683637663707
-        },
-        {
-            "x": 3.9766186774172247,
-            "y": 6.978710765417191,
-            "heading": 0.1886164241403376,
-            "angularVelocity": -2.1420071941558326e-7,
-            "velocityX": 3.750606280762983,
-            "velocityY": 0.4935498502608198,
-            "timestamp": 0.6303130910607657
-        },
-        {
-            "x": 4.314341007224031,
-            "y": 7.023152250589549,
-            "heading": 0.18861640485269274,
-            "angularVelocity": -2.1420071403103001e-7,
-            "velocityX": 3.750606392561398,
-            "velocityY": 0.4935490006767261,
-            "timestamp": 0.7203578183551609
-        },
-        {
-            "x": 4.652063347871557,
-            "y": 7.067593653380398,
-            "heading": 0.1886163855650432,
-            "angularVelocity": -2.1420076555553473e-7,
-            "velocityX": 3.750606512954019,
-            "velocityY": 0.49354808578132814,
-            "timestamp": 0.810402545649556
-        },
-        {
-            "x": 4.989785700043158,
-            "y": 7.112034968596579,
-            "heading": 0.18861636627738598,
-            "angularVelocity": -2.1420085095836123e-7,
-            "velocityX": 3.7506066409356835,
-            "velocityY": 0.4935471132127838,
-            "timestamp": 0.9004472729439512
-        },
-        {
-            "x": 5.327508064058316,
-            "y": 7.15647619381006,
-            "heading": 0.18861634698972354,
-            "angularVelocity": -2.1420090940402864e-7,
-            "velocityX": 3.7506067724653955,
-            "velocityY": 0.4935461136795145,
-            "timestamp": 0.9904920002383464
-        },
-        {
-            "x": 5.6652304398823485,
-            "y": 7.200917329284216,
-            "heading": 0.18861632770206457,
-            "angularVelocity": -2.1420087020950116e-7,
-            "velocityX": 3.750606903609938,
-            "velocityY": 0.4935451170711973,
-            "timestamp": 1.0805367275327415
-        },
-        {
-            "x": 6.002952827157885,
-            "y": 7.245358377734637,
-            "heading": 0.18861630841441618,
-            "angularVelocity": -2.1420075320811155e-7,
-            "velocityX": 3.750607030785673,
-            "velocityY": 0.49354415062109597,
-            "timestamp": 1.1705814548271367
-        },
-        {
-            "x": 6.340675225087303,
-            "y": 7.289799345222555,
-            "heading": 0.1886162891267749,
-            "angularVelocity": -2.1420067405327174e-7,
-            "velocityX": 3.750607149103328,
-            "velocityY": 0.4935432514845822,
-            "timestamp": 1.2606261821215319
-        },
-        {
-            "x": 6.678397632133336,
-            "y": 7.334240243430023,
-            "heading": 0.18861626983912938,
-            "angularVelocity": -2.1420072105835628e-7,
-            "velocityX": 3.7506072503487453,
-            "velocityY": 0.4935424820841753,
-            "timestamp": 1.350670909415927
-        },
-        {
-            "x": 7.011808183476814,
-            "y": 7.378579327724673,
-            "heading": 0.17443906284640512,
-            "angularVelocity": -0.15744627607536488,
-            "velocityX": 3.7027215403008924,
-            "velocityY": 0.49241177831197624,
-            "timestamp": 1.4407156367103222
-        },
-        {
-            "x": 7.279274281127278,
-            "y": 7.414016049929063,
-            "heading": 0.11701095642225132,
-            "angularVelocity": -0.637773117313093,
-            "velocityX": 2.9703693451811146,
-            "velocityY": 0.3935457774060549,
-            "timestamp": 1.5307603640047174
-        },
-        {
-            "x": 7.479727163444043,
-            "y": 7.440534681549159,
-            "heading": 0.061439635089441606,
-            "angularVelocity": -0.6171524197205129,
-            "velocityX": 2.226147919371209,
-            "velocityY": 0.294505102263186,
-            "timestamp": 1.6208050912991125
-        },
-        {
-            "x": 7.613267962945412,
-            "y": 7.458185352165826,
-            "heading": 0.019571051398075176,
-            "angularVelocity": -0.46497540666073534,
-            "velocityX": 1.4830496300440337,
-            "velocityY": 0.19602114579078314,
-            "timestamp": 1.7108498185935077
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -0.003,
-            "angularVelocity": -0.2506648870652985,
-            "velocityX": 0.7410987745724689,
-            "velocityY": 0.09789188216823574,
-            "timestamp": 1.8008945458879029
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -0.003,
-            "angularVelocity": 5.335131961970383e-32,
-            "velocityX": 0,
-            "velocityY": -3.2330177098423496e-35,
-            "timestamp": 1.890939273182298
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.1.traj
deleted file mode 100644
index cd2bebdf..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.1.traj	
+++ /dev/null
@@ -1,256 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 2.274971502954598e-31,
-            "velocityX": 3.487335890979115e-33,
-            "velocityY": 2.862188318424697e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3417817158351957,
-            "y": 6.761449131523058,
-            "heading": 0.37953631669999827,
-            "angularVelocity": -0.14722299547472617,
-            "velocityX": 0.5878646347531417,
-            "velocityY": 0.06259884318982863,
-            "timestamp": 0.07107370194626751
-        },
-        {
-            "x": 2.42534939307215,
-            "y": 6.770350816518497,
-            "heading": 0.3588464620969251,
-            "angularVelocity": -0.2911042204995314,
-            "velocityX": 1.1757890042104941,
-            "velocityY": 0.12524583287024788,
-            "timestamp": 0.14214740389253502
-        },
-        {
-            "x": 2.5507092244237364,
-            "y": 6.783710205716013,
-            "heading": 0.3282884162089386,
-            "angularVelocity": -0.42994870185747563,
-            "velocityX": 1.7638005045292426,
-            "velocityY": 0.18796529281121122,
-            "timestamp": 0.21322110583880255
-        },
-        {
-            "x": 2.717871083967313,
-            "y": 6.80153596037414,
-            "heading": 0.2884653946252666,
-            "angularVelocity": -0.5603059991696991,
-            "velocityX": 2.351950932145827,
-            "velocityY": 0.25080661580833774,
-            "timestamp": 0.28429480778507005
-        },
-        {
-            "x": 2.926852904271386,
-            "y": 6.82384561239607,
-            "heading": 0.2406022074409234,
-            "angularVelocity": -0.673430338841082,
-            "velocityX": 2.9403536692385535,
-            "velocityY": 0.31389461096009597,
-            "timestamp": 0.35536850973133755
-        },
-        {
-            "x": 3.1776924244019935,
-            "y": 6.85069218229951,
-            "heading": 0.1884744580400949,
-            "angularVelocity": -0.7334323100298853,
-            "velocityX": 3.529287391280776,
-            "velocityY": 0.3777285995842137,
-            "timestamp": 0.42644221167760504
-        },
-        {
-            "x": 3.4450596021465354,
-            "y": 6.879057072545224,
-            "heading": 0.1884744481252109,
-            "angularVelocity": -1.3950144320454384e-7,
-            "velocityX": 3.7618299092775445,
-            "velocityY": 0.39909121755204374,
-            "timestamp": 0.49751591362387254
-        },
-        {
-            "x": 3.712426777772062,
-            "y": 6.907421982754425,
-            "heading": 0.18847443820909762,
-            "angularVelocity": -1.3951873937625683e-7,
-            "velocityX": 3.7618298794631864,
-            "velocityY": 0.39909149843660235,
-            "timestamp": 0.5685896155701401
-        },
-        {
-            "x": 3.979793955045072,
-            "y": 6.93578687743453,
-            "heading": 0.188474428292985,
-            "angularVelocity": -1.3951872987555026e-7,
-            "velocityX": 3.7618299026431106,
-            "velocityY": 0.3990912799437423,
-            "timestamp": 0.6396633175164076
-        },
-        {
-            "x": 4.247161134956238,
-            "y": 6.964151747247427,
-            "heading": 0.18847441837687146,
-            "angularVelocity": -1.395187425580786e-7,
-            "velocityX": 3.761829939761708,
-            "velocityY": 0.399090930064572,
-            "timestamp": 0.7107370194626751
-        },
-        {
-            "x": 4.514528318048531,
-            "y": 6.992516587075052,
-            "heading": 0.18847440846075478,
-            "angularVelocity": -1.3951878716159625e-7,
-            "velocityX": 3.7618299845198404,
-            "velocityY": 0.39909050817474234,
-            "timestamp": 0.7818107214089426
-        },
-        {
-            "x": 4.781895504744899,
-            "y": 7.020881392930694,
-            "heading": 0.18847439854463183,
-            "angularVelocity": -1.395188750932112e-7,
-            "velocityX": 3.761830035228834,
-            "velocityY": 0.3990900301922755,
-            "timestamp": 0.8528844233552101
-        },
-        {
-            "x": 5.049262689335958,
-            "y": 7.049246218630208,
-            "heading": 0.18847438862849922,
-            "angularVelocity": -1.3951901124406535e-7,
-            "velocityX": 3.7618300056073353,
-            "velocityY": 0.3990903093936624,
-            "timestamp": 0.9239581253014776
-        },
-        {
-            "x": 5.3166271708653765,
-            "y": 7.077636511969196,
-            "heading": 0.18847437871238637,
-            "angularVelocity": -1.3951873300413244e-7,
-            "velocityX": 3.761791973795463,
-            "velocityY": 0.3994486365804798,
-            "timestamp": 0.9950318272477451
-        },
-        {
-            "x": 5.584732592713635,
-            "y": 7.097866624574175,
-            "heading": 0.18847436874075538,
-            "angularVelocity": -1.4029986815741496e-7,
-            "velocityX": 3.772216931248832,
-            "velocityY": 0.28463569578063386,
-            "timestamp": 1.0661055291940127
-        },
-        {
-            "x": 5.85271010689652,
-            "y": 7.076007397472714,
-            "heading": 0.188474358062239,
-            "angularVelocity": -1.5024567569917294e-7,
-            "velocityX": 3.7704172829702145,
-            "velocityY": -0.3075571766057785,
-            "timestamp": 1.1371792311402802
-        },
-        {
-            "x": 6.113985538482666,
-            "y": 7.0125651359558105,
-            "heading": 0.18847434515977443,
-            "angularVelocity": -1.8153640819270364e-7,
-            "velocityX": 3.6761196396335167,
-            "velocityY": -0.8926263833128883,
-            "timestamp": 1.2082529330865477
-        },
-        {
-            "x": 6.38971197835489,
-            "y": 6.891444245730791,
-            "heading": 0.18847433517810044,
-            "angularVelocity": -1.2538350348338093e-7,
-            "velocityX": 3.4635019217336325,
-            "velocityY": -1.521444357133088,
-            "timestamp": 1.2878620818911364
-        },
-        {
-            "x": 6.640022696468843,
-            "y": 6.723993100546061,
-            "heading": 0.18847432674418188,
-            "angularVelocity": -1.0594157434839283e-7,
-            "velocityX": 3.1442456284571,
-            "velocityY": -2.103415847287377,
-            "timestamp": 1.367471230695725
-        },
-        {
-            "x": 6.8648306229514935,
-            "y": 6.523601637914116,
-            "heading": 0.18847431867026868,
-            "angularVelocity": -1.014194138550125e-7,
-            "velocityX": 2.8238956182605124,
-            "velocityY": -2.5171913736164258,
-            "timestamp": 1.4470803795003138
-        },
-        {
-            "x": 7.089642217529903,
-            "y": 6.3232142903896476,
-            "heading": 0.1884743105959038,
-            "angularVelocity": -1.0142508708190059e-7,
-            "velocityX": 2.823941694568014,
-            "velocityY": -2.517139682229462,
-            "timestamp": 1.5266895283049025
-        },
-        {
-            "x": 7.2864279243188,
-            "y": 6.147809259471006,
-            "heading": 0.12465186300240168,
-            "angularVelocity": -0.8016973997575559,
-            "velocityX": 2.471898139143281,
-            "velocityY": -2.203327551576542,
-            "timestamp": 1.6062986771094911
-        },
-        {
-            "x": 7.443856673448633,
-            "y": 6.007485445692029,
-            "heading": 0.07359246308164451,
-            "angularVelocity": -0.6413760313670541,
-            "velocityX": 1.9775208188222257,
-            "velocityY": -1.7626593913656032,
-            "timestamp": 1.6859078259140798
-        },
-        {
-            "x": 7.561928317357885,
-            "y": 5.902242696623479,
-            "heading": 0.0352966131369852,
-            "angularVelocity": -0.4810483533581169,
-            "velocityX": 1.483141644926293,
-            "velocityY": -1.3219931458747511,
-            "timestamp": 1.7655169747186685
-        },
-        {
-            "x": 7.640642772199918,
-            "y": 5.832080900256126,
-            "heading": 0.009765510491971057,
-            "angularVelocity": -0.320705635324694,
-            "velocityX": 0.9887614178017027,
-            "velocityY": -0.8813283073733266,
-            "timestamp": 1.8451261235232572
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -0.003,
-            "angularVelocity": -0.1603523047748632,
-            "velocityX": 0.4943807136624207,
-            "velocityY": -0.44066417971947147,
-            "timestamp": 1.924735272327846
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -0.003,
-            "angularVelocity": 9.952815155734959e-29,
-            "velocityX": 0,
-            "velocityY": 1.270429811278963e-30,
-            "timestamp": 2.004344421132435
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.1.traj
deleted file mode 100644
index a91e184b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.1.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3668131239789423,
-            "y": 6.6799532352680275,
-            "heading": 0.31688592464549686,
-            "angularVelocity": -0.6546901107445974,
-            "velocityX": 0.5982690928509629,
-            "velocityY": -0.6899048465062367,
-            "timestamp": 0.11167737858654542
-        },
-        {
-            "x": 2.5010623988117353,
-            "y": 6.525329092787392,
-            "heading": 0.1875032700506318,
-            "angularVelocity": -1.1585395022018103,
-            "velocityX": 1.202116995688263,
-            "velocityY": -1.3845609955896643,
-            "timestamp": 0.22335475717309083
-        },
-        {
-            "x": 2.7034647042549174,
-            "y": 6.29238592031536,
-            "heading": 0.03684167640916454,
-            "angularVelocity": -1.3490788873122188,
-            "velocityX": 1.8123841014618285,
-            "velocityY": -2.0858581694904785,
-            "timestamp": 0.33503213575963625
-        },
-        {
-            "x": 2.970402662431944,
-            "y": 5.984552676414473,
-            "heading": 2.692880535790558e-7,
-            "angularVelocity": -0.32989140314179594,
-            "velocityX": 2.39025988571325,
-            "velocityY": -2.7564511971627637,
-            "timestamp": 0.44670951434618167
-        },
-        {
-            "x": 3.247779632307318,
-            "y": 5.665896160181276,
-            "heading": 2.2674846776964554e-7,
-            "angularVelocity": -3.809149744305609e-7,
-            "velocityX": 2.4837346057547505,
-            "velocityY": -2.853366727141142,
-            "timestamp": 0.5583868929327271
-        },
-        {
-            "x": 3.5251566242922663,
-            "y": 5.347239663193555,
-            "heading": 1.842089099097335e-7,
-            "angularVelocity": -3.8091472416520575e-7,
-            "velocityX": 2.4837348037318896,
-            "velocityY": -2.853366554810183,
-            "timestamp": 0.6700642715192726
-        },
-        {
-            "x": 3.8025335437012195,
-            "y": 5.0285831030297485,
-            "heading": 1.4166936288049968e-7,
-            "angularVelocity": -3.8091462718327505e-7,
-            "velocityX": 2.4837341538598574,
-            "velocityY": -2.8533671205119875,
-            "timestamp": 0.7817416501058181
-        },
-        {
-            "x": 4.079910865419093,
-            "y": 4.7099268930580935,
-            "heading": 9.912982095474378e-8,
-            "angularVelocity": -3.809145814844431e-7,
-            "velocityX": 2.4837377562807075,
-            "velocityY": -2.853363984763686,
-            "timestamp": 0.8934190286923636
-        },
-        {
-            "x": 4.382964098552189,
-            "y": 4.415582373772677,
-            "heading": 5.542611481257948e-8,
-            "angularVelocity": -3.9133893269333644e-7,
-            "velocityX": 2.713649236476739,
-            "velocityY": -2.635668234792112,
-            "timestamp": 1.005096407278909
-        },
-        {
-            "x": 4.748738765716553,
-            "y": 4.2041850090026855,
-            "heading": 0,
-            "angularVelocity": -4.963056575446211e-7,
-            "velocityX": 3.27527984443961,
-            "velocityY": -1.8929291450565913,
-            "timestamp": 1.1167737858654545
-        },
-        {
-            "x": 5.133803316447558,
-            "y": 4.089523361787239,
-            "heading": -1.387645596172588e-10,
-            "angularVelocity": -1.3065520492479572e-9,
-            "velocityX": 3.625615065458012,
-            "velocityY": -1.0796085871456598,
-            "timestamp": 1.2229804690844188
-        },
-        {
-            "x": 5.534974194059096,
-            "y": 4.0675249151482635,
-            "heading": -2.526284831735191e-10,
-            "angularVelocity": -1.0720975375429818e-9,
-            "velocityX": 3.7772658504404184,
-            "velocityY": -0.20712864739049783,
-            "timestamp": 1.3291871523033831
-        },
-        {
-            "x": 5.9365934232146325,
-            "y": 4.0786604378791305,
-            "heading": -3.6352969398434843e-10,
-            "angularVelocity": -1.0442018095219078e-9,
-            "velocityX": 3.781487350730336,
-            "velocityY": 0.10484766488641149,
-            "timestamp": 1.4353938355223474
-        },
-        {
-            "x": 6.3382126442540345,
-            "y": 4.089796253283455,
-            "heading": -4.744184411388132e-10,
-            "angularVelocity": -1.044084456790862e-9,
-            "velocityX": 3.781487274312031,
-            "velocityY": 0.1048504205837075,
-            "timestamp": 1.5416005187413118
-        },
-        {
-            "x": 6.739831868633552,
-            "y": 4.100931948201603,
-            "heading": -5.853040774969199e-10,
-            "angularVelocity": -1.0440551668485128e-9,
-            "velocityX": 3.7814873057612264,
-            "velocityY": 0.10484928613381475,
-            "timestamp": 1.647807201960276
-        },
-        {
-            "x": 7.11589910333758,
-            "y": 4.111359168329562,
-            "heading": -0.0011999964635320353,
-            "angularVelocity": -0.011298685184913941,
-            "velocityX": 3.5408999067290075,
-            "velocityY": 0.09817856854143046,
-            "timestamp": 1.7540138851792404
-        },
-        {
-            "x": 7.397949547057075,
-            "y": 4.119179583905487,
-            "heading": -0.002099997192055183,
-            "angularVelocity": -0.008474049854922212,
-            "velocityX": 2.6556750966226486,
-            "velocityY": 0.0736339309252122,
-            "timestamp": 1.8602205683982047
-        },
-        {
-            "x": 7.585983181328134,
-            "y": 4.124393194553852,
-            "heading": -0.0026999988369627403,
-            "angularVelocity": -0.005649377484767236,
-            "velocityX": 1.7704501126675238,
-            "velocityY": 0.049089289773120645,
-            "timestamp": 1.966427251617169
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -0.003,
-            "angularVelocity": -0.0028246919491873987,
-            "velocityX": 0.8852250707992916,
-            "velocityY": 0.024544646034960003,
-            "timestamp": 2.072633934836133
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -0.003,
-            "angularVelocity": 0,
-            "velocityX": 1.6661554323284825e-27,
-            "velocityY": 0,
-            "timestamp": 2.1788406180550974
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.1.traj
deleted file mode 100644
index 785ed7b8..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.1.traj	
+++ /dev/null
@@ -1,400 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 1.592522039385819e-32,
-            "velocityX": 0,
-            "velocityY": -2.1348488243236776e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 2.311435758812473,
-            "y": 6.743296066265471,
-            "heading": 0.3325883455828011,
-            "angularVelocity": -1.0899722636212008,
-            "velocityX": 0.2171102722189281,
-            "velocityY": -0.26017204737900074,
-            "timestamp": 0.052672582902671385
-        },
-        {
-            "x": 2.3359939039892414,
-            "y": 6.71538911279917,
-            "heading": 0.2289766220305027,
-            "angularVelocity": -1.9670902363712421,
-            "velocityX": 0.46624152117519013,
-            "velocityY": -0.529819346772266,
-            "timestamp": 0.10534516580534277
-        },
-        {
-            "x": 2.3766647023789824,
-            "y": 6.674615688796359,
-            "heading": 0.08902544501665462,
-            "angularVelocity": -2.6570023587499336,
-            "velocityX": 0.7721436115045406,
-            "velocityY": -0.7740919802272123,
-            "timestamp": 0.15801774870801416
-        },
-        {
-            "x": 2.4341603545379895,
-            "y": 6.621640281380937,
-            "heading": -0.08739056422595769,
-            "angularVelocity": -3.349294823240299,
-            "velocityX": 1.0915669783129407,
-            "velocityY": -1.0057491866937662,
-            "timestamp": 0.21069033161068554
-        },
-        {
-            "x": 2.508963105088746,
-            "y": 6.555426903517877,
-            "heading": -0.2922468734649172,
-            "angularVelocity": -3.8892398654057443,
-            "velocityX": 1.4201458601143102,
-            "velocityY": -1.2570748236404126,
-            "timestamp": 0.2633629145133569
-        },
-        {
-            "x": 2.602244928931498,
-            "y": 6.475307627430128,
-            "heading": -0.4990571722352239,
-            "angularVelocity": -3.9263367652285313,
-            "velocityX": 1.7709749304513536,
-            "velocityY": -1.5210812090949455,
-            "timestamp": 0.3160354974160283
-        },
-        {
-            "x": 2.7117676530035184,
-            "y": 6.382297804514516,
-            "heading": -0.6662019146269795,
-            "angularVelocity": -3.173277883497873,
-            "velocityX": 2.0793118172009177,
-            "velocityY": -1.7658109359755114,
-            "timestamp": 0.3687080803186997
-        },
-        {
-            "x": 2.833611348488951,
-            "y": 6.27498915923685,
-            "heading": -0.7793427065081729,
-            "angularVelocity": -2.1480015910792667,
-            "velocityX": 2.3132280357425485,
-            "velocityY": -2.0372770683364947,
-            "timestamp": 0.42138066322137113
-        },
-        {
-            "x": 2.968521622075034,
-            "y": 6.1546184694434425,
-            "heading": -0.8356933529016568,
-            "angularVelocity": -1.0698288044390758,
-            "velocityX": 2.5612997531442825,
-            "velocityY": -2.2852627146807634,
-            "timestamp": 0.47405324612404254
-        },
-        {
-            "x": 3.1177887530073343,
-            "y": 6.022632523021301,
-            "heading": -0.8357129106693775,
-            "angularVelocity": -0.00037130830959681015,
-            "velocityX": 2.8338676918144654,
-            "velocityY": -2.5057807904736045,
-            "timestamp": 0.5267258290267139
-        },
-        {
-            "x": 3.269422043665646,
-            "y": 5.893362401601714,
-            "heading": -0.8357131785361996,
-            "angularVelocity": -0.000005085507627644201,
-            "velocityX": 2.8787897289658386,
-            "velocityY": -2.45422028493365,
-            "timestamp": 0.5793984119293853
-        },
-        {
-            "x": 3.4210553878779164,
-            "y": 5.764092343000596,
-            "heading": -0.8357134464026715,
-            "angularVelocity": -0.000005085500977911915,
-            "velocityX": 2.8787907456989505,
-            "velocityY": -2.4542190923119205,
-            "timestamp": 0.6320709948320568
-        },
-        {
-            "x": 3.572688732091381,
-            "y": 5.634822284400852,
-            "heading": -0.8357137142690544,
-            "angularVelocity": -0.000005085499289329294,
-            "velocityX": 2.878790745721617,
-            "velocityY": -2.4542190922857987,
-            "timestamp": 0.6847435777347282
-        },
-        {
-            "x": 3.7243220763038787,
-            "y": 5.50555222579995,
-            "heading": -0.8357139821353484,
-            "angularVelocity": -0.000005085497600843703,
-            "velocityX": 2.8787907457032484,
-            "velocityY": -2.4542190923078118,
-            "timestamp": 0.7374161606373996
-        },
-        {
-            "x": 3.875955376681643,
-            "y": 5.376282115781124,
-            "heading": -0.8357142500017658,
-            "angularVelocity": -0.000005085499943938472,
-            "velocityX": 2.878789913491687,
-            "velocityY": -2.4542200684878277,
-            "timestamp": 0.790088743540071
-        },
-        {
-            "x": 4.025639474822054,
-            "y": 5.244759916762783,
-            "heading": -0.8357145286285158,
-            "angularVelocity": -0.000005289787108985714,
-            "velocityX": 2.8417839014463553,
-            "velocityY": -2.4969764490450044,
-            "timestamp": 0.8427613264427424
-        },
-        {
-            "x": 4.159073025842643,
-            "y": 5.096777164207917,
-            "heading": -0.835714908739186,
-            "angularVelocity": -0.000007216480551859566,
-            "velocityX": 2.5332638664625207,
-            "velocityY": -2.809483499761346,
-            "timestamp": 0.8954339093454138
-        },
-        {
-            "x": 4.274456024169922,
-            "y": 4.934326648712158,
-            "heading": -0.8357152799414105,
-            "angularVelocity": -0.000007047351849068254,
-            "velocityX": 2.19057034929318,
-            "velocityY": -3.0841570043363045,
-            "timestamp": 0.9481064922480852
-        },
-        {
-            "x": 4.3556906912761315,
-            "y": 4.793345794539918,
-            "heading": -0.8357152378475459,
-            "angularVelocity": 9.786625189030426e-7,
-            "velocityX": 1.8886677315079998,
-            "velocityY": -3.2777384277013035,
-            "timestamp": 0.9911181166598327
-        },
-        {
-            "x": 4.442446525481621,
-            "y": 4.655693706781043,
-            "heading": -0.8357151935315337,
-            "angularVelocity": 0.0000010303263992905167,
-            "velocityX": 2.0170322649286843,
-            "velocityY": -3.200346177143663,
-            "timestamp": 1.0341297410715804
-        },
-        {
-            "x": 4.541847641111602,
-            "y": 4.5268757636785075,
-            "heading": -0.8357148699573956,
-            "angularVelocity": 0.000007522946236810339,
-            "velocityX": 2.311029099445763,
-            "velocityY": -2.9949564766345187,
-            "timestamp": 1.077141365483328
-        },
-        {
-            "x": 4.65067034095477,
-            "y": 4.409944405402937,
-            "heading": -0.8255512517653536,
-            "angularVelocity": 0.23629933375095027,
-            "velocityX": 2.530076492843306,
-            "velocityY": -2.7185989805033612,
-            "timestamp": 1.1201529898950755
-        },
-        {
-            "x": 4.7668457664948924,
-            "y": 4.306168868140065,
-            "heading": -0.803496324956771,
-            "angularVelocity": 0.5127666557638517,
-            "velocityX": 2.701023900608423,
-            "velocityY": -2.4127323411326107,
-            "timestamp": 1.163164614306823
-        },
-        {
-            "x": 4.88912359332006,
-            "y": 4.216234980411927,
-            "heading": -0.7705722789003837,
-            "angularVelocity": 0.7654685566210554,
-            "velocityX": 2.8429018549638743,
-            "velocityY": -2.0909205117948377,
-            "timestamp": 1.2061762387185706
-        },
-        {
-            "x": 5.016560626655114,
-            "y": 4.140615450479534,
-            "heading": -0.7285529163528696,
-            "angularVelocity": 0.9769303792218019,
-            "velocityX": 2.9628509752411754,
-            "velocityY": -1.7581184381341082,
-            "timestamp": 1.2491878631303182
-        },
-        {
-            "x": 5.148349456504816,
-            "y": 4.079665503809577,
-            "heading": -0.6795461936491729,
-            "angularVelocity": 1.1393832103284065,
-            "velocityX": 3.0640281935900804,
-            "velocityY": -1.4170575397591803,
-            "timestamp": 1.2921994875420657
-        },
-        {
-            "x": 5.28369745675312,
-            "y": 4.033673765712218,
-            "heading": -0.6261187752553977,
-            "angularVelocity": 1.2421623020399764,
-            "velocityX": 3.1467772282354725,
-            "velocityY": -1.0692862389265216,
-            "timestamp": 1.3352111119538133
-        },
-        {
-            "x": 5.421794981888129,
-            "y": 4.0028549700404525,
-            "heading": -0.5712230670658915,
-            "angularVelocity": 1.2762993479156466,
-            "velocityX": 3.2107023862435504,
-            "velocityY": -0.7165224771968601,
-            "timestamp": 1.3782227363655608
-        },
-        {
-            "x": 5.5620425748748135,
-            "y": 3.9873099729843906,
-            "heading": -0.5170015662419462,
-            "angularVelocity": 1.2606243443606422,
-            "velocityX": 3.260690450658235,
-            "velocityY": -0.3614138565716755,
-            "timestamp": 1.4212343607773084
-        },
-        {
-            "x": 5.703982353210449,
-            "y": 3.987083911895752,
-            "heading": -0.46507695218435896,
-            "angularVelocity": 1.207222809362328,
-            "velocityX": 3.3000329626441363,
-            "velocityY": -0.005255813788261737,
-            "timestamp": 1.464245985189056
-        },
-        {
-            "x": 5.957415351644791,
-            "y": 3.94389686592464,
-            "heading": -0.40627705964587574,
-            "angularVelocity": 0.8004851872173009,
-            "velocityX": 3.450165509502976,
-            "velocityY": -0.5879362884366123,
-            "timestamp": 1.5377013013865826
-        },
-        {
-            "x": 6.214659603999478,
-            "y": 3.857155103461619,
-            "heading": -0.3855287596647175,
-            "angularVelocity": 0.2824615161326723,
-            "velocityX": 3.502050847660077,
-            "velocityY": -1.180877939858924,
-            "timestamp": 1.6111566175841092
-        },
-        {
-            "x": 6.460339332530576,
-            "y": 3.727320070546642,
-            "heading": -0.38552868222223635,
-            "angularVelocity": 0.0000010542801416145007,
-            "velocityX": 3.3446146752734305,
-            "velocityY": -1.7675375947717866,
-            "timestamp": 1.6846119337816359
-        },
-        {
-            "x": 6.681847040612612,
-            "y": 3.5595404285567858,
-            "heading": -0.3855286464284339,
-            "angularVelocity": 4.872867523834452e-7,
-            "velocityX": 3.0155435923301614,
-            "velocityY": -2.2841048228379357,
-            "timestamp": 1.7580672499791625
-        },
-        {
-            "x": 6.873376982747171,
-            "y": 3.358214932798401,
-            "heading": -0.3855286139444744,
-            "angularVelocity": 4.422274806376815e-7,
-            "velocityX": 2.607434724254966,
-            "velocityY": -2.740788634235891,
-            "timestamp": 1.8315225661766892
-        },
-        {
-            "x": 7.058369006072979,
-            "y": 3.1508658904491242,
-            "heading": -0.3855285817822777,
-            "angularVelocity": 4.3784709334515964e-7,
-            "velocityX": 2.518429337753462,
-            "velocityY": -2.8227915021385286,
-            "timestamp": 1.9049778823742158
-        },
-        {
-            "x": 7.235640342207717,
-            "y": 2.9543486280986375,
-            "heading": -0.3517392740895451,
-            "angularVelocity": 0.4599981245995971,
-            "velocityX": 2.413322075396734,
-            "velocityY": -2.6753306979448217,
-            "timestamp": 1.9784331985717425
-        },
-        {
-            "x": 7.3842205894533075,
-            "y": 2.7881464841241645,
-            "heading": -0.2579536451103976,
-            "angularVelocity": 1.2767711560447332,
-            "velocityX": 2.0227296666458727,
-            "velocityY": -2.2626292088586646,
-            "timestamp": 2.051888514769269
-        },
-        {
-            "x": 7.502781206912582,
-            "y": 2.6554541566341396,
-            "heading": -0.1644602811973769,
-            "angularVelocity": 1.2727923416953275,
-            "velocityX": 1.6140508760517314,
-            "velocityY": -1.806435998903144,
-            "timestamp": 2.125343830966796
-        },
-        {
-            "x": 7.591494122264987,
-            "y": 2.5561305844890163,
-            "heading": -0.0867965293639426,
-            "angularVelocity": 1.0572924582421084,
-            "velocityX": 1.2077126604948574,
-            "velocityY": -1.3521631555983635,
-            "timestamp": 2.1987991471643227
-        },
-        {
-            "x": 7.6505265885867635,
-            "y": 2.4900164938031613,
-            "heading": -0.031723308715129774,
-            "angularVelocity": 0.7497513250193758,
-            "velocityX": 0.8036513812429008,
-            "velocityY": -0.900058622143418,
-            "timestamp": 2.2722544633618496
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -0.003,
-            "angularVelocity": 0.39103104039319253,
-            "velocityX": 0.401242727401519,
-            "velocityY": -0.4494772538229548,
-            "timestamp": 2.3457097795593764
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -0.003,
-            "angularVelocity": -1.5819357203294863e-32,
-            "velocityX": 5.378736393413154e-34,
-            "velocityY": 0,
-            "timestamp": 2.4191650957569033
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.1.traj
deleted file mode 100644
index 95620590..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.1.traj	
+++ /dev/null
@@ -1,616 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 4.884595084783414e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 2.298507420370097,
-            "y": 6.738728491162364,
-            "heading": 0.38429180377773414,
-            "angularVelocity": -0.12150700609071861,
-            "velocityX": -0.031771662206367984,
-            "velocityY": -0.3889348314553929,
-            "timestamp": 0.04697832994094238
-        },
-        {
-            "x": 2.295523042319471,
-            "y": 6.702182630298916,
-            "heading": 0.3729896107436911,
-            "angularVelocity": -0.2405831166891495,
-            "velocityX": -0.06352669527371438,
-            "velocityY": -0.7779301841804652,
-            "timestamp": 0.09395665988188476
-        },
-        {
-            "x": 2.2910478858134478,
-            "y": 6.647358841243747,
-            "heading": 0.35624034482133005,
-            "angularVelocity": -0.3565317443897418,
-            "velocityX": -0.09526001694076108,
-            "velocityY": -1.1670016606398963,
-            "timestamp": 0.14093498982282715
-        },
-        {
-            "x": 2.285083332518943,
-            "y": 6.574252474793301,
-            "heading": 0.33424119989038353,
-            "angularVelocity": -0.46828282228427764,
-            "velocityX": -0.12696392787914923,
-            "velocityY": -1.556172101953168,
-            "timestamp": 0.18791331976376952
-        },
-        {
-            "x": 2.2776313636635503,
-            "y": 6.48285724279221,
-            "heading": 0.3072714314404645,
-            "angularVelocity": -0.5740895532860236,
-            "velocityX": -0.15862566559434335,
-            "velocityY": -1.9454763955207768,
-            "timestamp": 0.23489164970471188
-        },
-        {
-            "x": 2.268695072443392,
-            "y": 6.373164221156064,
-            "heading": 0.2757564747192681,
-            "angularVelocity": -0.6708402951065854,
-            "velocityX": -0.1902215602681591,
-            "velocityY": -2.334970650809517,
-            "timestamp": 0.28186997964565425
-        },
-        {
-            "x": 2.2582799597491072,
-            "y": 6.245160021300657,
-            "heading": 0.24041955506127732,
-            "angularVelocity": -0.752196165815466,
-            "velocityX": -0.22170036072754723,
-            "velocityY": -2.7247499009931837,
-            "timestamp": 0.3288483095865966
-        },
-        {
-            "x": 2.2463982479830333,
-            "y": 6.098824071496616,
-            "heading": 0.20274918029785605,
-            "angularVelocity": -0.8018670482917891,
-            "velocityX": -0.25291899011757896,
-            "velocityY": -3.114967049446051,
-            "timestamp": 0.375826639527539
-        },
-        {
-            "x": 2.2330953714575887,
-            "y": 5.934154199517091,
-            "heading": 0.1675471140087416,
-            "angularVelocity": -0.7493256216934009,
-            "velocityX": -0.28317048609790474,
-            "velocityY": -3.505230436810659,
-            "timestamp": 0.42280496946848134
-        },
-        {
-            "x": 2.218530566609672,
-            "y": 5.757035827571947,
-            "heading": 0.16754710135001605,
-            "angularVelocity": -2.6945882371217095e-7,
-            "velocityX": -0.3100324099691601,
-            "velocityY": -3.770214313020578,
-            "timestamp": 0.4697832994094237
-        },
-        {
-            "x": 2.203965673260931,
-            "y": 5.579917462904045,
-            "heading": 0.16754708869211127,
-            "angularVelocity": -2.694413527531555e-7,
-            "velocityX": -0.3100342938339967,
-            "velocityY": -3.7702141581142206,
-            "timestamp": 0.5167616293503661
-        },
-        {
-            "x": 2.1894007799094983,
-            "y": 5.402799098236364,
-            "heading": 0.16754707603420654,
-            "angularVelocity": -2.694413521606316e-7,
-            "velocityX": -0.31003429389130516,
-            "velocityY": -3.7702141581095083,
-            "timestamp": 0.5637399592913085
-        },
-        {
-            "x": 2.1748358865580655,
-            "y": 5.225680733568683,
-            "heading": 0.1675470633763018,
-            "angularVelocity": -2.6944135250249435e-7,
-            "velocityX": -0.3100342938913069,
-            "velocityY": -3.770214158109508,
-            "timestamp": 0.6107182892322509
-        },
-        {
-            "x": 2.160270993206632,
-            "y": 5.048562368901002,
-            "heading": 0.16754705071839698,
-            "angularVelocity": -2.694413536359467e-7,
-            "velocityX": -0.31003429389130693,
-            "velocityY": -3.7702141581095083,
-            "timestamp": 0.6576966191731933
-        },
-        {
-            "x": 2.1457060998551993,
-            "y": 4.871444004233322,
-            "heading": 0.1675470380604922,
-            "angularVelocity": -2.6944135247689087e-7,
-            "velocityX": -0.3100342938913068,
-            "velocityY": -3.7702141581095083,
-            "timestamp": 0.7046749491141358
-        },
-        {
-            "x": 2.131141206503766,
-            "y": 4.6943256395656405,
-            "heading": 0.16754702540258745,
-            "angularVelocity": -2.694413528693067e-7,
-            "velocityX": -0.31003429389130693,
-            "velocityY": -3.7702141581095083,
-            "timestamp": 0.7516532790550782
-        },
-        {
-            "x": 2.1165763131523327,
-            "y": 4.51720727489796,
-            "heading": 0.16754701274468267,
-            "angularVelocity": -2.694413529636153e-7,
-            "velocityX": -0.31003429389130693,
-            "velocityY": -3.7702141581095083,
-            "timestamp": 0.7986316089960206
-        },
-        {
-            "x": 2.1020114198009,
-            "y": 4.340088910230279,
-            "heading": 0.16754700008677786,
-            "angularVelocity": -2.694413530501801e-7,
-            "velocityX": -0.3100342938913069,
-            "velocityY": -3.770214158109508,
-            "timestamp": 0.845609938936963
-        },
-        {
-            "x": 2.0874465264494666,
-            "y": 4.162970545562599,
-            "heading": 0.16754698742887306,
-            "angularVelocity": -2.694413534833812e-7,
-            "velocityX": -0.31003429389130704,
-            "velocityY": -3.7702141581095083,
-            "timestamp": 0.8925882688779054
-        },
-        {
-            "x": 2.0728816330980333,
-            "y": 3.9858521808949186,
-            "heading": 0.16754697477096828,
-            "angularVelocity": -2.6944135303696276e-7,
-            "velocityX": -0.31003429389130693,
-            "velocityY": -3.770214158109508,
-            "timestamp": 0.9395665988188479
-        },
-        {
-            "x": 2.0583167397466005,
-            "y": 3.8087338162272375,
-            "heading": 0.16754696211306347,
-            "angularVelocity": -2.694413537071192e-7,
-            "velocityX": -0.31003429389130704,
-            "velocityY": -3.7702141581095083,
-            "timestamp": 0.9865449287597903
-        },
-        {
-            "x": 2.043751846395167,
-            "y": 3.631615451559557,
-            "heading": 0.1675469494551587,
-            "angularVelocity": -2.694413527332354e-7,
-            "velocityX": -0.31003429389130716,
-            "velocityY": -3.770214158109508,
-            "timestamp": 1.0335232587007326
-        },
-        {
-            "x": 2.0291869530437365,
-            "y": 3.4544970868918763,
-            "heading": 0.16754693679725388,
-            "angularVelocity": -2.694413532802898e-7,
-            "velocityX": -0.31003429389125564,
-            "velocityY": -3.7702141581095123,
-            "timestamp": 1.080501588641675
-        },
-        {
-            "x": 2.0146220597715567,
-            "y": 3.2773787222176782,
-            "heading": 0.1675469241393491,
-            "angularVelocity": -2.6944135307750017e-7,
-            "velocityX": -0.31003429220428774,
-            "velocityY": -3.7702141582482356,
-            "timestamp": 1.1274799185826172
-        },
-        {
-            "x": 2.0000597716588953,
-            "y": 3.1002601433334096,
-            "heading": 0.16754691148142445,
-            "angularVelocity": -2.6944177624221033e-7,
-            "velocityX": -0.30997883771037654,
-            "velocityY": -3.770218718011654,
-            "timestamp": 1.1744582485235595
-        },
-        {
-            "x": 2.001608158601577,
-            "y": 2.922550679334096,
-            "heading": 0.16754689857982888,
-            "angularVelocity": -2.7462865480192305e-7,
-            "velocityX": 0.032959599556405594,
-            "velocityY": -3.7827965409310345,
-            "timestamp": 1.2214365784645018
-        },
-        {
-            "x": 2.021524410839774,
-            "y": 2.7459539818451146,
-            "heading": 0.1675468848578902,
-            "angularVelocity": -2.9209081516381327e-7,
-            "velocityX": 0.4239455140962882,
-            "velocityY": -3.7591097365739743,
-            "timestamp": 1.2684149084054441
-        },
-        {
-            "x": 2.0595952756969864,
-            "y": 2.572363489269638,
-            "heading": 0.16754686955661854,
-            "angularVelocity": -3.257091448717022e-7,
-            "velocityX": 0.8103920446953395,
-            "velocityY": -3.6951184257444134,
-            "timestamp": 1.3153932383463864
-        },
-        {
-            "x": 2.1154126290237643,
-            "y": 2.4036404273247642,
-            "heading": 0.16754685154625998,
-            "angularVelocity": -3.8337587986161977e-7,
-            "velocityX": 1.1881510772508834,
-            "velocityY": -3.5915083008906312,
-            "timestamp": 1.3623715682873287
-        },
-        {
-            "x": 2.1883780268761943,
-            "y": 2.2415938375833533,
-            "heading": 0.1675468289010436,
-            "angularVelocity": -4.82035363826859e-7,
-            "velocityX": 1.5531713865554018,
-            "velocityY": -3.449390175111029,
-            "timestamp": 1.409349898228271
-        },
-        {
-            "x": 2.2777091497247044,
-            "y": 2.0879611790897656,
-            "heading": 0.16754679784052873,
-            "angularVelocity": -6.611668595012802e-7,
-            "velocityX": 1.9015389214731706,
-            "velocityY": -3.270287783467881,
-            "timestamp": 1.4563282281692134
-        },
-        {
-            "x": 2.382448196411133,
-            "y": 1.9443897008895872,
-            "heading": 0.16754674836915184,
-            "angularVelocity": -0.0000010530680204285256,
-            "velocityX": 2.2295183080815923,
-            "velocityY": -3.0561213729961265,
-            "timestamp": 1.5033065581101557
-        },
-        {
-            "x": 2.5069799221636964,
-            "y": 1.8074962230648766,
-            "heading": 0.1675467031876573,
-            "angularVelocity": -9.235757728412815e-7,
-            "velocityX": 2.545610454081449,
-            "velocityY": -2.798302730812182,
-            "timestamp": 1.55222673973217
-        },
-        {
-            "x": 2.645527049823341,
-            "y": 1.6848068830394283,
-            "heading": 0.16754667440242366,
-            "angularVelocity": -5.884122397185895e-7,
-            "velocityX": 2.8321057499364835,
-            "velocityY": -2.5079493975189484,
-            "timestamp": 1.6011469213541845
-        },
-        {
-            "x": 2.7964787325047507,
-            "y": 1.577748144471377,
-            "heading": 0.1675466534117406,
-            "angularVelocity": -4.2908023560369677e-7,
-            "velocityX": 3.0856729815058617,
-            "velocityY": -2.1884370625450487,
-            "timestamp": 1.6500671029761989
-        },
-        {
-            "x": 2.958079900807193,
-            "y": 1.4875647290195357,
-            "heading": 0.1675466366326211,
-            "angularVelocity": -3.429897224036844e-7,
-            "velocityX": 3.303364029820377,
-            "velocityY": -1.8434807979383812,
-            "timestamp": 1.6989872845982132
-        },
-        {
-            "x": 3.1284516660647324,
-            "y": 1.4153051353233332,
-            "heading": 0.16754662225394246,
-            "angularVelocity": -2.9392120351406106e-7,
-            "velocityX": 3.4826478481607013,
-            "velocityY": -1.4770916889581902,
-            "timestamp": 1.7479074662202276
-        },
-        {
-            "x": 3.3056131601804433,
-            "y": 1.3618094062641966,
-            "heading": 0.16754660920712522,
-            "angularVelocity": -2.666960100615425e-7,
-            "velocityX": 3.6214398279336546,
-            "velocityY": -1.0935308759169382,
-            "timestamp": 1.796827647842242
-        },
-        {
-            "x": 3.487504529935422,
-            "y": 1.3276990268102373,
-            "heading": 0.16754659675699374,
-            "angularVelocity": -2.544988811687778e-7,
-            "velocityX": 3.7181253978240782,
-            "velocityY": -0.6972660019440657,
-            "timestamp": 1.8457478294642564
-        },
-        {
-            "x": 3.671043389503124,
-            "y": 1.304003553111977,
-            "heading": 0.16754658441216208,
-            "angularVelocity": -2.5234639867690264e-7,
-            "velocityX": 3.751802497092686,
-            "velocityY": -0.48437010887133,
-            "timestamp": 1.8946680110862708
-        },
-        {
-            "x": 3.8545823032662914,
-            "y": 1.2803084992011935,
-            "heading": 0.16754657206733162,
-            "angularVelocity": -2.523463739841797e-7,
-            "velocityX": 3.751803604927206,
-            "velocityY": -0.4843615278018566,
-            "timestamp": 1.9435881927082852
-        },
-        {
-            "x": 4.038121217030901,
-            "y": 1.2566134453015836,
-            "heading": 0.16754655972250113,
-            "angularVelocity": -2.5234637491728997e-7,
-            "velocityX": 3.7518036049566934,
-            "velocityY": -0.48436152757345374,
-            "timestamp": 1.9925083743302996
-        },
-        {
-            "x": 4.221660130795511,
-            "y": 1.232918391401974,
-            "heading": 0.16754654737767066,
-            "angularVelocity": -2.5234637402611237e-7,
-            "velocityX": 3.7518036049566943,
-            "velocityY": -0.4843615275734478,
-            "timestamp": 2.041428555952314
-        },
-        {
-            "x": 4.4051990445601215,
-            "y": 1.2092233375023644,
-            "heading": 0.16754653503284023,
-            "angularVelocity": -2.5234637391390173e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.4843615275734475,
-            "timestamp": 2.090348737574329
-        },
-        {
-            "x": 4.588737958324731,
-            "y": 1.1855282836027548,
-            "heading": 0.16754652268800974,
-            "angularVelocity": -2.523463747404588e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.4843615275734478,
-            "timestamp": 2.1392689191963434
-        },
-        {
-            "x": 4.77227687208934,
-            "y": 1.1618332297031453,
-            "heading": 0.16754651034317924,
-            "angularVelocity": -2.523463747849071e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.48436152757344797,
-            "timestamp": 2.188189100818358
-        },
-        {
-            "x": 4.95581578585395,
-            "y": 1.1381381758035356,
-            "heading": 0.16754649799834875,
-            "angularVelocity": -2.52346374599026e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.4843615275734478,
-            "timestamp": 2.2371092824403727
-        },
-        {
-            "x": 5.1393546996185595,
-            "y": 1.114443121903926,
-            "heading": 0.1675464856535183,
-            "angularVelocity": -2.523463743979587e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.48436152757344764,
-            "timestamp": 2.2860294640623873
-        },
-        {
-            "x": 5.32289361338317,
-            "y": 1.0907480680043165,
-            "heading": 0.16754647330868783,
-            "angularVelocity": -2.523463747332428e-7,
-            "velocityX": 3.7518036049566943,
-            "velocityY": -0.48436152757344797,
-            "timestamp": 2.334949645684402
-        },
-        {
-            "x": 5.506432527147779,
-            "y": 1.0670530141047068,
-            "heading": 0.1675464609638573,
-            "angularVelocity": -2.5234637523589347e-7,
-            "velocityX": 3.7518036049566943,
-            "velocityY": -0.48436152757344764,
-            "timestamp": 2.3838698273064165
-        },
-        {
-            "x": 5.689971440912389,
-            "y": 1.043357960205097,
-            "heading": 0.16754644861902682,
-            "angularVelocity": -2.523463745889681e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.4843615275734477,
-            "timestamp": 2.432790008928431
-        },
-        {
-            "x": 5.8735103546769984,
-            "y": 1.0196629063054876,
-            "heading": 0.16754643627419633,
-            "angularVelocity": -2.5234637515727394e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.4843615275734478,
-            "timestamp": 2.4817101905504457
-        },
-        {
-            "x": 6.057049268441609,
-            "y": 0.995967852405878,
-            "heading": 0.16754642392936583,
-            "angularVelocity": -2.523463750187176e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.4843615275734476,
-            "timestamp": 2.5306303721724603
-        },
-        {
-            "x": 6.240588182206218,
-            "y": 0.9722727985062686,
-            "heading": 0.16754641158453532,
-            "angularVelocity": -2.523463753175626e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.48436152757344764,
-            "timestamp": 2.579550553794475
-        },
-        {
-            "x": 6.424127095970827,
-            "y": 0.948577744606659,
-            "heading": 0.1675463992397048,
-            "angularVelocity": -2.5234637555305e-7,
-            "velocityX": 3.7518036049566934,
-            "velocityY": -0.4843615275734476,
-            "timestamp": 2.6284707354164896
-        },
-        {
-            "x": 6.607666009735459,
-            "y": 0.9248826907072107,
-            "heading": 0.1675463868948743,
-            "angularVelocity": -2.5234637524939476e-7,
-            "velocityX": 3.7518036049571197,
-            "velocityY": -0.48436152757015266,
-            "timestamp": 2.677390917038504
-        },
-        {
-            "x": 6.79120492428193,
-            "y": 0.9011876428641385,
-            "heading": 0.16754637454996243,
-            "angularVelocity": -2.523480385625403e-7,
-            "velocityX": 3.7518036209390857,
-            "velocityY": -0.4843614037689742,
-            "timestamp": 2.726311098660519
-        },
-        {
-            "x": 6.9688913787459,
-            "y": 0.8784350451744124,
-            "heading": 0.1510406339197301,
-            "angularVelocity": -0.33740145851798203,
-            "velocityX": 3.6321707845828914,
-            "velocityY": -0.4650963454209109,
-            "timestamp": 2.7752312802825334
-        },
-        {
-            "x": 7.1269915101358645,
-            "y": 0.8581344466937117,
-            "heading": 0.12402265057850995,
-            "angularVelocity": -0.5522870611964742,
-            "velocityX": 3.2317977192222487,
-            "velocityY": -0.4149738984526803,
-            "timestamp": 2.824151461904548
-        },
-        {
-            "x": 7.265299167873697,
-            "y": 0.8403581253912503,
-            "heading": 0.0960275261585182,
-            "angularVelocity": -0.5722612527545016,
-            "velocityX": 2.8272106347943935,
-            "velocityY": -0.3633739841730647,
-            "timestamp": 2.8730716435265626
-        },
-        {
-            "x": 7.383819928892336,
-            "y": 0.8251162922066029,
-            "heading": 0.06979646830524168,
-            "angularVelocity": -0.5362011542792869,
-            "velocityX": 2.4227375510254454,
-            "velocityY": -0.3115653433671744,
-            "timestamp": 2.9219918251485772
-        },
-        {
-            "x": 7.482566860925546,
-            "y": 0.8124124441531555,
-            "heading": 0.04664460944238867,
-            "angularVelocity": -0.47325782724474763,
-            "velocityX": 2.01853159083065,
-            "velocityY": -0.2596852184974389,
-            "timestamp": 2.970912006770592
-        },
-        {
-            "x": 7.5615507925764724,
-            "y": 0.8022481812485672,
-            "heading": 0.027346506217350703,
-            "angularVelocity": -0.3944814304686405,
-            "velocityX": 1.6145469831081642,
-            "velocityY": -0.20777238692863387,
-            "timestamp": 3.0198321883926065
-        },
-        {
-            "x": 7.620780133649161,
-            "y": 0.7946243691967044,
-            "heading": 0.012413272398069258,
-            "angularVelocity": -0.3052571213791523,
-            "velocityX": 1.2107342840696753,
-            "velocityY": -0.15584185910773468,
-            "timestamp": 3.068752370014621
-        },
-        {
-            "x": 7.660261458521305,
-            "y": 0.7895415301241143,
-            "heading": 0.002207571288741933,
-            "angularVelocity": -0.2086194443876447,
-            "velocityX": 0.8070559749184711,
-            "velocityY": -0.10390065825721975,
-            "timestamp": 3.1176725516366357
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -0.003,
-            "angularVelocity": -0.10645036702804238,
-            "velocityX": 0.40348463199107626,
-            "velocityY": -0.05195258970523994,
-            "timestamp": 3.1665927332586503
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -0.003,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -1.0902826040652685e-32,
-            "timestamp": 3.215512914880665
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.1.traj
deleted file mode 100644
index 6d3241e6..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": -4.966859894029903e-31,
-            "velocityX": 7.6981862107013014e-34,
-            "velocityY": 1.676179535721191e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3730209621579745,
-            "y": 6.753775517490119,
-            "heading": 0.38350122654498725,
-            "angularVelocity": -0.06940058989840046,
-            "velocityX": 0.7797929692101098,
-            "velocityY": -0.03443434208804683,
-            "timestamp": 0.09364147285393071
-        },
-        {
-            "x": 2.5190628825903247,
-            "y": 6.747326552846718,
-            "heading": 0.37050348328006333,
-            "angularVelocity": -0.13880327667642306,
-            "velocityX": 1.5595858969471437,
-            "velocityY": -0.06886868015693057,
-            "timestamp": 0.18728294570786141
-        },
-        {
-            "x": 2.73812575373285,
-            "y": 6.7376531065331395,
-            "heading": 0.35100644039340695,
-            "angularVelocity": -0.2082094855243831,
-            "velocityX": 2.3393787439058484,
-            "velocityY": -0.10330301327774684,
-            "timestamp": 0.2809244185617921
-        },
-        {
-            "x": 3.0302095555912687,
-            "y": 6.7247551792664035,
-            "heading": 0.3250096622235805,
-            "angularVelocity": -0.27762034681324244,
-            "velocityX": 3.1191713773449834,
-            "velocityY": -0.13773733874150373,
-            "timestamp": 0.37456589141572283
-        },
-        {
-            "x": 3.3841048026937948,
-            "y": 6.709127593205063,
-            "heading": 0.3250096592315294,
-            "angularVelocity": -3.195216141250517e-8,
-            "velocityX": 3.779257590860006,
-            "velocityY": -0.16688744404647712,
-            "timestamp": 0.46820736426965354
-        },
-        {
-            "x": 3.7380000497922845,
-            "y": 6.693500007049851,
-            "heading": 0.3250096562396269,
-            "angularVelocity": -3.195060442591983e-8,
-            "velocityX": 3.7792575908168353,
-            "velocityY": -0.16688744504894126,
-            "timestamp": 0.5618488371235842
-        },
-        {
-            "x": 4.091895296890773,
-            "y": 6.677872420894639,
-            "heading": 0.3250096532477245,
-            "angularVelocity": -3.195060369258429e-8,
-            "velocityX": 3.7792575908168353,
-            "velocityY": -0.16688744504893968,
-            "timestamp": 0.6554903099775149
-        },
-        {
-            "x": 4.445790543993225,
-            "y": 6.6622448348316095,
-            "heading": 0.3250096502556742,
-            "angularVelocity": -3.195215253798021e-8,
-            "velocityX": 3.7792575908592094,
-            "velocityY": -0.1668874440645195,
-            "timestamp": 0.7491317828314457
-        },
-        {
-            "x": 4.737874310687331,
-            "y": 6.649346902155386,
-            "heading": 0.29900607193864137,
-            "angularVelocity": -0.27769296578215763,
-            "velocityX": 3.119171001824275,
-            "velocityY": -0.1377373965095818,
-            "timestamp": 0.8427732556853764
-        },
-        {
-            "x": 4.956937151198913,
-            "y": 6.639673451411702,
-            "heading": 0.27950309637569815,
-            "angularVelocity": -0.20827284074623165,
-            "velocityX": 2.339378416797119,
-            "velocityY": -0.10330306058697603,
-            "timestamp": 0.9364147285393072
-        },
-        {
-            "x": 5.10297904938153,
-            "y": 6.633224483848421,
-            "heading": 0.2665010284217395,
-            "angularVelocity": -0.1388494601558462,
-            "velocityX": 1.5595856593415998,
-            "velocityY": -0.06886871133840435,
-            "timestamp": 1.030056201393238
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": -0.06942467075336838,
-            "velocityX": 0.7797928459794067,
-            "velocityY": -0.03443435638235996,
-            "timestamp": 1.1236976742471687
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 2.7813300894409408e-31,
-            "velocityX": -8.985802616555554e-33,
-            "velocityY": -4.9789977648629575e-31,
-            "timestamp": 1.2173391471010995
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.1.traj
deleted file mode 100644
index 15f8fbd9..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": -5.274350592003984e-33,
-            "velocityX": 6.842220334639481e-35,
-            "velocityY": 4.754828012658472e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3477747650996332,
-            "y": 6.727866811231813,
-            "heading": 0.37690695075838176,
-            "angularVelocity": -0.1596548904259805,
-            "velocityX": 0.5825590927179829,
-            "velocityY": -0.3552462054262789,
-            "timestamp": 0.08200844463131846
-        },
-        {
-            "x": 2.4433243855136606,
-            "y": 6.669600518122756,
-            "heading": 0.35072364334029893,
-            "angularVelocity": -0.3192757469769597,
-            "velocityX": 1.16511928550267,
-            "velocityY": -0.7104913813571495,
-            "timestamp": 0.1640168892626369
-        },
-        {
-            "x": 2.5866489974259967,
-            "y": 6.582201260145896,
-            "heading": 0.3114536293241488,
-            "angularVelocity": -0.47885329605621113,
-            "velocityX": 1.7476811388958082,
-            "velocityY": -1.0657348565721152,
-            "timestamp": 0.24602533389395537
-        },
-        {
-            "x": 2.7777487646808345,
-            "y": 6.465669250143918,
-            "heading": 0.2590995246277934,
-            "angularVelocity": -0.638398946983098,
-            "velocityX": 2.330244990183096,
-            "velocityY": -1.4209757364118492,
-            "timestamp": 0.3280337785252738
-        },
-        {
-            "x": 3.0166238424465086,
-            "y": 6.320004790854863,
-            "heading": 0.19366077275903337,
-            "angularVelocity": -0.797951383701398,
-            "velocityX": 2.912810733572304,
-            "velocityY": -1.7762129247044165,
-            "timestamp": 0.4100422231565923
-        },
-        {
-            "x": 3.281496122930573,
-            "y": 6.158489372926003,
-            "heading": 0.19366076448026312,
-            "angularVelocity": -1.0095021672660313e-7,
-            "velocityX": 3.2298171447445223,
-            "velocityY": -1.9694973932864837,
-            "timestamp": 0.49205066778791073
-        },
-        {
-            "x": 3.546368403715213,
-            "y": 5.996973955490063,
-            "heading": 0.193660756201493,
-            "angularVelocity": -1.0095021513248387e-7,
-            "velocityX": 3.2298171484097047,
-            "velocityY": -1.9694973872758792,
-            "timestamp": 0.5740591124192291
-        },
-        {
-            "x": 3.785245611761769,
-            "y": 5.851315694507869,
-            "heading": 0.1281200370686441,
-            "angularVelocity": -0.7991947588751046,
-            "velocityX": 2.912836709932308,
-            "velocityY": -1.7761373433799759,
-            "timestamp": 0.6560675570505476
-        },
-        {
-            "x": 3.9763473726141734,
-            "y": 5.734789313275287,
-            "heading": 0.07567704387560718,
-            "angularVelocity": -0.6394828414171544,
-            "velocityX": 2.3302692998450527,
-            "velocityY": -1.4209070999510438,
-            "timestamp": 0.7380760016818662
-        },
-        {
-            "x": 4.119673683324307,
-            "y": 5.647394635056781,
-            "heading": 0.036339503004366135,
-            "angularVelocity": -0.47967670949118707,
-            "velocityX": 1.7477018538088223,
-            "velocityY": -1.0656790116115789,
-            "timestamp": 0.8200844463131847
-        },
-        {
-            "x": 4.2152245571841105,
-            "y": 5.589131547509505,
-            "heading": 0.010112949222819228,
-            "angularVelocity": -0.3198030873460947,
-            "velocityX": 1.1651345698527396,
-            "velocityY": -0.710452293165722,
-            "timestamp": 0.9020928909445032
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": -0.15989754813385992,
-            "velocityX": 0.5825673566993228,
-            "velocityY": -0.3552261921375451,
-            "timestamp": 0.9841013355758217
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": 2.1439897413450708e-33,
-            "velocityX": 3.9230953244452345e-34,
-            "velocityY": -1.8972025919917558e-33,
-            "timestamp": 1.0661097802071402
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.1.traj
deleted file mode 100644
index 8360b86f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.1.traj	
+++ /dev/null
@@ -1,544 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.542786541644192e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2937082670823616,
-            "y": 6.738516922494428,
-            "heading": 0.3838170687349535,
-            "angularVelocity": -0.12752172771189255,
-            "velocityX": -0.1297657401618881,
-            "velocityY": -0.3812097977411177,
-            "timestamp": 0.04848531599949176
-        },
-        {
-            "x": 2.281123433600762,
-            "y": 6.70154498788335,
-            "heading": 0.37169259737448246,
-            "angularVelocity": -0.2500648105569625,
-            "velocityX": -0.25955968775719646,
-            "velocityY": -0.7625387985814647,
-            "timestamp": 0.09697063199898352
-        },
-        {
-            "x": 2.262243832879689,
-            "y": 6.646077048114969,
-            "heading": 0.3539392998096368,
-            "angularVelocity": -0.36615823159004923,
-            "velocityX": -0.3893880102034898,
-            "velocityY": -1.1440152265685524,
-            "timestamp": 0.14545594799847528
-        },
-        {
-            "x": 2.2370673940379584,
-            "y": 6.572104077537847,
-            "heading": 0.3309787534387302,
-            "angularVelocity": -0.4735567026240549,
-            "velocityX": -0.5192590441623445,
-            "velocityY": -1.5256778068215682,
-            "timestamp": 0.19394126399796704
-        },
-        {
-            "x": 2.2055915440305824,
-            "y": 6.479614437749851,
-            "heading": 0.303410444299779,
-            "angularVelocity": -0.5685908933466691,
-            "velocityX": -0.6491831466623577,
-            "velocityY": -1.9075804268039651,
-            "timestamp": 0.2424265799974588
-        },
-        {
-            "x": 2.167813255965874,
-            "y": 6.3685929704461826,
-            "heading": 0.2721534840825349,
-            "angularVelocity": -0.6446685882563702,
-            "velocityX": -0.7791696782014628,
-            "velocityY": -2.2897956838082623,
-            "timestamp": 0.29091189599695055
-        },
-        {
-            "x": 2.123730000007902,
-            "y": 6.239021112222755,
-            "heading": 0.23878941688886735,
-            "angularVelocity": -0.6881272505944181,
-            "velocityX": -0.909208387103519,
-            "velocityY": -2.6723938073299895,
-            "timestamp": 0.3393972119964423
-        },
-        {
-            "x": 2.0733472424162183,
-            "y": 6.090890823809695,
-            "heading": 0.2066566550746183,
-            "angularVelocity": -0.6627318220196932,
-            "velocityX": -1.0391343554894152,
-            "velocityY": -3.0551577391940747,
-            "timestamp": 0.38788252799593403
-        },
-        {
-            "x": 2.0168156590767947,
-            "y": 5.924489369921669,
-            "heading": 0.187194468928521,
-            "angularVelocity": -0.4014037187247154,
-            "velocityX": -1.1659526636929207,
-            "velocityY": -3.431996893448088,
-            "timestamp": 0.4363678439954258
-        },
-        {
-            "x": 1.9576664650342146,
-            "y": 5.750871436780747,
-            "heading": 0.18719445417707173,
-            "angularVelocity": -3.042457077435804e-7,
-            "velocityX": -1.2199403638687387,
-            "velocityY": -3.580835342863344,
-            "timestamp": 0.4848531599949175
-        },
-        {
-            "x": 1.8985172819179166,
-            "y": 5.577253499917319,
-            "heading": 0.18719443942575115,
-            "angularVelocity": -3.042430533078605e-7,
-            "velocityX": -1.2199401385163373,
-            "velocityY": -3.5808354196392784,
-            "timestamp": 0.5333384759944093
-        },
-        {
-            "x": 1.8393681174712941,
-            "y": 5.403635556693396,
-            "heading": 0.18719442467442912,
-            "angularVelocity": -3.0424308271607693e-7,
-            "velocityX": -1.219939753457998,
-            "velocityY": -3.5808355508232466,
-            "timestamp": 0.5818237919939011
-        },
-        {
-            "x": 1.780218973506269,
-            "y": 5.230017606491683,
-            "heading": 0.18719440992310651,
-            "angularVelocity": -3.0424309544377536e-7,
-            "velocityX": -1.2199393310291196,
-            "velocityY": -3.580835694738771,
-            "timestamp": 0.6303091079933929
-        },
-        {
-            "x": 1.7210698517072343,
-            "y": 5.056399648738334,
-            "heading": 0.1871943951717844,
-            "angularVelocity": -3.042430844451726e-7,
-            "velocityX": -1.2199388738599835,
-            "velocityY": -3.580835850489736,
-            "timestamp": 0.6787944239928847
-        },
-        {
-            "x": 1.661920753431811,
-            "y": 4.882781682970833,
-            "heading": 0.18719438042046438,
-            "angularVelocity": -3.042430423387178e-7,
-            "velocityX": -1.2199383886901667,
-            "velocityY": -3.580836015780049,
-            "timestamp": 0.7272797399923765
-        },
-        {
-            "x": 1.6027716795006517,
-            "y": 4.709163708909599,
-            "heading": 0.18719436566914785,
-            "angularVelocity": -3.042429697472901e-7,
-            "velocityX": -1.2199378865945683,
-            "velocityY": -3.5808361868366427,
-            "timestamp": 0.7757650559918683
-        },
-        {
-            "x": 1.5436226301529286,
-            "y": 4.5355457264731545,
-            "heading": 0.18719435091783626,
-            "angularVelocity": -3.042428676797686e-7,
-            "velocityX": -1.219937379566087,
-            "velocityY": -3.5808363595737087,
-            "timestamp": 0.8242503719913601
-        },
-        {
-            "x": 1.4844769425124074,
-            "y": 4.361926598741778,
-            "heading": 0.18719433616649686,
-            "angularVelocity": -3.0424344145116126e-7,
-            "velocityX": -1.2198680450801807,
-            "velocityY": -3.58085998103429,
-            "timestamp": 0.8727356879908519
-        },
-        {
-            "x": 1.4424355744034427,
-            "y": 4.183392744566331,
-            "heading": 0.18719432108286724,
-            "angularVelocity": -3.110968611299024e-7,
-            "velocityX": -0.8670948562865489,
-            "velocityY": -3.682225236552195,
-            "timestamp": 0.9212210039903437
-        },
-        {
-            "x": 1.4196862795998648,
-            "y": 4.0013919676043574,
-            "heading": 0.18719430492048,
-            "angularVelocity": -3.3334602243497606e-7,
-            "velocityX": -0.4691996810625877,
-            "velocityY": -3.753729829541186,
-            "timestamp": 0.9697063199898355
-        },
-        {
-            "x": 1.4164891295727136,
-            "y": 3.81800278977475,
-            "heading": 0.18719428668091828,
-            "angularVelocity": -3.761873328628986e-7,
-            "velocityX": -0.06594058347012394,
-            "velocityY": -3.7823653213178425,
-            "timestamp": 1.0181916359893273
-        },
-        {
-            "x": 1.432880701620732,
-            "y": 3.635319656493888,
-            "heading": 0.1871942648241357,
-            "angularVelocity": -4.507917942103899e-7,
-            "velocityX": 0.3380729136345315,
-            "velocityY": -3.767803292913241,
-            "timestamp": 1.066676951988819
-        },
-        {
-            "x": 1.4686738067578264,
-            "y": 3.455429018696199,
-            "heading": 0.18719405074105777,
-            "angularVelocity": -0.0000044154209051021175,
-            "velocityX": 0.7382256751280081,
-            "velocityY": -3.710208628927779,
-            "timestamp": 1.1151622679883109
-        },
-        {
-            "x": 1.5224561218642167,
-            "y": 3.2829543962052377,
-            "heading": 0.17869632281657946,
-            "angularVelocity": -0.17526394846200616,
-            "velocityX": 1.10924955315069,
-            "velocityY": -3.557254788081333,
-            "timestamp": 1.1636475839878027
-        },
-        {
-            "x": 1.5921416539328836,
-            "y": 3.121386413493124,
-            "heading": 0.1576086367549348,
-            "angularVelocity": -0.43492933122045885,
-            "velocityX": 1.4372502402541338,
-            "velocityY": -3.33230751169348,
-            "timestamp": 1.2121328999872945
-        },
-        {
-            "x": 1.6761645416915945,
-            "y": 2.9727322431612,
-            "heading": 0.12423651758368265,
-            "angularVelocity": -0.6882933210462135,
-            "velocityX": 1.7329553500274961,
-            "velocityY": -3.0659626995783453,
-            "timestamp": 1.2606182159867863
-        },
-        {
-            "x": 1.7731411061925635,
-            "y": 2.838565031466778,
-            "heading": 0.08175388007293943,
-            "angularVelocity": -0.8761959499529206,
-            "velocityX": 2.0001223566816617,
-            "velocityY": -2.767172058765521,
-            "timestamp": 1.309103531986278
-        },
-        {
-            "x": 1.8819216264400211,
-            "y": 2.719965378093697,
-            "heading": 0.033593056188374194,
-            "angularVelocity": -0.9933074146707891,
-            "velocityX": 2.2435765964482535,
-            "velocityY": -2.4460942643771193,
-            "timestamp": 1.3575888479857698
-        },
-        {
-            "x": 2.0016674106146493,
-            "y": 2.617587012881804,
-            "heading": -0.0178597683247242,
-            "angularVelocity": -1.0612042729434041,
-            "velocityX": 2.4697329842357973,
-            "velocityY": -2.1115334220519952,
-            "timestamp": 1.4060741639852616
-        },
-        {
-            "x": 2.131748914718628,
-            "y": 2.531857967376709,
-            "heading": -0.07087516069292028,
-            "angularVelocity": -1.0934319242038333,
-            "velocityX": 2.682905152268552,
-            "velocityY": -1.7681445142231829,
-            "timestamp": 1.4545594799847534
-        },
-        {
-            "x": 2.218821194191698,
-            "y": 2.484029060579093,
-            "heading": -0.10483282223316899,
-            "angularVelocity": -1.096901896110445,
-            "velocityX": 2.812612651242981,
-            "velocityY": -1.5449714785171498,
-            "timestamp": 1.4855172727096597
-        },
-        {
-            "x": 2.3097767814437278,
-            "y": 2.4431840829179357,
-            "heading": -0.13859592156415457,
-            "angularVelocity": -1.0906171389530375,
-            "velocityX": 2.938051432149048,
-            "velocityY": -1.3193762883647426,
-            "timestamp": 1.516475065434566
-        },
-        {
-            "x": 2.4044569077078632,
-            "y": 2.4094074987036276,
-            "heading": -0.1718069818405491,
-            "angularVelocity": -1.0727851488087659,
-            "velocityX": 3.0583616572737307,
-            "velocityY": -1.0910527282945428,
-            "timestamp": 1.5474328581594723
-        },
-        {
-            "x": 2.5026677503931767,
-            "y": 2.3827944816766236,
-            "heading": -0.20403633414457553,
-            "angularVelocity": -1.0410739741365005,
-            "velocityX": 3.1724110164298485,
-            "velocityY": -0.859654861819398,
-            "timestamp": 1.5783906508843786
-        },
-        {
-            "x": 2.6041682782784212,
-            "y": 2.363451815109364,
-            "heading": -0.23475758891885987,
-            "angularVelocity": -0.9923593405260972,
-            "velocityX": 3.2786745743372427,
-            "velocityY": -0.6248076773199155,
-            "timestamp": 1.6093484436092849
-        },
-        {
-            "x": 2.7086523080371263,
-            "y": 2.3514976196224144,
-            "heading": -0.26331031649970843,
-            "angularVelocity": -0.9223114784903548,
-            "velocityX": 3.3750477847899405,
-            "velocityY": -0.38614495527174003,
-            "timestamp": 1.6403062363341911
-        },
-        {
-            "x": 2.8157215031828033,
-            "y": 2.3470577985799963,
-            "heading": -0.288841887845313,
-            "angularVelocity": -0.8247219551529203,
-            "velocityX": 3.458553912306755,
-            "velocityY": -0.14341529714262294,
-            "timestamp": 1.6712640290590974
-        },
-        {
-            "x": 2.9248447015312338,
-            "y": 2.3502540561946077,
-            "heading": -0.3102182612745416,
-            "angularVelocity": -0.6905005668986338,
-            "velocityX": 3.524902415281217,
-            "velocityY": 0.10324565588123534,
-            "timestamp": 1.7022218217840037
-        },
-        {
-            "x": 3.0353304312162184,
-            "y": 2.361176073397281,
-            "heading": -0.326082506948615,
-            "angularVelocity": -0.5124475706430824,
-            "velocityX": 3.568914963243171,
-            "velocityY": 0.3528034863374462,
-            "timestamp": 1.73317961450891
-        },
-        {
-            "x": 3.1466093544989446,
-            "y": 2.379927120590619,
-            "heading": -0.3369287204650216,
-            "angularVelocity": -0.35035487229182166,
-            "velocityX": 3.594536738181179,
-            "velocityY": 0.605697161937562,
-            "timestamp": 1.7641374072338163
-        },
-        {
-            "x": 3.2578769593015027,
-            "y": 2.406586601459548,
-            "heading": -0.34366338052401463,
-            "angularVelocity": -0.21754328930658381,
-            "velocityX": 3.5941711281316278,
-            "velocityY": 0.8611557389062998,
-            "timestamp": 1.7950951999587226
-        },
-        {
-            "x": 3.3679824864036325,
-            "y": 2.4411213005139327,
-            "heading": -0.34790283328354016,
-            "angularVelocity": -0.13694299193573645,
-            "velocityX": 3.556633642474382,
-            "velocityY": 1.1155413876329803,
-            "timestamp": 1.8260529926836289
-        },
-        {
-            "x": 3.475304114496225,
-            "y": 2.4831460278092354,
-            "heading": -0.3524511595124565,
-            "angularVelocity": -0.14692023650373393,
-            "velocityX": 3.466708012625811,
-            "velocityY": 1.3574846136083376,
-            "timestamp": 1.8570107854085351
-        },
-        {
-            "x": 3.5778542240444473,
-            "y": 2.531447362567012,
-            "heading": -0.3616437420636369,
-            "angularVelocity": -0.29693921116020117,
-            "velocityX": 3.312578207999158,
-            "velocityY": 1.560231867531513,
-            "timestamp": 1.8879685781334414
-        },
-        {
-            "x": 3.673558919058804,
-            "y": 2.583451056106446,
-            "heading": -0.37745622232401294,
-            "angularVelocity": -0.5107754419475534,
-            "velocityX": 3.091457322713648,
-            "velocityY": 1.6798256258655577,
-            "timestamp": 1.9189263708583477
-        },
-        {
-            "x": 3.7613706261508923,
-            "y": 2.636186152260406,
-            "heading": -0.3968957996596081,
-            "angularVelocity": -0.6279380932955558,
-            "velocityX": 2.836497675157173,
-            "velocityY": 1.7034514257095354,
-            "timestamp": 1.949884163583254
-        },
-        {
-            "x": 3.841306735175595,
-            "y": 2.6876448087218088,
-            "heading": -0.4179459699473055,
-            "angularVelocity": -0.6799635386030257,
-            "velocityX": 2.582099755467769,
-            "velocityY": 1.6622198138851267,
-            "timestamp": 1.9808419563081603
-        },
-        {
-            "x": 3.913645075614716,
-            "y": 2.736629100397619,
-            "heading": -0.43934657697326235,
-            "angularVelocity": -0.6912833617387507,
-            "velocityX": 2.336676296079001,
-            "velocityY": 1.582292772328868,
-            "timestamp": 2.0117997490330666
-        },
-        {
-            "x": 3.9786691618750996,
-            "y": 2.7823974078733573,
-            "heading": -0.46027689817629036,
-            "angularVelocity": -0.6760921681319296,
-            "velocityX": 2.100410931692797,
-            "velocityY": 1.4784099074015198,
-            "timestamp": 2.0427575417579726
-        },
-        {
-            "x": 4.036618763930541,
-            "y": 2.824460782764613,
-            "heading": -0.48017689659950147,
-            "angularVelocity": -0.6428106357887643,
-            "velocityX": 1.8718906276836884,
-            "velocityY": 1.3587330099747585,
-            "timestamp": 2.0737153344828787
-        },
-        {
-            "x": 4.08768908254176,
-            "y": 2.862477844590781,
-            "heading": -0.49864492956893264,
-            "angularVelocity": -0.5965552238942878,
-            "velocityX": 1.6496757073449355,
-            "velocityY": 1.2280288250525657,
-            "timestamp": 2.1046731272077848
-        },
-        {
-            "x": 4.132038835388808,
-            "y": 2.8961988047734253,
-            "heading": -0.5153800748468587,
-            "angularVelocity": -0.5405794084679139,
-            "velocityX": 1.4325876925792385,
-            "velocityY": 1.0892559583398733,
-            "timestamp": 2.135630919932691
-        },
-        {
-            "x": 4.169798307734006,
-            "y": 2.9254338939938918,
-            "heading": -0.5301487101337384,
-            "angularVelocity": -0.47705711511170307,
-            "velocityX": 1.2197081581622198,
-            "velocityY": 0.944353154640467,
-            "timestamp": 2.166588712657597
-        },
-        {
-            "x": 4.201075770562755,
-            "y": 2.9500345091711524,
-            "heading": -0.5427642633485259,
-            "angularVelocity": -0.4075081620800149,
-            "velocityX": 1.0103259979397883,
-            "velocityY": 0.7946501675963605,
-            "timestamp": 2.197546505382503
-        },
-        {
-            "x": 4.225962326087245,
-            "y": 2.9698813733488647,
-            "heading": -0.5530743601373866,
-            "angularVelocity": -0.3330372058810695,
-            "velocityX": 0.8038866254348108,
-            "velocityY": 0.6410942909939014,
-            "timestamp": 2.228504298107409
-        },
-        {
-            "x": 4.24453552816763,
-            "y": 2.984876771487546,
-            "heading": -0.5609523103628415,
-            "angularVelocity": -0.2544738992160421,
-            "velocityX": 0.5999524011761236,
-            "velocityY": 0.48438201883216997,
-            "timestamp": 2.259462090832315
-        },
-        {
-            "x": 4.256862100646349,
-            "y": 2.9949392682905573,
-            "heading": -0.5662912533122896,
-            "angularVelocity": -0.17245877305093363,
-            "velocityX": 0.398173493448939,
-            "velocityY": 0.32503922008988784,
-            "timestamp": 2.290419883557221
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.569,
-            "angularVelocity": -0.08749805620543083,
-            "velocityX": 0.19826669841112912,
-            "velocityY": 0.16347198117160036,
-            "timestamp": 2.321377676282127
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.569,
-            "angularVelocity": 1.3974941712649426e-29,
-            "velocityX": 2.480011116993708e-30,
-            "velocityY": -4.1979337006948246e-30,
-            "timestamp": 2.3523354690070333
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.1.traj
deleted file mode 100644
index 1ef15c59..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.1.traj	
+++ /dev/null
@@ -1,562 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 9.203343680058695e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.298008161759761,
-            "y": 6.742798044155305,
-            "heading": 0.37730350882287184,
-            "angularVelocity": -0.30162955292479776,
-            "velocityX": -0.04731994607959398,
-            "velocityY": -0.3373947598852993,
-            "timestamp": 0.0420929947149233
-        },
-        {
-            "x": 2.2940179124306126,
-            "y": 6.714368498753073,
-            "heading": 0.3522801711626307,
-            "angularVelocity": -0.5944774856175671,
-            "velocityX": -0.09479604281359731,
-            "velocityY": -0.6753984978919232,
-            "timestamp": 0.0841859894298466
-        },
-        {
-            "x": 2.28802134748838,
-            "y": 6.671681118579196,
-            "heading": 0.31538217591497186,
-            "angularVelocity": -0.8765828019021266,
-            "velocityX": -0.14245992671333493,
-            "velocityY": -1.014120769096589,
-            "timestamp": 0.1262789841447699
-        },
-        {
-            "x": 2.2800080756588827,
-            "y": 6.61469814393844,
-            "heading": 0.2672110996394858,
-            "angularVelocity": -1.1443965106718308,
-            "velocityX": -0.19037067530517543,
-            "velocityY": -1.3537400944426796,
-            "timestamp": 0.1683719788596932
-        },
-        {
-            "x": 2.2699650128963897,
-            "y": 6.543370292056319,
-            "heading": 0.20862519634351256,
-            "angularVelocity": -1.391820745773707,
-            "velocityX": -0.23859226055333851,
-            "velocityY": -1.694530226827328,
-            "timestamp": 0.21046497357461652
-        },
-        {
-            "x": 2.2578780077181495,
-            "y": 6.4576319980717,
-            "heading": 0.14091301714440063,
-            "angularVelocity": -1.608632972248606,
-            "velocityX": -0.2871500414760241,
-            "velocityY": -2.036877978516017,
-            "timestamp": 0.2525579682895398
-        },
-        {
-            "x": 2.2437361567314555,
-            "y": 6.357398182016401,
-            "heading": 0.06608849726258184,
-            "angularVelocity": -1.7776002964049293,
-            "velocityX": -0.3359668534508049,
-            "velocityY": -2.3812469683883926,
-            "timestamp": 0.2946509630044631
-        },
-        {
-            "x": 2.227539619778089,
-            "y": 6.242572554004172,
-            "heading": -0.012517898946656567,
-            "angularVelocity": -1.8674460380308828,
-            "velocityX": -0.3847798680768185,
-            "velocityY": -2.727903509595609,
-            "timestamp": 0.33674395771938637
-        },
-        {
-            "x": 2.209319968778144,
-            "y": 6.1131309245806085,
-            "heading": -0.08849823738903302,
-            "angularVelocity": -1.8050589880087344,
-            "velocityX": -0.4328428310539146,
-            "velocityY": -3.0751347177888695,
-            "timestamp": 0.37883695243430965
-        },
-        {
-            "x": 2.189363625025874,
-            "y": 5.970570114356374,
-            "heading": -0.13907359717832485,
-            "angularVelocity": -1.201514887021361,
-            "velocityX": -0.4741013056311528,
-            "velocityY": -3.38680607520881,
-            "timestamp": 0.42092994714923293
-        },
-        {
-            "x": 2.1674827409245396,
-            "y": 5.816618965425972,
-            "heading": -0.15173248317406665,
-            "angularVelocity": -0.3007361695568274,
-            "velocityX": -0.5198224609468644,
-            "velocityY": -3.657405465518488,
-            "timestamp": 0.4630229418641562
-        },
-        {
-            "x": 2.1445157307124236,
-            "y": 5.659048701686038,
-            "heading": -0.1517325271793822,
-            "angularVelocity": -0.0000010454308573871989,
-            "velocityX": -0.5456254744443967,
-            "velocityY": -3.7433844944292183,
-            "timestamp": 0.5051159365790795
-        },
-        {
-            "x": 2.1215487153797565,
-            "y": 5.501478438692377,
-            "heading": -0.1517325711844689,
-            "angularVelocity": -0.0000010454254199880688,
-            "velocityX": -0.5456255960929445,
-            "velocityY": -3.7433844767000393,
-            "timestamp": 0.5472089312940028
-        },
-        {
-            "x": 2.0985817251133763,
-            "y": 5.343908172045118,
-            "heading": -0.15173261518953962,
-            "angularVelocity": -0.000001045425040533604,
-            "velocityX": -0.5456250005951166,
-            "velocityY": -3.7433845634983034,
-            "timestamp": 0.5893019260089262
-        },
-        {
-            "x": 2.075614762728362,
-            "y": 5.186337901333946,
-            "heading": -0.15173265919459192,
-            "angularVelocity": -0.0000010454246033549915,
-            "velocityX": -0.5456243382196686,
-            "velocityY": -3.743384660044355,
-            "timestamp": 0.6313949207238495
-        },
-        {
-            "x": 2.0526478312972185,
-            "y": 5.028767626111028,
-            "heading": -0.15173270319962293,
-            "angularVelocity": -0.0000010454240970480152,
-            "velocityX": -0.5456236028509798,
-            "velocityY": -3.7433847672295513,
-            "timestamp": 0.6734879154387728
-        },
-        {
-            "x": 2.0296809339445536,
-            "y": 4.871197345920939,
-            "heading": -0.15173274720462945,
-            "angularVelocity": -0.0000010454235162635837,
-            "velocityX": -0.5456227932512114,
-            "velocityY": -3.743384885234266,
-            "timestamp": 0.7155809101536962
-        },
-        {
-            "x": 2.0067140734575584,
-            "y": 4.7136270603574335,
-            "heading": -0.15173279120960867,
-            "angularVelocity": -0.0000010454228670132247,
-            "velocityX": -0.5456219174363699,
-            "velocityY": -3.7433850128900636,
-            "timestamp": 0.7576739048686195
-        },
-        {
-            "x": 1.983747251812091,
-            "y": 4.55605676913253,
-            "heading": -0.15173283521455908,
-            "angularVelocity": -0.000001045422182833939,
-            "velocityX": -0.545620994681206,
-            "velocityY": -3.743385147387475,
-            "timestamp": 0.7997668995835429
-        },
-        {
-            "x": 1.9607804702256346,
-            "y": 4.398486472068785,
-            "heading": -0.15173287921948736,
-            "angularVelocity": -0.0000010454216570158517,
-            "velocityX": -0.5456200430024101,
-            "velocityY": -3.7433852861004024,
-            "timestamp": 0.8418598942984662
-        },
-        {
-            "x": 1.9378163633559966,
-            "y": 4.240915785172817,
-            "heading": -0.15173292322446258,
-            "angularVelocity": -0.0000010454227717672262,
-            "velocityX": -0.5455564999630786,
-            "velocityY": -3.7433945473141725,
-            "timestamp": 0.8839528890133895
-        },
-        {
-            "x": 1.9275487572437193,
-            "y": 4.082011890541997,
-            "heading": -0.15173296776197548,
-            "angularVelocity": -0.000001058074228463605,
-            "velocityX": -0.2439267194414494,
-            "velocityY": -3.775067459728259,
-            "timestamp": 0.9260458837283129
-        },
-        {
-            "x": 1.9320519221515333,
-            "y": 3.9228403083205765,
-            "heading": -0.15173301420750987,
-            "angularVelocity": -0.0000011034029462076059,
-            "velocityX": 0.10698133830372927,
-            "velocityY": -3.781426892988182,
-            "timestamp": 0.9681388784432362
-        },
-        {
-            "x": 1.9512874233679367,
-            "y": 3.764771127717043,
-            "heading": -0.15173306433513473,
-            "angularVelocity": -0.0000011908780826817618,
-            "velocityX": 0.45697630559851177,
-            "velocityY": -3.7552372235348,
-            "timestamp": 1.0102318731581594
-        },
-        {
-            "x": 1.9850897622274482,
-            "y": 3.609164986787025,
-            "heading": -0.15173312063283023,
-            "angularVelocity": -0.0000013374599716310608,
-            "velocityX": 0.8030395339756486,
-            "velocityY": -3.6967229816711127,
-            "timestamp": 1.0523248678730828
-        },
-        {
-            "x": 2.0331680016062417,
-            "y": 3.4573613296965586,
-            "heading": -0.1517331870158403,
-            "angularVelocity": -0.0000015770560044658274,
-            "velocityX": 1.142190991741159,
-            "velocityY": -3.606387668982997,
-            "timestamp": 1.094417862588006
-        },
-        {
-            "x": 2.0951083019579966,
-            "y": 3.3106668726225337,
-            "heading": -0.15173327041919607,
-            "angularVelocity": -0.0000019814070334134687,
-            "velocityX": 1.4715108956074094,
-            "velocityY": -3.4850088017619067,
-            "timestamp": 1.1365108573029294
-        },
-        {
-            "x": 2.170377492904663,
-            "y": 3.170344352722168,
-            "heading": -0.1517333779044128,
-            "angularVelocity": -0.000002553517929494838,
-            "velocityX": 1.7881643122906952,
-            "velocityY": -3.333631186155945,
-            "timestamp": 1.1786038520178528
-        },
-        {
-            "x": 2.2421653916580504,
-            "y": 3.058317386598826,
-            "heading": -0.1517334835913584,
-            "angularVelocity": -0.0000030048345176781924,
-            "velocityX": 2.041035009880172,
-            "velocityY": -3.1850905776457417,
-            "timestamp": 1.213776153524627
-        },
-        {
-            "x": 2.322415894828655,
-            "y": 2.9521882028356305,
-            "heading": -0.15173356325163515,
-            "angularVelocity": -0.000002264858236060329,
-            "velocityX": 2.2816392369190814,
-            "velocityY": -3.0174079948323826,
-            "timestamp": 1.2489484550314012
-        },
-        {
-            "x": 2.4106466853387967,
-            "y": 2.8525946456824203,
-            "heading": -0.1517336276365959,
-            "angularVelocity": -0.0000018305586497777263,
-            "velocityX": 2.5085304836576707,
-            "velocityY": -2.8315905666289245,
-            "timestamp": 1.2841207565381754
-        },
-        {
-            "x": 2.5063274836460994,
-            "y": 2.7601352747554455,
-            "heading": -0.15173368243148194,
-            "angularVelocity": -0.0000015578987915956282,
-            "velocityX": 2.720345107040385,
-            "velocityY": -2.628755212654101,
-            "timestamp": 1.3192930580449496
-        },
-        {
-            "x": 2.6088832332920924,
-            "y": 2.67536576592715,
-            "heading": -0.15173373101035148,
-            "angularVelocity": -0.000001381168347773627,
-            "velocityX": 2.915810033819959,
-            "velocityY": -2.410121180496788,
-            "timestamp": 1.3544653595517238
-        },
-        {
-            "x": 2.7176975542058304,
-            "y": 2.598795565150263,
-            "heading": -0.1517337755722331,
-            "angularVelocity": -0.0000012669595030854418,
-            "velocityX": 3.0937503732242226,
-            "velocityY": -2.1770028544233733,
-            "timestamp": 1.389637661058498
-        },
-        {
-            "x": 2.8321164386506434,
-            "y": 2.5308848045360426,
-            "heading": -0.15173381767917685,
-            "angularVelocity": -0.0000011971620259359605,
-            "velocityX": 3.2530963156555406,
-            "velocityY": -1.930802299108592,
-            "timestamp": 1.4248099625652721
-        },
-        {
-            "x": 2.951452140465328,
-            "y": 2.4720414272198683,
-            "heading": -0.15173385854226368,
-            "angularVelocity": -0.0000011617973536146762,
-            "velocityX": 3.3928886283344455,
-            "velocityY": -1.6730033235055968,
-            "timestamp": 1.4599822640720463
-        },
-        {
-            "x": 3.0749866313948626,
-            "y": 2.4226169148084327,
-            "heading": -0.151733899192453,
-            "angularVelocity": -0.000001155744366153098,
-            "velocityX": 3.512266347021429,
-            "velocityY": -1.4052112114959712,
-            "timestamp": 1.4951545655788205
-        },
-        {
-            "x": 3.1992028583958065,
-            "y": 2.3749316777596947,
-            "heading": -0.1517339398689439,
-            "angularVelocity": -0.0000011564921588237654,
-            "velocityX": 3.5316491011263524,
-            "velocityY": -1.3557610678264387,
-            "timestamp": 1.5303268670855947
-        },
-        {
-            "x": 3.3234191450837622,
-            "y": 2.3272465961911335,
-            "heading": -0.15173398054544393,
-            "angularVelocity": -0.0000011564924185481092,
-            "velocityX": 3.5316507981154315,
-            "velocityY": -1.355756647297499,
-            "timestamp": 1.565499168592369
-        },
-        {
-            "x": 3.4476354415156054,
-            "y": 2.2795615400047207,
-            "heading": -0.15173402122195195,
-            "angularVelocity": -0.0000011564926460273957,
-            "velocityX": 3.5316510751484063,
-            "velocityY": -1.3557559256458895,
-            "timestamp": 1.6006714700991431
-        },
-        {
-            "x": 3.57185174767356,
-            "y": 2.231876509154167,
-            "heading": -0.15173406189847174,
-            "angularVelocity": -0.0000011564929795008858,
-            "velocityX": 3.531651351675977,
-            "velocityY": -1.355755205310334,
-            "timestamp": 1.6358437716059173
-        },
-        {
-            "x": 3.6960680635749434,
-            "y": 2.1841915036846027,
-            "heading": -0.15173410257500647,
-            "angularVelocity": -0.000001156493404477995,
-            "velocityX": 3.5316516286959314,
-            "velocityY": -1.3557544836916666,
-            "timestamp": 1.6710160731126915
-        },
-        {
-            "x": 3.820284389246338,
-            "y": 2.136506523665285,
-            "heading": -0.15173414325155568,
-            "angularVelocity": -0.0000011564938164652249,
-            "velocityX": 3.5316519064716276,
-            "velocityY": -1.3557537601039011,
-            "timestamp": 1.7061883746194657
-        },
-        {
-            "x": 3.9445007247793735,
-            "y": 2.0888215693349235,
-            "heading": -0.15173418392811813,
-            "angularVelocity": -0.0000011564941935950735,
-            "velocityX": 3.5316521868525257,
-            "velocityY": -1.3557530297293134,
-            "timestamp": 1.74136067612624
-        },
-        {
-            "x": 4.068717070394662,
-            "y": 2.0411366412682117,
-            "heading": -0.1517342246046924,
-            "angularVelocity": -0.0000011564945294151802,
-            "velocityX": 3.531652473505735,
-            "velocityY": -1.3557522830153055,
-            "timestamp": 1.776532977633014
-        },
-        {
-            "x": 4.192933426524707,
-            "y": 1.9934517405918115,
-            "heading": -0.15173426528127656,
-            "angularVelocity": -0.0000011564948108738022,
-            "velocityX": 3.5316527724556495,
-            "velocityY": -1.3557515042686648,
-            "timestamp": 1.8117052791397883
-        },
-        {
-            "x": 4.3171498402859925,
-            "y": 1.9457669900421575,
-            "heading": -0.1517343059583456,
-            "angularVelocity": -0.0000011565085966842365,
-            "velocityX": 3.531654410996122,
-            "velocityY": -1.3557472359455842,
-            "timestamp": 1.8468775806465625
-        },
-        {
-            "x": 4.438199866372425,
-            "y": 1.9006463371040783,
-            "heading": -0.16306820061092758,
-            "angularVelocity": -0.3222392100329015,
-            "velocityX": 3.441629375976964,
-            "velocityY": -1.282846188765579,
-            "timestamp": 1.8820498821533367
-        },
-        {
-            "x": 4.551333178258229,
-            "y": 1.8581105785728176,
-            "heading": -0.19901052601489952,
-            "angularVelocity": -1.0218929061849813,
-            "velocityX": 3.2165456066050515,
-            "velocityY": -1.2093538582645278,
-            "timestamp": 1.9172221836601109
-        },
-        {
-            "x": 4.6559660625940325,
-            "y": 1.8184646011809935,
-            "heading": -0.2548690023306322,
-            "angularVelocity": -1.5881382202121255,
-            "velocityX": 2.9748660125539748,
-            "velocityY": -1.1271931518097666,
-            "timestamp": 1.952394485166885
-        },
-        {
-            "x": 4.750968691238499,
-            "y": 1.7823676048635404,
-            "heading": -0.3144227593810797,
-            "angularVelocity": -1.693200458860429,
-            "velocityX": 2.7010637511499986,
-            "velocityY": -1.0262904265875403,
-            "timestamp": 1.9875667866736593
-        },
-        {
-            "x": 4.836310937621718,
-            "y": 1.749878315803868,
-            "heading": -0.37309459063768413,
-            "angularVelocity": -1.6681260180060764,
-            "velocityX": 2.426404947278819,
-            "velocityY": -0.9237180300360781,
-            "timestamp": 2.0227390881804337
-        },
-        {
-            "x": 4.912035358987092,
-            "y": 1.7210053742684595,
-            "heading": -0.4285254423369764,
-            "angularVelocity": -1.5759802266171352,
-            "velocityX": 2.1529561081121917,
-            "velocityY": -0.8208999780650579,
-            "timestamp": 2.057911389687208
-        },
-        {
-            "x": 4.978188578205751,
-            "y": 1.6957473341798728,
-            "heading": -0.4792367761737124,
-            "angularVelocity": -1.4417974276425698,
-            "velocityX": 1.8808328253957978,
-            "velocityY": -0.71812304019178,
-            "timestamp": 2.0930836911939825
-        },
-        {
-            "x": 5.034811621143831,
-            "y": 1.6741013653395114,
-            "heading": -0.5242175587960242,
-            "angularVelocity": -1.278869471013956,
-            "velocityX": 1.6098759680874073,
-            "velocityY": -0.6154265690060781,
-            "timestamp": 2.128255992700757
-        },
-        {
-            "x": 5.081938937252143,
-            "y": 1.656065298518901,
-            "heading": -0.5627406592667319,
-            "angularVelocity": -1.0952681178195878,
-            "velocityX": 1.3398985590759445,
-            "velocityY": -0.5127917721601607,
-            "timestamp": 2.1634282942075314
-        },
-        {
-            "x": 5.11959917834615,
-            "y": 1.6416378108353973,
-            "heading": -0.5942629145071194,
-            "angularVelocity": -0.8962238434785516,
-            "velocityX": 1.0707357631047747,
-            "velocityY": -0.41019458680362747,
-            "timestamp": 2.1986005957143058
-        },
-        {
-            "x": 5.147816310703936,
-            "y": 1.63081813815951,
-            "heading": -0.6183637008514038,
-            "angularVelocity": -0.6852206228143011,
-            "velocityX": 0.8022543634897267,
-            "velocityY": -0.3076191267666608,
-            "timestamp": 2.23377289722108
-        },
-        {
-            "x": 5.1666106911988186,
-            "y": 1.6236057376407542,
-            "heading": -0.6347043261398958,
-            "angularVelocity": -0.4645878884367218,
-            "velocityX": 0.5343517395716112,
-            "velocityY": -0.20505910076332604,
-            "timestamp": 2.2689451987278546
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.643,
-            "angularVelocity": -0.23585814702817923,
-            "velocityX": 0.26695178873562564,
-            "velocityY": -0.10251639745723456,
-            "timestamp": 2.304117500234629
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.643,
-            "angularVelocity": -2.979902278450704e-33,
-            "velocityX": -7.596111021745797e-35,
-            "velocityY": -9.748244176889607e-35,
-            "timestamp": 2.3392898017414034
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.1.traj
deleted file mode 100644
index a5fd0bb0..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.1.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 1.792103156985809e-33,
-            "velocityX": -7.27073415334495e-34,
-            "velocityY": -7.853383039035825e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.298001341213542,
-            "y": 6.709118496985434,
-            "heading": 0.374359095500822,
-            "angularVelocity": -0.2058832051525312,
-            "velocityX": -0.02630859852023658,
-            "velocityY": -0.6302702831973344,
-            "timestamp": 0.07596979310473732
-        },
-        {
-            "x": 2.294004039374054,
-            "y": 6.613355753676287,
-            "heading": 0.3430620070158405,
-            "angularVelocity": -0.4119675361210102,
-            "velocityX": -0.0526169899393711,
-            "velocityY": -1.2605371081782701,
-            "timestamp": 0.15193958620947465
-        },
-        {
-            "x": 2.2880080040813597,
-            "y": 6.469712250400375,
-            "heading": 0.296082691458658,
-            "angularVelocity": -0.6183946755312791,
-            "velocityX": -0.07892657130746197,
-            "velocityY": -1.8907976105433808,
-            "timestamp": 0.22790937931421196
-        },
-        {
-            "x": 2.280012894319733,
-            "y": 6.278188663497132,
-            "heading": 0.23338596481601495,
-            "angularVelocity": -0.8252849465603376,
-            "velocityX": -0.10524064150870725,
-            "velocityY": -2.5210492101669324,
-            "timestamp": 0.3038791724189493
-        },
-        {
-            "x": 2.270017931664709,
-            "y": 6.038785816229407,
-            "heading": 0.1549313176962556,
-            "angularVelocity": -1.0327084478378958,
-            "velocityX": -0.13156495820969516,
-            "velocityY": -3.1512899730774246,
-            "timestamp": 0.37984896552368663
-        },
-        {
-            "x": 2.2620113109689415,
-            "y": 5.847271344563018,
-            "heading": 0.09176539516362914,
-            "angularVelocity": -0.8314610314331311,
-            "velocityX": -0.10539216139142096,
-            "velocityY": -2.5209292251507525,
-            "timestamp": 0.45581875862842397
-        },
-        {
-            "x": 2.2560057963370115,
-            "y": 5.703635646814395,
-            "heading": 0.0443838554377995,
-            "angularVelocity": -0.6236892031613949,
-            "velocityX": -0.079051349049357,
-            "velocityY": -1.8906948653999391,
-            "timestamp": 0.5317885517331613
-        },
-        {
-            "x": 2.252001935551058,
-            "y": 5.607878550007709,
-            "heading": 0.012794605457377246,
-            "angularVelocity": -0.4158132948561684,
-            "velocityX": -0.05270332618167983,
-            "velocityY": -1.2604627825518706,
-            "timestamp": 0.6077583448378986
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": -0.20790639031492017,
-            "velocityX": -0.026351730987313093,
-            "velocityY": -0.6302314123944018,
-            "timestamp": 0.6837281379426359
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": -6.008173298568887e-34,
-            "velocityX": -7.305982513747354e-33,
-            "velocityY": -3.8813573012664696e-35,
-            "timestamp": 0.7596979310473732
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.1.traj
deleted file mode 100644
index 5aab1ee6..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": -2.4663146867319515e-36,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3001378208847427,
-            "y": 6.713171893956039,
-            "heading": 0.36291265694481545,
-            "angularVelocity": -0.37089801282350154,
-            "velocityX": 0.0018871357066083216,
-            "velocityY": -0.6001237332286558,
-            "timestamp": 0.0730317826428328
-        },
-        {
-            "x": 2.3004082222445,
-            "y": 6.625427179140099,
-            "heading": 0.3106389081779897,
-            "angularVelocity": -0.7157671204942956,
-            "velocityX": 0.0037025162192713427,
-            "velocityY": -1.201459304985902,
-            "timestamp": 0.1460635652856656
-        },
-        {
-            "x": 2.300808308388558,
-            "y": 6.493639269598709,
-            "heading": 0.23621859479744584,
-            "angularVelocity": -1.019012691289514,
-            "velocityX": 0.005478246998500719,
-            "velocityY": -1.804528176258669,
-            "timestamp": 0.2190953479284984
-        },
-        {
-            "x": 2.30134752968001,
-            "y": 6.317621999647275,
-            "heading": 0.14515321533996162,
-            "angularVelocity": -1.2469280655909227,
-            "velocityX": 0.007383378468093947,
-            "velocityY": -2.4101461525629078,
-            "timestamp": 0.2921271305713312
-        },
-        {
-            "x": 2.3020704977057354,
-            "y": 6.097150895386178,
-            "heading": 0.04952404086318575,
-            "angularVelocity": -1.309418598536164,
-            "velocityX": 0.00989936161439861,
-            "velocityY": -3.0188377755932185,
-            "timestamp": 0.36515891321416405
-        },
-        {
-            "x": 2.3031517302727234,
-            "y": 5.834309739739576,
-            "heading": 0.004115026920641748,
-            "angularVelocity": -0.6217705812361183,
-            "velocityX": 0.014804959263771778,
-            "velocityY": -3.598996849522972,
-            "timestamp": 0.4381906958569969
-        },
-        {
-            "x": 2.303521267718626,
-            "y": 5.558035105167069,
-            "heading": 0.00411499765356137,
-            "angularVelocity": -4.007444337013033e-7,
-            "velocityX": 0.005059953797233799,
-            "velocityY": -3.7829370251531125,
-            "timestamp": 0.5112224784998297
-        },
-        {
-            "x": 2.3038907744757537,
-            "y": 5.281760470553516,
-            "heading": 0.004114968386478979,
-            "angularVelocity": -4.0074446127092794e-7,
-            "velocityX": 0.005059533586019425,
-            "velocityY": -3.782937025715139,
-            "timestamp": 0.5842542611426625
-        },
-        {
-            "x": 2.3020058670727765,
-            "y": 5.019317688425813,
-            "heading": -0.04121766182368216,
-            "angularVelocity": -0.6207246840990261,
-            "velocityX": -0.02580941248819387,
-            "velocityY": -3.5935420529332935,
-            "timestamp": 0.6572860437854954
-        },
-        {
-            "x": 2.301170882315387,
-            "y": 4.798980955675817,
-            "heading": -0.139726223563735,
-            "angularVelocity": -1.348845094221732,
-            "velocityX": -0.011433169603334988,
-            "velocityY": -3.016997871016921,
-            "timestamp": 0.7303178264283282
-        },
-        {
-            "x": 2.3006513716532107,
-            "y": 4.623098256624685,
-            "heading": -0.23378951684706242,
-            "angularVelocity": -1.2879775062228829,
-            "velocityX": -0.007113487352719777,
-            "velocityY": -2.4083035178163246,
-            "timestamp": 0.803349609071161
-        },
-        {
-            "x": 2.300310703422249,
-            "y": 4.491429967185545,
-            "heading": -0.31073488166813007,
-            "angularVelocity": -1.0535873839664371,
-            "velocityX": -0.004664657203118652,
-            "velocityY": -1.8028902578357346,
-            "timestamp": 0.8763813917139939
-        },
-        {
-            "x": 2.3001003821722423,
-            "y": 4.403776446553788,
-            "heading": -0.36487144716496317,
-            "angularVelocity": -0.7412740527174575,
-            "velocityX": -0.0028798591845282696,
-            "velocityY": -1.2002106132398922,
-            "timestamp": 0.9494131743568267
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.393,
-            "angularVelocity": -0.3851549533249317,
-            "velocityX": -0.0013744998219966374,
-            "velocityY": -0.5994163769475577,
-            "timestamp": 1.0224449569996594
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.393,
-            "angularVelocity": -7.841220088894993e-36,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 1.0954767396424923
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.1.traj
deleted file mode 100644
index 70287ec9..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.1.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -3.731399929679607e-32,
-            "velocityY": 5.877471754111438e-38,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3271275380851444,
-            "y": 5.587086964112038,
-            "heading": 3.953847862678034e-18,
-            "angularVelocity": 3.9932078666702264e-17,
-            "velocityX": 0.7789533333445218,
-            "velocityY": 0.2735661154121556,
-            "timestamp": 0.09901432445765257
-        },
-        {
-            "x": 2.4813826113143547,
-            "y": 5.641260891303218,
-            "heading": 1.1398162294527873e-19,
-            "angularVelocity": -3.8780916638560175e-17,
-            "velocityX": 1.557906636985473,
-            "velocityY": 0.5471322203925046,
-            "timestamp": 0.19802864891530514
-        },
-        {
-            "x": 2.7127652138054734,
-            "y": 5.722521779507741,
-            "heading": 2.1553394674505063e-18,
-            "angularVelocity": 2.0616792512846884e-17,
-            "velocityX": 2.336859881219295,
-            "velocityY": 0.8206983045092451,
-            "timestamp": 0.2970429733729577
-        },
-        {
-            "x": 3.0212753279120443,
-            "y": 5.830869622528225,
-            "heading": -1.0672476097054788e-17,
-            "angularVelocity": -1.295551486240031e-16,
-            "velocityX": 3.1158129472318716,
-            "velocityY": 1.0942643260352094,
-            "timestamp": 0.3960572978306103
-        },
-        {
-            "x": 3.3746798137461993,
-            "y": 5.9549842366139965,
-            "heading": 1.8895880094915182e-18,
-            "angularVelocity": 1.2687117924961395e-16,
-            "velocityX": 3.5692258445423475,
-            "velocityY": 1.2535015995473786,
-            "timestamp": 0.49507162228826285
-        },
-        {
-            "x": 3.7280842995803773,
-            "y": 6.079098850699775,
-            "heading": 1.0049319654091473e-17,
-            "angularVelocity": 8.240960778697987e-17,
-            "velocityX": 3.569225844542579,
-            "velocityY": 1.25350159954746,
-            "timestamp": 0.5940859467459154
-        },
-        {
-            "x": 4.081488785414555,
-            "y": 6.203213464785553,
-            "heading": 8.136834826577652e-18,
-            "angularVelocity": -1.931523381075779e-17,
-            "velocityX": 3.569225844542579,
-            "velocityY": 1.25350159954746,
-            "timestamp": 0.693100271203568
-        },
-        {
-            "x": 4.434893271248733,
-            "y": 6.327328078871332,
-            "heading": -2.4381204969164037e-18,
-            "angularVelocity": -1.0680227715899811e-16,
-            "velocityX": 3.5692258445425793,
-            "velocityY": 1.2535015995474599,
-            "timestamp": 0.7921145956612206
-        },
-        {
-            "x": 4.788297757082911,
-            "y": 6.4514426929571105,
-            "heading": 3.192038150785043e-17,
-            "angularVelocity": 3.47005367081625e-16,
-            "velocityX": 3.569225844542579,
-            "velocityY": 1.25350159954746,
-            "timestamp": 0.8911289201188731
-        },
-        {
-            "x": 5.141702242917089,
-            "y": 6.575557307042889,
-            "heading": 6.016026907559514e-17,
-            "angularVelocity": 2.8521012209839933e-16,
-            "velocityX": 3.569225844542579,
-            "velocityY": 1.2535015995474599,
-            "timestamp": 0.9901432445765257
-        },
-        {
-            "x": 5.495106728751266,
-            "y": 6.699671921128667,
-            "heading": 2.4186102644429564e-17,
-            "angularVelocity": -3.63322848771759e-16,
-            "velocityX": 3.569225844542579,
-            "velocityY": 1.2535015995474599,
-            "timestamp": 1.0891575690341782
-        },
-        {
-            "x": 5.848511214585444,
-            "y": 6.823786535214446,
-            "heading": -1.2421929874703125e-18,
-            "angularVelocity": -2.568143121812034e-16,
-            "velocityX": 3.5692258445425793,
-            "velocityY": 1.2535015995474599,
-            "timestamp": 1.1881718934918308
-        },
-        {
-            "x": 6.2019157004196215,
-            "y": 6.9479011493002245,
-            "heading": 2.3184212364315377e-18,
-            "angularVelocity": 3.596059704901295e-17,
-            "velocityX": 3.569225844542579,
-            "velocityY": 1.2535015995474599,
-            "timestamp": 1.2871862179494835
-        },
-        {
-            "x": 6.5553201862538,
-            "y": 7.072015763386003,
-            "heading": 3.1459219033425486e-17,
-            "angularVelocity": 2.9430890890068574e-16,
-            "velocityX": 3.569225844542579,
-            "velocityY": 1.25350159954746,
-            "timestamp": 1.3862005424071362
-        },
-        {
-            "x": 6.9087246720879545,
-            "y": 7.196130377471773,
-            "heading": 2.9741502939674576e-17,
-            "angularVelocity": -1.7348157482865667e-17,
-            "velocityX": 3.5692258445423475,
-            "velocityY": 1.2535015995473786,
-            "timestamp": 1.4852148668647889
-        },
-        {
-            "x": 7.217234786194527,
-            "y": 7.304478220492258,
-            "heading": 1.2383304928032073e-17,
-            "angularVelocity": -1.753099667759744e-16,
-            "velocityX": 3.115812947231872,
-            "velocityY": 1.0942643260352094,
-            "timestamp": 1.5842291913224416
-        },
-        {
-            "x": 7.448617388685645,
-            "y": 7.385739108696781,
-            "heading": -3.803725574168796e-18,
-            "angularVelocity": -1.6348170333424006e-16,
-            "velocityX": 2.336859881219295,
-            "velocityY": 0.8206983045092451,
-            "timestamp": 1.6832435157800942
-        },
-        {
-            "x": 7.602872461914855,
-            "y": 7.439913035887961,
-            "heading": -2.4779119751079125e-18,
-            "angularVelocity": 1.3390118812558602e-17,
-            "velocityX": 1.557906636985473,
-            "velocityY": 0.5471322203925049,
-            "timestamp": 1.782257840237747
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 2.5025792615041696e-17,
-            "velocityX": 0.7789533333445218,
-            "velocityY": 0.27356611541215564,
-            "timestamp": 1.8812721646953996
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 1.3533974507170236e-34,
-            "velocityY": 0,
-            "timestamp": 1.9802864891530523
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.1.traj
deleted file mode 100644
index 168ae5bd..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.1.traj	
+++ /dev/null
@@ -1,526 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -1.058013890027075e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 2.26040474690827,
-            "y": 5.564834298284281,
-            "heading": -0.00020682462174428096,
-            "angularVelocity": -0.005575654083684234,
-            "velocityX": 0.28049498700644215,
-            "velocityY": 0.1303247879443207,
-            "timestamp": 0.037094234800108185
-        },
-        {
-            "x": 2.2812142405469196,
-            "y": 5.574502894544252,
-            "heading": -0.0006204719876615776,
-            "angularVelocity": -0.011151257551113,
-            "velocityX": 0.5609899692172368,
-            "velocityY": 0.26064956756953994,
-            "timestamp": 0.07418846960021637
-        },
-        {
-            "x": 2.312428480686622,
-            "y": 5.589005788443611,
-            "heading": -0.0012409398807264826,
-            "angularVelocity": -0.016726801251156914,
-            "velocityX": 0.8414849452457597,
-            "velocityY": 0.39097433812857635,
-            "timestamp": 0.11128270440032456
-        },
-        {
-            "x": 2.3540474670292637,
-            "y": 5.608342979609529,
-            "heading": -0.002068225636714691,
-            "angularVelocity": -0.022302273128063925,
-            "velocityX": 1.1219799132376411,
-            "velocityY": 0.5212990986367875,
-            "timestamp": 0.14837693920043274
-        },
-        {
-            "x": 2.406071199181798,
-            "y": 5.632514467619691,
-            "heading": -0.0031023259847413323,
-            "angularVelocity": -0.02787765682725579,
-            "velocityX": 1.4024748706336019,
-            "velocityY": 0.6516238477600773,
-            "timestamp": 0.18547117400054092
-        },
-        {
-            "x": 2.4684996766150022,
-            "y": 5.66152025198224,
-            "heading": -0.004343236799506005,
-            "angularVelocity": -0.03345292931508493,
-            "velocityX": 1.682969813762594,
-            "velocityY": 0.7819485836236739,
-            "timestamp": 0.2225654088006491
-        },
-        {
-            "x": 2.5413328985945514,
-            "y": 5.69536033210275,
-            "heading": -0.005790952692459236,
-            "angularVelocity": -0.03902805653641911,
-            "velocityX": 1.9634647370954057,
-            "velocityY": 0.9122733034625292,
-            "timestamp": 0.2596596436007573
-        },
-        {
-            "x": 2.6245708640566887,
-            "y": 5.7340347072253275,
-            "heading": -0.00744546628275827,
-            "angularVelocity": -0.04460298478228464,
-            "velocityX": 2.2439596317510366,
-            "velocityY": 1.0425980029237354,
-            "timestamp": 0.2967538784008655
-        },
-        {
-            "x": 2.7182135713591755,
-            "y": 5.777543376315584,
-            "heading": -0.009306766753500902,
-            "angularVelocity": -0.05017762142212937,
-            "velocityX": 2.5244544821346007,
-            "velocityY": 1.1729226744995158,
-            "timestamp": 0.3338481132009737
-        },
-        {
-            "x": 2.8222610176995655,
-            "y": 5.8258863377887575,
-            "heading": -0.011374836524793099,
-            "angularVelocity": -0.055751784136736884,
-            "velocityX": 2.8049492569688192,
-            "velocityY": 1.303247303352706,
-            "timestamp": 0.3709423480010819
-        },
-        {
-            "x": 2.9367131973687606,
-            "y": 5.879063588696104,
-            "heading": -0.013649641477510719,
-            "angularVelocity": -0.061325027055469855,
-            "velocityX": 3.0854438778950355,
-            "velocityY": 1.4335718527125696,
-            "timestamp": 0.4080365828011901
-        },
-        {
-            "x": 3.06157009301301,
-            "y": 5.937075120660461,
-            "heading": -0.01613108463550858,
-            "angularVelocity": -0.06689565565567898,
-            "velocityX": 3.3659380309924987,
-            "velocityY": 1.5638961762378367,
-            "timestamp": 0.4451308176012983
-        },
-        {
-            "x": 3.1888299168589427,
-            "y": 5.996203084762226,
-            "heading": -0.01613108529402469,
-            "angularVelocity": -1.7752519083825314e-8,
-            "velocityX": 3.430717051630934,
-            "velocityY": 1.5939933636693306,
-            "timestamp": 0.4822250524014065
-        },
-        {
-            "x": 3.316089744181463,
-            "y": 6.055331041381533,
-            "heading": -0.016131085952509703,
-            "angularVelocity": -1.7751680763659775e-8,
-            "velocityX": 3.4307171453540595,
-            "velocityY": 1.5939931619544851,
-            "timestamp": 0.5193192872015147
-        },
-        {
-            "x": 3.443349575355442,
-            "y": 6.114458989711434,
-            "heading": -0.01613108661098856,
-            "angularVelocity": -1.7751514817327164e-8,
-            "velocityX": 3.430717249183109,
-            "velocityY": 1.5939929384856546,
-            "timestamp": 0.5564135220016229
-        },
-        {
-            "x": 3.5706094108109987,
-            "y": 6.173586928826195,
-            "heading": -0.016131087269461084,
-            "angularVelocity": -1.7751344035751635e-8,
-            "velocityX": 3.4307173646074394,
-            "velocityY": 1.5939926900605101,
-            "timestamp": 0.5935077568017311
-        },
-        {
-            "x": 3.6978692510432434,
-            "y": 6.232714857660196,
-            "heading": -0.01613108792792704,
-            "angularVelocity": -1.7751166882414398e-8,
-            "velocityX": 3.4307174933791567,
-            "velocityY": 1.5939924129080143,
-            "timestamp": 0.6306019916018393
-        },
-        {
-            "x": 3.825129096624906,
-            "y": 6.291842774980762,
-            "heading": -0.016131088586386323,
-            "angularVelocity": -1.775098711225461e-8,
-            "velocityX": 3.430717637590709,
-            "velocityY": 1.5939921025246122,
-            "timestamp": 0.6676962264019475
-        },
-        {
-            "x": 3.952388948222243,
-            "y": 6.3509706793539165,
-            "heading": -0.016131089244838845,
-            "angularVelocity": -1.775080474504649e-8,
-            "velocityX": 3.4307177997634843,
-            "velocityY": 1.5939917534835466,
-            "timestamp": 0.7047904612020557
-        },
-        {
-            "x": 4.07964880661482,
-            "y": 6.410098569101814,
-            "heading": -0.016131089903284688,
-            "angularVelocity": -1.775062484471552e-8,
-            "velocityX": 3.430717982952055,
-            "velocityY": 1.5939913592104822,
-            "timestamp": 0.7418846960021639
-        },
-        {
-            "x": 4.206908672718448,
-            "y": 6.469226442253365,
-            "heading": -0.016131090561724272,
-            "angularVelocity": -1.7750456035932977e-8,
-            "velocityX": 3.4307181908294035,
-            "velocityY": 1.5939909118000826,
-            "timestamp": 0.7789789308022721
-        },
-        {
-            "x": 4.33416854760619,
-            "y": 6.52835429649903,
-            "heading": -0.016131091220158388,
-            "angularVelocity": -1.775030869099235e-8,
-            "velocityX": 3.43071842763476,
-            "velocityY": 1.593990402128264,
-            "timestamp": 0.8160731656023803
-        },
-        {
-            "x": 4.461428432515134,
-            "y": 6.587482129176235,
-            "heading": -0.016131091878588796,
-            "angularVelocity": -1.7750208582663698e-8,
-            "velocityX": 3.430718697790006,
-            "velocityY": 1.5939898206778003,
-            "timestamp": 0.8531674004024885
-        },
-        {
-            "x": 4.588688397859823,
-            "y": 6.646609788732575,
-            "heading": -0.01613109253701839,
-            "angularVelocity": -1.7750186861935105e-8,
-            "velocityX": 3.4307208662063466,
-            "velocityY": 1.5939851536219738,
-            "timestamp": 0.8902616352025967
-        },
-        {
-            "x": 4.7169608374986005,
-            "y": 6.703507592945478,
-            "heading": -0.016131093196594153,
-            "angularVelocity": -1.7781085471466767e-8,
-            "velocityX": 3.4580155199319775,
-            "velocityY": 1.533871894635887,
-            "timestamp": 0.9273558700027049
-        },
-        {
-            "x": 4.84945193271468,
-            "y": 6.749738410352506,
-            "heading": -0.016131093868950192,
-            "angularVelocity": -1.8125621008136676e-8,
-            "velocityX": 3.571743585763167,
-            "velocityY": 1.2463073481945173,
-            "timestamp": 0.9644501048028131
-        },
-        {
-            "x": 4.985276817345946,
-            "y": 6.784991484609654,
-            "heading": -0.0161310945734632,
-            "angularVelocity": -1.8992520332334574e-8,
-            "velocityX": 3.661617104738627,
-            "velocityY": 0.9503653181449264,
-            "timestamp": 1.0015443396029213
-        },
-        {
-            "x": 5.123527608694991,
-            "y": 6.809030968493646,
-            "heading": -0.01613109533353302,
-            "angularVelocity": -2.0490241268499195e-8,
-            "velocityX": 3.7270155886499414,
-            "velocityY": 0.6480652320645928,
-            "timestamp": 1.0386385744030295
-        },
-        {
-            "x": 5.263280146337045,
-            "y": 6.821696104409015,
-            "heading": -0.016131096180260384,
-            "angularVelocity": -2.2826387114294202e-8,
-            "velocityX": 3.767500216547063,
-            "velocityY": 0.34143138370740883,
-            "timestamp": 1.0757328092031377
-        },
-        {
-            "x": 5.403600215911865,
-            "y": 6.822902202606201,
-            "heading": -0.0161310971470982,
-            "angularVelocity": -2.6064368823146806e-8,
-            "velocityX": 3.7827999507462917,
-            "velocityY": 0.032514437989762196,
-            "timestamp": 1.112827044003246
-        },
-        {
-            "x": 5.530811605245593,
-            "y": 6.814532094483614,
-            "heading": -0.016131098042384447,
-            "angularVelocity": -2.6566067685833464e-8,
-            "velocityX": 3.7747775034488793,
-            "velocityY": -0.2483684519763222,
-            "timestamp": 1.1465274115659696
-        },
-        {
-            "x": 5.657050747526861,
-            "y": 6.796742322973506,
-            "heading": -0.016131098828254187,
-            "angularVelocity": -2.3319322572510494e-8,
-            "velocityX": 3.745927757206515,
-            "velocityY": -0.5278806374143462,
-            "timestamp": 1.1802277791286933
-        },
-        {
-            "x": 5.781621117814577,
-            "y": 6.76963106353518,
-            "heading": -0.016131099539274135,
-            "angularVelocity": -2.109828463295381e-8,
-            "velocityX": 3.6964098405118033,
-            "velocityY": -0.8044796362492181,
-            "timestamp": 1.213928146691417
-        },
-        {
-            "x": 5.903835409646495,
-            "y": 6.733347943713289,
-            "heading": -0.016131100200026813,
-            "angularVelocity": -1.96066906061588e-8,
-            "velocityX": 3.626497295747634,
-            "velocityY": -1.0766386970219277,
-            "timestamp": 1.2476285142541406
-        },
-        {
-            "x": 6.023019351333038,
-            "y": 6.68809326224078,
-            "heading": -0.016131100829259,
-            "angularVelocity": -1.86713746487578e-8,
-            "velocityX": 3.5365769072018636,
-            "velocityY": -1.3428542400399708,
-            "timestamp": 1.2813288818168642
-        },
-        {
-            "x": 6.138515596479519,
-            "y": 6.634117196875684,
-            "heading": -0.016131101442406693,
-            "angularVelocity": -1.81940951537302e-8,
-            "velocityX": 3.4271509036665004,
-            "velocityY": -1.601646191681265,
-            "timestamp": 1.3150292493795879
-        },
-        {
-            "x": 6.2512259314426855,
-            "y": 6.5745422130600915,
-            "heading": -0.016131102051778846,
-            "angularVelocity": -1.80820624580199e-8,
-            "velocityX": 3.3444838473464196,
-            "velocityY": -1.7677843929954529,
-            "timestamp": 1.3487296169423115
-        },
-        {
-            "x": 6.363935646775815,
-            "y": 6.514966056976746,
-            "heading": -0.01613110266098242,
-            "angularVelocity": -1.807706019437417e-8,
-            "velocityX": 3.344465460898959,
-            "velocityY": -1.7678191780093255,
-            "timestamp": 1.3824299845050352
-        },
-        {
-            "x": 6.4758517936606514,
-            "y": 6.453912192428435,
-            "heading": -0.016131102003666662,
-            "angularVelocity": 1.950470585863807e-8,
-            "velocityX": 3.320917692560386,
-            "velocityY": -1.811667615632877,
-            "timestamp": 1.4161303520677588
-        },
-        {
-            "x": 6.587767622018337,
-            "y": 6.392857744000494,
-            "heading": -0.016131101346154864,
-            "angularVelocity": 1.951052313957626e-8,
-            "velocityX": 3.32090824081921,
-            "velocityY": -1.8116849412489389,
-            "timestamp": 1.4498307196304825
-        },
-        {
-            "x": 6.699683405881235,
-            "y": 6.331803214011495,
-            "heading": -0.016131100688641712,
-            "angularVelocity": 1.9510563155904532e-8,
-            "velocityX": 3.3209069205135386,
-            "velocityY": -1.811687361432026,
-            "timestamp": 1.4835310871932061
-        },
-        {
-            "x": 6.811599167638959,
-            "y": 6.270748643502715,
-            "heading": -0.016131100031139815,
-            "angularVelocity": 1.9510229240352237e-8,
-            "velocityX": 3.320906264580792,
-            "velocityY": -1.8116885637862563,
-            "timestamp": 1.5172314547559298
-        },
-        {
-            "x": 6.923514914598752,
-            "y": 6.209694045868981,
-            "heading": -0.016131099373601954,
-            "angularVelocity": 1.95112965056638e-8,
-            "velocityX": 3.3209058254778503,
-            "velocityY": -1.8116893686722635,
-            "timestamp": 1.5509318223186535
-        },
-        {
-            "x": 7.031584175346113,
-            "y": 6.1507377884047765,
-            "heading": -0.013826704963615632,
-            "angularVelocity": 0.0683789102803133,
-            "velocityX": 3.2067680136194574,
-            "velocityY": -1.7494247608568367,
-            "timestamp": 1.584632189881377
-        },
-        {
-            "x": 7.131340439149152,
-            "y": 6.096316611291437,
-            "heading": -0.011699535798216875,
-            "angularVelocity": 0.06312005830317652,
-            "velocityX": 2.960094236876553,
-            "velocityY": -1.614854111369865,
-            "timestamp": 1.6183325574441008
-        },
-        {
-            "x": 7.222783690004588,
-            "y": 6.046430524071638,
-            "heading": -0.00974961791861361,
-            "angularVelocity": 0.057860433598327515,
-            "velocityX": 2.713419985263968,
-            "velocityY": -1.4802831787205646,
-            "timestamp": 1.6520329250068244
-        },
-        {
-            "x": 7.305913922772626,
-            "y": 6.001079530283105,
-            "heading": -0.00797695998479322,
-            "angularVelocity": 0.05260055192336173,
-            "velocityX": 2.466745581136908,
-            "velocityY": -1.3457121410953312,
-            "timestamp": 1.685733292569548
-        },
-        {
-            "x": 7.380731135011445,
-            "y": 5.960263631930958,
-            "heading": -0.00638156583911186,
-            "angularVelocity": 0.04734055623313652,
-            "velocityX": 2.2200711045530213,
-            "velocityY": -1.2111410439717167,
-            "timestamp": 1.7194336601322717
-        },
-        {
-            "x": 7.447235325348076,
-            "y": 5.923982830388488,
-            "heading": -0.004963437343393232,
-            "angularVelocity": 0.042080505296519764,
-            "velocityX": 1.9733965872286794,
-            "velocityY": -1.0765699060980205,
-            "timestamp": 1.7531340276949954
-        },
-        {
-            "x": 7.505426492937412,
-            "y": 5.892237126701046,
-            "heading": -0.0037225753686417596,
-            "angularVelocity": 0.0368204285143562,
-            "velocityX": 1.72672204482727,
-            "velocityY": -0.9419987372053683,
-            "timestamp": 1.786834395257719
-        },
-        {
-            "x": 7.555304637231595,
-            "y": 5.8650265217185185,
-            "heading": -0.002658980227466916,
-            "angularVelocity": 0.03156034245604838,
-            "velocityX": 1.4800474861690802,
-            "velocityY": -0.8074275430937743,
-            "timestamp": 1.8205347628204427
-        },
-        {
-            "x": 7.596869757865521,
-            "y": 5.842351016163066,
-            "heading": -0.0017726518914738514,
-            "angularVelocity": 0.02630025724030852,
-            "velocityX": 1.233372916677093,
-            "velocityY": -0.6728563275533557,
-            "timestamp": 1.8542351303831663
-        },
-        {
-            "x": 7.630121854593904,
-            "y": 5.824210610667954,
-            "heading": -0.001063590116536395,
-            "angularVelocity": 0.021040179268589657,
-            "velocityX": 0.9866983399066315,
-            "velocityY": -0.5382850932218669,
-            "timestamp": 1.88793549794589
-        },
-        {
-            "x": 7.655060927254085,
-            "y": 5.810605305801891,
-            "heading": -0.000531794525727047,
-            "angularVelocity": 0.01578011248154109,
-            "velocityX": 0.7400237583095566,
-            "velocityY": -0.4037138420149424,
-            "timestamp": 1.9216358655086136
-        },
-        {
-            "x": 7.671686975742638,
-            "y": 5.8015351020850225,
-            "heading": -0.00017726465841382563,
-            "angularVelocity": 0.010520059362948236,
-            "velocityX": 0.49334917364355607,
-            "velocityY": -0.26914257537363395,
-            "timestamp": 1.9553362330713373
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 3.6111480760753294e-31,
-            "angularVelocity": 0.005260021514121506,
-            "velocityX": 0.24667458721003926,
-            "velocityY": -0.1345712944104092,
-            "timestamp": 1.989036600634061
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -8.452347473573201e-32,
-            "angularVelocity": -1.5098225699671103e-29,
-            "velocityX": 2.1451735090517096e-32,
-            "velocityY": 9.12030277003484e-33,
-            "timestamp": 2.0227369681967846
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.1.traj
deleted file mode 100644
index 72a99c84..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.1.traj	
+++ /dev/null
@@ -1,571 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 8.750120685231394e-32,
-            "velocityX": 3.618098191401073e-32,
-            "velocityY": 7.301888602720515e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 2.256418341264449,
-            "y": 5.558391022160101,
-            "heading": -2.5296452464584536e-15,
-            "angularVelocity": -8.979773235851594e-14,
-            "velocityX": 0.22783925602590605,
-            "velocityY": -0.05711574048502746,
-            "timestamp": 0.0281704802605187
-        },
-        {
-            "x": 2.2692550247581535,
-            "y": 5.555173071000616,
-            "heading": -7.593688933611638e-15,
-            "angularVelocity": -1.7976419432911875e-13,
-            "velocityX": 0.4556785463006615,
-            "velocityY": -0.11423132050732354,
-            "timestamp": 0.0563409605210374
-        },
-        {
-            "x": 2.2885100515708303,
-            "y": 5.55034615163608,
-            "heading": -1.5195474783343342e-14,
-            "angularVelocity": -2.6984935224723246e-13,
-            "velocityX": 0.6835178752583485,
-            "velocityY": -0.1713467189730698,
-            "timestamp": 0.0845114407815561
-        },
-        {
-            "x": 2.314183422940896,
-            "y": 5.54391026989229,
-            "heading": -2.533887633402318e-14,
-            "angularVelocity": -3.6007201361891127e-13,
-            "velocityX": 0.9113572481775378,
-            "velocityY": -0.2284619106337897,
-            "timestamp": 0.1126819210420748
-        },
-        {
-            "x": 2.346275140284958,
-            "y": 5.535865432453464,
-            "heading": -3.8024880419905343e-14,
-            "angularVelocity": -4.5032970578145327e-13,
-            "velocityX": 1.139196671383666,
-            "velocityY": -0.2855768650171331,
-            "timestamp": 0.14085240130259352
-        },
-        {
-            "x": 2.3847852052344276,
-            "y": 5.526211647048542,
-            "heading": -5.3249348657163737e-14,
-            "angularVelocity": -5.404404929621474e-13,
-            "velocityX": 1.3670361525018668,
-            "velocityY": -0.3426915450373883,
-            "timestamp": 0.16902288156311224
-        },
-        {
-            "x": 2.4297136196807942,
-            "y": 5.514948922688138,
-            "heading": -7.10022499667577e-14,
-            "angularVelocity": -6.301951953680268e-13,
-            "velocityX": 1.594875700764475,
-            "velocityY": -0.3998059051974526,
-            "timestamp": 0.19719336182363095
-        },
-        {
-            "x": 2.4810603858307716,
-            "y": 5.502077269965895,
-            "heading": -9.12659901579941e-14,
-            "angularVelocity": -7.193253365088161e-13,
-            "velocityX": 1.8227153273613368,
-            "velocityY": -0.45691988930278143,
-            "timestamp": 0.22536384208414967
-        },
-        {
-            "x": 2.5388255062707983,
-            "y": 5.4875967014378215,
-            "heading": -1.1400857239830863e-13,
-            "angularVelocity": -8.073196492084524e-13,
-            "velocityX": 2.050555045771992,
-            "velocityY": -0.5140334276930385,
-            "timestamp": 0.2535343223446684
-        },
-        {
-            "x": 2.603008984034667,
-            "y": 5.4715072320838045,
-            "heading": -1.3917722607752625e-13,
-            "angularVelocity": -8.934407027436765e-13,
-            "velocityX": 2.2783948718766736,
-            "velocityY": -0.571146434324935,
-            "timestamp": 0.2817048026051871
-        },
-        {
-            "x": 2.673610822650838,
-            "y": 5.453808879815544,
-            "heading": -1.6668826583849139e-13,
-            "angularVelocity": -9.76591080715578e-13,
-            "velocityX": 2.5062348232351805,
-            "velocityY": -0.6282588051246059,
-            "timestamp": 0.3098752828657058
-        },
-        {
-            "x": 2.7506310260932763,
-            "y": 5.434501665856061,
-            "heading": -1.9641690622397207e-13,
-            "angularVelocity": -1.0553118055097966e-12,
-            "velocityX": 2.7340749156620854,
-            "velocityY": -0.6853704225462842,
-            "timestamp": 0.33804576312622453
-        },
-        {
-            "x": 2.8340695983911575,
-            "y": 5.413585614341132,
-            "heading": -2.282023162882806e-13,
-            "angularVelocity": -1.128323328886794e-12,
-            "velocityX": 2.9619151511173074,
-            "velocityY": -0.7424811831924288,
-            "timestamp": 0.36621624338674325
-        },
-        {
-            "x": 2.9239265420414853,
-            "y": 5.391060748788907,
-            "heading": -2.61980248572899e-13,
-            "angularVelocity": -1.1990541861941579e-12,
-            "velocityX": 3.189755475211519,
-            "velocityY": -0.7995911089877218,
-            "timestamp": 0.39438672364726196
-        },
-        {
-            "x": 3.020201851829833,
-            "y": 5.366927076274376,
-            "heading": -2.9867647823192555e-13,
-            "angularVelocity": -1.302648350982896e-12,
-            "velocityX": 3.417595614203248,
-            "velocityY": -0.8567007836339452,
-            "timestamp": 0.4225572039077807
-        },
-        {
-            "x": 3.12289548969471,
-            "y": 5.341184519140017,
-            "heading": -3.474157859958988e-13,
-            "angularVelocity": -1.7301553723681501e-12,
-            "velocityX": 3.6454344020823566,
-            "velocityY": -0.9138132149787032,
-            "timestamp": 0.4507276841682994
-        },
-        {
-            "x": 3.2262642847742486,
-            "y": 5.315271868799975,
-            "heading": -3.474349355086831e-13,
-            "angularVelocity": -6.797723212352284e-16,
-            "velocityX": 3.6694012357471815,
-            "velocityY": -0.9198512095073661,
-            "timestamp": 0.4788981644288181
-        },
-        {
-            "x": 3.3296318094304937,
-            "y": 5.289354150477899,
-            "heading": -3.47428956348503e-13,
-            "angularVelocity": 2.1224913898406702e-16,
-            "velocityX": 3.669356138067555,
-            "velocityY": -0.920031113505745,
-            "timestamp": 0.5070686446893368
-        },
-        {
-            "x": 3.4329975109325144,
-            "y": 5.263429161946644,
-            "heading": -3.4742781851400116e-13,
-            "angularVelocity": 4.0391022494337805e-17,
-            "velocityX": 3.669291419461124,
-            "velocityY": -0.9202891924987843,
-            "timestamp": 0.5352391249498555
-        },
-        {
-            "x": 3.536360974747324,
-            "y": 5.237495253145949,
-            "heading": -3.474322284236003e-13,
-            "angularVelocity": -1.565436427905273e-16,
-            "velocityX": 3.669211985699591,
-            "velocityY": -0.9206058455823695,
-            "timestamp": 0.5634096052103742
-        },
-        {
-            "x": 3.6397250084501422,
-            "y": 5.211563615847215,
-            "heading": -3.474363074560882e-13,
-            "angularVelocity": -1.4479811668640362e-16,
-            "velocityX": 3.6692322156709736,
-            "velocityY": -0.9205252114596718,
-            "timestamp": 0.5915800854708929
-        },
-        {
-            "x": 3.7430830091153613,
-            "y": 5.185607942589789,
-            "heading": -3.474405753435316e-13,
-            "angularVelocity": -1.515021186684875e-16,
-            "velocityX": 3.669018053983158,
-            "velocityY": -0.9213784435831122,
-            "timestamp": 0.6197505657314116
-        },
-        {
-            "x": 3.8464299042958676,
-            "y": 5.1596080862072276,
-            "heading": -3.474364187384818e-13,
-            "angularVelocity": 1.4755179936907513e-16,
-            "velocityX": 3.668623829794913,
-            "velocityY": -0.92294686289042,
-            "timestamp": 0.6479210459919303
-        },
-        {
-            "x": 3.9490305912287407,
-            "y": 5.130804053406161,
-            "heading": -3.4744608105895375e-13,
-            "angularVelocity": -3.429945241470833e-16,
-            "velocityX": 3.642134815737201,
-            "velocityY": -1.0224899446047346,
-            "timestamp": 0.6760915262524491
-        },
-        {
-            "x": 4.049738686089906,
-            "y": 5.09595505292046,
-            "heading": -3.4745592092862155e-13,
-            "angularVelocity": -3.4929719255187864e-16,
-            "velocityX": 3.57495129404341,
-            "velocityY": -1.237075128411728,
-            "timestamp": 0.7042620065129678
-        },
-        {
-            "x": 4.148105144500732,
-            "y": 5.054959774017334,
-            "heading": -3.474586084587748e-13,
-            "angularVelocity": -9.540235481877701e-17,
-            "velocityX": 3.491827526585996,
-            "velocityY": -1.4552566560457803,
-            "timestamp": 0.7324324867734865
-        },
-        {
-            "x": 4.301558125072517,
-            "y": 4.97182210350292,
-            "heading": -3.4745624581283064e-13,
-            "angularVelocity": 5.1211249699831376e-17,
-            "velocityX": 3.3261517465815813,
-            "velocityY": -1.8020406443565171,
-            "timestamp": 0.7785677783246081
-        },
-        {
-            "x": 4.445898119107738,
-            "y": 4.873711676741488,
-            "heading": -3.4747329562610653e-13,
-            "angularVelocity": -3.6956119081305154e-16,
-            "velocityX": 3.1286242956827928,
-            "velocityY": -2.1265808335182323,
-            "timestamp": 0.8247030698757297
-        },
-        {
-            "x": 4.587571576014754,
-            "y": 4.771788561847154,
-            "heading": -3.4746251546031876e-13,
-            "angularVelocity": 2.3366419539300746e-16,
-            "velocityX": 3.0708260887450245,
-            "velocityY": -2.2092222996219535,
-            "timestamp": 0.8708383614268513
-        },
-        {
-            "x": 4.729219222551993,
-            "y": 4.66982957987028,
-            "heading": -3.474587781166732e-13,
-            "angularVelocity": 8.100834549122579e-17,
-            "velocityX": 3.070266639108493,
-            "velocityY": -2.209999732353969,
-            "timestamp": 0.916973652977973
-        },
-        {
-            "x": 4.870877206231954,
-            "y": 4.567884960544782,
-            "heading": -3.47487867941798e-13,
-            "angularVelocity": -6.305330291624225e-16,
-            "velocityX": 3.070490700636254,
-            "velocityY": -2.209688416384298,
-            "timestamp": 0.9631089445290946
-        },
-        {
-            "x": 5.012587244979431,
-            "y": 4.466012713767847,
-            "heading": -3.4750411523417436e-13,
-            "angularVelocity": -3.5216624493767476e-16,
-            "velocityX": 3.0716190140743462,
-            "velocityY": -2.208119713797033,
-            "timestamp": 1.009244236080216
-        },
-        {
-            "x": 5.160453670184351,
-            "y": 4.373302358522453,
-            "heading": -3.47491376477911e-13,
-            "angularVelocity": 2.7611738924244047e-16,
-            "velocityX": 3.2050610331965634,
-            "velocityY": -2.009532228521384,
-            "timestamp": 1.0553795276313376
-        },
-        {
-            "x": 5.316942928228011,
-            "y": 4.29603139716118,
-            "heading": -3.4750002331748596e-13,
-            "angularVelocity": -1.8742353831509497e-16,
-            "velocityX": 3.3919642161767567,
-            "velocityY": -1.6748774910066955,
-            "timestamp": 1.101514819182459
-        },
-        {
-            "x": 5.480467319488525,
-            "y": 4.235044002532959,
-            "heading": -3.475163170309309e-13,
-            "angularVelocity": -3.5317243928660403e-16,
-            "velocityX": 3.5444534056932553,
-            "velocityY": -1.321924985762245,
-            "timestamp": 1.1476501107335806
-        },
-        {
-            "x": 5.5914176787721175,
-            "y": 4.201991970759951,
-            "heading": -3.4752963658489967e-13,
-            "angularVelocity": -4.352387405096259e-16,
-            "velocityX": 3.625488497149835,
-            "velocityY": -1.0800304007696078,
-            "timestamp": 1.1782529824212593
-        },
-        {
-            "x": 5.704342870003183,
-            "y": 4.176490587918632,
-            "heading": -3.4752519346531394e-13,
-            "angularVelocity": 1.4518636145588672e-16,
-            "velocityX": 3.6900194329281493,
-            "velocityY": -0.8333003223399679,
-            "timestamp": 1.208855854108938
-        },
-        {
-            "x": 5.818728492967466,
-            "y": 4.15864825583045,
-            "heading": -3.47528320752536e-13,
-            "angularVelocity": -1.0218933861720526e-16,
-            "velocityX": 3.737741481635173,
-            "velocityY": -0.5830280331479286,
-            "timestamp": 1.2394587257966168
-        },
-        {
-            "x": 5.934052260797228,
-            "y": 4.148506842880417,
-            "heading": -3.475166383708343e-13,
-            "angularVelocity": 3.817413550372978e-16,
-            "velocityX": 3.768396933681978,
-            "velocityY": -0.3313876244719343,
-            "timestamp": 1.2700615974842955
-        },
-        {
-            "x": 6.049784939896788,
-            "y": 4.145614314337883,
-            "heading": -3.4752553093783573e-13,
-            "angularVelocity": -2.9057949488904467e-16,
-            "velocityX": 3.7817587931181613,
-            "velocityY": -0.0945182064161531,
-            "timestamp": 1.3006644691719742
-        },
-        {
-            "x": 6.1655453259831,
-            "y": 4.144216905783092,
-            "heading": -3.4753489515012823e-13,
-            "angularVelocity": -3.0599129336581387e-16,
-            "velocityX": 3.7826641652362696,
-            "velocityY": -0.04566266099543126,
-            "timestamp": 1.331267340859653
-        },
-        {
-            "x": 6.281306292241875,
-            "y": 4.142868414150292,
-            "heading": -3.4754223223995867e-13,
-            "angularVelocity": -2.397516775868154e-16,
-            "velocityX": 3.7826831233414593,
-            "velocityY": -0.04406421878574171,
-            "timestamp": 1.3618702125473316
-        },
-        {
-            "x": 6.39706739301833,
-            "y": 4.141531519398385,
-            "heading": -3.475633687164305e-13,
-            "angularVelocity": -6.906697084916283e-16,
-            "velocityX": 3.7826875189315166,
-            "velocityY": -0.04368527131792852,
-            "timestamp": 1.3924730842350104
-        },
-        {
-            "x": 6.512828617962261,
-            "y": 4.14020542040497,
-            "heading": -3.4758398313992887e-13,
-            "angularVelocity": -6.73610754719036e-16,
-            "velocityX": 3.7826915763114313,
-            "velocityY": -0.043332501854336236,
-            "timestamp": 1.423075955922689
-        },
-        {
-            "x": 6.628589931637748,
-            "y": 4.138887090943481,
-            "heading": -3.4757335612235213e-13,
-            "angularVelocity": 3.472555676974266e-16,
-            "velocityX": 3.7826944757635745,
-            "velocityY": -0.04307861939704374,
-            "timestamp": 1.4536788276103678
-        },
-        {
-            "x": 6.744351296535766,
-            "y": 4.137573267392239,
-            "heading": -3.475819448429605e-13,
-            "angularVelocity": -2.806508055564177e-16,
-            "velocityX": 3.782696149545524,
-            "velocityY": -0.042931381231176444,
-            "timestamp": 1.4842816992980465
-        },
-        {
-            "x": 6.860112695906494,
-            "y": 4.136261713498386,
-            "heading": -3.475979530506806e-13,
-            "angularVelocity": -5.230943272138926e-16,
-            "velocityX": 3.7826972759991557,
-            "velocityY": -0.04285721636551622,
-            "timestamp": 1.5148845709857253
-        },
-        {
-            "x": 6.9694302247553,
-            "y": 4.135029589899985,
-            "heading": -2.392571520797574e-13,
-            "angularVelocity": 3.5402168172320343e-12,
-            "velocityX": 3.5721330326269505,
-            "velocityY": -0.04026169865772549,
-            "timestamp": 1.545487442673404
-        },
-        {
-            "x": 7.070940187048315,
-            "y": 4.133884255746116,
-            "heading": -1.6833543298116243e-13,
-            "angularVelocity": 2.317485846705618e-12,
-            "velocityX": 3.3170077412667367,
-            "velocityY": -0.03742570844047695,
-            "timestamp": 1.5760903143610827
-        },
-        {
-            "x": 7.164641690009617,
-            "y": 4.132826276383854,
-            "heading": -1.1981581485168293e-13,
-            "angularVelocity": 1.5854597325485145e-12,
-            "velocityX": 3.061853276960511,
-            "velocityY": -0.034571244567840914,
-            "timestamp": 1.6066931860487614
-        },
-        {
-            "x": 7.250534738513507,
-            "y": 4.131855992058627,
-            "heading": -8.523066967233492e-14,
-            "angularVelocity": 1.1301275060622065e-12,
-            "velocityX": 2.8066989719298543,
-            "velocityY": -0.03170566260882202,
-            "timestamp": 1.6372960577364402
-        },
-        {
-            "x": 7.328619329487729,
-            "y": 4.1309736027819035,
-            "heading": -6.003549662940244e-14,
-            "angularVelocity": 8.232944437274219e-13,
-            "velocityX": 2.5515445665080714,
-            "velocityY": -0.028833544941282405,
-            "timestamp": 1.6678989294241189
-        },
-        {
-            "x": 7.398895462248961,
-            "y": 4.130179235828021,
-            "heading": -4.153185117983844e-14,
-            "angularVelocity": 6.046375734752488e-13,
-            "velocityX": 2.2963901387575416,
-            "velocityY": -0.02595726837221309,
-            "timestamp": 1.6985018011117976
-        },
-        {
-            "x": 7.461363136410412,
-            "y": 4.129472977946104,
-            "heading": -2.797073854746349e-14,
-            "angularVelocity": 4.431320366500053e-13,
-            "velocityX": 2.041235698368024,
-            "velocityY": -0.0230781571305896,
-            "timestamp": 1.7291046727994763
-        },
-        {
-            "x": 7.516022351719397,
-            "y": 4.1288548915008905,
-            "heading": -1.8141238812912564e-14,
-            "angularVelocity": 3.211953369611228e-13,
-            "velocityX": 1.7860812497215437,
-            "velocityY": -0.02019700801679021,
-            "timestamp": 1.759707544487155
-        },
-        {
-            "x": 7.5628731079961895,
-            "y": 4.128325023227497,
-            "heading": -1.1163529343344889e-14,
-            "angularVelocity": 2.2800831618830555e-13,
-            "velocityX": 1.5309267952022354,
-            "velocityY": -0.017314331754962387,
-            "timestamp": 1.7903104161748338
-        },
-        {
-            "x": 7.601915405105201,
-            "y": 4.127883409320133,
-            "heading": -6.372564950074351e-15,
-            "angularVelocity": 1.5655276164796086e-13,
-            "velocityX": 1.2757723362523365,
-            "velocityY": -0.014430472786738478,
-            "timestamp": 1.8209132878625125
-        },
-        {
-            "x": 7.63314924293974,
-            "y": 4.127530078564582,
-            "heading": -3.2435183420078772e-15,
-            "angularVelocity": 1.0224681844586284e-13,
-            "velocityX": 1.020617873816125,
-            "velocityY": -0.011545673194894595,
-            "timestamp": 1.8515161595501912
-        },
-        {
-            "x": 7.656574621413203,
-            "y": 4.127265054360105,
-            "heading": -1.3581702257498648e-15,
-            "angularVelocity": 6.160689169092944e-14,
-            "velocityX": 0.7654634085499833,
-            "velocityY": -0.008660108992989924,
-            "timestamp": 1.88211903123787
-        },
-        {
-            "x": 7.672191540453606,
-            "y": 4.127088356077157,
-            "heading": -3.7222995775515084e-16,
-            "angularVelocity": 3.2217238765593925e-14,
-            "velocityX": 0.5103089409316736,
-            "velocityY": -0.0057739118290542285,
-            "timestamp": 1.9127219029255487
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 2.9643987227812266e-34,
-            "angularVelocity": 1.2163231774130929e-14,
-            "velocityX": 0.25515447132169783,
-            "velocityY": -0.0028871825495910644,
-            "timestamp": 1.9433247746132274
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -1.7511816257564745e-34,
-            "angularVelocity": -1.536305146096975e-32,
-            "velocityX": 2.290424541792184e-34,
-            "velocityY": 7.21466542895957e-33,
-            "timestamp": 1.9739276463009061
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.1.traj
deleted file mode 100644
index 5f6c2964..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.1.traj	
+++ /dev/null
@@ -1,589 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.253608767824088,
-            "y": 5.559959646221142,
-            "heading": 0.04333845773860565,
-            "angularVelocity": 1.2657316926377489,
-            "velocityX": 0.10539673178657698,
-            "velocityY": -0.0011785619397610312,
-            "timestamp": 0.03423984560921402
-        },
-        {
-            "x": 2.260842860117559,
-            "y": 5.560038303784881,
-            "heading": 0.12991520296053788,
-            "angularVelocity": 2.5285378389274697,
-            "velocityX": 0.211277012637124,
-            "velocityY": 0.002297252290136546,
-            "timestamp": 0.06847969121842803
-        },
-        {
-            "x": 2.271629728646886,
-            "y": 5.560441749821537,
-            "heading": 0.2597959154554935,
-            "angularVelocity": 3.7932622120236568,
-            "velocityX": 0.3150384687021929,
-            "velocityY": 0.011782939714764799,
-            "timestamp": 0.10271953682764205
-        },
-        {
-            "x": 2.285721166472015,
-            "y": 5.561272024294721,
-            "heading": 0.433416913958427,
-            "angularVelocity": 5.070729596286839,
-            "velocityX": 0.41155085761651194,
-            "velocityY": 0.024248779701277118,
-            "timestamp": 0.13695938243685607
-        },
-        {
-            "x": 2.302746549134424,
-            "y": 5.561830584813036,
-            "heading": 0.6513804515506975,
-            "angularVelocity": 6.36578622695705,
-            "velocityX": 0.497238885266109,
-            "velocityY": 0.01631317280720512,
-            "timestamp": 0.17119922804607007
-        },
-        {
-            "x": 2.325552284167229,
-            "y": 5.559809655953846,
-            "heading": 0.9008347442106217,
-            "angularVelocity": 7.285497005652254,
-            "velocityX": 0.6660583488924382,
-            "velocityY": -0.05902272113767896,
-            "timestamp": 0.20543907365528408
-        },
-        {
-            "x": 2.357593597831257,
-            "y": 5.5547755319598515,
-            "heading": 1.1463891087836293,
-            "angularVelocity": 7.171596723173552,
-            "velocityX": 0.9357902494573596,
-            "velocityY": -0.14702531230571617,
-            "timestamp": 0.23967891926449808
-        },
-        {
-            "x": 2.398826095012215,
-            "y": 5.54977146589545,
-            "heading": 1.3795454342192353,
-            "angularVelocity": 6.809502825937482,
-            "velocityX": 1.2042255578939407,
-            "velocityY": -0.14614744825411982,
-            "timestamp": 0.2739187648737121
-        },
-        {
-            "x": 2.4467424620473803,
-            "y": 5.551292722520974,
-            "heading": 1.6025344347532522,
-            "angularVelocity": 6.512558586829906,
-            "velocityX": 1.399432917485785,
-            "velocityY": 0.04442942421196648,
-            "timestamp": 0.3081586104829261
-        },
-        {
-            "x": 2.499282041125752,
-            "y": 5.560003512593039,
-            "heading": 1.8091707816342142,
-            "angularVelocity": 6.034967249541443,
-            "velocityX": 1.5344572425359657,
-            "velocityY": 0.2544050627880495,
-            "timestamp": 0.3423984560921401
-        },
-        {
-            "x": 2.558927262082263,
-            "y": 5.571143049666431,
-            "heading": 1.9899644077603311,
-            "angularVelocity": 5.2802114877955235,
-            "velocityX": 1.7419827658469296,
-            "velocityY": 0.3253384142127747,
-            "timestamp": 0.3766383017013541
-        },
-        {
-            "x": 2.624771333399717,
-            "y": 5.5772713811756995,
-            "heading": 2.148730759711285,
-            "angularVelocity": 4.63688866366352,
-            "velocityX": 1.9230247726273466,
-            "velocityY": 0.17898245159199333,
-            "timestamp": 0.4108781473105681
-        },
-        {
-            "x": 2.6962684360037885,
-            "y": 5.576686266483743,
-            "heading": 2.2908744479705683,
-            "angularVelocity": 4.151411483614645,
-            "velocityX": 2.0881257298902987,
-            "velocityY": -0.0170887070763856,
-            "timestamp": 0.4451179929197821
-        },
-        {
-            "x": 2.773687793639176,
-            "y": 5.569334398339137,
-            "heading": 2.4184902740666434,
-            "angularVelocity": 3.727114530613817,
-            "velocityX": 2.2610895656186547,
-            "velocityY": -0.2147167434256298,
-            "timestamp": 0.4793578385289961
-        },
-        {
-            "x": 2.8582271385194487,
-            "y": 5.556743437974915,
-            "heading": 2.5300657438997165,
-            "angularVelocity": 3.258644069441941,
-            "velocityX": 2.4690340559690878,
-            "velocityY": -0.36772830426646747,
-            "timestamp": 0.5135976841382102
-        },
-        {
-            "x": 2.9508927177061977,
-            "y": 5.541492370817128,
-            "heading": 2.6233336392816216,
-            "angularVelocity": 2.7239578252305745,
-            "velocityX": 2.706366735538439,
-            "velocityY": -0.445418689437171,
-            "timestamp": 0.5478375297474242
-        },
-        {
-            "x": 3.0518533392296137,
-            "y": 5.525810656425357,
-            "heading": 2.697205719709221,
-            "angularVelocity": 2.1574887127329996,
-            "velocityX": 2.948629578406965,
-            "velocityY": -0.45799606022611716,
-            "timestamp": 0.5820773753566382
-        },
-        {
-            "x": 3.1608544462331407,
-            "y": 5.511189976920693,
-            "heading": 2.751249379299885,
-            "angularVelocity": 1.5783850256649679,
-            "velocityX": 3.183457900119567,
-            "velocityY": -0.42700775206563796,
-            "timestamp": 0.6163172209658522
-        },
-        {
-            "x": 3.2775207255393237,
-            "y": 5.498694402974223,
-            "heading": 2.785399702937351,
-            "angularVelocity": 0.9973854446433661,
-            "velocityX": 3.40732492306531,
-            "velocityY": -0.36494247342945346,
-            "timestamp": 0.6505570665750662
-        },
-        {
-            "x": 3.401135401206032,
-            "y": 5.488837187261409,
-            "heading": 2.800896234569334,
-            "angularVelocity": 0.4525876608454414,
-            "velocityX": 3.610257974803583,
-            "velocityY": -0.28788727102673717,
-            "timestamp": 0.6847969121842802
-        },
-        {
-            "x": 3.5179017921083777,
-            "y": 5.474779629986763,
-            "heading": 2.835487088185811,
-            "angularVelocity": 1.0102514483058458,
-            "velocityX": 3.410248756230483,
-            "velocityY": -0.4105613510962388,
-            "timestamp": 0.7190367577934942
-        },
-        {
-            "x": 3.6287098119916026,
-            "y": 5.4546983370085105,
-            "heading": 2.8876881760682602,
-            "angularVelocity": 1.5245713569573334,
-            "velocityX": 3.2362301263825226,
-            "velocityY": -0.5864890048700768,
-            "timestamp": 0.7532766034027082
-        },
-        {
-            "x": 3.7356372636451365,
-            "y": 5.425785607175325,
-            "heading": 2.9455736884009496,
-            "angularVelocity": 1.6905891747687094,
-            "velocityX": 3.1228952628442794,
-            "velocityY": -0.8444176461299453,
-            "timestamp": 0.7875164490119222
-        },
-        {
-            "x": 3.8388362586877394,
-            "y": 5.387864157402447,
-            "heading": 3.0009390226743027,
-            "angularVelocity": 1.6169855117119507,
-            "velocityX": 3.0140029315678847,
-            "velocityY": -1.1075239709221774,
-            "timestamp": 0.8217562946211362
-        },
-        {
-            "x": 3.9379980862804507,
-            "y": 5.341070528950818,
-            "heading": 3.053673275383994,
-            "angularVelocity": 1.5401428298350917,
-            "velocityX": 2.896094472050628,
-            "velocityY": -1.3666425072616961,
-            "timestamp": 0.8559961402303502
-        },
-        {
-            "x": 4.032804489135742,
-            "y": 5.285560607910156,
-            "heading": 3.1036105469083837,
-            "angularVelocity": 1.4584549268806075,
-            "velocityX": 2.76889107320562,
-            "velocityY": -1.62120827512502,
-            "timestamp": 0.8902359858395642
-        },
-        {
-            "x": 4.173725003261308,
-            "y": 5.173637687034127,
-            "heading": 3.175484729095798,
-            "angularVelocity": 1.2994086167535415,
-            "velocityX": 2.547692714675012,
-            "velocityY": -2.023447131813128,
-            "timestamp": 0.9455489807364162
-        },
-        {
-            "x": 4.301872286760783,
-            "y": 5.039921697201636,
-            "heading": 3.2346580028937466,
-            "angularVelocity": 1.0697897285853943,
-            "velocityX": 2.3167663175433564,
-            "velocityY": -2.4174425934058767,
-            "timestamp": 1.0008619756332682
-        },
-        {
-            "x": 4.416442024233276,
-            "y": 4.885133369968262,
-            "heading": 3.2770092965554323,
-            "angularVelocity": 0.765666255111711,
-            "velocityX": 2.0712987551323803,
-            "velocityY": -2.7984079965661373,
-            "timestamp": 1.0561749705301202
-        },
-        {
-            "x": 4.517908742039174,
-            "y": 4.711097539386313,
-            "heading": 3.282986865137276,
-            "angularVelocity": 0.10806806959180061,
-            "velocityX": 1.834410123608644,
-            "velocityY": -3.146382344808711,
-            "timestamp": 1.1114879654269723
-        },
-        {
-            "x": 4.627451956204509,
-            "y": 4.5560902871099245,
-            "heading": 3.2338916761611958,
-            "angularVelocity": -0.8875886953442504,
-            "velocityX": 1.9804245705663106,
-            "velocityY": -2.802365928032756,
-            "timestamp": 1.1668009603238243
-        },
-        {
-            "x": 4.751713919325069,
-            "y": 4.421499213962681,
-            "heading": 3.169974564154514,
-            "angularVelocity": -1.1555532678328913,
-            "velocityX": 2.246523865726009,
-            "velocityY": -2.433263167149585,
-            "timestamp": 1.2221139552206763
-        },
-        {
-            "x": 4.891049798654671,
-            "y": 4.307366204928411,
-            "heading": 3.098048256171184,
-            "angularVelocity": -1.3003509955925967,
-            "velocityX": 2.5190442063286667,
-            "velocityY": -2.063403170396147,
-            "timestamp": 1.2774269501175284
-        },
-        {
-            "x": 5.0446094980944505,
-            "y": 4.214363163694223,
-            "heading": 3.021068837736617,
-            "angularVelocity": -1.3917058473893738,
-            "velocityX": 2.7761957154216415,
-            "velocityY": -1.6813958710357464,
-            "timestamp": 1.3327399450143804
-        },
-        {
-            "x": 5.211468562298102,
-            "y": 4.143120621239898,
-            "heading": 2.9418200950482523,
-            "angularVelocity": -1.432732811451417,
-            "velocityX": 3.016634057056783,
-            "velocityY": -1.2879892435254758,
-            "timestamp": 1.3880529399112325
-        },
-        {
-            "x": 5.390789031982422,
-            "y": 4.09412145614624,
-            "heading": 2.865640793449113,
-            "angularVelocity": -1.3772405876991196,
-            "velocityX": 3.2419229878750393,
-            "velocityY": -0.8858526858838836,
-            "timestamp": 1.4433659348080845
-        },
-        {
-            "x": 5.535592658236132,
-            "y": 4.049337172847784,
-            "heading": 2.841642220937184,
-            "angularVelocity": -0.5722130304709164,
-            "velocityX": 3.452643767066762,
-            "velocityY": -1.0678197818199107,
-            "timestamp": 1.4853058614332775
-        },
-        {
-            "x": 5.681734895850587,
-            "y": 4.003264514213434,
-            "heading": 2.8233979117656802,
-            "angularVelocity": -0.4350105171749157,
-            "velocityX": 3.484561118108144,
-            "velocityY": -1.098539323783022,
-            "timestamp": 1.5272457880584704
-        },
-        {
-            "x": 5.827742551539107,
-            "y": 3.9572785916007636,
-            "heading": 2.804954082691072,
-            "angularVelocity": -0.4397677954812897,
-            "velocityX": 3.481352196758831,
-            "velocityY": -1.096471222366128,
-            "timestamp": 1.5691857146836634
-        },
-        {
-            "x": 5.9679573209760495,
-            "y": 3.908501280249766,
-            "heading": 2.7717324952229117,
-            "angularVelocity": -0.7921231661908814,
-            "velocityX": 3.3432287731451744,
-            "velocityY": -1.1630280564605782,
-            "timestamp": 1.6111256413088564
-        },
-        {
-            "x": 6.096940014064459,
-            "y": 3.8543767396001813,
-            "heading": 2.711734513417447,
-            "angularVelocity": -1.4305695463335533,
-            "velocityX": 3.07541532537948,
-            "velocityY": -1.2905254015650245,
-            "timestamp": 1.6530655679340494
-        },
-        {
-            "x": 6.215088187160598,
-            "y": 3.7918967611554555,
-            "heading": 2.632459037184743,
-            "angularVelocity": -1.890214948184595,
-            "velocityX": 2.8170810633981405,
-            "velocityY": -1.4897493503766595,
-            "timestamp": 1.6950054945592423
-        },
-        {
-            "x": 6.32424952389229,
-            "y": 3.717983083464812,
-            "heading": 2.5463998013121922,
-            "angularVelocity": -2.0519643880553406,
-            "velocityX": 2.6028022821127137,
-            "velocityY": -1.7623702194615716,
-            "timestamp": 1.7369454211844353
-        },
-        {
-            "x": 6.43630340548619,
-            "y": 3.6322698094657313,
-            "heading": 2.4886072976482807,
-            "angularVelocity": -1.3779829464268867,
-            "velocityX": 2.6717710451737124,
-            "velocityY": -2.043715401914737,
-            "timestamp": 1.7788853478096283
-        },
-        {
-            "x": 6.552059359988985,
-            "y": 3.5378523447078845,
-            "heading": 2.4573864362362134,
-            "angularVelocity": -0.7444185987992016,
-            "velocityX": 2.7600418936656252,
-            "velocityY": -2.251254886581771,
-            "timestamp": 1.8208252744348212
-        },
-        {
-            "x": 6.67032629558411,
-            "y": 3.432112823682954,
-            "heading": 2.4573457680499984,
-            "angularVelocity": -0.0009696770950155036,
-            "velocityX": 2.8199127922194083,
-            "velocityY": -2.521213782033988,
-            "timestamp": 1.8627652010600142
-        },
-        {
-            "x": 6.787202921818474,
-            "y": 3.3248215481814336,
-            "heading": 2.4573432508654287,
-            "angularVelocity": -0.00006001881195361165,
-            "velocityX": 2.7867627733081557,
-            "velocityY": -2.558213238195593,
-            "timestamp": 1.9047051276852072
-        },
-        {
-            "x": 6.904079764497574,
-            "y": 3.217530508972575,
-            "heading": 2.4573407325391345,
-            "angularVelocity": -0.000060046034804859665,
-            "velocityX": 2.7867679341358973,
-            "velocityY": -2.5582076041213115,
-            "timestamp": 1.9466450543104001
-        },
-        {
-            "x": 7.017539426879104,
-            "y": 3.1193532157248125,
-            "heading": 2.428286840001792,
-            "angularVelocity": -0.6927501995172902,
-            "velocityX": 2.7052899590285975,
-            "velocityY": -2.3409028376503684,
-            "timestamp": 1.988584980935593
-        },
-        {
-            "x": 7.118887938132189,
-            "y": 3.020968055873212,
-            "heading": 2.3710302150679574,
-            "angularVelocity": -1.365205653446252,
-            "velocityX": 2.4165161794108876,
-            "velocityY": -2.345859131582302,
-            "timestamp": 2.030524907560786
-        },
-        {
-            "x": 7.207551237715759,
-            "y": 2.9223074295194613,
-            "heading": 2.2882909026899103,
-            "angularVelocity": -1.9728053679604207,
-            "velocityX": 2.1140547139229047,
-            "velocityY": -2.3524272523282552,
-            "timestamp": 2.0724648341859786
-        },
-        {
-            "x": 7.28673413242529,
-            "y": 2.8318719270188626,
-            "heading": 2.1761461762136682,
-            "angularVelocity": -2.6739371167349164,
-            "velocityX": 1.8880074688058086,
-            "velocityY": -2.1563104606452623,
-            "timestamp": 2.1144047608111713
-        },
-        {
-            "x": 7.3579924776067,
-            "y": 2.7517426977667294,
-            "heading": 2.0378729016459984,
-            "angularVelocity": -3.2969364921256217,
-            "velocityX": 1.6990574594521515,
-            "velocityY": -1.9105715174046114,
-            "timestamp": 2.156344687436364
-        },
-        {
-            "x": 7.4224137082831785,
-            "y": 2.6828558926645387,
-            "heading": 1.874977709266495,
-            "angularVelocity": -3.884012335912244,
-            "velocityX": 1.5360358460374843,
-            "velocityY": -1.6425113405136766,
-            "timestamp": 2.198284614061557
-        },
-        {
-            "x": 7.480883603746462,
-            "y": 2.6255140291502124,
-            "heading": 1.6865689006450588,
-            "angularVelocity": -4.4923495051672475,
-            "velocityX": 1.3941344243592637,
-            "velocityY": -1.3672380504328716,
-            "timestamp": 2.2402245406867496
-        },
-        {
-            "x": 7.534437188323511,
-            "y": 2.579534136686106,
-            "heading": 1.4695142507662173,
-            "angularVelocity": -5.175370281846384,
-            "velocityX": 1.27691173748694,
-            "velocityY": -1.0963274417482318,
-            "timestamp": 2.2821644673119423
-        },
-        {
-            "x": 7.579534895925645,
-            "y": 2.5413039154996566,
-            "heading": 1.2172257697546927,
-            "angularVelocity": -6.015472636997345,
-            "velocityX": 1.0752929542572058,
-            "velocityY": -0.9115471643072651,
-            "timestamp": 2.324104393937135
-        },
-        {
-            "x": 7.615712715030757,
-            "y": 2.5097826728946813,
-            "heading": 0.9253708991340255,
-            "angularVelocity": -6.958878903840344,
-            "velocityX": 0.8626104530039771,
-            "velocityY": -0.7515807761580874,
-            "timestamp": 2.366044320562328
-        },
-        {
-            "x": 7.6401514150011085,
-            "y": 2.4865499215178333,
-            "heading": 0.6440959436378817,
-            "angularVelocity": -6.706615345559152,
-            "velocityX": 0.5827072657697844,
-            "velocityY": -0.5539530763721487,
-            "timestamp": 2.4079842471875206
-        },
-        {
-            "x": 7.656070937434432,
-            "y": 2.471863188851966,
-            "heading": 0.3945229348796906,
-            "angularVelocity": -5.950725927314207,
-            "velocityX": 0.37957916749813,
-            "velocityY": -0.3501849871393242,
-            "timestamp": 2.4499241738127133
-        },
-        {
-            "x": 7.668072859318959,
-            "y": 2.4642781587251137,
-            "heading": 0.19785094472919199,
-            "angularVelocity": -4.689373729909185,
-            "velocityX": 0.28616935818189254,
-            "velocityY": -0.18085463512222474,
-            "timestamp": 2.491864100437906
-        },
-        {
-            "x": 7.676136280306529,
-            "y": 2.459442710773055,
-            "heading": 0.06623696286278732,
-            "angularVelocity": -3.1381547956105695,
-            "velocityX": 0.19226120874341576,
-            "velocityY": -0.11529462116784107,
-            "timestamp": 2.533804027063099
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 2.1057025676027217e-33,
-            "angularVelocity": -1.57932948845455,
-            "velocityX": 0.09212509425682942,
-            "velocityY": -0.058243086471875524,
-            "timestamp": 2.5757439536882916
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -7.524501141381425e-34,
-            "angularVelocity": -5.907340858361994e-32,
-            "velocityX": -7.634877430876997e-33,
-            "velocityY": -7.90956322394541e-33,
-            "timestamp": 2.6176838803134843
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.1.traj
deleted file mode 100644
index cfa6b046..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.1.traj	
+++ /dev/null
@@ -1,625 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 6.892560738534845e-27,
-            "timestamp": 0
-        },
-        {
-            "x": 2.24821488195175,
-            "y": 5.550858605907564,
-            "heading": -1.0860081979744884e-8,
-            "angularVelocity": -3.249360952601713e-7,
-            "velocityX": -0.053411135206172675,
-            "velocityY": -0.27351257600550494,
-            "timestamp": 0.0334222075852833
-        },
-        {
-            "x": 2.2446446457975586,
-            "y": 5.532575817984525,
-            "heading": -3.253781151640391e-8,
-            "angularVelocity": -6.486025430910346e-7,
-            "velocityX": -0.10682227213842765,
-            "velocityY": -0.5470251441769277,
-            "timestamp": 0.0668444151705666
-        },
-        {
-            "x": 2.2392892913368425,
-            "y": 5.505151636566201,
-            "heading": -6.492851000422694e-8,
-            "angularVelocity": -9.691369834959166e-7,
-            "velocityX": -0.1602334150722015,
-            "velocityY": -0.8205377023156053,
-            "timestamp": 0.10026662275584991
-        },
-        {
-            "x": 2.2321488181807467,
-            "y": 5.468586062083933,
-            "heading": -1.0785000725838172e-7,
-            "angularVelocity": -0.000001284220850944888,
-            "velocityX": -0.21364456964054193,
-            "velocityY": -1.0940502475484135,
-            "timestamp": 0.1336888303411332
-        },
-        {
-            "x": 2.2232232256851456,
-            "y": 5.42287909509794,
-            "heading": -1.6102300741445552e-7,
-            "angularVelocity": -0.0000015909481231905412,
-            "velocityX": -0.26705574348127187,
-            "velocityY": -1.3675627760193463,
-            "timestamp": 0.16711103792641652
-        },
-        {
-            "x": 2.212512512848178,
-            "y": 5.368030736346982,
-            "heading": -2.2404505707142586e-7,
-            "angularVelocity": -0.0000018856338989468898,
-            "velocityX": -0.32046694729784664,
-            "velocityY": -1.6410752823864576,
-            "timestamp": 0.20053324551169982
-        },
-        {
-            "x": 2.200016678146418,
-            "y": 5.304040986826963,
-            "heading": -2.963548114247805e-7,
-            "angularVelocity": -0.000002163524111560841,
-            "velocityX": -0.3738781966956197,
-            "velocityY": -1.9145877589558649,
-            "timestamp": 0.23395545309698312
-        },
-        {
-            "x": 2.1857357192573095,
-            "y": 5.230909847922857,
-            "heading": -3.7718073304542066e-7,
-            "angularVelocity": -0.0000024183297502514874,
-            "velocityX": -0.4272895155847271,
-            "velocityY": -2.1881001940865983,
-            "timestamp": 0.2673776606822664
-        },
-        {
-            "x": 2.169669632551276,
-            "y": 5.148637321647842,
-            "heading": -4.654625982128869e-7,
-            "angularVelocity": -0.0000026414132559778984,
-            "velocityX": -0.48070094307146977,
-            "velocityY": -2.4616125689825754,
-            "timestamp": 0.30079986826754973
-        },
-        {
-            "x": 2.15181841205741,
-            "y": 5.057223411125392,
-            "heading": -5.597189955047307e-7,
-            "angularVelocity": -0.0000028201725975039026,
-            "velocityX": -0.5341125492089794,
-            "velocityY": -2.735124850423542,
-            "timestamp": 0.33422207585283303
-        },
-        {
-            "x": 2.1321820469941515,
-            "y": 4.956668121725741,
-            "heading": -6.57784320236276e-7,
-            "angularVelocity": -0.0000029341365576412115,
-            "velocityX": -0.5875244779329194,
-            "velocityY": -3.008636971185932,
-            "timestamp": 0.36764428343811634
-        },
-        {
-            "x": 2.1107605140888954,
-            "y": 4.846971464532025,
-            "heading": -7.56114109936986e-7,
-            "angularVelocity": -0.0000029420493990798148,
-            "velocityX": -0.6409371029779476,
-            "velocityY": -3.282148760338516,
-            "timestamp": 0.40106649102339964
-        },
-        {
-            "x": 2.087553735835974,
-            "y": 4.728133474365802,
-            "heading": -8.465523402036467e-7,
-            "angularVelocity": -0.0000027059321418483588,
-            "velocityX": -0.6943520470083785,
-            "velocityY": -3.5556595076225634,
-            "timestamp": 0.43448869860868294
-        },
-        {
-            "x": 2.0633251233427066,
-            "y": 4.604042454619442,
-            "heading": -8.465523885072994e-7,
-            "angularVelocity": -1.4452562228577176e-12,
-            "velocityX": -0.724925558303899,
-            "velocityY": -3.712831339152857,
-            "timestamp": 0.46791090619396625
-        },
-        {
-            "x": 2.039088654512122,
-            "y": 4.479952969280304,
-            "heading": -8.465523654496814e-7,
-            "angularVelocity": 6.898891300327124e-13,
-            "velocityX": -0.725160621670614,
-            "velocityY": -3.7127854293461913,
-            "timestamp": 0.5013331137792496
-        },
-        {
-            "x": 2.0148473415026222,
-            "y": 4.355864430051467,
-            "heading": -8.465523174606992e-7,
-            "angularVelocity": 1.4358411666699279e-12,
-            "velocityX": -0.7253055606758912,
-            "velocityY": -3.712757121497041,
-            "timestamp": 0.5347553213645329
-        },
-        {
-            "x": 1.9907968545860597,
-            "y": 4.231738764257757,
-            "heading": -8.465522534990754e-7,
-            "angularVelocity": 1.9137462023851052e-12,
-            "velocityX": -0.7195960008785613,
-            "velocityY": -3.7138679567028667,
-            "timestamp": 0.5681775289498161
-        },
-        {
-            "x": 1.9669031646192998,
-            "y": 4.107582820499775,
-            "heading": -8.465521766194915e-7,
-            "angularVelocity": 2.3002544763334585e-12,
-            "velocityX": -0.7149046006511871,
-            "velocityY": -3.7147738802470687,
-            "timestamp": 0.6015997365350993
-        },
-        {
-            "x": 1.9504985155072192,
-            "y": 3.982217377752346,
-            "heading": -8.465520941748615e-7,
-            "angularVelocity": 2.4667619198655976e-12,
-            "velocityX": -0.4908308067350749,
-            "velocityY": -3.7509623631996885,
-            "timestamp": 0.6350219441203826
-        },
-        {
-            "x": 1.94330794196602,
-            "y": 3.855987816020002,
-            "heading": -8.465520093116816e-7,
-            "angularVelocity": 2.5391254773053977e-12,
-            "velocityX": -0.2151435844765948,
-            "velocityY": -3.7768169984060798,
-            "timestamp": 0.6684441517056658
-        },
-        {
-            "x": 1.9454293697222904,
-            "y": 3.7295714172218672,
-            "heading": -8.465519250884413e-7,
-            "angularVelocity": 2.519978339764665e-12,
-            "velocityX": 0.0634735976376205,
-            "velocityY": -3.7824072056149025,
-            "timestamp": 0.7018663592909491
-        },
-        {
-            "x": 1.9568513812515056,
-            "y": 3.603654207925799,
-            "heading": -8.465518451486746e-7,
-            "angularVelocity": 2.391815832767542e-12,
-            "velocityX": 0.34174916483985474,
-            "velocityY": -3.767471342961152,
-            "timestamp": 0.7352885668762323
-        },
-        {
-            "x": 1.9775120291721782,
-            "y": 3.4789195174340257,
-            "heading": -8.465517739710846e-7,
-            "angularVelocity": 2.129649524174011e-12,
-            "velocityX": 0.6181712523959566,
-            "velocityY": -3.7320901132416737,
-            "timestamp": 0.7687107744615156
-        },
-        {
-            "x": 2.0072992100178335,
-            "y": 3.3560442616931043,
-            "heading": -8.465517105809759e-7,
-            "angularVelocity": 1.8966463157464128e-12,
-            "velocityX": 0.8912391789143821,
-            "velocityY": -3.6764554055067418,
-            "timestamp": 0.8021329820467988
-        },
-        {
-            "x": 2.046051283468891,
-            "y": 3.235695267642527,
-            "heading": -8.465516527951994e-7,
-            "angularVelocity": 1.7289634182989629e-12,
-            "velocityX": 1.1594707905613406,
-            "velocityY": -3.600869085126494,
-            "timestamp": 0.8355551896320821
-        },
-        {
-            "x": 2.0935579534966013,
-            "y": 3.1185256537724477,
-            "heading": -8.46551596321545e-7,
-            "angularVelocity": 1.6897044173558387e-12,
-            "velocityX": 1.421410297528723,
-            "velocityY": -3.5057413120014775,
-            "timestamp": 0.8689773972173653
-        },
-        {
-            "x": 2.1495614051818848,
-            "y": 3.005171298980713,
-            "heading": -8.465515057383081e-7,
-            "angularVelocity": 2.7102707524537993e-12,
-            "velocityX": 1.6756359238847198,
-            "velocityY": -3.3915878986274275,
-            "timestamp": 0.9023996048026486
-        },
-        {
-            "x": 2.288743031901554,
-            "y": 2.8026748842621148,
-            "heading": -8.46551464739476e-7,
-            "angularVelocity": 6.312009526868921e-13,
-            "velocityX": 2.142782210187666,
-            "velocityY": -3.117550249366458,
-            "timestamp": 0.9673533019435205
-        },
-        {
-            "x": 2.455414708004788,
-            "y": 2.6221288200825,
-            "heading": -8.46551435217149e-7,
-            "angularVelocity": 4.5451345461171864e-13,
-            "velocityX": 2.566007532127087,
-            "velocityY": -2.779611817758129,
-            "timestamp": 1.0323069990843925
-        },
-        {
-            "x": 2.646160177401748,
-            "y": 2.4672337011827246,
-            "heading": -8.465514075797998e-7,
-            "angularVelocity": 4.254931139136813e-13,
-            "velocityX": 2.9366376017527633,
-            "velocityY": -2.384700574689416,
-            "timestamp": 1.0972606962252645
-        },
-        {
-            "x": 2.857069726290126,
-            "y": 2.341164292711998,
-            "heading": -8.465513787578765e-7,
-            "angularVelocity": 4.4373032078954734e-13,
-            "velocityX": 3.2470753501649696,
-            "velocityY": -1.9409119729950552,
-            "timestamp": 1.1622143933661364
-        },
-        {
-            "x": 3.0838173053505575,
-            "y": 2.2464971410417167,
-            "heading": -8.465513471236816e-7,
-            "angularVelocity": 4.870268741320482e-13,
-            "velocityX": 3.490911049579749,
-            "velocityY": -1.457455939200772,
-            "timestamp": 1.2271680905070084
-        },
-        {
-            "x": 3.3145112489857773,
-            "y": 2.1619002222864983,
-            "heading": -8.465513138187862e-7,
-            "angularVelocity": 5.127482864627404e-13,
-            "velocityX": 3.5516676307835056,
-            "velocityY": -1.3024188380298123,
-            "timestamp": 1.2921217876478803
-        },
-        {
-            "x": 3.5452013267301767,
-            "y": 2.0772927620920076,
-            "heading": -8.465512839726007e-7,
-            "angularVelocity": 4.594994306997602e-13,
-            "velocityX": 3.5516081131487938,
-            "velocityY": -1.302581129610464,
-            "timestamp": 1.3570754847887523
-        },
-        {
-            "x": 3.77589133724735,
-            "y": 1.9926851185956267,
-            "heading": -8.465512625373994e-7,
-            "angularVelocity": 3.3000743037279784e-13,
-            "velocityX": 3.5516070781466733,
-            "velocityY": -1.302583951649345,
-            "timestamp": 1.4220291819296242
-        },
-        {
-            "x": 4.006581361028944,
-            "y": 1.9080775112659911,
-            "heading": -8.465512535495126e-7,
-            "angularVelocity": 1.3837377280671698e-13,
-            "velocityX": 3.5516072823601217,
-            "velocityY": -1.302583394841325,
-            "timestamp": 1.4869828790704962
-        },
-        {
-            "x": 4.2372714040114055,
-            "y": 1.823469956289169,
-            "heading": -8.465512575529088e-7,
-            "angularVelocity": -6.163459506326445e-14,
-            "velocityX": 3.551607577968653,
-            "velocityY": -1.3025825888392473,
-            "timestamp": 1.5519365762113682
-        },
-        {
-            "x": 4.467961461705261,
-            "y": 1.7388624414245573,
-            "heading": -8.465512722492451e-7,
-            "angularVelocity": -2.2625863777166434e-13,
-            "velocityX": 3.5516078044591044,
-            "velocityY": -1.3025819712885556,
-            "timestamp": 1.6168902733522401
-        },
-        {
-            "x": 4.698651553926265,
-            "y": 1.6542550207002664,
-            "heading": -8.465512926518018e-7,
-            "angularVelocity": -3.141092229421582e-13,
-            "velocityX": 3.551608336024854,
-            "velocityY": -1.3025805219428788,
-            "timestamp": 1.681843970493112
-        },
-        {
-            "x": 4.929586887359619,
-            "y": 1.5703192949295044,
-            "heading": -8.465513064459796e-7,
-            "angularVelocity": -2.1236938120360531e-13,
-            "velocityX": 3.555383967330626,
-            "velocityY": -1.2922393869021438,
-            "timestamp": 1.746797667633984
-        },
-        {
-            "x": 5.059495672066219,
-            "y": 1.5327262216225606,
-            "heading": -8.465512750899748e-7,
-            "angularVelocity": 8.770995258558909e-13,
-            "velocityX": 3.633847296905686,
-            "velocityY": -1.051564666141291,
-            "timestamp": 1.7825473219513848
-        },
-        {
-            "x": 5.189548300230332,
-            "y": 1.4956338313822377,
-            "heading": -8.465512186056332e-7,
-            "angularVelocity": 1.579996862146135e-12,
-            "velocityX": 3.637870929029155,
-            "velocityY": -1.0375594099736098,
-            "timestamp": 1.8182969762687855
-        },
-        {
-            "x": 5.319623170079142,
-            "y": 1.4586195133360007,
-            "heading": -8.46551136487956e-7,
-            "angularVelocity": 2.2970201754243682e-12,
-            "velocityX": 3.638493080071459,
-            "velocityY": -1.0353755512600933,
-            "timestamp": 1.8540466305861862
-        },
-        {
-            "x": 5.4496989055100356,
-            "y": 1.42160823723968,
-            "heading": -8.465510387564672e-7,
-            "angularVelocity": 2.7337743725450157e-12,
-            "velocityX": 3.6385172923918394,
-            "velocityY": -1.0352904609291835,
-            "timestamp": 1.889796284903587
-        },
-        {
-            "x": 5.57977446957584,
-            "y": 1.3845963588884114,
-            "heading": -8.465509335637302e-7,
-            "angularVelocity": 2.9424826347181554e-12,
-            "velocityX": 3.6385124989164392,
-            "velocityY": -1.03530730738433,
-            "timestamp": 1.9255459392209877
-        },
-        {
-            "x": 5.709850057975644,
-            "y": 1.3475845660573411,
-            "heading": -8.465508277220688e-7,
-            "angularVelocity": 2.960634540295608e-12,
-            "velocityX": 3.6385131795943324,
-            "velocityY": -1.0353049151878095,
-            "timestamp": 1.9612955935383884
-        },
-        {
-            "x": 5.839925662966751,
-            "y": 1.3105728315354166,
-            "heading": -8.465507264753305e-7,
-            "angularVelocity": 2.832103981671134e-12,
-            "velocityX": 3.6385136436912036,
-            "velocityY": -1.0353032841469862,
-            "timestamp": 1.9970452478557892
-        },
-        {
-            "x": 5.970001275498046,
-            "y": 1.273561123513075,
-            "heading": -8.465506337302748e-7,
-            "angularVelocity": 2.594292378130792e-12,
-            "velocityX": 3.638513854607602,
-            "velocityY": -1.0353025428927405,
-            "timestamp": 2.03279490217319
-        },
-        {
-            "x": 6.100076894564937,
-            "y": 1.2365494384597497,
-            "heading": -8.46550552278632e-7,
-            "angularVelocity": 2.2783896618382965e-12,
-            "velocityX": 3.6385140374232323,
-            "velocityY": -1.0353019003965735,
-            "timestamp": 2.0685445564905907
-        },
-        {
-            "x": 6.230152520105601,
-            "y": 1.1995377761581834,
-            "heading": -8.465504825990077e-7,
-            "angularVelocity": 1.9490992420524453e-12,
-            "velocityX": 3.6385142185095396,
-            "velocityY": -1.03530126397757,
-            "timestamp": 2.1042942108079914
-        },
-        {
-            "x": 6.3602281516940815,
-            "y": 1.16252613511139,
-            "heading": -8.465504225140753e-7,
-            "angularVelocity": 1.680713660601537e-12,
-            "velocityX": 3.6385143876808734,
-            "velocityY": -1.0353006694327234,
-            "timestamp": 2.140043865125392
-        },
-        {
-            "x": 6.490303788775648,
-            "y": 1.1255145133698101,
-            "heading": -8.465503700121188e-7,
-            "angularVelocity": 1.4686003981542915e-12,
-            "velocityX": 3.638514541335137,
-            "velocityY": -1.0353001294215285,
-            "timestamp": 2.175793519442793
-        },
-        {
-            "x": 6.62037943067546,
-            "y": 1.0885029085617548,
-            "heading": -8.465503232940394e-7,
-            "angularVelocity": 1.306812060469801e-12,
-            "velocityX": 3.638514676112538,
-            "velocityY": -1.0352996557519205,
-            "timestamp": 2.2115431737601936
-        },
-        {
-            "x": 6.7504550766965234,
-            "y": 1.0514913182376744,
-            "heading": -8.465502808582129e-7,
-            "angularVelocity": 1.187027603302574e-12,
-            "velocityX": 3.638514791393393,
-            "velocityY": -1.0352992506018621,
-            "timestamp": 2.2472928280775943
-        },
-        {
-            "x": 6.880530727542945,
-            "y": 1.0144797448710672,
-            "heading": -8.465502433332782e-7,
-            "angularVelocity": 1.0496586699357012e-12,
-            "velocityX": 3.6385149263697616,
-            "velocityY": -1.0352987762623522,
-            "timestamp": 2.283042482394995
-        },
-        {
-            "x": 7.003525953233288,
-            "y": 0.9794828656529073,
-            "heading": -7.145353891597533e-7,
-            "angularVelocity": 0.000003692758900357643,
-            "velocityX": 3.440459160761053,
-            "velocityY": -0.9789431502593731,
-            "timestamp": 2.318792136712396
-        },
-        {
-            "x": 7.116271603762692,
-            "y": 0.9474023877264399,
-            "heading": -5.944969754078434e-7,
-            "angularVelocity": 0.0000033577503345576707,
-            "velocityX": 3.153754985388111,
-            "velocityY": -0.8973647029323183,
-            "timestamp": 2.3545417910297965
-        },
-        {
-            "x": 7.218767661219051,
-            "y": 0.9182383156679585,
-            "heading": -4.859087151281335e-7,
-            "angularVelocity": 0.0000030374632243726353,
-            "velocityX": 2.867050308972391,
-            "velocityY": -0.8157861275958178,
-            "timestamp": 2.3902914453471973
-        },
-        {
-            "x": 7.311014119553375,
-            "y": 0.8919906507276494,
-            "heading": -3.8850367492824467e-7,
-            "angularVelocity": 0.000002724642854703664,
-            "velocityX": 2.580345463352478,
-            "velocityY": -0.7342075172887301,
-            "timestamp": 2.426041099664598
-        },
-        {
-            "x": 7.393010975686544,
-            "y": 0.8686593933386082,
-            "heading": -3.0210953963079075e-7,
-            "angularVelocity": 0.0000024166425366030095,
-            "velocityX": 2.293640531602499,
-            "velocityY": -0.6526288948669626,
-            "timestamp": 2.4617907539819988
-        },
-        {
-            "x": 7.464758227730078,
-            "y": 0.8482445436165693,
-            "heading": -2.2661033918382436e-7,
-            "angularVelocity": 0.000002111886167406066,
-            "velocityX": 2.006935547027439,
-            "velocityY": -0.5710502692078399,
-            "timestamp": 2.4975404082993995
-        },
-        {
-            "x": 7.526255874392654,
-            "y": 0.8307461015250325,
-            "heading": -1.6193015142138781e-7,
-            "angularVelocity": 0.0000018092535156938384,
-            "velocityX": 1.7202305263310733,
-            "velocityY": -0.48947164456971237,
-            "timestamp": 2.5332900626168002
-        },
-        {
-            "x": 7.577503914725456,
-            "y": 0.8161640669449958,
-            "heading": -1.0802379214331377e-7,
-            "angularVelocity": 0.0000015078847699136293,
-            "velocityX": 1.4335254790941394,
-            "velocityY": -0.4078930232603403,
-            "timestamp": 2.569039716934201
-        },
-        {
-            "x": 7.618502347994633,
-            "y": 0.8044984397090525,
-            "heading": -6.487055743300418e-8,
-            "angularVelocity": 0.0000012070951602673883,
-            "velocityX": 1.1468204113297347,
-            "velocityY": -0.3263144066337203,
-            "timestamp": 2.6047893712516017
-        },
-        {
-            "x": 7.649251173610278,
-            "y": 0.7957492196197445,
-            "heading": -3.246962719134405e-8,
-            "angularVelocity": 9.063284906782187e-7,
-            "velocityX": 0.8601153270642448,
-            "velocityY": -0.24473579553046104,
-            "timestamp": 2.6405390255690024
-        },
-        {
-            "x": 7.669750391083677,
-            "y": 0.7899164064601075,
-            "heading": -1.0836483282929717e-8,
-            "angularVelocity": 6.051287577519143e-7,
-            "velocityX": 0.573410229128317,
-            "velocityY": -0.16315719049619,
-            "timestamp": 2.676288679886403
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 2.0494644799121527e-30,
-            "angularVelocity": 3.031213445055282e-7,
-            "velocityX": 0.28670511958863415,
-            "velocityY": -0.08157859189950141,
-            "timestamp": 2.712038334203804
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -1.7051999701665118e-29,
-            "angularVelocity": -2.5846596071248955e-28,
-            "velocityX": 4.524644517512267e-29,
-            "velocityY": 1.710870028377411e-29,
-            "timestamp": 2.7477879885212046
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.1.traj
deleted file mode 100644
index 2aaf78f3..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 4.4337460853781175e-35,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3070751783284065,
-            "y": 5.580871519986164,
-            "heading": 0.00875997642576413,
-            "angularVelocity": 0.10256751513529143,
-            "velocityX": 0.6682733985254588,
-            "velocityY": 0.24437736336610263,
-            "timestamp": 0.08540692844327274
-        },
-        {
-            "x": 2.421225525991661,
-            "y": 5.622614560146346,
-            "heading": 0.026280802198455293,
-            "angularVelocity": 0.20514525100066663,
-            "velocityX": 1.3365466917484683,
-            "velocityY": 0.48875472893171557,
-            "timestamp": 0.1708138568865455
-        },
-        {
-            "x": 2.592451026856299,
-            "y": 5.685229121649564,
-            "heading": 0.052563939294491345,
-            "angularVelocity": 0.30774010463909035,
-            "velocityX": 2.0048197960703633,
-            "velocityY": 0.7331321081849562,
-            "timestamp": 0.25622078532981823
-        },
-        {
-            "x": 2.8207516562305845,
-            "y": 5.768715208168554,
-            "heading": 0.0876113178799828,
-            "angularVelocity": 0.41035755792072426,
-            "velocityX": 2.673092611285307,
-            "velocityY": 0.9775095304409794,
-            "timestamp": 0.341627713773091
-        },
-        {
-            "x": 3.106127374237044,
-            "y": 5.873072825511933,
-            "heading": 0.13142500230326418,
-            "angularVelocity": 0.5129991819385286,
-            "velocityX": 3.341364959588794,
-            "velocityY": 1.221887020708095,
-            "timestamp": 0.4270346422163637
-        },
-        {
-            "x": 3.40956412390744,
-            "y": 5.984036121763709,
-            "heading": 0.13142500754936084,
-            "angularVelocity": 6.142471997127927e-8,
-            "velocityX": 3.5528352933560723,
-            "velocityY": 1.299230615997127,
-            "timestamp": 0.5124415706596365
-        },
-        {
-            "x": 3.7130008735619784,
-            "y": 6.094999418058892,
-            "heading": 0.1314250127954367,
-            "angularVelocity": 6.142447634082945e-8,
-            "velocityX": 3.5528352931703995,
-            "velocityY": 1.2992306165053664,
-            "timestamp": 0.5978484991029092
-        },
-        {
-            "x": 4.016437623216516,
-            "y": 6.205962714354075,
-            "heading": 0.13142501804151255,
-            "angularVelocity": 6.142447606684555e-8,
-            "velocityX": 3.552835293170399,
-            "velocityY": 1.2992306165053664,
-            "timestamp": 0.683255427546182
-        },
-        {
-            "x": 4.3198743728836435,
-            "y": 6.316926010614789,
-            "heading": 0.13142502328760913,
-            "angularVelocity": 6.142471942956278e-8,
-            "velocityX": 3.5528352933178016,
-            "velocityY": 1.2992306161017806,
-            "timestamp": 0.7686623559894548
-        },
-        {
-            "x": 4.605249584503867,
-            "y": 6.421283973215489,
-            "heading": 0.17528171773503587,
-            "angularVelocity": 0.5135027713419801,
-            "velocityX": 3.3413590304886087,
-            "velocityY": 1.2218910632059061,
-            "timestamp": 0.8540692844327276
-        },
-        {
-            "x": 4.83354975240058,
-            "y": 6.5047703704067485,
-            "heading": 0.21036850612417926,
-            "angularVelocity": 0.4108189935954435,
-            "velocityX": 2.6730872080050085,
-            "velocityY": 0.9775131679944534,
-            "timestamp": 0.9394762128760004
-        },
-        {
-            "x": 5.004774875721697,
-            "y": 6.567385181417454,
-            "heading": 0.23668415165777779,
-            "angularVelocity": 0.3081207346202291,
-            "velocityX": 2.0048153755446823,
-            "velocityY": 0.7331350295812797,
-            "timestamp": 1.0248831413192732
-        },
-        {
-            "x": 5.118924957871047,
-            "y": 6.6091283935214395,
-            "heading": 0.25422806054578806,
-            "angularVelocity": 0.20541552316405948,
-            "velocityX": 1.3365435829385781,
-            "velocityY": 0.48875674216187737,
-            "timestamp": 1.110290069762546
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": 0.10270758607175808,
-            "velocityX": 0.6682718038134577,
-            "velocityY": 0.2443783760754632,
-            "timestamp": 1.1956969982058188
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": 1.86588452749739e-32,
-            "velocityX": -1.0770205854918045e-34,
-            "velocityY": 0,
-            "timestamp": 1.2811039266490916
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.1.traj
deleted file mode 100644
index 96b0e72c..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.1.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3319036557469652,
-            "y": 5.56,
-            "heading": -1.815106931159875e-19,
-            "angularVelocity": -1.831407645203353e-18,
-            "velocityX": 0.8263919813283321,
-            "velocityY": 8.319303153097966e-34,
-            "timestamp": 0.099109935233537
-        },
-        {
-            "x": 2.4957109641637625,
-            "y": 5.56,
-            "heading": -5.438547016393454e-19,
-            "angularVelocity": -3.655980680963076e-18,
-            "velocityX": 1.6527839316089838,
-            "velocityY": 1.3987786573815764e-33,
-            "timestamp": 0.198219870467074
-        },
-        {
-            "x": 2.741421919096125,
-            "y": 5.56,
-            "heading": -1.0860329369861945e-18,
-            "angularVelocity": -5.4704731072674304e-18,
-            "velocityX": 2.4791758197942864,
-            "velocityY": 2.272018181833636e-33,
-            "timestamp": 0.297329805700611
-        },
-        {
-            "x": 3.0690365020812695,
-            "y": 5.56,
-            "heading": -1.80338172905721e-18,
-            "angularVelocity": -7.237910009033217e-18,
-            "velocityX": 3.3055675216936846,
-            "velocityY": 3.0574601440031788e-33,
-            "timestamp": 0.396439740934148
-        },
-        {
-            "x": 3.4439634979187304,
-            "y": 5.56,
-            "heading": 1.7928981709293245e-18,
-            "angularVelocity": 3.6285765816640505e-17,
-            "velocityX": 3.7829405796099502,
-            "velocityY": 2.997011014948752e-33,
-            "timestamp": 0.495549676167685
-        },
-        {
-            "x": 3.771578080903875,
-            "y": 5.56,
-            "heading": 1.0808328664707184e-18,
-            "angularVelocity": -7.184600643588333e-18,
-            "velocityX": 3.305567521693685,
-            "velocityY": -4.3766441461200414e-33,
-            "timestamp": 0.594659611401222
-        },
-        {
-            "x": 4.017289035836238,
-            "y": 5.56,
-            "heading": 5.416591039281022e-19,
-            "angularVelocity": -5.4401585595028644e-18,
-            "velocityX": 2.479175819794286,
-            "velocityY": -5.119323199602909e-33,
-            "timestamp": 0.6937695466347591
-        },
-        {
-            "x": 4.181096344253034,
-            "y": 5.56,
-            "heading": 1.8086099379948042e-19,
-            "angularVelocity": -3.64038286599245e-18,
-            "velocityX": 1.6527839316089838,
-            "velocityY": -5.9637817525889115e-33,
-            "timestamp": 0.792879481868296
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -1.8248523050897816e-18,
-            "velocityX": 0.8263919813283321,
-            "velocityY": -3.8230331267939286e-33,
-            "timestamp": 0.891989417101833
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.0684896902092895e-40,
-            "timestamp": 0.9910993523353699
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.1.traj
deleted file mode 100644
index 7d72e43b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.1.traj	
+++ /dev/null
@@ -1,526 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -4.99624931676796e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2447003077480563,
-            "y": 5.548424158968356,
-            "heading": -0.001651505925702968,
-            "angularVelocity": -0.042250731032121805,
-            "velocityX": -0.13558284496887443,
-            "velocityY": -0.2961465280181771,
-            "timestamp": 0.03908822132445813
-        },
-        {
-            "x": 2.234100924566356,
-            "y": 5.525272476708423,
-            "heading": -0.0049545293965937804,
-            "angularVelocity": -0.08450175932731885,
-            "velocityX": -0.27116565611205723,
-            "velocityY": -0.5922930610671583,
-            "timestamp": 0.07817644264891627
-        },
-        {
-            "x": 2.2182018518980375,
-            "y": 5.490544953203296,
-            "heading": -0.009909100000268336,
-            "angularVelocity": -0.12675354456654908,
-            "velocityX": -0.4067484303351874,
-            "velocityY": -0.8884395945486457,
-            "timestamp": 0.1172646639733744
-        },
-        {
-            "x": 2.1970030913105703,
-            "y": 5.444241588640874,
-            "heading": -0.016515264878529855,
-            "angularVelocity": -0.16900653584180803,
-            "velocityX": -0.5423311644575062,
-            "velocityY": -1.1845861232230646,
-            "timestamp": 0.15635288529783253
-        },
-        {
-            "x": 2.170504644501127,
-            "y": 5.386362383452732,
-            "heading": -0.024773088013563174,
-            "angularVelocity": -0.21126116398317848,
-            "velocityX": -0.6779138551608025,
-            "velocityY": -1.480732640856411,
-            "timestamp": 0.19544110662229067
-        },
-        {
-            "x": 2.1387065133083243,
-            "y": 5.31690733837683,
-            "heading": -0.03468264893636917,
-            "angularVelocity": -0.2535178267784212,
-            "velocityX": -0.8134964988264785,
-            "velocityY": -1.7768791396104282,
-            "timestamp": 0.2345293279467488
-        },
-        {
-            "x": 2.1016086997403236,
-            "y": 5.235876454566401,
-            "heading": -0.04624404029676127,
-            "angularVelocity": -0.2957768598481422,
-            "velocityX": -0.9490790911170489,
-            "velocityY": -2.0730256088611454,
-            "timestamp": 0.27361754927120696
-        },
-        {
-            "x": 2.0592112060450187,
-            "y": 5.143269733801358,
-            "heading": -0.05945736298631538,
-            "angularVelocity": -0.33803847404262444,
-            "velocityX": -1.0846616258994186,
-            "velocityY": -2.369172032575987,
-            "timestamp": 0.31270577059566507
-        },
-        {
-            "x": 2.011514034901109,
-            "y": 5.039087178972333,
-            "heading": -0.07432271508161363,
-            "angularVelocity": -0.3803025973454465,
-            "velocityX": -1.2202440921521842,
-            "velocityY": -2.6653183823394446,
-            "timestamp": 0.3517939919201232
-        },
-        {
-            "x": 1.9585171900495353,
-            "y": 4.923328795522167,
-            "heading": -0.09084016031123929,
-            "angularVelocity": -0.4225683510264515,
-            "velocityX": -1.3558264626999135,
-            "velocityY": -2.9614645928578263,
-            "timestamp": 0.3908822132445813
-        },
-        {
-            "x": 1.9002206796290688,
-            "y": 4.795994598697166,
-            "heading": -0.10900958241827326,
-            "angularVelocity": -0.46483113048874997,
-            "velocityX": -1.491408625032185,
-            "velocityY": -3.257610413327559,
-            "timestamp": 0.4299704345690394
-        },
-        {
-            "x": 1.8386671955063139,
-            "y": 4.661546724325594,
-            "heading": -0.10900958703871873,
-            "angularVelocity": -1.182055697467884e-7,
-            "velocityX": -1.5747322860207698,
-            "velocityY": -3.439600724104782,
-            "timestamp": 0.4690586558934975
-        },
-        {
-            "x": 1.7777943297173011,
-            "y": 4.526789325478927,
-            "heading": -0.10900959166091434,
-            "angularVelocity": -1.1825034389504455e-7,
-            "velocityX": -1.5573199221351706,
-            "velocityY": -3.447519336526256,
-            "timestamp": 0.5081468772179556
-        },
-        {
-            "x": 1.728739704549767,
-            "y": 4.38729485969642,
-            "heading": -0.10900959636164997,
-            "angularVelocity": -1.2025964513922604e-7,
-            "velocityX": -1.2549720479816096,
-            "velocityY": -3.5687084511883413,
-            "timestamp": 0.5472350985424137
-        },
-        {
-            "x": 1.6918738865194354,
-            "y": 4.244095790476986,
-            "heading": -0.10900960128999353,
-            "angularVelocity": -1.2608257379035844e-7,
-            "velocityX": -0.9431439134643449,
-            "velocityY": -3.6634838927766116,
-            "timestamp": 0.5863233198668718
-        },
-        {
-            "x": 1.6674707596733624,
-            "y": 4.098254956364247,
-            "heading": -0.10900960662650709,
-            "angularVelocity": -1.3652485001388814e-7,
-            "velocityX": -0.6243089610931135,
-            "velocityY": -3.7310685718382324,
-            "timestamp": 0.6254115411913299
-        },
-        {
-            "x": 1.6557115284805652,
-            "y": 3.9508548842430677,
-            "heading": -0.1090096126146074,
-            "angularVelocity": -1.531944945007552e-7,
-            "velocityX": -0.3008382268193907,
-            "velocityY": -3.7709587985000237,
-            "timestamp": 0.664499762515788
-        },
-        {
-            "x": 1.6566835066189385,
-            "y": 3.8029896922639588,
-            "heading": -0.10900961961577929,
-            "angularVelocity": -1.79112061302596e-7,
-            "velocityX": 0.024866266754091815,
-            "velocityY": -3.7828580316236344,
-            "timestamp": 0.7035879838402461
-        },
-        {
-            "x": 1.6703794917330834,
-            "y": 3.6557569809887474,
-            "heading": -0.10900969829200789,
-            "angularVelocity": -0.000002012786100988185,
-            "velocityX": 0.3503865013576149,
-            "velocityY": -3.766677180142832,
-            "timestamp": 0.7426762051647042
-        },
-        {
-            "x": 1.6964297342683634,
-            "y": 3.5114772969924033,
-            "heading": -0.1124967438020359,
-            "angularVelocity": -0.0892096235606636,
-            "velocityX": 0.6664473760284892,
-            "velocityY": -3.6911294274233937,
-            "timestamp": 0.7817644264891623
-        },
-        {
-            "x": 1.7337062034817727,
-            "y": 3.3729422790966246,
-            "heading": -0.12243166775651844,
-            "angularVelocity": -0.2541666931326176,
-            "velocityX": 0.9536496660724848,
-            "velocityY": -3.54416274779685,
-            "timestamp": 0.8208526478136204
-        },
-        {
-            "x": 1.7809992442628195,
-            "y": 3.2421845220219927,
-            "heading": -0.1367067464310455,
-            "angularVelocity": -0.3652015413039525,
-            "velocityX": 1.2099051626962352,
-            "velocityY": -3.3451958836719715,
-            "timestamp": 0.8599408691380785
-        },
-        {
-            "x": 1.8374227958909133,
-            "y": 3.120270451905998,
-            "heading": -0.154016140064332,
-            "angularVelocity": -0.4428288892862246,
-            "velocityX": 1.443492431128254,
-            "velocityY": -3.118946475052685,
-            "timestamp": 0.8990290904625367
-        },
-        {
-            "x": 1.9023445230612375,
-            "y": 3.007828092475748,
-            "heading": -0.17349166731783877,
-            "angularVelocity": -0.4982454200677405,
-            "velocityX": 1.660902567846671,
-            "velocityY": -2.8766302384778886,
-            "timestamp": 0.9381173117869948
-        },
-        {
-            "x": 1.9753011558084586,
-            "y": 2.90526251520286,
-            "heading": -0.19452421295735298,
-            "angularVelocity": -0.5380788617862533,
-            "velocityX": 1.866460797529241,
-            "velocityY": -2.623950995915806,
-            "timestamp": 0.9772055331114529
-        },
-        {
-            "x": 2.0559421744907826,
-            "y": 2.8128534355639654,
-            "heading": -0.21666661660279088,
-            "angularVelocity": -0.5664725304751266,
-            "velocityX": 2.0630516291077963,
-            "velocityY": -2.3641157491367464,
-            "timestamp": 1.016293754435911
-        },
-        {
-            "x": 2.1439944855858766,
-            "y": 2.7308042438703497,
-            "heading": -0.2395781801271676,
-            "angularVelocity": -0.586150066389446,
-            "velocityX": 2.252655866948419,
-            "velocityY": -2.099077136627731,
-            "timestamp": 1.055381975760369
-        },
-        {
-            "x": 2.2392399311065674,
-            "y": 2.659268856048584,
-            "heading": -0.2629913490343549,
-            "angularVelocity": -0.5989827143282399,
-            "velocityX": 2.4366789353272003,
-            "velocityY": -1.830100869209989,
-            "timestamp": 1.0944701970848272
-        },
-        {
-            "x": 2.3166964235934535,
-            "y": 2.610513248595422,
-            "heading": -0.2811797949710667,
-            "angularVelocity": -0.6046182408202432,
-            "velocityX": 2.5747998696792043,
-            "velocityY": -1.620728330008343,
-            "timestamp": 1.124552726371468
-        },
-        {
-            "x": 2.398213254515867,
-            "y": 2.5681176642029904,
-            "heading": -0.29946946527559826,
-            "angularVelocity": -0.6079831296849904,
-            "velocityX": 2.7097731758410535,
-            "velocityY": -1.409309170398157,
-            "timestamp": 1.154635255658109
-        },
-        {
-            "x": 2.483678189488588,
-            "y": 2.5321524360945276,
-            "heading": -0.31778023201379,
-            "angularVelocity": -0.6086844149213316,
-            "velocityX": 2.8410155993982453,
-            "velocityY": -1.1955520018204469,
-            "timestamp": 1.1847177849447499
-        },
-        {
-            "x": 2.5729562458095865,
-            "y": 2.5026984837196036,
-            "heading": -0.33601688083426684,
-            "angularVelocity": -0.6062205955726221,
-            "velocityX": 2.9677709434035733,
-            "velocityY": -0.9791049181495252,
-            "timestamp": 1.2148003142313908
-        },
-        {
-            "x": 2.665882293188928,
-            "y": 2.479849558811985,
-            "heading": -0.35406456410202075,
-            "angularVelocity": -0.5999390242685017,
-            "velocityX": 3.08903704518594,
-            "velocityY": -0.7595413500609144,
-            "timestamp": 1.2448828435180317
-        },
-        {
-            "x": 2.7622502226261916,
-            "y": 2.4637149411085275,
-            "heading": -0.37178230702948656,
-            "angularVelocity": -0.5889711851900935,
-            "velocityX": 3.2034516951353296,
-            "velocityY": -0.5363451174508257,
-            "timestamp": 1.2749653728046726
-        },
-        {
-            "x": 2.8617965704136163,
-            "y": 2.4544224370162078,
-            "heading": -0.3889934776908366,
-            "angularVelocity": -0.57213176782302,
-            "velocityX": 3.309108314626244,
-            "velocityY": -0.30890035886875983,
-            "timestamp": 1.3050479020913135
-        },
-        {
-            "x": 2.96417486527032,
-            "y": 2.452120989561553,
-            "heading": -0.40547141981393114,
-            "angularVelocity": -0.5477578685651567,
-            "velocityX": 3.4032475754010525,
-            "velocityY": -0.07650445322366382,
-            "timestamp": 1.3351304313779544
-        },
-        {
-            "x": 3.068913958464018,
-            "y": 2.456980552046256,
-            "heading": -0.4209173641629785,
-            "angularVelocity": -0.5134523165215639,
-            "velocityX": 3.4817249638713506,
-            "velocityY": 0.16154102065001244,
-            "timestamp": 1.3652129606645953
-        },
-        {
-            "x": 3.175348297900835,
-            "y": 2.469181868617686,
-            "heading": -0.43492680962689906,
-            "angularVelocity": -0.4657003847794821,
-            "velocityX": 3.5380781457121575,
-            "velocityY": 0.4055947708108251,
-            "timestamp": 1.3952954899512362
-        },
-        {
-            "x": 3.2825012821379658,
-            "y": 2.488873812540701,
-            "heading": -0.44694408076253883,
-            "angularVelocity": -0.3994767534718848,
-            "velocityX": 3.5619672540204803,
-            "velocityY": 0.6545973490252659,
-            "timestamp": 1.425378019237877
-        },
-        {
-            "x": 3.3889723525651156,
-            "y": 2.516050236824646,
-            "heading": -0.4565561347315361,
-            "angularVelocity": -0.3195227993435668,
-            "velocityX": 3.539299153094488,
-            "velocityY": 0.9033955896790181,
-            "timestamp": 1.455460548524518
-        },
-        {
-            "x": 3.4932758480756925,
-            "y": 2.5504542246131905,
-            "heading": -0.4662703499641935,
-            "angularVelocity": -0.32291883239253294,
-            "velocityX": 3.4672448754798637,
-            "velocityY": 1.1436534295611822,
-            "timestamp": 1.485543077811159
-        },
-        {
-            "x": 3.5930108067726363,
-            "y": 2.5908608883384305,
-            "heading": -0.4766346317588353,
-            "angularVelocity": -0.3445282707409342,
-            "velocityX": 3.315378096922126,
-            "velocityY": 1.3431936969204072,
-            "timestamp": 1.5156256070977998
-        },
-        {
-            "x": 3.686096614652251,
-            "y": 2.634834387797071,
-            "heading": -0.48720467819938723,
-            "angularVelocity": -0.3513682755805826,
-            "velocityX": 3.094347785475507,
-            "velocityY": 1.4617620426674243,
-            "timestamp": 1.5457081363844407
-        },
-        {
-            "x": 3.771715868998783,
-            "y": 2.679896340499022,
-            "heading": -0.4975881116213702,
-            "angularVelocity": -0.34516490694787005,
-            "velocityX": 2.8461454663839936,
-            "velocityY": 1.497944280967274,
-            "timestamp": 1.5757906656710816
-        },
-        {
-            "x": 3.8498203373891897,
-            "y": 2.724287723719235,
-            "heading": -0.507530883254748,
-            "angularVelocity": -0.33051647814035945,
-            "velocityX": 2.596339810598824,
-            "velocityY": 1.475653286903962,
-            "timestamp": 1.6058731949577225
-        },
-        {
-            "x": 3.920599661686896,
-            "y": 2.7668759161025953,
-            "heading": -0.5168796397769617,
-            "angularVelocity": -0.3107702957131982,
-            "velocityX": 2.35283820796086,
-            "velocityY": 1.415711823216824,
-            "timestamp": 1.6359557242443634
-        },
-        {
-            "x": 3.984278053809844,
-            "y": 2.806919241781621,
-            "heading": -0.525538519539268,
-            "angularVelocity": -0.287837491315874,
-            "velocityX": 2.11678983227083,
-            "velocityY": 1.3311156551191636,
-            "timestamp": 1.6660382535310043
-        },
-        {
-            "x": 4.041059171114166,
-            "y": 2.843909698702473,
-            "heading": -0.5334440710807248,
-            "angularVelocity": -0.26279544070665484,
-            "velocityX": 1.8875114111345854,
-            "velocityY": 1.229632540814369,
-            "timestamp": 1.6961207828176452
-        },
-        {
-            "x": 4.091116341717457,
-            "y": 2.8774832028649056,
-            "heading": -0.5405519914461838,
-            "angularVelocity": -0.23628067632639732,
-            "velocityX": 1.6639947434713853,
-            "velocityY": 1.11604658778956,
-            "timestamp": 1.7262033121042861
-        },
-        {
-            "x": 4.1345948250930045,
-            "y": 2.9073683770064043,
-            "heading": -0.5468299348831553,
-            "angularVelocity": -0.20869067813911088,
-            "velocityX": 1.445306774615446,
-            "velocityY": 0.9934395428236782,
-            "timestamp": 1.756285841390927
-        },
-        {
-            "x": 4.171616484177909,
-            "y": 2.933356232054327,
-            "heading": -0.552253409487953,
-            "angularVelocity": -0.18028652288901045,
-            "velocityX": 1.2306697595852323,
-            "velocityY": 0.863885306993297,
-            "timestamp": 1.786368370677568
-        },
-        {
-            "x": 4.202284260430688,
-            "y": 2.9552814196152344,
-            "heading": -0.5568033106587852,
-            "angularVelocity": -0.15124729464987938,
-            "velocityX": 1.0194547127524736,
-            "velocityY": 0.7288345787681281,
-            "timestamp": 1.8164508999642088
-        },
-        {
-            "x": 4.226685878711894,
-            "y": 2.973010149818714,
-            "heading": -0.5604643670760648,
-            "angularVelocity": -0.12170041895060278,
-            "velocityX": 0.8111558057069341,
-            "velocityY": 0.5893364229633967,
-            "timestamp": 1.8465334292508497
-        },
-        {
-            "x": 4.244896778926994,
-            "y": 2.98643211128414,
-            "heading": -0.5632241207030344,
-            "angularVelocity": -0.09173941461743668,
-            "velocityX": 0.6053646633758383,
-            "velocityY": 0.4461713088529227,
-            "timestamp": 1.8766159585374906
-        },
-        {
-            "x": 4.256982406189367,
-            "y": 2.9954548894256625,
-            "heading": -0.565072233243941,
-            "angularVelocity": -0.06143474583855434,
-            "velocityX": 0.4017490400230914,
-            "velocityY": 0.2999341596429636,
-            "timestamp": 1.9066984878241315
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -0.030840716457660636,
-            "velocityX": 0.20003616561938206,
-            "velocityY": 0.15108804618886046,
-            "timestamp": 1.9367810171107724
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -4.9732030554004765e-30,
-            "velocityX": -4.223945033236224e-32,
-            "velocityY": 9.969983495378875e-32,
-            "timestamp": 1.9668635463974133
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.1.traj
deleted file mode 100644
index 848b8926..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.1.traj	
+++ /dev/null
@@ -1,553 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 1.4317938010297452e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2473468901462774,
-            "y": 5.551784954334613,
-            "heading": -0.004046039178700474,
-            "angularVelocity": -0.12521248615625988,
-            "velocityX": -0.08210560159156222,
-            "velocityY": -0.2542304328304919,
-            "timestamp": 0.032313384255078216
-        },
-        {
-            "x": 2.2420392078855844,
-            "y": 5.535352460743813,
-            "heading": -0.012062241078351911,
-            "angularVelocity": -0.24807682898124367,
-            "velocityX": -0.16425646471427996,
-            "velocityY": -0.5085352082308542,
-            "timestamp": 0.06462676851015643
-        },
-        {
-            "x": 2.234075272195187,
-            "y": 5.510699784095841,
-            "heading": -0.023960373728225737,
-            "angularVelocity": -0.36821066329520014,
-            "velocityX": -0.24645935032774877,
-            "velocityY": -0.7629246275588465,
-            "timestamp": 0.09694015276523466
-        },
-        {
-            "x": 2.2234531308615284,
-            "y": 5.477823761292619,
-            "heading": -0.03963572188039383,
-            "angularVelocity": -0.4851038822931769,
-            "velocityX": -0.32872265095504705,
-            "velocityY": -1.017411935057708,
-            "timestamp": 0.12925353702031286
-        },
-        {
-            "x": 2.2101704924088987,
-            "y": 5.436720678146679,
-            "heading": -0.05896132607089341,
-            "angularVelocity": -0.5980680958065965,
-            "velocityX": -0.4110568657178767,
-            "velocityY": -1.27201418525146,
-            "timestamp": 0.16156692127539107
-        },
-        {
-            "x": 2.1942246366351648,
-            "y": 5.38738610923438,
-            "heading": -0.08177969135834087,
-            "angularVelocity": -0.7061583245915709,
-            "velocityX": -0.4934752623822669,
-            "velocityY": -1.5267533887152571,
-            "timestamp": 0.19388030553046928
-        },
-        {
-            "x": 2.1756122932404036,
-            "y": 5.329814710705053,
-            "heading": -0.10789037911156549,
-            "angularVelocity": -0.8080455933402793,
-            "velocityX": -0.5759948647853634,
-            "velocityY": -1.7816579679449487,
-            "timestamp": 0.2261936897855475
-        },
-        {
-            "x": 2.1543294701171383,
-            "y": 5.263999960188385,
-            "heading": -0.1370304714941401,
-            "angularVelocity": -0.9017963625397062,
-            "velocityX": -0.6586380106540685,
-            "velocityY": -2.0367643945039555,
-            "timestamp": 0.25850707404062573
-        },
-        {
-            "x": 2.1303711997350336,
-            "y": 5.1899338649176165,
-            "heading": -0.1688416775308267,
-            "angularVelocity": -0.9844591264590365,
-            "velocityX": -0.7414348863300811,
-            "velocityY": -2.2921181726463207,
-            "timestamp": 0.29082045829570397
-        },
-        {
-            "x": 2.1037311567595207,
-            "y": 5.107606785885568,
-            "heading": -0.20280965108227242,
-            "angularVelocity": -1.051204457054369,
-            "velocityX": -0.8244275116843016,
-            "velocityY": -2.5477702484570597,
-            "timestamp": 0.3231338425507822
-        },
-        {
-            "x": 2.074401135228672,
-            "y": 5.017008143765364,
-            "heading": -0.23813648207052604,
-            "angularVelocity": -1.0932569213236978,
-            "velocityX": -0.9076740863575303,
-            "velocityY": -2.8037497219427316,
-            "timestamp": 0.35544722680586044
-        },
-        {
-            "x": 2.0423710011934597,
-            "y": 4.918132506074763,
-            "heading": -0.27341197795784133,
-            "angularVelocity": -1.0916682576129122,
-            "velocityX": -0.9912342756292476,
-            "velocityY": -3.0598973140692034,
-            "timestamp": 0.3877606110609387
-        },
-        {
-            "x": 2.00763899967256,
-            "y": 4.811032729430715,
-            "heading": -0.3053550995537523,
-            "angularVelocity": -0.9885415078704507,
-            "velocityX": -1.0748487761829064,
-            "velocityY": -3.3144091562374,
-            "timestamp": 0.4200739953160169
-        },
-        {
-            "x": 1.9707337568018743,
-            "y": 4.697468766706309,
-            "heading": -0.3150299299334663,
-            "angularVelocity": -0.2994062863655037,
-            "velocityX": -1.1421039213769328,
-            "velocityY": -3.514455862250325,
-            "timestamp": 0.45238737957109515
-        },
-        {
-            "x": 1.9331993710024595,
-            "y": 4.581134395027877,
-            "heading": -0.3150299515537849,
-            "angularVelocity": -6.690824588937528e-7,
-            "velocityX": -1.1615739627617825,
-            "velocityY": -3.6001915107405935,
-            "timestamp": 0.4847007638261734
-        },
-        {
-            "x": 1.8956658385726595,
-            "y": 4.464799748019651,
-            "heading": -0.3150299731737692,
-            "angularVelocity": -6.690721139443934e-7,
-            "velocityX": -1.1615475535931208,
-            "velocityY": -3.600200031352131,
-            "timestamp": 0.5170141480812516
-        },
-        {
-            "x": 1.8630740613066095,
-            "y": 4.346985096208152,
-            "heading": -0.31502999489570194,
-            "angularVelocity": -6.722271053919432e-7,
-            "velocityX": -1.0086154086732113,
-            "velocityY": -3.64600163453891,
-            "timestamp": 0.5493275323363298
-        },
-        {
-            "x": 1.8389498768606893,
-            "y": 4.227149626761845,
-            "heading": -0.3150300170939308,
-            "angularVelocity": -6.869670074324573e-7,
-            "velocityX": -0.7465694170405304,
-            "velocityY": -3.708539733886721,
-            "timestamp": 0.581640916591408
-        },
-        {
-            "x": 1.8234162959667812,
-            "y": 4.10590102408459,
-            "heading": -0.31503004024876186,
-            "angularVelocity": -7.165709048863888e-7,
-            "velocityX": -0.4807166210536207,
-            "velocityY": -3.7522718672898008,
-            "timestamp": 0.6139543008464863
-        },
-        {
-            "x": 1.816552246405149,
-            "y": 3.98385431066073,
-            "heading": -0.315030064931292,
-            "angularVelocity": -7.63848502164674e-7,
-            "velocityX": -0.21242125267501,
-            "velocityY": -3.776970943694307,
-            "timestamp": 0.6462676851015645
-        },
-        {
-            "x": 1.818392595454208,
-            "y": 3.861628584870307,
-            "heading": -0.315030091871416,
-            "angularVelocity": -8.337140982566696e-7,
-            "velocityX": 0.05695315088421205,
-            "velocityY": -3.782510826646546,
-            "timestamp": 0.6785810693566428
-        },
-        {
-            "x": 1.828928030254088,
-            "y": 3.739843861498511,
-            "heading": -0.3150301220653929,
-            "angularVelocity": -9.344108498281474e-7,
-            "velocityX": 0.32603935003261814,
-            "velocityY": -3.7688631562216113,
-            "timestamp": 0.710894453611721
-        },
-        {
-            "x": 1.8481051197294465,
-            "y": 3.619117921789427,
-            "heading": -0.3150301569615331,
-            "angularVelocity": -0.0000010799283629257,
-            "velocityX": 0.593472021499706,
-            "velocityY": -3.7360970536569806,
-            "timestamp": 0.7432078378667992
-        },
-        {
-            "x": 1.875826590719106,
-            "y": 3.500063178055521,
-            "heading": -0.31503019880828736,
-            "angularVelocity": -0.0000012950285239246719,
-            "velocityX": 0.8578943873792161,
-            "velocityY": -3.684378670896923,
-            "timestamp": 0.7755212221218775
-        },
-        {
-            "x": 1.9119518234377388,
-            "y": 3.3832835666467993,
-            "heading": -0.31503025136937535,
-            "angularVelocity": -0.0000016266042432729645,
-            "velocityX": 1.1179650027822445,
-            "velocityY": -3.61397031294762,
-            "timestamp": 0.8078346063769557
-        },
-        {
-            "x": 1.9562975654606443,
-            "y": 3.269371484637439,
-            "heading": -0.31503032157308203,
-            "angularVelocity": -0.0000021725891085409373,
-            "velocityX": 1.3723645184560365,
-            "velocityY": -3.5252290849559675,
-            "timestamp": 0.8401479906320339
-        },
-        {
-            "x": 2.008638858795166,
-            "y": 3.158904790878296,
-            "heading": -0.3150304332740777,
-            "angularVelocity": -0.0000034568027529519608,
-            "velocityX": 1.6198022751608165,
-            "velocityY": -3.4186049002831713,
-            "timestamp": 0.8724613748871122
-        },
-        {
-            "x": 2.0775165641117233,
-            "y": 3.0394044472847606,
-            "heading": -0.3150305189714284,
-            "angularVelocity": -0.000002350392551393865,
-            "velocityX": 1.8890857675343722,
-            "velocityY": -3.277495922091173,
-            "timestamp": 0.9089222420961518
-        },
-        {
-            "x": 2.155767726009908,
-            "y": 2.9258208622381168,
-            "heading": -0.3150305770370975,
-            "angularVelocity": -0.00000159254767063084,
-            "velocityX": 2.1461684235196876,
-            "velocityY": -3.1152189660064753,
-            "timestamp": 0.9453831093051914
-        },
-        {
-            "x": 2.242886953120307,
-            "y": 2.818887620265158,
-            "heading": -0.3150306203630764,
-            "angularVelocity": -0.0000011882871183876306,
-            "velocityX": 2.389389879591231,
-            "velocityY": -2.932822232666113,
-            "timestamp": 0.981843976514231
-        },
-        {
-            "x": 2.338311580267833,
-            "y": 2.719295349641276,
-            "heading": -0.3150306549359537,
-            "angularVelocity": -9.482187337248934e-7,
-            "velocityX": 2.617179306252704,
-            "velocityY": -2.731483868798106,
-            "timestamp": 1.0183048437232707
-        },
-        {
-            "x": 2.4414253013619946,
-            "y": 2.6276872624602983,
-            "heading": -0.31503068395926054,
-            "angularVelocity": -7.960125215618009e-7,
-            "velocityX": 2.8280655120730875,
-            "velocityY": -2.512504342142078,
-            "timestamp": 1.0547657109323103
-        },
-        {
-            "x": 2.5515621486647997,
-            "y": 2.5446549983777076,
-            "heading": -0.3150307093352344,
-            "angularVelocity": -6.959783403374032e-7,
-            "velocityX": 3.020686443670201,
-            "velocityY": -2.2772981127010774,
-            "timestamp": 1.09122657814135
-        },
-        {
-            "x": 2.6680107915990137,
-            "y": 2.4707347968336117,
-            "heading": -0.31503073229503964,
-            "angularVelocity": -6.297108928988803e-7,
-            "velocityX": 3.1937979496369038,
-            "velocityY": -2.0273846236374995,
-            "timestamp": 1.1276874453503896
-        },
-        {
-            "x": 2.7900191236562386,
-            "y": 2.4064040113359084,
-            "heading": -0.3150307537024253,
-            "angularVelocity": -5.871332010760787e-7,
-            "velocityX": 3.3462816821585726,
-            "velocityY": -1.764378919702536,
-            "timestamp": 1.1641483125594292
-        },
-        {
-            "x": 2.9167990841955884,
-            "y": 2.352077913506683,
-            "heading": -0.3150307742165802,
-            "angularVelocity": -5.626348607586924e-7,
-            "velocityX": 3.477151539278742,
-            "velocityY": -1.4899836999970275,
-            "timestamp": 1.2006091797684688
-        },
-        {
-            "x": 3.0475311021850517,
-            "y": 2.308104934680324,
-            "heading": -0.31503079438993686,
-            "angularVelocity": -5.532878997498012e-7,
-            "velocityX": 3.585543296047864,
-            "velocityY": -1.2060321707174542,
-            "timestamp": 1.2370700469775084
-        },
-        {
-            "x": 3.178817728061225,
-            "y": 2.2658165761507623,
-            "heading": -0.3150308145386853,
-            "angularVelocity": -5.526129783451472e-7,
-            "velocityX": 3.600754340906743,
-            "velocityY": -1.159828653748452,
-            "timestamp": 1.273530914186548
-        },
-        {
-            "x": 3.3101043979960063,
-            "y": 2.2235283544039026,
-            "heading": -0.3150308346874411,
-            "angularVelocity": -5.526131795163358e-7,
-            "velocityX": 3.600755549287413,
-            "velocityY": -1.1598249022550946,
-            "timestamp": 1.3099917813955877
-        },
-        {
-            "x": 3.4413910762638236,
-            "y": 2.1812401585275256,
-            "heading": -0.31503085483620696,
-            "angularVelocity": -5.52613457009699e-7,
-            "velocityX": 3.60075577783481,
-            "velocityY": -1.159824192714005,
-            "timestamp": 1.3464526486046273
-        },
-        {
-            "x": 3.572677762945226,
-            "y": 2.138951988771722,
-            "heading": -0.31503087498498206,
-            "angularVelocity": -5.52613710159953e-7,
-            "velocityX": 3.6007560085914068,
-            "velocityY": -1.1598234763137998,
-            "timestamp": 1.382913515813667
-        },
-        {
-            "x": 3.7039644581047573,
-            "y": 2.0966638453368867,
-            "heading": -0.31503089513376514,
-            "angularVelocity": -5.526139295643368e-7,
-            "velocityX": 3.6007562411181984,
-            "velocityY": -1.1598227544173942,
-            "timestamp": 1.4193743830227066
-        },
-        {
-            "x": 3.8352511618245644,
-            "y": 2.05437572847807,
-            "heading": -0.3150309152825546,
-            "angularVelocity": -5.526141044747151e-7,
-            "velocityX": 3.600756475898009,
-            "velocityY": -1.1598220255258234,
-            "timestamp": 1.4558352502317462
-        },
-        {
-            "x": 3.966537874250263,
-            "y": 2.01208763864737,
-            "heading": -0.3150309354313486,
-            "angularVelocity": -5.526142273042023e-7,
-            "velocityX": 3.6007567146716046,
-            "velocityY": -1.159821284234734,
-            "timestamp": 1.4922961174407858
-        },
-        {
-            "x": 4.097824595645338,
-            "y": 1.9697995766628096,
-            "heading": -0.3150309555801447,
-            "angularVelocity": -5.526142864672618e-7,
-            "velocityX": 3.600756960671674,
-            "velocityY": -1.159820520507965,
-            "timestamp": 1.5287569846498255
-        },
-        {
-            "x": 4.229111326453571,
-            "y": 1.9275115439021684,
-            "heading": -0.3150309757289402,
-            "angularVelocity": -5.5261426880692e-7,
-            "velocityX": 3.600757218843208,
-            "velocityY": -1.1598197189933317,
-            "timestamp": 1.565217851858865
-        },
-        {
-            "x": 4.360398019008411,
-            "y": 1.885223392382287,
-            "heading": -0.31503099587881916,
-            "angularVelocity": -5.52643987126892e-7,
-            "velocityX": 3.6007561696802117,
-            "velocityY": -1.1598229761632481,
-            "timestamp": 1.6016787190679047
-        },
-        {
-            "x": 4.485161526388635,
-            "y": 1.844329974236374,
-            "heading": -0.3370638509456068,
-            "angularVelocity": -0.6042877406197901,
-            "velocityX": 3.4218469534726492,
-            "velocityY": -1.1215700907896822,
-            "timestamp": 1.6381395862769443
-        },
-        {
-            "x": 4.60044458089581,
-            "y": 1.8067761222092775,
-            "heading": -0.3770388194107599,
-            "angularVelocity": -1.0963800788381923,
-            "velocityX": 3.1618297460185825,
-            "velocityY": -1.0299769287381584,
-            "timestamp": 1.674600453485984
-        },
-        {
-            "x": 4.705219804759057,
-            "y": 1.772711102552083,
-            "heading": -0.4189139196427851,
-            "angularVelocity": -1.148494356756437,
-            "velocityX": 2.8736349923479074,
-            "velocityY": -0.9342898911836356,
-            "timestamp": 1.7110613206950236
-        },
-        {
-            "x": 4.799468970200479,
-            "y": 1.7421012617230478,
-            "heading": -0.45948472427125614,
-            "angularVelocity": -1.1127218778387968,
-            "velocityX": 2.5849403115144614,
-            "velocityY": -0.8395258580532651,
-            "timestamp": 1.7475221879040632
-        },
-        {
-            "x": 4.883206648654635,
-            "y": 1.7149242324789875,
-            "heading": -0.49729979894596704,
-            "angularVelocity": -1.037141394852447,
-            "velocityX": 2.296645276538986,
-            "velocityY": -0.7453752838144925,
-            "timestamp": 1.7839830551131028
-        },
-        {
-            "x": 4.956447680828415,
-            "y": 1.6911655730288653,
-            "heading": -0.5315269969459382,
-            "angularVelocity": -0.9387379023033031,
-            "velocityX": 2.0087572726635705,
-            "velocityY": -0.6516207997442213,
-            "timestamp": 1.8204439223221425
-        },
-        {
-            "x": 5.019204214404415,
-            "y": 1.6708154773305386,
-            "heading": -0.5616288277349688,
-            "angularVelocity": -0.825592836737831,
-            "velocityX": 1.7212024392124468,
-            "velocityY": -0.5581352627093101,
-            "timestamp": 1.856904789531182
-        },
-        {
-            "x": 5.071485921099441,
-            "y": 1.6538669506708767,
-            "heading": -0.587231204588931,
-            "angularVelocity": -0.7021878198118158,
-            "velocityX": 1.4339128687006073,
-            "velocityY": -0.46484156733002446,
-            "timestamp": 1.8933656567402217
-        },
-        {
-            "x": 5.11330055140079,
-            "y": 1.6403147844077772,
-            "heading": -0.6080594902506735,
-            "angularVelocity": -0.5712504187662296,
-            "velocityX": 1.1468358682094448,
-            "velocityY": -0.37169072763413913,
-            "timestamp": 1.9298265239492614
-        },
-        {
-            "x": 5.1446544158470235,
-            "y": 1.6301549523614358,
-            "heading": -0.6239033665029361,
-            "angularVelocity": -0.43454469037794713,
-            "velocityX": 0.859931944747064,
-            "velocityY": -0.2786503126240163,
-            "timestamp": 1.966287391158301
-        },
-        {
-            "x": 5.165552752490355,
-            "y": 1.6233842390024875,
-            "heading": -0.6345958555798376,
-            "angularVelocity": -0.29325931869912014,
-            "velocityX": 0.5731716835893133,
-            "velocityY": -0.18569808886140712,
-            "timestamp": 2.0027482583673404
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -0.14821766002380998,
-            "velocityX": 0.2865331603263514,
-            "velocityY": -0.09281839027812194,
-            "timestamp": 2.03920912557638
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 2.046404654845657e-31,
-            "velocityX": 2.793064044552828e-33,
-            "velocityY": 5.221456528298987e-32,
-            "timestamp": 2.0756699927854196
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj
deleted file mode 100644
index 35bba061..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -5.451838641793523e-34,
-            "angularVelocity": -2.0131697388730357e-33,
-            "velocityX": -3.0924959567962797e-34,
-            "velocityY": 4.789643826321005e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2520020156777094,
-            "y": 5.6078780285533805,
-            "heading": 0.01582108665485387,
-            "angularVelocity": 0.208254594271014,
-            "velocityX": 0.026352738707599337,
-            "velocityY": 0.630223424496655,
-            "timestamp": 0.07596992858781765
-        },
-        {
-            "x": 2.2560055471462697,
-            "y": 5.703634194455967,
-            "heading": 0.04745824180691435,
-            "angularVelocity": 0.4164431340157103,
-            "velocityX": 0.05269889735295001,
-            "velocityY": 1.2604482810839739,
-            "timestamp": 0.1519398571756353
-        },
-        {
-            "x": 2.262009891384212,
-            "y": 5.847268762594484,
-            "heading": 0.09489870943903907,
-            "angularVelocity": 0.6244637649920359,
-            "velocityX": 0.07903580205425154,
-            "velocityY": 1.8906766244025437,
-            "timestamp": 0.22790978576345294
-        },
-        {
-            "x": 2.2700142224347877,
-            "y": 6.038782263911744,
-            "heading": 0.1581161098401003,
-            "angularVelocity": 0.8321371571119077,
-            "velocityX": 0.1053618346017344,
-            "velocityY": 2.5209119565760743,
-            "timestamp": 0.3038797143512706
-        },
-        {
-            "x": 2.280009491675263,
-            "y": 6.278187226323642,
-            "heading": 0.23645965423367718,
-            "angularVelocity": 1.0312441494928544,
-            "velocityX": 0.13156875919557368,
-            "velocityY": 3.151312194997739,
-            "timestamp": 0.37984964293908824
-        },
-        {
-            "x": 2.288005657404111,
-            "y": 6.469712008115255,
-            "heading": 0.2990934539478391,
-            "angularVelocity": 0.8244551611202343,
-            "velocityX": 0.10525435363026732,
-            "velocityY": 2.52106044262263,
-            "timestamp": 0.4558195715269059
-        },
-        {
-            "x": 2.294002815862431,
-            "y": 6.613355952122642,
-            "heading": 0.3460499471183657,
-            "angularVelocity": 0.6180931592721867,
-            "velocityX": 0.07894121489645621,
-            "velocityY": 1.8908000399308182,
-            "timestamp": 0.5317895001147235
-        },
-        {
-            "x": 2.2980009414583966,
-            "y": 6.709118669367686,
-            "heading": 0.3773492268804029,
-            "angularVelocity": 0.4119956454330042,
-            "velocityX": 0.05262773929482027,
-            "velocityY": 1.260534517079955,
-            "timestamp": 0.6077594287025412
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.20601273965271094,
-            "velocityX": 0.026313813620245378,
-            "velocityY": 0.6302668901019914,
-            "timestamp": 0.6837293572903589
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 9.82651204832558e-34,
-            "velocityX": 0,
-            "velocityY": 3.8258931367738186e-36,
-            "timestamp": 0.7596992858781766
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj
deleted file mode 100644
index f46e6706..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 5.7052362935559275e-34,
-            "velocityX": 0,
-            "velocityY": -1.2494033787640354e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2520019447146944,
-            "y": 5.512001877264397,
-            "heading": -0.01569721301150974,
-            "angularVelocity": -0.20637467835572687,
-            "velocityX": 0.02632000319280478,
-            "velocityY": -0.6310417737196751,
-            "timestamp": 0.07606172005488261
-        },
-        {
-            "x": 2.2560053559202022,
-            "y": 5.416005527882177,
-            "heading": -0.04708679827781808,
-            "angularVelocity": -0.41268571422864314,
-            "velocityX": 0.05263371907208186,
-            "velocityY": -1.262084913580087,
-            "timestamp": 0.15212344010976522
-        },
-        {
-            "x": 2.262009560249172,
-            "y": 5.272010699235591,
-            "heading": -0.0941564711571504,
-            "angularVelocity": -0.6188352412405218,
-            "velocityX": 0.07893858204412474,
-            "velocityY": -1.8931313746610776,
-            "timestamp": 0.22818516016464785
-        },
-        {
-            "x": 2.2700137636187,
-            "y": 5.080016884298785,
-            "heading": -0.15688079244309955,
-            "angularVelocity": -0.8246503134650409,
-            "velocityX": 0.10523300503528431,
-            "velocityY": -2.5241845017213995,
-            "timestamp": 0.30424688021953045
-        },
-        {
-            "x": 2.2800092014745617,
-            "y": 4.840012168782865,
-            "heading": -0.23463455266722952,
-            "angularVelocity": -1.0222456206358,
-            "velocityX": 0.13141219852311892,
-            "velocityY": -3.155394268532757,
-            "timestamp": 0.38030860027441304
-        },
-        {
-            "x": 2.2880054914438257,
-            "y": 4.648007616430203,
-            "heading": -0.2967978427641432,
-            "angularVelocity": -0.8172743142287549,
-            "velocityX": 0.10512896583845424,
-            "velocityY": -2.524325668866294,
-            "timestamp": 0.45637032032929564
-        },
-        {
-            "x": 2.2940027351611283,
-            "y": 4.5040038585170965,
-            "heading": -0.3434020516948695,
-            "angularVelocity": -0.6127156853289512,
-            "velocityX": 0.07884706936650254,
-            "velocityY": -1.8932487696728393,
-            "timestamp": 0.5324320403841782
-        },
-        {
-            "x": 2.2980009144821483,
-            "y": 4.408001268437703,
-            "heading": -0.37446662411182524,
-            "angularVelocity": -0.40841269950956927,
-            "velocityX": 0.05256493434720428,
-            "velocityY": -1.2621669613850792,
-            "timestamp": 0.6084937604390609
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.20422067601109492,
-            "velocityX": 0.026282412709165117,
-            "velocityY": -0.6310831309503169,
-            "timestamp": 0.6845554804939435
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 8.777318885868316e-35,
-            "velocityX": -1.1404320857258046e-34,
-            "velocityY": 1.134511753128188e-36,
-            "timestamp": 0.7606172005488262
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.1.traj
deleted file mode 100644
index 5529ce29..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.1.traj	
+++ /dev/null
@@ -1,553 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 1.095053106710567e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3031602212725026,
-            "y": 4.367935645195281,
-            "heading": -0.3933962364434778,
-            "angularVelocity": -0.10578752662991632,
-            "velocityX": 0.0984360180997807,
-            "velocityY": 0.24718310735800264,
-            "timestamp": 0.032104318454850914
-        },
-        {
-            "x": 2.309652031024921,
-            "y": 4.383743043384139,
-            "heading": -0.39998627296935113,
-            "angularVelocity": -0.20526947286363117,
-            "velocityX": 0.20220986038211264,
-            "velocityY": 0.4923760711845366,
-            "timestamp": 0.06420863690970183
-        },
-        {
-            "x": 2.3196739941431104,
-            "y": 4.407342390021744,
-            "heading": -0.40953275493580865,
-            "angularVelocity": -0.29735818811675285,
-            "velocityX": 0.31216869257896596,
-            "velocityY": 0.7350832465356143,
-            "timestamp": 0.09631295536455274
-        },
-        {
-            "x": 2.333458307096076,
-            "y": 4.438632228164309,
-            "heading": -0.4217541925110665,
-            "angularVelocity": -0.3806789292987277,
-            "velocityX": 0.4293600866297902,
-            "velocityY": 0.9746301945817274,
-            "timestamp": 0.12841727381940365
-        },
-        {
-            "x": 2.3512791398330983,
-            "y": 4.477480960055527,
-            "heading": -0.4363123371700568,
-            "angularVelocity": -0.4534637506622021,
-            "velocityX": 0.5550914517025046,
-            "velocityY": 1.21007807550416,
-            "timestamp": 0.16052159227425455
-        },
-        {
-            "x": 2.3734634122157945,
-            "y": 4.523713969255995,
-            "heading": -0.45279460426513807,
-            "angularVelocity": -0.5133971966500652,
-            "velocityX": 0.691005866201282,
-            "velocityY": 1.4400869236793286,
-            "timestamp": 0.19262591072910545
-        },
-        {
-            "x": 2.4004044405613056,
-            "y": 4.577093572392249,
-            "heading": -0.4706891378053212,
-            "angularVelocity": -0.5573871180398601,
-            "velocityX": 0.8391714773013875,
-            "velocityY": 1.6626923013900174,
-            "timestamp": 0.22473022918395635
-        },
-        {
-            "x": 2.432578225951104,
-            "y": 4.63728707308353,
-            "heading": -0.48934877664951465,
-            "angularVelocity": -0.581218968109697,
-            "velocityX": 1.002163787873114,
-            "velocityY": 1.8749347000134435,
-            "timestamp": 0.25683454763880725
-        },
-        {
-            "x": 2.4705256917588616,
-            "y": 4.703846202675345,
-            "heading": -0.5079484621261662,
-            "angularVelocity": -0.5793515131868818,
-            "velocityX": 1.1820050271780043,
-            "velocityY": 2.073214221489191,
-            "timestamp": 0.28893886609365815
-        },
-        {
-            "x": 2.5143889510907504,
-            "y": 4.77663720854218,
-            "heading": -0.5259955708448517,
-            "angularVelocity": -0.5621395995079442,
-            "velocityX": 1.366272870535308,
-            "velocityY": 2.267327554989265,
-            "timestamp": 0.32104318454850905
-        },
-        {
-            "x": 2.564366566619072,
-            "y": 4.855456871813396,
-            "heading": -0.5427715655324792,
-            "angularVelocity": -0.5225463580925951,
-            "velocityX": 1.5567256348583118,
-            "velocityY": 2.4551109341275055,
-            "timestamp": 0.35314750300335995
-        },
-        {
-            "x": 2.620751841763659,
-            "y": 4.939963572730219,
-            "heading": -0.5571329301868887,
-            "angularVelocity": -0.44733435704626356,
-            "velocityX": 1.7563143482981505,
-            "velocityY": 2.6322533847172953,
-            "timestamp": 0.38525182145821085
-        },
-        {
-            "x": 2.6840019979529464,
-            "y": 5.029487269137319,
-            "heading": -0.5670183536644556,
-            "angularVelocity": -0.30791569338152147,
-            "velocityX": 1.9701448039844656,
-            "velocityY": 2.788525055686805,
-            "timestamp": 0.41735613991306175
-        },
-        {
-            "x": 2.7547918742113016,
-            "y": 5.122370588176559,
-            "heading": -0.568384562504894,
-            "angularVelocity": -0.0425552980468726,
-            "velocityX": 2.2049954543625816,
-            "velocityY": 2.893172118569162,
-            "timestamp": 0.44946045836791265
-        },
-        {
-            "x": 2.8341176789452183,
-            "y": 5.214333601098647,
-            "heading": -0.5683844391321649,
-            "angularVelocity": 0.0000038428702119811485,
-            "velocityX": 2.4708764599838617,
-            "velocityY": 2.864506002562167,
-            "timestamp": 0.48156477682276355
-        },
-        {
-            "x": 2.9197482153809866,
-            "y": 5.3004567509455125,
-            "heading": -0.5683843814173083,
-            "angularVelocity": 0.0000017977287583592433,
-            "velocityX": 2.6672591276526463,
-            "velocityY": 2.6826032755681406,
-            "timestamp": 0.5136690952776145
-        },
-        {
-            "x": 3.0112544304014244,
-            "y": 5.380309105627853,
-            "heading": -0.5683843265105415,
-            "angularVelocity": 0.0000017102610943456471,
-            "velocityX": 2.850277452521701,
-            "velocityY": 2.487277678691065,
-            "timestamp": 0.5457734137324655
-        },
-        {
-            "x": 3.107551803887462,
-            "y": 5.454313172250514,
-            "heading": -0.5683842730108181,
-            "angularVelocity": 0.0000016664338651344868,
-            "velocityX": 2.999514648518804,
-            "velocityY": 2.305112526426488,
-            "timestamp": 0.5778777321873164
-        },
-        {
-            "x": 3.2038516202489813,
-            "y": 5.528314059986814,
-            "heading": -0.5683842195116714,
-            "angularVelocity": 0.000001666415897857932,
-            "velocityX": 2.9995907403219917,
-            "velocityY": 2.305013509019602,
-            "timestamp": 0.6099820506421674
-        },
-        {
-            "x": 3.3001514533185667,
-            "y": 5.602314925980335,
-            "heading": -0.5683841660125195,
-            "angularVelocity": 0.000001666416059709123,
-            "velocityX": 2.9995912607524815,
-            "velocityY": 2.305012831765639,
-            "timestamp": 0.6420863690970183
-        },
-        {
-            "x": 3.3964512995777554,
-            "y": 5.676315774809768,
-            "heading": -0.568384112513365,
-            "angularVelocity": 0.0000016664161452516387,
-            "velocityX": 2.999591671588281,
-            "velocityY": 2.30501229713074,
-            "timestamp": 0.6741906875518693
-        },
-        {
-            "x": 3.492751156794414,
-            "y": 5.750316609379858,
-            "heading": -0.5683840590142086,
-            "angularVelocity": 0.000001666416198927138,
-            "velocityX": 2.999592012896559,
-            "velocityY": 2.3050118529742454,
-            "timestamp": 0.7062950060067202
-        },
-        {
-            "x": 3.5890510233460025,
-            "y": 5.824317431802072,
-            "heading": -0.5683840055150509,
-            "angularVelocity": 0.0000016664162437925357,
-            "velocityX": 2.999592303665221,
-            "velocityY": 2.305011474586604,
-            "timestamp": 0.7383993244615712
-        },
-        {
-            "x": 3.6853508979893648,
-            "y": 5.898318243694168,
-            "heading": -0.5683839520158918,
-            "angularVelocity": 0.0000016664162889267752,
-            "velocityX": 2.9995925557115117,
-            "velocityY": 2.3050111465896648,
-            "timestamp": 0.7705036429164221
-        },
-        {
-            "x": 3.7816507797340138,
-            "y": 5.972319046345101,
-            "heading": -0.5683838985167311,
-            "angularVelocity": 0.0000016664163350532929,
-            "velocityX": 2.9995927769056334,
-            "velocityY": 2.305010858741686,
-            "timestamp": 0.8026079613712731
-        },
-        {
-            "x": 3.877950667767949,
-            "y": 6.046319840811553,
-            "heading": -0.5683838450175687,
-            "angularVelocity": 0.0000016664163859759036,
-            "velocityX": 2.999592972807203,
-            "velocityY": 2.305010603807763,
-            "timestamp": 0.8347122798261241
-        },
-        {
-            "x": 3.9742505615842325,
-            "y": 6.120320627753221,
-            "heading": -0.5683837915184047,
-            "angularVelocity": 0.0000016664164376020863,
-            "velocityX": 2.999593152918432,
-            "velocityY": 2.3050103694223405,
-            "timestamp": 0.866816598280975
-        },
-        {
-            "x": 4.070552264919673,
-            "y": 6.194319059842733,
-            "heading": -0.5683837380195041,
-            "angularVelocity": 0.0000016664082331173329,
-            "velocityX": 2.999649516649054,
-            "velocityY": 2.304937019409932,
-            "timestamp": 0.898920916735826
-        },
-        {
-            "x": 4.171102213813095,
-            "y": 6.262432889296395,
-            "heading": -0.5683836849080734,
-            "angularVelocity": 0.0000016543391427080076,
-            "velocityX": 3.131975812999348,
-            "velocityY": 2.1216407241116846,
-            "timestamp": 0.9310252351906769
-        },
-        {
-            "x": 4.276216983795166,
-            "y": 6.32326602935791,
-            "heading": -0.5683836309724583,
-            "angularVelocity": 0.0000016800112198234746,
-            "velocityX": 3.274162948822482,
-            "velocityY": 1.8948584797732355,
-            "timestamp": 0.9631295536455279
-        },
-        {
-            "x": 4.413834682670106,
-            "y": 6.38740625436239,
-            "heading": -0.5683835821873748,
-            "angularVelocity": 0.0000012155044166425132,
-            "velocityX": 3.428812837046335,
-            "velocityY": 1.5980853383274327,
-            "timestamp": 1.0032652231659112
-        },
-        {
-            "x": 4.5565820466376845,
-            "y": 6.439134291384663,
-            "heading": -0.5683835352510233,
-            "angularVelocity": 0.0000011694423477450916,
-            "velocityX": 3.556620972650851,
-            "velocityY": 1.2888295533727718,
-            "timestamp": 1.0434008926862943
-        },
-        {
-            "x": 4.7007810920363475,
-            "y": 6.4866657027970485,
-            "heading": -0.5683834885999294,
-            "angularVelocity": 0.000001162335011018839,
-            "velocityX": 3.5927903314389917,
-            "velocityY": 1.184268556632563,
-            "timestamp": 1.0835365622066775
-        },
-        {
-            "x": 4.84498021653845,
-            "y": 6.534196874227661,
-            "heading": -0.5683834419488406,
-            "angularVelocity": 0.0000011623348856187121,
-            "velocityX": 3.5927923023401758,
-            "velocityY": 1.1842625773683353,
-            "timestamp": 1.1236722317270607
-        },
-        {
-            "x": 4.989179343628637,
-            "y": 6.581728037806586,
-            "heading": -0.5683833952977476,
-            "angularVelocity": 0.0000011623349868626642,
-            "velocityX": 3.5927923668235673,
-            "velocityY": 1.1842623817396558,
-            "timestamp": 1.1638079012474438
-        },
-        {
-            "x": 5.133378473125504,
-            "y": 6.629259194084157,
-            "heading": -0.5683833486466496,
-            "angularVelocity": 0.0000011623351115999292,
-            "velocityX": 3.592792426787221,
-            "velocityY": 1.1842621998228242,
-            "timestamp": 1.203943570767827
-        },
-        {
-            "x": 5.277577604867278,
-            "y": 6.676790343551162,
-            "heading": -0.5683833019955459,
-            "angularVelocity": 0.0000011623352597981392,
-            "velocityX": 3.5927924827201663,
-            "velocityY": 1.184262030134241,
-            "timestamp": 1.2440792402882102
-        },
-        {
-            "x": 5.421776738706852,
-            "y": 6.7243214866538885,
-            "heading": -0.5683832553444353,
-            "angularVelocity": 0.0000011623354265532326,
-            "velocityX": 3.5927925349879053,
-            "velocityY": 1.1842618715650601,
-            "timestamp": 1.2842149098085933
-        },
-        {
-            "x": 5.565975874510482,
-            "y": 6.771852623798087,
-            "heading": -0.5683832086933173,
-            "angularVelocity": 0.0000011623356113966992,
-            "velocityX": 3.5927925839233144,
-            "velocityY": 1.18426172310541,
-            "timestamp": 1.3243505793289765
-        },
-        {
-            "x": 5.710175012156514,
-            "y": 6.819383755352828,
-            "heading": -0.5683831620421912,
-            "angularVelocity": 0.0000011623358144790056,
-            "velocityX": 3.5927926298276605,
-            "velocityY": 1.1842615838413222,
-            "timestamp": 1.3644862488493597
-        },
-        {
-            "x": 5.854374151534237,
-            "y": 6.866914881653982,
-            "heading": -0.5683831153910563,
-            "angularVelocity": 0.000001162336033125057,
-            "velocityX": 3.592792672973599,
-            "velocityY": 1.1842614529456208,
-            "timestamp": 1.4046219183697428
-        },
-        {
-            "x": 5.998573292523273,
-            "y": 6.9144460030667485,
-            "heading": -0.568383068739913,
-            "angularVelocity": 0.000001162336242705986,
-            "velocityX": 3.59279271312027,
-            "velocityY": 1.184261331149038,
-            "timestamp": 1.444757587890126
-        },
-        {
-            "x": 6.142772435013044,
-            "y": 6.9619771199265985,
-            "heading": -0.5683830220887615,
-            "angularVelocity": 0.0000011623364467362213,
-            "velocityX": 3.5927927505118338,
-            "velocityY": 1.1842612177108516,
-            "timestamp": 1.4848932574105091
-        },
-        {
-            "x": 6.286971578902684,
-            "y": 7.00950823253954,
-            "heading": -0.5683829754376022,
-            "angularVelocity": 0.0000011623366413601916,
-            "velocityX": 3.59279278539024,
-            "velocityY": 1.1842611118970372,
-            "timestamp": 1.5250289269308923
-        },
-        {
-            "x": 6.431170724100105,
-            "y": 7.057039341184945,
-            "heading": -0.5683829287864354,
-            "angularVelocity": 0.0000011623368281432719,
-            "velocityX": 3.592792817974252,
-            "velocityY": 1.1842610130439195,
-            "timestamp": 1.5651645964512755
-        },
-        {
-            "x": 6.575369870521147,
-            "y": 7.10457044611814,
-            "heading": -0.5683828821352613,
-            "angularVelocity": 0.0000011623370098744323,
-            "velocityX": 3.5927928484613587,
-            "velocityY": 1.1842609205523604,
-            "timestamp": 1.6053002659716586
-        },
-        {
-            "x": 6.719569018075523,
-            "y": 7.152101547613029,
-            "heading": -0.5683828354840802,
-            "angularVelocity": 0.0000011623371832362076,
-            "velocityX": 3.5927928766989634,
-            "velocityY": 1.1842608348853054,
-            "timestamp": 1.6454359354920418
-        },
-        {
-            "x": 6.8637678827817385,
-            "y": 7.1996335071853474,
-            "heading": -0.5683827888209949,
-            "angularVelocity": 0.0000011626337817909648,
-            "velocityX": 3.592785829397527,
-            "velocityY": 1.1842822143076228,
-            "timestamp": 1.685571605012425
-        },
-        {
-            "x": 6.99756534811195,
-            "y": 7.244965845733408,
-            "heading": -0.5424482126349314,
-            "angularVelocity": 0.6461727559544621,
-            "velocityX": 3.3336298342366253,
-            "velocityY": 1.1294775716906411,
-            "timestamp": 1.7257072745328081
-        },
-        {
-            "x": 7.121516587135396,
-            "y": 7.285176196921745,
-            "heading": -0.489772109644447,
-            "angularVelocity": 1.3124510845329824,
-            "velocityX": 3.088306249893159,
-            "velocityY": 1.0018607305881775,
-            "timestamp": 1.7658429440531913
-        },
-        {
-            "x": 7.233956124618328,
-            "y": 7.321947450112657,
-            "heading": -0.4161805837381191,
-            "angularVelocity": 1.8335691614401448,
-            "velocityX": 2.8014865287304875,
-            "velocityY": 0.9161739079059802,
-            "timestamp": 1.8059786135735745
-        },
-        {
-            "x": 7.333707629732149,
-            "y": 7.35452494917019,
-            "heading": -0.3377597360881954,
-            "angularVelocity": 1.953894094381483,
-            "velocityX": 2.4853579448365832,
-            "velocityY": 0.8116844554190893,
-            "timestamp": 1.8461142830939576
-        },
-        {
-            "x": 7.420722566948463,
-            "y": 7.382895867936689,
-            "heading": -0.26184604843223563,
-            "angularVelocity": 1.8914269666638126,
-            "velocityX": 2.1680200743162716,
-            "velocityY": 0.7068754328887092,
-            "timestamp": 1.8862499526143408
-        },
-        {
-            "x": 7.495090208476241,
-            "y": 7.40709623575076,
-            "heading": -0.19218016370533045,
-            "angularVelocity": 1.7357598754276362,
-            "velocityX": 1.8529064649092093,
-            "velocityY": 0.6029640990984653,
-            "timestamp": 1.926385622134724
-        },
-        {
-            "x": 7.556901497509452,
-            "y": 7.427169942598205,
-            "heading": -0.13107853540878284,
-            "angularVelocity": 1.522377202790068,
-            "velocityX": 1.5400587500308252,
-            "velocityY": 0.500146305949861,
-            "timestamp": 1.9665212916551071
-        },
-        {
-            "x": 7.606235479245265,
-            "y": 7.443160506733181,
-            "heading": -0.08018624320935208,
-            "angularVelocity": 1.2680065589434022,
-            "velocityX": 1.2291804852229526,
-            "velocityY": 0.3984127915657456,
-            "timestamp": 2.0066569611754903
-        },
-        {
-            "x": 7.643159935464096,
-            "y": 7.455107253348189,
-            "heading": -0.040758426269481916,
-            "angularVelocity": 0.982363503861482,
-            "velocityX": 0.9199910369024534,
-            "velocityY": 0.29765908374694494,
-            "timestamp": 2.0467926306958737
-        },
-        {
-            "x": 7.667732329164437,
-            "y": 7.4630440972663505,
-            "heading": -0.013776014916518935,
-            "angularVelocity": 0.6722800859036306,
-            "velocityX": 0.6122333075286244,
-            "velocityY": 0.1977503804721968,
-            "timestamp": 2.086928300216257
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 2.662292212835229e-35,
-            "angularVelocity": 0.3432362056280802,
-            "velocityX": 0.30565506897381545,
-            "velocityY": 0.09856326756028964,
-            "timestamp": 2.1270639697366405
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -1.5591265591207004e-41,
-            "angularVelocity": 3.563315968840566e-34,
-            "velocityX": -7.620676928265867e-35,
-            "velocityY": -1.1745137620932586e-33,
-            "timestamp": 2.167199639257024
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.1.traj
deleted file mode 100644
index 0646af70..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.1.traj	
+++ /dev/null
@@ -1,553 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -3.138879459525789e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 2.304641315068496,
-            "y": 4.371901353899003,
-            "heading": -0.39554860460981267,
-            "angularVelocity": -0.1412451028521288,
-            "velocityX": 0.11814916908296796,
-            "velocityY": 0.3029604871416256,
-            "timestamp": 0.039283518492098594
-        },
-        {
-            "x": 2.314310378834228,
-            "y": 4.395560050627161,
-            "heading": -0.4062436571914542,
-            "angularVelocity": -0.27225291909092403,
-            "velocityX": 0.24613538035494495,
-            "velocityY": 0.6022550330596644,
-            "timestamp": 0.07856703698419719
-        },
-        {
-            "x": 2.3294686593461313,
-            "y": 4.43078385259534,
-            "heading": -0.42159423390969814,
-            "angularVelocity": -0.3907637937605655,
-            "velocityX": 0.385868707635076,
-            "velocityY": 0.8966559850097278,
-            "timestamp": 0.11785055547629578
-        },
-        {
-            "x": 2.3506733572393026,
-            "y": 4.477309554936099,
-            "heading": -0.44099148005781696,
-            "angularVelocity": -0.49377568233887975,
-            "velocityX": 0.5397861165984255,
-            "velocityY": 1.1843568022074944,
-            "timestamp": 0.15713407396839438
-        },
-        {
-            "x": 2.3786040121360466,
-            "y": 4.534766426387325,
-            "heading": -0.4636665715847995,
-            "angularVelocity": -0.5772164102747018,
-            "velocityX": 0.7110018646207296,
-            "velocityY": 1.462620296162777,
-            "timestamp": 0.19641759246049298
-        },
-        {
-            "x": 2.4140943920114943,
-            "y": 4.602616046670193,
-            "heading": -0.48862953085625704,
-            "angularVelocity": -0.6354562989682879,
-            "velocityX": 0.903441983756771,
-            "velocityY": 1.727177780588029,
-            "timestamp": 0.23570111095259158
-        },
-        {
-            "x": 2.458163120855333,
-            "y": 4.68005234098581,
-            "heading": -0.5145799632934547,
-            "angularVelocity": -0.6605933845360225,
-            "velocityX": 1.1218121628463973,
-            "velocityY": 1.9712158505149888,
-            "timestamp": 0.27498462944469015
-        },
-        {
-            "x": 2.5115097740388146,
-            "y": 4.766403162212065,
-            "heading": -0.5400224328341426,
-            "angularVelocity": -0.6476626971635644,
-            "velocityX": 1.3579907103842537,
-            "velocityY": 2.1981437646332993,
-            "timestamp": 0.31426814793678876
-        },
-        {
-            "x": 2.5744195202560527,
-            "y": 4.861351309968927,
-            "heading": -0.563860333505168,
-            "angularVelocity": -0.6068168429413633,
-            "velocityX": 1.6014285031491418,
-            "velocityY": 2.416997035944537,
-            "timestamp": 0.35355166642888736
-        },
-        {
-            "x": 2.6473242968990576,
-            "y": 4.964346697364103,
-            "heading": -0.5842892856317753,
-            "angularVelocity": -0.5200387569839264,
-            "velocityX": 1.8558616804566808,
-            "velocityY": 2.6218473127835553,
-            "timestamp": 0.39283518492098596
-        },
-        {
-            "x": 2.7309171688764238,
-            "y": 5.07424676618957,
-            "heading": -0.5978668700067779,
-            "angularVelocity": -0.3456305569399124,
-            "velocityX": 2.127937496081736,
-            "velocityY": 2.7976126641403014,
-            "timestamp": 0.43211870341308456
-        },
-        {
-            "x": 2.826173576921219,
-            "y": 5.187875652325513,
-            "heading": -0.5974605263008922,
-            "angularVelocity": 0.010343872481393205,
-            "velocityX": 2.4248441000503926,
-            "velocityY": 2.8925333192550147,
-            "timestamp": 0.47140222190518316
-        },
-        {
-            "x": 2.9311441629204515,
-            "y": 5.293067244130248,
-            "heading": -0.5974604636085413,
-            "angularVelocity": 0.000001595894492248037,
-            "velocityX": 2.672127905761066,
-            "velocityY": 2.677753822532693,
-            "timestamp": 0.5106857403972818
-        },
-        {
-            "x": 3.044820699228354,
-            "y": 5.388784226639372,
-            "heading": -0.597460407679312,
-            "angularVelocity": 0.0000014237326879146737,
-            "velocityX": 2.8937463005192323,
-            "velocityY": 2.436568468997834,
-            "timestamp": 0.5499692588893803
-        },
-        {
-            "x": 3.1663502143390825,
-            "y": 5.474310087323753,
-            "heading": -0.5974603544159754,
-            "angularVelocity": 0.0000013558698005366095,
-            "velocityX": 3.0936514796952492,
-            "velocityY": 2.177143595261183,
-            "timestamp": 0.5892527773814789
-        },
-        {
-            "x": 3.289918343405979,
-            "y": 5.556863181758907,
-            "heading": -0.5974603016382944,
-            "angularVelocity": 0.0000013435069733969365,
-            "velocityX": 3.145546371864123,
-            "velocityY": 2.1014689519673895,
-            "timestamp": 0.6285362958735774
-        },
-        {
-            "x": 3.413486607435696,
-            "y": 5.639416074177005,
-            "heading": -0.5974602488606314,
-            "angularVelocity": 0.0000013435065140042136,
-            "velocityX": 3.145549807473418,
-            "velocityY": 2.101463809427605,
-            "timestamp": 0.667819814365676
-        },
-        {
-            "x": 3.5370548851900594,
-            "y": 5.721968946051526,
-            "heading": -0.5974601960829673,
-            "angularVelocity": 0.0000013435065431603766,
-            "velocityX": 3.145550156847558,
-            "velocityY": 2.1014632864709872,
-            "timestamp": 0.7071033328577745
-        },
-        {
-            "x": 3.6606231741521,
-            "y": 5.804521801149959,
-            "heading": -0.5974601433053026,
-            "angularVelocity": 0.0000013435065598734946,
-            "velocityX": 3.1455504421498475,
-            "velocityY": 2.1014628594194082,
-            "timestamp": 0.746386851349873
-        },
-        {
-            "x": 3.7841914725218415,
-            "y": 5.887074642166574,
-            "heading": -0.597460090527637,
-            "angularVelocity": 0.0000013435065802330538,
-            "velocityX": 3.1455506816319834,
-            "velocityY": 2.101462500953099,
-            "timestamp": 0.7856703698419716
-        },
-        {
-            "x": 3.9077597789317933,
-            "y": 5.969627471148282,
-            "heading": -0.5974600377499704,
-            "angularVelocity": 0.0000013435066065618314,
-            "velocityX": 3.14555088630331,
-            "velocityY": 2.1014621945928877,
-            "timestamp": 0.8249538883340701
-        },
-        {
-            "x": 4.031328092300017,
-            "y": 6.052180289714568,
-            "heading": -0.5974599849723026,
-            "angularVelocity": 0.0000013435066364860128,
-            "velocityX": 3.1455510634328734,
-            "velocityY": 2.101461929458217,
-            "timestamp": 0.8642374068261687
-        },
-        {
-            "x": 4.154896411746533,
-            "y": 6.134733099182619,
-            "heading": -0.5974599321946334,
-            "angularVelocity": 0.0000013435066718879358,
-            "velocityX": 3.14555121816168,
-            "velocityY": 2.1014616978538454,
-            "timestamp": 0.9035209253182672
-        },
-        {
-            "x": 4.27846473654201,
-            "y": 6.217285900644126,
-            "heading": -0.5974598794169629,
-            "angularVelocity": 0.000001343506708988157,
-            "velocityX": 3.14555135432466,
-            "velocityY": 2.1014614940395124,
-            "timestamp": 0.9428044438103658
-        },
-        {
-            "x": 4.4020330660739395,
-            "y": 6.299838695015915,
-            "heading": -0.5974598266392908,
-            "angularVelocity": 0.0000013435067467217265,
-            "velocityX": 3.1455514748956417,
-            "velocityY": 2.1014613135638847,
-            "timestamp": 0.9820879623024643
-        },
-        {
-            "x": 4.525601399950297,
-            "y": 6.382391482884783,
-            "heading": -0.5974597738616171,
-            "angularVelocity": 0.0000013435067857429384,
-            "velocityX": 3.1455515854872873,
-            "velocityY": 2.101461148025713,
-            "timestamp": 1.0213714807945629
-        },
-        {
-            "x": 4.6491721527053596,
-            "y": 6.464940649963802,
-            "heading": -0.5974597210842042,
-            "angularVelocity": 0.0000013435001464771913,
-            "velocityX": 3.1456131603868194,
-            "velocityY": 2.101368977314248,
-            "timestamp": 1.0606549992866614
-        },
-        {
-            "x": 4.778837800088318,
-            "y": 6.537541722502267,
-            "heading": -0.5974596685786843,
-            "angularVelocity": 0.0000013365788503576918,
-            "velocityX": 3.300764604603488,
-            "velocityY": 1.8481305984081453,
-            "timestamp": 1.09993851777876
-        },
-        {
-            "x": 4.914297553492409,
-            "y": 6.59865432938236,
-            "heading": -0.5974596146272461,
-            "angularVelocity": 0.0000013733860988547744,
-            "velocityX": 3.4482591835906553,
-            "velocityY": 1.5556805812161723,
-            "timestamp": 1.1392220362708585
-        },
-        {
-            "x": 5.054536029054771,
-            "y": 6.647820017049671,
-            "heading": -0.5974595570647036,
-            "angularVelocity": 0.0000014653102566024163,
-            "velocityX": 3.5699061831898264,
-            "velocityY": 1.2515601848929776,
-            "timestamp": 1.178505554762957
-        },
-        {
-            "x": 5.198501873691458,
-            "y": 6.684670110841039,
-            "heading": -0.5974594930097576,
-            "angularVelocity": 0.0000016305806703071774,
-            "velocityX": 3.6647899720507366,
-            "velocityY": 0.9380548180449542,
-            "timestamp": 1.2177890732550556
-        },
-        {
-            "x": 5.345115761176417,
-            "y": 6.7089283098414425,
-            "heading": -0.5974594180971905,
-            "angularVelocity": 0.0000019069719319939646,
-            "velocityX": 3.7321984667555648,
-            "velocityY": 0.6175159438731035,
-            "timestamp": 1.2570725917471541
-        },
-        {
-            "x": 5.493278503417969,
-            "y": 6.720412731170654,
-            "heading": -0.5974593280342978,
-            "angularVelocity": 0.0000022926381384477543,
-            "velocityX": 3.7716260642834416,
-            "velocityY": 0.2923470648746024,
-            "timestamp": 1.2963561102392527
-        },
-        {
-            "x": 5.617528419554277,
-            "y": 6.721027319451783,
-            "heading": -0.5974592417476945,
-            "angularVelocity": 0.0000026270679336343826,
-            "velocityX": 3.7828927995748978,
-            "velocityY": 0.018711655155499447,
-            "timestamp": 1.3292013222818118
-        },
-        {
-            "x": 5.7414972044827435,
-            "y": 6.712651079745108,
-            "heading": -0.5974591685782128,
-            "angularVelocity": 0.0000022277061710620633,
-            "velocityX": 3.7743335244066216,
-            "velocityY": -0.25502163589934396,
-            "timestamp": 1.3620465343243708
-        },
-        {
-            "x": 5.864535126960113,
-            "y": 6.695327921921503,
-            "heading": -0.597459104000633,
-            "angularVelocity": 0.000001966118527479946,
-            "velocityX": 3.745992637160019,
-            "velocityY": -0.5274180541456184,
-            "timestamp": 1.39489174636693
-        },
-        {
-            "x": 5.985997338800097,
-            "y": 6.669148654285341,
-            "heading": -0.5974590451258569,
-            "angularVelocity": 0.0000017924918823644274,
-            "velocityX": 3.6980188065957362,
-            "velocityY": -0.7970497374814797,
-            "timestamp": 1.427736958409489
-        },
-        {
-            "x": 6.105247258928482,
-            "y": 6.634250515985412,
-            "heading": -0.5974589899613981,
-            "angularVelocity": 0.0000016795281661896423,
-            "velocityX": 3.6306637318675383,
-            "velocityY": -1.062503059946642,
-            "timestamp": 1.460582170452048
-        },
-        {
-            "x": 6.221659925637703,
-            "y": 6.5908164867561565,
-            "heading": -0.5974589370314296,
-            "angularVelocity": 0.0000016114972403778573,
-            "velocityX": 3.544281174328253,
-            "velocityY": -1.3223854110871922,
-            "timestamp": 1.4934273824946072
-        },
-        {
-            "x": 6.334625365293315,
-            "y": 6.539074496952364,
-            "heading": -0.5974588851619594,
-            "angularVelocity": 0.000001579209477235118,
-            "velocityX": 3.439327458421827,
-            "velocityY": -1.5753282316046957,
-            "timestamp": 1.5262725945371662
-        },
-        {
-            "x": 6.443562887659525,
-            "y": 6.4793163144818955,
-            "heading": -0.5974588333468978,
-            "angularVelocity": 0.000001577552964572491,
-            "velocityX": 3.3166941417319027,
-            "velocityY": -1.819387933752184,
-            "timestamp": 1.5591178065797253
-        },
-        {
-            "x": 6.552407332287041,
-            "y": 6.419388766569548,
-            "heading": -0.5974587815290724,
-            "angularVelocity": 0.0000015776371105880298,
-            "velocityX": 3.313860312014723,
-            "velocityY": -1.82454440649205,
-            "timestamp": 1.5919630186222844
-        },
-        {
-            "x": 6.661251765496281,
-            "y": 6.359461197918566,
-            "heading": -0.5974587297112393,
-            "angularVelocity": 0.0000015776373457032493,
-            "velocityX": 3.3138599643757076,
-            "velocityY": -1.8245450378971961,
-            "timestamp": 1.6248082306648435
-        },
-        {
-            "x": 6.7700961954772865,
-            "y": 6.299533623404254,
-            "heading": -0.5974586778933993,
-            "angularVelocity": 0.0000015776375522202912,
-            "velocityX": 3.3138598660894165,
-            "velocityY": -1.8245452164111537,
-            "timestamp": 1.6576534427074026
-        },
-        {
-            "x": 6.878940549868732,
-            "y": 6.239605911600315,
-            "heading": -0.5974586260742238,
-            "angularVelocity": 0.0000015776782163840074,
-            "velocityX": 3.313857564701912,
-            "velocityY": -1.8245493963088628,
-            "timestamp": 1.6904986547499616
-        },
-        {
-            "x": 6.983465419726436,
-            "y": 6.181138244955353,
-            "heading": -0.5831453971579332,
-            "angularVelocity": 0.4357782466962171,
-            "velocityX": 3.182347238991671,
-            "velocityY": -1.7800970981449478,
-            "timestamp": 1.7233438667925207
-        },
-        {
-            "x": 7.08190934617205,
-            "y": 6.126284607678917,
-            "heading": -0.5460039664027865,
-            "angularVelocity": 1.1308019782912204,
-            "velocityX": 2.997207821891462,
-            "velocityY": -1.67006494600528,
-            "timestamp": 1.7561890788350798
-        },
-        {
-            "x": 7.174234078294548,
-            "y": 6.075101338854014,
-            "heading": -0.4864629486170815,
-            "angularVelocity": 1.8127761729340726,
-            "velocityX": 2.8109038237566843,
-            "velocityY": -1.5583175032799648,
-            "timestamp": 1.7890342908776389
-        },
-        {
-            "x": 7.258956033298187,
-            "y": 6.028257642494675,
-            "heading": -0.41936488473987615,
-            "angularVelocity": 2.0428567728619895,
-            "velocityX": 2.5794309043853687,
-            "velocityY": -1.4261955836547433,
-            "timestamp": 1.821879502920198
-        },
-        {
-            "x": 7.335844716602702,
-            "y": 5.985834491701992,
-            "heading": -0.35186229403264746,
-            "angularVelocity": 2.0551729311350044,
-            "velocityX": 2.3409403843979617,
-            "velocityY": -1.2916083701234404,
-            "timestamp": 1.854724714962757
-        },
-        {
-            "x": 7.404915151007572,
-            "y": 5.947794038432469,
-            "heading": -0.2870870788105502,
-            "angularVelocity": 1.9721356993583767,
-            "velocityX": 2.1029072461275145,
-            "velocityY": -1.1581734719873862,
-            "timestamp": 1.8875699270053161
-        },
-        {
-            "x": 7.466209510665893,
-            "y": 5.914092129035952,
-            "heading": -0.22688750133470567,
-            "angularVelocity": 1.8328265744751362,
-            "velocityX": 1.866158135283738,
-            "velocityY": -1.0260828687243908,
-            "timestamp": 1.9204151390478752
-        },
-        {
-            "x": 7.519766848297347,
-            "y": 5.8846886900937605,
-            "heading": -0.17246123732016092,
-            "angularVelocity": 1.657053208974533,
-            "velocityX": 1.6305980172109753,
-            "velocityY": -0.8952123342686217,
-            "timestamp": 1.9532603510904343
-        },
-        {
-            "x": 7.565618503926309,
-            "y": 5.859549729252428,
-            "heading": -0.12462132977080041,
-            "angularVelocity": 1.4565260680124767,
-            "velocityX": 1.395992072438578,
-            "velocityY": -0.7653767254946843,
-            "timestamp": 1.9861055631329934
-        },
-        {
-            "x": 7.603788954978725,
-            "y": 5.838647308998552,
-            "heading": -0.08394075693399232,
-            "angularVelocity": 1.2385541242393951,
-            "velocityX": 1.1621313634075436,
-            "velocityY": -0.6363916977236806,
-            "timestamp": 2.0189507751755524
-        },
-        {
-            "x": 7.634297586862898,
-            "y": 5.821958675739452,
-            "heading": -0.050840342274680116,
-            "angularVelocity": 1.0077698574891136,
-            "velocityX": 0.928860859374263,
-            "velocityY": -0.5080994221459559,
-            "timestamp": 2.0517959872181115
-        },
-        {
-            "x": 7.657160251333572,
-            "y": 5.809465047264352,
-            "heading": -0.025644988874275507,
-            "angularVelocity": 0.7670936442051631,
-            "velocityX": 0.6960729752956952,
-            "velocityY": -0.3803789867120345,
-            "timestamp": 2.0846411992606706
-        },
-        {
-            "x": 7.672390418071531,
-            "y": 5.801150340997017,
-            "heading": -0.00862072816733467,
-            "angularVelocity": 0.5183178810016766,
-            "velocityX": 0.46369518693407513,
-            "velocityY": -0.2531481987862983,
-            "timestamp": 2.1174864113032297
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 8.329701949714689e-33,
-            "angularVelocity": 0.26246529193262896,
-            "velocityX": 0.2316800975013136,
-            "velocityY": -0.12636060901771562,
-            "timestamp": 2.1503316233457888
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 5.789840595031671e-40,
-            "angularVelocity": 1.2947513524749855e-30,
-            "velocityX": 9.16355000594536e-32,
-            "velocityY": -1.9486684829089695e-31,
-            "timestamp": 2.183176835388348
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.1.traj
deleted file mode 100644
index ea265506..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.1.traj	
+++ /dev/null
@@ -1,310 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 1.9823938961383557e-29,
-            "timestamp": 0
-        },
-        {
-            "x": 2.312764146242865,
-            "y": 4.398513079462153,
-            "heading": -0.39804543849617763,
-            "angularVelocity": -0.11524733034669515,
-            "velocityX": 0.18284072142228638,
-            "velocityY": 0.5516827447028667,
-            "timestamp": 0.06981019405093988
-        },
-        {
-            "x": 2.339895425931373,
-            "y": 4.474992910905513,
-            "heading": -0.4126392714825622,
-            "angularVelocity": -0.20905017072795415,
-            "velocityX": 0.3886435220157258,
-            "velocityY": 1.0955395910772,
-            "timestamp": 0.13962038810187977
-        },
-        {
-            "x": 2.3838571031189573,
-            "y": 4.588440455462814,
-            "heading": -0.43142102712692415,
-            "angularVelocity": -0.2690403013169746,
-            "velocityX": 0.6297314852832604,
-            "velocityY": 1.6250856497386854,
-            "timestamp": 0.20943058215281965
-        },
-        {
-            "x": 2.448821264845437,
-            "y": 4.736675156040894,
-            "heading": -0.45019318323311186,
-            "angularVelocity": -0.26890279219946406,
-            "velocityX": 0.9305827409539079,
-            "velocityY": 2.1233961972720627,
-            "timestamp": 0.27924077620375953
-        },
-        {
-            "x": 2.542623225560298,
-            "y": 4.913429983875953,
-            "heading": -0.4600490129319591,
-            "angularVelocity": -0.14118037962391586,
-            "velocityX": 1.343671393412285,
-            "velocityY": 2.5319343433823196,
-            "timestamp": 0.34905097025469944
-        },
-        {
-            "x": 2.675927389267578,
-            "y": 5.095797559489528,
-            "heading": -0.44245218860567326,
-            "angularVelocity": 0.25206668688824874,
-            "velocityX": 1.9095228930350086,
-            "velocityY": 2.6123344605070336,
-            "timestamp": 0.41886116430563936
-        },
-        {
-            "x": 2.83772713712192,
-            "y": 5.250967901017311,
-            "heading": -0.38879601501351296,
-            "angularVelocity": 0.7686008371275667,
-            "velocityX": 2.3177094699953265,
-            "velocityY": 2.2227461710524636,
-            "timestamp": 0.48867135835657927
-        },
-        {
-            "x": 3.013235940099558,
-            "y": 5.368218424760011,
-            "heading": -0.31718431053283597,
-            "angularVelocity": 1.0258058360734288,
-            "velocityX": 2.514085591121856,
-            "velocityY": 1.6795616361728827,
-            "timestamp": 0.5584815524075192
-        },
-        {
-            "x": 3.1945050869972476,
-            "y": 5.445323486359522,
-            "heading": -0.23653218942173773,
-            "angularVelocity": 1.1553057859494003,
-            "velocityX": 2.5965999573717062,
-            "velocityY": 1.1044957351381008,
-            "timestamp": 0.6282917464584591
-        },
-        {
-            "x": 3.377225953863421,
-            "y": 5.481834951968023,
-            "heading": -0.15180568644047004,
-            "angularVelocity": 1.2136694952369833,
-            "velocityX": 2.6173952006491676,
-            "velocityY": 0.523010515923758,
-            "timestamp": 0.698101940509399
-        },
-        {
-            "x": 3.558790922164917,
-            "y": 5.477728366851807,
-            "heading": -0.06601521216801902,
-            "angularVelocity": 1.2289104110775417,
-            "velocityX": 2.6008374674955865,
-            "velocityY": -0.05882500648124433,
-            "timestamp": 0.7679121345603389
-        },
-        {
-            "x": 3.7207755893743903,
-            "y": 5.440802283745667,
-            "heading": 0.01087618717506099,
-            "angularVelocity": 1.2168377614801031,
-            "velocityX": 2.5634734381227786,
-            "velocityY": -0.5843703286511746,
-            "timestamp": 0.8311016588990232
-        },
-        {
-            "x": 3.8800031263189014,
-            "y": 5.370704697701765,
-            "heading": 0.08523277253593173,
-            "angularVelocity": 1.1767232961398173,
-            "velocityX": 2.519840726939224,
-            "velocityY": -1.1093228945227975,
-            "timestamp": 0.8942911832377075
-        },
-        {
-            "x": 4.035880089753818,
-            "y": 5.267514783368049,
-            "heading": 0.15438353932229767,
-            "angularVelocity": 1.0943390935099226,
-            "velocityX": 2.4668165343191313,
-            "velocityY": -1.633022489291642,
-            "timestamp": 0.9574807075763918
-        },
-        {
-            "x": 4.187424190703861,
-            "y": 5.131425904894565,
-            "heading": 0.2138534475851711,
-            "angularVelocity": 0.9411355581283213,
-            "velocityX": 2.3982472179488545,
-            "velocityY": -2.153662017503815,
-            "timestamp": 1.0206702319150762
-        },
-        {
-            "x": 4.332733928007212,
-            "y": 4.96304651892829,
-            "heading": 0.25476003945729764,
-            "angularVelocity": 0.6473635034690916,
-            "velocityX": 2.2995858699928062,
-            "velocityY": -2.664672471059999,
-            "timestamp": 1.0838597562537606
-        },
-        {
-            "x": 4.46716744223408,
-            "y": 4.765388489141315,
-            "heading": 0.2547605945579538,
-            "angularVelocity": 0.000008784690993212257,
-            "velocityX": 2.12746520302083,
-            "velocityY": -3.1280189533989655,
-            "timestamp": 1.147049280592445
-        },
-        {
-            "x": 4.612743650855858,
-            "y": 4.575786978724626,
-            "heading": 0.2547605407972418,
-            "angularVelocity": -8.507852266556338e-7,
-            "velocityX": 2.303802887345736,
-            "velocityY": -3.0005212478052368,
-            "timestamp": 1.2102388049311295
-        },
-        {
-            "x": 4.782527461386278,
-            "y": 4.409030186652972,
-            "heading": 0.25208197397278675,
-            "angularVelocity": -0.04238941267495421,
-            "velocityX": 2.6868980627285057,
-            "velocityY": -2.638994260793031,
-            "timestamp": 1.2734283292698139
-        },
-        {
-            "x": 4.970403968588328,
-            "y": 4.270066434839271,
-            "heading": 0.23891743895373094,
-            "angularVelocity": -0.2083341369570075,
-            "velocityX": 2.9732223682415455,
-            "velocityY": -2.1991580609051504,
-            "timestamp": 1.3366178536084983
-        },
-        {
-            "x": 5.173968377386639,
-            "y": 4.1604195633547185,
-            "heading": 0.21971651393307054,
-            "angularVelocity": -0.3038624711945638,
-            "velocityX": 3.2214898106793903,
-            "velocityY": -1.7352064702582655,
-            "timestamp": 1.3998073779471827
-        },
-        {
-            "x": 5.390789031982422,
-            "y": 4.081310272216797,
-            "heading": 0.19899217158132915,
-            "angularVelocity": -0.3279711719205329,
-            "velocityX": 3.4312753081359326,
-            "velocityY": -1.25193680386463,
-            "timestamp": 1.462996902285867
-        },
-        {
-            "x": 5.681530534379024,
-            "y": 4.031634954440038,
-            "heading": 0.1807325834147537,
-            "angularVelocity": -0.22892527550150313,
-            "velocityX": 3.645102941474752,
-            "velocityY": -0.6227925681552634,
-            "timestamp": 1.542759122191511
-        },
-        {
-            "x": 5.983259555492767,
-            "y": 4.033604453850848,
-            "heading": 0.1807316659794543,
-            "angularVelocity": -0.000011502133581259988,
-            "velocityX": 3.7828563632073102,
-            "velocityY": 0.024692133859281144,
-            "timestamp": 1.6225213420971547
-        },
-        {
-            "x": 6.284539191755923,
-            "y": 4.050188554583329,
-            "heading": 0.18073165818528908,
-            "angularVelocity": -9.771750653524246e-8,
-            "velocityX": 3.777222306745724,
-            "velocityY": 0.20791924738433482,
-            "timestamp": 1.7022835620027985
-        },
-        {
-            "x": 6.585818825161056,
-            "y": 4.0667727072366,
-            "heading": 0.18073165039112304,
-            "angularVelocity": -9.77175162921636e-8,
-            "velocityX": 3.777222270913928,
-            "velocityY": 0.20791989832899174,
-            "timestamp": 1.7820457819084423
-        },
-        {
-            "x": 6.88709845856161,
-            "y": 4.083356859971397,
-            "heading": 0.180731642596831,
-            "angularVelocity": -9.771909620772691e-8,
-            "velocityX": 3.777222270856537,
-            "velocityY": 0.20791989935110283,
-            "timestamp": 1.8618080018140861
-        },
-        {
-            "x": 7.15139892307148,
-            "y": 4.097904823415219,
-            "heading": 0.12048670978727856,
-            "angularVelocity": -0.7553066211144209,
-            "velocityX": 3.313604671767403,
-            "velocityY": 0.1823916568650699,
-            "timestamp": 1.94157022171973
-        },
-        {
-            "x": 7.3628393316310445,
-            "y": 4.109542993215659,
-            "heading": 0.07229126938267637,
-            "angularVelocity": -0.6042389550038586,
-            "velocityX": 2.650884200686756,
-            "velocityY": 0.1459108060709806,
-            "timestamp": 2.0213324416253737
-        },
-        {
-            "x": 7.521419659615098,
-            "y": 4.118271521022388,
-            "heading": 0.03614542857914898,
-            "angularVelocity": -0.4531699449471921,
-            "velocityX": 1.98816342087339,
-            "velocityY": 0.10943185654883955,
-            "timestamp": 2.1010946615310178
-        },
-        {
-            "x": 7.627139886542953,
-            "y": 4.12409050689369,
-            "heading": 0.012048510432709781,
-            "angularVelocity": -0.302109421915929,
-            "velocityX": 1.3254423842882344,
-            "velocityY": 0.07295416148382058,
-            "timestamp": 2.180856881436662
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 2.791295127527068e-32,
-            "angularVelocity": -0.15105535486544666,
-            "velocityX": 0.6627211920578459,
-            "velocityY": 0.03647708288149429,
-            "timestamp": 2.260619101342306
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -1.938584239537671e-30,
-            "velocityX": -1.689352589413745e-32,
-            "velocityY": 0,
-            "timestamp": 2.34038132124795
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.1.traj
deleted file mode 100644
index ce59bb95..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.1.traj	
+++ /dev/null
@@ -1,625 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.298402468716654,
-            "y": 4.3526843009931255,
-            "heading": -0.37533408415570846,
-            "angularVelocity": 0.4620245693037669,
-            "velocityX": -0.050327487964169394,
-            "velocityY": -0.2304685720750862,
-            "timestamp": 0.03174271850172795
-        },
-        {
-            "x": 2.2952744131725273,
-            "y": 4.338010072410149,
-            "heading": -0.34621912169362346,
-            "angularVelocity": 0.9172170449263883,
-            "velocityX": -0.09854403440450425,
-            "velocityY": -0.462286447903903,
-            "timestamp": 0.0634854370034559
-        },
-        {
-            "x": 2.2907034035575107,
-            "y": 4.3159283160610205,
-            "heading": -0.3028943022400045,
-            "angularVelocity": 1.364874260887912,
-            "velocityX": -0.14400183194038138,
-            "velocityY": -0.6956479278208441,
-            "timestamp": 0.09522815550518385
-        },
-        {
-            "x": 2.2847999846220546,
-            "y": 4.286378308280002,
-            "heading": -0.24568561151458213,
-            "angularVelocity": 1.8022618548662834,
-            "velocityX": -0.1859771063759033,
-            "velocityY": -0.9309224028625668,
-            "timestamp": 0.1269708740069118
-        },
-        {
-            "x": 2.2777013130983144,
-            "y": 4.249280679670162,
-            "heading": -0.1750916915555501,
-            "angularVelocity": 2.223940585151498,
-            "velocityX": -0.22363149278956135,
-            "velocityY": -1.1686972748670326,
-            "timestamp": 0.15871359250863973
-        },
-        {
-            "x": 2.2695785209744463,
-            "y": 4.204529290238701,
-            "heading": -0.09189311777806415,
-            "angularVelocity": 2.6210286233977427,
-            "velocityX": -0.25589465890975394,
-            "velocityY": -1.409815905623355,
-            "timestamp": 0.19045631101036767
-        },
-        {
-            "x": 2.2606569711878857,
-            "y": 4.151982062664549,
-            "heading": 0.002706319559794192,
-            "angularVelocity": 2.9801933105606198,
-            "velocityX": -0.2810581515277304,
-            "velocityY": -1.6554104391308149,
-            "timestamp": 0.2221990295120956
-        },
-        {
-            "x": 2.251280347316294,
-            "y": 4.091451352956859,
-            "heading": 0.10685098530745366,
-            "angularVelocity": 3.2808993893194844,
-            "velocityX": -0.2953944814487593,
-            "velocityY": -1.90691637530654,
-            "timestamp": 0.2539417480138236
-        },
-        {
-            "x": 2.242133674900212,
-            "y": 4.022709367456616,
-            "heading": 0.21739538615105536,
-            "angularVelocity": 3.4825120865934673,
-            "velocityX": -0.28815025454054716,
-            "velocityY": -2.165598560706175,
-            "timestamp": 0.2856844665155515
-        },
-        {
-            "x": 2.2351422077578347,
-            "y": 3.945850682832356,
-            "heading": 0.32707167243976176,
-            "angularVelocity": 3.455163623831899,
-            "velocityX": -0.22025420229828702,
-            "velocityY": -2.4213012700873704,
-            "timestamp": 0.31742718501727946
-        },
-        {
-            "x": 2.2344515310594897,
-            "y": 3.864378163543378,
-            "heading": 0.4256089749961829,
-            "angularVelocity": 3.1042490122910227,
-            "velocityX": -0.021758586880562782,
-            "velocityY": -2.566652231898267,
-            "timestamp": 0.3491699035190074
-        },
-        {
-            "x": 2.2416429003575074,
-            "y": 3.7806290345563274,
-            "heading": 0.5177286971400864,
-            "angularVelocity": 2.9020741288711216,
-            "velocityX": 0.22655177746121863,
-            "velocityY": -2.638372922674897,
-            "timestamp": 0.38091262202073534
-        },
-        {
-            "x": 2.256915898193716,
-            "y": 3.695399323053411,
-            "heading": 0.6036240465401915,
-            "angularVelocity": 2.705985922265222,
-            "velocityX": 0.4811496480800017,
-            "velocityY": -2.6850161399464656,
-            "timestamp": 0.4126553405224633
-        },
-        {
-            "x": 2.280071045497703,
-            "y": 3.6084777900686467,
-            "heading": 0.6811513337134051,
-            "angularVelocity": 2.4423644486842004,
-            "velocityX": 0.729463272111561,
-            "velocityY": -2.7383140791811433,
-            "timestamp": 0.4443980590241912
-        },
-        {
-            "x": 2.3096855944046597,
-            "y": 3.5244760350530098,
-            "heading": 0.76088689172136,
-            "angularVelocity": 2.511932240574098,
-            "velocityX": 0.932955660534971,
-            "velocityY": -2.6463314731869354,
-            "timestamp": 0.47614077752591916
-        },
-        {
-            "x": 2.3348942368332115,
-            "y": 3.446420745412462,
-            "heading": 0.8559069862879931,
-            "angularVelocity": 2.993445396350149,
-            "velocityX": 0.7941551202420774,
-            "velocityY": -2.458998262429792,
-            "timestamp": 0.5078834960276472
-        },
-        {
-            "x": 2.355075569752848,
-            "y": 3.373868279566975,
-            "heading": 0.9657696902827837,
-            "angularVelocity": 3.4610363944982643,
-            "velocityX": 0.6357783413709821,
-            "velocityY": -2.285641220097014,
-            "timestamp": 0.5396262145293751
-        },
-        {
-            "x": 2.370025604730125,
-            "y": 3.306495756130267,
-            "heading": 1.091029644988018,
-            "angularVelocity": 3.9461004166489144,
-            "velocityX": 0.4709752561508733,
-            "velocityY": -2.1224560030370347,
-            "timestamp": 0.571368933031103
-        },
-        {
-            "x": 2.3800631598495494,
-            "y": 3.2441375159647747,
-            "heading": 1.2334110423596254,
-            "angularVelocity": 4.485482154399005,
-            "velocityX": 0.31621598883778346,
-            "velocityY": -1.9644895934825695,
-            "timestamp": 0.603111651532831
-        },
-        {
-            "x": 2.3890320547539976,
-            "y": 3.1882487557001293,
-            "heading": 1.3963738258608078,
-            "angularVelocity": 5.133863487221998,
-            "velocityX": 0.28254967840764567,
-            "velocityY": -1.760679705539431,
-            "timestamp": 0.6348543700345589
-        },
-        {
-            "x": 2.399614951757013,
-            "y": 3.1248820965808815,
-            "heading": 1.5465533770663946,
-            "angularVelocity": 4.7311496397956585,
-            "velocityX": 0.33339605120587323,
-            "velocityY": -1.9962581061165,
-            "timestamp": 0.6665970885362869
-        },
-        {
-            "x": 2.4156041612276775,
-            "y": 3.056078363317597,
-            "heading": 1.6846738705269053,
-            "angularVelocity": 4.351249671731503,
-            "velocityX": 0.5037126694046519,
-            "velocityY": -2.167543818263053,
-            "timestamp": 0.6983398070380148
-        },
-        {
-            "x": 2.4386535219180887,
-            "y": 2.9838461830329104,
-            "heading": 1.8122491249375776,
-            "angularVelocity": 4.019039969866676,
-            "velocityX": 0.7261306459671242,
-            "velocityY": -2.275551171861821,
-            "timestamp": 0.7300825255397427
-        },
-        {
-            "x": 2.4695800740474128,
-            "y": 2.910121142766613,
-            "heading": 1.9308538821800434,
-            "angularVelocity": 3.736439814882526,
-            "velocityX": 0.9742880757878827,
-            "velocityY": -2.3225811696715355,
-            "timestamp": 0.7618252440414707
-        },
-        {
-            "x": 2.508646228883482,
-            "y": 2.8366969767650785,
-            "heading": 2.0420489888957944,
-            "angularVelocity": 3.503011461028351,
-            "velocityX": 1.2307123233299608,
-            "velocityY": -2.313102641084062,
-            "timestamp": 0.7935679625431986
-        },
-        {
-            "x": 2.555734299118471,
-            "y": 2.7651139971749186,
-            "heading": 2.147169748045839,
-            "angularVelocity": 3.3116495408016253,
-            "velocityX": 1.4834290337302587,
-            "velocityY": -2.255099215470886,
-            "timestamp": 0.8253106810449266
-        },
-        {
-            "x": 2.6104869842529297,
-            "y": 2.6966493129730225,
-            "heading": 2.247054267099527,
-            "angularVelocity": 3.1466907614812087,
-            "velocityX": 1.7248896036260801,
-            "velocityY": -2.1568626580665695,
-            "timestamp": 0.8570533995466545
-        },
-        {
-            "x": 2.751018773357776,
-            "y": 2.5737681266542305,
-            "heading": 2.423101617636397,
-            "angularVelocity": 2.7414576820608,
-            "velocityX": 2.188399607493837,
-            "velocityY": -1.9135395743649004,
-            "timestamp": 0.9212700940770386
-        },
-        {
-            "x": 2.920945282929504,
-            "y": 2.4615612619997704,
-            "heading": 2.5486951706813996,
-            "angularVelocity": 1.9557772937936253,
-            "velocityX": 2.646142265876451,
-            "velocityY": -1.7473161064272718,
-            "timestamp": 0.9854867886074228
-        },
-        {
-            "x": 3.118708267205298,
-            "y": 2.3583606370011725,
-            "heading": 2.6122511958810777,
-            "angularVelocity": 0.9897118757741349,
-            "velocityX": 3.079619493373657,
-            "velocityY": -1.6070684695514719,
-            "timestamp": 1.049703483137807
-        },
-        {
-            "x": 3.3434611246912245,
-            "y": 2.26616504003377,
-            "heading": 2.6122519169792926,
-            "angularVelocity": 0.000011229139401682067,
-            "velocityX": 3.499913209945475,
-            "velocityY": -1.4356951512629466,
-            "timestamp": 1.113920177668191
-        },
-        {
-            "x": 3.5722970045795748,
-            "y": 2.184629282343257,
-            "heading": 2.6122522165022306,
-            "angularVelocity": 0.000004664253435080619,
-            "velocityX": 3.5634951559220656,
-            "velocityY": -1.269697207039109,
-            "timestamp": 1.1781368721985752
-        },
-        {
-            "x": 3.80113295504209,
-            "y": 2.103093722723815,
-            "heading": 2.6122525160244625,
-            "angularVelocity": 0.000004664242440601037,
-            "velocityX": 3.56349625492233,
-            "velocityY": -1.2696941226220015,
-            "timestamp": 1.2423535667289594
-        },
-        {
-            "x": 4.029968923086795,
-            "y": 2.0215582124500746,
-            "heading": 2.612252815546594,
-            "angularVelocity": 0.00000466424087155042,
-            "velocityX": 3.563496528717025,
-            "velocityY": -1.2696933541971844,
-            "timestamp": 1.3065702612593435
-        },
-        {
-            "x": 4.258804909490432,
-            "y": 1.940022753702107,
-            "heading": 2.6122531150686714,
-            "angularVelocity": 0.000004664240033500949,
-            "velocityX": 3.5634968146073587,
-            "velocityY": -1.2696925518237052,
-            "timestamp": 1.3707869557897276
-        },
-        {
-            "x": 4.487640916105967,
-            "y": 1.8584873516805105,
-            "heading": 2.6122534145907514,
-            "angularVelocity": 0.000004664240074560689,
-            "velocityX": 3.563497129352575,
-            "velocityY": -1.2696916684650783,
-            "timestamp": 1.4350036503201118
-        },
-        {
-            "x": 4.716476945604857,
-            "y": 1.7769520138830965,
-            "heading": 2.6122537141129194,
-            "angularVelocity": 0.000004664241438058459,
-            "velocityX": 3.5634974856984654,
-            "velocityY": -1.2696906683484759,
-            "timestamp": 1.499220344850496
-        },
-        {
-            "x": 4.945312999982612,
-            "y": 1.6954167459104819,
-            "heading": 2.612254013635151,
-            "angularVelocity": 0.000004664242435524347,
-            "velocityX": 3.563497873118986,
-            "velocityY": -1.269689581017522,
-            "timestamp": 1.56343703938088
-        },
-        {
-            "x": 5.174149135873993,
-            "y": 1.613881706713074,
-            "heading": 2.6122543131567744,
-            "angularVelocity": 0.0000046642329655920085,
-            "velocityX": 3.563499142471556,
-            "velocityY": -1.269686018467187,
-            "timestamp": 1.6276537339112642
-        },
-        {
-            "x": 5.406651475687904,
-            "y": 1.5434785463415874,
-            "heading": 2.61225458420666,
-            "angularVelocity": 0.000004220863245612629,
-            "velocityX": 3.6205902766281897,
-            "velocityY": -1.096337344772168,
-            "timestamp": 1.6918704284416484
-        },
-        {
-            "x": 5.646764755249023,
-            "y": 1.5066063404083252,
-            "heading": 2.6122548161665473,
-            "angularVelocity": 0.0000036121430551488755,
-            "velocityX": 3.7391099202016678,
-            "velocityY": -0.5741841152508433,
-            "timestamp": 1.7560871229720325
-        },
-        {
-            "x": 5.787436421990979,
-            "y": 1.496717214982701,
-            "heading": 2.612254832075579,
-            "angularVelocity": 4.2677202387530557e-7,
-            "velocityX": 3.773625708136921,
-            "velocityY": -0.26528339929021244,
-            "timestamp": 1.7933647115888978
-        },
-        {
-            "x": 5.926771623718837,
-            "y": 1.4982765413849686,
-            "heading": 2.6080354125638485,
-            "angularVelocity": -0.11318917527356848,
-            "velocityX": 3.7377740057150746,
-            "velocityY": 0.04183013065287323,
-            "timestamp": 1.830642300205763
-        },
-        {
-            "x": 6.064421169286406,
-            "y": 1.511276136415188,
-            "heading": 2.60121232475526,
-            "angularVelocity": -0.18303458087688948,
-            "velocityX": 3.6925549820916683,
-            "velocityY": 0.34872413996054913,
-            "timestamp": 1.8679198888226283
-        },
-        {
-            "x": 6.200023778520788,
-            "y": 1.5356760041928015,
-            "heading": 2.5932728220304684,
-            "angularVelocity": -0.2129832700927332,
-            "velocityX": 3.6376443398227996,
-            "velocityY": 0.6545452289951588,
-            "timestamp": 1.9051974774394935
-        },
-        {
-            "x": 6.333175231231244,
-            "y": 1.5714002898334831,
-            "heading": 2.5855391112934316,
-            "angularVelocity": -0.20746274166290535,
-            "velocityX": 3.5718901798871894,
-            "velocityY": 0.9583314523868992,
-            "timestamp": 1.9424750660563588
-        },
-        {
-            "x": 6.463368734714764,
-            "y": 1.6183219287773098,
-            "heading": 2.5790588590244545,
-            "angularVelocity": -0.17383775371256402,
-            "velocityX": 3.4925409157130343,
-            "velocityY": 1.258709071181673,
-            "timestamp": 1.979752654673224
-        },
-        {
-            "x": 6.589861303526444,
-            "y": 1.6762129661852994,
-            "heading": 2.574283804517795,
-            "angularVelocity": -0.12809451157737914,
-            "velocityX": 3.393260495247066,
-            "velocityY": 1.5529716260079764,
-            "timestamp": 2.017030243290089
-        },
-        {
-            "x": 6.711340738368746,
-            "y": 1.7445488061219798,
-            "heading": 2.570091844488755,
-            "angularVelocity": -0.11245255352014773,
-            "velocityX": 3.258779318878531,
-            "velocityY": 1.8331614911851861,
-            "timestamp": 2.054307831906954
-        },
-        {
-            "x": 6.825305386557433,
-            "y": 1.8215960437325822,
-            "heading": 2.560973344137785,
-            "angularVelocity": -0.24461078866147912,
-            "velocityX": 3.057189384216965,
-            "velocityY": 2.0668514372666396,
-            "timestamp": 2.091585420523819
-        },
-        {
-            "x": 6.929260415482823,
-            "y": 1.9025407238929235,
-            "heading": 2.5356026772435865,
-            "angularVelocity": -0.680587662333947,
-            "velocityX": 2.788673645010342,
-            "velocityY": 2.171403332771371,
-            "timestamp": 2.128863009140684
-        },
-        {
-            "x": 7.023455735639986,
-            "y": 1.9838668818491878,
-            "heading": 2.488274688562586,
-            "angularVelocity": -1.2696097155701718,
-            "velocityX": 2.5268619471418052,
-            "velocityY": 2.1816367681967113,
-            "timestamp": 2.166140597757549
-        },
-        {
-            "x": 7.110101814938688,
-            "y": 2.060020595789651,
-            "heading": 2.4125158730705962,
-            "angularVelocity": -2.0322885224854694,
-            "velocityX": 2.3243477519224953,
-            "velocityY": 2.0428819772427436,
-            "timestamp": 2.2034181863744142
-        },
-        {
-            "x": 7.191835709933807,
-            "y": 2.1285351921147058,
-            "heading": 2.3080325133818067,
-            "angularVelocity": -2.8028465242925935,
-            "velocityX": 2.1925746280203615,
-            "velocityY": 1.8379567688575535,
-            "timestamp": 2.2406957749912793
-        },
-        {
-            "x": 7.274315415796274,
-            "y": 2.1867028423762775,
-            "heading": 2.1850264648931392,
-            "angularVelocity": -3.2997319046816447,
-            "velocityX": 2.2125815784434106,
-            "velocityY": 1.5603919786607532,
-            "timestamp": 2.2779733636081443
-        },
-        {
-            "x": 7.351412179448119,
-            "y": 2.2360068924730196,
-            "heading": 2.043511695780604,
-            "angularVelocity": -3.796242578000636,
-            "velocityX": 2.0681800114335913,
-            "velocityY": 1.322619083639869,
-            "timestamp": 2.3152509522250093
-        },
-        {
-            "x": 7.420172175937219,
-            "y": 2.2789013315216375,
-            "heading": 1.8838780266002726,
-            "angularVelocity": -4.282296014933475,
-            "velocityX": 1.8445398170951257,
-            "velocityY": 1.1506763350355667,
-            "timestamp": 2.3525285408418743
-        },
-        {
-            "x": 7.479537718839642,
-            "y": 2.3170603162912466,
-            "heading": 1.7061327210648876,
-            "angularVelocity": -4.768154597182533,
-            "velocityX": 1.592526370537947,
-            "velocityY": 1.0236441300375574,
-            "timestamp": 2.3898061294587394
-        },
-        {
-            "x": 7.529281054923766,
-            "y": 2.3515994880332096,
-            "heading": 1.5086439015102746,
-            "angularVelocity": -5.2977895535137876,
-            "velocityX": 1.3344032682848619,
-            "velocityY": 0.9265398601007353,
-            "timestamp": 2.4270837180756044
-        },
-        {
-            "x": 7.569719945861703,
-            "y": 2.383438536522646,
-            "heading": 1.288404786035775,
-            "angularVelocity": -5.908083748068905,
-            "velocityX": 1.0848043674059311,
-            "velocityY": 0.8541069760888933,
-            "timestamp": 2.4643613066924694
-        },
-        {
-            "x": 7.603202598509127,
-            "y": 2.4091886515491456,
-            "heading": 1.0408456353949866,
-            "angularVelocity": -6.640964714353526,
-            "velocityX": 0.8981979223912477,
-            "velocityY": 0.6907666504707783,
-            "timestamp": 2.5016388953093345
-        },
-        {
-            "x": 7.630132008026638,
-            "y": 2.427100499568469,
-            "heading": 0.7720308301798764,
-            "angularVelocity": -7.211164004677401,
-            "velocityX": 0.7224021326670985,
-            "velocityY": 0.48049910640464033,
-            "timestamp": 2.5389164839261995
-        },
-        {
-            "x": 7.649623319500903,
-            "y": 2.438227448776781,
-            "heading": 0.5254269726030665,
-            "angularVelocity": -6.615338242807932,
-            "velocityX": 0.5228694289911082,
-            "velocityY": 0.29848897477445496,
-            "timestamp": 2.5761940725430645
-        },
-        {
-            "x": 7.66290885295536,
-            "y": 2.4458359453096237,
-            "heading": 0.3183730749926382,
-            "angularVelocity": -5.554380132752273,
-            "velocityX": 0.3563946582222695,
-            "velocityY": 0.2041037742822349,
-            "timestamp": 2.6134716611599296
-        },
-        {
-            "x": 7.671732744074589,
-            "y": 2.451418069701132,
-            "heading": 0.15988099310288126,
-            "angularVelocity": -4.251672057404801,
-            "velocityX": 0.23670766931625417,
-            "velocityY": 0.14974478228408286,
-            "timestamp": 2.6507492497767946
-        },
-        {
-            "x": 7.677275184086734,
-            "y": 2.4551558590330185,
-            "heading": 0.05341064749493927,
-            "angularVelocity": -2.8561489505727446,
-            "velocityX": 0.14868021827027583,
-            "velocityY": 0.10026907508163654,
-            "timestamp": 2.6880268383936596
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 1.0247603884557974e-38,
-            "angularVelocity": -1.4327817189005392,
-            "velocityX": 0.07309528363731636,
-            "velocityY": 0.04947050051802475,
-            "timestamp": 2.7253044270105247
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -2.6319896813971663e-37,
-            "velocityX": 5.227147746061896e-36,
-            "velocityY": -6.467829854732196e-35,
-            "timestamp": 2.7625820156273897
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.1.traj
deleted file mode 100644
index 8826d851..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.1.traj	
+++ /dev/null
@@ -1,571 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -4.609910300520975e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.298808348352727,
-            "y": 4.35514296819253,
-            "heading": -0.3837399750205753,
-            "angularVelocity": 0.24884159467028183,
-            "velocityX": -0.047369219319970686,
-            "velocityY": -0.19307136062676852,
-            "timestamp": 0.02515666638336474
-        },
-        {
-            "x": 2.2965596540188837,
-            "y": 4.3453750993410925,
-            "heading": -0.37149402212882315,
-            "angularVelocity": 0.48678758564965857,
-            "velocityX": -0.08938761199815681,
-            "velocityY": -0.38828152755153583,
-            "timestamp": 0.05031333276672948
-        },
-        {
-            "x": 2.2934096466067952,
-            "y": 4.330641358670927,
-            "heading": -0.3535645425221108,
-            "angularVelocity": 0.7127128584321681,
-            "velocityX": -0.1252156133919033,
-            "velocityY": -0.5856793760205414,
-            "timestamp": 0.07546999915009422
-        },
-        {
-            "x": 2.289538880665139,
-            "y": 4.310886769515238,
-            "heading": -0.33028900557963653,
-            "angularVelocity": 0.9252234214095049,
-            "velocityX": -0.15386640990779057,
-            "velocityY": -0.7852625961900879,
-            "timestamp": 0.10062666553345896
-        },
-        {
-            "x": 2.2851573432175654,
-            "y": 4.286058471237428,
-            "heading": -0.3020481691352866,
-            "angularVelocity": 1.1225985197714665,
-            "velocityX": -0.17417003432819553,
-            "velocityY": -0.9869470739663532,
-            "timestamp": 0.1257833319168237
-        },
-        {
-            "x": 2.280510060472114,
-            "y": 4.25610900707676,
-            "heading": -0.26927550139535,
-            "angularVelocity": 1.3027428690475438,
-            "velocityX": -0.18473364771909595,
-            "velocityY": -1.1905180004483138,
-            "timestamp": 0.15093999830018845
-        },
-        {
-            "x": 2.2758836751536,
-            "y": 4.2210014881511375,
-            "heading": -0.23246752663964654,
-            "angularVelocity": 1.463149933889621,
-            "velocityX": -0.1839029562984313,
-            "velocityY": -1.3955552930033008,
-            "timestamp": 0.1760966646835532
-        },
-        {
-            "x": 2.271613693612169,
-            "y": 4.180717494993525,
-            "heading": -0.19219487185489256,
-            "angularVelocity": 1.6008740653882818,
-            "velocityX": -0.16973558723403512,
-            "velocityY": -1.6013247758554696,
-            "timestamp": 0.20125333106691795
-        },
-        {
-            "x": 2.268091592763015,
-            "y": 4.135268758759812,
-            "heading": -0.1491141249151469,
-            "angularVelocity": 1.7124982413501877,
-            "velocityX": -0.1400066604803655,
-            "velocityY": -1.8066279347634933,
-            "timestamp": 0.2264099974502827
-        },
-        {
-            "x": 2.2657701315686967,
-            "y": 4.084713672798125,
-            "heading": -0.10398137252966654,
-            "angularVelocity": 1.7940672940403972,
-            "velocityX": -0.09228015981695388,
-            "velocityY": -2.0096099058307106,
-            "timestamp": 0.25156666383364745
-        },
-        {
-            "x": 2.265163909208295,
-            "y": 4.029179243747037,
-            "heading": -0.057668550112221675,
-            "angularVelocity": 1.8409761337881332,
-            "velocityX": -0.02409788129965802,
-            "velocityY": -2.2075432493635576,
-            "timestamp": 0.2767233302170122
-        },
-        {
-            "x": 2.266840363517046,
-            "y": 3.9688874958283327,
-            "heading": -0.011179237419756825,
-            "angularVelocity": 1.8479917801512262,
-            "velocityX": 0.06664055893588815,
-            "velocityY": -2.3966509314037507,
-            "timestamp": 0.3018799966003769
-        },
-        {
-            "x": 2.2713096946462064,
-            "y": 3.9041248350083095,
-            "heading": 0.03446639703260334,
-            "angularVelocity": 1.8144548151476838,
-            "velocityX": 0.1776599117328043,
-            "velocityY": -2.574373719995273,
-            "timestamp": 0.3270366629837416
-        },
-        {
-            "x": 2.2787160490905882,
-            "y": 3.8349972418338227,
-            "heading": 0.07865637806724814,
-            "angularVelocity": 1.7565912892125506,
-            "velocityX": 0.29440921668697884,
-            "velocityY": -2.74788368701343,
-            "timestamp": 0.35219332936710634
-        },
-        {
-            "x": 2.289237241442295,
-            "y": 3.7616571782461574,
-            "heading": 0.12059430680114623,
-            "angularVelocity": 1.667070195025133,
-            "velocityX": 0.41822681079334173,
-            "velocityY": -2.915333155436007,
-            "timestamp": 0.37734999575047107
-        },
-        {
-            "x": 2.303092280593543,
-            "y": 3.684333317171128,
-            "heading": 0.15922184993862323,
-            "angularVelocity": 1.5354794052927534,
-            "velocityX": 0.550750204343837,
-            "velocityY": -3.073692670431113,
-            "timestamp": 0.4025066621338358
-        },
-        {
-            "x": 2.3205444941015814,
-            "y": 3.6033821039148743,
-            "heading": 0.19314125064776663,
-            "angularVelocity": 1.3483265307192482,
-            "velocityX": 0.6937411039317692,
-            "velocityY": -3.217883165544707,
-            "timestamp": 0.4276633285172005
-        },
-        {
-            "x": 2.341904587688066,
-            "y": 3.5193842561187316,
-            "heading": 0.22066827104785883,
-            "angularVelocity": 1.0942236932591152,
-            "velocityX": 0.8490828339882706,
-            "velocityY": -3.3389896147642064,
-            "timestamp": 0.45281999490056524
-        },
-        {
-            "x": 2.3675851289681646,
-            "y": 3.4337423994061407,
-            "heading": 0.23918679984335672,
-            "angularVelocity": 0.7361280907928144,
-            "velocityX": 1.0208244959308244,
-            "velocityY": -3.40434044032252,
-            "timestamp": 0.47797666128392996
-        },
-        {
-            "x": 2.3978885412195137,
-            "y": 3.3469640370766824,
-            "heading": 0.24945072521405584,
-            "angularVelocity": 0.40800021808479553,
-            "velocityX": 1.2045877537807594,
-            "velocityY": -3.4495175555868487,
-            "timestamp": 0.5031333276672947
-        },
-        {
-            "x": 2.4331457577160354,
-            "y": 3.2598611919071945,
-            "heading": 0.25313177220508776,
-            "angularVelocity": 0.1463249118518414,
-            "velocityX": 1.4015059054023244,
-            "velocityY": -3.462416038839161,
-            "timestamp": 0.5282899940506595
-        },
-        {
-            "x": 2.473498104501041,
-            "y": 3.173715186571369,
-            "heading": 0.2532432496800442,
-            "angularVelocity": 0.004431329384332257,
-            "velocityX": 1.604041893710102,
-            "velocityY": -3.424380799229854,
-            "timestamp": 0.5534466604340242
-        },
-        {
-            "x": 2.5185817721335604,
-            "y": 3.089905547060991,
-            "heading": 0.25324321969105484,
-            "angularVelocity": -0.0000011920891614956396,
-            "velocityX": 1.7921161311870697,
-            "velocityY": -3.3315081669882356,
-            "timestamp": 0.578603326817389
-        },
-        {
-            "x": 2.568241478882672,
-            "y": 3.0087236123099395,
-            "heading": 0.2532431890633549,
-            "angularVelocity": -0.0000012174784783024135,
-            "velocityX": 1.97401778090718,
-            "velocityY": -3.2270545514223747,
-            "timestamp": 0.6037599932007538
-        },
-        {
-            "x": 2.6223245423144714,
-            "y": 2.9304189809174512,
-            "heading": 0.25324315880142845,
-            "angularVelocity": -0.0000012029386552177117,
-            "velocityX": 2.1498501672527754,
-            "velocityY": -3.1126791681853656,
-            "timestamp": 0.6289166595841186
-        },
-        {
-            "x": 2.6806646810204167,
-            "y": 2.855232404738762,
-            "heading": 0.25324312847978453,
-            "angularVelocity": -0.0000012053124772832063,
-            "velocityX": 2.319072718813166,
-            "velocityY": -2.9887336832676343,
-            "timestamp": 0.6540733259674834
-        },
-        {
-            "x": 2.7430825233459473,
-            "y": 2.7833950519561768,
-            "heading": 0.2532430937019675,
-            "angularVelocity": -0.0000013824493495341482,
-            "velocityX": 2.4811650865953045,
-            "velocityY": -2.8555990562441647,
-            "timestamp": 0.6792299923508481
-        },
-        {
-            "x": 2.8747857816965947,
-            "y": 2.6605842580771375,
-            "heading": 0.2532430684120768,
-            "angularVelocity": -5.312693002073383e-7,
-            "velocityX": 2.7667141273306664,
-            "velocityY": -2.579908520631989,
-            "timestamp": 0.7268327631068262
-        },
-        {
-            "x": 3.0186320127358077,
-            "y": 2.5522491183421203,
-            "heading": 0.2532430469824583,
-            "angularVelocity": -4.5017586399077444e-7,
-            "velocityX": 3.021803747025554,
-            "velocityY": -2.2758158404343,
-            "timestamp": 0.7744355338628043
-        },
-        {
-            "x": 3.1730376030001257,
-            "y": 2.4595822138278454,
-            "heading": 0.2532430277420137,
-            "angularVelocity": -4.041874945252969e-7,
-            "velocityX": 3.2436261127705017,
-            "velocityY": -1.946670394236201,
-            "timestamp": 0.8220383046187824
-        },
-        {
-            "x": 3.336302611834685,
-            "y": 2.38360340882859,
-            "heading": 0.25324300957525164,
-            "angularVelocity": -3.816324501860361e-7,
-            "velocityX": 3.4297375182526793,
-            "velocityY": -1.5961004746706646,
-            "timestamp": 0.8696410753747604
-        },
-        {
-            "x": 3.505316504811254,
-            "y": 2.3214540241877932,
-            "heading": 0.25324299166355524,
-            "angularVelocity": -3.7627424124720095e-7,
-            "velocityX": 3.550505365390803,
-            "velocityY": -1.3055833442844713,
-            "timestamp": 0.9172438461307385
-        },
-        {
-            "x": 3.6743308817857088,
-            "y": 2.2593059557844626,
-            "heading": 0.25324297375187266,
-            "angularVelocity": -3.76273948990246e-7,
-            "velocityX": 3.5505155328218008,
-            "velocityY": -1.3055556938463668,
-            "timestamp": 0.9648466168867166
-        },
-        {
-            "x": 3.843345266545321,
-            "y": 2.1971579085532174,
-            "heading": 0.2532429558401917,
-            "angularVelocity": -3.762739157561073e-7,
-            "velocityX": 3.5505156963660025,
-            "velocityY": -1.3055552490805507,
-            "timestamp": 1.0124493876426945
-        },
-        {
-            "x": 4.012359658311276,
-            "y": 2.135009880376043,
-            "heading": 0.25324293792850894,
-            "angularVelocity": -3.7627395316214525e-7,
-            "velocityX": 3.5505158435494915,
-            "velocityY": -1.3055548488082471,
-            "timestamp": 1.0600521583986726
-        },
-        {
-            "x": 4.181374056391421,
-            "y": 2.07286186937061,
-            "heading": 0.25324292001682175,
-            "angularVelocity": -3.7627404706616014e-7,
-            "velocityX": 3.5505159761928384,
-            "velocityY": -1.30555448807838,
-            "timestamp": 1.1076549291546507
-        },
-        {
-            "x": 4.3503884601627245,
-            "y": 2.010713873842556,
-            "heading": 0.25324290210512795,
-            "angularVelocity": -3.7627418619029544e-7,
-            "velocityX": 3.5505160957480255,
-            "velocityY": -1.30555416294226,
-            "timestamp": 1.1552576999106288
-        },
-        {
-            "x": 4.51940286907005,
-            "y": 1.9485658922821711,
-            "heading": 0.2532428841934256,
-            "angularVelocity": -3.7627436389492574e-7,
-            "velocityX": 3.5505162036413855,
-            "velocityY": -1.3055538695209365,
-            "timestamp": 1.2028604706666068
-        },
-        {
-            "x": 4.688417282561973,
-            "y": 1.8864179231898264,
-            "heading": 0.25324286628171494,
-            "angularVelocity": -3.7627454024029507e-7,
-            "velocityX": 3.550516299950831,
-            "velocityY": -1.3055536076025602,
-            "timestamp": 1.250463241422585
-        },
-        {
-            "x": 4.857431700124039,
-            "y": 1.824269965166445,
-            "heading": 0.2532428483699966,
-            "angularVelocity": -3.7627470129766985e-7,
-            "velocityX": 3.5505163854530717,
-            "velocityY": -1.3055533750748494,
-            "timestamp": 1.298066012178563
-        },
-        {
-            "x": 5.02644612130461,
-            "y": 1.7621220169837726,
-            "heading": 0.2532428304582713,
-            "angularVelocity": -3.76274846815446e-7,
-            "velocityX": 3.5505164614676405,
-            "velocityY": -1.3055531683492934,
-            "timestamp": 1.345668782934541
-        },
-        {
-            "x": 5.1954605457099055,
-            "y": 1.6999740775709098,
-            "heading": 0.25324281254653974,
-            "angularVelocity": -3.7627497879654404e-7,
-            "velocityX": 3.5505165292100247,
-            "velocityY": -1.3055529841203253,
-            "timestamp": 1.3932715536905191
-        },
-        {
-            "x": 5.364474972997734,
-            "y": 1.6378261459972472,
-            "heading": 0.25324279463480254,
-            "angularVelocity": -3.7627509767034604e-7,
-            "velocityX": 3.550516589763906,
-            "velocityY": -1.3055528194408317,
-            "timestamp": 1.4408743244464972
-        },
-        {
-            "x": 5.533489402870974,
-            "y": 1.5756782214547507,
-            "heading": 0.2532427767230601,
-            "angularVelocity": -3.762752068122207e-7,
-            "velocityX": 3.5505166440761182,
-            "velocityY": -1.3055526717358554,
-            "timestamp": 1.4884770952024753
-        },
-        {
-            "x": 5.702503835071411,
-            "y": 1.513530303241192,
-            "heading": 0.25324275881131303,
-            "angularVelocity": -3.7627530496428226e-7,
-            "velocityX": 3.550516692963962,
-            "velocityY": -1.3055525387827163,
-            "timestamp": 1.5360798659584534
-        },
-        {
-            "x": 5.871518269374186,
-            "y": 1.4513823907450571,
-            "heading": 0.2532427408995616,
-            "angularVelocity": -3.762753964251323e-7,
-            "velocityX": 3.5505167371281625,
-            "velocityY": -1.3055524186757583,
-            "timestamp": 1.5836826367144314
-        },
-        {
-            "x": 6.040532705582966,
-            "y": 1.3892344834324082,
-            "heading": 0.25324272298780626,
-            "angularVelocity": -3.7627547903960503e-7,
-            "velocityX": 3.550516777167954,
-            "velocityY": -1.3055523097853368,
-            "timestamp": 1.6312854074704095
-        },
-        {
-            "x": 6.20954714352589,
-            "y": 1.3270865808358587,
-            "heading": 0.2532427050760471,
-            "angularVelocity": -3.7627555795965075e-7,
-            "velocityX": 3.5505168135974197,
-            "velocityY": -1.3055522107133783,
-            "timestamp": 1.6788881782263876
-        },
-        {
-            "x": 6.378561583051887,
-            "y": 1.2649386825445668,
-            "heading": 0.25324268716428455,
-            "angularVelocity": -3.7627562961930894e-7,
-            "velocityX": 3.5505168468533315,
-            "velocityY": -1.3055521202720615,
-            "timestamp": 1.7264909489823657
-        },
-        {
-            "x": 6.547576024027691,
-            "y": 1.2027907881961044,
-            "heading": 0.25324266925251887,
-            "angularVelocity": -3.7627569544162935e-7,
-            "velocityX": 3.5505168773096716,
-            "velocityY": -1.3055520374443248,
-            "timestamp": 1.7740937197383437
-        },
-        {
-            "x": 6.716590466335514,
-            "y": 1.1406428974701475,
-            "heading": 0.25324265134075025,
-            "angularVelocity": -3.762757576316506e-7,
-            "velocityX": 3.550516905291665,
-            "velocityY": -1.3055519613457025,
-            "timestamp": 1.8216964904943218
-        },
-        {
-            "x": 6.88560491686231,
-            "y": 1.0784950290966298,
-            "heading": 0.253242633428611,
-            "angularVelocity": -3.7628354454135297e-7,
-            "velocityX": 3.550517077949107,
-            "velocityY": -1.3055514917839766,
-            "timestamp": 1.8692992612502999
-        },
-        {
-            "x": 7.0444139114910005,
-            "y": 1.0203058482073266,
-            "heading": 0.2259027129119336,
-            "angularVelocity": -0.574334646544581,
-            "velocityX": 3.336129223292024,
-            "velocityY": -1.2223906290579827,
-            "timestamp": 1.916902032006278
-        },
-        {
-            "x": 7.185810534595712,
-            "y": 0.9684234171781619,
-            "heading": 0.18518533646408028,
-            "angularVelocity": -0.8553572786042075,
-            "velocityX": 2.9703443908662663,
-            "velocityY": -1.089903596055891,
-            "timestamp": 1.964504802762256
-        },
-        {
-            "x": 7.3094655243228,
-            "y": 0.9230369546143523,
-            "heading": 0.14380134057962407,
-            "angularVelocity": -0.8693610734677483,
-            "velocityX": 2.59764269523238,
-            "velocityY": -0.953441613650398,
-            "timestamp": 2.012107573518234
-        },
-        {
-            "x": 7.415398593682274,
-            "y": 0.8841490900710189,
-            "heading": 0.10540313701630608,
-            "angularVelocity": -0.8066379950897271,
-            "velocityX": 2.2253551143589885,
-            "velocityY": -0.8169243917057022,
-            "timestamp": 2.059710344274212
-        },
-        {
-            "x": 7.503637039429732,
-            "y": 0.8517540010892736,
-            "heading": 0.07171800478429124,
-            "angularVelocity": -0.70762965468317,
-            "velocityX": 1.8536409613588987,
-            "velocityY": -0.680529483206126,
-            "timestamp": 2.1073131150301903
-        },
-        {
-            "x": 7.574202084965293,
-            "y": 0.8258459262172375,
-            "heading": 0.04375776286271558,
-            "angularVelocity": -0.5873658502969467,
-            "velocityX": 1.482372652157004,
-            "velocityY": -0.5442556065663985,
-            "timestamp": 2.1549158857861683
-        },
-        {
-            "x": 7.627109788299601,
-            "y": 0.8064200554899893,
-            "heading": 0.022189516524924582,
-            "angularVelocity": -0.4530880449870122,
-            "velocityX": 1.1114416764840027,
-            "velocityY": -0.40808277372822227,
-            "timestamp": 2.2025186565421464
-        },
-        {
-            "x": 7.66237251850946,
-            "y": 0.7934724766877013,
-            "heading": 0.0074863600733853135,
-            "angularVelocity": -0.3088718622474896,
-            "velocityX": 0.7407705402406998,
-            "velocityY": -0.27199212559831976,
-            "timestamp": 2.2501214272981245
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -1.0670900456397286e-32,
-            "angularVelocity": -0.1572673177316084,
-            "velocityX": 0.37030368633165434,
-            "velocityY": -0.13596848639085585,
-            "timestamp": 2.2977241980541026
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 2.230662363627099e-31,
-            "velocityX": 2.3924515989499496e-33,
-            "velocityY": 1.2585115119762557e-33,
-            "timestamp": 2.3453269688100806
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.1.traj
deleted file mode 100644
index 714e9458..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.1.traj	
+++ /dev/null
@@ -1,175 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 5.020065556819226e-31,
-            "velocityX": -2.911156118414847e-31,
-            "velocityY": 8.635491730351355e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3221690164859408,
-            "y": 4.411229757983623,
-            "heading": -0.4048176043167742,
-            "angularVelocity": -0.1808500047435309,
-            "velocityX": 0.27057455786553103,
-            "velocityY": 0.6252631516043967,
-            "timestamp": 0.08193311544453755
-        },
-        {
-            "x": 2.3687303275118676,
-            "y": 4.5127321003230465,
-            "heading": -0.4313760168590284,
-            "angularVelocity": -0.32414747563532126,
-            "velocityX": 0.5682843960383053,
-            "velocityY": 1.2388439251785637,
-            "timestamp": 0.1638662308890751
-        },
-        {
-            "x": 2.4431953998066236,
-            "y": 4.662720767025029,
-            "heading": -0.4646753197408115,
-            "angularVelocity": -0.40642056268856935,
-            "velocityX": 0.9088519567543129,
-            "velocityY": 1.830623257619443,
-            "timestamp": 0.24579934633361264
-        },
-        {
-            "x": 2.551740018878988,
-            "y": 4.857106976368847,
-            "heading": -0.4953245435557511,
-            "angularVelocity": -0.3740761430663416,
-            "velocityX": 1.3247954564335083,
-            "velocityY": 2.3724986934666186,
-            "timestamp": 0.3277324617781502
-        },
-        {
-            "x": 2.701293241756868,
-            "y": 5.089437731072216,
-            "heading": -0.5138028115710582,
-            "angularVelocity": -0.2255286878210994,
-            "velocityX": 1.8253086321258618,
-            "velocityY": 2.835614799252222,
-            "timestamp": 0.4096655772226877
-        },
-        {
-            "x": 2.9065804913658706,
-            "y": 5.321655101693131,
-            "heading": -0.5138024716246538,
-            "angularVelocity": 0.000004149072112901325,
-            "velocityX": 2.5055467315650244,
-            "velocityY": 2.8342309377715424,
-            "timestamp": 0.4915986926672252
-        },
-        {
-            "x": 3.1502852438919966,
-            "y": 5.513164387705616,
-            "heading": -0.5138023715427595,
-            "angularVelocity": 0.0000012215072474193347,
-            "velocityX": 2.974435320857471,
-            "velocityY": 2.3373856220824885,
-            "timestamp": 0.5735318081117627
-        },
-        {
-            "x": 3.4075351073224858,
-            "y": 5.686052564856294,
-            "heading": -0.5138022777253332,
-            "angularVelocity": 0.0000011450489327031063,
-            "velocityX": 3.1397544452539083,
-            "velocityY": 2.110113550701131,
-            "timestamp": 0.6554649235563003
-        },
-        {
-            "x": 3.664785106053728,
-            "y": 5.858940540685368,
-            "heading": -0.5138021839079745,
-            "angularVelocity": 0.0000011450481052658978,
-            "velocityX": 3.13975609661006,
-            "velocityY": 2.110111093555414,
-            "timestamp": 0.7373980390008378
-        },
-        {
-            "x": 3.922035141067721,
-            "y": 6.031828462527248,
-            "heading": -0.5138020900906538,
-            "angularVelocity": 0.000001145047641454271,
-            "velocityX": 3.1397565394438236,
-            "velocityY": 2.1101104346375243,
-            "timestamp": 0.8193311544453753
-        },
-        {
-            "x": 4.179285313417939,
-            "y": 6.204716180018366,
-            "heading": -0.5138019962733545,
-            "angularVelocity": 0.00000114504738127865,
-            "velocityX": 3.139758215643051,
-            "velocityY": 2.110107940520708,
-            "timestamp": 0.9012642698899128
-        },
-        {
-            "x": 4.443584109358565,
-            "y": 6.354084975447565,
-            "heading": -0.4978326665645833,
-            "angularVelocity": 0.19490690207652947,
-            "velocityX": 3.225787210783358,
-            "velocityY": 1.8230576808752068,
-            "timestamp": 0.9831973853344503
-        },
-        {
-            "x": 4.674709010100242,
-            "y": 6.4693171293955105,
-            "heading": -0.3712044647358083,
-            "angularVelocity": 1.5455069801964878,
-            "velocityX": 2.8208972585465766,
-            "velocityY": 1.406417335929929,
-            "timestamp": 1.065130500778988
-        },
-        {
-            "x": 4.870816853917273,
-            "y": 6.548974021375514,
-            "heading": -0.15522232897542676,
-            "angularVelocity": 2.636078642787472,
-            "velocityX": 2.3935113751384223,
-            "velocityY": 0.9722185168697003,
-            "timestamp": 1.1470636162235255
-        },
-        {
-            "x": 5.022935942406859,
-            "y": 6.594453289671109,
-            "heading": 0.041233782182786366,
-            "angularVelocity": 2.3977620049271122,
-            "velocityX": 1.856625220025449,
-            "velocityY": 0.5550779809707214,
-            "timestamp": 1.228996731668063
-        },
-        {
-            "x": 5.124959992690689,
-            "y": 6.619307485763033,
-            "heading": 0.18624088374392397,
-            "angularVelocity": 1.769822870452196,
-            "velocityX": 1.2452114109208137,
-            "velocityY": 0.3033473822772884,
-            "timestamp": 1.3109298471126005
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": 0.9368509404239479,
-            "velocityX": 0.6229472299739557,
-            "velocityY": 0.13050296182384175,
-            "timestamp": 1.392862962557138
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": 1.5668772442975024e-30,
-            "velocityX": 0,
-            "velocityY": -2.8584411322391187e-31,
-            "timestamp": 1.4747960780016756
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.1.traj
deleted file mode 100644
index 72f3f88a..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3223907681513127,
-            "y": 4.418261357040706,
-            "heading": -0.429219543622513,
-            "angularVelocity": -0.4499475285553182,
-            "velocityX": 0.2568788379871729,
-            "velocityY": 0.6684053711347925,
-            "timestamp": 0.08716470506781053
-        },
-        {
-            "x": 2.374909672838796,
-            "y": 4.531319644218887,
-            "heading": -0.5050759499486299,
-            "angularVelocity": -0.8702651637162607,
-            "velocityX": 0.6025248940683728,
-            "velocityY": 1.297064988520603,
-            "timestamp": 0.17432941013562106
-        },
-        {
-            "x": 2.469260718438912,
-            "y": 4.691180971030917,
-            "heading": -0.6116327562753044,
-            "angularVelocity": -1.2224765315706085,
-            "velocityX": 1.082445532589298,
-            "velocityY": 1.834014429208093,
-            "timestamp": 0.2614941152034316
-        },
-        {
-            "x": 2.620331804373909,
-            "y": 4.8788277526908095,
-            "heading": -0.7357228691689549,
-            "angularVelocity": -1.4236279787456398,
-            "velocityX": 1.7331680961631113,
-            "velocityY": 2.1527839911109723,
-            "timestamp": 0.3486588202712421
-        },
-        {
-            "x": 2.832381156144981,
-            "y": 5.083303561006889,
-            "heading": -0.84759450212177,
-            "angularVelocity": -1.2834510581523242,
-            "velocityX": 2.432743294503276,
-            "velocityY": 2.3458555634072913,
-            "timestamp": 0.43582352533905266
-        },
-        {
-            "x": 3.098611796220168,
-            "y": 5.267387874253087,
-            "heading": -0.8666590247194526,
-            "angularVelocity": -0.21871837440225492,
-            "velocityX": 3.0543399403241303,
-            "velocityY": 2.111913452847551,
-            "timestamp": 0.5229882304068632
-        },
-        {
-            "x": 3.3974595011437105,
-            "y": 5.397776588765608,
-            "heading": -0.8564937886773937,
-            "angularVelocity": 0.11662101115527058,
-            "velocityX": 3.42854030987716,
-            "velocityY": 1.4958888969002289,
-            "timestamp": 0.6101529354746736
-        },
-        {
-            "x": 3.678181484629676,
-            "y": 5.472509187071852,
-            "heading": -0.7581230365630254,
-            "angularVelocity": 1.1285617502845884,
-            "velocityX": 3.220592363245829,
-            "velocityY": 0.8573722385466142,
-            "timestamp": 0.6973176405424841
-        },
-        {
-            "x": 3.915033154173224,
-            "y": 5.514676302538828,
-            "heading": -0.5395209628894968,
-            "angularVelocity": 2.5079196161274817,
-            "velocityX": 2.7172887163363586,
-            "velocityY": 0.4837636453214852,
-            "timestamp": 0.7844823456102946
-        },
-        {
-            "x": 4.091189385673594,
-            "y": 5.53976294658506,
-            "heading": -0.29904398170320484,
-            "angularVelocity": 2.7588802256510867,
-            "velocityX": 2.0209582693284847,
-            "velocityY": 0.2878073645372324,
-            "timestamp": 0.8716470506781051
-        },
-        {
-            "x": 4.2064124532392215,
-            "y": 5.553849604220975,
-            "heading": -0.10702579424844161,
-            "angularVelocity": 2.2029350905894844,
-            "velocityX": 1.3219005040628353,
-            "velocityY": 0.161609651807531,
-            "timestamp": 0.9588117557459156
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 2.9287791201697906e-29,
-            "angularVelocity": 1.2278570112200775,
-            "velocityX": 0.6492025266047246,
-            "velocityY": 0.0705606216901654,
-            "timestamp": 1.045976460813726
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 3.0874616220611523e-31,
-            "velocityY": -3.4071713040637054e-30,
-            "timestamp": 1.1331411658815367
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.1.traj
deleted file mode 100644
index 4ff6b6c8..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.1.traj	
+++ /dev/null
@@ -1,229 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.284359298751307,
-            "y": 4.3328670624584875,
-            "heading": -0.3913603360985968,
-            "angularVelocity": -0.02219557362709146,
-            "velocityX": -0.2551974739939649,
-            "velocityY": -0.44270758788444614,
-            "timestamp": 0.061288620940906494
-        },
-        {
-            "x": 2.2541133789559487,
-            "y": 4.2780305351291235,
-            "heading": -0.3941996942145126,
-            "angularVelocity": -0.04632765548197678,
-            "velocityX": -0.4934997611468709,
-            "velocityY": -0.89472607618691,
-            "timestamp": 0.12257724188181299
-        },
-        {
-            "x": 2.2108693669713233,
-            "y": 4.194701771021148,
-            "heading": -0.39869373638946143,
-            "angularVelocity": -0.0733258818024626,
-            "velocityX": -0.7055797849705345,
-            "velocityY": -1.3596123200148318,
-            "timestamp": 0.1838658628227195
-        },
-        {
-            "x": 2.157413012714945,
-            "y": 4.081769184110331,
-            "heading": -0.40512443208979565,
-            "angularVelocity": -0.1049247903054391,
-            "velocityX": -0.8722068376757797,
-            "velocityY": -1.8426354709417345,
-            "timestamp": 0.24515448376362597
-        },
-        {
-            "x": 2.099439422754663,
-            "y": 3.9378510886177804,
-            "heading": -0.41398926531008917,
-            "angularVelocity": -0.1446407682894496,
-            "velocityX": -0.9459111507204496,
-            "velocityY": -2.348202542056109,
-            "timestamp": 0.30644310470453245
-        },
-        {
-            "x": 2.05084134036617,
-            "y": 3.764062564560248,
-            "heading": -0.42610260644735526,
-            "angularVelocity": -0.1976442111325286,
-            "velocityX": -0.7929380958881483,
-            "velocityY": -2.835575697242347,
-            "timestamp": 0.36773172564543893
-        },
-        {
-            "x": 2.0313297442556637,
-            "y": 3.578686605657539,
-            "heading": -0.44115755081157715,
-            "angularVelocity": -0.24564012263773397,
-            "velocityX": -0.3183559331400068,
-            "velocityY": -3.024639093795978,
-            "timestamp": 0.4290203465863454
-        },
-        {
-            "x": 2.0424292488902682,
-            "y": 3.3999266361315956,
-            "heading": -0.45764648760363913,
-            "angularVelocity": -0.2690374907923018,
-            "velocityX": 0.18110220892890863,
-            "velocityY": -2.9166910069375023,
-            "timestamp": 0.4903089675272519
-        },
-        {
-            "x": 2.0816987250314285,
-            "y": 3.234855796713624,
-            "heading": -0.4747853916237668,
-            "angularVelocity": -0.27964251368378285,
-            "velocityX": 0.6407302944379066,
-            "velocityY": -2.6933358408754287,
-            "timestamp": 0.5515975884681584
-        },
-        {
-            "x": 2.1473776050382467,
-            "y": 3.086622748034336,
-            "heading": -0.49218081575104855,
-            "angularVelocity": -0.2838279579508591,
-            "velocityX": 1.071632531431,
-            "velocityY": -2.418606364502998,
-            "timestamp": 0.6128862094090649
-        },
-        {
-            "x": 2.2382969856262207,
-            "y": 2.9569339752197266,
-            "heading": -0.5096053711064815,
-            "angularVelocity": -0.2843032701328595,
-            "velocityX": 1.4834626590087072,
-            "velocityY": -2.116033463041253,
-            "timestamp": 0.6741748303499714
-        },
-        {
-            "x": 2.3750291535583634,
-            "y": 2.833789120602945,
-            "heading": -0.5294729435704262,
-            "angularVelocity": -0.282105779182567,
-            "velocityX": 1.9415021560293924,
-            "velocityY": -1.7485717103605045,
-            "timestamp": 0.7446007979480589
-        },
-        {
-            "x": 2.5431654959507464,
-            "y": 2.737552434720476,
-            "heading": -0.5488979650801319,
-            "angularVelocity": -0.2758218619098283,
-            "velocityX": 2.3874196993915153,
-            "velocityY": -1.366494336743516,
-            "timestamp": 0.8150267655461465
-        },
-        {
-            "x": 2.7413669689925393,
-            "y": 2.669711436345154,
-            "heading": -0.5674506234763109,
-            "angularVelocity": -0.2634349094364851,
-            "velocityX": 2.8143237473555907,
-            "velocityY": -0.9632952260234809,
-            "timestamp": 0.8854527331442341
-        },
-        {
-            "x": 2.9672720655352354,
-            "y": 2.6325722400915463,
-            "heading": -0.5844267226661307,
-            "angularVelocity": -0.24104885979984425,
-            "velocityX": 3.207696028145596,
-            "velocityY": -0.5273508837756647,
-            "timestamp": 0.9558787007423217
-        },
-        {
-            "x": 3.21586763898004,
-            "y": 2.629998478445332,
-            "heading": -0.5985057428976699,
-            "angularVelocity": -0.19991234358165255,
-            "velocityX": 3.529885096694866,
-            "velocityY": -0.03654563414595668,
-            "timestamp": 1.0263046683404093
-        },
-        {
-            "x": 3.4730987079905056,
-            "y": 2.6678365706373897,
-            "heading": -0.6068355688677516,
-            "angularVelocity": -0.11827776392962061,
-            "velocityX": 3.652503157336117,
-            "velocityY": 0.5372747224148254,
-            "timestamp": 1.096730635938497
-        },
-        {
-            "x": 3.704150591962856,
-            "y": 2.737591563426048,
-            "heading": -0.6062385008661115,
-            "angularVelocity": 0.00847795240879764,
-            "velocityX": 3.28077684769537,
-            "velocityY": 0.9904726220694814,
-            "timestamp": 1.1671566035365846
-        },
-        {
-            "x": 3.894448488053554,
-            "y": 2.8141322684658854,
-            "heading": -0.5991077413907381,
-            "angularVelocity": 0.10125184954600595,
-            "velocityX": 2.702098424500253,
-            "velocityY": 1.086825039829707,
-            "timestamp": 1.2375825711346722
-        },
-        {
-            "x": 4.043960756386648,
-            "y": 2.883897596305393,
-            "heading": -0.5885679211534333,
-            "angularVelocity": 0.1496581530473843,
-            "velocityX": 2.122970736963697,
-            "velocityY": 0.9906193726389452,
-            "timestamp": 1.3080085387327598
-        },
-        {
-            "x": 4.154363809487208,
-            "y": 2.940218619761556,
-            "heading": -0.5783208756256007,
-            "angularVelocity": 0.14550095479427785,
-            "velocityX": 1.5676469470837475,
-            "velocityY": 0.7997195548321091,
-            "timestamp": 1.3784345063308474
-        },
-        {
-            "x": 4.227038806897989,
-            "y": 2.9796144762337837,
-            "heading": -0.5703846528919979,
-            "angularVelocity": 0.11268887037369305,
-            "velocityX": 1.0319346668480025,
-            "velocityY": 0.5593938971070241,
-            "timestamp": 1.448860473928935
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 0.06225903656760019,
-            "velocityX": 0.5106240542868632,
-            "velocityY": 0.2894603292148455,
-            "timestamp": 1.5192864415270226
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 9.24329495349137e-32,
-            "velocityX": 0,
-            "velocityY": 1.2740379664097394e-33,
-            "timestamp": 1.5897124091251102
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.1.traj
deleted file mode 100644
index 2b0725c8..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.1.traj	
+++ /dev/null
@@ -1,256 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 9.57151893871228e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2927892107770984,
-            "y": 4.332309394440523,
-            "heading": -0.3722824674321399,
-            "angularVelocity": 0.3002660031020358,
-            "velocityX": -0.12220408518393268,
-            "velocityY": -0.4692835993371687,
-            "timestamp": 0.059006122520770025
-        },
-        {
-            "x": 2.2795380946752672,
-            "y": 4.276530177721837,
-            "heading": -0.3395678297253276,
-            "angularVelocity": 0.5544278510301563,
-            "velocityX": -0.224571883996739,
-            "velocityY": -0.9453123563415277,
-            "timestamp": 0.11801224504154005
-        },
-        {
-            "x": 2.2618545093830753,
-            "y": 4.192237257969867,
-            "heading": -0.2954402922678697,
-            "angularVelocity": 0.7478467584770477,
-            "velocityX": -0.29969068524994186,
-            "velocityY": -1.4285453127732544,
-            "timestamp": 0.17701836756231007
-        },
-        {
-            "x": 2.2420434066604593,
-            "y": 4.079048226765037,
-            "heading": -0.24482967906698166,
-            "angularVelocity": 0.8577179966887506,
-            "velocityX": -0.33574656114103973,
-            "velocityY": -1.9182590953165608,
-            "timestamp": 0.2360244900830801
-        },
-        {
-            "x": 2.2234541825747725,
-            "y": 3.936854095556344,
-            "heading": -0.19463836205748358,
-            "angularVelocity": 0.8506120189787272,
-            "velocityX": -0.3150389026010499,
-            "velocityY": -2.40981994976269,
-            "timestamp": 0.2950306126038501
-        },
-        {
-            "x": 2.211142623620853,
-            "y": 3.7664761504723194,
-            "heading": -0.15521354512815658,
-            "angularVelocity": 0.6681479013546628,
-            "velocityX": -0.2086488389333828,
-            "velocityY": -2.8874621446961175,
-            "timestamp": 0.35403673512462014
-        },
-        {
-            "x": 2.2125400151692816,
-            "y": 3.5715383450180926,
-            "heading": -0.14131017152189063,
-            "angularVelocity": 0.23562594883900537,
-            "velocityX": 0.023682144983125607,
-            "velocityY": -3.303687772156347,
-            "timestamp": 0.41304285764539017
-        },
-        {
-            "x": 2.235363094184648,
-            "y": 3.3576277458759463,
-            "heading": -0.144791073330025,
-            "angularVelocity": -0.058992214018278255,
-            "velocityX": 0.38679170974728244,
-            "velocityY": -3.6252271798888405,
-            "timestamp": 0.4720489801661602
-        },
-        {
-            "x": 2.287018648077521,
-            "y": 3.1404706845748738,
-            "heading": -0.14479216409186968,
-            "angularVelocity": -0.000018485570624385508,
-            "velocityX": 0.8754270181825738,
-            "velocityY": -3.6802462528296687,
-            "timestamp": 0.5310551026869302
-        },
-        {
-            "x": 2.3664206535422005,
-            "y": 2.9318538879811458,
-            "heading": -0.14479229465212481,
-            "angularVelocity": -0.000002212656069584398,
-            "velocityX": 1.3456570618876043,
-            "velocityY": -3.53551102295012,
-            "timestamp": 0.5900612252077002
-        },
-        {
-            "x": 2.4722259044647217,
-            "y": 2.73530650138855,
-            "heading": -0.14479252387880184,
-            "angularVelocity": -0.000003884794784189704,
-            "velocityX": 1.7931232625102267,
-            "velocityY": -3.330965977698533,
-            "timestamp": 0.6490673477284702
-        },
-        {
-            "x": 2.6222628757424284,
-            "y": 2.533912575740545,
-            "heading": -0.14479267276909355,
-            "angularVelocity": -0.0000022427572708026126,
-            "velocityX": 2.260029880862475,
-            "velocityY": -3.033627551346843,
-            "timestamp": 0.7154545103709612
-        },
-        {
-            "x": 2.800083925097921,
-            "y": 2.356570260194387,
-            "heading": -0.14479275333723068,
-            "angularVelocity": -0.0000012136101911737535,
-            "velocityX": 2.678545704884188,
-            "velocityY": -2.671334464182248,
-            "timestamp": 0.7818416730134522
-        },
-        {
-            "x": 3.001881619185789,
-            "y": 2.207076716019015,
-            "heading": -0.14479280967120484,
-            "angularVelocity": -8.485672817028763e-7,
-            "velocityX": 3.0397095772053357,
-            "velocityY": -2.2518441551784094,
-            "timestamp": 0.8482288356559432
-        },
-        {
-            "x": 3.2233351494929763,
-            "y": 2.0886327789398735,
-            "heading": -0.14479285532879432,
-            "angularVelocity": -6.877472642883405e-7,
-            "velocityX": 3.3357884490373766,
-            "velocityY": -1.7841391673415428,
-            "timestamp": 0.9146159982984342
-        },
-        {
-            "x": 3.4597028157497376,
-            "y": 2.003774307484127,
-            "heading": -0.14479289641721121,
-            "angularVelocity": -6.189211176196774e-7,
-            "velocityX": 3.5604423633775646,
-            "velocityY": -1.2782361540698368,
-            "timestamp": 0.9810031609409252
-        },
-        {
-            "x": 3.704835569204283,
-            "y": 1.949179734375487,
-            "heading": -0.14479293648657837,
-            "angularVelocity": -6.035710154638316e-7,
-            "velocityX": 3.692472214464675,
-            "velocityY": -0.8223664174750668,
-            "timestamp": 1.047390323583416
-        },
-        {
-            "x": 3.9499686718209266,
-            "y": 1.8945867290481686,
-            "heading": -0.1447929765559331,
-            "angularVelocity": -6.035708281824289e-7,
-            "velocityX": 3.692477473946853,
-            "velocityY": -0.8223428017448724,
-            "timestamp": 1.113777486225907
-        },
-        {
-            "x": 4.195101784677066,
-            "y": 1.8399937696982192,
-            "heading": -0.14479301662532418,
-            "angularVelocity": -6.035713756667977e-7,
-            "velocityX": 3.692477628185933,
-            "velocityY": -0.8223421091807183,
-            "timestamp": 1.1801646488683981
-        },
-        {
-            "x": 4.435904668374261,
-            "y": 1.7860806487644325,
-            "heading": -0.15898872227240893,
-            "angularVelocity": -0.21383208864538455,
-            "velocityX": 3.627250723064772,
-            "velocityY": -0.8121015989811171,
-            "timestamp": 1.2465518115108891
-        },
-        {
-            "x": 4.648245693564204,
-            "y": 1.7385415440023928,
-            "heading": -0.26462575063007965,
-            "angularVelocity": -1.5912267395211415,
-            "velocityX": 3.1985253886123286,
-            "velocityY": -0.7160888170209653,
-            "timestamp": 1.3129389741533801
-        },
-        {
-            "x": 4.824757883271093,
-            "y": 1.6989793937314068,
-            "heading": -0.37654836939407044,
-            "angularVelocity": -1.6859075506317058,
-            "velocityX": 2.6588301515075172,
-            "velocityY": -0.5959307296206723,
-            "timestamp": 1.3793261367958711
-        },
-        {
-            "x": 4.96555652453039,
-            "y": 1.6673723665346096,
-            "heading": -0.4762340168485147,
-            "angularVelocity": -1.501580177349536,
-            "velocityX": 2.1208715006774397,
-            "velocityY": -0.47610149219673153,
-            "timestamp": 1.4457132994383621
-        },
-        {
-            "x": 5.070906714490224,
-            "y": 1.643682366519874,
-            "heading": -0.5558849180153355,
-            "angularVelocity": -1.1997937251175768,
-            "velocityX": 1.5869060487969315,
-            "velocityY": -0.3568460990313944,
-            "timestamp": 1.5121004620808531
-        },
-        {
-            "x": 5.141006607602348,
-            "y": 1.6278935430132826,
-            "heading": -0.6113350442973207,
-            "angularVelocity": -0.8352537459779052,
-            "velocityX": 1.055925427776252,
-            "velocityY": -0.23782946699526453,
-            "timestamp": 1.5784876247233441
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -0.4317846186174008,
-            "velocityX": 0.5271108299370921,
-            "velocityY": -0.11890164753374041,
-            "timestamp": 1.6448747873658351
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 1.0662498702136072e-32,
-            "velocityX": 0,
-            "velocityY": 8.479039452208929e-33,
-            "timestamp": 1.7112619500083261
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.1.traj
deleted file mode 100644
index 087304c1..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 1.4326217036715317e-32,
-            "velocityX": -1.74824017803789e-33,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3001377838932537,
-            "y": 4.403828106793783,
-            "heading": -0.3629126711329447,
-            "angularVelocity": 0.37089781855009624,
-            "velocityX": 0.0018866291944089578,
-            "velocityY": 0.600123743495633,
-            "timestamp": 0.07303178264284323
-        },
-        {
-            "x": 2.300408122825978,
-            "y": 4.49157282256222,
-            "heading": -0.3106389398567459,
-            "angularVelocity": 0.7157668810008344,
-            "velocityX": 0.0037016614265912673,
-            "velocityY": 1.2014593180279707,
-            "timestamp": 0.14606356528568645
-        },
-        {
-            "x": 2.3008081314200264,
-            "y": 4.623360732427042,
-            "heading": -0.2362186265841639,
-            "angularVelocity": 1.019012689811082,
-            "velocityX": 0.005477185131915077,
-            "velocityY": 1.8045281806870437,
-            "timestamp": 0.2190953479285297
-        },
-        {
-            "x": 2.301347271135313,
-            "y": 4.79937800153835,
-            "heading": -0.14515320669009546,
-            "angularVelocity": 1.2469286192754925,
-            "velocityX": 0.007382261472700921,
-            "velocityY": 2.410146141058999,
-            "timestamp": 0.2921271305713729
-        },
-        {
-            "x": 2.302070167703843,
-            "y": 5.019849104537176,
-            "heading": -0.04952394454697364,
-            "angularVelocity": 1.3094197989222025,
-            "velocityX": 0.009898383174695338,
-            "velocityY": 3.018837758308937,
-            "timestamp": 0.3651589132142161
-        },
-        {
-            "x": 2.3031513688730865,
-            "y": 5.282690253403799,
-            "heading": -0.004114907781753178,
-            "angularVelocity": 0.6217708937393756,
-            "velocityX": 0.014804529344862738,
-            "velocityY": 3.598996756686451,
-            "timestamp": 0.43819069585705933
-        },
-        {
-            "x": 2.303520931398292,
-            "y": 5.5589648879428,
-            "heading": -0.0041148785146688655,
-            "angularVelocity": 4.00744487575221e-7,
-            "velocityX": 0.005060297199824665,
-            "velocityY": 3.7829370246937892,
-            "timestamp": 0.5112224784999025
-        },
-        {
-            "x": 2.3038904865521834,
-            "y": 5.835239522491655,
-            "heading": -0.004114849247579104,
-            "angularVelocity": 4.0074456218901027e-7,
-            "velocityX": 0.005060196266858216,
-            "velocityY": 3.7829370248287195,
-            "timestamp": 0.5842542611427458
-        },
-        {
-            "x": 2.302005656251481,
-            "y": 6.097682306820458,
-            "heading": 0.0412177605138735,
-            "angularVelocity": 0.6207244041015477,
-            "velocityX": -0.025808356752290516,
-            "velocityY": 3.5935420830717044,
-            "timestamp": 0.6572860437855891
-        },
-        {
-            "x": 2.3011707390613565,
-            "y": 6.318019040898657,
-            "heading": 0.13972629361592653,
-            "angularVelocity": 1.3488447020908976,
-            "velocityX": -0.01143224442716972,
-            "velocityY": 3.016997889203128,
-            "timestamp": 0.7303178264284323
-        },
-        {
-            "x": 2.3006512841511224,
-            "y": 6.493901741171369,
-            "heading": 0.23378956116370217,
-            "angularVelocity": 1.2879771538343168,
-            "velocityX": -0.007112723959845375,
-            "velocityY": 2.408303534542668,
-            "timestamp": 0.8033496090712756
-        },
-        {
-            "x": 2.300310658917655,
-            "y": 6.625570031648912,
-            "heading": 0.310734904704345,
-            "angularVelocity": 1.053587092580481,
-            "velocityX": -0.00466406845267762,
-            "velocityY": 1.802890272053981,
-            "timestamp": 0.8763813917141189
-        },
-        {
-            "x": 2.3001003670918414,
-            "y": 6.7132235530415985,
-            "heading": 0.3648714550351714,
-            "angularVelocity": 0.7412738450542471,
-            "velocityX": -0.0028794562888088465,
-            "velocityY": 1.2002106236588805,
-            "timestamp": 0.9494131743569622
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.38515484556072405,
-            "velocityX": -0.0013742933310585549,
-            "velocityY": 0.5994163824877027,
-            "timestamp": 1.0224449569998053
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 2.8585488767662063e-32,
-            "velocityX": -1.0732467562704116e-31,
-            "velocityY": 0,
-            "timestamp": 1.0954767396426486
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj
deleted file mode 100644
index 4345e451..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 2.0150450292109907e-34,
-            "velocityY": 3.039389955204995e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2980012520823867,
-            "y": 4.4080014432651184,
-            "heading": -0.3744767713085023,
-            "angularVelocity": 0.2040875990087458,
-            "velocityX": -0.02627801674742958,
-            "velocityY": 0.6310864511258738,
-            "timestamp": 0.07606159691668693
-        },
-        {
-            "x": 2.294003780157748,
-            "y": 4.504004078538053,
-            "heading": -0.34341554427294535,
-            "angularVelocity": 0.40836937817095076,
-            "velocityX": -0.05255571913664827,
-            "velocityY": 1.262169598911913,
-            "timestamp": 0.15212319383337386
-        },
-        {
-            "x": 2.2880075055108158,
-            "y": 4.648007445856594,
-            "heading": -0.296791131111893,
-            "angularVelocity": 0.6129823071177671,
-            "velocityX": -0.07883445641432706,
-            "velocityY": 1.8932466994648223,
-            "timestamp": 0.2281847907500608
-        },
-        {
-            "x": 2.2800121106149036,
-            "y": 4.840010896492559,
-            "heading": -0.2345694561462187,
-            "angularVelocity": 0.8180432371651087,
-            "velocityX": -0.10511736829125365,
-            "velocityY": 2.5243152710332057,
-            "timestamp": 0.3042463876667477
-        },
-        {
-            "x": 2.270016860554186,
-            "y": 5.080013636963748,
-            "heading": -0.15671112019223,
-            "angularVelocity": 1.023622157700289,
-            "velocityX": -0.13140994228224737,
-            "velocityY": 3.1553734105013973,
-            "timestamp": 0.3803079845834346
-        },
-        {
-            "x": 2.2620106394348882,
-            "y": 5.2720083260038075,
-            "heading": -0.09403333701953483,
-            "angularVelocity": 0.82403980081234,
-            "velocityX": -0.10525970323851294,
-            "velocityY": 2.524200080237073,
-            "timestamp": 0.4563695815001215
-        },
-        {
-            "x": 2.256005452365101,
-            "y": 5.416004188429854,
-            "heading": -0.0470178538159679,
-            "angularVelocity": 0.6181237984664553,
-            "velocityX": -0.07895163016843816,
-            "velocityY": 1.8931480308488688,
-            "timestamp": 0.5324311784168084
-        },
-        {
-            "x": 2.2520018201736485,
-            "y": 5.5120013950535185,
-            "heading": -0.01567260522346991,
-            "angularVelocity": 0.4121034774859067,
-            "velocityX": -0.05263670963729669,
-            "velocityY": 1.262098227162051,
-            "timestamp": 0.6084927753334953
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 1.3656663814986182e-35,
-            "angularVelocity": 0.20605148798856665,
-            "velocityX": -0.026318408432062,
-            "velocityY": 0.6310491350721452,
-            "timestamp": 0.6845543722501822
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 6.6551444751621536e-34,
-            "velocityX": 3.9479513180483775e-34,
-            "velocityY": 0,
-            "timestamp": 0.7606159691668691
-        }
-    ]
-}
\ No newline at end of file

From 3e7f6aedd9488430eb24d075965da1dd80272d42 Mon Sep 17 00:00:00 2001
From: Lewis-Seiden <lewy@seiden.us>
Date: Mon, 19 Feb 2024 20:48:14 -0800
Subject: [PATCH 43/62] adjust climb leds

---
 src/main/java/frc/robot/Robot.java            | 42 ++++++++++++-------
 .../frc/robot/subsystems/leds/LEDIOSim.java   |  3 +-
 .../robot/subsystems/leds/LEDSubsystem.java   |  2 +-
 3 files changed, 29 insertions(+), 18 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 391f916c..48893114 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -268,21 +268,24 @@ public void robotInit() {
         .and(() -> elevator.getExtensionMeters() > 0.9 * ElevatorSubsystem.CLIMB_EXTENSION_METERS)
         .onTrue(
             Commands.sequence(
-                elevator
-                    .setExtensionCmd(() -> 0.0)
-                    .until(() -> elevator.getExtensionMeters() < 0.05),
-                Commands.waitUntil(() -> controller.y().getAsBoolean()),
-                Commands.parallel(
-                    carriage
-                        .runVoltageCmd(-CarriageSubsystem.INDEXING_VOLTAGE)
-                        .withTimeout(0.25)
-                        .andThen(
-                            Commands.waitUntil(
-                                () ->
-                                    elevator.getExtensionMeters()
-                                        > 0.95 * ElevatorSubsystem.TRAP_EXTENSION_METERS),
-                            carriage.runVoltageCmd(-CarriageSubsystem.INDEXING_VOLTAGE)),
-                    elevator.setExtensionCmd(() -> ElevatorSubsystem.TRAP_EXTENSION_METERS))));
+                    elevator
+                        .setExtensionCmd(() -> 0.0)
+                        .until(() -> elevator.getExtensionMeters() < 0.05),
+                    Commands.waitUntil(() -> controller.y().getAsBoolean()),
+                    Commands.parallel(
+                        carriage
+                            .runVoltageCmd(-CarriageSubsystem.INDEXING_VOLTAGE)
+                            .withTimeout(0.25)
+                            .andThen(
+                                Commands.parallel(
+                                    Commands.waitUntil(
+                                        () ->
+                                            elevator.getExtensionMeters()
+                                                > 0.95 * ElevatorSubsystem.TRAP_EXTENSION_METERS),
+                                    carriage.runVoltageCmd(0.0)),
+                                carriage.runVoltageCmd(-CarriageSubsystem.INDEXING_VOLTAGE)),
+                        elevator.setExtensionCmd(() -> ElevatorSubsystem.TRAP_EXTENSION_METERS)))
+                .alongWith(leds.setRainbowCmd()));
 
     // Prep climb
     operator
@@ -290,7 +293,14 @@ public void robotInit() {
         .and(operator.rightBumper())
         .toggleOnFalse(
             Commands.parallel(
-                elevator.setExtensionCmd(() -> ElevatorSubsystem.CLIMB_EXTENSION_METERS)));
+                elevator.setExtensionCmd(() -> ElevatorSubsystem.CLIMB_EXTENSION_METERS),
+                leds.setBlinkingCmd(new Color("#ff0000"), new Color("#ffffff"), 25.0)
+                    .until(
+                        () ->
+                            elevator.getExtensionMeters()
+                                > 0.9 * ElevatorSubsystem.CLIMB_EXTENSION_METERS)
+                    .andThen(
+                        leds.setBlinkingCmd(new Color("#00ff00"), new Color("#ffffff"), 25.0))));
     operator.leftTrigger().onTrue(Commands.runOnce(() -> currentTarget = Target.SPEAKER));
     operator.leftBumper().onTrue(Commands.runOnce(() -> currentTarget = Target.AMP));
     operator.a().onTrue(Commands.runOnce(() -> flywheelIdleSpeed = -0.1));
diff --git a/src/main/java/frc/robot/subsystems/leds/LEDIOSim.java b/src/main/java/frc/robot/subsystems/leds/LEDIOSim.java
index 0c8ffebf..94bf0127 100644
--- a/src/main/java/frc/robot/subsystems/leds/LEDIOSim.java
+++ b/src/main/java/frc/robot/subsystems/leds/LEDIOSim.java
@@ -8,7 +8,8 @@
 import edu.wpi.first.wpilibj.AddressableLEDBuffer;
 import edu.wpi.first.wpilibj.util.Color;
 
-// This is a copy of LEDIOReal right now, but if we move leds to separate ports sim breaks w LEDIOReal
+// This is a copy of LEDIOReal right now, but if we move leds to separate ports sim breaks w
+// LEDIOReal
 public class LEDIOSim implements LEDIO {
   private final AddressableLED led;
   private final AddressableLEDBuffer buffer;
diff --git a/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java
index ec4758f8..f17d2633 100644
--- a/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/leds/LEDSubsystem.java
@@ -18,7 +18,7 @@
 
 public class LEDSubsystem extends SubsystemBase {
   public static final int LED_LENGTH = 36;
-  
+
   private final LEDIO io;
   private final LEDIOInputsAutoLogged inputs = new LEDIOInputsAutoLogged();
 

From 55eab86795819b9437bfc20596224c0ba6cdaad3 Mon Sep 17 00:00:00 2001
From: Lewis-Seiden <lewy@seiden.us>
Date: Mon, 19 Feb 2024 23:11:05 -0800
Subject: [PATCH 44/62] redid indexing logic

---
 src/main/java/frc/robot/Robot.java | 36 +++++++++++++++---------------
 1 file changed, 18 insertions(+), 18 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 48893114..b9c69deb 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -20,7 +20,6 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -159,12 +158,25 @@ public void robotInit() {
                         * SwerveSubsystem.MAX_ANGULAR_SPEED)));
     elevator.setDefaultCommand(elevator.setExtensionCmd(() -> 0.0));
     feeder.setDefaultCommand(
-        Commands.either(
-            feeder.indexCmd(), feeder.runVoltageCmd(0.0), () -> currentTarget == Target.SPEAKER));
+        Commands.repeatingSequence(
+            feeder.indexCmd().until(() -> currentTarget == Target.AMP),
+            Commands.sequence(
+                    feeder
+                        .runVoltageCmd(-FeederSubsystem.INDEXING_VOLTAGE)
+                        .until(() -> carriage.getBeambreak()),
+                    feeder.runVoltageCmd(-FeederSubsystem.INDEXING_VOLTAGE).withTimeout(0.5),
+                    feeder.runVoltageCmd(0.0))
+                .until(() -> currentTarget == Target.SPEAKER)));
     carriage.setDefaultCommand(
-        carriage
-            .runVoltageCmd(CarriageSubsystem.INDEXING_VOLTAGE)
-            .withInterruptBehavior(InterruptionBehavior.kCancelSelf));
+        Commands.repeatingSequence(
+            carriage.indexBackwardsCmd().until(() -> currentTarget == Target.AMP),
+            Commands.sequence(
+                    carriage
+                        .runVoltageCmd(CarriageSubsystem.INDEXING_VOLTAGE)
+                        .until(() -> feeder.getFirstBeambreak()),
+                    carriage.runVoltageCmd(CarriageSubsystem.INDEXING_VOLTAGE).withTimeout(0.5),
+                    carriage.runVoltageCmd(0.0))
+                .until(() -> currentTarget == Target.SPEAKER)));
     intake.setDefaultCommand(intake.runVoltageCmd(0.0, 0.0));
     shooter.setDefaultCommand(
         shooter.runStateCmd(
@@ -186,18 +198,6 @@ public void robotInit() {
                     controller.rumbleCmd(1.0, 1.0),
                     leds.setBlinkingCmd(new Color("#ff8000"), new Color("#000000"), 25.0))
                 .withTimeout(0.5));
-    // Handled by default commands
-    // new Trigger(() -> currentTarget == Target.SPEAKER)
-    //     .whileTrue(feeder.indexCmd())
-    //     .whileTrue(carriage.runVoltageCmd(5.0));
-    new Trigger(() -> currentTarget == Target.AMP)
-        .whileTrue(
-            Commands.either(
-                Commands.parallel(
-                    carriage.indexBackwardsCmd(),
-                    feeder.runVoltageCmd(-FeederSubsystem.INDEXING_VOLTAGE).withTimeout(1.0)),
-                Commands.parallel(carriage.indexForwardsCmd(), feeder.runVoltageCmd(0.0)),
-                () -> feeder.getFirstBeambreak()));
 
     // ---- Controller bindings here ----
     controller.leftTrigger().whileTrue(intake.runVelocityCmd(80.0, 30.0));

From 11cc1e1088df5eb897a9828544d2a444431baab9 Mon Sep 17 00:00:00 2001
From: Stoopipoopy <anotherguywithabeanie@gmail.com>
Date: Tue, 20 Feb 2024 11:33:15 -0800
Subject: [PATCH 45/62] updated makePath method to create a path between notes

---
 ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 src/main/java/frc/robot/Robot.java            |   2 +-
 .../robot/utils/dynamicauto/DynamicAuto.java  |  12 +++++++-----
 .../frc/robot/utils/dynamicauto/Note.java     |   8 ++++++--
 6 files changed, 14 insertions(+), 8 deletions(-)

diff --git a/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat
index 86cf1817e71f054658a63fc7a3ae2da55b29376b..f1a1f3d8b41ebe494304df94fbdd1ab2f5ac29ae 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx#VdL2@!-;}cY)2Tjob3Ry7BR5_0IRMFyZ`_I

delta 30
kcmZn=Xb{-Yz{qx-;q>`D{fUBBY$q5_oG$>e7BR5_0IVtsyZ`_I

diff --git a/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat
index e92615e27dbb98fe2103f24951c289c5d4477e5b..335610449fb6831a3e0db70398ff7f3d00dacbba 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx#VdL2@!-;}cY)2Tjob3Ry7BR5_0IRMFyZ`_I

delta 30
kcmZn=Xb{-Yz{qx-;q>`D{fUBBY$q5_oG$>e7BR5_0IVtsyZ`_I

diff --git a/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat
index 56a574f5e08735170b67330b5accdb5e13d9d168..952ae7d554ee2f34cfdab03eb9da44ecf50b23d8 100644
GIT binary patch
delta 36
ncmZn=Xb{-Yz{q)+VddEtLk0$ciGt>wM;Ml!ZG&<aF|h#v*5(Sl

delta 36
ncmZn=Xb{-Yz{q)=Vek1IeFg@BiGt>w#~2Qr&w_FmF|h#v*J291

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 5cf3fa12..3d93b182 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -144,7 +144,7 @@ public void robotInit() {
         .x()
         .toggleOnTrue(
             Commands.run(
-                () -> swerve.runChoreoTraj(DynamicAuto.makePath(swerve::getPose)), swerve));
+                () -> swerve.runChoreoTraj(DynamicAuto.makeNoteToNote(swerve::getPose)), swerve));
   }
 
   @Override
diff --git a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
index b57ad5d9..77d12419 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
@@ -39,11 +39,13 @@ public static ShootingLocation closestShootingLocation(Supplier<Pose2d> curPose)
     return closestLocation;
   }
 
-  public static ChoreoTrajectory makePath(Supplier<Pose2d> startingPose) {
-    return Choreo.getTrajectory(
-        getClosestNote(startingPose.get()).getName()
-            + " To "
-            + closestShootingLocation(startingPose).getName());
+  public static ChoreoTrajectory makeNoteToNote(Supplier<Pose2d> startingPose) {
+    Note closestNote = getClosestNote(startingPose.get());
+    closestNote.blacklist();
+    Note nextClosest = getClosestNote(startingPose.get());
+    closestNote.whitelist();
+    
+    return Choreo.getTrajectory(closestNote.getName() + " To " + nextClosest.getName());
   }
 
   public static Note getClosestNote(Pose2d curPose) {
diff --git a/src/main/java/frc/robot/utils/dynamicauto/Note.java b/src/main/java/frc/robot/utils/dynamicauto/Note.java
index b827ba29..faed9968 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/Note.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/Note.java
@@ -28,8 +28,12 @@ public Note() {
     this.name = "Uninitialized";
   }
 
-  public void setBlacklist(boolean blacklist) {
-    this.blacklisted = blacklist;
+  public void blacklist(){
+    this.blacklisted = true;
+  }
+
+  public void whitelist(){
+    this.blacklisted = false;
   }
 
   public void setPriority(Integer priority) {

From 933067b1b09342b35bb15c8fb59698d7e51e6562 Mon Sep 17 00:00:00 2001
From: Stoopipoopy <anotherguywithabeanie@gmail.com>
Date: Tue, 20 Feb 2024 11:39:19 -0800
Subject: [PATCH 46/62] put correct folder path for removing traj splits

---
 src/main/java/frc/robot/utils/dynamicauto/RemoveTrajSplits.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/utils/dynamicauto/RemoveTrajSplits.py b/src/main/java/frc/robot/utils/dynamicauto/RemoveTrajSplits.py
index f32c0193..878f1c16 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/RemoveTrajSplits.py
+++ b/src/main/java/frc/robot/utils/dynamicauto/RemoveTrajSplits.py
@@ -2,7 +2,7 @@
 import os
 import urllib.parse
 
-folder_path = "src/main/java/frc/robot/utils/dynamicauto/deploy/choreo"
+folder_path = "C:\\Users\\Robotics\\Desktop\\Crescendo\\src\\main\\java\\frc\\robot\\utils\\dynamicauto\\deploy\\choreo"
 files_in_folder = os.listdir(folder_path)
 
 print("Files in deploy/choreo folder:")

From 3cf7075f2318da628b70cb6a6602d51347d41499 Mon Sep 17 00:00:00 2001
From: Stoopipoopy <anotherguywithabeanie@gmail.com>
Date: Tue, 20 Feb 2024 12:11:49 -0800
Subject: [PATCH 47/62] added getAbsoluteClosestNote and blacklist/whitelist on
 bumpers

---
 ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat |  Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat |  Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat |  Bin 2048 -> 2048 bytes
 src/main/deploy/choreo/Amp Side To C1.traj    |  274 +++++
 src/main/deploy/choreo/Amp Side To C2.traj    |  319 +++++
 src/main/deploy/choreo/Amp Side To C3.traj    |  526 ++++++++
 src/main/deploy/choreo/Amp Side To C4.traj    |  400 ++++++
 src/main/deploy/choreo/Amp Side To C5.traj    |  805 ++++++++++++
 src/main/deploy/choreo/Amp Side To W1.traj    |  121 ++
 src/main/deploy/choreo/Amp Side To W2.traj    |  148 +++
 src/main/deploy/choreo/Amp Side To W3.traj    |  193 +++
 src/main/deploy/choreo/C1 To C2.traj          |   94 ++
 src/main/deploy/choreo/C1 To C3.traj          |  139 +++
 src/main/deploy/choreo/C1 To C4.traj          |  175 +++
 src/main/deploy/choreo/C1 To C5.traj          |  220 ++++
 src/main/deploy/choreo/C1 To S1.traj          |  130 ++
 src/main/deploy/choreo/C1 To S2.traj          |  148 +++
 src/main/deploy/choreo/C1 To S3.traj          |  265 ++++
 src/main/deploy/choreo/C1 To S4.traj          |  301 +++++
 src/main/deploy/choreo/C1 To W1.traj          |  202 +++
 src/main/deploy/choreo/C1 To W2.traj          |  193 +++
 src/main/deploy/choreo/C1 To W3.traj          |  274 +++++
 src/main/deploy/choreo/C2 To C1.traj          |   94 ++
 src/main/deploy/choreo/C2 To C3.traj          |   94 ++
 src/main/deploy/choreo/C2 To C4.traj          |  139 +++
 src/main/deploy/choreo/C2 To C5.traj          |  175 +++
 src/main/deploy/choreo/C2 To S1.traj          |  130 ++
 src/main/deploy/choreo/C2 To S2.traj          |  193 +++
 src/main/deploy/choreo/C2 To S3.traj          |  193 +++
 src/main/deploy/choreo/C2 To S4.traj          |  256 ++++
 src/main/deploy/choreo/C2 To W1.traj          |  202 +++
 src/main/deploy/choreo/C2 To W2.traj          |  184 +++
 src/main/deploy/choreo/C2 To W3.traj          |  346 ++++++
 src/main/deploy/choreo/C3 To C1.traj          |  139 +++
 src/main/deploy/choreo/C3 To C2.traj          |   94 ++
 src/main/deploy/choreo/C3 To C4.traj          |   94 ++
 src/main/deploy/choreo/C3 To C5.traj          |  139 +++
 src/main/deploy/choreo/C3 To S1.traj          |  157 +++
 src/main/deploy/choreo/C3 To S2.traj          |  148 +++
 src/main/deploy/choreo/C3 To S3.traj          |  166 +++
 src/main/deploy/choreo/C3 To S4.traj          |  238 ++++
 src/main/deploy/choreo/C3 To W1.traj          |  220 ++++
 src/main/deploy/choreo/C3 To W2.traj          |  193 +++
 src/main/deploy/choreo/C3 To W3.traj          |  202 +++
 src/main/deploy/choreo/C4 To C1.traj          |  175 +++
 src/main/deploy/choreo/C4 To C2.traj          |  139 +++
 src/main/deploy/choreo/C4 To C3.traj          |   94 ++
 src/main/deploy/choreo/C4 To C5.traj          |   94 ++
 src/main/deploy/choreo/C4 To S3.traj          |  229 ++++
 src/main/deploy/choreo/C4 To S4.traj          |  193 +++
 src/main/deploy/choreo/C4 To W1.traj          |  238 ++++
 src/main/deploy/choreo/C4 To W2.traj          |  256 ++++
 src/main/deploy/choreo/C4 To W3.traj          |  274 +++++
 src/main/deploy/choreo/C5 To C1.traj          |  220 ++++
 src/main/deploy/choreo/C5 To C2.traj          |  175 +++
 src/main/deploy/choreo/C5 To C3.traj          |  139 +++
 src/main/deploy/choreo/C5 To C4.traj          |   94 ++
 src/main/deploy/choreo/C5 To S3.traj          |  247 ++++
 src/main/deploy/choreo/C5 To S4.traj          |  148 +++
 src/main/deploy/choreo/C5 To W1.traj          |  310 +++++
 src/main/deploy/choreo/C5 To W2.traj          |  283 +++++
 src/main/deploy/choreo/C5 To W3.traj          |  292 +++++
 src/main/deploy/choreo/Center To C1.traj      |  220 ++++
 src/main/deploy/choreo/Center To C2.traj      |  211 ++++
 src/main/deploy/choreo/Center To C3.traj      |  211 ++++
 src/main/deploy/choreo/Center To C4.traj      | 1093 +++++++++++++++++
 src/main/deploy/choreo/Center To C5.traj      |  625 ++++++++++
 src/main/deploy/choreo/Center To W1.traj      |  112 ++
 src/main/deploy/choreo/Center To W2.traj      |   76 ++
 src/main/deploy/choreo/Center To W3.traj      |  112 ++
 src/main/deploy/choreo/S1 To C1.traj          |  130 ++
 src/main/deploy/choreo/S1 To C2.traj          |  130 ++
 src/main/deploy/choreo/S1 To C3.traj          |  157 +++
 src/main/deploy/choreo/S1 To C4.traj          |  247 ++++
 src/main/deploy/choreo/S1 To C5.traj          |  292 +++++
 src/main/deploy/choreo/S1 To W1.traj          |  130 ++
 src/main/deploy/choreo/S1 To W2.traj          |  148 +++
 src/main/deploy/choreo/S1 To W3.traj          |  175 +++
 src/main/deploy/choreo/S2 To C1.traj          |  148 +++
 src/main/deploy/choreo/S2 To C2.traj          |  202 +++
 src/main/deploy/choreo/S2 To C3.traj          |  148 +++
 src/main/deploy/choreo/S2 To C4.traj          |  166 +++
 src/main/deploy/choreo/S2 To C5.traj          |  202 +++
 src/main/deploy/choreo/S2 To W1.traj          |  130 ++
 src/main/deploy/choreo/S2 To W2.traj          |  103 ++
 src/main/deploy/choreo/S2 To W3.traj          |  130 ++
 src/main/deploy/choreo/S3 To C1.traj          |  220 ++++
 src/main/deploy/choreo/S3 To C2.traj          |  193 +++
 src/main/deploy/choreo/S3 To C3.traj          |  166 +++
 src/main/deploy/choreo/S3 To C4.traj          |  238 ++++
 src/main/deploy/choreo/S3 To C5.traj          |  184 +++
 src/main/deploy/choreo/S3 To W1.traj          |  202 +++
 src/main/deploy/choreo/S3 To W2.traj          |  157 +++
 src/main/deploy/choreo/S3 To W3.traj          |  121 ++
 src/main/deploy/choreo/S4 To C1.traj          |  292 +++++
 src/main/deploy/choreo/S4 To C2.traj          |  202 +++
 src/main/deploy/choreo/S4 To C3.traj          |  238 ++++
 src/main/deploy/choreo/S4 To C4.traj          |  148 +++
 src/main/deploy/choreo/S4 To C5.traj          |  148 +++
 src/main/deploy/choreo/S4 To W1.traj          |  247 ++++
 src/main/deploy/choreo/S4 To W2.traj          |  202 +++
 src/main/deploy/choreo/S4 To W3.traj          |  256 ++++
 src/main/deploy/choreo/Stage Side To C1.traj  |  616 ++++++++++
 src/main/deploy/choreo/Stage Side To C2.traj  |  607 +++++++++
 src/main/deploy/choreo/Stage Side To C3.traj  |  274 +++++
 src/main/deploy/choreo/Stage Side To C4.traj  |  679 ++++++++++
 src/main/deploy/choreo/Stage Side To C5.traj  |  337 +++++
 src/main/deploy/choreo/Stage Side To W1.traj  |  193 +++
 src/main/deploy/choreo/Stage Side To W2.traj  |  148 +++
 src/main/deploy/choreo/Stage Side To W3.traj  |  121 ++
 src/main/deploy/choreo/W1 To C1.traj          |  202 +++
 src/main/deploy/choreo/W1 To C2.traj          |  256 ++++
 src/main/deploy/choreo/W1 To C3.traj          |  193 +++
 src/main/deploy/choreo/W1 To C4.traj          |  400 ++++++
 src/main/deploy/choreo/W1 To C5.traj          |  616 ++++++++++
 src/main/deploy/choreo/W1 To S1.traj          |  130 ++
 src/main/deploy/choreo/W1 To S2.traj          |  130 ++
 src/main/deploy/choreo/W1 To S3.traj          |  544 ++++++++
 src/main/deploy/choreo/W1 To S4.traj          |  562 +++++++++
 src/main/deploy/choreo/W1 To W2.traj          |  103 ++
 src/main/deploy/choreo/W1 To W3.traj          |  148 +++
 src/main/deploy/choreo/W2 To C1.traj          |  193 +++
 src/main/deploy/choreo/W2 To C2.traj          |  526 ++++++++
 src/main/deploy/choreo/W2 To C3.traj          |  571 +++++++++
 src/main/deploy/choreo/W2 To C4.traj          |  589 +++++++++
 src/main/deploy/choreo/W2 To C5.traj          |  625 ++++++++++
 src/main/deploy/choreo/W2 To S1.traj          |  148 +++
 src/main/deploy/choreo/W2 To S2.traj          |  103 ++
 src/main/deploy/choreo/W2 To S3.traj          |  526 ++++++++
 src/main/deploy/choreo/W2 To S4.traj          |  553 +++++++++
 src/main/deploy/choreo/W2 To W1.traj          |  103 ++
 src/main/deploy/choreo/W2 To W3.traj          |  103 ++
 src/main/deploy/choreo/W3 To C1.traj          |  553 +++++++++
 src/main/deploy/choreo/W3 To C2.traj          |  553 +++++++++
 src/main/deploy/choreo/W3 To C3.traj          |  310 +++++
 src/main/deploy/choreo/W3 To C4.traj          |  625 ++++++++++
 src/main/deploy/choreo/W3 To C5.traj          |  571 +++++++++
 src/main/deploy/choreo/W3 To S1.traj          |  175 +++
 src/main/deploy/choreo/W3 To S2.traj          |  130 ++
 src/main/deploy/choreo/W3 To S3.traj          |  229 ++++
 src/main/deploy/choreo/W3 To S4.traj          |  256 ++++
 src/main/deploy/choreo/W3 To W1.traj          |  148 +++
 src/main/deploy/choreo/W3 To W2.traj          |  103 ++
 src/main/java/frc/robot/Robot.java            |   17 +-
 .../robot/utils/dynamicauto/DynamicAuto.java  |   55 +-
 .../frc/robot/utils/dynamicauto/Note.java     |    4 +-
 146 files changed, 34836 insertions(+), 27 deletions(-)
 create mode 100644 src/main/deploy/choreo/Amp Side To C1.traj
 create mode 100644 src/main/deploy/choreo/Amp Side To C2.traj
 create mode 100644 src/main/deploy/choreo/Amp Side To C3.traj
 create mode 100644 src/main/deploy/choreo/Amp Side To C4.traj
 create mode 100644 src/main/deploy/choreo/Amp Side To C5.traj
 create mode 100644 src/main/deploy/choreo/Amp Side To W1.traj
 create mode 100644 src/main/deploy/choreo/Amp Side To W2.traj
 create mode 100644 src/main/deploy/choreo/Amp Side To W3.traj
 create mode 100644 src/main/deploy/choreo/C1 To C2.traj
 create mode 100644 src/main/deploy/choreo/C1 To C3.traj
 create mode 100644 src/main/deploy/choreo/C1 To C4.traj
 create mode 100644 src/main/deploy/choreo/C1 To C5.traj
 create mode 100644 src/main/deploy/choreo/C1 To S1.traj
 create mode 100644 src/main/deploy/choreo/C1 To S2.traj
 create mode 100644 src/main/deploy/choreo/C1 To S3.traj
 create mode 100644 src/main/deploy/choreo/C1 To S4.traj
 create mode 100644 src/main/deploy/choreo/C1 To W1.traj
 create mode 100644 src/main/deploy/choreo/C1 To W2.traj
 create mode 100644 src/main/deploy/choreo/C1 To W3.traj
 create mode 100644 src/main/deploy/choreo/C2 To C1.traj
 create mode 100644 src/main/deploy/choreo/C2 To C3.traj
 create mode 100644 src/main/deploy/choreo/C2 To C4.traj
 create mode 100644 src/main/deploy/choreo/C2 To C5.traj
 create mode 100644 src/main/deploy/choreo/C2 To S1.traj
 create mode 100644 src/main/deploy/choreo/C2 To S2.traj
 create mode 100644 src/main/deploy/choreo/C2 To S3.traj
 create mode 100644 src/main/deploy/choreo/C2 To S4.traj
 create mode 100644 src/main/deploy/choreo/C2 To W1.traj
 create mode 100644 src/main/deploy/choreo/C2 To W2.traj
 create mode 100644 src/main/deploy/choreo/C2 To W3.traj
 create mode 100644 src/main/deploy/choreo/C3 To C1.traj
 create mode 100644 src/main/deploy/choreo/C3 To C2.traj
 create mode 100644 src/main/deploy/choreo/C3 To C4.traj
 create mode 100644 src/main/deploy/choreo/C3 To C5.traj
 create mode 100644 src/main/deploy/choreo/C3 To S1.traj
 create mode 100644 src/main/deploy/choreo/C3 To S2.traj
 create mode 100644 src/main/deploy/choreo/C3 To S3.traj
 create mode 100644 src/main/deploy/choreo/C3 To S4.traj
 create mode 100644 src/main/deploy/choreo/C3 To W1.traj
 create mode 100644 src/main/deploy/choreo/C3 To W2.traj
 create mode 100644 src/main/deploy/choreo/C3 To W3.traj
 create mode 100644 src/main/deploy/choreo/C4 To C1.traj
 create mode 100644 src/main/deploy/choreo/C4 To C2.traj
 create mode 100644 src/main/deploy/choreo/C4 To C3.traj
 create mode 100644 src/main/deploy/choreo/C4 To C5.traj
 create mode 100644 src/main/deploy/choreo/C4 To S3.traj
 create mode 100644 src/main/deploy/choreo/C4 To S4.traj
 create mode 100644 src/main/deploy/choreo/C4 To W1.traj
 create mode 100644 src/main/deploy/choreo/C4 To W2.traj
 create mode 100644 src/main/deploy/choreo/C4 To W3.traj
 create mode 100644 src/main/deploy/choreo/C5 To C1.traj
 create mode 100644 src/main/deploy/choreo/C5 To C2.traj
 create mode 100644 src/main/deploy/choreo/C5 To C3.traj
 create mode 100644 src/main/deploy/choreo/C5 To C4.traj
 create mode 100644 src/main/deploy/choreo/C5 To S3.traj
 create mode 100644 src/main/deploy/choreo/C5 To S4.traj
 create mode 100644 src/main/deploy/choreo/C5 To W1.traj
 create mode 100644 src/main/deploy/choreo/C5 To W2.traj
 create mode 100644 src/main/deploy/choreo/C5 To W3.traj
 create mode 100644 src/main/deploy/choreo/Center To C1.traj
 create mode 100644 src/main/deploy/choreo/Center To C2.traj
 create mode 100644 src/main/deploy/choreo/Center To C3.traj
 create mode 100644 src/main/deploy/choreo/Center To C4.traj
 create mode 100644 src/main/deploy/choreo/Center To C5.traj
 create mode 100644 src/main/deploy/choreo/Center To W1.traj
 create mode 100644 src/main/deploy/choreo/Center To W2.traj
 create mode 100644 src/main/deploy/choreo/Center To W3.traj
 create mode 100644 src/main/deploy/choreo/S1 To C1.traj
 create mode 100644 src/main/deploy/choreo/S1 To C2.traj
 create mode 100644 src/main/deploy/choreo/S1 To C3.traj
 create mode 100644 src/main/deploy/choreo/S1 To C4.traj
 create mode 100644 src/main/deploy/choreo/S1 To C5.traj
 create mode 100644 src/main/deploy/choreo/S1 To W1.traj
 create mode 100644 src/main/deploy/choreo/S1 To W2.traj
 create mode 100644 src/main/deploy/choreo/S1 To W3.traj
 create mode 100644 src/main/deploy/choreo/S2 To C1.traj
 create mode 100644 src/main/deploy/choreo/S2 To C2.traj
 create mode 100644 src/main/deploy/choreo/S2 To C3.traj
 create mode 100644 src/main/deploy/choreo/S2 To C4.traj
 create mode 100644 src/main/deploy/choreo/S2 To C5.traj
 create mode 100644 src/main/deploy/choreo/S2 To W1.traj
 create mode 100644 src/main/deploy/choreo/S2 To W2.traj
 create mode 100644 src/main/deploy/choreo/S2 To W3.traj
 create mode 100644 src/main/deploy/choreo/S3 To C1.traj
 create mode 100644 src/main/deploy/choreo/S3 To C2.traj
 create mode 100644 src/main/deploy/choreo/S3 To C3.traj
 create mode 100644 src/main/deploy/choreo/S3 To C4.traj
 create mode 100644 src/main/deploy/choreo/S3 To C5.traj
 create mode 100644 src/main/deploy/choreo/S3 To W1.traj
 create mode 100644 src/main/deploy/choreo/S3 To W2.traj
 create mode 100644 src/main/deploy/choreo/S3 To W3.traj
 create mode 100644 src/main/deploy/choreo/S4 To C1.traj
 create mode 100644 src/main/deploy/choreo/S4 To C2.traj
 create mode 100644 src/main/deploy/choreo/S4 To C3.traj
 create mode 100644 src/main/deploy/choreo/S4 To C4.traj
 create mode 100644 src/main/deploy/choreo/S4 To C5.traj
 create mode 100644 src/main/deploy/choreo/S4 To W1.traj
 create mode 100644 src/main/deploy/choreo/S4 To W2.traj
 create mode 100644 src/main/deploy/choreo/S4 To W3.traj
 create mode 100644 src/main/deploy/choreo/Stage Side To C1.traj
 create mode 100644 src/main/deploy/choreo/Stage Side To C2.traj
 create mode 100644 src/main/deploy/choreo/Stage Side To C3.traj
 create mode 100644 src/main/deploy/choreo/Stage Side To C4.traj
 create mode 100644 src/main/deploy/choreo/Stage Side To C5.traj
 create mode 100644 src/main/deploy/choreo/Stage Side To W1.traj
 create mode 100644 src/main/deploy/choreo/Stage Side To W2.traj
 create mode 100644 src/main/deploy/choreo/Stage Side To W3.traj
 create mode 100644 src/main/deploy/choreo/W1 To C1.traj
 create mode 100644 src/main/deploy/choreo/W1 To C2.traj
 create mode 100644 src/main/deploy/choreo/W1 To C3.traj
 create mode 100644 src/main/deploy/choreo/W1 To C4.traj
 create mode 100644 src/main/deploy/choreo/W1 To C5.traj
 create mode 100644 src/main/deploy/choreo/W1 To S1.traj
 create mode 100644 src/main/deploy/choreo/W1 To S2.traj
 create mode 100644 src/main/deploy/choreo/W1 To S3.traj
 create mode 100644 src/main/deploy/choreo/W1 To S4.traj
 create mode 100644 src/main/deploy/choreo/W1 To W2.traj
 create mode 100644 src/main/deploy/choreo/W1 To W3.traj
 create mode 100644 src/main/deploy/choreo/W2 To C1.traj
 create mode 100644 src/main/deploy/choreo/W2 To C2.traj
 create mode 100644 src/main/deploy/choreo/W2 To C3.traj
 create mode 100644 src/main/deploy/choreo/W2 To C4.traj
 create mode 100644 src/main/deploy/choreo/W2 To C5.traj
 create mode 100644 src/main/deploy/choreo/W2 To S1.traj
 create mode 100644 src/main/deploy/choreo/W2 To S2.traj
 create mode 100644 src/main/deploy/choreo/W2 To S3.traj
 create mode 100644 src/main/deploy/choreo/W2 To S4.traj
 create mode 100644 src/main/deploy/choreo/W2 To W1.traj
 create mode 100644 src/main/deploy/choreo/W2 To W3.traj
 create mode 100644 src/main/deploy/choreo/W3 To C1.traj
 create mode 100644 src/main/deploy/choreo/W3 To C2.traj
 create mode 100644 src/main/deploy/choreo/W3 To C3.traj
 create mode 100644 src/main/deploy/choreo/W3 To C4.traj
 create mode 100644 src/main/deploy/choreo/W3 To C5.traj
 create mode 100644 src/main/deploy/choreo/W3 To S1.traj
 create mode 100644 src/main/deploy/choreo/W3 To S2.traj
 create mode 100644 src/main/deploy/choreo/W3 To S3.traj
 create mode 100644 src/main/deploy/choreo/W3 To S4.traj
 create mode 100644 src/main/deploy/choreo/W3 To W1.traj
 create mode 100644 src/main/deploy/choreo/W3 To W2.traj

diff --git a/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat
index f1a1f3d8b41ebe494304df94fbdd1ab2f5ac29ae..b3edcb2cd6bc93f38fcad6c03cc3924776588efc 100644
GIT binary patch
delta 55
zcmZn=Xb@nz%b?saQ80C)0`J5!FP3`@N(dg?L5962n@lGPTCp8qIB>E7#9G9}1^@t=
B62AZd

delta 55
zcmZn=Xb@nz%%IyaQ80C)0`J5!FP19|ItU)yQHG6Yy9_4^TCp8r*mAZ5#9G9}1^@tY
B62AZd

diff --git a/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat
index 335610449fb6831a3e0db70398ff7f3d00dacbba..97ee8668298d0ec797b05df4ccc61a5d3e34a1cb 100644
GIT binary patch
delta 43
xcmZn=Xb_lifaL;%e#1n;)QJkb6U)5V4l?XL*<?CV(2DH<!-10xAl4!#HUL?54|D(k

delta 43
xcmZn=Xb_lifaMZ{Zo@>u)QJkb6U)5VjxuaK+hsUW(2DH{!<MrhAl4!#HUL?94|D(k

diff --git a/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat
index 952ae7d554ee2f34cfdab03eb9da44ecf50b23d8..5ad69a94f4ee1a2ee350f761cb96336784836181 100644
GIT binary patch
delta 61
zcmZn=Xb@nz%MevPQ80C)0`J5!FP3`@kq92=d4{DYYfKp!1SSfaa~@#Wbg~Z0S;WK!
E0B&g$BLDyZ

delta 61
zcmZn=Xb@nz%#cz&Q80C)0`J5!FP19|$p{|jVTP4wTMQW(1SSfaa~@$>cD4=5S;WK!
E0B#`@BLDyZ

diff --git a/src/main/deploy/choreo/Amp Side To C1.traj b/src/main/deploy/choreo/Amp Side To C1.traj
new file mode 100644
index 00000000..b2886244
--- /dev/null
+++ b/src/main/deploy/choreo/Amp Side To C1.traj	
@@ -0,0 +1,274 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": 1.04,
+            "angularVelocity": 0,
+            "velocityX": -1.8098951472235817e-30,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7613816362208097,
+            "y": 6.725489442192221,
+            "heading": 0.9931061799728784,
+            "angularVelocity": -0.5863053623193526,
+            "velocityX": 0.6424157559264662,
+            "velocityY": 0.06863355110715653,
+            "timestamp": 0.07998190540440472
+        },
+        {
+            "x": 0.8643774617687672,
+            "y": 6.736651681634022,
+            "heading": 0.9029654144342619,
+            "angularVelocity": -1.127014480123304,
+            "velocityX": 1.287739083323781,
+            "velocityY": 0.1395595589447693,
+            "timestamp": 0.15996381080880945
+        },
+        {
+            "x": 1.019343320389449,
+            "y": 6.753669544326843,
+            "heading": 0.7755395878520636,
+            "angularVelocity": -1.5931831823449016,
+            "velocityX": 1.9375114638385158,
+            "velocityY": 0.21277140881771536,
+            "timestamp": 0.23994571621321417
+        },
+        {
+            "x": 1.2269178654194646,
+            "y": 6.7767659053404685,
+            "heading": 0.62538736801047,
+            "angularVelocity": -1.8773273665136312,
+            "velocityX": 2.5952688171215317,
+            "velocityY": 0.2887698273359004,
+            "timestamp": 0.3199276216176189
+        },
+        {
+            "x": 1.4861217663901574,
+            "y": 6.806716511012647,
+            "heading": 0.5166970392894589,
+            "angularVelocity": -1.3589364765874345,
+            "velocityX": 3.240781770077933,
+            "velocityY": 0.37446726882464465,
+            "timestamp": 0.39990952702202365
+        },
+        {
+            "x": 1.7870125383049598,
+            "y": 6.838519066862702,
+            "heading": 0.5166969975980924,
+            "angularVelocity": -5.212599803241117e-7,
+            "velocityX": 3.76198554402321,
+            "velocityY": 0.39762188321538583,
+            "timestamp": 0.4798914324264284
+        },
+        {
+            "x": 2.0879033719171214,
+            "y": 6.870321038982823,
+            "heading": 0.5166969559088411,
+            "angularVelocity": -5.212335340785354e-7,
+            "velocityX": 3.761986315414672,
+            "velocityY": 0.39761458494048757,
+            "timestamp": 0.5598733378308332
+        },
+        {
+            "x": 2.3887942055296523,
+            "y": 6.902123011099449,
+            "heading": 0.5166969142195897,
+            "angularVelocity": -5.212335365150009e-7,
+            "velocityX": 3.7619863154192914,
+            "velocityY": 0.3976145848967806,
+            "timestamp": 0.6398552432352379
+        },
+        {
+            "x": 2.689685039142183,
+            "y": 6.933924983216074,
+            "heading": 0.516696872530338,
+            "angularVelocity": -5.212335398189914e-7,
+            "velocityX": 3.7619863154192914,
+            "velocityY": 0.3976145848967804,
+            "timestamp": 0.7198371486396427
+        },
+        {
+            "x": 2.990575872754714,
+            "y": 6.9657269553327,
+            "heading": 0.5166968308410861,
+            "angularVelocity": -5.212335433606452e-7,
+            "velocityX": 3.7619863154192914,
+            "velocityY": 0.39761458489678053,
+            "timestamp": 0.7998190540440474
+        },
+        {
+            "x": 3.291466706367245,
+            "y": 6.997528927449325,
+            "heading": 0.5166967891518339,
+            "angularVelocity": -5.212335460884143e-7,
+            "velocityX": 3.761986315419291,
+            "velocityY": 0.3976145848967808,
+            "timestamp": 0.8798009594484522
+        },
+        {
+            "x": 3.5923575399797762,
+            "y": 7.0293308995659505,
+            "heading": 0.5166967474625817,
+            "angularVelocity": -5.212335479348436e-7,
+            "velocityX": 3.761986315419291,
+            "velocityY": 0.3976145848967805,
+            "timestamp": 0.9597828648528569
+        },
+        {
+            "x": 3.893248373592307,
+            "y": 7.0611328716825765,
+            "heading": 0.516696705773329,
+            "angularVelocity": -5.212335516712931e-7,
+            "velocityX": 3.7619863154192905,
+            "velocityY": 0.3976145848967809,
+            "timestamp": 1.0397647702572617
+        },
+        {
+            "x": 4.194139207204837,
+            "y": 7.092934843799203,
+            "heading": 0.5166966640840763,
+            "angularVelocity": -5.212335535473894e-7,
+            "velocityX": 3.7619863154192905,
+            "velocityY": 0.3976145848967809,
+            "timestamp": 1.1197466756616663
+        },
+        {
+            "x": 4.495030040817368,
+            "y": 7.1247368159158295,
+            "heading": 0.5166966223948233,
+            "angularVelocity": -5.212335568545237e-7,
+            "velocityX": 3.7619863154192905,
+            "velocityY": 0.3976145848967808,
+            "timestamp": 1.199728581066071
+        },
+        {
+            "x": 4.7959208744299,
+            "y": 7.156538788032456,
+            "heading": 0.51669658070557,
+            "angularVelocity": -5.212335599678648e-7,
+            "velocityX": 3.7619863154192905,
+            "velocityY": 0.397614584896781,
+            "timestamp": 1.2797104864704756
+        },
+        {
+            "x": 5.096811708042431,
+            "y": 7.1883407601490825,
+            "heading": 0.5166965390163165,
+            "angularVelocity": -5.212335628762146e-7,
+            "velocityX": 3.7619863154192905,
+            "velocityY": 0.3976145848967809,
+            "timestamp": 1.3596923918748802
+        },
+        {
+            "x": 5.3977025416549615,
+            "y": 7.220142732265709,
+            "heading": 0.5166964973270628,
+            "angularVelocity": -5.212335655841188e-7,
+            "velocityX": 3.76198631541929,
+            "velocityY": 0.3976145848967813,
+            "timestamp": 1.4396742972792849
+        },
+        {
+            "x": 5.698593375267492,
+            "y": 7.251944704382335,
+            "heading": 0.5166964556378089,
+            "angularVelocity": -5.212335680260379e-7,
+            "velocityX": 3.7619863154192896,
+            "velocityY": 0.3976145848967809,
+            "timestamp": 1.5196562026836895
+        },
+        {
+            "x": 5.999484208880023,
+            "y": 7.283746676498962,
+            "heading": 0.5166964139485547,
+            "angularVelocity": -5.212335712969897e-7,
+            "velocityX": 3.7619863154192896,
+            "velocityY": 0.3976145848967812,
+            "timestamp": 1.5996381080880941
+        },
+        {
+            "x": 6.300375042492554,
+            "y": 7.315548648615588,
+            "heading": 0.5166963722593003,
+            "angularVelocity": -5.21233573548864e-7,
+            "velocityX": 3.7619863154192896,
+            "velocityY": 0.39761458489678125,
+            "timestamp": 1.6796200134924988
+        },
+        {
+            "x": 6.6012658761048035,
+            "y": 7.347350620734872,
+            "heading": 0.5166963305700458,
+            "angularVelocity": -5.212335762453642e-7,
+            "velocityX": 3.7619863154157773,
+            "velocityY": 0.3976145849300086,
+            "timestamp": 1.7596019188969034
+        },
+        {
+            "x": 6.902156662813089,
+            "y": 7.3791530366221005,
+            "heading": 0.5166962888792155,
+            "angularVelocity": -5.212532766918364e-7,
+            "velocityX": 3.761985728983584,
+            "velocityY": 0.3976201332842598,
+            "timestamp": 1.839583824301308
+        },
+        {
+            "x": 7.162091022778527,
+            "y": 7.409610892063227,
+            "heading": 0.4051307797997224,
+            "angularVelocity": -1.3948843618490365,
+            "velocityX": 3.249914573191993,
+            "velocityY": 0.3808093253983604,
+            "timestamp": 1.9195657297057127
+        },
+        {
+            "x": 7.370036606479622,
+            "y": 7.432974685540162,
+            "heading": 0.25885269617169443,
+            "angularVelocity": -1.8288897080960538,
+            "velocityX": 2.5999078497777726,
+            "velocityY": 0.2921134894049214,
+            "timestamp": 1.9995476351101173
+        },
+        {
+            "x": 7.525326975450784,
+            "y": 7.45013556763307,
+            "heading": 0.1335302309738367,
+            "angularVelocity": -1.5668852169025227,
+            "velocityX": 1.9415687609088876,
+            "velocityY": 0.21455955576625652,
+            "timestamp": 2.079529540514522
+        },
+        {
+            "x": 7.628525640862077,
+            "y": 7.461415071089343,
+            "heading": 0.043910704401711645,
+            "angularVelocity": -1.1204975190199653,
+            "velocityX": 1.2902751552304177,
+            "velocityY": 0.14102569073894916,
+            "timestamp": 2.159511445918927
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -0.003,
+            "angularVelocity": -0.5865164647493907,
+            "velocityX": 0.6435750546033896,
+            "velocityY": 0.0698274051164111,
+            "timestamp": 2.2394933513233317
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -0.003,
+            "angularVelocity": 0,
+            "velocityX": 4.312091780947568e-36,
+            "velocityY": 0,
+            "timestamp": 2.3194752567277366
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Amp Side To C2.traj b/src/main/deploy/choreo/Amp Side To C2.traj
new file mode 100644
index 00000000..8c7890ab
--- /dev/null
+++ b/src/main/deploy/choreo/Amp Side To C2.traj	
@@ -0,0 +1,319 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": 1.04,
+            "angularVelocity": 0,
+            "velocityX": 2.674890564447798e-33,
+            "velocityY": 3.559444452002106e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 0.744719766454751,
+            "y": 6.720759968199705,
+            "heading": 1.006760857121406,
+            "angularVelocity": -0.5061419234074507,
+            "velocityX": 0.5286878015432446,
+            "velocityY": 0.011572252862695013,
+            "timestamp": 0.06567158605400746
+        },
+        {
+            "x": 0.8143576636925598,
+            "y": 6.722372451876338,
+            "heading": 0.9431499891720047,
+            "angularVelocity": -0.9686208567749168,
+            "velocityX": 1.0603961533765867,
+            "velocityY": 0.024553749551892837,
+            "timestamp": 0.1313431721080149
+        },
+        {
+            "x": 0.9191612228651693,
+            "y": 6.724923861576486,
+            "heading": 0.8530685118915011,
+            "angularVelocity": -1.3716963864162066,
+            "velocityX": 1.5958737327650472,
+            "velocityY": 0.03885104431723037,
+            "timestamp": 0.19701475816202235
+        },
+        {
+            "x": 1.059454644842809,
+            "y": 6.7284663342548425,
+            "heading": 0.7426650921374723,
+            "angularVelocity": -1.6811444094442096,
+            "velocityX": 2.1362880114128,
+            "velocityY": 0.05394224338464699,
+            "timestamp": 0.2626863442160298
+        },
+        {
+            "x": 1.2356244745863587,
+            "y": 6.733039509062333,
+            "heading": 0.6243118436370656,
+            "angularVelocity": -1.8021987226420155,
+            "velocityX": 2.682588320596835,
+            "velocityY": 0.06963703912619325,
+            "timestamp": 0.3283579302700373
+        },
+        {
+            "x": 1.4470569399809903,
+            "y": 6.738777367450543,
+            "heading": 0.5308220213971302,
+            "angularVelocity": -1.4235962287107038,
+            "velocityX": 3.219542546463745,
+            "velocityY": 0.08737200870294055,
+            "timestamp": 0.39402951632404476
+        },
+        {
+            "x": 1.688226065456379,
+            "y": 6.748371696739962,
+            "heading": 0.5126606987274956,
+            "angularVelocity": -0.2765476480908373,
+            "velocityX": 3.6723511638207493,
+            "velocityY": 0.14609559272058092,
+            "timestamp": 0.45970110237805223
+        },
+        {
+            "x": 1.9365780121804386,
+            "y": 6.754665679712547,
+            "heading": 0.5126606479364623,
+            "angularVelocity": -7.734095747362075e-7,
+            "velocityX": 3.7817260347544686,
+            "velocityY": 0.09584027660688858,
+            "timestamp": 0.5253726884320596
+        },
+        {
+            "x": 2.1849299738494317,
+            "y": 6.760959072962077,
+            "heading": 0.5126605971462416,
+            "angularVelocity": -7.733971982996713e-7,
+            "velocityX": 3.7817262623252414,
+            "velocityY": 0.09583129672545204,
+            "timestamp": 0.5910442744860671
+        },
+        {
+            "x": 2.433281935680659,
+            "y": 6.767252459809602,
+            "heading": 0.5126605463560266,
+            "angularVelocity": -7.733971099299294e-7,
+            "velocityX": 3.7817262647956325,
+            "velocityY": 0.09583119924030911,
+            "timestamp": 0.6567158605400746
+        },
+        {
+            "x": 2.6816338977021794,
+            "y": 6.773545839147862,
+            "heading": 0.5126604955658177,
+            "angularVelocity": -7.733970196193196e-7,
+            "velocityX": 3.78172626769327,
+            "velocityY": 0.09583108489457716,
+            "timestamp": 0.722387446594082
+        },
+        {
+            "x": 2.9299858599438156,
+            "y": 6.779839209799928,
+            "heading": 0.5126604447756148,
+            "angularVelocity": -7.733969286509421e-7,
+            "velocityX": 3.7817262710450463,
+            "velocityY": 0.09583095262742326,
+            "timestamp": 0.7880590326480895
+        },
+        {
+            "x": 3.1783378224424883,
+            "y": 6.786132570308856,
+            "heading": 0.5126603939854177,
+            "angularVelocity": -7.733968385113031e-7,
+            "velocityX": 3.781726274959007,
+            "velocityY": 0.09583079817497217,
+            "timestamp": 0.853730618702097
+        },
+        {
+            "x": 3.426689785244194,
+            "y": 6.792425918859452,
+            "heading": 0.5126603431952265,
+            "angularVelocity": -7.733967515033378e-7,
+            "velocityX": 3.7817262795733857,
+            "velocityY": 0.09583061608201786,
+            "timestamp": 0.9194022047561045
+        },
+        {
+            "x": 3.6750417484067377,
+            "y": 6.798719253170605,
+            "heading": 0.5126602924050404,
+            "angularVelocity": -7.733966712943809e-7,
+            "velocityX": 3.781726285067957,
+            "velocityY": 0.09583039925395942,
+            "timestamp": 0.9850737908101119
+        },
+        {
+            "x": 3.9233937120031053,
+            "y": 6.805012570362013,
+            "heading": 0.5126602416148589,
+            "angularVelocity": -7.733966025007139e-7,
+            "velocityX": 3.7817262916739214,
+            "velocityY": 0.095830138566711,
+            "timestamp": 1.0507453768641193
+        },
+        {
+            "x": 4.171745676125189,
+            "y": 6.811305866807266,
+            "heading": 0.5126601908246804,
+            "angularVelocity": -7.733965561950186e-7,
+            "velocityX": 3.7817262996791623,
+            "velocityY": 0.09582982265909266,
+            "timestamp": 1.1164169629181266
+        },
+        {
+            "x": 4.420097640886595,
+            "y": 6.817599138023107,
+            "heading": 0.5126601400345029,
+            "angularVelocity": -7.733965406200526e-7,
+            "velocityX": 3.781726309414286,
+            "velocityY": 0.09582943848362173,
+            "timestamp": 1.182088548972134
+        },
+        {
+            "x": 4.6684496064150185,
+            "y": 6.8238923789700765,
+            "heading": 0.5126600892443262,
+            "angularVelocity": -7.73396529455525e-7,
+            "velocityX": 3.7817263210938923,
+            "velocityY": 0.09582897757087808,
+            "timestamp": 1.2477601350261414
+        },
+        {
+            "x": 4.916801572841165,
+            "y": 6.830185584489968,
+            "heading": 0.5126600384541553,
+            "angularVelocity": -7.733964406232834e-7,
+            "velocityX": 3.7817263347637717,
+            "velocityY": 0.09582843811261686,
+            "timestamp": 1.3134317210801487
+        },
+        {
+            "x": 5.165153539254683,
+            "y": 6.8364787905062485,
+            "heading": 0.5126599876639959,
+            "angularVelocity": -7.733962639482034e-7,
+            "velocityX": 3.781726334571479,
+            "velocityY": 0.09582844567126618,
+            "timestamp": 1.379103307134156
+        },
+        {
+            "x": 5.413504500375102,
+            "y": 6.842811545538402,
+            "heading": 0.5126599368726894,
+            "angularVelocity": -7.734137320837285e-7,
+            "velocityX": 3.781711026686433,
+            "velocityY": 0.09643066982787928,
+            "timestamp": 1.4447748931881634
+        },
+        {
+            "x": 5.661808131063952,
+            "y": 6.834835924305975,
+            "heading": 0.5126598860199024,
+            "angularVelocity": -7.743499133059843e-7,
+            "velocityX": 3.780990312684674,
+            "velocityY": -0.12144706275568991,
+            "timestamp": 1.5104464792421708
+        },
+        {
+            "x": 5.9063591957092285,
+            "y": 6.791097164154053,
+            "heading": 0.5126598314406843,
+            "angularVelocity": -8.310933457001734e-7,
+            "velocityX": 3.723848917614642,
+            "velocityY": -0.6660225948568639,
+            "timestamp": 1.5761180652961782
+        },
+        {
+            "x": 6.194622556481831,
+            "y": 6.683762426219015,
+            "heading": 0.5126597887806125,
+            "angularVelocity": -5.246475178274693e-7,
+            "velocityX": 3.5451571072929595,
+            "velocityY": -1.3200377183874616,
+            "timestamp": 1.65742993725234
+        },
+        {
+            "x": 6.459547388261047,
+            "y": 6.527457130068993,
+            "heading": 0.5126597482300577,
+            "angularVelocity": -4.987039864896594e-7,
+            "velocityX": 3.2581322432449182,
+            "velocityY": -1.9222936625379825,
+            "timestamp": 1.7387418092085019
+        },
+        {
+            "x": 6.723521508223802,
+            "y": 6.3695515377198895,
+            "heading": 0.5126597076877473,
+            "angularVelocity": -4.98602596336278e-7,
+            "velocityX": 3.2464400783305223,
+            "velocityY": -1.9419746286809643,
+            "timestamp": 1.8200536811646637
+        },
+        {
+            "x": 6.987495332330779,
+            "y": 6.211645450805702,
+            "heading": 0.5126596671480953,
+            "angularVelocity": -4.985699012832353e-7,
+            "velocityX": 3.246436439802837,
+            "velocityY": -1.9419807109984206,
+            "timestamp": 1.9013655531208256
+        },
+        {
+            "x": 7.218138260463332,
+            "y": 6.072816355413413,
+            "heading": 0.4174347514218684,
+            "angularVelocity": -1.1711071635093024,
+            "velocityX": 2.8365221779297074,
+            "velocityY": -1.7073656288101424,
+            "timestamp": 1.9826774250769874
+        },
+        {
+            "x": 7.40359391202109,
+            "y": 5.961763553999033,
+            "heading": 0.26911254592387807,
+            "angularVelocity": -1.8241150022712385,
+            "velocityX": 2.2807942689821186,
+            "velocityY": -1.365763679308134,
+            "timestamp": 2.063989297033149
+        },
+        {
+            "x": 7.542069024042313,
+            "y": 5.879081457057784,
+            "heading": 0.13934312775190755,
+            "angularVelocity": -1.595946754760921,
+            "velocityX": 1.7030122255201257,
+            "velocityY": -1.0168514750927211,
+            "timestamp": 2.145301168989311
+        },
+        {
+            "x": 7.63409233729861,
+            "y": 5.824280880315082,
+            "heading": 0.04592275308495019,
+            "angularVelocity": -1.1489143272623823,
+            "velocityX": 1.1317328090287153,
+            "velocityY": -0.6739554191077514,
+            "timestamp": 2.2266130409454727
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -0.003,
+            "angularVelocity": -0.6016680210157614,
+            "velocityX": 0.5645874531846384,
+            "velocityY": -0.3355091902172136,
+            "timestamp": 2.3079249129016346
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -0.003,
+            "angularVelocity": 3.440382093642737e-30,
+            "velocityX": 0,
+            "velocityY": 2.1122019835026996e-31,
+            "timestamp": 2.3892367848577964
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Amp Side To C3.traj b/src/main/deploy/choreo/Amp Side To C3.traj
new file mode 100644
index 00000000..463b42ef
--- /dev/null
+++ b/src/main/deploy/choreo/Amp Side To C3.traj	
@@ -0,0 +1,526 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": 1.04,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 1.3928609100313025e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 0.729279758485921,
+            "y": 6.711628327586588,
+            "heading": 1.093428142295198,
+            "angularVelocity": 0.9770378814200095,
+            "velocityX": 0.352568020824986,
+            "velocityY": -0.15309237280887605,
+            "timestamp": 0.05468379815278649
+        },
+        {
+            "x": 0.7686493967347447,
+            "y": 6.69501140584797,
+            "heading": 1.1949690599508662,
+            "angularVelocity": 1.8568739020644303,
+            "velocityX": 0.7199506906748694,
+            "velocityY": -0.30387285265355396,
+            "timestamp": 0.10936759630557298
+        },
+        {
+            "x": 0.8290790315170126,
+            "y": 6.670059182269693,
+            "heading": 1.3362954219692313,
+            "angularVelocity": 2.584428419245852,
+            "velocityX": 1.1050738394839994,
+            "velocityY": -0.4563001185206737,
+            "timestamp": 0.16405139445835948
+        },
+        {
+            "x": 0.9115429372168562,
+            "y": 6.636285478537955,
+            "heading": 1.5055888656223098,
+            "angularVelocity": 3.0958611027725738,
+            "velocityX": 1.508013497333151,
+            "velocityY": -0.6176181039468854,
+            "timestamp": 0.21873519261114596
+        },
+        {
+            "x": 1.0168329273736771,
+            "y": 6.593191021214006,
+            "heading": 1.6881423696003794,
+            "angularVelocity": 3.3383471913932383,
+            "velocityX": 1.9254330114861617,
+            "velocityY": -0.7880662788554578,
+            "timestamp": 0.27341899076393245
+        },
+        {
+            "x": 1.1449422871140338,
+            "y": 6.540878714080721,
+            "heading": 1.8564615478628756,
+            "angularVelocity": 3.078044758197163,
+            "velocityX": 2.3427297310698694,
+            "velocityY": -0.9566326572109004,
+            "timestamp": 0.32810278891671896
+        },
+        {
+            "x": 1.2912409780613738,
+            "y": 6.479735868066328,
+            "heading": 1.974688906048384,
+            "angularVelocity": 2.1620180415263293,
+            "velocityX": 2.675357160425865,
+            "velocityY": -1.1181162991560991,
+            "timestamp": 0.38278658706950547
+        },
+        {
+            "x": 1.4553681499836075,
+            "y": 6.408144093279595,
+            "heading": 2.0430197361851268,
+            "angularVelocity": 1.2495626208301445,
+            "velocityX": 3.001385738855639,
+            "velocityY": -1.3091953595963746,
+            "timestamp": 0.437470385222292
+        },
+        {
+            "x": 1.6374055328463037,
+            "y": 6.326368400043195,
+            "heading": 2.0610350683831213,
+            "angularVelocity": 0.3294455178051113,
+            "velocityX": 3.328908909254691,
+            "velocityY": -1.4954281889476506,
+            "timestamp": 0.4921541833750785
+        },
+        {
+            "x": 1.8283093127689483,
+            "y": 6.246687216839916,
+            "heading": 2.061035169590908,
+            "angularVelocity": 0.000001850781943801841,
+            "velocityX": 3.4910482880003957,
+            "velocityY": -1.45712598420193,
+            "timestamp": 0.546837981527865
+        },
+        {
+            "x": 2.01921313853121,
+            "y": 6.167006143460992,
+            "heading": 2.0610352707983473,
+            "angularVelocity": 0.0000018507755948234292,
+            "velocityX": 3.491049126267287,
+            "velocityY": -1.4571239758492094,
+            "timestamp": 0.6015217796806515
+        },
+        {
+            "x": 2.210116959480156,
+            "y": 6.087325058550032,
+            "heading": 2.061035372005735,
+            "angularVelocity": 0.000001850774661522582,
+            "velocityX": 3.4910490382464,
+            "velocityY": -1.4571241867349893,
+            "timestamp": 0.6562055778334379
+        },
+        {
+            "x": 2.40102077504018,
+            "y": 6.007643960727976,
+            "heading": 2.0610354732130722,
+            "angularVelocity": 0.0000018507737318277825,
+            "velocityX": 3.49104893969945,
+            "velocityY": -1.4571244228395928,
+            "timestamp": 0.7108893759862244
+        },
+        {
+            "x": 2.5919245845305627,
+            "y": 5.927962848363926,
+            "heading": 2.0610355744203592,
+            "angularVelocity": 0.0000018507728139380304,
+            "velocityX": 3.491048828704212,
+            "velocityY": -1.457124688768356,
+            "timestamp": 0.7655731741390108
+        },
+        {
+            "x": 2.78282838714305,
+            "y": 5.848281719521431,
+            "heading": 2.0610356756275974,
+            "angularVelocity": 0.0000018507719179726602,
+            "velocityX": 3.4910487029284663,
+            "velocityY": -1.457124990108895,
+            "timestamp": 0.8202569722917973
+        },
+        {
+            "x": 2.973732181912291,
+            "y": 5.768600571887663,
+            "heading": 2.061035776834788,
+            "angularVelocity": 0.0000018507710613284672,
+            "velocityX": 3.491048559499401,
+            "velocityY": -1.4571253337439838,
+            "timestamp": 0.8749407704445837
+        },
+        {
+            "x": 3.16463596766188,
+            "y": 5.688919402644143,
+            "heading": 2.061035878041936,
+            "angularVelocity": 0.000001850770272196717,
+            "velocityX": 3.4910483945574473,
+            "velocityY": -1.4571257289205004,
+            "timestamp": 0.9296245685973702
+        },
+        {
+            "x": 3.3555397425941185,
+            "y": 5.609238207483871,
+            "heading": 2.061035979249042,
+            "angularVelocity": 0.000001850769506382635,
+            "velocityX": 3.4910481967410822,
+            "velocityY": -1.457126202858873,
+            "timestamp": 0.9843083667501566
+        },
+        {
+            "x": 3.546443239175115,
+            "y": 5.529556345440524,
+            "heading": 2.0610360804562036,
+            "angularVelocity": 0.0000018507705174022697,
+            "velocityX": 3.491043106545231,
+            "velocityY": -1.4571383981177848,
+            "timestamp": 1.0389921649029432
+        },
+        {
+            "x": 3.7301891812097523,
+            "y": 5.434525969364101,
+            "heading": 2.0610361860102304,
+            "angularVelocity": 0.0000019302614304565207,
+            "velocityX": 3.360153249071168,
+            "velocityY": -1.7378159397580357,
+            "timestamp": 1.0936759630557298
+        },
+        {
+            "x": 3.9011674048607876,
+            "y": 5.318079465137689,
+            "heading": 2.0610363082632275,
+            "angularVelocity": 0.0000022356347107367405,
+            "velocityX": 3.126670593972313,
+            "velocityY": -2.129451650396711,
+            "timestamp": 1.1483597612085163
+        },
+        {
+            "x": 4.05689386248419,
+            "y": 5.181908226701039,
+            "heading": 2.0610364709885136,
+            "angularVelocity": 0.0000029757495174652245,
+            "velocityX": 2.8477622784778513,
+            "velocityY": -2.490156920998575,
+            "timestamp": 1.203043559361303
+        },
+        {
+            "x": 4.195106185477333,
+            "y": 5.027990436285506,
+            "heading": 2.061036659879246,
+            "angularVelocity": 0.0000034542357869786317,
+            "velocityX": 2.52748213660978,
+            "velocityY": -2.814687267798894,
+            "timestamp": 1.2577273575140895
+        },
+        {
+            "x": 4.313711649853664,
+            "y": 4.858669951905228,
+            "heading": 2.0606108116890636,
+            "angularVelocity": -0.007787465475470693,
+            "velocityX": 2.168932451344132,
+            "velocityY": -3.096355595256869,
+            "timestamp": 1.312411155666876
+        },
+        {
+            "x": 4.40996698448611,
+            "y": 4.67856050613378,
+            "heading": 2.051588761211157,
+            "angularVelocity": -0.16498580535132465,
+            "velocityX": 1.7602166982532683,
+            "velocityY": -3.293652815925874,
+            "timestamp": 1.3670949538196626
+        },
+        {
+            "x": 4.510742037442817,
+            "y": 4.51918682060045,
+            "heading": 1.996778499947209,
+            "angularVelocity": -1.002312624862085,
+            "velocityX": 1.8428685709639585,
+            "velocityY": -2.9144589607334965,
+            "timestamp": 1.4217787519724492
+        },
+        {
+            "x": 4.62612450472694,
+            "y": 4.379617585881877,
+            "heading": 1.9276644376664414,
+            "angularVelocity": -1.263885549567404,
+            "velocityX": 2.1099936577511538,
+            "velocityY": -2.5522959163995464,
+            "timestamp": 1.4764625501252358
+        },
+        {
+            "x": 4.756154634809633,
+            "y": 4.26013511834241,
+            "heading": 1.8517335763640235,
+            "angularVelocity": -1.3885440270675238,
+            "velocityX": 2.3778547664042544,
+            "velocityY": -2.1849701662205963,
+            "timestamp": 1.5311463482780223
+        },
+        {
+            "x": 4.900439739227295,
+            "y": 4.161101341247559,
+            "heading": 1.7737408289368037,
+            "angularVelocity": -1.4262496399629772,
+            "velocityX": 2.638534799915143,
+            "velocityY": -1.8110259426046127,
+            "timestamp": 1.5858301464308089
+        },
+        {
+            "x": 5.008276114908647,
+            "y": 4.1019129343639005,
+            "heading": 1.719841693932476,
+            "angularVelocity": -1.4074809230288232,
+            "velocityX": 2.815956908545699,
+            "velocityY": -1.5456009367595303,
+            "timestamp": 1.6241248999315097
+        },
+        {
+            "x": 5.122676890234505,
+            "y": 4.053024888243358,
+            "heading": 1.6679900254477509,
+            "angularVelocity": -1.354014943163857,
+            "velocityX": 2.987374636678728,
+            "velocityY": -1.276625167978901,
+            "timestamp": 1.6624196534322104
+        },
+        {
+            "x": 5.243382451295884,
+            "y": 4.014566134854959,
+            "heading": 1.6197010617895011,
+            "angularVelocity": -1.260981185252083,
+            "velocityX": 3.1520130051018556,
+            "velocityY": -1.0042825680467018,
+            "timestamp": 1.7007144069329112
+        },
+        {
+            "x": 5.370099790784461,
+            "y": 3.9866453379407707,
+            "heading": 1.576858310766692,
+            "angularVelocity": -1.1187629402556432,
+            "velocityX": 3.3090000040412137,
+            "velocityY": -0.7291024059908604,
+            "timestamp": 1.739009160433612
+        },
+        {
+            "x": 5.502536097274536,
+            "y": 3.9692960416426737,
+            "heading": 1.5420636410872666,
+            "angularVelocity": -0.9086014792806747,
+            "velocityX": 3.4583407486263615,
+            "velocityY": -0.45304629778540073,
+            "timestamp": 1.7773039139343128
+        },
+        {
+            "x": 5.640613600815518,
+            "y": 3.9622331858505495,
+            "heading": 1.5196895147171037,
+            "angularVelocity": -0.5842608797508935,
+            "velocityX": 3.6056506680074274,
+            "velocityY": -0.18443403198808295,
+            "timestamp": 1.8155986674350135
+        },
+        {
+            "x": 5.785301546169388,
+            "y": 3.963377277975263,
+            "heading": 1.519119424890785,
+            "angularVelocity": -0.014886891132695255,
+            "velocityX": 3.778270706226711,
+            "velocityY": 0.029875949578638192,
+            "timestamp": 1.8538934209357143
+        },
+        {
+            "x": 5.929550760809323,
+            "y": 3.9767385598159253,
+            "heading": 1.5191191822149777,
+            "angularVelocity": -0.000006337051042848807,
+            "velocityX": 3.7668140268168893,
+            "velocityY": 0.3489063283934661,
+            "timestamp": 1.892188174436415
+        },
+        {
+            "x": 6.073462686179593,
+            "y": 3.9933433315789473,
+            "heading": 1.5191189525644635,
+            "angularVelocity": -0.0000059969184602345635,
+            "velocityX": 3.7580063119517533,
+            "velocityY": 0.4336043516435729,
+            "timestamp": 1.9304829279371158
+        },
+        {
+            "x": 6.217374593429904,
+            "y": 4.009948260386706,
+            "heading": 1.519118722914699,
+            "angularVelocity": -0.000005996898884562842,
+            "velocityX": 3.7580058387809343,
+            "velocityY": 0.43360845259010783,
+            "timestamp": 1.9687776814378166
+        },
+        {
+            "x": 6.361286503020876,
+            "y": 4.026553168908082,
+            "heading": 1.5191184932648618,
+            "angularVelocity": -0.000005996900776462565,
+            "velocityX": 3.7580058999032113,
+            "velocityY": 0.4336079228469906,
+            "timestamp": 2.0070724349385176
+        },
+        {
+            "x": 6.5051984133944005,
+            "y": 4.04315807064667,
+            "heading": 1.5191182636148604,
+            "angularVelocity": -0.000005996905070522029,
+            "velocityX": 3.7580059203381437,
+            "velocityY": 0.4336077457264339,
+            "timestamp": 2.0453671884392186
+        },
+        {
+            "x": 6.6491104352583505,
+            "y": 4.059762006067292,
+            "heading": 1.5191180339567425,
+            "angularVelocity": -0.000005997117020238007,
+            "velocityX": 3.7580088317141747,
+            "velocityY": 0.4335825120357026,
+            "timestamp": 2.0836619419399196
+        },
+        {
+            "x": 6.790280334982046,
+            "y": 4.068588148423426,
+            "heading": 1.5085875786821552,
+            "angularVelocity": -0.27498428144717973,
+            "velocityX": 3.6864031445224414,
+            "velocityY": 0.23047915312922457,
+            "timestamp": 2.1219566954406206
+        },
+        {
+            "x": 6.921716704437578,
+            "y": 4.075833761978748,
+            "heading": 1.4694677109965053,
+            "angularVelocity": -1.0215464028234083,
+            "velocityX": 3.43222915517987,
+            "velocityY": 0.18920642889606504,
+            "timestamp": 2.1602514489413216
+        },
+        {
+            "x": 7.043039125371844,
+            "y": 4.08211395301362,
+            "heading": 1.4037305732475935,
+            "angularVelocity": -1.716609502335841,
+            "velocityX": 3.1681212135768497,
+            "velocityY": 0.16399612115943957,
+            "timestamp": 2.1985462024420226
+        },
+        {
+            "x": 7.153946697571625,
+            "y": 4.087967374487544,
+            "heading": 1.3134036899027293,
+            "angularVelocity": -2.3587273735346383,
+            "velocityX": 2.896155793188521,
+            "velocityY": 0.1528517861805002,
+            "timestamp": 2.2368409559427236
+        },
+        {
+            "x": 7.254278338500056,
+            "y": 4.0938696715910385,
+            "heading": 1.199938125135736,
+            "angularVelocity": -2.9629532610757465,
+            "velocityX": 2.619983986229206,
+            "velocityY": 0.1541280871121784,
+            "timestamp": 2.2751357094434246
+        },
+        {
+            "x": 7.34404947628579,
+            "y": 4.100268813517142,
+            "heading": 1.0638507127340207,
+            "angularVelocity": -3.5536829450860603,
+            "velocityX": 2.344215057660344,
+            "velocityY": 0.16710231405423262,
+            "timestamp": 2.3134304629441256
+        },
+        {
+            "x": 7.4235066081317465,
+            "y": 4.107648882181887,
+            "heading": 0.9045749783818139,
+            "angularVelocity": -4.159205107542786,
+            "velocityX": 2.0748829691383666,
+            "velocityY": 0.192717487125464,
+            "timestamp": 2.3517252164448266
+        },
+        {
+            "x": 7.492760083799764,
+            "y": 4.113297423743801,
+            "heading": 0.7219171880631509,
+            "angularVelocity": -4.769786292404788,
+            "velocityX": 1.808432470175071,
+            "velocityY": 0.1475017083426549,
+            "timestamp": 2.3900199699455276
+        },
+        {
+            "x": 7.549820937161571,
+            "y": 4.1180620764150975,
+            "heading": 0.5402485968001275,
+            "angularVelocity": -4.7439551023536115,
+            "velocityX": 1.4900436259698675,
+            "velocityY": 0.1244205076606485,
+            "timestamp": 2.4283147234462286
+        },
+        {
+            "x": 7.595294548959434,
+            "y": 4.121747510359626,
+            "heading": 0.3723680212509228,
+            "angularVelocity": -4.383905370904984,
+            "velocityX": 1.1874632329734431,
+            "velocityY": 0.09623861254154589,
+            "timestamp": 2.4666094769469296
+        },
+        {
+            "x": 7.630234770559726,
+            "y": 4.124236647711556,
+            "heading": 0.22836852122657442,
+            "angularVelocity": -3.7602931697083015,
+            "velocityX": 0.912402311184801,
+            "velocityY": 0.06499943528517171,
+            "timestamp": 2.5049042304476306
+        },
+        {
+            "x": 7.655576852148959,
+            "y": 4.125750923706981,
+            "heading": 0.11519495737029227,
+            "angularVelocity": -2.9553281718920323,
+            "velocityX": 0.6617637998053468,
+            "velocityY": 0.03954264897925492,
+            "timestamp": 2.5431989839483315
+        },
+        {
+            "x": 7.671995118301485,
+            "y": 4.1266005721732935,
+            "heading": 0.03711275718074835,
+            "angularVelocity": -2.0389790520029063,
+            "velocityX": 0.4287340863083462,
+            "velocityY": 0.02218707234390751,
+            "timestamp": 2.5814937374490325
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -0.003,
+            "angularVelocity": -1.047473962197309,
+            "velocityX": 0.20903337838088354,
+            "velocityY": 0.010430353774151994,
+            "timestamp": 2.6197884909497335
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -0.003,
+            "angularVelocity": 7.250240493925158e-33,
+            "velocityX": 3.887711813813661e-34,
+            "velocityY": -4.900942603355191e-34,
+            "timestamp": 2.6580832444504345
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Amp Side To C4.traj b/src/main/deploy/choreo/Amp Side To C4.traj
new file mode 100644
index 00000000..ca6ee59b
--- /dev/null
+++ b/src/main/deploy/choreo/Amp Side To C4.traj	
@@ -0,0 +1,400 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": 1.04,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7271139423991175,
+            "y": 6.709732017673653,
+            "heading": 0.9305052411067223,
+            "angularVelocity": -1.767895408627961,
+            "velocityX": 0.2763206248110281,
+            "velocityY": -0.16578618916650492,
+            "timestamp": 0.06193508867035021
+        },
+        {
+            "x": 0.7615532933104725,
+            "y": 6.686914726598375,
+            "heading": 0.7206788937202419,
+            "angularVelocity": -3.387842851138587,
+            "velocityX": 0.5560555680263672,
+            "velocityY": -0.36840652956392367,
+            "timestamp": 0.12387017734070042
+        },
+        {
+            "x": 0.8202948153873781,
+            "y": 6.650989300229197,
+            "heading": 0.4540271404715134,
+            "angularVelocity": -4.305342237709284,
+            "velocityX": 0.9484368770271357,
+            "velocityY": -0.5800496477915925,
+            "timestamp": 0.18580526601105063
+        },
+        {
+            "x": 0.9068549514543394,
+            "y": 6.604915057765644,
+            "heading": 0.1437481054753499,
+            "angularVelocity": -5.009745552276917,
+            "velocityX": 1.3975944480789861,
+            "velocityY": -0.7439117865606645,
+            "timestamp": 0.24774035468140085
+        },
+        {
+            "x": 1.022793030858839,
+            "y": 6.550181878656492,
+            "heading": -0.13251218552677876,
+            "angularVelocity": -4.460481076769346,
+            "velocityX": 1.871928851536495,
+            "velocityY": -0.8837184265686551,
+            "timestamp": 0.30967544335175107
+        },
+        {
+            "x": 1.1650239086947571,
+            "y": 6.483494014593417,
+            "heading": -0.35594040398018906,
+            "angularVelocity": -3.6074577957352734,
+            "velocityX": 2.296450701684509,
+            "velocityY": -1.0767380090149254,
+            "timestamp": 0.37161053202210126
+        },
+        {
+            "x": 1.3271753983931303,
+            "y": 6.398307258999271,
+            "heading": -0.5136957877872723,
+            "angularVelocity": -2.5471083870846947,
+            "velocityX": 2.61808763302859,
+            "velocityY": -1.3754199343695408,
+            "timestamp": 0.43354562069245145
+        },
+        {
+            "x": 1.510544798478812,
+            "y": 6.2989823057862875,
+            "heading": -0.5925289533905876,
+            "angularVelocity": -1.2728352747326335,
+            "velocityX": 2.96067066379232,
+            "velocityY": -1.6036943733404823,
+            "timestamp": 0.49548070936280164
+        },
+        {
+            "x": 1.7178378905511122,
+            "y": 6.190936000456915,
+            "heading": -0.5942013489806535,
+            "angularVelocity": -0.027002392762643195,
+            "velocityX": 3.3469410720572084,
+            "velocityY": -1.7445087695675872,
+            "timestamp": 0.5574157980331519
+        },
+        {
+            "x": 1.927701821836241,
+            "y": 6.086763021965953,
+            "heading": -0.5942019920614379,
+            "angularVelocity": -0.000010383141417040622,
+            "velocityX": 3.388449678374247,
+            "velocityY": -1.6819702809408126,
+            "timestamp": 0.6193508867035021
+        },
+        {
+            "x": 2.1375657815463693,
+            "y": 5.982590100737573,
+            "heading": -0.594202635139582,
+            "angularVelocity": -0.0000103830987882079,
+            "velocityX": 3.3884501373224802,
+            "velocityY": -1.6819693563827811,
+            "timestamp": 0.6812859753738524
+        },
+        {
+            "x": 2.347429741257157,
+            "y": 5.878417179509917,
+            "heading": -0.5942032782166262,
+            "angularVelocity": -0.000010383081029176535,
+            "velocityX": 3.3884501373331317,
+            "velocityY": -1.6819693563710962,
+            "timestamp": 0.7432210640442026
+        },
+        {
+            "x": 2.5572937009682177,
+            "y": 5.774244258282216,
+            "heading": -0.5942039212925836,
+            "angularVelocity": -0.000010383063479788062,
+            "velocityX": 3.3884501373375415,
+            "velocityY": -1.6819693563718265,
+            "timestamp": 0.8051561527145529
+        },
+        {
+            "x": 2.7671576606795463,
+            "y": 5.670071337054474,
+            "heading": -0.594204564367474,
+            "angularVelocity": -0.00001038304625561267,
+            "velocityX": 3.388450137341862,
+            "velocityY": -1.68196935637249,
+            "timestamp": 0.8670912413849031
+        },
+        {
+            "x": 2.977021620391137,
+            "y": 5.5658984158266955,
+            "heading": -0.5942052074413191,
+            "angularVelocity": -0.000010383029375253952,
+            "velocityX": 3.388450137346089,
+            "velocityY": -1.6819693563730826,
+            "timestamp": 0.9290263300552534
+        },
+        {
+            "x": 3.186885580102983,
+            "y": 5.461725494598884,
+            "heading": -0.5942058505141404,
+            "angularVelocity": -0.000010383012846871076,
+            "velocityX": 3.3884501373502207,
+            "velocityY": -1.6819693563736025,
+            "timestamp": 0.9909614187256036
+        },
+        {
+            "x": 3.3967495398150795,
+            "y": 5.357552573371046,
+            "heading": -0.594206493585961,
+            "angularVelocity": -0.000010382996688205053,
+            "velocityX": 3.3884501373542517,
+            "velocityY": -1.681969356374048,
+            "timestamp": 1.0528965073959538
+        },
+        {
+            "x": 3.6066134995274184,
+            "y": 5.253379652143185,
+            "heading": -0.5942071366568044,
+            "angularVelocity": -0.000010382980910469912,
+            "velocityX": 3.38845013735818,
+            "velocityY": -1.6819693563744145,
+            "timestamp": 1.114831596066304
+        },
+        {
+            "x": 3.8164774592399944,
+            "y": 5.149206730915306,
+            "heading": -0.5942077797266946,
+            "angularVelocity": -0.000010382965523044611,
+            "velocityX": 3.388450137362003,
+            "velocityY": -1.681969356374701,
+            "timestamp": 1.1767666847366542
+        },
+        {
+            "x": 4.026341418952801,
+            "y": 5.045033809687414,
+            "heading": -0.594208422795657,
+            "angularVelocity": -0.00001038295054018878,
+            "velocityX": 3.388450137365716,
+            "velocityY": -1.6819693563749047,
+            "timestamp": 1.2387017734070045
+        },
+        {
+            "x": 4.236205378665829,
+            "y": 4.940860888459515,
+            "heading": -0.594209065863717,
+            "angularVelocity": -0.000010382935971395078,
+            "velocityX": 3.388450137369317,
+            "velocityY": -1.681969356375023,
+            "timestamp": 1.3006368620773547
+        },
+        {
+            "x": 4.446069338379075,
+            "y": 4.836687967231613,
+            "heading": -0.5942097089309012,
+            "angularVelocity": -0.000010382921828372961,
+            "velocityX": 3.3884501373728035,
+            "velocityY": -1.6819693563750544,
+            "timestamp": 1.362571950747705
+        },
+        {
+            "x": 4.6559332980925285,
+            "y": 4.732515046003717,
+            "heading": -0.5942103519972364,
+            "angularVelocity": -0.000010382908121631514,
+            "velocityX": 3.388450137376173,
+            "velocityY": -1.6819693563749973,
+            "timestamp": 1.4245070394180552
+        },
+        {
+            "x": 4.865797257806183,
+            "y": 4.628342124775828,
+            "heading": -0.5942109950627501,
+            "angularVelocity": -0.00001038289485891648,
+            "velocityX": 3.388450137379422,
+            "velocityY": -1.6819693563748486,
+            "timestamp": 1.4864421280884055
+        },
+        {
+            "x": 5.075661217518705,
+            "y": 4.524169203545277,
+            "heading": -0.594211638127464,
+            "angularVelocity": -0.000010382881945026341,
+            "velocityX": 3.3884501373611275,
+            "velocityY": -1.6819693564178315,
+            "timestamp": 1.5483772167587557
+        },
+        {
+            "x": 5.285525078653758,
+            "y": 4.419996083725492,
+            "heading": -0.5942122811951368,
+            "angularVelocity": -0.000010382929719206097,
+            "velocityX": 3.3884485457355824,
+            "velocityY": -1.6819725628269917,
+            "timestamp": 1.610312305429106
+        },
+        {
+            "x": 5.489266395568848,
+            "y": 4.304305553436279,
+            "heading": -0.5942133396987443,
+            "angularVelocity": -0.000017090531880803138,
+            "velocityX": 3.2895943364108793,
+            "velocityY": -1.8679319392755818,
+            "timestamp": 1.6722473940994562
+        },
+        {
+            "x": 5.681024172067428,
+            "y": 4.156179263559471,
+            "heading": -0.5942132880529166,
+            "angularVelocity": 8.063055616728172e-7,
+            "velocityX": 2.9937628707653796,
+            "velocityY": -2.312578894659331,
+            "timestamp": 1.7362998206864544
+        },
+        {
+            "x": 5.864967854996084,
+            "y": 3.998454299605836,
+            "heading": -0.5942132372714615,
+            "angularVelocity": 7.928107921043856e-7,
+            "velocityX": 2.871767593048443,
+            "velocityY": -2.4624354198261003,
+            "timestamp": 1.8003522472734526
+        },
+        {
+            "x": 6.0489114264021975,
+            "y": 3.840729205591288,
+            "heading": -0.5942131864900138,
+            "angularVelocity": 7.928106775338953e-7,
+            "velocityX": 2.871765851934987,
+            "velocityY": -2.4624374503645132,
+            "timestamp": 1.8644046738604507
+        },
+        {
+            "x": 6.232854997807022,
+            "y": 3.683004111575237,
+            "heading": -0.5942131357085663,
+            "angularVelocity": 7.928106743629292e-7,
+            "velocityX": 2.87176585191486,
+            "velocityY": -2.462437450387986,
+            "timestamp": 1.928457100447449
+        },
+        {
+            "x": 6.4167985692118465,
+            "y": 3.5252790175591864,
+            "heading": -0.594213084927119,
+            "angularVelocity": 7.928106702567426e-7,
+            "velocityX": 2.871765851914861,
+            "velocityY": -2.462437450387986,
+            "timestamp": 1.992509527034447
+        },
+        {
+            "x": 6.6007421406166715,
+            "y": 3.367553923543135,
+            "heading": -0.5942130341456721,
+            "angularVelocity": 7.928106666471698e-7,
+            "velocityX": 2.8717658519148612,
+            "velocityY": -2.462437450387985,
+            "timestamp": 2.0565619536214452
+        },
+        {
+            "x": 6.784685712022088,
+            "y": 3.2098288295277735,
+            "heading": -0.5942129833642253,
+            "angularVelocity": 7.928106614555171e-7,
+            "velocityX": 2.8717658519240943,
+            "velocityY": -2.4624374503772177,
+            "timestamp": 2.120614380208443
+        },
+        {
+            "x": 6.968629334581004,
+            "y": 3.052103795169223,
+            "heading": -0.594212932582592,
+            "angularVelocity": 7.928135744203515e-7,
+            "velocityX": 2.871766650543336,
+            "velocityY": -2.462436519002493,
+            "timestamp": 2.184666806795441
+        },
+        {
+            "x": 7.14554518368725,
+            "y": 2.9064126516878526,
+            "heading": -0.554455950225117,
+            "angularVelocity": 0.6206943979472176,
+            "velocityX": 2.7620475684235433,
+            "velocityY": -2.2745608752775093,
+            "timestamp": 2.248719233382439
+        },
+        {
+            "x": 7.299470766691596,
+            "y": 2.7775737815312107,
+            "heading": -0.43989859569345574,
+            "angularVelocity": 1.7884935924490803,
+            "velocityX": 2.4031186827134454,
+            "velocityY": -2.0114596280228136,
+            "timestamp": 2.312771659969437
+        },
+        {
+            "x": 7.4271138269599435,
+            "y": 2.670230084319628,
+            "heading": -0.3103960824325961,
+            "angularVelocity": 2.021820564205559,
+            "velocityX": 1.9927903917110543,
+            "velocityY": -1.6758724521667427,
+            "timestamp": 2.376824086556435
+        },
+        {
+            "x": 7.52865057013146,
+            "y": 2.5846878922538004,
+            "heading": -0.1939655985882249,
+            "angularVelocity": 1.8177372825404448,
+            "velocityX": 1.5852130603296692,
+            "velocityY": -1.3355027533522315,
+            "timestamp": 2.440876513143433
+        },
+        {
+            "x": 7.604480145165485,
+            "y": 2.520737422138354,
+            "heading": -0.10094853997619462,
+            "angularVelocity": 1.4522019471922978,
+            "velocityX": 1.1838673267285131,
+            "velocityY": -0.9984082340516305,
+            "timestamp": 2.504928939730431
+        },
+        {
+            "x": 7.654868393442648,
+            "y": 2.4782123742697575,
+            "heading": -0.03632146129528005,
+            "angularVelocity": 1.008971589751342,
+            "velocityX": 0.7866719648587251,
+            "velocityY": -0.6639100208145539,
+            "timestamp": 2.568981366317429
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -0.003,
+            "angularVelocity": 0.5202216851226045,
+            "velocityX": 0.39235994475896174,
+            "velocityY": -0.3311720632620588,
+            "timestamp": 2.6330337929044267
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -0.003,
+            "angularVelocity": -9.133242422473854e-34,
+            "velocityX": 1.8081868884139647e-34,
+            "velocityY": 0,
+            "timestamp": 2.6970862194914247
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Amp Side To C5.traj b/src/main/deploy/choreo/Amp Side To C5.traj
new file mode 100644
index 00000000..6231149d
--- /dev/null
+++ b/src/main/deploy/choreo/Amp Side To C5.traj	
@@ -0,0 +1,805 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": 1.04,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7237404107060486,
+            "y": 6.712071337633356,
+            "heading": 0.9558489426883627,
+            "angularVelocity": -1.537881157714612,
+            "velocityX": 0.25110936688338653,
+            "velocityY": -0.14489824428928566,
+            "timestamp": 0.05471882979351295
+        },
+        {
+            "x": 0.7514918072858734,
+            "y": 6.694799584586779,
+            "heading": 0.7935491608961526,
+            "angularVelocity": -2.966068214628546,
+            "velocityX": 0.5071635611460984,
+            "velocityY": -0.31564551200661234,
+            "timestamp": 0.1094376595870259
+        },
+        {
+            "x": 0.7959632333479747,
+            "y": 6.666655428800684,
+            "heading": 0.574279005326375,
+            "angularVelocity": -4.007215731718184,
+            "velocityX": 0.8127261900504715,
+            "velocityY": -0.514341331718894,
+            "timestamp": 0.16415648938053887
+        },
+        {
+            "x": 0.8617224500002636,
+            "y": 6.63045184710829,
+            "heading": 0.31905346805964874,
+            "angularVelocity": -4.664309127769047,
+            "velocityX": 1.2017657705846174,
+            "velocityY": -0.6616293116832328,
+            "timestamp": 0.2188753191740518
+        },
+        {
+            "x": 0.9507709996198539,
+            "y": 6.585947725507511,
+            "heading": 0.05142997722620149,
+            "angularVelocity": -4.890884762034389,
+            "velocityX": 1.627384027685255,
+            "velocityY": -0.8133237090178916,
+            "timestamp": 0.2735941489675648
+        },
+        {
+            "x": 1.0614190010918925,
+            "y": 6.535018660742887,
+            "heading": -0.17495854287380355,
+            "angularVelocity": -4.137305584828904,
+            "velocityX": 2.0221192940269503,
+            "velocityY": -0.9307411170306262,
+            "timestamp": 0.32831297876107773
+        },
+        {
+            "x": 1.1921384640832198,
+            "y": 6.474205616338843,
+            "heading": -0.35862575739678126,
+            "angularVelocity": -3.356563274764148,
+            "velocityX": 2.388930163247485,
+            "velocityY": -1.111373262796896,
+            "timestamp": 0.3830318085545907
+        },
+        {
+            "x": 1.3384031857193457,
+            "y": 6.398894346098027,
+            "heading": -0.49152044440666937,
+            "angularVelocity": -2.4286829142980233,
+            "velocityX": 2.6730235677201417,
+            "velocityY": -1.376331886573771,
+            "timestamp": 0.4377506383481036
+        },
+        {
+            "x": 1.500957547301863,
+            "y": 6.312259668262383,
+            "heading": -0.5624501430724521,
+            "angularVelocity": -1.2962575942036991,
+            "velocityX": 2.9707207225727275,
+            "velocityY": -1.5832699303433067,
+            "timestamp": 0.4924694681416166
+        },
+        {
+            "x": 1.6817069101290123,
+            "y": 6.217739107445247,
+            "heading": -0.5720464000476684,
+            "angularVelocity": -0.17537394369443057,
+            "velocityX": 3.3032388212471107,
+            "velocityY": -1.7273863709040396,
+            "timestamp": 0.5471882979351296
+        },
+        {
+            "x": 1.8673643551319996,
+            "y": 6.126199810501224,
+            "heading": -0.5720469984173726,
+            "angularVelocity": -0.000010935352720012522,
+            "velocityX": 3.392935223643913,
+            "velocityY": -1.6729030443351731,
+            "timestamp": 0.6019071277286425
+        },
+        {
+            "x": 2.0530162492759167,
+            "y": 6.034649256644459,
+            "heading": -0.5720475972653951,
+            "angularVelocity": -0.000010944094102204816,
+            "velocityX": 3.3928337803365705,
+            "velocityY": -1.6731087671691436,
+            "timestamp": 0.6566259575221555
+        },
+        {
+            "x": 2.23866813267496,
+            "y": 5.943098680998449,
+            "heading": -0.5720481961130315,
+            "angularVelocity": -0.000010944087046546937,
+            "velocityX": 3.3928335839713664,
+            "velocityY": -1.673109165372953,
+            "timestamp": 0.7113447873156684
+        },
+        {
+            "x": 2.4243200161477754,
+            "y": 5.851548105501653,
+            "heading": -0.5720487949598734,
+            "angularVelocity": -0.000010944072528266654,
+            "velocityX": 3.392833585319575,
+            "velocityY": -1.673109162646019,
+            "timestamp": 0.7660636171091814
+        },
+        {
+            "x": 2.6099718996232633,
+            "y": 5.759997530009892,
+            "heading": -0.5720493938059237,
+            "angularVelocity": -0.000010944058060277418,
+            "velocityX": 3.3928335853684146,
+            "velocityY": -1.673109162553994,
+            "timestamp": 0.8207824469026943
+        },
+        {
+            "x": 2.7956237830994004,
+            "y": 5.668446954519065,
+            "heading": -0.5720499926511823,
+            "angularVelocity": -0.000010944043595406558,
+            "velocityX": 3.3928335853802802,
+            "velocityY": -1.6731091625369476,
+            "timestamp": 0.8755012766962073
+        },
+        {
+            "x": 2.9812756665756717,
+            "y": 5.576896379028125,
+            "heading": -0.5720505914956495,
+            "angularVelocity": -0.000010944029129446314,
+            "velocityX": 3.3928335853827263,
+            "velocityY": -1.6731091625390042,
+            "timestamp": 0.9302201064897202
+        },
+        {
+            "x": 3.166927550036217,
+            "y": 5.485345803504912,
+            "heading": -0.5720511903393257,
+            "angularVelocity": -0.000010944014673786385,
+            "velocityX": 3.392833585095338,
+            "velocityY": -1.673109163128796,
+            "timestamp": 0.9849389362832331
+        },
+        {
+            "x": 3.3525794334560626,
+            "y": 5.393795227898782,
+            "heading": -0.5720517891822119,
+            "angularVelocity": -0.000010944000236842356,
+            "velocityX": 3.392833584351539,
+            "velocityY": -1.6731091646441256,
+            "timestamp": 1.039657766076746
+        },
+        {
+            "x": 3.538231313226632,
+            "y": 5.302244644892125,
+            "heading": -0.5720523880244444,
+            "angularVelocity": -0.000010943988288632917,
+            "velocityX": 3.392833517660122,
+            "velocityY": -1.673109299890567,
+            "timestamp": 1.0943765958702592
+        },
+        {
+            "x": 3.723883292814953,
+            "y": 5.21069426429933,
+            "heading": -0.5720529868621156,
+            "angularVelocity": -0.000010943904932797782,
+            "velocityX": 3.3928353418539645,
+            "velocityY": -1.673105600727685,
+            "timestamp": 1.1490954256637722
+        },
+        {
+            "x": 3.909492929164672,
+            "y": 5.1190580682844296,
+            "heading": -0.5720535873273735,
+            "angularVelocity": -0.000010973649477031932,
+            "velocityX": 3.3920615088102064,
+            "velocityY": -1.6746738985594591,
+            "timestamp": 1.2038142554572853
+        },
+        {
+            "x": 4.092995643615723,
+            "y": 5.023272514343262,
+            "heading": -0.5720542830492642,
+            "angularVelocity": -0.000012714487741704976,
+            "velocityX": 3.353556995709023,
+            "velocityY": -1.7505044296932655,
+            "timestamp": 1.2585330852507983
+        },
+        {
+            "x": 4.152699494738863,
+            "y": 4.989124335806784,
+            "heading": -0.5720542274621303,
+            "angularVelocity": 0.0000030573365885939726,
+            "velocityX": 3.283759307267904,
+            "velocityY": -1.8781769850015708,
+            "timestamp": 1.2767146404552234
+        },
+        {
+            "x": 4.211236123155136,
+            "y": 4.953011857528808,
+            "heading": -0.5720541720789936,
+            "angularVelocity": 0.000003046116576982166,
+            "velocityX": 3.2195611298433677,
+            "velocityY": -1.9862150334199096,
+            "timestamp": 1.2948961956596485
+        },
+        {
+            "x": 4.269217408443118,
+            "y": 4.916014310130808,
+            "heading": -0.5720541167402081,
+            "angularVelocity": 0.000003043677217870583,
+            "velocityX": 3.1890168160017764,
+            "velocityY": -2.034894539109796,
+            "timestamp": 1.3130777508640736
+        },
+        {
+            "x": 4.327172181408397,
+            "y": 4.8789752463128,
+            "heading": -0.5720540614037476,
+            "angularVelocity": 0.000003043549349895456,
+            "velocityX": 3.187558617163689,
+            "velocityY": -2.0371779752347874,
+            "timestamp": 1.3312593060684987
+        },
+        {
+            "x": 4.385108415981365,
+            "y": 4.841907191568928,
+            "heading": -0.5720540060672449,
+            "angularVelocity": 0.0000030435516711711228,
+            "velocityX": 3.186538990839725,
+            "velocityY": -2.0387724992190503,
+            "timestamp": 1.3494408612729238
+        },
+        {
+            "x": 4.443034277698237,
+            "y": 4.804822929374826,
+            "heading": -0.5720539507307192,
+            "angularVelocity": 0.000003043552934036031,
+            "velocityX": 3.1859684755020403,
+            "velocityY": -2.0396639218758956,
+            "timestamp": 1.367622416477349
+        },
+        {
+            "x": 4.500957639641057,
+            "y": 4.767734762799275,
+            "heading": -0.5720538953941279,
+            "angularVelocity": 0.0000030435565398491907,
+            "velocityX": 3.1858309859372986,
+            "velocityY": -2.039878665966735,
+            "timestamp": 1.385803971681774
+        },
+        {
+            "x": 4.558883202891902,
+            "y": 4.7306500344023386,
+            "heading": -0.5720538400575734,
+            "angularVelocity": 0.000003043554516316428,
+            "velocityX": 3.1859520596302042,
+            "velocityY": -2.03968956340728,
+            "timestamp": 1.4039855268861992
+        },
+        {
+            "x": 4.616809360440233,
+            "y": 4.693566234312182,
+            "heading": -0.572053784721022,
+            "angularVelocity": 0.000003043554346667464,
+            "velocityX": 3.1859847464654854,
+            "velocityY": -2.039638505794633,
+            "timestamp": 1.4221670820906243
+        },
+        {
+            "x": 4.674735447300298,
+            "y": 4.656482323806888,
+            "heading": -0.5720537293844703,
+            "angularVelocity": 0.00000304355436453364,
+            "velocityX": 3.185980858554995,
+            "velocityY": -2.039644578714451,
+            "timestamp": 1.4403486372950494
+        },
+        {
+            "x": 4.732661331177625,
+            "y": 4.619398096238942,
+            "heading": -0.5720536740479176,
+            "angularVelocity": 0.0000030435544255368,
+            "velocityX": 3.1859696943429383,
+            "velocityY": -2.0396620174125277,
+            "timestamp": 1.4585301924994745
+        },
+        {
+            "x": 4.790587117727679,
+            "y": 4.582313716645222,
+            "heading": -0.5720536187113643,
+            "angularVelocity": 0.0000030435544545803464,
+            "velocityX": 3.1859643412654055,
+            "velocityY": -2.039670378951063,
+            "timestamp": 1.4767117477038996
+        },
+        {
+            "x": 4.848512993160408,
+            "y": 4.5452294758867104,
+            "heading": -0.5720535633748114,
+            "angularVelocity": 0.0000030435544295946876,
+            "velocityX": 3.1859692298833355,
+            "velocityY": -2.0396627429041696,
+            "timestamp": 1.4948933029083247
+        },
+        {
+            "x": 4.906438819485911,
+            "y": 4.508145158422396,
+            "heading": -0.5720535080382583,
+            "angularVelocity": 0.0000030435544442083865,
+            "velocityX": 3.1859665289468815,
+            "velocityY": -2.039666961784329,
+            "timestamp": 1.5130748581127498
+        },
+        {
+            "x": 4.9643646433421065,
+            "y": 4.471060837101057,
+            "heading": -0.5720534527017052,
+            "angularVelocity": 0.0000030435544458805193,
+            "velocityX": 3.185966393132936,
+            "velocityY": -2.039667173923745,
+            "timestamp": 1.5312564133171749
+        },
+        {
+            "x": 5.022290480818567,
+            "y": 4.433976537054754,
+            "heading": -0.572053397365152,
+            "angularVelocity": 0.0000030435544415454932,
+            "velocityX": 3.185967142258403,
+            "velocityY": -2.039666003779759,
+            "timestamp": 1.5494379685216
+        },
+        {
+            "x": 5.080216347467466,
+            "y": 4.396892282576102,
+            "heading": -0.572053342028599,
+            "angularVelocity": 0.000003043554433808661,
+            "velocityX": 3.1859687467658464,
+            "velocityY": -2.039663497522643,
+            "timestamp": 1.567619523726025
+        },
+        {
+            "x": 5.138142382220086,
+            "y": 4.359808290677983,
+            "heading": -0.5720532866920471,
+            "angularVelocity": 0.000003043554377469469,
+            "velocityX": 3.185977992604117,
+            "velocityY": -2.039649055384386,
+            "timestamp": 1.5858010789304502
+        },
+        {
+            "x": 5.196068138494407,
+            "y": 4.322723863792993,
+            "heading": -0.5720532313554916,
+            "angularVelocity": 0.000003043554576021048,
+            "velocityX": 3.1859626760759787,
+            "velocityY": -2.0396729800088242,
+            "timestamp": 1.6039826341348753
+        },
+        {
+            "x": 5.253993690845546,
+            "y": 4.285639118381674,
+            "heading": -0.5720531760189423,
+            "angularVelocity": 0.0000030435542278177066,
+            "velocityX": 3.1859514601384813,
+            "velocityY": -2.0396904992104243,
+            "timestamp": 1.6221641893393004
+        },
+        {
+            "x": 5.3119198680466875,
+            "y": 4.248555348991913,
+            "heading": -0.5720531206823879,
+            "angularVelocity": 0.000003043554518420827,
+            "velocityX": 3.1859858273860655,
+            "velocityY": -2.0396368172479113,
+            "timestamp": 1.6403457445437255
+        },
+        {
+            "x": 5.369845451022558,
+            "y": 4.211470651416335,
+            "heading": -0.5720530653458327,
+            "angularVelocity": 0.0000030435545594837867,
+            "velocityX": 3.1859531445232334,
+            "velocityY": -2.039687868206526,
+            "timestamp": 1.6585272997481506
+        },
+        {
+            "x": 5.427769955136907,
+            "y": 4.17438426873464,
+            "heading": -0.5720530100092767,
+            "angularVelocity": 0.0000030435545998300077,
+            "velocityX": 3.185893806281465,
+            "velocityY": -2.039780550383344,
+            "timestamp": 1.6767088549525757
+        },
+        {
+            "x": 5.485698211873678,
+            "y": 4.137303747848898,
+            "heading": -0.5720529546727399,
+            "angularVelocity": 0.000003043553546723704,
+            "velocityX": 3.1861002035013715,
+            "velocityY": -2.0394581469404574,
+            "timestamp": 1.6948904101570008
+        },
+        {
+            "x": 5.5436372749681775,
+            "y": 4.100240114388813,
+            "heading": -0.5720528993360909,
+            "angularVelocity": 0.0000030435597092212927,
+            "velocityX": 3.186694561772871,
+            "velocityY": -2.0385293250953618,
+            "timestamp": 1.713071965361426
+        },
+        {
+            "x": 5.601833960036281,
+            "y": 4.063582321171135,
+            "heading": -0.5720528439855975,
+            "angularVelocity": 0.0000030443211669066955,
+            "velocityX": 3.2008639752640304,
+            "velocityY": -2.0162077889097265,
+            "timestamp": 1.731253520565851
+        },
+        {
+            "x": 5.661045954546234,
+            "y": 4.0285882123488195,
+            "heading": -0.5720527885077281,
+            "angularVelocity": 0.000003051326955110969,
+            "velocityX": 3.2567068022627845,
+            "velocityY": -1.9247038236737246,
+            "timestamp": 1.7494350757702761
+        },
+        {
+            "x": 5.721517200862813,
+            "y": 3.9958181607132874,
+            "heading": -0.5720527326566293,
+            "angularVelocity": 0.0000030718548612359243,
+            "velocityX": 3.3259666533847816,
+            "velocityY": -1.8023789091228484,
+            "timestamp": 1.7676166309747012
+        },
+        {
+            "x": 5.7832255363464355,
+            "y": 3.9654417037963863,
+            "heading": -0.572052675208486,
+            "angularVelocity": 0.000003159693577578397,
+            "velocityX": 3.3940075416988695,
+            "velocityY": -1.670729295450284,
+            "timestamp": 1.7857981861791263
+        },
+        {
+            "x": 5.945861586676733,
+            "y": 3.9048707731215235,
+            "heading": -0.5720526214657033,
+            "angularVelocity": 0.0000011714587578241761,
+            "velocityX": 3.5450606755342,
+            "velocityY": -1.3202953710429959,
+            "timestamp": 1.831674989206145
+        },
+        {
+            "x": 6.113092819895856,
+            "y": 3.858469939627616,
+            "heading": -0.5720525698257588,
+            "angularVelocity": 0.0000011256221281562495,
+            "velocityX": 3.6452242132183756,
+            "velocityY": -1.0114225585101817,
+            "timestamp": 1.8775517922331635
+        },
+        {
+            "x": 6.280428909416468,
+            "y": 3.8124486851902244,
+            "heading": -0.5720525182264563,
+            "angularVelocity": 0.0000011247362300949913,
+            "velocityX": 3.6475098193329205,
+            "velocityY": -1.0031486808299002,
+            "timestamp": 1.923428595260182
+        },
+        {
+            "x": 6.444900242470224,
+            "y": 3.7570546132296903,
+            "heading": -0.5720524653781475,
+            "angularVelocity": 0.000001151961455361842,
+            "velocityX": 3.585065266142057,
+            "velocityY": -1.2074527496557126,
+            "timestamp": 1.9693053982872004
+        },
+        {
+            "x": 6.602974533610246,
+            "y": 3.685417746961916,
+            "heading": -0.5720524086130027,
+            "angularVelocity": 0.0000012373387213911858,
+            "velocityX": 3.445625691199315,
+            "velocityY": -1.561505195243581,
+            "timestamp": 2.015182201314219
+        },
+        {
+            "x": 6.753006171084989,
+            "y": 3.5981834135738024,
+            "heading": -0.5720523449049553,
+            "angularVelocity": 0.0000013886766999770178,
+            "velocityX": 3.27031588025751,
+            "velocityY": -1.9014911160397099,
+            "timestamp": 2.0610590043412373
+        },
+        {
+            "x": 6.893461047058093,
+            "y": 3.4962434932531963,
+            "heading": -0.5720522700606003,
+            "angularVelocity": 0.0000016314204553722347,
+            "velocityX": 3.0615663408441725,
+            "velocityY": -2.2220362709370285,
+            "timestamp": 2.106935807368256
+        },
+        {
+            "x": 7.0229029918463715,
+            "y": 3.380640293435665,
+            "heading": -0.5720521774325067,
+            "angularVelocity": 0.000002019061650871652,
+            "velocityX": 2.8215118806773165,
+            "velocityY": -2.519861720737319,
+            "timestamp": 2.1528126103952743
+        },
+        {
+            "x": 7.140008449554443,
+            "y": 3.2525558471679688,
+            "heading": -0.5720520639012779,
+            "angularVelocity": 0.0000024746979161572956,
+            "velocityX": 2.5526071997441684,
+            "velocityY": -2.7919217952549285,
+            "timestamp": 2.1986894134222927
+        },
+        {
+            "x": 7.220415929770595,
+            "y": 3.149946383889573,
+            "heading": -0.5720519473102574,
+            "angularVelocity": 0.000003383341158681086,
+            "velocityX": 2.333335244140948,
+            "velocityY": -2.977611988413324,
+            "timestamp": 2.2331497338662096
+        },
+        {
+            "x": 7.2928033634314815,
+            "y": 3.0415299314010706,
+            "heading": -0.5720518591979534,
+            "angularVelocity": 0.000002556920622107498,
+            "velocityX": 2.100602453152653,
+            "velocityY": -3.1461243276865574,
+            "timestamp": 2.2676100543101265
+        },
+        {
+            "x": 7.356753135957797,
+            "y": 2.927931973099431,
+            "heading": -0.5720517877574303,
+            "angularVelocity": 0.000002073124165462267,
+            "velocityX": 1.855750953634494,
+            "velocityY": -3.2964858375736252,
+            "timestamp": 2.3020703747540434
+        },
+        {
+            "x": 7.411896317838347,
+            "y": 2.8098078848104033,
+            "heading": -0.5720517267503814,
+            "angularVelocity": 0.0000017703564026272917,
+            "velocityX": 1.6001935318717442,
+            "velocityY": -3.427829073187373,
+            "timestamp": 2.3365306951979603
+        },
+        {
+            "x": 7.457914797691639,
+            "y": 2.687839154921363,
+            "heading": -0.572051672451037,
+            "angularVelocity": 0.0000015757063075533039,
+            "velocityX": 1.3354048732140973,
+            "velocityY": -3.5393962771625227,
+            "timestamp": 2.370991015641877
+        },
+        {
+            "x": 7.494543133171347,
+            "y": 2.5627294544923394,
+            "heading": -0.5720516223974168,
+            "angularVelocity": 0.000001452500141635371,
+            "velocityX": 1.0629133742199746,
+            "velocityY": -3.630543733121263,
+            "timestamp": 2.405451336085794
+        },
+        {
+            "x": 7.521592976236692,
+            "y": 2.435205423722826,
+            "heading": -0.572051574793162,
+            "angularVelocity": 0.0000013814222897397843,
+            "velocityX": 0.7849562255098341,
+            "velocityY": -3.70060490229679,
+            "timestamp": 2.439911656529711
+        },
+        {
+            "x": 7.539069360193098,
+            "y": 2.3060208712744585,
+            "heading": -0.572051528188319,
+            "angularVelocity": 0.000001352420480159191,
+            "velocityX": 0.5071451376970121,
+            "velocityY": -3.748791386272309,
+            "timestamp": 2.474371976973628
+        },
+        {
+            "x": 7.551504596507269,
+            "y": 2.176254012981795,
+            "heading": -0.5720514816592456,
+            "angularVelocity": 0.0000013502217280486013,
+            "velocityX": 0.3608566651205708,
+            "velocityY": -3.7656892513183533,
+            "timestamp": 2.5088322974175448
+        },
+        {
+            "x": 7.5639699142109125,
+            "y": 2.0464900408376128,
+            "heading": -0.5720514351306527,
+            "angularVelocity": 0.0000013502077822692335,
+            "velocityX": 0.36172959342888805,
+            "velocityY": -3.765605498514506,
+            "timestamp": 2.5432926178614617
+        },
+        {
+            "x": 7.576441327813496,
+            "y": 1.916726654418564,
+            "heading": -0.5720513886022516,
+            "angularVelocity": 0.00000135020222116771,
+            "velocityX": 0.36190648960876737,
+            "velocityY": -3.7655885014252446,
+            "timestamp": 2.5777529383053785
+        },
+        {
+            "x": 7.58884833483585,
+            "y": 1.786957094038731,
+            "heading": -0.5720513420716767,
+            "angularVelocity": 0.000001350265295572762,
+            "velocityX": 0.36003748258495927,
+            "velocityY": -3.765767662871606,
+            "timestamp": 2.6122132587492954
+        },
+        {
+            "x": 7.601019357724818,
+            "y": 1.6571651890693204,
+            "heading": -0.5720512933827582,
+            "angularVelocity": 0.0000014128980208836598,
+            "velocityX": 0.3531894866947264,
+            "velocityY": -3.7664160778959355,
+            "timestamp": 2.6466735791932123
+        },
+        {
+            "x": 7.610550391987499,
+            "y": 1.5345810963054864,
+            "heading": -0.5537181215211259,
+            "angularVelocity": 0.5320081654919462,
+            "velocityX": 0.2765799661741953,
+            "velocityY": -3.5572534203020942,
+            "timestamp": 2.6811338996371292
+        },
+        {
+            "x": 7.620334086621719,
+            "y": 1.4202071871878734,
+            "heading": -0.5150322166693104,
+            "angularVelocity": 1.122621738667962,
+            "velocityX": 0.2839118878810709,
+            "velocityY": -3.319003063356803,
+            "timestamp": 2.715594220081046
+        },
+        {
+            "x": 7.630352820350644,
+            "y": 1.314046573370508,
+            "heading": -0.4556179078059868,
+            "angularVelocity": 1.7241368651811046,
+            "velocityX": 0.290732459821803,
+            "velocityY": -3.0806624096878554,
+            "timestamp": 2.750054540524963
+        },
+        {
+            "x": 7.63947235322732,
+            "y": 1.2175511973082003,
+            "heading": -0.3892109145347841,
+            "angularVelocity": 1.9270567544269157,
+            "velocityX": 0.2646386556821465,
+            "velocityY": -2.8001880080992603,
+            "timestamp": 2.78451486096888
+        },
+        {
+            "x": 7.647652812069227,
+            "y": 1.1309118710627388,
+            "heading": -0.32231407882554747,
+            "angularVelocity": 1.9412714347239703,
+            "velocityX": 0.23738777633239685,
+            "velocityY": -2.5141764536539966,
+            "timestamp": 2.818975181412797
+        },
+        {
+            "x": 7.654895533140534,
+            "y": 1.0540989766744202,
+            "heading": -0.25819483301023066,
+            "angularVelocity": 1.8606688791433752,
+            "velocityX": 0.21017567387602423,
+            "velocityY": -2.22902437930995,
+            "timestamp": 2.8534355018567137
+        },
+        {
+            "x": 7.66120815675186,
+            "y": 0.9870533783479266,
+            "heading": -0.19885832997172487,
+            "angularVelocity": 1.7218790270703905,
+            "velocityX": 0.18318528469800885,
+            "velocityY": -1.9455883596791401,
+            "timestamp": 2.8878958223006306
+        },
+        {
+            "x": 7.666600355589264,
+            "y": 0.929716153410391,
+            "heading": -0.14570825222338094,
+            "angularVelocity": 1.542355876662632,
+            "velocityX": 0.15647558606309142,
+            "velocityY": -1.663862210186161,
+            "timestamp": 2.9223561427445475
+        },
+        {
+            "x": 7.671080940668327,
+            "y": 0.8820338579186632,
+            "heading": -0.09980369912266952,
+            "angularVelocity": 1.3320988461333123,
+            "velocityX": 0.1300215732566484,
+            "velocityY": -1.383686944215496,
+            "timestamp": 2.9568164631884644
+        },
+        {
+            "x": 7.674656709905022,
+            "y": 0.8439593502971015,
+            "heading": -0.06197555143316066,
+            "angularVelocity": 1.0977305841102087,
+            "velocityX": 0.10376482837700403,
+            "velocityY": -1.1048796741031013,
+            "timestamp": 2.9912767836323813
+        },
+        {
+            "x": 7.67733258738128,
+            "y": 0.8154516156506164,
+            "heading": -0.032889080662387146,
+            "angularVelocity": 0.8440568861838474,
+            "velocityX": 0.07765097485389878,
+            "velocityY": -0.8272626104241091,
+            "timestamp": 3.025737104076298
+        },
+        {
+            "x": 7.679112422858347,
+            "y": 0.7964752743679322,
+            "heading": -0.013083588528854375,
+            "angularVelocity": 0.5747332548975859,
+            "velocityX": 0.05164883710108163,
+            "velocityY": -0.5506722235380279,
+            "timestamp": 3.060197424520215
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -0.003,
+            "angularVelocity": 0.2926144736603049,
+            "velocityX": 0.025756497043992084,
+            "velocityY": -0.27496187632245256,
+            "timestamp": 3.094657744964132
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -0.003,
+            "angularVelocity": 9.445490920898867e-33,
+            "velocityX": 0,
+            "velocityY": 3.8255947334103544e-33,
+            "timestamp": 3.129118065408049
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Amp Side To W1.traj b/src/main/deploy/choreo/Amp Side To W1.traj
new file mode 100644
index 00000000..59066a5b
--- /dev/null
+++ b/src/main/deploy/choreo/Amp Side To W1.traj	
@@ -0,0 +1,121 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": 1.04,
+            "angularVelocity": -3.0520704892108665e-32,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7541195079597249,
+            "y": 6.720987197392724,
+            "heading": 1.0162961519009226,
+            "angularVelocity": -0.3240021115694771,
+            "velocityX": 0.6030587810307456,
+            "velocityY": 0.013493760103527496,
+            "timestamp": 0.07315954820244353
+        },
+        {
+            "x": 0.8424136303815098,
+            "y": 6.722972492793459,
+            "heading": 0.9702543819568796,
+            "angularVelocity": -0.6293337107086672,
+            "velocityX": 1.2068707993857855,
+            "velocityY": 0.027136518055622637,
+            "timestamp": 0.14631909640488705
+        },
+        {
+            "x": 0.9749626751701804,
+            "y": 6.7259693430054925,
+            "heading": 0.9040114694947134,
+            "angularVelocity": -0.9054581949963141,
+            "velocityX": 1.8117805268819878,
+            "velocityY": 0.0409632137659495,
+            "timestamp": 0.21947864460733058
+        },
+        {
+            "x": 1.1518931657803464,
+            "y": 6.729992727527654,
+            "heading": 0.8214709979365993,
+            "angularVelocity": -1.1282255506783914,
+            "velocityX": 2.4184196725842533,
+            "velocityY": 0.0549946605880228,
+            "timestamp": 0.2926381928097741
+        },
+        {
+            "x": 1.3734034446931793,
+            "y": 6.735064446892539,
+            "heading": 0.7325177816386138,
+            "angularVelocity": -1.215879792639604,
+            "velocityX": 3.0277699132296103,
+            "velocityY": 0.06932409356680827,
+            "timestamp": 0.3657977410122176
+        },
+        {
+            "x": 1.6366130282089124,
+            "y": 6.741790657706068,
+            "heading": 0.6993734786052862,
+            "angularVelocity": -0.4530413848603092,
+            "velocityX": 3.597747525550208,
+            "velocityY": 0.09193893317775545,
+            "timestamp": 0.43895728921466115
+        },
+        {
+            "x": 1.858120509684905,
+            "y": 6.746838490569188,
+            "heading": 0.6098796286126051,
+            "angularVelocity": -1.2232695825988174,
+            "velocityX": 3.0277316757485147,
+            "velocityY": 0.0689975948068392,
+            "timestamp": 0.5121168374171047
+        },
+        {
+            "x": 2.035048195511774,
+            "y": 6.750890505964207,
+            "heading": 0.5269052635268205,
+            "angularVelocity": -1.1341563353588457,
+            "velocityX": 2.418381334686234,
+            "velocityY": 0.0553860089978706,
+            "timestamp": 0.5852763856195482
+        },
+        {
+            "x": 2.1675926440263233,
+            "y": 6.753939666061267,
+            "heading": 0.4602292179018529,
+            "angularVelocity": -0.9113785864349585,
+            "velocityX": 1.811717701533345,
+            "velocityY": 0.041678224811154865,
+            "timestamp": 0.6584359338219917
+        },
+        {
+            "x": 2.2558825150080364,
+            "y": 6.755978237986949,
+            "heading": 0.41386063697230147,
+            "angularVelocity": -0.6338008102679221,
+            "velocityX": 1.206812687489562,
+            "velocityY": 0.027864741865846625,
+            "timestamp": 0.7315954820244353
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": -0.3261452203924221,
+            "velocityX": 0.6030311295783809,
+            "velocityY": 0.013966215458624026,
+            "timestamp": 0.8047550302268788
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 9.371559495672325e-33,
+            "velocityX": 2.5226214414524116e-33,
+            "velocityY": -5.278780681595662e-33,
+            "timestamp": 0.8779145784293223
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Amp Side To W2.traj b/src/main/deploy/choreo/Amp Side To W2.traj
new file mode 100644
index 00000000..fb2e2531
--- /dev/null
+++ b/src/main/deploy/choreo/Amp Side To W2.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": 1.04,
+            "angularVelocity": 3.547724392998225e-32,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.737435339105297,
+            "y": 6.6995140898970185,
+            "heading": 1.00766177496316,
+            "angularVelocity": -0.49661186995303425,
+            "velocityX": 0.4213191985755434,
+            "velocityY": -0.3145981609176562,
+            "timestamp": 0.06511770457661178
+        },
+        {
+            "x": 0.7924238483684921,
+            "y": 6.658398285939555,
+            "heading": 0.945783535120445,
+            "angularVelocity": -0.95025216636614,
+            "velocityX": 0.8444479058456402,
+            "velocityY": -0.6314074524707902,
+            "timestamp": 0.13023540915322357
+        },
+        {
+            "x": 0.8751312092590968,
+            "y": 6.596482141852294,
+            "heading": 0.8584519121155169,
+            "angularVelocity": -1.3411348506945964,
+            "velocityX": 1.2701209514118896,
+            "velocityY": -0.9508342545216032,
+            "timestamp": 0.19535311372983535
+        },
+        {
+            "x": 0.9857818913354864,
+            "y": 6.513571279138628,
+            "heading": 0.7519056782658311,
+            "angularVelocity": -1.6362099146835547,
+            "velocityX": 1.6992411325894867,
+            "velocityY": -1.273246089565702,
+            "timestamp": 0.26047081830644714
+        },
+        {
+            "x": 1.1246151480908613,
+            "y": 6.409448227792029,
+            "heading": 0.6367147770436085,
+            "angularVelocity": -1.7689644002530829,
+            "velocityX": 2.132035483407367,
+            "velocityY": -1.5989975694566836,
+            "timestamp": 0.32558852288305895
+        },
+        {
+            "x": 1.2913898404503776,
+            "y": 6.284148794801154,
+            "heading": 0.5378900995711877,
+            "angularVelocity": -1.5176314662036392,
+            "velocityX": 2.5611267080722016,
+            "velocityY": -1.9241991683453519,
+            "timestamp": 0.39070622745967076
+        },
+        {
+            "x": 1.479902092849603,
+            "y": 6.139172935751937,
+            "heading": 0.5154398140678585,
+            "angularVelocity": -0.3447646941687853,
+            "velocityX": 2.8949462150871557,
+            "velocityY": -2.2263662392867607,
+            "timestamp": 0.4558239320362826
+        },
+        {
+            "x": 1.6682099030328927,
+            "y": 5.994133301373275,
+            "heading": 0.4928532472983225,
+            "angularVelocity": -0.34685753922672635,
+            "velocityX": 2.8918066355017875,
+            "velocityY": -2.2273456246913312,
+            "timestamp": 0.5209416366128944
+        },
+        {
+            "x": 1.8350205910276742,
+            "y": 5.8693381796986746,
+            "heading": 0.39723540571375665,
+            "angularVelocity": -1.4683847074503305,
+            "velocityX": 2.561679486083952,
+            "velocityY": -1.9164545569596636,
+            "timestamp": 0.5860593411895062
+        },
+        {
+            "x": 1.9739536597530634,
+            "y": 5.765582600106914,
+            "heading": 0.2820330722411117,
+            "angularVelocity": -1.7691399631126123,
+            "velocityX": 2.1335682765342634,
+            "velocityY": -1.5933543767607987,
+            "timestamp": 0.651177045766118
+        },
+        {
+            "x": 2.08468090414567,
+            "y": 5.683000750382208,
+            "heading": 0.1756895135649168,
+            "angularVelocity": -1.6330974712273538,
+            "velocityX": 1.7004168852778658,
+            "velocityY": -1.2681935007022147,
+            "timestamp": 0.7162947503427298
+        },
+        {
+            "x": 2.167467858188685,
+            "y": 5.6213477608369935,
+            "heading": 0.08927498136102419,
+            "angularVelocity": -1.3270512645639152,
+            "velocityX": 1.2713432480657338,
+            "velocityY": -0.946793041094972,
+            "timestamp": 0.7814124549193416
+        },
+        {
+            "x": 2.2225251033432207,
+            "y": 5.5804064125777195,
+            "heading": 0.028526912579098198,
+            "angularVelocity": -0.9328963478811614,
+            "velocityX": 0.8455034696402836,
+            "velocityY": -0.628728370041152,
+            "timestamp": 0.8465301594959534
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -0.003,
+            "angularVelocity": -0.4841527013902356,
+            "velocityX": 0.42192667624600166,
+            "velocityY": -0.31337733279145685,
+            "timestamp": 0.9116478640725653
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -0.003,
+            "angularVelocity": -3.2592320489843035e-28,
+            "velocityX": -3.6408651267401044e-33,
+            "velocityY": 2.9208085569078165e-32,
+            "timestamp": 0.9767655686491771
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Amp Side To W3.traj b/src/main/deploy/choreo/Amp Side To W3.traj
new file mode 100644
index 00000000..dd55da0b
--- /dev/null
+++ b/src/main/deploy/choreo/Amp Side To W3.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": 1.04,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7269019070910594,
+            "y": 6.695506477136938,
+            "heading": 1.0019343442516249,
+            "angularVelocity": -0.6189075555866055,
+            "velocityX": 0.2748072454505401,
+            "velocityY": -0.3982389391394925,
+            "timestamp": 0.06150459047522247
+        },
+        {
+            "x": 0.7606735605009036,
+            "y": 6.646281390208111,
+            "heading": 0.9275075561119867,
+            "angularVelocity": -1.2101013528351434,
+            "velocityX": 0.5490915905447641,
+            "velocityY": -0.8003481780543988,
+            "timestamp": 0.12300918095044494
+        },
+        {
+            "x": 0.8112865108119746,
+            "y": 6.571983401826325,
+            "heading": 0.8196848361236891,
+            "angularVelocity": -1.7530841056771946,
+            "velocityX": 0.8229133780097379,
+            "velocityY": -1.2080072041406207,
+            "timestamp": 0.1845137714256674
+        },
+        {
+            "x": 0.8787770707149783,
+            "y": 6.472119277485466,
+            "heading": 0.6843886163577808,
+            "angularVelocity": -2.199774337501089,
+            "velocityX": 1.0973255716610744,
+            "velocityY": -1.6236857049083067,
+            "timestamp": 0.24601836190088988
+        },
+        {
+            "x": 0.9632585614164454,
+            "y": 6.3460508217508655,
+            "heading": 0.5331826539280757,
+            "angularVelocity": -2.4584500321259677,
+            "velocityX": 1.373580249030372,
+            "velocityY": -2.0497405927024026,
+            "timestamp": 0.3075229523761124
+        },
+        {
+            "x": 1.0643372089754837,
+            "y": 6.193350092274746,
+            "heading": 0.39338059647753487,
+            "angularVelocity": -2.273034522632274,
+            "velocityX": 1.6434325759759127,
+            "velocityY": -2.4827533733053033,
+            "timestamp": 0.3690275428513349
+        },
+        {
+            "x": 1.1776993188507112,
+            "y": 6.018028259717655,
+            "heading": 0.3265019863738018,
+            "angularVelocity": -1.087375911082207,
+            "velocityX": 1.843148763357691,
+            "velocityY": -2.85054873469518,
+            "timestamp": 0.4305321333265574
+        },
+        {
+            "x": 1.3085730997897271,
+            "y": 5.825657422218107,
+            "heading": 0.3265019166878518,
+            "angularVelocity": -0.0000011330203075824657,
+            "velocityX": 2.1278701301448275,
+            "velocityY": -3.1277476366100623,
+            "timestamp": 0.4920367238017799
+        },
+        {
+            "x": 1.4394483094166801,
+            "y": 5.633287556689,
+            "heading": 0.32650184700859936,
+            "angularVelocity": -0.00000113291141146818,
+            "velocityX": 2.1278933591091382,
+            "velocityY": -3.127731833392264,
+            "timestamp": 0.5535413142770024
+        },
+        {
+            "x": 1.5703235190623746,
+            "y": 5.4409176911726425,
+            "heading": 0.32650177732934843,
+            "angularVelocity": -0.0000011329113877746253,
+            "velocityX": 2.127893359413853,
+            "velocityY": -3.1277318331849595,
+            "timestamp": 0.6150459047522249
+        },
+        {
+            "x": 1.7011987286842072,
+            "y": 5.24854782564005,
+            "heading": 0.32650170765009895,
+            "angularVelocity": -0.0000011329113643360664,
+            "velocityX": 2.127893359025885,
+            "velocityY": -3.1277318334489093,
+            "timestamp": 0.6765504952274474
+        },
+        {
+            "x": 1.8320721192807423,
+            "y": 5.056176722584032,
+            "heading": 0.32650163796198384,
+            "angularVelocity": -0.000001133055510645955,
+            "velocityX": 2.1278637835863385,
+            "velocityY": -3.1277519542791743,
+            "timestamp": 0.7380550857026699
+        },
+        {
+            "x": 1.9454000953072241,
+            "y": 4.881259770298511,
+            "heading": 0.26077643177319376,
+            "angularVelocity": -1.0686227756490447,
+            "velocityX": 1.8425937828516514,
+            "velocityY": -2.843965807007333,
+            "timestamp": 0.7995596761778924
+        },
+        {
+            "x": 2.046443192812682,
+            "y": 4.7300582879948045,
+            "heading": 0.13106133632258532,
+            "angularVelocity": -2.1090311218780498,
+            "velocityX": 1.6428545694676804,
+            "velocityY": -2.4583771899858244,
+            "timestamp": 0.8610642666531149
+        },
+        {
+            "x": 2.131123897414369,
+            "y": 4.604963233646066,
+            "heading": -0.022718028385538325,
+            "angularVelocity": -2.50029084853552,
+            "velocityX": 1.376819257674782,
+            "velocityY": -2.0339141092100017,
+            "timestamp": 0.9225688571283374
+        },
+        {
+            "x": 2.198697305455373,
+            "y": 4.506052899677577,
+            "heading": -0.16200255999945734,
+            "angularVelocity": -2.264620096446796,
+            "velocityX": 1.0986725953118257,
+            "velocityY": -1.6081780758842066,
+            "timestamp": 0.9840734476035599
+        },
+        {
+            "x": 2.249342546290196,
+            "y": 4.432626729162034,
+            "heading": -0.2740984297313893,
+            "angularVelocity": -1.822561029442031,
+            "velocityX": 0.8234383879887088,
+            "velocityY": -1.1938323619132651,
+            "timestamp": 1.0455780380787822
+        },
+        {
+            "x": 2.283112085041758,
+            "y": 4.38410205128396,
+            "heading": -0.35252590091377134,
+            "angularVelocity": -1.2751482544051254,
+            "velocityX": 0.549057208423595,
+            "velocityY": -0.788960263016835,
+            "timestamp": 1.1070826285540047
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.393,
+            "angularVelocity": -0.6580663129938888,
+            "velocityX": 0.2745797480766085,
+            "velocityY": -0.39187402269867344,
+            "timestamp": 1.1685872190292272
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.393,
+            "angularVelocity": 0,
+            "velocityX": 1.1335977805694889e-32,
+            "velocityY": 0,
+            "timestamp": 1.2300918095044497
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C1 To C2.traj b/src/main/deploy/choreo/C1 To C2.traj
new file mode 100644
index 00000000..50686485
--- /dev/null
+++ b/src/main/deploy/choreo/C1 To C2.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -5.382102664363315e-41,
+            "angularVelocity": -2.8652892976895567e-45,
+            "velocityX": 1.592735858156421e-40,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 7.383499994424153,
+            "heading": 1.659495816830368e-21,
+            "angularVelocity": 1.658316243120447e-20,
+            "velocityX": 2.780383744489643e-33,
+            "velocityY": -0.8344065481228213,
+            "timestamp": 0.10007112931184184
+        },
+        {
+            "x": 7.68,
+            "y": 7.216499986370153,
+            "heading": 4.9512387638524226e-21,
+            "angularVelocity": 3.289403167370527e-20,
+            "velocityX": 3.618624574631574e-33,
+            "velocityY": -1.668813065290736,
+            "timestamp": 0.20014225862368368
+        },
+        {
+            "x": 7.68,
+            "y": 6.9659999820333836,
+            "heading": 9.801760970151702e-21,
+            "angularVelocity": 4.8470744390330394e-20,
+            "velocityX": 3.439292531132709e-33,
+            "velocityY": -2.503219520548835,
+            "timestamp": 0.30021338793552554
+        },
+        {
+            "x": 7.68,
+            "y": 6.632,
+            "heading": 1.506109654033251e-20,
+            "angularVelocity": 5.255597191401202e-20,
+            "velocityX": 4.639675238422859e-33,
+            "velocityY": -3.3376257900775017,
+            "timestamp": 0.40028451724736736
+        },
+        {
+            "x": 7.68,
+            "y": 6.298000017966616,
+            "heading": 5.3586032763760104e-21,
+            "angularVelocity": -9.69559666912617e-20,
+            "velocityX": 4.935439546802769e-33,
+            "velocityY": -3.3376257900775017,
+            "timestamp": 0.5003556465592092
+        },
+        {
+            "x": 7.68,
+            "y": 6.047500013629846,
+            "heading": 1.8760590350448333e-21,
+            "angularVelocity": -3.480068842301423e-20,
+            "velocityX": 4.701730454770122e-33,
+            "velocityY": -2.5032195205488352,
+            "timestamp": 0.600426775871051
+        },
+        {
+            "x": 7.68,
+            "y": 5.880500005575846,
+            "heading": 3.168722428707802e-22,
+            "angularVelocity": -1.5580785203142817e-20,
+            "velocityX": 3.49539235617723e-33,
+            "velocityY": -1.668813065290736,
+            "timestamp": 0.7004979051828928
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 5.881496870004905e-41,
+            "angularVelocity": -3.1664700918333573e-21,
+            "velocityX": 3.364378386644386e-33,
+            "velocityY": -0.8344065481228213,
+            "timestamp": 0.8005690344947346
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 9.145026397391945e-42,
+            "angularVelocity": -4.963805231001231e-40,
+            "velocityX": -4.870998572760777e-42,
+            "velocityY": 3.489748854003666e-39,
+            "timestamp": 0.9006401638065764
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C1 To C3.traj b/src/main/deploy/choreo/C1 To C3.traj
new file mode 100644
index 00000000..688fbdaa
--- /dev/null
+++ b/src/main/deploy/choreo/C1 To C3.traj	
@@ -0,0 +1,139 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 1.0127320738607452e-39,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 7.390554627994461,
+            "heading": -1.4011773737891353e-18,
+            "angularVelocity": -1.4633626501290335e-17,
+            "velocityX": 9.653222160408549e-34,
+            "velocityY": -0.7983807350946673,
+            "timestamp": 0.09575052183150974
+        },
+        {
+            "x": 7.68,
+            "y": 7.237663887017311,
+            "heading": -4.2066729097937526e-18,
+            "angularVelocity": -2.93000547907741e-17,
+            "velocityX": 5.229262005564601e-34,
+            "velocityY": -1.5967614385035824,
+            "timestamp": 0.19150104366301948
+        },
+        {
+            "x": 7.68,
+            "y": 7.0083277831364,
+            "heading": -8.423669065336014e-18,
+            "angularVelocity": -4.4041495279144365e-17,
+            "velocityX": -1.080014219031917e-33,
+            "velocityY": -2.3951420785410225,
+            "timestamp": 0.28725156549452924
+        },
+        {
+            "x": 7.68,
+            "y": 6.702546334555254,
+            "heading": -1.4072329853209785e-17,
+            "angularVelocity": -5.899352482411367e-17,
+            "velocityX": -1.5994734307756055e-33,
+            "velocityY": -3.1935225284643685,
+            "timestamp": 0.38300208732603896
+        },
+        {
+            "x": 7.68,
+            "y": 6.340327800733179,
+            "heading": -3.9014262815434015e-17,
+            "angularVelocity": -2.604887418380216e-16,
+            "velocityX": 4.89416509473128e-33,
+            "velocityY": -3.7829405719528495,
+            "timestamp": 0.4787526091575487
+        },
+        {
+            "x": 7.68,
+            "y": 5.978109266911059,
+            "heading": -3.149786356993182e-17,
+            "angularVelocity": 7.849982539759592e-17,
+            "velocityX": -6.0692119772378415e-33,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.5745031309890585
+        },
+        {
+            "x": 7.68,
+            "y": 5.61589073308894,
+            "heading": -3.435394827038493e-17,
+            "angularVelocity": -2.98283982773363e-17,
+            "velocityX": 7.480170031140856e-33,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.6702536528205683
+        },
+        {
+            "x": 7.68,
+            "y": 5.253672199266821,
+            "heading": -8.183196831726821e-18,
+            "angularVelocity": 2.7332228522691796e-16,
+            "velocityX": -1.1202872015905622e-32,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.766004174652078
+        },
+        {
+            "x": 7.68,
+            "y": 4.891453665444746,
+            "heading": 1.4014689490668664e-17,
+            "angularVelocity": 2.318304474774222e-16,
+            "velocityX": 5.155427490211361e-33,
+            "velocityY": -3.7829405719528495,
+            "timestamp": 0.8617546964835878
+        },
+        {
+            "x": 7.68,
+            "y": 4.5856722168636,
+            "heading": 8.395813027039913e-18,
+            "angularVelocity": -5.868246309956073e-17,
+            "velocityX": -4.168254505442575e-33,
+            "velocityY": -3.1935225284643685,
+            "timestamp": 0.9575052183150976
+        },
+        {
+            "x": 7.68,
+            "y": 4.356336112982689,
+            "heading": 4.1945848336017725e-18,
+            "angularVelocity": -4.387681773072387e-17,
+            "velocityX": 2.1988846402360483e-33,
+            "velocityY": -2.3951420785410225,
+            "timestamp": 1.0532557401466074
+        },
+        {
+            "x": 7.68,
+            "y": 4.203445372005539,
+            "heading": 1.3975677775871127e-18,
+            "angularVelocity": -2.9211507184809557e-17,
+            "velocityX": 3.5559383909284404e-33,
+            "velocityY": -1.5967614385035824,
+            "timestamp": 1.1490062619781172
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": -1.4595928576161535e-17,
+            "velocityX": -6.471812078129625e-34,
+            "velocityY": -0.7983807350946673,
+            "timestamp": 1.244756783809627
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 8.546727720642512e-43,
+            "velocityY": 6.233759417908357e-39,
+            "timestamp": 1.3405073056411367
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C1 To C4.traj b/src/main/deploy/choreo/C1 To C4.traj
new file mode 100644
index 00000000..d59fe62f
--- /dev/null
+++ b/src/main/deploy/choreo/C1 To C4.traj	
@@ -0,0 +1,175 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 3.5626996813915863e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 7.385155846048237,
+            "heading": 1.781123128781041e-18,
+            "angularVelocity": 1.7977717932873702e-17,
+            "velocityX": 2.837123063321612e-19,
+            "velocityY": -0.8260917454276266,
+            "timestamp": 0.099073927811004
+        },
+        {
+            "x": 7.68,
+            "y": 7.22146754125807,
+            "heading": 5.344190150368438e-18,
+            "angularVelocity": 3.5963720225105045e-17,
+            "velocityX": 5.673750850451309e-19,
+            "velocityY": -1.65218345943065,
+            "timestamp": 0.198147855622008
+        },
+        {
+            "x": 7.68,
+            "y": 6.975935091856216,
+            "heading": 1.0690981078424305e-17,
+            "angularVelocity": 5.396768903659916e-17,
+            "velocityX": 8.509388117154945e-19,
+            "velocityY": -2.47827511058448,
+            "timestamp": 0.29722178343301203
+        },
+        {
+            "x": 7.68,
+            "y": 6.648558516522808,
+            "heading": 1.7824905328944302e-17,
+            "angularVelocity": 7.200607069613354e-17,
+            "velocityX": 1.134205375948437e-18,
+            "velocityY": -3.304366573190878,
+            "timestamp": 0.396295711244016
+        },
+        {
+            "x": 7.68,
+            "y": 6.273767735073314,
+            "heading": 3.2118763957552995e-17,
+            "angularVelocity": 1.4427467391699638e-16,
+            "velocityX": 1.2981920193437937e-18,
+            "velocityY": -3.782940575087071,
+            "timestamp": 0.49536963905502
+        },
+        {
+            "x": 7.68,
+            "y": 5.898976953623795,
+            "heading": 3.194072142377472e-17,
+            "angularVelocity": -1.7970674806511035e-18,
+            "velocityX": 1.2970624694923187e-18,
+            "velocityY": -3.7829405750873115,
+            "timestamp": 0.5944435668660241
+        },
+        {
+            "x": 7.68,
+            "y": 5.524186172174278,
+            "heading": 3.523836351949418e-17,
+            "angularVelocity": 3.3284660945411024e-17,
+            "velocityX": 1.2988038949897902e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.6935174946770281
+        },
+        {
+            "x": 7.68,
+            "y": 5.149395390724759,
+            "heading": 4.1591047407718544e-17,
+            "angularVelocity": 6.412064231794501e-17,
+            "velocityX": 1.299093645390414e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.792591422488032
+        },
+        {
+            "x": 7.68,
+            "y": 4.774604609275242,
+            "heading": 3.0405238206332753e-17,
+            "angularVelocity": -1.1290366142265185e-16,
+            "velocityX": 1.2929821768433727e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.891665350299036
+        },
+        {
+            "x": 7.68,
+            "y": 4.3998138278257235,
+            "heading": 3.8390344063969795e-17,
+            "angularVelocity": 8.059744913789504e-17,
+            "velocityX": 1.304793755296235e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.99073927811004
+        },
+        {
+            "x": 7.68,
+            "y": 4.025023046376205,
+            "heading": 1.0979594277826173e-17,
+            "angularVelocity": -2.766696586253564e-16,
+            "velocityX": 1.307186742413441e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 1.0898132059210441
+        },
+        {
+            "x": 7.68,
+            "y": 3.650232264926687,
+            "heading": -2.14668342522941e-17,
+            "angularVelocity": -3.2749714528425564e-16,
+            "velocityX": 1.3056923664932481e-18,
+            "velocityY": -3.7829405750873115,
+            "timestamp": 1.1888871337320481
+        },
+        {
+            "x": 7.68,
+            "y": 3.2754414834771928,
+            "heading": -1.7689075007663427e-17,
+            "angularVelocity": 3.813071035032691e-17,
+            "velocityX": 1.3012288375156972e-18,
+            "velocityY": -3.782940575087071,
+            "timestamp": 1.2879610615430521
+        },
+        {
+            "x": 7.68,
+            "y": 2.948064908143785,
+            "heading": -1.0623620375651477e-17,
+            "angularVelocity": 7.131497447110055e-17,
+            "velocityX": 1.136256754825581e-18,
+            "velocityY": -3.304366573190878,
+            "timestamp": 1.3870349893540561
+        },
+        {
+            "x": 7.68,
+            "y": 2.70253245874193,
+            "heading": -5.31498515459649e-18,
+            "angularVelocity": 5.35825654501949e-17,
+            "velocityX": 8.521926280380639e-19,
+            "velocityY": -2.4782751105844802,
+            "timestamp": 1.48610891716506
+        },
+        {
+            "x": 7.68,
+            "y": 2.538844153951763,
+            "heading": -1.7723662552803497e-18,
+            "angularVelocity": 3.575732766130284e-17,
+            "velocityX": 5.681284366395801e-19,
+            "velocityY": -1.6521834594306501,
+            "timestamp": 1.585182844976064
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": 1.7889330669293064e-17,
+            "velocityX": 2.8406422370407584e-19,
+            "velocityY": -0.8260917454276266,
+            "timestamp": 1.684256772787068
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 1.0461754707969188e-34,
+            "velocityY": 0,
+            "timestamp": 1.783330700598072
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C1 To C5.traj b/src/main/deploy/choreo/C1 To C5.traj
new file mode 100644
index 00000000..6f64999b
--- /dev/null
+++ b/src/main/deploy/choreo/C1 To C5.traj	
@@ -0,0 +1,220 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -8.452820466081124e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 7.3890447295128565,
+            "heading": -6.111572142469688e-20,
+            "angularVelocity": -6.320692328199756e-19,
+            "velocityX": -1.9797111149811777e-21,
+            "velocityY": -0.8062267267095136,
+            "timestamp": 0.09669149868710691
+        },
+        {
+            "x": 7.68,
+            "y": 7.2331341916331855,
+            "heading": -1.8226742072400974e-19,
+            "angularVelocity": -1.2529715733337834e-18,
+            "velocityX": -3.753564400558873e-21,
+            "velocityY": -1.6124534214139752,
+            "timestamp": 0.19338299737421383
+        },
+        {
+            "x": 7.68,
+            "y": 6.999268392550218,
+            "heading": -3.611632829024713e-19,
+            "angularVelocity": -1.850171571559818e-18,
+            "velocityX": -5.1157697830729356e-21,
+            "velocityY": -2.418680052108357,
+            "timestamp": 0.29007449606132074
+        },
+        {
+            "x": 7.68,
+            "y": 6.687447350831621,
+            "heading": -5.912144198792796e-19,
+            "angularVelocity": -2.3792281637439373e-18,
+            "velocityX": -5.242727749103024e-21,
+            "velocityY": -3.224906490772764,
+            "timestamp": 0.38676599474842766
+        },
+        {
+            "x": 7.68,
+            "y": 6.321669157855709,
+            "heading": -4.100561450064452e-17,
+            "angularVelocity": -4.179726307846966e-16,
+            "velocityX": -1.166132844762965e-20,
+            "velocityY": -3.782940568121389,
+            "timestamp": 0.48345749343553457
+        },
+        {
+            "x": 7.68,
+            "y": 5.955890964879757,
+            "heading": -6.040572418063157e-17,
+            "angularVelocity": -2.0063924898701218e-16,
+            "velocityX": -1.9392305823460385e-22,
+            "velocityY": -3.782940568121787,
+            "timestamp": 0.5801489921226415
+        },
+        {
+            "x": 7.68,
+            "y": 5.590112771903805,
+            "heading": -8.327536807485113e-17,
+            "angularVelocity": -2.365217646302656e-16,
+            "velocityX": 3.675624932403311e-22,
+            "velocityY": -3.782940568121787,
+            "timestamp": 0.6768404908097484
+        },
+        {
+            "x": 7.68,
+            "y": 5.224334578927854,
+            "heading": -7.575439540555723e-17,
+            "angularVelocity": 7.778318436801094e-17,
+            "velocityX": -2.1057162419585226e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 0.7735319894968553
+        },
+        {
+            "x": 7.68,
+            "y": 4.858556385951902,
+            "heading": -4.8431591576069396e-17,
+            "angularVelocity": 2.82577105541659e-16,
+            "velocityX": 1.016892835365518e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 0.8702234881839622
+        },
+        {
+            "x": 7.68,
+            "y": 4.49277819297595,
+            "heading": -7.892411277004063e-17,
+            "angularVelocity": -3.1535886409915893e-16,
+            "velocityX": 1.448731313682566e-20,
+            "velocityY": -3.782940568121787,
+            "timestamp": 0.9669149868710691
+        },
+        {
+            "x": 7.68,
+            "y": 4.126999999999999,
+            "heading": -8.668140475632877e-17,
+            "angularVelocity": -8.022723912254832e-17,
+            "velocityX": 2.655349226445793e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 1.063606485558176
+        },
+        {
+            "x": 7.68,
+            "y": 3.7612218070240475,
+            "heading": -4.236036305527668e-17,
+            "angularVelocity": 4.583757859030679e-16,
+            "velocityX": 7.500561292003624e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 1.160297984245283
+        },
+        {
+            "x": 7.68,
+            "y": 3.395443614048096,
+            "heading": -2.060715967072721e-17,
+            "angularVelocity": 2.249753461257506e-16,
+            "velocityX": 1.3311646441840309e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 1.25698948293239
+        },
+        {
+            "x": 7.68,
+            "y": 3.0296654210721448,
+            "heading": -1.7378554757194933e-17,
+            "angularVelocity": 3.3390783650741015e-17,
+            "velocityX": 1.5677048108756238e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 1.353680981619497
+        },
+        {
+            "x": 7.68,
+            "y": 2.663887228096194,
+            "heading": -2.7058170522872628e-18,
+            "angularVelocity": 1.5174796030816139e-16,
+            "velocityX": 2.708497475030379e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 1.450372480306604
+        },
+        {
+            "x": 7.68,
+            "y": 2.2981090351202424,
+            "heading": -9.881529062229235e-18,
+            "angularVelocity": -7.421243963841227e-17,
+            "velocityX": 2.3345387345088647e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 1.547063978993711
+        },
+        {
+            "x": 7.68,
+            "y": 1.9323308421442913,
+            "heading": -6.5997498705085955e-18,
+            "angularVelocity": 3.39407211211041e-17,
+            "velocityX": -6.072343928076645e-20,
+            "velocityY": -3.782940568121787,
+            "timestamp": 1.643755477680818
+        },
+        {
+            "x": 7.68,
+            "y": 1.5665526491683786,
+            "heading": 6.487893960505722e-19,
+            "angularVelocity": 7.496563157826373e-17,
+            "velocityX": 5.228734565126193e-20,
+            "velocityY": -3.782940568121389,
+            "timestamp": 1.7404469763679251
+        },
+        {
+            "x": 7.68,
+            "y": 1.254731607449781,
+            "heading": 3.9009642634108723e-19,
+            "angularVelocity": -2.6754468948553036e-18,
+            "velocityX": 1.8077268140623735e-21,
+            "velocityY": -3.224906490772764,
+            "timestamp": 1.8371384750550321
+        },
+        {
+            "x": 7.68,
+            "y": 1.020865808366814,
+            "heading": 1.9475770164363563e-19,
+            "angularVelocity": -2.020226466917104e-18,
+            "velocityX": 1.3557598034014801e-21,
+            "velocityY": -2.418680052108357,
+            "timestamp": 1.9338299737421392
+        },
+        {
+            "x": 7.68,
+            "y": 0.8649552704871435,
+            "heading": 6.488190339479676e-20,
+            "angularVelocity": -1.3431976959152128e-18,
+            "velocityX": 9.038296351119003e-22,
+            "velocityY": -1.6124534214139752,
+            "timestamp": 2.030521472429246
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": -6.710197305720325e-19,
+            "velocityX": 4.519117474728098e-22,
+            "velocityY": -0.8062267267095136,
+            "timestamp": 2.127212971116353
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 1.0656726462878492e-32,
+            "velocityY": 0,
+            "timestamp": 2.2239044698034602
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C1 To S1.traj b/src/main/deploy/choreo/C1 To S1.traj
new file mode 100644
index 00000000..ce3947a6
--- /dev/null
+++ b/src/main/deploy/choreo/C1 To S1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -7.649625564275916e-33,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.617797764168564,
+            "y": 7.446201717117849,
+            "heading": 0.013442479376395878,
+            "angularVelocity": 0.15144225686963014,
+            "velocityX": -0.7007670767707749,
+            "velocityY": -0.23431234749607086,
+            "timestamp": 0.08876306820531243
+        },
+        {
+            "x": 7.493368305965553,
+            "y": 7.404602586388856,
+            "heading": 0.03870227011836747,
+            "angularVelocity": 0.28457545746724416,
+            "velocityX": -1.401815650572125,
+            "velocityY": -0.4686535917478179,
+            "timestamp": 0.17752613641062487
+        },
+        {
+            "x": 7.30667604944407,
+            "y": 7.3422008161122045,
+            "heading": 0.0730276629215222,
+            "angularVelocity": 0.38670804756608035,
+            "velocityX": -2.1032650210965835,
+            "velocityY": -0.7030150212056434,
+            "timestamp": 0.2662892046159373
+        },
+        {
+            "x": 7.057674574154849,
+            "y": 7.259002273186937,
+            "heading": 0.11075765125141449,
+            "angularVelocity": 0.42506403948785515,
+            "velocityX": -2.8052373619302045,
+            "velocityY": -0.9373103544914424,
+            "timestamp": 0.35505227282124974
+        },
+        {
+            "x": 6.746464870184761,
+            "y": 7.155111739606062,
+            "heading": 0.13339737013954753,
+            "angularVelocity": 0.25505786749611997,
+            "velocityX": -3.5060719538216505,
+            "velocityY": -1.170425219424191,
+            "timestamp": 0.44381534102656217
+        },
+        {
+            "x": 6.428047828205506,
+            "y": 7.048517057389171,
+            "heading": 0.13339738106725169,
+            "angularVelocity": 1.2311157711164836e-7,
+            "velocityX": -3.5872694400628884,
+            "velocityY": -1.2008900139638257,
+            "timestamp": 0.5325784092318746
+        },
+        {
+            "x": 6.109630786224927,
+            "y": 6.941922375176235,
+            "heading": 0.1333973919949535,
+            "angularVelocity": 1.23111550991103e-7,
+            "velocityX": -3.587269440077816,
+            "velocityY": -1.200890013919235,
+            "timestamp": 0.621341477437187
+        },
+        {
+            "x": 5.798381397539494,
+            "y": 6.838018370585249,
+            "heading": 0.15626352052401435,
+            "angularVelocity": 0.2576085864583032,
+            "velocityX": -3.5065190397107555,
+            "velocityY": -1.1705769831075976,
+            "timestamp": 0.7101045456424995
+        },
+        {
+            "x": 5.5493631968567065,
+            "y": 6.754814060290535,
+            "heading": 0.1928079538664876,
+            "angularVelocity": 0.4117076401528937,
+            "velocityX": -2.8054257893230465,
+            "velocityY": -0.9373753293689796,
+            "timestamp": 0.7988676138478119
+        },
+        {
+            "x": 5.362653942099421,
+            "y": 6.692406067264648,
+            "heading": 0.22585011955614995,
+            "angularVelocity": 0.37225127926004886,
+            "velocityX": -2.1034565223155943,
+            "velocityY": -0.703085126367409,
+            "timestamp": 0.8876306820531243
+        },
+        {
+            "x": 5.238210383284219,
+            "y": 6.650801525067523,
+            "heading": 0.25010704549903756,
+            "angularVelocity": 0.2732772360607878,
+            "velocityX": -1.4019745073178984,
+            "velocityY": -0.4687145570597535,
+            "timestamp": 0.9763937502584368
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.263,
+            "angularVelocity": 0.1452513388947291,
+            "velocityX": -0.7008588655398115,
+            "velocityY": -0.2343488737838604,
+            "timestamp": 1.0651568184637492
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.263,
+            "angularVelocity": -2.2743517080702824e-31,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 1.1539198866690616
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C1 To S2.traj b/src/main/deploy/choreo/C1 To S2.traj
new file mode 100644
index 00000000..f95228f2
--- /dev/null
+++ b/src/main/deploy/choreo/C1 To S2.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -6.443666081756986e-29,
+            "angularVelocity": 1.4558689567742243e-28,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.593939912495806,
+            "y": 7.445359803187238,
+            "heading": -0.04618112566514864,
+            "angularVelocity": -0.44533178373248467,
+            "velocityX": -0.8298908206418381,
+            "velocityY": -0.2086797865609081,
+            "timestamp": 0.10370049332228601
+        },
+        {
+            "x": 7.421570715628637,
+            "y": 7.402025139488172,
+            "heading": -0.13159426902714605,
+            "angularVelocity": -0.823652237569843,
+            "velocityX": -1.662182997832739,
+            "velocityY": -0.41788290788915955,
+            "timestamp": 0.20740098664457202
+        },
+        {
+            "x": 7.162505124622784,
+            "y": 7.336932458231965,
+            "heading": -0.24235041158328322,
+            "angularVelocity": -1.0680387239038096,
+            "velocityX": -2.4982098224038025,
+            "velocityY": -0.627698858229146,
+            "timestamp": 0.311101479966858
+        },
+        {
+            "x": 6.816624735331939,
+            "y": 7.250238713519174,
+            "heading": -0.3315039987658666,
+            "angularVelocity": -0.8597219196007858,
+            "velocityX": -3.3353784365894983,
+            "velocityY": -0.8360012757447058,
+            "timestamp": 0.41480197328914403
+        },
+        {
+            "x": 6.436297385949381,
+            "y": 7.154089090014683,
+            "heading": -0.3315040257643058,
+            "angularVelocity": -2.603501519346576e-7,
+            "velocityX": -3.6675558350581356,
+            "velocityY": -0.927185786914993,
+            "timestamp": 0.5185024666114301
+        },
+        {
+            "x": 6.055970043958053,
+            "y": 7.05793943727301,
+            "heading": -0.33150405276269507,
+            "angularVelocity": -2.60349670609762e-7,
+            "velocityX": -3.6675557637833287,
+            "velocityY": -0.9271860688537472,
+            "timestamp": 0.6222029599337161
+        },
+        {
+            "x": 5.675642817406181,
+            "y": 6.961789327902759,
+            "heading": -0.33150407976108204,
+            "angularVelocity": -2.6034964813703677e-7,
+            "velocityX": -3.6675546505826837,
+            "velocityY": -0.9271904721942118,
+            "timestamp": 0.7259034532560021
+        },
+        {
+            "x": 5.306931756724127,
+            "y": 6.827827408155551,
+            "heading": -0.331504107569482,
+            "angularVelocity": -2.6816073038858223e-7,
+            "velocityX": -3.5555381548297533,
+            "velocityY": -1.2918156457642633,
+            "timestamp": 0.8296039465782882
+        },
+        {
+            "x": 4.97827117667767,
+            "y": 6.613640897314209,
+            "heading": -0.3315041419146958,
+            "angularVelocity": -3.311962425493206e-7,
+            "velocityX": -3.1693251354651193,
+            "velocityY": -2.065433866121419,
+            "timestamp": 0.9333044399005742
+        },
+        {
+            "x": 4.706831845168329,
+            "y": 6.330419557819514,
+            "heading": -0.3315041418010325,
+            "angularVelocity": 1.0960729474265148e-9,
+            "velocityX": -2.6175317282796997,
+            "velocityY": -2.7311474653691747,
+            "timestamp": 1.0370049332228601
+        },
+        {
+            "x": 4.518922560808715,
+            "y": 6.028421366315536,
+            "heading": -0.2325817741817243,
+            "angularVelocity": 0.9539237900428917,
+            "velocityX": -1.8120384806234937,
+            "velocityY": -2.912215572257732,
+            "timestamp": 1.1407054265451462
+        },
+        {
+            "x": 4.388098262013575,
+            "y": 5.795527878516759,
+            "heading": -0.12496957799500227,
+            "angularVelocity": 1.0377211596504115,
+            "velocityX": -1.2615590784949995,
+            "velocityY": -2.245828157007679,
+            "timestamp": 1.2444059198674322
+        },
+        {
+            "x": 4.304052100915554,
+            "y": 5.638778154553602,
+            "heading": -0.0435889655527213,
+            "angularVelocity": 0.7847659141732163,
+            "velocityX": -0.8104702148022851,
+            "velocityY": -1.5115619891604988,
+            "timestamp": 1.3481064131897182
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 7.875266725539043e-28,
+            "angularVelocity": 0.42033518025077415,
+            "velocityX": -0.39587179964482383,
+            "velocityY": -0.7596700076322139,
+            "timestamp": 1.4518069065120043
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": -1.3117957600781845e-26,
+            "velocityX": -2.8963805377195657e-28,
+            "velocityY": 0,
+            "timestamp": 1.5555073998342903
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C1 To S3.traj b/src/main/deploy/choreo/C1 To S3.traj
new file mode 100644
index 00000000..f3e004b9
--- /dev/null
+++ b/src/main/deploy/choreo/C1 To S3.traj	
@@ -0,0 +1,265 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 3.237981257278863e-31,
+            "velocityX": 3.6994687797817426e-32,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.663124715083084,
+            "y": 7.408544235319849,
+            "heading": 0.027485198347684953,
+            "angularVelocity": 0.3204918925418427,
+            "velocityX": -0.19677471240300645,
+            "velocityY": -0.6816250119548423,
+            "timestamp": 0.08575941852911824
+        },
+        {
+            "x": 7.629347887601235,
+            "y": 7.291532761279695,
+            "heading": 0.07929730291499706,
+            "angularVelocity": 0.6041564350126787,
+            "velocityX": -0.3938556028149893,
+            "velocityY": -1.3644154315298311,
+            "timestamp": 0.17151883705823648
+        },
+        {
+            "x": 7.578636110956943,
+            "y": 7.115823334781324,
+            "heading": 0.15015093083574435,
+            "angularVelocity": 0.8261906288076145,
+            "velocityX": -0.5913260317532774,
+            "velocityY": -2.048864480566788,
+            "timestamp": 0.25727825558735473
+        },
+        {
+            "x": 7.510961813450331,
+            "y": 6.881225838812792,
+            "heading": 0.22908991381513497,
+            "angularVelocity": 0.9204701283345157,
+            "velocityX": -0.7891179612374983,
+            "velocityY": -2.7355303941208287,
+            "timestamp": 0.34303767411647296
+        },
+        {
+            "x": 7.426596388399054,
+            "y": 6.588221528517866,
+            "heading": 0.2780284674480044,
+            "angularVelocity": 0.5706493172671544,
+            "velocityX": -0.9837453016618997,
+            "velocityY": -3.4165846191627605,
+            "timestamp": 0.4287970926455912
+        },
+        {
+            "x": 7.336364964950643,
+            "y": 6.276599275391096,
+            "heading": 0.2780284905344079,
+            "angularVelocity": 2.691996271350766e-7,
+            "velocityX": -1.0521459333096546,
+            "velocityY": -3.6336796409243712,
+            "timestamp": 0.5145565111747095
+        },
+        {
+            "x": 7.246133490581996,
+            "y": 5.964977037008454,
+            "heading": 0.27802851362075814,
+            "angularVelocity": 2.6919900573505555e-7,
+            "velocityX": -1.0521465270664079,
+            "velocityY": -3.633679469000075,
+            "timestamp": 0.6003159297038277
+        },
+        {
+            "x": 7.155901958204915,
+            "y": 5.653354815422387,
+            "heading": 0.2780285367070952,
+            "angularVelocity": 2.6919885218329885e-7,
+            "velocityX": -1.0521472034753196,
+            "velocityY": -3.633679273143183,
+            "timestamp": 0.686075348232946
+        },
+        {
+            "x": 7.06567006903972,
+            "y": 5.341732697146042,
+            "heading": 0.27802855979342844,
+            "angularVelocity": 2.6919880800587287e-7,
+            "velocityX": -1.052151363812688,
+            "velocityY": -3.6336780684973866,
+            "timestamp": 0.7718347667620643
+        },
+        {
+            "x": 6.941702537617662,
+            "y": 5.041929112902265,
+            "heading": 0.2780285835706332,
+            "angularVelocity": 2.772547333115093e-7,
+            "velocityX": -1.445526725207054,
+            "velocityY": -3.495867735413615,
+            "timestamp": 0.8575941852911826
+        },
+        {
+            "x": 6.763533066668612,
+            "y": 4.770809627708675,
+            "heading": 0.2780286115416881,
+            "angularVelocity": 3.2615723612466423e-7,
+            "velocityX": -2.0775498948673006,
+            "velocityY": -3.1613960290733045,
+            "timestamp": 0.9433536038203009
+        },
+        {
+            "x": 6.537527544905599,
+            "y": 4.538061625407243,
+            "heading": 0.2780286466069666,
+            "angularVelocity": 4.0887962009830865e-7,
+            "velocityX": -2.6353434484432303,
+            "velocityY": -2.713964323608448,
+            "timestamp": 1.0291130223494191
+        },
+        {
+            "x": 6.274440682462666,
+            "y": 4.354088811409382,
+            "heading": 0.26884633764727317,
+            "angularVelocity": -0.10707055991260324,
+            "velocityX": -3.0677314160380695,
+            "velocityY": -2.145219932145364,
+            "timestamp": 1.1148724408785373
+        },
+        {
+            "x": 5.982323590323029,
+            "y": 4.22411616098336,
+            "heading": 0.2547621599051439,
+            "angularVelocity": -0.16422893232822805,
+            "velocityX": -3.40623918806603,
+            "velocityY": -1.5155495764222278,
+            "timestamp": 1.2006318594076555
+        },
+        {
+            "x": 5.6751302181547745,
+            "y": 4.153390532297334,
+            "heading": 0.22346642432676106,
+            "angularVelocity": -0.3649247641266729,
+            "velocityX": -3.582036555716069,
+            "velocityY": -0.8246980902979518,
+            "timestamp": 1.2863912779367737
+        },
+        {
+            "x": 5.366531848907471,
+            "y": 4.105798244476318,
+            "heading": 0.18319904977282217,
+            "angularVelocity": -0.4695388010388101,
+            "velocityX": -3.5984195618411468,
+            "velocityY": -0.5549511486584576,
+            "timestamp": 1.3721506964658918
+        },
+        {
+            "x": 5.15360011207683,
+            "y": 4.041515222282991,
+            "heading": 0.15622154454817708,
+            "angularVelocity": -0.44121073940112593,
+            "velocityX": -3.482448368249538,
+            "velocityY": -1.051333676582719,
+            "timestamp": 1.4332949588681712
+        },
+        {
+            "x": 4.961585993321137,
+            "y": 3.955708543808847,
+            "heading": 0.09940090208560654,
+            "angularVelocity": -0.9292882149552651,
+            "velocityX": -3.1403456548775974,
+            "velocityY": -1.403348001969601,
+            "timestamp": 1.4944392212704505
+        },
+        {
+            "x": 4.792433392014809,
+            "y": 3.849704139398821,
+            "heading": 0.01980990286006859,
+            "angularVelocity": -1.301692032882722,
+            "velocityX": -2.766450925410501,
+            "velocityY": -1.7336770490843985,
+            "timestamp": 1.5555834836727298
+        },
+        {
+            "x": 4.64716063510186,
+            "y": 3.7242164869664776,
+            "heading": -0.07646566712737501,
+            "angularVelocity": -1.5745642551712946,
+            "velocityX": -2.3759016987918233,
+            "velocityY": -2.0523209783239644,
+            "timestamp": 1.6167277460750091
+        },
+        {
+            "x": 4.526373969972362,
+            "y": 3.5797426645365276,
+            "heading": -0.18496768957846801,
+            "angularVelocity": -1.7745250034621045,
+            "velocityX": -1.975437438999914,
+            "velocityY": -2.362835313629761,
+            "timestamp": 1.6778720084772885
+        },
+        {
+            "x": 4.435318488505818,
+            "y": 3.4263170797911884,
+            "heading": -0.3033986089581413,
+            "angularVelocity": -1.936909772506439,
+            "velocityX": -1.489190937777188,
+            "velocityY": -2.5092392763841707,
+            "timestamp": 1.7390162708795678
+        },
+        {
+            "x": 4.37042278768838,
+            "y": 3.2883178140362905,
+            "heading": -0.39683380701899557,
+            "angularVelocity": -1.5281106417823267,
+            "velocityX": -1.0613538910728308,
+            "velocityY": -2.2569454652502787,
+            "timestamp": 1.800160533281847
+        },
+        {
+            "x": 4.3241368483863285,
+            "y": 3.1745112953889034,
+            "heading": -0.4672868225779202,
+            "angularVelocity": -1.1522424638210744,
+            "velocityX": -0.7569956277749857,
+            "velocityY": -1.8612787884925825,
+            "timestamp": 1.8613047956841264
+        },
+        {
+            "x": 4.292271472981756,
+            "y": 3.08778416242541,
+            "heading": -0.5177338397629753,
+            "angularVelocity": -0.8250490757931602,
+            "velocityX": -0.5211507041318677,
+            "velocityY": -1.418401818193496,
+            "timestamp": 1.9224490580864058
+        },
+        {
+            "x": 4.272405555044274,
+            "y": 3.0293931910282037,
+            "heading": -0.5502025950282511,
+            "angularVelocity": -0.5310188395381721,
+            "velocityX": -0.3249024055075056,
+            "velocityY": -0.9549705745576195,
+            "timestamp": 1.983593320488685
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -0.25836283489389134,
+            "velocityX": -0.15382563587721435,
+            "velocityY": -0.48071871134564137,
+            "timestamp": 2.0447375828909644
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 0,
+            "velocityX": 3.3381626772503296e-32,
+            "velocityY": -2.986960154995716e-30,
+            "timestamp": 2.105881845293244
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C1 To S4.traj b/src/main/deploy/choreo/C1 To S4.traj
new file mode 100644
index 00000000..6d9a6add
--- /dev/null
+++ b/src/main/deploy/choreo/C1 To S4.traj	
@@ -0,0 +1,301 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 9.776523821034258e-32,
+            "velocityX": -4.560355515853987e-33,
+            "velocityY": 2.3850698853192112e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 7.67836355889096,
+            "y": 7.400720406813562,
+            "heading": -0.028945523101477338,
+            "angularVelocity": -0.3234212568317029,
+            "velocityX": -0.018284687354286195,
+            "velocityY": -0.7405714954779761,
+            "timestamp": 0.0894978993806123
+        },
+        {
+            "x": 7.675074058245281,
+            "y": 7.268033905727148,
+            "heading": -0.0827237859595526,
+            "angularVelocity": -0.6008885485609575,
+            "velocityX": -0.03675505982201489,
+            "velocityY": -1.4825655351097347,
+            "timestamp": 0.1789957987612246
+        },
+        {
+            "x": 7.670107822881543,
+            "y": 7.068767684711926,
+            "heading": -0.1543450175390465,
+            "angularVelocity": -0.8002560068466026,
+            "velocityX": -0.05548996566520156,
+            "velocityY": -2.226490480718353,
+            "timestamp": 0.2684936981418369
+        },
+        {
+            "x": 7.663417838093003,
+            "y": 6.802742204879413,
+            "heading": -0.22912917227010834,
+            "angularVelocity": -0.8355967597967521,
+            "velocityX": -0.07475018782384167,
+            "velocityY": -2.9724214945109684,
+            "timestamp": 0.3579915975224492
+        },
+        {
+            "x": 7.654753648982881,
+            "y": 6.471615180012679,
+            "heading": -0.2502157544796387,
+            "angularVelocity": -0.23560980040272358,
+            "velocityX": -0.0968088543986332,
+            "velocityY": -3.6998301318619107,
+            "timestamp": 0.4474894969030615
+        },
+        {
+            "x": 7.646619499365586,
+            "y": 6.133147678353669,
+            "heading": -0.250215779137968,
+            "angularVelocity": -2.7551852588992195e-7,
+            "velocityX": -0.09088648642693918,
+            "velocityY": -3.78184855735655,
+            "timestamp": 0.5369873962836738
+        },
+        {
+            "x": 7.638485287213415,
+            "y": 5.7946801781975115,
+            "heading": -0.25021580379628167,
+            "angularVelocity": -2.755183512776458e-7,
+            "velocityX": -0.09088718515702116,
+            "velocityY": -3.7818485405645093,
+            "timestamp": 0.626485295664286
+        },
+        {
+            "x": 7.630351002745582,
+            "y": 5.456212679779282,
+            "heading": -0.2502158284545949,
+            "angularVelocity": -2.7551834628402704e-7,
+            "velocityX": -0.09088799317222593,
+            "velocityY": -3.781848521145859,
+            "timestamp": 0.7159831950448983
+        },
+        {
+            "x": 7.622216641973054,
+            "y": 5.117745183194865,
+            "heading": -0.25021585311291117,
+            "angularVelocity": -2.7551838019723297e-7,
+            "velocityX": -0.09088884575864668,
+            "velocityY": -3.781848500655846,
+            "timestamp": 0.8054810944255105
+        },
+        {
+            "x": 7.614082202627753,
+            "y": 4.779277688498788,
+            "heading": -0.25021587777122717,
+            "angularVelocity": -2.755183772950637e-7,
+            "velocityX": -0.09088972368733664,
+            "velocityY": -3.7818484795565834,
+            "timestamp": 0.8949789938061228
+        },
+        {
+            "x": 7.605947683187911,
+            "y": 4.440810195727643,
+            "heading": -0.2502159024295383,
+            "angularVelocity": -2.755183223713384e-7,
+            "velocityX": -0.09089061861940764,
+            "velocityY": -3.7818484580484664,
+            "timestamp": 0.984476893186735
+        },
+        {
+            "x": 7.597813082423135,
+            "y": 4.1023427049110195,
+            "heading": -0.25021592708784146,
+            "angularVelocity": -2.755182337798815e-7,
+            "velocityX": -0.09089152729921718,
+            "velocityY": -3.781848436209727,
+            "timestamp": 1.0739747925673473
+        },
+        {
+            "x": 7.589678399478048,
+            "y": 3.7638752160694957,
+            "heading": -0.2502159517461411,
+            "angularVelocity": -2.755181940361221e-7,
+            "velocityX": -0.09089244553653594,
+            "velocityY": -3.781848414141052,
+            "timestamp": 1.1634726919479597
+        },
+        {
+            "x": 7.581543633206817,
+            "y": 3.4254077292306317,
+            "heading": -0.2502159764044425,
+            "angularVelocity": -2.7551821391136536e-7,
+            "velocityX": -0.09089337657676429,
+            "velocityY": -3.7818483917644334,
+            "timestamp": 1.252970591328572
+        },
+        {
+            "x": 7.573407384740196,
+            "y": 3.0869402780182864,
+            "heading": -0.2502160010627749,
+            "angularVelocity": -2.7551856112041726e-7,
+            "velocityX": -0.09090993780780313,
+            "velocityY": -3.7818479936934355,
+            "timestamp": 1.3424684907091844
+        },
+        {
+            "x": 7.507089727245593,
+            "y": 2.754933679576005,
+            "heading": -0.25021602759782946,
+            "angularVelocity": -2.964880153822211e-7,
+            "velocityX": -0.7409968049928212,
+            "velocityY": -3.709658000243552,
+            "timestamp": 1.4319663900897968
+        },
+        {
+            "x": 7.3768883522731565,
+            "y": 2.4424052627551904,
+            "heading": -0.2502160612558337,
+            "angularVelocity": -3.7607591242222915e-7,
+            "velocityX": -1.454798111168213,
+            "velocityY": -3.492019577931195,
+            "timestamp": 1.5214642894704091
+        },
+        {
+            "x": 7.1878697455519145,
+            "y": 2.1615167326419513,
+            "heading": -0.2502161148657548,
+            "angularVelocity": -5.990075908840489e-7,
+            "velocityX": -2.111989309574672,
+            "velocityY": -3.1384929932119427,
+            "timestamp": 1.6109621888510215
+        },
+        {
+            "x": 6.94738938516625,
+            "y": 1.9231986374023808,
+            "heading": -0.25021645217340494,
+            "angularVelocity": -0.000003768889018580663,
+            "velocityX": -2.686994466350113,
+            "velocityY": -2.6628345121941095,
+            "timestamp": 1.7004600882316339
+        },
+        {
+            "x": 6.667655944824219,
+            "y": 1.738860011100769,
+            "heading": -0.25893161706240975,
+            "angularVelocity": -0.09737842954215001,
+            "velocityX": -3.125586659329227,
+            "velocityY": -2.059697798242895,
+            "timestamp": 1.7899579876122462
+        },
+        {
+            "x": 6.480451878153193,
+            "y": 1.646736643588724,
+            "heading": -0.268441983388845,
+            "angularVelocity": -0.16928620019223647,
+            "velocityX": -3.3322654479868237,
+            "velocityY": -1.6398122111953808,
+            "timestamp": 1.846137205502623
+        },
+        {
+            "x": 6.284980922842322,
+            "y": 1.5795583069683912,
+            "heading": -0.28291661109989463,
+            "angularVelocity": -0.25765092955355146,
+            "velocityX": -3.47941752575298,
+            "velocityY": -1.1957862558966221,
+            "timestamp": 1.9023164233929999
+        },
+        {
+            "x": 6.086191677834544,
+            "y": 1.5383458560749974,
+            "heading": -0.3070406119029933,
+            "angularVelocity": -0.429411474723066,
+            "velocityX": -3.53848366838567,
+            "velocityY": -0.7335889042423714,
+            "timestamp": 1.9584956412833767
+        },
+        {
+            "x": 5.891572555778559,
+            "y": 1.5225106097486452,
+            "heading": -0.35143250880916643,
+            "angularVelocity": -0.7901836047765775,
+            "velocityX": -3.4642547433776456,
+            "velocityY": -0.2818701811985366,
+            "timestamp": 2.0146748591737533
+        },
+        {
+            "x": 5.713348199791213,
+            "y": 1.5270062964148978,
+            "heading": -0.407385332387855,
+            "angularVelocity": -0.9959701412692558,
+            "velocityX": -3.172425011952234,
+            "velocityY": 0.08002401662169539,
+            "timestamp": 2.07085407706413
+        },
+        {
+            "x": 5.558872974287929,
+            "y": 1.5428236796222032,
+            "heading": -0.4653888399418059,
+            "angularVelocity": -1.0324726781908529,
+            "velocityX": -2.7496862951120815,
+            "velocityY": 0.2815522145247918,
+            "timestamp": 2.1270332949545065
+        },
+        {
+            "x": 5.4303297125098124,
+            "y": 1.5629685460150702,
+            "heading": -0.519265179295127,
+            "angularVelocity": -0.9590083553396718,
+            "velocityX": -2.2880927610801525,
+            "velocityY": 0.35858217948453525,
+            "timestamp": 2.183212512844883
+        },
+        {
+            "x": 5.3280145107455015,
+            "y": 1.583083454211334,
+            "heading": -0.5654730488836437,
+            "angularVelocity": -0.8225082392332849,
+            "velocityX": -1.8212286608183241,
+            "velocityY": 0.3580489182942135,
+            "timestamp": 2.2393917307352598
+        },
+        {
+            "x": 5.251723840078425,
+            "y": 1.6004232571954151,
+            "heading": -0.6018676415285119,
+            "angularVelocity": -0.6478301765590008,
+            "velocityX": -1.3579874112157035,
+            "velocityY": 0.3086515554188953,
+            "timestamp": 2.2955709486256364
+        },
+        {
+            "x": 5.20115218978415,
+            "y": 1.613158164546165,
+            "heading": -0.6270407807035692,
+            "angularVelocity": -0.44808632302748147,
+            "velocityX": -0.9001843064628601,
+            "velocityY": 0.22668359989631479,
+            "timestamp": 2.351750166516013
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -0.2306763921441399,
+            "velocityX": -0.44771342016952703,
+            "velocityY": 0.12178587938311458,
+            "timestamp": 2.4079293844063896
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 3.4924263364045203e-31,
+            "velocityX": 0,
+            "velocityY": 4.738614736798697e-32,
+            "timestamp": 2.464108602296766
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C1 To W1.traj b/src/main/deploy/choreo/C1 To W1.traj
new file mode 100644
index 00000000..1a381a4d
--- /dev/null
+++ b/src/main/deploy/choreo/C1 To W1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 3.136513796099116e-33,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.613270105003341,
+            "y": 7.458186274184093,
+            "heading": 0.022686687743237347,
+            "angularVelocity": 0.25194782208363986,
+            "velocityX": -0.7410712353675238,
+            "velocityY": -0.0978811472565877,
+            "timestamp": 0.09004518298914281
+        },
+        {
+            "x": 7.479732366880497,
+            "y": 7.440537508755927,
+            "heading": 0.06475240251532689,
+            "angularVelocity": 0.4671622998107697,
+            "velocityX": -1.483008126475186,
+            "velocityY": -0.19599899564082535,
+            "timestamp": 0.18009036597828562
+        },
+        {
+            "x": 7.2792817573683966,
+            "y": 7.414022951557291,
+            "heading": 0.12053408331695856,
+            "angularVelocity": 0.6194854510802376,
+            "velocityX": -2.226111412714547,
+            "velocityY": -0.2944583632178676,
+            "timestamp": 0.2701355489674284
+        },
+        {
+            "x": 7.01181645042657,
+            "y": 7.378594749595234,
+            "heading": 0.17807816149187164,
+            "angularVelocity": 0.6390578181383835,
+            "velocityX": -2.9703455316879728,
+            "velocityY": -0.39344916391950197,
+            "timestamp": 0.36018073195657124
+        },
+        {
+            "x": 6.678421151155383,
+            "y": 7.334256434894407,
+            "heading": 0.1920911587056688,
+            "angularVelocity": 0.15562184170903165,
+            "velocityX": -3.7025334193766524,
+            "velocityY": -0.49240073959507324,
+            "timestamp": 0.45022591494571407
+        },
+        {
+            "x": 6.340697514229505,
+            "y": 7.289811670135368,
+            "heading": 0.1920911781237931,
+            "angularVelocity": 2.1564867368321945e-7,
+            "velocityX": -3.7506019279965175,
+            "velocityY": -0.49358292452353475,
+            "timestamp": 0.5402710979348568
+        },
+        {
+            "x": 6.002973877037569,
+            "y": 7.245366907398008,
+            "heading": 0.19209119754191162,
+            "angularVelocity": 2.1564861007875057e-7,
+            "velocityX": -3.750601930951228,
+            "velocityY": -0.49358290207171196,
+            "timestamp": 0.6303162809239996
+        },
+        {
+            "x": 5.665250239845632,
+            "y": 7.200922144660658,
+            "heading": 0.19209121696003015,
+            "angularVelocity": 2.1564860966767442e-7,
+            "velocityX": -3.7506019309512406,
+            "velocityY": -0.493582902071613,
+            "timestamp": 0.7203614639131424
+        },
+        {
+            "x": 5.327526602653695,
+            "y": 7.156477381923307,
+            "heading": 0.19209123637814873,
+            "angularVelocity": 2.1564861026621434e-7,
+            "velocityX": -3.7506019309512415,
+            "velocityY": -0.493582902071613,
+            "timestamp": 0.8104066469022851
+        },
+        {
+            "x": 4.989802965461758,
+            "y": 7.112032619185957,
+            "heading": 0.19209125579626718,
+            "angularVelocity": 2.156486089426075e-7,
+            "velocityX": -3.7506019309512415,
+            "velocityY": -0.49358290207161315,
+            "timestamp": 0.9004518298914279
+        },
+        {
+            "x": 4.652079328269822,
+            "y": 7.067587856448606,
+            "heading": 0.19209127521438563,
+            "angularVelocity": 2.156486089612396e-7,
+            "velocityX": -3.750601930951241,
+            "velocityY": -0.4935829020716131,
+            "timestamp": 0.9904970128805707
+        },
+        {
+            "x": 4.314355691077885,
+            "y": 7.023143093711255,
+            "heading": 0.19209129463250418,
+            "angularVelocity": 2.1564860999442945e-7,
+            "velocityX": -3.7506019309512415,
+            "velocityY": -0.4935829020716131,
+            "timestamp": 1.0805421958697135
+        },
+        {
+            "x": 3.976632053885947,
+            "y": 6.978698330973905,
+            "heading": 0.1920913140506227,
+            "angularVelocity": 2.1564860984546372e-7,
+            "velocityX": -3.750601930951241,
+            "velocityY": -0.49358290207161315,
+            "timestamp": 1.1705873788588563
+        },
+        {
+            "x": 3.638908416694012,
+            "y": 6.934253568236545,
+            "heading": 0.19209133346874124,
+            "angularVelocity": 2.1564860987115903e-7,
+            "velocityX": -3.750601930951227,
+            "velocityY": -0.4935829020717237,
+            "timestamp": 1.2606325618479992
+        },
+        {
+            "x": 3.3011847797995815,
+            "y": 6.8898088032385445,
+            "heading": 0.19209135288686577,
+            "angularVelocity": 2.1564867640744468e-7,
+            "velocityX": -3.7506019276472724,
+            "velocityY": -0.4935829271773418,
+            "timestamp": 1.350677744837142
+        },
+        {
+            "x": 2.967975868183031,
+            "y": 6.845440652800829,
+            "heading": 0.2065131865620181,
+            "angularVelocity": 0.16016218965195775,
+            "velocityX": -3.7004634846122446,
+            "velocityY": -0.492732081438091,
+            "timestamp": 1.440722927826285
+        },
+        {
+            "x": 2.7005740636525677,
+            "y": 6.810001078107782,
+            "heading": 0.266815731322117,
+            "angularVelocity": 0.6696920674520688,
+            "velocityX": -2.9696403033875236,
+            "velocityY": -0.39357546419024264,
+            "timestamp": 1.5307681108154279
+        },
+        {
+            "x": 2.5001864407535797,
+            "y": 6.78347721885534,
+            "heading": 0.32524318999567287,
+            "angularVelocity": 0.6488682318586739,
+            "velocityX": -2.2254119126300136,
+            "velocityY": -0.29456166750908425,
+            "timestamp": 1.6208132938045707
+        },
+        {
+            "x": 2.366700230698382,
+            "y": 6.765819830273621,
+            "heading": 0.3692681816828941,
+            "angularVelocity": 0.48892111966199864,
+            "velocityX": -1.4824358796771253,
+            "velocityY": -0.19609476038099796,
+            "timestamp": 1.7108584767937136
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0.26355455704906927,
+            "velocityX": -0.740741797442122,
+            "velocityY": -0.09794894053004785,
+            "timestamp": 1.8009036597828565
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0,
+            "velocityX": 6.927431243507319e-35,
+            "velocityY": 0,
+            "timestamp": 1.8909488427719994
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C1 To W2.traj b/src/main/deploy/choreo/C1 To W2.traj
new file mode 100644
index 00000000..28a53b93
--- /dev/null
+++ b/src/main/deploy/choreo/C1 To W2.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -2.4374604803646449e-32,
+            "velocityY": 2.350988701644575e-38,
+            "timestamp": 0
+        },
+        {
+            "x": 7.602872461914855,
+            "y": 7.439913035887961,
+            "heading": 5.3325436298288445e-18,
+            "angularVelocity": 5.3856284215509377e-17,
+            "velocityX": -0.7789533333445217,
+            "velocityY": -0.2735661154121556,
+            "timestamp": 0.09901432445765257
+        },
+        {
+            "x": 7.448617388685645,
+            "y": 7.385739108696781,
+            "heading": -4.4022234029758545e-18,
+            "angularVelocity": -9.831675377471393e-17,
+            "velocityX": -1.557906636985473,
+            "velocityY": -0.5471322203925048,
+            "timestamp": 0.19802864891530514
+        },
+        {
+            "x": 7.217234786194527,
+            "y": 7.304478220492258,
+            "heading": -7.053417683545123e-18,
+            "angularVelocity": -2.6775864730895626e-17,
+            "velocityX": -2.3368598812192953,
+            "velocityY": -0.8206983045092451,
+            "timestamp": 0.2970429733729577
+        },
+        {
+            "x": 6.9087246720879545,
+            "y": 7.196130377471774,
+            "heading": 2.6673179344101e-18,
+            "angularVelocity": 9.817504407722252e-17,
+            "velocityX": -3.1158129472318725,
+            "velocityY": -1.0942643260352094,
+            "timestamp": 0.3960572978306103
+        },
+        {
+            "x": 6.5553201862538,
+            "y": 7.072015763386003,
+            "heading": 8.41895708879283e-18,
+            "angularVelocity": 5.808896021749495e-17,
+            "velocityX": -3.5692258445423475,
+            "velocityY": -1.2535015995473784,
+            "timestamp": 0.49507162228826285
+        },
+        {
+            "x": 6.2019157004196215,
+            "y": 6.9479011493002245,
+            "heading": -3.69857396739816e-19,
+            "angularVelocity": -8.876306064099344e-17,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.2535015995474599,
+            "timestamp": 0.5940859467459154
+        },
+        {
+            "x": 5.848511214585444,
+            "y": 6.823786535214446,
+            "heading": -8.032367976884792e-18,
+            "angularVelocity": -7.738789939855791e-17,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.2535015995474599,
+            "timestamp": 0.693100271203568
+        },
+        {
+            "x": 5.495106728751266,
+            "y": 6.699671921128667,
+            "heading": -3.490709714785344e-17,
+            "angularVelocity": -2.714226382747745e-16,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.2535015995474599,
+            "timestamp": 0.7921145956612206
+        },
+        {
+            "x": 5.141702242917089,
+            "y": 6.575557307042889,
+            "heading": -1.2741337636064699e-17,
+            "angularVelocity": 2.238641694845762e-16,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.2535015995474599,
+            "timestamp": 0.8911289201188731
+        },
+        {
+            "x": 4.788297757082911,
+            "y": 6.4514426929571105,
+            "heading": -2.2645871045802615e-17,
+            "angularVelocity": -1.0003131833516608e-16,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.2535015995474597,
+            "timestamp": 0.9901432445765257
+        },
+        {
+            "x": 4.4348932712487334,
+            "y": 6.327328078871332,
+            "heading": -7.505924377434873e-18,
+            "angularVelocity": 1.529066299375997e-16,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.25350159954746,
+            "timestamp": 1.0891575690341782
+        },
+        {
+            "x": 4.081488785414556,
+            "y": 6.203213464785553,
+            "heading": -3.3249179069580955e-17,
+            "angularVelocity": -2.5999525658031657e-16,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.25350159954746,
+            "timestamp": 1.1881718934918308
+        },
+        {
+            "x": 3.728084299580378,
+            "y": 6.079098850699775,
+            "heading": -3.31250701341244e-17,
+            "angularVelocity": 1.2534442479544336e-18,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.2535015995474597,
+            "timestamp": 1.2871862179494835
+        },
+        {
+            "x": 3.3746798137461993,
+            "y": 5.9549842366139965,
+            "heading": -3.474935359347948e-17,
+            "angularVelocity": -1.6404530025230332e-17,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.2535015995474599,
+            "timestamp": 1.3862005424071362
+        },
+        {
+            "x": 3.021275327912045,
+            "y": 5.830869622528225,
+            "heading": -2.4652412430964293e-17,
+            "angularVelocity": 1.0197454982216833e-16,
+            "velocityX": -3.5692258445423475,
+            "velocityY": -1.2535015995473784,
+            "timestamp": 1.4852148668647889
+        },
+        {
+            "x": 2.7127652138054734,
+            "y": 5.722521779507741,
+            "heading": 1.414690860074665e-18,
+            "angularVelocity": 2.632659789801892e-16,
+            "velocityX": -3.115812947231872,
+            "velocityY": -1.0942643260352094,
+            "timestamp": 1.5842291913224416
+        },
+        {
+            "x": 2.4813826113143547,
+            "y": 5.641260891303218,
+            "heading": 8.307803002216867e-18,
+            "angularVelocity": 6.961732115429119e-17,
+            "velocityX": -2.336859881219295,
+            "velocityY": -0.8206983045092451,
+            "timestamp": 1.6832435157800942
+        },
+        {
+            "x": 2.3271275380851444,
+            "y": 5.587086964112038,
+            "heading": -3.972937436005726e-18,
+            "angularVelocity": -1.2402993582700068e-16,
+            "velocityX": -1.557906636985473,
+            "velocityY": -0.5471322203925048,
+            "timestamp": 1.782257840237747
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 4.012487544797503e-17,
+            "velocityX": -0.7789533333445219,
+            "velocityY": -0.2735661154121556,
+            "timestamp": 1.8812721646953996
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -5.265297803216144e-35,
+            "velocityY": 0,
+            "timestamp": 1.9802864891530523
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C1 To W3.traj b/src/main/deploy/choreo/C1 To W3.traj
new file mode 100644
index 00000000..96b5b24b
--- /dev/null
+++ b/src/main/deploy/choreo/C1 To W3.traj	
@@ -0,0 +1,274 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": -1.8418609272007183e-28,
+            "velocityX": -1.1316944174618986e-32,
+            "velocityY": -1.0903166764521522e-29,
+            "timestamp": 0
+        },
+        {
+            "x": 7.615058283165484,
+            "y": 7.4500526040017725,
+            "heading": -0.011528934363875379,
+            "angularVelocity": -0.12842736126317786,
+            "velocityX": -0.7234227436435784,
+            "velocityY": -0.18878668917675853,
+            "timestamp": 0.08977007898235294
+        },
+        {
+            "x": 7.485153650546927,
+            "y": 7.41615410854214,
+            "heading": -0.0328638632229319,
+            "angularVelocity": -0.23766191476385995,
+            "velocityX": -1.4470816344507456,
+            "velocityY": -0.3776146333375848,
+            "timestamp": 0.17954015796470588
+        },
+        {
+            "x": 7.290257872847477,
+            "y": 7.365300387326372,
+            "heading": -0.06109799257845879,
+            "angularVelocity": -0.3145160355888266,
+            "velocityX": -2.17105498746146,
+            "velocityY": -0.5664885426438214,
+            "timestamp": 0.2693102369470588
+        },
+        {
+            "x": 7.030342832843462,
+            "y": 7.297491054945299,
+            "heading": -0.0902650461777,
+            "angularVelocity": -0.32490840968750134,
+            "velocityX": -2.895341554228826,
+            "velocityY": -0.7553667452421856,
+            "timestamp": 0.35908031592941175
+        },
+        {
+            "x": 6.705594231461252,
+            "y": 7.212800171241254,
+            "heading": -0.10097705422378679,
+            "angularVelocity": -0.11932715407194204,
+            "velocityX": -3.617559492690666,
+            "velocityY": -0.9434199531077018,
+            "timestamp": 0.4488503949117647
+        },
+        {
+            "x": 6.377014064882571,
+            "y": 7.1270111914036525,
+            "heading": -0.10097706428057987,
+            "angularVelocity": -1.120283753063034e-7,
+            "velocityX": -3.6602414780458985,
+            "velocityY": -0.9556522708907337,
+            "timestamp": 0.5386204738941176
+        },
+        {
+            "x": 6.048433901032778,
+            "y": 7.0412222011141505,
+            "heading": -0.10097707433737516,
+            "angularVelocity": -1.1202840770590317e-7,
+            "velocityX": -3.660241447647229,
+            "velocityY": -0.9556523873204973,
+            "timestamp": 0.6283905528764705
+        },
+        {
+            "x": 5.719853740215168,
+            "y": 6.955433199211206,
+            "heading": -0.10097708439415136,
+            "angularVelocity": -1.1202819821797078e-7,
+            "velocityX": -3.6602414138700015,
+            "velocityY": -0.9556525166893554,
+            "timestamp": 0.7181606318588234
+        },
+        {
+            "x": 5.391273582926602,
+            "y": 6.86964418379178,
+            "heading": -0.100977094450907,
+            "angularVelocity": -1.1202797265919916e-7,
+            "velocityX": -3.660241374557919,
+            "velocityY": -0.9556526672572584,
+            "timestamp": 0.8079307108411763
+        },
+        {
+            "x": 5.062693429765362,
+            "y": 6.783855152564382,
+            "heading": -0.10097710450764096,
+            "angularVelocity": -1.1202773594839044e-7,
+            "velocityX": -3.6602413285812334,
+            "velocityY": -0.9556528433514181,
+            "timestamp": 0.8977007898235292
+        },
+        {
+            "x": 4.734113281444647,
+            "y": 6.698066102797359,
+            "heading": -0.10097711456435234,
+            "angularVelocity": -1.1202748993058552e-7,
+            "velocityX": -3.6602412746598207,
+            "velocityY": -0.9556530498750426,
+            "timestamp": 0.9874708688058821
+        },
+        {
+            "x": 4.4055331387963745,
+            "y": 6.612277031304317,
+            "heading": -0.10097712462104143,
+            "angularVelocity": -1.1202724880866421e-7,
+            "velocityX": -3.660241211471189,
+            "velocityY": -0.9556532918937597,
+            "timestamp": 1.077240947788235
+        },
+        {
+            "x": 4.076953002755506,
+            "y": 6.526487934504167,
+            "heading": -0.10097713467770983,
+            "angularVelocity": -1.1202702659530083e-7,
+            "velocityX": -3.6602411378675055,
+            "velocityY": -0.9556535738043043,
+            "timestamp": 1.167011026770588
+        },
+        {
+            "x": 3.748372874246301,
+            "y": 6.440698808855463,
+            "heading": -0.10097714473442117,
+            "angularVelocity": -1.1202751392033635e-7,
+            "velocityX": -3.6602410539684915,
+            "velocityY": -0.9556538951629009,
+            "timestamp": 1.256781105752941
+        },
+        {
+            "x": 3.4197928479820465,
+            "y": 6.354909291415878,
+            "heading": -0.10097715479114391,
+            "angularVelocity": -1.120276421337113e-7,
+            "velocityX": -3.6602399150000573,
+            "velocityY": -0.9556582595606669,
+            "timestamp": 1.346551184735294
+        },
+        {
+            "x": 3.1047619528127512,
+            "y": 6.228101525660344,
+            "heading": -0.10097716526412484,
+            "angularVelocity": -1.1666457370126234e-7,
+            "velocityX": -3.5093084326123236,
+            "velocityY": -1.412583871971558,
+            "timestamp": 1.436321263717647
+        },
+        {
+            "x": 2.8208656132222933,
+            "y": 6.041748259045348,
+            "heading": -0.1009771779112871,
+            "angularVelocity": -1.4088397962304153e-7,
+            "velocityX": -3.1624828986324123,
+            "velocityY": -2.0758950947495576,
+            "timestamp": 1.5260913427
+        },
+        {
+            "x": 2.5792187192315943,
+            "y": 5.803145197833315,
+            "heading": -0.10097728674036179,
+            "angularVelocity": -0.0000012123108078932896,
+            "velocityX": -2.691842278966871,
+            "velocityY": -2.65793529332427,
+            "timestamp": 1.615861421682353
+        },
+        {
+            "x": 2.3963290188287294,
+            "y": 5.533479989641919,
+            "heading": -0.13644335482170092,
+            "angularVelocity": -0.3950767169047361,
+            "velocityX": -2.037312459521214,
+            "velocityY": -3.003954226707092,
+            "timestamp": 1.705631500664706
+        },
+        {
+            "x": 2.279822826385498,
+            "y": 5.254662990570068,
+            "heading": -0.18962986506951118,
+            "angularVelocity": -0.5924748073105779,
+            "velocityX": -1.2978287839789375,
+            "velocityY": -3.105901233821009,
+            "timestamp": 1.795401579647059
+        },
+        {
+            "x": 2.2328990354823257,
+            "y": 5.0773593431323425,
+            "heading": -0.225508532252886,
+            "angularVelocity": -0.6284306472549296,
+            "velocityX": -0.8218908506157174,
+            "velocityY": -3.1055514229095325,
+            "timestamp": 1.8524940644563022
+        },
+        {
+            "x": 2.2122650091837905,
+            "y": 4.906948206481987,
+            "heading": -0.2618449581428327,
+            "angularVelocity": -0.636448492486558,
+            "velocityX": -0.3614140524419242,
+            "velocityY": -2.9848260628208765,
+            "timestamp": 1.9095865492655453
+        },
+        {
+            "x": 2.2138710973194917,
+            "y": 4.752151212947765,
+            "heading": -0.29639527524079207,
+            "angularVelocity": -0.6051640108688915,
+            "velocityX": 0.02813134059521416,
+            "velocityY": -2.7113374737719043,
+            "timestamp": 1.9666790340747884
+        },
+        {
+            "x": 2.229883400142308,
+            "y": 4.620372877512837,
+            "heading": -0.32691110500805654,
+            "angularVelocity": -0.5344981895462751,
+            "velocityX": 0.280462531565201,
+            "velocityY": -2.308155545781648,
+            "timestamp": 2.0237715188840317
+        },
+        {
+            "x": 2.251883940293385,
+            "y": 4.51506115985361,
+            "heading": -0.3519685302004407,
+            "angularVelocity": -0.4388918309689961,
+            "velocityX": 0.38534914402167997,
+            "velocityY": -1.844581086479138,
+            "timestamp": 2.080864003693275
+        },
+        {
+            "x": 2.273515445313811,
+            "y": 4.436872238113893,
+            "heading": -0.37095335253129014,
+            "angularVelocity": -0.33252751906728534,
+            "velocityX": 0.3788853312819565,
+            "velocityY": -1.3695133781784317,
+            "timestamp": 2.1379564885025184
+        },
+        {
+            "x": 2.2905088224909966,
+            "y": 4.385405157743103,
+            "heading": -0.3836505931421081,
+            "angularVelocity": -0.22239775783770355,
+            "velocityX": 0.2976464806908316,
+            "velocityY": -0.9014685653065743,
+            "timestamp": 2.1950489733117617
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.11121265573212626,
+            "velocityX": 0.16624215149881386,
+            "velocityY": -0.4449825196428863,
+            "timestamp": 2.252141458121005
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -5.463060715454085e-29,
+            "velocityX": 0,
+            "velocityY": -5.448481045128469e-30,
+            "timestamp": 2.3092339429302484
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C2 To C1.traj b/src/main/deploy/choreo/C2 To C1.traj
new file mode 100644
index 00000000..90a593eb
--- /dev/null
+++ b/src/main/deploy/choreo/C2 To C1.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -3.817834980565894e-40,
+            "angularVelocity": -1.9855221195343998e-39,
+            "velocityX": -9.621145750064654e-40,
+            "velocityY": 1.4575297691012433e-39,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 5.880500005575846,
+            "heading": 3.479114156010015e-22,
+            "angularVelocity": 3.4766412561552746e-21,
+            "velocityX": 2.7068848151306367e-33,
+            "velocityY": 0.8344065481228213,
+            "timestamp": 0.10007112931184182
+        },
+        {
+            "x": 7.68,
+            "y": 6.047500013629847,
+            "heading": 1.081691491825702e-21,
+            "angularVelocity": 7.332585052389997e-21,
+            "velocityX": 5.1589068200097806e-33,
+            "velocityY": 1.6688130652907358,
+            "timestamp": 0.20014225862368365
+        },
+        {
+            "x": 7.68,
+            "y": 6.298000017966616,
+            "heading": 2.2511586271387426e-21,
+            "angularVelocity": 1.1686358860616172e-20,
+            "velocityX": 5.8498916949463956e-33,
+            "velocityY": 2.503219520548835,
+            "timestamp": 0.3002133879355255
+        },
+        {
+            "x": 7.68,
+            "y": 6.632,
+            "heading": 4.481673966100434e-21,
+            "angularVelocity": 2.228929929167172e-20,
+            "velocityX": 6.576791251337367e-33,
+            "velocityY": 3.3376257900775013,
+            "timestamp": 0.4002845172473673
+        },
+        {
+            "x": 7.68,
+            "y": 6.9659999820333836,
+            "heading": 1.4391637904192512e-21,
+            "angularVelocity": -3.0403475801105514e-20,
+            "velocityX": 1.0509973010204301e-32,
+            "velocityY": 3.3376257900775013,
+            "timestamp": 0.5003556465592092
+        },
+        {
+            "x": 7.68,
+            "y": 7.216499986370152,
+            "heading": 4.7457500532831525e-22,
+            "angularVelocity": -9.63903176962576e-21,
+            "velocityX": 7.983324746463204e-33,
+            "velocityY": 2.503219520548835,
+            "timestamp": 0.600426775871051
+        },
+        {
+            "x": 7.68,
+            "y": 7.383499994424153,
+            "heading": 1.0909640932435146e-22,
+            "angularVelocity": -3.652188228166023e-21,
+            "velocityX": 7.341551448907972e-33,
+            "velocityY": 1.6688130652907358,
+            "timestamp": 0.7004979051828928
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -9.42965601107167e-40,
+            "angularVelocity": -1.09018866193586e-21,
+            "velocityX": 7.44470103227488e-33,
+            "velocityY": 0.8344065481228213,
+            "timestamp": 0.8005690344947346
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 8.249839679439821e-43,
+            "angularVelocity": 9.431229878048832e-39,
+            "velocityX": 1.012759498983901e-41,
+            "velocityY": 5.142787784847508e-39,
+            "timestamp": 0.9006401638065764
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C2 To C3.traj b/src/main/deploy/choreo/C2 To C3.traj
new file mode 100644
index 00000000..74357de2
--- /dev/null
+++ b/src/main/deploy/choreo/C2 To C3.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -5.382102664363315e-41,
+            "angularVelocity": -2.8652892976895567e-45,
+            "velocityX": 1.592735858156421e-40,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 5.713499994424153,
+            "heading": 1.659495816830368e-21,
+            "angularVelocity": 1.658316243120447e-20,
+            "velocityX": 2.780383744489643e-33,
+            "velocityY": -0.8344065481228213,
+            "timestamp": 0.10007112931184184
+        },
+        {
+            "x": 7.68,
+            "y": 5.546499986370153,
+            "heading": 4.9512387638524226e-21,
+            "angularVelocity": 3.289403167370527e-20,
+            "velocityX": 3.618624574631574e-33,
+            "velocityY": -1.668813065290736,
+            "timestamp": 0.20014225862368368
+        },
+        {
+            "x": 7.68,
+            "y": 5.295999982033384,
+            "heading": 9.801760970151702e-21,
+            "angularVelocity": 4.8470744390330394e-20,
+            "velocityX": 3.439292531132709e-33,
+            "velocityY": -2.503219520548835,
+            "timestamp": 0.30021338793552554
+        },
+        {
+            "x": 7.68,
+            "y": 4.962,
+            "heading": 1.506109654033251e-20,
+            "angularVelocity": 5.255597191401202e-20,
+            "velocityX": 4.639675238422859e-33,
+            "velocityY": -3.3376257900775017,
+            "timestamp": 0.40028451724736736
+        },
+        {
+            "x": 7.68,
+            "y": 4.628000017966616,
+            "heading": 5.3586032763760104e-21,
+            "angularVelocity": -9.69559666912617e-20,
+            "velocityX": 4.935439546802769e-33,
+            "velocityY": -3.3376257900775017,
+            "timestamp": 0.5003556465592092
+        },
+        {
+            "x": 7.68,
+            "y": 4.377500013629846,
+            "heading": 1.8760590350448333e-21,
+            "angularVelocity": -3.480068842301423e-20,
+            "velocityX": 4.701730454770122e-33,
+            "velocityY": -2.5032195205488352,
+            "timestamp": 0.600426775871051
+        },
+        {
+            "x": 7.68,
+            "y": 4.210500005575846,
+            "heading": 3.168722428707802e-22,
+            "angularVelocity": -1.5580785203142817e-20,
+            "velocityX": 3.49539235617723e-33,
+            "velocityY": -1.668813065290736,
+            "timestamp": 0.7004979051828928
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 5.881496870004905e-41,
+            "angularVelocity": -3.1664700918333573e-21,
+            "velocityX": 3.364378386644386e-33,
+            "velocityY": -0.8344065481228213,
+            "timestamp": 0.8005690344947346
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 9.145026397391945e-42,
+            "angularVelocity": -4.963805231001231e-40,
+            "velocityX": -4.870998572760777e-42,
+            "velocityY": 3.489748854003666e-39,
+            "timestamp": 0.9006401638065764
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C2 To C4.traj b/src/main/deploy/choreo/C2 To C4.traj
new file mode 100644
index 00000000..bcede3cb
--- /dev/null
+++ b/src/main/deploy/choreo/C2 To C4.traj	
@@ -0,0 +1,139 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 7.067561628822357e-40,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 5.720554627994461,
+            "heading": -3.8243496398495774e-19,
+            "angularVelocity": -3.994077070459528e-18,
+            "velocityX": -4.592295404090674e-34,
+            "velocityY": -0.7983807350946677,
+            "timestamp": 0.09575052183150977
+        },
+        {
+            "x": 7.68,
+            "y": 5.567663887017311,
+            "heading": -1.1460790562861162e-18,
+            "angularVelocity": -7.975351749671857e-18,
+            "velocityX": 3.216033275420413e-35,
+            "velocityY": -1.5967614385035829,
+            "timestamp": 0.19150104366301954
+        },
+        {
+            "x": 7.68,
+            "y": 5.3383277831364,
+            "heading": -2.289011643788579e-18,
+            "angularVelocity": -1.1936567706665239e-17,
+            "velocityX": -6.99395718741277e-36,
+            "velocityY": -2.3951420785410233,
+            "timestamp": 0.2872515654945293
+        },
+        {
+            "x": 7.68,
+            "y": 5.032546334555254,
+            "heading": -3.801510097022925e-18,
+            "angularVelocity": -1.5796242396621782e-17,
+            "velocityX": -1.9389720200265424e-34,
+            "velocityY": -3.1935225284643693,
+            "timestamp": 0.3830020873260391
+        },
+        {
+            "x": 7.68,
+            "y": 4.670327800733179,
+            "heading": -7.244345840319028e-19,
+            "angularVelocity": 3.2136383741131935e-17,
+            "velocityX": -8.432373407035536e-35,
+            "velocityY": -3.7829405719528495,
+            "timestamp": 0.47875260915754886
+        },
+        {
+            "x": 7.68,
+            "y": 4.308109266911059,
+            "heading": -3.757861819981246e-18,
+            "angularVelocity": -3.168052954622205e-17,
+            "velocityX": -1.6004910806259417e-34,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.5745031309890586
+        },
+        {
+            "x": 7.68,
+            "y": 3.9458907330889397,
+            "heading": -5.3976563030855884e-18,
+            "angularVelocity": -1.712569761227889e-17,
+            "velocityX": -2.8603575042124878e-33,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.6702536528205684
+        },
+        {
+            "x": 7.68,
+            "y": 3.5836721992668203,
+            "heading": -4.35645304230582e-18,
+            "angularVelocity": 1.0874126227865062e-17,
+            "velocityX": -1.9073271103237224e-34,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.7660041746520782
+        },
+        {
+            "x": 7.68,
+            "y": 3.221453665444745,
+            "heading": 3.661990694649205e-18,
+            "angularVelocity": 8.37430813282138e-17,
+            "velocityX": -3.48109229998139e-33,
+            "velocityY": -3.7829405719528495,
+            "timestamp": 0.8617546964835879
+        },
+        {
+            "x": 7.68,
+            "y": 2.915672216863599,
+            "heading": 2.222559506115206e-18,
+            "angularVelocity": -1.503314198751682e-17,
+            "velocityX": 7.233727398507661e-34,
+            "velocityY": -3.1935225284643693,
+            "timestamp": 0.9575052183150977
+        },
+        {
+            "x": 7.68,
+            "y": 2.686336112982689,
+            "heading": 1.1174854461234148e-18,
+            "angularVelocity": -1.1541180553049724e-17,
+            "velocityX": 7.218959232022663e-34,
+            "velocityY": -2.3951420785410233,
+            "timestamp": 1.0532557401466074
+        },
+        {
+            "x": 7.68,
+            "y": 2.5334453720055388,
+            "heading": 3.7383227049768543e-19,
+            "angularVelocity": -7.766570474567622e-18,
+            "velocityX": 1.7872274250399634e-33,
+            "velocityY": -1.5967614385035829,
+            "timestamp": 1.1490062619781172
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": -3.904232200157263e-18,
+            "velocityX": 6.089132400706592e-33,
+            "velocityY": -0.7983807350946677,
+            "timestamp": 1.244756783809627
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 3.5763678522980893e-43,
+            "velocityY": -5.882608067207678e-38,
+            "timestamp": 1.3405073056411367
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C2 To C5.traj b/src/main/deploy/choreo/C2 To C5.traj
new file mode 100644
index 00000000..3fd40c6e
--- /dev/null
+++ b/src/main/deploy/choreo/C2 To C5.traj	
@@ -0,0 +1,175 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -1.2882449649302714e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 5.715155846048237,
+            "heading": -2.6675793503802873e-18,
+            "angularVelocity": -2.692513973443352e-17,
+            "velocityX": 2.411941793716939e-19,
+            "velocityY": -0.8260917454276263,
+            "timestamp": 0.09907392781100396
+        },
+        {
+            "x": 7.68,
+            "y": 5.55146754125807,
+            "heading": -8.002991694377614e-18,
+            "angularVelocity": -5.385283961043983e-17,
+            "velocityX": 4.822926900034719e-19,
+            "velocityY": -1.6521834594306495,
+            "timestamp": 0.19814785562200793
+        },
+        {
+            "x": 7.68,
+            "y": 5.305935091856216,
+            "heading": -1.6007101079545214e-17,
+            "angularVelocity": -8.07892607262788e-17,
+            "velocityX": 7.23199861883135e-19,
+            "velocityY": -2.478275110584479,
+            "timestamp": 0.29722178343301187
+        },
+        {
+            "x": 7.68,
+            "y": 4.978558516522808,
+            "heading": -2.6680861930648114e-17,
+            "angularVelocity": -1.0773531530269989e-16,
+            "velocityX": 9.635330187135107e-19,
+            "velocityY": -3.3043665731908765,
+            "timestamp": 0.39629571124401586
+        },
+        {
+            "x": 7.68,
+            "y": 4.603767735073314,
+            "heading": -3.033567109313287e-17,
+            "angularVelocity": -3.688971703490728e-17,
+            "velocityX": 1.1102275506466142e-18,
+            "velocityY": -3.782940575087071,
+            "timestamp": 0.49536963905501985
+        },
+        {
+            "x": 7.68,
+            "y": 4.228976953623796,
+            "heading": -3.5529792774719166e-17,
+            "angularVelocity": -5.2426726146536295e-17,
+            "velocityX": 1.1087814850277944e-18,
+            "velocityY": -3.7829405750873115,
+            "timestamp": 0.5944435668660238
+        },
+        {
+            "x": 7.68,
+            "y": 3.8541861721742774,
+            "heading": -4.8775545061581196e-17,
+            "angularVelocity": -1.3369564101798844e-16,
+            "velocityX": 1.1066129122730268e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.6935174946770278
+        },
+        {
+            "x": 7.68,
+            "y": 3.4793953907247595,
+            "heading": -3.3383639047364345e-17,
+            "angularVelocity": 1.5535778538404392e-16,
+            "velocityX": 1.113993391664377e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.7925914224880318
+        },
+        {
+            "x": 7.68,
+            "y": 3.104604609275241,
+            "heading": -4.339521377299963e-18,
+            "angularVelocity": 2.9315601300747485e-16,
+            "velocityX": 1.114171335036007e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.8916653502990358
+        },
+        {
+            "x": 7.68,
+            "y": 2.7298138278257227,
+            "heading": 1.3174711336089981e-17,
+            "angularVelocity": 1.7677943229992181e-16,
+            "velocityX": 1.1158489583207238e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.9907392781100398
+        },
+        {
+            "x": 7.68,
+            "y": 2.3550230463762043,
+            "heading": 2.965421117619106e-17,
+            "angularVelocity": 1.6633538413393434e-16,
+            "velocityX": 1.1181357718618657e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 1.0898132059210437
+        },
+        {
+            "x": 7.68,
+            "y": 1.9802322649266861,
+            "heading": 2.902288280090785e-17,
+            "angularVelocity": -6.372295813531433e-18,
+            "velocityX": 1.121394742098521e-18,
+            "velocityY": -3.7829405750873115,
+            "timestamp": 1.1888871337320477
+        },
+        {
+            "x": 7.68,
+            "y": 1.605441483477192,
+            "heading": 2.667314494747493e-17,
+            "angularVelocity": -2.3717015216307205e-17,
+            "velocityX": 1.1050727300415562e-18,
+            "velocityY": -3.782940575087071,
+            "timestamp": 1.2879610615430517
+        },
+        {
+            "x": 7.68,
+            "y": 1.2780649081437845,
+            "heading": 1.6004201006274906e-17,
+            "angularVelocity": -1.0768669595616384e-16,
+            "velocityX": 9.663666503471993e-19,
+            "velocityY": -3.304366573190877,
+            "timestamp": 1.3870349893540557
+        },
+        {
+            "x": 7.68,
+            "y": 1.0325324587419298,
+            "heading": 8.002125258512226e-18,
+            "angularVelocity": -8.076873426351199e-17,
+            "velocityX": 7.247750412790524e-19,
+            "velocityY": -2.478275110584479,
+            "timestamp": 1.4861089171650597
+        },
+        {
+            "x": 7.68,
+            "y": 0.868844153951763,
+            "heading": 2.6673738748743342e-18,
+            "angularVelocity": -5.384616822543564e-17,
+            "velocityX": 4.831833763651305e-19,
+            "velocityY": -1.6521834594306495,
+            "timestamp": 1.5851828449760637
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": -2.6923065773077948e-17,
+            "velocityX": 2.415916928361511e-19,
+            "velocityY": -0.8260917454276263,
+            "timestamp": 1.6842567727870676
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -1.1487176577652646e-33,
+            "velocityY": 0,
+            "timestamp": 1.7833307005980716
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C2 To S1.traj b/src/main/deploy/choreo/C2 To S1.traj
new file mode 100644
index 00000000..6cbc4c1d
--- /dev/null
+++ b/src/main/deploy/choreo/C2 To S1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 7.776001513534033e-33,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.620476769374415,
+            "y": 5.82809535402629,
+            "heading": -0.014618874829729222,
+            "angularVelocity": -0.16274311325472623,
+            "velocityX": -0.6626362135126213,
+            "velocityY": 0.34616581514913597,
+            "timestamp": 0.0898279167539809
+        },
+        {
+            "x": 7.5013982904113785,
+            "y": 5.890300493780534,
+            "heading": -0.04163561529349077,
+            "angularVelocity": -0.30076107116849365,
+            "velocityX": -1.3256288608937272,
+            "velocityY": 0.6924922897255835,
+            "timestamp": 0.1796558335079618
+        },
+        {
+            "x": 7.3227216232188646,
+            "y": 5.983633458070523,
+            "heading": -0.07726722819497155,
+            "angularVelocity": -0.39666524827763777,
+            "velocityX": -1.9890995321851956,
+            "velocityY": 1.0390195794655717,
+            "timestamp": 0.2694837502619427
+        },
+        {
+            "x": 7.084404558965627,
+            "y": 6.108106280619183,
+            "heading": -0.11365614248079976,
+            "angularVelocity": -0.40509582767559393,
+            "velocityX": -2.6530400889284445,
+            "velocityY": 1.3856808333823862,
+            "timestamp": 0.3593116670159236
+        },
+        {
+            "x": 6.786764320803032,
+            "y": 6.263508027124416,
+            "heading": -0.12453213515982267,
+            "angularVelocity": -0.12107586452004557,
+            "velocityX": -3.313449191722499,
+            "velocityY": 1.7299938829800985,
+            "timestamp": 0.4491395837699045
+        },
+        {
+            "x": 6.485598483527544,
+            "y": 6.42090073842616,
+            "heading": -0.1245321478638912,
+            "angularVelocity": -1.4142672994383305e-7,
+            "velocityX": -3.3526975594938513,
+            "velocityY": 1.7521580928210598,
+            "timestamp": 0.5389675005238854
+        },
+        {
+            "x": 6.164419225817841,
+            "y": 6.531883674964022,
+            "heading": -0.12453214704403566,
+            "angularVelocity": 9.126957138704023e-9,
+            "velocityX": -3.575494894191328,
+            "velocityY": 1.2355060714790946,
+            "timestamp": 0.6287954172778664
+        },
+        {
+            "x": 5.838530026083828,
+            "y": 6.576482035403971,
+            "heading": -0.08897109734241883,
+            "angularVelocity": 0.39587971074750417,
+            "velocityX": -3.62792783702814,
+            "velocityY": 0.4964866385813549,
+            "timestamp": 0.7186233340318473
+        },
+        {
+            "x": 5.57314678987786,
+            "y": 6.601508855183474,
+            "heading": 0.02708371106201491,
+            "angularVelocity": 1.2919681608812392,
+            "velocityX": -2.9543514510393742,
+            "velocityY": 0.27860848479928674,
+            "timestamp": 0.8084512507858282
+        },
+        {
+            "x": 5.3743330310505755,
+            "y": 6.616929267325104,
+            "heading": 0.1370261836602847,
+            "angularVelocity": 1.2239232141983023,
+            "velocityX": -2.213273623742076,
+            "velocityY": 0.17166614454461948,
+            "timestamp": 0.8982791675398092
+        },
+        {
+            "x": 5.242037120273562,
+            "y": 6.625930657483227,
+            "heading": 0.21902488355780253,
+            "angularVelocity": 0.9128420524556349,
+            "velocityX": -1.4727705546076917,
+            "velocityY": 0.10020704568687104,
+            "timestamp": 0.9881070842937901
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.263,
+            "angularVelocity": 0.48954843918551255,
+            "velocityX": -0.7351514168409599,
+            "velocityY": 0.0453015350218741,
+            "timestamp": 1.077935001047771
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.263,
+            "angularVelocity": -4.780109867389741e-33,
+            "velocityX": 0,
+            "velocityY": -5.233682494366388e-34,
+            "timestamp": 1.167762917801752
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C2 To S2.traj b/src/main/deploy/choreo/C2 To S2.traj
new file mode 100644
index 00000000..4e0731ce
--- /dev/null
+++ b/src/main/deploy/choreo/C2 To S2.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 1.3211551091100433e-31,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6375302942972665,
+            "y": 5.822387711771769,
+            "heading": 0.010500881646553203,
+            "angularVelocity": 0.13619602846032355,
+            "velocityX": -0.5508304389364825,
+            "velocityY": 0.32927763890619355,
+            "timestamp": 0.07710123243140166
+        },
+        {
+            "x": 7.5525908811480225,
+            "y": 5.873163180716977,
+            "heading": 0.031500279151306286,
+            "angularVelocity": 0.27236137273728567,
+            "velocityX": -1.1016609004897018,
+            "velocityY": 0.6585558666702785,
+            "timestamp": 0.1542024648628033
+        },
+        {
+            "x": 7.4251817593524025,
+            "y": 5.949326484276257,
+            "heading": 0.06299451206522548,
+            "angularVelocity": 0.40847898173275193,
+            "velocityX": -1.6524913776051289,
+            "velocityY": 0.987835098836381,
+            "timestamp": 0.23130369729420497
+        },
+        {
+            "x": 7.255302939480135,
+            "y": 6.050877733979633,
+            "heading": 0.10497913316622995,
+            "angularVelocity": 0.5445389104299847,
+            "velocityX": -2.203321717631576,
+            "velocityY": 1.3171157775425744,
+            "timestamp": 0.3084049297256066
+        },
+        {
+            "x": 7.042954462706059,
+            "y": 6.177817072431174,
+            "heading": 0.1574496496087124,
+            "angularVelocity": 0.6805405670936119,
+            "velocityX": -2.754151523621958,
+            "velocityY": 1.6463983058180132,
+            "timestamp": 0.3855061621570083
+        },
+        {
+            "x": 6.792607460054783,
+            "y": 6.3274746833219355,
+            "heading": 0.15744965721454035,
+            "angularVelocity": 9.864729365588338e-8,
+            "velocityX": -3.246990933303352,
+            "velocityY": 1.941053419916652,
+            "timestamp": 0.46260739458840994
+        },
+        {
+            "x": 6.542259846259959,
+            "y": 6.4771312718889495,
+            "heading": 0.15744966481969572,
+            "angularVelocity": 9.863857053631023e-8,
+            "velocityX": -3.246998859811527,
+            "velocityY": 1.9410401604172332,
+            "timestamp": 0.5397086270198116
+        },
+        {
+            "x": 6.273256747030451,
+            "y": 6.589862608843688,
+            "heading": 0.1574496727775284,
+            "angularVelocity": 1.0321278187268036e-7,
+            "velocityX": -3.4889597837342676,
+            "velocityY": 1.4621210764047974,
+            "timestamp": 0.6168098594512132
+        },
+        {
+            "x": 5.98904964850956,
+            "y": 6.655416753006686,
+            "heading": 0.15744969119167346,
+            "angularVelocity": 2.3883074855376557e-7,
+            "velocityX": -3.686155065987494,
+            "velocityY": 0.8502347121535809,
+            "timestamp": 0.6939110918826149
+        },
+        {
+            "x": 5.6978465137603145,
+            "y": 6.6719003531074685,
+            "heading": 0.15744999446473232,
+            "angularVelocity": 0.00000393343983387992,
+            "velocityX": -3.7768933850484796,
+            "velocityY": 0.21379165521703997,
+            "timestamp": 0.7710123243140166
+        },
+        {
+            "x": 5.40805721282959,
+            "y": 6.638837814331055,
+            "heading": 0.15745035498223803,
+            "angularVelocity": 0.00000467589809324531,
+            "velocityX": -3.7585560151526085,
+            "velocityY": -0.428819848059242,
+            "timestamp": 0.8481135567454182
+        },
+        {
+            "x": 5.104307335648147,
+            "y": 6.562247743535093,
+            "heading": 0.18966206475475966,
+            "angularVelocity": 0.37702586381684944,
+            "velocityX": -3.555277277652879,
+            "velocityY": -0.896457772827513,
+            "timestamp": 0.9335498964342235
+        },
+        {
+            "x": 4.836190414730584,
+            "y": 6.438293970082571,
+            "heading": 0.27176288986041636,
+            "angularVelocity": 0.9609590650149814,
+            "velocityX": -3.1382070193334055,
+            "velocityY": -1.4508319750590115,
+            "timestamp": 1.0189862361230289
+        },
+        {
+            "x": 4.616497381460216,
+            "y": 6.278221072521664,
+            "heading": 0.3274582685749672,
+            "angularVelocity": 0.6518933151562509,
+            "velocityX": -2.5714237532949347,
+            "velocityY": -1.8735926438791781,
+            "timestamp": 1.1044225758118342
+        },
+        {
+            "x": 4.449006665280017,
+            "y": 6.09487376587715,
+            "heading": 0.3096216512620487,
+            "angularVelocity": -0.2087708506460702,
+            "velocityX": -1.960415401575849,
+            "velocityY": -2.1460107878256482,
+            "timestamp": 1.1898589155006396
+        },
+        {
+            "x": 4.339814814870372,
+            "y": 5.903698766708507,
+            "heading": 0.23521329843339078,
+            "angularVelocity": -0.8709215902704173,
+            "velocityX": -1.2780492564097095,
+            "velocityY": -2.237630964352897,
+            "timestamp": 1.275295255189445
+        },
+        {
+            "x": 4.286146359443328,
+            "y": 5.736442488850756,
+            "heading": 0.1356278137676922,
+            "angularVelocity": -1.1656103834554516,
+            "velocityX": -0.628168945703036,
+            "velocityY": -1.957671389796991,
+            "timestamp": 1.3607315948782504
+        },
+        {
+            "x": 4.266951931468271,
+            "y": 5.619203136466468,
+            "heading": 0.049466079264437586,
+            "angularVelocity": -1.0084904715849423,
+            "velocityX": -0.2246635102225946,
+            "velocityY": -1.372242219309977,
+            "timestamp": 1.4461679345670557
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 4.182976778726496e-34,
+            "angularVelocity": -0.578981724224301,
+            "velocityX": -0.046255861178807596,
+            "velocityY": -0.6929502912005617,
+            "timestamp": 1.531604274255861
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -6.30298269203181e-33,
+            "timestamp": 1.6170406139446665
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C2 To S3.traj b/src/main/deploy/choreo/C2 To S3.traj
new file mode 100644
index 00000000..90749c58
--- /dev/null
+++ b/src/main/deploy/choreo/C2 To S3.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 7.446350164213638e-30,
+            "velocityX": -3.476389258222187e-32,
+            "velocityY": 2.4606016306403766e-37,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6436015698643995,
+            "y": 5.74806653588022,
+            "heading": -0.0038807165751560974,
+            "angularVelocity": -0.045372809336175934,
+            "velocityX": -0.4255654848002544,
+            "velocityY": -0.572123393880186,
+            "timestamp": 0.08552956345292874
+        },
+        {
+            "x": 7.570804707716712,
+            "y": 5.6501996114987145,
+            "heading": -0.011642164214821673,
+            "angularVelocity": -0.09074578807989701,
+            "velocityX": -0.8511309915401587,
+            "velocityY": -1.1442467426525054,
+            "timestamp": 0.17105912690585748
+        },
+        {
+            "x": 7.461609412342726,
+            "y": 5.503399231866818,
+            "heading": -0.02328436718357624,
+            "angularVelocity": -0.136119050521775,
+            "velocityX": -1.2766965124764296,
+            "velocityY": -1.7163700328330225,
+            "timestamp": 0.25658869035878623
+        },
+        {
+            "x": 7.3160156842382476,
+            "y": 5.307665404737857,
+            "heading": -0.038807352623976644,
+            "angularVelocity": -0.18149263031072865,
+            "velocityX": -1.7022620276158245,
+            "velocityY": -2.288493232362672,
+            "timestamp": 0.34211825381171496
+        },
+        {
+            "x": 7.134023531165023,
+            "y": 5.062998148299065,
+            "heading": -0.05821111114579378,
+            "angularVelocity": -0.22686610031040333,
+            "velocityX": -2.127827452005934,
+            "velocityY": -2.8606162192496707,
+            "timestamp": 0.4276478172646437
+        },
+        {
+            "x": 6.9409164317403365,
+            "y": 4.803390071813602,
+            "heading": -0.058211113467083825,
+            "angularVelocity": -2.714020682501112e-8,
+            "velocityX": -2.2577818900123794,
+            "velocityY": -3.0353022511138943,
+            "timestamp": 0.5131773807175725
+        },
+        {
+            "x": 6.70968017178257,
+            "y": 4.577079452070077,
+            "heading": -0.05821111591888997,
+            "angularVelocity": -2.866618337367041e-8,
+            "velocityX": -2.7035828387575864,
+            "velocityY": -2.645992924634468,
+            "timestamp": 0.5987069441705012
+        },
+        {
+            "x": 6.4400791107327935,
+            "y": 4.398188508585333,
+            "heading": -0.05821111892081074,
+            "angularVelocity": -3.509804859460385e-8,
+            "velocityX": -3.152138864804934,
+            "velocityY": -2.091568532127445,
+            "timestamp": 0.6842365076234299
+        },
+        {
+            "x": 6.141694589328426,
+            "y": 4.273075039260189,
+            "heading": -0.058211144902897714,
+            "angularVelocity": -3.0377902004237926e-7,
+            "velocityX": -3.488671160686832,
+            "velocityY": -1.462809632999017,
+            "timestamp": 0.7697660710763586
+        },
+        {
+            "x": 5.825131041705898,
+            "y": 4.206185466847777,
+            "heading": -0.05821124348692034,
+            "angularVelocity": -0.000001152630957627144,
+            "velocityX": -3.7012178578082944,
+            "velocityY": -0.7820637649954203,
+            "timestamp": 0.8552956345292874
+        },
+        {
+            "x": 5.503318785454965,
+            "y": 4.1726658015863975,
+            "heading": -0.058211338856237596,
+            "angularVelocity": -0.0000011150450605169934,
+            "velocityX": -3.76258504380235,
+            "velocityY": -0.39190735820634004,
+            "timestamp": 0.9408251979822161
+        },
+        {
+            "x": 5.1935160355708865,
+            "y": 4.079344267897855,
+            "heading": -0.0582116136415855,
+            "angularVelocity": -0.0000032127528402629677,
+            "velocityX": -3.622171532005757,
+            "velocityY": -1.091102654112135,
+            "timestamp": 1.026354761435145
+        },
+        {
+            "x": 4.918641828284195,
+            "y": 3.9369980738928456,
+            "heading": -0.09326111746178506,
+            "angularVelocity": -0.4097940221510551,
+            "velocityX": -3.2137917719873403,
+            "velocityY": -1.664292301495839,
+            "timestamp": 1.1118843248880736
+        },
+        {
+            "x": 4.689025118769517,
+            "y": 3.755353610578967,
+            "heading": -0.16867561276178636,
+            "angularVelocity": -0.8817360016283226,
+            "velocityX": -2.6846472756878708,
+            "velocityY": -2.1237623107224977,
+            "timestamp": 1.1974138883410024
+        },
+        {
+            "x": 4.510985991586203,
+            "y": 3.542062267326439,
+            "heading": -0.2715725395002121,
+            "angularVelocity": -1.2030568447254515,
+            "velocityX": -2.0816092120158958,
+            "velocityY": -2.493773318156982,
+            "timestamp": 1.282943451793931
+        },
+        {
+            "x": 4.39344009542189,
+            "y": 3.3364721307805043,
+            "heading": -0.3792171957634167,
+            "angularVelocity": -1.2585666513128753,
+            "velocityX": -1.3743305989045989,
+            "velocityY": -2.4037318588569803,
+            "timestamp": 1.3684730152468598
+        },
+        {
+            "x": 4.321460732883776,
+            "y": 3.1711461674458645,
+            "heading": -0.4697306425856333,
+            "angularVelocity": -1.0582708851545903,
+            "velocityX": -0.8415728975131295,
+            "velocityY": -1.932968632835679,
+            "timestamp": 1.4540025786997886
+        },
+        {
+            "x": 4.280782976343733,
+            "y": 3.057632956228743,
+            "heading": -0.5333100471714295,
+            "angularVelocity": -0.7433617338733072,
+            "velocityX": -0.475598785938263,
+            "velocityY": -1.3271809960727023,
+            "timestamp": 1.5395321421527173
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -0.38220647351440606,
+            "velocityX": -0.20791613596297567,
+            "velocityY": -0.6738366700592523,
+            "timestamp": 1.625061705605646
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -1.9453350650223036e-32,
+            "velocityX": 0,
+            "velocityY": 3.675541041669649e-33,
+            "timestamp": 1.7105912690585747
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C2 To S4.traj b/src/main/deploy/choreo/C2 To S4.traj
new file mode 100644
index 00000000..4efefc66
--- /dev/null
+++ b/src/main/deploy/choreo/C2 To S4.traj	
@@ -0,0 +1,256 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": -1.007826409753455e-39,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.661226431831969,
+            "y": 5.757681823957774,
+            "heading": -0.0001846118691686639,
+            "angularVelocity": -0.0025538708536768893,
+            "velocityX": -0.25970848342391806,
+            "velocityY": -0.5439170529292338,
+            "timestamp": 0.07228708096295171
+        },
+        {
+            "x": 7.623679292701499,
+            "y": 5.679045474684997,
+            "heading": -0.0005538375109882563,
+            "angularVelocity": -0.005107768039614528,
+            "velocityX": -0.5194170055049473,
+            "velocityY": -1.0878340669625215,
+            "timestamp": 0.14457416192590342
+        },
+        {
+            "x": 7.567358579979895,
+            "y": 5.561090955652203,
+            "heading": -0.0011076778331591435,
+            "angularVelocity": -0.0076616777824344475,
+            "velocityX": -0.779125563950612,
+            "velocityY": -1.6317510329853882,
+            "timestamp": 0.2168612428888551
+        },
+        {
+            "x": 7.492264291454711,
+            "y": 5.403818271607319,
+            "heading": -0.001846132228095339,
+            "angularVelocity": -0.010215579120073546,
+            "velocityX": -1.0388341530026832,
+            "velocityY": -2.175667933326667,
+            "timestamp": 0.28914832385180683
+        },
+        {
+            "x": 7.39839642615194,
+            "y": 5.207227430397753,
+            "heading": -0.0027691974892554232,
+            "angularVelocity": -0.012769436099282666,
+            "velocityX": -1.298542755528879,
+            "velocityY": -2.719584725109078,
+            "timestamp": 0.36143540481475855
+        },
+        {
+            "x": 7.285754988600711,
+            "y": 4.97131845200793,
+            "heading": -0.0038768638943260385,
+            "angularVelocity": -0.015323158582628504,
+            "velocityX": -1.5582512954003445,
+            "velocityY": -3.263501240432299,
+            "timestamp": 0.4337224857777103
+        },
+        {
+            "x": 7.167927209247298,
+            "y": 4.7245478329210355,
+            "heading": -0.0038768640537450113,
+            "angularVelocity": -2.205359112715029e-9,
+            "velocityX": -1.629997750411335,
+            "velocityY": -3.4137582511232942,
+            "timestamp": 0.506009566740662
+        },
+        {
+            "x": 7.050099366875978,
+            "y": 4.477777243923851,
+            "heading": -0.003876864213154645,
+            "angularVelocity": -2.2052299170164517e-9,
+            "velocityX": -1.6299986221840805,
+            "velocityY": -3.413757834870371,
+            "timestamp": 0.5782966477036137
+        },
+        {
+            "x": 6.932271561638543,
+            "y": 4.2310066371960025,
+            "heading": -0.0038768643725604667,
+            "angularVelocity": -2.2051771914474314e-9,
+            "velocityX": -1.629998108483918,
+            "velocityY": -3.413758080151585,
+            "timestamp": 0.6505837286665653
+        },
+        {
+            "x": 6.8252544845676155,
+            "y": 3.9793591274718993,
+            "heading": -0.0038768645333797187,
+            "angularVelocity": -2.2247301918048757e-9,
+            "velocityX": -1.4804454080221456,
+            "velocityY": -3.4812238421008592,
+            "timestamp": 0.722870809629517
+        },
+        {
+            "x": 6.7595626994044276,
+            "y": 3.7139091088268694,
+            "heading": -0.003876864707631206,
+            "angularVelocity": -2.410548124556829e-9,
+            "velocityX": -0.9087624550347577,
+            "velocityY": -3.6721640313720423,
+            "timestamp": 0.7951578905924687
+        },
+        {
+            "x": 6.7368645668029785,
+            "y": 3.4413950443267822,
+            "heading": -0.0038768649214938546,
+            "angularVelocity": -2.9585182521427424e-9,
+            "velocityX": -0.31399985030634436,
+            "velocityY": -3.7698861382956523,
+            "timestamp": 0.8674449715554203
+        },
+        {
+            "x": 6.763602448674763,
+            "y": 3.1456190677825386,
+            "heading": -0.0038769163309766456,
+            "angularVelocity": -6.548510424231184e-7,
+            "velocityX": 0.340585605327871,
+            "velocityY": -3.7675774205236117,
+            "timestamp": 0.9459505849856912
+        },
+        {
+            "x": 6.763837716877819,
+            "y": 2.855181760345028,
+            "heading": -0.02573615579182971,
+            "angularVelocity": -0.27844173818562123,
+            "velocityX": 0.0029968328731749647,
+            "velocityY": -3.699573759722024,
+            "timestamp": 1.0244561984159621
+        },
+        {
+            "x": 6.746691140594586,
+            "y": 2.576419895706607,
+            "heading": -0.08408576500639096,
+            "angularVelocity": -0.7432539746522423,
+            "velocityX": -0.2184121049950426,
+            "velocityY": -3.5508526391685153,
+            "timestamp": 1.102961811846233
+        },
+        {
+            "x": 6.69086397201793,
+            "y": 2.322743038831463,
+            "heading": -0.2046015958190934,
+            "angularVelocity": -1.5351237388870964,
+            "velocityX": -0.7111232705192682,
+            "velocityY": -3.2313212494092722,
+            "timestamp": 1.1814674252765038
+        },
+        {
+            "x": 6.586917702656428,
+            "y": 2.0871040226876985,
+            "heading": -0.32491541634118276,
+            "angularVelocity": -1.5325505433946174,
+            "velocityX": -1.324061615719081,
+            "velocityY": -3.001556269005664,
+            "timestamp": 1.2599730387067747
+        },
+        {
+            "x": 6.439847045017927,
+            "y": 1.879401635438828,
+            "heading": -0.448122685606705,
+            "angularVelocity": -1.5694071274910255,
+            "velocityX": -1.8733775995410846,
+            "velocityY": -2.6457010928696563,
+            "timestamp": 1.3384786521370455
+        },
+        {
+            "x": 6.25456019704424,
+            "y": 1.7060438180074393,
+            "heading": -0.5681346722700569,
+            "angularVelocity": -1.528705801018258,
+            "velocityX": -2.3601732395640638,
+            "velocityY": -2.2082219328859343,
+            "timestamp": 1.4169842655673164
+        },
+        {
+            "x": 6.049779045368227,
+            "y": 1.57972317182915,
+            "heading": -0.6633309119306381,
+            "angularVelocity": -1.2126042393788183,
+            "velocityX": -2.6084905617341763,
+            "velocityY": -1.6090651439911137,
+            "timestamp": 1.4954898789975872
+        },
+        {
+            "x": 5.840944041976222,
+            "y": 1.503518626439596,
+            "heading": -0.7208467997653518,
+            "angularVelocity": -0.7326340795464229,
+            "velocityX": -2.660128292322611,
+            "velocityY": -0.9706891273098467,
+            "timestamp": 1.573995492427858
+        },
+        {
+            "x": 5.636524773547887,
+            "y": 1.4770268115818306,
+            "heading": -0.7348631463149735,
+            "angularVelocity": -0.17853941822989652,
+            "velocityX": -2.603880913686534,
+            "velocityY": -0.33745121782018417,
+            "timestamp": 1.652501105858129
+        },
+        {
+            "x": 5.451777868175388,
+            "y": 1.4973120784666072,
+            "heading": -0.7197568228935092,
+            "angularVelocity": 0.19242348109134802,
+            "velocityX": -2.353295481686686,
+            "velocityY": 0.25839256581051945,
+            "timestamp": 1.7310067192883998
+        },
+        {
+            "x": 5.309771976878378,
+            "y": 1.5457401851903851,
+            "heading": -0.687868735789328,
+            "angularVelocity": 0.4061886241103559,
+            "velocityX": -1.808862896449324,
+            "velocityY": 0.6168744451222204,
+            "timestamp": 1.8095123327186706
+        },
+        {
+            "x": 5.2191255322417085,
+            "y": 1.592492981200916,
+            "heading": -0.6576952711638115,
+            "angularVelocity": 0.38434786134518467,
+            "velocityX": -1.154649211386418,
+            "velocityY": 0.5955344333696231,
+            "timestamp": 1.8880179461489415
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 0.22540134890517805,
+            "velocityX": -0.5493305555788384,
+            "velocityY": 0.35038282738234927,
+            "timestamp": 1.9665235595792123
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 0,
+            "velocityX": 6.617232561290069e-35,
+            "velocityY": 1.0472745338276337e-32,
+            "timestamp": 2.045029173009483
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C2 To W1.traj b/src/main/deploy/choreo/C2 To W1.traj
new file mode 100644
index 00000000..0428af29
--- /dev/null
+++ b/src/main/deploy/choreo/C2 To W1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 2.477402495362019e-32,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.61783610738437,
+            "y": 5.830159445563511,
+            "heading": -0.009839426193567341,
+            "angularVelocity": -0.10699963479066096,
+            "velocityX": -0.6760062707098423,
+            "velocityY": 0.3605950687933442,
+            "timestamp": 0.09195756801242966
+        },
+        {
+            "x": 7.493508328314676,
+            "y": 5.8964783245931205,
+            "heading": -0.029519357600464063,
+            "angularVelocity": -0.21401100346886773,
+            "velocityX": -1.3520124744152573,
+            "velocityY": 0.7211900060324206,
+            "timestamp": 0.18391513602485932
+        },
+        {
+            "x": 7.307016677169599,
+            "y": 5.995956620127639,
+            "heading": -0.05904164496194892,
+            "angularVelocity": -0.32104249818236175,
+            "velocityX": -2.028018521758536,
+            "velocityY": 1.0817847588256284,
+            "timestamp": 0.275872704037289
+        },
+        {
+            "x": 7.058361185168299,
+            "y": 6.1285943089313815,
+            "heading": -0.09840880514573262,
+            "angularVelocity": -0.42810136277704247,
+            "velocityX": -2.7040242296065236,
+            "velocityY": 1.4423792589405482,
+            "timestamp": 0.36783027204971863
+        },
+        {
+            "x": 6.751428461707863,
+            "y": 6.292319225940392,
+            "heading": -0.09840881013370367,
+            "angularVelocity": -5.424209394751822e-8,
+            "velocityX": -3.3377646896767494,
+            "velocityY": 1.7804398327158815,
+            "timestamp": 0.4597878400621483
+        },
+        {
+            "x": 6.443018732830252,
+            "y": 6.45324462570272,
+            "heading": -0.09840881513118924,
+            "angularVelocity": -5.434556026601013e-8,
+            "velocityX": -3.3538265043712756,
+            "velocityY": 1.7499962563230937,
+            "timestamp": 0.551745408074578
+        },
+        {
+            "x": 6.108495593860041,
+            "y": 6.548679499028843,
+            "heading": -0.09840879814777771,
+            "angularVelocity": 1.8468748020115896e-7,
+            "velocityX": -3.637798891386455,
+            "velocityY": 1.0378142374668238,
+            "timestamp": 0.6437029760870077
+        },
+        {
+            "x": 5.7623127419169835,
+            "y": 6.582899007638951,
+            "heading": -0.09840874396641834,
+            "angularVelocity": 5.891995683553032e-7,
+            "velocityX": -3.764593381767812,
+            "velocityY": 0.37212281000605224,
+            "timestamp": 0.7356605440994373
+        },
+        {
+            "x": 5.416121749902788,
+            "y": 6.617036066627776,
+            "heading": -0.09840868977891702,
+            "angularVelocity": 5.892663593006056e-7,
+            "velocityX": -3.7646819016288298,
+            "velocityY": 0.37122620494064046,
+            "timestamp": 0.8276181121118669
+        },
+        {
+            "x": 5.068594601971295,
+            "y": 6.632477066079001,
+            "heading": -0.0984086463109951,
+            "angularVelocity": 4.72695427552955e-7,
+            "velocityX": -3.7792120370616824,
+            "velocityY": 0.1679143955735957,
+            "timestamp": 0.9195756801242965
+        },
+        {
+            "x": 4.721067428734796,
+            "y": 6.647917495985935,
+            "heading": -0.09840860284326601,
+            "angularVelocity": 4.7269333042542933e-7,
+            "velocityX": -3.7792123122430223,
+            "velocityY": 0.16790820201819795,
+            "timestamp": 1.011533248136726
+        },
+        {
+            "x": 4.3735402523552525,
+            "y": 6.663357855150138,
+            "heading": -0.09840855937552709,
+            "angularVelocity": 4.726934374928119e-7,
+            "velocityX": -3.779212346422309,
+            "velocityY": 0.16790743272066663,
+            "timestamp": 1.1034908161491557
+        },
+        {
+            "x": 4.026013074283842,
+            "y": 6.678798176234082,
+            "heading": -0.09840851590777203,
+            "angularVelocity": 4.7269361285314424e-7,
+            "velocityX": -3.779212364820652,
+            "velocityY": 0.1679070186138197,
+            "timestamp": 1.1954483841615853
+        },
+        {
+            "x": 3.6784858950907164,
+            "y": 6.69423847207055,
+            "heading": -0.09840847244000124,
+            "angularVelocity": 4.7269378404370134e-7,
+            "velocityX": -3.7792123770188395,
+            "velocityY": 0.16790674405809006,
+            "timestamp": 1.2874059521740149
+        },
+        {
+            "x": 3.330958715199463,
+            "y": 6.709678752193432,
+            "heading": -0.09840842897219951,
+            "angularVelocity": 4.7269412044437995e-7,
+            "velocityX": -3.7792123846106827,
+            "velocityY": 0.16790657317944163,
+            "timestamp": 1.3793635201864445
+        },
+        {
+            "x": 2.991492162018093,
+            "y": 6.7253542779205,
+            "heading": -0.07153342912076269,
+            "angularVelocity": 0.2922543563549246,
+            "velocityX": -3.6915564484642,
+            "velocityY": 0.17046477050097905,
+            "timestamp": 1.471321088198874
+        },
+        {
+            "x": 2.713831676976805,
+            "y": 6.738399816428541,
+            "heading": 0.08699262812295999,
+            "angularVelocity": 1.7239044123295555,
+            "velocityX": -3.0194413689122035,
+            "velocityY": 0.14186476208546492,
+            "timestamp": 1.5632786562113037
+        },
+        {
+            "x": 2.5064819943185284,
+            "y": 6.7478572981535265,
+            "heading": 0.2316797715135602,
+            "angularVelocity": 1.5734120259796696,
+            "velocityX": -2.2548408699787488,
+            "velocityY": 0.10284614882059591,
+            "timestamp": 1.6552362242237333
+        },
+        {
+            "x": 2.368710598807467,
+            "y": 6.7539967145328434,
+            "heading": 0.3372042735506754,
+            "angularVelocity": 1.147534719739997,
+            "velocityX": -1.4982061671361233,
+            "velocityY": 0.06676357924654308,
+            "timestamp": 1.747193792236163
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0.6067551334304849,
+            "velocityX": -0.7471989559160562,
+            "velocityY": 0.03265947036300673,
+            "timestamp": 1.8391513602485925
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0,
+            "velocityX": -4.676461204476467e-32,
+            "velocityY": 0,
+            "timestamp": 1.931108928261022
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C2 To W2.traj b/src/main/deploy/choreo/C2 To W2.traj
new file mode 100644
index 00000000..5429265f
--- /dev/null
+++ b/src/main/deploy/choreo/C2 To W2.traj	
@@ -0,0 +1,184 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -7.048442713490885e-38,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.598761685357721,
+            "y": 5.839328892781844,
+            "heading": -0.010877614975927053,
+            "angularVelocity": -0.10375029848266179,
+            "velocityX": -0.774848108801196,
+            "velocityY": 0.4037314494285639,
+            "timestamp": 0.10484418006512992
+        },
+        {
+            "x": 7.436285061134455,
+            "y": 5.923986667743732,
+            "heading": -0.03263383242489801,
+            "angularVelocity": -0.20751001567713226,
+            "velocityX": -1.5496961693280003,
+            "velocityY": 0.8074627977375359,
+            "timestamp": 0.20968836013025985
+        },
+        {
+            "x": 7.192570139480125,
+            "y": 6.050973310343331,
+            "heading": -0.06527034306657675,
+            "angularVelocity": -0.31128585889464805,
+            "velocityX": -2.3245441139692504,
+            "velocityY": 1.2111940073422762,
+            "timestamp": 0.3145325401953898
+        },
+        {
+            "x": 6.867616948374112,
+            "y": 6.220288799737083,
+            "heading": -0.10878948434468085,
+            "angularVelocity": -0.41508399656585776,
+            "velocityX": -3.0993917917441864,
+            "velocityY": 1.6149250181418864,
+            "timestamp": 0.4193767202605197
+        },
+        {
+            "x": 6.5158804421759,
+            "y": 6.403559919066418,
+            "heading": -0.10878948853448296,
+            "angularVelocity": -3.996218113987742e-8,
+            "velocityX": -3.3548500830442913,
+            "velocityY": 1.7480333120587623,
+            "timestamp": 0.5242209003256496
+        },
+        {
+            "x": 6.14377891604402,
+            "y": 6.540845489176542,
+            "heading": -0.10878948531130445,
+            "angularVelocity": 3.074256021150283e-8,
+            "velocityX": -3.549090907103546,
+            "velocityY": 1.309424805695852,
+            "timestamp": 0.6290650803907796
+        },
+        {
+            "x": 5.750100268474419,
+            "y": 6.5890532439035745,
+            "heading": -0.10878941324219087,
+            "angularVelocity": 6.873926003335433e-7,
+            "velocityX": -3.754892711498581,
+            "velocityY": 0.4598038221776824,
+            "timestamp": 0.7339092604559095
+        },
+        {
+            "x": 5.35529697720587,
+            "y": 6.626964014921626,
+            "heading": -0.10878934177449004,
+            "angularVelocity": 6.816563474487017e-7,
+            "velocityX": -3.7656195224503146,
+            "velocityY": 0.3615915637329853,
+            "timestamp": 0.8387534405210395
+        },
+        {
+            "x": 4.9606240931120595,
+            "y": 6.587718813133039,
+            "heading": -0.10878931155021784,
+            "angularVelocity": 2.882780157548741e-7,
+            "velocityX": -3.7643757035310585,
+            "velocityY": -0.3743193161910223,
+            "timestamp": 0.9435976205861695
+        },
+        {
+            "x": 4.585497747567555,
+            "y": 6.458927912145357,
+            "heading": -0.10878930710495487,
+            "angularVelocity": 4.239875765382173e-8,
+            "velocityX": -3.5779415253328617,
+            "velocityY": -1.2284029586351546,
+            "timestamp": 1.0484418006512994
+        },
+        {
+            "x": 4.215349929173217,
+            "y": 6.316458743577884,
+            "heading": -0.10878930270384918,
+            "angularVelocity": 4.197758707259389e-8,
+            "velocityX": -3.530456513317188,
+            "velocityY": -1.3588657804273945,
+            "timestamp": 1.1532859807164293
+        },
+        {
+            "x": 3.8452021463353314,
+            "y": 6.173989482631574,
+            "heading": -0.10878929830273087,
+            "angularVelocity": 4.197770719831182e-8,
+            "velocityX": -3.530456174181034,
+            "velocityY": -1.3588666615333942,
+            "timestamp": 1.2581301607815591
+        },
+        {
+            "x": 3.475054376591526,
+            "y": 6.0315201876656985,
+            "heading": -0.10878929390160265,
+            "angularVelocity": 4.197780205439048e-8,
+            "velocityX": -3.530456049290165,
+            "velocityY": -1.3588669860107905,
+            "timestamp": 1.362974340846689
+        },
+        {
+            "x": 3.104906616234201,
+            "y": 5.889050868312964,
+            "heading": -0.10878928950045777,
+            "angularVelocity": 4.19779608468647e-8,
+            "velocityX": -3.530455959762256,
+            "velocityY": -1.358867218611777,
+            "timestamp": 1.4678185209118189
+        },
+        {
+            "x": 2.7629439773689555,
+            "y": 5.757430528072475,
+            "heading": -0.06527490613894259,
+            "angularVelocity": 0.4150386157294016,
+            "velocityX": -3.2616272896866185,
+            "velocityY": -1.2553900479618894,
+            "timestamp": 1.5726627009769487
+        },
+        {
+            "x": 2.506471990883214,
+            "y": 5.658715266844828,
+            "heading": -0.03263773447849684,
+            "angularVelocity": 0.311292163667756,
+            "velocityX": -2.446220537243172,
+            "velocityY": -0.9415425936501655,
+            "timestamp": 1.6775068810420786
+        },
+        {
+            "x": 2.335490664707938,
+            "y": 5.592905089907167,
+            "heading": -0.010879217132893882,
+            "angularVelocity": 0.20753195200808147,
+            "velocityX": -1.6308137091544919,
+            "velocityY": -0.6276950890052175,
+            "timestamp": 1.7823510611072084
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0.10376557979790346,
+            "velocityX": -0.8154068700316104,
+            "velocityY": -0.3138475582214142,
+            "timestamp": 1.8871952411723383
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 2.158258486540926e-34,
+            "velocityY": 0,
+            "timestamp": 1.9920394212374681
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C2 To W3.traj b/src/main/deploy/choreo/C2 To W3.traj
new file mode 100644
index 00000000..cf63c7e3
--- /dev/null
+++ b/src/main/deploy/choreo/C2 To W3.traj	
@@ -0,0 +1,346 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": -2.981242670977675e-31,
+            "velocityX": -5.763030958279131e-32,
+            "velocityY": 3.5928963704249306e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6464505983184665,
+            "y": 5.761517456765686,
+            "heading": 0.04441523610418216,
+            "angularVelocity": 0.571175712894524,
+            "velocityX": -0.43144211544178024,
+            "velocityY": -0.4563021319898165,
+            "timestamp": 0.07776107264628128
+        },
+        {
+            "x": 7.57910774134765,
+            "y": 5.690410071738714,
+            "heading": 0.1294441379757556,
+            "angularVelocity": 1.0934635927458562,
+            "velocityX": -0.8660227370723712,
+            "velocityY": -0.914434209908928,
+            "timestamp": 0.15552214529256256
+        },
+        {
+            "x": 7.4776816969238515,
+            "y": 5.583411802725407,
+            "heading": 0.24873158333085876,
+            "angularVelocity": 1.5340252043296483,
+            "velocityX": -1.3043292867777738,
+            "velocityY": -1.3759875651409672,
+            "timestamp": 0.23328321793884382
+        },
+        {
+            "x": 7.341761998810456,
+            "y": 5.440105324158156,
+            "heading": 0.38885380810835574,
+            "angularVelocity": 1.8019584865410045,
+            "velocityX": -1.747914393255167,
+            "velocityY": -1.842907687489359,
+            "timestamp": 0.3110442905851251
+        },
+        {
+            "x": 7.171108690296084,
+            "y": 5.260470754663091,
+            "heading": 0.5147184350397563,
+            "angularVelocity": 1.6186071339824826,
+            "velocityX": -2.1945853202236263,
+            "velocityY": -2.3100834824152265,
+            "timestamp": 0.3888053632314064
+        },
+        {
+            "x": 6.970033766909185,
+            "y": 5.052418524467747,
+            "heading": 0.5282282166004705,
+            "angularVelocity": 0.17373450623768458,
+            "velocityX": -2.585804394720051,
+            "velocityY": -2.67553189681076,
+            "timestamp": 0.4665664358776877
+        },
+        {
+            "x": 6.768438732548067,
+            "y": 4.838192511419719,
+            "heading": 0.5282282675213732,
+            "angularVelocity": 6.548379670470849e-7,
+            "velocityX": -2.592492972391604,
+            "velocityY": -2.7549261572367425,
+            "timestamp": 0.5443275085239689
+        },
+        {
+            "x": 6.5423880710685225,
+            "y": 4.649952125007829,
+            "heading": 0.5282283002658353,
+            "angularVelocity": 4.210906690145718e-7,
+            "velocityX": -2.9069900116707847,
+            "velocityY": -2.42075346964613,
+            "timestamp": 0.6220885811702502
+        },
+        {
+            "x": 6.292219183977312,
+            "y": 4.49519788912002,
+            "heading": 0.5282283052888641,
+            "angularVelocity": 6.459567356551782e-8,
+            "velocityX": -3.217148099656167,
+            "velocityY": -1.9901247580747212,
+            "timestamp": 0.6998496538165315
+        },
+        {
+            "x": 6.0420493950115075,
+            "y": 4.3404451111724525,
+            "heading": 0.5282283103117955,
+            "angularVelocity": 6.459441933764687e-8,
+            "velocityX": -3.217159697677684,
+            "velocityY": -1.990106009101862,
+            "timestamp": 0.7776107264628128
+        },
+        {
+            "x": 5.780250458218128,
+            "y": 4.206300927357815,
+            "heading": 0.5282283154384368,
+            "angularVelocity": 6.592812032299238e-8,
+            "velocityX": -3.366709432935023,
+            "velocityY": -1.7250814482051704,
+            "timestamp": 0.8553717991090941
+        },
+        {
+            "x": 5.499389832161033,
+            "y": 4.11883296395985,
+            "heading": 0.5282284192975907,
+            "angularVelocity": 0.0000013356188425542589,
+            "velocityX": -3.611840944307328,
+            "velocityY": -1.1248296920470104,
+            "timestamp": 0.9331328717553754
+        },
+        {
+            "x": 5.2375954046767585,
+            "y": 3.9846908767107623,
+            "heading": 0.5282448470390027,
+            "angularVelocity": 0.0002112591919891304,
+            "velocityX": -3.3666514436488617,
+            "velocityY": -1.7250544865716144,
+            "timestamp": 1.0108939444016567
+        },
+        {
+            "x": 4.9990709441074195,
+            "y": 3.8334805845295827,
+            "heading": 0.56802081206443,
+            "angularVelocity": 0.5115151279660829,
+            "velocityX": -3.067401881842984,
+            "velocityY": -1.944549978997939,
+            "timestamp": 1.0886550170479379
+        },
+        {
+            "x": 4.779271759525853,
+            "y": 3.6735037985529155,
+            "heading": 0.6417219019727908,
+            "angularVelocity": 0.9477890080506808,
+            "velocityX": -2.826596613210087,
+            "velocityY": -2.057286255609729,
+            "timestamp": 1.166416089694219
+        },
+        {
+            "x": 4.5625385904493,
+            "y": 3.4912294193947493,
+            "heading": 0.6783171685956464,
+            "angularVelocity": 0.47061164895862556,
+            "velocityX": -2.7871679453603333,
+            "velocityY": -2.3440311836655594,
+            "timestamp": 1.2441771623405002
+        },
+        {
+            "x": 4.372028277853454,
+            "y": 3.267088484433369,
+            "heading": 0.6783172621445687,
+            "angularVelocity": 0.0000012030302442626518,
+            "velocityX": -2.4499445045265382,
+            "velocityY": -2.8824311102413667,
+            "timestamp": 1.3219382349867814
+        },
+        {
+            "x": 4.1999208824099785,
+            "y": 3.028525386131778,
+            "heading": 0.678317151805174,
+            "angularVelocity": -0.0000014189541221835924,
+            "velocityX": -2.213284739864076,
+            "velocityY": -3.067898759405809,
+            "timestamp": 1.3996993076330626
+        },
+        {
+            "x": 4.015537599692405,
+            "y": 2.8383590216563426,
+            "heading": 0.6532286408155417,
+            "angularVelocity": -0.32263586568249475,
+            "velocityX": -2.371151482905778,
+            "velocityY": -2.4455213644037763,
+            "timestamp": 1.4774603802793438
+        },
+        {
+            "x": 3.8266113642227695,
+            "y": 2.6983370404519986,
+            "heading": 0.6187227841336836,
+            "angularVelocity": -0.4437420357974964,
+            "velocityX": -2.429573423311953,
+            "velocityY": -1.8006693637223163,
+            "timestamp": 1.555221452925625
+        },
+        {
+            "x": 3.635277950307217,
+            "y": 2.6086556112169066,
+            "heading": 0.5790475746735078,
+            "angularVelocity": -0.5102194209775099,
+            "velocityX": -2.460529509229992,
+            "velocityY": -1.1532946522360021,
+            "timestamp": 1.6329825255719062
+        },
+        {
+            "x": 3.4425296783447266,
+            "y": 2.5693652629852295,
+            "heading": 0.5361762744054714,
+            "angularVelocity": -0.5513208448531661,
+            "velocityX": -2.4787244491760223,
+            "velocityY": -0.5052701421751292,
+            "timestamp": 1.7107435982181873
+        },
+        {
+            "x": 3.273952117174966,
+            "y": 2.5733990398146904,
+            "heading": 0.49726118807033537,
+            "angularVelocity": -0.5743772146540196,
+            "velocityX": -2.4881638242809143,
+            "velocityY": 0.05953756545439798,
+            "timestamp": 1.7784953915385004
+        },
+        {
+            "x": 3.1059881626789445,
+            "y": 2.6157002802478813,
+            "heading": 0.456882179671172,
+            "angularVelocity": -0.5959843484623638,
+            "velocityX": -2.4791071389347814,
+            "velocityY": 0.6243560260198714,
+            "timestamp": 1.8462471848588136
+        },
+        {
+            "x": 2.9403803042277463,
+            "y": 2.6962017103764078,
+            "heading": 0.41517848324514556,
+            "angularVelocity": -0.6155364217277942,
+            "velocityX": -2.444331734043506,
+            "velocityY": 1.1881815400506988,
+            "timestamp": 1.9139989781791267
+        },
+        {
+            "x": 2.7796953039379972,
+            "y": 2.8146585273254012,
+            "heading": 0.3723609476398507,
+            "angularVelocity": -0.6319764172568042,
+            "velocityX": -2.3716715442508174,
+            "velocityY": 1.7483938231562413,
+            "timestamp": 1.9817507714994398
+        },
+        {
+            "x": 2.628009703912687,
+            "y": 2.970316258543535,
+            "heading": 0.3287788631859365,
+            "angularVelocity": -0.6432609724124856,
+            "velocityX": -2.23884258396202,
+            "velocityY": 2.2974702748047298,
+            "timestamp": 2.0495025648197527
+        },
+        {
+            "x": 2.4923790399755665,
+            "y": 3.160718496478581,
+            "heading": 0.28508218813026037,
+            "angularVelocity": -0.6449523018392979,
+            "velocityX": -2.001875630005736,
+            "velocityY": 2.8102907480968544,
+            "timestamp": 2.1172543581400656
+        },
+        {
+            "x": 2.384956097652455,
+            "y": 3.376988776463332,
+            "heading": 0.24257394609876834,
+            "angularVelocity": -0.627411319291935,
+            "velocityX": -1.5855365158416335,
+            "velocityY": 3.192096760631575,
+            "timestamp": 2.1850061514603785
+        },
+        {
+            "x": 2.3144199991578422,
+            "y": 3.6018152629752547,
+            "heading": 0.17996830737750358,
+            "angularVelocity": -0.9240440090682317,
+            "velocityX": -1.0410956675513523,
+            "velocityY": 3.3183842890918167,
+            "timestamp": 2.2527579447806914
+        },
+        {
+            "x": 2.277461106698391,
+            "y": 3.8129334421080285,
+            "heading": 0.07321465766316056,
+            "angularVelocity": -1.57565791963083,
+            "velocityX": -0.5455042685692306,
+            "velocityY": 3.1160530044520236,
+            "timestamp": 2.3205097381010042
+        },
+        {
+            "x": 2.2642620699526455,
+            "y": 3.9942911908489713,
+            "heading": -0.04721496632936169,
+            "angularVelocity": -1.7775119755599884,
+            "velocityX": -0.1948145738865332,
+            "velocityY": 2.676796286166625,
+            "timestamp": 2.388261531421317
+        },
+        {
+            "x": 2.2685016834700633,
+            "y": 4.1415779064179725,
+            "heading": -0.1677701441610935,
+            "angularVelocity": -1.7793651197065368,
+            "velocityX": 0.06257566493294611,
+            "velocityY": 2.173916118687331,
+            "timestamp": 2.45601332474163
+        },
+        {
+            "x": 2.2803498467055876,
+            "y": 4.251629868834139,
+            "heading": -0.2721018795383186,
+            "angularVelocity": -1.5399110527447033,
+            "velocityX": 0.17487600925202312,
+            "velocityY": 1.624340213341216,
+            "timestamp": 2.523765118061943
+        },
+        {
+            "x": 2.2924250535428135,
+            "y": 4.3241863896986095,
+            "heading": -0.3487735727111741,
+            "angularVelocity": -1.1316555535345214,
+            "velocityX": 0.17822711762236845,
+            "velocityY": 1.0709166105971757,
+            "timestamp": 2.591516911382256
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.6084920452794201,
+            "velocityX": 0.1118043683563332,
+            "velocityY": 0.5286001823165565,
+            "timestamp": 2.6592687047025687
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -7.720140427747849e-31,
+            "velocityX": 0,
+            "velocityY": -1.167304580662352e-31,
+            "timestamp": 2.7270204980228816
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C3 To C1.traj b/src/main/deploy/choreo/C3 To C1.traj
new file mode 100644
index 00000000..57334e24
--- /dev/null
+++ b/src/main/deploy/choreo/C3 To C1.traj	
@@ -0,0 +1,139 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": -2.1749467140472343e-39,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 4.203445372005539,
+            "heading": -7.837837605587582e-19,
+            "angularVelocity": -8.185686568731239e-18,
+            "velocityX": 1.377912473786286e-33,
+            "velocityY": 0.7983807350946673,
+            "timestamp": 0.09575052183150974
+        },
+        {
+            "x": 7.68,
+            "y": 4.356336112982689,
+            "heading": -2.3565356003209568e-18,
+            "angularVelocity": -1.6425517161035916e-17,
+            "velocityX": 3.260516050321855e-34,
+            "velocityY": 1.5967614385035824,
+            "timestamp": 0.19150104366301948
+        },
+        {
+            "x": 7.68,
+            "y": 4.5856722168636,
+            "heading": -4.728579560129296e-18,
+            "angularVelocity": -2.4773170043438095e-17,
+            "velocityX": -1.693775140889528e-33,
+            "velocityY": 2.3951420785410225,
+            "timestamp": 0.28725156549452924
+        },
+        {
+            "x": 7.68,
+            "y": 4.891453665444746,
+            "heading": -7.932188825954499e-18,
+            "angularVelocity": -3.345787785122934e-17,
+            "velocityX": -9.607950389092107e-34,
+            "velocityY": 3.1935225284643685,
+            "timestamp": 0.38300208732603896
+        },
+        {
+            "x": 7.68,
+            "y": 5.253672199266821,
+            "heading": -4.35728250993897e-17,
+            "angularVelocity": -3.722239377050204e-16,
+            "velocityX": -3.660377688643743e-33,
+            "velocityY": 3.7829405719528495,
+            "timestamp": 0.4787526091575487
+        },
+        {
+            "x": 7.68,
+            "y": 5.61589073308894,
+            "heading": -3.405116375197899e-17,
+            "angularVelocity": 9.944239639932065e-17,
+            "velocityX": -5.791062266320423e-33,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.5745031309890585
+        },
+        {
+            "x": 7.68,
+            "y": 5.978109266911059,
+            "heading": 5.876472199762529e-18,
+            "angularVelocity": 4.169965362904391e-16,
+            "velocityX": -1.6171107443968654e-32,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.6702536528205683
+        },
+        {
+            "x": 7.68,
+            "y": 6.340327800733179,
+            "heading": 2.547705878984039e-17,
+            "angularVelocity": 2.047047495424479e-16,
+            "velocityX": -4.3724642588945734e-33,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.766004174652078
+        },
+        {
+            "x": 7.68,
+            "y": 6.702546334555254,
+            "heading": 7.687233753276096e-18,
+            "angularVelocity": -1.8579350479017399e-16,
+            "velocityX": -2.6103711792186515e-33,
+            "velocityY": 3.7829405719528495,
+            "timestamp": 0.8617546964835878
+        },
+        {
+            "x": 7.68,
+            "y": 7.0083277831364,
+            "heading": 4.613320516549708e-18,
+            "angularVelocity": -3.210335754994756e-17,
+            "velocityX": 1.2273832122580678e-32,
+            "velocityY": 3.1935225284643685,
+            "timestamp": 0.9575052183150976
+        },
+        {
+            "x": 7.68,
+            "y": 7.237663887017311,
+            "heading": 2.3068261479777784e-18,
+            "angularVelocity": -2.4088582761110343e-17,
+            "velocityX": 8.179933156696061e-33,
+            "velocityY": 2.3951420785410225,
+            "timestamp": 1.0532557401466074
+        },
+        {
+            "x": 7.68,
+            "y": 7.390554627994461,
+            "heading": 7.689558058679801e-19,
+            "angularVelocity": -1.6061221530256843e-17,
+            "velocityX": 9.843160057308665e-33,
+            "velocityY": 1.5967614385035824,
+            "timestamp": 1.1490062619781172
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": -8.030826271263815e-18,
+            "velocityX": 3.1854559448557055e-33,
+            "velocityY": 0.7983807350946673,
+            "timestamp": 1.244756783809627
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 2.257351446612169e-43,
+            "velocityY": -1.1946741683702123e-38,
+            "timestamp": 1.3405073056411367
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C3 To C2.traj b/src/main/deploy/choreo/C3 To C2.traj
new file mode 100644
index 00000000..5ffe4bce
--- /dev/null
+++ b/src/main/deploy/choreo/C3 To C2.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -3.817834980565894e-40,
+            "angularVelocity": -1.9855221195343998e-39,
+            "velocityX": -9.621145750064654e-40,
+            "velocityY": 1.4575297691012433e-39,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 4.210500005575846,
+            "heading": 3.479114156010015e-22,
+            "angularVelocity": 3.4766412561552746e-21,
+            "velocityX": 2.7068848151306367e-33,
+            "velocityY": 0.8344065481228213,
+            "timestamp": 0.10007112931184182
+        },
+        {
+            "x": 7.68,
+            "y": 4.377500013629847,
+            "heading": 1.081691491825702e-21,
+            "angularVelocity": 7.332585052389997e-21,
+            "velocityX": 5.1589068200097806e-33,
+            "velocityY": 1.6688130652907358,
+            "timestamp": 0.20014225862368365
+        },
+        {
+            "x": 7.68,
+            "y": 4.628000017966616,
+            "heading": 2.2511586271387426e-21,
+            "angularVelocity": 1.1686358860616172e-20,
+            "velocityX": 5.8498916949463956e-33,
+            "velocityY": 2.503219520548835,
+            "timestamp": 0.3002133879355255
+        },
+        {
+            "x": 7.68,
+            "y": 4.962,
+            "heading": 4.481673966100434e-21,
+            "angularVelocity": 2.228929929167172e-20,
+            "velocityX": 6.576791251337367e-33,
+            "velocityY": 3.3376257900775013,
+            "timestamp": 0.4002845172473673
+        },
+        {
+            "x": 7.68,
+            "y": 5.295999982033384,
+            "heading": 1.4391637904192512e-21,
+            "angularVelocity": -3.0403475801105514e-20,
+            "velocityX": 1.0509973010204301e-32,
+            "velocityY": 3.3376257900775013,
+            "timestamp": 0.5003556465592092
+        },
+        {
+            "x": 7.68,
+            "y": 5.5464999863701525,
+            "heading": 4.7457500532831525e-22,
+            "angularVelocity": -9.63903176962576e-21,
+            "velocityX": 7.983324746463204e-33,
+            "velocityY": 2.503219520548835,
+            "timestamp": 0.600426775871051
+        },
+        {
+            "x": 7.68,
+            "y": 5.713499994424153,
+            "heading": 1.0909640932435146e-22,
+            "angularVelocity": -3.652188228166023e-21,
+            "velocityX": 7.341551448907972e-33,
+            "velocityY": 1.6688130652907358,
+            "timestamp": 0.7004979051828928
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -9.42965601107167e-40,
+            "angularVelocity": -1.09018866193586e-21,
+            "velocityX": 7.44470103227488e-33,
+            "velocityY": 0.8344065481228213,
+            "timestamp": 0.8005690344947346
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 8.249839679439821e-43,
+            "angularVelocity": 9.431229878048832e-39,
+            "velocityX": 1.012759498983901e-41,
+            "velocityY": 5.142787784847508e-39,
+            "timestamp": 0.9006401638065764
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C3 To C4.traj b/src/main/deploy/choreo/C3 To C4.traj
new file mode 100644
index 00000000..1862a6fc
--- /dev/null
+++ b/src/main/deploy/choreo/C3 To C4.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -1.6773104267984464e-40,
+            "angularVelocity": -1.9855220924111835e-39,
+            "velocityX": -5.3948128499400994e-40,
+            "velocityY": -2.7550648847397363e-40,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 4.043499994424153,
+            "heading": -2.181197470484939e-22,
+            "angularVelocity": -2.1796471324426728e-21,
+            "velocityX": 2.2751291494664054e-33,
+            "velocityY": -0.8344065481228213,
+            "timestamp": 0.10007112931184182
+        },
+        {
+            "x": 7.68,
+            "y": 3.8764999863701526,
+            "heading": -1.0976856482379068e-21,
+            "angularVelocity": -8.789407177499062e-21,
+            "velocityX": 3.967849397509383e-33,
+            "velocityY": -1.668813065290736,
+            "timestamp": 0.20014225862368365
+        },
+        {
+            "x": 7.68,
+            "y": 3.6259999820333837,
+            "heading": -3.357689035831156e-21,
+            "angularVelocity": -2.2583970074080117e-20,
+            "velocityX": 6.1318806903820435e-33,
+            "velocityY": -2.503219520548835,
+            "timestamp": 0.3002133879355255
+        },
+        {
+            "x": 7.68,
+            "y": 3.2919999999999994,
+            "heading": -1.0603568936345678e-20,
+            "angularVelocity": -7.2407296109284e-20,
+            "velocityX": 8.195284274358776e-33,
+            "velocityY": -3.3376257900775017,
+            "timestamp": 0.4002845172473673
+        },
+        {
+            "x": 7.68,
+            "y": 2.9580000179666155,
+            "heading": -4.785144304302103e-21,
+            "angularVelocity": 5.814288954429956e-20,
+            "velocityX": 7.586727405106972e-33,
+            "velocityY": -3.3376257900775013,
+            "timestamp": 0.5003556465592092
+        },
+        {
+            "x": 7.68,
+            "y": 2.7075000136298466,
+            "heading": -2.6182090767908737e-21,
+            "angularVelocity": 2.165395003165498e-20,
+            "velocityX": 5.326134827450793e-33,
+            "velocityY": -2.503219520548835,
+            "timestamp": 0.600426775871051
+        },
+        {
+            "x": 7.68,
+            "y": 2.540500005575846,
+            "heading": -9.668053802454109e-22,
+            "angularVelocity": 1.6502299039719674e-20,
+            "velocityX": 3.4348416075821124e-33,
+            "velocityY": -1.6688130652907358,
+            "timestamp": 0.7004979051828928
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 1.8181769494337196e-40,
+            "angularVelocity": 9.661181877631647e-21,
+            "velocityX": 1.7280403247932335e-33,
+            "velocityY": -0.8344065481228213,
+            "timestamp": 0.8005690344947346
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -1.6883012335149235e-41,
+            "angularVelocity": -1.9855220810412424e-39,
+            "velocityX": -1.2101303998889038e-42,
+            "velocityY": -7.163168700323315e-39,
+            "timestamp": 0.9006401638065764
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C3 To C5.traj b/src/main/deploy/choreo/C3 To C5.traj
new file mode 100644
index 00000000..9c555123
--- /dev/null
+++ b/src/main/deploy/choreo/C3 To C5.traj	
@@ -0,0 +1,139 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": -1.1842498081533988e-39,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 4.050554627994461,
+            "heading": 1.1070575354189048e-18,
+            "angularVelocity": 1.156189558439314e-17,
+            "velocityX": 1.1339790734570631e-33,
+            "velocityY": -0.7983807350946678,
+            "timestamp": 0.09575052183150978
+        },
+        {
+            "x": 7.68,
+            "y": 3.897663887017311,
+            "heading": 3.320268139408775e-18,
+            "angularVelocity": 2.3114345091427606e-17,
+            "velocityX": 5.50963145740529e-34,
+            "velocityY": -1.596761438503583,
+            "timestamp": 0.19150104366301957
+        },
+        {
+            "x": 7.68,
+            "y": 3.6683277831364007,
+            "heading": 6.638014774282681e-18,
+            "angularVelocity": 3.4649906568297676e-17,
+            "velocityX": 4.767145470149766e-34,
+            "velocityY": -2.3951420785410233,
+            "timestamp": 0.28725156549452935
+        },
+        {
+            "x": 7.68,
+            "y": 3.362546334555254,
+            "heading": 1.1056344671920077e-17,
+            "angularVelocity": 4.614418609717799e-17,
+            "velocityX": -5.447572473811757e-34,
+            "velocityY": -3.19352252846437,
+            "timestamp": 0.38300208732603913
+        },
+        {
+            "x": 7.68,
+            "y": 3.0003278007331793,
+            "heading": 2.1759975564527066e-17,
+            "angularVelocity": 1.11786658577609e-16,
+            "velocityX": -1.9220305893434716e-33,
+            "velocityY": -3.7829405719528495,
+            "timestamp": 0.4787526091575489
+        },
+        {
+            "x": 7.68,
+            "y": 2.63810926691106,
+            "heading": -1.7246492437320545e-17,
+            "angularVelocity": -4.0737603572006183e-16,
+            "velocityX": 5.877602560652089e-33,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.5745031309890587
+        },
+        {
+            "x": 7.68,
+            "y": 2.27589073308894,
+            "heading": -9.722837303851897e-18,
+            "angularVelocity": 7.857560449339153e-17,
+            "velocityX": 3.350655179860002e-33,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.6702536528205685
+        },
+        {
+            "x": 7.68,
+            "y": 1.9136721992668206,
+            "heading": -4.541786403704224e-17,
+            "angularVelocity": -3.727919811862982e-16,
+            "velocityX": 1.4553646654154268e-32,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.7660041746520783
+        },
+        {
+            "x": 7.68,
+            "y": 1.5514536654447455,
+            "heading": -1.11893051502424e-17,
+            "angularVelocity": 3.5747647356983727e-16,
+            "velocityX": -5.35720215261278e-33,
+            "velocityY": -3.7829405719528495,
+            "timestamp": 0.861754696483588
+        },
+        {
+            "x": 7.68,
+            "y": 1.2456722168635992,
+            "heading": -6.7032993083141955e-18,
+            "angularVelocity": 4.6850980602319425e-17,
+            "velocityX": -4.3527390380082413e-33,
+            "velocityY": -3.19352252846437,
+            "timestamp": 0.9575052183150978
+        },
+        {
+            "x": 7.68,
+            "y": 1.016336112982689,
+            "heading": -3.348658686079055e-18,
+            "angularVelocity": 3.5035220254491646e-17,
+            "velocityX": -6.634490282826358e-33,
+            "velocityY": -2.395142078541024,
+            "timestamp": 1.0532557401466076
+        },
+        {
+            "x": 7.68,
+            "y": 0.8634453720055388,
+            "heading": -1.115588951241814e-18,
+            "angularVelocity": 2.3321750022200153e-17,
+            "velocityX": -4.820835446954302e-33,
+            "velocityY": -1.596761438503583,
+            "timestamp": 1.1490062619781174
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 1.1650996045214628e-17,
+            "velocityX": -3.709395222464697e-33,
+            "velocityY": -0.7983807350946678,
+            "timestamp": 1.2447567838096272
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 6.978360472267901e-43,
+            "velocityY": -3.6401965239950315e-38,
+            "timestamp": 1.340507305641137
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C3 To S1.traj b/src/main/deploy/choreo/C3 To S1.traj
new file mode 100644
index 00000000..e393ae58
--- /dev/null
+++ b/src/main/deploy/choreo/C3 To S1.traj	
@@ -0,0 +1,157 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -7.709535672280055e-33,
+            "angularVelocity": 3.0776034223095e-32,
+            "velocityX": -2.0787724059290825e-32,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.653625318689537,
+            "y": 4.193887618013532,
+            "heading": -0.05699658004084171,
+            "angularVelocity": -0.6065329552716214,
+            "velocityX": -0.28066795214939116,
+            "velocityY": 0.7117891037630026,
+            "timestamp": 0.0939711182145365
+        },
+        {
+            "x": 7.600609429037226,
+            "y": 4.328102182971276,
+            "heading": -0.1617251039431606,
+            "angularVelocity": -1.1144756590341216,
+            "velocityX": -0.5641721697008566,
+            "velocityY": 1.4282533560080801,
+            "timestamp": 0.187942236429073
+        },
+        {
+            "x": 7.520673672603614,
+            "y": 4.5302373715153195,
+            "heading": -0.299017155202882,
+            "angularVelocity": -1.4610026343017766,
+            "velocityX": -0.8506417498525306,
+            "velocityY": 2.151035258328725,
+            "timestamp": 0.2819133546436095
+        },
+        {
+            "x": 7.413680530437971,
+            "y": 4.80084724043498,
+            "heading": -0.43363333911195256,
+            "angularVelocity": -1.4325272111984575,
+            "velocityX": -1.1385747472045327,
+            "velocityY": 2.8797131933862543,
+            "timestamp": 0.375884472858146
+        },
+        {
+            "x": 7.279380160390009,
+            "y": 5.1284214711267,
+            "heading": -0.4371879316114584,
+            "angularVelocity": -0.037826436111898676,
+            "velocityX": -1.4291664566697349,
+            "velocityY": 3.4859032957750506,
+            "timestamp": 0.46985559107268254
+        },
+        {
+            "x": 7.147763767829931,
+            "y": 5.458645997615663,
+            "heading": -0.43718797458387054,
+            "angularVelocity": -4.5729382513446123e-7,
+            "velocityX": -1.4006047289934083,
+            "velocityY": 3.514106597466034,
+            "timestamp": 0.563826709287219
+        },
+        {
+            "x": 6.993957305230812,
+            "y": 5.779137314807096,
+            "heading": -0.43718801851214967,
+            "angularVelocity": -4.6746574877776706e-7,
+            "velocityX": -1.6367418577267254,
+            "velocityY": 3.410529993478958,
+            "timestamp": 0.6577978275017555
+        },
+        {
+            "x": 6.777425503833482,
+            "y": 6.0610683140345785,
+            "heading": -0.43718807107725144,
+            "angularVelocity": -5.593750798607861e-7,
+            "velocityX": -2.3042377861566687,
+            "velocityY": 3.000187766030755,
+            "timestamp": 0.751768945716292
+        },
+        {
+            "x": 6.507457353311083,
+            "y": 6.292343710685516,
+            "heading": -0.4371880918313372,
+            "angularVelocity": -2.2085600557820128e-7,
+            "velocityX": -2.87288430372895,
+            "velocityY": 2.4611327506280603,
+            "timestamp": 0.8457400639308286
+        },
+        {
+            "x": 6.1957042493791805,
+            "y": 6.462993619393473,
+            "heading": -0.4369141825473297,
+            "angularVelocity": 0.0029148241418374306,
+            "velocityX": -3.3175417070186306,
+            "velocityY": 1.8159825268692036,
+            "timestamp": 0.9397111821453651
+        },
+        {
+            "x": 5.885878112264826,
+            "y": 6.564806359719212,
+            "heading": -0.3410861416211598,
+            "angularVelocity": 1.0197605684269313,
+            "velocityX": -3.297035759508787,
+            "velocityY": 1.083447151211916,
+            "timestamp": 1.0336823003599016
+        },
+        {
+            "x": 5.602663899704114,
+            "y": 6.605182844765102,
+            "heading": -0.13545254769192583,
+            "angularVelocity": 2.1882637754695176,
+            "velocityX": -3.0138431673669435,
+            "velocityY": 0.4296690920896744,
+            "timestamp": 1.127653418574438
+        },
+        {
+            "x": 5.388992479168701,
+            "y": 6.621862905384601,
+            "heading": 0.0527880784098822,
+            "angularVelocity": 2.0031753338515554,
+            "velocityX": -2.2737988500636965,
+            "velocityY": 0.17750199142483714,
+            "timestamp": 1.2216245367889744
+        },
+        {
+            "x": 5.246852509457665,
+            "y": 6.628278045699216,
+            "heading": 0.19017469868317557,
+            "angularVelocity": 1.4620089968455965,
+            "velocityX": -1.5125920858633375,
+            "velocityY": 0.06826714884852915,
+            "timestamp": 1.3155956550035108
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.263,
+            "angularVelocity": 0.7749753615846516,
+            "velocityX": -0.7539817638001085,
+            "velocityY": 0.01832429296896703,
+            "timestamp": 1.4095667732180472
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.263,
+            "angularVelocity": 6.971645129510918e-32,
+            "velocityX": 3.634932162806737e-32,
+            "velocityY": 0,
+            "timestamp": 1.5035378914325837
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C3 To S2.traj b/src/main/deploy/choreo/C3 To S2.traj
new file mode 100644
index 00000000..67cbd06a
--- /dev/null
+++ b/src/main/deploy/choreo/C3 To S2.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 1.358270377512338e-32,
+            "angularVelocity": -4.2989327725546786e-32,
+            "velocityX": 9.28706448380456e-34,
+            "velocityY": 4.1113328098198935e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 7.585313636391529,
+            "y": 4.127746947940637,
+            "heading": -0.08924586799649764,
+            "angularVelocity": -0.8241960155421162,
+            "velocityX": -0.8744396280098535,
+            "velocityY": 0.006898151480975746,
+            "timestamp": 0.10828233370892486
+        },
+        {
+            "x": 7.394793136195202,
+            "y": 4.129317895600586,
+            "heading": -0.2483761167773939,
+            "angularVelocity": -1.4695864351119023,
+            "velocityX": -1.7594790735529262,
+            "velocityY": 0.014507885138232623,
+            "timestamp": 0.21656466741784972
+        },
+        {
+            "x": 7.106938899563738,
+            "y": 4.131715387581193,
+            "heading": -0.440143741107867,
+            "angularVelocity": -1.7709964106053178,
+            "velocityX": -2.6583674988502586,
+            "velocityY": 0.022141118486160887,
+            "timestamp": 0.32484700112677456
+        },
+        {
+            "x": 6.726884682874397,
+            "y": 4.136782702937484,
+            "heading": -0.5152103460374668,
+            "angularVelocity": -0.6932488648738155,
+            "velocityX": -3.509845084341914,
+            "velocityY": 0.04679724921622947,
+            "timestamp": 0.43312933483569943
+        },
+        {
+            "x": 6.317264615210477,
+            "y": 4.138917325587368,
+            "heading": -0.515210397277194,
+            "angularVelocity": -4.7320486478177106e-7,
+            "velocityX": -3.782889171607859,
+            "velocityY": 0.019713489511795114,
+            "timestamp": 0.5414116685446243
+        },
+        {
+            "x": 5.907644548289307,
+            "y": 4.14105209076814,
+            "heading": -0.5152104485168384,
+            "angularVelocity": -4.732041015646636e-7,
+            "velocityX": -3.7828891647484775,
+            "velocityY": 0.019714805801191258,
+            "timestamp": 0.6496940022535491
+        },
+        {
+            "x": 5.500258277815471,
+            "y": 4.183825709430579,
+            "heading": -0.5152105024899405,
+            "angularVelocity": -4.984479029228616e-7,
+            "velocityX": -3.762259793632967,
+            "velocityY": 0.3950193646307994,
+            "timestamp": 0.7579763359624739
+        },
+        {
+            "x": 5.114610765355458,
+            "y": 4.3219164843017905,
+            "heading": -0.5152109227554116,
+            "angularVelocity": -0.000003881200715936794,
+            "velocityX": -3.5614998241234512,
+            "velocityY": 1.2752844359857862,
+            "timestamp": 0.8662586696713988
+        },
+        {
+            "x": 4.790273412388094,
+            "y": 4.533322369220829,
+            "heading": -0.5901314422767667,
+            "angularVelocity": -0.6918997490647923,
+            "velocityX": -2.9952933397170605,
+            "velocityY": 1.952358041038544,
+            "timestamp": 0.9745410033803236
+        },
+        {
+            "x": 4.547771251602889,
+            "y": 4.790487049727204,
+            "heading": -0.7662492650374113,
+            "angularVelocity": -1.6264686650923976,
+            "velocityX": -2.239535780943542,
+            "velocityY": 2.3749458632620883,
+            "timestamp": 1.0828233370892484
+        },
+        {
+            "x": 4.394005608234175,
+            "y": 5.057835455573767,
+            "heading": -0.8024780004938983,
+            "angularVelocity": -0.3345766037318139,
+            "velocityX": -1.4200436775037824,
+            "velocityY": 2.468993756315091,
+            "timestamp": 1.1911056707981733
+        },
+        {
+            "x": 4.299277084870805,
+            "y": 5.304155354811625,
+            "heading": -0.5153879845736841,
+            "angularVelocity": 2.651309831315093,
+            "velocityX": -0.8748289782709269,
+            "velocityY": 2.2747930414945974,
+            "timestamp": 1.2993880045070982
+        },
+        {
+            "x": 4.268346904630433,
+            "y": 5.476762087961931,
+            "heading": -0.20205360473699635,
+            "angularVelocity": 2.893679597624539,
+            "velocityX": -0.2856438273995415,
+            "velocityY": 1.5940433424190128,
+            "timestamp": 1.4076703382160232
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 2.864290108945775e-30,
+            "angularVelocity": 1.8659886411401074,
+            "velocityX": -0.04937928882107947,
+            "velocityY": 0.7687118404911842,
+            "timestamp": 1.515952671924948
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": -1.339793214188515e-31,
+            "velocityX": -1.755749143439243e-31,
+            "velocityY": 0,
+            "timestamp": 1.624235005633873
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C3 To S3.traj b/src/main/deploy/choreo/C3 To S3.traj
new file mode 100644
index 00000000..c2db4e34
--- /dev/null
+++ b/src/main/deploy/choreo/C3 To S3.traj	
@@ -0,0 +1,166 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 4.8594393424661863e-32,
+            "velocityX": 8.070887902586468e-34,
+            "velocityY": -1.5484627527783442e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.613090966537703,
+            "y": 4.130415229492922,
+            "heading": -0.02261774188189381,
+            "angularVelocity": -0.25176724339812206,
+            "velocityX": -0.7447915447554241,
+            "velocityY": 0.038016302406854975,
+            "timestamp": 0.08983591977315897
+        },
+        {
+            "x": 7.479193313628693,
+            "y": 4.137259099498979,
+            "heading": -0.06454119971556918,
+            "angularVelocity": -0.46666698504702897,
+            "velocityX": -1.4904689933291897,
+            "velocityY": 0.0761818883097586,
+            "timestamp": 0.17967183954631794
+        },
+        {
+            "x": 7.278200146895047,
+            "y": 4.147551727639681,
+            "heading": -0.12012980810986089,
+            "angularVelocity": -0.6187793093216658,
+            "velocityX": -2.237336326507015,
+            "velocityY": 0.11457141160316474,
+            "timestamp": 0.2695077593194769
+        },
+        {
+            "x": 7.010006404514947,
+            "y": 4.161329433657577,
+            "heading": -0.1775477987284744,
+            "angularVelocity": -0.6391429036434741,
+            "velocityX": -2.985373145364283,
+            "velocityY": 0.15336522465649807,
+            "timestamp": 0.35934367909263587
+        },
+        {
+            "x": 6.675632733992502,
+            "y": 4.178685588756415,
+            "heading": -0.19285230682653456,
+            "angularVelocity": -0.17036067687364617,
+            "velocityX": -3.7220487235639528,
+            "velocityY": 0.19319839038333309,
+            "timestamp": 0.44917959886579484
+        },
+        {
+            "x": 6.3362226416697665,
+            "y": 4.195852203162663,
+            "heading": -0.19285232652882817,
+            "angularVelocity": -2.193200640663355e-7,
+            "velocityX": -3.7781111740035236,
+            "velocityY": 0.1910885362012055,
+            "timestamp": 0.5390155186389538
+        },
+        {
+            "x": 5.996812544590127,
+            "y": 4.213018723498308,
+            "heading": -0.19285234623213549,
+            "angularVelocity": -2.1933156264800868e-7,
+            "velocityX": -3.7781112269558137,
+            "velocityY": 0.1910874890356692,
+            "timestamp": 0.6288514384121128
+        },
+        {
+            "x": 5.65703115405788,
+            "y": 4.206499194085455,
+            "heading": -0.1928524148916534,
+            "angularVelocity": -7.642946895727943e-7,
+            "velocityX": -3.782244244720454,
+            "velocityY": -0.07257152185432766,
+            "timestamp": 0.7186873581852717
+        },
+        {
+            "x": 5.325195518902672,
+            "y": 4.133157682156235,
+            "heading": -0.192852601151229,
+            "angularVelocity": -0.0000020733763948999505,
+            "velocityX": -3.6937968241559735,
+            "velocityY": -0.8163940672507154,
+            "timestamp": 0.8085232779584307
+        },
+        {
+            "x": 5.022921580906399,
+            "y": 3.999719932132448,
+            "heading": -0.21619143682617745,
+            "angularVelocity": -0.2597940304978627,
+            "velocityX": -3.364733602738329,
+            "velocityY": -1.485349628067107,
+            "timestamp": 0.8983591977315897
+        },
+        {
+            "x": 4.76405537852928,
+            "y": 3.8156128777716756,
+            "heading": -0.2707600447886353,
+            "angularVelocity": -0.6074252715762222,
+            "velocityX": -2.881544520603651,
+            "velocityY": -2.0493701720293274,
+            "timestamp": 0.9881951175047486
+        },
+        {
+            "x": 4.5584097754694755,
+            "y": 3.590752248351908,
+            "heading": -0.3464948618413025,
+            "angularVelocity": -0.8430349156856024,
+            "velocityX": -2.2891244791432657,
+            "velocityY": -2.5030147182477354,
+            "timestamp": 1.0780310372779076
+        },
+        {
+            "x": 4.420003796304514,
+            "y": 3.3682412112408735,
+            "heading": -0.42679529224450924,
+            "angularVelocity": -0.8938566066393836,
+            "velocityX": -1.5406529984228516,
+            "velocityY": -2.476860454851527,
+            "timestamp": 1.1678669570510665
+        },
+        {
+            "x": 4.3340794956684014,
+            "y": 3.187695881574611,
+            "heading": -0.4942860320434666,
+            "angularVelocity": -0.7512667535241839,
+            "velocityX": -0.9564581834687845,
+            "velocityY": -2.009723172222817,
+            "timestamp": 1.2577028768242253
+        },
+        {
+            "x": 4.284833149556787,
+            "y": 3.063288277698013,
+            "heading": -0.5416494132844137,
+            "angularVelocity": -0.5272209752980105,
+            "velocityX": -0.5481810197605272,
+            "velocityY": -1.3848314147664058,
+            "timestamp": 1.3475387965973842
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -0.27105624093568903,
+            "velocityX": -0.24303362855860308,
+            "velocityY": -0.7044874461979944,
+            "timestamp": 1.437374716370543
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 6.221864662981848e-31,
+            "velocityX": 0,
+            "velocityY": 2.6923638006152617e-32,
+            "timestamp": 1.527210636143702
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C3 To S4.traj b/src/main/deploy/choreo/C3 To S4.traj
new file mode 100644
index 00000000..2274eedc
--- /dev/null
+++ b/src/main/deploy/choreo/C3 To S4.traj	
@@ -0,0 +1,238 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 3.185769476987751e-31,
+            "velocityX": 7.333541525795099e-32,
+            "velocityY": -1.3517842061565195e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 7.679078910576486,
+            "y": 4.080614551713116,
+            "heading": -0.023748331346269917,
+            "angularVelocity": -0.3167839884646853,
+            "velocityX": -0.012286605616975374,
+            "velocityY": -0.6187452541731726,
+            "timestamp": 0.07496695606800095
+        },
+        {
+            "x": 7.6772274783441,
+            "y": 3.987757014244699,
+            "heading": -0.06889026621284731,
+            "angularVelocity": -0.6021577670253255,
+            "velocityX": -0.024696644088144176,
+            "velocityY": -1.2386462294692595,
+            "timestamp": 0.1499339121360019
+        },
+        {
+            "x": 7.674435217742111,
+            "y": 3.848312298051719,
+            "heading": -0.1318457680879735,
+            "angularVelocity": -0.8397766853177616,
+            "velocityX": -0.037246551660047825,
+            "velocityY": -1.8600824083946148,
+            "timestamp": 0.22490086820400285
+        },
+        {
+            "x": 7.670687411814683,
+            "y": 3.6621317365631327,
+            "heading": -0.20653378464827088,
+            "angularVelocity": -0.996279167218362,
+            "velocityX": -0.049992771802326974,
+            "velocityY": -2.483501681990474,
+            "timestamp": 0.2998678242720038
+        },
+        {
+            "x": 7.665952301925262,
+            "y": 3.4291021726627307,
+            "heading": -0.27999342706200125,
+            "angularVelocity": -0.9798936260280143,
+            "velocityX": -0.06316262707958344,
+            "velocityY": -3.1084303821676644,
+            "timestamp": 0.3748347803400048
+        },
+        {
+            "x": 7.659724245559776,
+            "y": 3.1517739981457797,
+            "heading": -0.2973184560258276,
+            "angularVelocity": -0.23110220652567398,
+            "velocityX": -0.0830773542389579,
+            "velocityY": -3.699338869586643,
+            "timestamp": 0.44980173640800575
+        },
+        {
+            "x": 7.61848609595722,
+            "y": 2.8711927485616604,
+            "heading": -0.2973184850947423,
+            "angularVelocity": -3.8775636846908307e-7,
+            "velocityX": -0.5500843540339733,
+            "velocityY": -3.742732322352917,
+            "timestamp": 0.5247686924760067
+        },
+        {
+            "x": 7.5316071002697536,
+            "y": 2.6012326390018865,
+            "heading": -0.2973185190564447,
+            "angularVelocity": -4.530222940715824e-7,
+            "velocityX": -1.1588972027711093,
+            "velocityY": -3.6010547008857787,
+            "timestamp": 0.5997356485440076
+        },
+        {
+            "x": 7.401459139481823,
+            "y": 2.3492645367371554,
+            "heading": -0.29731856454052086,
+            "angularVelocity": -6.067216612944958e-7,
+            "velocityX": -1.7360710320141561,
+            "velocityY": -3.3610555300681284,
+            "timestamp": 0.6747026046120086
+        },
+        {
+            "x": 7.231595664022054,
+            "y": 2.1221680676798096,
+            "heading": -0.2973186386883636,
+            "angularVelocity": -9.89073675332688e-7,
+            "velocityX": -2.265844638345654,
+            "velocityY": -3.0292875817359826,
+            "timestamp": 0.7496695606800096
+        },
+        {
+            "x": 7.026654548839687,
+            "y": 1.9261437915792998,
+            "heading": -0.2973188151543491,
+            "angularVelocity": -0.000002353916908579506,
+            "velocityX": -2.7337526549226907,
+            "velocityY": -2.614809062311352,
+            "timestamp": 0.8246365167480105
+        },
+        {
+            "x": 6.792231559753418,
+            "y": 1.766543984413147,
+            "heading": -0.2973195709317279,
+            "angularVelocity": -0.00001008147347055336,
+            "velocityX": -3.127017573897922,
+            "velocityY": -2.1289354075064555,
+            "timestamp": 0.8996034728160115
+        },
+        {
+            "x": 6.610935711613446,
+            "y": 1.6724691195207,
+            "heading": -0.2973321063250889,
+            "angularVelocity": -0.00023216320074468077,
+            "velocityX": -3.3577106919370485,
+            "velocityY": -1.7423243992220112,
+            "timestamp": 0.9535973587619662
+        },
+        {
+            "x": 6.419882816475647,
+            "y": 1.600658398177247,
+            "heading": -0.29770902924967546,
+            "angularVelocity": -0.006980844552986395,
+            "velocityX": -3.538417207626563,
+            "velocityY": -1.329978757508455,
+            "timestamp": 1.007591244707921
+        },
+        {
+            "x": 6.223144239006287,
+            "y": 1.5524565272909876,
+            "heading": -0.30199019992690734,
+            "angularVelocity": -0.07928991592723345,
+            "velocityX": -3.6437195438439516,
+            "velocityY": -0.892728316211746,
+            "timestamp": 1.0615851306538757
+        },
+        {
+            "x": 6.027009837481886,
+            "y": 1.528287215105613,
+            "heading": -0.31920772077283077,
+            "angularVelocity": -0.31887908314582397,
+            "velocityX": -3.63252983348375,
+            "velocityY": -0.44763053745692566,
+            "timestamp": 1.1155790165998305
+        },
+        {
+            "x": 5.840489459577601,
+            "y": 1.5253276945335545,
+            "heading": -0.3622266817701244,
+            "angularVelocity": -0.7967376350788925,
+            "velocityX": -3.454472198777908,
+            "velocityY": -0.05481214252704887,
+            "timestamp": 1.1695729025457853
+        },
+        {
+            "x": 5.673855769295343,
+            "y": 1.5359985529910274,
+            "heading": -0.41631972448131044,
+            "angularVelocity": -1.0018364443214895,
+            "velocityX": -3.086158504113808,
+            "velocityY": 0.19763086635687102,
+            "timestamp": 1.22356678849174
+        },
+        {
+            "x": 5.530708201179271,
+            "y": 1.5529237053226916,
+            "heading": -0.47196047387840134,
+            "angularVelocity": -1.0305009247305976,
+            "velocityX": -2.651181066303626,
+            "velocityY": 0.3134642383140704,
+            "timestamp": 1.2775606744376948
+        },
+        {
+            "x": 5.411784437145291,
+            "y": 1.57153818466476,
+            "heading": -0.5236032237572267,
+            "angularVelocity": -0.956455512954414,
+            "velocityX": -2.2025413053807186,
+            "velocityY": 0.34475161429762524,
+            "timestamp": 1.3315545603836496
+        },
+        {
+            "x": 5.3170603467926,
+            "y": 1.5890782395606275,
+            "heading": -0.5679992830659408,
+            "angularVelocity": -0.8222423433861833,
+            "velocityX": -1.754348454332506,
+            "velocityY": 0.32485261226475975,
+            "timestamp": 1.3855484463296044
+        },
+        {
+            "x": 5.246334967071984,
+            "y": 1.6037618746917708,
+            "heading": -0.6030801100679523,
+            "angularVelocity": -0.6497185077054303,
+            "velocityX": -1.309877562644932,
+            "velocityY": 0.2719499601462664,
+            "timestamp": 1.4395423322755592
+        },
+        {
+            "x": 5.199384048260516,
+            "y": 1.6143645569232876,
+            "heading": -0.6274260701015184,
+            "angularVelocity": -0.45090216432919045,
+            "velocityX": -0.8695599138477248,
+            "velocityY": 0.19636820069091412,
+            "timestamp": 1.493536218221514
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -0.23287692075121041,
+            "velocityX": -0.4330869662524824,
+            "velocityY": 0.10437187429616973,
+            "timestamp": 1.5475301041674687
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 0,
+            "velocityX": 3.9288115092123874e-33,
+            "velocityY": 2.0888895254257856e-32,
+            "timestamp": 1.6015239901134235
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C3 To W1.traj b/src/main/deploy/choreo/C3 To W1.traj
new file mode 100644
index 00000000..47660d82
--- /dev/null
+++ b/src/main/deploy/choreo/C3 To W1.traj	
@@ -0,0 +1,220 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 1.832751405105417e-34,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.608571974046609,
+            "y": 4.129750388677263,
+            "heading": -0.02440208863451916,
+            "angularVelocity": -0.26295738388657636,
+            "velocityX": -0.7697098032139116,
+            "velocityY": 0.029638242122484983,
+            "timestamp": 0.09279864392416992
+        },
+        {
+            "x": 7.46571594445247,
+            "y": 4.135252523590067,
+            "heading": -0.07320447592180841,
+            "angularVelocity": -0.5258954788948677,
+            "velocityX": -1.5394193659863489,
+            "velocityY": 0.05929111331948823,
+            "timestamp": 0.18559728784833984
+        },
+        {
+            "x": 7.25143160309568,
+            "y": 4.143508368339934,
+            "heading": -0.1463869418885076,
+            "angularVelocity": -0.7886156830752087,
+            "velocityX": -2.3091322490864354,
+            "velocityY": 0.08896514432486853,
+            "timestamp": 0.27839593177250976
+        },
+        {
+            "x": 6.965717910609067,
+            "y": 4.154520147774138,
+            "heading": -0.24389107124776152,
+            "angularVelocity": -1.050706402983198,
+            "velocityX": -3.078856332427471,
+            "velocityY": 0.1186631503279318,
+            "timestamp": 0.3711945756966797
+        },
+        {
+            "x": 6.614926201978592,
+            "y": 4.168029747138803,
+            "heading": -0.24389108357148506,
+            "angularVelocity": -1.3280068591073525e-7,
+            "velocityX": -3.780138305869253,
+            "velocityY": 0.1455797067002448,
+            "timestamp": 0.4639932196208496
+        },
+        {
+            "x": 6.264134491970206,
+            "y": 4.181539310738243,
+            "heading": -0.2438910958936344,
+            "angularVelocity": -1.3278372185354406e-7,
+            "velocityX": -3.7801383207176356,
+            "velocityY": 0.14557932129349355,
+            "timestamp": 0.5567918635450195
+        },
+        {
+            "x": 5.913342805476888,
+            "y": 4.195049484934711,
+            "heading": -0.2438911082157909,
+            "angularVelocity": -1.32783799697389e-7,
+            "velocityX": -3.780138067318815,
+            "velocityY": 0.14558590109896277,
+            "timestamp": 0.6495905074691894
+        },
+        {
+            "x": 5.568285864608835,
+            "y": 4.259648543779433,
+            "heading": -0.24389114467773698,
+            "angularVelocity": -3.929146432741106e-7,
+            "velocityX": -3.718340336417024,
+            "velocityY": 0.6961207202291138,
+            "timestamp": 0.7423891513933594
+        },
+        {
+            "x": 5.223229300660808,
+            "y": 4.324249615558004,
+            "heading": -0.2438911811402195,
+            "angularVelocity": -3.929204239211486e-7,
+            "velocityX": -3.7183362747195923,
+            "velocityY": 0.6961424116429128,
+            "timestamp": 0.8351877953175293
+        },
+        {
+            "x": 4.889178228675737,
+            "y": 4.432171862538056,
+            "heading": -0.2438912192984801,
+            "angularVelocity": -4.111941617608203e-7,
+            "velocityX": -3.5997408783046705,
+            "velocityY": 1.1629722420106654,
+            "timestamp": 0.9279864392416992
+        },
+        {
+            "x": 4.584073695522117,
+            "y": 4.605805251226935,
+            "heading": -0.24389116821114407,
+            "angularVelocity": 5.505181312773355e-7,
+            "velocityX": -3.287812410307864,
+            "velocityY": 1.8710767889103372,
+            "timestamp": 1.0207850831658691
+        },
+        {
+            "x": 4.320679858830311,
+            "y": 4.837885010533044,
+            "heading": -0.24389110572504064,
+            "angularVelocity": 6.733514712028696e-7,
+            "velocityX": -2.8383371303038154,
+            "velocityY": 2.5008960205899418,
+            "timestamp": 1.113583727090039
+        },
+        {
+            "x": 4.066259556785684,
+            "y": 5.079768532204951,
+            "heading": -0.24389104453640137,
+            "angularVelocity": 6.593699721479525e-7,
+            "velocityX": -2.7416381456233947,
+            "velocityY": 2.606541555386963,
+            "timestamp": 1.206382371014209
+        },
+        {
+            "x": 3.8118393280605467,
+            "y": 5.321652130996455,
+            "heading": -0.243890983347767,
+            "angularVelocity": 6.593699195322184e-7,
+            "velocityX": -2.741637355531139,
+            "velocityY": 2.606542386429263,
+            "timestamp": 1.299181014938379
+        },
+        {
+            "x": 3.557419123484207,
+            "y": 5.563535755188363,
+            "heading": -0.24389092215913197,
+            "angularVelocity": 6.593699267467298e-7,
+            "velocityX": -2.7416370953032203,
+            "velocityY": 2.606542660144498,
+            "timestamp": 1.3919796588625488
+        },
+        {
+            "x": 3.3029989365055474,
+            "y": 5.805419397890013,
+            "heading": -0.24389086097049373,
+            "angularVelocity": 6.593699609263459e-7,
+            "velocityX": -2.7416369056702816,
+            "velocityY": 2.606542859605842,
+            "timestamp": 1.4847783027867187
+        },
+        {
+            "x": 3.0485787435735707,
+            "y": 6.04730303432977,
+            "heading": -0.24389079978182,
+            "angularVelocity": 6.593703437871394e-7,
+            "velocityX": -2.741636969823341,
+            "velocityY": 2.6065427921275584,
+            "timestamp": 1.5775769467108887
+        },
+        {
+            "x": 2.7984729856087975,
+            "y": 6.275491677578651,
+            "heading": -0.21231080129448254,
+            "angularVelocity": 0.3403066807004949,
+            "velocityX": -2.6951445343225506,
+            "velocityY": 2.45896527793385,
+            "timestamp": 1.6703755906350586
+        },
+        {
+            "x": 2.596978431795094,
+            "y": 6.469397893884434,
+            "heading": -0.03732218251039644,
+            "angularVelocity": 1.8856807748945335,
+            "velocityX": -2.171309248638947,
+            "velocityY": 2.089537175394848,
+            "timestamp": 1.7631742345592285
+        },
+        {
+            "x": 2.4472498512530994,
+            "y": 6.613867328015145,
+            "heading": 0.15789573469158452,
+            "angularVelocity": 2.1036720898801096,
+            "velocityX": -1.6134781092743464,
+            "velocityY": 1.5568054447946784,
+            "timestamp": 1.8559728784833984
+        },
+        {
+            "x": 2.3487140682465175,
+            "y": 6.7094536627437265,
+            "heading": 0.3099829221841124,
+            "angularVelocity": 1.6388945038552638,
+            "velocityX": -1.0618235228426263,
+            "velocityY": 1.0300402105734285,
+            "timestamp": 1.9487715224075683
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0.8945936525078311,
+            "velocityX": -0.5249437511858832,
+            "velocityY": 0.5123602592202229,
+            "timestamp": 2.0415701663317383
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0,
+            "velocityX": 1.4564927652554838e-31,
+            "velocityY": 0,
+            "timestamp": 2.134368810255908
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C3 To W2.traj b/src/main/deploy/choreo/C3 To W2.traj
new file mode 100644
index 00000000..2f31d71f
--- /dev/null
+++ b/src/main/deploy/choreo/C3 To W2.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": -8.248091288877123e-33,
+            "velocityX": -1.7499589770527321e-34,
+            "velocityY": 1.243057151683689e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 7.599512207946833,
+            "y": 4.126338330595212,
+            "heading": -0.05672955044148203,
+            "angularVelocity": -0.5720722032058021,
+            "velocityX": -0.8116550928518662,
+            "velocityY": -0.0066724074357217035,
+            "timestamp": 0.09916501819801522
+        },
+        {
+            "x": 7.438184902121127,
+            "y": 4.125091465076226,
+            "heading": -0.16291927611759752,
+            "angularVelocity": -1.070838563898341,
+            "velocityX": -1.6268570183042166,
+            "velocityY": -0.01257364281924666,
+            "timestamp": 0.19833003639603045
+        },
+        {
+            "x": 7.19545269041534,
+            "y": 4.123364615129754,
+            "heading": -0.30423484322627714,
+            "angularVelocity": -1.4250546178138856,
+            "velocityX": -2.4477604715515007,
+            "velocityY": -0.017413902380612675,
+            "timestamp": 0.2974950545940457
+        },
+        {
+            "x": 6.870785634071806,
+            "y": 4.1216134281783505,
+            "heading": -0.4343749203987102,
+            "angularVelocity": -1.3123587282822462,
+            "velocityX": -3.274007933878773,
+            "velocityY": -0.01765932163604032,
+            "timestamp": 0.3966600727920609
+        },
+        {
+            "x": 6.495672194218458,
+            "y": 4.117557524267507,
+            "heading": -0.4343749552288927,
+            "angularVelocity": -3.5123456983963544e-7,
+            "velocityX": -3.782719417287976,
+            "velocityY": -0.040900551268437134,
+            "timestamp": 0.4958250909900761
+        },
+        {
+            "x": 6.120558752684864,
+            "y": 4.113501775750749,
+            "heading": -0.43437499005887026,
+            "angularVelocity": -3.512325030859258e-7,
+            "velocityX": -3.782719434231919,
+            "velocityY": -0.04089898424321494,
+            "timestamp": 0.5949901091880914
+        },
+        {
+            "x": 5.747019040696225,
+            "y": 4.148065138700183,
+            "heading": -0.43437502656757854,
+            "angularVelocity": -3.681611615448704e-7,
+            "velocityX": -3.766849628794958,
+            "velocityY": 0.3485439076955189,
+            "timestamp": 0.6941551273861066
+        },
+        {
+            "x": 5.389911454629062,
+            "y": 4.262959433025878,
+            "heading": -0.434375162279621,
+            "angularVelocity": -0.0000013685475476133038,
+            "velocityX": -3.601144764115114,
+            "velocityY": 1.1586171859140066,
+            "timestamp": 0.7933201455841219
+        },
+        {
+            "x": 5.045913588511523,
+            "y": 4.412598985545015,
+            "heading": -0.4343752985502662,
+            "angularVelocity": -0.0000013741806101348962,
+            "velocityX": -3.468943709873945,
+            "velocityY": 1.5089953618556682,
+            "timestamp": 0.8924851637821372
+        },
+        {
+            "x": 4.713243052034498,
+            "y": 4.585968073714987,
+            "heading": -0.43437543856751193,
+            "angularVelocity": -0.0000014119620841874337,
+            "velocityX": -3.354716638207433,
+            "velocityY": 1.7482887748155678,
+            "timestamp": 0.9916501819801524
+        },
+        {
+            "x": 4.426185676495707,
+            "y": 4.82747296096862,
+            "heading": -0.4343754384530987,
+            "angularVelocity": 1.153766536466057e-9,
+            "velocityX": -2.894744343873237,
+            "velocityY": 2.4353838847827403,
+            "timestamp": 1.0908152001781677
+        },
+        {
+            "x": 4.09351517988242,
+            "y": 5.000842166204109,
+            "heading": -0.4343753660253311,
+            "angularVelocity": 7.303761838407394e-7,
+            "velocityX": -3.354716236213479,
+            "velocityY": 1.748289955327798,
+            "timestamp": 1.189980218376183
+        },
+        {
+            "x": 3.752063829141965,
+            "y": 5.156204697333757,
+            "heading": -0.43437529472941433,
+            "angularVelocity": 7.189623727917814e-7,
+            "velocityX": -3.4432641363372296,
+            "velocityY": 1.5667070298864525,
+            "timestamp": 1.2891452365741982
+        },
+        {
+            "x": 3.4085469650969755,
+            "y": 5.306945220863998,
+            "heading": -0.43437522535251666,
+            "angularVelocity": 6.996105981774619e-7,
+            "velocityX": -3.46409318817495,
+            "velocityY": 1.5200977750968405,
+            "timestamp": 1.3883102547722135
+        },
+        {
+            "x": 3.0417944650952866,
+            "y": 5.3858068126004595,
+            "heading": -0.43437520041032157,
+            "angularVelocity": 2.5152211488411685e-7,
+            "velocityX": -3.6984060172242224,
+            "velocityY": 0.7952561615930772,
+            "timestamp": 1.4874752729702287
+        },
+        {
+            "x": 2.7246409461958954,
+            "y": 5.456187357907523,
+            "heading": -0.2777696543737322,
+            "angularVelocity": 1.5792418423589194,
+            "velocityX": -3.1982399102281316,
+            "velocityY": 0.7097315826285274,
+            "timestamp": 1.586640291168244
+        },
+        {
+            "x": 2.4871805723397817,
+            "y": 5.508279050924135,
+            "heading": -0.14311381126711797,
+            "angularVelocity": 1.3578966207390795,
+            "velocityX": -2.394598197742947,
+            "velocityY": 0.5253031155865293,
+            "timestamp": 1.6858053093662593
+        },
+        {
+            "x": 2.3290265259060328,
+            "y": 5.5428007138640085,
+            "heading": -0.04864902301750531,
+            "angularVelocity": 0.9526019353012469,
+            "velocityX": -1.594857231992267,
+            "velocityY": 0.3481233964071807,
+            "timestamp": 1.7849703275642745
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -2.5681051863102055e-32,
+            "angularVelocity": 0.490586538494469,
+            "velocityX": -0.7969193909512572,
+            "velocityY": 0.1734410626703791,
+            "timestamp": 1.8841353457622898
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 1.14717044306562e-31,
+            "velocityX": 0,
+            "velocityY": 5.846493116157662e-32,
+            "timestamp": 1.983300363960305
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C3 To W3.traj b/src/main/deploy/choreo/C3 To W3.traj
new file mode 100644
index 00000000..3874f8b2
--- /dev/null
+++ b/src/main/deploy/choreo/C3 To W3.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 5.58164160972529e-32,
+            "velocityX": 1.1273756142039836e-32,
+            "velocityY": 2.4507602066878393e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.588395006557631,
+            "y": 4.128528104169273,
+            "heading": -0.06094869084906696,
+            "angularVelocity": -0.5766457987157873,
+            "velocityX": -0.8666902254019087,
+            "velocityY": 0.01445765014696692,
+            "timestamp": 0.10569519622756653
+        },
+        {
+            "x": 7.4046836501246664,
+            "y": 4.131675342578391,
+            "heading": -0.17161815397097052,
+            "angularVelocity": -1.0470623743734504,
+            "velocityX": -1.7381239922903071,
+            "velocityY": 0.029776551077511915,
+            "timestamp": 0.21139039245513305
+        },
+        {
+            "x": 7.128148507793846,
+            "y": 4.136585421276787,
+            "heading": -0.30937735862178317,
+            "angularVelocity": -1.3033629679272352,
+            "velocityX": -2.6163454177749763,
+            "velocityY": 0.0464550790730766,
+            "timestamp": 0.3170855886826996
+        },
+        {
+            "x": 6.759745872373483,
+            "y": 4.143898882898015,
+            "heading": -0.3925495634756188,
+            "angularVelocity": -0.7869061965196801,
+            "velocityX": -3.485519196418107,
+            "velocityY": 0.06919388848555069,
+            "timestamp": 0.4227807849102661
+        },
+        {
+            "x": 6.3599483672427946,
+            "y": 4.149634642956697,
+            "heading": -0.39254959749689916,
+            "angularVelocity": -3.2188104690533836e-7,
+            "velocityX": -3.782551330619671,
+            "velocityY": 0.05426698907234567,
+            "timestamp": 0.5284759811378327
+        },
+        {
+            "x": 5.960150865134662,
+            "y": 4.155370613693523,
+            "heading": -0.39254963151811834,
+            "angularVelocity": -3.2188046698591794e-7,
+            "velocityX": -3.7825513020227493,
+            "velocityY": 0.05426898233365955,
+            "timestamp": 0.6341711773653992
+        },
+        {
+            "x": 5.56430885680334,
+            "y": 4.211762521698523,
+            "heading": -0.3925496691964927,
+            "angularVelocity": -3.5648142721372326e-7,
+            "velocityX": -3.745127711188092,
+            "velocityY": 0.5335333110464675,
+            "timestamp": 0.7398663735929657
+        },
+        {
+            "x": 5.196451370697568,
+            "y": 4.356905683947299,
+            "heading": -0.407192768864793,
+            "angularVelocity": -0.13854082485237101,
+            "velocityX": -3.4803614472105,
+            "velocityY": 1.3732238306864724,
+            "timestamp": 0.8455615698205322
+        },
+        {
+            "x": 4.853119399386178,
+            "y": 4.529141900436451,
+            "heading": -0.46036689242459117,
+            "angularVelocity": -0.5030893120753747,
+            "velocityX": -3.2483214333807724,
+            "velocityY": 1.6295557663597022,
+            "timestamp": 0.9512567660480987
+        },
+        {
+            "x": 4.514017884763102,
+            "y": 4.740993233583884,
+            "heading": -0.4603674462778229,
+            "angularVelocity": -0.000005240098429403888,
+            "velocityX": -3.208296372268186,
+            "velocityY": 2.0043610372916896,
+            "timestamp": 1.0569519622756653
+        },
+        {
+            "x": 4.21750261375463,
+            "y": 5.009227519029098,
+            "heading": -0.46036738913342323,
+            "angularVelocity": 5.406527608893175e-7,
+            "velocityX": -2.8053807702864813,
+            "velocityY": 2.537809616888297,
+            "timestamp": 1.1626471585032319
+        },
+        {
+            "x": 3.866969646487487,
+            "y": 5.201574981603133,
+            "heading": -0.46036721071098435,
+            "angularVelocity": 0.0000016880846552508654,
+            "velocityX": -3.3164512653198517,
+            "velocityY": 1.8198316426784653,
+            "timestamp": 1.2683423547307984
+        },
+        {
+            "x": 3.5045677865422378,
+            "y": 5.303672838764994,
+            "heading": -0.3887465759877037,
+            "angularVelocity": 0.6776148517580518,
+            "velocityX": -3.4287448519891264,
+            "velocityY": 0.9659649710289621,
+            "timestamp": 1.3740375509583649
+        },
+        {
+            "x": 3.16557460185915,
+            "y": 5.315860378892371,
+            "heading": -0.29320626763004903,
+            "angularVelocity": 0.9039229006392311,
+            "velocityX": -3.207271444515043,
+            "velocityY": 0.1153083636945626,
+            "timestamp": 1.4797327471859314
+        },
+        {
+            "x": 2.8681536233597122,
+            "y": 5.2454821587095966,
+            "heading": -0.23849853505250618,
+            "angularVelocity": 0.5175990445181124,
+            "velocityX": -2.8139498209462337,
+            "velocityY": -0.6658601591622668,
+            "timestamp": 1.585427943413498
+        },
+        {
+            "x": 2.618332286986499,
+            "y": 5.0970676603891505,
+            "heading": -0.24744806192055374,
+            "angularVelocity": -0.08467297651615932,
+            "velocityX": -2.3636016137889215,
+            "velocityY": -1.4041744905880433,
+            "timestamp": 1.6911231396410644
+        },
+        {
+            "x": 2.4325478975720265,
+            "y": 4.8813111498736355,
+            "heading": -0.27927376577641677,
+            "angularVelocity": -0.3011083283987745,
+            "velocityX": -1.7577373054350616,
+            "velocityY": -2.0413085761341616,
+            "timestamp": 1.796818335868631
+        },
+        {
+            "x": 2.3348070320385017,
+            "y": 4.635496280912222,
+            "heading": -0.32770640318337707,
+            "angularVelocity": -0.4582293153861289,
+            "velocityX": -0.9247427416008974,
+            "velocityY": -2.3256957528340565,
+            "timestamp": 1.9025135320961974
+        },
+        {
+            "x": 2.305548232287739,
+            "y": 4.452786448669473,
+            "heading": -0.3683698320727586,
+            "angularVelocity": -0.3847235289845278,
+            "velocityX": -0.2768224176221525,
+            "velocityY": -1.72864840374928,
+            "timestamp": 2.008208728323764
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.2046466509288718,
+            "velocityX": -0.05249275734153,
+            "velocityY": -0.8778681716972112,
+            "timestamp": 2.1139039245513307
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 5.518495272475637e-32,
+            "velocityX": 0,
+            "velocityY": 3.7393231194786592e-34,
+            "timestamp": 2.219599120778897
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C4 To C1.traj b/src/main/deploy/choreo/C4 To C1.traj
new file mode 100644
index 00000000..1bf739bf
--- /dev/null
+++ b/src/main/deploy/choreo/C4 To C1.traj	
@@ -0,0 +1,175 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -2.2542235108498663e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 2.538844153951763,
+            "heading": 2.1223723168001845e-18,
+            "angularVelocity": 2.1422107346760384e-17,
+            "velocityX": -4.524824041974102e-19,
+            "velocityY": 0.8260917454276265,
+            "timestamp": 0.09907392781100399
+        },
+        {
+            "x": 7.68,
+            "y": 2.70253245874193,
+            "heading": 6.368502121517421e-18,
+            "angularVelocity": 4.285819588010552e-17,
+            "velocityX": -9.048618887508432e-19,
+            "velocityY": 1.6521834594306501,
+            "timestamp": 0.19814785562200798
+        },
+        {
+            "x": 7.68,
+            "y": 2.9480649081437846,
+            "heading": 1.2741259045974704e-17,
+            "angularVelocity": 6.432324896396774e-17,
+            "velocityX": -1.3570355356096521e-18,
+            "velocityY": 2.47827511058448,
+            "timestamp": 0.297221783433012
+        },
+        {
+            "x": 7.68,
+            "y": 3.2754414834771923,
+            "heading": 2.1247069179422305e-17,
+            "angularVelocity": 8.585316360490521e-17,
+            "velocityX": -1.8085916683463265e-18,
+            "velocityY": 3.3043665731908773,
+            "timestamp": 0.39629571124401597
+        },
+        {
+            "x": 7.68,
+            "y": 3.6502322649266867,
+            "heading": 5.059469084858907e-18,
+            "angularVelocity": -1.633891019789118e-16,
+            "velocityX": -2.0715339797290713e-18,
+            "velocityY": 3.782940575087071,
+            "timestamp": 0.49536963905501996
+        },
+        {
+            "x": 7.68,
+            "y": 4.025023046376204,
+            "heading": -4.370025378852898e-19,
+            "angularVelocity": -5.5478487063848375e-17,
+            "velocityX": -2.0691441879135133e-18,
+            "velocityY": 3.7829405750873115,
+            "timestamp": 0.594443566866024
+        },
+        {
+            "x": 7.68,
+            "y": 4.3998138278257235,
+            "heading": 3.678666355048194e-17,
+            "angularVelocity": 3.7571606285132846e-16,
+            "velocityX": -2.072328507072934e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.693517494677028
+        },
+        {
+            "x": 7.68,
+            "y": 4.774604609275241,
+            "heading": 5.708604515927294e-17,
+            "angularVelocity": 2.04891257037182e-16,
+            "velocityX": -2.07516702935077e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.7925914224880319
+        },
+        {
+            "x": 7.68,
+            "y": 5.14939539072476,
+            "heading": 3.2711500565517004e-17,
+            "angularVelocity": -2.460238039643844e-16,
+            "velocityX": -2.078706211560206e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.8916653502990359
+        },
+        {
+            "x": 7.68,
+            "y": 5.524186172174278,
+            "heading": 3.4723638610714384e-17,
+            "angularVelocity": 2.0309460720999983e-17,
+            "velocityX": -2.0667219645048642e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.9907392781100399
+        },
+        {
+            "x": 7.68,
+            "y": 5.898976953623796,
+            "heading": 1.3963468905806427e-18,
+            "angularVelocity": -3.363881139719203e-16,
+            "velocityX": -2.078319856885538e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 1.089813205921044
+        },
+        {
+            "x": 7.68,
+            "y": 6.273767735073314,
+            "heading": -1.338725801529433e-17,
+            "angularVelocity": -1.492179146704526e-16,
+            "velocityX": -2.0709974384556896e-18,
+            "velocityY": 3.7829405750873115,
+            "timestamp": 1.188887133732048
+        },
+        {
+            "x": 7.68,
+            "y": 6.648558516522808,
+            "heading": -2.1247111946894708e-17,
+            "angularVelocity": -7.933322222365232e-17,
+            "velocityX": -2.070808166139798e-18,
+            "velocityY": 3.782940575087071,
+            "timestamp": 1.287961061543052
+        },
+        {
+            "x": 7.68,
+            "y": 6.975935091856216,
+            "heading": -1.273985947141283e-17,
+            "angularVelocity": 8.586772184549593e-17,
+            "velocityX": -1.8108827209778567e-18,
+            "velocityY": 3.304366573190878,
+            "timestamp": 1.387034989354056
+        },
+        {
+            "x": 7.68,
+            "y": 7.22146754125807,
+            "heading": -6.367750753434283e-18,
+            "angularVelocity": 6.431670631033216e-17,
+            "velocityX": -1.3581621409711223e-18,
+            "velocityY": 2.47827511058448,
+            "timestamp": 1.4861089171650599
+        },
+        {
+            "x": 7.68,
+            "y": 7.385155846048237,
+            "heading": -2.122107432521882e-18,
+            "angularVelocity": 4.285328556951628e-17,
+            "velocityX": -9.05441456368543e-19,
+            "velocityY": 1.65218345943065,
+            "timestamp": 1.5851828449760639
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 2.141943374454498e-17,
+            "velocityX": -4.527207369006345e-19,
+            "velocityY": 0.8260917454276265,
+            "timestamp": 1.6842567727870679
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -3.1368402443636883e-34,
+            "velocityY": 0,
+            "timestamp": 1.7833307005980719
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C4 To C2.traj b/src/main/deploy/choreo/C4 To C2.traj
new file mode 100644
index 00000000..0dd47978
--- /dev/null
+++ b/src/main/deploy/choreo/C4 To C2.traj	
@@ -0,0 +1,139 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": -1.7120676318191985e-39,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 2.5334453720055388,
+            "heading": -1.6066276958150233e-18,
+            "angularVelocity": -1.677931007769515e-17,
+            "velocityX": 1.4398812722194594e-33,
+            "velocityY": 0.7983807350946674,
+            "timestamp": 0.09575052183150976
+        },
+        {
+            "x": 7.68,
+            "y": 2.686336112982689,
+            "heading": -4.816833322743681e-18,
+            "angularVelocity": -3.3526769001814837e-17,
+            "velocityX": 1.5865173753469157e-33,
+            "velocityY": 1.5967614385035827,
+            "timestamp": 0.1915010436630195
+        },
+        {
+            "x": 7.68,
+            "y": 2.915672216863599,
+            "heading": -9.624489104750263e-18,
+            "angularVelocity": -5.0210230614541765e-17,
+            "velocityX": 1.0182538660036936e-34,
+            "velocityY": 2.395142078541023,
+            "timestamp": 0.28725156549452924
+        },
+        {
+            "x": 7.68,
+            "y": 3.221453665444745,
+            "heading": -1.6009153132358185e-17,
+            "angularVelocity": -6.66802008672839e-17,
+            "velocityX": 3.094783239015371e-34,
+            "velocityY": 3.193522528464369,
+            "timestamp": 0.383002087326039
+        },
+        {
+            "x": 7.68,
+            "y": 3.5836721992668203,
+            "heading": -3.473454086768081e-17,
+            "angularVelocity": -1.9556434134398854e-16,
+            "velocityX": 3.082893791022991e-33,
+            "velocityY": 3.7829405719528495,
+            "timestamp": 0.4787526091575488
+        },
+        {
+            "x": 7.68,
+            "y": 3.9458907330889397,
+            "heading": -3.402618832360637e-17,
+            "angularVelocity": 7.397897479044197e-18,
+            "velocityX": 3.015867889332214e-33,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.5745031309890586
+        },
+        {
+            "x": 7.68,
+            "y": 4.308109266911059,
+            "heading": -1.7375698441954866e-17,
+            "angularVelocity": 1.7389450796885638e-16,
+            "velocityX": 5.330140345912211e-33,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.6702536528205684
+        },
+        {
+            "x": 7.68,
+            "y": 4.670327800733179,
+            "heading": 7.579747606314133e-18,
+            "angularVelocity": 2.6062986990486198e-16,
+            "velocityX": 7.600730979130334e-33,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.7660041746520782
+        },
+        {
+            "x": 7.68,
+            "y": 5.032546334555254,
+            "heading": 1.6243735534665814e-17,
+            "angularVelocity": 9.048502047432954e-17,
+            "velocityX": -2.799411465702586e-33,
+            "velocityY": 3.7829405719528495,
+            "timestamp": 0.8617546964835879
+        },
+        {
+            "x": 7.68,
+            "y": 5.3383277831364,
+            "heading": 9.733952786983855e-18,
+            "angularVelocity": -6.798691664538921e-17,
+            "velocityX": -8.030123288193386e-33,
+            "velocityY": 3.193522528464369,
+            "timestamp": 0.9575052183150977
+        },
+        {
+            "x": 7.68,
+            "y": 5.567663887017311,
+            "heading": 4.864296415465134e-18,
+            "angularVelocity": -5.08577528116881e-17,
+            "velocityX": -5.179461626817983e-33,
+            "velocityY": 2.395142078541023,
+            "timestamp": 1.0532557401466074
+        },
+        {
+            "x": 7.68,
+            "y": 5.720554627994461,
+            "heading": 1.6208272352196832e-18,
+            "angularVelocity": -3.3874167136237696e-17,
+            "velocityX": -4.721587370568867e-33,
+            "velocityY": 1.5967614385035827,
+            "timestamp": 1.1490062619781172
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": -1.69276073284528e-17,
+            "velocityX": -2.4599412486527172e-33,
+            "velocityY": 0.7983807350946674,
+            "timestamp": 1.244756783809627
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -3.907521125363084e-43,
+            "velocityY": 8.448865646535191e-39,
+            "timestamp": 1.3405073056411367
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C4 To C3.traj b/src/main/deploy/choreo/C4 To C3.traj
new file mode 100644
index 00000000..14b92c55
--- /dev/null
+++ b/src/main/deploy/choreo/C4 To C3.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -2.24426408200091e-39,
+            "angularVelocity": 3.072610310721988e-47,
+            "velocityX": -5.239815815905201e-40,
+            "velocityY": -4.5917748078995606e-40,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 2.540500005575846,
+            "heading": -7.160737227180709e-23,
+            "angularVelocity": -7.15564807370433e-22,
+            "velocityX": 1.97902852084548e-33,
+            "velocityY": 0.8344065481228216,
+            "timestamp": 0.10007112931184187
+        },
+        {
+            "x": 7.68,
+            "y": 2.7075000136298466,
+            "heading": 1.9234971876596208e-22,
+            "angularVelocity": 2.6376946581601568e-21,
+            "velocityX": 2.759296119129438e-33,
+            "velocityY": 1.6688130652907365,
+            "timestamp": 0.20014225862368373
+        },
+        {
+            "x": 7.68,
+            "y": 2.9580000179666155,
+            "heading": 1.2715262688933367e-21,
+            "angularVelocity": 1.0784094425251139e-20,
+            "velocityX": 3.360924516770788e-33,
+            "velocityY": 2.5032195205488357,
+            "timestamp": 0.3002133879355256
+        },
+        {
+            "x": 7.68,
+            "y": 3.2919999999999994,
+            "heading": 4.5990005279924634e-21,
+            "angularVelocity": 3.325109025879841e-20,
+            "velocityX": 4.212493187097162e-33,
+            "velocityY": 3.3376257900775026,
+            "timestamp": 0.40028451724736747
+        },
+        {
+            "x": 7.68,
+            "y": 3.6259999820333837,
+            "heading": 2.8774590551268706e-21,
+            "angularVelocity": -1.7203177491039364e-20,
+            "velocityX": -1.8223734723751632e-33,
+            "velocityY": 3.3376257900775026,
+            "timestamp": 0.5003556465592094
+        },
+        {
+            "x": 7.68,
+            "y": 3.8764999863701526,
+            "heading": 1.4966782173181084e-21,
+            "angularVelocity": -1.3797993485663648e-20,
+            "velocityX": -1.777649344678044e-33,
+            "velocityY": 2.5032195205488357,
+            "timestamp": 0.6004267758710513
+        },
+        {
+            "x": 7.68,
+            "y": 4.043499994424153,
+            "heading": 5.147844205579642e-22,
+            "angularVelocity": -9.81195850164629e-21,
+            "velocityX": -2.007686184897088e-33,
+            "velocityY": 1.6688130652907365,
+            "timestamp": 0.7004979051828932
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -4.8498876719579466e-42,
+            "angularVelocity": -5.144185056490002e-21,
+            "velocityX": -2.233495211598662e-33,
+            "velocityY": 0.8344065481228216,
+            "timestamp": 0.8005690344947352
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 9.449822208481667e-41,
+            "angularVelocity": 9.927610425813228e-40,
+            "velocityX": 1.9465664275137074e-41,
+            "velocityY": 5.142787784847508e-39,
+            "timestamp": 0.9006401638065771
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C4 To C5.traj b/src/main/deploy/choreo/C4 To C5.traj
new file mode 100644
index 00000000..27f393e3
--- /dev/null
+++ b/src/main/deploy/choreo/C4 To C5.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -4.712717929333765e-40,
+            "angularVelocity": 9.927649781151023e-40,
+            "velocityX": 1.7772849076267516e-40,
+            "velocityY": -4.543888050052464e-40,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 2.3734999944241535,
+            "heading": 2.8227518572323466e-21,
+            "angularVelocity": 2.820745460648545e-20,
+            "velocityX": 2.2465911667965736e-32,
+            "velocityY": -0.8344065481228212,
+            "timestamp": 0.10007112931184181
+        },
+        {
+            "x": 7.68,
+            "y": 2.206499986370153,
+            "heading": 8.436925006375396e-21,
+            "angularVelocity": 5.6101826293588e-20,
+            "velocityX": 4.4654374072917263e-32,
+            "velocityY": -1.6688130652907358,
+            "timestamp": 0.20014225862368362
+        },
+        {
+            "x": 7.68,
+            "y": 1.955999982033384,
+            "heading": 1.682221146414935e-20,
+            "angularVelocity": 8.379326253074398e-20,
+            "velocityX": 6.797738711780288e-32,
+            "velocityY": -2.503219520548835,
+            "timestamp": 0.30021338793552543
+        },
+        {
+            "x": 7.68,
+            "y": 1.6219999999999999,
+            "heading": 2.78675063552584e-20,
+            "angularVelocity": 1.1037443968745167e-19,
+            "velocityX": 8.936946075377416e-32,
+            "velocityY": -3.3376257900775013,
+            "timestamp": 0.40028451724736724
+        },
+        {
+            "x": 7.68,
+            "y": 1.288000017966616,
+            "heading": 1.0287640204896551e-20,
+            "angularVelocity": -1.75673704816788e-19,
+            "velocityX": 9.060995032099377e-32,
+            "velocityY": -3.3376257900775013,
+            "timestamp": 0.500355646559209
+        },
+        {
+            "x": 7.68,
+            "y": 1.037500013629847,
+            "heading": 3.2969803204781124e-21,
+            "angularVelocity": -6.985690973056607e-20,
+            "velocityX": 6.916062716279199e-32,
+            "velocityY": -2.503219520548835,
+            "timestamp": 0.6004267758710509
+        },
+        {
+            "x": 7.68,
+            "y": 0.8705000055758465,
+            "heading": 6.18120991020044e-22,
+            "angularVelocity": -2.676955216055553e-20,
+            "velocityX": 4.34564572466329e-32,
+            "velocityY": -1.6688130652907358,
+            "timestamp": 0.7004979051828927
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -2.2737142324276756e-40,
+            "angularVelocity": -6.1768164103594875e-21,
+            "velocityX": 2.131975080632403e-32,
+            "velocityY": -0.8344065481228212,
+            "timestamp": 0.8005690344947345
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 2.0993519451368808e-41,
+            "angularVelocity": 2.4819025971975228e-39,
+            "velocityX": -8.732390511178075e-43,
+            "velocityY": -2.571393892423754e-39,
+            "timestamp": 0.9006401638065763
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C4 To S3.traj b/src/main/deploy/choreo/C4 To S3.traj
new file mode 100644
index 00000000..80fc230a
--- /dev/null
+++ b/src/main/deploy/choreo/C4 To S3.traj	
@@ -0,0 +1,229 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 7.203126847267154e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6459509070752,
+            "y": 2.420050479995149,
+            "heading": -0.026297254997510774,
+            "angularVelocity": -0.33695614983619343,
+            "velocityX": -0.4362832264603014,
+            "velocityY": -0.47344743777665743,
+            "timestamp": 0.078043552581802
+        },
+        {
+            "x": 7.57752123433784,
+            "y": 2.3463325784745406,
+            "heading": -0.07655763531528098,
+            "angularVelocity": -0.6440042598660941,
+            "velocityX": -0.8768139131756897,
+            "velocityY": -0.9445738831958564,
+            "timestamp": 0.156087105163604
+        },
+        {
+            "x": 7.474195561438856,
+            "y": 2.236153787211583,
+            "heading": -0.14701004173730176,
+            "angularVelocity": -0.9027319245646475,
+            "velocityX": -1.3239488654839289,
+            "velocityY": -1.4117603263569152,
+            "timestamp": 0.23413065774540598
+        },
+        {
+            "x": 7.33507656702923,
+            "y": 2.0901340736402396,
+            "heading": -0.23055657416165956,
+            "angularVelocity": -1.0705116522834195,
+            "velocityX": -1.7825815177216175,
+            "velocityY": -1.8710029046700958,
+            "timestamp": 0.312174210327208
+        },
+        {
+            "x": 7.158294386463989,
+            "y": 1.910067298253472,
+            "heading": -0.30975533817585227,
+            "angularVelocity": -1.014802137964151,
+            "velocityX": -2.2651734155741057,
+            "velocityY": -2.3072601057983104,
+            "timestamp": 0.39021776290901
+        },
+        {
+            "x": 6.940360292129115,
+            "y": 1.7108993896694566,
+            "heading": -0.30975560484167247,
+            "angularVelocity": -0.00000341688469228089,
+            "velocityX": -2.7924676302559823,
+            "velocityY": -2.552009768843728,
+            "timestamp": 0.468261315490812
+        },
+        {
+            "x": 6.6915168927330235,
+            "y": 1.5520279357314815,
+            "heading": -0.30975564017638474,
+            "angularVelocity": -4.5275632793576304e-7,
+            "velocityX": -3.1885196299240257,
+            "velocityY": -2.035676858398594,
+            "timestamp": 0.546304868072614
+        },
+        {
+            "x": 6.419127595403782,
+            "y": 1.43815413781978,
+            "heading": -0.30975568823518523,
+            "angularVelocity": -6.157946287917162e-7,
+            "velocityX": -3.490221655962278,
+            "velocityY": -1.4591057703627237,
+            "timestamp": 0.624348420654416
+        },
+        {
+            "x": 6.131252530334066,
+            "y": 1.37264761019134,
+            "heading": -0.30975577587401576,
+            "angularVelocity": -0.0000011229477336220622,
+            "velocityX": -3.6886463461279972,
+            "velocityY": -0.8393586076157934,
+            "timestamp": 0.7023919732362179
+        },
+        {
+            "x": 5.83657654735769,
+            "y": 1.3574693553002635,
+            "heading": -0.31020540722452916,
+            "angularVelocity": -0.005761287587978277,
+            "velocityX": -3.7757889438387497,
+            "velocityY": -0.1944844178532558,
+            "timestamp": 0.7804355258180199
+        },
+        {
+            "x": 5.546474933624268,
+            "y": 1.3928169012069702,
+            "heading": -0.3189695864044355,
+            "angularVelocity": -0.11229856778804584,
+            "velocityX": -3.717175911864151,
+            "velocityY": 0.4529207697160109,
+            "timestamp": 0.8584790783998218
+        },
+        {
+            "x": 5.303276771996363,
+            "y": 1.4607318697473424,
+            "heading": -0.3307229091538368,
+            "angularVelocity": -0.17360472172913416,
+            "velocityX": -3.5922053767043294,
+            "velocityY": 1.0031511484968414,
+            "timestamp": 0.9261807090485242
+        },
+        {
+            "x": 5.073563214559545,
+            "y": 1.564404927734292,
+            "heading": -0.34378229938421223,
+            "angularVelocity": -0.1928962434919207,
+            "velocityX": -3.3930284283517023,
+            "velocityY": 1.5313229089691542,
+            "timestamp": 0.9938823396972266
+        },
+        {
+            "x": 4.862974125111252,
+            "y": 1.7011642845126724,
+            "heading": -0.35827284529055603,
+            "angularVelocity": -0.2140354045770704,
+            "velocityX": -3.1105467834446325,
+            "velocityY": 2.020030469989471,
+            "timestamp": 1.0615839703459289
+        },
+        {
+            "x": 4.677671719496688,
+            "y": 1.8665243414779629,
+            "heading": -0.3791415933598452,
+            "angularVelocity": -0.30824587043792007,
+            "velocityX": -2.737044940262413,
+            "velocityY": 2.442482631227878,
+            "timestamp": 1.1292856009946313
+        },
+        {
+            "x": 4.523629451068858,
+            "y": 2.053477766497112,
+            "heading": -0.41478083919993697,
+            "angularVelocity": -0.5264163580502632,
+            "velocityX": -2.27531105162187,
+            "velocityY": 2.761431640977936,
+            "timestamp": 1.1969872316433336
+        },
+        {
+            "x": 4.406611773563404,
+            "y": 2.2498215105595722,
+            "heading": -0.45494733767836987,
+            "angularVelocity": -0.5932870167770028,
+            "velocityX": -1.7284321867022447,
+            "velocityY": 2.900133160474012,
+            "timestamp": 1.264688862292036
+        },
+        {
+            "x": 4.327688857963163,
+            "y": 2.4438365235550945,
+            "heading": -0.48799769437613305,
+            "angularVelocity": -0.4881766713911416,
+            "velocityX": -1.1657461547677304,
+            "velocityY": 2.865736188870776,
+            "timestamp": 1.3323904929407384
+        },
+        {
+            "x": 4.28360260298108,
+            "y": 2.622213717107768,
+            "heading": -0.5154307830643909,
+            "angularVelocity": -0.40520573028252516,
+            "velocityX": -0.6511845366152605,
+            "velocityY": 2.634754759132393,
+            "timestamp": 1.4000921235894408
+        },
+        {
+            "x": 4.264391797811532,
+            "y": 2.7716303897908317,
+            "heading": -0.5365689504315967,
+            "angularVelocity": -0.3122253801676767,
+            "velocityX": -0.283756904899847,
+            "velocityY": 2.2069877970647664,
+            "timestamp": 1.4677937542381432
+        },
+        {
+            "x": 4.259328393836262,
+            "y": 2.8855840206662466,
+            "heading": -0.5517027167443236,
+            "angularVelocity": -0.22353621571242488,
+            "velocityX": -0.07478998551086755,
+            "velocityY": 1.6831740946789628,
+            "timestamp": 1.5354953848868456
+        },
+        {
+            "x": 4.260599830778067,
+            "y": 2.9618797181043535,
+            "heading": -0.5613571988137664,
+            "angularVelocity": -0.1426033904496253,
+            "velocityX": 0.018780004700190214,
+            "velocityY": 1.1269403219251917,
+            "timestamp": 1.603197015535548
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -0.0685773908510417,
+            "velocityX": 0.035452162657366115,
+            "velocityY": 0.5630629798778553,
+            "timestamp": 1.6708986461842503
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 0,
+            "velocityX": 1.1764020723168042e-36,
+            "velocityY": 0,
+            "timestamp": 1.7386002768329527
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C4 To S4.traj b/src/main/deploy/choreo/C4 To S4.traj
new file mode 100644
index 00000000..e5689e4c
--- /dev/null
+++ b/src/main/deploy/choreo/C4 To S4.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": 1.0550386775092505e-35,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6504501923310215,
+            "y": 2.437078665707293,
+            "heading": -0.0029944704440250615,
+            "angularVelocity": -0.0457972069493345,
+            "velocityX": -0.45193254781641656,
+            "velocityY": -0.30467539632270557,
+            "timestamp": 0.0653854381848624
+        },
+        {
+            "x": 7.5913505770823395,
+            "y": 2.3972359988448533,
+            "heading": -0.008983367804687599,
+            "angularVelocity": -0.09159374819405963,
+            "velocityX": -0.9038650942674934,
+            "velocityY": -0.609350766294381,
+            "timestamp": 0.1307708763697248
+        },
+        {
+            "x": 7.50270115478166,
+            "y": 2.337472001372052,
+            "heading": -0.017966623739921285,
+            "angularVelocity": -0.13738924422033513,
+            "velocityX": -1.3557976326478658,
+            "velocityY": -0.91402610629957,
+            "timestamp": 0.19615631455458718
+        },
+        {
+            "x": 7.384501926877228,
+            "y": 2.2577866758321745,
+            "heading": -0.02994413949472175,
+            "angularVelocity": -0.18318322989496186,
+            "velocityX": -1.8077301488788986,
+            "velocityY": -1.2187014074079496,
+            "timestamp": 0.2615417527394496
+        },
+        {
+            "x": 7.2367528971895485,
+            "y": 2.1581800263627264,
+            "heading": -0.044915766222073404,
+            "angularVelocity": -0.2289749391144676,
+            "velocityX": -2.25966260667937,
+            "velocityY": -1.5233766452376303,
+            "timestamp": 0.326927190924312
+        },
+        {
+            "x": 7.059454079201559,
+            "y": 2.0386520636124454,
+            "heading": -0.06288119646669635,
+            "angularVelocity": -0.2747619461359239,
+            "velocityX": -2.711594858272837,
+            "velocityY": -1.82805172020631,
+            "timestamp": 0.3923126291091744
+        },
+        {
+            "x": 6.854358935700705,
+            "y": 1.900385597577332,
+            "heading": -0.06288119947800026,
+            "angularVelocity": -4.6054656889756363e-8,
+            "velocityX": -3.136709781174733,
+            "velocityY": -2.114636987584247,
+            "timestamp": 0.45769806729403684
+        },
+        {
+            "x": 6.636845838072133,
+            "y": 1.782615967982618,
+            "heading": -0.06288120256062167,
+            "angularVelocity": -4.714538109405926e-8,
+            "velocityX": -3.3266290425951324,
+            "velocityY": -1.801159904469057,
+            "timestamp": 0.5230835054788993
+        },
+        {
+            "x": 6.4046630859375,
+            "y": 1.6973352432250977,
+            "heading": -0.06288120593828476,
+            "angularVelocity": -5.1657726572043936e-8,
+            "velocityX": -3.5509856411482486,
+            "velocityY": -1.3042770244409516,
+            "timestamp": 0.5884689436637617
+        },
+        {
+            "x": 6.207746736822867,
+            "y": 1.6503996785686041,
+            "heading": -0.06288121580879107,
+            "angularVelocity": -1.8445407171610623e-7,
+            "velocityX": -3.679854027466254,
+            "velocityY": -0.8771035386810692,
+            "timestamp": 0.6419809437716079
+        },
+        {
+            "x": 6.009908768236521,
+            "y": 1.6271742750630454,
+            "heading": -0.0724170210773865,
+            "angularVelocity": -0.17819938050114592,
+            "velocityX": -3.697076696584537,
+            "velocityY": -0.43402234001253925,
+            "timestamp": 0.695492943879454
+        },
+        {
+            "x": 5.8260492909953046,
+            "y": 1.6186155743000838,
+            "heading": -0.12620190803666176,
+            "angularVelocity": -1.0050995449783064,
+            "velocityX": -3.43585507681777,
+            "velocityY": -0.1599398405163814,
+            "timestamp": 0.7490049439873002
+        },
+        {
+            "x": 5.6625912601386625,
+            "y": 1.614879890442449,
+            "heading": -0.22199295256207294,
+            "angularVelocity": -1.7900852954918034,
+            "velocityX": -3.0546051451490293,
+            "velocityY": -0.06981020799271546,
+            "timestamp": 0.8025169440951464
+        },
+        {
+            "x": 5.5227239807129855,
+            "y": 1.6141995251565013,
+            "heading": -0.32512734843535046,
+            "angularVelocity": -1.9273134187738115,
+            "velocityX": -2.61375540334492,
+            "velocityY": -0.012714256327113658,
+            "timestamp": 0.8560289442029926
+        },
+        {
+            "x": 5.406619958232834,
+            "y": 1.6150039384726416,
+            "heading": -0.42188556792063076,
+            "angularVelocity": -1.8081592781110263,
+            "velocityX": -2.169681982474204,
+            "velocityY": 0.015032391136922594,
+            "timestamp": 0.9095409443108388
+        },
+        {
+            "x": 5.314084878105213,
+            "y": 1.6164421457440925,
+            "heading": -0.5051954003859285,
+            "angularVelocity": -1.5568439284160218,
+            "velocityX": -1.7292397955809797,
+            "velocityY": 0.026876350511144828,
+            "timestamp": 0.9630529444186849
+        },
+        {
+            "x": 5.244912091070787,
+            "y": 1.617980616061371,
+            "heading": -0.5709381839957368,
+            "angularVelocity": -1.2285615091439794,
+            "velocityX": -1.2926593454742539,
+            "velocityY": 0.028750005871169333,
+            "timestamp": 1.016564944526531
+        },
+        {
+            "x": 5.198930392622704,
+            "y": 1.6192554664106382,
+            "heading": -0.6164892293058035,
+            "angularVelocity": -0.851230475748707,
+            "velocityX": -0.8592782619863396,
+            "velocityY": 0.023823634823929046,
+            "timestamp": 1.0700769446343772
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -0.4393551100092281,
+            "velocityX": -0.42850935447171357,
+            "velocityY": 0.013913394899485362,
+            "timestamp": 1.1235889447422234
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 6.2403191565519e-36,
+            "timestamp": 1.1771009448500696
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C4 To W1.traj b/src/main/deploy/choreo/C4 To W1.traj
new file mode 100644
index 00000000..f850aede
--- /dev/null
+++ b/src/main/deploy/choreo/C4 To W1.traj	
@@ -0,0 +1,238 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": 2.322866633708497e-28,
+            "velocityX": -2.1618763976557105e-31,
+            "velocityY": -1.247476705840714e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 7.635666877682845,
+            "y": 2.515362381859407,
+            "heading": -0.0008300252680408195,
+            "angularVelocity": -0.00885317162168204,
+            "velocityX": -0.4728636048938783,
+            "velocityY": 0.6225017511467659,
+            "timestamp": 0.09375456655646779
+        },
+        {
+            "x": 7.547000631163484,
+            "y": 2.632087139923421,
+            "heading": -0.0024900693866490363,
+            "angularVelocity": -0.01770627479364842,
+            "velocityX": -0.9457272298940119,
+            "velocityY": 1.2450034419786018,
+            "timestamp": 0.18750913311293557
+        },
+        {
+            "x": 7.4140012603329,
+            "y": 2.8071742656560237,
+            "heading": -0.004980126198969091,
+            "angularVelocity": -0.026559312295687615,
+            "velocityX": -1.418590856056911,
+            "velocityY": 1.867505041764004,
+            "timestamp": 0.28126369966940334
+        },
+        {
+            "x": 7.236668772838196,
+            "y": 3.0406237394955027,
+            "heading": -0.008300187115435396,
+            "angularVelocity": -0.035412258180156565,
+            "velocityX": -1.891454400654691,
+            "velocityY": 2.4900064329013105,
+            "timestamp": 0.37501826622587114
+        },
+        {
+            "x": 7.022136423676778,
+            "y": 3.3230511197393278,
+            "heading": -0.008300187546286333,
+            "angularVelocity": -4.59551947810532e-9,
+            "velocityX": -2.2882335980104607,
+            "velocityY": 3.012411988206685,
+            "timestamp": 0.46877283278233894
+        },
+        {
+            "x": 6.786560921928144,
+            "y": 3.5881804025568163,
+            "heading": -0.008300187986713265,
+            "angularVelocity": -4.6976584720676315e-9,
+            "velocityX": -2.51268295936015,
+            "velocityY": 2.827907936172931,
+            "timestamp": 0.5625273993388067
+        },
+        {
+            "x": 6.5015204114211205,
+            "y": 3.799228403383322,
+            "heading": -0.008300188501414158,
+            "angularVelocity": -5.489875460815635e-9,
+            "velocityX": -3.0402840200359327,
+            "velocityY": 2.251069026055312,
+            "timestamp": 0.6562819658952744
+        },
+        {
+            "x": 6.179186778264032,
+            "y": 3.9471823835437703,
+            "heading": -0.008300169798343478,
+            "angularVelocity": 1.9948970346527163e-7,
+            "velocityX": -3.4380579527606097,
+            "velocityY": 1.5780989192812942,
+            "timestamp": 0.7500365324517422
+        },
+        {
+            "x": 5.833324835964247,
+            "y": 4.025724182042927,
+            "heading": -0.008299866211032016,
+            "angularVelocity": 0.0000032381069276099122,
+            "velocityX": -3.689014359545628,
+            "velocityY": 0.8377383778085268,
+            "timestamp": 0.8437910990082099
+        },
+        {
+            "x": 5.481092764022396,
+            "y": 4.0672194252777,
+            "heading": -0.008299498284594501,
+            "angularVelocity": 0.000003924357511610969,
+            "velocityX": -3.7569590994770814,
+            "velocityY": 0.44259436909433375,
+            "timestamp": 0.9375456655646777
+        },
+        {
+            "x": 5.14904862351455,
+            "y": 4.179095043376677,
+            "heading": 0.0033651913425959347,
+            "angularVelocity": 0.12441729566489874,
+            "velocityX": -3.5416316527670983,
+            "velocityY": 1.1932818017093059,
+            "timestamp": 1.0313002321211455
+        },
+        {
+            "x": 4.858987953002521,
+            "y": 4.350333701259038,
+            "heading": 0.047887100206889445,
+            "angularVelocity": 0.474877229979814,
+            "velocityX": -3.093829785211866,
+            "velocityY": 1.8264567174893411,
+            "timestamp": 1.1250547986776134
+        },
+        {
+            "x": 4.6181973815778665,
+            "y": 4.575631660267378,
+            "heading": 0.10901822024185409,
+            "angularVelocity": 0.652033519862158,
+            "velocityX": -2.5683076597621346,
+            "velocityY": 2.403061176467013,
+            "timestamp": 1.2188093652340812
+        },
+        {
+            "x": 4.427445933507109,
+            "y": 4.854263962903063,
+            "heading": 0.15298068479753676,
+            "angularVelocity": 0.46891011467909954,
+            "velocityX": -2.0345830083473238,
+            "velocityY": 2.971933132108998,
+            "timestamp": 1.312563931790549
+        },
+        {
+            "x": 4.19709169118396,
+            "y": 5.123942070734021,
+            "heading": 0.15298069409509907,
+            "angularVelocity": 9.916916750047786e-8,
+            "velocityX": -2.4569922381797515,
+            "velocityY": 2.8764263729866686,
+            "timestamp": 1.406318498347017
+        },
+        {
+            "x": 3.9282946631899107,
+            "y": 5.355323824233057,
+            "heading": 0.15298070467968702,
+            "angularVelocity": 1.1289677246612154e-7,
+            "velocityX": -2.8670286458223346,
+            "velocityY": 2.4679518235485323,
+            "timestamp": 1.5000730649034848
+        },
+        {
+            "x": 3.6594937496643456,
+            "y": 5.586701063819283,
+            "heading": 0.1529807152642756,
+            "angularVelocity": 1.1289677885329184e-7,
+            "velocityX": -2.867070089473112,
+            "velocityY": 2.4679036774904017,
+            "timestamp": 1.5938276314599527
+        },
+        {
+            "x": 3.3906928188108107,
+            "y": 5.818078283274842,
+            "heading": 0.1529807258488866,
+            "angularVelocity": 1.1289701863908675e-7,
+            "velocityX": -2.8670702742957928,
+            "velocityY": 2.467903462773746,
+            "timestamp": 1.6875821980164205
+        },
+        {
+            "x": 3.1218918754104785,
+            "y": 6.049455488154229,
+            "heading": 0.15298073643351034,
+            "angularVelocity": 1.1289715379702627e-7,
+            "velocityX": -2.867070408121775,
+            "velocityY": 2.467903307302158,
+            "timestamp": 1.7813367645728884
+        },
+        {
+            "x": 2.853090944613356,
+            "y": 6.280832707674931,
+            "heading": 0.15298074701856604,
+            "angularVelocity": 1.1290176153813175e-7,
+            "velocityX": -2.8670702736940825,
+            "velocityY": 2.4679034634685753,
+            "timestamp": 1.8750913311293562
+        },
+        {
+            "x": 2.631853660621228,
+            "y": 6.471299719512446,
+            "heading": 0.24894777935018825,
+            "angularVelocity": 1.0235984854542721,
+            "velocityX": -2.3597494193403232,
+            "velocityY": 2.0315491696374846,
+            "timestamp": 1.968845897685824
+        },
+        {
+            "x": 2.465926649664366,
+            "y": 6.614149879341798,
+            "heading": 0.32096506390227886,
+            "angularVelocity": 0.7681469521670183,
+            "velocityX": -1.7698019099360354,
+            "velocityY": 1.52366082075921,
+            "timestamp": 2.062600464242292
+        },
+        {
+            "x": 2.355308907973467,
+            "y": 6.709383273555383,
+            "heading": 0.3689895410149908,
+            "angularVelocity": 0.5122361382129277,
+            "velocityX": -1.1798651068828128,
+            "velocityY": 1.0157733933549364,
+            "timestamp": 2.1563550307987596
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0.25609908793666974,
+            "velocityX": -0.5899329494543092,
+            "velocityY": 0.5078870095990298,
+            "timestamp": 2.250109597355227
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0,
+            "velocityX": -3.330818616870831e-32,
+            "velocityY": 5.736512740310497e-33,
+            "timestamp": 2.343864163911695
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C4 To W2.traj b/src/main/deploy/choreo/C4 To W2.traj
new file mode 100644
index 00000000..f49877f8
--- /dev/null
+++ b/src/main/deploy/choreo/C4 To W2.traj	
@@ -0,0 +1,256 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": 6.426641305668581e-31,
+            "velocityX": -5.011351780601486e-32,
+            "velocityY": -1.0551805148640748e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6491080594765135,
+            "y": 2.4958059548034544,
+            "heading": -0.02134777834454554,
+            "angularVelocity": -0.2757848804434067,
+            "velocityX": -0.3990827516771383,
+            "velocityY": 0.5013212819261742,
+            "timestamp": 0.07740735572675958
+        },
+        {
+            "x": 7.587308326375472,
+            "y": 2.5734484495799954,
+            "heading": -0.06302080337998518,
+            "angularVelocity": -0.538360013001088,
+            "velocityX": -0.7983702907923742,
+            "velocityY": 1.0030376835324535,
+            "timestamp": 0.15481471145351916
+        },
+        {
+            "x": 7.4945787588029695,
+            "y": 2.6899753845455243,
+            "heading": -0.12336744193035674,
+            "angularVelocity": -0.7795982433941759,
+            "velocityX": -1.1979425818371583,
+            "velocityY": 1.5053728921687748,
+            "timestamp": 0.23222206718027874
+        },
+        {
+            "x": 7.370887761034633,
+            "y": 2.845471450080925,
+            "heading": -0.19922081386712712,
+            "angularVelocity": -0.9799245979222482,
+            "velocityX": -1.5979230475841766,
+            "velocityY": 2.0088021877957765,
+            "timestamp": 0.3096294229070383
+        },
+        {
+            "x": 7.216202117777622,
+            "y": 3.0401051125017453,
+            "heading": -0.28194072984640106,
+            "angularVelocity": -1.0686312069781692,
+            "velocityX": -1.9983326107022177,
+            "velocityY": 2.5144078439762083,
+            "timestamp": 0.38703677863379793
+        },
+        {
+            "x": 7.032473858656331,
+            "y": 3.268122248690632,
+            "heading": -0.2819407809538647,
+            "angularVelocity": -6.602404015783628e-7,
+            "velocityX": -2.3735245493959742,
+            "velocityY": 2.9456778887237176,
+            "timestamp": 0.46444413436055754
+        },
+        {
+            "x": 6.834683469529352,
+            "y": 3.4840547774927524,
+            "heading": -0.28194080060331267,
+            "angularVelocity": -2.538447128540298e-7,
+            "velocityX": -2.5551885511392456,
+            "velocityY": 2.7895608469606037,
+            "timestamp": 0.5418514900873171
+        },
+        {
+            "x": 6.603064880376339,
+            "y": 3.663221523268011,
+            "heading": -0.2819408226641663,
+            "angularVelocity": -2.8499686418440244e-7,
+            "velocityX": -2.992203867170001,
+            "velocityY": 2.314595868740169,
+            "timestamp": 0.6192588458140768
+        },
+        {
+            "x": 6.344360158554223,
+            "y": 3.8004066501958644,
+            "heading": -0.2819407635550332,
+            "angularVelocity": 7.636113202840995e-7,
+            "velocityX": -3.342120647233029,
+            "velocityY": 1.7722492344539278,
+            "timestamp": 0.6966662015408364
+        },
+        {
+            "x": 6.0661003337524155,
+            "y": 3.891617196013079,
+            "heading": -0.28194070942615473,
+            "angularVelocity": 6.992730598438293e-7,
+            "velocityX": -3.59474654817093,
+            "velocityY": 1.1783188427102327,
+            "timestamp": 0.774073557267596
+        },
+        {
+            "x": 5.7830200242104235,
+            "y": 3.966540038544743,
+            "heading": -0.2819406560965727,
+            "angularVelocity": 6.889472132026456e-7,
+            "velocityX": -3.6570207945254936,
+            "velocityY": 0.967903396624646,
+            "timestamp": 0.8514809129943556
+        },
+        {
+            "x": 5.499939604590639,
+            "y": 4.041462465169378,
+            "heading": -0.2819406027672389,
+            "angularVelocity": 6.889440059098256e-7,
+            "velocityX": -3.657022216584049,
+            "velocityY": 0.9678980236594484,
+            "timestamp": 0.9288882687211152
+        },
+        {
+            "x": 5.234359644348809,
+            "y": 4.1279159927443425,
+            "heading": -0.3272205989423401,
+            "angularVelocity": -0.5849572789301486,
+            "velocityX": -3.430939576069496,
+            "velocityY": 1.1168644990295,
+            "timestamp": 1.0062956244478747
+        },
+        {
+            "x": 4.992805004119873,
+            "y": 4.258058071136475,
+            "heading": -0.38392632852173914,
+            "angularVelocity": -0.7325625458589549,
+            "velocityX": -3.120564421314147,
+            "velocityY": 1.6812624222886623,
+            "timestamp": 1.0837029801746343
+        },
+        {
+            "x": 4.760679474369801,
+            "y": 4.451318301231298,
+            "heading": -0.43925940371465827,
+            "angularVelocity": -0.650567727765224,
+            "velocityX": -2.7291701738847216,
+            "velocityY": 2.272219071899485,
+            "timestamp": 1.1687565000948184
+        },
+        {
+            "x": 4.558064597648056,
+            "y": 4.695759350557987,
+            "heading": -0.45036550730749886,
+            "angularVelocity": -0.13057782444839394,
+            "velocityX": -2.3822044862091225,
+            "velocityY": 2.8739674684372165,
+            "timestamp": 1.2538100200150024
+        },
+        {
+            "x": 4.30999794561686,
+            "y": 4.900667947770545,
+            "heading": -0.4503653988140509,
+            "angularVelocity": 0.0000012755903345298267,
+            "velocityX": -2.9165947777821004,
+            "velocityY": 2.4091724528843765,
+            "timestamp": 1.3388635399351865
+        },
+        {
+            "x": 4.028045524182051,
+            "y": 5.05567579900462,
+            "heading": -0.4503653036134663,
+            "angularVelocity": 0.0000011193021141522574,
+            "velocityX": -3.3150000340890857,
+            "velocityY": 1.8224742653747859,
+            "timestamp": 1.4239170598553705
+        },
+        {
+            "x": 3.731575050469511,
+            "y": 5.180695241574213,
+            "heading": -0.45036521232637566,
+            "angularVelocity": 0.000001073289978617534,
+            "velocityX": -3.485693172848094,
+            "velocityY": 1.4698914599542185,
+            "timestamp": 1.5089705797755546
+        },
+        {
+            "x": 3.4348291016010033,
+            "y": 5.305059402921897,
+            "heading": -0.4503651210307298,
+            "angularVelocity": 0.000001073390565931622,
+            "velocityX": -3.488932017711167,
+            "velocityY": 1.4621871201127652,
+            "timestamp": 1.5940240996957387
+        },
+        {
+            "x": 3.120089238118937,
+            "y": 5.371868296261872,
+            "heading": -0.45036508370615375,
+            "angularVelocity": 4.3883634791364236e-7,
+            "velocityX": -3.7004919229372337,
+            "velocityY": 0.785492398232055,
+            "timestamp": 1.6790776196159227
+        },
+        {
+            "x": 2.8304866761911076,
+            "y": 5.435580552385651,
+            "heading": -0.37567464629819297,
+            "angularVelocity": 0.8781580994889028,
+            "velocityX": -3.404945053415936,
+            "velocityY": 0.7490842963759552,
+            "timestamp": 1.7641311395361068
+        },
+        {
+            "x": 2.5976382602327472,
+            "y": 5.485810295855226,
+            "heading": -0.2445738789443125,
+            "angularVelocity": 1.5413914377313083,
+            "velocityX": -2.7376693660283826,
+            "velocityY": 0.5905663106789283,
+            "timestamp": 1.8491846594562908
+        },
+        {
+            "x": 2.4235493783626687,
+            "y": 5.523078200691987,
+            "heading": -0.12850773623159037,
+            "angularVelocity": 1.3646248012033382,
+            "velocityX": -2.0468157230112984,
+            "velocityY": 0.43817004718549557,
+            "timestamp": 1.934238179376475
+        },
+        {
+            "x": 2.3077761259926057,
+            "y": 5.547737838884683,
+            "heading": -0.04437897835044122,
+            "angularVelocity": 0.9891272925576834,
+            "velocityX": -1.3611812007158846,
+            "velocityY": 0.2899308366756611,
+            "timestamp": 2.0192916992966587
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 1.7803711242706045e-30,
+            "angularVelocity": 0.521777092730431,
+            "velocityX": -0.6792914161204248,
+            "velocityY": 0.14416994295838573,
+            "timestamp": 2.104345219216843
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 1.3562125763475036e-31,
+            "velocityY": 2.290518308039125e-31,
+            "timestamp": 2.189398739137027
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C4 To W3.traj b/src/main/deploy/choreo/C4 To W3.traj
new file mode 100644
index 00000000..15a1cd3f
--- /dev/null
+++ b/src/main/deploy/choreo/C4 To W3.traj	
@@ -0,0 +1,274 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": -4.62162066936404e-33,
+            "velocityX": 3.0688151507515204e-31,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.623584376302727,
+            "y": 2.4297857949616017,
+            "heading": 0.033539307380936634,
+            "angularVelocity": 0.38483343876921716,
+            "velocityX": -0.6473186288895079,
+            "velocityY": -0.3122585684118786,
+            "timestamp": 0.08715278871867994
+        },
+        {
+            "x": 7.510615150776605,
+            "y": 2.375312721693156,
+            "heading": 0.0969954791736376,
+            "angularVelocity": 0.7281025968948721,
+            "velocityX": -1.2962204329545195,
+            "velocityY": -0.6250296068468768,
+            "timestamp": 0.1743055774373599
+        },
+        {
+            "x": 7.340880293995689,
+            "y": 2.2935273668964844,
+            "heading": 0.1838752961952568,
+            "angularVelocity": 0.9968678948651678,
+            "velocityX": -1.9475550843106406,
+            "velocityY": -0.9384135149210906,
+            "timestamp": 0.26145836615603985
+        },
+        {
+            "x": 7.114069036249808,
+            "y": 2.1844157133503646,
+            "heading": 0.27996244701307155,
+            "angularVelocity": 1.1025137833279668,
+            "velocityX": -2.6024555390648842,
+            "velocityY": -1.251958258023393,
+            "timestamp": 0.3486111548747198
+        },
+        {
+            "x": 6.831104073589339,
+            "y": 2.0490142604857513,
+            "heading": 0.32912237638568254,
+            "angularVelocity": 0.5640660510737534,
+            "velocityX": -3.246768885088134,
+            "velocityY": -1.5536101007814562,
+            "timestamp": 0.4357639435933997
+        },
+        {
+            "x": 6.534528280813305,
+            "y": 1.9049975621731787,
+            "heading": 0.3291224030694369,
+            "angularVelocity": 3.0617212321522066e-7,
+            "velocityX": -3.4029409401155197,
+            "velocityY": -1.6524623070575855,
+            "timestamp": 0.5229167323120797
+        },
+        {
+            "x": 6.237952262222522,
+            "y": 1.7609813288852085,
+            "heading": 0.3291224297529421,
+            "angularVelocity": 3.0616926449614015e-7,
+            "velocityX": -3.402943531137019,
+            "velocityY": -1.6524569713178037,
+            "timestamp": 0.6100695210307596
+        },
+        {
+            "x": 5.936787262454918,
+            "y": 1.6268259970502223,
+            "heading": 0.32912245667190637,
+            "angularVelocity": 3.0887094574020004e-7,
+            "velocityX": -3.4555979699024117,
+            "velocityY": -1.5393119808021931,
+            "timestamp": 0.6972223097494396
+        },
+        {
+            "x": 5.615527521313305,
+            "y": 1.5527301986214108,
+            "heading": 0.32912248658360005,
+            "angularVelocity": 3.4320982850713803e-7,
+            "velocityX": -3.6861670850098154,
+            "velocityY": -0.850182759704744,
+            "timestamp": 0.7843750984681195
+        },
+        {
+            "x": 5.286027461953728,
+            "y": 1.5414290964422628,
+            "heading": 0.32912252585289026,
+            "angularVelocity": 4.50579847209181e-7,
+            "velocityX": -3.780717337951944,
+            "velocityY": -0.12967000075725457,
+            "timestamp": 0.8715278871867994
+        },
+        {
+            "x": 4.960446284223111,
+            "y": 1.593339788205863,
+            "heading": 0.3291233220408656,
+            "angularVelocity": 0.000009135542155699871,
+            "velocityX": -3.7357516898462033,
+            "velocityY": 0.5956285797252272,
+            "timestamp": 0.9586806759054793
+        },
+        {
+            "x": 4.650797848549272,
+            "y": 1.7065466836899894,
+            "heading": 0.3291235959637348,
+            "angularVelocity": 0.0000031430189815062194,
+            "velocityX": -3.5529377800330906,
+            "velocityY": 1.2989474823295228,
+            "timestamp": 1.0458334646241594
+        },
+        {
+            "x": 4.368508202692925,
+            "y": 1.8768714467040708,
+            "heading": 0.32912381701495697,
+            "angularVelocity": 0.000002536364302217629,
+            "velocityX": -3.2390202310972316,
+            "velocityY": 1.9543237286860882,
+            "timestamp": 1.1329862533428394
+        },
+        {
+            "x": 4.092384496633659,
+            "y": 2.0570204638146983,
+            "heading": 0.3291240376025158,
+            "angularVelocity": 0.000002531044182467546,
+            "velocityX": -3.1682716080441704,
+            "velocityY": 2.067048223690569,
+            "timestamp": 1.2201390420615195
+        },
+        {
+            "x": 3.8162567615509033,
+            "y": 2.2371633052825928,
+            "heading": 0.32912425818941665,
+            "angularVelocity": 0.0000025310366317804206,
+            "velocityX": -3.1683178374712258,
+            "velocityY": 2.066977363735053,
+            "timestamp": 1.3072918307801995
+        },
+        {
+            "x": 3.558387727399027,
+            "y": 2.3518646644990793,
+            "heading": 0.32912440943088817,
+            "angularVelocity": 0.0000020272145178599712,
+            "velocityX": -3.4564319203160125,
+            "velocityY": 1.5374371746628512,
+            "timestamp": 1.3818973893726278
+        },
+        {
+            "x": 3.300516056227942,
+            "y": 2.466560095044955,
+            "heading": 0.3291245606718786,
+            "angularVelocity": 0.000002027208070545675,
+            "velocityX": -3.4564672664652627,
+            "velocityY": 1.5373577077876843,
+            "timestamp": 1.4565029479650562
+        },
+        {
+            "x": 3.052458337002912,
+            "y": 2.601171414843865,
+            "heading": 0.329124716605593,
+            "angularVelocity": 0.0000020901085294853097,
+            "velocityX": -3.3249227524744356,
+            "velocityY": 1.804306841723335,
+            "timestamp": 1.5311085065574845
+        },
+        {
+            "x": 2.829814908557799,
+            "y": 2.7746151638451657,
+            "heading": 0.3291249161701291,
+            "angularVelocity": 0.000002674928516490017,
+            "velocityX": -2.984273995740964,
+            "velocityY": 2.3248100044237634,
+            "timestamp": 1.6057140651499129
+        },
+        {
+            "x": 2.638606432152533,
+            "y": 2.982201542308141,
+            "heading": 0.3291253317978473,
+            "angularVelocity": 0.000005571002027267265,
+            "velocityX": -2.562925337103115,
+            "velocityY": 2.7824519027734036,
+            "timestamp": 1.6803196237423412
+        },
+        {
+            "x": 2.4840036736729245,
+            "y": 3.218316897029485,
+            "heading": 0.3291273340604081,
+            "angularVelocity": 0.000026837980957261232,
+            "velocityX": -2.0722686271167303,
+            "velocityY": 3.1648493647939433,
+            "timestamp": 1.7549251823347696
+        },
+        {
+            "x": 2.3733884856918945,
+            "y": 3.4659660107751304,
+            "heading": 0.2954274954004952,
+            "angularVelocity": -0.4517068070492687,
+            "velocityX": -1.4826668423638905,
+            "velocityY": 3.31944587532086,
+            "timestamp": 1.829530740927198
+        },
+        {
+            "x": 2.305023098579148,
+            "y": 3.7064756804968377,
+            "heading": 0.19598318894217118,
+            "angularVelocity": -1.3329342790875696,
+            "velocityX": -0.9163578210871337,
+            "velocityY": 3.2237500028063106,
+            "timestamp": 1.9041362995196263
+        },
+        {
+            "x": 2.272475779606133,
+            "y": 3.9200958593762563,
+            "heading": 0.053473340322528436,
+            "angularVelocity": -1.9101773555262378,
+            "velocityX": -0.43625863256144176,
+            "velocityY": 2.8633279196585013,
+            "timestamp": 1.9787418581120546
+        },
+        {
+            "x": 2.2677016800416907,
+            "y": 4.09662450199971,
+            "heading": -0.09830221598718161,
+            "angularVelocity": -2.034373298360552,
+            "velocityX": -0.06399120460344819,
+            "velocityY": 2.3661593848232187,
+            "timestamp": 2.053347416704483
+        },
+        {
+            "x": 2.2778239174156503,
+            "y": 4.229436050413757,
+            "heading": -0.23380831521279008,
+            "angularVelocity": -1.8163003103546322,
+            "velocityX": 0.13567671853055638,
+            "velocityY": 1.7801830174558415,
+            "timestamp": 2.1279529752969113
+        },
+        {
+            "x": 2.29107059750262,
+            "y": 4.316936633046431,
+            "heading": -0.33501872696397506,
+            "angularVelocity": -1.3566068488823915,
+            "velocityX": 0.1775562081015563,
+            "velocityY": 1.1728426712906341,
+            "timestamp": 2.2025585338893396
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.7369594715641588,
+            "velocityX": 0.11968816621507666,
+            "velocityY": 0.5772139203303202,
+            "timestamp": 2.277164092481768
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -8.092865962806098e-32,
+            "velocityX": 0,
+            "velocityY": 4.648171279746289e-33,
+            "timestamp": 2.3517696510741963
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C5 To C1.traj b/src/main/deploy/choreo/C5 To C1.traj
new file mode 100644
index 00000000..8293ba75
--- /dev/null
+++ b/src/main/deploy/choreo/C5 To C1.traj	
@@ -0,0 +1,220 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -1.1520076473353784e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 0.8649552704871435,
+            "heading": -1.2072196888077206e-18,
+            "angularVelocity": -1.2485272283928241e-17,
+            "velocityX": -1.2865550388289577e-22,
+            "velocityY": 0.8062267267095138,
+            "timestamp": 0.09669149868710694
+        },
+        {
+            "x": 7.68,
+            "y": 1.0208658083668138,
+            "heading": -3.6224298175784965e-18,
+            "angularVelocity": -2.497851580797786e-17,
+            "velocityX": -3.7534290948711127e-22,
+            "velocityY": 1.6124534214139756,
+            "timestamp": 0.19338299737421388
+        },
+        {
+            "x": 7.68,
+            "y": 1.254731607449781,
+            "heading": -7.246204108405477e-18,
+            "angularVelocity": -3.7477692864160655e-17,
+            "velocityX": -8.581224837351263e-22,
+            "velocityY": 2.4186800521083573,
+            "timestamp": 0.29007449606132085
+        },
+        {
+            "x": 7.68,
+            "y": 1.5665526491683786,
+            "heading": -1.208276035533523e-17,
+            "angularVelocity": -5.0020491067317066e-17,
+            "velocityX": -2.0493771641846554e-21,
+            "velocityY": 3.2249064907727645,
+            "timestamp": 0.38676599474842777
+        },
+        {
+            "x": 7.68,
+            "y": 1.9323308421442915,
+            "heading": -4.9593132234185194e-18,
+            "angularVelocity": 7.367190734076976e-17,
+            "velocityX": 8.922575847291113e-21,
+            "velocityY": 3.782940568121389,
+            "timestamp": 0.4834574934355347
+        },
+        {
+            "x": 7.68,
+            "y": 2.298109035120243,
+            "heading": 4.533704301231546e-17,
+            "angularVelocity": 5.201735097557347e-16,
+            "velocityX": 4.097272417616201e-22,
+            "velocityY": 3.782940568121787,
+            "timestamp": 0.5801489921226416
+        },
+        {
+            "x": 7.68,
+            "y": 2.6638872280961943,
+            "heading": 2.5191507323029372e-17,
+            "angularVelocity": -2.083485721374208e-16,
+            "velocityX": 1.8682331175835897e-21,
+            "velocityY": 3.782940568121787,
+            "timestamp": 0.6768404908097485
+        },
+        {
+            "x": 7.68,
+            "y": 3.0296654210721456,
+            "heading": 5.2668856754382746e-17,
+            "angularVelocity": 2.8417544256148633e-16,
+            "velocityX": 2.5170076181953607e-21,
+            "velocityY": 3.782940568121787,
+            "timestamp": 0.7735319894968554
+        },
+        {
+            "x": 7.68,
+            "y": 3.395443614048097,
+            "heading": 4.929611386988728e-17,
+            "angularVelocity": -3.48814831737134e-17,
+            "velocityX": 3.0934195236040766e-21,
+            "velocityY": 3.782940568121787,
+            "timestamp": 0.8702234881839623
+        },
+        {
+            "x": 7.68,
+            "y": 3.7612218070240484,
+            "heading": 4.8005098046144365e-17,
+            "angularVelocity": -1.3351906227498234e-17,
+            "velocityX": -1.4483318353601114e-20,
+            "velocityY": 3.782940568121787,
+            "timestamp": 0.9669149868710692
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.8136822799879804e-17,
+            "angularVelocity": -1.0205938867695289e-16,
+            "velocityX": -1.0465805642283078e-22,
+            "velocityY": 3.782940568121787,
+            "timestamp": 1.0636064855581762
+        },
+        {
+            "x": 7.68,
+            "y": 4.492778192975952,
+            "heading": 4.310486593468924e-17,
+            "angularVelocity": 5.1380350929111975e-17,
+            "velocityX": 2.042774398860212e-21,
+            "velocityY": 3.782940568121787,
+            "timestamp": 1.1602979842452832
+        },
+        {
+            "x": 7.68,
+            "y": 4.8585563859519025,
+            "heading": 1.5339712420766394e-17,
+            "angularVelocity": -2.8715196155735124e-16,
+            "velocityX": 3.895366641644577e-22,
+            "velocityY": 3.782940568121787,
+            "timestamp": 1.2569894829323902
+        },
+        {
+            "x": 7.68,
+            "y": 5.224334578927854,
+            "heading": -2.710858238227145e-17,
+            "angularVelocity": -4.390075175101735e-16,
+            "velocityX": -3.863371094269421e-21,
+            "velocityY": 3.782940568121787,
+            "timestamp": 1.3536809816194972
+        },
+        {
+            "x": 7.68,
+            "y": 5.590112771903805,
+            "heading": -3.378098304971619e-17,
+            "angularVelocity": -6.900710774001486e-17,
+            "velocityX": -4.042329982282679e-22,
+            "velocityY": 3.782940568121787,
+            "timestamp": 1.4503724803066043
+        },
+        {
+            "x": 7.68,
+            "y": 5.955890964879757,
+            "heading": -3.941496183315801e-17,
+            "angularVelocity": -5.826757119208287e-17,
+            "velocityX": 2.6937052538433574e-21,
+            "velocityY": 3.782940568121787,
+            "timestamp": 1.5470639789937113
+        },
+        {
+            "x": 7.68,
+            "y": 6.321669157855708,
+            "heading": -1.9923808577496458e-17,
+            "angularVelocity": 2.0158083720198419e-16,
+            "velocityX": -4.499746264941967e-21,
+            "velocityY": 3.782940568121787,
+            "timestamp": 1.6437554776808183
+        },
+        {
+            "x": 7.68,
+            "y": 6.68744735083162,
+            "heading": 1.1959451428437676e-17,
+            "angularVelocity": 3.297421225035248e-16,
+            "velocityX": 5.431014750114918e-21,
+            "velocityY": 3.782940568121389,
+            "timestamp": 1.7404469763679253
+        },
+        {
+            "x": 7.68,
+            "y": 6.999268392550218,
+            "heading": 7.18685516064291e-18,
+            "angularVelocity": -4.935900605111643e-17,
+            "velocityX": -2.4046856363551417e-22,
+            "velocityY": 3.2249064907727645,
+            "timestamp": 1.8371384750550324
+        },
+        {
+            "x": 7.68,
+            "y": 7.2331341916331855,
+            "heading": 3.596188330325507e-18,
+            "angularVelocity": -3.713528985888002e-17,
+            "velocityX": -1.8035546521541034e-22,
+            "velocityY": 2.4186800521083573,
+            "timestamp": 1.9338299737421394
+        },
+        {
+            "x": 7.68,
+            "y": 7.3890447295128565,
+            "heading": 1.199348678010309e-18,
+            "angularVelocity": -2.4788525206448663e-17,
+            "velocityX": -1.2023814854884031e-22,
+            "velocityY": 1.6124534214139756,
+            "timestamp": 2.030521472429246
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": -1.2403868947555692e-17,
+            "velocityX": -6.011942579686287e-23,
+            "velocityY": 0.8062267267095138,
+            "timestamp": 2.127212971116353
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -2.719452927302828e-33,
+            "velocityY": 0,
+            "timestamp": 2.2239044698034602
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C5 To C2.traj b/src/main/deploy/choreo/C5 To C2.traj
new file mode 100644
index 00000000..f16ed7e1
--- /dev/null
+++ b/src/main/deploy/choreo/C5 To C2.traj	
@@ -0,0 +1,175 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.5195168592304314e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 0.8688441539517631,
+            "heading": 1.4487899486085006e-18,
+            "angularVelocity": 1.4623322004407643e-17,
+            "velocityX": 2.341988908112997e-19,
+            "velocityY": 0.8260917454276263,
+            "timestamp": 0.09907392781100396
+        },
+        {
+            "x": 7.68,
+            "y": 1.03253245874193,
+            "heading": 4.3483011920968684e-18,
+            "angularVelocity": 2.926613800008521e-17,
+            "velocityX": 4.684469740721679e-19,
+            "velocityY": 1.6521834594306495,
+            "timestamp": 0.19814785562200793
+        },
+        {
+            "x": 7.68,
+            "y": 1.2780649081437847,
+            "heading": 8.701494230539972e-18,
+            "angularVelocity": 4.393883572228609e-17,
+            "velocityX": 7.027934476495029e-19,
+            "velocityY": 2.4782751105844794,
+            "timestamp": 0.29722178343301187
+        },
+        {
+            "x": 7.68,
+            "y": 1.6054414834771922,
+            "heading": 1.4516368843485863e-17,
+            "angularVelocity": 5.869227900187472e-17,
+            "velocityX": 9.37435089599571e-19,
+            "velocityY": 3.304366573190877,
+            "timestamp": 0.39629571124401586
+        },
+        {
+            "x": 7.68,
+            "y": 1.9802322649266864,
+            "heading": 6.647154079192576e-18,
+            "angularVelocity": -7.942770553424421e-17,
+            "velocityX": 1.0709424285811041e-18,
+            "velocityY": 3.782940575087071,
+            "timestamp": 0.49536963905501985
+        },
+        {
+            "x": 7.68,
+            "y": 2.3550230463762047,
+            "heading": -4.11606275908414e-17,
+            "angularVelocity": -4.825465460619937e-16,
+            "velocityX": 1.073083844244948e-18,
+            "velocityY": 3.7829405750873115,
+            "timestamp": 0.5944435668660238
+        },
+        {
+            "x": 7.68,
+            "y": 2.729813827825723,
+            "heading": -7.635066611173202e-17,
+            "angularVelocity": -3.5518969822232415e-16,
+            "velocityX": 1.0738946259920485e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.6935174946770278
+        },
+        {
+            "x": 7.68,
+            "y": 3.1046046092752415,
+            "heading": -5.864574302033697e-17,
+            "angularVelocity": 1.787041604453991e-16,
+            "velocityX": 1.0691415340399175e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.7925914224880318
+        },
+        {
+            "x": 7.68,
+            "y": 3.4793953907247595,
+            "heading": -2.6391173603704442e-17,
+            "angularVelocity": 3.255606205313886e-16,
+            "velocityX": 1.0732289061667476e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.8916653502990358
+        },
+        {
+            "x": 7.68,
+            "y": 3.854186172174278,
+            "heading": -6.511485011951871e-17,
+            "angularVelocity": -3.9085637736788184e-16,
+            "velocityX": 1.0716697150075795e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.9907392781100398
+        },
+        {
+            "x": 7.68,
+            "y": 4.228976953623796,
+            "heading": -6.241329479526944e-17,
+            "angularVelocity": 2.7268075304360027e-17,
+            "velocityX": 1.0686111075844087e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 1.0898132059210437
+        },
+        {
+            "x": 7.68,
+            "y": 4.603767735073314,
+            "heading": -2.2737833863559818e-17,
+            "angularVelocity": 4.004631875225205e-16,
+            "velocityX": 1.0710197916722359e-18,
+            "velocityY": 3.7829405750873115,
+            "timestamp": 1.1888871337320477
+        },
+        {
+            "x": 7.68,
+            "y": 4.978558516522808,
+            "heading": -1.448944579275074e-17,
+            "angularVelocity": 8.325488100707829e-17,
+            "velocityX": 1.0714504382145178e-18,
+            "velocityY": 3.782940575087071,
+            "timestamp": 1.2879610615430517
+        },
+        {
+            "x": 7.68,
+            "y": 5.305935091856216,
+            "heading": -8.687037170545135e-18,
+            "angularVelocity": 5.856645386228716e-17,
+            "velocityX": 9.36753995288457e-19,
+            "velocityY": 3.304366573190877,
+            "timestamp": 1.3870349893540557
+        },
+        {
+            "x": 7.68,
+            "y": 5.55146754125807,
+            "heading": -4.341905072762387e-18,
+            "angularVelocity": 4.385747283685971e-17,
+            "velocityX": 7.025655481927048e-19,
+            "velocityY": 2.4782751105844794,
+            "timestamp": 1.4861089171650597
+        },
+        {
+            "x": 7.68,
+            "y": 5.715155846048237,
+            "heading": -1.4469357825353783e-18,
+            "angularVelocity": 2.9220293918225814e-17,
+            "velocityX": 4.683770471216647e-19,
+            "velocityY": 1.6521834594306495,
+            "timestamp": 1.5851828449760637
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 1.460460702963924e-17,
+            "velocityX": 2.341885280588165e-19,
+            "velocityY": 0.8260917454276263,
+            "timestamp": 1.6842567727870676
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 3.1812192470207854e-34,
+            "velocityY": 0,
+            "timestamp": 1.7833307005980716
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C5 To C3.traj b/src/main/deploy/choreo/C5 To C3.traj
new file mode 100644
index 00000000..864eb2e4
--- /dev/null
+++ b/src/main/deploy/choreo/C5 To C3.traj	
@@ -0,0 +1,139 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 2.417290808823166e-40,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 0.8634453720055388,
+            "heading": 1.009810999063182e-18,
+            "angularVelocity": 1.0546271494620871e-17,
+            "velocityX": -3.226662245674165e-34,
+            "velocityY": 0.7983807350946677,
+            "timestamp": 0.09575052183150978
+        },
+        {
+            "x": 7.68,
+            "y": 1.0163361129826893,
+            "heading": 3.0323418642587253e-18,
+            "angularVelocity": 2.112292263585315e-17,
+            "velocityX": -6.037770263383471e-34,
+            "velocityY": 1.596761438503583,
+            "timestamp": 0.19150104366301957
+        },
+        {
+            "x": 7.68,
+            "y": 1.2456722168635992,
+            "heading": 6.073900773319643e-18,
+            "angularVelocity": 3.1765455168164525e-17,
+            "velocityX": -4.821612934727609e-34,
+            "velocityY": 2.3951420785410233,
+            "timestamp": 0.28725156549452935
+        },
+        {
+            "x": 7.68,
+            "y": 1.5514536654447455,
+            "heading": 1.0152788689831222e-17,
+            "angularVelocity": 4.2599119433487623e-17,
+            "velocityX": -1.2140491970354773e-33,
+            "velocityY": 3.19352252846437,
+            "timestamp": 0.38300208732603913
+        },
+        {
+            "x": 7.68,
+            "y": 1.9136721992668209,
+            "heading": 4.411917253468209e-18,
+            "angularVelocity": -5.995655508243724e-17,
+            "velocityX": 4.90795348832912e-33,
+            "velocityY": 3.7829405719528495,
+            "timestamp": 0.4787526091575489
+        },
+        {
+            "x": 7.68,
+            "y": 2.27589073308894,
+            "heading": 1.2796107303848674e-17,
+            "angularVelocity": 8.756286535058141e-17,
+            "velocityX": -1.5435756690671252e-33,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.5745031309890587
+        },
+        {
+            "x": 7.68,
+            "y": 2.63810926691106,
+            "heading": 6.994895399550348e-18,
+            "angularVelocity": -6.058673930264937e-17,
+            "velocityX": 4.776897057793067e-34,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.6702536528205685
+        },
+        {
+            "x": 7.68,
+            "y": 3.0003278007331793,
+            "heading": -9.534020306729642e-18,
+            "angularVelocity": -1.7262481070720064e-16,
+            "velocityX": 2.9275045647973183e-33,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.7660041746520783
+        },
+        {
+            "x": 7.68,
+            "y": 3.3625463345552546,
+            "heading": -1.0013819273727212e-17,
+            "angularVelocity": -5.010927959660929e-18,
+            "velocityX": 2.237784317663532e-33,
+            "velocityY": 3.7829405719528495,
+            "timestamp": 0.861754696483588
+        },
+        {
+            "x": 7.68,
+            "y": 3.6683277831364007,
+            "heading": -6.0064636534747415e-18,
+            "angularVelocity": 4.185204993640188e-17,
+            "velocityX": -2.2313398142102424e-33,
+            "velocityY": 3.19352252846437,
+            "timestamp": 0.9575052183150978
+        },
+        {
+            "x": 7.68,
+            "y": 3.897663887017311,
+            "heading": -3.002975312877452e-18,
+            "angularVelocity": 3.136785349810471e-17,
+            "velocityX": -2.8833027864886262e-33,
+            "velocityY": 2.3951420785410233,
+            "timestamp": 1.0532557401466076
+        },
+        {
+            "x": 7.68,
+            "y": 4.050554627994461,
+            "heading": -1.0009557447862449e-18,
+            "angularVelocity": 2.0908706606963636e-17,
+            "velocityX": -3.742421273800505e-33,
+            "velocityY": 1.596761438503583,
+            "timestamp": 1.1490062619781174
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 1.0453788927770355e-17,
+            "velocityX": -1.5032681111068507e-33,
+            "velocityY": 0.7983807350946677,
+            "timestamp": 1.2447567838096272
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 9.665729080515376e-43,
+            "velocityY": 2.80463986608038e-38,
+            "timestamp": 1.340507305641137
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C5 To C4.traj b/src/main/deploy/choreo/C5 To C4.traj
new file mode 100644
index 00000000..17170fc9
--- /dev/null
+++ b/src/main/deploy/choreo/C5 To C4.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 9.069206296242399e-40,
+            "angularVelocity": -9.927610039652183e-40,
+            "velocityX": -8.253947176298157e-40,
+            "velocityY": 1.8367099231598242e-40,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 0.8705000055758465,
+            "heading": 6.490773345690525e-22,
+            "angularVelocity": 6.486159683422905e-21,
+            "velocityX": -7.132918469580153e-33,
+            "velocityY": 0.8344065481228213,
+            "timestamp": 0.10007112931184182
+        },
+        {
+            "x": 7.68,
+            "y": 1.037500013629847,
+            "heading": 2.023593010760001e-21,
+            "angularVelocity": 1.3735386658326633e-20,
+            "velocityX": -1.5648094960655976e-32,
+            "velocityY": 1.668813065290736,
+            "timestamp": 0.20014225862368365
+        },
+        {
+            "x": 7.68,
+            "y": 1.2880000179666162,
+            "heading": 3.787875676222919e-21,
+            "angularVelocity": 1.7630286060953193e-20,
+            "velocityX": -2.3089760494009302e-32,
+            "velocityY": 2.503219520548835,
+            "timestamp": 0.3002133879355255
+        },
+        {
+            "x": 7.68,
+            "y": 1.6219999999999999,
+            "heading": 5.860880748165694e-21,
+            "angularVelocity": 2.0715315514205814e-20,
+            "velocityX": -3.0704903308303665e-32,
+            "velocityY": 3.3376257900775013,
+            "timestamp": 0.4002845172473673
+        },
+        {
+            "x": 7.68,
+            "y": 1.9559999820333838,
+            "heading": 3.934658943161884e-21,
+            "angularVelocity": -1.9248526176341983e-20,
+            "velocityX": -4.040562356218352e-32,
+            "velocityY": 3.3376257900775017,
+            "timestamp": 0.5003556465592092
+        },
+        {
+            "x": 7.68,
+            "y": 2.206499986370153,
+            "heading": 1.6176834885162177e-21,
+            "angularVelocity": -2.3153285409554772e-20,
+            "velocityX": -3.08903871620124e-32,
+            "velocityY": 2.503219520548835,
+            "timestamp": 0.600426775871051
+        },
+        {
+            "x": 7.68,
+            "y": 2.3734999944241535,
+            "heading": 4.992981261257204e-22,
+            "angularVelocity": -1.1175904096118944e-20,
+            "velocityX": -2.3331216062245415e-32,
+            "velocityY": 1.668813065290736,
+            "timestamp": 0.7004979051828928
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 1.7562757360140444e-40,
+            "angularVelocity": -4.9894322348928345e-21,
+            "velocityX": -1.3647931662662733e-32,
+            "velocityY": 0.8344065481228213,
+            "timestamp": 0.8005690344947346
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -7.274275970183695e-41,
+            "angularVelocity": -2.4819026120794767e-39,
+            "velocityX": -8.637198685981915e-42,
+            "velocityY": 5.142787784847508e-39,
+            "timestamp": 0.9006401638065764
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C5 To S3.traj b/src/main/deploy/choreo/C5 To S3.traj
new file mode 100644
index 00000000..d9cc7c97
--- /dev/null
+++ b/src/main/deploy/choreo/C5 To S3.traj	
@@ -0,0 +1,247 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": -3.982415645335998e-31,
+            "velocityX": 3.346280928312519e-33,
+            "velocityY": 3.541078180282768e-38,
+            "timestamp": 0
+        },
+        {
+            "x": 7.629395539536042,
+            "y": 0.7881897280541793,
+            "heading": -0.02207380596023684,
+            "angularVelocity": -0.2822732032732244,
+            "velocityX": -0.6471146471374848,
+            "velocityY": 0.015213885156899113,
+            "timestamp": 0.0782001468948451
+        },
+        {
+            "x": 7.528184091504648,
+            "y": 0.7905692108612611,
+            "heading": -0.06614402373093474,
+            "angularVelocity": -0.5635567133951742,
+            "velocityX": -1.2942616101155584,
+            "velocityY": 0.030428111729043,
+            "timestamp": 0.1564002937896902
+        },
+        {
+            "x": 7.376360973312952,
+            "y": 0.7941378827255743,
+            "heading": -0.13206235338942335,
+            "angularVelocity": -0.8429438086302324,
+            "velocityX": -1.9414684526853105,
+            "velocityY": 0.04563510435477082,
+            "timestamp": 0.23460044068453528
+        },
+        {
+            "x": 7.173916466766183,
+            "y": 0.7988917309385671,
+            "heading": -0.21951772550448787,
+            "angularVelocity": -1.1183530413716716,
+            "velocityX": -2.588799568612108,
+            "velocityY": 0.060790783668257076,
+            "timestamp": 0.3128005875793804
+        },
+        {
+            "x": 6.920824289019536,
+            "y": 0.8048108023879232,
+            "heading": -0.3276760191057486,
+            "angularVelocity": -1.383095785572821,
+            "velocityX": -3.236466781667369,
+            "velocityY": 0.07569130857196414,
+            "timestamp": 0.3910007344742255
+        },
+        {
+            "x": 6.625087122175782,
+            "y": 0.8120803100358527,
+            "heading": -0.3276760358371292,
+            "angularVelocity": -2.1395587179409026e-7,
+            "velocityX": -3.781798098684327,
+            "velocityY": 0.09296028123113304,
+            "timestamp": 0.4692008813690706
+        },
+        {
+            "x": 6.329343587367458,
+            "y": 0.8190859886904246,
+            "heading": -0.3276760513572638,
+            "angularVelocity": -1.9846682128968708e-7,
+            "velocityX": -3.78187953030339,
+            "velocityY": 0.08958651527359804,
+            "timestamp": 0.5474010282639157
+        },
+        {
+            "x": 6.033600055760057,
+            "y": 0.826091802470254,
+            "heading": -0.3276760668773978,
+            "angularVelocity": -1.9846681457625392e-7,
+            "velocityX": -3.7818794893709464,
+            "velocityY": 0.0895882432148227,
+            "timestamp": 0.6256011751587608
+        },
+        {
+            "x": 5.7383102527596765,
+            "y": 0.8439033174946344,
+            "heading": -0.3276760825327013,
+            "angularVelocity": -2.0019532088764418e-7,
+            "velocityX": -3.7760773441696487,
+            "velocityY": 0.22776830647924015,
+            "timestamp": 0.7038013220536059
+        },
+        {
+            "x": 5.450465299571764,
+            "y": 0.9121572529445121,
+            "heading": -0.32767609968005607,
+            "angularVelocity": -2.1927522433491614e-7,
+            "velocityX": -3.680874839978332,
+            "velocityY": 0.8728108342551084,
+            "timestamp": 0.782001468948451
+        },
+        {
+            "x": 5.178616904201158,
+            "y": 1.028826716826592,
+            "heading": -0.32767612118071976,
+            "angularVelocity": -2.749440322201668e-7,
+            "velocityX": -3.4763156613508643,
+            "velocityY": 1.4919340757663395,
+            "timestamp": 0.8602016158432961
+        },
+        {
+            "x": 4.930841529412244,
+            "y": 1.190445624246428,
+            "heading": -0.3276761534331376,
+            "angularVelocity": -4.1243423626599064e-7,
+            "velocityX": -3.1684771017365216,
+            "velocityY": 2.0667340642903906,
+            "timestamp": 0.9384017627381412
+        },
+        {
+            "x": 4.7145004488669215,
+            "y": 1.3922123936122235,
+            "heading": -0.32767621878386327,
+            "angularVelocity": -8.356854591223944e-7,
+            "velocityX": -2.7665047846524513,
+            "velocityY": 2.5801328690255976,
+            "timestamp": 1.0166019096329864
+        },
+        {
+            "x": 4.536026477813721,
+            "y": 1.628126621246338,
+            "heading": -0.327696539002069,
+            "angularVelocity": -0.00025984882823803823,
+            "velocityX": -2.282271557538034,
+            "velocityY": 3.016800313073996,
+            "timestamp": 1.0948020565278314
+        },
+        {
+            "x": 4.4347589269746495,
+            "y": 1.8009845745755027,
+            "heading": -0.3311064514524199,
+            "angularVelocity": -0.06394544615832723,
+            "velocityX": -1.899051313343304,
+            "velocityY": 3.241572651564751,
+            "timestamp": 1.1481273933038434
+        },
+        {
+            "x": 4.355560152796487,
+            "y": 1.9823487857199633,
+            "heading": -0.3404702376957046,
+            "angularVelocity": -0.17559732033810904,
+            "velocityX": -1.4851996999249673,
+            "velocityY": 3.4010889027473166,
+            "timestamp": 1.2014527300798554
+        },
+        {
+            "x": 4.29961502910342,
+            "y": 2.1668323093896307,
+            "heading": -0.3620606217709223,
+            "angularVelocity": -0.40488040734269476,
+            "velocityX": -1.049128370778876,
+            "velocityY": 3.4595847832064672,
+            "timestamp": 1.2547780668558675
+        },
+        {
+            "x": 4.26624261518445,
+            "y": 2.3445509425061104,
+            "heading": -0.3930653835662206,
+            "angularVelocity": -0.5814264600999738,
+            "velocityX": -0.6258265945712352,
+            "velocityY": 3.332724064417915,
+            "timestamp": 1.3081034036318795
+        },
+        {
+            "x": 4.250245555353669,
+            "y": 2.506179624728893,
+            "heading": -0.42827286207411436,
+            "angularVelocity": -0.6602392152823773,
+            "velocityX": -0.2999898509389163,
+            "velocityY": 3.0309922448647635,
+            "timestamp": 1.3614287404078915
+        },
+        {
+            "x": 4.245253885327146,
+            "y": 2.646807044638539,
+            "heading": -0.46326114250173545,
+            "angularVelocity": -0.6561286349527023,
+            "velocityX": -0.09360784813172378,
+            "velocityY": 2.6371595270071015,
+            "timestamp": 1.4147540771839036
+        },
+        {
+            "x": 4.246491895421447,
+            "y": 2.764573131222025,
+            "heading": -0.49520995766037457,
+            "angularVelocity": -0.5991301150594935,
+            "velocityX": 0.023216170194527277,
+            "velocityY": 2.2084452476729117,
+            "timestamp": 1.4680794139599156
+        },
+        {
+            "x": 4.250760184591696,
+            "y": 2.858861309855261,
+            "heading": -0.522398988898447,
+            "angularVelocity": -0.5098707834110707,
+            "velocityX": 0.08004242313776105,
+            "velocityY": 1.7681684605074017,
+            "timestamp": 1.5214047507359276
+        },
+        {
+            "x": 4.255888225984497,
+            "y": 2.929511890736146,
+            "heading": -0.5437259487715407,
+            "angularVelocity": -0.3999404628668233,
+            "velocityX": 0.09616519468543994,
+            "velocityY": 1.324897040550944,
+            "timestamp": 1.5747300875119397
+        },
+        {
+            "x": 4.260343621579396,
+            "y": 2.9765343465211713,
+            "heading": -0.5584397105575287,
+            "angularVelocity": -0.2759244043319095,
+            "velocityX": 0.08355119468920533,
+            "velocityY": 0.8818032595380478,
+            "timestamp": 1.6280554242879517
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -0.14177668439456792,
+            "velocityX": 0.04981456435476623,
+            "velocityY": 0.4400469813703916,
+            "timestamp": 1.6813807610639637
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 0,
+            "velocityX": 3.549235715029611e-32,
+            "velocityY": -9.598836837089386e-32,
+            "timestamp": 1.7347060978399758
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C5 To S4.traj b/src/main/deploy/choreo/C5 To S4.traj
new file mode 100644
index 00000000..773c6f5e
--- /dev/null
+++ b/src/main/deploy/choreo/C5 To S4.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 2.8467808772943154e-31,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.633526252649942,
+            "y": 0.8024218861992577,
+            "heading": -0.02675508445787236,
+            "angularVelocity": -0.3471453590528024,
+            "velocityX": -0.602993637929559,
+            "velocityY": 0.20009790028293237,
+            "timestamp": 0.07707170428800919
+        },
+        {
+            "x": 7.540488865885011,
+            "y": 0.8333059401759839,
+            "heading": -0.07783349665964928,
+            "angularVelocity": -0.6627388439588938,
+            "velocityX": -1.2071536191448495,
+            "velocityY": 0.400718451240102,
+            "timestamp": 0.15414340857601838
+        },
+        {
+            "x": 7.400762361365792,
+            "y": 0.8797032018771439,
+            "heading": -0.14939031986058435,
+            "angularVelocity": -0.9284448016555393,
+            "velocityX": -1.8129416730824437,
+            "velocityY": 0.6020012419574645,
+            "timestamp": 0.23121511286402757
+        },
+        {
+            "x": 7.214174023420098,
+            "y": 0.9416744881041137,
+            "heading": -0.23458534610519022,
+            "angularVelocity": -1.105399537114693,
+            "velocityX": -2.4209707008480317,
+            "velocityY": 0.8040731264406625,
+            "timestamp": 0.30828681715203676
+        },
+        {
+            "x": 6.98058449689491,
+            "y": 1.0192605514016468,
+            "heading": -0.3182722929391017,
+            "angularVelocity": -1.085832311702643,
+            "velocityX": -3.030807852026836,
+            "velocityY": 1.0066737723562156,
+            "timestamp": 0.38535852144004595
+        },
+        {
+            "x": 6.7044949607857305,
+            "y": 1.1109419048147462,
+            "heading": -0.3204977394277859,
+            "angularVelocity": -0.02887501333002772,
+            "velocityX": -3.5822425189594846,
+            "velocityY": 1.189559180766205,
+            "timestamp": 0.46243022572805514
+        },
+        {
+            "x": 6.427835578289735,
+            "y": 1.202949830409229,
+            "heading": -0.32049776831244,
+            "angularVelocity": -3.7477637567079044e-7,
+            "velocityX": -3.5896362361749183,
+            "velocityY": 1.1937964320946928,
+            "timestamp": 0.5395019300160644
+        },
+        {
+            "x": 6.1511761970625,
+            "y": 1.294957759818759,
+            "heading": -0.3204977971970938,
+            "angularVelocity": -3.7477637370825724e-7,
+            "velocityX": -3.589636219712834,
+            "velocityY": 1.1937964815946673,
+            "timestamp": 0.6165736343040735
+        },
+        {
+            "x": 5.87524941446386,
+            "y": 1.3871813457379307,
+            "heading": -0.32266464886855784,
+            "angularVelocity": -0.028114749653993878,
+            "velocityX": -3.5801308034856563,
+            "velocityY": 1.1965946098005253,
+            "timestamp": 0.6936453385920827
+        },
+        {
+            "x": 5.641703675217654,
+            "y": 1.4649149367510799,
+            "heading": -0.4060390277887848,
+            "angularVelocity": -1.081776764773039,
+            "velocityX": -3.030239715129037,
+            "velocityY": 1.0085879342004245,
+            "timestamp": 0.7707170428800918
+        },
+        {
+            "x": 5.455154020778328,
+            "y": 1.5270189169325792,
+            "heading": -0.49098864699750666,
+            "angularVelocity": -1.1022153979010723,
+            "velocityX": -2.420468785044752,
+            "velocityY": 0.8057948212669979,
+            "timestamp": 0.8477887471681009
+        },
+        {
+            "x": 5.315462796839212,
+            "y": 1.573537321073397,
+            "heading": -0.5623285861822422,
+            "angularVelocity": -0.9256307466375128,
+            "velocityX": -1.8124839100106522,
+            "velocityY": 0.6035730566821683,
+            "timestamp": 0.9248604514561101
+        },
+        {
+            "x": 5.222455052636063,
+            "y": 1.6045200564296747,
+            "heading": -0.613275527154411,
+            "angularVelocity": -0.6610330139033284,
+            "velocityX": -1.2067690089684353,
+            "velocityY": 0.40199883527290925,
+            "timestamp": 1.0019321557441192
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -0.34674817551357506,
+            "velocityX": -0.602751075316368,
+            "velocityY": 0.2008511906325378,
+            "timestamp": 1.0790038600321283
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 8.760742141155177e-29,
+            "velocityX": 3.313743560366677e-30,
+            "velocityY": 1.2206105198294578e-30,
+            "timestamp": 1.1560755643201375
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C5 To W1.traj b/src/main/deploy/choreo/C5 To W1.traj
new file mode 100644
index 00000000..bec33f37
--- /dev/null
+++ b/src/main/deploy/choreo/C5 To W1.traj	
@@ -0,0 +1,310 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": -3.516607019090483e-32,
+            "velocityX": -4.092268936373955e-33,
+            "velocityY": -4.396806616029776e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6580594556627535,
+            "y": 0.8642766421574168,
+            "heading": -0.06977499787943942,
+            "angularVelocity": -0.7009716360726537,
+            "velocityX": -0.22041848409624198,
+            "velocityY": 0.7763344454252913,
+            "timestamp": 0.09954040119279155
+        },
+        {
+            "x": 7.613877684624128,
+            "y": 1.0194223152597515,
+            "heading": -0.19807907998906352,
+            "angularVelocity": -1.2889648883484262,
+            "velocityX": -0.4438576749661031,
+            "velocityY": 1.558620130552277,
+            "timestamp": 0.1990808023855831
+        },
+        {
+            "x": 7.54706531781781,
+            "y": 1.253315507091588,
+            "heading": -0.3641253773176819,
+            "angularVelocity": -1.6681296773861405,
+            "velocityX": -0.6712085344815377,
+            "velocityY": 2.349731255139591,
+            "timestamp": 0.2986212035783746
+        },
+        {
+            "x": 7.457648537893544,
+            "y": 1.5658706931334108,
+            "heading": -0.49008580394214096,
+            "angularVelocity": -1.265420122031624,
+            "velocityX": -0.8982963585919527,
+            "velocityY": 3.139983185686188,
+            "timestamp": 0.3981616047711662
+        },
+        {
+            "x": 7.353528135913809,
+            "y": 1.9277448806018234,
+            "heading": -0.490085841920944,
+            "angularVelocity": -3.8154159059291386e-7,
+            "velocityX": -1.0460114760646044,
+            "velocityY": 3.6354503611807685,
+            "timestamp": 0.49770200596395775
+        },
+        {
+            "x": 7.249407681423134,
+            "y": 2.2896190529615703,
+            "heading": -0.4900858798995876,
+            "angularVelocity": -3.8153998933914064e-7,
+            "velocityX": -1.0460120035985523,
+            "velocityY": 3.635450209396512,
+            "timestamp": 0.5972424071567493
+        },
+        {
+            "x": 7.145287117295142,
+            "y": 2.651493193775847,
+            "heading": -0.4900859178782245,
+            "angularVelocity": -3.8153992177326955e-7,
+            "velocityX": -1.0460131050338848,
+            "velocityY": 3.6354498924852887,
+            "timestamp": 0.6967828083495408
+        },
+        {
+            "x": 7.025437006860337,
+            "y": 3.00846649318931,
+            "heading": -0.49008595631381496,
+            "angularVelocity": -3.8613055604326664e-7,
+            "velocityX": -1.204034834083884,
+            "velocityY": 3.586215196401226,
+            "timestamp": 0.7963232095423323
+        },
+        {
+            "x": 6.830624533504276,
+            "y": 3.3307118703307705,
+            "heading": -0.49008600153372534,
+            "angularVelocity": -4.542870012894294e-7,
+            "velocityX": -1.9571196320451347,
+            "velocityY": 3.237332513029861,
+            "timestamp": 0.8958636107351238
+        },
+        {
+            "x": 6.570226686668516,
+            "y": 3.602717269957059,
+            "heading": -0.4900860357215963,
+            "angularVelocity": -3.4345723556551647e-7,
+            "velocityX": -2.6160015804177474,
+            "velocityY": 2.7326130532612987,
+            "timestamp": 0.9954040119279153
+        },
+        {
+            "x": 6.270670526052627,
+            "y": 3.8032215967371097,
+            "heading": -0.4416564414377051,
+            "angularVelocity": 0.4865320382835506,
+            "velocityX": -3.009392739292897,
+            "velocityY": 2.0143009710369753,
+            "timestamp": 1.094944413120707
+        },
+        {
+            "x": 5.9498699270968665,
+            "y": 3.9255658761230467,
+            "heading": -0.3340733830984328,
+            "angularVelocity": 1.0807979177309486,
+            "velocityX": -3.222818022748657,
+            "velocityY": 1.229091684581206,
+            "timestamp": 1.1944848143134985
+        },
+        {
+            "x": 5.637666976812727,
+            "y": 3.9694128449739723,
+            "heading": -0.13726885518093873,
+            "angularVelocity": 1.977132154976145,
+            "velocityX": -3.136444564649281,
+            "velocityY": 0.4404941945733328,
+            "timestamp": 1.2940252155062901
+        },
+        {
+            "x": 5.333615486534618,
+            "y": 3.942111372133816,
+            "heading": 0.08955200005531357,
+            "angularVelocity": 2.278681344642582,
+            "velocityX": -3.0545535946677287,
+            "velocityY": -0.2742752943830166,
+            "timestamp": 1.3935656166990817
+        },
+        {
+            "x": 5.078082684497777,
+            "y": 3.9650753252247246,
+            "heading": 0.19884455131252843,
+            "angularVelocity": 1.0979717777662474,
+            "velocityX": -2.5671265031564414,
+            "velocityY": 0.2306998245509574,
+            "timestamp": 1.4931060178918734
+        },
+        {
+            "x": 4.8543877601623535,
+            "y": 4.064273357391357,
+            "heading": 0.3064368289582943,
+            "angularVelocity": 1.0808905364705061,
+            "velocityX": -2.2472777048805277,
+            "velocityY": 0.9965605018459247,
+            "timestamp": 1.592646419084665
+        },
+        {
+            "x": 4.700013932982481,
+            "y": 4.187105651970831,
+            "heading": 0.3888797273509616,
+            "angularVelocity": 1.0676410431648378,
+            "velocityX": -1.99915137751061,
+            "velocityY": 1.5906864226746236,
+            "timestamp": 1.6698660978526145
+        },
+        {
+            "x": 4.564169886311024,
+            "y": 4.35607150380987,
+            "heading": 0.4692148602751942,
+            "angularVelocity": 1.040345339504005,
+            "velocityX": -1.7591894817340108,
+            "velocityY": 2.1881190719116157,
+            "timestamp": 1.747085776620564
+        },
+        {
+            "x": 4.445019228108457,
+            "y": 4.571847534724939,
+            "heading": 0.5443267073647724,
+            "angularVelocity": 0.9727034389160626,
+            "velocityX": -1.543008985580261,
+            "velocityY": 2.7943140188848803,
+            "timestamp": 1.8243054553885136
+        },
+        {
+            "x": 4.302573716554072,
+            "y": 4.826880596020871,
+            "heading": 0.5443268349443496,
+            "angularVelocity": 0.0000016521640508021742,
+            "velocityX": -1.8446788930894609,
+            "velocityY": 3.302695185541032,
+            "timestamp": 1.9015251341564632
+        },
+        {
+            "x": 4.139811862406642,
+            "y": 5.06945260129317,
+            "heading": 0.544326895208066,
+            "angularVelocity": 7.80419154390702e-7,
+            "velocityX": -2.1077768872432143,
+            "velocityY": 3.141323677365259,
+            "timestamp": 1.9787448129244127
+        },
+        {
+            "x": 3.9382711614032555,
+            "y": 5.28090933180296,
+            "heading": 0.5443268879285122,
+            "angularVelocity": -9.42707078520697e-8,
+            "velocityX": -2.60996554529876,
+            "velocityY": 2.738378790013248,
+            "timestamp": 2.0559644916923623
+        },
+        {
+            "x": 3.721249786731846,
+            "y": 5.476445328956613,
+            "heading": 0.5443268807879367,
+            "angularVelocity": -9.247093001843253e-8,
+            "velocityX": -2.810441303745562,
+            "velocityY": 2.53220422919983,
+            "timestamp": 2.1331841704603116
+        },
+        {
+            "x": 3.5042282470745802,
+            "y": 5.671981142995692,
+            "heading": 0.5443268736473624,
+            "angularVelocity": -9.247091428352055e-8,
+            "velocityX": -2.810443440323428,
+            "velocityY": 2.53220185785385,
+            "timestamp": 2.210403849228261
+        },
+        {
+            "x": 3.287206691377032,
+            "y": 5.867516939231946,
+            "heading": 0.5443268665067905,
+            "angularVelocity": -9.247088327815795e-8,
+            "velocityX": -2.810443648046155,
+            "velocityY": 2.5322016273060566,
+            "timestamp": 2.2876235279962103
+        },
+        {
+            "x": 3.0701851252220784,
+            "y": 6.063052723861702,
+            "heading": 0.5443268593662193,
+            "angularVelocity": -9.247087442211068e-8,
+            "velocityX": -2.8104437834702534,
+            "velocityY": 2.532201477001163,
+            "timestamp": 2.3648432067641596
+        },
+        {
+            "x": 2.853163562032648,
+            "y": 6.258588511782302,
+            "heading": 0.5443268522252032,
+            "angularVelocity": -9.247663534512907e-8,
+            "velocityX": -2.8104437450665345,
+            "velocityY": 2.5322015196178005,
+            "timestamp": 2.442062885532109
+        },
+        {
+            "x": 2.6687757934039245,
+            "y": 6.424725614600548,
+            "heading": 0.4938774787934042,
+            "angularVelocity": -0.6533227570578579,
+            "velocityX": -2.3878339248576954,
+            "velocityY": 2.1514865830703513,
+            "timestamp": 2.5192825643000583
+        },
+        {
+            "x": 2.521265507868239,
+            "y": 6.557635351096379,
+            "heading": 0.4535239115954702,
+            "angularVelocity": -0.522581391709739,
+            "velocityX": -1.9102680545844264,
+            "velocityY": 1.7211899688838848,
+            "timestamp": 2.5965022430680076
+        },
+        {
+            "x": 2.4106327602038906,
+            "y": 6.6573176734827975,
+            "heading": 0.42326153130587874,
+            "angularVelocity": -0.391899846935805,
+            "velocityX": -1.432701474928527,
+            "velocityY": 1.2908927358526117,
+            "timestamp": 2.673721921835957
+        },
+        {
+            "x": 2.336877585524826,
+            "y": 6.723772559032594,
+            "heading": 0.40308732310971707,
+            "angularVelocity": -0.2612573442164476,
+            "velocityX": -0.9551344405446706,
+            "velocityY": 0.8605952085024652,
+            "timestamp": 2.7509416006039062
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": -0.13063150832354717,
+            "velocityX": -0.4775671967717696,
+            "velocityY": 0.43029758084408004,
+            "timestamp": 2.8281612793718556
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": -5.871044564465464e-32,
+            "velocityX": 0,
+            "velocityY": -1.090928833064942e-33,
+            "timestamp": 2.905380958139805
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C5 To W2.traj b/src/main/deploy/choreo/C5 To W2.traj
new file mode 100644
index 00000000..d94da80a
--- /dev/null
+++ b/src/main/deploy/choreo/C5 To W2.traj	
@@ -0,0 +1,283 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": -2.906139149989869e-32,
+            "velocityX": 1.181772084740038e-33,
+            "velocityY": 2.7502272506846476e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 7.666043555718406,
+            "y": 0.8524996120976488,
+            "heading": 0.004999198621269784,
+            "angularVelocity": 0.05577597491216818,
+            "velocityX": -0.15571181404981524,
+            "velocityY": 0.7307780702234378,
+            "timestamp": 0.08962996396104499
+        },
+        {
+            "x": 7.638131101049187,
+            "y": 0.9835016263770707,
+            "heading": 0.014524734067527434,
+            "angularVelocity": 0.10627623871853438,
+            "velocityX": -0.31141878715192206,
+            "velocityY": 1.4615872693685117,
+            "timestamp": 0.17925992792208997
+        },
+        {
+            "x": 7.596263489657983,
+            "y": 1.180010140566484,
+            "heading": 0.02778512041931341,
+            "angularVelocity": 0.14794590743726296,
+            "velocityX": -0.46711623592083296,
+            "velocityY": 2.1924421867983774,
+            "timestamp": 0.26888989188313495
+        },
+        {
+            "x": 7.540442918712468,
+            "y": 1.4420312351509346,
+            "heading": 0.04318451816280708,
+            "angularVelocity": 0.17181082154832236,
+            "velocityX": -0.6227891709269913,
+            "velocityY": 2.9233649440976053,
+            "timestamp": 0.35851985584417995
+        },
+        {
+            "x": 7.470680035396563,
+            "y": 1.7695653388184471,
+            "heading": 0.05582613302662815,
+            "angularVelocity": 0.1410422843561041,
+            "velocityX": -0.7783433154812639,
+            "velocityY": 3.654292484261909,
+            "timestamp": 0.44814981980522495
+        },
+        {
+            "x": 7.399999368806171,
+            "y": 2.101181376235394,
+            "heading": 0.05582613685023459,
+            "angularVelocity": 4.265991273772723e-8,
+            "velocityX": -0.7885830080341364,
+            "velocityY": 3.6998345504308543,
+            "timestamp": 0.5377797837662699
+        },
+        {
+            "x": 7.329318638814834,
+            "y": 2.4327974001390613,
+            "heading": 0.05582614067384277,
+            "angularVelocity": 4.265993214747779e-8,
+            "velocityX": -0.788583715397405,
+            "velocityY": 3.6998343996634295,
+            "timestamp": 0.6274097477273148
+        },
+        {
+            "x": 7.258637432835217,
+            "y": 2.764413322590103,
+            "heading": 0.05582614449746126,
+            "angularVelocity": 4.266004720083137e-8,
+            "velocityX": -0.7885890259906463,
+            "velocityY": 3.699833267758185,
+            "timestamp": 0.7170397116883598
+        },
+        {
+            "x": 7.145396314308057,
+            "y": 3.084009006822374,
+            "heading": 0.05582614848984762,
+            "angularVelocity": 4.4542987394171874e-8,
+            "velocityX": -1.2634292542656518,
+            "velocityY": 3.5657236721770227,
+            "timestamp": 0.8066696756494047
+        },
+        {
+            "x": 6.971535677728531,
+            "y": 3.375105946924151,
+            "heading": 0.05582615332930866,
+            "angularVelocity": 5.39937854332775e-8,
+            "velocityX": -1.9397601973274063,
+            "velocityY": 3.2477636633692444,
+            "timestamp": 0.8962996396104497
+        },
+        {
+            "x": 6.743840876905014,
+            "y": 3.626342927147427,
+            "heading": 0.0558261606002303,
+            "angularVelocity": 8.112155042111421e-8,
+            "velocityX": -2.5403870621043505,
+            "velocityY": 2.8030467615993793,
+            "timestamp": 0.9859296035714946
+        },
+        {
+            "x": 6.471198532281258,
+            "y": 3.8279144449501397,
+            "heading": 0.0558261762788256,
+            "angularVelocity": 1.7492582391526053e-7,
+            "velocityX": -3.041866052096724,
+            "velocityY": 2.2489300329331785,
+            "timestamp": 1.0755595675325396
+        },
+        {
+            "x": 6.164249513947577,
+            "y": 3.9719534129902763,
+            "heading": 0.05582625958870765,
+            "angularVelocity": 9.294869524458774e-7,
+            "velocityX": -3.4246250335109787,
+            "velocityY": 1.6070403431461682,
+            "timestamp": 1.1651895314935845
+        },
+        {
+            "x": 5.836252922691461,
+            "y": 4.05240893270077,
+            "heading": 0.05954498226559844,
+            "angularVelocity": 0.04148972634315725,
+            "velocityX": -3.6594524505071675,
+            "velocityY": 0.8976408798452788,
+            "timestamp": 1.2548194954546295
+        },
+        {
+            "x": 5.497492188342187,
+            "y": 4.066758597782368,
+            "heading": 0.05954602093201425,
+            "angularVelocity": 0.000011588383726853398,
+            "velocityX": -3.779547813904135,
+            "velocityY": 0.16009897190000646,
+            "timestamp": 1.3444494594156744
+        },
+        {
+            "x": 5.204246867986104,
+            "y": 4.110932074419811,
+            "heading": 0.17401114275805463,
+            "angularVelocity": 1.2770854384789139,
+            "velocityX": -3.2717331057226993,
+            "velocityY": 0.4928427356797903,
+            "timestamp": 1.4340794233767193
+        },
+        {
+            "x": 4.937438011169434,
+            "y": 4.2165327072143555,
+            "heading": 0.3021760231951433,
+            "angularVelocity": 1.4299334148208713,
+            "velocityX": -2.976781926774297,
+            "velocityY": 1.1781844834885962,
+            "timestamp": 1.5237093873377643
+        },
+        {
+            "x": 4.703348327317259,
+            "y": 4.378813716693983,
+            "heading": 0.4253911826920849,
+            "angularVelocity": 1.3960058537628992,
+            "velocityX": -2.6521945051027034,
+            "velocityY": 1.8386149895276152,
+            "timestamp": 1.61197202546698
+        },
+        {
+            "x": 4.500555108729279,
+            "y": 4.597666686435954,
+            "heading": 0.5261840356750895,
+            "angularVelocity": 1.1419651068603307,
+            "velocityX": -2.297611117074157,
+            "velocityY": 2.479565242787912,
+            "timestamp": 1.7002346635961958
+        },
+        {
+            "x": 4.326794639818804,
+            "y": 4.871425359866347,
+            "heading": 0.5588913487111756,
+            "angularVelocity": 0.37056804248478087,
+            "velocityX": -1.968675224233522,
+            "velocityY": 3.1016371052677254,
+            "timestamp": 1.7884973017254115
+        },
+        {
+            "x": 4.0969805858398445,
+            "y": 5.11364296690146,
+            "heading": 0.5588923269288569,
+            "angularVelocity": 0.00001108303243636796,
+            "velocityX": -2.603752378696331,
+            "velocityY": 2.744282429905488,
+            "timestamp": 1.8767599398546273
+        },
+        {
+            "x": 3.8246168380678105,
+            "y": 5.306780372295281,
+            "heading": 0.5588922897097742,
+            "angularVelocity": -4.2168559122096544e-7,
+            "velocityX": -3.0858328455274044,
+            "velocityY": 2.188212470049549,
+            "timestamp": 1.965022577983843
+        },
+        {
+            "x": 3.520011793550478,
+            "y": 5.443527703198532,
+            "heading": 0.5588922240132788,
+            "angularVelocity": -7.44329614921164e-7,
+            "velocityX": -3.451121006278936,
+            "velocityY": 1.549322950250525,
+            "timestamp": 2.053285216113059
+        },
+        {
+            "x": 3.194693864220709,
+            "y": 5.518709582083228,
+            "heading": 0.5588921711301718,
+            "angularVelocity": -5.991562005821763e-7,
+            "velocityX": -3.6857943091787817,
+            "velocityY": 0.851797323060201,
+            "timestamp": 2.1415478542422743
+        },
+        {
+            "x": 2.8828791952129387,
+            "y": 5.532174882793972,
+            "heading": 0.5041280478943673,
+            "angularVelocity": -0.6204677811196886,
+            "velocityX": -3.5328047701370098,
+            "velocityY": 0.15255946339412532,
+            "timestamp": 2.22981049237149
+        },
+        {
+            "x": 2.6283322264519793,
+            "y": 5.543894951157175,
+            "heading": 0.3346872638422791,
+            "angularVelocity": -1.9197339626765784,
+            "velocityX": -2.883971906530878,
+            "velocityY": 0.132786291137652,
+            "timestamp": 2.3180731305007054
+        },
+        {
+            "x": 2.4385492640365714,
+            "y": 5.5521384887253,
+            "heading": 0.1780668377028913,
+            "angularVelocity": -1.7744815865360473,
+            "velocityX": -2.150207227406541,
+            "velocityY": 0.09339781523475092,
+            "timestamp": 2.406335768629921
+        },
+        {
+            "x": 2.31267915340354,
+            "y": 5.55742889143524,
+            "heading": 0.062002293486176877,
+            "angularVelocity": -1.3149906537667377,
+            "velocityX": -1.4260859781775244,
+            "velocityY": 0.0599393222554144,
+            "timestamp": 2.4945984067591365
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 9.107235017738231e-34,
+            "angularVelocity": -0.7024749633633888,
+            "velocityX": -0.7101436659051381,
+            "velocityY": 0.029130202985730554,
+            "timestamp": 2.582861044888352
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": -2.2232006496445675e-32,
+            "velocityX": 0,
+            "velocityY": -2.9660881962078245e-33,
+            "timestamp": 2.6711236830175675
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C5 To W3.traj b/src/main/deploy/choreo/C5 To W3.traj
new file mode 100644
index 00000000..62c8640c
--- /dev/null
+++ b/src/main/deploy/choreo/C5 To W3.traj	
@@ -0,0 +1,292 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": -1.5069359770438582e-30,
+            "velocityX": 5.69693995403372e-32,
+            "velocityY": 3.042278119963715e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 7.627290492953592,
+            "y": 0.8035142159340071,
+            "heading": 0.015175150751654183,
+            "angularVelocity": 0.18617297608634506,
+            "velocityX": -0.6466549133823012,
+            "velocityY": 0.20260100070713255,
+            "timestamp": 0.08151102845675925
+        },
+        {
+            "x": 7.521871327482097,
+            "y": 0.8365426259859824,
+            "heading": 0.045515680376982795,
+            "angularVelocity": 0.3722260680519286,
+            "velocityX": -1.2933116839203953,
+            "velocityY": 0.4052017337690261,
+            "timestamp": 0.1630220569135185
+        },
+        {
+            "x": 7.363742244741542,
+            "y": 0.8860852747673063,
+            "heading": 0.09100430929463568,
+            "angularVelocity": 0.5580671693007231,
+            "velocityX": -1.9399716300284648,
+            "velocityY": 0.6078030141357551,
+            "timestamp": 0.24453308537027776
+        },
+        {
+            "x": 7.152902908699907,
+            "y": 0.95214234591484,
+            "heading": 0.15161661460632753,
+            "angularVelocity": 0.7436086436309897,
+            "velocityX": -2.5866356986709165,
+            "velocityY": 0.8104065474106494,
+            "timestamp": 0.326044113827037
+        },
+        {
+            "x": 6.889352971860368,
+            "y": 1.0347142415767305,
+            "heading": 0.22732297101557483,
+            "angularVelocity": 0.9287866665726976,
+            "velocityX": -3.233304030501219,
+            "velocityY": 1.0130150143510155,
+            "timestamp": 0.4075551422837963
+        },
+        {
+            "x": 6.595109187456524,
+            "y": 1.1269159867790934,
+            "heading": 0.22732298101704992,
+            "angularVelocity": 1.22700882310899e-7,
+            "velocityX": -3.6098647014365244,
+            "velocityY": 1.1311566906713055,
+            "timestamp": 0.4890661707405556
+        },
+        {
+            "x": 6.300865420236581,
+            "y": 1.219117786820698,
+            "heading": 0.2273229910184091,
+            "angularVelocity": 1.2269945998464997e-7,
+            "velocityX": -3.6098644906196586,
+            "velocityY": 1.131157363454394,
+            "timestamp": 0.5705771991973149
+        },
+        {
+            "x": 6.006621672014901,
+            "y": 1.3113196474914972,
+            "heading": 0.2273230010197575,
+            "angularVelocity": 1.2269932759504928e-7,
+            "velocityX": -3.6098642575436695,
+            "velocityY": 1.1311581072702415,
+            "timestamp": 0.6520882276540741
+        },
+        {
+            "x": 5.7123779465456614,
+            "y": 1.4035215807721424,
+            "heading": 0.22732301102110228,
+            "angularVelocity": 1.2269928353992546e-7,
+            "velocityX": -3.6098639784103828,
+            "velocityY": 1.131158998068056,
+            "timestamp": 0.7335992561108334
+        },
+        {
+            "x": 5.418134249112936,
+            "y": 1.4957236035255936,
+            "heading": 0.2273230210224565,
+            "angularVelocity": 1.2269939906393907e-7,
+            "velocityX": -3.60986363445063,
+            "velocityY": 1.1311600957453605,
+            "timestamp": 0.8151102845675927
+        },
+        {
+            "x": 5.123890658648026,
+            "y": 1.5879259676440571,
+            "heading": 0.22732303102385681,
+            "angularVelocity": 1.2269996504792678e-7,
+            "velocityX": -3.609862322139673,
+            "velocityY": 1.1311642837064038,
+            "timestamp": 0.896621313024352
+        },
+        {
+            "x": 4.838467298400658,
+            "y": 1.7046053225590203,
+            "heading": 0.22732304878104165,
+            "angularVelocity": 2.1785008908970575e-7,
+            "velocityX": -3.5016532811726435,
+            "velocityY": 1.4314548242617389,
+            "timestamp": 0.9781323414811113
+        },
+        {
+            "x": 4.5530441892872355,
+            "y": 1.8212852918009321,
+            "heading": 0.2273230665381978,
+            "angularVelocity": 2.1784973753126392e-7,
+            "velocityX": -3.5016502001914627,
+            "velocityY": 1.4314623609958343,
+            "timestamp": 1.0596433699378704
+        },
+        {
+            "x": 4.267621124217833,
+            "y": 1.9379653687834795,
+            "heading": 0.2273230842953322,
+            "angularVelocity": 2.1784947052459537e-7,
+            "velocityX": -3.5016496598471556,
+            "velocityY": 1.4314636827879597,
+            "timestamp": 1.1411543983946297
+        },
+        {
+            "x": 3.9821980933555543,
+            "y": 2.054645529443536,
+            "heading": 0.22732310205245027,
+            "angularVelocity": 2.1784927045604472e-7,
+            "velocityX": -3.5016492401846184,
+            "velocityY": 1.4314647093669521,
+            "timestamp": 1.222665426851389
+        },
+        {
+            "x": 3.6967750938784922,
+            "y": 2.171325766878081,
+            "heading": 0.22732311980956,
+            "angularVelocity": 2.1784916783717796e-7,
+            "velocityX": -3.501648855142053,
+            "velocityY": 1.4314656512577588,
+            "timestamp": 1.3041764553081483
+        },
+        {
+            "x": 3.411352191599502,
+            "y": 2.288006242078097,
+            "heading": 0.2273231375666713,
+            "angularVelocity": 2.178491871140625e-7,
+            "velocityX": -3.501647662689034,
+            "velocityY": 1.4314685682307748,
+            "timestamp": 1.3856874837649076
+        },
+        {
+            "x": 3.135544385824121,
+            "y": 2.425885248705818,
+            "heading": 0.22732315564878777,
+            "angularVelocity": 2.218364413735132e-7,
+            "velocityX": -3.3836869807340535,
+            "velocityY": 1.6915380561155315,
+            "timestamp": 1.4671985122216669
+        },
+        {
+            "x": 2.8888590335845947,
+            "y": 2.6108903884887695,
+            "heading": 0.22732317584620773,
+            "angularVelocity": 2.4778757367040615e-7,
+            "velocityX": -3.026404609412922,
+            "velocityY": 2.2696945834893776,
+            "timestamp": 1.5487095406784261
+        },
+        {
+            "x": 2.714424659127437,
+            "y": 2.7861164379371415,
+            "heading": 0.22732319539367835,
+            "angularVelocity": 2.990800632496739e-7,
+            "velocityX": -2.668879509458687,
+            "velocityY": 2.6809922892290525,
+            "timestamp": 1.6140681954393303
+        },
+        {
+            "x": 2.566977650329029,
+            "y": 2.984587845024715,
+            "heading": 0.2273232142228265,
+            "angularVelocity": 2.880895914276183e-7,
+            "velocityX": -2.2559676195571057,
+            "velocityY": 3.0366507360596735,
+            "timestamp": 1.6794268502002345
+        },
+        {
+            "x": 2.4495779823740462,
+            "y": 3.20218572489824,
+            "heading": 0.22732323403642601,
+            "angularVelocity": 3.0315188694924184e-7,
+            "velocityX": -1.7962375202558922,
+            "velocityY": 3.3292894517113587,
+            "timestamp": 1.7447855049611387
+        },
+        {
+            "x": 2.3646621591221844,
+            "y": 3.4343941523127826,
+            "heading": 0.22732276450933128,
+            "angularVelocity": -0.000007183855179950579,
+            "velocityX": -1.2992284428512828,
+            "velocityY": 3.5528336417572115,
+            "timestamp": 1.810144159722043
+        },
+        {
+            "x": 2.3096843159184894,
+            "y": 3.656196634862642,
+            "heading": 0.16843360905648927,
+            "angularVelocity": -0.9010154151467895,
+            "velocityX": -0.8411715847703609,
+            "velocityY": 3.3936206820850954,
+            "timestamp": 1.875502814482947
+        },
+        {
+            "x": 2.277656929328602,
+            "y": 3.853541817964372,
+            "heading": 0.0672675417750508,
+            "angularVelocity": -1.547860304829062,
+            "velocityX": -0.49002518039951914,
+            "velocityY": 3.0194192922676324,
+            "timestamp": 1.9408614692438513
+        },
+        {
+            "x": 2.265357919525011,
+            "y": 4.021569174077037,
+            "heading": -0.049806740829473055,
+            "angularVelocity": -1.791259061753831,
+            "velocityX": -0.188177217670429,
+            "velocityY": 2.570850895376378,
+            "timestamp": 2.0062201240047557
+        },
+        {
+            "x": 2.269649055536998,
+            "y": 4.158080976089645,
+            "heading": -0.1685192187929243,
+            "angularVelocity": -1.8163237661135343,
+            "velocityX": 0.06565520706762865,
+            "velocityY": 2.088656850603768,
+            "timestamp": 2.07157877876566
+        },
+        {
+            "x": 2.2810300353091644,
+            "y": 4.259951613943982,
+            "heading": -0.2719091294332062,
+            "angularVelocity": -1.581885536330179,
+            "velocityX": 0.1741311814602323,
+            "velocityY": 1.5586403702310236,
+            "timestamp": 2.1369374335265645
+        },
+        {
+            "x": 2.2926691286250658,
+            "y": 4.326989538481201,
+            "heading": -0.34849979264864867,
+            "angularVelocity": -1.1718518916219969,
+            "velocityX": 0.1780803683686499,
+            "velocityY": 1.0256931508529106,
+            "timestamp": 2.202296088287469
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.6349611616573703,
+            "velocityX": 0.11216374329844404,
+            "velocityY": 0.5050664160631704,
+            "timestamp": 2.2676547430483733
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -1.761626207104486e-30,
+            "velocityX": 0,
+            "velocityY": -1.839087679439853e-31,
+            "timestamp": 2.3330133978092777
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Center To C1.traj b/src/main/deploy/choreo/Center To C1.traj
new file mode 100644
index 00000000..1fe0cdb8
--- /dev/null
+++ b/src/main/deploy/choreo/Center To C1.traj	
@@ -0,0 +1,220 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -2.1086675817598662e-32,
+            "velocityY": -7.052966104933725e-38,
+            "timestamp": 0
+        },
+        {
+            "x": 1.4233949672121562,
+            "y": 5.582111248416047,
+            "heading": 6.989955400155978e-19,
+            "angularVelocity": 7.290266672073875e-18,
+            "velocityX": 0.7654825584964474,
+            "velocityY": 0.23061220206204183,
+            "timestamp": 0.09588065253415788
+        },
+        {
+            "x": 1.5701848986828006,
+            "y": 5.626333744358309,
+            "heading": -5.64955528358279e-19,
+            "angularVelocity": -1.3182545571253806e-17,
+            "velocityX": 1.530965086187225,
+            "velocityY": 0.4612243948434498,
+            "timestamp": 0.19176130506831576
+        },
+        {
+            "x": 1.7903697885046006,
+            "y": 5.69266748604712,
+            "heading": -1.1328157855510893e-18,
+            "angularVelocity": -5.922574643212496e-18,
+            "velocityX": 2.2964475522666925,
+            "velocityY": 0.6918365690635988,
+            "timestamp": 0.28764195760247363
+        },
+        {
+            "x": 2.08394961895559,
+            "y": 5.781112468143492,
+            "heading": -1.2347885191323161e-17,
+            "angularVelocity": -1.1696905584501664e-16,
+            "velocityX": 3.061929833512548,
+            "velocityY": 0.9224486876000673,
+            "timestamp": 0.3835226101366315
+        },
+        {
+            "x": 2.4312425305333245,
+            "y": 5.885739258408696,
+            "heading": -1.8571615119787707e-17,
+            "angularVelocity": -6.491121789400923e-17,
+            "velocityX": 3.6221375470302584,
+            "velocityY": 1.0912190050847865,
+            "timestamp": 0.4794032626707894
+        },
+        {
+            "x": 2.7785354421111035,
+            "y": 5.990366048673914,
+            "heading": -2.5105020741934312e-17,
+            "angularVelocity": -6.814102166319416e-17,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849244,
+            "timestamp": 0.5752839152049473
+        },
+        {
+            "x": 3.125828353688883,
+            "y": 6.094992838939131,
+            "heading": -3.974939977960392e-17,
+            "angularVelocity": -1.527354961675144e-16,
+            "velocityX": 3.622137547030716,
+            "velocityY": 1.0912190050849244,
+            "timestamp": 0.6711645677391052
+        },
+        {
+            "x": 3.473121265266662,
+            "y": 6.199619629204348,
+            "heading": -4.6160884572564577e-17,
+            "angularVelocity": -6.68694321901595e-17,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849242,
+            "timestamp": 0.767045220273263
+        },
+        {
+            "x": 3.8204141768444413,
+            "y": 6.304246419469565,
+            "heading": -5.207468497060529e-17,
+            "angularVelocity": -6.167876669314405e-17,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849244,
+            "timestamp": 0.862925872807421
+        },
+        {
+            "x": 4.16770708842222,
+            "y": 6.408873209734782,
+            "heading": -3.0597254964935646e-17,
+            "angularVelocity": 2.24001708769928e-16,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849242,
+            "timestamp": 0.9588065253415788
+        },
+        {
+            "x": 4.515,
+            "y": 6.5135,
+            "heading": -2.0783137065340813e-18,
+            "angularVelocity": 2.974420855995074e-16,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849244,
+            "timestamp": 1.0546871778757367
+        },
+        {
+            "x": 4.862292911577779,
+            "y": 6.618126790265217,
+            "heading": -2.7789619178473824e-17,
+            "angularVelocity": -2.681594752696605e-16,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849244,
+            "timestamp": 1.1505678304098945
+        },
+        {
+            "x": 5.209585823155558,
+            "y": 6.722753580530434,
+            "heading": 2.2712942266126744e-18,
+            "angularVelocity": 3.135242889004865e-16,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849242,
+            "timestamp": 1.2464484829440523
+        },
+        {
+            "x": 5.5568787347333375,
+            "y": 6.827380370795651,
+            "heading": 3.7041174502192387e-17,
+            "angularVelocity": 3.6263708429820527e-16,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849244,
+            "timestamp": 1.34232913547821
+        },
+        {
+            "x": 5.9041716463111165,
+            "y": 6.932007161060868,
+            "heading": 6.494346998398645e-17,
+            "angularVelocity": 2.910106965725311e-16,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849244,
+            "timestamp": 1.4382097880123679
+        },
+        {
+            "x": 6.251464557888895,
+            "y": 7.036633951326086,
+            "heading": 5.759102638783417e-17,
+            "angularVelocity": -7.668328700140211e-17,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849244,
+            "timestamp": 1.5340904405465257
+        },
+        {
+            "x": 6.598757469466675,
+            "y": 7.141260741591303,
+            "heading": 2.5440135069531877e-17,
+            "angularVelocity": -3.353219911269207e-16,
+            "velocityX": 3.6221375470307158,
+            "velocityY": 1.0912190050849244,
+            "timestamp": 1.6299710930806834
+        },
+        {
+            "x": 6.94605038104441,
+            "y": 7.245887531856507,
+            "heading": 2.2711688502269004e-17,
+            "angularVelocity": -2.84566958514003e-17,
+            "velocityX": 3.6221375470302584,
+            "velocityY": 1.0912190050847868,
+            "timestamp": 1.7258517456148412
+        },
+        {
+            "x": 7.239630211495399,
+            "y": 7.334332513952879,
+            "heading": 1.1340524791180054e-17,
+            "angularVelocity": -1.1859706178712186e-16,
+            "velocityX": 3.061929833512548,
+            "velocityY": 0.9224486876000674,
+            "timestamp": 1.821732398148999
+        },
+        {
+            "x": 7.4598151013172,
+            "y": 7.40066625564169,
+            "heading": 5.896173994762203e-18,
+            "angularVelocity": -5.67825796795303e-17,
+            "velocityX": 2.2964475522666925,
+            "velocityY": 0.6918365690635988,
+            "timestamp": 1.9176130506831568
+        },
+        {
+            "x": 7.606605032787844,
+            "y": 7.444888751583952,
+            "heading": 1.9652682638036787e-18,
+            "angularVelocity": -4.099790316709085e-17,
+            "velocityX": 1.530965086187225,
+            "velocityY": 0.46122439484344985,
+            "timestamp": 2.0134937032173146
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": -2.0497026255693862e-17,
+            "velocityX": 0.7654825584964475,
+            "velocityY": 0.23061220206204183,
+            "timestamp": 2.1093743557514726
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 2.8084467268312453e-34,
+            "velocityY": 0,
+            "timestamp": 2.2052550082856306
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Center To C2.traj b/src/main/deploy/choreo/Center To C2.traj
new file mode 100644
index 00000000..a76ce3f5
--- /dev/null
+++ b/src/main/deploy/choreo/Center To C2.traj	
@@ -0,0 +1,211 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 4.006684802425094e-34,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 1.4307759237486215,
+            "y": 5.581186236587336,
+            "heading": -0.002104963011526108,
+            "angularVelocity": -0.021033383553861286,
+            "velocityX": 0.8071357913745165,
+            "velocityY": 0.21169884590096585,
+            "timestamp": 0.1000772417873624
+        },
+        {
+            "x": 1.592327768448065,
+            "y": 5.623558708255768,
+            "heading": -0.006314883423264977,
+            "angularVelocity": -0.04206671103789844,
+            "velocityX": 1.6142715547926287,
+            "velocityY": 0.42339767675114676,
+            "timestamp": 0.2001544835747248
+        },
+        {
+            "x": 1.8346555283201378,
+            "y": 5.6871174126709505,
+            "heading": -0.012629752552513055,
+            "angularVelocity": -0.06309995176191506,
+            "velocityX": 2.4214072604734147,
+            "velocityY": 0.6350964842758995,
+            "timestamp": 0.3002317253620872
+        },
+        {
+            "x": 2.1577591856288034,
+            "y": 5.77186234429474,
+            "heading": -0.02104955396970995,
+            "angularVelocity": -0.08413302831713466,
+            "velocityX": 3.228542788930726,
+            "velocityY": 0.8467952364619561,
+            "timestamp": 0.4003089671494496
+        },
+        {
+            "x": 2.5239589562380984,
+            "y": 5.867910684343787,
+            "heading": -0.021049554851793276,
+            "angularVelocity": -8.814025154574047e-9,
+            "velocityX": 3.6591712967806616,
+            "velocityY": 0.9597420785549242,
+            "timestamp": 0.500386208936812
+        },
+        {
+            "x": 2.8901587297210503,
+            "y": 5.963959013436644,
+            "heading": -0.021049555733868682,
+            "angularVelocity": -8.813945991956408e-9,
+            "velocityX": 3.659171325495056,
+            "velocityY": 0.9597419690775807,
+            "timestamp": 0.6004634507241744
+        },
+        {
+            "x": 3.2563585069426413,
+            "y": 6.060007328275347,
+            "heading": -0.02104955661593894,
+            "angularVelocity": -8.813894597525165e-9,
+            "velocityX": 3.659171362852588,
+            "velocityY": 0.9597418266460623,
+            "timestamp": 0.7005406925115368
+        },
+        {
+            "x": 3.622558289188601,
+            "y": 6.15605562395784,
+            "heading": -0.021049557498003604,
+            "angularVelocity": -8.813838677794314e-9,
+            "velocityX": 3.6591714130574977,
+            "velocityY": 0.9597416352318217,
+            "timestamp": 0.8006179342988993
+        },
+        {
+            "x": 3.9887580784892815,
+            "y": 6.252103892743073,
+            "heading": -0.02104955838006166,
+            "angularVelocity": -8.81377261667878e-9,
+            "velocityX": 3.659171483550255,
+            "velocityY": 0.9597413664668089,
+            "timestamp": 0.9006951760862617
+        },
+        {
+            "x": 4.354957878489299,
+            "y": 6.3481521207353335,
+            "heading": -0.021049559262112345,
+            "angularVelocity": -8.813699036774153e-9,
+            "velocityX": 3.6591715904610522,
+            "velocityY": 0.9597409588519333,
+            "timestamp": 1.0007724178736241
+        },
+        {
+            "x": 4.721157716449094,
+            "y": 6.444200203999558,
+            "heading": -0.021049560144156942,
+            "angularVelocity": -8.81363814954091e-9,
+            "velocityX": 3.659171969765827,
+            "velocityY": 0.9597395126886197,
+            "timestamp": 1.1008496596609865
+        },
+        {
+            "x": 5.092065894328832,
+            "y": 6.520059807846628,
+            "heading": -0.02104956519242719,
+            "angularVelocity": -5.0443738888274115e-8,
+            "velocityX": 3.70621902897584,
+            "velocityY": 0.7580105375830706,
+            "timestamp": 1.2009269014483488
+        },
+        {
+            "x": 5.47058189966855,
+            "y": 6.512767035208857,
+            "heading": -0.021049620221549888,
+            "angularVelocity": -5.498665002090286e-7,
+            "velocityX": 3.7822385847120277,
+            "velocityY": -0.07287143917560433,
+            "timestamp": 1.3010041432357111
+        },
+        {
+            "x": 5.849069453983326,
+            "y": 6.50412284194313,
+            "heading": -0.02104967520004647,
+            "angularVelocity": -5.493606298291521e-7,
+            "velocityX": 3.7819542940538056,
+            "velocityY": -0.08637521489745259,
+            "timestamp": 1.4010813850230734
+        },
+        {
+            "x": 6.2213754396920145,
+            "y": 6.435451104158958,
+            "heading": -0.021049699334872447,
+            "angularVelocity": -2.4116198195152956e-7,
+            "velocityX": 3.720186318681132,
+            "velocityY": -0.6861873544644725,
+            "timestamp": 1.5011586268104358
+        },
+        {
+            "x": 6.5695694546238315,
+            "y": 6.286829159405189,
+            "heading": -0.02104969841623417,
+            "angularVelocity": 9.179292491243133e-9,
+            "velocityX": 3.4792527123362977,
+            "velocityY": -1.4850723511100814,
+            "timestamp": 1.601235868597798
+        },
+        {
+            "x": 6.915952468622566,
+            "y": 6.134033995374984,
+            "heading": -0.021049697500180466,
+            "angularVelocity": 9.153466759383175e-9,
+            "velocityX": 3.4611566807037675,
+            "velocityY": -1.5267723340622583,
+            "timestamp": 1.7013131103851604
+        },
+        {
+            "x": 7.221571466882516,
+            "y": 5.999220411113471,
+            "heading": -0.012629808861323527,
+            "angularVelocity": 0.0841338998605394,
+            "velocityX": 3.053831148837094,
+            "velocityY": -1.347095322110863,
+            "timestamp": 1.8013903521725227
+        },
+        {
+            "x": 7.450785730315982,
+            "y": 5.898110210460681,
+            "heading": -0.0063148981970481905,
+            "angularVelocity": 0.0631003667916113,
+            "velocityX": 2.290373508899117,
+            "velocityY": -1.0103216160535444,
+            "timestamp": 1.901467593959885
+        },
+        {
+            "x": 7.603595242863294,
+            "y": 5.830703404850431,
+            "heading": -0.0021049639599730564,
+            "angularVelocity": 0.04206684918455414,
+            "velocityX": 1.5269157084883698,
+            "velocityY": -0.673547795746317,
+            "timestamp": 2.0015448357472474
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 0.02103339303101045,
+            "velocityX": 0.7634578628680123,
+            "velocityY": -0.33677391831044023,
+            "timestamp": 2.1016220775346097
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 1.7450911582816108e-33,
+            "velocityY": 0,
+            "timestamp": 2.201699319321972
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Center To C3.traj b/src/main/deploy/choreo/Center To C3.traj
new file mode 100644
index 00000000..4db64f81
--- /dev/null
+++ b/src/main/deploy/choreo/Center To C3.traj	
@@ -0,0 +1,211 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 2.6743025901632004e-33,
+            "velocityX": 2.068786142168461e-33,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 1.4007220754094205,
+            "y": 5.545584664540576,
+            "heading": -0.37232774466736046,
+            "angularVelocity": -3.437582019103784,
+            "velocityX": 0.4683005682394807,
+            "velocityY": -0.13309214444637307,
+            "timestamp": 0.10831094141120465
+        },
+        {
+            "x": 1.5327694511201624,
+            "y": 5.533363500300387,
+            "heading": -0.9362708794209632,
+            "angularVelocity": -5.206705134364786,
+            "velocityX": 1.2191508446909478,
+            "velocityY": -0.11283406903270367,
+            "timestamp": 0.2166218828224093
+        },
+        {
+            "x": 1.760106911270163,
+            "y": 5.516367699163042,
+            "heading": -1.4218044496098983,
+            "angularVelocity": -4.482774905866594,
+            "velocityX": 2.0989334705060814,
+            "velocityY": -0.1569167520465022,
+            "timestamp": 0.3249328242336139
+        },
+        {
+            "x": 2.067387230758427,
+            "y": 5.467280597023701,
+            "heading": -1.7292049865361503,
+            "angularVelocity": -2.838130044121764,
+            "velocityX": 2.83702011527781,
+            "velocityY": -0.453205387191498,
+            "timestamp": 0.4332437656448186
+        },
+        {
+            "x": 2.4488593918735733,
+            "y": 5.393167697716653,
+            "heading": -1.7983473850598053,
+            "angularVelocity": -0.6383694723984937,
+            "velocityX": 3.5220094677866336,
+            "velocityY": -0.6842605035226905,
+            "timestamp": 0.5415547070560233
+        },
+        {
+            "x": 2.8531804118778052,
+            "y": 5.326790341993422,
+            "heading": -1.7983490370676205,
+            "angularVelocity": -0.000015252455509683049,
+            "velocityX": 3.7329656148885193,
+            "velocityY": -0.6128407237384188,
+            "timestamp": 0.6498656484672279
+        },
+        {
+            "x": 3.257501404598323,
+            "y": 5.2604128200678275,
+            "heading": -1.7983506890680847,
+            "angularVelocity": -0.00001525238764202395,
+            "velocityX": 3.732965362986781,
+            "velocityY": -0.6128422582312392,
+            "timestamp": 0.7581765898784326
+        },
+        {
+            "x": 3.6466232677858144,
+            "y": 5.172310745117625,
+            "heading": -1.8337945854134767,
+            "angularVelocity": -0.32724206699329256,
+            "velocityX": 3.5926367005728674,
+            "velocityY": -0.8134180517896293,
+            "timestamp": 0.8664875312896373
+        },
+        {
+            "x": 4.014238623942389,
+            "y": 5.062846847355338,
+            "heading": -1.917513965365452,
+            "angularVelocity": -0.7729540419571551,
+            "velocityX": 3.394074055370967,
+            "velocityY": -1.0106448742487064,
+            "timestamp": 0.974798472700842
+        },
+        {
+            "x": 4.354761888880782,
+            "y": 4.880183720994759,
+            "heading": -1.9914398534549198,
+            "angularVelocity": -0.6825338892476871,
+            "velocityX": 3.143941512294591,
+            "velocityY": -1.6864697507068689,
+            "timestamp": 1.0831094141120465
+        },
+        {
+            "x": 4.661144802240214,
+            "y": 4.608133464126214,
+            "heading": -1.9914398486412812,
+            "angularVelocity": 4.444277523534959e-8,
+            "velocityX": 2.8287346538355926,
+            "velocityY": -2.5117523061285136,
+            "timestamp": 1.1914203555232512
+        },
+        {
+            "x": 5.019380877625839,
+            "y": 4.418922250546549,
+            "heading": -1.9767149939198712,
+            "angularVelocity": 0.13594983599585594,
+            "velocityX": 3.3074781801182462,
+            "velocityY": -1.7469261287400477,
+            "timestamp": 1.2997312969344559
+        },
+        {
+            "x": 5.376950498025801,
+            "y": 4.3069722509438675,
+            "heading": -1.8614424797513431,
+            "angularVelocity": 1.0642739566900585,
+            "velocityX": 3.3013250161167162,
+            "velocityY": -1.0335982509621156,
+            "timestamp": 1.4080422383456606
+        },
+        {
+            "x": 5.749629435463048,
+            "y": 4.240661073830716,
+            "heading": -1.7582912287295662,
+            "angularVelocity": 0.9523622422425545,
+            "velocityX": 3.4408244687151703,
+            "velocityY": -0.6122297179691145,
+            "timestamp": 1.5163531797568652
+        },
+        {
+            "x": 6.158173088993434,
+            "y": 4.209455444526307,
+            "heading": -1.7582906674190417,
+            "angularVelocity": 0.000005182399092709644,
+            "velocityX": 3.771951828756997,
+            "velocityY": -0.2881115139230142,
+            "timestamp": 1.62466412116807
+        },
+        {
+            "x": 6.566716775041419,
+            "y": 4.178250240929697,
+            "heading": -1.7582901061123783,
+            "angularVelocity": 0.000005182363444203767,
+            "velocityX": 3.771952128981517,
+            "velocityY": -0.2881075835001592,
+            "timestamp": 1.7329750625792746
+        },
+        {
+            "x": 6.948239527399369,
+            "y": 4.140315578246664,
+            "heading": -1.6699172621402418,
+            "angularVelocity": 0.8159179748667066,
+            "velocityX": 3.5224765604196056,
+            "velocityY": -0.35023850950582514,
+            "timestamp": 1.8412860039904793
+        },
+        {
+            "x": 7.2588942750640015,
+            "y": 4.14084991195257,
+            "heading": -1.390296709754989,
+            "angularVelocity": 2.5816464037891405,
+            "velocityX": 2.86817512263351,
+            "velocityY": 0.004933330824608595,
+            "timestamp": 1.949596945401684
+        },
+        {
+            "x": 7.4846066646178775,
+            "y": 4.133171401549216,
+            "heading": -0.9384850139510863,
+            "angularVelocity": 4.17143171241205,
+            "velocityX": 2.083929717653862,
+            "velocityY": -0.07089321081793584,
+            "timestamp": 2.0579078868128886
+        },
+        {
+            "x": 7.620450068903259,
+            "y": 4.128683952785512,
+            "heading": -0.34333254233125515,
+            "angularVelocity": 5.494850879010672,
+            "velocityX": 1.2541983525897882,
+            "velocityY": -0.04143116757398245,
+            "timestamp": 2.166218828224093
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 9.09513036114211e-34,
+            "angularVelocity": 3.1698786646843584,
+            "velocityX": 0.5498053134877472,
+            "velocityY": -0.015547393121808115,
+            "timestamp": 2.2745297696352975
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": -7.032634787663221e-33,
+            "velocityX": 0,
+            "velocityY": -1.950631326224301e-32,
+            "timestamp": 2.382840711046502
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Center To C4.traj b/src/main/deploy/choreo/Center To C4.traj
new file mode 100644
index 00000000..1f520266
--- /dev/null
+++ b/src/main/deploy/choreo/Center To C4.traj	
@@ -0,0 +1,1093 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": -1.9296664556128306e-33,
+            "angularVelocity": 0,
+            "velocityX": -7.813211441980168e-34,
+            "velocityY": -2.2407271587906947e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 1.35439418502164,
+            "y": 5.559271848784049,
+            "heading": -0.0037613483747314606,
+            "angularVelocity": -0.16062668985124567,
+            "velocityX": 0.18765169410035007,
+            "velocityY": -0.031095370031394967,
+            "timestamp": 0.023416708507251784
+        },
+        {
+            "x": 1.3631871179840578,
+            "y": 5.557813607446842,
+            "heading": -0.011205273313316288,
+            "angularVelocity": -0.317889464963847,
+            "velocityX": 0.37549824560930783,
+            "velocityY": -0.06227354014150002,
+            "timestamp": 0.04683341701450357
+        },
+        {
+            "x": 1.3763837173940232,
+            "y": 5.5556231238892515,
+            "heading": -0.02224504855076525,
+            "angularVelocity": -0.47144863395424863,
+            "velocityX": 0.563554839736692,
+            "velocityY": -0.09354361467639355,
+            "timestamp": 0.07025012552175536
+        },
+        {
+            "x": 1.3939893513502628,
+            "y": 5.552698044632534,
+            "heading": -0.03678367986427699,
+            "angularVelocity": -0.6208657083043512,
+            "velocityX": 0.7518406761047259,
+            "velocityY": -0.12491419346204678,
+            "timestamp": 0.09366683402900713
+        },
+        {
+            "x": 1.4160099408355828,
+            "y": 5.549035829570074,
+            "heading": -0.0547111148835232,
+            "angularVelocity": -0.7655830456997818,
+            "velocityX": 0.9403793653792271,
+            "velocityY": -0.15639324635770782,
+            "timestamp": 0.11708354253625891
+        },
+        {
+            "x": 1.4424520698548373,
+            "y": 5.544633771060564,
+            "heading": -0.07590088077413953,
+            "angularVelocity": -0.9048994176125892,
+            "velocityX": 1.129199221618411,
+            "velocityY": -0.18798792785714832,
+            "timestamp": 0.1405002510435107
+        },
+        {
+            "x": 1.4733230980825243,
+            "y": 5.539489016814175,
+            "heading": -0.10020600379509263,
+            "angularVelocity": -1.0379393420482537,
+            "velocityX": 1.3183333694454236,
+            "velocityY": -0.2197044151102235,
+            "timestamp": 0.16391695955076246
+        },
+        {
+            "x": 1.5086312686123933,
+            "y": 5.533598592872994,
+            "heading": -0.12745397379473564,
+            "angularVelocity": -1.163612298082128,
+            "velocityX": 1.5078195348818708,
+            "velocityY": -0.25154790389774145,
+            "timestamp": 0.18733366805801424
+        },
+        {
+            "x": 1.5483857981367395,
+            "y": 5.526959418437362,
+            "heading": -0.15744034724886685,
+            "angularVelocity": -1.2805545854082991,
+            "velocityX": 1.6976992950155392,
+            "velocityY": -0.28352295684851286,
+            "timestamp": 0.21075037656526602
+        },
+        {
+            "x": 1.5925969268012463,
+            "y": 5.5195682985391485,
+            "heading": -0.18992026053497643,
+            "angularVelocity": -1.3870400819163256,
+            "velocityX": 1.888016355962865,
+            "velocityY": -0.31563444947560276,
+            "timestamp": 0.2341670850725178
+        },
+        {
+            "x": 1.6412758830317984,
+            "y": 5.511421875068327,
+            "heading": -0.22459650029504458,
+            "angularVelocity": -1.4808332157070316,
+            "velocityX": 2.0788129217851563,
+            "velocityY": -0.34788934868040844,
+            "timestamp": 0.25758379357976957
+        },
+        {
+            "x": 1.6944346649736404,
+            "y": 5.502516516620167,
+            "heading": -0.26110152083126115,
+            "angularVelocity": -1.5589304758570732,
+            "velocityX": 2.2701218630013136,
+            "velocityY": -0.38029932539011047,
+            "timestamp": 0.2810005020870214
+        },
+        {
+            "x": 1.7520853949904163,
+            "y": 5.492848149517658,
+            "heading": -0.298968117565775,
+            "angularVelocity": -1.6170759747377448,
+            "velocityX": 2.4619484843022064,
+            "velocityY": -0.4128832666435571,
+            "timestamp": 0.3044172105942732
+        },
+        {
+            "x": 1.8142385663960479,
+            "y": 5.482412126535505,
+            "heading": -0.337577206769387,
+            "angularVelocity": -1.6487837815317001,
+            "velocityX": 2.654223217852448,
+            "velocityY": -0.44566566556215986,
+            "timestamp": 0.327833919101525
+        },
+        {
+            "x": 1.8808979849725118,
+            "y": 5.471203531590798,
+            "heading": -0.3760544549024592,
+            "angularVelocity": -1.6431535679387552,
+            "velocityX": 2.8466604756095246,
+            "velocityY": -0.47865800358925525,
+            "timestamp": 0.3512506276087768
+        },
+        {
+            "x": 1.9520436894250033,
+            "y": 5.459218834920382,
+            "heading": -0.41303307424029884,
+            "angularVelocity": -1.5791552995754587,
+            "velocityX": 3.0382452952539354,
+            "velocityY": -0.5118010785637118,
+            "timestamp": 0.3746673361160286
+        },
+        {
+            "x": 2.027554046757936,
+            "y": 5.446452385802885,
+            "heading": -0.44595627669268334,
+            "angularVelocity": -1.4059705462956014,
+            "velocityX": 3.2246358325529645,
+            "velocityY": -0.5451854650516427,
+            "timestamp": 0.3980840446232804
+        },
+        {
+            "x": 2.106704107988529,
+            "y": 5.4327632732947455,
+            "heading": -0.46854805770485686,
+            "angularVelocity": -0.9647718425148408,
+            "velocityX": 3.3800677497471567,
+            "velocityY": -0.5845873899786794,
+            "timestamp": 0.4215007531305322
+        },
+        {
+            "x": 2.1895060473348913,
+            "y": 5.418170740153337,
+            "heading": -0.4808901513114338,
+            "angularVelocity": -0.5270635539044761,
+            "velocityX": 3.5360195614476866,
+            "velocityY": -0.6231675616108724,
+            "timestamp": 0.444917461637784
+        },
+        {
+            "x": 2.275961949822614,
+            "y": 5.402702256569038,
+            "heading": -0.4829709400280361,
+            "angularVelocity": -0.0888591458512548,
+            "velocityX": 3.6920604132278863,
+            "velocityY": -0.6605746311236733,
+            "timestamp": 0.4683341701450358
+        },
+        {
+            "x": 2.3633379318433296,
+            "y": 5.388122823546896,
+            "heading": -0.48297098226531077,
+            "angularVelocity": -0.0000018037238101032338,
+            "velocityX": 3.7313519956767345,
+            "velocityY": -0.6226081269122786,
+            "timestamp": 0.4917508786522876
+        },
+        {
+            "x": 2.4507217300981603,
+            "y": 5.373590311340196,
+            "heading": -0.48297102439627604,
+            "angularVelocity": -0.0000017991839149984119,
+            "velocityX": 3.7316857844376363,
+            "velocityY": -0.6206043946012527,
+            "timestamp": 0.5151675871595394
+        },
+        {
+            "x": 2.5381084152434026,
+            "y": 5.359075168632344,
+            "heading": -0.4829710665257657,
+            "angularVelocity": -0.0000017991208983915868,
+            "velocityX": 3.7318090677936033,
+            "velocityY": -0.6198626379688947,
+            "timestamp": 0.5385842956667911
+        },
+        {
+            "x": 2.6254950830921073,
+            "y": 5.344559921791865,
+            "heading": -0.482971108655264,
+            "angularVelocity": -0.0000017991212673817243,
+            "velocityX": 3.7318083291527238,
+            "velocityY": -0.6198670849061145,
+            "timestamp": 0.5620010041740429
+        },
+        {
+            "x": 2.712881673840345,
+            "y": 5.330044210788612,
+            "heading": -0.4829711507848006,
+            "angularVelocity": -0.0000017991229009417428,
+            "velocityX": 3.731805036611997,
+            "velocityY": -0.6198869067684731,
+            "timestamp": 0.5854177126812946
+        },
+        {
+            "x": 2.800268245782709,
+            "y": 5.315528386572539,
+            "heading": -0.4829711929143463,
+            "angularVelocity": -0.0000017991232935293638,
+            "velocityX": 3.7318042335155903,
+            "velocityY": -0.6198917414709255,
+            "timestamp": 0.6088344211885464
+        },
+        {
+            "x": 2.8876548130761686,
+            "y": 5.301012534370082,
+            "heading": -0.4829712350438941,
+            "angularVelocity": -0.0000017991233825733017,
+            "velocityX": 3.731804034986268,
+            "velocityY": -0.6198929366167053,
+            "timestamp": 0.6322511296957981
+        },
+        {
+            "x": 2.975041345179212,
+            "y": 5.286496470320027,
+            "heading": -0.48297127717345933,
+            "angularVelocity": -0.000001799124124149535,
+            "velocityX": 3.7318025321954105,
+            "velocityY": -0.6199019834729332,
+            "timestamp": 0.6556678382030499
+        },
+        {
+            "x": 3.0624277772479402,
+            "y": 5.271979804077107,
+            "heading": -0.4829713193030743,
+            "angularVelocity": -0.0000017991262516148556,
+            "velocityX": 3.7317982602749975,
+            "velocityY": -0.6199276998481766,
+            "timestamp": 0.6790845467103016
+        },
+        {
+            "x": 3.149814115388955,
+            "y": 5.257462572426413,
+            "heading": -0.482971361432736,
+            "angularVelocity": -0.0000017991282469783394,
+            "velocityX": 3.731794249134225,
+            "velocityY": -0.619951845332987,
+            "timestamp": 0.7025012552175534
+        },
+        {
+            "x": 3.2372009926614522,
+            "y": 5.242948586436876,
+            "heading": -0.48297140356212753,
+            "angularVelocity": -0.0000017991167072996656,
+            "velocityX": 3.7318172725014778,
+            "velocityY": -0.6198132408343549,
+            "timestamp": 0.7259179637248051
+        },
+        {
+            "x": 3.3245908005140246,
+            "y": 5.228452256133779,
+            "heading": -0.4829714456900571,
+            "angularVelocity": -0.0000017990542795775989,
+            "velocityX": 3.7319424216049217,
+            "velocityY": -0.6190592626894866,
+            "timestamp": 0.7493346722320569
+        },
+        {
+            "x": 3.4119833565424647,
+            "y": 5.213972502683899,
+            "heading": -0.48297148781659605,
+            "angularVelocity": -0.0000017989948869691892,
+            "velocityX": 3.7320597812190472,
+            "velocityY": -0.6183513556300411,
+            "timestamp": 0.7727513807393086
+        },
+        {
+            "x": 3.4993677183765524,
+            "y": 5.199443379550061,
+            "heading": -0.48297152994719206,
+            "angularVelocity": -0.0000017991681461203675,
+            "velocityX": 3.731709851836638,
+            "velocityY": -0.6204596657670539,
+            "timestamp": 0.7961680892465604
+        },
+        {
+            "x": 3.5867330233447126,
+            "y": 5.184800101210533,
+            "heading": -0.48297157208742414,
+            "angularVelocity": -0.0000017995796480013664,
+            "velocityX": 3.7308960369522555,
+            "velocityY": -0.6253346124593939,
+            "timestamp": 0.8195847977538121
+        },
+        {
+            "x": 3.6740864894013328,
+            "y": 5.170086363489935,
+            "heading": -0.48297161423379187,
+            "angularVelocity": -0.0000017998416699478457,
+            "velocityX": 3.7303904615618975,
+            "velocityY": -0.6283435486246598,
+            "timestamp": 0.8430015062610638
+        },
+        {
+            "x": 3.7614250146254347,
+            "y": 5.155284197469098,
+            "heading": -0.482971656398251,
+            "angularVelocity": -0.0000018006142553635733,
+            "velocityX": 3.729752420031712,
+            "velocityY": -0.6321198393988197,
+            "timestamp": 0.8664182147683156
+        },
+        {
+            "x": 3.848175799548237,
+            "y": 5.137355912505346,
+            "heading": -0.48297169899264486,
+            "angularVelocity": -0.000001818974422860047,
+            "velocityX": 3.704653234929829,
+            "velocityY": -0.7656193421980864,
+            "timestamp": 0.8898349232755673
+        },
+        {
+            "x": 3.9339293985898425,
+            "y": 5.115142351916783,
+            "heading": -0.48297174253890224,
+            "angularVelocity": -0.0000018596233270255789,
+            "velocityX": 3.6620688605764276,
+            "velocityY": -0.9486201095127581,
+            "timestamp": 0.9132516317828191
+        },
+        {
+            "x": 4.018431663513184,
+            "y": 5.088562488555908,
+            "heading": -0.4829717875114574,
+            "angularVelocity": -0.000001920532733427299,
+            "velocityX": 3.6086312001179417,
+            "velocityY": -1.135081104701801,
+            "timestamp": 0.9366683402900708
+        },
+        {
+            "x": 4.095489617333613,
+            "y": 5.060208566496142,
+            "heading": -0.48297174072333765,
+            "angularVelocity": 0.00000215563208981735,
+            "velocityX": 3.550230245224857,
+            "velocityY": -1.306327856847248,
+            "timestamp": 0.9583733984124503
+        },
+        {
+            "x": 4.1711135466589555,
+            "y": 5.028225843813575,
+            "heading": -0.482971694875483,
+            "angularVelocity": 0.0000021123119922022846,
+            "velocityX": 3.4841615672696387,
+            "velocityY": -1.4735147218783384,
+            "timestamp": 0.9800784565348297
+        },
+        {
+            "x": 4.245175973739862,
+            "y": 4.9927768637698655,
+            "heading": -0.48297164958118555,
+            "angularVelocity": 0.0000020868083942836762,
+            "velocityX": 3.4122197076519116,
+            "velocityY": -1.633212859593547,
+            "timestamp": 1.0017835146572092
+        },
+        {
+            "x": 4.317835043195099,
+            "y": 4.954533786792328,
+            "heading": -0.4829716044969262,
+            "angularVelocity": 0.0000020771314744606885,
+            "velocityX": 3.3475639201504173,
+            "velocityY": -1.7619430808199938,
+            "timestamp": 1.0234885727795886
+        },
+        {
+            "x": 4.39018173013391,
+            "y": 4.915702997439957,
+            "heading": -0.48297155943721365,
+            "angularVelocity": 0.000002076000546478681,
+            "velocityX": 3.333171767193961,
+            "velocityY": -1.789020288884605,
+            "timestamp": 1.045193630901968
+        },
+        {
+            "x": 4.462524157787397,
+            "y": 4.876864273483723,
+            "heading": -0.4829715143782692,
+            "angularVelocity": 0.000002075965162802355,
+            "velocityX": 3.3329755325063237,
+            "velocityY": -1.7893858536194671,
+            "timestamp": 1.0668986890243475
+        },
+        {
+            "x": 4.534839835738717,
+            "y": 4.8379757656949245,
+            "heading": -0.48297146931947993,
+            "angularVelocity": 0.0000020759580074306272,
+            "velocityX": 3.331743114605303,
+            "velocityY": -1.7916795048180625,
+            "timestamp": 1.088603747146727
+        },
+        {
+            "x": 4.60713879668486,
+            "y": 4.7990561876016065,
+            "heading": -0.48297142426091105,
+            "angularVelocity": 0.000002075947856864141,
+            "velocityX": 3.3309729252282607,
+            "velocityY": -1.793110982419974,
+            "timestamp": 1.1103088052691064
+        },
+        {
+            "x": 4.679445820973998,
+            "y": 4.760151592099993,
+            "heading": -0.48297137920221006,
+            "angularVelocity": 0.0000020759539417382525,
+            "velocityX": 3.331344421260875,
+            "velocityY": -1.7924207013070022,
+            "timestamp": 1.1320138633914858
+        },
+        {
+            "x": 4.75175835900604,
+            "y": 4.72125724602898,
+            "heading": -0.4829713341434503,
+            "angularVelocity": 0.000002075956651977989,
+            "velocityX": 3.3315984515826855,
+            "velocityY": -1.791948487386363,
+            "timestamp": 1.1537189215138652
+        },
+        {
+            "x": 4.824072564767644,
+            "y": 4.68236600077068,
+            "heading": -0.48297128908467135,
+            "angularVelocity": 0.0000020759575350386475,
+            "velocityX": 3.331675287569862,
+            "velocityY": -1.7918056260909452,
+            "timestamp": 1.1754239796362447
+        },
+        {
+            "x": 4.8963869623773055,
+            "y": 4.643475112237639,
+            "heading": -0.48297124402589003,
+            "angularVelocity": 0.000002075957644932215,
+            "velocityX": 3.3316841264339563,
+            "velocityY": -1.79178919096958,
+            "timestamp": 1.1971290377586241
+        },
+        {
+            "x": 4.968701267365325,
+            "y": 4.604584051483172,
+            "heading": -0.4829711989671097,
+            "angularVelocity": 0.000002075957594698055,
+            "velocityX": 3.331679859150302,
+            "velocityY": -1.7917971255911598,
+            "timestamp": 1.2188340958810036
+        },
+        {
+            "x": 5.0410154847403765,
+            "y": 4.565692827821048,
+            "heading": -0.4829711539083305,
+            "angularVelocity": 0.0000020759575472868184,
+            "velocityX": 3.331675822627313,
+            "velocityY": -1.7918046311069282,
+            "timestamp": 1.240539154003383
+        },
+        {
+            "x": 5.113329654064444,
+            "y": 4.526801514813212,
+            "heading": -0.4829711088495519,
+            "angularVelocity": 0.000002075957520958662,
+            "velocityX": 3.331673608812237,
+            "velocityY": -1.7918087474613025,
+            "timestamp": 1.2622442121257624
+        },
+        {
+            "x": 5.185643801945598,
+            "y": 4.487910161934636,
+            "heading": -0.48297106379077354,
+            "angularVelocity": 0.0000020759575090327586,
+            "velocityX": 3.3316726208897984,
+            "velocityY": -1.7918105843944965,
+            "timestamp": 1.2839492702481419
+        },
+        {
+            "x": 5.257957938797454,
+            "y": 4.4490187885483135,
+            "heading": -0.48297101873199527,
+            "angularVelocity": 0.0000020759575026231074,
+            "velocityX": 3.331672112745739,
+            "velocityY": -1.7918115292316654,
+            "timestamp": 1.3056543283705213
+        },
+        {
+            "x": 5.330272065176824,
+            "y": 4.410127395689581,
+            "heading": -0.4829709736732172,
+            "angularVelocity": 0.000002075957497099591,
+            "velocityX": 3.3316716302550873,
+            "velocityY": -1.7918124263686461,
+            "timestamp": 1.3273593864929008
+        },
+        {
+            "x": 5.402586187963667,
+            "y": 4.371235996150952,
+            "heading": -0.4829709286144391,
+            "angularVelocity": 0.00000207595749402331,
+            "velocityX": 3.3316714647394585,
+            "velocityY": -1.7918127341261798,
+            "timestamp": 1.3490644446152802
+        },
+        {
+            "x": 5.474900315939471,
+            "y": 4.332344606260608,
+            "heading": -0.482970883555661,
+            "angularVelocity": 0.0000020759574957870433,
+            "velocityX": 3.331671703806323,
+            "velocityY": -1.7918122896084423,
+            "timestamp": 1.3707695027376596
+        },
+        {
+            "x": 5.547214451711641,
+            "y": 4.293453230866729,
+            "heading": -0.4829708384968828,
+            "angularVelocity": 0.0000020759574997718594,
+            "velocityX": 3.331672063002153,
+            "velocityY": -1.7918116217242377,
+            "timestamp": 1.392474560860039
+        },
+        {
+            "x": 5.619528591620017,
+            "y": 4.254561863163663,
+            "heading": -0.4829707934381046,
+            "angularVelocity": 0.0000020759575011693265,
+            "velocityX": 3.3316722535663335,
+            "velocityY": -1.7918112673914555,
+            "timestamp": 1.4141796189824185
+        },
+        {
+            "x": 5.6918427353771435,
+            "y": 4.215670502616919,
+            "heading": -0.4829707483793263,
+            "angularVelocity": 0.000002075957502624911,
+            "velocityX": 3.3316724308867816,
+            "velocityY": -1.7918109376838844,
+            "timestamp": 1.435884677104798
+        },
+        {
+            "x": 5.764156898177578,
+            "y": 4.176779177479102,
+            "heading": -0.4829707033205479,
+            "angularVelocity": 0.0000020759575111736717,
+            "velocityX": 3.3316733082540817,
+            "velocityY": -1.7918093063163627,
+            "timestamp": 1.4575897352271774
+        },
+        {
+            "x": 5.836471080177112,
+            "y": 4.137887888039928,
+            "heading": -0.48297065826176927,
+            "angularVelocity": 0.0000020759575207599675,
+            "velocityX": 3.331674192798964,
+            "velocityY": -1.7918076616010294,
+            "timestamp": 1.4792947933495568
+        },
+        {
+            "x": 5.908785251268908,
+            "y": 4.098996578318978,
+            "heading": -0.48297061320299073,
+            "angularVelocity": 0.0000020759575149169034,
+            "velocityX": 3.331673690255448,
+            "velocityY": -1.7918085960272527,
+            "timestamp": 1.5009998514719363
+        },
+        {
+            "x": 5.981099339043632,
+            "y": 4.0601051136793025,
+            "heading": -0.48297056814421313,
+            "angularVelocity": 0.000002075957470564626,
+            "velocityX": 3.3316698516536873,
+            "velocityY": -1.7918157334753977,
+            "timestamp": 1.5227049095943157
+        },
+        {
+            "x": 6.053413282760847,
+            "y": 4.021213381182831,
+            "heading": -0.48297052308543725,
+            "angularVelocity": 0.00000207595739406405,
+            "velocityX": 3.3316632146058507,
+            "velocityY": -1.7918280742300665,
+            "timestamp": 1.5444099677166951
+        },
+        {
+            "x": 6.125727369888532,
+            "y": 3.982321915340118,
+            "heading": -0.4829704780266597,
+            "angularVelocity": 0.0000020759574679524,
+            "velocityX": 3.3316698218431826,
+            "velocityY": -1.7918157889019397,
+            "timestamp": 1.5661150258390746
+        },
+        {
+            "x": 6.198042995964267,
+            "y": 3.9434333111252817,
+            "heading": -0.4829704329678644,
+            "angularVelocity": 0.000002075958288103883,
+            "velocityX": 3.3317407245813113,
+            "velocityY": -1.7916839473808401,
+            "timestamp": 1.587820083961454
+        },
+        {
+            "x": 6.270359121166566,
+            "y": 3.9045456350763237,
+            "heading": -0.4829703879090624,
+            "angularVelocity": 0.0000020759585995923127,
+            "velocityX": 3.331763720444871,
+            "velocityY": -1.791641184728093,
+            "timestamp": 1.6095251420838335
+        },
+        {
+            "x": 6.342677550055512,
+            "y": 3.8656622432711547,
+            "heading": -0.48297034285023976,
+            "angularVelocity": 0.0000020759595452668008,
+            "velocityX": 3.331869856379567,
+            "velocityY": -1.791443800149147,
+            "timestamp": 1.631230200206213
+        },
+        {
+            "x": 6.414977737835259,
+            "y": 3.8267449442462484,
+            "heading": -0.4829702977916256,
+            "angularVelocity": 0.0000020759499429522823,
+            "velocityX": 3.3310294481622025,
+            "velocityY": -1.7930059807042389,
+            "timestamp": 1.6529352583285923
+        },
+        {
+            "x": 6.487268603977522,
+            "y": 3.787810332568674,
+            "heading": -0.4829702527330872,
+            "angularVelocity": 0.00000207594645249869,
+            "velocityX": 3.330599979721225,
+            "velocityY": -1.793803612876013,
+            "timestamp": 1.6746403164509718
+        },
+        {
+            "x": 6.559543297837492,
+            "y": 3.7488457083598847,
+            "heading": -0.4829702076720725,
+            "angularVelocity": 0.000002076060537305802,
+            "velocityX": 3.3298548869330626,
+            "velocityY": -1.7951863565208734,
+            "timestamp": 1.6963453745733512
+        },
+        {
+            "x": 6.630628961599716,
+            "y": 3.7077518923991635,
+            "heading": -0.48297016252441444,
+            "angularVelocity": 0.000002080052392839484,
+            "velocityX": 3.2750736423474676,
+            "velocityY": -1.893282926450661,
+            "timestamp": 1.7180504326957307
+        },
+        {
+            "x": 6.699799614538253,
+            "y": 3.6635105764693066,
+            "heading": -0.4829701170038858,
+            "angularVelocity": 0.0000020972313632639956,
+            "velocityX": 3.186844861162469,
+            "velocityY": -2.0382952066016227,
+            "timestamp": 1.73975549081811
+        },
+        {
+            "x": 6.766806665010948,
+            "y": 3.616056089090901,
+            "heading": -0.48297007073382403,
+            "angularVelocity": 0.000002131764013783056,
+            "velocityX": 3.0871629135886995,
+            "velocityY": -2.1863331169555953,
+            "timestamp": 1.7614605489404895
+        },
+        {
+            "x": 6.831474779993965,
+            "y": 3.565460555976799,
+            "heading": -0.48297002329923955,
+            "angularVelocity": 0.0000021854161466442962,
+            "velocityX": 2.9794029860873867,
+            "velocityY": -2.331048036306895,
+            "timestamp": 1.783165607062869
+        },
+        {
+            "x": 6.893649101257324,
+            "y": 3.5118300914764404,
+            "heading": -0.48296997476148124,
+            "angularVelocity": 0.00000223624180421772,
+            "velocityX": 2.8645083976649595,
+            "velocityY": -2.470874033046681,
+            "timestamp": 1.8048706651852484
+        },
+        {
+            "x": 6.934561334359113,
+            "y": 3.474160479757131,
+            "heading": -0.4829699264500687,
+            "angularVelocity": 0.0000032862628672599958,
+            "velocityX": 2.782952215912443,
+            "velocityY": -2.5623810156238895,
+            "timestamp": 1.8195716840772946
+        },
+        {
+            "x": 6.974231870495054,
+            "y": 3.435185370851295,
+            "heading": -0.4829698798888691,
+            "angularVelocity": 0.0000031672090196575476,
+            "velocityX": 2.6984888889168737,
+            "velocityY": -2.6511841928806597,
+            "timestamp": 1.8342727029693409
+        },
+        {
+            "x": 7.012622614846585,
+            "y": 3.394949051793438,
+            "heading": -0.482969834780297,
+            "angularVelocity": 0.0000030683976682796836,
+            "velocityX": 2.611434257274787,
+            "velocityY": -2.736974855506367,
+            "timestamp": 1.848973721861387
+        },
+        {
+            "x": 7.049699862488227,
+            "y": 3.3534992220248374,
+            "heading": -0.4829697908553284,
+            "angularVelocity": 0.0000029878860039287864,
+            "velocityX": 2.5220869324714577,
+            "velocityY": -2.8195208830745364,
+            "timestamp": 1.8636747407534333
+        },
+        {
+            "x": 7.085438739774718,
+            "y": 3.310890028476691,
+            "heading": -0.48296974786836877,
+            "angularVelocity": 0.0000029240802913465185,
+            "velocityX": 2.4310476402306533,
+            "velocityY": -2.8983837012274143,
+            "timestamp": 1.8783757596454795
+        },
+        {
+            "x": 7.119836677140904,
+            "y": 3.2671911422114968,
+            "heading": -0.4829697055890564,
+            "angularVelocity": 0.0000028759443619428334,
+            "velocityX": 2.339833559754191,
+            "velocityY": -2.9725073198044742,
+            "timestamp": 1.8930767785375258
+        },
+        {
+            "x": 7.1529563800127525,
+            "y": 3.222515689615774,
+            "heading": -0.48296966398118896,
+            "angularVelocity": 0.000002830270996857658,
+            "velocityX": 2.2528848588700137,
+            "velocityY": -3.0389357991976094,
+            "timestamp": 1.907777797429572
+        },
+        {
+            "x": 7.184894346745796,
+            "y": 3.177381185858669,
+            "heading": -0.4821430167851757,
+            "angularVelocity": 0.056230605652804695,
+            "velocityX": 2.17250021699707,
+            "velocityY": -3.0701616050246474,
+            "timestamp": 1.9224788163216182
+        },
+        {
+            "x": 7.2158791345376,
+            "y": 3.1333943550888805,
+            "heading": -0.47750834116458535,
+            "angularVelocity": 0.31526220424747137,
+            "velocityX": 2.107662606199986,
+            "velocityY": -2.992094023740475,
+            "timestamp": 1.9371798352136644
+        },
+        {
+            "x": 7.245895346179036,
+            "y": 3.090550131820991,
+            "heading": -0.46908945700490695,
+            "angularVelocity": 0.5726735147747635,
+            "velocityX": 2.041777638805421,
+            "velocityY": -2.9143710094182476,
+            "timestamp": 1.9518808541057107
+        },
+        {
+            "x": 7.274933319615603,
+            "y": 3.048846636162261,
+            "heading": -0.45692051225089847,
+            "angularVelocity": 0.827761996863506,
+            "velocityX": 1.9752354343465999,
+            "velocityY": -2.8367758700924766,
+            "timestamp": 1.9665818729977569
+        },
+        {
+            "x": 7.3029845213490745,
+            "y": 3.008282422680816,
+            "heading": -0.4410407652781448,
+            "angularVelocity": 1.08018002625281,
+            "velocityX": 1.90811276003785,
+            "velocityY": -2.7592790526506894,
+            "timestamp": 1.981282891889803
+        },
+        {
+            "x": 7.330040747407425,
+            "y": 2.9688555797343485,
+            "heading": -0.42149121219843927,
+            "angularVelocity": 1.3298093977882255,
+            "velocityX": 1.8404320310743014,
+            "velocityY": -2.6819122698903146,
+            "timestamp": 1.9959839107818493
+        },
+        {
+            "x": 7.356112777651364,
+            "y": 2.9307983654229495,
+            "heading": -0.3996724931243264,
+            "angularVelocity": 1.4841637327544344,
+            "velocityX": 1.7734845751435842,
+            "velocityY": -2.5887467114262686,
+            "timestamp": 2.0106849296738956
+        },
+        {
+            "x": 7.38118332940277,
+            "y": 2.8941879380599844,
+            "heading": -0.37646864699164706,
+            "angularVelocity": 1.578383532670194,
+            "velocityX": 1.7053615083080305,
+            "velocityY": -2.4903326518935414,
+            "timestamp": 2.025385948565942
+        },
+        {
+            "x": 7.40523963577506,
+            "y": 2.8590504496296347,
+            "heading": -0.3524751392807844,
+            "angularVelocity": 1.6320982842790503,
+            "velocityX": 1.6363700059799433,
+            "velocityY": -2.390139669119207,
+            "timestamp": 2.040086967457988
+        },
+        {
+            "x": 7.428276340738969,
+            "y": 2.825395406477394,
+            "heading": -0.3281233875567818,
+            "angularVelocity": 1.656466936259613,
+            "velocityX": 1.5670141731720877,
+            "velocityY": -2.289300040995877,
+            "timestamp": 2.0547879863500342
+        },
+        {
+            "x": 7.450292281624529,
+            "y": 2.793225377698084,
+            "heading": -0.3037369697425905,
+            "angularVelocity": 1.658825010243686,
+            "velocityX": 1.497579252650967,
+            "velocityY": -2.1882856566299744,
+            "timestamp": 2.0694890052420805
+        },
+        {
+            "x": 7.471288319158757,
+            "y": 2.7625397684205146,
+            "heading": -0.27956516086770095,
+            "angularVelocity": 1.6442267745106667,
+            "velocityX": 1.4282028809299567,
+            "velocityY": -2.0873117368873455,
+            "timestamp": 2.0841900241341267
+        },
+        {
+            "x": 7.491266209557209,
+            "y": 2.733336527235843,
+            "heading": -0.2558043926700512,
+            "angularVelocity": 1.6162667616531794,
+            "velocityX": 1.3589459713748013,
+            "velocityY": -1.9864773590946931,
+            "timestamp": 2.098891043026173
+        },
+        {
+            "x": 7.510228057377649,
+            "y": 2.7056129838220437,
+            "heading": -0.2326125564520332,
+            "angularVelocity": 1.577566588297197,
+            "velocityX": 1.28983221909181,
+            "velocityY": -1.885824623271541,
+            "timestamp": 2.113592061918219
+        },
+        {
+            "x": 7.528176060430696,
+            "y": 2.679366275566135,
+            "heading": -0.21011884554685348,
+            "angularVelocity": 1.5300783619392286,
+            "velocityX": 1.2208679673730278,
+            "velocityY": -1.7853666095287006,
+            "timestamp": 2.1282930808102654
+        },
+        {
+            "x": 7.545112396890263,
+            "y": 2.654593565371254,
+            "heading": -0.18843070859149175,
+            "angularVelocity": 1.4752812110931899,
+            "velocityX": 1.152051880480808,
+            "velocityY": -1.6851015821961506,
+            "timestamp": 2.1429940997023116
+        },
+        {
+            "x": 7.561039180049459,
+            "y": 2.631292148556631,
+            "heading": -0.1676388680105614,
+            "angularVelocity": 1.4143128944742427,
+            "velocityX": 1.0833795450607198,
+            "velocityY": -1.5850205340005987,
+            "timestamp": 2.157695118594358
+        },
+        {
+            "x": 7.575958444025175,
+            "y": 2.6094594995019675,
+            "heading": -0.14782100448684538,
+            "angularVelocity": 1.3480605439149753,
+            "velocityX": 1.0148455753489192,
+            "velocityY": -1.485111284801969,
+            "timestamp": 2.172396137486404
+        },
+        {
+            "x": 7.589872142500143,
+            "y": 2.5890932855015207,
+            "heading": -0.12904449811203259,
+            "angularVelocity": 1.2772248313327248,
+            "velocityX": 0.9464445000131985,
+            "velocityY": -1.3853607120705473,
+            "timestamp": 2.1870971563784503
+        },
+        {
+            "x": 7.602782151943804,
+            "y": 2.570191363211945,
+            "heading": -0.11136848807718236,
+            "angularVelocity": 1.2023663233582889,
+            "velocityX": 0.8781710668125251,
+            "velocityY": -1.2857559349035097,
+            "timestamp": 2.2017981752704965
+        },
+        {
+            "x": 7.614690275426409,
+            "y": 2.552751766479238,
+            "heading": -0.09484543137092857,
+            "angularVelocity": 1.1239395600799826,
+            "velocityX": 0.8100202829510676,
+            "velocityY": -1.186284900439311,
+            "timestamp": 2.2164991941625427
+        },
+        {
+            "x": 7.62559824546286,
+            "y": 2.536772690588083,
+            "heading": -0.07952228805027751,
+            "angularVelocity": 1.042318456507909,
+            "velocityX": 0.7419873490775946,
+            "velocityY": -1.086936627215761,
+            "timestamp": 2.231200213054589
+        },
+        {
+            "x": 7.635507725447758,
+            "y": 2.522252475791086,
+            "heading": -0.06544142530276546,
+            "angularVelocity": 0.9578154310876013,
+            "velocityX": 0.6740675634570735,
+            "velocityY": -0.9877012541528096,
+            "timestamp": 2.245901231946635
+        },
+        {
+            "x": 7.644420309753795,
+            "y": 2.509189591670685,
+            "heading": -0.05264130826624494,
+            "angularVelocity": 0.8706959109783889,
+            "velocityX": 0.6062562310465265,
+            "velocityY": -0.8885699839124228,
+            "timestamp": 2.2606022508386814
+        },
+        {
+            "x": 7.652337522761704,
+            "y": 2.497582623097395,
+            "heading": -0.04115702850586921,
+            "angularVelocity": 0.7811893750159932,
+            "velocityX": 0.538548590818527,
+            "velocityY": -0.7895349743118973,
+            "timestamp": 2.2753032697307276
+        },
+        {
+            "x": 7.6592608171290975,
+            "y": 2.487430258069545,
+            "heading": -0.031020708755951136,
+            "angularVelocity": 0.6894977704846257,
+            "velocityX": 0.4709397639872257,
+            "velocityY": -0.6905892103399419,
+            "timestamp": 2.290004288622774
+        },
+        {
+            "x": 7.665191571570965,
+            "y": 2.4787312774326344,
+            "heading": -0.022261813501037925,
+            "angularVelocity": 0.5958019181685587,
+            "velocityX": 0.40342472079099084,
+            "velocityY": -0.5917263763001924,
+            "timestamp": 2.30470530751482
+        },
+        {
+            "x": 7.670131088361328,
+            "y": 2.4714845463094046,
+            "heading": -0.014907388223763604,
+            "angularVelocity": 0.5002663646159557,
+            "velocityX": 0.33599826152420176,
+            "velocityY": -0.49294073944433947,
+            "timestamp": 2.3194063264068663
+        },
+        {
+            "x": 7.674080590697013,
+            "y": 2.465689006980458,
+            "heading": -0.008982245050778671,
+            "angularVelocity": 0.40304302827545885,
+            "velocityX": 0.26865500715889973,
+            "velocityY": -0.3942270513020354,
+            "timestamp": 2.3341073452989125
+        },
+        {
+            "x": 7.677041220003002,
+            "y": 2.461343672911727,
+            "heading": -0.0045091086563273375,
+            "angularVelocity": 0.304273902870198,
+            "velocityX": 0.20138939536970257,
+            "velocityY": -0.29558046966947404,
+            "timestamp": 2.3488083641909587
+        },
+        {
+            "x": 7.679014033209882,
+            "y": 2.458447623611078,
+            "heading": -0.00150873334272897,
+            "angularVelocity": 0.20409301801671295,
+            "velocityX": 0.1341956786374654,
+            "velocityY": -0.1969965022095263,
+            "timestamp": 2.363509383083005
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -1.1328442579469643e-34,
+            "angularVelocity": 0.10262780789604178,
+            "velocityX": 0.06706792211866815,
+            "velocityY": -0.0984709714141921,
+            "timestamp": 2.378210401975051
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -2.155134012002005e-33,
+            "angularVelocity": -1.3889170145055665e-31,
+            "velocityX": 1.4892305241424507e-32,
+            "velocityY": 6.576507695252416e-33,
+            "timestamp": 2.3929114208670974
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Center To C5.traj b/src/main/deploy/choreo/Center To C5.traj
new file mode 100644
index 00000000..39d8ef61
--- /dev/null
+++ b/src/main/deploy/choreo/Center To C5.traj	
@@ -0,0 +1,625 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -4.1566893564125267e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 1.3546830312136449,
+            "y": 5.550921973415328,
+            "heading": -1.4050458772244498e-18,
+            "angularVelocity": -4.014308801297789e-17,
+            "velocityX": 0.13379730014070312,
+            "velocityY": -0.25936522568857817,
+            "timestamp": 0.03500093954603885
+        },
+        {
+            "x": 1.3640490935091651,
+            "y": 5.532765920501419,
+            "heading": -3.603876179363921e-18,
+            "angularVelocity": -6.28220327965604e-17,
+            "velocityX": 0.26759459651667833,
+            "velocityY": -0.5187304440792354,
+            "timestamp": 0.0700018790920777
+        },
+        {
+            "x": 1.3780981867284385,
+            "y": 5.505531841564791,
+            "heading": -4.091508575199503e-18,
+            "angularVelocity": -1.3931950355515604e-17,
+            "velocityX": 0.4013918883749871,
+            "velocityY": -0.7780956537124009,
+            "timestamp": 0.10500281863811654
+        },
+        {
+            "x": 1.3968303106782045,
+            "y": 5.469219736980081,
+            "heading": -4.985304542720085e-18,
+            "angularVelocity": -2.5536326104216816e-17,
+            "velocityX": 0.5351891747117141,
+            "velocityY": -1.0374608526419855,
+            "timestamp": 0.1400037581841554
+        },
+        {
+            "x": 1.4202454651168877,
+            "y": 5.423829607215581,
+            "heading": -4.3424315250592746e-18,
+            "angularVelocity": 1.8367331153854942e-17,
+            "velocityX": 0.6689864541464801,
+            "velocityY": -1.2968260381921248,
+            "timestamp": 0.17500469773019423
+        },
+        {
+            "x": 1.4483436497338926,
+            "y": 5.369361452873384,
+            "heading": -2.0560193041515625e-18,
+            "angularVelocity": 6.532433512064817e-17,
+            "velocityX": 0.8027837247073224,
+            "velocityY": -1.5561912065401715,
+            "timestamp": 0.21000563727623306
+        },
+        {
+            "x": 1.4811248641150936,
+            "y": 5.305815274756268,
+            "heading": 2.1208651292681738e-19,
+            "angularVelocity": 6.480130833433249e-17,
+            "velocityX": 0.9365809834363137,
+            "velocityY": -1.815556351952184,
+            "timestamp": 0.2450065768222719
+        },
+        {
+            "x": 1.518589107680717,
+            "y": 5.233191073988126,
+            "heading": 3.0112738874394488e-18,
+            "angularVelocity": 7.99746695569e-17,
+            "velocityX": 1.0703782256008032,
+            "velocityY": -2.074921465253921,
+            "timestamp": 0.2800075163683107
+        },
+        {
+            "x": 1.5607363795611104,
+            "y": 5.151488852254779,
+            "heading": 5.4707987602321704e-18,
+            "angularVelocity": 7.027028770373186e-17,
+            "velocityX": 1.2041754429187383,
+            "velocityY": -2.334286530390624,
+            "timestamp": 0.31500845591434956
+        },
+        {
+            "x": 1.6075666783068712,
+            "y": 5.060708612365892,
+            "heading": 7.441304754324509e-18,
+            "angularVelocity": 5.629869559433992e-17,
+            "velocityX": 1.3379726188262824,
+            "velocityY": -2.5936515152532893,
+            "timestamp": 0.3500093954603884
+        },
+        {
+            "x": 1.6590800010192646,
+            "y": 4.9608503599406655,
+            "heading": 8.015191331788108e-18,
+            "angularVelocity": 1.639633984973558e-17,
+            "velocityX": 1.4717697119150586,
+            "velocityY": -2.853016339571725,
+            "timestamp": 0.38501033500642723
+        },
+        {
+            "x": 1.7152763390026586,
+            "y": 4.851914111835609,
+            "heading": 8.871725964928404e-18,
+            "angularVelocity": 2.4471795525975083e-17,
+            "velocityX": 1.6055665565638684,
+            "velocityY": -3.112380682288518,
+            "timestamp": 0.42001127455246606
+        },
+        {
+            "x": 1.775979033805623,
+            "y": 4.734242319347404,
+            "heading": 1.526334137039101e-17,
+            "angularVelocity": 1.8261268092684144e-16,
+            "velocityX": 1.7343161523752855,
+            "velocityY": -3.361960964887902,
+            "timestamp": 0.4550122140985049
+        },
+        {
+            "x": 1.8366817286107127,
+            "y": 4.616570526856247,
+            "heading": 2.6738432547349554e-17,
+            "angularVelocity": 3.2785094702569765e-16,
+            "velocityX": 1.734316152436003,
+            "velocityY": -3.3619609649722197,
+            "timestamp": 0.49001315364454373
+        },
+        {
+            "x": 1.8973844234157844,
+            "y": 4.498898734365081,
+            "heading": 1.9852147320466475e-17,
+            "angularVelocity": -1.967457250062152e-16,
+            "velocityX": 1.7343161524354893,
+            "velocityY": -3.3619609649724835,
+            "timestamp": 0.5250140931905826
+        },
+        {
+            "x": 1.9580871182208561,
+            "y": 4.381226941873916,
+            "heading": 2.777024505719557e-17,
+            "angularVelocity": 2.2622529107579697e-16,
+            "velocityX": 1.7343161524354895,
+            "velocityY": -3.361960964972483,
+            "timestamp": 0.5600150327366215
+        },
+        {
+            "x": 2.018789813025928,
+            "y": 4.26355514938275,
+            "heading": 1.7414652311267274e-17,
+            "angularVelocity": -2.9586613617347484e-16,
+            "velocityX": 1.7343161524354893,
+            "velocityY": -3.361960964972483,
+            "timestamp": 0.5950159722826603
+        },
+        {
+            "x": 2.0794925078309996,
+            "y": 4.145883356891583,
+            "heading": 6.771469247853252e-18,
+            "angularVelocity": -3.040827818183264e-16,
+            "velocityX": 1.7343161524354895,
+            "velocityY": -3.361960964972483,
+            "timestamp": 0.6300169118286992
+        },
+        {
+            "x": 2.140195202636071,
+            "y": 4.028211564400419,
+            "heading": 1.7156471914154247e-17,
+            "angularVelocity": 2.9670636674740876e-16,
+            "velocityX": 1.7343161524354882,
+            "velocityY": -3.361960964972482,
+            "timestamp": 0.6650178513747381
+        },
+        {
+            "x": 2.2008978974411426,
+            "y": 3.910539771909253,
+            "heading": 9.427453062324153e-18,
+            "angularVelocity": -2.208231822366884e-16,
+            "velocityX": 1.734316152435471,
+            "velocityY": -3.361960964972468,
+            "timestamp": 0.700018790920777
+        },
+        {
+            "x": 2.261600592246207,
+            "y": 3.7928679794180944,
+            "heading": -2.0403441270686145e-18,
+            "angularVelocity": -3.2764255297515643e-16,
+            "velocityX": 1.7343161524352513,
+            "velocityY": -3.3619609649722837,
+            "timestamp": 0.7350197304668159
+        },
+        {
+            "x": 2.322303287051213,
+            "y": 3.6751961869269927,
+            "heading": 3.357966916114392e-18,
+            "angularVelocity": 1.5423331811082687e-16,
+            "velocityX": 1.7343161524334347,
+            "velocityY": -3.3619609649707387,
+            "timestamp": 0.7700206700128548
+        },
+        {
+            "x": 2.383005981879539,
+            "y": 3.55752439444761,
+            "heading": 1.6083387472754582e-17,
+            "angularVelocity": 3.6357368464068287e-16,
+            "velocityX": 1.7343161531001825,
+            "velocityY": -3.361960964635663,
+            "timestamp": 0.8050216095588937
+        },
+        {
+            "x": 2.4437089810285184,
+            "y": 3.439852758955193,
+            "heading": 2.2946423291224054e-17,
+            "angularVelocity": 1.9608147402560587e-16,
+            "velocityX": 1.7343248477441577,
+            "velocityY": -3.3619564794134043,
+            "timestamp": 0.8400225491049326
+        },
+        {
+            "x": 2.507799181067132,
+            "y": 3.323991142238629,
+            "heading": 9.669545465348722e-18,
+            "angularVelocity": -3.793291836756151e-16,
+            "velocityX": 1.8310994181462568,
+            "velocityY": -3.3102430454713967,
+            "timestamp": 0.8750234886509715
+        },
+        {
+            "x": 2.5806292795037855,
+            "y": 3.213414270581727,
+            "heading": 3.38196516760506e-17,
+            "angularVelocity": 6.899845125281262e-16,
+            "velocityX": 2.0808040978720626,
+            "velocityY": -3.159254382625257,
+            "timestamp": 0.9100244281970103
+        },
+        {
+            "x": 2.661766753531331,
+            "y": 3.1087807297901673,
+            "heading": 5.146705636243318e-17,
+            "angularVelocity": 5.041980276892245e-16,
+            "velocityX": 2.3181513147947848,
+            "velocityY": -2.9894494876003863,
+            "timestamp": 0.9450253677430492
+        },
+        {
+            "x": 2.7507288455963135,
+            "y": 3.0107133388519287,
+            "heading": 6.422105835490463e-17,
+            "angularVelocity": 3.643902751722035e-16,
+            "velocityX": 2.5417058290140595,
+            "velocityY": -2.8018502420229328,
+            "timestamp": 0.9800263072890881
+        },
+        {
+            "x": 2.9215337593923705,
+            "y": 2.8658352980219064,
+            "heading": 5.1318206888106e-17,
+            "angularVelocity": -2.1793112103404205e-16,
+            "velocityX": 2.884920936848368,
+            "velocityY": -2.4470121145413257,
+            "timestamp": 1.0392324060123015
+        },
+        {
+            "x": 3.109750180151373,
+            "y": 2.744432888397421,
+            "heading": 6.648964720130561e-17,
+            "angularVelocity": 2.5624793121609224e-16,
+            "velocityX": 3.179003934013426,
+            "velocityY": -2.0505051378665997,
+            "timestamp": 1.098438504735515
+        },
+        {
+            "x": 3.309758578030313,
+            "y": 2.6436330516151556,
+            "heading": 6.809268296964168e-17,
+            "angularVelocity": 2.7075517621497836e-17,
+            "velocityX": 3.3781722186064402,
+            "velocityY": -1.702524553319066,
+            "timestamp": 1.1576446034587284
+        },
+        {
+            "x": 3.5097675494851996,
+            "y": 2.542834352942729,
+            "heading": 6.982732716570642e-17,
+            "angularVelocity": 2.929840393934814e-17,
+            "velocityX": 3.378181906393955,
+            "velocityY": -1.702505330465446,
+            "timestamp": 1.216850702181942
+        },
+        {
+            "x": 3.709776520948374,
+            "y": 2.4420356542872104,
+            "heading": 8.871000937094426e-17,
+            "angularVelocity": 3.189313704575333e-16,
+            "velocityX": 3.3781819065336705,
+            "velocityY": -1.702505330180368,
+            "timestamp": 1.2760568009051554
+        },
+        {
+            "x": 3.9097854924115545,
+            "y": 2.341236955631689,
+            "heading": 7.04137580064196e-17,
+            "angularVelocity": -3.090264644873572e-16,
+            "velocityX": 3.3781819065337895,
+            "velocityY": -1.7025053301803954,
+            "timestamp": 1.3352628996283689
+        },
+        {
+            "x": 4.109794463874735,
+            "y": 2.240438256976168,
+            "heading": 6.635408667627181e-17,
+            "angularVelocity": -6.856846534504647e-17,
+            "velocityX": 3.3781819065337926,
+            "velocityY": -1.7025053301803963,
+            "timestamp": 1.3944689983515823
+        },
+        {
+            "x": 4.3098034353379155,
+            "y": 2.1396395583206465,
+            "heading": 8.756379552984881e-17,
+            "angularVelocity": 3.5823520399025277e-16,
+            "velocityX": 3.3781819065337926,
+            "velocityY": -1.7025053301803963,
+            "timestamp": 1.4536750970747958
+        },
+        {
+            "x": 4.509812406801097,
+            "y": 2.0388408596651253,
+            "heading": 7.076319345079811e-17,
+            "angularVelocity": -2.837647208878057e-16,
+            "velocityX": 3.3781819065337926,
+            "velocityY": -1.7025053301803963,
+            "timestamp": 1.5128811957980093
+        },
+        {
+            "x": 4.709821378264278,
+            "y": 1.9380421610096037,
+            "heading": 6.652684249010322e-17,
+            "angularVelocity": -7.15526111686399e-17,
+            "velocityX": 3.378181906533802,
+            "velocityY": -1.7025053301803985,
+            "timestamp": 1.5720872945212228
+        },
+        {
+            "x": 4.9098303497278835,
+            "y": 1.837243462354927,
+            "heading": 7.855425123263822e-17,
+            "angularVelocity": 2.0314476045367986e-16,
+            "velocityX": 3.378181906540968,
+            "velocityY": -1.7025053301661324,
+            "timestamp": 1.6312933932444362
+        },
+        {
+            "x": 5.109839349236117,
+            "y": 1.7364448193478421,
+            "heading": 7.31685607290898e-17,
+            "angularVelocity": -9.096513061477316e-17,
+            "velocityX": 3.378182380218786,
+            "velocityY": -1.7025043902702668,
+            "timestamp": 1.6904994919676497
+        },
+        {
+            "x": 5.311619758605957,
+            "y": 1.6392407417297363,
+            "heading": 6.073866217998324e-17,
+            "angularVelocity": -2.0994287441931112e-16,
+            "velocityX": 3.4081017618384997,
+            "velocityY": -1.6417916348765775,
+            "timestamp": 1.7497055906908632
+        },
+        {
+            "x": 5.430192834880249,
+            "y": 1.5926107881863976,
+            "heading": 7.355026472308393e-17,
+            "angularVelocity": 3.8038299911394555e-16,
+            "velocityX": 3.5204949744296554,
+            "velocityY": -1.3844670499027307,
+            "timestamp": 1.78338638760593
+        },
+        {
+            "x": 5.550146685206584,
+            "y": 1.5496573158737061,
+            "heading": 8.480161924468705e-17,
+            "angularVelocity": 3.3405844137167443e-16,
+            "velocityX": 3.5614908586879754,
+            "velocityY": -1.2753104512640399,
+            "timestamp": 1.8170671845209967
+        },
+        {
+            "x": 5.670100671705041,
+            "y": 1.506704223843596,
+            "heading": 8.467696633181045e-17,
+            "angularVelocity": -3.701008417078351e-18,
+            "velocityX": 3.5614949017075612,
+            "velocityY": -1.2752991604806918,
+            "timestamp": 1.8507479814360634
+        },
+        {
+            "x": 5.790054658213967,
+            "y": 1.4637511318444054,
+            "heading": 8.225246246818014e-17,
+            "angularVelocity": -7.198475350046388e-17,
+            "velocityX": 3.561494902018399,
+            "velocityY": -1.275299159562779,
+            "timestamp": 1.8844287783511302
+        },
+        {
+            "x": 5.910008644722922,
+            "y": 1.4207980398452125,
+            "heading": 9.443730259968232e-17,
+            "angularVelocity": 3.617741041647218e-16,
+            "velocityX": 3.5614949020192235,
+            "velocityY": -1.2752991595628402,
+            "timestamp": 1.918109575266197
+        },
+        {
+            "x": 6.02996263123188,
+            "y": 1.3778449478460182,
+            "heading": 8.582959689044548e-17,
+            "angularVelocity": -2.555671628240573e-16,
+            "velocityX": 3.5614949020193487,
+            "velocityY": -1.2752991595628858,
+            "timestamp": 1.9517903721812637
+        },
+        {
+            "x": 6.149916617740839,
+            "y": 1.3348918558468235,
+            "heading": 7.701354643967334e-17,
+            "angularVelocity": -2.6175302422337863e-16,
+            "velocityX": 3.56149490201936,
+            "velocityY": -1.2752991595628895,
+            "timestamp": 1.9854711690963305
+        },
+        {
+            "x": 6.269870604249798,
+            "y": 1.291938763847629,
+            "heading": 8.367010625720842e-17,
+            "angularVelocity": 1.9763664839406868e-16,
+            "velocityX": 3.5614949020193607,
+            "velocityY": -1.27529915956289,
+            "timestamp": 2.019151966011397
+        },
+        {
+            "x": 6.3898245907587565,
+            "y": 1.2489856718484342,
+            "heading": 7.968637324735258e-17,
+            "angularVelocity": -1.1827906031741492e-16,
+            "velocityX": 3.5614949020193607,
+            "velocityY": -1.2752991595628902,
+            "timestamp": 2.0528327629264638
+        },
+        {
+            "x": 6.509778577267715,
+            "y": 1.2060325798492397,
+            "heading": 7.914041276645377e-17,
+            "angularVelocity": -1.6209844508000374e-17,
+            "velocityX": 3.561494902019361,
+            "velocityY": -1.27529915956289,
+            "timestamp": 2.0865135598415305
+        },
+        {
+            "x": 6.629732563776674,
+            "y": 1.1630794878500452,
+            "heading": 7.575912442206911e-17,
+            "angularVelocity": -1.0039217162560839e-16,
+            "velocityX": 3.561494902019361,
+            "velocityY": -1.27529915956289,
+            "timestamp": 2.1201943567565973
+        },
+        {
+            "x": 6.749686550285633,
+            "y": 1.1201263958508503,
+            "heading": 6.889954024340196e-17,
+            "angularVelocity": -2.0366454499176497e-16,
+            "velocityX": 3.5614949020193576,
+            "velocityY": -1.2752991595628989,
+            "timestamp": 2.153875153671664
+        },
+        {
+            "x": 6.869640536794461,
+            "y": 1.077173303851719,
+            "heading": 8.054846416919904e-17,
+            "angularVelocity": 3.458624792985843e-16,
+            "velocityX": 3.5614949020154905,
+            "velocityY": -1.275299159561009,
+            "timestamp": 2.187555950586731
+        },
+        {
+            "x": 6.985406128017608,
+            "y": 1.0357199913369377,
+            "heading": 6.97905590140722e-17,
+            "angularVelocity": -3.1940767538205783e-16,
+            "velocityX": 3.437139314579595,
+            "velocityY": -1.2307699434580792,
+            "timestamp": 2.2212367475017976
+        },
+        {
+            "x": 7.092266700241795,
+            "y": 0.9974553856875888,
+            "heading": 5.947830002893604e-17,
+            "angularVelocity": -3.0617621445548207e-16,
+            "velocityX": 3.172744768885924,
+            "velocityY": -1.1360956139440452,
+            "timestamp": 2.2549175444168643
+        },
+        {
+            "x": 7.190222235939473,
+            "y": 0.9623794931799317,
+            "heading": 5.044588900647289e-17,
+            "angularVelocity": -2.6817687744625345e-16,
+            "velocityX": 2.9083497027904013,
+            "velocityY": -1.0414210980846788,
+            "timestamp": 2.288598341331931
+        },
+        {
+            "x": 7.279272729268112,
+            "y": 0.930492315906057,
+            "heading": 4.27627289839913e-17,
+            "angularVelocity": -2.281169282784766e-16,
+            "velocityX": 2.6439544632272356,
+            "velocityY": -0.9467465201100714,
+            "timestamp": 2.322279138246998
+        },
+        {
+            "x": 7.35941817730645,
+            "y": 0.9017938549120106,
+            "heading": 3.626946964953948e-17,
+            "angularVelocity": -1.9278817213060433e-16,
+            "velocityX": 2.379559136930222,
+            "velocityY": -0.8520719110778341,
+            "timestamp": 2.3559599351620646
+        },
+        {
+            "x": 7.430658578301727,
+            "y": 0.8762841108254199,
+            "heading": 2.9442071838197796e-17,
+            "angularVelocity": -2.0270891340137855e-16,
+            "velocityX": 2.1151637585928986,
+            "velocityY": -0.7573972834110186,
+            "timestamp": 2.3896407320771313
+        },
+        {
+            "x": 7.492993931085436,
+            "y": 0.8539630840647031,
+            "heading": 2.3413209096667504e-17,
+            "angularVelocity": -1.790000012026981e-16,
+            "velocityX": 1.850768345562039,
+            "velocityY": -0.662722643321152,
+            "timestamp": 2.423321528992198
+        },
+        {
+            "x": 7.546424234822929,
+            "y": 0.8348307749287305,
+            "heading": 1.7731266545962487e-17,
+            "angularVelocity": -1.6869976443484408e-16,
+            "velocityX": 1.5863729077500839,
+            "velocityY": -0.5680479943576784,
+            "timestamp": 2.457002325907265
+        },
+        {
+            "x": 7.590949488888224,
+            "y": 0.8188871836416546,
+            "heading": 1.2363765500283435e-17,
+            "angularVelocity": -1.593638362048541e-16,
+            "velocityX": 1.321977451352309,
+            "velocityY": -0.4733733387389999,
+            "timestamp": 2.4906831228223316
+        },
+        {
+            "x": 7.626569692794441,
+            "y": 0.8061323103778162,
+            "heading": 7.88828312817125e-18,
+            "angularVelocity": -1.3287934794211324e-16,
+            "velocityX": 1.057581980498898,
+            "velocityY": -0.37869867794405154,
+            "timestamp": 2.5243639197373984
+        },
+        {
+            "x": 7.653284846152079,
+            "y": 0.7965661552766882,
+            "heading": 4.236479358871306e-18,
+            "angularVelocity": -1.0842391112822455e-16,
+            "velocityX": 0.7931864980809786,
+            "velocityY": -0.28402401300808766,
+            "timestamp": 2.558044716652465
+        },
+        {
+            "x": 7.671094948642455,
+            "y": 0.7901887184523844,
+            "heading": 1.249730071610374e-18,
+            "angularVelocity": -8.867810508445172e-17,
+            "velocityX": 0.5287910062011898,
+            "velocityY": -0.18934934468402032,
+            "timestamp": 2.591725513567532
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -2.906959217388984e-34,
+            "angularVelocity": -3.7105121777105e-17,
+            "velocityX": 0.26439550643651005,
+            "velocityY": -0.09467467353653367,
+            "timestamp": 2.6254063104825986
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -4.078160951605287e-34,
+            "angularVelocity": -3.477050502256718e-33,
+            "velocityX": 7.685636324015334e-33,
+            "velocityY": -2.3747808746440768e-32,
+            "timestamp": 2.6590871073976654
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Center To W1.traj b/src/main/deploy/choreo/Center To W1.traj
new file mode 100644
index 00000000..641af2c0
--- /dev/null
+++ b/src/main/deploy/choreo/Center To W1.traj	
@@ -0,0 +1,112 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": -1.3982338019676807e-32,
+            "velocityX": 1.9225496754980614e-32,
+            "velocityY": -8.567103847838605e-38,
+            "timestamp": 0
+        },
+        {
+            "x": 1.3816658535274804,
+            "y": 5.599900984360149,
+            "heading": 0.013082876743969601,
+            "angularVelocity": 0.16716651866961724,
+            "velocityX": 0.4046105912700521,
+            "velocityY": 0.509835013927756,
+            "timestamp": 0.07826254233257365
+        },
+        {
+            "x": 1.4449977124682947,
+            "y": 5.679702872674784,
+            "heading": 0.039246215534929935,
+            "angularVelocity": 0.3343021835373132,
+            "velocityX": 0.8092231232623148,
+            "velocityY": 1.0196690004717623,
+            "timestamp": 0.1565250846651473
+        },
+        {
+            "x": 1.5399958043003432,
+            "y": 5.799405527723277,
+            "heading": 0.07848745324353076,
+            "angularVelocity": 0.501405098007769,
+            "velocityX": 1.2138385618545031,
+            "velocityY": 1.5295012336785618,
+            "timestamp": 0.23478762699772096
+        },
+        {
+            "x": 1.6666604122853452,
+            "y": 5.959008727776544,
+            "heading": 0.1308059995871385,
+            "angularVelocity": 0.6685004701391144,
+            "velocityX": 1.6184576198246376,
+            "velocityY": 2.039330633740969,
+            "timestamp": 0.3130501693302946
+        },
+        {
+            "x": 1.8249918316073899,
+            "y": 6.158512135429991,
+            "heading": 0.19620604644996878,
+            "angularVelocity": 0.8356494040905814,
+            "velocityX": 2.023080449510844,
+            "velocityY": 2.5491557226145924,
+            "timestamp": 0.3913127116628683
+        },
+        {
+            "x": 1.9833278311922586,
+            "y": 6.358008426099132,
+            "heading": 0.26178707056088246,
+            "angularVelocity": 0.8379618417228061,
+            "velocityX": 2.023138973840461,
+            "velocityY": 2.549064785314409,
+            "timestamp": 0.469575253995442
+        },
+        {
+            "x": 2.109996681019138,
+            "y": 6.517605179473628,
+            "heading": 0.31426571552794874,
+            "angularVelocity": 0.6705461310477281,
+            "velocityX": 1.618511819979031,
+            "velocityY": 2.039248261272889,
+            "timestamp": 0.5478377963280157
+        },
+        {
+            "x": 2.204998332006287,
+            "y": 6.637302612224517,
+            "heading": 0.35363163522076196,
+            "angularVelocity": 0.5029982226430836,
+            "velocityX": 1.2138840389754155,
+            "velocityY": 1.5294345057465644,
+            "timestamp": 0.6261003386605893
+        },
+        {
+            "x": 2.268332773183591,
+            "y": 6.717100865272008,
+            "heading": 0.3798775217934225,
+            "angularVelocity": 0.3353569382033373,
+            "velocityX": 0.8092561178011136,
+            "velocityY": 1.0196225508288703,
+            "timestamp": 0.704362880993163
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0.1676725265429579,
+            "velocityX": 0.40462813847574725,
+            "velocityY": 0.509811380244222,
+            "timestamp": 0.7826254233257367
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 9.092584352802723e-32,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0.8608879656583104
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Center To W2.traj b/src/main/deploy/choreo/Center To W2.traj
new file mode 100644
index 00000000..bb0e9680
--- /dev/null
+++ b/src/main/deploy/choreo/Center To W2.traj	
@@ -0,0 +1,76 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 7.039472547405692e-42,
+            "angularVelocity": 1.0705191064262213e-40,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 1.4250000045295101,
+            "y": 5.56,
+            "heading": 8.194631828276944e-23,
+            "angularVelocity": 8.6403876700238e-22,
+            "velocityX": 0.7907971475606789,
+            "velocityY": 8.424071056371489e-35,
+            "timestamp": 0.0948410155004449
+        },
+        {
+            "x": 1.5750000090590195,
+            "y": 5.56,
+            "heading": 2.6954329855279996e-22,
+            "angularVelocity": 1.978015178210688e-21,
+            "velocityX": 1.5815942473623736,
+            "velocityY": 1.2488550168110521e-34,
+            "timestamp": 0.1896820310008898
+        },
+        {
+            "x": 1.7999999999999998,
+            "y": 5.56,
+            "heading": 5.87848829010012e-22,
+            "angularVelocity": 3.3562006983676933e-21,
+            "velocityX": 2.3723912038871497,
+            "velocityY": 2.135553914770072e-34,
+            "timestamp": 0.2845230465013347
+        },
+        {
+            "x": 2.0249999909409806,
+            "y": 5.56,
+            "heading": 4.475372520266276e-22,
+            "angularVelocity": -1.4794393741169351e-21,
+            "velocityX": 2.37239120388715,
+            "velocityY": 2.583104828416982e-34,
+            "timestamp": 0.3793640620017796
+        },
+        {
+            "x": 2.17499999547049,
+            "y": 5.56,
+            "heading": 1.6184867431561565e-22,
+            "angularVelocity": -3.012289189189882e-21,
+            "velocityX": 1.5815942473623736,
+            "velocityY": 1.9272829246954695e-34,
+            "timestamp": 0.47420507750222446
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 1.246024226890136e-41,
+            "angularVelocity": -1.7065260802739357e-21,
+            "velocityX": 0.7907971475606789,
+            "velocityY": -3.1542839688982834e-34,
+            "timestamp": 0.5690460930026694
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 7.131086050293936e-41,
+            "angularVelocity": 6.207915462550707e-40,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0.6638871085031143
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Center To W3.traj b/src/main/deploy/choreo/Center To W3.traj
new file mode 100644
index 00000000..aceaac81
--- /dev/null
+++ b/src/main/deploy/choreo/Center To W3.traj	
@@ -0,0 +1,112 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 5.56203910119815e-35,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 1.3816658774685813,
+            "y": 5.519999060819193,
+            "heading": -0.012983997100427386,
+            "angularVelocity": -0.16577951920330813,
+            "velocityX": 0.4043095436088268,
+            "velocityY": -0.5107315115509736,
+            "timestamp": 0.07832087559926142
+        },
+        {
+            "x": 1.4449977779940397,
+            "y": 5.439997258819368,
+            "heading": -0.038949622534069785,
+            "angularVelocity": -0.33152879401526647,
+            "velocityX": 0.8086209460872757,
+            "velocityY": -1.0214620481155445,
+            "timestamp": 0.15664175119852283
+        },
+        {
+            "x": 1.5399959199593471,
+            "y": 5.3199947242673264,
+            "heading": -0.07789432529768456,
+            "angularVelocity": -0.497245497648426,
+            "velocityX": 1.2129351368768808,
+            "velocityY": -1.5321909214351659,
+            "timestamp": 0.23496262679778424
+        },
+        {
+            "x": 1.6666605759505058,
+            "y": 5.159991667949077,
+            "heading": -0.1298174067167751,
+            "angularVelocity": -0.662953280613991,
+            "velocityX": 1.6172528080412967,
+            "velocityY": -2.042917103441561,
+            "timestamp": 0.31328350239704567
+        },
+        {
+            "x": 1.8249920311985446,
+            "y": 4.959988411604181,
+            "heading": -0.19472272142711608,
+            "angularVelocity": -0.828710279522869,
+            "velocityX": 2.0215741210320677,
+            "velocityY": -2.5536391774810774,
+            "timestamp": 0.3916043779963071
+        },
+        {
+            "x": 1.9833279596837488,
+            "y": 4.759991951944026,
+            "heading": -0.259798921868918,
+            "angularVelocity": -0.8308921464919682,
+            "velocityX": 2.021631235270526,
+            "velocityY": -2.553552397491847,
+            "timestamp": 0.4699252535955685
+        },
+        {
+            "x": 2.1099967561163506,
+            "y": 4.599995051908685,
+            "heading": -0.3118731002379449,
+            "angularVelocity": -0.6648824846579983,
+            "velocityX": 1.6173056731479674,
+            "velocityY": -2.042838500095239,
+            "timestamp": 0.54824612919483
+        },
+        {
+            "x": 2.2049983692603425,
+            "y": 4.4799975041635465,
+            "heading": -0.3509353765811358,
+            "angularVelocity": -0.4987466757018699,
+            "velocityX": 1.2129794568446812,
+            "velocityY": -1.532127249944463,
+            "timestamp": 0.6265670047940913
+        },
+        {
+            "x": 2.268332785802464,
+            "y": 4.399999173265512,
+            "heading": -0.3769787554180828,
+            "angularVelocity": -0.33252154853581545,
+            "velocityX": 0.808653070557838,
+            "velocityY": -1.021417729129522,
+            "timestamp": 0.7048878803933527
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.16625509460009477,
+            "velocityX": 0.40432661094808764,
+            "velocityY": -0.5107089643656818,
+            "timestamp": 0.7832087559926141
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -5.703180031220666e-32,
+            "velocityX": 1.0257416304055705e-32,
+            "velocityY": 0,
+            "timestamp": 0.8615296315918755
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S1 To C1.traj b/src/main/deploy/choreo/S1 To C1.traj
new file mode 100644
index 00000000..66955466
--- /dev/null
+++ b/src/main/deploy/choreo/S1 To C1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.238209882810899,
+            "y": 6.650800869780916,
+            "heading": 0.2470077179050236,
+            "angularVelocity": -0.14636941117769697,
+            "velocityX": 0.7008486923098889,
+            "velocityY": 0.23433997503541223,
+            "timestamp": 0.08876364255722656
+        },
+        {
+            "x": 5.362652939631404,
+            "y": 6.692404642961414,
+            "heading": 0.22258646311132924,
+            "angularVelocity": -0.2751267759001645,
+            "velocityX": 1.401959780326283,
+            "velocityY": 0.4687028605622118,
+            "timestamp": 0.17752728511445312
+        },
+        {
+            "x": 5.549362223961953,
+            "y": 6.754812642679732,
+            "heading": 0.18937073322571307,
+            "angularVelocity": -0.37420422291202055,
+            "velocityX": 2.1034432449090223,
+            "velocityY": 0.7030806523975865,
+            "timestamp": 0.26629092767167967
+        },
+        {
+            "x": 5.798381050208957,
+            "y": 6.8380191875714225,
+            "heading": 0.15275465831989177,
+            "angularVelocity": -0.4125120809759026,
+            "velocityX": 2.8054146841303256,
+            "velocityY": 0.9373944386978721,
+            "timestamp": 0.35505457022890624
+        },
+        {
+            "x": 6.109622945204874,
+            "y": 6.941929047341865,
+            "heading": 0.1302624797347834,
+            "angularVelocity": -0.25339404667863696,
+            "velocityX": 3.506411927553799,
+            "velocityY": 1.1706353725121932,
+            "timestamp": 0.4438182127861328
+        },
+        {
+            "x": 6.4280450770381,
+            "y": 7.048515369175043,
+            "heading": 0.1302624686930749,
+            "angularVelocity": -1.2439475582812342e-7,
+            "velocityX": 3.587303570016191,
+            "velocityY": 1.2007880564873126,
+            "timestamp": 0.5325818553433593
+        },
+        {
+            "x": 6.746467208870004,
+            "y": 7.1551016910121685,
+            "heading": 0.13026245765136438,
+            "angularVelocity": -1.2439477842907252e-7,
+            "velocityX": 3.5873035700012967,
+            "velocityY": 1.2007880565318134,
+            "timestamp": 0.6213454979005859
+        },
+        {
+            "x": 7.057671454315644,
+            "y": 7.258999412253756,
+            "heading": 0.10797351007767278,
+            "angularVelocity": -0.2511044717394208,
+            "velocityX": 3.5059877724700192,
+            "velocityY": 1.1704986213770205,
+            "timestamp": 0.7101091404578125
+        },
+        {
+            "x": 7.3066745305511045,
+            "y": 7.342200909102172,
+            "heading": 0.07025614763700679,
+            "angularVelocity": -0.4249190474112408,
+            "velocityX": 2.8052372464872497,
+            "velocityY": 0.9373375680789704,
+            "timestamp": 0.7988727830150391
+        },
+        {
+            "x": 7.49336787770315,
+            "y": 7.4046033027166684,
+            "heading": 0.03585076588111199,
+            "angularVelocity": -0.3876066908033862,
+            "velocityX": 2.103263698666567,
+            "velocityY": 0.7030174947390144,
+            "timestamp": 0.8876364255722656
+        },
+        {
+            "x": 7.617797738081694,
+            "y": 7.446202129881464,
+            "heading": 0.010501179707511226,
+            "angularVelocity": -0.2855852401136722,
+            "velocityX": 1.4018111108759532,
+            "velocityY": 0.4686471393732002,
+            "timestamp": 0.9764000681294922
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -0.003,
+            "angularVelocity": -0.15210258748116404,
+            "velocityX": 0.7007628362953491,
+            "velocityY": 0.23430618121741137,
+            "timestamp": 1.0651637106867187
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -0.003,
+            "angularVelocity": -3.7910464085672074e-31,
+            "velocityX": 1.2889893239032156e-32,
+            "velocityY": 1.1630630989344696e-31,
+            "timestamp": 1.1539273532439451
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S1 To C2.traj b/src/main/deploy/choreo/S1 To C2.traj
new file mode 100644
index 00000000..63556f1f
--- /dev/null
+++ b/src/main/deploy/choreo/S1 To C2.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.242063425541118,
+            "y": 6.625917379024557,
+            "heading": 0.21680591849920633,
+            "angularVelocity": -0.48090771817524164,
+            "velocityX": 0.735526954803639,
+            "velocityY": -0.04545446665971497,
+            "timestamp": 0.08981781715770647
+        },
+        {
+            "x": 5.374390185654174,
+            "y": 6.616899293663565,
+            "heading": 0.13592925245739512,
+            "angularVelocity": -0.9004523668149724,
+            "velocityX": 1.473279626476675,
+            "velocityY": -0.10040419202302918,
+            "timestamp": 0.17963563431541293
+        },
+        {
+            "x": 5.573225936717345,
+            "y": 6.601485332040172,
+            "heading": 0.027174297785337913,
+            "angularVelocity": -1.2108394315695736,
+            "velocityX": 2.2137673499017074,
+            "velocityY": -0.1716136298027423,
+            "timestamp": 0.2694534514731194
+        },
+        {
+            "x": 5.8386201791233034,
+            "y": 6.576536765329541,
+            "heading": -0.0884788299359431,
+            "angularVelocity": -1.287641265186969,
+            "velocityX": 2.9548061932964313,
+            "velocityY": -0.27776857087079165,
+            "timestamp": 0.35927126863082587
+        },
+        {
+            "x": 6.16462893780028,
+            "y": 6.53182752327052,
+            "heading": -0.12460373299787371,
+            "angularVelocity": -0.4022019706680331,
+            "velocityX": 3.6296669078981743,
+            "velocityY": -0.49777698316268804,
+            "timestamp": 0.44908908578853235
+        },
+        {
+            "x": 6.485747891959683,
+            "y": 6.420787077377064,
+            "heading": -0.12460373462910332,
+            "angularVelocity": -1.816153684215185e-8,
+            "velocityX": 3.5752255434527758,
+            "velocityY": -1.2362852873443355,
+            "timestamp": 0.5389069029462388
+        },
+        {
+            "x": 6.7868856142210525,
+            "y": 6.263423057279399,
+            "heading": -0.12460372221510443,
+            "angularVelocity": 1.382130993675135e-7,
+            "velocityX": 3.352761532075727,
+            "velocityY": -1.7520356770790513,
+            "timestamp": 0.6287247201039452
+        },
+        {
+            "x": 7.0844936481565455,
+            "y": 6.108042080528118,
+            "heading": -0.11377963349551397,
+            "angularVelocity": 0.12051159850149781,
+            "velocityX": 3.313463223148008,
+            "velocityY": -1.7299571696165401,
+            "timestamp": 0.7185425372616516
+        },
+        {
+            "x": 7.322771112714566,
+            "y": 5.983596379959823,
+            "heading": -0.07830444960996434,
+            "angularVelocity": 0.39496822577262786,
+            "velocityX": 2.652897521876331,
+            "velocityY": -1.3855346801602506,
+            "timestamp": 0.808360354419358
+        },
+        {
+            "x": 7.501421172621153,
+            "y": 5.8902827497271355,
+            "heading": -0.04357887494407857,
+            "angularVelocity": 0.3866223402525239,
+            "velocityX": 1.9890269610193678,
+            "velocityY": -1.038921153793384,
+            "timestamp": 0.8981781715770645
+        },
+        {
+            "x": 7.62048387761172,
+            "y": 5.828089668977501,
+            "heading": -0.01724918093308134,
+            "angularVelocity": 0.29314555668577735,
+            "velocityX": 1.3256022998366965,
+            "velocityY": -0.6924358965486016,
+            "timestamp": 0.9879959887347709
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -0.003,
+            "angularVelocity": 0.15864537108558247,
+            "velocityX": 0.6626315832612425,
+            "velocityY": -0.346141444552285,
+            "timestamp": 1.0778138058924773
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -0.003,
+            "angularVelocity": 0,
+            "velocityX": 4.386752331989148e-33,
+            "velocityY": -2.33652586669764e-33,
+            "timestamp": 1.1676316230501838
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S1 To C3.traj b/src/main/deploy/choreo/S1 To C3.traj
new file mode 100644
index 00000000..b1b40339
--- /dev/null
+++ b/src/main/deploy/choreo/S1 To C3.traj	
@@ -0,0 +1,157 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.246662494261999,
+            "y": 6.628692732737936,
+            "heading": 0.18398762968545185,
+            "angularVelocity": -0.8088463398355284,
+            "velocityX": 0.7519183997414132,
+            "velocityY": -0.013910608703967415,
+            "timestamp": 0.09397628025368156
+        },
+        {
+            "x": 5.3885394117424426,
+            "y": 6.6228569584156665,
+            "heading": 0.04133864379940616,
+            "angularVelocity": -1.5179254328962157,
+            "velocityX": 1.509709866122152,
+            "velocityY": -0.0620983753189278,
+            "timestamp": 0.18795256050736311
+        },
+        {
+            "x": 5.602094427902648,
+            "y": 6.606624752166298,
+            "heading": -0.14949344735311892,
+            "angularVelocity": -2.0306410366252936,
+            "velocityX": 2.272435295201409,
+            "velocityY": -0.172726630651381,
+            "timestamp": 0.2819288407610447
+        },
+        {
+            "x": 5.885007714232348,
+            "y": 6.565697735942791,
+            "heading": -0.3456589598447775,
+            "angularVelocity": -2.087393882393782,
+            "velocityX": 3.010475468554385,
+            "velocityY": -0.4355036836213049,
+            "timestamp": 0.37590512101472623
+        },
+        {
+            "x": 6.196762938930662,
+            "y": 6.464967292979978,
+            "heading": -0.4305176558630451,
+            "angularVelocity": -0.9029799412064233,
+            "velocityX": 3.3173820442430326,
+            "velocityY": -1.0718709305252299,
+            "timestamp": 0.4698814012684078
+        },
+        {
+            "x": 6.507653773495931,
+            "y": 6.294193106326001,
+            "heading": -0.4331106719684992,
+            "angularVelocity": -0.027592240280786797,
+            "velocityX": 3.3081840835372915,
+            "velocityY": -1.8172052159649787,
+            "timestamp": 0.5638576815220894
+        },
+        {
+            "x": 6.777249226823148,
+            "y": 6.0624534109685,
+            "heading": -0.433110646067504,
+            "angularVelocity": 2.7561204856502443e-7,
+            "velocityX": 2.8687606340606973,
+            "velocityY": -2.465938157287546,
+            "timestamp": 0.6578339617757709
+        },
+        {
+            "x": 6.99331744885121,
+            "y": 5.780142381909384,
+            "heading": -0.4331105937998759,
+            "angularVelocity": 5.561789419211182e-7,
+            "velocityX": 2.2991782761011916,
+            "velocityY": -3.004066859180208,
+            "timestamp": 0.7518102420294525
+        },
+        {
+            "x": 7.1465883664001515,
+            "y": 5.459372959434879,
+            "heading": -0.43311054984028735,
+            "angularVelocity": 4.677732342413875e-7,
+            "velocityX": 1.6309532270823943,
+            "velocityY": -3.4133019694822235,
+            "timestamp": 0.845786522283134
+        },
+        {
+            "x": 7.2797482024400875,
+            "y": 5.1297467242247805,
+            "heading": -0.4331105067287741,
+            "angularVelocity": 4.5874887908824285e-7,
+            "velocityX": 1.4169515507581365,
+            "velocityY": -3.507547163180951,
+            "timestamp": 0.9397628025368155
+        },
+        {
+            "x": 7.407925031698652,
+            "y": 4.799268903667217,
+            "heading": -0.4305589051420175,
+            "angularVelocity": 0.027151549091629798,
+            "velocityX": 1.3639274603395894,
+            "velocityY": -3.5166088683810885,
+            "timestamp": 1.033739082790497
+        },
+        {
+            "x": 7.517390810231398,
+            "y": 4.529437032753176,
+            "heading": -0.29559925755962413,
+            "angularVelocity": 1.4361033147734772,
+            "velocityX": 1.164823487770018,
+            "velocityY": -2.871276349581525,
+            "timestamp": 1.1277153630441787
+        },
+        {
+            "x": 7.59899827381295,
+            "y": 4.327770783855267,
+            "heading": -0.16069313013903275,
+            "angularVelocity": 1.4355338076419173,
+            "velocityX": 0.8683836321384404,
+            "velocityY": -2.1459271249460685,
+            "timestamp": 1.2216916432978604
+        },
+        {
+            "x": 7.653087401223331,
+            "y": 4.193799869992724,
+            "heading": -0.05829734753961687,
+            "angularVelocity": 1.0895917812772173,
+            "velocityX": 0.5755614849233384,
+            "velocityY": -1.4255822160751557,
+            "timestamp": 1.315667923551542
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -0.003,
+            "angularVelocity": 0.5884181347713066,
+            "velocityX": 0.28637650590148805,
+            "velocityY": -0.7108162805806236,
+            "timestamp": 1.4096442038052237
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -0.003,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.8051389340682807e-33,
+            "timestamp": 1.5036204840589054
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S1 To C4.traj b/src/main/deploy/choreo/S1 To C4.traj
new file mode 100644
index 00000000..3f479acb
--- /dev/null
+++ b/src/main/deploy/choreo/S1 To C4.traj	
@@ -0,0 +1,247 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": 0,
+            "velocityX": -1.7123315632321391e-31,
+            "velocityY": 4.571975133912053e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 5.2081944432473115,
+            "y": 6.63354239454729,
+            "heading": 0.2524699257138384,
+            "angularVelocity": -0.12069194203891866,
+            "velocityX": 0.5160121574790498,
+            "velocityY": 0.0567774581143745,
+            "timestamp": 0.062390861883169
+        },
+        {
+            "x": 5.2726441787772975,
+            "y": 6.640106281427696,
+            "heading": 0.23785051964847434,
+            "angularVelocity": -0.2343196683632906,
+            "velocityX": 1.0329996025807835,
+            "velocityY": 0.1052059016703473,
+            "timestamp": 0.124781723766338
+        },
+        {
+            "x": 5.369418980987113,
+            "y": 6.648966513737627,
+            "heading": 0.21675536507543205,
+            "angularVelocity": -0.33811288923278293,
+            "velocityX": 1.551105390898958,
+            "velocityY": 0.14201169918963874,
+            "timestamp": 0.187172585649507
+        },
+        {
+            "x": 5.498590594297912,
+            "y": 6.659046571836944,
+            "heading": 0.19009632617593986,
+            "angularVelocity": -0.42729076173705777,
+            "velocityX": 2.070361097955043,
+            "velocityY": 0.16156305258605722,
+            "timestamp": 0.249563447532676
+        },
+        {
+            "x": 5.6601948909824875,
+            "y": 6.668590441412665,
+            "heading": 0.15936442465931713,
+            "angularVelocity": -0.49257055583183496,
+            "velocityX": 2.5901917653772815,
+            "velocityY": 0.15296902924013772,
+            "timestamp": 0.311954309415845
+        },
+        {
+            "x": 5.854024302844021,
+            "y": 6.674275452040902,
+            "heading": 0.12740102584180835,
+            "angularVelocity": -0.512308980077189,
+            "velocityX": 3.106695532183737,
+            "velocityY": 0.09111928344386595,
+            "timestamp": 0.374345171299014
+        },
+        {
+            "x": 6.078018113514707,
+            "y": 6.668089747759068,
+            "heading": 0.10126004011963459,
+            "angularVelocity": -0.4189874115076864,
+            "velocityX": 3.590170161299073,
+            "velocityY": -0.09914439543117176,
+            "timestamp": 0.436736033182183
+        },
+        {
+            "x": 6.311203816285634,
+            "y": 6.63161682590639,
+            "heading": 0.10125992071184643,
+            "angularVelocity": -0.000001913866624365403,
+            "velocityX": 3.737497699704501,
+            "velocityY": -0.584587562213517,
+            "timestamp": 0.499126895065352
+        },
+        {
+            "x": 6.53718081427167,
+            "y": 6.563497401833266,
+            "heading": 0.10125989664992657,
+            "angularVelocity": -3.856641685769623e-7,
+            "velocityX": 3.621956664249834,
+            "velocityY": -1.0918173273625358,
+            "timestamp": 0.561517756948521
+        },
+        {
+            "x": 6.7516755548198235,
+            "y": 6.465019697743108,
+            "heading": 0.10125985817262735,
+            "angularVelocity": -6.167136938553595e-7,
+            "velocityX": 3.4379191771674527,
+            "velocityY": -1.5783994821960954,
+            "timestamp": 0.62390861883169
+        },
+        {
+            "x": 6.950631618499756,
+            "y": 6.338046073913574,
+            "heading": 0.10125971935519859,
+            "angularVelocity": -0.000002224964167076665,
+            "velocityX": 3.1888654471946203,
+            "velocityY": -2.035131748416977,
+            "timestamp": 0.686299480714859
+        },
+        {
+            "x": 7.196058677643184,
+            "y": 6.101503366378845,
+            "heading": 0.10125966337035743,
+            "angularVelocity": -6.213285755242982e-7,
+            "velocityX": 2.723788105663008,
+            "velocityY": -2.625188174087521,
+            "timestamp": 0.7764045291337008
+        },
+        {
+            "x": 7.389899318639429,
+            "y": 5.821123760827136,
+            "heading": 0.10125964077104323,
+            "angularVelocity": -2.508107435019506e-7,
+            "velocityX": 2.1512739230237647,
+            "velocityY": -3.1116969633976366,
+            "timestamp": 0.8665095775525427
+        },
+        {
+            "x": 7.524507781478929,
+            "y": 5.507966472295818,
+            "heading": 0.101259626680438,
+            "angularVelocity": -1.5637975290365853e-7,
+            "velocityX": 1.4939058931947202,
+            "velocityY": -3.475468844716046,
+            "timestamp": 0.9566146259713845
+        },
+        {
+            "x": 7.594574766806078,
+            "y": 5.174383579102459,
+            "heading": 0.10125961555168732,
+            "angularVelocity": -1.235086255399969e-7,
+            "velocityX": 0.7776144240159958,
+            "velocityY": -3.702155418004344,
+            "timestamp": 1.0467196743902263
+        },
+        {
+            "x": 7.605242469312774,
+            "y": 4.833688510035603,
+            "heading": 0.10125960514477245,
+            "angularVelocity": -1.1549757817967306e-7,
+            "velocityX": 0.11839184034432584,
+            "velocityY": -3.781087464524514,
+            "timestamp": 1.136824722809068
+        },
+        {
+            "x": 7.615908675824772,
+            "y": 4.492993394135096,
+            "heading": 0.10125959473787158,
+            "angularVelocity": -1.1549742264017462e-7,
+            "velocityX": 0.11837523756082047,
+            "velocityY": -3.781087984291716,
+            "timestamp": 1.2269297712279097
+        },
+        {
+            "x": 7.6265748461946865,
+            "y": 4.152298277103123,
+            "heading": 0.10125958433097336,
+            "angularVelocity": -1.1549739343149193e-7,
+            "velocityX": 0.11837483645025074,
+            "velocityY": -3.781087996848918,
+            "timestamp": 1.3170348196467514
+        },
+        {
+            "x": 7.637240986091898,
+            "y": 3.8116031591171406,
+            "heading": 0.10125957392407539,
+            "angularVelocity": -1.1549739036497336e-7,
+            "velocityX": 0.11837449825941278,
+            "velocityY": -3.78108800743665,
+            "timestamp": 1.407139868065593
+        },
+        {
+            "x": 7.647907101492937,
+            "y": 3.470908040364265,
+            "heading": 0.10125956351717343,
+            "angularVelocity": -1.1549743491773867e-7,
+            "velocityX": 0.11837422639702805,
+            "velocityY": -3.781088015947766,
+            "timestamp": 1.4972449164844348
+        },
+        {
+            "x": 7.658649980665503,
+            "y": 3.1330359890940187,
+            "heading": 0.09207038783752043,
+            "angularVelocity": -0.10198291706072676,
+            "velocityX": 0.11922616280763662,
+            "velocityY": -3.7497571689844853,
+            "timestamp": 1.5873499649032765
+        },
+        {
+            "x": 7.667206120655169,
+            "y": 2.862563871388905,
+            "heading": 0.061604548821908564,
+            "angularVelocity": -0.33811467337536444,
+            "velocityX": 0.09495738740298482,
+            "velocityY": -3.0017421049246735,
+            "timestamp": 1.6774550133221182
+        },
+        {
+            "x": 7.673608699461006,
+            "y": 2.659753859006467,
+            "heading": 0.031813701428875164,
+            "angularVelocity": -0.33062351017837394,
+            "velocityX": 0.07105682665061783,
+            "velocityY": -2.250817417462574,
+            "timestamp": 1.76756006174096
+        },
+        {
+            "x": 7.677870998153504,
+            "y": 2.524576540197495,
+            "heading": 0.009226184845951672,
+            "angularVelocity": -0.25067981183391225,
+            "velocityX": 0.04730366130750142,
+            "velocityY": -1.5002191462193974,
+            "timestamp": 1.8576651101598016
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -0.003,
+            "angularVelocity": -0.1356881224803119,
+            "velocityX": 0.023627997363707604,
+            "velocityY": -0.7499750722442785,
+            "timestamp": 1.9477701585786433
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -0.003,
+            "angularVelocity": 0,
+            "velocityX": -1.899719241029588e-32,
+            "velocityY": -4.254972095731821e-34,
+            "timestamp": 2.037875206997485
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S1 To C5.traj b/src/main/deploy/choreo/S1 To C5.traj
new file mode 100644
index 00000000..7f9f6ab6
--- /dev/null
+++ b/src/main/deploy/choreo/S1 To C5.traj	
@@ -0,0 +1,292 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": -8.010592837331029e-32,
+            "velocityX": 9.396271853657066e-34,
+            "velocityY": -2.4298772375808502e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 5.207063492876026,
+            "y": 6.6289176408913075,
+            "heading": 0.24374543730705361,
+            "angularVelocity": -0.264834788594711,
+            "velocityX": 0.5061159579768882,
+            "velocityY": -0.01763482359684202,
+            "timestamp": 0.06137623678217253
+        },
+        {
+            "x": 5.269220913885746,
+            "y": 6.626580685647671,
+            "heading": 0.2121773997323976,
+            "angularVelocity": -0.514336479877237,
+            "velocityX": 1.0127277961065038,
+            "velocityY": -0.03807589657100854,
+            "timestamp": 0.12275247356434506
+        },
+        {
+            "x": 5.36250981468421,
+            "y": 6.6227475288112485,
+            "heading": 0.16662318019476377,
+            "angularVelocity": -0.7422126530713462,
+            "velocityX": 1.5199514615005028,
+            "velocityY": -0.06245343535846346,
+            "timestamp": 0.1841287103465176
+        },
+        {
+            "x": 5.486975524936631,
+            "y": 6.617054798693029,
+            "heading": 0.10910232843404971,
+            "angularVelocity": -0.9371844019185364,
+            "velocityX": 2.0279136808950478,
+            "velocityY": -0.0927513711605005,
+            "timestamp": 0.24550494712869012
+        },
+        {
+            "x": 5.642662315095453,
+            "y": 6.608892934358269,
+            "heading": 0.04300979987381644,
+            "angularVelocity": -1.076842309423355,
+            "velocityX": 2.536597196588689,
+            "velocityY": -0.1329808532205177,
+            "timestamp": 0.30688118391086266
+        },
+        {
+            "x": 5.829535978248522,
+            "y": 6.597025931717244,
+            "heading": -0.024847542112422006,
+            "angularVelocity": -1.1055963275666998,
+            "velocityX": 3.0447233807490215,
+            "velocityY": -0.19334848897856358,
+            "timestamp": 0.3682574206930352
+        },
+        {
+            "x": 6.046434546533161,
+            "y": 6.5777851666622515,
+            "heading": -0.07267029178585922,
+            "angularVelocity": -0.7791737027338695,
+            "velocityX": 3.533917679808606,
+            "velocityY": -0.31348883645767445,
+            "timestamp": 0.4296336574752077
+        },
+        {
+            "x": 6.274079716361941,
+            "y": 6.532107529947078,
+            "heading": -0.07267036448034916,
+            "angularVelocity": -0.0000011844077407082938,
+            "velocityX": 3.709011528952235,
+            "velocityY": -0.7442234830605138,
+            "timestamp": 0.49100989425738023
+        },
+        {
+            "x": 6.493476634626264,
+            "y": 6.456122214314626,
+            "heading": -0.07267035803800323,
+            "angularVelocity": 1.049648246960038e-7,
+            "velocityX": 3.5746231728571716,
+            "velocityY": -1.238025001469686,
+            "timestamp": 0.5523861310395528
+        },
+        {
+            "x": 6.700610073343003,
+            "y": 6.351219816335689,
+            "heading": -0.07267035070446132,
+            "angularVelocity": 1.1948503655702063e-7,
+            "velocityX": 3.3748149051866063,
+            "velocityY": -1.7091695985083282,
+            "timestamp": 0.6137623678217253
+        },
+        {
+            "x": 6.891689244621814,
+            "y": 6.21932014936447,
+            "heading": -0.07267034149477326,
+            "angularVelocity": 1.5005299351249212e-7,
+            "velocityX": 3.1132435173071933,
+            "velocityY": -2.1490347712151214,
+            "timestamp": 0.6751386046038979
+        },
+        {
+            "x": 7.0632171630859375,
+            "y": 6.06283712387085,
+            "heading": -0.07267032569899272,
+            "angularVelocity": 2.573598736854518e-7,
+            "velocityX": 2.7946959190881038,
+            "velocityY": -2.549570219643589,
+            "timestamp": 0.7365148413860704
+        },
+        {
+            "x": 7.263317671759933,
+            "y": 5.7907628781558955,
+            "heading": -0.07267031513885969,
+            "angularVelocity": 1.182832965996254e-7,
+            "velocityX": 2.24131152203967,
+            "velocityY": -3.0474842158680824,
+            "timestamp": 0.8257931530827405
+        },
+        {
+            "x": 7.406264443711533,
+            "y": 5.484771313458312,
+            "heading": -0.07267030869611313,
+            "angularVelocity": 7.216474459034548e-8,
+            "velocityX": 1.6011365944875051,
+            "velocityY": -3.4273896860551396,
+            "timestamp": 0.9150714647794106
+        },
+        {
+            "x": 7.486522259331217,
+            "y": 5.1567114044359705,
+            "heading": -0.07267030368970051,
+            "angularVelocity": 5.607647052205946e-8,
+            "velocityX": 0.8989620669840581,
+            "velocityY": -3.6745756364317224,
+            "timestamp": 1.0043497764760807
+        },
+        {
+            "x": 7.50149841248159,
+            "y": 4.81930906784098,
+            "heading": -0.07267029905812865,
+            "angularVelocity": 5.187790600157932e-8,
+            "velocityX": 0.1677468230047605,
+            "velocityY": -3.7792195011633143,
+            "timestamp": 1.093628088172751
+        },
+        {
+            "x": 7.516449925771596,
+            "y": 4.481905638472984,
+            "heading": -0.0726702944266145,
+            "angularVelocity": 5.1877259457701335e-8,
+            "velocityX": 0.16747083368699095,
+            "velocityY": -3.7792317412357557,
+            "timestamp": 1.1829063998694211
+        },
+        {
+            "x": 7.531401402222247,
+            "y": 4.144502207472513,
+            "heading": -0.07267028979509418,
+            "angularVelocity": 5.187732873980297e-8,
+            "velocityX": 0.16747042105201052,
+            "velocityY": -3.779231759520992,
+            "timestamp": 1.2721847115660914
+        },
+        {
+            "x": 7.546352847973806,
+            "y": 3.807098775111667,
+            "heading": -0.07267028516356967,
+            "angularVelocity": 5.1877375603275234e-8,
+            "velocityX": 0.16747007719365256,
+            "velocityY": -3.7792317747584647,
+            "timestamp": 1.3614630232627616
+        },
+        {
+            "x": 7.561304268557873,
+            "y": 3.4696953416355707,
+            "heading": -0.07267028053204275,
+            "angularVelocity": 5.1877402813012475e-8,
+            "velocityX": 0.16746979529438435,
+            "velocityY": -3.7792317872503007,
+            "timestamp": 1.4507413349594318
+        },
+        {
+            "x": 7.576255668582546,
+            "y": 3.132291907248425,
+            "heading": -0.07267027590051456,
+            "angularVelocity": 5.1877416880821484e-8,
+            "velocityX": 0.16746956501007867,
+            "velocityY": -3.779231797454896,
+            "timestamp": 1.540019646656102
+        },
+        {
+            "x": 7.591207051730301,
+            "y": 2.794888472113414,
+            "heading": -0.07267027126898577,
+            "angularVelocity": 5.1877423431260937e-8,
+            "velocityX": 0.16746937597291248,
+            "velocityY": -3.779231805831691,
+            "timestamp": 1.6292979583527722
+        },
+        {
+            "x": 7.60615842090353,
+            "y": 2.457485036359151,
+            "heading": -0.07267026663745696,
+            "angularVelocity": 5.1877423851232326e-8,
+            "velocityX": 0.16746921944524512,
+            "velocityY": -3.779231812767884,
+            "timestamp": 1.7185762700494425
+        },
+        {
+            "x": 7.621109778388665,
+            "y": 2.1200816000869556,
+            "heading": -0.07267026200592838,
+            "angularVelocity": 5.187742117887202e-8,
+            "velocityX": 0.1674690885277524,
+            "velocityY": -3.779231818569208,
+            "timestamp": 1.8078545817461127
+        },
+        {
+            "x": 7.636061125621916,
+            "y": 1.7826781633604734,
+            "heading": -0.07267025737439563,
+            "angularVelocity": 5.1877468094637746e-8,
+            "velocityX": 0.16746897369713376,
+            "velocityY": -3.7792318236576405,
+            "timestamp": 1.897132893442783
+        },
+        {
+            "x": 7.6506850878742485,
+            "y": 1.4507667606163608,
+            "heading": -0.05536549896524321,
+            "angularVelocity": 0.19382936438075055,
+            "velocityX": 0.16380195788222543,
+            "velocityY": -3.7177159428351065,
+            "timestamp": 1.9864112051394531
+        },
+        {
+            "x": 7.662404359721407,
+            "y": 1.1852476637582392,
+            "heading": -0.0353858682020255,
+            "angularVelocity": 0.2237904187872895,
+            "velocityX": 0.13126672788097452,
+            "velocityY": -2.974060461181691,
+            "timestamp": 2.0756895168361233
+        },
+        {
+            "x": 7.6711999602966845,
+            "y": 0.9861189087552604,
+            "heading": -0.018423007440779238,
+            "angularVelocity": 0.18999979321832794,
+            "velocityX": 0.0985188945458782,
+            "velocityY": -2.2304269784976873,
+            "timestamp": 2.1649678285327933
+        },
+        {
+            "x": 7.677066105120016,
+            "y": 0.8533716446291538,
+            "heading": -0.006322956528169105,
+            "angularVelocity": 0.13553180702747777,
+            "velocityX": 0.06570626966225444,
+            "velocityY": -1.4868926349898441,
+            "timestamp": 2.2542461402294633
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -2.444948224573009e-31,
+            "angularVelocity": 0.0708229849781762,
+            "velocityX": 0.03286234724008497,
+            "velocityY": -0.7434240563896022,
+            "timestamp": 2.3435244519261333
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 1.883272219480887e-30,
+            "velocityX": 0,
+            "velocityY": -3.281240907044904e-32,
+            "timestamp": 2.4328027636228033
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S1 To W1.traj b/src/main/deploy/choreo/S1 To W1.traj
new file mode 100644
index 00000000..462787d1
--- /dev/null
+++ b/src/main/deploy/choreo/S1 To W1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": -5.95142511939464e-31,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.1029790507443495,
+            "y": 6.633224483607701,
+            "heading": 0.26650125878935643,
+            "angularVelocity": 0.06942713064122713,
+            "velocityX": -0.7797928290155524,
+            "velocityY": 0.034434353705267144,
+            "timestamp": 0.09364147314336706
+        },
+        {
+            "x": 4.9569371541735245,
+            "y": 6.639673450192742,
+            "heading": 0.2795035687162854,
+            "angularVelocity": 0.13885204376302582,
+            "velocityX": -1.559585637308702,
+            "velocityY": 0.06886870067889261,
+            "timestamp": 0.18728294628673411
+        },
+        {
+            "x": 4.737874314376445,
+            "y": 6.649346898931416,
+            "heading": 0.2990065710588269,
+            "angularVelocity": 0.20827312608258508,
+            "velocityX": -2.339378401936154,
+            "velocityY": 0.1033030388561254,
+            "timestamp": 0.28092441943010116
+        },
+        {
+            "x": 4.445790546320906,
+            "y": 6.66224482863892,
+            "heading": 0.3250097249050781,
+            "angularVelocity": 0.2776884319880837,
+            "velocityX": -3.119171006722006,
+            "velocityY": 0.13773736438080994,
+            "timestamp": 0.37456589257346823
+        },
+        {
+            "x": 4.09189529815297,
+            "y": 6.677872415392626,
+            "heading": 0.32500972789735083,
+            "angularVelocity": 3.195452790688951e-8,
+            "velocityX": -3.7792575905562438,
+            "velocityY": 0.16688745092443205,
+            "timestamp": 0.4682073657168353
+        },
+        {
+            "x": 3.7380000499890724,
+            "y": 6.693500002240241,
+            "heading": 0.32500973088947493,
+            "angularVelocity": 3.1952970747065184e-8,
+            "velocityX": -3.7792575905130557,
+            "velocityY": 0.16688745192729154,
+            "timestamp": 0.5618488388602023
+        },
+        {
+            "x": 3.384104801825175,
+            "y": 6.7091275890878554,
+            "heading": 0.3250097338815991,
+            "angularVelocity": 3.195297127919844e-8,
+            "velocityX": -3.7792575905130557,
+            "velocityY": 0.1668874519272919,
+            "timestamp": 0.6554903120035693
+        },
+        {
+            "x": 3.030209553657211,
+            "y": 6.724755175840924,
+            "heading": 0.32500973687387263,
+            "angularVelocity": 3.195453683514771e-8,
+            "velocityX": -3.7792575905565435,
+            "velocityY": 0.16688745091763021,
+            "timestamp": 0.7491317851469363
+        },
+        {
+            "x": 2.7381257502813003,
+            "y": 6.7376531044804615,
+            "heading": 0.35100602630019134,
+            "angularVelocity": 0.2776151266490911,
+            "velocityX": -3.119171383909283,
+            "velocityY": 0.13773735297596457,
+            "timestamp": 0.8427732582903034
+        },
+        {
+            "x": 2.5190628797280783,
+            "y": 6.747326551971013,
+            "heading": 0.37050304728737704,
+            "angularVelocity": 0.20820925101574836,
+            "velocityX": -2.3393787303818834,
+            "velocityY": 0.10330302552738789,
+            "timestamp": 0.9364147314336704
+        },
+        {
+            "x": 2.3730209608288053,
+            "y": 6.7537755172838185,
+            "heading": 0.3835010053218344,
+            "angularVelocity": 0.13880556977745576,
+            "velocityX": -1.559585875754842,
+            "velocityY": 0.06886868709264785,
+            "timestamp": 1.0300562045770374
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 0.06940295213233785,
+            "velocityX": -0.7797929526056125,
+            "velocityY": 0.03443434418469861,
+            "timestamp": 1.1236976777204044
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": -1.6940949553652625e-30,
+            "velocityX": -1.6586267397188983e-32,
+            "velocityY": 0,
+            "timestamp": 1.2173391508637714
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S1 To W2.traj b/src/main/deploy/choreo/S1 To W2.traj
new file mode 100644
index 00000000..53d96783
--- /dev/null
+++ b/src/main/deploy/choreo/S1 To W2.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 3.363577949177338e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 5.118924970919473,
+            "y": 6.609128397315466,
+            "heading": 0.25122660793754975,
+            "angularVelocity": -0.10272459100864365,
+            "velocityX": -0.6682716305586017,
+            "velocityY": -0.24437832416494054,
+            "timestamp": 0.08540693106008139
+        },
+        {
+            "x": 5.0047749070529814,
+            "y": 6.567385203608531,
+            "heading": 0.23368024533248913,
+            "angularVelocity": -0.20544424658833893,
+            "velocityX": -1.3365433279201937,
+            "velocityY": -0.48875651178205104,
+            "timestamp": 0.17081386212016278
+        },
+        {
+            "x": 4.833549798383878,
+            "y": 6.504770434941582,
+            "heading": 0.20736177398264571,
+            "angularVelocity": -0.30815381167752187,
+            "velocityX": -2.004815142563213,
+            "velocityY": -0.7331345113302418,
+            "timestamp": 0.25622079318024416
+        },
+        {
+            "x": 4.605249631158782,
+            "y": 6.421284110658292,
+            "heading": 0.17227269285622732,
+            "angularVelocity": -0.4108458258702004,
+            "velocityX": -2.6730871182397737,
+            "velocityY": -0.977512284390117,
+            "timestamp": 0.34162772424032556
+        },
+        {
+            "x": 4.319874394275373,
+            "y": 6.316926253811125,
+            "heading": 0.12841549400557886,
+            "angularVelocity": -0.5135086614902031,
+            "velocityX": -3.3413592239095435,
+            "velocityY": -1.2218897875367427,
+            "timestamp": 0.42703465530040696
+        },
+        {
+            "x": 4.016437653324315,
+            "y": 6.205962904892466,
+            "heading": 0.1284154887581796,
+            "angularVelocity": -6.143996998796484e-8,
+            "velocityX": -3.552835082407984,
+            "velocityY": -1.2992311928477889,
+            "timestamp": 0.5124415863604883
+        },
+        {
+            "x": 3.713000912388581,
+            "y": 6.094999555931864,
+            "heading": 0.12841548351080107,
+            "angularVelocity": -6.143972697353832e-8,
+            "velocityX": -3.5528350822285746,
+            "velocityY": -1.299231193338901,
+            "timestamp": 0.5978485174205697
+        },
+        {
+            "x": 3.4095641714528466,
+            "y": 5.98403620697126,
+            "heading": 0.12841547826342262,
+            "angularVelocity": -6.143972595662007e-8,
+            "velocityX": -3.5528350822285746,
+            "velocityY": -1.299231193338901,
+            "timestamp": 0.683255448480651
+        },
+        {
+            "x": 3.106127430499178,
+            "y": 5.873072858059743,
+            "heading": 0.12841547301602332,
+            "angularVelocity": -6.143997022352915e-8,
+            "velocityX": -3.55283508243856,
+            "velocityY": -1.2992311927641775,
+            "timestamp": 0.7686623795407324
+        },
+        {
+            "x": 2.820751661400481,
+            "y": 5.768715245814204,
+            "heading": 0.08460804498823243,
+            "angularVelocity": -0.5129259122655233,
+            "velocityX": -3.3413654554329186,
+            "velocityY": -1.2218869235814813,
+            "timestamp": 0.8540693106008137
+        },
+        {
+            "x": 2.5924510107246115,
+            "y": 5.685229149936981,
+            "heading": 0.04956396734437731,
+            "angularVelocity": -0.41031889577208425,
+            "velocityX": -2.6730927787964465,
+            "velocityY": -0.977509610063062,
+            "timestamp": 0.939476241660895
+        },
+        {
+            "x": 2.4212255084791887,
+            "y": 5.622614575599846,
+            "heading": 0.02328185696555881,
+            "angularVelocity": -0.30772807373595673,
+            "velocityX": -2.00481975081121,
+            "velocityY": -0.7331322359901695,
+            "timestamp": 1.0248831727209764
+        },
+        {
+            "x": 2.307075169405903,
+            "y": 5.580871525201533,
+            "heading": 0.005760683198102726,
+            "angularVelocity": -0.20514931926462068,
+            "velocityX": -1.3365465502206675,
+            "velocityY": -0.48875483383130996,
+            "timestamp": 1.1102901037810577
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -0.003,
+            "angularVelocity": -0.10257578734376759,
+            "velocityX": -0.6682732735795421,
+            "velocityY": -0.24437741694350015,
+            "timestamp": 1.195697034841139
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -0.003,
+            "angularVelocity": 2.4865352537032555e-34,
+            "velocityX": 0,
+            "velocityY": -6.568641934425214e-34,
+            "timestamp": 1.2811039659012204
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S1 To W3.traj b/src/main/deploy/choreo/S1 To W3.traj
new file mode 100644
index 00000000..359cb1db
--- /dev/null
+++ b/src/main/deploy/choreo/S1 To W3.traj	
@@ -0,0 +1,175 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": 2.521971410376041e-31,
+            "velocityX": -3.46085485178236e-32,
+            "velocityY": -2.8734015412732535e-37,
+            "timestamp": 0
+        },
+        {
+            "x": 5.132269964046874,
+            "y": 6.606195610268081,
+            "heading": 0.16914444930535336,
+            "angularVelocity": -1.1148212857556328,
+            "velocityX": -0.5365789380469493,
+            "velocityY": -0.29208606589986635,
+            "timestamp": 0.08149786145594107
+        },
+        {
+            "x": 5.043666795437562,
+            "y": 6.5570487224925476,
+            "heading": 0.005335724612659364,
+            "angularVelocity": -2.0099757437347154,
+            "velocityX": -1.0871839705537814,
+            "velocityY": -0.6030451216453527,
+            "timestamp": 0.16299572291188213
+        },
+        {
+            "x": 4.909391168326261,
+            "y": 6.4806746643853055,
+            "heading": -0.2131663273161798,
+            "angularVelocity": -2.681077123072297,
+            "velocityX": -1.647596939508567,
+            "velocityY": -0.9371295975481627,
+            "timestamp": 0.2444935843678232
+        },
+        {
+            "x": 4.728031239930722,
+            "y": 6.375889843725219,
+            "heading": -0.4550458803770597,
+            "angularVelocity": -2.967925154596154,
+            "velocityX": -2.225333587355345,
+            "velocityY": -1.2857370584715861,
+            "timestamp": 0.32599144582376427
+        },
+        {
+            "x": 4.50844214888531,
+            "y": 6.243100931440797,
+            "heading": -0.5880935326141025,
+            "angularVelocity": -1.6325293677671568,
+            "velocityX": -2.6944153763362864,
+            "velocityY": -1.6293545611158133,
+            "timestamp": 0.40748930727970534
+        },
+        {
+            "x": 4.251218650398233,
+            "y": 6.083818287155582,
+            "heading": -0.6031902279140122,
+            "angularVelocity": -0.1852403858237583,
+            "velocityX": -3.15619936390767,
+            "velocityY": -1.9544395575499292,
+            "timestamp": 0.4889871687356464
+        },
+        {
+            "x": 3.9855391766757995,
+            "y": 5.92740773635831,
+            "heading": -0.6031903133870022,
+            "angularVelocity": -0.0000010487758630839914,
+            "velocityX": -3.259956383837921,
+            "velocityY": -1.9191982219291623,
+            "timestamp": 0.5704850301915875
+        },
+        {
+            "x": 3.719859695364488,
+            "y": 5.770997198451498,
+            "heading": -0.603190398859952,
+            "angularVelocity": -0.0000010487753700599598,
+            "velocityX": -3.259956476955436,
+            "velocityY": -1.919198063759863,
+            "timestamp": 0.6519828916475285
+        },
+        {
+            "x": 3.4541802321431443,
+            "y": 5.614586629816999,
+            "heading": -0.6031904843329001,
+            "angularVelocity": -0.000001048775349610677,
+            "velocityX": -3.259956254986808,
+            "velocityY": -1.9191984407966034,
+            "timestamp": 0.7334807531034696
+        },
+        {
+            "x": 3.1885009063173535,
+            "y": 5.458175827802239,
+            "heading": -0.6031905698058837,
+            "angularVelocity": -0.000001048775783641969,
+            "velocityX": -3.259954569107567,
+            "velocityY": -1.9192013044332228,
+            "timestamp": 0.8149786145594107
+        },
+        {
+            "x": 2.937386380849553,
+            "y": 5.279314604480798,
+            "heading": -0.603190661977047,
+            "angularVelocity": -0.0000011309641953921016,
+            "velocityX": -3.0812406728433848,
+            "velocityY": -2.1946738248847923,
+            "timestamp": 0.8964764760153517
+        },
+        {
+            "x": 2.722322578778117,
+            "y": 5.058413395221267,
+            "heading": -0.6031910253459357,
+            "angularVelocity": -0.00000445863096683889,
+            "velocityX": -2.6388889012468426,
+            "velocityY": -2.710515408787179,
+            "timestamp": 0.9779743374712928
+        },
+        {
+            "x": 2.561151389441887,
+            "y": 4.8410288515668665,
+            "heading": -0.5595876383179096,
+            "angularVelocity": 0.5350249227287813,
+            "velocityX": -1.9776124975176395,
+            "velocityY": -2.667365005300435,
+            "timestamp": 1.0594721989272338
+        },
+        {
+            "x": 2.4444023928247787,
+            "y": 4.656624455699537,
+            "heading": -0.5072145719014195,
+            "angularVelocity": 0.6426311743750899,
+            "velocityX": -1.432540615562337,
+            "velocityY": -2.262689996681954,
+            "timestamp": 1.1409700603831747
+        },
+        {
+            "x": 2.367465069003143,
+            "y": 4.510717874734717,
+            "heading": -0.4552059179368463,
+            "angularVelocity": 0.6381597386170672,
+            "velocityX": -0.9440410146618312,
+            "velocityY": -1.7903117745451358,
+            "timestamp": 1.2224679218391157
+        },
+        {
+            "x": 2.321242179529733,
+            "y": 4.410775659264178,
+            "heading": -0.4150276653385325,
+            "angularVelocity": 0.49299763061923907,
+            "velocityX": -0.5671669004271778,
+            "velocityY": -1.2263170307182782,
+            "timestamp": 1.3039657832950566
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.393,
+            "angularVelocity": 0.27028519454392347,
+            "velocityX": -0.26064707895700284,
+            "velocityY": -0.6230305723006956,
+            "timestamp": 1.3854636447509976
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.393,
+            "angularVelocity": -9.678618240747916e-31,
+            "velocityX": 0,
+            "velocityY": -3.108003047332484e-31,
+            "timestamp": 1.4669615062069385
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S2 To C1.traj b/src/main/deploy/choreo/S2 To C1.traj
new file mode 100644
index 00000000..29c2d4bc
--- /dev/null
+++ b/src/main/deploy/choreo/S2 To C1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 5.380258045140596e-28,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.304265282414897,
+            "y": 5.638698322322049,
+            "heading": -0.04261698283734242,
+            "angularVelocity": -0.41098530784979936,
+            "velocityX": 0.3979499173257931,
+            "velocityY": 0.7589428456312429,
+            "timestamp": 0.10369466261532896
+        },
+        {
+            "x": 4.38866009880019,
+            "y": 5.795292929312062,
+            "heading": -0.1228057028496872,
+            "angularVelocity": -0.7733157907057984,
+            "velocityX": 0.8138781134605525,
+            "velocityY": 1.510151082422871,
+            "timestamp": 0.20738932523065792
+        },
+        {
+            "x": 4.519821720835651,
+            "y": 6.028001091828693,
+            "heading": -0.23019023538729733,
+            "angularVelocity": -1.0355839908170448,
+            "velocityX": 1.264883058851596,
+            "velocityY": 2.2441672179396264,
+            "timestamp": 0.3110839878459869
+        },
+        {
+            "x": 4.707676475524912,
+            "y": 6.330157785456564,
+            "heading": -0.33387571773962255,
+            "angularVelocity": -0.9999114683169592,
+            "velocityX": 1.8116145031122473,
+            "velocityY": 2.9139078715050806,
+            "timestamp": 0.41477865046131585
+        },
+        {
+            "x": 4.978690211282069,
+            "y": 6.613755936139097,
+            "heading": -0.3338757185080882,
+            "angularVelocity": -7.410850442524386e-9,
+            "velocityX": 2.6135745941189206,
+            "velocityY": 2.7349348898948014,
+            "timestamp": 0.5184733130766448
+        },
+        {
+            "x": 5.307019127730767,
+            "y": 6.828410259978494,
+            "heading": -0.3338757060420003,
+            "angularVelocity": 1.2021918642580205e-7,
+            "velocityX": 3.166304881734213,
+            "velocityY": 2.070061451819255,
+            "timestamp": 0.6221679756919738
+        },
+        {
+            "x": 5.675511606586513,
+            "y": 6.962908031631864,
+            "heading": -0.3338756959696621,
+            "angularVelocity": 9.713458668099172e-8,
+            "velocityX": 3.5536301441350284,
+            "velocityY": 1.2970558779125847,
+            "timestamp": 0.7258626383073028
+        },
+        {
+            "x": 6.055844888125957,
+            "y": 7.058944236891981,
+            "heading": -0.3338756861984713,
+            "angularVelocity": 9.423041207795053e-8,
+            "velocityX": 3.6678192680981745,
+            "velocityY": 0.9261441508940896,
+            "timestamp": 0.8295573009226318
+        },
+        {
+            "x": 6.436176952400414,
+            "y": 7.154985262770466,
+            "heading": -0.3338756764272739,
+            "angularVelocity": 9.423047569354886e-8,
+            "velocityX": 3.667807529162388,
+            "velocityY": 0.9261906394812687,
+            "timestamp": 0.9332519635379608
+        },
+        {
+            "x": 6.8165090646020285,
+            "y": 7.2510260988513195,
+            "heading": -0.33387566665551793,
+            "angularVelocity": 9.423586238606553e-8,
+            "velocityX": 3.6678079913574164,
+            "velocityY": 0.9261888091302356,
+            "timestamp": 1.0369466261532898
+        },
+        {
+            "x": 7.162467192455402,
+            "y": 7.33732437680055,
+            "heading": -0.24283297058924916,
+            "angularVelocity": 0.8779882567727307,
+            "velocityX": 3.336315670718354,
+            "velocityY": 0.8322345217454595,
+            "timestamp": 1.1406412887686188
+        },
+        {
+            "x": 7.421550289934561,
+            "y": 7.4021983509049765,
+            "heading": -0.1315877257451376,
+            "angularVelocity": 1.0728155339758783,
+            "velocityX": 2.4985191228237835,
+            "velocityY": 0.6256250077700213,
+            "timestamp": 1.2443359513839478
+        },
+        {
+            "x": 7.593930847137365,
+            "y": 7.445412549605325,
+            "heading": -0.046073342510862675,
+            "angularVelocity": 0.8246748779298502,
+            "velocityX": 1.662386017323544,
+            "velocityY": 0.41674467721309977,
+            "timestamp": 1.3480306139992768
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0.44431739637147044,
+            "velocityX": 0.8300249086292989,
+            "velocityY": 0.20818285001567757,
+            "timestamp": 1.4517252766146058
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 6.677691966309128e-30,
+            "velocityY": 0,
+            "timestamp": 1.5554199392299348
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S2 To C2.traj b/src/main/deploy/choreo/S2 To C2.traj
new file mode 100644
index 00000000..05a18e04
--- /dev/null
+++ b/src/main/deploy/choreo/S2 To C2.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.272659932573731,
+            "y": 5.605321030989923,
+            "heading": -5.269773313916661e-8,
+            "angularVelocity": -7.069117507058631e-7,
+            "velocityX": 0.1295790656325301,
+            "velocityY": 0.6079397351586845,
+            "timestamp": 0.07454855863477708
+        },
+        {
+            "x": 4.294668340488188,
+            "y": 5.695305527660525,
+            "heading": -1.390931476532318e-7,
+            "angularVelocity": -0.0000011589489187991593,
+            "velocityX": 0.2952224471421504,
+            "velocityY": 1.2070588395619357,
+            "timestamp": 0.14909711726955416
+        },
+        {
+            "x": 4.333068049168991,
+            "y": 5.8286332482919585,
+            "heading": -2.3393980900082764e-7,
+            "angularVelocity": -0.0000012723197345504802,
+            "velocityX": 0.5150965944736963,
+            "velocityY": 1.7884681217524945,
+            "timestamp": 0.22364567590433124
+        },
+        {
+            "x": 4.394361213548298,
+            "y": 6.0022499148295365,
+            "heading": -3.0377830712505715e-7,
+            "angularVelocity": -9.368513338342214e-7,
+            "velocityX": 0.8221911395208825,
+            "velocityY": 2.3289070872699873,
+            "timestamp": 0.2981942345391083
+        },
+        {
+            "x": 4.489214393007107,
+            "y": 6.207820209268236,
+            "heading": -3.104589480689307e-7,
+            "angularVelocity": -8.962358439949817e-8,
+            "velocityX": 1.2723677183517785,
+            "velocityY": 2.75753546703125,
+            "timestamp": 0.37274279317388537
+        },
+        {
+            "x": 4.629403012529928,
+            "y": 6.42298249437822,
+            "heading": -2.4199647648725644e-7,
+            "angularVelocity": 9.18393943226279e-7,
+            "velocityX": 1.880500737963715,
+            "velocityY": 2.8862031546566858,
+            "timestamp": 0.4472913518086624
+        },
+        {
+            "x": 4.811531801184116,
+            "y": 6.618438666310206,
+            "heading": -1.472447710538824e-7,
+            "angularVelocity": 0.0000012710752897138218,
+            "velocityX": 2.443089336474201,
+            "velocityY": 2.6218638627509074,
+            "timestamp": 0.5218399104434395
+        },
+        {
+            "x": 5.024847227590037,
+            "y": 6.779620899070013,
+            "heading": -6.254572131926062e-8,
+            "angularVelocity": 0.000001136235548213432,
+            "velocityX": 2.861429252247402,
+            "velocityY": 2.1621106520395927,
+            "timestamp": 0.5963884690782165
+        },
+        {
+            "x": 5.261847496032715,
+            "y": 6.900974273681641,
+            "heading": 1.6289151195668303e-9,
+            "angularVelocity": 8.609140934997053e-7,
+            "velocityX": 3.1791395133448552,
+            "velocityY": 1.6278433384167708,
+            "timestamp": 0.6709370277129936
+        },
+        {
+            "x": 5.5631639462020495,
+            "y": 6.984947323321856,
+            "heading": 4.965855378058324e-8,
+            "angularVelocity": 5.539886921154719e-7,
+            "velocityX": 3.4757171947403775,
+            "velocityY": 0.9686380297960585,
+            "timestamp": 0.7576289054784
+        },
+        {
+            "x": 5.883696640724643,
+            "y": 7.009274337947639,
+            "heading": 7.188189606013307e-8,
+            "angularVelocity": 2.563216327096045e-7,
+            "velocityX": 3.697378610459799,
+            "velocityY": 0.2806146929817695,
+            "timestamp": 0.8443207832438064
+        },
+        {
+            "x": 6.209423466668327,
+            "y": 6.971151903870608,
+            "heading": 7.193737396357795e-8,
+            "angularVelocity": 6.438452889791163e-10,
+            "velocityX": 3.7572934667332714,
+            "velocityY": -0.4397463179458525,
+            "timestamp": 0.9310126610092129
+        },
+        {
+            "x": 6.521952745942683,
+            "y": 6.871769894938554,
+            "heading": 7.193784305655878e-8,
+            "angularVelocity": 5.630908705768223e-12,
+            "velocityX": 3.6050583669434406,
+            "velocityY": -1.1463820079014435,
+            "timestamp": 1.0177045387746193
+        },
+        {
+            "x": 6.809873374118548,
+            "y": 6.714756988695251,
+            "heading": 7.193831945924844e-8,
+            "angularVelocity": 5.91084758847972e-12,
+            "velocityX": 3.321194967875227,
+            "velocityY": -1.8111605177424126,
+            "timestamp": 1.1043964165400257
+        },
+        {
+            "x": 7.062672913559571,
+            "y": 6.505846154651799,
+            "heading": 7.193893641048392e-8,
+            "angularVelocity": 7.48396872358976e-12,
+            "velocityX": 2.9160694861627596,
+            "velocityY": -2.4098086166543093,
+            "timestamp": 1.1910882943054322
+        },
+        {
+            "x": 7.268448732261103,
+            "y": 6.269564240975314,
+            "heading": 6.958123303548702e-8,
+            "angularVelocity": -2.7156604096657526e-8,
+            "velocityX": 2.3736458816624144,
+            "velocityY": -2.7255369226670942,
+            "timestamp": 1.2777801720708386
+        },
+        {
+            "x": 7.433069281193563,
+            "y": 6.080538590855167,
+            "heading": 5.4913746267249155e-8,
+            "angularVelocity": -1.6915879682512025e-7,
+            "velocityX": 1.8989154825033356,
+            "velocityY": -2.180430912690726,
+            "timestamp": 1.364472049836245
+        },
+        {
+            "x": 7.556534655605292,
+            "y": 5.9387693118426395,
+            "heading": 3.4027865091828515e-8,
+            "angularVelocity": -2.408962130800618e-7,
+            "velocityX": 1.424186181835385,
+            "velocityY": -1.635323662267605,
+            "timestamp": 1.4511639276016515
+        },
+        {
+            "x": 7.638844889158882,
+            "y": 5.8442564415645775,
+            "heading": 1.3493455787871544e-8,
+            "angularVelocity": -2.368497714278514e-7,
+            "velocityX": 0.9494572693998051,
+            "velocityY": -1.0902159778725726,
+            "timestamp": 1.537855805367058
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -2.563219583366834e-29,
+            "angularVelocity": -1.5563981959470815e-7,
+            "velocityX": 0.4747285662416774,
+            "velocityY": -0.5451080630467725,
+            "timestamp": 1.6245476831324643
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 4.328374921762355e-28,
+            "velocityX": 0,
+            "velocityY": 6.636862452110489e-29,
+            "timestamp": 1.7112395608978708
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S2 To C3.traj b/src/main/deploy/choreo/S2 To C3.traj
new file mode 100644
index 00000000..2b663e69
--- /dev/null
+++ b/src/main/deploy/choreo/S2 To C3.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": -9.522541691773037e-34,
+            "velocityX": 0,
+            "velocityY": -4.2308696063129985e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 4.271262994735721,
+            "y": 5.464085078742271,
+            "heading": 0.013066573413208545,
+            "angularVelocity": 0.12156077000957308,
+            "velocityX": 0.0768721814736735,
+            "velocityY": -0.8923144052220247,
+            "timestamp": 0.10749005137249076
+        },
+        {
+            "x": 4.301424317332477,
+            "y": 5.27437671657762,
+            "heading": 0.033784324267673776,
+            "angularVelocity": 0.192741101059399,
+            "velocityX": 0.2805964106597774,
+            "velocityY": -1.7648922829820315,
+            "timestamp": 0.21498010274498153
+        },
+        {
+            "x": 4.379013311524821,
+            "y": 5.000817118183451,
+            "heading": 0.051063906614390905,
+            "angularVelocity": 0.16075517804747627,
+            "velocityX": 0.7218248870629933,
+            "velocityY": -2.544975975927198,
+            "timestamp": 0.32247015411747226
+        },
+        {
+            "x": 4.5426840546799285,
+            "y": 4.684552935799795,
+            "heading": 0.043724640930798246,
+            "angularVelocity": -0.06827855778168279,
+            "velocityX": 1.5226594560638123,
+            "velocityY": -2.9422646872470954,
+            "timestamp": 0.42996020548996305
+        },
+        {
+            "x": 4.7987371937718075,
+            "y": 4.3933637164950845,
+            "heading": -0.0025480008668042047,
+            "angularVelocity": -0.4304830187237655,
+            "velocityX": 2.382110119238521,
+            "velocityY": -2.7089876280330105,
+            "timestamp": 0.5374502568624538
+        },
+        {
+            "x": 5.129552682758233,
+            "y": 4.161939062753506,
+            "heading": -0.00976861092561694,
+            "angularVelocity": -0.06717468236935505,
+            "velocityX": 3.0776382070935524,
+            "velocityY": -2.152986725623672,
+            "timestamp": 0.6449403082349446
+        },
+        {
+            "x": 5.508331065778466,
+            "y": 4.0140416634822165,
+            "heading": -0.009768639154040644,
+            "angularVelocity": -2.6261429169311795e-7,
+            "velocityX": 3.5238459576843355,
+            "velocityY": -1.3759170954228361,
+            "timestamp": 0.7524303596074354
+        },
+        {
+            "x": 5.911272135069668,
+            "y": 3.959404581326429,
+            "heading": -0.009768652411608816,
+            "angularVelocity": -1.233376298753571e-7,
+            "velocityX": 3.7486359355701584,
+            "velocityY": -0.5082989677477323,
+            "timestamp": 0.8599204109799262
+        },
+        {
+            "x": 6.316087697151271,
+            "y": 3.997759236961094,
+            "heading": -0.009768652034230711,
+            "angularVelocity": 3.51081891151765e-9,
+            "velocityX": 3.7660746916826353,
+            "velocityY": 0.3568205163634492,
+            "timestamp": 0.967410462352417
+        },
+        {
+            "x": 6.720902827559142,
+            "y": 4.036118448432054,
+            "heading": -0.009768651656829377,
+            "angularVelocity": 3.5110350109919825e-9,
+            "velocityX": 3.766070675741356,
+            "velocityY": 0.35686290015837574,
+            "timestamp": 1.0749005137249077
+        },
+        {
+            "x": 7.104541677751467,
+            "y": 4.072471064803872,
+            "heading": -0.005861206189315246,
+            "angularVelocity": 0.03635169411142489,
+            "velocityX": 3.569063790497984,
+            "velocityY": 0.3381951716242375,
+            "timestamp": 1.1823905650973985
+        },
+        {
+            "x": 7.392270834070994,
+            "y": 4.099735530737146,
+            "heading": -0.0029306066828158664,
+            "angularVelocity": 0.027263913907193366,
+            "velocityX": 2.6767980166131413,
+            "velocityY": 0.2536464127158502,
+            "timestamp": 1.2898806164698893
+        },
+        {
+            "x": 7.584090276967534,
+            "y": 4.117911843086064,
+            "heading": -0.0009768696053991616,
+            "angularVelocity": 0.018175980497453854,
+            "velocityX": 1.7845320608492279,
+            "velocityY": 0.16909762454137037,
+            "timestamp": 1.39737066784238
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -6.809535330810639e-32,
+            "angularVelocity": 0.009088000172350516,
+            "velocityX": 0.8922660451626774,
+            "velocityY": 0.08454881915017429,
+            "timestamp": 1.5048607192148709
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": -9.489213882041181e-31,
+            "velocityX": 2.198140684032685e-33,
+            "velocityY": -6.756921472052438e-32,
+            "timestamp": 1.6123507705873616
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S2 To C4.traj b/src/main/deploy/choreo/S2 To C4.traj
new file mode 100644
index 00000000..c13783bc
--- /dev/null
+++ b/src/main/deploy/choreo/S2 To C4.traj	
@@ -0,0 +1,166 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 7.441191049306378e-32,
+            "velocityX": -6.15829104129807e-34,
+            "velocityY": 2.4252394047543234e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 4.262142393092874,
+            "y": 5.471176724349191,
+            "heading": -0.06580713819401311,
+            "angularVelocity": -0.6311237133024392,
+            "velocityX": -0.008224883662067025,
+            "velocityY": -0.8518601035521746,
+            "timestamp": 0.10426979181255681
+        },
+        {
+            "x": 4.27936690849282,
+            "y": 5.294863623901166,
+            "heading": -0.18680392528158013,
+            "angularVelocity": -1.1604203382805245,
+            "velocityX": 0.16519180771848577,
+            "velocityY": -1.6909317395107013,
+            "timestamp": 0.20853958362511363
+        },
+        {
+            "x": 4.348116425232891,
+            "y": 5.0445566747334585,
+            "heading": -0.3412830267774945,
+            "angularVelocity": -1.4815326549574157,
+            "velocityX": 0.6593426105967491,
+            "velocityY": -2.400570144205118,
+            "timestamp": 0.31280937543767046
+        },
+        {
+            "x": 4.502267029635194,
+            "y": 4.763921050939977,
+            "heading": -0.4878370803946758,
+            "angularVelocity": -1.4055274405902505,
+            "velocityX": 1.4783822018117698,
+            "velocityY": -2.6914374615609895,
+            "timestamp": 0.41707916725022726
+        },
+        {
+            "x": 4.740029978369904,
+            "y": 4.496898841850153,
+            "heading": -0.5207249369577782,
+            "angularVelocity": -0.31541116551017917,
+            "velocityX": 2.280266840487519,
+            "velocityY": -2.5608779345205184,
+            "timestamp": 0.5213489590627841
+        },
+        {
+            "x": 5.0582868126043,
+            "y": 4.2697391199027495,
+            "heading": -0.509978725166827,
+            "angularVelocity": 0.10306160206274768,
+            "velocityX": 3.0522438829312937,
+            "velocityY": -2.1785765368723764,
+            "timestamp": 0.6256187508753409
+        },
+        {
+            "x": 5.423286105863135,
+            "y": 4.1201949004052105,
+            "heading": -0.5099785687835147,
+            "angularVelocity": 0.0000014997949991595825,
+            "velocityX": 3.5005276879710774,
+            "velocityY": -1.4342046425715584,
+            "timestamp": 0.7298885426878977
+        },
+        {
+            "x": 5.805099761015269,
+            "y": 4.021168099470067,
+            "heading": -0.5099784851647622,
+            "angularVelocity": 8.019460959727046e-7,
+            "velocityX": 3.6617859162748747,
+            "velocityY": -0.9497170677501874,
+            "timestamp": 0.8341583345004545
+        },
+        {
+            "x": 6.154222032422552,
+            "y": 3.8375880828272497,
+            "heading": -0.5099784796190555,
+            "angularVelocity": 5.318612994948929e-8,
+            "velocityX": 3.348259024386382,
+            "velocityY": -1.7606251384181746,
+            "timestamp": 0.9384281263130113
+        },
+        {
+            "x": 6.493379431179238,
+            "y": 3.636192348012897,
+            "heading": -0.5099784741544249,
+            "angularVelocity": 5.240856867670856e-8,
+            "velocityX": 3.2526908595576858,
+            "velocityY": -1.9314868795019449,
+            "timestamp": 1.0426979181255682
+        },
+        {
+            "x": 6.795285003697281,
+            "y": 3.3823400148372036,
+            "heading": -0.5099784394461939,
+            "angularVelocity": 3.32869476219209e-7,
+            "velocityX": 2.895427019368865,
+            "velocityY": -2.4345721686299853,
+            "timestamp": 1.146967709938125
+        },
+        {
+            "x": 7.066681771760518,
+            "y": 3.0961027469536373,
+            "heading": -0.509978390292543,
+            "angularVelocity": 4.714083530965781e-7,
+            "velocityX": 2.6028321659174227,
+            "velocityY": -2.7451600593787298,
+            "timestamp": 1.2512375017506818
+        },
+        {
+            "x": 7.312943361341716,
+            "y": 2.8410544331524363,
+            "heading": -0.40533862017886096,
+            "angularVelocity": 1.0035482788897323,
+            "velocityX": 2.361773101301441,
+            "velocityY": -2.446042227260748,
+            "timestamp": 1.3555072935632386
+        },
+        {
+            "x": 7.49731350893092,
+            "y": 2.6484381177603322,
+            "heading": -0.22178416093118458,
+            "angularVelocity": 1.7603800300824186,
+            "velocityX": 1.768202893515326,
+            "velocityY": -1.8472878102448496,
+            "timestamp": 1.4597770853757954
+        },
+        {
+            "x": 7.619307085639959,
+            "y": 2.520663921373434,
+            "heading": -0.0776901780416505,
+            "angularVelocity": 1.3819341190262298,
+            "velocityX": 1.1699800545142,
+            "velocityY": -1.225419118670484,
+            "timestamp": 1.5640468771883522
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 1.429761379305419e-34,
+            "angularVelocity": 0.7450880709660586,
+            "velocityX": 0.5820757220763062,
+            "velocityY": -0.6105691808408042,
+            "timestamp": 1.668316669000909
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": -5.61453765178046e-32,
+            "velocityX": 0,
+            "velocityY": 2.318867528557263e-33,
+            "timestamp": 1.7725864608134658
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S2 To C5.traj b/src/main/deploy/choreo/S2 To C5.traj
new file mode 100644
index 00000000..004abebc
--- /dev/null
+++ b/src/main/deploy/choreo/S2 To C5.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": -1.2613927548149136e-32,
+            "velocityX": 4.983015637334932e-33,
+            "velocityY": -3.6905945238681026e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 4.258006222511338,
+            "y": 5.473607841813564,
+            "heading": -0.06530003091818118,
+            "angularVelocity": -0.6342708718414432,
+            "velocityX": -0.04850545332028736,
+            "velocityY": -0.8391424739420397,
+            "timestamp": 0.10295290831912149
+        },
+        {
+            "x": 4.271377859079598,
+            "y": 5.302453682559796,
+            "heading": -0.1916286288786279,
+            "angularVelocity": -1.2270522515873779,
+            "velocityX": 0.12988109599401854,
+            "velocityY": -1.662450940416803,
+            "timestamp": 0.20590581663824298
+        },
+        {
+            "x": 4.340766271943036,
+            "y": 5.0642424556348775,
+            "heading": -0.3653686640748466,
+            "angularVelocity": -1.6875680156375907,
+            "velocityX": 0.673982056420939,
+            "velocityY": -2.3137882242873524,
+            "timestamp": 0.30885872495736444
+        },
+        {
+            "x": 4.494848034080926,
+            "y": 4.801255485663876,
+            "heading": -0.5565822720790902,
+            "angularVelocity": -1.8572919514963226,
+            "velocityX": 1.4966236957608356,
+            "velocityY": -2.5544394448365244,
+            "timestamp": 0.41181163327648596
+        },
+        {
+            "x": 4.73233691115851,
+            "y": 4.546498868370588,
+            "heading": -0.6490495118957932,
+            "angularVelocity": -0.8981508276588281,
+            "velocityX": 2.3067719111095206,
+            "velocityY": -2.4744965582091383,
+            "timestamp": 0.5147645415956075
+        },
+        {
+            "x": 5.049349589860016,
+            "y": 4.3202563768587865,
+            "heading": -0.6490493910522949,
+            "angularVelocity": 0.0000011737744981731646,
+            "velocityX": 3.079200810130272,
+            "velocityY": -2.1975337579636096,
+            "timestamp": 0.617717449914729
+        },
+        {
+            "x": 5.4092082222704905,
+            "y": 4.171311637049865,
+            "heading": -0.6490493274377749,
+            "angularVelocity": 6.178992034513071e-7,
+            "velocityX": 3.4953712166637025,
+            "velocityY": -1.446726879703489,
+            "timestamp": 0.7206703582338505
+        },
+        {
+            "x": 5.789554256648214,
+            "y": 4.087528164867666,
+            "heading": -0.6490492895940018,
+            "angularVelocity": 3.675833320529158e-7,
+            "velocityX": 3.6943690138288297,
+            "velocityY": -0.8138038405141147,
+            "timestamp": 0.823623266552972
+        },
+        {
+            "x": 6.141218018459777,
+            "y": 3.9201502365347105,
+            "heading": -0.6490493503243893,
+            "angularVelocity": -5.898851085689342e-7,
+            "velocityX": 3.4157729738097102,
+            "velocityY": -1.6257717345306835,
+            "timestamp": 0.9265761748720935
+        },
+        {
+            "x": 6.456138339829479,
+            "y": 3.69100407104571,
+            "heading": -0.6490494074378677,
+            "angularVelocity": -5.547534235510974e-7,
+            "velocityX": 3.0588773693847333,
+            "velocityY": -2.2257376622981755,
+            "timestamp": 1.029529083191215
+        },
+        {
+            "x": 6.721775326533507,
+            "y": 3.406188756476768,
+            "heading": -0.6490494689481024,
+            "angularVelocity": -5.974599048336077e-7,
+            "velocityX": 2.580179531020508,
+            "velocityY": -2.7664620574496457,
+            "timestamp": 1.1324819915103363
+        },
+        {
+            "x": 6.916359430113237,
+            "y": 3.0688168532177102,
+            "heading": -0.6490494077682868,
+            "angularVelocity": 5.942504852606566e-7,
+            "velocityX": 1.8900301774533879,
+            "velocityY": -3.276953597204961,
+            "timestamp": 1.2354348998294578
+        },
+        {
+            "x": 7.040594979350531,
+            "y": 2.699698573462511,
+            "heading": -0.6490493528706274,
+            "angularVelocity": 5.332307776860117e-7,
+            "velocityX": 1.206722095234099,
+            "velocityY": -3.585311826364818,
+            "timestamp": 1.3383878081485792
+        },
+        {
+            "x": 7.164812386541997,
+            "y": 2.330574188065911,
+            "heading": -0.649049297974002,
+            "angularVelocity": 5.332207349098274e-7,
+            "velocityX": 1.2065458783003051,
+            "velocityY": -3.585371131551044,
+            "timestamp": 1.4413407164677006
+        },
+        {
+            "x": 7.289029734328084,
+            "y": 1.9614497826782673,
+            "heading": -0.6490492430773671,
+            "angularVelocity": 5.332208261000816e-7,
+            "velocityX": 1.206545301285243,
+            "velocityY": -3.5853713257276327,
+            "timestamp": 1.544293624786822
+        },
+        {
+            "x": 7.413246520819429,
+            "y": 1.5923251884092744,
+            "heading": -0.6490491881699392,
+            "angularVelocity": 5.333256623024379e-7,
+            "velocityX": 1.2065398493290926,
+            "velocityY": -3.585373160365936,
+            "timestamp": 1.6472465331059434
+        },
+        {
+            "x": 7.520074458284658,
+            "y": 1.2704783284902474,
+            "heading": -0.5256216946651037,
+            "angularVelocity": 1.1988733054752465,
+            "velocityX": 1.037638850707321,
+            "velocityY": -3.1261560763432104,
+            "timestamp": 1.7501994414250648
+        },
+        {
+            "x": 7.601104601009551,
+            "y": 1.0271731610238788,
+            "heading": -0.2913266412644364,
+            "angularVelocity": 2.275749730881093,
+            "velocityX": 0.7870602593733868,
+            "velocityY": -2.3632665792422265,
+            "timestamp": 1.8531523497441862
+        },
+        {
+            "x": 7.654009357663848,
+            "y": 0.8666565360101991,
+            "heading": -0.10310334462657153,
+            "angularVelocity": 1.828246522715339,
+            "velocityX": 0.5138733574219042,
+            "velocityY": -1.5591266690216163,
+            "timestamp": 1.9561052580633076
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -1.429149737287701e-32,
+            "angularVelocity": 1.0014612147428026,
+            "velocityX": 0.2524517545010862,
+            "velocityY": -0.7737181718391956,
+            "timestamp": 2.059058166382429
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": -8.032741852653566e-32,
+            "velocityX": 0,
+            "velocityY": 2.733638748586157e-32,
+            "timestamp": 2.1620110747015504
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S2 To W1.traj b/src/main/deploy/choreo/S2 To W1.traj
new file mode 100644
index 00000000..c913a793
--- /dev/null
+++ b/src/main/deploy/choreo/S2 To W1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 8.330960047205791e-35,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.215224456008129,
+            "y": 5.5891314798928855,
+            "heading": 0.013109691961439362,
+            "angularVelocity": 0.15985781811927008,
+            "velocityX": -0.5825685488618567,
+            "velocityY": 0.35522534228568337,
+            "timestamp": 0.08200845048227925
+        },
+        {
+            "x": 4.11967337131145,
+            "y": 5.647394543004508,
+            "heading": 0.03932481805987993,
+            "angularVelocity": 0.31966371690079975,
+            "velocityX": -1.1651370576417237,
+            "velocityY": 0.7104519445128596,
+            "timestamp": 0.1640169009645585
+        },
+        {
+            "x": 3.976346757700282,
+            "y": 5.734789362523958,
+            "heading": 0.07863903763442207,
+            "angularVelocity": 0.47939229851730114,
+            "velocityX": -1.7477054226519961,
+            "velocityY": 1.0656806585844962,
+            "timestamp": 0.24602535144683776
+        },
+        {
+            "x": 3.7852446629452063,
+            "y": 5.851316183985278,
+            "heading": 0.13104540832787342,
+            "angularVelocity": 0.6390362259652201,
+            "velocityX": -2.3302732051542674,
+            "velocityY": 1.4209123666652002,
+            "timestamp": 0.328033801929117
+        },
+        {
+            "x": 3.5463672091086966,
+            "y": 5.996975313640393,
+            "heading": 0.19653856458379848,
+            "angularVelocity": 0.7986147265406159,
+            "velocityX": -2.912839499243156,
+            "velocityY": 1.77614780914058,
+            "timestamp": 0.41004225241139625
+        },
+        {
+            "x": 3.281494446824011,
+            "y": 6.15848998396632,
+            "heading": 0.19653857286419524,
+            "angularVelocity": 1.009700426817188e-7,
+            "velocityX": -3.229822789322431,
+            "velocityY": 1.9694881366015824,
+            "timestamp": 0.4920507028936755
+        },
+        {
+            "x": 3.016621684855716,
+            "y": 6.320004654811106,
+            "heading": 0.1965385811445921,
+            "angularVelocity": 1.0097004416983622e-7,
+            "velocityX": -3.229822785464406,
+            "velocityY": 1.969488142928474,
+            "timestamp": 0.5740591533759547
+        },
+        {
+            "x": 2.77774755563319,
+            "y": 6.465669559960414,
+            "heading": 0.2620174097526663,
+            "angularVelocity": 0.7984400171324189,
+            "velocityX": -2.9127989593480157,
+            "velocityY": 1.7762182347389206,
+            "timestamp": 0.6560676038582339
+        },
+        {
+            "x": 2.5866484397575076,
+            "y": 6.582201656937504,
+            "heading": 0.3144069758542471,
+            "angularVelocity": 0.6388313130352502,
+            "velocityX": -2.330236881100143,
+            "velocityY": 1.4209766955939638,
+            "timestamp": 0.7380760543405132
+        },
+        {
+            "x": 2.443324199843316,
+            "y": 6.6696008076986475,
+            "heading": 0.35370275100271265,
+            "angularVelocity": 0.4791673896698825,
+            "velocityX": -1.7476764781107736,
+            "velocityY": 1.0657334731623573,
+            "timestamp": 0.8200845048227924
+        },
+        {
+            "x": 2.347774735979553,
+            "y": 6.727866934294423,
+            "heading": 0.37990105353306003,
+            "angularVelocity": 0.3194585725773273,
+            "velocityX": -1.1651172934234333,
+            "velocityY": 0.7104893002260324,
+            "timestamp": 0.9020929553050716
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0.15972678900658524,
+            "velocityX": -0.5825586960684825,
+            "velocityY": 0.3552446794720561,
+            "timestamp": 0.9841014057873508
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": -4.59054414969708e-34,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 1.06610985626963
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S2 To W2.traj b/src/main/deploy/choreo/S2 To W2.traj
new file mode 100644
index 00000000..a4697851
--- /dev/null
+++ b/src/main/deploy/choreo/S2 To W2.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.181096344253034,
+            "y": 5.56,
+            "heading": 2.5710705899848466e-19,
+            "angularVelocity": 2.5941602968880693e-18,
+            "velocityX": -0.8263919813283321,
+            "velocityY": -1.4479549544654444e-34,
+            "timestamp": 0.099109935233537
+        },
+        {
+            "x": 4.017289035836238,
+            "y": 5.56,
+            "heading": 7.7182898862159e-19,
+            "angularVelocity": 5.193444314378615e-18,
+            "velocityX": -1.6527839316089838,
+            "velocityY": -7.36495654370163e-34,
+            "timestamp": 0.198219870467074
+        },
+        {
+            "x": 3.771578080903875,
+            "y": 5.56,
+            "heading": 1.545129672215716e-18,
+            "angularVelocity": 7.802453727507411e-18,
+            "velocityX": -2.479175819794286,
+            "velocityY": -8.54847043233614e-34,
+            "timestamp": 0.297329805700611
+        },
+        {
+            "x": 3.4439634979187304,
+            "y": 5.56,
+            "heading": 2.5806642382479214e-18,
+            "angularVelocity": 1.0448342676399075e-17,
+            "velocityX": -3.3055675216936846,
+            "velocityY": -5.0910568375180236e-34,
+            "timestamp": 0.396439740934148
+        },
+        {
+            "x": 3.0690365020812695,
+            "y": 5.56,
+            "heading": -2.5850068638158606e-18,
+            "angularVelocity": -5.212061828001078e-17,
+            "velocityX": -3.7829405796099502,
+            "velocityY": -4.536187828596351e-34,
+            "timestamp": 0.495549676167685
+        },
+        {
+            "x": 2.741421919096125,
+            "y": 5.56,
+            "heading": -1.5478395588580842e-18,
+            "angularVelocity": 1.0464816695997814e-17,
+            "velocityX": -3.305567521693684,
+            "velocityY": -2.872506388563931e-33,
+            "timestamp": 0.594659611401222
+        },
+        {
+            "x": 2.4957109641637625,
+            "y": 5.56,
+            "heading": -7.728706253244835e-19,
+            "angularVelocity": 7.819286045545264e-18,
+            "velocityX": -2.479175819794286,
+            "velocityY": -1.6595590840772702e-33,
+            "timestamp": 0.6937695466347591
+        },
+        {
+            "x": 2.3319036557469652,
+            "y": 5.56,
+            "heading": -2.5738443375571516e-19,
+            "angularVelocity": 5.201155568994596e-18,
+            "velocityX": -1.6527839316089838,
+            "velocityY": -1.4347514653062086e-33,
+            "timestamp": 0.792879481868296
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 2.5969589542999405e-18,
+            "velocityX": -0.8263919813283321,
+            "velocityY": 2.9921012122849815e-33,
+            "timestamp": 0.891989417101833
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 1.808288986442481e-43,
+            "timestamp": 0.9910993523353699
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S2 To W3.traj b/src/main/deploy/choreo/S2 To W3.traj
new file mode 100644
index 00000000..4a59e123
--- /dev/null
+++ b/src/main/deploy/choreo/S2 To W3.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 9.014129666558106e-32,
+            "angularVelocity": 9.039417129994225e-31,
+            "velocityX": 0,
+            "velocityY": -2.779554313855136e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 4.206523478699805,
+            "y": 5.55440930401173,
+            "heading": -0.10760119525493232,
+            "angularVelocity": -1.2344158099898592,
+            "velocityX": -0.6479064718660169,
+            "velocityY": -0.06413723844253004,
+            "timestamp": 0.08716770668695249
+        },
+        {
+            "x": 4.091196989898444,
+            "y": 5.541197271810027,
+            "heading": -0.295913717265277,
+            "angularVelocity": -2.1603473254910437,
+            "velocityX": -1.3230414471673093,
+            "velocityY": -0.15157026270235532,
+            "timestamp": 0.17433541337390498
+        },
+        {
+            "x": 3.9151950960136452,
+            "y": 5.516370757100726,
+            "heading": -0.5340653762567812,
+            "angularVelocity": -2.7321088054637612,
+            "velocityX": -2.019118095155108,
+            "velocityY": -0.2848132141236808,
+            "timestamp": 0.26150312006085746
+        },
+        {
+            "x": 3.678758800597895,
+            "y": 5.473177706772957,
+            "heading": -0.7546654655667131,
+            "angularVelocity": -2.530754767957573,
+            "velocityX": -2.712429917020411,
+            "velocityY": -0.4955166536948123,
+            "timestamp": 0.34867082674780997
+        },
+        {
+            "x": 3.3968844521889907,
+            "y": 5.398848640381046,
+            "heading": -0.8527884173152881,
+            "angularVelocity": -1.1256800881657552,
+            "velocityX": -3.233701552126478,
+            "velocityY": -0.8527133409492049,
+            "timestamp": 0.4358385334347625
+        },
+        {
+            "x": 3.098615494079836,
+            "y": 5.268356714792353,
+            "heading": -0.864385170870599,
+            "angularVelocity": -0.1330395624260059,
+            "velocityX": -3.421782784538955,
+            "velocityY": -1.4970214377364781,
+            "timestamp": 0.5230062401217149
+        },
+        {
+            "x": 2.832096469446971,
+            "y": 5.0836195976119996,
+            "heading": -0.8474300397750907,
+            "angularVelocity": 0.19451161146636953,
+            "velocityX": -3.0575431517318843,
+            "velocityY": -2.1193297862453186,
+            "timestamp": 0.6101739468086674
+        },
+        {
+            "x": 2.620164345186853,
+            "y": 4.879059766080367,
+            "heading": -0.7372424721392209,
+            "angularVelocity": 1.2640870320425897,
+            "velocityX": -2.431314672774813,
+            "velocityY": -2.346738709855847,
+            "timestamp": 0.6973416534956199
+        },
+        {
+            "x": 2.469226841034766,
+            "y": 4.6911608685478585,
+            "heading": -0.6128307853924054,
+            "angularVelocity": 1.427268095897297,
+            "velocityX": -1.7315759458276552,
+            "velocityY": -2.155602168212534,
+            "timestamp": 0.7845093601825724
+        },
+        {
+            "x": 2.3748885049182054,
+            "y": 4.5313089505917965,
+            "heading": -0.5057978702393489,
+            "angularVelocity": 1.2278964219794164,
+            "velocityX": -1.0822624536327627,
+            "velocityY": -1.8338433352404204,
+            "timestamp": 0.871677066869525
+        },
+        {
+            "x": 2.3223904484665,
+            "y": 4.418253055823363,
+            "heading": -0.42949016513376354,
+            "angularVelocity": 0.87541255822675,
+            "velocityX": -0.6022649722820163,
+            "velocityY": -1.2969928780443263,
+            "timestamp": 0.9588447735564775
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 0.4530366420626772,
+            "velocityX": -0.256866324898396,
+            "velocityY": -0.6682871218875669,
+            "timestamp": 1.0460124802434299
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -3.4100744325418914e-31,
+            "velocityX": -9.400788860397695e-33,
+            "velocityY": 0,
+            "timestamp": 1.1331801869303824
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S3 To C1.traj b/src/main/deploy/choreo/S3 To C1.traj
new file mode 100644
index 00000000..97fb7eaf
--- /dev/null
+++ b/src/main/deploy/choreo/S3 To C1.traj	
@@ -0,0 +1,220 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 3.2688003829398086e-32,
+            "velocityX": -2.078713781970587e-32,
+            "velocityY": -4.638967376158192e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 4.29154268151456,
+            "y": 3.0623077457400183,
+            "heading": -0.5127665183876435,
+            "angularVelocity": 0.580669777278592,
+            "velocityX": 0.31134301225465,
+            "velocityY": 0.6796516730776717,
+            "timestamp": 0.09167599846825866
+        },
+        {
+            "x": 4.3492977760941525,
+            "y": 3.186661868715021,
+            "heading": -0.4070750475472326,
+            "angularVelocity": 1.1528804987818582,
+            "velocityX": 0.6299914431757085,
+            "velocityY": 1.3564523436094174,
+            "timestamp": 0.18335199693651733
+        },
+        {
+            "x": 4.438053987492854,
+            "y": 3.3723523918382017,
+            "heading": -0.25179906449139605,
+            "angularVelocity": 1.6937473891773143,
+            "velocityX": 0.968151019696088,
+            "velocityY": 2.025508597950785,
+            "timestamp": 0.27502799540477596
+        },
+        {
+            "x": 4.5709698511757795,
+            "y": 3.6117104026755453,
+            "heading": -0.06452556421817643,
+            "angularVelocity": 2.042775681773022,
+            "velocityX": 1.449843643960376,
+            "velocityY": 2.6109125053076734,
+            "timestamp": 0.36670399387303465
+        },
+        {
+            "x": 4.761013106791701,
+            "y": 3.810602792551275,
+            "heading": 0.11153690871840848,
+            "angularVelocity": 1.9204860146414893,
+            "velocityX": 2.0729881189319217,
+            "velocityY": 2.1695143025313537,
+            "timestamp": 0.45837999234129334
+        },
+        {
+            "x": 5.007328053154023,
+            "y": 3.968591564774131,
+            "heading": 0.25752334504560864,
+            "angularVelocity": 1.5924171949733021,
+            "velocityX": 2.6867986220799525,
+            "velocityY": 1.7233384404049508,
+            "timestamp": 0.550055990809552
+        },
+        {
+            "x": 5.305296422655045,
+            "y": 4.081628168088871,
+            "heading": 0.35020186087766364,
+            "angularVelocity": 1.0109354398157564,
+            "velocityX": 3.2502331524012615,
+            "velocityY": 1.2330010602925316,
+            "timestamp": 0.6417319892778107
+        },
+        {
+            "x": 5.6446965706540295,
+            "y": 4.152909996485894,
+            "heading": 0.3502021264888923,
+            "angularVelocity": 0.0000028972820926514052,
+            "velocityX": 3.702170182705961,
+            "velocityY": 0.7775407913523397,
+            "timestamp": 0.7334079877460694
+        },
+        {
+            "x": 5.981889969698284,
+            "y": 4.233991234618027,
+            "heading": 0.3502024148432213,
+            "angularVelocity": 0.0000031453633856654694,
+            "velocityX": 3.6780990082262504,
+            "velocityY": 0.884432561268565,
+            "timestamp": 0.8250839862143281
+        },
+        {
+            "x": 6.295899946950055,
+            "y": 4.381202864314355,
+            "heading": 0.350202827556894,
+            "angularVelocity": 0.000004501872678764248,
+            "velocityX": 3.425214696303447,
+            "velocityY": 1.6057815803042044,
+            "timestamp": 0.9167599846825868
+        },
+        {
+            "x": 6.57390491063703,
+            "y": 4.588535293884794,
+            "heading": 0.350203324900352,
+            "angularVelocity": 0.0000054250127210279085,
+            "velocityX": 3.0324727118541346,
+            "velocityY": 2.2615780906081357,
+            "timestamp": 1.0084359831508454
+        },
+        {
+            "x": 6.804553755441848,
+            "y": 4.847523149382792,
+            "heading": 0.35020327996165895,
+            "angularVelocity": -4.901903857536901e-7,
+            "velocityX": 2.5159130923965525,
+            "velocityY": 2.825034467311176,
+            "timestamp": 1.100111981619104
+        },
+        {
+            "x": 6.978428828519116,
+            "y": 5.147591577808381,
+            "heading": 0.35020324373344786,
+            "angularVelocity": -3.9517661894346874e-7,
+            "velocityX": 1.8966258997164847,
+            "velocityY": 3.273140554116584,
+            "timestamp": 1.1917879800873628
+        },
+        {
+            "x": 7.088431556948234,
+            "y": 5.476488224562697,
+            "heading": 0.3502032113343976,
+            "angularVelocity": -3.5340820734555857e-7,
+            "velocityX": 1.1999076123202526,
+            "velocityY": 3.587598196360961,
+            "timestamp": 1.2834639785556214
+        },
+        {
+            "x": 7.187536215850895,
+            "y": 5.808831226900728,
+            "heading": 0.3502031791295722,
+            "angularVelocity": -3.5128960565806065e-7,
+            "velocityX": 1.0810316828670656,
+            "velocityY": 3.6251909757285055,
+            "timestamp": 1.3751399770238801
+        },
+        {
+            "x": 7.286640767886556,
+            "y": 6.141174261106461,
+            "heading": 0.3502031469247365,
+            "angularVelocity": -3.512897194087527e-7,
+            "velocityX": 1.0810305171639119,
+            "velocityY": 3.6251913233407715,
+            "timestamp": 1.4668159754921388
+        },
+        {
+            "x": 7.3857452620780215,
+            "y": 6.473517312561292,
+            "heading": 0.350203114719879,
+            "angularVelocity": -3.512899557892109e-7,
+            "velocityX": 1.0810298862005645,
+            "velocityY": 3.625191511493593,
+            "timestamp": 1.5584919739603975
+        },
+        {
+            "x": 7.481976228665169,
+            "y": 6.80241098377653,
+            "heading": 0.33998449018149474,
+            "angularVelocity": -0.11146455679915658,
+            "velocityX": 1.0496855032395944,
+            "velocityY": 3.5875657392388547,
+            "timestamp": 1.6501679724286562
+        },
+        {
+            "x": 7.561455250393006,
+            "y": 7.068914029375232,
+            "heading": 0.2316909043200923,
+            "angularVelocity": -1.1812643185871456,
+            "velocityX": 0.8669556160367877,
+            "velocityY": 2.907010014087551,
+            "timestamp": 1.741843970896915
+        },
+        {
+            "x": 7.620860593043084,
+            "y": 7.268277067392774,
+            "heading": 0.12481105079738766,
+            "angularVelocity": -1.1658433538600752,
+            "velocityX": 0.6479923168837385,
+            "velocityY": 2.1746481232660675,
+            "timestamp": 1.8335199693651736
+        },
+        {
+            "x": 7.66032628893014,
+            "y": 7.4008489817464715,
+            "heading": 0.04379045785830094,
+            "angularVelocity": -0.8837710446877722,
+            "velocityX": 0.430491039600946,
+            "velocityY": 1.4460918513977026,
+            "timestamp": 1.9251959678334323
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 6.52409657127002e-31,
+            "angularVelocity": -0.4776654586801488,
+            "velocityX": 0.21460045593800534,
+            "velocityY": 0.7215740145599008,
+            "timestamp": 2.0168719663016907
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": -6.745238901014272e-30,
+            "velocityX": 0,
+            "velocityY": 3.373152313643601e-31,
+            "timestamp": 2.1085479647699494
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S3 To C2.traj b/src/main/deploy/choreo/S3 To C2.traj
new file mode 100644
index 00000000..69777c27
--- /dev/null
+++ b/src/main/deploy/choreo/S3 To C2.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -2.8196964588027846e-31,
+            "velocityX": -1.564828148658428e-31,
+            "velocityY": 3.07478616058615e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 4.294807569207289,
+            "y": 3.0477321348748183,
+            "heading": -0.5960373953696199,
+            "angularVelocity": -0.36026847684168845,
+            "velocityX": 0.381499939303545,
+            "velocityY": 0.5724991570056517,
+            "timestamp": 0.08337503084628527
+        },
+        {
+            "x": 4.359939756522268,
+            "y": 3.142193816557181,
+            "heading": -0.6551934033575421,
+            "angularVelocity": -0.7095170746861289,
+            "velocityX": 0.7811953609355742,
+            "velocityY": 1.1329732741750633,
+            "timestamp": 0.16675006169257053
+        },
+        {
+            "x": 4.461050205756915,
+            "y": 3.2814565261988764,
+            "heading": -0.7417418377173162,
+            "angularVelocity": -1.038061797174498,
+            "velocityX": 1.2127185826300881,
+            "velocityY": 1.6703167390540123,
+            "timestamp": 0.2501250925388558
+        },
+        {
+            "x": 4.603825208189836,
+            "y": 3.4604941356958814,
+            "heading": -0.8514332359801334,
+            "angularVelocity": -1.315638472926628,
+            "velocityX": 1.7124431737380506,
+            "velocityY": 2.147376830685539,
+            "timestamp": 0.33350012338514107
+        },
+        {
+            "x": 4.8030511460470855,
+            "y": 3.6526461815261504,
+            "heading": -0.9644207302174959,
+            "angularVelocity": -1.3551718433024906,
+            "velocityX": 2.3895156119888465,
+            "velocityY": 2.3046713611960277,
+            "timestamp": 0.41687515423142635
+        },
+        {
+            "x": 5.051097476442208,
+            "y": 3.8467657037250893,
+            "heading": -0.9658198239131095,
+            "angularVelocity": -0.016780727772000164,
+            "velocityX": 2.975067329839247,
+            "velocityY": 2.328269269930804,
+            "timestamp": 0.5002501850777116
+        },
+        {
+            "x": 5.320526470824812,
+            "y": 4.010737925204951,
+            "heading": -0.965820109061322,
+            "angularVelocity": -0.000003420067250854742,
+            "velocityX": 3.2315309709370754,
+            "velocityY": 1.9666825884858703,
+            "timestamp": 0.5836252159239969
+        },
+        {
+            "x": 5.614243645591117,
+            "y": 4.1256688963704224,
+            "heading": -0.9658201941489747,
+            "angularVelocity": -0.000001020541184209393,
+            "velocityX": 3.5228433715103167,
+            "velocityY": 1.37848190278171,
+            "timestamp": 0.6670002467702821
+        },
+        {
+            "x": 5.906557788271019,
+            "y": 4.244122886725936,
+            "heading": -0.965820105112684,
+            "angularVelocity": 0.0000010679011417361832,
+            "velocityX": 3.506015406684877,
+            "velocityY": 1.4207369898777267,
+            "timestamp": 0.7503752776165674
+        },
+        {
+            "x": 6.172253111887642,
+            "y": 4.41406398245588,
+            "heading": -0.9657938677546691,
+            "angularVelocity": 0.0003146908342779248,
+            "velocityX": 3.1867493291423483,
+            "velocityY": 2.0382732576526004,
+            "timestamp": 0.8337503084628527
+        },
+        {
+            "x": 6.426785390452526,
+            "y": 4.5994304003649855,
+            "heading": -0.9640144900160125,
+            "angularVelocity": 0.021341854037057975,
+            "velocityX": 3.0528597828545716,
+            "velocityY": 2.2232845496735987,
+            "timestamp": 0.917125339309138
+        },
+        {
+            "x": 6.681598941757368,
+            "y": 4.785246665794811,
+            "heading": -0.9639002296367303,
+            "angularVelocity": 0.0013704388246982366,
+            "velocityX": 3.0562333676929114,
+            "velocityY": 2.2286800201898096,
+            "timestamp": 1.0005003701554231
+        },
+        {
+            "x": 6.9014822245071255,
+            "y": 5.01136660335564,
+            "heading": -0.9639000238890051,
+            "angularVelocity": 0.000002467737919950576,
+            "velocityX": 2.6372797769052228,
+            "velocityY": 2.7120822057351384,
+            "timestamp": 1.0838754010017084
+        },
+        {
+            "x": 7.122012116423131,
+            "y": 5.23107273201418,
+            "heading": -0.9515772318649645,
+            "angularVelocity": 0.1477995498047812,
+            "velocityX": 2.645035206314798,
+            "velocityY": 2.635154990990089,
+            "timestamp": 1.1672504318479937
+        },
+        {
+            "x": 7.311148990719613,
+            "y": 5.417840497645029,
+            "heading": -0.7917020160601519,
+            "angularVelocity": 1.9175431083146124,
+            "velocityX": 2.2685073981583956,
+            "velocityY": 2.2400923122317544,
+            "timestamp": 1.250625462694279
+        },
+        {
+            "x": 7.462007390054419,
+            "y": 5.572027096128993,
+            "heading": -0.5225549044290918,
+            "angularVelocity": 3.228150069620655,
+            "velocityX": 1.8093954245478587,
+            "velocityY": 1.8493138403538476,
+            "timestamp": 1.3340004935405643
+        },
+        {
+            "x": 7.571928087470151,
+            "y": 5.685474744918936,
+            "heading": -0.27088873612536774,
+            "angularVelocity": 3.018483660506339,
+            "velocityX": 1.318388686636741,
+            "velocityY": 1.3606909363440005,
+            "timestamp": 1.4173755243868496
+        },
+        {
+            "x": 7.6442805622690475,
+            "y": 5.7599789636443175,
+            "heading": -0.0924144798498188,
+            "angularVelocity": 2.1406199729580164,
+            "velocityX": 0.8677954786282445,
+            "velocityY": 0.8936034921863096,
+            "timestamp": 1.5007505552331348
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -2.5332548087748344e-31,
+            "angularVelocity": 1.1084191383412998,
+            "velocityX": 0.42841888474747514,
+            "velocityY": 0.4440302567795869,
+            "timestamp": 1.5841255860794201
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 5.737670210968143e-33,
+            "velocityX": 0,
+            "velocityY": -3.2710687085311585e-31,
+            "timestamp": 1.6675006169257054
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S3 To C3.traj b/src/main/deploy/choreo/S3 To C3.traj
new file mode 100644
index 00000000..9d039264
--- /dev/null
+++ b/src/main/deploy/choreo/S3 To C3.traj	
@@ -0,0 +1,166 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -1.0027901855857707e-30,
+            "velocityX": -5.17158696002297e-32,
+            "velocityY": 1.9371271312239428e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 4.300239460670815,
+            "y": 3.0530479185441526,
+            "heading": -0.6028935811234369,
+            "angularVelocity": -0.4158951640704737,
+            "velocityX": 0.4197942063083121,
+            "velocityY": 0.5980003056006388,
+            "timestamp": 0.08870884855296232
+        },
+        {
+            "x": 4.375862012794379,
+            "y": 3.1584586937478076,
+            "heading": -0.6740120078870278,
+            "angularVelocity": -0.8017061197804932,
+            "velocityX": 0.8524803709791626,
+            "velocityY": 1.1882780232540273,
+            "timestamp": 0.17741769710592464
+        },
+        {
+            "x": 4.491898262382256,
+            "y": 3.3149103934712074,
+            "heading": -0.7745169367898204,
+            "angularVelocity": -1.1329752391362358,
+            "velocityX": 1.3080572172977813,
+            "velocityY": 1.763653821185524,
+            "timestamp": 0.26612654565888694
+        },
+        {
+            "x": 4.65291407862912,
+            "y": 3.5188640922932937,
+            "heading": -0.893163244153065,
+            "angularVelocity": -1.3374799616794542,
+            "velocityX": 1.8151043427277864,
+            "velocityY": 2.299135905256609,
+            "timestamp": 0.3548353942118493
+        },
+        {
+            "x": 4.870914712338585,
+            "y": 3.7498942741357597,
+            "heading": -0.9455687285938831,
+            "angularVelocity": -0.5907582534963245,
+            "velocityX": 2.457484650804715,
+            "velocityY": 2.6043645657798487,
+            "timestamp": 0.44354424276481164
+        },
+        {
+            "x": 5.142859309500119,
+            "y": 3.9465163862068953,
+            "heading": -0.9455688623878027,
+            "angularVelocity": -0.0000015082364580335284,
+            "velocityX": 3.065585920655626,
+            "velocityY": 2.216488155110533,
+            "timestamp": 0.532253091317774
+        },
+        {
+            "x": 5.447866241414174,
+            "y": 4.086462405550872,
+            "heading": -0.9455689961292948,
+            "angularVelocity": -0.0000015076454528851402,
+            "velocityX": 3.438292085732067,
+            "velocityY": 1.5775880492961702,
+            "timestamp": 0.6209619398707363
+        },
+        {
+            "x": 5.774274975125892,
+            "y": 4.16438201701327,
+            "heading": -0.9455689173593601,
+            "angularVelocity": 8.879602884106285e-7,
+            "velocityX": 3.6795510147653454,
+            "velocityY": 0.8783747363813094,
+            "timestamp": 0.7096707884236987
+        },
+        {
+            "x": 6.109606649873876,
+            "y": 4.1772967927030695,
+            "heading": -0.9455688167433821,
+            "angularVelocity": 0.0000011342270768412302,
+            "velocityX": 3.780137835379285,
+            "velocityY": 0.14558610443566755,
+            "timestamp": 0.798379636976661
+        },
+        {
+            "x": 6.445060320326861,
+            "y": 4.168079453807152,
+            "heading": -0.9455687201101868,
+            "angularVelocity": 0.0000010893298350239914,
+            "velocityX": 3.781513072539862,
+            "velocityY": -0.10390551840399499,
+            "timestamp": 0.8870884855296234
+        },
+        {
+            "x": 6.780513985890507,
+            "y": 4.158861936977066,
+            "heading": -0.9455686234766241,
+            "angularVelocity": 0.0000010893339756447378,
+            "velocityX": 3.781513017423149,
+            "velocityY": -0.10390752422611307,
+            "timestamp": 0.9757973340825857
+        },
+        {
+            "x": 7.079210544299498,
+            "y": 4.144050975293544,
+            "heading": -0.856180476499644,
+            "angularVelocity": 1.0076576174203435,
+            "velocityX": 3.3671563015572095,
+            "velocityY": -0.16696149172400718,
+            "timestamp": 1.064506182635548
+        },
+        {
+            "x": 7.324091725395792,
+            "y": 4.136558502839443,
+            "heading": -0.6317167360096363,
+            "angularVelocity": 2.5303421716267107,
+            "velocityX": 2.760504561730298,
+            "velocityY": -0.08446138774564337,
+            "timestamp": 1.1532150311885103
+        },
+        {
+            "x": 7.50528856962614,
+            "y": 4.13147246408345,
+            "heading": -0.35065072299807204,
+            "angularVelocity": 3.1684101146207286,
+            "velocityX": 2.042601692909661,
+            "velocityY": -0.05733406350050504,
+            "timestamp": 1.2419238797414727
+        },
+        {
+            "x": 7.622685008969739,
+            "y": 4.128418008534293,
+            "heading": -0.12509027443555182,
+            "angularVelocity": 2.5427051781407424,
+            "velocityX": 1.323390408720159,
+            "velocityY": -0.03443236609404116,
+            "timestamp": 1.330632728294435
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.405276849475189e-32,
+            "angularVelocity": 1.4101217237744719,
+            "velocityX": 0.6461022994345517,
+            "velocityY": -0.01598497283444213,
+            "timestamp": 1.4193415768473974
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": -2.3485646679922316e-31,
+            "velocityX": 0,
+            "velocityY": -2.6996723431981715e-31,
+            "timestamp": 1.5080504254003597
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S3 To C4.traj b/src/main/deploy/choreo/S3 To C4.traj
new file mode 100644
index 00000000..0b021e4e
--- /dev/null
+++ b/src/main/deploy/choreo/S3 To C4.traj	
@@ -0,0 +1,238 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 0,
+            "velocityX": 5.832812358477626e-33,
+            "velocityY": -2.114400767819492e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 4.2637473414422145,
+            "y": 2.974020167404826,
+            "heading": -0.5570803398431683,
+            "angularVelocity": 0.15939480257853877,
+            "velocityX": 0.013355031418934325,
+            "velocityY": -0.46426099366130524,
+            "timestamp": 0.05595954204614445
+        },
+        {
+            "x": 4.266239342094047,
+            "y": 2.922097369670203,
+            "heading": -0.5396590874905302,
+            "angularVelocity": 0.31131870840312753,
+            "velocityX": 0.04453218451605929,
+            "velocityY": -0.9278631639230981,
+            "timestamp": 0.1119190840922889
+        },
+        {
+            "x": 4.271816064940073,
+            "y": 2.844342168020101,
+            "heading": -0.5143157133438189,
+            "angularVelocity": 0.4528874472527088,
+            "velocityX": 0.0996563345966598,
+            "velocityY": -1.3894895992176792,
+            "timestamp": 0.16787862613843335
+        },
+        {
+            "x": 4.282354343554946,
+            "y": 2.7410289237168692,
+            "heading": -0.48190029722305777,
+            "angularVelocity": 0.5792652143941603,
+            "velocityX": 0.18831960072480589,
+            "velocityY": -1.8462131841257783,
+            "timestamp": 0.2238381681845778
+        },
+        {
+            "x": 4.3006106845058465,
+            "y": 2.6128237251760056,
+            "heading": -0.4437500478548854,
+            "angularVelocity": 0.6817469902936669,
+            "velocityX": 0.32624178617911886,
+            "velocityY": -2.2910337335345985,
+            "timestamp": 0.27979771023072225
+        },
+        {
+            "x": 4.330801306241698,
+            "y": 2.4614150778969766,
+            "heading": -0.40214700400499553,
+            "angularVelocity": 0.7434486117771366,
+            "velocityX": 0.5395080201148661,
+            "velocityY": -2.705680599640681,
+            "timestamp": 0.3357572522768667
+        },
+        {
+            "x": 4.379172513113433,
+            "y": 2.2912661424924936,
+            "heading": -0.3611794534128463,
+            "angularVelocity": 0.7320923133783194,
+            "velocityX": 0.864396045840541,
+            "velocityY": -3.0405705476320635,
+            "timestamp": 0.39171679432301115
+        },
+        {
+            "x": 4.451949891834961,
+            "y": 2.111933259909663,
+            "heading": -0.32494756197390784,
+            "angularVelocity": 0.6474658318159084,
+            "velocityX": 1.3005356380778725,
+            "velocityY": -3.2046881733762564,
+            "timestamp": 0.4476763363691556
+        },
+        {
+            "x": 4.550285732793113,
+            "y": 1.9373903674143325,
+            "heading": -0.2971520965733793,
+            "angularVelocity": 0.49670644869849423,
+            "velocityX": 1.7572667209653445,
+            "velocityY": -3.1190907951212283,
+            "timestamp": 0.5036358784153001
+        },
+        {
+            "x": 4.671000177640236,
+            "y": 1.7762856565305696,
+            "heading": -0.2715476133805437,
+            "angularVelocity": 0.457553479828869,
+            "velocityX": 2.157173565637473,
+            "velocityY": -2.878949773229245,
+            "timestamp": 0.5595954204614446
+        },
+        {
+            "x": 4.810884164899462,
+            "y": 1.6328902215117949,
+            "heading": -0.24301377044973924,
+            "angularVelocity": 0.5099012945331528,
+            "velocityX": 2.4997343106181424,
+            "velocityY": -2.5624840693036233,
+            "timestamp": 0.6155549625075891
+        },
+        {
+            "x": 4.966899091534391,
+            "y": 1.5100232904396305,
+            "heading": -0.21315700918191657,
+            "angularVelocity": 0.5335419157506132,
+            "velocityX": 2.7879950573269263,
+            "velocityY": -2.1956386092446096,
+            "timestamp": 0.6715145045537336
+        },
+        {
+            "x": 5.136752128601074,
+            "y": 1.409298062324524,
+            "heading": -0.1837509654270181,
+            "angularVelocity": 0.5254875697633724,
+            "velocityX": 3.035282828558936,
+            "velocityY": -1.7999651968567865,
+            "timestamp": 0.7274740465998781
+        },
+        {
+            "x": 5.4382274814533025,
+            "y": 1.3074419969375322,
+            "heading": -0.14111678525881038,
+            "angularVelocity": 0.4782734646509064,
+            "velocityX": 3.3819733590888914,
+            "velocityY": -1.142629061849971,
+            "timestamp": 0.8166158939305274
+        },
+        {
+            "x": 5.764139683697637,
+            "y": 1.267066681764717,
+            "heading": -0.11133632809533998,
+            "angularVelocity": 0.3340794257159682,
+            "velocityX": 3.6561077877984602,
+            "velocityY": -0.4529333459183563,
+            "timestamp": 0.9057577412611766
+        },
+        {
+            "x": 6.100462721846291,
+            "y": 1.2915989985388854,
+            "heading": -0.11133120767369757,
+            "angularVelocity": 0.000057441278584387276,
+            "velocityX": 3.7728973340784897,
+            "velocityY": 0.2752053890377148,
+            "timestamp": 0.9948995885918258
+        },
+        {
+            "x": 6.42549869748969,
+            "y": 1.3814198753662668,
+            "heading": -0.1113311879575033,
+            "angularVelocity": 2.2117776172639892e-7,
+            "velocityX": 3.6462782113742715,
+            "velocityY": 1.0076174043625032,
+            "timestamp": 1.084041435922475
+        },
+        {
+            "x": 6.7266979846048365,
+            "y": 1.533061465523067,
+            "heading": -0.11133117504978503,
+            "angularVelocity": 1.4479976182990285e-7,
+            "velocityX": 3.3788764327254777,
+            "velocityY": 1.7011268522889103,
+            "timestamp": 1.1731832832531242
+        },
+        {
+            "x": 6.992432961420095,
+            "y": 1.740669527522903,
+            "heading": -0.11133116451645807,
+            "angularVelocity": 1.1816366034373416e-7,
+            "velocityX": 2.981035111708839,
+            "velocityY": 2.3289629754897407,
+            "timestamp": 1.2623251305837733
+        },
+        {
+            "x": 7.221595592407175,
+            "y": 1.979432543612985,
+            "heading": -0.09587073631794915,
+            "angularVelocity": 0.17343625537800947,
+            "velocityX": 2.5707637641507293,
+            "velocityY": 2.678461611912118,
+            "timestamp": 1.3514669779144224
+        },
+        {
+            "x": 7.404993837760709,
+            "y": 2.170501480308595,
+            "heading": -0.06411284637687509,
+            "angularVelocity": 0.3562624164974272,
+            "velocityX": 2.0573754173309005,
+            "velocityY": 2.1434258142181197,
+            "timestamp": 1.4406088252450715
+        },
+        {
+            "x": 7.54251389523076,
+            "y": 2.3137697288578556,
+            "heading": -0.0342508837604198,
+            "angularVelocity": 0.33499376006509896,
+            "velocityX": 1.5427104282454527,
+            "velocityY": 1.6071940714649837,
+            "timestamp": 1.5297506725757206
+        },
+        {
+            "x": 7.634176122098128,
+            "y": 2.4092619199619465,
+            "heading": -0.011960250388889534,
+            "angularVelocity": 0.25005801471490263,
+            "velocityX": 1.028273808679041,
+            "velocityY": 1.0712386377846093,
+            "timestamp": 1.6188925199063697
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 9.313254165846089e-33,
+            "angularVelocity": 0.1341709954082624,
+            "velocityX": 0.5140557355952123,
+            "velocityY": 0.5355294002487934,
+            "timestamp": 1.7080343672370188
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": -1.1876173054192403e-31,
+            "velocityX": 4.5955148793706025e-32,
+            "velocityY": 0,
+            "timestamp": 1.797176214567668
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S3 To C5.traj b/src/main/deploy/choreo/S3 To C5.traj
new file mode 100644
index 00000000..d933d22a
--- /dev/null
+++ b/src/main/deploy/choreo/S3 To C5.traj	
@@ -0,0 +1,184 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -2.2118032332823513e-27,
+            "velocityX": -2.2398078489200873e-28,
+            "velocityY": 7.32316842173409e-29,
+            "timestamp": 0
+        },
+        {
+            "x": 4.257275610783411,
+            "y": 2.9390850995701614,
+            "heading": -0.6153074756658753,
+            "angularVelocity": -0.5685220144659363,
+            "velocityX": -0.06600299944485946,
+            "velocityY": -0.7023572274929988,
+            "timestamp": 0.0867292284401611
+        },
+        {
+            "x": 4.259999029271463,
+            "y": 2.8165594202827866,
+            "heading": -0.6936339270516045,
+            "angularVelocity": -0.9031148183195571,
+            "velocityX": 0.03140139186100546,
+            "velocityY": -1.4127380295088343,
+            "timestamp": 0.1734584568803222
+        },
+        {
+            "x": 4.293815085802935,
+            "y": 2.6396142169455077,
+            "heading": -0.7636289676296801,
+            "angularVelocity": -0.8070524993355632,
+            "velocityX": 0.3899038091270867,
+            "velocityY": -2.0402026689233352,
+            "timestamp": 0.2601876853204833
+        },
+        {
+            "x": 4.379794167011224,
+            "y": 2.432035667571809,
+            "heading": -0.7730870281638328,
+            "angularVelocity": -0.10905274616477362,
+            "velocityX": 0.9913506986587586,
+            "velocityY": -2.3934093858210397,
+            "timestamp": 0.3469169137606444
+        },
+        {
+            "x": 4.522000800858502,
+            "y": 2.2059673134468256,
+            "heading": -0.7069342437539652,
+            "angularVelocity": 0.7627507542686203,
+            "velocityX": 1.6396621577856372,
+            "velocityY": -2.6065993920487904,
+            "timestamp": 0.43364614220080544
+        },
+        {
+            "x": 4.726803922559788,
+            "y": 1.9764479224139355,
+            "heading": -0.6474090008701758,
+            "angularVelocity": 0.6863342837743838,
+            "velocityX": 2.3614083208706247,
+            "velocityY": -2.6463903249323897,
+            "timestamp": 0.5203753706409665
+        },
+        {
+            "x": 4.985426543182091,
+            "y": 1.774560944536545,
+            "heading": -0.6474088034070749,
+            "angularVelocity": 0.000002276776866191791,
+            "velocityX": 2.9819545875556583,
+            "velocityY": -2.3277847792302735,
+            "timestamp": 0.6071045990811276
+        },
+        {
+            "x": 5.274431548584119,
+            "y": 1.6192543598542732,
+            "heading": -0.6474086941993331,
+            "angularVelocity": 0.000001259180368575944,
+            "velocityX": 3.3322676864516145,
+            "velocityY": -1.7907064028526984,
+            "timestamp": 0.6938338275212886
+        },
+        {
+            "x": 5.576835908979959,
+            "y": 1.4919922741488194,
+            "heading": -0.6474086295363626,
+            "angularVelocity": 7.455729944345581e-7,
+            "velocityX": 3.4867641028824017,
+            "velocityY": -1.4673494506325018,
+            "timestamp": 0.7805630559614497
+        },
+        {
+            "x": 5.888110144336631,
+            "y": 1.388298557512795,
+            "heading": -0.6474085736154427,
+            "angularVelocity": 6.447759407143774e-7,
+            "velocityX": 3.589034988031027,
+            "velocityY": -1.1956028953671916,
+            "timestamp": 0.8672922844016108
+        },
+        {
+            "x": 6.199390629137113,
+            "y": 1.2846236026664921,
+            "heading": -0.6474085176958322,
+            "angularVelocity": 6.447608435198244e-7,
+            "velocityX": 3.5891070449826046,
+            "velocityY": -1.1953865693365275,
+            "timestamp": 0.9540215128417718
+        },
+        {
+            "x": 6.510671944712167,
+            "y": 1.1809511422971397,
+            "heading": -0.6474084617763125,
+            "angularVelocity": 6.447597973785494e-7,
+            "velocityX": 3.589116623928252,
+            "velocityY": -1.1953578076724478,
+            "timestamp": 1.040750741281933
+        },
+        {
+            "x": 6.8219538378473334,
+            "y": 1.0772804160640193,
+            "heading": -0.6474084058668956,
+            "angularVelocity": 6.446433092317229e-7,
+            "velocityX": 3.5891232832762197,
+            "velocityY": -1.1953378128419065,
+            "timestamp": 1.127479969722094
+        },
+        {
+            "x": 7.104843996542823,
+            "y": 0.9795715918943764,
+            "heading": -0.5659567926753784,
+            "angularVelocity": 0.9391483662017565,
+            "velocityX": 3.2617626581409067,
+            "velocityY": -1.126596257420401,
+            "timestamp": 1.2142091981622551
+        },
+        {
+            "x": 7.336171547926519,
+            "y": 0.9014041069183023,
+            "heading": -0.36771202193482067,
+            "angularVelocity": 2.2857896271651064,
+            "velocityX": 2.6672386638755765,
+            "velocityY": -0.9012819136285574,
+            "timestamp": 1.3009384266024162
+        },
+        {
+            "x": 7.508580909570239,
+            "y": 0.8437450323938049,
+            "heading": -0.19262457792893875,
+            "angularVelocity": 2.018782446873579,
+            "velocityX": 1.9879037868147627,
+            "velocityY": -0.6648171044699364,
+            "timestamp": 1.3876676550425773
+        },
+        {
+            "x": 7.622996353926532,
+            "y": 0.8057914476404057,
+            "heading": -0.06643070405801102,
+            "angularVelocity": 1.4550328204291798,
+            "velocityX": 1.3192258989739947,
+            "velocityY": -0.4376100818144245,
+            "timestamp": 1.4743968834827383
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -1.2156690964354266e-28,
+            "angularVelocity": 0.7659552062526378,
+            "velocityX": 0.6572599237729437,
+            "velocityY": -0.21666799046150395,
+            "timestamp": 1.5611261119228994
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 1.25023472254267e-27,
+            "velocityX": 0,
+            "velocityY": -4.9030509543456705e-29,
+            "timestamp": 1.6478553403630605
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S3 To W1.traj b/src/main/deploy/choreo/S3 To W1.traj
new file mode 100644
index 00000000..44ee68f9
--- /dev/null
+++ b/src/main/deploy/choreo/S3 To W1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -1.6794679447375943e-30,
+            "velocityX": 6.742046899995187e-33,
+            "velocityY": 9.708842875589211e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 4.290023963257714,
+            "y": 3.0497874441329906,
+            "heading": -0.5200869306801483,
+            "angularVelocity": 0.5503538158132694,
+            "velocityX": 0.3239326300245928,
+            "velocityY": 0.5967954280576505,
+            "timestamp": 0.08342464065958187
+        },
+        {
+            "x": 4.342730369898429,
+            "y": 3.150178003649595,
+            "heading": -0.42997936088427535,
+            "angularVelocity": 1.0801073769506666,
+            "velocityX": 0.6317846408926776,
+            "velocityY": 1.2033681982072046,
+            "timestamp": 0.16684928131916374
+        },
+        {
+            "x": 4.418537982231504,
+            "y": 3.3026029150734924,
+            "heading": -0.2990998156933407,
+            "angularVelocity": 1.5688355880967437,
+            "velocityX": 0.9086957010988046,
+            "velocityY": 1.8270970089745318,
+            "timestamp": 0.2502739219787456
+        },
+        {
+            "x": 4.5104462205151705,
+            "y": 3.5100667276192694,
+            "heading": -0.13617594937576347,
+            "angularVelocity": 1.9529465758491624,
+            "velocityX": 1.1016917490685052,
+            "velocityY": 2.4868409489750483,
+            "timestamp": 0.3336985626383275
+        },
+        {
+            "x": 4.573472263152817,
+            "y": 3.7674681923433693,
+            "heading": 0.004287761033867021,
+            "angularVelocity": 1.6837196935950753,
+            "velocityX": 0.7554847361563879,
+            "velocityY": 3.0854369007645883,
+            "timestamp": 0.41712320329790936
+        },
+        {
+            "x": 4.6071871310155705,
+            "y": 4.066840423815047,
+            "heading": 0.04705620405427594,
+            "angularVelocity": 0.5126596013152507,
+            "velocityX": 0.40413560785150865,
+            "velocityY": 3.5885348633778387,
+            "timestamp": 0.5005478439574912
+        },
+        {
+            "x": 4.587417601167403,
+            "y": 4.370555626705338,
+            "heading": 0.08458578048595149,
+            "angularVelocity": 0.4498620088136386,
+            "velocityX": -0.23697470785444333,
+            "velocityY": 3.6405934804036506,
+            "timestamp": 0.5839724846170731
+        },
+        {
+            "x": 4.510022832963655,
+            "y": 4.67116829443229,
+            "heading": 0.1002321163817716,
+            "angularVelocity": 0.1875505338964035,
+            "velocityX": -0.9277207260569706,
+            "velocityY": 3.603403806719601,
+            "timestamp": 0.667397125276655
+        },
+        {
+            "x": 4.376634060333898,
+            "y": 4.957183607465761,
+            "heading": 0.10023219176740035,
+            "angularVelocity": 9.036374403612271e-7,
+            "velocityX": -1.5989133615097586,
+            "velocityY": 3.4284272700744434,
+            "timestamp": 0.7508217659362368
+        },
+        {
+            "x": 4.19313075849175,
+            "y": 5.213940045953902,
+            "heading": 0.10023221692184411,
+            "angularVelocity": 3.015229499928316e-7,
+            "velocityX": -2.1996295146291525,
+            "velocityY": 3.0777050576201734,
+            "timestamp": 0.8342464065958187
+        },
+        {
+            "x": 3.9657176811920825,
+            "y": 5.432756463781841,
+            "heading": 0.10023223856547658,
+            "angularVelocity": 2.5943932502769966e-7,
+            "velocityX": -2.725970115084308,
+            "velocityY": 2.6229231087830502,
+            "timestamp": 0.9176710472554006
+        },
+        {
+            "x": 3.7186426713834653,
+            "y": 5.629101242954768,
+            "heading": 0.10023225962798245,
+            "angularVelocity": 2.524734383899783e-7,
+            "velocityX": -2.961655068037016,
+            "velocityY": 2.3535585843770224,
+            "timestamp": 1.0010956879149824
+        },
+        {
+            "x": 3.4715770348675625,
+            "y": 5.825457816649942,
+            "heading": 0.10023228069061084,
+            "angularVelocity": 2.5247490699416256e-7,
+            "velocityX": -2.961542711631996,
+            "velocityY": 2.353699963736333,
+            "timestamp": 1.0845203285745642
+        },
+        {
+            "x": 3.224511384705754,
+            "y": 6.021814373175139,
+            "heading": 0.10023230175325984,
+            "angularVelocity": 2.5247515406639525e-7,
+            "velocityX": -2.96154287520364,
+            "velocityY": 2.353699757922098,
+            "timestamp": 1.167944969234146
+        },
+        {
+            "x": 2.9774457295747583,
+            "y": 6.218170923447359,
+            "heading": 0.10023232281654228,
+            "angularVelocity": 2.524827469327523e-7,
+            "velocityX": -2.9615429347686173,
+            "velocityY": 2.3536996829685104,
+            "timestamp": 1.2513696098937277
+        },
+        {
+            "x": 2.751412346836958,
+            "y": 6.39797212959724,
+            "heading": 0.1658210049038553,
+            "angularVelocity": 0.7862027521934517,
+            "velocityX": -2.7094319010631414,
+            "velocityY": 2.155252989144637,
+            "timestamp": 1.3347942505533095
+        },
+        {
+            "x": 2.5706626829115913,
+            "y": 6.54172747182469,
+            "heading": 0.24638520673123038,
+            "angularVelocity": 0.9657123026291593,
+            "velocityX": -2.166622025534686,
+            "velocityY": 1.723176043563091,
+            "timestamp": 1.4182188912128912
+        },
+        {
+            "x": 2.435257350873027,
+            "y": 6.649419264379792,
+            "heading": 0.31518856404188306,
+            "angularVelocity": 0.8247366337651717,
+            "velocityX": -1.6230855891977036,
+            "velocityY": 1.2908871012647558,
+            "timestamp": 1.501643531872473
+        },
+        {
+            "x": 2.3450659538089185,
+            "y": 6.721154353305962,
+            "heading": 0.3643262231924431,
+            "angularVelocity": 0.5890065424563048,
+            "velocityX": -1.0811122031935256,
+            "velocityY": 0.8598789081859941,
+            "timestamp": 1.5850681725320548
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 0.3077481257883978,
+            "velocityX": -0.5401995555822935,
+            "velocityY": 0.42967696846676384,
+            "timestamp": 1.6684928131916366
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": -2.8369790280137563e-29,
+            "velocityX": 0,
+            "velocityY": -1.9155196850548007e-31,
+            "timestamp": 1.7519174538512183
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S3 To W2.traj b/src/main/deploy/choreo/S3 To W2.traj
new file mode 100644
index 00000000..01699583
--- /dev/null
+++ b/src/main/deploy/choreo/S3 To W2.traj	
@@ -0,0 +1,157 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.1175745999315144e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 4.331197427717942,
+            "y": 3.064203342806604,
+            "heading": -0.5918486780870994,
+            "angularVelocity": -0.24352382070717674,
+            "velocityX": 0.642497001367488,
+            "velocityY": 0.604868198278966,
+            "timestamp": 0.10614435176007272
+        },
+        {
+            "x": 4.457228812945604,
+            "y": 3.20188389088458,
+            "heading": -0.6500618235234895,
+            "angularVelocity": -0.5484337552691863,
+            "velocityX": 1.1873583769444531,
+            "velocityY": 1.2971066834454588,
+            "timestamp": 0.21228870352014545
+        },
+        {
+            "x": 4.610677951553499,
+            "y": 3.4285671398637003,
+            "heading": -0.7523465196948876,
+            "angularVelocity": -0.9636376733695737,
+            "velocityX": 1.4456646638603075,
+            "velocityY": 2.1356129197671536,
+            "timestamp": 0.31843305528021815
+        },
+        {
+            "x": 4.708912918877337,
+            "y": 3.730071149255109,
+            "heading": -0.9027593729452231,
+            "angularVelocity": -1.4170594172577993,
+            "velocityX": 0.9254846413861598,
+            "velocityY": 2.840509215911215,
+            "timestamp": 0.4245774070402909
+        },
+        {
+            "x": 4.717930123488916,
+            "y": 4.059451821332611,
+            "heading": -1.0196863730682848,
+            "angularVelocity": -1.1015847587195386,
+            "velocityX": 0.08495227925044571,
+            "velocityY": 3.10313894819415,
+            "timestamp": 0.5307217588003637
+        },
+        {
+            "x": 4.639893034223715,
+            "y": 4.389369622514019,
+            "heading": -1.0106655427651627,
+            "angularVelocity": 0.08498643737080536,
+            "velocityX": -0.7351977563685651,
+            "velocityY": 3.1081993126412395,
+            "timestamp": 0.6368661105604364
+        },
+        {
+            "x": 4.471904603111427,
+            "y": 4.710090421849215,
+            "heading": -0.9107146957571205,
+            "angularVelocity": 0.9416501712118399,
+            "velocityX": -1.5826412647185075,
+            "velocityY": 3.021553139823678,
+            "timestamp": 0.7430104623205092
+        },
+        {
+            "x": 4.218561716713538,
+            "y": 4.992188206454831,
+            "heading": -0.8355436069817835,
+            "angularVelocity": 0.7081967860640641,
+            "velocityX": -2.3867768957743736,
+            "velocityY": 2.657680601255776,
+            "timestamp": 0.8491548140805819
+        },
+        {
+            "x": 3.8896224481808446,
+            "y": 5.222473200709689,
+            "heading": -0.83554222765338,
+            "angularVelocity": 0.000012994835623464205,
+            "velocityX": -3.098980426921093,
+            "velocityY": 2.169545439171289,
+            "timestamp": 0.9552991658406547
+        },
+        {
+            "x": 3.516178298161353,
+            "y": 5.370026719708121,
+            "heading": -0.8355421446101063,
+            "angularVelocity": 7.823616840140413e-7,
+            "velocityX": -3.518266811442025,
+            "velocityY": 1.390121250464274,
+            "timestamp": 1.0614435176007273
+        },
+        {
+            "x": 3.1189612059977283,
+            "y": 5.42877328608909,
+            "heading": -0.8355420737846555,
+            "angularVelocity": 6.672559542292647e-7,
+            "velocityX": -3.742234848835754,
+            "velocityY": 0.55345918465599,
+            "timestamp": 1.1675878693608
+        },
+        {
+            "x": 2.774416222135212,
+            "y": 5.484170620762628,
+            "heading": -0.7035735462776811,
+            "angularVelocity": 1.243292980914089,
+            "velocityX": -3.2460039384980304,
+            "velocityY": 0.5219056290320314,
+            "timestamp": 1.2737322211208726
+        },
+        {
+            "x": 2.5095166350816602,
+            "y": 5.523131311595933,
+            "heading": -0.3884677902217159,
+            "angularVelocity": 2.9686530732056857,
+            "velocityX": -2.4956541036901063,
+            "velocityY": 0.3670538298766209,
+            "timestamp": 1.3798765728809452
+        },
+        {
+            "x": 2.335800273937377,
+            "y": 5.547984865708467,
+            "heading": -0.13742924633332027,
+            "angularVelocity": 2.365067379711733,
+            "velocityX": -1.6366048523895966,
+            "velocityY": 0.23414862590816604,
+            "timestamp": 1.4860209246410179
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 1.2947391364164484,
+            "velocityX": -0.8083357476365635,
+            "velocityY": 0.11319617193282665,
+            "timestamp": 1.5921652764010905
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -1.986468774957569e-34,
+            "velocityY": 0,
+            "timestamp": 1.6983096281611632
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S3 To W3.traj b/src/main/deploy/choreo/S3 To W3.traj
new file mode 100644
index 00000000..26f24eb2
--- /dev/null
+++ b/src/main/deploy/choreo/S3 To W3.traj	
@@ -0,0 +1,121 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 0,
+            "velocityX": -3.809622996347125e-33,
+            "velocityY": 4.7029981856297106e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 4.1621681366579715,
+            "y": 2.9362991956112916,
+            "heading": -0.5618109581410129,
+            "angularVelocity": 0.03502487973446616,
+            "velocityX": -0.8430624486077395,
+            "velocityY": -0.5326069988815002,
+            "timestamp": 0.11960189130537771
+        },
+        {
+            "x": 3.949371568032143,
+            "y": 2.832059073063443,
+            "heading": -0.5332490097615643,
+            "angularVelocity": 0.2388085010004113,
+            "velocityX": -1.7792073879709687,
+            "velocityY": -0.8715591485229383,
+            "timestamp": 0.23920378261075542
+        },
+        {
+            "x": 3.6196334559490815,
+            "y": 2.7428786025637635,
+            "heading": -0.4400400521459035,
+            "angularVelocity": 0.7793267865446122,
+            "velocityX": -2.756964028613438,
+            "velocityY": -0.7456443165432319,
+            "timestamp": 0.35880567391613316
+        },
+        {
+            "x": 3.2124362068162218,
+            "y": 2.7443781288173086,
+            "heading": -0.34020495702705544,
+            "angularVelocity": 0.8347283979314292,
+            "velocityX": -3.4046054346513173,
+            "velocityY": 0.012537646664080927,
+            "timestamp": 0.47840756522151084
+        },
+        {
+            "x": 2.829910158877178,
+            "y": 2.8625567119312745,
+            "heading": -0.23363822858373923,
+            "angularVelocity": 0.891012067453128,
+            "velocityX": -3.1983277502054186,
+            "velocityY": 0.9880996180254248,
+            "timestamp": 0.5980094565268885
+        },
+        {
+            "x": 2.5228974587582895,
+            "y": 3.073051960500077,
+            "heading": -0.12255766699698727,
+            "angularVelocity": 0.9287525504349555,
+            "velocityX": -2.566955227614215,
+            "velocityY": 1.7599658857512914,
+            "timestamp": 0.7176113478322662
+        },
+        {
+            "x": 2.307806503132395,
+            "y": 3.359545759981809,
+            "heading": -0.008191563913849259,
+            "angularVelocity": 0.9562231987710198,
+            "velocityX": -1.7983909224036065,
+            "velocityY": 2.395395226236259,
+            "timestamp": 0.8372132391376439
+        },
+        {
+            "x": 2.197079056424326,
+            "y": 3.692587049160117,
+            "heading": -0.017161158826022753,
+            "angularVelocity": -0.07499542703130056,
+            "velocityX": -0.9258001315827844,
+            "velocityY": 2.7845821294578066,
+            "timestamp": 0.9568151304430216
+        },
+        {
+            "x": 2.2005818293568207,
+            "y": 4.024472502975606,
+            "heading": -0.15274865723996348,
+            "angularVelocity": -1.133656808718376,
+            "velocityX": 0.029286935969546842,
+            "velocityY": 2.774918107006309,
+            "timestamp": 1.0764170217483993
+        },
+        {
+            "x": 2.2590478504225837,
+            "y": 4.250579625353832,
+            "heading": -0.3027008584759083,
+            "angularVelocity": -1.2537611203242134,
+            "velocityX": 0.48883859968803833,
+            "velocityY": 1.8904978835235247,
+            "timestamp": 1.196018913053777
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.729914389908579,
+            "velocityX": 0.3424038627687911,
+            "velocityY": 0.914871608232253,
+            "timestamp": 1.3156208043591548
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 3.1982065543283105e-28,
+            "velocityX": 4.1170744495691044e-30,
+            "velocityY": 1.8090480101607624e-30,
+            "timestamp": 1.4352226956645326
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S4 To C1.traj b/src/main/deploy/choreo/S4 To C1.traj
new file mode 100644
index 00000000..f8bd91fc
--- /dev/null
+++ b/src/main/deploy/choreo/S4 To C1.traj	
@@ -0,0 +1,292 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 1.1411819845129358e-30,
+            "velocityX": 2.0257584812603577e-32,
+            "velocityY": 3.042151659469263e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 5.128157808100627,
+            "y": 1.6381505638691025,
+            "heading": -0.6351424860741187,
+            "angularVelocity": 0.06199634399134786,
+            "velocityX": -0.6106088487961494,
+            "velocityY": 0.23165524966799825,
+            "timestamp": 0.07835161903352221
+        },
+        {
+            "x": 5.034138141125347,
+            "y": 1.6783506940439978,
+            "heading": -0.625731099901857,
+            "angularVelocity": 0.12011731586854772,
+            "velocityX": -1.199970953185457,
+            "velocityY": 0.5130733821555872,
+            "timestamp": 0.15670323806704442
+        },
+        {
+            "x": 4.897649658060979,
+            "y": 1.7471051706265737,
+            "heading": -0.6123788413002932,
+            "angularVelocity": 0.1704145844880519,
+            "velocityX": -1.74199952404266,
+            "velocityY": 0.8775118808095047,
+            "timestamp": 0.23505485710056662
+        },
+        {
+            "x": 4.7291295198915435,
+            "y": 1.855780594555545,
+            "heading": -0.5966040827715752,
+            "angularVelocity": 0.20133289807283902,
+            "velocityX": -2.150818837544829,
+            "velocityY": 1.3870220586313022,
+            "timestamp": 0.31340647613408884
+        },
+        {
+            "x": 4.559409610033689,
+            "y": 2.0156280635815147,
+            "heading": -0.5822183456142529,
+            "angularVelocity": 0.18360484869071408,
+            "velocityX": -2.1661314973624393,
+            "velocityY": 2.0401297509574126,
+            "timestamp": 0.39175809516761106
+        },
+        {
+            "x": 4.423281260574217,
+            "y": 2.2140742145732073,
+            "heading": -0.5727758831618049,
+            "angularVelocity": 0.1205139417528553,
+            "velocityX": -1.7374031467203144,
+            "velocityY": 2.532763884646578,
+            "timestamp": 0.4701097142011333
+        },
+        {
+            "x": 4.3324586171803325,
+            "y": 2.4362881098121476,
+            "heading": -0.5691499320560302,
+            "angularVelocity": 0.04627793465535804,
+            "velocityX": -1.1591674111421555,
+            "velocityY": 2.8361110846205655,
+            "timestamp": 0.5484613332346555
+        },
+        {
+            "x": 4.290356765831772,
+            "y": 2.674111890908862,
+            "heading": -0.5714790836196856,
+            "angularVelocity": -0.02972691046318988,
+            "velocityX": -0.5373450079001806,
+            "velocityY": 3.0353397164002804,
+            "timestamp": 0.6268129522681777
+        },
+        {
+            "x": 4.298268630722052,
+            "y": 2.9211188280591673,
+            "heading": -0.5947192771376703,
+            "angularVelocity": -0.29661408155511804,
+            "velocityX": 0.1009789585444941,
+            "velocityY": 3.1525441362561364,
+            "timestamp": 0.7051645713016998
+        },
+        {
+            "x": 4.356924477851345,
+            "y": 3.1722484967201265,
+            "heading": -0.6368890523591307,
+            "angularVelocity": -0.5382119188043637,
+            "velocityX": 0.7486232939768359,
+            "velocityY": 3.2051624683532776,
+            "timestamp": 0.783516190335222
+        },
+        {
+            "x": 4.466386404708156,
+            "y": 3.418800428561669,
+            "heading": -0.694172117002664,
+            "angularVelocity": -0.7311025011368958,
+            "velocityX": 1.3970601782967371,
+            "velocityY": 3.1467369134523744,
+            "timestamp": 0.8618678093687442
+        },
+        {
+            "x": 4.622742851027307,
+            "y": 3.644917946871822,
+            "heading": -0.7581397869413499,
+            "angularVelocity": -0.8164179723116849,
+            "velocityX": 1.9955739045067467,
+            "velocityY": 2.8859329404975034,
+            "timestamp": 0.9402194284022664
+        },
+        {
+            "x": 4.8130531116785615,
+            "y": 3.833426042269654,
+            "heading": -0.8482959054101304,
+            "angularVelocity": -1.1506605681014435,
+            "velocityX": 2.4289256941816704,
+            "velocityY": 2.4059246984696085,
+            "timestamp": 1.0185710474357885
+        },
+        {
+            "x": 5.034329891204834,
+            "y": 3.981222629547119,
+            "heading": -0.9454274313666489,
+            "angularVelocity": -1.2396875412997066,
+            "velocityX": 2.824150697276595,
+            "velocityY": 1.8863246107809373,
+            "timestamp": 1.0969226664693108
+        },
+        {
+            "x": 5.3307679727813255,
+            "y": 4.095215339701575,
+            "heading": -1.0325645415374454,
+            "angularVelocity": -0.9491279076997933,
+            "velocityX": 3.2289073573561264,
+            "velocityY": 1.241648503948074,
+            "timestamp": 1.1887302189223545
+        },
+        {
+            "x": 5.6706736122505745,
+            "y": 4.158722341660029,
+            "heading": -1.0366531166779425,
+            "angularVelocity": -0.04453419170049298,
+            "velocityX": 3.7023712144281196,
+            "velocityY": 0.691740496958957,
+            "timestamp": 1.2805377713753983
+        },
+        {
+            "x": 5.985432145120126,
+            "y": 4.2874622186845945,
+            "heading": -1.0150469515971576,
+            "angularVelocity": 0.23534191363870294,
+            "velocityX": 3.4284601262030105,
+            "velocityY": 1.402279807975625,
+            "timestamp": 1.372345323828442
+        },
+        {
+            "x": 6.260473058973373,
+            "y": 4.473603429076733,
+            "heading": -0.9641067919300658,
+            "angularVelocity": 0.5548580515001295,
+            "velocityX": 2.9958419160985765,
+            "velocityY": 2.027515225257119,
+            "timestamp": 1.4641528762814857
+        },
+        {
+            "x": 6.487005993334624,
+            "y": 4.6803674717401,
+            "heading": -0.8309910438969466,
+            "angularVelocity": 1.4499433268434323,
+            "velocityX": 2.4674760224886074,
+            "velocityY": 2.2521463337029823,
+            "timestamp": 1.5559604287345294
+        },
+        {
+            "x": 6.71888754498383,
+            "y": 4.9198952355114685,
+            "heading": -0.7843851972685187,
+            "angularVelocity": 0.507647196588375,
+            "velocityX": 2.5257350343568326,
+            "velocityY": 2.609020253468568,
+            "timestamp": 1.6477679811875732
+        },
+        {
+            "x": 6.906293835583718,
+            "y": 5.2122954621107285,
+            "heading": -0.7843850302000089,
+            "angularVelocity": 0.0000018197686939661552,
+            "velocityX": 2.0412949217411986,
+            "velocityY": 3.1849256274293083,
+            "timestamp": 1.739575533640617
+        },
+        {
+            "x": 7.030998336898649,
+            "y": 5.536437116039629,
+            "heading": -0.7843849409482478,
+            "angularVelocity": 9.721614257680115e-7,
+            "velocityX": 1.3583250830993825,
+            "velocityY": 3.530664365490929,
+            "timestamp": 1.8313830860936606
+        },
+        {
+            "x": 7.139916048661206,
+            "y": 5.86621879705104,
+            "heading": -0.7843848550588985,
+            "angularVelocity": 9.355368597772783e-7,
+            "velocityX": 1.1863698448802928,
+            "velocityY": 3.592097514854049,
+            "timestamp": 1.9231906385467044
+        },
+        {
+            "x": 7.248833621341376,
+            "y": 6.196000523997438,
+            "heading": -0.7843847691695015,
+            "angularVelocity": 9.355373786921671e-7,
+            "velocityX": 1.1863683299462482,
+            "velocityY": 3.592098015194004,
+            "timestamp": 2.0149981909997483
+        },
+        {
+            "x": 7.357751176995467,
+            "y": 6.5257822565670125,
+            "heading": -0.7843846832799689,
+            "angularVelocity": 9.355388566444766e-7,
+            "velocityX": 1.1863681444922285,
+            "velocityY": 3.5920980764436137,
+            "timestamp": 2.1068057434527923
+        },
+        {
+            "x": 7.4669549324183055,
+            "y": 6.834400664448086,
+            "heading": -0.7318192896940902,
+            "angularVelocity": 0.5725606682823186,
+            "velocityX": 1.189485532562175,
+            "velocityY": 3.361579735380924,
+            "timestamp": 2.198613295905836
+        },
+        {
+            "x": 7.555413306559794,
+            "y": 7.089894078912264,
+            "heading": -0.5139853289463429,
+            "angularVelocity": 2.3727237566774426,
+            "velocityX": 0.9635195773978621,
+            "velocityY": 2.782923709842436,
+            "timestamp": 2.29042084835888
+        },
+        {
+            "x": 7.619158448834915,
+            "y": 7.279787689667393,
+            "heading": -0.2782484542424907,
+            "angularVelocity": 2.5677285626846773,
+            "velocityX": 0.6943344046528581,
+            "velocityY": 2.0683876835977384,
+            "timestamp": 2.382228400811924
+        },
+        {
+            "x": 7.660128944224933,
+            "y": 7.404974004983973,
+            "heading": -0.09793823848426357,
+            "angularVelocity": 1.964001990472943,
+            "velocityX": 0.44626497815605787,
+            "velocityY": 1.3635731698719276,
+            "timestamp": 2.474035953264968
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 2.533782828853193e-32,
+            "angularVelocity": 1.0667775783953661,
+            "velocityX": 0.21644249567844834,
+            "velocityY": 0.6756088508922024,
+            "timestamp": 2.565843505718012
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 2.4912680070874024e-31,
+            "velocityX": 0,
+            "velocityY": -3.645397861822831e-32,
+            "timestamp": 2.657651058171056
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S4 To C2.traj b/src/main/deploy/choreo/S4 To C2.traj
new file mode 100644
index 00000000..0e0e8d38
--- /dev/null
+++ b/src/main/deploy/choreo/S4 To C2.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -2.491480567253294e-33,
+            "velocityX": -2.0103104784295923e-34,
+            "velocityY": 1.605618083193578e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 5.240158813928264,
+            "y": 1.6204190327819517,
+            "heading": -0.5237338108165747,
+            "angularVelocity": 1.2526389382742513,
+            "velocityX": 0.6912398963489377,
+            "velocityY": 0.004514612397400031,
+            "timestamp": 0.0928170006782674
+        },
+        {
+            "x": 5.369626464101249,
+            "y": 1.6247166229091008,
+            "heading": -0.2974184918320366,
+            "angularVelocity": 2.4382959730515035,
+            "velocityX": 1.394870004707003,
+            "velocityY": 0.046301756097956205,
+            "timestamp": 0.1856340013565348
+        },
+        {
+            "x": 5.5674551093283755,
+            "y": 1.639197763374398,
+            "heading": 0.0009013327241277373,
+            "angularVelocity": 3.214064475001015,
+            "velocityX": 2.1313837312289596,
+            "velocityY": 0.15601819019657054,
+            "timestamp": 0.2784510020348022
+        },
+        {
+            "x": 5.831585704851482,
+            "y": 1.6774487923340957,
+            "heading": 0.2480008567342784,
+            "angularVelocity": 2.6622226769282764,
+            "velocityX": 2.8457135394696165,
+            "velocityY": 0.41211231434085943,
+            "timestamp": 0.3712680027130696
+        },
+        {
+            "x": 6.141513989428359,
+            "y": 1.75469627200855,
+            "heading": 0.3453177753240257,
+            "angularVelocity": 1.0484816130514494,
+            "velocityX": 3.339132726893265,
+            "velocityY": 0.8322557194259916,
+            "timestamp": 0.46408500339133696
+        },
+        {
+            "x": 6.452443961209774,
+            "y": 1.9008832260175597,
+            "heading": 0.36957097138497697,
+            "angularVelocity": 0.2613012258930921,
+            "velocityX": 3.3499247929718696,
+            "velocityY": 1.5750019171136467,
+            "timestamp": 0.5569020040696043
+        },
+        {
+            "x": 6.733109637262523,
+            "y": 2.111863899724239,
+            "heading": 0.36957109822659895,
+            "angularVelocity": 0.0000013665774702958542,
+            "velocityX": 3.023860650546378,
+            "velocityY": 2.273082217319261,
+            "timestamp": 0.6497190047478717
+        },
+        {
+            "x": 6.964965499466835,
+            "y": 2.3755472934997988,
+            "heading": 0.36957101579118784,
+            "angularVelocity": -8.881499138395027e-7,
+            "velocityX": 2.4979891669630336,
+            "velocityY": 2.840895437782658,
+            "timestamp": 0.7425360054261391
+        },
+        {
+            "x": 7.138307566970259,
+            "y": 2.680897277378083,
+            "heading": 0.36957096673523665,
+            "angularVelocity": -5.285233395413412e-7,
+            "velocityX": 1.867568077364202,
+            "velocityY": 3.289806626446832,
+            "timestamp": 0.8353530061044064
+        },
+        {
+            "x": 7.245881008413397,
+            "y": 3.0151337865477195,
+            "heading": 0.3695709277774119,
+            "angularVelocity": -4.197272531605919e-7,
+            "velocityX": 1.1589842448800958,
+            "velocityY": 3.6010268240427665,
+            "timestamp": 0.9281700067826738
+        },
+        {
+            "x": 7.300266060372569,
+            "y": 3.36201756600148,
+            "heading": 0.3695708905369613,
+            "angularVelocity": -4.012244559973519e-7,
+            "velocityX": 0.5859384763755527,
+            "velocityY": 3.7372871017042186,
+            "timestamp": 1.0209870074609413
+        },
+        {
+            "x": 7.354650396010165,
+            "y": 3.708901457760651,
+            "heading": 0.3695708532965109,
+            "angularVelocity": -4.0122445392101915e-7,
+            "velocityX": 0.5859307588068667,
+            "velocityY": 3.7372883116701696,
+            "timestamp": 1.1138040081392087
+        },
+        {
+            "x": 7.409034640641628,
+            "y": 4.055785363787717,
+            "heading": 0.36957081605605097,
+            "angularVelocity": -4.012245569815441e-7,
+            "velocityX": 0.5859297783169686,
+            "velocityY": 3.737288465390881,
+            "timestamp": 1.206621008817476
+        },
+        {
+            "x": 7.463418795891099,
+            "y": 4.402669283828022,
+            "heading": 0.36957077881557854,
+            "angularVelocity": -4.0122469066112833e-7,
+            "velocityX": 0.5859288153253637,
+            "velocityY": 3.7372886163679513,
+            "timestamp": 1.2994380094957434
+        },
+        {
+            "x": 7.517802854177964,
+            "y": 4.7495532190700205,
+            "heading": 0.3695707415750949,
+            "angularVelocity": -4.0122481249990114e-7,
+            "velocityX": 0.5859277706610763,
+            "velocityY": 3.737288780149305,
+            "timestamp": 1.3922550101740108
+        },
+        {
+            "x": 7.56989445358868,
+            "y": 5.095450165697103,
+            "heading": 0.36620989603022114,
+            "angularVelocity": -0.036209374579162214,
+            "velocityX": 0.5612290747390308,
+            "velocityY": 3.7266550750338174,
+            "timestamp": 1.4850720108522781
+        },
+        {
+            "x": 7.614129631330889,
+            "y": 5.376850109081837,
+            "heading": 0.24917441522367106,
+            "angularVelocity": -1.2609272003114111,
+            "velocityX": 0.4765848650458117,
+            "velocityY": 3.0317715647821197,
+            "timestamp": 1.5778890115305455
+        },
+        {
+            "x": 7.6471559939174325,
+            "y": 5.587299345919279,
+            "heading": 0.1342077337923999,
+            "angularVelocity": -1.2386381868746383,
+            "velocityX": 0.35582234229936965,
+            "velocityY": 2.267356575838128,
+            "timestamp": 1.6707060122088129
+        },
+        {
+            "x": 7.669079055841979,
+            "y": 5.72720580380319,
+            "heading": 0.04709984861251457,
+            "angularVelocity": -0.9384906271840042,
+            "velocityX": 0.23619662092442378,
+            "velocityY": 1.5073365532341503,
+            "timestamp": 1.7635230128870802
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -9.59663427624948e-34,
+            "angularVelocity": -0.5074485090913172,
+            "velocityX": 0.11766103276571077,
+            "velocityY": 0.751954875580805,
+            "timestamp": 1.8563400135653476
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 1.9250732473517817e-32,
+            "velocityX": 0,
+            "velocityY": 2.1529996497623123e-34,
+            "timestamp": 1.949157014243615
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S4 To C3.traj b/src/main/deploy/choreo/S4 To C3.traj
new file mode 100644
index 00000000..4d225c1e
--- /dev/null
+++ b/src/main/deploy/choreo/S4 To C3.traj	
@@ -0,0 +1,238 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 0,
+            "velocityX": 2.653642496841617e-31,
+            "velocityY": -1.2761459725684476e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 5.194122324075795,
+            "y": 1.6116215778070426,
+            "heading": -0.6335243646936302,
+            "angularVelocity": 0.13206571622689564,
+            "velocityX": 0.3695911822570569,
+            "velocityY": -0.1708716250073132,
+            "timestamp": 0.049033431926404414
+        },
+        {
+            "x": 5.2306889078335095,
+            "y": 1.5955898422898989,
+            "heading": -0.6207226842720189,
+            "angularVelocity": 0.2610806529090014,
+            "velocityX": 0.745747999295838,
+            "velocityY": -0.3269551994890449,
+            "timestamp": 0.09806686385280883
+        },
+        {
+            "x": 5.286073830286703,
+            "y": 1.5728623577205654,
+            "heading": -0.6018022859553105,
+            "angularVelocity": 0.38586730672362957,
+            "velocityX": 1.1295338767299938,
+            "velocityY": -0.4635099701654054,
+            "timestamp": 0.14710029577921324
+        },
+        {
+            "x": 5.360700050095615,
+            "y": 1.5447484727758143,
+            "heading": -0.577064216904969,
+            "angularVelocity": 0.5045143298894789,
+            "velocityX": 1.5219456782249914,
+            "velocityY": -0.5733615584348081,
+            "timestamp": 0.19613372770561766
+        },
+        {
+            "x": 5.45500431506412,
+            "y": 1.513111103841015,
+            "heading": -0.5469692052395991,
+            "angularVelocity": 0.6137651492712723,
+            "velocityX": 1.9232646230039372,
+            "velocityY": -0.6452203668359155,
+            "timestamp": 0.24516715963202207
+        },
+        {
+            "x": 5.5692993756365,
+            "y": 1.480706991826158,
+            "heading": -0.5122625268421303,
+            "angularVelocity": 0.7078166270271339,
+            "velocityX": 2.330961878090668,
+            "velocityY": -0.6608575158164777,
+            "timestamp": 0.2942005915584265
+        },
+        {
+            "x": 5.703331364670941,
+            "y": 1.4516882815399292,
+            "heading": -0.47420565589215874,
+            "angularVelocity": 0.7761412867722233,
+            "velocityX": 2.733481703578104,
+            "velocityY": -0.5918147913809916,
+            "timestamp": 0.3432340234848309
+        },
+        {
+            "x": 5.855159622936256,
+            "y": 1.4318923943368613,
+            "heading": -0.4349045247507558,
+            "angularVelocity": 0.8015170384282412,
+            "velocityX": 3.096423242273411,
+            "velocityY": -0.40372224470801127,
+            "timestamp": 0.3922674554112353
+        },
+        {
+            "x": 6.019815732483991,
+            "y": 1.4274954608427641,
+            "heading": -0.3972296573283927,
+            "angularVelocity": 0.768350611865634,
+            "velocityX": 3.358037630221609,
+            "velocityY": -0.08967215471871418,
+            "timestamp": 0.4413008873376397
+        },
+        {
+            "x": 6.190507691154086,
+            "y": 1.4421792499476667,
+            "heading": -0.36375877276285506,
+            "angularVelocity": 0.6826135404074718,
+            "velocityX": 3.4811342376834093,
+            "velocityY": 0.2994648452702519,
+            "timestamp": 0.49033431926404414
+        },
+        {
+            "x": 6.361514712702491,
+            "y": 1.4768435219754552,
+            "heading": -0.33601787986089265,
+            "angularVelocity": 0.5657546659914753,
+            "velocityX": 3.487559708345244,
+            "velocityY": 0.7069517809772359,
+            "timestamp": 0.5393677511904486
+        },
+        {
+            "x": 6.529238224029541,
+            "y": 1.531233787536621,
+            "heading": -0.3133159497925571,
+            "angularVelocity": 0.4629888052692109,
+            "velocityX": 3.4205949846286416,
+            "velocityY": 1.1092485968081267,
+            "timestamp": 0.588401183116853
+        },
+        {
+            "x": 6.794330666624461,
+            "y": 1.6764087741891647,
+            "heading": -0.29089151826229404,
+            "angularVelocity": 0.27241251742532613,
+            "velocityX": 3.2203491776466797,
+            "velocityY": 1.7635891250039288,
+            "timestamp": 0.6707191020664587
+        },
+        {
+            "x": 7.034738463760469,
+            "y": 1.8721289187979775,
+            "heading": -0.28740556521383775,
+            "angularVelocity": 0.042347438965477144,
+            "velocityX": 2.9204795286821774,
+            "velocityY": 2.377612883150898,
+            "timestamp": 0.7530370210160644
+        },
+        {
+            "x": 7.236654527584481,
+            "y": 2.109198968129657,
+            "heading": -0.2874055261243408,
+            "angularVelocity": 4.748601214403234e-7,
+            "velocityX": 2.452881054338489,
+            "velocityY": 2.879932490479633,
+            "timestamp": 0.8353549399656701
+        },
+        {
+            "x": 7.392410351066591,
+            "y": 2.3788514084218195,
+            "heading": -0.2874055078665042,
+            "angularVelocity": 2.2179662469221753e-7,
+            "velocityX": 1.8921253776776576,
+            "velocityY": 3.2757441360636306,
+            "timestamp": 0.9176728589152758
+        },
+        {
+            "x": 7.49687844031107,
+            "y": 2.6722091028580075,
+            "heading": -0.28740549372573876,
+            "angularVelocity": 1.7178234776742224e-7,
+            "velocityX": 1.2690807855393922,
+            "velocityY": 3.5637161164844247,
+            "timestamp": 0.9999907778648816
+        },
+        {
+            "x": 7.546620338444338,
+            "y": 2.979614475892834,
+            "heading": -0.28740548096651763,
+            "angularVelocity": 1.5499931641741705e-7,
+            "velocityX": 0.6042657390764874,
+            "velocityY": 3.7343676438541644,
+            "timestamp": 1.0823086968144873
+        },
+        {
+            "x": 7.582493232504898,
+            "y": 3.288945132948743,
+            "heading": -0.2874054683444637,
+            "angularVelocity": 1.5333300623146833e-7,
+            "velocityX": 0.43578475401492967,
+            "velocityY": 3.7577560390622633,
+            "timestamp": 1.164626615764093
+        },
+        {
+            "x": 7.6150468277369106,
+            "y": 3.5683420563080905,
+            "heading": -0.1936159509616472,
+            "angularVelocity": 1.1393572454157397,
+            "velocityX": 0.39546183440153926,
+            "velocityY": 3.3941203437178378,
+            "timestamp": 1.2469445347136987
+        },
+        {
+            "x": 7.641044736890772,
+            "y": 3.791820131624945,
+            "heading": -0.11680359430817472,
+            "angularVelocity": 0.9331183007718732,
+            "velocityX": 0.31582320697043825,
+            "velocityY": 2.714816873027222,
+            "timestamp": 1.3292624536633044
+        },
+        {
+            "x": 7.660527962829294,
+            "y": 3.959415202649684,
+            "heading": -0.05861701943494889,
+            "angularVelocity": 0.7068518691380767,
+            "velocityX": 0.23668268327338735,
+            "velocityY": 2.035948833052459,
+            "timestamp": 1.4115803726129101
+        },
+        {
+            "x": 7.673510674167818,
+            "y": 4.0711397209924485,
+            "heading": -0.019593791766857216,
+            "angularVelocity": 0.4740550801809447,
+            "velocityX": 0.1577142802460941,
+            "velocityY": 1.3572320555281903,
+            "timestamp": 1.4938982915625159
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -2.071552209824535e-33,
+            "angularVelocity": 0.23802583953553896,
+            "velocityX": 0.07883248161483059,
+            "velocityY": 0.6785919726876813,
+            "timestamp": 1.5762162105121216
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": -4.785738309812717e-31,
+            "velocityX": -1.2484093100405624e-30,
+            "velocityY": 4.284452606087597e-31,
+            "timestamp": 1.6585341294617273
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S4 To C4.traj b/src/main/deploy/choreo/S4 To C4.traj
new file mode 100644
index 00000000..751754fd
--- /dev/null
+++ b/src/main/deploy/choreo/S4 To C4.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.222401190948388,
+            "y": 1.635609178560311,
+            "heading": -0.6120137040899031,
+            "angularVelocity": 0.36292240482976423,
+            "velocityX": 0.6017242103081577,
+            "velocityY": 0.20241766322785323,
+            "timestamp": 0.07711371780208913
+        },
+        {
+            "x": 5.315327100770923,
+            "y": 1.6668589219810437,
+            "heading": -0.5590312951883875,
+            "angularVelocity": 0.6870685321838068,
+            "velocityX": 1.205050313629373,
+            "velocityY": 0.4052423396435706,
+            "timestamp": 0.15422743560417826
+        },
+        {
+            "x": 5.454938302762681,
+            "y": 1.713800132384211,
+            "heading": -0.4856728473163301,
+            "angularVelocity": 0.9513021802467544,
+            "velocityX": 1.8104587091763231,
+            "velocityY": 0.6087271077195515,
+            "timestamp": 0.23134115340626737
+        },
+        {
+            "x": 5.641435450684747,
+            "y": 1.7765099385015581,
+            "heading": -0.4002091399727602,
+            "angularVelocity": 1.1082815065785667,
+            "velocityX": 2.418469154874715,
+            "velocityY": 0.8132120704942623,
+            "timestamp": 0.3084548712083565
+        },
+        {
+            "x": 5.8748863814192855,
+            "y": 1.8550365409977465,
+            "heading": -0.32154781798640775,
+            "angularVelocity": 1.0200691164734093,
+            "velocityX": 3.0273592998548686,
+            "velocityY": 1.0183220928048784,
+            "timestamp": 0.38556858901044566
+        },
+        {
+            "x": 6.151206632813102,
+            "y": 1.945953456728306,
+            "heading": -0.3195100296755088,
+            "angularVelocity": 0.026425756259478697,
+            "velocityX": 3.583282705976986,
+            "velocityY": 1.178997956808355,
+            "timestamp": 0.4626823068125348
+        },
+        {
+            "x": 6.4277763897122595,
+            "y": 2.0387313847088295,
+            "heading": -0.31950999788997536,
+            "angularVelocity": 4.121903903341971e-7,
+            "velocityX": 3.5865182587742526,
+            "velocityY": 1.2031313056210848,
+            "timestamp": 0.539796024614624
+        },
+        {
+            "x": 6.7043461469693755,
+            "y": 2.1315093116222874,
+            "heading": -0.31950996610444216,
+            "angularVelocity": 4.121903870025697e-7,
+            "velocityX": 3.5865182634161936,
+            "velocityY": 1.2031312917835268,
+            "timestamp": 0.616909742416713
+        },
+        {
+            "x": 6.980831721332989,
+            "y": 2.2223162260189957,
+            "heading": -0.31777519026878787,
+            "angularVelocity": 0.022496332495664188,
+            "velocityX": 3.5854265913259273,
+            "velocityY": 1.1775714747635835,
+            "timestamp": 0.6940234602188021
+        },
+        {
+            "x": 7.214355031558839,
+            "y": 2.3007720927806434,
+            "heading": -0.23870969056825803,
+            "angularVelocity": 1.0253104370281303,
+            "velocityX": 3.028297907062184,
+            "velocityY": 1.0174048016074528,
+            "timestamp": 0.7711371780208912
+        },
+        {
+            "x": 7.400917361819277,
+            "y": 2.3634042679352882,
+            "heading": -0.15373574527352316,
+            "angularVelocity": 1.1019303402387481,
+            "velocityX": 2.419314430400663,
+            "velocityY": 0.8122053629341165,
+            "timestamp": 0.8482508958229803
+        },
+        {
+            "x": 7.540592119323217,
+            "y": 2.4102654434053137,
+            "heading": -0.08076255152981597,
+            "angularVelocity": 0.9463062581289249,
+            "velocityX": 1.8112828882458099,
+            "velocityY": 0.6076892258040765,
+            "timestamp": 0.9253646136250694
+        },
+        {
+            "x": 7.633568850123268,
+            "y": 2.4414404537843537,
+            "heading": -0.02793953661151031,
+            "angularVelocity": 0.6850015331107969,
+            "velocityX": 1.2057093530190521,
+            "velocityY": 0.4042732119212608,
+            "timestamp": 1.0024783314271586
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -3.3818227179407965e-32,
+            "angularVelocity": 0.3623160367292381,
+            "velocityX": 0.6021127135368662,
+            "velocityY": 0.20177403786417583,
+            "timestamp": 1.0795920492292477
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -1.7311819232040874e-32,
+            "angularVelocity": 2.140534191184349e-31,
+            "velocityX": 0,
+            "velocityY": -2.0522160776390996e-31,
+            "timestamp": 1.1567057670313368
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S4 To C5.traj b/src/main/deploy/choreo/S4 To C5.traj
new file mode 100644
index 00000000..0bd0e944
--- /dev/null
+++ b/src/main/deploy/choreo/S4 To C5.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.22245562506892,
+            "y": 1.6045082039129626,
+            "heading": -0.6133597798196372,
+            "angularVelocity": 0.3456549186542876,
+            "velocityX": 0.6027583554308628,
+            "velocityY": -0.20100492713723658,
+            "timestamp": 0.07707172310487943
+        },
+        {
+            "x": 5.31546298986369,
+            "y": 1.573504853521539,
+            "heading": -0.5625082909022232,
+            "angularVelocity": 0.6597943690478437,
+            "velocityX": 1.2067637915426601,
+            "velocityY": -0.4022662157070755,
+            "timestamp": 0.15414344620975887
+        },
+        {
+            "x": 5.455153171011431,
+            "y": 1.526958597742617,
+            "heading": -0.4912836919139272,
+            "angularVelocity": 0.924133989989731,
+            "velocityX": 1.8124699373549893,
+            "velocityY": -0.6039342822993798,
+            "timestamp": 0.2312151693146383
+        },
+        {
+            "x": 5.641701159953251,
+            "y": 1.4648201004985182,
+            "heading": -0.406509898431932,
+            "angularVelocity": 1.0999338027856826,
+            "velocityX": 2.420446584384324,
+            "velocityY": -0.8062424809101578,
+            "timestamp": 0.30828689241951773
+        },
+        {
+            "x": 5.875244890448534,
+            "y": 1.3870507382512116,
+            "heading": -0.323292875073049,
+            "angularVelocity": 1.0797348236997513,
+            "velocityX": 3.03021291190643,
+            "velocityY": -1.0090518171168696,
+            "timestamp": 0.38535861552439715
+        },
+        {
+            "x": 6.151315012657669,
+            "y": 1.295160148354896,
+            "heading": -0.32123146568205607,
+            "angularVelocity": 0.026746636872095383,
+            "velocityX": 3.5819897504232276,
+            "velocityY": -1.192273718485189,
+            "timestamp": 0.46243033862927657
+        },
+        {
+            "x": 6.427941847872596,
+            "y": 1.2030541885235448,
+            "heading": -0.3212314367193528,
+            "angularVelocity": 3.757889680371681e-7,
+            "velocityX": 3.5892130611702004,
+            "velocityY": -1.1950681277232245,
+            "timestamp": 0.539502061734156
+        },
+        {
+            "x": 6.704568684105482,
+            "y": 1.1109482317494896,
+            "heading": -0.3212314077566493,
+            "angularVelocity": 3.7578897090221527e-7,
+            "velocityX": 3.589213074378161,
+            "velocityY": -1.1950680880550335,
+            "timestamp": 0.6165737848390355
+        },
+        {
+            "x": 6.9807077062893965,
+            "y": 1.0195126541212949,
+            "heading": -0.3189025007582472,
+            "angularVelocity": 0.030217398866675224,
+            "velocityX": 3.582883722583232,
+            "velocityY": -1.1863699674103443,
+            "timestamp": 0.6936455079439149
+        },
+        {
+            "x": 7.214256981896637,
+            "y": 0.9418352430707998,
+            "heading": -0.2350676331360113,
+            "angularVelocity": 1.0877513080660361,
+            "velocityX": 3.0302848593306506,
+            "velocityY": -1.0078587570280646,
+            "timestamp": 0.7707172310487943
+        },
+        {
+            "x": 7.4008131962231865,
+            "y": 0.8797969152412516,
+            "heading": -0.14969687190170503,
+            "angularVelocity": 1.1076794159400525,
+            "velocityX": 2.4205533081527695,
+            "velocityY": -0.8049427900441033,
+            "timestamp": 0.8477889541536737
+        },
+        {
+            "x": 7.54051565231881,
+            "y": 0.8333524229780926,
+            "heading": -0.0780215306756512,
+            "angularVelocity": 0.9299823377312819,
+            "velocityX": 1.812629203910703,
+            "velocityY": -0.6026139080860716,
+            "timestamp": 0.9248606772585531
+        },
+        {
+            "x": 7.633536087871107,
+            "y": 0.802437413208921,
+            "heading": -0.026843150662094223,
+            "angularVelocity": 0.6640357572376224,
+            "velocityX": 1.2069333836706095,
+            "velocityY": -0.401120002560503,
+            "timestamp": 1.0019324003634327
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.8658238709310755e-33,
+            "angularVelocity": 0.3482879268907221,
+            "velocityX": 0.6028658794310917,
+            "velocityY": -0.20029931325025216,
+            "timestamp": 1.079004123468312
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 4.381697144346615e-34,
+            "angularVelocity": -4.4473487408728983e-32,
+            "velocityX": 0,
+            "velocityY": -1.5918634852021244e-32,
+            "timestamp": 1.1560758465731915
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S4 To W1.traj b/src/main/deploy/choreo/S4 To W1.traj
new file mode 100644
index 00000000..90e7d73f
--- /dev/null
+++ b/src/main/deploy/choreo/S4 To W1.traj	
@@ -0,0 +1,247 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 0,
+            "velocityX": -4.2485019622555225e-35,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.123544064624381,
+            "y": 1.640479835564366,
+            "heading": -0.5261105113915723,
+            "angularVelocity": 1.3044458865146935,
+            "velocityX": -0.6008098724480858,
+            "velocityY": 0.23456806756139406,
+            "timestamp": 0.08730871076049408
+        },
+        {
+            "x": 5.016418569113309,
+            "y": 1.6815339921073649,
+            "heading": -0.31680443246750034,
+            "angularVelocity": 2.397310384049101,
+            "velocityX": -1.2269737415426907,
+            "velocityY": 0.4702183342922002,
+            "timestamp": 0.17461742152098816
+        },
+        {
+            "x": 4.8522086939147036,
+            "y": 1.7459427773647251,
+            "heading": -0.051891218250848345,
+            "angularVelocity": 3.0342128741697256,
+            "velocityX": -1.8807960141464901,
+            "velocityY": 0.7377131639710823,
+            "timestamp": 0.26192613228148226
+        },
+        {
+            "x": 4.632595451769563,
+            "y": 1.839668047804236,
+            "heading": 0.17554509517769362,
+            "angularVelocity": 2.6049670353333583,
+            "velocityX": -2.515364620920639,
+            "velocityY": 1.073492777789593,
+            "timestamp": 0.34923484304197633
+        },
+        {
+            "x": 4.3704901470738005,
+            "y": 1.9627849470607086,
+            "heading": 0.27538033292006053,
+            "angularVelocity": 1.1434739658020574,
+            "velocityX": -3.002052171114658,
+            "velocityY": 1.4101330575617794,
+            "timestamp": 0.4365435538024704
+        },
+        {
+            "x": 4.075718453010027,
+            "y": 2.1117712847634245,
+            "heading": 0.2753804447713452,
+            "angularVelocity": 0.0000012811010915301473,
+            "velocityX": -3.3762002839830365,
+            "velocityY": 1.7064315393617047,
+            "timestamp": 0.5238522645629645
+        },
+        {
+            "x": 3.780946849777969,
+            "y": 2.260757802178556,
+            "heading": 0.2753805566219007,
+            "angularVelocity": 0.0000012810927397041138,
+            "velocityX": -3.3761992436319233,
+            "velocityY": 1.7064335977177825,
+            "timestamp": 0.6111609753234586
+        },
+        {
+            "x": 3.4861752863833098,
+            "y": 2.409744398412427,
+            "heading": 0.27538066847245973,
+            "angularVelocity": 0.0000012810927810381003,
+            "velocityX": -3.3761987873498507,
+            "velocityY": 1.7064345004769594,
+            "timestamp": 0.6984696860839527
+        },
+        {
+            "x": 3.191403823869153,
+            "y": 2.5587311942392557,
+            "heading": 0.2753807803230405,
+            "angularVelocity": 0.000001281093028963251,
+            "velocityX": -3.3761976319038296,
+            "velocityY": 1.7064367865370367,
+            "timestamp": 0.7857783968444467
+        },
+        {
+            "x": 2.907963654667952,
+            "y": 2.72828634801538,
+            "heading": 0.27538089480545036,
+            "angularVelocity": 0.0000013112369766496227,
+            "velocityX": -3.246413407463273,
+            "velocityY": 1.9420187550501031,
+            "timestamp": 0.8730871076049408
+        },
+        {
+            "x": 2.662249448997808,
+            "y": 2.9489938499602837,
+            "heading": 0.27538103946019826,
+            "angularVelocity": 0.000001656819194794751,
+            "velocityX": -2.81431490088302,
+            "velocityY": 2.527897846875211,
+            "timestamp": 0.9603958183654349
+        },
+        {
+            "x": 2.463361015566044,
+            "y": 3.212680537293485,
+            "heading": 0.27538116270778873,
+            "angularVelocity": 0.0000014116299433562534,
+            "velocityX": -2.277990726232991,
+            "velocityY": 3.0201647125055904,
+            "timestamp": 1.047704529125929
+        },
+        {
+            "x": 2.3186638703555946,
+            "y": 3.509581240155329,
+            "heading": 0.2753811333053821,
+            "angularVelocity": -3.3676372418890626e-7,
+            "velocityX": -1.657304797540575,
+            "velocityY": 3.4005851223287915,
+            "timestamp": 1.1350132398864232
+        },
+        {
+            "x": 2.233516432992889,
+            "y": 3.8287006658490608,
+            "heading": 0.2753811088610493,
+            "angularVelocity": -2.799758765995743e-7,
+            "velocityX": -0.9752456154836949,
+            "velocityY": 3.655069728026828,
+            "timestamp": 1.2223219506469174
+        },
+        {
+            "x": 2.1898375161467403,
+            "y": 4.15608338798651,
+            "heading": 0.27538108549661844,
+            "angularVelocity": -2.6760709935894875e-7,
+            "velocityX": -0.5002813174732252,
+            "velocityY": 3.749714310127982,
+            "timestamp": 1.3096306614074116
+        },
+        {
+            "x": 2.1796859113101297,
+            "y": 4.486211000706444,
+            "heading": 0.2753810837562719,
+            "angularVelocity": -1.9933252293602923e-8,
+            "velocityX": -0.11627253166592438,
+            "velocityY": 3.78115321878412,
+            "timestamp": 1.3969393721679058
+        },
+        {
+            "x": 2.197179274626327,
+            "y": 4.816031069152981,
+            "heading": 0.2753810893961792,
+            "angularVelocity": 6.459730400705482e-8,
+            "velocityX": 0.20036217650934227,
+            "velocityY": 3.7776307263464504,
+            "timestamp": 1.4842480829284
+        },
+        {
+            "x": 2.214672942788018,
+            "y": 5.1458511214306375,
+            "heading": 0.27538109503608554,
+            "angularVelocity": 6.459729260879602e-8,
+            "velocityX": 0.20036566809100828,
+            "velocityY": 3.777630541154376,
+            "timestamp": 1.5715567936888941
+        },
+        {
+            "x": 2.2321666974024836,
+            "y": 5.475671169122818,
+            "heading": 0.2753811006759881,
+            "angularVelocity": 6.459724966098765e-8,
+            "velocityX": 0.20036665828744704,
+            "velocityY": 3.7776304886341237,
+            "timestamp": 1.6588655044493883
+        },
+        {
+            "x": 2.2496605217353887,
+            "y": 5.805491213117091,
+            "heading": 0.2753811063158993,
+            "angularVelocity": 6.459734855412882e-8,
+            "velocityX": 0.20036745681532722,
+            "velocityY": 3.7776304462797263,
+            "timestamp": 1.7461742152098825
+        },
+        {
+            "x": 2.2664262161021513,
+            "y": 6.122693971906954,
+            "heading": 0.30917552472065174,
+            "angularVelocity": 0.3870681185232194,
+            "velocityX": 0.19202773950877436,
+            "velocityY": 3.633116971112041,
+            "timestamp": 1.8334829259703767
+        },
+        {
+            "x": 2.279851444819284,
+            "y": 6.376429629998848,
+            "heading": 0.34011357351362637,
+            "angularVelocity": 0.35435237244361656,
+            "velocityX": 0.15376734578020115,
+            "velocityY": 2.9061894956614607,
+            "timestamp": 1.9207916367308708
+        },
+        {
+            "x": 2.289924296428913,
+            "y": 6.566719611746641,
+            "heading": 0.3645867540299274,
+            "angularVelocity": 0.2803062867740199,
+            "velocityX": 0.11537052284807026,
+            "velocityY": 2.179507406423607,
+            "timestamp": 2.008100347491365
+        },
+        {
+            "x": 2.296641079194263,
+            "y": 6.6935744437684805,
+            "heading": 0.3814115840440414,
+            "angularVelocity": 0.192705056203017,
+            "velocityX": 0.07693141619941274,
+            "velocityY": 1.4529458849739352,
+            "timestamp": 2.095409058251859
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 0.09836837448577633,
+            "velocityX": 0.03847177190545204,
+            "velocityY": 0.7264516412973858,
+            "timestamp": 2.182717769012353
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 2.6936232860428826e-33,
+            "velocityX": 0,
+            "velocityY": 5.62663175256514e-35,
+            "timestamp": 2.2700264797728473
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S4 To W2.traj b/src/main/deploy/choreo/S4 To W2.traj
new file mode 100644
index 00000000..97118e90
--- /dev/null
+++ b/src/main/deploy/choreo/S4 To W2.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -2.256840111784074e-31,
+            "velocityX": -2.1308644711626517e-33,
+            "velocityY": -1.0217798620245829e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 5.1132615869041915,
+            "y": 1.6556053336408048,
+            "heading": -0.6461329738139044,
+            "angularVelocity": -0.06592692141286781,
+            "velocityX": -0.6744118848768044,
+            "velocityY": 0.3827425493802767,
+            "timestamp": 0.09302684976743651
+        },
+        {
+            "x": 4.987784771636742,
+            "y": 1.7268160147391884,
+            "heading": -0.6583988506620269,
+            "angularVelocity": -0.131853081973501,
+            "velocityX": -1.348823652323362,
+            "velocityY": 0.7654852472851404,
+            "timestamp": 0.18605369953487302
+        },
+        {
+            "x": 4.799569566809376,
+            "y": 1.8336320552005005,
+            "heading": -0.6767974581881981,
+            "angularVelocity": -0.19777738977689827,
+            "velocityX": -2.02323528419909,
+            "velocityY": 1.148228073167565,
+            "timestamp": 0.2790805493023095
+        },
+        {
+            "x": 4.54861599428641,
+            "y": 1.9760534609244509,
+            "heading": -0.7013284716868837,
+            "angularVelocity": -0.26369820713065356,
+            "velocityX": -2.697646681042515,
+            "velocityY": 1.5309709624694186,
+            "timestamp": 0.37210739906974605
+        },
+        {
+            "x": 4.242554383646119,
+            "y": 2.1497500028424636,
+            "heading": -0.7013284745367188,
+            "angularVelocity": -3.063454187141868e-8,
+            "velocityX": -3.29003520387323,
+            "velocityY": 1.8671656876724017,
+            "timestamp": 0.4651342488371826
+        },
+        {
+            "x": 3.9364928269084327,
+            "y": 2.3234466397396827,
+            "heading": -0.7013284773864732,
+            "angularVelocity": -3.0633677229161635e-8,
+            "velocityX": -3.290034624442601,
+            "velocityY": 1.8671667086594281,
+            "timestamp": 0.558161098604619
+        },
+        {
+            "x": 3.630431318940711,
+            "y": 2.497143362571822,
+            "heading": -0.7013284802362207,
+            "angularVelocity": -3.063360291364155e-8,
+            "velocityX": -3.2900341001857365,
+            "velocityY": 1.867167632424124,
+            "timestamp": 0.6511879483720555
+        },
+        {
+            "x": 3.3243698648507793,
+            "y": 2.6708401803390394,
+            "heading": -0.7013284830859674,
+            "angularVelocity": -3.063359264364299e-8,
+            "velocityX": -3.2900335210218676,
+            "velocityY": 1.8671686529367946,
+            "timestamp": 0.744214798139492
+        },
+        {
+            "x": 3.0183085209148324,
+            "y": 2.844537192202297,
+            "heading": -0.7013284859357423,
+            "angularVelocity": -3.063389607722874e-8,
+            "velocityX": -3.290032336912287,
+            "velocityY": 1.8671707393886097,
+            "timestamp": 0.8372416479069285
+        },
+        {
+            "x": 2.714360997870643,
+            "y": 3.021907148464231,
+            "heading": -0.7013284887932081,
+            "angularVelocity": -3.071657002046057e-8,
+            "velocityX": -3.2673096402172703,
+            "velocityY": 1.9066533662630898,
+            "timestamp": 0.9302684976743649
+        },
+        {
+            "x": 2.4529780195268613,
+            "y": 3.2575406255854364,
+            "heading": -0.7013284919399162,
+            "angularVelocity": -3.382580562568125e-8,
+            "velocityX": -2.8097584621776335,
+            "velocityY": 2.532962018065539,
+            "timestamp": 1.0232953474418014
+        },
+        {
+            "x": 2.2451503238556145,
+            "y": 3.5415333045009083,
+            "heading": -0.7013284961513817,
+            "angularVelocity": -4.527150531202637e-8,
+            "velocityX": -2.2340614155032408,
+            "velocityY": 3.0528033532839474,
+            "timestamp": 1.1163221972092379
+        },
+        {
+            "x": 2.099615704405981,
+            "y": 3.861945316168419,
+            "heading": -0.7013284332934493,
+            "angularVelocity": 6.756966674200994e-7,
+            "velocityX": -1.564436717071087,
+            "velocityY": 3.444296055047848,
+            "timestamp": 1.2093490469766743
+        },
+        {
+            "x": 2.0224929199626733,
+            "y": 4.205305507968674,
+            "heading": -0.7013282219960775,
+            "angularVelocity": 0.0000022713589924444385,
+            "velocityX": -0.8290379028862299,
+            "velocityY": 3.6909794608614943,
+            "timestamp": 1.3023758967441108
+        },
+        {
+            "x": 2.0170244087221176,
+            "y": 4.5571780060792255,
+            "heading": -0.7013280555552984,
+            "angularVelocity": 0.0000017891692504432307,
+            "velocityX": -0.05878422470745493,
+            "velocityY": 3.782483218449498,
+            "timestamp": 1.3954027465115473
+        },
+        {
+            "x": 2.079587370397571,
+            "y": 4.887709205686539,
+            "heading": -0.661535006789308,
+            "angularVelocity": 0.4277587477752017,
+            "velocityX": 0.6725258549747536,
+            "velocityY": 3.5530731227987227,
+            "timestamp": 1.4884295962789837
+        },
+        {
+            "x": 2.143441139356313,
+            "y": 5.160361503306635,
+            "heading": -0.462273406431507,
+            "angularVelocity": 2.1419794484704924,
+            "velocityX": 0.6864014971846709,
+            "velocityY": 2.9308989641347156,
+            "timestamp": 1.5814564460464202
+        },
+        {
+            "x": 2.1958862911911914,
+            "y": 5.3618445595482385,
+            "heading": -0.25085794958853325,
+            "angularVelocity": 2.272628358065485,
+            "velocityX": 0.5637636012182422,
+            "velocityY": 2.165859176628076,
+            "timestamp": 1.6744832958138567
+        },
+        {
+            "x": 2.231834886718389,
+            "y": 5.494391410801975,
+            "heading": -0.08836645960984135,
+            "angularVelocity": 1.7467160328971074,
+            "velocityX": 0.38643247209883785,
+            "velocityY": 1.4248236029178551,
+            "timestamp": 1.7675101455812932
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -3.8128513304390615e-33,
+            "angularVelocity": 0.9499027413134385,
+            "velocityX": 0.19526742362042082,
+            "velocityY": 0.7052650859622128,
+            "timestamp": 1.8605369953487296
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 5.82518331797089e-32,
+            "velocityX": 0,
+            "velocityY": -4.1086986258215814e-33,
+            "timestamp": 1.953563845116166
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S4 To W3.traj b/src/main/deploy/choreo/S4 To W3.traj
new file mode 100644
index 00000000..8a67252f
--- /dev/null
+++ b/src/main/deploy/choreo/S4 To W3.traj	
@@ -0,0 +1,256 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -2.137980318394988e-34,
+            "velocityX": 1.3258659962604387e-36,
+            "velocityY": 7.61509960712835e-36,
+            "timestamp": 0
+        },
+        {
+            "x": 5.139985955005962,
+            "y": 1.6266018860262446,
+            "heading": -0.6327443563347177,
+            "angularVelocity": 0.10940864810395588,
+            "velocityX": -0.5430597418126536,
+            "velocityY": 0.09955056482775779,
+            "timestamp": 0.06631691178916982
+        },
+        {
+            "x": 5.067957879081044,
+            "y": 1.6398056681484225,
+            "heading": -0.6182319813670383,
+            "angularVelocity": 0.2188336968074761,
+            "velocityX": -1.0861192715653594,
+            "velocityY": 0.19910128149746392,
+            "timestamp": 0.13263382357833964
+        },
+        {
+            "x": 4.959915797050431,
+            "y": 1.6596113536414707,
+            "heading": -0.5964609094580124,
+            "angularVelocity": 0.328288385596709,
+            "velocityX": -1.6291784269764191,
+            "velocityY": 0.2986521078667379,
+            "timestamp": 0.19895073536750946
+        },
+        {
+            "x": 4.815859744494909,
+            "y": 1.686018943789264,
+            "heading": -0.5674282989321845,
+            "angularVelocity": 0.43778592432238833,
+            "velocityX": -2.172237045860861,
+            "velocityY": 0.39820295359570684,
+            "timestamp": 0.2652676471566793
+        },
+        {
+            "x": 4.635789767966758,
+            "y": 1.719028429378711,
+            "heading": -0.5311304969514992,
+            "angularVelocity": 0.5473385445944894,
+            "velocityX": -2.7152949627783816,
+            "velocityY": 0.49775366039945246,
+            "timestamp": 0.3315845589458491
+        },
+        {
+            "x": 4.41970592970577,
+            "y": 1.758639783689655,
+            "heading": -0.48756329690665473,
+            "angularVelocity": 0.6569545967904882,
+            "velocityX": -3.258351941175237,
+            "velocityY": 0.5973039642870227,
+            "timestamp": 0.3979014707350189
+        },
+        {
+            "x": 4.172944378919847,
+            "y": 1.8038720825822125,
+            "heading": -0.4875632898742738,
+            "angularVelocity": 1.0604204507812919e-7,
+            "velocityX": -3.7209445393116334,
+            "velocityY": 0.6820628052819626,
+            "timestamp": 0.46421838252418873
+        },
+        {
+            "x": 3.9261828364502485,
+            "y": 1.849104426845956,
+            "heading": -0.4875632828424199,
+            "angularVelocity": 1.0603409772331237e-7,
+            "velocityX": -3.7209444139088643,
+            "velocityY": 0.6820634894390606,
+            "timestamp": 0.5305352943133586
+        },
+        {
+            "x": 3.6794213056437455,
+            "y": 1.8943368347367484,
+            "heading": -0.487563275810563,
+            "angularVelocity": 1.0603414195839125e-7,
+            "velocityX": -3.720944238039768,
+            "velocityY": 0.6820644488783205,
+            "timestamp": 0.5968522061025283
+        },
+        {
+            "x": 3.4326599282156307,
+            "y": 1.9395700793621615,
+            "heading": -0.4875632687787025,
+            "angularVelocity": 1.0603419804191013e-7,
+            "velocityX": -3.720941925230195,
+            "velocityY": 0.6820770660916139,
+            "timestamp": 0.6631691178916981
+        },
+        {
+            "x": 3.192328571865682,
+            "y": 2.011528836252569,
+            "heading": -0.4875632615410612,
+            "angularVelocity": 1.0913718718412795e-7,
+            "velocityX": -3.6239829308396314,
+            "velocityY": 1.0850740022269671,
+            "timestamp": 0.7294860296808678
+        },
+        {
+            "x": 2.965054771843869,
+            "y": 2.1177542675713954,
+            "heading": -0.48756325344928275,
+            "angularVelocity": 1.2201681646141545e-7,
+            "velocityX": -3.4270866041583203,
+            "velocityY": 1.6017849512735258,
+            "timestamp": 0.7958029414700376
+        },
+        {
+            "x": 2.7556945515371734,
+            "y": 2.2559769801698577,
+            "heading": -0.48756324342419455,
+            "angularVelocity": 1.5116940667572282e-7,
+            "velocityX": -3.156965767228115,
+            "velocityY": 2.084275471660842,
+            "timestamp": 0.8621198532592074
+        },
+        {
+            "x": 2.5687211592217127,
+            "y": 2.4232437322251608,
+            "heading": -0.4875632292213124,
+            "angularVelocity": 2.1416682100483036e-7,
+            "velocityX": -2.8193923280063653,
+            "velocityY": 2.5222337340898235,
+            "timestamp": 0.9284367650483771
+        },
+        {
+            "x": 2.4081295337898796,
+            "y": 2.6159806661875145,
+            "heading": -0.48756314249031313,
+            "angularVelocity": 0.0000013078262679985986,
+            "velocityX": -2.421578766248568,
+            "velocityY": 2.906301405817713,
+            "timestamp": 0.9947536768375469
+        },
+        {
+            "x": 2.2783797783684188,
+            "y": 2.8285106690600896,
+            "heading": -0.4828540771785898,
+            "angularVelocity": 0.07100851328382284,
+            "velocityX": -1.9565108193510647,
+            "velocityY": 3.2047632668456583,
+            "timestamp": 1.0610705886267167
+        },
+        {
+            "x": 2.1829307079315186,
+            "y": 3.053825616836548,
+            "heading": -0.47038335932986286,
+            "angularVelocity": 0.18804732476646344,
+            "velocityX": -1.4392870213912456,
+            "velocityY": 3.3975488559051734,
+            "timestamp": 1.1273875004158864
+        },
+        {
+            "x": 2.124568521000596,
+            "y": 3.2749812318761844,
+            "heading": -0.4550563591461243,
+            "angularVelocity": 0.24322153086635045,
+            "velocityX": -0.9261395106595871,
+            "velocityY": 3.5094804335357668,
+            "timestamp": 1.1904041261609186
+        },
+        {
+            "x": 2.099309698866093,
+            "y": 3.4965263857952165,
+            "heading": -0.4375068558264715,
+            "angularVelocity": 0.2784900510328627,
+            "velocityX": -0.40082790590377965,
+            "velocityY": 3.5156619590425686,
+            "timestamp": 1.2534207519059508
+        },
+        {
+            "x": 2.105558929434475,
+            "y": 3.707892405315209,
+            "heading": -0.4199092504656253,
+            "angularVelocity": 0.2792533740547583,
+            "velocityX": 0.09916796550908585,
+            "velocityY": 3.354131025281934,
+            "timestamp": 1.316437377650983
+        },
+        {
+            "x": 2.135680280502732,
+            "y": 3.8963278332771982,
+            "heading": -0.40525850037923283,
+            "angularVelocity": 0.23249023433387317,
+            "velocityX": 0.4779905415775436,
+            "velocityY": 2.990249410122441,
+            "timestamp": 1.3794540033960152
+        },
+        {
+            "x": 2.177481885950485,
+            "y": 4.0538159629335215,
+            "heading": -0.39590861500997965,
+            "angularVelocity": 0.14837172347315433,
+            "velocityX": 0.6633424902324003,
+            "velocityY": 2.4991520538330105,
+            "timestamp": 1.4424706291410474
+        },
+        {
+            "x": 2.2205433619064863,
+            "y": 4.17824561445827,
+            "heading": -0.39150641146352744,
+            "angularVelocity": 0.06985781124276201,
+            "velocityX": 0.6833351587282585,
+            "velocityY": 1.9745527478446014,
+            "timestamp": 1.5054872548860796
+        },
+        {
+            "x": 2.2579292824582606,
+            "y": 4.270063380418578,
+            "heading": -0.38992694464863337,
+            "angularVelocity": 0.0250642873403699,
+            "velocityX": 0.5932707457717501,
+            "velocityY": 1.4570403425249505,
+            "timestamp": 1.5685038806311118
+        },
+        {
+            "x": 2.285342089257776,
+            "y": 4.3303097045050025,
+            "heading": -0.38974725509942826,
+            "angularVelocity": 0.002851462563739305,
+            "velocityX": 0.43500911823537725,
+            "velocityY": 0.9560385592555154,
+            "timestamp": 1.631520506376144
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.004010765374750144,
+            "velocityX": 0.23260386555018367,
+            "velocityY": 0.47115019479344145,
+            "timestamp": 1.6945371321211762
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -1.6105528525551922e-32,
+            "velocityX": 0,
+            "velocityY": -4.5276793712997926e-36,
+            "timestamp": 1.7575537578662084
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Stage Side To C1.traj b/src/main/deploy/choreo/Stage Side To C1.traj
new file mode 100644
index 00000000..a684ff7a
--- /dev/null
+++ b/src/main/deploy/choreo/Stage Side To C1.traj	
@@ -0,0 +1,616 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": -1.04,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 5.951430910881942e-24,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7172662637575132,
+            "y": 4.364294720467062,
+            "heading": -1.0315470131742317,
+            "angularVelocity": 0.26098153509522004,
+            "velocityX": 0.22434208273715575,
+            "velocityY": 0.13259724206381326,
+            "timestamp": 0.03238921413610362
+        },
+        {
+            "x": 0.7318144654254066,
+            "y": 4.3728967855708385,
+            "heading": -1.0149312072454106,
+            "angularVelocity": 0.5130042939294357,
+            "velocityX": 0.4491680967238058,
+            "velocityY": 0.2655842487449578,
+            "timestamp": 0.06477842827220724
+        },
+        {
+            "x": 0.7536624687467911,
+            "y": 4.385820612299006,
+            "heading": -0.9904958605622504,
+            "angularVelocity": 0.7544285137786853,
+            "velocityX": 0.6745456444104029,
+            "velocityY": 0.39901637235962667,
+            "timestamp": 0.09716764240831086
+        },
+        {
+            "x": 0.7828310955502448,
+            "y": 4.4030821899572965,
+            "heading": -0.9586530378385134,
+            "angularVelocity": 0.9831304516969581,
+            "velocityX": 0.9005660551343861,
+            "velocityY": 0.5329421574032179,
+            "timestamp": 0.12955685654441448
+        },
+        {
+            "x": 0.8193448564336872,
+            "y": 4.424698840047832,
+            "heading": -0.919902879162077,
+            "angularVelocity": 1.1963908266993857,
+            "velocityX": 1.1273432177146,
+            "velocityY": 0.6674027347406944,
+            "timestamp": 0.1619460706805181
+        },
+        {
+            "x": 0.8632324943492647,
+            "y": 4.450689040734006,
+            "heading": -0.8748582540701377,
+            "angularVelocity": 1.3907291761589549,
+            "velocityX": 1.3550078038681697,
+            "velocityY": 0.8024338156819394,
+            "timestamp": 0.19433528481662174
+        },
+        {
+            "x": 0.9145271224696989,
+            "y": 4.481072488182425,
+            "heading": -0.8242801160685745,
+            "angularVelocity": 1.5615734852049024,
+            "velocityX": 1.5836947418633758,
+            "velocityY": 0.9380730054376618,
+            "timestamp": 0.22672449895272537
+        },
+        {
+            "x": 0.9732655893831887,
+            "y": 4.515870568727518,
+            "heading": -0.7691375276578237,
+            "angularVelocity": 1.7024984977725797,
+            "velocityX": 1.8135193606940647,
+            "velocityY": 1.0743724870528544,
+            "timestamp": 0.25911371308882897
+        },
+        {
+            "x": 1.0394861119792222,
+            "y": 4.555106942812012,
+            "heading": -0.7107271690719036,
+            "angularVelocity": 1.8033891881560444,
+            "velocityX": 2.044523905944935,
+            "velocityY": 1.2114024724285466,
+            "timestamp": 0.2915029272249326
+        },
+        {
+            "x": 1.1132202582415271,
+            "y": 4.598805444215395,
+            "heading": -0.6509416110612904,
+            "angularVelocity": 1.8458477491731227,
+            "velocityX": 2.276503096137642,
+            "velocityY": 1.3491683132463759,
+            "timestamp": 0.3238921413610362
+        },
+        {
+            "x": 1.194458181637227,
+            "y": 4.646971254862031,
+            "heading": -0.5929406477835183,
+            "angularVelocity": 1.790749322723445,
+            "velocityX": 2.508178279791774,
+            "velocityY": 1.48709414326129,
+            "timestamp": 0.35628135549713985
+        },
+        {
+            "x": 1.2829200414273145,
+            "y": 4.699514481647374,
+            "heading": -0.5430956567390244,
+            "angularVelocity": 1.5389379574026985,
+            "velocityX": 2.7312135273909255,
+            "velocityY": 1.6222445708176134,
+            "timestamp": 0.3886705696332435
+        },
+        {
+            "x": 1.3771379621947135,
+            "y": 4.756486240363935,
+            "heading": -0.5116699283377882,
+            "angularVelocity": 0.9702528832339478,
+            "velocityX": 2.908928891311882,
+            "velocityY": 1.758973171660129,
+            "timestamp": 0.4210597837693471
+        },
+        {
+            "x": 1.4771406301349097,
+            "y": 4.8178582459332,
+            "heading": -0.4985254849792972,
+            "angularVelocity": 0.40582779511896505,
+            "velocityX": 3.087529926474048,
+            "velocityY": 1.894828485537579,
+            "timestamp": 0.45344899790545073
+        },
+        {
+            "x": 1.5826858211522454,
+            "y": 4.880091207833619,
+            "heading": -0.49852543793275184,
+            "angularVelocity": 0.000001452537415626245,
+            "velocityX": 3.2586524197166784,
+            "velocityY": 1.9214100607352578,
+            "timestamp": 0.48583821204155436
+        },
+        {
+            "x": 1.6882312828144117,
+            "y": 4.9423237107273525,
+            "heading": -0.4985253908851185,
+            "angularVelocity": 0.0000014525710071923091,
+            "velocityX": 3.2586607757338757,
+            "velocityY": 1.9213958891446032,
+            "timestamp": 0.5182274261776579
+        },
+        {
+            "x": 1.7937767445018398,
+            "y": 5.004556213578243,
+            "heading": -0.49852534383749064,
+            "angularVelocity": 0.0000014525708350435827,
+            "velocityX": 3.2586607765138327,
+            "velocityY": 1.921395887821837,
+            "timestamp": 0.5506166403137616
+        },
+        {
+            "x": 1.8993222061892705,
+            "y": 5.06678871642913,
+            "heading": -0.49852529678986307,
+            "angularVelocity": 0.0000014525708270038968,
+            "velocityX": 3.2586607765139086,
+            "velocityY": 1.9213958878217097,
+            "timestamp": 0.5830058544498652
+        },
+        {
+            "x": 2.0048676678767015,
+            "y": 5.129021219280016,
+            "heading": -0.4985252497422358,
+            "angularVelocity": 0.0000014525708183379938,
+            "velocityX": 3.258660776513909,
+            "velocityY": 1.9213958878217101,
+            "timestamp": 0.6153950685859688
+        },
+        {
+            "x": 2.1104131295641317,
+            "y": 5.191253722130902,
+            "heading": -0.49852520269460865,
+            "angularVelocity": 0.000001452570810915054,
+            "velocityX": 3.2586607765139104,
+            "velocityY": 1.9213958878217086,
+            "timestamp": 0.6477842827220724
+        },
+        {
+            "x": 2.2159585912515625,
+            "y": 5.253486224981789,
+            "heading": -0.4985251556469819,
+            "angularVelocity": 0.0000014525708028649706,
+            "velocityX": 3.2586607765139086,
+            "velocityY": 1.9213958878217123,
+            "timestamp": 0.6801734968581761
+        },
+        {
+            "x": 2.321504052938993,
+            "y": 5.315718727832675,
+            "heading": -0.4985251085993554,
+            "angularVelocity": 0.000001452570794356237,
+            "velocityX": 3.2586607765139117,
+            "velocityY": 1.9213958878217081,
+            "timestamp": 0.7125627109942797
+        },
+        {
+            "x": 2.427049514626424,
+            "y": 5.3779512306835615,
+            "heading": -0.49852506155172915,
+            "angularVelocity": 0.0000014525707870212498,
+            "velocityX": 3.258660776513912,
+            "velocityY": 1.9213958878217077,
+            "timestamp": 0.7449519251303833
+        },
+        {
+            "x": 2.5325949763138547,
+            "y": 5.440183733534448,
+            "heading": -0.4985250145041031,
+            "angularVelocity": 0.000001452570779107594,
+            "velocityX": 3.258660776513913,
+            "velocityY": 1.9213958878217068,
+            "timestamp": 0.777341139266487
+        },
+        {
+            "x": 2.6381404380012854,
+            "y": 5.502416236385335,
+            "heading": -0.49852496745647734,
+            "angularVelocity": 0.0000014525707705186945,
+            "velocityX": 3.258660776513915,
+            "velocityY": 1.9213958878217055,
+            "timestamp": 0.8097303534025906
+        },
+        {
+            "x": 2.743685899688716,
+            "y": 5.564648739236221,
+            "heading": -0.4985249204088519,
+            "angularVelocity": 0.00000145257076360687,
+            "velocityX": 3.2586607765139153,
+            "velocityY": 1.9213958878217066,
+            "timestamp": 0.8421195675386942
+        },
+        {
+            "x": 2.849231361376147,
+            "y": 5.626881242087108,
+            "heading": -0.4985248733612266,
+            "angularVelocity": 0.0000014525707544682094,
+            "velocityX": 3.2586607765139153,
+            "velocityY": 1.9213958878217066,
+            "timestamp": 0.8745087816747978
+        },
+        {
+            "x": 2.954776823063578,
+            "y": 5.689113744937994,
+            "heading": -0.4985248263136016,
+            "angularVelocity": 0.0000014525707471414822,
+            "velocityX": 3.2586607765139166,
+            "velocityY": 1.921395887821705,
+            "timestamp": 0.9068979958109015
+        },
+        {
+            "x": 3.0603222847510096,
+            "y": 5.751346247788881,
+            "heading": -0.49852477926597694,
+            "angularVelocity": 0.0000014525707385370494,
+            "velocityX": 3.258660776513917,
+            "velocityY": 1.9213958878217057,
+            "timestamp": 0.9392872099470051
+        },
+        {
+            "x": 3.1658677464384404,
+            "y": 5.813578750639766,
+            "heading": -0.4985247322183525,
+            "angularVelocity": 0.0000014525707310188908,
+            "velocityX": 3.2586607765139175,
+            "velocityY": 1.9213958878217063,
+            "timestamp": 0.9716764240831087
+        },
+        {
+            "x": 3.271413208125871,
+            "y": 5.875811253490653,
+            "heading": -0.4985246851707283,
+            "angularVelocity": 0.0000014525707228586851,
+            "velocityX": 3.258660776513917,
+            "velocityY": 1.921395887821708,
+            "timestamp": 1.0040656382192124
+        },
+        {
+            "x": 3.376958669813326,
+            "y": 5.938043756341499,
+            "heading": -0.4985246381231044,
+            "angularVelocity": 0.000001452570714543944,
+            "velocityX": 3.258660776514645,
+            "velocityY": 1.921395887820474,
+            "timestamp": 1.0364548523553159
+        },
+        {
+            "x": 3.482504131751534,
+            "y": 6.000276258767071,
+            "heading": -0.49852459107548075,
+            "angularVelocity": 0.0000014525707064344823,
+            "velocityX": 3.258660784256519,
+            "velocityY": 1.9213958746903632,
+            "timestamp": 1.0688440664914194
+        },
+        {
+            "x": 3.588052280044538,
+            "y": 6.0625042049472775,
+            "heading": -0.4985245440280508,
+            "angularVelocity": 0.0000014525647243024202,
+            "velocityX": 3.258743724052004,
+            "velocityY": 1.921255203004168,
+            "timestamp": 1.101233280627523
+        },
+        {
+            "x": 3.69740891456604,
+            "y": 6.1177659034729,
+            "heading": -0.4985244969178424,
+            "angularVelocity": 0.0000014545029788824699,
+            "velocityX": 3.3763287390046313,
+            "velocityY": 1.7061759601022248,
+            "timestamp": 1.1336224947636264
+        },
+        {
+            "x": 3.8477077097405896,
+            "y": 6.176720729441604,
+            "heading": -0.4985244519972541,
+            "angularVelocity": 0.0000010525498975792195,
+            "velocityX": 3.5217032391581933,
+            "velocityY": 1.3813909907718895,
+            "timestamp": 1.1763003653755966
+        },
+        {
+            "x": 4.000682189533232,
+            "y": 6.228336695265556,
+            "heading": -0.49852440748602983,
+            "angularVelocity": 0.000001042957945295385,
+            "velocityX": 3.58439813418752,
+            "velocityY": 1.2094316113670969,
+            "timestamp": 1.2189782359875667
+        },
+        {
+            "x": 4.153656816760498,
+            "y": 6.279952224134777,
+            "heading": -0.4985243629748153,
+            "angularVelocity": 0.00000104295771772215,
+            "velocityX": 3.5844015887793454,
+            "velocityY": 1.2094213729291292,
+            "timestamp": 1.2616561065995369
+        },
+        {
+            "x": 4.306631443993679,
+            "y": 6.331567752986466,
+            "heading": -0.4985243184636008,
+            "angularVelocity": 0.0000010429577168479619,
+            "velocityX": 3.584401588917953,
+            "velocityY": 1.209421372518333,
+            "timestamp": 1.304333977211507
+        },
+        {
+            "x": 4.459606071226857,
+            "y": 6.38318328183816,
+            "heading": -0.4985242739523863,
+            "angularVelocity": 0.000001042957716433214,
+            "velocityX": 3.584401588917909,
+            "velocityY": 1.2094213725184633,
+            "timestamp": 1.3470118478234772
+        },
+        {
+            "x": 4.612580698460036,
+            "y": 6.434798810689855,
+            "heading": -0.4985242294411719,
+            "angularVelocity": 0.0000010429577140470852,
+            "velocityX": 3.5844015889179057,
+            "velocityY": 1.209421372518471,
+            "timestamp": 1.3896897184354473
+        },
+        {
+            "x": 4.765555325693214,
+            "y": 6.4864143395415494,
+            "heading": -0.49852418492995754,
+            "angularVelocity": 0.0000010429577143791187,
+            "velocityX": 3.584401588917906,
+            "velocityY": 1.2094213725184708,
+            "timestamp": 1.4323675890474175
+        },
+        {
+            "x": 4.918529952926393,
+            "y": 6.5380298683932425,
+            "heading": -0.4985241404187432,
+            "angularVelocity": 0.0000010429577134079309,
+            "velocityX": 3.584401588917911,
+            "velocityY": 1.2094213725184557,
+            "timestamp": 1.4750454596593876
+        },
+        {
+            "x": 5.071504580159572,
+            "y": 6.589645397244938,
+            "heading": -0.49852409590752894,
+            "angularVelocity": 0.000001042957711441133,
+            "velocityX": 3.584401588917906,
+            "velocityY": 1.20942137251847,
+            "timestamp": 1.5177233302713578
+        },
+        {
+            "x": 5.224479207392751,
+            "y": 6.641260926096632,
+            "heading": -0.49852405139631467,
+            "angularVelocity": 0.0000010429577109856096,
+            "velocityX": 3.5844015889179066,
+            "velocityY": 1.2094213725184684,
+            "timestamp": 1.560401200883328
+        },
+        {
+            "x": 5.377453834625929,
+            "y": 6.692876454948327,
+            "heading": -0.4985240068851004,
+            "angularVelocity": 0.000001042957710628888,
+            "velocityX": 3.584401588917908,
+            "velocityY": 1.209421372518467,
+            "timestamp": 1.603079071495298
+        },
+        {
+            "x": 5.530428461859109,
+            "y": 6.744491983800022,
+            "heading": -0.4985239623738862,
+            "angularVelocity": 0.0000010429577097054993,
+            "velocityX": 3.5844015889179075,
+            "velocityY": 1.2094213725184686,
+            "timestamp": 1.6457569421072682
+        },
+        {
+            "x": 5.683403089092287,
+            "y": 6.796107512651717,
+            "heading": -0.4985239178626721,
+            "angularVelocity": 0.0000010429577084041104,
+            "velocityX": 3.584401588917908,
+            "velocityY": 1.2094213725184642,
+            "timestamp": 1.6884348127192383
+        },
+        {
+            "x": 5.836377716325465,
+            "y": 6.847723041503412,
+            "heading": -0.4985238733514579,
+            "angularVelocity": 0.000001042957708437487,
+            "velocityX": 3.5844015889179084,
+            "velocityY": 1.209421372518465,
+            "timestamp": 1.7311126833312085
+        },
+        {
+            "x": 5.989352343558645,
+            "y": 6.899338570355106,
+            "heading": -0.4985238288402439,
+            "angularVelocity": 0.0000010429577061050693,
+            "velocityX": 3.5844015889179084,
+            "velocityY": 1.2094213725184642,
+            "timestamp": 1.7737905539431786
+        },
+        {
+            "x": 6.142326970791823,
+            "y": 6.9509540992067995,
+            "heading": -0.49852378432902983,
+            "angularVelocity": 0.0000010429577060929704,
+            "velocityX": 3.584401588917909,
+            "velocityY": 1.2094213725184646,
+            "timestamp": 1.8164684245551488
+        },
+        {
+            "x": 6.295301598025002,
+            "y": 7.002569628058494,
+            "heading": -0.4985237398178158,
+            "angularVelocity": 0.0000010429577057751362,
+            "velocityX": 3.58440158891791,
+            "velocityY": 1.2094213725184613,
+            "timestamp": 1.859146295167119
+        },
+        {
+            "x": 6.44827622525818,
+            "y": 7.054185156910189,
+            "heading": -0.49852369530660184,
+            "angularVelocity": 0.0000010429577050460837,
+            "velocityX": 3.5844015889179097,
+            "velocityY": 1.2094213725184615,
+            "timestamp": 1.901824165779089
+        },
+        {
+            "x": 6.60125085249139,
+            "y": 7.105800685761796,
+            "heading": -0.4985236507953879,
+            "angularVelocity": 0.0000010429577031279755,
+            "velocityX": 3.584401588918602,
+            "velocityY": 1.2094213725164094,
+            "timestamp": 1.9445020363910592
+        },
+        {
+            "x": 6.754225480461122,
+            "y": 7.15741621243045,
+            "heading": -0.49852360628410947,
+            "angularVelocity": 0.000001042959214800434,
+            "velocityX": 3.584401606176385,
+            "velocityY": 1.209421321366894,
+            "timestamp": 1.9871799070030294
+        },
+        {
+            "x": 6.905473027890085,
+            "y": 7.208390081717835,
+            "heading": -0.49401401466016337,
+            "angularVelocity": 0.10566580664128356,
+            "velocityX": 3.543933782547739,
+            "velocityY": 1.1943864245440503,
+            "timestamp": 2.0298577776149997
+        },
+        {
+            "x": 7.045508663452743,
+            "y": 7.253974234854833,
+            "heading": -0.45930885159823864,
+            "angularVelocity": 0.8131887220303519,
+            "velocityX": 3.2812235839006783,
+            "velocityY": 1.0680981146283737,
+            "timestamp": 2.07253564822697
+        },
+        {
+            "x": 7.17296615196941,
+            "y": 7.296903697598188,
+            "heading": -0.39602280772923965,
+            "angularVelocity": 1.4828772607799376,
+            "velocityX": 2.986500654531669,
+            "velocityY": 1.0058951425592673,
+            "timestamp": 2.1152135188389405
+        },
+        {
+            "x": 7.286216905364156,
+            "y": 7.335009151903716,
+            "heading": -0.3243524220820172,
+            "angularVelocity": 1.6793336832302157,
+            "velocityX": 2.6536177126649325,
+            "velocityY": 0.8928621264164087,
+            "timestamp": 2.157891389450911
+        },
+        {
+            "x": 7.38508554947564,
+            "y": 7.368236895639727,
+            "heading": -0.2530399098538711,
+            "angularVelocity": 1.6709482269282783,
+            "velocityX": 2.31662551795061,
+            "velocityY": 0.7785707969856192,
+            "timestamp": 2.2005692600628812
+        },
+        {
+            "x": 7.469627579006154,
+            "y": 7.396613050645165,
+            "heading": -0.18659143584410423,
+            "angularVelocity": 1.5569772591027433,
+            "velocityX": 1.9809336388681473,
+            "velocityY": 0.6648915374301301,
+            "timestamp": 2.2432471306748516
+        },
+        {
+            "x": 7.539923583097788,
+            "y": 7.420174905803342,
+            "heading": -0.12774543015301906,
+            "angularVelocity": 1.3788411850749698,
+            "velocityX": 1.6471300719468636,
+            "velocityY": 0.5520860066427349,
+            "timestamp": 2.285925001286822
+        },
+        {
+            "x": 7.596048224439389,
+            "y": 7.4389607760624,
+            "heading": -0.07839593087005978,
+            "angularVelocity": 1.1563252471438406,
+            "velocityX": 1.315075952403783,
+            "velocityY": 0.4401782467044836,
+            "timestamp": 2.3286028718987923
+        },
+        {
+            "x": 7.638066692552635,
+            "y": 7.453006402993039,
+            "heading": -0.03996068961729096,
+            "angularVelocity": 0.9005894788478175,
+            "velocityX": 0.9845493111706484,
+            "velocityY": 0.3291079599154316,
+            "timestamp": 2.3712807425107627
+        },
+        {
+            "x": 7.666034942577791,
+            "y": 7.46234355914255,
+            "heading": -0.013541291231896178,
+            "angularVelocity": 0.6190420938664342,
+            "velocityX": 0.655333774251397,
+            "velocityY": 0.2187821467102802,
+            "timestamp": 2.413958613122733
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0.31729069510085045,
+            "velocityX": 0.3272201077973249,
+            "velocityY": 0.10910668200355174,
+            "timestamp": 2.4566364837347034
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 5.5875375349932e-23,
+            "velocityY": -3.709612464642882e-23,
+            "timestamp": 2.499314354346674
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Stage Side To C2.traj b/src/main/deploy/choreo/Stage Side To C2.traj
new file mode 100644
index 00000000..a6fd22bb
--- /dev/null
+++ b/src/main/deploy/choreo/Stage Side To C2.traj	
@@ -0,0 +1,607 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": -1.04,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7226799898554153,
+            "y": 4.366381680411806,
+            "heading": -1.026991220057009,
+            "angularVelocity": 0.3105337423596819,
+            "velocityX": 0.3026851649532591,
+            "velocityY": 0.15233766037290783,
+            "timestamp": 0.04189167928786116
+        },
+        {
+            "x": 0.7480755502425919,
+            "y": 4.379165077654378,
+            "heading": -1.001621000893582,
+            "angularVelocity": 0.6056147567896238,
+            "velocityX": 0.6062196793943158,
+            "velocityY": 0.3051536118838673,
+            "timestamp": 0.08378335857572232
+        },
+        {
+            "x": 0.7862289943958503,
+            "y": 4.398373013543058,
+            "heading": -0.964694126725153,
+            "angularVelocity": 0.8814846956762871,
+            "velocityX": 0.9107642568130221,
+            "velocityY": 0.4585143449774818,
+            "timestamp": 0.12567503786358347
+        },
+        {
+            "x": 0.8371909372554502,
+            "y": 4.424030289360705,
+            "heading": -0.9172220550063085,
+            "angularVelocity": 1.1332100437568369,
+            "velocityX": 1.2165170679697972,
+            "velocityY": 0.6124671116987632,
+            "timestamp": 0.16756671715144464
+        },
+        {
+            "x": 0.9010212440299602,
+            "y": 4.456163358075127,
+            "heading": -0.8604944995891576,
+            "angularVelocity": 1.354148517832005,
+            "velocityX": 1.523698926841682,
+            "velocityY": 0.767051339565964,
+            "timestamp": 0.2094583964393058
+        },
+        {
+            "x": 0.9777886193671033,
+            "y": 4.494801080149917,
+            "heading": -0.796206518536166,
+            "angularVelocity": 1.5346241102256366,
+            "velocityX": 1.8325208404664681,
+            "velocityY": 0.9223244981249834,
+            "timestamp": 0.25135007572716694
+        },
+        {
+            "x": 1.0675667159355893,
+            "y": 4.539976112871383,
+            "heading": -0.7267376359019178,
+            "angularVelocity": 1.658297872398206,
+            "velocityX": 2.143100923492954,
+            "velocityY": 1.07837722166836,
+            "timestamp": 0.2932417550150281
+        },
+        {
+            "x": 1.1704147696438687,
+            "y": 4.591719023811467,
+            "heading": -0.6558650979752687,
+            "angularVelocity": 1.6918046526529587,
+            "velocityX": 2.4550950321555027,
+            "velocityY": 1.23515962643868,
+            "timestamp": 0.3351334343028892
+        },
+        {
+            "x": 1.286251945776662,
+            "y": 4.64999734523705,
+            "heading": -0.5908433350340152,
+            "angularVelocity": 1.5521402829056499,
+            "velocityX": 2.765159528144268,
+            "velocityY": 1.3911669910657207,
+            "timestamp": 0.37702511359075036
+        },
+        {
+            "x": 1.4132089603720126,
+            "y": 4.714842752574763,
+            "heading": -0.5508840698501896,
+            "angularVelocity": 0.9538711711517488,
+            "velocityX": 3.0306021805179513,
+            "velocityY": 1.547930482617412,
+            "timestamp": 0.4189167928786115
+        },
+        {
+            "x": 1.550306465812279,
+            "y": 4.786404286835006,
+            "heading": -0.541265046364201,
+            "angularVelocity": 0.22961656466170327,
+            "velocityX": 3.272666738857444,
+            "velocityY": 1.7082517453765305,
+            "timestamp": 0.46080847216647264
+        },
+        {
+            "x": 1.6919561664396436,
+            "y": 4.857462558344818,
+            "heading": -0.5412650016128938,
+            "angularVelocity": 0.000001068262432444083,
+            "velocityX": 3.3813325948098276,
+            "velocityY": 1.6962383155263616,
+            "timestamp": 0.5027001514543338
+        },
+        {
+            "x": 1.8336059249814771,
+            "y": 4.928520714406238,
+            "heading": -0.541264956861776,
+            "angularVelocity": 0.0000010682579123299043,
+            "velocityX": 3.3813339772912605,
+            "velocityY": 1.6962355596475316,
+            "timestamp": 0.544591830742195
+        },
+        {
+            "x": 1.9752556835258195,
+            "y": 4.999578870462656,
+            "heading": -0.5412649121106583,
+            "angularVelocity": 0.0000010682579080478101,
+            "velocityX": 3.3813339773511535,
+            "velocityY": 1.696235559528139,
+            "timestamp": 0.5864835100300562
+        },
+        {
+            "x": 2.1169054420701623,
+            "y": 5.0706370265190746,
+            "heading": -0.5412648673595408,
+            "angularVelocity": 0.0000010682579032833847,
+            "velocityX": 3.381333977351157,
+            "velocityY": 1.6962355595281329,
+            "timestamp": 0.6283751893179174
+        },
+        {
+            "x": 2.258555200614505,
+            "y": 5.141695182575493,
+            "heading": -0.5412648226084235,
+            "angularVelocity": 0.0000010682578992307337,
+            "velocityX": 3.381333977351157,
+            "velocityY": 1.6962355595281327,
+            "timestamp": 0.6702668686057786
+        },
+        {
+            "x": 2.400204959158848,
+            "y": 5.212753338631911,
+            "heading": -0.5412647778573064,
+            "angularVelocity": 0.0000010682578937479676,
+            "velocityX": 3.3813339773511575,
+            "velocityY": 1.696235559528132,
+            "timestamp": 0.7121585478936397
+        },
+        {
+            "x": 2.5418547177031905,
+            "y": 5.2838114946883294,
+            "heading": -0.5412647331061895,
+            "angularVelocity": 0.0000010682578893581349,
+            "velocityX": 3.3813339773511584,
+            "velocityY": 1.696235559528132,
+            "timestamp": 0.7540502271815009
+        },
+        {
+            "x": 2.6835044762475335,
+            "y": 5.354869650744748,
+            "heading": -0.5412646883550728,
+            "angularVelocity": 0.0000010682578845109594,
+            "velocityX": 3.3813339773511584,
+            "velocityY": 1.6962355595281313,
+            "timestamp": 0.7959419064693621
+        },
+        {
+            "x": 2.8251542347918766,
+            "y": 5.425927806801167,
+            "heading": -0.5412646436039563,
+            "angularVelocity": 0.0000010682578796288815,
+            "velocityX": 3.3813339773511593,
+            "velocityY": 1.6962355595281313,
+            "timestamp": 0.8378335857572233
+        },
+        {
+            "x": 2.9668039933362196,
+            "y": 5.496985962857585,
+            "heading": -0.5412645988528401,
+            "angularVelocity": 0.0000010682578747880459,
+            "velocityX": 3.38133397735116,
+            "velocityY": 1.6962355595281309,
+            "timestamp": 0.8797252650450845
+        },
+        {
+            "x": 3.108453751880562,
+            "y": 5.5680441189140035,
+            "heading": -0.541264554101724,
+            "angularVelocity": 0.0000010682578699288912,
+            "velocityX": 3.38133397735116,
+            "velocityY": 1.69623555952813,
+            "timestamp": 0.9216169443329457
+        },
+        {
+            "x": 3.250103510424905,
+            "y": 5.639102274970423,
+            "heading": -0.5412645093506081,
+            "angularVelocity": 0.000001068257865104235,
+            "velocityX": 3.3813339773511606,
+            "velocityY": 1.69623555952813,
+            "timestamp": 0.9635086236208069
+        },
+        {
+            "x": 3.3917532689692482,
+            "y": 5.71016043102684,
+            "heading": -0.5412644645994924,
+            "angularVelocity": 0.0000010682578613220535,
+            "velocityX": 3.381333977351161,
+            "velocityY": 1.6962355595281293,
+            "timestamp": 1.005400302908668
+        },
+        {
+            "x": 3.5334030275135913,
+            "y": 5.7812185870832575,
+            "heading": -0.5412644198483769,
+            "angularVelocity": 0.0000010682578558700584,
+            "velocityX": 3.381333977351162,
+            "velocityY": 1.6962355595281287,
+            "timestamp": 1.0472919821965292
+        },
+        {
+            "x": 3.675052786057934,
+            "y": 5.852276743139676,
+            "heading": -0.5412643750972616,
+            "angularVelocity": 0.0000010682578513840427,
+            "velocityX": 3.381333977351162,
+            "velocityY": 1.6962355595281287,
+            "timestamp": 1.0891836614843904
+        },
+        {
+            "x": 3.816702544602277,
+            "y": 5.923334899196093,
+            "heading": -0.5412643303461464,
+            "angularVelocity": 0.0000010682578474926016,
+            "velocityX": 3.381333977351163,
+            "velocityY": 1.6962355595281278,
+            "timestamp": 1.1310753407722516
+        },
+        {
+            "x": 3.95835230314662,
+            "y": 5.994393055252511,
+            "heading": -0.5412642855950315,
+            "angularVelocity": 0.000001068257842976485,
+            "velocityX": 3.3813339773511637,
+            "velocityY": 1.6962355595281275,
+            "timestamp": 1.1729670200601128
+        },
+        {
+            "x": 4.100002061690963,
+            "y": 6.065451211308929,
+            "heading": -0.5412642408439168,
+            "angularVelocity": 0.0000010682578369198447,
+            "velocityX": 3.3813339773511637,
+            "velocityY": 1.6962355595281269,
+            "timestamp": 1.214858699347974
+        },
+        {
+            "x": 4.2416518202353055,
+            "y": 6.136509367365346,
+            "heading": -0.5412641960928023,
+            "angularVelocity": 0.0000010682578319796932,
+            "velocityX": 3.3813339773511646,
+            "velocityY": 1.696235559528127,
+            "timestamp": 1.2567503786358352
+        },
+        {
+            "x": 4.383301578779649,
+            "y": 6.207567523421765,
+            "heading": -0.5412641513416879,
+            "angularVelocity": 0.0000010682578277344439,
+            "velocityX": 3.381333977351165,
+            "velocityY": 1.6962355595281264,
+            "timestamp": 1.2986420579236964
+        },
+        {
+            "x": 4.524951337323992,
+            "y": 6.278625679478183,
+            "heading": -0.5412641065905739,
+            "angularVelocity": 0.0000010682578220378627,
+            "velocityX": 3.3813339773511655,
+            "velocityY": 1.6962355595281264,
+            "timestamp": 1.3405337372115576
+        },
+        {
+            "x": 4.666601095868337,
+            "y": 6.349683835534597,
+            "heading": -0.54126406183946,
+            "angularVelocity": 0.0000010682578180389232,
+            "velocityX": 3.381333977351213,
+            "velocityY": 1.6962355595280314,
+            "timestamp": 1.3824254164994187
+        },
+        {
+            "x": 4.8082508544580715,
+            "y": 6.420741991500528,
+            "heading": -0.5412640170883463,
+            "angularVelocity": 0.000001068257814314864,
+            "velocityX": 3.3813339784347316,
+            "velocityY": 1.6962355573681103,
+            "timestamp": 1.42431709578728
+        },
+        {
+            "x": 4.949901660762853,
+            "y": 6.491798058877407,
+            "heading": -0.54126397233727,
+            "angularVelocity": 0.0000010682569195340074,
+            "velocityX": 3.3813589885337056,
+            "velocityY": 1.6961857004731962,
+            "timestamp": 1.4662087750751411
+        },
+        {
+            "x": 5.0966757016987385,
+            "y": 6.551558383908352,
+            "heading": -0.5412639273749705,
+            "angularVelocity": 0.000001073299046263079,
+            "velocityX": 3.503656177813225,
+            "velocityY": 1.4265440308634512,
+            "timestamp": 1.5081004543630023
+        },
+        {
+            "x": 5.2483360143420885,
+            "y": 6.597526467706988,
+            "heading": -0.5412638804954443,
+            "angularVelocity": 0.000001119065339105663,
+            "velocityX": 3.6202968040790893,
+            "velocityY": 1.097308214425216,
+            "timestamp": 1.5499921336508635
+        },
+        {
+            "x": 5.403589728009299,
+            "y": 6.629310092724924,
+            "heading": -0.5412638294474256,
+            "angularVelocity": 0.0000012185717939310704,
+            "velocityX": 3.7060751993343666,
+            "velocityY": 0.7587097380253649,
+            "timestamp": 1.5918838129387247
+        },
+        {
+            "x": 5.561113217492597,
+            "y": 6.646638205385644,
+            "heading": -0.54126377105557,
+            "angularVelocity": 0.0000013938771748607603,
+            "velocityX": 3.760257219598817,
+            "velocityY": 0.4136409176067717,
+            "timestamp": 1.633775492226586
+        },
+        {
+            "x": 5.7195634841918945,
+            "y": 6.649363040924072,
+            "heading": -0.5412637028328066,
+            "angularVelocity": 0.000001628551649307522,
+            "velocityX": 3.7823804009024578,
+            "velocityY": 0.06504479134637085,
+            "timestamp": 1.675667171514447
+        },
+        {
+            "x": 5.8386728592662305,
+            "y": 6.64312635201416,
+            "heading": -0.5412636336326876,
+            "angularVelocity": 0.000002194803906563273,
+            "velocityX": 3.777764046962115,
+            "velocityY": -0.19780759592812253,
+            "timestamp": 1.7071962383364254
+        },
+        {
+            "x": 5.95706146956484,
+            "y": 6.62863229973747,
+            "heading": -0.5412635727602775,
+            "angularVelocity": 0.0000019306759164775456,
+            "velocityX": 3.7549037200201067,
+            "velocityY": -0.459704448549871,
+            "timestamp": 1.7387253051584037
+        },
+        {
+            "x": 6.0741575637122,
+            "y": 6.605950895283392,
+            "heading": -0.5412635175985968,
+            "angularVelocity": 0.000001749550058024173,
+            "velocityX": 3.713909289117771,
+            "velocityY": -0.7193807727372066,
+            "timestamp": 1.770254371980382
+        },
+        {
+            "x": 6.189395638406918,
+            "y": 6.575191700510336,
+            "heading": -0.5412634663439824,
+            "angularVelocity": 0.0000016256305579593937,
+            "velocityX": 3.654978922953296,
+            "velocityY": -0.9755821492190131,
+            "timestamp": 1.8017834388023604
+        },
+        {
+            "x": 6.302219177732501,
+            "y": 6.536503313694644,
+            "heading": -0.5412634176727114,
+            "angularVelocity": 0.0000015436952600896132,
+            "velocityX": 3.578397672300798,
+            "velocityY": -1.2270704691051513,
+            "timestamp": 1.8333125056243387
+        },
+        {
+            "x": 6.412083374588637,
+            "y": 6.49007271186475,
+            "heading": -0.5412633705487488,
+            "angularVelocity": 0.0000014946196396789507,
+            "velocityX": 3.484536902930231,
+            "velocityY": -1.4726284825381617,
+            "timestamp": 1.864841572446317
+        },
+        {
+            "x": 6.518458035998333,
+            "y": 6.436124829047777,
+            "heading": -0.5412633241017745,
+            "angularVelocity": 0.0000014731477658308475,
+            "velocityX": 3.3738601275551834,
+            "velocityY": -1.7110523163142606,
+            "timestamp": 1.8963706392682953
+        },
+        {
+            "x": 6.622870515934444,
+            "y": 6.378470889852871,
+            "heading": -0.5412632777559726,
+            "angularVelocity": 0.0000014699389024112294,
+            "velocityX": 3.3116260790606757,
+            "velocityY": -1.8285964351699866,
+            "timestamp": 1.9278997060902736
+        },
+        {
+            "x": 6.7272827038996414,
+            "y": 6.320816421895741,
+            "heading": -0.5412632314101712,
+            "angularVelocity": 0.0000014699388892769531,
+            "velocityX": 3.311616818687876,
+            "velocityY": -1.8286132057971307,
+            "timestamp": 1.959428772912252
+        },
+        {
+            "x": 6.831694893880165,
+            "y": 6.263161957588452,
+            "heading": -0.5412631850642508,
+            "angularVelocity": 0.0000014699426583817394,
+            "velocityX": 3.311616882607505,
+            "velocityY": -1.828613090035986,
+            "timestamp": 1.9909578397342302
+        },
+        {
+            "x": 6.935462774507961,
+            "y": 6.205910954727142,
+            "heading": -0.5387292297574777,
+            "angularVelocity": 0.08036886473933494,
+            "velocityX": 3.2911814743423053,
+            "velocityY": -1.815816598206477,
+            "timestamp": 2.0224869065562086
+        },
+        {
+            "x": 7.033610221214887,
+            "y": 6.151825220973909,
+            "heading": -0.514724354544779,
+            "angularVelocity": 0.7613569836442236,
+            "velocityX": 3.11291949302188,
+            "velocityY": -1.715424502051262,
+            "timestamp": 2.054015973378187
+        },
+        {
+            "x": 7.126006777010342,
+            "y": 6.101041958255997,
+            "heading": -0.47044950446923295,
+            "angularVelocity": 1.4042550109565184,
+            "velocityX": 2.930519838000589,
+            "velocityY": -1.610680804625417,
+            "timestamp": 2.0855450402001656
+        },
+        {
+            "x": 7.2116589199714145,
+            "y": 6.053937431211348,
+            "heading": -0.4144026036835486,
+            "angularVelocity": 1.7776263757548019,
+            "velocityX": 2.7166088817245364,
+            "velocityY": -1.4940032101366643,
+            "timestamp": 2.117074107022144
+        },
+        {
+            "x": 7.290084520639544,
+            "y": 6.01082680132011,
+            "heading": -0.35575031779267857,
+            "angularVelocity": 1.8602607626174494,
+            "velocityX": 2.4874063387585092,
+            "velocityY": -1.3673297130756235,
+            "timestamp": 2.1486031738441227
+        },
+        {
+            "x": 7.3612581622075055,
+            "y": 5.971723406014521,
+            "heading": -0.2979353118382045,
+            "angularVelocity": 1.8337049517168786,
+            "velocityX": 2.2573976568931324,
+            "velocityY": -1.2402331958119186,
+            "timestamp": 2.1801322406661012
+        },
+        {
+            "x": 7.425208576949436,
+            "y": 5.936609973900717,
+            "heading": -0.2429131400794928,
+            "angularVelocity": 1.7451252861171487,
+            "velocityX": 2.028300269811719,
+            "velocityY": -1.1136844712868785,
+            "timestamp": 2.2116613074880798
+        },
+        {
+            "x": 7.481970224994158,
+            "y": 5.90546408447042,
+            "heading": -0.1919554556036592,
+            "angularVelocity": 1.616212898515343,
+            "velocityX": 1.8002958465346786,
+            "velocityY": -0.9878468527519235,
+            "timestamp": 2.2431903743100583
+        },
+        {
+            "x": 7.531573575573894,
+            "y": 5.8782638409384305,
+            "heading": -0.14593987362659777,
+            "angularVelocity": 1.4594653954358323,
+            "velocityX": 1.5732578087328088,
+            "velocityY": -0.8627037294052898,
+            "timestamp": 2.274719441132037
+        },
+        {
+            "x": 7.574043708984396,
+            "y": 5.854989674792864,
+            "heading": -0.10549518757833008,
+            "angularVelocity": 1.2827745989638484,
+            "velocityX": 1.347015236774987,
+            "velocityY": -0.7381812559495933,
+            "timestamp": 2.3062485079540154
+        },
+        {
+            "x": 7.60940100035129,
+            "y": 5.835624772967359,
+            "heading": -0.07108683169331761,
+            "angularVelocity": 1.091321734299697,
+            "velocityX": 1.1214188978865451,
+            "velocityY": -0.614192038566993,
+            "timestamp": 2.337777574775994
+        },
+        {
+            "x": 7.637662176953836,
+            "y": 5.820154874880602,
+            "heading": -0.043071374084194446,
+            "angularVelocity": 0.8885596826352652,
+            "velocityX": 0.8963530941818176,
+            "velocityY": -0.4906551206892414,
+            "timestamp": 2.3693066415979724
+        },
+        {
+            "x": 7.65884126375683,
+            "y": 5.808567782468078,
+            "heading": -0.021732750340195603,
+            "angularVelocity": 0.6767921126395045,
+            "velocityX": 0.6717321169883477,
+            "velocityY": -0.3675050859559891,
+            "timestamp": 2.400835708419951
+        },
+        {
+            "x": 7.672950320288622,
+            "y": 5.8008527701995645,
+            "heading": -0.007307045765720019,
+            "angularVelocity": 0.45753668054709684,
+            "velocityX": 0.44749362902029066,
+            "velocityY": -0.24469523034329732,
+            "timestamp": 2.4323647752419295
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 5.476349034092433e-35,
+            "angularVelocity": 0.23175585268595406,
+            "velocityX": 0.22359303404641942,
+            "velocityY": -0.12219740664444498,
+            "timestamp": 2.463893842063908
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 1.465348958332225e-34,
+            "angularVelocity": 2.9106988947740147e-33,
+            "velocityX": 3.969530612508356e-33,
+            "velocityY": 4.7510565840732025e-33,
+            "timestamp": 2.4954229088858866
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Stage Side To C3.traj b/src/main/deploy/choreo/Stage Side To C3.traj
new file mode 100644
index 00000000..49c665b8
--- /dev/null
+++ b/src/main/deploy/choreo/Stage Side To C3.traj	
@@ -0,0 +1,274 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": -1.04,
+            "angularVelocity": 0,
+            "velocityX": -1.418990828065155e-30,
+            "velocityY": 1.8798559266210403e-30,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7612279700458028,
+            "y": 4.384337560333447,
+            "heading": -0.9750502376080931,
+            "angularVelocity": 0.7696859194569483,
+            "velocityX": 0.6070760811828038,
+            "velocityY": 0.28841179417367857,
+            "timestamp": 0.0843847610434429
+        },
+        {
+            "x": 0.8642667385584091,
+            "y": 4.433290258850185,
+            "heading": -0.8542284973914237,
+            "angularVelocity": 1.4317957261788106,
+            "velocityX": 1.2210589594435701,
+            "velocityY": 0.5801130193585202,
+            "timestamp": 0.1687695220868858
+        },
+        {
+            "x": 1.0199495140663002,
+            "y": 4.507092326164681,
+            "heading": -0.6915219404263555,
+            "angularVelocity": 1.9281509475538106,
+            "velocityX": 1.8449157594667183,
+            "velocityY": 0.8745899899690757,
+            "timestamp": 0.25315428313032873
+        },
+        {
+            "x": 1.2292345273519185,
+            "y": 4.606077015104397,
+            "heading": -0.5137189126301165,
+            "angularVelocity": 2.1070513869757743,
+            "velocityX": 2.480128055086372,
+            "velocityY": 1.173016166850306,
+            "timestamp": 0.3375390441737716
+        },
+        {
+            "x": 1.485641598676529,
+            "y": 4.729632973105035,
+            "heading": -0.42871970592241276,
+            "angularVelocity": 1.007281476615537,
+            "velocityX": 3.038547104406527,
+            "velocityY": 1.4641975218379706,
+            "timestamp": 0.4219238052172145
+        },
+        {
+            "x": 1.7745745246623985,
+            "y": 4.865356167675212,
+            "heading": -0.42871965586223265,
+            "angularVelocity": 5.932372081830595e-7,
+            "velocityX": 3.4239941242128182,
+            "velocityY": 1.6083851265555307,
+            "timestamp": 0.5063085662606575
+        },
+        {
+            "x": 2.063508085893233,
+            "y": 5.001078009918069,
+            "heading": -0.4287196058112515,
+            "angularVelocity": 5.931281972756146e-7,
+            "velocityX": 3.4240016521710777,
+            "velocityY": 1.6083691008265977,
+            "timestamp": 0.5906933273041004
+        },
+        {
+            "x": 2.3524294773436862,
+            "y": 5.136825756955775,
+            "heading": -0.4287195557601797,
+            "angularVelocity": 5.931292703362374e-7,
+            "velocityX": 3.423857434421327,
+            "velocityY": 1.6086760851027126,
+            "timestamp": 0.6750780883475433
+        },
+        {
+            "x": 2.647360543237261,
+            "y": 5.258967835966316,
+            "heading": -0.4287195052637089,
+            "angularVelocity": 5.984074633873495e-7,
+            "velocityX": 3.4950749666965533,
+            "velocityY": 1.447442375852424,
+            "timestamp": 0.7594628493909863
+        },
+        {
+            "x": 2.959809428031792,
+            "y": 5.324379711821066,
+            "heading": -0.42871944828896014,
+            "angularVelocity": 6.751781731100131e-7,
+            "velocityX": 3.702669545200082,
+            "velocityY": 0.7751621862272086,
+            "timestamp": 0.8438476104344292
+        },
+        {
+            "x": 3.2789674137401184,
+            "y": 5.330797896945005,
+            "heading": -0.4287193686010616,
+            "angularVelocity": 9.443399202542765e-7,
+            "velocityX": 3.782175617514861,
+            "velocityY": 0.0760585802995927,
+            "timestamp": 0.9282323714778722
+        },
+        {
+            "x": 3.593793413911528,
+            "y": 5.278000284107237,
+            "heading": -0.42871953037751775,
+            "angularVelocity": -0.000001917128805011847,
+            "velocityX": 3.7308395055989028,
+            "velocityY": -0.625677103128148,
+            "timestamp": 1.0126171325213151
+        },
+        {
+            "x": 3.893396265271842,
+            "y": 5.167813441772834,
+            "heading": -0.42871965571467335,
+            "angularVelocity": -0.0000014853055701792625,
+            "velocityX": 3.550437871193987,
+            "velocityY": -1.3057670718262655,
+            "timestamp": 1.097001893564758
+        },
+        {
+            "x": 4.167411681517661,
+            "y": 5.0040496950416875,
+            "heading": -0.4287197602752934,
+            "angularVelocity": -0.0000012390936318239587,
+            "velocityX": 3.247214459785327,
+            "velocityY": -1.9406791546980178,
+            "timestamp": 1.181386654608201
+        },
+        {
+            "x": 4.4320204493985145,
+            "y": 4.825485382292427,
+            "heading": -0.42871985986303984,
+            "angularVelocity": -0.0000011801626881351292,
+            "velocityX": 3.1357411528921184,
+            "velocityY": -2.1160729797797915,
+            "timestamp": 1.265771415651644
+        },
+        {
+            "x": 4.666313927400726,
+            "y": 4.608668432226026,
+            "heading": -0.4287198793836171,
+            "angularVelocity": -2.3132822841838536e-7,
+            "velocityX": 2.77649038884734,
+            "velocityY": -2.5693851281356843,
+            "timestamp": 1.350156176695087
+        },
+        {
+            "x": 4.936706954544898,
+            "y": 4.438990462685779,
+            "heading": -0.4287197198154701,
+            "angularVelocity": 0.0000018909592816311697,
+            "velocityX": 3.204287406881873,
+            "velocityY": -2.010765539204243,
+            "timestamp": 1.4345409377385299
+        },
+        {
+            "x": 5.233776210787073,
+            "y": 4.322144997944839,
+            "heading": -0.42871957094251306,
+            "angularVelocity": 0.0000017642161351814778,
+            "velocityX": 3.5204135506067917,
+            "velocityY": -1.384674949557486,
+            "timestamp": 1.5189256987819728
+        },
+        {
+            "x": 5.532138329985476,
+            "y": 4.208641466951728,
+            "heading": -0.42871942351676046,
+            "angularVelocity": 0.0000017470660671471722,
+            "velocityX": 3.5357345983929718,
+            "velocityY": -1.3450714274656952,
+            "timestamp": 1.6033104598254158
+        },
+        {
+            "x": 5.845046534221127,
+            "y": 4.145463344127181,
+            "heading": -0.42871936362457447,
+            "angularVelocity": 7.097512069029325e-7,
+            "velocityX": 3.708112701469523,
+            "velocityY": -0.7486911385809435,
+            "timestamp": 1.6876952208688587
+        },
+        {
+            "x": 6.164234411105391,
+            "y": 4.140760073731674,
+            "heading": -0.4287193263126739,
+            "angularVelocity": 4.4216396519651743e-7,
+            "velocityX": 3.7825298423246796,
+            "velocityY": -0.055736016049057806,
+            "timestamp": 1.7720799819123016
+        },
+        {
+            "x": 6.483450051427475,
+            "y": 4.138663317552119,
+            "heading": -0.42871928910258295,
+            "angularVelocity": 4.4095747260310005e-7,
+            "velocityX": 3.782858852414673,
+            "velocityY": -0.024847569082867782,
+            "timestamp": 1.8564647429557446
+        },
+        {
+            "x": 6.802665691714121,
+            "y": 4.136566553568004,
+            "heading": -0.4287192518958351,
+            "angularVelocity": 4.4091785477775863e-7,
+            "velocityX": 3.7828588519947024,
+            "velocityY": -0.024847661569458286,
+            "timestamp": 1.9408495039991875
+        },
+        {
+            "x": 7.0950029322992965,
+            "y": 4.132696093446151,
+            "heading": -0.35666839777179793,
+            "angularVelocity": 0.853837271482519,
+            "velocityX": 3.4643368893901423,
+            "velocityY": -0.04586681379319509,
+            "timestamp": 2.0252342650426303
+        },
+        {
+            "x": 7.329695147990892,
+            "y": 4.13025195968354,
+            "heading": -0.23302792235716066,
+            "angularVelocity": 1.465199093839195,
+            "velocityX": 2.7812156222231823,
+            "velocityY": -0.02896416049909081,
+            "timestamp": 2.109619026086073
+        },
+        {
+            "x": 7.505137224924894,
+            "y": 4.1285705338055685,
+            "heading": -0.12281843303988041,
+            "angularVelocity": 1.3060354494646,
+            "velocityX": 2.0790729838492856,
+            "velocityY": -0.019925705271038107,
+            "timestamp": 2.194003787129516
+        },
+        {
+            "x": 7.621791455543556,
+            "y": 4.127510522704197,
+            "heading": -0.04250049300917318,
+            "angularVelocity": 0.9518062152170125,
+            "velocityX": 1.3824087332380297,
+            "velocityY": -0.012561641322782502,
+            "timestamp": 2.278388548172959
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -1.5179160305718205e-31,
+            "angularVelocity": 0.5036512811515541,
+            "velocityX": 0.6897992449901723,
+            "velocityY": -0.0060499395609808024,
+            "timestamp": 2.362773309216402
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 2.3304076579629275e-30,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 2.447158070259845
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Stage Side To C4.traj b/src/main/deploy/choreo/Stage Side To C4.traj
new file mode 100644
index 00000000..00bde21f
--- /dev/null
+++ b/src/main/deploy/choreo/Stage Side To C4.traj	
@@ -0,0 +1,679 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": -1.04,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.687708804701003e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7140616140612069,
+            "y": 4.355992841184312,
+            "heading": -1.0347167301054676,
+            "angularVelocity": 0.1988304134727778,
+            "velocityX": 0.15285465616518656,
+            "velocityY": -0.15080528916361516,
+            "timestamp": 0.026571739213607736
+        },
+        {
+            "x": 0.7221925258884458,
+            "y": 4.3479754290630535,
+            "heading": -1.0242624006876555,
+            "angularVelocity": 0.39343790535390527,
+            "velocityX": 0.30599848063671325,
+            "velocityY": -0.3017270362623706,
+            "timestamp": 0.05314347842721547
+        },
+        {
+            "x": 0.7344011981721603,
+            "y": 4.335944497876898,
+            "heading": -1.0087613580778494,
+            "angularVelocity": 0.5833657513042195,
+            "velocityX": 0.45946078973491633,
+            "velocityY": -0.4527716868451825,
+            "timestamp": 0.0797152176408232
+        },
+        {
+            "x": 0.7506970627866689,
+            "y": 4.31989642008046,
+            "heading": -0.9883558514269565,
+            "angularVelocity": 0.7679401971717026,
+            "velocityX": 0.6132780576953482,
+            "velocityY": -0.6039528563572185,
+            "timestamp": 0.10628695685443094
+        },
+        {
+            "x": 0.7710907256657638,
+            "y": 4.29982697901967,
+            "heading": -0.963213241169254,
+            "angularVelocity": 0.9462162057057438,
+            "velocityX": 0.7674944690353961,
+            "velocityY": -0.7552927152962536,
+            "timestamp": 0.13285869606803868
+        },
+        {
+            "x": 0.7955941760833964,
+            "y": 4.27573110299105,
+            "heading": -0.9335350310204893,
+            "angularVelocity": 1.1169088297226009,
+            "velocityX": 0.9221620843352277,
+            "velocityY": -0.9068234425649082,
+            "timestamp": 0.1594304352816464
+        },
+        {
+            "x": 0.8242209914920048,
+            "y": 4.247602572989807,
+            "heading": -0.8995682913456332,
+            "angularVelocity": 1.2783032153748224,
+            "velocityX": 1.0773406730541775,
+            "velocityY": -1.0585882156647894,
+            "timestamp": 0.18600217449525414
+        },
+        {
+            "x": 0.856986533327333,
+            "y": 4.215433742997202,
+            "heading": -0.8616204330063739,
+            "angularVelocity": 1.4281285103018635,
+            "velocityX": 1.2330973735640383,
+            "velocityY": -1.210640738794065,
+            "timestamp": 0.21257391370886186
+        },
+        {
+            "x": 0.893908134971617,
+            "y": 4.179215360896215,
+            "heading": -0.8200791656995261,
+            "angularVelocity": 1.5633627506615773,
+            "velocityX": 1.3895063980372095,
+            "velocityY": -1.3630414558050108,
+            "timestamp": 0.2391456529224696
+        },
+        {
+            "x": 0.9350052770321623,
+            "y": 4.138936674970777,
+            "heading": -0.7754414952973147,
+            "angularVelocity": 1.6798926876172218,
+            "velocityX": 1.546648555074586,
+            "velocityY": -1.5158468025612346,
+            "timestamp": 0.2657173921360773
+        },
+        {
+            "x": 0.9802996637151041,
+            "y": 4.094586228803994,
+            "heading": -0.7283605055619151,
+            "angularVelocity": 1.7718444907546276,
+            "velocityX": 1.7046075275248067,
+            "velocityY": -1.669083299751435,
+            "timestamp": 0.29228913134968504
+        },
+        {
+            "x": 1.0298146765069804,
+            "y": 4.046154350498487,
+            "heading": -0.6797316150493184,
+            "angularVelocity": 1.8300981400454652,
+            "velocityX": 1.8634464381059026,
+            "velocityY": -1.8226837888242364,
+            "timestamp": 0.31886087056329276
+        },
+        {
+            "x": 1.0835715343165668,
+            "y": 3.9936405744754793,
+            "heading": -0.6308802721211105,
+            "angularVelocity": 1.8384699072761677,
+            "velocityX": 2.0230839004342207,
+            "velocityY": -1.9763017994740086,
+            "timestamp": 0.3454326097769005
+        },
+        {
+            "x": 1.1415677463510772,
+            "y": 3.937082152581159,
+            "heading": -0.5840793547040092,
+            "angularVelocity": 1.7613042579137594,
+            "velocityX": 2.1826276243449603,
+            "velocityY": -2.12851787531304,
+            "timestamp": 0.3720043489905082
+        },
+        {
+            "x": 1.203636670620202,
+            "y": 3.876777756204273,
+            "heading": -0.5447644235662668,
+            "angularVelocity": 1.4795768851144226,
+            "velocityX": 2.33589994882,
+            "velocityY": -2.2694937614773476,
+            "timestamp": 0.39857608820411594
+        },
+        {
+            "x": 1.2693974629594884,
+            "y": 3.8133907541919347,
+            "heading": -0.5190150912676422,
+            "angularVelocity": 0.969049563960718,
+            "velocityX": 2.4748395959572558,
+            "velocityY": -2.3855044452595306,
+            "timestamp": 0.42514782741772367
+        },
+        {
+            "x": 1.3388335264794184,
+            "y": 3.7468848835898942,
+            "heading": -0.5067181615747146,
+            "angularVelocity": 0.4627822663045784,
+            "velocityX": 2.613154636275026,
+            "velocityY": -2.5028798479243823,
+            "timestamp": 0.4517195666313314
+        },
+        {
+            "x": 1.410439403600934,
+            "y": 3.676338986398213,
+            "heading": -0.5067181155488927,
+            "angularVelocity": 0.0000017321343363764108,
+            "velocityX": 2.6948133332892708,
+            "velocityY": -2.6549220818618244,
+            "timestamp": 0.4782913058449391
+        },
+        {
+            "x": 1.482040293717765,
+            "y": 3.605788027611728,
+            "heading": -0.5067180695571892,
+            "angularVelocity": 0.0000017308503249172533,
+            "velocityX": 2.694625652511417,
+            "velocityY": -2.655112569761896,
+            "timestamp": 0.5048630450585468
+        },
+        {
+            "x": 1.5536411829944505,
+            "y": 3.5352370679725946,
+            "heading": -0.5067180235654862,
+            "angularVelocity": 0.0000017308503100877725,
+            "velocityX": 2.6946256208934063,
+            "velocityY": -2.6551126018504427,
+            "timestamp": 0.5314347842721546
+        },
+        {
+            "x": 1.6252420722709946,
+            "y": 3.4646861083333174,
+            "heading": -0.5067179775737835,
+            "angularVelocity": 0.0000017308502987237163,
+            "velocityX": 2.6946256208880808,
+            "velocityY": -2.6551126018558486,
+            "timestamp": 0.5580065234857624
+        },
+        {
+            "x": 1.6968429615475384,
+            "y": 3.39413514869404,
+            "heading": -0.506717931582081,
+            "angularVelocity": 0.0000017308502871423956,
+            "velocityX": 2.6946256208880808,
+            "velocityY": -2.65511260185585,
+            "timestamp": 0.5845782626993702
+        },
+        {
+            "x": 1.7684438508240825,
+            "y": 3.323584189054763,
+            "heading": -0.5067178855903789,
+            "angularVelocity": 0.0000017308502762001758,
+            "velocityX": 2.694625620888081,
+            "velocityY": -2.65511260185585,
+            "timestamp": 0.611150001912978
+        },
+        {
+            "x": 1.8400447401006266,
+            "y": 3.2530332294154856,
+            "heading": -0.506717839598677,
+            "angularVelocity": 0.000001730850265567339,
+            "velocityX": 2.6946256208880826,
+            "velocityY": -2.6551126018558495,
+            "timestamp": 0.6377217411265858
+        },
+        {
+            "x": 1.9116456293771706,
+            "y": 3.1824822697762087,
+            "heading": -0.5067177936069754,
+            "angularVelocity": 0.0000017308502535085818,
+            "velocityX": 2.694625620888084,
+            "velocityY": -2.65511260185585,
+            "timestamp": 0.6642934803401935
+        },
+        {
+            "x": 1.9832465186537145,
+            "y": 3.1119313101369315,
+            "heading": -0.5067177476152741,
+            "angularVelocity": 0.0000017308502432657092,
+            "velocityX": 2.6946256208880843,
+            "velocityY": -2.65511260185585,
+            "timestamp": 0.6908652195538013
+        },
+        {
+            "x": 2.0548474079302586,
+            "y": 3.041380350497654,
+            "heading": -0.5067177016235731,
+            "angularVelocity": 0.000001730850231529751,
+            "velocityX": 2.694625620888085,
+            "velocityY": -2.6551126018558504,
+            "timestamp": 0.7174369587674091
+        },
+        {
+            "x": 2.1264482972068026,
+            "y": 2.970829390858377,
+            "heading": -0.5067176556318724,
+            "angularVelocity": 0.0000017308502204807436,
+            "velocityX": 2.6946256208880865,
+            "velocityY": -2.6551126018558495,
+            "timestamp": 0.7440086979810169
+        },
+        {
+            "x": 2.1980491864833867,
+            "y": 2.9002784312191405,
+            "heading": -0.5067176096401721,
+            "angularVelocity": 0.0000017308502087562552,
+            "velocityX": 2.694625620889591,
+            "velocityY": -2.6551126018543236,
+            "timestamp": 0.7705804371946247
+        },
+        {
+            "x": 2.2696500759972245,
+            "y": 2.829727471820689,
+            "heading": -0.506717563648472,
+            "angularVelocity": 0.0000017308501985869009,
+            "velocityX": 2.6946256298184115,
+            "velocityY": -2.6551125927926265,
+            "timestamp": 0.7971521764082324
+        },
+        {
+            "x": 2.3412523738653572,
+            "y": 2.7591779417640674,
+            "heading": -0.5067175176565489,
+            "angularVelocity": 0.0000017308585893832803,
+            "velocityX": 2.694678631779742,
+            "velocityY": -2.6550588009870313,
+            "timestamp": 0.8237239156218402
+        },
+        {
+            "x": 2.4158866145337003,
+            "y": 2.6918440324740667,
+            "heading": -0.5067174710515008,
+            "angularVelocity": 0.000001753932919872258,
+            "velocityX": 2.808782672009746,
+            "velocityY": -2.5340422299311864,
+            "timestamp": 0.850295654835448
+        },
+        {
+            "x": 2.4943342208862305,
+            "y": 2.6289942264556885,
+            "heading": -0.5067174224716422,
+            "angularVelocity": 0.000001828252870098291,
+            "velocityX": 2.952294756541798,
+            "velocityY": -2.365287628074865,
+            "timestamp": 0.8768673940490558
+        },
+        {
+            "x": 2.693828949223235,
+            "y": 2.509453130751911,
+            "heading": -0.5067173754009504,
+            "angularVelocity": 7.656472375459629e-7,
+            "velocityX": 3.244961599231293,
+            "velocityY": -1.9444436869204162,
+            "timestamp": 0.9383456957905196
+        },
+        {
+            "x": 2.900420237054028,
+            "y": 2.4026436735913763,
+            "heading": -0.5067173289743672,
+            "angularVelocity": 7.551702291921459e-7,
+            "velocityX": 3.3603935368868174,
+            "velocityY": -1.7373521085488381,
+            "timestamp": 0.9998239975319835
+        },
+        {
+            "x": 3.1070116472391316,
+            "y": 2.295834453089378,
+            "heading": -0.5067172825477892,
+            "angularVelocity": 7.55170142432061e-7,
+            "velocityX": 3.3603955270899863,
+            "velocityY": -1.7373482590844154,
+            "timestamp": 1.0613022992734473
+        },
+        {
+            "x": 3.3136030574258837,
+            "y": 2.1890252325905677,
+            "heading": -0.5067172361212112,
+            "angularVelocity": 7.551701448333524e-7,
+            "velocityX": 3.3603955271167996,
+            "velocityY": -1.737348259032553,
+            "timestamp": 1.1227806010149113
+        },
+        {
+            "x": 3.5201944676126358,
+            "y": 2.0822160120917577,
+            "heading": -0.5067171896946332,
+            "angularVelocity": 7.551701452492318e-7,
+            "velocityX": 3.3603955271167996,
+            "velocityY": -1.737348259032552,
+            "timestamp": 1.1842589027563752
+        },
+        {
+            "x": 3.726785877799388,
+            "y": 1.9754067915929476,
+            "heading": -0.5067171432680552,
+            "angularVelocity": 7.5517014486464e-7,
+            "velocityX": 3.3603955271168005,
+            "velocityY": -1.7373482590325509,
+            "timestamp": 1.2457372044978392
+        },
+        {
+            "x": 3.93337728798614,
+            "y": 1.8685975710941374,
+            "heading": -0.506717096841477,
+            "angularVelocity": 7.551701472516521e-7,
+            "velocityX": 3.360395527116801,
+            "velocityY": -1.7373482590325502,
+            "timestamp": 1.3072155062393032
+        },
+        {
+            "x": 4.139968698173285,
+            "y": 1.761788350596088,
+            "heading": -0.5067170504148988,
+            "angularVelocity": 7.551701467640916e-7,
+            "velocityX": 3.360395527123197,
+            "velocityY": -1.7373482590201774,
+            "timestamp": 1.3686938079807671
+        },
+        {
+            "x": 4.346560137549085,
+            "y": 1.6549791865550314,
+            "heading": -0.5067170039883208,
+            "angularVelocity": 7.55170145558408e-7,
+            "velocityX": 3.36039600190298,
+            "velocityY": -1.737347340696294,
+            "timestamp": 1.430172109722231
+        },
+        {
+            "x": 4.5552180589852185,
+            "y": 1.552265545137979,
+            "heading": -0.5067169574392034,
+            "angularVelocity": 7.571633584219393e-7,
+            "velocityX": 3.3940091955305984,
+            "velocityY": -1.6707299731374539,
+            "timestamp": 1.491650411463695
+        },
+        {
+            "x": 4.775846094749007,
+            "y": 1.478702717632789,
+            "heading": -0.5067169090858323,
+            "angularVelocity": 7.865111701229533e-7,
+            "velocityX": 3.5887138960279197,
+            "velocityY": -1.1965657056459655,
+            "timestamp": 1.553128713205159
+        },
+        {
+            "x": 5.004393851392972,
+            "y": 1.4356429788646896,
+            "heading": -0.5067168550876733,
+            "angularVelocity": 8.783287361409535e-7,
+            "velocityX": 3.717535295706156,
+            "velocityY": -0.7004054690576753,
+            "timestamp": 1.614607014946623
+        },
+        {
+            "x": 5.236664772033691,
+            "y": 1.4238771200180054,
+            "heading": -0.5067167933216484,
+            "angularVelocity": 0.000001004680076164895,
+            "velocityX": 3.7780959145145716,
+            "velocityY": -0.19138230096471237,
+            "timestamp": 1.676085316688087
+        },
+        {
+            "x": 5.366036550697627,
+            "y": 1.4270871399969536,
+            "heading": -0.5067167282581706,
+            "angularVelocity": 0.000001901925327561791,
+            "velocityX": 3.781775437099809,
+            "velocityY": 0.09383479793163368,
+            "timestamp": 1.7102945871261934
+        },
+        {
+            "x": 5.494798697340742,
+            "y": 1.4400359689986706,
+            "heading": -0.5067166730622293,
+            "angularVelocity": 0.0000016134790500794753,
+            "velocityX": 3.76395476998197,
+            "velocityY": 0.37851812786083194,
+            "timestamp": 1.7445038575643
+        },
+        {
+            "x": 5.6222191444596294,
+            "y": 1.4626499710733982,
+            "heading": -0.5067166242756445,
+            "angularVelocity": 0.0000014261217576278581,
+            "velocityX": 3.724734421022641,
+            "velocityY": 0.6610489433161765,
+            "timestamp": 1.7787131280024064
+        },
+        {
+            "x": 5.747573461036382,
+            "y": 1.4948005438980272,
+            "heading": -0.5067165796372217,
+            "angularVelocity": 0.0000013048633380814483,
+            "velocityX": 3.6643376187618593,
+            "velocityY": 0.9398204759378852,
+            "timestamp": 1.812922398440513
+        },
+        {
+            "x": 5.870148986660447,
+            "y": 1.5363048197489622,
+            "heading": -0.5067165375293654,
+            "angularVelocity": 0.0000012308902156872765,
+            "velocityX": 3.583108439738183,
+            "velocityY": 1.2132464480944425,
+            "timestamp": 1.8471316688786195
+        },
+        {
+            "x": 5.9892489774115445,
+            "y": 1.5869265226922225,
+            "heading": -0.5067164966802475,
+            "angularVelocity": 0.0000011940949728764683,
+            "velocityX": 3.481512152285735,
+            "velocityY": 1.4797656394002383,
+            "timestamp": 1.881340939316726
+        },
+        {
+            "x": 6.104355729464833,
+            "y": 1.6460687267313168,
+            "heading": -0.5067164560018657,
+            "angularVelocity": 0.0000011891040447074693,
+            "velocityX": 3.3647824282469623,
+            "velocityY": 1.7288355840881788,
+            "timestamp": 1.9155502097548325
+        },
+        {
+            "x": 6.219456425834801,
+            "y": 1.705222715286976,
+            "heading": -0.5067164153233215,
+            "angularVelocity": 0.0000011891087921450935,
+            "velocityX": 3.3646054094668667,
+            "velocityY": 1.7291800672184492,
+            "timestamp": 1.949759480192939
+        },
+        {
+            "x": 6.334557121741728,
+            "y": 1.764376704743614,
+            "heading": -0.5067163746447773,
+            "angularVelocity": 0.0000011891087898511233,
+            "velocityX": 3.3646053959312843,
+            "velocityY": 1.7291800935557256,
+            "timestamp": 1.9839687506310455
+        },
+        {
+            "x": 6.449657817648618,
+            "y": 1.8235306942003213,
+            "heading": -0.5067163339662332,
+            "angularVelocity": 0.000001189108787218646,
+            "velocityX": 3.3646053959302495,
+            "velocityY": 1.7291800935577397,
+            "timestamp": 2.018178021069152
+        },
+        {
+            "x": 6.564758513555509,
+            "y": 1.8826846836570283,
+            "heading": -0.5067162932876892,
+            "angularVelocity": 0.0000011891087819705439,
+            "velocityX": 3.3646053959302504,
+            "velocityY": 1.7291800935577386,
+            "timestamp": 2.0523872915072587
+        },
+        {
+            "x": 6.679859209462396,
+            "y": 1.9418386731137416,
+            "heading": -0.5067162526091454,
+            "angularVelocity": 0.0000011891087784797324,
+            "velocityX": 3.3646053959301545,
+            "velocityY": 1.7291800935579253,
+            "timestamp": 2.0865965619453655
+        },
+        {
+            "x": 6.7949599053264,
+            "y": 2.0009926626538963,
+            "heading": -0.5067162119306018,
+            "angularVelocity": 0.0000011891087739781964,
+            "velocityX": 3.3646053946765995,
+            "velocityY": 1.7291800959970696,
+            "timestamp": 2.120805832383472
+        },
+        {
+            "x": 6.910060040297267,
+            "y": 2.06014774355027,
+            "heading": -0.5067161712446878,
+            "angularVelocity": 0.0000011893242211155864,
+            "velocityX": 3.3645889987368625,
+            "velocityY": 1.7292119983499907,
+            "timestamp": 2.155015102821579
+        },
+        {
+            "x": 7.017868923227757,
+            "y": 2.1174564154331827,
+            "heading": -0.4884934235175169,
+            "angularVelocity": 0.5326844885552473,
+            "velocityX": 3.151452268634164,
+            "velocityY": 1.6752380611740627,
+            "timestamp": 2.1892243732596857
+        },
+        {
+            "x": 7.119036711180674,
+            "y": 2.1700871649460596,
+            "heading": -0.45052101262699384,
+            "angularVelocity": 1.1100035284068692,
+            "velocityX": 2.9573208272872957,
+            "velocityY": 1.5384937719762113,
+            "timestamp": 2.2234336436977924
+        },
+        {
+            "x": 7.212945348428852,
+            "y": 2.2182419084801723,
+            "heading": -0.3958429177265348,
+            "angularVelocity": 1.5983414495607555,
+            "velocityX": 2.745122478366898,
+            "velocityY": 1.4076518708938115,
+            "timestamp": 2.257642914135899
+        },
+        {
+            "x": 7.29827375889861,
+            "y": 2.2619654936466005,
+            "heading": -0.3361817714679226,
+            "angularVelocity": 1.7440052212324946,
+            "velocityX": 2.494306641941948,
+            "velocityY": 1.2781209481077944,
+            "timestamp": 2.291852184574006
+        },
+        {
+            "x": 7.374929402835313,
+            "y": 2.3012175879094494,
+            "heading": -0.2767819040095215,
+            "angularVelocity": 1.7363675605380382,
+            "velocityX": 2.2407856979994336,
+            "velocityY": 1.1474110309913224,
+            "timestamp": 2.3260614550121126
+        },
+        {
+            "x": 7.442942536538503,
+            "y": 2.336017519822898,
+            "heading": -0.22036844778019443,
+            "angularVelocity": 1.6490692583284896,
+            "velocityX": 1.9881492014348112,
+            "velocityY": 1.0172661231232827,
+            "timestamp": 2.3602707254502193
+        },
+        {
+            "x": 7.502356073864926,
+            "y": 2.3663915967666074,
+            "heading": -0.16856655782281169,
+            "angularVelocity": 1.5142646801283255,
+            "velocityX": 1.7367671559648723,
+            "velocityY": 0.8878902284298598,
+            "timestamp": 2.394479995888326
+        },
+        {
+            "x": 7.553208169092694,
+            "y": 2.392365594084153,
+            "heading": -0.12246425110299042,
+            "angularVelocity": 1.3476553615264124,
+            "velocityX": 1.4865004303372251,
+            "velocityY": 0.7592677945160818,
+            "timestamp": 2.428689266326433
+        },
+        {
+            "x": 7.595530700913324,
+            "y": 2.4139635888667987,
+            "heading": -0.08285639859822795,
+            "angularVelocity": 1.1578104998299616,
+            "velocityX": 1.2371655775940182,
+            "velocityY": 0.6313491783381474,
+            "timestamp": 2.4628985367645395
+        },
+        {
+            "x": 7.629350339345417,
+            "y": 2.431207580669044,
+            "heading": -0.050358801794747596,
+            "angularVelocity": 0.9499646261757312,
+            "velocityX": 0.988610338629752,
+            "velocityY": 0.5040736496688569,
+            "timestamp": 2.4971078072026462
+        },
+        {
+            "x": 7.654689602970606,
+            "y": 2.444117248020803,
+            "heading": -0.025464280349729525,
+            "angularVelocity": 0.7277127260009457,
+            "velocityX": 0.7407133592934628,
+            "velocityY": 0.3773733606834889,
+            "timestamp": 2.531317077640753
+        },
+        {
+            "x": 7.671567523131761,
+            "y": 2.4527098140869614,
+            "heading": -0.008571412442380029,
+            "angularVelocity": 0.49380965133159116,
+            "velocityX": 0.4933727011714593,
+            "velocityY": 0.2511765365386793,
+            "timestamp": 2.5655263480788597
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 5.0379367595035626e-36,
+            "angularVelocity": 0.2505581771434718,
+            "velocityX": 0.24649683434481964,
+            "velocityY": 0.12541003821758284,
+            "timestamp": 2.5997356185169664
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 2.854280403338002e-36,
+            "angularVelocity": -6.383230494440131e-35,
+            "velocityX": -1.2082929677962258e-35,
+            "velocityY": 1.8766665143888344e-34,
+            "timestamp": 2.633944888955073
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Stage Side To C5.traj b/src/main/deploy/choreo/Stage Side To C5.traj
new file mode 100644
index 00000000..818f5ad4
--- /dev/null
+++ b/src/main/deploy/choreo/Stage Side To C5.traj	
@@ -0,0 +1,337 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": -1.04,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 1.4815405069278332e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7317173879428727,
+            "y": 4.344479531295635,
+            "heading": -1.013582317544266,
+            "angularVelocity": 0.4585462516411977,
+            "velocityX": 0.37696065327942924,
+            "velocityY": -0.26939731598432753,
+            "timestamp": 0.05761181638969176
+        },
+        {
+            "x": 0.7752730705207751,
+            "y": 4.313424298338961,
+            "heading": -0.9621480921974644,
+            "angularVelocity": 0.892772152832256,
+            "velocityX": 0.7560199505477795,
+            "velocityY": -0.5390427676609577,
+            "timestamp": 0.11522363277938352
+        },
+        {
+            "x": 0.8408250898493527,
+            "y": 4.266812487548848,
+            "heading": -0.8877439224939486,
+            "angularVelocity": 1.2914741170498636,
+            "velocityX": 1.1378224717856051,
+            "velocityY": -0.8090668496689294,
+            "timestamp": 0.17283544916907528
+        },
+        {
+            "x": 0.9285847241631874,
+            "y": 4.204600540366139,
+            "heading": -0.7936992773737622,
+            "angularVelocity": 1.6323846567179892,
+            "velocityX": 1.5232922656043415,
+            "velocityY": -1.0798470015578314,
+            "timestamp": 0.23044726555876704
+        },
+        {
+            "x": 1.0388213735909644,
+            "y": 4.1267152282410935,
+            "heading": -0.6856850144743787,
+            "angularVelocity": 1.8748629997840256,
+            "velocityX": 1.9134381857035365,
+            "velocityY": -1.3518982216811457,
+            "timestamp": 0.2880590819484588
+        },
+        {
+            "x": 1.1718232312823271,
+            "y": 4.033123229014291,
+            "heading": -0.5741627817643435,
+            "angularVelocity": 1.935752762171023,
+            "velocityX": 2.3085864328894954,
+            "velocityY": -1.6245278328622093,
+            "timestamp": 0.34567089833815057
+        },
+        {
+            "x": 1.3270687303513053,
+            "y": 3.9246703032584405,
+            "heading": -0.488682735559766,
+            "angularVelocity": 1.4837242003685942,
+            "velocityX": 2.6946815566251776,
+            "velocityY": -1.882477112373353,
+            "timestamp": 0.40328271472784233
+        },
+        {
+            "x": 1.5010681185276094,
+            "y": 3.804139759424889,
+            "heading": -0.46956462349945394,
+            "angularVelocity": 0.3318435914430377,
+            "velocityX": 3.020203129846763,
+            "velocityY": -2.0921149754812625,
+            "timestamp": 0.4608945311175341
+        },
+        {
+            "x": 1.6785494425409546,
+            "y": 3.677652137140934,
+            "heading": -0.4695645649556455,
+            "angularVelocity": 0.0000010161770978802793,
+            "velocityX": 3.080641006227699,
+            "velocityY": -2.195515264236428,
+            "timestamp": 0.5185063475072258
+        },
+        {
+            "x": 1.85603075289391,
+            "y": 3.551164495689284,
+            "heading": -0.4695645064119953,
+            "angularVelocity": 0.000001016174352223477,
+            "velocityX": 3.080640769116793,
+            "velocityY": -2.1955155969406546,
+            "timestamp": 0.5761181638969175
+        },
+        {
+            "x": 2.033512086622271,
+            "y": 3.4246768870368545,
+            "heading": -0.4695644478683731,
+            "angularVelocity": 0.0000010161738659585955,
+            "velocityX": 3.0806411748565776,
+            "velocityY": -2.195515027626543,
+            "timestamp": 0.6337299802866092
+        },
+        {
+            "x": 2.2109934521039283,
+            "y": 3.298189322939119,
+            "heading": -0.46956438932476746,
+            "angularVelocity": 0.0000010161735782211743,
+            "velocityX": 3.080641726016013,
+            "velocityY": -2.1955142542661994,
+            "timestamp": 0.6913417966763009
+        },
+        {
+            "x": 2.3884748618363862,
+            "y": 3.17170182093203,
+            "heading": -0.4695643307811488,
+            "angularVelocity": 0.000001016173805422874,
+            "velocityX": 3.0806424941015065,
+            "velocityY": -2.1955131765246763,
+            "timestamp": 0.7489536130659926
+        },
+        {
+            "x": 2.5659563234551954,
+            "y": 3.045214391729511,
+            "heading": -0.4695642722374779,
+            "angularVelocity": 0.0000010161747112988211,
+            "velocityX": 3.0806433947214558,
+            "velocityY": -2.1955119128156926,
+            "timestamp": 0.8065654294556843
+        },
+        {
+            "x": 2.743437831089148,
+            "y": 2.918727027093402,
+            "heading": -0.46956421369376844,
+            "angularVelocity": 0.000001016175381373278,
+            "velocityX": 3.08064419343162,
+            "velocityY": -2.1955107921009867,
+            "timestamp": 0.864177245845376
+        },
+        {
+            "x": 2.920919379832659,
+            "y": 2.7922397201404365,
+            "heading": -0.4695641551500532,
+            "angularVelocity": 0.0000010161754817816058,
+            "velocityX": 3.080644906992851,
+            "velocityY": -2.195509790862922,
+            "timestamp": 0.9217890622350677
+        },
+        {
+            "x": 3.0984009676669872,
+            "y": 2.6657524680380327,
+            "heading": -0.4695640966063568,
+            "angularVelocity": 0.000001016175153333812,
+            "velocityX": 3.080645585513673,
+            "velocityY": -2.195508838791537,
+            "timestamp": 0.9794008786247594
+        },
+        {
+            "x": 3.275882647398041,
+            "y": 2.5392653448813247,
+            "heading": -0.4695640380626867,
+            "angularVelocity": 0.0000010161746978103879,
+            "velocityX": 3.080647180615714,
+            "velocityY": -2.1955066006101345,
+            "timestamp": 1.0370126950144511
+        },
+        {
+            "x": 3.4563703536987305,
+            "y": 2.417105913162231,
+            "heading": -0.4695639788949644,
+            "angularVelocity": 0.0000010270067149252654,
+            "velocityX": 3.1328244379564545,
+            "velocityY": -2.1203884788634437,
+            "timestamp": 1.0946245114041429
+        },
+        {
+            "x": 3.7578524899325245,
+            "y": 2.288322056940197,
+            "heading": -0.4695639274563231,
+            "angularVelocity": 5.9355601779034e-7,
+            "velocityX": 3.4788348155609183,
+            "velocityY": -1.4860507766867785,
+            "timestamp": 1.1812863255510104
+        },
+        {
+            "x": 4.061243078218464,
+            "y": 2.1641008385808727,
+            "heading": -0.46956387627292245,
+            "angularVelocity": 5.906107685245515e-7,
+            "velocityX": 3.5008566491785738,
+            "velocityY": -1.43340200735705,
+            "timestamp": 1.267948139697878
+        },
+        {
+            "x": 4.364633703659485,
+            "y": 2.039879710966971,
+            "heading": -0.46956382508952316,
+            "angularVelocity": 5.906107531304183e-7,
+            "velocityX": 3.5008570779150574,
+            "velocityY": -1.433400960235855,
+            "timestamp": 1.3546099538447456
+        },
+        {
+            "x": 4.668024366282006,
+            "y": 1.9156586741630988,
+            "heading": -0.46956377390610504,
+            "angularVelocity": 5.906109701114576e-7,
+            "velocityX": 3.5008575069563967,
+            "velocityY": -1.4333999123691519,
+            "timestamp": 1.4412717679916132
+        },
+        {
+            "x": 4.971415087165123,
+            "y": 1.7914377796518506,
+            "heading": -0.4695637227226054,
+            "angularVelocity": 5.906119107964098e-7,
+            "velocityX": 3.5008581792316793,
+            "velocityY": -1.4333982704392505,
+            "timestamp": 1.5279335821384807
+        },
+        {
+            "x": 5.2748058711699946,
+            "y": 1.6672170393060612,
+            "heading": -0.4695636715390663,
+            "angularVelocity": 5.906123657897232e-7,
+            "velocityX": 3.5008589076003997,
+            "velocityY": -1.4333964915074275,
+            "timestamp": 1.6145953962853483
+        },
+        {
+            "x": 5.5781967938554144,
+            "y": 1.5429966376674569,
+            "heading": -0.4695636203556594,
+            "angularVelocity": 5.906108405690599e-7,
+            "velocityX": 3.5008605078501667,
+            "velocityY": -1.4333925831287677,
+            "timestamp": 1.701257210432216
+        },
+        {
+            "x": 5.886988948403778,
+            "y": 1.4328868446307816,
+            "heading": -0.4695635793301605,
+            "angularVelocity": 4.733976467078746e-7,
+            "velocityX": 3.5631859035981908,
+            "velocityY": -1.2705687518849984,
+            "timestamp": 1.7879190245790835
+        },
+        {
+            "x": 6.195781169913944,
+            "y": 1.3227772393821355,
+            "heading": -0.4695635383046595,
+            "angularVelocity": 4.7339767079441426e-7,
+            "velocityX": 3.5631866762776085,
+            "velocityY": -1.2705665849787193,
+            "timestamp": 1.874580838725951
+        },
+        {
+            "x": 6.5045734104803365,
+            "y": 1.212667687574965,
+            "heading": -0.46956349727913566,
+            "angularVelocity": 4.733979354519185e-7,
+            "velocityX": 3.5631868961694693,
+            "velocityY": -1.270565968311784,
+            "timestamp": 1.9612426528728186
+        },
+        {
+            "x": 6.813365656566466,
+            "y": 1.1025581512474965,
+            "heading": -0.46956345625352064,
+            "angularVelocity": 4.733989867793667e-7,
+            "velocityX": 3.563186959862305,
+            "velocityY": -1.270565789689833,
+            "timestamp": 2.0479044670196864
+        },
+        {
+            "x": 7.101065618447083,
+            "y": 0.9964425714529646,
+            "heading": -0.4012732100130825,
+            "angularVelocity": 0.7880085007764233,
+            "velocityX": 3.3198008224596522,
+            "velocityY": -1.2244790954259896,
+            "timestamp": 2.134566281166554
+        },
+        {
+            "x": 7.333266238045498,
+            "y": 0.911992130350644,
+            "heading": -0.2613611242014092,
+            "angularVelocity": 1.6144606155435655,
+            "velocityX": 2.6793879390165927,
+            "velocityY": -0.9744827284506232,
+            "timestamp": 2.2212280953134216
+        },
+        {
+            "x": 7.506890250099573,
+            "y": 0.8492417427348896,
+            "heading": -0.1371754084070899,
+            "angularVelocity": 1.43299233943868,
+            "velocityX": 2.0034661605379225,
+            "velocityY": -0.7240834759056616,
+            "timestamp": 2.307889909460289
+        },
+        {
+            "x": 7.622366585478705,
+            "y": 0.8076799304753898,
+            "heading": -0.04732610702177306,
+            "angularVelocity": 1.0367807582823931,
+            "velocityX": 1.332493861522807,
+            "velocityY": -0.4795862245517285,
+            "timestamp": 2.3945517236071567
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 2.9489608951695904e-31,
+            "angularVelocity": 0.5461010421679925,
+            "velocityX": 0.6650381726791765,
+            "velocityY": -0.2386279433331854,
+            "timestamp": 2.4812135377540243
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -7.162012659830636e-32,
+            "angularVelocity": -4.0878892657023166e-30,
+            "velocityX": 2.3668785600931567e-32,
+            "velocityY": 6.81692291678628e-32,
+            "timestamp": 2.567875351900892
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Stage Side To W1.traj b/src/main/deploy/choreo/Stage Side To W1.traj
new file mode 100644
index 00000000..9649eb8c
--- /dev/null
+++ b/src/main/deploy/choreo/Stage Side To W1.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": -1.04,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7269596910839872,
+            "y": 4.384958038291164,
+            "heading": -1.001571192673364,
+            "angularVelocity": 0.6206074004453302,
+            "velocityX": 0.27389113865872694,
+            "velocityY": 0.40306073338263126,
+            "timestamp": 0.06192128437247234
+        },
+        {
+            "x": 0.760846036232504,
+            "y": 4.4351182711466235,
+            "heading": -0.9264796681168371,
+            "angularVelocity": 1.2126932656117428,
+            "velocityX": 0.5472487447883301,
+            "velocityY": 0.8100644772439347,
+            "timestamp": 0.12384256874494468
+        },
+        {
+            "x": 0.8116293344011996,
+            "y": 4.510831947021006,
+            "heading": -0.817810113065978,
+            "angularVelocity": 1.7549628718484593,
+            "velocityX": 0.8201266928382989,
+            "velocityY": 1.2227407206049843,
+            "timestamp": 0.185763853117417
+        },
+        {
+            "x": 0.8793448344180163,
+            "y": 4.612606899775036,
+            "heading": -0.6817312187907495,
+            "angularVelocity": 2.197610977457753,
+            "velocityX": 1.0935738930977383,
+            "velocityY": 1.6436182450904597,
+            "timestamp": 0.24768513748988935
+        },
+        {
+            "x": 0.9640984700059907,
+            "y": 4.74109575406918,
+            "heading": -0.5303096586797065,
+            "angularVelocity": 2.4453879089491037,
+            "velocityX": 1.3687318738119136,
+            "velocityY": 2.075035354907217,
+            "timestamp": 0.3096064218623617
+        },
+        {
+            "x": 1.0654137163163762,
+            "y": 4.896663616218439,
+            "heading": -0.3925803264267699,
+            "angularVelocity": 2.224264784697608,
+            "velocityX": 1.6361941994120859,
+            "velocityY": 2.5123487622362273,
+            "timestamp": 0.37152770623483405
+        },
+        {
+            "x": 1.179096696888173,
+            "y": 5.075186082347611,
+            "heading": -0.3289347906925715,
+            "angularVelocity": 1.0278458591290565,
+            "velocityX": 1.8359273668802587,
+            "velocityY": 2.88305496144612,
+            "timestamp": 0.4334489906073064
+        },
+        {
+            "x": 1.3095067112863388,
+            "y": 5.269772108097061,
+            "heading": -0.32893471990297235,
+            "angularVelocity": 0.0000011432191669124052,
+            "velocityX": 2.1060611988232667,
+            "velocityY": 3.142473992932148,
+            "timestamp": 0.49537027497977876
+        },
+        {
+            "x": 1.4399171936865056,
+            "y": 5.464357820195416,
+            "heading": -0.32893464911575754,
+            "angularVelocity": 0.0000011431806608935866,
+            "velocityX": 2.106068756838354,
+            "velocityY": 3.1424689276125553,
+            "timestamp": 0.557291559352251
+        },
+        {
+            "x": 1.570327676092705,
+            "y": 5.6589435322897295,
+            "heading": -0.3289345783285442,
+            "angularVelocity": 0.0000011431806365404781,
+            "velocityX": 2.106068756935774,
+            "velocityY": 3.1424689275472675,
+            "timestamp": 0.6192128437247234
+        },
+        {
+            "x": 1.7007381584918697,
+            "y": 5.853529244388756,
+            "heading": -0.32893450754133247,
+            "angularVelocity": 0.000001143180612539941,
+            "velocityX": 2.1060687568221708,
+            "velocityY": 3.1424689276234057,
+            "timestamp": 0.6811341280971958
+        },
+        {
+            "x": 1.8311480951447636,
+            "y": 6.048115322242248,
+            "heading": -0.3289344367512267,
+            "angularVelocity": 0.0000011432273482693713,
+            "velocityX": 2.1060599432732174,
+            "velocityY": 3.1424748343882203,
+            "timestamp": 0.7430554124696681
+        },
+        {
+            "x": 1.9446381116074218,
+            "y": 6.226114434087632,
+            "heading": -0.2658064774598036,
+            "angularVelocity": 1.0194872398268149,
+            "velocityX": 1.8328110860877296,
+            "velocityY": 2.8746030326934724,
+            "timestamp": 0.8049766968421405
+        },
+        {
+            "x": 2.0457809471284243,
+            "y": 6.380047389358405,
+            "heading": -0.1375002927378408,
+            "angularVelocity": 2.0720853261080365,
+            "velocityX": 1.6334098451931771,
+            "velocityY": 2.485945775039604,
+            "timestamp": 0.8668979812146128
+        },
+        {
+            "x": 2.1306775060879315,
+            "y": 6.50749359169558,
+            "heading": 0.01763716177237307,
+            "angularVelocity": 2.5053978786522335,
+            "velocityX": 1.3710400199200075,
+            "velocityY": 2.0581970097802293,
+            "timestamp": 0.9288192655870852
+        },
+        {
+            "x": 2.198423978678441,
+            "y": 6.608249711273454,
+            "heading": 0.15869859646801654,
+            "angularVelocity": 2.278076692452355,
+            "velocityX": 1.0940740857860296,
+            "velocityY": 1.6271645622174213,
+            "timestamp": 0.9907405499595575
+        },
+        {
+            "x": 2.24920191473134,
+            "y": 6.683037424747583,
+            "heading": 0.27235604329703667,
+            "angularVelocity": 1.8355150087866652,
+            "velocityX": 0.8200400971571714,
+            "velocityY": 1.2077868576540292,
+            "timestamp": 1.0526618343320298
+        },
+        {
+            "x": 2.2830642302273354,
+            "y": 6.732456156660043,
+            "heading": 0.35192243545091534,
+            "angularVelocity": 1.2849603001653922,
+            "velocityX": 0.5468606770541943,
+            "velocityY": 0.7980895812043076,
+            "timestamp": 1.114583118704502
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0.6633836000880191,
+            "velocityX": 0.2735048205846554,
+            "velocityY": 0.3963716771816175,
+            "timestamp": 1.1765044030769745
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0,
+            "velocityX": 2.8187230537991962e-33,
+            "velocityY": 0,
+            "timestamp": 1.2384256874494468
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Stage Side To W2.traj b/src/main/deploy/choreo/Stage Side To W2.traj
new file mode 100644
index 00000000..f9120604
--- /dev/null
+++ b/src/main/deploy/choreo/Stage Side To W2.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": -1.04,
+            "angularVelocity": 2.3377521162047767e-32,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.737441410859622,
+            "y": 4.3811882786622816,
+            "heading": -1.0071421705955907,
+            "angularVelocity": 0.501398563530153,
+            "velocityX": 0.41874598035403643,
+            "velocityY": 0.32332545020514236,
+            "timestamp": 0.06553235648117978
+        },
+        {
+            "x": 0.7924436512054522,
+            "y": 4.423719236654154,
+            "heading": -0.9443396668116766,
+            "angularVelocity": 0.9583434375956188,
+            "velocityX": 0.8393142456524665,
+            "velocityY": 0.6490069986127681,
+            "timestamp": 0.13106471296235955
+        },
+        {
+            "x": 0.8751754235694627,
+            "y": 4.487775832809834,
+            "heading": -0.8558630826468019,
+            "angularVelocity": 1.350120595621899,
+            "velocityX": 1.2624568504227318,
+            "velocityY": 0.9774804324956131,
+            "timestamp": 0.19659706944353933
+        },
+        {
+            "x": 0.9858651777004729,
+            "y": 4.5735665588886505,
+            "heading": -0.748264717894899,
+            "angularVelocity": 1.641912034443748,
+            "velocityX": 1.6890855155315267,
+            "velocityY": 1.3091353750334698,
+            "timestamp": 0.2621294259247191
+        },
+        {
+            "x": 1.1247487447497322,
+            "y": 4.6813201098081265,
+            "heading": -0.6327126866941822,
+            "angularVelocity": 1.7632821007116781,
+            "velocityX": 2.1193128785036808,
+            "velocityY": 1.644280119095407,
+            "timestamp": 0.3276617824058989
+        },
+        {
+            "x": 1.291489149115039,
+            "y": 4.810947116825512,
+            "heading": -0.5360472598717524,
+            "angularVelocity": 1.4750793655678647,
+            "velocityX": 2.5443981159626317,
+            "velocityY": 1.9780611285451524,
+            "timestamp": 0.39319413888707866
+        },
+        {
+            "x": 1.4799949106081978,
+            "y": 4.9607414161097925,
+            "heading": -0.5171264547525753,
+            "angularVelocity": 0.288724626049585,
+            "velocityX": 2.876529574322522,
+            "velocityY": 2.285806696533185,
+            "timestamp": 0.45872649536825844
+        },
+        {
+            "x": 1.6682619137790426,
+            "y": 5.11061369240764,
+            "heading": -0.49798057816746544,
+            "angularVelocity": 0.29215913501614493,
+            "velocityX": 2.8728862088900566,
+            "velocityY": 2.2869965974882214,
+            "timestamp": 0.5242588518494382
+        },
+        {
+            "x": 1.8349963336194852,
+            "y": 5.239757443274396,
+            "heading": -0.40436368242247644,
+            "angularVelocity": 1.4285598866244509,
+            "velocityX": 2.54430679428301,
+            "velocityY": 1.970686814899508,
+            "timestamp": 0.589791208330618
+        },
+        {
+            "x": 1.9739513715778099,
+            "y": 5.347175023297594,
+            "heading": -0.28864983484418905,
+            "angularVelocity": 1.7657513599639834,
+            "velocityX": 2.120403498662996,
+            "velocityY": 1.6391533250303247,
+            "timestamp": 0.6553235648117979
+        },
+        {
+            "x": 2.0846889840994356,
+            "y": 5.432668431655269,
+            "heading": -0.18116230899252247,
+            "angularVelocity": 1.6402206730126385,
+            "velocityX": 1.6898158172204303,
+            "velocityY": 1.304598414406612,
+            "timestamp": 0.7208559212929777
+        },
+        {
+            "x": 2.167475833751633,
+            "y": 5.496492915332372,
+            "heading": -0.09361421148490373,
+            "angularVelocity": 1.3359522258711838,
+            "velocityX": 1.2632973098712375,
+            "velocityY": 0.973938480228949,
+            "timestamp": 0.7863882777741575
+        },
+        {
+            "x": 2.2225290058379836,
+            "y": 5.538875443360028,
+            "heading": -0.032000086006940494,
+            "angularVelocity": 0.9402092155141033,
+            "velocityX": 0.8400914455466154,
+            "velocityY": 0.6467420111747162,
+            "timestamp": 0.8519206342553374
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 2.4308347188047162e-30,
+            "angularVelocity": 0.48830971027463277,
+            "velocityX": 0.41919741082263007,
+            "velocityY": 0.3223530752482305,
+            "timestamp": 0.9174529907365172
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 1.7485008040489027e-29,
+            "angularVelocity": 1.1123032826758857e-30,
+            "velocityX": 0,
+            "velocityY": 1.931278949788811e-31,
+            "timestamp": 0.982985347217697
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Stage Side To W3.traj b/src/main/deploy/choreo/Stage Side To W3.traj
new file mode 100644
index 00000000..ff6d2947
--- /dev/null
+++ b/src/main/deploy/choreo/Stage Side To W3.traj	
@@ -0,0 +1,121 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": -1.04,
+            "angularVelocity": -2.204776300650663e-32,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7541176231747116,
+            "y": 4.360036687345491,
+            "heading": -1.0162545684059647,
+            "angularVelocity": 0.32461244675544165,
+            "velocityX": 0.6031109414489596,
+            "velocityY": 0.0005015351663374791,
+            "timestamp": 0.07315009584923313
+        },
+        {
+            "x": 0.8424088865359771,
+            "y": 4.360100324606402,
+            "heading": -0.9701479863262087,
+            "angularVelocity": 0.630301048063541,
+            "velocityX": 1.2069876646948012,
+            "velocityY": 0.0008699545799604022,
+            "timestamp": 0.14630019169846625
+        },
+        {
+            "x": 0.974955221752511,
+            "y": 4.3601789275184695,
+            "heading": -0.9038423136459668,
+            "angularVelocity": 0.9064331619859869,
+            "velocityX": 1.8119776013652948,
+            "velocityY": 0.0010745428444254071,
+            "timestamp": 0.21945028754769938
+        },
+        {
+            "x": 1.151884261990258,
+            "y": 4.360259672825357,
+            "heading": -0.8212724833638784,
+            "angularVelocity": 1.128772687492223,
+            "velocityX": 2.4187123500481573,
+            "velocityY": 0.0011038305000959348,
+            "timestamp": 0.2926003833969325
+        },
+        {
+            "x": 1.3733948258568207,
+            "y": 4.360324542882412,
+            "heading": -0.7323749312717828,
+            "angularVelocity": 1.215275948172809,
+            "velocityX": 3.028165052894887,
+            "velocityY": 0.0008868075468144582,
+            "timestamp": 0.36575047924616566
+        },
+        {
+            "x": 1.6366160230983884,
+            "y": 4.359789775146019,
+            "heading": -0.6992109641912426,
+            "angularVelocity": 0.4533687440268879,
+            "velocityX": 3.5983711871554758,
+            "velocityY": -0.007310554144235059,
+            "timestamp": 0.4389005750953988
+        },
+        {
+            "x": 1.8581232786849116,
+            "y": 4.3598960912826,
+            "heading": -0.6098314600860953,
+            "angularVelocity": 1.2218644838062247,
+            "velocityX": 3.0281198269795637,
+            "velocityY": 0.001453397091854858,
+            "timestamp": 0.512050670944632
+        },
+        {
+            "x": 2.035050245808528,
+            "y": 4.359956526837273,
+            "heading": -0.5268891037054299,
+            "angularVelocity": 1.1338653137464598,
+            "velocityX": 2.4186840094956525,
+            "velocityY": 0.000826185584123381,
+            "timestamp": 0.5852007667938651
+        },
+        {
+            "x": 2.1675937895751547,
+            "y": 4.359986275638159,
+            "heading": -0.46022423203591045,
+            "angularVelocity": 0.9113435996982586,
+            "velocityX": 1.8119394407877087,
+            "velocityY": 0.00040668163915013244,
+            "timestamp": 0.6583508626430983
+        },
+        {
+            "x": 2.255882919836075,
+            "y": 4.359997704013322,
+            "heading": -0.41385948514831483,
+            "angularVelocity": 0.6338302957681125,
+            "velocityX": 1.2069585040994306,
+            "velocityY": 0.0001562318549097459,
+            "timestamp": 0.7315009584923314
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 0.32617161838698483,
+            "velocityX": 0.6031035182080025,
+            "velocityY": 0.00003138733655167869,
+            "timestamp": 0.8046510543415646
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -2.2755735462869925e-32,
+            "velocityX": -9.780003655586715e-35,
+            "velocityY": 0,
+            "timestamp": 0.8778011501907977
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W1 To C1.traj b/src/main/deploy/choreo/W1 To C1.traj
new file mode 100644
index 00000000..b6c78aaa
--- /dev/null
+++ b/src/main/deploy/choreo/W1 To C1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 6.154725095706714e-32,
+            "velocityX": 1.2115062621827067e-33,
+            "velocityY": 6.540799795634554e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3666999539229265,
+            "y": 6.765820977804648,
+            "heading": 0.36628259128993745,
+            "angularVelocity": -0.26339586361920003,
+            "velocityX": 0.7407424724032508,
+            "velocityY": 0.09796218023747214,
+            "timestamp": 0.09004472729439511
+        },
+        {
+            "x": 2.5001846245776895,
+            "y": 6.783480891306791,
+            "heading": 0.3222458902940237,
+            "angularVelocity": -0.4890536327789505,
+            "velocityX": 1.4824262859761226,
+            "velocityY": 0.1961237935054848,
+            "timestamp": 0.18008945458879022
+        },
+        {
+            "x": 2.70056880915549,
+            "y": 6.810010034997764,
+            "heading": 0.26375227569196347,
+            "angularVelocity": -0.6496062163731077,
+            "velocityX": 2.225384990313294,
+            "velocityY": 0.2946218450330504,
+            "timestamp": 0.27013418188318533
+        },
+        {
+            "x": 2.967965472322839,
+            "y": 6.8454603445891635,
+            "heading": 0.20325713210858826,
+            "angularVelocity": -0.6718343805472418,
+            "velocityX": 2.9695982341432847,
+            "velocityY": 0.3936966733820705,
+            "timestamp": 0.36017890917758044
+        },
+        {
+            "x": 3.3011740475809854,
+            "y": 6.889827568786931,
+            "heading": 0.1886164627156386,
+            "angularVelocity": -0.16259330038374148,
+            "velocityX": 3.700478476310374,
+            "velocityY": 0.4927242885939573,
+            "timestamp": 0.45022363647197555
+        },
+        {
+            "x": 3.6388963576772766,
+            "y": 6.934269203744266,
+            "heading": 0.18861644342798295,
+            "angularVelocity": -2.1420083336262225e-7,
+            "velocityX": 3.7506061736644574,
+            "velocityY": 0.4935506641275844,
+            "timestamp": 0.5402683637663707
+        },
+        {
+            "x": 3.9766186774172247,
+            "y": 6.978710765417191,
+            "heading": 0.1886164241403376,
+            "angularVelocity": -2.1420071941558326e-7,
+            "velocityX": 3.750606280762983,
+            "velocityY": 0.4935498502608198,
+            "timestamp": 0.6303130910607657
+        },
+        {
+            "x": 4.314341007224031,
+            "y": 7.023152250589549,
+            "heading": 0.18861640485269274,
+            "angularVelocity": -2.1420071403103001e-7,
+            "velocityX": 3.750606392561398,
+            "velocityY": 0.4935490006767261,
+            "timestamp": 0.7203578183551609
+        },
+        {
+            "x": 4.652063347871557,
+            "y": 7.067593653380398,
+            "heading": 0.1886163855650432,
+            "angularVelocity": -2.1420076555553473e-7,
+            "velocityX": 3.750606512954019,
+            "velocityY": 0.49354808578132814,
+            "timestamp": 0.810402545649556
+        },
+        {
+            "x": 4.989785700043158,
+            "y": 7.112034968596579,
+            "heading": 0.18861636627738598,
+            "angularVelocity": -2.1420085095836123e-7,
+            "velocityX": 3.7506066409356835,
+            "velocityY": 0.4935471132127838,
+            "timestamp": 0.9004472729439512
+        },
+        {
+            "x": 5.327508064058316,
+            "y": 7.15647619381006,
+            "heading": 0.18861634698972354,
+            "angularVelocity": -2.1420090940402864e-7,
+            "velocityX": 3.7506067724653955,
+            "velocityY": 0.4935461136795145,
+            "timestamp": 0.9904920002383464
+        },
+        {
+            "x": 5.6652304398823485,
+            "y": 7.200917329284216,
+            "heading": 0.18861632770206457,
+            "angularVelocity": -2.1420087020950116e-7,
+            "velocityX": 3.750606903609938,
+            "velocityY": 0.4935451170711973,
+            "timestamp": 1.0805367275327415
+        },
+        {
+            "x": 6.002952827157885,
+            "y": 7.245358377734637,
+            "heading": 0.18861630841441618,
+            "angularVelocity": -2.1420075320811155e-7,
+            "velocityX": 3.750607030785673,
+            "velocityY": 0.49354415062109597,
+            "timestamp": 1.1705814548271367
+        },
+        {
+            "x": 6.340675225087303,
+            "y": 7.289799345222555,
+            "heading": 0.1886162891267749,
+            "angularVelocity": -2.1420067405327174e-7,
+            "velocityX": 3.750607149103328,
+            "velocityY": 0.4935432514845822,
+            "timestamp": 1.2606261821215319
+        },
+        {
+            "x": 6.678397632133336,
+            "y": 7.334240243430023,
+            "heading": 0.18861626983912938,
+            "angularVelocity": -2.1420072105835628e-7,
+            "velocityX": 3.7506072503487453,
+            "velocityY": 0.4935424820841753,
+            "timestamp": 1.350670909415927
+        },
+        {
+            "x": 7.011808183476814,
+            "y": 7.378579327724673,
+            "heading": 0.17443906284640512,
+            "angularVelocity": -0.15744627607536488,
+            "velocityX": 3.7027215403008924,
+            "velocityY": 0.49241177831197624,
+            "timestamp": 1.4407156367103222
+        },
+        {
+            "x": 7.279274281127278,
+            "y": 7.414016049929063,
+            "heading": 0.11701095642225132,
+            "angularVelocity": -0.637773117313093,
+            "velocityX": 2.9703693451811146,
+            "velocityY": 0.3935457774060549,
+            "timestamp": 1.5307603640047174
+        },
+        {
+            "x": 7.479727163444043,
+            "y": 7.440534681549159,
+            "heading": 0.061439635089441606,
+            "angularVelocity": -0.6171524197205129,
+            "velocityX": 2.226147919371209,
+            "velocityY": 0.294505102263186,
+            "timestamp": 1.6208050912991125
+        },
+        {
+            "x": 7.613267962945412,
+            "y": 7.458185352165826,
+            "heading": 0.019571051398075176,
+            "angularVelocity": -0.46497540666073534,
+            "velocityX": 1.4830496300440337,
+            "velocityY": 0.19602114579078314,
+            "timestamp": 1.7108498185935077
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -0.003,
+            "angularVelocity": -0.2506648870652985,
+            "velocityX": 0.7410987745724689,
+            "velocityY": 0.09789188216823574,
+            "timestamp": 1.8008945458879029
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -0.003,
+            "angularVelocity": 5.335131961970383e-32,
+            "velocityX": 0,
+            "velocityY": -3.2330177098423496e-35,
+            "timestamp": 1.890939273182298
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W1 To C2.traj b/src/main/deploy/choreo/W1 To C2.traj
new file mode 100644
index 00000000..cd2bebdf
--- /dev/null
+++ b/src/main/deploy/choreo/W1 To C2.traj	
@@ -0,0 +1,256 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 2.274971502954598e-31,
+            "velocityX": 3.487335890979115e-33,
+            "velocityY": 2.862188318424697e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3417817158351957,
+            "y": 6.761449131523058,
+            "heading": 0.37953631669999827,
+            "angularVelocity": -0.14722299547472617,
+            "velocityX": 0.5878646347531417,
+            "velocityY": 0.06259884318982863,
+            "timestamp": 0.07107370194626751
+        },
+        {
+            "x": 2.42534939307215,
+            "y": 6.770350816518497,
+            "heading": 0.3588464620969251,
+            "angularVelocity": -0.2911042204995314,
+            "velocityX": 1.1757890042104941,
+            "velocityY": 0.12524583287024788,
+            "timestamp": 0.14214740389253502
+        },
+        {
+            "x": 2.5507092244237364,
+            "y": 6.783710205716013,
+            "heading": 0.3282884162089386,
+            "angularVelocity": -0.42994870185747563,
+            "velocityX": 1.7638005045292426,
+            "velocityY": 0.18796529281121122,
+            "timestamp": 0.21322110583880255
+        },
+        {
+            "x": 2.717871083967313,
+            "y": 6.80153596037414,
+            "heading": 0.2884653946252666,
+            "angularVelocity": -0.5603059991696991,
+            "velocityX": 2.351950932145827,
+            "velocityY": 0.25080661580833774,
+            "timestamp": 0.28429480778507005
+        },
+        {
+            "x": 2.926852904271386,
+            "y": 6.82384561239607,
+            "heading": 0.2406022074409234,
+            "angularVelocity": -0.673430338841082,
+            "velocityX": 2.9403536692385535,
+            "velocityY": 0.31389461096009597,
+            "timestamp": 0.35536850973133755
+        },
+        {
+            "x": 3.1776924244019935,
+            "y": 6.85069218229951,
+            "heading": 0.1884744580400949,
+            "angularVelocity": -0.7334323100298853,
+            "velocityX": 3.529287391280776,
+            "velocityY": 0.3777285995842137,
+            "timestamp": 0.42644221167760504
+        },
+        {
+            "x": 3.4450596021465354,
+            "y": 6.879057072545224,
+            "heading": 0.1884744481252109,
+            "angularVelocity": -1.3950144320454384e-7,
+            "velocityX": 3.7618299092775445,
+            "velocityY": 0.39909121755204374,
+            "timestamp": 0.49751591362387254
+        },
+        {
+            "x": 3.712426777772062,
+            "y": 6.907421982754425,
+            "heading": 0.18847443820909762,
+            "angularVelocity": -1.3951873937625683e-7,
+            "velocityX": 3.7618298794631864,
+            "velocityY": 0.39909149843660235,
+            "timestamp": 0.5685896155701401
+        },
+        {
+            "x": 3.979793955045072,
+            "y": 6.93578687743453,
+            "heading": 0.188474428292985,
+            "angularVelocity": -1.3951872987555026e-7,
+            "velocityX": 3.7618299026431106,
+            "velocityY": 0.3990912799437423,
+            "timestamp": 0.6396633175164076
+        },
+        {
+            "x": 4.247161134956238,
+            "y": 6.964151747247427,
+            "heading": 0.18847441837687146,
+            "angularVelocity": -1.395187425580786e-7,
+            "velocityX": 3.761829939761708,
+            "velocityY": 0.399090930064572,
+            "timestamp": 0.7107370194626751
+        },
+        {
+            "x": 4.514528318048531,
+            "y": 6.992516587075052,
+            "heading": 0.18847440846075478,
+            "angularVelocity": -1.3951878716159625e-7,
+            "velocityX": 3.7618299845198404,
+            "velocityY": 0.39909050817474234,
+            "timestamp": 0.7818107214089426
+        },
+        {
+            "x": 4.781895504744899,
+            "y": 7.020881392930694,
+            "heading": 0.18847439854463183,
+            "angularVelocity": -1.395188750932112e-7,
+            "velocityX": 3.761830035228834,
+            "velocityY": 0.3990900301922755,
+            "timestamp": 0.8528844233552101
+        },
+        {
+            "x": 5.049262689335958,
+            "y": 7.049246218630208,
+            "heading": 0.18847438862849922,
+            "angularVelocity": -1.3951901124406535e-7,
+            "velocityX": 3.7618300056073353,
+            "velocityY": 0.3990903093936624,
+            "timestamp": 0.9239581253014776
+        },
+        {
+            "x": 5.3166271708653765,
+            "y": 7.077636511969196,
+            "heading": 0.18847437871238637,
+            "angularVelocity": -1.3951873300413244e-7,
+            "velocityX": 3.761791973795463,
+            "velocityY": 0.3994486365804798,
+            "timestamp": 0.9950318272477451
+        },
+        {
+            "x": 5.584732592713635,
+            "y": 7.097866624574175,
+            "heading": 0.18847436874075538,
+            "angularVelocity": -1.4029986815741496e-7,
+            "velocityX": 3.772216931248832,
+            "velocityY": 0.28463569578063386,
+            "timestamp": 1.0661055291940127
+        },
+        {
+            "x": 5.85271010689652,
+            "y": 7.076007397472714,
+            "heading": 0.188474358062239,
+            "angularVelocity": -1.5024567569917294e-7,
+            "velocityX": 3.7704172829702145,
+            "velocityY": -0.3075571766057785,
+            "timestamp": 1.1371792311402802
+        },
+        {
+            "x": 6.113985538482666,
+            "y": 7.0125651359558105,
+            "heading": 0.18847434515977443,
+            "angularVelocity": -1.8153640819270364e-7,
+            "velocityX": 3.6761196396335167,
+            "velocityY": -0.8926263833128883,
+            "timestamp": 1.2082529330865477
+        },
+        {
+            "x": 6.38971197835489,
+            "y": 6.891444245730791,
+            "heading": 0.18847433517810044,
+            "angularVelocity": -1.2538350348338093e-7,
+            "velocityX": 3.4635019217336325,
+            "velocityY": -1.521444357133088,
+            "timestamp": 1.2878620818911364
+        },
+        {
+            "x": 6.640022696468843,
+            "y": 6.723993100546061,
+            "heading": 0.18847432674418188,
+            "angularVelocity": -1.0594157434839283e-7,
+            "velocityX": 3.1442456284571,
+            "velocityY": -2.103415847287377,
+            "timestamp": 1.367471230695725
+        },
+        {
+            "x": 6.8648306229514935,
+            "y": 6.523601637914116,
+            "heading": 0.18847431867026868,
+            "angularVelocity": -1.014194138550125e-7,
+            "velocityX": 2.8238956182605124,
+            "velocityY": -2.5171913736164258,
+            "timestamp": 1.4470803795003138
+        },
+        {
+            "x": 7.089642217529903,
+            "y": 6.3232142903896476,
+            "heading": 0.1884743105959038,
+            "angularVelocity": -1.0142508708190059e-7,
+            "velocityX": 2.823941694568014,
+            "velocityY": -2.517139682229462,
+            "timestamp": 1.5266895283049025
+        },
+        {
+            "x": 7.2864279243188,
+            "y": 6.147809259471006,
+            "heading": 0.12465186300240168,
+            "angularVelocity": -0.8016973997575559,
+            "velocityX": 2.471898139143281,
+            "velocityY": -2.203327551576542,
+            "timestamp": 1.6062986771094911
+        },
+        {
+            "x": 7.443856673448633,
+            "y": 6.007485445692029,
+            "heading": 0.07359246308164451,
+            "angularVelocity": -0.6413760313670541,
+            "velocityX": 1.9775208188222257,
+            "velocityY": -1.7626593913656032,
+            "timestamp": 1.6859078259140798
+        },
+        {
+            "x": 7.561928317357885,
+            "y": 5.902242696623479,
+            "heading": 0.0352966131369852,
+            "angularVelocity": -0.4810483533581169,
+            "velocityX": 1.483141644926293,
+            "velocityY": -1.3219931458747511,
+            "timestamp": 1.7655169747186685
+        },
+        {
+            "x": 7.640642772199918,
+            "y": 5.832080900256126,
+            "heading": 0.009765510491971057,
+            "angularVelocity": -0.320705635324694,
+            "velocityX": 0.9887614178017027,
+            "velocityY": -0.8813283073733266,
+            "timestamp": 1.8451261235232572
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -0.003,
+            "angularVelocity": -0.1603523047748632,
+            "velocityX": 0.4943807136624207,
+            "velocityY": -0.44066417971947147,
+            "timestamp": 1.924735272327846
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -0.003,
+            "angularVelocity": 9.952815155734959e-29,
+            "velocityX": 0,
+            "velocityY": 1.270429811278963e-30,
+            "timestamp": 2.004344421132435
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W1 To C3.traj b/src/main/deploy/choreo/W1 To C3.traj
new file mode 100644
index 00000000..a91e184b
--- /dev/null
+++ b/src/main/deploy/choreo/W1 To C3.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3668131239789423,
+            "y": 6.6799532352680275,
+            "heading": 0.31688592464549686,
+            "angularVelocity": -0.6546901107445974,
+            "velocityX": 0.5982690928509629,
+            "velocityY": -0.6899048465062367,
+            "timestamp": 0.11167737858654542
+        },
+        {
+            "x": 2.5010623988117353,
+            "y": 6.525329092787392,
+            "heading": 0.1875032700506318,
+            "angularVelocity": -1.1585395022018103,
+            "velocityX": 1.202116995688263,
+            "velocityY": -1.3845609955896643,
+            "timestamp": 0.22335475717309083
+        },
+        {
+            "x": 2.7034647042549174,
+            "y": 6.29238592031536,
+            "heading": 0.03684167640916454,
+            "angularVelocity": -1.3490788873122188,
+            "velocityX": 1.8123841014618285,
+            "velocityY": -2.0858581694904785,
+            "timestamp": 0.33503213575963625
+        },
+        {
+            "x": 2.970402662431944,
+            "y": 5.984552676414473,
+            "heading": 2.692880535790558e-7,
+            "angularVelocity": -0.32989140314179594,
+            "velocityX": 2.39025988571325,
+            "velocityY": -2.7564511971627637,
+            "timestamp": 0.44670951434618167
+        },
+        {
+            "x": 3.247779632307318,
+            "y": 5.665896160181276,
+            "heading": 2.2674846776964554e-7,
+            "angularVelocity": -3.809149744305609e-7,
+            "velocityX": 2.4837346057547505,
+            "velocityY": -2.853366727141142,
+            "timestamp": 0.5583868929327271
+        },
+        {
+            "x": 3.5251566242922663,
+            "y": 5.347239663193555,
+            "heading": 1.842089099097335e-7,
+            "angularVelocity": -3.8091472416520575e-7,
+            "velocityX": 2.4837348037318896,
+            "velocityY": -2.853366554810183,
+            "timestamp": 0.6700642715192726
+        },
+        {
+            "x": 3.8025335437012195,
+            "y": 5.0285831030297485,
+            "heading": 1.4166936288049968e-7,
+            "angularVelocity": -3.8091462718327505e-7,
+            "velocityX": 2.4837341538598574,
+            "velocityY": -2.8533671205119875,
+            "timestamp": 0.7817416501058181
+        },
+        {
+            "x": 4.079910865419093,
+            "y": 4.7099268930580935,
+            "heading": 9.912982095474378e-8,
+            "angularVelocity": -3.809145814844431e-7,
+            "velocityX": 2.4837377562807075,
+            "velocityY": -2.853363984763686,
+            "timestamp": 0.8934190286923636
+        },
+        {
+            "x": 4.382964098552189,
+            "y": 4.415582373772677,
+            "heading": 5.542611481257948e-8,
+            "angularVelocity": -3.9133893269333644e-7,
+            "velocityX": 2.713649236476739,
+            "velocityY": -2.635668234792112,
+            "timestamp": 1.005096407278909
+        },
+        {
+            "x": 4.748738765716553,
+            "y": 4.2041850090026855,
+            "heading": 0,
+            "angularVelocity": -4.963056575446211e-7,
+            "velocityX": 3.27527984443961,
+            "velocityY": -1.8929291450565913,
+            "timestamp": 1.1167737858654545
+        },
+        {
+            "x": 5.133803316447558,
+            "y": 4.089523361787239,
+            "heading": -1.387645596172588e-10,
+            "angularVelocity": -1.3065520492479572e-9,
+            "velocityX": 3.625615065458012,
+            "velocityY": -1.0796085871456598,
+            "timestamp": 1.2229804690844188
+        },
+        {
+            "x": 5.534974194059096,
+            "y": 4.0675249151482635,
+            "heading": -2.526284831735191e-10,
+            "angularVelocity": -1.0720975375429818e-9,
+            "velocityX": 3.7772658504404184,
+            "velocityY": -0.20712864739049783,
+            "timestamp": 1.3291871523033831
+        },
+        {
+            "x": 5.9365934232146325,
+            "y": 4.0786604378791305,
+            "heading": -3.6352969398434843e-10,
+            "angularVelocity": -1.0442018095219078e-9,
+            "velocityX": 3.781487350730336,
+            "velocityY": 0.10484766488641149,
+            "timestamp": 1.4353938355223474
+        },
+        {
+            "x": 6.3382126442540345,
+            "y": 4.089796253283455,
+            "heading": -4.744184411388132e-10,
+            "angularVelocity": -1.044084456790862e-9,
+            "velocityX": 3.781487274312031,
+            "velocityY": 0.1048504205837075,
+            "timestamp": 1.5416005187413118
+        },
+        {
+            "x": 6.739831868633552,
+            "y": 4.100931948201603,
+            "heading": -5.853040774969199e-10,
+            "angularVelocity": -1.0440551668485128e-9,
+            "velocityX": 3.7814873057612264,
+            "velocityY": 0.10484928613381475,
+            "timestamp": 1.647807201960276
+        },
+        {
+            "x": 7.11589910333758,
+            "y": 4.111359168329562,
+            "heading": -0.0011999964635320353,
+            "angularVelocity": -0.011298685184913941,
+            "velocityX": 3.5408999067290075,
+            "velocityY": 0.09817856854143046,
+            "timestamp": 1.7540138851792404
+        },
+        {
+            "x": 7.397949547057075,
+            "y": 4.119179583905487,
+            "heading": -0.002099997192055183,
+            "angularVelocity": -0.008474049854922212,
+            "velocityX": 2.6556750966226486,
+            "velocityY": 0.0736339309252122,
+            "timestamp": 1.8602205683982047
+        },
+        {
+            "x": 7.585983181328134,
+            "y": 4.124393194553852,
+            "heading": -0.0026999988369627403,
+            "angularVelocity": -0.005649377484767236,
+            "velocityX": 1.7704501126675238,
+            "velocityY": 0.049089289773120645,
+            "timestamp": 1.966427251617169
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -0.003,
+            "angularVelocity": -0.0028246919491873987,
+            "velocityX": 0.8852250707992916,
+            "velocityY": 0.024544646034960003,
+            "timestamp": 2.072633934836133
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -0.003,
+            "angularVelocity": 0,
+            "velocityX": 1.6661554323284825e-27,
+            "velocityY": 0,
+            "timestamp": 2.1788406180550974
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W1 To C4.traj b/src/main/deploy/choreo/W1 To C4.traj
new file mode 100644
index 00000000..785ed7b8
--- /dev/null
+++ b/src/main/deploy/choreo/W1 To C4.traj	
@@ -0,0 +1,400 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 1.592522039385819e-32,
+            "velocityX": 0,
+            "velocityY": -2.1348488243236776e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 2.311435758812473,
+            "y": 6.743296066265471,
+            "heading": 0.3325883455828011,
+            "angularVelocity": -1.0899722636212008,
+            "velocityX": 0.2171102722189281,
+            "velocityY": -0.26017204737900074,
+            "timestamp": 0.052672582902671385
+        },
+        {
+            "x": 2.3359939039892414,
+            "y": 6.71538911279917,
+            "heading": 0.2289766220305027,
+            "angularVelocity": -1.9670902363712421,
+            "velocityX": 0.46624152117519013,
+            "velocityY": -0.529819346772266,
+            "timestamp": 0.10534516580534277
+        },
+        {
+            "x": 2.3766647023789824,
+            "y": 6.674615688796359,
+            "heading": 0.08902544501665462,
+            "angularVelocity": -2.6570023587499336,
+            "velocityX": 0.7721436115045406,
+            "velocityY": -0.7740919802272123,
+            "timestamp": 0.15801774870801416
+        },
+        {
+            "x": 2.4341603545379895,
+            "y": 6.621640281380937,
+            "heading": -0.08739056422595769,
+            "angularVelocity": -3.349294823240299,
+            "velocityX": 1.0915669783129407,
+            "velocityY": -1.0057491866937662,
+            "timestamp": 0.21069033161068554
+        },
+        {
+            "x": 2.508963105088746,
+            "y": 6.555426903517877,
+            "heading": -0.2922468734649172,
+            "angularVelocity": -3.8892398654057443,
+            "velocityX": 1.4201458601143102,
+            "velocityY": -1.2570748236404126,
+            "timestamp": 0.2633629145133569
+        },
+        {
+            "x": 2.602244928931498,
+            "y": 6.475307627430128,
+            "heading": -0.4990571722352239,
+            "angularVelocity": -3.9263367652285313,
+            "velocityX": 1.7709749304513536,
+            "velocityY": -1.5210812090949455,
+            "timestamp": 0.3160354974160283
+        },
+        {
+            "x": 2.7117676530035184,
+            "y": 6.382297804514516,
+            "heading": -0.6662019146269795,
+            "angularVelocity": -3.173277883497873,
+            "velocityX": 2.0793118172009177,
+            "velocityY": -1.7658109359755114,
+            "timestamp": 0.3687080803186997
+        },
+        {
+            "x": 2.833611348488951,
+            "y": 6.27498915923685,
+            "heading": -0.7793427065081729,
+            "angularVelocity": -2.1480015910792667,
+            "velocityX": 2.3132280357425485,
+            "velocityY": -2.0372770683364947,
+            "timestamp": 0.42138066322137113
+        },
+        {
+            "x": 2.968521622075034,
+            "y": 6.1546184694434425,
+            "heading": -0.8356933529016568,
+            "angularVelocity": -1.0698288044390758,
+            "velocityX": 2.5612997531442825,
+            "velocityY": -2.2852627146807634,
+            "timestamp": 0.47405324612404254
+        },
+        {
+            "x": 3.1177887530073343,
+            "y": 6.022632523021301,
+            "heading": -0.8357129106693775,
+            "angularVelocity": -0.00037130830959681015,
+            "velocityX": 2.8338676918144654,
+            "velocityY": -2.5057807904736045,
+            "timestamp": 0.5267258290267139
+        },
+        {
+            "x": 3.269422043665646,
+            "y": 5.893362401601714,
+            "heading": -0.8357131785361996,
+            "angularVelocity": -0.000005085507627644201,
+            "velocityX": 2.8787897289658386,
+            "velocityY": -2.45422028493365,
+            "timestamp": 0.5793984119293853
+        },
+        {
+            "x": 3.4210553878779164,
+            "y": 5.764092343000596,
+            "heading": -0.8357134464026715,
+            "angularVelocity": -0.000005085500977911915,
+            "velocityX": 2.8787907456989505,
+            "velocityY": -2.4542190923119205,
+            "timestamp": 0.6320709948320568
+        },
+        {
+            "x": 3.572688732091381,
+            "y": 5.634822284400852,
+            "heading": -0.8357137142690544,
+            "angularVelocity": -0.000005085499289329294,
+            "velocityX": 2.878790745721617,
+            "velocityY": -2.4542190922857987,
+            "timestamp": 0.6847435777347282
+        },
+        {
+            "x": 3.7243220763038787,
+            "y": 5.50555222579995,
+            "heading": -0.8357139821353484,
+            "angularVelocity": -0.000005085497600843703,
+            "velocityX": 2.8787907457032484,
+            "velocityY": -2.4542190923078118,
+            "timestamp": 0.7374161606373996
+        },
+        {
+            "x": 3.875955376681643,
+            "y": 5.376282115781124,
+            "heading": -0.8357142500017658,
+            "angularVelocity": -0.000005085499943938472,
+            "velocityX": 2.878789913491687,
+            "velocityY": -2.4542200684878277,
+            "timestamp": 0.790088743540071
+        },
+        {
+            "x": 4.025639474822054,
+            "y": 5.244759916762783,
+            "heading": -0.8357145286285158,
+            "angularVelocity": -0.000005289787108985714,
+            "velocityX": 2.8417839014463553,
+            "velocityY": -2.4969764490450044,
+            "timestamp": 0.8427613264427424
+        },
+        {
+            "x": 4.159073025842643,
+            "y": 5.096777164207917,
+            "heading": -0.835714908739186,
+            "angularVelocity": -0.000007216480551859566,
+            "velocityX": 2.5332638664625207,
+            "velocityY": -2.809483499761346,
+            "timestamp": 0.8954339093454138
+        },
+        {
+            "x": 4.274456024169922,
+            "y": 4.934326648712158,
+            "heading": -0.8357152799414105,
+            "angularVelocity": -0.000007047351849068254,
+            "velocityX": 2.19057034929318,
+            "velocityY": -3.0841570043363045,
+            "timestamp": 0.9481064922480852
+        },
+        {
+            "x": 4.3556906912761315,
+            "y": 4.793345794539918,
+            "heading": -0.8357152378475459,
+            "angularVelocity": 9.786625189030426e-7,
+            "velocityX": 1.8886677315079998,
+            "velocityY": -3.2777384277013035,
+            "timestamp": 0.9911181166598327
+        },
+        {
+            "x": 4.442446525481621,
+            "y": 4.655693706781043,
+            "heading": -0.8357151935315337,
+            "angularVelocity": 0.0000010303263992905167,
+            "velocityX": 2.0170322649286843,
+            "velocityY": -3.200346177143663,
+            "timestamp": 1.0341297410715804
+        },
+        {
+            "x": 4.541847641111602,
+            "y": 4.5268757636785075,
+            "heading": -0.8357148699573956,
+            "angularVelocity": 0.000007522946236810339,
+            "velocityX": 2.311029099445763,
+            "velocityY": -2.9949564766345187,
+            "timestamp": 1.077141365483328
+        },
+        {
+            "x": 4.65067034095477,
+            "y": 4.409944405402937,
+            "heading": -0.8255512517653536,
+            "angularVelocity": 0.23629933375095027,
+            "velocityX": 2.530076492843306,
+            "velocityY": -2.7185989805033612,
+            "timestamp": 1.1201529898950755
+        },
+        {
+            "x": 4.7668457664948924,
+            "y": 4.306168868140065,
+            "heading": -0.803496324956771,
+            "angularVelocity": 0.5127666557638517,
+            "velocityX": 2.701023900608423,
+            "velocityY": -2.4127323411326107,
+            "timestamp": 1.163164614306823
+        },
+        {
+            "x": 4.88912359332006,
+            "y": 4.216234980411927,
+            "heading": -0.7705722789003837,
+            "angularVelocity": 0.7654685566210554,
+            "velocityX": 2.8429018549638743,
+            "velocityY": -2.0909205117948377,
+            "timestamp": 1.2061762387185706
+        },
+        {
+            "x": 5.016560626655114,
+            "y": 4.140615450479534,
+            "heading": -0.7285529163528696,
+            "angularVelocity": 0.9769303792218019,
+            "velocityX": 2.9628509752411754,
+            "velocityY": -1.7581184381341082,
+            "timestamp": 1.2491878631303182
+        },
+        {
+            "x": 5.148349456504816,
+            "y": 4.079665503809577,
+            "heading": -0.6795461936491729,
+            "angularVelocity": 1.1393832103284065,
+            "velocityX": 3.0640281935900804,
+            "velocityY": -1.4170575397591803,
+            "timestamp": 1.2921994875420657
+        },
+        {
+            "x": 5.28369745675312,
+            "y": 4.033673765712218,
+            "heading": -0.6261187752553977,
+            "angularVelocity": 1.2421623020399764,
+            "velocityX": 3.1467772282354725,
+            "velocityY": -1.0692862389265216,
+            "timestamp": 1.3352111119538133
+        },
+        {
+            "x": 5.421794981888129,
+            "y": 4.0028549700404525,
+            "heading": -0.5712230670658915,
+            "angularVelocity": 1.2762993479156466,
+            "velocityX": 3.2107023862435504,
+            "velocityY": -0.7165224771968601,
+            "timestamp": 1.3782227363655608
+        },
+        {
+            "x": 5.5620425748748135,
+            "y": 3.9873099729843906,
+            "heading": -0.5170015662419462,
+            "angularVelocity": 1.2606243443606422,
+            "velocityX": 3.260690450658235,
+            "velocityY": -0.3614138565716755,
+            "timestamp": 1.4212343607773084
+        },
+        {
+            "x": 5.703982353210449,
+            "y": 3.987083911895752,
+            "heading": -0.46507695218435896,
+            "angularVelocity": 1.207222809362328,
+            "velocityX": 3.3000329626441363,
+            "velocityY": -0.005255813788261737,
+            "timestamp": 1.464245985189056
+        },
+        {
+            "x": 5.957415351644791,
+            "y": 3.94389686592464,
+            "heading": -0.40627705964587574,
+            "angularVelocity": 0.8004851872173009,
+            "velocityX": 3.450165509502976,
+            "velocityY": -0.5879362884366123,
+            "timestamp": 1.5377013013865826
+        },
+        {
+            "x": 6.214659603999478,
+            "y": 3.857155103461619,
+            "heading": -0.3855287596647175,
+            "angularVelocity": 0.2824615161326723,
+            "velocityX": 3.502050847660077,
+            "velocityY": -1.180877939858924,
+            "timestamp": 1.6111566175841092
+        },
+        {
+            "x": 6.460339332530576,
+            "y": 3.727320070546642,
+            "heading": -0.38552868222223635,
+            "angularVelocity": 0.0000010542801416145007,
+            "velocityX": 3.3446146752734305,
+            "velocityY": -1.7675375947717866,
+            "timestamp": 1.6846119337816359
+        },
+        {
+            "x": 6.681847040612612,
+            "y": 3.5595404285567858,
+            "heading": -0.3855286464284339,
+            "angularVelocity": 4.872867523834452e-7,
+            "velocityX": 3.0155435923301614,
+            "velocityY": -2.2841048228379357,
+            "timestamp": 1.7580672499791625
+        },
+        {
+            "x": 6.873376982747171,
+            "y": 3.358214932798401,
+            "heading": -0.3855286139444744,
+            "angularVelocity": 4.422274806376815e-7,
+            "velocityX": 2.607434724254966,
+            "velocityY": -2.740788634235891,
+            "timestamp": 1.8315225661766892
+        },
+        {
+            "x": 7.058369006072979,
+            "y": 3.1508658904491242,
+            "heading": -0.3855285817822777,
+            "angularVelocity": 4.3784709334515964e-7,
+            "velocityX": 2.518429337753462,
+            "velocityY": -2.8227915021385286,
+            "timestamp": 1.9049778823742158
+        },
+        {
+            "x": 7.235640342207717,
+            "y": 2.9543486280986375,
+            "heading": -0.3517392740895451,
+            "angularVelocity": 0.4599981245995971,
+            "velocityX": 2.413322075396734,
+            "velocityY": -2.6753306979448217,
+            "timestamp": 1.9784331985717425
+        },
+        {
+            "x": 7.3842205894533075,
+            "y": 2.7881464841241645,
+            "heading": -0.2579536451103976,
+            "angularVelocity": 1.2767711560447332,
+            "velocityX": 2.0227296666458727,
+            "velocityY": -2.2626292088586646,
+            "timestamp": 2.051888514769269
+        },
+        {
+            "x": 7.502781206912582,
+            "y": 2.6554541566341396,
+            "heading": -0.1644602811973769,
+            "angularVelocity": 1.2727923416953275,
+            "velocityX": 1.6140508760517314,
+            "velocityY": -1.806435998903144,
+            "timestamp": 2.125343830966796
+        },
+        {
+            "x": 7.591494122264987,
+            "y": 2.5561305844890163,
+            "heading": -0.0867965293639426,
+            "angularVelocity": 1.0572924582421084,
+            "velocityX": 1.2077126604948574,
+            "velocityY": -1.3521631555983635,
+            "timestamp": 2.1987991471643227
+        },
+        {
+            "x": 7.6505265885867635,
+            "y": 2.4900164938031613,
+            "heading": -0.031723308715129774,
+            "angularVelocity": 0.7497513250193758,
+            "velocityX": 0.8036513812429008,
+            "velocityY": -0.900058622143418,
+            "timestamp": 2.2722544633618496
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -0.003,
+            "angularVelocity": 0.39103104039319253,
+            "velocityX": 0.401242727401519,
+            "velocityY": -0.4494772538229548,
+            "timestamp": 2.3457097795593764
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -0.003,
+            "angularVelocity": -1.5819357203294863e-32,
+            "velocityX": 5.378736393413154e-34,
+            "velocityY": 0,
+            "timestamp": 2.4191650957569033
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W1 To C5.traj b/src/main/deploy/choreo/W1 To C5.traj
new file mode 100644
index 00000000..95620590
--- /dev/null
+++ b/src/main/deploy/choreo/W1 To C5.traj	
@@ -0,0 +1,616 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 4.884595084783414e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 2.298507420370097,
+            "y": 6.738728491162364,
+            "heading": 0.38429180377773414,
+            "angularVelocity": -0.12150700609071861,
+            "velocityX": -0.031771662206367984,
+            "velocityY": -0.3889348314553929,
+            "timestamp": 0.04697832994094238
+        },
+        {
+            "x": 2.295523042319471,
+            "y": 6.702182630298916,
+            "heading": 0.3729896107436911,
+            "angularVelocity": -0.2405831166891495,
+            "velocityX": -0.06352669527371438,
+            "velocityY": -0.7779301841804652,
+            "timestamp": 0.09395665988188476
+        },
+        {
+            "x": 2.2910478858134478,
+            "y": 6.647358841243747,
+            "heading": 0.35624034482133005,
+            "angularVelocity": -0.3565317443897418,
+            "velocityX": -0.09526001694076108,
+            "velocityY": -1.1670016606398963,
+            "timestamp": 0.14093498982282715
+        },
+        {
+            "x": 2.285083332518943,
+            "y": 6.574252474793301,
+            "heading": 0.33424119989038353,
+            "angularVelocity": -0.46828282228427764,
+            "velocityX": -0.12696392787914923,
+            "velocityY": -1.556172101953168,
+            "timestamp": 0.18791331976376952
+        },
+        {
+            "x": 2.2776313636635503,
+            "y": 6.48285724279221,
+            "heading": 0.3072714314404645,
+            "angularVelocity": -0.5740895532860236,
+            "velocityX": -0.15862566559434335,
+            "velocityY": -1.9454763955207768,
+            "timestamp": 0.23489164970471188
+        },
+        {
+            "x": 2.268695072443392,
+            "y": 6.373164221156064,
+            "heading": 0.2757564747192681,
+            "angularVelocity": -0.6708402951065854,
+            "velocityX": -0.1902215602681591,
+            "velocityY": -2.334970650809517,
+            "timestamp": 0.28186997964565425
+        },
+        {
+            "x": 2.2582799597491072,
+            "y": 6.245160021300657,
+            "heading": 0.24041955506127732,
+            "angularVelocity": -0.752196165815466,
+            "velocityX": -0.22170036072754723,
+            "velocityY": -2.7247499009931837,
+            "timestamp": 0.3288483095865966
+        },
+        {
+            "x": 2.2463982479830333,
+            "y": 6.098824071496616,
+            "heading": 0.20274918029785605,
+            "angularVelocity": -0.8018670482917891,
+            "velocityX": -0.25291899011757896,
+            "velocityY": -3.114967049446051,
+            "timestamp": 0.375826639527539
+        },
+        {
+            "x": 2.2330953714575887,
+            "y": 5.934154199517091,
+            "heading": 0.1675471140087416,
+            "angularVelocity": -0.7493256216934009,
+            "velocityX": -0.28317048609790474,
+            "velocityY": -3.505230436810659,
+            "timestamp": 0.42280496946848134
+        },
+        {
+            "x": 2.218530566609672,
+            "y": 5.757035827571947,
+            "heading": 0.16754710135001605,
+            "angularVelocity": -2.6945882371217095e-7,
+            "velocityX": -0.3100324099691601,
+            "velocityY": -3.770214313020578,
+            "timestamp": 0.4697832994094237
+        },
+        {
+            "x": 2.203965673260931,
+            "y": 5.579917462904045,
+            "heading": 0.16754708869211127,
+            "angularVelocity": -2.694413527531555e-7,
+            "velocityX": -0.3100342938339967,
+            "velocityY": -3.7702141581142206,
+            "timestamp": 0.5167616293503661
+        },
+        {
+            "x": 2.1894007799094983,
+            "y": 5.402799098236364,
+            "heading": 0.16754707603420654,
+            "angularVelocity": -2.694413521606316e-7,
+            "velocityX": -0.31003429389130516,
+            "velocityY": -3.7702141581095083,
+            "timestamp": 0.5637399592913085
+        },
+        {
+            "x": 2.1748358865580655,
+            "y": 5.225680733568683,
+            "heading": 0.1675470633763018,
+            "angularVelocity": -2.6944135250249435e-7,
+            "velocityX": -0.3100342938913069,
+            "velocityY": -3.770214158109508,
+            "timestamp": 0.6107182892322509
+        },
+        {
+            "x": 2.160270993206632,
+            "y": 5.048562368901002,
+            "heading": 0.16754705071839698,
+            "angularVelocity": -2.694413536359467e-7,
+            "velocityX": -0.31003429389130693,
+            "velocityY": -3.7702141581095083,
+            "timestamp": 0.6576966191731933
+        },
+        {
+            "x": 2.1457060998551993,
+            "y": 4.871444004233322,
+            "heading": 0.1675470380604922,
+            "angularVelocity": -2.6944135247689087e-7,
+            "velocityX": -0.3100342938913068,
+            "velocityY": -3.7702141581095083,
+            "timestamp": 0.7046749491141358
+        },
+        {
+            "x": 2.131141206503766,
+            "y": 4.6943256395656405,
+            "heading": 0.16754702540258745,
+            "angularVelocity": -2.694413528693067e-7,
+            "velocityX": -0.31003429389130693,
+            "velocityY": -3.7702141581095083,
+            "timestamp": 0.7516532790550782
+        },
+        {
+            "x": 2.1165763131523327,
+            "y": 4.51720727489796,
+            "heading": 0.16754701274468267,
+            "angularVelocity": -2.694413529636153e-7,
+            "velocityX": -0.31003429389130693,
+            "velocityY": -3.7702141581095083,
+            "timestamp": 0.7986316089960206
+        },
+        {
+            "x": 2.1020114198009,
+            "y": 4.340088910230279,
+            "heading": 0.16754700008677786,
+            "angularVelocity": -2.694413530501801e-7,
+            "velocityX": -0.3100342938913069,
+            "velocityY": -3.770214158109508,
+            "timestamp": 0.845609938936963
+        },
+        {
+            "x": 2.0874465264494666,
+            "y": 4.162970545562599,
+            "heading": 0.16754698742887306,
+            "angularVelocity": -2.694413534833812e-7,
+            "velocityX": -0.31003429389130704,
+            "velocityY": -3.7702141581095083,
+            "timestamp": 0.8925882688779054
+        },
+        {
+            "x": 2.0728816330980333,
+            "y": 3.9858521808949186,
+            "heading": 0.16754697477096828,
+            "angularVelocity": -2.6944135303696276e-7,
+            "velocityX": -0.31003429389130693,
+            "velocityY": -3.770214158109508,
+            "timestamp": 0.9395665988188479
+        },
+        {
+            "x": 2.0583167397466005,
+            "y": 3.8087338162272375,
+            "heading": 0.16754696211306347,
+            "angularVelocity": -2.694413537071192e-7,
+            "velocityX": -0.31003429389130704,
+            "velocityY": -3.7702141581095083,
+            "timestamp": 0.9865449287597903
+        },
+        {
+            "x": 2.043751846395167,
+            "y": 3.631615451559557,
+            "heading": 0.1675469494551587,
+            "angularVelocity": -2.694413527332354e-7,
+            "velocityX": -0.31003429389130716,
+            "velocityY": -3.770214158109508,
+            "timestamp": 1.0335232587007326
+        },
+        {
+            "x": 2.0291869530437365,
+            "y": 3.4544970868918763,
+            "heading": 0.16754693679725388,
+            "angularVelocity": -2.694413532802898e-7,
+            "velocityX": -0.31003429389125564,
+            "velocityY": -3.7702141581095123,
+            "timestamp": 1.080501588641675
+        },
+        {
+            "x": 2.0146220597715567,
+            "y": 3.2773787222176782,
+            "heading": 0.1675469241393491,
+            "angularVelocity": -2.6944135307750017e-7,
+            "velocityX": -0.31003429220428774,
+            "velocityY": -3.7702141582482356,
+            "timestamp": 1.1274799185826172
+        },
+        {
+            "x": 2.0000597716588953,
+            "y": 3.1002601433334096,
+            "heading": 0.16754691148142445,
+            "angularVelocity": -2.6944177624221033e-7,
+            "velocityX": -0.30997883771037654,
+            "velocityY": -3.770218718011654,
+            "timestamp": 1.1744582485235595
+        },
+        {
+            "x": 2.001608158601577,
+            "y": 2.922550679334096,
+            "heading": 0.16754689857982888,
+            "angularVelocity": -2.7462865480192305e-7,
+            "velocityX": 0.032959599556405594,
+            "velocityY": -3.7827965409310345,
+            "timestamp": 1.2214365784645018
+        },
+        {
+            "x": 2.021524410839774,
+            "y": 2.7459539818451146,
+            "heading": 0.1675468848578902,
+            "angularVelocity": -2.9209081516381327e-7,
+            "velocityX": 0.4239455140962882,
+            "velocityY": -3.7591097365739743,
+            "timestamp": 1.2684149084054441
+        },
+        {
+            "x": 2.0595952756969864,
+            "y": 2.572363489269638,
+            "heading": 0.16754686955661854,
+            "angularVelocity": -3.257091448717022e-7,
+            "velocityX": 0.8103920446953395,
+            "velocityY": -3.6951184257444134,
+            "timestamp": 1.3153932383463864
+        },
+        {
+            "x": 2.1154126290237643,
+            "y": 2.4036404273247642,
+            "heading": 0.16754685154625998,
+            "angularVelocity": -3.8337587986161977e-7,
+            "velocityX": 1.1881510772508834,
+            "velocityY": -3.5915083008906312,
+            "timestamp": 1.3623715682873287
+        },
+        {
+            "x": 2.1883780268761943,
+            "y": 2.2415938375833533,
+            "heading": 0.1675468289010436,
+            "angularVelocity": -4.82035363826859e-7,
+            "velocityX": 1.5531713865554018,
+            "velocityY": -3.449390175111029,
+            "timestamp": 1.409349898228271
+        },
+        {
+            "x": 2.2777091497247044,
+            "y": 2.0879611790897656,
+            "heading": 0.16754679784052873,
+            "angularVelocity": -6.611668595012802e-7,
+            "velocityX": 1.9015389214731706,
+            "velocityY": -3.270287783467881,
+            "timestamp": 1.4563282281692134
+        },
+        {
+            "x": 2.382448196411133,
+            "y": 1.9443897008895872,
+            "heading": 0.16754674836915184,
+            "angularVelocity": -0.0000010530680204285256,
+            "velocityX": 2.2295183080815923,
+            "velocityY": -3.0561213729961265,
+            "timestamp": 1.5033065581101557
+        },
+        {
+            "x": 2.5069799221636964,
+            "y": 1.8074962230648766,
+            "heading": 0.1675467031876573,
+            "angularVelocity": -9.235757728412815e-7,
+            "velocityX": 2.545610454081449,
+            "velocityY": -2.798302730812182,
+            "timestamp": 1.55222673973217
+        },
+        {
+            "x": 2.645527049823341,
+            "y": 1.6848068830394283,
+            "heading": 0.16754667440242366,
+            "angularVelocity": -5.884122397185895e-7,
+            "velocityX": 2.8321057499364835,
+            "velocityY": -2.5079493975189484,
+            "timestamp": 1.6011469213541845
+        },
+        {
+            "x": 2.7964787325047507,
+            "y": 1.577748144471377,
+            "heading": 0.1675466534117406,
+            "angularVelocity": -4.2908023560369677e-7,
+            "velocityX": 3.0856729815058617,
+            "velocityY": -2.1884370625450487,
+            "timestamp": 1.6500671029761989
+        },
+        {
+            "x": 2.958079900807193,
+            "y": 1.4875647290195357,
+            "heading": 0.1675466366326211,
+            "angularVelocity": -3.429897224036844e-7,
+            "velocityX": 3.303364029820377,
+            "velocityY": -1.8434807979383812,
+            "timestamp": 1.6989872845982132
+        },
+        {
+            "x": 3.1284516660647324,
+            "y": 1.4153051353233332,
+            "heading": 0.16754662225394246,
+            "angularVelocity": -2.9392120351406106e-7,
+            "velocityX": 3.4826478481607013,
+            "velocityY": -1.4770916889581902,
+            "timestamp": 1.7479074662202276
+        },
+        {
+            "x": 3.3056131601804433,
+            "y": 1.3618094062641966,
+            "heading": 0.16754660920712522,
+            "angularVelocity": -2.666960100615425e-7,
+            "velocityX": 3.6214398279336546,
+            "velocityY": -1.0935308759169382,
+            "timestamp": 1.796827647842242
+        },
+        {
+            "x": 3.487504529935422,
+            "y": 1.3276990268102373,
+            "heading": 0.16754659675699374,
+            "angularVelocity": -2.544988811687778e-7,
+            "velocityX": 3.7181253978240782,
+            "velocityY": -0.6972660019440657,
+            "timestamp": 1.8457478294642564
+        },
+        {
+            "x": 3.671043389503124,
+            "y": 1.304003553111977,
+            "heading": 0.16754658441216208,
+            "angularVelocity": -2.5234639867690264e-7,
+            "velocityX": 3.751802497092686,
+            "velocityY": -0.48437010887133,
+            "timestamp": 1.8946680110862708
+        },
+        {
+            "x": 3.8545823032662914,
+            "y": 1.2803084992011935,
+            "heading": 0.16754657206733162,
+            "angularVelocity": -2.523463739841797e-7,
+            "velocityX": 3.751803604927206,
+            "velocityY": -0.4843615278018566,
+            "timestamp": 1.9435881927082852
+        },
+        {
+            "x": 4.038121217030901,
+            "y": 1.2566134453015836,
+            "heading": 0.16754655972250113,
+            "angularVelocity": -2.5234637491728997e-7,
+            "velocityX": 3.7518036049566934,
+            "velocityY": -0.48436152757345374,
+            "timestamp": 1.9925083743302996
+        },
+        {
+            "x": 4.221660130795511,
+            "y": 1.232918391401974,
+            "heading": 0.16754654737767066,
+            "angularVelocity": -2.5234637402611237e-7,
+            "velocityX": 3.7518036049566943,
+            "velocityY": -0.4843615275734478,
+            "timestamp": 2.041428555952314
+        },
+        {
+            "x": 4.4051990445601215,
+            "y": 1.2092233375023644,
+            "heading": 0.16754653503284023,
+            "angularVelocity": -2.5234637391390173e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.4843615275734475,
+            "timestamp": 2.090348737574329
+        },
+        {
+            "x": 4.588737958324731,
+            "y": 1.1855282836027548,
+            "heading": 0.16754652268800974,
+            "angularVelocity": -2.523463747404588e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.4843615275734478,
+            "timestamp": 2.1392689191963434
+        },
+        {
+            "x": 4.77227687208934,
+            "y": 1.1618332297031453,
+            "heading": 0.16754651034317924,
+            "angularVelocity": -2.523463747849071e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.48436152757344797,
+            "timestamp": 2.188189100818358
+        },
+        {
+            "x": 4.95581578585395,
+            "y": 1.1381381758035356,
+            "heading": 0.16754649799834875,
+            "angularVelocity": -2.52346374599026e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.4843615275734478,
+            "timestamp": 2.2371092824403727
+        },
+        {
+            "x": 5.1393546996185595,
+            "y": 1.114443121903926,
+            "heading": 0.1675464856535183,
+            "angularVelocity": -2.523463743979587e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.48436152757344764,
+            "timestamp": 2.2860294640623873
+        },
+        {
+            "x": 5.32289361338317,
+            "y": 1.0907480680043165,
+            "heading": 0.16754647330868783,
+            "angularVelocity": -2.523463747332428e-7,
+            "velocityX": 3.7518036049566943,
+            "velocityY": -0.48436152757344797,
+            "timestamp": 2.334949645684402
+        },
+        {
+            "x": 5.506432527147779,
+            "y": 1.0670530141047068,
+            "heading": 0.1675464609638573,
+            "angularVelocity": -2.5234637523589347e-7,
+            "velocityX": 3.7518036049566943,
+            "velocityY": -0.48436152757344764,
+            "timestamp": 2.3838698273064165
+        },
+        {
+            "x": 5.689971440912389,
+            "y": 1.043357960205097,
+            "heading": 0.16754644861902682,
+            "angularVelocity": -2.523463745889681e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.4843615275734477,
+            "timestamp": 2.432790008928431
+        },
+        {
+            "x": 5.8735103546769984,
+            "y": 1.0196629063054876,
+            "heading": 0.16754643627419633,
+            "angularVelocity": -2.5234637515727394e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.4843615275734478,
+            "timestamp": 2.4817101905504457
+        },
+        {
+            "x": 6.057049268441609,
+            "y": 0.995967852405878,
+            "heading": 0.16754642392936583,
+            "angularVelocity": -2.523463750187176e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.4843615275734476,
+            "timestamp": 2.5306303721724603
+        },
+        {
+            "x": 6.240588182206218,
+            "y": 0.9722727985062686,
+            "heading": 0.16754641158453532,
+            "angularVelocity": -2.523463753175626e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.48436152757344764,
+            "timestamp": 2.579550553794475
+        },
+        {
+            "x": 6.424127095970827,
+            "y": 0.948577744606659,
+            "heading": 0.1675463992397048,
+            "angularVelocity": -2.5234637555305e-7,
+            "velocityX": 3.7518036049566934,
+            "velocityY": -0.4843615275734476,
+            "timestamp": 2.6284707354164896
+        },
+        {
+            "x": 6.607666009735459,
+            "y": 0.9248826907072107,
+            "heading": 0.1675463868948743,
+            "angularVelocity": -2.5234637524939476e-7,
+            "velocityX": 3.7518036049571197,
+            "velocityY": -0.48436152757015266,
+            "timestamp": 2.677390917038504
+        },
+        {
+            "x": 6.79120492428193,
+            "y": 0.9011876428641385,
+            "heading": 0.16754637454996243,
+            "angularVelocity": -2.523480385625403e-7,
+            "velocityX": 3.7518036209390857,
+            "velocityY": -0.4843614037689742,
+            "timestamp": 2.726311098660519
+        },
+        {
+            "x": 6.9688913787459,
+            "y": 0.8784350451744124,
+            "heading": 0.1510406339197301,
+            "angularVelocity": -0.33740145851798203,
+            "velocityX": 3.6321707845828914,
+            "velocityY": -0.4650963454209109,
+            "timestamp": 2.7752312802825334
+        },
+        {
+            "x": 7.1269915101358645,
+            "y": 0.8581344466937117,
+            "heading": 0.12402265057850995,
+            "angularVelocity": -0.5522870611964742,
+            "velocityX": 3.2317977192222487,
+            "velocityY": -0.4149738984526803,
+            "timestamp": 2.824151461904548
+        },
+        {
+            "x": 7.265299167873697,
+            "y": 0.8403581253912503,
+            "heading": 0.0960275261585182,
+            "angularVelocity": -0.5722612527545016,
+            "velocityX": 2.8272106347943935,
+            "velocityY": -0.3633739841730647,
+            "timestamp": 2.8730716435265626
+        },
+        {
+            "x": 7.383819928892336,
+            "y": 0.8251162922066029,
+            "heading": 0.06979646830524168,
+            "angularVelocity": -0.5362011542792869,
+            "velocityX": 2.4227375510254454,
+            "velocityY": -0.3115653433671744,
+            "timestamp": 2.9219918251485772
+        },
+        {
+            "x": 7.482566860925546,
+            "y": 0.8124124441531555,
+            "heading": 0.04664460944238867,
+            "angularVelocity": -0.47325782724474763,
+            "velocityX": 2.01853159083065,
+            "velocityY": -0.2596852184974389,
+            "timestamp": 2.970912006770592
+        },
+        {
+            "x": 7.5615507925764724,
+            "y": 0.8022481812485672,
+            "heading": 0.027346506217350703,
+            "angularVelocity": -0.3944814304686405,
+            "velocityX": 1.6145469831081642,
+            "velocityY": -0.20777238692863387,
+            "timestamp": 3.0198321883926065
+        },
+        {
+            "x": 7.620780133649161,
+            "y": 0.7946243691967044,
+            "heading": 0.012413272398069258,
+            "angularVelocity": -0.3052571213791523,
+            "velocityX": 1.2107342840696753,
+            "velocityY": -0.15584185910773468,
+            "timestamp": 3.068752370014621
+        },
+        {
+            "x": 7.660261458521305,
+            "y": 0.7895415301241143,
+            "heading": 0.002207571288741933,
+            "angularVelocity": -0.2086194443876447,
+            "velocityX": 0.8070559749184711,
+            "velocityY": -0.10390065825721975,
+            "timestamp": 3.1176725516366357
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -0.003,
+            "angularVelocity": -0.10645036702804238,
+            "velocityX": 0.40348463199107626,
+            "velocityY": -0.05195258970523994,
+            "timestamp": 3.1665927332586503
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -0.003,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -1.0902826040652685e-32,
+            "timestamp": 3.215512914880665
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W1 To S1.traj b/src/main/deploy/choreo/W1 To S1.traj
new file mode 100644
index 00000000..6d3241e6
--- /dev/null
+++ b/src/main/deploy/choreo/W1 To S1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": -4.966859894029903e-31,
+            "velocityX": 7.6981862107013014e-34,
+            "velocityY": 1.676179535721191e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3730209621579745,
+            "y": 6.753775517490119,
+            "heading": 0.38350122654498725,
+            "angularVelocity": -0.06940058989840046,
+            "velocityX": 0.7797929692101098,
+            "velocityY": -0.03443434208804683,
+            "timestamp": 0.09364147285393071
+        },
+        {
+            "x": 2.5190628825903247,
+            "y": 6.747326552846718,
+            "heading": 0.37050348328006333,
+            "angularVelocity": -0.13880327667642306,
+            "velocityX": 1.5595858969471437,
+            "velocityY": -0.06886868015693057,
+            "timestamp": 0.18728294570786141
+        },
+        {
+            "x": 2.73812575373285,
+            "y": 6.7376531065331395,
+            "heading": 0.35100644039340695,
+            "angularVelocity": -0.2082094855243831,
+            "velocityX": 2.3393787439058484,
+            "velocityY": -0.10330301327774684,
+            "timestamp": 0.2809244185617921
+        },
+        {
+            "x": 3.0302095555912687,
+            "y": 6.7247551792664035,
+            "heading": 0.3250096622235805,
+            "angularVelocity": -0.27762034681324244,
+            "velocityX": 3.1191713773449834,
+            "velocityY": -0.13773733874150373,
+            "timestamp": 0.37456589141572283
+        },
+        {
+            "x": 3.3841048026937948,
+            "y": 6.709127593205063,
+            "heading": 0.3250096592315294,
+            "angularVelocity": -3.195216141250517e-8,
+            "velocityX": 3.779257590860006,
+            "velocityY": -0.16688744404647712,
+            "timestamp": 0.46820736426965354
+        },
+        {
+            "x": 3.7380000497922845,
+            "y": 6.693500007049851,
+            "heading": 0.3250096562396269,
+            "angularVelocity": -3.195060442591983e-8,
+            "velocityX": 3.7792575908168353,
+            "velocityY": -0.16688744504894126,
+            "timestamp": 0.5618488371235842
+        },
+        {
+            "x": 4.091895296890773,
+            "y": 6.677872420894639,
+            "heading": 0.3250096532477245,
+            "angularVelocity": -3.195060369258429e-8,
+            "velocityX": 3.7792575908168353,
+            "velocityY": -0.16688744504893968,
+            "timestamp": 0.6554903099775149
+        },
+        {
+            "x": 4.445790543993225,
+            "y": 6.6622448348316095,
+            "heading": 0.3250096502556742,
+            "angularVelocity": -3.195215253798021e-8,
+            "velocityX": 3.7792575908592094,
+            "velocityY": -0.1668874440645195,
+            "timestamp": 0.7491317828314457
+        },
+        {
+            "x": 4.737874310687331,
+            "y": 6.649346902155386,
+            "heading": 0.29900607193864137,
+            "angularVelocity": -0.27769296578215763,
+            "velocityX": 3.119171001824275,
+            "velocityY": -0.1377373965095818,
+            "timestamp": 0.8427732556853764
+        },
+        {
+            "x": 4.956937151198913,
+            "y": 6.639673451411702,
+            "heading": 0.27950309637569815,
+            "angularVelocity": -0.20827284074623165,
+            "velocityX": 2.339378416797119,
+            "velocityY": -0.10330306058697603,
+            "timestamp": 0.9364147285393072
+        },
+        {
+            "x": 5.10297904938153,
+            "y": 6.633224483848421,
+            "heading": 0.2665010284217395,
+            "angularVelocity": -0.1388494601558462,
+            "velocityX": 1.5595856593415998,
+            "velocityY": -0.06886871133840435,
+            "timestamp": 1.030056201393238
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": -0.06942467075336838,
+            "velocityX": 0.7797928459794067,
+            "velocityY": -0.03443435638235996,
+            "timestamp": 1.1236976742471687
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": 2.7813300894409408e-31,
+            "velocityX": -8.985802616555554e-33,
+            "velocityY": -4.9789977648629575e-31,
+            "timestamp": 1.2173391471010995
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W1 To S2.traj b/src/main/deploy/choreo/W1 To S2.traj
new file mode 100644
index 00000000..15f8fbd9
--- /dev/null
+++ b/src/main/deploy/choreo/W1 To S2.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": -5.274350592003984e-33,
+            "velocityX": 6.842220334639481e-35,
+            "velocityY": 4.754828012658472e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3477747650996332,
+            "y": 6.727866811231813,
+            "heading": 0.37690695075838176,
+            "angularVelocity": -0.1596548904259805,
+            "velocityX": 0.5825590927179829,
+            "velocityY": -0.3552462054262789,
+            "timestamp": 0.08200844463131846
+        },
+        {
+            "x": 2.4433243855136606,
+            "y": 6.669600518122756,
+            "heading": 0.35072364334029893,
+            "angularVelocity": -0.3192757469769597,
+            "velocityX": 1.16511928550267,
+            "velocityY": -0.7104913813571495,
+            "timestamp": 0.1640168892626369
+        },
+        {
+            "x": 2.5866489974259967,
+            "y": 6.582201260145896,
+            "heading": 0.3114536293241488,
+            "angularVelocity": -0.47885329605621113,
+            "velocityX": 1.7476811388958082,
+            "velocityY": -1.0657348565721152,
+            "timestamp": 0.24602533389395537
+        },
+        {
+            "x": 2.7777487646808345,
+            "y": 6.465669250143918,
+            "heading": 0.2590995246277934,
+            "angularVelocity": -0.638398946983098,
+            "velocityX": 2.330244990183096,
+            "velocityY": -1.4209757364118492,
+            "timestamp": 0.3280337785252738
+        },
+        {
+            "x": 3.0166238424465086,
+            "y": 6.320004790854863,
+            "heading": 0.19366077275903337,
+            "angularVelocity": -0.797951383701398,
+            "velocityX": 2.912810733572304,
+            "velocityY": -1.7762129247044165,
+            "timestamp": 0.4100422231565923
+        },
+        {
+            "x": 3.281496122930573,
+            "y": 6.158489372926003,
+            "heading": 0.19366076448026312,
+            "angularVelocity": -1.0095021672660313e-7,
+            "velocityX": 3.2298171447445223,
+            "velocityY": -1.9694973932864837,
+            "timestamp": 0.49205066778791073
+        },
+        {
+            "x": 3.546368403715213,
+            "y": 5.996973955490063,
+            "heading": 0.193660756201493,
+            "angularVelocity": -1.0095021513248387e-7,
+            "velocityX": 3.2298171484097047,
+            "velocityY": -1.9694973872758792,
+            "timestamp": 0.5740591124192291
+        },
+        {
+            "x": 3.785245611761769,
+            "y": 5.851315694507869,
+            "heading": 0.1281200370686441,
+            "angularVelocity": -0.7991947588751046,
+            "velocityX": 2.912836709932308,
+            "velocityY": -1.7761373433799759,
+            "timestamp": 0.6560675570505476
+        },
+        {
+            "x": 3.9763473726141734,
+            "y": 5.734789313275287,
+            "heading": 0.07567704387560718,
+            "angularVelocity": -0.6394828414171544,
+            "velocityX": 2.3302692998450527,
+            "velocityY": -1.4209070999510438,
+            "timestamp": 0.7380760016818662
+        },
+        {
+            "x": 4.119673683324307,
+            "y": 5.647394635056781,
+            "heading": 0.036339503004366135,
+            "angularVelocity": -0.47967670949118707,
+            "velocityX": 1.7477018538088223,
+            "velocityY": -1.0656790116115789,
+            "timestamp": 0.8200844463131847
+        },
+        {
+            "x": 4.2152245571841105,
+            "y": 5.589131547509505,
+            "heading": 0.010112949222819228,
+            "angularVelocity": -0.3198030873460947,
+            "velocityX": 1.1651345698527396,
+            "velocityY": -0.710452293165722,
+            "timestamp": 0.9020928909445032
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -0.003,
+            "angularVelocity": -0.15989754813385992,
+            "velocityX": 0.5825673566993228,
+            "velocityY": -0.3552261921375451,
+            "timestamp": 0.9841013355758217
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -0.003,
+            "angularVelocity": 2.1439897413450708e-33,
+            "velocityX": 3.9230953244452345e-34,
+            "velocityY": -1.8972025919917558e-33,
+            "timestamp": 1.0661097802071402
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W1 To S3.traj b/src/main/deploy/choreo/W1 To S3.traj
new file mode 100644
index 00000000..8360b86f
--- /dev/null
+++ b/src/main/deploy/choreo/W1 To S3.traj	
@@ -0,0 +1,544 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.542786541644192e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2937082670823616,
+            "y": 6.738516922494428,
+            "heading": 0.3838170687349535,
+            "angularVelocity": -0.12752172771189255,
+            "velocityX": -0.1297657401618881,
+            "velocityY": -0.3812097977411177,
+            "timestamp": 0.04848531599949176
+        },
+        {
+            "x": 2.281123433600762,
+            "y": 6.70154498788335,
+            "heading": 0.37169259737448246,
+            "angularVelocity": -0.2500648105569625,
+            "velocityX": -0.25955968775719646,
+            "velocityY": -0.7625387985814647,
+            "timestamp": 0.09697063199898352
+        },
+        {
+            "x": 2.262243832879689,
+            "y": 6.646077048114969,
+            "heading": 0.3539392998096368,
+            "angularVelocity": -0.36615823159004923,
+            "velocityX": -0.3893880102034898,
+            "velocityY": -1.1440152265685524,
+            "timestamp": 0.14545594799847528
+        },
+        {
+            "x": 2.2370673940379584,
+            "y": 6.572104077537847,
+            "heading": 0.3309787534387302,
+            "angularVelocity": -0.4735567026240549,
+            "velocityX": -0.5192590441623445,
+            "velocityY": -1.5256778068215682,
+            "timestamp": 0.19394126399796704
+        },
+        {
+            "x": 2.2055915440305824,
+            "y": 6.479614437749851,
+            "heading": 0.303410444299779,
+            "angularVelocity": -0.5685908933466691,
+            "velocityX": -0.6491831466623577,
+            "velocityY": -1.9075804268039651,
+            "timestamp": 0.2424265799974588
+        },
+        {
+            "x": 2.167813255965874,
+            "y": 6.3685929704461826,
+            "heading": 0.2721534840825349,
+            "angularVelocity": -0.6446685882563702,
+            "velocityX": -0.7791696782014628,
+            "velocityY": -2.2897956838082623,
+            "timestamp": 0.29091189599695055
+        },
+        {
+            "x": 2.123730000007902,
+            "y": 6.239021112222755,
+            "heading": 0.23878941688886735,
+            "angularVelocity": -0.6881272505944181,
+            "velocityX": -0.909208387103519,
+            "velocityY": -2.6723938073299895,
+            "timestamp": 0.3393972119964423
+        },
+        {
+            "x": 2.0733472424162183,
+            "y": 6.090890823809695,
+            "heading": 0.2066566550746183,
+            "angularVelocity": -0.6627318220196932,
+            "velocityX": -1.0391343554894152,
+            "velocityY": -3.0551577391940747,
+            "timestamp": 0.38788252799593403
+        },
+        {
+            "x": 2.0168156590767947,
+            "y": 5.924489369921669,
+            "heading": 0.187194468928521,
+            "angularVelocity": -0.4014037187247154,
+            "velocityX": -1.1659526636929207,
+            "velocityY": -3.431996893448088,
+            "timestamp": 0.4363678439954258
+        },
+        {
+            "x": 1.9576664650342146,
+            "y": 5.750871436780747,
+            "heading": 0.18719445417707173,
+            "angularVelocity": -3.042457077435804e-7,
+            "velocityX": -1.2199403638687387,
+            "velocityY": -3.580835342863344,
+            "timestamp": 0.4848531599949175
+        },
+        {
+            "x": 1.8985172819179166,
+            "y": 5.577253499917319,
+            "heading": 0.18719443942575115,
+            "angularVelocity": -3.042430533078605e-7,
+            "velocityX": -1.2199401385163373,
+            "velocityY": -3.5808354196392784,
+            "timestamp": 0.5333384759944093
+        },
+        {
+            "x": 1.8393681174712941,
+            "y": 5.403635556693396,
+            "heading": 0.18719442467442912,
+            "angularVelocity": -3.0424308271607693e-7,
+            "velocityX": -1.219939753457998,
+            "velocityY": -3.5808355508232466,
+            "timestamp": 0.5818237919939011
+        },
+        {
+            "x": 1.780218973506269,
+            "y": 5.230017606491683,
+            "heading": 0.18719440992310651,
+            "angularVelocity": -3.0424309544377536e-7,
+            "velocityX": -1.2199393310291196,
+            "velocityY": -3.580835694738771,
+            "timestamp": 0.6303091079933929
+        },
+        {
+            "x": 1.7210698517072343,
+            "y": 5.056399648738334,
+            "heading": 0.1871943951717844,
+            "angularVelocity": -3.042430844451726e-7,
+            "velocityX": -1.2199388738599835,
+            "velocityY": -3.580835850489736,
+            "timestamp": 0.6787944239928847
+        },
+        {
+            "x": 1.661920753431811,
+            "y": 4.882781682970833,
+            "heading": 0.18719438042046438,
+            "angularVelocity": -3.042430423387178e-7,
+            "velocityX": -1.2199383886901667,
+            "velocityY": -3.580836015780049,
+            "timestamp": 0.7272797399923765
+        },
+        {
+            "x": 1.6027716795006517,
+            "y": 4.709163708909599,
+            "heading": 0.18719436566914785,
+            "angularVelocity": -3.042429697472901e-7,
+            "velocityX": -1.2199378865945683,
+            "velocityY": -3.5808361868366427,
+            "timestamp": 0.7757650559918683
+        },
+        {
+            "x": 1.5436226301529286,
+            "y": 4.5355457264731545,
+            "heading": 0.18719435091783626,
+            "angularVelocity": -3.042428676797686e-7,
+            "velocityX": -1.219937379566087,
+            "velocityY": -3.5808363595737087,
+            "timestamp": 0.8242503719913601
+        },
+        {
+            "x": 1.4844769425124074,
+            "y": 4.361926598741778,
+            "heading": 0.18719433616649686,
+            "angularVelocity": -3.0424344145116126e-7,
+            "velocityX": -1.2198680450801807,
+            "velocityY": -3.58085998103429,
+            "timestamp": 0.8727356879908519
+        },
+        {
+            "x": 1.4424355744034427,
+            "y": 4.183392744566331,
+            "heading": 0.18719432108286724,
+            "angularVelocity": -3.110968611299024e-7,
+            "velocityX": -0.8670948562865489,
+            "velocityY": -3.682225236552195,
+            "timestamp": 0.9212210039903437
+        },
+        {
+            "x": 1.4196862795998648,
+            "y": 4.0013919676043574,
+            "heading": 0.18719430492048,
+            "angularVelocity": -3.3334602243497606e-7,
+            "velocityX": -0.4691996810625877,
+            "velocityY": -3.753729829541186,
+            "timestamp": 0.9697063199898355
+        },
+        {
+            "x": 1.4164891295727136,
+            "y": 3.81800278977475,
+            "heading": 0.18719428668091828,
+            "angularVelocity": -3.761873328628986e-7,
+            "velocityX": -0.06594058347012394,
+            "velocityY": -3.7823653213178425,
+            "timestamp": 1.0181916359893273
+        },
+        {
+            "x": 1.432880701620732,
+            "y": 3.635319656493888,
+            "heading": 0.1871942648241357,
+            "angularVelocity": -4.507917942103899e-7,
+            "velocityX": 0.3380729136345315,
+            "velocityY": -3.767803292913241,
+            "timestamp": 1.066676951988819
+        },
+        {
+            "x": 1.4686738067578264,
+            "y": 3.455429018696199,
+            "heading": 0.18719405074105777,
+            "angularVelocity": -0.0000044154209051021175,
+            "velocityX": 0.7382256751280081,
+            "velocityY": -3.710208628927779,
+            "timestamp": 1.1151622679883109
+        },
+        {
+            "x": 1.5224561218642167,
+            "y": 3.2829543962052377,
+            "heading": 0.17869632281657946,
+            "angularVelocity": -0.17526394846200616,
+            "velocityX": 1.10924955315069,
+            "velocityY": -3.557254788081333,
+            "timestamp": 1.1636475839878027
+        },
+        {
+            "x": 1.5921416539328836,
+            "y": 3.121386413493124,
+            "heading": 0.1576086367549348,
+            "angularVelocity": -0.43492933122045885,
+            "velocityX": 1.4372502402541338,
+            "velocityY": -3.33230751169348,
+            "timestamp": 1.2121328999872945
+        },
+        {
+            "x": 1.6761645416915945,
+            "y": 2.9727322431612,
+            "heading": 0.12423651758368265,
+            "angularVelocity": -0.6882933210462135,
+            "velocityX": 1.7329553500274961,
+            "velocityY": -3.0659626995783453,
+            "timestamp": 1.2606182159867863
+        },
+        {
+            "x": 1.7731411061925635,
+            "y": 2.838565031466778,
+            "heading": 0.08175388007293943,
+            "angularVelocity": -0.8761959499529206,
+            "velocityX": 2.0001223566816617,
+            "velocityY": -2.767172058765521,
+            "timestamp": 1.309103531986278
+        },
+        {
+            "x": 1.8819216264400211,
+            "y": 2.719965378093697,
+            "heading": 0.033593056188374194,
+            "angularVelocity": -0.9933074146707891,
+            "velocityX": 2.2435765964482535,
+            "velocityY": -2.4460942643771193,
+            "timestamp": 1.3575888479857698
+        },
+        {
+            "x": 2.0016674106146493,
+            "y": 2.617587012881804,
+            "heading": -0.0178597683247242,
+            "angularVelocity": -1.0612042729434041,
+            "velocityX": 2.4697329842357973,
+            "velocityY": -2.1115334220519952,
+            "timestamp": 1.4060741639852616
+        },
+        {
+            "x": 2.131748914718628,
+            "y": 2.531857967376709,
+            "heading": -0.07087516069292028,
+            "angularVelocity": -1.0934319242038333,
+            "velocityX": 2.682905152268552,
+            "velocityY": -1.7681445142231829,
+            "timestamp": 1.4545594799847534
+        },
+        {
+            "x": 2.218821194191698,
+            "y": 2.484029060579093,
+            "heading": -0.10483282223316899,
+            "angularVelocity": -1.096901896110445,
+            "velocityX": 2.812612651242981,
+            "velocityY": -1.5449714785171498,
+            "timestamp": 1.4855172727096597
+        },
+        {
+            "x": 2.3097767814437278,
+            "y": 2.4431840829179357,
+            "heading": -0.13859592156415457,
+            "angularVelocity": -1.0906171389530375,
+            "velocityX": 2.938051432149048,
+            "velocityY": -1.3193762883647426,
+            "timestamp": 1.516475065434566
+        },
+        {
+            "x": 2.4044569077078632,
+            "y": 2.4094074987036276,
+            "heading": -0.1718069818405491,
+            "angularVelocity": -1.0727851488087659,
+            "velocityX": 3.0583616572737307,
+            "velocityY": -1.0910527282945428,
+            "timestamp": 1.5474328581594723
+        },
+        {
+            "x": 2.5026677503931767,
+            "y": 2.3827944816766236,
+            "heading": -0.20403633414457553,
+            "angularVelocity": -1.0410739741365005,
+            "velocityX": 3.1724110164298485,
+            "velocityY": -0.859654861819398,
+            "timestamp": 1.5783906508843786
+        },
+        {
+            "x": 2.6041682782784212,
+            "y": 2.363451815109364,
+            "heading": -0.23475758891885987,
+            "angularVelocity": -0.9923593405260972,
+            "velocityX": 3.2786745743372427,
+            "velocityY": -0.6248076773199155,
+            "timestamp": 1.6093484436092849
+        },
+        {
+            "x": 2.7086523080371263,
+            "y": 2.3514976196224144,
+            "heading": -0.26331031649970843,
+            "angularVelocity": -0.9223114784903548,
+            "velocityX": 3.3750477847899405,
+            "velocityY": -0.38614495527174003,
+            "timestamp": 1.6403062363341911
+        },
+        {
+            "x": 2.8157215031828033,
+            "y": 2.3470577985799963,
+            "heading": -0.288841887845313,
+            "angularVelocity": -0.8247219551529203,
+            "velocityX": 3.458553912306755,
+            "velocityY": -0.14341529714262294,
+            "timestamp": 1.6712640290590974
+        },
+        {
+            "x": 2.9248447015312338,
+            "y": 2.3502540561946077,
+            "heading": -0.3102182612745416,
+            "angularVelocity": -0.6905005668986338,
+            "velocityX": 3.524902415281217,
+            "velocityY": 0.10324565588123534,
+            "timestamp": 1.7022218217840037
+        },
+        {
+            "x": 3.0353304312162184,
+            "y": 2.361176073397281,
+            "heading": -0.326082506948615,
+            "angularVelocity": -0.5124475706430824,
+            "velocityX": 3.568914963243171,
+            "velocityY": 0.3528034863374462,
+            "timestamp": 1.73317961450891
+        },
+        {
+            "x": 3.1466093544989446,
+            "y": 2.379927120590619,
+            "heading": -0.3369287204650216,
+            "angularVelocity": -0.35035487229182166,
+            "velocityX": 3.594536738181179,
+            "velocityY": 0.605697161937562,
+            "timestamp": 1.7641374072338163
+        },
+        {
+            "x": 3.2578769593015027,
+            "y": 2.406586601459548,
+            "heading": -0.34366338052401463,
+            "angularVelocity": -0.21754328930658381,
+            "velocityX": 3.5941711281316278,
+            "velocityY": 0.8611557389062998,
+            "timestamp": 1.7950951999587226
+        },
+        {
+            "x": 3.3679824864036325,
+            "y": 2.4411213005139327,
+            "heading": -0.34790283328354016,
+            "angularVelocity": -0.13694299193573645,
+            "velocityX": 3.556633642474382,
+            "velocityY": 1.1155413876329803,
+            "timestamp": 1.8260529926836289
+        },
+        {
+            "x": 3.475304114496225,
+            "y": 2.4831460278092354,
+            "heading": -0.3524511595124565,
+            "angularVelocity": -0.14692023650373393,
+            "velocityX": 3.466708012625811,
+            "velocityY": 1.3574846136083376,
+            "timestamp": 1.8570107854085351
+        },
+        {
+            "x": 3.5778542240444473,
+            "y": 2.531447362567012,
+            "heading": -0.3616437420636369,
+            "angularVelocity": -0.29693921116020117,
+            "velocityX": 3.312578207999158,
+            "velocityY": 1.560231867531513,
+            "timestamp": 1.8879685781334414
+        },
+        {
+            "x": 3.673558919058804,
+            "y": 2.583451056106446,
+            "heading": -0.37745622232401294,
+            "angularVelocity": -0.5107754419475534,
+            "velocityX": 3.091457322713648,
+            "velocityY": 1.6798256258655577,
+            "timestamp": 1.9189263708583477
+        },
+        {
+            "x": 3.7613706261508923,
+            "y": 2.636186152260406,
+            "heading": -0.3968957996596081,
+            "angularVelocity": -0.6279380932955558,
+            "velocityX": 2.836497675157173,
+            "velocityY": 1.7034514257095354,
+            "timestamp": 1.949884163583254
+        },
+        {
+            "x": 3.841306735175595,
+            "y": 2.6876448087218088,
+            "heading": -0.4179459699473055,
+            "angularVelocity": -0.6799635386030257,
+            "velocityX": 2.582099755467769,
+            "velocityY": 1.6622198138851267,
+            "timestamp": 1.9808419563081603
+        },
+        {
+            "x": 3.913645075614716,
+            "y": 2.736629100397619,
+            "heading": -0.43934657697326235,
+            "angularVelocity": -0.6912833617387507,
+            "velocityX": 2.336676296079001,
+            "velocityY": 1.582292772328868,
+            "timestamp": 2.0117997490330666
+        },
+        {
+            "x": 3.9786691618750996,
+            "y": 2.7823974078733573,
+            "heading": -0.46027689817629036,
+            "angularVelocity": -0.6760921681319296,
+            "velocityX": 2.100410931692797,
+            "velocityY": 1.4784099074015198,
+            "timestamp": 2.0427575417579726
+        },
+        {
+            "x": 4.036618763930541,
+            "y": 2.824460782764613,
+            "heading": -0.48017689659950147,
+            "angularVelocity": -0.6428106357887643,
+            "velocityX": 1.8718906276836884,
+            "velocityY": 1.3587330099747585,
+            "timestamp": 2.0737153344828787
+        },
+        {
+            "x": 4.08768908254176,
+            "y": 2.862477844590781,
+            "heading": -0.49864492956893264,
+            "angularVelocity": -0.5965552238942878,
+            "velocityX": 1.6496757073449355,
+            "velocityY": 1.2280288250525657,
+            "timestamp": 2.1046731272077848
+        },
+        {
+            "x": 4.132038835388808,
+            "y": 2.8961988047734253,
+            "heading": -0.5153800748468587,
+            "angularVelocity": -0.5405794084679139,
+            "velocityX": 1.4325876925792385,
+            "velocityY": 1.0892559583398733,
+            "timestamp": 2.135630919932691
+        },
+        {
+            "x": 4.169798307734006,
+            "y": 2.9254338939938918,
+            "heading": -0.5301487101337384,
+            "angularVelocity": -0.47705711511170307,
+            "velocityX": 1.2197081581622198,
+            "velocityY": 0.944353154640467,
+            "timestamp": 2.166588712657597
+        },
+        {
+            "x": 4.201075770562755,
+            "y": 2.9500345091711524,
+            "heading": -0.5427642633485259,
+            "angularVelocity": -0.4075081620800149,
+            "velocityX": 1.0103259979397883,
+            "velocityY": 0.7946501675963605,
+            "timestamp": 2.197546505382503
+        },
+        {
+            "x": 4.225962326087245,
+            "y": 2.9698813733488647,
+            "heading": -0.5530743601373866,
+            "angularVelocity": -0.3330372058810695,
+            "velocityX": 0.8038866254348108,
+            "velocityY": 0.6410942909939014,
+            "timestamp": 2.228504298107409
+        },
+        {
+            "x": 4.24453552816763,
+            "y": 2.984876771487546,
+            "heading": -0.5609523103628415,
+            "angularVelocity": -0.2544738992160421,
+            "velocityX": 0.5999524011761236,
+            "velocityY": 0.48438201883216997,
+            "timestamp": 2.259462090832315
+        },
+        {
+            "x": 4.256862100646349,
+            "y": 2.9949392682905573,
+            "heading": -0.5662912533122896,
+            "angularVelocity": -0.17245877305093363,
+            "velocityX": 0.398173493448939,
+            "velocityY": 0.32503922008988784,
+            "timestamp": 2.290419883557221
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.569,
+            "angularVelocity": -0.08749805620543083,
+            "velocityX": 0.19826669841112912,
+            "velocityY": 0.16347198117160036,
+            "timestamp": 2.321377676282127
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.569,
+            "angularVelocity": 1.3974941712649426e-29,
+            "velocityX": 2.480011116993708e-30,
+            "velocityY": -4.1979337006948246e-30,
+            "timestamp": 2.3523354690070333
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W1 To S4.traj b/src/main/deploy/choreo/W1 To S4.traj
new file mode 100644
index 00000000..1ef15c59
--- /dev/null
+++ b/src/main/deploy/choreo/W1 To S4.traj	
@@ -0,0 +1,562 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 9.203343680058695e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 2.298008161759761,
+            "y": 6.742798044155305,
+            "heading": 0.37730350882287184,
+            "angularVelocity": -0.30162955292479776,
+            "velocityX": -0.04731994607959398,
+            "velocityY": -0.3373947598852993,
+            "timestamp": 0.0420929947149233
+        },
+        {
+            "x": 2.2940179124306126,
+            "y": 6.714368498753073,
+            "heading": 0.3522801711626307,
+            "angularVelocity": -0.5944774856175671,
+            "velocityX": -0.09479604281359731,
+            "velocityY": -0.6753984978919232,
+            "timestamp": 0.0841859894298466
+        },
+        {
+            "x": 2.28802134748838,
+            "y": 6.671681118579196,
+            "heading": 0.31538217591497186,
+            "angularVelocity": -0.8765828019021266,
+            "velocityX": -0.14245992671333493,
+            "velocityY": -1.014120769096589,
+            "timestamp": 0.1262789841447699
+        },
+        {
+            "x": 2.2800080756588827,
+            "y": 6.61469814393844,
+            "heading": 0.2672110996394858,
+            "angularVelocity": -1.1443965106718308,
+            "velocityX": -0.19037067530517543,
+            "velocityY": -1.3537400944426796,
+            "timestamp": 0.1683719788596932
+        },
+        {
+            "x": 2.2699650128963897,
+            "y": 6.543370292056319,
+            "heading": 0.20862519634351256,
+            "angularVelocity": -1.391820745773707,
+            "velocityX": -0.23859226055333851,
+            "velocityY": -1.694530226827328,
+            "timestamp": 0.21046497357461652
+        },
+        {
+            "x": 2.2578780077181495,
+            "y": 6.4576319980717,
+            "heading": 0.14091301714440063,
+            "angularVelocity": -1.608632972248606,
+            "velocityX": -0.2871500414760241,
+            "velocityY": -2.036877978516017,
+            "timestamp": 0.2525579682895398
+        },
+        {
+            "x": 2.2437361567314555,
+            "y": 6.357398182016401,
+            "heading": 0.06608849726258184,
+            "angularVelocity": -1.7776002964049293,
+            "velocityX": -0.3359668534508049,
+            "velocityY": -2.3812469683883926,
+            "timestamp": 0.2946509630044631
+        },
+        {
+            "x": 2.227539619778089,
+            "y": 6.242572554004172,
+            "heading": -0.012517898946656567,
+            "angularVelocity": -1.8674460380308828,
+            "velocityX": -0.3847798680768185,
+            "velocityY": -2.727903509595609,
+            "timestamp": 0.33674395771938637
+        },
+        {
+            "x": 2.209319968778144,
+            "y": 6.1131309245806085,
+            "heading": -0.08849823738903302,
+            "angularVelocity": -1.8050589880087344,
+            "velocityX": -0.4328428310539146,
+            "velocityY": -3.0751347177888695,
+            "timestamp": 0.37883695243430965
+        },
+        {
+            "x": 2.189363625025874,
+            "y": 5.970570114356374,
+            "heading": -0.13907359717832485,
+            "angularVelocity": -1.201514887021361,
+            "velocityX": -0.4741013056311528,
+            "velocityY": -3.38680607520881,
+            "timestamp": 0.42092994714923293
+        },
+        {
+            "x": 2.1674827409245396,
+            "y": 5.816618965425972,
+            "heading": -0.15173248317406665,
+            "angularVelocity": -0.3007361695568274,
+            "velocityX": -0.5198224609468644,
+            "velocityY": -3.657405465518488,
+            "timestamp": 0.4630229418641562
+        },
+        {
+            "x": 2.1445157307124236,
+            "y": 5.659048701686038,
+            "heading": -0.1517325271793822,
+            "angularVelocity": -0.0000010454308573871989,
+            "velocityX": -0.5456254744443967,
+            "velocityY": -3.7433844944292183,
+            "timestamp": 0.5051159365790795
+        },
+        {
+            "x": 2.1215487153797565,
+            "y": 5.501478438692377,
+            "heading": -0.1517325711844689,
+            "angularVelocity": -0.0000010454254199880688,
+            "velocityX": -0.5456255960929445,
+            "velocityY": -3.7433844767000393,
+            "timestamp": 0.5472089312940028
+        },
+        {
+            "x": 2.0985817251133763,
+            "y": 5.343908172045118,
+            "heading": -0.15173261518953962,
+            "angularVelocity": -0.000001045425040533604,
+            "velocityX": -0.5456250005951166,
+            "velocityY": -3.7433845634983034,
+            "timestamp": 0.5893019260089262
+        },
+        {
+            "x": 2.075614762728362,
+            "y": 5.186337901333946,
+            "heading": -0.15173265919459192,
+            "angularVelocity": -0.0000010454246033549915,
+            "velocityX": -0.5456243382196686,
+            "velocityY": -3.743384660044355,
+            "timestamp": 0.6313949207238495
+        },
+        {
+            "x": 2.0526478312972185,
+            "y": 5.028767626111028,
+            "heading": -0.15173270319962293,
+            "angularVelocity": -0.0000010454240970480152,
+            "velocityX": -0.5456236028509798,
+            "velocityY": -3.7433847672295513,
+            "timestamp": 0.6734879154387728
+        },
+        {
+            "x": 2.0296809339445536,
+            "y": 4.871197345920939,
+            "heading": -0.15173274720462945,
+            "angularVelocity": -0.0000010454235162635837,
+            "velocityX": -0.5456227932512114,
+            "velocityY": -3.743384885234266,
+            "timestamp": 0.7155809101536962
+        },
+        {
+            "x": 2.0067140734575584,
+            "y": 4.7136270603574335,
+            "heading": -0.15173279120960867,
+            "angularVelocity": -0.0000010454228670132247,
+            "velocityX": -0.5456219174363699,
+            "velocityY": -3.7433850128900636,
+            "timestamp": 0.7576739048686195
+        },
+        {
+            "x": 1.983747251812091,
+            "y": 4.55605676913253,
+            "heading": -0.15173283521455908,
+            "angularVelocity": -0.000001045422182833939,
+            "velocityX": -0.545620994681206,
+            "velocityY": -3.743385147387475,
+            "timestamp": 0.7997668995835429
+        },
+        {
+            "x": 1.9607804702256346,
+            "y": 4.398486472068785,
+            "heading": -0.15173287921948736,
+            "angularVelocity": -0.0000010454216570158517,
+            "velocityX": -0.5456200430024101,
+            "velocityY": -3.7433852861004024,
+            "timestamp": 0.8418598942984662
+        },
+        {
+            "x": 1.9378163633559966,
+            "y": 4.240915785172817,
+            "heading": -0.15173292322446258,
+            "angularVelocity": -0.0000010454227717672262,
+            "velocityX": -0.5455564999630786,
+            "velocityY": -3.7433945473141725,
+            "timestamp": 0.8839528890133895
+        },
+        {
+            "x": 1.9275487572437193,
+            "y": 4.082011890541997,
+            "heading": -0.15173296776197548,
+            "angularVelocity": -0.000001058074228463605,
+            "velocityX": -0.2439267194414494,
+            "velocityY": -3.775067459728259,
+            "timestamp": 0.9260458837283129
+        },
+        {
+            "x": 1.9320519221515333,
+            "y": 3.9228403083205765,
+            "heading": -0.15173301420750987,
+            "angularVelocity": -0.0000011034029462076059,
+            "velocityX": 0.10698133830372927,
+            "velocityY": -3.781426892988182,
+            "timestamp": 0.9681388784432362
+        },
+        {
+            "x": 1.9512874233679367,
+            "y": 3.764771127717043,
+            "heading": -0.15173306433513473,
+            "angularVelocity": -0.0000011908780826817618,
+            "velocityX": 0.45697630559851177,
+            "velocityY": -3.7552372235348,
+            "timestamp": 1.0102318731581594
+        },
+        {
+            "x": 1.9850897622274482,
+            "y": 3.609164986787025,
+            "heading": -0.15173312063283023,
+            "angularVelocity": -0.0000013374599716310608,
+            "velocityX": 0.8030395339756486,
+            "velocityY": -3.6967229816711127,
+            "timestamp": 1.0523248678730828
+        },
+        {
+            "x": 2.0331680016062417,
+            "y": 3.4573613296965586,
+            "heading": -0.1517331870158403,
+            "angularVelocity": -0.0000015770560044658274,
+            "velocityX": 1.142190991741159,
+            "velocityY": -3.606387668982997,
+            "timestamp": 1.094417862588006
+        },
+        {
+            "x": 2.0951083019579966,
+            "y": 3.3106668726225337,
+            "heading": -0.15173327041919607,
+            "angularVelocity": -0.0000019814070334134687,
+            "velocityX": 1.4715108956074094,
+            "velocityY": -3.4850088017619067,
+            "timestamp": 1.1365108573029294
+        },
+        {
+            "x": 2.170377492904663,
+            "y": 3.170344352722168,
+            "heading": -0.1517333779044128,
+            "angularVelocity": -0.000002553517929494838,
+            "velocityX": 1.7881643122906952,
+            "velocityY": -3.333631186155945,
+            "timestamp": 1.1786038520178528
+        },
+        {
+            "x": 2.2421653916580504,
+            "y": 3.058317386598826,
+            "heading": -0.1517334835913584,
+            "angularVelocity": -0.0000030048345176781924,
+            "velocityX": 2.041035009880172,
+            "velocityY": -3.1850905776457417,
+            "timestamp": 1.213776153524627
+        },
+        {
+            "x": 2.322415894828655,
+            "y": 2.9521882028356305,
+            "heading": -0.15173356325163515,
+            "angularVelocity": -0.000002264858236060329,
+            "velocityX": 2.2816392369190814,
+            "velocityY": -3.0174079948323826,
+            "timestamp": 1.2489484550314012
+        },
+        {
+            "x": 2.4106466853387967,
+            "y": 2.8525946456824203,
+            "heading": -0.1517336276365959,
+            "angularVelocity": -0.0000018305586497777263,
+            "velocityX": 2.5085304836576707,
+            "velocityY": -2.8315905666289245,
+            "timestamp": 1.2841207565381754
+        },
+        {
+            "x": 2.5063274836460994,
+            "y": 2.7601352747554455,
+            "heading": -0.15173368243148194,
+            "angularVelocity": -0.0000015578987915956282,
+            "velocityX": 2.720345107040385,
+            "velocityY": -2.628755212654101,
+            "timestamp": 1.3192930580449496
+        },
+        {
+            "x": 2.6088832332920924,
+            "y": 2.67536576592715,
+            "heading": -0.15173373101035148,
+            "angularVelocity": -0.000001381168347773627,
+            "velocityX": 2.915810033819959,
+            "velocityY": -2.410121180496788,
+            "timestamp": 1.3544653595517238
+        },
+        {
+            "x": 2.7176975542058304,
+            "y": 2.598795565150263,
+            "heading": -0.1517337755722331,
+            "angularVelocity": -0.0000012669595030854418,
+            "velocityX": 3.0937503732242226,
+            "velocityY": -2.1770028544233733,
+            "timestamp": 1.389637661058498
+        },
+        {
+            "x": 2.8321164386506434,
+            "y": 2.5308848045360426,
+            "heading": -0.15173381767917685,
+            "angularVelocity": -0.0000011971620259359605,
+            "velocityX": 3.2530963156555406,
+            "velocityY": -1.930802299108592,
+            "timestamp": 1.4248099625652721
+        },
+        {
+            "x": 2.951452140465328,
+            "y": 2.4720414272198683,
+            "heading": -0.15173385854226368,
+            "angularVelocity": -0.0000011617973536146762,
+            "velocityX": 3.3928886283344455,
+            "velocityY": -1.6730033235055968,
+            "timestamp": 1.4599822640720463
+        },
+        {
+            "x": 3.0749866313948626,
+            "y": 2.4226169148084327,
+            "heading": -0.151733899192453,
+            "angularVelocity": -0.000001155744366153098,
+            "velocityX": 3.512266347021429,
+            "velocityY": -1.4052112114959712,
+            "timestamp": 1.4951545655788205
+        },
+        {
+            "x": 3.1992028583958065,
+            "y": 2.3749316777596947,
+            "heading": -0.1517339398689439,
+            "angularVelocity": -0.0000011564921588237654,
+            "velocityX": 3.5316491011263524,
+            "velocityY": -1.3557610678264387,
+            "timestamp": 1.5303268670855947
+        },
+        {
+            "x": 3.3234191450837622,
+            "y": 2.3272465961911335,
+            "heading": -0.15173398054544393,
+            "angularVelocity": -0.0000011564924185481092,
+            "velocityX": 3.5316507981154315,
+            "velocityY": -1.355756647297499,
+            "timestamp": 1.565499168592369
+        },
+        {
+            "x": 3.4476354415156054,
+            "y": 2.2795615400047207,
+            "heading": -0.15173402122195195,
+            "angularVelocity": -0.0000011564926460273957,
+            "velocityX": 3.5316510751484063,
+            "velocityY": -1.3557559256458895,
+            "timestamp": 1.6006714700991431
+        },
+        {
+            "x": 3.57185174767356,
+            "y": 2.231876509154167,
+            "heading": -0.15173406189847174,
+            "angularVelocity": -0.0000011564929795008858,
+            "velocityX": 3.531651351675977,
+            "velocityY": -1.355755205310334,
+            "timestamp": 1.6358437716059173
+        },
+        {
+            "x": 3.6960680635749434,
+            "y": 2.1841915036846027,
+            "heading": -0.15173410257500647,
+            "angularVelocity": -0.000001156493404477995,
+            "velocityX": 3.5316516286959314,
+            "velocityY": -1.3557544836916666,
+            "timestamp": 1.6710160731126915
+        },
+        {
+            "x": 3.820284389246338,
+            "y": 2.136506523665285,
+            "heading": -0.15173414325155568,
+            "angularVelocity": -0.0000011564938164652249,
+            "velocityX": 3.5316519064716276,
+            "velocityY": -1.3557537601039011,
+            "timestamp": 1.7061883746194657
+        },
+        {
+            "x": 3.9445007247793735,
+            "y": 2.0888215693349235,
+            "heading": -0.15173418392811813,
+            "angularVelocity": -0.0000011564941935950735,
+            "velocityX": 3.5316521868525257,
+            "velocityY": -1.3557530297293134,
+            "timestamp": 1.74136067612624
+        },
+        {
+            "x": 4.068717070394662,
+            "y": 2.0411366412682117,
+            "heading": -0.1517342246046924,
+            "angularVelocity": -0.0000011564945294151802,
+            "velocityX": 3.531652473505735,
+            "velocityY": -1.3557522830153055,
+            "timestamp": 1.776532977633014
+        },
+        {
+            "x": 4.192933426524707,
+            "y": 1.9934517405918115,
+            "heading": -0.15173426528127656,
+            "angularVelocity": -0.0000011564948108738022,
+            "velocityX": 3.5316527724556495,
+            "velocityY": -1.3557515042686648,
+            "timestamp": 1.8117052791397883
+        },
+        {
+            "x": 4.3171498402859925,
+            "y": 1.9457669900421575,
+            "heading": -0.1517343059583456,
+            "angularVelocity": -0.0000011565085966842365,
+            "velocityX": 3.531654410996122,
+            "velocityY": -1.3557472359455842,
+            "timestamp": 1.8468775806465625
+        },
+        {
+            "x": 4.438199866372425,
+            "y": 1.9006463371040783,
+            "heading": -0.16306820061092758,
+            "angularVelocity": -0.3222392100329015,
+            "velocityX": 3.441629375976964,
+            "velocityY": -1.282846188765579,
+            "timestamp": 1.8820498821533367
+        },
+        {
+            "x": 4.551333178258229,
+            "y": 1.8581105785728176,
+            "heading": -0.19901052601489952,
+            "angularVelocity": -1.0218929061849813,
+            "velocityX": 3.2165456066050515,
+            "velocityY": -1.2093538582645278,
+            "timestamp": 1.9172221836601109
+        },
+        {
+            "x": 4.6559660625940325,
+            "y": 1.8184646011809935,
+            "heading": -0.2548690023306322,
+            "angularVelocity": -1.5881382202121255,
+            "velocityX": 2.9748660125539748,
+            "velocityY": -1.1271931518097666,
+            "timestamp": 1.952394485166885
+        },
+        {
+            "x": 4.750968691238499,
+            "y": 1.7823676048635404,
+            "heading": -0.3144227593810797,
+            "angularVelocity": -1.693200458860429,
+            "velocityX": 2.7010637511499986,
+            "velocityY": -1.0262904265875403,
+            "timestamp": 1.9875667866736593
+        },
+        {
+            "x": 4.836310937621718,
+            "y": 1.749878315803868,
+            "heading": -0.37309459063768413,
+            "angularVelocity": -1.6681260180060764,
+            "velocityX": 2.426404947278819,
+            "velocityY": -0.9237180300360781,
+            "timestamp": 2.0227390881804337
+        },
+        {
+            "x": 4.912035358987092,
+            "y": 1.7210053742684595,
+            "heading": -0.4285254423369764,
+            "angularVelocity": -1.5759802266171352,
+            "velocityX": 2.1529561081121917,
+            "velocityY": -0.8208999780650579,
+            "timestamp": 2.057911389687208
+        },
+        {
+            "x": 4.978188578205751,
+            "y": 1.6957473341798728,
+            "heading": -0.4792367761737124,
+            "angularVelocity": -1.4417974276425698,
+            "velocityX": 1.8808328253957978,
+            "velocityY": -0.71812304019178,
+            "timestamp": 2.0930836911939825
+        },
+        {
+            "x": 5.034811621143831,
+            "y": 1.6741013653395114,
+            "heading": -0.5242175587960242,
+            "angularVelocity": -1.278869471013956,
+            "velocityX": 1.6098759680874073,
+            "velocityY": -0.6154265690060781,
+            "timestamp": 2.128255992700757
+        },
+        {
+            "x": 5.081938937252143,
+            "y": 1.656065298518901,
+            "heading": -0.5627406592667319,
+            "angularVelocity": -1.0952681178195878,
+            "velocityX": 1.3398985590759445,
+            "velocityY": -0.5127917721601607,
+            "timestamp": 2.1634282942075314
+        },
+        {
+            "x": 5.11959917834615,
+            "y": 1.6416378108353973,
+            "heading": -0.5942629145071194,
+            "angularVelocity": -0.8962238434785516,
+            "velocityX": 1.0707357631047747,
+            "velocityY": -0.41019458680362747,
+            "timestamp": 2.1986005957143058
+        },
+        {
+            "x": 5.147816310703936,
+            "y": 1.63081813815951,
+            "heading": -0.6183637008514038,
+            "angularVelocity": -0.6852206228143011,
+            "velocityX": 0.8022543634897267,
+            "velocityY": -0.3076191267666608,
+            "timestamp": 2.23377289722108
+        },
+        {
+            "x": 5.1666106911988186,
+            "y": 1.6236057376407542,
+            "heading": -0.6347043261398958,
+            "angularVelocity": -0.4645878884367218,
+            "velocityX": 0.5343517395716112,
+            "velocityY": -0.20505910076332604,
+            "timestamp": 2.2689451987278546
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.643,
+            "angularVelocity": -0.23585814702817923,
+            "velocityX": 0.26695178873562564,
+            "velocityY": -0.10251639745723456,
+            "timestamp": 2.304117500234629
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.643,
+            "angularVelocity": -2.979902278450704e-33,
+            "velocityX": -7.596111021745797e-35,
+            "velocityY": -9.748244176889607e-35,
+            "timestamp": 2.3392898017414034
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W1 To W2.traj b/src/main/deploy/choreo/W1 To W2.traj
new file mode 100644
index 00000000..a5fd0bb0
--- /dev/null
+++ b/src/main/deploy/choreo/W1 To W2.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 1.792103156985809e-33,
+            "velocityX": -7.27073415334495e-34,
+            "velocityY": -7.853383039035825e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 2.298001341213542,
+            "y": 6.709118496985434,
+            "heading": 0.374359095500822,
+            "angularVelocity": -0.2058832051525312,
+            "velocityX": -0.02630859852023658,
+            "velocityY": -0.6302702831973344,
+            "timestamp": 0.07596979310473732
+        },
+        {
+            "x": 2.294004039374054,
+            "y": 6.613355753676287,
+            "heading": 0.3430620070158405,
+            "angularVelocity": -0.4119675361210102,
+            "velocityX": -0.0526169899393711,
+            "velocityY": -1.2605371081782701,
+            "timestamp": 0.15193958620947465
+        },
+        {
+            "x": 2.2880080040813597,
+            "y": 6.469712250400375,
+            "heading": 0.296082691458658,
+            "angularVelocity": -0.6183946755312791,
+            "velocityX": -0.07892657130746197,
+            "velocityY": -1.8907976105433808,
+            "timestamp": 0.22790937931421196
+        },
+        {
+            "x": 2.280012894319733,
+            "y": 6.278188663497132,
+            "heading": 0.23338596481601495,
+            "angularVelocity": -0.8252849465603376,
+            "velocityX": -0.10524064150870725,
+            "velocityY": -2.5210492101669324,
+            "timestamp": 0.3038791724189493
+        },
+        {
+            "x": 2.270017931664709,
+            "y": 6.038785816229407,
+            "heading": 0.1549313176962556,
+            "angularVelocity": -1.0327084478378958,
+            "velocityX": -0.13156495820969516,
+            "velocityY": -3.1512899730774246,
+            "timestamp": 0.37984896552368663
+        },
+        {
+            "x": 2.2620113109689415,
+            "y": 5.847271344563018,
+            "heading": 0.09176539516362914,
+            "angularVelocity": -0.8314610314331311,
+            "velocityX": -0.10539216139142096,
+            "velocityY": -2.5209292251507525,
+            "timestamp": 0.45581875862842397
+        },
+        {
+            "x": 2.2560057963370115,
+            "y": 5.703635646814395,
+            "heading": 0.0443838554377995,
+            "angularVelocity": -0.6236892031613949,
+            "velocityX": -0.079051349049357,
+            "velocityY": -1.8906948653999391,
+            "timestamp": 0.5317885517331613
+        },
+        {
+            "x": 2.252001935551058,
+            "y": 5.607878550007709,
+            "heading": 0.012794605457377246,
+            "angularVelocity": -0.4158132948561684,
+            "velocityX": -0.05270332618167983,
+            "velocityY": -1.2604627825518706,
+            "timestamp": 0.6077583448378986
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -0.003,
+            "angularVelocity": -0.20790639031492017,
+            "velocityX": -0.026351730987313093,
+            "velocityY": -0.6302314123944018,
+            "timestamp": 0.6837281379426359
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -0.003,
+            "angularVelocity": -6.008173298568887e-34,
+            "velocityX": -7.305982513747354e-33,
+            "velocityY": -3.8813573012664696e-35,
+            "timestamp": 0.7596979310473732
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W1 To W3.traj b/src/main/deploy/choreo/W1 To W3.traj
new file mode 100644
index 00000000..5aab1ee6
--- /dev/null
+++ b/src/main/deploy/choreo/W1 To W3.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": -2.4663146867319515e-36,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3001378208847427,
+            "y": 6.713171893956039,
+            "heading": 0.36291265694481545,
+            "angularVelocity": -0.37089801282350154,
+            "velocityX": 0.0018871357066083216,
+            "velocityY": -0.6001237332286558,
+            "timestamp": 0.0730317826428328
+        },
+        {
+            "x": 2.3004082222445,
+            "y": 6.625427179140099,
+            "heading": 0.3106389081779897,
+            "angularVelocity": -0.7157671204942956,
+            "velocityX": 0.0037025162192713427,
+            "velocityY": -1.201459304985902,
+            "timestamp": 0.1460635652856656
+        },
+        {
+            "x": 2.300808308388558,
+            "y": 6.493639269598709,
+            "heading": 0.23621859479744584,
+            "angularVelocity": -1.019012691289514,
+            "velocityX": 0.005478246998500719,
+            "velocityY": -1.804528176258669,
+            "timestamp": 0.2190953479284984
+        },
+        {
+            "x": 2.30134752968001,
+            "y": 6.317621999647275,
+            "heading": 0.14515321533996162,
+            "angularVelocity": -1.2469280655909227,
+            "velocityX": 0.007383378468093947,
+            "velocityY": -2.4101461525629078,
+            "timestamp": 0.2921271305713312
+        },
+        {
+            "x": 2.3020704977057354,
+            "y": 6.097150895386178,
+            "heading": 0.04952404086318575,
+            "angularVelocity": -1.309418598536164,
+            "velocityX": 0.00989936161439861,
+            "velocityY": -3.0188377755932185,
+            "timestamp": 0.36515891321416405
+        },
+        {
+            "x": 2.3031517302727234,
+            "y": 5.834309739739576,
+            "heading": 0.004115026920641748,
+            "angularVelocity": -0.6217705812361183,
+            "velocityX": 0.014804959263771778,
+            "velocityY": -3.598996849522972,
+            "timestamp": 0.4381906958569969
+        },
+        {
+            "x": 2.303521267718626,
+            "y": 5.558035105167069,
+            "heading": 0.00411499765356137,
+            "angularVelocity": -4.007444337013033e-7,
+            "velocityX": 0.005059953797233799,
+            "velocityY": -3.7829370251531125,
+            "timestamp": 0.5112224784998297
+        },
+        {
+            "x": 2.3038907744757537,
+            "y": 5.281760470553516,
+            "heading": 0.004114968386478979,
+            "angularVelocity": -4.0074446127092794e-7,
+            "velocityX": 0.005059533586019425,
+            "velocityY": -3.782937025715139,
+            "timestamp": 0.5842542611426625
+        },
+        {
+            "x": 2.3020058670727765,
+            "y": 5.019317688425813,
+            "heading": -0.04121766182368216,
+            "angularVelocity": -0.6207246840990261,
+            "velocityX": -0.02580941248819387,
+            "velocityY": -3.5935420529332935,
+            "timestamp": 0.6572860437854954
+        },
+        {
+            "x": 2.301170882315387,
+            "y": 4.798980955675817,
+            "heading": -0.139726223563735,
+            "angularVelocity": -1.348845094221732,
+            "velocityX": -0.011433169603334988,
+            "velocityY": -3.016997871016921,
+            "timestamp": 0.7303178264283282
+        },
+        {
+            "x": 2.3006513716532107,
+            "y": 4.623098256624685,
+            "heading": -0.23378951684706242,
+            "angularVelocity": -1.2879775062228829,
+            "velocityX": -0.007113487352719777,
+            "velocityY": -2.4083035178163246,
+            "timestamp": 0.803349609071161
+        },
+        {
+            "x": 2.300310703422249,
+            "y": 4.491429967185545,
+            "heading": -0.31073488166813007,
+            "angularVelocity": -1.0535873839664371,
+            "velocityX": -0.004664657203118652,
+            "velocityY": -1.8028902578357346,
+            "timestamp": 0.8763813917139939
+        },
+        {
+            "x": 2.3001003821722423,
+            "y": 4.403776446553788,
+            "heading": -0.36487144716496317,
+            "angularVelocity": -0.7412740527174575,
+            "velocityX": -0.0028798591845282696,
+            "velocityY": -1.2002106132398922,
+            "timestamp": 0.9494131743568267
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.393,
+            "angularVelocity": -0.3851549533249317,
+            "velocityX": -0.0013744998219966374,
+            "velocityY": -0.5994163769475577,
+            "timestamp": 1.0224449569996594
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.393,
+            "angularVelocity": -7.841220088894993e-36,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 1.0954767396424923
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W2 To C1.traj b/src/main/deploy/choreo/W2 To C1.traj
new file mode 100644
index 00000000..70287ec9
--- /dev/null
+++ b/src/main/deploy/choreo/W2 To C1.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -3.731399929679607e-32,
+            "velocityY": 5.877471754111438e-38,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3271275380851444,
+            "y": 5.587086964112038,
+            "heading": 3.953847862678034e-18,
+            "angularVelocity": 3.9932078666702264e-17,
+            "velocityX": 0.7789533333445218,
+            "velocityY": 0.2735661154121556,
+            "timestamp": 0.09901432445765257
+        },
+        {
+            "x": 2.4813826113143547,
+            "y": 5.641260891303218,
+            "heading": 1.1398162294527873e-19,
+            "angularVelocity": -3.8780916638560175e-17,
+            "velocityX": 1.557906636985473,
+            "velocityY": 0.5471322203925046,
+            "timestamp": 0.19802864891530514
+        },
+        {
+            "x": 2.7127652138054734,
+            "y": 5.722521779507741,
+            "heading": 2.1553394674505063e-18,
+            "angularVelocity": 2.0616792512846884e-17,
+            "velocityX": 2.336859881219295,
+            "velocityY": 0.8206983045092451,
+            "timestamp": 0.2970429733729577
+        },
+        {
+            "x": 3.0212753279120443,
+            "y": 5.830869622528225,
+            "heading": -1.0672476097054788e-17,
+            "angularVelocity": -1.295551486240031e-16,
+            "velocityX": 3.1158129472318716,
+            "velocityY": 1.0942643260352094,
+            "timestamp": 0.3960572978306103
+        },
+        {
+            "x": 3.3746798137461993,
+            "y": 5.9549842366139965,
+            "heading": 1.8895880094915182e-18,
+            "angularVelocity": 1.2687117924961395e-16,
+            "velocityX": 3.5692258445423475,
+            "velocityY": 1.2535015995473786,
+            "timestamp": 0.49507162228826285
+        },
+        {
+            "x": 3.7280842995803773,
+            "y": 6.079098850699775,
+            "heading": 1.0049319654091473e-17,
+            "angularVelocity": 8.240960778697987e-17,
+            "velocityX": 3.569225844542579,
+            "velocityY": 1.25350159954746,
+            "timestamp": 0.5940859467459154
+        },
+        {
+            "x": 4.081488785414555,
+            "y": 6.203213464785553,
+            "heading": 8.136834826577652e-18,
+            "angularVelocity": -1.931523381075779e-17,
+            "velocityX": 3.569225844542579,
+            "velocityY": 1.25350159954746,
+            "timestamp": 0.693100271203568
+        },
+        {
+            "x": 4.434893271248733,
+            "y": 6.327328078871332,
+            "heading": -2.4381204969164037e-18,
+            "angularVelocity": -1.0680227715899811e-16,
+            "velocityX": 3.5692258445425793,
+            "velocityY": 1.2535015995474599,
+            "timestamp": 0.7921145956612206
+        },
+        {
+            "x": 4.788297757082911,
+            "y": 6.4514426929571105,
+            "heading": 3.192038150785043e-17,
+            "angularVelocity": 3.47005367081625e-16,
+            "velocityX": 3.569225844542579,
+            "velocityY": 1.25350159954746,
+            "timestamp": 0.8911289201188731
+        },
+        {
+            "x": 5.141702242917089,
+            "y": 6.575557307042889,
+            "heading": 6.016026907559514e-17,
+            "angularVelocity": 2.8521012209839933e-16,
+            "velocityX": 3.569225844542579,
+            "velocityY": 1.2535015995474599,
+            "timestamp": 0.9901432445765257
+        },
+        {
+            "x": 5.495106728751266,
+            "y": 6.699671921128667,
+            "heading": 2.4186102644429564e-17,
+            "angularVelocity": -3.63322848771759e-16,
+            "velocityX": 3.569225844542579,
+            "velocityY": 1.2535015995474599,
+            "timestamp": 1.0891575690341782
+        },
+        {
+            "x": 5.848511214585444,
+            "y": 6.823786535214446,
+            "heading": -1.2421929874703125e-18,
+            "angularVelocity": -2.568143121812034e-16,
+            "velocityX": 3.5692258445425793,
+            "velocityY": 1.2535015995474599,
+            "timestamp": 1.1881718934918308
+        },
+        {
+            "x": 6.2019157004196215,
+            "y": 6.9479011493002245,
+            "heading": 2.3184212364315377e-18,
+            "angularVelocity": 3.596059704901295e-17,
+            "velocityX": 3.569225844542579,
+            "velocityY": 1.2535015995474599,
+            "timestamp": 1.2871862179494835
+        },
+        {
+            "x": 6.5553201862538,
+            "y": 7.072015763386003,
+            "heading": 3.1459219033425486e-17,
+            "angularVelocity": 2.9430890890068574e-16,
+            "velocityX": 3.569225844542579,
+            "velocityY": 1.25350159954746,
+            "timestamp": 1.3862005424071362
+        },
+        {
+            "x": 6.9087246720879545,
+            "y": 7.196130377471773,
+            "heading": 2.9741502939674576e-17,
+            "angularVelocity": -1.7348157482865667e-17,
+            "velocityX": 3.5692258445423475,
+            "velocityY": 1.2535015995473786,
+            "timestamp": 1.4852148668647889
+        },
+        {
+            "x": 7.217234786194527,
+            "y": 7.304478220492258,
+            "heading": 1.2383304928032073e-17,
+            "angularVelocity": -1.753099667759744e-16,
+            "velocityX": 3.115812947231872,
+            "velocityY": 1.0942643260352094,
+            "timestamp": 1.5842291913224416
+        },
+        {
+            "x": 7.448617388685645,
+            "y": 7.385739108696781,
+            "heading": -3.803725574168796e-18,
+            "angularVelocity": -1.6348170333424006e-16,
+            "velocityX": 2.336859881219295,
+            "velocityY": 0.8206983045092451,
+            "timestamp": 1.6832435157800942
+        },
+        {
+            "x": 7.602872461914855,
+            "y": 7.439913035887961,
+            "heading": -2.4779119751079125e-18,
+            "angularVelocity": 1.3390118812558602e-17,
+            "velocityX": 1.557906636985473,
+            "velocityY": 0.5471322203925049,
+            "timestamp": 1.782257840237747
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 2.5025792615041696e-17,
+            "velocityX": 0.7789533333445218,
+            "velocityY": 0.27356611541215564,
+            "timestamp": 1.8812721646953996
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 1.3533974507170236e-34,
+            "velocityY": 0,
+            "timestamp": 1.9802864891530523
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W2 To C2.traj b/src/main/deploy/choreo/W2 To C2.traj
new file mode 100644
index 00000000..168ae5bd
--- /dev/null
+++ b/src/main/deploy/choreo/W2 To C2.traj	
@@ -0,0 +1,526 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -1.058013890027075e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 2.26040474690827,
+            "y": 5.564834298284281,
+            "heading": -0.00020682462174428096,
+            "angularVelocity": -0.005575654083684234,
+            "velocityX": 0.28049498700644215,
+            "velocityY": 0.1303247879443207,
+            "timestamp": 0.037094234800108185
+        },
+        {
+            "x": 2.2812142405469196,
+            "y": 5.574502894544252,
+            "heading": -0.0006204719876615776,
+            "angularVelocity": -0.011151257551113,
+            "velocityX": 0.5609899692172368,
+            "velocityY": 0.26064956756953994,
+            "timestamp": 0.07418846960021637
+        },
+        {
+            "x": 2.312428480686622,
+            "y": 5.589005788443611,
+            "heading": -0.0012409398807264826,
+            "angularVelocity": -0.016726801251156914,
+            "velocityX": 0.8414849452457597,
+            "velocityY": 0.39097433812857635,
+            "timestamp": 0.11128270440032456
+        },
+        {
+            "x": 2.3540474670292637,
+            "y": 5.608342979609529,
+            "heading": -0.002068225636714691,
+            "angularVelocity": -0.022302273128063925,
+            "velocityX": 1.1219799132376411,
+            "velocityY": 0.5212990986367875,
+            "timestamp": 0.14837693920043274
+        },
+        {
+            "x": 2.406071199181798,
+            "y": 5.632514467619691,
+            "heading": -0.0031023259847413323,
+            "angularVelocity": -0.02787765682725579,
+            "velocityX": 1.4024748706336019,
+            "velocityY": 0.6516238477600773,
+            "timestamp": 0.18547117400054092
+        },
+        {
+            "x": 2.4684996766150022,
+            "y": 5.66152025198224,
+            "heading": -0.004343236799506005,
+            "angularVelocity": -0.03345292931508493,
+            "velocityX": 1.682969813762594,
+            "velocityY": 0.7819485836236739,
+            "timestamp": 0.2225654088006491
+        },
+        {
+            "x": 2.5413328985945514,
+            "y": 5.69536033210275,
+            "heading": -0.005790952692459236,
+            "angularVelocity": -0.03902805653641911,
+            "velocityX": 1.9634647370954057,
+            "velocityY": 0.9122733034625292,
+            "timestamp": 0.2596596436007573
+        },
+        {
+            "x": 2.6245708640566887,
+            "y": 5.7340347072253275,
+            "heading": -0.00744546628275827,
+            "angularVelocity": -0.04460298478228464,
+            "velocityX": 2.2439596317510366,
+            "velocityY": 1.0425980029237354,
+            "timestamp": 0.2967538784008655
+        },
+        {
+            "x": 2.7182135713591755,
+            "y": 5.777543376315584,
+            "heading": -0.009306766753500902,
+            "angularVelocity": -0.05017762142212937,
+            "velocityX": 2.5244544821346007,
+            "velocityY": 1.1729226744995158,
+            "timestamp": 0.3338481132009737
+        },
+        {
+            "x": 2.8222610176995655,
+            "y": 5.8258863377887575,
+            "heading": -0.011374836524793099,
+            "angularVelocity": -0.055751784136736884,
+            "velocityX": 2.8049492569688192,
+            "velocityY": 1.303247303352706,
+            "timestamp": 0.3709423480010819
+        },
+        {
+            "x": 2.9367131973687606,
+            "y": 5.879063588696104,
+            "heading": -0.013649641477510719,
+            "angularVelocity": -0.061325027055469855,
+            "velocityX": 3.0854438778950355,
+            "velocityY": 1.4335718527125696,
+            "timestamp": 0.4080365828011901
+        },
+        {
+            "x": 3.06157009301301,
+            "y": 5.937075120660461,
+            "heading": -0.01613108463550858,
+            "angularVelocity": -0.06689565565567898,
+            "velocityX": 3.3659380309924987,
+            "velocityY": 1.5638961762378367,
+            "timestamp": 0.4451308176012983
+        },
+        {
+            "x": 3.1888299168589427,
+            "y": 5.996203084762226,
+            "heading": -0.01613108529402469,
+            "angularVelocity": -1.7752519083825314e-8,
+            "velocityX": 3.430717051630934,
+            "velocityY": 1.5939933636693306,
+            "timestamp": 0.4822250524014065
+        },
+        {
+            "x": 3.316089744181463,
+            "y": 6.055331041381533,
+            "heading": -0.016131085952509703,
+            "angularVelocity": -1.7751680763659775e-8,
+            "velocityX": 3.4307171453540595,
+            "velocityY": 1.5939931619544851,
+            "timestamp": 0.5193192872015147
+        },
+        {
+            "x": 3.443349575355442,
+            "y": 6.114458989711434,
+            "heading": -0.01613108661098856,
+            "angularVelocity": -1.7751514817327164e-8,
+            "velocityX": 3.430717249183109,
+            "velocityY": 1.5939929384856546,
+            "timestamp": 0.5564135220016229
+        },
+        {
+            "x": 3.5706094108109987,
+            "y": 6.173586928826195,
+            "heading": -0.016131087269461084,
+            "angularVelocity": -1.7751344035751635e-8,
+            "velocityX": 3.4307173646074394,
+            "velocityY": 1.5939926900605101,
+            "timestamp": 0.5935077568017311
+        },
+        {
+            "x": 3.6978692510432434,
+            "y": 6.232714857660196,
+            "heading": -0.01613108792792704,
+            "angularVelocity": -1.7751166882414398e-8,
+            "velocityX": 3.4307174933791567,
+            "velocityY": 1.5939924129080143,
+            "timestamp": 0.6306019916018393
+        },
+        {
+            "x": 3.825129096624906,
+            "y": 6.291842774980762,
+            "heading": -0.016131088586386323,
+            "angularVelocity": -1.775098711225461e-8,
+            "velocityX": 3.430717637590709,
+            "velocityY": 1.5939921025246122,
+            "timestamp": 0.6676962264019475
+        },
+        {
+            "x": 3.952388948222243,
+            "y": 6.3509706793539165,
+            "heading": -0.016131089244838845,
+            "angularVelocity": -1.775080474504649e-8,
+            "velocityX": 3.4307177997634843,
+            "velocityY": 1.5939917534835466,
+            "timestamp": 0.7047904612020557
+        },
+        {
+            "x": 4.07964880661482,
+            "y": 6.410098569101814,
+            "heading": -0.016131089903284688,
+            "angularVelocity": -1.775062484471552e-8,
+            "velocityX": 3.430717982952055,
+            "velocityY": 1.5939913592104822,
+            "timestamp": 0.7418846960021639
+        },
+        {
+            "x": 4.206908672718448,
+            "y": 6.469226442253365,
+            "heading": -0.016131090561724272,
+            "angularVelocity": -1.7750456035932977e-8,
+            "velocityX": 3.4307181908294035,
+            "velocityY": 1.5939909118000826,
+            "timestamp": 0.7789789308022721
+        },
+        {
+            "x": 4.33416854760619,
+            "y": 6.52835429649903,
+            "heading": -0.016131091220158388,
+            "angularVelocity": -1.775030869099235e-8,
+            "velocityX": 3.43071842763476,
+            "velocityY": 1.593990402128264,
+            "timestamp": 0.8160731656023803
+        },
+        {
+            "x": 4.461428432515134,
+            "y": 6.587482129176235,
+            "heading": -0.016131091878588796,
+            "angularVelocity": -1.7750208582663698e-8,
+            "velocityX": 3.430718697790006,
+            "velocityY": 1.5939898206778003,
+            "timestamp": 0.8531674004024885
+        },
+        {
+            "x": 4.588688397859823,
+            "y": 6.646609788732575,
+            "heading": -0.01613109253701839,
+            "angularVelocity": -1.7750186861935105e-8,
+            "velocityX": 3.4307208662063466,
+            "velocityY": 1.5939851536219738,
+            "timestamp": 0.8902616352025967
+        },
+        {
+            "x": 4.7169608374986005,
+            "y": 6.703507592945478,
+            "heading": -0.016131093196594153,
+            "angularVelocity": -1.7781085471466767e-8,
+            "velocityX": 3.4580155199319775,
+            "velocityY": 1.533871894635887,
+            "timestamp": 0.9273558700027049
+        },
+        {
+            "x": 4.84945193271468,
+            "y": 6.749738410352506,
+            "heading": -0.016131093868950192,
+            "angularVelocity": -1.8125621008136676e-8,
+            "velocityX": 3.571743585763167,
+            "velocityY": 1.2463073481945173,
+            "timestamp": 0.9644501048028131
+        },
+        {
+            "x": 4.985276817345946,
+            "y": 6.784991484609654,
+            "heading": -0.0161310945734632,
+            "angularVelocity": -1.8992520332334574e-8,
+            "velocityX": 3.661617104738627,
+            "velocityY": 0.9503653181449264,
+            "timestamp": 1.0015443396029213
+        },
+        {
+            "x": 5.123527608694991,
+            "y": 6.809030968493646,
+            "heading": -0.01613109533353302,
+            "angularVelocity": -2.0490241268499195e-8,
+            "velocityX": 3.7270155886499414,
+            "velocityY": 0.6480652320645928,
+            "timestamp": 1.0386385744030295
+        },
+        {
+            "x": 5.263280146337045,
+            "y": 6.821696104409015,
+            "heading": -0.016131096180260384,
+            "angularVelocity": -2.2826387114294202e-8,
+            "velocityX": 3.767500216547063,
+            "velocityY": 0.34143138370740883,
+            "timestamp": 1.0757328092031377
+        },
+        {
+            "x": 5.403600215911865,
+            "y": 6.822902202606201,
+            "heading": -0.0161310971470982,
+            "angularVelocity": -2.6064368823146806e-8,
+            "velocityX": 3.7827999507462917,
+            "velocityY": 0.032514437989762196,
+            "timestamp": 1.112827044003246
+        },
+        {
+            "x": 5.530811605245593,
+            "y": 6.814532094483614,
+            "heading": -0.016131098042384447,
+            "angularVelocity": -2.6566067685833464e-8,
+            "velocityX": 3.7747775034488793,
+            "velocityY": -0.2483684519763222,
+            "timestamp": 1.1465274115659696
+        },
+        {
+            "x": 5.657050747526861,
+            "y": 6.796742322973506,
+            "heading": -0.016131098828254187,
+            "angularVelocity": -2.3319322572510494e-8,
+            "velocityX": 3.745927757206515,
+            "velocityY": -0.5278806374143462,
+            "timestamp": 1.1802277791286933
+        },
+        {
+            "x": 5.781621117814577,
+            "y": 6.76963106353518,
+            "heading": -0.016131099539274135,
+            "angularVelocity": -2.109828463295381e-8,
+            "velocityX": 3.6964098405118033,
+            "velocityY": -0.8044796362492181,
+            "timestamp": 1.213928146691417
+        },
+        {
+            "x": 5.903835409646495,
+            "y": 6.733347943713289,
+            "heading": -0.016131100200026813,
+            "angularVelocity": -1.96066906061588e-8,
+            "velocityX": 3.626497295747634,
+            "velocityY": -1.0766386970219277,
+            "timestamp": 1.2476285142541406
+        },
+        {
+            "x": 6.023019351333038,
+            "y": 6.68809326224078,
+            "heading": -0.016131100829259,
+            "angularVelocity": -1.86713746487578e-8,
+            "velocityX": 3.5365769072018636,
+            "velocityY": -1.3428542400399708,
+            "timestamp": 1.2813288818168642
+        },
+        {
+            "x": 6.138515596479519,
+            "y": 6.634117196875684,
+            "heading": -0.016131101442406693,
+            "angularVelocity": -1.81940951537302e-8,
+            "velocityX": 3.4271509036665004,
+            "velocityY": -1.601646191681265,
+            "timestamp": 1.3150292493795879
+        },
+        {
+            "x": 6.2512259314426855,
+            "y": 6.5745422130600915,
+            "heading": -0.016131102051778846,
+            "angularVelocity": -1.80820624580199e-8,
+            "velocityX": 3.3444838473464196,
+            "velocityY": -1.7677843929954529,
+            "timestamp": 1.3487296169423115
+        },
+        {
+            "x": 6.363935646775815,
+            "y": 6.514966056976746,
+            "heading": -0.01613110266098242,
+            "angularVelocity": -1.807706019437417e-8,
+            "velocityX": 3.344465460898959,
+            "velocityY": -1.7678191780093255,
+            "timestamp": 1.3824299845050352
+        },
+        {
+            "x": 6.4758517936606514,
+            "y": 6.453912192428435,
+            "heading": -0.016131102003666662,
+            "angularVelocity": 1.950470585863807e-8,
+            "velocityX": 3.320917692560386,
+            "velocityY": -1.811667615632877,
+            "timestamp": 1.4161303520677588
+        },
+        {
+            "x": 6.587767622018337,
+            "y": 6.392857744000494,
+            "heading": -0.016131101346154864,
+            "angularVelocity": 1.951052313957626e-8,
+            "velocityX": 3.32090824081921,
+            "velocityY": -1.8116849412489389,
+            "timestamp": 1.4498307196304825
+        },
+        {
+            "x": 6.699683405881235,
+            "y": 6.331803214011495,
+            "heading": -0.016131100688641712,
+            "angularVelocity": 1.9510563155904532e-8,
+            "velocityX": 3.3209069205135386,
+            "velocityY": -1.811687361432026,
+            "timestamp": 1.4835310871932061
+        },
+        {
+            "x": 6.811599167638959,
+            "y": 6.270748643502715,
+            "heading": -0.016131100031139815,
+            "angularVelocity": 1.9510229240352237e-8,
+            "velocityX": 3.320906264580792,
+            "velocityY": -1.8116885637862563,
+            "timestamp": 1.5172314547559298
+        },
+        {
+            "x": 6.923514914598752,
+            "y": 6.209694045868981,
+            "heading": -0.016131099373601954,
+            "angularVelocity": 1.95112965056638e-8,
+            "velocityX": 3.3209058254778503,
+            "velocityY": -1.8116893686722635,
+            "timestamp": 1.5509318223186535
+        },
+        {
+            "x": 7.031584175346113,
+            "y": 6.1507377884047765,
+            "heading": -0.013826704963615632,
+            "angularVelocity": 0.0683789102803133,
+            "velocityX": 3.2067680136194574,
+            "velocityY": -1.7494247608568367,
+            "timestamp": 1.584632189881377
+        },
+        {
+            "x": 7.131340439149152,
+            "y": 6.096316611291437,
+            "heading": -0.011699535798216875,
+            "angularVelocity": 0.06312005830317652,
+            "velocityX": 2.960094236876553,
+            "velocityY": -1.614854111369865,
+            "timestamp": 1.6183325574441008
+        },
+        {
+            "x": 7.222783690004588,
+            "y": 6.046430524071638,
+            "heading": -0.00974961791861361,
+            "angularVelocity": 0.057860433598327515,
+            "velocityX": 2.713419985263968,
+            "velocityY": -1.4802831787205646,
+            "timestamp": 1.6520329250068244
+        },
+        {
+            "x": 7.305913922772626,
+            "y": 6.001079530283105,
+            "heading": -0.00797695998479322,
+            "angularVelocity": 0.05260055192336173,
+            "velocityX": 2.466745581136908,
+            "velocityY": -1.3457121410953312,
+            "timestamp": 1.685733292569548
+        },
+        {
+            "x": 7.380731135011445,
+            "y": 5.960263631930958,
+            "heading": -0.00638156583911186,
+            "angularVelocity": 0.04734055623313652,
+            "velocityX": 2.2200711045530213,
+            "velocityY": -1.2111410439717167,
+            "timestamp": 1.7194336601322717
+        },
+        {
+            "x": 7.447235325348076,
+            "y": 5.923982830388488,
+            "heading": -0.004963437343393232,
+            "angularVelocity": 0.042080505296519764,
+            "velocityX": 1.9733965872286794,
+            "velocityY": -1.0765699060980205,
+            "timestamp": 1.7531340276949954
+        },
+        {
+            "x": 7.505426492937412,
+            "y": 5.892237126701046,
+            "heading": -0.0037225753686417596,
+            "angularVelocity": 0.0368204285143562,
+            "velocityX": 1.72672204482727,
+            "velocityY": -0.9419987372053683,
+            "timestamp": 1.786834395257719
+        },
+        {
+            "x": 7.555304637231595,
+            "y": 5.8650265217185185,
+            "heading": -0.002658980227466916,
+            "angularVelocity": 0.03156034245604838,
+            "velocityX": 1.4800474861690802,
+            "velocityY": -0.8074275430937743,
+            "timestamp": 1.8205347628204427
+        },
+        {
+            "x": 7.596869757865521,
+            "y": 5.842351016163066,
+            "heading": -0.0017726518914738514,
+            "angularVelocity": 0.02630025724030852,
+            "velocityX": 1.233372916677093,
+            "velocityY": -0.6728563275533557,
+            "timestamp": 1.8542351303831663
+        },
+        {
+            "x": 7.630121854593904,
+            "y": 5.824210610667954,
+            "heading": -0.001063590116536395,
+            "angularVelocity": 0.021040179268589657,
+            "velocityX": 0.9866983399066315,
+            "velocityY": -0.5382850932218669,
+            "timestamp": 1.88793549794589
+        },
+        {
+            "x": 7.655060927254085,
+            "y": 5.810605305801891,
+            "heading": -0.000531794525727047,
+            "angularVelocity": 0.01578011248154109,
+            "velocityX": 0.7400237583095566,
+            "velocityY": -0.4037138420149424,
+            "timestamp": 1.9216358655086136
+        },
+        {
+            "x": 7.671686975742638,
+            "y": 5.8015351020850225,
+            "heading": -0.00017726465841382563,
+            "angularVelocity": 0.010520059362948236,
+            "velocityX": 0.49334917364355607,
+            "velocityY": -0.26914257537363395,
+            "timestamp": 1.9553362330713373
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 3.6111480760753294e-31,
+            "angularVelocity": 0.005260021514121506,
+            "velocityX": 0.24667458721003926,
+            "velocityY": -0.1345712944104092,
+            "timestamp": 1.989036600634061
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -8.452347473573201e-32,
+            "angularVelocity": -1.5098225699671103e-29,
+            "velocityX": 2.1451735090517096e-32,
+            "velocityY": 9.12030277003484e-33,
+            "timestamp": 2.0227369681967846
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W2 To C3.traj b/src/main/deploy/choreo/W2 To C3.traj
new file mode 100644
index 00000000..72a99c84
--- /dev/null
+++ b/src/main/deploy/choreo/W2 To C3.traj	
@@ -0,0 +1,571 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 8.750120685231394e-32,
+            "velocityX": 3.618098191401073e-32,
+            "velocityY": 7.301888602720515e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 2.256418341264449,
+            "y": 5.558391022160101,
+            "heading": -2.5296452464584536e-15,
+            "angularVelocity": -8.979773235851594e-14,
+            "velocityX": 0.22783925602590605,
+            "velocityY": -0.05711574048502746,
+            "timestamp": 0.0281704802605187
+        },
+        {
+            "x": 2.2692550247581535,
+            "y": 5.555173071000616,
+            "heading": -7.593688933611638e-15,
+            "angularVelocity": -1.7976419432911875e-13,
+            "velocityX": 0.4556785463006615,
+            "velocityY": -0.11423132050732354,
+            "timestamp": 0.0563409605210374
+        },
+        {
+            "x": 2.2885100515708303,
+            "y": 5.55034615163608,
+            "heading": -1.5195474783343342e-14,
+            "angularVelocity": -2.6984935224723246e-13,
+            "velocityX": 0.6835178752583485,
+            "velocityY": -0.1713467189730698,
+            "timestamp": 0.0845114407815561
+        },
+        {
+            "x": 2.314183422940896,
+            "y": 5.54391026989229,
+            "heading": -2.533887633402318e-14,
+            "angularVelocity": -3.6007201361891127e-13,
+            "velocityX": 0.9113572481775378,
+            "velocityY": -0.2284619106337897,
+            "timestamp": 0.1126819210420748
+        },
+        {
+            "x": 2.346275140284958,
+            "y": 5.535865432453464,
+            "heading": -3.8024880419905343e-14,
+            "angularVelocity": -4.5032970578145327e-13,
+            "velocityX": 1.139196671383666,
+            "velocityY": -0.2855768650171331,
+            "timestamp": 0.14085240130259352
+        },
+        {
+            "x": 2.3847852052344276,
+            "y": 5.526211647048542,
+            "heading": -5.3249348657163737e-14,
+            "angularVelocity": -5.404404929621474e-13,
+            "velocityX": 1.3670361525018668,
+            "velocityY": -0.3426915450373883,
+            "timestamp": 0.16902288156311224
+        },
+        {
+            "x": 2.4297136196807942,
+            "y": 5.514948922688138,
+            "heading": -7.10022499667577e-14,
+            "angularVelocity": -6.301951953680268e-13,
+            "velocityX": 1.594875700764475,
+            "velocityY": -0.3998059051974526,
+            "timestamp": 0.19719336182363095
+        },
+        {
+            "x": 2.4810603858307716,
+            "y": 5.502077269965895,
+            "heading": -9.12659901579941e-14,
+            "angularVelocity": -7.193253365088161e-13,
+            "velocityX": 1.8227153273613368,
+            "velocityY": -0.45691988930278143,
+            "timestamp": 0.22536384208414967
+        },
+        {
+            "x": 2.5388255062707983,
+            "y": 5.4875967014378215,
+            "heading": -1.1400857239830863e-13,
+            "angularVelocity": -8.073196492084524e-13,
+            "velocityX": 2.050555045771992,
+            "velocityY": -0.5140334276930385,
+            "timestamp": 0.2535343223446684
+        },
+        {
+            "x": 2.603008984034667,
+            "y": 5.4715072320838045,
+            "heading": -1.3917722607752625e-13,
+            "angularVelocity": -8.934407027436765e-13,
+            "velocityX": 2.2783948718766736,
+            "velocityY": -0.571146434324935,
+            "timestamp": 0.2817048026051871
+        },
+        {
+            "x": 2.673610822650838,
+            "y": 5.453808879815544,
+            "heading": -1.6668826583849139e-13,
+            "angularVelocity": -9.76591080715578e-13,
+            "velocityX": 2.5062348232351805,
+            "velocityY": -0.6282588051246059,
+            "timestamp": 0.3098752828657058
+        },
+        {
+            "x": 2.7506310260932763,
+            "y": 5.434501665856061,
+            "heading": -1.9641690622397207e-13,
+            "angularVelocity": -1.0553118055097966e-12,
+            "velocityX": 2.7340749156620854,
+            "velocityY": -0.6853704225462842,
+            "timestamp": 0.33804576312622453
+        },
+        {
+            "x": 2.8340695983911575,
+            "y": 5.413585614341132,
+            "heading": -2.282023162882806e-13,
+            "angularVelocity": -1.128323328886794e-12,
+            "velocityX": 2.9619151511173074,
+            "velocityY": -0.7424811831924288,
+            "timestamp": 0.36621624338674325
+        },
+        {
+            "x": 2.9239265420414853,
+            "y": 5.391060748788907,
+            "heading": -2.61980248572899e-13,
+            "angularVelocity": -1.1990541861941579e-12,
+            "velocityX": 3.189755475211519,
+            "velocityY": -0.7995911089877218,
+            "timestamp": 0.39438672364726196
+        },
+        {
+            "x": 3.020201851829833,
+            "y": 5.366927076274376,
+            "heading": -2.9867647823192555e-13,
+            "angularVelocity": -1.302648350982896e-12,
+            "velocityX": 3.417595614203248,
+            "velocityY": -0.8567007836339452,
+            "timestamp": 0.4225572039077807
+        },
+        {
+            "x": 3.12289548969471,
+            "y": 5.341184519140017,
+            "heading": -3.474157859958988e-13,
+            "angularVelocity": -1.7301553723681501e-12,
+            "velocityX": 3.6454344020823566,
+            "velocityY": -0.9138132149787032,
+            "timestamp": 0.4507276841682994
+        },
+        {
+            "x": 3.2262642847742486,
+            "y": 5.315271868799975,
+            "heading": -3.474349355086831e-13,
+            "angularVelocity": -6.797723212352284e-16,
+            "velocityX": 3.6694012357471815,
+            "velocityY": -0.9198512095073661,
+            "timestamp": 0.4788981644288181
+        },
+        {
+            "x": 3.3296318094304937,
+            "y": 5.289354150477899,
+            "heading": -3.47428956348503e-13,
+            "angularVelocity": 2.1224913898406702e-16,
+            "velocityX": 3.669356138067555,
+            "velocityY": -0.920031113505745,
+            "timestamp": 0.5070686446893368
+        },
+        {
+            "x": 3.4329975109325144,
+            "y": 5.263429161946644,
+            "heading": -3.4742781851400116e-13,
+            "angularVelocity": 4.0391022494337805e-17,
+            "velocityX": 3.669291419461124,
+            "velocityY": -0.9202891924987843,
+            "timestamp": 0.5352391249498555
+        },
+        {
+            "x": 3.536360974747324,
+            "y": 5.237495253145949,
+            "heading": -3.474322284236003e-13,
+            "angularVelocity": -1.565436427905273e-16,
+            "velocityX": 3.669211985699591,
+            "velocityY": -0.9206058455823695,
+            "timestamp": 0.5634096052103742
+        },
+        {
+            "x": 3.6397250084501422,
+            "y": 5.211563615847215,
+            "heading": -3.474363074560882e-13,
+            "angularVelocity": -1.4479811668640362e-16,
+            "velocityX": 3.6692322156709736,
+            "velocityY": -0.9205252114596718,
+            "timestamp": 0.5915800854708929
+        },
+        {
+            "x": 3.7430830091153613,
+            "y": 5.185607942589789,
+            "heading": -3.474405753435316e-13,
+            "angularVelocity": -1.515021186684875e-16,
+            "velocityX": 3.669018053983158,
+            "velocityY": -0.9213784435831122,
+            "timestamp": 0.6197505657314116
+        },
+        {
+            "x": 3.8464299042958676,
+            "y": 5.1596080862072276,
+            "heading": -3.474364187384818e-13,
+            "angularVelocity": 1.4755179936907513e-16,
+            "velocityX": 3.668623829794913,
+            "velocityY": -0.92294686289042,
+            "timestamp": 0.6479210459919303
+        },
+        {
+            "x": 3.9490305912287407,
+            "y": 5.130804053406161,
+            "heading": -3.4744608105895375e-13,
+            "angularVelocity": -3.429945241470833e-16,
+            "velocityX": 3.642134815737201,
+            "velocityY": -1.0224899446047346,
+            "timestamp": 0.6760915262524491
+        },
+        {
+            "x": 4.049738686089906,
+            "y": 5.09595505292046,
+            "heading": -3.4745592092862155e-13,
+            "angularVelocity": -3.4929719255187864e-16,
+            "velocityX": 3.57495129404341,
+            "velocityY": -1.237075128411728,
+            "timestamp": 0.7042620065129678
+        },
+        {
+            "x": 4.148105144500732,
+            "y": 5.054959774017334,
+            "heading": -3.474586084587748e-13,
+            "angularVelocity": -9.540235481877701e-17,
+            "velocityX": 3.491827526585996,
+            "velocityY": -1.4552566560457803,
+            "timestamp": 0.7324324867734865
+        },
+        {
+            "x": 4.301558125072517,
+            "y": 4.97182210350292,
+            "heading": -3.4745624581283064e-13,
+            "angularVelocity": 5.1211249699831376e-17,
+            "velocityX": 3.3261517465815813,
+            "velocityY": -1.8020406443565171,
+            "timestamp": 0.7785677783246081
+        },
+        {
+            "x": 4.445898119107738,
+            "y": 4.873711676741488,
+            "heading": -3.4747329562610653e-13,
+            "angularVelocity": -3.6956119081305154e-16,
+            "velocityX": 3.1286242956827928,
+            "velocityY": -2.1265808335182323,
+            "timestamp": 0.8247030698757297
+        },
+        {
+            "x": 4.587571576014754,
+            "y": 4.771788561847154,
+            "heading": -3.4746251546031876e-13,
+            "angularVelocity": 2.3366419539300746e-16,
+            "velocityX": 3.0708260887450245,
+            "velocityY": -2.2092222996219535,
+            "timestamp": 0.8708383614268513
+        },
+        {
+            "x": 4.729219222551993,
+            "y": 4.66982957987028,
+            "heading": -3.474587781166732e-13,
+            "angularVelocity": 8.100834549122579e-17,
+            "velocityX": 3.070266639108493,
+            "velocityY": -2.209999732353969,
+            "timestamp": 0.916973652977973
+        },
+        {
+            "x": 4.870877206231954,
+            "y": 4.567884960544782,
+            "heading": -3.47487867941798e-13,
+            "angularVelocity": -6.305330291624225e-16,
+            "velocityX": 3.070490700636254,
+            "velocityY": -2.209688416384298,
+            "timestamp": 0.9631089445290946
+        },
+        {
+            "x": 5.012587244979431,
+            "y": 4.466012713767847,
+            "heading": -3.4750411523417436e-13,
+            "angularVelocity": -3.5216624493767476e-16,
+            "velocityX": 3.0716190140743462,
+            "velocityY": -2.208119713797033,
+            "timestamp": 1.009244236080216
+        },
+        {
+            "x": 5.160453670184351,
+            "y": 4.373302358522453,
+            "heading": -3.47491376477911e-13,
+            "angularVelocity": 2.7611738924244047e-16,
+            "velocityX": 3.2050610331965634,
+            "velocityY": -2.009532228521384,
+            "timestamp": 1.0553795276313376
+        },
+        {
+            "x": 5.316942928228011,
+            "y": 4.29603139716118,
+            "heading": -3.4750002331748596e-13,
+            "angularVelocity": -1.8742353831509497e-16,
+            "velocityX": 3.3919642161767567,
+            "velocityY": -1.6748774910066955,
+            "timestamp": 1.101514819182459
+        },
+        {
+            "x": 5.480467319488525,
+            "y": 4.235044002532959,
+            "heading": -3.475163170309309e-13,
+            "angularVelocity": -3.5317243928660403e-16,
+            "velocityX": 3.5444534056932553,
+            "velocityY": -1.321924985762245,
+            "timestamp": 1.1476501107335806
+        },
+        {
+            "x": 5.5914176787721175,
+            "y": 4.201991970759951,
+            "heading": -3.4752963658489967e-13,
+            "angularVelocity": -4.352387405096259e-16,
+            "velocityX": 3.625488497149835,
+            "velocityY": -1.0800304007696078,
+            "timestamp": 1.1782529824212593
+        },
+        {
+            "x": 5.704342870003183,
+            "y": 4.176490587918632,
+            "heading": -3.4752519346531394e-13,
+            "angularVelocity": 1.4518636145588672e-16,
+            "velocityX": 3.6900194329281493,
+            "velocityY": -0.8333003223399679,
+            "timestamp": 1.208855854108938
+        },
+        {
+            "x": 5.818728492967466,
+            "y": 4.15864825583045,
+            "heading": -3.47528320752536e-13,
+            "angularVelocity": -1.0218933861720526e-16,
+            "velocityX": 3.737741481635173,
+            "velocityY": -0.5830280331479286,
+            "timestamp": 1.2394587257966168
+        },
+        {
+            "x": 5.934052260797228,
+            "y": 4.148506842880417,
+            "heading": -3.475166383708343e-13,
+            "angularVelocity": 3.817413550372978e-16,
+            "velocityX": 3.768396933681978,
+            "velocityY": -0.3313876244719343,
+            "timestamp": 1.2700615974842955
+        },
+        {
+            "x": 6.049784939896788,
+            "y": 4.145614314337883,
+            "heading": -3.4752553093783573e-13,
+            "angularVelocity": -2.9057949488904467e-16,
+            "velocityX": 3.7817587931181613,
+            "velocityY": -0.0945182064161531,
+            "timestamp": 1.3006644691719742
+        },
+        {
+            "x": 6.1655453259831,
+            "y": 4.144216905783092,
+            "heading": -3.4753489515012823e-13,
+            "angularVelocity": -3.0599129336581387e-16,
+            "velocityX": 3.7826641652362696,
+            "velocityY": -0.04566266099543126,
+            "timestamp": 1.331267340859653
+        },
+        {
+            "x": 6.281306292241875,
+            "y": 4.142868414150292,
+            "heading": -3.4754223223995867e-13,
+            "angularVelocity": -2.397516775868154e-16,
+            "velocityX": 3.7826831233414593,
+            "velocityY": -0.04406421878574171,
+            "timestamp": 1.3618702125473316
+        },
+        {
+            "x": 6.39706739301833,
+            "y": 4.141531519398385,
+            "heading": -3.475633687164305e-13,
+            "angularVelocity": -6.906697084916283e-16,
+            "velocityX": 3.7826875189315166,
+            "velocityY": -0.04368527131792852,
+            "timestamp": 1.3924730842350104
+        },
+        {
+            "x": 6.512828617962261,
+            "y": 4.14020542040497,
+            "heading": -3.4758398313992887e-13,
+            "angularVelocity": -6.73610754719036e-16,
+            "velocityX": 3.7826915763114313,
+            "velocityY": -0.043332501854336236,
+            "timestamp": 1.423075955922689
+        },
+        {
+            "x": 6.628589931637748,
+            "y": 4.138887090943481,
+            "heading": -3.4757335612235213e-13,
+            "angularVelocity": 3.472555676974266e-16,
+            "velocityX": 3.7826944757635745,
+            "velocityY": -0.04307861939704374,
+            "timestamp": 1.4536788276103678
+        },
+        {
+            "x": 6.744351296535766,
+            "y": 4.137573267392239,
+            "heading": -3.475819448429605e-13,
+            "angularVelocity": -2.806508055564177e-16,
+            "velocityX": 3.782696149545524,
+            "velocityY": -0.042931381231176444,
+            "timestamp": 1.4842816992980465
+        },
+        {
+            "x": 6.860112695906494,
+            "y": 4.136261713498386,
+            "heading": -3.475979530506806e-13,
+            "angularVelocity": -5.230943272138926e-16,
+            "velocityX": 3.7826972759991557,
+            "velocityY": -0.04285721636551622,
+            "timestamp": 1.5148845709857253
+        },
+        {
+            "x": 6.9694302247553,
+            "y": 4.135029589899985,
+            "heading": -2.392571520797574e-13,
+            "angularVelocity": 3.5402168172320343e-12,
+            "velocityX": 3.5721330326269505,
+            "velocityY": -0.04026169865772549,
+            "timestamp": 1.545487442673404
+        },
+        {
+            "x": 7.070940187048315,
+            "y": 4.133884255746116,
+            "heading": -1.6833543298116243e-13,
+            "angularVelocity": 2.317485846705618e-12,
+            "velocityX": 3.3170077412667367,
+            "velocityY": -0.03742570844047695,
+            "timestamp": 1.5760903143610827
+        },
+        {
+            "x": 7.164641690009617,
+            "y": 4.132826276383854,
+            "heading": -1.1981581485168293e-13,
+            "angularVelocity": 1.5854597325485145e-12,
+            "velocityX": 3.061853276960511,
+            "velocityY": -0.034571244567840914,
+            "timestamp": 1.6066931860487614
+        },
+        {
+            "x": 7.250534738513507,
+            "y": 4.131855992058627,
+            "heading": -8.523066967233492e-14,
+            "angularVelocity": 1.1301275060622065e-12,
+            "velocityX": 2.8066989719298543,
+            "velocityY": -0.03170566260882202,
+            "timestamp": 1.6372960577364402
+        },
+        {
+            "x": 7.328619329487729,
+            "y": 4.1309736027819035,
+            "heading": -6.003549662940244e-14,
+            "angularVelocity": 8.232944437274219e-13,
+            "velocityX": 2.5515445665080714,
+            "velocityY": -0.028833544941282405,
+            "timestamp": 1.6678989294241189
+        },
+        {
+            "x": 7.398895462248961,
+            "y": 4.130179235828021,
+            "heading": -4.153185117983844e-14,
+            "angularVelocity": 6.046375734752488e-13,
+            "velocityX": 2.2963901387575416,
+            "velocityY": -0.02595726837221309,
+            "timestamp": 1.6985018011117976
+        },
+        {
+            "x": 7.461363136410412,
+            "y": 4.129472977946104,
+            "heading": -2.797073854746349e-14,
+            "angularVelocity": 4.431320366500053e-13,
+            "velocityX": 2.041235698368024,
+            "velocityY": -0.0230781571305896,
+            "timestamp": 1.7291046727994763
+        },
+        {
+            "x": 7.516022351719397,
+            "y": 4.1288548915008905,
+            "heading": -1.8141238812912564e-14,
+            "angularVelocity": 3.211953369611228e-13,
+            "velocityX": 1.7860812497215437,
+            "velocityY": -0.02019700801679021,
+            "timestamp": 1.759707544487155
+        },
+        {
+            "x": 7.5628731079961895,
+            "y": 4.128325023227497,
+            "heading": -1.1163529343344889e-14,
+            "angularVelocity": 2.2800831618830555e-13,
+            "velocityX": 1.5309267952022354,
+            "velocityY": -0.017314331754962387,
+            "timestamp": 1.7903104161748338
+        },
+        {
+            "x": 7.601915405105201,
+            "y": 4.127883409320133,
+            "heading": -6.372564950074351e-15,
+            "angularVelocity": 1.5655276164796086e-13,
+            "velocityX": 1.2757723362523365,
+            "velocityY": -0.014430472786738478,
+            "timestamp": 1.8209132878625125
+        },
+        {
+            "x": 7.63314924293974,
+            "y": 4.127530078564582,
+            "heading": -3.2435183420078772e-15,
+            "angularVelocity": 1.0224681844586284e-13,
+            "velocityX": 1.020617873816125,
+            "velocityY": -0.011545673194894595,
+            "timestamp": 1.8515161595501912
+        },
+        {
+            "x": 7.656574621413203,
+            "y": 4.127265054360105,
+            "heading": -1.3581702257498648e-15,
+            "angularVelocity": 6.160689169092944e-14,
+            "velocityX": 0.7654634085499833,
+            "velocityY": -0.008660108992989924,
+            "timestamp": 1.88211903123787
+        },
+        {
+            "x": 7.672191540453606,
+            "y": 4.127088356077157,
+            "heading": -3.7222995775515084e-16,
+            "angularVelocity": 3.2217238765593925e-14,
+            "velocityX": 0.5103089409316736,
+            "velocityY": -0.0057739118290542285,
+            "timestamp": 1.9127219029255487
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 2.9643987227812266e-34,
+            "angularVelocity": 1.2163231774130929e-14,
+            "velocityX": 0.25515447132169783,
+            "velocityY": -0.0028871825495910644,
+            "timestamp": 1.9433247746132274
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -1.7511816257564745e-34,
+            "angularVelocity": -1.536305146096975e-32,
+            "velocityX": 2.290424541792184e-34,
+            "velocityY": 7.21466542895957e-33,
+            "timestamp": 1.9739276463009061
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W2 To C4.traj b/src/main/deploy/choreo/W2 To C4.traj
new file mode 100644
index 00000000..5f6c2964
--- /dev/null
+++ b/src/main/deploy/choreo/W2 To C4.traj	
@@ -0,0 +1,589 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.253608767824088,
+            "y": 5.559959646221142,
+            "heading": 0.04333845773860565,
+            "angularVelocity": 1.2657316926377489,
+            "velocityX": 0.10539673178657698,
+            "velocityY": -0.0011785619397610312,
+            "timestamp": 0.03423984560921402
+        },
+        {
+            "x": 2.260842860117559,
+            "y": 5.560038303784881,
+            "heading": 0.12991520296053788,
+            "angularVelocity": 2.5285378389274697,
+            "velocityX": 0.211277012637124,
+            "velocityY": 0.002297252290136546,
+            "timestamp": 0.06847969121842803
+        },
+        {
+            "x": 2.271629728646886,
+            "y": 5.560441749821537,
+            "heading": 0.2597959154554935,
+            "angularVelocity": 3.7932622120236568,
+            "velocityX": 0.3150384687021929,
+            "velocityY": 0.011782939714764799,
+            "timestamp": 0.10271953682764205
+        },
+        {
+            "x": 2.285721166472015,
+            "y": 5.561272024294721,
+            "heading": 0.433416913958427,
+            "angularVelocity": 5.070729596286839,
+            "velocityX": 0.41155085761651194,
+            "velocityY": 0.024248779701277118,
+            "timestamp": 0.13695938243685607
+        },
+        {
+            "x": 2.302746549134424,
+            "y": 5.561830584813036,
+            "heading": 0.6513804515506975,
+            "angularVelocity": 6.36578622695705,
+            "velocityX": 0.497238885266109,
+            "velocityY": 0.01631317280720512,
+            "timestamp": 0.17119922804607007
+        },
+        {
+            "x": 2.325552284167229,
+            "y": 5.559809655953846,
+            "heading": 0.9008347442106217,
+            "angularVelocity": 7.285497005652254,
+            "velocityX": 0.6660583488924382,
+            "velocityY": -0.05902272113767896,
+            "timestamp": 0.20543907365528408
+        },
+        {
+            "x": 2.357593597831257,
+            "y": 5.5547755319598515,
+            "heading": 1.1463891087836293,
+            "angularVelocity": 7.171596723173552,
+            "velocityX": 0.9357902494573596,
+            "velocityY": -0.14702531230571617,
+            "timestamp": 0.23967891926449808
+        },
+        {
+            "x": 2.398826095012215,
+            "y": 5.54977146589545,
+            "heading": 1.3795454342192353,
+            "angularVelocity": 6.809502825937482,
+            "velocityX": 1.2042255578939407,
+            "velocityY": -0.14614744825411982,
+            "timestamp": 0.2739187648737121
+        },
+        {
+            "x": 2.4467424620473803,
+            "y": 5.551292722520974,
+            "heading": 1.6025344347532522,
+            "angularVelocity": 6.512558586829906,
+            "velocityX": 1.399432917485785,
+            "velocityY": 0.04442942421196648,
+            "timestamp": 0.3081586104829261
+        },
+        {
+            "x": 2.499282041125752,
+            "y": 5.560003512593039,
+            "heading": 1.8091707816342142,
+            "angularVelocity": 6.034967249541443,
+            "velocityX": 1.5344572425359657,
+            "velocityY": 0.2544050627880495,
+            "timestamp": 0.3423984560921401
+        },
+        {
+            "x": 2.558927262082263,
+            "y": 5.571143049666431,
+            "heading": 1.9899644077603311,
+            "angularVelocity": 5.2802114877955235,
+            "velocityX": 1.7419827658469296,
+            "velocityY": 0.3253384142127747,
+            "timestamp": 0.3766383017013541
+        },
+        {
+            "x": 2.624771333399717,
+            "y": 5.5772713811756995,
+            "heading": 2.148730759711285,
+            "angularVelocity": 4.63688866366352,
+            "velocityX": 1.9230247726273466,
+            "velocityY": 0.17898245159199333,
+            "timestamp": 0.4108781473105681
+        },
+        {
+            "x": 2.6962684360037885,
+            "y": 5.576686266483743,
+            "heading": 2.2908744479705683,
+            "angularVelocity": 4.151411483614645,
+            "velocityX": 2.0881257298902987,
+            "velocityY": -0.0170887070763856,
+            "timestamp": 0.4451179929197821
+        },
+        {
+            "x": 2.773687793639176,
+            "y": 5.569334398339137,
+            "heading": 2.4184902740666434,
+            "angularVelocity": 3.727114530613817,
+            "velocityX": 2.2610895656186547,
+            "velocityY": -0.2147167434256298,
+            "timestamp": 0.4793578385289961
+        },
+        {
+            "x": 2.8582271385194487,
+            "y": 5.556743437974915,
+            "heading": 2.5300657438997165,
+            "angularVelocity": 3.258644069441941,
+            "velocityX": 2.4690340559690878,
+            "velocityY": -0.36772830426646747,
+            "timestamp": 0.5135976841382102
+        },
+        {
+            "x": 2.9508927177061977,
+            "y": 5.541492370817128,
+            "heading": 2.6233336392816216,
+            "angularVelocity": 2.7239578252305745,
+            "velocityX": 2.706366735538439,
+            "velocityY": -0.445418689437171,
+            "timestamp": 0.5478375297474242
+        },
+        {
+            "x": 3.0518533392296137,
+            "y": 5.525810656425357,
+            "heading": 2.697205719709221,
+            "angularVelocity": 2.1574887127329996,
+            "velocityX": 2.948629578406965,
+            "velocityY": -0.45799606022611716,
+            "timestamp": 0.5820773753566382
+        },
+        {
+            "x": 3.1608544462331407,
+            "y": 5.511189976920693,
+            "heading": 2.751249379299885,
+            "angularVelocity": 1.5783850256649679,
+            "velocityX": 3.183457900119567,
+            "velocityY": -0.42700775206563796,
+            "timestamp": 0.6163172209658522
+        },
+        {
+            "x": 3.2775207255393237,
+            "y": 5.498694402974223,
+            "heading": 2.785399702937351,
+            "angularVelocity": 0.9973854446433661,
+            "velocityX": 3.40732492306531,
+            "velocityY": -0.36494247342945346,
+            "timestamp": 0.6505570665750662
+        },
+        {
+            "x": 3.401135401206032,
+            "y": 5.488837187261409,
+            "heading": 2.800896234569334,
+            "angularVelocity": 0.4525876608454414,
+            "velocityX": 3.610257974803583,
+            "velocityY": -0.28788727102673717,
+            "timestamp": 0.6847969121842802
+        },
+        {
+            "x": 3.5179017921083777,
+            "y": 5.474779629986763,
+            "heading": 2.835487088185811,
+            "angularVelocity": 1.0102514483058458,
+            "velocityX": 3.410248756230483,
+            "velocityY": -0.4105613510962388,
+            "timestamp": 0.7190367577934942
+        },
+        {
+            "x": 3.6287098119916026,
+            "y": 5.4546983370085105,
+            "heading": 2.8876881760682602,
+            "angularVelocity": 1.5245713569573334,
+            "velocityX": 3.2362301263825226,
+            "velocityY": -0.5864890048700768,
+            "timestamp": 0.7532766034027082
+        },
+        {
+            "x": 3.7356372636451365,
+            "y": 5.425785607175325,
+            "heading": 2.9455736884009496,
+            "angularVelocity": 1.6905891747687094,
+            "velocityX": 3.1228952628442794,
+            "velocityY": -0.8444176461299453,
+            "timestamp": 0.7875164490119222
+        },
+        {
+            "x": 3.8388362586877394,
+            "y": 5.387864157402447,
+            "heading": 3.0009390226743027,
+            "angularVelocity": 1.6169855117119507,
+            "velocityX": 3.0140029315678847,
+            "velocityY": -1.1075239709221774,
+            "timestamp": 0.8217562946211362
+        },
+        {
+            "x": 3.9379980862804507,
+            "y": 5.341070528950818,
+            "heading": 3.053673275383994,
+            "angularVelocity": 1.5401428298350917,
+            "velocityX": 2.896094472050628,
+            "velocityY": -1.3666425072616961,
+            "timestamp": 0.8559961402303502
+        },
+        {
+            "x": 4.032804489135742,
+            "y": 5.285560607910156,
+            "heading": 3.1036105469083837,
+            "angularVelocity": 1.4584549268806075,
+            "velocityX": 2.76889107320562,
+            "velocityY": -1.62120827512502,
+            "timestamp": 0.8902359858395642
+        },
+        {
+            "x": 4.173725003261308,
+            "y": 5.173637687034127,
+            "heading": 3.175484729095798,
+            "angularVelocity": 1.2994086167535415,
+            "velocityX": 2.547692714675012,
+            "velocityY": -2.023447131813128,
+            "timestamp": 0.9455489807364162
+        },
+        {
+            "x": 4.301872286760783,
+            "y": 5.039921697201636,
+            "heading": 3.2346580028937466,
+            "angularVelocity": 1.0697897285853943,
+            "velocityX": 2.3167663175433564,
+            "velocityY": -2.4174425934058767,
+            "timestamp": 1.0008619756332682
+        },
+        {
+            "x": 4.416442024233276,
+            "y": 4.885133369968262,
+            "heading": 3.2770092965554323,
+            "angularVelocity": 0.765666255111711,
+            "velocityX": 2.0712987551323803,
+            "velocityY": -2.7984079965661373,
+            "timestamp": 1.0561749705301202
+        },
+        {
+            "x": 4.517908742039174,
+            "y": 4.711097539386313,
+            "heading": 3.282986865137276,
+            "angularVelocity": 0.10806806959180061,
+            "velocityX": 1.834410123608644,
+            "velocityY": -3.146382344808711,
+            "timestamp": 1.1114879654269723
+        },
+        {
+            "x": 4.627451956204509,
+            "y": 4.5560902871099245,
+            "heading": 3.2338916761611958,
+            "angularVelocity": -0.8875886953442504,
+            "velocityX": 1.9804245705663106,
+            "velocityY": -2.802365928032756,
+            "timestamp": 1.1668009603238243
+        },
+        {
+            "x": 4.751713919325069,
+            "y": 4.421499213962681,
+            "heading": 3.169974564154514,
+            "angularVelocity": -1.1555532678328913,
+            "velocityX": 2.246523865726009,
+            "velocityY": -2.433263167149585,
+            "timestamp": 1.2221139552206763
+        },
+        {
+            "x": 4.891049798654671,
+            "y": 4.307366204928411,
+            "heading": 3.098048256171184,
+            "angularVelocity": -1.3003509955925967,
+            "velocityX": 2.5190442063286667,
+            "velocityY": -2.063403170396147,
+            "timestamp": 1.2774269501175284
+        },
+        {
+            "x": 5.0446094980944505,
+            "y": 4.214363163694223,
+            "heading": 3.021068837736617,
+            "angularVelocity": -1.3917058473893738,
+            "velocityX": 2.7761957154216415,
+            "velocityY": -1.6813958710357464,
+            "timestamp": 1.3327399450143804
+        },
+        {
+            "x": 5.211468562298102,
+            "y": 4.143120621239898,
+            "heading": 2.9418200950482523,
+            "angularVelocity": -1.432732811451417,
+            "velocityX": 3.016634057056783,
+            "velocityY": -1.2879892435254758,
+            "timestamp": 1.3880529399112325
+        },
+        {
+            "x": 5.390789031982422,
+            "y": 4.09412145614624,
+            "heading": 2.865640793449113,
+            "angularVelocity": -1.3772405876991196,
+            "velocityX": 3.2419229878750393,
+            "velocityY": -0.8858526858838836,
+            "timestamp": 1.4433659348080845
+        },
+        {
+            "x": 5.535592658236132,
+            "y": 4.049337172847784,
+            "heading": 2.841642220937184,
+            "angularVelocity": -0.5722130304709164,
+            "velocityX": 3.452643767066762,
+            "velocityY": -1.0678197818199107,
+            "timestamp": 1.4853058614332775
+        },
+        {
+            "x": 5.681734895850587,
+            "y": 4.003264514213434,
+            "heading": 2.8233979117656802,
+            "angularVelocity": -0.4350105171749157,
+            "velocityX": 3.484561118108144,
+            "velocityY": -1.098539323783022,
+            "timestamp": 1.5272457880584704
+        },
+        {
+            "x": 5.827742551539107,
+            "y": 3.9572785916007636,
+            "heading": 2.804954082691072,
+            "angularVelocity": -0.4397677954812897,
+            "velocityX": 3.481352196758831,
+            "velocityY": -1.096471222366128,
+            "timestamp": 1.5691857146836634
+        },
+        {
+            "x": 5.9679573209760495,
+            "y": 3.908501280249766,
+            "heading": 2.7717324952229117,
+            "angularVelocity": -0.7921231661908814,
+            "velocityX": 3.3432287731451744,
+            "velocityY": -1.1630280564605782,
+            "timestamp": 1.6111256413088564
+        },
+        {
+            "x": 6.096940014064459,
+            "y": 3.8543767396001813,
+            "heading": 2.711734513417447,
+            "angularVelocity": -1.4305695463335533,
+            "velocityX": 3.07541532537948,
+            "velocityY": -1.2905254015650245,
+            "timestamp": 1.6530655679340494
+        },
+        {
+            "x": 6.215088187160598,
+            "y": 3.7918967611554555,
+            "heading": 2.632459037184743,
+            "angularVelocity": -1.890214948184595,
+            "velocityX": 2.8170810633981405,
+            "velocityY": -1.4897493503766595,
+            "timestamp": 1.6950054945592423
+        },
+        {
+            "x": 6.32424952389229,
+            "y": 3.717983083464812,
+            "heading": 2.5463998013121922,
+            "angularVelocity": -2.0519643880553406,
+            "velocityX": 2.6028022821127137,
+            "velocityY": -1.7623702194615716,
+            "timestamp": 1.7369454211844353
+        },
+        {
+            "x": 6.43630340548619,
+            "y": 3.6322698094657313,
+            "heading": 2.4886072976482807,
+            "angularVelocity": -1.3779829464268867,
+            "velocityX": 2.6717710451737124,
+            "velocityY": -2.043715401914737,
+            "timestamp": 1.7788853478096283
+        },
+        {
+            "x": 6.552059359988985,
+            "y": 3.5378523447078845,
+            "heading": 2.4573864362362134,
+            "angularVelocity": -0.7444185987992016,
+            "velocityX": 2.7600418936656252,
+            "velocityY": -2.251254886581771,
+            "timestamp": 1.8208252744348212
+        },
+        {
+            "x": 6.67032629558411,
+            "y": 3.432112823682954,
+            "heading": 2.4573457680499984,
+            "angularVelocity": -0.0009696770950155036,
+            "velocityX": 2.8199127922194083,
+            "velocityY": -2.521213782033988,
+            "timestamp": 1.8627652010600142
+        },
+        {
+            "x": 6.787202921818474,
+            "y": 3.3248215481814336,
+            "heading": 2.4573432508654287,
+            "angularVelocity": -0.00006001881195361165,
+            "velocityX": 2.7867627733081557,
+            "velocityY": -2.558213238195593,
+            "timestamp": 1.9047051276852072
+        },
+        {
+            "x": 6.904079764497574,
+            "y": 3.217530508972575,
+            "heading": 2.4573407325391345,
+            "angularVelocity": -0.000060046034804859665,
+            "velocityX": 2.7867679341358973,
+            "velocityY": -2.5582076041213115,
+            "timestamp": 1.9466450543104001
+        },
+        {
+            "x": 7.017539426879104,
+            "y": 3.1193532157248125,
+            "heading": 2.428286840001792,
+            "angularVelocity": -0.6927501995172902,
+            "velocityX": 2.7052899590285975,
+            "velocityY": -2.3409028376503684,
+            "timestamp": 1.988584980935593
+        },
+        {
+            "x": 7.118887938132189,
+            "y": 3.020968055873212,
+            "heading": 2.3710302150679574,
+            "angularVelocity": -1.365205653446252,
+            "velocityX": 2.4165161794108876,
+            "velocityY": -2.345859131582302,
+            "timestamp": 2.030524907560786
+        },
+        {
+            "x": 7.207551237715759,
+            "y": 2.9223074295194613,
+            "heading": 2.2882909026899103,
+            "angularVelocity": -1.9728053679604207,
+            "velocityX": 2.1140547139229047,
+            "velocityY": -2.3524272523282552,
+            "timestamp": 2.0724648341859786
+        },
+        {
+            "x": 7.28673413242529,
+            "y": 2.8318719270188626,
+            "heading": 2.1761461762136682,
+            "angularVelocity": -2.6739371167349164,
+            "velocityX": 1.8880074688058086,
+            "velocityY": -2.1563104606452623,
+            "timestamp": 2.1144047608111713
+        },
+        {
+            "x": 7.3579924776067,
+            "y": 2.7517426977667294,
+            "heading": 2.0378729016459984,
+            "angularVelocity": -3.2969364921256217,
+            "velocityX": 1.6990574594521515,
+            "velocityY": -1.9105715174046114,
+            "timestamp": 2.156344687436364
+        },
+        {
+            "x": 7.4224137082831785,
+            "y": 2.6828558926645387,
+            "heading": 1.874977709266495,
+            "angularVelocity": -3.884012335912244,
+            "velocityX": 1.5360358460374843,
+            "velocityY": -1.6425113405136766,
+            "timestamp": 2.198284614061557
+        },
+        {
+            "x": 7.480883603746462,
+            "y": 2.6255140291502124,
+            "heading": 1.6865689006450588,
+            "angularVelocity": -4.4923495051672475,
+            "velocityX": 1.3941344243592637,
+            "velocityY": -1.3672380504328716,
+            "timestamp": 2.2402245406867496
+        },
+        {
+            "x": 7.534437188323511,
+            "y": 2.579534136686106,
+            "heading": 1.4695142507662173,
+            "angularVelocity": -5.175370281846384,
+            "velocityX": 1.27691173748694,
+            "velocityY": -1.0963274417482318,
+            "timestamp": 2.2821644673119423
+        },
+        {
+            "x": 7.579534895925645,
+            "y": 2.5413039154996566,
+            "heading": 1.2172257697546927,
+            "angularVelocity": -6.015472636997345,
+            "velocityX": 1.0752929542572058,
+            "velocityY": -0.9115471643072651,
+            "timestamp": 2.324104393937135
+        },
+        {
+            "x": 7.615712715030757,
+            "y": 2.5097826728946813,
+            "heading": 0.9253708991340255,
+            "angularVelocity": -6.958878903840344,
+            "velocityX": 0.8626104530039771,
+            "velocityY": -0.7515807761580874,
+            "timestamp": 2.366044320562328
+        },
+        {
+            "x": 7.6401514150011085,
+            "y": 2.4865499215178333,
+            "heading": 0.6440959436378817,
+            "angularVelocity": -6.706615345559152,
+            "velocityX": 0.5827072657697844,
+            "velocityY": -0.5539530763721487,
+            "timestamp": 2.4079842471875206
+        },
+        {
+            "x": 7.656070937434432,
+            "y": 2.471863188851966,
+            "heading": 0.3945229348796906,
+            "angularVelocity": -5.950725927314207,
+            "velocityX": 0.37957916749813,
+            "velocityY": -0.3501849871393242,
+            "timestamp": 2.4499241738127133
+        },
+        {
+            "x": 7.668072859318959,
+            "y": 2.4642781587251137,
+            "heading": 0.19785094472919199,
+            "angularVelocity": -4.689373729909185,
+            "velocityX": 0.28616935818189254,
+            "velocityY": -0.18085463512222474,
+            "timestamp": 2.491864100437906
+        },
+        {
+            "x": 7.676136280306529,
+            "y": 2.459442710773055,
+            "heading": 0.06623696286278732,
+            "angularVelocity": -3.1381547956105695,
+            "velocityX": 0.19226120874341576,
+            "velocityY": -0.11529462116784107,
+            "timestamp": 2.533804027063099
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 2.1057025676027217e-33,
+            "angularVelocity": -1.57932948845455,
+            "velocityX": 0.09212509425682942,
+            "velocityY": -0.058243086471875524,
+            "timestamp": 2.5757439536882916
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -7.524501141381425e-34,
+            "angularVelocity": -5.907340858361994e-32,
+            "velocityX": -7.634877430876997e-33,
+            "velocityY": -7.90956322394541e-33,
+            "timestamp": 2.6176838803134843
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W2 To C5.traj b/src/main/deploy/choreo/W2 To C5.traj
new file mode 100644
index 00000000..cfa6b046
--- /dev/null
+++ b/src/main/deploy/choreo/W2 To C5.traj	
@@ -0,0 +1,625 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 6.892560738534845e-27,
+            "timestamp": 0
+        },
+        {
+            "x": 2.24821488195175,
+            "y": 5.550858605907564,
+            "heading": -1.0860081979744884e-8,
+            "angularVelocity": -3.249360952601713e-7,
+            "velocityX": -0.053411135206172675,
+            "velocityY": -0.27351257600550494,
+            "timestamp": 0.0334222075852833
+        },
+        {
+            "x": 2.2446446457975586,
+            "y": 5.532575817984525,
+            "heading": -3.253781151640391e-8,
+            "angularVelocity": -6.486025430910346e-7,
+            "velocityX": -0.10682227213842765,
+            "velocityY": -0.5470251441769277,
+            "timestamp": 0.0668444151705666
+        },
+        {
+            "x": 2.2392892913368425,
+            "y": 5.505151636566201,
+            "heading": -6.492851000422694e-8,
+            "angularVelocity": -9.691369834959166e-7,
+            "velocityX": -0.1602334150722015,
+            "velocityY": -0.8205377023156053,
+            "timestamp": 0.10026662275584991
+        },
+        {
+            "x": 2.2321488181807467,
+            "y": 5.468586062083933,
+            "heading": -1.0785000725838172e-7,
+            "angularVelocity": -0.000001284220850944888,
+            "velocityX": -0.21364456964054193,
+            "velocityY": -1.0940502475484135,
+            "timestamp": 0.1336888303411332
+        },
+        {
+            "x": 2.2232232256851456,
+            "y": 5.42287909509794,
+            "heading": -1.6102300741445552e-7,
+            "angularVelocity": -0.0000015909481231905412,
+            "velocityX": -0.26705574348127187,
+            "velocityY": -1.3675627760193463,
+            "timestamp": 0.16711103792641652
+        },
+        {
+            "x": 2.212512512848178,
+            "y": 5.368030736346982,
+            "heading": -2.2404505707142586e-7,
+            "angularVelocity": -0.0000018856338989468898,
+            "velocityX": -0.32046694729784664,
+            "velocityY": -1.6410752823864576,
+            "timestamp": 0.20053324551169982
+        },
+        {
+            "x": 2.200016678146418,
+            "y": 5.304040986826963,
+            "heading": -2.963548114247805e-7,
+            "angularVelocity": -0.000002163524111560841,
+            "velocityX": -0.3738781966956197,
+            "velocityY": -1.9145877589558649,
+            "timestamp": 0.23395545309698312
+        },
+        {
+            "x": 2.1857357192573095,
+            "y": 5.230909847922857,
+            "heading": -3.7718073304542066e-7,
+            "angularVelocity": -0.0000024183297502514874,
+            "velocityX": -0.4272895155847271,
+            "velocityY": -2.1881001940865983,
+            "timestamp": 0.2673776606822664
+        },
+        {
+            "x": 2.169669632551276,
+            "y": 5.148637321647842,
+            "heading": -4.654625982128869e-7,
+            "angularVelocity": -0.0000026414132559778984,
+            "velocityX": -0.48070094307146977,
+            "velocityY": -2.4616125689825754,
+            "timestamp": 0.30079986826754973
+        },
+        {
+            "x": 2.15181841205741,
+            "y": 5.057223411125392,
+            "heading": -5.597189955047307e-7,
+            "angularVelocity": -0.0000028201725975039026,
+            "velocityX": -0.5341125492089794,
+            "velocityY": -2.735124850423542,
+            "timestamp": 0.33422207585283303
+        },
+        {
+            "x": 2.1321820469941515,
+            "y": 4.956668121725741,
+            "heading": -6.57784320236276e-7,
+            "angularVelocity": -0.0000029341365576412115,
+            "velocityX": -0.5875244779329194,
+            "velocityY": -3.008636971185932,
+            "timestamp": 0.36764428343811634
+        },
+        {
+            "x": 2.1107605140888954,
+            "y": 4.846971464532025,
+            "heading": -7.56114109936986e-7,
+            "angularVelocity": -0.0000029420493990798148,
+            "velocityX": -0.6409371029779476,
+            "velocityY": -3.282148760338516,
+            "timestamp": 0.40106649102339964
+        },
+        {
+            "x": 2.087553735835974,
+            "y": 4.728133474365802,
+            "heading": -8.465523402036467e-7,
+            "angularVelocity": -0.0000027059321418483588,
+            "velocityX": -0.6943520470083785,
+            "velocityY": -3.5556595076225634,
+            "timestamp": 0.43448869860868294
+        },
+        {
+            "x": 2.0633251233427066,
+            "y": 4.604042454619442,
+            "heading": -8.465523885072994e-7,
+            "angularVelocity": -1.4452562228577176e-12,
+            "velocityX": -0.724925558303899,
+            "velocityY": -3.712831339152857,
+            "timestamp": 0.46791090619396625
+        },
+        {
+            "x": 2.039088654512122,
+            "y": 4.479952969280304,
+            "heading": -8.465523654496814e-7,
+            "angularVelocity": 6.898891300327124e-13,
+            "velocityX": -0.725160621670614,
+            "velocityY": -3.7127854293461913,
+            "timestamp": 0.5013331137792496
+        },
+        {
+            "x": 2.0148473415026222,
+            "y": 4.355864430051467,
+            "heading": -8.465523174606992e-7,
+            "angularVelocity": 1.4358411666699279e-12,
+            "velocityX": -0.7253055606758912,
+            "velocityY": -3.712757121497041,
+            "timestamp": 0.5347553213645329
+        },
+        {
+            "x": 1.9907968545860597,
+            "y": 4.231738764257757,
+            "heading": -8.465522534990754e-7,
+            "angularVelocity": 1.9137462023851052e-12,
+            "velocityX": -0.7195960008785613,
+            "velocityY": -3.7138679567028667,
+            "timestamp": 0.5681775289498161
+        },
+        {
+            "x": 1.9669031646192998,
+            "y": 4.107582820499775,
+            "heading": -8.465521766194915e-7,
+            "angularVelocity": 2.3002544763334585e-12,
+            "velocityX": -0.7149046006511871,
+            "velocityY": -3.7147738802470687,
+            "timestamp": 0.6015997365350993
+        },
+        {
+            "x": 1.9504985155072192,
+            "y": 3.982217377752346,
+            "heading": -8.465520941748615e-7,
+            "angularVelocity": 2.4667619198655976e-12,
+            "velocityX": -0.4908308067350749,
+            "velocityY": -3.7509623631996885,
+            "timestamp": 0.6350219441203826
+        },
+        {
+            "x": 1.94330794196602,
+            "y": 3.855987816020002,
+            "heading": -8.465520093116816e-7,
+            "angularVelocity": 2.5391254773053977e-12,
+            "velocityX": -0.2151435844765948,
+            "velocityY": -3.7768169984060798,
+            "timestamp": 0.6684441517056658
+        },
+        {
+            "x": 1.9454293697222904,
+            "y": 3.7295714172218672,
+            "heading": -8.465519250884413e-7,
+            "angularVelocity": 2.519978339764665e-12,
+            "velocityX": 0.0634735976376205,
+            "velocityY": -3.7824072056149025,
+            "timestamp": 0.7018663592909491
+        },
+        {
+            "x": 1.9568513812515056,
+            "y": 3.603654207925799,
+            "heading": -8.465518451486746e-7,
+            "angularVelocity": 2.391815832767542e-12,
+            "velocityX": 0.34174916483985474,
+            "velocityY": -3.767471342961152,
+            "timestamp": 0.7352885668762323
+        },
+        {
+            "x": 1.9775120291721782,
+            "y": 3.4789195174340257,
+            "heading": -8.465517739710846e-7,
+            "angularVelocity": 2.129649524174011e-12,
+            "velocityX": 0.6181712523959566,
+            "velocityY": -3.7320901132416737,
+            "timestamp": 0.7687107744615156
+        },
+        {
+            "x": 2.0072992100178335,
+            "y": 3.3560442616931043,
+            "heading": -8.465517105809759e-7,
+            "angularVelocity": 1.8966463157464128e-12,
+            "velocityX": 0.8912391789143821,
+            "velocityY": -3.6764554055067418,
+            "timestamp": 0.8021329820467988
+        },
+        {
+            "x": 2.046051283468891,
+            "y": 3.235695267642527,
+            "heading": -8.465516527951994e-7,
+            "angularVelocity": 1.7289634182989629e-12,
+            "velocityX": 1.1594707905613406,
+            "velocityY": -3.600869085126494,
+            "timestamp": 0.8355551896320821
+        },
+        {
+            "x": 2.0935579534966013,
+            "y": 3.1185256537724477,
+            "heading": -8.46551596321545e-7,
+            "angularVelocity": 1.6897044173558387e-12,
+            "velocityX": 1.421410297528723,
+            "velocityY": -3.5057413120014775,
+            "timestamp": 0.8689773972173653
+        },
+        {
+            "x": 2.1495614051818848,
+            "y": 3.005171298980713,
+            "heading": -8.465515057383081e-7,
+            "angularVelocity": 2.7102707524537993e-12,
+            "velocityX": 1.6756359238847198,
+            "velocityY": -3.3915878986274275,
+            "timestamp": 0.9023996048026486
+        },
+        {
+            "x": 2.288743031901554,
+            "y": 2.8026748842621148,
+            "heading": -8.46551464739476e-7,
+            "angularVelocity": 6.312009526868921e-13,
+            "velocityX": 2.142782210187666,
+            "velocityY": -3.117550249366458,
+            "timestamp": 0.9673533019435205
+        },
+        {
+            "x": 2.455414708004788,
+            "y": 2.6221288200825,
+            "heading": -8.46551435217149e-7,
+            "angularVelocity": 4.5451345461171864e-13,
+            "velocityX": 2.566007532127087,
+            "velocityY": -2.779611817758129,
+            "timestamp": 1.0323069990843925
+        },
+        {
+            "x": 2.646160177401748,
+            "y": 2.4672337011827246,
+            "heading": -8.465514075797998e-7,
+            "angularVelocity": 4.254931139136813e-13,
+            "velocityX": 2.9366376017527633,
+            "velocityY": -2.384700574689416,
+            "timestamp": 1.0972606962252645
+        },
+        {
+            "x": 2.857069726290126,
+            "y": 2.341164292711998,
+            "heading": -8.465513787578765e-7,
+            "angularVelocity": 4.4373032078954734e-13,
+            "velocityX": 3.2470753501649696,
+            "velocityY": -1.9409119729950552,
+            "timestamp": 1.1622143933661364
+        },
+        {
+            "x": 3.0838173053505575,
+            "y": 2.2464971410417167,
+            "heading": -8.465513471236816e-7,
+            "angularVelocity": 4.870268741320482e-13,
+            "velocityX": 3.490911049579749,
+            "velocityY": -1.457455939200772,
+            "timestamp": 1.2271680905070084
+        },
+        {
+            "x": 3.3145112489857773,
+            "y": 2.1619002222864983,
+            "heading": -8.465513138187862e-7,
+            "angularVelocity": 5.127482864627404e-13,
+            "velocityX": 3.5516676307835056,
+            "velocityY": -1.3024188380298123,
+            "timestamp": 1.2921217876478803
+        },
+        {
+            "x": 3.5452013267301767,
+            "y": 2.0772927620920076,
+            "heading": -8.465512839726007e-7,
+            "angularVelocity": 4.594994306997602e-13,
+            "velocityX": 3.5516081131487938,
+            "velocityY": -1.302581129610464,
+            "timestamp": 1.3570754847887523
+        },
+        {
+            "x": 3.77589133724735,
+            "y": 1.9926851185956267,
+            "heading": -8.465512625373994e-7,
+            "angularVelocity": 3.3000743037279784e-13,
+            "velocityX": 3.5516070781466733,
+            "velocityY": -1.302583951649345,
+            "timestamp": 1.4220291819296242
+        },
+        {
+            "x": 4.006581361028944,
+            "y": 1.9080775112659911,
+            "heading": -8.465512535495126e-7,
+            "angularVelocity": 1.3837377280671698e-13,
+            "velocityX": 3.5516072823601217,
+            "velocityY": -1.302583394841325,
+            "timestamp": 1.4869828790704962
+        },
+        {
+            "x": 4.2372714040114055,
+            "y": 1.823469956289169,
+            "heading": -8.465512575529088e-7,
+            "angularVelocity": -6.163459506326445e-14,
+            "velocityX": 3.551607577968653,
+            "velocityY": -1.3025825888392473,
+            "timestamp": 1.5519365762113682
+        },
+        {
+            "x": 4.467961461705261,
+            "y": 1.7388624414245573,
+            "heading": -8.465512722492451e-7,
+            "angularVelocity": -2.2625863777166434e-13,
+            "velocityX": 3.5516078044591044,
+            "velocityY": -1.3025819712885556,
+            "timestamp": 1.6168902733522401
+        },
+        {
+            "x": 4.698651553926265,
+            "y": 1.6542550207002664,
+            "heading": -8.465512926518018e-7,
+            "angularVelocity": -3.141092229421582e-13,
+            "velocityX": 3.551608336024854,
+            "velocityY": -1.3025805219428788,
+            "timestamp": 1.681843970493112
+        },
+        {
+            "x": 4.929586887359619,
+            "y": 1.5703192949295044,
+            "heading": -8.465513064459796e-7,
+            "angularVelocity": -2.1236938120360531e-13,
+            "velocityX": 3.555383967330626,
+            "velocityY": -1.2922393869021438,
+            "timestamp": 1.746797667633984
+        },
+        {
+            "x": 5.059495672066219,
+            "y": 1.5327262216225606,
+            "heading": -8.465512750899748e-7,
+            "angularVelocity": 8.770995258558909e-13,
+            "velocityX": 3.633847296905686,
+            "velocityY": -1.051564666141291,
+            "timestamp": 1.7825473219513848
+        },
+        {
+            "x": 5.189548300230332,
+            "y": 1.4956338313822377,
+            "heading": -8.465512186056332e-7,
+            "angularVelocity": 1.579996862146135e-12,
+            "velocityX": 3.637870929029155,
+            "velocityY": -1.0375594099736098,
+            "timestamp": 1.8182969762687855
+        },
+        {
+            "x": 5.319623170079142,
+            "y": 1.4586195133360007,
+            "heading": -8.46551136487956e-7,
+            "angularVelocity": 2.2970201754243682e-12,
+            "velocityX": 3.638493080071459,
+            "velocityY": -1.0353755512600933,
+            "timestamp": 1.8540466305861862
+        },
+        {
+            "x": 5.4496989055100356,
+            "y": 1.42160823723968,
+            "heading": -8.465510387564672e-7,
+            "angularVelocity": 2.7337743725450157e-12,
+            "velocityX": 3.6385172923918394,
+            "velocityY": -1.0352904609291835,
+            "timestamp": 1.889796284903587
+        },
+        {
+            "x": 5.57977446957584,
+            "y": 1.3845963588884114,
+            "heading": -8.465509335637302e-7,
+            "angularVelocity": 2.9424826347181554e-12,
+            "velocityX": 3.6385124989164392,
+            "velocityY": -1.03530730738433,
+            "timestamp": 1.9255459392209877
+        },
+        {
+            "x": 5.709850057975644,
+            "y": 1.3475845660573411,
+            "heading": -8.465508277220688e-7,
+            "angularVelocity": 2.960634540295608e-12,
+            "velocityX": 3.6385131795943324,
+            "velocityY": -1.0353049151878095,
+            "timestamp": 1.9612955935383884
+        },
+        {
+            "x": 5.839925662966751,
+            "y": 1.3105728315354166,
+            "heading": -8.465507264753305e-7,
+            "angularVelocity": 2.832103981671134e-12,
+            "velocityX": 3.6385136436912036,
+            "velocityY": -1.0353032841469862,
+            "timestamp": 1.9970452478557892
+        },
+        {
+            "x": 5.970001275498046,
+            "y": 1.273561123513075,
+            "heading": -8.465506337302748e-7,
+            "angularVelocity": 2.594292378130792e-12,
+            "velocityX": 3.638513854607602,
+            "velocityY": -1.0353025428927405,
+            "timestamp": 2.03279490217319
+        },
+        {
+            "x": 6.100076894564937,
+            "y": 1.2365494384597497,
+            "heading": -8.46550552278632e-7,
+            "angularVelocity": 2.2783896618382965e-12,
+            "velocityX": 3.6385140374232323,
+            "velocityY": -1.0353019003965735,
+            "timestamp": 2.0685445564905907
+        },
+        {
+            "x": 6.230152520105601,
+            "y": 1.1995377761581834,
+            "heading": -8.465504825990077e-7,
+            "angularVelocity": 1.9490992420524453e-12,
+            "velocityX": 3.6385142185095396,
+            "velocityY": -1.03530126397757,
+            "timestamp": 2.1042942108079914
+        },
+        {
+            "x": 6.3602281516940815,
+            "y": 1.16252613511139,
+            "heading": -8.465504225140753e-7,
+            "angularVelocity": 1.680713660601537e-12,
+            "velocityX": 3.6385143876808734,
+            "velocityY": -1.0353006694327234,
+            "timestamp": 2.140043865125392
+        },
+        {
+            "x": 6.490303788775648,
+            "y": 1.1255145133698101,
+            "heading": -8.465503700121188e-7,
+            "angularVelocity": 1.4686003981542915e-12,
+            "velocityX": 3.638514541335137,
+            "velocityY": -1.0353001294215285,
+            "timestamp": 2.175793519442793
+        },
+        {
+            "x": 6.62037943067546,
+            "y": 1.0885029085617548,
+            "heading": -8.465503232940394e-7,
+            "angularVelocity": 1.306812060469801e-12,
+            "velocityX": 3.638514676112538,
+            "velocityY": -1.0352996557519205,
+            "timestamp": 2.2115431737601936
+        },
+        {
+            "x": 6.7504550766965234,
+            "y": 1.0514913182376744,
+            "heading": -8.465502808582129e-7,
+            "angularVelocity": 1.187027603302574e-12,
+            "velocityX": 3.638514791393393,
+            "velocityY": -1.0352992506018621,
+            "timestamp": 2.2472928280775943
+        },
+        {
+            "x": 6.880530727542945,
+            "y": 1.0144797448710672,
+            "heading": -8.465502433332782e-7,
+            "angularVelocity": 1.0496586699357012e-12,
+            "velocityX": 3.6385149263697616,
+            "velocityY": -1.0352987762623522,
+            "timestamp": 2.283042482394995
+        },
+        {
+            "x": 7.003525953233288,
+            "y": 0.9794828656529073,
+            "heading": -7.145353891597533e-7,
+            "angularVelocity": 0.000003692758900357643,
+            "velocityX": 3.440459160761053,
+            "velocityY": -0.9789431502593731,
+            "timestamp": 2.318792136712396
+        },
+        {
+            "x": 7.116271603762692,
+            "y": 0.9474023877264399,
+            "heading": -5.944969754078434e-7,
+            "angularVelocity": 0.0000033577503345576707,
+            "velocityX": 3.153754985388111,
+            "velocityY": -0.8973647029323183,
+            "timestamp": 2.3545417910297965
+        },
+        {
+            "x": 7.218767661219051,
+            "y": 0.9182383156679585,
+            "heading": -4.859087151281335e-7,
+            "angularVelocity": 0.0000030374632243726353,
+            "velocityX": 2.867050308972391,
+            "velocityY": -0.8157861275958178,
+            "timestamp": 2.3902914453471973
+        },
+        {
+            "x": 7.311014119553375,
+            "y": 0.8919906507276494,
+            "heading": -3.8850367492824467e-7,
+            "angularVelocity": 0.000002724642854703664,
+            "velocityX": 2.580345463352478,
+            "velocityY": -0.7342075172887301,
+            "timestamp": 2.426041099664598
+        },
+        {
+            "x": 7.393010975686544,
+            "y": 0.8686593933386082,
+            "heading": -3.0210953963079075e-7,
+            "angularVelocity": 0.0000024166425366030095,
+            "velocityX": 2.293640531602499,
+            "velocityY": -0.6526288948669626,
+            "timestamp": 2.4617907539819988
+        },
+        {
+            "x": 7.464758227730078,
+            "y": 0.8482445436165693,
+            "heading": -2.2661033918382436e-7,
+            "angularVelocity": 0.000002111886167406066,
+            "velocityX": 2.006935547027439,
+            "velocityY": -0.5710502692078399,
+            "timestamp": 2.4975404082993995
+        },
+        {
+            "x": 7.526255874392654,
+            "y": 0.8307461015250325,
+            "heading": -1.6193015142138781e-7,
+            "angularVelocity": 0.0000018092535156938384,
+            "velocityX": 1.7202305263310733,
+            "velocityY": -0.48947164456971237,
+            "timestamp": 2.5332900626168002
+        },
+        {
+            "x": 7.577503914725456,
+            "y": 0.8161640669449958,
+            "heading": -1.0802379214331377e-7,
+            "angularVelocity": 0.0000015078847699136293,
+            "velocityX": 1.4335254790941394,
+            "velocityY": -0.4078930232603403,
+            "timestamp": 2.569039716934201
+        },
+        {
+            "x": 7.618502347994633,
+            "y": 0.8044984397090525,
+            "heading": -6.487055743300418e-8,
+            "angularVelocity": 0.0000012070951602673883,
+            "velocityX": 1.1468204113297347,
+            "velocityY": -0.3263144066337203,
+            "timestamp": 2.6047893712516017
+        },
+        {
+            "x": 7.649251173610278,
+            "y": 0.7957492196197445,
+            "heading": -3.246962719134405e-8,
+            "angularVelocity": 9.063284906782187e-7,
+            "velocityX": 0.8601153270642448,
+            "velocityY": -0.24473579553046104,
+            "timestamp": 2.6405390255690024
+        },
+        {
+            "x": 7.669750391083677,
+            "y": 0.7899164064601075,
+            "heading": -1.0836483282929717e-8,
+            "angularVelocity": 6.051287577519143e-7,
+            "velocityX": 0.573410229128317,
+            "velocityY": -0.16315719049619,
+            "timestamp": 2.676288679886403
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 2.0494644799121527e-30,
+            "angularVelocity": 3.031213445055282e-7,
+            "velocityX": 0.28670511958863415,
+            "velocityY": -0.08157859189950141,
+            "timestamp": 2.712038334203804
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -1.7051999701665118e-29,
+            "angularVelocity": -2.5846596071248955e-28,
+            "velocityX": 4.524644517512267e-29,
+            "velocityY": 1.710870028377411e-29,
+            "timestamp": 2.7477879885212046
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W2 To S1.traj b/src/main/deploy/choreo/W2 To S1.traj
new file mode 100644
index 00000000..2aaf78f3
--- /dev/null
+++ b/src/main/deploy/choreo/W2 To S1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 4.4337460853781175e-35,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3070751783284065,
+            "y": 5.580871519986164,
+            "heading": 0.00875997642576413,
+            "angularVelocity": 0.10256751513529143,
+            "velocityX": 0.6682733985254588,
+            "velocityY": 0.24437736336610263,
+            "timestamp": 0.08540692844327274
+        },
+        {
+            "x": 2.421225525991661,
+            "y": 5.622614560146346,
+            "heading": 0.026280802198455293,
+            "angularVelocity": 0.20514525100066663,
+            "velocityX": 1.3365466917484683,
+            "velocityY": 0.48875472893171557,
+            "timestamp": 0.1708138568865455
+        },
+        {
+            "x": 2.592451026856299,
+            "y": 5.685229121649564,
+            "heading": 0.052563939294491345,
+            "angularVelocity": 0.30774010463909035,
+            "velocityX": 2.0048197960703633,
+            "velocityY": 0.7331321081849562,
+            "timestamp": 0.25622078532981823
+        },
+        {
+            "x": 2.8207516562305845,
+            "y": 5.768715208168554,
+            "heading": 0.0876113178799828,
+            "angularVelocity": 0.41035755792072426,
+            "velocityX": 2.673092611285307,
+            "velocityY": 0.9775095304409794,
+            "timestamp": 0.341627713773091
+        },
+        {
+            "x": 3.106127374237044,
+            "y": 5.873072825511933,
+            "heading": 0.13142500230326418,
+            "angularVelocity": 0.5129991819385286,
+            "velocityX": 3.341364959588794,
+            "velocityY": 1.221887020708095,
+            "timestamp": 0.4270346422163637
+        },
+        {
+            "x": 3.40956412390744,
+            "y": 5.984036121763709,
+            "heading": 0.13142500754936084,
+            "angularVelocity": 6.142471997127927e-8,
+            "velocityX": 3.5528352933560723,
+            "velocityY": 1.299230615997127,
+            "timestamp": 0.5124415706596365
+        },
+        {
+            "x": 3.7130008735619784,
+            "y": 6.094999418058892,
+            "heading": 0.1314250127954367,
+            "angularVelocity": 6.142447634082945e-8,
+            "velocityX": 3.5528352931703995,
+            "velocityY": 1.2992306165053664,
+            "timestamp": 0.5978484991029092
+        },
+        {
+            "x": 4.016437623216516,
+            "y": 6.205962714354075,
+            "heading": 0.13142501804151255,
+            "angularVelocity": 6.142447606684555e-8,
+            "velocityX": 3.552835293170399,
+            "velocityY": 1.2992306165053664,
+            "timestamp": 0.683255427546182
+        },
+        {
+            "x": 4.3198743728836435,
+            "y": 6.316926010614789,
+            "heading": 0.13142502328760913,
+            "angularVelocity": 6.142471942956278e-8,
+            "velocityX": 3.5528352933178016,
+            "velocityY": 1.2992306161017806,
+            "timestamp": 0.7686623559894548
+        },
+        {
+            "x": 4.605249584503867,
+            "y": 6.421283973215489,
+            "heading": 0.17528171773503587,
+            "angularVelocity": 0.5135027713419801,
+            "velocityX": 3.3413590304886087,
+            "velocityY": 1.2218910632059061,
+            "timestamp": 0.8540692844327276
+        },
+        {
+            "x": 4.83354975240058,
+            "y": 6.5047703704067485,
+            "heading": 0.21036850612417926,
+            "angularVelocity": 0.4108189935954435,
+            "velocityX": 2.6730872080050085,
+            "velocityY": 0.9775131679944534,
+            "timestamp": 0.9394762128760004
+        },
+        {
+            "x": 5.004774875721697,
+            "y": 6.567385181417454,
+            "heading": 0.23668415165777779,
+            "angularVelocity": 0.3081207346202291,
+            "velocityX": 2.0048153755446823,
+            "velocityY": 0.7331350295812797,
+            "timestamp": 1.0248831413192732
+        },
+        {
+            "x": 5.118924957871047,
+            "y": 6.6091283935214395,
+            "heading": 0.25422806054578806,
+            "angularVelocity": 0.20541552316405948,
+            "velocityX": 1.3365435829385781,
+            "velocityY": 0.48875674216187737,
+            "timestamp": 1.110290069762546
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.263,
+            "angularVelocity": 0.10270758607175808,
+            "velocityX": 0.6682718038134577,
+            "velocityY": 0.2443783760754632,
+            "timestamp": 1.1956969982058188
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.263,
+            "angularVelocity": 1.86588452749739e-32,
+            "velocityX": -1.0770205854918045e-34,
+            "velocityY": 0,
+            "timestamp": 1.2811039266490916
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W2 To S2.traj b/src/main/deploy/choreo/W2 To S2.traj
new file mode 100644
index 00000000..96b0e72c
--- /dev/null
+++ b/src/main/deploy/choreo/W2 To S2.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3319036557469652,
+            "y": 5.56,
+            "heading": -1.815106931159875e-19,
+            "angularVelocity": -1.831407645203353e-18,
+            "velocityX": 0.8263919813283321,
+            "velocityY": 8.319303153097966e-34,
+            "timestamp": 0.099109935233537
+        },
+        {
+            "x": 2.4957109641637625,
+            "y": 5.56,
+            "heading": -5.438547016393454e-19,
+            "angularVelocity": -3.655980680963076e-18,
+            "velocityX": 1.6527839316089838,
+            "velocityY": 1.3987786573815764e-33,
+            "timestamp": 0.198219870467074
+        },
+        {
+            "x": 2.741421919096125,
+            "y": 5.56,
+            "heading": -1.0860329369861945e-18,
+            "angularVelocity": -5.4704731072674304e-18,
+            "velocityX": 2.4791758197942864,
+            "velocityY": 2.272018181833636e-33,
+            "timestamp": 0.297329805700611
+        },
+        {
+            "x": 3.0690365020812695,
+            "y": 5.56,
+            "heading": -1.80338172905721e-18,
+            "angularVelocity": -7.237910009033217e-18,
+            "velocityX": 3.3055675216936846,
+            "velocityY": 3.0574601440031788e-33,
+            "timestamp": 0.396439740934148
+        },
+        {
+            "x": 3.4439634979187304,
+            "y": 5.56,
+            "heading": 1.7928981709293245e-18,
+            "angularVelocity": 3.6285765816640505e-17,
+            "velocityX": 3.7829405796099502,
+            "velocityY": 2.997011014948752e-33,
+            "timestamp": 0.495549676167685
+        },
+        {
+            "x": 3.771578080903875,
+            "y": 5.56,
+            "heading": 1.0808328664707184e-18,
+            "angularVelocity": -7.184600643588333e-18,
+            "velocityX": 3.305567521693685,
+            "velocityY": -4.3766441461200414e-33,
+            "timestamp": 0.594659611401222
+        },
+        {
+            "x": 4.017289035836238,
+            "y": 5.56,
+            "heading": 5.416591039281022e-19,
+            "angularVelocity": -5.4401585595028644e-18,
+            "velocityX": 2.479175819794286,
+            "velocityY": -5.119323199602909e-33,
+            "timestamp": 0.6937695466347591
+        },
+        {
+            "x": 4.181096344253034,
+            "y": 5.56,
+            "heading": 1.8086099379948042e-19,
+            "angularVelocity": -3.64038286599245e-18,
+            "velocityX": 1.6527839316089838,
+            "velocityY": -5.9637817525889115e-33,
+            "timestamp": 0.792879481868296
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": -1.8248523050897816e-18,
+            "velocityX": 0.8263919813283321,
+            "velocityY": -3.8230331267939286e-33,
+            "timestamp": 0.891989417101833
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.0684896902092895e-40,
+            "timestamp": 0.9910993523353699
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W2 To S3.traj b/src/main/deploy/choreo/W2 To S3.traj
new file mode 100644
index 00000000..7d72e43b
--- /dev/null
+++ b/src/main/deploy/choreo/W2 To S3.traj	
@@ -0,0 +1,526 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -4.99624931676796e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2447003077480563,
+            "y": 5.548424158968356,
+            "heading": -0.001651505925702968,
+            "angularVelocity": -0.042250731032121805,
+            "velocityX": -0.13558284496887443,
+            "velocityY": -0.2961465280181771,
+            "timestamp": 0.03908822132445813
+        },
+        {
+            "x": 2.234100924566356,
+            "y": 5.525272476708423,
+            "heading": -0.0049545293965937804,
+            "angularVelocity": -0.08450175932731885,
+            "velocityX": -0.27116565611205723,
+            "velocityY": -0.5922930610671583,
+            "timestamp": 0.07817644264891627
+        },
+        {
+            "x": 2.2182018518980375,
+            "y": 5.490544953203296,
+            "heading": -0.009909100000268336,
+            "angularVelocity": -0.12675354456654908,
+            "velocityX": -0.4067484303351874,
+            "velocityY": -0.8884395945486457,
+            "timestamp": 0.1172646639733744
+        },
+        {
+            "x": 2.1970030913105703,
+            "y": 5.444241588640874,
+            "heading": -0.016515264878529855,
+            "angularVelocity": -0.16900653584180803,
+            "velocityX": -0.5423311644575062,
+            "velocityY": -1.1845861232230646,
+            "timestamp": 0.15635288529783253
+        },
+        {
+            "x": 2.170504644501127,
+            "y": 5.386362383452732,
+            "heading": -0.024773088013563174,
+            "angularVelocity": -0.21126116398317848,
+            "velocityX": -0.6779138551608025,
+            "velocityY": -1.480732640856411,
+            "timestamp": 0.19544110662229067
+        },
+        {
+            "x": 2.1387065133083243,
+            "y": 5.31690733837683,
+            "heading": -0.03468264893636917,
+            "angularVelocity": -0.2535178267784212,
+            "velocityX": -0.8134964988264785,
+            "velocityY": -1.7768791396104282,
+            "timestamp": 0.2345293279467488
+        },
+        {
+            "x": 2.1016086997403236,
+            "y": 5.235876454566401,
+            "heading": -0.04624404029676127,
+            "angularVelocity": -0.2957768598481422,
+            "velocityX": -0.9490790911170489,
+            "velocityY": -2.0730256088611454,
+            "timestamp": 0.27361754927120696
+        },
+        {
+            "x": 2.0592112060450187,
+            "y": 5.143269733801358,
+            "heading": -0.05945736298631538,
+            "angularVelocity": -0.33803847404262444,
+            "velocityX": -1.0846616258994186,
+            "velocityY": -2.369172032575987,
+            "timestamp": 0.31270577059566507
+        },
+        {
+            "x": 2.011514034901109,
+            "y": 5.039087178972333,
+            "heading": -0.07432271508161363,
+            "angularVelocity": -0.3803025973454465,
+            "velocityX": -1.2202440921521842,
+            "velocityY": -2.6653183823394446,
+            "timestamp": 0.3517939919201232
+        },
+        {
+            "x": 1.9585171900495353,
+            "y": 4.923328795522167,
+            "heading": -0.09084016031123929,
+            "angularVelocity": -0.4225683510264515,
+            "velocityX": -1.3558264626999135,
+            "velocityY": -2.9614645928578263,
+            "timestamp": 0.3908822132445813
+        },
+        {
+            "x": 1.9002206796290688,
+            "y": 4.795994598697166,
+            "heading": -0.10900958241827326,
+            "angularVelocity": -0.46483113048874997,
+            "velocityX": -1.491408625032185,
+            "velocityY": -3.257610413327559,
+            "timestamp": 0.4299704345690394
+        },
+        {
+            "x": 1.8386671955063139,
+            "y": 4.661546724325594,
+            "heading": -0.10900958703871873,
+            "angularVelocity": -1.182055697467884e-7,
+            "velocityX": -1.5747322860207698,
+            "velocityY": -3.439600724104782,
+            "timestamp": 0.4690586558934975
+        },
+        {
+            "x": 1.7777943297173011,
+            "y": 4.526789325478927,
+            "heading": -0.10900959166091434,
+            "angularVelocity": -1.1825034389504455e-7,
+            "velocityX": -1.5573199221351706,
+            "velocityY": -3.447519336526256,
+            "timestamp": 0.5081468772179556
+        },
+        {
+            "x": 1.728739704549767,
+            "y": 4.38729485969642,
+            "heading": -0.10900959636164997,
+            "angularVelocity": -1.2025964513922604e-7,
+            "velocityX": -1.2549720479816096,
+            "velocityY": -3.5687084511883413,
+            "timestamp": 0.5472350985424137
+        },
+        {
+            "x": 1.6918738865194354,
+            "y": 4.244095790476986,
+            "heading": -0.10900960128999353,
+            "angularVelocity": -1.2608257379035844e-7,
+            "velocityX": -0.9431439134643449,
+            "velocityY": -3.6634838927766116,
+            "timestamp": 0.5863233198668718
+        },
+        {
+            "x": 1.6674707596733624,
+            "y": 4.098254956364247,
+            "heading": -0.10900960662650709,
+            "angularVelocity": -1.3652485001388814e-7,
+            "velocityX": -0.6243089610931135,
+            "velocityY": -3.7310685718382324,
+            "timestamp": 0.6254115411913299
+        },
+        {
+            "x": 1.6557115284805652,
+            "y": 3.9508548842430677,
+            "heading": -0.1090096126146074,
+            "angularVelocity": -1.531944945007552e-7,
+            "velocityX": -0.3008382268193907,
+            "velocityY": -3.7709587985000237,
+            "timestamp": 0.664499762515788
+        },
+        {
+            "x": 1.6566835066189385,
+            "y": 3.8029896922639588,
+            "heading": -0.10900961961577929,
+            "angularVelocity": -1.79112061302596e-7,
+            "velocityX": 0.024866266754091815,
+            "velocityY": -3.7828580316236344,
+            "timestamp": 0.7035879838402461
+        },
+        {
+            "x": 1.6703794917330834,
+            "y": 3.6557569809887474,
+            "heading": -0.10900969829200789,
+            "angularVelocity": -0.000002012786100988185,
+            "velocityX": 0.3503865013576149,
+            "velocityY": -3.766677180142832,
+            "timestamp": 0.7426762051647042
+        },
+        {
+            "x": 1.6964297342683634,
+            "y": 3.5114772969924033,
+            "heading": -0.1124967438020359,
+            "angularVelocity": -0.0892096235606636,
+            "velocityX": 0.6664473760284892,
+            "velocityY": -3.6911294274233937,
+            "timestamp": 0.7817644264891623
+        },
+        {
+            "x": 1.7337062034817727,
+            "y": 3.3729422790966246,
+            "heading": -0.12243166775651844,
+            "angularVelocity": -0.2541666931326176,
+            "velocityX": 0.9536496660724848,
+            "velocityY": -3.54416274779685,
+            "timestamp": 0.8208526478136204
+        },
+        {
+            "x": 1.7809992442628195,
+            "y": 3.2421845220219927,
+            "heading": -0.1367067464310455,
+            "angularVelocity": -0.3652015413039525,
+            "velocityX": 1.2099051626962352,
+            "velocityY": -3.3451958836719715,
+            "timestamp": 0.8599408691380785
+        },
+        {
+            "x": 1.8374227958909133,
+            "y": 3.120270451905998,
+            "heading": -0.154016140064332,
+            "angularVelocity": -0.4428288892862246,
+            "velocityX": 1.443492431128254,
+            "velocityY": -3.118946475052685,
+            "timestamp": 0.8990290904625367
+        },
+        {
+            "x": 1.9023445230612375,
+            "y": 3.007828092475748,
+            "heading": -0.17349166731783877,
+            "angularVelocity": -0.4982454200677405,
+            "velocityX": 1.660902567846671,
+            "velocityY": -2.8766302384778886,
+            "timestamp": 0.9381173117869948
+        },
+        {
+            "x": 1.9753011558084586,
+            "y": 2.90526251520286,
+            "heading": -0.19452421295735298,
+            "angularVelocity": -0.5380788617862533,
+            "velocityX": 1.866460797529241,
+            "velocityY": -2.623950995915806,
+            "timestamp": 0.9772055331114529
+        },
+        {
+            "x": 2.0559421744907826,
+            "y": 2.8128534355639654,
+            "heading": -0.21666661660279088,
+            "angularVelocity": -0.5664725304751266,
+            "velocityX": 2.0630516291077963,
+            "velocityY": -2.3641157491367464,
+            "timestamp": 1.016293754435911
+        },
+        {
+            "x": 2.1439944855858766,
+            "y": 2.7308042438703497,
+            "heading": -0.2395781801271676,
+            "angularVelocity": -0.586150066389446,
+            "velocityX": 2.252655866948419,
+            "velocityY": -2.099077136627731,
+            "timestamp": 1.055381975760369
+        },
+        {
+            "x": 2.2392399311065674,
+            "y": 2.659268856048584,
+            "heading": -0.2629913490343549,
+            "angularVelocity": -0.5989827143282399,
+            "velocityX": 2.4366789353272003,
+            "velocityY": -1.830100869209989,
+            "timestamp": 1.0944701970848272
+        },
+        {
+            "x": 2.3166964235934535,
+            "y": 2.610513248595422,
+            "heading": -0.2811797949710667,
+            "angularVelocity": -0.6046182408202432,
+            "velocityX": 2.5747998696792043,
+            "velocityY": -1.620728330008343,
+            "timestamp": 1.124552726371468
+        },
+        {
+            "x": 2.398213254515867,
+            "y": 2.5681176642029904,
+            "heading": -0.29946946527559826,
+            "angularVelocity": -0.6079831296849904,
+            "velocityX": 2.7097731758410535,
+            "velocityY": -1.409309170398157,
+            "timestamp": 1.154635255658109
+        },
+        {
+            "x": 2.483678189488588,
+            "y": 2.5321524360945276,
+            "heading": -0.31778023201379,
+            "angularVelocity": -0.6086844149213316,
+            "velocityX": 2.8410155993982453,
+            "velocityY": -1.1955520018204469,
+            "timestamp": 1.1847177849447499
+        },
+        {
+            "x": 2.5729562458095865,
+            "y": 2.5026984837196036,
+            "heading": -0.33601688083426684,
+            "angularVelocity": -0.6062205955726221,
+            "velocityX": 2.9677709434035733,
+            "velocityY": -0.9791049181495252,
+            "timestamp": 1.2148003142313908
+        },
+        {
+            "x": 2.665882293188928,
+            "y": 2.479849558811985,
+            "heading": -0.35406456410202075,
+            "angularVelocity": -0.5999390242685017,
+            "velocityX": 3.08903704518594,
+            "velocityY": -0.7595413500609144,
+            "timestamp": 1.2448828435180317
+        },
+        {
+            "x": 2.7622502226261916,
+            "y": 2.4637149411085275,
+            "heading": -0.37178230702948656,
+            "angularVelocity": -0.5889711851900935,
+            "velocityX": 3.2034516951353296,
+            "velocityY": -0.5363451174508257,
+            "timestamp": 1.2749653728046726
+        },
+        {
+            "x": 2.8617965704136163,
+            "y": 2.4544224370162078,
+            "heading": -0.3889934776908366,
+            "angularVelocity": -0.57213176782302,
+            "velocityX": 3.309108314626244,
+            "velocityY": -0.30890035886875983,
+            "timestamp": 1.3050479020913135
+        },
+        {
+            "x": 2.96417486527032,
+            "y": 2.452120989561553,
+            "heading": -0.40547141981393114,
+            "angularVelocity": -0.5477578685651567,
+            "velocityX": 3.4032475754010525,
+            "velocityY": -0.07650445322366382,
+            "timestamp": 1.3351304313779544
+        },
+        {
+            "x": 3.068913958464018,
+            "y": 2.456980552046256,
+            "heading": -0.4209173641629785,
+            "angularVelocity": -0.5134523165215639,
+            "velocityX": 3.4817249638713506,
+            "velocityY": 0.16154102065001244,
+            "timestamp": 1.3652129606645953
+        },
+        {
+            "x": 3.175348297900835,
+            "y": 2.469181868617686,
+            "heading": -0.43492680962689906,
+            "angularVelocity": -0.4657003847794821,
+            "velocityX": 3.5380781457121575,
+            "velocityY": 0.4055947708108251,
+            "timestamp": 1.3952954899512362
+        },
+        {
+            "x": 3.2825012821379658,
+            "y": 2.488873812540701,
+            "heading": -0.44694408076253883,
+            "angularVelocity": -0.3994767534718848,
+            "velocityX": 3.5619672540204803,
+            "velocityY": 0.6545973490252659,
+            "timestamp": 1.425378019237877
+        },
+        {
+            "x": 3.3889723525651156,
+            "y": 2.516050236824646,
+            "heading": -0.4565561347315361,
+            "angularVelocity": -0.3195227993435668,
+            "velocityX": 3.539299153094488,
+            "velocityY": 0.9033955896790181,
+            "timestamp": 1.455460548524518
+        },
+        {
+            "x": 3.4932758480756925,
+            "y": 2.5504542246131905,
+            "heading": -0.4662703499641935,
+            "angularVelocity": -0.32291883239253294,
+            "velocityX": 3.4672448754798637,
+            "velocityY": 1.1436534295611822,
+            "timestamp": 1.485543077811159
+        },
+        {
+            "x": 3.5930108067726363,
+            "y": 2.5908608883384305,
+            "heading": -0.4766346317588353,
+            "angularVelocity": -0.3445282707409342,
+            "velocityX": 3.315378096922126,
+            "velocityY": 1.3431936969204072,
+            "timestamp": 1.5156256070977998
+        },
+        {
+            "x": 3.686096614652251,
+            "y": 2.634834387797071,
+            "heading": -0.48720467819938723,
+            "angularVelocity": -0.3513682755805826,
+            "velocityX": 3.094347785475507,
+            "velocityY": 1.4617620426674243,
+            "timestamp": 1.5457081363844407
+        },
+        {
+            "x": 3.771715868998783,
+            "y": 2.679896340499022,
+            "heading": -0.4975881116213702,
+            "angularVelocity": -0.34516490694787005,
+            "velocityX": 2.8461454663839936,
+            "velocityY": 1.497944280967274,
+            "timestamp": 1.5757906656710816
+        },
+        {
+            "x": 3.8498203373891897,
+            "y": 2.724287723719235,
+            "heading": -0.507530883254748,
+            "angularVelocity": -0.33051647814035945,
+            "velocityX": 2.596339810598824,
+            "velocityY": 1.475653286903962,
+            "timestamp": 1.6058731949577225
+        },
+        {
+            "x": 3.920599661686896,
+            "y": 2.7668759161025953,
+            "heading": -0.5168796397769617,
+            "angularVelocity": -0.3107702957131982,
+            "velocityX": 2.35283820796086,
+            "velocityY": 1.415711823216824,
+            "timestamp": 1.6359557242443634
+        },
+        {
+            "x": 3.984278053809844,
+            "y": 2.806919241781621,
+            "heading": -0.525538519539268,
+            "angularVelocity": -0.287837491315874,
+            "velocityX": 2.11678983227083,
+            "velocityY": 1.3311156551191636,
+            "timestamp": 1.6660382535310043
+        },
+        {
+            "x": 4.041059171114166,
+            "y": 2.843909698702473,
+            "heading": -0.5334440710807248,
+            "angularVelocity": -0.26279544070665484,
+            "velocityX": 1.8875114111345854,
+            "velocityY": 1.229632540814369,
+            "timestamp": 1.6961207828176452
+        },
+        {
+            "x": 4.091116341717457,
+            "y": 2.8774832028649056,
+            "heading": -0.5405519914461838,
+            "angularVelocity": -0.23628067632639732,
+            "velocityX": 1.6639947434713853,
+            "velocityY": 1.11604658778956,
+            "timestamp": 1.7262033121042861
+        },
+        {
+            "x": 4.1345948250930045,
+            "y": 2.9073683770064043,
+            "heading": -0.5468299348831553,
+            "angularVelocity": -0.20869067813911088,
+            "velocityX": 1.445306774615446,
+            "velocityY": 0.9934395428236782,
+            "timestamp": 1.756285841390927
+        },
+        {
+            "x": 4.171616484177909,
+            "y": 2.933356232054327,
+            "heading": -0.552253409487953,
+            "angularVelocity": -0.18028652288901045,
+            "velocityX": 1.2306697595852323,
+            "velocityY": 0.863885306993297,
+            "timestamp": 1.786368370677568
+        },
+        {
+            "x": 4.202284260430688,
+            "y": 2.9552814196152344,
+            "heading": -0.5568033106587852,
+            "angularVelocity": -0.15124729464987938,
+            "velocityX": 1.0194547127524736,
+            "velocityY": 0.7288345787681281,
+            "timestamp": 1.8164508999642088
+        },
+        {
+            "x": 4.226685878711894,
+            "y": 2.973010149818714,
+            "heading": -0.5604643670760648,
+            "angularVelocity": -0.12170041895060278,
+            "velocityX": 0.8111558057069341,
+            "velocityY": 0.5893364229633967,
+            "timestamp": 1.8465334292508497
+        },
+        {
+            "x": 4.244896778926994,
+            "y": 2.98643211128414,
+            "heading": -0.5632241207030344,
+            "angularVelocity": -0.09173941461743668,
+            "velocityX": 0.6053646633758383,
+            "velocityY": 0.4461713088529227,
+            "timestamp": 1.8766159585374906
+        },
+        {
+            "x": 4.256982406189367,
+            "y": 2.9954548894256625,
+            "heading": -0.565072233243941,
+            "angularVelocity": -0.06143474583855434,
+            "velocityX": 0.4017490400230914,
+            "velocityY": 0.2999341596429636,
+            "timestamp": 1.9066984878241315
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -0.030840716457660636,
+            "velocityX": 0.20003616561938206,
+            "velocityY": 0.15108804618886046,
+            "timestamp": 1.9367810171107724
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -4.9732030554004765e-30,
+            "velocityX": -4.223945033236224e-32,
+            "velocityY": 9.969983495378875e-32,
+            "timestamp": 1.9668635463974133
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W2 To S4.traj b/src/main/deploy/choreo/W2 To S4.traj
new file mode 100644
index 00000000..848b8926
--- /dev/null
+++ b/src/main/deploy/choreo/W2 To S4.traj	
@@ -0,0 +1,553 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 1.4317938010297452e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2473468901462774,
+            "y": 5.551784954334613,
+            "heading": -0.004046039178700474,
+            "angularVelocity": -0.12521248615625988,
+            "velocityX": -0.08210560159156222,
+            "velocityY": -0.2542304328304919,
+            "timestamp": 0.032313384255078216
+        },
+        {
+            "x": 2.2420392078855844,
+            "y": 5.535352460743813,
+            "heading": -0.012062241078351911,
+            "angularVelocity": -0.24807682898124367,
+            "velocityX": -0.16425646471427996,
+            "velocityY": -0.5085352082308542,
+            "timestamp": 0.06462676851015643
+        },
+        {
+            "x": 2.234075272195187,
+            "y": 5.510699784095841,
+            "heading": -0.023960373728225737,
+            "angularVelocity": -0.36821066329520014,
+            "velocityX": -0.24645935032774877,
+            "velocityY": -0.7629246275588465,
+            "timestamp": 0.09694015276523466
+        },
+        {
+            "x": 2.2234531308615284,
+            "y": 5.477823761292619,
+            "heading": -0.03963572188039383,
+            "angularVelocity": -0.4851038822931769,
+            "velocityX": -0.32872265095504705,
+            "velocityY": -1.017411935057708,
+            "timestamp": 0.12925353702031286
+        },
+        {
+            "x": 2.2101704924088987,
+            "y": 5.436720678146679,
+            "heading": -0.05896132607089341,
+            "angularVelocity": -0.5980680958065965,
+            "velocityX": -0.4110568657178767,
+            "velocityY": -1.27201418525146,
+            "timestamp": 0.16156692127539107
+        },
+        {
+            "x": 2.1942246366351648,
+            "y": 5.38738610923438,
+            "heading": -0.08177969135834087,
+            "angularVelocity": -0.7061583245915709,
+            "velocityX": -0.4934752623822669,
+            "velocityY": -1.5267533887152571,
+            "timestamp": 0.19388030553046928
+        },
+        {
+            "x": 2.1756122932404036,
+            "y": 5.329814710705053,
+            "heading": -0.10789037911156549,
+            "angularVelocity": -0.8080455933402793,
+            "velocityX": -0.5759948647853634,
+            "velocityY": -1.7816579679449487,
+            "timestamp": 0.2261936897855475
+        },
+        {
+            "x": 2.1543294701171383,
+            "y": 5.263999960188385,
+            "heading": -0.1370304714941401,
+            "angularVelocity": -0.9017963625397062,
+            "velocityX": -0.6586380106540685,
+            "velocityY": -2.0367643945039555,
+            "timestamp": 0.25850707404062573
+        },
+        {
+            "x": 2.1303711997350336,
+            "y": 5.1899338649176165,
+            "heading": -0.1688416775308267,
+            "angularVelocity": -0.9844591264590365,
+            "velocityX": -0.7414348863300811,
+            "velocityY": -2.2921181726463207,
+            "timestamp": 0.29082045829570397
+        },
+        {
+            "x": 2.1037311567595207,
+            "y": 5.107606785885568,
+            "heading": -0.20280965108227242,
+            "angularVelocity": -1.051204457054369,
+            "velocityX": -0.8244275116843016,
+            "velocityY": -2.5477702484570597,
+            "timestamp": 0.3231338425507822
+        },
+        {
+            "x": 2.074401135228672,
+            "y": 5.017008143765364,
+            "heading": -0.23813648207052604,
+            "angularVelocity": -1.0932569213236978,
+            "velocityX": -0.9076740863575303,
+            "velocityY": -2.8037497219427316,
+            "timestamp": 0.35544722680586044
+        },
+        {
+            "x": 2.0423710011934597,
+            "y": 4.918132506074763,
+            "heading": -0.27341197795784133,
+            "angularVelocity": -1.0916682576129122,
+            "velocityX": -0.9912342756292476,
+            "velocityY": -3.0598973140692034,
+            "timestamp": 0.3877606110609387
+        },
+        {
+            "x": 2.00763899967256,
+            "y": 4.811032729430715,
+            "heading": -0.3053550995537523,
+            "angularVelocity": -0.9885415078704507,
+            "velocityX": -1.0748487761829064,
+            "velocityY": -3.3144091562374,
+            "timestamp": 0.4200739953160169
+        },
+        {
+            "x": 1.9707337568018743,
+            "y": 4.697468766706309,
+            "heading": -0.3150299299334663,
+            "angularVelocity": -0.2994062863655037,
+            "velocityX": -1.1421039213769328,
+            "velocityY": -3.514455862250325,
+            "timestamp": 0.45238737957109515
+        },
+        {
+            "x": 1.9331993710024595,
+            "y": 4.581134395027877,
+            "heading": -0.3150299515537849,
+            "angularVelocity": -6.690824588937528e-7,
+            "velocityX": -1.1615739627617825,
+            "velocityY": -3.6001915107405935,
+            "timestamp": 0.4847007638261734
+        },
+        {
+            "x": 1.8956658385726595,
+            "y": 4.464799748019651,
+            "heading": -0.3150299731737692,
+            "angularVelocity": -6.690721139443934e-7,
+            "velocityX": -1.1615475535931208,
+            "velocityY": -3.600200031352131,
+            "timestamp": 0.5170141480812516
+        },
+        {
+            "x": 1.8630740613066095,
+            "y": 4.346985096208152,
+            "heading": -0.31502999489570194,
+            "angularVelocity": -6.722271053919432e-7,
+            "velocityX": -1.0086154086732113,
+            "velocityY": -3.64600163453891,
+            "timestamp": 0.5493275323363298
+        },
+        {
+            "x": 1.8389498768606893,
+            "y": 4.227149626761845,
+            "heading": -0.3150300170939308,
+            "angularVelocity": -6.869670074324573e-7,
+            "velocityX": -0.7465694170405304,
+            "velocityY": -3.708539733886721,
+            "timestamp": 0.581640916591408
+        },
+        {
+            "x": 1.8234162959667812,
+            "y": 4.10590102408459,
+            "heading": -0.31503004024876186,
+            "angularVelocity": -7.165709048863888e-7,
+            "velocityX": -0.4807166210536207,
+            "velocityY": -3.7522718672898008,
+            "timestamp": 0.6139543008464863
+        },
+        {
+            "x": 1.816552246405149,
+            "y": 3.98385431066073,
+            "heading": -0.315030064931292,
+            "angularVelocity": -7.63848502164674e-7,
+            "velocityX": -0.21242125267501,
+            "velocityY": -3.776970943694307,
+            "timestamp": 0.6462676851015645
+        },
+        {
+            "x": 1.818392595454208,
+            "y": 3.861628584870307,
+            "heading": -0.315030091871416,
+            "angularVelocity": -8.337140982566696e-7,
+            "velocityX": 0.05695315088421205,
+            "velocityY": -3.782510826646546,
+            "timestamp": 0.6785810693566428
+        },
+        {
+            "x": 1.828928030254088,
+            "y": 3.739843861498511,
+            "heading": -0.3150301220653929,
+            "angularVelocity": -9.344108498281474e-7,
+            "velocityX": 0.32603935003261814,
+            "velocityY": -3.7688631562216113,
+            "timestamp": 0.710894453611721
+        },
+        {
+            "x": 1.8481051197294465,
+            "y": 3.619117921789427,
+            "heading": -0.3150301569615331,
+            "angularVelocity": -0.0000010799283629257,
+            "velocityX": 0.593472021499706,
+            "velocityY": -3.7360970536569806,
+            "timestamp": 0.7432078378667992
+        },
+        {
+            "x": 1.875826590719106,
+            "y": 3.500063178055521,
+            "heading": -0.31503019880828736,
+            "angularVelocity": -0.0000012950285239246719,
+            "velocityX": 0.8578943873792161,
+            "velocityY": -3.684378670896923,
+            "timestamp": 0.7755212221218775
+        },
+        {
+            "x": 1.9119518234377388,
+            "y": 3.3832835666467993,
+            "heading": -0.31503025136937535,
+            "angularVelocity": -0.0000016266042432729645,
+            "velocityX": 1.1179650027822445,
+            "velocityY": -3.61397031294762,
+            "timestamp": 0.8078346063769557
+        },
+        {
+            "x": 1.9562975654606443,
+            "y": 3.269371484637439,
+            "heading": -0.31503032157308203,
+            "angularVelocity": -0.0000021725891085409373,
+            "velocityX": 1.3723645184560365,
+            "velocityY": -3.5252290849559675,
+            "timestamp": 0.8401479906320339
+        },
+        {
+            "x": 2.008638858795166,
+            "y": 3.158904790878296,
+            "heading": -0.3150304332740777,
+            "angularVelocity": -0.0000034568027529519608,
+            "velocityX": 1.6198022751608165,
+            "velocityY": -3.4186049002831713,
+            "timestamp": 0.8724613748871122
+        },
+        {
+            "x": 2.0775165641117233,
+            "y": 3.0394044472847606,
+            "heading": -0.3150305189714284,
+            "angularVelocity": -0.000002350392551393865,
+            "velocityX": 1.8890857675343722,
+            "velocityY": -3.277495922091173,
+            "timestamp": 0.9089222420961518
+        },
+        {
+            "x": 2.155767726009908,
+            "y": 2.9258208622381168,
+            "heading": -0.3150305770370975,
+            "angularVelocity": -0.00000159254767063084,
+            "velocityX": 2.1461684235196876,
+            "velocityY": -3.1152189660064753,
+            "timestamp": 0.9453831093051914
+        },
+        {
+            "x": 2.242886953120307,
+            "y": 2.818887620265158,
+            "heading": -0.3150306203630764,
+            "angularVelocity": -0.0000011882871183876306,
+            "velocityX": 2.389389879591231,
+            "velocityY": -2.932822232666113,
+            "timestamp": 0.981843976514231
+        },
+        {
+            "x": 2.338311580267833,
+            "y": 2.719295349641276,
+            "heading": -0.3150306549359537,
+            "angularVelocity": -9.482187337248934e-7,
+            "velocityX": 2.617179306252704,
+            "velocityY": -2.731483868798106,
+            "timestamp": 1.0183048437232707
+        },
+        {
+            "x": 2.4414253013619946,
+            "y": 2.6276872624602983,
+            "heading": -0.31503068395926054,
+            "angularVelocity": -7.960125215618009e-7,
+            "velocityX": 2.8280655120730875,
+            "velocityY": -2.512504342142078,
+            "timestamp": 1.0547657109323103
+        },
+        {
+            "x": 2.5515621486647997,
+            "y": 2.5446549983777076,
+            "heading": -0.3150307093352344,
+            "angularVelocity": -6.959783403374032e-7,
+            "velocityX": 3.020686443670201,
+            "velocityY": -2.2772981127010774,
+            "timestamp": 1.09122657814135
+        },
+        {
+            "x": 2.6680107915990137,
+            "y": 2.4707347968336117,
+            "heading": -0.31503073229503964,
+            "angularVelocity": -6.297108928988803e-7,
+            "velocityX": 3.1937979496369038,
+            "velocityY": -2.0273846236374995,
+            "timestamp": 1.1276874453503896
+        },
+        {
+            "x": 2.7900191236562386,
+            "y": 2.4064040113359084,
+            "heading": -0.3150307537024253,
+            "angularVelocity": -5.871332010760787e-7,
+            "velocityX": 3.3462816821585726,
+            "velocityY": -1.764378919702536,
+            "timestamp": 1.1641483125594292
+        },
+        {
+            "x": 2.9167990841955884,
+            "y": 2.352077913506683,
+            "heading": -0.3150307742165802,
+            "angularVelocity": -5.626348607586924e-7,
+            "velocityX": 3.477151539278742,
+            "velocityY": -1.4899836999970275,
+            "timestamp": 1.2006091797684688
+        },
+        {
+            "x": 3.0475311021850517,
+            "y": 2.308104934680324,
+            "heading": -0.31503079438993686,
+            "angularVelocity": -5.532878997498012e-7,
+            "velocityX": 3.585543296047864,
+            "velocityY": -1.2060321707174542,
+            "timestamp": 1.2370700469775084
+        },
+        {
+            "x": 3.178817728061225,
+            "y": 2.2658165761507623,
+            "heading": -0.3150308145386853,
+            "angularVelocity": -5.526129783451472e-7,
+            "velocityX": 3.600754340906743,
+            "velocityY": -1.159828653748452,
+            "timestamp": 1.273530914186548
+        },
+        {
+            "x": 3.3101043979960063,
+            "y": 2.2235283544039026,
+            "heading": -0.3150308346874411,
+            "angularVelocity": -5.526131795163358e-7,
+            "velocityX": 3.600755549287413,
+            "velocityY": -1.1598249022550946,
+            "timestamp": 1.3099917813955877
+        },
+        {
+            "x": 3.4413910762638236,
+            "y": 2.1812401585275256,
+            "heading": -0.31503085483620696,
+            "angularVelocity": -5.52613457009699e-7,
+            "velocityX": 3.60075577783481,
+            "velocityY": -1.159824192714005,
+            "timestamp": 1.3464526486046273
+        },
+        {
+            "x": 3.572677762945226,
+            "y": 2.138951988771722,
+            "heading": -0.31503087498498206,
+            "angularVelocity": -5.52613710159953e-7,
+            "velocityX": 3.6007560085914068,
+            "velocityY": -1.1598234763137998,
+            "timestamp": 1.382913515813667
+        },
+        {
+            "x": 3.7039644581047573,
+            "y": 2.0966638453368867,
+            "heading": -0.31503089513376514,
+            "angularVelocity": -5.526139295643368e-7,
+            "velocityX": 3.6007562411181984,
+            "velocityY": -1.1598227544173942,
+            "timestamp": 1.4193743830227066
+        },
+        {
+            "x": 3.8352511618245644,
+            "y": 2.05437572847807,
+            "heading": -0.3150309152825546,
+            "angularVelocity": -5.526141044747151e-7,
+            "velocityX": 3.600756475898009,
+            "velocityY": -1.1598220255258234,
+            "timestamp": 1.4558352502317462
+        },
+        {
+            "x": 3.966537874250263,
+            "y": 2.01208763864737,
+            "heading": -0.3150309354313486,
+            "angularVelocity": -5.526142273042023e-7,
+            "velocityX": 3.6007567146716046,
+            "velocityY": -1.159821284234734,
+            "timestamp": 1.4922961174407858
+        },
+        {
+            "x": 4.097824595645338,
+            "y": 1.9697995766628096,
+            "heading": -0.3150309555801447,
+            "angularVelocity": -5.526142864672618e-7,
+            "velocityX": 3.600756960671674,
+            "velocityY": -1.159820520507965,
+            "timestamp": 1.5287569846498255
+        },
+        {
+            "x": 4.229111326453571,
+            "y": 1.9275115439021684,
+            "heading": -0.3150309757289402,
+            "angularVelocity": -5.5261426880692e-7,
+            "velocityX": 3.600757218843208,
+            "velocityY": -1.1598197189933317,
+            "timestamp": 1.565217851858865
+        },
+        {
+            "x": 4.360398019008411,
+            "y": 1.885223392382287,
+            "heading": -0.31503099587881916,
+            "angularVelocity": -5.52643987126892e-7,
+            "velocityX": 3.6007561696802117,
+            "velocityY": -1.1598229761632481,
+            "timestamp": 1.6016787190679047
+        },
+        {
+            "x": 4.485161526388635,
+            "y": 1.844329974236374,
+            "heading": -0.3370638509456068,
+            "angularVelocity": -0.6042877406197901,
+            "velocityX": 3.4218469534726492,
+            "velocityY": -1.1215700907896822,
+            "timestamp": 1.6381395862769443
+        },
+        {
+            "x": 4.60044458089581,
+            "y": 1.8067761222092775,
+            "heading": -0.3770388194107599,
+            "angularVelocity": -1.0963800788381923,
+            "velocityX": 3.1618297460185825,
+            "velocityY": -1.0299769287381584,
+            "timestamp": 1.674600453485984
+        },
+        {
+            "x": 4.705219804759057,
+            "y": 1.772711102552083,
+            "heading": -0.4189139196427851,
+            "angularVelocity": -1.148494356756437,
+            "velocityX": 2.8736349923479074,
+            "velocityY": -0.9342898911836356,
+            "timestamp": 1.7110613206950236
+        },
+        {
+            "x": 4.799468970200479,
+            "y": 1.7421012617230478,
+            "heading": -0.45948472427125614,
+            "angularVelocity": -1.1127218778387968,
+            "velocityX": 2.5849403115144614,
+            "velocityY": -0.8395258580532651,
+            "timestamp": 1.7475221879040632
+        },
+        {
+            "x": 4.883206648654635,
+            "y": 1.7149242324789875,
+            "heading": -0.49729979894596704,
+            "angularVelocity": -1.037141394852447,
+            "velocityX": 2.296645276538986,
+            "velocityY": -0.7453752838144925,
+            "timestamp": 1.7839830551131028
+        },
+        {
+            "x": 4.956447680828415,
+            "y": 1.6911655730288653,
+            "heading": -0.5315269969459382,
+            "angularVelocity": -0.9387379023033031,
+            "velocityX": 2.0087572726635705,
+            "velocityY": -0.6516207997442213,
+            "timestamp": 1.8204439223221425
+        },
+        {
+            "x": 5.019204214404415,
+            "y": 1.6708154773305386,
+            "heading": -0.5616288277349688,
+            "angularVelocity": -0.825592836737831,
+            "velocityX": 1.7212024392124468,
+            "velocityY": -0.5581352627093101,
+            "timestamp": 1.856904789531182
+        },
+        {
+            "x": 5.071485921099441,
+            "y": 1.6538669506708767,
+            "heading": -0.587231204588931,
+            "angularVelocity": -0.7021878198118158,
+            "velocityX": 1.4339128687006073,
+            "velocityY": -0.46484156733002446,
+            "timestamp": 1.8933656567402217
+        },
+        {
+            "x": 5.11330055140079,
+            "y": 1.6403147844077772,
+            "heading": -0.6080594902506735,
+            "angularVelocity": -0.5712504187662296,
+            "velocityX": 1.1468358682094448,
+            "velocityY": -0.37169072763413913,
+            "timestamp": 1.9298265239492614
+        },
+        {
+            "x": 5.1446544158470235,
+            "y": 1.6301549523614358,
+            "heading": -0.6239033665029361,
+            "angularVelocity": -0.43454469037794713,
+            "velocityX": 0.859931944747064,
+            "velocityY": -0.2786503126240163,
+            "timestamp": 1.966287391158301
+        },
+        {
+            "x": 5.165552752490355,
+            "y": 1.6233842390024875,
+            "heading": -0.6345958555798376,
+            "angularVelocity": -0.29325931869912014,
+            "velocityX": 0.5731716835893133,
+            "velocityY": -0.18569808886140712,
+            "timestamp": 2.0027482583673404
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -0.14821766002380998,
+            "velocityX": 0.2865331603263514,
+            "velocityY": -0.09281839027812194,
+            "timestamp": 2.03920912557638
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 2.046404654845657e-31,
+            "velocityX": 2.793064044552828e-33,
+            "velocityY": 5.221456528298987e-32,
+            "timestamp": 2.0756699927854196
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W2 To W1.traj b/src/main/deploy/choreo/W2 To W1.traj
new file mode 100644
index 00000000..35bba061
--- /dev/null
+++ b/src/main/deploy/choreo/W2 To W1.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -5.451838641793523e-34,
+            "angularVelocity": -2.0131697388730357e-33,
+            "velocityX": -3.0924959567962797e-34,
+            "velocityY": 4.789643826321005e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2520020156777094,
+            "y": 5.6078780285533805,
+            "heading": 0.01582108665485387,
+            "angularVelocity": 0.208254594271014,
+            "velocityX": 0.026352738707599337,
+            "velocityY": 0.630223424496655,
+            "timestamp": 0.07596992858781765
+        },
+        {
+            "x": 2.2560055471462697,
+            "y": 5.703634194455967,
+            "heading": 0.04745824180691435,
+            "angularVelocity": 0.4164431340157103,
+            "velocityX": 0.05269889735295001,
+            "velocityY": 1.2604482810839739,
+            "timestamp": 0.1519398571756353
+        },
+        {
+            "x": 2.262009891384212,
+            "y": 5.847268762594484,
+            "heading": 0.09489870943903907,
+            "angularVelocity": 0.6244637649920359,
+            "velocityX": 0.07903580205425154,
+            "velocityY": 1.8906766244025437,
+            "timestamp": 0.22790978576345294
+        },
+        {
+            "x": 2.2700142224347877,
+            "y": 6.038782263911744,
+            "heading": 0.1581161098401003,
+            "angularVelocity": 0.8321371571119077,
+            "velocityX": 0.1053618346017344,
+            "velocityY": 2.5209119565760743,
+            "timestamp": 0.3038797143512706
+        },
+        {
+            "x": 2.280009491675263,
+            "y": 6.278187226323642,
+            "heading": 0.23645965423367718,
+            "angularVelocity": 1.0312441494928544,
+            "velocityX": 0.13156875919557368,
+            "velocityY": 3.151312194997739,
+            "timestamp": 0.37984964293908824
+        },
+        {
+            "x": 2.288005657404111,
+            "y": 6.469712008115255,
+            "heading": 0.2990934539478391,
+            "angularVelocity": 0.8244551611202343,
+            "velocityX": 0.10525435363026732,
+            "velocityY": 2.52106044262263,
+            "timestamp": 0.4558195715269059
+        },
+        {
+            "x": 2.294002815862431,
+            "y": 6.613355952122642,
+            "heading": 0.3460499471183657,
+            "angularVelocity": 0.6180931592721867,
+            "velocityX": 0.07894121489645621,
+            "velocityY": 1.8908000399308182,
+            "timestamp": 0.5317895001147235
+        },
+        {
+            "x": 2.2980009414583966,
+            "y": 6.709118669367686,
+            "heading": 0.3773492268804029,
+            "angularVelocity": 0.4119956454330042,
+            "velocityX": 0.05262773929482027,
+            "velocityY": 1.260534517079955,
+            "timestamp": 0.6077594287025412
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0.20601273965271094,
+            "velocityX": 0.026313813620245378,
+            "velocityY": 0.6302668901019914,
+            "timestamp": 0.6837293572903589
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 9.82651204832558e-34,
+            "velocityX": 0,
+            "velocityY": 3.8258931367738186e-36,
+            "timestamp": 0.7596992858781766
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W2 To W3.traj b/src/main/deploy/choreo/W2 To W3.traj
new file mode 100644
index 00000000..f46e6706
--- /dev/null
+++ b/src/main/deploy/choreo/W2 To W3.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 5.7052362935559275e-34,
+            "velocityX": 0,
+            "velocityY": -1.2494033787640354e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2520019447146944,
+            "y": 5.512001877264397,
+            "heading": -0.01569721301150974,
+            "angularVelocity": -0.20637467835572687,
+            "velocityX": 0.02632000319280478,
+            "velocityY": -0.6310417737196751,
+            "timestamp": 0.07606172005488261
+        },
+        {
+            "x": 2.2560053559202022,
+            "y": 5.416005527882177,
+            "heading": -0.04708679827781808,
+            "angularVelocity": -0.41268571422864314,
+            "velocityX": 0.05263371907208186,
+            "velocityY": -1.262084913580087,
+            "timestamp": 0.15212344010976522
+        },
+        {
+            "x": 2.262009560249172,
+            "y": 5.272010699235591,
+            "heading": -0.0941564711571504,
+            "angularVelocity": -0.6188352412405218,
+            "velocityX": 0.07893858204412474,
+            "velocityY": -1.8931313746610776,
+            "timestamp": 0.22818516016464785
+        },
+        {
+            "x": 2.2700137636187,
+            "y": 5.080016884298785,
+            "heading": -0.15688079244309955,
+            "angularVelocity": -0.8246503134650409,
+            "velocityX": 0.10523300503528431,
+            "velocityY": -2.5241845017213995,
+            "timestamp": 0.30424688021953045
+        },
+        {
+            "x": 2.2800092014745617,
+            "y": 4.840012168782865,
+            "heading": -0.23463455266722952,
+            "angularVelocity": -1.0222456206358,
+            "velocityX": 0.13141219852311892,
+            "velocityY": -3.155394268532757,
+            "timestamp": 0.38030860027441304
+        },
+        {
+            "x": 2.2880054914438257,
+            "y": 4.648007616430203,
+            "heading": -0.2967978427641432,
+            "angularVelocity": -0.8172743142287549,
+            "velocityX": 0.10512896583845424,
+            "velocityY": -2.524325668866294,
+            "timestamp": 0.45637032032929564
+        },
+        {
+            "x": 2.2940027351611283,
+            "y": 4.5040038585170965,
+            "heading": -0.3434020516948695,
+            "angularVelocity": -0.6127156853289512,
+            "velocityX": 0.07884706936650254,
+            "velocityY": -1.8932487696728393,
+            "timestamp": 0.5324320403841782
+        },
+        {
+            "x": 2.2980009144821483,
+            "y": 4.408001268437703,
+            "heading": -0.37446662411182524,
+            "angularVelocity": -0.40841269950956927,
+            "velocityX": 0.05256493434720428,
+            "velocityY": -1.2621669613850792,
+            "timestamp": 0.6084937604390609
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.20422067601109492,
+            "velocityX": 0.026282412709165117,
+            "velocityY": -0.6310831309503169,
+            "timestamp": 0.6845554804939435
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 8.777318885868316e-35,
+            "velocityX": -1.1404320857258046e-34,
+            "velocityY": 1.134511753128188e-36,
+            "timestamp": 0.7606172005488262
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W3 To C1.traj b/src/main/deploy/choreo/W3 To C1.traj
new file mode 100644
index 00000000..5529ce29
--- /dev/null
+++ b/src/main/deploy/choreo/W3 To C1.traj	
@@ -0,0 +1,553 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 1.095053106710567e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3031602212725026,
+            "y": 4.367935645195281,
+            "heading": -0.3933962364434778,
+            "angularVelocity": -0.10578752662991632,
+            "velocityX": 0.0984360180997807,
+            "velocityY": 0.24718310735800264,
+            "timestamp": 0.032104318454850914
+        },
+        {
+            "x": 2.309652031024921,
+            "y": 4.383743043384139,
+            "heading": -0.39998627296935113,
+            "angularVelocity": -0.20526947286363117,
+            "velocityX": 0.20220986038211264,
+            "velocityY": 0.4923760711845366,
+            "timestamp": 0.06420863690970183
+        },
+        {
+            "x": 2.3196739941431104,
+            "y": 4.407342390021744,
+            "heading": -0.40953275493580865,
+            "angularVelocity": -0.29735818811675285,
+            "velocityX": 0.31216869257896596,
+            "velocityY": 0.7350832465356143,
+            "timestamp": 0.09631295536455274
+        },
+        {
+            "x": 2.333458307096076,
+            "y": 4.438632228164309,
+            "heading": -0.4217541925110665,
+            "angularVelocity": -0.3806789292987277,
+            "velocityX": 0.4293600866297902,
+            "velocityY": 0.9746301945817274,
+            "timestamp": 0.12841727381940365
+        },
+        {
+            "x": 2.3512791398330983,
+            "y": 4.477480960055527,
+            "heading": -0.4363123371700568,
+            "angularVelocity": -0.4534637506622021,
+            "velocityX": 0.5550914517025046,
+            "velocityY": 1.21007807550416,
+            "timestamp": 0.16052159227425455
+        },
+        {
+            "x": 2.3734634122157945,
+            "y": 4.523713969255995,
+            "heading": -0.45279460426513807,
+            "angularVelocity": -0.5133971966500652,
+            "velocityX": 0.691005866201282,
+            "velocityY": 1.4400869236793286,
+            "timestamp": 0.19262591072910545
+        },
+        {
+            "x": 2.4004044405613056,
+            "y": 4.577093572392249,
+            "heading": -0.4706891378053212,
+            "angularVelocity": -0.5573871180398601,
+            "velocityX": 0.8391714773013875,
+            "velocityY": 1.6626923013900174,
+            "timestamp": 0.22473022918395635
+        },
+        {
+            "x": 2.432578225951104,
+            "y": 4.63728707308353,
+            "heading": -0.48934877664951465,
+            "angularVelocity": -0.581218968109697,
+            "velocityX": 1.002163787873114,
+            "velocityY": 1.8749347000134435,
+            "timestamp": 0.25683454763880725
+        },
+        {
+            "x": 2.4705256917588616,
+            "y": 4.703846202675345,
+            "heading": -0.5079484621261662,
+            "angularVelocity": -0.5793515131868818,
+            "velocityX": 1.1820050271780043,
+            "velocityY": 2.073214221489191,
+            "timestamp": 0.28893886609365815
+        },
+        {
+            "x": 2.5143889510907504,
+            "y": 4.77663720854218,
+            "heading": -0.5259955708448517,
+            "angularVelocity": -0.5621395995079442,
+            "velocityX": 1.366272870535308,
+            "velocityY": 2.267327554989265,
+            "timestamp": 0.32104318454850905
+        },
+        {
+            "x": 2.564366566619072,
+            "y": 4.855456871813396,
+            "heading": -0.5427715655324792,
+            "angularVelocity": -0.5225463580925951,
+            "velocityX": 1.5567256348583118,
+            "velocityY": 2.4551109341275055,
+            "timestamp": 0.35314750300335995
+        },
+        {
+            "x": 2.620751841763659,
+            "y": 4.939963572730219,
+            "heading": -0.5571329301868887,
+            "angularVelocity": -0.44733435704626356,
+            "velocityX": 1.7563143482981505,
+            "velocityY": 2.6322533847172953,
+            "timestamp": 0.38525182145821085
+        },
+        {
+            "x": 2.6840019979529464,
+            "y": 5.029487269137319,
+            "heading": -0.5670183536644556,
+            "angularVelocity": -0.30791569338152147,
+            "velocityX": 1.9701448039844656,
+            "velocityY": 2.788525055686805,
+            "timestamp": 0.41735613991306175
+        },
+        {
+            "x": 2.7547918742113016,
+            "y": 5.122370588176559,
+            "heading": -0.568384562504894,
+            "angularVelocity": -0.0425552980468726,
+            "velocityX": 2.2049954543625816,
+            "velocityY": 2.893172118569162,
+            "timestamp": 0.44946045836791265
+        },
+        {
+            "x": 2.8341176789452183,
+            "y": 5.214333601098647,
+            "heading": -0.5683844391321649,
+            "angularVelocity": 0.0000038428702119811485,
+            "velocityX": 2.4708764599838617,
+            "velocityY": 2.864506002562167,
+            "timestamp": 0.48156477682276355
+        },
+        {
+            "x": 2.9197482153809866,
+            "y": 5.3004567509455125,
+            "heading": -0.5683843814173083,
+            "angularVelocity": 0.0000017977287583592433,
+            "velocityX": 2.6672591276526463,
+            "velocityY": 2.6826032755681406,
+            "timestamp": 0.5136690952776145
+        },
+        {
+            "x": 3.0112544304014244,
+            "y": 5.380309105627853,
+            "heading": -0.5683843265105415,
+            "angularVelocity": 0.0000017102610943456471,
+            "velocityX": 2.850277452521701,
+            "velocityY": 2.487277678691065,
+            "timestamp": 0.5457734137324655
+        },
+        {
+            "x": 3.107551803887462,
+            "y": 5.454313172250514,
+            "heading": -0.5683842730108181,
+            "angularVelocity": 0.0000016664338651344868,
+            "velocityX": 2.999514648518804,
+            "velocityY": 2.305112526426488,
+            "timestamp": 0.5778777321873164
+        },
+        {
+            "x": 3.2038516202489813,
+            "y": 5.528314059986814,
+            "heading": -0.5683842195116714,
+            "angularVelocity": 0.000001666415897857932,
+            "velocityX": 2.9995907403219917,
+            "velocityY": 2.305013509019602,
+            "timestamp": 0.6099820506421674
+        },
+        {
+            "x": 3.3001514533185667,
+            "y": 5.602314925980335,
+            "heading": -0.5683841660125195,
+            "angularVelocity": 0.000001666416059709123,
+            "velocityX": 2.9995912607524815,
+            "velocityY": 2.305012831765639,
+            "timestamp": 0.6420863690970183
+        },
+        {
+            "x": 3.3964512995777554,
+            "y": 5.676315774809768,
+            "heading": -0.568384112513365,
+            "angularVelocity": 0.0000016664161452516387,
+            "velocityX": 2.999591671588281,
+            "velocityY": 2.30501229713074,
+            "timestamp": 0.6741906875518693
+        },
+        {
+            "x": 3.492751156794414,
+            "y": 5.750316609379858,
+            "heading": -0.5683840590142086,
+            "angularVelocity": 0.000001666416198927138,
+            "velocityX": 2.999592012896559,
+            "velocityY": 2.3050118529742454,
+            "timestamp": 0.7062950060067202
+        },
+        {
+            "x": 3.5890510233460025,
+            "y": 5.824317431802072,
+            "heading": -0.5683840055150509,
+            "angularVelocity": 0.0000016664162437925357,
+            "velocityX": 2.999592303665221,
+            "velocityY": 2.305011474586604,
+            "timestamp": 0.7383993244615712
+        },
+        {
+            "x": 3.6853508979893648,
+            "y": 5.898318243694168,
+            "heading": -0.5683839520158918,
+            "angularVelocity": 0.0000016664162889267752,
+            "velocityX": 2.9995925557115117,
+            "velocityY": 2.3050111465896648,
+            "timestamp": 0.7705036429164221
+        },
+        {
+            "x": 3.7816507797340138,
+            "y": 5.972319046345101,
+            "heading": -0.5683838985167311,
+            "angularVelocity": 0.0000016664163350532929,
+            "velocityX": 2.9995927769056334,
+            "velocityY": 2.305010858741686,
+            "timestamp": 0.8026079613712731
+        },
+        {
+            "x": 3.877950667767949,
+            "y": 6.046319840811553,
+            "heading": -0.5683838450175687,
+            "angularVelocity": 0.0000016664163859759036,
+            "velocityX": 2.999592972807203,
+            "velocityY": 2.305010603807763,
+            "timestamp": 0.8347122798261241
+        },
+        {
+            "x": 3.9742505615842325,
+            "y": 6.120320627753221,
+            "heading": -0.5683837915184047,
+            "angularVelocity": 0.0000016664164376020863,
+            "velocityX": 2.999593152918432,
+            "velocityY": 2.3050103694223405,
+            "timestamp": 0.866816598280975
+        },
+        {
+            "x": 4.070552264919673,
+            "y": 6.194319059842733,
+            "heading": -0.5683837380195041,
+            "angularVelocity": 0.0000016664082331173329,
+            "velocityX": 2.999649516649054,
+            "velocityY": 2.304937019409932,
+            "timestamp": 0.898920916735826
+        },
+        {
+            "x": 4.171102213813095,
+            "y": 6.262432889296395,
+            "heading": -0.5683836849080734,
+            "angularVelocity": 0.0000016543391427080076,
+            "velocityX": 3.131975812999348,
+            "velocityY": 2.1216407241116846,
+            "timestamp": 0.9310252351906769
+        },
+        {
+            "x": 4.276216983795166,
+            "y": 6.32326602935791,
+            "heading": -0.5683836309724583,
+            "angularVelocity": 0.0000016800112198234746,
+            "velocityX": 3.274162948822482,
+            "velocityY": 1.8948584797732355,
+            "timestamp": 0.9631295536455279
+        },
+        {
+            "x": 4.413834682670106,
+            "y": 6.38740625436239,
+            "heading": -0.5683835821873748,
+            "angularVelocity": 0.0000012155044166425132,
+            "velocityX": 3.428812837046335,
+            "velocityY": 1.5980853383274327,
+            "timestamp": 1.0032652231659112
+        },
+        {
+            "x": 4.5565820466376845,
+            "y": 6.439134291384663,
+            "heading": -0.5683835352510233,
+            "angularVelocity": 0.0000011694423477450916,
+            "velocityX": 3.556620972650851,
+            "velocityY": 1.2888295533727718,
+            "timestamp": 1.0434008926862943
+        },
+        {
+            "x": 4.7007810920363475,
+            "y": 6.4866657027970485,
+            "heading": -0.5683834885999294,
+            "angularVelocity": 0.000001162335011018839,
+            "velocityX": 3.5927903314389917,
+            "velocityY": 1.184268556632563,
+            "timestamp": 1.0835365622066775
+        },
+        {
+            "x": 4.84498021653845,
+            "y": 6.534196874227661,
+            "heading": -0.5683834419488406,
+            "angularVelocity": 0.0000011623348856187121,
+            "velocityX": 3.5927923023401758,
+            "velocityY": 1.1842625773683353,
+            "timestamp": 1.1236722317270607
+        },
+        {
+            "x": 4.989179343628637,
+            "y": 6.581728037806586,
+            "heading": -0.5683833952977476,
+            "angularVelocity": 0.0000011623349868626642,
+            "velocityX": 3.5927923668235673,
+            "velocityY": 1.1842623817396558,
+            "timestamp": 1.1638079012474438
+        },
+        {
+            "x": 5.133378473125504,
+            "y": 6.629259194084157,
+            "heading": -0.5683833486466496,
+            "angularVelocity": 0.0000011623351115999292,
+            "velocityX": 3.592792426787221,
+            "velocityY": 1.1842621998228242,
+            "timestamp": 1.203943570767827
+        },
+        {
+            "x": 5.277577604867278,
+            "y": 6.676790343551162,
+            "heading": -0.5683833019955459,
+            "angularVelocity": 0.0000011623352597981392,
+            "velocityX": 3.5927924827201663,
+            "velocityY": 1.184262030134241,
+            "timestamp": 1.2440792402882102
+        },
+        {
+            "x": 5.421776738706852,
+            "y": 6.7243214866538885,
+            "heading": -0.5683832553444353,
+            "angularVelocity": 0.0000011623354265532326,
+            "velocityX": 3.5927925349879053,
+            "velocityY": 1.1842618715650601,
+            "timestamp": 1.2842149098085933
+        },
+        {
+            "x": 5.565975874510482,
+            "y": 6.771852623798087,
+            "heading": -0.5683832086933173,
+            "angularVelocity": 0.0000011623356113966992,
+            "velocityX": 3.5927925839233144,
+            "velocityY": 1.18426172310541,
+            "timestamp": 1.3243505793289765
+        },
+        {
+            "x": 5.710175012156514,
+            "y": 6.819383755352828,
+            "heading": -0.5683831620421912,
+            "angularVelocity": 0.0000011623358144790056,
+            "velocityX": 3.5927926298276605,
+            "velocityY": 1.1842615838413222,
+            "timestamp": 1.3644862488493597
+        },
+        {
+            "x": 5.854374151534237,
+            "y": 6.866914881653982,
+            "heading": -0.5683831153910563,
+            "angularVelocity": 0.000001162336033125057,
+            "velocityX": 3.592792672973599,
+            "velocityY": 1.1842614529456208,
+            "timestamp": 1.4046219183697428
+        },
+        {
+            "x": 5.998573292523273,
+            "y": 6.9144460030667485,
+            "heading": -0.568383068739913,
+            "angularVelocity": 0.000001162336242705986,
+            "velocityX": 3.59279271312027,
+            "velocityY": 1.184261331149038,
+            "timestamp": 1.444757587890126
+        },
+        {
+            "x": 6.142772435013044,
+            "y": 6.9619771199265985,
+            "heading": -0.5683830220887615,
+            "angularVelocity": 0.0000011623364467362213,
+            "velocityX": 3.5927927505118338,
+            "velocityY": 1.1842612177108516,
+            "timestamp": 1.4848932574105091
+        },
+        {
+            "x": 6.286971578902684,
+            "y": 7.00950823253954,
+            "heading": -0.5683829754376022,
+            "angularVelocity": 0.0000011623366413601916,
+            "velocityX": 3.59279278539024,
+            "velocityY": 1.1842611118970372,
+            "timestamp": 1.5250289269308923
+        },
+        {
+            "x": 6.431170724100105,
+            "y": 7.057039341184945,
+            "heading": -0.5683829287864354,
+            "angularVelocity": 0.0000011623368281432719,
+            "velocityX": 3.592792817974252,
+            "velocityY": 1.1842610130439195,
+            "timestamp": 1.5651645964512755
+        },
+        {
+            "x": 6.575369870521147,
+            "y": 7.10457044611814,
+            "heading": -0.5683828821352613,
+            "angularVelocity": 0.0000011623370098744323,
+            "velocityX": 3.5927928484613587,
+            "velocityY": 1.1842609205523604,
+            "timestamp": 1.6053002659716586
+        },
+        {
+            "x": 6.719569018075523,
+            "y": 7.152101547613029,
+            "heading": -0.5683828354840802,
+            "angularVelocity": 0.0000011623371832362076,
+            "velocityX": 3.5927928766989634,
+            "velocityY": 1.1842608348853054,
+            "timestamp": 1.6454359354920418
+        },
+        {
+            "x": 6.8637678827817385,
+            "y": 7.1996335071853474,
+            "heading": -0.5683827888209949,
+            "angularVelocity": 0.0000011626337817909648,
+            "velocityX": 3.592785829397527,
+            "velocityY": 1.1842822143076228,
+            "timestamp": 1.685571605012425
+        },
+        {
+            "x": 6.99756534811195,
+            "y": 7.244965845733408,
+            "heading": -0.5424482126349314,
+            "angularVelocity": 0.6461727559544621,
+            "velocityX": 3.3336298342366253,
+            "velocityY": 1.1294775716906411,
+            "timestamp": 1.7257072745328081
+        },
+        {
+            "x": 7.121516587135396,
+            "y": 7.285176196921745,
+            "heading": -0.489772109644447,
+            "angularVelocity": 1.3124510845329824,
+            "velocityX": 3.088306249893159,
+            "velocityY": 1.0018607305881775,
+            "timestamp": 1.7658429440531913
+        },
+        {
+            "x": 7.233956124618328,
+            "y": 7.321947450112657,
+            "heading": -0.4161805837381191,
+            "angularVelocity": 1.8335691614401448,
+            "velocityX": 2.8014865287304875,
+            "velocityY": 0.9161739079059802,
+            "timestamp": 1.8059786135735745
+        },
+        {
+            "x": 7.333707629732149,
+            "y": 7.35452494917019,
+            "heading": -0.3377597360881954,
+            "angularVelocity": 1.953894094381483,
+            "velocityX": 2.4853579448365832,
+            "velocityY": 0.8116844554190893,
+            "timestamp": 1.8461142830939576
+        },
+        {
+            "x": 7.420722566948463,
+            "y": 7.382895867936689,
+            "heading": -0.26184604843223563,
+            "angularVelocity": 1.8914269666638126,
+            "velocityX": 2.1680200743162716,
+            "velocityY": 0.7068754328887092,
+            "timestamp": 1.8862499526143408
+        },
+        {
+            "x": 7.495090208476241,
+            "y": 7.40709623575076,
+            "heading": -0.19218016370533045,
+            "angularVelocity": 1.7357598754276362,
+            "velocityX": 1.8529064649092093,
+            "velocityY": 0.6029640990984653,
+            "timestamp": 1.926385622134724
+        },
+        {
+            "x": 7.556901497509452,
+            "y": 7.427169942598205,
+            "heading": -0.13107853540878284,
+            "angularVelocity": 1.522377202790068,
+            "velocityX": 1.5400587500308252,
+            "velocityY": 0.500146305949861,
+            "timestamp": 1.9665212916551071
+        },
+        {
+            "x": 7.606235479245265,
+            "y": 7.443160506733181,
+            "heading": -0.08018624320935208,
+            "angularVelocity": 1.2680065589434022,
+            "velocityX": 1.2291804852229526,
+            "velocityY": 0.3984127915657456,
+            "timestamp": 2.0066569611754903
+        },
+        {
+            "x": 7.643159935464096,
+            "y": 7.455107253348189,
+            "heading": -0.040758426269481916,
+            "angularVelocity": 0.982363503861482,
+            "velocityX": 0.9199910369024534,
+            "velocityY": 0.29765908374694494,
+            "timestamp": 2.0467926306958737
+        },
+        {
+            "x": 7.667732329164437,
+            "y": 7.4630440972663505,
+            "heading": -0.013776014916518935,
+            "angularVelocity": 0.6722800859036306,
+            "velocityX": 0.6122333075286244,
+            "velocityY": 0.1977503804721968,
+            "timestamp": 2.086928300216257
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 2.662292212835229e-35,
+            "angularVelocity": 0.3432362056280802,
+            "velocityX": 0.30565506897381545,
+            "velocityY": 0.09856326756028964,
+            "timestamp": 2.1270639697366405
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -1.5591265591207004e-41,
+            "angularVelocity": 3.563315968840566e-34,
+            "velocityX": -7.620676928265867e-35,
+            "velocityY": -1.1745137620932586e-33,
+            "timestamp": 2.167199639257024
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W3 To C2.traj b/src/main/deploy/choreo/W3 To C2.traj
new file mode 100644
index 00000000..0646af70
--- /dev/null
+++ b/src/main/deploy/choreo/W3 To C2.traj	
@@ -0,0 +1,553 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -3.138879459525789e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 2.304641315068496,
+            "y": 4.371901353899003,
+            "heading": -0.39554860460981267,
+            "angularVelocity": -0.1412451028521288,
+            "velocityX": 0.11814916908296796,
+            "velocityY": 0.3029604871416256,
+            "timestamp": 0.039283518492098594
+        },
+        {
+            "x": 2.314310378834228,
+            "y": 4.395560050627161,
+            "heading": -0.4062436571914542,
+            "angularVelocity": -0.27225291909092403,
+            "velocityX": 0.24613538035494495,
+            "velocityY": 0.6022550330596644,
+            "timestamp": 0.07856703698419719
+        },
+        {
+            "x": 2.3294686593461313,
+            "y": 4.43078385259534,
+            "heading": -0.42159423390969814,
+            "angularVelocity": -0.3907637937605655,
+            "velocityX": 0.385868707635076,
+            "velocityY": 0.8966559850097278,
+            "timestamp": 0.11785055547629578
+        },
+        {
+            "x": 2.3506733572393026,
+            "y": 4.477309554936099,
+            "heading": -0.44099148005781696,
+            "angularVelocity": -0.49377568233887975,
+            "velocityX": 0.5397861165984255,
+            "velocityY": 1.1843568022074944,
+            "timestamp": 0.15713407396839438
+        },
+        {
+            "x": 2.3786040121360466,
+            "y": 4.534766426387325,
+            "heading": -0.4636665715847995,
+            "angularVelocity": -0.5772164102747018,
+            "velocityX": 0.7110018646207296,
+            "velocityY": 1.462620296162777,
+            "timestamp": 0.19641759246049298
+        },
+        {
+            "x": 2.4140943920114943,
+            "y": 4.602616046670193,
+            "heading": -0.48862953085625704,
+            "angularVelocity": -0.6354562989682879,
+            "velocityX": 0.903441983756771,
+            "velocityY": 1.727177780588029,
+            "timestamp": 0.23570111095259158
+        },
+        {
+            "x": 2.458163120855333,
+            "y": 4.68005234098581,
+            "heading": -0.5145799632934547,
+            "angularVelocity": -0.6605933845360225,
+            "velocityX": 1.1218121628463973,
+            "velocityY": 1.9712158505149888,
+            "timestamp": 0.27498462944469015
+        },
+        {
+            "x": 2.5115097740388146,
+            "y": 4.766403162212065,
+            "heading": -0.5400224328341426,
+            "angularVelocity": -0.6476626971635644,
+            "velocityX": 1.3579907103842537,
+            "velocityY": 2.1981437646332993,
+            "timestamp": 0.31426814793678876
+        },
+        {
+            "x": 2.5744195202560527,
+            "y": 4.861351309968927,
+            "heading": -0.563860333505168,
+            "angularVelocity": -0.6068168429413633,
+            "velocityX": 1.6014285031491418,
+            "velocityY": 2.416997035944537,
+            "timestamp": 0.35355166642888736
+        },
+        {
+            "x": 2.6473242968990576,
+            "y": 4.964346697364103,
+            "heading": -0.5842892856317753,
+            "angularVelocity": -0.5200387569839264,
+            "velocityX": 1.8558616804566808,
+            "velocityY": 2.6218473127835553,
+            "timestamp": 0.39283518492098596
+        },
+        {
+            "x": 2.7309171688764238,
+            "y": 5.07424676618957,
+            "heading": -0.5978668700067779,
+            "angularVelocity": -0.3456305569399124,
+            "velocityX": 2.127937496081736,
+            "velocityY": 2.7976126641403014,
+            "timestamp": 0.43211870341308456
+        },
+        {
+            "x": 2.826173576921219,
+            "y": 5.187875652325513,
+            "heading": -0.5974605263008922,
+            "angularVelocity": 0.010343872481393205,
+            "velocityX": 2.4248441000503926,
+            "velocityY": 2.8925333192550147,
+            "timestamp": 0.47140222190518316
+        },
+        {
+            "x": 2.9311441629204515,
+            "y": 5.293067244130248,
+            "heading": -0.5974604636085413,
+            "angularVelocity": 0.000001595894492248037,
+            "velocityX": 2.672127905761066,
+            "velocityY": 2.677753822532693,
+            "timestamp": 0.5106857403972818
+        },
+        {
+            "x": 3.044820699228354,
+            "y": 5.388784226639372,
+            "heading": -0.597460407679312,
+            "angularVelocity": 0.0000014237326879146737,
+            "velocityX": 2.8937463005192323,
+            "velocityY": 2.436568468997834,
+            "timestamp": 0.5499692588893803
+        },
+        {
+            "x": 3.1663502143390825,
+            "y": 5.474310087323753,
+            "heading": -0.5974603544159754,
+            "angularVelocity": 0.0000013558698005366095,
+            "velocityX": 3.0936514796952492,
+            "velocityY": 2.177143595261183,
+            "timestamp": 0.5892527773814789
+        },
+        {
+            "x": 3.289918343405979,
+            "y": 5.556863181758907,
+            "heading": -0.5974603016382944,
+            "angularVelocity": 0.0000013435069733969365,
+            "velocityX": 3.145546371864123,
+            "velocityY": 2.1014689519673895,
+            "timestamp": 0.6285362958735774
+        },
+        {
+            "x": 3.413486607435696,
+            "y": 5.639416074177005,
+            "heading": -0.5974602488606314,
+            "angularVelocity": 0.0000013435065140042136,
+            "velocityX": 3.145549807473418,
+            "velocityY": 2.101463809427605,
+            "timestamp": 0.667819814365676
+        },
+        {
+            "x": 3.5370548851900594,
+            "y": 5.721968946051526,
+            "heading": -0.5974601960829673,
+            "angularVelocity": 0.0000013435065431603766,
+            "velocityX": 3.145550156847558,
+            "velocityY": 2.1014632864709872,
+            "timestamp": 0.7071033328577745
+        },
+        {
+            "x": 3.6606231741521,
+            "y": 5.804521801149959,
+            "heading": -0.5974601433053026,
+            "angularVelocity": 0.0000013435065598734946,
+            "velocityX": 3.1455504421498475,
+            "velocityY": 2.1014628594194082,
+            "timestamp": 0.746386851349873
+        },
+        {
+            "x": 3.7841914725218415,
+            "y": 5.887074642166574,
+            "heading": -0.597460090527637,
+            "angularVelocity": 0.0000013435065802330538,
+            "velocityX": 3.1455506816319834,
+            "velocityY": 2.101462500953099,
+            "timestamp": 0.7856703698419716
+        },
+        {
+            "x": 3.9077597789317933,
+            "y": 5.969627471148282,
+            "heading": -0.5974600377499704,
+            "angularVelocity": 0.0000013435066065618314,
+            "velocityX": 3.14555088630331,
+            "velocityY": 2.1014621945928877,
+            "timestamp": 0.8249538883340701
+        },
+        {
+            "x": 4.031328092300017,
+            "y": 6.052180289714568,
+            "heading": -0.5974599849723026,
+            "angularVelocity": 0.0000013435066364860128,
+            "velocityX": 3.1455510634328734,
+            "velocityY": 2.101461929458217,
+            "timestamp": 0.8642374068261687
+        },
+        {
+            "x": 4.154896411746533,
+            "y": 6.134733099182619,
+            "heading": -0.5974599321946334,
+            "angularVelocity": 0.0000013435066718879358,
+            "velocityX": 3.14555121816168,
+            "velocityY": 2.1014616978538454,
+            "timestamp": 0.9035209253182672
+        },
+        {
+            "x": 4.27846473654201,
+            "y": 6.217285900644126,
+            "heading": -0.5974598794169629,
+            "angularVelocity": 0.000001343506708988157,
+            "velocityX": 3.14555135432466,
+            "velocityY": 2.1014614940395124,
+            "timestamp": 0.9428044438103658
+        },
+        {
+            "x": 4.4020330660739395,
+            "y": 6.299838695015915,
+            "heading": -0.5974598266392908,
+            "angularVelocity": 0.0000013435067467217265,
+            "velocityX": 3.1455514748956417,
+            "velocityY": 2.1014613135638847,
+            "timestamp": 0.9820879623024643
+        },
+        {
+            "x": 4.525601399950297,
+            "y": 6.382391482884783,
+            "heading": -0.5974597738616171,
+            "angularVelocity": 0.0000013435067857429384,
+            "velocityX": 3.1455515854872873,
+            "velocityY": 2.101461148025713,
+            "timestamp": 1.0213714807945629
+        },
+        {
+            "x": 4.6491721527053596,
+            "y": 6.464940649963802,
+            "heading": -0.5974597210842042,
+            "angularVelocity": 0.0000013435001464771913,
+            "velocityX": 3.1456131603868194,
+            "velocityY": 2.101368977314248,
+            "timestamp": 1.0606549992866614
+        },
+        {
+            "x": 4.778837800088318,
+            "y": 6.537541722502267,
+            "heading": -0.5974596685786843,
+            "angularVelocity": 0.0000013365788503576918,
+            "velocityX": 3.300764604603488,
+            "velocityY": 1.8481305984081453,
+            "timestamp": 1.09993851777876
+        },
+        {
+            "x": 4.914297553492409,
+            "y": 6.59865432938236,
+            "heading": -0.5974596146272461,
+            "angularVelocity": 0.0000013733860988547744,
+            "velocityX": 3.4482591835906553,
+            "velocityY": 1.5556805812161723,
+            "timestamp": 1.1392220362708585
+        },
+        {
+            "x": 5.054536029054771,
+            "y": 6.647820017049671,
+            "heading": -0.5974595570647036,
+            "angularVelocity": 0.0000014653102566024163,
+            "velocityX": 3.5699061831898264,
+            "velocityY": 1.2515601848929776,
+            "timestamp": 1.178505554762957
+        },
+        {
+            "x": 5.198501873691458,
+            "y": 6.684670110841039,
+            "heading": -0.5974594930097576,
+            "angularVelocity": 0.0000016305806703071774,
+            "velocityX": 3.6647899720507366,
+            "velocityY": 0.9380548180449542,
+            "timestamp": 1.2177890732550556
+        },
+        {
+            "x": 5.345115761176417,
+            "y": 6.7089283098414425,
+            "heading": -0.5974594180971905,
+            "angularVelocity": 0.0000019069719319939646,
+            "velocityX": 3.7321984667555648,
+            "velocityY": 0.6175159438731035,
+            "timestamp": 1.2570725917471541
+        },
+        {
+            "x": 5.493278503417969,
+            "y": 6.720412731170654,
+            "heading": -0.5974593280342978,
+            "angularVelocity": 0.0000022926381384477543,
+            "velocityX": 3.7716260642834416,
+            "velocityY": 0.2923470648746024,
+            "timestamp": 1.2963561102392527
+        },
+        {
+            "x": 5.617528419554277,
+            "y": 6.721027319451783,
+            "heading": -0.5974592417476945,
+            "angularVelocity": 0.0000026270679336343826,
+            "velocityX": 3.7828927995748978,
+            "velocityY": 0.018711655155499447,
+            "timestamp": 1.3292013222818118
+        },
+        {
+            "x": 5.7414972044827435,
+            "y": 6.712651079745108,
+            "heading": -0.5974591685782128,
+            "angularVelocity": 0.0000022277061710620633,
+            "velocityX": 3.7743335244066216,
+            "velocityY": -0.25502163589934396,
+            "timestamp": 1.3620465343243708
+        },
+        {
+            "x": 5.864535126960113,
+            "y": 6.695327921921503,
+            "heading": -0.597459104000633,
+            "angularVelocity": 0.000001966118527479946,
+            "velocityX": 3.745992637160019,
+            "velocityY": -0.5274180541456184,
+            "timestamp": 1.39489174636693
+        },
+        {
+            "x": 5.985997338800097,
+            "y": 6.669148654285341,
+            "heading": -0.5974590451258569,
+            "angularVelocity": 0.0000017924918823644274,
+            "velocityX": 3.6980188065957362,
+            "velocityY": -0.7970497374814797,
+            "timestamp": 1.427736958409489
+        },
+        {
+            "x": 6.105247258928482,
+            "y": 6.634250515985412,
+            "heading": -0.5974589899613981,
+            "angularVelocity": 0.0000016795281661896423,
+            "velocityX": 3.6306637318675383,
+            "velocityY": -1.062503059946642,
+            "timestamp": 1.460582170452048
+        },
+        {
+            "x": 6.221659925637703,
+            "y": 6.5908164867561565,
+            "heading": -0.5974589370314296,
+            "angularVelocity": 0.0000016114972403778573,
+            "velocityX": 3.544281174328253,
+            "velocityY": -1.3223854110871922,
+            "timestamp": 1.4934273824946072
+        },
+        {
+            "x": 6.334625365293315,
+            "y": 6.539074496952364,
+            "heading": -0.5974588851619594,
+            "angularVelocity": 0.000001579209477235118,
+            "velocityX": 3.439327458421827,
+            "velocityY": -1.5753282316046957,
+            "timestamp": 1.5262725945371662
+        },
+        {
+            "x": 6.443562887659525,
+            "y": 6.4793163144818955,
+            "heading": -0.5974588333468978,
+            "angularVelocity": 0.000001577552964572491,
+            "velocityX": 3.3166941417319027,
+            "velocityY": -1.819387933752184,
+            "timestamp": 1.5591178065797253
+        },
+        {
+            "x": 6.552407332287041,
+            "y": 6.419388766569548,
+            "heading": -0.5974587815290724,
+            "angularVelocity": 0.0000015776371105880298,
+            "velocityX": 3.313860312014723,
+            "velocityY": -1.82454440649205,
+            "timestamp": 1.5919630186222844
+        },
+        {
+            "x": 6.661251765496281,
+            "y": 6.359461197918566,
+            "heading": -0.5974587297112393,
+            "angularVelocity": 0.0000015776373457032493,
+            "velocityX": 3.3138599643757076,
+            "velocityY": -1.8245450378971961,
+            "timestamp": 1.6248082306648435
+        },
+        {
+            "x": 6.7700961954772865,
+            "y": 6.299533623404254,
+            "heading": -0.5974586778933993,
+            "angularVelocity": 0.0000015776375522202912,
+            "velocityX": 3.3138598660894165,
+            "velocityY": -1.8245452164111537,
+            "timestamp": 1.6576534427074026
+        },
+        {
+            "x": 6.878940549868732,
+            "y": 6.239605911600315,
+            "heading": -0.5974586260742238,
+            "angularVelocity": 0.0000015776782163840074,
+            "velocityX": 3.313857564701912,
+            "velocityY": -1.8245493963088628,
+            "timestamp": 1.6904986547499616
+        },
+        {
+            "x": 6.983465419726436,
+            "y": 6.181138244955353,
+            "heading": -0.5831453971579332,
+            "angularVelocity": 0.4357782466962171,
+            "velocityX": 3.182347238991671,
+            "velocityY": -1.7800970981449478,
+            "timestamp": 1.7233438667925207
+        },
+        {
+            "x": 7.08190934617205,
+            "y": 6.126284607678917,
+            "heading": -0.5460039664027865,
+            "angularVelocity": 1.1308019782912204,
+            "velocityX": 2.997207821891462,
+            "velocityY": -1.67006494600528,
+            "timestamp": 1.7561890788350798
+        },
+        {
+            "x": 7.174234078294548,
+            "y": 6.075101338854014,
+            "heading": -0.4864629486170815,
+            "angularVelocity": 1.8127761729340726,
+            "velocityX": 2.8109038237566843,
+            "velocityY": -1.5583175032799648,
+            "timestamp": 1.7890342908776389
+        },
+        {
+            "x": 7.258956033298187,
+            "y": 6.028257642494675,
+            "heading": -0.41936488473987615,
+            "angularVelocity": 2.0428567728619895,
+            "velocityX": 2.5794309043853687,
+            "velocityY": -1.4261955836547433,
+            "timestamp": 1.821879502920198
+        },
+        {
+            "x": 7.335844716602702,
+            "y": 5.985834491701992,
+            "heading": -0.35186229403264746,
+            "angularVelocity": 2.0551729311350044,
+            "velocityX": 2.3409403843979617,
+            "velocityY": -1.2916083701234404,
+            "timestamp": 1.854724714962757
+        },
+        {
+            "x": 7.404915151007572,
+            "y": 5.947794038432469,
+            "heading": -0.2870870788105502,
+            "angularVelocity": 1.9721356993583767,
+            "velocityX": 2.1029072461275145,
+            "velocityY": -1.1581734719873862,
+            "timestamp": 1.8875699270053161
+        },
+        {
+            "x": 7.466209510665893,
+            "y": 5.914092129035952,
+            "heading": -0.22688750133470567,
+            "angularVelocity": 1.8328265744751362,
+            "velocityX": 1.866158135283738,
+            "velocityY": -1.0260828687243908,
+            "timestamp": 1.9204151390478752
+        },
+        {
+            "x": 7.519766848297347,
+            "y": 5.8846886900937605,
+            "heading": -0.17246123732016092,
+            "angularVelocity": 1.657053208974533,
+            "velocityX": 1.6305980172109753,
+            "velocityY": -0.8952123342686217,
+            "timestamp": 1.9532603510904343
+        },
+        {
+            "x": 7.565618503926309,
+            "y": 5.859549729252428,
+            "heading": -0.12462132977080041,
+            "angularVelocity": 1.4565260680124767,
+            "velocityX": 1.395992072438578,
+            "velocityY": -0.7653767254946843,
+            "timestamp": 1.9861055631329934
+        },
+        {
+            "x": 7.603788954978725,
+            "y": 5.838647308998552,
+            "heading": -0.08394075693399232,
+            "angularVelocity": 1.2385541242393951,
+            "velocityX": 1.1621313634075436,
+            "velocityY": -0.6363916977236806,
+            "timestamp": 2.0189507751755524
+        },
+        {
+            "x": 7.634297586862898,
+            "y": 5.821958675739452,
+            "heading": -0.050840342274680116,
+            "angularVelocity": 1.0077698574891136,
+            "velocityX": 0.928860859374263,
+            "velocityY": -0.5080994221459559,
+            "timestamp": 2.0517959872181115
+        },
+        {
+            "x": 7.657160251333572,
+            "y": 5.809465047264352,
+            "heading": -0.025644988874275507,
+            "angularVelocity": 0.7670936442051631,
+            "velocityX": 0.6960729752956952,
+            "velocityY": -0.3803789867120345,
+            "timestamp": 2.0846411992606706
+        },
+        {
+            "x": 7.672390418071531,
+            "y": 5.801150340997017,
+            "heading": -0.00862072816733467,
+            "angularVelocity": 0.5183178810016766,
+            "velocityX": 0.46369518693407513,
+            "velocityY": -0.2531481987862983,
+            "timestamp": 2.1174864113032297
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 8.329701949714689e-33,
+            "angularVelocity": 0.26246529193262896,
+            "velocityX": 0.2316800975013136,
+            "velocityY": -0.12636060901771562,
+            "timestamp": 2.1503316233457888
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 5.789840595031671e-40,
+            "angularVelocity": 1.2947513524749855e-30,
+            "velocityX": 9.16355000594536e-32,
+            "velocityY": -1.9486684829089695e-31,
+            "timestamp": 2.183176835388348
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W3 To C3.traj b/src/main/deploy/choreo/W3 To C3.traj
new file mode 100644
index 00000000..ea265506
--- /dev/null
+++ b/src/main/deploy/choreo/W3 To C3.traj	
@@ -0,0 +1,310 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 1.9823938961383557e-29,
+            "timestamp": 0
+        },
+        {
+            "x": 2.312764146242865,
+            "y": 4.398513079462153,
+            "heading": -0.39804543849617763,
+            "angularVelocity": -0.11524733034669515,
+            "velocityX": 0.18284072142228638,
+            "velocityY": 0.5516827447028667,
+            "timestamp": 0.06981019405093988
+        },
+        {
+            "x": 2.339895425931373,
+            "y": 4.474992910905513,
+            "heading": -0.4126392714825622,
+            "angularVelocity": -0.20905017072795415,
+            "velocityX": 0.3886435220157258,
+            "velocityY": 1.0955395910772,
+            "timestamp": 0.13962038810187977
+        },
+        {
+            "x": 2.3838571031189573,
+            "y": 4.588440455462814,
+            "heading": -0.43142102712692415,
+            "angularVelocity": -0.2690403013169746,
+            "velocityX": 0.6297314852832604,
+            "velocityY": 1.6250856497386854,
+            "timestamp": 0.20943058215281965
+        },
+        {
+            "x": 2.448821264845437,
+            "y": 4.736675156040894,
+            "heading": -0.45019318323311186,
+            "angularVelocity": -0.26890279219946406,
+            "velocityX": 0.9305827409539079,
+            "velocityY": 2.1233961972720627,
+            "timestamp": 0.27924077620375953
+        },
+        {
+            "x": 2.542623225560298,
+            "y": 4.913429983875953,
+            "heading": -0.4600490129319591,
+            "angularVelocity": -0.14118037962391586,
+            "velocityX": 1.343671393412285,
+            "velocityY": 2.5319343433823196,
+            "timestamp": 0.34905097025469944
+        },
+        {
+            "x": 2.675927389267578,
+            "y": 5.095797559489528,
+            "heading": -0.44245218860567326,
+            "angularVelocity": 0.25206668688824874,
+            "velocityX": 1.9095228930350086,
+            "velocityY": 2.6123344605070336,
+            "timestamp": 0.41886116430563936
+        },
+        {
+            "x": 2.83772713712192,
+            "y": 5.250967901017311,
+            "heading": -0.38879601501351296,
+            "angularVelocity": 0.7686008371275667,
+            "velocityX": 2.3177094699953265,
+            "velocityY": 2.2227461710524636,
+            "timestamp": 0.48867135835657927
+        },
+        {
+            "x": 3.013235940099558,
+            "y": 5.368218424760011,
+            "heading": -0.31718431053283597,
+            "angularVelocity": 1.0258058360734288,
+            "velocityX": 2.514085591121856,
+            "velocityY": 1.6795616361728827,
+            "timestamp": 0.5584815524075192
+        },
+        {
+            "x": 3.1945050869972476,
+            "y": 5.445323486359522,
+            "heading": -0.23653218942173773,
+            "angularVelocity": 1.1553057859494003,
+            "velocityX": 2.5965999573717062,
+            "velocityY": 1.1044957351381008,
+            "timestamp": 0.6282917464584591
+        },
+        {
+            "x": 3.377225953863421,
+            "y": 5.481834951968023,
+            "heading": -0.15180568644047004,
+            "angularVelocity": 1.2136694952369833,
+            "velocityX": 2.6173952006491676,
+            "velocityY": 0.523010515923758,
+            "timestamp": 0.698101940509399
+        },
+        {
+            "x": 3.558790922164917,
+            "y": 5.477728366851807,
+            "heading": -0.06601521216801902,
+            "angularVelocity": 1.2289104110775417,
+            "velocityX": 2.6008374674955865,
+            "velocityY": -0.05882500648124433,
+            "timestamp": 0.7679121345603389
+        },
+        {
+            "x": 3.7207755893743903,
+            "y": 5.440802283745667,
+            "heading": 0.01087618717506099,
+            "angularVelocity": 1.2168377614801031,
+            "velocityX": 2.5634734381227786,
+            "velocityY": -0.5843703286511746,
+            "timestamp": 0.8311016588990232
+        },
+        {
+            "x": 3.8800031263189014,
+            "y": 5.370704697701765,
+            "heading": 0.08523277253593173,
+            "angularVelocity": 1.1767232961398173,
+            "velocityX": 2.519840726939224,
+            "velocityY": -1.1093228945227975,
+            "timestamp": 0.8942911832377075
+        },
+        {
+            "x": 4.035880089753818,
+            "y": 5.267514783368049,
+            "heading": 0.15438353932229767,
+            "angularVelocity": 1.0943390935099226,
+            "velocityX": 2.4668165343191313,
+            "velocityY": -1.633022489291642,
+            "timestamp": 0.9574807075763918
+        },
+        {
+            "x": 4.187424190703861,
+            "y": 5.131425904894565,
+            "heading": 0.2138534475851711,
+            "angularVelocity": 0.9411355581283213,
+            "velocityX": 2.3982472179488545,
+            "velocityY": -2.153662017503815,
+            "timestamp": 1.0206702319150762
+        },
+        {
+            "x": 4.332733928007212,
+            "y": 4.96304651892829,
+            "heading": 0.25476003945729764,
+            "angularVelocity": 0.6473635034690916,
+            "velocityX": 2.2995858699928062,
+            "velocityY": -2.664672471059999,
+            "timestamp": 1.0838597562537606
+        },
+        {
+            "x": 4.46716744223408,
+            "y": 4.765388489141315,
+            "heading": 0.2547605945579538,
+            "angularVelocity": 0.000008784690993212257,
+            "velocityX": 2.12746520302083,
+            "velocityY": -3.1280189533989655,
+            "timestamp": 1.147049280592445
+        },
+        {
+            "x": 4.612743650855858,
+            "y": 4.575786978724626,
+            "heading": 0.2547605407972418,
+            "angularVelocity": -8.507852266556338e-7,
+            "velocityX": 2.303802887345736,
+            "velocityY": -3.0005212478052368,
+            "timestamp": 1.2102388049311295
+        },
+        {
+            "x": 4.782527461386278,
+            "y": 4.409030186652972,
+            "heading": 0.25208197397278675,
+            "angularVelocity": -0.04238941267495421,
+            "velocityX": 2.6868980627285057,
+            "velocityY": -2.638994260793031,
+            "timestamp": 1.2734283292698139
+        },
+        {
+            "x": 4.970403968588328,
+            "y": 4.270066434839271,
+            "heading": 0.23891743895373094,
+            "angularVelocity": -0.2083341369570075,
+            "velocityX": 2.9732223682415455,
+            "velocityY": -2.1991580609051504,
+            "timestamp": 1.3366178536084983
+        },
+        {
+            "x": 5.173968377386639,
+            "y": 4.1604195633547185,
+            "heading": 0.21971651393307054,
+            "angularVelocity": -0.3038624711945638,
+            "velocityX": 3.2214898106793903,
+            "velocityY": -1.7352064702582655,
+            "timestamp": 1.3998073779471827
+        },
+        {
+            "x": 5.390789031982422,
+            "y": 4.081310272216797,
+            "heading": 0.19899217158132915,
+            "angularVelocity": -0.3279711719205329,
+            "velocityX": 3.4312753081359326,
+            "velocityY": -1.25193680386463,
+            "timestamp": 1.462996902285867
+        },
+        {
+            "x": 5.681530534379024,
+            "y": 4.031634954440038,
+            "heading": 0.1807325834147537,
+            "angularVelocity": -0.22892527550150313,
+            "velocityX": 3.645102941474752,
+            "velocityY": -0.6227925681552634,
+            "timestamp": 1.542759122191511
+        },
+        {
+            "x": 5.983259555492767,
+            "y": 4.033604453850848,
+            "heading": 0.1807316659794543,
+            "angularVelocity": -0.000011502133581259988,
+            "velocityX": 3.7828563632073102,
+            "velocityY": 0.024692133859281144,
+            "timestamp": 1.6225213420971547
+        },
+        {
+            "x": 6.284539191755923,
+            "y": 4.050188554583329,
+            "heading": 0.18073165818528908,
+            "angularVelocity": -9.771750653524246e-8,
+            "velocityX": 3.777222306745724,
+            "velocityY": 0.20791924738433482,
+            "timestamp": 1.7022835620027985
+        },
+        {
+            "x": 6.585818825161056,
+            "y": 4.0667727072366,
+            "heading": 0.18073165039112304,
+            "angularVelocity": -9.77175162921636e-8,
+            "velocityX": 3.777222270913928,
+            "velocityY": 0.20791989832899174,
+            "timestamp": 1.7820457819084423
+        },
+        {
+            "x": 6.88709845856161,
+            "y": 4.083356859971397,
+            "heading": 0.180731642596831,
+            "angularVelocity": -9.771909620772691e-8,
+            "velocityX": 3.777222270856537,
+            "velocityY": 0.20791989935110283,
+            "timestamp": 1.8618080018140861
+        },
+        {
+            "x": 7.15139892307148,
+            "y": 4.097904823415219,
+            "heading": 0.12048670978727856,
+            "angularVelocity": -0.7553066211144209,
+            "velocityX": 3.313604671767403,
+            "velocityY": 0.1823916568650699,
+            "timestamp": 1.94157022171973
+        },
+        {
+            "x": 7.3628393316310445,
+            "y": 4.109542993215659,
+            "heading": 0.07229126938267637,
+            "angularVelocity": -0.6042389550038586,
+            "velocityX": 2.650884200686756,
+            "velocityY": 0.1459108060709806,
+            "timestamp": 2.0213324416253737
+        },
+        {
+            "x": 7.521419659615098,
+            "y": 4.118271521022388,
+            "heading": 0.03614542857914898,
+            "angularVelocity": -0.4531699449471921,
+            "velocityX": 1.98816342087339,
+            "velocityY": 0.10943185654883955,
+            "timestamp": 2.1010946615310178
+        },
+        {
+            "x": 7.627139886542953,
+            "y": 4.12409050689369,
+            "heading": 0.012048510432709781,
+            "angularVelocity": -0.302109421915929,
+            "velocityX": 1.3254423842882344,
+            "velocityY": 0.07295416148382058,
+            "timestamp": 2.180856881436662
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 2.791295127527068e-32,
+            "angularVelocity": -0.15105535486544666,
+            "velocityX": 0.6627211920578459,
+            "velocityY": 0.03647708288149429,
+            "timestamp": 2.260619101342306
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": -1.938584239537671e-30,
+            "velocityX": -1.689352589413745e-32,
+            "velocityY": 0,
+            "timestamp": 2.34038132124795
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W3 To C4.traj b/src/main/deploy/choreo/W3 To C4.traj
new file mode 100644
index 00000000..ce59bb95
--- /dev/null
+++ b/src/main/deploy/choreo/W3 To C4.traj	
@@ -0,0 +1,625 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.298402468716654,
+            "y": 4.3526843009931255,
+            "heading": -0.37533408415570846,
+            "angularVelocity": 0.4620245693037669,
+            "velocityX": -0.050327487964169394,
+            "velocityY": -0.2304685720750862,
+            "timestamp": 0.03174271850172795
+        },
+        {
+            "x": 2.2952744131725273,
+            "y": 4.338010072410149,
+            "heading": -0.34621912169362346,
+            "angularVelocity": 0.9172170449263883,
+            "velocityX": -0.09854403440450425,
+            "velocityY": -0.462286447903903,
+            "timestamp": 0.0634854370034559
+        },
+        {
+            "x": 2.2907034035575107,
+            "y": 4.3159283160610205,
+            "heading": -0.3028943022400045,
+            "angularVelocity": 1.364874260887912,
+            "velocityX": -0.14400183194038138,
+            "velocityY": -0.6956479278208441,
+            "timestamp": 0.09522815550518385
+        },
+        {
+            "x": 2.2847999846220546,
+            "y": 4.286378308280002,
+            "heading": -0.24568561151458213,
+            "angularVelocity": 1.8022618548662834,
+            "velocityX": -0.1859771063759033,
+            "velocityY": -0.9309224028625668,
+            "timestamp": 0.1269708740069118
+        },
+        {
+            "x": 2.2777013130983144,
+            "y": 4.249280679670162,
+            "heading": -0.1750916915555501,
+            "angularVelocity": 2.223940585151498,
+            "velocityX": -0.22363149278956135,
+            "velocityY": -1.1686972748670326,
+            "timestamp": 0.15871359250863973
+        },
+        {
+            "x": 2.2695785209744463,
+            "y": 4.204529290238701,
+            "heading": -0.09189311777806415,
+            "angularVelocity": 2.6210286233977427,
+            "velocityX": -0.25589465890975394,
+            "velocityY": -1.409815905623355,
+            "timestamp": 0.19045631101036767
+        },
+        {
+            "x": 2.2606569711878857,
+            "y": 4.151982062664549,
+            "heading": 0.002706319559794192,
+            "angularVelocity": 2.9801933105606198,
+            "velocityX": -0.2810581515277304,
+            "velocityY": -1.6554104391308149,
+            "timestamp": 0.2221990295120956
+        },
+        {
+            "x": 2.251280347316294,
+            "y": 4.091451352956859,
+            "heading": 0.10685098530745366,
+            "angularVelocity": 3.2808993893194844,
+            "velocityX": -0.2953944814487593,
+            "velocityY": -1.90691637530654,
+            "timestamp": 0.2539417480138236
+        },
+        {
+            "x": 2.242133674900212,
+            "y": 4.022709367456616,
+            "heading": 0.21739538615105536,
+            "angularVelocity": 3.4825120865934673,
+            "velocityX": -0.28815025454054716,
+            "velocityY": -2.165598560706175,
+            "timestamp": 0.2856844665155515
+        },
+        {
+            "x": 2.2351422077578347,
+            "y": 3.945850682832356,
+            "heading": 0.32707167243976176,
+            "angularVelocity": 3.455163623831899,
+            "velocityX": -0.22025420229828702,
+            "velocityY": -2.4213012700873704,
+            "timestamp": 0.31742718501727946
+        },
+        {
+            "x": 2.2344515310594897,
+            "y": 3.864378163543378,
+            "heading": 0.4256089749961829,
+            "angularVelocity": 3.1042490122910227,
+            "velocityX": -0.021758586880562782,
+            "velocityY": -2.566652231898267,
+            "timestamp": 0.3491699035190074
+        },
+        {
+            "x": 2.2416429003575074,
+            "y": 3.7806290345563274,
+            "heading": 0.5177286971400864,
+            "angularVelocity": 2.9020741288711216,
+            "velocityX": 0.22655177746121863,
+            "velocityY": -2.638372922674897,
+            "timestamp": 0.38091262202073534
+        },
+        {
+            "x": 2.256915898193716,
+            "y": 3.695399323053411,
+            "heading": 0.6036240465401915,
+            "angularVelocity": 2.705985922265222,
+            "velocityX": 0.4811496480800017,
+            "velocityY": -2.6850161399464656,
+            "timestamp": 0.4126553405224633
+        },
+        {
+            "x": 2.280071045497703,
+            "y": 3.6084777900686467,
+            "heading": 0.6811513337134051,
+            "angularVelocity": 2.4423644486842004,
+            "velocityX": 0.729463272111561,
+            "velocityY": -2.7383140791811433,
+            "timestamp": 0.4443980590241912
+        },
+        {
+            "x": 2.3096855944046597,
+            "y": 3.5244760350530098,
+            "heading": 0.76088689172136,
+            "angularVelocity": 2.511932240574098,
+            "velocityX": 0.932955660534971,
+            "velocityY": -2.6463314731869354,
+            "timestamp": 0.47614077752591916
+        },
+        {
+            "x": 2.3348942368332115,
+            "y": 3.446420745412462,
+            "heading": 0.8559069862879931,
+            "angularVelocity": 2.993445396350149,
+            "velocityX": 0.7941551202420774,
+            "velocityY": -2.458998262429792,
+            "timestamp": 0.5078834960276472
+        },
+        {
+            "x": 2.355075569752848,
+            "y": 3.373868279566975,
+            "heading": 0.9657696902827837,
+            "angularVelocity": 3.4610363944982643,
+            "velocityX": 0.6357783413709821,
+            "velocityY": -2.285641220097014,
+            "timestamp": 0.5396262145293751
+        },
+        {
+            "x": 2.370025604730125,
+            "y": 3.306495756130267,
+            "heading": 1.091029644988018,
+            "angularVelocity": 3.9461004166489144,
+            "velocityX": 0.4709752561508733,
+            "velocityY": -2.1224560030370347,
+            "timestamp": 0.571368933031103
+        },
+        {
+            "x": 2.3800631598495494,
+            "y": 3.2441375159647747,
+            "heading": 1.2334110423596254,
+            "angularVelocity": 4.485482154399005,
+            "velocityX": 0.31621598883778346,
+            "velocityY": -1.9644895934825695,
+            "timestamp": 0.603111651532831
+        },
+        {
+            "x": 2.3890320547539976,
+            "y": 3.1882487557001293,
+            "heading": 1.3963738258608078,
+            "angularVelocity": 5.133863487221998,
+            "velocityX": 0.28254967840764567,
+            "velocityY": -1.760679705539431,
+            "timestamp": 0.6348543700345589
+        },
+        {
+            "x": 2.399614951757013,
+            "y": 3.1248820965808815,
+            "heading": 1.5465533770663946,
+            "angularVelocity": 4.7311496397956585,
+            "velocityX": 0.33339605120587323,
+            "velocityY": -1.9962581061165,
+            "timestamp": 0.6665970885362869
+        },
+        {
+            "x": 2.4156041612276775,
+            "y": 3.056078363317597,
+            "heading": 1.6846738705269053,
+            "angularVelocity": 4.351249671731503,
+            "velocityX": 0.5037126694046519,
+            "velocityY": -2.167543818263053,
+            "timestamp": 0.6983398070380148
+        },
+        {
+            "x": 2.4386535219180887,
+            "y": 2.9838461830329104,
+            "heading": 1.8122491249375776,
+            "angularVelocity": 4.019039969866676,
+            "velocityX": 0.7261306459671242,
+            "velocityY": -2.275551171861821,
+            "timestamp": 0.7300825255397427
+        },
+        {
+            "x": 2.4695800740474128,
+            "y": 2.910121142766613,
+            "heading": 1.9308538821800434,
+            "angularVelocity": 3.736439814882526,
+            "velocityX": 0.9742880757878827,
+            "velocityY": -2.3225811696715355,
+            "timestamp": 0.7618252440414707
+        },
+        {
+            "x": 2.508646228883482,
+            "y": 2.8366969767650785,
+            "heading": 2.0420489888957944,
+            "angularVelocity": 3.503011461028351,
+            "velocityX": 1.2307123233299608,
+            "velocityY": -2.313102641084062,
+            "timestamp": 0.7935679625431986
+        },
+        {
+            "x": 2.555734299118471,
+            "y": 2.7651139971749186,
+            "heading": 2.147169748045839,
+            "angularVelocity": 3.3116495408016253,
+            "velocityX": 1.4834290337302587,
+            "velocityY": -2.255099215470886,
+            "timestamp": 0.8253106810449266
+        },
+        {
+            "x": 2.6104869842529297,
+            "y": 2.6966493129730225,
+            "heading": 2.247054267099527,
+            "angularVelocity": 3.1466907614812087,
+            "velocityX": 1.7248896036260801,
+            "velocityY": -2.1568626580665695,
+            "timestamp": 0.8570533995466545
+        },
+        {
+            "x": 2.751018773357776,
+            "y": 2.5737681266542305,
+            "heading": 2.423101617636397,
+            "angularVelocity": 2.7414576820608,
+            "velocityX": 2.188399607493837,
+            "velocityY": -1.9135395743649004,
+            "timestamp": 0.9212700940770386
+        },
+        {
+            "x": 2.920945282929504,
+            "y": 2.4615612619997704,
+            "heading": 2.5486951706813996,
+            "angularVelocity": 1.9557772937936253,
+            "velocityX": 2.646142265876451,
+            "velocityY": -1.7473161064272718,
+            "timestamp": 0.9854867886074228
+        },
+        {
+            "x": 3.118708267205298,
+            "y": 2.3583606370011725,
+            "heading": 2.6122511958810777,
+            "angularVelocity": 0.9897118757741349,
+            "velocityX": 3.079619493373657,
+            "velocityY": -1.6070684695514719,
+            "timestamp": 1.049703483137807
+        },
+        {
+            "x": 3.3434611246912245,
+            "y": 2.26616504003377,
+            "heading": 2.6122519169792926,
+            "angularVelocity": 0.000011229139401682067,
+            "velocityX": 3.499913209945475,
+            "velocityY": -1.4356951512629466,
+            "timestamp": 1.113920177668191
+        },
+        {
+            "x": 3.5722970045795748,
+            "y": 2.184629282343257,
+            "heading": 2.6122522165022306,
+            "angularVelocity": 0.000004664253435080619,
+            "velocityX": 3.5634951559220656,
+            "velocityY": -1.269697207039109,
+            "timestamp": 1.1781368721985752
+        },
+        {
+            "x": 3.80113295504209,
+            "y": 2.103093722723815,
+            "heading": 2.6122525160244625,
+            "angularVelocity": 0.000004664242440601037,
+            "velocityX": 3.56349625492233,
+            "velocityY": -1.2696941226220015,
+            "timestamp": 1.2423535667289594
+        },
+        {
+            "x": 4.029968923086795,
+            "y": 2.0215582124500746,
+            "heading": 2.612252815546594,
+            "angularVelocity": 0.00000466424087155042,
+            "velocityX": 3.563496528717025,
+            "velocityY": -1.2696933541971844,
+            "timestamp": 1.3065702612593435
+        },
+        {
+            "x": 4.258804909490432,
+            "y": 1.940022753702107,
+            "heading": 2.6122531150686714,
+            "angularVelocity": 0.000004664240033500949,
+            "velocityX": 3.5634968146073587,
+            "velocityY": -1.2696925518237052,
+            "timestamp": 1.3707869557897276
+        },
+        {
+            "x": 4.487640916105967,
+            "y": 1.8584873516805105,
+            "heading": 2.6122534145907514,
+            "angularVelocity": 0.000004664240074560689,
+            "velocityX": 3.563497129352575,
+            "velocityY": -1.2696916684650783,
+            "timestamp": 1.4350036503201118
+        },
+        {
+            "x": 4.716476945604857,
+            "y": 1.7769520138830965,
+            "heading": 2.6122537141129194,
+            "angularVelocity": 0.000004664241438058459,
+            "velocityX": 3.5634974856984654,
+            "velocityY": -1.2696906683484759,
+            "timestamp": 1.499220344850496
+        },
+        {
+            "x": 4.945312999982612,
+            "y": 1.6954167459104819,
+            "heading": 2.612254013635151,
+            "angularVelocity": 0.000004664242435524347,
+            "velocityX": 3.563497873118986,
+            "velocityY": -1.269689581017522,
+            "timestamp": 1.56343703938088
+        },
+        {
+            "x": 5.174149135873993,
+            "y": 1.613881706713074,
+            "heading": 2.6122543131567744,
+            "angularVelocity": 0.0000046642329655920085,
+            "velocityX": 3.563499142471556,
+            "velocityY": -1.269686018467187,
+            "timestamp": 1.6276537339112642
+        },
+        {
+            "x": 5.406651475687904,
+            "y": 1.5434785463415874,
+            "heading": 2.61225458420666,
+            "angularVelocity": 0.000004220863245612629,
+            "velocityX": 3.6205902766281897,
+            "velocityY": -1.096337344772168,
+            "timestamp": 1.6918704284416484
+        },
+        {
+            "x": 5.646764755249023,
+            "y": 1.5066063404083252,
+            "heading": 2.6122548161665473,
+            "angularVelocity": 0.0000036121430551488755,
+            "velocityX": 3.7391099202016678,
+            "velocityY": -0.5741841152508433,
+            "timestamp": 1.7560871229720325
+        },
+        {
+            "x": 5.787436421990979,
+            "y": 1.496717214982701,
+            "heading": 2.612254832075579,
+            "angularVelocity": 4.2677202387530557e-7,
+            "velocityX": 3.773625708136921,
+            "velocityY": -0.26528339929021244,
+            "timestamp": 1.7933647115888978
+        },
+        {
+            "x": 5.926771623718837,
+            "y": 1.4982765413849686,
+            "heading": 2.6080354125638485,
+            "angularVelocity": -0.11318917527356848,
+            "velocityX": 3.7377740057150746,
+            "velocityY": 0.04183013065287323,
+            "timestamp": 1.830642300205763
+        },
+        {
+            "x": 6.064421169286406,
+            "y": 1.511276136415188,
+            "heading": 2.60121232475526,
+            "angularVelocity": -0.18303458087688948,
+            "velocityX": 3.6925549820916683,
+            "velocityY": 0.34872413996054913,
+            "timestamp": 1.8679198888226283
+        },
+        {
+            "x": 6.200023778520788,
+            "y": 1.5356760041928015,
+            "heading": 2.5932728220304684,
+            "angularVelocity": -0.2129832700927332,
+            "velocityX": 3.6376443398227996,
+            "velocityY": 0.6545452289951588,
+            "timestamp": 1.9051974774394935
+        },
+        {
+            "x": 6.333175231231244,
+            "y": 1.5714002898334831,
+            "heading": 2.5855391112934316,
+            "angularVelocity": -0.20746274166290535,
+            "velocityX": 3.5718901798871894,
+            "velocityY": 0.9583314523868992,
+            "timestamp": 1.9424750660563588
+        },
+        {
+            "x": 6.463368734714764,
+            "y": 1.6183219287773098,
+            "heading": 2.5790588590244545,
+            "angularVelocity": -0.17383775371256402,
+            "velocityX": 3.4925409157130343,
+            "velocityY": 1.258709071181673,
+            "timestamp": 1.979752654673224
+        },
+        {
+            "x": 6.589861303526444,
+            "y": 1.6762129661852994,
+            "heading": 2.574283804517795,
+            "angularVelocity": -0.12809451157737914,
+            "velocityX": 3.393260495247066,
+            "velocityY": 1.5529716260079764,
+            "timestamp": 2.017030243290089
+        },
+        {
+            "x": 6.711340738368746,
+            "y": 1.7445488061219798,
+            "heading": 2.570091844488755,
+            "angularVelocity": -0.11245255352014773,
+            "velocityX": 3.258779318878531,
+            "velocityY": 1.8331614911851861,
+            "timestamp": 2.054307831906954
+        },
+        {
+            "x": 6.825305386557433,
+            "y": 1.8215960437325822,
+            "heading": 2.560973344137785,
+            "angularVelocity": -0.24461078866147912,
+            "velocityX": 3.057189384216965,
+            "velocityY": 2.0668514372666396,
+            "timestamp": 2.091585420523819
+        },
+        {
+            "x": 6.929260415482823,
+            "y": 1.9025407238929235,
+            "heading": 2.5356026772435865,
+            "angularVelocity": -0.680587662333947,
+            "velocityX": 2.788673645010342,
+            "velocityY": 2.171403332771371,
+            "timestamp": 2.128863009140684
+        },
+        {
+            "x": 7.023455735639986,
+            "y": 1.9838668818491878,
+            "heading": 2.488274688562586,
+            "angularVelocity": -1.2696097155701718,
+            "velocityX": 2.5268619471418052,
+            "velocityY": 2.1816367681967113,
+            "timestamp": 2.166140597757549
+        },
+        {
+            "x": 7.110101814938688,
+            "y": 2.060020595789651,
+            "heading": 2.4125158730705962,
+            "angularVelocity": -2.0322885224854694,
+            "velocityX": 2.3243477519224953,
+            "velocityY": 2.0428819772427436,
+            "timestamp": 2.2034181863744142
+        },
+        {
+            "x": 7.191835709933807,
+            "y": 2.1285351921147058,
+            "heading": 2.3080325133818067,
+            "angularVelocity": -2.8028465242925935,
+            "velocityX": 2.1925746280203615,
+            "velocityY": 1.8379567688575535,
+            "timestamp": 2.2406957749912793
+        },
+        {
+            "x": 7.274315415796274,
+            "y": 2.1867028423762775,
+            "heading": 2.1850264648931392,
+            "angularVelocity": -3.2997319046816447,
+            "velocityX": 2.2125815784434106,
+            "velocityY": 1.5603919786607532,
+            "timestamp": 2.2779733636081443
+        },
+        {
+            "x": 7.351412179448119,
+            "y": 2.2360068924730196,
+            "heading": 2.043511695780604,
+            "angularVelocity": -3.796242578000636,
+            "velocityX": 2.0681800114335913,
+            "velocityY": 1.322619083639869,
+            "timestamp": 2.3152509522250093
+        },
+        {
+            "x": 7.420172175937219,
+            "y": 2.2789013315216375,
+            "heading": 1.8838780266002726,
+            "angularVelocity": -4.282296014933475,
+            "velocityX": 1.8445398170951257,
+            "velocityY": 1.1506763350355667,
+            "timestamp": 2.3525285408418743
+        },
+        {
+            "x": 7.479537718839642,
+            "y": 2.3170603162912466,
+            "heading": 1.7061327210648876,
+            "angularVelocity": -4.768154597182533,
+            "velocityX": 1.592526370537947,
+            "velocityY": 1.0236441300375574,
+            "timestamp": 2.3898061294587394
+        },
+        {
+            "x": 7.529281054923766,
+            "y": 2.3515994880332096,
+            "heading": 1.5086439015102746,
+            "angularVelocity": -5.2977895535137876,
+            "velocityX": 1.3344032682848619,
+            "velocityY": 0.9265398601007353,
+            "timestamp": 2.4270837180756044
+        },
+        {
+            "x": 7.569719945861703,
+            "y": 2.383438536522646,
+            "heading": 1.288404786035775,
+            "angularVelocity": -5.908083748068905,
+            "velocityX": 1.0848043674059311,
+            "velocityY": 0.8541069760888933,
+            "timestamp": 2.4643613066924694
+        },
+        {
+            "x": 7.603202598509127,
+            "y": 2.4091886515491456,
+            "heading": 1.0408456353949866,
+            "angularVelocity": -6.640964714353526,
+            "velocityX": 0.8981979223912477,
+            "velocityY": 0.6907666504707783,
+            "timestamp": 2.5016388953093345
+        },
+        {
+            "x": 7.630132008026638,
+            "y": 2.427100499568469,
+            "heading": 0.7720308301798764,
+            "angularVelocity": -7.211164004677401,
+            "velocityX": 0.7224021326670985,
+            "velocityY": 0.48049910640464033,
+            "timestamp": 2.5389164839261995
+        },
+        {
+            "x": 7.649623319500903,
+            "y": 2.438227448776781,
+            "heading": 0.5254269726030665,
+            "angularVelocity": -6.615338242807932,
+            "velocityX": 0.5228694289911082,
+            "velocityY": 0.29848897477445496,
+            "timestamp": 2.5761940725430645
+        },
+        {
+            "x": 7.66290885295536,
+            "y": 2.4458359453096237,
+            "heading": 0.3183730749926382,
+            "angularVelocity": -5.554380132752273,
+            "velocityX": 0.3563946582222695,
+            "velocityY": 0.2041037742822349,
+            "timestamp": 2.6134716611599296
+        },
+        {
+            "x": 7.671732744074589,
+            "y": 2.451418069701132,
+            "heading": 0.15988099310288126,
+            "angularVelocity": -4.251672057404801,
+            "velocityX": 0.23670766931625417,
+            "velocityY": 0.14974478228408286,
+            "timestamp": 2.6507492497767946
+        },
+        {
+            "x": 7.677275184086734,
+            "y": 2.4551558590330185,
+            "heading": 0.05341064749493927,
+            "angularVelocity": -2.8561489505727446,
+            "velocityX": 0.14868021827027583,
+            "velocityY": 0.10026907508163654,
+            "timestamp": 2.6880268383936596
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 1.0247603884557974e-38,
+            "angularVelocity": -1.4327817189005392,
+            "velocityX": 0.07309528363731636,
+            "velocityY": 0.04947050051802475,
+            "timestamp": 2.7253044270105247
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": -2.6319896813971663e-37,
+            "velocityX": 5.227147746061896e-36,
+            "velocityY": -6.467829854732196e-35,
+            "timestamp": 2.7625820156273897
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W3 To C5.traj b/src/main/deploy/choreo/W3 To C5.traj
new file mode 100644
index 00000000..8826d851
--- /dev/null
+++ b/src/main/deploy/choreo/W3 To C5.traj	
@@ -0,0 +1,571 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -4.609910300520975e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 2.298808348352727,
+            "y": 4.35514296819253,
+            "heading": -0.3837399750205753,
+            "angularVelocity": 0.24884159467028183,
+            "velocityX": -0.047369219319970686,
+            "velocityY": -0.19307136062676852,
+            "timestamp": 0.02515666638336474
+        },
+        {
+            "x": 2.2965596540188837,
+            "y": 4.3453750993410925,
+            "heading": -0.37149402212882315,
+            "angularVelocity": 0.48678758564965857,
+            "velocityX": -0.08938761199815681,
+            "velocityY": -0.38828152755153583,
+            "timestamp": 0.05031333276672948
+        },
+        {
+            "x": 2.2934096466067952,
+            "y": 4.330641358670927,
+            "heading": -0.3535645425221108,
+            "angularVelocity": 0.7127128584321681,
+            "velocityX": -0.1252156133919033,
+            "velocityY": -0.5856793760205414,
+            "timestamp": 0.07546999915009422
+        },
+        {
+            "x": 2.289538880665139,
+            "y": 4.310886769515238,
+            "heading": -0.33028900557963653,
+            "angularVelocity": 0.9252234214095049,
+            "velocityX": -0.15386640990779057,
+            "velocityY": -0.7852625961900879,
+            "timestamp": 0.10062666553345896
+        },
+        {
+            "x": 2.2851573432175654,
+            "y": 4.286058471237428,
+            "heading": -0.3020481691352866,
+            "angularVelocity": 1.1225985197714665,
+            "velocityX": -0.17417003432819553,
+            "velocityY": -0.9869470739663532,
+            "timestamp": 0.1257833319168237
+        },
+        {
+            "x": 2.280510060472114,
+            "y": 4.25610900707676,
+            "heading": -0.26927550139535,
+            "angularVelocity": 1.3027428690475438,
+            "velocityX": -0.18473364771909595,
+            "velocityY": -1.1905180004483138,
+            "timestamp": 0.15093999830018845
+        },
+        {
+            "x": 2.2758836751536,
+            "y": 4.2210014881511375,
+            "heading": -0.23246752663964654,
+            "angularVelocity": 1.463149933889621,
+            "velocityX": -0.1839029562984313,
+            "velocityY": -1.3955552930033008,
+            "timestamp": 0.1760966646835532
+        },
+        {
+            "x": 2.271613693612169,
+            "y": 4.180717494993525,
+            "heading": -0.19219487185489256,
+            "angularVelocity": 1.6008740653882818,
+            "velocityX": -0.16973558723403512,
+            "velocityY": -1.6013247758554696,
+            "timestamp": 0.20125333106691795
+        },
+        {
+            "x": 2.268091592763015,
+            "y": 4.135268758759812,
+            "heading": -0.1491141249151469,
+            "angularVelocity": 1.7124982413501877,
+            "velocityX": -0.1400066604803655,
+            "velocityY": -1.8066279347634933,
+            "timestamp": 0.2264099974502827
+        },
+        {
+            "x": 2.2657701315686967,
+            "y": 4.084713672798125,
+            "heading": -0.10398137252966654,
+            "angularVelocity": 1.7940672940403972,
+            "velocityX": -0.09228015981695388,
+            "velocityY": -2.0096099058307106,
+            "timestamp": 0.25156666383364745
+        },
+        {
+            "x": 2.265163909208295,
+            "y": 4.029179243747037,
+            "heading": -0.057668550112221675,
+            "angularVelocity": 1.8409761337881332,
+            "velocityX": -0.02409788129965802,
+            "velocityY": -2.2075432493635576,
+            "timestamp": 0.2767233302170122
+        },
+        {
+            "x": 2.266840363517046,
+            "y": 3.9688874958283327,
+            "heading": -0.011179237419756825,
+            "angularVelocity": 1.8479917801512262,
+            "velocityX": 0.06664055893588815,
+            "velocityY": -2.3966509314037507,
+            "timestamp": 0.3018799966003769
+        },
+        {
+            "x": 2.2713096946462064,
+            "y": 3.9041248350083095,
+            "heading": 0.03446639703260334,
+            "angularVelocity": 1.8144548151476838,
+            "velocityX": 0.1776599117328043,
+            "velocityY": -2.574373719995273,
+            "timestamp": 0.3270366629837416
+        },
+        {
+            "x": 2.2787160490905882,
+            "y": 3.8349972418338227,
+            "heading": 0.07865637806724814,
+            "angularVelocity": 1.7565912892125506,
+            "velocityX": 0.29440921668697884,
+            "velocityY": -2.74788368701343,
+            "timestamp": 0.35219332936710634
+        },
+        {
+            "x": 2.289237241442295,
+            "y": 3.7616571782461574,
+            "heading": 0.12059430680114623,
+            "angularVelocity": 1.667070195025133,
+            "velocityX": 0.41822681079334173,
+            "velocityY": -2.915333155436007,
+            "timestamp": 0.37734999575047107
+        },
+        {
+            "x": 2.303092280593543,
+            "y": 3.684333317171128,
+            "heading": 0.15922184993862323,
+            "angularVelocity": 1.5354794052927534,
+            "velocityX": 0.550750204343837,
+            "velocityY": -3.073692670431113,
+            "timestamp": 0.4025066621338358
+        },
+        {
+            "x": 2.3205444941015814,
+            "y": 3.6033821039148743,
+            "heading": 0.19314125064776663,
+            "angularVelocity": 1.3483265307192482,
+            "velocityX": 0.6937411039317692,
+            "velocityY": -3.217883165544707,
+            "timestamp": 0.4276633285172005
+        },
+        {
+            "x": 2.341904587688066,
+            "y": 3.5193842561187316,
+            "heading": 0.22066827104785883,
+            "angularVelocity": 1.0942236932591152,
+            "velocityX": 0.8490828339882706,
+            "velocityY": -3.3389896147642064,
+            "timestamp": 0.45281999490056524
+        },
+        {
+            "x": 2.3675851289681646,
+            "y": 3.4337423994061407,
+            "heading": 0.23918679984335672,
+            "angularVelocity": 0.7361280907928144,
+            "velocityX": 1.0208244959308244,
+            "velocityY": -3.40434044032252,
+            "timestamp": 0.47797666128392996
+        },
+        {
+            "x": 2.3978885412195137,
+            "y": 3.3469640370766824,
+            "heading": 0.24945072521405584,
+            "angularVelocity": 0.40800021808479553,
+            "velocityX": 1.2045877537807594,
+            "velocityY": -3.4495175555868487,
+            "timestamp": 0.5031333276672947
+        },
+        {
+            "x": 2.4331457577160354,
+            "y": 3.2598611919071945,
+            "heading": 0.25313177220508776,
+            "angularVelocity": 0.1463249118518414,
+            "velocityX": 1.4015059054023244,
+            "velocityY": -3.462416038839161,
+            "timestamp": 0.5282899940506595
+        },
+        {
+            "x": 2.473498104501041,
+            "y": 3.173715186571369,
+            "heading": 0.2532432496800442,
+            "angularVelocity": 0.004431329384332257,
+            "velocityX": 1.604041893710102,
+            "velocityY": -3.424380799229854,
+            "timestamp": 0.5534466604340242
+        },
+        {
+            "x": 2.5185817721335604,
+            "y": 3.089905547060991,
+            "heading": 0.25324321969105484,
+            "angularVelocity": -0.0000011920891614956396,
+            "velocityX": 1.7921161311870697,
+            "velocityY": -3.3315081669882356,
+            "timestamp": 0.578603326817389
+        },
+        {
+            "x": 2.568241478882672,
+            "y": 3.0087236123099395,
+            "heading": 0.2532431890633549,
+            "angularVelocity": -0.0000012174784783024135,
+            "velocityX": 1.97401778090718,
+            "velocityY": -3.2270545514223747,
+            "timestamp": 0.6037599932007538
+        },
+        {
+            "x": 2.6223245423144714,
+            "y": 2.9304189809174512,
+            "heading": 0.25324315880142845,
+            "angularVelocity": -0.0000012029386552177117,
+            "velocityX": 2.1498501672527754,
+            "velocityY": -3.1126791681853656,
+            "timestamp": 0.6289166595841186
+        },
+        {
+            "x": 2.6806646810204167,
+            "y": 2.855232404738762,
+            "heading": 0.25324312847978453,
+            "angularVelocity": -0.0000012053124772832063,
+            "velocityX": 2.319072718813166,
+            "velocityY": -2.9887336832676343,
+            "timestamp": 0.6540733259674834
+        },
+        {
+            "x": 2.7430825233459473,
+            "y": 2.7833950519561768,
+            "heading": 0.2532430937019675,
+            "angularVelocity": -0.0000013824493495341482,
+            "velocityX": 2.4811650865953045,
+            "velocityY": -2.8555990562441647,
+            "timestamp": 0.6792299923508481
+        },
+        {
+            "x": 2.8747857816965947,
+            "y": 2.6605842580771375,
+            "heading": 0.2532430684120768,
+            "angularVelocity": -5.312693002073383e-7,
+            "velocityX": 2.7667141273306664,
+            "velocityY": -2.579908520631989,
+            "timestamp": 0.7268327631068262
+        },
+        {
+            "x": 3.0186320127358077,
+            "y": 2.5522491183421203,
+            "heading": 0.2532430469824583,
+            "angularVelocity": -4.5017586399077444e-7,
+            "velocityX": 3.021803747025554,
+            "velocityY": -2.2758158404343,
+            "timestamp": 0.7744355338628043
+        },
+        {
+            "x": 3.1730376030001257,
+            "y": 2.4595822138278454,
+            "heading": 0.2532430277420137,
+            "angularVelocity": -4.041874945252969e-7,
+            "velocityX": 3.2436261127705017,
+            "velocityY": -1.946670394236201,
+            "timestamp": 0.8220383046187824
+        },
+        {
+            "x": 3.336302611834685,
+            "y": 2.38360340882859,
+            "heading": 0.25324300957525164,
+            "angularVelocity": -3.816324501860361e-7,
+            "velocityX": 3.4297375182526793,
+            "velocityY": -1.5961004746706646,
+            "timestamp": 0.8696410753747604
+        },
+        {
+            "x": 3.505316504811254,
+            "y": 2.3214540241877932,
+            "heading": 0.25324299166355524,
+            "angularVelocity": -3.7627424124720095e-7,
+            "velocityX": 3.550505365390803,
+            "velocityY": -1.3055833442844713,
+            "timestamp": 0.9172438461307385
+        },
+        {
+            "x": 3.6743308817857088,
+            "y": 2.2593059557844626,
+            "heading": 0.25324297375187266,
+            "angularVelocity": -3.76273948990246e-7,
+            "velocityX": 3.5505155328218008,
+            "velocityY": -1.3055556938463668,
+            "timestamp": 0.9648466168867166
+        },
+        {
+            "x": 3.843345266545321,
+            "y": 2.1971579085532174,
+            "heading": 0.2532429558401917,
+            "angularVelocity": -3.762739157561073e-7,
+            "velocityX": 3.5505156963660025,
+            "velocityY": -1.3055552490805507,
+            "timestamp": 1.0124493876426945
+        },
+        {
+            "x": 4.012359658311276,
+            "y": 2.135009880376043,
+            "heading": 0.25324293792850894,
+            "angularVelocity": -3.7627395316214525e-7,
+            "velocityX": 3.5505158435494915,
+            "velocityY": -1.3055548488082471,
+            "timestamp": 1.0600521583986726
+        },
+        {
+            "x": 4.181374056391421,
+            "y": 2.07286186937061,
+            "heading": 0.25324292001682175,
+            "angularVelocity": -3.7627404706616014e-7,
+            "velocityX": 3.5505159761928384,
+            "velocityY": -1.30555448807838,
+            "timestamp": 1.1076549291546507
+        },
+        {
+            "x": 4.3503884601627245,
+            "y": 2.010713873842556,
+            "heading": 0.25324290210512795,
+            "angularVelocity": -3.7627418619029544e-7,
+            "velocityX": 3.5505160957480255,
+            "velocityY": -1.30555416294226,
+            "timestamp": 1.1552576999106288
+        },
+        {
+            "x": 4.51940286907005,
+            "y": 1.9485658922821711,
+            "heading": 0.2532428841934256,
+            "angularVelocity": -3.7627436389492574e-7,
+            "velocityX": 3.5505162036413855,
+            "velocityY": -1.3055538695209365,
+            "timestamp": 1.2028604706666068
+        },
+        {
+            "x": 4.688417282561973,
+            "y": 1.8864179231898264,
+            "heading": 0.25324286628171494,
+            "angularVelocity": -3.7627454024029507e-7,
+            "velocityX": 3.550516299950831,
+            "velocityY": -1.3055536076025602,
+            "timestamp": 1.250463241422585
+        },
+        {
+            "x": 4.857431700124039,
+            "y": 1.824269965166445,
+            "heading": 0.2532428483699966,
+            "angularVelocity": -3.7627470129766985e-7,
+            "velocityX": 3.5505163854530717,
+            "velocityY": -1.3055533750748494,
+            "timestamp": 1.298066012178563
+        },
+        {
+            "x": 5.02644612130461,
+            "y": 1.7621220169837726,
+            "heading": 0.2532428304582713,
+            "angularVelocity": -3.76274846815446e-7,
+            "velocityX": 3.5505164614676405,
+            "velocityY": -1.3055531683492934,
+            "timestamp": 1.345668782934541
+        },
+        {
+            "x": 5.1954605457099055,
+            "y": 1.6999740775709098,
+            "heading": 0.25324281254653974,
+            "angularVelocity": -3.7627497879654404e-7,
+            "velocityX": 3.5505165292100247,
+            "velocityY": -1.3055529841203253,
+            "timestamp": 1.3932715536905191
+        },
+        {
+            "x": 5.364474972997734,
+            "y": 1.6378261459972472,
+            "heading": 0.25324279463480254,
+            "angularVelocity": -3.7627509767034604e-7,
+            "velocityX": 3.550516589763906,
+            "velocityY": -1.3055528194408317,
+            "timestamp": 1.4408743244464972
+        },
+        {
+            "x": 5.533489402870974,
+            "y": 1.5756782214547507,
+            "heading": 0.2532427767230601,
+            "angularVelocity": -3.762752068122207e-7,
+            "velocityX": 3.5505166440761182,
+            "velocityY": -1.3055526717358554,
+            "timestamp": 1.4884770952024753
+        },
+        {
+            "x": 5.702503835071411,
+            "y": 1.513530303241192,
+            "heading": 0.25324275881131303,
+            "angularVelocity": -3.7627530496428226e-7,
+            "velocityX": 3.550516692963962,
+            "velocityY": -1.3055525387827163,
+            "timestamp": 1.5360798659584534
+        },
+        {
+            "x": 5.871518269374186,
+            "y": 1.4513823907450571,
+            "heading": 0.2532427408995616,
+            "angularVelocity": -3.762753964251323e-7,
+            "velocityX": 3.5505167371281625,
+            "velocityY": -1.3055524186757583,
+            "timestamp": 1.5836826367144314
+        },
+        {
+            "x": 6.040532705582966,
+            "y": 1.3892344834324082,
+            "heading": 0.25324272298780626,
+            "angularVelocity": -3.7627547903960503e-7,
+            "velocityX": 3.550516777167954,
+            "velocityY": -1.3055523097853368,
+            "timestamp": 1.6312854074704095
+        },
+        {
+            "x": 6.20954714352589,
+            "y": 1.3270865808358587,
+            "heading": 0.2532427050760471,
+            "angularVelocity": -3.7627555795965075e-7,
+            "velocityX": 3.5505168135974197,
+            "velocityY": -1.3055522107133783,
+            "timestamp": 1.6788881782263876
+        },
+        {
+            "x": 6.378561583051887,
+            "y": 1.2649386825445668,
+            "heading": 0.25324268716428455,
+            "angularVelocity": -3.7627562961930894e-7,
+            "velocityX": 3.5505168468533315,
+            "velocityY": -1.3055521202720615,
+            "timestamp": 1.7264909489823657
+        },
+        {
+            "x": 6.547576024027691,
+            "y": 1.2027907881961044,
+            "heading": 0.25324266925251887,
+            "angularVelocity": -3.7627569544162935e-7,
+            "velocityX": 3.5505168773096716,
+            "velocityY": -1.3055520374443248,
+            "timestamp": 1.7740937197383437
+        },
+        {
+            "x": 6.716590466335514,
+            "y": 1.1406428974701475,
+            "heading": 0.25324265134075025,
+            "angularVelocity": -3.762757576316506e-7,
+            "velocityX": 3.550516905291665,
+            "velocityY": -1.3055519613457025,
+            "timestamp": 1.8216964904943218
+        },
+        {
+            "x": 6.88560491686231,
+            "y": 1.0784950290966298,
+            "heading": 0.253242633428611,
+            "angularVelocity": -3.7628354454135297e-7,
+            "velocityX": 3.550517077949107,
+            "velocityY": -1.3055514917839766,
+            "timestamp": 1.8692992612502999
+        },
+        {
+            "x": 7.0444139114910005,
+            "y": 1.0203058482073266,
+            "heading": 0.2259027129119336,
+            "angularVelocity": -0.574334646544581,
+            "velocityX": 3.336129223292024,
+            "velocityY": -1.2223906290579827,
+            "timestamp": 1.916902032006278
+        },
+        {
+            "x": 7.185810534595712,
+            "y": 0.9684234171781619,
+            "heading": 0.18518533646408028,
+            "angularVelocity": -0.8553572786042075,
+            "velocityX": 2.9703443908662663,
+            "velocityY": -1.089903596055891,
+            "timestamp": 1.964504802762256
+        },
+        {
+            "x": 7.3094655243228,
+            "y": 0.9230369546143523,
+            "heading": 0.14380134057962407,
+            "angularVelocity": -0.8693610734677483,
+            "velocityX": 2.59764269523238,
+            "velocityY": -0.953441613650398,
+            "timestamp": 2.012107573518234
+        },
+        {
+            "x": 7.415398593682274,
+            "y": 0.8841490900710189,
+            "heading": 0.10540313701630608,
+            "angularVelocity": -0.8066379950897271,
+            "velocityX": 2.2253551143589885,
+            "velocityY": -0.8169243917057022,
+            "timestamp": 2.059710344274212
+        },
+        {
+            "x": 7.503637039429732,
+            "y": 0.8517540010892736,
+            "heading": 0.07171800478429124,
+            "angularVelocity": -0.70762965468317,
+            "velocityX": 1.8536409613588987,
+            "velocityY": -0.680529483206126,
+            "timestamp": 2.1073131150301903
+        },
+        {
+            "x": 7.574202084965293,
+            "y": 0.8258459262172375,
+            "heading": 0.04375776286271558,
+            "angularVelocity": -0.5873658502969467,
+            "velocityX": 1.482372652157004,
+            "velocityY": -0.5442556065663985,
+            "timestamp": 2.1549158857861683
+        },
+        {
+            "x": 7.627109788299601,
+            "y": 0.8064200554899893,
+            "heading": 0.022189516524924582,
+            "angularVelocity": -0.4530880449870122,
+            "velocityX": 1.1114416764840027,
+            "velocityY": -0.40808277372822227,
+            "timestamp": 2.2025186565421464
+        },
+        {
+            "x": 7.66237251850946,
+            "y": 0.7934724766877013,
+            "heading": 0.0074863600733853135,
+            "angularVelocity": -0.3088718622474896,
+            "velocityX": 0.7407705402406998,
+            "velocityY": -0.27199212559831976,
+            "timestamp": 2.2501214272981245
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -1.0670900456397286e-32,
+            "angularVelocity": -0.1572673177316084,
+            "velocityX": 0.37030368633165434,
+            "velocityY": -0.13596848639085585,
+            "timestamp": 2.2977241980541026
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 2.230662363627099e-31,
+            "velocityX": 2.3924515989499496e-33,
+            "velocityY": 1.2585115119762557e-33,
+            "timestamp": 2.3453269688100806
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W3 To S1.traj b/src/main/deploy/choreo/W3 To S1.traj
new file mode 100644
index 00000000..714e9458
--- /dev/null
+++ b/src/main/deploy/choreo/W3 To S1.traj	
@@ -0,0 +1,175 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 5.020065556819226e-31,
+            "velocityX": -2.911156118414847e-31,
+            "velocityY": 8.635491730351355e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3221690164859408,
+            "y": 4.411229757983623,
+            "heading": -0.4048176043167742,
+            "angularVelocity": -0.1808500047435309,
+            "velocityX": 0.27057455786553103,
+            "velocityY": 0.6252631516043967,
+            "timestamp": 0.08193311544453755
+        },
+        {
+            "x": 2.3687303275118676,
+            "y": 4.5127321003230465,
+            "heading": -0.4313760168590284,
+            "angularVelocity": -0.32414747563532126,
+            "velocityX": 0.5682843960383053,
+            "velocityY": 1.2388439251785637,
+            "timestamp": 0.1638662308890751
+        },
+        {
+            "x": 2.4431953998066236,
+            "y": 4.662720767025029,
+            "heading": -0.4646753197408115,
+            "angularVelocity": -0.40642056268856935,
+            "velocityX": 0.9088519567543129,
+            "velocityY": 1.830623257619443,
+            "timestamp": 0.24579934633361264
+        },
+        {
+            "x": 2.551740018878988,
+            "y": 4.857106976368847,
+            "heading": -0.4953245435557511,
+            "angularVelocity": -0.3740761430663416,
+            "velocityX": 1.3247954564335083,
+            "velocityY": 2.3724986934666186,
+            "timestamp": 0.3277324617781502
+        },
+        {
+            "x": 2.701293241756868,
+            "y": 5.089437731072216,
+            "heading": -0.5138028115710582,
+            "angularVelocity": -0.2255286878210994,
+            "velocityX": 1.8253086321258618,
+            "velocityY": 2.835614799252222,
+            "timestamp": 0.4096655772226877
+        },
+        {
+            "x": 2.9065804913658706,
+            "y": 5.321655101693131,
+            "heading": -0.5138024716246538,
+            "angularVelocity": 0.000004149072112901325,
+            "velocityX": 2.5055467315650244,
+            "velocityY": 2.8342309377715424,
+            "timestamp": 0.4915986926672252
+        },
+        {
+            "x": 3.1502852438919966,
+            "y": 5.513164387705616,
+            "heading": -0.5138023715427595,
+            "angularVelocity": 0.0000012215072474193347,
+            "velocityX": 2.974435320857471,
+            "velocityY": 2.3373856220824885,
+            "timestamp": 0.5735318081117627
+        },
+        {
+            "x": 3.4075351073224858,
+            "y": 5.686052564856294,
+            "heading": -0.5138022777253332,
+            "angularVelocity": 0.0000011450489327031063,
+            "velocityX": 3.1397544452539083,
+            "velocityY": 2.110113550701131,
+            "timestamp": 0.6554649235563003
+        },
+        {
+            "x": 3.664785106053728,
+            "y": 5.858940540685368,
+            "heading": -0.5138021839079745,
+            "angularVelocity": 0.0000011450481052658978,
+            "velocityX": 3.13975609661006,
+            "velocityY": 2.110111093555414,
+            "timestamp": 0.7373980390008378
+        },
+        {
+            "x": 3.922035141067721,
+            "y": 6.031828462527248,
+            "heading": -0.5138020900906538,
+            "angularVelocity": 0.000001145047641454271,
+            "velocityX": 3.1397565394438236,
+            "velocityY": 2.1101104346375243,
+            "timestamp": 0.8193311544453753
+        },
+        {
+            "x": 4.179285313417939,
+            "y": 6.204716180018366,
+            "heading": -0.5138019962733545,
+            "angularVelocity": 0.00000114504738127865,
+            "velocityX": 3.139758215643051,
+            "velocityY": 2.110107940520708,
+            "timestamp": 0.9012642698899128
+        },
+        {
+            "x": 4.443584109358565,
+            "y": 6.354084975447565,
+            "heading": -0.4978326665645833,
+            "angularVelocity": 0.19490690207652947,
+            "velocityX": 3.225787210783358,
+            "velocityY": 1.8230576808752068,
+            "timestamp": 0.9831973853344503
+        },
+        {
+            "x": 4.674709010100242,
+            "y": 6.4693171293955105,
+            "heading": -0.3712044647358083,
+            "angularVelocity": 1.5455069801964878,
+            "velocityX": 2.8208972585465766,
+            "velocityY": 1.406417335929929,
+            "timestamp": 1.065130500778988
+        },
+        {
+            "x": 4.870816853917273,
+            "y": 6.548974021375514,
+            "heading": -0.15522232897542676,
+            "angularVelocity": 2.636078642787472,
+            "velocityX": 2.3935113751384223,
+            "velocityY": 0.9722185168697003,
+            "timestamp": 1.1470636162235255
+        },
+        {
+            "x": 5.022935942406859,
+            "y": 6.594453289671109,
+            "heading": 0.041233782182786366,
+            "angularVelocity": 2.3977620049271122,
+            "velocityX": 1.856625220025449,
+            "velocityY": 0.5550779809707214,
+            "timestamp": 1.228996731668063
+        },
+        {
+            "x": 5.124959992690689,
+            "y": 6.619307485763033,
+            "heading": 0.18624088374392397,
+            "angularVelocity": 1.769822870452196,
+            "velocityX": 1.2452114109208137,
+            "velocityY": 0.3033473822772884,
+            "timestamp": 1.3109298471126005
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.263,
+            "angularVelocity": 0.9368509404239479,
+            "velocityX": 0.6229472299739557,
+            "velocityY": 0.13050296182384175,
+            "timestamp": 1.392862962557138
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.263,
+            "angularVelocity": 1.5668772442975024e-30,
+            "velocityX": 0,
+            "velocityY": -2.8584411322391187e-31,
+            "timestamp": 1.4747960780016756
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W3 To S2.traj b/src/main/deploy/choreo/W3 To S2.traj
new file mode 100644
index 00000000..72f3f88a
--- /dev/null
+++ b/src/main/deploy/choreo/W3 To S2.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3223907681513127,
+            "y": 4.418261357040706,
+            "heading": -0.429219543622513,
+            "angularVelocity": -0.4499475285553182,
+            "velocityX": 0.2568788379871729,
+            "velocityY": 0.6684053711347925,
+            "timestamp": 0.08716470506781053
+        },
+        {
+            "x": 2.374909672838796,
+            "y": 4.531319644218887,
+            "heading": -0.5050759499486299,
+            "angularVelocity": -0.8702651637162607,
+            "velocityX": 0.6025248940683728,
+            "velocityY": 1.297064988520603,
+            "timestamp": 0.17432941013562106
+        },
+        {
+            "x": 2.469260718438912,
+            "y": 4.691180971030917,
+            "heading": -0.6116327562753044,
+            "angularVelocity": -1.2224765315706085,
+            "velocityX": 1.082445532589298,
+            "velocityY": 1.834014429208093,
+            "timestamp": 0.2614941152034316
+        },
+        {
+            "x": 2.620331804373909,
+            "y": 4.8788277526908095,
+            "heading": -0.7357228691689549,
+            "angularVelocity": -1.4236279787456398,
+            "velocityX": 1.7331680961631113,
+            "velocityY": 2.1527839911109723,
+            "timestamp": 0.3486588202712421
+        },
+        {
+            "x": 2.832381156144981,
+            "y": 5.083303561006889,
+            "heading": -0.84759450212177,
+            "angularVelocity": -1.2834510581523242,
+            "velocityX": 2.432743294503276,
+            "velocityY": 2.3458555634072913,
+            "timestamp": 0.43582352533905266
+        },
+        {
+            "x": 3.098611796220168,
+            "y": 5.267387874253087,
+            "heading": -0.8666590247194526,
+            "angularVelocity": -0.21871837440225492,
+            "velocityX": 3.0543399403241303,
+            "velocityY": 2.111913452847551,
+            "timestamp": 0.5229882304068632
+        },
+        {
+            "x": 3.3974595011437105,
+            "y": 5.397776588765608,
+            "heading": -0.8564937886773937,
+            "angularVelocity": 0.11662101115527058,
+            "velocityX": 3.42854030987716,
+            "velocityY": 1.4958888969002289,
+            "timestamp": 0.6101529354746736
+        },
+        {
+            "x": 3.678181484629676,
+            "y": 5.472509187071852,
+            "heading": -0.7581230365630254,
+            "angularVelocity": 1.1285617502845884,
+            "velocityX": 3.220592363245829,
+            "velocityY": 0.8573722385466142,
+            "timestamp": 0.6973176405424841
+        },
+        {
+            "x": 3.915033154173224,
+            "y": 5.514676302538828,
+            "heading": -0.5395209628894968,
+            "angularVelocity": 2.5079196161274817,
+            "velocityX": 2.7172887163363586,
+            "velocityY": 0.4837636453214852,
+            "timestamp": 0.7844823456102946
+        },
+        {
+            "x": 4.091189385673594,
+            "y": 5.53976294658506,
+            "heading": -0.29904398170320484,
+            "angularVelocity": 2.7588802256510867,
+            "velocityX": 2.0209582693284847,
+            "velocityY": 0.2878073645372324,
+            "timestamp": 0.8716470506781051
+        },
+        {
+            "x": 4.2064124532392215,
+            "y": 5.553849604220975,
+            "heading": -0.10702579424844161,
+            "angularVelocity": 2.2029350905894844,
+            "velocityX": 1.3219005040628353,
+            "velocityY": 0.161609651807531,
+            "timestamp": 0.9588117557459156
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 2.9287791201697906e-29,
+            "angularVelocity": 1.2278570112200775,
+            "velocityX": 0.6492025266047246,
+            "velocityY": 0.0705606216901654,
+            "timestamp": 1.045976460813726
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 3.0874616220611523e-31,
+            "velocityY": -3.4071713040637054e-30,
+            "timestamp": 1.1331411658815367
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W3 To S3.traj b/src/main/deploy/choreo/W3 To S3.traj
new file mode 100644
index 00000000..4ff6b6c8
--- /dev/null
+++ b/src/main/deploy/choreo/W3 To S3.traj	
@@ -0,0 +1,229 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.284359298751307,
+            "y": 4.3328670624584875,
+            "heading": -0.3913603360985968,
+            "angularVelocity": -0.02219557362709146,
+            "velocityX": -0.2551974739939649,
+            "velocityY": -0.44270758788444614,
+            "timestamp": 0.061288620940906494
+        },
+        {
+            "x": 2.2541133789559487,
+            "y": 4.2780305351291235,
+            "heading": -0.3941996942145126,
+            "angularVelocity": -0.04632765548197678,
+            "velocityX": -0.4934997611468709,
+            "velocityY": -0.89472607618691,
+            "timestamp": 0.12257724188181299
+        },
+        {
+            "x": 2.2108693669713233,
+            "y": 4.194701771021148,
+            "heading": -0.39869373638946143,
+            "angularVelocity": -0.0733258818024626,
+            "velocityX": -0.7055797849705345,
+            "velocityY": -1.3596123200148318,
+            "timestamp": 0.1838658628227195
+        },
+        {
+            "x": 2.157413012714945,
+            "y": 4.081769184110331,
+            "heading": -0.40512443208979565,
+            "angularVelocity": -0.1049247903054391,
+            "velocityX": -0.8722068376757797,
+            "velocityY": -1.8426354709417345,
+            "timestamp": 0.24515448376362597
+        },
+        {
+            "x": 2.099439422754663,
+            "y": 3.9378510886177804,
+            "heading": -0.41398926531008917,
+            "angularVelocity": -0.1446407682894496,
+            "velocityX": -0.9459111507204496,
+            "velocityY": -2.348202542056109,
+            "timestamp": 0.30644310470453245
+        },
+        {
+            "x": 2.05084134036617,
+            "y": 3.764062564560248,
+            "heading": -0.42610260644735526,
+            "angularVelocity": -0.1976442111325286,
+            "velocityX": -0.7929380958881483,
+            "velocityY": -2.835575697242347,
+            "timestamp": 0.36773172564543893
+        },
+        {
+            "x": 2.0313297442556637,
+            "y": 3.578686605657539,
+            "heading": -0.44115755081157715,
+            "angularVelocity": -0.24564012263773397,
+            "velocityX": -0.3183559331400068,
+            "velocityY": -3.024639093795978,
+            "timestamp": 0.4290203465863454
+        },
+        {
+            "x": 2.0424292488902682,
+            "y": 3.3999266361315956,
+            "heading": -0.45764648760363913,
+            "angularVelocity": -0.2690374907923018,
+            "velocityX": 0.18110220892890863,
+            "velocityY": -2.9166910069375023,
+            "timestamp": 0.4903089675272519
+        },
+        {
+            "x": 2.0816987250314285,
+            "y": 3.234855796713624,
+            "heading": -0.4747853916237668,
+            "angularVelocity": -0.27964251368378285,
+            "velocityX": 0.6407302944379066,
+            "velocityY": -2.6933358408754287,
+            "timestamp": 0.5515975884681584
+        },
+        {
+            "x": 2.1473776050382467,
+            "y": 3.086622748034336,
+            "heading": -0.49218081575104855,
+            "angularVelocity": -0.2838279579508591,
+            "velocityX": 1.071632531431,
+            "velocityY": -2.418606364502998,
+            "timestamp": 0.6128862094090649
+        },
+        {
+            "x": 2.2382969856262207,
+            "y": 2.9569339752197266,
+            "heading": -0.5096053711064815,
+            "angularVelocity": -0.2843032701328595,
+            "velocityX": 1.4834626590087072,
+            "velocityY": -2.116033463041253,
+            "timestamp": 0.6741748303499714
+        },
+        {
+            "x": 2.3750291535583634,
+            "y": 2.833789120602945,
+            "heading": -0.5294729435704262,
+            "angularVelocity": -0.282105779182567,
+            "velocityX": 1.9415021560293924,
+            "velocityY": -1.7485717103605045,
+            "timestamp": 0.7446007979480589
+        },
+        {
+            "x": 2.5431654959507464,
+            "y": 2.737552434720476,
+            "heading": -0.5488979650801319,
+            "angularVelocity": -0.2758218619098283,
+            "velocityX": 2.3874196993915153,
+            "velocityY": -1.366494336743516,
+            "timestamp": 0.8150267655461465
+        },
+        {
+            "x": 2.7413669689925393,
+            "y": 2.669711436345154,
+            "heading": -0.5674506234763109,
+            "angularVelocity": -0.2634349094364851,
+            "velocityX": 2.8143237473555907,
+            "velocityY": -0.9632952260234809,
+            "timestamp": 0.8854527331442341
+        },
+        {
+            "x": 2.9672720655352354,
+            "y": 2.6325722400915463,
+            "heading": -0.5844267226661307,
+            "angularVelocity": -0.24104885979984425,
+            "velocityX": 3.207696028145596,
+            "velocityY": -0.5273508837756647,
+            "timestamp": 0.9558787007423217
+        },
+        {
+            "x": 3.21586763898004,
+            "y": 2.629998478445332,
+            "heading": -0.5985057428976699,
+            "angularVelocity": -0.19991234358165255,
+            "velocityX": 3.529885096694866,
+            "velocityY": -0.03654563414595668,
+            "timestamp": 1.0263046683404093
+        },
+        {
+            "x": 3.4730987079905056,
+            "y": 2.6678365706373897,
+            "heading": -0.6068355688677516,
+            "angularVelocity": -0.11827776392962061,
+            "velocityX": 3.652503157336117,
+            "velocityY": 0.5372747224148254,
+            "timestamp": 1.096730635938497
+        },
+        {
+            "x": 3.704150591962856,
+            "y": 2.737591563426048,
+            "heading": -0.6062385008661115,
+            "angularVelocity": 0.00847795240879764,
+            "velocityX": 3.28077684769537,
+            "velocityY": 0.9904726220694814,
+            "timestamp": 1.1671566035365846
+        },
+        {
+            "x": 3.894448488053554,
+            "y": 2.8141322684658854,
+            "heading": -0.5991077413907381,
+            "angularVelocity": 0.10125184954600595,
+            "velocityX": 2.702098424500253,
+            "velocityY": 1.086825039829707,
+            "timestamp": 1.2375825711346722
+        },
+        {
+            "x": 4.043960756386648,
+            "y": 2.883897596305393,
+            "heading": -0.5885679211534333,
+            "angularVelocity": 0.1496581530473843,
+            "velocityX": 2.122970736963697,
+            "velocityY": 0.9906193726389452,
+            "timestamp": 1.3080085387327598
+        },
+        {
+            "x": 4.154363809487208,
+            "y": 2.940218619761556,
+            "heading": -0.5783208756256007,
+            "angularVelocity": 0.14550095479427785,
+            "velocityX": 1.5676469470837475,
+            "velocityY": 0.7997195548321091,
+            "timestamp": 1.3784345063308474
+        },
+        {
+            "x": 4.227038806897989,
+            "y": 2.9796144762337837,
+            "heading": -0.5703846528919979,
+            "angularVelocity": 0.11268887037369305,
+            "velocityX": 1.0319346668480025,
+            "velocityY": 0.5593938971070241,
+            "timestamp": 1.448860473928935
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 0.06225903656760019,
+            "velocityX": 0.5106240542868632,
+            "velocityY": 0.2894603292148455,
+            "timestamp": 1.5192864415270226
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 9.24329495349137e-32,
+            "velocityX": 0,
+            "velocityY": 1.2740379664097394e-33,
+            "timestamp": 1.5897124091251102
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W3 To S4.traj b/src/main/deploy/choreo/W3 To S4.traj
new file mode 100644
index 00000000..2b0725c8
--- /dev/null
+++ b/src/main/deploy/choreo/W3 To S4.traj	
@@ -0,0 +1,256 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 9.57151893871228e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2927892107770984,
+            "y": 4.332309394440523,
+            "heading": -0.3722824674321399,
+            "angularVelocity": 0.3002660031020358,
+            "velocityX": -0.12220408518393268,
+            "velocityY": -0.4692835993371687,
+            "timestamp": 0.059006122520770025
+        },
+        {
+            "x": 2.2795380946752672,
+            "y": 4.276530177721837,
+            "heading": -0.3395678297253276,
+            "angularVelocity": 0.5544278510301563,
+            "velocityX": -0.224571883996739,
+            "velocityY": -0.9453123563415277,
+            "timestamp": 0.11801224504154005
+        },
+        {
+            "x": 2.2618545093830753,
+            "y": 4.192237257969867,
+            "heading": -0.2954402922678697,
+            "angularVelocity": 0.7478467584770477,
+            "velocityX": -0.29969068524994186,
+            "velocityY": -1.4285453127732544,
+            "timestamp": 0.17701836756231007
+        },
+        {
+            "x": 2.2420434066604593,
+            "y": 4.079048226765037,
+            "heading": -0.24482967906698166,
+            "angularVelocity": 0.8577179966887506,
+            "velocityX": -0.33574656114103973,
+            "velocityY": -1.9182590953165608,
+            "timestamp": 0.2360244900830801
+        },
+        {
+            "x": 2.2234541825747725,
+            "y": 3.936854095556344,
+            "heading": -0.19463836205748358,
+            "angularVelocity": 0.8506120189787272,
+            "velocityX": -0.3150389026010499,
+            "velocityY": -2.40981994976269,
+            "timestamp": 0.2950306126038501
+        },
+        {
+            "x": 2.211142623620853,
+            "y": 3.7664761504723194,
+            "heading": -0.15521354512815658,
+            "angularVelocity": 0.6681479013546628,
+            "velocityX": -0.2086488389333828,
+            "velocityY": -2.8874621446961175,
+            "timestamp": 0.35403673512462014
+        },
+        {
+            "x": 2.2125400151692816,
+            "y": 3.5715383450180926,
+            "heading": -0.14131017152189063,
+            "angularVelocity": 0.23562594883900537,
+            "velocityX": 0.023682144983125607,
+            "velocityY": -3.303687772156347,
+            "timestamp": 0.41304285764539017
+        },
+        {
+            "x": 2.235363094184648,
+            "y": 3.3576277458759463,
+            "heading": -0.144791073330025,
+            "angularVelocity": -0.058992214018278255,
+            "velocityX": 0.38679170974728244,
+            "velocityY": -3.6252271798888405,
+            "timestamp": 0.4720489801661602
+        },
+        {
+            "x": 2.287018648077521,
+            "y": 3.1404706845748738,
+            "heading": -0.14479216409186968,
+            "angularVelocity": -0.000018485570624385508,
+            "velocityX": 0.8754270181825738,
+            "velocityY": -3.6802462528296687,
+            "timestamp": 0.5310551026869302
+        },
+        {
+            "x": 2.3664206535422005,
+            "y": 2.9318538879811458,
+            "heading": -0.14479229465212481,
+            "angularVelocity": -0.000002212656069584398,
+            "velocityX": 1.3456570618876043,
+            "velocityY": -3.53551102295012,
+            "timestamp": 0.5900612252077002
+        },
+        {
+            "x": 2.4722259044647217,
+            "y": 2.73530650138855,
+            "heading": -0.14479252387880184,
+            "angularVelocity": -0.000003884794784189704,
+            "velocityX": 1.7931232625102267,
+            "velocityY": -3.330965977698533,
+            "timestamp": 0.6490673477284702
+        },
+        {
+            "x": 2.6222628757424284,
+            "y": 2.533912575740545,
+            "heading": -0.14479267276909355,
+            "angularVelocity": -0.0000022427572708026126,
+            "velocityX": 2.260029880862475,
+            "velocityY": -3.033627551346843,
+            "timestamp": 0.7154545103709612
+        },
+        {
+            "x": 2.800083925097921,
+            "y": 2.356570260194387,
+            "heading": -0.14479275333723068,
+            "angularVelocity": -0.0000012136101911737535,
+            "velocityX": 2.678545704884188,
+            "velocityY": -2.671334464182248,
+            "timestamp": 0.7818416730134522
+        },
+        {
+            "x": 3.001881619185789,
+            "y": 2.207076716019015,
+            "heading": -0.14479280967120484,
+            "angularVelocity": -8.485672817028763e-7,
+            "velocityX": 3.0397095772053357,
+            "velocityY": -2.2518441551784094,
+            "timestamp": 0.8482288356559432
+        },
+        {
+            "x": 3.2233351494929763,
+            "y": 2.0886327789398735,
+            "heading": -0.14479285532879432,
+            "angularVelocity": -6.877472642883405e-7,
+            "velocityX": 3.3357884490373766,
+            "velocityY": -1.7841391673415428,
+            "timestamp": 0.9146159982984342
+        },
+        {
+            "x": 3.4597028157497376,
+            "y": 2.003774307484127,
+            "heading": -0.14479289641721121,
+            "angularVelocity": -6.189211176196774e-7,
+            "velocityX": 3.5604423633775646,
+            "velocityY": -1.2782361540698368,
+            "timestamp": 0.9810031609409252
+        },
+        {
+            "x": 3.704835569204283,
+            "y": 1.949179734375487,
+            "heading": -0.14479293648657837,
+            "angularVelocity": -6.035710154638316e-7,
+            "velocityX": 3.692472214464675,
+            "velocityY": -0.8223664174750668,
+            "timestamp": 1.047390323583416
+        },
+        {
+            "x": 3.9499686718209266,
+            "y": 1.8945867290481686,
+            "heading": -0.1447929765559331,
+            "angularVelocity": -6.035708281824289e-7,
+            "velocityX": 3.692477473946853,
+            "velocityY": -0.8223428017448724,
+            "timestamp": 1.113777486225907
+        },
+        {
+            "x": 4.195101784677066,
+            "y": 1.8399937696982192,
+            "heading": -0.14479301662532418,
+            "angularVelocity": -6.035713756667977e-7,
+            "velocityX": 3.692477628185933,
+            "velocityY": -0.8223421091807183,
+            "timestamp": 1.1801646488683981
+        },
+        {
+            "x": 4.435904668374261,
+            "y": 1.7860806487644325,
+            "heading": -0.15898872227240893,
+            "angularVelocity": -0.21383208864538455,
+            "velocityX": 3.627250723064772,
+            "velocityY": -0.8121015989811171,
+            "timestamp": 1.2465518115108891
+        },
+        {
+            "x": 4.648245693564204,
+            "y": 1.7385415440023928,
+            "heading": -0.26462575063007965,
+            "angularVelocity": -1.5912267395211415,
+            "velocityX": 3.1985253886123286,
+            "velocityY": -0.7160888170209653,
+            "timestamp": 1.3129389741533801
+        },
+        {
+            "x": 4.824757883271093,
+            "y": 1.6989793937314068,
+            "heading": -0.37654836939407044,
+            "angularVelocity": -1.6859075506317058,
+            "velocityX": 2.6588301515075172,
+            "velocityY": -0.5959307296206723,
+            "timestamp": 1.3793261367958711
+        },
+        {
+            "x": 4.96555652453039,
+            "y": 1.6673723665346096,
+            "heading": -0.4762340168485147,
+            "angularVelocity": -1.501580177349536,
+            "velocityX": 2.1208715006774397,
+            "velocityY": -0.47610149219673153,
+            "timestamp": 1.4457132994383621
+        },
+        {
+            "x": 5.070906714490224,
+            "y": 1.643682366519874,
+            "heading": -0.5558849180153355,
+            "angularVelocity": -1.1997937251175768,
+            "velocityX": 1.5869060487969315,
+            "velocityY": -0.3568460990313944,
+            "timestamp": 1.5121004620808531
+        },
+        {
+            "x": 5.141006607602348,
+            "y": 1.6278935430132826,
+            "heading": -0.6113350442973207,
+            "angularVelocity": -0.8352537459779052,
+            "velocityX": 1.055925427776252,
+            "velocityY": -0.23782946699526453,
+            "timestamp": 1.5784876247233441
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -0.4317846186174008,
+            "velocityX": 0.5271108299370921,
+            "velocityY": -0.11890164753374041,
+            "timestamp": 1.6448747873658351
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 1.0662498702136072e-32,
+            "velocityX": 0,
+            "velocityY": 8.479039452208929e-33,
+            "timestamp": 1.7112619500083261
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W3 To W1.traj b/src/main/deploy/choreo/W3 To W1.traj
new file mode 100644
index 00000000..087304c1
--- /dev/null
+++ b/src/main/deploy/choreo/W3 To W1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 1.4326217036715317e-32,
+            "velocityX": -1.74824017803789e-33,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3001377838932537,
+            "y": 4.403828106793783,
+            "heading": -0.3629126711329447,
+            "angularVelocity": 0.37089781855009624,
+            "velocityX": 0.0018866291944089578,
+            "velocityY": 0.600123743495633,
+            "timestamp": 0.07303178264284323
+        },
+        {
+            "x": 2.300408122825978,
+            "y": 4.49157282256222,
+            "heading": -0.3106389398567459,
+            "angularVelocity": 0.7157668810008344,
+            "velocityX": 0.0037016614265912673,
+            "velocityY": 1.2014593180279707,
+            "timestamp": 0.14606356528568645
+        },
+        {
+            "x": 2.3008081314200264,
+            "y": 4.623360732427042,
+            "heading": -0.2362186265841639,
+            "angularVelocity": 1.019012689811082,
+            "velocityX": 0.005477185131915077,
+            "velocityY": 1.8045281806870437,
+            "timestamp": 0.2190953479285297
+        },
+        {
+            "x": 2.301347271135313,
+            "y": 4.79937800153835,
+            "heading": -0.14515320669009546,
+            "angularVelocity": 1.2469286192754925,
+            "velocityX": 0.007382261472700921,
+            "velocityY": 2.410146141058999,
+            "timestamp": 0.2921271305713729
+        },
+        {
+            "x": 2.302070167703843,
+            "y": 5.019849104537176,
+            "heading": -0.04952394454697364,
+            "angularVelocity": 1.3094197989222025,
+            "velocityX": 0.009898383174695338,
+            "velocityY": 3.018837758308937,
+            "timestamp": 0.3651589132142161
+        },
+        {
+            "x": 2.3031513688730865,
+            "y": 5.282690253403799,
+            "heading": -0.004114907781753178,
+            "angularVelocity": 0.6217708937393756,
+            "velocityX": 0.014804529344862738,
+            "velocityY": 3.598996756686451,
+            "timestamp": 0.43819069585705933
+        },
+        {
+            "x": 2.303520931398292,
+            "y": 5.5589648879428,
+            "heading": -0.0041148785146688655,
+            "angularVelocity": 4.00744487575221e-7,
+            "velocityX": 0.005060297199824665,
+            "velocityY": 3.7829370246937892,
+            "timestamp": 0.5112224784999025
+        },
+        {
+            "x": 2.3038904865521834,
+            "y": 5.835239522491655,
+            "heading": -0.004114849247579104,
+            "angularVelocity": 4.0074456218901027e-7,
+            "velocityX": 0.005060196266858216,
+            "velocityY": 3.7829370248287195,
+            "timestamp": 0.5842542611427458
+        },
+        {
+            "x": 2.302005656251481,
+            "y": 6.097682306820458,
+            "heading": 0.0412177605138735,
+            "angularVelocity": 0.6207244041015477,
+            "velocityX": -0.025808356752290516,
+            "velocityY": 3.5935420830717044,
+            "timestamp": 0.6572860437855891
+        },
+        {
+            "x": 2.3011707390613565,
+            "y": 6.318019040898657,
+            "heading": 0.13972629361592653,
+            "angularVelocity": 1.3488447020908976,
+            "velocityX": -0.01143224442716972,
+            "velocityY": 3.016997889203128,
+            "timestamp": 0.7303178264284323
+        },
+        {
+            "x": 2.3006512841511224,
+            "y": 6.493901741171369,
+            "heading": 0.23378956116370217,
+            "angularVelocity": 1.2879771538343168,
+            "velocityX": -0.007112723959845375,
+            "velocityY": 2.408303534542668,
+            "timestamp": 0.8033496090712756
+        },
+        {
+            "x": 2.300310658917655,
+            "y": 6.625570031648912,
+            "heading": 0.310734904704345,
+            "angularVelocity": 1.053587092580481,
+            "velocityX": -0.00466406845267762,
+            "velocityY": 1.802890272053981,
+            "timestamp": 0.8763813917141189
+        },
+        {
+            "x": 2.3001003670918414,
+            "y": 6.7132235530415985,
+            "heading": 0.3648714550351714,
+            "angularVelocity": 0.7412738450542471,
+            "velocityX": -0.0028794562888088465,
+            "velocityY": 1.2002106236588805,
+            "timestamp": 0.9494131743569622
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0.38515484556072405,
+            "velocityX": -0.0013742933310585549,
+            "velocityY": 0.5994163824877027,
+            "timestamp": 1.0224449569998053
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 2.8585488767662063e-32,
+            "velocityX": -1.0732467562704116e-31,
+            "velocityY": 0,
+            "timestamp": 1.0954767396426486
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W3 To W2.traj b/src/main/deploy/choreo/W3 To W2.traj
new file mode 100644
index 00000000..4345e451
--- /dev/null
+++ b/src/main/deploy/choreo/W3 To W2.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 0,
+            "velocityX": 2.0150450292109907e-34,
+            "velocityY": 3.039389955204995e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2980012520823867,
+            "y": 4.4080014432651184,
+            "heading": -0.3744767713085023,
+            "angularVelocity": 0.2040875990087458,
+            "velocityX": -0.02627801674742958,
+            "velocityY": 0.6310864511258738,
+            "timestamp": 0.07606159691668693
+        },
+        {
+            "x": 2.294003780157748,
+            "y": 4.504004078538053,
+            "heading": -0.34341554427294535,
+            "angularVelocity": 0.40836937817095076,
+            "velocityX": -0.05255571913664827,
+            "velocityY": 1.262169598911913,
+            "timestamp": 0.15212319383337386
+        },
+        {
+            "x": 2.2880075055108158,
+            "y": 4.648007445856594,
+            "heading": -0.296791131111893,
+            "angularVelocity": 0.6129823071177671,
+            "velocityX": -0.07883445641432706,
+            "velocityY": 1.8932466994648223,
+            "timestamp": 0.2281847907500608
+        },
+        {
+            "x": 2.2800121106149036,
+            "y": 4.840010896492559,
+            "heading": -0.2345694561462187,
+            "angularVelocity": 0.8180432371651087,
+            "velocityX": -0.10511736829125365,
+            "velocityY": 2.5243152710332057,
+            "timestamp": 0.3042463876667477
+        },
+        {
+            "x": 2.270016860554186,
+            "y": 5.080013636963748,
+            "heading": -0.15671112019223,
+            "angularVelocity": 1.023622157700289,
+            "velocityX": -0.13140994228224737,
+            "velocityY": 3.1553734105013973,
+            "timestamp": 0.3803079845834346
+        },
+        {
+            "x": 2.2620106394348882,
+            "y": 5.2720083260038075,
+            "heading": -0.09403333701953483,
+            "angularVelocity": 0.82403980081234,
+            "velocityX": -0.10525970323851294,
+            "velocityY": 2.524200080237073,
+            "timestamp": 0.4563695815001215
+        },
+        {
+            "x": 2.256005452365101,
+            "y": 5.416004188429854,
+            "heading": -0.0470178538159679,
+            "angularVelocity": 0.6181237984664553,
+            "velocityX": -0.07895163016843816,
+            "velocityY": 1.8931480308488688,
+            "timestamp": 0.5324311784168084
+        },
+        {
+            "x": 2.2520018201736485,
+            "y": 5.5120013950535185,
+            "heading": -0.01567260522346991,
+            "angularVelocity": 0.4121034774859067,
+            "velocityX": -0.05263670963729669,
+            "velocityY": 1.262098227162051,
+            "timestamp": 0.6084927753334953
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 1.3656663814986182e-35,
+            "angularVelocity": 0.20605148798856665,
+            "velocityX": -0.026318408432062,
+            "velocityY": 0.6310491350721452,
+            "timestamp": 0.6845543722501822
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 6.6551444751621536e-34,
+            "velocityX": 3.9479513180483775e-34,
+            "velocityY": 0,
+            "timestamp": 0.7606159691668691
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 3d93b182..efc9a9d9 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -140,18 +140,27 @@ public void robotInit() {
     // Test binding for elevator
     controller.b().whileTrue(elevator.setExtensionCmd(() -> 1.0));
 
+    controller.x().toggleOnTrue(swerve.runChoreoTraj(DynamicAuto.makeNoteToNote(swerve::getPose)));
     controller
-        .x()
+        .leftBumper()
         .toggleOnTrue(
-            Commands.run(
-                () -> swerve.runChoreoTraj(DynamicAuto.makeNoteToNote(swerve::getPose)), swerve));
+            Commands.runOnce(
+                () -> DynamicAuto.getAbsoluteClosestNote(swerve::getPose).blacklist(), swerve));
+    controller
+        .rightBumper()
+        .toggleOnTrue(
+            Commands.runOnce(
+                () -> DynamicAuto.getAbsoluteClosestNote(swerve::getPose).whitelist(), swerve));
   }
 
   @Override
   public void robotPeriodic() {
     CommandScheduler.getInstance().run();
     Logger.recordOutput(
-        "DynamicAuto/Closest Note", DynamicAuto.getClosestNote(swerve.getPose()).getPose());
+        "DynamicAuto/Closest Note", DynamicAuto.getClosestNote(swerve::getPose).getPose());
+
+    Logger.recordOutput(
+        "DynamicAuto/Absolute Closest Note", DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getPose());
     Logger.recordOutput(
         "DynamicAuto/Closest Shooting Location",
         DynamicAuto.closestShootingLocation(() -> swerve.getPose()).getPose());
diff --git a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
index 77d12419..a12c982e 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
@@ -9,21 +9,21 @@
 public class DynamicAuto {
 
   public static final Note[] notes = {
-    new Note(new Pose2d(2.204, 7.0, Rotation2d.fromRadians(Math.PI)), false, 0, "W1"), // w1
-    new Note(new Pose2d(2.204, 5.5, Rotation2d.fromRadians(Math.PI)), false, 1, "W2"), // w2
-    new Note(new Pose2d(2.204, 4.1, Rotation2d.fromRadians(Math.PI)), false, 2, "W3"), // w3
-    new Note(new Pose2d(7.538, 7.3, Rotation2d.fromRadians(Math.PI)), false, 0, "C1"), // c1
-    new Note(new Pose2d(7.538, 5.7, Rotation2d.fromRadians(Math.PI)), false, 0, "C2"), // c2
-    new Note(new Pose2d(7.538, 4.1, Rotation2d.fromRadians(Math.PI)), false, 1, "C3"), // c3
-    new Note(new Pose2d(7.538, 2.5, Rotation2d.fromRadians(Math.PI)), false, 2, "C4"), // c4
-    new Note(new Pose2d(7.538, 0.7, Rotation2d.fromRadians(Math.PI)), false, 3, "C5") // c5
+      new Note(new Pose2d(2.204, 7.0, Rotation2d.fromRadians(Math.PI)), false, 0, "W1"), // w1
+      new Note(new Pose2d(2.204, 5.5, Rotation2d.fromRadians(Math.PI)), false, 1, "W2"), // w2
+      new Note(new Pose2d(2.204, 4.1, Rotation2d.fromRadians(Math.PI)), false, 2, "W3"), // w3
+      new Note(new Pose2d(7.538, 7.3, Rotation2d.fromRadians(Math.PI)), false, 0, "C1"), // c1
+      new Note(new Pose2d(7.538, 5.7, Rotation2d.fromRadians(Math.PI)), false, 0, "C2"), // c2
+      new Note(new Pose2d(7.538, 4.1, Rotation2d.fromRadians(Math.PI)), false, 1, "C3"), // c3
+      new Note(new Pose2d(7.538, 2.5, Rotation2d.fromRadians(Math.PI)), false, 2, "C4"), // c4
+      new Note(new Pose2d(7.538, 0.7, Rotation2d.fromRadians(Math.PI)), false, 3, "C5") // c5
   };
 
   public static final ShootingLocation[] shootingLocations = {
-    new ShootingLocation(new Pose2d(5.639, 6.463, Rotation2d.fromRadians(-3.04)), "S1"), // Left
-    new ShootingLocation(new Pose2d(4.216, 5.216, Rotation2d.fromRadians(-Math.PI)), "S2"),
-    new ShootingLocation(new Pose2d(4.263, 3.000, Rotation2d.fromRadians(-0.566)), "S3"), // Middle
-    new ShootingLocation(new Pose2d(5.176, 1.620, Rotation2d.fromRadians(2.961)), "S4") // Right
+      new ShootingLocation(new Pose2d(5.639, 6.463, Rotation2d.fromRadians(-3.04)), "S1"), // Left
+      new ShootingLocation(new Pose2d(4.216, 5.216, Rotation2d.fromRadians(-Math.PI)), "S2"),
+      new ShootingLocation(new Pose2d(4.263, 3.000, Rotation2d.fromRadians(-0.566)), "S3"), // Middle
+      new ShootingLocation(new Pose2d(5.176, 1.620, Rotation2d.fromRadians(2.961)), "S4") // Right
   };
 
   public static ShootingLocation closestShootingLocation(Supplier<Pose2d> curPose) {
@@ -40,15 +40,28 @@ public static ShootingLocation closestShootingLocation(Supplier<Pose2d> curPose)
   }
 
   public static ChoreoTrajectory makeNoteToNote(Supplier<Pose2d> startingPose) {
-    Note closestNote = getClosestNote(startingPose.get());
+    Note closestNote = getClosestNote(startingPose);
     closestNote.blacklist();
-    Note nextClosest = getClosestNote(startingPose.get());
+    Note nextClosest = getClosestNote(startingPose);
     closestNote.whitelist();
-    
+    System.out.println(closestNote.getName() + " To " + nextClosest.getName());
     return Choreo.getTrajectory(closestNote.getName() + " To " + nextClosest.getName());
   }
 
-  public static Note getClosestNote(Pose2d curPose) {
+  public static Note getAbsoluteClosestNote(Supplier<Pose2d> curPose) {
+    Note closestNote = new Note();
+    double shortestDistance = 99999999;
+    for (Note note : notes) {
+      if (shortestDistance > curPose.get().minus(note.getPose()).getTranslation().getNorm()) {
+        shortestDistance = curPose.get().minus(note.getPose()).getTranslation().getNorm();
+        closestNote = note;
+      }
+    }
+
+    return closestNote;
+  }
+
+  public static Note getClosestNote(Supplier<Pose2d> curPose) {
     Note closestNote = new Note();
     double shortestDistance = 99999999;
     for (Note note : notes) {
@@ -56,15 +69,15 @@ public static Note getClosestNote(Pose2d curPose) {
 
         continue;
       } else if (note.getPriority() > closestNote.getPriority()) {
-        shortestDistance = curPose.minus(note.getPose()).getTranslation().getNorm();
+        shortestDistance = curPose.get().minus(note.getPose()).getTranslation().getNorm();
         closestNote = note;
-        if (shortestDistance > curPose.minus(note.getPose()).getTranslation().getNorm()) {
-          shortestDistance = curPose.minus(note.getPose()).getTranslation().getNorm();
+        if (shortestDistance > curPose.get().minus(note.getPose()).getTranslation().getNorm()) {
+          shortestDistance = curPose.get().minus(note.getPose()).getTranslation().getNorm();
           closestNote = note;
         }
       } else if (note.getPriority() == closestNote.getPriority()) {
-        if (shortestDistance > curPose.minus(note.getPose()).getTranslation().getNorm()) {
-          shortestDistance = curPose.minus(note.getPose()).getTranslation().getNorm();
+        if (shortestDistance > curPose.get().minus(note.getPose()).getTranslation().getNorm()) {
+          shortestDistance = curPose.get().minus(note.getPose()).getTranslation().getNorm();
           closestNote = note;
         }
       }
diff --git a/src/main/java/frc/robot/utils/dynamicauto/Note.java b/src/main/java/frc/robot/utils/dynamicauto/Note.java
index faed9968..89829469 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/Note.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/Note.java
@@ -28,11 +28,11 @@ public Note() {
     this.name = "Uninitialized";
   }
 
-  public void blacklist(){
+  public void blacklist() {
     this.blacklisted = true;
   }
 
-  public void whitelist(){
+  public void whitelist() {
     this.blacklisted = false;
   }
 

From f664e4f01a085f599f14d5eaa1e37c735019360d Mon Sep 17 00:00:00 2001
From: Stoopipoopy <anotherguywithabeanie@gmail.com>
Date: Tue, 20 Feb 2024 13:41:16 -0800
Subject: [PATCH 48/62] added more path creators and added c4/5 to s1/2

---
 src/main/deploy/choreo/C4 To S1.traj          |  256 ++++
 src/main/deploy/choreo/C4 To S2.traj          |  211 +++
 src/main/deploy/choreo/C5 To S1.traj          |  292 ++++
 src/main/deploy/choreo/C5 To S2.traj          |  247 ++++
 src/main/java/frc/robot/Robot.java            |   22 +-
 .../subsystems/swerve/SwerveSubsystem.java    |   33 +-
 .../robot/utils/dynamicauto/ChoreoBuilder.py  |    4 +-
 .../robot/utils/dynamicauto/DynamicAuto.java  |   59 +-
 .../frc/robot/utils/dynamicauto/Note.java     |    2 +
 .../frc/robot/utils/dynamicauto/data.chor     | 1258 +++++++++++++++++
 10 files changed, 2351 insertions(+), 33 deletions(-)
 create mode 100644 src/main/deploy/choreo/C4 To S1.traj
 create mode 100644 src/main/deploy/choreo/C4 To S2.traj
 create mode 100644 src/main/deploy/choreo/C5 To S1.traj
 create mode 100644 src/main/deploy/choreo/C5 To S2.traj

diff --git a/src/main/deploy/choreo/C4 To S1.traj b/src/main/deploy/choreo/C4 To S1.traj
new file mode 100644
index 00000000..d3b44a34
--- /dev/null
+++ b/src/main/deploy/choreo/C4 To S1.traj	
@@ -0,0 +1,256 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": -3.908498355076501e-28,
+            "velocityX": -5.285170566576542e-31,
+            "velocityY": -1.5421992925545815e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 7.644672083885401,
+            "y": 2.5062543236054426,
+            "heading": 0.0003371503707738104,
+            "angularVelocity": 0.003954317681676867,
+            "velocityX": -0.4143486570570975,
+            "velocityY": 0.5776865743787781,
+            "timestamp": 0.08526132645270777
+        },
+        {
+            "x": 7.574016248997627,
+            "y": 2.604762966378743,
+            "heading": 0.0010114541718947692,
+            "angularVelocity": 0.00790867124800936,
+            "velocityX": -0.8286973452956875,
+            "velocityY": 1.1553730967106797,
+            "timestamp": 0.17052265290541554
+        },
+        {
+            "x": 7.4680324930779305,
+            "y": 2.7525259224839496,
+            "heading": 0.0020229127878395882,
+            "angularVelocity": 0.01186304105238048,
+            "velocityX": -1.243046060026343,
+            "velocityY": 1.73305955059478,
+            "timestamp": 0.2557839793581233
+        },
+        {
+            "x": 7.326720815099078,
+            "y": 2.949543182754876,
+            "heading": 0.0033715247016779517,
+            "angularVelocity": 0.015817393065211405,
+            "velocityX": -1.657394786805044,
+            "velocityY": 2.310745896971908,
+            "timestamp": 0.3410453058108311
+        },
+        {
+            "x": 7.1500812197312955,
+            "y": 3.1958147252550657,
+            "heading": 0.005057281636821727,
+            "angularVelocity": 0.01977164800486184,
+            "velocityX": -2.0717434588083226,
+            "velocityY": 2.8884319860640817,
+            "timestamp": 0.4263066322635389
+        },
+        {
+            "x": 6.957409028465344,
+            "y": 3.454481547786369,
+            "heading": 0.005057281852786698,
+            "angularVelocity": 2.532976909732762e-9,
+            "velocityX": -2.2597841164587846,
+            "velocityY": 3.0338118499096547,
+            "timestamp": 0.5115679587162466
+        },
+        {
+            "x": 6.719745009541024,
+            "y": 3.6725340903222867,
+            "heading": 0.005057282088140234,
+            "angularVelocity": 2.760378532567681e-9,
+            "velocityX": -2.787477380570894,
+            "velocityY": 2.5574612970252826,
+            "timestamp": 0.5968292851689544
+        },
+        {
+            "x": 6.445480997294438,
+            "y": 3.842270194716782,
+            "heading": 0.005057282389511224,
+            "angularVelocity": 3.534673960970524e-9,
+            "velocityX": -3.2167457821427474,
+            "velocityY": 1.9907748501725318,
+            "timestamp": 0.6820906116216622
+        },
+        {
+            "x": 6.1443030532206215,
+            "y": 3.9576952313720115,
+            "heading": 0.0050573451663417065,
+            "angularVelocity": 7.362872839766494e-7,
+            "velocityX": -3.5324097877003195,
+            "velocityY": 1.3537795089212774,
+            "timestamp": 0.76735193807437
+        },
+        {
+            "x": 5.8268478532983945,
+            "y": 4.0147328665402,
+            "heading": 0.005057406885810731,
+            "angularVelocity": 7.238858646916699e-7,
+            "velocityX": -3.723319975538169,
+            "velocityY": 0.6689742881224171,
+            "timestamp": 0.8526132645270778
+        },
+        {
+            "x": 5.505959202112108,
+            "y": 4.047314610372366,
+            "heading": 0.005057466820580134,
+            "angularVelocity": 7.029537528496964e-7,
+            "velocityX": -3.7635897133768776,
+            "velocityY": 0.3821397717793548,
+            "timestamp": 0.9378745909797855
+        },
+        {
+            "x": 5.196832806272751,
+            "y": 4.139357882135735,
+            "heading": 0.005057539347948128,
+            "angularVelocity": 8.506478964233103e-7,
+            "velocityX": -3.625634372587421,
+            "velocityY": 1.0795430424660248,
+            "timestamp": 1.0231359174324932
+        },
+        {
+            "x": 4.911616541791353,
+            "y": 4.287091740854739,
+            "heading": 0.008468262161445878,
+            "angularVelocity": 0.040003163864724633,
+            "velocityX": -3.3452008823671444,
+            "velocityY": 1.7327182776242007,
+            "timestamp": 1.108397243885201
+        },
+        {
+            "x": 4.672649683479067,
+            "y": 4.471709481894194,
+            "heading": 0.05890642847923189,
+            "angularVelocity": 0.5915714476464851,
+            "velocityX": -2.8027579238375626,
+            "velocityY": 2.165316313039838,
+            "timestamp": 1.1936585703379088
+        },
+        {
+            "x": 4.485112052095124,
+            "y": 4.685980588264911,
+            "heading": 0.1533943172973333,
+            "angularVelocity": 1.1082150929337078,
+            "velocityX": -2.1995626761432403,
+            "velocityY": 2.513110167122247,
+            "timestamp": 1.2789198967906166
+        },
+        {
+            "x": 4.353217601776123,
+            "y": 4.923890113830566,
+            "heading": 0.2635187752392858,
+            "angularVelocity": 1.291610892363634,
+            "velocityX": -1.5469434479432043,
+            "velocityY": 2.7903568413003317,
+            "timestamp": 1.3641812232433244
+        },
+        {
+            "x": 4.2800604900800625,
+            "y": 5.16873168779887,
+            "heading": 0.3704891219101591,
+            "angularVelocity": 1.3052114311350342,
+            "velocityX": -0.8926352155228909,
+            "velocityY": 2.987463639297507,
+            "timestamp": 1.4461375592962549
+        },
+        {
+            "x": 4.26199164107549,
+            "y": 5.42343977988949,
+            "heading": 0.46980573799025893,
+            "angularVelocity": 1.2118235253560785,
+            "velocityX": -0.22046921415441664,
+            "velocityY": 3.107851135829132,
+            "timestamp": 1.5280938953491854
+        },
+        {
+            "x": 4.2996600311730235,
+            "y": 5.679753559001703,
+            "heading": 0.550426603173402,
+            "angularVelocity": 0.9837050930723119,
+            "velocityX": 0.45961535021819666,
+            "velocityY": 3.127443117352554,
+            "timestamp": 1.610050231402116
+        },
+        {
+            "x": 4.391931306983731,
+            "y": 5.927283560440951,
+            "heading": 0.5996302346047028,
+            "angularVelocity": 0.6003639718722656,
+            "velocityX": 1.1258589665496648,
+            "velocityY": 3.0202668074301977,
+            "timestamp": 1.6920065674550464
+        },
+        {
+            "x": 4.533405002431738,
+            "y": 6.1528417215915425,
+            "heading": 0.5943564820218653,
+            "angularVelocity": -0.06434832053139185,
+            "velocityX": 1.7262081525536632,
+            "velocityY": 2.752174780055045,
+            "timestamp": 1.773962903507977
+        },
+        {
+            "x": 4.71098037645994,
+            "y": 6.338671203945781,
+            "heading": 0.5303639205613101,
+            "angularVelocity": -0.7808128638054942,
+            "velocityX": 2.166707085507742,
+            "velocityY": 2.2674205717817246,
+            "timestamp": 1.8559192395609074
+        },
+        {
+            "x": 4.8864330916758485,
+            "y": 6.469038507428809,
+            "heading": 0.43960837441640055,
+            "angularVelocity": -1.107364610427659,
+            "velocityX": 2.1408072110807552,
+            "velocityY": 1.5906921875921145,
+            "timestamp": 1.937875575613838
+        },
+        {
+            "x": 5.028345407892232,
+            "y": 6.554592883552385,
+            "heading": 0.3555321434700494,
+            "angularVelocity": -1.0258661501483803,
+            "velocityX": 1.7315600361235026,
+            "velocityY": 1.0439019146530064,
+            "timestamp": 2.0198319116667687
+        },
+        {
+            "x": 5.126173645194142,
+            "y": 6.6061448414321,
+            "heading": 0.29324170563915075,
+            "angularVelocity": -0.760044199524953,
+            "velocityX": 1.1936628943333356,
+            "velocityY": 0.6290173568329146,
+            "timestamp": 2.1017882477196994
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": -0.405602632332653,
+            "velocityX": 0.6079622053097948,
+            "velocityY": 0.29107155976928517,
+            "timestamp": 2.18374458377263
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": 0,
+            "velocityX": 1.6766347007166246e-32,
+            "velocityY": 1.3162553729765107e-31,
+            "timestamp": 2.265700919825561
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C4 To S2.traj b/src/main/deploy/choreo/C4 To S2.traj
new file mode 100644
index 00000000..3bd7e899
--- /dev/null
+++ b/src/main/deploy/choreo/C4 To S2.traj	
@@ -0,0 +1,211 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": -1.8537798939082814e-32,
+            "velocityX": 2.7477856809061307e-34,
+            "velocityY": 6.925951302123011e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6474171775045905,
+            "y": 2.4907449431726314,
+            "heading": -0.04287864331544942,
+            "angularVelocity": -0.5625230567182208,
+            "velocityX": -0.42745263118016213,
+            "velocityY": 0.4426984418000905,
+            "timestamp": 0.07622557476240162
+        },
+        {
+            "x": 7.582037622461459,
+            "y": 2.5583409566544844,
+            "heading": -0.12553871024043178,
+            "angularVelocity": -1.084413822823079,
+            "velocityX": -0.8577115390329612,
+            "velocityY": 0.8867891608892778,
+            "timestamp": 0.15245114952480324
+        },
+        {
+            "x": 7.483606625879984,
+            "y": 2.659997722482693,
+            "heading": -0.2429013405408753,
+            "angularVelocity": -1.5396752424139517,
+            "velocityX": -1.2913119630555505,
+            "velocityY": 1.333630689503849,
+            "timestamp": 0.22867672428720487
+        },
+        {
+            "x": 7.3517522665520065,
+            "y": 2.7960803575645476,
+            "heading": -0.3842395530561246,
+            "angularVelocity": -1.8542098627108627,
+            "velocityX": -1.7297916052318956,
+            "velocityY": 1.785262170944986,
+            "timestamp": 0.3049022990496065
+        },
+        {
+            "x": 7.186045392769314,
+            "y": 2.9668045775727996,
+            "heading": -0.5218107815453775,
+            "angularVelocity": -1.8047909631126855,
+            "velocityX": -2.173901270002986,
+            "velocityY": 2.2397236169147496,
+            "timestamp": 0.3811278738120081
+        },
+        {
+            "x": 6.990773522032128,
+            "y": 3.1645972581511623,
+            "heading": -0.5514162056749212,
+            "angularVelocity": -0.38839227151549954,
+            "velocityX": -2.5617631791673237,
+            "velocityY": 2.59483357383517,
+            "timestamp": 0.4573534485744097
+        },
+        {
+            "x": 6.791088913724043,
+            "y": 3.3726253897030523,
+            "heading": -0.5514162553808045,
+            "angularVelocity": -6.520893213469087e-7,
+            "velocityX": -2.619653691435051,
+            "velocityY": 2.729112009982516,
+            "timestamp": 0.5335790233368113
+        },
+        {
+            "x": 6.578307530314685,
+            "y": 3.567237130643777,
+            "heading": -0.5514163004289083,
+            "angularVelocity": -5.90984115130558e-7,
+            "velocityX": -2.7914697143656473,
+            "velocityY": 2.55310296507882,
+            "timestamp": 0.609804598099213
+        },
+        {
+            "x": 6.335948512349157,
+            "y": 3.7234797461898013,
+            "heading": -0.551416294920752,
+            "angularVelocity": 7.226126339733625e-8,
+            "velocityX": -3.179497415676709,
+            "velocityY": 2.0497400778287207,
+            "timestamp": 0.6860301728616146
+        },
+        {
+            "x": 6.088743524617576,
+            "y": 3.871936280628758,
+            "heading": -0.5514162894475348,
+            "angularVelocity": 7.180289728327137e-8,
+            "velocityX": -3.2430714822699573,
+            "velocityY": 1.9475948184280927,
+            "timestamp": 0.7622557476240163
+        },
+        {
+            "x": 5.841537785943911,
+            "y": 4.020391564616265,
+            "heading": -0.5514162839746023,
+            "angularVelocity": 7.179916458555982e-8,
+            "velocityX": -3.2430813338464923,
+            "velocityY": 1.947578413809903,
+            "timestamp": 0.8384813223864179
+        },
+        {
+            "x": 5.581140041351318,
+            "y": 4.144257068634033,
+            "heading": -0.5514162991479159,
+            "angularVelocity": -1.9905804152585698e-7,
+            "velocityX": -3.416146685731968,
+            "velocityY": 1.6249861598795787,
+            "timestamp": 0.9147068971488196
+        },
+        {
+            "x": 5.261335999197585,
+            "y": 4.231658536911035,
+            "heading": -0.5514164840698423,
+            "angularVelocity": -0.000002110046686592666,
+            "velocityX": -3.6491154543506057,
+            "velocityY": 0.9972921119903242,
+            "timestamp": 1.0023456814389826
+        },
+        {
+            "x": 4.96430123021485,
+            "y": 4.378912897735688,
+            "heading": -0.551417377431188,
+            "angularVelocity": -0.000010193675698949444,
+            "velocityX": -3.3893072728996803,
+            "velocityY": 1.6802419387415284,
+            "timestamp": 1.0899844657291458
+        },
+        {
+            "x": 4.714895393240272,
+            "y": 4.564810737884653,
+            "heading": -0.6189140340648516,
+            "angularVelocity": -0.7701687920520328,
+            "velocityX": -2.8458386203626582,
+            "velocityY": 2.121182324180531,
+            "timestamp": 1.177623250019309
+        },
+        {
+            "x": 4.521287798442847,
+            "y": 4.7820097530438215,
+            "heading": -0.6965700023585185,
+            "angularVelocity": -0.8860913455457793,
+            "velocityX": -2.209154273026079,
+            "velocityY": 2.4783435429688825,
+            "timestamp": 1.265262034309472
+        },
+        {
+            "x": 4.386338131009024,
+            "y": 5.001818806087968,
+            "heading": -0.6848521841568085,
+            "angularVelocity": 0.1337058506301677,
+            "velocityX": -1.539839564490294,
+            "velocityY": 2.5081253103235808,
+            "timestamp": 1.3529008185996352
+        },
+        {
+            "x": 4.301033328473477,
+            "y": 5.211510806429668,
+            "heading": -0.519384386353178,
+            "angularVelocity": 1.888065873389841,
+            "velocityX": -0.9733681637243018,
+            "velocityY": 2.3926849515327757,
+            "timestamp": 1.4405396028897983
+        },
+        {
+            "x": 4.267176527868507,
+            "y": 5.385808771912563,
+            "heading": -0.30251041793580363,
+            "angularVelocity": 2.474634605830756,
+            "velocityX": -0.38632211616348305,
+            "velocityY": 1.988822265103685,
+            "timestamp": 1.5281783871799615
+        },
+        {
+            "x": 4.261077190237836,
+            "y": 5.502861748658668,
+            "heading": -0.11181365176971161,
+            "angularVelocity": 2.175940340919318,
+            "velocityX": -0.06959632861263781,
+            "velocityY": 1.335629854911658,
+            "timestamp": 1.6158171714701246
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 9.015876776981611e-33,
+            "angularVelocity": 1.275846677647969,
+            "velocityX": 0.021940169272516404,
+            "velocityY": 0.6519744860010044,
+            "timestamp": 1.7034559557602877
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": -9.923717904588528e-32,
+            "velocityX": -3.152679648787722e-32,
+            "velocityY": -1.9738855170003778e-32,
+            "timestamp": 1.7910947400504509
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C5 To S1.traj b/src/main/deploy/choreo/C5 To S1.traj
new file mode 100644
index 00000000..73f0ef42
--- /dev/null
+++ b/src/main/deploy/choreo/C5 To S1.traj	
@@ -0,0 +1,292 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 4.273608378959021e-31,
+            "velocityX": -7.420825794722196e-33,
+            "velocityY": 7.583788429881001e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 7.678491547493202,
+            "y": 0.8436471873828707,
+            "heading": -0.006289831251442517,
+            "angularVelocity": -0.0762785379812196,
+            "velocityX": -0.018293424295956956,
+            "velocityY": 0.686976241742982,
+            "timestamp": 0.08245872846948342
+        },
+        {
+            "x": 7.67547464227306,
+            "y": 0.9569415605229419,
+            "heading": -0.018869443032064528,
+            "angularVelocity": -0.1525564608394078,
+            "velocityX": -0.03658685109677269,
+            "velocityY": 1.3739524637710059,
+            "timestamp": 0.16491745693896684
+        },
+        {
+            "x": 7.670949285584863,
+            "y": 1.1268831170081794,
+            "heading": -0.03773871517098893,
+            "angularVelocity": -0.22883292635185057,
+            "velocityX": -0.05488026279561822,
+            "velocityY": 2.060928656547622,
+            "timestamp": 0.24737618540845024
+        },
+        {
+            "x": 7.664915480002808,
+            "y": 1.3534718520373763,
+            "heading": -0.06289740953278049,
+            "angularVelocity": -0.3051065039294483,
+            "velocityX": -0.07317364327643945,
+            "velocityY": 2.7479047910986583,
+            "timestamp": 0.3298349138779337
+        },
+        {
+            "x": 7.657373229393316,
+            "y": 1.6367077493690674,
+            "heading": -0.09434508673453766,
+            "angularVelocity": -0.38137475298804396,
+            "velocityX": -0.09146697686811679,
+            "velocityY": 3.4348807286849197,
+            "timestamp": 0.4122936423474171
+        },
+        {
+            "x": 7.649069464629009,
+            "y": 1.9485336685806538,
+            "heading": -0.09434509076634809,
+            "angularVelocity": -4.889488962341613e-8,
+            "velocityX": -0.10070207142933957,
+            "velocityY": 3.781599898511507,
+            "timestamp": 0.49475237081690054
+        },
+        {
+            "x": 7.6407656424773585,
+            "y": 2.260359586264075,
+            "heading": -0.09434509479812381,
+            "angularVelocity": -4.889446872332144e-8,
+            "velocityX": -0.1007027673816724,
+            "velocityY": 3.781599879979021,
+            "timestamp": 0.5772110992863839
+        },
+        {
+            "x": 7.63246175736497,
+            "y": 2.5721855022708704,
+            "heading": -0.09434509882989094,
+            "angularVelocity": -4.889436444733497e-8,
+            "velocityX": -0.10070353092409444,
+            "velocityY": 3.7815998596461093,
+            "timestamp": 0.6596698277558674
+        },
+        {
+            "x": 7.6241578045918486,
+            "y": 2.8840114164758646,
+            "heading": -0.09434510286165439,
+            "angularVelocity": -4.8894319896339695e-8,
+            "velocityX": -0.10070435146468282,
+            "velocityY": 3.781599837795173,
+            "timestamp": 0.7421285562253508
+        },
+        {
+            "x": 7.615853780314312,
+            "y": 3.195837328776685,
+            "heading": -0.09434510689341911,
+            "angularVelocity": -4.889433549543518e-8,
+            "velocityX": -0.10070521861867329,
+            "velocityY": 3.78159981470272,
+            "timestamp": 0.8245872846948342
+        },
+        {
+            "x": 7.607549681647345,
+            "y": 3.5076632390964844,
+            "heading": -0.09434511092518724,
+            "angularVelocity": -4.889437661104965e-8,
+            "velocityX": -0.10070612076003535,
+            "velocityY": 3.7815997906783263,
+            "timestamp": 0.9070460131643177
+        },
+        {
+            "x": 7.599245507241111,
+            "y": 3.819489147399297,
+            "heading": -0.0943451149569584,
+            "angularVelocity": -4.889441348359375e-8,
+            "velocityX": -0.10070703927126574,
+            "velocityY": 3.7815997662177643,
+            "timestamp": 0.9895047416338011
+        },
+        {
+            "x": 7.590941256893013,
+            "y": 4.131315053679708,
+            "heading": -0.09434511898873191,
+            "angularVelocity": -4.88944418703562e-8,
+            "velocityX": -0.10070796023943433,
+            "velocityY": 3.781599741691539,
+            "timestamp": 1.0719634701032845
+        },
+        {
+            "x": 7.582636930618974,
+            "y": 4.443140957938123,
+            "heading": -0.09434512302050634,
+            "angularVelocity": -4.889445311998471e-8,
+            "velocityX": -0.10070888101448454,
+            "velocityY": 3.781599717170225,
+            "timestamp": 1.1544221985727678
+        },
+        {
+            "x": 7.574332505518219,
+            "y": 4.754966859564639,
+            "heading": -0.09434512705227821,
+            "angularVelocity": -4.889442208864286e-8,
+            "velocityX": -0.10071007951363715,
+            "velocityY": 3.78159968525245,
+            "timestamp": 1.2368809270422512
+        },
+        {
+            "x": 7.562081052721255,
+            "y": 5.066662637971005,
+            "heading": -0.09434513109398723,
+            "angularVelocity": -4.901493268640619e-8,
+            "velocityX": -0.1485767853126217,
+            "velocityY": 3.7800216446669976,
+            "timestamp": 1.3193396555117345
+        },
+        {
+            "x": 7.493618412578439,
+            "y": 5.370993415792126,
+            "heading": -0.09434513545729191,
+            "angularVelocity": -5.29150129274595e-8,
+            "velocityX": -0.8302655329951328,
+            "velocityY": 3.6907042282825056,
+            "timestamp": 1.4017983839812178
+        },
+        {
+            "x": 7.37120325739307,
+            "y": 5.657906089273735,
+            "heading": -0.09434514092208332,
+            "angularVelocity": -6.627304961707395e-8,
+            "velocityX": -1.4845627316539582,
+            "velocityY": 3.479470018601967,
+            "timestamp": 1.4842571124507011
+        },
+        {
+            "x": 7.198879241943359,
+            "y": 5.9179229736328125,
+            "heading": -0.09434514894422652,
+            "angularVelocity": -9.728676819194244e-8,
+            "velocityX": -2.089821400938593,
+            "velocityY": 3.153297281988846,
+            "timestamp": 1.5667158409201845
+        },
+        {
+            "x": 7.016660952404083,
+            "y": 6.116497130944734,
+            "heading": -0.09434515627038953,
+            "angularVelocity": -1.028329950777357e-7,
+            "velocityX": -2.5576898068579945,
+            "velocityY": 2.7872673997010713,
+            "timestamp": 1.6379591516026175
+        },
+        {
+            "x": 6.805603401067166,
+            "y": 6.284097595272953,
+            "heading": -0.09434516139165254,
+            "angularVelocity": -7.188412437441302e-8,
+            "velocityX": -2.9624893806199624,
+            "velocityY": 2.352508084236811,
+            "timestamp": 1.7092024622850506
+        },
+        {
+            "x": 6.57091097390028,
+            "y": 6.416591642615984,
+            "heading": -0.09434516555011045,
+            "angularVelocity": -5.83698017255904e-8,
+            "velocityX": -3.2942380824078485,
+            "velocityY": 1.8597401787463752,
+            "timestamp": 1.7804457729674836
+        },
+        {
+            "x": 6.318370883291981,
+            "y": 6.510712371224652,
+            "heading": -0.09434516933867851,
+            "angularVelocity": -5.317787750894471e-8,
+            "velocityX": -3.544755124224867,
+            "velocityY": 1.3211167154795889,
+            "timestamp": 1.8516890836499167
+        },
+        {
+            "x": 6.054210380502568,
+            "y": 6.5641393107028225,
+            "heading": -0.09434515209433943,
+            "angularVelocity": 2.420485363909629e-7,
+            "velocityX": -3.7078639420184567,
+            "velocityY": 0.7499221887135695,
+            "timestamp": 1.9229323943323497
+        },
+        {
+            "x": 5.803536575930751,
+            "y": 6.585943895129495,
+            "heading": -0.03682720962895,
+            "angularVelocity": 0.8073451656643992,
+            "velocityX": -3.5185591765828246,
+            "velocityY": 0.30605798941413415,
+            "timestamp": 1.9941757050147828
+        },
+        {
+            "x": 5.594045383796189,
+            "y": 6.601646508747958,
+            "heading": 0.04817170409455108,
+            "angularVelocity": 1.1930792225867084,
+            "velocityX": -2.9405033276509123,
+            "velocityY": 0.2204082526211729,
+            "timestamp": 2.065419015697216
+        },
+        {
+            "x": 5.426658878791764,
+            "y": 6.613398993876379,
+            "heading": 0.12734083591800166,
+            "angularVelocity": 1.111250039689289,
+            "velocityX": -2.3495048644012306,
+            "velocityY": 0.16496264724146908,
+            "timestamp": 2.136662326379649
+        },
+        {
+            "x": 5.301258489415666,
+            "y": 6.621853943751229,
+            "heading": 0.1915936210653456,
+            "angularVelocity": 0.9018781487254319,
+            "velocityX": -1.7601707188351954,
+            "velocityY": 0.11867710517466466,
+            "timestamp": 2.207905637062082
+        },
+        {
+            "x": 5.217732575552286,
+            "y": 6.627325619944874,
+            "heading": 0.2366430637253094,
+            "angularVelocity": 0.6323322460514409,
+            "velocityX": -1.1724035992052257,
+            "velocityY": 0.07680266597989953,
+            "timestamp": 2.279148947744515
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": 0.32784742947732926,
+            "velocityX": -0.5857753542407405,
+            "velocityY": 0.037538682993647744,
+            "timestamp": 2.350392258426948
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": -8.68876182413906e-32,
+            "velocityX": 0,
+            "velocityY": -2.1409750340846636e-32,
+            "timestamp": 2.421635569109381
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C5 To S2.traj b/src/main/deploy/choreo/C5 To S2.traj
new file mode 100644
index 00000000..dfbc040a
--- /dev/null
+++ b/src/main/deploy/choreo/C5 To S2.traj	
@@ -0,0 +1,247 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": -1.3156383032943886e-37,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.664225467591612,
+            "y": 0.8361290574305452,
+            "heading": -0.011405957532827086,
+            "angularVelocity": -0.14485338302751044,
+            "velocityX": -0.20033341159264173,
+            "velocityY": 0.6239292188565086,
+            "timestamp": 0.07874139557141634
+        },
+        {
+            "x": 7.632676408078482,
+            "y": 0.9343872155736361,
+            "heading": -0.03421427612768491,
+            "angularVelocity": -0.2896610915940815,
+            "velocityX": -0.4006667558300489,
+            "velocityY": 1.2478589873857826,
+            "timestamp": 0.15748279114283267
+        },
+        {
+            "x": 7.585352811707443,
+            "y": 1.0817745497576996,
+            "heading": -0.06841862032572539,
+            "angularVelocity": -0.4343883410984004,
+            "velocityX": -0.6010002239307173,
+            "velocityY": 1.8717897125710332,
+            "timestamp": 0.236224186714249
+        },
+        {
+            "x": 7.522254637419436,
+            "y": 1.278291160108196,
+            "heading": -0.11400992700252059,
+            "angularVelocity": -0.5790004907322871,
+            "velocityX": -0.8013342134732577,
+            "velocityY": 2.4957217093296378,
+            "timestamp": 0.31496558228566535
+        },
+        {
+            "x": 7.443381794868827,
+            "y": 1.5239371518673812,
+            "heading": -0.1709767215367488,
+            "angularVelocity": -0.723466914966739,
+            "velocityX": -1.0016693503872847,
+            "velocityY": 3.1196550426438785,
+            "timestamp": 0.3937069778570817
+        },
+        {
+            "x": 7.3523133020023,
+            "y": 1.807548605142893,
+            "heading": -0.17097672945138376,
+            "angularVelocity": -1.00514283666892e-7,
+            "velocityX": -1.1565516740674315,
+            "velocityY": 3.6018088226323544,
+            "timestamp": 0.47244837342849805
+        },
+        {
+            "x": 7.26124476293705,
+            "y": 2.0911600435840003,
+            "heading": -0.17097673736581726,
+            "angularVelocity": -1.0051172494783456e-7,
+            "velocityX": -1.1565522607819834,
+            "velocityY": 3.6018086342383846,
+            "timestamp": 0.5511897689999143
+        },
+        {
+            "x": 7.170176175790629,
+            "y": 2.3747714665860937,
+            "heading": -0.17097674528023646,
+            "angularVelocity": -1.0051154369078753e-7,
+            "velocityX": -1.1565528714032516,
+            "velocityY": 3.601808438165991,
+            "timestamp": 0.6299311645713307
+        },
+        {
+            "x": 7.079107531002594,
+            "y": 2.6583828710792723,
+            "heading": -0.17097675319464586,
+            "angularVelocity": -1.0051141913755183e-7,
+            "velocityX": -1.1565536034402273,
+            "velocityY": 3.601808203106465,
+            "timestamp": 0.708672560142747
+        },
+        {
+            "x": 6.988038816793172,
+            "y": 2.9419942532809915,
+            "heading": -0.17097676110905413,
+            "angularVelocity": -1.0051140450859988e-7,
+            "velocityX": -1.1565544850779974,
+            "velocityY": 3.601807920009381,
+            "timestamp": 0.7874139557141634
+        },
+        {
+            "x": 6.896968817946089,
+            "y": 3.2256052229773635,
+            "heading": -0.17097676902348397,
+            "angularVelocity": -1.0051167869642574e-7,
+            "velocityX": -1.156570799719756,
+            "velocityY": 3.6018026812738513,
+            "timestamp": 0.8661553512855797
+        },
+        {
+            "x": 6.765153291475094,
+            "y": 3.4927261567236307,
+            "heading": -0.1709767773203105,
+            "angularVelocity": -1.053680400636008e-7,
+            "velocityX": -1.6740308641271406,
+            "velocityY": 3.392382517579277,
+            "timestamp": 0.9448967468569961
+        },
+        {
+            "x": 6.589137393143544,
+            "y": 3.7330328933669676,
+            "heading": -0.17097678717611917,
+            "angularVelocity": -1.2516680215717606e-7,
+            "velocityX": -2.235366760446956,
+            "velocityY": 3.0518475688608455,
+            "timestamp": 1.0236381424284124
+        },
+        {
+            "x": 6.374222917834714,
+            "y": 3.939286833341542,
+            "heading": -0.17097680095561982,
+            "angularVelocity": -1.7499690668870768e-7,
+            "velocityX": -2.7293709204570815,
+            "velocityY": 2.6193838511219654,
+            "timestamp": 1.1023795379998287
+        },
+        {
+            "x": 6.126883506774902,
+            "y": 4.1052751541137695,
+            "heading": -0.17097682837855596,
+            "angularVelocity": -3.482658130630958e-7,
+            "velocityX": -3.1411611296053437,
+            "velocityY": 2.108018527836233,
+            "timestamp": 1.181120933571245
+        },
+        {
+            "x": 5.792856415059812,
+            "y": 4.239084767822678,
+            "heading": -0.17097684795948573,
+            "angularVelocity": -2.0585572281638246e-7,
+            "velocityX": -3.5116508401962974,
+            "velocityY": 1.4067500932170736,
+            "timestamp": 1.2762406100184678
+        },
+        {
+            "x": 5.438271606876556,
+            "y": 4.300308177252639,
+            "heading": -0.1709790019096565,
+            "angularVelocity": -0.000022644633069121558,
+            "velocityX": -3.7277755920458318,
+            "velocityY": 0.6436461068486719,
+            "timestamp": 1.3713602864656906
+        },
+        {
+            "x": 5.110881399181515,
+            "y": 4.377812934831655,
+            "heading": -0.2483895057711766,
+            "angularVelocity": -0.8138221948691259,
+            "velocityX": -3.4418768011337173,
+            "velocityY": 0.8148130909803968,
+            "timestamp": 1.4664799629129135
+        },
+        {
+            "x": 4.831528638348494,
+            "y": 4.508515906317057,
+            "heading": -0.41196599842011083,
+            "angularVelocity": -1.7196914325049715,
+            "velocityX": -2.936855667165985,
+            "velocityY": 1.374089740075198,
+            "timestamp": 1.5615996393601363
+        },
+        {
+            "x": 4.597313308728367,
+            "y": 4.699607232649008,
+            "heading": -0.5851564423497488,
+            "angularVelocity": -1.8207635938052467,
+            "velocityX": -2.4623226063019743,
+            "velocityY": 2.0089568580269215,
+            "timestamp": 1.6567193158073592
+        },
+        {
+            "x": 4.422373656654268,
+            "y": 4.922240435570765,
+            "heading": -0.6321779806524386,
+            "angularVelocity": -0.49434081421397164,
+            "velocityX": -1.8391531448402672,
+            "velocityY": 2.340558875274187,
+            "timestamp": 1.751838992254582
+        },
+        {
+            "x": 4.3095074978210945,
+            "y": 5.147658123857161,
+            "heading": -0.526303235570326,
+            "angularVelocity": 1.11306881012003,
+            "velocityX": -1.1865700457443962,
+            "velocityY": 2.3698323701875594,
+            "timestamp": 1.846958668701805
+        },
+        {
+            "x": 4.263497791071102,
+            "y": 5.351217934034869,
+            "heading": -0.3174810959543699,
+            "angularVelocity": 2.1953621733755666,
+            "velocityX": -0.48370335632419137,
+            "velocityY": 2.1400389254967007,
+            "timestamp": 1.9420783451490278
+        },
+        {
+            "x": 4.258299402660224,
+            "y": 5.491389588363689,
+            "heading": -0.11860771311510487,
+            "angularVelocity": 2.090770177814994,
+            "velocityX": -0.05465103125915711,
+            "velocityY": 1.4736346838458172,
+            "timestamp": 2.0371980215962506
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 6.542074475732666e-36,
+            "angularVelocity": 1.2469314188732994,
+            "velocityX": 0.04941771792488758,
+            "velocityY": 0.7213061923563153,
+            "timestamp": 2.1323176980434737
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -6.642487421560091e-37,
+            "velocityY": 5.186129384888016e-33,
+            "timestamp": 2.227437374490697
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index efc9a9d9..0a43236c 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -8,6 +8,7 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -140,7 +141,9 @@ public void robotInit() {
     // Test binding for elevator
     controller.b().whileTrue(elevator.setExtensionCmd(() -> 1.0));
 
-    controller.x().toggleOnTrue(swerve.runChoreoTraj(DynamicAuto.makeNoteToNote(swerve::getPose)));
+    controller
+        .x()
+        .toggleOnTrue(swerve.runChoreoTraj(() -> DynamicAuto.makeNoteToNote(swerve::getPose)));
     controller
         .leftBumper()
         .toggleOnTrue(
@@ -151,6 +154,17 @@ public void robotInit() {
         .toggleOnTrue(
             Commands.runOnce(
                 () -> DynamicAuto.getAbsoluteClosestNote(swerve::getPose).whitelist(), swerve));
+
+    SmartDashboard.putData(
+        "Start To Note", swerve.runChoreoTraj(() -> DynamicAuto.makeStartToNote(swerve::getPose)));
+    SmartDashboard.putData(
+        "Note To Note", swerve.runChoreoTraj(() -> DynamicAuto.makeNoteToNote(swerve::getPose)));
+    SmartDashboard.putData(
+        "Shoot To Note",
+        swerve.runChoreoTraj(() -> DynamicAuto.makeShootingToNote(swerve::getPose)));
+    SmartDashboard.putData(
+        "Note To Shooting",
+        swerve.runChoreoTraj(() -> DynamicAuto.makeNoteToShooting(swerve::getPose)));
   }
 
   @Override
@@ -160,10 +174,12 @@ public void robotPeriodic() {
         "DynamicAuto/Closest Note", DynamicAuto.getClosestNote(swerve::getPose).getPose());
 
     Logger.recordOutput(
-        "DynamicAuto/Absolute Closest Note", DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getPose());
+        "DynamicAuto/Absolute Closest Note",
+        DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getPose());
     Logger.recordOutput(
         "DynamicAuto/Closest Shooting Location",
-        DynamicAuto.closestShootingLocation(() -> swerve.getPose()).getPose());
+        DynamicAuto.closestShootingLocation(() -> swerve.getPose(), DynamicAuto.shootingLocations)
+            .getPose());
   }
 
   @Override
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
index 26e27c48..a74538dc 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
@@ -53,6 +53,7 @@
 import frc.robot.utils.autoaim.AutoAim;
 import java.util.Arrays;
 import java.util.Optional;
+import java.util.Set;
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
 import java.util.function.DoubleSupplier;
@@ -416,20 +417,24 @@ public Command pointTowardsTranslation(DoubleSupplier x, DoubleSupplier y) {
                         getVelocity().omegaRadiansPerSecond)));
   }
 
-  public Command runChoreoTraj(ChoreoTrajectory traj) {
-    return Choreo.choreoSwerveCommand(
-        traj,
-        this::getPose,
-        new PIDController(5.0, 0.0, 0.0),
-        new PIDController(5.0, 0.0, 0.0),
-        new PIDController(5.0, 0.0, 0.0),
-        (ChassisSpeeds speeds) -> //
-        this.runVelocity(speeds),
-        () -> {
-          Optional<Alliance> alliance = DriverStation.getAlliance();
-          return alliance.isPresent() && alliance.get() == Alliance.Red;
-        },
-        this);
+  public Command runChoreoTraj(Supplier<ChoreoTrajectory> traj) {
+
+    return Commands.defer(
+        () ->
+            Choreo.choreoSwerveCommand(
+                traj.get(),
+                this::getPose,
+                new PIDController(5.0, 0.0, 0.0),
+                new PIDController(5.0, 0.0, 0.0),
+                new PIDController(5.0, 0.0, 0.0),
+                (ChassisSpeeds speeds) -> //
+                this.runVelocity(speeds),
+                () -> {
+                  Optional<Alliance> alliance = DriverStation.getAlliance();
+                  return alliance.isPresent() && alliance.get() == Alliance.Red;
+                },
+                this),
+        Set.of(this));
   }
 
   public Command runModuleSteerCharacterizationCmd() {
diff --git a/src/main/java/frc/robot/utils/dynamicauto/ChoreoBuilder.py b/src/main/java/frc/robot/utils/dynamicauto/ChoreoBuilder.py
index f413d057..324be529 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/ChoreoBuilder.py
+++ b/src/main/java/frc/robot/utils/dynamicauto/ChoreoBuilder.py
@@ -17,8 +17,8 @@
     {"name": "C1", "x": 7.68, "y": 7.467, "angle": 0, "allowed_destinations": ["W1", "W2", "W3", "S1", "S2", "S3", "S4", "C2", "C3", "C4", "C5"]},
     {"name": "C2", "x": 7.68, "y": 5.797, "angle": 0, "allowed_destinations": ["W1", "W2", "W3", "S1", "S2", "S3", "S4", "C1",  "C3", "C4", "C5"]},
     {"name": "C3", "x": 7.68, "y": 4.127, "angle": 0, "allowed_destinations": ["W1", "W2", "W3", "S1", "S2", "S3", "S4", "C1", "C2",  "C4", "C5"]},
-    {"name": "C4", "x": 7.68, "y": 2.457, "angle": 0, "allowed_destinations": ["W1", "W2", "W3", "S3", "S4", "S3", "S4", "C1", "C2", "C3",  "C5"]},
-    {"name": "C5", "x": 7.68, "y": 0.787, "angle": 0, "allowed_destinations": ["W1", "W2", "W3", "S3", "S4", "S3", "S4", "C1", "C2", "C3", "C4"]},
+    {"name": "C4", "x": 7.68, "y": 2.457, "angle": 0, "allowed_destinations": ["W1", "W2", "W3", "S1", "S2", "S3", "S4", "C1", "C2", "C3",  "C5"]},
+    {"name": "C5", "x": 7.68, "y": 0.787, "angle": 0, "allowed_destinations": ["W1", "W2", "W3", "S1", "S2", "S3", "S4", "C1", "C2", "C3", "C4"]},
 ]
 
 obastacles = [
diff --git a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
index a12c982e..f5a86736 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
@@ -5,28 +5,39 @@
 import com.google.common.base.Supplier;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import org.littletonrobotics.junction.Logger;
 
 public class DynamicAuto {
 
   public static final Note[] notes = {
-      new Note(new Pose2d(2.204, 7.0, Rotation2d.fromRadians(Math.PI)), false, 0, "W1"), // w1
-      new Note(new Pose2d(2.204, 5.5, Rotation2d.fromRadians(Math.PI)), false, 1, "W2"), // w2
-      new Note(new Pose2d(2.204, 4.1, Rotation2d.fromRadians(Math.PI)), false, 2, "W3"), // w3
-      new Note(new Pose2d(7.538, 7.3, Rotation2d.fromRadians(Math.PI)), false, 0, "C1"), // c1
-      new Note(new Pose2d(7.538, 5.7, Rotation2d.fromRadians(Math.PI)), false, 0, "C2"), // c2
-      new Note(new Pose2d(7.538, 4.1, Rotation2d.fromRadians(Math.PI)), false, 1, "C3"), // c3
-      new Note(new Pose2d(7.538, 2.5, Rotation2d.fromRadians(Math.PI)), false, 2, "C4"), // c4
-      new Note(new Pose2d(7.538, 0.7, Rotation2d.fromRadians(Math.PI)), false, 3, "C5") // c5
+    new Note(new Pose2d(2.204, 7.0, Rotation2d.fromRadians(Math.PI)), false, 0, "W1"), // w1
+    new Note(new Pose2d(2.204, 5.5, Rotation2d.fromRadians(Math.PI)), false, 1, "W2"), // w2
+    new Note(new Pose2d(2.204, 4.1, Rotation2d.fromRadians(Math.PI)), false, 2, "W3"), // w3
+    new Note(new Pose2d(7.538, 7.3, Rotation2d.fromRadians(Math.PI)), false, 0, "C1"), // c1
+    new Note(new Pose2d(7.538, 5.7, Rotation2d.fromRadians(Math.PI)), false, 0, "C2"), // c2
+    new Note(new Pose2d(7.538, 4.1, Rotation2d.fromRadians(Math.PI)), false, 1, "C3"), // c3
+    new Note(new Pose2d(7.538, 2.5, Rotation2d.fromRadians(Math.PI)), false, 2, "C4"), // c4
+    new Note(new Pose2d(7.538, 0.7, Rotation2d.fromRadians(Math.PI)), false, 3, "C5") // c5
   };
 
   public static final ShootingLocation[] shootingLocations = {
-      new ShootingLocation(new Pose2d(5.639, 6.463, Rotation2d.fromRadians(-3.04)), "S1"), // Left
-      new ShootingLocation(new Pose2d(4.216, 5.216, Rotation2d.fromRadians(-Math.PI)), "S2"),
-      new ShootingLocation(new Pose2d(4.263, 3.000, Rotation2d.fromRadians(-0.566)), "S3"), // Middle
-      new ShootingLocation(new Pose2d(5.176, 1.620, Rotation2d.fromRadians(2.961)), "S4") // Right
+    new ShootingLocation(new Pose2d(5.639, 6.463, Rotation2d.fromRadians(-3.04)), "S1"), // Left
+    new ShootingLocation(new Pose2d(4.216, 5.216, Rotation2d.fromRadians(-Math.PI)), "S2"),
+    new ShootingLocation(new Pose2d(4.263, 3.000, Rotation2d.fromRadians(-0.566)), "S3"), // Middle
+    new ShootingLocation(new Pose2d(5.176, 1.620, Rotation2d.fromRadians(2.961)), "S4") // Right
   };
 
-  public static ShootingLocation closestShootingLocation(Supplier<Pose2d> curPose) {
+  public static final ShootingLocation[] startingLocations = {
+    new ShootingLocation(
+        new Pose2d(0.71, 6.72, Rotation2d.fromRadians(1.04)), "Amp Side"), // Speaker (Amp Side)
+    new ShootingLocation(
+        new Pose2d(1.35, 5.56, Rotation2d.fromRadians(0)), "Center"), // Speaker (Center)
+    new ShootingLocation(
+        new Pose2d(0.71, 4.36, Rotation2d.fromRadians(-1.04)), "Stage Side") // Speaker (Stage Side)
+  };
+
+  public static ShootingLocation closestShootingLocation(
+      Supplier<Pose2d> curPose, ShootingLocation[] shootingLocations) {
 
     ShootingLocation closestLocation = new ShootingLocation();
     double shortestDistance = 9999999;
@@ -39,12 +50,32 @@ public static ShootingLocation closestShootingLocation(Supplier<Pose2d> curPose)
     return closestLocation;
   }
 
+  public static ChoreoTrajectory makeStartToNote(Supplier<Pose2d> startingPose) {
+    ShootingLocation startingLocation = closestShootingLocation(startingPose, startingLocations);
+    Note closestNote = getClosestNote(startingPose);
+    return Choreo.getTrajectory(startingLocation.getName() + " To " + closestNote.getName());
+  }
+
+  public static ChoreoTrajectory makeNoteToShooting(Supplier<Pose2d> startingPose) {
+    Note closestNote = getClosestNote(startingPose);
+    ShootingLocation startingLocation = closestShootingLocation(startingPose, shootingLocations);
+    return Choreo.getTrajectory(closestNote.getName() + " To " + startingLocation.getName());
+  }
+
+  public static ChoreoTrajectory makeShootingToNote(Supplier<Pose2d> startingPose) {
+    Note closestNote = getClosestNote(startingPose);
+    ShootingLocation startingLocation = closestShootingLocation(startingPose, shootingLocations);
+    return Choreo.getTrajectory(startingLocation.getName() + " To " + closestNote.getName());
+  }
+
   public static ChoreoTrajectory makeNoteToNote(Supplier<Pose2d> startingPose) {
     Note closestNote = getClosestNote(startingPose);
     closestNote.blacklist();
     Note nextClosest = getClosestNote(startingPose);
+    Logger.recordOutput("DynamicAuto/FHFHH", nextClosest.getPose());
     closestNote.whitelist();
-    System.out.println(closestNote.getName() + " To " + nextClosest.getName());
+    Logger.recordOutput(
+        "DynamicAuto/Path Name", closestNote.getName() + " To " + nextClosest.getName());
     return Choreo.getTrajectory(closestNote.getName() + " To " + nextClosest.getName());
   }
 
diff --git a/src/main/java/frc/robot/utils/dynamicauto/Note.java b/src/main/java/frc/robot/utils/dynamicauto/Note.java
index 89829469..e34d87cc 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/Note.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/Note.java
@@ -29,10 +29,12 @@ public Note() {
   }
 
   public void blacklist() {
+    System.out.println("Blacklisted " + name);
     this.blacklisted = true;
   }
 
   public void whitelist() {
+    System.out.println("Whitelisted " + name);
     this.blacklisted = false;
   }
 
diff --git a/src/main/java/frc/robot/utils/dynamicauto/data.chor b/src/main/java/frc/robot/utils/dynamicauto/data.chor
index 9d822e94..f31a047f 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/data.chor
+++ b/src/main/java/frc/robot/utils/dynamicauto/data.chor
@@ -41613,6 +41613,1264 @@
             "defaultControlIntervalCount": 40,
             "usesDefaultFieldObstacles": true,
             "circleObstacles": []
+        },
+        "C5 To S1": {
+            "waypoints": [
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 19
+                },
+                {
+                    "x": 7.198879241943359,
+                    "y": 5.9179229736328125,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 12
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 0.26,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 0,
+                    "angularVelocity": 4.273608378959021e-31,
+                    "velocityX": -7.420825794722196e-33,
+                    "velocityY": 7.583788429881001e-34,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.678491547493202,
+                    "y": 0.8436471873828707,
+                    "heading": -0.006289831251442517,
+                    "angularVelocity": -0.0762785379812196,
+                    "velocityX": -0.018293424295956956,
+                    "velocityY": 0.686976241742982,
+                    "timestamp": 0.08245872846948342
+                },
+                {
+                    "x": 7.67547464227306,
+                    "y": 0.9569415605229419,
+                    "heading": -0.018869443032064528,
+                    "angularVelocity": -0.1525564608394078,
+                    "velocityX": -0.03658685109677269,
+                    "velocityY": 1.3739524637710059,
+                    "timestamp": 0.16491745693896684
+                },
+                {
+                    "x": 7.670949285584863,
+                    "y": 1.1268831170081794,
+                    "heading": -0.03773871517098893,
+                    "angularVelocity": -0.22883292635185057,
+                    "velocityX": -0.05488026279561822,
+                    "velocityY": 2.060928656547622,
+                    "timestamp": 0.24737618540845024
+                },
+                {
+                    "x": 7.664915480002808,
+                    "y": 1.3534718520373763,
+                    "heading": -0.06289740953278049,
+                    "angularVelocity": -0.3051065039294483,
+                    "velocityX": -0.07317364327643945,
+                    "velocityY": 2.7479047910986583,
+                    "timestamp": 0.3298349138779337
+                },
+                {
+                    "x": 7.657373229393316,
+                    "y": 1.6367077493690674,
+                    "heading": -0.09434508673453766,
+                    "angularVelocity": -0.38137475298804396,
+                    "velocityX": -0.09146697686811679,
+                    "velocityY": 3.4348807286849197,
+                    "timestamp": 0.4122936423474171
+                },
+                {
+                    "x": 7.649069464629009,
+                    "y": 1.9485336685806538,
+                    "heading": -0.09434509076634809,
+                    "angularVelocity": -4.889488962341613e-8,
+                    "velocityX": -0.10070207142933957,
+                    "velocityY": 3.781599898511507,
+                    "timestamp": 0.49475237081690054
+                },
+                {
+                    "x": 7.6407656424773585,
+                    "y": 2.260359586264075,
+                    "heading": -0.09434509479812381,
+                    "angularVelocity": -4.889446872332144e-8,
+                    "velocityX": -0.1007027673816724,
+                    "velocityY": 3.781599879979021,
+                    "timestamp": 0.5772110992863839
+                },
+                {
+                    "x": 7.63246175736497,
+                    "y": 2.5721855022708704,
+                    "heading": -0.09434509882989094,
+                    "angularVelocity": -4.889436444733497e-8,
+                    "velocityX": -0.10070353092409444,
+                    "velocityY": 3.7815998596461093,
+                    "timestamp": 0.6596698277558674
+                },
+                {
+                    "x": 7.6241578045918486,
+                    "y": 2.8840114164758646,
+                    "heading": -0.09434510286165439,
+                    "angularVelocity": -4.8894319896339695e-8,
+                    "velocityX": -0.10070435146468282,
+                    "velocityY": 3.781599837795173,
+                    "timestamp": 0.7421285562253508
+                },
+                {
+                    "x": 7.615853780314312,
+                    "y": 3.195837328776685,
+                    "heading": -0.09434510689341911,
+                    "angularVelocity": -4.889433549543518e-8,
+                    "velocityX": -0.10070521861867329,
+                    "velocityY": 3.78159981470272,
+                    "timestamp": 0.8245872846948342
+                },
+                {
+                    "x": 7.607549681647345,
+                    "y": 3.5076632390964844,
+                    "heading": -0.09434511092518724,
+                    "angularVelocity": -4.889437661104965e-8,
+                    "velocityX": -0.10070612076003535,
+                    "velocityY": 3.7815997906783263,
+                    "timestamp": 0.9070460131643177
+                },
+                {
+                    "x": 7.599245507241111,
+                    "y": 3.819489147399297,
+                    "heading": -0.0943451149569584,
+                    "angularVelocity": -4.889441348359375e-8,
+                    "velocityX": -0.10070703927126574,
+                    "velocityY": 3.7815997662177643,
+                    "timestamp": 0.9895047416338011
+                },
+                {
+                    "x": 7.590941256893013,
+                    "y": 4.131315053679708,
+                    "heading": -0.09434511898873191,
+                    "angularVelocity": -4.88944418703562e-8,
+                    "velocityX": -0.10070796023943433,
+                    "velocityY": 3.781599741691539,
+                    "timestamp": 1.0719634701032845
+                },
+                {
+                    "x": 7.582636930618974,
+                    "y": 4.443140957938123,
+                    "heading": -0.09434512302050634,
+                    "angularVelocity": -4.889445311998471e-8,
+                    "velocityX": -0.10070888101448454,
+                    "velocityY": 3.781599717170225,
+                    "timestamp": 1.1544221985727678
+                },
+                {
+                    "x": 7.574332505518219,
+                    "y": 4.754966859564639,
+                    "heading": -0.09434512705227821,
+                    "angularVelocity": -4.889442208864286e-8,
+                    "velocityX": -0.10071007951363715,
+                    "velocityY": 3.78159968525245,
+                    "timestamp": 1.2368809270422512
+                },
+                {
+                    "x": 7.562081052721255,
+                    "y": 5.066662637971005,
+                    "heading": -0.09434513109398723,
+                    "angularVelocity": -4.901493268640619e-8,
+                    "velocityX": -0.1485767853126217,
+                    "velocityY": 3.7800216446669976,
+                    "timestamp": 1.3193396555117345
+                },
+                {
+                    "x": 7.493618412578439,
+                    "y": 5.370993415792126,
+                    "heading": -0.09434513545729191,
+                    "angularVelocity": -5.29150129274595e-8,
+                    "velocityX": -0.8302655329951328,
+                    "velocityY": 3.6907042282825056,
+                    "timestamp": 1.4017983839812178
+                },
+                {
+                    "x": 7.37120325739307,
+                    "y": 5.657906089273735,
+                    "heading": -0.09434514092208332,
+                    "angularVelocity": -6.627304961707395e-8,
+                    "velocityX": -1.4845627316539582,
+                    "velocityY": 3.479470018601967,
+                    "timestamp": 1.4842571124507011
+                },
+                {
+                    "x": 7.198879241943359,
+                    "y": 5.9179229736328125,
+                    "heading": -0.09434514894422652,
+                    "angularVelocity": -9.728676819194244e-8,
+                    "velocityX": -2.089821400938593,
+                    "velocityY": 3.153297281988846,
+                    "timestamp": 1.5667158409201845
+                },
+                {
+                    "x": 7.016660952404083,
+                    "y": 6.116497130944734,
+                    "heading": -0.09434515627038953,
+                    "angularVelocity": -1.028329950777357e-7,
+                    "velocityX": -2.5576898068579945,
+                    "velocityY": 2.7872673997010713,
+                    "timestamp": 1.6379591516026175
+                },
+                {
+                    "x": 6.805603401067166,
+                    "y": 6.284097595272953,
+                    "heading": -0.09434516139165254,
+                    "angularVelocity": -7.188412437441302e-8,
+                    "velocityX": -2.9624893806199624,
+                    "velocityY": 2.352508084236811,
+                    "timestamp": 1.7092024622850506
+                },
+                {
+                    "x": 6.57091097390028,
+                    "y": 6.416591642615984,
+                    "heading": -0.09434516555011045,
+                    "angularVelocity": -5.83698017255904e-8,
+                    "velocityX": -3.2942380824078485,
+                    "velocityY": 1.8597401787463752,
+                    "timestamp": 1.7804457729674836
+                },
+                {
+                    "x": 6.318370883291981,
+                    "y": 6.510712371224652,
+                    "heading": -0.09434516933867851,
+                    "angularVelocity": -5.317787750894471e-8,
+                    "velocityX": -3.544755124224867,
+                    "velocityY": 1.3211167154795889,
+                    "timestamp": 1.8516890836499167
+                },
+                {
+                    "x": 6.054210380502568,
+                    "y": 6.5641393107028225,
+                    "heading": -0.09434515209433943,
+                    "angularVelocity": 2.420485363909629e-7,
+                    "velocityX": -3.7078639420184567,
+                    "velocityY": 0.7499221887135695,
+                    "timestamp": 1.9229323943323497
+                },
+                {
+                    "x": 5.803536575930751,
+                    "y": 6.585943895129495,
+                    "heading": -0.03682720962895,
+                    "angularVelocity": 0.8073451656643992,
+                    "velocityX": -3.5185591765828246,
+                    "velocityY": 0.30605798941413415,
+                    "timestamp": 1.9941757050147828
+                },
+                {
+                    "x": 5.594045383796189,
+                    "y": 6.601646508747958,
+                    "heading": 0.04817170409455108,
+                    "angularVelocity": 1.1930792225867084,
+                    "velocityX": -2.9405033276509123,
+                    "velocityY": 0.2204082526211729,
+                    "timestamp": 2.065419015697216
+                },
+                {
+                    "x": 5.426658878791764,
+                    "y": 6.613398993876379,
+                    "heading": 0.12734083591800166,
+                    "angularVelocity": 1.111250039689289,
+                    "velocityX": -2.3495048644012306,
+                    "velocityY": 0.16496264724146908,
+                    "timestamp": 2.136662326379649
+                },
+                {
+                    "x": 5.301258489415666,
+                    "y": 6.621853943751229,
+                    "heading": 0.1915936210653456,
+                    "angularVelocity": 0.9018781487254319,
+                    "velocityX": -1.7601707188351954,
+                    "velocityY": 0.11867710517466466,
+                    "timestamp": 2.207905637062082
+                },
+                {
+                    "x": 5.217732575552286,
+                    "y": 6.627325619944874,
+                    "heading": 0.2366430637253094,
+                    "angularVelocity": 0.6323322460514409,
+                    "velocityX": -1.1724035992052257,
+                    "velocityY": 0.07680266597989953,
+                    "timestamp": 2.279148947744515
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 0.26,
+                    "angularVelocity": 0.32784742947732926,
+                    "velocityX": -0.5857753542407405,
+                    "velocityY": 0.037538682993647744,
+                    "timestamp": 2.350392258426948
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 0.26,
+                    "angularVelocity": -8.68876182413906e-32,
+                    "velocityX": 0,
+                    "velocityY": -2.1409750340846636e-32,
+                    "timestamp": 2.421635569109381
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
+        },
+        "C5 To S2": {
+            "waypoints": [
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 6.126883506774902,
+                    "y": 4.1052751541137695,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 11
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 0.787,
+                    "heading": 0,
+                    "angularVelocity": -1.3156383032943886e-37,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.664225467591612,
+                    "y": 0.8361290574305452,
+                    "heading": -0.011405957532827086,
+                    "angularVelocity": -0.14485338302751044,
+                    "velocityX": -0.20033341159264173,
+                    "velocityY": 0.6239292188565086,
+                    "timestamp": 0.07874139557141634
+                },
+                {
+                    "x": 7.632676408078482,
+                    "y": 0.9343872155736361,
+                    "heading": -0.03421427612768491,
+                    "angularVelocity": -0.2896610915940815,
+                    "velocityX": -0.4006667558300489,
+                    "velocityY": 1.2478589873857826,
+                    "timestamp": 0.15748279114283267
+                },
+                {
+                    "x": 7.585352811707443,
+                    "y": 1.0817745497576996,
+                    "heading": -0.06841862032572539,
+                    "angularVelocity": -0.4343883410984004,
+                    "velocityX": -0.6010002239307173,
+                    "velocityY": 1.8717897125710332,
+                    "timestamp": 0.236224186714249
+                },
+                {
+                    "x": 7.522254637419436,
+                    "y": 1.278291160108196,
+                    "heading": -0.11400992700252059,
+                    "angularVelocity": -0.5790004907322871,
+                    "velocityX": -0.8013342134732577,
+                    "velocityY": 2.4957217093296378,
+                    "timestamp": 0.31496558228566535
+                },
+                {
+                    "x": 7.443381794868827,
+                    "y": 1.5239371518673812,
+                    "heading": -0.1709767215367488,
+                    "angularVelocity": -0.723466914966739,
+                    "velocityX": -1.0016693503872847,
+                    "velocityY": 3.1196550426438785,
+                    "timestamp": 0.3937069778570817
+                },
+                {
+                    "x": 7.3523133020023,
+                    "y": 1.807548605142893,
+                    "heading": -0.17097672945138376,
+                    "angularVelocity": -1.00514283666892e-7,
+                    "velocityX": -1.1565516740674315,
+                    "velocityY": 3.6018088226323544,
+                    "timestamp": 0.47244837342849805
+                },
+                {
+                    "x": 7.26124476293705,
+                    "y": 2.0911600435840003,
+                    "heading": -0.17097673736581726,
+                    "angularVelocity": -1.0051172494783456e-7,
+                    "velocityX": -1.1565522607819834,
+                    "velocityY": 3.6018086342383846,
+                    "timestamp": 0.5511897689999143
+                },
+                {
+                    "x": 7.170176175790629,
+                    "y": 2.3747714665860937,
+                    "heading": -0.17097674528023646,
+                    "angularVelocity": -1.0051154369078753e-7,
+                    "velocityX": -1.1565528714032516,
+                    "velocityY": 3.601808438165991,
+                    "timestamp": 0.6299311645713307
+                },
+                {
+                    "x": 7.079107531002594,
+                    "y": 2.6583828710792723,
+                    "heading": -0.17097675319464586,
+                    "angularVelocity": -1.0051141913755183e-7,
+                    "velocityX": -1.1565536034402273,
+                    "velocityY": 3.601808203106465,
+                    "timestamp": 0.708672560142747
+                },
+                {
+                    "x": 6.988038816793172,
+                    "y": 2.9419942532809915,
+                    "heading": -0.17097676110905413,
+                    "angularVelocity": -1.0051140450859988e-7,
+                    "velocityX": -1.1565544850779974,
+                    "velocityY": 3.601807920009381,
+                    "timestamp": 0.7874139557141634
+                },
+                {
+                    "x": 6.896968817946089,
+                    "y": 3.2256052229773635,
+                    "heading": -0.17097676902348397,
+                    "angularVelocity": -1.0051167869642574e-7,
+                    "velocityX": -1.156570799719756,
+                    "velocityY": 3.6018026812738513,
+                    "timestamp": 0.8661553512855797
+                },
+                {
+                    "x": 6.765153291475094,
+                    "y": 3.4927261567236307,
+                    "heading": -0.1709767773203105,
+                    "angularVelocity": -1.053680400636008e-7,
+                    "velocityX": -1.6740308641271406,
+                    "velocityY": 3.392382517579277,
+                    "timestamp": 0.9448967468569961
+                },
+                {
+                    "x": 6.589137393143544,
+                    "y": 3.7330328933669676,
+                    "heading": -0.17097678717611917,
+                    "angularVelocity": -1.2516680215717606e-7,
+                    "velocityX": -2.235366760446956,
+                    "velocityY": 3.0518475688608455,
+                    "timestamp": 1.0236381424284124
+                },
+                {
+                    "x": 6.374222917834714,
+                    "y": 3.939286833341542,
+                    "heading": -0.17097680095561982,
+                    "angularVelocity": -1.7499690668870768e-7,
+                    "velocityX": -2.7293709204570815,
+                    "velocityY": 2.6193838511219654,
+                    "timestamp": 1.1023795379998287
+                },
+                {
+                    "x": 6.126883506774902,
+                    "y": 4.1052751541137695,
+                    "heading": -0.17097682837855596,
+                    "angularVelocity": -3.482658130630958e-7,
+                    "velocityX": -3.1411611296053437,
+                    "velocityY": 2.108018527836233,
+                    "timestamp": 1.181120933571245
+                },
+                {
+                    "x": 5.792856415059812,
+                    "y": 4.239084767822678,
+                    "heading": -0.17097684795948573,
+                    "angularVelocity": -2.0585572281638246e-7,
+                    "velocityX": -3.5116508401962974,
+                    "velocityY": 1.4067500932170736,
+                    "timestamp": 1.2762406100184678
+                },
+                {
+                    "x": 5.438271606876556,
+                    "y": 4.300308177252639,
+                    "heading": -0.1709790019096565,
+                    "angularVelocity": -0.000022644633069121558,
+                    "velocityX": -3.7277755920458318,
+                    "velocityY": 0.6436461068486719,
+                    "timestamp": 1.3713602864656906
+                },
+                {
+                    "x": 5.110881399181515,
+                    "y": 4.377812934831655,
+                    "heading": -0.2483895057711766,
+                    "angularVelocity": -0.8138221948691259,
+                    "velocityX": -3.4418768011337173,
+                    "velocityY": 0.8148130909803968,
+                    "timestamp": 1.4664799629129135
+                },
+                {
+                    "x": 4.831528638348494,
+                    "y": 4.508515906317057,
+                    "heading": -0.41196599842011083,
+                    "angularVelocity": -1.7196914325049715,
+                    "velocityX": -2.936855667165985,
+                    "velocityY": 1.374089740075198,
+                    "timestamp": 1.5615996393601363
+                },
+                {
+                    "x": 4.597313308728367,
+                    "y": 4.699607232649008,
+                    "heading": -0.5851564423497488,
+                    "angularVelocity": -1.8207635938052467,
+                    "velocityX": -2.4623226063019743,
+                    "velocityY": 2.0089568580269215,
+                    "timestamp": 1.6567193158073592
+                },
+                {
+                    "x": 4.422373656654268,
+                    "y": 4.922240435570765,
+                    "heading": -0.6321779806524386,
+                    "angularVelocity": -0.49434081421397164,
+                    "velocityX": -1.8391531448402672,
+                    "velocityY": 2.340558875274187,
+                    "timestamp": 1.751838992254582
+                },
+                {
+                    "x": 4.3095074978210945,
+                    "y": 5.147658123857161,
+                    "heading": -0.526303235570326,
+                    "angularVelocity": 1.11306881012003,
+                    "velocityX": -1.1865700457443962,
+                    "velocityY": 2.3698323701875594,
+                    "timestamp": 1.846958668701805
+                },
+                {
+                    "x": 4.263497791071102,
+                    "y": 5.351217934034869,
+                    "heading": -0.3174810959543699,
+                    "angularVelocity": 2.1953621733755666,
+                    "velocityX": -0.48370335632419137,
+                    "velocityY": 2.1400389254967007,
+                    "timestamp": 1.9420783451490278
+                },
+                {
+                    "x": 4.258299402660224,
+                    "y": 5.491389588363689,
+                    "heading": -0.11860771311510487,
+                    "angularVelocity": 2.090770177814994,
+                    "velocityX": -0.05465103125915711,
+                    "velocityY": 1.4736346838458172,
+                    "timestamp": 2.0371980215962506
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 6.542074475732666e-36,
+                    "angularVelocity": 1.2469314188732994,
+                    "velocityX": 0.04941771792488758,
+                    "velocityY": 0.7213061923563153,
+                    "timestamp": 2.1323176980434737
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 0,
+                    "angularVelocity": 0,
+                    "velocityX": -6.642487421560091e-37,
+                    "velocityY": 5.186129384888016e-33,
+                    "timestamp": 2.227437374490697
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
+        },
+        "C4 To S1": {
+            "waypoints": [
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 4.353217601776123,
+                    "y": 4.923890113830566,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 11
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 0.26,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 0,
+                    "angularVelocity": -3.908498355076501e-28,
+                    "velocityX": -5.285170566576542e-31,
+                    "velocityY": -1.5421992925545815e-31,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.644672083885401,
+                    "y": 2.5062543236054426,
+                    "heading": 0.0003371503707738104,
+                    "angularVelocity": 0.003954317681676867,
+                    "velocityX": -0.4143486570570975,
+                    "velocityY": 0.5776865743787781,
+                    "timestamp": 0.08526132645270777
+                },
+                {
+                    "x": 7.574016248997627,
+                    "y": 2.604762966378743,
+                    "heading": 0.0010114541718947692,
+                    "angularVelocity": 0.00790867124800936,
+                    "velocityX": -0.8286973452956875,
+                    "velocityY": 1.1553730967106797,
+                    "timestamp": 0.17052265290541554
+                },
+                {
+                    "x": 7.4680324930779305,
+                    "y": 2.7525259224839496,
+                    "heading": 0.0020229127878395882,
+                    "angularVelocity": 0.01186304105238048,
+                    "velocityX": -1.243046060026343,
+                    "velocityY": 1.73305955059478,
+                    "timestamp": 0.2557839793581233
+                },
+                {
+                    "x": 7.326720815099078,
+                    "y": 2.949543182754876,
+                    "heading": 0.0033715247016779517,
+                    "angularVelocity": 0.015817393065211405,
+                    "velocityX": -1.657394786805044,
+                    "velocityY": 2.310745896971908,
+                    "timestamp": 0.3410453058108311
+                },
+                {
+                    "x": 7.1500812197312955,
+                    "y": 3.1958147252550657,
+                    "heading": 0.005057281636821727,
+                    "angularVelocity": 0.01977164800486184,
+                    "velocityX": -2.0717434588083226,
+                    "velocityY": 2.8884319860640817,
+                    "timestamp": 0.4263066322635389
+                },
+                {
+                    "x": 6.957409028465344,
+                    "y": 3.454481547786369,
+                    "heading": 0.005057281852786698,
+                    "angularVelocity": 2.532976909732762e-9,
+                    "velocityX": -2.2597841164587846,
+                    "velocityY": 3.0338118499096547,
+                    "timestamp": 0.5115679587162466
+                },
+                {
+                    "x": 6.719745009541024,
+                    "y": 3.6725340903222867,
+                    "heading": 0.005057282088140234,
+                    "angularVelocity": 2.760378532567681e-9,
+                    "velocityX": -2.787477380570894,
+                    "velocityY": 2.5574612970252826,
+                    "timestamp": 0.5968292851689544
+                },
+                {
+                    "x": 6.445480997294438,
+                    "y": 3.842270194716782,
+                    "heading": 0.005057282389511224,
+                    "angularVelocity": 3.534673960970524e-9,
+                    "velocityX": -3.2167457821427474,
+                    "velocityY": 1.9907748501725318,
+                    "timestamp": 0.6820906116216622
+                },
+                {
+                    "x": 6.1443030532206215,
+                    "y": 3.9576952313720115,
+                    "heading": 0.0050573451663417065,
+                    "angularVelocity": 7.362872839766494e-7,
+                    "velocityX": -3.5324097877003195,
+                    "velocityY": 1.3537795089212774,
+                    "timestamp": 0.76735193807437
+                },
+                {
+                    "x": 5.8268478532983945,
+                    "y": 4.0147328665402,
+                    "heading": 0.005057406885810731,
+                    "angularVelocity": 7.238858646916699e-7,
+                    "velocityX": -3.723319975538169,
+                    "velocityY": 0.6689742881224171,
+                    "timestamp": 0.8526132645270778
+                },
+                {
+                    "x": 5.505959202112108,
+                    "y": 4.047314610372366,
+                    "heading": 0.005057466820580134,
+                    "angularVelocity": 7.029537528496964e-7,
+                    "velocityX": -3.7635897133768776,
+                    "velocityY": 0.3821397717793548,
+                    "timestamp": 0.9378745909797855
+                },
+                {
+                    "x": 5.196832806272751,
+                    "y": 4.139357882135735,
+                    "heading": 0.005057539347948128,
+                    "angularVelocity": 8.506478964233103e-7,
+                    "velocityX": -3.625634372587421,
+                    "velocityY": 1.0795430424660248,
+                    "timestamp": 1.0231359174324932
+                },
+                {
+                    "x": 4.911616541791353,
+                    "y": 4.287091740854739,
+                    "heading": 0.008468262161445878,
+                    "angularVelocity": 0.040003163864724633,
+                    "velocityX": -3.3452008823671444,
+                    "velocityY": 1.7327182776242007,
+                    "timestamp": 1.108397243885201
+                },
+                {
+                    "x": 4.672649683479067,
+                    "y": 4.471709481894194,
+                    "heading": 0.05890642847923189,
+                    "angularVelocity": 0.5915714476464851,
+                    "velocityX": -2.8027579238375626,
+                    "velocityY": 2.165316313039838,
+                    "timestamp": 1.1936585703379088
+                },
+                {
+                    "x": 4.485112052095124,
+                    "y": 4.685980588264911,
+                    "heading": 0.1533943172973333,
+                    "angularVelocity": 1.1082150929337078,
+                    "velocityX": -2.1995626761432403,
+                    "velocityY": 2.513110167122247,
+                    "timestamp": 1.2789198967906166
+                },
+                {
+                    "x": 4.353217601776123,
+                    "y": 4.923890113830566,
+                    "heading": 0.2635187752392858,
+                    "angularVelocity": 1.291610892363634,
+                    "velocityX": -1.5469434479432043,
+                    "velocityY": 2.7903568413003317,
+                    "timestamp": 1.3641812232433244
+                },
+                {
+                    "x": 4.2800604900800625,
+                    "y": 5.16873168779887,
+                    "heading": 0.3704891219101591,
+                    "angularVelocity": 1.3052114311350342,
+                    "velocityX": -0.8926352155228909,
+                    "velocityY": 2.987463639297507,
+                    "timestamp": 1.4461375592962549
+                },
+                {
+                    "x": 4.26199164107549,
+                    "y": 5.42343977988949,
+                    "heading": 0.46980573799025893,
+                    "angularVelocity": 1.2118235253560785,
+                    "velocityX": -0.22046921415441664,
+                    "velocityY": 3.107851135829132,
+                    "timestamp": 1.5280938953491854
+                },
+                {
+                    "x": 4.2996600311730235,
+                    "y": 5.679753559001703,
+                    "heading": 0.550426603173402,
+                    "angularVelocity": 0.9837050930723119,
+                    "velocityX": 0.45961535021819666,
+                    "velocityY": 3.127443117352554,
+                    "timestamp": 1.610050231402116
+                },
+                {
+                    "x": 4.391931306983731,
+                    "y": 5.927283560440951,
+                    "heading": 0.5996302346047028,
+                    "angularVelocity": 0.6003639718722656,
+                    "velocityX": 1.1258589665496648,
+                    "velocityY": 3.0202668074301977,
+                    "timestamp": 1.6920065674550464
+                },
+                {
+                    "x": 4.533405002431738,
+                    "y": 6.1528417215915425,
+                    "heading": 0.5943564820218653,
+                    "angularVelocity": -0.06434832053139185,
+                    "velocityX": 1.7262081525536632,
+                    "velocityY": 2.752174780055045,
+                    "timestamp": 1.773962903507977
+                },
+                {
+                    "x": 4.71098037645994,
+                    "y": 6.338671203945781,
+                    "heading": 0.5303639205613101,
+                    "angularVelocity": -0.7808128638054942,
+                    "velocityX": 2.166707085507742,
+                    "velocityY": 2.2674205717817246,
+                    "timestamp": 1.8559192395609074
+                },
+                {
+                    "x": 4.8864330916758485,
+                    "y": 6.469038507428809,
+                    "heading": 0.43960837441640055,
+                    "angularVelocity": -1.107364610427659,
+                    "velocityX": 2.1408072110807552,
+                    "velocityY": 1.5906921875921145,
+                    "timestamp": 1.937875575613838
+                },
+                {
+                    "x": 5.028345407892232,
+                    "y": 6.554592883552385,
+                    "heading": 0.3555321434700494,
+                    "angularVelocity": -1.0258661501483803,
+                    "velocityX": 1.7315600361235026,
+                    "velocityY": 1.0439019146530064,
+                    "timestamp": 2.0198319116667687
+                },
+                {
+                    "x": 5.126173645194142,
+                    "y": 6.6061448414321,
+                    "heading": 0.29324170563915075,
+                    "angularVelocity": -0.760044199524953,
+                    "velocityX": 1.1936628943333356,
+                    "velocityY": 0.6290173568329146,
+                    "timestamp": 2.1017882477196994
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 0.26,
+                    "angularVelocity": -0.405602632332653,
+                    "velocityX": 0.6079622053097948,
+                    "velocityY": 0.29107155976928517,
+                    "timestamp": 2.18374458377263
+                },
+                {
+                    "x": 5.176,
+                    "y": 6.63,
+                    "heading": 0.26,
+                    "angularVelocity": 0,
+                    "velocityX": 1.6766347007166246e-32,
+                    "velocityY": 1.3162553729765107e-31,
+                    "timestamp": 2.265700919825561
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
+        },
+        "C4 To S2": {
+            "waypoints": [
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 12
+                },
+                {
+                    "x": 5.581140041351318,
+                    "y": 4.144257068634033,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 10
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
+                {
+                    "x": 7.68,
+                    "y": 2.457,
+                    "heading": 0,
+                    "angularVelocity": -1.8537798939082814e-32,
+                    "velocityX": 2.7477856809061307e-34,
+                    "velocityY": 6.925951302123011e-34,
+                    "timestamp": 0
+                },
+                {
+                    "x": 7.6474171775045905,
+                    "y": 2.4907449431726314,
+                    "heading": -0.04287864331544942,
+                    "angularVelocity": -0.5625230567182208,
+                    "velocityX": -0.42745263118016213,
+                    "velocityY": 0.4426984418000905,
+                    "timestamp": 0.07622557476240162
+                },
+                {
+                    "x": 7.582037622461459,
+                    "y": 2.5583409566544844,
+                    "heading": -0.12553871024043178,
+                    "angularVelocity": -1.084413822823079,
+                    "velocityX": -0.8577115390329612,
+                    "velocityY": 0.8867891608892778,
+                    "timestamp": 0.15245114952480324
+                },
+                {
+                    "x": 7.483606625879984,
+                    "y": 2.659997722482693,
+                    "heading": -0.2429013405408753,
+                    "angularVelocity": -1.5396752424139517,
+                    "velocityX": -1.2913119630555505,
+                    "velocityY": 1.333630689503849,
+                    "timestamp": 0.22867672428720487
+                },
+                {
+                    "x": 7.3517522665520065,
+                    "y": 2.7960803575645476,
+                    "heading": -0.3842395530561246,
+                    "angularVelocity": -1.8542098627108627,
+                    "velocityX": -1.7297916052318956,
+                    "velocityY": 1.785262170944986,
+                    "timestamp": 0.3049022990496065
+                },
+                {
+                    "x": 7.186045392769314,
+                    "y": 2.9668045775727996,
+                    "heading": -0.5218107815453775,
+                    "angularVelocity": -1.8047909631126855,
+                    "velocityX": -2.173901270002986,
+                    "velocityY": 2.2397236169147496,
+                    "timestamp": 0.3811278738120081
+                },
+                {
+                    "x": 6.990773522032128,
+                    "y": 3.1645972581511623,
+                    "heading": -0.5514162056749212,
+                    "angularVelocity": -0.38839227151549954,
+                    "velocityX": -2.5617631791673237,
+                    "velocityY": 2.59483357383517,
+                    "timestamp": 0.4573534485744097
+                },
+                {
+                    "x": 6.791088913724043,
+                    "y": 3.3726253897030523,
+                    "heading": -0.5514162553808045,
+                    "angularVelocity": -6.520893213469087e-7,
+                    "velocityX": -2.619653691435051,
+                    "velocityY": 2.729112009982516,
+                    "timestamp": 0.5335790233368113
+                },
+                {
+                    "x": 6.578307530314685,
+                    "y": 3.567237130643777,
+                    "heading": -0.5514163004289083,
+                    "angularVelocity": -5.90984115130558e-7,
+                    "velocityX": -2.7914697143656473,
+                    "velocityY": 2.55310296507882,
+                    "timestamp": 0.609804598099213
+                },
+                {
+                    "x": 6.335948512349157,
+                    "y": 3.7234797461898013,
+                    "heading": -0.551416294920752,
+                    "angularVelocity": 7.226126339733625e-8,
+                    "velocityX": -3.179497415676709,
+                    "velocityY": 2.0497400778287207,
+                    "timestamp": 0.6860301728616146
+                },
+                {
+                    "x": 6.088743524617576,
+                    "y": 3.871936280628758,
+                    "heading": -0.5514162894475348,
+                    "angularVelocity": 7.180289728327137e-8,
+                    "velocityX": -3.2430714822699573,
+                    "velocityY": 1.9475948184280927,
+                    "timestamp": 0.7622557476240163
+                },
+                {
+                    "x": 5.841537785943911,
+                    "y": 4.020391564616265,
+                    "heading": -0.5514162839746023,
+                    "angularVelocity": 7.179916458555982e-8,
+                    "velocityX": -3.2430813338464923,
+                    "velocityY": 1.947578413809903,
+                    "timestamp": 0.8384813223864179
+                },
+                {
+                    "x": 5.581140041351318,
+                    "y": 4.144257068634033,
+                    "heading": -0.5514162991479159,
+                    "angularVelocity": -1.9905804152585698e-7,
+                    "velocityX": -3.416146685731968,
+                    "velocityY": 1.6249861598795787,
+                    "timestamp": 0.9147068971488196
+                },
+                {
+                    "x": 5.261335999197585,
+                    "y": 4.231658536911035,
+                    "heading": -0.5514164840698423,
+                    "angularVelocity": -0.000002110046686592666,
+                    "velocityX": -3.6491154543506057,
+                    "velocityY": 0.9972921119903242,
+                    "timestamp": 1.0023456814389826
+                },
+                {
+                    "x": 4.96430123021485,
+                    "y": 4.378912897735688,
+                    "heading": -0.551417377431188,
+                    "angularVelocity": -0.000010193675698949444,
+                    "velocityX": -3.3893072728996803,
+                    "velocityY": 1.6802419387415284,
+                    "timestamp": 1.0899844657291458
+                },
+                {
+                    "x": 4.714895393240272,
+                    "y": 4.564810737884653,
+                    "heading": -0.6189140340648516,
+                    "angularVelocity": -0.7701687920520328,
+                    "velocityX": -2.8458386203626582,
+                    "velocityY": 2.121182324180531,
+                    "timestamp": 1.177623250019309
+                },
+                {
+                    "x": 4.521287798442847,
+                    "y": 4.7820097530438215,
+                    "heading": -0.6965700023585185,
+                    "angularVelocity": -0.8860913455457793,
+                    "velocityX": -2.209154273026079,
+                    "velocityY": 2.4783435429688825,
+                    "timestamp": 1.265262034309472
+                },
+                {
+                    "x": 4.386338131009024,
+                    "y": 5.001818806087968,
+                    "heading": -0.6848521841568085,
+                    "angularVelocity": 0.1337058506301677,
+                    "velocityX": -1.539839564490294,
+                    "velocityY": 2.5081253103235808,
+                    "timestamp": 1.3529008185996352
+                },
+                {
+                    "x": 4.301033328473477,
+                    "y": 5.211510806429668,
+                    "heading": -0.519384386353178,
+                    "angularVelocity": 1.888065873389841,
+                    "velocityX": -0.9733681637243018,
+                    "velocityY": 2.3926849515327757,
+                    "timestamp": 1.4405396028897983
+                },
+                {
+                    "x": 4.267176527868507,
+                    "y": 5.385808771912563,
+                    "heading": -0.30251041793580363,
+                    "angularVelocity": 2.474634605830756,
+                    "velocityX": -0.38632211616348305,
+                    "velocityY": 1.988822265103685,
+                    "timestamp": 1.5281783871799615
+                },
+                {
+                    "x": 4.261077190237836,
+                    "y": 5.502861748658668,
+                    "heading": -0.11181365176971161,
+                    "angularVelocity": 2.175940340919318,
+                    "velocityX": -0.06959632861263781,
+                    "velocityY": 1.335629854911658,
+                    "timestamp": 1.6158171714701246
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 9.015876776981611e-33,
+                    "angularVelocity": 1.275846677647969,
+                    "velocityX": 0.021940169272516404,
+                    "velocityY": 0.6519744860010044,
+                    "timestamp": 1.7034559557602877
+                },
+                {
+                    "x": 4.263,
+                    "y": 5.56,
+                    "heading": 0,
+                    "angularVelocity": -9.923717904588528e-32,
+                    "velocityX": -3.152679648787722e-32,
+                    "velocityY": -1.9738855170003778e-32,
+                    "timestamp": 1.7910947400504509
+                }
+            ],
+            "constraints": [
+                {
+                    "scope": [
+                        "first"
+                    ],
+                    "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        "last"
+                    ],
+                    "type": "StopPoint"
+                }
+            ],
+            "usesControlIntervalGuessing": true,
+            "defaultControlIntervalCount": 40,
+            "usesDefaultFieldObstacles": true,
+            "circleObstacles": [
+                {
+                    "x": 3.42,
+                    "y": 4.04,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 2.81,
+                    "radius": 0.5
+                },
+                {
+                    "x": 5.61,
+                    "y": 5.44,
+                    "radius": 0.5
+                }
+            ]
         }
     },
     "splitTrajectoriesAtStopPoints": false,

From 94d1c9a7b1e80893acc39d31200ffc4f44eb0905 Mon Sep 17 00:00:00 2001
From: Stoopipoopy <anotherguywithabeanie@gmail.com>
Date: Tue, 20 Feb 2024 14:05:10 -0800
Subject: [PATCH 49/62] blacklist protection for makeNoteToNote

---
 ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 simgui.json                                   |   6 ++++++
 src/main/java/frc/robot/Robot.java            |   4 ++++
 .../robot/utils/dynamicauto/DynamicAuto.java  |  13 +++++++++----
 .../frc/robot/utils/dynamicauto/Note.java     |   7 +++++++
 7 files changed, 26 insertions(+), 4 deletions(-)

diff --git a/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat
index b3edcb2cd6bc93f38fcad6c03cc3924776588efc..4305ca4857de52a528069f155279462dcb0195d1 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx<!Fb<4hlzq#YzG(|_AxjDS&Nw10HLi3y#N3J

delta 30
kcmZn=Xb{-Yz{qxxVeiQ%(}{vsYzG()oNNHG7BR5_0IRMFyZ`_I

diff --git a/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat
index 97ee8668298d0ec797b05df4ccc61a5d3e34a1cb..4e24c67869f36d675c58f5f743de02d00b0878c9 100644
GIT binary patch
delta 55
zcmZn=Xb@nz$)MISQ80C)0`J5!FP0k&>Ifd&c?RQs{~RU?TCp8qaM;J-2xKi{Vgms7
CBN4s;

delta 55
zcmZn=Xb@nz$e`CSQ80C)0`J5!FO~}o`UoD|L5962n@lGPTCp8qIB>E7#9G9}1^@sP
B61@Nb

diff --git a/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat
index 5ad69a94f4ee1a2ee350f761cb96336784836181..ef8edd82f9de826b80c02b2135205886d8445037 100644
GIT binary patch
delta 36
ncmZn=Xb{-Yz{q))L3`g12L=X#iGt>w=NL5h{ep59F|h#v)dC8<

delta 36
ncmZn=Xb{-Yz{q)?Vd=>lQw9csiGt>w2N*V;tb=kEF|h#v*9Ho_

diff --git a/simgui.json b/simgui.json
index 181ebf07..92fc7902 100644
--- a/simgui.json
+++ b/simgui.json
@@ -17,11 +17,17 @@
       "/AdvantageKit/RealOutputs/Shooter/Mechanism2d": "Mechanism2d",
       "/FMSInfo": "FMSInfo",
       "/SmartDashboard/Auto Chooser": "String Chooser",
+      "/SmartDashboard/Blacklist Note": "Command",
+      "/SmartDashboard/Note To Note": "Command",
+      "/SmartDashboard/Note To Shooting": "Command",
       "/SmartDashboard/SendableChooser[0]": "String Chooser",
+      "/SmartDashboard/Shoot To Note": "Command",
+      "/SmartDashboard/Start To Note": "Command",
       "/SmartDashboard/VisionSystemSim-Left Camera Sim System/Sim Field": "Field2d",
       "/SmartDashboard/VisionSystemSim-Left Camera/Sim Field": "Field2d",
       "/SmartDashboard/VisionSystemSim-Right Camera Sim System/Sim Field": "Field2d",
       "/SmartDashboard/VisionSystemSim-Right Camera/Sim Field": "Field2d",
+      "/SmartDashboard/Whitelist Note": "Command",
       "/SmartDashboard/reset Kicker value": "Command",
       "/SmartDashboard/reset Pivot value": "Command"
     },
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 0a43236c..55100b5e 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -165,6 +165,10 @@ public void robotInit() {
     SmartDashboard.putData(
         "Note To Shooting",
         swerve.runChoreoTraj(() -> DynamicAuto.makeNoteToShooting(swerve::getPose)));
+    SmartDashboard.putData("Blacklist Note", Commands.runOnce(
+                () -> DynamicAuto.getAbsoluteClosestNote(swerve::getPose).blacklist(), swerve));
+    SmartDashboard.putData("Whitelist Note", Commands.runOnce(
+                () -> DynamicAuto.getAbsoluteClosestNote(swerve::getPose).whitelist(), swerve));          
   }
 
   @Override
diff --git a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
index f5a86736..5b781df8 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
@@ -69,11 +69,16 @@ public static ChoreoTrajectory makeShootingToNote(Supplier<Pose2d> startingPose)
   }
 
   public static ChoreoTrajectory makeNoteToNote(Supplier<Pose2d> startingPose) {
-    Note closestNote = getClosestNote(startingPose);
+    Note closestNote = getAbsoluteClosestNote(startingPose);
+    Note nextClosest = new Note();
+    if(!closestNote.getBlacklist()){
     closestNote.blacklist();
-    Note nextClosest = getClosestNote(startingPose);
-    Logger.recordOutput("DynamicAuto/FHFHH", nextClosest.getPose());
-    closestNote.whitelist();
+    nextClosest = getClosestNote(startingPose);
+    closestNote.whitelist();  
+    } else{
+      nextClosest = getClosestNote(startingPose);
+    }
+    
     Logger.recordOutput(
         "DynamicAuto/Path Name", closestNote.getName() + " To " + nextClosest.getName());
     return Choreo.getTrajectory(closestNote.getName() + " To " + nextClosest.getName());
diff --git a/src/main/java/frc/robot/utils/dynamicauto/Note.java b/src/main/java/frc/robot/utils/dynamicauto/Note.java
index e34d87cc..7d7cf3ef 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/Note.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/Note.java
@@ -21,6 +21,13 @@ public Note(Pose2d pose, boolean blacklisted, int priority, String name) {
     this.name = name;
   }
 
+  public Note(String name) {
+    this.pose = new Pose2d();
+    this.blacklisted = true;
+    this.priority = -1;
+    this.name = name;
+  }
+
   public Note() {
     this.pose = new Pose2d();
     this.blacklisted = true;

From 65f907a9cf4b5f00ee21fc2e9758c0057e2a19ba Mon Sep 17 00:00:00 2001
From: Stoopipoopy <anotherguywithabeanie@gmail.com>
Date: Tue, 20 Feb 2024 15:30:08 -0800
Subject: [PATCH 50/62] added uninitalized saftey and dynamic auto demo

---
 src/main/java/frc/robot/Robot.java            | 117 ++++++++++++++++--
 .../robot/utils/dynamicauto/DynamicAuto.java  |  20 +--
 .../frc/robot/utils/dynamicauto/Note.java     |   2 +
 3 files changed, 121 insertions(+), 18 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 55100b5e..2ecb4cbf 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -4,11 +4,13 @@
 
 package frc.robot;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -25,6 +27,7 @@
 import frc.robot.subsystems.swerve.GyroIOPigeon2;
 import frc.robot.subsystems.swerve.SwerveSubsystem;
 import frc.robot.utils.dynamicauto.DynamicAuto;
+import frc.robot.utils.dynamicauto.Note;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
@@ -156,19 +159,46 @@ public void robotInit() {
                 () -> DynamicAuto.getAbsoluteClosestNote(swerve::getPose).whitelist(), swerve));
 
     SmartDashboard.putData(
-        "Start To Note", swerve.runChoreoTraj(() -> DynamicAuto.makeStartToNote(swerve::getPose)));
+        "Whitelist Note",
+        Commands.runOnce(
+            () -> DynamicAuto.getAbsoluteClosestNote(swerve::getPose).whitelist(), swerve));
     SmartDashboard.putData(
-        "Note To Note", swerve.runChoreoTraj(() -> DynamicAuto.makeNoteToNote(swerve::getPose)));
+        "Blacklist All",
+        Commands.runOnce(
+            () -> {
+              for (Note note : DynamicAuto.notes) {
+                note.blacklist();
+              }
+            },
+            swerve));
     SmartDashboard.putData(
-        "Shoot To Note",
-        swerve.runChoreoTraj(() -> DynamicAuto.makeShootingToNote(swerve::getPose)));
-    SmartDashboard.putData(
-        "Note To Shooting",
-        swerve.runChoreoTraj(() -> DynamicAuto.makeNoteToShooting(swerve::getPose)));
-    SmartDashboard.putData("Blacklist Note", Commands.runOnce(
-                () -> DynamicAuto.getAbsoluteClosestNote(swerve::getPose).blacklist(), swerve));
-    SmartDashboard.putData("Whitelist Note", Commands.runOnce(
-                () -> DynamicAuto.getAbsoluteClosestNote(swerve::getPose).whitelist(), swerve));          
+        "Whitelist All",
+        Commands.runOnce(
+            () -> {
+              for (Note note : DynamicAuto.notes) {
+                note.whitelist();
+              }
+            },
+            swerve));
+    SmartDashboard.putData("Dynamic Demo", dynamicAutoDemo());
+  }
+
+  public Command dynamicAutoDemo() {
+    return Commands.sequence(
+            Commands.runOnce(
+                () -> swerve.setPose(new Pose2d(0.71, 6.72, Rotation2d.fromRadians(1.04))), swerve),
+            startToNote(),
+            blacklistNote(),
+            noteToNote(),
+            blacklistNote(),
+            noteToShoot(),
+            shootToNote(),
+            blacklistNote(),
+            noteToShoot(),
+            shootToNote(),
+            blacklistNote(),
+            noteToNote())
+        .asProxy();
   }
 
   @Override
@@ -186,6 +216,71 @@ public void robotPeriodic() {
             .getPose());
   }
 
+  public Command startToNote() {
+    return swerve
+        .runChoreoTraj(() -> DynamicAuto.makeStartToNote(swerve::getPose))
+        .onlyIf(
+            () -> {
+              if (DynamicAuto.notesLeft > 0) {
+                return true;
+              } else {
+                System.out.println("No more avalible notes!!!!! >:(");
+                return false;
+              }
+            });
+  }
+
+  public Command noteToNote() {
+    return swerve
+        .runChoreoTraj(
+            () -> {
+              System.out.println(DynamicAuto.notesLeft);
+              return DynamicAuto.makeNoteToNote(swerve::getPose);
+            })
+        .onlyIf(
+            () -> {
+              if (DynamicAuto.notesLeft > 0) {
+                return true;
+              } else {
+                System.out.println("No more avalible notes!!!!! >:(");
+                return false;
+              }
+            });
+  }
+
+  public Command shootToNote() {
+    return swerve
+        .runChoreoTraj(() -> DynamicAuto.makeShootingToNote(swerve::getPose))
+        .onlyIf(
+            () -> {
+              if (DynamicAuto.notesLeft > 0) {
+                return true;
+              } else {
+                System.out.println("No more avalible notes!!!!! >:(");
+                return false;
+              }
+            });
+  }
+
+  public Command noteToShoot() {
+    return swerve
+        .runChoreoTraj(() -> DynamicAuto.makeNoteToShooting(swerve::getPose))
+        .onlyIf(
+            () -> {
+              if (DynamicAuto.notesLeft > 0) {
+                return true;
+              } else {
+                System.out.println("No more avalible notes!!!!! >:(");
+                return false;
+              }
+            });
+  }
+
+  public Command blacklistNote() {
+    return Commands.runOnce(
+        () -> DynamicAuto.getAbsoluteClosestNote(swerve::getPose).blacklist(), swerve);
+  }
+
   @Override
   public void disabledPeriodic() {}
 
diff --git a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
index 5b781df8..ec431b17 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
@@ -36,6 +36,8 @@ public class DynamicAuto {
         new Pose2d(0.71, 4.36, Rotation2d.fromRadians(-1.04)), "Stage Side") // Speaker (Stage Side)
   };
 
+  public static int notesLeft = notes.length;
+
   public static ShootingLocation closestShootingLocation(
       Supplier<Pose2d> curPose, ShootingLocation[] shootingLocations) {
 
@@ -59,6 +61,10 @@ public static ChoreoTrajectory makeStartToNote(Supplier<Pose2d> startingPose) {
   public static ChoreoTrajectory makeNoteToShooting(Supplier<Pose2d> startingPose) {
     Note closestNote = getClosestNote(startingPose);
     ShootingLocation startingLocation = closestShootingLocation(startingPose, shootingLocations);
+    if ("Uninitialized" == closestNote.getName()) {
+      closestNote = getAbsoluteClosestNote(startingPose);
+      System.out.println("No more avaliable notes!");
+    }
     return Choreo.getTrajectory(closestNote.getName() + " To " + startingLocation.getName());
   }
 
@@ -71,14 +77,14 @@ public static ChoreoTrajectory makeShootingToNote(Supplier<Pose2d> startingPose)
   public static ChoreoTrajectory makeNoteToNote(Supplier<Pose2d> startingPose) {
     Note closestNote = getAbsoluteClosestNote(startingPose);
     Note nextClosest = new Note();
-    if(!closestNote.getBlacklist()){
-    closestNote.blacklist();
-    nextClosest = getClosestNote(startingPose);
-    closestNote.whitelist();  
-    } else{
+    if (!closestNote.getBlacklist()) {
+      closestNote.blacklist();
+      nextClosest = getClosestNote(startingPose);
+      closestNote.whitelist();
+    } else {
       nextClosest = getClosestNote(startingPose);
     }
-    
+
     Logger.recordOutput(
         "DynamicAuto/Path Name", closestNote.getName() + " To " + nextClosest.getName());
     return Choreo.getTrajectory(closestNote.getName() + " To " + nextClosest.getName());
@@ -99,7 +105,7 @@ public static Note getAbsoluteClosestNote(Supplier<Pose2d> curPose) {
 
   public static Note getClosestNote(Supplier<Pose2d> curPose) {
     Note closestNote = new Note();
-    double shortestDistance = 99999999;
+    double shortestDistance = Double.POSITIVE_INFINITY;
     for (Note note : notes) {
       if (note.getBlacklist()) {
 
diff --git a/src/main/java/frc/robot/utils/dynamicauto/Note.java b/src/main/java/frc/robot/utils/dynamicauto/Note.java
index 7d7cf3ef..4c94d8ad 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/Note.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/Note.java
@@ -37,11 +37,13 @@ public Note() {
 
   public void blacklist() {
     System.out.println("Blacklisted " + name);
+    DynamicAuto.notesLeft -= 1;
     this.blacklisted = true;
   }
 
   public void whitelist() {
     System.out.println("Whitelisted " + name);
+    DynamicAuto.notesLeft += 1;
     this.blacklisted = false;
   }
 

From a9cfeb9341b1dbabb4e1d6b9f70a89917eaee101 Mon Sep 17 00:00:00 2001
From: Stoopipoopy <anotherguywithabeanie@gmail.com>
Date: Tue, 20 Feb 2024 17:07:10 -0800
Subject: [PATCH 51/62] added a test command

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

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 2ecb4cbf..80354c6e 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -181,6 +181,10 @@ public void robotInit() {
             },
             swerve));
     SmartDashboard.putData("Dynamic Demo", dynamicAutoDemo());
+    SmartDashboard.putData("gloob", Commands.sequence(Commands.runOnce(
+                () -> swerve.setPose(new Pose2d(0.71, 6.72, Rotation2d.fromRadians(1.04))), swerve),startToNote(), blacklistNote(), noteToShoot(), noteToNote()));
+    SmartDashboard.putData("gleeb", Commands.sequence(Commands.runOnce(
+                () -> swerve.setPose(new Pose2d(0.71, 6.72, Rotation2d.fromRadians(1.04))), swerve),startToNote(), blacklistNote(), noteToNote()));
   }
 
   public Command dynamicAutoDemo() {

From 413b35412d95f5ae1a709797b3d4a5586bce16ca Mon Sep 17 00:00:00 2001
From: Stoopipoopy <anotherguywithabeanie@gmail.com>
Date: Wed, 21 Feb 2024 12:50:20 -0800
Subject: [PATCH 52/62] added more uninitialized protection and updateWhitelist

---
 src/main/deploy/choreo/C4 To S4.traj          | 354 +++++++++---------
 src/main/java/frc/robot/Robot.java            |  39 +-
 .../subsystems/swerve/SwerveSubsystem.java    |   6 +-
 .../robot/utils/dynamicauto/DynamicAuto.java  |  23 +-
 .../frc/robot/utils/dynamicauto/Note.java     |   4 +-
 .../utils/dynamicauto/ShootingLocation.java   |   4 +
 6 files changed, 244 insertions(+), 186 deletions(-)

diff --git a/src/main/deploy/choreo/C4 To S4.traj b/src/main/deploy/choreo/C4 To S4.traj
index e5689e4c..1d7c5583 100644
--- a/src/main/deploy/choreo/C4 To S4.traj	
+++ b/src/main/deploy/choreo/C4 To S4.traj	
@@ -4,190 +4,208 @@
             "x": 7.68,
             "y": 2.457,
             "heading": 0,
-            "angularVelocity": 1.0550386775092505e-35,
+            "angularVelocity": -6.19176557444807e-32,
             "velocityX": 0,
             "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.6504501923310215,
-            "y": 2.437078665707293,
-            "heading": -0.0029944704440250615,
-            "angularVelocity": -0.0457972069493345,
-            "velocityX": -0.45193254781641656,
-            "velocityY": -0.30467539632270557,
-            "timestamp": 0.0653854381848624
-        },
-        {
-            "x": 7.5913505770823395,
-            "y": 2.3972359988448533,
-            "heading": -0.008983367804687599,
-            "angularVelocity": -0.09159374819405963,
-            "velocityX": -0.9038650942674934,
-            "velocityY": -0.609350766294381,
-            "timestamp": 0.1307708763697248
-        },
-        {
-            "x": 7.50270115478166,
-            "y": 2.337472001372052,
-            "heading": -0.017966623739921285,
-            "angularVelocity": -0.13738924422033513,
-            "velocityX": -1.3557976326478658,
-            "velocityY": -0.91402610629957,
-            "timestamp": 0.19615631455458718
-        },
-        {
-            "x": 7.384501926877228,
-            "y": 2.2577866758321745,
-            "heading": -0.02994413949472175,
-            "angularVelocity": -0.18318322989496186,
-            "velocityX": -1.8077301488788986,
-            "velocityY": -1.2187014074079496,
-            "timestamp": 0.2615417527394496
-        },
-        {
-            "x": 7.2367528971895485,
-            "y": 2.1581800263627264,
-            "heading": -0.044915766222073404,
-            "angularVelocity": -0.2289749391144676,
-            "velocityX": -2.25966260667937,
-            "velocityY": -1.5233766452376303,
-            "timestamp": 0.326927190924312
-        },
-        {
-            "x": 7.059454079201559,
-            "y": 2.0386520636124454,
-            "heading": -0.06288119646669635,
-            "angularVelocity": -0.2747619461359239,
-            "velocityX": -2.711594858272837,
-            "velocityY": -1.82805172020631,
-            "timestamp": 0.3923126291091744
-        },
-        {
-            "x": 6.854358935700705,
-            "y": 1.900385597577332,
-            "heading": -0.06288119947800026,
-            "angularVelocity": -4.6054656889756363e-8,
-            "velocityX": -3.136709781174733,
-            "velocityY": -2.114636987584247,
-            "timestamp": 0.45769806729403684
-        },
-        {
-            "x": 6.636845838072133,
-            "y": 1.782615967982618,
-            "heading": -0.06288120256062167,
-            "angularVelocity": -4.714538109405926e-8,
-            "velocityX": -3.3266290425951324,
-            "velocityY": -1.801159904469057,
-            "timestamp": 0.5230835054788993
-        },
-        {
-            "x": 6.4046630859375,
-            "y": 1.6973352432250977,
-            "heading": -0.06288120593828476,
-            "angularVelocity": -5.1657726572043936e-8,
-            "velocityX": -3.5509856411482486,
-            "velocityY": -1.3042770244409516,
-            "timestamp": 0.5884689436637617
-        },
-        {
-            "x": 6.207746736822867,
-            "y": 1.6503996785686041,
-            "heading": -0.06288121580879107,
-            "angularVelocity": -1.8445407171610623e-7,
-            "velocityX": -3.679854027466254,
-            "velocityY": -0.8771035386810692,
-            "timestamp": 0.6419809437716079
-        },
-        {
-            "x": 6.009908768236521,
-            "y": 1.6271742750630454,
-            "heading": -0.0724170210773865,
-            "angularVelocity": -0.17819938050114592,
-            "velocityX": -3.697076696584537,
-            "velocityY": -0.43402234001253925,
-            "timestamp": 0.695492943879454
-        },
-        {
-            "x": 5.8260492909953046,
-            "y": 1.6186155743000838,
-            "heading": -0.12620190803666176,
-            "angularVelocity": -1.0050995449783064,
-            "velocityX": -3.43585507681777,
-            "velocityY": -0.1599398405163814,
-            "timestamp": 0.7490049439873002
-        },
-        {
-            "x": 5.6625912601386625,
-            "y": 1.614879890442449,
-            "heading": -0.22199295256207294,
-            "angularVelocity": -1.7900852954918034,
-            "velocityX": -3.0546051451490293,
-            "velocityY": -0.06981020799271546,
-            "timestamp": 0.8025169440951464
-        },
-        {
-            "x": 5.5227239807129855,
-            "y": 1.6141995251565013,
-            "heading": -0.32512734843535046,
-            "angularVelocity": -1.9273134187738115,
-            "velocityX": -2.61375540334492,
-            "velocityY": -0.012714256327113658,
-            "timestamp": 0.8560289442029926
-        },
-        {
-            "x": 5.406619958232834,
-            "y": 1.6150039384726416,
-            "heading": -0.42188556792063076,
-            "angularVelocity": -1.8081592781110263,
-            "velocityX": -2.169681982474204,
-            "velocityY": 0.015032391136922594,
-            "timestamp": 0.9095409443108388
-        },
-        {
-            "x": 5.314084878105213,
-            "y": 1.6164421457440925,
-            "heading": -0.5051954003859285,
-            "angularVelocity": -1.5568439284160218,
-            "velocityX": -1.7292397955809797,
-            "velocityY": 0.026876350511144828,
-            "timestamp": 0.9630529444186849
-        },
-        {
-            "x": 5.244912091070787,
-            "y": 1.617980616061371,
-            "heading": -0.5709381839957368,
-            "angularVelocity": -1.2285615091439794,
-            "velocityX": -1.2926593454742539,
-            "velocityY": 0.028750005871169333,
-            "timestamp": 1.016564944526531
-        },
-        {
-            "x": 5.198930392622704,
-            "y": 1.6192554664106382,
-            "heading": -0.6164892293058035,
-            "angularVelocity": -0.851230475748707,
-            "velocityX": -0.8592782619863396,
-            "velocityY": 0.023823634823929046,
-            "timestamp": 1.0700769446343772
+            "x": 7.676458612092857,
+            "y": 2.4118388742702104,
+            "heading": -0.005126526914020665,
+            "angularVelocity": -0.06953454590838633,
+            "velocityX": -0.04803423529000399,
+            "velocityY": -0.6125508405591464,
+            "timestamp": 0.07372633051742336
+        },
+        {
+            "x": 7.666722992989933,
+            "y": 2.3218088358271687,
+            "heading": -0.015946167020864214,
+            "angularVelocity": -0.1467540840688735,
+            "velocityX": -0.1320507752738741,
+            "velocityY": -1.221138198676054,
+            "timestamp": 0.14745266103484672
+        },
+        {
+            "x": 7.646538544706082,
+            "y": 2.1877155788672784,
+            "heading": -0.03333492073697428,
+            "angularVelocity": -0.23585540734325022,
+            "velocityX": -0.2737753003871665,
+            "velocityY": -1.8187973824114398,
+            "timestamp": 0.22117899155227008
+        },
+        {
+            "x": 7.6083275574144436,
+            "y": 2.012099664253373,
+            "heading": -0.058740519234419,
+            "angularVelocity": -0.34459328599625155,
+            "velocityX": -0.5182814202669078,
+            "velocityY": -2.3819972238059934,
+            "timestamp": 0.29490532206969344
+        },
+        {
+            "x": 7.537878490375706,
+            "y": 1.8047562055419635,
+            "heading": -0.09435741731352677,
+            "angularVelocity": -0.4830960367773968,
+            "velocityX": -0.9555482626670088,
+            "velocityY": -2.812339326482677,
+            "timestamp": 0.36863165258711683
+        },
+        {
+            "x": 7.422206286264909,
+            "y": 1.5965261152652936,
+            "heading": -0.14021536920865627,
+            "angularVelocity": -0.6220023643288746,
+            "velocityX": -1.5689402049307317,
+            "velocityY": -2.8243653090459993,
+            "timestamp": 0.4423579831045402
+        },
+        {
+            "x": 7.269711858201117,
+            "y": 1.4146723813359454,
+            "heading": -0.19176235416262147,
+            "angularVelocity": -0.6991665608772336,
+            "velocityX": -2.068384890358183,
+            "velocityY": -2.4666049788870437,
+            "timestamp": 0.5160843136219636
+        },
+        {
+            "x": 7.089538504757627,
+            "y": 1.268699285071451,
+            "heading": -0.24604454859029715,
+            "angularVelocity": -0.7362660537519551,
+            "velocityX": -2.4438128437832747,
+            "velocityY": -1.9799316640341589,
+            "timestamp": 0.589810644139387
+        },
+        {
+            "x": 6.887025833129883,
+            "y": 1.1621593236923218,
+            "heading": -0.30153109883638657,
+            "angularVelocity": -0.7526015448846538,
+            "velocityX": -2.746816099573618,
+            "velocityY": -1.4450734307731645,
+            "timestamp": 0.6635369746568104
+        },
+        {
+            "x": 6.705802863807946,
+            "y": 1.102049450480248,
+            "heading": -0.3478019503216008,
+            "angularVelocity": -0.7562242219318657,
+            "velocityX": -2.9618041287927626,
+            "velocityY": -0.982401244869012,
+            "timestamp": 0.7247236573142499
+        },
+        {
+            "x": 6.5128639776244945,
+            "y": 1.0708739549800326,
+            "heading": -0.3939137418380571,
+            "angularVelocity": -0.7536246371554138,
+            "velocityX": -3.153282345173718,
+            "velocityY": -0.5095143934302662,
+            "timestamp": 0.7859103399716895
+        },
+        {
+            "x": 6.310456930732261,
+            "y": 1.0694436089370258,
+            "heading": -0.43928747590731515,
+            "angularVelocity": -0.741562250127008,
+            "velocityX": -3.308024525948419,
+            "velocityY": -0.023376754236126687,
+            "timestamp": 0.847097022629129
+        },
+        {
+            "x": 6.102475562929418,
+            "y": 1.0987242176578527,
+            "heading": -0.48296228626459325,
+            "angularVelocity": -0.7137960167214212,
+            "velocityX": -3.399128025411204,
+            "velocityY": 0.47854545219843847,
+            "timestamp": 0.9082837052865685
+        },
+        {
+            "x": 5.896578065500401,
+            "y": 1.1591365694762163,
+            "heading": -0.523188323608364,
+            "angularVelocity": -0.6574312513227849,
+            "velocityX": -3.3650704448508275,
+            "velocityY": 0.9873447814876488,
+            "timestamp": 0.9694703879440081
+        },
+        {
+            "x": 5.707810116262367,
+            "y": 1.2455849986940297,
+            "heading": -0.5570591599834606,
+            "angularVelocity": -0.5535654966739442,
+            "velocityX": -3.0851149472324435,
+            "velocityY": 1.4128634772014765,
+            "timestamp": 1.0306570706014475
+        },
+        {
+            "x": 5.549198847906331,
+            "y": 1.3398221505031715,
+            "heading": -0.5834741067263961,
+            "angularVelocity": -0.431710718667697,
+            "velocityX": -2.5922514747864094,
+            "velocityY": 1.5401578859363727,
+            "timestamp": 1.091843753258887
+        },
+        {
+            "x": 5.421048809950807,
+            "y": 1.4274575794538809,
+            "heading": -0.6037133289661716,
+            "angularVelocity": -0.3307782242924832,
+            "velocityX": -2.094410619921754,
+            "velocityY": 1.4322631191062654,
+            "timestamp": 1.1530304359163264
+        },
+        {
+            "x": 5.321165420085697,
+            "y": 1.5019229236562417,
+            "heading": -0.6188925850446988,
+            "angularVelocity": -0.24808104344388057,
+            "velocityX": -1.6324367579186874,
+            "velocityY": 1.2170188179552628,
+            "timestamp": 1.2142171185737658
+        },
+        {
+            "x": 5.247799603622136,
+            "y": 1.559962853683462,
+            "heading": -0.6297183288548622,
+            "angularVelocity": -0.1769297392828535,
+            "velocityX": -1.19904876808419,
+            "velocityY": 0.9485712822864297,
+            "timestamp": 1.2754038012312052
+        },
+        {
+            "x": 5.199709983819949,
+            "y": 1.599716552093825,
+            "heading": -0.6366489788204296,
+            "angularVelocity": -0.11327056255638758,
+            "velocityX": -0.7859491267310955,
+            "velocityY": 0.6497116150736374,
+            "timestamp": 1.3365904838886447
         },
         {
             "x": 5.176,
             "y": 1.62,
             "heading": -0.64,
-            "angularVelocity": -0.4393551100092281,
-            "velocityX": -0.42850935447171357,
-            "velocityY": 0.013913394899485362,
-            "timestamp": 1.1235889447422234
+            "angularVelocity": -0.054767165566589775,
+            "velocityX": -0.3875023581960166,
+            "velocityY": 0.3315010231839871,
+            "timestamp": 1.397777166546084
         },
         {
             "x": 5.176,
             "y": 1.62,
             "heading": -0.64,
-            "angularVelocity": 0,
+            "angularVelocity": 7.169943122382087e-31,
             "velocityX": 0,
-            "velocityY": 6.2403191565519e-36,
-            "timestamp": 1.1771009448500696
+            "velocityY": -4.0382085379705675e-33,
+            "timestamp": 1.4589638492035235
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 80354c6e..93012b00 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -181,10 +181,23 @@ public void robotInit() {
             },
             swerve));
     SmartDashboard.putData("Dynamic Demo", dynamicAutoDemo());
-    SmartDashboard.putData("gloob", Commands.sequence(Commands.runOnce(
-                () -> swerve.setPose(new Pose2d(0.71, 6.72, Rotation2d.fromRadians(1.04))), swerve),startToNote(), blacklistNote(), noteToShoot(), noteToNote()));
-    SmartDashboard.putData("gleeb", Commands.sequence(Commands.runOnce(
-                () -> swerve.setPose(new Pose2d(0.71, 6.72, Rotation2d.fromRadians(1.04))), swerve),startToNote(), blacklistNote(), noteToNote()));
+    SmartDashboard.putData(
+        "gloob",
+        Commands.sequence(
+            Commands.runOnce(
+                () -> swerve.setPose(new Pose2d(0.71, 6.72, Rotation2d.fromRadians(1.04))), swerve),
+            startToNote(),
+            blacklistNote(),
+            noteToShoot(),
+            noteToNote()));
+    SmartDashboard.putData(
+        "gleeb",
+        Commands.sequence(
+            Commands.runOnce(
+                () -> swerve.setPose(new Pose2d(0.71, 6.72, Rotation2d.fromRadians(1.04))), swerve),
+            startToNote(),
+            blacklistNote(),
+            noteToNote()));
   }
 
   public Command dynamicAutoDemo() {
@@ -195,6 +208,14 @@ public Command dynamicAutoDemo() {
             blacklistNote(),
             noteToNote(),
             blacklistNote(),
+            noteToNote(),
+            blacklistNote(),
+            noteToNote(),
+            blacklistNote(),
+            noteToNote(),
+            blacklistNote(),
+            noteToNote(),
+            blacklistNote(),
             noteToShoot(),
             shootToNote(),
             blacklistNote(),
@@ -225,7 +246,7 @@ public Command startToNote() {
         .runChoreoTraj(() -> DynamicAuto.makeStartToNote(swerve::getPose))
         .onlyIf(
             () -> {
-              if (DynamicAuto.notesLeft > 0) {
+              if (DynamicAuto.whitelistCount > 0) {
                 return true;
               } else {
                 System.out.println("No more avalible notes!!!!! >:(");
@@ -238,12 +259,12 @@ public Command noteToNote() {
     return swerve
         .runChoreoTraj(
             () -> {
-              System.out.println(DynamicAuto.notesLeft);
+              System.out.println(DynamicAuto.whitelistCount);
               return DynamicAuto.makeNoteToNote(swerve::getPose);
             })
         .onlyIf(
             () -> {
-              if (DynamicAuto.notesLeft > 0) {
+              if (DynamicAuto.whitelistCount > 0) {
                 return true;
               } else {
                 System.out.println("No more avalible notes!!!!! >:(");
@@ -257,7 +278,7 @@ public Command shootToNote() {
         .runChoreoTraj(() -> DynamicAuto.makeShootingToNote(swerve::getPose))
         .onlyIf(
             () -> {
-              if (DynamicAuto.notesLeft > 0) {
+              if (DynamicAuto.whitelistCount > 0) {
                 return true;
               } else {
                 System.out.println("No more avalible notes!!!!! >:(");
@@ -271,7 +292,7 @@ public Command noteToShoot() {
         .runChoreoTraj(() -> DynamicAuto.makeNoteToShooting(swerve::getPose))
         .onlyIf(
             () -> {
-              if (DynamicAuto.notesLeft > 0) {
+              if (DynamicAuto.whitelistCount > 0) {
                 return true;
               } else {
                 System.out.println("No more avalible notes!!!!! >:(");
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
index a74538dc..6f7c7413 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
@@ -424,9 +424,9 @@ public Command runChoreoTraj(Supplier<ChoreoTrajectory> traj) {
             Choreo.choreoSwerveCommand(
                 traj.get(),
                 this::getPose,
-                new PIDController(5.0, 0.0, 0.0),
-                new PIDController(5.0, 0.0, 0.0),
-                new PIDController(5.0, 0.0, 0.0),
+                new PIDController(85.0, 0.0, 0.0),
+                new PIDController(85.0, 0.0, 0.0),
+                new PIDController(6.0, 0.0, 0.70),
                 (ChassisSpeeds speeds) -> //
                 this.runVelocity(speeds),
                 () -> {
diff --git a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
index ec431b17..cc44188e 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
@@ -13,11 +13,11 @@ public class DynamicAuto {
     new Note(new Pose2d(2.204, 7.0, Rotation2d.fromRadians(Math.PI)), false, 0, "W1"), // w1
     new Note(new Pose2d(2.204, 5.5, Rotation2d.fromRadians(Math.PI)), false, 1, "W2"), // w2
     new Note(new Pose2d(2.204, 4.1, Rotation2d.fromRadians(Math.PI)), false, 2, "W3"), // w3
-    new Note(new Pose2d(7.538, 7.3, Rotation2d.fromRadians(Math.PI)), false, 0, "C1"), // c1
+    new Note(new Pose2d(7.538, 7.3, Rotation2d.fromRadians(Math.PI)), false, 3, "C1"), // c1
     new Note(new Pose2d(7.538, 5.7, Rotation2d.fromRadians(Math.PI)), false, 0, "C2"), // c2
     new Note(new Pose2d(7.538, 4.1, Rotation2d.fromRadians(Math.PI)), false, 1, "C3"), // c3
     new Note(new Pose2d(7.538, 2.5, Rotation2d.fromRadians(Math.PI)), false, 2, "C4"), // c4
-    new Note(new Pose2d(7.538, 0.7, Rotation2d.fromRadians(Math.PI)), false, 3, "C5") // c5
+    new Note(new Pose2d(7.538, 0.7, Rotation2d.fromRadians(Math.PI)), false, 0, "C5") // c5
   };
 
   public static final ShootingLocation[] shootingLocations = {
@@ -36,12 +36,19 @@ public class DynamicAuto {
         new Pose2d(0.71, 4.36, Rotation2d.fromRadians(-1.04)), "Stage Side") // Speaker (Stage Side)
   };
 
-  public static int notesLeft = notes.length;
+  public static int whitelistCount = notes.length;
+
+  public static void updateWhitelistCount(){
+    whitelistCount = 0;
+    for(Note note : notes){
+      whitelistCount += note.getBlacklist() ? 1 : 0;
+    }
+  }
 
   public static ShootingLocation closestShootingLocation(
       Supplier<Pose2d> curPose, ShootingLocation[] shootingLocations) {
 
-    ShootingLocation closestLocation = new ShootingLocation();
+    ShootingLocation closestLocation = new ShootingLocation("shooting shooting");
     double shortestDistance = 9999999;
     for (ShootingLocation location : shootingLocations) {
       if (shortestDistance > curPose.get().minus(location.getPose()).getTranslation().getNorm()) {
@@ -55,6 +62,10 @@ public static ShootingLocation closestShootingLocation(
   public static ChoreoTrajectory makeStartToNote(Supplier<Pose2d> startingPose) {
     ShootingLocation startingLocation = closestShootingLocation(startingPose, startingLocations);
     Note closestNote = getClosestNote(startingPose);
+    if ("Uninitialized" == closestNote.getName()) {
+      closestNote = getAbsoluteClosestNote(startingPose);
+      System.out.println("No more avaliable notes!");
+    }
     return Choreo.getTrajectory(startingLocation.getName() + " To " + closestNote.getName());
   }
 
@@ -71,6 +82,10 @@ public static ChoreoTrajectory makeNoteToShooting(Supplier<Pose2d> startingPose)
   public static ChoreoTrajectory makeShootingToNote(Supplier<Pose2d> startingPose) {
     Note closestNote = getClosestNote(startingPose);
     ShootingLocation startingLocation = closestShootingLocation(startingPose, shootingLocations);
+    if ("Uninitialized" == closestNote.getName()) {
+      closestNote = getAbsoluteClosestNote(startingPose);
+      System.out.println("No more avaliable notes!");
+    }
     return Choreo.getTrajectory(startingLocation.getName() + " To " + closestNote.getName());
   }
 
diff --git a/src/main/java/frc/robot/utils/dynamicauto/Note.java b/src/main/java/frc/robot/utils/dynamicauto/Note.java
index 4c94d8ad..261314bb 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/Note.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/Note.java
@@ -37,13 +37,13 @@ public Note() {
 
   public void blacklist() {
     System.out.println("Blacklisted " + name);
-    DynamicAuto.notesLeft -= 1;
+    DynamicAuto.whitelistCount -= 1;
     this.blacklisted = true;
   }
 
   public void whitelist() {
     System.out.println("Whitelisted " + name);
-    DynamicAuto.notesLeft += 1;
+    DynamicAuto.whitelistCount += 1;
     this.blacklisted = false;
   }
 
diff --git a/src/main/java/frc/robot/utils/dynamicauto/ShootingLocation.java b/src/main/java/frc/robot/utils/dynamicauto/ShootingLocation.java
index 4d95a1ad..dc54041e 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/ShootingLocation.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/ShootingLocation.java
@@ -21,6 +21,10 @@ public ShootingLocation() {
     this.name = "Uninitialized";
   }
 
+  public ShootingLocation(String name) {
+    this.name = name;
+  }
+
   public Pose2d getPose() {
     return pose;
   }

From 2031a40750d2e1c4e822299d13b8095cbb89f7dc Mon Sep 17 00:00:00 2001
From: Stoopipoopy <anotherguywithabeanie@gmail.com>
Date: Wed, 21 Feb 2024 15:45:02 -0800
Subject: [PATCH 53/62] added a way to change the priorities of a note during
 runtime withe the dashboard

---
 ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 simgui.json                                   |   7 ++++
 src/main/java/frc/robot/Robot.java            |  30 ++++++++++++------
 .../robot/utils/dynamicauto/DynamicAuto.java  |  17 ++++++++--
 .../frc/robot/utils/dynamicauto/Note.java     |   2 --
 7 files changed, 42 insertions(+), 14 deletions(-)

diff --git a/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat
index 4305ca4857de52a528069f155279462dcb0195d1..4426df0d3f50d214daed279bbe5d05e243503649 100644
GIT binary patch
delta 55
zcmZn=Xb@nz&7j&aQ80C)0`J5!FP1wDDhM9iVTSFi+kz(wTCp8r*s{6<#9G9}1^@sJ
B62AZd

delta 55
zcmZn=Xb@nz%b?saQ80C)0`J5!FP3`@N(dg?c?RQs{~RU?TCp8qaM;J-2xKi{Vgms7
Ci4nj6

diff --git a/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat
index 4e24c67869f36d675c58f5f743de02d00b0878c9..f7c538337ebb2ced70b4fe2745f928bef4ee9188 100644
GIT binary patch
delta 55
zcmZn=Xb@nz%AnOSQ80C)0`J5!FP3Wzng|}-VTSFi+kz(wTCp8r*s{6<#9G9}1_1w*
B61@Nb

delta 55
zcmZn=Xb@nz$)MISQ80C)0`J5!FP0k&>Ifd&c?RQs{~RU?TCp8qaM;J-2xKi{Vgms7
CBN4s;

diff --git a/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat
index ef8edd82f9de826b80c02b2135205886d8445037..20e6065463c3276831d22a7954bf61c03fb8c348 100644
GIT binary patch
delta 61
zcmZn=Xb@nz%@9*PQ80C)0`J5!FP1wD(Fh*rd4{E{Yl0aV1SSfabDm>ZvAPP%S;WK!
E0B#KvBLDyZ

delta 61
zcmZn=Xb@nz%MevPQ80C)0`J5!FP3`@kq92=SqANWKO7hs1SSfabDm?+*!K&{S;WK!
E0BmIwBLDyZ

diff --git a/simgui.json b/simgui.json
index 92fc7902..3416ea7e 100644
--- a/simgui.json
+++ b/simgui.json
@@ -17,17 +17,24 @@
       "/AdvantageKit/RealOutputs/Shooter/Mechanism2d": "Mechanism2d",
       "/FMSInfo": "FMSInfo",
       "/SmartDashboard/Auto Chooser": "String Chooser",
+      "/SmartDashboard/Blacklist All": "Command",
       "/SmartDashboard/Blacklist Note": "Command",
+      "/SmartDashboard/Dynamic Demo": "Command",
+      "/SmartDashboard/Note Picker": "String Chooser",
       "/SmartDashboard/Note To Note": "Command",
       "/SmartDashboard/Note To Shooting": "Command",
       "/SmartDashboard/SendableChooser[0]": "String Chooser",
       "/SmartDashboard/Shoot To Note": "Command",
       "/SmartDashboard/Start To Note": "Command",
+      "/SmartDashboard/Update Note": "Command",
       "/SmartDashboard/VisionSystemSim-Left Camera Sim System/Sim Field": "Field2d",
       "/SmartDashboard/VisionSystemSim-Left Camera/Sim Field": "Field2d",
       "/SmartDashboard/VisionSystemSim-Right Camera Sim System/Sim Field": "Field2d",
       "/SmartDashboard/VisionSystemSim-Right Camera/Sim Field": "Field2d",
+      "/SmartDashboard/Whitelist All": "Command",
       "/SmartDashboard/Whitelist Note": "Command",
+      "/SmartDashboard/gleeb": "Command",
+      "/SmartDashboard/gloob": "Command",
       "/SmartDashboard/reset Kicker value": "Command",
       "/SmartDashboard/reset Pivot value": "Command"
     },
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 93012b00..e67a102c 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -31,6 +31,9 @@
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.networktables.LoggedDashboardBoolean;
+import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
+import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
 import org.littletonrobotics.junction.networktables.NT4Publisher;
 import org.littletonrobotics.junction.wpilog.WPILOGReader;
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
@@ -70,6 +73,9 @@ public void setYaw(Rotation2d yaw) {}
   private AutoManager autoManager = new AutoManager(swerve, intake, elevator, shooter, feeder);
   ;
 
+  private LoggedDashboardChooser<Note> noteDropdown =
+      new LoggedDashboardChooser<Note>("Note Picker");
+
   @Override
   public void robotInit() {
     // Metadata about the current code running on the robot
@@ -114,6 +120,10 @@ public void robotInit() {
     Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may
     // be added.
 
+    for (Note note : DynamicAuto.notes) {
+      noteDropdown.addOption(note.getName(), note);
+    }
+
     // Default Commands here
     swerve.setDefaultCommand(
         swerve.runVelocityFieldRelative(
@@ -198,6 +208,7 @@ public void robotInit() {
             startToNote(),
             blacklistNote(),
             noteToNote()));
+    SmartDashboard.putData("Update Note", updateNote());
   }
 
   public Command dynamicAutoDemo() {
@@ -206,16 +217,6 @@ public Command dynamicAutoDemo() {
                 () -> swerve.setPose(new Pose2d(0.71, 6.72, Rotation2d.fromRadians(1.04))), swerve),
             startToNote(),
             blacklistNote(),
-            noteToNote(),
-            blacklistNote(),
-            noteToNote(),
-            blacklistNote(),
-            noteToNote(),
-            blacklistNote(),
-            noteToNote(),
-            blacklistNote(),
-            noteToNote(),
-            blacklistNote(),
             noteToShoot(),
             shootToNote(),
             blacklistNote(),
@@ -235,12 +236,21 @@ public void robotPeriodic() {
     Logger.recordOutput(
         "DynamicAuto/Absolute Closest Note",
         DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getPose());
+
     Logger.recordOutput(
         "DynamicAuto/Closest Shooting Location",
         DynamicAuto.closestShootingLocation(() -> swerve.getPose(), DynamicAuto.shootingLocations)
             .getPose());
   }
 
+  private LoggedDashboardNumber priority = new LoggedDashboardNumber("Note Priority");
+  private LoggedDashboardBoolean blacklist = new LoggedDashboardBoolean("Note Blacklisted");
+
+  public Command updateNote() {
+    return Commands.runOnce(
+        () -> DynamicAuto.updateNote(noteDropdown.get(), () -> blacklist.get(), () -> (int) priority.get()));
+  }
+
   public Command startToNote() {
     return swerve
         .runChoreoTraj(() -> DynamicAuto.makeStartToNote(swerve::getPose))
diff --git a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
index cc44188e..25b385bf 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
@@ -5,6 +5,10 @@
 import com.google.common.base.Supplier;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+
+import java.util.function.BooleanSupplier;
+import java.util.function.IntSupplier;
+
 import org.littletonrobotics.junction.Logger;
 
 public class DynamicAuto {
@@ -38,13 +42,22 @@ public class DynamicAuto {
 
   public static int whitelistCount = notes.length;
 
-  public static void updateWhitelistCount(){
+  public static void updateWhitelistCount() {
     whitelistCount = 0;
-    for(Note note : notes){
+    for (Note note : notes) {
       whitelistCount += note.getBlacklist() ? 1 : 0;
     }
   }
 
+  public static void updateNote(Note note, BooleanSupplier blacklist, IntSupplier priority) {
+    if (blacklist.getAsBoolean()) {
+      note.blacklist();
+    } else {
+      note.whitelist();
+    }
+    note.setPriority(priority.getAsInt());
+  }
+
   public static ShootingLocation closestShootingLocation(
       Supplier<Pose2d> curPose, ShootingLocation[] shootingLocations) {
 
diff --git a/src/main/java/frc/robot/utils/dynamicauto/Note.java b/src/main/java/frc/robot/utils/dynamicauto/Note.java
index 261314bb..7d7cf3ef 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/Note.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/Note.java
@@ -37,13 +37,11 @@ public Note() {
 
   public void blacklist() {
     System.out.println("Blacklisted " + name);
-    DynamicAuto.whitelistCount -= 1;
     this.blacklisted = true;
   }
 
   public void whitelist() {
     System.out.println("Whitelisted " + name);
-    DynamicAuto.whitelistCount += 1;
     this.blacklisted = false;
   }
 

From 935f7f6fe9fc797bfab8caf49947fb9675dc3c41 Mon Sep 17 00:00:00 2001
From: Stoopipoopy <anotherguywithabeanie@gmail.com>
Date: Wed, 21 Feb 2024 15:45:27 -0800
Subject: [PATCH 54/62] empty commit

---
 ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 3 files changed, 0 insertions(+), 0 deletions(-)

diff --git a/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 020 - 0 - ext.dat
index 4426df0d3f50d214daed279bbe5d05e243503649..e362a2f92ce234e565d8f155bf532c33706dfb26 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx*VgBlA!4n0o*iJDlT0I5CTExT#0ILoPyZ`_I

delta 30
kcmZn=Xb{-Yz{qx(Vf*T~;E94(Y)2TjtnL7@7BR5_0IMnryZ`_I

diff --git a/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 021 - 0 - ext.dat
index f7c538337ebb2ced70b4fe2745f928bef4ee9188..ee033f97db837eb41ccc6ab133e668417f45a0ae 100644
GIT binary patch
delta 30
kcmZn=Xb{-Yz{qx*VgBlA!4n0o*iJDlT0I5CTExT#0ILoPyZ`_I

delta 30
kcmZn=Xb{-Yz{qx(Vf*T~;E94(Y)2TjtnL7@7BR5_0IMnryZ`_I

diff --git a/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 022 - 0 - ext.dat
index 20e6065463c3276831d22a7954bf61c03fb8c348..708226e322c761b20db35d7d968c315c2de9eade 100644
GIT binary patch
delta 36
ncmZn=Xb{-Yz{q)&Ve#sYU<L+(iGt>w#~2o@?t*d_F|h#v)>aC<

delta 36
ncmZn=Xb{-Yz{q)?Vd?6cU<L+(iGt>w=NMM3u7YwFF|h#v*0Kt`


From 5124a32c3f17cf305a2a79b0219fb9c55f2ee030 Mon Sep 17 00:00:00 2001
From: Stoopipoopy <anotherguywithabeanie@gmail.com>
Date: Fri, 23 Feb 2024 15:42:14 -0800
Subject: [PATCH 55/62] implemented dynamic auto in sim with notes existing or
 not

---
 ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 018 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 simgui.json                                   |  16 +-
 src/main/java/frc/robot/Robot.java            | 178 ++++++++++++++++--
 .../subsystems/swerve/SwerveSubsystem.java    |  11 +-
 .../robot/utils/dynamicauto/DynamicAuto.java  |  36 ++--
 .../utils/dynamicauto/LocalADStarAK.java      | 150 +++++++++++++++
 .../frc/robot/utils/dynamicauto/Note.java     |  13 ++
 10 files changed, 364 insertions(+), 40 deletions(-)
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/LocalADStarAK.java

diff --git a/ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat
index 51223a6e646f55e0270d60c47f7cc29513ee6652..1482fb11399ae14b8fa6f11965cc037aba934c41 100644
GIT binary patch
delta 81
zcmZn=Xb@nz&M>2DqG0Mo1>T8eN-Pr?8CWI?s<7N(n2ssRb)G@=jf@}z0|V1UK{Kv%
Q3=(gopgi-<j7<Mo0iX~S5&!@I

delta 81
zcmZn=Xb@nz%MevPQ80C)0`J5!C6*Hm3@j4`RaovZL}H3^9b}OIt-!><z`!(7(2VN<
QgVJv~D9?N|Bh!CY0Gjj_5&!@I

diff --git a/ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat
index 61891ad70817b1de2afb92ea92508dec7d27cab4..f32d6eed53f12be0a080246242a66b5b04708fa8 100644
GIT binary patch
literal 2048
zcmdT^O-NKx6uyl{)|5;TgF@6qBve!yWkQJ$3Y<lTG-LF$nF1sFp%#HI`gw~4k<{sb
zN}(40U`#P!3pYuYGA)9VASNbg=to=@(qs#@d+s@(^X6d`M2jvw&O7IR=R4my_r8}_
zXZn@0Ym_YCmdR?o|2s?day4hJQ(820JU{wQo&K-&CslhkK1cU7E5ENl^`5J??8Cpv
zN7-jR$@s)sr;L8%=X7-JrhShDj&Eci`EFZ2wRh`)22-5!Qe#V7#`Swb9lG}Ttx_5B
z!C!$Fmz^y>S!aD;0dKSvOltPrf{qW3jXSA6mj7VSF3PuV1#Xjducp|%A&P%#%{Ecq
zg~N<LC;VkMcpvpHU1uNl(nlE&-_3ud&ujQssw%VfKmbp=x0yL9#d<M^x<`yB80oS7
z`?w$JTPgMjJ#53@c)n5a&gZy&jCDzP+5p!ok=&?Q$DS<n&6WLRFZ75G9jAFApJ3z%
z1@D|gj$8Nf^>e&aV|?SxZwVd-@#&9W`W=~wJ$0U2>U_w|ZAWdPq0h9l9}wOZ>q<jT
ze6tIGF<flUV?%nx=t*2@FnrJh-trFSwSU8pIY2-9LH^D4tmjmN8ZH{r-$5JmqQ33F
zZ|*1Rq*hgKBK^>fW4A;P@}U>`-~*m?Y_9O=`;*VF!hDYC8+zS-y&N9q0e)DW7m-{$
zSh5H-${t>9=9&1#d1&Bknmp;W^h5k~&rt{S+8*Y4X-Gsrs{RMxo3PmCzWt_7)v|kL
zeuQzZVmP7)_=x0V`_oCz2c9hHfe*YT379YR)~@FBdgbN+8^P)hL~4L@=gC-2?}V*B
z!S(H{F>=-WO1>|nqH|0wF!$9myGl*zyFn`x>@@wq=;Hh}(ZYdl{j==7{HAQwyLKV&
YNy+l4`LDhF9{PBvYcOzk3I7*<0_%fDPXGV_

literal 2048
zcmcgsO-NK>6ut9iiW#zD`dQRQrXm+XCQ%YW_=t+2i~*xvU_lV2H2o}E2)!0VwMtAv
zFeoZrv?<bs5YdY!a}hbXC<)4_u%NLFr8grC_3n4?nbAQAndk$#-tRl--1F|e`i|B)
z^xu!uK*T?lpZB!Zsa$2D3jfakFs&$5TMMmU!n+hN#4F%aNVFz{5(7o)A46F`u{Ce(
ze>O35ffKp6$RW<bQ&=%{PLDOU`Wfr7bpP2chdFOq8M$&IOVqCKPGH>B@t@{B52_|T
z_R(_psi~jVt+9LFs2`$Tg^?lm{#%2ixX7FwfM0n(bw~Ji$K&s;cRKZCD<jj^-1>HO
zZjSoSr75Ey5qU~zEb)a0K6q##{2ZfwhWSeQ&~4Wo_>wM>nFD<J&6IIVJ>a6shmKn%
z?@nvpmARpSMBpy#wP<o8^Nn~iKYZ_tKJ>HHAJzq*nVj$CU;M*!$0=zOrE2!v<~KJd
z`ZhosaUaIL?&Wx4<G0!0aXdI}=5e4cWaN_}$up|Ee$%7WY<##3-*Irz?9b1f_(`70
z8a>p7uB*DSzv!b6@DKao-`p&IMkZEWi~2ik2gEPx5A-h2@tjd-bi<8X>>s|Nmr)<!
z!!P*I1D@lN#YA|AzPnrIi0=lzJWuHG&=2S%T8o1B_<)a?@V=OSHs@u|<?{fJBYy+y
z^Y^0;`el8TeiiG?h+aq|c5B-jZ{9t}-$mw^_yInqeE9f$PWZrc<o+WEUe}Q>Zm%`_
z2tDZRUoHRNIBz`K+)&q+H+`tD6FhL>*?O@fDfOM}UFK%2UiPI+eNNGHv#-Ga_Pz1>
vaCNEKo0r|fKOY`E6!Y)<I$zg(T~fOYdosH7q46>7HGS&oPRDzz<^RGTheUz&

diff --git a/ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat
index 3822b64e3b6fd0d64a8a778ffc7b280d49751f7a..fbc622691ac9f32ec41738b80f5d27f0fcf390de 100644
GIT binary patch
delta 101
zcmZn=Xb=#&&5-)}?3pvR3{W6o^UrpoVA8}gzKN1zOp+WE1yw}uFr*Mt!*-BC{;h)G
V#6~N&0}M)U<$x^f&5TU{SpoTq9pC@}

delta 71
zcmZn=Xb@nz%AnOSQ89F)f$YRGzR3|RJQH;cSgtT=V+gXHXAu4?$23vVitPY{!e0d-
L%X%{-(|=X~3Skw9

diff --git a/ctre_sim/Talon FX vers. C - 018 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 018 - 0 - ext.dat
index e585805a23469e4c37ea917b60e9f6f246c48d42..8bcbb98a8bb796718b5dc1d12341cf2f786fc52e 100644
GIT binary patch
delta 77
zcmZn=Xb@nz&Y;mSQ80C)0`J5!zR3|RJQHOMSZ*+=V+eAdW#E1%A;`cWFj3H)^Be=m
OJ1GdqVlyMte^vlp+!Y7_

delta 63
zcmZn=Xb@nz%b?saQ80C)0`J5!zKN1HEcY0cPz2b{GYJ2cW11*v#dd%};jaRaWxbh^
H=|3v~Xay4{

diff --git a/simgui.json b/simgui.json
index 71717021..f43cbda2 100644
--- a/simgui.json
+++ b/simgui.json
@@ -13,31 +13,31 @@
   },
   "NTProvider": {
     "types": {
-      "/AdvantageKit/RealOutputs/Elevator/Mechanism2d": "Mechanism2d",
       "/AdvantageKit/RealOutputs/Elevator/Mechanism2d": "Mechanism2d",
       "/AdvantageKit/RealOutputs/Shooter/Mechanism2d": "Mechanism2d",
       "/FMSInfo": "FMSInfo",
       "/SmartDashboard/Auto Chooser": "String Chooser",
       "/SmartDashboard/Blacklist All": "Command",
       "/SmartDashboard/Blacklist Note": "Command",
+      "/SmartDashboard/Dynamic Auto": "Command",
       "/SmartDashboard/Dynamic Demo": "Command",
       "/SmartDashboard/Note Picker": "String Chooser",
       "/SmartDashboard/Note To Note": "Command",
       "/SmartDashboard/Note To Shooting": "Command",
-      "/SmartDashboard/SendableChooser[0]": "String Chooser",
-      "/SmartDashboard/Shoot To Note": "Command",
-      "/SmartDashboard/Start To Note": "Command",
-      "/SmartDashboard/Update Note": "Command",
       "/SmartDashboard/Run Elevator Sysid": "Command",
       "/SmartDashboard/Run Flywheel Sysid": "Command",
       "/SmartDashboard/Run Pivot Sysid": "Command",
       "/SmartDashboard/Run Swerve Azimuth Sysid": "Command",
+      "/SmartDashboard/Run Swerve Drive Sysid": "Command",
+      "/SmartDashboard/SendableChooser[0]": "String Chooser",
+      "/SmartDashboard/Shoot To Note": "Command",
+      "/SmartDashboard/Shooter shoot": "Command",
+      "/SmartDashboard/Start To Note": "Command",
+      "/SmartDashboard/Update Note": "Command",
       "/SmartDashboard/Whitelist All": "Command",
       "/SmartDashboard/Whitelist Note": "Command",
       "/SmartDashboard/gleeb": "Command",
-      "/SmartDashboard/gloob": "Command",
-      "/SmartDashboard/Run Swerve Drive Sysid": "Command",
-      "/SmartDashboard/Shooter shoot": "Command"
+      "/SmartDashboard/gloob": "Command"
     },
     "windows": {
       "/SmartDashboard/Auto Chooser": {
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 8017e1c2..e636ed8a 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -5,16 +5,15 @@
 package frc.robot;
 
 import com.choreo.lib.Choreo;
+import com.choreo.lib.ChoreoTrajectory;
 import com.ctre.phoenix6.SignalLogger;
 import com.pathplanner.lib.auto.NamedCommands;
-import com.pathplanner.lib.commands.PathPlannerAuto;
-
-import edu.wpi.first.math.geometry.Pose2d;
+import com.pathplanner.lib.pathfinding.Pathfinding;
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
@@ -46,9 +45,11 @@
 import frc.robot.subsystems.swerve.SwerveSubsystem.AutoAimStates;
 import frc.robot.utils.CommandXboxControllerSubsystem;
 import frc.robot.utils.autoaim.AutoAim;
-import java.util.function.Supplier;
 import frc.robot.utils.dynamicauto.DynamicAuto;
+import frc.robot.utils.dynamicauto.LocalADStarAK;
 import frc.robot.utils.dynamicauto.Note;
+import java.util.Map;
+import java.util.function.Supplier;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
@@ -71,6 +72,15 @@ public static enum Target {
     SPEAKER
   }
 
+  private static enum CommandSelector {
+    START_TO_NOTE,
+    NOTE_TO_SHOOT,
+    NOTE_TO_NOTE,
+    SHOOT_TO_NOTE,
+    DYNAMIC_TO_NOTE,
+    DYNAMIC_TO_SHOOT
+  }
+
   public static final RobotMode mode = Robot.isReal() ? RobotMode.REAL : RobotMode.SIM;
   private Command autonomousCommand;
 
@@ -80,6 +90,10 @@ public static enum Target {
   private Target currentTarget = Target.SPEAKER;
   private double flywheelIdleSpeed = -0.1;
 
+  private int dynamicAutoCounter = 0;
+  private boolean atShootingLocation = false;
+  private ChoreoTrajectory curTrajectory = Choreo.getTrajectory("Amp Side To C1");
+
   private final SwerveSubsystem swerve =
       new SwerveSubsystem(
           mode == RobotMode.REAL
@@ -115,6 +129,7 @@ public void setYaw(Rotation2d yaw) {}
 
   @Override
   public void robotInit() {
+    Pathfinding.setPathfinder(new LocalADStarAK());
     // Metadata about the current code running on the robot
     Logger.recordMetadata("Codebase", "Comp2024");
     Logger.recordMetadata("RuntimeType", getRuntimeType().toString());
@@ -333,7 +348,7 @@ public void robotInit() {
             startToNote(),
             blacklistNote(),
             noteToNote()));
-    
+
     NamedCommands.registerCommand(
         "auto aim amp 4 local sgmt 1", autonomousAutoAim("amp 4 local sgmt 1"));
 
@@ -345,6 +360,8 @@ public void robotInit() {
     SmartDashboard.putData("Run Pivot Sysid", shooter.runPivotSysidCmd());
     SmartDashboard.putData("Run Flywheel Sysid", shooter.runFlywheelSysidCmd());
     SmartDashboard.putData("Update Note", updateNote());
+
+    SmartDashboard.putData("Dynamic Auto", dynamicAuto());
   }
 
   public Command dynamicAutoDemo() {
@@ -372,7 +389,19 @@ public void robotPeriodic() {
         new Pose3d[] {
           shooter.getMechanismPose(), elevator.getCarriagePose(), elevator.getFirstStagePose()
         });
-    
+    Logger.recordOutput(
+        "DynamicAuto/Closest Note Within Distance",
+        swerve
+                .getPose()
+                .minus(DynamicAuto.getClosestNote(swerve::getPose).getPose())
+                .getTranslation()
+                .getNorm()
+            < 1.5);
+    Logger.recordOutput("DynamicAuto/Current Path Ending Pose", curTrajectory.getFinalPose());
+    Logger.recordOutput("DynamicAuto/Current Path Initial Pose", curTrajectory.getInitialPose());
+    Logger.recordOutput(
+        "DynamicAuto/Closest Note Exists",
+        DynamicAuto.getClosestNote(swerve::getPose).getExistence());
     Logger.recordOutput(
         "DynamicAuto/Closest Note", DynamicAuto.getClosestNote(swerve::getPose).getPose());
 
@@ -498,12 +527,18 @@ public Command autonomousAutoAim(String pathName) {
 
   public Command updateNote() {
     return Commands.runOnce(
-        () -> DynamicAuto.updateNote(noteDropdown.get(), () -> blacklist.get(), () -> (int) priority.get()));
+        () ->
+            DynamicAuto.updateNote(
+                noteDropdown.get(), () -> blacklist.get(), () -> (int) priority.get()));
   }
 
   public Command startToNote() {
     return swerve
-        .runChoreoTraj(() -> DynamicAuto.makeStartToNote(swerve::getPose))
+        .runChoreoTraj(
+            () -> {
+              curTrajectory = DynamicAuto.makeStartToNote(swerve::getPose);
+              return DynamicAuto.makeStartToNote(swerve::getPose);
+            })
         .onlyIf(
             () -> {
               if (DynamicAuto.whitelistCount > 0) {
@@ -520,6 +555,7 @@ public Command noteToNote() {
         .runChoreoTraj(
             () -> {
               System.out.println(DynamicAuto.whitelistCount);
+              curTrajectory = DynamicAuto.makeNoteToNote(swerve::getPose);
               return DynamicAuto.makeNoteToNote(swerve::getPose);
             })
         .onlyIf(
@@ -535,7 +571,11 @@ public Command noteToNote() {
 
   public Command shootToNote() {
     return swerve
-        .runChoreoTraj(() -> DynamicAuto.makeShootingToNote(swerve::getPose))
+        .runChoreoTraj(
+            () -> {
+              curTrajectory = DynamicAuto.makeShootingToNote(swerve::getPose);
+              return DynamicAuto.makeShootingToNote(swerve::getPose);
+            })
         .onlyIf(
             () -> {
               if (DynamicAuto.whitelistCount > 0) {
@@ -549,7 +589,11 @@ public Command shootToNote() {
 
   public Command noteToShoot() {
     return swerve
-        .runChoreoTraj(() -> DynamicAuto.makeNoteToShooting(swerve::getPose))
+        .runChoreoTraj(
+            () -> {
+              curTrajectory = DynamicAuto.makeNoteToShooting(swerve::getPose);
+              return DynamicAuto.makeNoteToShooting(swerve::getPose);
+            })
         .onlyIf(
             () -> {
               if (DynamicAuto.whitelistCount > 0) {
@@ -562,8 +606,108 @@ public Command noteToShoot() {
   }
 
   public Command blacklistNote() {
-    return Commands.runOnce(
-        () -> DynamicAuto.getAbsoluteClosestNote(swerve::getPose).blacklist(), swerve);
+    return Commands.runOnce(() -> DynamicAuto.getAbsoluteClosestNote(swerve::getPose).blacklist());
+  }
+
+  public CommandSelector selectAuto() {
+    if (dynamicAutoCounter == 0) {
+      swerve.setPose(new Pose2d(0.71, 6.72, Rotation2d.fromRadians(1.04)));
+      System.out.println("start to note");
+      return CommandSelector.START_TO_NOTE;
+
+    } else if (atShootingLocation) {
+      atShootingLocation = false;
+      System.out.println("shoot to note");
+      return CommandSelector.SHOOT_TO_NOTE;
+    } else if ((carriage.getBeambreak() || feeder.getFirstBeambreak())
+        || (DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getExistence()
+            && swerve
+                    .getPose()
+                    .minus(DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getPose())
+                    .getTranslation()
+                    .getNorm()
+                < 1.5)) {
+      System.out.println("note to shoot");
+      atShootingLocation = true;
+      DynamicAuto.getAbsoluteClosestNote(swerve::getPose).blacklist();
+      return CommandSelector.NOTE_TO_SHOOT;
+    } else if (!(carriage.getBeambreak() || feeder.getFirstBeambreak())
+        || (!DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getExistence()
+            && swerve
+                    .getPose()
+                    .minus(DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getPose())
+                    .getTranslation()
+                    .getNorm()
+                < 1.5)) {
+
+      System.out.println("note to note 1");
+      DynamicAuto.getAbsoluteClosestNote(swerve::getPose).blacklist();
+      return CommandSelector.NOTE_TO_NOTE;
+    } else if ((carriage.getBeambreak() || feeder.getFirstBeambreak())
+        || (DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getExistence()
+            && swerve
+                    .getPose()
+                    .minus(DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getPose())
+                    .getTranslation()
+                    .getNorm()
+                > 1.5)) {
+      return CommandSelector.DYNAMIC_TO_NOTE;
+    } else {
+      atShootingLocation = true;
+      return CommandSelector.DYNAMIC_TO_SHOOT;
+    }
+  }
+
+  double distance = 0;
+
+  public Command dynamicAuto() {
+
+    return Commands.repeatingSequence(
+            Commands.select(
+                Map.ofEntries(
+                    Map.entry(
+                        CommandSelector.NOTE_TO_NOTE,
+                        Commands.race(noteToNote(), intake.runVelocityCmd(80.0, 30.0))),
+                    Map.entry(
+                        CommandSelector.NOTE_TO_SHOOT,
+                        Commands.race(
+                            noteToShoot(),
+                            shooter.runStateCmd(
+                                () -> AutoAim.shotMap.get(distance).getRotation(),
+                                () -> AutoAim.shotMap.get(distance).getLeftRPS(),
+                                () -> AutoAim.shotMap.get(distance).getRightRPS()))),
+                    Map.entry(
+                        CommandSelector.START_TO_NOTE,
+                        Commands.race(startToNote(), intake.runVelocityCmd(80.0, 30.0))),
+                    Map.entry(
+                        CommandSelector.SHOOT_TO_NOTE,
+                        Commands.race(shootToNote(), intake.runVelocityCmd(80.0, 30.0))),
+                    Map.entry(
+                        CommandSelector.DYNAMIC_TO_NOTE,
+                        Commands.race(
+                            DynamicAuto.DynamicToNote(swerve::getPose),
+                            intake.runVelocityCmd(80.0, 30.0))),
+                    Map.entry(
+                        CommandSelector.DYNAMIC_TO_SHOOT,
+                        Commands.race(
+                            DynamicAuto.DynamicToShoot(swerve::getPose),
+                            shooter.runStateCmd(
+                                () -> AutoAim.shotMap.get(distance).getRotation(),
+                                () -> AutoAim.shotMap.get(distance).getLeftRPS(),
+                                () -> AutoAim.shotMap.get(distance).getRightRPS())))),
+                this::selectAuto),
+            Commands.runOnce(
+                () -> {
+                  dynamicAutoCounter++;
+                  System.out.println(dynamicAutoCounter);
+                  distance =
+                      swerve
+                          .getPose()
+                          .minus(FieldConstants.getSpeaker())
+                          .getTranslation()
+                          .getNorm();
+                }))
+        .asProxy();
   }
 
   @Override
@@ -571,12 +715,16 @@ public void disabledPeriodic() {}
 
   @Override
   public void autonomousInit() {
-    autoManager.chooser.get().schedule();
+    dynamicAutoCounter = 0;
+    for (Note note : DynamicAuto.notes) {
+      note.whitelist();
+    }
+    dynamicAuto().schedule();
   }
 
   @Override
   public void teleopInit() {
-    autoManager.chooser.get().cancel();
+    dynamicAuto().cancel();
   }
 
   @Override
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
index 426ca45e..b377f99b 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
@@ -16,10 +16,9 @@
 import static edu.wpi.first.units.Units.Seconds;
 import static edu.wpi.first.units.Units.Volts;
 
-import com.choreo.lib.Choreo;
-import com.choreo.lib.ChoreoTrajectoryState;
 import com.choreo.lib.Choreo;
 import com.choreo.lib.ChoreoTrajectory;
+import com.choreo.lib.ChoreoTrajectoryState;
 import com.ctre.phoenix6.SignalLogger;
 import com.google.common.collect.Streams;
 import com.pathplanner.lib.auto.AutoBuilder;
@@ -45,8 +44,8 @@
 import edu.wpi.first.units.Measure;
 import edu.wpi.first.units.Voltage;
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -674,9 +673,9 @@ public Command runChoreoTraj(Supplier<ChoreoTrajectory> traj) {
             Choreo.choreoSwerveCommand(
                 traj.get(),
                 this::getPose,
-                new PIDController(85.0, 0.0, 0.0),
-                new PIDController(85.0, 0.0, 0.0),
-                new PIDController(6.0, 0.0, 0.70),
+                new PIDController(6.0, 0.0, 0.0),
+                new PIDController(6.0, 0.0, 0.0),
+                new PIDController(1.0, 0.0, 0.0),
                 (ChassisSpeeds speeds) -> //
                 this.runVelocity(speeds),
                 () -> {
diff --git a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
index 25b385bf..ca7b1312 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
@@ -3,25 +3,27 @@
 import com.choreo.lib.Choreo;
 import com.choreo.lib.ChoreoTrajectory;
 import com.google.common.base.Supplier;
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.PathConstraints;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.Command;
 import java.util.function.BooleanSupplier;
 import java.util.function.IntSupplier;
-
 import org.littletonrobotics.junction.Logger;
 
 public class DynamicAuto {
 
   public static final Note[] notes = {
-    new Note(new Pose2d(2.204, 7.0, Rotation2d.fromRadians(Math.PI)), false, 0, "W1"), // w1
-    new Note(new Pose2d(2.204, 5.5, Rotation2d.fromRadians(Math.PI)), false, 1, "W2"), // w2
-    new Note(new Pose2d(2.204, 4.1, Rotation2d.fromRadians(Math.PI)), false, 2, "W3"), // w3
-    new Note(new Pose2d(7.538, 7.3, Rotation2d.fromRadians(Math.PI)), false, 3, "C1"), // c1
-    new Note(new Pose2d(7.538, 5.7, Rotation2d.fromRadians(Math.PI)), false, 0, "C2"), // c2
-    new Note(new Pose2d(7.538, 4.1, Rotation2d.fromRadians(Math.PI)), false, 1, "C3"), // c3
-    new Note(new Pose2d(7.538, 2.5, Rotation2d.fromRadians(Math.PI)), false, 2, "C4"), // c4
-    new Note(new Pose2d(7.538, 0.7, Rotation2d.fromRadians(Math.PI)), false, 0, "C5") // c5
+    new Note(new Pose2d(2.204, 7.0, Rotation2d.fromRadians(Math.PI)), false, 0, "W1", true), // w1
+    new Note(new Pose2d(2.204, 5.5, Rotation2d.fromRadians(Math.PI)), false, 1, "W2", true), // w2
+    new Note(new Pose2d(2.204, 4.1, Rotation2d.fromRadians(Math.PI)), false, 2, "W3", true), // w3
+    new Note(new Pose2d(7.538, 7.3, Rotation2d.fromRadians(Math.PI)), false, 3, "C1", true), // c1
+    new Note(new Pose2d(7.538, 5.7, Rotation2d.fromRadians(Math.PI)), false, 0, "C2", true), // c2
+    new Note(new Pose2d(7.538, 4.1, Rotation2d.fromRadians(Math.PI)), false, 1, "C3", true), // c3
+    new Note(new Pose2d(7.538, 2.5, Rotation2d.fromRadians(Math.PI)), false, 2, "C4", true), // c4
+    new Note(new Pose2d(7.538, 0.7, Rotation2d.fromRadians(Math.PI)), false, 0, "C5", true) // c5
   };
 
   public static final ShootingLocation[] shootingLocations = {
@@ -58,6 +60,18 @@ public static void updateNote(Note note, BooleanSupplier blacklist, IntSupplier
     note.setPriority(priority.getAsInt());
   }
 
+  public static Command DynamicToNote(Supplier<Pose2d> curPose) {
+    return AutoBuilder.pathfindToPose(
+        getClosestNote(curPose).getPose(),
+        new PathConstraints(3.0, 4.0, Units.degreesToRadians(540), Units.degreesToRadians(720)));
+  }
+
+  public static Command DynamicToShoot(Supplier<Pose2d> curPose) {
+    return AutoBuilder.pathfindToPose(
+        closestShootingLocation(curPose, shootingLocations).getPose(),
+        new PathConstraints(3.0, 4.0, Units.degreesToRadians(540), Units.degreesToRadians(720)));
+  }
+
   public static ShootingLocation closestShootingLocation(
       Supplier<Pose2d> curPose, ShootingLocation[] shootingLocations) {
 
@@ -83,7 +97,7 @@ public static ChoreoTrajectory makeStartToNote(Supplier<Pose2d> startingPose) {
   }
 
   public static ChoreoTrajectory makeNoteToShooting(Supplier<Pose2d> startingPose) {
-    Note closestNote = getClosestNote(startingPose);
+    Note closestNote = getAbsoluteClosestNote(startingPose);
     ShootingLocation startingLocation = closestShootingLocation(startingPose, shootingLocations);
     if ("Uninitialized" == closestNote.getName()) {
       closestNote = getAbsoluteClosestNote(startingPose);
diff --git a/src/main/java/frc/robot/utils/dynamicauto/LocalADStarAK.java b/src/main/java/frc/robot/utils/dynamicauto/LocalADStarAK.java
new file mode 100644
index 00000000..a813443b
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/LocalADStarAK.java
@@ -0,0 +1,150 @@
+package frc.robot.utils.dynamicauto;
+
+import com.pathplanner.lib.path.GoalEndState;
+import com.pathplanner.lib.path.PathConstraints;
+import com.pathplanner.lib.path.PathPlannerPath;
+import com.pathplanner.lib.path.PathPoint;
+import com.pathplanner.lib.pathfinding.LocalADStar;
+import com.pathplanner.lib.pathfinding.Pathfinder;
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.geometry.Translation2d;
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+import org.littletonrobotics.junction.LogTable;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.inputs.LoggableInputs;
+
+public class LocalADStarAK implements Pathfinder {
+  private final ADStarIO io = new ADStarIO();
+
+  /**
+   * Get if a new path has been calculated since the last time a path was retrieved
+   *
+   * @return True if a new path is available
+   */
+  @Override
+  public boolean isNewPathAvailable() {
+    if (!Logger.hasReplaySource()) {
+      io.updateIsNewPathAvailable();
+    }
+
+    Logger.processInputs("LocalADStarAK", io);
+
+    return io.isNewPathAvailable;
+  }
+
+  /**
+   * Get the most recently calculated path
+   *
+   * @param constraints The path constraints to use when creating the path
+   * @param goalEndState The goal end state to use when creating the path
+   * @return The PathPlannerPath created from the points calculated by the pathfinder
+   */
+  @Override
+  public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) {
+    if (!Logger.hasReplaySource()) {
+      io.updateCurrentPathPoints(constraints, goalEndState);
+    }
+
+    Logger.processInputs("LocalADStarAK", io);
+
+    if (io.currentPathPoints.isEmpty()) {
+      return null;
+    }
+
+    return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState);
+  }
+
+  /**
+   * Set the start position to pathfind from
+   *
+   * @param startPosition Start position on the field. If this is within an obstacle it will be
+   *     moved to the nearest non-obstacle node.
+   */
+  @Override
+  public void setStartPosition(Translation2d startPosition) {
+    if (!Logger.hasReplaySource()) {
+      io.adStar.setStartPosition(startPosition);
+    }
+  }
+
+  /**
+   * Set the goal position to pathfind to
+   *
+   * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved
+   *     to the nearest non-obstacle node.
+   */
+  @Override
+  public void setGoalPosition(Translation2d goalPosition) {
+    if (!Logger.hasReplaySource()) {
+      io.adStar.setGoalPosition(goalPosition);
+    }
+  }
+
+  /**
+   * Set the dynamic obstacles that should be avoided while pathfinding.
+   *
+   * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents
+   *     opposite corners of a bounding box.
+   * @param currentRobotPos The current position of the robot. This is needed to change the start
+   *     position of the path to properly avoid obstacles
+   */
+  @Override
+  public void setDynamicObstacles(
+      List<Pair<Translation2d, Translation2d>> obs, Translation2d currentRobotPos) {
+    if (!Logger.hasReplaySource()) {
+      io.adStar.setDynamicObstacles(obs, currentRobotPos);
+    }
+  }
+
+  private static class ADStarIO implements LoggableInputs {
+    public LocalADStar adStar = new LocalADStar();
+    public boolean isNewPathAvailable = false;
+    public List<PathPoint> currentPathPoints = Collections.emptyList();
+
+    @Override
+    public void toLog(LogTable table) {
+      table.put("IsNewPathAvailable", isNewPathAvailable);
+
+      double[] pointsLogged = new double[currentPathPoints.size() * 2];
+      int idx = 0;
+      for (PathPoint point : currentPathPoints) {
+        pointsLogged[idx] = point.position.getX();
+        pointsLogged[idx + 1] = point.position.getY();
+        idx += 2;
+      }
+
+      table.put("CurrentPathPoints", pointsLogged);
+    }
+
+    @Override
+    public void fromLog(LogTable table) {
+      isNewPathAvailable = table.get("IsNewPathAvailable", false);
+
+      double[] pointsLogged = table.get("CurrentPathPoints", new double[0]);
+
+      List<PathPoint> pathPoints = new ArrayList<>();
+      for (int i = 0; i < pointsLogged.length; i += 2) {
+        pathPoints.add(
+            new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null));
+      }
+
+      currentPathPoints = pathPoints;
+    }
+
+    public void updateIsNewPathAvailable() {
+      isNewPathAvailable = adStar.isNewPathAvailable();
+    }
+
+    public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState goalEndState) {
+      PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState);
+
+      if (currentPath != null) {
+        currentPathPoints = currentPath.getAllPathPoints();
+      } else {
+        currentPathPoints = Collections.emptyList();
+      }
+    }
+  }
+}
diff --git a/src/main/java/frc/robot/utils/dynamicauto/Note.java b/src/main/java/frc/robot/utils/dynamicauto/Note.java
index 7d7cf3ef..6fc6dcd3 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/Note.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/Note.java
@@ -13,6 +13,15 @@ public class Note {
   private int priority;
   private final Pose2d pose;
   private final String name;
+  private boolean isThere;
+
+  public Note(Pose2d pose, boolean blacklisted, int priority, String name, boolean isThere) {
+    this.pose = pose;
+    this.blacklisted = blacklisted;
+    this.priority = priority;
+    this.name = name;
+    this.isThere = isThere;
+  }
 
   public Note(Pose2d pose, boolean blacklisted, int priority, String name) {
     this.pose = pose;
@@ -53,6 +62,10 @@ public boolean getBlacklist() {
     return blacklisted;
   }
 
+  public boolean getExistence() {
+    return isThere;
+  }
+
   public int getPriority() {
     return priority;
   }

From 8eb6f1d893d83f3e7a0cb30f970dceeaf7f776f7 Mon Sep 17 00:00:00 2001
From: Stoopipoopy <anotherguywithabeanie@gmail.com>
Date: Fri, 23 Feb 2024 16:10:40 -0800
Subject: [PATCH 56/62] fixed dynamic path generating

---
 ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 018 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 src/main/java/frc/robot/Robot.java            |  12 ++++++++----
 5 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat
index 1482fb11399ae14b8fa6f11965cc037aba934c41..15b5b132f6c40983db40107c3beb462af4a25398 100644
GIT binary patch
delta 65
zcmZn=Xb@nz%P^^GqG0Mo1>T8eUM%+*CL(xTCmGaV3WzW;FfdING~+tKp!Jdu$}`{0
I$n>8T0HJphqyPW_

delta 65
zcmZn=Xb@nz&M>2DqG0Mo1>T8eUMx2lrXzS<=NUxb$OtkpFfdING~+tQAn`^D$}`{0
I$n>8T0HIS7qyPW_

diff --git a/ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat
index f32d6eed53f12be0a080246242a66b5b04708fa8..62d279e8a81acfa7cde641a8604bdb092125a0fc 100644
GIT binary patch
delta 49
ycmZn=Xb@n&$j~v7c`x%NhISyub(mr1=OtVW3=E7D70tK~G0geA2*z8)#0CJEB@YDv

delta 49
ycmZn=Xb@n&%}_Uyc`x%FhFT!Sb)2F1+ZuKT1_s87ie_9V7`nc#gYgzIu>k;>MGpl4

diff --git a/ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat
index fbc622691ac9f32ec41738b80f5d27f0fcf390de..042ec9c87895fc97cd0f61566d95031815d18cd6 100644
GIT binary patch
delta 45
wcmZn=Xb@n&%aFX0xt5XZG=t(R5fKIk2F8hsW?ZKjR9*=|dFGoLnf|i^01S}}GXMYp

delta 45
wcmZn=Xb@n&&5*j0xt5XZAcOo{1wjS|2F8hsW?Tmtl-|lgdFGoLnf|i^01QD3GXMYp

diff --git a/ctre_sim/Talon FX vers. C - 018 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 018 - 0 - ext.dat
index 8bcbb98a8bb796718b5dc1d12341cf2f786fc52e..02f099523064b8c7a1a3729f282d0fb42e566955 100644
GIT binary patch
delta 63
zcmZn=Xb@nz%b?USQ80C)0`J5!FP3`@iU=O(aR%AfJR%GX0uu$zIZrT1z2<{(EH*PT
H{bvOLTG|oN

delta 63
zcmZn=Xb@nz&Y;mSQ80C)0`J5!FP0k&>Ifd^SqAQR5`qj20uu$zInOb0ypw`(EH*PT
H{bvOLTDB3;

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index e636ed8a..290ecc03 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -396,7 +396,7 @@ public void robotPeriodic() {
                 .minus(DynamicAuto.getClosestNote(swerve::getPose).getPose())
                 .getTranslation()
                 .getNorm()
-            < 1.5);
+            > 1.5);
     Logger.recordOutput("DynamicAuto/Current Path Ending Pose", curTrajectory.getFinalPose());
     Logger.recordOutput("DynamicAuto/Current Path Initial Pose", curTrajectory.getInitialPose());
     Logger.recordOutput(
@@ -611,7 +611,7 @@ public Command blacklistNote() {
 
   public CommandSelector selectAuto() {
     if (dynamicAutoCounter == 0) {
-      swerve.setPose(new Pose2d(0.71, 6.72, Rotation2d.fromRadians(1.04)));
+      //  swerve.setPose(new Pose2d(0.71, 6.72, Rotation2d.fromRadians(1.04)));
       System.out.println("start to note");
       return CommandSelector.START_TO_NOTE;
 
@@ -631,7 +631,7 @@ public CommandSelector selectAuto() {
       atShootingLocation = true;
       DynamicAuto.getAbsoluteClosestNote(swerve::getPose).blacklist();
       return CommandSelector.NOTE_TO_SHOOT;
-    } else if (!(carriage.getBeambreak() || feeder.getFirstBeambreak())
+    } else if ((!(carriage.getBeambreak() || feeder.getFirstBeambreak()) && Robot.isReal())
         || (!DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getExistence()
             && swerve
                     .getPose()
@@ -640,7 +640,8 @@ public CommandSelector selectAuto() {
                     .getNorm()
                 < 1.5)) {
 
-      System.out.println("note to note 1");
+      System.out.println(
+          "note to note 1");
       DynamicAuto.getAbsoluteClosestNote(swerve::getPose).blacklist();
       return CommandSelector.NOTE_TO_NOTE;
     } else if ((carriage.getBeambreak() || feeder.getFirstBeambreak())
@@ -651,9 +652,12 @@ public CommandSelector selectAuto() {
                     .getTranslation()
                     .getNorm()
                 > 1.5)) {
+
+      System.out.println("dynamic to note");
       return CommandSelector.DYNAMIC_TO_NOTE;
     } else {
       atShootingLocation = true;
+      System.out.println("dynamoci to shoot");
       return CommandSelector.DYNAMIC_TO_SHOOT;
     }
   }

From 7aa00842af7613daa3770137a5d8e8297ea0c80c Mon Sep 17 00:00:00 2001
From: Stoopipoopy <anotherguywithabeanie@gmail.com>
Date: Fri, 23 Feb 2024 16:28:03 -0800
Subject: [PATCH 57/62] added a way to flip notes for red alliance

---
 ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 018 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 .../robot/utils/dynamicauto/DynamicAuto.java  |  11 +++++++++++
 .../frc/robot/utils/dynamicauto/Note.java     |   9 ++++++++-
 6 files changed, 19 insertions(+), 1 deletion(-)

diff --git a/ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat
index 15b5b132f6c40983db40107c3beb462af4a25398..8356e56c9c25dd412ef81784cc6311b6de7e4a67 100644
GIT binary patch
delta 40
qcmZn=Xb{-Yz{qu!LHLD+C<6ln(?mftt|JU$FVvts^UaJ*|5*Xj7zxh+

delta 40
qcmZn=Xb{-Yz{quyLH(tG2m=EH(?mftt`iJeFZrN6^UaJ*|5*Xjj0w*G

diff --git a/ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat
index 62d279e8a81acfa7cde641a8604bdb092125a0fc..54190244d90ba175cefe5fdc9f75e5b4f1d72b4a 100644
GIT binary patch
delta 38
ocmZn=Xb{-Yz{qu;q4d)pZUzPh#)*n%T;~`nKJ9|>7BR5_0Ozs_+yDRo

delta 38
ocmZn=Xb{-Yz{qu&Vdm#0Tnr2hj1v{jxDGMQ`Me0mTg1c$0O#fk+yDRo

diff --git a/ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat
index 042ec9c87895fc97cd0f61566d95031815d18cd6..b89a239738ecaaed513123ae5d49b7cdf3c7794f 100644
GIT binary patch
delta 40
qcmZn=Xb{-Yz{qu+f%m13C<6ln<3vR>u44=WFSVdN^UaJ*|5*Xk+zIah

delta 40
qcmZn=Xb{-Yz{qu)LGhJ{2m=EH<3vR>u2T#uuY{mH^UaJ*|5*XlPzmn<

diff --git a/ctre_sim/Talon FX vers. C - 018 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 018 - 0 - ext.dat
index 02f099523064b8c7a1a3729f282d0fb42e566955..c8dd93ae9df6ff4768403184ef996463e08fd6c2 100644
GIT binary patch
delta 38
pcmZn=Xb{-Yz{q)+f%BD$C<BAQL_u@TBMfY>)F2#-&5TU{Spmhx3C{ok

delta 38
pcmZn=Xb{-Yz{q)=LH0F|2m^z_L_u@T6AV(X`5+vN&5TU{Spmj53C{ok

diff --git a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
index ca7b1312..3eaf3b5d 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
@@ -9,6 +9,8 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.swerve.SwerveSubsystem;
+
 import java.util.function.BooleanSupplier;
 import java.util.function.IntSupplier;
 import org.littletonrobotics.junction.Logger;
@@ -86,6 +88,15 @@ public static ShootingLocation closestShootingLocation(
     return closestLocation;
   }
 
+  public static void flipNote(Note note){
+    Pose2d pose = note.getPose();
+    note.setPose(new Pose2d(
+        16.5410515 - pose.getX(),
+        pose.getY(),
+        Rotation2d.fromRadians(Math.PI - pose.getRotation().getRadians())));
+        
+  }
+
   public static ChoreoTrajectory makeStartToNote(Supplier<Pose2d> startingPose) {
     ShootingLocation startingLocation = closestShootingLocation(startingPose, startingLocations);
     Note closestNote = getClosestNote(startingPose);
diff --git a/src/main/java/frc/robot/utils/dynamicauto/Note.java b/src/main/java/frc/robot/utils/dynamicauto/Note.java
index 6fc6dcd3..c38f38cf 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/Note.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/Note.java
@@ -11,7 +11,7 @@ public class Note {
 
   private boolean blacklisted;
   private int priority;
-  private final Pose2d pose;
+  private Pose2d pose;
   private final String name;
   private boolean isThere;
 
@@ -28,6 +28,7 @@ public Note(Pose2d pose, boolean blacklisted, int priority, String name) {
     this.blacklisted = blacklisted;
     this.priority = priority;
     this.name = name;
+    this.isThere = false;
   }
 
   public Note(String name) {
@@ -35,6 +36,7 @@ public Note(String name) {
     this.blacklisted = true;
     this.priority = -1;
     this.name = name;
+    this.isThere = false;
   }
 
   public Note() {
@@ -42,6 +44,7 @@ public Note() {
     this.blacklisted = true;
     this.priority = -1;
     this.name = "Uninitialized";
+    this.isThere = false;
   }
 
   public void blacklist() {
@@ -58,6 +61,10 @@ public void setPriority(Integer priority) {
     this.priority = priority;
   }
 
+  public void setPose(Pose2d pose){
+    this.pose = pose;
+  }
+
   public boolean getBlacklist() {
     return blacklisted;
   }

From d866a4273d00b503dd735d06a1ac4980e7258bbe Mon Sep 17 00:00:00 2001
From: Sneha-Narayan <59103434+Sneha-Narayan@users.noreply.github.com>
Date: Sat, 24 Feb 2024 14:29:43 -0800
Subject: [PATCH 58/62] edited choreo paths so robot doesn't run over notes

---
 src/main/java/frc/robot/Robot.java            |     3 +-
 .../robot/utils/dynamicauto/DynamicAuto.java  |    14 +-
 .../frc/robot/utils/dynamicauto/Note.java     |     2 +-
 .../frc/robot/utils/dynamicauto/data.chor     | 26072 +++++++++-------
 .../deploy/choreo/Amp Side To C1.1.traj       |   328 +
 .../deploy/choreo/Amp Side To C1.traj         |   554 +-
 .../deploy/choreo/Amp Side To C2.1.traj       |   364 +
 .../deploy/choreo/Amp Side To C2.traj         |   629 +-
 .../deploy/choreo/Amp Side To C3.1.traj       |   670 +
 .../deploy/choreo/Amp Side To C3.traj         |  1140 +-
 .../deploy/choreo/Amp Side To C4.1.traj       |   454 +
 .../deploy/choreo/Amp Side To C4.traj         |   792 +-
 .../deploy/choreo/Amp Side To C5.1.traj       |   859 +
 .../deploy/choreo/Amp Side To C5.traj         |  1590 +-
 .../deploy/choreo/Amp Side To W1.1.traj       |   121 +
 .../deploy/choreo/Amp Side To W2.1.traj       |   148 +
 .../deploy/choreo/Amp Side To W3.1.traj       |   238 +
 .../deploy/choreo/Amp Side To W3.traj         |   383 +-
 .../dynamicauto/deploy/choreo/C1 To C2.1.traj |    94 +
 .../dynamicauto/deploy/choreo/C1 To C3.1.traj |   139 +
 .../dynamicauto/deploy/choreo/C1 To C4.1.traj |   175 +
 .../dynamicauto/deploy/choreo/C1 To C5.1.traj |   220 +
 .../dynamicauto/deploy/choreo/C1 To S1.1.traj |   130 +
 .../dynamicauto/deploy/choreo/C1 To S2.1.traj |   148 +
 .../dynamicauto/deploy/choreo/C1 To S3.1.traj |   265 +
 .../dynamicauto/deploy/choreo/C1 To S4.1.traj |   301 +
 .../dynamicauto/deploy/choreo/C1 To W1.1.traj |   202 +
 .../dynamicauto/deploy/choreo/C1 To W2.1.traj |   193 +
 .../dynamicauto/deploy/choreo/C1 To W3.1.traj |   346 +
 .../dynamicauto/deploy/choreo/C1 To W3.traj   |   574 +-
 .../dynamicauto/deploy/choreo/C2 To C1.1.traj |    94 +
 .../dynamicauto/deploy/choreo/C2 To C3.1.traj |    94 +
 .../dynamicauto/deploy/choreo/C2 To C4.1.traj |   139 +
 .../dynamicauto/deploy/choreo/C2 To C5.1.traj |   175 +
 .../dynamicauto/deploy/choreo/C2 To S1.1.traj |   130 +
 .../dynamicauto/deploy/choreo/C2 To S2.1.traj |   193 +
 .../dynamicauto/deploy/choreo/C2 To S3.1.traj |   193 +
 .../dynamicauto/deploy/choreo/C2 To S4.1.traj |   256 +
 .../dynamicauto/deploy/choreo/C2 To W1.1.traj |   202 +
 .../dynamicauto/deploy/choreo/C2 To W2.1.traj |   184 +
 .../dynamicauto/deploy/choreo/C2 To W3.1.traj |   346 +
 .../dynamicauto/deploy/choreo/C3 To C1.1.traj |   139 +
 .../dynamicauto/deploy/choreo/C3 To C2.1.traj |    94 +
 .../dynamicauto/deploy/choreo/C3 To C4.1.traj |    94 +
 .../dynamicauto/deploy/choreo/C3 To C5.1.traj |   139 +
 .../dynamicauto/deploy/choreo/C3 To S1.1.traj |   157 +
 .../dynamicauto/deploy/choreo/C3 To S2.1.traj |   148 +
 .../dynamicauto/deploy/choreo/C3 To S3.1.traj |   166 +
 .../dynamicauto/deploy/choreo/C3 To S4.1.traj |   238 +
 .../dynamicauto/deploy/choreo/C3 To W1.1.traj |   265 +
 .../dynamicauto/deploy/choreo/C3 To W1.traj   |   437 +-
 .../dynamicauto/deploy/choreo/C3 To W2.1.traj |   193 +
 .../dynamicauto/deploy/choreo/C3 To W3.1.traj |   202 +
 .../dynamicauto/deploy/choreo/C4 To C1.1.traj |   175 +
 .../dynamicauto/deploy/choreo/C4 To C2.1.traj |   139 +
 .../dynamicauto/deploy/choreo/C4 To C3.1.traj |    94 +
 .../dynamicauto/deploy/choreo/C4 To C5.1.traj |    94 +
 .../dynamicauto/deploy/choreo/C4 To S1.1.traj |   256 +
 .../dynamicauto/deploy/choreo/C4 To S1.traj   |   256 +
 .../dynamicauto/deploy/choreo/C4 To S2.1.traj |   211 +
 .../dynamicauto/deploy/choreo/C4 To S2.traj   |   211 +
 .../dynamicauto/deploy/choreo/C4 To S3.1.traj |   229 +
 .../dynamicauto/deploy/choreo/C4 To S4.1.traj |   193 +
 .../dynamicauto/deploy/choreo/C4 To W1.1.traj |   238 +
 .../dynamicauto/deploy/choreo/C4 To W2.1.traj |   319 +
 .../dynamicauto/deploy/choreo/C4 To W2.traj   |   525 +-
 .../dynamicauto/deploy/choreo/C4 To W3.1.traj |   274 +
 .../dynamicauto/deploy/choreo/C5 To C1.1.traj |   220 +
 .../dynamicauto/deploy/choreo/C5 To C2.1.traj |   175 +
 .../dynamicauto/deploy/choreo/C5 To C3.1.traj |   139 +
 .../dynamicauto/deploy/choreo/C5 To C4.1.traj |    94 +
 .../dynamicauto/deploy/choreo/C5 To S1.1.traj |   292 +
 .../dynamicauto/deploy/choreo/C5 To S1.traj   |   292 +
 .../dynamicauto/deploy/choreo/C5 To S2.1.traj |   247 +
 .../dynamicauto/deploy/choreo/C5 To S2.traj   |   247 +
 .../dynamicauto/deploy/choreo/C5 To S3.1.traj |   247 +
 .../dynamicauto/deploy/choreo/C5 To S4.1.traj |   148 +
 .../dynamicauto/deploy/choreo/C5 To W1.1.traj |   310 +
 .../dynamicauto/deploy/choreo/C5 To W2.1.traj |   346 +
 .../dynamicauto/deploy/choreo/C5 To W2.traj   |   577 +-
 .../dynamicauto/deploy/choreo/C5 To W3.1.traj |   292 +
 .../deploy/choreo/Center To C1.1.traj         |   265 +
 .../deploy/choreo/Center To C1.traj           |   437 +-
 .../deploy/choreo/Center To C2.1.traj         |   265 +
 .../deploy/choreo/Center To C2.traj           |   430 +-
 .../deploy/choreo/Center To C3.1.traj         |   274 +
 .../deploy/choreo/Center To C3.traj           |   441 +-
 .../deploy/choreo/Center To C4.1.traj         |  1453 +
 .../deploy/choreo/Center To C4.traj           |  2488 +-
 .../deploy/choreo/Center To C5.1.traj         |   625 +
 .../deploy/choreo/Center To W1.1.traj         |   112 +
 .../deploy/choreo/Center To W2.1.traj         |    76 +
 .../deploy/choreo/Center To W3.1.traj         |   112 +
 .../dynamicauto/deploy/choreo/S1 To C1.1.traj |   130 +
 .../dynamicauto/deploy/choreo/S1 To C2.1.traj |   130 +
 .../dynamicauto/deploy/choreo/S1 To C3.1.traj |   157 +
 .../dynamicauto/deploy/choreo/S1 To C4.1.traj |   247 +
 .../dynamicauto/deploy/choreo/S1 To C5.1.traj |   292 +
 .../dynamicauto/deploy/choreo/S1 To W1.1.traj |   130 +
 .../dynamicauto/deploy/choreo/S1 To W2.1.traj |   148 +
 .../dynamicauto/deploy/choreo/S1 To W3.1.traj |   283 +
 .../dynamicauto/deploy/choreo/S1 To W3.traj   |   412 +-
 .../dynamicauto/deploy/choreo/S2 To C1.1.traj |   148 +
 .../dynamicauto/deploy/choreo/S2 To C2.1.traj |   202 +
 .../dynamicauto/deploy/choreo/S2 To C3.1.traj |   148 +
 .../dynamicauto/deploy/choreo/S2 To C4.1.traj |   166 +
 .../dynamicauto/deploy/choreo/S2 To C5.1.traj |   202 +
 .../dynamicauto/deploy/choreo/S2 To W1.1.traj |   130 +
 .../dynamicauto/deploy/choreo/S2 To W2.1.traj |   103 +
 .../dynamicauto/deploy/choreo/S2 To W3.1.traj |   130 +
 .../dynamicauto/deploy/choreo/S3 To C1.1.traj |   220 +
 .../dynamicauto/deploy/choreo/S3 To C2.1.traj |   193 +
 .../dynamicauto/deploy/choreo/S3 To C3.1.traj |   166 +
 .../dynamicauto/deploy/choreo/S3 To C4.1.traj |   238 +
 .../dynamicauto/deploy/choreo/S3 To C5.1.traj |   184 +
 .../dynamicauto/deploy/choreo/S3 To W1.1.traj |   202 +
 .../dynamicauto/deploy/choreo/S3 To W2.1.traj |   157 +
 .../dynamicauto/deploy/choreo/S3 To W3.1.traj |   121 +
 .../dynamicauto/deploy/choreo/S4 To C1.1.traj |   292 +
 .../dynamicauto/deploy/choreo/S4 To C2.1.traj |   202 +
 .../dynamicauto/deploy/choreo/S4 To C3.1.traj |   238 +
 .../dynamicauto/deploy/choreo/S4 To C4.1.traj |   148 +
 .../dynamicauto/deploy/choreo/S4 To C5.1.traj |   148 +
 .../dynamicauto/deploy/choreo/S4 To W1.1.traj |   247 +
 .../dynamicauto/deploy/choreo/S4 To W2.1.traj |   202 +
 .../dynamicauto/deploy/choreo/S4 To W3.1.traj |   256 +
 .../deploy/choreo/Stage Side To C1.1.traj     |   427 +
 .../deploy/choreo/Stage Side To C1.traj       |  1001 +-
 .../deploy/choreo/Stage Side To C2.1.traj     |   661 +
 .../deploy/choreo/Stage Side To C2.traj       |  1218 +-
 .../deploy/choreo/Stage Side To C3.1.traj     |   391 +
 .../deploy/choreo/Stage Side To C3.traj       |   621 +-
 .../deploy/choreo/Stage Side To C4.1.traj     |   679 +
 .../deploy/choreo/Stage Side To C5.1.traj     |   337 +
 .../deploy/choreo/Stage Side To W1.1.traj     |   238 +
 .../deploy/choreo/Stage Side To W1.traj       |   381 +-
 .../deploy/choreo/Stage Side To W2.1.traj     |   193 +
 .../deploy/choreo/Stage Side To W2.traj       |   297 +-
 .../deploy/choreo/Stage Side To W3.1.traj     |   121 +
 .../dynamicauto/deploy/choreo/W1 To C1.1.traj |   256 +
 .../dynamicauto/deploy/choreo/W1 To C1.traj   |   412 +-
 .../dynamicauto/deploy/choreo/W1 To C2.1.traj |   310 +
 .../dynamicauto/deploy/choreo/W1 To C2.traj   |   514 +-
 .../dynamicauto/deploy/choreo/W1 To C3.1.traj |   373 +
 .../dynamicauto/deploy/choreo/W1 To C3.traj   |   502 +-
 .../dynamicauto/deploy/choreo/W1 To C4.1.traj |   472 +
 .../dynamicauto/deploy/choreo/W1 To C4.traj   |   808 +-
 .../dynamicauto/deploy/choreo/W1 To C5.1.traj |   616 +
 .../dynamicauto/deploy/choreo/W1 To S1.1.traj |   202 +
 .../dynamicauto/deploy/choreo/W1 To S1.traj   |   288 +-
 .../dynamicauto/deploy/choreo/W1 To S2.1.traj |   202 +
 .../dynamicauto/deploy/choreo/W1 To S2.traj   |   288 +-
 .../dynamicauto/deploy/choreo/W1 To S3.1.traj |   544 +
 .../dynamicauto/deploy/choreo/W1 To S4.1.traj |   562 +
 .../dynamicauto/deploy/choreo/W1 To W2.1.traj |   103 +
 .../dynamicauto/deploy/choreo/W1 To W3.1.traj |   148 +
 .../dynamicauto/deploy/choreo/W2 To C1.1.traj |   292 +
 .../dynamicauto/deploy/choreo/W2 To C1.traj   |   445 +-
 .../dynamicauto/deploy/choreo/W2 To C2.1.traj |   616 +
 .../dynamicauto/deploy/choreo/W2 To C2.traj   |  1092 +-
 .../dynamicauto/deploy/choreo/W2 To C3.1.traj |   625 +
 .../dynamicauto/deploy/choreo/W2 To C3.traj   |  1140 +-
 .../dynamicauto/deploy/choreo/W2 To C4.1.traj |   589 +
 .../dynamicauto/deploy/choreo/W2 To C4.traj   |  1118 +-
 .../dynamicauto/deploy/choreo/W2 To C5.1.traj |   625 +
 .../dynamicauto/deploy/choreo/W2 To S1.1.traj |   202 +
 .../dynamicauto/deploy/choreo/W2 To S1.traj   |   320 +-
 .../dynamicauto/deploy/choreo/W2 To S2.1.traj |   103 +
 .../dynamicauto/deploy/choreo/W2 To S3.1.traj |   526 +
 .../dynamicauto/deploy/choreo/W2 To S4.1.traj |   553 +
 .../dynamicauto/deploy/choreo/W2 To W1.1.traj |   103 +
 .../dynamicauto/deploy/choreo/W2 To W3.1.traj |   103 +
 .../dynamicauto/deploy/choreo/W3 To C1.1.traj |   634 +
 .../dynamicauto/deploy/choreo/W3 To C1.traj   |  1169 +-
 .../dynamicauto/deploy/choreo/W3 To C2.1.traj |   634 +
 .../dynamicauto/deploy/choreo/W3 To C2.traj   |  1137 +-
 .../dynamicauto/deploy/choreo/W3 To C3.1.traj |   400 +
 .../dynamicauto/deploy/choreo/W3 To C3.traj   |   656 +-
 .../dynamicauto/deploy/choreo/W3 To C4.1.traj |   625 +
 .../dynamicauto/deploy/choreo/W3 To C5.1.traj |   571 +
 .../dynamicauto/deploy/choreo/W3 To S1.1.traj |   256 +
 .../dynamicauto/deploy/choreo/W3 To S1.traj   |   391 +-
 .../dynamicauto/deploy/choreo/W3 To S2.1.traj |   265 +
 .../dynamicauto/deploy/choreo/W3 To S2.traj   |   355 +-
 .../dynamicauto/deploy/choreo/W3 To S3.1.traj |   229 +
 .../dynamicauto/deploy/choreo/W3 To S4.1.traj |   256 +
 .../dynamicauto/deploy/choreo/W3 To W1.1.traj |   148 +
 .../dynamicauto/deploy/choreo/W3 To W2.1.traj |   103 +
 188 files changed, 68072 insertions(+), 22990 deletions(-)
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S2.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.1.traj
 create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 290ecc03..4f474f83 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -640,8 +640,7 @@ public CommandSelector selectAuto() {
                     .getNorm()
                 < 1.5)) {
 
-      System.out.println(
-          "note to note 1");
+      System.out.println("note to note 1");
       DynamicAuto.getAbsoluteClosestNote(swerve::getPose).blacklist();
       return CommandSelector.NOTE_TO_NOTE;
     } else if ((carriage.getBeambreak() || feeder.getFirstBeambreak())
diff --git a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
index 3eaf3b5d..ba4e3246 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
@@ -9,8 +9,6 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.swerve.SwerveSubsystem;
-
 import java.util.function.BooleanSupplier;
 import java.util.function.IntSupplier;
 import org.littletonrobotics.junction.Logger;
@@ -88,13 +86,13 @@ public static ShootingLocation closestShootingLocation(
     return closestLocation;
   }
 
-  public static void flipNote(Note note){
+  public static void flipNote(Note note) {
     Pose2d pose = note.getPose();
-    note.setPose(new Pose2d(
-        16.5410515 - pose.getX(),
-        pose.getY(),
-        Rotation2d.fromRadians(Math.PI - pose.getRotation().getRadians())));
-        
+    note.setPose(
+        new Pose2d(
+            16.5410515 - pose.getX(),
+            pose.getY(),
+            Rotation2d.fromRadians(Math.PI - pose.getRotation().getRadians())));
   }
 
   public static ChoreoTrajectory makeStartToNote(Supplier<Pose2d> startingPose) {
diff --git a/src/main/java/frc/robot/utils/dynamicauto/Note.java b/src/main/java/frc/robot/utils/dynamicauto/Note.java
index c38f38cf..fb683b13 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/Note.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/Note.java
@@ -61,7 +61,7 @@ public void setPriority(Integer priority) {
     this.priority = priority;
   }
 
-  public void setPose(Pose2d pose){
+  public void setPose(Pose2d pose) {
     this.pose = pose;
   }
 
diff --git a/src/main/java/frc/robot/utils/dynamicauto/data.chor b/src/main/java/frc/robot/utils/dynamicauto/data.chor
index f31a047f..aae8a25b 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/data.chor
+++ b/src/main/java/frc/robot/utils/dynamicauto/data.chor
@@ -367,7 +367,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 20
+                    "controlIntervalCount": 14
+                },
+                {
+                    "x": 1.8406627178192139,
+                    "y": 5.925227165222168,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 11
                 },
                 {
                     "x": 2.3,
@@ -384,190 +393,235 @@
                     "x": 0.71,
                     "y": 6.72,
                     "heading": 1.04,
-                    "angularVelocity": 0,
+                    "angularVelocity": -9.490919672428962e-31,
                     "velocityX": 0,
                     "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7269019070910594,
-                    "y": 6.695506477136938,
-                    "heading": 1.0019343442516249,
-                    "angularVelocity": -0.6189075555866055,
-                    "velocityX": 0.2748072454505401,
-                    "velocityY": -0.3982389391394925,
-                    "timestamp": 0.06150459047522247
-                },
-                {
-                    "x": 0.7606735605009036,
-                    "y": 6.646281390208111,
-                    "heading": 0.9275075561119867,
-                    "angularVelocity": -1.2101013528351434,
-                    "velocityX": 0.5490915905447641,
-                    "velocityY": -0.8003481780543988,
-                    "timestamp": 0.12300918095044494
-                },
-                {
-                    "x": 0.8112865108119746,
-                    "y": 6.571983401826325,
-                    "heading": 0.8196848361236891,
-                    "angularVelocity": -1.7530841056771946,
-                    "velocityX": 0.8229133780097379,
-                    "velocityY": -1.2080072041406207,
-                    "timestamp": 0.1845137714256674
-                },
-                {
-                    "x": 0.8787770707149783,
-                    "y": 6.472119277485466,
-                    "heading": 0.6843886163577808,
-                    "angularVelocity": -2.199774337501089,
-                    "velocityX": 1.0973255716610744,
-                    "velocityY": -1.6236857049083067,
-                    "timestamp": 0.24601836190088988
-                },
-                {
-                    "x": 0.9632585614164454,
-                    "y": 6.3460508217508655,
-                    "heading": 0.5331826539280757,
-                    "angularVelocity": -2.4584500321259677,
-                    "velocityX": 1.373580249030372,
-                    "velocityY": -2.0497405927024026,
-                    "timestamp": 0.3075229523761124
-                },
-                {
-                    "x": 1.0643372089754837,
-                    "y": 6.193350092274746,
-                    "heading": 0.39338059647753487,
-                    "angularVelocity": -2.273034522632274,
-                    "velocityX": 1.6434325759759127,
-                    "velocityY": -2.4827533733053033,
-                    "timestamp": 0.3690275428513349
-                },
-                {
-                    "x": 1.1776993188507112,
-                    "y": 6.018028259717655,
-                    "heading": 0.3265019863738018,
-                    "angularVelocity": -1.087375911082207,
-                    "velocityX": 1.843148763357691,
-                    "velocityY": -2.85054873469518,
-                    "timestamp": 0.4305321333265574
-                },
-                {
-                    "x": 1.3085730997897271,
-                    "y": 5.825657422218107,
-                    "heading": 0.3265019166878518,
-                    "angularVelocity": -0.0000011330203075824657,
-                    "velocityX": 2.1278701301448275,
-                    "velocityY": -3.1277476366100623,
-                    "timestamp": 0.4920367238017799
-                },
-                {
-                    "x": 1.4394483094166801,
-                    "y": 5.633287556689,
-                    "heading": 0.32650184700859936,
-                    "angularVelocity": -0.00000113291141146818,
-                    "velocityX": 2.1278933591091382,
-                    "velocityY": -3.127731833392264,
-                    "timestamp": 0.5535413142770024
-                },
-                {
-                    "x": 1.5703235190623746,
-                    "y": 5.4409176911726425,
-                    "heading": 0.32650177732934843,
-                    "angularVelocity": -0.0000011329113877746253,
-                    "velocityX": 2.127893359413853,
-                    "velocityY": -3.1277318331849595,
-                    "timestamp": 0.6150459047522249
-                },
-                {
-                    "x": 1.7011987286842072,
-                    "y": 5.24854782564005,
-                    "heading": 0.32650170765009895,
-                    "angularVelocity": -0.0000011329113643360664,
-                    "velocityX": 2.127893359025885,
-                    "velocityY": -3.1277318334489093,
-                    "timestamp": 0.6765504952274474
-                },
-                {
-                    "x": 1.8320721192807423,
-                    "y": 5.056176722584032,
-                    "heading": 0.32650163796198384,
-                    "angularVelocity": -0.000001133055510645955,
-                    "velocityX": 2.1278637835863385,
-                    "velocityY": -3.1277519542791743,
-                    "timestamp": 0.7380550857026699
-                },
-                {
-                    "x": 1.9454000953072241,
-                    "y": 4.881259770298511,
-                    "heading": 0.26077643177319376,
-                    "angularVelocity": -1.0686227756490447,
-                    "velocityX": 1.8425937828516514,
-                    "velocityY": -2.843965807007333,
-                    "timestamp": 0.7995596761778924
-                },
-                {
-                    "x": 2.046443192812682,
-                    "y": 4.7300582879948045,
-                    "heading": 0.13106133632258532,
-                    "angularVelocity": -2.1090311218780498,
-                    "velocityX": 1.6428545694676804,
-                    "velocityY": -2.4583771899858244,
-                    "timestamp": 0.8610642666531149
-                },
-                {
-                    "x": 2.131123897414369,
-                    "y": 4.604963233646066,
-                    "heading": -0.022718028385538325,
-                    "angularVelocity": -2.50029084853552,
-                    "velocityX": 1.376819257674782,
-                    "velocityY": -2.0339141092100017,
-                    "timestamp": 0.9225688571283374
-                },
-                {
-                    "x": 2.198697305455373,
-                    "y": 4.506052899677577,
-                    "heading": -0.16200255999945734,
-                    "angularVelocity": -2.264620096446796,
-                    "velocityX": 1.0986725953118257,
-                    "velocityY": -1.6081780758842066,
-                    "timestamp": 0.9840734476035599
-                },
-                {
-                    "x": 2.249342546290196,
-                    "y": 4.432626729162034,
-                    "heading": -0.2740984297313893,
-                    "angularVelocity": -1.822561029442031,
-                    "velocityX": 0.8234383879887088,
-                    "velocityY": -1.1938323619132651,
-                    "timestamp": 1.0455780380787822
-                },
-                {
-                    "x": 2.283112085041758,
-                    "y": 4.38410205128396,
-                    "heading": -0.35252590091377134,
-                    "angularVelocity": -1.2751482544051254,
-                    "velocityX": 0.549057208423595,
-                    "velocityY": -0.788960263016835,
-                    "timestamp": 1.1070826285540047
+                    "x": 0.7219893017338576,
+                    "y": 6.7135407128428675,
+                    "heading": 1.0202523530723182,
+                    "angularVelocity": -0.4720627806088485,
+                    "velocityX": 0.2866013928023673,
+                    "velocityY": -0.15440771588183827,
+                    "timestamp": 0.041832670862584576
+                },
+                {
+                    "x": 0.7460365692584342,
+                    "y": 6.700508326600243,
+                    "heading": 0.982041462546113,
+                    "angularVelocity": -0.9134222065746536,
+                    "velocityX": 0.574844183475,
+                    "velocityY": -0.3115360787128853,
+                    "timestamp": 0.08366534172516915
+                },
+                {
+                    "x": 0.7822226491271561,
+                    "y": 6.680770273598179,
+                    "heading": 0.9269846511448561,
+                    "angularVelocity": -1.3161199193358153,
+                    "velocityX": 0.8650195916869635,
+                    "velocityY": -0.4718334401092994,
+                    "timestamp": 0.12549801258775373
+                },
+                {
+                    "x": 0.830641015185364,
+                    "y": 6.654177161634895,
+                    "heading": 0.8571062408800131,
+                    "angularVelocity": -1.6704266981755755,
+                    "velocityX": 1.1574294698338659,
+                    "velocityY": -0.6357019863882251,
+                    "timestamp": 0.1673306834503383
+                },
+                {
+                    "x": 0.8913887859512984,
+                    "y": 6.620557389613832,
+                    "heading": 0.7749072042273928,
+                    "angularVelocity": -1.9649483276514406,
+                    "velocityX": 1.4521609429503868,
+                    "velocityY": -0.8036726158720227,
+                    "timestamp": 0.20916335431292288
+                },
+                {
+                    "x": 0.9645435989149714,
+                    "y": 6.579696019447196,
+                    "heading": 0.6835223857085977,
+                    "angularVelocity": -2.1845322479884017,
+                    "velocityX": 1.7487483217121933,
+                    "velocityY": -0.976781288979697,
+                    "timestamp": 0.25099602517550745
+                },
+                {
+                    "x": 1.05011662601444,
+                    "y": 6.531295841858201,
+                    "heading": 0.5872583086768085,
+                    "angularVelocity": -2.301169756719744,
+                    "velocityX": 2.0456027629834046,
+                    "velocityY": -1.1569946788233771,
+                    "timestamp": 0.29282869603809203
+                },
+                {
+                    "x": 1.147914322294414,
+                    "y": 6.47495195963137,
+                    "heading": 0.4931548271581024,
+                    "angularVelocity": -2.249521237308087,
+                    "velocityX": 2.337830558350811,
+                    "velocityY": -1.3468870398426824,
+                    "timestamp": 0.3346613669006766
+                },
+                {
+                    "x": 1.256785786947157,
+                    "y": 6.410210097873117,
+                    "heading": 0.4149981322548756,
+                    "angularVelocity": -1.8683171141513,
+                    "velocityX": 2.602546344945923,
+                    "velocityY": -1.5476387336328392,
+                    "timestamp": 0.3764940377632612
+                },
+                {
+                    "x": 1.3739249067400805,
+                    "y": 6.336279355727049,
+                    "heading": 0.36626571694971094,
+                    "angularVelocity": -1.1649367420322734,
+                    "velocityX": 2.800182665307358,
+                    "velocityY": -1.7672967233894428,
+                    "timestamp": 0.41832670862584576
+                },
+                {
+                    "x": 1.4978217219933585,
+                    "y": 6.2516824812952025,
+                    "heading": 0.34588578820783056,
+                    "angularVelocity": -0.48717732627787996,
+                    "velocityX": 2.961723760365251,
+                    "velocityY": -2.022268066740437,
+                    "timestamp": 0.46015937948843033
+                },
+                {
+                    "x": 1.6219909683827523,
+                    "y": 6.153574664712983,
+                    "heading": 0.34588517926846474,
+                    "angularVelocity": -0.000014556550044979093,
+                    "velocityX": 2.9682361615699526,
+                    "velocityY": -2.345243910065211,
+                    "timestamp": 0.5019920503510149
+                },
+                {
+                    "x": 1.7365961394307947,
+                    "y": 6.044446925458561,
+                    "heading": 0.3458850779757818,
+                    "angularVelocity": -0.0000024213773805789224,
+                    "velocityX": 2.7396092261115483,
+                    "velocityY": -2.608672528056193,
+                    "timestamp": 0.5438247212135995
+                },
+                {
+                    "x": 1.840662717819214,
+                    "y": 5.925227165222168,
+                    "heading": 0.34588493628260575,
+                    "angularVelocity": -0.000003387141510558926,
+                    "velocityX": 2.4876866870453034,
+                    "velocityY": -2.849919877887629,
+                    "timestamp": 0.5856573920761841
+                },
+                {
+                    "x": 1.973177224121443,
+                    "y": 5.721056374144466,
+                    "heading": 0.3458848333105023,
+                    "angularVelocity": -0.0000016003693825600053,
+                    "velocityX": 2.0595107939713087,
+                    "velocityY": -3.173176731904646,
+                    "timestamp": 0.6500001023402711
+                },
+                {
+                    "x": 2.0754764812101274,
+                    "y": 5.5001929040123185,
+                    "heading": 0.34588474927124485,
+                    "angularVelocity": -0.0000013061193280868856,
+                    "velocityX": 1.5899121542881502,
+                    "velocityY": -3.432610613162356,
+                    "timestamp": 0.7143428126043581
+                },
+                {
+                    "x": 2.145503113949715,
+                    "y": 5.26707902131208,
+                    "heading": 0.3458846699393237,
+                    "angularVelocity": -0.0000012329589594933622,
+                    "velocityX": 1.0883382507855357,
+                    "velocityY": -3.6230037830772255,
+                    "timestamp": 0.7786855228684451
+                },
+                {
+                    "x": 2.1857192532601815,
+                    "y": 5.0438534457178905,
+                    "heading": 0.2952188838034157,
+                    "angularVelocity": -0.787436306738375,
+                    "velocityX": 0.6250302348985738,
+                    "velocityY": -3.4693219275035725,
+                    "timestamp": 0.8430282331325321
+                },
+                {
+                    "x": 2.219169026901941,
+                    "y": 4.846871449995286,
+                    "heading": 0.15700289500909007,
+                    "angularVelocity": -2.1481219586034865,
+                    "velocityX": 0.5198688943072862,
+                    "velocityY": -3.0614500836865797,
+                    "timestamp": 0.9073709433966191
+                },
+                {
+                    "x": 2.2467778858930516,
+                    "y": 4.683227688496762,
+                    "heading": -0.005808843023566441,
+                    "angularVelocity": -2.5303835875804483,
+                    "velocityX": 0.42909070627880197,
+                    "velocityY": -2.54331471004047,
+                    "timestamp": 0.9717136536607061
+                },
+                {
+                    "x": 2.268435157539554,
+                    "y": 4.553295114204883,
+                    "heading": -0.15269665079354766,
+                    "angularVelocity": -2.282897427962143,
+                    "velocityX": 0.33659246801420317,
+                    "velocityY": -2.019382984623824,
+                    "timestamp": 1.036056363924793
+                },
+                {
+                    "x": 2.2843848876654955,
+                    "y": 4.456384809980395,
+                    "heading": -0.26981466762495615,
+                    "angularVelocity": -1.820222001073865,
+                    "velocityX": 0.24788713531789203,
+                    "velocityY": -1.5061582551734227,
+                    "timestamp": 1.10039907418888
+                },
+                {
+                    "x": 2.2948521877927464,
+                    "y": 4.392049751527424,
+                    "heading": -0.3510408876957304,
+                    "angularVelocity": -1.2623997300920147,
+                    "velocityX": 0.16268043550387537,
+                    "velocityY": -0.9998810772644539,
+                    "timestamp": 1.164741784452967
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
                     "heading": -0.393,
-                    "angularVelocity": -0.6580663129938888,
-                    "velocityX": 0.2745797480766085,
-                    "velocityY": -0.39187402269867344,
-                    "timestamp": 1.1685872190292272
+                    "angularVelocity": -0.6521191310103414,
+                    "velocityX": 0.0800061450027687,
+                    "velocityY": -0.498110063997614,
+                    "timestamp": 1.229084494717054
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
                     "heading": -0.393,
-                    "angularVelocity": 0,
-                    "velocityX": 1.1335977805694889e-32,
-                    "velocityY": 0,
-                    "timestamp": 1.2300918095044497
+                    "angularVelocity": -1.3043243176582522e-30,
+                    "velocityX": 1.4896025231223568e-31,
+                    "velocityY": 2.1651038615450103e-31,
+                    "timestamp": 1.293427204981141
                 }
             ],
             "constraints": [
@@ -598,7 +652,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 29
+                    "controlIntervalCount": 14
+                },
+                {
+                    "x": 1.9168630838394165,
+                    "y": 6.151812553405762,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 21
                 },
                 {
                     "x": 7.68,
@@ -615,271 +678,325 @@
                     "x": 0.71,
                     "y": 6.72,
                     "heading": 1.04,
-                    "angularVelocity": 0,
-                    "velocityX": -1.8098951472235817e-30,
-                    "velocityY": 0,
+                    "angularVelocity": 1.821187546700747e-33,
+                    "velocityX": -3.3437580769683535e-34,
+                    "velocityY": -3.622513119436198e-34,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7613816362208097,
-                    "y": 6.725489442192221,
-                    "heading": 0.9931061799728784,
-                    "angularVelocity": -0.5863053623193526,
-                    "velocityX": 0.6424157559264662,
-                    "velocityY": 0.06863355110715653,
-                    "timestamp": 0.07998190540440472
-                },
-                {
-                    "x": 0.8643774617687672,
-                    "y": 6.736651681634022,
-                    "heading": 0.9029654144342619,
-                    "angularVelocity": -1.127014480123304,
-                    "velocityX": 1.287739083323781,
-                    "velocityY": 0.1395595589447693,
-                    "timestamp": 0.15996381080880945
-                },
-                {
-                    "x": 1.019343320389449,
-                    "y": 6.753669544326843,
-                    "heading": 0.7755395878520636,
-                    "angularVelocity": -1.5931831823449016,
-                    "velocityX": 1.9375114638385158,
-                    "velocityY": 0.21277140881771536,
-                    "timestamp": 0.23994571621321417
-                },
-                {
-                    "x": 1.2269178654194646,
-                    "y": 6.7767659053404685,
-                    "heading": 0.62538736801047,
-                    "angularVelocity": -1.8773273665136312,
-                    "velocityX": 2.5952688171215317,
-                    "velocityY": 0.2887698273359004,
-                    "timestamp": 0.3199276216176189
-                },
-                {
-                    "x": 1.4861217663901574,
-                    "y": 6.806716511012647,
-                    "heading": 0.5166970392894589,
-                    "angularVelocity": -1.3589364765874345,
-                    "velocityX": 3.240781770077933,
-                    "velocityY": 0.37446726882464465,
-                    "timestamp": 0.39990952702202365
-                },
-                {
-                    "x": 1.7870125383049598,
-                    "y": 6.838519066862702,
-                    "heading": 0.5166969975980924,
-                    "angularVelocity": -5.212599803241117e-7,
-                    "velocityX": 3.76198554402321,
-                    "velocityY": 0.39762188321538583,
-                    "timestamp": 0.4798914324264284
-                },
-                {
-                    "x": 2.0879033719171214,
-                    "y": 6.870321038982823,
-                    "heading": 0.5166969559088411,
-                    "angularVelocity": -5.212335340785354e-7,
-                    "velocityX": 3.761986315414672,
-                    "velocityY": 0.39761458494048757,
-                    "timestamp": 0.5598733378308332
-                },
-                {
-                    "x": 2.3887942055296523,
-                    "y": 6.902123011099449,
-                    "heading": 0.5166969142195897,
-                    "angularVelocity": -5.212335365150009e-7,
-                    "velocityX": 3.7619863154192914,
-                    "velocityY": 0.3976145848967806,
-                    "timestamp": 0.6398552432352379
-                },
-                {
-                    "x": 2.689685039142183,
-                    "y": 6.933924983216074,
-                    "heading": 0.516696872530338,
-                    "angularVelocity": -5.212335398189914e-7,
-                    "velocityX": 3.7619863154192914,
-                    "velocityY": 0.3976145848967804,
-                    "timestamp": 0.7198371486396427
-                },
-                {
-                    "x": 2.990575872754714,
-                    "y": 6.9657269553327,
-                    "heading": 0.5166968308410861,
-                    "angularVelocity": -5.212335433606452e-7,
-                    "velocityX": 3.7619863154192914,
-                    "velocityY": 0.39761458489678053,
-                    "timestamp": 0.7998190540440474
-                },
-                {
-                    "x": 3.291466706367245,
-                    "y": 6.997528927449325,
-                    "heading": 0.5166967891518339,
-                    "angularVelocity": -5.212335460884143e-7,
-                    "velocityX": 3.761986315419291,
-                    "velocityY": 0.3976145848967808,
-                    "timestamp": 0.8798009594484522
-                },
-                {
-                    "x": 3.5923575399797762,
-                    "y": 7.0293308995659505,
-                    "heading": 0.5166967474625817,
-                    "angularVelocity": -5.212335479348436e-7,
-                    "velocityX": 3.761986315419291,
-                    "velocityY": 0.3976145848967805,
-                    "timestamp": 0.9597828648528569
-                },
-                {
-                    "x": 3.893248373592307,
-                    "y": 7.0611328716825765,
-                    "heading": 0.516696705773329,
-                    "angularVelocity": -5.212335516712931e-7,
-                    "velocityX": 3.7619863154192905,
-                    "velocityY": 0.3976145848967809,
-                    "timestamp": 1.0397647702572617
-                },
-                {
-                    "x": 4.194139207204837,
-                    "y": 7.092934843799203,
-                    "heading": 0.5166966640840763,
-                    "angularVelocity": -5.212335535473894e-7,
-                    "velocityX": 3.7619863154192905,
-                    "velocityY": 0.3976145848967809,
-                    "timestamp": 1.1197466756616663
-                },
-                {
-                    "x": 4.495030040817368,
-                    "y": 7.1247368159158295,
-                    "heading": 0.5166966223948233,
-                    "angularVelocity": -5.212335568545237e-7,
-                    "velocityX": 3.7619863154192905,
-                    "velocityY": 0.3976145848967808,
-                    "timestamp": 1.199728581066071
-                },
-                {
-                    "x": 4.7959208744299,
-                    "y": 7.156538788032456,
-                    "heading": 0.51669658070557,
-                    "angularVelocity": -5.212335599678648e-7,
-                    "velocityX": 3.7619863154192905,
-                    "velocityY": 0.397614584896781,
-                    "timestamp": 1.2797104864704756
-                },
-                {
-                    "x": 5.096811708042431,
-                    "y": 7.1883407601490825,
-                    "heading": 0.5166965390163165,
-                    "angularVelocity": -5.212335628762146e-7,
-                    "velocityX": 3.7619863154192905,
-                    "velocityY": 0.3976145848967809,
-                    "timestamp": 1.3596923918748802
-                },
-                {
-                    "x": 5.3977025416549615,
-                    "y": 7.220142732265709,
-                    "heading": 0.5166964973270628,
-                    "angularVelocity": -5.212335655841188e-7,
-                    "velocityX": 3.76198631541929,
-                    "velocityY": 0.3976145848967813,
-                    "timestamp": 1.4396742972792849
-                },
-                {
-                    "x": 5.698593375267492,
-                    "y": 7.251944704382335,
-                    "heading": 0.5166964556378089,
-                    "angularVelocity": -5.212335680260379e-7,
-                    "velocityX": 3.7619863154192896,
-                    "velocityY": 0.3976145848967809,
-                    "timestamp": 1.5196562026836895
-                },
-                {
-                    "x": 5.999484208880023,
-                    "y": 7.283746676498962,
-                    "heading": 0.5166964139485547,
-                    "angularVelocity": -5.212335712969897e-7,
-                    "velocityX": 3.7619863154192896,
-                    "velocityY": 0.3976145848967812,
-                    "timestamp": 1.5996381080880941
-                },
-                {
-                    "x": 6.300375042492554,
-                    "y": 7.315548648615588,
-                    "heading": 0.5166963722593003,
-                    "angularVelocity": -5.21233573548864e-7,
-                    "velocityX": 3.7619863154192896,
-                    "velocityY": 0.39761458489678125,
-                    "timestamp": 1.6796200134924988
-                },
-                {
-                    "x": 6.6012658761048035,
-                    "y": 7.347350620734872,
-                    "heading": 0.5166963305700458,
-                    "angularVelocity": -5.212335762453642e-7,
-                    "velocityX": 3.7619863154157773,
-                    "velocityY": 0.3976145849300086,
-                    "timestamp": 1.7596019188969034
-                },
-                {
-                    "x": 6.902156662813089,
-                    "y": 7.3791530366221005,
-                    "heading": 0.5166962888792155,
-                    "angularVelocity": -5.212532766918364e-7,
-                    "velocityX": 3.761985728983584,
-                    "velocityY": 0.3976201332842598,
-                    "timestamp": 1.839583824301308
-                },
-                {
-                    "x": 7.162091022778527,
-                    "y": 7.409610892063227,
-                    "heading": 0.4051307797997224,
-                    "angularVelocity": -1.3948843618490365,
-                    "velocityX": 3.249914573191993,
-                    "velocityY": 0.3808093253983604,
-                    "timestamp": 1.9195657297057127
-                },
-                {
-                    "x": 7.370036606479622,
-                    "y": 7.432974685540162,
-                    "heading": 0.25885269617169443,
-                    "angularVelocity": -1.8288897080960538,
-                    "velocityX": 2.5999078497777726,
-                    "velocityY": 0.2921134894049214,
-                    "timestamp": 1.9995476351101173
-                },
-                {
-                    "x": 7.525326975450784,
-                    "y": 7.45013556763307,
-                    "heading": 0.1335302309738367,
-                    "angularVelocity": -1.5668852169025227,
-                    "velocityX": 1.9415687609088876,
-                    "velocityY": 0.21455955576625652,
-                    "timestamp": 2.079529540514522
-                },
+                    "x": 0.7214806358425874,
+                    "y": 6.713430005414815,
+                    "heading": 1.0269113199415918,
+                    "angularVelocity": -0.32293003145597954,
+                    "velocityX": 0.28325561303637664,
+                    "velocityY": -0.1620979769229106,
+                    "timestamp": 0.04053100914583816
+                },
+                {
+                    "x": 0.7444896856754503,
+                    "y": 6.700284020801432,
+                    "heading": 1.001394622951622,
+                    "angularVelocity": -0.6295598734824499,
+                    "velocityX": 0.5676900308618518,
+                    "velocityY": -0.32434387621788874,
+                    "timestamp": 0.08106201829167632
+                },
+                {
+                    "x": 0.7790845165676208,
+                    "y": 6.680557699463914,
+                    "heading": 0.9642667952582056,
+                    "angularVelocity": -0.916035116713321,
+                    "velocityX": 0.8535398358252526,
+                    "velocityY": -0.4866970192264045,
+                    "timestamp": 0.12159302743751449
+                },
+                {
+                    "x": 0.8253351719483972,
+                    "y": 6.654251504147771,
+                    "heading": 0.9165522532050936,
+                    "angularVelocity": -1.177235481145466,
+                    "velocityX": 1.141117785011419,
+                    "velocityY": -0.6490387451614623,
+                    "timestamp": 0.16212403658335264
+                },
+                {
+                    "x": 0.883327254582786,
+                    "y": 6.621374965178265,
+                    "heading": 0.8595502901552765,
+                    "angularVelocity": -1.4063790724951688,
+                    "velocityX": 1.4308077656226736,
+                    "velocityY": -0.8111453344576391,
+                    "timestamp": 0.2026550457291908
+                },
+                {
+                    "x": 0.953164245540845,
+                    "y": 6.581952530815573,
+                    "heading": 0.7949436194315174,
+                    "angularVelocity": -1.5940059743218395,
+                    "velocityX": 1.7230508795567456,
+                    "velocityY": -0.9726487248526893,
+                    "timestamp": 0.24318605487502895
+                },
+                {
+                    "x": 1.0349687566216221,
+                    "y": 6.5360339228116,
+                    "heading": 0.7250282822193501,
+                    "angularVelocity": -1.724983874953593,
+                    "velocityX": 2.0183191291001217,
+                    "velocityY": -1.132925356947043,
+                    "timestamp": 0.2837170640208671
+                },
+                {
+                    "x": 1.1288774102342096,
+                    "y": 6.483723562804198,
+                    "heading": 0.6532984652847575,
+                    "angularVelocity": -1.7697515666707262,
+                    "velocityX": 2.316958190571729,
+                    "velocityY": -1.2906256495904058,
+                    "timestamp": 0.3242480731667053
+                },
+                {
+                    "x": 1.2349754296776467,
+                    "y": 6.425294263584176,
+                    "heading": 0.5860844259008366,
+                    "angularVelocity": -1.6583361924711089,
+                    "velocityX": 2.6176999211067367,
+                    "velocityY": -1.4415949775586157,
+                    "timestamp": 0.36477908231254347
+                },
+                {
+                    "x": 1.3525875597286727,
+                    "y": 6.361480505266573,
+                    "heading": 0.5370018573395918,
+                    "angularVelocity": -1.2109880705076077,
+                    "velocityX": 2.901781439189822,
+                    "velocityY": -1.5744428688658776,
+                    "timestamp": 0.40531009145838165
+                },
+                {
+                    "x": 1.481480430033789,
+                    "y": 6.296375538238497,
+                    "heading": 0.5147559483468728,
+                    "angularVelocity": -0.5488614633964374,
+                    "velocityX": 3.1801051348446676,
+                    "velocityY": -1.6063001736230227,
+                    "timestamp": 0.44584110060421983
+                },
+                {
+                    "x": 1.6221023727568105,
+                    "y": 6.235265511699909,
+                    "heading": 0.514755821640399,
+                    "angularVelocity": -0.0000031261613391085408,
+                    "velocityX": 3.469490291175273,
+                    "velocityY": -1.5077351348125194,
+                    "timestamp": 0.486372109750058
+                },
+                {
+                    "x": 1.7676170388967807,
+                    "y": 6.186949296866798,
+                    "heading": 0.5147557675556996,
+                    "angularVelocity": -0.0000013344029787740425,
+                    "velocityX": 3.5902058499550584,
+                    "velocityY": -1.1920802331681561,
+                    "timestamp": 0.5269031188958961
+                },
+                {
+                    "x": 1.9168630838394165,
+                    "y": 6.151812553405762,
+                    "heading": 0.5147557009631692,
+                    "angularVelocity": -0.000001643002030243893,
+                    "velocityX": 3.6822681716515033,
+                    "velocityY": -0.866910155989649,
+                    "timestamp": 0.5674341280417343
+                },
+                {
+                    "x": 2.2488381415683802,
+                    "y": 6.139406390407425,
+                    "heading": 0.5147556500188685,
+                    "angularVelocity": -5.801183495483463e-7,
+                    "velocityX": 3.780301616970564,
+                    "velocityY": -0.14127277622555945,
+                    "timestamp": 0.6552512102128125
+                },
+                {
+                    "x": 2.57698446655607,
+                    "y": 6.191188083838006,
+                    "heading": 0.5147556048038259,
+                    "angularVelocity": -5.148775328112685e-7,
+                    "velocityX": 3.7367026650739894,
+                    "velocityY": 0.58965399612918,
+                    "timestamp": 0.7430682923838907
+                },
+                {
+                    "x": 2.8993349699211755,
+                    "y": 6.271509362001365,
+                    "heading": 0.5147555599835898,
+                    "angularVelocity": -5.103817495962876e-7,
+                    "velocityX": 3.6707038698590346,
+                    "velocityY": 0.9146429849136081,
+                    "timestamp": 0.8308853745549689
+                },
+                {
+                    "x": 3.221685430514468,
+                    "y": 6.351830811819113,
+                    "heading": 0.514755515163351,
+                    "angularVelocity": -5.103817801462039e-7,
+                    "velocityX": 3.6707033828033078,
+                    "velocityY": 0.9146449395946943,
+                    "timestamp": 0.9187024567260471
+                },
+                {
+                    "x": 3.5440358911075664,
+                    "y": 6.432152261637641,
+                    "heading": 0.514755470343112,
+                    "angularVelocity": -5.103817833267703e-7,
+                    "velocityX": 3.670703382801096,
+                    "velocityY": 0.9146449396035715,
+                    "timestamp": 1.0065195388971253
+                },
+                {
+                    "x": 3.8663863517006654,
+                    "y": 6.512473711456168,
+                    "heading": 0.5147554255228727,
+                    "angularVelocity": -5.103817863197995e-7,
+                    "velocityX": 3.6707033828010953,
+                    "velocityY": 0.9146449396035711,
+                    "timestamp": 1.0943366210682035
+                },
+                {
+                    "x": 4.188736812293763,
+                    "y": 6.592795161274696,
+                    "heading": 0.5147553807026332,
+                    "angularVelocity": -5.103817901912557e-7,
+                    "velocityX": 3.6707033828010953,
+                    "velocityY": 0.9146449396035711,
+                    "timestamp": 1.1821537032392817
+                },
+                {
+                    "x": 4.511087272886861,
+                    "y": 6.673116611093223,
+                    "heading": 0.5147553358823933,
+                    "angularVelocity": -5.103817929337743e-7,
+                    "velocityX": 3.6707033828010958,
+                    "velocityY": 0.9146449396035705,
+                    "timestamp": 1.26997078541036
+                },
+                {
+                    "x": 4.833437733479959,
+                    "y": 6.753438060911751,
+                    "heading": 0.5147552910621531,
+                    "angularVelocity": -5.103817959943963e-7,
+                    "velocityX": 3.6707033828010953,
+                    "velocityY": 0.9146449396035703,
+                    "timestamp": 1.357787867581438
+                },
+                {
+                    "x": 5.155788194073057,
+                    "y": 6.833759510730278,
+                    "heading": 0.5147552462419127,
+                    "angularVelocity": -5.10381799453682e-7,
+                    "velocityX": 3.670703382801095,
+                    "velocityY": 0.9146449396035702,
+                    "timestamp": 1.4456049497525163
+                },
+                {
+                    "x": 5.4781386546661555,
+                    "y": 6.914080960548806,
+                    "heading": 0.514755201421672,
+                    "angularVelocity": -5.10381803359495e-7,
+                    "velocityX": 3.670703382801095,
+                    "velocityY": 0.9146449396035701,
+                    "timestamp": 1.5334220319235945
+                },
+                {
+                    "x": 5.8004891152592535,
+                    "y": 6.994402410367334,
+                    "heading": 0.5147551566014309,
+                    "angularVelocity": -5.1038180633665e-7,
+                    "velocityX": 3.670703382801095,
+                    "velocityY": 0.9146449396035699,
+                    "timestamp": 1.6212391140946727
+                },
+                {
+                    "x": 6.1228395758523515,
+                    "y": 7.074723860185862,
+                    "heading": 0.5147551117811896,
+                    "angularVelocity": -5.103818092114042e-7,
+                    "velocityX": 3.670703382801095,
+                    "velocityY": 0.9146449396035696,
+                    "timestamp": 1.709056196265751
+                },
+                {
+                    "x": 6.4451900364454335,
+                    "y": 7.1550453100044535,
+                    "heading": 0.514755066960948,
+                    "angularVelocity": -5.10381812997418e-7,
+                    "velocityX": 3.6707033828009097,
+                    "velocityY": 0.9146449396043118,
+                    "timestamp": 1.796873278436829
+                },
+                {
+                    "x": 6.767540493462307,
+                    "y": 7.235366774175193,
+                    "heading": 0.5147550221406476,
+                    "angularVelocity": -5.103824823798507e-7,
+                    "velocityX": 3.670703342077523,
+                    "velocityY": 0.9146451030365995,
+                    "timestamp": 1.8846903606079073
+                },
+                {
+                    "x": 7.069460815974501,
+                    "y": 7.313239150235045,
+                    "heading": 0.4557447568037101,
+                    "angularVelocity": -0.6719679574638842,
+                    "velocityX": 3.438059145759549,
+                    "velocityY": 0.8867565869263073,
+                    "timestamp": 1.9725074427789855
+                },
+                {
+                    "x": 7.3146543806816275,
+                    "y": 7.37536322701953,
+                    "heading": 0.2979903486408247,
+                    "angularVelocity": -1.7963977424752164,
+                    "velocityX": 2.7920941876599903,
+                    "velocityY": 0.7074258817146762,
+                    "timestamp": 2.0603245249500635
+                },
+                {
+                    "x": 7.497733370662191,
+                    "y": 7.421431644323229,
+                    "heading": 0.15573272655395068,
+                    "angularVelocity": -1.6199310950657548,
+                    "velocityX": 2.084776508787937,
+                    "velocityY": 0.5245951717451859,
+                    "timestamp": 2.1481416071211417
+                },
                 {
-                    "x": 7.628525640862077,
-                    "y": 7.461415071089343,
-                    "heading": 0.043910704401711645,
-                    "angularVelocity": -1.1204975190199653,
-                    "velocityX": 1.2902751552304177,
-                    "velocityY": 0.14102569073894916,
-                    "timestamp": 2.159511445918927
+                    "x": 7.619355739034295,
+                    "y": 7.45187793306489,
+                    "heading": 0.05195662802885072,
+                    "angularVelocity": -1.1817302050975884,
+                    "velocityX": 1.3849511435049664,
+                    "velocityY": 0.34670121107359286,
+                    "timestamp": 2.23595868929222
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
                     "heading": -0.003,
-                    "angularVelocity": -0.5865164647493907,
-                    "velocityX": 0.6435750546033896,
-                    "velocityY": 0.0698274051164111,
-                    "timestamp": 2.2394933513233317
+                    "angularVelocity": -0.6258079484101811,
+                    "velocityX": 0.6905747659386144,
+                    "velocityY": 0.1721996058312404,
+                    "timestamp": 2.323775771463298
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
                     "heading": -0.003,
                     "angularVelocity": 0,
-                    "velocityX": 4.312091780947568e-36,
+                    "velocityX": 2.3316560699920504e-34,
                     "velocityY": 0,
-                    "timestamp": 2.3194752567277366
+                    "timestamp": 2.4115928536343763
                 }
             ],
             "constraints": [
@@ -910,7 +1027,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 24
+                    "controlIntervalCount": 14
+                },
+                {
+                    "x": 2.159510374069214,
+                    "y": 6.264003276824951,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 15
                 },
                 {
                     "x": 5.9063591957092285,
@@ -936,316 +1062,361 @@
                     "x": 0.71,
                     "y": 6.72,
                     "heading": 1.04,
-                    "angularVelocity": 0,
-                    "velocityX": 2.674890564447798e-33,
-                    "velocityY": 3.559444452002106e-32,
+                    "angularVelocity": -7.919640222280865e-31,
+                    "velocityX": 0,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.744719766454751,
-                    "y": 6.720759968199705,
-                    "heading": 1.006760857121406,
-                    "angularVelocity": -0.5061419234074507,
-                    "velocityX": 0.5286878015432446,
-                    "velocityY": 0.011572252862695013,
-                    "timestamp": 0.06567158605400746
-                },
-                {
-                    "x": 0.8143576636925598,
-                    "y": 6.722372451876338,
-                    "heading": 0.9431499891720047,
-                    "angularVelocity": -0.9686208567749168,
-                    "velocityX": 1.0603961533765867,
-                    "velocityY": 0.024553749551892837,
-                    "timestamp": 0.1313431721080149
-                },
-                {
-                    "x": 0.9191612228651693,
-                    "y": 6.724923861576486,
-                    "heading": 0.8530685118915011,
-                    "angularVelocity": -1.3716963864162066,
-                    "velocityX": 1.5958737327650472,
-                    "velocityY": 0.03885104431723037,
-                    "timestamp": 0.19701475816202235
-                },
-                {
-                    "x": 1.059454644842809,
-                    "y": 6.7284663342548425,
-                    "heading": 0.7426650921374723,
-                    "angularVelocity": -1.6811444094442096,
-                    "velocityX": 2.1362880114128,
-                    "velocityY": 0.05394224338464699,
-                    "timestamp": 0.2626863442160298
-                },
-                {
-                    "x": 1.2356244745863587,
-                    "y": 6.733039509062333,
-                    "heading": 0.6243118436370656,
-                    "angularVelocity": -1.8021987226420155,
-                    "velocityX": 2.682588320596835,
-                    "velocityY": 0.06963703912619325,
-                    "timestamp": 0.3283579302700373
-                },
-                {
-                    "x": 1.4470569399809903,
-                    "y": 6.738777367450543,
-                    "heading": 0.5308220213971302,
-                    "angularVelocity": -1.4235962287107038,
-                    "velocityX": 3.219542546463745,
-                    "velocityY": 0.08737200870294055,
-                    "timestamp": 0.39402951632404476
-                },
-                {
-                    "x": 1.688226065456379,
-                    "y": 6.748371696739962,
-                    "heading": 0.5126606987274956,
-                    "angularVelocity": -0.2765476480908373,
-                    "velocityX": 3.6723511638207493,
-                    "velocityY": 0.14609559272058092,
-                    "timestamp": 0.45970110237805223
-                },
-                {
-                    "x": 1.9365780121804386,
-                    "y": 6.754665679712547,
-                    "heading": 0.5126606479364623,
-                    "angularVelocity": -7.734095747362075e-7,
-                    "velocityX": 3.7817260347544686,
-                    "velocityY": 0.09584027660688858,
-                    "timestamp": 0.5253726884320596
-                },
-                {
-                    "x": 2.1849299738494317,
-                    "y": 6.760959072962077,
-                    "heading": 0.5126605971462416,
-                    "angularVelocity": -7.733971982996713e-7,
-                    "velocityX": 3.7817262623252414,
-                    "velocityY": 0.09583129672545204,
-                    "timestamp": 0.5910442744860671
-                },
-                {
-                    "x": 2.433281935680659,
-                    "y": 6.767252459809602,
-                    "heading": 0.5126605463560266,
-                    "angularVelocity": -7.733971099299294e-7,
-                    "velocityX": 3.7817262647956325,
-                    "velocityY": 0.09583119924030911,
-                    "timestamp": 0.6567158605400746
-                },
-                {
-                    "x": 2.6816338977021794,
-                    "y": 6.773545839147862,
-                    "heading": 0.5126604955658177,
-                    "angularVelocity": -7.733970196193196e-7,
-                    "velocityX": 3.78172626769327,
-                    "velocityY": 0.09583108489457716,
-                    "timestamp": 0.722387446594082
-                },
-                {
-                    "x": 2.9299858599438156,
-                    "y": 6.779839209799928,
-                    "heading": 0.5126604447756148,
-                    "angularVelocity": -7.733969286509421e-7,
-                    "velocityX": 3.7817262710450463,
-                    "velocityY": 0.09583095262742326,
-                    "timestamp": 0.7880590326480895
-                },
-                {
-                    "x": 3.1783378224424883,
-                    "y": 6.786132570308856,
-                    "heading": 0.5126603939854177,
-                    "angularVelocity": -7.733968385113031e-7,
-                    "velocityX": 3.781726274959007,
-                    "velocityY": 0.09583079817497217,
-                    "timestamp": 0.853730618702097
-                },
-                {
-                    "x": 3.426689785244194,
-                    "y": 6.792425918859452,
-                    "heading": 0.5126603431952265,
-                    "angularVelocity": -7.733967515033378e-7,
-                    "velocityX": 3.7817262795733857,
-                    "velocityY": 0.09583061608201786,
-                    "timestamp": 0.9194022047561045
-                },
-                {
-                    "x": 3.6750417484067377,
-                    "y": 6.798719253170605,
-                    "heading": 0.5126602924050404,
-                    "angularVelocity": -7.733966712943809e-7,
-                    "velocityX": 3.781726285067957,
-                    "velocityY": 0.09583039925395942,
-                    "timestamp": 0.9850737908101119
-                },
-                {
-                    "x": 3.9233937120031053,
-                    "y": 6.805012570362013,
-                    "heading": 0.5126602416148589,
-                    "angularVelocity": -7.733966025007139e-7,
-                    "velocityX": 3.7817262916739214,
-                    "velocityY": 0.095830138566711,
-                    "timestamp": 1.0507453768641193
-                },
-                {
-                    "x": 4.171745676125189,
-                    "y": 6.811305866807266,
-                    "heading": 0.5126601908246804,
-                    "angularVelocity": -7.733965561950186e-7,
-                    "velocityX": 3.7817262996791623,
-                    "velocityY": 0.09582982265909266,
-                    "timestamp": 1.1164169629181266
-                },
-                {
-                    "x": 4.420097640886595,
-                    "y": 6.817599138023107,
-                    "heading": 0.5126601400345029,
-                    "angularVelocity": -7.733965406200526e-7,
-                    "velocityX": 3.781726309414286,
-                    "velocityY": 0.09582943848362173,
-                    "timestamp": 1.182088548972134
-                },
-                {
-                    "x": 4.6684496064150185,
-                    "y": 6.8238923789700765,
-                    "heading": 0.5126600892443262,
-                    "angularVelocity": -7.73396529455525e-7,
-                    "velocityX": 3.7817263210938923,
-                    "velocityY": 0.09582897757087808,
-                    "timestamp": 1.2477601350261414
-                },
-                {
-                    "x": 4.916801572841165,
-                    "y": 6.830185584489968,
-                    "heading": 0.5126600384541553,
-                    "angularVelocity": -7.733964406232834e-7,
-                    "velocityX": 3.7817263347637717,
-                    "velocityY": 0.09582843811261686,
-                    "timestamp": 1.3134317210801487
-                },
-                {
-                    "x": 5.165153539254683,
-                    "y": 6.8364787905062485,
-                    "heading": 0.5126599876639959,
-                    "angularVelocity": -7.733962639482034e-7,
-                    "velocityX": 3.781726334571479,
-                    "velocityY": 0.09582844567126618,
-                    "timestamp": 1.379103307134156
-                },
-                {
-                    "x": 5.413504500375102,
-                    "y": 6.842811545538402,
-                    "heading": 0.5126599368726894,
-                    "angularVelocity": -7.734137320837285e-7,
-                    "velocityX": 3.781711026686433,
-                    "velocityY": 0.09643066982787928,
-                    "timestamp": 1.4447748931881634
-                },
-                {
-                    "x": 5.661808131063952,
-                    "y": 6.834835924305975,
-                    "heading": 0.5126598860199024,
-                    "angularVelocity": -7.743499133059843e-7,
-                    "velocityX": 3.780990312684674,
-                    "velocityY": -0.12144706275568991,
-                    "timestamp": 1.5104464792421708
+                    "x": 0.7245966822673344,
+                    "y": 6.714849739719678,
+                    "heading": 1.024358911223375,
+                    "angularVelocity": -0.35643014194665995,
+                    "velocityX": 0.33263013891161514,
+                    "velocityY": -0.1173644641361684,
+                    "timestamp": 0.04388262084457728
+                },
+                {
+                    "x": 0.7538441442080397,
+                    "y": 6.704534883949825,
+                    "heading": 0.9938971402916456,
+                    "angularVelocity": -0.6941648047781096,
+                    "velocityX": 0.6664930530083778,
+                    "velocityY": -0.23505559994630235,
+                    "timestamp": 0.08776524168915456
+                },
+                {
+                    "x": 0.7978084886925392,
+                    "y": 6.689039857166789,
+                    "heading": 0.9496619935965729,
+                    "angularVelocity": -1.0080333818652591,
+                    "velocityX": 1.0018623235886674,
+                    "velocityY": -0.3531016717960754,
+                    "timestamp": 0.13164786253373184
+                },
+                {
+                    "x": 0.856569122575896,
+                    "y": 6.668349449234606,
+                    "heading": 0.8929800253175195,
+                    "angularVelocity": -1.291672356576085,
+                    "velocityX": 1.3390411227143098,
+                    "velocityY": -0.4714943532072734,
+                    "timestamp": 0.17553048337830912
+                },
+                {
+                    "x": 0.9302192448226602,
+                    "y": 6.642448160442897,
+                    "heading": 0.8255526698192454,
+                    "angularVelocity": -1.5365389350167389,
+                    "velocityX": 1.6783437458676926,
+                    "velocityY": -0.5902402430203169,
+                    "timestamp": 0.2194131042228864
+                },
+                {
+                    "x": 1.018865391503866,
+                    "y": 6.6113159895257,
+                    "heading": 0.7496899973128085,
+                    "angularVelocity": -1.728763484184805,
+                    "velocityX": 2.020074119893375,
+                    "velocityY": -0.7094419229748447,
+                    "timestamp": 0.2632957250674637
+                },
+                {
+                    "x": 1.122620892464334,
+                    "y": 6.574924283447733,
+                    "heading": 0.6689630682339129,
+                    "angularVelocity": -1.839610477341524,
+                    "velocityX": 2.364387061747502,
+                    "velocityY": -0.8292965501492129,
+                    "timestamp": 0.30717834591204096
+                },
+                {
+                    "x": 1.2415335684286195,
+                    "y": 6.533261700289126,
+                    "heading": 0.5901648629283005,
+                    "angularVelocity": -1.7956585953398425,
+                    "velocityX": 2.7097897453635067,
+                    "velocityY": -0.9494096377266702,
+                    "timestamp": 0.35106096675661824
+                },
+                {
+                    "x": 1.3748018786634473,
+                    "y": 6.486525528750173,
+                    "heading": 0.5293493772235701,
+                    "angularVelocity": -1.3858672188271242,
+                    "velocityX": 3.03692686694496,
+                    "velocityY": -1.0650268976507753,
+                    "timestamp": 0.3949435876011955
+                },
+                {
+                    "x": 1.5197726766571482,
+                    "y": 6.4335443318697845,
+                    "heading": 0.5007639504532418,
+                    "angularVelocity": -0.6514065527572436,
+                    "velocityX": 3.3036039143420037,
+                    "velocityY": -1.2073389387557742,
+                    "timestamp": 0.4388262084457728
+                },
+                {
+                    "x": 1.6765216724762246,
+                    "y": 6.378886133078717,
+                    "heading": 0.5007639037196894,
+                    "angularVelocity": -0.0000010649672122148667,
+                    "velocityX": 3.572006247627606,
+                    "velocityY": -1.2455545666831078,
+                    "timestamp": 0.4827088292903501
+                },
+                {
+                    "x": 1.8335562881896872,
+                    "y": 6.325054024772622,
+                    "heading": 0.5007638569717894,
+                    "angularVelocity": -0.0000010652941670826544,
+                    "velocityX": 3.5785149722331555,
+                    "velocityY": -1.2267295633425939,
+                    "timestamp": 0.5265914501349274
+                },
+                {
+                    "x": 1.9950553751457016,
+                    "y": 6.286637737718841,
+                    "heading": 0.5007638092073705,
+                    "angularVelocity": -0.0000010884586644439279,
+                    "velocityX": 3.6802516314615477,
+                    "velocityY": -0.8754328322739401,
+                    "timestamp": 0.5704740709795046
+                },
+                {
+                    "x": 2.159510374069214,
+                    "y": 6.264003276824951,
+                    "heading": 0.500763757205963,
+                    "angularVelocity": -0.0000011850114334401085,
+                    "velocityX": 3.7476111444210547,
+                    "velocityY": -0.5157955577412445,
+                    "timestamp": 0.6143566918240819
+                },
+                {
+                    "x": 2.4127218287177645,
+                    "y": 6.26676408374459,
+                    "heading": 0.5007637073683532,
+                    "angularVelocity": -7.445219928061725e-7,
+                    "velocityX": 3.7827154561393628,
+                    "velocityY": 0.04124358046976413,
+                    "timestamp": 0.681295761808209
+                },
+                {
+                    "x": 2.6627712176567337,
+                    "y": 6.306751164644663,
+                    "heading": 0.5007636590304977,
+                    "angularVelocity": -7.221172264287654e-7,
+                    "velocityX": 3.7354774871878274,
+                    "velocityY": 0.5973653489565757,
+                    "timestamp": 0.7482348317923362
+                },
+                {
+                    "x": 2.9117460938747426,
+                    "y": 6.352959114176208,
+                    "heading": 0.5007636106421453,
+                    "angularVelocity": -7.22871595873179e-7,
+                    "velocityX": 3.719425386056917,
+                    "velocityY": 0.6902986483459334,
+                    "timestamp": 0.8151739017764633
+                },
+                {
+                    "x": 3.160720959556854,
+                    "y": 6.399167120476624,
+                    "heading": 0.5007635622537979,
+                    "angularVelocity": -7.228715220114282e-7,
+                    "velocityX": 3.719425228661581,
+                    "velocityY": 0.6902994964137207,
+                    "timestamp": 0.8821129717605904
+                },
+                {
+                    "x": 3.409695827610573,
+                    "y": 6.445375113998521,
+                    "heading": 0.5007635138654579,
+                    "angularVelocity": -7.228714109969877e-7,
+                    "velocityX": 3.719425264090935,
+                    "velocityY": 0.6902993055159711,
+                    "timestamp": 0.9490520417447176
+                },
+                {
+                    "x": 3.658670698533997,
+                    "y": 6.491583092058085,
+                    "heading": 0.5007634654771249,
+                    "angularVelocity": -7.228713077638952e-7,
+                    "velocityX": 3.719425306961334,
+                    "velocityY": 0.6902990745248123,
+                    "timestamp": 1.0159911117288447
+                },
+                {
+                    "x": 3.9076455729725814,
+                    "y": 6.537791051177508,
+                    "heading": 0.5007634170887977,
+                    "angularVelocity": -7.228712197353323e-7,
+                    "velocityX": 3.7194253594741626,
+                    "velocityY": 0.6902987915788225,
+                    "timestamp": 1.082930181712972
+                },
+                {
+                    "x": 4.156620451761658,
+                    "y": 6.583998986855888,
+                    "heading": 0.5007633687004744,
+                    "angularVelocity": -7.228711615866084e-7,
+                    "velocityX": 3.7194254244659963,
+                    "velocityY": 0.6902984413940524,
+                    "timestamp": 1.1498692516970992
+                },
+                {
+                    "x": 4.405595335954841,
+                    "y": 6.630206893416186,
+                    "heading": 0.5007633203121516,
+                    "angularVelocity": -7.22871154439704e-7,
+                    "velocityX": 3.719425505197721,
+                    "velocityY": 0.6902980064002329,
+                    "timestamp": 1.2168083216812264
+                },
+                {
+                    "x": 4.65457022677848,
+                    "y": 6.676414764250624,
+                    "heading": 0.5007632719238245,
+                    "angularVelocity": -7.228712197620323e-7,
+                    "velocityX": 3.719425604249829,
+                    "velocityY": 0.690297472693087,
+                    "timestamp": 1.2837473916653537
+                },
+                {
+                    "x": 4.903545125380086,
+                    "y": 6.722622593176218,
+                    "heading": 0.5007632235354964,
+                    "angularVelocity": -7.228712343344299e-7,
+                    "velocityX": 3.7194257204445353,
+                    "velocityY": 0.69029684661859,
+                    "timestamp": 1.350686461649481
+                },
+                {
+                    "x": 5.152520035964963,
+                    "y": 6.768830357534066,
+                    "heading": 0.500763175147177,
+                    "angularVelocity": -7.228711020086952e-7,
+                    "velocityX": 3.7194258994621494,
+                    "velocityY": 0.6902958820437133,
+                    "timestamp": 1.4176255316336082
+                },
+                {
+                    "x": 5.401781723000098,
+                    "y": 6.813465264887065,
+                    "heading": 0.5007631267776413,
+                    "angularVelocity": -7.225904944160688e-7,
+                    "velocityX": 3.7237100410005364,
+                    "velocityY": 0.6667990362518919,
+                    "timestamp": 1.4845646016177354
+                },
+                {
+                    "x": 5.654897775782049,
+                    "y": 6.8209436894170015,
+                    "heading": 0.5007630775196736,
+                    "angularVelocity": -7.358627453849351e-7,
+                    "velocityX": 3.781290251597022,
+                    "velocityY": 0.11171987498067794,
+                    "timestamp": 1.5515036716018626
                 },
                 {
                     "x": 5.9063591957092285,
                     "y": 6.791097164154053,
-                    "heading": 0.5126598314406843,
-                    "angularVelocity": -8.310933457001734e-7,
-                    "velocityX": 3.723848917614642,
-                    "velocityY": -0.6660225948568639,
-                    "timestamp": 1.5761180652961782
-                },
-                {
-                    "x": 6.194622556481831,
-                    "y": 6.683762426219015,
-                    "heading": 0.5126597887806125,
-                    "angularVelocity": -5.246475178274693e-7,
-                    "velocityX": 3.5451571072929595,
-                    "velocityY": -1.3200377183874616,
-                    "timestamp": 1.65742993725234
-                },
-                {
-                    "x": 6.459547388261047,
-                    "y": 6.527457130068993,
-                    "heading": 0.5126597482300577,
-                    "angularVelocity": -4.987039864896594e-7,
-                    "velocityX": 3.2581322432449182,
-                    "velocityY": -1.9222936625379825,
-                    "timestamp": 1.7387418092085019
-                },
-                {
-                    "x": 6.723521508223802,
-                    "y": 6.3695515377198895,
-                    "heading": 0.5126597076877473,
-                    "angularVelocity": -4.98602596336278e-7,
-                    "velocityX": 3.2464400783305223,
-                    "velocityY": -1.9419746286809643,
-                    "timestamp": 1.8200536811646637
-                },
-                {
-                    "x": 6.987495332330779,
-                    "y": 6.211645450805702,
-                    "heading": 0.5126596671480953,
-                    "angularVelocity": -4.985699012832353e-7,
-                    "velocityX": 3.246436439802837,
-                    "velocityY": -1.9419807109984206,
-                    "timestamp": 1.9013655531208256
-                },
-                {
-                    "x": 7.218138260463332,
-                    "y": 6.072816355413413,
-                    "heading": 0.4174347514218684,
-                    "angularVelocity": -1.1711071635093024,
-                    "velocityX": 2.8365221779297074,
-                    "velocityY": -1.7073656288101424,
-                    "timestamp": 1.9826774250769874
-                },
-                {
-                    "x": 7.40359391202109,
-                    "y": 5.961763553999033,
-                    "heading": 0.26911254592387807,
-                    "angularVelocity": -1.8241150022712385,
-                    "velocityX": 2.2807942689821186,
-                    "velocityY": -1.365763679308134,
-                    "timestamp": 2.063989297033149
-                },
-                {
-                    "x": 7.542069024042313,
-                    "y": 5.879081457057784,
-                    "heading": 0.13934312775190755,
-                    "angularVelocity": -1.595946754760921,
-                    "velocityX": 1.7030122255201257,
-                    "velocityY": -1.0168514750927211,
-                    "timestamp": 2.145301168989311
-                },
-                {
-                    "x": 7.63409233729861,
-                    "y": 5.824280880315082,
-                    "heading": 0.04592275308495019,
-                    "angularVelocity": -1.1489143272623823,
-                    "velocityX": 1.1317328090287153,
-                    "velocityY": -0.6739554191077514,
-                    "timestamp": 2.2266130409454727
+                    "heading": 0.500763020915307,
+                    "angularVelocity": -8.456102929151622e-7,
+                    "velocityX": 3.756571759763139,
+                    "velocityY": -0.44587600739018746,
+                    "timestamp": 1.6184427415859899
+                },
+                {
+                    "x": 6.200798567763329,
+                    "y": 6.700713487178965,
+                    "heading": 0.5007629755779084,
+                    "angularVelocity": -5.568470838728409e-7,
+                    "velocityX": 3.616389792279645,
+                    "velocityY": -1.1101185433221765,
+                    "timestamp": 1.6998607824959264
+                },
+                {
+                    "x": 6.474342410112796,
+                    "y": 6.559159535827226,
+                    "heading": 0.5007629354438458,
+                    "angularVelocity": -4.929382012989922e-7,
+                    "velocityX": 3.3597448340971368,
+                    "velocityY": -1.7386066990769444,
+                    "timestamp": 1.781278823405863
+                },
+                {
+                    "x": 6.734766818786864,
+                    "y": 6.394712333057686,
+                    "heading": 0.5007628957809265,
+                    "angularVelocity": -4.871514819255018e-7,
+                    "velocityX": 3.1986081433983946,
+                    "velocityY": -2.0197882549329598,
+                    "timestamp": 1.8626968643157995
+                },
+                {
+                    "x": 6.9951910734023155,
+                    "y": 6.230264886316009,
+                    "heading": 0.5007628561176116,
+                    "angularVelocity": -4.871563403692846e-7,
+                    "velocityX": 3.198606251205787,
+                    "velocityY": -2.019791251469529,
+                    "timestamp": 1.944114905225736
+                },
+                {
+                    "x": 7.223286683326485,
+                    "y": 6.0852317833380125,
+                    "heading": 0.4070504938542789,
+                    "angularVelocity": -1.1510024217727797,
+                    "velocityX": 2.801536457705794,
+                    "velocityY": -1.781338648745319,
+                    "timestamp": 2.0255329461356726
+                },
+                {
+                    "x": 7.40663062232063,
+                    "y": 5.969243166944327,
+                    "heading": 0.26222076447726717,
+                    "angularVelocity": -1.7788407551738947,
+                    "velocityX": 2.2518834516904875,
+                    "velocityY": -1.4246058379369684,
+                    "timestamp": 2.106950987045609
+                },
+                {
+                    "x": 7.543566910533025,
+                    "y": 5.882836756425157,
+                    "heading": 0.1356651998410208,
+                    "angularVelocity": -1.5543921620054602,
+                    "velocityX": 1.6818912206923584,
+                    "velocityY": -1.061268602799615,
+                    "timestamp": 2.1883690279555457
+                },
+                {
+                    "x": 7.634586620516168,
+                    "y": 5.825537139087579,
+                    "heading": 0.04464428184824825,
+                    "angularVelocity": -1.1179453223820228,
+                    "velocityX": 1.1179304852572838,
+                    "velocityY": -0.7037705242866615,
+                    "timestamp": 2.269787068865482
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
                     "heading": -0.003,
-                    "angularVelocity": -0.6016680210157614,
-                    "velocityX": 0.5645874531846384,
-                    "velocityY": -0.3355091902172136,
-                    "timestamp": 2.3079249129016346
+                    "angularVelocity": -0.5851808925364741,
+                    "velocityX": 0.5577803024524691,
+                    "velocityY": -0.35050142165846687,
+                    "timestamp": 2.3512051097754187
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
                     "heading": -0.003,
-                    "angularVelocity": 3.440382093642737e-30,
+                    "angularVelocity": 0,
                     "velocityX": 0,
-                    "velocityY": 2.1122019835026996e-31,
-                    "timestamp": 2.3892367848577964
+                    "velocityY": 0,
+                    "timestamp": 2.4326231506853553
                 }
             ],
             "constraints": [
@@ -1282,6 +1453,15 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 2.817133903503418,
+                    "y": 6.303859233856201,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
                     "controlIntervalCount": 29
                 },
                 {
@@ -1310,521 +1490,665 @@
                     "heading": 1.04,
                     "angularVelocity": 0,
                     "velocityX": 0,
-                    "velocityY": 1.3928609100313025e-33,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.729279758485921,
-                    "y": 6.711628327586588,
-                    "heading": 1.093428142295198,
-                    "angularVelocity": 0.9770378814200095,
-                    "velocityX": 0.352568020824986,
-                    "velocityY": -0.15309237280887605,
-                    "timestamp": 0.05468379815278649
-                },
-                {
-                    "x": 0.7686493967347447,
-                    "y": 6.69501140584797,
-                    "heading": 1.1949690599508662,
-                    "angularVelocity": 1.8568739020644303,
-                    "velocityX": 0.7199506906748694,
-                    "velocityY": -0.30387285265355396,
-                    "timestamp": 0.10936759630557298
-                },
-                {
-                    "x": 0.8290790315170126,
-                    "y": 6.670059182269693,
-                    "heading": 1.3362954219692313,
-                    "angularVelocity": 2.584428419245852,
-                    "velocityX": 1.1050738394839994,
-                    "velocityY": -0.4563001185206737,
-                    "timestamp": 0.16405139445835948
-                },
-                {
-                    "x": 0.9115429372168562,
-                    "y": 6.636285478537955,
-                    "heading": 1.5055888656223098,
-                    "angularVelocity": 3.0958611027725738,
-                    "velocityX": 1.508013497333151,
-                    "velocityY": -0.6176181039468854,
-                    "timestamp": 0.21873519261114596
-                },
-                {
-                    "x": 1.0168329273736771,
-                    "y": 6.593191021214006,
-                    "heading": 1.6881423696003794,
-                    "angularVelocity": 3.3383471913932383,
-                    "velocityX": 1.9254330114861617,
-                    "velocityY": -0.7880662788554578,
-                    "timestamp": 0.27341899076393245
-                },
-                {
-                    "x": 1.1449422871140338,
-                    "y": 6.540878714080721,
-                    "heading": 1.8564615478628756,
-                    "angularVelocity": 3.078044758197163,
-                    "velocityX": 2.3427297310698694,
-                    "velocityY": -0.9566326572109004,
-                    "timestamp": 0.32810278891671896
-                },
-                {
-                    "x": 1.2912409780613738,
-                    "y": 6.479735868066328,
-                    "heading": 1.974688906048384,
-                    "angularVelocity": 2.1620180415263293,
-                    "velocityX": 2.675357160425865,
-                    "velocityY": -1.1181162991560991,
-                    "timestamp": 0.38278658706950547
-                },
-                {
-                    "x": 1.4553681499836075,
-                    "y": 6.408144093279595,
-                    "heading": 2.0430197361851268,
-                    "angularVelocity": 1.2495626208301445,
-                    "velocityX": 3.001385738855639,
-                    "velocityY": -1.3091953595963746,
-                    "timestamp": 0.437470385222292
-                },
-                {
-                    "x": 1.6374055328463037,
-                    "y": 6.326368400043195,
-                    "heading": 2.0610350683831213,
-                    "angularVelocity": 0.3294455178051113,
-                    "velocityX": 3.328908909254691,
-                    "velocityY": -1.4954281889476506,
-                    "timestamp": 0.4921541833750785
-                },
-                {
-                    "x": 1.8283093127689483,
-                    "y": 6.246687216839916,
-                    "heading": 2.061035169590908,
-                    "angularVelocity": 0.000001850781943801841,
-                    "velocityX": 3.4910482880003957,
-                    "velocityY": -1.45712598420193,
-                    "timestamp": 0.546837981527865
-                },
-                {
-                    "x": 2.01921313853121,
-                    "y": 6.167006143460992,
-                    "heading": 2.0610352707983473,
-                    "angularVelocity": 0.0000018507755948234292,
-                    "velocityX": 3.491049126267287,
-                    "velocityY": -1.4571239758492094,
-                    "timestamp": 0.6015217796806515
-                },
-                {
-                    "x": 2.210116959480156,
-                    "y": 6.087325058550032,
-                    "heading": 2.061035372005735,
-                    "angularVelocity": 0.000001850774661522582,
-                    "velocityX": 3.4910490382464,
-                    "velocityY": -1.4571241867349893,
-                    "timestamp": 0.6562055778334379
-                },
-                {
-                    "x": 2.40102077504018,
-                    "y": 6.007643960727976,
-                    "heading": 2.0610354732130722,
-                    "angularVelocity": 0.0000018507737318277825,
-                    "velocityX": 3.49104893969945,
-                    "velocityY": -1.4571244228395928,
-                    "timestamp": 0.7108893759862244
-                },
-                {
-                    "x": 2.5919245845305627,
-                    "y": 5.927962848363926,
-                    "heading": 2.0610355744203592,
-                    "angularVelocity": 0.0000018507728139380304,
-                    "velocityX": 3.491048828704212,
-                    "velocityY": -1.457124688768356,
-                    "timestamp": 0.7655731741390108
-                },
-                {
-                    "x": 2.78282838714305,
-                    "y": 5.848281719521431,
-                    "heading": 2.0610356756275974,
-                    "angularVelocity": 0.0000018507719179726602,
-                    "velocityX": 3.4910487029284663,
-                    "velocityY": -1.457124990108895,
-                    "timestamp": 0.8202569722917973
-                },
-                {
-                    "x": 2.973732181912291,
-                    "y": 5.768600571887663,
-                    "heading": 2.061035776834788,
-                    "angularVelocity": 0.0000018507710613284672,
-                    "velocityX": 3.491048559499401,
-                    "velocityY": -1.4571253337439838,
-                    "timestamp": 0.8749407704445837
-                },
-                {
-                    "x": 3.16463596766188,
-                    "y": 5.688919402644143,
-                    "heading": 2.061035878041936,
-                    "angularVelocity": 0.000001850770272196717,
-                    "velocityX": 3.4910483945574473,
-                    "velocityY": -1.4571257289205004,
-                    "timestamp": 0.9296245685973702
-                },
-                {
-                    "x": 3.3555397425941185,
-                    "y": 5.609238207483871,
-                    "heading": 2.061035979249042,
-                    "angularVelocity": 0.000001850769506382635,
-                    "velocityX": 3.4910481967410822,
-                    "velocityY": -1.457126202858873,
-                    "timestamp": 0.9843083667501566
-                },
-                {
-                    "x": 3.546443239175115,
-                    "y": 5.529556345440524,
-                    "heading": 2.0610360804562036,
-                    "angularVelocity": 0.0000018507705174022697,
-                    "velocityX": 3.491043106545231,
-                    "velocityY": -1.4571383981177848,
-                    "timestamp": 1.0389921649029432
-                },
-                {
-                    "x": 3.7301891812097523,
-                    "y": 5.434525969364101,
-                    "heading": 2.0610361860102304,
-                    "angularVelocity": 0.0000019302614304565207,
-                    "velocityX": 3.360153249071168,
-                    "velocityY": -1.7378159397580357,
-                    "timestamp": 1.0936759630557298
-                },
-                {
-                    "x": 3.9011674048607876,
-                    "y": 5.318079465137689,
-                    "heading": 2.0610363082632275,
-                    "angularVelocity": 0.0000022356347107367405,
-                    "velocityX": 3.126670593972313,
-                    "velocityY": -2.129451650396711,
-                    "timestamp": 1.1483597612085163
-                },
-                {
-                    "x": 4.05689386248419,
-                    "y": 5.181908226701039,
-                    "heading": 2.0610364709885136,
-                    "angularVelocity": 0.0000029757495174652245,
-                    "velocityX": 2.8477622784778513,
-                    "velocityY": -2.490156920998575,
-                    "timestamp": 1.203043559361303
-                },
-                {
-                    "x": 4.195106185477333,
-                    "y": 5.027990436285506,
-                    "heading": 2.061036659879246,
-                    "angularVelocity": 0.0000034542357869786317,
-                    "velocityX": 2.52748213660978,
-                    "velocityY": -2.814687267798894,
-                    "timestamp": 1.2577273575140895
-                },
-                {
-                    "x": 4.313711649853664,
-                    "y": 4.858669951905228,
-                    "heading": 2.0606108116890636,
-                    "angularVelocity": -0.007787465475470693,
-                    "velocityX": 2.168932451344132,
-                    "velocityY": -3.096355595256869,
-                    "timestamp": 1.312411155666876
-                },
-                {
-                    "x": 4.40996698448611,
-                    "y": 4.67856050613378,
-                    "heading": 2.051588761211157,
-                    "angularVelocity": -0.16498580535132465,
-                    "velocityX": 1.7602166982532683,
-                    "velocityY": -3.293652815925874,
-                    "timestamp": 1.3670949538196626
-                },
-                {
-                    "x": 4.510742037442817,
-                    "y": 4.51918682060045,
-                    "heading": 1.996778499947209,
-                    "angularVelocity": -1.002312624862085,
-                    "velocityX": 1.8428685709639585,
-                    "velocityY": -2.9144589607334965,
-                    "timestamp": 1.4217787519724492
-                },
-                {
-                    "x": 4.62612450472694,
-                    "y": 4.379617585881877,
-                    "heading": 1.9276644376664414,
-                    "angularVelocity": -1.263885549567404,
-                    "velocityX": 2.1099936577511538,
-                    "velocityY": -2.5522959163995464,
-                    "timestamp": 1.4764625501252358
-                },
-                {
-                    "x": 4.756154634809633,
-                    "y": 4.26013511834241,
-                    "heading": 1.8517335763640235,
-                    "angularVelocity": -1.3885440270675238,
-                    "velocityX": 2.3778547664042544,
-                    "velocityY": -2.1849701662205963,
-                    "timestamp": 1.5311463482780223
+                    "x": 0.7276374646295759,
+                    "y": 6.716389445106627,
+                    "heading": 1.0769704869840742,
+                    "angularVelocity": 0.7463572831464532,
+                    "velocityX": 0.35606374858489526,
+                    "velocityY": -0.07288959818107699,
+                    "timestamp": 0.0495345698619811
+                },
+                {
+                    "x": 0.7632753901023804,
+                    "y": 6.70941368019147,
+                    "heading": 1.1486786527951847,
+                    "angularVelocity": 1.4476388108510687,
+                    "velocityX": 0.7194556361777777,
+                    "velocityY": -0.1408261933931805,
+                    "timestamp": 0.0990691397239622
+                },
+                {
+                    "x": 0.8173493088554353,
+                    "y": 6.699320044331327,
+                    "heading": 1.251948933720564,
+                    "angularVelocity": 2.084812308111838,
+                    "velocityX": 1.0916400183503512,
+                    "velocityY": -0.20376952677844481,
+                    "timestamp": 0.1486037095859433
+                },
+                {
+                    "x": 0.8904201618241163,
+                    "y": 6.686277798850656,
+                    "heading": 1.3814946265878076,
+                    "angularVelocity": 2.615258257579252,
+                    "velocityX": 1.4751486320027993,
+                    "velocityY": -0.263295825859426,
+                    "timestamp": 0.1981382794479244
+                },
+                {
+                    "x": 0.9831303708035419,
+                    "y": 6.670301294322549,
+                    "heading": 1.5290667264747733,
+                    "angularVelocity": 2.9791739445430228,
+                    "velocityX": 1.871626406322422,
+                    "velocityY": -0.32253241669944677,
+                    "timestamp": 0.2476728493099055
+                },
+                {
+                    "x": 1.0960137924135722,
+                    "y": 6.651369432356597,
+                    "heading": 1.6827078385240544,
+                    "angularVelocity": 3.1016946846903584,
+                    "velocityX": 2.278881636090858,
+                    "velocityY": -0.3821949401851956,
+                    "timestamp": 0.2972074191718866
+                },
+                {
+                    "x": 1.2286250246243187,
+                    "y": 6.629796552300239,
+                    "heading": 1.8192744268134131,
+                    "angularVelocity": 2.7569955421025307,
+                    "velocityX": 2.677145124715176,
+                    "velocityY": -0.4355116056504451,
+                    "timestamp": 0.34674198903386766
+                },
+                {
+                    "x": 1.37806258880499,
+                    "y": 6.607677950088394,
+                    "heading": 1.9114236020956594,
+                    "angularVelocity": 1.8603003021733433,
+                    "velocityX": 3.0168337909701455,
+                    "velocityY": -0.446528601597435,
+                    "timestamp": 0.3962765588958488
+                },
+                {
+                    "x": 1.5443963470492121,
+                    "y": 6.584716640433465,
+                    "heading": 1.9605466016146158,
+                    "angularVelocity": 0.991691250288047,
+                    "velocityX": 3.3579328276762688,
+                    "velocityY": -0.4635411131687398,
+                    "timestamp": 0.4458111287578299
+                },
+                {
+                    "x": 1.7276120279383835,
+                    "y": 6.5605525122280195,
+                    "heading": 1.9670840049159166,
+                    "angularVelocity": 0.13197658361397824,
+                    "velocityX": 3.698743754106468,
+                    "velocityY": -0.4878235194494175,
+                    "timestamp": 0.49534569861981104
+                },
+                {
+                    "x": 1.9125583307039895,
+                    "y": 6.530411390267246,
+                    "heading": 1.9670841172507754,
+                    "angularVelocity": 0.0000022678072915558937,
+                    "velocityX": 3.7336814124171944,
+                    "velocityY": -0.6084865992338514,
+                    "timestamp": 0.5448802684817922
+                },
+                {
+                    "x": 2.0974975773537823,
+                    "y": 6.500227003772903,
+                    "heading": 1.967084229554738,
+                    "angularVelocity": 0.0000022671835628273576,
+                    "velocityX": 3.733538964104325,
+                    "velocityY": -0.6093600202458971,
+                    "timestamp": 0.5944148383437733
+                },
+                {
+                    "x": 2.2824570922079714,
+                    "y": 6.470167063452025,
+                    "heading": 1.9670843418904687,
+                    "angularVelocity": 0.0000022678248970866596,
+                    "velocityX": 3.7339481370144885,
+                    "velocityY": -0.6068477107210625,
+                    "timestamp": 0.6439494082057544
+                },
+                {
+                    "x": 2.4663667602317028,
+                    "y": 6.434238576599238,
+                    "heading": 1.9670844533817493,
+                    "angularVelocity": 0.000002250777202166417,
+                    "velocityX": 3.712753911780929,
+                    "velocityY": -0.7253214664890316,
+                    "timestamp": 0.6934839780677355
+                },
+                {
+                    "x": 2.6453124624138216,
+                    "y": 6.3786323067408315,
+                    "heading": 1.96708456679077,
+                    "angularVelocity": 0.000002289492388953058,
+                    "velocityX": 3.6125417598312857,
+                    "velocityY": -1.1225750019579073,
+                    "timestamp": 0.7430185479297167
+                },
+                {
+                    "x": 2.817133903503418,
+                    "y": 6.303859233856201,
+                    "heading": 1.96708468933932,
+                    "angularVelocity": 0.0000024740004860391374,
+                    "velocityX": 3.468717737283832,
+                    "velocityY": -1.5095129137971557,
+                    "timestamp": 0.7925531177916978
+                },
+                {
+                    "x": 2.91295588800905,
+                    "y": 6.254800700059255,
+                    "heading": 1.967084804070116,
+                    "angularVelocity": 0.000004031753460568297,
+                    "velocityX": 3.367279151013272,
+                    "velocityY": -1.7239653184522483,
+                    "timestamp": 0.8210099163546827
+                },
+                {
+                    "x": 3.0055158966135553,
+                    "y": 6.199834910977839,
+                    "heading": 1.9670849118890987,
+                    "angularVelocity": 0.000003788865518014884,
+                    "velocityX": 3.2526500969440626,
+                    "velocityY": -1.9315521020251178,
+                    "timestamp": 0.8494667149176676
+                },
+                {
+                    "x": 3.0944547185216633,
+                    "y": 6.139184440666953,
+                    "heading": 1.96708501618758,
+                    "angularVelocity": 0.0000036651516103387324,
+                    "velocityX": 3.125398020841055,
+                    "velocityY": -2.13131741353383,
+                    "timestamp": 0.8779235134806525
+                },
+                {
+                    "x": 3.179446617500541,
+                    "y": 6.0731165142841,
+                    "heading": 1.9670851198382022,
+                    "angularVelocity": 0.0000036423852113204396,
+                    "velocityX": 2.986699251881383,
+                    "velocityY": -2.3216921691473744,
+                    "timestamp": 0.9063803120436373
+                },
+                {
+                    "x": 3.2604149841370136,
+                    "y": 6.002174866532259,
+                    "heading": 1.9670852253780247,
+                    "angularVelocity": 0.0000037087735735672327,
+                    "velocityX": 2.845308352497901,
+                    "velocityY": -2.4929595504099007,
+                    "timestamp": 0.9348371106066222
+                },
+                {
+                    "x": 3.3400950350688365,
+                    "y": 5.929789210280793,
+                    "heading": 1.9670853319364994,
+                    "angularVelocity": 0.000003744570018753785,
+                    "velocityX": 2.8000356665421786,
+                    "velocityY": -2.5437034349136662,
+                    "timestamp": 0.9632939091696071
+                },
+                {
+                    "x": 3.4197035202865202,
+                    "y": 5.857324854756433,
+                    "heading": 1.9670854385695549,
+                    "angularVelocity": 0.0000037471908580905978,
+                    "velocityX": 2.7975207766800336,
+                    "velocityY": -2.5464690050744796,
+                    "timestamp": 0.991750707732592
+                },
+                {
+                    "x": 3.4993494510128746,
+                    "y": 5.7849016577830055,
+                    "heading": 1.9670855451672207,
+                    "angularVelocity": 0.0000037459472354931967,
+                    "velocityX": 2.7988366488248966,
+                    "velocityY": -2.5450226529586133,
+                    "timestamp": 1.0202075062955769
+                },
+                {
+                    "x": 3.579011291529686,
+                    "y": 5.712495961052324,
+                    "heading": 1.96708565174968,
+                    "angularVelocity": 0.0000037454128599628807,
+                    "velocityX": 2.7993957345647376,
+                    "velocityY": -2.544407677155567,
+                    "timestamp": 1.0486643048585618
+                },
+                {
+                    "x": 3.658639843469943,
+                    "y": 5.640053656617691,
+                    "heading": 1.9670857583635384,
+                    "angularVelocity": 0.000003746516260337957,
+                    "velocityX": 2.7982259411140946,
+                    "velocityY": -2.545694108020349,
+                    "timestamp": 1.0771211034215467
+                },
+                {
+                    "x": 3.7382184197836335,
+                    "y": 5.567556457074997,
+                    "heading": 1.9670858650249143,
+                    "angularVelocity": 0.0000037481860642492875,
+                    "velocityX": 2.796469748251449,
+                    "velocityY": -2.547623176320097,
+                    "timestamp": 1.1055779019845315
+                },
+                {
+                    "x": 3.81773211309123,
+                    "y": 5.494988100908759,
+                    "heading": 1.967085971748553,
+                    "angularVelocity": 0.0000037503740493629023,
+                    "velocityX": 2.794189695354055,
+                    "velocityY": -2.5501236903290603,
+                    "timestamp": 1.1340347005475164
+                },
+                {
+                    "x": 3.8971703522513814,
+                    "y": 5.422337155346324,
+                    "heading": 1.967086078550155,
+                    "angularVelocity": 0.000003753113746455045,
+                    "velocityX": 2.7915381621131705,
+                    "velocityY": -2.5530259632560406,
+                    "timestamp": 1.1624914991105013
+                },
+                {
+                    "x": 3.9762575655393007,
+                    "y": 5.349304242205494,
+                    "heading": 1.9670861858107123,
+                    "angularVelocity": 0.0000037692418878753097,
+                    "velocityX": 2.779202766357265,
+                    "velocityY": -2.566448681118608,
+                    "timestamp": 1.1909482976734862
+                },
+                {
+                    "x": 4.0513085153658634,
+                    "y": 5.27212945123473,
+                    "heading": 1.9670862984844089,
+                    "angularVelocity": 0.000003959464953334102,
+                    "velocityX": 2.637364482882678,
+                    "velocityY": -2.7119983577895854,
+                    "timestamp": 1.219405096236471
+                },
+                {
+                    "x": 4.121436785634375,
+                    "y": 5.190455596279305,
+                    "heading": 1.9670864209307273,
+                    "angularVelocity": 0.000004302884530781832,
+                    "velocityX": 2.4643766625150745,
+                    "velocityY": -2.8700999086264516,
+                    "timestamp": 1.247861894799456
+                },
+                {
+                    "x": 4.18632086285232,
+                    "y": 5.104556536444466,
+                    "heading": 1.9670865586915784,
+                    "angularVelocity": 0.0000048410523378145445,
+                    "velocityX": 2.280090540555213,
+                    "velocityY": -3.0185777800939864,
+                    "timestamp": 1.2763186933624409
+                },
+                {
+                    "x": 4.245696839289754,
+                    "y": 5.014761859934199,
+                    "heading": 1.9670867202387443,
+                    "angularVelocity": 0.000005676926915437751,
+                    "velocityX": 2.086530440380401,
+                    "velocityY": -3.1554735966350926,
+                    "timestamp": 1.3047754919254257
+                },
+                {
+                    "x": 4.299328195356381,
+                    "y": 4.921422294262094,
+                    "heading": 1.967086761089818,
+                    "angularVelocity": 0.0000014355470678059053,
+                    "velocityX": 1.8846588082611082,
+                    "velocityY": -3.2800445020371574,
+                    "timestamp": 1.3332322904884106
+                },
+                {
+                    "x": 4.347003427577455,
+                    "y": 4.824904615507251,
+                    "heading": 1.9670865846023797,
+                    "angularVelocity": -0.000006201942843803764,
+                    "velocityX": 1.6753547351988247,
+                    "velocityY": -3.391726533861692,
+                    "timestamp": 1.3616890890513955
+                },
+                {
+                    "x": 4.391601867669633,
+                    "y": 4.730655732437251,
+                    "heading": 1.956126696369797,
+                    "angularVelocity": -0.3851412943835458,
+                    "velocityX": 1.5672332217368503,
+                    "velocityY": -3.3119988132677265,
+                    "timestamp": 1.3901458876143804
+                },
+                {
+                    "x": 4.4407405541886815,
+                    "y": 4.641134625282056,
+                    "heading": 1.9393492777589223,
+                    "angularVelocity": -0.5895750561568437,
+                    "velocityX": 1.7267819642570823,
+                    "velocityY": -3.145860099373803,
+                    "timestamp": 1.4186026861773653
+                },
+                {
+                    "x": 4.494270192698628,
+                    "y": 4.5565595160525465,
+                    "heading": 1.9177097235934124,
+                    "angularVelocity": -0.7604353004627978,
+                    "velocityX": 1.8810843528824521,
+                    "velocityY": -2.97205284854274,
+                    "timestamp": 1.4470594847403502
+                },
+                {
+                    "x": 4.552047682392307,
+                    "y": 4.477111099902014,
+                    "heading": 1.8920595280024661,
+                    "angularVelocity": -0.901373200296037,
+                    "velocityX": 2.030358037844879,
+                    "velocityY": -2.791895791605024,
+                    "timestamp": 1.475516283303335
+                },
+                {
+                    "x": 4.613941492209361,
+                    "y": 4.402936898219151,
+                    "heading": 1.8631628723317806,
+                    "angularVelocity": -1.0154570130820861,
+                    "velocityX": 2.175009591471355,
+                    "velocityY": -2.6065546874046253,
+                    "timestamp": 1.50397308186632
+                },
+                {
+                    "x": 4.679832286793745,
+                    "y": 4.334157239741354,
+                    "heading": 1.831710656249818,
+                    "angularVelocity": -1.105261929317901,
+                    "velocityX": 2.315467582853696,
+                    "velocityY": -2.416985112558446,
+                    "timestamp": 1.5324298804293048
+                },
+                {
+                    "x": 4.749611538460051,
+                    "y": 4.270871230504256,
+                    "heading": 1.798335587748108,
+                    "angularVelocity": -1.1728328619898605,
+                    "velocityX": 2.4521118042094026,
+                    "velocityY": -2.223932853761988,
+                    "timestamp": 1.5608866789922897
+                },
+                {
+                    "x": 4.82317903379308,
+                    "y": 4.213162042716923,
+                    "heading": 1.7636285592897518,
+                    "angularVelocity": -1.2196392500559503,
+                    "velocityX": 2.585234427203579,
+                    "velocityY": -2.0279578414139063,
+                    "timestamp": 1.5893434775552746
                 },
                 {
                     "x": 4.900439739227295,
                     "y": 4.161101341247559,
-                    "heading": 1.7737408289368037,
-                    "angularVelocity": -1.4262496399629772,
-                    "velocityX": 2.638534799915143,
-                    "velocityY": -1.8110259426046127,
-                    "timestamp": 1.5858301464308089
-                },
-                {
-                    "x": 5.008276114908647,
-                    "y": 4.1019129343639005,
-                    "heading": 1.719841693932476,
-                    "angularVelocity": -1.4074809230288232,
-                    "velocityX": 2.815956908545699,
-                    "velocityY": -1.5456009367595303,
-                    "timestamp": 1.6241248999315097
-                },
-                {
-                    "x": 5.122676890234505,
-                    "y": 4.053024888243358,
-                    "heading": 1.6679900254477509,
-                    "angularVelocity": -1.354014943163857,
-                    "velocityX": 2.987374636678728,
-                    "velocityY": -1.276625167978901,
-                    "timestamp": 1.6624196534322104
-                },
-                {
-                    "x": 5.243382451295884,
-                    "y": 4.014566134854959,
-                    "heading": 1.6197010617895011,
-                    "angularVelocity": -1.260981185252083,
-                    "velocityX": 3.1520130051018556,
-                    "velocityY": -1.0042825680467018,
-                    "timestamp": 1.7007144069329112
-                },
-                {
-                    "x": 5.370099790784461,
-                    "y": 3.9866453379407707,
-                    "heading": 1.576858310766692,
-                    "angularVelocity": -1.1187629402556432,
-                    "velocityX": 3.3090000040412137,
-                    "velocityY": -0.7291024059908604,
-                    "timestamp": 1.739009160433612
-                },
-                {
-                    "x": 5.502536097274536,
-                    "y": 3.9692960416426737,
-                    "heading": 1.5420636410872666,
-                    "angularVelocity": -0.9086014792806747,
-                    "velocityX": 3.4583407486263615,
-                    "velocityY": -0.45304629778540073,
-                    "timestamp": 1.7773039139343128
-                },
-                {
-                    "x": 5.640613600815518,
-                    "y": 3.9622331858505495,
-                    "heading": 1.5196895147171037,
-                    "angularVelocity": -0.5842608797508935,
-                    "velocityX": 3.6056506680074274,
-                    "velocityY": -0.18443403198808295,
-                    "timestamp": 1.8155986674350135
-                },
-                {
-                    "x": 5.785301546169388,
-                    "y": 3.963377277975263,
-                    "heading": 1.519119424890785,
-                    "angularVelocity": -0.014886891132695255,
-                    "velocityX": 3.778270706226711,
-                    "velocityY": 0.029875949578638192,
-                    "timestamp": 1.8538934209357143
-                },
-                {
-                    "x": 5.929550760809323,
-                    "y": 3.9767385598159253,
-                    "heading": 1.5191191822149777,
-                    "angularVelocity": -0.000006337051042848807,
-                    "velocityX": 3.7668140268168893,
-                    "velocityY": 0.3489063283934661,
-                    "timestamp": 1.892188174436415
-                },
-                {
-                    "x": 6.073462686179593,
-                    "y": 3.9933433315789473,
-                    "heading": 1.5191189525644635,
-                    "angularVelocity": -0.0000059969184602345635,
-                    "velocityX": 3.7580063119517533,
-                    "velocityY": 0.4336043516435729,
-                    "timestamp": 1.9304829279371158
-                },
-                {
-                    "x": 6.217374593429904,
-                    "y": 4.009948260386706,
-                    "heading": 1.519118722914699,
-                    "angularVelocity": -0.000005996898884562842,
-                    "velocityX": 3.7580058387809343,
-                    "velocityY": 0.43360845259010783,
-                    "timestamp": 1.9687776814378166
-                },
-                {
-                    "x": 6.361286503020876,
-                    "y": 4.026553168908082,
-                    "heading": 1.5191184932648618,
-                    "angularVelocity": -0.000005996900776462565,
-                    "velocityX": 3.7580058999032113,
-                    "velocityY": 0.4336079228469906,
-                    "timestamp": 2.0070724349385176
-                },
-                {
-                    "x": 6.5051984133944005,
-                    "y": 4.04315807064667,
-                    "heading": 1.5191182636148604,
-                    "angularVelocity": -0.000005996905070522029,
-                    "velocityX": 3.7580059203381437,
-                    "velocityY": 0.4336077457264339,
-                    "timestamp": 2.0453671884392186
-                },
-                {
-                    "x": 6.6491104352583505,
-                    "y": 4.059762006067292,
-                    "heading": 1.5191180339567425,
-                    "angularVelocity": -0.000005997117020238007,
-                    "velocityX": 3.7580088317141747,
-                    "velocityY": 0.4335825120357026,
-                    "timestamp": 2.0836619419399196
-                },
-                {
-                    "x": 6.790280334982046,
-                    "y": 4.068588148423426,
-                    "heading": 1.5085875786821552,
-                    "angularVelocity": -0.27498428144717973,
-                    "velocityX": 3.6864031445224414,
-                    "velocityY": 0.23047915312922457,
-                    "timestamp": 2.1219566954406206
-                },
-                {
-                    "x": 6.921716704437578,
-                    "y": 4.075833761978748,
-                    "heading": 1.4694677109965053,
-                    "angularVelocity": -1.0215464028234083,
-                    "velocityX": 3.43222915517987,
-                    "velocityY": 0.18920642889606504,
-                    "timestamp": 2.1602514489413216
-                },
-                {
-                    "x": 7.043039125371844,
-                    "y": 4.08211395301362,
-                    "heading": 1.4037305732475935,
-                    "angularVelocity": -1.716609502335841,
-                    "velocityX": 3.1681212135768497,
-                    "velocityY": 0.16399612115943957,
-                    "timestamp": 2.1985462024420226
-                },
-                {
-                    "x": 7.153946697571625,
-                    "y": 4.087967374487544,
-                    "heading": 1.3134036899027293,
-                    "angularVelocity": -2.3587273735346383,
-                    "velocityX": 2.896155793188521,
-                    "velocityY": 0.1528517861805002,
-                    "timestamp": 2.2368409559427236
-                },
-                {
-                    "x": 7.254278338500056,
-                    "y": 4.0938696715910385,
-                    "heading": 1.199938125135736,
-                    "angularVelocity": -2.9629532610757465,
-                    "velocityX": 2.619983986229206,
-                    "velocityY": 0.1541280871121784,
-                    "timestamp": 2.2751357094434246
-                },
-                {
-                    "x": 7.34404947628579,
-                    "y": 4.100268813517142,
-                    "heading": 1.0638507127340207,
-                    "angularVelocity": -3.5536829450860603,
-                    "velocityX": 2.344215057660344,
-                    "velocityY": 0.16710231405423262,
-                    "timestamp": 2.3134304629441256
-                },
-                {
-                    "x": 7.4235066081317465,
-                    "y": 4.107648882181887,
-                    "heading": 0.9045749783818139,
-                    "angularVelocity": -4.159205107542786,
-                    "velocityX": 2.0748829691383666,
-                    "velocityY": 0.192717487125464,
-                    "timestamp": 2.3517252164448266
-                },
-                {
-                    "x": 7.492760083799764,
-                    "y": 4.113297423743801,
-                    "heading": 0.7219171880631509,
-                    "angularVelocity": -4.769786292404788,
-                    "velocityX": 1.808432470175071,
-                    "velocityY": 0.1475017083426549,
-                    "timestamp": 2.3900199699455276
-                },
-                {
-                    "x": 7.549820937161571,
-                    "y": 4.1180620764150975,
-                    "heading": 0.5402485968001275,
-                    "angularVelocity": -4.7439551023536115,
-                    "velocityX": 1.4900436259698675,
-                    "velocityY": 0.1244205076606485,
-                    "timestamp": 2.4283147234462286
-                },
-                {
-                    "x": 7.595294548959434,
-                    "y": 4.121747510359626,
-                    "heading": 0.3723680212509228,
-                    "angularVelocity": -4.383905370904984,
-                    "velocityX": 1.1874632329734431,
-                    "velocityY": 0.09623861254154589,
-                    "timestamp": 2.4666094769469296
-                },
-                {
-                    "x": 7.630234770559726,
-                    "y": 4.124236647711556,
-                    "heading": 0.22836852122657442,
-                    "angularVelocity": -3.7602931697083015,
-                    "velocityX": 0.912402311184801,
-                    "velocityY": 0.06499943528517171,
-                    "timestamp": 2.5049042304476306
-                },
-                {
-                    "x": 7.655576852148959,
-                    "y": 4.125750923706981,
-                    "heading": 0.11519495737029227,
-                    "angularVelocity": -2.9553281718920323,
-                    "velocityX": 0.6617637998053468,
-                    "velocityY": 0.03954264897925492,
-                    "timestamp": 2.5431989839483315
-                },
+                    "heading": 1.7281561592052472,
+                    "angularVelocity": -1.2465351647339973,
+                    "velocityX": 2.715017476867699,
+                    "velocityY": -1.8294644548346262,
+                    "timestamp": 1.6178002761182595
+                },
+                {
+                    "x": 5.010449341197396,
+                    "y": 4.1015961278120825,
+                    "heading": 1.6804127870661807,
+                    "angularVelocity": -1.251568549956003,
+                    "velocityX": 2.8838465288429167,
+                    "velocityY": -1.5598993191587458,
+                    "timestamp": 1.6559471056633712
+                },
+                {
+                    "x": 5.126648667318374,
+                    "y": 4.05252161275018,
+                    "heading": 1.6338488947780365,
+                    "angularVelocity": -1.2206490773575045,
+                    "velocityX": 3.0461070423551457,
+                    "velocityY": -1.2864637939008783,
+                    "timestamp": 1.694093935208483
+                },
+                {
+                    "x": 5.248756740712563,
+                    "y": 4.014017499979329,
+                    "heading": 1.589989878848282,
+                    "angularVelocity": -1.1497421005375177,
+                    "velocityX": 3.2010018879727395,
+                    "velocityY": -1.0093660005294856,
+                    "timestamp": 1.7322407647535947
+                },
+                {
+                    "x": 5.376454980725802,
+                    "y": 3.9862068849459353,
+                    "heading": 1.5506775545962859,
+                    "angularVelocity": -1.0305528590747366,
+                    "velocityX": 3.347545301559156,
+                    "velocityY": -0.7290413217820071,
+                    "timestamp": 1.7703875942987064
+                },
+                {
+                    "x": 5.509414833155046,
+                    "y": 3.969152206173251,
+                    "heading": 1.5183946275971996,
+                    "angularVelocity": -0.8462807364186931,
+                    "velocityX": 3.485475831533742,
+                    "velocityY": -0.4470798484702507,
+                    "timestamp": 1.8085344238438181
+                },
+                {
+                    "x": 5.647435915880888,
+                    "y": 3.96261903739833,
+                    "heading": 1.4980750155673637,
+                    "angularVelocity": -0.5326684359468331,
+                    "velocityX": 3.618153444771329,
+                    "velocityY": -0.17126374204166636,
+                    "timestamp": 1.8466812533889299
+                },
+                {
+                    "x": 5.791726765964232,
+                    "y": 3.964767379969892,
+                    "heading": 1.498073903211314,
+                    "angularVelocity": -0.00002915985583373997,
+                    "velocityX": 3.7825122507942877,
+                    "velocityY": 0.056317722783366074,
+                    "timestamp": 1.8848280829340416
+                },
+                {
+                    "x": 5.935369435750422,
+                    "y": 3.9785994401105067,
+                    "heading": 1.4980736722429098,
+                    "angularVelocity": -0.00000605472084526459,
+                    "velocityX": 3.765520529467262,
+                    "velocityY": 0.3626005176676574,
+                    "timestamp": 1.9229749124791533
+                },
+                {
+                    "x": 6.078764518036592,
+                    "y": 3.9947980029743344,
+                    "heading": 1.498073449658831,
+                    "angularVelocity": -0.000005834929965972279,
+                    "velocityX": 3.7590301473577066,
+                    "velocityY": 0.42463719939010114,
+                    "timestamp": 1.961121742024265
+                },
+                {
+                    "x": 6.222150100858587,
+                    "y": 4.0110804389753145,
+                    "heading": 1.4980732273393433,
+                    "angularVelocity": -0.000005827993841710607,
+                    "velocityX": 3.7587811236710817,
+                    "velocityY": 0.4268358915939984,
+                    "timestamp": 1.9992685715693768
+                },
+                {
+                    "x": 6.3655338107868324,
+                    "y": 4.0273793596330805,
+                    "heading": 1.4980730050704238,
+                    "angularVelocity": -0.000005826668223171757,
+                    "velocityX": 3.758732026701189,
+                    "velocityY": 0.42726802860026514,
+                    "timestamp": 2.0374154011144885
+                },
+                {
+                    "x": 6.508922861230399,
+                    "y": 4.043631229943055,
+                    "heading": 1.4980727826574463,
+                    "angularVelocity": -0.000005830444635518525,
+                    "velocityX": 3.7588720256305224,
+                    "velocityY": 0.42603462734214087,
+                    "timestamp": 2.0755622306596
+                },
+                {
+                    "x": 6.6523224677448685,
+                    "y": 4.059789689420356,
+                    "heading": 1.4980725589565675,
+                    "angularVelocity": -0.000005864206314008162,
+                    "velocityX": 3.759148747731203,
+                    "velocityY": 0.4235859092458616,
+                    "timestamp": 2.1137090602047115
+                },
+                {
+                    "x": 6.792869039761006,
+                    "y": 4.068475137054533,
+                    "heading": 1.4874839096219183,
+                    "angularVelocity": -0.2775761304689174,
+                    "velocityX": 3.684357879593819,
+                    "velocityY": 0.2276846526483585,
+                    "timestamp": 2.151855889749823
+                },
+                {
+                    "x": 6.923724640852256,
+                    "y": 4.075654653912489,
+                    "heading": 1.4487910280540974,
+                    "angularVelocity": -1.014314479844837,
+                    "velocityX": 3.430313938317647,
+                    "velocityY": 0.18820743280285396,
+                    "timestamp": 2.1900027192949345
+                },
+                {
+                    "x": 7.04453293582936,
+                    "y": 4.081916427546979,
+                    "heading": 1.3838287724616933,
+                    "angularVelocity": -1.7029529417506197,
+                    "velocityX": 3.1669288488112004,
+                    "velocityY": 0.1641492545881335,
+                    "timestamp": 2.228149548840046
+                },
+                {
+                    "x": 7.155015675902827,
+                    "y": 4.087769066352678,
+                    "heading": 1.2944912667640571,
+                    "angularVelocity": -2.3419378953075096,
+                    "velocityX": 2.896249606872667,
+                    "velocityY": 0.1534239902944478,
+                    "timestamp": 2.2662963783851575
+                },
+                {
+                    "x": 7.255027442080941,
+                    "y": 4.093665982164644,
+                    "heading": 1.1821266374518384,
+                    "angularVelocity": -2.945582389205927,
+                    "velocityX": 2.621758279016409,
+                    "velocityY": 0.15458468980091475,
+                    "timestamp": 2.304443207930269
+                },
+                {
+                    "x": 7.344594486390613,
+                    "y": 4.100041344791564,
+                    "heading": 1.0471923484078691,
+                    "angularVelocity": -3.5372346969048754,
+                    "velocityX": 2.3479551348760803,
+                    "velocityY": 0.16712693303897977,
+                    "timestamp": 2.3425900374753805
+                },
+                {
+                    "x": 7.423972884493799,
+                    "y": 4.107363761305008,
+                    "heading": 0.8891062073498277,
+                    "angularVelocity": -4.144148883225662,
+                    "velocityX": 2.080864885752389,
+                    "velocityY": 0.19195347557968287,
+                    "timestamp": 2.380736867020492
+                },
+                {
+                    "x": 7.492968640563269,
+                    "y": 4.113132211800853,
+                    "heading": 0.7092469399580156,
+                    "angularVelocity": -4.714920467480658,
+                    "velocityX": 1.8086891333366921,
+                    "velocityY": 0.1512170359820358,
+                    "timestamp": 2.4188836965656035
+                },
+                {
+                    "x": 7.549873361136887,
+                    "y": 4.117954510008317,
+                    "heading": 0.5306887118642236,
+                    "angularVelocity": -4.680814374954733,
+                    "velocityX": 1.4917287033341673,
+                    "velocityY": 0.12641412838299443,
+                    "timestamp": 2.457030526110715
+                },
+                {
+                    "x": 7.595282353021441,
+                    "y": 4.121668824298383,
+                    "heading": 0.3658074925223697,
+                    "angularVelocity": -4.322278451654422,
+                    "velocityX": 1.1903739426332636,
+                    "velocityY": 0.09736888581300335,
+                    "timestamp": 2.4951773556558265
+                },
+                {
+                    "x": 7.630209510034256,
+                    "y": 4.124185175295285,
+                    "heading": 0.22438733020455492,
+                    "angularVelocity": -3.7072586111122043,
+                    "velocityX": 0.9155978997292245,
+                    "velocityY": 0.06596487904592362,
+                    "timestamp": 2.533324185200938
+                },
+                {
+                    "x": 7.65555835714788,
+                    "y": 4.125725160446138,
+                    "heading": 0.1131980096846438,
+                    "angularVelocity": -2.91477225881568,
+                    "velocityX": 0.6645073107231708,
+                    "velocityY": 0.04036993818976583,
+                    "timestamp": 2.5714710147460496
+                },
                 {
-                    "x": 7.671995118301485,
-                    "y": 4.1266005721732935,
-                    "heading": 0.03711275718074835,
-                    "angularVelocity": -2.0389790520029063,
-                    "velocityX": 0.4287340863083462,
-                    "velocityY": 0.02218707234390751,
-                    "timestamp": 2.5814937374490325
+                    "x": 7.671987700512514,
+                    "y": 4.126592780034806,
+                    "heading": 0.03644789338444829,
+                    "angularVelocity": -2.011965796774904,
+                    "velocityX": 0.43068699445162756,
+                    "velocityY": 0.02274421227203338,
+                    "timestamp": 2.609617844291161
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
                     "heading": -0.003,
-                    "angularVelocity": -1.047473962197309,
-                    "velocityX": 0.20903337838088354,
-                    "velocityY": 0.010430353774151994,
-                    "timestamp": 2.6197884909497335
+                    "angularVelocity": -1.0341067358634992,
+                    "velocityX": 0.21003841165919293,
+                    "velocityY": 0.010675067103991773,
+                    "timestamp": 2.6477646738362726
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
                     "heading": -0.003,
-                    "angularVelocity": 7.250240493925158e-33,
-                    "velocityX": 3.887711813813661e-34,
-                    "velocityY": -4.900942603355191e-34,
-                    "timestamp": 2.6580832444504345
+                    "angularVelocity": 1.475285929099793e-30,
+                    "velocityX": 9.681452358723228e-32,
+                    "velocityY": 1.6696229962948817e-32,
+                    "timestamp": 2.685911503381384
                 }
             ],
             "constraints": [
@@ -1871,7 +2195,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 27
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 2.837061882019043,
+                    "y": 6.323787212371826,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 17
                 },
                 {
                     "x": 5.489266395568848,
@@ -1903,391 +2236,445 @@
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7271139423991175,
-                    "y": 6.709732017673653,
-                    "heading": 0.9305052411067223,
-                    "angularVelocity": -1.767895408627961,
-                    "velocityX": 0.2763206248110281,
-                    "velocityY": -0.16578618916650492,
-                    "timestamp": 0.06193508867035021
-                },
-                {
-                    "x": 0.7615532933104725,
-                    "y": 6.686914726598375,
-                    "heading": 0.7206788937202419,
-                    "angularVelocity": -3.387842851138587,
-                    "velocityX": 0.5560555680263672,
-                    "velocityY": -0.36840652956392367,
-                    "timestamp": 0.12387017734070042
-                },
-                {
-                    "x": 0.8202948153873781,
-                    "y": 6.650989300229197,
-                    "heading": 0.4540271404715134,
-                    "angularVelocity": -4.305342237709284,
-                    "velocityX": 0.9484368770271357,
-                    "velocityY": -0.5800496477915925,
-                    "timestamp": 0.18580526601105063
-                },
-                {
-                    "x": 0.9068549514543394,
-                    "y": 6.604915057765644,
-                    "heading": 0.1437481054753499,
-                    "angularVelocity": -5.009745552276917,
-                    "velocityX": 1.3975944480789861,
-                    "velocityY": -0.7439117865606645,
-                    "timestamp": 0.24774035468140085
-                },
-                {
-                    "x": 1.022793030858839,
-                    "y": 6.550181878656492,
-                    "heading": -0.13251218552677876,
-                    "angularVelocity": -4.460481076769346,
-                    "velocityX": 1.871928851536495,
-                    "velocityY": -0.8837184265686551,
-                    "timestamp": 0.30967544335175107
-                },
-                {
-                    "x": 1.1650239086947571,
-                    "y": 6.483494014593417,
-                    "heading": -0.35594040398018906,
-                    "angularVelocity": -3.6074577957352734,
-                    "velocityX": 2.296450701684509,
-                    "velocityY": -1.0767380090149254,
-                    "timestamp": 0.37161053202210126
-                },
-                {
-                    "x": 1.3271753983931303,
-                    "y": 6.398307258999271,
-                    "heading": -0.5136957877872723,
-                    "angularVelocity": -2.5471083870846947,
-                    "velocityX": 2.61808763302859,
-                    "velocityY": -1.3754199343695408,
-                    "timestamp": 0.43354562069245145
-                },
-                {
-                    "x": 1.510544798478812,
-                    "y": 6.2989823057862875,
-                    "heading": -0.5925289533905876,
-                    "angularVelocity": -1.2728352747326335,
-                    "velocityX": 2.96067066379232,
-                    "velocityY": -1.6036943733404823,
-                    "timestamp": 0.49548070936280164
-                },
-                {
-                    "x": 1.7178378905511122,
-                    "y": 6.190936000456915,
-                    "heading": -0.5942013489806535,
-                    "angularVelocity": -0.027002392762643195,
-                    "velocityX": 3.3469410720572084,
-                    "velocityY": -1.7445087695675872,
-                    "timestamp": 0.5574157980331519
-                },
-                {
-                    "x": 1.927701821836241,
-                    "y": 6.086763021965953,
-                    "heading": -0.5942019920614379,
-                    "angularVelocity": -0.000010383141417040622,
-                    "velocityX": 3.388449678374247,
-                    "velocityY": -1.6819702809408126,
-                    "timestamp": 0.6193508867035021
-                },
-                {
-                    "x": 2.1375657815463693,
-                    "y": 5.982590100737573,
-                    "heading": -0.594202635139582,
-                    "angularVelocity": -0.0000103830987882079,
-                    "velocityX": 3.3884501373224802,
-                    "velocityY": -1.6819693563827811,
-                    "timestamp": 0.6812859753738524
-                },
-                {
-                    "x": 2.347429741257157,
-                    "y": 5.878417179509917,
-                    "heading": -0.5942032782166262,
-                    "angularVelocity": -0.000010383081029176535,
-                    "velocityX": 3.3884501373331317,
-                    "velocityY": -1.6819693563710962,
-                    "timestamp": 0.7432210640442026
-                },
-                {
-                    "x": 2.5572937009682177,
-                    "y": 5.774244258282216,
-                    "heading": -0.5942039212925836,
-                    "angularVelocity": -0.000010383063479788062,
-                    "velocityX": 3.3884501373375415,
-                    "velocityY": -1.6819693563718265,
-                    "timestamp": 0.8051561527145529
-                },
-                {
-                    "x": 2.7671576606795463,
-                    "y": 5.670071337054474,
-                    "heading": -0.594204564367474,
-                    "angularVelocity": -0.00001038304625561267,
-                    "velocityX": 3.388450137341862,
-                    "velocityY": -1.68196935637249,
-                    "timestamp": 0.8670912413849031
-                },
-                {
-                    "x": 2.977021620391137,
-                    "y": 5.5658984158266955,
-                    "heading": -0.5942052074413191,
-                    "angularVelocity": -0.000010383029375253952,
-                    "velocityX": 3.388450137346089,
-                    "velocityY": -1.6819693563730826,
-                    "timestamp": 0.9290263300552534
-                },
-                {
-                    "x": 3.186885580102983,
-                    "y": 5.461725494598884,
-                    "heading": -0.5942058505141404,
-                    "angularVelocity": -0.000010383012846871076,
-                    "velocityX": 3.3884501373502207,
-                    "velocityY": -1.6819693563736025,
-                    "timestamp": 0.9909614187256036
-                },
-                {
-                    "x": 3.3967495398150795,
-                    "y": 5.357552573371046,
-                    "heading": -0.594206493585961,
-                    "angularVelocity": -0.000010382996688205053,
-                    "velocityX": 3.3884501373542517,
-                    "velocityY": -1.681969356374048,
-                    "timestamp": 1.0528965073959538
-                },
-                {
-                    "x": 3.6066134995274184,
-                    "y": 5.253379652143185,
-                    "heading": -0.5942071366568044,
-                    "angularVelocity": -0.000010382980910469912,
-                    "velocityX": 3.38845013735818,
-                    "velocityY": -1.6819693563744145,
-                    "timestamp": 1.114831596066304
-                },
-                {
-                    "x": 3.8164774592399944,
-                    "y": 5.149206730915306,
-                    "heading": -0.5942077797266946,
-                    "angularVelocity": -0.000010382965523044611,
-                    "velocityX": 3.388450137362003,
-                    "velocityY": -1.681969356374701,
-                    "timestamp": 1.1767666847366542
-                },
-                {
-                    "x": 4.026341418952801,
-                    "y": 5.045033809687414,
-                    "heading": -0.594208422795657,
-                    "angularVelocity": -0.00001038295054018878,
-                    "velocityX": 3.388450137365716,
-                    "velocityY": -1.6819693563749047,
-                    "timestamp": 1.2387017734070045
-                },
-                {
-                    "x": 4.236205378665829,
-                    "y": 4.940860888459515,
-                    "heading": -0.594209065863717,
-                    "angularVelocity": -0.000010382935971395078,
-                    "velocityX": 3.388450137369317,
-                    "velocityY": -1.681969356375023,
-                    "timestamp": 1.3006368620773547
-                },
-                {
-                    "x": 4.446069338379075,
-                    "y": 4.836687967231613,
-                    "heading": -0.5942097089309012,
-                    "angularVelocity": -0.000010382921828372961,
-                    "velocityX": 3.3884501373728035,
-                    "velocityY": -1.6819693563750544,
-                    "timestamp": 1.362571950747705
-                },
-                {
-                    "x": 4.6559332980925285,
-                    "y": 4.732515046003717,
-                    "heading": -0.5942103519972364,
-                    "angularVelocity": -0.000010382908121631514,
-                    "velocityX": 3.388450137376173,
-                    "velocityY": -1.6819693563749973,
-                    "timestamp": 1.4245070394180552
-                },
-                {
-                    "x": 4.865797257806183,
-                    "y": 4.628342124775828,
-                    "heading": -0.5942109950627501,
-                    "angularVelocity": -0.00001038289485891648,
-                    "velocityX": 3.388450137379422,
-                    "velocityY": -1.6819693563748486,
-                    "timestamp": 1.4864421280884055
-                },
-                {
-                    "x": 5.075661217518705,
-                    "y": 4.524169203545277,
-                    "heading": -0.594211638127464,
-                    "angularVelocity": -0.000010382881945026341,
-                    "velocityX": 3.3884501373611275,
-                    "velocityY": -1.6819693564178315,
-                    "timestamp": 1.5483772167587557
-                },
-                {
-                    "x": 5.285525078653758,
-                    "y": 4.419996083725492,
-                    "heading": -0.5942122811951368,
-                    "angularVelocity": -0.000010382929719206097,
-                    "velocityX": 3.3884485457355824,
-                    "velocityY": -1.6819725628269917,
-                    "timestamp": 1.610312305429106
+                    "x": 0.7232220177061155,
+                    "y": 6.714742544338994,
+                    "heading": 0.9631209877994957,
+                    "angularVelocity": -1.4677311041544436,
+                    "velocityX": 0.25242736725510606,
+                    "velocityY": -0.10037240309808587,
+                    "timestamp": 0.052379493752565884
+                },
+                {
+                    "x": 0.7500587763634208,
+                    "y": 6.703146440373859,
+                    "heading": 0.8150187561291465,
+                    "angularVelocity": -2.8274849766583348,
+                    "velocityX": 0.5123523870635092,
+                    "velocityY": -0.22138633145086953,
+                    "timestamp": 0.10475898750513177
+                },
+                {
+                    "x": 0.7962886642360024,
+                    "y": 6.691501707403842,
+                    "heading": 0.6243144861489608,
+                    "angularVelocity": -3.6408192656662295,
+                    "velocityX": 0.882595163881611,
+                    "velocityY": -0.22231472921494633,
+                    "timestamp": 0.15713848125769764
+                },
+                {
+                    "x": 0.8619102089829233,
+                    "y": 6.679485060515379,
+                    "heading": 0.3880805528376709,
+                    "angularVelocity": -4.510046134222473,
+                    "velocityX": 1.2528098315899883,
+                    "velocityY": -0.22941510174244611,
+                    "timestamp": 0.20951797501026354
+                },
+                {
+                    "x": 0.948820798934311,
+                    "y": 6.665315928828611,
+                    "heading": 0.12168049392786562,
+                    "angularVelocity": -5.0859609328842605,
+                    "velocityX": 1.6592483761286883,
+                    "velocityY": -0.2705091376732434,
+                    "timestamp": 0.26189746876282943
+                },
+                {
+                    "x": 1.058148493976547,
+                    "y": 6.648822752325803,
+                    "heading": -0.13051602501618126,
+                    "angularVelocity": -4.814794891592335,
+                    "velocityX": 2.0872232091185596,
+                    "velocityY": -0.3148785015128142,
+                    "timestamp": 0.31427696251539533
+                },
+                {
+                    "x": 1.1845022977802875,
+                    "y": 6.626138581630819,
+                    "heading": -0.32549006587483453,
+                    "angularVelocity": -3.722335343286937,
+                    "velocityX": 2.41227615525686,
+                    "velocityY": -0.43307350014000373,
+                    "timestamp": 0.36665645626796123
+                },
+                {
+                    "x": 1.3283887493945847,
+                    "y": 6.598463049204702,
+                    "heading": -0.4648019456307372,
+                    "angularVelocity": -2.6596644941624366,
+                    "velocityX": 2.7469996616232804,
+                    "velocityY": -0.5283657867495356,
+                    "timestamp": 0.4190359500205271
+                },
+                {
+                    "x": 1.4904287339592295,
+                    "y": 6.567163978467113,
+                    "heading": -0.552248498766723,
+                    "angularVelocity": -1.6694806854963555,
+                    "velocityX": 3.0935767598308836,
+                    "velocityY": -0.5975443536251414,
+                    "timestamp": 0.471415443773093
+                },
+                {
+                    "x": 1.6709503203675815,
+                    "y": 6.533637134258406,
+                    "heading": -0.590326905081816,
+                    "angularVelocity": -0.7269716369343046,
+                    "velocityX": 3.4464171658686396,
+                    "velocityY": -0.6400757587899589,
+                    "timestamp": 0.5237949375256589
+                },
+                {
+                    "x": 1.8675588987397893,
+                    "y": 6.50898247632772,
+                    "heading": -0.5903272348122566,
+                    "angularVelocity": -0.000006295029161022994,
+                    "velocityX": 3.7535410193340524,
+                    "velocityY": -0.4706929403928793,
+                    "timestamp": 0.5761744312782248
+                },
+                {
+                    "x": 2.0641677208301488,
+                    "y": 6.4843297618820115,
+                    "heading": -0.5903275645018402,
+                    "angularVelocity": -0.000006294249141189924,
+                    "velocityX": 3.7535456722646963,
+                    "velocityY": -0.47065583646465164,
+                    "timestamp": 0.6285539250307907
+                },
+                {
+                    "x": 2.2607765429184905,
+                    "y": 6.459677047420138,
+                    "heading": -0.5903278941913652,
+                    "angularVelocity": -0.000006294248023551401,
+                    "velocityX": 3.753545672226176,
+                    "velocityY": -0.4706558367732492,
+                    "timestamp": 0.6809334187833566
+                },
+                {
+                    "x": 2.4573850313302983,
+                    "y": 6.435021672008896,
+                    "heading": -0.5903282238892777,
+                    "angularVelocity": -0.000006294408153628446,
+                    "velocityX": 3.753539301860412,
+                    "velocityY": -0.4707066381303828,
+                    "timestamp": 0.7333129125359225
+                },
+                {
+                    "x": 2.6504394143479257,
+                    "y": 6.390381144575869,
+                    "heading": -0.5903286607455448,
+                    "angularVelocity": -0.000008340215525789833,
+                    "velocityX": 3.685686309409395,
+                    "velocityY": -0.8522519832647445,
+                    "timestamp": 0.7856924062884884
+                },
+                {
+                    "x": 2.837061882019043,
+                    "y": 6.323787212371826,
+                    "heading": -0.5903296599203023,
+                    "angularVelocity": -0.000019075685653139892,
+                    "velocityX": 3.56289177884571,
+                    "velocityY": -1.2713741090859834,
+                    "timestamp": 0.8380719000410543
+                },
+                {
+                    "x": 3.013317183081598,
+                    "y": 6.237060061778313,
+                    "heading": -0.5903324266212199,
+                    "angularVelocity": -0.000053280252957349455,
+                    "velocityX": 3.394268952515208,
+                    "velocityY": -1.6701640905268176,
+                    "timestamp": 0.8899992232815851
+                },
+                {
+                    "x": 3.177545337709341,
+                    "y": 6.131367851066497,
+                    "heading": -0.5941789632152575,
+                    "angularVelocity": -0.0740753875608063,
+                    "velocityX": 3.1626539628670427,
+                    "velocityY": -2.0353872319249358,
+                    "timestamp": 0.9419265465221159
+                },
+                {
+                    "x": 3.3316611475687807,
+                    "y": 6.009565055315307,
+                    "heading": -0.5941812553691556,
+                    "angularVelocity": -0.00004414157624491074,
+                    "velocityX": 2.967913619301448,
+                    "velocityY": -2.3456397932739814,
+                    "timestamp": 0.9938538697626468
+                },
+                {
+                    "x": 3.48577581317155,
+                    "y": 5.887760811491488,
+                    "heading": -0.5941835469667753,
+                    "angularVelocity": -0.00004413086361592441,
+                    "velocityX": 2.9678915835676754,
+                    "velocityY": -2.3456676797995843,
+                    "timestamp": 1.0457811930031775
+                },
+                {
+                    "x": 3.639890478746931,
+                    "y": 5.7659565676417115,
+                    "heading": -0.5941858385862709,
+                    "angularVelocity": -0.00004413128489121798,
+                    "velocityX": 2.967891583040235,
+                    "velocityY": -2.3456676802994654,
+                    "timestamp": 1.0977085162437084
+                },
+                {
+                    "x": 3.7940051443206335,
+                    "y": 5.644152323798514,
+                    "heading": -0.594188130227655,
+                    "angularVelocity": -0.0000441317064133718,
+                    "velocityX": 2.9678915830079062,
+                    "velocityY": -2.3456676801727796,
+                    "timestamp": 1.1496358394842392
+                },
+                {
+                    "x": 3.948119809892657,
+                    "y": 5.522348079961896,
+                    "heading": -0.5941904218909294,
+                    "angularVelocity": -0.0000441321279705331,
+                    "velocityX": 2.9678915829755823,
+                    "velocityY": -2.345667680046072,
+                    "timestamp": 1.20156316272477
+                },
+                {
+                    "x": 4.1022344754630025,
+                    "y": 5.400543836131857,
+                    "heading": -0.5941927135760963,
+                    "angularVelocity": -0.00004413254956830847,
+                    "velocityX": 2.9678915829432535,
+                    "velocityY": -2.3456676799193525,
+                    "timestamp": 1.2534904859653009
+                },
+                {
+                    "x": 4.256349141031668,
+                    "y": 5.2787395923083995,
+                    "heading": -0.5941950052831575,
+                    "angularVelocity": -0.000044132971203721087,
+                    "velocityX": 2.9678915829109185,
+                    "velocityY": -2.3456676797926255,
+                    "timestamp": 1.3054178092058317
+                },
+                {
+                    "x": 4.410463806598655,
+                    "y": 5.1569353484915235,
+                    "heading": -0.594197297012115,
+                    "angularVelocity": -0.00004413339287361589,
+                    "velocityX": 2.9678915828785764,
+                    "velocityY": -2.345667679665889,
+                    "timestamp": 1.3573451324463626
+                },
+                {
+                    "x": 4.564578472163962,
+                    "y": 5.035131104681229,
+                    "heading": -0.5941995887629704,
+                    "angularVelocity": -0.000044133814578891406,
+                    "velocityX": 2.9678915828462293,
+                    "velocityY": -2.3456676795391433,
+                    "timestamp": 1.4092724556868934
+                },
+                {
+                    "x": 4.718693137727589,
+                    "y": 4.913326860877516,
+                    "heading": -0.5942018805357254,
+                    "angularVelocity": -0.00004413423631330234,
+                    "velocityX": 2.967891582813878,
+                    "velocityY": -2.3456676794123914,
+                    "timestamp": 1.4611997789274243
+                },
+                {
+                    "x": 4.872807803289535,
+                    "y": 4.791522617080385,
+                    "heading": -0.5942041723303811,
+                    "angularVelocity": -0.00004413465807134274,
+                    "velocityX": 2.9678915827815215,
+                    "velocityY": -2.3456676792856337,
+                    "timestamp": 1.513127102167955
+                },
+                {
+                    "x": 5.026922468849802,
+                    "y": 4.669718373289836,
+                    "heading": -0.5942064641469391,
+                    "angularVelocity": -0.000044135079855748624,
+                    "velocityX": 2.9678915827491625,
+                    "velocityY": -2.3456676791588693,
+                    "timestamp": 1.565054425408486
+                },
+                {
+                    "x": 5.181037134408388,
+                    "y": 4.5479141295058705,
+                    "heading": -0.5942087559854001,
+                    "angularVelocity": -0.000044135501659061016,
+                    "velocityX": 2.967891582716799,
+                    "velocityY": -2.345667679032102,
+                    "timestamp": 1.6169817486490168
+                },
+                {
+                    "x": 5.335151799963721,
+                    "y": 4.4261098857264995,
+                    "heading": -0.594211047845765,
+                    "angularVelocity": -0.00004413592347400816,
+                    "velocityX": 2.967891582654166,
+                    "velocityY": -2.345667678943628,
+                    "timestamp": 1.6689090718895476
                 },
                 {
                     "x": 5.489266395568848,
                     "y": 4.304305553436279,
                     "heading": -0.5942133396987443,
-                    "angularVelocity": -0.000017090531880803138,
-                    "velocityX": 3.2895943364108793,
-                    "velocityY": -1.8679319392755818,
-                    "timestamp": 1.6722473940994562
-                },
-                {
-                    "x": 5.681024172067428,
-                    "y": 4.156179263559471,
-                    "heading": -0.5942132880529166,
-                    "angularVelocity": 8.063055616728172e-7,
-                    "velocityX": 2.9937628707653796,
-                    "velocityY": -2.312578894659331,
-                    "timestamp": 1.7362998206864544
-                },
-                {
-                    "x": 5.864967854996084,
-                    "y": 3.998454299605836,
-                    "heading": -0.5942132372714615,
-                    "angularVelocity": 7.928107921043856e-7,
-                    "velocityX": 2.871767593048443,
-                    "velocityY": -2.4624354198261003,
-                    "timestamp": 1.8003522472734526
-                },
-                {
-                    "x": 6.0489114264021975,
-                    "y": 3.840729205591288,
-                    "heading": -0.5942131864900138,
-                    "angularVelocity": 7.928106775338953e-7,
-                    "velocityX": 2.871765851934987,
-                    "velocityY": -2.4624374503645132,
-                    "timestamp": 1.8644046738604507
-                },
-                {
-                    "x": 6.232854997807022,
-                    "y": 3.683004111575237,
-                    "heading": -0.5942131357085663,
-                    "angularVelocity": 7.928106743629292e-7,
-                    "velocityX": 2.87176585191486,
-                    "velocityY": -2.462437450387986,
-                    "timestamp": 1.928457100447449
-                },
-                {
-                    "x": 6.4167985692118465,
-                    "y": 3.5252790175591864,
-                    "heading": -0.594213084927119,
-                    "angularVelocity": 7.928106702567426e-7,
-                    "velocityX": 2.871765851914861,
-                    "velocityY": -2.462437450387986,
-                    "timestamp": 1.992509527034447
-                },
-                {
-                    "x": 6.6007421406166715,
-                    "y": 3.367553923543135,
-                    "heading": -0.5942130341456721,
-                    "angularVelocity": 7.928106666471698e-7,
-                    "velocityX": 2.8717658519148612,
-                    "velocityY": -2.462437450387985,
-                    "timestamp": 2.0565619536214452
-                },
-                {
-                    "x": 6.784685712022088,
-                    "y": 3.2098288295277735,
-                    "heading": -0.5942129833642253,
-                    "angularVelocity": 7.928106614555171e-7,
-                    "velocityX": 2.8717658519240943,
-                    "velocityY": -2.4624374503772177,
-                    "timestamp": 2.120614380208443
-                },
-                {
-                    "x": 6.968629334581004,
-                    "y": 3.052103795169223,
-                    "heading": -0.594212932582592,
-                    "angularVelocity": 7.928135744203515e-7,
-                    "velocityX": 2.871766650543336,
-                    "velocityY": -2.462436519002493,
-                    "timestamp": 2.184666806795441
-                },
-                {
-                    "x": 7.14554518368725,
-                    "y": 2.9064126516878526,
-                    "heading": -0.554455950225117,
-                    "angularVelocity": 0.6206943979472176,
-                    "velocityX": 2.7620475684235433,
-                    "velocityY": -2.2745608752775093,
-                    "timestamp": 2.248719233382439
-                },
-                {
-                    "x": 7.299470766691596,
-                    "y": 2.7775737815312107,
-                    "heading": -0.43989859569345574,
-                    "angularVelocity": 1.7884935924490803,
-                    "velocityX": 2.4031186827134454,
-                    "velocityY": -2.0114596280228136,
-                    "timestamp": 2.312771659969437
-                },
-                {
-                    "x": 7.4271138269599435,
-                    "y": 2.670230084319628,
-                    "heading": -0.3103960824325961,
-                    "angularVelocity": 2.021820564205559,
-                    "velocityX": 1.9927903917110543,
-                    "velocityY": -1.6758724521667427,
-                    "timestamp": 2.376824086556435
-                },
-                {
-                    "x": 7.52865057013146,
-                    "y": 2.5846878922538004,
-                    "heading": -0.1939655985882249,
-                    "angularVelocity": 1.8177372825404448,
-                    "velocityX": 1.5852130603296692,
-                    "velocityY": -1.3355027533522315,
-                    "timestamp": 2.440876513143433
-                },
-                {
-                    "x": 7.604480145165485,
-                    "y": 2.520737422138354,
-                    "heading": -0.10094853997619462,
-                    "angularVelocity": 1.4522019471922978,
-                    "velocityX": 1.1838673267285131,
-                    "velocityY": -0.9984082340516305,
-                    "timestamp": 2.504928939730431
-                },
+                    "angularVelocity": -0.000044135781248471857,
+                    "velocityX": 2.9678902355751515,
+                    "velocityY": -2.345669383457615,
+                    "timestamp": 1.7208363951300785
+                },
+                {
+                    "x": 5.673870655984677,
+                    "y": 4.147383169820797,
+                    "heading": -0.5942132891163008,
+                    "angularVelocity": 7.897648233188991e-7,
+                    "velocityX": 2.882303439061699,
+                    "velocityY": -2.450094732060046,
+                    "timestamp": 1.7848838711570736
+                },
+                {
+                    "x": 5.858474845870276,
+                    "y": 3.9904607032332415,
+                    "heading": -0.5942132385338614,
+                    "angularVelocity": 7.897647575237561e-7,
+                    "velocityX": 2.882302337843733,
+                    "velocityY": -2.450096027537681,
+                    "timestamp": 1.8489313471840687
+                },
+                {
+                    "x": 6.043079035755059,
+                    "y": 3.833538236644727,
+                    "heading": -0.5942131879514223,
+                    "angularVelocity": 7.897647543646741e-7,
+                    "velocityX": 2.882302337831004,
+                    "velocityY": -2.4500960275526547,
+                    "timestamp": 1.9129788232110638
+                },
+                {
+                    "x": 6.227683225639842,
+                    "y": 3.676615770056212,
+                    "heading": -0.5942131373689834,
+                    "angularVelocity": 7.897647493463809e-7,
+                    "velocityX": 2.8823023378310055,
+                    "velocityY": -2.450096027552654,
+                    "timestamp": 1.977026299238059
+                },
+                {
+                    "x": 6.412287415524625,
+                    "y": 3.5196933034676974,
+                    "heading": -0.5942130867865447,
+                    "angularVelocity": 7.89764745190247e-7,
+                    "velocityX": 2.882302337831007,
+                    "velocityY": -2.4500960275526538,
+                    "timestamp": 2.041073775265054
+                },
+                {
+                    "x": 6.596891605409407,
+                    "y": 3.362770836879183,
+                    "heading": -0.5942130362041063,
+                    "angularVelocity": 7.897647430611541e-7,
+                    "velocityX": 2.882302337831007,
+                    "velocityY": -2.450096027552653,
+                    "timestamp": 2.105121251292049
+                },
+                {
+                    "x": 6.781495795294782,
+                    "y": 3.2058483702913625,
+                    "heading": -0.5942129856216681,
+                    "angularVelocity": 7.897647381910717e-7,
+                    "velocityX": 2.8823023378402177,
+                    "velocityY": -2.4500960275418184,
+                    "timestamp": 2.169168727319044
+                },
+                {
+                    "x": 6.966100036209046,
+                    "y": 3.048925963734244,
+                    "heading": -0.5942129350390435,
+                    "angularVelocity": 7.897676514556789e-7,
+                    "velocityX": 2.882303134575621,
+                    "velocityY": -2.4500950902573897,
+                    "timestamp": 2.233216203346039
+                },
+                {
+                    "x": 7.143621774279278,
+                    "y": 2.9040156366918444,
+                    "heading": -0.5543160517170741,
+                    "angularVelocity": 0.6229267068253146,
+                    "velocityX": 2.771721058850299,
+                    "velocityY": -2.262545474568311,
+                    "timestamp": 2.2972636793730343
+                },
+                {
+                    "x": 7.2980980894932665,
+                    "y": 2.7758641788664877,
+                    "heading": -0.43954991420980805,
+                    "angularVelocity": 1.7918916501704738,
+                    "velocityX": 2.411903244225882,
+                    "velocityY": -2.000882248214473,
+                    "timestamp": 2.3613111554000294
+                },
+                {
+                    "x": 7.426195766165712,
+                    "y": 2.6690947773000513,
+                    "heading": -0.31008673137290993,
+                    "angularVelocity": 2.0213627588124035,
+                    "velocityX": 2.000042540606181,
+                    "velocityY": -1.667035271170323,
+                    "timestamp": 2.4253586314270246
+                },
+                {
+                    "x": 7.528097771332223,
+                    "y": 2.5840084569104262,
+                    "heading": -0.19375204823220424,
+                    "angularVelocity": 1.816382008428752,
+                    "velocityX": 1.591038577750685,
+                    "velocityY": -1.3284882663254731,
+                    "timestamp": 2.4894061074540197
+                },
+                {
+                    "x": 7.6042028509496316,
+                    "y": 2.5203982409108687,
+                    "heading": -0.10083265861354884,
+                    "angularVelocity": 1.4507892485800875,
+                    "velocityX": 1.1882604021012733,
+                    "velocityY": -0.9931728765197068,
+                    "timestamp": 2.553453583481015
+                },
                 {
-                    "x": 7.654868393442648,
-                    "y": 2.4782123742697575,
-                    "heading": -0.03632146129528005,
-                    "angularVelocity": 1.008971589751342,
-                    "velocityX": 0.7866719648587251,
-                    "velocityY": -0.6639100208145539,
-                    "timestamp": 2.568981366317429
+                    "x": 7.654775762663328,
+                    "y": 2.4780994279137802,
+                    "heading": -0.036281315483046445,
+                    "angularVelocity": 1.0078670875850742,
+                    "velocityX": 0.7896159981757911,
+                    "velocityY": -0.660429038284975,
+                    "timestamp": 2.61750105950801
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
                     "heading": -0.003,
-                    "angularVelocity": 0.5202216851226045,
-                    "velocityX": 0.39235994475896174,
-                    "velocityY": -0.3311720632620588,
-                    "timestamp": 2.6330337929044267
+                    "angularVelocity": 0.5196350824038538,
+                    "velocityX": 0.39383655533967327,
+                    "velocityY": -0.32943418261927393,
+                    "timestamp": 2.681548535535005
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
                     "heading": -0.003,
-                    "angularVelocity": -9.133242422473854e-34,
-                    "velocityX": 1.8081868884139647e-34,
+                    "angularVelocity": 1.9104375826405664e-34,
+                    "velocityX": -1.672523979523505e-35,
                     "velocityY": 0,
-                    "timestamp": 2.6970862194914247
+                    "timestamp": 2.745596011562
                 }
             ],
             "constraints": [
@@ -2318,7 +2705,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 23
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 2.797205924987793,
+                    "y": 6.264003276824951,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 13
                 },
                 {
                     "x": 4.092995643615723,
@@ -2363,801 +2759,855 @@
                     "y": 6.72,
                     "heading": 1.04,
                     "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
+                    "velocityX": 1.8950109818726043e-31,
+                    "velocityY": 4.417214679587952e-30,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7237404107060486,
-                    "y": 6.712071337633356,
-                    "heading": 0.9558489426883627,
-                    "angularVelocity": -1.537881157714612,
-                    "velocityX": 0.25110936688338653,
-                    "velocityY": -0.14489824428928566,
-                    "timestamp": 0.05471882979351295
-                },
-                {
-                    "x": 0.7514918072858734,
-                    "y": 6.694799584586779,
-                    "heading": 0.7935491608961526,
-                    "angularVelocity": -2.966068214628546,
-                    "velocityX": 0.5071635611460984,
-                    "velocityY": -0.31564551200661234,
-                    "timestamp": 0.1094376595870259
-                },
-                {
-                    "x": 0.7959632333479747,
-                    "y": 6.666655428800684,
-                    "heading": 0.574279005326375,
-                    "angularVelocity": -4.007215731718184,
-                    "velocityX": 0.8127261900504715,
-                    "velocityY": -0.514341331718894,
-                    "timestamp": 0.16415648938053887
-                },
-                {
-                    "x": 0.8617224500002636,
-                    "y": 6.63045184710829,
-                    "heading": 0.31905346805964874,
-                    "angularVelocity": -4.664309127769047,
-                    "velocityX": 1.2017657705846174,
-                    "velocityY": -0.6616293116832328,
-                    "timestamp": 0.2188753191740518
-                },
-                {
-                    "x": 0.9507709996198539,
-                    "y": 6.585947725507511,
-                    "heading": 0.05142997722620149,
-                    "angularVelocity": -4.890884762034389,
-                    "velocityX": 1.627384027685255,
-                    "velocityY": -0.8133237090178916,
-                    "timestamp": 0.2735941489675648
-                },
-                {
-                    "x": 1.0614190010918925,
-                    "y": 6.535018660742887,
-                    "heading": -0.17495854287380355,
-                    "angularVelocity": -4.137305584828904,
-                    "velocityX": 2.0221192940269503,
-                    "velocityY": -0.9307411170306262,
-                    "timestamp": 0.32831297876107773
-                },
-                {
-                    "x": 1.1921384640832198,
-                    "y": 6.474205616338843,
-                    "heading": -0.35862575739678126,
-                    "angularVelocity": -3.356563274764148,
-                    "velocityX": 2.388930163247485,
-                    "velocityY": -1.111373262796896,
-                    "timestamp": 0.3830318085545907
-                },
-                {
-                    "x": 1.3384031857193457,
-                    "y": 6.398894346098027,
-                    "heading": -0.49152044440666937,
-                    "angularVelocity": -2.4286829142980233,
-                    "velocityX": 2.6730235677201417,
-                    "velocityY": -1.376331886573771,
-                    "timestamp": 0.4377506383481036
-                },
-                {
-                    "x": 1.500957547301863,
-                    "y": 6.312259668262383,
-                    "heading": -0.5624501430724521,
-                    "angularVelocity": -1.2962575942036991,
-                    "velocityX": 2.9707207225727275,
-                    "velocityY": -1.5832699303433067,
-                    "timestamp": 0.4924694681416166
-                },
-                {
-                    "x": 1.6817069101290123,
-                    "y": 6.217739107445247,
-                    "heading": -0.5720464000476684,
-                    "angularVelocity": -0.17537394369443057,
-                    "velocityX": 3.3032388212471107,
-                    "velocityY": -1.7273863709040396,
-                    "timestamp": 0.5471882979351296
-                },
-                {
-                    "x": 1.8673643551319996,
-                    "y": 6.126199810501224,
-                    "heading": -0.5720469984173726,
-                    "angularVelocity": -0.000010935352720012522,
-                    "velocityX": 3.392935223643913,
-                    "velocityY": -1.6729030443351731,
-                    "timestamp": 0.6019071277286425
-                },
-                {
-                    "x": 2.0530162492759167,
-                    "y": 6.034649256644459,
-                    "heading": -0.5720475972653951,
-                    "angularVelocity": -0.000010944094102204816,
-                    "velocityX": 3.3928337803365705,
-                    "velocityY": -1.6731087671691436,
-                    "timestamp": 0.6566259575221555
-                },
-                {
-                    "x": 2.23866813267496,
-                    "y": 5.943098680998449,
-                    "heading": -0.5720481961130315,
-                    "angularVelocity": -0.000010944087046546937,
-                    "velocityX": 3.3928335839713664,
-                    "velocityY": -1.673109165372953,
-                    "timestamp": 0.7113447873156684
-                },
-                {
-                    "x": 2.4243200161477754,
-                    "y": 5.851548105501653,
-                    "heading": -0.5720487949598734,
-                    "angularVelocity": -0.000010944072528266654,
-                    "velocityX": 3.392833585319575,
-                    "velocityY": -1.673109162646019,
-                    "timestamp": 0.7660636171091814
-                },
-                {
-                    "x": 2.6099718996232633,
-                    "y": 5.759997530009892,
-                    "heading": -0.5720493938059237,
-                    "angularVelocity": -0.000010944058060277418,
-                    "velocityX": 3.3928335853684146,
-                    "velocityY": -1.673109162553994,
-                    "timestamp": 0.8207824469026943
-                },
-                {
-                    "x": 2.7956237830994004,
-                    "y": 5.668446954519065,
-                    "heading": -0.5720499926511823,
-                    "angularVelocity": -0.000010944043595406558,
-                    "velocityX": 3.3928335853802802,
-                    "velocityY": -1.6731091625369476,
-                    "timestamp": 0.8755012766962073
-                },
-                {
-                    "x": 2.9812756665756717,
-                    "y": 5.576896379028125,
-                    "heading": -0.5720505914956495,
-                    "angularVelocity": -0.000010944029129446314,
-                    "velocityX": 3.3928335853827263,
-                    "velocityY": -1.6731091625390042,
-                    "timestamp": 0.9302201064897202
-                },
-                {
-                    "x": 3.166927550036217,
-                    "y": 5.485345803504912,
-                    "heading": -0.5720511903393257,
-                    "angularVelocity": -0.000010944014673786385,
-                    "velocityX": 3.392833585095338,
-                    "velocityY": -1.673109163128796,
-                    "timestamp": 0.9849389362832331
-                },
-                {
-                    "x": 3.3525794334560626,
-                    "y": 5.393795227898782,
-                    "heading": -0.5720517891822119,
-                    "angularVelocity": -0.000010944000236842356,
-                    "velocityX": 3.392833584351539,
-                    "velocityY": -1.6731091646441256,
-                    "timestamp": 1.039657766076746
-                },
-                {
-                    "x": 3.538231313226632,
-                    "y": 5.302244644892125,
-                    "heading": -0.5720523880244444,
-                    "angularVelocity": -0.000010943988288632917,
-                    "velocityX": 3.392833517660122,
-                    "velocityY": -1.673109299890567,
-                    "timestamp": 1.0943765958702592
-                },
-                {
-                    "x": 3.723883292814953,
-                    "y": 5.21069426429933,
-                    "heading": -0.5720529868621156,
-                    "angularVelocity": -0.000010943904932797782,
-                    "velocityX": 3.3928353418539645,
-                    "velocityY": -1.673105600727685,
-                    "timestamp": 1.1490954256637722
-                },
-                {
-                    "x": 3.909492929164672,
-                    "y": 5.1190580682844296,
-                    "heading": -0.5720535873273735,
-                    "angularVelocity": -0.000010973649477031932,
-                    "velocityX": 3.3920615088102064,
-                    "velocityY": -1.6746738985594591,
-                    "timestamp": 1.2038142554572853
+                    "x": 0.7239179438589063,
+                    "y": 6.714864648059082,
+                    "heading": 0.9709045474564667,
+                    "angularVelocity": -1.3401351995297424,
+                    "velocityX": 0.269944341976283,
+                    "velocityY": -0.09960229862677873,
+                    "timestamp": 0.051558568544241005
+                },
+                {
+                    "x": 0.7525251092531191,
+                    "y": 6.703796484721446,
+                    "heading": 0.8397844996535714,
+                    "angularVelocity": -2.5431281648227726,
+                    "velocityX": 0.5548479370541143,
+                    "velocityY": -0.21467165691630782,
+                    "timestamp": 0.10311713708848201
+                },
+                {
+                    "x": 0.8002738106410594,
+                    "y": 6.689991944618376,
+                    "heading": 0.6713194026020708,
+                    "angularVelocity": -3.2674510136339485,
+                    "velocityX": 0.9261060331222868,
+                    "velocityY": -0.26774482870254956,
+                    "timestamp": 0.154675705632723
+                },
+                {
+                    "x": 0.8672232338097706,
+                    "y": 6.675504894458598,
+                    "heading": 0.46260124525274376,
+                    "angularVelocity": -4.048175952947979,
+                    "velocityX": 1.2985120622830493,
+                    "velocityY": -0.28098239669505554,
+                    "timestamp": 0.20623427417696402
+                },
+                {
+                    "x": 0.954736838142347,
+                    "y": 6.658878661036629,
+                    "heading": 0.2244001490575843,
+                    "angularVelocity": -4.620009882366156,
+                    "velocityX": 1.6973629564117712,
+                    "velocityY": -0.3224727507251158,
+                    "timestamp": 0.257792842721205
+                },
+                {
+                    "x": 1.0641109683124677,
+                    "y": 6.639652990231364,
+                    "heading": -0.004541406089891365,
+                    "angularVelocity": -4.440417211175481,
+                    "velocityX": 2.12135699765032,
+                    "velocityY": -0.37288992592389836,
+                    "timestamp": 0.309351411265446
+                },
+                {
+                    "x": 1.1925189459621883,
+                    "y": 6.613890706063681,
+                    "heading": -0.19800120779802596,
+                    "angularVelocity": -3.752233763861444,
+                    "velocityX": 2.4905264299488024,
+                    "velocityY": -0.49967027586341123,
+                    "timestamp": 0.36090997980968703
+                },
+                {
+                    "x": 1.3374709433458876,
+                    "y": 6.583331183982878,
+                    "heading": -0.33396197230189334,
+                    "angularVelocity": -2.6370158897471825,
+                    "velocityX": 2.811404611812854,
+                    "velocityY": -0.5927147115125508,
+                    "timestamp": 0.41246854835392804
+                },
+                {
+                    "x": 1.49983426972339,
+                    "y": 6.549745693102723,
+                    "heading": -0.41661329783619183,
+                    "angularVelocity": -1.6030570255902088,
+                    "velocityX": 3.149104619501603,
+                    "velocityY": -0.6514046419107491,
+                    "timestamp": 0.46402711689816906
+                },
+                {
+                    "x": 1.6800135166896315,
+                    "y": 6.515230594671984,
+                    "heading": -0.44907632730235497,
+                    "angularVelocity": -0.6296340333474829,
+                    "velocityX": 3.4946518503843107,
+                    "velocityY": -0.6694347691375233,
+                    "timestamp": 0.51558568544241
+                },
+                {
+                    "x": 1.8735253740457867,
+                    "y": 6.490840556482913,
+                    "heading": -0.4490767643443874,
+                    "angularVelocity": -0.00000847661300213044,
+                    "velocityX": 3.7532434049278374,
+                    "velocityY": -0.47305499119601163,
+                    "timestamp": 0.567144253986651
+                },
+                {
+                    "x": 2.067041889759694,
+                    "y": 6.46648750315131,
+                    "heading": -0.4490772002167485,
+                    "angularVelocity": -0.000008453926735380078,
+                    "velocityX": 3.753333755723225,
+                    "velocityY": -0.47233765442907966,
+                    "timestamp": 0.618702822530892
+                },
+                {
+                    "x": 2.2605372007770526,
+                    "y": 6.441966542289934,
+                    "heading": -0.4490776373561405,
+                    "angularVelocity": -0.0000084785013311663,
+                    "velocityX": 3.7529224817654048,
+                    "velocityY": -0.4755942911914249,
+                    "timestamp": 0.670261391075133
+                },
+                {
+                    "x": 2.45117838314973,
+                    "y": 6.400765025555903,
+                    "heading": -0.4490787415251664,
+                    "angularVelocity": -0.000021415820050526827,
+                    "velocityX": 3.6975654630332984,
+                    "velocityY": -0.7991206485676197,
+                    "timestamp": 0.7218199596193741
+                },
+                {
+                    "x": 2.6301938207404594,
+                    "y": 6.341459154774922,
+                    "heading": -0.46856582254307677,
+                    "angularVelocity": -0.377960086337202,
+                    "velocityX": 3.472079280806918,
+                    "velocityY": -1.150262166999653,
+                    "timestamp": 0.7733785281636151
+                },
+                {
+                    "x": 2.797205924987793,
+                    "y": 6.264003276824951,
+                    "heading": -0.5033519686488042,
+                    "angularVelocity": -0.6746918521648854,
+                    "velocityX": 3.239269610517634,
+                    "velocityY": -1.502289146830184,
+                    "timestamp": 0.8249370967078561
+                },
+                {
+                    "x": 2.911639196984497,
+                    "y": 6.1983709493948735,
+                    "heading": -0.5311684488221697,
+                    "angularVelocity": -0.7502107513814861,
+                    "velocityX": 3.0862665022221747,
+                    "velocityY": -1.770104534065962,
+                    "timestamp": 0.8620153178901536
+                },
+                {
+                    "x": 3.0203603627243916,
+                    "y": 6.122823189814021,
+                    "heading": -0.5565632404510816,
+                    "angularVelocity": -0.6848977868761134,
+                    "velocityX": 2.9322109387469486,
+                    "velocityY": -2.037523839376905,
+                    "timestamp": 0.8990935390724512
+                },
+                {
+                    "x": 3.1248710366711436,
+                    "y": 6.036966714195615,
+                    "heading": -0.5720315951438852,
+                    "angularVelocity": -0.41718168240112047,
+                    "velocityX": 2.8186539325316726,
+                    "velocityY": -2.3155500150894683,
+                    "timestamp": 0.9361717602547487
+                },
+                {
+                    "x": 3.2269084939172057,
+                    "y": 5.940732357266897,
+                    "heading": -0.5720474132070273,
+                    "angularVelocity": -0.00042661332192557474,
+                    "velocityX": 2.7519512531259087,
+                    "velocityY": -2.5954415789079244,
+                    "timestamp": 0.9732499814370462
+                },
+                {
+                    "x": 3.3218654576085616,
+                    "y": 5.837498151619579,
+                    "heading": -0.5720480482162372,
+                    "angularVelocity": -0.000017126204806487387,
+                    "velocityX": 2.560990270400005,
+                    "velocityY": -2.7842275695894796,
+                    "timestamp": 1.0103282026193439
+                },
+                {
+                    "x": 3.4166998907386708,
+                    "y": 5.734151371991349,
+                    "heading": -0.5720486788754773,
+                    "angularVelocity": -0.00001700888607859636,
+                    "velocityX": 2.557685619889607,
+                    "velocityY": -2.7872636910247786,
+                    "timestamp": 1.0474064238016414
+                },
+                {
+                    "x": 3.5114576080251196,
+                    "y": 5.630734246765037,
+                    "heading": -0.5720493072137337,
+                    "angularVelocity": -0.000016946289124090365,
+                    "velocityX": 2.555616592803881,
+                    "velocityY": -2.7891609125222083,
+                    "timestamp": 1.084484644983939
+                },
+                {
+                    "x": 3.6061908144763444,
+                    "y": 5.527294668181728,
+                    "heading": -0.572049934817026,
+                    "angularVelocity": -0.00001692646713700802,
+                    "velocityX": 2.5549555353779705,
+                    "velocityY": -2.7897664797435726,
+                    "timestamp": 1.1215628661662365
+                },
+                {
+                    "x": 3.7009275327159292,
+                    "y": 5.423858305931887,
+                    "heading": -0.5720505625260676,
+                    "angularVelocity": -0.00001692931919251171,
+                    "velocityX": 2.555050248355605,
+                    "velocityY": -2.789679735188215,
+                    "timestamp": 1.158641087348534
+                },
+                {
+                    "x": 3.7956729593070784,
+                    "y": 5.320429920428362,
+                    "heading": -0.5720511904967662,
+                    "angularVelocity": -0.000016936376088998586,
+                    "velocityX": 2.5552851126613465,
+                    "velocityY": -2.7894646022814182,
+                    "timestamp": 1.1957193085308315
+                },
+                {
+                    "x": 3.890467288385621,
+                    "y": 5.217046353888283,
+                    "heading": -0.5720518199780259,
+                    "angularVelocity": -0.000016977115933005807,
+                    "velocityX": 2.556604013243175,
+                    "velocityY": -2.7882558343819337,
+                    "timestamp": 1.232797529713129
+                },
+                {
+                    "x": 3.9877865474692196,
+                    "y": 5.11603605874111,
+                    "heading": -0.5720525488313646,
+                    "angularVelocity": -0.00001965718190938232,
+                    "velocityX": 2.624701400996587,
+                    "velocityY": -2.7242486809308257,
+                    "timestamp": 1.2698757508954266
                 },
                 {
                     "x": 4.092995643615723,
                     "y": 5.023272514343262,
                     "heading": -0.5720542830492642,
-                    "angularVelocity": -0.000012714487741704976,
-                    "velocityX": 3.353556995709023,
-                    "velocityY": -1.7505044296932655,
-                    "timestamp": 1.2585330852507983
-                },
-                {
-                    "x": 4.152699494738863,
-                    "y": 4.989124335806784,
-                    "heading": -0.5720542274621303,
-                    "angularVelocity": 0.0000030573365885939726,
-                    "velocityX": 3.283759307267904,
-                    "velocityY": -1.8781769850015708,
-                    "timestamp": 1.2767146404552234
-                },
-                {
-                    "x": 4.211236123155136,
-                    "y": 4.953011857528808,
-                    "heading": -0.5720541720789936,
-                    "angularVelocity": 0.000003046116576982166,
-                    "velocityX": 3.2195611298433677,
-                    "velocityY": -1.9862150334199096,
-                    "timestamp": 1.2948961956596485
-                },
-                {
-                    "x": 4.269217408443118,
-                    "y": 4.916014310130808,
-                    "heading": -0.5720541167402081,
-                    "angularVelocity": 0.000003043677217870583,
-                    "velocityX": 3.1890168160017764,
-                    "velocityY": -2.034894539109796,
-                    "timestamp": 1.3130777508640736
-                },
-                {
-                    "x": 4.327172181408397,
-                    "y": 4.8789752463128,
-                    "heading": -0.5720540614037476,
-                    "angularVelocity": 0.000003043549349895456,
-                    "velocityX": 3.187558617163689,
-                    "velocityY": -2.0371779752347874,
-                    "timestamp": 1.3312593060684987
-                },
-                {
-                    "x": 4.385108415981365,
-                    "y": 4.841907191568928,
-                    "heading": -0.5720540060672449,
-                    "angularVelocity": 0.0000030435516711711228,
-                    "velocityX": 3.186538990839725,
-                    "velocityY": -2.0387724992190503,
-                    "timestamp": 1.3494408612729238
-                },
-                {
-                    "x": 4.443034277698237,
-                    "y": 4.804822929374826,
-                    "heading": -0.5720539507307192,
-                    "angularVelocity": 0.000003043552934036031,
-                    "velocityX": 3.1859684755020403,
-                    "velocityY": -2.0396639218758956,
-                    "timestamp": 1.367622416477349
-                },
-                {
-                    "x": 4.500957639641057,
-                    "y": 4.767734762799275,
-                    "heading": -0.5720538953941279,
-                    "angularVelocity": 0.0000030435565398491907,
-                    "velocityX": 3.1858309859372986,
-                    "velocityY": -2.039878665966735,
-                    "timestamp": 1.385803971681774
-                },
-                {
-                    "x": 4.558883202891902,
-                    "y": 4.7306500344023386,
-                    "heading": -0.5720538400575734,
-                    "angularVelocity": 0.000003043554516316428,
-                    "velocityX": 3.1859520596302042,
-                    "velocityY": -2.03968956340728,
-                    "timestamp": 1.4039855268861992
-                },
-                {
-                    "x": 4.616809360440233,
-                    "y": 4.693566234312182,
-                    "heading": -0.572053784721022,
-                    "angularVelocity": 0.000003043554346667464,
-                    "velocityX": 3.1859847464654854,
-                    "velocityY": -2.039638505794633,
-                    "timestamp": 1.4221670820906243
-                },
-                {
-                    "x": 4.674735447300298,
-                    "y": 4.656482323806888,
-                    "heading": -0.5720537293844703,
-                    "angularVelocity": 0.00000304355436453364,
-                    "velocityX": 3.185980858554995,
-                    "velocityY": -2.039644578714451,
-                    "timestamp": 1.4403486372950494
-                },
-                {
-                    "x": 4.732661331177625,
-                    "y": 4.619398096238942,
-                    "heading": -0.5720536740479176,
-                    "angularVelocity": 0.0000030435544255368,
-                    "velocityX": 3.1859696943429383,
-                    "velocityY": -2.0396620174125277,
-                    "timestamp": 1.4585301924994745
-                },
-                {
-                    "x": 4.790587117727679,
-                    "y": 4.582313716645222,
-                    "heading": -0.5720536187113643,
-                    "angularVelocity": 0.0000030435544545803464,
-                    "velocityX": 3.1859643412654055,
-                    "velocityY": -2.039670378951063,
-                    "timestamp": 1.4767117477038996
-                },
-                {
-                    "x": 4.848512993160408,
-                    "y": 4.5452294758867104,
-                    "heading": -0.5720535633748114,
-                    "angularVelocity": 0.0000030435544295946876,
-                    "velocityX": 3.1859692298833355,
-                    "velocityY": -2.0396627429041696,
-                    "timestamp": 1.4948933029083247
-                },
-                {
-                    "x": 4.906438819485911,
-                    "y": 4.508145158422396,
-                    "heading": -0.5720535080382583,
-                    "angularVelocity": 0.0000030435544442083865,
-                    "velocityX": 3.1859665289468815,
-                    "velocityY": -2.039666961784329,
-                    "timestamp": 1.5130748581127498
-                },
-                {
-                    "x": 4.9643646433421065,
-                    "y": 4.471060837101057,
-                    "heading": -0.5720534527017052,
-                    "angularVelocity": 0.0000030435544458805193,
-                    "velocityX": 3.185966393132936,
-                    "velocityY": -2.039667173923745,
-                    "timestamp": 1.5312564133171749
-                },
-                {
-                    "x": 5.022290480818567,
-                    "y": 4.433976537054754,
-                    "heading": -0.572053397365152,
-                    "angularVelocity": 0.0000030435544415454932,
-                    "velocityX": 3.185967142258403,
-                    "velocityY": -2.039666003779759,
-                    "timestamp": 1.5494379685216
-                },
-                {
-                    "x": 5.080216347467466,
-                    "y": 4.396892282576102,
-                    "heading": -0.572053342028599,
-                    "angularVelocity": 0.000003043554433808661,
-                    "velocityX": 3.1859687467658464,
-                    "velocityY": -2.039663497522643,
-                    "timestamp": 1.567619523726025
-                },
-                {
-                    "x": 5.138142382220086,
-                    "y": 4.359808290677983,
-                    "heading": -0.5720532866920471,
-                    "angularVelocity": 0.000003043554377469469,
-                    "velocityX": 3.185977992604117,
-                    "velocityY": -2.039649055384386,
-                    "timestamp": 1.5858010789304502
-                },
-                {
-                    "x": 5.196068138494407,
-                    "y": 4.322723863792993,
-                    "heading": -0.5720532313554916,
-                    "angularVelocity": 0.000003043554576021048,
-                    "velocityX": 3.1859626760759787,
-                    "velocityY": -2.0396729800088242,
-                    "timestamp": 1.6039826341348753
-                },
-                {
-                    "x": 5.253993690845546,
-                    "y": 4.285639118381674,
-                    "heading": -0.5720531760189423,
-                    "angularVelocity": 0.0000030435542278177066,
-                    "velocityX": 3.1859514601384813,
-                    "velocityY": -2.0396904992104243,
-                    "timestamp": 1.6221641893393004
-                },
-                {
-                    "x": 5.3119198680466875,
-                    "y": 4.248555348991913,
-                    "heading": -0.5720531206823879,
-                    "angularVelocity": 0.000003043554518420827,
-                    "velocityX": 3.1859858273860655,
-                    "velocityY": -2.0396368172479113,
-                    "timestamp": 1.6403457445437255
-                },
-                {
-                    "x": 5.369845451022558,
-                    "y": 4.211470651416335,
-                    "heading": -0.5720530653458327,
-                    "angularVelocity": 0.0000030435545594837867,
-                    "velocityX": 3.1859531445232334,
-                    "velocityY": -2.039687868206526,
-                    "timestamp": 1.6585272997481506
-                },
-                {
-                    "x": 5.427769955136907,
-                    "y": 4.17438426873464,
-                    "heading": -0.5720530100092767,
-                    "angularVelocity": 0.0000030435545998300077,
-                    "velocityX": 3.185893806281465,
-                    "velocityY": -2.039780550383344,
-                    "timestamp": 1.6767088549525757
-                },
-                {
-                    "x": 5.485698211873678,
-                    "y": 4.137303747848898,
-                    "heading": -0.5720529546727399,
-                    "angularVelocity": 0.000003043553546723704,
-                    "velocityX": 3.1861002035013715,
-                    "velocityY": -2.0394581469404574,
-                    "timestamp": 1.6948904101570008
-                },
-                {
-                    "x": 5.5436372749681775,
-                    "y": 4.100240114388813,
-                    "heading": -0.5720528993360909,
-                    "angularVelocity": 0.0000030435597092212927,
-                    "velocityX": 3.186694561772871,
-                    "velocityY": -2.0385293250953618,
-                    "timestamp": 1.713071965361426
-                },
-                {
-                    "x": 5.601833960036281,
-                    "y": 4.063582321171135,
-                    "heading": -0.5720528439855975,
-                    "angularVelocity": 0.0000030443211669066955,
-                    "velocityX": 3.2008639752640304,
-                    "velocityY": -2.0162077889097265,
-                    "timestamp": 1.731253520565851
-                },
-                {
-                    "x": 5.661045954546234,
-                    "y": 4.0285882123488195,
-                    "heading": -0.5720527885077281,
-                    "angularVelocity": 0.000003051326955110969,
-                    "velocityX": 3.2567068022627845,
-                    "velocityY": -1.9247038236737246,
-                    "timestamp": 1.7494350757702761
-                },
-                {
-                    "x": 5.721517200862813,
-                    "y": 3.9958181607132874,
-                    "heading": -0.5720527326566293,
-                    "angularVelocity": 0.0000030718548612359243,
-                    "velocityX": 3.3259666533847816,
-                    "velocityY": -1.8023789091228484,
-                    "timestamp": 1.7676166309747012
+                    "angularVelocity": -0.000046771874277348444,
+                    "velocityX": 2.8374903863181586,
+                    "velocityY": -2.5018337298792988,
+                    "timestamp": 1.3069539720777241
+                },
+                {
+                    "x": 4.14638765114505,
+                    "y": 4.979871763007162,
+                    "heading": -0.5720542261628284,
+                    "angularVelocity": 0.000003127581707131923,
+                    "velocityX": 2.935460161172297,
+                    "velocityY": -2.386146961079825,
+                    "timestamp": 1.3251426050680648
+                },
+                {
+                    "x": 4.201473544713913,
+                    "y": 4.938642209546033,
+                    "heading": -0.5720541700948967,
+                    "angularVelocity": 0.000003082580845535541,
+                    "velocityX": 3.0285889873152927,
+                    "velocityY": -2.266775819987239,
+                    "timestamp": 1.3433312380584055
+                },
+                {
+                    "x": 4.258151834683336,
+                    "y": 4.899630631332575,
+                    "heading": -0.5720541145494444,
+                    "angularVelocity": 0.0000030538552492495714,
+                    "velocityX": 3.11613797471883,
+                    "velocityY": -2.1448328873407987,
+                    "timestamp": 1.3615198710487462
+                },
+                {
+                    "x": 4.316201884310526,
+                    "y": 4.862691173522813,
+                    "heading": -0.5720540592428014,
+                    "angularVelocity": 0.000003040725654248182,
+                    "velocityX": 3.1915564879460585,
+                    "velocityY": -2.030908965471825,
+                    "timestamp": 1.3797085040390868
+                },
+                {
+                    "x": 4.374575644994192,
+                    "y": 4.826265415180517,
+                    "heading": -0.5720540039764063,
+                    "angularVelocity": 0.0000030385128512512867,
+                    "velocityX": 3.209353925315247,
+                    "velocityY": -2.002666080603007,
+                    "timestamp": 1.3978971370294275
+                },
+                {
+                    "x": 4.432961770826435,
+                    "y": 4.789859479973715,
+                    "heading": -0.572053948711591,
+                    "angularVelocity": 0.000003038425994779569,
+                    "velocityX": 3.210033753678121,
+                    "velocityY": -2.0015762166465794,
+                    "timestamp": 1.4160857700197682
+                },
+                {
+                    "x": 4.491358693973173,
+                    "y": 4.753470866702408,
+                    "heading": -0.5720538934471577,
+                    "angularVelocity": 0.000003038404992596871,
+                    "velocityX": 3.21062738347468,
+                    "velocityY": -2.000623867148186,
+                    "timestamp": 1.434274403010109
+                },
+                {
+                    "x": 4.549763643159385,
+                    "y": 4.7170951368878224,
+                    "heading": -0.5720538381830028,
+                    "angularVelocity": 0.0000030383896855688938,
+                    "velocityX": 3.2110686502526127,
+                    "velocityY": -1.9999155425186408,
+                    "timestamp": 1.4524630360004496
+                },
+                {
+                    "x": 4.608173712818011,
+                    "y": 4.680727629815019,
+                    "heading": -0.5720537829190162,
+                    "angularVelocity": 0.0000030383804361409647,
+                    "velocityX": 3.211350170717194,
+                    "velocityY": -1.9994634611677902,
+                    "timestamp": 1.4706516689907903
+                },
+                {
+                    "x": 4.666585738503753,
+                    "y": 4.644363264529719,
+                    "heading": -0.5720537276550893,
+                    "angularVelocity": 0.0000030383771462609746,
+                    "velocityX": 3.211457711899863,
+                    "velocityY": -1.999290727599226,
+                    "timestamp": 1.488840301981131
+                },
+                {
+                    "x": 4.724997877507934,
+                    "y": 4.607999081268276,
+                    "heading": -0.5720536723911652,
+                    "angularVelocity": 0.0000030383769981998487,
+                    "velocityX": 3.2114639420784044,
+                    "velocityY": -1.9992807200388194,
+                    "timestamp": 1.5070289349714716
+                },
+                {
+                    "x": 4.783409732751982,
+                    "y": 4.571634442201932,
+                    "heading": -0.5720536171272324,
+                    "angularVelocity": 0.0000030383774810111486,
+                    "velocityX": 3.2114483411183334,
+                    "velocityY": -1.9993057799158553,
+                    "timestamp": 1.5252175679618123
+                },
+                {
+                    "x": 4.841821302805823,
+                    "y": 4.535269345043953,
+                    "heading": -0.5720535618632904,
+                    "angularVelocity": 0.0000030383779810703436,
+                    "velocityX": 3.211432661533899,
+                    "velocityY": -1.9993309655158582,
+                    "timestamp": 1.543406200952153
+                },
+                {
+                    "x": 4.900232750957554,
+                    "y": 4.4989040520829935,
+                    "heading": -0.5720535065993444,
+                    "angularVelocity": 0.0000030383782054284467,
+                    "velocityX": 3.2114259594301156,
+                    "velocityY": -1.9993417306445722,
+                    "timestamp": 1.5615948339424937
+                },
+                {
+                    "x": 4.958643009851236,
+                    "y": 4.46253684895675,
+                    "heading": -0.5720534513353597,
+                    "angularVelocity": 0.000003038380333213315,
+                    "velocityX": 3.2113605747448064,
+                    "velocityY": -1.999446750371714,
+                    "timestamp": 1.5797834669328343
+                },
+                {
+                    "x": 5.017052204377269,
+                    "y": 4.426167936377253,
+                    "heading": -0.5720533960713398,
+                    "angularVelocity": 0.0000030383822663214653,
+                    "velocityX": 3.21130205645789,
+                    "velocityY": -1.999540735073477,
+                    "timestamp": 1.597972099923175
+                },
+                {
+                    "x": 5.075461342766336,
+                    "y": 4.389798933639636,
+                    "heading": -0.572053340807318,
+                    "angularVelocity": 0.0000030383823681408014,
+                    "velocityX": 3.211298970081695,
+                    "velocityY": -1.999545691911999,
+                    "timestamp": 1.6161607329135157
+                },
+                {
+                    "x": 5.133870231273224,
+                    "y": 4.35342952959225,
+                    "heading": -0.5720532855432875,
+                    "angularVelocity": 0.0000030383828455618877,
+                    "velocityX": 3.211285231711017,
+                    "velocityY": -1.9995677556796618,
+                    "timestamp": 1.6343493659038564
+                },
+                {
+                    "x": 5.192278843825858,
+                    "y": 4.317059682370623,
+                    "heading": -0.5720532302792477,
+                    "angularVelocity": 0.0000030383833608448334,
+                    "velocityX": 3.2112700599133515,
+                    "velocityY": -1.9995921211310104,
+                    "timestamp": 1.652537998894197
+                },
+                {
+                    "x": 5.250688378518623,
+                    "y": 4.28069131610433,
+                    "heading": -0.5720531750152431,
+                    "angularVelocity": 0.000003038381418986995,
+                    "velocityX": 3.211320758617258,
+                    "velocityY": -1.9995106991068379,
+                    "timestamp": 1.6707266318845377
+                },
+                {
+                    "x": 5.309100997862405,
+                    "y": 4.2443279043993805,
+                    "heading": -0.5720531197513381,
+                    "angularVelocity": 0.0000030383759466885435,
+                    "velocityX": 3.2114903508569643,
+                    "velocityY": -1.9992383003329772,
+                    "timestamp": 1.6889152648748784
+                },
+                {
+                    "x": 5.367519658270811,
+                    "y": 4.20797419858894,
+                    "heading": -0.5720530644876303,
+                    "angularVelocity": 0.0000030383651093388606,
+                    "velocityX": 3.2118224849224113,
+                    "velocityY": -1.998704676145307,
+                    "timestamp": 1.707103897865219
+                },
+                {
+                    "x": 5.425947098435591,
+                    "y": 4.171634605233085,
+                    "heading": -0.5720530092241862,
+                    "angularVelocity": 0.000003038350601996349,
+                    "velocityX": 3.212305190598148,
+                    "velocityY": -1.9979287819592548,
+                    "timestamp": 1.7252925308555598
+                },
+                {
+                    "x": 5.484385608043213,
+                    "y": 4.135312815587752,
+                    "heading": -0.5720529539612775,
+                    "angularVelocity": 0.0000030383211709796797,
+                    "velocityX": 3.2129137818460802,
+                    "velocityY": -1.9969499447620875,
+                    "timestamp": 1.7434811638459005
+                },
+                {
+                    "x": 5.542805855367704,
+                    "y": 4.098961660035809,
+                    "heading": -0.5720528986974772,
+                    "angularVelocity": 0.0000030383701899944643,
+                    "velocityX": 3.21190973260657,
+                    "velocityY": -1.9985644644794618,
+                    "timestamp": 1.7616697968362411
+                },
+                {
+                    "x": 5.601240344083477,
+                    "y": 4.062633401947461,
+                    "heading": -0.5720528434300965,
+                    "angularVelocity": 0.000003038567036318681,
+                    "velocityX": 3.2126927156544984,
+                    "velocityY": -1.9973055758301874,
+                    "timestamp": 1.7798584298265818
+                },
+                {
+                    "x": 5.6606145093793865,
+                    "y": 4.027862145195579,
+                    "heading": -0.5720527881030576,
+                    "angularVelocity": 0.0000030418470110587657,
+                    "velocityX": 3.2643555635790893,
+                    "velocityY": -1.9117025875630587,
+                    "timestamp": 1.7980470628169225
+                },
+                {
+                    "x": 5.721296662408543,
+                    "y": 3.9954274711883437,
+                    "heading": -0.5720527324898503,
+                    "angularVelocity": 0.0000030575803747562297,
+                    "velocityX": 3.3362679351103024,
+                    "velocityY": -1.7832386867362542,
+                    "timestamp": 1.8162356958072632
                 },
                 {
                     "x": 5.7832255363464355,
                     "y": 3.9654417037963863,
-                    "heading": -0.572052675208486,
-                    "angularVelocity": 0.000003159693577578397,
-                    "velocityX": 3.3940075416988695,
-                    "velocityY": -1.670729295450284,
-                    "timestamp": 1.7857981861791263
-                },
-                {
-                    "x": 5.945861586676733,
-                    "y": 3.9048707731215235,
-                    "heading": -0.5720526214657033,
-                    "angularVelocity": 0.0000011714587578241761,
-                    "velocityX": 3.5450606755342,
-                    "velocityY": -1.3202953710429959,
-                    "timestamp": 1.831674989206145
-                },
-                {
-                    "x": 6.113092819895856,
-                    "y": 3.858469939627616,
-                    "heading": -0.5720525698257588,
-                    "angularVelocity": 0.0000011256221281562495,
-                    "velocityX": 3.6452242132183756,
-                    "velocityY": -1.0114225585101817,
-                    "timestamp": 1.8775517922331635
-                },
-                {
-                    "x": 6.280428909416468,
-                    "y": 3.8124486851902244,
-                    "heading": -0.5720525182264563,
-                    "angularVelocity": 0.0000011247362300949913,
-                    "velocityX": 3.6475098193329205,
-                    "velocityY": -1.0031486808299002,
-                    "timestamp": 1.923428595260182
-                },
-                {
-                    "x": 6.444900242470224,
-                    "y": 3.7570546132296903,
-                    "heading": -0.5720524653781475,
-                    "angularVelocity": 0.000001151961455361842,
-                    "velocityX": 3.585065266142057,
-                    "velocityY": -1.2074527496557126,
-                    "timestamp": 1.9693053982872004
-                },
-                {
-                    "x": 6.602974533610246,
-                    "y": 3.685417746961916,
-                    "heading": -0.5720524086130027,
-                    "angularVelocity": 0.0000012373387213911858,
-                    "velocityX": 3.445625691199315,
-                    "velocityY": -1.561505195243581,
-                    "timestamp": 2.015182201314219
-                },
-                {
-                    "x": 6.753006171084989,
-                    "y": 3.5981834135738024,
-                    "heading": -0.5720523449049553,
-                    "angularVelocity": 0.0000013886766999770178,
-                    "velocityX": 3.27031588025751,
-                    "velocityY": -1.9014911160397099,
-                    "timestamp": 2.0610590043412373
-                },
-                {
-                    "x": 6.893461047058093,
-                    "y": 3.4962434932531963,
-                    "heading": -0.5720522700606003,
-                    "angularVelocity": 0.0000016314204553722347,
-                    "velocityX": 3.0615663408441725,
-                    "velocityY": -2.2220362709370285,
-                    "timestamp": 2.106935807368256
-                },
-                {
-                    "x": 7.0229029918463715,
-                    "y": 3.380640293435665,
-                    "heading": -0.5720521774325067,
-                    "angularVelocity": 0.000002019061650871652,
-                    "velocityX": 2.8215118806773165,
-                    "velocityY": -2.519861720737319,
-                    "timestamp": 2.1528126103952743
+                    "heading": -0.5720526754836338,
+                    "angularVelocity": 0.0000031341671777608764,
+                    "velocityX": 3.404811893823314,
+                    "velocityY": -1.6485992876907705,
+                    "timestamp": 1.8344243287976039
+                },
+                {
+                    "x": 5.946280542451374,
+                    "y": 3.905964298639421,
+                    "heading": -0.5720526220643124,
+                    "angularVelocity": 0.0000011643081031480314,
+                    "velocityX": 3.5538876198319724,
+                    "velocityY": -1.296347894456755,
+                    "timestamp": 1.880305072239023
+                },
+                {
+                    "x": 6.113507668800577,
+                    "y": 3.8594929620139826,
+                    "heading": -0.5720525704992387,
+                    "angularVelocity": 0.0000011238935971124524,
+                    "velocityX": 3.6448216355273746,
+                    "velocityY": -1.0128723542630858,
+                    "timestamp": 1.9261858156804423
+                },
+                {
+                    "x": 6.280741389533688,
+                    "y": 3.813045361829368,
+                    "heading": -0.5720525189366377,
+                    "angularVelocity": 0.000001123839702331554,
+                    "velocityX": 3.64496536431679,
+                    "velocityY": -1.0123550034458102,
+                    "timestamp": 1.9720665591218616
+                },
+                {
+                    "x": 6.445214711488264,
+                    "y": 3.7576105066241974,
+                    "heading": -0.5720524661781627,
+                    "angularVelocity": 0.000001149904532146113,
+                    "velocityX": 3.584800716329628,
+                    "velocityY": -1.2082379457532038,
+                    "timestamp": 2.0179473025632806
+                },
+                {
+                    "x": 6.603250853740864,
+                    "y": 3.685853457646542,
+                    "heading": -0.5720524095450499,
+                    "angularVelocity": 0.0000012343547357711312,
+                    "velocityX": 3.444498288357164,
+                    "velocityY": -1.563990545825532,
+                    "timestamp": 2.0638280460046996
+                },
+                {
+                    "x": 6.753231641982998,
+                    "y": 3.5985021065376834,
+                    "heading": -0.5720523460454168,
+                    "angularVelocity": 0.0000013840149106858422,
+                    "velocityX": 3.268926721589291,
+                    "velocityY": -1.9038782843733606,
+                    "timestamp": 2.1097087894461186
+                },
+                {
+                    "x": 6.893623224143754,
+                    "y": 3.4964496816334383,
+                    "heading": -0.5720522715269948,
+                    "angularVelocity": 0.000001624176426799149,
+                    "velocityX": 3.059923872852999,
+                    "velocityY": -2.2242975429267773,
+                    "timestamp": 2.1555895328875376
+                },
+                {
+                    "x": 7.022989830955657,
+                    "y": 3.3807398195880776,
+                    "heading": -0.5720521794208858,
+                    "angularVelocity": 0.000002007511259312441,
+                    "velocityX": 2.819627519265176,
+                    "velocityY": -2.521970076471132,
+                    "timestamp": 2.2014702763289566
                 },
                 {
                     "x": 7.140008449554443,
                     "y": 3.2525558471679688,
-                    "heading": -0.5720520639012779,
-                    "angularVelocity": 0.0000024746979161572956,
-                    "velocityX": 2.5526071997441684,
-                    "velocityY": -2.7919217952549285,
-                    "timestamp": 2.1986894134222927
-                },
-                {
-                    "x": 7.220415929770595,
-                    "y": 3.149946383889573,
-                    "heading": -0.5720519473102574,
-                    "angularVelocity": 0.000003383341158681086,
-                    "velocityX": 2.333335244140948,
-                    "velocityY": -2.977611988413324,
-                    "timestamp": 2.2331497338662096
-                },
-                {
-                    "x": 7.2928033634314815,
-                    "y": 3.0415299314010706,
-                    "heading": -0.5720518591979534,
-                    "angularVelocity": 0.000002556920622107498,
-                    "velocityX": 2.100602453152653,
-                    "velocityY": -3.1461243276865574,
-                    "timestamp": 2.2676100543101265
-                },
-                {
-                    "x": 7.356753135957797,
-                    "y": 2.927931973099431,
-                    "heading": -0.5720517877574303,
-                    "angularVelocity": 0.000002073124165462267,
-                    "velocityX": 1.855750953634494,
-                    "velocityY": -3.2964858375736252,
-                    "timestamp": 2.3020703747540434
-                },
-                {
-                    "x": 7.411896317838347,
-                    "y": 2.8098078848104033,
-                    "heading": -0.5720517267503814,
-                    "angularVelocity": 0.0000017703564026272917,
-                    "velocityX": 1.6001935318717442,
-                    "velocityY": -3.427829073187373,
-                    "timestamp": 2.3365306951979603
-                },
-                {
-                    "x": 7.457914797691639,
-                    "y": 2.687839154921363,
-                    "heading": -0.572051672451037,
-                    "angularVelocity": 0.0000015757063075533039,
-                    "velocityX": 1.3354048732140973,
-                    "velocityY": -3.5393962771625227,
-                    "timestamp": 2.370991015641877
-                },
-                {
-                    "x": 7.494543133171347,
-                    "y": 2.5627294544923394,
-                    "heading": -0.5720516223974168,
-                    "angularVelocity": 0.000001452500141635371,
-                    "velocityX": 1.0629133742199746,
-                    "velocityY": -3.630543733121263,
-                    "timestamp": 2.405451336085794
-                },
-                {
-                    "x": 7.521592976236692,
-                    "y": 2.435205423722826,
-                    "heading": -0.572051574793162,
-                    "angularVelocity": 0.0000013814222897397843,
-                    "velocityX": 0.7849562255098341,
-                    "velocityY": -3.70060490229679,
-                    "timestamp": 2.439911656529711
-                },
-                {
-                    "x": 7.539069360193098,
-                    "y": 2.3060208712744585,
-                    "heading": -0.572051528188319,
-                    "angularVelocity": 0.000001352420480159191,
-                    "velocityX": 0.5071451376970121,
-                    "velocityY": -3.748791386272309,
-                    "timestamp": 2.474371976973628
-                },
-                {
-                    "x": 7.551504596507269,
-                    "y": 2.176254012981795,
-                    "heading": -0.5720514816592456,
-                    "angularVelocity": 0.0000013502217280486013,
-                    "velocityX": 0.3608566651205708,
-                    "velocityY": -3.7656892513183533,
-                    "timestamp": 2.5088322974175448
-                },
-                {
-                    "x": 7.5639699142109125,
-                    "y": 2.0464900408376128,
-                    "heading": -0.5720514351306527,
-                    "angularVelocity": 0.0000013502077822692335,
-                    "velocityX": 0.36172959342888805,
-                    "velocityY": -3.765605498514506,
-                    "timestamp": 2.5432926178614617
-                },
-                {
-                    "x": 7.576441327813496,
-                    "y": 1.916726654418564,
-                    "heading": -0.5720513886022516,
-                    "angularVelocity": 0.00000135020222116771,
-                    "velocityX": 0.36190648960876737,
-                    "velocityY": -3.7655885014252446,
-                    "timestamp": 2.5777529383053785
-                },
-                {
-                    "x": 7.58884833483585,
-                    "y": 1.786957094038731,
-                    "heading": -0.5720513420716767,
-                    "angularVelocity": 0.000001350265295572762,
-                    "velocityX": 0.36003748258495927,
-                    "velocityY": -3.765767662871606,
-                    "timestamp": 2.6122132587492954
-                },
-                {
-                    "x": 7.601019357724818,
-                    "y": 1.6571651890693204,
-                    "heading": -0.5720512933827582,
-                    "angularVelocity": 0.0000014128980208836598,
-                    "velocityX": 0.3531894866947264,
-                    "velocityY": -3.7664160778959355,
-                    "timestamp": 2.6466735791932123
-                },
-                {
-                    "x": 7.610550391987499,
-                    "y": 1.5345810963054864,
-                    "heading": -0.5537181215211259,
-                    "angularVelocity": 0.5320081654919462,
-                    "velocityX": 0.2765799661741953,
-                    "velocityY": -3.5572534203020942,
-                    "timestamp": 2.6811338996371292
-                },
-                {
-                    "x": 7.620334086621719,
-                    "y": 1.4202071871878734,
-                    "heading": -0.5150322166693104,
-                    "angularVelocity": 1.122621738667962,
-                    "velocityX": 0.2839118878810709,
-                    "velocityY": -3.319003063356803,
-                    "timestamp": 2.715594220081046
-                },
-                {
-                    "x": 7.630352820350644,
-                    "y": 1.314046573370508,
-                    "heading": -0.4556179078059868,
-                    "angularVelocity": 1.7241368651811046,
-                    "velocityX": 0.290732459821803,
-                    "velocityY": -3.0806624096878554,
-                    "timestamp": 2.750054540524963
-                },
-                {
-                    "x": 7.63947235322732,
-                    "y": 1.2175511973082003,
-                    "heading": -0.3892109145347841,
-                    "angularVelocity": 1.9270567544269157,
-                    "velocityX": 0.2646386556821465,
-                    "velocityY": -2.8001880080992603,
-                    "timestamp": 2.78451486096888
-                },
-                {
-                    "x": 7.647652812069227,
-                    "y": 1.1309118710627388,
-                    "heading": -0.32231407882554747,
-                    "angularVelocity": 1.9412714347239703,
-                    "velocityX": 0.23738777633239685,
-                    "velocityY": -2.5141764536539966,
-                    "timestamp": 2.818975181412797
-                },
-                {
-                    "x": 7.654895533140534,
-                    "y": 1.0540989766744202,
-                    "heading": -0.25819483301023066,
-                    "angularVelocity": 1.8606688791433752,
-                    "velocityX": 0.21017567387602423,
-                    "velocityY": -2.22902437930995,
-                    "timestamp": 2.8534355018567137
-                },
-                {
-                    "x": 7.66120815675186,
-                    "y": 0.9870533783479266,
-                    "heading": -0.19885832997172487,
-                    "angularVelocity": 1.7218790270703905,
-                    "velocityX": 0.18318528469800885,
-                    "velocityY": -1.9455883596791401,
-                    "timestamp": 2.8878958223006306
-                },
-                {
-                    "x": 7.666600355589264,
-                    "y": 0.929716153410391,
-                    "heading": -0.14570825222338094,
-                    "angularVelocity": 1.542355876662632,
-                    "velocityX": 0.15647558606309142,
-                    "velocityY": -1.663862210186161,
-                    "timestamp": 2.9223561427445475
-                },
-                {
-                    "x": 7.671080940668327,
-                    "y": 0.8820338579186632,
-                    "heading": -0.09980369912266952,
-                    "angularVelocity": 1.3320988461333123,
-                    "velocityX": 0.1300215732566484,
-                    "velocityY": -1.383686944215496,
-                    "timestamp": 2.9568164631884644
-                },
-                {
-                    "x": 7.674656709905022,
-                    "y": 0.8439593502971015,
-                    "heading": -0.06197555143316066,
-                    "angularVelocity": 1.0977305841102087,
-                    "velocityX": 0.10376482837700403,
-                    "velocityY": -1.1048796741031013,
-                    "timestamp": 2.9912767836323813
-                },
-                {
-                    "x": 7.67733258738128,
-                    "y": 0.8154516156506164,
-                    "heading": -0.032889080662387146,
-                    "angularVelocity": 0.8440568861838474,
-                    "velocityX": 0.07765097485389878,
-                    "velocityY": -0.8272626104241091,
-                    "timestamp": 3.025737104076298
-                },
+                    "heading": -0.5720520666435536,
+                    "angularVelocity": 0.0000024580537218711263,
+                    "velocityX": 2.550495258387596,
+                    "velocityY": -2.7938512501200163,
+                    "timestamp": 2.2473510197703757
+                },
+                {
+                    "x": 7.220333202972963,
+                    "y": 3.149893277120657,
+                    "heading": -0.57205195105401,
+                    "angularVelocity": 0.0000033545158672128766,
+                    "velocityX": 2.3310989161613462,
+                    "velocityY": -2.9793630927244275,
+                    "timestamp": 2.28180891122307
+                },
+                {
+                    "x": 7.292634698776439,
+                    "y": 3.0414305376048008,
+                    "heading": -0.5720518635503085,
+                    "angularVelocity": 0.000002539438655350069,
+                    "velocityX": 2.098256531535021,
+                    "velocityY": -3.1476893954673923,
+                    "timestamp": 2.3162668026757647
+                },
+                {
+                    "x": 7.356495877093559,
+                    "y": 2.9277932908128785,
+                    "heading": -0.5720517925210026,
+                    "angularVelocity": 0.000002061336399152925,
+                    "velocityX": 1.853310682252877,
+                    "velocityY": -3.297858400531866,
+                    "timestamp": 2.350724694128459
+                },
+                {
+                    "x": 7.411548371625277,
+                    "y": 2.8096370468188216,
+                    "heading": -0.5720517318127031,
+                    "angularVelocity": 0.0000017618112132015592,
+                    "velocityX": 1.5976745009881768,
+                    "velocityY": -3.4290038946879324,
+                    "timestamp": 2.3851825855811537
+                },
+                {
+                    "x": 7.457474639290446,
+                    "y": 2.6876433834723,
+                    "heading": -0.5720516777421675,
+                    "angularVelocity": 0.0000015691771403552433,
+                    "velocityX": 1.332822924707698,
+                    "velocityY": -3.540369366882859,
+                    "timestamp": 2.419640477033848
+                },
+                {
+                    "x": 7.49400980759948,
+                    "y": 2.5625160164724585,
+                    "heading": -0.5720516278694773,
+                    "angularVelocity": 0.0000014473517720699968,
+                    "velocityX": 1.0602845028769212,
+                    "velocityY": -3.631312356172488,
+                    "timestamp": 2.4540983684865427
+                },
+                {
+                    "x": 7.520944004354476,
+                    "y": 2.4349769014207707,
+                    "heading": -0.572051580412575,
+                    "angularVelocity": 0.0000013772433620936625,
+                    "velocityX": 0.7816553950223419,
+                    "velocityY": -3.7013035236575447,
+                    "timestamp": 2.4885562599392372
+                },
+                {
+                    "x": 7.53814696549347,
+                    "y": 2.3057649240296647,
+                    "heading": -0.5720515339265989,
+                    "angularVelocity": 0.0000013490661810968306,
+                    "velocityX": 0.4992459031544392,
+                    "velocityY": -3.7498515418008185,
+                    "timestamp": 2.5230141513919317
+                },
+                {
+                    "x": 7.550702118784975,
+                    "y": 2.1760188446169613,
+                    "heading": -0.5720514875103294,
+                    "angularVelocity": 0.0000013470432314212435,
+                    "velocityX": 0.3643622044850161,
+                    "velocityY": -3.7653516783177263,
+                    "timestamp": 2.5574720428446263
+                },
+                {
+                    "x": 7.56324456343349,
+                    "y": 2.046271536053824,
+                    "heading": -0.5720514410938347,
+                    "angularVelocity": 0.000001347049770461967,
+                    "velocityX": 0.36399338786688684,
+                    "velocityY": -3.765387349404451,
+                    "timestamp": 2.5919299342973208
+                },
+                {
+                    "x": 7.57578727298306,
+                    "y": 1.916524253098162,
+                    "heading": -0.5720513946773481,
+                    "angularVelocity": 0.0000013470495296572014,
+                    "velocityX": 0.3640010755350196,
+                    "velocityY": -3.7653866062523553,
+                    "timestamp": 2.6263878257500153
+                },
+                {
+                    "x": 7.588311679081848,
+                    "y": 1.7867752020286953,
+                    "heading": -0.572051348260254,
+                    "angularVelocity": 0.0000013470671654480904,
+                    "velocityX": 0.3634698923895803,
+                    "velocityY": -3.765437918557532,
+                    "timestamp": 2.66084571720271
+                },
+                {
+                    "x": 7.600787981664821,
+                    "y": 1.657021516812979,
+                    "heading": -0.5720513014102728,
+                    "angularVelocity": 0.0000013596299477389166,
+                    "velocityX": 0.3620738837176714,
+                    "velocityY": -3.7655724057824624,
+                    "timestamp": 2.6953036086554043
+                },
+                {
+                    "x": 7.6103162713524775,
+                    "y": 1.534453918276911,
+                    "heading": -0.5536995765422551,
+                    "angularVelocity": 0.5325840930573226,
+                    "velocityX": 0.27651981261615693,
+                    "velocityY": -3.557025498914872,
+                    "timestamp": 2.729761500108099
+                },
+                {
+                    "x": 7.620120451226168,
+                    "y": 1.4200989858077522,
+                    "heading": -0.5149948955625814,
+                    "angularVelocity": 1.1232457747100673,
+                    "velocityX": 0.2845264019455608,
+                    "velocityY": -3.3186863051722235,
+                    "timestamp": 2.7642193915607933
+                },
+                {
+                    "x": 7.630164619508576,
+                    "y": 1.3139569074919104,
+                    "heading": -0.4555658179030021,
+                    "angularVelocity": 1.7246870064917483,
+                    "velocityX": 0.29149108836702503,
+                    "velocityY": -3.080341652987332,
+                    "timestamp": 2.798677283013488
+                },
+                {
+                    "x": 7.6393124908526255,
+                    "y": 1.2174783858270968,
+                    "heading": -0.38913465703239974,
+                    "angularVelocity": 1.9278939618746356,
+                    "velocityX": 0.26547971911102464,
+                    "velocityY": -2.799896267514583,
+                    "timestamp": 2.8331351744661823
+                },
+                {
+                    "x": 7.647521237661079,
+                    "y": 1.1308545482085492,
+                    "heading": -0.3222243620613333,
+                    "angularVelocity": 1.9417988782892024,
+                    "velocityX": 0.23822545322294675,
+                    "velocityY": -2.5139041875930466,
+                    "timestamp": 2.867593065918877
+                },
+                {
+                    "x": 7.654790830767336,
+                    "y": 1.0540553472510912,
+                    "heading": -0.25810310965019956,
+                    "angularVelocity": 1.8608582739068054,
+                    "velocityX": 0.21097034089302885,
+                    "velocityY": -2.2287841106847384,
+                    "timestamp": 2.9020509573715714
+                },
+                {
+                    "x": 7.661128119511832,
+                    "y": 0.9870214726082436,
+                    "heading": -0.19877373026472553,
+                    "angularVelocity": 1.7217936700197296,
+                    "velocityX": 0.18391400278139633,
+                    "velocityY": -1.9453852750938303,
+                    "timestamp": 2.936508848824266
+                },
+                {
+                    "x": 7.6665422514065025,
+                    "y": 0.9296939844226774,
+                    "heading": -0.14563708266620207,
+                    "angularVelocity": 1.5420748443496368,
+                    "velocityX": 0.15712313395848782,
+                    "velocityY": -1.6636969288812407,
+                    "timestamp": 2.9709667402769604
+                },
+                {
+                    "x": 7.6710416569080175,
+                    "y": 0.8820195033603357,
+                    "heading": -0.09974947152576126,
+                    "angularVelocity": 1.331701076470322,
+                    "velocityX": 0.1305769248148914,
+                    "velocityY": -1.3835577004993256,
+                    "timestamp": 3.005424631729655
+                },
+                {
+                    "x": 7.674632846509551,
+                    "y": 0.8439509935506372,
+                    "heading": -0.06193916709248371,
+                    "angularVelocity": 1.0972901370124863,
+                    "velocityX": 0.10421965622732904,
+                    "velocityY": -1.1047834967488865,
+                    "timestamp": 3.0398825231823494
+                },
+                {
+                    "x": 7.677320522590369,
+                    "y": 0.8154475641909131,
+                    "heading": -0.032869067302209315,
+                    "angularVelocity": 0.8436412840348516,
+                    "velocityX": 0.07799885505174348,
+                    "velocityY": -0.8271959820541263,
+                    "timestamp": 3.074340414635044
+                },
                 {
-                    "x": 7.679112422858347,
-                    "y": 0.7964752743679322,
-                    "heading": -0.013083588528854375,
-                    "angularVelocity": 0.5747332548975859,
-                    "velocityX": 0.05164883710108163,
-                    "velocityY": -0.5506722235380279,
-                    "timestamp": 3.060197424520215
+                    "x": 7.6791083601324726,
+                    "y": 0.7964739655020694,
+                    "heading": -0.013076341373692615,
+                    "angularVelocity": 0.5744032816313593,
+                    "velocityX": 0.05188470526556513,
+                    "velocityY": -0.5506314486738934,
+                    "timestamp": 3.1087983060877384
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
                     "heading": -0.003,
-                    "angularVelocity": 0.2926144736603049,
-                    "velocityX": 0.025756497043992084,
-                    "velocityY": -0.27496187632245256,
-                    "timestamp": 3.094657744964132
+                    "angularVelocity": 0.2924247813459689,
+                    "velocityX": 0.02587621673680596,
+                    "velocityY": -0.2749432743171821,
+                    "timestamp": 3.143256197540433
                 },
                 {
                     "x": 7.68,
                     "y": 0.787,
                     "heading": -0.003,
-                    "angularVelocity": 9.445490920898867e-33,
+                    "angularVelocity": -5.23227119774542e-33,
                     "velocityX": 0,
-                    "velocityY": 3.8255947334103544e-33,
-                    "timestamp": 3.129118065408049
+                    "velocityY": -1.249481443135334e-33,
+                    "timestamp": 3.1777140889931275
                 }
             ],
             "constraints": [
@@ -3602,7 +4052,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 23
+                    "controlIntervalCount": 10
+                },
+                {
+                    "x": 2.896845817565918,
+                    "y": 6.343715190887451,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 18
                 },
                 {
                     "x": 7.68,
@@ -3619,217 +4078,262 @@
                     "x": 1.35,
                     "y": 5.56,
                     "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": -2.1086675817598662e-32,
-                    "velocityY": -7.052966104933725e-38,
+                    "angularVelocity": -1.4340376968599448e-32,
+                    "velocityX": -4.9971810322519555e-29,
+                    "velocityY": 8.414879145245036e-29,
                     "timestamp": 0
                 },
                 {
-                    "x": 1.4233949672121562,
-                    "y": 5.582111248416047,
-                    "heading": 6.989955400155978e-19,
-                    "angularVelocity": 7.290266672073875e-18,
-                    "velocityX": 0.7654825584964474,
-                    "velocityY": 0.23061220206204183,
-                    "timestamp": 0.09588065253415788
-                },
-                {
-                    "x": 1.5701848986828006,
-                    "y": 5.626333744358309,
-                    "heading": -5.64955528358279e-19,
-                    "angularVelocity": -1.3182545571253806e-17,
-                    "velocityX": 1.530965086187225,
-                    "velocityY": 0.4612243948434498,
-                    "timestamp": 0.19176130506831576
-                },
-                {
-                    "x": 1.7903697885046006,
-                    "y": 5.69266748604712,
-                    "heading": -1.1328157855510893e-18,
-                    "angularVelocity": -5.922574643212496e-18,
-                    "velocityX": 2.2964475522666925,
-                    "velocityY": 0.6918365690635988,
-                    "timestamp": 0.28764195760247363
-                },
-                {
-                    "x": 2.08394961895559,
-                    "y": 5.781112468143492,
-                    "heading": -1.2347885191323161e-17,
-                    "angularVelocity": -1.1696905584501664e-16,
-                    "velocityX": 3.061929833512548,
-                    "velocityY": 0.9224486876000673,
-                    "timestamp": 0.3835226101366315
-                },
-                {
-                    "x": 2.4312425305333245,
-                    "y": 5.885739258408696,
-                    "heading": -1.8571615119787707e-17,
-                    "angularVelocity": -6.491121789400923e-17,
-                    "velocityX": 3.6221375470302584,
-                    "velocityY": 1.0912190050847865,
-                    "timestamp": 0.4794032626707894
-                },
-                {
-                    "x": 2.7785354421111035,
-                    "y": 5.990366048673914,
-                    "heading": -2.5105020741934312e-17,
-                    "angularVelocity": -6.814102166319416e-17,
-                    "velocityX": 3.6221375470307158,
-                    "velocityY": 1.0912190050849244,
-                    "timestamp": 0.5752839152049473
-                },
-                {
-                    "x": 3.125828353688883,
-                    "y": 6.094992838939131,
-                    "heading": -3.974939977960392e-17,
-                    "angularVelocity": -1.527354961675144e-16,
-                    "velocityX": 3.622137547030716,
-                    "velocityY": 1.0912190050849244,
-                    "timestamp": 0.6711645677391052
-                },
-                {
-                    "x": 3.473121265266662,
-                    "y": 6.199619629204348,
-                    "heading": -4.6160884572564577e-17,
-                    "angularVelocity": -6.68694321901595e-17,
-                    "velocityX": 3.6221375470307158,
-                    "velocityY": 1.0912190050849242,
-                    "timestamp": 0.767045220273263
-                },
-                {
-                    "x": 3.8204141768444413,
-                    "y": 6.304246419469565,
-                    "heading": -5.207468497060529e-17,
-                    "angularVelocity": -6.167876669314405e-17,
-                    "velocityX": 3.6221375470307158,
-                    "velocityY": 1.0912190050849244,
-                    "timestamp": 0.862925872807421
-                },
-                {
-                    "x": 4.16770708842222,
-                    "y": 6.408873209734782,
-                    "heading": -3.0597254964935646e-17,
-                    "angularVelocity": 2.24001708769928e-16,
-                    "velocityX": 3.6221375470307158,
-                    "velocityY": 1.0912190050849242,
-                    "timestamp": 0.9588065253415788
-                },
-                {
-                    "x": 4.515,
-                    "y": 6.5135,
-                    "heading": -2.0783137065340813e-18,
-                    "angularVelocity": 2.974420855995074e-16,
-                    "velocityX": 3.6221375470307158,
-                    "velocityY": 1.0912190050849244,
-                    "timestamp": 1.0546871778757367
-                },
-                {
-                    "x": 4.862292911577779,
-                    "y": 6.618126790265217,
-                    "heading": -2.7789619178473824e-17,
-                    "angularVelocity": -2.681594752696605e-16,
-                    "velocityX": 3.6221375470307158,
-                    "velocityY": 1.0912190050849244,
-                    "timestamp": 1.1505678304098945
-                },
-                {
-                    "x": 5.209585823155558,
-                    "y": 6.722753580530434,
-                    "heading": 2.2712942266126744e-18,
-                    "angularVelocity": 3.135242889004865e-16,
-                    "velocityX": 3.6221375470307158,
-                    "velocityY": 1.0912190050849242,
-                    "timestamp": 1.2464484829440523
-                },
-                {
-                    "x": 5.5568787347333375,
-                    "y": 6.827380370795651,
-                    "heading": 3.7041174502192387e-17,
-                    "angularVelocity": 3.6263708429820527e-16,
-                    "velocityX": 3.6221375470307158,
-                    "velocityY": 1.0912190050849244,
-                    "timestamp": 1.34232913547821
-                },
-                {
-                    "x": 5.9041716463111165,
-                    "y": 6.932007161060868,
-                    "heading": 6.494346998398645e-17,
-                    "angularVelocity": 2.910106965725311e-16,
-                    "velocityX": 3.6221375470307158,
-                    "velocityY": 1.0912190050849244,
-                    "timestamp": 1.4382097880123679
-                },
-                {
-                    "x": 6.251464557888895,
-                    "y": 7.036633951326086,
-                    "heading": 5.759102638783417e-17,
-                    "angularVelocity": -7.668328700140211e-17,
-                    "velocityX": 3.6221375470307158,
-                    "velocityY": 1.0912190050849244,
-                    "timestamp": 1.5340904405465257
-                },
-                {
-                    "x": 6.598757469466675,
-                    "y": 7.141260741591303,
-                    "heading": 2.5440135069531877e-17,
-                    "angularVelocity": -3.353219911269207e-16,
-                    "velocityX": 3.6221375470307158,
-                    "velocityY": 1.0912190050849244,
-                    "timestamp": 1.6299710930806834
-                },
-                {
-                    "x": 6.94605038104441,
-                    "y": 7.245887531856507,
-                    "heading": 2.2711688502269004e-17,
-                    "angularVelocity": -2.84566958514003e-17,
-                    "velocityX": 3.6221375470302584,
-                    "velocityY": 1.0912190050847868,
-                    "timestamp": 1.7258517456148412
-                },
-                {
-                    "x": 7.239630211495399,
-                    "y": 7.334332513952879,
-                    "heading": 1.1340524791180054e-17,
-                    "angularVelocity": -1.1859706178712186e-16,
-                    "velocityX": 3.061929833512548,
-                    "velocityY": 0.9224486876000674,
-                    "timestamp": 1.821732398148999
-                },
-                {
-                    "x": 7.4598151013172,
-                    "y": 7.40066625564169,
-                    "heading": 5.896173994762203e-18,
-                    "angularVelocity": -5.67825796795303e-17,
-                    "velocityX": 2.2964475522666925,
-                    "velocityY": 0.6918365690635988,
-                    "timestamp": 1.9176130506831568
-                },
+                    "x": 1.3816327251489127,
+                    "y": 5.576194684745321,
+                    "heading": 4.506697153639657e-18,
+                    "angularVelocity": 6.903205799499945e-17,
+                    "velocityX": 0.4845393494564494,
+                    "velocityY": 0.24806468536018447,
+                    "timestamp": 0.06528412023584423
+                },
+                {
+                    "x": 1.4448981743168023,
+                    "y": 5.608584053657482,
+                    "heading": 9.944941296273304e-18,
+                    "angularVelocity": 8.330117912909241e-17,
+                    "velocityX": 0.9690786816049286,
+                    "velocityY": 0.4961293618593833,
+                    "timestamp": 0.13056824047168847
+                },
+                {
+                    "x": 1.5397963458087682,
+                    "y": 5.657168105868761,
+                    "heading": 6.849951425963079e-18,
+                    "angularVelocity": -4.74080026246192e-17,
+                    "velocityX": 1.4536179877914959,
+                    "velocityY": 0.7441940250671262,
+                    "timestamp": 0.1958523607075327
+                },
+                {
+                    "x": 1.666327236799984,
+                    "y": 5.721946839932961,
+                    "heading": -2.8668788112285783e-18,
+                    "angularVelocity": -1.4883910439092565e-16,
+                    "velocityX": 1.9381572507083278,
+                    "velocityY": 0.9922586661225022,
+                    "timestamp": 0.26113648094337694
+                },
+                {
+                    "x": 1.8244908416408254,
+                    "y": 5.802920252957701,
+                    "heading": -1.6578007712929266e-17,
+                    "angularVelocity": -2.1002241242045617e-16,
+                    "velocityX": 2.4226964270861338,
+                    "velocityY": 1.2403232628733727,
+                    "timestamp": 0.32642060117922117
+                },
+                {
+                    "x": 2.0142871433826555,
+                    "y": 5.900088336265959,
+                    "heading": -2.5925213294324244e-17,
+                    "angularVelocity": -1.4317730798527664e-16,
+                    "velocityX": 2.9072353438504677,
+                    "velocityY": 1.4883877267125722,
+                    "timestamp": 0.3917047214150654
+                },
+                {
+                    "x": 2.234118517125294,
+                    "y": 6.012633170077576,
+                    "heading": -1.2540245641654488e-17,
+                    "angularVelocity": 2.0502639235211304e-16,
+                    "velocityX": 3.3673023845382315,
+                    "velocityY": 1.723923572915424,
+                    "timestamp": 0.45698884165090964
+                },
+                {
+                    "x": 2.4539498908690707,
+                    "y": 6.125178003888657,
+                    "heading": 1.1802241639861868e-17,
+                    "angularVelocity": 3.728699597195476e-16,
+                    "velocityX": 3.3673023845556527,
+                    "velocityY": 1.72392357290723,
+                    "timestamp": 0.5222729618867539
+                },
+                {
+                    "x": 2.6737813045614214,
+                    "y": 6.237722759669026,
+                    "heading": 3.649409202314283e-17,
+                    "angularVelocity": 3.78221385080107e-16,
+                    "velocityX": 3.367302996474358,
+                    "velocityY": 1.7239223776592436,
+                    "timestamp": 0.5875570821225982
+                },
+                {
+                    "x": 2.896845817565918,
+                    "y": 6.343715190887451,
+                    "heading": 3.1275322529875387e-17,
+                    "angularVelocity": -7.99393399896351e-17,
+                    "velocityX": 3.416826514605052,
+                    "velocityY": 1.6235560935112305,
+                    "timestamp": 0.6528412023584425
+                },
+                {
+                    "x": 3.2165948847884085,
+                    "y": 6.4256885407325814,
+                    "heading": 6.106570262583634e-20,
+                    "angularVelocity": -3.577262143903813e-16,
+                    "velocityX": 3.664435260812148,
+                    "velocityY": 0.9394430333407913,
+                    "timestamp": 0.7400985952347529
+                },
+                {
+                    "x": 3.5380520485012785,
+                    "y": 6.500684460821759,
+                    "heading": -4.2865718547534536e-17,
+                    "angularVelocity": -4.919558421997663e-16,
+                    "velocityX": 3.6840106392881062,
+                    "velocityY": 0.8594792672236512,
+                    "timestamp": 0.8273559881110633
+                },
+                {
+                    "x": 3.8595092199655934,
+                    "y": 6.5756803476856875,
+                    "heading": -3.198465678230611e-17,
+                    "angularVelocity": 1.2470074347342177e-16,
+                    "velocityX": 3.684010728122356,
+                    "velocityY": 0.8594788864507628,
+                    "timestamp": 0.9146133809873737
+                },
+                {
+                    "x": 4.180966391429945,
+                    "y": 6.6506762345494606,
+                    "heading": 1.830329484329921e-18,
+                    "angularVelocity": 3.8753147618851267e-16,
+                    "velocityX": 3.6840107281227708,
+                    "velocityY": 0.8594788864489847,
+                    "timestamp": 1.001870773863684
+                },
+                {
+                    "x": 4.502423562894296,
+                    "y": 6.725672121413234,
+                    "heading": 1.519427405027002e-17,
+                    "angularVelocity": 1.531554419106218e-16,
+                    "velocityX": 3.684010728122771,
+                    "velocityY": 0.8594788864489847,
+                    "timestamp": 1.0891281667399946
+                },
+                {
+                    "x": 4.823880734358647,
+                    "y": 6.800668008277006,
+                    "heading": 2.800360653678013e-17,
+                    "angularVelocity": 1.4679939507274274e-16,
+                    "velocityX": 3.6840107281227708,
+                    "velocityY": 0.8594788864489846,
+                    "timestamp": 1.176385559616305
+                },
+                {
+                    "x": 5.1453379058229975,
+                    "y": 6.875663895140779,
+                    "heading": -9.132380596349043e-18,
+                    "angularVelocity": -4.2559129844470954e-16,
+                    "velocityX": 3.6840107281227708,
+                    "velocityY": 0.8594788864489846,
+                    "timestamp": 1.2636429524926156
+                },
+                {
+                    "x": 5.466795077287348,
+                    "y": 6.950659782004552,
+                    "heading": 1.9053427809876355e-18,
+                    "angularVelocity": 1.2649613980635305e-16,
+                    "velocityX": 3.6840107281227708,
+                    "velocityY": 0.8594788864489846,
+                    "timestamp": 1.3509003453689261
+                },
+                {
+                    "x": 5.788252248751699,
+                    "y": 7.025655668868324,
+                    "heading": -3.1119644574102345e-18,
+                    "angularVelocity": -5.750008191867264e-17,
+                    "velocityX": 3.684010728122771,
+                    "velocityY": 0.8594788864489846,
+                    "timestamp": 1.4381577382452366
+                },
+                {
+                    "x": 6.10970942021605,
+                    "y": 7.100651555732097,
+                    "heading": 3.013239534330982e-17,
+                    "angularVelocity": 3.80991898964227e-16,
+                    "velocityX": 3.6840107281227708,
+                    "velocityY": 0.8594788864489846,
+                    "timestamp": 1.5254151311215471
+                },
+                {
+                    "x": 6.431166591680401,
+                    "y": 7.17564744259587,
+                    "heading": 3.192919981892433e-17,
+                    "angularVelocity": 2.0592002767735807e-17,
+                    "velocityX": 3.6840107281227708,
+                    "velocityY": 0.8594788864489847,
+                    "timestamp": 1.6126725239978577
+                },
+                {
+                    "x": 6.752623763144745,
+                    "y": 7.250643329459641,
+                    "heading": 5.4408724078031107e-17,
+                    "angularVelocity": 2.576231481446227e-16,
+                    "velocityX": 3.6840107281226917,
+                    "velocityY": 0.8594788864489661,
+                    "timestamp": 1.6999299168741682
+                },
+                {
+                    "x": 7.061749152944237,
+                    "y": 7.322762214393876,
+                    "heading": 4.954044927264927e-17,
+                    "angularVelocity": -5.579212125566035e-17,
+                    "velocityX": 3.5426842312110383,
+                    "velocityY": 0.8265074460391663,
+                    "timestamp": 1.7871873097504787
+                },
+                {
+                    "x": 7.309049484595465,
+                    "y": 7.380457326963315,
+                    "heading": 3.4625344335160436e-17,
+                    "angularVelocity": -1.7093228018551423e-16,
+                    "velocityX": 2.834147612017054,
+                    "velocityY": 0.6612060098016332,
+                    "timestamp": 1.8744447026267892
+                },
+                {
+                    "x": 7.49452473986686,
+                    "y": 7.4237286629145345,
+                    "heading": 2.1983691983745354e-17,
+                    "angularVelocity": -1.448777241544276e-16,
+                    "velocityX": 2.125610783882924,
+                    "velocityY": 0.49590452481841046,
+                    "timestamp": 1.9617020955030997
+                },
                 {
-                    "x": 7.606605032787844,
-                    "y": 7.444888751583952,
-                    "heading": 1.9652682638036787e-18,
-                    "angularVelocity": -4.099790316709085e-17,
-                    "velocityX": 1.530965086187225,
-                    "velocityY": 0.46122439484344985,
-                    "timestamp": 2.0134937032173146
+                    "x": 7.618174912681235,
+                    "y": 7.452576220829731,
+                    "heading": 1.0249539478821592e-17,
+                    "angularVelocity": -1.3447746039961902e-16,
+                    "velocityX": 1.4170738861021466,
+                    "velocityY": 0.3306030235866404,
+                    "timestamp": 2.04895948837941
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
                     "heading": 0,
-                    "angularVelocity": -2.0497026255693862e-17,
-                    "velocityX": 0.7654825584964475,
-                    "velocityY": 0.23061220206204183,
-                    "timestamp": 2.1093743557514726
+                    "angularVelocity": -1.174632794233726e-16,
+                    "velocityX": 0.7085369534980623,
+                    "velocityY": 0.16530151423060047,
+                    "timestamp": 2.1362168812557205
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
                     "heading": 0,
                     "angularVelocity": 0,
-                    "velocityX": 2.8084467268312453e-34,
+                    "velocityX": -1.0281234266250986e-33,
                     "velocityY": 0,
-                    "timestamp": 2.2052550082856306
+                    "timestamp": 2.223474274132031
                 }
             ],
             "constraints": [
@@ -3860,7 +4364,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 22
+                    "controlIntervalCount": 10
+                },
+                {
+                    "x": 2.976557731628418,
+                    "y": 6.303859233856201,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 18
                 },
                 {
                     "x": 7.68,
@@ -3877,208 +4390,262 @@
                     "x": 1.35,
                     "y": 5.56,
                     "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 4.006684802425094e-34,
-                    "velocityY": 0,
+                    "angularVelocity": -1.3272791352480801e-30,
+                    "velocityX": -6.138871086325045e-33,
+                    "velocityY": -8.954734510196666e-33,
                     "timestamp": 0
                 },
                 {
-                    "x": 1.4307759237486215,
-                    "y": 5.581186236587336,
-                    "heading": -0.002104963011526108,
-                    "angularVelocity": -0.021033383553861286,
-                    "velocityX": 0.8071357913745165,
-                    "velocityY": 0.21169884590096585,
-                    "timestamp": 0.1000772417873624
-                },
-                {
-                    "x": 1.592327768448065,
-                    "y": 5.623558708255768,
-                    "heading": -0.006314883423264977,
-                    "angularVelocity": -0.04206671103789844,
-                    "velocityX": 1.6142715547926287,
-                    "velocityY": 0.42339767675114676,
-                    "timestamp": 0.2001544835747248
-                },
-                {
-                    "x": 1.8346555283201378,
-                    "y": 5.6871174126709505,
-                    "heading": -0.012629752552513055,
-                    "angularVelocity": -0.06309995176191506,
-                    "velocityX": 2.4214072604734147,
-                    "velocityY": 0.6350964842758995,
-                    "timestamp": 0.3002317253620872
-                },
-                {
-                    "x": 2.1577591856288034,
-                    "y": 5.77186234429474,
-                    "heading": -0.02104955396970995,
-                    "angularVelocity": -0.08413302831713466,
-                    "velocityX": 3.228542788930726,
-                    "velocityY": 0.8467952364619561,
-                    "timestamp": 0.4003089671494496
-                },
-                {
-                    "x": 2.5239589562380984,
-                    "y": 5.867910684343787,
-                    "heading": -0.021049554851793276,
-                    "angularVelocity": -8.814025154574047e-9,
-                    "velocityX": 3.6591712967806616,
-                    "velocityY": 0.9597420785549242,
-                    "timestamp": 0.500386208936812
-                },
-                {
-                    "x": 2.8901587297210503,
-                    "y": 5.963959013436644,
-                    "heading": -0.021049555733868682,
-                    "angularVelocity": -8.813945991956408e-9,
-                    "velocityX": 3.659171325495056,
-                    "velocityY": 0.9597419690775807,
-                    "timestamp": 0.6004634507241744
-                },
-                {
-                    "x": 3.2563585069426413,
-                    "y": 6.060007328275347,
-                    "heading": -0.02104955661593894,
-                    "angularVelocity": -8.813894597525165e-9,
-                    "velocityX": 3.659171362852588,
-                    "velocityY": 0.9597418266460623,
-                    "timestamp": 0.7005406925115368
-                },
-                {
-                    "x": 3.622558289188601,
-                    "y": 6.15605562395784,
-                    "heading": -0.021049557498003604,
-                    "angularVelocity": -8.813838677794314e-9,
-                    "velocityX": 3.6591714130574977,
-                    "velocityY": 0.9597416352318217,
-                    "timestamp": 0.8006179342988993
-                },
-                {
-                    "x": 3.9887580784892815,
-                    "y": 6.252103892743073,
-                    "heading": -0.02104955838006166,
-                    "angularVelocity": -8.81377261667878e-9,
-                    "velocityX": 3.659171483550255,
-                    "velocityY": 0.9597413664668089,
-                    "timestamp": 0.9006951760862617
-                },
-                {
-                    "x": 4.354957878489299,
-                    "y": 6.3481521207353335,
-                    "heading": -0.021049559262112345,
-                    "angularVelocity": -8.813699036774153e-9,
-                    "velocityX": 3.6591715904610522,
-                    "velocityY": 0.9597409588519333,
-                    "timestamp": 1.0007724178736241
-                },
-                {
-                    "x": 4.721157716449094,
-                    "y": 6.444200203999558,
-                    "heading": -0.021049560144156942,
-                    "angularVelocity": -8.81363814954091e-9,
-                    "velocityX": 3.659171969765827,
-                    "velocityY": 0.9597395126886197,
-                    "timestamp": 1.1008496596609865
-                },
-                {
-                    "x": 5.092065894328832,
-                    "y": 6.520059807846628,
-                    "heading": -0.02104956519242719,
-                    "angularVelocity": -5.0443738888274115e-8,
-                    "velocityX": 3.70621902897584,
-                    "velocityY": 0.7580105375830706,
-                    "timestamp": 1.2009269014483488
-                },
-                {
-                    "x": 5.47058189966855,
-                    "y": 6.512767035208857,
-                    "heading": -0.021049620221549888,
-                    "angularVelocity": -5.498665002090286e-7,
-                    "velocityX": 3.7822385847120277,
-                    "velocityY": -0.07287143917560433,
-                    "timestamp": 1.3010041432357111
-                },
-                {
-                    "x": 5.849069453983326,
-                    "y": 6.50412284194313,
-                    "heading": -0.02104967520004647,
-                    "angularVelocity": -5.493606298291521e-7,
-                    "velocityX": 3.7819542940538056,
-                    "velocityY": -0.08637521489745259,
-                    "timestamp": 1.4010813850230734
-                },
-                {
-                    "x": 6.2213754396920145,
-                    "y": 6.435451104158958,
-                    "heading": -0.021049699334872447,
-                    "angularVelocity": -2.4116198195152956e-7,
-                    "velocityX": 3.720186318681132,
-                    "velocityY": -0.6861873544644725,
-                    "timestamp": 1.5011586268104358
-                },
-                {
-                    "x": 6.5695694546238315,
-                    "y": 6.286829159405189,
-                    "heading": -0.02104969841623417,
-                    "angularVelocity": 9.179292491243133e-9,
-                    "velocityX": 3.4792527123362977,
-                    "velocityY": -1.4850723511100814,
-                    "timestamp": 1.601235868597798
-                },
-                {
-                    "x": 6.915952468622566,
-                    "y": 6.134033995374984,
-                    "heading": -0.021049697500180466,
-                    "angularVelocity": 9.153466759383175e-9,
-                    "velocityX": 3.4611566807037675,
-                    "velocityY": -1.5267723340622583,
-                    "timestamp": 1.7013131103851604
-                },
-                {
-                    "x": 7.221571466882516,
-                    "y": 5.999220411113471,
-                    "heading": -0.012629808861323527,
-                    "angularVelocity": 0.0841338998605394,
-                    "velocityX": 3.053831148837094,
-                    "velocityY": -1.347095322110863,
-                    "timestamp": 1.8013903521725227
-                },
-                {
-                    "x": 7.450785730315982,
-                    "y": 5.898110210460681,
-                    "heading": -0.0063148981970481905,
-                    "angularVelocity": 0.0631003667916113,
-                    "velocityX": 2.290373508899117,
-                    "velocityY": -1.0103216160535444,
-                    "timestamp": 1.901467593959885
-                },
+                    "x": 1.3835743361511121,
+                    "y": 5.575814713247826,
+                    "heading": -0.005199936040550204,
+                    "angularVelocity": -0.07791207752010428,
+                    "velocityX": 0.5030535492153302,
+                    "velocityY": 0.23695621540617479,
+                    "timestamp": 0.06674107796969504
+                },
+                {
+                    "x": 1.4507230090250494,
+                    "y": 5.607444140582854,
+                    "heading": -0.015599510401707435,
+                    "angularVelocity": -0.15581969422009245,
+                    "velocityX": 1.0061071069967922,
+                    "velocityY": 0.4739124433888902,
+                    "timestamp": 0.13348215593939008
+                },
+                {
+                    "x": 1.5514460199136038,
+                    "y": 5.654888284136176,
+                    "heading": -0.03119820909467289,
+                    "angularVelocity": -0.23371960968398373,
+                    "velocityX": 1.5091606841335288,
+                    "velocityY": 0.7108687033024069,
+                    "timestamp": 0.20022323390908514
+                },
+                {
+                    "x": 1.6857433700165965,
+                    "y": 5.718147147394209,
+                    "heading": -0.051995311102797384,
+                    "angularVelocity": -0.3116087219563307,
+                    "velocityX": 2.0122142792475226,
+                    "velocityY": 0.9478250154538677,
+                    "timestamp": 0.26696431187878017
+                },
+                {
+                    "x": 1.853615058416753,
+                    "y": 5.79722073482494,
+                    "heading": -0.07798989994858517,
+                    "angularVelocity": -0.3894841023932979,
+                    "velocityX": 2.515267860617741,
+                    "velocityY": 1.184781394550373,
+                    "timestamp": 0.3337053898484752
+                },
+                {
+                    "x": 2.0550610733526136,
+                    "y": 5.892109048523229,
+                    "heading": -0.10918084380142022,
+                    "angularVelocity": -0.4673425242996203,
+                    "velocityX": 3.0183212657627503,
+                    "velocityY": 1.4217378050347718,
+                    "timestamp": 0.4004464678181702
+                },
+                {
+                    "x": 2.283467758752316,
+                    "y": 5.999697539336407,
+                    "heading": -0.1091808488901602,
+                    "angularVelocity": -7.624599630551263e-8,
+                    "velocityX": 3.422280435797193,
+                    "velocityY": 1.6120280655645223,
+                    "timestamp": 0.46718754578786525
+                },
+                {
+                    "x": 2.511874447545338,
+                    "y": 6.107286022946165,
+                    "heading": -0.10918085397868219,
+                    "angularVelocity": -7.624272995870229e-8,
+                    "velocityX": 3.4222804866402323,
+                    "velocityY": 1.6120279576336907,
+                    "timestamp": 0.5339286237575603
+                },
+                {
+                    "x": 2.7402812851669234,
+                    "y": 6.214874190597382,
+                    "heading": -0.10918085906719958,
+                    "angularVelocity": -7.624266119750657e-8,
+                    "velocityX": 3.4222827165797036,
+                    "velocityY": 1.612023223539615,
+                    "timestamp": 0.6006697017272554
+                },
+                {
+                    "x": 2.976557731628418,
+                    "y": 6.303859233856201,
+                    "heading": -0.10918086426900071,
+                    "angularVelocity": -7.794002274324629e-8,
+                    "velocityX": 3.5401952388119984,
+                    "velocityY": 1.3332874740084952,
+                    "timestamp": 0.6674107796969505
+                },
+                {
+                    "x": 3.298504794987031,
+                    "y": 6.35898613680169,
+                    "heading": -0.1091808690742104,
+                    "angularVelocity": -5.565219068812679e-8,
+                    "velocityX": 3.7286737752062145,
+                    "velocityY": 0.6384597367556102,
+                    "timestamp": 0.753754365316963
+                },
+                {
+                    "x": 3.623164348220099,
+                    "y": 6.394833894219333,
+                    "heading": -0.10918087380844538,
+                    "angularVelocity": -5.483018746401896e-8,
+                    "velocityX": 3.760088846227146,
+                    "velocityY": 0.4151756863029092,
+                    "timestamp": 0.8400979509369756
+                },
+                {
+                    "x": 3.94782391548866,
+                    "y": 6.430681524522641,
+                    "heading": -0.10918087854267373,
+                    "angularVelocity": -5.483011064647812e-8,
+                    "velocityX": 3.7600890087811236,
+                    "velocityY": 0.41517421411094135,
+                    "timestamp": 0.9264415365569881
+                },
+                {
+                    "x": 4.272483486014202,
+                    "y": 6.466529125328635,
+                    "heading": -0.10918088327689503,
+                    "angularVelocity": -5.483002882633892e-8,
+                    "velocityX": 3.7600890465022867,
+                    "velocityY": 0.4151738724837589,
+                    "timestamp": 1.0127851221770008
+                },
+                {
+                    "x": 4.597143061517746,
+                    "y": 6.502376681050562,
+                    "heading": -0.10918088801110931,
+                    "angularVelocity": -5.482994771911856e-8,
+                    "velocityX": 3.760089104155704,
+                    "velocityY": 0.4151733503364993,
+                    "timestamp": 1.0991287077970133
+                },
+                {
+                    "x": 4.921802670469747,
+                    "y": 6.538223933839528,
+                    "heading": -0.10918089274532473,
+                    "angularVelocity": -5.482996090104271e-8,
+                    "velocityX": 3.76008949154355,
+                    "velocityY": 0.41516984187713296,
+                    "timestamp": 1.1854722934170259
+                },
+                {
+                    "x": 5.24839709757225,
+                    "y": 6.543220677288062,
+                    "heading": -0.10918090874246278,
+                    "angularVelocity": -1.8527303358226695e-7,
+                    "velocityX": 3.782497851546312,
+                    "velocityY": 0.05787046498769136,
+                    "timestamp": 1.2718158790370384
+                },
+                {
+                    "x": 5.573089202193745,
+                    "y": 6.507668986743235,
+                    "heading": -0.10918094712419395,
+                    "angularVelocity": -4.4452324855688095e-7,
+                    "velocityX": 3.760465844566904,
+                    "velocityY": -0.4117467474802974,
+                    "timestamp": 1.358159464657051
+                },
+                {
+                    "x": 5.897772644305168,
+                    "y": 6.472038270745821,
+                    "heading": -0.10918098550925279,
+                    "angularVelocity": -4.4456178841484606e-7,
+                    "velocityX": 3.76036551852636,
+                    "velocityY": -0.4126619915255808,
+                    "timestamp": 1.4445030502770635
+                },
+                {
+                    "x": 6.218673539970231,
+                    "y": 6.411116178321686,
+                    "heading": -0.10918101517811797,
+                    "angularVelocity": -3.436140042414391e-7,
+                    "velocityX": 3.716557441537206,
+                    "velocityY": -0.7055775132184731,
+                    "timestamp": 1.530846635897076
+                },
+                {
+                    "x": 6.522221927429097,
+                    "y": 6.290504141989104,
+                    "heading": -0.10918101099769996,
+                    "angularVelocity": 4.8416080721557094e-8,
+                    "velocityX": 3.5155870037033834,
+                    "velocityY": -1.3968847305389978,
+                    "timestamp": 1.6171902215170886
+                },
+                {
+                    "x": 6.822696463094792,
+                    "y": 6.162426273800445,
+                    "heading": -0.10918100683974102,
+                    "angularVelocity": 4.8155967911031915e-8,
+                    "velocityX": 3.479986770390183,
+                    "velocityY": -1.4833512792984263,
+                    "timestamp": 1.7035338071371011
+                },
+                {
+                    "x": 7.1084642887127405,
+                    "y": 6.040617535278895,
+                    "heading": -0.0727866812513063,
+                    "angularVelocity": 0.42150584003543307,
+                    "velocityX": 3.309659004382518,
+                    "velocityY": -1.4107444999749623,
+                    "timestamp": 1.7898773927571137
+                },
+                {
+                    "x": 7.337078567452098,
+                    "y": 5.943170529564944,
+                    "heading": -0.043671737033755105,
+                    "angularVelocity": 0.33719869297161775,
+                    "velocityX": 2.6477274148129606,
+                    "velocityY": -1.1285957725082563,
+                    "timestamp": 1.8762209783771262
+                },
+                {
+                    "x": 7.508539282574383,
+                    "y": 5.870085268029055,
+                    "heading": -0.021835852662427283,
+                    "angularVelocity": 0.2528952696894573,
+                    "velocityX": 1.985795631384405,
+                    "velocityY": -0.8464469133529862,
+                    "timestamp": 1.9625645639971387
+                },
                 {
-                    "x": 7.603595242863294,
-                    "y": 5.830703404850431,
-                    "heading": -0.0021049639599730564,
-                    "angularVelocity": 0.04206684918455414,
-                    "velocityX": 1.5269157084883698,
-                    "velocityY": -0.673547795746317,
-                    "timestamp": 2.0015448357472474
+                    "x": 7.622846427592322,
+                    "y": 5.821361756830362,
+                    "heading": -0.007278657657073996,
+                    "angularVelocity": 0.16859613717477176,
+                    "velocityX": 1.323863772822564,
+                    "velocityY": -0.564297982864862,
+                    "timestamp": 2.0489081496171515
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
                     "heading": 0,
-                    "angularVelocity": 0.02103339303101045,
-                    "velocityX": 0.7634578628680123,
-                    "velocityY": -0.33677391831044023,
-                    "timestamp": 2.1016220775346097
+                    "angularVelocity": 0.08429876527373403,
+                    "velocityX": 0.6619318852381739,
+                    "velocityY": -0.282149005689615,
+                    "timestamp": 2.1352517352371643
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
                     "heading": 0,
                     "angularVelocity": 0,
-                    "velocityX": 1.7450911582816108e-33,
-                    "velocityY": 0,
-                    "timestamp": 2.201699319321972
+                    "velocityX": -1.8142028796058936e-31,
+                    "velocityY": -1.068352585526189e-31,
+                    "timestamp": 2.221595320857177
                 }
             ],
             "constraints": [
@@ -4115,7 +4682,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 22
+                    "controlIntervalCount": 10
+                },
+                {
+                    "x": 2.936701774597168,
+                    "y": 6.303859233856201,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 19
                 },
                 {
                     "x": 7.68,
@@ -4132,208 +4708,271 @@
                     "x": 1.35,
                     "y": 5.56,
                     "heading": 0,
-                    "angularVelocity": 2.6743025901632004e-33,
-                    "velocityX": 2.068786142168461e-33,
-                    "velocityY": 0,
+                    "angularVelocity": -7.013886661360986e-29,
+                    "velocityX": -3.078938571640024e-30,
+                    "velocityY": -8.674299238088502e-31,
                     "timestamp": 0
                 },
                 {
-                    "x": 1.4007220754094205,
-                    "y": 5.545584664540576,
-                    "heading": -0.37232774466736046,
-                    "angularVelocity": -3.437582019103784,
-                    "velocityX": 0.4683005682394807,
-                    "velocityY": -0.13309214444637307,
-                    "timestamp": 0.10831094141120465
-                },
-                {
-                    "x": 1.5327694511201624,
-                    "y": 5.533363500300387,
-                    "heading": -0.9362708794209632,
-                    "angularVelocity": -5.206705134364786,
-                    "velocityX": 1.2191508446909478,
-                    "velocityY": -0.11283406903270367,
-                    "timestamp": 0.2166218828224093
-                },
-                {
-                    "x": 1.760106911270163,
-                    "y": 5.516367699163042,
-                    "heading": -1.4218044496098983,
-                    "angularVelocity": -4.482774905866594,
-                    "velocityX": 2.0989334705060814,
-                    "velocityY": -0.1569167520465022,
-                    "timestamp": 0.3249328242336139
-                },
-                {
-                    "x": 2.067387230758427,
-                    "y": 5.467280597023701,
-                    "heading": -1.7292049865361503,
-                    "angularVelocity": -2.838130044121764,
-                    "velocityX": 2.83702011527781,
-                    "velocityY": -0.453205387191498,
-                    "timestamp": 0.4332437656448186
-                },
-                {
-                    "x": 2.4488593918735733,
-                    "y": 5.393167697716653,
-                    "heading": -1.7983473850598053,
-                    "angularVelocity": -0.6383694723984937,
-                    "velocityX": 3.5220094677866336,
-                    "velocityY": -0.6842605035226905,
-                    "timestamp": 0.5415547070560233
-                },
-                {
-                    "x": 2.8531804118778052,
-                    "y": 5.326790341993422,
-                    "heading": -1.7983490370676205,
-                    "angularVelocity": -0.000015252455509683049,
-                    "velocityX": 3.7329656148885193,
-                    "velocityY": -0.6128407237384188,
-                    "timestamp": 0.6498656484672279
-                },
-                {
-                    "x": 3.257501404598323,
-                    "y": 5.2604128200678275,
-                    "heading": -1.7983506890680847,
-                    "angularVelocity": -0.00001525238764202395,
-                    "velocityX": 3.732965362986781,
-                    "velocityY": -0.6128422582312392,
-                    "timestamp": 0.7581765898784326
-                },
-                {
-                    "x": 3.6466232677858144,
-                    "y": 5.172310745117625,
-                    "heading": -1.8337945854134767,
-                    "angularVelocity": -0.32724206699329256,
-                    "velocityX": 3.5926367005728674,
-                    "velocityY": -0.8134180517896293,
-                    "timestamp": 0.8664875312896373
-                },
-                {
-                    "x": 4.014238623942389,
-                    "y": 5.062846847355338,
-                    "heading": -1.917513965365452,
-                    "angularVelocity": -0.7729540419571551,
-                    "velocityX": 3.394074055370967,
-                    "velocityY": -1.0106448742487064,
-                    "timestamp": 0.974798472700842
-                },
-                {
-                    "x": 4.354761888880782,
-                    "y": 4.880183720994759,
-                    "heading": -1.9914398534549198,
-                    "angularVelocity": -0.6825338892476871,
-                    "velocityX": 3.143941512294591,
-                    "velocityY": -1.6864697507068689,
-                    "timestamp": 1.0831094141120465
-                },
-                {
-                    "x": 4.661144802240214,
-                    "y": 4.608133464126214,
-                    "heading": -1.9914398486412812,
-                    "angularVelocity": 4.444277523534959e-8,
-                    "velocityX": 2.8287346538355926,
-                    "velocityY": -2.5117523061285136,
-                    "timestamp": 1.1914203555232512
-                },
-                {
-                    "x": 5.019380877625839,
-                    "y": 4.418922250546549,
-                    "heading": -1.9767149939198712,
-                    "angularVelocity": 0.13594983599585594,
-                    "velocityX": 3.3074781801182462,
-                    "velocityY": -1.7469261287400477,
-                    "timestamp": 1.2997312969344559
-                },
-                {
-                    "x": 5.376950498025801,
-                    "y": 4.3069722509438675,
-                    "heading": -1.8614424797513431,
-                    "angularVelocity": 1.0642739566900585,
-                    "velocityX": 3.3013250161167162,
-                    "velocityY": -1.0335982509621156,
-                    "timestamp": 1.4080422383456606
-                },
-                {
-                    "x": 5.749629435463048,
-                    "y": 4.240661073830716,
-                    "heading": -1.7582912287295662,
-                    "angularVelocity": 0.9523622422425545,
-                    "velocityX": 3.4408244687151703,
-                    "velocityY": -0.6122297179691145,
-                    "timestamp": 1.5163531797568652
-                },
-                {
-                    "x": 6.158173088993434,
-                    "y": 4.209455444526307,
-                    "heading": -1.7582906674190417,
-                    "angularVelocity": 0.000005182399092709644,
-                    "velocityX": 3.771951828756997,
-                    "velocityY": -0.2881115139230142,
-                    "timestamp": 1.62466412116807
-                },
-                {
-                    "x": 6.566716775041419,
-                    "y": 4.178250240929697,
-                    "heading": -1.7582901061123783,
-                    "angularVelocity": 0.000005182363444203767,
-                    "velocityX": 3.771952128981517,
-                    "velocityY": -0.2881075835001592,
-                    "timestamp": 1.7329750625792746
-                },
-                {
-                    "x": 6.948239527399369,
-                    "y": 4.140315578246664,
-                    "heading": -1.6699172621402418,
-                    "angularVelocity": 0.8159179748667066,
-                    "velocityX": 3.5224765604196056,
-                    "velocityY": -0.35023850950582514,
-                    "timestamp": 1.8412860039904793
-                },
-                {
-                    "x": 7.2588942750640015,
-                    "y": 4.14084991195257,
-                    "heading": -1.390296709754989,
-                    "angularVelocity": 2.5816464037891405,
-                    "velocityX": 2.86817512263351,
-                    "velocityY": 0.004933330824608595,
-                    "timestamp": 1.949596945401684
-                },
-                {
-                    "x": 7.4846066646178775,
-                    "y": 4.133171401549216,
-                    "heading": -0.9384850139510863,
-                    "angularVelocity": 4.17143171241205,
-                    "velocityX": 2.083929717653862,
-                    "velocityY": -0.07089321081793584,
-                    "timestamp": 2.0579078868128886
-                },
+                    "x": 1.3762618085283742,
+                    "y": 5.591108793803365,
+                    "heading": -0.012389964804724785,
+                    "angularVelocity": -0.17699325527244014,
+                    "velocityX": 0.3751554628363854,
+                    "velocityY": 0.44439566776177375,
+                    "timestamp": 0.07000246865606696
+                },
+                {
+                    "x": 1.4301971031295795,
+                    "y": 5.652094846604856,
+                    "heading": -0.03662832018577037,
+                    "angularVelocity": -0.3462500086931304,
+                    "velocityX": 0.7704770365480613,
+                    "velocityY": 0.871198601597385,
+                    "timestamp": 0.14000493731213393
+                },
+                {
+                    "x": 1.513800165351683,
+                    "y": 5.741006259883096,
+                    "heading": -0.07186447572819399,
+                    "angularVelocity": -0.5033558989907949,
+                    "velocityX": 1.1942873419625006,
+                    "velocityY": 1.2701182541879128,
+                    "timestamp": 0.2100074059682009
+                },
+                {
+                    "x": 1.6300142433743692,
+                    "y": 5.854390700910866,
+                    "heading": -0.1166088429554257,
+                    "angularVelocity": -0.6391827043487479,
+                    "velocityX": 1.660142567166188,
+                    "velocityY": 1.6197206070673247,
+                    "timestamp": 0.28000987462426785
+                },
+                {
+                    "x": 1.7831654317988928,
+                    "y": 5.985149557302319,
+                    "heading": -0.16783055719619017,
+                    "angularVelocity": -0.7317129698935645,
+                    "velocityX": 2.187796964375065,
+                    "velocityY": 1.8679177877841964,
+                    "timestamp": 0.3500123432803348
+                },
+                {
+                    "x": 1.9771685219359858,
+                    "y": 6.116694524674393,
+                    "heading": -0.21853613173426145,
+                    "angularVelocity": -0.7243398055979559,
+                    "velocityX": 2.771374979505968,
+                    "velocityY": 1.8791475486148668,
+                    "timestamp": 0.4200148119364018
+                },
+                {
+                    "x": 2.202537147154592,
+                    "y": 6.2222527712605,
+                    "heading": -0.25772100339188303,
+                    "angularVelocity": -0.5597641398939388,
+                    "velocityX": 3.2194382504688988,
+                    "velocityY": 1.507921772083263,
+                    "timestamp": 0.49001728059246874
+                },
+                {
+                    "x": 2.4433286317618386,
+                    "y": 6.289980043332844,
+                    "heading": -0.29446027058227175,
+                    "angularVelocity": -0.5248281652898821,
+                    "velocityX": 3.4397570432873823,
+                    "velocityY": 0.9674983378806481,
+                    "timestamp": 0.5600197492485357
+                },
+                {
+                    "x": 2.6903031212328568,
+                    "y": 6.317347154244271,
+                    "heading": -0.33666392758398794,
+                    "angularVelocity": -0.6028881239905549,
+                    "velocityX": 3.528082569267318,
+                    "velocityY": 0.39094494004013314,
+                    "timestamp": 0.6300222179046027
+                },
+                {
+                    "x": 2.936701774597168,
+                    "y": 6.303859233856201,
+                    "heading": -0.37968716420906834,
+                    "angularVelocity": -0.6145959914234042,
+                    "velocityX": 3.5198566292706706,
+                    "velocityY": -0.1926777818987785,
+                    "timestamp": 0.7000246865606696
+                },
+                {
+                    "x": 3.257320421397908,
+                    "y": 6.21346874174157,
+                    "heading": -0.4320815750431537,
+                    "angularVelocity": -0.5604086145235933,
+                    "velocityX": 3.429324784540344,
+                    "velocityY": -0.9668132467920023,
+                    "timestamp": 0.7935179153869726
+                },
+                {
+                    "x": 3.5608980813516515,
+                    "y": 6.0524118876009405,
+                    "heading": -0.46560329407698275,
+                    "angularVelocity": -0.35854702479102396,
+                    "velocityX": 3.247055040935253,
+                    "velocityY": -1.7226579524796546,
+                    "timestamp": 0.8870111442132755
+                },
+                {
+                    "x": 3.8328887233978395,
+                    "y": 5.82633673900318,
+                    "heading": -0.46560402675062595,
+                    "angularVelocity": -0.00000783664977303189,
+                    "velocityX": 2.909201505399863,
+                    "velocityY": -2.418091143453467,
+                    "timestamp": 0.9805043730395785
+                },
+                {
+                    "x": 4.052764522028411,
+                    "y": 5.549310224236533,
+                    "heading": -0.46560404923284493,
+                    "angularVelocity": -2.404689503787649e-7,
+                    "velocityX": 2.3517831332905046,
+                    "velocityY": -2.963065007427709,
+                    "timestamp": 1.0739976018658814
+                },
+                {
+                    "x": 4.211188193978367,
+                    "y": 5.233096683639544,
+                    "heading": -0.46560406449531083,
+                    "angularVelocity": -1.6324674914477828e-7,
+                    "velocityX": 1.6944935364707439,
+                    "velocityY": -3.382207937052538,
+                    "timestamp": 1.1674908306921843
+                },
+                {
+                    "x": 4.383758324037826,
+                    "y": 4.9243757535417085,
+                    "heading": -0.4656040804041062,
+                    "angularVelocity": -1.701598650622723e-7,
+                    "velocityX": 1.8458035113972522,
+                    "velocityY": -3.302067261698653,
+                    "timestamp": 1.2609840595184871
+                },
+                {
+                    "x": 4.615944430524366,
+                    "y": 4.657582435311582,
+                    "heading": -0.46560407064369363,
+                    "angularVelocity": 1.0439700291593356e-7,
+                    "velocityX": 2.4834537153263514,
+                    "velocityY": -2.8536111286282813,
+                    "timestamp": 1.35447728834479
+                },
+                {
+                    "x": 4.89788653877274,
+                    "y": 4.444046330431443,
+                    "heading": -0.46560381887866387,
+                    "angularVelocity": 0.0000026928691303704606,
+                    "velocityX": 3.015642007317779,
+                    "velocityY": -2.2839740113892115,
+                    "timestamp": 1.4479705171710928
+                },
+                {
+                    "x": 5.217611687922929,
+                    "y": 4.2928350259750445,
+                    "heading": -0.4656035898817651,
+                    "angularVelocity": 0.000002449342071651111,
+                    "velocityX": 3.4197679678406536,
+                    "velocityY": -1.6173503295872917,
+                    "timestamp": 1.5414637459973957
+                },
+                {
+                    "x": 5.553860912374937,
+                    "y": 4.183174208089645,
+                    "heading": -0.46560339968651515,
+                    "angularVelocity": 0.0000020343211207896953,
+                    "velocityX": 3.5965088453273077,
+                    "velocityY": -1.1729279142677982,
+                    "timestamp": 1.6349569748236985
+                },
+                {
+                    "x": 5.905448307826856,
+                    "y": 4.144763712168745,
+                    "heading": -0.4656033544319255,
+                    "angularVelocity": 4.840413598760029e-7,
+                    "velocityX": 3.7605653357540834,
+                    "velocityY": -0.4108371954108264,
+                    "timestamp": 1.7284502036500013
+                },
+                {
+                    "x": 6.2591141169961295,
+                    "y": 4.141672024564684,
+                    "heading": -0.4656033129772286,
+                    "angularVelocity": 4.433978530009588e-7,
+                    "velocityX": 3.7827959693886872,
+                    "velocityY": -0.0330685723754928,
+                    "timestamp": 1.8219434324763042
+                },
+                {
+                    "x": 6.612779928543081,
+                    "y": 4.138580608962963,
+                    "heading": -0.465603271522502,
+                    "angularVelocity": 4.433981701164172e-7,
+                    "velocityX": 3.7827959948202516,
+                    "velocityY": -0.03306566304887272,
+                    "timestamp": 1.915436661302607
+                },
+                {
+                    "x": 6.963404638780936,
+                    "y": 4.132792400258934,
+                    "heading": -0.45779034574220756,
+                    "angularVelocity": 0.08356675535090403,
+                    "velocityX": 3.7502684915210915,
+                    "velocityY": -0.061910458935820756,
+                    "timestamp": 2.00892989012891
+                },
+                {
+                    "x": 7.251193869015599,
+                    "y": 4.13050050188738,
+                    "heading": -0.30806732380169355,
+                    "angularVelocity": 1.6014317167147898,
+                    "velocityX": 3.0781826004676223,
+                    "velocityY": -0.024514057331484977,
+                    "timestamp": 2.1024231189552127
+                },
+                {
+                    "x": 7.466125497714746,
+                    "y": 4.12875254378885,
+                    "heading": -0.16499798934349721,
+                    "angularVelocity": 1.5302641298654869,
+                    "velocityX": 2.29890048078732,
+                    "velocityY": -0.018696092973500365,
+                    "timestamp": 2.1959163477815156
+                },
                 {
-                    "x": 7.620450068903259,
-                    "y": 4.128683952785512,
-                    "heading": -0.34333254233125515,
-                    "angularVelocity": 5.494850879010672,
-                    "velocityX": 1.2541983525897882,
-                    "velocityY": -0.04143116757398245,
-                    "timestamp": 2.166218828224093
+                    "x": 7.608856917870355,
+                    "y": 4.12758509655783,
+                    "heading": -0.05770853615881093,
+                    "angularVelocity": 1.147563888118738,
+                    "velocityX": 1.5266498114080966,
+                    "velocityY": -0.012486970935497041,
+                    "timestamp": 2.2894095766078184
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 9.09513036114211e-34,
-                    "angularVelocity": 3.1698786646843584,
-                    "velocityX": 0.5498053134877472,
-                    "velocityY": -0.015547393121808115,
-                    "timestamp": 2.2745297696352975
+                    "heading": -1.6446149791629635e-35,
+                    "angularVelocity": 0.6172482957672322,
+                    "velocityX": 0.7609436856846457,
+                    "velocityY": -0.0062581704063020965,
+                    "timestamp": 2.3829028054341213
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
                     "heading": 0,
-                    "angularVelocity": -7.032634787663221e-33,
+                    "angularVelocity": -1.0953722106158818e-30,
                     "velocityX": 0,
-                    "velocityY": -1.950631326224301e-32,
-                    "timestamp": 2.382840711046502
+                    "velocityY": -1.282624680344039e-29,
+                    "timestamp": 2.476396034260424
                 }
             ],
             "constraints": [
@@ -4377,6 +5016,15 @@
                     "headingConstrained": true,
                     "controlIntervalCount": 40
                 },
+                {
+                    "x": 2.956629753112793,
+                    "y": 6.283931255340576,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 40
+                },
                 {
                     "x": 4.018431663513184,
                     "y": 5.088562488555908,
@@ -4409,1091 +5057,1451 @@
                 {
                     "x": 1.35,
                     "y": 5.56,
-                    "heading": -1.9296664556128306e-33,
+                    "heading": 0,
                     "angularVelocity": 0,
-                    "velocityX": -7.813211441980168e-34,
-                    "velocityY": -2.2407271587906947e-32,
+                    "velocityX": 0,
+                    "velocityY": 3.439885530040909e-30,
                     "timestamp": 0
                 },
                 {
-                    "x": 1.35439418502164,
-                    "y": 5.559271848784049,
-                    "heading": -0.0037613483747314606,
-                    "angularVelocity": -0.16062668985124567,
-                    "velocityX": 0.18765169410035007,
-                    "velocityY": -0.031095370031394967,
-                    "timestamp": 0.023416708507251784
-                },
-                {
-                    "x": 1.3631871179840578,
-                    "y": 5.557813607446842,
-                    "heading": -0.011205273313316288,
-                    "angularVelocity": -0.317889464963847,
-                    "velocityX": 0.37549824560930783,
-                    "velocityY": -0.06227354014150002,
-                    "timestamp": 0.04683341701450357
-                },
-                {
-                    "x": 1.3763837173940232,
-                    "y": 5.5556231238892515,
-                    "heading": -0.02224504855076525,
-                    "angularVelocity": -0.47144863395424863,
-                    "velocityX": 0.563554839736692,
-                    "velocityY": -0.09354361467639355,
-                    "timestamp": 0.07025012552175536
-                },
-                {
-                    "x": 1.3939893513502628,
-                    "y": 5.552698044632534,
-                    "heading": -0.03678367986427699,
-                    "angularVelocity": -0.6208657083043512,
-                    "velocityX": 0.7518406761047259,
-                    "velocityY": -0.12491419346204678,
-                    "timestamp": 0.09366683402900713
-                },
-                {
-                    "x": 1.4160099408355828,
-                    "y": 5.549035829570074,
-                    "heading": -0.0547111148835232,
-                    "angularVelocity": -0.7655830456997818,
-                    "velocityX": 0.9403793653792271,
-                    "velocityY": -0.15639324635770782,
-                    "timestamp": 0.11708354253625891
-                },
-                {
-                    "x": 1.4424520698548373,
-                    "y": 5.544633771060564,
-                    "heading": -0.07590088077413953,
-                    "angularVelocity": -0.9048994176125892,
-                    "velocityX": 1.129199221618411,
-                    "velocityY": -0.18798792785714832,
-                    "timestamp": 0.1405002510435107
-                },
-                {
-                    "x": 1.4733230980825243,
-                    "y": 5.539489016814175,
-                    "heading": -0.10020600379509263,
-                    "angularVelocity": -1.0379393420482537,
-                    "velocityX": 1.3183333694454236,
-                    "velocityY": -0.2197044151102235,
-                    "timestamp": 0.16391695955076246
-                },
-                {
-                    "x": 1.5086312686123933,
-                    "y": 5.533598592872994,
-                    "heading": -0.12745397379473564,
-                    "angularVelocity": -1.163612298082128,
-                    "velocityX": 1.5078195348818708,
-                    "velocityY": -0.25154790389774145,
-                    "timestamp": 0.18733366805801424
-                },
-                {
-                    "x": 1.5483857981367395,
-                    "y": 5.526959418437362,
-                    "heading": -0.15744034724886685,
-                    "angularVelocity": -1.2805545854082991,
-                    "velocityX": 1.6976992950155392,
-                    "velocityY": -0.28352295684851286,
-                    "timestamp": 0.21075037656526602
-                },
-                {
-                    "x": 1.5925969268012463,
-                    "y": 5.5195682985391485,
-                    "heading": -0.18992026053497643,
-                    "angularVelocity": -1.3870400819163256,
-                    "velocityX": 1.888016355962865,
-                    "velocityY": -0.31563444947560276,
-                    "timestamp": 0.2341670850725178
-                },
-                {
-                    "x": 1.6412758830317984,
-                    "y": 5.511421875068327,
-                    "heading": -0.22459650029504458,
-                    "angularVelocity": -1.4808332157070316,
-                    "velocityX": 2.0788129217851563,
-                    "velocityY": -0.34788934868040844,
-                    "timestamp": 0.25758379357976957
-                },
-                {
-                    "x": 1.6944346649736404,
-                    "y": 5.502516516620167,
-                    "heading": -0.26110152083126115,
-                    "angularVelocity": -1.5589304758570732,
-                    "velocityX": 2.2701218630013136,
-                    "velocityY": -0.38029932539011047,
-                    "timestamp": 0.2810005020870214
-                },
-                {
-                    "x": 1.7520853949904163,
-                    "y": 5.492848149517658,
-                    "heading": -0.298968117565775,
-                    "angularVelocity": -1.6170759747377448,
-                    "velocityX": 2.4619484843022064,
-                    "velocityY": -0.4128832666435571,
-                    "timestamp": 0.3044172105942732
-                },
-                {
-                    "x": 1.8142385663960479,
-                    "y": 5.482412126535505,
-                    "heading": -0.337577206769387,
-                    "angularVelocity": -1.6487837815317001,
-                    "velocityX": 2.654223217852448,
-                    "velocityY": -0.44566566556215986,
-                    "timestamp": 0.327833919101525
-                },
-                {
-                    "x": 1.8808979849725118,
-                    "y": 5.471203531590798,
-                    "heading": -0.3760544549024592,
-                    "angularVelocity": -1.6431535679387552,
-                    "velocityX": 2.8466604756095246,
-                    "velocityY": -0.47865800358925525,
-                    "timestamp": 0.3512506276087768
-                },
-                {
-                    "x": 1.9520436894250033,
-                    "y": 5.459218834920382,
-                    "heading": -0.41303307424029884,
-                    "angularVelocity": -1.5791552995754587,
-                    "velocityX": 3.0382452952539354,
-                    "velocityY": -0.5118010785637118,
-                    "timestamp": 0.3746673361160286
-                },
-                {
-                    "x": 2.027554046757936,
-                    "y": 5.446452385802885,
-                    "heading": -0.44595627669268334,
-                    "angularVelocity": -1.4059705462956014,
-                    "velocityX": 3.2246358325529645,
-                    "velocityY": -0.5451854650516427,
-                    "timestamp": 0.3980840446232804
-                },
-                {
-                    "x": 2.106704107988529,
-                    "y": 5.4327632732947455,
-                    "heading": -0.46854805770485686,
-                    "angularVelocity": -0.9647718425148408,
-                    "velocityX": 3.3800677497471567,
-                    "velocityY": -0.5845873899786794,
-                    "timestamp": 0.4215007531305322
-                },
-                {
-                    "x": 2.1895060473348913,
-                    "y": 5.418170740153337,
-                    "heading": -0.4808901513114338,
-                    "angularVelocity": -0.5270635539044761,
-                    "velocityX": 3.5360195614476866,
-                    "velocityY": -0.6231675616108724,
-                    "timestamp": 0.444917461637784
-                },
-                {
-                    "x": 2.275961949822614,
-                    "y": 5.402702256569038,
-                    "heading": -0.4829709400280361,
-                    "angularVelocity": -0.0888591458512548,
-                    "velocityX": 3.6920604132278863,
-                    "velocityY": -0.6605746311236733,
-                    "timestamp": 0.4683341701450358
-                },
-                {
-                    "x": 2.3633379318433296,
-                    "y": 5.388122823546896,
-                    "heading": -0.48297098226531077,
-                    "angularVelocity": -0.0000018037238101032338,
-                    "velocityX": 3.7313519956767345,
-                    "velocityY": -0.6226081269122786,
-                    "timestamp": 0.4917508786522876
-                },
-                {
-                    "x": 2.4507217300981603,
-                    "y": 5.373590311340196,
-                    "heading": -0.48297102439627604,
-                    "angularVelocity": -0.0000017991839149984119,
-                    "velocityX": 3.7316857844376363,
-                    "velocityY": -0.6206043946012527,
-                    "timestamp": 0.5151675871595394
-                },
-                {
-                    "x": 2.5381084152434026,
-                    "y": 5.359075168632344,
-                    "heading": -0.4829710665257657,
-                    "angularVelocity": -0.0000017991208983915868,
-                    "velocityX": 3.7318090677936033,
-                    "velocityY": -0.6198626379688947,
-                    "timestamp": 0.5385842956667911
-                },
-                {
-                    "x": 2.6254950830921073,
-                    "y": 5.344559921791865,
-                    "heading": -0.482971108655264,
-                    "angularVelocity": -0.0000017991212673817243,
-                    "velocityX": 3.7318083291527238,
-                    "velocityY": -0.6198670849061145,
-                    "timestamp": 0.5620010041740429
-                },
-                {
-                    "x": 2.712881673840345,
-                    "y": 5.330044210788612,
-                    "heading": -0.4829711507848006,
-                    "angularVelocity": -0.0000017991229009417428,
-                    "velocityX": 3.731805036611997,
-                    "velocityY": -0.6198869067684731,
-                    "timestamp": 0.5854177126812946
-                },
-                {
-                    "x": 2.800268245782709,
-                    "y": 5.315528386572539,
-                    "heading": -0.4829711929143463,
-                    "angularVelocity": -0.0000017991232935293638,
-                    "velocityX": 3.7318042335155903,
-                    "velocityY": -0.6198917414709255,
-                    "timestamp": 0.6088344211885464
-                },
-                {
-                    "x": 2.8876548130761686,
-                    "y": 5.301012534370082,
-                    "heading": -0.4829712350438941,
-                    "angularVelocity": -0.0000017991233825733017,
-                    "velocityX": 3.731804034986268,
-                    "velocityY": -0.6198929366167053,
-                    "timestamp": 0.6322511296957981
-                },
-                {
-                    "x": 2.975041345179212,
-                    "y": 5.286496470320027,
-                    "heading": -0.48297127717345933,
-                    "angularVelocity": -0.000001799124124149535,
-                    "velocityX": 3.7318025321954105,
-                    "velocityY": -0.6199019834729332,
-                    "timestamp": 0.6556678382030499
-                },
-                {
-                    "x": 3.0624277772479402,
-                    "y": 5.271979804077107,
-                    "heading": -0.4829713193030743,
-                    "angularVelocity": -0.0000017991262516148556,
-                    "velocityX": 3.7317982602749975,
-                    "velocityY": -0.6199276998481766,
-                    "timestamp": 0.6790845467103016
-                },
-                {
-                    "x": 3.149814115388955,
-                    "y": 5.257462572426413,
-                    "heading": -0.482971361432736,
-                    "angularVelocity": -0.0000017991282469783394,
-                    "velocityX": 3.731794249134225,
-                    "velocityY": -0.619951845332987,
-                    "timestamp": 0.7025012552175534
-                },
-                {
-                    "x": 3.2372009926614522,
-                    "y": 5.242948586436876,
-                    "heading": -0.48297140356212753,
-                    "angularVelocity": -0.0000017991167072996656,
-                    "velocityX": 3.7318172725014778,
-                    "velocityY": -0.6198132408343549,
-                    "timestamp": 0.7259179637248051
-                },
-                {
-                    "x": 3.3245908005140246,
-                    "y": 5.228452256133779,
-                    "heading": -0.4829714456900571,
-                    "angularVelocity": -0.0000017990542795775989,
-                    "velocityX": 3.7319424216049217,
-                    "velocityY": -0.6190592626894866,
-                    "timestamp": 0.7493346722320569
-                },
-                {
-                    "x": 3.4119833565424647,
-                    "y": 5.213972502683899,
-                    "heading": -0.48297148781659605,
-                    "angularVelocity": -0.0000017989948869691892,
-                    "velocityX": 3.7320597812190472,
-                    "velocityY": -0.6183513556300411,
-                    "timestamp": 0.7727513807393086
-                },
-                {
-                    "x": 3.4993677183765524,
-                    "y": 5.199443379550061,
-                    "heading": -0.48297152994719206,
-                    "angularVelocity": -0.0000017991681461203675,
-                    "velocityX": 3.731709851836638,
-                    "velocityY": -0.6204596657670539,
-                    "timestamp": 0.7961680892465604
-                },
-                {
-                    "x": 3.5867330233447126,
-                    "y": 5.184800101210533,
-                    "heading": -0.48297157208742414,
-                    "angularVelocity": -0.0000017995796480013664,
-                    "velocityX": 3.7308960369522555,
-                    "velocityY": -0.6253346124593939,
-                    "timestamp": 0.8195847977538121
-                },
-                {
-                    "x": 3.6740864894013328,
-                    "y": 5.170086363489935,
-                    "heading": -0.48297161423379187,
-                    "angularVelocity": -0.0000017998416699478457,
-                    "velocityX": 3.7303904615618975,
-                    "velocityY": -0.6283435486246598,
-                    "timestamp": 0.8430015062610638
-                },
-                {
-                    "x": 3.7614250146254347,
-                    "y": 5.155284197469098,
-                    "heading": -0.482971656398251,
-                    "angularVelocity": -0.0000018006142553635733,
-                    "velocityX": 3.729752420031712,
-                    "velocityY": -0.6321198393988197,
-                    "timestamp": 0.8664182147683156
-                },
-                {
-                    "x": 3.848175799548237,
-                    "y": 5.137355912505346,
-                    "heading": -0.48297169899264486,
-                    "angularVelocity": -0.000001818974422860047,
-                    "velocityX": 3.704653234929829,
-                    "velocityY": -0.7656193421980864,
-                    "timestamp": 0.8898349232755673
-                },
-                {
-                    "x": 3.9339293985898425,
-                    "y": 5.115142351916783,
-                    "heading": -0.48297174253890224,
-                    "angularVelocity": -0.0000018596233270255789,
-                    "velocityX": 3.6620688605764276,
-                    "velocityY": -0.9486201095127581,
-                    "timestamp": 0.9132516317828191
+                    "x": 1.3518715996750759,
+                    "y": 5.562488632849977,
+                    "heading": -0.00039025483625088287,
+                    "angularVelocity": -0.020188245249371933,
+                    "velocityX": 0.09681959001984766,
+                    "velocityY": 0.12873928941829918,
+                    "timestamp": 0.019330795293514086
+                },
+                {
+                    "x": 1.3556369831983786,
+                    "y": 5.567448998179342,
+                    "heading": -0.001175615506548702,
+                    "angularVelocity": -0.04062743712160119,
+                    "velocityX": 0.19478678792724713,
+                    "velocityY": 0.256604306964468,
+                    "timestamp": 0.03866159058702817
+                },
+                {
+                    "x": 1.3613203761105706,
+                    "y": 5.574862277415471,
+                    "heading": -0.0023613813379046128,
+                    "angularVelocity": -0.061340768103509684,
+                    "velocityX": 0.2940071955600798,
+                    "velocityY": 0.38349582226532697,
+                    "timestamp": 0.05799238588054226
+                },
+                {
+                    "x": 1.3689483300707626,
+                    "y": 5.58470739953387,
+                    "heading": -0.003953362765501964,
+                    "angularVelocity": -0.08235467829569511,
+                    "velocityX": 0.39460114518679135,
+                    "velocityY": 0.5092973138928011,
+                    "timestamp": 0.07732318117405634
+                },
+                {
+                    "x": 1.3785500603498269,
+                    "y": 5.596960627127346,
+                    "heading": -0.0059579558445008625,
+                    "angularVelocity": -0.10369946236363117,
+                    "velocityX": 0.4967064279184545,
+                    "velocityY": 0.6338708473927627,
+                    "timestamp": 0.09665397646757043
+                },
+                {
+                    "x": 1.3901578478597558,
+                    "y": 5.611595038412612,
+                    "heading": -0.008382231168191269,
+                    "angularVelocity": -0.12541001479146147,
+                    "velocityX": 0.6004816322183893,
+                    "velocityY": 0.7570516920313731,
+                    "timestamp": 0.11598477176108452
+                },
+                {
+                    "x": 1.4038075203372553,
+                    "y": 5.62857987161075,
+                    "heading": -0.01123404053815137,
+                    "angularVelocity": -0.14752674820972528,
+                    "velocityX": 0.7061102386242472,
+                    "velocityY": 0.8786412012669059,
+                    "timestamp": 0.1353155670545986
+                },
+                {
+                    "x": 1.4195390306280726,
+                    "y": 5.647879684835849,
+                    "heading": -0.014522145551621221,
+                    "angularVelocity": -0.17009672719326588,
+                    "velocityX": 0.8138056428591633,
+                    "velocityY": 0.9983972688167116,
+                    "timestamp": 0.1546463623481127
+                },
+                {
+                    "x": 1.4373971536587393,
+                    "y": 5.669453265066441,
+                    "heading": -0.018256373225668265,
+                    "angularVelocity": -0.19317506689959266,
+                    "velocityX": 0.923817295642181,
+                    "velocityY": 1.116021348476537,
+                    "timestamp": 0.17397715764162677
+                },
+                {
+                    "x": 1.457432326992564,
+                    "y": 5.693252190583062,
+                    "heading": -0.022447804782031403,
+                    "angularVelocity": -0.21682664850159125,
+                    "velocityX": 1.0364381304346626,
+                    "velocityY": 1.2311405276019616,
+                    "timestamp": 0.19330795293514086
+                },
+                {
+                    "x": 1.47970166115617,
+                    "y": 5.719218907072145,
+                    "heading": -0.027109004507978946,
+                    "angularVelocity": -0.24112819235695526,
+                    "velocityX": 1.1520133458284385,
+                    "velocityY": 1.3432823686149544,
+                    "timestamp": 0.21263874822865494
+                },
+                {
+                    "x": 1.5042701411060604,
+                    "y": 5.747284109919373,
+                    "heading": -0.03225429550223853,
+                    "angularVelocity": -0.2661706834165162,
+                    "velocityX": 1.2709502933970824,
+                    "velocityY": 1.4518390175412745,
+                    "timestamp": 0.23196954352216903
+                },
+                {
+                    "x": 1.5312120197646597,
+                    "y": 5.777363120811461,
+                    "heading": -0.037900086436595266,
+                    "angularVelocity": -0.2920620103121596,
+                    "velocityX": 1.3937284136281272,
+                    "velocityY": 1.5560151786502046,
+                    "timestamp": 0.25130033881568314
+                },
+                {
+                    "x": 1.5606123466917503,
+                    "y": 5.809350786193417,
+                    "heading": -0.044065244214455006,
+                    "angularVelocity": -0.3189293396494718,
+                    "velocityX": 1.5209062266028461,
+                    "velocityY": 1.6547516486654796,
+                    "timestamp": 0.27063113410919726
+                },
+                {
+                    "x": 1.5925684302904153,
+                    "y": 5.843114184319679,
+                    "heading": -0.050771482089097104,
+                    "angularVelocity": -0.34691991575183656,
+                    "velocityX": 1.653117893674399,
+                    "velocityY": 1.7466119532904336,
+                    "timestamp": 0.28996192940271137
+                },
+                {
+                    "x": 1.6271906907680806,
+                    "y": 5.8784820940308995,
+                    "heading": -0.058043668844819325,
+                    "angularVelocity": -0.37619697717051565,
+                    "velocityX": 1.791041700663091,
+                    "velocityY": 1.8296148282676763,
+                    "timestamp": 0.3092927246962255
+                },
+                {
+                    "x": 1.6646015916598045,
+                    "y": 5.915229821006168,
+                    "heading": -0.06590980894370155,
+                    "angularVelocity": -0.4069227354304089,
+                    "velocityX": 1.935300660096276,
+                    "velocityY": 1.9009940572697726,
+                    "timestamp": 0.3286235199897396
+                },
+                {
+                    "x": 1.7049297157110095,
+                    "y": 5.953057987948439,
+                    "heading": -0.07440007857488001,
+                    "angularVelocity": -0.43920953598980744,
+                    "velocityX": 2.086211324411271,
+                    "velocityY": 1.9568862205562443,
+                    "timestamp": 0.3479543152832537
+                },
+                {
+                    "x": 1.7482931222517035,
+                    "y": 5.991565656180575,
+                    "heading": -0.08354352935308812,
+                    "angularVelocity": -0.47299920357005454,
+                    "velocityX": 2.2432293075517364,
+                    "velocityY": 1.9920374535783947,
+                    "timestamp": 0.3672851105767678
+                },
+                {
+                    "x": 1.7947629094651933,
+                    "y": 6.030225122456552,
+                    "heading": -0.09335994332155541,
+                    "angularVelocity": -0.5078122146252688,
+                    "velocityX": 2.4039252657691588,
+                    "velocityY": 1.9998901074156783,
+                    "timestamp": 0.38661590587028194
+                },
+                {
+                    "x": 1.844301928458138,
+                    "y": 6.068381679285448,
+                    "heading": -0.10384499839936924,
+                    "angularVelocity": -0.5424016404194022,
+                    "velocityX": 2.562699477220483,
+                    "velocityY": 1.9738741344852324,
+                    "timestamp": 0.40594670116379605
+                },
+                {
+                    "x": 1.8967008920878712,
+                    "y": 6.105314232951604,
+                    "heading": -0.1149551002367539,
+                    "angularVelocity": -0.5747358899978738,
+                    "velocityX": 2.710647070341413,
+                    "velocityY": 1.9105553137044158,
+                    "timestamp": 0.42527749645731017
+                },
+                {
+                    "x": 1.951571944971935,
+                    "y": 6.140356350737874,
+                    "heading": -0.126607814812745,
+                    "angularVelocity": -0.6028057510857173,
+                    "velocityX": 2.83853054418689,
+                    "velocityY": 1.8127613093097652,
+                    "timestamp": 0.4446082917508243
+                },
+                {
+                    "x": 2.008427579479744,
+                    "y": 6.172999646985284,
+                    "heading": -0.13870025114422718,
+                    "angularVelocity": -0.6255529660251172,
+                    "velocityX": 2.9411947953783932,
+                    "velocityY": 1.6886680424557017,
+                    "timestamp": 0.4639390870443384
+                },
+                {
+                    "x": 2.066782288063465,
+                    "y": 6.202913115188225,
+                    "heading": -0.15112974078850008,
+                    "angularVelocity": -0.6429890470384952,
+                    "velocityX": 3.018743290054933,
+                    "velocityY": 1.5474515015415655,
+                    "timestamp": 0.4832698823378525
+                },
+                {
+                    "x": 2.126212031523665,
+                    "y": 6.229902950259444,
+                    "heading": -0.1638060412217236,
+                    "angularVelocity": -0.6557567984529158,
+                    "velocityX": 3.074355842986947,
+                    "velocityY": 1.3962092434072775,
+                    "timestamp": 0.5026006776313666
+                },
+                {
+                    "x": 2.1863688602207922,
+                    "y": 6.253863406934038,
+                    "heading": -0.17665473442964857,
+                    "angularVelocity": -0.6646748368514358,
+                    "velocityX": 3.111968637798953,
+                    "velocityY": 1.2394966844759612,
+                    "timestamp": 0.5219314729248807
+                },
+                {
+                    "x": 2.246973575487724,
+                    "y": 6.2747405977079325,
+                    "heading": -0.1896159238006329,
+                    "angularVelocity": -0.6704943678821678,
+                    "velocityX": 3.1351382261683427,
+                    "velocityY": 1.0799964748940398,
+                    "timestamp": 0.5412622682183948
+                },
+                {
+                    "x": 2.30780256528041,
+                    "y": 6.292510136082229,
+                    "heading": -0.20264154258755296,
+                    "angularVelocity": -0.6738273614273119,
+                    "velocityX": 3.146740155750102,
+                    "velocityY": 0.9192347290676759,
+                    "timestamp": 0.560593063511909
+                },
+                {
+                    "x": 2.3686753728703707,
+                    "y": 6.307164203375093,
+                    "heading": -0.2156927432279827,
+                    "angularVelocity": -0.6751507344764429,
+                    "velocityX": 3.1490068911124522,
+                    "velocityY": 0.7580685155659881,
+                    "timestamp": 0.5799238588054231
+                },
+                {
+                    "x": 2.4294446969864714,
+                    "y": 6.318704202261767,
+                    "heading": -0.228737776875863,
+                    "angularVelocity": -0.6748317102223548,
+                    "velocityX": 3.143653594867338,
+                    "velocityY": 0.5969748637576952,
+                    "timestamp": 0.5992546540989372
+                },
+                {
+                    "x": 2.4899887888473393,
+                    "y": 6.32713657767754,
+                    "heading": -0.24175037423147705,
+                    "angularVelocity": -0.6731537506881662,
+                    "velocityX": 3.132002121049924,
+                    "velocityY": 0.43621461443967424,
+                    "timestamp": 0.6185854493924513
+                },
+                {
+                    "x": 2.550205774126716,
+                    "y": 6.332470420209207,
+                    "heading": -0.25470853343764965,
+                    "angularVelocity": -0.6703376146412596,
+                    "velocityX": 3.1150805936877464,
+                    "velocityY": 0.2759246296222352,
+                    "timestamp": 0.6379162446859654
+                },
+                {
+                    "x": 2.610009417191064,
+                    "y": 6.334716080800322,
+                    "heading": -0.26759361438505785,
+                    "angularVelocity": -0.6665572084213006,
+                    "velocityX": 3.09369801688466,
+                    "velocityY": 0.11617010873154458,
+                    "timestamp": 0.6572470399794795
+                },
+                {
+                    "x": 2.669325940254228,
+                    "y": 6.333884367249437,
+                    "heading": -0.28038965768785973,
+                    "angularVelocity": -0.6619512083445117,
+                    "velocityX": 3.0684988466597254,
+                    "velocityY": -0.04302531469893782,
+                    "timestamp": 0.6765778352729936
+                },
+                {
+                    "x": 2.7280916104140402,
+                    "y": 6.329986080406231,
+                    "heading": -0.2930828674707726,
+                    "angularVelocity": -0.656631534822156,
+                    "velocityX": 3.040002714194029,
+                    "velocityY": -0.2016620001410218,
+                    "timestamp": 0.6959086305665078
+                },
+                {
+                    "x": 2.7862508879843872,
+                    "y": 6.323031751356315,
+                    "heading": -0.3056612140555153,
+                    "angularVelocity": -0.650689554866023,
+                    "velocityX": 3.0086334621659443,
+                    "velocityY": -0.35975390274029156,
+                    "timestamp": 0.7152394258600219
+                },
+                {
+                    "x": 2.843754988622848,
+                    "y": 6.3130314987287885,
+                    "heading": -0.31811412514136855,
+                    "angularVelocity": -0.6442006599713835,
+                    "velocityX": 2.974740550780866,
+                    "velocityY": -0.5173223592555559,
+                    "timestamp": 0.734570221153536
+                },
+                {
+                    "x": 2.9005607537314586,
+                    "y": 6.299994958196607,
+                    "heading": -0.3304322429824912,
+                    "angularVelocity": -0.6372276801904713,
+                    "velocityX": 2.9386150050261657,
+                    "velocityY": -0.6743923534566658,
+                    "timestamp": 0.7539010164470501
+                },
+                {
+                    "x": 2.956629753112793,
+                    "y": 6.283931255340576,
+                    "heading": -0.34260723133624194,
+                    "angularVelocity": -0.6298234588328613,
+                    "velocityX": 2.9005014294547,
+                    "velocityY": -0.8309902728844937,
+                    "timestamp": 0.7732318117405642
+                },
+                {
+                    "x": 2.9906169983281425,
+                    "y": 6.272983358953205,
+                    "heading": -0.34999362222052477,
+                    "angularVelocity": -0.625060967472129,
+                    "velocityX": 2.8761137487624757,
+                    "velocityY": -0.9264474104986867,
+                    "timestamp": 0.7850488844241373
+                },
+                {
+                    "x": 3.024312445989771,
+                    "y": 6.26090838895792,
+                    "heading": -0.35731821138357023,
+                    "angularVelocity": -0.6198311002374715,
+                    "velocityX": 2.8514208691014065,
+                    "velocityY": -1.0218241284130187,
+                    "timestamp": 0.7968659571077105
+                },
+                {
+                    "x": 3.0577122238990846,
+                    "y": 6.2477073818247915,
+                    "heading": -0.36457506252100974,
+                    "angularVelocity": -0.6140988831800235,
+                    "velocityX": 2.8264003111145226,
+                    "velocityY": -1.1171131367820717,
+                    "timestamp": 0.8086830297912836
+                },
+                {
+                    "x": 3.090812163934883,
+                    "y": 6.233381471982772,
+                    "heading": -0.3717577779780659,
+                    "angularVelocity": -0.6078252752934934,
+                    "velocityX": 2.8010270328463904,
+                    "velocityY": -1.2123061459996218,
+                    "timestamp": 0.8205001024748567
+                },
+                {
+                    "x": 3.1236077670194575,
+                    "y": 6.217931905831193,
+                    "heading": -0.3788594429566703,
+                    "angularVelocity": -0.6009665141923228,
+                    "velocityX": 2.7752730276563167,
+                    "velocityY": -1.307393680759415,
+                    "timestamp": 0.8323171751584298
+                },
+                {
+                    "x": 3.1560941623709735,
+                    "y": 6.201360058471141,
+                    "heading": -0.3858725603738981,
+                    "angularVelocity": -0.5934733249949906,
+                    "velocityX": 2.749106840704776,
+                    "velocityY": -1.4023648498911914,
+                    "timestamp": 0.844134247842003
+                },
+                {
+                    "x": 3.188266059838492,
+                    "y": 6.1836674538325775,
+                    "heading": -0.3927889743327873,
+                    "angularVelocity": -0.5852899566661331,
+                    "velocityX": 2.722492983583009,
+                    "velocityY": -1.4972070590002187,
+                    "timestamp": 0.8559513205255761
+                },
+                {
+                    "x": 3.2201176938016274,
+                    "y": 6.164855789080512,
+                    "heading": -0.399599779609834,
+                    "angularVelocity": -0.576352998701121,
+                    "velocityX": 2.695391220484986,
+                    "velocityY": -1.5919056483603757,
+                    "timestamp": 0.8677683932091492
+                },
+                {
+                    "x": 3.251642756703662,
+                    "y": 6.144926964467571,
+                    "heading": -0.40629521382129,
+                    "angularVelocity": -0.5665899153487628,
+                    "velocityX": 2.6677556909553144,
+                    "velocityY": -1.686443432022221,
+                    "timestamp": 0.8795854658927224
+                },
+                {
+                    "x": 3.282834319738247,
+                    "y": 6.123883120194607,
+                    "heading": -0.41286452792903033,
+                    "angularVelocity": -0.5559172126335613,
+                    "velocityX": 2.6395338227837533,
+                    "velocityY": -1.7808001047684463,
+                    "timestamp": 0.8914025385762955
+                },
+                {
+                    "x": 3.3136847374734915,
+                    "y": 6.101726682396313,
+                    "heading": -0.4192958293777808,
+                    "angularVelocity": -0.5442381223304499,
+                    "velocityX": 2.6106649727330486,
+                    "velocityY": -1.874951469926556,
+                    "timestamp": 0.9032196112598686
+                },
+                {
+                    "x": 3.3441855321977045,
+                    "y": 6.078460421163092,
+                    "heading": -0.42557589025707343,
+                    "angularVelocity": -0.5314396422408687,
+                    "velocityX": 2.5810787105179256,
+                    "velocityY": -1.9688684208198879,
+                    "timestamp": 0.9150366839434417
+                },
+                {
+                    "x": 3.37432725239814,
+                    "y": 6.0540875246663015,
+                    "heading": -0.4316899102075926,
+                    "angularVelocity": -0.5173887065125765,
+                    "velocityX": 2.5506926298537165,
+                    "velocityY": -2.0625155780475546,
+                    "timestamp": 0.9268537566270149
+                },
+                {
+                    "x": 3.4040992978747684,
+                    "y": 6.028611695167346,
+                    "heading": -0.43762121996231873,
+                    "angularVelocity": -0.5019271619587302,
+                    "velocityX": 2.519409525001445,
+                    "velocityY": -2.1558494375996973,
+                    "timestamp": 0.938670829310588
+                },
+                {
+                    "x": 3.4334897013019283,
+                    "y": 6.00203727528326,
+                    "heading": -0.44335090582849124,
+                    "angularVelocity": -0.4848650778070557,
+                    "velocityX": 2.48711370524239,
+                    "velocityY": -2.2488158104525207,
+                    "timestamp": 0.9504879019941611
+                },
+                {
+                    "x": 3.462484852215012,
+                    "y": 5.974369416891247,
+                    "heading": -0.4488573271132443,
+                    "angularVelocity": -0.46597168623725893,
+                    "velocityX": 2.4536661226929253,
+                    "velocityY": -2.341346214318684,
+                    "timestamp": 0.9623049746777342
+                },
+                {
+                    "x": 3.4910691438628887,
+                    "y": 5.945614311410449,
+                    "heading": -0.45411548589649775,
+                    "angularVelocity": -0.4449628875146618,
+                    "velocityX": 2.4188978449470153,
+                    "velocityY": -2.4333526797013256,
+                    "timestamp": 0.9741220473613074
+                },
+                {
+                    "x": 3.5192245152965964,
+                    "y": 5.915779510545288,
+                    "heading": -0.4590961890390477,
+                    "angularVelocity": -0.42148366824158007,
+                    "velocityX": 2.3826011896200363,
+                    "velocityY": -2.524720094735007,
+                    "timestamp": 0.9859391200448805
+                },
+                {
+                    "x": 3.5469298492407,
+                    "y": 5.884874383909108,
+                    "heading": -0.46376491150225324,
+                    "angularVelocity": -0.39508282535110545,
+                    "velocityX": 2.3445175201991417,
+                    "velocityY": -2.6152946219194915,
+                    "timestamp": 0.9977561927284536
+                },
+                {
+                    "x": 3.5741601690568516,
+                    "y": 5.852910789899007,
+                    "heading": -0.46808022074004524,
+                    "angularVelocity": -0.36517582258680764,
+                    "velocityX": 2.304320244556386,
+                    "velocityY": -2.7048656520944543,
+                    "timestamp": 1.0095732654120266
+                },
+                {
+                    "x": 3.6008855536785225,
+                    "y": 5.819904089620565,
+                    "heading": -0.4719915430906053,
+                    "angularVelocity": -0.3309891083260508,
+                    "velocityX": 2.2615909487314654,
+                    "velocityY": -2.7931367744165776,
+                    "timestamp": 1.0213903380955998
+                },
+                {
+                    "x": 3.62706965768041,
+                    "y": 5.785874731899473,
+                    "heading": -0.4754359323425916,
+                    "angularVelocity": -0.2914756762708573,
+                    "velocityX": 2.2157859821143067,
+                    "velocityY": -2.8796774490856496,
+                    "timestamp": 1.0332074107791729
+                },
+                {
+                    "x": 3.652667693323084,
+                    "y": 5.750850822222342,
+                    "heading": -0.4783333470461038,
+                    "angularVelocity": -0.24518887046705806,
+                    "velocityX": 2.1661909280000957,
+                    "velocityY": -2.9638397439848143,
+                    "timestamp": 1.045024483462746
+                },
+                {
+                    "x": 3.677623743737937,
+                    "y": 5.7148724362586085,
+                    "heading": -0.48057991095642455,
+                    "angularVelocity": -0.1901117112907446,
+                    "velocityX": 2.1118640024567648,
+                    "velocityY": -3.044610702424323,
+                    "timestamp": 1.0568415561463191
+                },
+                {
+                    "x": 3.701867477642102,
+                    "y": 5.67799904418439,
+                    "heading": -0.48203966919964636,
+                    "angularVelocity": -0.12352959843022769,
+                    "velocityX": 2.0515854097999866,
+                    "velocityY": -3.120349096733213,
+                    "timestamp": 1.0686586288298923
+                },
+                {
+                    "x": 3.725329217347855,
+                    "y": 5.640287080856699,
+                    "heading": -0.4827696693750387,
+                    "angularVelocity": -0.06177504318881954,
+                    "velocityX": 1.9854104594250916,
+                    "velocityY": -3.191311785711063,
+                    "timestamp": 1.0804757015134654
+                },
+                {
+                    "x": 3.74792659306077,
+                    "y": 5.60180905514361,
+                    "heading": -0.48297166201638797,
+                    "angularVelocity": -0.017093289239954308,
+                    "velocityX": 1.9122651030425624,
+                    "velocityY": -3.2561385330714976,
+                    "timestamp": 1.0922927741970385
+                },
+                {
+                    "x": 3.769555777117098,
+                    "y": 5.56268673154527,
+                    "heading": -0.48297173035274543,
+                    "angularVelocity": -0.000005782849890954889,
+                    "velocityX": 1.8303335043707702,
+                    "velocityY": -3.310661163379628,
+                    "timestamp": 1.1041098468806116
+                },
+                {
+                    "x": 3.7901587344932506,
+                    "y": 5.52301434523992,
+                    "heading": -0.48297173483145855,
+                    "angularVelocity": -3.790036003454015e-7,
+                    "velocityX": 1.743490788949144,
+                    "velocityY": -3.3572092994315077,
+                    "timestamp": 1.1159269195641848
+                },
+                {
+                    "x": 3.809721481337777,
+                    "y": 5.482818833345491,
+                    "heading": -0.4829717389588181,
+                    "angularVelocity": -3.4927089311686727e-7,
+                    "velocityX": 1.6554647135005363,
+                    "velocityY": -3.401477926957743,
+                    "timestamp": 1.127743992247758
+                },
+                {
+                    "x": 3.8282308263038667,
+                    "y": 5.442127501029994,
+                    "heading": -0.482971742791846,
+                    "angularVelocity": -3.2436357183409427e-7,
+                    "velocityX": 1.5663223424036268,
+                    "velocityY": -3.443435900336205,
+                    "timestamp": 1.139561064931331
+                },
+                {
+                    "x": 3.8456768429335884,
+                    "y": 5.400969063215312,
+                    "heading": -0.48297174636762497,
+                    "angularVelocity": -3.0259430956931755e-7,
+                    "velocityX": 1.4763399614162735,
+                    "velocityY": -3.482963921504598,
+                    "timestamp": 1.1513781376149042
+                },
+                {
+                    "x": 3.863665898181474,
+                    "y": 5.360045056052632,
+                    "heading": -0.48297175000247344,
+                    "angularVelocity": -3.075929693793739e-7,
+                    "velocityX": 1.5222936957043627,
+                    "velocityY": -3.463125619898205,
+                    "timestamp": 1.1631952102984773
+                },
+                {
+                    "x": 3.8827072670006824,
+                    "y": 5.319599949808908,
+                    "heading": -0.4829717539017329,
+                    "angularVelocity": -3.299683104273868e-7,
+                    "velocityX": 1.6113439706329509,
+                    "velocityY": -3.4225994310711667,
+                    "timestamp": 1.1750122829820504
+                },
+                {
+                    "x": 3.902794276163034,
+                    "y": 5.279663847505986,
+                    "heading": -0.48297175811002735,
+                    "angularVelocity": -3.5611987502134074e-7,
+                    "velocityX": 1.6998295347945822,
+                    "velocityY": -3.379525824397886,
+                    "timestamp": 1.1868293556656235
+                },
+                {
+                    "x": 3.92391456197569,
+                    "y": 5.240264436686968,
+                    "heading": -0.48297176267932185,
+                    "angularVelocity": -3.8666890356003695e-7,
+                    "velocityX": 1.78726884214022,
+                    "velocityY": -3.3341092057245874,
+                    "timestamp": 1.1986464283491967
+                },
+                {
+                    "x": 3.9460538007835475,
+                    "y": 5.201428448582004,
+                    "heading": -0.4829717676728866,
+                    "angularVelocity": -4.225720568590589e-7,
+                    "velocityX": 1.8734960341433124,
+                    "velocityY": -3.286430501434571,
+                    "timestamp": 1.2104635010327698
+                },
+                {
+                    "x": 3.969196975663963,
+                    "y": 5.163182231498957,
+                    "heading": -0.4829717731698369,
+                    "angularVelocity": -4.651702214495778e-7,
+                    "velocityX": 1.9584524441985565,
+                    "velocityY": -3.2365221156854496,
+                    "timestamp": 1.222280573716343
+                },
+                {
+                    "x": 3.993328387583419,
+                    "y": 5.125551733327025,
+                    "heading": -0.48297177927069973,
+                    "angularVelocity": -5.162753076431712e-7,
+                    "velocityX": 2.0420803498146496,
+                    "velocityY": -3.184417933236803,
+                    "timestamp": 1.234097646399916
                 },
                 {
                     "x": 4.018431663513184,
                     "y": 5.088562488555908,
                     "heading": -0.4829717875114574,
-                    "angularVelocity": -0.000001920532733427299,
-                    "velocityX": 3.6086312001179417,
-                    "velocityY": -1.135081104701801,
-                    "timestamp": 0.9366683402900708
-                },
-                {
-                    "x": 4.095489617333613,
-                    "y": 5.060208566496142,
-                    "heading": -0.48297174072333765,
-                    "angularVelocity": 0.00000215563208981735,
-                    "velocityX": 3.550230245224857,
-                    "velocityY": -1.306327856847248,
-                    "timestamp": 0.9583733984124503
-                },
-                {
-                    "x": 4.1711135466589555,
-                    "y": 5.028225843813575,
-                    "heading": -0.482971694875483,
-                    "angularVelocity": 0.0000021123119922022846,
-                    "velocityX": 3.4841615672696387,
-                    "velocityY": -1.4735147218783384,
-                    "timestamp": 0.9800784565348297
-                },
-                {
-                    "x": 4.245175973739862,
-                    "y": 4.9927768637698655,
-                    "heading": -0.48297164958118555,
-                    "angularVelocity": 0.0000020868083942836762,
-                    "velocityX": 3.4122197076519116,
-                    "velocityY": -1.633212859593547,
-                    "timestamp": 1.0017835146572092
-                },
-                {
-                    "x": 4.317835043195099,
-                    "y": 4.954533786792328,
-                    "heading": -0.4829716044969262,
-                    "angularVelocity": 0.0000020771314744606885,
-                    "velocityX": 3.3475639201504173,
-                    "velocityY": -1.7619430808199938,
-                    "timestamp": 1.0234885727795886
-                },
-                {
-                    "x": 4.39018173013391,
-                    "y": 4.915702997439957,
-                    "heading": -0.48297155943721365,
-                    "angularVelocity": 0.000002076000546478681,
-                    "velocityX": 3.333171767193961,
-                    "velocityY": -1.789020288884605,
-                    "timestamp": 1.045193630901968
-                },
-                {
-                    "x": 4.462524157787397,
-                    "y": 4.876864273483723,
-                    "heading": -0.4829715143782692,
-                    "angularVelocity": 0.000002075965162802355,
-                    "velocityX": 3.3329755325063237,
-                    "velocityY": -1.7893858536194671,
-                    "timestamp": 1.0668986890243475
-                },
-                {
-                    "x": 4.534839835738717,
-                    "y": 4.8379757656949245,
-                    "heading": -0.48297146931947993,
-                    "angularVelocity": 0.0000020759580074306272,
-                    "velocityX": 3.331743114605303,
-                    "velocityY": -1.7916795048180625,
-                    "timestamp": 1.088603747146727
-                },
-                {
-                    "x": 4.60713879668486,
-                    "y": 4.7990561876016065,
-                    "heading": -0.48297142426091105,
-                    "angularVelocity": 0.000002075947856864141,
-                    "velocityX": 3.3309729252282607,
-                    "velocityY": -1.793110982419974,
-                    "timestamp": 1.1103088052691064
-                },
-                {
-                    "x": 4.679445820973998,
-                    "y": 4.760151592099993,
-                    "heading": -0.48297137920221006,
-                    "angularVelocity": 0.0000020759539417382525,
-                    "velocityX": 3.331344421260875,
-                    "velocityY": -1.7924207013070022,
-                    "timestamp": 1.1320138633914858
-                },
-                {
-                    "x": 4.75175835900604,
-                    "y": 4.72125724602898,
-                    "heading": -0.4829713341434503,
-                    "angularVelocity": 0.000002075956651977989,
-                    "velocityX": 3.3315984515826855,
-                    "velocityY": -1.791948487386363,
-                    "timestamp": 1.1537189215138652
-                },
-                {
-                    "x": 4.824072564767644,
-                    "y": 4.68236600077068,
-                    "heading": -0.48297128908467135,
-                    "angularVelocity": 0.0000020759575350386475,
-                    "velocityX": 3.331675287569862,
-                    "velocityY": -1.7918056260909452,
-                    "timestamp": 1.1754239796362447
-                },
-                {
-                    "x": 4.8963869623773055,
-                    "y": 4.643475112237639,
-                    "heading": -0.48297124402589003,
-                    "angularVelocity": 0.000002075957644932215,
-                    "velocityX": 3.3316841264339563,
-                    "velocityY": -1.79178919096958,
-                    "timestamp": 1.1971290377586241
-                },
-                {
-                    "x": 4.968701267365325,
-                    "y": 4.604584051483172,
-                    "heading": -0.4829711989671097,
-                    "angularVelocity": 0.000002075957594698055,
-                    "velocityX": 3.331679859150302,
-                    "velocityY": -1.7917971255911598,
-                    "timestamp": 1.2188340958810036
-                },
-                {
-                    "x": 5.0410154847403765,
-                    "y": 4.565692827821048,
-                    "heading": -0.4829711539083305,
-                    "angularVelocity": 0.0000020759575472868184,
-                    "velocityX": 3.331675822627313,
-                    "velocityY": -1.7918046311069282,
-                    "timestamp": 1.240539154003383
-                },
-                {
-                    "x": 5.113329654064444,
-                    "y": 4.526801514813212,
-                    "heading": -0.4829711088495519,
-                    "angularVelocity": 0.000002075957520958662,
-                    "velocityX": 3.331673608812237,
-                    "velocityY": -1.7918087474613025,
-                    "timestamp": 1.2622442121257624
-                },
-                {
-                    "x": 5.185643801945598,
-                    "y": 4.487910161934636,
-                    "heading": -0.48297106379077354,
-                    "angularVelocity": 0.0000020759575090327586,
-                    "velocityX": 3.3316726208897984,
-                    "velocityY": -1.7918105843944965,
-                    "timestamp": 1.2839492702481419
-                },
-                {
-                    "x": 5.257957938797454,
-                    "y": 4.4490187885483135,
-                    "heading": -0.48297101873199527,
-                    "angularVelocity": 0.0000020759575026231074,
-                    "velocityX": 3.331672112745739,
-                    "velocityY": -1.7918115292316654,
-                    "timestamp": 1.3056543283705213
-                },
-                {
-                    "x": 5.330272065176824,
-                    "y": 4.410127395689581,
-                    "heading": -0.4829709736732172,
-                    "angularVelocity": 0.000002075957497099591,
-                    "velocityX": 3.3316716302550873,
-                    "velocityY": -1.7918124263686461,
-                    "timestamp": 1.3273593864929008
-                },
-                {
-                    "x": 5.402586187963667,
-                    "y": 4.371235996150952,
-                    "heading": -0.4829709286144391,
-                    "angularVelocity": 0.00000207595749402331,
-                    "velocityX": 3.3316714647394585,
-                    "velocityY": -1.7918127341261798,
-                    "timestamp": 1.3490644446152802
-                },
-                {
-                    "x": 5.474900315939471,
-                    "y": 4.332344606260608,
-                    "heading": -0.482970883555661,
-                    "angularVelocity": 0.0000020759574957870433,
-                    "velocityX": 3.331671703806323,
-                    "velocityY": -1.7918122896084423,
-                    "timestamp": 1.3707695027376596
-                },
-                {
-                    "x": 5.547214451711641,
-                    "y": 4.293453230866729,
-                    "heading": -0.4829708384968828,
-                    "angularVelocity": 0.0000020759574997718594,
-                    "velocityX": 3.331672063002153,
-                    "velocityY": -1.7918116217242377,
-                    "timestamp": 1.392474560860039
-                },
-                {
-                    "x": 5.619528591620017,
-                    "y": 4.254561863163663,
-                    "heading": -0.4829707934381046,
-                    "angularVelocity": 0.0000020759575011693265,
-                    "velocityX": 3.3316722535663335,
-                    "velocityY": -1.7918112673914555,
-                    "timestamp": 1.4141796189824185
-                },
-                {
-                    "x": 5.6918427353771435,
-                    "y": 4.215670502616919,
-                    "heading": -0.4829707483793263,
-                    "angularVelocity": 0.000002075957502624911,
-                    "velocityX": 3.3316724308867816,
-                    "velocityY": -1.7918109376838844,
-                    "timestamp": 1.435884677104798
-                },
-                {
-                    "x": 5.764156898177578,
-                    "y": 4.176779177479102,
-                    "heading": -0.4829707033205479,
-                    "angularVelocity": 0.0000020759575111736717,
-                    "velocityX": 3.3316733082540817,
-                    "velocityY": -1.7918093063163627,
-                    "timestamp": 1.4575897352271774
-                },
-                {
-                    "x": 5.836471080177112,
-                    "y": 4.137887888039928,
-                    "heading": -0.48297065826176927,
-                    "angularVelocity": 0.0000020759575207599675,
-                    "velocityX": 3.331674192798964,
-                    "velocityY": -1.7918076616010294,
-                    "timestamp": 1.4792947933495568
-                },
-                {
-                    "x": 5.908785251268908,
-                    "y": 4.098996578318978,
-                    "heading": -0.48297061320299073,
-                    "angularVelocity": 0.0000020759575149169034,
-                    "velocityX": 3.331673690255448,
-                    "velocityY": -1.7918085960272527,
-                    "timestamp": 1.5009998514719363
-                },
-                {
-                    "x": 5.981099339043632,
-                    "y": 4.0601051136793025,
-                    "heading": -0.48297056814421313,
-                    "angularVelocity": 0.000002075957470564626,
-                    "velocityX": 3.3316698516536873,
-                    "velocityY": -1.7918157334753977,
-                    "timestamp": 1.5227049095943157
-                },
-                {
-                    "x": 6.053413282760847,
-                    "y": 4.021213381182831,
-                    "heading": -0.48297052308543725,
-                    "angularVelocity": 0.00000207595739406405,
-                    "velocityX": 3.3316632146058507,
-                    "velocityY": -1.7918280742300665,
-                    "timestamp": 1.5444099677166951
-                },
-                {
-                    "x": 6.125727369888532,
-                    "y": 3.982321915340118,
-                    "heading": -0.4829704780266597,
-                    "angularVelocity": 0.0000020759574679524,
-                    "velocityX": 3.3316698218431826,
-                    "velocityY": -1.7918157889019397,
-                    "timestamp": 1.5661150258390746
-                },
-                {
-                    "x": 6.198042995964267,
-                    "y": 3.9434333111252817,
-                    "heading": -0.4829704329678644,
-                    "angularVelocity": 0.000002075958288103883,
-                    "velocityX": 3.3317407245813113,
-                    "velocityY": -1.7916839473808401,
-                    "timestamp": 1.587820083961454
-                },
-                {
-                    "x": 6.270359121166566,
-                    "y": 3.9045456350763237,
-                    "heading": -0.4829703879090624,
-                    "angularVelocity": 0.0000020759585995923127,
-                    "velocityX": 3.331763720444871,
-                    "velocityY": -1.791641184728093,
-                    "timestamp": 1.6095251420838335
-                },
-                {
-                    "x": 6.342677550055512,
-                    "y": 3.8656622432711547,
-                    "heading": -0.48297034285023976,
-                    "angularVelocity": 0.0000020759595452668008,
-                    "velocityX": 3.331869856379567,
-                    "velocityY": -1.791443800149147,
-                    "timestamp": 1.631230200206213
-                },
-                {
-                    "x": 6.414977737835259,
-                    "y": 3.8267449442462484,
-                    "heading": -0.4829702977916256,
-                    "angularVelocity": 0.0000020759499429522823,
-                    "velocityX": 3.3310294481622025,
-                    "velocityY": -1.7930059807042389,
-                    "timestamp": 1.6529352583285923
-                },
-                {
-                    "x": 6.487268603977522,
-                    "y": 3.787810332568674,
-                    "heading": -0.4829702527330872,
-                    "angularVelocity": 0.00000207594645249869,
-                    "velocityX": 3.330599979721225,
-                    "velocityY": -1.793803612876013,
-                    "timestamp": 1.6746403164509718
-                },
-                {
-                    "x": 6.559543297837492,
-                    "y": 3.7488457083598847,
-                    "heading": -0.4829702076720725,
-                    "angularVelocity": 0.000002076060537305802,
-                    "velocityX": 3.3298548869330626,
-                    "velocityY": -1.7951863565208734,
-                    "timestamp": 1.6963453745733512
-                },
-                {
-                    "x": 6.630628961599716,
-                    "y": 3.7077518923991635,
-                    "heading": -0.48297016252441444,
-                    "angularVelocity": 0.000002080052392839484,
-                    "velocityX": 3.2750736423474676,
-                    "velocityY": -1.893282926450661,
-                    "timestamp": 1.7180504326957307
-                },
-                {
-                    "x": 6.699799614538253,
-                    "y": 3.6635105764693066,
-                    "heading": -0.4829701170038858,
-                    "angularVelocity": 0.0000020972313632639956,
-                    "velocityX": 3.186844861162469,
-                    "velocityY": -2.0382952066016227,
-                    "timestamp": 1.73975549081811
-                },
-                {
-                    "x": 6.766806665010948,
-                    "y": 3.616056089090901,
-                    "heading": -0.48297007073382403,
-                    "angularVelocity": 0.000002131764013783056,
-                    "velocityX": 3.0871629135886995,
-                    "velocityY": -2.1863331169555953,
-                    "timestamp": 1.7614605489404895
-                },
-                {
-                    "x": 6.831474779993965,
-                    "y": 3.565460555976799,
-                    "heading": -0.48297002329923955,
-                    "angularVelocity": 0.0000021854161466442962,
-                    "velocityX": 2.9794029860873867,
-                    "velocityY": -2.331048036306895,
-                    "timestamp": 1.783165607062869
+                    "angularVelocity": -6.973603296302728e-7,
+                    "velocityX": 2.124322715274553,
+                    "velocityY": -3.1301529373289303,
+                    "timestamp": 1.2459147190834892
+                },
+                {
+                    "x": 4.068270320597518,
+                    "y": 5.022262107474161,
+                    "heading": -0.48297170333499495,
+                    "angularVelocity": 0.000003839169547390073,
+                    "velocityX": 2.2730707479901486,
+                    "velocityY": -3.023866725833157,
+                    "timestamp": 1.2678404145896236
+                },
+                {
+                    "x": 4.121253956507243,
+                    "y": 4.9584470041490425,
+                    "heading": -0.48297162876403243,
+                    "angularVelocity": 0.0000034010762602073378,
+                    "velocityX": 2.4165087896481814,
+                    "velocityY": -2.910516718033651,
+                    "timestamp": 1.289766110095758
+                },
+                {
+                    "x": 4.177258824192737,
+                    "y": 4.89726621863139,
+                    "heading": -0.4829715614847619,
+                    "angularVelocity": 0.0000030685124925835,
+                    "velocityX": 2.5543029031769633,
+                    "velocityY": -2.7903692040481487,
+                    "timestamp": 1.3116918056018925
+                },
+                {
+                    "x": 4.236154120064876,
+                    "y": 4.83886263701252,
+                    "heading": -0.482971499836949,
+                    "angularVelocity": 0.000002811669663014854,
+                    "velocityX": 2.686131249777681,
+                    "velocityY": -2.6637048572771733,
+                    "timestamp": 1.333617501108027
+                },
+                {
+                    "x": 4.2978022891900425,
+                    "y": 4.7833726571672255,
+                    "heading": -0.48297144258671054,
+                    "angularVelocity": 0.0000026111025032599054,
+                    "velocityX": 2.811685910164931,
+                    "velocityY": -2.530819596111362,
+                    "timestamp": 1.3555431966141613
+                },
+                {
+                    "x": 4.362059346053497,
+                    "y": 4.730925869094327,
+                    "heading": -0.48297138878758666,
+                    "angularVelocity": 0.0000024537020442790352,
+                    "velocityX": 2.9306735946176077,
+                    "velocityY": -2.392023918156931,
+                    "timestamp": 1.3774688921202958
+                },
+                {
+                    "x": 4.428775209886728,
+                    "y": 4.681644750011071,
+                    "heading": -0.4829713376913353,
+                    "angularVelocity": 0.000002330427847533908,
+                    "velocityX": 3.042816307221113,
+                    "velocityY": -2.247642227334136,
+                    "timestamp": 1.3993945876264302
+                },
+                {
+                    "x": 4.497794001617253,
+                    "y": 4.635644296035039,
+                    "heading": -0.4829712886902182,
+                    "angularVelocity": 0.000002234871730357886,
+                    "velocityX": 3.147849595521449,
+                    "velocityY": -2.098015726031637,
+                    "timestamp": 1.4213202831325646
+                },
+                {
+                    "x": 4.568948026555497,
+                    "y": 4.593021059923411,
+                    "heading": -0.4829712412701429,
+                    "angularVelocity": 0.0000021627626491587156,
+                    "velocityX": 3.2452345659144926,
+                    "velocityY": -1.9439855898636562,
+                    "timestamp": 1.443245978638699
+                },
+                {
+                    "x": 4.64205867202195,
+                    "y": 4.55384875924605,
+                    "heading": -0.4829711949807044,
+                    "angularVelocity": 0.000002111195903735364,
+                    "velocityX": 3.3344732643018493,
+                    "velocityY": -1.7865932994667033,
+                    "timestamp": 1.4651716741448335
+                },
+                {
+                    "x": 4.71691342103475,
+                    "y": 4.5181219873551415,
+                    "heading": -0.4829711494130406,
+                    "angularVelocity": 0.000002078276777149451,
+                    "velocityX": 3.414019363346236,
+                    "velocityY": -1.6294475986389252,
+                    "timestamp": 1.487097369650968
+                },
+                {
+                    "x": 4.7930985438005,
+                    "y": 4.485327738304056,
+                    "heading": -0.4829711041848689,
+                    "angularVelocity": 0.000002062793021590678,
+                    "velocityX": 3.4746958309446994,
+                    "velocityY": -1.4956993743693363,
+                    "timestamp": 1.5090230651571024
+                },
+                {
+                    "x": 4.8695687201769084,
+                    "y": 4.453203793512952,
+                    "heading": -0.4829710590084327,
+                    "angularVelocity": 0.0000020604334399265313,
+                    "velocityX": 3.48769672346274,
+                    "velocityY": -1.4651277439372516,
+                    "timestamp": 1.5309487606632368
+                },
+                {
+                    "x": 4.94605170532014,
+                    "y": 4.421110356684282,
+                    "heading": -0.48297101383350444,
+                    "angularVelocity": 0.0000020603646626736416,
+                    "velocityX": 3.4882809132254033,
+                    "velocityY": -1.463736318862417,
+                    "timestamp": 1.5528744561693713
+                },
+                {
+                    "x": 5.022532332392591,
+                    "y": 4.389011300831823,
+                    "heading": -0.48297096865831407,
+                    "angularVelocity": 0.000002060376618948169,
+                    "velocityX": 3.4881733649487248,
+                    "velocityY": -1.463992594598631,
+                    "timestamp": 1.5748001516755057
+                },
+                {
+                    "x": 5.099012556138694,
+                    "y": 4.356911284014184,
+                    "heading": -0.48297092348307896,
+                    "angularVelocity": 0.000002060378658787562,
+                    "velocityX": 3.488154969803465,
+                    "velocityY": -1.4640364228636573,
+                    "timestamp": 1.5967258471816401
+                },
+                {
+                    "x": 5.175492855161546,
+                    "y": 4.324811446548553,
+                    "heading": -0.48297087830785207,
+                    "angularVelocity": 0.0000020603782854914656,
+                    "velocityX": 3.48815840306965,
+                    "velocityY": -1.46402824287194,
+                    "timestamp": 1.6186515426877746
+                },
+                {
+                    "x": 5.2519731561146505,
+                    "y": 4.292711613681912,
+                    "heading": -0.48297083313262523,
+                    "angularVelocity": 0.0000020603782806948455,
+                    "velocityX": 3.488158491105669,
+                    "velocityY": -1.464028033118535,
+                    "timestamp": 1.640577238193909
+                },
+                {
+                    "x": 5.328453454863713,
+                    "y": 4.260611775564003,
+                    "heading": -0.48297078795739806,
+                    "angularVelocity": 0.0000020603782977173096,
+                    "velocityX": 3.488158390582454,
+                    "velocityY": -1.4640282726214169,
+                    "timestamp": 1.6625029337000434
+                },
+                {
+                    "x": 5.404933754197733,
+                    "y": 4.228511938839819,
+                    "heading": -0.4829707427821709,
+                    "angularVelocity": 0.000002060378298482567,
+                    "velocityX": 3.4881584172616003,
+                    "velocityY": -1.4640282090556718,
+                    "timestamp": 1.6844286292061779
+                },
+                {
+                    "x": 5.481414053769572,
+                    "y": 4.196412102682258,
+                    "heading": -0.48297069760694356,
+                    "angularVelocity": 0.000002060378303797646,
+                    "velocityX": 3.488158428108085,
+                    "velocityY": -1.4640281832128117,
+                    "timestamp": 1.7063543247123123
+                },
+                {
+                    "x": 5.5578943539285115,
+                    "y": 4.164312267923512,
+                    "heading": -0.4829706524317162,
+                    "angularVelocity": 0.000002060378305885037,
+                    "velocityX": 3.488158454884962,
+                    "velocityY": -1.4640281194148257,
+                    "timestamp": 1.7282800202184467
+                },
+                {
+                    "x": 5.634374653438418,
+                    "y": 4.132212431618396,
+                    "heading": -0.4829706072564887,
+                    "angularVelocity": 0.0000020603783152723795,
+                    "velocityX": 3.4881584252834856,
+                    "velocityY": -1.4640281899425134,
+                    "timestamp": 1.7502057157245812
+                },
+                {
+                    "x": 5.71085495402441,
+                    "y": 4.100112597877132,
+                    "heading": -0.4829705620812611,
+                    "angularVelocity": 0.0000020603783147781934,
+                    "velocityX": 3.4881584743622165,
+                    "velocityY": -1.4640280730089004,
+                    "timestamp": 1.7721314112307156
+                },
+                {
+                    "x": 5.7873352532620705,
+                    "y": 4.068012760923364,
+                    "heading": -0.4829705169060333,
+                    "angularVelocity": 0.0000020603783267765002,
+                    "velocityX": 3.488158412866673,
+                    "velocityY": -1.4640282195266534,
+                    "timestamp": 1.79405710673685
+                },
+                {
+                    "x": 5.863815552376704,
+                    "y": 4.035912923676468,
+                    "heading": -0.4829704717308054,
+                    "angularVelocity": 0.000002060378332596705,
+                    "velocityX": 3.4881584072556215,
+                    "velocityY": -1.4640282328957663,
+                    "timestamp": 1.8159828022429845
+                },
+                {
+                    "x": 5.940295849970619,
+                    "y": 4.00381308280633,
+                    "heading": -0.48297042655557715,
+                    "angularVelocity": 0.0000020603783450065593,
+                    "velocityX": 3.4881583378978336,
+                    "velocityY": -1.4640283981467839,
+                    "timestamp": 1.837908497749119
+                },
+                {
+                    "x": 6.016776146567219,
+                    "y": 3.971713239559965,
+                    "heading": -0.4829703813803487,
+                    "angularVelocity": 0.0000020603783564721777,
+                    "velocityX": 3.4881582924116907,
+                    "velocityY": -1.4640285065230472,
+                    "timestamp": 1.8598341932552533
+                },
+                {
+                    "x": 6.093256433235532,
+                    "y": 3.939613372658632,
+                    "heading": -0.48297033620511903,
+                    "angularVelocity": 0.000002060378411227836,
+                    "velocityX": 3.4881578395965716,
+                    "velocityY": -1.464029585392675,
+                    "timestamp": 1.8817598887613878
+                },
+                {
+                    "x": 6.169736625426417,
+                    "y": 3.9075132806585757,
+                    "heading": -0.4829702910298788,
+                    "angularVelocity": 0.0000020603788925938424,
+                    "velocityX": 3.488153530614592,
+                    "velocityY": -1.4640398518273146,
+                    "timestamp": 1.9036855842675222
+                },
+                {
+                    "x": 6.2462171159811755,
+                    "y": 3.8754138995336747,
+                    "heading": -0.4829702458546715,
+                    "angularVelocity": 0.0000020603773887814844,
+                    "velocityX": 3.4881671385698874,
+                    "velocityY": -1.4640074298145878,
+                    "timestamp": 1.9256112797736566
+                },
+                {
+                    "x": 6.322701012459848,
+                    "y": 3.843322634623867,
+                    "heading": -0.4829702006798376,
+                    "angularVelocity": 0.0000020603603619090433,
+                    "velocityX": 3.4883224779483077,
+                    "velocityY": -1.4636372607092853,
+                    "timestamp": 1.947536975279791
+                },
+                {
+                    "x": 6.399174860669393,
+                    "y": 3.811207431927173,
+                    "heading": -0.4829701555038708,
+                    "angularVelocity": 0.000002060412030184656,
+                    "velocityX": 3.4878641905846774,
+                    "velocityY": -1.4647290293616972,
+                    "timestamp": 1.9694626707859255
+                },
+                {
+                    "x": 6.47530722887592,
+                    "y": 3.7782908978955247,
+                    "heading": -0.4829701102655904,
+                    "angularVelocity": 0.0000020632540676234596,
+                    "velocityX": 3.4722897700184046,
+                    "velocityY": -1.501276619595699,
+                    "timestamp": 1.99138836629206
+                },
+                {
+                    "x": 6.550061088980348,
+                    "y": 3.742353506827061,
+                    "heading": -0.48297006466550657,
+                    "angularVelocity": 0.0000020797554102610617,
+                    "velocityX": 3.409417962749362,
+                    "velocityY": -1.6390536418100325,
+                    "timestamp": 2.0133140617981944
+                },
+                {
+                    "x": 6.623057371447113,
+                    "y": 3.702968505083503,
+                    "heading": -0.482970018320009,
+                    "angularVelocity": 0.0000021137526750992832,
+                    "velocityX": 3.3292573294334487,
+                    "velocityY": -1.796294294637834,
+                    "timestamp": 2.035239757304329
+                },
+                {
+                    "x": 6.694087047655727,
+                    "y": 3.6601383683558693,
+                    "heading": -0.48296997081845616,
+                    "angularVelocity": 0.0000021664787259977457,
+                    "velocityX": 3.2395631960090396,
+                    "velocityY": -1.9534220346951927,
+                    "timestamp": 2.0571654528104633
+                },
+                {
+                    "x": 6.762971951373863,
+                    "y": 3.6139376604858007,
+                    "heading": -0.48296992170702124,
+                    "angularVelocity": 0.000002239903173580672,
+                    "velocityX": 3.141743152404488,
+                    "velocityY": -2.1071490232631853,
+                    "timestamp": 2.0790911483165977
+                },
+                {
+                    "x": 6.829544518243924,
+                    "y": 3.564463136615443,
+                    "heading": -0.4829698704667692,
+                    "angularVelocity": 0.0000023369955126814126,
+                    "velocityX": 3.0362807351500685,
+                    "velocityY": -2.256463146473092,
+                    "timestamp": 2.101016843822732
                 },
                 {
                     "x": 6.893649101257324,
                     "y": 3.5118300914764404,
-                    "heading": -0.48296997476148124,
-                    "angularVelocity": 0.00000223624180421772,
-                    "velocityX": 2.8645083976649595,
-                    "velocityY": -2.470874033046681,
-                    "timestamp": 1.8048706651852484
-                },
-                {
-                    "x": 6.934561334359113,
-                    "y": 3.474160479757131,
-                    "heading": -0.4829699264500687,
-                    "angularVelocity": 0.0000032862628672599958,
-                    "velocityX": 2.782952215912443,
-                    "velocityY": -2.5623810156238895,
-                    "timestamp": 1.8195716840772946
-                },
-                {
-                    "x": 6.974231870495054,
-                    "y": 3.435185370851295,
-                    "heading": -0.4829698798888691,
-                    "angularVelocity": 0.0000031672090196575476,
-                    "velocityX": 2.6984888889168737,
-                    "velocityY": -2.6511841928806597,
-                    "timestamp": 1.8342727029693409
-                },
-                {
-                    "x": 7.012622614846585,
-                    "y": 3.394949051793438,
-                    "heading": -0.482969834780297,
-                    "angularVelocity": 0.0000030683976682796836,
-                    "velocityX": 2.611434257274787,
-                    "velocityY": -2.736974855506367,
-                    "timestamp": 1.848973721861387
-                },
-                {
-                    "x": 7.049699862488227,
-                    "y": 3.3534992220248374,
-                    "heading": -0.4829697908553284,
-                    "angularVelocity": 0.0000029878860039287864,
-                    "velocityX": 2.5220869324714577,
-                    "velocityY": -2.8195208830745364,
-                    "timestamp": 1.8636747407534333
-                },
-                {
-                    "x": 7.085438739774718,
-                    "y": 3.310890028476691,
-                    "heading": -0.48296974786836877,
-                    "angularVelocity": 0.0000029240802913465185,
-                    "velocityX": 2.4310476402306533,
-                    "velocityY": -2.8983837012274143,
-                    "timestamp": 1.8783757596454795
-                },
-                {
-                    "x": 7.119836677140904,
-                    "y": 3.2671911422114968,
-                    "heading": -0.4829697055890564,
-                    "angularVelocity": 0.0000028759443619428334,
-                    "velocityX": 2.339833559754191,
-                    "velocityY": -2.9725073198044742,
-                    "timestamp": 1.8930767785375258
-                },
-                {
-                    "x": 7.1529563800127525,
-                    "y": 3.222515689615774,
-                    "heading": -0.48296966398118896,
-                    "angularVelocity": 0.000002830270996857658,
-                    "velocityX": 2.2528848588700137,
-                    "velocityY": -3.0389357991976094,
-                    "timestamp": 1.907777797429572
-                },
-                {
-                    "x": 7.184894346745796,
-                    "y": 3.177381185858669,
-                    "heading": -0.4821430167851757,
-                    "angularVelocity": 0.056230605652804695,
-                    "velocityX": 2.17250021699707,
-                    "velocityY": -3.0701616050246474,
-                    "timestamp": 1.9224788163216182
-                },
-                {
-                    "x": 7.2158791345376,
-                    "y": 3.1333943550888805,
-                    "heading": -0.47750834116458535,
-                    "angularVelocity": 0.31526220424747137,
-                    "velocityX": 2.107662606199986,
-                    "velocityY": -2.992094023740475,
-                    "timestamp": 1.9371798352136644
-                },
-                {
-                    "x": 7.245895346179036,
-                    "y": 3.090550131820991,
-                    "heading": -0.46908945700490695,
-                    "angularVelocity": 0.5726735147747635,
-                    "velocityX": 2.041777638805421,
-                    "velocityY": -2.9143710094182476,
-                    "timestamp": 1.9518808541057107
-                },
-                {
-                    "x": 7.274933319615603,
-                    "y": 3.048846636162261,
-                    "heading": -0.45692051225089847,
-                    "angularVelocity": 0.827761996863506,
-                    "velocityX": 1.9752354343465999,
-                    "velocityY": -2.8367758700924766,
-                    "timestamp": 1.9665818729977569
-                },
-                {
-                    "x": 7.3029845213490745,
-                    "y": 3.008282422680816,
-                    "heading": -0.4410407652781448,
-                    "angularVelocity": 1.08018002625281,
-                    "velocityX": 1.90811276003785,
-                    "velocityY": -2.7592790526506894,
-                    "timestamp": 1.981282891889803
-                },
-                {
-                    "x": 7.330040747407425,
-                    "y": 2.9688555797343485,
-                    "heading": -0.42149121219843927,
-                    "angularVelocity": 1.3298093977882255,
-                    "velocityX": 1.8404320310743014,
-                    "velocityY": -2.6819122698903146,
-                    "timestamp": 1.9959839107818493
-                },
-                {
-                    "x": 7.356112777651364,
-                    "y": 2.9307983654229495,
-                    "heading": -0.3996724931243264,
-                    "angularVelocity": 1.4841637327544344,
-                    "velocityX": 1.7734845751435842,
-                    "velocityY": -2.5887467114262686,
-                    "timestamp": 2.0106849296738956
-                },
-                {
-                    "x": 7.38118332940277,
-                    "y": 2.8941879380599844,
-                    "heading": -0.37646864699164706,
-                    "angularVelocity": 1.578383532670194,
-                    "velocityX": 1.7053615083080305,
-                    "velocityY": -2.4903326518935414,
-                    "timestamp": 2.025385948565942
-                },
-                {
-                    "x": 7.40523963577506,
-                    "y": 2.8590504496296347,
-                    "heading": -0.3524751392807844,
-                    "angularVelocity": 1.6320982842790503,
-                    "velocityX": 1.6363700059799433,
-                    "velocityY": -2.390139669119207,
-                    "timestamp": 2.040086967457988
-                },
-                {
-                    "x": 7.428276340738969,
-                    "y": 2.825395406477394,
-                    "heading": -0.3281233875567818,
-                    "angularVelocity": 1.656466936259613,
-                    "velocityX": 1.5670141731720877,
-                    "velocityY": -2.289300040995877,
-                    "timestamp": 2.0547879863500342
-                },
-                {
-                    "x": 7.450292281624529,
-                    "y": 2.793225377698084,
-                    "heading": -0.3037369697425905,
-                    "angularVelocity": 1.658825010243686,
-                    "velocityX": 1.497579252650967,
-                    "velocityY": -2.1882856566299744,
-                    "timestamp": 2.0694890052420805
-                },
-                {
-                    "x": 7.471288319158757,
-                    "y": 2.7625397684205146,
-                    "heading": -0.27956516086770095,
-                    "angularVelocity": 1.6442267745106667,
-                    "velocityX": 1.4282028809299567,
-                    "velocityY": -2.0873117368873455,
-                    "timestamp": 2.0841900241341267
-                },
-                {
-                    "x": 7.491266209557209,
-                    "y": 2.733336527235843,
-                    "heading": -0.2558043926700512,
-                    "angularVelocity": 1.6162667616531794,
-                    "velocityX": 1.3589459713748013,
-                    "velocityY": -1.9864773590946931,
-                    "timestamp": 2.098891043026173
-                },
-                {
-                    "x": 7.510228057377649,
-                    "y": 2.7056129838220437,
-                    "heading": -0.2326125564520332,
-                    "angularVelocity": 1.577566588297197,
-                    "velocityX": 1.28983221909181,
-                    "velocityY": -1.885824623271541,
-                    "timestamp": 2.113592061918219
-                },
-                {
-                    "x": 7.528176060430696,
-                    "y": 2.679366275566135,
-                    "heading": -0.21011884554685348,
-                    "angularVelocity": 1.5300783619392286,
-                    "velocityX": 1.2208679673730278,
-                    "velocityY": -1.7853666095287006,
-                    "timestamp": 2.1282930808102654
-                },
-                {
-                    "x": 7.545112396890263,
-                    "y": 2.654593565371254,
-                    "heading": -0.18843070859149175,
-                    "angularVelocity": 1.4752812110931899,
-                    "velocityX": 1.152051880480808,
-                    "velocityY": -1.6851015821961506,
-                    "timestamp": 2.1429940997023116
-                },
-                {
-                    "x": 7.561039180049459,
-                    "y": 2.631292148556631,
-                    "heading": -0.1676388680105614,
-                    "angularVelocity": 1.4143128944742427,
-                    "velocityX": 1.0833795450607198,
-                    "velocityY": -1.5850205340005987,
-                    "timestamp": 2.157695118594358
-                },
-                {
-                    "x": 7.575958444025175,
-                    "y": 2.6094594995019675,
-                    "heading": -0.14782100448684538,
-                    "angularVelocity": 1.3480605439149753,
-                    "velocityX": 1.0148455753489192,
-                    "velocityY": -1.485111284801969,
-                    "timestamp": 2.172396137486404
-                },
-                {
-                    "x": 7.589872142500143,
-                    "y": 2.5890932855015207,
-                    "heading": -0.12904449811203259,
-                    "angularVelocity": 1.2772248313327248,
-                    "velocityX": 0.9464445000131985,
-                    "velocityY": -1.3853607120705473,
-                    "timestamp": 2.1870971563784503
-                },
-                {
-                    "x": 7.602782151943804,
-                    "y": 2.570191363211945,
-                    "heading": -0.11136848807718236,
-                    "angularVelocity": 1.2023663233582889,
-                    "velocityX": 0.8781710668125251,
-                    "velocityY": -1.2857559349035097,
-                    "timestamp": 2.2017981752704965
-                },
-                {
-                    "x": 7.614690275426409,
-                    "y": 2.552751766479238,
-                    "heading": -0.09484543137092857,
-                    "angularVelocity": 1.1239395600799826,
-                    "velocityX": 0.8100202829510676,
-                    "velocityY": -1.186284900439311,
-                    "timestamp": 2.2164991941625427
-                },
-                {
-                    "x": 7.62559824546286,
-                    "y": 2.536772690588083,
-                    "heading": -0.07952228805027751,
-                    "angularVelocity": 1.042318456507909,
-                    "velocityX": 0.7419873490775946,
-                    "velocityY": -1.086936627215761,
-                    "timestamp": 2.231200213054589
-                },
-                {
-                    "x": 7.635507725447758,
-                    "y": 2.522252475791086,
-                    "heading": -0.06544142530276546,
-                    "angularVelocity": 0.9578154310876013,
-                    "velocityX": 0.6740675634570735,
-                    "velocityY": -0.9877012541528096,
-                    "timestamp": 2.245901231946635
-                },
-                {
-                    "x": 7.644420309753795,
-                    "y": 2.509189591670685,
-                    "heading": -0.05264130826624494,
-                    "angularVelocity": 0.8706959109783889,
-                    "velocityX": 0.6062562310465265,
-                    "velocityY": -0.8885699839124228,
-                    "timestamp": 2.2606022508386814
-                },
-                {
-                    "x": 7.652337522761704,
-                    "y": 2.497582623097395,
-                    "heading": -0.04115702850586921,
-                    "angularVelocity": 0.7811893750159932,
-                    "velocityX": 0.538548590818527,
-                    "velocityY": -0.7895349743118973,
-                    "timestamp": 2.2753032697307276
-                },
-                {
-                    "x": 7.6592608171290975,
-                    "y": 2.487430258069545,
-                    "heading": -0.031020708755951136,
-                    "angularVelocity": 0.6894977704846257,
-                    "velocityX": 0.4709397639872257,
-                    "velocityY": -0.6905892103399419,
-                    "timestamp": 2.290004288622774
-                },
-                {
-                    "x": 7.665191571570965,
-                    "y": 2.4787312774326344,
-                    "heading": -0.022261813501037925,
-                    "angularVelocity": 0.5958019181685587,
-                    "velocityX": 0.40342472079099084,
-                    "velocityY": -0.5917263763001924,
-                    "timestamp": 2.30470530751482
-                },
-                {
-                    "x": 7.670131088361328,
-                    "y": 2.4714845463094046,
-                    "heading": -0.014907388223763604,
-                    "angularVelocity": 0.5002663646159557,
-                    "velocityX": 0.33599826152420176,
-                    "velocityY": -0.49294073944433947,
-                    "timestamp": 2.3194063264068663
-                },
-                {
-                    "x": 7.674080590697013,
-                    "y": 2.465689006980458,
-                    "heading": -0.008982245050778671,
-                    "angularVelocity": 0.40304302827545885,
-                    "velocityX": 0.26865500715889973,
-                    "velocityY": -0.3942270513020354,
-                    "timestamp": 2.3341073452989125
-                },
-                {
-                    "x": 7.677041220003002,
-                    "y": 2.461343672911727,
-                    "heading": -0.0045091086563273375,
-                    "angularVelocity": 0.304273902870198,
-                    "velocityX": 0.20138939536970257,
-                    "velocityY": -0.29558046966947404,
-                    "timestamp": 2.3488083641909587
-                },
+                    "heading": -0.48296981752333895,
+                    "angularVelocity": 0.0000024146750677216304,
+                    "velocityX": 2.923719477698919,
+                    "velocityY": -2.400518839837981,
+                    "timestamp": 2.1229425393288666
+                },
+                {
+                    "x": 6.935492251520178,
+                    "y": 3.475140361342702,
+                    "heading": -0.48296976475625003,
+                    "angularVelocity": 0.000003586928740757824,
+                    "velocityX": 2.8443562341956334,
+                    "velocityY": -2.4940441143007757,
+                    "timestamp": 2.1376534780501846
+                },
+                {
+                    "x": 6.976123919914837,
+                    "y": 3.4371133556518583,
+                    "heading": -0.4829697143572031,
+                    "angularVelocity": 0.0000034259572361559534,
+                    "velocityX": 2.7620037826529793,
+                    "velocityY": -2.5849475965623756,
+                    "timestamp": 2.1523644167715026
+                },
+                {
+                    "x": 7.015501922342544,
+                    "y": 3.3977895885157845,
+                    "heading": -0.4829696659490819,
+                    "angularVelocity": 0.0000032906208164279545,
+                    "velocityX": 2.676783798347444,
+                    "velocityY": -2.673097066136968,
+                    "timestamp": 2.1670753554928206
+                },
+                {
+                    "x": 7.053586816421239,
+                    "y": 3.357212172853621,
+                    "heading": -0.4829696191989941,
+                    "angularVelocity": 0.000003177913298539426,
+                    "velocityX": 2.5888826539333314,
+                    "velocityY": -2.758315864871489,
+                    "timestamp": 2.1817862942141386
+                },
+                {
+                    "x": 7.090341266441407,
+                    "y": 3.315425855846777,
+                    "heading": -0.48296957381059136,
+                    "angularVelocity": 0.0000030853505387280562,
+                    "velocityX": 2.4984435539051053,
+                    "velocityY": -2.8404929011286835,
+                    "timestamp": 2.1964972329354566
+                },
+                {
+                    "x": 7.125730460448936,
+                    "y": 3.2724771552259067,
+                    "heading": -0.48296952951403316,
+                    "angularVelocity": 0.000003011130632666104,
+                    "velocityX": 2.405638054643597,
+                    "velocityY": -2.9195078189422343,
+                    "timestamp": 2.2112081716567746
+                },
+                {
+                    "x": 7.1597225937186835,
+                    "y": 3.228414468562161,
+                    "heading": -0.48296947336189594,
+                    "angularVelocity": 0.000003817032908672554,
+                    "velocityX": 2.3106705774318574,
+                    "velocityY": -2.9952328330951756,
+                    "timestamp": 2.2259191103780926
+                },
+                {
+                    "x": 7.192119389825632,
+                    "y": 3.1836076694729414,
+                    "heading": -0.4820443638475483,
+                    "angularVelocity": 0.06288582475110666,
+                    "velocityX": 2.2022249375566023,
+                    "velocityY": -3.0458150861769275,
+                    "timestamp": 2.2406300490994107
+                },
+                {
+                    "x": 7.223229630823693,
+                    "y": 3.1396490395754038,
+                    "heading": -0.47743685492285376,
+                    "angularVelocity": 0.3132029173651504,
+                    "velocityX": 2.11476926030427,
+                    "velocityY": -2.988159405071387,
+                    "timestamp": 2.2553409878207287
+                },
+                {
+                    "x": 7.253205323915578,
+                    "y": 3.0967066455951215,
+                    "heading": -0.4690721734359265,
+                    "angularVelocity": 0.5686028366637977,
+                    "velocityX": 2.0376465200313345,
+                    "velocityY": -2.9190791147849615,
+                    "timestamp": 2.2700519265420467
+                },
+                {
+                    "x": 7.28210093319025,
+                    "y": 3.0548329265465135,
+                    "heading": -0.45697001578434165,
+                    "angularVelocity": 0.8226638612835941,
+                    "velocityX": 1.9642260648397363,
+                    "velocityY": -2.8464341971547444,
+                    "timestamp": 2.2847628652633647
+                },
+                {
+                    "x": 7.309941019631162,
+                    "y": 3.0140523923628573,
+                    "heading": -0.44116512849373885,
+                    "angularVelocity": 1.0743629342769347,
+                    "velocityX": 1.892475182468747,
+                    "velocityY": -2.7721231769227326,
+                    "timestamp": 2.2994738039846827
+                },
+                {
+                    "x": 7.336737361434823,
+                    "y": 2.9743781069216433,
+                    "heading": -0.42169586611687776,
+                    "angularVelocity": 1.3234547941286585,
+                    "velocityX": 1.8215249421732278,
+                    "velocityY": -2.696924118358335,
+                    "timestamp": 2.3141847427060007
+                },
+                {
+                    "x": 7.362502163456035,
+                    "y": 2.936038971640856,
+                    "heading": -0.39992762164634665,
+                    "angularVelocity": 1.479731843283817,
+                    "velocityX": 1.7514043467447793,
+                    "velocityY": -2.606165113394762,
+                    "timestamp": 2.3288956814273187
+                },
+                {
+                    "x": 7.3872337303102205,
+                    "y": 2.8991268417513614,
+                    "heading": -0.37676830696947144,
+                    "angularVelocity": 1.5742921043722566,
+                    "velocityX": 1.6811685047907687,
+                    "velocityY": -2.5091620996289765,
+                    "timestamp": 2.3436066201486367
+                },
+                {
+                    "x": 7.410930819346905,
+                    "y": 2.863676250815302,
+                    "heading": -0.3528129850042874,
+                    "angularVelocity": 1.6284019952084616,
+                    "velocityX": 1.6108481916483202,
+                    "velocityY": -2.4098116107768903,
+                    "timestamp": 2.3583175588699548
+                },
+                {
+                    "x": 7.433596368468817,
+                    "y": 2.8297026440026514,
+                    "heading": -0.328491545307115,
+                    "angularVelocity": 1.6532894438563033,
+                    "velocityX": 1.5407275872250867,
+                    "velocityY": -2.3094112113606737,
+                    "timestamp": 2.3730284975912728
+                },
+                {
+                    "x": 7.455235422231159,
+                    "y": 2.7972130374670496,
+                    "heading": -0.30412705199668255,
+                    "angularVelocity": 1.6562160832826247,
+                    "velocityX": 1.4709498946511312,
+                    "velocityY": -2.208533877482683,
+                    "timestamp": 2.387739436312591
+                },
+                {
+                    "x": 7.475853646381716,
+                    "y": 2.766210258525358,
+                    "heading": -0.2799689621545816,
+                    "angularVelocity": 1.6421854716240782,
+                    "velocityX": 1.4015573405031783,
+                    "velocityY": -2.1074643521399103,
+                    "timestamp": 2.402450375033909
+                },
+                {
+                    "x": 7.495456618827617,
+                    "y": 2.736694950498885,
+                    "heading": -0.2562142168141467,
+                    "angularVelocity": 1.6147674727249637,
+                    "velocityX": 1.332543953669861,
+                    "velocityY": -2.006351095984268,
+                    "timestamp": 2.417161313755227
+                },
+                {
+                    "x": 7.514049552575163,
+                    "y": 2.7086666100983954,
+                    "heading": -0.23302134486774898,
+                    "angularVelocity": 1.576573214378762,
+                    "velocityX": 1.2638849294234806,
+                    "velocityY": -1.9052720517334563,
+                    "timestamp": 2.431872252476545
+                },
+                {
+                    "x": 7.5316372223467996,
+                    "y": 2.682124150428342,
+                    "heading": -0.2105202201245662,
+                    "angularVelocity": 1.5295505724985052,
+                    "velocityX": 1.1955504747055303,
+                    "velocityY": -1.804266890976241,
+                    "timestamp": 2.446583191197863
+                },
+                {
+                    "x": 7.5482239779420395,
+                    "y": 2.6570662133282936,
+                    "heading": -0.18881898554249216,
+                    "angularVelocity": 1.4751767370647726,
+                    "velocityX": 1.127511704688336,
+                    "velocityY": -1.7033540533846026,
+                    "timestamp": 2.461294129919181
+                },
+                {
+                    "x": 7.563813788966016,
+                    "y": 2.6334913430066904,
+                    "heading": -0.16800906628713844,
+                    "angularVelocity": 1.414588127214289,
+                    "velocityX": 1.0597427750401904,
+                    "velocityY": -1.602540175593351,
+                    "timestamp": 2.476005068640499
+                },
+                {
+                    "x": 7.5784102960217234,
+                    "y": 2.611398080774854,
+                    "heading": -0.1481688563613113,
+                    "angularVelocity": 1.3486705574455213,
+                    "velocityX": 0.9922213213052737,
+                    "velocityY": -1.5018254545389353,
+                    "timestamp": 2.490716007361817
+                },
+                {
+                    "x": 7.5920168580101715,
+                    "y": 2.5907850143215074,
+                    "heading": -0.1293664624719475,
+                    "angularVelocity": 1.2781233234366292,
+                    "velocityX": 0.9249281943327172,
+                    "velocityY": -1.401206737641836,
+                    "timestamp": 2.505426946083135
+                },
+                {
+                    "x": 7.604636591974509,
+                    "y": 2.571650800836196,
+                    "heading": -0.1116617644111604,
+                    "angularVelocity": 1.2035056631111207,
+                    "velocityX": 0.8578469534407521,
+                    "velocityY": -1.3006793004707733,
+                    "timestamp": 2.520137884804453
+                },
+                {
+                    "x": 7.616272404986451,
+                    "y": 2.5539941753706046,
+                    "heading": -0.09510797189421191,
+                    "angularVelocity": 1.1252709857977845,
+                    "velocityX": 0.790963325479677,
+                    "velocityY": -1.20023785021998,
+                    "timestamp": 2.534848823525771
+                },
+                {
+                    "x": 7.626927018849464,
+                    "y": 2.537813951229366,
+                    "heading": -0.07975280576069126,
+                    "angularVelocity": 1.0437924067530058,
+                    "velocityX": 0.7242647165386202,
+                    "velocityY": -1.0998770675179592,
+                    "timestamp": 2.549559762247089
+                },
+                {
+                    "x": 7.636602988821212,
+                    "y": 2.523109016437205,
+                    "heading": -0.0656393963876239,
+                    "angularVelocity": 0.9593819701400241,
+                    "velocityX": 0.6577398053956987,
+                    "velocityY": -0.9995918731448548,
+                    "timestamp": 2.564270700968407
+                },
+                {
+                    "x": 7.64530271759392,
+                    "y": 2.509878328655525,
+                    "heading": -0.05280696792868869,
+                    "angularVelocity": 0.8723052078477697,
+                    "velocityX": 0.5913782211668082,
+                    "velocityY": -0.899377533434203,
+                    "timestamp": 2.578981639689725
+                },
+                {
+                    "x": 7.653028465638572,
+                    "y": 2.498120909886367,
+                    "heading": -0.04129135982777762,
+                    "angularVelocity": 0.7827922010322448,
+                    "velocityX": 0.5251702961317726,
+                    "velocityY": -0.799229674726393,
+                    "timestamp": 2.593692578411043
+                },
+                {
+                    "x": 7.6597823588265515,
+                    "y": 2.48783584165749,
+                    "heading": -0.03112542462311605,
+                    "angularVelocity": 0.6910459894669937,
+                    "velocityX": 0.4591068806636599,
+                    "velocityY": -0.6991442506638482,
+                    "timestamp": 2.608403517132361
+                },
+                {
+                    "x": 7.665566394043072,
+                    "y": 2.4790222609826817,
+                    "heading": -0.02233933188139214,
+                    "angularVelocity": 0.5972489525084974,
+                    "velocityX": 0.39317920671771145,
+                    "velocityY": -0.5991174894934748,
+                    "timestamp": 2.623114455853679
+                },
+                {
+                    "x": 7.670382443324971,
+                    "y": 2.471679357147111,
+                    "heading": -0.014960801225629315,
+                    "angularVelocity": 0.5015676290643761,
+                    "velocityX": 0.3273787875222405,
+                    "velocityY": -0.4991458379831521,
+                    "timestamp": 2.637825394574997
+                },
+                {
+                    "x": 7.674232256901065,
+                    "y": 2.4658063692226495,
+                    "heading": -0.00901528221740954,
+                    "angularVelocity": 0.40415633025538694,
+                    "velocityX": 0.2616973429788216,
+                    "velocityY": -0.399225911800697,
+                    "timestamp": 2.652536333296315
+                },
+                {
+                    "x": 7.677117465389097,
+                    "y": 2.461402584136771,
+                    "heading": -0.004526094899743595,
+                    "angularVelocity": 0.30515981357196037,
+                    "velocityX": 0.19612674233020058,
+                    "velocityY": -0.2993544578835629,
+                    "timestamp": 2.667247272017633
+                },
                 {
-                    "x": 7.679014033209882,
-                    "y": 2.458447623611078,
-                    "heading": -0.00150873334272897,
-                    "angularVelocity": 0.20409301801671295,
-                    "velocityX": 0.1341956786374654,
-                    "velocityY": -0.1969965022095263,
-                    "timestamp": 2.363509383083005
+                    "x": 7.679039581308467,
+                    "y": 2.45846733507745,
+                    "heading": -0.001514541807715108,
+                    "angularVelocity": 0.20471522239871504,
+                    "velocityX": 0.13065895764927307,
+                    "velocityY": -0.1995283315991124,
+                    "timestamp": 2.681958210738951
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": -1.1328442579469643e-34,
-                    "angularVelocity": 0.10262780789604178,
-                    "velocityX": 0.06706792211866815,
-                    "velocityY": -0.0984709714141921,
-                    "timestamp": 2.378210401975051
+                    "heading": 7.235538726171091e-30,
+                    "angularVelocity": 0.10295344412796348,
+                    "velocityX": 0.06528602353168424,
+                    "velocityY": -0.09974448981448966,
+                    "timestamp": 2.696669149460269
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": -2.155134012002005e-33,
-                    "angularVelocity": -1.3889170145055665e-31,
-                    "velocityX": 1.4892305241424507e-32,
-                    "velocityY": 6.576507695252416e-33,
-                    "timestamp": 2.3929114208670974
+                    "heading": 1.9723672087239994e-30,
+                    "angularVelocity": -3.5777263674767652e-28,
+                    "velocityX": 6.106461964279306e-29,
+                    "velocityY": 3.383375524617378e-29,
+                    "timestamp": 2.711380088181587
                 }
             ],
             "constraints": [
@@ -5527,8 +6535,8 @@
                     "controlIntervalCount": 28
                 },
                 {
-                    "x": 2.7507288455963135,
-                    "y": 3.0107133388519287,
+                    "x": 2.697566270828247,
+                    "y": 2.995814085006714,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
@@ -6205,7 +7213,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 20
+                    "controlIntervalCount": 15
+                },
+                {
+                    "x": 2.079798460006714,
+                    "y": 5.466883659362793,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 10
                 },
                 {
                     "x": 2.3,
@@ -6222,190 +7239,235 @@
                     "x": 0.71,
                     "y": 4.36,
                     "heading": -1.04,
-                    "angularVelocity": 0,
+                    "angularVelocity": 3.226894361928835e-32,
                     "velocityX": 0,
                     "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7269596910839872,
-                    "y": 4.384958038291164,
-                    "heading": -1.001571192673364,
-                    "angularVelocity": 0.6206074004453302,
-                    "velocityX": 0.27389113865872694,
-                    "velocityY": 0.40306073338263126,
-                    "timestamp": 0.06192128437247234
-                },
-                {
-                    "x": 0.760846036232504,
-                    "y": 4.4351182711466235,
-                    "heading": -0.9264796681168371,
-                    "angularVelocity": 1.2126932656117428,
-                    "velocityX": 0.5472487447883301,
-                    "velocityY": 0.8100644772439347,
-                    "timestamp": 0.12384256874494468
-                },
-                {
-                    "x": 0.8116293344011996,
-                    "y": 4.510831947021006,
-                    "heading": -0.817810113065978,
-                    "angularVelocity": 1.7549628718484593,
-                    "velocityX": 0.8201266928382989,
-                    "velocityY": 1.2227407206049843,
-                    "timestamp": 0.185763853117417
-                },
-                {
-                    "x": 0.8793448344180163,
-                    "y": 4.612606899775036,
-                    "heading": -0.6817312187907495,
-                    "angularVelocity": 2.197610977457753,
-                    "velocityX": 1.0935738930977383,
-                    "velocityY": 1.6436182450904597,
-                    "timestamp": 0.24768513748988935
-                },
-                {
-                    "x": 0.9640984700059907,
-                    "y": 4.74109575406918,
-                    "heading": -0.5303096586797065,
-                    "angularVelocity": 2.4453879089491037,
-                    "velocityX": 1.3687318738119136,
-                    "velocityY": 2.075035354907217,
-                    "timestamp": 0.3096064218623617
-                },
-                {
-                    "x": 1.0654137163163762,
-                    "y": 4.896663616218439,
-                    "heading": -0.3925803264267699,
-                    "angularVelocity": 2.224264784697608,
-                    "velocityX": 1.6361941994120859,
-                    "velocityY": 2.5123487622362273,
-                    "timestamp": 0.37152770623483405
-                },
-                {
-                    "x": 1.179096696888173,
-                    "y": 5.075186082347611,
-                    "heading": -0.3289347906925715,
-                    "angularVelocity": 1.0278458591290565,
-                    "velocityX": 1.8359273668802587,
-                    "velocityY": 2.88305496144612,
-                    "timestamp": 0.4334489906073064
-                },
-                {
-                    "x": 1.3095067112863388,
-                    "y": 5.269772108097061,
-                    "heading": -0.32893471990297235,
-                    "angularVelocity": 0.0000011432191669124052,
-                    "velocityX": 2.1060611988232667,
-                    "velocityY": 3.142473992932148,
-                    "timestamp": 0.49537027497977876
-                },
-                {
-                    "x": 1.4399171936865056,
-                    "y": 5.464357820195416,
-                    "heading": -0.32893464911575754,
-                    "angularVelocity": 0.0000011431806608935866,
-                    "velocityX": 2.106068756838354,
-                    "velocityY": 3.1424689276125553,
-                    "timestamp": 0.557291559352251
-                },
-                {
-                    "x": 1.570327676092705,
-                    "y": 5.6589435322897295,
-                    "heading": -0.3289345783285442,
-                    "angularVelocity": 0.0000011431806365404781,
-                    "velocityX": 2.106068756935774,
-                    "velocityY": 3.1424689275472675,
-                    "timestamp": 0.6192128437247234
-                },
-                {
-                    "x": 1.7007381584918697,
-                    "y": 5.853529244388756,
-                    "heading": -0.32893450754133247,
-                    "angularVelocity": 0.000001143180612539941,
-                    "velocityX": 2.1060687568221708,
-                    "velocityY": 3.1424689276234057,
-                    "timestamp": 0.6811341280971958
-                },
-                {
-                    "x": 1.8311480951447636,
-                    "y": 6.048115322242248,
-                    "heading": -0.3289344367512267,
-                    "angularVelocity": 0.0000011432273482693713,
-                    "velocityX": 2.1060599432732174,
-                    "velocityY": 3.1424748343882203,
-                    "timestamp": 0.7430554124696681
-                },
-                {
-                    "x": 1.9446381116074218,
-                    "y": 6.226114434087632,
-                    "heading": -0.2658064774598036,
-                    "angularVelocity": 1.0194872398268149,
-                    "velocityX": 1.8328110860877296,
-                    "velocityY": 2.8746030326934724,
-                    "timestamp": 0.8049766968421405
-                },
-                {
-                    "x": 2.0457809471284243,
-                    "y": 6.380047389358405,
-                    "heading": -0.1375002927378408,
-                    "angularVelocity": 2.0720853261080365,
-                    "velocityX": 1.6334098451931771,
-                    "velocityY": 2.485945775039604,
-                    "timestamp": 0.8668979812146128
-                },
-                {
-                    "x": 2.1306775060879315,
-                    "y": 6.50749359169558,
-                    "heading": 0.01763716177237307,
-                    "angularVelocity": 2.5053978786522335,
-                    "velocityX": 1.3710400199200075,
-                    "velocityY": 2.0581970097802293,
-                    "timestamp": 0.9288192655870852
-                },
-                {
-                    "x": 2.198423978678441,
-                    "y": 6.608249711273454,
-                    "heading": 0.15869859646801654,
-                    "angularVelocity": 2.278076692452355,
-                    "velocityX": 1.0940740857860296,
-                    "velocityY": 1.6271645622174213,
-                    "timestamp": 0.9907405499595575
-                },
-                {
-                    "x": 2.24920191473134,
-                    "y": 6.683037424747583,
-                    "heading": 0.27235604329703667,
-                    "angularVelocity": 1.8355150087866652,
-                    "velocityX": 0.8200400971571714,
-                    "velocityY": 1.2077868576540292,
-                    "timestamp": 1.0526618343320298
-                },
-                {
-                    "x": 2.2830642302273354,
-                    "y": 6.732456156660043,
-                    "heading": 0.35192243545091534,
-                    "angularVelocity": 1.2849603001653922,
-                    "velocityX": 0.5468606770541943,
-                    "velocityY": 0.7980895812043076,
-                    "timestamp": 1.114583118704502
+                    "x": 0.724288196557008,
+                    "y": 4.368098642385211,
+                    "heading": -1.0157828821188275,
+                    "angularVelocity": 0.526633856876912,
+                    "velocityX": 0.3107160851076989,
+                    "velocityY": 0.17611589024403787,
+                    "timestamp": 0.0459847341088113
+                },
+                {
+                    "x": 0.7529777435005003,
+                    "y": 4.384403118337835,
+                    "heading": -0.9690307892055188,
+                    "angularVelocity": 1.0166872510929454,
+                    "velocityX": 0.6238928526933836,
+                    "velocityY": 0.3545627971674321,
+                    "timestamp": 0.0919694682176226
+                },
+                {
+                    "x": 0.7962133909539325,
+                    "y": 4.4090308090891845,
+                    "heading": -0.9019535610081195,
+                    "angularVelocity": 1.4586847025950622,
+                    "velocityX": 0.9402174067404033,
+                    "velocityY": 0.5355623171174385,
+                    "timestamp": 0.1379542023264339
+                },
+                {
+                    "x": 0.8541780196778807,
+                    "y": 4.442096684944602,
+                    "heading": -0.8174192357524789,
+                    "angularVelocity": 1.8383127986701173,
+                    "velocityX": 1.2605189493276492,
+                    "velocityY": 0.7190620212605457,
+                    "timestamp": 0.1839389364352452
+                },
+                {
+                    "x": 0.92708218377146,
+                    "y": 4.483710427261066,
+                    "heading": -0.7190735575075958,
+                    "angularVelocity": 2.1386592779291416,
+                    "velocityX": 1.585399274486893,
+                    "velocityY": 0.9049468942886179,
+                    "timestamp": 0.2299236705440565
+                },
+                {
+                    "x": 1.015132605778429,
+                    "y": 4.5339957868438265,
+                    "heading": -0.6117681897735129,
+                    "angularVelocity": 2.333499797567627,
+                    "velocityX": 1.914775059884505,
+                    "velocityY": 1.0935228953105474,
+                    "timestamp": 0.2759084046528678
+                },
+                {
+                    "x": 1.1184514719230099,
+                    "y": 4.593093468568762,
+                    "heading": -0.5033518248752775,
+                    "angularVelocity": 2.357659927786404,
+                    "velocityX": 2.246807949353511,
+                    "velocityY": 1.2851587134349265,
+                    "timestamp": 0.32189313876167913
+                },
+                {
+                    "x": 1.2364935087746176,
+                    "y": 4.660915178231087,
+                    "heading": -0.4100171287779816,
+                    "angularVelocity": 2.029688719661721,
+                    "velocityX": 2.566983133408849,
+                    "velocityY": 1.4748744551145279,
+                    "timestamp": 0.36787787287049045
+                },
+                {
+                    "x": 1.3657550031232673,
+                    "y": 4.73811911608404,
+                    "heading": -0.35248337281854053,
+                    "angularVelocity": 1.2511490405338719,
+                    "velocityX": 2.8109653530405243,
+                    "velocityY": 1.6789036481164823,
+                    "timestamp": 0.4138626069793018
+                },
+                {
+                    "x": 1.5059170232916794,
+                    "y": 4.82520017003363,
+                    "heading": -0.3304805961437169,
+                    "angularVelocity": 0.47848002388715516,
+                    "velocityX": 3.0480119736421156,
+                    "velocityY": 1.8936948454141527,
+                    "timestamp": 0.4598473410881131
+                },
+                {
+                    "x": 1.6456219690029539,
+                    "y": 4.9288521484832435,
+                    "heading": -0.3304805109628974,
+                    "angularVelocity": 0.0000018523716874218418,
+                    "velocityX": 3.038072273739655,
+                    "velocityY": 2.2540519252400664,
+                    "timestamp": 0.5058320751969244
+                },
+                {
+                    "x": 1.7741171019961581,
+                    "y": 5.046113419715312,
+                    "heading": -0.3304804288379279,
+                    "angularVelocity": 0.000001785918111046389,
+                    "velocityX": 2.794299792821407,
+                    "velocityY": 2.5500043330598703,
+                    "timestamp": 0.5518168093057357
+                },
+                {
+                    "x": 1.8900823069364519,
+                    "y": 5.175779429346925,
+                    "heading": -0.330480329845049,
+                    "angularVelocity": 0.000002152733527656335,
+                    "velocityX": 2.5218196253105503,
+                    "velocityY": 2.8197620828859327,
+                    "timestamp": 0.597801543414547
+                },
+                {
+                    "x": 1.992326225217661,
+                    "y": 5.31651812358221,
+                    "heading": -0.3304801981354216,
+                    "angularVelocity": 0.0000028642033047051737,
+                    "velocityX": 2.2234317597502637,
+                    "velocityY": 3.0605525281991963,
+                    "timestamp": 0.6437862775233584
+                },
+                {
+                    "x": 2.079798460006714,
+                    "y": 5.466883659362793,
+                    "heading": -0.3304799560936257,
+                    "angularVelocity": 0.000005263524964464214,
+                    "velocityX": 1.9022015998192814,
+                    "velocityY": 3.2699011681743237,
+                    "timestamp": 0.6897710116321697
+                },
+                {
+                    "x": 2.16894705974312,
+                    "y": 5.683675424803782,
+                    "heading": -0.33027266768763375,
+                    "angularVelocity": 0.0033444025204108644,
+                    "velocityX": 1.4383284005893175,
+                    "velocityY": 3.49773024107632,
+                    "timestamp": 0.751751710685232
+                },
+                {
+                    "x": 2.226835014262279,
+                    "y": 5.903744633222095,
+                    "heading": -0.30773378760994813,
+                    "angularVelocity": 0.3636435280988788,
+                    "velocityX": 0.9339674350815139,
+                    "velocityY": 3.550608685938014,
+                    "timestamp": 0.8137324097382944
+                },
+                {
+                    "x": 2.2584550007033997,
+                    "y": 6.112330245271792,
+                    "heading": -0.23499422209122486,
+                    "angularVelocity": 1.1735841419993758,
+                    "velocityX": 0.5101585965339779,
+                    "velocityY": 3.3653317119112898,
+                    "timestamp": 0.8757131087913568
+                },
+                {
+                    "x": 2.277604108948135,
+                    "y": 6.297377159682904,
+                    "heading": -0.09486114062468509,
+                    "angularVelocity": 2.260914826833697,
+                    "velocityX": 0.30895276331652,
+                    "velocityY": 2.985557072415336,
+                    "timestamp": 0.9376938078444191
+                },
+                {
+                    "x": 2.288511286424087,
+                    "y": 6.451498238253786,
+                    "heading": 0.04992678964099742,
+                    "angularVelocity": 2.336016412815077,
+                    "velocityX": 0.1759769999789598,
+                    "velocityY": 2.48659793977057,
+                    "timestamp": 0.9996745068974815
+                },
+                {
+                    "x": 2.294708988562371,
+                    "y": 6.5742371130661414,
+                    "heading": 0.17908010356662757,
+                    "angularVelocity": 2.083766654762434,
+                    "velocityX": 0.09999406642681147,
+                    "velocityY": 1.9802757420867263,
+                    "timestamp": 1.061655205950544
+                },
+                {
+                    "x": 2.2980089897573315,
+                    "y": 6.66587189325695,
+                    "heading": 0.2827001929036045,
+                    "angularVelocity": 1.6718122079950113,
+                    "velocityX": 0.05324240038246661,
+                    "velocityY": 1.478440572481471,
+                    "timestamp": 1.1236359050036064
+                },
+                {
+                    "x": 2.2995212581337525,
+                    "y": 6.72670613551003,
+                    "heading": 0.35522073089449935,
+                    "angularVelocity": 1.1700503398453694,
+                    "velocityX": 0.024399020978011547,
+                    "velocityY": 0.9815030030719727,
+                    "timestamp": 1.185616604056669
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.393,
-                    "angularVelocity": 0.6633836000880191,
-                    "velocityX": 0.2735048205846554,
-                    "velocityY": 0.3963716771816175,
-                    "timestamp": 1.1765044030769745
+                    "angularVelocity": 0.609532801060419,
+                    "velocityX": 0.007724047543277551,
+                    "velocityY": 0.48876287219728576,
+                    "timestamp": 1.2475973031097314
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.393,
-                    "angularVelocity": 0,
-                    "velocityX": 2.8187230537991962e-33,
+                    "angularVelocity": 2.340826570383063e-30,
+                    "velocityX": -1.14239845433479e-30,
                     "velocityY": 0,
-                    "timestamp": 1.2384256874494468
+                    "timestamp": 1.3095780021627939
                 }
             ],
             "constraints": [
@@ -6436,7 +7498,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 15
+                    "controlIntervalCount": 14
+                },
+                {
+                    "x": 1.7808787822723389,
+                    "y": 5.128108024597168,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 6
                 },
                 {
                     "x": 2.25,
@@ -6453,145 +7524,190 @@
                     "x": 0.71,
                     "y": 4.36,
                     "heading": -1.04,
-                    "angularVelocity": 2.3377521162047767e-32,
+                    "angularVelocity": -1.335008934228872e-34,
                     "velocityX": 0,
                     "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.737441410859622,
-                    "y": 4.3811882786622816,
-                    "heading": -1.0071421705955907,
-                    "angularVelocity": 0.501398563530153,
-                    "velocityX": 0.41874598035403643,
-                    "velocityY": 0.32332545020514236,
-                    "timestamp": 0.06553235648117978
-                },
-                {
-                    "x": 0.7924436512054522,
-                    "y": 4.423719236654154,
-                    "heading": -0.9443396668116766,
-                    "angularVelocity": 0.9583434375956188,
-                    "velocityX": 0.8393142456524665,
-                    "velocityY": 0.6490069986127681,
-                    "timestamp": 0.13106471296235955
-                },
-                {
-                    "x": 0.8751754235694627,
-                    "y": 4.487775832809834,
-                    "heading": -0.8558630826468019,
-                    "angularVelocity": 1.350120595621899,
-                    "velocityX": 1.2624568504227318,
-                    "velocityY": 0.9774804324956131,
-                    "timestamp": 0.19659706944353933
-                },
-                {
-                    "x": 0.9858651777004729,
-                    "y": 4.5735665588886505,
-                    "heading": -0.748264717894899,
-                    "angularVelocity": 1.641912034443748,
-                    "velocityX": 1.6890855155315267,
-                    "velocityY": 1.3091353750334698,
-                    "timestamp": 0.2621294259247191
-                },
-                {
-                    "x": 1.1247487447497322,
-                    "y": 4.6813201098081265,
-                    "heading": -0.6327126866941822,
-                    "angularVelocity": 1.7632821007116781,
-                    "velocityX": 2.1193128785036808,
-                    "velocityY": 1.644280119095407,
-                    "timestamp": 0.3276617824058989
-                },
-                {
-                    "x": 1.291489149115039,
-                    "y": 4.810947116825512,
-                    "heading": -0.5360472598717524,
-                    "angularVelocity": 1.4750793655678647,
-                    "velocityX": 2.5443981159626317,
-                    "velocityY": 1.9780611285451524,
-                    "timestamp": 0.39319413888707866
-                },
-                {
-                    "x": 1.4799949106081978,
-                    "y": 4.9607414161097925,
-                    "heading": -0.5171264547525753,
-                    "angularVelocity": 0.288724626049585,
-                    "velocityX": 2.876529574322522,
-                    "velocityY": 2.285806696533185,
-                    "timestamp": 0.45872649536825844
-                },
-                {
-                    "x": 1.6682619137790426,
-                    "y": 5.11061369240764,
-                    "heading": -0.49798057816746544,
-                    "angularVelocity": 0.29215913501614493,
-                    "velocityX": 2.8728862088900566,
-                    "velocityY": 2.2869965974882214,
-                    "timestamp": 0.5242588518494382
-                },
-                {
-                    "x": 1.8349963336194852,
-                    "y": 5.239757443274396,
-                    "heading": -0.40436368242247644,
-                    "angularVelocity": 1.4285598866244509,
-                    "velocityX": 2.54430679428301,
-                    "velocityY": 1.970686814899508,
-                    "timestamp": 0.589791208330618
-                },
-                {
-                    "x": 1.9739513715778099,
-                    "y": 5.347175023297594,
-                    "heading": -0.28864983484418905,
-                    "angularVelocity": 1.7657513599639834,
-                    "velocityX": 2.120403498662996,
-                    "velocityY": 1.6391533250303247,
-                    "timestamp": 0.6553235648117979
-                },
-                {
-                    "x": 2.0846889840994356,
-                    "y": 5.432668431655269,
-                    "heading": -0.18116230899252247,
-                    "angularVelocity": 1.6402206730126385,
-                    "velocityX": 1.6898158172204303,
-                    "velocityY": 1.304598414406612,
-                    "timestamp": 0.7208559212929777
-                },
-                {
-                    "x": 2.167475833751633,
-                    "y": 5.496492915332372,
-                    "heading": -0.09361421148490373,
-                    "angularVelocity": 1.3359522258711838,
-                    "velocityX": 1.2632973098712375,
-                    "velocityY": 0.973938480228949,
-                    "timestamp": 0.7863882777741575
-                },
-                {
-                    "x": 2.2225290058379836,
-                    "y": 5.538875443360028,
-                    "heading": -0.032000086006940494,
-                    "angularVelocity": 0.9402092155141033,
-                    "velocityX": 0.8400914455466154,
-                    "velocityY": 0.6467420111747162,
-                    "timestamp": 0.8519206342553374
+                    "x": 0.7208748853953925,
+                    "y": 4.367346013203549,
+                    "heading": -1.0266172190247755,
+                    "angularVelocity": 0.3311622793955978,
+                    "velocityX": 0.2691033980434437,
+                    "velocityY": 0.1817800411932904,
+                    "timestamp": 0.040411549889224324
+                },
+                {
+                    "x": 0.7426520542538281,
+                    "y": 4.382068365397769,
+                    "heading": -1.0004187382318452,
+                    "angularVelocity": 0.6482919082476458,
+                    "velocityX": 0.5388847722527592,
+                    "velocityY": 0.3643105061443391,
+                    "timestamp": 0.08082309977844865
+                },
+                {
+                    "x": 0.7753644718100721,
+                    "y": 4.404202344534013,
+                    "heading": -0.9621111981448724,
+                    "angularVelocity": 0.9479354341019116,
+                    "velocityX": 0.8094818844096516,
+                    "velocityY": 0.5477141855958662,
+                    "timestamp": 0.12123464966767297
+                },
+                {
+                    "x": 0.8190534716968692,
+                    "y": 4.4337878475210974,
+                    "heading": -0.9126009199564967,
+                    "angularVelocity": 1.2251516787674004,
+                    "velocityX": 1.0811018138764033,
+                    "velocityY": 0.7321051300478436,
+                    "timestamp": 0.1616461995568973
+                },
+                {
+                    "x": 0.8737711662497897,
+                    "y": 4.470868927169861,
+                    "heading": -0.8530717234087362,
+                    "angularVelocity": 1.4730738294111758,
+                    "velocityX": 1.3540112839748029,
+                    "velocityY": 0.9175861789614466,
+                    "timestamp": 0.20205774944612162
+                },
+                {
+                    "x": 0.9395812969768862,
+                    "y": 4.515493926508116,
+                    "heading": -0.7850998870787868,
+                    "angularVelocity": 1.681990334849146,
+                    "velocityX": 1.6284980632392125,
+                    "velocityY": 1.104263495475429,
+                    "timestamp": 0.24246929933534594
+                },
+                {
+                    "x": 1.0165562063250766,
+                    "y": 4.567717120220609,
+                    "heading": -0.7108767199363526,
+                    "angularVelocity": 1.8366820214986515,
+                    "velocityX": 1.9047749853493152,
+                    "velocityY": 1.292283860818163,
+                    "timestamp": 0.28288084922457024
+                },
+                {
+                    "x": 1.104761821592339,
+                    "y": 4.627598200600785,
+                    "heading": -0.633776622573869,
+                    "angularVelocity": 1.9078728129415852,
+                    "velocityX": 2.1826833048732497,
+                    "velocityY": 1.481781335888433,
+                    "timestamp": 0.3232923991137946
+                },
+                {
+                    "x": 1.2041714779159751,
+                    "y": 4.695156297767735,
+                    "heading": -0.5601638063396884,
+                    "angularVelocity": 1.8215786436295356,
+                    "velocityX": 2.4599317916817567,
+                    "velocityY": 1.6717521934233843,
+                    "timestamp": 0.36370394900301894
+                },
+                {
+                    "x": 1.3134031887504467,
+                    "y": 4.770142726541239,
+                    "heading": -0.5079341517285784,
+                    "angularVelocity": 1.2924437383441445,
+                    "velocityX": 2.7029824674850675,
+                    "velocityY": 1.8555692364943988,
+                    "timestamp": 0.4041154988922433
+                },
+                {
+                    "x": 1.4312329739367307,
+                    "y": 4.852476770750293,
+                    "heading": -0.48455184709669846,
+                    "angularVelocity": 0.5786045003464412,
+                    "velocityX": 2.9157452636505585,
+                    "velocityY": 2.0373889255608297,
+                    "timestamp": 0.44452704878146765
+                },
+                {
+                    "x": 1.55675883861541,
+                    "y": 4.939734199489085,
+                    "heading": -0.4845517983636149,
+                    "angularVelocity": 0.0000012059196871647072,
+                    "velocityX": 3.10618783547697,
+                    "velocityY": 2.1592200491686473,
+                    "timestamp": 0.484938598670692
+                },
+                {
+                    "x": 1.6733111473031865,
+                    "y": 5.037206282378932,
+                    "heading": -0.48207265962656115,
+                    "angularVelocity": 0.061347281751120815,
+                    "velocityX": 2.884133595649469,
+                    "velocityY": 2.4119857604332493,
+                    "timestamp": 0.5253501485599164
+                },
+                {
+                    "x": 1.7808787822723389,
+                    "y": 5.128108024597168,
+                    "heading": -0.4506827811882082,
+                    "angularVelocity": 0.7767551238296608,
+                    "velocityX": 2.661804243193232,
+                    "velocityY": 2.2494000469522843,
+                    "timestamp": 0.5657616984491407
+                },
+                {
+                    "x": 1.9395772431433458,
+                    "y": 5.270947819082425,
+                    "heading": -0.3258768071798875,
+                    "angularVelocity": 1.7295324507132326,
+                    "velocityX": 2.1992067297704168,
+                    "velocityY": 1.9794409825205124,
+                    "timestamp": 0.6379233823521242
+                },
+                {
+                    "x": 2.0647143213126102,
+                    "y": 5.386258683660538,
+                    "heading": -0.20467464704644728,
+                    "angularVelocity": 1.6795916278282566,
+                    "velocityX": 1.7341208159374817,
+                    "velocityY": 1.5979514105178143,
+                    "timestamp": 0.7100850662551077
+                },
+                {
+                    "x": 2.157719068276139,
+                    "y": 5.473035188211263,
+                    "heading": -0.10556396576995164,
+                    "angularVelocity": 1.373453000483514,
+                    "velocityX": 1.2888383686911677,
+                    "velocityY": 1.2025288194132213,
+                    "timestamp": 0.7822467501580912
+                },
+                {
+                    "x": 2.219336350583297,
+                    "y": 5.530988274582093,
+                    "heading": -0.03600663550492556,
+                    "angularVelocity": 0.9639094669484316,
+                    "velocityX": 0.853878110577325,
+                    "velocityY": 0.8031005269880169,
+                    "timestamp": 0.8544084340610747
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 2.4308347188047162e-30,
-                    "angularVelocity": 0.48830971027463277,
-                    "velocityX": 0.41919741082263007,
-                    "velocityY": 0.3223530752482305,
-                    "timestamp": 0.9174529907365172
+                    "heading": 7.006093267694686e-35,
+                    "angularVelocity": 0.49897166414982813,
+                    "velocityX": 0.4249297931839902,
+                    "velocityY": 0.40203781077103196,
+                    "timestamp": 0.9265701179640582
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 1.7485008040489027e-29,
-                    "angularVelocity": 1.1123032826758857e-30,
-                    "velocityX": 0,
-                    "velocityY": 1.931278949788811e-31,
-                    "timestamp": 0.982985347217697
+                    "heading": 0,
+                    "angularVelocity": 2.908643221674668e-34,
+                    "velocityX": -1.689604381582848e-35,
+                    "velocityY": 1.5152673195076234e-35,
+                    "timestamp": 0.9987318018670417
                 }
             ],
             "constraints": [
@@ -6781,16 +7897,34 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 35
+                    "controlIntervalCount": 14
                 },
                 {
-                    "x": 3.69740891456604,
-                    "y": 6.1177659034729,
-                    "heading": 3.14,
+                    "x": 1.7609508037567139,
+                    "y": 5.167963981628418,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
-                    "controlIntervalCount": 32
+                    "controlIntervalCount": 9
+                },
+                {
+                    "x": 2.896845817565918,
+                    "y": 6.323787212371826,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 7
+                },
+                {
+                    "x": 3.793605089187622,
+                    "y": 6.363643169403076,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 16
                 },
                 {
                     "x": 7.68,
@@ -6807,613 +7941,424 @@
                     "x": 0.71,
                     "y": 4.36,
                     "heading": -1.04,
-                    "angularVelocity": 0,
+                    "angularVelocity": -3.8536087109009435e-31,
                     "velocityX": 0,
-                    "velocityY": 5.951430910881942e-24,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7172662637575132,
-                    "y": 4.364294720467062,
-                    "heading": -1.0315470131742317,
-                    "angularVelocity": 0.26098153509522004,
-                    "velocityX": 0.22434208273715575,
-                    "velocityY": 0.13259724206381326,
-                    "timestamp": 0.03238921413610362
-                },
-                {
-                    "x": 0.7318144654254066,
-                    "y": 4.3728967855708385,
-                    "heading": -1.0149312072454106,
-                    "angularVelocity": 0.5130042939294357,
-                    "velocityX": 0.4491680967238058,
-                    "velocityY": 0.2655842487449578,
-                    "timestamp": 0.06477842827220724
-                },
-                {
-                    "x": 0.7536624687467911,
-                    "y": 4.385820612299006,
-                    "heading": -0.9904958605622504,
-                    "angularVelocity": 0.7544285137786853,
-                    "velocityX": 0.6745456444104029,
-                    "velocityY": 0.39901637235962667,
-                    "timestamp": 0.09716764240831086
-                },
-                {
-                    "x": 0.7828310955502448,
-                    "y": 4.4030821899572965,
-                    "heading": -0.9586530378385134,
-                    "angularVelocity": 0.9831304516969581,
-                    "velocityX": 0.9005660551343861,
-                    "velocityY": 0.5329421574032179,
-                    "timestamp": 0.12955685654441448
-                },
-                {
-                    "x": 0.8193448564336872,
-                    "y": 4.424698840047832,
-                    "heading": -0.919902879162077,
-                    "angularVelocity": 1.1963908266993857,
-                    "velocityX": 1.1273432177146,
-                    "velocityY": 0.6674027347406944,
-                    "timestamp": 0.1619460706805181
-                },
-                {
-                    "x": 0.8632324943492647,
-                    "y": 4.450689040734006,
-                    "heading": -0.8748582540701377,
-                    "angularVelocity": 1.3907291761589549,
-                    "velocityX": 1.3550078038681697,
-                    "velocityY": 0.8024338156819394,
-                    "timestamp": 0.19433528481662174
-                },
-                {
-                    "x": 0.9145271224696989,
-                    "y": 4.481072488182425,
-                    "heading": -0.8242801160685745,
-                    "angularVelocity": 1.5615734852049024,
-                    "velocityX": 1.5836947418633758,
-                    "velocityY": 0.9380730054376618,
-                    "timestamp": 0.22672449895272537
-                },
-                {
-                    "x": 0.9732655893831887,
-                    "y": 4.515870568727518,
-                    "heading": -0.7691375276578237,
-                    "angularVelocity": 1.7024984977725797,
-                    "velocityX": 1.8135193606940647,
-                    "velocityY": 1.0743724870528544,
-                    "timestamp": 0.25911371308882897
-                },
-                {
-                    "x": 1.0394861119792222,
-                    "y": 4.555106942812012,
-                    "heading": -0.7107271690719036,
-                    "angularVelocity": 1.8033891881560444,
-                    "velocityX": 2.044523905944935,
-                    "velocityY": 1.2114024724285466,
-                    "timestamp": 0.2915029272249326
-                },
-                {
-                    "x": 1.1132202582415271,
-                    "y": 4.598805444215395,
-                    "heading": -0.6509416110612904,
-                    "angularVelocity": 1.8458477491731227,
-                    "velocityX": 2.276503096137642,
-                    "velocityY": 1.3491683132463759,
-                    "timestamp": 0.3238921413610362
-                },
-                {
-                    "x": 1.194458181637227,
-                    "y": 4.646971254862031,
-                    "heading": -0.5929406477835183,
-                    "angularVelocity": 1.790749322723445,
-                    "velocityX": 2.508178279791774,
-                    "velocityY": 1.48709414326129,
-                    "timestamp": 0.35628135549713985
-                },
-                {
-                    "x": 1.2829200414273145,
-                    "y": 4.699514481647374,
-                    "heading": -0.5430956567390244,
-                    "angularVelocity": 1.5389379574026985,
-                    "velocityX": 2.7312135273909255,
-                    "velocityY": 1.6222445708176134,
-                    "timestamp": 0.3886705696332435
-                },
-                {
-                    "x": 1.3771379621947135,
-                    "y": 4.756486240363935,
-                    "heading": -0.5116699283377882,
-                    "angularVelocity": 0.9702528832339478,
-                    "velocityX": 2.908928891311882,
-                    "velocityY": 1.758973171660129,
-                    "timestamp": 0.4210597837693471
-                },
-                {
-                    "x": 1.4771406301349097,
-                    "y": 4.8178582459332,
-                    "heading": -0.4985254849792972,
-                    "angularVelocity": 0.40582779511896505,
-                    "velocityX": 3.087529926474048,
-                    "velocityY": 1.894828485537579,
-                    "timestamp": 0.45344899790545073
-                },
-                {
-                    "x": 1.5826858211522454,
-                    "y": 4.880091207833619,
-                    "heading": -0.49852543793275184,
-                    "angularVelocity": 0.000001452537415626245,
-                    "velocityX": 3.2586524197166784,
-                    "velocityY": 1.9214100607352578,
-                    "timestamp": 0.48583821204155436
-                },
-                {
-                    "x": 1.6882312828144117,
-                    "y": 4.9423237107273525,
-                    "heading": -0.4985253908851185,
-                    "angularVelocity": 0.0000014525710071923091,
-                    "velocityX": 3.2586607757338757,
-                    "velocityY": 1.9213958891446032,
-                    "timestamp": 0.5182274261776579
-                },
-                {
-                    "x": 1.7937767445018398,
-                    "y": 5.004556213578243,
-                    "heading": -0.49852534383749064,
-                    "angularVelocity": 0.0000014525708350435827,
-                    "velocityX": 3.2586607765138327,
-                    "velocityY": 1.921395887821837,
-                    "timestamp": 0.5506166403137616
-                },
-                {
-                    "x": 1.8993222061892705,
-                    "y": 5.06678871642913,
-                    "heading": -0.49852529678986307,
-                    "angularVelocity": 0.0000014525708270038968,
-                    "velocityX": 3.2586607765139086,
-                    "velocityY": 1.9213958878217097,
-                    "timestamp": 0.5830058544498652
-                },
-                {
-                    "x": 2.0048676678767015,
-                    "y": 5.129021219280016,
-                    "heading": -0.4985252497422358,
-                    "angularVelocity": 0.0000014525708183379938,
-                    "velocityX": 3.258660776513909,
-                    "velocityY": 1.9213958878217101,
-                    "timestamp": 0.6153950685859688
-                },
-                {
-                    "x": 2.1104131295641317,
-                    "y": 5.191253722130902,
-                    "heading": -0.49852520269460865,
-                    "angularVelocity": 0.000001452570810915054,
-                    "velocityX": 3.2586607765139104,
-                    "velocityY": 1.9213958878217086,
-                    "timestamp": 0.6477842827220724
-                },
-                {
-                    "x": 2.2159585912515625,
-                    "y": 5.253486224981789,
-                    "heading": -0.4985251556469819,
-                    "angularVelocity": 0.0000014525708028649706,
-                    "velocityX": 3.2586607765139086,
-                    "velocityY": 1.9213958878217123,
-                    "timestamp": 0.6801734968581761
-                },
-                {
-                    "x": 2.321504052938993,
-                    "y": 5.315718727832675,
-                    "heading": -0.4985251085993554,
-                    "angularVelocity": 0.000001452570794356237,
-                    "velocityX": 3.2586607765139117,
-                    "velocityY": 1.9213958878217081,
-                    "timestamp": 0.7125627109942797
-                },
-                {
-                    "x": 2.427049514626424,
-                    "y": 5.3779512306835615,
-                    "heading": -0.49852506155172915,
-                    "angularVelocity": 0.0000014525707870212498,
-                    "velocityX": 3.258660776513912,
-                    "velocityY": 1.9213958878217077,
-                    "timestamp": 0.7449519251303833
-                },
-                {
-                    "x": 2.5325949763138547,
-                    "y": 5.440183733534448,
-                    "heading": -0.4985250145041031,
-                    "angularVelocity": 0.000001452570779107594,
-                    "velocityX": 3.258660776513913,
-                    "velocityY": 1.9213958878217068,
-                    "timestamp": 0.777341139266487
-                },
-                {
-                    "x": 2.6381404380012854,
-                    "y": 5.502416236385335,
-                    "heading": -0.49852496745647734,
-                    "angularVelocity": 0.0000014525707705186945,
-                    "velocityX": 3.258660776513915,
-                    "velocityY": 1.9213958878217055,
-                    "timestamp": 0.8097303534025906
-                },
-                {
-                    "x": 2.743685899688716,
-                    "y": 5.564648739236221,
-                    "heading": -0.4985249204088519,
-                    "angularVelocity": 0.00000145257076360687,
-                    "velocityX": 3.2586607765139153,
-                    "velocityY": 1.9213958878217066,
-                    "timestamp": 0.8421195675386942
-                },
-                {
-                    "x": 2.849231361376147,
-                    "y": 5.626881242087108,
-                    "heading": -0.4985248733612266,
-                    "angularVelocity": 0.0000014525707544682094,
-                    "velocityX": 3.2586607765139153,
-                    "velocityY": 1.9213958878217066,
-                    "timestamp": 0.8745087816747978
-                },
-                {
-                    "x": 2.954776823063578,
-                    "y": 5.689113744937994,
-                    "heading": -0.4985248263136016,
-                    "angularVelocity": 0.0000014525707471414822,
-                    "velocityX": 3.2586607765139166,
-                    "velocityY": 1.921395887821705,
-                    "timestamp": 0.9068979958109015
-                },
-                {
-                    "x": 3.0603222847510096,
-                    "y": 5.751346247788881,
-                    "heading": -0.49852477926597694,
-                    "angularVelocity": 0.0000014525707385370494,
-                    "velocityX": 3.258660776513917,
-                    "velocityY": 1.9213958878217057,
-                    "timestamp": 0.9392872099470051
-                },
-                {
-                    "x": 3.1658677464384404,
-                    "y": 5.813578750639766,
-                    "heading": -0.4985247322183525,
-                    "angularVelocity": 0.0000014525707310188908,
-                    "velocityX": 3.2586607765139175,
-                    "velocityY": 1.9213958878217063,
-                    "timestamp": 0.9716764240831087
-                },
-                {
-                    "x": 3.271413208125871,
-                    "y": 5.875811253490653,
-                    "heading": -0.4985246851707283,
-                    "angularVelocity": 0.0000014525707228586851,
-                    "velocityX": 3.258660776513917,
-                    "velocityY": 1.921395887821708,
-                    "timestamp": 1.0040656382192124
-                },
-                {
-                    "x": 3.376958669813326,
-                    "y": 5.938043756341499,
-                    "heading": -0.4985246381231044,
-                    "angularVelocity": 0.000001452570714543944,
-                    "velocityX": 3.258660776514645,
-                    "velocityY": 1.921395887820474,
-                    "timestamp": 1.0364548523553159
-                },
-                {
-                    "x": 3.482504131751534,
-                    "y": 6.000276258767071,
-                    "heading": -0.49852459107548075,
-                    "angularVelocity": 0.0000014525707064344823,
-                    "velocityX": 3.258660784256519,
-                    "velocityY": 1.9213958746903632,
-                    "timestamp": 1.0688440664914194
-                },
-                {
-                    "x": 3.588052280044538,
-                    "y": 6.0625042049472775,
-                    "heading": -0.4985245440280508,
-                    "angularVelocity": 0.0000014525647243024202,
-                    "velocityX": 3.258743724052004,
-                    "velocityY": 1.921255203004168,
-                    "timestamp": 1.101233280627523
-                },
-                {
-                    "x": 3.69740891456604,
-                    "y": 6.1177659034729,
-                    "heading": -0.4985244969178424,
-                    "angularVelocity": 0.0000014545029788824699,
-                    "velocityX": 3.3763287390046313,
-                    "velocityY": 1.7061759601022248,
-                    "timestamp": 1.1336224947636264
-                },
-                {
-                    "x": 3.8477077097405896,
-                    "y": 6.176720729441604,
-                    "heading": -0.4985244519972541,
-                    "angularVelocity": 0.0000010525498975792195,
-                    "velocityX": 3.5217032391581933,
-                    "velocityY": 1.3813909907718895,
-                    "timestamp": 1.1763003653755966
-                },
-                {
-                    "x": 4.000682189533232,
-                    "y": 6.228336695265556,
-                    "heading": -0.49852440748602983,
-                    "angularVelocity": 0.000001042957945295385,
-                    "velocityX": 3.58439813418752,
-                    "velocityY": 1.2094316113670969,
-                    "timestamp": 1.2189782359875667
-                },
-                {
-                    "x": 4.153656816760498,
-                    "y": 6.279952224134777,
-                    "heading": -0.4985243629748153,
-                    "angularVelocity": 0.00000104295771772215,
-                    "velocityX": 3.5844015887793454,
-                    "velocityY": 1.2094213729291292,
-                    "timestamp": 1.2616561065995369
-                },
-                {
-                    "x": 4.306631443993679,
-                    "y": 6.331567752986466,
-                    "heading": -0.4985243184636008,
-                    "angularVelocity": 0.0000010429577168479619,
-                    "velocityX": 3.584401588917953,
-                    "velocityY": 1.209421372518333,
-                    "timestamp": 1.304333977211507
-                },
-                {
-                    "x": 4.459606071226857,
-                    "y": 6.38318328183816,
-                    "heading": -0.4985242739523863,
-                    "angularVelocity": 0.000001042957716433214,
-                    "velocityX": 3.584401588917909,
-                    "velocityY": 1.2094213725184633,
-                    "timestamp": 1.3470118478234772
-                },
-                {
-                    "x": 4.612580698460036,
-                    "y": 6.434798810689855,
-                    "heading": -0.4985242294411719,
-                    "angularVelocity": 0.0000010429577140470852,
-                    "velocityX": 3.5844015889179057,
-                    "velocityY": 1.209421372518471,
-                    "timestamp": 1.3896897184354473
-                },
-                {
-                    "x": 4.765555325693214,
-                    "y": 6.4864143395415494,
-                    "heading": -0.49852418492995754,
-                    "angularVelocity": 0.0000010429577143791187,
-                    "velocityX": 3.584401588917906,
-                    "velocityY": 1.2094213725184708,
-                    "timestamp": 1.4323675890474175
-                },
-                {
-                    "x": 4.918529952926393,
-                    "y": 6.5380298683932425,
-                    "heading": -0.4985241404187432,
-                    "angularVelocity": 0.0000010429577134079309,
-                    "velocityX": 3.584401588917911,
-                    "velocityY": 1.2094213725184557,
-                    "timestamp": 1.4750454596593876
-                },
-                {
-                    "x": 5.071504580159572,
-                    "y": 6.589645397244938,
-                    "heading": -0.49852409590752894,
-                    "angularVelocity": 0.000001042957711441133,
-                    "velocityX": 3.584401588917906,
-                    "velocityY": 1.20942137251847,
-                    "timestamp": 1.5177233302713578
-                },
-                {
-                    "x": 5.224479207392751,
-                    "y": 6.641260926096632,
-                    "heading": -0.49852405139631467,
-                    "angularVelocity": 0.0000010429577109856096,
-                    "velocityX": 3.5844015889179066,
-                    "velocityY": 1.2094213725184684,
-                    "timestamp": 1.560401200883328
-                },
-                {
-                    "x": 5.377453834625929,
-                    "y": 6.692876454948327,
-                    "heading": -0.4985240068851004,
-                    "angularVelocity": 0.000001042957710628888,
-                    "velocityX": 3.584401588917908,
-                    "velocityY": 1.209421372518467,
-                    "timestamp": 1.603079071495298
-                },
-                {
-                    "x": 5.530428461859109,
-                    "y": 6.744491983800022,
-                    "heading": -0.4985239623738862,
-                    "angularVelocity": 0.0000010429577097054993,
-                    "velocityX": 3.5844015889179075,
-                    "velocityY": 1.2094213725184686,
-                    "timestamp": 1.6457569421072682
-                },
-                {
-                    "x": 5.683403089092287,
-                    "y": 6.796107512651717,
-                    "heading": -0.4985239178626721,
-                    "angularVelocity": 0.0000010429577084041104,
-                    "velocityX": 3.584401588917908,
-                    "velocityY": 1.2094213725184642,
-                    "timestamp": 1.6884348127192383
-                },
-                {
-                    "x": 5.836377716325465,
-                    "y": 6.847723041503412,
-                    "heading": -0.4985238733514579,
-                    "angularVelocity": 0.000001042957708437487,
-                    "velocityX": 3.5844015889179084,
-                    "velocityY": 1.209421372518465,
-                    "timestamp": 1.7311126833312085
-                },
-                {
-                    "x": 5.989352343558645,
-                    "y": 6.899338570355106,
-                    "heading": -0.4985238288402439,
-                    "angularVelocity": 0.0000010429577061050693,
-                    "velocityX": 3.5844015889179084,
-                    "velocityY": 1.2094213725184642,
-                    "timestamp": 1.7737905539431786
-                },
-                {
-                    "x": 6.142326970791823,
-                    "y": 6.9509540992067995,
-                    "heading": -0.49852378432902983,
-                    "angularVelocity": 0.0000010429577060929704,
-                    "velocityX": 3.584401588917909,
-                    "velocityY": 1.2094213725184646,
-                    "timestamp": 1.8164684245551488
-                },
-                {
-                    "x": 6.295301598025002,
-                    "y": 7.002569628058494,
-                    "heading": -0.4985237398178158,
-                    "angularVelocity": 0.0000010429577057751362,
-                    "velocityX": 3.58440158891791,
-                    "velocityY": 1.2094213725184613,
-                    "timestamp": 1.859146295167119
-                },
-                {
-                    "x": 6.44827622525818,
-                    "y": 7.054185156910189,
-                    "heading": -0.49852369530660184,
-                    "angularVelocity": 0.0000010429577050460837,
-                    "velocityX": 3.5844015889179097,
-                    "velocityY": 1.2094213725184615,
-                    "timestamp": 1.901824165779089
-                },
-                {
-                    "x": 6.60125085249139,
-                    "y": 7.105800685761796,
-                    "heading": -0.4985236507953879,
-                    "angularVelocity": 0.0000010429577031279755,
-                    "velocityX": 3.584401588918602,
-                    "velocityY": 1.2094213725164094,
-                    "timestamp": 1.9445020363910592
-                },
-                {
-                    "x": 6.754225480461122,
-                    "y": 7.15741621243045,
-                    "heading": -0.49852360628410947,
-                    "angularVelocity": 0.000001042959214800434,
-                    "velocityX": 3.584401606176385,
-                    "velocityY": 1.209421321366894,
-                    "timestamp": 1.9871799070030294
-                },
-                {
-                    "x": 6.905473027890085,
-                    "y": 7.208390081717835,
-                    "heading": -0.49401401466016337,
-                    "angularVelocity": 0.10566580664128356,
-                    "velocityX": 3.543933782547739,
-                    "velocityY": 1.1943864245440503,
-                    "timestamp": 2.0298577776149997
-                },
-                {
-                    "x": 7.045508663452743,
-                    "y": 7.253974234854833,
-                    "heading": -0.45930885159823864,
-                    "angularVelocity": 0.8131887220303519,
-                    "velocityX": 3.2812235839006783,
-                    "velocityY": 1.0680981146283737,
-                    "timestamp": 2.07253564822697
-                },
-                {
-                    "x": 7.17296615196941,
-                    "y": 7.296903697598188,
-                    "heading": -0.39602280772923965,
-                    "angularVelocity": 1.4828772607799376,
-                    "velocityX": 2.986500654531669,
-                    "velocityY": 1.0058951425592673,
-                    "timestamp": 2.1152135188389405
-                },
-                {
-                    "x": 7.286216905364156,
-                    "y": 7.335009151903716,
-                    "heading": -0.3243524220820172,
-                    "angularVelocity": 1.6793336832302157,
-                    "velocityX": 2.6536177126649325,
-                    "velocityY": 0.8928621264164087,
-                    "timestamp": 2.157891389450911
-                },
-                {
-                    "x": 7.38508554947564,
-                    "y": 7.368236895639727,
-                    "heading": -0.2530399098538711,
-                    "angularVelocity": 1.6709482269282783,
-                    "velocityX": 2.31662551795061,
-                    "velocityY": 0.7785707969856192,
-                    "timestamp": 2.2005692600628812
-                },
-                {
-                    "x": 7.469627579006154,
-                    "y": 7.396613050645165,
-                    "heading": -0.18659143584410423,
-                    "angularVelocity": 1.5569772591027433,
-                    "velocityX": 1.9809336388681473,
-                    "velocityY": 0.6648915374301301,
-                    "timestamp": 2.2432471306748516
-                },
-                {
-                    "x": 7.539923583097788,
-                    "y": 7.420174905803342,
-                    "heading": -0.12774543015301906,
-                    "angularVelocity": 1.3788411850749698,
-                    "velocityX": 1.6471300719468636,
-                    "velocityY": 0.5520860066427349,
-                    "timestamp": 2.285925001286822
-                },
-                {
-                    "x": 7.596048224439389,
-                    "y": 7.4389607760624,
-                    "heading": -0.07839593087005978,
-                    "angularVelocity": 1.1563252471438406,
-                    "velocityX": 1.315075952403783,
-                    "velocityY": 0.4401782467044836,
-                    "timestamp": 2.3286028718987923
-                },
-                {
-                    "x": 7.638066692552635,
-                    "y": 7.453006402993039,
-                    "heading": -0.03996068961729096,
-                    "angularVelocity": 0.9005894788478175,
-                    "velocityX": 0.9845493111706484,
-                    "velocityY": 0.3291079599154316,
-                    "timestamp": 2.3712807425107627
-                },
+                    "x": 0.7208977987123659,
+                    "y": 4.366230614894293,
+                    "heading": -1.0125695703411368,
+                    "angularVelocity": 0.6607032256639885,
+                    "velocityX": 0.262489900865184,
+                    "velocityY": 0.1500737469179648,
+                    "timestamp": 0.04151702094612918
+                },
+                {
+                    "x": 0.7428348149738224,
+                    "y": 4.378911218024912,
+                    "heading": -0.9596996326030717,
+                    "angularVelocity": 1.2734521055122956,
+                    "velocityX": 0.5283860874778604,
+                    "velocityY": 0.30543143129192235,
+                    "timestamp": 0.08303404189225835
+                },
+                {
+                    "x": 0.7759938113776359,
+                    "y": 4.398288777276395,
+                    "heading": -0.8840273814949616,
+                    "angularVelocity": 1.8226801775305617,
+                    "velocityX": 0.7986843865038376,
+                    "velocityY": 0.46673770925264374,
+                    "timestamp": 0.12455106283838753
+                },
+                {
+                    "x": 0.8206097262268935,
+                    "y": 4.424610495629411,
+                    "heading": -0.7889470653064761,
+                    "angularVelocity": 2.2901526656373563,
+                    "velocityX": 1.0746415285232955,
+                    "velocityY": 0.6339982434446707,
+                    "timestamp": 0.1660680837845167
+                },
+                {
+                    "x": 0.876940069443398,
+                    "y": 4.458114052595694,
+                    "heading": -0.6785120124719297,
+                    "angularVelocity": 2.6599946315634684,
+                    "velocityX": 1.3568011849799015,
+                    "velocityY": 0.8069836467704494,
+                    "timestamp": 0.2075851047306459
+                },
+                {
+                    "x": 0.9451906760990277,
+                    "y": 4.499074634849957,
+                    "heading": -0.5574056685886438,
+                    "angularVelocity": 2.9170287540888538,
+                    "velocityX": 1.6439186892586013,
+                    "velocityY": 0.9865973357595108,
+                    "timestamp": 0.2491021256767751
+                },
+                {
+                    "x": 1.0254198074155758,
+                    "y": 4.547882919999799,
+                    "heading": -0.43194974381010975,
+                    "angularVelocity": 3.0217949631302035,
+                    "velocityX": 1.9324395028417536,
+                    "velocityY": 1.175621083531942,
+                    "timestamp": 0.2906191466229043
+                },
+                {
+                    "x": 1.117246624722019,
+                    "y": 4.6049782109254505,
+                    "heading": -0.31304235816438886,
+                    "angularVelocity": 2.8640635319318393,
+                    "velocityX": 2.211787243252437,
+                    "velocityY": 1.3752261030390898,
+                    "timestamp": 0.33213616756903347
+                },
+                {
+                    "x": 1.217280699738284,
+                    "y": 4.671027805780365,
+                    "heading": -0.22202476317889255,
+                    "angularVelocity": 2.1922959044630015,
+                    "velocityX": 2.409471410436724,
+                    "velocityY": 1.5909040039398776,
+                    "timestamp": 0.37365318851516266
+                },
+                {
+                    "x": 1.3245506705159111,
+                    "y": 4.7468577293550815,
+                    "heading": -0.15914757808634442,
+                    "angularVelocity": 1.5144917351940563,
+                    "velocityX": 2.5837588616171234,
+                    "velocityY": 1.8264779564293179,
+                    "timestamp": 0.41517020946129185
+                },
+                {
+                    "x": 1.4375421795348913,
+                    "y": 4.833718356767073,
+                    "heading": -0.12357000071830693,
+                    "angularVelocity": 0.8569395529247944,
+                    "velocityX": 2.7215707303608654,
+                    "velocityY": 2.0921690774675916,
+                    "timestamp": 0.45668723040742104
+                },
+                {
+                    "x": 1.5526379689004826,
+                    "y": 4.933380534066268,
+                    "heading": -0.11179394732799099,
+                    "angularVelocity": 0.2836439879911631,
+                    "velocityX": 2.7722554928653214,
+                    "velocityY": 2.4005136936152596,
+                    "timestamp": 0.49820425135355023
+                },
+                {
+                    "x": 1.6621482250840773,
+                    "y": 5.045879001435295,
+                    "heading": -0.11165007196252263,
+                    "angularVelocity": 0.0034654549371103873,
+                    "velocityX": 2.6377195108883376,
+                    "velocityY": 2.709695079403094,
+                    "timestamp": 0.5397212722996794
+                },
+                {
+                    "x": 1.760950803756714,
+                    "y": 5.167963981628418,
+                    "heading": -0.11164985662454338,
+                    "angularVelocity": 0.000005186739663230103,
+                    "velocityX": 2.3798089655981185,
+                    "velocityY": 2.940600684033376,
+                    "timestamp": 0.5812382932458086
+                },
+                {
+                    "x": 1.8636898338676644,
+                    "y": 5.328509480252694,
+                    "heading": -0.11164971710954129,
+                    "angularVelocity": 0.000002768959602908003,
+                    "velocityX": 2.039065475146862,
+                    "velocityY": 3.1863526751214604,
+                    "timestamp": 0.6316236444586746
+                },
+                {
+                    "x": 1.9479932631191832,
+                    "y": 5.499457142063362,
+                    "heading": -0.11164961128911181,
+                    "angularVelocity": 0.0000021002221265930453,
+                    "velocityX": 1.6731733970781186,
+                    "velocityY": 3.3928048072606183,
+                    "timestamp": 0.6820089956715406
+                },
+                {
+                    "x": 2.043351099956472,
+                    "y": 5.66449366180702,
+                    "heading": -0.11164949905977092,
+                    "angularVelocity": 0.000002227420038927282,
+                    "velocityX": 1.8925706488092502,
+                    "velocityY": 3.275486143730564,
+                    "timestamp": 0.7323943468844066
+                },
+                {
+                    "x": 2.1564178056024077,
+                    "y": 5.817933945525861,
+                    "heading": -0.11163496800446338,
+                    "angularVelocity": 0.0002883986749930945,
+                    "velocityX": 2.244039247910364,
+                    "velocityY": 3.0453352020399613,
+                    "timestamp": 0.7827796980972727
+                },
+                {
+                    "x": 2.2829725450073877,
+                    "y": 5.955279135270152,
+                    "heading": -0.10213363222908782,
+                    "angularVelocity": 0.1885733760372676,
+                    "velocityX": 2.5117367718340784,
+                    "velocityY": 2.7258952540120527,
+                    "timestamp": 0.8331650493101387
+                },
+                {
+                    "x": 2.421245283059438,
+                    "y": 6.075180412328107,
+                    "heading": -0.08356505866258204,
+                    "angularVelocity": 0.36853119252760197,
+                    "velocityX": 2.7443043409080725,
+                    "velocityY": 2.379685249204375,
+                    "timestamp": 0.8835504005230047
+                },
+                {
+                    "x": 2.570177190376964,
+                    "y": 6.176890921850726,
+                    "heading": -0.058269582676031995,
+                    "angularVelocity": 0.5020402830847994,
+                    "velocityX": 2.9558572825813,
+                    "velocityY": 2.018652387526583,
+                    "timestamp": 0.9339357517358707
+                },
+                {
+                    "x": 2.728959152818659,
+                    "y": 6.259893890741026,
+                    "heading": -0.029260248722302817,
+                    "angularVelocity": 0.5757493647196714,
+                    "velocityX": 3.151351704820358,
+                    "velocityY": 1.6473631103484576,
+                    "timestamp": 0.9843211029487368
+                },
+                {
+                    "x": 2.896845817565918,
+                    "y": 6.323787212371826,
+                    "heading": -5.388333556894659e-33,
+                    "angularVelocity": 0.580729279745141,
+                    "velocityX": 3.332053081047339,
+                    "velocityY": 1.2680932075002167,
+                    "timestamp": 1.034706454161603
+                },
+                {
+                    "x": 3.0174812814006593,
+                    "y": 6.35877580192698,
+                    "heading": 0.017969633030989543,
+                    "angularVelocity": 0.512929568417621,
+                    "velocityX": 3.443447970619488,
+                    "velocityY": 0.9987227956847937,
+                    "timestamp": 1.069739788442565
+                },
+                {
+                    "x": 3.141783379846136,
+                    "y": 6.384241994405335,
+                    "heading": 0.0331535119160805,
+                    "angularVelocity": 0.43341232568386434,
+                    "velocityX": 3.5481092792260864,
+                    "velocityY": 0.7269131814328351,
+                    "timestamp": 1.1047731227235271
+                },
+                {
+                    "x": 3.269386252376355,
+                    "y": 6.400068652575719,
+                    "heading": 0.0449202302050253,
+                    "angularVelocity": 0.3358720639272233,
+                    "velocityX": 3.642327376147768,
+                    "velocityY": 0.45175997361343984,
+                    "timestamp": 1.1398064570044892
+                },
+                {
+                    "x": 3.3996512457384864,
+                    "y": 6.406094302159117,
+                    "heading": 0.052174145586997546,
+                    "angularVelocity": 0.20705752202617325,
+                    "velocityX": 3.7183155995559303,
+                    "velocityY": 0.17199760476778314,
+                    "timestamp": 1.1748397912854514
+                },
+                {
+                    "x": 3.531556253779136,
+                    "y": 6.402132340099008,
+                    "heading": 0.05397985174916118,
+                    "angularVelocity": 0.05154251518314789,
+                    "velocityX": 3.7651285767486513,
+                    "velocityY": -0.11309120703085081,
+                    "timestamp": 1.2098731255664135
+                },
+                {
+                    "x": 3.663324219452337,
+                    "y": 6.387950136374773,
+                    "heading": 0.05397986327261661,
+                    "angularVelocity": 3.2892831051068895e-7,
+                    "velocityX": 3.7612168061547906,
+                    "velocityY": -0.4048202666228151,
+                    "timestamp": 1.2449064598473756
+                },
+                {
+                    "x": 3.793605089187622,
+                    "y": 6.363643169403076,
+                    "heading": 0.053979860367415415,
+                    "angularVelocity": -8.292676841260746e-8,
+                    "velocityX": 3.718768778627541,
+                    "velocityY": -0.6938239671072872,
+                    "timestamp": 1.2799397941283377
+                },
+                {
+                    "x": 4.111061973016203,
+                    "y": 6.363890421291427,
+                    "heading": 0.05397985774152245,
+                    "angularVelocity": -3.129115871783651e-8,
+                    "velocityX": 3.7829393158349456,
+                    "velocityY": 0.002946349368989899,
+                    "timestamp": 1.3638578423242849
+                },
+                {
+                    "x": 4.423042820635495,
+                    "y": 6.422600616815994,
+                    "heading": 0.053979855524806185,
+                    "angularVelocity": -2.641525047123925e-8,
+                    "velocityX": 3.7176847451316233,
+                    "velocityY": 0.6996134536813446,
+                    "timestamp": 1.447775890520232
+                },
+                {
+                    "x": 4.7253380303410815,
+                    "y": 6.519536384153899,
+                    "heading": 0.05397985339265137,
+                    "angularVelocity": -2.5407583503873633e-8,
+                    "velocityX": 3.602266928321921,
+                    "velocityY": 1.1551241886794503,
+                    "timestamp": 1.5316939387161792
+                },
+                {
+                    "x": 5.027633110558163,
+                    "y": 6.616472555302148,
+                    "heading": 0.053979851260496835,
+                    "angularVelocity": -2.5407580227573106e-8,
+                    "velocityX": 3.6022653852867035,
+                    "velocityY": 1.1551290006400656,
+                    "timestamp": 1.6156119869121264
+                },
+                {
+                    "x": 5.329928190774575,
+                    "y": 6.71340872645249,
+                    "heading": 0.05397984912834238,
+                    "angularVelocity": -2.540757910781892e-8,
+                    "velocityX": 3.602265385278706,
+                    "velocityY": 1.1551290006650068,
+                    "timestamp": 1.6995300351080735
+                },
+                {
+                    "x": 5.632223270990985,
+                    "y": 6.810344897602833,
+                    "heading": 0.053979846996187915,
+                    "angularVelocity": -2.5407579299100972e-8,
+                    "velocityX": 3.602265385278706,
+                    "velocityY": 1.155129000665007,
+                    "timestamp": 1.7834480833040207
+                },
+                {
+                    "x": 5.934518351207396,
+                    "y": 6.907281068753174,
+                    "heading": 0.05397984486403343,
+                    "angularVelocity": -2.5407579624099417e-8,
+                    "velocityX": 3.602265385278706,
+                    "velocityY": 1.1551290006650068,
+                    "timestamp": 1.8673661314999679
+                },
+                {
+                    "x": 6.236813431423807,
+                    "y": 7.004217239903516,
+                    "heading": 0.053979842731878905,
+                    "angularVelocity": -2.5407579955507513e-8,
+                    "velocityX": 3.602265385278706,
+                    "velocityY": 1.1551290006650072,
+                    "timestamp": 1.951284179695915
+                },
+                {
+                    "x": 6.539108511640218,
+                    "y": 7.1011534110538586,
+                    "heading": 0.053979840599724424,
+                    "angularVelocity": -2.5407579566600852e-8,
+                    "velocityX": 3.6022653852787063,
+                    "velocityY": 1.155129000665007,
+                    "timestamp": 2.0352022278918622
+                },
+                {
+                    "x": 6.841403591857615,
+                    "y": 7.198089582201063,
+                    "heading": 0.05397983846756001,
+                    "angularVelocity": -2.5407697930922655e-8,
+                    "velocityX": 3.6022653852904623,
+                    "velocityY": 1.1551290006276251,
+                    "timestamp": 2.119120276087809
+                },
+                {
+                    "x": 7.120935706136836,
+                    "y": 7.287726381323771,
+                    "heading": 0.035986533919804255,
+                    "angularVelocity": -0.21441519356800662,
+                    "velocityX": 3.331013057245073,
+                    "velocityY": 1.0681468533849583,
+                    "timestamp": 2.203038324283756
+                },
+                {
+                    "x": 7.344561416731701,
+                    "y": 7.359435826630496,
+                    "heading": 0.021591909406666148,
+                    "angularVelocity": -0.17153192695269756,
+                    "velocityX": 2.664810674250942,
+                    "velocityY": 0.8545175543082761,
+                    "timestamp": 2.286956372479703
+                },
+                {
+                    "x": 7.512280706030795,
+                    "y": 7.413217912582047,
+                    "heading": 0.010795955954566746,
+                    "angularVelocity": -0.12864876727003446,
+                    "velocityX": 1.9986080813923648,
+                    "velocityY": 0.6408881892244497,
+                    "timestamp": 2.37087442067565
+                },
                 {
-                    "x": 7.666034942577791,
-                    "y": 7.46234355914255,
-                    "heading": -0.013541291231896178,
-                    "angularVelocity": 0.6190420938664342,
-                    "velocityX": 0.655333774251397,
-                    "velocityY": 0.2187821467102802,
-                    "timestamp": 2.413958613122733
+                    "x": 7.624093568104708,
+                    "y": 7.449072637343766,
+                    "heading": 0.0035986551256689905,
+                    "angularVelocity": -0.08576582729978673,
+                    "velocityX": 1.3324054178766367,
+                    "velocityY": 0.42725880227814755,
+                    "timestamp": 2.454792468871597
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 0,
-                    "angularVelocity": 0.31729069510085045,
-                    "velocityX": 0.3272201077973249,
-                    "velocityY": 0.10910668200355174,
-                    "timestamp": 2.4566364837347034
+                    "heading": -6.709954361314771e-31,
+                    "angularVelocity": -0.04288296979061891,
+                    "velocityX": 0.6662027191665668,
+                    "velocityY": 0.21362940442053102,
+                    "timestamp": 2.538710517067544
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 5.5875375349932e-23,
-                    "velocityY": -3.709612464642882e-23,
-                    "timestamp": 2.499314354346674
+                    "heading": 6.378058636012386e-33,
+                    "angularVelocity": 8.071845383822447e-30,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 2.622628565263491
                 }
             ],
             "constraints": [
@@ -7444,7 +8389,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 40
+                    "controlIntervalCount": 18
+                },
+                {
+                    "x": 2.837061882019043,
+                    "y": 6.303859233856201,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 28
                 },
                 {
                     "x": 5.7195634841918945,
@@ -7471,603 +8425,657 @@
                     "y": 4.36,
                     "heading": -1.04,
                     "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
+                    "velocityX": -4.815368277433195e-31,
+                    "velocityY": 1.7671411030208604e-30,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7226799898554153,
-                    "y": 4.366381680411806,
-                    "heading": -1.026991220057009,
-                    "angularVelocity": 0.3105337423596819,
-                    "velocityX": 0.3026851649532591,
-                    "velocityY": 0.15233766037290783,
-                    "timestamp": 0.04189167928786116
-                },
-                {
-                    "x": 0.7480755502425919,
-                    "y": 4.379165077654378,
-                    "heading": -1.001621000893582,
-                    "angularVelocity": 0.6056147567896238,
-                    "velocityX": 0.6062196793943158,
-                    "velocityY": 0.3051536118838673,
-                    "timestamp": 0.08378335857572232
-                },
-                {
-                    "x": 0.7862289943958503,
-                    "y": 4.398373013543058,
-                    "heading": -0.964694126725153,
-                    "angularVelocity": 0.8814846956762871,
-                    "velocityX": 0.9107642568130221,
-                    "velocityY": 0.4585143449774818,
-                    "timestamp": 0.12567503786358347
-                },
-                {
-                    "x": 0.8371909372554502,
-                    "y": 4.424030289360705,
-                    "heading": -0.9172220550063085,
-                    "angularVelocity": 1.1332100437568369,
-                    "velocityX": 1.2165170679697972,
-                    "velocityY": 0.6124671116987632,
-                    "timestamp": 0.16756671715144464
-                },
-                {
-                    "x": 0.9010212440299602,
-                    "y": 4.456163358075127,
-                    "heading": -0.8604944995891576,
-                    "angularVelocity": 1.354148517832005,
-                    "velocityX": 1.523698926841682,
-                    "velocityY": 0.767051339565964,
-                    "timestamp": 0.2094583964393058
-                },
-                {
-                    "x": 0.9777886193671033,
-                    "y": 4.494801080149917,
-                    "heading": -0.796206518536166,
-                    "angularVelocity": 1.5346241102256366,
-                    "velocityX": 1.8325208404664681,
-                    "velocityY": 0.9223244981249834,
-                    "timestamp": 0.25135007572716694
-                },
-                {
-                    "x": 1.0675667159355893,
-                    "y": 4.539976112871383,
-                    "heading": -0.7267376359019178,
-                    "angularVelocity": 1.658297872398206,
-                    "velocityX": 2.143100923492954,
-                    "velocityY": 1.07837722166836,
-                    "timestamp": 0.2932417550150281
-                },
-                {
-                    "x": 1.1704147696438687,
-                    "y": 4.591719023811467,
-                    "heading": -0.6558650979752687,
-                    "angularVelocity": 1.6918046526529587,
-                    "velocityX": 2.4550950321555027,
-                    "velocityY": 1.23515962643868,
-                    "timestamp": 0.3351334343028892
-                },
-                {
-                    "x": 1.286251945776662,
-                    "y": 4.64999734523705,
-                    "heading": -0.5908433350340152,
-                    "angularVelocity": 1.5521402829056499,
-                    "velocityX": 2.765159528144268,
-                    "velocityY": 1.3911669910657207,
-                    "timestamp": 0.37702511359075036
-                },
-                {
-                    "x": 1.4132089603720126,
-                    "y": 4.714842752574763,
-                    "heading": -0.5508840698501896,
-                    "angularVelocity": 0.9538711711517488,
-                    "velocityX": 3.0306021805179513,
-                    "velocityY": 1.547930482617412,
-                    "timestamp": 0.4189167928786115
-                },
-                {
-                    "x": 1.550306465812279,
-                    "y": 4.786404286835006,
-                    "heading": -0.541265046364201,
-                    "angularVelocity": 0.22961656466170327,
-                    "velocityX": 3.272666738857444,
-                    "velocityY": 1.7082517453765305,
-                    "timestamp": 0.46080847216647264
-                },
-                {
-                    "x": 1.6919561664396436,
-                    "y": 4.857462558344818,
-                    "heading": -0.5412650016128938,
-                    "angularVelocity": 0.000001068262432444083,
-                    "velocityX": 3.3813325948098276,
-                    "velocityY": 1.6962383155263616,
-                    "timestamp": 0.5027001514543338
-                },
-                {
-                    "x": 1.8336059249814771,
-                    "y": 4.928520714406238,
-                    "heading": -0.541264956861776,
-                    "angularVelocity": 0.0000010682579123299043,
-                    "velocityX": 3.3813339772912605,
-                    "velocityY": 1.6962355596475316,
-                    "timestamp": 0.544591830742195
-                },
-                {
-                    "x": 1.9752556835258195,
-                    "y": 4.999578870462656,
-                    "heading": -0.5412649121106583,
-                    "angularVelocity": 0.0000010682579080478101,
-                    "velocityX": 3.3813339773511535,
-                    "velocityY": 1.696235559528139,
-                    "timestamp": 0.5864835100300562
-                },
-                {
-                    "x": 2.1169054420701623,
-                    "y": 5.0706370265190746,
-                    "heading": -0.5412648673595408,
-                    "angularVelocity": 0.0000010682579032833847,
-                    "velocityX": 3.381333977351157,
-                    "velocityY": 1.6962355595281329,
-                    "timestamp": 0.6283751893179174
-                },
-                {
-                    "x": 2.258555200614505,
-                    "y": 5.141695182575493,
-                    "heading": -0.5412648226084235,
-                    "angularVelocity": 0.0000010682578992307337,
-                    "velocityX": 3.381333977351157,
-                    "velocityY": 1.6962355595281327,
-                    "timestamp": 0.6702668686057786
-                },
-                {
-                    "x": 2.400204959158848,
-                    "y": 5.212753338631911,
-                    "heading": -0.5412647778573064,
-                    "angularVelocity": 0.0000010682578937479676,
-                    "velocityX": 3.3813339773511575,
-                    "velocityY": 1.696235559528132,
-                    "timestamp": 0.7121585478936397
-                },
-                {
-                    "x": 2.5418547177031905,
-                    "y": 5.2838114946883294,
-                    "heading": -0.5412647331061895,
-                    "angularVelocity": 0.0000010682578893581349,
-                    "velocityX": 3.3813339773511584,
-                    "velocityY": 1.696235559528132,
-                    "timestamp": 0.7540502271815009
-                },
-                {
-                    "x": 2.6835044762475335,
-                    "y": 5.354869650744748,
-                    "heading": -0.5412646883550728,
-                    "angularVelocity": 0.0000010682578845109594,
-                    "velocityX": 3.3813339773511584,
-                    "velocityY": 1.6962355595281313,
-                    "timestamp": 0.7959419064693621
-                },
-                {
-                    "x": 2.8251542347918766,
-                    "y": 5.425927806801167,
-                    "heading": -0.5412646436039563,
-                    "angularVelocity": 0.0000010682578796288815,
-                    "velocityX": 3.3813339773511593,
-                    "velocityY": 1.6962355595281313,
-                    "timestamp": 0.8378335857572233
-                },
-                {
-                    "x": 2.9668039933362196,
-                    "y": 5.496985962857585,
-                    "heading": -0.5412645988528401,
-                    "angularVelocity": 0.0000010682578747880459,
-                    "velocityX": 3.38133397735116,
-                    "velocityY": 1.6962355595281309,
-                    "timestamp": 0.8797252650450845
-                },
-                {
-                    "x": 3.108453751880562,
-                    "y": 5.5680441189140035,
-                    "heading": -0.541264554101724,
-                    "angularVelocity": 0.0000010682578699288912,
-                    "velocityX": 3.38133397735116,
-                    "velocityY": 1.69623555952813,
-                    "timestamp": 0.9216169443329457
-                },
-                {
-                    "x": 3.250103510424905,
-                    "y": 5.639102274970423,
-                    "heading": -0.5412645093506081,
-                    "angularVelocity": 0.000001068257865104235,
-                    "velocityX": 3.3813339773511606,
-                    "velocityY": 1.69623555952813,
-                    "timestamp": 0.9635086236208069
-                },
-                {
-                    "x": 3.3917532689692482,
-                    "y": 5.71016043102684,
-                    "heading": -0.5412644645994924,
-                    "angularVelocity": 0.0000010682578613220535,
-                    "velocityX": 3.381333977351161,
-                    "velocityY": 1.6962355595281293,
-                    "timestamp": 1.005400302908668
-                },
-                {
-                    "x": 3.5334030275135913,
-                    "y": 5.7812185870832575,
-                    "heading": -0.5412644198483769,
-                    "angularVelocity": 0.0000010682578558700584,
-                    "velocityX": 3.381333977351162,
-                    "velocityY": 1.6962355595281287,
-                    "timestamp": 1.0472919821965292
-                },
-                {
-                    "x": 3.675052786057934,
-                    "y": 5.852276743139676,
-                    "heading": -0.5412643750972616,
-                    "angularVelocity": 0.0000010682578513840427,
-                    "velocityX": 3.381333977351162,
-                    "velocityY": 1.6962355595281287,
-                    "timestamp": 1.0891836614843904
-                },
-                {
-                    "x": 3.816702544602277,
-                    "y": 5.923334899196093,
-                    "heading": -0.5412643303461464,
-                    "angularVelocity": 0.0000010682578474926016,
-                    "velocityX": 3.381333977351163,
-                    "velocityY": 1.6962355595281278,
-                    "timestamp": 1.1310753407722516
-                },
-                {
-                    "x": 3.95835230314662,
-                    "y": 5.994393055252511,
-                    "heading": -0.5412642855950315,
-                    "angularVelocity": 0.000001068257842976485,
-                    "velocityX": 3.3813339773511637,
-                    "velocityY": 1.6962355595281275,
-                    "timestamp": 1.1729670200601128
-                },
-                {
-                    "x": 4.100002061690963,
-                    "y": 6.065451211308929,
-                    "heading": -0.5412642408439168,
-                    "angularVelocity": 0.0000010682578369198447,
-                    "velocityX": 3.3813339773511637,
-                    "velocityY": 1.6962355595281269,
-                    "timestamp": 1.214858699347974
-                },
-                {
-                    "x": 4.2416518202353055,
-                    "y": 6.136509367365346,
-                    "heading": -0.5412641960928023,
-                    "angularVelocity": 0.0000010682578319796932,
-                    "velocityX": 3.3813339773511646,
-                    "velocityY": 1.696235559528127,
-                    "timestamp": 1.2567503786358352
-                },
-                {
-                    "x": 4.383301578779649,
-                    "y": 6.207567523421765,
-                    "heading": -0.5412641513416879,
-                    "angularVelocity": 0.0000010682578277344439,
-                    "velocityX": 3.381333977351165,
-                    "velocityY": 1.6962355595281264,
-                    "timestamp": 1.2986420579236964
-                },
-                {
-                    "x": 4.524951337323992,
-                    "y": 6.278625679478183,
-                    "heading": -0.5412641065905739,
-                    "angularVelocity": 0.0000010682578220378627,
-                    "velocityX": 3.3813339773511655,
-                    "velocityY": 1.6962355595281264,
-                    "timestamp": 1.3405337372115576
-                },
-                {
-                    "x": 4.666601095868337,
-                    "y": 6.349683835534597,
-                    "heading": -0.54126406183946,
-                    "angularVelocity": 0.0000010682578180389232,
-                    "velocityX": 3.381333977351213,
-                    "velocityY": 1.6962355595280314,
-                    "timestamp": 1.3824254164994187
-                },
-                {
-                    "x": 4.8082508544580715,
-                    "y": 6.420741991500528,
-                    "heading": -0.5412640170883463,
-                    "angularVelocity": 0.000001068257814314864,
-                    "velocityX": 3.3813339784347316,
-                    "velocityY": 1.6962355573681103,
-                    "timestamp": 1.42431709578728
-                },
-                {
-                    "x": 4.949901660762853,
-                    "y": 6.491798058877407,
-                    "heading": -0.54126397233727,
-                    "angularVelocity": 0.0000010682569195340074,
-                    "velocityX": 3.3813589885337056,
-                    "velocityY": 1.6961857004731962,
-                    "timestamp": 1.4662087750751411
-                },
-                {
-                    "x": 5.0966757016987385,
-                    "y": 6.551558383908352,
-                    "heading": -0.5412639273749705,
-                    "angularVelocity": 0.000001073299046263079,
-                    "velocityX": 3.503656177813225,
-                    "velocityY": 1.4265440308634512,
-                    "timestamp": 1.5081004543630023
-                },
-                {
-                    "x": 5.2483360143420885,
-                    "y": 6.597526467706988,
-                    "heading": -0.5412638804954443,
-                    "angularVelocity": 0.000001119065339105663,
-                    "velocityX": 3.6202968040790893,
-                    "velocityY": 1.097308214425216,
-                    "timestamp": 1.5499921336508635
-                },
-                {
-                    "x": 5.403589728009299,
-                    "y": 6.629310092724924,
-                    "heading": -0.5412638294474256,
-                    "angularVelocity": 0.0000012185717939310704,
-                    "velocityX": 3.7060751993343666,
-                    "velocityY": 0.7587097380253649,
-                    "timestamp": 1.5918838129387247
-                },
-                {
-                    "x": 5.561113217492597,
-                    "y": 6.646638205385644,
-                    "heading": -0.54126377105557,
-                    "angularVelocity": 0.0000013938771748607603,
-                    "velocityX": 3.760257219598817,
-                    "velocityY": 0.4136409176067717,
-                    "timestamp": 1.633775492226586
+                    "x": 0.7256756209676614,
+                    "y": 4.374259536946564,
+                    "heading": -0.9853384433657985,
+                    "angularVelocity": 0.9927336490852509,
+                    "velocityX": 0.2846921559393299,
+                    "velocityY": 0.25897400328871373,
+                    "timestamp": 0.05506165393280401
+                },
+                {
+                    "x": 0.7571509327835594,
+                    "y": 4.40368458563067,
+                    "heading": -0.8810495211913171,
+                    "angularVelocity": 1.8940390403219183,
+                    "velocityX": 0.5716376019938672,
+                    "velocityY": 0.5344018310849077,
+                    "timestamp": 0.11012330786560802
+                },
+                {
+                    "x": 0.8047782151983651,
+                    "y": 4.4492655723711865,
+                    "heading": -0.7351323547259855,
+                    "angularVelocity": 2.6500687146592434,
+                    "velocityX": 0.8649809624852585,
+                    "velocityY": 0.8278172463956943,
+                    "timestamp": 0.16518496179841202
+                },
+                {
+                    "x": 0.8693373364829333,
+                    "y": 4.511878147553763,
+                    "heading": -0.5600607568180229,
+                    "angularVelocity": 3.179555741677643,
+                    "velocityX": 1.1724878690220806,
+                    "velocityY": 1.1371357507548814,
+                    "timestamp": 0.22024661573121604
+                },
+                {
+                    "x": 0.9516435423656624,
+                    "y": 4.592143379904835,
+                    "heading": -0.3723491539523138,
+                    "angularVelocity": 3.4091166802965285,
+                    "velocityX": 1.4948008278694735,
+                    "velocityY": 1.4577337696599522,
+                    "timestamp": 0.27530826966402006
+                },
+                {
+                    "x": 1.0514451430006502,
+                    "y": 4.68995617229961,
+                    "heading": -0.20247463803020274,
+                    "angularVelocity": 3.085169147472857,
+                    "velocityX": 1.8125427317637794,
+                    "velocityY": 1.7764230713854863,
+                    "timestamp": 0.33036992359682404
+                },
+                {
+                    "x": 1.163482365369112,
+                    "y": 4.8045671318094305,
+                    "heading": -0.08453465328971378,
+                    "angularVelocity": 2.1419622607883397,
+                    "velocityX": 2.034759480799993,
+                    "velocityY": 2.0815023037586085,
+                    "timestamp": 0.38543157752962803
+                },
+                {
+                    "x": 1.290791904505612,
+                    "y": 4.932877435439672,
+                    "heading": -0.017113580237308034,
+                    "angularVelocity": 1.2244650902572813,
+                    "velocityX": 2.3121270438301877,
+                    "velocityY": 2.330302387705078,
+                    "timestamp": 0.440493231462432
+                },
+                {
+                    "x": 1.4344972301022125,
+                    "y": 5.07384452901374,
+                    "heading": -0.000001124802148976757,
+                    "angularVelocity": 0.3107871669853269,
+                    "velocityX": 2.6098984562178145,
+                    "velocityY": 2.560168166154319,
+                    "timestamp": 0.495554885395236
+                },
+                {
+                    "x": 1.5830888632571813,
+                    "y": 5.219814728179112,
+                    "heading": -9.965376040774715e-7,
+                    "angularVelocity": 0.0000023294713396498257,
+                    "velocityX": 2.6986409332390493,
+                    "velocityY": 2.6510318659074916,
+                    "timestamp": 0.55061653932804
+                },
+                {
+                    "x": 1.7316392407262138,
+                    "y": 5.365826918457485,
+                    "heading": -8.792165857555993e-7,
+                    "angularVelocity": 0.000002130720927018926,
+                    "velocityX": 2.697891669756242,
+                    "velocityY": 2.6517944858193996,
+                    "timestamp": 0.605678193260844
+                },
+                {
+                    "x": 1.8801815584157808,
+                    "y": 5.511847308171012,
+                    "heading": -7.618956854973843e-7,
+                    "angularVelocity": 0.000002130718782810582,
+                    "velocityX": 2.69774529239705,
+                    "velocityY": 2.6519433995297197,
+                    "timestamp": 0.6607398471936481
+                },
+                {
+                    "x": 2.0287247432411455,
+                    "y": 5.657866815749449,
+                    "heading": -6.445747704723799e-7,
+                    "angularVelocity": 0.000002130719050997138,
+                    "velocityX": 2.697761040854699,
+                    "velocityY": 2.651927378662928,
+                    "timestamp": 0.7158015011264521
+                },
+                {
+                    "x": 2.1772589110251723,
+                    "y": 5.8038954956427204,
+                    "heading": -5.272539834240943e-7,
+                    "angularVelocity": 0.000002130716726753334,
+                    "velocityX": 2.6975972782370956,
+                    "velocityY": 2.652093961273629,
+                    "timestamp": 0.7708631550592562
+                },
+                {
+                    "x": 2.325792733071576,
+                    "y": 5.949924527226785,
+                    "heading": -4.0993217710172413e-7,
+                    "angularVelocity": 0.000002130735238257545,
+                    "velocityX": 2.6975909991452314,
+                    "velocityY": 2.6521003484836045,
+                    "timestamp": 0.8259248089920602
+                },
+                {
+                    "x": 2.481459175645397,
+                    "y": 6.088325151306142,
+                    "heading": -2.908915465695403e-7,
+                    "angularVelocity": 0.0000021619515945987695,
+                    "velocityX": 2.8271297982536217,
+                    "velocityY": 2.5135573342462116,
+                    "timestamp": 0.8809864629248643
+                },
+                {
+                    "x": 2.6527099574524438,
+                    "y": 6.206899777175588,
+                    "heading": -1.5899541452377036e-7,
+                    "angularVelocity": 0.0000023954262651640655,
+                    "velocityX": 3.1101641446636745,
+                    "velocityY": 2.1534882699533155,
+                    "timestamp": 0.9360481168576683
+                },
+                {
+                    "x": 2.837061882019043,
+                    "y": 6.303859233856201,
+                    "heading": 0,
+                    "angularVelocity": 0.0000028875887873359224,
+                    "velocityX": 3.348100018792042,
+                    "velocityY": 1.760925249319746,
+                    "timestamp": 0.9911097707904724
+                },
+                {
+                    "x": 2.932152988543463,
+                    "y": 6.346714053639414,
+                    "heading": 2.0172187169325966e-12,
+                    "angularVelocity": 7.316288159012044e-11,
+                    "velocityX": 3.4488770423117883,
+                    "velocityY": 1.5543094355402358,
+                    "timestamp": 1.018681384784641
+                },
+                {
+                    "x": 3.0296712234656686,
+                    "y": 6.383714706348048,
+                    "heading": 2.030359324162707e-12,
+                    "angularVelocity": 4.765991109637323e-13,
+                    "velocityX": 3.5369070139469976,
+                    "velocityY": 1.3419835602475594,
+                    "timestamp": 1.0462529987788096
+                },
+                {
+                    "x": 3.129255935132477,
+                    "y": 6.414726637699066,
+                    "heading": 2.0283850653761223e-12,
+                    "angularVelocity": -7.160477709378183e-14,
+                    "velocityX": 3.611856443643096,
+                    "velocityY": 1.1247775106178561,
+                    "timestamp": 1.0738246127729782
+                },
+                {
+                    "x": 3.2305380260225394,
+                    "y": 6.439642353966136,
+                    "heading": 2.0263773134602995e-12,
+                    "angularVelocity": -7.281954448876825e-14,
+                    "velocityX": 3.6734190066355246,
+                    "velocityY": 0.9036727510191612,
+                    "timestamp": 1.1013962267671469
+                },
+                {
+                    "x": 3.3331381427053266,
+                    "y": 6.458405909393185,
+                    "heading": 2.0244535837319738e-12,
+                    "angularVelocity": -6.977212868461473e-14,
+                    "velocityX": 3.7212227294451083,
+                    "velocityY": 0.6805388843797584,
+                    "timestamp": 1.1289678407613155
+                },
+                {
+                    "x": 3.43663787164561,
+                    "y": 6.471315638605137,
+                    "heading": 2.022533482460756e-12,
+                    "angularVelocity": -6.96405259775829e-14,
+                    "velocityX": 3.753850933863969,
+                    "velocityY": 0.4682253717820687,
+                    "timestamp": 1.1565394547554841
+                },
+                {
+                    "x": 3.5403567621307404,
+                    "y": 6.4823268492843,
+                    "heading": 2.0206101184182045e-12,
+                    "angularVelocity": -6.975886505788547e-14,
+                    "velocityX": 3.7617997447283904,
+                    "velocityY": 0.39936764973541705,
+                    "timestamp": 1.1841110687496528
+                },
+                {
+                    "x": 3.6440832149520914,
+                    "y": 6.493266592802732,
+                    "heading": 2.018711397348351e-12,
+                    "angularVelocity": -6.886508434036338e-14,
+                    "velocityX": 3.7620740245091877,
+                    "velocityY": 0.3967755939732401,
+                    "timestamp": 1.2116826827438214
+                },
+                {
+                    "x": 3.747806936839693,
+                    "y": 6.504232199027323,
+                    "heading": 2.016809036122773e-12,
+                    "angularVelocity": -6.89971097222047e-14,
+                    "velocityX": 3.7619749757661194,
+                    "velocityY": 0.39771361326329024,
+                    "timestamp": 1.23925429673799
+                },
+                {
+                    "x": 3.8515283310140283,
+                    "y": 6.515219800794399,
+                    "heading": 2.0149121825042556e-12,
+                    "angularVelocity": -6.87973533298754e-14,
+                    "velocityX": 3.761890551500436,
+                    "velocityY": 0.3985113736873028,
+                    "timestamp": 1.2668259107321587
+                },
+                {
+                    "x": 3.9552475387122907,
+                    "y": 6.52622802304771,
+                    "heading": 2.0130014782113644e-12,
+                    "angularVelocity": -6.92997060168648e-14,
+                    "velocityX": 3.7618112497917684,
+                    "velocityY": 0.3992592619503583,
+                    "timestamp": 1.2943975247263273
+                },
+                {
+                    "x": 4.058964880445898,
+                    "y": 6.537253812271398,
+                    "heading": 2.011079593605957e-12,
+                    "angularVelocity": -6.970520685257517e-14,
+                    "velocityX": 3.7617435727707904,
+                    "velocityY": 0.3998964016833969,
+                    "timestamp": 1.321969138720496
+                },
+                {
+                    "x": 4.162680825175896,
+                    "y": 6.5482927349999205,
+                    "heading": 2.009190619489212e-12,
+                    "angularVelocity": -6.851157050326969e-14,
+                    "velocityX": 3.761692904585482,
+                    "velocityY": 0.4003727432280311,
+                    "timestamp": 1.3495407527146646
+                },
+                {
+                    "x": 4.2663959076467215,
+                    "y": 6.559339756077265,
+                    "heading": 2.007295781849634e-12,
+                    "angularVelocity": -6.87242357042844e-14,
+                    "velocityX": 3.7616616311467697,
+                    "velocityY": 0.40066646375103865,
+                    "timestamp": 1.3771123667088332
+                },
+                {
+                    "x": 4.370110729285335,
+                    "y": 6.570389225581033,
+                    "heading": 2.0053851186045697e-12,
+                    "angularVelocity": -6.929821753536312e-14,
+                    "velocityX": 3.761652170976742,
+                    "velocityY": 0.40075526616572615,
+                    "timestamp": 1.4046839807030018
+                },
+                {
+                    "x": 4.473826107172322,
+                    "y": 6.581433472559441,
+                    "heading": 2.003480405786031e-12,
+                    "angularVelocity": -6.908240040814547e-14,
+                    "velocityX": 3.7616723456572294,
+                    "velocityY": 0.4005658493780133,
+                    "timestamp": 1.4322555946971705
+                },
+                {
+                    "x": 4.577543170052599,
+                    "y": 6.592461884602437,
+                    "heading": 2.001567952942652e-12,
+                    "angularVelocity": -6.936312480979614e-14,
+                    "velocityX": 3.7617334589969245,
+                    "velocityY": 0.399991529116124,
+                    "timestamp": 1.459827208691339
+                },
+                {
+                    "x": 4.681263099195036,
+                    "y": 6.603463307611616,
+                    "heading": 1.9996669629945417e-12,
+                    "angularVelocity": -6.894737463787157e-14,
+                    "velocityX": 3.761837415995553,
+                    "velocityY": 0.3990126587978969,
+                    "timestamp": 1.4873988226855077
+                },
+                {
+                    "x": 4.784986899417181,
+                    "y": 6.61442817334549,
+                    "heading": 1.997767883198675e-12,
+                    "angularVelocity": -6.88780949705506e-14,
+                    "velocityX": 3.7619778169075735,
+                    "velocityY": 0.3976867561604882,
+                    "timestamp": 1.5149704366796763
+                },
+                {
+                    "x": 4.888715211119033,
+                    "y": 6.625350277801733,
+                    "heading": 1.9958591963322718e-12,
+                    "angularVelocity": -6.922653588980761e-14,
+                    "velocityX": 3.762141444605259,
+                    "velocityY": 0.39613583941828256,
+                    "timestamp": 1.542542050673845
+                },
+                {
+                    "x": 4.992447807319009,
+                    "y": 6.636231614926857,
+                    "heading": 1.9939696519007763e-12,
+                    "angularVelocity": -6.853225532454514e-14,
+                    "velocityX": 3.7622968398643226,
+                    "velocityY": 0.3946572415062904,
+                    "timestamp": 1.5701136646680136
+                },
+                {
+                    "x": 5.09618354900695,
+                    "y": 6.6470829236468765,
+                    "heading": 1.992073077512808e-12,
+                    "angularVelocity": -6.878722613480481e-14,
+                    "velocityX": 3.7624109241486314,
+                    "velocityY": 0.3935681357313447,
+                    "timestamp": 1.5976852786621822
+                },
+                {
+                    "x": 5.199926346512542,
+                    "y": 6.657866567184633,
+                    "heading": 1.990167650654994e-12,
+                    "angularVelocity": -6.910829798877678e-14,
+                    "velocityX": 3.7626668329101425,
+                    "velocityY": 0.3911139746618113,
+                    "timestamp": 1.6252568926563509
+                },
+                {
+                    "x": 5.303729435372611,
+                    "y": 6.668053506977846,
+                    "heading": 1.9882412477007472e-12,
+                    "angularVelocity": -6.986908358991098e-14,
+                    "velocityX": 3.764853551267859,
+                    "velocityY": 0.3694720154839007,
+                    "timestamp": 1.6528285066505195
+                },
+                {
+                    "x": 5.40792279202557,
+                    "y": 6.672807389271601,
+                    "heading": 1.9863066963577084e-12,
+                    "angularVelocity": -7.016461895491627e-14,
+                    "velocityX": 3.779008246489486,
+                    "velocityY": 0.17241944175311613,
+                    "timestamp": 1.6804001206446881
+                },
+                {
+                    "x": 5.512213692786393,
+                    "y": 6.671303096171843,
+                    "heading": 1.984343863250107e-12,
+                    "angularVelocity": -7.119037640734205e-14,
+                    "velocityX": 3.782546091892586,
+                    "velocityY": -0.05455948646048947,
+                    "timestamp": 1.7079717346388568
+                },
+                {
+                    "x": 5.616221821617454,
+                    "y": 6.663482363169505,
+                    "heading": 1.9823084185583715e-12,
+                    "angularVelocity": -7.382393974706967e-14,
+                    "velocityX": 3.772290184136858,
+                    "velocityY": -0.2836516209983201,
+                    "timestamp": 1.7355433486330254
                 },
                 {
                     "x": 5.7195634841918945,
                     "y": 6.649363040924072,
-                    "heading": -0.5412637028328066,
-                    "angularVelocity": 0.000001628551649307522,
-                    "velocityX": 3.7823804009024578,
-                    "velocityY": 0.06504479134637085,
-                    "timestamp": 1.675667171514447
-                },
-                {
-                    "x": 5.8386728592662305,
-                    "y": 6.64312635201416,
-                    "heading": -0.5412636336326876,
-                    "angularVelocity": 0.000002194803906563273,
-                    "velocityX": 3.777764046962115,
-                    "velocityY": -0.19780759592812253,
-                    "timestamp": 1.7071962383364254
-                },
-                {
-                    "x": 5.95706146956484,
-                    "y": 6.62863229973747,
-                    "heading": -0.5412635727602775,
-                    "angularVelocity": 0.0000019306759164775456,
-                    "velocityX": 3.7549037200201067,
-                    "velocityY": -0.459704448549871,
-                    "timestamp": 1.7387253051584037
-                },
-                {
-                    "x": 6.0741575637122,
-                    "y": 6.605950895283392,
-                    "heading": -0.5412635175985968,
-                    "angularVelocity": 0.000001749550058024173,
-                    "velocityX": 3.713909289117771,
-                    "velocityY": -0.7193807727372066,
-                    "timestamp": 1.770254371980382
-                },
-                {
-                    "x": 6.189395638406918,
-                    "y": 6.575191700510336,
-                    "heading": -0.5412634663439824,
-                    "angularVelocity": 0.0000016256305579593937,
-                    "velocityX": 3.654978922953296,
-                    "velocityY": -0.9755821492190131,
-                    "timestamp": 1.8017834388023604
-                },
-                {
-                    "x": 6.302219177732501,
-                    "y": 6.536503313694644,
-                    "heading": -0.5412634176727114,
-                    "angularVelocity": 0.0000015436952600896132,
-                    "velocityX": 3.578397672300798,
-                    "velocityY": -1.2270704691051513,
-                    "timestamp": 1.8333125056243387
-                },
-                {
-                    "x": 6.412083374588637,
-                    "y": 6.49007271186475,
-                    "heading": -0.5412633705487488,
-                    "angularVelocity": 0.0000014946196396789507,
-                    "velocityX": 3.484536902930231,
-                    "velocityY": -1.4726284825381617,
-                    "timestamp": 1.864841572446317
-                },
-                {
-                    "x": 6.518458035998333,
-                    "y": 6.436124829047777,
-                    "heading": -0.5412633241017745,
-                    "angularVelocity": 0.0000014731477658308475,
-                    "velocityX": 3.3738601275551834,
-                    "velocityY": -1.7110523163142606,
-                    "timestamp": 1.8963706392682953
-                },
-                {
-                    "x": 6.622870515934444,
-                    "y": 6.378470889852871,
-                    "heading": -0.5412632777559726,
-                    "angularVelocity": 0.0000014699389024112294,
-                    "velocityX": 3.3116260790606757,
-                    "velocityY": -1.8285964351699866,
-                    "timestamp": 1.9278997060902736
-                },
-                {
-                    "x": 6.7272827038996414,
-                    "y": 6.320816421895741,
-                    "heading": -0.5412632314101712,
-                    "angularVelocity": 0.0000014699388892769531,
-                    "velocityX": 3.311616818687876,
-                    "velocityY": -1.8286132057971307,
-                    "timestamp": 1.959428772912252
-                },
-                {
-                    "x": 6.831694893880165,
-                    "y": 6.263161957588452,
-                    "heading": -0.5412631850642508,
-                    "angularVelocity": 0.0000014699426583817394,
-                    "velocityX": 3.311616882607505,
-                    "velocityY": -1.828613090035986,
-                    "timestamp": 1.9909578397342302
-                },
-                {
-                    "x": 6.935462774507961,
-                    "y": 6.205910954727142,
-                    "heading": -0.5387292297574777,
-                    "angularVelocity": 0.08036886473933494,
-                    "velocityX": 3.2911814743423053,
-                    "velocityY": -1.815816598206477,
-                    "timestamp": 2.0224869065562086
-                },
-                {
-                    "x": 7.033610221214887,
-                    "y": 6.151825220973909,
-                    "heading": -0.514724354544779,
-                    "angularVelocity": 0.7613569836442236,
-                    "velocityX": 3.11291949302188,
-                    "velocityY": -1.715424502051262,
-                    "timestamp": 2.054015973378187
-                },
-                {
-                    "x": 7.126006777010342,
-                    "y": 6.101041958255997,
-                    "heading": -0.47044950446923295,
-                    "angularVelocity": 1.4042550109565184,
-                    "velocityX": 2.930519838000589,
-                    "velocityY": -1.610680804625417,
-                    "timestamp": 2.0855450402001656
-                },
-                {
-                    "x": 7.2116589199714145,
-                    "y": 6.053937431211348,
-                    "heading": -0.4144026036835486,
-                    "angularVelocity": 1.7776263757548019,
-                    "velocityX": 2.7166088817245364,
-                    "velocityY": -1.4940032101366643,
-                    "timestamp": 2.117074107022144
-                },
-                {
-                    "x": 7.290084520639544,
-                    "y": 6.01082680132011,
-                    "heading": -0.35575031779267857,
-                    "angularVelocity": 1.8602607626174494,
-                    "velocityX": 2.4874063387585092,
-                    "velocityY": -1.3673297130756235,
-                    "timestamp": 2.1486031738441227
-                },
-                {
-                    "x": 7.3612581622075055,
-                    "y": 5.971723406014521,
-                    "heading": -0.2979353118382045,
-                    "angularVelocity": 1.8337049517168786,
-                    "velocityX": 2.2573976568931324,
-                    "velocityY": -1.2402331958119186,
-                    "timestamp": 2.1801322406661012
-                },
-                {
-                    "x": 7.425208576949436,
-                    "y": 5.936609973900717,
-                    "heading": -0.2429131400794928,
-                    "angularVelocity": 1.7451252861171487,
-                    "velocityX": 2.028300269811719,
-                    "velocityY": -1.1136844712868785,
-                    "timestamp": 2.2116613074880798
-                },
-                {
-                    "x": 7.481970224994158,
-                    "y": 5.90546408447042,
-                    "heading": -0.1919554556036592,
-                    "angularVelocity": 1.616212898515343,
-                    "velocityX": 1.8002958465346786,
-                    "velocityY": -0.9878468527519235,
-                    "timestamp": 2.2431903743100583
-                },
-                {
-                    "x": 7.531573575573894,
-                    "y": 5.8782638409384305,
-                    "heading": -0.14593987362659777,
-                    "angularVelocity": 1.4594653954358323,
-                    "velocityX": 1.5732578087328088,
-                    "velocityY": -0.8627037294052898,
-                    "timestamp": 2.274719441132037
-                },
-                {
-                    "x": 7.574043708984396,
-                    "y": 5.854989674792864,
-                    "heading": -0.10549518757833008,
-                    "angularVelocity": 1.2827745989638484,
-                    "velocityX": 1.347015236774987,
-                    "velocityY": -0.7381812559495933,
-                    "timestamp": 2.3062485079540154
-                },
-                {
-                    "x": 7.60940100035129,
-                    "y": 5.835624772967359,
-                    "heading": -0.07108683169331761,
-                    "angularVelocity": 1.091321734299697,
-                    "velocityX": 1.1214188978865451,
-                    "velocityY": -0.614192038566993,
-                    "timestamp": 2.337777574775994
-                },
-                {
-                    "x": 7.637662176953836,
-                    "y": 5.820154874880602,
-                    "heading": -0.043071374084194446,
-                    "angularVelocity": 0.8885596826352652,
-                    "velocityX": 0.8963530941818176,
-                    "velocityY": -0.4906551206892414,
-                    "timestamp": 2.3693066415979724
-                },
-                {
-                    "x": 7.65884126375683,
-                    "y": 5.808567782468078,
-                    "heading": -0.021732750340195603,
-                    "angularVelocity": 0.6767921126395045,
-                    "velocityX": 0.6717321169883477,
-                    "velocityY": -0.3675050859559891,
-                    "timestamp": 2.400835708419951
-                },
+                    "heading": 1.9801191994112705e-12,
+                    "angularVelocity": -7.94012151883752e-14,
+                    "velocityX": 3.7481179954212767,
+                    "velocityY": -0.5120963266444406,
+                    "timestamp": 1.763114962627194
+                },
+                {
+                    "x": 5.834816669509981,
+                    "y": 6.625475968242897,
+                    "heading": 1.9780897732255602e-12,
+                    "angularVelocity": -6.522540784880574e-14,
+                    "velocityX": 3.704217712719512,
+                    "velocityY": -0.7677264405820726,
+                    "timestamp": 1.794229004580604
+                },
+                {
+                    "x": 5.948163245427426,
+                    "y": 6.593751483162918,
+                    "heading": 1.9761554326166073e-12,
+                    "angularVelocity": -6.216937380396162e-14,
+                    "velocityX": 3.6429396118708874,
+                    "velocityY": -1.0196195379349016,
+                    "timestamp": 1.825343046534014
+                },
+                {
+                    "x": 6.0590772896693,
+                    "y": 6.554356506595256,
+                    "heading": 1.974328708451782e-12,
+                    "angularVelocity": -5.871059954922337e-14,
+                    "velocityX": 3.564758458833813,
+                    "velocityY": -1.2661478256826604,
+                    "timestamp": 1.8564570884874239
+                },
+                {
+                    "x": 6.167114375070259,
+                    "y": 6.507645933449736,
+                    "heading": 1.9725156971860347e-12,
+                    "angularVelocity": -5.826986956344564e-14,
+                    "velocityX": 3.472293492526525,
+                    "velocityY": -1.5012698515774907,
+                    "timestamp": 1.8875711304408338
+                },
+                {
+                    "x": 6.273666318458446,
+                    "y": 6.457640205385319,
+                    "heading": 1.9706978622350046e-12,
+                    "angularVelocity": -5.842490200021065e-14,
+                    "velocityX": 3.424561281617029,
+                    "velocityY": -1.6071755684828801,
+                    "timestamp": 1.9186851723942437
+                },
+                {
+                    "x": 6.3802136872176485,
+                    "y": 6.4076247308783065,
+                    "heading": 1.9688925756308956e-12,
+                    "angularVelocity": -5.802160020867771e-14,
+                    "velocityX": 3.4244142538170945,
+                    "velocityY": -1.6074888174954127,
+                    "timestamp": 1.9497992143476537
+                },
+                {
+                    "x": 6.486760876374776,
+                    "y": 6.3576088737324294,
+                    "heading": 1.967091079960254e-12,
+                    "angularVelocity": -5.789976010142433e-14,
+                    "velocityX": 3.4244084814345546,
+                    "velocityY": -1.607501115448455,
+                    "timestamp": 1.9809132563010636
+                },
+                {
+                    "x": 6.593299366426369,
+                    "y": 6.307574489358799,
+                    "heading": 1.9652854794360616e-12,
+                    "angularVelocity": -5.803168954665514e-14,
+                    "velocityX": 3.4241288936675214,
+                    "velocityY": -1.6080965773861522,
+                    "timestamp": 2.0120272982544734
+                },
+                {
+                    "x": 6.6998332452145375,
+                    "y": 6.257530287401571,
+                    "heading": 1.963481306814657e-12,
+                    "angularVelocity": -5.79857968421928e-14,
+                    "velocityX": 3.423980688450087,
+                    "velocityY": -1.608412112843002,
+                    "timestamp": 2.043141340207883
+                },
+                {
+                    "x": 6.806367665236628,
+                    "y": 6.207487237643806,
+                    "heading": 1.9616709532128727e-12,
+                    "angularVelocity": -5.818445266643988e-14,
+                    "velocityX": 3.4239980836191717,
+                    "velocityY": -1.6083750813421427,
+                    "timestamp": 2.074255382161293
+                },
+                {
+                    "x": 6.912905936531216,
+                    "y": 6.157452377320687,
+                    "heading": 1.9503301873398137e-12,
+                    "angularVelocity": -3.6449015032516537e-13,
+                    "velocityX": 3.424121862863347,
+                    "velocityY": -1.6081118743030238,
+                    "timestamp": 2.1053694241147025
+                },
+                {
+                    "x": 7.015182351606042,
+                    "y": 6.109386281530938,
+                    "heading": 7.52788173701755e-9,
+                    "angularVelocity": 2.418821392510085e-7,
+                    "velocityX": 3.287146531073864,
+                    "velocityY": -1.5448361181009935,
+                    "timestamp": 2.1364834660681122
+                },
+                {
+                    "x": 7.110154929626141,
+                    "y": 6.064756817323096,
+                    "heading": 9.121631465411093e-9,
+                    "angularVelocity": 5.122284110672045e-8,
+                    "velocityX": 3.0524024542461334,
+                    "velocityY": -1.4343833653841132,
+                    "timestamp": 2.167597508021522
+                },
+                {
+                    "x": 7.19782257145347,
+                    "y": 6.02356173384038,
+                    "heading": 9.19440476065517e-9,
+                    "angularVelocity": 2.3389208664172086e-9,
+                    "velocityX": 2.8176230513092126,
+                    "velocityY": -1.3240029548204029,
+                    "timestamp": 2.1987115499749317
+                },
+                {
+                    "x": 7.278184934221974,
+                    "y": 5.985800317976241,
+                    "heading": 8.586659012919787e-9,
+                    "angularVelocity": -1.9532843759144805e-8,
+                    "velocityX": 2.582832628715854,
+                    "velocityY": -1.2136454633793592,
+                    "timestamp": 2.2298255919283414
+                },
+                {
+                    "x": 7.351241854096833,
+                    "y": 5.951472228972044,
+                    "heading": 7.633275836953741e-9,
+                    "angularVelocity": -3.064157105146478e-8,
+                    "velocityX": 2.3480369404994192,
+                    "velocityY": -1.1032989238626065,
+                    "timestamp": 2.260939633881751
+                },
+                {
+                    "x": 7.416993235519351,
+                    "y": 5.9205772680990165,
+                    "heading": 6.513007078345892e-9,
+                    "angularVelocity": -3.600524486341296e-8,
+                    "velocityX": 2.1132381810427594,
+                    "velocityY": -0.9929587714545899,
+                    "timestamp": 2.292053675835161
+                },
+                {
+                    "x": 7.47543901596506,
+                    "y": 5.89311530534189,
+                    "heading": 5.336882897741431e-9,
+                    "angularVelocity": -3.780042846669445e-8,
+                    "velocityX": 1.8784374120603946,
+                    "velocityY": -0.8826227977151127,
+                    "timestamp": 2.3231677177885706
+                },
+                {
+                    "x": 7.52657915135954,
+                    "y": 5.869086249058835,
+                    "heading": 4.180544199369271e-9,
+                    "angularVelocity": -3.716452645721565e-8,
+                    "velocityX": 1.643635226533715,
+                    "velocityY": -0.7722897693284123,
+                    "timestamp": 2.3542817597419803
+                },
+                {
+                    "x": 7.570413608969555,
+                    "y": 5.848490031193872,
+                    "heading": 3.0988314877379083e-9,
+                    "angularVelocity": -3.476606009342686e-8,
+                    "velocityX": 1.4088319889664873,
+                    "velocityY": -0.6619589282491846,
+                    "timestamp": 2.38539580169539
+                },
+                {
+                    "x": 7.606942363528039,
+                    "y": 5.831326599216932,
+                    "heading": 2.1333266690310216e-9,
+                    "angularVelocity": -3.103115843804935e-8,
+                    "velocityX": 1.1740279393200113,
+                    "velocityY": -0.5516297754774387,
+                    "timestamp": 2.4165098436487997
+                },
+                {
+                    "x": 7.636165394942908,
+                    "y": 5.817595911358632,
+                    "heading": 1.3166428525752622e-9,
+                    "angularVelocity": -2.6248077074736502e-8,
+                    "velocityX": 0.9392232439174092,
+                    "velocityY": -0.4413019651665061,
+                    "timestamp": 2.4476238856022094
+                },
+                {
+                    "x": 7.658082686855903,
+                    "y": 5.80729793361315,
+                    "heading": 6.750439688094791e-10,
+                    "angularVelocity": -2.0620877517571588e-8,
+                    "velocityX": 0.7044180227631803,
+                    "velocityY": -0.3309752477968783,
+                    "timestamp": 2.478737927555619
+                },
                 {
-                    "x": 7.672950320288622,
-                    "y": 5.8008527701995645,
-                    "heading": -0.007307045765720019,
-                    "angularVelocity": 0.45753668054709684,
-                    "velocityX": 0.44749362902029066,
-                    "velocityY": -0.24469523034329732,
-                    "timestamp": 2.4323647752419295
+                    "x": 7.672694225690866,
+                    "y": 5.800432637759072,
+                    "heading": 2.301347828245949e-10,
+                    "angularVelocity": -1.4299304537044113e-8,
+                    "velocityX": 0.46961236527350575,
+                    "velocityY": -0.22064943745862983,
+                    "timestamp": 2.509851969509029
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 5.476349034092433e-35,
-                    "angularVelocity": 0.23175585268595406,
-                    "velocityX": 0.22359303404641942,
-                    "velocityY": -0.12219740664444498,
-                    "timestamp": 2.463893842063908
+                    "heading": -5.36340746801057e-34,
+                    "angularVelocity": -7.3964922450186615e-9,
+                    "velocityX": 0.23480633985361782,
+                    "velocityY": -0.1103243919326242,
+                    "timestamp": 2.5409660114624386
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 1.465348958332225e-34,
-                    "angularVelocity": 2.9106988947740147e-33,
-                    "velocityX": 3.969530612508356e-33,
-                    "velocityY": 4.7510565840732025e-33,
-                    "timestamp": 2.4954229088858866
+                    "heading": -7.670487347269043e-34,
+                    "angularVelocity": -7.414948879954709e-33,
+                    "velocityX": 1.169026940913066e-31,
+                    "velocityY": 1.7301899688761208e-31,
+                    "timestamp": 2.5720800534158483
                 }
             ],
             "constraints": [
@@ -8098,7 +9106,25 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 29
+                    "controlIntervalCount": 18
+                },
+                {
+                    "x": 2.797205924987793,
+                    "y": 6.204219341278076,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 9
+                },
+                {
+                    "x": 4.192164897918701,
+                    "y": 5.566523551940918,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 15
                 },
                 {
                     "x": 7.68,
@@ -8115,271 +9141,388 @@
                     "x": 0.71,
                     "y": 4.36,
                     "heading": -1.04,
-                    "angularVelocity": 0,
-                    "velocityX": -1.418990828065155e-30,
-                    "velocityY": 1.8798559266210403e-30,
+                    "angularVelocity": 5.350140627575144e-31,
+                    "velocityX": 5.576066956455863e-32,
+                    "velocityY": -7.476130633471959e-32,
                     "timestamp": 0
                 },
                 {
-                    "x": 0.7612279700458028,
-                    "y": 4.384337560333447,
-                    "heading": -0.9750502376080931,
-                    "angularVelocity": 0.7696859194569483,
-                    "velocityX": 0.6070760811828038,
-                    "velocityY": 0.28841179417367857,
-                    "timestamp": 0.0843847610434429
-                },
-                {
-                    "x": 0.8642667385584091,
-                    "y": 4.433290258850185,
-                    "heading": -0.8542284973914237,
-                    "angularVelocity": 1.4317957261788106,
-                    "velocityX": 1.2210589594435701,
-                    "velocityY": 0.5801130193585202,
-                    "timestamp": 0.1687695220868858
-                },
-                {
-                    "x": 1.0199495140663002,
-                    "y": 4.507092326164681,
-                    "heading": -0.6915219404263555,
-                    "angularVelocity": 1.9281509475538106,
-                    "velocityX": 1.8449157594667183,
-                    "velocityY": 0.8745899899690757,
-                    "timestamp": 0.25315428313032873
-                },
-                {
-                    "x": 1.2292345273519185,
-                    "y": 4.606077015104397,
-                    "heading": -0.5137189126301165,
-                    "angularVelocity": 2.1070513869757743,
-                    "velocityX": 2.480128055086372,
-                    "velocityY": 1.173016166850306,
-                    "timestamp": 0.3375390441737716
-                },
-                {
-                    "x": 1.485641598676529,
-                    "y": 4.729632973105035,
-                    "heading": -0.42871970592241276,
-                    "angularVelocity": 1.007281476615537,
-                    "velocityX": 3.038547104406527,
-                    "velocityY": 1.4641975218379706,
-                    "timestamp": 0.4219238052172145
-                },
-                {
-                    "x": 1.7745745246623985,
-                    "y": 4.865356167675212,
-                    "heading": -0.42871965586223265,
-                    "angularVelocity": 5.932372081830595e-7,
-                    "velocityX": 3.4239941242128182,
-                    "velocityY": 1.6083851265555307,
-                    "timestamp": 0.5063085662606575
-                },
-                {
-                    "x": 2.063508085893233,
-                    "y": 5.001078009918069,
-                    "heading": -0.4287196058112515,
-                    "angularVelocity": 5.931281972756146e-7,
-                    "velocityX": 3.4240016521710777,
-                    "velocityY": 1.6083691008265977,
-                    "timestamp": 0.5906933273041004
-                },
-                {
-                    "x": 2.3524294773436862,
-                    "y": 5.136825756955775,
-                    "heading": -0.4287195557601797,
-                    "angularVelocity": 5.931292703362374e-7,
-                    "velocityX": 3.423857434421327,
-                    "velocityY": 1.6086760851027126,
-                    "timestamp": 0.6750780883475433
-                },
-                {
-                    "x": 2.647360543237261,
-                    "y": 5.258967835966316,
-                    "heading": -0.4287195052637089,
-                    "angularVelocity": 5.984074633873495e-7,
-                    "velocityX": 3.4950749666965533,
-                    "velocityY": 1.447442375852424,
-                    "timestamp": 0.7594628493909863
-                },
-                {
-                    "x": 2.959809428031792,
-                    "y": 5.324379711821066,
-                    "heading": -0.42871944828896014,
-                    "angularVelocity": 6.751781731100131e-7,
-                    "velocityX": 3.702669545200082,
-                    "velocityY": 0.7751621862272086,
-                    "timestamp": 0.8438476104344292
-                },
-                {
-                    "x": 3.2789674137401184,
-                    "y": 5.330797896945005,
-                    "heading": -0.4287193686010616,
-                    "angularVelocity": 9.443399202542765e-7,
-                    "velocityX": 3.782175617514861,
-                    "velocityY": 0.0760585802995927,
-                    "timestamp": 0.9282323714778722
-                },
-                {
-                    "x": 3.593793413911528,
-                    "y": 5.278000284107237,
-                    "heading": -0.42871953037751775,
-                    "angularVelocity": -0.000001917128805011847,
-                    "velocityX": 3.7308395055989028,
-                    "velocityY": -0.625677103128148,
-                    "timestamp": 1.0126171325213151
-                },
-                {
-                    "x": 3.893396265271842,
-                    "y": 5.167813441772834,
-                    "heading": -0.42871965571467335,
-                    "angularVelocity": -0.0000014853055701792625,
-                    "velocityX": 3.550437871193987,
-                    "velocityY": -1.3057670718262655,
-                    "timestamp": 1.097001893564758
-                },
-                {
-                    "x": 4.167411681517661,
-                    "y": 5.0040496950416875,
-                    "heading": -0.4287197602752934,
-                    "angularVelocity": -0.0000012390936318239587,
-                    "velocityX": 3.247214459785327,
-                    "velocityY": -1.9406791546980178,
-                    "timestamp": 1.181386654608201
-                },
-                {
-                    "x": 4.4320204493985145,
-                    "y": 4.825485382292427,
-                    "heading": -0.42871985986303984,
-                    "angularVelocity": -0.0000011801626881351292,
-                    "velocityX": 3.1357411528921184,
-                    "velocityY": -2.1160729797797915,
-                    "timestamp": 1.265771415651644
-                },
-                {
-                    "x": 4.666313927400726,
-                    "y": 4.608668432226026,
-                    "heading": -0.4287198793836171,
-                    "angularVelocity": -2.3132822841838536e-7,
-                    "velocityX": 2.77649038884734,
-                    "velocityY": -2.5693851281356843,
-                    "timestamp": 1.350156176695087
-                },
-                {
-                    "x": 4.936706954544898,
-                    "y": 4.438990462685779,
-                    "heading": -0.4287197198154701,
-                    "angularVelocity": 0.0000018909592816311697,
-                    "velocityX": 3.204287406881873,
-                    "velocityY": -2.010765539204243,
-                    "timestamp": 1.4345409377385299
-                },
-                {
-                    "x": 5.233776210787073,
-                    "y": 4.322144997944839,
-                    "heading": -0.42871957094251306,
-                    "angularVelocity": 0.0000017642161351814778,
-                    "velocityX": 3.5204135506067917,
-                    "velocityY": -1.384674949557486,
-                    "timestamp": 1.5189256987819728
-                },
-                {
-                    "x": 5.532138329985476,
-                    "y": 4.208641466951728,
-                    "heading": -0.42871942351676046,
-                    "angularVelocity": 0.0000017470660671471722,
-                    "velocityX": 3.5357345983929718,
-                    "velocityY": -1.3450714274656952,
-                    "timestamp": 1.6033104598254158
-                },
-                {
-                    "x": 5.845046534221127,
-                    "y": 4.145463344127181,
-                    "heading": -0.42871936362457447,
-                    "angularVelocity": 7.097512069029325e-7,
-                    "velocityX": 3.708112701469523,
-                    "velocityY": -0.7486911385809435,
-                    "timestamp": 1.6876952208688587
-                },
-                {
-                    "x": 6.164234411105391,
-                    "y": 4.140760073731674,
-                    "heading": -0.4287193263126739,
-                    "angularVelocity": 4.4216396519651743e-7,
-                    "velocityX": 3.7825298423246796,
-                    "velocityY": -0.055736016049057806,
-                    "timestamp": 1.7720799819123016
-                },
-                {
-                    "x": 6.483450051427475,
-                    "y": 4.138663317552119,
-                    "heading": -0.42871928910258295,
-                    "angularVelocity": 4.4095747260310005e-7,
-                    "velocityX": 3.782858852414673,
-                    "velocityY": -0.024847569082867782,
-                    "timestamp": 1.8564647429557446
-                },
-                {
-                    "x": 6.802665691714121,
-                    "y": 4.136566553568004,
-                    "heading": -0.4287192518958351,
-                    "angularVelocity": 4.4091785477775863e-7,
-                    "velocityX": 3.7828588519947024,
-                    "velocityY": -0.024847661569458286,
-                    "timestamp": 1.9408495039991875
-                },
-                {
-                    "x": 7.0950029322992965,
-                    "y": 4.132696093446151,
-                    "heading": -0.35666839777179793,
-                    "angularVelocity": 0.853837271482519,
-                    "velocityX": 3.4643368893901423,
-                    "velocityY": -0.04586681379319509,
-                    "timestamp": 2.0252342650426303
-                },
-                {
-                    "x": 7.329695147990892,
-                    "y": 4.13025195968354,
-                    "heading": -0.23302792235716066,
-                    "angularVelocity": 1.465199093839195,
-                    "velocityX": 2.7812156222231823,
-                    "velocityY": -0.02896416049909081,
-                    "timestamp": 2.109619026086073
-                },
-                {
-                    "x": 7.505137224924894,
-                    "y": 4.1285705338055685,
-                    "heading": -0.12281843303988041,
-                    "angularVelocity": 1.3060354494646,
-                    "velocityX": 2.0790729838492856,
-                    "velocityY": -0.019925705271038107,
-                    "timestamp": 2.194003787129516
-                },
+                    "x": 0.7244661136787854,
+                    "y": 4.377063116032538,
+                    "heading": -0.9951964797725402,
+                    "angularVelocity": 0.8134823535576418,
+                    "velocityX": 0.2626563301835885,
+                    "velocityY": 0.30980922299649477,
+                    "timestamp": 0.055076204212072884
+                },
+                {
+                    "x": 0.7533392249389502,
+                    "y": 4.41173941127898,
+                    "heading": -0.9083722132270291,
+                    "angularVelocity": 1.5764388230385076,
+                    "velocityX": 0.524239309393705,
+                    "velocityY": 0.6296057570147754,
+                    "timestamp": 0.11015240842414577
+                },
+                {
+                    "x": 0.7966290780145974,
+                    "y": 4.464680613281826,
+                    "heading": -0.783928161336893,
+                    "angularVelocity": 2.2594885335771653,
+                    "velocityX": 0.7859992113646113,
+                    "velocityY": 0.9612354874529038,
+                    "timestamp": 0.16522861263621866
+                },
+                {
+                    "x": 0.8545700129369538,
+                    "y": 4.536623492592967,
+                    "heading": -0.6296494882874061,
+                    "angularVelocity": 2.8011856527989654,
+                    "velocityX": 1.0520139459729396,
+                    "velocityY": 1.3062425114505738,
+                    "timestamp": 0.22030481684829153
+                },
+                {
+                    "x": 0.9276258124093708,
+                    "y": 4.628245682798297,
+                    "heading": -0.4576364570724038,
+                    "angularVelocity": 3.1231823920301154,
+                    "velocityX": 1.3264494261643394,
+                    "velocityY": 1.663553099130404,
+                    "timestamp": 0.27538102106036444
+                },
+                {
+                    "x": 1.0160400191585532,
+                    "y": 4.739959244546144,
+                    "heading": -0.28714155882633985,
+                    "angularVelocity": 3.095618165506983,
+                    "velocityX": 1.605306829220418,
+                    "velocityY": 2.0283453325449616,
+                    "timestamp": 0.3304572252724373
+                },
+                {
+                    "x": 1.114934453543302,
+                    "y": 4.869596690219766,
+                    "heading": -0.17212037106177128,
+                    "angularVelocity": 2.0884007787042775,
+                    "velocityX": 1.795592775492439,
+                    "velocityY": 2.3537832268624075,
+                    "timestamp": 0.3855334294845102
+                },
+                {
+                    "x": 1.2246983786188343,
+                    "y": 5.017091614146892,
+                    "heading": -0.11040110898627345,
+                    "angularVelocity": 1.1206157533631977,
+                    "velocityX": 1.9929464393166578,
+                    "velocityY": 2.678015415862639,
+                    "timestamp": 0.44060963369658307
+                },
+                {
+                    "x": 1.345743830891035,
+                    "y": 5.182217862842374,
+                    "heading": -0.10127955705630985,
+                    "angularVelocity": 0.16561693131283492,
+                    "velocityX": 2.197781310529416,
+                    "velocityY": 2.998141412571981,
+                    "timestamp": 0.49568583790865595
+                },
+                {
+                    "x": 1.4740265309381277,
+                    "y": 5.346392338052181,
+                    "heading": -0.10127945031307138,
+                    "angularVelocity": 0.0000019381008548903397,
+                    "velocityX": 2.3291855690186902,
+                    "velocityY": 2.9808603835088525,
+                    "timestamp": 0.5507620421207289
+                },
+                {
+                    "x": 1.6022800251300493,
+                    "y": 5.510589629967512,
+                    "heading": -0.10127934358262022,
+                    "angularVelocity": 0.0000019378686800961446,
+                    "velocityX": 2.3286552881910736,
+                    "velocityY": 2.9812746587087395,
+                    "timestamp": 0.6058382463328018
+                },
+                {
+                    "x": 1.7347437736631814,
+                    "y": 5.671409606244062,
+                    "heading": -0.10127923743109664,
+                    "angularVelocity": 0.0000019273572882478923,
+                    "velocityX": 2.4050994513545807,
+                    "velocityY": 2.9199538816671686,
+                    "timestamp": 0.6609144505448747
+                },
+                {
+                    "x": 1.8857033856482435,
+                    "y": 5.815009412217227,
+                    "heading": -0.10127912902413344,
+                    "angularVelocity": 0.0000019683085417376046,
+                    "velocityX": 2.7409225843485396,
+                    "velocityY": 2.607293077428296,
+                    "timestamp": 0.7159906547569477
+                },
+                {
+                    "x": 2.0529507817587227,
+                    "y": 5.93925921930888,
+                    "heading": -0.1012790059378887,
+                    "angularVelocity": 0.0000022348352886277144,
+                    "velocityX": 3.036654368309179,
+                    "velocityY": 2.2559616965109663,
+                    "timestamp": 0.7710668589690206
+                },
+                {
+                    "x": 2.234021022235051,
+                    "y": 6.042327659180149,
+                    "heading": -0.10127786160272408,
+                    "angularVelocity": 0.0000207773062979661,
+                    "velocityX": 3.287631075284561,
+                    "velocityY": 1.8713787804693067,
+                    "timestamp": 0.8261430631810935
+                },
+                {
+                    "x": 2.4203468055894013,
+                    "y": 6.121035990183916,
+                    "heading": -0.0857184634932982,
+                    "angularVelocity": 0.28250672558184264,
+                    "velocityX": 3.3830541886455214,
+                    "velocityY": 1.4290805281478167,
+                    "timestamp": 0.8812192673931665
+                },
+                {
+                    "x": 2.608481388818861,
+                    "y": 6.1750393209771115,
+                    "heading": -0.05159654365961516,
+                    "angularVelocity": 0.6195401502670522,
+                    "velocityX": 3.4158959558113233,
+                    "velocityY": 0.9805202004345134,
+                    "timestamp": 0.9362954716052394
+                },
+                {
+                    "x": 2.797205924987793,
+                    "y": 6.204219341278076,
+                    "heading": -3.617531000727075e-31,
+                    "angularVelocity": 0.9368209809983128,
+                    "velocityX": 3.4266075316708653,
+                    "velocityY": 0.5298117529778321,
+                    "timestamp": 0.9913716758173123
+                },
+                {
+                    "x": 2.966124692846499,
+                    "y": 6.210295134186947,
+                    "heading": 0.05716064392929573,
+                    "angularVelocity": 1.15887425245862,
+                    "velocityX": 3.4246572006891465,
+                    "velocityY": 0.12318055713425537,
+                    "timestamp": 1.0406959608361792
+                },
+                {
+                    "x": 3.13289635801508,
+                    "y": 6.196205704844962,
+                    "heading": 0.11248864097572012,
+                    "angularVelocity": 1.12171919015676,
+                    "velocityX": 3.3811268648859167,
+                    "velocityY": -0.2856489320949118,
+                    "timestamp": 1.090020245855046
+                },
+                {
+                    "x": 3.2974428000278966,
+                    "y": 6.1620266805098955,
+                    "heading": 0.16165741156418048,
+                    "angularVelocity": 0.9968471021861408,
+                    "velocityX": 3.336012715640518,
+                    "velocityY": -0.6929451551500881,
+                    "timestamp": 1.139344530873913
+                },
+                {
+                    "x": 3.45979854217056,
+                    "y": 6.107879810654357,
+                    "heading": 0.20068182384964747,
+                    "angularVelocity": 0.7911804959877227,
+                    "velocityX": 3.2915984910995078,
+                    "velocityY": -1.0977730307662261,
+                    "timestamp": 1.1886688158927798
+                },
+                {
+                    "x": 3.619952741472391,
+                    "y": 6.03393942235358,
+                    "heading": 0.22581697371114728,
+                    "angularVelocity": 0.509589745738515,
+                    "velocityX": 3.2469644362928305,
+                    "velocityY": -1.4990666012186962,
+                    "timestamp": 1.2379931009116467
+                },
+                {
+                    "x": 3.7772983820923596,
+                    "y": 5.940432276289927,
+                    "heading": 0.23452421295694514,
+                    "angularVelocity": 0.17653047058793564,
+                    "velocityX": 3.190023749148815,
+                    "velocityY": -1.8957628281461119,
+                    "timestamp": 1.2873173859305136
+                },
+                {
+                    "x": 3.926570083103172,
+                    "y": 5.828476756706829,
+                    "heading": 0.23452463928791645,
+                    "angularVelocity": 0.000008643429319627254,
+                    "velocityX": 3.026332788274898,
+                    "velocityY": -2.2697849454943535,
+                    "timestamp": 1.3366416709493805
+                },
+                {
+                    "x": 4.062952838723869,
+                    "y": 5.701135334142216,
+                    "heading": 0.23452497322201252,
+                    "angularVelocity": 0.000006770176109915362,
+                    "velocityX": 2.7650224543249706,
+                    "velocityY": -2.5817185695831255,
+                    "timestamp": 1.3859659559682473
+                },
+                {
+                    "x": 4.192164897918701,
+                    "y": 5.566523551940918,
+                    "heading": 0.23452535083185597,
+                    "angularVelocity": 0.000007655657721212718,
+                    "velocityX": 2.6196438355955265,
+                    "velocityY": -2.7291177591283535,
+                    "timestamp": 1.4352902409871142
+                },
+                {
+                    "x": 4.379227990202405,
+                    "y": 5.2613971819703,
+                    "heading": 0.26255662300085747,
+                    "angularVelocity": 0.28814889358818807,
+                    "velocityX": 1.922924608906585,
+                    "velocityY": -3.136562100411476,
+                    "timestamp": 1.5325707539189606
+                },
+                {
+                    "x": 4.5172116067735955,
+                    "y": 4.993723531483578,
+                    "heading": 0.47596391395635385,
+                    "angularVelocity": 2.193731144335268,
+                    "velocityX": 1.4184096322339512,
+                    "velocityY": -2.751564958073857,
+                    "timestamp": 1.629851266850807
+                },
+                {
+                    "x": 4.704361215089976,
+                    "y": 4.7238802473185055,
+                    "heading": 0.606383868813914,
+                    "angularVelocity": 1.3406585854344948,
+                    "velocityX": 1.9238139548821627,
+                    "velocityY": -2.773867818255899,
+                    "timestamp": 1.7271317797826533
+                },
+                {
+                    "x": 4.9212614384999,
+                    "y": 4.498429708685273,
+                    "heading": 0.7825723935837035,
+                    "angularVelocity": 1.8111389368724442,
+                    "velocityX": 2.229636921855878,
+                    "velocityY": -2.3175303237882856,
+                    "timestamp": 1.8244122927144997
+                },
+                {
+                    "x": 5.185303406160076,
+                    "y": 4.293106225066136,
+                    "heading": 0.8921386359376694,
+                    "angularVelocity": 1.1262917829260124,
+                    "velocityX": 2.71423288901818,
+                    "velocityY": -2.110633234047443,
+                    "timestamp": 1.921692805646346
+                },
+                {
+                    "x": 5.506747569472596,
+                    "y": 4.137612897565104,
+                    "heading": 0.9241000729554552,
+                    "angularVelocity": 0.328549223832528,
+                    "velocityX": 3.304301690285293,
+                    "velocityY": -1.5984015998144245,
+                    "timestamp": 2.0189733185781926
+                },
+                {
+                    "x": 5.864556115895607,
+                    "y": 4.051579577380226,
+                    "heading": 0.924099942108789,
+                    "angularVelocity": -0.0000013450449852620257,
+                    "velocityX": 3.6781112233000655,
+                    "velocityY": -0.8843839078557418,
+                    "timestamp": 2.116253831510039
+                },
+                {
+                    "x": 6.232480317048254,
+                    "y": 4.043802644882202,
+                    "heading": 0.9240998495204155,
+                    "angularVelocity": -9.517669138022849e-7,
+                    "velocityX": 3.7820956126167884,
+                    "velocityY": -0.07994337471752075,
+                    "timestamp": 2.2135343444418853
+                },
+                {
+                    "x": 6.59997941709367,
+                    "y": 4.063118723469172,
+                    "heading": 0.9240997607721795,
+                    "angularVelocity": -9.122920231628579e-7,
+                    "velocityX": 3.777725764078584,
+                    "velocityY": 0.198560616148301,
+                    "timestamp": 2.3108148573737317
+                },
+                {
+                    "x": 6.956026800754634,
+                    "y": 4.090327871531077,
+                    "heading": 0.8973557490580134,
+                    "angularVelocity": -0.27491643401286797,
+                    "velocityX": 3.660007260759475,
+                    "velocityY": 0.2796978268501417,
+                    "timestamp": 2.408095370305578
+                },
+                {
+                    "x": 7.247734347026976,
+                    "y": 4.106615231962139,
+                    "heading": 0.7104712895715125,
+                    "angularVelocity": -1.9210883439464366,
+                    "velocityX": 2.9986226170159034,
+                    "velocityY": 0.1674267532128742,
+                    "timestamp": 2.5053758832374244
+                },
+                {
+                    "x": 7.467840517564824,
+                    "y": 4.117242335862297,
+                    "heading": 0.4016498999018689,
+                    "angularVelocity": -3.1745452440819273,
+                    "velocityX": 2.262592619058774,
+                    "velocityY": 0.1092418571806094,
+                    "timestamp": 2.602656396169271
+                },
                 {
-                    "x": 7.621791455543556,
-                    "y": 4.127510522704197,
-                    "heading": -0.04250049300917318,
-                    "angularVelocity": 0.9518062152170125,
-                    "velocityX": 1.3824087332380297,
-                    "velocityY": -0.012561641322782502,
-                    "timestamp": 2.278388548172959
+                    "x": 7.6104608023102855,
+                    "y": 4.123883769495069,
+                    "heading": 0.14488296734927883,
+                    "angularVelocity": -2.639448794153438,
+                    "velocityX": 1.4660724994879464,
+                    "velocityY": 0.06827095615156697,
+                    "timestamp": 2.699936909101117
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": -1.5179160305718205e-31,
-                    "angularVelocity": 0.5036512811515541,
-                    "velocityX": 0.6897992449901723,
-                    "velocityY": -0.0060499395609808024,
-                    "timestamp": 2.362773309216402
+                    "heading": 9.165039287514682e-34,
+                    "angularVelocity": -1.4893318608505086,
+                    "velocityX": 0.7148317334473038,
+                    "velocityY": 0.032033450595745375,
+                    "timestamp": 2.7972174220329635
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
                     "heading": 0,
-                    "angularVelocity": 2.3304076579629275e-30,
+                    "angularVelocity": 3.6915361070859757e-32,
                     "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 2.447158070259845
+                    "velocityY": 3.3618150907593784e-32,
+                    "timestamp": 2.89449793496481
                 }
             ],
             "constraints": [
@@ -9883,7 +11026,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 13
+                    "controlIntervalCount": 9
+                },
+                {
+                    "x": 3.096125602722168,
+                    "y": 6.283931255340576,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 12
                 },
                 {
                     "x": 5.176,
@@ -9900,127 +11052,199 @@
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.39,
-                    "angularVelocity": -4.966859894029903e-31,
-                    "velocityX": 7.6981862107013014e-34,
-                    "velocityY": 1.676179535721191e-31,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.3730209621579745,
-                    "y": 6.753775517490119,
-                    "heading": 0.38350122654498725,
-                    "angularVelocity": -0.06940058989840046,
-                    "velocityX": 0.7797929692101098,
-                    "velocityY": -0.03443434208804683,
-                    "timestamp": 0.09364147285393071
-                },
-                {
-                    "x": 2.5190628825903247,
-                    "y": 6.747326552846718,
-                    "heading": 0.37050348328006333,
-                    "angularVelocity": -0.13880327667642306,
-                    "velocityX": 1.5595858969471437,
-                    "velocityY": -0.06886868015693057,
-                    "timestamp": 0.18728294570786141
-                },
-                {
-                    "x": 2.73812575373285,
-                    "y": 6.7376531065331395,
-                    "heading": 0.35100644039340695,
-                    "angularVelocity": -0.2082094855243831,
-                    "velocityX": 2.3393787439058484,
-                    "velocityY": -0.10330301327774684,
-                    "timestamp": 0.2809244185617921
-                },
-                {
-                    "x": 3.0302095555912687,
-                    "y": 6.7247551792664035,
-                    "heading": 0.3250096622235805,
-                    "angularVelocity": -0.27762034681324244,
-                    "velocityX": 3.1191713773449834,
-                    "velocityY": -0.13773733874150373,
-                    "timestamp": 0.37456589141572283
-                },
-                {
-                    "x": 3.3841048026937948,
-                    "y": 6.709127593205063,
-                    "heading": 0.3250096592315294,
-                    "angularVelocity": -3.195216141250517e-8,
-                    "velocityX": 3.779257590860006,
-                    "velocityY": -0.16688744404647712,
-                    "timestamp": 0.46820736426965354
-                },
-                {
-                    "x": 3.7380000497922845,
-                    "y": 6.693500007049851,
-                    "heading": 0.3250096562396269,
-                    "angularVelocity": -3.195060442591983e-8,
-                    "velocityX": 3.7792575908168353,
-                    "velocityY": -0.16688744504894126,
-                    "timestamp": 0.5618488371235842
-                },
-                {
-                    "x": 4.091895296890773,
-                    "y": 6.677872420894639,
-                    "heading": 0.3250096532477245,
-                    "angularVelocity": -3.195060369258429e-8,
-                    "velocityX": 3.7792575908168353,
-                    "velocityY": -0.16688744504893968,
-                    "timestamp": 0.6554903099775149
-                },
-                {
-                    "x": 4.445790543993225,
-                    "y": 6.6622448348316095,
-                    "heading": 0.3250096502556742,
-                    "angularVelocity": -3.195215253798021e-8,
-                    "velocityX": 3.7792575908592094,
-                    "velocityY": -0.1668874440645195,
-                    "timestamp": 0.7491317828314457
-                },
-                {
-                    "x": 4.737874310687331,
-                    "y": 6.649346902155386,
-                    "heading": 0.29900607193864137,
-                    "angularVelocity": -0.27769296578215763,
-                    "velocityX": 3.119171001824275,
-                    "velocityY": -0.1377373965095818,
-                    "timestamp": 0.8427732556853764
-                },
-                {
-                    "x": 4.956937151198913,
-                    "y": 6.639673451411702,
-                    "heading": 0.27950309637569815,
-                    "angularVelocity": -0.20827284074623165,
-                    "velocityX": 2.339378416797119,
-                    "velocityY": -0.10330306058697603,
-                    "timestamp": 0.9364147285393072
-                },
-                {
-                    "x": 5.10297904938153,
-                    "y": 6.633224483848421,
-                    "heading": 0.2665010284217395,
-                    "angularVelocity": -0.1388494601558462,
-                    "velocityX": 1.5595856593415998,
-                    "velocityY": -0.06886871133840435,
-                    "timestamp": 1.030056201393238
+                    "x": 2.3161416084367663,
+                    "y": 6.743433917099698,
+                    "heading": 0.3883998707887416,
+                    "angularVelocity": -0.031816354442463515,
+                    "velocityX": 0.32095354030299916,
+                    "velocityY": -0.26974278009236136,
+                    "timestamp": 0.05029266360959302
+                },
+                {
+                    "x": 2.3488347297971495,
+                    "y": 6.716804087923855,
+                    "heading": 0.385297889956982,
+                    "angularVelocity": -0.06167859503006947,
+                    "velocityX": 0.6500574639309191,
+                    "velocityY": -0.5294972917434406,
+                    "timestamp": 0.10058532721918605
+                },
+                {
+                    "x": 2.398581891811533,
+                    "y": 6.677772540045158,
+                    "heading": 0.38082205724745694,
+                    "angularVelocity": -0.08899573791258333,
+                    "velocityX": 0.9891534558709463,
+                    "velocityY": -0.7760883015003333,
+                    "timestamp": 0.15087799082877906
+                },
+                {
+                    "x": 2.4660071913042128,
+                    "y": 6.627244851321165,
+                    "heading": 0.3751449326788243,
+                    "angularVelocity": -0.11288176368431184,
+                    "velocityX": 1.3406587492776654,
+                    "velocityY": -1.004673149074478,
+                    "timestamp": 0.2011706544383721
+                },
+                {
+                    "x": 2.551890499441383,
+                    "y": 6.566518101686868,
+                    "heading": 0.3685091874361843,
+                    "angularVelocity": -0.13194260884949985,
+                    "velocityX": 1.7076707013145411,
+                    "velocityY": -1.207467357579225,
+                    "timestamp": 0.2514633180479651
+                },
+                {
+                    "x": 2.6571889825280843,
+                    "y": 6.497556440570772,
+                    "heading": 0.3612736159712354,
+                    "angularVelocity": -0.14386932299145247,
+                    "velocityX": 2.093714580402863,
+                    "velocityY": -1.3712071735039586,
+                    "timestamp": 0.3017559816575581
+                },
+                {
+                    "x": 2.7829575448660773,
+                    "y": 6.423518691483174,
+                    "heading": 0.3539965965907385,
+                    "angularVelocity": -0.144693457419283,
+                    "velocityX": 2.50073377131696,
+                    "velocityY": -1.4721381564184353,
+                    "timestamp": 0.35204864526715113
+                },
+                {
+                    "x": 2.929814044412336,
+                    "y": 6.349664492887633,
+                    "heading": 0.3475640372215199,
+                    "angularVelocity": -0.1279025390095239,
+                    "velocityX": 2.9200382124570217,
+                    "velocityY": -1.468488508957291,
+                    "timestamp": 0.40234130887674413
+                },
+                {
+                    "x": 3.096125602722168,
+                    "y": 6.283931255340576,
+                    "heading": 0.3432166982946709,
+                    "angularVelocity": -0.08644081690714242,
+                    "velocityX": 3.3068751259797926,
+                    "velocityY": -1.3070144396670602,
+                    "timestamp": 0.45263397248633713
+                },
+                {
+                    "x": 3.3513045644934745,
+                    "y": 6.2237675636491945,
+                    "heading": 0.3432161755192605,
+                    "angularVelocity": -0.0000075431393477149555,
+                    "velocityX": 3.681983552011348,
+                    "velocityY": -0.8681033957434081,
+                    "timestamp": 0.5219387257984235
+                },
+                {
+                    "x": 3.6126701345327414,
+                    "y": 6.203172404760708,
+                    "heading": 0.34321617069844046,
+                    "angularVelocity": -6.955973168022537e-8,
+                    "velocityX": 3.771250275753975,
+                    "velocityY": -0.2971680570846853,
+                    "timestamp": 0.5912434791105099
+                },
+                {
+                    "x": 3.8741231287698,
+                    "y": 6.222626349014288,
+                    "heading": 0.3432161668255195,
+                    "angularVelocity": -5.588247279499863e-8,
+                    "velocityX": 3.7725117216666155,
+                    "velocityY": 0.2807014428862732,
+                    "timestamp": 0.6605482324225962
+                },
+                {
+                    "x": 4.129562675072727,
+                    "y": 6.28167517120369,
+                    "heading": 0.34321616334811994,
+                    "angularVelocity": -5.017548406400545e-8,
+                    "velocityX": 3.68574353266443,
+                    "velocityY": 0.8520169161197405,
+                    "timestamp": 0.7298529857346825
+                },
+                {
+                    "x": 4.378319272574482,
+                    "y": 6.3644778320450675,
+                    "heading": 0.34321615995103527,
+                    "angularVelocity": -4.901661807423297e-8,
+                    "velocityX": 3.5893150990896703,
+                    "velocityY": 1.1947616416511535,
+                    "timestamp": 0.7991577390467688
+                },
+                {
+                    "x": 4.606228025075672,
+                    "y": 6.4403413045373235,
+                    "heading": 0.31944011002240325,
+                    "angularVelocity": -0.34306521259177225,
+                    "velocityX": 3.2885010278428317,
+                    "velocityY": 1.094635921300162,
+                    "timestamp": 0.8684624923588551
+                },
+                {
+                    "x": 4.796152006740457,
+                    "y": 6.503560868780938,
+                    "heading": 0.2996267220530126,
+                    "angularVelocity": -0.2858878651536105,
+                    "velocityX": 2.74041782977768,
+                    "velocityY": 0.9121966563957079,
+                    "timestamp": 0.9377672456709414
+                },
+                {
+                    "x": 4.9480911999499915,
+                    "y": 6.554136520865842,
+                    "heading": 0.2837760268788632,
+                    "angularVelocity": -0.22871007278204386,
+                    "velocityX": 2.1923343774897495,
+                    "velocityY": 0.7297573350727579,
+                    "timestamp": 1.0070719989830277
+                },
+                {
+                    "x": 5.062045598488017,
+                    "y": 6.592068260177227,
+                    "heading": 0.27188801539640783,
+                    "angularVelocity": -0.1715324117657857,
+                    "velocityX": 1.644250835507307,
+                    "velocityY": 0.5473180048787404,
+                    "timestamp": 1.076376752295114
+                },
+                {
+                    "x": 5.138015199132455,
+                    "y": 6.617356086604739,
+                    "heading": 0.26396267467345363,
+                    "angularVelocity": -0.11435493734845723,
+                    "velocityX": 1.0961672470333998,
+                    "velocityY": 0.364878673092444,
+                    "timestamp": 1.1456815056072003
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
                     "heading": 0.26,
-                    "angularVelocity": -0.06942467075336838,
-                    "velocityX": 0.7797928459794067,
-                    "velocityY": -0.03443435638235996,
-                    "timestamp": 1.1236976742471687
+                    "angularVelocity": -0.05717753089184424,
+                    "velocityX": 0.548083631385222,
+                    "velocityY": 0.1824393391651535,
+                    "timestamp": 1.2149862589192866
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
                     "heading": 0.26,
-                    "angularVelocity": 2.7813300894409408e-31,
-                    "velocityX": -8.985802616555554e-33,
-                    "velocityY": -4.9789977648629575e-31,
-                    "timestamp": 1.2173391471010995
+                    "angularVelocity": -1.0031982410108139e-28,
+                    "velocityX": 0,
+                    "velocityY": 0,
+                    "timestamp": 1.2842910122313729
                 }
             ],
             "constraints": [
@@ -10051,7 +11275,25 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 13
+                    "controlIntervalCount": 8
+                },
+                {
+                    "x": 2.916773796081543,
+                    "y": 6.423426628112793,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 6
+                },
+                {
+                    "x": 3.654109001159668,
+                    "y": 6.343714714050293,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 7
                 },
                 {
                     "x": 4.263,
@@ -10068,127 +11310,199 @@
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.39,
-                    "angularVelocity": -5.274350592003984e-33,
-                    "velocityX": 6.842220334639481e-35,
-                    "velocityY": 4.754828012658472e-35,
+                    "angularVelocity": -4.207802187241835e-33,
+                    "velocityX": 0,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.3477747650996332,
-                    "y": 6.727866811231813,
-                    "heading": 0.37690695075838176,
-                    "angularVelocity": -0.1596548904259805,
-                    "velocityX": 0.5825590927179829,
-                    "velocityY": -0.3552462054262789,
-                    "timestamp": 0.08200844463131846
-                },
-                {
-                    "x": 2.4433243855136606,
-                    "y": 6.669600518122756,
-                    "heading": 0.35072364334029893,
-                    "angularVelocity": -0.3192757469769597,
-                    "velocityX": 1.16511928550267,
-                    "velocityY": -0.7104913813571495,
-                    "timestamp": 0.1640168892626369
-                },
-                {
-                    "x": 2.5866489974259967,
-                    "y": 6.582201260145896,
-                    "heading": 0.3114536293241488,
-                    "angularVelocity": -0.47885329605621113,
-                    "velocityX": 1.7476811388958082,
-                    "velocityY": -1.0657348565721152,
-                    "timestamp": 0.24602533389395537
-                },
-                {
-                    "x": 2.7777487646808345,
-                    "y": 6.465669250143918,
-                    "heading": 0.2590995246277934,
-                    "angularVelocity": -0.638398946983098,
-                    "velocityX": 2.330244990183096,
-                    "velocityY": -1.4209757364118492,
-                    "timestamp": 0.3280337785252738
-                },
-                {
-                    "x": 3.0166238424465086,
-                    "y": 6.320004790854863,
-                    "heading": 0.19366077275903337,
-                    "angularVelocity": -0.797951383701398,
-                    "velocityX": 2.912810733572304,
-                    "velocityY": -1.7762129247044165,
-                    "timestamp": 0.4100422231565923
-                },
-                {
-                    "x": 3.281496122930573,
-                    "y": 6.158489372926003,
-                    "heading": 0.19366076448026312,
-                    "angularVelocity": -1.0095021672660313e-7,
-                    "velocityX": 3.2298171447445223,
-                    "velocityY": -1.9694973932864837,
-                    "timestamp": 0.49205066778791073
-                },
-                {
-                    "x": 3.546368403715213,
-                    "y": 5.996973955490063,
-                    "heading": 0.193660756201493,
-                    "angularVelocity": -1.0095021513248387e-7,
-                    "velocityX": 3.2298171484097047,
-                    "velocityY": -1.9694973872758792,
-                    "timestamp": 0.5740591124192291
-                },
-                {
-                    "x": 3.785245611761769,
-                    "y": 5.851315694507869,
-                    "heading": 0.1281200370686441,
-                    "angularVelocity": -0.7991947588751046,
-                    "velocityX": 2.912836709932308,
-                    "velocityY": -1.7761373433799759,
-                    "timestamp": 0.6560675570505476
-                },
-                {
-                    "x": 3.9763473726141734,
-                    "y": 5.734789313275287,
-                    "heading": 0.07567704387560718,
-                    "angularVelocity": -0.6394828414171544,
-                    "velocityX": 2.3302692998450527,
-                    "velocityY": -1.4209070999510438,
-                    "timestamp": 0.7380760016818662
-                },
-                {
-                    "x": 4.119673683324307,
-                    "y": 5.647394635056781,
-                    "heading": 0.036339503004366135,
-                    "angularVelocity": -0.47967670949118707,
-                    "velocityX": 1.7477018538088223,
-                    "velocityY": -1.0656790116115789,
-                    "timestamp": 0.8200844463131847
-                },
-                {
-                    "x": 4.2152245571841105,
-                    "y": 5.589131547509505,
-                    "heading": 0.010112949222819228,
-                    "angularVelocity": -0.3198030873460947,
-                    "velocityX": 1.1651345698527396,
-                    "velocityY": -0.710452293165722,
-                    "timestamp": 0.9020928909445032
+                    "x": 2.3149122846118377,
+                    "y": 6.742667851221533,
+                    "heading": 0.387869941582996,
+                    "angularVelocity": -0.04275911471750884,
+                    "velocityX": 0.2993514559631543,
+                    "velocityY": -0.28770572153040486,
+                    "timestamp": 0.049815306773293704
+                },
+                {
+                    "x": 2.3455895487746456,
+                    "y": 6.714948533605446,
+                    "heading": 0.38350547033909727,
+                    "angularVelocity": -0.08761305563705182,
+                    "velocityX": 0.6158200390577908,
+                    "velocityY": -0.5564417728518571,
+                    "timestamp": 0.09963061354658741
+                },
+                {
+                    "x": 2.393100480232861,
+                    "y": 6.675215395028912,
+                    "heading": 0.37677783190043923,
+                    "angularVelocity": -0.13505163120415611,
+                    "velocityX": 0.9537416215146959,
+                    "velocityY": -0.7976090312433709,
+                    "timestamp": 0.1494459203198811
+                },
+                {
+                    "x": 2.4587594870185163,
+                    "y": 6.62556801103228,
+                    "heading": 0.3675326753009839,
+                    "angularVelocity": -0.18558867140063587,
+                    "velocityX": 1.318048829538744,
+                    "velocityY": -0.996629092792259,
+                    "timestamp": 0.19926122709317481
+                },
+                {
+                    "x": 2.5440298105146963,
+                    "y": 6.569365137451258,
+                    "heading": 0.35560686650228135,
+                    "angularVelocity": -0.23940048895004992,
+                    "velocityX": 1.7117293663218704,
+                    "velocityY": -1.1282249818675432,
+                    "timestamp": 0.24907653386646852
+                },
+                {
+                    "x": 2.649943674204089,
+                    "y": 6.511985811495891,
+                    "heading": 0.34090990459713355,
+                    "angularVelocity": -0.2950290353938729,
+                    "velocityX": 2.126130913363638,
+                    "velocityY": -1.1518412647039136,
+                    "timestamp": 0.2988918406397622
+                },
+                {
+                    "x": 2.7755461673317816,
+                    "y": 6.460929479776747,
+                    "heading": 0.3236377937131317,
+                    "angularVelocity": -0.3467229653455557,
+                    "velocityX": 2.5213634375333727,
+                    "velocityY": -1.0249125223997653,
+                    "timestamp": 0.3487071474130559
+                },
+                {
+                    "x": 2.916773796081543,
+                    "y": 6.423426628112793,
+                    "heading": 0.3044078287975214,
+                    "angularVelocity": -0.38602522319421667,
+                    "velocityX": 2.835024772455506,
+                    "velocityY": -0.7528379145513634,
+                    "timestamp": 0.39852245418634963
+                },
+                {
+                    "x": 3.0358362362248608,
+                    "y": 6.404912706911414,
+                    "heading": 0.2883220118823199,
+                    "angularVelocity": -0.4054270239814924,
+                    "velocityX": 3.0008504404627585,
+                    "velocityY": -0.46662497866622815,
+                    "timestamp": 0.43819868680957713
+                },
+                {
+                    "x": 3.161507467990405,
+                    "y": 6.397735862008237,
+                    "heading": 0.27120244348460076,
+                    "angularVelocity": -0.43148170241679884,
+                    "velocityX": 3.1674184633137656,
+                    "velocityY": -0.1808852410792362,
+                    "timestamp": 0.47787491943280463
+                },
+                {
+                    "x": 3.294397924367021,
+                    "y": 6.401341217255427,
+                    "heading": 0.24704597239250262,
+                    "angularVelocity": -0.6088398392423459,
+                    "velocityX": 3.349371842799791,
+                    "velocityY": 0.09086939481941644,
+                    "timestamp": 0.5175511520560321
+                },
+                {
+                    "x": 3.420816904565574,
+                    "y": 6.393526992528847,
+                    "heading": 0.22271106774313612,
+                    "angularVelocity": -0.6133370796675788,
+                    "velocityX": 3.186264719209789,
+                    "velocityY": -0.19694976589104882,
+                    "timestamp": 0.5572273846792596
+                },
+                {
+                    "x": 3.540724707702168,
+                    "y": 6.374315752768638,
+                    "heading": 0.19854345438454005,
+                    "angularVelocity": -0.6091206689927945,
+                    "velocityX": 3.0221569743089773,
+                    "velocityY": -0.48420019971772477,
+                    "timestamp": 0.5969036173024871
+                },
+                {
+                    "x": 3.654109001159668,
+                    "y": 6.343714714050293,
+                    "heading": 0.17465129572461457,
+                    "angularVelocity": -0.602178107150174,
+                    "velocityX": 2.857738398053468,
+                    "velocityY": -0.7712687595352792,
+                    "timestamp": 0.6365798499257146
+                },
+                {
+                    "x": 3.8638905650676802,
+                    "y": 6.228810110896502,
+                    "heading": 0.12580249974502505,
+                    "angularVelocity": -0.584756466458008,
+                    "velocityX": 2.5112415481035373,
+                    "velocityY": -1.375493671286808,
+                    "timestamp": 0.7201168414454817
+                },
+                {
+                    "x": 4.035438177373403,
+                    "y": 6.070055732251625,
+                    "heading": 0.08069123606907251,
+                    "angularVelocity": -0.5400154213754852,
+                    "velocityX": 2.0535526739088925,
+                    "velocityY": -1.90040813963597,
+                    "timestamp": 0.8036538329652487
+                },
+                {
+                    "x": 4.153723645387517,
+                    "y": 5.887986786487416,
+                    "heading": 0.04380352514936875,
+                    "angularVelocity": -0.4415733706546099,
+                    "velocityX": 1.4159651414562335,
+                    "velocityY": -2.179500870834227,
+                    "timestamp": 0.8871908244850157
+                },
+                {
+                    "x": 4.218422318430694,
+                    "y": 5.7283456029173445,
+                    "heading": 0.01885386609449773,
+                    "angularVelocity": -0.29866599934929844,
+                    "velocityX": 0.7744912985986951,
+                    "velocityY": -1.9110238550103982,
+                    "timestamp": 0.9707278160047828
+                },
+                {
+                    "x": 4.250477583142556,
+                    "y": 5.616790769692197,
+                    "heading": 0.0038966042591740443,
+                    "angularVelocity": -0.17904956311230244,
+                    "velocityX": 0.38372539073628414,
+                    "velocityY": -1.33539443060689,
+                    "timestamp": 1.0542648075245498
                 },
                 {
                     "x": 4.263,
                     "y": 5.56,
                     "heading": -0.003,
-                    "angularVelocity": -0.15989754813385992,
-                    "velocityX": 0.5825673566993228,
-                    "velocityY": -0.3552261921375451,
-                    "timestamp": 0.9841013355758217
+                    "angularVelocity": -0.08255748900826321,
+                    "velocityX": 0.14990265545387924,
+                    "velocityY": -0.6798278063289014,
+                    "timestamp": 1.1378017990443168
                 },
                 {
                     "x": 4.263,
                     "y": 5.56,
                     "heading": -0.003,
-                    "angularVelocity": 2.1439897413450708e-33,
-                    "velocityX": 3.9230953244452345e-34,
-                    "velocityY": -1.8972025919917558e-33,
-                    "timestamp": 1.0661097802071402
+                    "angularVelocity": 2.6015085807950757e-30,
+                    "velocityX": -2.4770702901017708e-31,
+                    "velocityY": 1.2993067663186955e-31,
+                    "timestamp": 1.2213387905640838
                 }
             ],
             "constraints": [
@@ -11436,7 +12750,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 21
+                    "controlIntervalCount": 9
+                },
+                {
+                    "x": 2.916773557662964,
+                    "y": 6.303858757019043,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 18
                 },
                 {
                     "x": 7.68,
@@ -11453,199 +12776,253 @@
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.39,
-                    "angularVelocity": 6.154725095706714e-32,
-                    "velocityX": 1.2115062621827067e-33,
-                    "velocityY": 6.540799795634554e-33,
+                    "angularVelocity": 4.0882515102287785e-31,
+                    "velocityX": 3.918479950533569e-32,
+                    "velocityY": 1.7984537919855076e-32,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.3666999539229265,
-                    "y": 6.765820977804648,
-                    "heading": 0.36628259128993745,
-                    "angularVelocity": -0.26339586361920003,
-                    "velocityX": 0.7407424724032508,
-                    "velocityY": 0.09796218023747214,
-                    "timestamp": 0.09004472729439511
-                },
-                {
-                    "x": 2.5001846245776895,
-                    "y": 6.783480891306791,
-                    "heading": 0.3222458902940237,
-                    "angularVelocity": -0.4890536327789505,
-                    "velocityX": 1.4824262859761226,
-                    "velocityY": 0.1961237935054848,
-                    "timestamp": 0.18008945458879022
-                },
-                {
-                    "x": 2.70056880915549,
-                    "y": 6.810010034997764,
-                    "heading": 0.26375227569196347,
-                    "angularVelocity": -0.6496062163731077,
-                    "velocityX": 2.225384990313294,
-                    "velocityY": 0.2946218450330504,
-                    "timestamp": 0.27013418188318533
-                },
-                {
-                    "x": 2.967965472322839,
-                    "y": 6.8454603445891635,
-                    "heading": 0.20325713210858826,
-                    "angularVelocity": -0.6718343805472418,
-                    "velocityX": 2.9695982341432847,
-                    "velocityY": 0.3936966733820705,
-                    "timestamp": 0.36017890917758044
-                },
-                {
-                    "x": 3.3011740475809854,
-                    "y": 6.889827568786931,
-                    "heading": 0.1886164627156386,
-                    "angularVelocity": -0.16259330038374148,
-                    "velocityX": 3.700478476310374,
-                    "velocityY": 0.4927242885939573,
-                    "timestamp": 0.45022363647197555
-                },
-                {
-                    "x": 3.6388963576772766,
-                    "y": 6.934269203744266,
-                    "heading": 0.18861644342798295,
-                    "angularVelocity": -2.1420083336262225e-7,
-                    "velocityX": 3.7506061736644574,
-                    "velocityY": 0.4935506641275844,
-                    "timestamp": 0.5402683637663707
-                },
-                {
-                    "x": 3.9766186774172247,
-                    "y": 6.978710765417191,
-                    "heading": 0.1886164241403376,
-                    "angularVelocity": -2.1420071941558326e-7,
-                    "velocityX": 3.750606280762983,
-                    "velocityY": 0.4935498502608198,
-                    "timestamp": 0.6303130910607657
-                },
-                {
-                    "x": 4.314341007224031,
-                    "y": 7.023152250589549,
-                    "heading": 0.18861640485269274,
-                    "angularVelocity": -2.1420071403103001e-7,
-                    "velocityX": 3.750606392561398,
-                    "velocityY": 0.4935490006767261,
-                    "timestamp": 0.7203578183551609
-                },
-                {
-                    "x": 4.652063347871557,
-                    "y": 7.067593653380398,
-                    "heading": 0.1886163855650432,
-                    "angularVelocity": -2.1420076555553473e-7,
-                    "velocityX": 3.750606512954019,
-                    "velocityY": 0.49354808578132814,
-                    "timestamp": 0.810402545649556
-                },
-                {
-                    "x": 4.989785700043158,
-                    "y": 7.112034968596579,
-                    "heading": 0.18861636627738598,
-                    "angularVelocity": -2.1420085095836123e-7,
-                    "velocityX": 3.7506066409356835,
-                    "velocityY": 0.4935471132127838,
-                    "timestamp": 0.9004472729439512
-                },
-                {
-                    "x": 5.327508064058316,
-                    "y": 7.15647619381006,
-                    "heading": 0.18861634698972354,
-                    "angularVelocity": -2.1420090940402864e-7,
-                    "velocityX": 3.7506067724653955,
-                    "velocityY": 0.4935461136795145,
-                    "timestamp": 0.9904920002383464
-                },
-                {
-                    "x": 5.6652304398823485,
-                    "y": 7.200917329284216,
-                    "heading": 0.18861632770206457,
-                    "angularVelocity": -2.1420087020950116e-7,
-                    "velocityX": 3.750606903609938,
-                    "velocityY": 0.4935451170711973,
-                    "timestamp": 1.0805367275327415
-                },
-                {
-                    "x": 6.002952827157885,
-                    "y": 7.245358377734637,
-                    "heading": 0.18861630841441618,
-                    "angularVelocity": -2.1420075320811155e-7,
-                    "velocityX": 3.750607030785673,
-                    "velocityY": 0.49354415062109597,
-                    "timestamp": 1.1705814548271367
-                },
-                {
-                    "x": 6.340675225087303,
-                    "y": 7.289799345222555,
-                    "heading": 0.1886162891267749,
-                    "angularVelocity": -2.1420067405327174e-7,
-                    "velocityX": 3.750607149103328,
-                    "velocityY": 0.4935432514845822,
-                    "timestamp": 1.2606261821215319
-                },
-                {
-                    "x": 6.678397632133336,
-                    "y": 7.334240243430023,
-                    "heading": 0.18861626983912938,
-                    "angularVelocity": -2.1420072105835628e-7,
-                    "velocityX": 3.7506072503487453,
-                    "velocityY": 0.4935424820841753,
-                    "timestamp": 1.350670909415927
-                },
-                {
-                    "x": 7.011808183476814,
-                    "y": 7.378579327724673,
-                    "heading": 0.17443906284640512,
-                    "angularVelocity": -0.15744627607536488,
-                    "velocityX": 3.7027215403008924,
-                    "velocityY": 0.49241177831197624,
-                    "timestamp": 1.4407156367103222
-                },
-                {
-                    "x": 7.279274281127278,
-                    "y": 7.414016049929063,
-                    "heading": 0.11701095642225132,
-                    "angularVelocity": -0.637773117313093,
-                    "velocityX": 2.9703693451811146,
-                    "velocityY": 0.3935457774060549,
-                    "timestamp": 1.5307603640047174
-                },
-                {
-                    "x": 7.479727163444043,
-                    "y": 7.440534681549159,
-                    "heading": 0.061439635089441606,
-                    "angularVelocity": -0.6171524197205129,
-                    "velocityX": 2.226147919371209,
-                    "velocityY": 0.294505102263186,
-                    "timestamp": 1.6208050912991125
-                },
+                    "x": 2.31173856270728,
+                    "y": 6.7441134449795,
+                    "heading": 0.37806624882590184,
+                    "angularVelocity": -0.25868952140989293,
+                    "velocityX": 0.2544583948907238,
+                    "velocityY": -0.279343577911277,
+                    "timestamp": 0.04613155998378907
+                },
                 {
-                    "x": 7.613267962945412,
-                    "y": 7.458185352165826,
-                    "heading": 0.019571051398075176,
-                    "angularVelocity": -0.46497540666073534,
-                    "velocityX": 1.4830496300440337,
-                    "velocityY": 0.19602114579078314,
-                    "timestamp": 1.7108498185935077
+                    "x": 2.335747257207826,
+                    "y": 6.718800139650514,
+                    "heading": 0.35485995839909595,
+                    "angularVelocity": -0.5030458635034395,
+                    "velocityX": 0.5204396840033778,
+                    "velocityY": -0.5487199075401175,
+                    "timestamp": 0.09226311996757813
+                },
+                {
+                    "x": 2.3726756647692695,
+                    "y": 6.681679707204912,
+                    "heading": 0.3212123448310367,
+                    "angularVelocity": -0.7293838226993238,
+                    "velocityX": 0.8005020331941938,
+                    "velocityY": -0.8046645823086491,
+                    "timestamp": 0.1383946799513672
+                },
+                {
+                    "x": 2.4233218216100387,
+                    "y": 6.6336167386650216,
+                    "heading": 0.2781690598764637,
+                    "angularVelocity": -0.9330550488580632,
+                    "velocityX": 1.097863520300784,
+                    "velocityY": -1.0418674017696439,
+                    "timestamp": 0.18452623993515627
+                },
+                {
+                    "x": 2.488661485197386,
+                    "y": 6.57586429791715,
+                    "heading": 0.22709762602154188,
+                    "angularVelocity": -1.1070823070554872,
+                    "velocityX": 1.416376632620007,
+                    "velocityY": -1.2519073876575257,
+                    "timestamp": 0.23065779991894533
+                },
+                {
+                    "x": 2.5698490572295953,
+                    "y": 6.510313323679299,
+                    "heading": 0.16995873595290442,
+                    "angularVelocity": -1.2386073674663585,
+                    "velocityX": 1.7599138650576658,
+                    "velocityY": -1.4209572418727212,
+                    "timestamp": 0.2767893599027344
+                },
+                {
+                    "x": 2.6680837365477243,
+                    "y": 6.439909324941273,
+                    "heading": 0.10980534967726638,
+                    "angularVelocity": -1.3039530052045993,
+                    "velocityX": 2.1294462912732417,
+                    "velocityY": -1.5261569034900655,
+                    "timestamp": 0.3229209198865235
+                },
+                {
+                    "x": 2.784053925984014,
+                    "y": 6.36914274527536,
+                    "heading": 0.05120415857101511,
+                    "angularVelocity": -1.2703058627725585,
+                    "velocityX": 2.5139013178189233,
+                    "velocityY": -1.5340166187915734,
+                    "timestamp": 0.36905247987031253
+                },
+                {
+                    "x": 2.916773557662964,
+                    "y": 6.303858757019043,
+                    "heading": 0,
+                    "angularVelocity": -1.1099593984900726,
+                    "velocityX": 2.8769812190523862,
+                    "velocityY": -1.4151697510176988,
+                    "timestamp": 0.41518403985410157
+                },
+                {
+                    "x": 3.221120615978503,
+                    "y": 6.218848325437687,
+                    "heading": -0.053482212757092314,
+                    "angularVelocity": -0.6045874919246804,
+                    "velocityX": 3.4404789027216576,
+                    "velocityY": -0.9609969552052925,
+                    "timestamp": 0.5036447067905502
+                },
+                {
+                    "x": 3.554544204530565,
+                    "y": 6.190324835643033,
+                    "heading": -0.053482256222138806,
+                    "angularVelocity": -4.913488445928212e-7,
+                    "velocityX": 3.769173352396249,
+                    "velocityY": -0.32244262656470185,
+                    "timestamp": 0.5921053737269988
+                },
+                {
+                    "x": 3.8871649424892416,
+                    "y": 6.227044557920147,
+                    "heading": -0.05348225420791585,
+                    "angularVelocity": 2.276970118304524e-8,
+                    "velocityX": 3.760097560621335,
+                    "velocityY": 0.41509660223897277,
+                    "timestamp": 0.6805660406634474
+                },
+                {
+                    "x": 4.206338511891951,
+                    "y": 6.327608602444822,
+                    "heading": -0.05348225236183454,
+                    "angularVelocity": 2.0868950806255528e-8,
+                    "velocityX": 3.608084592353424,
+                    "velocityY": 1.1368221381026216,
+                    "timestamp": 0.769026707599896
+                },
+                {
+                    "x": 4.5243112033895745,
+                    "y": 6.4319075091933104,
+                    "heading": -0.05348225052024108,
+                    "angularVelocity": 2.0818218151781936e-8,
+                    "velocityX": 3.5945093170737707,
+                    "velocityY": 1.1790427357213897,
+                    "timestamp": 0.8574873745363446
+                },
+                {
+                    "x": 4.842283928689835,
+                    "y": 6.536206312888755,
+                    "heading": -0.053482248678638385,
+                    "angularVelocity": 2.0818322537631325e-8,
+                    "velocityX": 3.59450969919429,
+                    "velocityY": 1.1790415707624449,
+                    "timestamp": 0.9459480414727932
+                },
+                {
+                    "x": 5.160256832233298,
+                    "y": 6.640504573177801,
+                    "heading": -0.05348224683713029,
+                    "angularVelocity": 2.0817253121924564e-8,
+                    "velocityX": 3.5945117141373055,
+                    "velocityY": 1.1790354278469999,
+                    "timestamp": 1.034408708409242
+                },
+                {
+                    "x": 5.478229837776281,
+                    "y": 6.744802522501547,
+                    "heading": -0.05348224499575697,
+                    "angularVelocity": 2.081572963279097e-8,
+                    "velocityX": 3.594512867186711,
+                    "velocityY": 1.1790319125524373,
+                    "timestamp": 1.1228693753456906
+                },
+                {
+                    "x": 5.79620288183899,
+                    "y": 6.849100354390164,
+                    "heading": -0.05348224315440185,
+                    "angularVelocity": 2.0815523766741823e-8,
+                    "velocityX": 3.5945133026313907,
+                    "velocityY": 1.1790305850118254,
+                    "timestamp": 1.2113300422821394
+                },
+                {
+                    "x": 6.114175956134068,
+                    "y": 6.9533980941092235,
+                    "heading": -0.053482241313040024,
+                    "angularVelocity": 2.081559959710574e-8,
+                    "velocityX": 3.5945136443919665,
+                    "velocityY": 1.179029543084821,
+                    "timestamp": 1.299790709218588
+                },
+                {
+                    "x": 6.432149055252676,
+                    "y": 7.057695758148605,
+                    "heading": -0.05348223947166161,
+                    "angularVelocity": 2.0815787162382163e-8,
+                    "velocityX": 3.594513925008545,
+                    "velocityY": 1.1790286875669842,
+                    "timestamp": 1.3882513761550368
+                },
+                {
+                    "x": 6.750122174049358,
+                    "y": 7.16199336219523,
+                    "heading": -0.05348223763026628,
+                    "angularVelocity": 2.081597839391937e-8,
+                    "velocityX": 3.594514147458541,
+                    "velocityY": 1.1790280093813261,
+                    "timestamp": 1.4767120430914855
+                },
+                {
+                    "x": 7.060081426834384,
+                    "y": 7.263662245896702,
+                    "heading": -0.036655668946107355,
+                    "angularVelocity": 0.1902152591303347,
+                    "velocityX": 3.503921726112515,
+                    "velocityY": 1.1493117474969148,
+                    "timestamp": 1.5651727100279342
+                },
+                {
+                    "x": 7.308048847978653,
+                    "y": 7.344997351864717,
+                    "heading": -0.023193676123292754,
+                    "angularVelocity": 0.15218054858760893,
+                    "velocityX": 2.8031375947279678,
+                    "velocityY": 0.9194493867703608,
+                    "timestamp": 1.653633376964383
+                },
+                {
+                    "x": 7.4940244209186755,
+                    "y": 7.405998678341617,
+                    "heading": -0.013096928746230797,
+                    "angularVelocity": 0.11413826875525969,
+                    "velocityX": 2.102353276102148,
+                    "velocityY": 0.6895870061744361,
+                    "timestamp": 1.7420940439008317
+                },
+                {
+                    "x": 7.618008139456229,
+                    "y": 7.4466662269505,
+                    "heading": -0.006365663946800556,
+                    "angularVelocity": 0.07609330827525898,
+                    "velocityX": 1.4015688874087386,
+                    "velocityY": 0.4597246439267693,
+                    "timestamp": 1.8305547108372804
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
                     "heading": -0.003,
-                    "angularVelocity": -0.2506648870652985,
-                    "velocityX": 0.7410987745724689,
-                    "velocityY": 0.09789188216823574,
-                    "timestamp": 1.8008945458879029
+                    "angularVelocity": 0.038047010760369614,
+                    "velocityX": 0.7007844581174931,
+                    "velocityY": 0.2298623077769412,
+                    "timestamp": 1.919015377773729
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
                     "heading": -0.003,
-                    "angularVelocity": 5.335131961970383e-32,
+                    "angularVelocity": -2.311552385941746e-29,
                     "velocityX": 0,
-                    "velocityY": -3.2330177098423496e-35,
-                    "timestamp": 1.890939273182298
+                    "velocityY": -6.724711823689692e-32,
+                    "timestamp": 2.007476044710178
                 }
             ],
             "constraints": [
@@ -11679,7 +13056,7 @@
                 {
                     "x": 5.61,
                     "y": 5.44,
-                    "radius": 0.5
+                    "radius": 0.49965229137145656
                 }
             ]
         },
@@ -11692,7 +13069,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 17
+                    "controlIntervalCount": 9
+                },
+                {
+                    "x": 2.996485471725464,
+                    "y": 6.224146842956543,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 14
                 },
                 {
                     "x": 6.113985538482666,
@@ -11718,253 +13104,307 @@
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.39,
-                    "angularVelocity": 2.274971502954598e-31,
-                    "velocityX": 3.487335890979115e-33,
-                    "velocityY": 2.862188318424697e-32,
+                    "angularVelocity": 0,
+                    "velocityX": 0,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.3417817158351957,
-                    "y": 6.761449131523058,
-                    "heading": 0.37953631669999827,
-                    "angularVelocity": -0.14722299547472617,
-                    "velocityX": 0.5878646347531417,
-                    "velocityY": 0.06259884318982863,
-                    "timestamp": 0.07107370194626751
-                },
-                {
-                    "x": 2.42534939307215,
-                    "y": 6.770350816518497,
-                    "heading": 0.3588464620969251,
-                    "angularVelocity": -0.2911042204995314,
-                    "velocityX": 1.1757890042104941,
-                    "velocityY": 0.12524583287024788,
-                    "timestamp": 0.14214740389253502
-                },
-                {
-                    "x": 2.5507092244237364,
-                    "y": 6.783710205716013,
-                    "heading": 0.3282884162089386,
-                    "angularVelocity": -0.42994870185747563,
-                    "velocityX": 1.7638005045292426,
-                    "velocityY": 0.18796529281121122,
-                    "timestamp": 0.21322110583880255
-                },
-                {
-                    "x": 2.717871083967313,
-                    "y": 6.80153596037414,
-                    "heading": 0.2884653946252666,
-                    "angularVelocity": -0.5603059991696991,
-                    "velocityX": 2.351950932145827,
-                    "velocityY": 0.25080661580833774,
-                    "timestamp": 0.28429480778507005
-                },
-                {
-                    "x": 2.926852904271386,
-                    "y": 6.82384561239607,
-                    "heading": 0.2406022074409234,
-                    "angularVelocity": -0.673430338841082,
-                    "velocityX": 2.9403536692385535,
-                    "velocityY": 0.31389461096009597,
-                    "timestamp": 0.35536850973133755
-                },
-                {
-                    "x": 3.1776924244019935,
-                    "y": 6.85069218229951,
-                    "heading": 0.1884744580400949,
-                    "angularVelocity": -0.7334323100298853,
-                    "velocityX": 3.529287391280776,
-                    "velocityY": 0.3777285995842137,
-                    "timestamp": 0.42644221167760504
-                },
-                {
-                    "x": 3.4450596021465354,
-                    "y": 6.879057072545224,
-                    "heading": 0.1884744481252109,
-                    "angularVelocity": -1.3950144320454384e-7,
-                    "velocityX": 3.7618299092775445,
-                    "velocityY": 0.39909121755204374,
-                    "timestamp": 0.49751591362387254
-                },
-                {
-                    "x": 3.712426777772062,
-                    "y": 6.907421982754425,
-                    "heading": 0.18847443820909762,
-                    "angularVelocity": -1.3951873937625683e-7,
-                    "velocityX": 3.7618298794631864,
-                    "velocityY": 0.39909149843660235,
-                    "timestamp": 0.5685896155701401
-                },
-                {
-                    "x": 3.979793955045072,
-                    "y": 6.93578687743453,
-                    "heading": 0.188474428292985,
-                    "angularVelocity": -1.3951872987555026e-7,
-                    "velocityX": 3.7618299026431106,
-                    "velocityY": 0.3990912799437423,
-                    "timestamp": 0.6396633175164076
-                },
-                {
-                    "x": 4.247161134956238,
-                    "y": 6.964151747247427,
-                    "heading": 0.18847441837687146,
-                    "angularVelocity": -1.395187425580786e-7,
-                    "velocityX": 3.761829939761708,
-                    "velocityY": 0.399090930064572,
-                    "timestamp": 0.7107370194626751
-                },
-                {
-                    "x": 4.514528318048531,
-                    "y": 6.992516587075052,
-                    "heading": 0.18847440846075478,
-                    "angularVelocity": -1.3951878716159625e-7,
-                    "velocityX": 3.7618299845198404,
-                    "velocityY": 0.39909050817474234,
-                    "timestamp": 0.7818107214089426
-                },
-                {
-                    "x": 4.781895504744899,
-                    "y": 7.020881392930694,
-                    "heading": 0.18847439854463183,
-                    "angularVelocity": -1.395188750932112e-7,
-                    "velocityX": 3.761830035228834,
-                    "velocityY": 0.3990900301922755,
-                    "timestamp": 0.8528844233552101
-                },
-                {
-                    "x": 5.049262689335958,
-                    "y": 7.049246218630208,
-                    "heading": 0.18847438862849922,
-                    "angularVelocity": -1.3951901124406535e-7,
-                    "velocityX": 3.7618300056073353,
-                    "velocityY": 0.3990903093936624,
-                    "timestamp": 0.9239581253014776
-                },
-                {
-                    "x": 5.3166271708653765,
-                    "y": 7.077636511969196,
-                    "heading": 0.18847437871238637,
-                    "angularVelocity": -1.3951873300413244e-7,
-                    "velocityX": 3.761791973795463,
-                    "velocityY": 0.3994486365804798,
-                    "timestamp": 0.9950318272477451
-                },
-                {
-                    "x": 5.584732592713635,
-                    "y": 7.097866624574175,
-                    "heading": 0.18847436874075538,
-                    "angularVelocity": -1.4029986815741496e-7,
-                    "velocityX": 3.772216931248832,
-                    "velocityY": 0.28463569578063386,
-                    "timestamp": 1.0661055291940127
-                },
-                {
-                    "x": 5.85271010689652,
-                    "y": 7.076007397472714,
-                    "heading": 0.188474358062239,
-                    "angularVelocity": -1.5024567569917294e-7,
-                    "velocityX": 3.7704172829702145,
-                    "velocityY": -0.3075571766057785,
-                    "timestamp": 1.1371792311402802
+                    "x": 2.3119316870686255,
+                    "y": 6.7398994499669,
+                    "heading": 0.3779608263213178,
+                    "angularVelocity": -0.23922354190742906,
+                    "velocityX": 0.23708773688854562,
+                    "velocityY": -0.3397952597632521,
+                    "timestamp": 0.05032604058401226
+                },
+                {
+                    "x": 2.3368398722232935,
+                    "y": 6.706448700500007,
+                    "heading": 0.3544233266085098,
+                    "angularVelocity": -0.4677002092687843,
+                    "velocityX": 0.49493631657919795,
+                    "velocityY": -0.6646807314605355,
+                    "timestamp": 0.10065208116802452
+                },
+                {
+                    "x": 2.3760366485894098,
+                    "y": 6.657739626207859,
+                    "heading": 0.32015686455898285,
+                    "angularVelocity": -0.6808892901544442,
+                    "velocityX": 0.7788567491352839,
+                    "velocityY": -0.9678701866250936,
+                    "timestamp": 0.1509781217520368
+                },
+                {
+                    "x": 2.431161593733442,
+                    "y": 6.595431091068774,
+                    "heading": 0.27624213154004224,
+                    "angularVelocity": -0.8726045703046355,
+                    "velocityX": 1.0953562907856245,
+                    "velocityY": -1.2380973034243006,
+                    "timestamp": 0.20130416233604903
+                },
+                {
+                    "x": 2.504176998220011,
+                    "y": 6.522134358575033,
+                    "heading": 0.22423189981090158,
+                    "angularVelocity": -1.0334656000271036,
+                    "velocityX": 1.450847387143028,
+                    "velocityY": -1.456437495244302,
+                    "timestamp": 0.2516302029200613
+                },
+                {
+                    "x": 2.597103761438428,
+                    "y": 6.441982348073212,
+                    "heading": 0.16656086207364196,
+                    "angularVelocity": -1.1459482420634122,
+                    "velocityX": 1.8464946206783215,
+                    "velocityY": -1.5926548079620442,
+                    "timestamp": 0.3019562435040735
+                },
+                {
+                    "x": 2.711127221877276,
+                    "y": 6.361040590595544,
+                    "heading": 0.10700626596614256,
+                    "angularVelocity": -1.1833753543173746,
+                    "velocityX": 2.265695038108593,
+                    "velocityY": -1.6083474189181441,
+                    "timestamp": 0.35228228408808576
+                },
+                {
+                    "x": 2.8452820247148356,
+                    "y": 6.2864334030632,
+                    "heading": 0.05015861793730577,
+                    "angularVelocity": -1.1295871355889644,
+                    "velocityX": 2.6657134414062895,
+                    "velocityY": -1.482476798622636,
+                    "timestamp": 0.402608324672098
+                },
+                {
+                    "x": 2.996485471725464,
+                    "y": 6.224146842956543,
+                    "heading": 0,
+                    "angularVelocity": -0.9966732402395141,
+                    "velocityX": 3.004477309479889,
+                    "velocityY": -1.237660650109664,
+                    "timestamp": 0.45293436525611025
+                },
+                {
+                    "x": 3.2087154283406467,
+                    "y": 6.171188923340848,
+                    "heading": -0.04850064735870574,
+                    "angularVelocity": -0.763447552582083,
+                    "velocityX": 3.340706769624593,
+                    "velocityY": -0.8336093706421461,
+                    "timestamp": 0.5164628253247648
+                },
+                {
+                    "x": 3.43894020563635,
+                    "y": 6.146133056520132,
+                    "heading": -0.0773967080078905,
+                    "angularVelocity": -0.45485221297638595,
+                    "velocityX": 3.623962819921881,
+                    "velocityY": -0.3944038119865702,
+                    "timestamp": 0.5799912853934195
+                },
+                {
+                    "x": 3.6791864985656395,
+                    "y": 6.1522055528567385,
+                    "heading": -0.07740096735197836,
+                    "angularVelocity": -0.00006704623623761945,
+                    "velocityX": 3.781711262474177,
+                    "velocityY": 0.09558702241564195,
+                    "timestamp": 0.6435197454620741
+                },
+                {
+                    "x": 3.9162306085794563,
+                    "y": 6.191776893986129,
+                    "heading": -0.0774009621424873,
+                    "angularVelocity": 8.200247652314305e-8,
+                    "velocityX": 3.731305776303155,
+                    "velocityY": 0.6228915526463914,
+                    "timestamp": 0.7070482055307288
+                },
+                {
+                    "x": 4.1454234559206995,
+                    "y": 6.264071031070342,
+                    "heading": -0.0774009579813014,
+                    "angularVelocity": 6.550112963869378e-8,
+                    "velocityX": 3.607719234710775,
+                    "velocityY": 1.1379803163195765,
+                    "timestamp": 0.7705766655993834
+                },
+                {
+                    "x": 4.362271301821872,
+                    "y": 6.367670338469585,
+                    "heading": -0.07740095431740261,
+                    "angularVelocity": 5.7673344791685836e-8,
+                    "velocityX": 3.4133968565714166,
+                    "velocityY": 1.630754268044481,
+                    "timestamp": 0.8341051256680381
+                },
+                {
+                    "x": 4.562525576285781,
+                    "y": 6.500538810004899,
+                    "heading": -0.07740095080920495,
+                    "angularVelocity": 5.522245710837058e-8,
+                    "velocityX": 3.152197837748554,
+                    "velocityY": 2.091479494256989,
+                    "timestamp": 0.8976335857366927
+                },
+                {
+                    "x": 4.76192015617459,
+                    "y": 6.634693969014619,
+                    "heading": -0.07740094730433215,
+                    "angularVelocity": 5.517012038625121e-8,
+                    "velocityX": 3.138665405604548,
+                    "velocityY": 2.111733211614557,
+                    "timestamp": 0.9611620458053474
+                },
+                {
+                    "x": 4.961315208646695,
+                    "y": 6.76884842562192,
+                    "heading": -0.07740094379946605,
+                    "angularVelocity": 5.5170015187013835e-8,
+                    "velocityX": 3.138672844527154,
+                    "velocityY": 2.1117221551145704,
+                    "timestamp": 1.024690505874002
+                },
+                {
+                    "x": 5.173514155427892,
+                    "y": 6.881664257521493,
+                    "heading": -0.07740094020412008,
+                    "angularVelocity": 5.659425663991619e-8,
+                    "velocityX": 3.3402186445551516,
+                    "velocityY": 1.7758313640477164,
+                    "timestamp": 1.0882189659426564
+                },
+                {
+                    "x": 5.399391012764095,
+                    "y": 6.963734040630747,
+                    "heading": -0.07740093622781413,
+                    "angularVelocity": 6.259093865144696e-8,
+                    "velocityX": 3.5555223138118204,
+                    "velocityY": 1.2918585311301378,
+                    "timestamp": 1.151747426011311
+                },
+                {
+                    "x": 5.634517116552883,
+                    "y": 7.013448397757994,
+                    "heading": -0.07740093140263109,
+                    "angularVelocity": 7.595309327466021e-8,
+                    "velocityX": 3.7011144852982407,
+                    "velocityY": 0.7825525295830946,
+                    "timestamp": 1.2152758860799655
+                },
+                {
+                    "x": 5.874282332827973,
+                    "y": 7.02983251834796,
+                    "heading": -0.07740092481326658,
+                    "angularVelocity": 1.0372303211074395e-7,
+                    "velocityX": 3.7741386461434376,
+                    "velocityY": 0.2579020579478282,
+                    "timestamp": 1.27880434614862
                 },
                 {
                     "x": 6.113985538482666,
                     "y": 7.0125651359558105,
-                    "heading": 0.18847434515977443,
-                    "angularVelocity": -1.8153640819270364e-7,
-                    "velocityX": 3.6761196396335167,
-                    "velocityY": -0.8926263833128883,
-                    "timestamp": 1.2082529330865477
-                },
-                {
-                    "x": 6.38971197835489,
-                    "y": 6.891444245730791,
-                    "heading": 0.18847433517810044,
-                    "angularVelocity": -1.2538350348338093e-7,
-                    "velocityX": 3.4635019217336325,
-                    "velocityY": -1.521444357133088,
-                    "timestamp": 1.2878620818911364
-                },
-                {
-                    "x": 6.640022696468843,
-                    "y": 6.723993100546061,
-                    "heading": 0.18847432674418188,
-                    "angularVelocity": -1.0594157434839283e-7,
-                    "velocityX": 3.1442456284571,
-                    "velocityY": -2.103415847287377,
-                    "timestamp": 1.367471230695725
-                },
-                {
-                    "x": 6.8648306229514935,
-                    "y": 6.523601637914116,
-                    "heading": 0.18847431867026868,
-                    "angularVelocity": -1.014194138550125e-7,
-                    "velocityX": 2.8238956182605124,
-                    "velocityY": -2.5171913736164258,
-                    "timestamp": 1.4470803795003138
-                },
-                {
-                    "x": 7.089642217529903,
-                    "y": 6.3232142903896476,
-                    "heading": 0.1884743105959038,
-                    "angularVelocity": -1.0142508708190059e-7,
-                    "velocityX": 2.823941694568014,
-                    "velocityY": -2.517139682229462,
-                    "timestamp": 1.5266895283049025
-                },
-                {
-                    "x": 7.2864279243188,
-                    "y": 6.147809259471006,
-                    "heading": 0.12465186300240168,
-                    "angularVelocity": -0.8016973997575559,
-                    "velocityX": 2.471898139143281,
-                    "velocityY": -2.203327551576542,
-                    "timestamp": 1.6062986771094911
-                },
-                {
-                    "x": 7.443856673448633,
-                    "y": 6.007485445692029,
-                    "heading": 0.07359246308164451,
-                    "angularVelocity": -0.6413760313670541,
-                    "velocityX": 1.9775208188222257,
-                    "velocityY": -1.7626593913656032,
-                    "timestamp": 1.6859078259140798
-                },
-                {
-                    "x": 7.561928317357885,
-                    "y": 5.902242696623479,
-                    "heading": 0.0352966131369852,
-                    "angularVelocity": -0.4810483533581169,
-                    "velocityX": 1.483141644926293,
-                    "velocityY": -1.3219931458747511,
-                    "timestamp": 1.7655169747186685
-                },
-                {
-                    "x": 7.640642772199918,
-                    "y": 5.832080900256126,
-                    "heading": 0.009765510491971057,
-                    "angularVelocity": -0.320705635324694,
-                    "velocityX": 0.9887614178017027,
-                    "velocityY": -0.8813283073733266,
-                    "timestamp": 1.8451261235232572
+                    "heading": -0.07740091266766538,
+                    "angularVelocity": 1.9118362343988029e-7,
+                    "velocityX": 3.773162538422118,
+                    "velocityY": -0.27180546126086014,
+                    "timestamp": 1.3423328062172746
+                },
+                {
+                    "x": 6.409905183229014,
+                    "y": 6.936971954690801,
+                    "heading": -0.07740090470499697,
+                    "angularVelocity": 9.862508855382814e-8,
+                    "velocityX": 3.66524129970236,
+                    "velocityY": -0.9362921822438863,
+                    "timestamp": 1.4230695491897984
+                },
+                {
+                    "x": 6.687740416068466,
+                    "y": 6.8101241855802925,
+                    "heading": -0.0774008999193729,
+                    "angularVelocity": 5.9274425678096215e-8,
+                    "velocityX": 3.441248960637469,
+                    "velocityY": -1.5711281436467635,
+                    "timestamp": 1.5038062921623223
+                },
+                {
+                    "x": 6.938692776760068,
+                    "y": 6.63603889201252,
+                    "heading": -0.0774008963221294,
+                    "angularVelocity": 4.455522177089406e-8,
+                    "velocityX": 3.108279470438973,
+                    "velocityY": -2.156209021548162,
+                    "timestamp": 1.5845430351348462
+                },
+                {
+                    "x": 7.154815342738161,
+                    "y": 6.420229219532517,
+                    "heading": -0.07740089316210408,
+                    "angularVelocity": 3.913986631676039e-8,
+                    "velocityX": 2.6768799188697083,
+                    "velocityY": -2.6730044405364133,
+                    "timestamp": 1.66527977810737
+                },
+                {
+                    "x": 7.329876929572681,
+                    "y": 6.2124862031192585,
+                    "heading": -0.05260075245643602,
+                    "angularVelocity": 0.3071729152375715,
+                    "velocityX": 2.1683013258795385,
+                    "velocityY": -2.5730913678789142,
+                    "timestamp": 1.746016521079894
+                },
+                {
+                    "x": 7.469926171014247,
+                    "y": 6.046291741893866,
+                    "heading": -0.03276050638448697,
+                    "angularVelocity": 0.2457399858041329,
+                    "velocityX": 1.7346407135748194,
+                    "velocityY": -2.0584736900019895,
+                    "timestamp": 1.8267532640524178
+                },
+                {
+                    "x": 7.574963090588109,
+                    "y": 5.921645878284834,
+                    "heading": -0.01788026453634592,
+                    "angularVelocity": 0.184305698995044,
+                    "velocityX": 1.300980392652287,
+                    "velocityY": -1.5438554866085237,
+                    "timestamp": 1.9074900070249416
+                },
+                {
+                    "x": 7.644987698235257,
+                    "y": 5.8385486280487005,
+                    "heading": -0.007960086700925195,
+                    "angularVelocity": 0.12287067164441078,
+                    "velocityX": 0.8673201948582164,
+                    "velocityY": -1.0292370880556136,
+                    "timestamp": 1.9882267499974655
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
                     "heading": -0.003,
-                    "angularVelocity": -0.1603523047748632,
-                    "velocityX": 0.4943807136624207,
-                    "velocityY": -0.44066417971947147,
-                    "timestamp": 1.924735272327846
+                    "angularVelocity": 0.06143530836524976,
+                    "velocityX": 0.43366007192855915,
+                    "velocityY": -0.5146185803264369,
+                    "timestamp": 2.0689634929699894
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
                     "heading": -0.003,
-                    "angularVelocity": 9.952815155734959e-29,
-                    "velocityX": 0,
-                    "velocityY": 1.270429811278963e-30,
-                    "timestamp": 2.004344421132435
+                    "angularVelocity": -2.7405624218065555e-29,
+                    "velocityX": 1.4947392660092065e-31,
+                    "velocityY": 0,
+                    "timestamp": 2.1497002359425132
                 }
             ],
             "constraints": [
@@ -12003,6 +13443,24 @@
                     "headingConstrained": true,
                     "controlIntervalCount": 10
                 },
+                {
+                    "x": 2.916773796081543,
+                    "y": 6.28393030166626,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 10
+                },
+                {
+                    "x": 4.1324920654296875,
+                    "y": 5.8253936767578125,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 10
+                },
                 {
                     "x": 4.748738765716553,
                     "y": 4.2041850090026855,
@@ -12033,184 +13491,364 @@
                     "timestamp": 0
                 },
                 {
-                    "x": 2.3668131239789423,
-                    "y": 6.6799532352680275,
-                    "heading": 0.31688592464549686,
-                    "angularVelocity": -0.6546901107445974,
-                    "velocityX": 0.5982690928509629,
-                    "velocityY": -0.6899048465062367,
-                    "timestamp": 0.11167737858654542
-                },
-                {
-                    "x": 2.5010623988117353,
-                    "y": 6.525329092787392,
-                    "heading": 0.1875032700506318,
-                    "angularVelocity": -1.1585395022018103,
-                    "velocityX": 1.202116995688263,
-                    "velocityY": -1.3845609955896643,
-                    "timestamp": 0.22335475717309083
-                },
-                {
-                    "x": 2.7034647042549174,
-                    "y": 6.29238592031536,
-                    "heading": 0.03684167640916454,
-                    "angularVelocity": -1.3490788873122188,
-                    "velocityX": 1.8123841014618285,
-                    "velocityY": -2.0858581694904785,
-                    "timestamp": 0.33503213575963625
-                },
-                {
-                    "x": 2.970402662431944,
-                    "y": 5.984552676414473,
-                    "heading": 2.692880535790558e-7,
-                    "angularVelocity": -0.32989140314179594,
-                    "velocityX": 2.39025988571325,
-                    "velocityY": -2.7564511971627637,
-                    "timestamp": 0.44670951434618167
-                },
-                {
-                    "x": 3.247779632307318,
-                    "y": 5.665896160181276,
-                    "heading": 2.2674846776964554e-7,
-                    "angularVelocity": -3.809149744305609e-7,
-                    "velocityX": 2.4837346057547505,
-                    "velocityY": -2.853366727141142,
-                    "timestamp": 0.5583868929327271
-                },
-                {
-                    "x": 3.5251566242922663,
-                    "y": 5.347239663193555,
-                    "heading": 1.842089099097335e-7,
-                    "angularVelocity": -3.8091472416520575e-7,
-                    "velocityX": 2.4837348037318896,
-                    "velocityY": -2.853366554810183,
-                    "timestamp": 0.6700642715192726
-                },
-                {
-                    "x": 3.8025335437012195,
-                    "y": 5.0285831030297485,
-                    "heading": 1.4166936288049968e-7,
-                    "angularVelocity": -3.8091462718327505e-7,
-                    "velocityX": 2.4837341538598574,
-                    "velocityY": -2.8533671205119875,
-                    "timestamp": 0.7817416501058181
+                    "x": 2.308859553563438,
+                    "y": 6.744970476661412,
+                    "heading": 0.38132184535212243,
+                    "angularVelocity": -0.20370231005101772,
+                    "velocityX": 0.20796028650336576,
+                    "velocityY": -0.2823689819220092,
+                    "timestamp": 0.04260214155501803
                 },
                 {
-                    "x": 4.079910865419093,
-                    "y": 4.7099268930580935,
-                    "heading": 9.912982095474378e-8,
-                    "angularVelocity": -3.809145814844431e-7,
-                    "velocityX": 2.4837377562807075,
-                    "velocityY": -2.853363984763686,
-                    "timestamp": 0.8934190286923636
+                    "x": 2.3271744503017344,
+                    "y": 6.7213636093363665,
+                    "heading": 0.36416244677454657,
+                    "angularVelocity": -0.4027825351318442,
+                    "velocityX": 0.4299055415945176,
+                    "velocityY": -0.554123958640874,
+                    "timestamp": 0.08520428311003606
                 },
-                {
-                    "x": 4.382964098552189,
-                    "y": 4.415582373772677,
-                    "heading": 5.542611481257948e-8,
-                    "angularVelocity": -3.9133893269333644e-7,
-                    "velocityX": 2.713649236476739,
-                    "velocityY": -2.635668234792112,
-                    "timestamp": 1.005096407278909
+                {
+                    "x": 2.355668020695042,
+                    "y": 6.686794512902429,
+                    "heading": 0.33880572426100314,
+                    "angularVelocity": -0.5951983066578187,
+                    "velocityX": 0.6688295318795241,
+                    "velocityY": -0.8114403448308736,
+                    "timestamp": 0.1278064246650541
+                },
+                {
+                    "x": 2.3952194813587413,
+                    "y": 6.64212767454663,
+                    "heading": 0.305649210011626,
+                    "angularVelocity": -0.7782828054913067,
+                    "velocityX": 0.9283913723590802,
+                    "velocityY": -1.0484646246742158,
+                    "timestamp": 0.1704085662200721
+                },
+                {
+                    "x": 2.4468808744427974,
+                    "y": 6.58861694166169,
+                    "heading": 0.2652376454484053,
+                    "angularVelocity": -0.9485805898051318,
+                    "velocityX": 1.2126477965277631,
+                    "velocityY": -1.2560573466907532,
+                    "timestamp": 0.21301070777509012
+                },
+                {
+                    "x": 2.5118430486187977,
+                    "y": 6.5281213977332095,
+                    "heading": 0.2183393877421899,
+                    "angularVelocity": -1.100842727487048,
+                    "velocityX": 1.5248570096436473,
+                    "velocityY": -1.4200118050486843,
+                    "timestamp": 0.25561284933010814
+                },
+                {
+                    "x": 2.5912567436103777,
+                    "y": 6.463385022311091,
+                    "heading": 0.16616355843360497,
+                    "angularVelocity": -1.2247231571962895,
+                    "velocityX": 1.864077534436207,
+                    "velocityY": -1.5195568358579936,
+                    "timestamp": 0.29821499088512615
+                },
+                {
+                    "x": 2.6857686356352475,
+                    "y": 6.398206753066201,
+                    "heading": 0.11066645077656016,
+                    "angularVelocity": -1.3026835185121792,
+                    "velocityX": 2.21847748904391,
+                    "velocityY": -1.5299294088471524,
+                    "timestamp": 0.34081713244014417
+                },
+                {
+                    "x": 2.794870523929059,
+                    "y": 6.337041243081873,
+                    "heading": 0.05441835033855986,
+                    "angularVelocity": -1.3203115708481314,
+                    "velocityX": 2.5609484479298716,
+                    "velocityY": -1.43573791719686,
+                    "timestamp": 0.3834192739951622
+                },
+                {
+                    "x": 2.916773796081543,
+                    "y": 6.28393030166626,
+                    "heading": 0,
+                    "angularVelocity": -1.2773618497155108,
+                    "velocityX": 2.8614353105947288,
+                    "velocityY": -1.246673042176136,
+                    "timestamp": 0.4260214155501802
+                },
+                {
+                    "x": 3.0391710673922954,
+                    "y": 6.243974094587608,
+                    "heading": -0.047264422869834,
+                    "angularVelocity": -1.1928681768219465,
+                    "velocityX": 3.0890847917160027,
+                    "velocityY": -1.0084220857175146,
+                    "timestamp": 0.4656439185125414
+                },
+                {
+                    "x": 3.1702962457898094,
+                    "y": 6.213670351410701,
+                    "heading": -0.08947327835139807,
+                    "angularVelocity": -1.0652748394431244,
+                    "velocityX": 3.309361312233972,
+                    "velocityY": -0.7648114306582993,
+                    "timestamp": 0.5052664214749026
+                },
+                {
+                    "x": 3.3090884442062665,
+                    "y": 6.193565770320959,
+                    "heading": -0.12122975141158922,
+                    "angularVelocity": -0.8014756940104903,
+                    "velocityX": 3.502862970274757,
+                    "velocityY": -0.5074031064832265,
+                    "timestamp": 0.5448889244372639
+                },
+                {
+                    "x": 3.4545421039318374,
+                    "y": 6.168639069925164,
+                    "heading": -0.11367597167549683,
+                    "angularVelocity": 0.19064367900402435,
+                    "velocityX": 3.6709861530893746,
+                    "velocityY": -0.6291046383282068,
+                    "timestamp": 0.5845114273996251
+                },
+                {
+                    "x": 3.5909551218669695,
+                    "y": 6.134279746914322,
+                    "heading": -0.10324626427429143,
+                    "angularVelocity": 0.26322686911306303,
+                    "velocityX": 3.442816776737095,
+                    "velocityY": -0.8671668986555798,
+                    "timestamp": 0.6241339303619864
+                },
+                {
+                    "x": 3.7180733265010666,
+                    "y": 6.090709420708735,
+                    "heading": -0.09157048631886859,
+                    "angularVelocity": 0.2946754263988333,
+                    "velocityX": 3.208232573163052,
+                    "velocityY": -1.0996358873889647,
+                    "timestamp": 0.6637564333243476
+                },
+                {
+                    "x": 3.835817587911086,
+                    "y": 6.038004134131813,
+                    "heading": -0.07918178962672723,
+                    "angularVelocity": 0.31266820028784686,
+                    "velocityX": 2.9716512740719234,
+                    "velocityY": -1.3301856933921488,
+                    "timestamp": 0.7033789362867089
+                },
+                {
+                    "x": 3.9441493716247042,
+                    "y": 5.9762022678896844,
+                    "heading": -0.06634465187684353,
+                    "angularVelocity": 0.3239860379864983,
+                    "velocityX": 2.734097434897688,
+                    "velocityY": -1.5597668400917784,
+                    "timestamp": 0.7430014392490701
+                },
+                {
+                    "x": 4.0430458841375625,
+                    "y": 5.905326959185002,
+                    "heading": -0.05321706206750646,
+                    "angularVelocity": 0.3313165203573185,
+                    "velocityX": 2.4959683290781256,
+                    "velocityY": -1.7887640458253868,
+                    "timestamp": 0.7826239422114314
+                },
+                {
+                    "x": 4.1324920654296875,
+                    "y": 5.8253936767578125,
+                    "heading": -0.03990404951279715,
+                    "angularVelocity": 0.33599625362778834,
+                    "velocityX": 2.2574591357111773,
+                    "velocityY": -2.017370848659449,
+                    "timestamp": 0.8222464451737926
+                },
+                {
+                    "x": 4.241428434426057,
+                    "y": 5.691951873954815,
+                    "heading": -0.020558340356236417,
+                    "angularVelocity": 0.3400028267859621,
+                    "velocityX": 1.9145678816329739,
+                    "velocityY": -2.345253399462351,
+                    "timestamp": 0.8791451166920127
+                },
+                {
+                    "x": 4.330837596388679,
+                    "y": 5.539871857431794,
+                    "heading": -0.0011062192039580838,
+                    "angularVelocity": 0.34187302854776463,
+                    "velocityX": 1.571375211001045,
+                    "velocityY": -2.6728219212345348,
+                    "timestamp": 0.9360437882102327
+                },
+                {
+                    "x": 4.400691025411647,
+                    "y": 5.369183558238886,
+                    "heading": 0.018249380247094018,
+                    "angularVelocity": 0.34017664972817613,
+                    "velocityX": 1.2276811946408939,
+                    "velocityY": -2.999864401724217,
+                    "timestamp": 0.9929424597284527
+                },
+                {
+                    "x": 4.450931892836431,
+                    "y": 5.179947405025881,
+                    "heading": 0.037101343157351356,
+                    "angularVelocity": 0.3313251857597514,
+                    "velocityX": 0.8829884087662048,
+                    "velocityY": -3.3258448424829754,
+                    "timestamp": 1.0498411312466729
+                },
+                {
+                    "x": 4.481391591894744,
+                    "y": 4.9723492336445485,
+                    "heading": 0.054218740493726776,
+                    "angularVelocity": 0.3008400175194617,
+                    "velocityX": 0.5353323416094045,
+                    "velocityY": -3.6485591990465953,
+                    "timestamp": 1.106739802764893
+                },
+                {
+                    "x": 4.496396423991072,
+                    "y": 4.780842919446315,
+                    "heading": 0.0400753404119281,
+                    "angularVelocity": -0.248571710101697,
+                    "velocityX": 0.26371146629537756,
+                    "velocityY": -3.3657431551263746,
+                    "timestamp": 1.163638474283113
+                },
+                {
+                    "x": 4.530633514554346,
+                    "y": 4.608273372061934,
+                    "heading": 0.027581622688211512,
+                    "angularVelocity": -0.21957837310341866,
+                    "velocityX": 0.6017203855508131,
+                    "velocityY": -3.032927532044774,
+                    "timestamp": 1.220537145801333
+                },
+                {
+                    "x": 4.584102924369241,
+                    "y": 4.454640545632531,
+                    "heading": 0.016737783854959394,
+                    "angularVelocity": -0.1905815820283204,
+                    "velocityX": 0.9397303731032456,
+                    "velocityY": -2.7001127149375717,
+                    "timestamp": 1.277435817319553
+                },
+                {
+                    "x": 4.656804671911905,
+                    "y": 4.319944426632471,
+                    "heading": 0.007543896915478472,
+                    "angularVelocity": -0.1615835079126032,
+                    "velocityX": 1.277740685375807,
+                    "velocityY": -2.3672981355448273,
+                    "timestamp": 1.334334488837773
                 },
                 {
                     "x": 4.748738765716553,
                     "y": 4.2041850090026855,
                     "heading": 0,
-                    "angularVelocity": -4.963056575446211e-7,
-                    "velocityX": 3.27527984443961,
-                    "velocityY": -1.8929291450565913,
-                    "timestamp": 1.1167737858654545
+                    "angularVelocity": -0.13258476365415262,
+                    "velocityX": 1.6157511476381003,
+                    "velocityY": -2.0344836626408243,
+                    "timestamp": 1.391233160355993
                 },
                 {
-                    "x": 5.133803316447558,
-                    "y": 4.089523361787239,
-                    "heading": -1.387645596172588e-10,
-                    "angularVelocity": -1.3065520492479572e-9,
-                    "velocityX": 3.625615065458012,
-                    "velocityY": -1.0796085871456598,
-                    "timestamp": 1.2229804690844188
+                    "x": 5.007932614843106,
+                    "y": 4.0487951090615,
+                    "heading": -0.008478617739268947,
+                    "angularVelocity": -0.07486216912690727,
+                    "velocityX": 2.288558626731898,
+                    "velocityY": -1.372019039864535,
+                    "timestamp": 1.5044895332096289
                 },
                 {
-                    "x": 5.534974194059096,
-                    "y": 4.0675249151482635,
-                    "heading": -2.526284831735191e-10,
-                    "angularVelocity": -1.0720975375429818e-9,
-                    "velocityX": 3.7772658504404184,
-                    "velocityY": -0.20712864739049783,
-                    "timestamp": 1.3291871523033831
+                    "x": 5.344428643764395,
+                    "y": 3.9673190703928256,
+                    "heading": -0.018205683784992727,
+                    "angularVelocity": -0.08588537492980046,
+                    "velocityX": 2.971100172492302,
+                    "velocityY": -0.7193947379364598,
+                    "timestamp": 1.6177459060632646
                 },
                 {
-                    "x": 5.9365934232146325,
-                    "y": 4.0786604378791305,
-                    "heading": -3.6352969398434843e-10,
-                    "angularVelocity": -1.0442018095219078e-9,
-                    "velocityX": 3.781487350730336,
-                    "velocityY": 0.10484766488641149,
-                    "timestamp": 1.4353938355223474
+                    "x": 5.761576858213634,
+                    "y": 3.95566669131606,
+                    "heading": -0.05529230717881331,
+                    "angularVelocity": -0.3274572764373147,
+                    "velocityX": 3.6832206783483272,
+                    "velocityY": -0.10288497488635052,
+                    "timestamp": 1.7310022789169004
                 },
                 {
-                    "x": 6.3382126442540345,
-                    "y": 4.089796253283455,
-                    "heading": -4.744184411388132e-10,
-                    "angularVelocity": -1.044084456790862e-9,
-                    "velocityX": 3.781487274312031,
-                    "velocityY": 0.1048504205837075,
-                    "timestamp": 1.5416005187413118
+                    "x": 6.1883195012502945,
+                    "y": 3.9937898497766295,
+                    "heading": -0.05529230006422074,
+                    "angularVelocity": 6.281847442205705e-8,
+                    "velocityX": 3.767934927495438,
+                    "velocityY": 0.33660938894658915,
+                    "timestamp": 1.8442586517705362
                 },
                 {
-                    "x": 6.739831868633552,
-                    "y": 4.100931948201603,
-                    "heading": -5.853040774969199e-10,
-                    "angularVelocity": -1.0440551668485128e-9,
-                    "velocityX": 3.7814873057612264,
-                    "velocityY": 0.10484928613381475,
-                    "timestamp": 1.647807201960276
+                    "x": 6.615062139687873,
+                    "y": 4.031913059718256,
+                    "heading": -0.05529229294956677,
+                    "angularVelocity": 6.281901664491759e-8,
+                    "velocityX": 3.7679348868877285,
+                    "velocityY": 0.3366098434998829,
+                    "timestamp": 1.957515024624172
                 },
                 {
-                    "x": 7.11589910333758,
-                    "y": 4.111359168329562,
-                    "heading": -0.0011999964635320353,
-                    "angularVelocity": -0.011298685184913941,
-                    "velocityX": 3.5408999067290075,
-                    "velocityY": 0.09817856854143046,
-                    "timestamp": 1.7540138851792404
+                    "x": 7.0409980156903185,
+                    "y": 4.069937864184792,
+                    "heading": -0.05280872948360114,
+                    "angularVelocity": 0.021928686248633502,
+                    "velocityX": 3.7608115576232954,
+                    "velocityY": 0.33574096987616325,
+                    "timestamp": 2.0707713974778077
                 },
                 {
-                    "x": 7.397949547057075,
-                    "y": 4.119179583905487,
-                    "heading": -0.002099997192055183,
-                    "angularVelocity": -0.008474049854922212,
-                    "velocityX": 2.6556750966226486,
-                    "velocityY": 0.0736339309252122,
-                    "timestamp": 1.8602205683982047
+                    "x": 7.360520062907597,
+                    "y": 4.098469259621894,
+                    "heading": -0.032478143170072554,
+                    "angularVelocity": 0.17950942451425975,
+                    "velocityX": 2.821227973018394,
+                    "velocityY": 0.25191867546363345,
+                    "timestamp": 2.1840277703314435
                 },
                 {
-                    "x": 7.585983181328134,
-                    "y": 4.124393194553852,
-                    "heading": -0.0026999988369627403,
-                    "angularVelocity": -0.005649377484767236,
-                    "velocityX": 1.7704501126675238,
-                    "velocityY": 0.049089289773120645,
-                    "timestamp": 1.966427251617169
+                    "x": 7.573513556720189,
+                    "y": 4.1174900304811795,
+                    "heading": -0.013829576144119837,
+                    "angularVelocity": 0.16465799280057086,
+                    "velocityX": 1.88063142449255,
+                    "velocityY": 0.1679443759325155,
+                    "timestamp": 2.2972841431850792
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
                     "heading": -0.003,
-                    "angularVelocity": -0.0028246919491873987,
-                    "velocityX": 0.8852250707992916,
-                    "velocityY": 0.024544646034960003,
-                    "timestamp": 2.072633934836133
+                    "angularVelocity": 0.09562001564464007,
+                    "velocityX": 0.9402247361163992,
+                    "velocityY": 0.08396851567117221,
+                    "timestamp": 2.410540516038715
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
                     "heading": -0.003,
                     "angularVelocity": 0,
-                    "velocityX": 1.6661554323284825e-27,
+                    "velocityX": 2.757511206578506e-35,
                     "velocityY": 0,
-                    "timestamp": 2.1788406180550974
+                    "timestamp": 2.5237968888923508
                 }
             ],
             "constraints": [
@@ -12252,7 +13890,25 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 18
+                    "controlIntervalCount": 4
+                },
+                {
+                    "x": 2.2457261085510254,
+                    "y": 6.7668328285217285,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 11
+                },
+                {
+                    "x": 3.284848213195801,
+                    "y": 6.196334362030029,
+                    "heading": -0.5,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 11
                 },
                 {
                     "x": 4.274456024169922,
@@ -12287,397 +13943,469 @@
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.39,
-                    "angularVelocity": 1.592522039385819e-32,
+                    "angularVelocity": 0,
                     "velocityX": 0,
-                    "velocityY": -2.1348488243236776e-32,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.311435758812473,
-                    "y": 6.743296066265471,
-                    "heading": 0.3325883455828011,
-                    "angularVelocity": -1.0899722636212008,
-                    "velocityX": 0.2171102722189281,
-                    "velocityY": -0.26017204737900074,
-                    "timestamp": 0.052672582902671385
-                },
-                {
-                    "x": 2.3359939039892414,
-                    "y": 6.71538911279917,
-                    "heading": 0.2289766220305027,
-                    "angularVelocity": -1.9670902363712421,
-                    "velocityX": 0.46624152117519013,
-                    "velocityY": -0.529819346772266,
-                    "timestamp": 0.10534516580534277
-                },
-                {
-                    "x": 2.3766647023789824,
-                    "y": 6.674615688796359,
-                    "heading": 0.08902544501665462,
-                    "angularVelocity": -2.6570023587499336,
-                    "velocityX": 0.7721436115045406,
-                    "velocityY": -0.7740919802272123,
-                    "timestamp": 0.15801774870801416
-                },
-                {
-                    "x": 2.4341603545379895,
-                    "y": 6.621640281380937,
-                    "heading": -0.08739056422595769,
-                    "angularVelocity": -3.349294823240299,
-                    "velocityX": 1.0915669783129407,
-                    "velocityY": -1.0057491866937662,
-                    "timestamp": 0.21069033161068554
-                },
-                {
-                    "x": 2.508963105088746,
-                    "y": 6.555426903517877,
-                    "heading": -0.2922468734649172,
-                    "angularVelocity": -3.8892398654057443,
-                    "velocityX": 1.4201458601143102,
-                    "velocityY": -1.2570748236404126,
-                    "timestamp": 0.2633629145133569
-                },
-                {
-                    "x": 2.602244928931498,
-                    "y": 6.475307627430128,
-                    "heading": -0.4990571722352239,
-                    "angularVelocity": -3.9263367652285313,
-                    "velocityX": 1.7709749304513536,
-                    "velocityY": -1.5210812090949455,
-                    "timestamp": 0.3160354974160283
-                },
-                {
-                    "x": 2.7117676530035184,
-                    "y": 6.382297804514516,
-                    "heading": -0.6662019146269795,
-                    "angularVelocity": -3.173277883497873,
-                    "velocityX": 2.0793118172009177,
-                    "velocityY": -1.7658109359755114,
-                    "timestamp": 0.3687080803186997
-                },
-                {
-                    "x": 2.833611348488951,
-                    "y": 6.27498915923685,
-                    "heading": -0.7793427065081729,
-                    "angularVelocity": -2.1480015910792667,
-                    "velocityX": 2.3132280357425485,
-                    "velocityY": -2.0372770683364947,
-                    "timestamp": 0.42138066322137113
-                },
-                {
-                    "x": 2.968521622075034,
-                    "y": 6.1546184694434425,
-                    "heading": -0.8356933529016568,
-                    "angularVelocity": -1.0698288044390758,
-                    "velocityX": 2.5612997531442825,
-                    "velocityY": -2.2852627146807634,
-                    "timestamp": 0.47405324612404254
-                },
-                {
-                    "x": 3.1177887530073343,
-                    "y": 6.022632523021301,
-                    "heading": -0.8357129106693775,
-                    "angularVelocity": -0.00037130830959681015,
-                    "velocityX": 2.8338676918144654,
-                    "velocityY": -2.5057807904736045,
-                    "timestamp": 0.5267258290267139
-                },
-                {
-                    "x": 3.269422043665646,
-                    "y": 5.893362401601714,
-                    "heading": -0.8357131785361996,
-                    "angularVelocity": -0.000005085507627644201,
-                    "velocityX": 2.8787897289658386,
-                    "velocityY": -2.45422028493365,
-                    "timestamp": 0.5793984119293853
-                },
-                {
-                    "x": 3.4210553878779164,
-                    "y": 5.764092343000596,
-                    "heading": -0.8357134464026715,
-                    "angularVelocity": -0.000005085500977911915,
-                    "velocityX": 2.8787907456989505,
-                    "velocityY": -2.4542190923119205,
-                    "timestamp": 0.6320709948320568
-                },
-                {
-                    "x": 3.572688732091381,
-                    "y": 5.634822284400852,
-                    "heading": -0.8357137142690544,
-                    "angularVelocity": -0.000005085499289329294,
-                    "velocityX": 2.878790745721617,
-                    "velocityY": -2.4542190922857987,
-                    "timestamp": 0.6847435777347282
-                },
-                {
-                    "x": 3.7243220763038787,
-                    "y": 5.50555222579995,
-                    "heading": -0.8357139821353484,
-                    "angularVelocity": -0.000005085497600843703,
-                    "velocityX": 2.8787907457032484,
-                    "velocityY": -2.4542190923078118,
-                    "timestamp": 0.7374161606373996
-                },
-                {
-                    "x": 3.875955376681643,
-                    "y": 5.376282115781124,
-                    "heading": -0.8357142500017658,
-                    "angularVelocity": -0.000005085499943938472,
-                    "velocityX": 2.878789913491687,
-                    "velocityY": -2.4542200684878277,
-                    "timestamp": 0.790088743540071
-                },
-                {
-                    "x": 4.025639474822054,
-                    "y": 5.244759916762783,
-                    "heading": -0.8357145286285158,
-                    "angularVelocity": -0.000005289787108985714,
-                    "velocityX": 2.8417839014463553,
-                    "velocityY": -2.4969764490450044,
-                    "timestamp": 0.8427613264427424
-                },
-                {
-                    "x": 4.159073025842643,
-                    "y": 5.096777164207917,
-                    "heading": -0.835714908739186,
-                    "angularVelocity": -0.000007216480551859566,
-                    "velocityX": 2.5332638664625207,
-                    "velocityY": -2.809483499761346,
-                    "timestamp": 0.8954339093454138
+                    "x": 2.285270702118571,
+                    "y": 6.762383358598232,
+                    "heading": 0.3634342922015001,
+                    "angularVelocity": -0.5809173586087107,
+                    "velocityX": -0.3220883435269421,
+                    "velocityY": 0.11771892098839686,
+                    "timestamp": 0.04573061452687351
+                },
+                {
+                    "x": 2.2599794598619596,
+                    "y": 6.767831656358603,
+                    "heading": 0.2777726454574198,
+                    "angularVelocity": -1.8731794363656886,
+                    "velocityX": -0.5530483794778248,
+                    "velocityY": 0.11913895793336499,
+                    "timestamp": 0.09146122905374703
+                },
+                {
+                    "x": 2.2450548028766546,
+                    "y": 6.7710281276739375,
+                    "heading": 0.1344358774143225,
+                    "angularVelocity": -3.134372225827269,
+                    "velocityX": -0.3263602980128451,
+                    "velocityY": 0.06989784301807915,
+                    "timestamp": 0.13719184358062053
+                },
+                {
+                    "x": 2.2457261085510254,
+                    "y": 6.7668328285217285,
+                    "heading": 0,
+                    "angularVelocity": -2.939734766416754,
+                    "velocityX": 0.014679568191384731,
+                    "velocityY": -0.09173940030374239,
+                    "timestamp": 0.18292245810749405
+                },
+                {
+                    "x": 2.262817902753147,
+                    "y": 6.755151488221845,
+                    "heading": -0.1222613161134929,
+                    "angularVelocity": -2.5936618258096975,
+                    "velocityX": 0.3625867573309899,
+                    "velocityY": -0.24780893395554135,
+                    "timestamp": 0.230060953533885
+                },
+                {
+                    "x": 2.2962801062686125,
+                    "y": 6.73609161362754,
+                    "heading": -0.22784823217966704,
+                    "angularVelocity": -2.2399297031255614,
+                    "velocityX": 0.7098699950598514,
+                    "velocityY": -0.40433778002244786,
+                    "timestamp": 0.277199448960276
+                },
+                {
+                    "x": 2.3460880772414248,
+                    "y": 6.709619977067709,
+                    "heading": -0.31657680000453947,
+                    "angularVelocity": -1.882295288008833,
+                    "velocityX": 1.056630478386546,
+                    "velocityY": -0.561571520694102,
+                    "timestamp": 0.32433794438666697
+                },
+                {
+                    "x": 2.4122219538028076,
+                    "y": 6.675695367304005,
+                    "heading": -0.3884103898720453,
+                    "angularVelocity": -1.5238838070197687,
+                    "velocityX": 1.4029696103613034,
+                    "velocityY": -0.7196795200363996,
+                    "timestamp": 0.37147643981305795
+                },
+                {
+                    "x": 2.494664207392629,
+                    "y": 6.634268490634927,
+                    "heading": -0.4434154648490763,
+                    "angularVelocity": -1.1668822791122553,
+                    "velocityX": 1.7489368899896385,
+                    "velocityY": -0.8788332401012526,
+                    "timestamp": 0.4186149352394489
+                },
+                {
+                    "x": 2.593395417678489,
+                    "y": 6.585276348843886,
+                    "heading": -0.4817298536594349,
+                    "angularVelocity": -0.8128046613242179,
+                    "velocityX": 2.094492185055806,
+                    "velocityY": -1.0393234096224127,
+                    "timestamp": 0.4657534306658399
+                },
+                {
+                    "x": 2.7083870607833584,
+                    "y": 6.52862575501488,
+                    "heading": -0.5035659675601283,
+                    "angularVelocity": -0.46323315377732527,
+                    "velocityX": 2.4394423721997165,
+                    "velocityY": -1.2017904542040352,
+                    "timestamp": 0.5128919260922309
+                },
+                {
+                    "x": 2.839583375891639,
+                    "y": 6.464143202111409,
+                    "heading": -0.5093019910748294,
+                    "angularVelocity": -0.12168448446963974,
+                    "velocityX": 2.78320964471958,
+                    "velocityY": -1.367938291626405,
+                    "timestamp": 0.5600304215186218
+                },
+                {
+                    "x": 2.9868119835174247,
+                    "y": 6.391337595074747,
+                    "heading": -0.5000000631264199,
+                    "angularVelocity": 0.197331880546237,
+                    "velocityX": 3.1233200443517304,
+                    "velocityY": -1.5445042608591435,
+                    "timestamp": 0.6071689169450127
+                },
+                {
+                    "x": 3.140901786552832,
+                    "y": 6.301587367083572,
+                    "heading": -0.5000000197789056,
+                    "angularVelocity": 9.19577807856047e-7,
+                    "velocityX": 3.2688740198760886,
+                    "velocityY": -1.903968872561393,
+                    "timestamp": 0.6543074123714037
+                },
+                {
+                    "x": 3.284848213195801,
+                    "y": 6.196334362030029,
+                    "heading": -0.5,
+                    "angularVelocity": 4.1959136469938736e-7,
+                    "velocityX": 3.0536915813881054,
+                    "velocityY": -2.2328460868682716,
+                    "timestamp": 0.7014459077977946
+                },
+                {
+                    "x": 3.3993231490932048,
+                    "y": 6.0978407623199296,
+                    "heading": -0.5170802154644099,
+                    "angularVelocity": -0.41193186552987154,
+                    "velocityX": 2.7608477187370504,
+                    "velocityY": -2.375418059313161,
+                    "timestamp": 0.7429095985042958
+                },
+                {
+                    "x": 3.501098994358481,
+                    "y": 5.994630951270013,
+                    "heading": -0.5506635755138305,
+                    "angularVelocity": -0.8099462319284754,
+                    "velocityX": 2.4545775721162104,
+                    "velocityY": -2.489161222534419,
+                    "timestamp": 0.7843732892107971
+                },
+                {
+                    "x": 3.5900181651549583,
+                    "y": 5.890084132174874,
+                    "heading": -0.6049860857837608,
+                    "angularVelocity": -1.3101224069596595,
+                    "velocityX": 2.144506899445626,
+                    "velocityY": -2.521406495990601,
+                    "timestamp": 0.8258369799172983
+                },
+                {
+                    "x": 3.6727603343537245,
+                    "y": 5.794679716077001,
+                    "heading": -0.692814707704561,
+                    "angularVelocity": -2.1182056016767716,
+                    "velocityX": 1.9955331469267976,
+                    "velocityY": -2.3009147153167167,
+                    "timestamp": 0.8673006706237996
+                },
+                {
+                    "x": 3.757696306555582,
+                    "y": 5.694133573084931,
+                    "heading": -0.7686614093789431,
+                    "angularVelocity": -1.8292318021376788,
+                    "velocityX": 2.04844216119069,
+                    "velocityY": -2.424920244168773,
+                    "timestamp": 0.9087643613303008
+                },
+                {
+                    "x": 3.844342447773648,
+                    "y": 5.582483853612312,
+                    "heading": -0.8164085841781574,
+                    "angularVelocity": -1.1515418426489148,
+                    "velocityX": 2.0896871393165974,
+                    "velocityY": -2.6927105998111585,
+                    "timestamp": 0.950228052036802
+                },
+                {
+                    "x": 3.9316331213435283,
+                    "y": 5.459771926422603,
+                    "heading": -0.8357045871440653,
+                    "angularVelocity": -0.4653710906373953,
+                    "velocityX": 2.105231639596513,
+                    "velocityY": -2.9595032448602216,
+                    "timestamp": 0.9916917427433033
+                },
+                {
+                    "x": 4.018219294626101,
+                    "y": 5.3289825292048665,
+                    "heading": -0.8357080754934182,
+                    "angularVelocity": -0.00008413021836472818,
+                    "velocityX": 2.088240863443359,
+                    "velocityY": -3.1543115190486835,
+                    "timestamp": 1.0331554334498045
+                },
+                {
+                    "x": 4.10449796775504,
+                    "y": 5.197989757794864,
+                    "heading": -0.8357107471897811,
+                    "angularVelocity": -0.00006443460091393523,
+                    "velocityX": 2.0808247326516955,
+                    "velocityY": -3.159216393376985,
+                    "timestamp": 1.0746191241563057
+                },
+                {
+                    "x": 4.190712810420311,
+                    "y": 5.066954954654073,
+                    "heading": -0.8357133876254744,
+                    "angularVelocity": -0.00006368067213797694,
+                    "velocityX": 2.0792853023028366,
+                    "velocityY": -3.160230093078832,
+                    "timestamp": 1.1160828148628068
                 },
                 {
                     "x": 4.274456024169922,
                     "y": 4.934326648712158,
                     "heading": -0.8357152799414105,
-                    "angularVelocity": -0.000007047351849068254,
-                    "velocityX": 2.19057034929318,
-                    "velocityY": -3.0841570043363045,
-                    "timestamp": 0.9481064922480852
-                },
-                {
-                    "x": 4.3556906912761315,
-                    "y": 4.793345794539918,
-                    "heading": -0.8357152378475459,
-                    "angularVelocity": 9.786625189030426e-7,
-                    "velocityX": 1.8886677315079998,
-                    "velocityY": -3.2777384277013035,
-                    "timestamp": 0.9911181166598327
-                },
-                {
-                    "x": 4.442446525481621,
-                    "y": 4.655693706781043,
-                    "heading": -0.8357151935315337,
-                    "angularVelocity": 0.0000010303263992905167,
-                    "velocityX": 2.0170322649286843,
-                    "velocityY": -3.200346177143663,
-                    "timestamp": 1.0341297410715804
-                },
-                {
-                    "x": 4.541847641111602,
-                    "y": 4.5268757636785075,
-                    "heading": -0.8357148699573956,
-                    "angularVelocity": 0.000007522946236810339,
-                    "velocityX": 2.311029099445763,
-                    "velocityY": -2.9949564766345187,
-                    "timestamp": 1.077141365483328
-                },
-                {
-                    "x": 4.65067034095477,
-                    "y": 4.409944405402937,
-                    "heading": -0.8255512517653536,
-                    "angularVelocity": 0.23629933375095027,
-                    "velocityX": 2.530076492843306,
-                    "velocityY": -2.7185989805033612,
-                    "timestamp": 1.1201529898950755
-                },
-                {
-                    "x": 4.7668457664948924,
-                    "y": 4.306168868140065,
-                    "heading": -0.803496324956771,
-                    "angularVelocity": 0.5127666557638517,
-                    "velocityX": 2.701023900608423,
-                    "velocityY": -2.4127323411326107,
-                    "timestamp": 1.163164614306823
-                },
-                {
-                    "x": 4.88912359332006,
-                    "y": 4.216234980411927,
-                    "heading": -0.7705722789003837,
-                    "angularVelocity": 0.7654685566210554,
-                    "velocityX": 2.8429018549638743,
-                    "velocityY": -2.0909205117948377,
-                    "timestamp": 1.2061762387185706
-                },
-                {
-                    "x": 5.016560626655114,
-                    "y": 4.140615450479534,
-                    "heading": -0.7285529163528696,
-                    "angularVelocity": 0.9769303792218019,
-                    "velocityX": 2.9628509752411754,
-                    "velocityY": -1.7581184381341082,
-                    "timestamp": 1.2491878631303182
-                },
-                {
-                    "x": 5.148349456504816,
-                    "y": 4.079665503809577,
-                    "heading": -0.6795461936491729,
-                    "angularVelocity": 1.1393832103284065,
-                    "velocityX": 3.0640281935900804,
-                    "velocityY": -1.4170575397591803,
-                    "timestamp": 1.2921994875420657
-                },
-                {
-                    "x": 5.28369745675312,
-                    "y": 4.033673765712218,
-                    "heading": -0.6261187752553977,
-                    "angularVelocity": 1.2421623020399764,
-                    "velocityX": 3.1467772282354725,
-                    "velocityY": -1.0692862389265216,
-                    "timestamp": 1.3352111119538133
-                },
-                {
-                    "x": 5.421794981888129,
-                    "y": 4.0028549700404525,
-                    "heading": -0.5712230670658915,
-                    "angularVelocity": 1.2762993479156466,
-                    "velocityX": 3.2107023862435504,
-                    "velocityY": -0.7165224771968601,
-                    "timestamp": 1.3782227363655608
-                },
-                {
-                    "x": 5.5620425748748135,
-                    "y": 3.9873099729843906,
-                    "heading": -0.5170015662419462,
-                    "angularVelocity": 1.2606243443606422,
-                    "velocityX": 3.260690450658235,
-                    "velocityY": -0.3614138565716755,
-                    "timestamp": 1.4212343607773084
+                    "angularVelocity": -0.000045637904003859675,
+                    "velocityX": 2.0196758253473424,
+                    "velocityY": -3.198661375339825,
+                    "timestamp": 1.157546505569308
+                },
+                {
+                    "x": 4.34828200363932,
+                    "y": 4.789661343281582,
+                    "heading": -0.8357152360129204,
+                    "angularVelocity": 0.0000010231801667591938,
+                    "velocityX": 1.7195509731200054,
+                    "velocityY": -3.36953696405698,
+                    "timestamp": 1.2004797949125405
+                },
+                {
+                    "x": 4.435451827820083,
+                    "y": 4.652622129190998,
+                    "heading": -0.8357151829793803,
+                    "angularVelocity": 0.0000012352545290785843,
+                    "velocityX": 2.0303551280191665,
+                    "velocityY": -3.1919104309715474,
+                    "timestamp": 1.2434130842557731
+                },
+                {
+                    "x": 4.535184717372419,
+                    "y": 4.524436546290902,
+                    "heading": -0.8357137971440172,
+                    "angularVelocity": 0.000032278807056995315,
+                    "velocityX": 2.322973410097133,
+                    "velocityY": -2.985692101886486,
+                    "timestamp": 1.2863463735990057
+                },
+                {
+                    "x": 4.644325012525673,
+                    "y": 4.4080959710874374,
+                    "heading": -0.8258667739389303,
+                    "angularVelocity": 0.22935636555504585,
+                    "velocityX": 2.542090224690803,
+                    "velocityY": -2.7097987827900663,
+                    "timestamp": 1.3292796629422383
+                },
+                {
+                    "x": 4.760978449232698,
+                    "y": 4.3048079626404245,
+                    "heading": -0.804942882637706,
+                    "angularVelocity": 0.4873582159975465,
+                    "velocityX": 2.717085937078615,
+                    "velocityY": -2.405779059257989,
+                    "timestamp": 1.372212952285471
+                },
+                {
+                    "x": 4.883873293679825,
+                    "y": 4.215276634825419,
+                    "heading": -0.7738338273486849,
+                    "angularVelocity": 0.7245905395302493,
+                    "velocityX": 2.8624604899158337,
+                    "velocityY": -2.085359151013129,
+                    "timestamp": 1.4151462416287035
+                },
+                {
+                    "x": 5.012023716462111,
+                    "y": 4.13998910043764,
+                    "heading": -0.7341562289641704,
+                    "angularVelocity": 0.9241686111518187,
+                    "velocityX": 2.9848731542039757,
+                    "velocityY": -1.7535934362235386,
+                    "timestamp": 1.458079530971936
+                },
+                {
+                    "x": 5.144586044931236,
+                    "y": 4.079308119917677,
+                    "heading": -0.6878803412234658,
+                    "angularVelocity": 1.0778556325081365,
+                    "velocityX": 3.0876350379153963,
+                    "velocityY": -1.4133783236324275,
+                    "timestamp": 1.5010128203151687
+                },
+                {
+                    "x": 5.2807921990430025,
+                    "y": 4.033507276074697,
+                    "heading": -0.6371968549741135,
+                    "angularVelocity": 1.1805171936433834,
+                    "velocityX": 3.1725068401551706,
+                    "velocityY": -1.0667909341122943,
+                    "timestamp": 1.5439461096584013
+                },
+                {
+                    "x": 5.419841766333411,
+                    "y": 4.00280308410274,
+                    "heading": -0.5848216118259447,
+                    "angularVelocity": 1.2199215096111542,
+                    "velocityX": 3.2387354758394924,
+                    "velocityY": -0.7151604836631573,
+                    "timestamp": 1.5868793990016339
+                },
+                {
+                    "x": 5.561074826834354,
+                    "y": 3.9873115142730846,
+                    "heading": -0.5330346380339317,
+                    "angularVelocity": 1.2062195695745372,
+                    "velocityX": 3.289593289064067,
+                    "velocityY": -0.36082885953151356,
+                    "timestamp": 1.6298126883448665
                 },
                 {
                     "x": 5.703982353210449,
                     "y": 3.987083911895752,
-                    "heading": -0.46507695218435896,
-                    "angularVelocity": 1.207222809362328,
-                    "velocityX": 3.3000329626441363,
-                    "velocityY": -0.005255813788261737,
-                    "timestamp": 1.464245985189056
-                },
-                {
-                    "x": 5.957415351644791,
-                    "y": 3.94389686592464,
-                    "heading": -0.40627705964587574,
-                    "angularVelocity": 0.8004851872173009,
-                    "velocityX": 3.450165509502976,
-                    "velocityY": -0.5879362884366123,
-                    "timestamp": 1.5377013013865826
-                },
-                {
-                    "x": 6.214659603999478,
-                    "y": 3.857155103461619,
-                    "heading": -0.3855287596647175,
-                    "angularVelocity": 0.2824615161326723,
-                    "velocityX": 3.502050847660077,
-                    "velocityY": -1.180877939858924,
-                    "timestamp": 1.6111566175841092
-                },
-                {
-                    "x": 6.460339332530576,
-                    "y": 3.727320070546642,
-                    "heading": -0.38552868222223635,
-                    "angularVelocity": 0.0000010542801416145007,
-                    "velocityX": 3.3446146752734305,
-                    "velocityY": -1.7675375947717866,
-                    "timestamp": 1.6846119337816359
-                },
-                {
-                    "x": 6.681847040612612,
-                    "y": 3.5595404285567858,
-                    "heading": -0.3855286464284339,
-                    "angularVelocity": 4.872867523834452e-7,
-                    "velocityX": 3.0155435923301614,
-                    "velocityY": -2.2841048228379357,
-                    "timestamp": 1.7580672499791625
-                },
-                {
-                    "x": 6.873376982747171,
-                    "y": 3.358214932798401,
-                    "heading": -0.3855286139444744,
-                    "angularVelocity": 4.422274806376815e-7,
-                    "velocityX": 2.607434724254966,
-                    "velocityY": -2.740788634235891,
-                    "timestamp": 1.8315225661766892
-                },
-                {
-                    "x": 7.058369006072979,
-                    "y": 3.1508658904491242,
-                    "heading": -0.3855285817822777,
-                    "angularVelocity": 4.3784709334515964e-7,
-                    "velocityX": 2.518429337753462,
-                    "velocityY": -2.8227915021385286,
-                    "timestamp": 1.9049778823742158
-                },
-                {
-                    "x": 7.235640342207717,
-                    "y": 2.9543486280986375,
-                    "heading": -0.3517392740895451,
-                    "angularVelocity": 0.4599981245995971,
-                    "velocityX": 2.413322075396734,
-                    "velocityY": -2.6753306979448217,
-                    "timestamp": 1.9784331985717425
-                },
-                {
-                    "x": 7.3842205894533075,
-                    "y": 2.7881464841241645,
-                    "heading": -0.2579536451103976,
-                    "angularVelocity": 1.2767711560447332,
-                    "velocityX": 2.0227296666458727,
-                    "velocityY": -2.2626292088586646,
-                    "timestamp": 2.051888514769269
-                },
-                {
-                    "x": 7.502781206912582,
-                    "y": 2.6554541566341396,
-                    "heading": -0.1644602811973769,
-                    "angularVelocity": 1.2727923416953275,
-                    "velocityX": 1.6140508760517314,
-                    "velocityY": -1.806435998903144,
-                    "timestamp": 2.125343830966796
-                },
-                {
-                    "x": 7.591494122264987,
-                    "y": 2.5561305844890163,
-                    "heading": -0.0867965293639426,
-                    "angularVelocity": 1.0572924582421084,
-                    "velocityX": 1.2077126604948574,
-                    "velocityY": -1.3521631555983635,
-                    "timestamp": 2.1987991471643227
-                },
-                {
-                    "x": 7.6505265885867635,
-                    "y": 2.4900164938031613,
-                    "heading": -0.031723308715129774,
-                    "angularVelocity": 0.7497513250193758,
-                    "velocityX": 0.8036513812429008,
-                    "velocityY": -0.900058622143418,
-                    "timestamp": 2.2722544633618496
+                    "heading": -0.4835776002687649,
+                    "angularVelocity": 1.1519508176925284,
+                    "velocityX": 3.328594863384743,
+                    "velocityY": -0.005301303040469438,
+                    "timestamp": 1.672745977688099
+                },
+                {
+                    "x": 5.958820715702131,
+                    "y": 3.94375176713671,
+                    "heading": -0.42940135113015304,
+                    "angularVelocity": 0.7375353070821884,
+                    "velocityX": 3.4692746900164138,
+                    "velocityY": -0.5899077030899651,
+                    "timestamp": 1.7462017819301305
+                },
+                {
+                    "x": 6.2169360873076736,
+                    "y": 3.85673688488041,
+                    "heading": -0.41197233440562925,
+                    "angularVelocity": 0.2372721516614769,
+                    "velocityX": 3.513886673339918,
+                    "velocityY": -1.184588245329031,
+                    "timestamp": 1.819657586172162
+                },
+                {
+                    "x": 6.462551803453154,
+                    "y": 3.726776847306083,
+                    "heading": -0.411972258677301,
+                    "angularVelocity": 0.0000010309372967585872,
+                    "velocityX": 3.343721012654018,
+                    "velocityY": -1.76922761809427,
+                    "timestamp": 1.8931133904141935
+                },
+                {
+                    "x": 6.683975982637168,
+                    "y": 3.558883929748973,
+                    "heading": -0.4119722206140506,
+                    "angularVelocity": 5.181789344885153e-7,
+                    "velocityX": 3.0143864255360504,
+                    "velocityY": -2.285631738561,
+                    "timestamp": 1.966569194656225
+                },
+                {
+                    "x": 6.875523396649561,
+                    "y": 3.357572509103632,
+                    "heading": -0.4119721861737391,
+                    "angularVelocity": 4.688575916806749e-7,
+                    "velocityX": 2.6076552559586523,
+                    "velocityY": -2.7405788109272424,
+                    "timestamp": 2.0400249988982564
+                },
+                {
+                    "x": 7.060174216266159,
+                    "y": 3.149917085871916,
+                    "heading": -0.4119721521055279,
+                    "angularVelocity": 4.637919568273184e-7,
+                    "velocityX": 2.513767584766965,
+                    "velocityY": -2.826943702739948,
+                    "timestamp": 2.1134808031402876
+                },
+                {
+                    "x": 7.236976316543012,
+                    "y": 2.954068221435557,
+                    "heading": -0.3758421094017394,
+                    "angularVelocity": 0.4918609642437705,
+                    "velocityX": 2.40691803869303,
+                    "velocityY": -2.666213602277847,
+                    "timestamp": 2.186936607382319
+                },
+                {
+                    "x": 7.385172224991548,
+                    "y": 2.787906831681346,
+                    "heading": -0.27501394829018777,
+                    "angularVelocity": 1.3726370863672974,
+                    "velocityX": 2.0174839820722426,
+                    "velocityY": -2.262059363024933,
+                    "timestamp": 2.26039241162435
+                },
+                {
+                    "x": 7.503381996539121,
+                    "y": 2.655285343615516,
+                    "heading": -0.17500745818288596,
+                    "angularVelocity": 1.3614511629031412,
+                    "velocityX": 1.6092638664478731,
+                    "velocityY": -1.8054596152654476,
+                    "timestamp": 2.3338482158663814
+                },
+                {
+                    "x": 7.59180643545552,
+                    "y": 2.55603678143989,
+                    "heading": -0.09217156397731928,
+                    "angularVelocity": 1.1276970562139184,
+                    "velocityX": 1.2037774254713123,
+                    "velocityY": -1.3511330139223812,
+                    "timestamp": 2.4073040201084126
+                },
+                {
+                    "x": 7.650633785300877,
+                    "y": 2.489982671853645,
+                    "heading": -0.033538816068743524,
+                    "angularVelocity": 0.7982044239197745,
+                    "velocityX": 0.8008536623127058,
+                    "velocityY": -0.8992360817206846,
+                    "timestamp": 2.480759824350444
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
                     "heading": -0.003,
-                    "angularVelocity": 0.39103104039319253,
-                    "velocityX": 0.401242727401519,
-                    "velocityY": -0.4494772538229548,
-                    "timestamp": 2.3457097795593764
+                    "angularVelocity": 0.41574408426759185,
+                    "velocityX": 0.3997807253237885,
+                    "velocityY": -0.44901382802875656,
+                    "timestamp": 2.554215628592475
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
                     "heading": -0.003,
-                    "angularVelocity": -1.5819357203294863e-32,
-                    "velocityX": 5.378736393413154e-34,
-                    "velocityY": 0,
-                    "timestamp": 2.4191650957569033
+                    "angularVelocity": 5.454413556338505e-31,
+                    "velocityX": 0,
+                    "velocityY": -3.390988673475489e-31,
+                    "timestamp": 2.6276714328345063
                 }
             ],
             "constraints": [
@@ -12695,7 +14423,7 @@
                 },
                 {
                     "scope": [
-                        2
+                        4
                     ],
                     "type": "WptVelocityDirection",
                     "direction": 3.14
@@ -13660,7 +15388,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 15
+                    "controlIntervalCount": 7
+                },
+                {
+                    "x": 2.3068509101867676,
+                    "y": 6.379708766937256,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 14
                 },
                 {
                     "x": 5.176,
@@ -13676,146 +15413,200 @@
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 4.4337460853781175e-35,
-                    "velocityX": 0,
-                    "velocityY": 0,
+                    "heading": -9.427656700789375e-18,
+                    "angularVelocity": -1.3516738833302546e-18,
+                    "velocityX": 8.730641062152612e-18,
+                    "velocityY": -7.295689550902649e-17,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.3070751783284065,
-                    "y": 5.580871519986164,
-                    "heading": 0.00875997642576413,
-                    "angularVelocity": 0.10256751513529143,
-                    "velocityX": 0.6682733985254588,
-                    "velocityY": 0.24437736336610263,
-                    "timestamp": 0.08540692844327274
-                },
-                {
-                    "x": 2.421225525991661,
-                    "y": 5.622614560146346,
-                    "heading": 0.026280802198455293,
-                    "angularVelocity": 0.20514525100066663,
-                    "velocityX": 1.3365466917484683,
-                    "velocityY": 0.48875472893171557,
-                    "timestamp": 0.1708138568865455
-                },
-                {
-                    "x": 2.592451026856299,
-                    "y": 5.685229121649564,
-                    "heading": 0.052563939294491345,
-                    "angularVelocity": 0.30774010463909035,
-                    "velocityX": 2.0048197960703633,
-                    "velocityY": 0.7331321081849562,
-                    "timestamp": 0.25622078532981823
-                },
-                {
-                    "x": 2.8207516562305845,
-                    "y": 5.768715208168554,
-                    "heading": 0.0876113178799828,
-                    "angularVelocity": 0.41035755792072426,
-                    "velocityX": 2.673092611285307,
-                    "velocityY": 0.9775095304409794,
-                    "timestamp": 0.341627713773091
-                },
-                {
-                    "x": 3.106127374237044,
-                    "y": 5.873072825511933,
-                    "heading": 0.13142500230326418,
-                    "angularVelocity": 0.5129991819385286,
-                    "velocityX": 3.341364959588794,
-                    "velocityY": 1.221887020708095,
-                    "timestamp": 0.4270346422163637
-                },
-                {
-                    "x": 3.40956412390744,
-                    "y": 5.984036121763709,
-                    "heading": 0.13142500754936084,
-                    "angularVelocity": 6.142471997127927e-8,
-                    "velocityX": 3.5528352933560723,
-                    "velocityY": 1.299230615997127,
-                    "timestamp": 0.5124415706596365
-                },
-                {
-                    "x": 3.7130008735619784,
-                    "y": 6.094999418058892,
-                    "heading": 0.1314250127954367,
-                    "angularVelocity": 6.142447634082945e-8,
-                    "velocityX": 3.5528352931703995,
-                    "velocityY": 1.2992306165053664,
-                    "timestamp": 0.5978484991029092
-                },
-                {
-                    "x": 4.016437623216516,
-                    "y": 6.205962714354075,
-                    "heading": 0.13142501804151255,
-                    "angularVelocity": 6.142447606684555e-8,
-                    "velocityX": 3.552835293170399,
-                    "velocityY": 1.2992306165053664,
-                    "timestamp": 0.683255427546182
-                },
-                {
-                    "x": 4.3198743728836435,
-                    "y": 6.316926010614789,
-                    "heading": 0.13142502328760913,
-                    "angularVelocity": 6.142471942956278e-8,
-                    "velocityX": 3.5528352933178016,
-                    "velocityY": 1.2992306161017806,
-                    "timestamp": 0.7686623559894548
-                },
-                {
-                    "x": 4.605249584503867,
-                    "y": 6.421283973215489,
-                    "heading": 0.17528171773503587,
-                    "angularVelocity": 0.5135027713419801,
-                    "velocityX": 3.3413590304886087,
-                    "velocityY": 1.2218910632059061,
-                    "timestamp": 0.8540692844327276
-                },
-                {
-                    "x": 4.83354975240058,
-                    "y": 6.5047703704067485,
-                    "heading": 0.21036850612417926,
-                    "angularVelocity": 0.4108189935954435,
-                    "velocityX": 2.6730872080050085,
-                    "velocityY": 0.9775131679944534,
-                    "timestamp": 0.9394762128760004
-                },
-                {
-                    "x": 5.004774875721697,
-                    "y": 6.567385181417454,
-                    "heading": 0.23668415165777779,
-                    "angularVelocity": 0.3081207346202291,
-                    "velocityX": 2.0048153755446823,
-                    "velocityY": 0.7331350295812797,
-                    "timestamp": 1.0248831413192732
-                },
-                {
-                    "x": 5.118924957871047,
-                    "y": 6.6091283935214395,
-                    "heading": 0.25422806054578806,
-                    "angularVelocity": 0.20541552316405948,
-                    "velocityX": 1.3365435829385781,
-                    "velocityY": 0.48875674216187737,
-                    "timestamp": 1.110290069762546
-                },
-                {
-                    "x": 5.176,
+                    "x": 2.220337966696873,
+                    "y": 5.62696613740055,
+                    "heading": -0.005158227148275901,
+                    "angularVelocity": -0.05503188081512997,
+                    "velocityX": -0.31645707615215996,
+                    "velocityY": 0.7144455616516493,
+                    "timestamp": 0.09373161650796653
+                },
+                {
+                    "x": 2.1631184668461043,
+                    "y": 5.761793353879717,
+                    "heading": -0.015168353452777405,
+                    "angularVelocity": -0.10679562219702789,
+                    "velocityX": -0.6104610374014564,
+                    "velocityY": 1.4384390401258744,
+                    "timestamp": 0.18746323301593307
+                },
+                {
+                    "x": 2.0853065565036237,
+                    "y": 5.966914141221312,
+                    "heading": -0.029010799448120003,
+                    "angularVelocity": -0.14768171627730414,
+                    "velocityX": -0.8301564961900216,
+                    "velocityY": 2.1883841865051155,
+                    "timestamp": 0.2811948495238996
+                },
+                {
+                    "x": 2.0799249337959744,
+                    "y": 6.161512978496455,
+                    "heading": -0.03183861533006777,
+                    "angularVelocity": -0.0301692853201504,
+                    "velocityX": -0.05741523413491959,
+                    "velocityY": 2.076128040089899,
+                    "timestamp": 0.37492646603186613
+                },
+                {
+                    "x": 2.1174615185646393,
+                    "y": 6.2967780571782885,
+                    "heading": -0.027602730416951004,
+                    "angularVelocity": 0.045191634060389424,
+                    "velocityX": 0.4004687656856407,
+                    "velocityY": 1.443110486314265,
+                    "timestamp": 0.46865808253983265
+                },
+                {
+                    "x": 2.193636869673838,
+                    "y": 6.369832500172102,
+                    "heading": -0.016913451231696384,
+                    "angularVelocity": 0.11404134040882687,
+                    "velocityX": 0.8126964406159006,
+                    "velocityY": 0.7794002249774918,
+                    "timestamp": 0.5623896990477992
+                },
+                {
+                    "x": 2.306850910186767,
+                    "y": 6.379708766937256,
+                    "heading": 7.831941019246608e-17,
+                    "angularVelocity": 0.1804455301403977,
+                    "velocityX": 1.20785328079034,
+                    "velocityY": 0.10536750707073723,
+                    "timestamp": 0.6561213155557658
+                },
+                {
+                    "x": 2.454400050444712,
+                    "y": 6.392580268241816,
+                    "heading": 0.024770297736306776,
+                    "angularVelocity": 0.3129674474840377,
+                    "velocityX": 1.864252028642711,
+                    "velocityY": 0.1626286834118752,
+                    "timestamp": 0.7352678783546651
+                },
+                {
+                    "x": 2.653900880802349,
+                    "y": 6.4099837935603645,
+                    "heading": 0.06003005706528144,
+                    "angularVelocity": 0.445499565389401,
+                    "velocityX": 2.520650591795653,
+                    "velocityY": 0.2198898436406934,
+                    "timestamp": 0.8144144411535644
+                },
+                {
+                    "x": 2.905353386807521,
+                    "y": 6.431919341632162,
+                    "heading": 0.10577908294347244,
+                    "angularVelocity": 0.5780292189622053,
+                    "velocityX": 3.177048972348669,
+                    "velocityY": 0.2771509879403426,
+                    "timestamp": 0.8935610039524637
+                },
+                {
+                    "x": 3.203627341926949,
+                    "y": 6.457939375335675,
+                    "heading": 0.10577908974967822,
+                    "angularVelocity": 8.599496273628574e-8,
+                    "velocityX": 3.768628030977189,
+                    "velocityY": 0.3287575958241692,
+                    "timestamp": 0.972707566751363
+                },
+                {
+                    "x": 3.501901297052475,
+                    "y": 6.48395940903972,
+                    "heading": 0.10577909654992135,
+                    "angularVelocity": 8.591962579346019e-8,
+                    "velocityX": 3.7686280310542424,
+                    "velocityY": 0.328757595830891,
+                    "timestamp": 1.0518541295502624
+                },
+                {
+                    "x": 3.8001752521780374,
+                    "y": 6.509979442743767,
+                    "heading": 0.10577910335027264,
+                    "angularVelocity": 8.592099253060419e-8,
+                    "velocityX": 3.768628031054697,
+                    "velocityY": 0.32875759583093067,
+                    "timestamp": 1.1310006923491618
+                },
+                {
+                    "x": 4.0984492073035454,
+                    "y": 6.53599947644781,
+                    "heading": 0.1057791101506597,
+                    "angularVelocity": 8.592144417290625e-8,
+                    "velocityX": 3.7686280310540092,
+                    "velocityY": 0.32875759583087066,
+                    "timestamp": 1.2101472551480612
+                },
+                {
+                    "x": 4.396723162417087,
+                    "y": 6.562019510150809,
+                    "heading": 0.10577911696012733,
+                    "angularVelocity": 8.603617553663074e-8,
+                    "velocityX": 3.7686280309028084,
+                    "velocityY": 0.3287575958176806,
+                    "timestamp": 1.2892938179469606
+                },
+                {
+                    "x": 4.656482038612004,
+                    "y": 6.5846796673565295,
+                    "heading": 0.15819030641666595,
+                    "angularVelocity": 0.6622042398696235,
+                    "velocityX": 3.2819981943490006,
+                    "velocityY": 0.28630627034678896,
+                    "timestamp": 1.36844038074586
+                },
+                {
+                    "x": 4.864289197548449,
+                    "y": 6.6028077981790565,
+                    "heading": 0.2001158851669991,
+                    "angularVelocity": 0.5297207770962873,
+                    "velocityX": 2.6255992880506356,
+                    "velocityY": 0.22904508018355874,
+                    "timestamp": 1.4475869435447595
+                },
+                {
+                    "x": 5.020144593431014,
+                    "y": 6.616403898623411,
+                    "heading": 0.2315582663704034,
+                    "angularVelocity": 0.39726780407754475,
+                    "velocityX": 1.9691998031370395,
+                    "velocityY": 0.17178383954462473,
+                    "timestamp": 1.526733506343659
+                },
+                {
+                    "x": 5.124048198578653,
+                    "y": 6.625467966274828,
+                    "heading": 0.2525193347871735,
+                    "angularVelocity": 0.2648386446045577,
+                    "velocityX": 1.3127999684793037,
+                    "velocityY": 0.1145225683956373,
+                    "timestamp": 1.6058800691425583
+                },
+                {
+                    "x": 5.176000000000001,
                     "y": 6.63,
-                    "heading": 0.263,
-                    "angularVelocity": 0.10270758607175808,
-                    "velocityX": 0.6682718038134577,
-                    "velocityY": 0.2443783760754632,
-                    "timestamp": 1.1956969982058188
+                    "heading": 0.26299999999999985,
+                    "angularVelocity": 0.13242097751555232,
+                    "velocityX": 0.6563999696784293,
+                    "velocityY": 0.05726128292756526,
+                    "timestamp": 1.6850266319414577
                 },
                 {
-                    "x": 5.176,
+                    "x": 5.176000000000001,
                     "y": 6.63,
-                    "heading": 0.263,
-                    "angularVelocity": 1.86588452749739e-32,
-                    "velocityX": -1.0770205854918045e-34,
-                    "velocityY": 0,
-                    "timestamp": 1.2811039266490916
+                    "heading": 0.26299999999999996,
+                    "angularVelocity": 1.6428599245730974e-17,
+                    "velocityX": 1.9833104251084488e-16,
+                    "velocityY": 1.8748049703629392e-17,
+                    "timestamp": 1.7641731947403572
                 }
             ],
             "constraints": [
@@ -13830,6 +15621,13 @@
                         "last"
                     ],
                     "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        1,
+                        2
+                    ],
+                    "type": "StraightLine"
                 }
             ],
             "usesControlIntervalGuessing": true,
@@ -15182,207 +16980,324 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 20
+                    "controlIntervalCount": 7
                 },
                 {
-                    "x": 7.68,
-                    "y": 7.467,
+                    "x": 2.38835072517395,
+                    "y": 6.318583965301514,
                     "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": -3.731399929679607e-32,
-                    "velocityY": 5.877471754111438e-38,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.3271275380851444,
-                    "y": 5.587086964112038,
-                    "heading": 3.953847862678034e-18,
-                    "angularVelocity": 3.9932078666702264e-17,
-                    "velocityX": 0.7789533333445218,
-                    "velocityY": 0.2735661154121556,
-                    "timestamp": 0.09901432445765257
-                },
-                {
-                    "x": 2.4813826113143547,
-                    "y": 5.641260891303218,
-                    "heading": 1.1398162294527873e-19,
-                    "angularVelocity": -3.8780916638560175e-17,
-                    "velocityX": 1.557906636985473,
-                    "velocityY": 0.5471322203925046,
-                    "timestamp": 0.19802864891530514
-                },
-                {
-                    "x": 2.7127652138054734,
-                    "y": 5.722521779507741,
-                    "heading": 2.1553394674505063e-18,
-                    "angularVelocity": 2.0616792512846884e-17,
-                    "velocityX": 2.336859881219295,
-                    "velocityY": 0.8206983045092451,
-                    "timestamp": 0.2970429733729577
-                },
-                {
-                    "x": 3.0212753279120443,
-                    "y": 5.830869622528225,
-                    "heading": -1.0672476097054788e-17,
-                    "angularVelocity": -1.295551486240031e-16,
-                    "velocityX": 3.1158129472318716,
-                    "velocityY": 1.0942643260352094,
-                    "timestamp": 0.3960572978306103
-                },
-                {
-                    "x": 3.3746798137461993,
-                    "y": 5.9549842366139965,
-                    "heading": 1.8895880094915182e-18,
-                    "angularVelocity": 1.2687117924961395e-16,
-                    "velocityX": 3.5692258445423475,
-                    "velocityY": 1.2535015995473786,
-                    "timestamp": 0.49507162228826285
-                },
-                {
-                    "x": 3.7280842995803773,
-                    "y": 6.079098850699775,
-                    "heading": 1.0049319654091473e-17,
-                    "angularVelocity": 8.240960778697987e-17,
-                    "velocityX": 3.569225844542579,
-                    "velocityY": 1.25350159954746,
-                    "timestamp": 0.5940859467459154
-                },
-                {
-                    "x": 4.081488785414555,
-                    "y": 6.203213464785553,
-                    "heading": 8.136834826577652e-18,
-                    "angularVelocity": -1.931523381075779e-17,
-                    "velocityX": 3.569225844542579,
-                    "velocityY": 1.25350159954746,
-                    "timestamp": 0.693100271203568
-                },
-                {
-                    "x": 4.434893271248733,
-                    "y": 6.327328078871332,
-                    "heading": -2.4381204969164037e-18,
-                    "angularVelocity": -1.0680227715899811e-16,
-                    "velocityX": 3.5692258445425793,
-                    "velocityY": 1.2535015995474599,
-                    "timestamp": 0.7921145956612206
-                },
-                {
-                    "x": 4.788297757082911,
-                    "y": 6.4514426929571105,
-                    "heading": 3.192038150785043e-17,
-                    "angularVelocity": 3.47005367081625e-16,
-                    "velocityX": 3.569225844542579,
-                    "velocityY": 1.25350159954746,
-                    "timestamp": 0.8911289201188731
-                },
-                {
-                    "x": 5.141702242917089,
-                    "y": 6.575557307042889,
-                    "heading": 6.016026907559514e-17,
-                    "angularVelocity": 2.8521012209839933e-16,
-                    "velocityX": 3.569225844542579,
-                    "velocityY": 1.2535015995474599,
-                    "timestamp": 0.9901432445765257
-                },
-                {
-                    "x": 5.495106728751266,
-                    "y": 6.699671921128667,
-                    "heading": 2.4186102644429564e-17,
-                    "angularVelocity": -3.63322848771759e-16,
-                    "velocityX": 3.569225844542579,
-                    "velocityY": 1.2535015995474599,
-                    "timestamp": 1.0891575690341782
-                },
-                {
-                    "x": 5.848511214585444,
-                    "y": 6.823786535214446,
-                    "heading": -1.2421929874703125e-18,
-                    "angularVelocity": -2.568143121812034e-16,
-                    "velocityX": 3.5692258445425793,
-                    "velocityY": 1.2535015995474599,
-                    "timestamp": 1.1881718934918308
-                },
-                {
-                    "x": 6.2019157004196215,
-                    "y": 6.9479011493002245,
-                    "heading": 2.3184212364315377e-18,
-                    "angularVelocity": 3.596059704901295e-17,
-                    "velocityX": 3.569225844542579,
-                    "velocityY": 1.2535015995474599,
-                    "timestamp": 1.2871862179494835
+                    "headingConstrained": false,
+                    "controlIntervalCount": 8
                 },
                 {
-                    "x": 6.5553201862538,
-                    "y": 7.072015763386003,
-                    "heading": 3.1459219033425486e-17,
-                    "angularVelocity": 2.9430890890068574e-16,
-                    "velocityX": 3.569225844542579,
-                    "velocityY": 1.25350159954746,
-                    "timestamp": 1.3862005424071362
+                    "x": 3.5700972080230713,
+                    "y": 6.2574591636657715,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 16
                 },
                 {
-                    "x": 6.9087246720879545,
-                    "y": 7.196130377471773,
-                    "heading": 2.9741502939674576e-17,
-                    "angularVelocity": -1.7348157482865667e-17,
-                    "velocityX": 3.5692258445423475,
-                    "velocityY": 1.2535015995473786,
-                    "timestamp": 1.4852148668647889
-                },
+                    "x": 7.68,
+                    "y": 7.467,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": true,
+                    "controlIntervalCount": 40
+                }
+            ],
+            "trajectory": [
                 {
-                    "x": 7.217234786194527,
-                    "y": 7.304478220492258,
-                    "heading": 1.2383304928032073e-17,
-                    "angularVelocity": -1.753099667759744e-16,
-                    "velocityX": 3.115812947231872,
-                    "velocityY": 1.0942643260352094,
-                    "timestamp": 1.5842291913224416
+                    "x": 2.25,
+                    "y": 5.56,
+                    "heading": -5.349509897792121e-32,
+                    "angularVelocity": -4.120122643775229e-32,
+                    "velocityX": 8.47273432885147e-31,
+                    "velocityY": -1.4633096492322666e-31,
+                    "timestamp": 0
                 },
                 {
-                    "x": 7.448617388685645,
-                    "y": 7.385739108696781,
-                    "heading": -3.803725574168796e-18,
-                    "angularVelocity": -1.6348170333424006e-16,
-                    "velocityX": 2.336859881219295,
-                    "velocityY": 0.8206983045092451,
-                    "timestamp": 1.6832435157800942
-                },
+                    "x": 2.237003637990514,
+                    "y": 5.5993039448743,
+                    "heading": -2.4015234029354595e-10,
+                    "angularVelocity": -3.4082853646292762e-9,
+                    "velocityX": -0.1844472530475389,
+                    "velocityY": 0.5578103057326728,
+                    "timestamp": 0.070461130729763
+                },
+                {
+                    "x": 2.214179391204466,
+                    "y": 5.678821295080181,
+                    "heading": -7.831932260526302e-10,
+                    "angularVelocity": -7.70693396452574e-9,
+                    "velocityX": -0.32392677423960453,
+                    "velocityY": 1.1285278760503061,
+                    "timestamp": 0.140922261459526
+                },
+                {
+                    "x": 2.1888413071411663,
+                    "y": 5.799659166026344,
+                    "heading": -1.7630537350139963e-9,
+                    "angularVelocity": -1.3906355788165327e-8,
+                    "velocityX": -0.3596037106073644,
+                    "velocityY": 1.714957873876257,
+                    "timestamp": 0.21138339218928898
+                },
+                {
+                    "x": 2.184731423610446,
+                    "y": 5.956036692159833,
+                    "heading": -3.5093546140825654e-9,
+                    "angularVelocity": -2.4783811808465378e-8,
+                    "velocityX": -0.0583283788886903,
+                    "velocityY": 2.219344545207332,
+                    "timestamp": 0.281844522919052
+                },
+                {
+                    "x": 2.2210562385399535,
+                    "y": 6.10354071192916,
+                    "heading": -5.82104685853391e-9,
+                    "angularVelocity": -3.2807944511631915e-8,
+                    "velocityX": 0.5155298325992919,
+                    "velocityY": 2.0934097741478808,
+                    "timestamp": 0.352305653648815
+                },
+                {
+                    "x": 2.290250351402012,
+                    "y": 6.2258789967414945,
+                    "heading": -8.381712631190757e-9,
+                    "angularVelocity": -3.6341419943068046e-8,
+                    "velocityX": 0.9820182013148953,
+                    "velocityY": 1.7362520802853605,
+                    "timestamp": 0.422766784378578
+                },
+                {
+                    "x": 2.38835072517395,
+                    "y": 6.318583965301514,
+                    "heading": -1.1074238763723672e-8,
+                    "angularVelocity": -3.821280465498069e-8,
+                    "velocityX": 1.392262269357751,
+                    "velocityY": 1.315689481497163,
+                    "timestamp": 0.493227915108341
+                },
+                {
+                    "x": 2.4799014983496455,
+                    "y": 6.371301429692481,
+                    "heading": -1.319458225812021e-8,
+                    "angularVelocity": -3.903749361340101e-8,
+                    "velocityX": 1.6855317920452093,
+                    "velocityY": 0.9705757706637901,
+                    "timestamp": 0.5475435759628242
+                },
+                {
+                    "x": 2.5874215406057264,
+                    "y": 6.405308001046697,
+                    "heading": -1.535577963780212e-8,
+                    "angularVelocity": -3.978965207197302e-8,
+                    "velocityX": 1.979540349233062,
+                    "velocityY": 0.626091459097589,
+                    "timestamp": 0.6018592368173074
+                },
+                {
+                    "x": 2.7109670001208555,
+                    "y": 6.420651747906481,
+                    "heading": -1.7552309308039307e-8,
+                    "angularVelocity": -4.044015305772357e-8,
+                    "velocityX": 2.2745826447141577,
+                    "velocityY": 0.2824921324780203,
+                    "timestamp": 0.6561748976717905
+                },
+                {
+                    "x": 2.850621987061303,
+                    "y": 6.417405226954699,
+                    "heading": -1.9775875083794707e-8,
+                    "angularVelocity": -4.093791389017331e-8,
+                    "velocityX": 2.571173483741952,
+                    "velocityY": -0.059771360579454216,
+                    "timestamp": 0.7104905585262736
+                },
+                {
+                    "x": 3.006526369448995,
+                    "y": 6.395690573007363,
+                    "heading": -2.2012612720253862e-8,
+                    "angularVelocity": -4.1180421619075874e-8,
+                    "velocityX": 2.8703394184132693,
+                    "velocityY": -0.39978624220811715,
+                    "timestamp": 0.7648062193807568
+                },
+                {
+                    "x": 3.178958586879986,
+                    "y": 6.35575655651524,
+                    "heading": -2.4234645855460274e-8,
+                    "angularVelocity": -4.090970062204104e-8,
+                    "velocityX": 3.1746316756057436,
+                    "velocityY": -0.7352210368724164,
+                    "timestamp": 0.8191218802352399
+                },
+                {
+                    "x": 3.368741313034932,
+                    "y": 6.298384837056506,
+                    "heading": -2.6356859092562007e-8,
+                    "angularVelocity": -3.9071928501624834e-8,
+                    "velocityX": 3.494070092598876,
+                    "velocityY": -1.0562647781255272,
+                    "timestamp": 0.873437541089723
+                },
+                {
+                    "x": 3.5700972080230713,
+                    "y": 6.2574591636657715,
+                    "heading": -2.635685976829305e-8,
+                    "angularVelocity": -1.2440816646696897e-14,
+                    "velocityX": 3.7071425040054167,
+                    "velocityY": -0.7534783293344505,
+                    "timestamp": 0.9277532019442062
+                },
+                {
+                    "x": 3.903312814324664,
+                    "y": 6.255439725872096,
+                    "heading": -2.6356859842955344e-8,
+                    "angularVelocity": -8.476143761744691e-16,
+                    "velocityX": 3.7828709995267413,
+                    "velocityY": -0.02292591506032139,
+                    "timestamp": 1.0158385758158621
+                },
+                {
+                    "x": 4.231215212285405,
+                    "y": 6.314741822504602,
+                    "heading": -2.6356859970624137e-8,
+                    "angularVelocity": -1.4493758784250442e-15,
+                    "velocityX": 3.722552150839135,
+                    "velocityY": 0.6732343181536394,
+                    "timestamp": 1.103923949687518
+                },
+                {
+                    "x": 4.548594281876572,
+                    "y": 6.416266440930298,
+                    "heading": -2.6357038616847798e-8,
+                    "angularVelocity": -2.0281055655946166e-12,
+                    "velocityX": 3.603084776124604,
+                    "velocityY": 1.1525706703347043,
+                    "timestamp": 1.1920093235591738
+                },
+                {
+                    "x": 4.864492921080643,
+                    "y": 6.522307835013861,
+                    "heading": -2.6356860621748925e-8,
+                    "angularVelocity": 2.02071358534527e-12,
+                    "velocityX": 3.5862780084505173,
+                    "velocityY": 1.2038479203550279,
+                    "timestamp": 1.2800946974308296
+                },
+                {
+                    "x": 5.180407145227995,
+                    "y": 6.628302763705304,
+                    "heading": -2.6356859722122177e-8,
+                    "angularVelocity": 1.021313250736518e-14,
+                    "velocityX": 3.586454938732789,
+                    "velocityY": 1.2033204153921748,
+                    "timestamp": 1.3681800713024854
+                },
+                {
+                    "x": 5.4963196348481045,
+                    "y": 6.734302860663818,
+                    "heading": -2.6356858783882045e-8,
+                    "angularVelocity": 1.0651495820397439e-14,
+                    "velocityX": 3.58643524725062,
+                    "velocityY": 1.20337908892381,
+                    "timestamp": 1.4562654451741412
+                },
+                {
+                    "x": 5.81223242108415,
+                    "y": 6.8403020736225555,
+                    "heading": -2.6356857847826716e-8,
+                    "angularVelocity": 1.0626692486301617e-14,
+                    "velocityX": 3.586438614621235,
+                    "velocityY": 1.2033690532024592,
+                    "timestamp": 1.544350819045797
+                },
+                {
+                    "x": 6.128145213013902,
+                    "y": 6.946301269622288,
+                    "heading": -2.6356856889534724e-8,
+                    "angularVelocity": 1.0879135888139458e-14,
+                    "velocityX": 3.5864386792612257,
+                    "velocityY": 1.2033688606688582,
+                    "timestamp": 1.632436192917453
+                },
+                {
+                    "x": 6.444057998192988,
+                    "y": 7.052300485742325,
+                    "heading": -2.6356856001580027e-8,
+                    "angularVelocity": 1.0080624290280854e-14,
+                    "velocityX": 3.5864386026236104,
+                    "velocityY": 1.2033690890866424,
+                    "timestamp": 1.7205215667891087
+                },
+                {
+                    "x": 6.759970783516966,
+                    "y": 7.158299701416182,
+                    "heading": -2.635685511579164e-8,
+                    "angularVelocity": 1.0056027630784321e-14,
+                    "velocityX": 3.586438604268464,
+                    "velocityY": 1.2033690840214906,
+                    "timestamp": 1.8086069406607646
+                },
+                {
+                    "x": 7.0666471679768685,
+                    "y": 7.261199790661366,
+                    "heading": -1.748457582258916e-8,
+                    "angularVelocity": 1.0072373848515286e-7,
+                    "velocityX": 3.4815812317231813,
+                    "velocityY": 1.1681858715050373,
+                    "timestamp": 1.8966923145324204
+                },
+                {
+                    "x": 7.311988294075674,
+                    "y": 7.343519871122703,
+                    "heading": -1.0463867035430883e-8,
+                    "angularVelocity": 7.970353325086049e-8,
+                    "velocityX": 2.7852651957526704,
+                    "velocityY": 0.9345488001177432,
+                    "timestamp": 1.9847776884040762
+                },
+                {
+                    "x": 7.495994144762843,
+                    "y": 7.405259934452133,
+                    "heading": -5.222908649794349e-9,
+                    "angularVelocity": 5.94986783168245e-8,
+                    "velocityX": 2.088948966213813,
+                    "velocityY": 0.7009116339580341,
+                    "timestamp": 2.072863062275732
+                },
                 {
-                    "x": 7.602872461914855,
-                    "y": 7.439913035887961,
-                    "heading": -2.4779119751079125e-18,
-                    "angularVelocity": 1.3390118812558602e-17,
-                    "velocityX": 1.557906636985473,
-                    "velocityY": 0.5471322203925049,
-                    "timestamp": 1.782257840237747
+                    "x": 7.618664714352163,
+                    "y": 7.446419977873495,
+                    "heading": -1.738724662079323e-9,
+                    "angularVelocity": 3.955466249815561e-8,
+                    "velocityX": 1.3926326721215396,
+                    "velocityY": 0.4672744362816174,
+                    "timestamp": 2.160948436147388
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 0,
-                    "angularVelocity": 2.5025792615041696e-17,
-                    "velocityX": 0.7789533333445218,
-                    "velocityY": 0.27356611541215564,
-                    "timestamp": 1.8812721646953996
+                    "heading": -8.289937690891028e-31,
+                    "angularVelocity": 1.9739103024170296e-8,
+                    "velocityX": 0.6963163457465601,
+                    "velocityY": 0.23363722286151184,
+                    "timestamp": 2.2490338100190437
                 },
                 {
                     "x": 7.68,
                     "y": 7.467,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 1.3533974507170236e-34,
+                    "heading": 1.1211425849455439e-31,
+                    "angularVelocity": 1.0684035256675139e-29,
+                    "velocityX": 0,
                     "velocityY": 0,
-                    "timestamp": 1.9802864891530523
+                    "timestamp": 2.3371191838906995
                 }
             ],
             "constraints": [
@@ -15413,7 +17328,25 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 30
+                    "controlIntervalCount": 6
+                },
+                {
+                    "x": 2.1438515186309814,
+                    "y": 6.298208713531494,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 8
+                },
+                {
+                    "x": 3.345972776412964,
+                    "y": 6.298208713531494,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 26
                 },
                 {
                     "x": 5.403600215911865,
@@ -15440,522 +17373,612 @@
                     "y": 5.56,
                     "heading": 0,
                     "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -1.058013890027075e-32,
+                    "velocityX": 5.330261523342824e-32,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.26040474690827,
-                    "y": 5.564834298284281,
-                    "heading": -0.00020682462174428096,
-                    "angularVelocity": -0.005575654083684234,
-                    "velocityX": 0.28049498700644215,
-                    "velocityY": 0.1303247879443207,
-                    "timestamp": 0.037094234800108185
-                },
-                {
-                    "x": 2.2812142405469196,
-                    "y": 5.574502894544252,
-                    "heading": -0.0006204719876615776,
-                    "angularVelocity": -0.011151257551113,
-                    "velocityX": 0.5609899692172368,
-                    "velocityY": 0.26064956756953994,
-                    "timestamp": 0.07418846960021637
-                },
-                {
-                    "x": 2.312428480686622,
-                    "y": 5.589005788443611,
-                    "heading": -0.0012409398807264826,
-                    "angularVelocity": -0.016726801251156914,
-                    "velocityX": 0.8414849452457597,
-                    "velocityY": 0.39097433812857635,
-                    "timestamp": 0.11128270440032456
-                },
-                {
-                    "x": 2.3540474670292637,
-                    "y": 5.608342979609529,
-                    "heading": -0.002068225636714691,
-                    "angularVelocity": -0.022302273128063925,
-                    "velocityX": 1.1219799132376411,
-                    "velocityY": 0.5212990986367875,
-                    "timestamp": 0.14837693920043274
-                },
-                {
-                    "x": 2.406071199181798,
-                    "y": 5.632514467619691,
-                    "heading": -0.0031023259847413323,
-                    "angularVelocity": -0.02787765682725579,
-                    "velocityX": 1.4024748706336019,
-                    "velocityY": 0.6516238477600773,
-                    "timestamp": 0.18547117400054092
-                },
-                {
-                    "x": 2.4684996766150022,
-                    "y": 5.66152025198224,
-                    "heading": -0.004343236799506005,
-                    "angularVelocity": -0.03345292931508493,
-                    "velocityX": 1.682969813762594,
-                    "velocityY": 0.7819485836236739,
-                    "timestamp": 0.2225654088006491
-                },
-                {
-                    "x": 2.5413328985945514,
-                    "y": 5.69536033210275,
-                    "heading": -0.005790952692459236,
-                    "angularVelocity": -0.03902805653641911,
-                    "velocityX": 1.9634647370954057,
-                    "velocityY": 0.9122733034625292,
-                    "timestamp": 0.2596596436007573
-                },
-                {
-                    "x": 2.6245708640566887,
-                    "y": 5.7340347072253275,
-                    "heading": -0.00744546628275827,
-                    "angularVelocity": -0.04460298478228464,
-                    "velocityX": 2.2439596317510366,
-                    "velocityY": 1.0425980029237354,
-                    "timestamp": 0.2967538784008655
-                },
-                {
-                    "x": 2.7182135713591755,
-                    "y": 5.777543376315584,
-                    "heading": -0.009306766753500902,
-                    "angularVelocity": -0.05017762142212937,
-                    "velocityX": 2.5244544821346007,
-                    "velocityY": 1.1729226744995158,
-                    "timestamp": 0.3338481132009737
-                },
-                {
-                    "x": 2.8222610176995655,
-                    "y": 5.8258863377887575,
-                    "heading": -0.011374836524793099,
-                    "angularVelocity": -0.055751784136736884,
-                    "velocityX": 2.8049492569688192,
-                    "velocityY": 1.303247303352706,
-                    "timestamp": 0.3709423480010819
-                },
-                {
-                    "x": 2.9367131973687606,
-                    "y": 5.879063588696104,
-                    "heading": -0.013649641477510719,
-                    "angularVelocity": -0.061325027055469855,
-                    "velocityX": 3.0854438778950355,
-                    "velocityY": 1.4335718527125696,
-                    "timestamp": 0.4080365828011901
-                },
-                {
-                    "x": 3.06157009301301,
-                    "y": 5.937075120660461,
-                    "heading": -0.01613108463550858,
-                    "angularVelocity": -0.06689565565567898,
-                    "velocityX": 3.3659380309924987,
-                    "velocityY": 1.5638961762378367,
-                    "timestamp": 0.4451308176012983
-                },
-                {
-                    "x": 3.1888299168589427,
-                    "y": 5.996203084762226,
-                    "heading": -0.01613108529402469,
-                    "angularVelocity": -1.7752519083825314e-8,
-                    "velocityX": 3.430717051630934,
-                    "velocityY": 1.5939933636693306,
-                    "timestamp": 0.4822250524014065
-                },
-                {
-                    "x": 3.316089744181463,
-                    "y": 6.055331041381533,
-                    "heading": -0.016131085952509703,
-                    "angularVelocity": -1.7751680763659775e-8,
-                    "velocityX": 3.4307171453540595,
-                    "velocityY": 1.5939931619544851,
-                    "timestamp": 0.5193192872015147
-                },
-                {
-                    "x": 3.443349575355442,
-                    "y": 6.114458989711434,
-                    "heading": -0.01613108661098856,
-                    "angularVelocity": -1.7751514817327164e-8,
-                    "velocityX": 3.430717249183109,
-                    "velocityY": 1.5939929384856546,
-                    "timestamp": 0.5564135220016229
-                },
-                {
-                    "x": 3.5706094108109987,
-                    "y": 6.173586928826195,
-                    "heading": -0.016131087269461084,
-                    "angularVelocity": -1.7751344035751635e-8,
-                    "velocityX": 3.4307173646074394,
-                    "velocityY": 1.5939926900605101,
-                    "timestamp": 0.5935077568017311
-                },
-                {
-                    "x": 3.6978692510432434,
-                    "y": 6.232714857660196,
-                    "heading": -0.01613108792792704,
-                    "angularVelocity": -1.7751166882414398e-8,
-                    "velocityX": 3.4307174933791567,
-                    "velocityY": 1.5939924129080143,
-                    "timestamp": 0.6306019916018393
-                },
-                {
-                    "x": 3.825129096624906,
-                    "y": 6.291842774980762,
-                    "heading": -0.016131088586386323,
-                    "angularVelocity": -1.775098711225461e-8,
-                    "velocityX": 3.430717637590709,
-                    "velocityY": 1.5939921025246122,
-                    "timestamp": 0.6676962264019475
-                },
-                {
-                    "x": 3.952388948222243,
-                    "y": 6.3509706793539165,
-                    "heading": -0.016131089244838845,
-                    "angularVelocity": -1.775080474504649e-8,
-                    "velocityX": 3.4307177997634843,
-                    "velocityY": 1.5939917534835466,
-                    "timestamp": 0.7047904612020557
-                },
-                {
-                    "x": 4.07964880661482,
-                    "y": 6.410098569101814,
-                    "heading": -0.016131089903284688,
-                    "angularVelocity": -1.775062484471552e-8,
-                    "velocityX": 3.430717982952055,
-                    "velocityY": 1.5939913592104822,
-                    "timestamp": 0.7418846960021639
-                },
-                {
-                    "x": 4.206908672718448,
-                    "y": 6.469226442253365,
-                    "heading": -0.016131090561724272,
-                    "angularVelocity": -1.7750456035932977e-8,
-                    "velocityX": 3.4307181908294035,
-                    "velocityY": 1.5939909118000826,
-                    "timestamp": 0.7789789308022721
-                },
-                {
-                    "x": 4.33416854760619,
-                    "y": 6.52835429649903,
-                    "heading": -0.016131091220158388,
-                    "angularVelocity": -1.775030869099235e-8,
-                    "velocityX": 3.43071842763476,
-                    "velocityY": 1.593990402128264,
-                    "timestamp": 0.8160731656023803
-                },
-                {
-                    "x": 4.461428432515134,
-                    "y": 6.587482129176235,
-                    "heading": -0.016131091878588796,
-                    "angularVelocity": -1.7750208582663698e-8,
-                    "velocityX": 3.430718697790006,
-                    "velocityY": 1.5939898206778003,
-                    "timestamp": 0.8531674004024885
-                },
-                {
-                    "x": 4.588688397859823,
-                    "y": 6.646609788732575,
-                    "heading": -0.01613109253701839,
-                    "angularVelocity": -1.7750186861935105e-8,
-                    "velocityX": 3.4307208662063466,
-                    "velocityY": 1.5939851536219738,
-                    "timestamp": 0.8902616352025967
-                },
-                {
-                    "x": 4.7169608374986005,
-                    "y": 6.703507592945478,
-                    "heading": -0.016131093196594153,
-                    "angularVelocity": -1.7781085471466767e-8,
-                    "velocityX": 3.4580155199319775,
-                    "velocityY": 1.533871894635887,
-                    "timestamp": 0.9273558700027049
-                },
-                {
-                    "x": 4.84945193271468,
-                    "y": 6.749738410352506,
-                    "heading": -0.016131093868950192,
-                    "angularVelocity": -1.8125621008136676e-8,
-                    "velocityX": 3.571743585763167,
-                    "velocityY": 1.2463073481945173,
-                    "timestamp": 0.9644501048028131
-                },
-                {
-                    "x": 4.985276817345946,
-                    "y": 6.784991484609654,
-                    "heading": -0.0161310945734632,
-                    "angularVelocity": -1.8992520332334574e-8,
-                    "velocityX": 3.661617104738627,
-                    "velocityY": 0.9503653181449264,
-                    "timestamp": 1.0015443396029213
-                },
-                {
-                    "x": 5.123527608694991,
-                    "y": 6.809030968493646,
-                    "heading": -0.01613109533353302,
-                    "angularVelocity": -2.0490241268499195e-8,
-                    "velocityX": 3.7270155886499414,
-                    "velocityY": 0.6480652320645928,
-                    "timestamp": 1.0386385744030295
-                },
-                {
-                    "x": 5.263280146337045,
-                    "y": 6.821696104409015,
-                    "heading": -0.016131096180260384,
-                    "angularVelocity": -2.2826387114294202e-8,
-                    "velocityX": 3.767500216547063,
-                    "velocityY": 0.34143138370740883,
-                    "timestamp": 1.0757328092031377
+                    "x": 2.216523220338838,
+                    "y": 5.608939409819235,
+                    "heading": -0.000395848810607471,
+                    "angularVelocity": -0.004694167537861253,
+                    "velocityX": -0.3969839194829395,
+                    "velocityY": 0.5803473011402309,
+                    "timestamp": 0.08432779772229798
+                },
+                {
+                    "x": 2.1530740053514283,
+                    "y": 5.7090393916763,
+                    "heading": -0.0013136261454272055,
+                    "angularVelocity": -0.010883449581379904,
+                    "velocityX": -0.7524116210926717,
+                    "velocityY": 1.1870342231242381,
+                    "timestamp": 0.16865559544459596
+                },
+                {
+                    "x": 2.071477218025725,
+                    "y": 5.86558734321221,
+                    "heading": -0.0031284327789465032,
+                    "angularVelocity": -0.02152085886904205,
+                    "velocityX": -0.9676143517279073,
+                    "velocityY": 1.8564216754650953,
+                    "timestamp": 0.25298339316689394
+                },
+                {
+                    "x": 2.0458269370451427,
+                    "y": 6.041770495606775,
+                    "heading": -0.006595224749493174,
+                    "angularVelocity": -0.04111090368993278,
+                    "velocityX": -0.30417349525777404,
+                    "velocityY": 2.089265427928733,
+                    "timestamp": 0.3373111908891919
+                },
+                {
+                    "x": 2.0715234655116643,
+                    "y": 6.188301290095102,
+                    "heading": -0.01049068188832321,
+                    "angularVelocity": -0.046194223542478534,
+                    "velocityX": 0.30472192041755103,
+                    "velocityY": 1.7376333598900595,
+                    "timestamp": 0.4216389886114899
+                },
+                {
+                    "x": 2.1438515186309814,
+                    "y": 6.298208713531494,
+                    "heading": -0.014554322717054646,
+                    "angularVelocity": -0.04818862745724922,
+                    "velocityX": 0.8577011978600798,
+                    "velocityY": 1.303335630776598,
+                    "timestamp": 0.5059667863337879
+                },
+                {
+                    "x": 2.2162950329491684,
+                    "y": 6.355974505839093,
+                    "heading": -0.017431077998725047,
+                    "angularVelocity": -0.04874949538291163,
+                    "velocityX": 1.2276278031981451,
+                    "velocityY": 0.978899124069331,
+                    "timestamp": 0.5649777619156877
+                },
+                {
+                    "x": 2.310686009240105,
+                    "y": 6.394730061346725,
+                    "heading": -0.020327199149560698,
+                    "angularVelocity": -0.0490776694043422,
+                    "velocityX": 1.5995494966853068,
+                    "velocityY": 0.6567516487478545,
+                    "timestamp": 0.6239887374975874
+                },
+                {
+                    "x": 2.4271886355684367,
+                    "y": 6.414666608347948,
+                    "heading": -0.023223370555002502,
+                    "angularVelocity": -0.04907852101890474,
+                    "velocityX": 1.9742535211376202,
+                    "velocityY": 0.33784472811424154,
+                    "timestamp": 0.6829997130794871
+                },
+                {
+                    "x": 2.5660476863967387,
+                    "y": 6.416075601325133,
+                    "heading": -0.02609042518710973,
+                    "angularVelocity": -0.048585108174133695,
+                    "velocityX": 2.35310549366504,
+                    "velocityY": 0.02387679517735602,
+                    "timestamp": 0.7420106886613869
+                },
+                {
+                    "x": 2.72766661902605,
+                    "y": 6.399454975329908,
+                    "heading": -0.02887926513240676,
+                    "angularVelocity": -0.04725968208112396,
+                    "velocityX": 2.738794453669127,
+                    "velocityY": -0.281653130308932,
+                    "timestamp": 0.8010216642432866
+                },
+                {
+                    "x": 2.912832200300888,
+                    "y": 6.365845166245685,
+                    "heading": -0.031490096116184224,
+                    "angularVelocity": -0.04424314219571676,
+                    "velocityX": 3.1378159647242496,
+                    "velocityY": -0.5695518291775498,
+                    "timestamp": 0.8600326398251863
+                },
+                {
+                    "x": 3.123679163604623,
+                    "y": 6.318688161285352,
+                    "heading": -0.033616023768519236,
+                    "angularVelocity": -0.03602597027708684,
+                    "velocityX": 3.5730126679757386,
+                    "velocityY": -0.7991226122822729,
+                    "timestamp": 0.919043615407086
+                },
+                {
+                    "x": 3.345972776412964,
+                    "y": 6.298208713531494,
+                    "heading": -0.03361602546428586,
+                    "angularVelocity": -2.8736461454443448e-8,
+                    "velocityX": 3.766987591991692,
+                    "velocityY": -0.34704472434005124,
+                    "timestamp": 0.9780545909889857
+                },
+                {
+                    "x": 3.4285825306560853,
+                    "y": 6.294597584481868,
+                    "heading": -0.033616025709707474,
+                    "angularVelocity": -1.1227840838251836e-8,
+                    "velocityX": 3.779329612797739,
+                    "velocityY": -0.16520624081174923,
+                    "timestamp": 0.9999128999238256
+                },
+                {
+                    "x": 3.5112703400714786,
+                    "y": 6.2949695138762145,
+                    "heading": -0.033616025930811776,
+                    "angularVelocity": -1.0115343355589466e-8,
+                    "velocityX": 3.7829005739596693,
+                    "velocityY": 0.01701546974451014,
+                    "timestamp": 1.0217712088586655
+                },
+                {
+                    "x": 3.5938442713679413,
+                    "y": 6.299323633866166,
+                    "heading": -0.03361602613301592,
+                    "angularVelocity": -9.2506762233922e-9,
+                    "velocityX": 3.7776907418875507,
+                    "velocityY": 0.19919747693799758,
+                    "timestamp": 1.0436295177935053
+                },
+                {
+                    "x": 3.6761126565475006,
+                    "y": 6.3076498312062705,
+                    "heading": -0.03361602632044191,
+                    "angularVelocity": -8.574588182780684e-9,
+                    "velocityX": 3.763712253532677,
+                    "velocityY": 0.3809168113107395,
+                    "timestamp": 1.0654878267283452
+                },
+                {
+                    "x": 3.7578845383740482,
+                    "y": 6.31992876945557,
+                    "heading": -0.033616026496316675,
+                    "angularVelocity": -8.046128410971048e-9,
+                    "velocityX": 3.7409976256768736,
+                    "velocityY": 0.5617515191089957,
+                    "timestamp": 1.087346135663185
+                },
+                {
+                    "x": 3.8389701148011586,
+                    "y": 6.336131931270735,
+                    "heading": -0.03361602666323835,
+                    "angularVelocity": -7.63653221094489e-9,
+                    "velocityX": 3.709599707316274,
+                    "velocityY": 0.741281581455633,
+                    "timestamp": 1.109204444598025
+                },
+                {
+                    "x": 3.919181182263655,
+                    "y": 6.356221678659879,
+                    "heading": -0.03361602682336018,
+                    "angularVelocity": -7.3254443467155425e-9,
+                    "velocityX": 3.6695916276783906,
+                    "velocityY": 0.9190897360373237,
+                    "timestamp": 1.1310627535328648
+                },
+                {
+                    "x": 3.998331580016101,
+                    "y": 6.3801513240916305,
+                    "heading": -0.03361602697852209,
+                    "angularVelocity": -7.098532288064829e-9,
+                    "velocityX": 3.621066844118403,
+                    "velocityY": 1.0947619737229577,
+                    "timestamp": 1.1529210624677046
+                },
+                {
+                    "x": 4.076237649565588,
+                    "y": 6.407865179672378,
+                    "heading": -0.03361602713034875,
+                    "angularVelocity": -6.945947165054901e-9,
+                    "velocityX": 3.5641398326708003,
+                    "velocityY": 1.2678865352009827,
+                    "timestamp": 1.1747793714025445
+                },
+                {
+                    "x": 4.152718837418924,
+                    "y": 6.439298305619894,
+                    "heading": -0.033616027280326394,
+                    "angularVelocity": -6.861356032634536e-9,
+                    "velocityX": 3.4989526445677317,
+                    "velocityY": 1.4380401540310954,
+                    "timestamp": 1.1966376803373844
+                },
+                {
+                    "x": 4.227916828679672,
+                    "y": 6.473688356846056,
+                    "heading": -0.03361602742978782,
+                    "angularVelocity": -6.8377396934186395e-9,
+                    "velocityX": 3.4402474356508805,
+                    "velocityY": 1.5733171000866744,
+                    "timestamp": 1.2184959892722242
+                },
+                {
+                    "x": 4.303113417879144,
+                    "y": 6.50808147368302,
+                    "heading": -0.033616027579243535,
+                    "angularVelocity": -6.837478280080917e-9,
+                    "velocityX": 3.4401832924786557,
+                    "velocityY": 1.5734573493077775,
+                    "timestamp": 1.240354298207064
+                },
+                {
+                    "x": 4.378310012144908,
+                    "y": 6.542474579443129,
+                    "heading": -0.03361602772869459,
+                    "angularVelocity": -6.837264944013175e-9,
+                    "velocityX": 3.4401835242573666,
+                    "velocityY": 1.573456842550575,
+                    "timestamp": 1.262212607141904
+                },
+                {
+                    "x": 4.453506612352025,
+                    "y": 6.576867672213151,
+                    "heading": -0.03361602787814215,
+                    "angularVelocity": -6.837105393969464e-9,
+                    "velocityX": 3.4401837960695145,
+                    "velocityY": 1.5734562482645857,
+                    "timestamp": 1.2840709160767438
+                },
+                {
+                    "x": 4.528703218974218,
+                    "y": 6.6112607509573404,
+                    "heading": -0.03361602802758787,
+                    "angularVelocity": -6.837020955916861e-9,
+                    "velocityX": 3.440184089554037,
+                    "velocityY": 1.5734556065940832,
+                    "timestamp": 1.3059292250115837
+                },
+                {
+                    "x": 4.603899833102385,
+                    "y": 6.645653813290562,
+                    "heading": -0.03361602817703386,
+                    "angularVelocity": -6.837033689050042e-9,
+                    "velocityX": 3.440184432946286,
+                    "velocityY": 1.5734548558055208,
+                    "timestamp": 1.3277875339464236
+                },
+                {
+                    "x": 4.679098505825837,
+                    "y": 6.680042374380203,
+                    "heading": -0.033616028326483124,
+                    "angularVelocity": -6.83718315967473e-9,
+                    "velocityX": 3.4402786120196116,
+                    "velocityY": 1.573248927543637,
+                    "timestamp": 1.3496458428812634
+                },
+                {
+                    "x": 4.755659310004632,
+                    "y": 6.7112810805754854,
+                    "heading": -0.033616028476497646,
+                    "angularVelocity": -6.8630434261390676e-9,
+                    "velocityX": 3.502595027228562,
+                    "velocityY": 1.4291456072107467,
+                    "timestamp": 1.3715041518161033
+                },
+                {
+                    "x": 4.833635561443148,
+                    "y": 6.738796851922075,
+                    "heading": -0.0336160286284395,
+                    "angularVelocity": -6.951217281059994e-9,
+                    "velocityX": 3.5673505974759867,
+                    "velocityY": 1.258824341288896,
+                    "timestamp": 1.3933624607509432
+                },
+                {
+                    "x": 4.9128465230031875,
+                    "y": 6.762525251770201,
+                    "heading": -0.033616028783803605,
+                    "angularVelocity": -7.107782622056134e-9,
+                    "velocityX": 3.623837589457144,
+                    "velocityY": 1.0855551506230685,
+                    "timestamp": 1.415220769685783
+                },
+                {
+                    "x": 4.993108392358412,
+                    "y": 6.78241106619245,
+                    "heading": -0.033616028944227425,
+                    "angularVelocity": -7.33926038728002e-9,
+                    "velocityX": 3.6719157732872607,
+                    "velocityY": 0.9097599673208229,
+                    "timestamp": 1.437079078620623
+                },
+                {
+                    "x": 5.0742348898890315,
+                    "y": 6.798408083033217,
+                    "heading": -0.03361602911156847,
+                    "angularVelocity": -7.655717557583249e-9,
+                    "velocityX": 3.711471814789518,
+                    "velocityY": 0.7318506151803719,
+                    "timestamp": 1.4589373875554628
+                },
+                {
+                    "x": 5.156037716665883,
+                    "y": 6.810479143599293,
+                    "heading": -0.033616029288003786,
+                    "angularVelocity": -8.071773672975751e-9,
+                    "velocityX": 3.742413332189024,
+                    "velocityY": 0.5522412828028046,
+                    "timestamp": 1.4807956964903026
+                },
+                {
+                    "x": 5.238326998632702,
+                    "y": 6.818596213450421,
+                    "heading": -0.033616029476163836,
+                    "angularVelocity": -8.608170503938931e-9,
+                    "velocityX": 3.764668264691683,
+                    "velocityY": 0.37134939739963974,
+                    "timestamp": 1.5026540054251425
+                },
+                {
+                    "x": 5.3209117299494695,
+                    "y": 6.822740441774992,
+                    "heading": -0.033616029679319044,
+                    "angularVelocity": -9.29418657606336e-9,
+                    "velocityX": 3.778184834103796,
+                    "velocityY": 0.18959510257284434,
+                    "timestamp": 1.5245123143599824
                 },
                 {
                     "x": 5.403600215911865,
                     "y": 6.822902202606201,
-                    "heading": -0.0161310971470982,
-                    "angularVelocity": -2.6064368823146806e-8,
-                    "velocityX": 3.7827999507462917,
-                    "velocityY": 0.032514437989762196,
-                    "timestamp": 1.112827044003246
-                },
-                {
-                    "x": 5.530811605245593,
-                    "y": 6.814532094483614,
-                    "heading": -0.016131098042384447,
-                    "angularVelocity": -2.6566067685833464e-8,
-                    "velocityX": 3.7747775034488793,
-                    "velocityY": -0.2483684519763222,
-                    "timestamp": 1.1465274115659696
-                },
-                {
-                    "x": 5.657050747526861,
-                    "y": 6.796742322973506,
-                    "heading": -0.016131098828254187,
-                    "angularVelocity": -2.3319322572510494e-8,
-                    "velocityX": 3.745927757206515,
-                    "velocityY": -0.5278806374143462,
-                    "timestamp": 1.1802277791286933
-                },
-                {
-                    "x": 5.781621117814577,
-                    "y": 6.76963106353518,
-                    "heading": -0.016131099539274135,
-                    "angularVelocity": -2.109828463295381e-8,
-                    "velocityX": 3.6964098405118033,
-                    "velocityY": -0.8044796362492181,
-                    "timestamp": 1.213928146691417
-                },
-                {
-                    "x": 5.903835409646495,
-                    "y": 6.733347943713289,
-                    "heading": -0.016131100200026813,
-                    "angularVelocity": -1.96066906061588e-8,
-                    "velocityX": 3.626497295747634,
-                    "velocityY": -1.0766386970219277,
-                    "timestamp": 1.2476285142541406
-                },
-                {
-                    "x": 6.023019351333038,
-                    "y": 6.68809326224078,
-                    "heading": -0.016131100829259,
-                    "angularVelocity": -1.86713746487578e-8,
-                    "velocityX": 3.5365769072018636,
-                    "velocityY": -1.3428542400399708,
-                    "timestamp": 1.2813288818168642
-                },
-                {
-                    "x": 6.138515596479519,
-                    "y": 6.634117196875684,
-                    "heading": -0.016131101442406693,
-                    "angularVelocity": -1.81940951537302e-8,
-                    "velocityX": 3.4271509036665004,
-                    "velocityY": -1.601646191681265,
-                    "timestamp": 1.3150292493795879
-                },
-                {
-                    "x": 6.2512259314426855,
-                    "y": 6.5745422130600915,
-                    "heading": -0.016131102051778846,
-                    "angularVelocity": -1.80820624580199e-8,
-                    "velocityX": 3.3444838473464196,
-                    "velocityY": -1.7677843929954529,
-                    "timestamp": 1.3487296169423115
-                },
-                {
-                    "x": 6.363935646775815,
-                    "y": 6.514966056976746,
-                    "heading": -0.01613110266098242,
-                    "angularVelocity": -1.807706019437417e-8,
-                    "velocityX": 3.344465460898959,
-                    "velocityY": -1.7678191780093255,
-                    "timestamp": 1.3824299845050352
-                },
-                {
-                    "x": 6.4758517936606514,
-                    "y": 6.453912192428435,
-                    "heading": -0.016131102003666662,
-                    "angularVelocity": 1.950470585863807e-8,
-                    "velocityX": 3.320917692560386,
-                    "velocityY": -1.811667615632877,
-                    "timestamp": 1.4161303520677588
-                },
-                {
-                    "x": 6.587767622018337,
-                    "y": 6.392857744000494,
-                    "heading": -0.016131101346154864,
-                    "angularVelocity": 1.951052313957626e-8,
-                    "velocityX": 3.32090824081921,
-                    "velocityY": -1.8116849412489389,
-                    "timestamp": 1.4498307196304825
-                },
-                {
-                    "x": 6.699683405881235,
-                    "y": 6.331803214011495,
-                    "heading": -0.016131100688641712,
-                    "angularVelocity": 1.9510563155904532e-8,
-                    "velocityX": 3.3209069205135386,
-                    "velocityY": -1.811687361432026,
-                    "timestamp": 1.4835310871932061
-                },
-                {
-                    "x": 6.811599167638959,
-                    "y": 6.270748643502715,
-                    "heading": -0.016131100031139815,
-                    "angularVelocity": 1.9510229240352237e-8,
-                    "velocityX": 3.320906264580792,
-                    "velocityY": -1.8116885637862563,
-                    "timestamp": 1.5172314547559298
-                },
-                {
-                    "x": 6.923514914598752,
-                    "y": 6.209694045868981,
-                    "heading": -0.016131099373601954,
-                    "angularVelocity": 1.95112965056638e-8,
-                    "velocityX": 3.3209058254778503,
-                    "velocityY": -1.8116893686722635,
-                    "timestamp": 1.5509318223186535
-                },
-                {
-                    "x": 7.031584175346113,
-                    "y": 6.1507377884047765,
-                    "heading": -0.013826704963615632,
-                    "angularVelocity": 0.0683789102803133,
-                    "velocityX": 3.2067680136194574,
-                    "velocityY": -1.7494247608568367,
-                    "timestamp": 1.584632189881377
-                },
-                {
-                    "x": 7.131340439149152,
-                    "y": 6.096316611291437,
-                    "heading": -0.011699535798216875,
-                    "angularVelocity": 0.06312005830317652,
-                    "velocityX": 2.960094236876553,
-                    "velocityY": -1.614854111369865,
-                    "timestamp": 1.6183325574441008
-                },
-                {
-                    "x": 7.222783690004588,
-                    "y": 6.046430524071638,
-                    "heading": -0.00974961791861361,
-                    "angularVelocity": 0.057860433598327515,
-                    "velocityX": 2.713419985263968,
-                    "velocityY": -1.4802831787205646,
-                    "timestamp": 1.6520329250068244
-                },
-                {
-                    "x": 7.305913922772626,
-                    "y": 6.001079530283105,
-                    "heading": -0.00797695998479322,
-                    "angularVelocity": 0.05260055192336173,
-                    "velocityX": 2.466745581136908,
-                    "velocityY": -1.3457121410953312,
-                    "timestamp": 1.685733292569548
-                },
-                {
-                    "x": 7.380731135011445,
-                    "y": 5.960263631930958,
-                    "heading": -0.00638156583911186,
-                    "angularVelocity": 0.04734055623313652,
-                    "velocityX": 2.2200711045530213,
-                    "velocityY": -1.2111410439717167,
-                    "timestamp": 1.7194336601322717
-                },
-                {
-                    "x": 7.447235325348076,
-                    "y": 5.923982830388488,
-                    "heading": -0.004963437343393232,
-                    "angularVelocity": 0.042080505296519764,
-                    "velocityX": 1.9733965872286794,
-                    "velocityY": -1.0765699060980205,
-                    "timestamp": 1.7531340276949954
-                },
-                {
-                    "x": 7.505426492937412,
-                    "y": 5.892237126701046,
-                    "heading": -0.0037225753686417596,
-                    "angularVelocity": 0.0368204285143562,
-                    "velocityX": 1.72672204482727,
-                    "velocityY": -0.9419987372053683,
-                    "timestamp": 1.786834395257719
-                },
-                {
-                    "x": 7.555304637231595,
-                    "y": 5.8650265217185185,
-                    "heading": -0.002658980227466916,
-                    "angularVelocity": 0.03156034245604838,
-                    "velocityX": 1.4800474861690802,
-                    "velocityY": -0.8074275430937743,
-                    "timestamp": 1.8205347628204427
-                },
-                {
-                    "x": 7.596869757865521,
-                    "y": 5.842351016163066,
-                    "heading": -0.0017726518914738514,
-                    "angularVelocity": 0.02630025724030852,
-                    "velocityX": 1.233372916677093,
-                    "velocityY": -0.6728563275533557,
-                    "timestamp": 1.8542351303831663
-                },
-                {
-                    "x": 7.630121854593904,
-                    "y": 5.824210610667954,
-                    "heading": -0.001063590116536395,
-                    "angularVelocity": 0.021040179268589657,
-                    "velocityX": 0.9866983399066315,
-                    "velocityY": -0.5382850932218669,
-                    "timestamp": 1.88793549794589
-                },
-                {
-                    "x": 7.655060927254085,
-                    "y": 5.810605305801891,
-                    "heading": -0.000531794525727047,
-                    "angularVelocity": 0.01578011248154109,
-                    "velocityX": 0.7400237583095566,
-                    "velocityY": -0.4037138420149424,
-                    "timestamp": 1.9216358655086136
-                },
+                    "heading": -0.03361602991707219,
+                    "angularVelocity": -1.087701477803747e-8,
+                    "velocityX": 3.7829315254392575,
+                    "velocityY": 0.00740042753033698,
+                    "timestamp": 1.5463706232948222
+                },
+                {
+                    "x": 5.530709925548806,
+                    "y": 6.813694137770519,
+                    "heading": -0.03361603012559057,
+                    "angularVelocity": -6.189541308885362e-9,
+                    "velocityX": 3.773052419307907,
+                    "velocityY": -0.2733269661668366,
+                    "timestamp": 1.5800594501413512
+                },
+                {
+                    "x": 5.656785942150605,
+                    "y": 6.795079480224979,
+                    "heading": -0.03361603030947626,
+                    "angularVelocity": -5.458358559418031e-9,
+                    "velocityX": 3.7423688624167126,
+                    "velocityY": -0.5525469209818521,
+                    "timestamp": 1.6137482769878801
+                },
+                {
+                    "x": 5.781133118086899,
+                    "y": 6.767160888262499,
+                    "heading": -0.033616030476517275,
+                    "angularVelocity": -4.958350514129881e-9,
+                    "velocityX": 3.6910509381274874,
+                    "velocityY": -0.8287196253423904,
+                    "timestamp": 1.6474371038344091
+                },
+                {
+                    "x": 5.903065850616414,
+                    "y": 6.730092343263882,
+                    "heading": -0.03361603063230265,
+                    "angularVelocity": -4.624244623037642e-9,
+                    "velocityX": 3.619381971506033,
+                    "velocityY": -1.100321633860491,
+                    "timestamp": 1.681125930680938
+                },
+                {
+                    "x": 6.0219118917079,
+                    "y": 6.684078355941691,
+                    "heading": -0.033616030781137125,
+                    "angularVelocity": -4.4179180116936e-9,
+                    "velocityX": 3.5277583761789866,
+                    "velocityY": -1.3658530625542256,
+                    "timestamp": 1.714814757527467
+                },
+                {
+                    "x": 6.137016291187876,
+                    "y": 6.629373275360583,
+                    "heading": -0.033616030926600995,
+                    "angularVelocity": -4.3178668766413575e-9,
+                    "velocityX": 3.4166936119307345,
+                    "velocityY": -1.623834538089393,
+                    "timestamp": 1.748503584373996
+                },
+                {
+                    "x": 6.249923393155274,
+                    "y": 6.570266127170688,
+                    "heading": -0.0336160310713635,
+                    "angularVelocity": -4.297048049789566e-9,
+                    "velocityX": 3.3514702806883885,
+                    "velocityY": -1.7545030125020786,
+                    "timestamp": 1.782192411220525
+                },
+                {
+                    "x": 6.362830075113665,
+                    "y": 6.51115817668204,
+                    "heading": -0.03361603121596069,
+                    "angularVelocity": -4.292140831453005e-9,
+                    "velocityX": 3.351457813379591,
+                    "velocityY": -1.7545268274825585,
+                    "timestamp": 1.815881238067054
+                },
+                {
+                    "x": 6.474865356079977,
+                    "y": 6.45041474520773,
+                    "heading": -0.033616029845538055,
+                    "angularVelocity": 4.06788471811577e-8,
+                    "velocityX": 3.32559164130869,
+                    "velocityY": -1.8030735160660076,
+                    "timestamp": 1.849570064913583
+                },
+                {
+                    "x": 6.586900214909621,
+                    "y": 6.389670535150393,
+                    "heading": -0.03361602847479329,
+                    "angularVelocity": 4.068840886286375e-8,
+                    "velocityX": 3.3255791108435817,
+                    "velocityY": -1.8030966270823277,
+                    "timestamp": 1.8832588917601119
+                },
+                {
+                    "x": 6.6989350261872,
+                    "y": 6.328926237389509,
+                    "heading": -0.03361602710404103,
+                    "angularVelocity": 4.068863135772345e-8,
+                    "velocityX": 3.325577699335118,
+                    "velocityY": -1.8030992304246154,
+                    "timestamp": 1.9169477186066408
+                },
+                {
+                    "x": 6.810969814733213,
+                    "y": 6.268181897703298,
+                    "heading": -0.03361602573329801,
+                    "angularVelocity": 4.068835717596455e-8,
+                    "velocityX": 3.3255770245842737,
+                    "velocityY": -1.8031004749122261,
+                    "timestamp": 1.9506365454531698
+                },
+                {
+                    "x": 6.92300458832202,
+                    "y": 6.207437530430852,
+                    "heading": -0.03361602436246198,
+                    "angularVelocity": 4.069111827209e-8,
+                    "velocityX": 3.3255765806030024,
+                    "velocityY": -1.803101293766319,
+                    "timestamp": 1.9843253722996987
+                },
+                {
+                    "x": 7.031146753085382,
+                    "y": 6.14880363204561,
+                    "heading": -0.02881387895998304,
+                    "angularVelocity": 0.1425441563861497,
+                    "velocityX": 3.210030591329563,
+                    "velocityY": -1.7404553341186793,
+                    "timestamp": 2.0180141991462275
+                },
+                {
+                    "x": 7.130970312732727,
+                    "y": 6.094680017318762,
+                    "heading": -0.024381039069153343,
+                    "angularVelocity": 0.13158190135330103,
+                    "velocityX": 2.9631058422454424,
+                    "velocityY": -1.6065746359589999,
+                    "timestamp": 2.0517030259927562
+                },
+                {
+                    "x": 7.222475251377904,
+                    "y": 6.04506669568433,
+                    "heading": -0.02031756633641395,
+                    "angularVelocity": 0.12061781644251243,
+                    "velocityX": 2.716180621605835,
+                    "velocityY": -1.4726936577651888,
+                    "timestamp": 2.085391852839285
+                },
+                {
+                    "x": 7.305661563921799,
+                    "y": 5.999963670644433,
+                    "heading": -0.016623481896744244,
+                    "angularVelocity": 0.10965310417300114,
+                    "velocityX": 2.469255249607073,
+                    "velocityY": -1.33881257561641,
+                    "timestamp": 2.1190806796858137
+                },
+                {
+                    "x": 7.380529247941795,
+                    "y": 5.959370944187338,
+                    "heading": -0.013298795761870387,
+                    "angularVelocity": 0.09868809472109723,
+                    "velocityX": 2.222329805696702,
+                    "velocityY": -1.2049314344491047,
+                    "timestamp": 2.1527695065323424
+                },
+                {
+                    "x": 7.447078302075198,
+                    "y": 5.923288517676822,
+                    "heading": -0.010343513390295376,
+                    "angularVelocity": 0.08772292324209098,
+                    "velocityX": 1.9754043213368524,
+                    "velocityY": -1.0710502528001746,
+                    "timestamp": 2.186458333378871
+                },
+                {
+                    "x": 7.505308725482669,
+                    "y": 5.891716392152232,
+                    "heading": -0.007757637975370072,
+                    "angularVelocity": 0.07675765697347174,
+                    "velocityX": 1.7284788120626102,
+                    "velocityY": -0.9371690402998547,
+                    "timestamp": 2.2201471602254
+                },
+                {
+                    "x": 7.555220517619577,
+                    "y": 5.86465456845934,
+                    "heading": -0.005541171447259703,
+                    "angularVelocity": 0.06579233341092419,
+                    "velocityX": 1.481553286621783,
+                    "velocityY": -0.8032878026941531,
+                    "timestamp": 2.2538359870719287
+                },
+                {
+                    "x": 7.596813678122498,
+                    "y": 5.842103047317274,
+                    "heading": -0.0036941149792240496,
+                    "angularVelocity": 0.054826975021999756,
+                    "velocityX": 1.234627750393344,
+                    "velocityY": -0.6694065437422472,
+                    "timestamp": 2.2875248139184574
+                },
+                {
+                    "x": 7.630088206746783,
+                    "y": 5.824061829356817,
+                    "heading": -0.0022164692716530373,
+                    "angularVelocity": 0.04386159584309864,
+                    "velocityX": 0.9877022069028755,
+                    "velocityY": -0.535525266066523,
+                    "timestamp": 2.321213640764986
+                },
+                {
+                    "x": 7.655044103329761,
+                    "y": 5.81053091514454,
+                    "heading": -0.0011082347348293014,
+                    "angularVelocity": 0.0328962044856008,
+                    "velocityX": 0.7407766585837434,
+                    "velocityY": -0.4016439715731056,
+                    "timestamp": 2.354902467611515
+                },
                 {
-                    "x": 7.671686975742638,
-                    "y": 5.8015351020850225,
-                    "heading": -0.00017726465841382563,
-                    "angularVelocity": 0.010520059362948236,
-                    "velocityX": 0.49334917364355607,
-                    "velocityY": -0.26914257537363395,
-                    "timestamp": 1.9553362330713373
+                    "x": 7.671681367767583,
+                    "y": 5.8015103051986525,
+                    "heading": -0.0003694116016791507,
+                    "angularVelocity": 0.021930806214050736,
+                    "velocityX": 0.49385110718203246,
+                    "velocityY": -0.2677626616973202,
+                    "timestamp": 2.3885912944580436
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 3.6111480760753294e-31,
-                    "angularVelocity": 0.005260021514121506,
-                    "velocityX": 0.24667458721003926,
-                    "velocityY": -0.1345712944104092,
-                    "timestamp": 1.989036600634061
+                    "heading": -1.3509198173227418e-31,
+                    "angularVelocity": 0.010965404149034802,
+                    "velocityX": 0.24692555399191934,
+                    "velocityY": -0.13388133754848214,
+                    "timestamp": 2.4222801213045724
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": -8.452347473573201e-32,
-                    "angularVelocity": -1.5098225699671103e-29,
-                    "velocityX": 2.1451735090517096e-32,
-                    "velocityY": 9.12030277003484e-33,
-                    "timestamp": 2.0227369681967846
+                    "heading": 2.046162663204931e-31,
+                    "angularVelocity": 9.569528449856341e-30,
+                    "velocityX": -2.6100111398714645e-32,
+                    "velocityY": -7.871960721903e-33,
+                    "timestamp": 2.455968948151101
                 }
             ],
             "constraints": [
@@ -15992,7 +18015,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 26
+                    "controlIntervalCount": 7
+                },
+                {
+                    "x": 2.892774820327759,
+                    "y": 6.261821746826172,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 25
                 },
                 {
                     "x": 4.148105144500732,
@@ -16027,568 +18059,622 @@
                     "x": 2.25,
                     "y": 5.56,
                     "heading": 0,
-                    "angularVelocity": 8.750120685231394e-32,
-                    "velocityX": 3.618098191401073e-32,
-                    "velocityY": 7.301888602720515e-32,
+                    "angularVelocity": 0,
+                    "velocityX": 3.8917516758910124e-38,
+                    "velocityY": -7.197289285396863e-34,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.256418341264449,
-                    "y": 5.558391022160101,
-                    "heading": -2.5296452464584536e-15,
-                    "angularVelocity": -8.979773235851594e-14,
-                    "velocityX": 0.22783925602590605,
-                    "velocityY": -0.05711574048502746,
-                    "timestamp": 0.0281704802605187
-                },
-                {
-                    "x": 2.2692550247581535,
-                    "y": 5.555173071000616,
-                    "heading": -7.593688933611638e-15,
-                    "angularVelocity": -1.7976419432911875e-13,
-                    "velocityX": 0.4556785463006615,
-                    "velocityY": -0.11423132050732354,
-                    "timestamp": 0.0563409605210374
-                },
-                {
-                    "x": 2.2885100515708303,
-                    "y": 5.55034615163608,
-                    "heading": -1.5195474783343342e-14,
-                    "angularVelocity": -2.6984935224723246e-13,
-                    "velocityX": 0.6835178752583485,
-                    "velocityY": -0.1713467189730698,
-                    "timestamp": 0.0845114407815561
-                },
-                {
-                    "x": 2.314183422940896,
-                    "y": 5.54391026989229,
-                    "heading": -2.533887633402318e-14,
-                    "angularVelocity": -3.6007201361891127e-13,
-                    "velocityX": 0.9113572481775378,
-                    "velocityY": -0.2284619106337897,
-                    "timestamp": 0.1126819210420748
-                },
-                {
-                    "x": 2.346275140284958,
-                    "y": 5.535865432453464,
-                    "heading": -3.8024880419905343e-14,
-                    "angularVelocity": -4.5032970578145327e-13,
-                    "velocityX": 1.139196671383666,
-                    "velocityY": -0.2855768650171331,
-                    "timestamp": 0.14085240130259352
-                },
-                {
-                    "x": 2.3847852052344276,
-                    "y": 5.526211647048542,
-                    "heading": -5.3249348657163737e-14,
-                    "angularVelocity": -5.404404929621474e-13,
-                    "velocityX": 1.3670361525018668,
-                    "velocityY": -0.3426915450373883,
-                    "timestamp": 0.16902288156311224
-                },
-                {
-                    "x": 2.4297136196807942,
-                    "y": 5.514948922688138,
-                    "heading": -7.10022499667577e-14,
-                    "angularVelocity": -6.301951953680268e-13,
-                    "velocityX": 1.594875700764475,
-                    "velocityY": -0.3998059051974526,
-                    "timestamp": 0.19719336182363095
-                },
-                {
-                    "x": 2.4810603858307716,
-                    "y": 5.502077269965895,
-                    "heading": -9.12659901579941e-14,
-                    "angularVelocity": -7.193253365088161e-13,
-                    "velocityX": 1.8227153273613368,
-                    "velocityY": -0.45691988930278143,
-                    "timestamp": 0.22536384208414967
-                },
-                {
-                    "x": 2.5388255062707983,
-                    "y": 5.4875967014378215,
-                    "heading": -1.1400857239830863e-13,
-                    "angularVelocity": -8.073196492084524e-13,
-                    "velocityX": 2.050555045771992,
-                    "velocityY": -0.5140334276930385,
-                    "timestamp": 0.2535343223446684
-                },
-                {
-                    "x": 2.603008984034667,
-                    "y": 5.4715072320838045,
-                    "heading": -1.3917722607752625e-13,
-                    "angularVelocity": -8.934407027436765e-13,
-                    "velocityX": 2.2783948718766736,
-                    "velocityY": -0.571146434324935,
-                    "timestamp": 0.2817048026051871
-                },
-                {
-                    "x": 2.673610822650838,
-                    "y": 5.453808879815544,
-                    "heading": -1.6668826583849139e-13,
-                    "angularVelocity": -9.76591080715578e-13,
-                    "velocityX": 2.5062348232351805,
-                    "velocityY": -0.6282588051246059,
-                    "timestamp": 0.3098752828657058
-                },
-                {
-                    "x": 2.7506310260932763,
-                    "y": 5.434501665856061,
-                    "heading": -1.9641690622397207e-13,
-                    "angularVelocity": -1.0553118055097966e-12,
-                    "velocityX": 2.7340749156620854,
-                    "velocityY": -0.6853704225462842,
-                    "timestamp": 0.33804576312622453
-                },
-                {
-                    "x": 2.8340695983911575,
-                    "y": 5.413585614341132,
-                    "heading": -2.282023162882806e-13,
-                    "angularVelocity": -1.128323328886794e-12,
-                    "velocityX": 2.9619151511173074,
-                    "velocityY": -0.7424811831924288,
-                    "timestamp": 0.36621624338674325
-                },
-                {
-                    "x": 2.9239265420414853,
-                    "y": 5.391060748788907,
-                    "heading": -2.61980248572899e-13,
-                    "angularVelocity": -1.1990541861941579e-12,
-                    "velocityX": 3.189755475211519,
-                    "velocityY": -0.7995911089877218,
-                    "timestamp": 0.39438672364726196
-                },
-                {
-                    "x": 3.020201851829833,
-                    "y": 5.366927076274376,
-                    "heading": -2.9867647823192555e-13,
-                    "angularVelocity": -1.302648350982896e-12,
-                    "velocityX": 3.417595614203248,
-                    "velocityY": -0.8567007836339452,
-                    "timestamp": 0.4225572039077807
-                },
-                {
-                    "x": 3.12289548969471,
-                    "y": 5.341184519140017,
-                    "heading": -3.474157859958988e-13,
-                    "angularVelocity": -1.7301553723681501e-12,
-                    "velocityX": 3.6454344020823566,
-                    "velocityY": -0.9138132149787032,
-                    "timestamp": 0.4507276841682994
-                },
-                {
-                    "x": 3.2262642847742486,
-                    "y": 5.315271868799975,
-                    "heading": -3.474349355086831e-13,
-                    "angularVelocity": -6.797723212352284e-16,
-                    "velocityX": 3.6694012357471815,
-                    "velocityY": -0.9198512095073661,
-                    "timestamp": 0.4788981644288181
-                },
-                {
-                    "x": 3.3296318094304937,
-                    "y": 5.289354150477899,
-                    "heading": -3.47428956348503e-13,
-                    "angularVelocity": 2.1224913898406702e-16,
-                    "velocityX": 3.669356138067555,
-                    "velocityY": -0.920031113505745,
-                    "timestamp": 0.5070686446893368
-                },
-                {
-                    "x": 3.4329975109325144,
-                    "y": 5.263429161946644,
-                    "heading": -3.4742781851400116e-13,
-                    "angularVelocity": 4.0391022494337805e-17,
-                    "velocityX": 3.669291419461124,
-                    "velocityY": -0.9202891924987843,
-                    "timestamp": 0.5352391249498555
-                },
-                {
-                    "x": 3.536360974747324,
-                    "y": 5.237495253145949,
-                    "heading": -3.474322284236003e-13,
-                    "angularVelocity": -1.565436427905273e-16,
-                    "velocityX": 3.669211985699591,
-                    "velocityY": -0.9206058455823695,
-                    "timestamp": 0.5634096052103742
-                },
-                {
-                    "x": 3.6397250084501422,
-                    "y": 5.211563615847215,
-                    "heading": -3.474363074560882e-13,
-                    "angularVelocity": -1.4479811668640362e-16,
-                    "velocityX": 3.6692322156709736,
-                    "velocityY": -0.9205252114596718,
-                    "timestamp": 0.5915800854708929
-                },
-                {
-                    "x": 3.7430830091153613,
-                    "y": 5.185607942589789,
-                    "heading": -3.474405753435316e-13,
-                    "angularVelocity": -1.515021186684875e-16,
-                    "velocityX": 3.669018053983158,
-                    "velocityY": -0.9213784435831122,
-                    "timestamp": 0.6197505657314116
-                },
-                {
-                    "x": 3.8464299042958676,
-                    "y": 5.1596080862072276,
-                    "heading": -3.474364187384818e-13,
-                    "angularVelocity": 1.4755179936907513e-16,
-                    "velocityX": 3.668623829794913,
-                    "velocityY": -0.92294686289042,
-                    "timestamp": 0.6479210459919303
-                },
-                {
-                    "x": 3.9490305912287407,
-                    "y": 5.130804053406161,
-                    "heading": -3.4744608105895375e-13,
-                    "angularVelocity": -3.429945241470833e-16,
-                    "velocityX": 3.642134815737201,
-                    "velocityY": -1.0224899446047346,
-                    "timestamp": 0.6760915262524491
-                },
-                {
-                    "x": 4.049738686089906,
-                    "y": 5.09595505292046,
-                    "heading": -3.4745592092862155e-13,
-                    "angularVelocity": -3.4929719255187864e-16,
-                    "velocityX": 3.57495129404341,
-                    "velocityY": -1.237075128411728,
-                    "timestamp": 0.7042620065129678
+                    "x": 2.2621897956017167,
+                    "y": 5.610801651791703,
+                    "heading": 3.480716271732917e-18,
+                    "angularVelocity": 4.397302355093269e-17,
+                    "velocityX": 0.15399765670735568,
+                    "velocityY": 0.6417938075748972,
+                    "timestamp": 0.07915572134244127
+                },
+                {
+                    "x": 2.2905203388747193,
+                    "y": 5.711291077569501,
+                    "heading": 3.7329161294500195e-18,
+                    "angularVelocity": 3.1861229325774303e-18,
+                    "velocityX": 0.35790897729855475,
+                    "velocityY": 1.2695156341645175,
+                    "timestamp": 0.15831144268488254
+                },
+                {
+                    "x": 2.3436883815778624,
+                    "y": 5.857742406339618,
+                    "heading": 7.49671396174733e-18,
+                    "angularVelocity": 4.754928420848352e-17,
+                    "velocityX": 0.6716891944314332,
+                    "velocityY": 1.8501673193848232,
+                    "timestamp": 0.23746716402732382
+                },
+                {
+                    "x": 2.444399562681219,
+                    "y": 6.0258512044397365,
+                    "heading": 1.0345374349306828e-17,
+                    "angularVelocity": 3.5988057740869555e-17,
+                    "velocityX": 1.2723171413935193,
+                    "velocityY": 2.1237731808778637,
+                    "timestamp": 0.3166228853697651
+                },
+                {
+                    "x": 2.5795093648860483,
+                    "y": 6.154639121411333,
+                    "heading": 6.772254039775596e-18,
+                    "angularVelocity": -4.5140391310627574e-17,
+                    "velocityX": 1.706886121602264,
+                    "velocityY": 1.6270196871106504,
+                    "timestamp": 0.3957786067122063
+                },
+                {
+                    "x": 2.7311723872223266,
+                    "y": 6.233875155156547,
+                    "heading": -7.527039882559706e-19,
+                    "angularVelocity": -9.506524797608463e-17,
+                    "velocityX": 1.916008341079448,
+                    "velocityY": 1.00101461273312,
+                    "timestamp": 0.4749343280546476
+                },
+                {
+                    "x": 2.892774820327759,
+                    "y": 6.261821746826172,
+                    "heading": -9.856118161245683e-18,
+                    "angularVelocity": -1.1500639891239944e-16,
+                    "velocityX": 2.041576153495111,
+                    "velocityY": 0.3530583916824199,
+                    "timestamp": 0.5540900493970888
+                },
+                {
+                    "x": 2.9421812890727397,
+                    "y": 6.2655363212140305,
+                    "heading": -1.2690994415516207e-17,
+                    "angularVelocity": -1.1860957835731596e-16,
+                    "velocityX": 2.067137973857932,
+                    "velocityY": 0.15541563622967255,
+                    "timestamp": 0.5779909545953872
+                },
+                {
+                    "x": 2.992183666499164,
+                    "y": 6.264525133875901,
+                    "heading": -1.5700573063161643e-17,
+                    "angularVelocity": -1.2591902515397632e-16,
+                    "velocityX": 2.092070447188938,
+                    "velocityY": -0.042307491274448855,
+                    "timestamp": 0.6018918597936855
+                },
+                {
+                    "x": 3.0427650175812166,
+                    "y": 6.258786080293358,
+                    "heading": -1.8888665404790212e-17,
+                    "angularVelocity": -1.333879346368914e-16,
+                    "velocityX": 2.1162943688699283,
+                    "velocityY": -0.24011867060800707,
+                    "timestamp": 0.6257927649919839
+                },
+                {
+                    "x": 3.093906133667358,
+                    "y": 6.248316847924188,
+                    "heading": -2.226054198779156e-17,
+                    "angularVelocity": -1.4107736063473173e-16,
+                    "velocityX": 2.139714611720349,
+                    "velocityY": -0.4380266053653904,
+                    "timestamp": 0.6496936701902822
+                },
+                {
+                    "x": 3.1455850428469736,
+                    "y": 6.233114888531311,
+                    "heading": -2.51599280773308e-17,
+                    "angularVelocity": -1.2130863242066928e-16,
+                    "velocityX": 2.162215562584425,
+                    "velocityY": -0.6360411568829696,
+                    "timestamp": 0.6735945753885806
+                },
+                {
+                    "x": 3.197776369275469,
+                    "y": 6.213177387662576,
+                    "heading": -2.8136873311901383e-17,
+                    "angularVelocity": -1.2455366258796381e-16,
+                    "velocityX": 2.1836548028403113,
+                    "velocityY": -0.8341734634450271,
+                    "timestamp": 0.6974954805868789
+                },
+                {
+                    "x": 3.2504504785577715,
+                    "y": 6.188501232669928,
+                    "heading": -3.110871206110287e-17,
+                    "angularVelocity": -1.2434001017092495e-16,
+                    "velocityX": 2.203854157207935,
+                    "velocityY": -1.0324360013948046,
+                    "timestamp": 0.7213963857851773
+                },
+                {
+                    "x": 3.3035723121371006,
+                    "y": 6.159082982949556,
+                    "heading": -3.205106262321403e-17,
+                    "angularVelocity": -3.942740156809933e-17,
+                    "velocityX": 2.222586681909921,
+                    "velocityY": -1.2308424922109886,
+                    "timestamp": 0.7452972909834756
+                },
+                {
+                    "x": 3.3570997558109252,
+                    "y": 6.124918851159325,
+                    "heading": -3.3149188675890806e-17,
+                    "angularVelocity": -4.594495761931231e-17,
+                    "velocityX": 2.239557172823534,
+                    "velocityY": -1.429407443223672,
+                    "timestamp": 0.769198196181774
+                },
+                {
+                    "x": 3.4109812853154073,
+                    "y": 6.086004715644374,
+                    "heading": -3.4399834546372206e-17,
+                    "angularVelocity": -5.232629825878709e-17,
+                    "velocityX": 2.2543719184459183,
+                    "velocityY": -1.6281448418833084,
+                    "timestamp": 0.7930991013800723
+                },
+                {
+                    "x": 3.4651524420580837,
+                    "y": 6.042336211273467,
+                    "heading": -3.565040387084894e-17,
+                    "angularVelocity": -5.2323095531564346e-17,
+                    "velocityX": 2.2664897539752324,
+                    "velocityY": -1.827064875100061,
+                    "timestamp": 0.8170000065783707
+                },
+                {
+                    "x": 3.5195303175635244,
+                    "y": 5.993909012954667,
+                    "heading": -3.501524266469143e-17,
+                    "angularVelocity": 2.6574776159275344e-17,
+                    "velocityX": 2.2751387470175053,
+                    "velocityY": -2.0261658676529293,
+                    "timestamp": 0.840900911776669
+                },
+                {
+                    "x": 3.5740044311548194,
+                    "y": 5.940719606067823,
+                    "heading": -3.37448298204232e-17,
+                    "angularVelocity": 5.315333634409861e-17,
+                    "velocityX": 2.2791652926673844,
+                    "velocityY": -2.225413909872768,
+                    "timestamp": 0.8648018169749674
+                },
+                {
+                    "x": 3.628420546352513,
+                    "y": 5.8827673726499405,
+                    "heading": -3.046153632575651e-17,
+                    "angularVelocity": 1.3737109446355186e-16,
+                    "velocityX": 2.2767386735448056,
+                    "velocityY": -2.4246878073056584,
+                    "timestamp": 0.8887027221732657
+                },
+                {
+                    "x": 3.6825492209054214,
+                    "y": 5.820060642331504,
+                    "heading": -2.5159851350702206e-17,
+                    "angularVelocity": 2.2181942251709706e-16,
+                    "velocityX": 2.264712323814494,
+                    "velocityY": -2.6236131978340897,
+                    "timestamp": 0.9126036273715641
+                },
+                {
+                    "x": 3.736016878561411,
+                    "y": 5.75263682695494,
+                    "heading": -1.874886441281884e-17,
+                    "angularVelocity": 2.6823197281004355e-16,
+                    "velocityX": 2.237055760540655,
+                    "velocityY": -2.8209732985913787,
+                    "timestamp": 0.9365045325698624
+                },
+                {
+                    "x": 3.788130152189531,
+                    "y": 5.680646456241098,
+                    "heading": -1.0550078419468136e-17,
+                    "angularVelocity": 3.4303244721005534e-16,
+                    "velocityX": 2.1803891189790523,
+                    "velocityY": -3.012035323204781,
+                    "timestamp": 0.9604054377681608
+                },
+                {
+                    "x": 3.837400250784585,
+                    "y": 5.604834562813646,
+                    "heading": 5.433473713273816e-19,
+                    "angularVelocity": 4.641424958072882e-16,
+                    "velocityX": 2.061432325942265,
+                    "velocityY": -3.171925615304708,
+                    "timestamp": 0.9843063429664591
+                },
+                {
+                    "x": 3.882609579100313,
+                    "y": 5.526533156121729,
+                    "heading": 5.968487826268029e-18,
+                    "angularVelocity": 2.269847275628469e-16,
+                    "velocityX": 1.8915320545661611,
+                    "velocityY": -3.2760854052293937,
+                    "timestamp": 1.0082072481647575
+                },
+                {
+                    "x": 3.923632674014372,
+                    "y": 5.445959589727867,
+                    "heading": 1.2150851940113108e-17,
+                    "angularVelocity": 2.586665259140546e-16,
+                    "velocityX": 1.716382478977385,
+                    "velocityY": -3.3711512482631147,
+                    "timestamp": 1.0321081533630558
+                },
+                {
+                    "x": 3.960355768910532,
+                    "y": 5.363337507789521,
+                    "heading": 1.8690802571261126e-17,
+                    "angularVelocity": 2.736277382336804e-16,
+                    "velocityX": 1.5364729742024472,
+                    "velocityY": -3.456859949564964,
+                    "timestamp": 1.0560090585613542
+                },
+                {
+                    "x": 4.001130960542707,
+                    "y": 5.282638203170642,
+                    "heading": 2.7305426078750436e-17,
+                    "angularVelocity": 3.6043084711714437e-16,
+                    "velocityX": 1.7060103495610794,
+                    "velocityY": -3.376412062611935,
+                    "timestamp": 1.0799099637596525
+                },
+                {
+                    "x": 4.046099298269293,
+                    "y": 5.204198147271007,
+                    "heading": 3.276849439101543e-17,
+                    "angularVelocity": 2.285716070976872e-16,
+                    "velocityX": 1.8814491482016318,
+                    "velocityY": -3.281886407600119,
+                    "timestamp": 1.1038108689579509
+                },
+                {
+                    "x": 4.095136049561521,
+                    "y": 5.128235061004533,
+                    "heading": 2.8631224607755774e-17,
+                    "angularVelocity": -1.7310096621588863e-16,
+                    "velocityX": 2.051669210240635,
+                    "velocityY": -3.178251435928111,
+                    "timestamp": 1.1277117741562492
                 },
                 {
                     "x": 4.148105144500732,
                     "y": 5.054959774017334,
-                    "heading": -3.474586084587748e-13,
-                    "angularVelocity": -9.540235481877701e-17,
-                    "velocityX": 3.491827526585996,
-                    "velocityY": -1.4552566560457803,
-                    "timestamp": 0.7324324867734865
-                },
-                {
-                    "x": 4.301558125072517,
-                    "y": 4.97182210350292,
-                    "heading": -3.4745624581283064e-13,
-                    "angularVelocity": 5.1211249699831376e-17,
-                    "velocityX": 3.3261517465815813,
-                    "velocityY": -1.8020406443565171,
-                    "timestamp": 0.7785677783246081
-                },
-                {
-                    "x": 4.445898119107738,
-                    "y": 4.873711676741488,
-                    "heading": -3.4747329562610653e-13,
-                    "angularVelocity": -3.6956119081305154e-16,
-                    "velocityX": 3.1286242956827928,
-                    "velocityY": -2.1265808335182323,
-                    "timestamp": 0.8247030698757297
-                },
-                {
-                    "x": 4.587571576014754,
-                    "y": 4.771788561847154,
-                    "heading": -3.4746251546031876e-13,
-                    "angularVelocity": 2.3366419539300746e-16,
-                    "velocityX": 3.0708260887450245,
-                    "velocityY": -2.2092222996219535,
-                    "timestamp": 0.8708383614268513
-                },
-                {
-                    "x": 4.729219222551993,
-                    "y": 4.66982957987028,
-                    "heading": -3.474587781166732e-13,
-                    "angularVelocity": 8.100834549122579e-17,
-                    "velocityX": 3.070266639108493,
-                    "velocityY": -2.209999732353969,
-                    "timestamp": 0.916973652977973
-                },
-                {
-                    "x": 4.870877206231954,
-                    "y": 4.567884960544782,
-                    "heading": -3.47487867941798e-13,
-                    "angularVelocity": -6.305330291624225e-16,
-                    "velocityX": 3.070490700636254,
-                    "velocityY": -2.209688416384298,
-                    "timestamp": 0.9631089445290946
-                },
-                {
-                    "x": 5.012587244979431,
-                    "y": 4.466012713767847,
-                    "heading": -3.4750411523417436e-13,
-                    "angularVelocity": -3.5216624493767476e-16,
-                    "velocityX": 3.0716190140743462,
-                    "velocityY": -2.208119713797033,
-                    "timestamp": 1.009244236080216
-                },
-                {
-                    "x": 5.160453670184351,
-                    "y": 4.373302358522453,
-                    "heading": -3.47491376477911e-13,
-                    "angularVelocity": 2.7611738924244047e-16,
-                    "velocityX": 3.2050610331965634,
-                    "velocityY": -2.009532228521384,
-                    "timestamp": 1.0553795276313376
-                },
-                {
-                    "x": 5.316942928228011,
-                    "y": 4.29603139716118,
-                    "heading": -3.4750002331748596e-13,
-                    "angularVelocity": -1.8742353831509497e-16,
-                    "velocityX": 3.3919642161767567,
-                    "velocityY": -1.6748774910066955,
-                    "timestamp": 1.101514819182459
+                    "heading": 1.6930966409187752e-17,
+                    "angularVelocity": -4.895320115071494e-16,
+                    "velocityX": 2.2161961858659356,
+                    "velocityY": -3.065795474240693,
+                    "timestamp": 1.1516126793545476
+                },
+                {
+                    "x": 4.265239674165176,
+                    "y": 4.923669357008322,
+                    "heading": 4.8393962188151695e-17,
+                    "angularVelocity": 6.764650813120357e-16,
+                    "velocityX": 2.5184321191325005,
+                    "velocityY": -2.8227884986351293,
+                    "timestamp": 1.1981235734837035
+                },
+                {
+                    "x": 4.395200374134661,
+                    "y": 4.805061224730511,
+                    "heading": 5.597930320273315e-17,
+                    "angularVelocity": 1.6308740471674414e-16,
+                    "velocityX": 2.7941991312528214,
+                    "velocityY": -2.550115075157559,
+                    "timestamp": 1.2446344676128593
+                },
+                {
+                    "x": 4.536621337779305,
+                    "y": 4.700381789438767,
+                    "heading": 7.583650477943765e-17,
+                    "angularVelocity": 4.269365693457232e-16,
+                    "velocityX": 3.040598687523153,
+                    "velocityY": -2.250643365424577,
+                    "timestamp": 1.2911453617420152
+                },
+                {
+                    "x": 4.6880158468821485,
+                    "y": 4.610730375722861,
+                    "heading": 8.702958111273957e-17,
+                    "angularVelocity": 2.4065493779199536e-16,
+                    "velocityX": 3.2550332978427186,
+                    "velocityY": -1.9275358041271147,
+                    "timestamp": 1.337656255871171
+                },
+                {
+                    "x": 4.842298757317137,
+                    "y": 4.526146352730071,
+                    "heading": 1.0187276641692621e-16,
+                    "angularVelocity": 3.1913351876162675e-16,
+                    "velocityX": 3.317134906212745,
+                    "velocityY": -1.81858518475069,
+                    "timestamp": 1.384167150000327
+                },
+                {
+                    "x": 4.996581769126158,
+                    "y": 4.441562514645757,
+                    "heading": 8.403781588381205e-17,
+                    "angularVelocity": -3.8345748597281753e-16,
+                    "velocityX": 3.3171370857888243,
+                    "velocityY": -1.818581209155721,
+                    "timestamp": 1.4306780441294829
+                },
+                {
+                    "x": 5.15086732735743,
+                    "y": 4.356983321464328,
+                    "heading": 9.522247870814417e-17,
+                    "angularVelocity": 2.4047404444902996e-16,
+                    "velocityX": 3.317191834731107,
+                    "velocityY": -1.818481342167903,
+                    "timestamp": 1.4771889382586387
+                },
+                {
+                    "x": 5.312538078249567,
+                    "y": 4.287555361855474,
+                    "heading": 1.0159270601289723e-16,
+                    "angularVelocity": 1.369620477960438e-16,
+                    "velocityX": 3.4759759819537814,
+                    "velocityY": -1.4927246811478623,
+                    "timestamp": 1.5236998323877946
                 },
                 {
                     "x": 5.480467319488525,
                     "y": 4.235044002532959,
-                    "heading": -3.475163170309309e-13,
-                    "angularVelocity": -3.5317243928660403e-16,
-                    "velocityX": 3.5444534056932553,
-                    "velocityY": -1.321924985762245,
-                    "timestamp": 1.1476501107335806
-                },
-                {
-                    "x": 5.5914176787721175,
-                    "y": 4.201991970759951,
-                    "heading": -3.4752963658489967e-13,
-                    "angularVelocity": -4.352387405096259e-16,
-                    "velocityX": 3.625488497149835,
-                    "velocityY": -1.0800304007696078,
-                    "timestamp": 1.1782529824212593
-                },
-                {
-                    "x": 5.704342870003183,
-                    "y": 4.176490587918632,
-                    "heading": -3.4752519346531394e-13,
-                    "angularVelocity": 1.4518636145588672e-16,
-                    "velocityX": 3.6900194329281493,
-                    "velocityY": -0.8333003223399679,
-                    "timestamp": 1.208855854108938
-                },
-                {
-                    "x": 5.818728492967466,
-                    "y": 4.15864825583045,
-                    "heading": -3.47528320752536e-13,
-                    "angularVelocity": -1.0218933861720526e-16,
-                    "velocityX": 3.737741481635173,
-                    "velocityY": -0.5830280331479286,
-                    "timestamp": 1.2394587257966168
-                },
-                {
-                    "x": 5.934052260797228,
-                    "y": 4.148506842880417,
-                    "heading": -3.475166383708343e-13,
-                    "angularVelocity": 3.817413550372978e-16,
-                    "velocityX": 3.768396933681978,
-                    "velocityY": -0.3313876244719343,
-                    "timestamp": 1.2700615974842955
-                },
-                {
-                    "x": 6.049784939896788,
-                    "y": 4.145614314337883,
-                    "heading": -3.4752553093783573e-13,
-                    "angularVelocity": -2.9057949488904467e-16,
-                    "velocityX": 3.7817587931181613,
-                    "velocityY": -0.0945182064161531,
-                    "timestamp": 1.3006644691719742
-                },
-                {
-                    "x": 6.1655453259831,
-                    "y": 4.144216905783092,
-                    "heading": -3.4753489515012823e-13,
-                    "angularVelocity": -3.0599129336581387e-16,
-                    "velocityX": 3.7826641652362696,
-                    "velocityY": -0.04566266099543126,
-                    "timestamp": 1.331267340859653
-                },
-                {
-                    "x": 6.281306292241875,
-                    "y": 4.142868414150292,
-                    "heading": -3.4754223223995867e-13,
-                    "angularVelocity": -2.397516775868154e-16,
-                    "velocityX": 3.7826831233414593,
-                    "velocityY": -0.04406421878574171,
-                    "timestamp": 1.3618702125473316
-                },
-                {
-                    "x": 6.39706739301833,
-                    "y": 4.141531519398385,
-                    "heading": -3.475633687164305e-13,
-                    "angularVelocity": -6.906697084916283e-16,
-                    "velocityX": 3.7826875189315166,
-                    "velocityY": -0.04368527131792852,
-                    "timestamp": 1.3924730842350104
-                },
-                {
-                    "x": 6.512828617962261,
-                    "y": 4.14020542040497,
-                    "heading": -3.4758398313992887e-13,
-                    "angularVelocity": -6.73610754719036e-16,
-                    "velocityX": 3.7826915763114313,
-                    "velocityY": -0.043332501854336236,
-                    "timestamp": 1.423075955922689
-                },
-                {
-                    "x": 6.628589931637748,
-                    "y": 4.138887090943481,
-                    "heading": -3.4757335612235213e-13,
-                    "angularVelocity": 3.472555676974266e-16,
-                    "velocityX": 3.7826944757635745,
-                    "velocityY": -0.04307861939704374,
-                    "timestamp": 1.4536788276103678
-                },
-                {
-                    "x": 6.744351296535766,
-                    "y": 4.137573267392239,
-                    "heading": -3.475819448429605e-13,
-                    "angularVelocity": -2.806508055564177e-16,
-                    "velocityX": 3.782696149545524,
-                    "velocityY": -0.042931381231176444,
-                    "timestamp": 1.4842816992980465
-                },
-                {
-                    "x": 6.860112695906494,
-                    "y": 4.136261713498386,
-                    "heading": -3.475979530506806e-13,
-                    "angularVelocity": -5.230943272138926e-16,
-                    "velocityX": 3.7826972759991557,
-                    "velocityY": -0.04285721636551622,
-                    "timestamp": 1.5148845709857253
-                },
-                {
-                    "x": 6.9694302247553,
-                    "y": 4.135029589899985,
-                    "heading": -2.392571520797574e-13,
-                    "angularVelocity": 3.5402168172320343e-12,
-                    "velocityX": 3.5721330326269505,
-                    "velocityY": -0.04026169865772549,
-                    "timestamp": 1.545487442673404
-                },
-                {
-                    "x": 7.070940187048315,
-                    "y": 4.133884255746116,
-                    "heading": -1.6833543298116243e-13,
-                    "angularVelocity": 2.317485846705618e-12,
-                    "velocityX": 3.3170077412667367,
-                    "velocityY": -0.03742570844047695,
-                    "timestamp": 1.5760903143610827
-                },
-                {
-                    "x": 7.164641690009617,
-                    "y": 4.132826276383854,
-                    "heading": -1.1981581485168293e-13,
-                    "angularVelocity": 1.5854597325485145e-12,
-                    "velocityX": 3.061853276960511,
-                    "velocityY": -0.034571244567840914,
-                    "timestamp": 1.6066931860487614
-                },
-                {
-                    "x": 7.250534738513507,
-                    "y": 4.131855992058627,
-                    "heading": -8.523066967233492e-14,
-                    "angularVelocity": 1.1301275060622065e-12,
-                    "velocityX": 2.8066989719298543,
-                    "velocityY": -0.03170566260882202,
-                    "timestamp": 1.6372960577364402
-                },
-                {
-                    "x": 7.328619329487729,
-                    "y": 4.1309736027819035,
-                    "heading": -6.003549662940244e-14,
-                    "angularVelocity": 8.232944437274219e-13,
-                    "velocityX": 2.5515445665080714,
-                    "velocityY": -0.028833544941282405,
-                    "timestamp": 1.6678989294241189
-                },
-                {
-                    "x": 7.398895462248961,
-                    "y": 4.130179235828021,
-                    "heading": -4.153185117983844e-14,
-                    "angularVelocity": 6.046375734752488e-13,
-                    "velocityX": 2.2963901387575416,
-                    "velocityY": -0.02595726837221309,
-                    "timestamp": 1.6985018011117976
-                },
-                {
-                    "x": 7.461363136410412,
-                    "y": 4.129472977946104,
-                    "heading": -2.797073854746349e-14,
-                    "angularVelocity": 4.431320366500053e-13,
-                    "velocityX": 2.041235698368024,
-                    "velocityY": -0.0230781571305896,
-                    "timestamp": 1.7291046727994763
-                },
-                {
-                    "x": 7.516022351719397,
-                    "y": 4.1288548915008905,
-                    "heading": -1.8141238812912564e-14,
-                    "angularVelocity": 3.211953369611228e-13,
-                    "velocityX": 1.7860812497215437,
-                    "velocityY": -0.02019700801679021,
-                    "timestamp": 1.759707544487155
-                },
-                {
-                    "x": 7.5628731079961895,
-                    "y": 4.128325023227497,
-                    "heading": -1.1163529343344889e-14,
-                    "angularVelocity": 2.2800831618830555e-13,
-                    "velocityX": 1.5309267952022354,
-                    "velocityY": -0.017314331754962387,
-                    "timestamp": 1.7903104161748338
-                },
-                {
-                    "x": 7.601915405105201,
-                    "y": 4.127883409320133,
-                    "heading": -6.372564950074351e-15,
-                    "angularVelocity": 1.5655276164796086e-13,
-                    "velocityX": 1.2757723362523365,
-                    "velocityY": -0.014430472786738478,
-                    "timestamp": 1.8209132878625125
-                },
-                {
-                    "x": 7.63314924293974,
-                    "y": 4.127530078564582,
-                    "heading": -3.2435183420078772e-15,
-                    "angularVelocity": 1.0224681844586284e-13,
-                    "velocityX": 1.020617873816125,
-                    "velocityY": -0.011545673194894595,
-                    "timestamp": 1.8515161595501912
-                },
-                {
-                    "x": 7.656574621413203,
-                    "y": 4.127265054360105,
-                    "heading": -1.3581702257498648e-15,
-                    "angularVelocity": 6.160689169092944e-14,
-                    "velocityX": 0.7654634085499833,
-                    "velocityY": -0.008660108992989924,
-                    "timestamp": 1.88211903123787
-                },
+                    "heading": 8.575039074051001e-17,
+                    "angularVelocity": -3.4061515197699265e-16,
+                    "velocityX": 3.6105356472537267,
+                    "velocityY": -1.1290120369797083,
+                    "timestamp": 1.5702107265169505
+                },
+                {
+                    "x": 5.592904232264291,
+                    "y": 4.2080430634395825,
+                    "heading": 8.77612564470466e-17,
+                    "angularVelocity": 6.578528400068062e-17,
+                    "velocityX": 3.6783631124997167,
+                    "velocityY": -0.8833332035895192,
+                    "timestamp": 1.6007778318670682
+                },
+                {
+                    "x": 5.706904061688869,
+                    "y": 4.1886742955111504,
+                    "heading": 8.48190648175679e-17,
+                    "angularVelocity": -9.625352468867059e-17,
+                    "velocityX": 3.7294937848649896,
+                    "velocityY": -0.6336474359145746,
+                    "timestamp": 1.631344937217186
+                },
+                {
+                    "x": 5.821949322476148,
+                    "y": 4.1770253308728975,
+                    "heading": 6.614143850361712e-17,
+                    "angularVelocity": -6.110368020256626e-16,
+                    "velocityX": 3.7636949743701025,
+                    "velocityY": -0.3810947914375838,
+                    "timestamp": 1.6619120425673037
+                },
+                {
+                    "x": 5.937517692705647,
+                    "y": 4.173144318562267,
+                    "heading": 5.289613863539961e-17,
+                    "angularVelocity": -4.3331874941909564e-16,
+                    "velocityX": 3.7808084509727644,
+                    "velocityY": -0.12696695569208769,
+                    "timestamp": 1.6924791479174215
+                },
+                {
+                    "x": 6.05311068319219,
+                    "y": 4.170083108620971,
+                    "heading": 6.789574595295852e-17,
+                    "angularVelocity": 4.90710754117939e-16,
+                    "velocityX": 3.7816139003851448,
+                    "velocityY": -0.1001471976568426,
+                    "timestamp": 1.7230462532675392
+                },
+                {
+                    "x": 6.168703676027133,
+                    "y": 4.167021987357848,
+                    "heading": 5.651709601242885e-17,
+                    "angularVelocity": -3.7225147131887107e-16,
+                    "velocityX": 3.7816139772128325,
+                    "velocityY": -0.1001442965586977,
+                    "timestamp": 1.753613358617657
+                },
+                {
+                    "x": 6.284296668862326,
+                    "y": 4.163960866104177,
+                    "heading": 5.090280597716889e-17,
+                    "angularVelocity": -1.8367097476040197e-16,
+                    "velocityX": 3.7816139772210215,
+                    "velocityY": -0.10014429624946593,
+                    "timestamp": 1.7841804639677747
+                },
+                {
+                    "x": 6.399889661697518,
+                    "y": 4.160899744850507,
+                    "heading": 6.173261084277577e-17,
+                    "angularVelocity": 3.542960558928146e-16,
+                    "velocityX": 3.7816139772210224,
+                    "velocityY": -0.10014429624943298,
+                    "timestamp": 1.8147475693178925
+                },
+                {
+                    "x": 6.515482654532711,
+                    "y": 4.157838623596838,
+                    "heading": 8.001124700396023e-17,
+                    "angularVelocity": 5.979838768447224e-16,
+                    "velocityX": 3.7816139772210224,
+                    "velocityY": -0.10014429624943295,
+                    "timestamp": 1.8453146746680102
+                },
+                {
+                    "x": 6.631075647367904,
+                    "y": 4.154777502343168,
+                    "heading": 7.855690079559015e-17,
+                    "angularVelocity": -4.757880053117145e-17,
+                    "velocityX": 3.7816139772210224,
+                    "velocityY": -0.10014429624943294,
+                    "timestamp": 1.875881780018128
+                },
+                {
+                    "x": 6.746668640203097,
+                    "y": 4.151716381089498,
+                    "heading": 7.690538849750639e-17,
+                    "angularVelocity": -5.4029070766342576e-17,
+                    "velocityX": 3.7816139772210224,
+                    "velocityY": -0.10014429624943297,
+                    "timestamp": 1.9064488853682457
+                },
+                {
+                    "x": 6.862261633037354,
+                    "y": 4.148655259835854,
+                    "heading": 7.874213255028351e-17,
+                    "angularVelocity": 6.00889103413278e-17,
+                    "velocityX": 3.781613977190394,
+                    "velocityY": -0.10014429624862185,
+                    "timestamp": 1.9370159907183635
+                },
+                {
+                    "x": 6.971293364430436,
+                    "y": 4.1457678932048365,
+                    "heading": 7.034301862221845e-17,
+                    "angularVelocity": -2.747762270062427e-16,
+                    "velocityX": 3.56696292122612,
+                    "velocityY": -0.09445992997849091,
+                    "timestamp": 1.9675830960684813
+                },
+                {
+                    "x": 7.072537143209538,
+                    "y": 4.14308676630068,
+                    "heading": 6.196577755471392e-17,
+                    "angularVelocity": -2.740606584102676e-16,
+                    "velocityX": 3.3121807779784156,
+                    "velocityY": -0.08771281655386487,
+                    "timestamp": 1.998150201418599
+                },
+                {
+                    "x": 7.165992951009495,
+                    "y": 4.140611879609727,
+                    "heading": 5.371713190017333e-17,
+                    "angularVelocity": -2.6985367144933565e-16,
+                    "velocityX": 3.057398033916134,
+                    "velocityY": -0.08096568721853159,
+                    "timestamp": 2.0287173067687165
+                },
+                {
+                    "x": 7.251660781708473,
+                    "y": 4.138343233294095,
+                    "heading": 4.5597032779539255e-17,
+                    "angularVelocity": -2.6564828431304464e-16,
+                    "velocityX": 2.802615089578594,
+                    "velocityY": -0.07421855257953036,
+                    "timestamp": 2.059284412118834
+                },
+                {
+                    "x": 7.329540632245536,
+                    "y": 4.136280827434844,
+                    "heading": 3.7578034898451724e-17,
+                    "angularVelocity": -2.623407674507654e-16,
+                    "velocityX": 2.5478320451027607,
+                    "velocityY": -0.06747141528867756,
+                    "timestamp": 2.0898515174689516
+                },
+                {
+                    "x": 7.399632500784112,
+                    "y": 4.134424662080611,
+                    "heading": 2.960093117982924e-17,
+                    "angularVelocity": -2.609702038052913e-16,
+                    "velocityX": 2.293048940543746,
+                    "velocityY": -0.06072427640670841,
+                    "timestamp": 2.120418622819069
+                },
+                {
+                    "x": 7.461936386099821,
+                    "y": 4.132774737263818,
+                    "heading": 2.4038424741612248e-17,
+                    "angularVelocity": -1.8197687815335696e-16,
+                    "velocityX": 2.0382657959291963,
+                    "velocityY": -0.05397713646399285,
+                    "timestamp": 2.1509857281691867
+                },
+                {
+                    "x": 7.516452287318104,
+                    "y": 4.131331053007627,
+                    "heading": 1.8665464618967105e-17,
+                    "angularVelocity": -1.757758880397929e-16,
+                    "velocityX": 1.7834826227035099,
+                    "velocityY": -0.04722999576360025,
+                    "timestamp": 2.181552833519304
+                },
+                {
+                    "x": 7.5631802037830385,
+                    "y": 4.130093609329407,
+                    "heading": 1.3420986699374698e-17,
+                    "angularVelocity": -1.7157260548142223e-16,
+                    "velocityX": 1.5286994280194506,
+                    "velocityY": -0.04048285449494931,
+                    "timestamp": 2.2121199388694217
+                },
+                {
+                    "x": 7.6021201349844665,
+                    "y": 4.129062406242667,
+                    "heading": 8.352476313992174e-18,
+                    "angularVelocity": -1.6581584391601633e-16,
+                    "velocityX": 1.2739162166455336,
+                    "velocityY": -0.033735712784319354,
+                    "timestamp": 2.2426870442195392
+                },
+                {
+                    "x": 7.633272080514257,
+                    "y": 4.128237443758217,
+                    "heading": 5.582680805550875e-18,
+                    "angularVelocity": -9.061360111293989e-17,
+                    "velocityX": 1.019132991919723,
+                    "velocityY": -0.026988570720105975,
+                    "timestamp": 2.2732541495696568
+                },
+                {
+                    "x": 7.656636040038488,
+                    "y": 4.127618721884898,
+                    "heading": 2.786250494304085e-18,
+                    "angularVelocity": -9.148495626615499e-17,
+                    "velocityX": 0.7643497562696309,
+                    "velocityY": -0.020241428366596953,
+                    "timestamp": 2.3038212549197743
+                },
                 {
-                    "x": 7.672191540453606,
-                    "y": 4.127088356077157,
-                    "heading": -3.7222995775515084e-16,
-                    "angularVelocity": 3.2217238765593925e-14,
-                    "velocityX": 0.5103089409316736,
-                    "velocityY": -0.0057739118290542285,
-                    "timestamp": 1.9127219029255487
+                    "x": 7.672212013278888,
+                    "y": 4.1272062406300805,
+                    "heading": 1.8952405540316967e-19,
+                    "angularVelocity": -8.495166341555795e-17,
+                    "velocityX": 0.5095665115159678,
+                    "velocityY": -0.013494285772008131,
+                    "timestamp": 2.334388360269892
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 2.9643987227812266e-34,
-                    "angularVelocity": 1.2163231774130929e-14,
-                    "velocityX": 0.25515447132169783,
-                    "velocityY": -0.0028871825495910644,
-                    "timestamp": 1.9433247746132274
+                    "heading": 6.998640646947103e-35,
+                    "angularVelocity": -6.20026191774287e-18,
+                    "velocityX": 0.25478325905928073,
+                    "velocityY": -0.006747142973428655,
+                    "timestamp": 2.3649554656200094
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": -1.7511816257564745e-34,
-                    "angularVelocity": -1.536305146096975e-32,
-                    "velocityX": 2.290424541792184e-34,
-                    "velocityY": 7.21466542895957e-33,
-                    "timestamp": 1.9739276463009061
+                    "heading": 3.616659043590546e-35,
+                    "angularVelocity": -1.1064131915191804e-33,
+                    "velocityX": -5.781348077610764e-36,
+                    "velocityY": -1.0341970128847813e-32,
+                    "timestamp": 2.395522570970127
                 }
             ],
             "constraints": [
@@ -16619,25 +18705,25 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 26
+                    "controlIntervalCount": 10
                 },
                 {
                     "x": 4.032804489135742,
                     "y": 5.285560607910156,
-                    "heading": 3.14,
+                    "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 10
+                    "headingConstrained": true,
+                    "controlIntervalCount": 11
                 },
                 {
-                    "x": 5.390789031982422,
-                    "y": 4.09412145614624,
-                    "heading": 3.14,
+                    "x": 5.406457424163818,
+                    "y": 4.101301193237305,
+                    "heading": -0.2,
                     "isInitialGuess": false,
                     "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 28
+                    "headingConstrained": true,
+                    "controlIntervalCount": 13
                 },
                 {
                     "x": 7.68,
@@ -16656,584 +18742,584 @@
                     "heading": 0,
                     "angularVelocity": 0,
                     "velocityX": 0,
-                    "velocityY": 0,
+                    "velocityY": -8.663522895809945e-37,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.253608767824088,
-                    "y": 5.559959646221142,
-                    "heading": 0.04333845773860565,
-                    "angularVelocity": 1.2657316926377489,
-                    "velocityX": 0.10539673178657698,
-                    "velocityY": -0.0011785619397610312,
-                    "timestamp": 0.03423984560921402
-                },
-                {
-                    "x": 2.260842860117559,
-                    "y": 5.560038303784881,
-                    "heading": 0.12991520296053788,
-                    "angularVelocity": 2.5285378389274697,
-                    "velocityX": 0.211277012637124,
-                    "velocityY": 0.002297252290136546,
-                    "timestamp": 0.06847969121842803
-                },
-                {
-                    "x": 2.271629728646886,
-                    "y": 5.560441749821537,
-                    "heading": 0.2597959154554935,
-                    "angularVelocity": 3.7932622120236568,
-                    "velocityX": 0.3150384687021929,
-                    "velocityY": 0.011782939714764799,
-                    "timestamp": 0.10271953682764205
-                },
-                {
-                    "x": 2.285721166472015,
-                    "y": 5.561272024294721,
-                    "heading": 0.433416913958427,
-                    "angularVelocity": 5.070729596286839,
-                    "velocityX": 0.41155085761651194,
-                    "velocityY": 0.024248779701277118,
-                    "timestamp": 0.13695938243685607
-                },
-                {
-                    "x": 2.302746549134424,
-                    "y": 5.561830584813036,
-                    "heading": 0.6513804515506975,
-                    "angularVelocity": 6.36578622695705,
-                    "velocityX": 0.497238885266109,
-                    "velocityY": 0.01631317280720512,
-                    "timestamp": 0.17119922804607007
-                },
-                {
-                    "x": 2.325552284167229,
-                    "y": 5.559809655953846,
-                    "heading": 0.9008347442106217,
-                    "angularVelocity": 7.285497005652254,
-                    "velocityX": 0.6660583488924382,
-                    "velocityY": -0.05902272113767896,
-                    "timestamp": 0.20543907365528408
-                },
-                {
-                    "x": 2.357593597831257,
-                    "y": 5.5547755319598515,
-                    "heading": 1.1463891087836293,
-                    "angularVelocity": 7.171596723173552,
-                    "velocityX": 0.9357902494573596,
-                    "velocityY": -0.14702531230571617,
-                    "timestamp": 0.23967891926449808
-                },
-                {
-                    "x": 2.398826095012215,
-                    "y": 5.54977146589545,
-                    "heading": 1.3795454342192353,
-                    "angularVelocity": 6.809502825937482,
-                    "velocityX": 1.2042255578939407,
-                    "velocityY": -0.14614744825411982,
-                    "timestamp": 0.2739187648737121
-                },
-                {
-                    "x": 2.4467424620473803,
-                    "y": 5.551292722520974,
-                    "heading": 1.6025344347532522,
-                    "angularVelocity": 6.512558586829906,
-                    "velocityX": 1.399432917485785,
-                    "velocityY": 0.04442942421196648,
-                    "timestamp": 0.3081586104829261
-                },
-                {
-                    "x": 2.499282041125752,
-                    "y": 5.560003512593039,
-                    "heading": 1.8091707816342142,
-                    "angularVelocity": 6.034967249541443,
-                    "velocityX": 1.5344572425359657,
-                    "velocityY": 0.2544050627880495,
-                    "timestamp": 0.3423984560921401
-                },
-                {
-                    "x": 2.558927262082263,
-                    "y": 5.571143049666431,
-                    "heading": 1.9899644077603311,
-                    "angularVelocity": 5.2802114877955235,
-                    "velocityX": 1.7419827658469296,
-                    "velocityY": 0.3253384142127747,
-                    "timestamp": 0.3766383017013541
-                },
-                {
-                    "x": 2.624771333399717,
-                    "y": 5.5772713811756995,
-                    "heading": 2.148730759711285,
-                    "angularVelocity": 4.63688866366352,
-                    "velocityX": 1.9230247726273466,
-                    "velocityY": 0.17898245159199333,
-                    "timestamp": 0.4108781473105681
-                },
-                {
-                    "x": 2.6962684360037885,
-                    "y": 5.576686266483743,
-                    "heading": 2.2908744479705683,
-                    "angularVelocity": 4.151411483614645,
-                    "velocityX": 2.0881257298902987,
-                    "velocityY": -0.0170887070763856,
-                    "timestamp": 0.4451179929197821
-                },
-                {
-                    "x": 2.773687793639176,
-                    "y": 5.569334398339137,
-                    "heading": 2.4184902740666434,
-                    "angularVelocity": 3.727114530613817,
-                    "velocityX": 2.2610895656186547,
-                    "velocityY": -0.2147167434256298,
-                    "timestamp": 0.4793578385289961
-                },
-                {
-                    "x": 2.8582271385194487,
-                    "y": 5.556743437974915,
-                    "heading": 2.5300657438997165,
-                    "angularVelocity": 3.258644069441941,
-                    "velocityX": 2.4690340559690878,
-                    "velocityY": -0.36772830426646747,
-                    "timestamp": 0.5135976841382102
-                },
-                {
-                    "x": 2.9508927177061977,
-                    "y": 5.541492370817128,
-                    "heading": 2.6233336392816216,
-                    "angularVelocity": 2.7239578252305745,
-                    "velocityX": 2.706366735538439,
-                    "velocityY": -0.445418689437171,
-                    "timestamp": 0.5478375297474242
-                },
-                {
-                    "x": 3.0518533392296137,
-                    "y": 5.525810656425357,
-                    "heading": 2.697205719709221,
-                    "angularVelocity": 2.1574887127329996,
-                    "velocityX": 2.948629578406965,
-                    "velocityY": -0.45799606022611716,
-                    "timestamp": 0.5820773753566382
-                },
-                {
-                    "x": 3.1608544462331407,
-                    "y": 5.511189976920693,
-                    "heading": 2.751249379299885,
-                    "angularVelocity": 1.5783850256649679,
-                    "velocityX": 3.183457900119567,
-                    "velocityY": -0.42700775206563796,
-                    "timestamp": 0.6163172209658522
-                },
-                {
-                    "x": 3.2775207255393237,
-                    "y": 5.498694402974223,
-                    "heading": 2.785399702937351,
-                    "angularVelocity": 0.9973854446433661,
-                    "velocityX": 3.40732492306531,
-                    "velocityY": -0.36494247342945346,
-                    "timestamp": 0.6505570665750662
-                },
-                {
-                    "x": 3.401135401206032,
-                    "y": 5.488837187261409,
-                    "heading": 2.800896234569334,
-                    "angularVelocity": 0.4525876608454414,
-                    "velocityX": 3.610257974803583,
-                    "velocityY": -0.28788727102673717,
-                    "timestamp": 0.6847969121842802
-                },
-                {
-                    "x": 3.5179017921083777,
-                    "y": 5.474779629986763,
-                    "heading": 2.835487088185811,
-                    "angularVelocity": 1.0102514483058458,
-                    "velocityX": 3.410248756230483,
-                    "velocityY": -0.4105613510962388,
-                    "timestamp": 0.7190367577934942
-                },
-                {
-                    "x": 3.6287098119916026,
-                    "y": 5.4546983370085105,
-                    "heading": 2.8876881760682602,
-                    "angularVelocity": 1.5245713569573334,
-                    "velocityX": 3.2362301263825226,
-                    "velocityY": -0.5864890048700768,
-                    "timestamp": 0.7532766034027082
-                },
-                {
-                    "x": 3.7356372636451365,
-                    "y": 5.425785607175325,
-                    "heading": 2.9455736884009496,
-                    "angularVelocity": 1.6905891747687094,
-                    "velocityX": 3.1228952628442794,
-                    "velocityY": -0.8444176461299453,
-                    "timestamp": 0.7875164490119222
-                },
-                {
-                    "x": 3.8388362586877394,
-                    "y": 5.387864157402447,
-                    "heading": 3.0009390226743027,
-                    "angularVelocity": 1.6169855117119507,
-                    "velocityX": 3.0140029315678847,
-                    "velocityY": -1.1075239709221774,
-                    "timestamp": 0.8217562946211362
-                },
-                {
-                    "x": 3.9379980862804507,
-                    "y": 5.341070528950818,
-                    "heading": 3.053673275383994,
-                    "angularVelocity": 1.5401428298350917,
-                    "velocityX": 2.896094472050628,
-                    "velocityY": -1.3666425072616961,
-                    "timestamp": 0.8559961402303502
+                    "x": 2.2925575950089905,
+                    "y": 5.560958672147739,
+                    "heading": -0.030620645935394094,
+                    "angularVelocity": -0.4244497974010598,
+                    "velocityX": 0.5899144850684857,
+                    "velocityY": 0.0132886876305762,
+                    "timestamp": 0.07214197326253131
+                },
+                {
+                    "x": 2.3778169001331375,
+                    "y": 5.562706698566846,
+                    "heading": -0.08906407022187784,
+                    "angularVelocity": -0.8101167966920272,
+                    "velocityX": 1.181826629746885,
+                    "velocityY": 0.02423036604148944,
+                    "timestamp": 0.14428394652506263
+                },
+                {
+                    "x": 2.5059697216292194,
+                    "y": 5.56499180242334,
+                    "heading": -0.17113009935557016,
+                    "angularVelocity": -1.1375628558848325,
+                    "velocityX": 1.7763975075885672,
+                    "velocityY": 0.031675094998839956,
+                    "timestamp": 0.21642591978759396
+                },
+                {
+                    "x": 2.6772662600517445,
+                    "y": 5.56737716078426,
+                    "heading": -0.269909808145774,
+                    "angularVelocity": -1.3692404618700307,
+                    "velocityX": 2.3744365544197357,
+                    "velocityY": 0.033064778422963216,
+                    "timestamp": 0.28856789305012526
+                },
+                {
+                    "x": 2.891943869686031,
+                    "y": 5.568884231737924,
+                    "heading": -0.3711645487758272,
+                    "angularVelocity": -1.4035482542399815,
+                    "velocityX": 2.975765700961003,
+                    "velocityY": 0.020890348371527884,
+                    "timestamp": 0.36070986631265656
+                },
+                {
+                    "x": 3.1471065368375797,
+                    "y": 5.565499357743455,
+                    "heading": -0.4176508519135821,
+                    "angularVelocity": -0.644372492676169,
+                    "velocityX": 3.5369515915927057,
+                    "velocityY": -0.04691962031799383,
+                    "timestamp": 0.43285183957518786
+                },
+                {
+                    "x": 3.4167471065440913,
+                    "y": 5.523390468620082,
+                    "heading": -0.41765089729124494,
+                    "angularVelocity": -6.290050140591191e-7,
+                    "velocityX": 3.7376378481534562,
+                    "velocityY": -0.5836947233219467,
+                    "timestamp": 0.5049938128377192
+                },
+                {
+                    "x": 3.6595394652992312,
+                    "y": 5.451715121543425,
+                    "heading": -0.35516305517194063,
+                    "angularVelocity": 0.8661787208384958,
+                    "velocityX": 3.3654798694179893,
+                    "velocityY": -0.9935318350085096,
+                    "timestamp": 0.5771357861002505
+                },
+                {
+                    "x": 3.865437953511339,
+                    "y": 5.371729850961066,
+                    "heading": -0.21394633729907842,
+                    "angularVelocity": 1.957483438371744,
+                    "velocityX": 2.8540734180200986,
+                    "velocityY": -1.1087203047702217,
+                    "timestamp": 0.6492777593627819
                 },
                 {
                     "x": 4.032804489135742,
                     "y": 5.285560607910156,
-                    "heading": 3.1036105469083837,
-                    "angularVelocity": 1.4584549268806075,
-                    "velocityX": 2.76889107320562,
-                    "velocityY": -1.62120827512502,
-                    "timestamp": 0.8902359858395642
-                },
-                {
-                    "x": 4.173725003261308,
-                    "y": 5.173637687034127,
-                    "heading": 3.175484729095798,
-                    "angularVelocity": 1.2994086167535415,
-                    "velocityX": 2.547692714675012,
-                    "velocityY": -2.023447131813128,
-                    "timestamp": 0.9455489807364162
-                },
-                {
-                    "x": 4.301872286760783,
-                    "y": 5.039921697201636,
-                    "heading": 3.2346580028937466,
-                    "angularVelocity": 1.0697897285853943,
-                    "velocityX": 2.3167663175433564,
-                    "velocityY": -2.4174425934058767,
-                    "timestamp": 1.0008619756332682
-                },
-                {
-                    "x": 4.416442024233276,
-                    "y": 4.885133369968262,
-                    "heading": 3.2770092965554323,
-                    "angularVelocity": 0.765666255111711,
-                    "velocityX": 2.0712987551323803,
-                    "velocityY": -2.7984079965661373,
-                    "timestamp": 1.0561749705301202
-                },
-                {
-                    "x": 4.517908742039174,
-                    "y": 4.711097539386313,
-                    "heading": 3.282986865137276,
-                    "angularVelocity": 0.10806806959180061,
-                    "velocityX": 1.834410123608644,
-                    "velocityY": -3.146382344808711,
-                    "timestamp": 1.1114879654269723
-                },
-                {
-                    "x": 4.627451956204509,
-                    "y": 4.5560902871099245,
-                    "heading": 3.2338916761611958,
-                    "angularVelocity": -0.8875886953442504,
-                    "velocityX": 1.9804245705663106,
-                    "velocityY": -2.802365928032756,
-                    "timestamp": 1.1668009603238243
-                },
-                {
-                    "x": 4.751713919325069,
-                    "y": 4.421499213962681,
-                    "heading": 3.169974564154514,
-                    "angularVelocity": -1.1555532678328913,
-                    "velocityX": 2.246523865726009,
-                    "velocityY": -2.433263167149585,
-                    "timestamp": 1.2221139552206763
-                },
-                {
-                    "x": 4.891049798654671,
-                    "y": 4.307366204928411,
-                    "heading": 3.098048256171184,
-                    "angularVelocity": -1.3003509955925967,
-                    "velocityX": 2.5190442063286667,
-                    "velocityY": -2.063403170396147,
-                    "timestamp": 1.2774269501175284
-                },
-                {
-                    "x": 5.0446094980944505,
-                    "y": 4.214363163694223,
-                    "heading": 3.021068837736617,
-                    "angularVelocity": -1.3917058473893738,
-                    "velocityX": 2.7761957154216415,
-                    "velocityY": -1.6813958710357464,
-                    "timestamp": 1.3327399450143804
-                },
-                {
-                    "x": 5.211468562298102,
-                    "y": 4.143120621239898,
-                    "heading": 2.9418200950482523,
-                    "angularVelocity": -1.432732811451417,
-                    "velocityX": 3.016634057056783,
-                    "velocityY": -1.2879892435254758,
-                    "timestamp": 1.3880529399112325
+                    "heading": 0,
+                    "angularVelocity": 2.9656291285588754,
+                    "velocityX": 2.319960600680286,
+                    "velocityY": -1.1944397852458482,
+                    "timestamp": 0.7214197326253132
+                },
+                {
+                    "x": 4.094074407124677,
+                    "y": 5.249842876180867,
+                    "heading": 0.09747269322901686,
+                    "angularVelocity": 3.3393263465697696,
+                    "velocityX": 2.099051997177287,
+                    "velocityY": -1.2236571972325518,
+                    "timestamp": 0.7506090609846522
+                },
+                {
+                    "x": 4.1489483150763835,
+                    "y": 5.213294373235887,
+                    "heading": 0.20635910635937496,
+                    "angularVelocity": 3.7303500714335676,
+                    "velocityX": 1.8799304758291948,
+                    "velocityY": -1.2521186679955063,
+                    "timestamp": 0.7797983893439911
+                },
+                {
+                    "x": 4.197569720233833,
+                    "y": 5.1758877650462445,
+                    "heading": 0.3277826636682362,
+                    "angularVelocity": 4.159861296363549,
+                    "velocityX": 1.6657253828827165,
+                    "velocityY": -1.2815165778788733,
+                    "timestamp": 0.8089877177033301
+                },
+                {
+                    "x": 4.240207786511328,
+                    "y": 5.137521274665071,
+                    "heading": 0.4634475312713068,
+                    "angularVelocity": 4.64775571170911,
+                    "velocityX": 1.460741602293634,
+                    "velocityY": -1.3144012739470166,
+                    "timestamp": 0.8381770460626691
+                },
+                {
+                    "x": 4.277338609770458,
+                    "y": 5.097982888442082,
+                    "heading": 0.6156568342246089,
+                    "angularVelocity": 5.214553109256573,
+                    "velocityX": 1.27206843549211,
+                    "velocityY": -1.3545493659959265,
+                    "timestamp": 0.867366374422008
+                },
+                {
+                    "x": 4.3089076073694805,
+                    "y": 5.060359750747959,
+                    "heading": 0.7826775871522255,
+                    "angularVelocity": 5.721979994588639,
+                    "velocityX": 1.0815253167318162,
+                    "velocityY": -1.2889346829415846,
+                    "timestamp": 0.896555702781347
+                },
+                {
+                    "x": 4.3340758913958135,
+                    "y": 5.025518984665433,
+                    "heading": 0.9544942987723,
+                    "angularVelocity": 5.886285203444992,
+                    "velocityX": 0.8622426565111364,
+                    "velocityY": -1.1936131470246671,
+                    "timestamp": 0.925745031140686
+                },
+                {
+                    "x": 4.353180255759456,
+                    "y": 4.9936006932957335,
+                    "heading": 1.134787723551591,
+                    "angularVelocity": 6.17668973262302,
+                    "velocityX": 0.6544982511572759,
+                    "velocityY": -1.0934918055244334,
+                    "timestamp": 0.9549343595000249
+                },
+                {
+                    "x": 4.3789611483924675,
+                    "y": 4.9633684481765155,
+                    "heading": 1.3088307728290547,
+                    "angularVelocity": 5.962557518791945,
+                    "velocityX": 0.8832300735266126,
+                    "velocityY": -1.0357293853095562,
+                    "timestamp": 0.9841236878593639
+                },
+                {
+                    "x": 4.410526959907193,
+                    "y": 4.931127063214471,
+                    "heading": 1.468917613249843,
+                    "angularVelocity": 5.4844304209407735,
+                    "velocityX": 1.0814161643642388,
+                    "velocityY": -1.104560699894603,
+                    "timestamp": 1.013313016218703
+                },
+                {
+                    "x": 4.4435007518243745,
+                    "y": 4.892801223101424,
+                    "heading": 1.6159090393580717,
+                    "angularVelocity": 5.035793365940867,
+                    "velocityX": 1.1296523000205303,
+                    "velocityY": -1.3130086325122463,
+                    "timestamp": 1.042502344578042
+                },
+                {
+                    "x": 4.478070868346437,
+                    "y": 4.848243847113431,
+                    "heading": 1.7513264495243417,
+                    "angularVelocity": 4.639278043646516,
+                    "velocityX": 1.1843409377729661,
+                    "velocityY": -1.5264954177589778,
+                    "timestamp": 1.0716916729373809
+                },
+                {
+                    "x": 4.514676965101799,
+                    "y": 4.797506620196597,
+                    "heading": 1.8758559761070788,
+                    "angularVelocity": 4.266268995630874,
+                    "velocityX": 1.2540917798696012,
+                    "velocityY": -1.738211523479656,
+                    "timestamp": 1.1008810012967198
+                },
+                {
+                    "x": 4.554041400948399,
+                    "y": 4.740897535447995,
+                    "heading": 1.989313365398794,
+                    "angularVelocity": 3.886947582177413,
+                    "velocityX": 1.3485899833664619,
+                    "velocityY": -1.939376064146053,
+                    "timestamp": 1.1300703296560588
+                },
+                {
+                    "x": 4.597166214346246,
+                    "y": 4.6791223056158096,
+                    "heading": 2.090829457044177,
+                    "angularVelocity": 3.477849520744562,
+                    "velocityX": 1.4774171185768328,
+                    "velocityY": -2.1163635240829413,
+                    "timestamp": 1.1592596580153978
+                },
+                {
+                    "x": 4.645233022395554,
+                    "y": 4.613520579676299,
+                    "heading": 2.179525396238879,
+                    "angularVelocity": 3.0386426882728963,
+                    "velocityX": 1.646725387359914,
+                    "velocityY": -2.2474558212478137,
+                    "timestamp": 1.1884489863747367
+                },
+                {
+                    "x": 4.699569055464054,
+                    "y": 4.547167372776357,
+                    "heading": 2.256949250765549,
+                    "angularVelocity": 2.6524712584521715,
+                    "velocityX": 1.8615033686143418,
+                    "velocityY": -2.2732008795506418,
+                    "timestamp": 1.2176383147340757
+                },
+                {
+                    "x": 4.760368188580363,
+                    "y": 4.482555153301175,
+                    "heading": 2.3258054605639615,
+                    "angularVelocity": 2.358951495928562,
+                    "velocityX": 2.082923333069965,
+                    "velocityY": -2.213556224376411,
+                    "timestamp": 1.2468276430934147
+                },
+                {
+                    "x": 4.827470532546925,
+                    "y": 4.4190609396421525,
+                    "heading": 2.3838171568684117,
+                    "angularVelocity": 1.9874282679714124,
+                    "velocityX": 2.2988656381705637,
+                    "velocityY": -2.175254355885453,
+                    "timestamp": 1.2760169714527536
+                },
+                {
+                    "x": 4.899988963730756,
+                    "y": 4.356794264470432,
+                    "heading": 2.432111827676766,
+                    "angularVelocity": 1.6545317594778726,
+                    "velocityX": 2.484415889639006,
+                    "velocityY": -2.1331999971078073,
+                    "timestamp": 1.3052062998120926
+                },
+                {
+                    "x": 4.972562290971905,
+                    "y": 4.300811163566705,
+                    "heading": 2.4919687455960093,
+                    "angularVelocity": 2.050643892259759,
+                    "velocityX": 2.486296578932036,
+                    "velocityY": -1.9179304235623322,
+                    "timestamp": 1.3343956281714315
+                },
+                {
+                    "x": 5.045616975616562,
+                    "y": 4.25136299402395,
+                    "heading": 2.5615851998228103,
+                    "angularVelocity": 2.384996782720671,
+                    "velocityX": 2.5027874483889563,
+                    "velocityY": -1.6940495832592366,
+                    "timestamp": 1.3635849565307705
+                },
+                {
+                    "x": 5.124307057757338,
+                    "y": 4.205650619988245,
+                    "heading": 2.6233534867290937,
+                    "angularVelocity": 2.116125665718561,
+                    "velocityX": 2.6958510717359463,
+                    "velocityY": -1.5660646066588564,
+                    "timestamp": 1.3927742848901095
+                },
+                {
+                    "x": 5.208588203701103,
+                    "y": 4.163420547805767,
+                    "heading": 2.6756025601758724,
+                    "angularVelocity": 1.7900060187599918,
+                    "velocityX": 2.8873958628376504,
+                    "velocityY": -1.4467640934590689,
+                    "timestamp": 1.4219636132494484
+                },
+                {
+                    "x": 5.297563997608419,
+                    "y": 4.1261005954207315,
+                    "heading": 2.720276777571372,
+                    "angularVelocity": 1.5304982987457612,
+                    "velocityX": 3.048230257714994,
+                    "velocityY": -1.2785478283571272,
+                    "timestamp": 1.4511529416087874
                 },
                 {
                     "x": 5.390789031982422,
                     "y": 4.09412145614624,
-                    "heading": 2.865640793449113,
-                    "angularVelocity": -1.3772405876991196,
-                    "velocityX": 3.2419229878750393,
-                    "velocityY": -0.8858526858838836,
-                    "timestamp": 1.4433659348080845
-                },
-                {
-                    "x": 5.535592658236132,
-                    "y": 4.049337172847784,
-                    "heading": 2.841642220937184,
-                    "angularVelocity": -0.5722130304709164,
-                    "velocityX": 3.452643767066762,
-                    "velocityY": -1.0678197818199107,
-                    "timestamp": 1.4853058614332775
-                },
-                {
-                    "x": 5.681734895850587,
-                    "y": 4.003264514213434,
-                    "heading": 2.8233979117656802,
-                    "angularVelocity": -0.4350105171749157,
-                    "velocityX": 3.484561118108144,
-                    "velocityY": -1.098539323783022,
-                    "timestamp": 1.5272457880584704
-                },
-                {
-                    "x": 5.827742551539107,
-                    "y": 3.9572785916007636,
-                    "heading": 2.804954082691072,
-                    "angularVelocity": -0.4397677954812897,
-                    "velocityX": 3.481352196758831,
-                    "velocityY": -1.096471222366128,
-                    "timestamp": 1.5691857146836634
-                },
-                {
-                    "x": 5.9679573209760495,
-                    "y": 3.908501280249766,
-                    "heading": 2.7717324952229117,
-                    "angularVelocity": -0.7921231661908814,
-                    "velocityX": 3.3432287731451744,
-                    "velocityY": -1.1630280564605782,
-                    "timestamp": 1.6111256413088564
-                },
-                {
-                    "x": 6.096940014064459,
-                    "y": 3.8543767396001813,
-                    "heading": 2.711734513417447,
-                    "angularVelocity": -1.4305695463335533,
-                    "velocityX": 3.07541532537948,
-                    "velocityY": -1.2905254015650245,
-                    "timestamp": 1.6530655679340494
-                },
-                {
-                    "x": 6.215088187160598,
-                    "y": 3.7918967611554555,
-                    "heading": 2.632459037184743,
-                    "angularVelocity": -1.890214948184595,
-                    "velocityX": 2.8170810633981405,
-                    "velocityY": -1.4897493503766595,
-                    "timestamp": 1.6950054945592423
-                },
-                {
-                    "x": 6.32424952389229,
-                    "y": 3.717983083464812,
-                    "heading": 2.5463998013121922,
-                    "angularVelocity": -2.0519643880553406,
-                    "velocityX": 2.6028022821127137,
-                    "velocityY": -1.7623702194615716,
-                    "timestamp": 1.7369454211844353
-                },
-                {
-                    "x": 6.43630340548619,
-                    "y": 3.6322698094657313,
-                    "heading": 2.4886072976482807,
-                    "angularVelocity": -1.3779829464268867,
-                    "velocityX": 2.6717710451737124,
-                    "velocityY": -2.043715401914737,
-                    "timestamp": 1.7788853478096283
-                },
-                {
-                    "x": 6.552059359988985,
-                    "y": 3.5378523447078845,
-                    "heading": 2.4573864362362134,
-                    "angularVelocity": -0.7444185987992016,
-                    "velocityX": 2.7600418936656252,
-                    "velocityY": -2.251254886581771,
-                    "timestamp": 1.8208252744348212
-                },
-                {
-                    "x": 6.67032629558411,
-                    "y": 3.432112823682954,
-                    "heading": 2.4573457680499984,
-                    "angularVelocity": -0.0009696770950155036,
-                    "velocityX": 2.8199127922194083,
-                    "velocityY": -2.521213782033988,
-                    "timestamp": 1.8627652010600142
-                },
-                {
-                    "x": 6.787202921818474,
-                    "y": 3.3248215481814336,
-                    "heading": 2.4573432508654287,
-                    "angularVelocity": -0.00006001881195361165,
-                    "velocityX": 2.7867627733081557,
-                    "velocityY": -2.558213238195593,
-                    "timestamp": 1.9047051276852072
-                },
-                {
-                    "x": 6.904079764497574,
-                    "y": 3.217530508972575,
-                    "heading": 2.4573407325391345,
-                    "angularVelocity": -0.000060046034804859665,
-                    "velocityX": 2.7867679341358973,
-                    "velocityY": -2.5582076041213115,
-                    "timestamp": 1.9466450543104001
-                },
-                {
-                    "x": 7.017539426879104,
-                    "y": 3.1193532157248125,
-                    "heading": 2.428286840001792,
-                    "angularVelocity": -0.6927501995172902,
-                    "velocityX": 2.7052899590285975,
-                    "velocityY": -2.3409028376503684,
-                    "timestamp": 1.988584980935593
-                },
-                {
-                    "x": 7.118887938132189,
-                    "y": 3.020968055873212,
-                    "heading": 2.3710302150679574,
-                    "angularVelocity": -1.365205653446252,
-                    "velocityX": 2.4165161794108876,
-                    "velocityY": -2.345859131582302,
-                    "timestamp": 2.030524907560786
-                },
-                {
-                    "x": 7.207551237715759,
-                    "y": 2.9223074295194613,
-                    "heading": 2.2882909026899103,
-                    "angularVelocity": -1.9728053679604207,
-                    "velocityX": 2.1140547139229047,
-                    "velocityY": -2.3524272523282552,
-                    "timestamp": 2.0724648341859786
-                },
-                {
-                    "x": 7.28673413242529,
-                    "y": 2.8318719270188626,
-                    "heading": 2.1761461762136682,
-                    "angularVelocity": -2.6739371167349164,
-                    "velocityX": 1.8880074688058086,
-                    "velocityY": -2.1563104606452623,
-                    "timestamp": 2.1144047608111713
-                },
-                {
-                    "x": 7.3579924776067,
-                    "y": 2.7517426977667294,
-                    "heading": 2.0378729016459984,
-                    "angularVelocity": -3.2969364921256217,
-                    "velocityX": 1.6990574594521515,
-                    "velocityY": -1.9105715174046114,
-                    "timestamp": 2.156344687436364
-                },
-                {
-                    "x": 7.4224137082831785,
-                    "y": 2.6828558926645387,
-                    "heading": 1.874977709266495,
-                    "angularVelocity": -3.884012335912244,
-                    "velocityX": 1.5360358460374843,
-                    "velocityY": -1.6425113405136766,
-                    "timestamp": 2.198284614061557
-                },
-                {
-                    "x": 7.480883603746462,
-                    "y": 2.6255140291502124,
-                    "heading": 1.6865689006450588,
-                    "angularVelocity": -4.4923495051672475,
-                    "velocityX": 1.3941344243592637,
-                    "velocityY": -1.3672380504328716,
-                    "timestamp": 2.2402245406867496
-                },
-                {
-                    "x": 7.534437188323511,
-                    "y": 2.579534136686106,
-                    "heading": 1.4695142507662173,
-                    "angularVelocity": -5.175370281846384,
-                    "velocityX": 1.27691173748694,
-                    "velocityY": -1.0963274417482318,
-                    "timestamp": 2.2821644673119423
-                },
-                {
-                    "x": 7.579534895925645,
-                    "y": 2.5413039154996566,
-                    "heading": 1.2172257697546927,
-                    "angularVelocity": -6.015472636997345,
-                    "velocityX": 1.0752929542572058,
-                    "velocityY": -0.9115471643072651,
-                    "timestamp": 2.324104393937135
-                },
-                {
-                    "x": 7.615712715030757,
-                    "y": 2.5097826728946813,
-                    "heading": 0.9253708991340255,
-                    "angularVelocity": -6.958878903840344,
-                    "velocityX": 0.8626104530039771,
-                    "velocityY": -0.7515807761580874,
-                    "timestamp": 2.366044320562328
-                },
-                {
-                    "x": 7.6401514150011085,
-                    "y": 2.4865499215178333,
-                    "heading": 0.6440959436378817,
-                    "angularVelocity": -6.706615345559152,
-                    "velocityX": 0.5827072657697844,
-                    "velocityY": -0.5539530763721487,
-                    "timestamp": 2.4079842471875206
-                },
-                {
-                    "x": 7.656070937434432,
-                    "y": 2.471863188851966,
-                    "heading": 0.3945229348796906,
-                    "angularVelocity": -5.950725927314207,
-                    "velocityX": 0.37957916749813,
-                    "velocityY": -0.3501849871393242,
-                    "timestamp": 2.4499241738127133
-                },
-                {
-                    "x": 7.668072859318959,
-                    "y": 2.4642781587251137,
-                    "heading": 0.19785094472919199,
-                    "angularVelocity": -4.689373729909185,
-                    "velocityX": 0.28616935818189254,
-                    "velocityY": -0.18085463512222474,
-                    "timestamp": 2.491864100437906
-                },
+                    "heading": 2.757748451080741,
+                    "angularVelocity": 1.2837456569082273,
+                    "velocityX": 3.1938053944353855,
+                    "velocityY": -1.0955763997309995,
+                    "timestamp": 1.4803422699681263
+                },
+                {
+                    "x": 5.532416911390752,
+                    "y": 4.059853570653742,
+                    "heading": 2.796595493071039,
+                    "angularVelocity": 0.9277997104504593,
+                    "velocityX": 3.3825562713263437,
+                    "velocityY": -0.818433852585974,
+                    "timestamp": 1.522212343382395
+                },
+                {
+                    "x": 5.681515537517707,
+                    "y": 4.021534249773671,
+                    "heading": 2.782742222711128,
+                    "angularVelocity": -0.3308632928068908,
+                    "velocityX": 3.5609831550031092,
+                    "velocityY": -0.9151959324488058,
+                    "timestamp": 1.5640824167966638
+                },
+                {
+                    "x": 5.826340094588043,
+                    "y": 3.969241008458542,
+                    "heading": 2.7680381624736805,
+                    "angularVelocity": -0.351183053632681,
+                    "velocityX": 3.4589038246343846,
+                    "velocityY": -1.2489407600921083,
+                    "timestamp": 1.6059524902109326
+                },
+                {
+                    "x": 5.965226819905552,
+                    "y": 3.9129528098786466,
+                    "heading": 2.7396888789742415,
+                    "angularVelocity": -0.6770774729470177,
+                    "velocityX": 3.317088172818383,
+                    "velocityY": -1.3443539499673571,
+                    "timestamp": 1.6478225636252013
+                },
+                {
+                    "x": 6.097371617524639,
+                    "y": 3.854212614126153,
+                    "heading": 2.694967938521201,
+                    "angularVelocity": -1.0680884174853156,
+                    "velocityX": 3.1560679703526437,
+                    "velocityY": -1.402915996141418,
+                    "timestamp": 1.68969263703947
+                },
+                {
+                    "x": 6.217931948857121,
+                    "y": 3.789923024350862,
+                    "heading": 2.6256097109731904,
+                    "angularVelocity": -1.6565107699184056,
+                    "velocityX": 2.8793914483893763,
+                    "velocityY": -1.535454431598441,
+                    "timestamp": 1.7315627104537388
+                },
+                {
+                    "x": 6.32897922975643,
+                    "y": 3.7148340577600942,
+                    "heading": 2.547026684843302,
+                    "angularVelocity": -1.8768303879569581,
+                    "velocityX": 2.652187394098668,
+                    "velocityY": -1.7933803422751633,
+                    "timestamp": 1.7734327838680075
+                },
+                {
+                    "x": 6.440862810984381,
+                    "y": 3.629355422988249,
+                    "heading": 2.48919642870263,
+                    "angularVelocity": -1.381183538144076,
+                    "velocityX": 2.672161095132505,
+                    "velocityY": -2.0415210149288825,
+                    "timestamp": 1.8153028572822762
+                },
+                {
+                    "x": 6.556707402178449,
+                    "y": 3.5350910926988375,
+                    "heading": 2.4587564932761583,
+                    "angularVelocity": -0.7270093635923316,
+                    "velocityX": 2.766763507861174,
+                    "velocityY": -2.2513533558144507,
+                    "timestamp": 1.857172930696545
+                },
+                {
+                    "x": 6.674645090324846,
+                    "y": 3.429370276775565,
+                    "heading": 2.4587373064570563,
+                    "angularVelocity": -0.00045824660759140524,
+                    "velocityX": 2.816753794040547,
+                    "velocityY": -2.5249732637737297,
+                    "timestamp": 1.8990430041108137
+                },
+                {
+                    "x": 6.791280321048844,
+                    "y": 3.3222067883695963,
+                    "heading": 2.45873486083509,
+                    "angularVelocity": -0.00005840978453659185,
+                    "velocityX": 2.785646673460325,
+                    "velocityY": -2.559429197691564,
+                    "timestamp": 1.9409130775250825
+                },
+                {
+                    "x": 6.907916035753451,
+                    "y": 3.2150438274589583,
+                    "heading": 2.4587324135695865,
+                    "angularVelocity": -0.000058449037785704465,
+                    "velocityX": 2.78565823256613,
+                    "velocityY": -2.5594165993059286,
+                    "timestamp": 1.9827831509393512
+                },
+                {
+                    "x": 7.020962902689577,
+                    "y": 3.1172887235192603,
+                    "heading": 2.428615335233058,
+                    "angularVelocity": -0.7192984363448716,
+                    "velocityX": 2.6999443210338705,
+                    "velocityY": -2.334724923276202,
+                    "timestamp": 2.02465322435362
+                },
+                {
+                    "x": 7.121900964071713,
+                    "y": 3.0192154080188542,
+                    "heading": 2.3705886859446603,
+                    "angularVelocity": -1.385874075602246,
+                    "velocityX": 2.4107447909976107,
+                    "velocityY": -2.3423248994587165,
+                    "timestamp": 2.0665232977678887
+                },
+                {
+                    "x": 7.210190441218643,
+                    "y": 2.92089200334631,
+                    "heading": 2.287196223783446,
+                    "angularVelocity": -1.9916961056197946,
+                    "velocityX": 2.10865350708562,
+                    "velocityY": -2.348297880916461,
+                    "timestamp": 2.1083933711821574
+                },
+                {
+                    "x": 7.2890247338542125,
+                    "y": 2.830773686809754,
+                    "heading": 2.1745743754916034,
+                    "angularVelocity": -2.689793427815253,
+                    "velocityX": 1.8828314881507633,
+                    "velocityY": -2.1523324223703058,
+                    "timestamp": 2.150263444596426
+                },
+                {
+                    "x": 7.359953440317656,
+                    "y": 2.7509272688501483,
+                    "heading": 2.035955336818745,
+                    "angularVelocity": -3.3106949037643396,
+                    "velocityX": 1.6940191568728353,
+                    "velocityY": -1.9070044890916185,
+                    "timestamp": 2.192133518010695
+                },
+                {
+                    "x": 7.4240572342258595,
+                    "y": 2.6822786221440054,
+                    "heading": 1.872800890040008,
+                    "angularVelocity": -3.896684038847076,
+                    "velocityX": 1.5310169933057254,
+                    "velocityY": -1.6395635619484732,
+                    "timestamp": 2.2340035914249636
+                },
+                {
+                    "x": 7.482217323317858,
+                    "y": 2.6251237224270416,
+                    "heading": 1.684191854325982,
+                    "angularVelocity": -4.504626343687046,
+                    "velocityX": 1.3890610727273844,
+                    "velocityY": -1.3650537258787387,
+                    "timestamp": 2.2758736648392324
+                },
+                {
+                    "x": 7.535468730024687,
+                    "y": 2.5792763511542147,
+                    "heading": 1.466995470236355,
+                    "angularVelocity": -5.187389616938403,
+                    "velocityX": 1.2718250140130243,
+                    "velocityY": -1.0949914230912903,
+                    "timestamp": 2.317743738253501
+                },
+                {
+                    "x": 7.580274182888533,
+                    "y": 2.5411410245640176,
+                    "heading": 1.2146613001768354,
+                    "angularVelocity": -6.02659965658257,
+                    "velocityX": 1.0701068617801175,
+                    "velocityY": -0.91080152195771,
+                    "timestamp": 2.35961381166777
+                },
+                {
+                    "x": 7.616169603666072,
+                    "y": 2.509680561991982,
+                    "heading": 0.922946754094809,
+                    "angularVelocity": -6.967137200734248,
+                    "velocityX": 0.8573049400316086,
+                    "velocityY": -0.7513830286553573,
+                    "timestamp": 2.4014838850820386
+                },
+                {
+                    "x": 7.640414148022587,
+                    "y": 2.4865073161659272,
+                    "heading": 0.6422191973798425,
+                    "angularVelocity": -6.704730463149791,
+                    "velocityX": 0.5790423178062244,
+                    "velocityY": -0.5534560590991822,
+                    "timestamp": 2.4433539584963073
+                },
+                {
+                    "x": 7.656220323606262,
+                    "y": 2.4718614980447855,
+                    "heading": 0.3932969564478367,
+                    "angularVelocity": -5.9451111649394885,
+                    "velocityX": 0.37750532288984423,
+                    "velocityY": -0.34979203347062976,
+                    "timestamp": 2.485224031910576
+                },
+                {
+                    "x": 7.668143711832241,
+                    "y": 2.4642737767821443,
+                    "heading": 0.1972336516813064,
+                    "angularVelocity": -4.682659684559189,
+                    "velocityX": 0.28477113254632147,
+                    "velocityY": -0.18122063430765137,
+                    "timestamp": 2.527094105324845
+                },
                 {
-                    "x": 7.676136280306529,
-                    "y": 2.459442710773055,
-                    "heading": 0.06623696286278732,
-                    "angularVelocity": -3.1381547956105695,
-                    "velocityX": 0.19226120874341576,
-                    "velocityY": -0.11529462116784107,
-                    "timestamp": 2.533804027063099
+                    "x": 7.676158456210791,
+                    "y": 2.459440783193878,
+                    "heading": 0.06602780599823356,
+                    "angularVelocity": -3.1336425992116688,
+                    "velocityX": 0.19141940113768102,
+                    "velocityY": -0.11542835238065828,
+                    "timestamp": 2.5689641787391135
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": 2.1057025676027217e-33,
-                    "angularVelocity": -1.57932948845455,
-                    "velocityX": 0.09212509425682942,
-                    "velocityY": -0.058243086471875524,
-                    "timestamp": 2.5757439536882916
+                    "heading": 1.4385689598890085e-34,
+                    "angularVelocity": -1.576968956919288,
+                    "velocityX": 0.09174915341561693,
+                    "velocityY": -0.05829421815741107,
+                    "timestamp": 2.6108342521533823
                 },
                 {
                     "x": 7.68,
                     "y": 2.457,
-                    "heading": -7.524501141381425e-34,
-                    "angularVelocity": -5.907340858361994e-32,
-                    "velocityX": -7.634877430876997e-33,
-                    "velocityY": -7.90956322394541e-33,
-                    "timestamp": 2.6176838803134843
+                    "heading": 5.954582727394106e-35,
+                    "angularVelocity": -1.9052580558070114e-33,
+                    "velocityX": -2.275662344047759e-34,
+                    "velocityY": 3.071010312134925e-34,
+                    "timestamp": 2.652704325567651
                 }
             ],
             "constraints": [
@@ -18311,7 +20397,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 18
+                    "controlIntervalCount": 12
+                },
+                {
+                    "x": 2.0410311222076416,
+                    "y": 6.2410478591918945,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 15
                 },
                 {
                     "x": 5.176,
@@ -18328,172 +20423,253 @@
                     "x": 2.3,
                     "y": 4.36,
                     "heading": -0.39,
-                    "angularVelocity": 5.020065556819226e-31,
-                    "velocityX": -2.911156118414847e-31,
-                    "velocityY": 8.635491730351355e-32,
+                    "angularVelocity": -4.289844309028223e-18,
+                    "velocityX": -1.3032340882001426e-17,
+                    "velocityY": -3.394190195762378e-17,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.3221690164859408,
-                    "y": 4.411229757983623,
-                    "heading": -0.4048176043167742,
-                    "angularVelocity": -0.1808500047435309,
-                    "velocityX": 0.27057455786553103,
-                    "velocityY": 0.6252631516043967,
-                    "timestamp": 0.08193311544453755
-                },
-                {
-                    "x": 2.3687303275118676,
-                    "y": 4.5127321003230465,
-                    "heading": -0.4313760168590284,
-                    "angularVelocity": -0.32414747563532126,
-                    "velocityX": 0.5682843960383053,
-                    "velocityY": 1.2388439251785637,
-                    "timestamp": 0.1638662308890751
-                },
-                {
-                    "x": 2.4431953998066236,
-                    "y": 4.662720767025029,
-                    "heading": -0.4646753197408115,
-                    "angularVelocity": -0.40642056268856935,
-                    "velocityX": 0.9088519567543129,
-                    "velocityY": 1.830623257619443,
-                    "timestamp": 0.24579934633361264
-                },
-                {
-                    "x": 2.551740018878988,
-                    "y": 4.857106976368847,
-                    "heading": -0.4953245435557511,
-                    "angularVelocity": -0.3740761430663416,
-                    "velocityX": 1.3247954564335083,
-                    "velocityY": 2.3724986934666186,
-                    "timestamp": 0.3277324617781502
-                },
-                {
-                    "x": 2.701293241756868,
-                    "y": 5.089437731072216,
-                    "heading": -0.5138028115710582,
-                    "angularVelocity": -0.2255286878210994,
-                    "velocityX": 1.8253086321258618,
-                    "velocityY": 2.835614799252222,
-                    "timestamp": 0.4096655772226877
-                },
-                {
-                    "x": 2.9065804913658706,
-                    "y": 5.321655101693131,
-                    "heading": -0.5138024716246538,
-                    "angularVelocity": 0.000004149072112901325,
-                    "velocityX": 2.5055467315650244,
-                    "velocityY": 2.8342309377715424,
-                    "timestamp": 0.4915986926672252
-                },
-                {
-                    "x": 3.1502852438919966,
-                    "y": 5.513164387705616,
-                    "heading": -0.5138023715427595,
-                    "angularVelocity": 0.0000012215072474193347,
-                    "velocityX": 2.974435320857471,
-                    "velocityY": 2.3373856220824885,
-                    "timestamp": 0.5735318081117627
-                },
-                {
-                    "x": 3.4075351073224858,
-                    "y": 5.686052564856294,
-                    "heading": -0.5138022777253332,
-                    "angularVelocity": 0.0000011450489327031063,
-                    "velocityX": 3.1397544452539083,
-                    "velocityY": 2.110113550701131,
-                    "timestamp": 0.6554649235563003
-                },
-                {
-                    "x": 3.664785106053728,
-                    "y": 5.858940540685368,
-                    "heading": -0.5138021839079745,
-                    "angularVelocity": 0.0000011450481052658978,
-                    "velocityX": 3.13975609661006,
-                    "velocityY": 2.110111093555414,
-                    "timestamp": 0.7373980390008378
-                },
-                {
-                    "x": 3.922035141067721,
-                    "y": 6.031828462527248,
-                    "heading": -0.5138020900906538,
-                    "angularVelocity": 0.000001145047641454271,
-                    "velocityX": 3.1397565394438236,
-                    "velocityY": 2.1101104346375243,
-                    "timestamp": 0.8193311544453753
-                },
-                {
-                    "x": 4.179285313417939,
-                    "y": 6.204716180018366,
-                    "heading": -0.5138019962733545,
-                    "angularVelocity": 0.00000114504738127865,
-                    "velocityX": 3.139758215643051,
-                    "velocityY": 2.110107940520708,
-                    "timestamp": 0.9012642698899128
-                },
-                {
-                    "x": 4.443584109358565,
-                    "y": 6.354084975447565,
-                    "heading": -0.4978326665645833,
-                    "angularVelocity": 0.19490690207652947,
-                    "velocityX": 3.225787210783358,
-                    "velocityY": 1.8230576808752068,
-                    "timestamp": 0.9831973853344503
-                },
-                {
-                    "x": 4.674709010100242,
-                    "y": 6.4693171293955105,
-                    "heading": -0.3712044647358083,
-                    "angularVelocity": 1.5455069801964878,
-                    "velocityX": 2.8208972585465766,
-                    "velocityY": 1.406417335929929,
-                    "timestamp": 1.065130500778988
-                },
-                {
-                    "x": 4.870816853917273,
-                    "y": 6.548974021375514,
-                    "heading": -0.15522232897542676,
-                    "angularVelocity": 2.636078642787472,
-                    "velocityX": 2.3935113751384223,
-                    "velocityY": 0.9722185168697003,
-                    "timestamp": 1.1470636162235255
-                },
-                {
-                    "x": 5.022935942406859,
-                    "y": 6.594453289671109,
-                    "heading": 0.041233782182786366,
-                    "angularVelocity": 2.3977620049271122,
-                    "velocityX": 1.856625220025449,
-                    "velocityY": 0.5550779809707214,
-                    "timestamp": 1.228996731668063
-                },
-                {
-                    "x": 5.124959992690689,
-                    "y": 6.619307485763033,
-                    "heading": 0.18624088374392397,
-                    "angularVelocity": 1.769822870452196,
-                    "velocityX": 1.2452114109208137,
-                    "velocityY": 0.3033473822772884,
-                    "timestamp": 1.3109298471126005
-                },
-                {
-                    "x": 5.176,
+                    "x": 2.271980215040446,
+                    "y": 4.410826628196332,
+                    "heading": -0.3872050871501322,
+                    "angularVelocity": 0.033498492691473286,
+                    "velocityX": -0.3358317815939762,
+                    "velocityY": 0.6091837294305799,
+                    "timestamp": 0.0834339883696639
+                },
+                {
+                    "x": 2.216375801038755,
+                    "y": 4.512717243328352,
+                    "heading": -0.38158865604258857,
+                    "angularVelocity": 0.06731586511476574,
+                    "velocityX": -0.6664479918611741,
+                    "velocityY": 1.2212123275291613,
+                    "timestamp": 0.1668679767393278
+                },
+                {
+                    "x": 2.133913144213912,
+                    "y": 4.66605742038127,
+                    "heading": -0.3731070083031397,
+                    "angularVelocity": 0.10165698541488709,
+                    "velocityX": -0.9883580832768961,
+                    "velocityY": 1.837862243543364,
+                    "timestamp": 0.2503019651089917
+                },
+                {
+                    "x": 2.026047479965626,
+                    "y": 4.871580643769176,
+                    "heading": -0.3616752437087536,
+                    "angularVelocity": 0.13701567930528194,
+                    "velocityX": -1.2928264171046289,
+                    "velocityY": 2.4633033539909763,
+                    "timestamp": 0.3337359534786556
+                },
+                {
+                    "x": 1.8971303225339013,
+                    "y": 5.131188087013384,
+                    "heading": -0.34705934044989933,
+                    "angularVelocity": 0.1751792470161148,
+                    "velocityX": -1.5451395762230817,
+                    "velocityY": 3.111531023711595,
+                    "timestamp": 0.41716994184831946
+                },
+                {
+                    "x": 1.812941926504828,
+                    "y": 5.427772716701387,
+                    "heading": -0.3288482106163032,
+                    "angularVelocity": 0.21826991841509946,
+                    "velocityX": -1.0090419704726812,
+                    "velocityY": 3.5547219482539485,
+                    "timestamp": 0.5006039302179833
+                },
+                {
+                    "x": 1.7681664127359014,
+                    "y": 5.682327706651028,
+                    "heading": -0.2856043053433686,
+                    "angularVelocity": 0.5183008282076023,
+                    "velocityX": -0.5366579573130098,
+                    "velocityY": 3.0509747277288763,
+                    "timestamp": 0.5840379185876472
+                },
+                {
+                    "x": 1.7580426073358177,
+                    "y": 5.890413977811611,
+                    "heading": -0.23166998700826372,
+                    "angularVelocity": 0.6464310215597402,
+                    "velocityX": -0.12133910409739124,
+                    "velocityY": 2.4940228224314014,
+                    "timestamp": 0.6674719069573111
+                },
+                {
+                    "x": 1.7809381631803756,
+                    "y": 6.0507944700387695,
+                    "heading": -0.17161076742769205,
+                    "angularVelocity": 0.7198411672948628,
+                    "velocityX": 0.27441521485536685,
+                    "velocityY": 1.9222441041228078,
+                    "timestamp": 0.7509058953269749
+                },
+                {
+                    "x": 1.8360347454575048,
+                    "y": 6.162894796860879,
+                    "heading": -0.10766394629623156,
+                    "angularVelocity": 0.7664361057242964,
+                    "velocityX": 0.6603613629651003,
+                    "velocityY": 1.34358106345549,
+                    "timestamp": 0.8343398836966388
+                },
+                {
+                    "x": 1.9228414263613316,
+                    "y": 6.226384193614056,
+                    "heading": -0.041156065591054634,
+                    "angularVelocity": 0.797131744563275,
+                    "velocityX": 1.0404234844874343,
+                    "velocityY": 0.760953635248516,
+                    "timestamp": 0.9177738720663027
+                },
+                {
+                    "x": 2.041031122207642,
+                    "y": 6.2410478591918945,
+                    "heading": 0.027035456766794728,
+                    "angularVelocity": 0.8173110705883178,
+                    "velocityX": 1.4165653369372246,
+                    "velocityY": 0.17575170340583585,
+                    "timestamp": 1.0012078604359667
+                },
+                {
+                    "x": 2.201330972597037,
+                    "y": 6.260936085330867,
+                    "heading": 0.09277658873792281,
+                    "angularVelocity": 0.8449644804697937,
+                    "velocityX": 2.060318642882427,
+                    "velocityY": 0.2556214680702958,
+                    "timestamp": 1.0790112870770416
+                },
+                {
+                    "x": 2.4117170311040814,
+                    "y": 6.287038452225851,
+                    "heading": 0.16066894258910833,
+                    "angularVelocity": 0.8726139294965615,
+                    "velocityX": 2.7040718846096095,
+                    "velocityY": 0.33549122476729665,
+                    "timestamp": 1.1568147137181166
+                },
+                {
+                    "x": 2.672189282535082,
+                    "y": 6.319354957991783,
+                    "heading": 0.23071153953234388,
+                    "angularVelocity": 0.9002507982373139,
+                    "velocityX": 3.3478249310538675,
+                    "velocityY": 0.4153609572357597,
+                    "timestamp": 1.2346181403591916
+                },
+                {
+                    "x": 2.9642755449736304,
+                    "y": 6.355593779334123,
+                    "heading": 0.23071154098757585,
+                    "angularVelocity": 1.8703321803502023e-8,
+                    "velocityX": 3.7541567903686675,
+                    "velocityY": 0.4657741041344694,
+                    "timestamp": 1.3124215670002666
+                },
+                {
+                    "x": 3.256361807413129,
+                    "y": 6.39183260067658,
+                    "heading": 0.23071154244151956,
+                    "angularVelocity": 1.868721301612308e-8,
+                    "velocityX": 3.7541567903805326,
+                    "velocityY": 0.46577410413594156,
+                    "timestamp": 1.3902249936413416
+                },
+                {
+                    "x": 3.5484480698526375,
+                    "y": 6.4280714220190385,
+                    "heading": 0.23071154389546672,
+                    "angularVelocity": 1.868722261906901e-8,
+                    "velocityX": 3.7541567903806516,
+                    "velocityY": 0.4657741041359563,
+                    "timestamp": 1.4680284202824165
+                },
+                {
+                    "x": 3.8405343322921546,
+                    "y": 6.464310243361498,
+                    "heading": 0.23071154534942268,
+                    "angularVelocity": 1.868730306107698e-8,
+                    "velocityX": 3.754156790380741,
+                    "velocityY": 0.46577410413596737,
+                    "timestamp": 1.5458318469234915
+                },
+                {
+                    "x": 4.132620594731676,
+                    "y": 6.500549064703958,
+                    "heading": 0.23071154680338032,
+                    "angularVelocity": 1.86873022983191e-8,
+                    "velocityX": 3.754156790380778,
+                    "velocityY": 0.465774104135972,
+                    "timestamp": 1.6236352735645665
+                },
+                {
+                    "x": 4.424706857172369,
+                    "y": 6.536787886046564,
+                    "heading": 0.23071154826355325,
+                    "angularVelocity": 1.876697436713731e-8,
+                    "velocityX": 3.7541567903965425,
+                    "velocityY": 0.46577410413792786,
+                    "timestamp": 1.7014387002056415
+                },
+                {
+                    "x": 4.6751378783325555,
+                    "y": 6.567858587416207,
+                    "heading": 0.24147534520321134,
+                    "angularVelocity": 0.13834605240872366,
+                    "velocityX": 3.2187659589270945,
+                    "velocityY": 0.39934875250390567,
+                    "timestamp": 1.7792421268467165
+                },
+                {
+                    "x": 4.875482718759792,
+                    "y": 6.592715151427428,
+                    "heading": 0.2500855408103817,
+                    "angularVelocity": 0.11066602050088868,
+                    "velocityX": 2.575013069173925,
+                    "velocityY": 0.31947903947593354,
+                    "timestamp": 1.8570455534877914
+                },
+                {
+                    "x": 5.025741356581141,
+                    "y": 6.611357575366475,
+                    "heading": 0.25654288837544836,
+                    "angularVelocity": 0.08299567055584532,
+                    "velocityX": 1.931259898289217,
+                    "velocityY": 0.23960929156828065,
+                    "timestamp": 1.9348489801288664
+                },
+                {
+                    "x": 5.125913784825429,
+                    "y": 6.623785858368443,
+                    "heading": 0.2608476597362627,
+                    "angularVelocity": 0.05532881451478021,
+                    "velocityX": 1.2875066378045663,
+                    "velocityY": 0.15973953254406137,
+                    "timestamp": 2.0126524067699414
+                },
+                {
+                    "x": 5.175999999999999,
                     "y": 6.63,
-                    "heading": 0.263,
-                    "angularVelocity": 0.9368509404239479,
-                    "velocityX": 0.6229472299739557,
-                    "velocityY": 0.13050296182384175,
-                    "timestamp": 1.392862962557138
+                    "heading": 0.26300000000000007,
+                    "angularVelocity": 0.02766382352150404,
+                    "velocityX": 0.6437533324290226,
+                    "velocityY": 0.07986976795027836,
+                    "timestamp": 2.090455833411016
                 },
                 {
                     "x": 5.176,
                     "y": 6.63,
                     "heading": 0.263,
-                    "angularVelocity": 1.5668772442975024e-30,
-                    "velocityX": 0,
-                    "velocityY": -2.8584411322391187e-31,
-                    "timestamp": 1.4747960780016756
+                    "angularVelocity": -5.112167395133813e-18,
+                    "velocityX": -1.2520036782163602e-16,
+                    "velocityY": -1.3106574021367763e-17,
+                    "timestamp": 2.168259260052091
                 }
             ],
             "constraints": [
@@ -18508,6 +20684,13 @@
                         "last"
                     ],
                     "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        1,
+                        2
+                    ],
+                    "type": "StraightLine"
                 }
             ],
             "usesControlIntervalGuessing": true,
@@ -18535,7 +20718,25 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 13
+                    "controlIntervalCount": 12
+                },
+                {
+                    "x": 2.2487735748291016,
+                    "y": 6.386467456817627,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 8
+                },
+                {
+                    "x": 3.536776304244995,
+                    "y": 6.365693092346191,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 8
                 },
                 {
                     "x": 4.263,
@@ -18552,127 +20753,262 @@
                     "x": 2.3,
                     "y": 4.36,
                     "heading": -0.39,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
+                    "angularVelocity": -7.3580792676211765e-19,
+                    "velocityX": -1.2600069549143939e-17,
+                    "velocityY": 7.063684033512061e-18,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.3223907681513127,
-                    "y": 4.418261357040706,
-                    "heading": -0.429219543622513,
-                    "angularVelocity": -0.4499475285553182,
-                    "velocityX": 0.2568788379871729,
-                    "velocityY": 0.6684053711347925,
-                    "timestamp": 0.08716470506781053
-                },
-                {
-                    "x": 2.374909672838796,
-                    "y": 4.531319644218887,
-                    "heading": -0.5050759499486299,
-                    "angularVelocity": -0.8702651637162607,
-                    "velocityX": 0.6025248940683728,
-                    "velocityY": 1.297064988520603,
-                    "timestamp": 0.17432941013562106
-                },
-                {
-                    "x": 2.469260718438912,
-                    "y": 4.691180971030917,
-                    "heading": -0.6116327562753044,
-                    "angularVelocity": -1.2224765315706085,
-                    "velocityX": 1.082445532589298,
-                    "velocityY": 1.834014429208093,
-                    "timestamp": 0.2614941152034316
-                },
-                {
-                    "x": 2.620331804373909,
-                    "y": 4.8788277526908095,
-                    "heading": -0.7357228691689549,
-                    "angularVelocity": -1.4236279787456398,
-                    "velocityX": 1.7331680961631113,
-                    "velocityY": 2.1527839911109723,
-                    "timestamp": 0.3486588202712421
-                },
-                {
-                    "x": 2.832381156144981,
-                    "y": 5.083303561006889,
-                    "heading": -0.84759450212177,
-                    "angularVelocity": -1.2834510581523242,
-                    "velocityX": 2.432743294503276,
-                    "velocityY": 2.3458555634072913,
-                    "timestamp": 0.43582352533905266
-                },
-                {
-                    "x": 3.098611796220168,
-                    "y": 5.267387874253087,
-                    "heading": -0.8666590247194526,
-                    "angularVelocity": -0.21871837440225492,
-                    "velocityX": 3.0543399403241303,
-                    "velocityY": 2.111913452847551,
-                    "timestamp": 0.5229882304068632
-                },
-                {
-                    "x": 3.3974595011437105,
-                    "y": 5.397776588765608,
-                    "heading": -0.8564937886773937,
-                    "angularVelocity": 0.11662101115527058,
-                    "velocityX": 3.42854030987716,
-                    "velocityY": 1.4958888969002289,
-                    "timestamp": 0.6101529354746736
-                },
-                {
-                    "x": 3.678181484629676,
-                    "y": 5.472509187071852,
-                    "heading": -0.7581230365630254,
-                    "angularVelocity": 1.1285617502845884,
-                    "velocityX": 3.220592363245829,
-                    "velocityY": 0.8573722385466142,
-                    "timestamp": 0.6973176405424841
-                },
-                {
-                    "x": 3.915033154173224,
-                    "y": 5.514676302538828,
-                    "heading": -0.5395209628894968,
-                    "angularVelocity": 2.5079196161274817,
-                    "velocityX": 2.7172887163363586,
-                    "velocityY": 0.4837636453214852,
-                    "timestamp": 0.7844823456102946
-                },
-                {
-                    "x": 4.091189385673594,
-                    "y": 5.53976294658506,
-                    "heading": -0.29904398170320484,
-                    "angularVelocity": 2.7588802256510867,
-                    "velocityX": 2.0209582693284847,
-                    "velocityY": 0.2878073645372324,
-                    "timestamp": 0.8716470506781051
-                },
-                {
-                    "x": 4.2064124532392215,
-                    "y": 5.553849604220975,
-                    "heading": -0.10702579424844161,
-                    "angularVelocity": 2.2029350905894844,
-                    "velocityX": 1.3219005040628353,
-                    "velocityY": 0.161609651807531,
-                    "timestamp": 0.9588117557459156
+                    "x": 2.277488982590109,
+                    "y": 4.417838766017227,
+                    "heading": -0.3894665385190853,
+                    "angularVelocity": 0.006183201184696677,
+                    "velocityX": -0.26091883762393897,
+                    "velocityY": 0.6703927825220056,
+                    "timestamp": 0.08627593781609397
+                },
+                {
+                    "x": 2.232568196785243,
+                    "y": 4.5335556038331974,
+                    "heading": -0.3884042841517919,
+                    "angularVelocity": 0.012312290010209009,
+                    "velocityX": -0.520664126544967,
+                    "velocityY": 1.3412411472435377,
+                    "timestamp": 0.17255187563218793
+                },
+                {
+                    "x": 2.165406410226816,
+                    "y": 4.707215578005854,
+                    "heading": -0.38682100523451757,
+                    "angularVelocity": 0.018351338245094856,
+                    "velocityX": -0.7784532774547995,
+                    "velocityY": 2.012843656858665,
+                    "timestamp": 0.2588278134482819
+                },
+                {
+                    "x": 2.0763412317308223,
+                    "y": 4.9389471550535315,
+                    "heading": -0.3847322299034305,
+                    "angularVelocity": 0.024210404244363496,
+                    "velocityX": -1.0323293000400813,
+                    "velocityY": 2.685935185562845,
+                    "timestamp": 0.34510375126437587
+                },
+                {
+                    "x": 1.9663857850872422,
+                    "y": 5.229122444622766,
+                    "heading": -0.3821844673054777,
+                    "angularVelocity": 0.029530395872206988,
+                    "velocityX": -1.2744624912447062,
+                    "velocityY": 3.3633397319630034,
+                    "timestamp": 0.43137968908046986
+                },
+                {
+                    "x": 1.9100206214147308,
+                    "y": 5.550595198428695,
+                    "heading": -0.3821844591902738,
+                    "angularVelocity": 9.406103402067565e-8,
+                    "velocityX": -0.653312674417546,
+                    "velocityY": 3.7260997903167445,
+                    "timestamp": 0.5176556268965639
+                },
+                {
+                    "x": 1.8984249514650446,
+                    "y": 5.829240348205722,
+                    "heading": -0.3690339918971659,
+                    "angularVelocity": 0.15242334799236965,
+                    "velocityX": -0.13440213161644662,
+                    "velocityY": 3.229697141872688,
+                    "timestamp": 0.6039315647126579
+                },
+                {
+                    "x": 1.916554046418155,
+                    "y": 6.053409726810643,
+                    "heading": -0.3524665993885735,
+                    "angularVelocity": 0.1920279620015428,
+                    "velocityX": 0.2101292134517386,
+                    "velocityY": 2.5982838816862235,
+                    "timestamp": 0.6902075025287518
+                },
+                {
+                    "x": 1.9615881089279723,
+                    "y": 6.221650822615583,
+                    "heading": -0.33415152161256073,
+                    "angularVelocity": 0.21228488776393065,
+                    "velocityX": 0.5219770848021326,
+                    "velocityY": 1.9500349699305692,
+                    "timestamp": 0.7764834403448458
+                },
+                {
+                    "x": 2.0323565347582764,
+                    "y": 6.333414099671539,
+                    "heading": -0.3147709502575115,
+                    "angularVelocity": 0.22463472256153053,
+                    "velocityX": 0.8202568134484147,
+                    "velocityY": 1.2954165423758215,
+                    "timestamp": 0.8627593781609398
+                },
+                {
+                    "x": 2.1282194334415006,
+                    "y": 6.388411890330483,
+                    "heading": -0.2946952221482423,
+                    "angularVelocity": 0.232692087938443,
+                    "velocityX": 1.111119752619376,
+                    "velocityY": 0.6374638404531465,
+                    "timestamp": 0.9490353159770338
+                },
+                {
+                    "x": 2.2487735748291016,
+                    "y": 6.386467456817627,
+                    "heading": -0.27415678824354056,
+                    "angularVelocity": 0.23805517997945658,
+                    "velocityX": 1.3973089651552055,
+                    "velocityY": -0.022537379043057543,
+                    "timestamp": 1.0353112537931277
+                },
+                {
+                    "x": 2.4244121861833303,
+                    "y": 6.383634558684574,
+                    "heading": -0.2535069736087744,
+                    "angularVelocity": 0.24641991696382376,
+                    "velocityX": 2.0959438518486566,
+                    "velocityY": -0.03380575250000802,
+                    "timestamp": 1.1191105460305186
+                },
+                {
+                    "x": 2.658595728295013,
+                    "y": 6.37985738170666,
+                    "heading": -0.2319955871365083,
+                    "angularVelocity": 0.2567012906424869,
+                    "velocityX": 2.79457661108014,
+                    "velocityY": -0.04507409164284618,
+                    "timestamp": 1.2029098382679095
+                },
+                {
+                    "x": 2.9513225155845477,
+                    "y": 6.37513595307082,
+                    "heading": -0.20870362422898678,
+                    "angularVelocity": 0.27794939892259674,
+                    "velocityX": 3.493189255826692,
+                    "velocityY": -0.05634210635652628,
+                    "timestamp": 1.2867091305053004
+                },
+                {
+                    "x": 3.1855040701620707,
+                    "y": 6.371358808150105,
+                    "heading": -0.1849223646442006,
+                    "angularVelocity": 0.2837883107343998,
+                    "velocityX": 2.794552893288424,
+                    "velocityY": -0.04507370909548172,
+                    "timestamp": 1.3705084227426914
+                },
+                {
+                    "x": 3.3611402034786066,
+                    "y": 6.368525949985646,
+                    "heading": -0.1612618078271142,
+                    "angularVelocity": 0.28234793141284903,
+                    "velocityX": 2.0959142807433877,
+                    "velocityY": -0.03380527554378375,
+                    "timestamp": 1.4543077149800823
+                },
+                {
+                    "x": 3.478230941686313,
+                    "y": 6.366637378155631,
+                    "heading": -0.13785841940389754,
+                    "angularVelocity": 0.2792790702446555,
+                    "velocityX": 1.3972759802792332,
+                    "velocityY": -0.022536847025679124,
+                    "timestamp": 1.5381070072174732
+                },
+                {
+                    "x": 3.536776304244995,
+                    "y": 6.365693092346191,
+                    "heading": -0.11477234815509793,
+                    "angularVelocity": 0.2754924371366116,
+                    "velocityX": 0.6986379120342898,
+                    "velocityY": -0.01126842225306853,
+                    "timestamp": 1.621906299454864
+                },
+                {
+                    "x": 3.536776304244995,
+                    "y": 6.365693092346191,
+                    "heading": -0.09203745526508908,
+                    "angularVelocity": 0.27130172920323553,
+                    "velocityX": -5.971595919811331e-16,
+                    "velocityY": 2.294938009446505e-16,
+                    "timestamp": 1.705705591692255
+                },
+                {
+                    "x": 3.5821674028193073,
+                    "y": 6.315334924760779,
+                    "heading": -0.06814456679249249,
+                    "angularVelocity": 0.26497230333398186,
+                    "velocityX": 0.5033876064793726,
+                    "velocityY": -0.558472437189569,
+                    "timestamp": 1.7958768593000762
+                },
+                {
+                    "x": 3.6729493357217975,
+                    "y": 6.214618882752082,
+                    "heading": -0.0452520324499122,
+                    "angularVelocity": 0.2538783689072417,
+                    "velocityX": 1.006772282467237,
+                    "velocityY": -1.1169416232090381,
+                    "timestamp": 1.8860481269078975
+                },
+                {
+                    "x": 3.809120897123708,
+                    "y": 6.063546304100605,
+                    "heading": -0.024382898724538877,
+                    "angularVelocity": 0.23143884165108508,
+                    "velocityX": 1.5101435858055878,
+                    "velocityY": -1.6753959732332042,
+                    "timestamp": 1.9762193945157187
+                },
+                {
+                    "x": 3.9906611673735113,
+                    "y": 5.862140397658975,
+                    "heading": -0.011123644415836652,
+                    "angularVelocity": 0.14704522472038728,
+                    "velocityX": 2.0132828900595015,
+                    "velocityY": -2.2335929369164194,
+                    "timestamp": 2.06639066212354
+                },
+                {
+                    "x": 4.126829478795296,
+                    "y": 5.711071424627299,
+                    "heading": -0.0049044530235942295,
+                    "angularVelocity": 0.06897087683516459,
+                    "velocityX": 1.5101075435028497,
+                    "velocityY": -1.6753559868839316,
+                    "timestamp": 2.1565619297313616
+                },
+                {
+                    "x": 4.217609649564756,
+                    "y": 5.610357337578787,
+                    "heading": -0.0015015365594071913,
+                    "angularVelocity": 0.03773836782456074,
+                    "velocityX": 1.0067527403993772,
+                    "velocityY": -1.1169199426866636,
+                    "timestamp": 2.246733197339183
                 },
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 2.9287791201697906e-29,
-                    "angularVelocity": 1.2278570112200775,
-                    "velocityX": 0.6492025266047246,
-                    "velocityY": 0.0705606216901654,
-                    "timestamp": 1.045976460813726
+                    "heading": 4.052308831945937e-18,
+                    "angularVelocity": 0.016652051138278397,
+                    "velocityX": 0.5033793096117788,
+                    "velocityY": -0.5584632324101773,
+                    "timestamp": 2.3369044649470045
                 },
                 {
                     "x": 4.263,
                     "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 3.0874616220611523e-31,
-                    "velocityY": -3.4071713040637054e-30,
-                    "timestamp": 1.1331411658815367
+                    "heading": 1.897041849287005e-18,
+                    "angularVelocity": -2.8636978124151836e-18,
+                    "velocityX": 2.1901737652514486e-16,
+                    "velocityY": -2.4329458809860463e-16,
+                    "timestamp": 2.427075732554826
                 }
             ],
             "constraints": [
@@ -18687,6 +21023,20 @@
                         "last"
                     ],
                     "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        1,
+                        2
+                    ],
+                    "type": "StraightLine"
+                },
+                {
+                    "scope": [
+                        2,
+                        3
+                    ],
+                    "type": "StraightLine"
                 }
             ],
             "usesControlIntervalGuessing": true,
@@ -19310,16 +21660,25 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
+                    "controlIntervalCount": 12
+                },
+                {
+                    "x": 2.1241281032562256,
+                    "y": 6.282596111297607,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
                     "controlIntervalCount": 30
                 },
                 {
-                    "x": 4.276216983795166,
-                    "y": 6.32326602935791,
+                    "x": 5.676522731781006,
+                    "y": 6.552661418914795,
                     "heading": 3.14,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
-                    "controlIntervalCount": 30
+                    "controlIntervalCount": 27
                 },
                 {
                     "x": 7.68,
@@ -19333,553 +21692,634 @@
             ],
             "trajectory": [
                 {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 1.095053106710567e-32,
+                    "x": 2.2999999999310377,
+                    "y": 4.359999999993798,
+                    "heading": -0.3900000000006091,
+                    "angularVelocity": -1.7156392127364986e-12,
+                    "velocityX": -4.6004235672195495e-11,
+                    "velocityY": -2.541589729484622e-10,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.3031602212725026,
-                    "y": 4.367935645195281,
-                    "heading": -0.3933962364434778,
-                    "angularVelocity": -0.10578752662991632,
-                    "velocityX": 0.0984360180997807,
-                    "velocityY": 0.24718310735800264,
-                    "timestamp": 0.032104318454850914
-                },
-                {
-                    "x": 2.309652031024921,
-                    "y": 4.383743043384139,
-                    "heading": -0.39998627296935113,
-                    "angularVelocity": -0.20526947286363117,
-                    "velocityX": 0.20220986038211264,
-                    "velocityY": 0.4923760711845366,
-                    "timestamp": 0.06420863690970183
-                },
-                {
-                    "x": 2.3196739941431104,
-                    "y": 4.407342390021744,
-                    "heading": -0.40953275493580865,
-                    "angularVelocity": -0.29735818811675285,
-                    "velocityX": 0.31216869257896596,
-                    "velocityY": 0.7350832465356143,
-                    "timestamp": 0.09631295536455274
-                },
-                {
-                    "x": 2.333458307096076,
-                    "y": 4.438632228164309,
-                    "heading": -0.4217541925110665,
-                    "angularVelocity": -0.3806789292987277,
-                    "velocityX": 0.4293600866297902,
-                    "velocityY": 0.9746301945817274,
-                    "timestamp": 0.12841727381940365
-                },
-                {
-                    "x": 2.3512791398330983,
-                    "y": 4.477480960055527,
-                    "heading": -0.4363123371700568,
-                    "angularVelocity": -0.4534637506622021,
-                    "velocityX": 0.5550914517025046,
-                    "velocityY": 1.21007807550416,
-                    "timestamp": 0.16052159227425455
-                },
-                {
-                    "x": 2.3734634122157945,
-                    "y": 4.523713969255995,
-                    "heading": -0.45279460426513807,
-                    "angularVelocity": -0.5133971966500652,
-                    "velocityX": 0.691005866201282,
-                    "velocityY": 1.4400869236793286,
-                    "timestamp": 0.19262591072910545
-                },
-                {
-                    "x": 2.4004044405613056,
-                    "y": 4.577093572392249,
-                    "heading": -0.4706891378053212,
-                    "angularVelocity": -0.5573871180398601,
-                    "velocityX": 0.8391714773013875,
-                    "velocityY": 1.6626923013900174,
-                    "timestamp": 0.22473022918395635
-                },
-                {
-                    "x": 2.432578225951104,
-                    "y": 4.63728707308353,
-                    "heading": -0.48934877664951465,
-                    "angularVelocity": -0.581218968109697,
-                    "velocityX": 1.002163787873114,
-                    "velocityY": 1.8749347000134435,
-                    "timestamp": 0.25683454763880725
-                },
-                {
-                    "x": 2.4705256917588616,
-                    "y": 4.703846202675345,
-                    "heading": -0.5079484621261662,
-                    "angularVelocity": -0.5793515131868818,
-                    "velocityX": 1.1820050271780043,
-                    "velocityY": 2.073214221489191,
-                    "timestamp": 0.28893886609365815
-                },
-                {
-                    "x": 2.5143889510907504,
-                    "y": 4.77663720854218,
-                    "heading": -0.5259955708448517,
-                    "angularVelocity": -0.5621395995079442,
-                    "velocityX": 1.366272870535308,
-                    "velocityY": 2.267327554989265,
-                    "timestamp": 0.32104318454850905
-                },
-                {
-                    "x": 2.564366566619072,
-                    "y": 4.855456871813396,
-                    "heading": -0.5427715655324792,
-                    "angularVelocity": -0.5225463580925951,
-                    "velocityX": 1.5567256348583118,
-                    "velocityY": 2.4551109341275055,
-                    "timestamp": 0.35314750300335995
-                },
-                {
-                    "x": 2.620751841763659,
-                    "y": 4.939963572730219,
-                    "heading": -0.5571329301868887,
-                    "angularVelocity": -0.44733435704626356,
-                    "velocityX": 1.7563143482981505,
-                    "velocityY": 2.6322533847172953,
-                    "timestamp": 0.38525182145821085
-                },
-                {
-                    "x": 2.6840019979529464,
-                    "y": 5.029487269137319,
-                    "heading": -0.5670183536644556,
-                    "angularVelocity": -0.30791569338152147,
-                    "velocityX": 1.9701448039844656,
-                    "velocityY": 2.788525055686805,
-                    "timestamp": 0.41735613991306175
-                },
-                {
-                    "x": 2.7547918742113016,
-                    "y": 5.122370588176559,
-                    "heading": -0.568384562504894,
-                    "angularVelocity": -0.0425552980468726,
-                    "velocityX": 2.2049954543625816,
-                    "velocityY": 2.893172118569162,
-                    "timestamp": 0.44946045836791265
-                },
-                {
-                    "x": 2.8341176789452183,
-                    "y": 5.214333601098647,
-                    "heading": -0.5683844391321649,
-                    "angularVelocity": 0.0000038428702119811485,
-                    "velocityX": 2.4708764599838617,
-                    "velocityY": 2.864506002562167,
-                    "timestamp": 0.48156477682276355
-                },
-                {
-                    "x": 2.9197482153809866,
-                    "y": 5.3004567509455125,
-                    "heading": -0.5683843814173083,
-                    "angularVelocity": 0.0000017977287583592433,
-                    "velocityX": 2.6672591276526463,
-                    "velocityY": 2.6826032755681406,
-                    "timestamp": 0.5136690952776145
-                },
-                {
-                    "x": 3.0112544304014244,
-                    "y": 5.380309105627853,
-                    "heading": -0.5683843265105415,
-                    "angularVelocity": 0.0000017102610943456471,
-                    "velocityX": 2.850277452521701,
-                    "velocityY": 2.487277678691065,
-                    "timestamp": 0.5457734137324655
-                },
-                {
-                    "x": 3.107551803887462,
-                    "y": 5.454313172250514,
-                    "heading": -0.5683842730108181,
-                    "angularVelocity": 0.0000016664338651344868,
-                    "velocityX": 2.999514648518804,
-                    "velocityY": 2.305112526426488,
-                    "timestamp": 0.5778777321873164
-                },
-                {
-                    "x": 3.2038516202489813,
-                    "y": 5.528314059986814,
-                    "heading": -0.5683842195116714,
-                    "angularVelocity": 0.000001666415897857932,
-                    "velocityX": 2.9995907403219917,
-                    "velocityY": 2.305013509019602,
-                    "timestamp": 0.6099820506421674
-                },
-                {
-                    "x": 3.3001514533185667,
-                    "y": 5.602314925980335,
-                    "heading": -0.5683841660125195,
-                    "angularVelocity": 0.000001666416059709123,
-                    "velocityX": 2.9995912607524815,
-                    "velocityY": 2.305012831765639,
-                    "timestamp": 0.6420863690970183
-                },
-                {
-                    "x": 3.3964512995777554,
-                    "y": 5.676315774809768,
-                    "heading": -0.568384112513365,
-                    "angularVelocity": 0.0000016664161452516387,
-                    "velocityX": 2.999591671588281,
-                    "velocityY": 2.30501229713074,
-                    "timestamp": 0.6741906875518693
-                },
-                {
-                    "x": 3.492751156794414,
-                    "y": 5.750316609379858,
-                    "heading": -0.5683840590142086,
-                    "angularVelocity": 0.000001666416198927138,
-                    "velocityX": 2.999592012896559,
-                    "velocityY": 2.3050118529742454,
-                    "timestamp": 0.7062950060067202
-                },
-                {
-                    "x": 3.5890510233460025,
-                    "y": 5.824317431802072,
-                    "heading": -0.5683840055150509,
-                    "angularVelocity": 0.0000016664162437925357,
-                    "velocityX": 2.999592303665221,
-                    "velocityY": 2.305011474586604,
-                    "timestamp": 0.7383993244615712
-                },
-                {
-                    "x": 3.6853508979893648,
-                    "y": 5.898318243694168,
-                    "heading": -0.5683839520158918,
-                    "angularVelocity": 0.0000016664162889267752,
-                    "velocityX": 2.9995925557115117,
-                    "velocityY": 2.3050111465896648,
-                    "timestamp": 0.7705036429164221
-                },
-                {
-                    "x": 3.7816507797340138,
-                    "y": 5.972319046345101,
-                    "heading": -0.5683838985167311,
-                    "angularVelocity": 0.0000016664163350532929,
-                    "velocityX": 2.9995927769056334,
-                    "velocityY": 2.305010858741686,
-                    "timestamp": 0.8026079613712731
-                },
-                {
-                    "x": 3.877950667767949,
-                    "y": 6.046319840811553,
-                    "heading": -0.5683838450175687,
-                    "angularVelocity": 0.0000016664163859759036,
-                    "velocityX": 2.999592972807203,
-                    "velocityY": 2.305010603807763,
-                    "timestamp": 0.8347122798261241
-                },
-                {
-                    "x": 3.9742505615842325,
-                    "y": 6.120320627753221,
-                    "heading": -0.5683837915184047,
-                    "angularVelocity": 0.0000016664164376020863,
-                    "velocityX": 2.999593152918432,
-                    "velocityY": 2.3050103694223405,
-                    "timestamp": 0.866816598280975
-                },
-                {
-                    "x": 4.070552264919673,
-                    "y": 6.194319059842733,
-                    "heading": -0.5683837380195041,
-                    "angularVelocity": 0.0000016664082331173329,
-                    "velocityX": 2.999649516649054,
-                    "velocityY": 2.304937019409932,
-                    "timestamp": 0.898920916735826
-                },
-                {
-                    "x": 4.171102213813095,
-                    "y": 6.262432889296395,
-                    "heading": -0.5683836849080734,
-                    "angularVelocity": 0.0000016543391427080076,
-                    "velocityX": 3.131975812999348,
-                    "velocityY": 2.1216407241116846,
-                    "timestamp": 0.9310252351906769
-                },
-                {
-                    "x": 4.276216983795166,
-                    "y": 6.32326602935791,
-                    "heading": -0.5683836309724583,
-                    "angularVelocity": 0.0000016800112198234746,
-                    "velocityX": 3.274162948822482,
-                    "velocityY": 1.8948584797732355,
-                    "timestamp": 0.9631295536455279
-                },
-                {
-                    "x": 4.413834682670106,
-                    "y": 6.38740625436239,
-                    "heading": -0.5683835821873748,
-                    "angularVelocity": 0.0000012155044166425132,
-                    "velocityX": 3.428812837046335,
-                    "velocityY": 1.5980853383274327,
-                    "timestamp": 1.0032652231659112
-                },
-                {
-                    "x": 4.5565820466376845,
-                    "y": 6.439134291384663,
-                    "heading": -0.5683835352510233,
-                    "angularVelocity": 0.0000011694423477450916,
-                    "velocityX": 3.556620972650851,
-                    "velocityY": 1.2888295533727718,
-                    "timestamp": 1.0434008926862943
-                },
-                {
-                    "x": 4.7007810920363475,
-                    "y": 6.4866657027970485,
-                    "heading": -0.5683834885999294,
-                    "angularVelocity": 0.000001162335011018839,
-                    "velocityX": 3.5927903314389917,
-                    "velocityY": 1.184268556632563,
-                    "timestamp": 1.0835365622066775
-                },
-                {
-                    "x": 4.84498021653845,
-                    "y": 6.534196874227661,
-                    "heading": -0.5683834419488406,
-                    "angularVelocity": 0.0000011623348856187121,
-                    "velocityX": 3.5927923023401758,
-                    "velocityY": 1.1842625773683353,
-                    "timestamp": 1.1236722317270607
-                },
-                {
-                    "x": 4.989179343628637,
-                    "y": 6.581728037806586,
-                    "heading": -0.5683833952977476,
-                    "angularVelocity": 0.0000011623349868626642,
-                    "velocityX": 3.5927923668235673,
-                    "velocityY": 1.1842623817396558,
-                    "timestamp": 1.1638079012474438
-                },
-                {
-                    "x": 5.133378473125504,
-                    "y": 6.629259194084157,
-                    "heading": -0.5683833486466496,
-                    "angularVelocity": 0.0000011623351115999292,
-                    "velocityX": 3.592792426787221,
-                    "velocityY": 1.1842621998228242,
-                    "timestamp": 1.203943570767827
-                },
-                {
-                    "x": 5.277577604867278,
-                    "y": 6.676790343551162,
-                    "heading": -0.5683833019955459,
-                    "angularVelocity": 0.0000011623352597981392,
-                    "velocityX": 3.5927924827201663,
-                    "velocityY": 1.184262030134241,
-                    "timestamp": 1.2440792402882102
-                },
-                {
-                    "x": 5.421776738706852,
-                    "y": 6.7243214866538885,
-                    "heading": -0.5683832553444353,
-                    "angularVelocity": 0.0000011623354265532326,
-                    "velocityX": 3.5927925349879053,
-                    "velocityY": 1.1842618715650601,
-                    "timestamp": 1.2842149098085933
-                },
-                {
-                    "x": 5.565975874510482,
-                    "y": 6.771852623798087,
-                    "heading": -0.5683832086933173,
-                    "angularVelocity": 0.0000011623356113966992,
-                    "velocityX": 3.5927925839233144,
-                    "velocityY": 1.18426172310541,
-                    "timestamp": 1.3243505793289765
-                },
-                {
-                    "x": 5.710175012156514,
-                    "y": 6.819383755352828,
-                    "heading": -0.5683831620421912,
-                    "angularVelocity": 0.0000011623358144790056,
-                    "velocityX": 3.5927926298276605,
-                    "velocityY": 1.1842615838413222,
-                    "timestamp": 1.3644862488493597
-                },
-                {
-                    "x": 5.854374151534237,
-                    "y": 6.866914881653982,
-                    "heading": -0.5683831153910563,
-                    "angularVelocity": 0.000001162336033125057,
-                    "velocityX": 3.592792672973599,
-                    "velocityY": 1.1842614529456208,
-                    "timestamp": 1.4046219183697428
-                },
-                {
-                    "x": 5.998573292523273,
-                    "y": 6.9144460030667485,
-                    "heading": -0.568383068739913,
-                    "angularVelocity": 0.000001162336242705986,
-                    "velocityX": 3.59279271312027,
-                    "velocityY": 1.184261331149038,
-                    "timestamp": 1.444757587890126
-                },
-                {
-                    "x": 6.142772435013044,
-                    "y": 6.9619771199265985,
-                    "heading": -0.5683830220887615,
-                    "angularVelocity": 0.0000011623364467362213,
-                    "velocityX": 3.5927927505118338,
-                    "velocityY": 1.1842612177108516,
-                    "timestamp": 1.4848932574105091
-                },
-                {
-                    "x": 6.286971578902684,
-                    "y": 7.00950823253954,
-                    "heading": -0.5683829754376022,
-                    "angularVelocity": 0.0000011623366413601916,
-                    "velocityX": 3.59279278539024,
-                    "velocityY": 1.1842611118970372,
-                    "timestamp": 1.5250289269308923
-                },
-                {
-                    "x": 6.431170724100105,
-                    "y": 7.057039341184945,
-                    "heading": -0.5683829287864354,
-                    "angularVelocity": 0.0000011623368281432719,
-                    "velocityX": 3.592792817974252,
-                    "velocityY": 1.1842610130439195,
-                    "timestamp": 1.5651645964512755
-                },
-                {
-                    "x": 6.575369870521147,
-                    "y": 7.10457044611814,
-                    "heading": -0.5683828821352613,
-                    "angularVelocity": 0.0000011623370098744323,
-                    "velocityX": 3.5927928484613587,
-                    "velocityY": 1.1842609205523604,
-                    "timestamp": 1.6053002659716586
-                },
-                {
-                    "x": 6.719569018075523,
-                    "y": 7.152101547613029,
-                    "heading": -0.5683828354840802,
-                    "angularVelocity": 0.0000011623371832362076,
-                    "velocityX": 3.5927928766989634,
-                    "velocityY": 1.1842608348853054,
-                    "timestamp": 1.6454359354920418
-                },
-                {
-                    "x": 6.8637678827817385,
-                    "y": 7.1996335071853474,
-                    "heading": -0.5683827888209949,
-                    "angularVelocity": 0.0000011626337817909648,
-                    "velocityX": 3.592785829397527,
-                    "velocityY": 1.1842822143076228,
-                    "timestamp": 1.685571605012425
-                },
-                {
-                    "x": 6.99756534811195,
-                    "y": 7.244965845733408,
-                    "heading": -0.5424482126349314,
-                    "angularVelocity": 0.6461727559544621,
-                    "velocityX": 3.3336298342366253,
-                    "velocityY": 1.1294775716906411,
-                    "timestamp": 1.7257072745328081
-                },
-                {
-                    "x": 7.121516587135396,
-                    "y": 7.285176196921745,
-                    "heading": -0.489772109644447,
-                    "angularVelocity": 1.3124510845329824,
-                    "velocityX": 3.088306249893159,
-                    "velocityY": 1.0018607305881775,
-                    "timestamp": 1.7658429440531913
-                },
-                {
-                    "x": 7.233956124618328,
-                    "y": 7.321947450112657,
-                    "heading": -0.4161805837381191,
-                    "angularVelocity": 1.8335691614401448,
-                    "velocityX": 2.8014865287304875,
-                    "velocityY": 0.9161739079059802,
-                    "timestamp": 1.8059786135735745
-                },
-                {
-                    "x": 7.333707629732149,
-                    "y": 7.35452494917019,
-                    "heading": -0.3377597360881954,
-                    "angularVelocity": 1.953894094381483,
-                    "velocityX": 2.4853579448365832,
-                    "velocityY": 0.8116844554190893,
-                    "timestamp": 1.8461142830939576
-                },
-                {
-                    "x": 7.420722566948463,
-                    "y": 7.382895867936689,
-                    "heading": -0.26184604843223563,
-                    "angularVelocity": 1.8914269666638126,
-                    "velocityX": 2.1680200743162716,
-                    "velocityY": 0.7068754328887092,
-                    "timestamp": 1.8862499526143408
-                },
-                {
-                    "x": 7.495090208476241,
-                    "y": 7.40709623575076,
-                    "heading": -0.19218016370533045,
-                    "angularVelocity": 1.7357598754276362,
-                    "velocityX": 1.8529064649092093,
-                    "velocityY": 0.6029640990984653,
-                    "timestamp": 1.926385622134724
-                },
-                {
-                    "x": 7.556901497509452,
-                    "y": 7.427169942598205,
-                    "heading": -0.13107853540878284,
-                    "angularVelocity": 1.522377202790068,
-                    "velocityX": 1.5400587500308252,
-                    "velocityY": 0.500146305949861,
-                    "timestamp": 1.9665212916551071
-                },
-                {
-                    "x": 7.606235479245265,
-                    "y": 7.443160506733181,
-                    "heading": -0.08018624320935208,
-                    "angularVelocity": 1.2680065589434022,
-                    "velocityX": 1.2291804852229526,
-                    "velocityY": 0.3984127915657456,
-                    "timestamp": 2.0066569611754903
-                },
-                {
-                    "x": 7.643159935464096,
-                    "y": 7.455107253348189,
-                    "heading": -0.040758426269481916,
-                    "angularVelocity": 0.982363503861482,
-                    "velocityX": 0.9199910369024534,
-                    "velocityY": 0.29765908374694494,
-                    "timestamp": 2.0467926306958737
-                },
-                {
-                    "x": 7.667732329164437,
-                    "y": 7.4630440972663505,
-                    "heading": -0.013776014916518935,
-                    "angularVelocity": 0.6722800859036306,
-                    "velocityX": 0.6122333075286244,
-                    "velocityY": 0.1977503804721968,
-                    "timestamp": 2.086928300216257
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 2.662292212835229e-35,
-                    "angularVelocity": 0.3432362056280802,
-                    "velocityX": 0.30565506897381545,
-                    "velocityY": 0.09856326756028964,
-                    "timestamp": 2.1270639697366405
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": -1.5591265591207004e-41,
-                    "angularVelocity": 3.563315968840566e-34,
-                    "velocityX": -7.620676928265867e-35,
-                    "velocityY": -1.1745137620932586e-33,
-                    "timestamp": 2.167199639257024
+                    "x": 2.2830691122580777,
+                    "y": 4.4226688824706715,
+                    "heading": -0.3961360368292833,
+                    "angularVelocity": -0.04017087715885992,
+                    "velocityX": -0.11084167601962927,
+                    "velocityY": 0.4102752396150543,
+                    "timestamp": 0.1527483904267538
+                },
+                {
+                    "x": 2.2499539829007165,
+                    "y": 4.551962677072163,
+                    "heading": -0.39368124618911415,
+                    "angularVelocity": 0.016070811835100392,
+                    "velocityX": -0.21679527487321065,
+                    "velocityY": 0.8464494731937718,
+                    "timestamp": 0.3054967808535076
+                },
+                {
+                    "x": 2.208537052758008,
+                    "y": 4.723131432963358,
+                    "heading": -0.3887340677228252,
+                    "angularVelocity": 0.032387761683442845,
+                    "velocityX": -0.27114478888671906,
+                    "velocityY": 1.120592861369484,
+                    "timestamp": 0.45824517128026143
+                },
+                {
+                    "x": 2.1635048676051025,
+                    "y": 4.925567881437362,
+                    "heading": -0.37513416937632715,
+                    "angularVelocity": 0.08903464258184278,
+                    "velocityX": -0.294812828865451,
+                    "velocityY": 1.3252934968125494,
+                    "timestamp": 0.6109935617070152
+                },
+                {
+                    "x": 2.119479495251874,
+                    "y": 5.143090740041083,
+                    "heading": -0.3554033967273168,
+                    "angularVelocity": 0.12917172215836753,
+                    "velocityX": -0.28822151359768483,
+                    "velocityY": 1.4240599066467436,
+                    "timestamp": 0.763741952133769
+                },
+                {
+                    "x": 2.08196432941898,
+                    "y": 5.365761039079461,
+                    "heading": -0.32917795560380303,
+                    "angularVelocity": 0.1716904580681245,
+                    "velocityX": -0.24560105455579473,
+                    "velocityY": 1.4577587261323537,
+                    "timestamp": 0.9164903425605229
+                },
+                {
+                    "x": 2.052346293594587,
+                    "y": 5.582587972754811,
+                    "heading": -0.29825458565802576,
+                    "angularVelocity": 0.20244645364142877,
+                    "velocityX": -0.19390080434302895,
+                    "velocityY": 1.419503885313628,
+                    "timestamp": 1.0692387329872766
+                },
+                {
+                    "x": 2.030803808580604,
+                    "y": 5.789012383645482,
+                    "heading": -0.2654877856320327,
+                    "angularVelocity": 0.2145148628152058,
+                    "velocityX": -0.14103248420843936,
+                    "velocityY": 1.3514015455604398,
+                    "timestamp": 1.2219871234140305
+                },
+                {
+                    "x": 2.0185215947450232,
+                    "y": 5.980748966625485,
+                    "heading": -0.23040407839794677,
+                    "angularVelocity": 0.2296829912414591,
+                    "velocityX": -0.08040813886408783,
+                    "velocityY": 1.2552445394842306,
+                    "timestamp": 1.3747355138407844
+                },
+                {
+                    "x": 2.017537204929899,
+                    "y": 6.152494473346335,
+                    "heading": -0.19497588875865757,
+                    "angularVelocity": 0.23193821893113326,
+                    "velocityX": -0.006444518152378274,
+                    "velocityY": 1.1243686841111533,
+                    "timestamp": 1.5274839042675383
+                },
+                {
+                    "x": 2.0415938446669073,
+                    "y": 6.276321571691329,
+                    "heading": -0.16155868009406138,
+                    "angularVelocity": 0.21877290194991583,
+                    "velocityX": 0.15749193627933206,
+                    "velocityY": 0.8106605773582652,
+                    "timestamp": 1.6802322946942922
+                },
+                {
+                    "x": 2.1241281032079797,
+                    "y": 6.2825961111606565,
+                    "heading": -0.060349903928264906,
+                    "angularVelocity": 0.6625848944275851,
+                    "velocityX": 0.5403281719922621,
+                    "velocityY": 0.04107761453342325,
+                    "timestamp": 1.8329806851210462
+                },
+                {
+                    "x": 2.1855581068088026,
+                    "y": 6.287266231959223,
+                    "heading": 0.012078838000808553,
+                    "angularVelocity": 0.7458652653001706,
+                    "velocityX": 0.6326011577496066,
+                    "velocityY": 0.048092524107779686,
+                    "timestamp": 1.9300876899626762
+                },
+                {
+                    "x": 2.2551653620091354,
+                    "y": 6.2925580157259535,
+                    "heading": 0.08826667660975476,
+                    "angularVelocity": 0.784576135772582,
+                    "velocityX": 0.7168098264960764,
+                    "velocityY": 0.05449435852189788,
+                    "timestamp": 2.0271946948043063
+                },
+                {
+                    "x": 2.3319122711008053,
+                    "y": 6.29839258064769,
+                    "heading": 0.16657489622034788,
+                    "angularVelocity": 0.8064116460755234,
+                    "velocityX": 0.7903333988427459,
+                    "velocityY": 0.06008387443491743,
+                    "timestamp": 2.1243016996459363
+                },
+                {
+                    "x": 2.4149946395663093,
+                    "y": 6.304708789079692,
+                    "heading": 0.2457212902089042,
+                    "angularVelocity": 0.8150430973252747,
+                    "velocityX": 0.8555754417689245,
+                    "velocityY": 0.06504379984951533,
+                    "timestamp": 2.2214087044875663
+                },
+                {
+                    "x": 2.5038373372195277,
+                    "y": 6.311462917641332,
+                    "heading": 0.32495163133637983,
+                    "angularVelocity": 0.8159075780845946,
+                    "velocityX": 0.9148948410382448,
+                    "velocityY": 0.06955346543875748,
+                    "timestamp": 2.3185157093291964
+                },
+                {
+                    "x": 2.598073257877524,
+                    "y": 6.3186270576568155,
+                    "heading": 0.40431668105413066,
+                    "angularVelocity": 0.8172947961305416,
+                    "velocityX": 0.9704338108518987,
+                    "velocityY": 0.0737757297255109,
+                    "timestamp": 2.4156227141708264
+                },
+                {
+                    "x": 2.697382502344975,
+                    "y": 6.326176889286233,
+                    "heading": 0.48416024389571777,
+                    "angularVelocity": 0.8222224852016302,
+                    "velocityX": 1.0226784849096606,
+                    "velocityY": 0.07774755027256483,
+                    "timestamp": 2.5127297190124565
+                },
+                {
+                    "x": 2.8014852531337993,
+                    "y": 6.334091139837385,
+                    "heading": 0.5646614354219063,
+                    "angularVelocity": 0.8289946917215433,
+                    "velocityX": 1.0720416213516444,
+                    "velocityY": 0.08150030637886811,
+                    "timestamp": 2.6098367238540865
+                },
+                {
+                    "x": 2.910148663901152,
+                    "y": 6.342352107524804,
+                    "heading": 0.6457203378655492,
+                    "angularVelocity": 0.8347379532340086,
+                    "velocityX": 1.119006925166821,
+                    "velocityY": 0.0850707709678732,
+                    "timestamp": 2.7069437286957165
+                },
+                {
+                    "x": 3.0231702594110863,
+                    "y": 6.350944399479571,
+                    "heading": 0.7270565797219191,
+                    "angularVelocity": 0.8375939718950361,
+                    "velocityX": 1.1638871555335075,
+                    "velocityY": 0.08848272107248593,
+                    "timestamp": 2.8040507335373466
+                },
+                {
+                    "x": 3.140351376164449,
+                    "y": 6.359852912614292,
+                    "heading": 0.8083350217640537,
+                    "angularVelocity": 0.83699875417797,
+                    "velocityX": 1.2067215654083316,
+                    "velocityY": 0.09173914083795442,
+                    "timestamp": 2.9011577383789766
+                },
+                {
+                    "x": 3.261472243862432,
+                    "y": 6.3690609392298585,
+                    "heading": 0.8891526159463483,
+                    "angularVelocity": 0.8322529806195065,
+                    "velocityX": 1.2472927986960072,
+                    "velocityY": 0.09482350610318373,
+                    "timestamp": 2.9982647432206067
+                },
+                {
+                    "x": 3.386280226554517,
+                    "y": 6.378549273066906,
+                    "heading": 0.9690024828336246,
+                    "angularVelocity": 0.8222874040565998,
+                    "velocityX": 1.2852624076036416,
+                    "velocityY": 0.09771008689812195,
+                    "timestamp": 3.0953717480622367
+                },
+                {
+                    "x": 3.5144942146424594,
+                    "y": 6.388296543201246,
+                    "heading": 1.0472679478096496,
+                    "angularVelocity": 0.8059713626886263,
+                    "velocityX": 1.3203371728362745,
+                    "velocityY": 0.10037659168026988,
+                    "timestamp": 3.1924787529038667
+                },
+                {
+                    "x": 3.64579126909874,
+                    "y": 6.398278198691388,
+                    "heading": 1.1233630936332748,
+                    "angularVelocity": 0.783621593451087,
+                    "velocityX": 1.3520863383515835,
+                    "velocityY": 0.10279027288912505,
+                    "timestamp": 3.2895857577454968
+                },
+                {
+                    "x": 3.779855631903331,
+                    "y": 6.408470234520327,
+                    "heading": 1.1966377830577097,
+                    "angularVelocity": 0.7545767639735752,
+                    "velocityX": 1.3805838535787394,
+                    "velocityY": 0.10495675240615632,
+                    "timestamp": 3.386692762587127
+                },
+                {
+                    "x": 3.916399996401096,
+                    "y": 6.418850808641588,
+                    "heading": 1.2663868507253695,
+                    "angularVelocity": 0.7182701992663435,
+                    "velocityX": 1.406122708347489,
+                    "velocityY": 0.10689830431744617,
+                    "timestamp": 3.483799767428757
+                },
+                {
+                    "x": 4.055149110841729,
+                    "y": 6.429398995480507,
+                    "heading": 1.3319162676094702,
+                    "angularVelocity": 0.6748165803464835,
+                    "velocityX": 1.4288270430729209,
+                    "velocityY": 0.10862436634220823,
+                    "timestamp": 3.580906772270387
+                },
+                {
+                    "x": 4.195643021249515,
+                    "y": 6.440079827739076,
+                    "heading": 1.392465239071673,
+                    "angularVelocity": 0.6235283604234658,
+                    "velocityX": 1.4467948093350342,
+                    "velocityY": 0.10999033795341527,
+                    "timestamp": 3.678013777112017
+                },
+                {
+                    "x": 4.337478451024624,
+                    "y": 6.45086264692987,
+                    "heading": 1.4461855651355748,
+                    "angularVelocity": 0.5532075282023133,
+                    "velocityX": 1.4606096661273347,
+                    "velocityY": 0.11104059096144768,
+                    "timestamp": 3.775120781953647
+                },
+                {
+                    "x": 4.47965775917095,
+                    "y": 6.461671608979337,
+                    "heading": 1.49351259060778,
+                    "angularVelocity": 0.48736984118650195,
+                    "velocityX": 1.4641508974652757,
+                    "velocityY": 0.11130980792840979,
+                    "timestamp": 3.872227786795277
+                },
+                {
+                    "x": 4.621471854948602,
+                    "y": 6.4724528062914635,
+                    "heading": 1.5350394838354429,
+                    "angularVelocity": 0.42764055328666206,
+                    "velocityX": 1.4603899702927075,
+                    "velocityY": 0.11102388888102613,
+                    "timestamp": 3.969334791636907
+                },
+                {
+                    "x": 4.762134624452166,
+                    "y": 6.483146475819268,
+                    "heading": 1.5711187532761213,
+                    "angularVelocity": 0.37154136862160403,
+                    "velocityX": 1.4485337063274482,
+                    "velocityY": 0.11012253482900584,
+                    "timestamp": 4.0664417964785375
+                },
+                {
+                    "x": 4.901310758087736,
+                    "y": 6.493727126167039,
+                    "heading": 1.6012523506615222,
+                    "angularVelocity": 0.3103133235343918,
+                    "velocityX": 1.433224451514287,
+                    "velocityY": 0.10895867241503827,
+                    "timestamp": 4.163548801320168
+                },
+                {
+                    "x": 5.0385652541984065,
+                    "y": 6.504161687000508,
+                    "heading": 1.6187441808494436,
+                    "angularVelocity": 0.18012943788636607,
+                    "velocityX": 1.413435585164251,
+                    "velocityY": 0.1074542543793728,
+                    "timestamp": 4.2606558061617985
+                },
+                {
+                    "x": 5.1726571421935885,
+                    "y": 6.5143558154807435,
+                    "heading": 1.6310181673546418,
+                    "angularVelocity": 0.12639650992409435,
+                    "velocityX": 1.3808673050498854,
+                    "velocityY": 0.104978301436634,
+                    "timestamp": 4.357762811003429
+                },
+                {
+                    "x": 5.303636473288953,
+                    "y": 6.524313316659959,
+                    "heading": 1.6376756877148975,
+                    "angularVelocity": 0.06855860046187665,
+                    "velocityX": 1.3488144481980282,
+                    "velocityY": 0.1025415325257505,
+                    "timestamp": 4.4548698158450595
+                },
+                {
+                    "x": 5.4313983902221485,
+                    "y": 6.53402621894025,
+                    "heading": 1.6386463375950402,
+                    "angularVelocity": 0.009995672907119661,
+                    "velocityX": 1.3156817797402116,
+                    "velocityY": 0.1000226726538996,
+                    "timestamp": 4.55197682068669
+                },
+                {
+                    "x": 5.555758593734715,
+                    "y": 6.5434805112143435,
+                    "heading": 1.6339151298589827,
+                    "angularVelocity": -0.048721590949420676,
+                    "velocityX": 1.2806512124922533,
+                    "velocityY": 0.09735952745677612,
+                    "timestamp": 4.6490838255283204
+                },
+                {
+                    "x": 5.676522731590665,
+                    "y": 6.552661418256687,
+                    "heading": 1.6211253547169766,
+                    "angularVelocity": -0.13170805962068485,
+                    "velocityX": 1.2436192217305724,
+                    "velocityY": 0.0945442281350229,
+                    "timestamp": 4.746190830369951
+                },
+                {
+                    "x": 5.8040475153273645,
+                    "y": 6.568758338060969,
+                    "heading": 1.594817916740329,
+                    "angularVelocity": -0.24917360707174604,
+                    "velocityX": 1.2078641151592902,
+                    "velocityY": 0.15246363592342288,
+                    "timestamp": 4.851769580629666
+                },
+                {
+                    "x": 5.9283875752902055,
+                    "y": 6.58985033542802,
+                    "heading": 1.559498986776326,
+                    "angularVelocity": -0.33452688097090233,
+                    "velocityX": 1.1776996757735678,
+                    "velocityY": 0.19977502680380196,
+                    "timestamp": 4.957348330889381
+                },
+                {
+                    "x": 6.049721376535089,
+                    "y": 6.61532142807792,
+                    "heading": 1.5174190958344496,
+                    "angularVelocity": -0.3985640187289682,
+                    "velocityX": 1.1492255865917638,
+                    "velocityY": 0.2412520781772084,
+                    "timestamp": 5.0629270811490965
+                },
+                {
+                    "x": 6.167915961542227,
+                    "y": 6.644893212673861,
+                    "heading": 1.4694436889435494,
+                    "angularVelocity": -0.45440400479138554,
+                    "velocityX": 1.119492176613303,
+                    "velocityY": 0.280092203729064,
+                    "timestamp": 5.168505831408812
+                },
+                {
+                    "x": 6.2827599678090085,
+                    "y": 6.678289224223258,
+                    "heading": 1.4161083022197893,
+                    "angularVelocity": -0.5051716053929952,
+                    "velocityX": 1.087756824956101,
+                    "velocityY": 0.31631376267017064,
+                    "timestamp": 5.274084581668527
+                },
+                {
+                    "x": 6.394018817655917,
+                    "y": 6.715173356117064,
+                    "heading": 1.3577889107275223,
+                    "angularVelocity": -0.5523781194839231,
+                    "velocityX": 1.0537996466964168,
+                    "velocityY": 0.34935185331081736,
+                    "timestamp": 5.379663331928242
+                },
+                {
+                    "x": 6.501464569355021,
+                    "y": 6.755149800954155,
+                    "heading": 1.2948750452839908,
+                    "angularVelocity": -0.5958951523626541,
+                    "velocityX": 1.0176834949234126,
+                    "velocityY": 0.3786410134453019,
+                    "timestamp": 5.485242082187957
+                },
+                {
+                    "x": 6.6048776092971675,
+                    "y": 6.797770420027359,
+                    "heading": 1.22768881668393,
+                    "angularVelocity": -0.6363612797096425,
+                    "velocityX": 0.9794872502530397,
+                    "velocityY": 0.4036855817031342,
+                    "timestamp": 5.5908208324476725
+                },
+                {
+                    "x": 6.70404339771049,
+                    "y": 6.8425429323292,
+                    "heading": 1.1564439654233045,
+                    "angularVelocity": -0.6748029419532755,
+                    "velocityX": 0.939258970068964,
+                    "velocityY": 0.4240674597490353,
+                    "timestamp": 5.696399582707388
+                },
+                {
+                    "x": 6.79877778418725,
+                    "y": 6.888932915699562,
+                    "heading": 1.0813771004184407,
+                    "angularVelocity": -0.7110035384939538,
+                    "velocityX": 0.8972864903430491,
+                    "velocityY": 0.4393875033009405,
+                    "timestamp": 5.801978332967103
+                },
+                {
+                    "x": 6.88894352921239,
+                    "y": 6.936373258053616,
+                    "heading": 1.0028508284852347,
+                    "angularVelocity": -0.7437696672526416,
+                    "velocityX": 0.8540141324253476,
+                    "velocityY": 0.44933608637180433,
+                    "timestamp": 5.907557083226818
+                },
+                {
+                    "x": 6.974457896759412,
+                    "y": 6.984282785155121,
+                    "heading": 0.9213875618911144,
+                    "angularVelocity": -0.7715877144447874,
+                    "velocityX": 0.809958131723675,
+                    "velocityY": 0.45378001785130806,
+                    "timestamp": 6.013135833486533
+                },
+                {
+                    "x": 7.055294443748765,
+                    "y": 7.032089281864471,
+                    "heading": 0.8376815844807217,
+                    "angularVelocity": -0.792829780737162,
+                    "velocityX": 0.7656516726874414,
+                    "velocityY": 0.45280415475213037,
+                    "timestamp": 6.118714583746248
+                },
+                {
+                    "x": 7.131475603500295,
+                    "y": 7.0792526481673885,
+                    "heading": 0.752591160441856,
+                    "angularVelocity": -0.805942709434023,
+                    "velocityX": 0.7215576931722669,
+                    "velocityY": 0.44671267879627613,
+                    "timestamp": 6.224293334005964
+                },
+                {
+                    "x": 7.203056823429108,
+                    "y": 7.125284419454981,
+                    "heading": 0.6670903070961578,
+                    "angularVelocity": -0.8098301328249344,
+                    "velocityX": 0.6779888895797812,
+                    "velocityY": 0.4359946600433992,
+                    "timestamp": 6.329872084265679
+                },
+                {
+                    "x": 7.270104931236086,
+                    "y": 7.169760877903185,
+                    "heading": 0.5821807537401323,
+                    "angularVelocity": -0.8042295741051197,
+                    "velocityX": 0.6350530514787667,
+                    "velocityY": 0.4212633543513555,
+                    "timestamp": 6.435450834525394
+                },
+                {
+                    "x": 7.332675465601759,
+                    "y": 7.212330496694183,
+                    "heading": 0.49884759114131233,
+                    "angularVelocity": -0.789298626772507,
+                    "velocityX": 0.592643253784208,
+                    "velocityY": 0.4032025262817465,
+                    "timestamp": 6.541029584785109
+                },
+                {
+                    "x": 7.390806680523043,
+                    "y": 7.252712685445242,
+                    "heading": 0.4183197118253645,
+                    "angularVelocity": -0.7627280973825078,
+                    "velocityX": 0.5505957828143127,
+                    "velocityY": 0.38248405756315146,
+                    "timestamp": 6.646608335044824
+                },
+                {
+                    "x": 7.444527736090263,
+                    "y": 7.290684851015479,
+                    "heading": 0.34198396458440167,
+                    "angularVelocity": -0.7230218869110173,
+                    "velocityX": 0.5088245046789369,
+                    "velocityY": 0.3596572750296197,
+                    "timestamp": 6.75218708530454
+                },
+                {
+                    "x": 7.4938504955203875,
+                    "y": 7.326060981562411,
+                    "heading": 0.27119241907250147,
+                    "angularVelocity": -0.6705094095505424,
+                    "velocityX": 0.46716559024619986,
+                    "velocityY": 0.3350686622032283,
+                    "timestamp": 6.857765835564255
+                },
+                {
+                    "x": 7.538762197286186,
+                    "y": 7.358679050922846,
+                    "heading": 0.2071469006235064,
+                    "angularVelocity": -0.6066137198463124,
+                    "velocityX": 0.4253858037038331,
+                    "velocityY": 0.3089454019040213,
+                    "timestamp": 6.96334458582397
+                },
+                {
+                    "x": 7.57923356269503,
+                    "y": 7.3883979284340455,
+                    "heading": 0.15090824268085423,
+                    "angularVelocity": -0.5326702370042972,
+                    "velocityX": 0.38332870100058564,
+                    "velocityY": 0.28148540800086536,
+                    "timestamp": 7.068923336083685
+                },
+                {
+                    "x": 7.612613012416259,
+                    "y": 7.4137927319874946,
+                    "heading": 0.0952672119425909,
+                    "angularVelocity": -0.5270097494122625,
+                    "velocityX": 0.316156891737545,
+                    "velocityY": 0.2405294965409898,
+                    "timestamp": 7.1745020863434
+                },
+                {
+                    "x": 7.641161660602441,
+                    "y": 7.4358681154587,
+                    "heading": 0.04946855188730579,
+                    "angularVelocity": -0.43378672242570576,
+                    "velocityX": 0.27040145789287734,
+                    "velocityY": 0.20908926766306235,
+                    "timestamp": 7.280080836603116
+                },
+                {
+                    "x": 7.664737893553612,
+                    "y": 7.454419635462092,
+                    "heading": 0.014221249681079469,
+                    "angularVelocity": -0.3338484505242331,
+                    "velocityX": 0.22330471529975876,
+                    "velocityY": 0.17571263220528385,
+                    "timestamp": 7.385659586862831
+                },
+                {
+                    "x": 7.679999999654181,
+                    "y": 7.467000000250119,
+                    "heading": 5.571589197087409e-11,
+                    "angularVelocity": -0.13469802934148392,
+                    "velocityX": 0.1445566071683118,
+                    "velocityY": 0.11915622105321995,
+                    "timestamp": 7.491238337122546
+                },
+                {
+                    "x": 7.6799999998383255,
+                    "y": 7.467000000125839,
+                    "heading": 2.732434354976469e-11,
+                    "angularVelocity": -1.9015024944340445e-11,
+                    "velocityX": 1.5544110263140067e-10,
+                    "velocityY": -2.911726567326558e-11,
+                    "timestamp": 7.596817087382261
                 }
             ],
             "constraints": [
@@ -19894,6 +22334,13 @@
                         "last"
                     ],
                     "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        1,
+                        2
+                    ],
+                    "type": "StraightLine"
                 }
             ],
             "usesControlIntervalGuessing": true,
@@ -19916,7 +22363,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 33
+                    "controlIntervalCount": 12
+                },
+                {
+                    "x": 2.1656765937805176,
+                    "y": 6.261821746826172,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 30
                 },
                 {
                     "x": 5.493278503417969,
@@ -19943,549 +22399,630 @@
                     "y": 4.36,
                     "heading": -0.39,
                     "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -3.138879459525789e-33,
+                    "velocityX": -2.538537990246625e-31,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.304641315068496,
-                    "y": 4.371901353899003,
-                    "heading": -0.39554860460981267,
-                    "angularVelocity": -0.1412451028521288,
-                    "velocityX": 0.11814916908296796,
-                    "velocityY": 0.3029604871416256,
-                    "timestamp": 0.039283518492098594
-                },
-                {
-                    "x": 2.314310378834228,
-                    "y": 4.395560050627161,
-                    "heading": -0.4062436571914542,
-                    "angularVelocity": -0.27225291909092403,
-                    "velocityX": 0.24613538035494495,
-                    "velocityY": 0.6022550330596644,
-                    "timestamp": 0.07856703698419719
-                },
-                {
-                    "x": 2.3294686593461313,
-                    "y": 4.43078385259534,
-                    "heading": -0.42159423390969814,
-                    "angularVelocity": -0.3907637937605655,
-                    "velocityX": 0.385868707635076,
-                    "velocityY": 0.8966559850097278,
-                    "timestamp": 0.11785055547629578
-                },
-                {
-                    "x": 2.3506733572393026,
-                    "y": 4.477309554936099,
-                    "heading": -0.44099148005781696,
-                    "angularVelocity": -0.49377568233887975,
-                    "velocityX": 0.5397861165984255,
-                    "velocityY": 1.1843568022074944,
-                    "timestamp": 0.15713407396839438
-                },
-                {
-                    "x": 2.3786040121360466,
-                    "y": 4.534766426387325,
-                    "heading": -0.4636665715847995,
-                    "angularVelocity": -0.5772164102747018,
-                    "velocityX": 0.7110018646207296,
-                    "velocityY": 1.462620296162777,
-                    "timestamp": 0.19641759246049298
-                },
-                {
-                    "x": 2.4140943920114943,
-                    "y": 4.602616046670193,
-                    "heading": -0.48862953085625704,
-                    "angularVelocity": -0.6354562989682879,
-                    "velocityX": 0.903441983756771,
-                    "velocityY": 1.727177780588029,
-                    "timestamp": 0.23570111095259158
-                },
-                {
-                    "x": 2.458163120855333,
-                    "y": 4.68005234098581,
-                    "heading": -0.5145799632934547,
-                    "angularVelocity": -0.6605933845360225,
-                    "velocityX": 1.1218121628463973,
-                    "velocityY": 1.9712158505149888,
-                    "timestamp": 0.27498462944469015
-                },
-                {
-                    "x": 2.5115097740388146,
-                    "y": 4.766403162212065,
-                    "heading": -0.5400224328341426,
-                    "angularVelocity": -0.6476626971635644,
-                    "velocityX": 1.3579907103842537,
-                    "velocityY": 2.1981437646332993,
-                    "timestamp": 0.31426814793678876
-                },
-                {
-                    "x": 2.5744195202560527,
-                    "y": 4.861351309968927,
-                    "heading": -0.563860333505168,
-                    "angularVelocity": -0.6068168429413633,
-                    "velocityX": 1.6014285031491418,
-                    "velocityY": 2.416997035944537,
-                    "timestamp": 0.35355166642888736
-                },
-                {
-                    "x": 2.6473242968990576,
-                    "y": 4.964346697364103,
-                    "heading": -0.5842892856317753,
-                    "angularVelocity": -0.5200387569839264,
-                    "velocityX": 1.8558616804566808,
-                    "velocityY": 2.6218473127835553,
-                    "timestamp": 0.39283518492098596
-                },
-                {
-                    "x": 2.7309171688764238,
-                    "y": 5.07424676618957,
-                    "heading": -0.5978668700067779,
-                    "angularVelocity": -0.3456305569399124,
-                    "velocityX": 2.127937496081736,
-                    "velocityY": 2.7976126641403014,
-                    "timestamp": 0.43211870341308456
-                },
-                {
-                    "x": 2.826173576921219,
-                    "y": 5.187875652325513,
-                    "heading": -0.5974605263008922,
-                    "angularVelocity": 0.010343872481393205,
-                    "velocityX": 2.4248441000503926,
-                    "velocityY": 2.8925333192550147,
-                    "timestamp": 0.47140222190518316
-                },
-                {
-                    "x": 2.9311441629204515,
-                    "y": 5.293067244130248,
-                    "heading": -0.5974604636085413,
-                    "angularVelocity": 0.000001595894492248037,
-                    "velocityX": 2.672127905761066,
-                    "velocityY": 2.677753822532693,
-                    "timestamp": 0.5106857403972818
-                },
-                {
-                    "x": 3.044820699228354,
-                    "y": 5.388784226639372,
-                    "heading": -0.597460407679312,
-                    "angularVelocity": 0.0000014237326879146737,
-                    "velocityX": 2.8937463005192323,
-                    "velocityY": 2.436568468997834,
-                    "timestamp": 0.5499692588893803
-                },
-                {
-                    "x": 3.1663502143390825,
-                    "y": 5.474310087323753,
-                    "heading": -0.5974603544159754,
-                    "angularVelocity": 0.0000013558698005366095,
-                    "velocityX": 3.0936514796952492,
-                    "velocityY": 2.177143595261183,
-                    "timestamp": 0.5892527773814789
-                },
-                {
-                    "x": 3.289918343405979,
-                    "y": 5.556863181758907,
-                    "heading": -0.5974603016382944,
-                    "angularVelocity": 0.0000013435069733969365,
-                    "velocityX": 3.145546371864123,
-                    "velocityY": 2.1014689519673895,
-                    "timestamp": 0.6285362958735774
-                },
-                {
-                    "x": 3.413486607435696,
-                    "y": 5.639416074177005,
-                    "heading": -0.5974602488606314,
-                    "angularVelocity": 0.0000013435065140042136,
-                    "velocityX": 3.145549807473418,
-                    "velocityY": 2.101463809427605,
-                    "timestamp": 0.667819814365676
-                },
-                {
-                    "x": 3.5370548851900594,
-                    "y": 5.721968946051526,
-                    "heading": -0.5974601960829673,
-                    "angularVelocity": 0.0000013435065431603766,
-                    "velocityX": 3.145550156847558,
-                    "velocityY": 2.1014632864709872,
-                    "timestamp": 0.7071033328577745
-                },
-                {
-                    "x": 3.6606231741521,
-                    "y": 5.804521801149959,
-                    "heading": -0.5974601433053026,
-                    "angularVelocity": 0.0000013435065598734946,
-                    "velocityX": 3.1455504421498475,
-                    "velocityY": 2.1014628594194082,
-                    "timestamp": 0.746386851349873
-                },
-                {
-                    "x": 3.7841914725218415,
-                    "y": 5.887074642166574,
-                    "heading": -0.597460090527637,
-                    "angularVelocity": 0.0000013435065802330538,
-                    "velocityX": 3.1455506816319834,
-                    "velocityY": 2.101462500953099,
-                    "timestamp": 0.7856703698419716
-                },
-                {
-                    "x": 3.9077597789317933,
-                    "y": 5.969627471148282,
-                    "heading": -0.5974600377499704,
-                    "angularVelocity": 0.0000013435066065618314,
-                    "velocityX": 3.14555088630331,
-                    "velocityY": 2.1014621945928877,
-                    "timestamp": 0.8249538883340701
-                },
-                {
-                    "x": 4.031328092300017,
-                    "y": 6.052180289714568,
-                    "heading": -0.5974599849723026,
-                    "angularVelocity": 0.0000013435066364860128,
-                    "velocityX": 3.1455510634328734,
-                    "velocityY": 2.101461929458217,
-                    "timestamp": 0.8642374068261687
-                },
-                {
-                    "x": 4.154896411746533,
-                    "y": 6.134733099182619,
-                    "heading": -0.5974599321946334,
-                    "angularVelocity": 0.0000013435066718879358,
-                    "velocityX": 3.14555121816168,
-                    "velocityY": 2.1014616978538454,
-                    "timestamp": 0.9035209253182672
-                },
-                {
-                    "x": 4.27846473654201,
-                    "y": 6.217285900644126,
-                    "heading": -0.5974598794169629,
-                    "angularVelocity": 0.000001343506708988157,
-                    "velocityX": 3.14555135432466,
-                    "velocityY": 2.1014614940395124,
-                    "timestamp": 0.9428044438103658
-                },
-                {
-                    "x": 4.4020330660739395,
-                    "y": 6.299838695015915,
-                    "heading": -0.5974598266392908,
-                    "angularVelocity": 0.0000013435067467217265,
-                    "velocityX": 3.1455514748956417,
-                    "velocityY": 2.1014613135638847,
-                    "timestamp": 0.9820879623024643
-                },
-                {
-                    "x": 4.525601399950297,
-                    "y": 6.382391482884783,
-                    "heading": -0.5974597738616171,
-                    "angularVelocity": 0.0000013435067857429384,
-                    "velocityX": 3.1455515854872873,
-                    "velocityY": 2.101461148025713,
-                    "timestamp": 1.0213714807945629
-                },
-                {
-                    "x": 4.6491721527053596,
-                    "y": 6.464940649963802,
-                    "heading": -0.5974597210842042,
-                    "angularVelocity": 0.0000013435001464771913,
-                    "velocityX": 3.1456131603868194,
-                    "velocityY": 2.101368977314248,
-                    "timestamp": 1.0606549992866614
-                },
-                {
-                    "x": 4.778837800088318,
-                    "y": 6.537541722502267,
-                    "heading": -0.5974596685786843,
-                    "angularVelocity": 0.0000013365788503576918,
-                    "velocityX": 3.300764604603488,
-                    "velocityY": 1.8481305984081453,
-                    "timestamp": 1.09993851777876
-                },
-                {
-                    "x": 4.914297553492409,
-                    "y": 6.59865432938236,
-                    "heading": -0.5974596146272461,
-                    "angularVelocity": 0.0000013733860988547744,
-                    "velocityX": 3.4482591835906553,
-                    "velocityY": 1.5556805812161723,
-                    "timestamp": 1.1392220362708585
-                },
-                {
-                    "x": 5.054536029054771,
-                    "y": 6.647820017049671,
-                    "heading": -0.5974595570647036,
-                    "angularVelocity": 0.0000014653102566024163,
-                    "velocityX": 3.5699061831898264,
-                    "velocityY": 1.2515601848929776,
-                    "timestamp": 1.178505554762957
-                },
-                {
-                    "x": 5.198501873691458,
-                    "y": 6.684670110841039,
-                    "heading": -0.5974594930097576,
-                    "angularVelocity": 0.0000016305806703071774,
-                    "velocityX": 3.6647899720507366,
-                    "velocityY": 0.9380548180449542,
-                    "timestamp": 1.2177890732550556
-                },
-                {
-                    "x": 5.345115761176417,
-                    "y": 6.7089283098414425,
-                    "heading": -0.5974594180971905,
-                    "angularVelocity": 0.0000019069719319939646,
-                    "velocityX": 3.7321984667555648,
-                    "velocityY": 0.6175159438731035,
-                    "timestamp": 1.2570725917471541
+                    "x": 2.2837844841867163,
+                    "y": 4.387687502531162,
+                    "heading": -0.3845108824411683,
+                    "angularVelocity": 0.0884354987329395,
+                    "velocityX": -0.26124913754506907,
+                    "velocityY": 0.4460749963360829,
+                    "timestamp": 0.06206916495779045
+                },
+                {
+                    "x": 2.252431932443338,
+                    "y": 4.44366896754607,
+                    "heading": -0.37369458929412197,
+                    "angularVelocity": 0.17426194076711193,
+                    "velocityX": -0.5051228216917889,
+                    "velocityY": 0.9019207049574874,
+                    "timestamp": 0.1241383299155809
+                },
+                {
+                    "x": 2.2074803876744995,
+                    "y": 4.528718281766582,
+                    "heading": -0.3577885652255258,
+                    "angularVelocity": 0.2562628976859123,
+                    "velocityX": -0.724217005329779,
+                    "velocityY": 1.3702345484796434,
+                    "timestamp": 0.18620749487337135
+                },
+                {
+                    "x": 2.1512700092503536,
+                    "y": 4.643824218738039,
+                    "heading": -0.3371682293163389,
+                    "angularVelocity": 0.3322154555127859,
+                    "velocityX": -0.905608742482958,
+                    "velocityY": 1.8544785812676818,
+                    "timestamp": 0.2482766598311618
+                },
+                {
+                    "x": 2.087674884006875,
+                    "y": 4.7901724592691535,
+                    "heading": -0.3124919375355934,
+                    "angularVelocity": 0.3975612012535503,
+                    "velocityX": -1.0245848367214008,
+                    "velocityY": 2.357825187926933,
+                    "timestamp": 0.31034582478895223
+                },
+                {
+                    "x": 2.0237484850052736,
+                    "y": 4.968633105417482,
+                    "heading": -0.2850705208032402,
+                    "angularVelocity": 0.44178807225385713,
+                    "velocityX": -1.029922007895696,
+                    "velocityY": 2.8751900604698215,
+                    "timestamp": 0.3724149897467427
+                },
+                {
+                    "x": 1.9724195760606846,
+                    "y": 5.1766438961084935,
+                    "heading": -0.25765844821795814,
+                    "angularVelocity": 0.4416375281458893,
+                    "velocityX": -0.8269630980122488,
+                    "velocityY": 3.351274192787545,
+                    "timestamp": 0.43448415470453317
+                },
+                {
+                    "x": 1.9484287592243066,
+                    "y": 5.401486281993408,
+                    "heading": -0.23420864800659155,
+                    "angularVelocity": 0.3778011227958159,
+                    "velocityX": -0.38651747373332257,
+                    "velocityY": 3.6224490217904175,
+                    "timestamp": 0.49655331966232363
+                },
+                {
+                    "x": 1.9564542032821701,
+                    "y": 5.628890453578366,
+                    "heading": -0.21272720707613346,
+                    "angularVelocity": 0.34608876961188295,
+                    "velocityX": 0.12929840547219124,
+                    "velocityY": 3.663722103228105,
+                    "timestamp": 0.5586224846201141
+                },
+                {
+                    "x": 1.99619273658118,
+                    "y": 5.8512380599288125,
+                    "heading": -0.18843692284079885,
+                    "angularVelocity": 0.39134221077502945,
+                    "velocityX": 0.6402298681838031,
+                    "velocityY": 3.5822554806643043,
+                    "timestamp": 0.6206916495779046
+                },
+                {
+                    "x": 2.0663437221615903,
+                    "y": 6.063242956223716,
+                    "heading": -0.16364621155748288,
+                    "angularVelocity": 0.3994046206362532,
+                    "velocityX": 1.130206691653697,
+                    "velocityY": 3.4156234651942974,
+                    "timestamp": 0.682760814535695
+                },
+                {
+                    "x": 2.1656765937805176,
+                    "y": 6.261821746826172,
+                    "heading": -0.13977158803318993,
+                    "angularVelocity": 0.38464547638239754,
+                    "velocityX": 1.6003577893593972,
+                    "velocityY": 3.1993146796407252,
+                    "timestamp": 0.7448299794934855
+                },
+                {
+                    "x": 2.225814155156259,
+                    "y": 6.362141570414172,
+                    "heading": -0.12770962458788607,
+                    "angularVelocity": 0.3689752694416071,
+                    "velocityX": 1.8396070434406133,
+                    "velocityY": 3.068781803712599,
+                    "timestamp": 0.7775204182423698
+                },
+                {
+                    "x": 2.2936460979223643,
+                    "y": 6.4579721340232545,
+                    "heading": -0.11638786741998439,
+                    "angularVelocity": 0.3463323712222401,
+                    "velocityX": 2.074978047465827,
+                    "velocityY": 2.9314554125487073,
+                    "timestamp": 0.8102108569912542
+                },
+                {
+                    "x": 2.3690076803030626,
+                    "y": 6.549046337769426,
+                    "heading": -0.10608509188235166,
+                    "angularVelocity": 0.31516173940473013,
+                    "velocityX": 2.3053096031972573,
+                    "velocityY": 2.7859584401928488,
+                    "timestamp": 0.8429012957401385
+                },
+                {
+                    "x": 2.4516786592945876,
+                    "y": 6.635038598364521,
+                    "heading": -0.09714822011858378,
+                    "angularVelocity": 0.27337876474752265,
+                    "velocityX": 2.528903928955548,
+                    "velocityY": 2.6305018802439832,
+                    "timestamp": 0.8755917344890228
+                },
+                {
+                    "x": 2.5413546021547084,
+                    "y": 6.715546857152131,
+                    "heading": -0.09001694608551258,
+                    "angularVelocity": 0.21814555896345397,
+                    "velocityX": 2.7431856619836545,
+                    "velocityY": 2.462746352407108,
+                    "timestamp": 0.9082821732379072
+                },
+                {
+                    "x": 2.6375981860796323,
+                    "y": 6.79006985383775,
+                    "heading": -0.08525828181365991,
+                    "angularVelocity": 0.14556746421386957,
+                    "velocityX": 2.9440896974181805,
+                    "velocityY": 2.2796572801553694,
+                    "timestamp": 0.9409726119867915
+                },
+                {
+                    "x": 2.73984178672467,
+                    "y": 6.858044565031264,
+                    "heading": -0.08306946201564708,
+                    "angularVelocity": 0.06695596274838787,
+                    "velocityX": 3.1276301131997113,
+                    "velocityY": 2.079345331378129,
+                    "timestamp": 0.9736630507356758
+                },
+                {
+                    "x": 2.847369129600613,
+                    "y": 6.918863246924361,
+                    "heading": -0.08274471887343776,
+                    "angularVelocity": 0.009933887583435854,
+                    "velocityX": 3.2892597037781757,
+                    "velocityY": 1.8604425091962247,
+                    "timestamp": 1.0063534894845603
+                },
+                {
+                    "x": 2.9591159211859868,
+                    "y": 6.971834136920771,
+                    "heading": -0.08274467756065068,
+                    "angularVelocity": 0.0000012637574442965605,
+                    "velocityX": 3.418332572524967,
+                    "velocityY": 1.6203786802362972,
+                    "timestamp": 1.0390439282334447
+                },
+                {
+                    "x": 3.0743869446602656,
+                    "y": 7.0166209198254075,
+                    "heading": -0.0827446619031275,
+                    "angularVelocity": 4.789633846658565e-7,
+                    "velocityX": 3.526138769803835,
+                    "velocityY": 1.3700269748141811,
+                    "timestamp": 1.0717343669823292
+                },
+                {
+                    "x": 3.192583723464696,
+                    "y": 7.052991069898469,
+                    "heading": -0.08274464937093684,
+                    "angularVelocity": 3.833595125790563e-7,
+                    "velocityX": 3.61563757869459,
+                    "velocityY": 1.1125623107197633,
+                    "timestamp": 1.1044248057312136
+                },
+                {
+                    "x": 3.313092599420772,
+                    "y": 7.080755764680444,
+                    "heading": -0.08274463888121661,
+                    "angularVelocity": 3.208803751652956e-7,
+                    "velocityX": 3.6863645936910636,
+                    "velocityY": 0.8493215706001247,
+                    "timestamp": 1.137115244480098
+                },
+                {
+                    "x": 3.4352879115666495,
+                    "y": 7.0997708632875725,
+                    "heading": -0.08274462977736693,
+                    "angularVelocity": 2.7848661636087506e-7,
+                    "velocityX": 3.7379526498424447,
+                    "velocityY": 0.5816715631449295,
+                    "timestamp": 1.1698056832289825
+                },
+                {
+                    "x": 3.5585352449602814,
+                    "y": 7.109937657437644,
+                    "heading": -0.08274462163132143,
+                    "angularVelocity": 2.491874023135916e-7,
+                    "velocityX": 3.770133963033834,
+                    "velocityY": 0.31100207091115845,
+                    "timestamp": 1.202496121977867
+                },
+                {
+                    "x": 3.6821947259931562,
+                    "y": 7.1112033907952945,
+                    "heading": -0.08274461414532981,
+                    "angularVelocity": 2.2899636479565772e-7,
+                    "velocityX": 3.782741552745206,
+                    "velocityY": 0.03871876321990418,
+                    "timestamp": 1.2351865607267514
+                },
+                {
+                    "x": 3.8056243495254822,
+                    "y": 7.103561554173401,
+                    "heading": -0.08274460709859606,
+                    "angularVelocity": 2.1555947310703947e-7,
+                    "velocityX": 3.775710215468651,
+                    "velocityY": -0.23376366040158306,
+                    "timestamp": 1.2678769994756358
+                },
+                {
+                    "x": 3.9281833350664277,
+                    "y": 7.087052015079957,
+                    "heading": -0.08274460031581693,
+                    "angularVelocity": 2.0748510569540933e-7,
+                    "velocityX": 3.7490774131963343,
+                    "velocityY": -0.5050265375893709,
+                    "timestamp": 1.3005674382245203
+                },
+                {
+                    "x": 4.0492357154991785,
+                    "y": 7.0617618768818975,
+                    "heading": -0.08274459364684544,
+                    "angularVelocity": 2.0400373145008536e-7,
+                    "velocityX": 3.7029903869628296,
+                    "velocityY": -0.7736249241717706,
+                    "timestamp": 1.3332578769734047
+                },
+                {
+                    "x": 4.169706806774688,
+                    "y": 7.033833646086136,
+                    "heading": -0.0827445869919534,
+                    "angularVelocity": 2.0357304165195936e-7,
+                    "velocityX": 3.6852087609158533,
+                    "velocityY": -0.8543241346506245,
+                    "timestamp": 1.3659483157222891
+                },
+                {
+                    "x": 4.290177832890272,
+                    "y": 7.00590513421804,
+                    "heading": -0.08274458033705716,
+                    "angularVelocity": 2.0357317018224517e-7,
+                    "velocityX": 3.6852067676748597,
+                    "velocityY": -0.8543327326486478,
+                    "timestamp": 1.3986387544711736
+                },
+                {
+                    "x": 4.410648856777724,
+                    "y": 6.977976612738755,
+                    "heading": -0.08274457368215653,
+                    "angularVelocity": 2.0357330400976238e-7,
+                    "velocityX": 3.685206699516314,
+                    "velocityY": -0.8543330266547546,
+                    "timestamp": 1.431329193220058
+                },
+                {
+                    "x": 4.531119878449975,
+                    "y": 6.950048081704109,
+                    "heading": -0.08274456702725173,
+                    "angularVelocity": 2.0357343141081232e-7,
+                    "velocityX": 3.6852066317533563,
+                    "velocityY": -0.8543333189530982,
+                    "timestamp": 1.4640196319689425
+                },
+                {
+                    "x": 4.651590897922017,
+                    "y": 6.922119541178764,
+                    "heading": -0.082744560372343,
+                    "angularVelocity": 2.0357355233562972e-7,
+                    "velocityX": 3.6852065644490066,
+                    "velocityY": -0.8543336092734171,
+                    "timestamp": 1.496710070717827
+                },
+                {
+                    "x": 4.772061915216332,
+                    "y": 6.894190991259702,
+                    "heading": -0.08274455371743049,
+                    "angularVelocity": 2.0357366776279015e-7,
+                    "velocityX": 3.68520649783238,
+                    "velocityY": -0.8543338966271132,
+                    "timestamp": 1.5294005094667114
+                },
+                {
+                    "x": 4.892532930362486,
+                    "y": 6.866262432074455,
+                    "heading": -0.08274454706251447,
+                    "angularVelocity": 2.0357377502591166e-7,
+                    "velocityX": 3.6852064321201583,
+                    "velocityY": -0.8543341800795031,
+                    "timestamp": 1.5620909482155958
+                },
+                {
+                    "x": 5.0130039433965745,
+                    "y": 6.838333863778735,
+                    "heading": -0.08274454040759514,
+                    "angularVelocity": 2.0357387593869802e-7,
+                    "velocityX": 3.685206367512119,
+                    "velocityY": -0.854334458768776,
+                    "timestamp": 1.5947813869644802
+                },
+                {
+                    "x": 5.133474954360567,
+                    "y": 6.810405286553609,
+                    "heading": -0.08274453375267282,
+                    "angularVelocity": 2.0357396812707888e-7,
+                    "velocityX": 3.6852063041879153,
+                    "velocityY": -0.8543347319191732,
+                    "timestamp": 1.6274718257133647
+                },
+                {
+                    "x": 5.253945963272805,
+                    "y": 6.782476700477742,
+                    "heading": -0.08274452709774775,
+                    "angularVelocity": 2.035740515232947e-7,
+                    "velocityX": 3.6852062414248348,
+                    "velocityY": -0.8543350026628553,
+                    "timestamp": 1.6601622644622491
+                },
+                {
+                    "x": 5.374416652936201,
+                    "y": 6.754546737348104,
+                    "heading": -0.08274452044281651,
+                    "angularVelocity": 2.0357424100563973e-7,
+                    "velocityX": 3.685196475607398,
+                    "velocityY": -0.8543771267177642,
+                    "timestamp": 1.6928527032111336
                 },
                 {
                     "x": 5.493278503417969,
                     "y": 6.720412731170654,
-                    "heading": -0.5974593280342978,
-                    "angularVelocity": 0.0000022926381384477543,
-                    "velocityX": 3.7716260642834416,
-                    "velocityY": 0.2923470648746024,
-                    "timestamp": 1.2963561102392527
-                },
-                {
-                    "x": 5.617528419554277,
-                    "y": 6.721027319451783,
-                    "heading": -0.5974592417476945,
-                    "angularVelocity": 0.0000026270679336343826,
-                    "velocityX": 3.7828927995748978,
-                    "velocityY": 0.018711655155499447,
-                    "timestamp": 1.3292013222818118
-                },
-                {
-                    "x": 5.7414972044827435,
-                    "y": 6.712651079745108,
-                    "heading": -0.5974591685782128,
-                    "angularVelocity": 0.0000022277061710620633,
-                    "velocityX": 3.7743335244066216,
-                    "velocityY": -0.25502163589934396,
-                    "timestamp": 1.3620465343243708
-                },
-                {
-                    "x": 5.864535126960113,
-                    "y": 6.695327921921503,
-                    "heading": -0.597459104000633,
-                    "angularVelocity": 0.000001966118527479946,
-                    "velocityX": 3.745992637160019,
-                    "velocityY": -0.5274180541456184,
-                    "timestamp": 1.39489174636693
-                },
-                {
-                    "x": 5.985997338800097,
-                    "y": 6.669148654285341,
-                    "heading": -0.5974590451258569,
-                    "angularVelocity": 0.0000017924918823644274,
-                    "velocityX": 3.6980188065957362,
-                    "velocityY": -0.7970497374814797,
-                    "timestamp": 1.427736958409489
-                },
-                {
-                    "x": 6.105247258928482,
-                    "y": 6.634250515985412,
-                    "heading": -0.5974589899613981,
-                    "angularVelocity": 0.0000016795281661896423,
-                    "velocityX": 3.6306637318675383,
-                    "velocityY": -1.062503059946642,
-                    "timestamp": 1.460582170452048
-                },
-                {
-                    "x": 6.221659925637703,
-                    "y": 6.5908164867561565,
-                    "heading": -0.5974589370314296,
-                    "angularVelocity": 0.0000016114972403778573,
-                    "velocityX": 3.544281174328253,
-                    "velocityY": -1.3223854110871922,
-                    "timestamp": 1.4934273824946072
-                },
-                {
-                    "x": 6.334625365293315,
-                    "y": 6.539074496952364,
-                    "heading": -0.5974588851619594,
-                    "angularVelocity": 0.000001579209477235118,
-                    "velocityX": 3.439327458421827,
-                    "velocityY": -1.5753282316046957,
-                    "timestamp": 1.5262725945371662
-                },
-                {
-                    "x": 6.443562887659525,
-                    "y": 6.4793163144818955,
-                    "heading": -0.5974588333468978,
-                    "angularVelocity": 0.000001577552964572491,
-                    "velocityX": 3.3166941417319027,
-                    "velocityY": -1.819387933752184,
-                    "timestamp": 1.5591178065797253
-                },
-                {
-                    "x": 6.552407332287041,
-                    "y": 6.419388766569548,
-                    "heading": -0.5974587815290724,
-                    "angularVelocity": 0.0000015776371105880298,
-                    "velocityX": 3.313860312014723,
-                    "velocityY": -1.82454440649205,
-                    "timestamp": 1.5919630186222844
-                },
-                {
-                    "x": 6.661251765496281,
-                    "y": 6.359461197918566,
-                    "heading": -0.5974587297112393,
-                    "angularVelocity": 0.0000015776373457032493,
-                    "velocityX": 3.3138599643757076,
-                    "velocityY": -1.8245450378971961,
-                    "timestamp": 1.6248082306648435
-                },
-                {
-                    "x": 6.7700961954772865,
-                    "y": 6.299533623404254,
-                    "heading": -0.5974586778933993,
-                    "angularVelocity": 0.0000015776375522202912,
-                    "velocityX": 3.3138598660894165,
-                    "velocityY": -1.8245452164111537,
-                    "timestamp": 1.6576534427074026
-                },
-                {
-                    "x": 6.878940549868732,
-                    "y": 6.239605911600315,
-                    "heading": -0.5974586260742238,
-                    "angularVelocity": 0.0000015776782163840074,
-                    "velocityX": 3.313857564701912,
-                    "velocityY": -1.8245493963088628,
-                    "timestamp": 1.6904986547499616
-                },
-                {
-                    "x": 6.983465419726436,
-                    "y": 6.181138244955353,
-                    "heading": -0.5831453971579332,
-                    "angularVelocity": 0.4357782466962171,
-                    "velocityX": 3.182347238991671,
-                    "velocityY": -1.7800970981449478,
-                    "timestamp": 1.7233438667925207
-                },
-                {
-                    "x": 7.08190934617205,
-                    "y": 6.126284607678917,
-                    "heading": -0.5460039664027865,
-                    "angularVelocity": 1.1308019782912204,
-                    "velocityX": 2.997207821891462,
-                    "velocityY": -1.67006494600528,
-                    "timestamp": 1.7561890788350798
-                },
-                {
-                    "x": 7.174234078294548,
-                    "y": 6.075101338854014,
-                    "heading": -0.4864629486170815,
-                    "angularVelocity": 1.8127761729340726,
-                    "velocityX": 2.8109038237566843,
-                    "velocityY": -1.5583175032799648,
-                    "timestamp": 1.7890342908776389
-                },
-                {
-                    "x": 7.258956033298187,
-                    "y": 6.028257642494675,
-                    "heading": -0.41936488473987615,
-                    "angularVelocity": 2.0428567728619895,
-                    "velocityX": 2.5794309043853687,
-                    "velocityY": -1.4261955836547433,
-                    "timestamp": 1.821879502920198
-                },
-                {
-                    "x": 7.335844716602702,
-                    "y": 5.985834491701992,
-                    "heading": -0.35186229403264746,
-                    "angularVelocity": 2.0551729311350044,
-                    "velocityX": 2.3409403843979617,
-                    "velocityY": -1.2916083701234404,
-                    "timestamp": 1.854724714962757
-                },
-                {
-                    "x": 7.404915151007572,
-                    "y": 5.947794038432469,
-                    "heading": -0.2870870788105502,
-                    "angularVelocity": 1.9721356993583767,
-                    "velocityX": 2.1029072461275145,
-                    "velocityY": -1.1581734719873862,
-                    "timestamp": 1.8875699270053161
-                },
-                {
-                    "x": 7.466209510665893,
-                    "y": 5.914092129035952,
-                    "heading": -0.22688750133470567,
-                    "angularVelocity": 1.8328265744751362,
-                    "velocityX": 1.866158135283738,
-                    "velocityY": -1.0260828687243908,
-                    "timestamp": 1.9204151390478752
-                },
-                {
-                    "x": 7.519766848297347,
-                    "y": 5.8846886900937605,
-                    "heading": -0.17246123732016092,
-                    "angularVelocity": 1.657053208974533,
-                    "velocityX": 1.6305980172109753,
-                    "velocityY": -0.8952123342686217,
-                    "timestamp": 1.9532603510904343
-                },
-                {
-                    "x": 7.565618503926309,
-                    "y": 5.859549729252428,
-                    "heading": -0.12462132977080041,
-                    "angularVelocity": 1.4565260680124767,
-                    "velocityX": 1.395992072438578,
-                    "velocityY": -0.7653767254946843,
-                    "timestamp": 1.9861055631329934
-                },
-                {
-                    "x": 7.603788954978725,
-                    "y": 5.838647308998552,
-                    "heading": -0.08394075693399232,
-                    "angularVelocity": 1.2385541242393951,
-                    "velocityX": 1.1621313634075436,
-                    "velocityY": -0.6363916977236806,
-                    "timestamp": 2.0189507751755524
-                },
-                {
-                    "x": 7.634297586862898,
-                    "y": 5.821958675739452,
-                    "heading": -0.050840342274680116,
-                    "angularVelocity": 1.0077698574891136,
-                    "velocityX": 0.928860859374263,
-                    "velocityY": -0.5080994221459559,
-                    "timestamp": 2.0517959872181115
-                },
-                {
-                    "x": 7.657160251333572,
-                    "y": 5.809465047264352,
-                    "heading": -0.025644988874275507,
-                    "angularVelocity": 0.7670936442051631,
-                    "velocityX": 0.6960729752956952,
-                    "velocityY": -0.3803789867120345,
-                    "timestamp": 2.0846411992606706
-                },
+                    "heading": -0.08274451374626977,
+                    "angularVelocity": 2.0484725764732438e-7,
+                    "velocityX": 3.6359821106976624,
+                    "velocityY": -1.0441587046150123,
+                    "timestamp": 1.725543141960018
+                },
+                {
+                    "x": 5.6078450645507765,
+                    "y": 6.6784969077855525,
+                    "heading": -0.0827445073182203,
+                    "angularVelocity": 1.9932954157588389e-7,
+                    "velocityX": 3.5526329088991675,
+                    "velocityY": -1.2997818219306485,
+                    "timestamp": 1.7577914952435287
+                },
+                {
+                    "x": 5.720102544926552,
+                    "y": 6.630740323854381,
+                    "heading": -0.08274450093221485,
+                    "angularVelocity": 1.980257845223315e-7,
+                    "velocityX": 3.481029849458213,
+                    "velocityY": -1.4808999241394636,
+                    "timestamp": 1.7900398485270395
+                },
+                {
+                    "x": 5.832359462510262,
+                    "y": 6.582982417035901,
+                    "heading": -0.08274449454621227,
+                    "angularVelocity": 1.9802569455219195e-7,
+                    "velocityX": 3.481012397650423,
+                    "velocityY": -1.480940945995501,
+                    "timestamp": 1.8222882018105502
+                },
+                {
+                    "x": 5.944616376948871,
+                    "y": 6.535224502824747,
+                    "heading": -0.08274448816020867,
+                    "angularVelocity": 1.9802572730548282e-7,
+                    "velocityX": 3.4810123001228868,
+                    "velocityY": -1.4809411752374333,
+                    "timestamp": 1.854536555094061
+                },
+                {
+                    "x": 6.0568732883758765,
+                    "y": 6.487466581534697,
+                    "heading": -0.0827444817742042,
+                    "angularVelocity": 1.9802575385283685e-7,
+                    "velocityX": 3.4810122067350586,
+                    "velocityY": -1.4809413947492354,
+                    "timestamp": 1.8867849083775716
+                },
+                {
+                    "x": 6.169130196876316,
+                    "y": 6.439708653365641,
+                    "heading": -0.08274447538819907,
+                    "angularVelocity": 1.980257744166014e-7,
+                    "velocityX": 3.4810121159842042,
+                    "velocityY": -1.4809416080626625,
+                    "timestamp": 1.9190332616610823
+                },
+                {
+                    "x": 6.281387102537832,
+                    "y": 6.391950718523587,
+                    "heading": -0.08274446900219351,
+                    "angularVelocity": 1.9802578764982062e-7,
+                    "velocityX": 3.481012027951073,
+                    "velocityY": -1.4809418149879348,
+                    "timestamp": 1.951281614944593
+                },
+                {
+                    "x": 6.393644005449787,
+                    "y": 6.344192777218584,
+                    "heading": -0.08274446261618765,
+                    "angularVelocity": 1.9802579747327125e-7,
+                    "velocityX": 3.481011942689016,
+                    "velocityY": -1.4809420153996509,
+                    "timestamp": 1.9835299682281038
+                },
+                {
+                    "x": 6.505900905702341,
+                    "y": 6.296434829662559,
+                    "heading": -0.08274445623018166,
+                    "angularVelocity": 1.9802580083930334e-7,
+                    "velocityX": 3.4810118602227385,
+                    "velocityY": -1.4809422092397408,
+                    "timestamp": 2.0157783215116143
+                },
+                {
+                    "x": 6.618157803385852,
+                    "y": 6.2486768760679015,
+                    "heading": -0.08274444984417571,
+                    "angularVelocity": 1.9802579953185327e-7,
+                    "velocityX": 3.4810117805584304,
+                    "velocityY": -1.4809423964936643,
+                    "timestamp": 2.048026674795125
+                },
+                {
+                    "x": 6.730414698590335,
+                    "y": 6.200918916646197,
+                    "heading": -0.08274444345816992,
+                    "angularVelocity": 1.9802579502845229e-7,
+                    "velocityX": 3.4810117036854167,
+                    "velocityY": -1.4809425771865148,
+                    "timestamp": 2.0802750280786353
+                },
+                {
+                    "x": 6.842671592155369,
+                    "y": 6.153160953370972,
+                    "heading": -0.08274443707212797,
+                    "angularVelocity": 1.980269160510039e-7,
+                    "velocityX": 3.4810116528472412,
+                    "velocityY": -1.4809426966816255,
+                    "timestamp": 2.112523381362146
+                },
+                {
+                    "x": 6.954235049253699,
+                    "y": 6.105720053055757,
+                    "heading": -0.08066018407058251,
+                    "angularVelocity": 0.06463130018523208,
+                    "velocityX": 3.459508648938503,
+                    "velocityY": -1.4711107850419303,
+                    "timestamp": 2.1447717346456563
+                },
+                {
+                    "x": 7.05792262192444,
+                    "y": 6.061617369901677,
+                    "heading": -0.07347223812764758,
+                    "angularVelocity": 0.2228934258982459,
+                    "velocityX": 3.215282707900597,
+                    "velocityY": -1.3675948897716332,
+                    "timestamp": 2.177020087929167
+                },
+                {
+                    "x": 7.153639251658588,
+                    "y": 6.020903567478063,
+                    "heading": -0.06480991744029473,
+                    "angularVelocity": 0.2686128067130366,
+                    "velocityX": 2.9681090656833784,
+                    "velocityY": -1.2625079508922343,
+                    "timestamp": 2.2092684412126773
+                },
+                {
+                    "x": 7.241376863236811,
+                    "y": 5.983582900712933,
+                    "heading": -0.05573995757660787,
+                    "angularVelocity": 0.28125342661540725,
+                    "velocityX": 2.7206850162822076,
+                    "velocityY": -1.1572890695232254,
+                    "timestamp": 2.241516794496188
+                },
+                {
+                    "x": 7.321134939051253,
+                    "y": 5.949655957408732,
+                    "heading": -0.04677486060395727,
+                    "angularVelocity": 0.2780016980662186,
+                    "velocityX": 2.4732449162054873,
+                    "velocityY": -1.0520519607910965,
+                    "timestamp": 2.2737651477796983
+                },
+                {
+                    "x": 7.392914197093691,
+                    "y": 5.919122647933878,
+                    "heading": -0.03821600737741098,
+                    "angularVelocity": 0.26540434952773495,
+                    "velocityX": 2.2258270805765674,
+                    "velocityY": -0.946817631474723,
+                    "timestamp": 2.306013501063209
+                },
+                {
+                    "x": 7.456715523277294,
+                    "y": 5.891982736437705,
+                    "heading": -0.030261865024148932,
+                    "angularVelocity": 0.24665266729552143,
+                    "velocityX": 1.9784367165261438,
+                    "velocityY": -0.8415906157307577,
+                    "timestamp": 2.3382618543467193
+                },
+                {
+                    "x": 7.512539743308544,
+                    "y": 5.868235970303718,
+                    "heading": -0.023053012750234723,
+                    "angularVelocity": 0.2235417173255794,
+                    "velocityX": 1.7310719570848203,
+                    "velocityY": -0.736371433456386,
+                    "timestamp": 2.37051020763023
+                },
+                {
+                    "x": 7.560387579465668,
+                    "y": 5.847882114762634,
+                    "heading": -0.016694240391467878,
+                    "angularVelocity": 0.19718130420078028,
+                    "velocityX": 1.4837295950113008,
+                    "velocityY": -0.6311595312214304,
+                    "timestamp": 2.4027585609137403
+                },
+                {
+                    "x": 7.600259653459209,
+                    "y": 5.830920960662069,
+                    "heading": -0.011266668966831511,
+                    "angularVelocity": 0.1683053821980557,
+                    "velocityX": 1.2364065117684333,
+                    "velocityY": -0.5259541146628961,
+                    "timestamp": 2.435006914197251
+                },
+                {
+                    "x": 7.632156499892169,
+                    "y": 5.817352324088122,
+                    "heading": -0.006834955017927562,
+                    "angularVelocity": 0.1374245038171387,
+                    "velocityX": 0.9891000062092736,
+                    "velocityY": -0.4207544011521409,
+                    "timestamp": 2.4672552674807613
+                },
+                {
+                    "x": 7.656078580521334,
+                    "y": 5.807176043662066,
+                    "heading": -0.0034518424131983775,
+                    "angularVelocity": 0.10490807313441347,
+                    "velocityX": 0.7418078194211917,
+                    "velocityY": -0.31555969188850075,
+                    "timestamp": 2.499503620764272
+                },
                 {
-                    "x": 7.672390418071531,
-                    "y": 5.801150340997017,
-                    "heading": -0.00862072816733467,
-                    "angularVelocity": 0.5183178810016766,
-                    "velocityX": 0.46369518693407513,
-                    "velocityY": -0.2531481987862983,
-                    "timestamp": 2.1174864113032297
+                    "x": 7.672026296758129,
+                    "y": 5.800391977434502,
+                    "heading": -0.0011611797809329624,
+                    "angularVelocity": 0.07103192563441285,
+                    "velocityX": 0.49452808013452626,
+                    "velocityY": -0.21036938438134845,
+                    "timestamp": 2.5317519740477823
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 8.329701949714689e-33,
-                    "angularVelocity": 0.26246529193262896,
-                    "velocityX": 0.2316800975013136,
-                    "velocityY": -0.12636060901771562,
-                    "timestamp": 2.1503316233457888
+                    "heading": 1.9934013230001416e-31,
+                    "angularVelocity": 0.036007413176266045,
+                    "velocityX": 0.2472592374491084,
+                    "velocityY": -0.10518296561320875,
+                    "timestamp": 2.564000327331293
                 },
                 {
                     "x": 7.68,
                     "y": 5.797,
-                    "heading": 5.789840595031671e-40,
-                    "angularVelocity": 1.2947513524749855e-30,
-                    "velocityX": 9.16355000594536e-32,
-                    "velocityY": -1.9486684829089695e-31,
-                    "timestamp": 2.183176835388348
+                    "heading": 1.0109342582388432e-32,
+                    "angularVelocity": -5.855934233748412e-30,
+                    "velocityX": 8.513095472636782e-32,
+                    "velocityY": -2.1397535642954256e-34,
+                    "timestamp": 2.5962486806148033
                 }
             ],
             "constraints": [
@@ -20500,6 +23037,13 @@
                         "last"
                     ],
                     "type": "StopPoint"
+                },
+                {
+                    "scope": [
+                        1,
+                        2
+                    ],
+                    "type": "StraightLine"
                 }
             ],
             "usesControlIntervalGuessing": true,
@@ -20522,15 +23066,24 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 11
+                    "controlIntervalCount": 12
                 },
                 {
-                    "x": 3.558790922164917,
-                    "y": 5.477728366851807,
+                    "x": 2.477290153503418,
+                    "y": 6.3241448402404785,
                     "heading": 0.002,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
+                    "controlIntervalCount": 9
+                },
+                {
+                    "x": 3.9730355739593506,
+                    "y": 5.887885093688965,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
                     "controlIntervalCount": 11
                 },
                 {
@@ -20557,307 +23110,397 @@
                     "x": 2.3,
                     "y": 4.36,
                     "heading": -0.39,
-                    "angularVelocity": 0,
+                    "angularVelocity": 1.5561411635921053e-32,
                     "velocityX": 0,
-                    "velocityY": 1.9823938961383557e-29,
+                    "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 2.312764146242865,
-                    "y": 4.398513079462153,
-                    "heading": -0.39804543849617763,
-                    "angularVelocity": -0.11524733034669515,
-                    "velocityX": 0.18284072142228638,
-                    "velocityY": 0.5516827447028667,
-                    "timestamp": 0.06981019405093988
-                },
-                {
-                    "x": 2.339895425931373,
-                    "y": 4.474992910905513,
-                    "heading": -0.4126392714825622,
-                    "angularVelocity": -0.20905017072795415,
-                    "velocityX": 0.3886435220157258,
-                    "velocityY": 1.0955395910772,
-                    "timestamp": 0.13962038810187977
-                },
-                {
-                    "x": 2.3838571031189573,
-                    "y": 4.588440455462814,
-                    "heading": -0.43142102712692415,
-                    "angularVelocity": -0.2690403013169746,
-                    "velocityX": 0.6297314852832604,
-                    "velocityY": 1.6250856497386854,
-                    "timestamp": 0.20943058215281965
-                },
-                {
-                    "x": 2.448821264845437,
-                    "y": 4.736675156040894,
-                    "heading": -0.45019318323311186,
-                    "angularVelocity": -0.26890279219946406,
-                    "velocityX": 0.9305827409539079,
-                    "velocityY": 2.1233961972720627,
-                    "timestamp": 0.27924077620375953
-                },
-                {
-                    "x": 2.542623225560298,
-                    "y": 4.913429983875953,
-                    "heading": -0.4600490129319591,
-                    "angularVelocity": -0.14118037962391586,
-                    "velocityX": 1.343671393412285,
-                    "velocityY": 2.5319343433823196,
-                    "timestamp": 0.34905097025469944
-                },
-                {
-                    "x": 2.675927389267578,
-                    "y": 5.095797559489528,
-                    "heading": -0.44245218860567326,
-                    "angularVelocity": 0.25206668688824874,
-                    "velocityX": 1.9095228930350086,
-                    "velocityY": 2.6123344605070336,
-                    "timestamp": 0.41886116430563936
-                },
-                {
-                    "x": 2.83772713712192,
-                    "y": 5.250967901017311,
-                    "heading": -0.38879601501351296,
-                    "angularVelocity": 0.7686008371275667,
-                    "velocityX": 2.3177094699953265,
-                    "velocityY": 2.2227461710524636,
-                    "timestamp": 0.48867135835657927
-                },
-                {
-                    "x": 3.013235940099558,
-                    "y": 5.368218424760011,
-                    "heading": -0.31718431053283597,
-                    "angularVelocity": 1.0258058360734288,
-                    "velocityX": 2.514085591121856,
-                    "velocityY": 1.6795616361728827,
-                    "timestamp": 0.5584815524075192
-                },
-                {
-                    "x": 3.1945050869972476,
-                    "y": 5.445323486359522,
-                    "heading": -0.23653218942173773,
-                    "angularVelocity": 1.1553057859494003,
-                    "velocityX": 2.5965999573717062,
-                    "velocityY": 1.1044957351381008,
-                    "timestamp": 0.6282917464584591
-                },
-                {
-                    "x": 3.377225953863421,
-                    "y": 5.481834951968023,
-                    "heading": -0.15180568644047004,
-                    "angularVelocity": 1.2136694952369833,
-                    "velocityX": 2.6173952006491676,
-                    "velocityY": 0.523010515923758,
-                    "timestamp": 0.698101940509399
-                },
-                {
-                    "x": 3.558790922164917,
-                    "y": 5.477728366851807,
-                    "heading": -0.06601521216801902,
-                    "angularVelocity": 1.2289104110775417,
-                    "velocityX": 2.6008374674955865,
-                    "velocityY": -0.05882500648124433,
-                    "timestamp": 0.7679121345603389
-                },
-                {
-                    "x": 3.7207755893743903,
-                    "y": 5.440802283745667,
-                    "heading": 0.01087618717506099,
-                    "angularVelocity": 1.2168377614801031,
-                    "velocityX": 2.5634734381227786,
-                    "velocityY": -0.5843703286511746,
-                    "timestamp": 0.8311016588990232
-                },
-                {
-                    "x": 3.8800031263189014,
-                    "y": 5.370704697701765,
-                    "heading": 0.08523277253593173,
-                    "angularVelocity": 1.1767232961398173,
-                    "velocityX": 2.519840726939224,
-                    "velocityY": -1.1093228945227975,
-                    "timestamp": 0.8942911832377075
-                },
-                {
-                    "x": 4.035880089753818,
-                    "y": 5.267514783368049,
-                    "heading": 0.15438353932229767,
-                    "angularVelocity": 1.0943390935099226,
-                    "velocityX": 2.4668165343191313,
-                    "velocityY": -1.633022489291642,
-                    "timestamp": 0.9574807075763918
-                },
-                {
-                    "x": 4.187424190703861,
-                    "y": 5.131425904894565,
-                    "heading": 0.2138534475851711,
-                    "angularVelocity": 0.9411355581283213,
-                    "velocityX": 2.3982472179488545,
-                    "velocityY": -2.153662017503815,
-                    "timestamp": 1.0206702319150762
-                },
-                {
-                    "x": 4.332733928007212,
-                    "y": 4.96304651892829,
-                    "heading": 0.25476003945729764,
-                    "angularVelocity": 0.6473635034690916,
-                    "velocityX": 2.2995858699928062,
-                    "velocityY": -2.664672471059999,
-                    "timestamp": 1.0838597562537606
-                },
-                {
-                    "x": 4.46716744223408,
-                    "y": 4.765388489141315,
-                    "heading": 0.2547605945579538,
-                    "angularVelocity": 0.000008784690993212257,
-                    "velocityX": 2.12746520302083,
-                    "velocityY": -3.1280189533989655,
-                    "timestamp": 1.147049280592445
-                },
-                {
-                    "x": 4.612743650855858,
-                    "y": 4.575786978724626,
-                    "heading": 0.2547605407972418,
-                    "angularVelocity": -8.507852266556338e-7,
-                    "velocityX": 2.303802887345736,
-                    "velocityY": -3.0005212478052368,
-                    "timestamp": 1.2102388049311295
-                },
-                {
-                    "x": 4.782527461386278,
-                    "y": 4.409030186652972,
-                    "heading": 0.25208197397278675,
-                    "angularVelocity": -0.04238941267495421,
-                    "velocityX": 2.6868980627285057,
-                    "velocityY": -2.638994260793031,
-                    "timestamp": 1.2734283292698139
-                },
-                {
-                    "x": 4.970403968588328,
-                    "y": 4.270066434839271,
-                    "heading": 0.23891743895373094,
-                    "angularVelocity": -0.2083341369570075,
-                    "velocityX": 2.9732223682415455,
-                    "velocityY": -2.1991580609051504,
-                    "timestamp": 1.3366178536084983
-                },
-                {
-                    "x": 5.173968377386639,
-                    "y": 4.1604195633547185,
-                    "heading": 0.21971651393307054,
-                    "angularVelocity": -0.3038624711945638,
-                    "velocityX": 3.2214898106793903,
-                    "velocityY": -1.7352064702582655,
-                    "timestamp": 1.3998073779471827
+                    "x": 2.2879322251049214,
+                    "y": 4.397976336389526,
+                    "heading": -0.38546801638598305,
+                    "angularVelocity": 0.06554072441421986,
+                    "velocityX": -0.17452197008732642,
+                    "velocityY": 0.5492068837058847,
+                    "timestamp": 0.06914759722835051
+                },
+                {
+                    "x": 2.264556974735031,
+                    "y": 4.474163552813777,
+                    "heading": -0.3765424472118302,
+                    "angularVelocity": 0.12907996129426796,
+                    "velocityX": -0.3380486279621587,
+                    "velocityY": 1.1018056950361832,
+                    "timestamp": 0.13829519445670102
+                },
+                {
+                    "x": 2.231004605798313,
+                    "y": 4.588879932488501,
+                    "heading": -0.3634307413762601,
+                    "angularVelocity": 0.18961910985573333,
+                    "velocityX": -0.48522826940697783,
+                    "velocityY": 1.6590074604604372,
+                    "timestamp": 0.20744279168505153
+                },
+                {
+                    "x": 2.1891253320707884,
+                    "y": 4.7425700174397605,
+                    "heading": -0.3464768905998804,
+                    "angularVelocity": 0.24518351259275634,
+                    "velocityX": -0.6056504550588213,
+                    "velocityY": 2.2226381119759067,
+                    "timestamp": 0.27659038891340204
+                },
+                {
+                    "x": 2.142455487111455,
+                    "y": 4.9358291258163405,
+                    "heading": -0.3263533789303141,
+                    "angularVelocity": 0.29102257309767376,
+                    "velocityX": -0.6749308266671239,
+                    "velocityY": 2.7948781465017833,
+                    "timestamp": 0.34573798614175255
+                },
+                {
+                    "x": 2.099906819940071,
+                    "y": 5.168740583772433,
+                    "heading": -0.30483662973794956,
+                    "angularVelocity": 0.3111713212639373,
+                    "velocityX": -0.6153311015404023,
+                    "velocityY": 3.3683232287448175,
+                    "timestamp": 0.41488558337010306
+                },
+                {
+                    "x": 2.090216091849313,
+                    "y": 5.4241817814008675,
+                    "heading": -0.28862249413017305,
+                    "angularVelocity": 0.23448588609832868,
+                    "velocityX": -0.14014555066512774,
+                    "velocityY": 3.69414423446863,
+                    "timestamp": 0.48403318059845357
+                },
+                {
+                    "x": 2.1169158362249996,
+                    "y": 5.665111810235099,
+                    "heading": -0.24564536248803095,
+                    "angularVelocity": 0.6215274769324497,
+                    "velocityX": 0.3861268568382121,
+                    "velocityY": 3.484286345315426,
+                    "timestamp": 0.5531807778268041
+                },
+                {
+                    "x": 2.1721169779622786,
+                    "y": 5.878339660700231,
+                    "heading": -0.1915552026069021,
+                    "angularVelocity": 0.7822420741739773,
+                    "velocityX": 0.7983088921367857,
+                    "velocityY": 3.08366247001803,
+                    "timestamp": 0.6223283750551546
+                },
+                {
+                    "x": 2.2518565055578685,
+                    "y": 6.060184802876031,
+                    "heading": -0.1318933186992544,
+                    "angularVelocity": 0.8628193357100991,
+                    "velocityX": 1.1531785744091572,
+                    "velocityY": 2.629811439074749,
+                    "timestamp": 0.6914759722835051
+                },
+                {
+                    "x": 2.3540132373645406,
+                    "y": 6.209070989598032,
+                    "heading": -0.06941508964740774,
+                    "angularVelocity": 0.9035488079854361,
+                    "velocityX": 1.4773721127192665,
+                    "velocityY": 2.153165007746782,
+                    "timestamp": 0.7606235695118556
+                },
+                {
+                    "x": 2.477290153503418,
+                    "y": 6.3241448402404785,
+                    "heading": -0.005748673882586236,
+                    "angularVelocity": 0.9207321485592678,
+                    "velocityX": 1.782808384965753,
+                    "velocityY": 1.6641771407079455,
+                    "timestamp": 0.8297711667402061
+                },
+                {
+                    "x": 2.6074231512941006,
+                    "y": 6.400809151237731,
+                    "heading": 0.05275689385662717,
+                    "angularVelocity": 0.9223034785287629,
+                    "velocityX": 2.051464863433381,
+                    "velocityY": 1.20856464509814,
+                    "timestamp": 0.8932053498501396
+                },
+                {
+                    "x": 2.7533092580755354,
+                    "y": 6.447850328813001,
+                    "heading": 0.11062964933050726,
+                    "angularVelocity": 0.9123275911695883,
+                    "velocityX": 2.2998027188043615,
+                    "velocityY": 0.7415745780760178,
+                    "timestamp": 0.956639532960073
+                },
+                {
+                    "x": 2.9132889651123484,
+                    "y": 6.4644464131499,
+                    "heading": 0.16695291665745207,
+                    "angularVelocity": 0.8879008850767368,
+                    "velocityX": 2.5219794627047807,
+                    "velocityY": 0.26162683151755056,
+                    "timestamp": 1.0200737160700064
+                },
+                {
+                    "x": 3.0851729747755345,
+                    "y": 6.449682823375658,
+                    "heading": 0.22055798508605634,
+                    "angularVelocity": 0.8450501890441832,
+                    "velocityX": 2.7096433064356162,
+                    "velocityY": -0.23273870727739393,
+                    "timestamp": 1.0835078991799398
+                },
+                {
+                    "x": 3.266000929743579,
+                    "y": 6.40260327654133,
+                    "heading": 0.269929363572885,
+                    "angularVelocity": 0.7783087298743283,
+                    "velocityX": 2.8506389789001907,
+                    "velocityY": -0.7421794453112333,
+                    "timestamp": 1.1469420822898733
+                },
+                {
+                    "x": 3.4516899170753974,
+                    "y": 6.322371527535034,
+                    "heading": 0.31309009761375267,
+                    "angularVelocity": 0.6804018263443473,
+                    "velocityX": 2.9272700967841274,
+                    "velocityY": -1.2648030615800407,
+                    "timestamp": 1.2103762653998067
+                },
+                {
+                    "x": 3.636598494450249,
+                    "y": 6.208685958737944,
+                    "heading": 0.3475353077130109,
+                    "angularVelocity": 0.5430070730006203,
+                    "velocityX": 2.9149674246500377,
+                    "velocityY": -1.7921814899081216,
+                    "timestamp": 1.2738104485097401
+                },
+                {
+                    "x": 3.8132641934440024,
+                    "y": 6.062639398740876,
+                    "heading": 0.3704029865204341,
+                    "angularVelocity": 0.36049457383033545,
+                    "velocityX": 2.785023631307647,
+                    "velocityY": -2.3023321628339066,
+                    "timestamp": 1.3372446316196736
+                },
+                {
+                    "x": 3.9730355739593506,
+                    "y": 5.887885093688965,
+                    "heading": 0.37894290950178183,
+                    "angularVelocity": 0.1346265146219495,
+                    "velocityX": 2.5186953261297353,
+                    "velocityY": -2.754891707981806,
+                    "timestamp": 1.400678814729607
+                },
+                {
+                    "x": 4.101195684299111,
+                    "y": 5.704704428790184,
+                    "heading": 0.3789451050812407,
+                    "angularVelocity": 0.000037151619885842266,
+                    "velocityX": 2.1686126132704535,
+                    "velocityY": -3.099622022424022,
+                    "timestamp": 1.459776557478272
+                },
+                {
+                    "x": 4.204458426061581,
+                    "y": 5.506418480888323,
+                    "heading": 0.3789451145933903,
+                    "angularVelocity": 1.6095622464276883e-7,
+                    "velocityX": 1.7473212505255857,
+                    "velocityY": -3.3552203295641,
+                    "timestamp": 1.518874300226937
+                },
+                {
+                    "x": 4.281071316930325,
+                    "y": 5.296392335982766,
+                    "heading": 0.3789451203460425,
+                    "angularVelocity": 9.73413180945881e-8,
+                    "velocityX": 1.2963759241186865,
+                    "velocityY": -3.5538776125279528,
+                    "timestamp": 1.577972042975602
+                },
+                {
+                    "x": 4.344371609432443,
+                    "y": 5.081977806335691,
+                    "heading": 0.37894512554853116,
+                    "angularVelocity": 8.803193422611526e-8,
+                    "velocityX": 1.0711118489099893,
+                    "velocityY": -3.628133997587826,
+                    "timestamp": 1.6370697857242669
+                },
+                {
+                    "x": 4.4350051048376455,
+                    "y": 4.877610290730759,
+                    "heading": 0.3789451268821095,
+                    "angularVelocity": 2.2565639514634894e-8,
+                    "velocityX": 1.5336202567072863,
+                    "velocityY": -3.4581272667918515,
+                    "timestamp": 1.6961675284729318
+                },
+                {
+                    "x": 4.551081441257298,
+                    "y": 4.6873967879425225,
+                    "heading": 0.3767623699353526,
+                    "angularVelocity": -0.03693469235752831,
+                    "velocityX": 1.964141624038618,
+                    "velocityY": -3.218625516664622,
+                    "timestamp": 1.7552652712215968
+                },
+                {
+                    "x": 4.687723026941447,
+                    "y": 4.517482449028109,
+                    "heading": 0.36165339974597627,
+                    "angularVelocity": -0.25566069848849177,
+                    "velocityX": 2.312128675793254,
+                    "velocityY": -2.875140927749109,
+                    "timestamp": 1.8143630139702618
+                },
+                {
+                    "x": 4.8416817092622,
+                    "y": 4.370738170800582,
+                    "heading": 0.3345492154544829,
+                    "angularVelocity": -0.45863315638965824,
+                    "velocityX": 2.6051533469768047,
+                    "velocityY": -2.483077549196378,
+                    "timestamp": 1.8734607567189268
+                },
+                {
+                    "x": 5.0108564738059584,
+                    "y": 4.248670932935222,
+                    "heading": 0.2976430739087661,
+                    "angularVelocity": -0.6244932518551631,
+                    "velocityX": 2.8626265010319796,
+                    "velocityY": -2.065514386638646,
+                    "timestamp": 1.9325584994675917
+                },
+                {
+                    "x": 5.194237326297875,
+                    "y": 4.152011519418658,
+                    "heading": 0.2577973678603408,
+                    "angularVelocity": -0.6742339757109184,
+                    "velocityX": 3.103009420708511,
+                    "velocityY": -1.635585540509506,
+                    "timestamp": 1.9916562422162567
                 },
                 {
                     "x": 5.390789031982422,
                     "y": 4.081310272216797,
-                    "heading": 0.19899217158132915,
-                    "angularVelocity": -0.3279711719205329,
-                    "velocityX": 3.4312753081359326,
-                    "velocityY": -1.25193680386463,
-                    "timestamp": 1.462996902285867
-                },
-                {
-                    "x": 5.681530534379024,
-                    "y": 4.031634954440038,
-                    "heading": 0.1807325834147537,
-                    "angularVelocity": -0.22892527550150313,
-                    "velocityX": 3.645102941474752,
-                    "velocityY": -0.6227925681552634,
-                    "timestamp": 1.542759122191511
-                },
-                {
-                    "x": 5.983259555492767,
-                    "y": 4.033604453850848,
-                    "heading": 0.1807316659794543,
-                    "angularVelocity": -0.000011502133581259988,
-                    "velocityX": 3.7828563632073102,
-                    "velocityY": 0.024692133859281144,
-                    "timestamp": 1.6225213420971547
-                },
-                {
-                    "x": 6.284539191755923,
-                    "y": 4.050188554583329,
-                    "heading": 0.18073165818528908,
-                    "angularVelocity": -9.771750653524246e-8,
-                    "velocityX": 3.777222306745724,
-                    "velocityY": 0.20791924738433482,
-                    "timestamp": 1.7022835620027985
-                },
-                {
-                    "x": 6.585818825161056,
-                    "y": 4.0667727072366,
-                    "heading": 0.18073165039112304,
-                    "angularVelocity": -9.77175162921636e-8,
-                    "velocityX": 3.777222270913928,
-                    "velocityY": 0.20791989832899174,
-                    "timestamp": 1.7820457819084423
-                },
-                {
-                    "x": 6.88709845856161,
-                    "y": 4.083356859971397,
-                    "heading": 0.180731642596831,
-                    "angularVelocity": -9.771909620772691e-8,
-                    "velocityX": 3.777222270856537,
-                    "velocityY": 0.20791989935110283,
-                    "timestamp": 1.8618080018140861
-                },
-                {
-                    "x": 7.15139892307148,
-                    "y": 4.097904823415219,
-                    "heading": 0.12048670978727856,
-                    "angularVelocity": -0.7553066211144209,
-                    "velocityX": 3.313604671767403,
-                    "velocityY": 0.1823916568650699,
-                    "timestamp": 1.94157022171973
-                },
-                {
-                    "x": 7.3628393316310445,
-                    "y": 4.109542993215659,
-                    "heading": 0.07229126938267637,
-                    "angularVelocity": -0.6042389550038586,
-                    "velocityX": 2.650884200686756,
-                    "velocityY": 0.1459108060709806,
-                    "timestamp": 2.0213324416253737
-                },
-                {
-                    "x": 7.521419659615098,
-                    "y": 4.118271521022388,
-                    "heading": 0.03614542857914898,
-                    "angularVelocity": -0.4531699449471921,
-                    "velocityX": 1.98816342087339,
-                    "velocityY": 0.10943185654883955,
-                    "timestamp": 2.1010946615310178
-                },
-                {
-                    "x": 7.627139886542953,
-                    "y": 4.12409050689369,
-                    "heading": 0.012048510432709781,
-                    "angularVelocity": -0.302109421915929,
-                    "velocityX": 1.3254423842882344,
-                    "velocityY": 0.07295416148382058,
-                    "timestamp": 2.180856881436662
+                    "heading": 0.22104423660220585,
+                    "angularVelocity": -0.6219041464028032,
+                    "velocityX": 3.3258750088027327,
+                    "velocityY": -1.1963442919045442,
+                    "timestamp": 2.050753984964922
+                },
+                {
+                    "x": 5.67793189235796,
+                    "y": 4.03407988691648,
+                    "heading": 0.19119773345256094,
+                    "angularVelocity": -0.37371337139653454,
+                    "velocityX": 3.5953667967399574,
+                    "velocityY": -0.5913800499300379,
+                    "timestamp": 2.130618676555184
+                },
+                {
+                    "x": 5.980039011502812,
+                    "y": 4.037197033313024,
+                    "heading": 0.19119714274274896,
+                    "angularVelocity": -0.000007396382559589573,
+                    "velocityX": 3.782736940809626,
+                    "velocityY": 0.039030344129324,
+                    "timestamp": 2.210483368145446
+                },
+                {
+                    "x": 6.281741681649569,
+                    "y": 4.053135447217484,
+                    "heading": 0.19119713453456852,
+                    "angularVelocity": -1.0277608664279704e-7,
+                    "velocityX": 3.777672763010384,
+                    "velocityY": 0.1995677136803443,
+                    "timestamp": 2.290348059735708
+                },
+                {
+                    "x": 6.583444349801777,
+                    "y": 4.069073898877065,
+                    "heading": 0.19119712632638652,
+                    "angularVelocity": -1.0277610566434499e-7,
+                    "velocityX": 3.777672738036278,
+                    "velocityY": 0.1995681864189316,
+                    "timestamp": 2.3702127513259703
+                },
+                {
+                    "x": 6.885147017932413,
+                    "y": 4.08501235094329,
+                    "heading": 0.1911971181180728,
+                    "angularVelocity": -1.0277775515776054e-7,
+                    "velocityX": 3.7776727377661725,
+                    "velocityY": 0.1995681915106077,
+                    "timestamp": 2.4500774429162324
+                },
+                {
+                    "x": 7.150097958068984,
+                    "y": 4.099008563262217,
+                    "heading": 0.12746367267042205,
+                    "angularVelocity": -0.7980177995885095,
+                    "velocityX": 3.317497818634029,
+                    "velocityY": 0.17524906238576554,
+                    "timestamp": 2.5299421345064945
+                },
+                {
+                    "x": 7.362058748903403,
+                    "y": 4.110205268667911,
+                    "heading": 0.07647732188798662,
+                    "angularVelocity": -0.6384091613855594,
+                    "velocityX": 2.6539987397918514,
+                    "velocityY": 0.14019593869007996,
+                    "timestamp": 2.6098068260967566
+                },
+                {
+                    "x": 7.521029367100823,
+                    "y": 4.118602666583934,
+                    "heading": 0.03823840686026882,
+                    "angularVelocity": -0.47879625234031903,
+                    "velocityX": 1.9904993687699384,
+                    "velocityY": 0.10514531201226385,
+                    "timestamp": 2.6896715176870187
+                },
+                {
+                    "x": 7.627009789128585,
+                    "y": 4.124200888756926,
+                    "heading": 0.012746172901683662,
+                    "angularVelocity": -0.31919279284744,
+                    "velocityX": 1.3269997030913787,
+                    "velocityY": 0.07009633495752653,
+                    "timestamp": 2.769536209277281
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
-                    "heading": 2.791295127527068e-32,
-                    "angularVelocity": -0.15105535486544666,
-                    "velocityX": 0.6627211920578459,
-                    "velocityY": 0.03647708288149429,
-                    "timestamp": 2.260619101342306
+                    "heading": 1.6188506035771725e-33,
+                    "angularVelocity": -0.15959709663791943,
+                    "velocityX": 0.6634998497618533,
+                    "velocityY": 0.03504816943929136,
+                    "timestamp": 2.849400900867543
                 },
                 {
                     "x": 7.68,
                     "y": 4.127,
                     "heading": 0,
-                    "angularVelocity": -1.938584239537671e-30,
-                    "velocityX": -1.689352589413745e-32,
+                    "angularVelocity": -1.4796214296723179e-31,
+                    "velocityX": -2.3067017155365004e-33,
                     "velocityY": 0,
-                    "timestamp": 2.34038132124795
+                    "timestamp": 2.929265592457805
                 }
             ],
             "constraints": [
@@ -22574,7 +25217,25 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 18
+                    "controlIntervalCount": 12
+                },
+                {
+                    "x": 2.9135491847991943,
+                    "y": 6.344918727874756,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 8
+                },
+                {
+                    "x": 1.9371600151062012,
+                    "y": 5.555497646331787,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 10
                 },
                 {
                     "x": 2.3,
@@ -22591,172 +25252,280 @@
                     "x": 5.176,
                     "y": 6.63,
                     "heading": 0.26,
-                    "angularVelocity": 2.521971410376041e-31,
-                    "velocityX": -3.46085485178236e-32,
-                    "velocityY": -2.8734015412732535e-37,
+                    "angularVelocity": 1.3458298549409377e-33,
+                    "velocityX": 0,
+                    "velocityY": 3.5102521636207017e-36,
                     "timestamp": 0
                 },
                 {
-                    "x": 5.132269964046874,
-                    "y": 6.606195610268081,
-                    "heading": 0.16914444930535336,
-                    "angularVelocity": -1.1148212857556328,
-                    "velocityX": -0.5365789380469493,
-                    "velocityY": -0.29208606589986635,
-                    "timestamp": 0.08149786145594107
-                },
-                {
-                    "x": 5.043666795437562,
-                    "y": 6.5570487224925476,
-                    "heading": 0.005335724612659364,
-                    "angularVelocity": -2.0099757437347154,
-                    "velocityX": -1.0871839705537814,
-                    "velocityY": -0.6030451216453527,
-                    "timestamp": 0.16299572291188213
-                },
-                {
-                    "x": 4.909391168326261,
-                    "y": 6.4806746643853055,
-                    "heading": -0.2131663273161798,
-                    "angularVelocity": -2.681077123072297,
-                    "velocityX": -1.647596939508567,
-                    "velocityY": -0.9371295975481627,
-                    "timestamp": 0.2444935843678232
-                },
-                {
-                    "x": 4.728031239930722,
-                    "y": 6.375889843725219,
-                    "heading": -0.4550458803770597,
-                    "angularVelocity": -2.967925154596154,
-                    "velocityX": -2.225333587355345,
-                    "velocityY": -1.2857370584715861,
-                    "timestamp": 0.32599144582376427
-                },
-                {
-                    "x": 4.50844214888531,
-                    "y": 6.243100931440797,
-                    "heading": -0.5880935326141025,
-                    "angularVelocity": -1.6325293677671568,
-                    "velocityX": -2.6944153763362864,
-                    "velocityY": -1.6293545611158133,
-                    "timestamp": 0.40748930727970534
-                },
-                {
-                    "x": 4.251218650398233,
-                    "y": 6.083818287155582,
-                    "heading": -0.6031902279140122,
-                    "angularVelocity": -0.1852403858237583,
-                    "velocityX": -3.15619936390767,
-                    "velocityY": -1.9544395575499292,
-                    "timestamp": 0.4889871687356464
-                },
-                {
-                    "x": 3.9855391766757995,
-                    "y": 5.92740773635831,
-                    "heading": -0.6031903133870022,
-                    "angularVelocity": -0.0000010487758630839914,
-                    "velocityX": -3.259956383837921,
-                    "velocityY": -1.9191982219291623,
-                    "timestamp": 0.5704850301915875
-                },
-                {
-                    "x": 3.719859695364488,
-                    "y": 5.770997198451498,
-                    "heading": -0.603190398859952,
-                    "angularVelocity": -0.0000010487753700599598,
-                    "velocityX": -3.259956476955436,
-                    "velocityY": -1.919198063759863,
-                    "timestamp": 0.6519828916475285
-                },
-                {
-                    "x": 3.4541802321431443,
-                    "y": 5.614586629816999,
-                    "heading": -0.6031904843329001,
-                    "angularVelocity": -0.000001048775349610677,
-                    "velocityX": -3.259956254986808,
-                    "velocityY": -1.9191984407966034,
-                    "timestamp": 0.7334807531034696
-                },
-                {
-                    "x": 3.1885009063173535,
-                    "y": 5.458175827802239,
-                    "heading": -0.6031905698058837,
-                    "angularVelocity": -0.000001048775783641969,
-                    "velocityX": -3.259954569107567,
-                    "velocityY": -1.9192013044332228,
-                    "timestamp": 0.8149786145594107
-                },
-                {
-                    "x": 2.937386380849553,
-                    "y": 5.279314604480798,
-                    "heading": -0.603190661977047,
-                    "angularVelocity": -0.0000011309641953921016,
-                    "velocityX": -3.0812406728433848,
-                    "velocityY": -2.1946738248847923,
-                    "timestamp": 0.8964764760153517
-                },
-                {
-                    "x": 2.722322578778117,
-                    "y": 5.058413395221267,
-                    "heading": -0.6031910253459357,
-                    "angularVelocity": -0.00000445863096683889,
-                    "velocityX": -2.6388889012468426,
-                    "velocityY": -2.710515408787179,
-                    "timestamp": 0.9779743374712928
-                },
-                {
-                    "x": 2.561151389441887,
-                    "y": 4.8410288515668665,
-                    "heading": -0.5595876383179096,
-                    "angularVelocity": 0.5350249227287813,
-                    "velocityX": -1.9776124975176395,
-                    "velocityY": -2.667365005300435,
-                    "timestamp": 1.0594721989272338
-                },
-                {
-                    "x": 2.4444023928247787,
-                    "y": 4.656624455699537,
-                    "heading": -0.5072145719014195,
-                    "angularVelocity": 0.6426311743750899,
-                    "velocityX": -1.432540615562337,
-                    "velocityY": -2.262689996681954,
-                    "timestamp": 1.1409700603831747
-                },
-                {
-                    "x": 2.367465069003143,
-                    "y": 4.510717874734717,
-                    "heading": -0.4552059179368463,
-                    "angularVelocity": 0.6381597386170672,
-                    "velocityX": -0.9440410146618312,
-                    "velocityY": -1.7903117745451358,
-                    "timestamp": 1.2224679218391157
-                },
-                {
-                    "x": 2.321242179529733,
-                    "y": 4.410775659264178,
-                    "heading": -0.4150276653385325,
-                    "angularVelocity": 0.49299763061923907,
-                    "velocityX": -0.5671669004271778,
-                    "velocityY": -1.2263170307182782,
-                    "timestamp": 1.3039657832950566
+                    "x": 5.139424093284861,
+                    "y": 6.625864418761083,
+                    "heading": 0.2540205855596587,
+                    "angularVelocity": -0.08994806739995133,
+                    "velocityX": -0.5502097496757357,
+                    "velocityY": -0.06221136596695031,
+                    "timestamp": 0.06647629696982885
+                },
+                {
+                    "x": 5.066272282481334,
+                    "y": 6.617593263412388,
+                    "heading": 0.24206149463416465,
+                    "angularVelocity": -0.17990007672842862,
+                    "velocityX": -1.1004194598373498,
+                    "velocityY": -0.12442262469054285,
+                    "timestamp": 0.1329525939396577
+                },
+                {
+                    "x": 4.956544571814103,
+                    "y": 6.605186545652003,
+                    "heading": 0.22412231737841096,
+                    "angularVelocity": -0.26985825133875346,
+                    "velocityX": -1.6506291064472642,
+                    "velocityY": -0.1866337074403447,
+                    "timestamp": 0.19942889090948654
+                },
+                {
+                    "x": 4.810240966932036,
+                    "y": 6.588644282109891,
+                    "heading": 0.2002025750538153,
+                    "angularVelocity": -0.35982362759243297,
+                    "velocityX": -2.2008386680814915,
+                    "velocityY": -0.24884454002634754,
+                    "timestamp": 0.2659051878793154
+                },
+                {
+                    "x": 4.627361475577568,
+                    "y": 6.567966494763936,
+                    "heading": 0.17030184062671833,
+                    "angularVelocity": -0.4497954276945935,
+                    "velocityX": -2.7510481132465947,
+                    "velocityY": -0.31105504200002715,
+                    "timestamp": 0.3323814848491442
+                },
+                {
+                    "x": 4.407906114797318,
+                    "y": 6.543153211904946,
+                    "heading": 0.13441995513073204,
+                    "angularVelocity": -0.5397696191211097,
+                    "velocityX": -3.3012573019801734,
+                    "velocityY": -0.37326511839627413,
+                    "timestamp": 0.398857781818973
+                },
+                {
+                    "x": 4.158022417123658,
+                    "y": 6.514899899511502,
+                    "heading": 0.134419949269586,
+                    "angularVelocity": -8.816896134746106e-8,
+                    "velocityX": -3.758989430278769,
+                    "velocityY": -0.4250133307856765,
+                    "timestamp": 0.46533407878880184
+                },
+                {
+                    "x": 3.908138721969202,
+                    "y": 6.486646564834816,
+                    "heading": 0.13441994340875443,
+                    "angularVelocity": -8.816423004519242e-8,
+                    "velocityX": -3.758989392382515,
+                    "velocityY": -0.42501366599148993,
+                    "timestamp": 0.5318103757586307
+                },
+                {
+                    "x": 3.6582550295868868,
+                    "y": 6.45839320564021,
+                    "heading": 0.1344199375479281,
+                    "angularVelocity": -8.816415176708587e-8,
+                    "velocityX": -3.7589893506813095,
+                    "velocityY": -0.42501403481347844,
+                    "timestamp": 0.5982866727284595
+                },
+                {
+                    "x": 3.4083713401101514,
+                    "y": 6.430139820747532,
+                    "heading": 0.13441993168710784,
+                    "angularVelocity": -8.816406061478911e-8,
+                    "velocityX": -3.758989306972801,
+                    "velocityY": -0.42501442138844536,
+                    "timestamp": 0.6647629696982883
+                },
+                {
+                    "x": 3.1584877665624913,
+                    "y": 6.401885410552911,
+                    "heading": 0.1344199258262894,
+                    "angularVelocity": -8.816403354486871e-8,
+                    "velocityX": -3.7589875630568486,
+                    "velocityY": -0.4250298449602976,
+                    "timestamp": 0.7312392666681171
+                },
+                {
+                    "x": 2.9135491847991943,
+                    "y": 6.344918727874756,
+                    "heading": 0.13441991981204132,
+                    "angularVelocity": -9.047206787345137e-8,
+                    "velocityX": -3.684600269994962,
+                    "velocityY": -0.8569472921154051,
+                    "timestamp": 0.7977155636379459
+                },
+                {
+                    "x": 2.741853960900858,
+                    "y": 6.285250221506208,
+                    "heading": 0.13441991359516897,
+                    "angularVelocity": -1.293850671116252e-7,
+                    "velocityX": -3.573307735472737,
+                    "velocityY": -1.2418163448570063,
+                    "timestamp": 0.8457649443822611
+                },
+                {
+                    "x": 2.5774323687799447,
+                    "y": 6.207758184927692,
+                    "heading": 0.1344190973329971,
+                    "angularVelocity": -0.000016987985264431854,
+                    "velocityX": -3.4219294728447913,
+                    "velocityY": -1.6127582786315857,
+                    "timestamp": 0.8938143251265763
+                },
+                {
+                    "x": 2.4266376481277105,
+                    "y": 6.11739996688274,
+                    "heading": 0.11881021575379135,
+                    "angularVelocity": -0.32485083756365457,
+                    "velocityX": -3.1383280765814066,
+                    "velocityY": -1.8805282533353447,
+                    "timestamp": 0.9418637058708915
+                },
+                {
+                    "x": 2.292578699649266,
+                    "y": 6.017904789932588,
+                    "heading": 0.09375834131328029,
+                    "angularVelocity": -0.5213776754755571,
+                    "velocityX": -2.7900244790211017,
+                    "velocityY": -2.0706859361954097,
+                    "timestamp": 0.9899130866152067
+                },
+                {
+                    "x": 2.176263979579871,
+                    "y": 5.911118232592638,
+                    "heading": 0.0629895244075883,
+                    "angularVelocity": -0.6403582403990148,
+                    "velocityX": -2.420732968200766,
+                    "velocityY": -2.2224335815729503,
+                    "timestamp": 1.037962467359522
+                },
+                {
+                    "x": 2.0781210346390075,
+                    "y": 5.798061969403822,
+                    "heading": 0.02853978404947447,
+                    "angularVelocity": -0.7169653349214,
+                    "velocityX": -2.0425433880846535,
+                    "velocityY": -2.3529182153339443,
+                    "timestamp": 1.0860118481038372
+                },
+                {
+                    "x": 1.9983746548109367,
+                    "y": 5.679376225369417,
+                    "heading": -0.008333608258354223,
+                    "angularVelocity": -0.7674061920598474,
+                    "velocityX": -1.659675496182245,
+                    "velocityY": -2.470078535787315,
+                    "timestamp": 1.1340612288481524
+                },
+                {
+                    "x": 1.9371600151062012,
+                    "y": 5.555497646331787,
+                    "heading": -0.04678243253848999,
+                    "angularVelocity": -0.8001939605576434,
+                    "velocityX": -1.273994352403345,
+                    "velocityY": -2.5781514167024167,
+                    "timestamp": 1.1821106095924676
+                },
+                {
+                    "x": 1.8879136197731217,
+                    "y": 5.371859224657917,
+                    "heading": -0.10269858839018778,
+                    "angularVelocity": -0.8284337962966012,
+                    "velocityX": -0.7296170063605633,
+                    "velocityY": -2.720721274486196,
+                    "timestamp": 1.2496068369333404
+                },
+                {
+                    "x": 1.8760341540552343,
+                    "y": 5.181395172179021,
+                    "heading": -0.15967327513417093,
+                    "angularVelocity": -0.8441166119736836,
+                    "velocityX": -0.17600192167619955,
+                    "velocityY": -2.821847382921196,
+                    "timestamp": 1.3171030642742134
+                },
+                {
+                    "x": 1.9020919030528052,
+                    "y": 4.989002664772072,
+                    "heading": -0.21623702759974286,
+                    "angularVelocity": -0.8380283564577732,
+                    "velocityX": 0.3860623033932351,
+                    "velocityY": -2.8504186824445474,
+                    "timestamp": 1.3845992916150864
+                },
+                {
+                    "x": 1.9652989688718845,
+                    "y": 4.8044863939922715,
+                    "heading": -0.26949747833980514,
+                    "angularVelocity": -0.7890878177691865,
+                    "velocityX": 0.9364533146403533,
+                    "velocityY": -2.7337271733418085,
+                    "timestamp": 1.4520955189559595
+                },
+                {
+                    "x": 2.0553904334698565,
+                    "y": 4.6466969161112965,
+                    "heading": -0.31411027699435934,
+                    "angularVelocity": -0.6609672927236123,
+                    "velocityX": 1.3347629659801235,
+                    "velocityY": -2.337752554436838,
+                    "timestamp": 1.5195917462968325
+                },
+                {
+                    "x": 2.145781200762326,
+                    "y": 4.526760098072049,
+                    "heading": -0.34739291091529667,
+                    "angularVelocity": -0.49310361826375293,
+                    "velocityX": 1.3391973278146792,
+                    "velocityY": -1.7769410641803345,
+                    "timestamp": 1.5870879736377055
+                },
+                {
+                    "x": 2.220455144634577,
+                    "y": 4.441296194130274,
+                    "heading": -0.37087004938287466,
+                    "angularVelocity": -0.3478288993696278,
+                    "velocityX": 1.1063424848196184,
+                    "velocityY": -1.2662026798943848,
+                    "timestamp": 1.6545842009785785
+                },
+                {
+                    "x": 2.2728997785605394,
+                    "y": 4.3865419949716715,
+                    "heading": -0.3858025738529217,
+                    "angularVelocity": -0.22123494983851594,
+                    "velocityX": 0.7770009671963372,
+                    "velocityY": -0.8112186608901896,
+                    "timestamp": 1.7220804283194515
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
                     "heading": -0.393,
-                    "angularVelocity": 0.27028519454392347,
-                    "velocityX": -0.26064707895700284,
-                    "velocityY": -0.6230305723006956,
-                    "timestamp": 1.3854636447509976
+                    "angularVelocity": -0.1066344954471229,
+                    "velocityX": 0.4015072028040527,
+                    "velocityY": -0.3932367188113757,
+                    "timestamp": 1.7895766556603245
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
                     "heading": -0.393,
-                    "angularVelocity": -9.678618240747916e-31,
+                    "angularVelocity": 8.903141052781706e-32,
                     "velocityX": 0,
-                    "velocityY": -3.108003047332484e-31,
-                    "timestamp": 1.4669615062069385
+                    "velocityY": -2.026142404451774e-33,
+                    "timestamp": 1.8570728830011975
                 }
             ],
             "constraints": [
@@ -30142,16 +32911,25 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 20
+                    "controlIntervalCount": 18
                 },
                 {
-                    "x": 2.279822826385498,
-                    "y": 5.254662990570068,
+                    "x": 2.9550976753234863,
+                    "y": 6.407241344451904,
                     "heading": 0,
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": false,
-                    "controlIntervalCount": 9
+                    "controlIntervalCount": 8
+                },
+                {
+                    "x": 1.9163856506347656,
+                    "y": 5.763239860534668,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 11
                 },
                 {
                     "x": 2.3,
@@ -30168,271 +32946,343 @@
                     "x": 7.68,
                     "y": 7.467,
                     "heading": 0,
-                    "angularVelocity": -1.8418609272007183e-28,
-                    "velocityX": -1.1316944174618986e-32,
-                    "velocityY": -1.0903166764521522e-29,
+                    "angularVelocity": -3.2387949852399556e-31,
+                    "velocityX": -1.054254683790836e-34,
+                    "velocityY": 3.285843466136746e-33,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.615058283165484,
-                    "y": 7.4500526040017725,
-                    "heading": -0.011528934363875379,
-                    "angularVelocity": -0.12842736126317786,
-                    "velocityX": -0.7234227436435784,
-                    "velocityY": -0.18878668917675853,
-                    "timestamp": 0.08977007898235294
-                },
-                {
-                    "x": 7.485153650546927,
-                    "y": 7.41615410854214,
-                    "heading": -0.0328638632229319,
-                    "angularVelocity": -0.23766191476385995,
-                    "velocityX": -1.4470816344507456,
-                    "velocityY": -0.3776146333375848,
-                    "timestamp": 0.17954015796470588
-                },
-                {
-                    "x": 7.290257872847477,
-                    "y": 7.365300387326372,
-                    "heading": -0.06109799257845879,
-                    "angularVelocity": -0.3145160355888266,
-                    "velocityX": -2.17105498746146,
-                    "velocityY": -0.5664885426438214,
-                    "timestamp": 0.2693102369470588
-                },
-                {
-                    "x": 7.030342832843462,
-                    "y": 7.297491054945299,
-                    "heading": -0.0902650461777,
-                    "angularVelocity": -0.32490840968750134,
-                    "velocityX": -2.895341554228826,
-                    "velocityY": -0.7553667452421856,
-                    "timestamp": 0.35908031592941175
-                },
-                {
-                    "x": 6.705594231461252,
-                    "y": 7.212800171241254,
-                    "heading": -0.10097705422378679,
-                    "angularVelocity": -0.11932715407194204,
-                    "velocityX": -3.617559492690666,
-                    "velocityY": -0.9434199531077018,
-                    "timestamp": 0.4488503949117647
-                },
-                {
-                    "x": 6.377014064882571,
-                    "y": 7.1270111914036525,
-                    "heading": -0.10097706428057987,
-                    "angularVelocity": -1.120283753063034e-7,
-                    "velocityX": -3.6602414780458985,
-                    "velocityY": -0.9556522708907337,
-                    "timestamp": 0.5386204738941176
-                },
-                {
-                    "x": 6.048433901032778,
-                    "y": 7.0412222011141505,
-                    "heading": -0.10097707433737516,
-                    "angularVelocity": -1.1202840770590317e-7,
-                    "velocityX": -3.660241447647229,
-                    "velocityY": -0.9556523873204973,
-                    "timestamp": 0.6283905528764705
-                },
-                {
-                    "x": 5.719853740215168,
-                    "y": 6.955433199211206,
-                    "heading": -0.10097708439415136,
-                    "angularVelocity": -1.1202819821797078e-7,
-                    "velocityX": -3.6602414138700015,
-                    "velocityY": -0.9556525166893554,
-                    "timestamp": 0.7181606318588234
-                },
-                {
-                    "x": 5.391273582926602,
-                    "y": 6.86964418379178,
-                    "heading": -0.100977094450907,
-                    "angularVelocity": -1.1202797265919916e-7,
-                    "velocityX": -3.660241374557919,
-                    "velocityY": -0.9556526672572584,
-                    "timestamp": 0.8079307108411763
-                },
-                {
-                    "x": 5.062693429765362,
-                    "y": 6.783855152564382,
-                    "heading": -0.10097710450764096,
-                    "angularVelocity": -1.1202773594839044e-7,
-                    "velocityX": -3.6602413285812334,
-                    "velocityY": -0.9556528433514181,
-                    "timestamp": 0.8977007898235292
-                },
-                {
-                    "x": 4.734113281444647,
-                    "y": 6.698066102797359,
-                    "heading": -0.10097711456435234,
-                    "angularVelocity": -1.1202748993058552e-7,
-                    "velocityX": -3.6602412746598207,
-                    "velocityY": -0.9556530498750426,
-                    "timestamp": 0.9874708688058821
-                },
-                {
-                    "x": 4.4055331387963745,
-                    "y": 6.612277031304317,
-                    "heading": -0.10097712462104143,
-                    "angularVelocity": -1.1202724880866421e-7,
-                    "velocityX": -3.660241211471189,
-                    "velocityY": -0.9556532918937597,
-                    "timestamp": 1.077240947788235
-                },
-                {
-                    "x": 4.076953002755506,
-                    "y": 6.526487934504167,
-                    "heading": -0.10097713467770983,
-                    "angularVelocity": -1.1202702659530083e-7,
-                    "velocityX": -3.6602411378675055,
-                    "velocityY": -0.9556535738043043,
-                    "timestamp": 1.167011026770588
-                },
-                {
-                    "x": 3.748372874246301,
-                    "y": 6.440698808855463,
-                    "heading": -0.10097714473442117,
-                    "angularVelocity": -1.1202751392033635e-7,
-                    "velocityX": -3.6602410539684915,
-                    "velocityY": -0.9556538951629009,
-                    "timestamp": 1.256781105752941
-                },
-                {
-                    "x": 3.4197928479820465,
-                    "y": 6.354909291415878,
-                    "heading": -0.10097715479114391,
-                    "angularVelocity": -1.120276421337113e-7,
-                    "velocityX": -3.6602399150000573,
-                    "velocityY": -0.9556582595606669,
-                    "timestamp": 1.346551184735294
-                },
-                {
-                    "x": 3.1047619528127512,
-                    "y": 6.228101525660344,
-                    "heading": -0.10097716526412484,
-                    "angularVelocity": -1.1666457370126234e-7,
-                    "velocityX": -3.5093084326123236,
-                    "velocityY": -1.412583871971558,
-                    "timestamp": 1.436321263717647
-                },
-                {
-                    "x": 2.8208656132222933,
-                    "y": 6.041748259045348,
-                    "heading": -0.1009771779112871,
-                    "angularVelocity": -1.4088397962304153e-7,
-                    "velocityX": -3.1624828986324123,
-                    "velocityY": -2.0758950947495576,
-                    "timestamp": 1.5260913427
-                },
-                {
-                    "x": 2.5792187192315943,
-                    "y": 5.803145197833315,
-                    "heading": -0.10097728674036179,
-                    "angularVelocity": -0.0000012123108078932896,
-                    "velocityX": -2.691842278966871,
-                    "velocityY": -2.65793529332427,
-                    "timestamp": 1.615861421682353
-                },
-                {
-                    "x": 2.3963290188287294,
-                    "y": 5.533479989641919,
-                    "heading": -0.13644335482170092,
-                    "angularVelocity": -0.3950767169047361,
-                    "velocityX": -2.037312459521214,
-                    "velocityY": -3.003954226707092,
-                    "timestamp": 1.705631500664706
-                },
-                {
-                    "x": 2.279822826385498,
-                    "y": 5.254662990570068,
-                    "heading": -0.18962986506951118,
-                    "angularVelocity": -0.5924748073105779,
-                    "velocityX": -1.2978287839789375,
-                    "velocityY": -3.105901233821009,
-                    "timestamp": 1.795401579647059
-                },
-                {
-                    "x": 2.2328990354823257,
-                    "y": 5.0773593431323425,
-                    "heading": -0.225508532252886,
-                    "angularVelocity": -0.6284306472549296,
-                    "velocityX": -0.8218908506157174,
-                    "velocityY": -3.1055514229095325,
-                    "timestamp": 1.8524940644563022
-                },
-                {
-                    "x": 2.2122650091837905,
-                    "y": 4.906948206481987,
-                    "heading": -0.2618449581428327,
-                    "angularVelocity": -0.636448492486558,
-                    "velocityX": -0.3614140524419242,
-                    "velocityY": -2.9848260628208765,
-                    "timestamp": 1.9095865492655453
-                },
-                {
-                    "x": 2.2138710973194917,
-                    "y": 4.752151212947765,
-                    "heading": -0.29639527524079207,
-                    "angularVelocity": -0.6051640108688915,
-                    "velocityX": 0.02813134059521416,
-                    "velocityY": -2.7113374737719043,
-                    "timestamp": 1.9666790340747884
-                },
-                {
-                    "x": 2.229883400142308,
-                    "y": 4.620372877512837,
-                    "heading": -0.32691110500805654,
-                    "angularVelocity": -0.5344981895462751,
-                    "velocityX": 0.280462531565201,
-                    "velocityY": -2.308155545781648,
-                    "timestamp": 2.0237715188840317
-                },
-                {
-                    "x": 2.251883940293385,
-                    "y": 4.51506115985361,
-                    "heading": -0.3519685302004407,
-                    "angularVelocity": -0.4388918309689961,
-                    "velocityX": 0.38534914402167997,
-                    "velocityY": -1.844581086479138,
-                    "timestamp": 2.080864003693275
-                },
-                {
-                    "x": 2.273515445313811,
-                    "y": 4.436872238113893,
-                    "heading": -0.37095335253129014,
-                    "angularVelocity": -0.33252751906728534,
-                    "velocityX": 0.3788853312819565,
-                    "velocityY": -1.3695133781784317,
-                    "timestamp": 2.1379564885025184
-                },
-                {
-                    "x": 2.2905088224909966,
-                    "y": 4.385405157743103,
-                    "heading": -0.3836505931421081,
-                    "angularVelocity": -0.22239775783770355,
-                    "velocityX": 0.2976464806908316,
-                    "velocityY": -0.9014685653065743,
-                    "timestamp": 2.1950489733117617
+                    "x": 7.625901434953002,
+                    "y": 7.454866143797358,
+                    "heading": -0.004441360542181856,
+                    "angularVelocity": -0.05445952619049241,
+                    "velocityX": -0.6633512843785839,
+                    "velocityY": -0.14878415147416332,
+                    "timestamp": 0.08155341871038292
+                },
+                {
+                    "x": 7.517704306524476,
+                    "y": 7.430598431564336,
+                    "heading": -0.01332401825146925,
+                    "angularVelocity": -0.10891827528203153,
+                    "velocityX": -1.3267025483353667,
+                    "velocityY": -0.29756830083607516,
+                    "timestamp": 0.16310683742076584
+                },
+                {
+                    "x": 7.355408617374589,
+                    "y": 7.3941968638363065,
+                    "heading": -0.02664786051722782,
+                    "angularVelocity": -0.1633756435530466,
+                    "velocityX": -1.9900537796734206,
+                    "velocityY": -0.4463524436333081,
+                    "timestamp": 0.24466025613114878
+                },
+                {
+                    "x": 7.139014373014635,
+                    "y": 7.345661441877259,
+                    "heading": -0.044412717144295275,
+                    "angularVelocity": -0.21783092490771921,
+                    "velocityX": -2.653404943432558,
+                    "velocityY": -0.5951365709316123,
+                    "timestamp": 0.3262136748415317
+                },
+                {
+                    "x": 6.868521591139737,
+                    "y": 7.284992169702681,
+                    "heading": -0.06661833727396635,
+                    "angularVelocity": -0.2722831302576181,
+                    "velocityX": -3.3167558902158003,
+                    "velocityY": -0.7439206489924082,
+                    "timestamp": 0.4077670935519146
+                },
+                {
+                    "x": 6.5674889621165065,
+                    "y": 7.217472965131769,
+                    "heading": -0.06661834019422361,
+                    "angularVelocity": -3.5807907402848094e-8,
+                    "velocityX": -3.691232492561401,
+                    "velocityY": -0.827913846391786,
+                    "timestamp": 0.4893205122622975
+                },
+                {
+                    "x": 6.266456335099212,
+                    "y": 7.149953751617259,
+                    "heading": -0.06661834311445712,
+                    "angularVelocity": -3.5807616132797033e-8,
+                    "velocityX": -3.6912324679648125,
+                    "velocityY": -0.8279139560572942,
+                    "timestamp": 0.5708739309726805
+                },
+                {
+                    "x": 5.9654237103471415,
+                    "y": 7.082434528003299,
+                    "heading": -0.06661834603468975,
+                    "angularVelocity": -3.5807605171686195e-8,
+                    "velocityX": -3.6912324401888563,
+                    "velocityY": -0.8279140798957559,
+                    "timestamp": 0.6524273496830634
+                },
+                {
+                    "x": 5.664391088171292,
+                    "y": 7.014915292903322,
+                    "heading": -0.06661834895492107,
+                    "angularVelocity": -3.580758928944026e-8,
+                    "velocityX": -3.691232408599494,
+                    "velocityY": -0.8279142207361639,
+                    "timestamp": 0.7339807683934463
+                },
+                {
+                    "x": 5.363358468935584,
+                    "y": 6.947396044694795,
+                    "heading": -0.06661835187515067,
+                    "angularVelocity": -3.580756830329485e-8,
+                    "velocityX": -3.6912323725477565,
+                    "velocityY": -0.8279143814719224,
+                    "timestamp": 0.8155341871038292
+                },
+                {
+                    "x": 5.062325853062933,
+                    "y": 6.879876781492171,
+                    "heading": -0.06661835479537813,
+                    "angularVelocity": -3.58075419139389e-8,
+                    "velocityX": -3.691232331310291,
+                    "velocityY": -0.8279145653280606,
+                    "timestamp": 0.8970876058142121
+                },
+                {
+                    "x": 4.761293241038404,
+                    "y": 6.812357501132806,
+                    "heading": -0.06661835771560275,
+                    "angularVelocity": -3.580750706252144e-8,
+                    "velocityX": -3.691232284125005,
+                    "velocityY": -0.8279147757023306,
+                    "timestamp": 0.978641024524595
+                },
+                {
+                    "x": 4.46026063340559,
+                    "y": 6.744838201193102,
+                    "heading": -0.06661836063582369,
+                    "angularVelocity": -3.580746208542298e-8,
+                    "velocityX": -3.6912322302742044,
+                    "velocityY": -0.8279150157945213,
+                    "timestamp": 1.060194443234978
+                },
+                {
+                    "x": 4.159228030751222,
+                    "y": 6.677318879057164,
+                    "heading": -0.06661836355604021,
+                    "angularVelocity": -3.5807407853670686e-8,
+                    "velocityX": -3.691232169229015,
+                    "velocityY": -0.8279152879625719,
+                    "timestamp": 1.141747861945361
+                },
+                {
+                    "x": 3.8581954336724973,
+                    "y": 6.609799532062401,
+                    "heading": -0.06661836647625141,
+                    "angularVelocity": -3.580734253915704e-8,
+                    "velocityX": -3.6912321008610123,
+                    "velocityY": -0.8279155927790207,
+                    "timestamp": 1.223301280655744
+                },
+                {
+                    "x": 3.557162842693998,
+                    "y": 6.542280157870002,
+                    "heading": -0.0666183693964589,
+                    "angularVelocity": -3.580729730790545e-8,
+                    "velocityX": -3.691232026060653,
+                    "velocityY": -0.8279159262737572,
+                    "timestamp": 1.304854699366127
+                },
+                {
+                    "x": 3.256130258261278,
+                    "y": 6.474760754493496,
+                    "heading": -0.06661837231666672,
+                    "angularVelocity": -3.580730095676624e-8,
+                    "velocityX": -3.6912319457969542,
+                    "velocityY": -0.8279162841264173,
+                    "timestamp": 1.38640811807651
+                },
+                {
+                    "x": 2.9550976753234863,
+                    "y": 6.407241344451904,
+                    "heading": -0.06661837523687889,
+                    "angularVelocity": -3.580735442493409e-8,
+                    "velocityX": -3.6912319274662786,
+                    "velocityY": -0.8279163658530455,
+                    "timestamp": 1.467961536786893
+                },
+                {
+                    "x": 2.779035844872165,
+                    "y": 6.36860208155972,
+                    "heading": -0.0666183783001983,
+                    "angularVelocity": -6.428976142053906e-8,
+                    "velocityX": -3.695002547647375,
+                    "velocityY": -0.8109206547436999,
+                    "timestamp": 1.5156101725269335
+                },
+                {
+                    "x": 2.608054120329012,
+                    "y": 6.311726454658268,
+                    "heading": -0.06677425221357433,
+                    "angularVelocity": -0.0032713195629861798,
+                    "velocityX": -3.5883865694652988,
+                    "velocityY": -1.1936464920308683,
+                    "timestamp": 1.563258808266974
+                },
+                {
+                    "x": 2.4510058546542055,
+                    "y": 6.242296424291011,
+                    "heading": -0.07609605863752185,
+                    "angularVelocity": -0.19563637614744786,
+                    "velocityX": -3.29596562914219,
+                    "velocityY": -1.457125252146435,
+                    "timestamp": 1.6109074440070146
+                },
+                {
+                    "x": 2.3100257060565728,
+                    "y": 6.162946485717711,
+                    "heading": -0.09018973905193617,
+                    "angularVelocity": -0.2957835034721525,
+                    "velocityX": -2.9587447029271896,
+                    "velocityY": -1.6653139663050547,
+                    "timestamp": 1.658556079747055
+                },
+                {
+                    "x": 2.1858070710130613,
+                    "y": 6.074835566620609,
+                    "heading": -0.10721183288872499,
+                    "angularVelocity": -0.3572419980708675,
+                    "velocityX": -2.6069714927652616,
+                    "velocityY": -1.8491803118526937,
+                    "timestamp": 1.7062047154870956
+                },
+                {
+                    "x": 2.078679040228791,
+                    "y": 5.978601109574319,
+                    "heading": -0.12617474237211068,
+                    "angularVelocity": -0.3979738179094536,
+                    "velocityX": -2.248291669223545,
+                    "velocityY": -2.0196686757492435,
+                    "timestamp": 1.7538533512271361
+                },
+                {
+                    "x": 1.9888310933691378,
+                    "y": 5.874644187394139,
+                    "heading": -0.14646656775118436,
+                    "angularVelocity": -0.42586372231046465,
+                    "velocityX": -1.8856352435727148,
+                    "velocityY": -2.181739740617511,
+                    "timestamp": 1.8015019869671767
+                },
+                {
+                    "x": 1.9163856506347656,
+                    "y": 5.763239860534668,
+                    "heading": -0.16767175460275374,
+                    "angularVelocity": -0.44503240276194317,
+                    "velocityX": -1.5204095900989456,
+                    "velocityY": -2.3380381227986136,
+                    "timestamp": 1.8491506227072172
+                },
+                {
+                    "x": 1.8485889050766984,
+                    "y": 5.589343182132367,
+                    "heading": -0.1992083750464638,
+                    "angularVelocity": -0.46340502818407264,
+                    "velocityX": -0.9962181217957636,
+                    "velocityY": -2.555270476750044,
+                    "timestamp": 1.9172047406506962
+                },
+                {
+                    "x": 1.817164507457459,
+                    "y": 5.402475290970386,
+                    "heading": -0.23166704244574374,
+                    "angularVelocity": -0.47695375945076895,
+                    "velocityX": -0.4617560049104581,
+                    "velocityY": -2.7458719150120894,
+                    "timestamp": 1.9852588585941753
+                },
+                {
+                    "x": 1.8230483770902466,
+                    "y": 5.205640044676734,
+                    "heading": -0.2645113992669886,
+                    "angularVelocity": -0.4826211523089798,
+                    "velocityX": 0.08645868627131759,
+                    "velocityY": -2.8923341047058035,
+                    "timestamp": 2.0533129765376543
+                },
+                {
+                    "x": 1.8673165212991787,
+                    "y": 5.004575832748109,
+                    "heading": -0.29674148680613366,
+                    "angularVelocity": -0.4735949640250024,
+                    "velocityX": 0.6504844313118886,
+                    "velocityY": -2.9544753205907597,
+                    "timestamp": 2.1213670944811334
+                },
+                {
+                    "x": 1.9491517735306563,
+                    "y": 4.812420400099087,
+                    "heading": -0.3261812067352161,
+                    "angularVelocity": -0.43259277790693457,
+                    "velocityX": 1.202502577425556,
+                    "velocityY": -2.823568043429816,
+                    "timestamp": 2.1894212124246124
+                },
+                {
+                    "x": 2.050948548896826,
+                    "y": 4.653276186897604,
+                    "heading": -0.34937275798086526,
+                    "angularVelocity": -0.34078101291423535,
+                    "velocityX": 1.4958209501847175,
+                    "velocityY": -2.338494980328238,
+                    "timestamp": 2.2574753303680914
+                },
+                {
+                    "x": 2.14520275900139,
+                    "y": 4.5319666650276815,
+                    "heading": -0.36647677931046935,
+                    "angularVelocity": -0.2513297041615066,
+                    "velocityX": 1.3849890785871777,
+                    "velocityY": -1.7825449147790617,
+                    "timestamp": 2.3255294483115705
+                },
+                {
+                    "x": 2.2207736095496604,
+                    "y": 4.444424274499807,
+                    "heading": -0.37856224680514944,
+                    "angularVelocity": -0.17758613086130492,
+                    "velocityX": 1.1104522816836297,
+                    "velocityY": -1.2863643402237204,
+                    "timestamp": 2.3935835662550495
+                },
+                {
+                    "x": 2.273137984258771,
+                    "y": 4.387723780671385,
+                    "heading": -0.38627299411990695,
+                    "angularVelocity": -0.113303170297753,
+                    "velocityX": 0.7694519639887757,
+                    "velocityY": -0.8331677133117752,
+                    "timestamp": 2.4616376841985286
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
                     "heading": -0.39,
-                    "angularVelocity": -0.11121265573212626,
-                    "velocityX": 0.16624215149881386,
-                    "velocityY": -0.4449825196428863,
-                    "timestamp": 2.252141458121005
+                    "angularVelocity": -0.054765324902410414,
+                    "velocityX": 0.3947155080835744,
+                    "velocityY": -0.4073784439379148,
+                    "timestamp": 2.5296918021420076
                 },
                 {
                     "x": 2.3,
                     "y": 4.36,
                     "heading": -0.39,
-                    "angularVelocity": -5.463060715454085e-29,
+                    "angularVelocity": 3.1303206075717874e-30,
                     "velocityX": 0,
-                    "velocityY": -5.448481045128469e-30,
-                    "timestamp": 2.3092339429302484
+                    "velocityY": -1.5697460792586675e-31,
+                    "timestamp": 2.5977459200854867
                 }
             ],
             "constraints": [
@@ -34824,7 +37674,25 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 23
+                    "controlIntervalCount": 16
+                },
+                {
+                    "x": 3.82761549949646,
+                    "y": 5.887885093688965,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 12
+                },
+                {
+                    "x": 2.6227097511291504,
+                    "y": 6.303369998931885,
+                    "heading": 0.7,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 7
                 },
                 {
                     "x": 2.3,
@@ -34841,217 +37709,262 @@
                     "x": 7.68,
                     "y": 4.127,
                     "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 1.832751405105417e-34,
+                    "angularVelocity": -6.709404579113994e-34,
+                    "velocityX": 0,
                     "velocityY": 0,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.608571974046609,
-                    "y": 4.129750388677263,
-                    "heading": -0.02440208863451916,
-                    "angularVelocity": -0.26295738388657636,
-                    "velocityX": -0.7697098032139116,
-                    "velocityY": 0.029638242122484983,
-                    "timestamp": 0.09279864392416992
-                },
-                {
-                    "x": 7.46571594445247,
-                    "y": 4.135252523590067,
-                    "heading": -0.07320447592180841,
-                    "angularVelocity": -0.5258954788948677,
-                    "velocityX": -1.5394193659863489,
-                    "velocityY": 0.05929111331948823,
-                    "timestamp": 0.18559728784833984
-                },
-                {
-                    "x": 7.25143160309568,
-                    "y": 4.143508368339934,
-                    "heading": -0.1463869418885076,
-                    "angularVelocity": -0.7886156830752087,
-                    "velocityX": -2.3091322490864354,
-                    "velocityY": 0.08896514432486853,
-                    "timestamp": 0.27839593177250976
-                },
-                {
-                    "x": 6.965717910609067,
-                    "y": 4.154520147774138,
-                    "heading": -0.24389107124776152,
-                    "angularVelocity": -1.050706402983198,
-                    "velocityX": -3.078856332427471,
-                    "velocityY": 0.1186631503279318,
-                    "timestamp": 0.3711945756966797
-                },
-                {
-                    "x": 6.614926201978592,
-                    "y": 4.168029747138803,
-                    "heading": -0.24389108357148506,
-                    "angularVelocity": -1.3280068591073525e-7,
-                    "velocityX": -3.780138305869253,
-                    "velocityY": 0.1455797067002448,
-                    "timestamp": 0.4639932196208496
-                },
-                {
-                    "x": 6.264134491970206,
-                    "y": 4.181539310738243,
-                    "heading": -0.2438910958936344,
-                    "angularVelocity": -1.3278372185354406e-7,
-                    "velocityX": -3.7801383207176356,
-                    "velocityY": 0.14557932129349355,
-                    "timestamp": 0.5567918635450195
-                },
-                {
-                    "x": 5.913342805476888,
-                    "y": 4.195049484934711,
-                    "heading": -0.2438911082157909,
-                    "angularVelocity": -1.32783799697389e-7,
-                    "velocityX": -3.780138067318815,
-                    "velocityY": 0.14558590109896277,
-                    "timestamp": 0.6495905074691894
-                },
-                {
-                    "x": 5.568285864608835,
-                    "y": 4.259648543779433,
-                    "heading": -0.24389114467773698,
-                    "angularVelocity": -3.929146432741106e-7,
-                    "velocityX": -3.718340336417024,
-                    "velocityY": 0.6961207202291138,
-                    "timestamp": 0.7423891513933594
-                },
-                {
-                    "x": 5.223229300660808,
-                    "y": 4.324249615558004,
-                    "heading": -0.2438911811402195,
-                    "angularVelocity": -3.929204239211486e-7,
-                    "velocityX": -3.7183362747195923,
-                    "velocityY": 0.6961424116429128,
-                    "timestamp": 0.8351877953175293
-                },
-                {
-                    "x": 4.889178228675737,
-                    "y": 4.432171862538056,
-                    "heading": -0.2438912192984801,
-                    "angularVelocity": -4.111941617608203e-7,
-                    "velocityX": -3.5997408783046705,
-                    "velocityY": 1.1629722420106654,
-                    "timestamp": 0.9279864392416992
-                },
-                {
-                    "x": 4.584073695522117,
-                    "y": 4.605805251226935,
-                    "heading": -0.24389116821114407,
-                    "angularVelocity": 5.505181312773355e-7,
-                    "velocityX": -3.287812410307864,
-                    "velocityY": 1.8710767889103372,
-                    "timestamp": 1.0207850831658691
-                },
-                {
-                    "x": 4.320679858830311,
-                    "y": 4.837885010533044,
-                    "heading": -0.24389110572504064,
-                    "angularVelocity": 6.733514712028696e-7,
-                    "velocityX": -2.8383371303038154,
-                    "velocityY": 2.5008960205899418,
-                    "timestamp": 1.113583727090039
-                },
-                {
-                    "x": 4.066259556785684,
-                    "y": 5.079768532204951,
-                    "heading": -0.24389104453640137,
-                    "angularVelocity": 6.593699721479525e-7,
-                    "velocityX": -2.7416381456233947,
-                    "velocityY": 2.606541555386963,
-                    "timestamp": 1.206382371014209
-                },
-                {
-                    "x": 3.8118393280605467,
-                    "y": 5.321652130996455,
-                    "heading": -0.243890983347767,
-                    "angularVelocity": 6.593699195322184e-7,
-                    "velocityX": -2.741637355531139,
-                    "velocityY": 2.606542386429263,
-                    "timestamp": 1.299181014938379
-                },
-                {
-                    "x": 3.557419123484207,
-                    "y": 5.563535755188363,
-                    "heading": -0.24389092215913197,
-                    "angularVelocity": 6.593699267467298e-7,
-                    "velocityX": -2.7416370953032203,
-                    "velocityY": 2.606542660144498,
-                    "timestamp": 1.3919796588625488
-                },
-                {
-                    "x": 3.3029989365055474,
-                    "y": 5.805419397890013,
-                    "heading": -0.24389086097049373,
-                    "angularVelocity": 6.593699609263459e-7,
-                    "velocityX": -2.7416369056702816,
-                    "velocityY": 2.606542859605842,
-                    "timestamp": 1.4847783027867187
-                },
-                {
-                    "x": 3.0485787435735707,
-                    "y": 6.04730303432977,
-                    "heading": -0.24389079978182,
-                    "angularVelocity": 6.593703437871394e-7,
-                    "velocityX": -2.741636969823341,
-                    "velocityY": 2.6065427921275584,
-                    "timestamp": 1.5775769467108887
-                },
-                {
-                    "x": 2.7984729856087975,
-                    "y": 6.275491677578651,
-                    "heading": -0.21231080129448254,
-                    "angularVelocity": 0.3403066807004949,
-                    "velocityX": -2.6951445343225506,
-                    "velocityY": 2.45896527793385,
-                    "timestamp": 1.6703755906350586
-                },
-                {
-                    "x": 2.596978431795094,
-                    "y": 6.469397893884434,
-                    "heading": -0.03732218251039644,
-                    "angularVelocity": 1.8856807748945335,
-                    "velocityX": -2.171309248638947,
-                    "velocityY": 2.089537175394848,
-                    "timestamp": 1.7631742345592285
-                },
-                {
-                    "x": 2.4472498512530994,
-                    "y": 6.613867328015145,
-                    "heading": 0.15789573469158452,
-                    "angularVelocity": 2.1036720898801096,
-                    "velocityX": -1.6134781092743464,
-                    "velocityY": 1.5568054447946784,
-                    "timestamp": 1.8559728784833984
-                },
-                {
-                    "x": 2.3487140682465175,
-                    "y": 6.7094536627437265,
-                    "heading": 0.3099829221841124,
-                    "angularVelocity": 1.6388945038552638,
-                    "velocityX": -1.0618235228426263,
-                    "velocityY": 1.0300402105734285,
-                    "timestamp": 1.9487715224075683
+                    "x": 7.616979036560275,
+                    "y": 4.122277530863763,
+                    "heading": 0.08798648745533431,
+                    "angularVelocity": 0.9786973137324638,
+                    "velocityX": -0.7009990898727472,
+                    "velocityY": -0.05252929161611988,
+                    "timestamp": 0.08990163375413819
+                },
+                {
+                    "x": 7.489636713382173,
+                    "y": 4.1124897070611155,
+                    "heading": 0.24774232666841967,
+                    "angularVelocity": 1.7770070747544424,
+                    "velocityX": -1.4164628367750984,
+                    "velocityY": -0.10887259100780497,
+                    "timestamp": 0.17980326750827638
+                },
+                {
+                    "x": 7.296297558859626,
+                    "y": 4.097684850959325,
+                    "heading": 0.45307882023210666,
+                    "angularVelocity": 2.2840129260079944,
+                    "velocityX": -2.150563304013888,
+                    "velocityY": -0.16467838773963095,
+                    "timestamp": 0.2697049012624146
+                },
+                {
+                    "x": 7.035802443968742,
+                    "y": 4.077851050872189,
+                    "heading": 0.6494071349161524,
+                    "angularVelocity": 2.183812534719467,
+                    "velocityX": -2.8975570744718895,
+                    "velocityY": -0.22061668135394896,
+                    "timestamp": 0.35960653501655276
+                },
+                {
+                    "x": 6.7206589840239435,
+                    "y": 4.046579170766799,
+                    "heading": 0.7075219081795038,
+                    "angularVelocity": 0.6464262198202391,
+                    "velocityX": -3.505425283000413,
+                    "velocityY": -0.34784551514284123,
+                    "timestamp": 0.44950816877069094
+                },
+                {
+                    "x": 6.381493052293109,
+                    "y": 4.021491444364005,
+                    "heading": 0.7075219804762528,
+                    "angularVelocity": 8.041761424949666e-7,
+                    "velocityX": -3.7726336838147025,
+                    "velocityY": -0.27905751380892024,
+                    "timestamp": 0.5394098025248292
+                },
+                {
+                    "x": 6.042319780035644,
+                    "y": 3.9965031541240665,
+                    "heading": 0.7075220527697934,
+                    "angularVelocity": 8.041404542109454e-7,
+                    "velocityX": -3.7727153344624633,
+                    "velocityY": -0.27795145868295346,
+                    "timestamp": 0.6293114362789674
+                },
+                {
+                    "x": 5.7039977727154,
+                    "y": 4.0311606356057235,
+                    "heading": 0.7075221294336903,
+                    "angularVelocity": 8.52753100011537e-7,
+                    "velocityX": -3.7632464861037187,
+                    "velocityY": 0.3855044678769589,
+                    "timestamp": 0.7192130700331055
+                },
+                {
+                    "x": 5.379151739194528,
+                    "y": 4.1318481159369504,
+                    "heading": 0.707522232494757,
+                    "angularVelocity": 0.0000011463759035114744,
+                    "velocityX": -3.6133496128586025,
+                    "velocityY": 1.1199738661767524,
+                    "timestamp": 0.8091147037872437
+                },
+                {
+                    "x": 5.0805369696371825,
+                    "y": 4.294612195205207,
+                    "heading": 0.7075222695179495,
+                    "angularVelocity": 4.1181890719021464e-7,
+                    "velocityX": -3.3215722238596133,
+                    "velocityY": 1.8104685362379804,
+                    "timestamp": 0.8990163375413819
+                },
+                {
+                    "x": 4.819878772182475,
+                    "y": 4.513061803267287,
+                    "heading": 0.7075222309589103,
+                    "angularVelocity": -4.2890254069180694e-7,
+                    "velocityX": -2.899371085597298,
+                    "velocityY": 2.429873617864304,
+                    "timestamp": 0.9889179712955201
+                },
+                {
+                    "x": 4.607411800528633,
+                    "y": 4.7786191711792085,
+                    "heading": 0.7075221980431953,
+                    "angularVelocity": -3.6613033230177587e-7,
+                    "velocityX": -2.363327147478702,
+                    "velocityY": 2.953865873429686,
+                    "timestamp": 1.0788196050496583
+                },
+                {
+                    "x": 4.4192729102850965,
+                    "y": 5.061932933767741,
+                    "heading": 0.7075221661049191,
+                    "angularVelocity": -3.552580165687493e-7,
+                    "velocityX": -2.0927193687943375,
+                    "velocityY": 3.151375016869355,
+                    "timestamp": 1.1687212388037966
+                },
+                {
+                    "x": 4.23999192605832,
+                    "y": 5.350933382498888,
+                    "heading": 0.7075221408571981,
+                    "angularVelocity": -2.808371773455553e-7,
+                    "velocityX": -1.9941905028900322,
+                    "velocityY": 3.2146295530233306,
+                    "timestamp": 1.258622872557935
+                },
+                {
+                    "x": 4.059747399496467,
+                    "y": 5.639333870205677,
+                    "heading": 0.7075221155890865,
+                    "angularVelocity": -2.8106398720718004e-7,
+                    "velocityX": -2.0049082428773954,
+                    "velocityY": 3.2079560255325883,
+                    "timestamp": 1.3485245063120732
+                },
+                {
+                    "x": 3.82761549949646,
+                    "y": 5.887885093688965,
+                    "heading": 0.7075220884594695,
+                    "angularVelocity": -3.017700109769218e-7,
+                    "velocityX": -2.5820654231362323,
+                    "velocityY": 2.764701964849852,
+                    "timestamp": 1.4384261400662115
+                },
+                {
+                    "x": 3.6506709942338365,
+                    "y": 6.0327931031008335,
+                    "heading": 0.7075220641283702,
+                    "angularVelocity": -4.024467762457879e-7,
+                    "velocityX": -2.92673770558828,
+                    "velocityY": 2.3968403786158894,
+                    "timestamp": 1.4988840707255708
+                },
+                {
+                    "x": 3.4560308994306905,
+                    "y": 6.152888579058325,
+                    "heading": 0.7075220417785688,
+                    "angularVelocity": -3.696752622643165e-7,
+                    "velocityX": -3.2194303159267204,
+                    "velocityY": 1.9864304756722417,
+                    "timestamp": 1.5593420013849302
+                },
+                {
+                    "x": 3.2531901663020784,
+                    "y": 6.258545225655943,
+                    "heading": 0.7075220198614292,
+                    "angularVelocity": -3.625188534499236e-7,
+                    "velocityX": -3.3550723770436006,
+                    "velocityY": 1.7476060699673126,
+                    "timestamp": 1.6197999320442895
+                },
+                {
+                    "x": 3.0503686595470367,
+                    "y": 6.36423877502023,
+                    "heading": 0.7075219979437177,
+                    "angularVelocity": -3.625283082596746e-7,
+                    "velocityX": -3.354754364616084,
+                    "velocityY": 1.7482164574835888,
+                    "timestamp": 1.6802578627036489
+                },
+                {
+                    "x": 2.862919086961248,
+                    "y": 6.462612745515211,
+                    "heading": 0.6632456903097049,
+                    "angularVelocity": -0.7323490425675934,
+                    "velocityX": -3.1004960067524197,
+                    "velocityY": 1.6271474961510723,
+                    "timestamp": 1.7407157933630082
+                },
+                {
+                    "x": 2.701858233172079,
+                    "y": 6.5469153850893225,
+                    "heading": 0.5979529513983839,
+                    "angularVelocity": -1.0799697938593102,
+                    "velocityX": -2.6640153249158556,
+                    "velocityY": 1.394401671618308,
+                    "timestamp": 1.8011737240223675
+                },
+                {
+                    "x": 2.567771439572364,
+                    "y": 6.617045170730925,
+                    "heading": 0.5348900965776917,
+                    "angularVelocity": -1.0430865584205178,
+                    "velocityX": -2.2178528463909877,
+                    "velocityY": 1.1599766131037368,
+                    "timestamp": 1.8616316546817269
+                },
+                {
+                    "x": 2.4605956436791856,
+                    "y": 6.67307612750581,
+                    "heading": 0.4804771381393031,
+                    "angularVelocity": -0.9000135771262476,
+                    "velocityX": -1.7727334482731019,
+                    "velocityY": 0.9267759607999744,
+                    "timestamp": 1.9220895853410862
+                },
+                {
+                    "x": 2.380269344431837,
+                    "y": 6.715058292556038,
+                    "heading": 0.43766723065506036,
+                    "angularVelocity": -0.7080941576620494,
+                    "velocityX": -1.3286313039718116,
+                    "velocityY": 0.694402944201169,
+                    "timestamp": 1.9825475160004455
+                },
+                {
+                    "x": 2.3267482247734668,
+                    "y": 6.743025081240933,
+                    "heading": 0.4081449342684466,
+                    "angularVelocity": -0.48831139380108807,
+                    "velocityX": -0.8852621827225426,
+                    "velocityY": 0.462582631920201,
+                    "timestamp": 2.0430054466598047
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.393,
-                    "angularVelocity": 0.8945936525078311,
-                    "velocityX": -0.5249437511858832,
-                    "velocityY": 0.5123602592202229,
-                    "timestamp": 2.0415701663317383
+                    "angularVelocity": -0.2505036825320822,
+                    "velocityX": -0.4424270642703138,
+                    "velocityY": 0.23115112619031306,
+                    "timestamp": 2.103463377319164
                 },
                 {
                     "x": 2.3,
                     "y": 6.757,
                     "heading": 0.393,
                     "angularVelocity": 0,
-                    "velocityX": 1.4564927652554838e-31,
-                    "velocityY": 0,
-                    "timestamp": 2.134368810255908
+                    "velocityX": 0,
+                    "velocityY": -2.9211624967517807e-31,
+                    "timestamp": 2.1639213079785233
                 }
             ],
             "constraints": [
@@ -35335,7 +38248,16 @@
                     "isInitialGuess": false,
                     "translationConstrained": true,
                     "headingConstrained": true,
-                    "controlIntervalCount": 21
+                    "controlIntervalCount": 19
+                },
+                {
+                    "x": 2.7473552227020264,
+                    "y": 6.261821746826172,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 12
                 },
                 {
                     "x": 2.3,
@@ -37436,6 +40358,15 @@
                     "headingConstrained": false,
                     "controlIntervalCount": 13
                 },
+                {
+                    "x": 2.851226329803467,
+                    "y": 6.282596111297607,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 7
+                },
                 {
                     "x": 2.25,
                     "y": 5.56,
@@ -37451,253 +40382,316 @@
                     "x": 7.68,
                     "y": 2.457,
                     "heading": 0,
-                    "angularVelocity": 6.426641305668581e-31,
-                    "velocityX": -5.011351780601486e-32,
-                    "velocityY": -1.0551805148640748e-32,
+                    "angularVelocity": -1.0202939485165084e-30,
+                    "velocityX": 2.0196671556133315e-31,
+                    "velocityY": 6.267725686062039e-32,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.6491080594765135,
-                    "y": 2.4958059548034544,
-                    "heading": -0.02134777834454554,
-                    "angularVelocity": -0.2757848804434067,
-                    "velocityX": -0.3990827516771383,
-                    "velocityY": 0.5013212819261742,
-                    "timestamp": 0.07740735572675958
-                },
-                {
-                    "x": 7.587308326375472,
-                    "y": 2.5734484495799954,
-                    "heading": -0.06302080337998518,
-                    "angularVelocity": -0.538360013001088,
-                    "velocityX": -0.7983702907923742,
-                    "velocityY": 1.0030376835324535,
-                    "timestamp": 0.15481471145351916
-                },
-                {
-                    "x": 7.4945787588029695,
-                    "y": 2.6899753845455243,
-                    "heading": -0.12336744193035674,
-                    "angularVelocity": -0.7795982433941759,
-                    "velocityX": -1.1979425818371583,
-                    "velocityY": 1.5053728921687748,
-                    "timestamp": 0.23222206718027874
-                },
-                {
-                    "x": 7.370887761034633,
-                    "y": 2.845471450080925,
-                    "heading": -0.19922081386712712,
-                    "angularVelocity": -0.9799245979222482,
-                    "velocityX": -1.5979230475841766,
-                    "velocityY": 2.0088021877957765,
-                    "timestamp": 0.3096294229070383
-                },
-                {
-                    "x": 7.216202117777622,
-                    "y": 3.0401051125017453,
-                    "heading": -0.28194072984640106,
-                    "angularVelocity": -1.0686312069781692,
-                    "velocityX": -1.9983326107022177,
-                    "velocityY": 2.5144078439762083,
-                    "timestamp": 0.38703677863379793
-                },
-                {
-                    "x": 7.032473858656331,
-                    "y": 3.268122248690632,
-                    "heading": -0.2819407809538647,
-                    "angularVelocity": -6.602404015783628e-7,
-                    "velocityX": -2.3735245493959742,
-                    "velocityY": 2.9456778887237176,
-                    "timestamp": 0.46444413436055754
-                },
-                {
-                    "x": 6.834683469529352,
-                    "y": 3.4840547774927524,
-                    "heading": -0.28194080060331267,
-                    "angularVelocity": -2.538447128540298e-7,
-                    "velocityX": -2.5551885511392456,
-                    "velocityY": 2.7895608469606037,
-                    "timestamp": 0.5418514900873171
-                },
-                {
-                    "x": 6.603064880376339,
-                    "y": 3.663221523268011,
-                    "heading": -0.2819408226641663,
-                    "angularVelocity": -2.8499686418440244e-7,
-                    "velocityX": -2.992203867170001,
-                    "velocityY": 2.314595868740169,
-                    "timestamp": 0.6192588458140768
-                },
-                {
-                    "x": 6.344360158554223,
-                    "y": 3.8004066501958644,
-                    "heading": -0.2819407635550332,
-                    "angularVelocity": 7.636113202840995e-7,
-                    "velocityX": -3.342120647233029,
-                    "velocityY": 1.7722492344539278,
-                    "timestamp": 0.6966662015408364
-                },
-                {
-                    "x": 6.0661003337524155,
-                    "y": 3.891617196013079,
-                    "heading": -0.28194070942615473,
-                    "angularVelocity": 6.992730598438293e-7,
-                    "velocityX": -3.59474654817093,
-                    "velocityY": 1.1783188427102327,
-                    "timestamp": 0.774073557267596
-                },
-                {
-                    "x": 5.7830200242104235,
-                    "y": 3.966540038544743,
-                    "heading": -0.2819406560965727,
-                    "angularVelocity": 6.889472132026456e-7,
-                    "velocityX": -3.6570207945254936,
-                    "velocityY": 0.967903396624646,
-                    "timestamp": 0.8514809129943556
-                },
-                {
-                    "x": 5.499939604590639,
-                    "y": 4.041462465169378,
-                    "heading": -0.2819406027672389,
-                    "angularVelocity": 6.889440059098256e-7,
-                    "velocityX": -3.657022216584049,
-                    "velocityY": 0.9678980236594484,
-                    "timestamp": 0.9288882687211152
-                },
-                {
-                    "x": 5.234359644348809,
-                    "y": 4.1279159927443425,
-                    "heading": -0.3272205989423401,
-                    "angularVelocity": -0.5849572789301486,
-                    "velocityX": -3.430939576069496,
-                    "velocityY": 1.1168644990295,
-                    "timestamp": 1.0062956244478747
+                    "x": 7.650233104605526,
+                    "y": 2.498844879842391,
+                    "heading": 0.006167890922909293,
+                    "angularVelocity": 0.07857204140894247,
+                    "velocityX": -0.37919700052787514,
+                    "velocityY": 0.5330570324317279,
+                    "timestamp": 0.07849981764896931
+                },
+                {
+                    "x": 7.590699315345095,
+                    "y": 2.5825346366761623,
+                    "heading": 0.018503878854508167,
+                    "angularVelocity": 0.15714670812054757,
+                    "velocityX": -0.758393981584137,
+                    "velocityY": 1.066114028544761,
+                    "timestamp": 0.15699963529793862
+                },
+                {
+                    "x": 7.501398636107905,
+                    "y": 2.7080692662264956,
+                    "heading": 0.037008343975294504,
+                    "angularVelocity": 0.23572621790707007,
+                    "velocityX": -1.1375909130963788,
+                    "velocityY": 1.599170970201378,
+                    "timestamp": 0.23549945294690794
+                },
+                {
+                    "x": 7.382331074013197,
+                    "y": 2.875448760857071,
+                    "heading": 0.061681864177524894,
+                    "angularVelocity": 0.31431308939549074,
+                    "velocityX": -1.5167877539174253,
+                    "velocityY": 2.132227814579811,
+                    "timestamp": 0.31399927059587723
+                },
+                {
+                    "x": 7.2334966444595095,
+                    "y": 3.084673101412061,
+                    "heading": 0.09252521384117814,
+                    "angularVelocity": 0.39290982561993243,
+                    "velocityX": -1.8959843985782956,
+                    "velocityY": 2.665284414934403,
+                    "timestamp": 0.39249908824484653
+                },
+                {
+                    "x": 7.061360166565958,
+                    "y": 3.326653173894572,
+                    "heading": 0.09252521821441809,
+                    "angularVelocity": 5.5710192458533506e-8,
+                    "velocityX": -2.192826468251186,
+                    "velocityY": 3.0825558546464755,
+                    "timestamp": 0.4709989058938158
+                },
+                {
+                    "x": 6.8697148114409385,
+                    "y": 3.553495365981472,
+                    "heading": 0.09252522268181232,
+                    "angularVelocity": 5.690961282999965e-8,
+                    "velocityX": -2.4413477746153216,
+                    "velocityY": 2.8897161659824335,
+                    "timestamp": 0.5494987235427852
+                },
+                {
+                    "x": 6.64183630533757,
+                    "y": 3.743907306900521,
+                    "heading": 0.09252522772743905,
+                    "angularVelocity": 6.427564912453075e-8,
+                    "velocityX": -2.902917649087824,
+                    "velocityY": 2.425635455237886,
+                    "timestamp": 0.6279985411917545
+                },
+                {
+                    "x": 6.384546476442235,
+                    "y": 3.8921883279204876,
+                    "heading": 0.09252523431940601,
+                    "angularVelocity": 8.397429651316677e-8,
+                    "velocityX": -3.277585051800601,
+                    "velocityY": 1.8889345919635134,
+                    "timestamp": 0.7064983588407238
+                },
+                {
+                    "x": 6.105547891541977,
+                    "y": 3.9938992218783795,
+                    "heading": 0.09252527901816404,
+                    "angularVelocity": 5.694122529822745e-7,
+                    "velocityX": -3.5541303566827107,
+                    "velocityY": 1.2956831876058104,
+                    "timestamp": 0.7849981764896931
+                },
+                {
+                    "x": 5.813193382307703,
+                    "y": 4.045994945482373,
+                    "heading": 0.09252629740574593,
+                    "angularVelocity": 0.000012973120350997966,
+                    "velocityX": -3.7242699153978625,
+                    "velocityY": 0.6636413327346956,
+                    "timestamp": 0.8634979941386624
+                },
+                {
+                    "x": 5.523572440187452,
+                    "y": 4.068080526231475,
+                    "heading": 0.11166809511552328,
+                    "angularVelocity": 0.24384512325081395,
+                    "velocityX": -3.6894473235002847,
+                    "velocityY": 0.28134563124547074,
+                    "timestamp": 0.9419978117876316
+                },
+                {
+                    "x": 5.248612904615211,
+                    "y": 4.139233352038044,
+                    "heading": 0.1457449942262438,
+                    "angularVelocity": 0.43410163400767243,
+                    "velocityX": -3.502677379478612,
+                    "velocityY": 0.9064075298205869,
+                    "timestamp": 1.020497629436601
                 },
                 {
                     "x": 4.992805004119873,
                     "y": 4.258058071136475,
-                    "heading": -0.38392632852173914,
-                    "angularVelocity": -0.7325625458589549,
-                    "velocityX": -3.120564421314147,
-                    "velocityY": 1.6812624222886623,
-                    "timestamp": 1.0837029801746343
-                },
-                {
-                    "x": 4.760679474369801,
-                    "y": 4.451318301231298,
-                    "heading": -0.43925940371465827,
-                    "angularVelocity": -0.650567727765224,
-                    "velocityX": -2.7291701738847216,
-                    "velocityY": 2.272219071899485,
-                    "timestamp": 1.1687565000948184
-                },
-                {
-                    "x": 4.558064597648056,
-                    "y": 4.695759350557987,
-                    "heading": -0.45036550730749886,
-                    "angularVelocity": -0.13057782444839394,
-                    "velocityX": -2.3822044862091225,
-                    "velocityY": 2.8739674684372165,
-                    "timestamp": 1.2538100200150024
-                },
-                {
-                    "x": 4.30999794561686,
-                    "y": 4.900667947770545,
-                    "heading": -0.4503653988140509,
-                    "angularVelocity": 0.0000012755903345298267,
-                    "velocityX": -2.9165947777821004,
-                    "velocityY": 2.4091724528843765,
-                    "timestamp": 1.3388635399351865
-                },
-                {
-                    "x": 4.028045524182051,
-                    "y": 5.05567579900462,
-                    "heading": -0.4503653036134663,
-                    "angularVelocity": 0.0000011193021141522574,
-                    "velocityX": -3.3150000340890857,
-                    "velocityY": 1.8224742653747859,
-                    "timestamp": 1.4239170598553705
-                },
-                {
-                    "x": 3.731575050469511,
-                    "y": 5.180695241574213,
-                    "heading": -0.45036521232637566,
-                    "angularVelocity": 0.000001073289978617534,
-                    "velocityX": -3.485693172848094,
-                    "velocityY": 1.4698914599542185,
-                    "timestamp": 1.5089705797755546
-                },
-                {
-                    "x": 3.4348291016010033,
-                    "y": 5.305059402921897,
-                    "heading": -0.4503651210307298,
-                    "angularVelocity": 0.000001073390565931622,
-                    "velocityX": -3.488932017711167,
-                    "velocityY": 1.4621871201127652,
-                    "timestamp": 1.5940240996957387
-                },
-                {
-                    "x": 3.120089238118937,
-                    "y": 5.371868296261872,
-                    "heading": -0.45036508370615375,
-                    "angularVelocity": 4.3883634791364236e-7,
-                    "velocityX": -3.7004919229372337,
-                    "velocityY": 0.785492398232055,
-                    "timestamp": 1.6790776196159227
-                },
-                {
-                    "x": 2.8304866761911076,
-                    "y": 5.435580552385651,
-                    "heading": -0.37567464629819297,
-                    "angularVelocity": 0.8781580994889028,
-                    "velocityX": -3.404945053415936,
-                    "velocityY": 0.7490842963759552,
-                    "timestamp": 1.7641311395361068
-                },
-                {
-                    "x": 2.5976382602327472,
-                    "y": 5.485810295855226,
-                    "heading": -0.2445738789443125,
-                    "angularVelocity": 1.5413914377313083,
-                    "velocityX": -2.7376693660283826,
-                    "velocityY": 0.5905663106789283,
-                    "timestamp": 1.8491846594562908
-                },
-                {
-                    "x": 2.4235493783626687,
-                    "y": 5.523078200691987,
-                    "heading": -0.12850773623159037,
-                    "angularVelocity": 1.3646248012033382,
-                    "velocityX": -2.0468157230112984,
-                    "velocityY": 0.43817004718549557,
-                    "timestamp": 1.934238179376475
-                },
-                {
-                    "x": 2.3077761259926057,
-                    "y": 5.547737838884683,
-                    "heading": -0.04437897835044122,
-                    "angularVelocity": 0.9891272925576834,
-                    "velocityX": -1.3611812007158846,
-                    "velocityY": 0.2899308366756611,
-                    "timestamp": 2.0192916992966587
+                    "heading": 0.1827080793073441,
+                    "angularVelocity": 0.47086841967443066,
+                    "velocityX": -3.258706939158039,
+                    "velocityY": 1.513694205377446,
+                    "timestamp": 1.0989974470855703
+                },
+                {
+                    "x": 4.782367562736006,
+                    "y": 4.400850786547769,
+                    "heading": 0.2087590252622848,
+                    "angularVelocity": 0.37195010692359587,
+                    "velocityX": -3.004582979783548,
+                    "velocityY": 2.0387653429944033,
+                    "timestamp": 1.1690362653840851
+                },
+                {
+                    "x": 4.593366685154892,
+                    "y": 4.57831882888555,
+                    "heading": 0.22294244363545143,
+                    "angularVelocity": 0.20250796226614393,
+                    "velocityX": -2.6985160825467664,
+                    "velocityY": 2.5338526070127387,
+                    "timestamp": 1.2390750836825999
+                },
+                {
+                    "x": 4.4303113780727985,
+                    "y": 4.787155425256865,
+                    "heading": 0.2229432862167668,
+                    "angularVelocity": 0.000012030204616055241,
+                    "velocityX": -2.3280705049466333,
+                    "velocityY": 2.9817264403466797,
+                    "timestamp": 1.3091139019811147
+                },
+                {
+                    "x": 4.301341913399859,
+                    "y": 5.018600438478609,
+                    "heading": 0.22294334445620553,
+                    "angularVelocity": 8.315308587772638e-7,
+                    "velocityX": -1.8413997809506994,
+                    "velocityY": 3.3045248170136206,
+                    "timestamp": 1.3791527202796294
+                },
+                {
+                    "x": 4.209527987601824,
+                    "y": 5.267136397095996,
+                    "heading": 0.22294334228224533,
+                    "angularVelocity": -3.103936119065208e-8,
+                    "velocityX": -1.3109005552544912,
+                    "velocityY": 3.5485458586481258,
+                    "timestamp": 1.4491915385781442
+                },
+                {
+                    "x": 4.095163246631834,
+                    "y": 5.506135613505712,
+                    "heading": 0.22294334005402738,
+                    "angularVelocity": -3.181404259759038e-8,
+                    "velocityX": -1.6328765068917115,
+                    "velocityY": 3.412382193414732,
+                    "timestamp": 1.519230356876659
+                },
+                {
+                    "x": 3.9453760475707735,
+                    "y": 5.724684722787353,
+                    "heading": 0.22294333755566106,
+                    "angularVelocity": -3.567116580835512e-8,
+                    "velocityX": -2.138631157692488,
+                    "velocityY": 3.120399723908581,
+                    "timestamp": 1.5892691751751737
+                },
+                {
+                    "x": 3.7637358458155963,
+                    "y": 5.917575181509908,
+                    "heading": 0.22294327041118944,
+                    "angularVelocity": -9.586751071941434e-7,
+                    "velocityX": -2.5934218504529762,
+                    "velocityY": 2.7540507308438293,
+                    "timestamp": 1.6593079934736885
+                },
+                {
+                    "x": 3.5705120272666107,
+                    "y": 6.071441420591724,
+                    "heading": 0.20870193370475335,
+                    "angularVelocity": -0.20333490844666557,
+                    "velocityX": -2.7588103746352264,
+                    "velocityY": 2.1968708613263175,
+                    "timestamp": 1.7293468117722033
+                },
+                {
+                    "x": 3.3793495617108644,
+                    "y": 6.184488312917552,
+                    "heading": 0.18879422691132347,
+                    "angularVelocity": -0.2842381878658059,
+                    "velocityX": -2.7293787959290245,
+                    "velocityY": 1.614060532031302,
+                    "timestamp": 1.799385630070718
+                },
+                {
+                    "x": 3.1945436254003248,
+                    "y": 6.257137552443532,
+                    "heading": 0.1660945073730067,
+                    "angularVelocity": -0.3241019778712817,
+                    "velocityX": -2.638621564444474,
+                    "velocityY": 1.037271063258847,
+                    "timestamp": 1.8694244483692328
+                },
+                {
+                    "x": 3.018114205815821,
+                    "y": 6.289753496686376,
+                    "heading": 0.14197845255388317,
+                    "angularVelocity": -0.34432412489220054,
+                    "velocityX": -2.519023362625089,
+                    "velocityY": 0.46568381699052885,
+                    "timestamp": 1.9394632666677476
+                },
+                {
+                    "x": 2.851226329803467,
+                    "y": 6.282596111297607,
+                    "heading": 0.11724722927389757,
+                    "angularVelocity": -0.3531073750355408,
+                    "velocityX": -2.382791144492779,
+                    "velocityY": -0.10219169258892027,
+                    "timestamp": 2.0095020849662624
+                },
+                {
+                    "x": 2.661249908723212,
+                    "y": 6.213695182206588,
+                    "heading": 0.08663876117142615,
+                    "angularVelocity": -0.3547239131111928,
+                    "velocityX": -2.2016514926122546,
+                    "velocityY": -0.7984982163199495,
+                    "timestamp": 2.0957902289879784
+                },
+                {
+                    "x": 2.4936356536291973,
+                    "y": 6.0868793491721815,
+                    "heading": 0.056963129639284946,
+                    "angularVelocity": -0.3439131976774355,
+                    "velocityX": -1.9424946149239757,
+                    "velocityY": -1.4696785343117862,
+                    "timestamp": 2.1820783730096944
+                },
+                {
+                    "x": 2.3641635269275443,
+                    "y": 5.911089762977232,
+                    "heading": 0.030889400871559385,
+                    "angularVelocity": -0.3021704669089439,
+                    "velocityX": -1.500462527842606,
+                    "velocityY": -2.037239161740341,
+                    "timestamp": 2.2683665170314105
+                },
+                {
+                    "x": 2.2965373597407854,
+                    "y": 5.740054606624193,
+                    "heading": 0.01408623201300084,
+                    "angularVelocity": -0.19473322840668433,
+                    "velocityX": -0.7837249016474405,
+                    "velocityY": -1.9821397051956142,
+                    "timestamp": 2.3546546610531265
+                },
+                {
+                    "x": 2.263390196849459,
+                    "y": 5.620608972310546,
+                    "heading": 0.004408633441468959,
+                    "angularVelocity": -0.11215444116055903,
+                    "velocityX": -0.38414504410927525,
+                    "velocityY": -1.384264729156902,
+                    "timestamp": 2.4409428050748425
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 1.7803711242706045e-30,
-                    "angularVelocity": 0.521777092730431,
-                    "velocityX": -0.6792914161204248,
-                    "velocityY": 0.14416994295838573,
-                    "timestamp": 2.104345219216843
+                    "heading": -1.7581112791577623e-31,
+                    "angularVelocity": -0.05109199521503166,
+                    "velocityX": -0.1551800308289656,
+                    "velocityY": -0.7024020854510996,
+                    "timestamp": 2.5272309490965585
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
                     "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 1.3562125763475036e-31,
-                    "velocityY": 2.290518308039125e-31,
-                    "timestamp": 2.189398739137027
+                    "angularVelocity": 5.961493131289069e-30,
+                    "velocityX": -9.776555553264502e-32,
+                    "velocityY": 0,
+                    "timestamp": 2.6135190931182746
                 }
             ],
             "constraints": [
@@ -39657,6 +42651,15 @@
                     "headingConstrained": false,
                     "controlIntervalCount": 13
                 },
+                {
+                    "x": 2.851226329803467,
+                    "y": 6.365692615509033,
+                    "heading": 0,
+                    "isInitialGuess": false,
+                    "translationConstrained": true,
+                    "headingConstrained": false,
+                    "controlIntervalCount": 7
+                },
                 {
                     "x": 2.25,
                     "y": 5.56,
@@ -39672,280 +42675,343 @@
                     "x": 7.68,
                     "y": 0.787,
                     "heading": 0,
-                    "angularVelocity": -2.906139149989869e-32,
-                    "velocityX": 1.181772084740038e-33,
-                    "velocityY": 2.7502272506846476e-34,
+                    "angularVelocity": -1.6344658510237988e-32,
+                    "velocityX": 1.0699335804910329e-32,
+                    "velocityY": 2.3378742346984854e-33,
                     "timestamp": 0
                 },
                 {
-                    "x": 7.666043555718406,
-                    "y": 0.8524996120976488,
-                    "heading": 0.004999198621269784,
-                    "angularVelocity": 0.05577597491216818,
-                    "velocityX": -0.15571181404981524,
-                    "velocityY": 0.7307780702234378,
-                    "timestamp": 0.08962996396104499
-                },
-                {
-                    "x": 7.638131101049187,
-                    "y": 0.9835016263770707,
-                    "heading": 0.014524734067527434,
-                    "angularVelocity": 0.10627623871853438,
-                    "velocityX": -0.31141878715192206,
-                    "velocityY": 1.4615872693685117,
-                    "timestamp": 0.17925992792208997
-                },
-                {
-                    "x": 7.596263489657983,
-                    "y": 1.180010140566484,
-                    "heading": 0.02778512041931341,
-                    "angularVelocity": 0.14794590743726296,
-                    "velocityX": -0.46711623592083296,
-                    "velocityY": 2.1924421867983774,
-                    "timestamp": 0.26888989188313495
-                },
-                {
-                    "x": 7.540442918712468,
-                    "y": 1.4420312351509346,
-                    "heading": 0.04318451816280708,
-                    "angularVelocity": 0.17181082154832236,
-                    "velocityX": -0.6227891709269913,
-                    "velocityY": 2.9233649440976053,
-                    "timestamp": 0.35851985584417995
-                },
-                {
-                    "x": 7.470680035396563,
-                    "y": 1.7695653388184471,
-                    "heading": 0.05582613302662815,
-                    "angularVelocity": 0.1410422843561041,
-                    "velocityX": -0.7783433154812639,
-                    "velocityY": 3.654292484261909,
-                    "timestamp": 0.44814981980522495
-                },
-                {
-                    "x": 7.399999368806171,
-                    "y": 2.101181376235394,
-                    "heading": 0.05582613685023459,
-                    "angularVelocity": 4.265991273772723e-8,
-                    "velocityX": -0.7885830080341364,
-                    "velocityY": 3.6998345504308543,
-                    "timestamp": 0.5377797837662699
-                },
-                {
-                    "x": 7.329318638814834,
-                    "y": 2.4327974001390613,
-                    "heading": 0.05582614067384277,
-                    "angularVelocity": 4.265993214747779e-8,
-                    "velocityX": -0.788583715397405,
-                    "velocityY": 3.6998343996634295,
-                    "timestamp": 0.6274097477273148
-                },
-                {
-                    "x": 7.258637432835217,
-                    "y": 2.764413322590103,
-                    "heading": 0.05582614449746126,
-                    "angularVelocity": 4.266004720083137e-8,
-                    "velocityX": -0.7885890259906463,
-                    "velocityY": 3.699833267758185,
-                    "timestamp": 0.7170397116883598
-                },
-                {
-                    "x": 7.145396314308057,
-                    "y": 3.084009006822374,
-                    "heading": 0.05582614848984762,
-                    "angularVelocity": 4.4542987394171874e-8,
-                    "velocityX": -1.2634292542656518,
-                    "velocityY": 3.5657236721770227,
-                    "timestamp": 0.8066696756494047
-                },
-                {
-                    "x": 6.971535677728531,
-                    "y": 3.375105946924151,
-                    "heading": 0.05582615332930866,
-                    "angularVelocity": 5.39937854332775e-8,
-                    "velocityX": -1.9397601973274063,
-                    "velocityY": 3.2477636633692444,
-                    "timestamp": 0.8962996396104497
-                },
-                {
-                    "x": 6.743840876905014,
-                    "y": 3.626342927147427,
-                    "heading": 0.0558261606002303,
-                    "angularVelocity": 8.112155042111421e-8,
-                    "velocityX": -2.5403870621043505,
-                    "velocityY": 2.8030467615993793,
-                    "timestamp": 0.9859296035714946
-                },
-                {
-                    "x": 6.471198532281258,
-                    "y": 3.8279144449501397,
-                    "heading": 0.0558261762788256,
-                    "angularVelocity": 1.7492582391526053e-7,
-                    "velocityX": -3.041866052096724,
-                    "velocityY": 2.2489300329331785,
-                    "timestamp": 1.0755595675325396
-                },
-                {
-                    "x": 6.164249513947577,
-                    "y": 3.9719534129902763,
-                    "heading": 0.05582625958870765,
-                    "angularVelocity": 9.294869524458774e-7,
-                    "velocityX": -3.4246250335109787,
-                    "velocityY": 1.6070403431461682,
-                    "timestamp": 1.1651895314935845
-                },
-                {
-                    "x": 5.836252922691461,
-                    "y": 4.05240893270077,
-                    "heading": 0.05954498226559844,
-                    "angularVelocity": 0.04148972634315725,
-                    "velocityX": -3.6594524505071675,
-                    "velocityY": 0.8976408798452788,
-                    "timestamp": 1.2548194954546295
-                },
-                {
-                    "x": 5.497492188342187,
-                    "y": 4.066758597782368,
-                    "heading": 0.05954602093201425,
-                    "angularVelocity": 0.000011588383726853398,
-                    "velocityX": -3.779547813904135,
-                    "velocityY": 0.16009897190000646,
-                    "timestamp": 1.3444494594156744
-                },
-                {
-                    "x": 5.204246867986104,
-                    "y": 4.110932074419811,
-                    "heading": 0.17401114275805463,
-                    "angularVelocity": 1.2770854384789139,
-                    "velocityX": -3.2717331057226993,
-                    "velocityY": 0.4928427356797903,
-                    "timestamp": 1.4340794233767193
+                    "x": 7.662237951731404,
+                    "y": 0.8490986120749341,
+                    "heading": -0.06160217222995063,
+                    "angularVelocity": -0.6883164695444892,
+                    "velocityX": -0.19846557213082008,
+                    "velocityY": 0.6938634772078657,
+                    "timestamp": 0.0894968738300239
+                },
+                {
+                    "x": 7.626433684211601,
+                    "y": 0.9738312717265619,
+                    "heading": -0.1754105069837687,
+                    "angularVelocity": -1.271645923297472,
+                    "velocityX": -0.4000616556484858,
+                    "velocityY": 1.3937096829608293,
+                    "timestamp": 0.1789937476600478
+                },
+                {
+                    "x": 7.572249558904937,
+                    "y": 1.161886141289839,
+                    "heading": -0.32706746606296133,
+                    "angularVelocity": -1.6945503523086813,
+                    "velocityX": -0.6054303685464192,
+                    "velocityY": 2.101245121929494,
+                    "timestamp": 0.2684906214900717
+                },
+                {
+                    "x": 7.499449827352778,
+                    "y": 1.4140559658703253,
+                    "heading": -0.4849879770703222,
+                    "angularVelocity": -1.7645366173045323,
+                    "velocityX": -0.8134332344437395,
+                    "velocityY": 2.8176383575075215,
+                    "timestamp": 0.3579874953200956
+                },
+                {
+                    "x": 7.40801256781841,
+                    "y": 1.7212752924886132,
+                    "heading": -0.5291211000298751,
+                    "angularVelocity": -0.4931247435901767,
+                    "velocityX": -1.0216810445024997,
+                    "velocityY": 3.432738077553094,
+                    "timestamp": 0.4474843691501195
+                },
+                {
+                    "x": 7.313226703333856,
+                    "y": 2.0462974852478544,
+                    "heading": -0.5291211457832385,
+                    "angularVelocity": -5.112286210233085e-7,
+                    "velocityX": -1.0590969318612773,
+                    "velocityY": 3.631659731227445,
+                    "timestamp": 0.5369812429801434
+                },
+                {
+                    "x": 7.218440797992873,
+                    "y": 2.371319666092203,
+                    "heading": -0.5291211915365511,
+                    "angularVelocity": -5.112280543858435e-7,
+                    "velocityX": -1.059097388373643,
+                    "velocityY": 3.6316595980955055,
+                    "timestamp": 0.6264781168101673
+                },
+                {
+                    "x": 7.1236547448927325,
+                    "y": 2.6963418038456624,
+                    "heading": -0.5291212372898596,
+                    "angularVelocity": -5.112280079310825e-7,
+                    "velocityX": -1.0590990393716089,
+                    "velocityY": 3.631659116616238,
+                    "timestamp": 0.7159749906401912
+                },
+                {
+                    "x": 7.010927268830994,
+                    "y": 3.0155851307576884,
+                    "heading": -0.5291212838180788,
+                    "angularVelocity": -5.198865285819975e-7,
+                    "velocityX": -1.2595688680236534,
+                    "velocityY": 3.567089142335248,
+                    "timestamp": 0.8054718644702151
+                },
+                {
+                    "x": 6.837725577807192,
+                    "y": 3.30648882172331,
+                    "heading": -0.5291213383056133,
+                    "angularVelocity": -6.088205344213367e-7,
+                    "velocityX": -1.9352820228419736,
+                    "velocityY": 3.2504341047500516,
+                    "timestamp": 0.894968738300239
+                },
+                {
+                    "x": 6.610788937088648,
+                    "y": 3.5577327922782125,
+                    "heading": -0.5291214021937537,
+                    "angularVelocity": -7.138589055640395e-7,
+                    "velocityX": -2.535693494161062,
+                    "velocityY": 2.8072932584447,
+                    "timestamp": 0.984465612130263
+                },
+                {
+                    "x": 6.351860794366729,
+                    "y": 3.7516776606884883,
+                    "heading": -0.4841599241371699,
+                    "angularVelocity": 0.5023804310972586,
+                    "velocityX": -2.8931529297178065,
+                    "velocityY": 2.167057463690003,
+                    "timestamp": 1.0739624859602868
+                },
+                {
+                    "x": 6.072750894879938,
+                    "y": 3.8834732684056306,
+                    "heading": -0.3889789775410024,
+                    "angularVelocity": 1.0635114113252733,
+                    "velocityX": -3.1186552953445856,
+                    "velocityY": 1.4726280603661466,
+                    "timestamp": 1.1634593597903107
+                },
+                {
+                    "x": 5.7916594444015015,
+                    "y": 3.950870142776961,
+                    "heading": -0.22937923493241555,
+                    "angularVelocity": 1.783299636942698,
+                    "velocityX": -3.1407963032574324,
+                    "velocityY": 0.7530640064515952,
+                    "timestamp": 1.2529562336203346
+                },
+                {
+                    "x": 5.498028130376245,
+                    "y": 3.9763121038871923,
+                    "heading": -0.08643118620464799,
+                    "angularVelocity": 1.5972406924431666,
+                    "velocityX": -3.280911404603181,
+                    "velocityY": 0.28427765151386064,
+                    "timestamp": 1.3424531074503585
+                },
+                {
+                    "x": 5.202994132142315,
+                    "y": 4.066458551574186,
+                    "heading": -0.003961984667318494,
+                    "angularVelocity": 0.9214757790753437,
+                    "velocityX": -3.2965844013084786,
+                    "velocityY": 1.0072580619765965,
+                    "timestamp": 1.4319499812803824
                 },
                 {
                     "x": 4.937438011169434,
                     "y": 4.2165327072143555,
-                    "heading": 0.3021760231951433,
-                    "angularVelocity": 1.4299334148208713,
-                    "velocityX": -2.976781926774297,
-                    "velocityY": 1.1781844834885962,
-                    "timestamp": 1.5237093873377643
-                },
-                {
-                    "x": 4.703348327317259,
-                    "y": 4.378813716693983,
-                    "heading": 0.4253911826920849,
-                    "angularVelocity": 1.3960058537628992,
-                    "velocityX": -2.6521945051027034,
-                    "velocityY": 1.8386149895276152,
-                    "timestamp": 1.61197202546698
-                },
-                {
-                    "x": 4.500555108729279,
-                    "y": 4.597666686435954,
-                    "heading": 0.5261840356750895,
-                    "angularVelocity": 1.1419651068603307,
-                    "velocityX": -2.297611117074157,
-                    "velocityY": 2.479565242787912,
-                    "timestamp": 1.7002346635961958
-                },
-                {
-                    "x": 4.326794639818804,
-                    "y": 4.871425359866347,
-                    "heading": 0.5588913487111756,
-                    "angularVelocity": 0.37056804248478087,
-                    "velocityX": -1.968675224233522,
-                    "velocityY": 3.1016371052677254,
-                    "timestamp": 1.7884973017254115
-                },
-                {
-                    "x": 4.0969805858398445,
-                    "y": 5.11364296690146,
-                    "heading": 0.5588923269288569,
-                    "angularVelocity": 0.00001108303243636796,
-                    "velocityX": -2.603752378696331,
-                    "velocityY": 2.744282429905488,
-                    "timestamp": 1.8767599398546273
-                },
-                {
-                    "x": 3.8246168380678105,
-                    "y": 5.306780372295281,
-                    "heading": 0.5588922897097742,
-                    "angularVelocity": -4.2168559122096544e-7,
-                    "velocityX": -3.0858328455274044,
-                    "velocityY": 2.188212470049549,
-                    "timestamp": 1.965022577983843
-                },
-                {
-                    "x": 3.520011793550478,
-                    "y": 5.443527703198532,
-                    "heading": 0.5588922240132788,
-                    "angularVelocity": -7.44329614921164e-7,
-                    "velocityX": -3.451121006278936,
-                    "velocityY": 1.549322950250525,
-                    "timestamp": 2.053285216113059
-                },
-                {
-                    "x": 3.194693864220709,
-                    "y": 5.518709582083228,
-                    "heading": 0.5588921711301718,
-                    "angularVelocity": -5.991562005821763e-7,
-                    "velocityX": -3.6857943091787817,
-                    "velocityY": 0.851797323060201,
-                    "timestamp": 2.1415478542422743
-                },
-                {
-                    "x": 2.8828791952129387,
-                    "y": 5.532174882793972,
-                    "heading": 0.5041280478943673,
-                    "angularVelocity": -0.6204677811196886,
-                    "velocityX": -3.5328047701370098,
-                    "velocityY": 0.15255946339412532,
-                    "timestamp": 2.22981049237149
-                },
-                {
-                    "x": 2.6283322264519793,
-                    "y": 5.543894951157175,
-                    "heading": 0.3346872638422791,
-                    "angularVelocity": -1.9197339626765784,
-                    "velocityX": -2.883971906530878,
-                    "velocityY": 0.132786291137652,
-                    "timestamp": 2.3180731305007054
-                },
-                {
-                    "x": 2.4385492640365714,
-                    "y": 5.5521384887253,
-                    "heading": 0.1780668377028913,
-                    "angularVelocity": -1.7744815865360473,
-                    "velocityX": -2.150207227406541,
-                    "velocityY": 0.09339781523475092,
-                    "timestamp": 2.406335768629921
-                },
-                {
-                    "x": 2.31267915340354,
-                    "y": 5.55742889143524,
-                    "heading": 0.062002293486176877,
-                    "angularVelocity": -1.3149906537667377,
-                    "velocityX": -1.4260859781775244,
-                    "velocityY": 0.0599393222554144,
-                    "timestamp": 2.4945984067591365
+                    "heading": 0.07947932900863551,
+                    "angularVelocity": 0.9323377466170348,
+                    "velocityX": -2.967211139433056,
+                    "velocityY": 1.6768647799385226,
+                    "timestamp": 1.5214468551104063
+                },
+                {
+                    "x": 4.74667334234301,
+                    "y": 4.373655515147294,
+                    "heading": 0.13953709439935258,
+                    "angularVelocity": 0.8363208396611413,
+                    "velocityX": -2.6564502853656404,
+                    "velocityY": 2.187978154123957,
+                    "timestamp": 1.5932587274715584
+                },
+                {
+                    "x": 4.579753248112816,
+                    "y": 4.566217048926455,
+                    "heading": 0.1819755811120341,
+                    "angularVelocity": 0.5909675561619205,
+                    "velocityX": -2.324408050394913,
+                    "velocityY": 2.681472122196475,
+                    "timestamp": 1.6650705998327104
+                },
+                {
+                    "x": 4.438339398678278,
+                    "y": 4.792399271574889,
+                    "heading": 0.19511904158847473,
+                    "angularVelocity": 0.18302628861061485,
+                    "velocityX": -1.9692266025783034,
+                    "velocityY": 3.1496494271996025,
+                    "timestamp": 1.7368824721938625
+                },
+                {
+                    "x": 4.33242543936342,
+                    "y": 5.0425620352859015,
+                    "heading": 0.1951190405932856,
+                    "angularVelocity": -1.3858280983712203e-8,
+                    "velocityX": -1.4748809052380785,
+                    "velocityY": 3.483585032470794,
+                    "timestamp": 1.8086943445550145
+                },
+                {
+                    "x": 4.2346577127829645,
+                    "y": 5.296019422041832,
+                    "heading": 0.19511903926490842,
+                    "angularVelocity": -1.849801636869094e-8,
+                    "velocityX": -1.3614423822396335,
+                    "velocityY": 3.5294635611400946,
+                    "timestamp": 1.8805062169161666
+                },
+                {
+                    "x": 4.124562899973198,
+                    "y": 5.544370599514641,
+                    "heading": 0.19511903792287785,
+                    "angularVelocity": -1.8688143477674107e-8,
+                    "velocityX": -1.533100435761989,
+                    "velocityY": 3.4583581977060263,
+                    "timestamp": 1.9523180892773186
+                },
+                {
+                    "x": 3.9766612552398675,
+                    "y": 5.772239592130343,
+                    "heading": 0.1951190364645304,
+                    "angularVelocity": -2.030788798410176e-8,
+                    "velocityX": -2.059570929852819,
+                    "velocityY": 3.1731381611903378,
+                    "timestamp": 2.024129961638471
+                },
+                {
+                    "x": 3.7946576924754596,
+                    "y": 5.973917226463319,
+                    "heading": 0.19511901792814293,
+                    "angularVelocity": -2.581242745470272e-7,
+                    "velocityX": -2.534449482797023,
+                    "velocityY": 2.8084163203364336,
+                    "timestamp": 2.095941833999623
+                },
+                {
+                    "x": 3.5938514397947183,
+                    "y": 6.137095652578835,
+                    "heading": 0.18216101901891746,
+                    "angularVelocity": -0.1804436854683003,
+                    "velocityX": -2.7962820920593496,
+                    "velocityY": 2.272304296633136,
+                    "timestamp": 2.167753706360775
+                },
+                {
+                    "x": 3.3950104775935763,
+                    "y": 6.257338696148904,
+                    "heading": 0.16464788328752675,
+                    "angularVelocity": -0.2438752138826665,
+                    "velocityX": -2.768914883616214,
+                    "velocityY": 1.6744173298441403,
+                    "timestamp": 2.239565578721927
+                },
+                {
+                    "x": 3.203569657298003,
+                    "y": 6.335228188048846,
+                    "heading": 0.14501288414051958,
+                    "angularVelocity": -0.273422743362824,
+                    "velocityX": -2.6658658798476953,
+                    "velocityY": 1.0846325174230818,
+                    "timestamp": 2.311377451083079
+                },
+                {
+                    "x": 3.0218761949257575,
+                    "y": 6.3712387750954615,
+                    "heading": 0.12433283555566121,
+                    "angularVelocity": -0.2879753431418064,
+                    "velocityX": -2.530131249864694,
+                    "velocityY": 0.5014572919852904,
+                    "timestamp": 2.383189323444231
+                },
+                {
+                    "x": 2.851226329803467,
+                    "y": 6.365692615509033,
+                    "heading": 0.10320964185538832,
+                    "angularVelocity": -0.2941462603013797,
+                    "velocityX": -2.3763461320722596,
+                    "velocityY": -0.07723179195964242,
+                    "timestamp": 2.4550011958053832
+                },
+                {
+                    "x": 2.6537141886903095,
+                    "y": 6.292049225449601,
+                    "heading": 0.07629879670595036,
+                    "angularVelocity": -0.2952020622183997,
+                    "velocityX": -2.1666354603877345,
+                    "velocityY": -0.8078408720936208,
+                    "timestamp": 2.5461619585290816
+                },
+                {
+                    "x": 2.4816525262537397,
+                    "y": 6.153957047067382,
+                    "heading": 0.05014624127109134,
+                    "angularVelocity": -0.2868839032657673,
+                    "velocityX": -1.8874530806425645,
+                    "velocityY": -1.5148203487586651,
+                    "timestamp": 2.63732272125278
+                },
+                {
+                    "x": 2.3499481448537543,
+                    "y": 5.9595438725680525,
+                    "heading": 0.026875208437583037,
+                    "angularVelocity": -0.25527466135886906,
+                    "velocityX": -1.4447485679685645,
+                    "velocityY": -2.1326409377308697,
+                    "timestamp": 2.728483483976478
+                },
+                {
+                    "x": 2.2874888909600317,
+                    "y": 5.764180997032623,
+                    "heading": 0.012070368781366221,
+                    "angularVelocity": -0.1624036396129025,
+                    "velocityX": -0.6851550165615855,
+                    "velocityY": -2.1430588083994038,
+                    "timestamp": 2.8196442467001765
+                },
+                {
+                    "x": 2.260118679493569,
+                    "y": 5.628541423814637,
+                    "heading": 0.0037496226116310134,
+                    "angularVelocity": -0.09127552162935264,
+                    "velocityX": -0.3002411415689918,
+                    "velocityY": -1.487916173201615,
+                    "timestamp": 2.910805009423875
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
-                    "heading": 9.107235017738231e-34,
-                    "angularVelocity": -0.7024749633633888,
-                    "velocityX": -0.7101436659051381,
-                    "velocityY": 0.029130202985730554,
-                    "timestamp": 2.582861044888352
+                    "heading": -4.5794503744404666e-32,
+                    "angularVelocity": -0.04113197936919252,
+                    "velocityX": -0.11099818815950499,
+                    "velocityY": -0.7518741810265671,
+                    "timestamp": 3.001965772147573
                 },
                 {
                     "x": 2.25,
                     "y": 5.56,
                     "heading": 0,
-                    "angularVelocity": -2.2232006496445675e-32,
+                    "angularVelocity": -2.1498446437166114e-31,
                     "velocityX": 0,
-                    "velocityY": -2.9660881962078245e-33,
-                    "timestamp": 2.6711236830175675
+                    "velocityY": -6.253874378421497e-34,
+                    "timestamp": 3.0931265348712715
                 }
             ],
             "constraints": [
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.1.traj
new file mode 100644
index 00000000..0c5693aa
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.1.traj	
@@ -0,0 +1,328 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": 1.04,
+            "angularVelocity": 1.821187546700747e-33,
+            "velocityX": -3.3437580769683535e-34,
+            "velocityY": -3.622513119436198e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7214806358425874,
+            "y": 6.713430005414815,
+            "heading": 1.0269113199415918,
+            "angularVelocity": -0.32293003145597954,
+            "velocityX": 0.28325561303637664,
+            "velocityY": -0.1620979769229106,
+            "timestamp": 0.04053100914583816
+        },
+        {
+            "x": 0.7444896856754503,
+            "y": 6.700284020801432,
+            "heading": 1.001394622951622,
+            "angularVelocity": -0.6295598734824499,
+            "velocityX": 0.5676900308618518,
+            "velocityY": -0.32434387621788874,
+            "timestamp": 0.08106201829167632
+        },
+        {
+            "x": 0.7790845165676208,
+            "y": 6.680557699463914,
+            "heading": 0.9642667952582056,
+            "angularVelocity": -0.916035116713321,
+            "velocityX": 0.8535398358252526,
+            "velocityY": -0.4866970192264045,
+            "timestamp": 0.12159302743751449
+        },
+        {
+            "x": 0.8253351719483972,
+            "y": 6.654251504147771,
+            "heading": 0.9165522532050936,
+            "angularVelocity": -1.177235481145466,
+            "velocityX": 1.141117785011419,
+            "velocityY": -0.6490387451614623,
+            "timestamp": 0.16212403658335264
+        },
+        {
+            "x": 0.883327254582786,
+            "y": 6.621374965178265,
+            "heading": 0.8595502901552765,
+            "angularVelocity": -1.4063790724951688,
+            "velocityX": 1.4308077656226736,
+            "velocityY": -0.8111453344576391,
+            "timestamp": 0.2026550457291908
+        },
+        {
+            "x": 0.953164245540845,
+            "y": 6.581952530815573,
+            "heading": 0.7949436194315174,
+            "angularVelocity": -1.5940059743218395,
+            "velocityX": 1.7230508795567456,
+            "velocityY": -0.9726487248526893,
+            "timestamp": 0.24318605487502895
+        },
+        {
+            "x": 1.0349687566216221,
+            "y": 6.5360339228116,
+            "heading": 0.7250282822193501,
+            "angularVelocity": -1.724983874953593,
+            "velocityX": 2.0183191291001217,
+            "velocityY": -1.132925356947043,
+            "timestamp": 0.2837170640208671
+        },
+        {
+            "x": 1.1288774102342096,
+            "y": 6.483723562804198,
+            "heading": 0.6532984652847575,
+            "angularVelocity": -1.7697515666707262,
+            "velocityX": 2.316958190571729,
+            "velocityY": -1.2906256495904058,
+            "timestamp": 0.3242480731667053
+        },
+        {
+            "x": 1.2349754296776467,
+            "y": 6.425294263584176,
+            "heading": 0.5860844259008366,
+            "angularVelocity": -1.6583361924711089,
+            "velocityX": 2.6176999211067367,
+            "velocityY": -1.4415949775586157,
+            "timestamp": 0.36477908231254347
+        },
+        {
+            "x": 1.3525875597286727,
+            "y": 6.361480505266573,
+            "heading": 0.5370018573395918,
+            "angularVelocity": -1.2109880705076077,
+            "velocityX": 2.901781439189822,
+            "velocityY": -1.5744428688658776,
+            "timestamp": 0.40531009145838165
+        },
+        {
+            "x": 1.481480430033789,
+            "y": 6.296375538238497,
+            "heading": 0.5147559483468728,
+            "angularVelocity": -0.5488614633964374,
+            "velocityX": 3.1801051348446676,
+            "velocityY": -1.6063001736230227,
+            "timestamp": 0.44584110060421983
+        },
+        {
+            "x": 1.6221023727568105,
+            "y": 6.235265511699909,
+            "heading": 0.514755821640399,
+            "angularVelocity": -0.0000031261613391085408,
+            "velocityX": 3.469490291175273,
+            "velocityY": -1.5077351348125194,
+            "timestamp": 0.486372109750058
+        },
+        {
+            "x": 1.7676170388967807,
+            "y": 6.186949296866798,
+            "heading": 0.5147557675556996,
+            "angularVelocity": -0.0000013344029787740425,
+            "velocityX": 3.5902058499550584,
+            "velocityY": -1.1920802331681561,
+            "timestamp": 0.5269031188958961
+        },
+        {
+            "x": 1.9168630838394165,
+            "y": 6.151812553405762,
+            "heading": 0.5147557009631692,
+            "angularVelocity": -0.000001643002030243893,
+            "velocityX": 3.6822681716515033,
+            "velocityY": -0.866910155989649,
+            "timestamp": 0.5674341280417343
+        },
+        {
+            "x": 2.2488381415683802,
+            "y": 6.139406390407425,
+            "heading": 0.5147556500188685,
+            "angularVelocity": -5.801183495483463e-7,
+            "velocityX": 3.780301616970564,
+            "velocityY": -0.14127277622555945,
+            "timestamp": 0.6552512102128125
+        },
+        {
+            "x": 2.57698446655607,
+            "y": 6.191188083838006,
+            "heading": 0.5147556048038259,
+            "angularVelocity": -5.148775328112685e-7,
+            "velocityX": 3.7367026650739894,
+            "velocityY": 0.58965399612918,
+            "timestamp": 0.7430682923838907
+        },
+        {
+            "x": 2.8993349699211755,
+            "y": 6.271509362001365,
+            "heading": 0.5147555599835898,
+            "angularVelocity": -5.103817495962876e-7,
+            "velocityX": 3.6707038698590346,
+            "velocityY": 0.9146429849136081,
+            "timestamp": 0.8308853745549689
+        },
+        {
+            "x": 3.221685430514468,
+            "y": 6.351830811819113,
+            "heading": 0.514755515163351,
+            "angularVelocity": -5.103817801462039e-7,
+            "velocityX": 3.6707033828033078,
+            "velocityY": 0.9146449395946943,
+            "timestamp": 0.9187024567260471
+        },
+        {
+            "x": 3.5440358911075664,
+            "y": 6.432152261637641,
+            "heading": 0.514755470343112,
+            "angularVelocity": -5.103817833267703e-7,
+            "velocityX": 3.670703382801096,
+            "velocityY": 0.9146449396035715,
+            "timestamp": 1.0065195388971253
+        },
+        {
+            "x": 3.8663863517006654,
+            "y": 6.512473711456168,
+            "heading": 0.5147554255228727,
+            "angularVelocity": -5.103817863197995e-7,
+            "velocityX": 3.6707033828010953,
+            "velocityY": 0.9146449396035711,
+            "timestamp": 1.0943366210682035
+        },
+        {
+            "x": 4.188736812293763,
+            "y": 6.592795161274696,
+            "heading": 0.5147553807026332,
+            "angularVelocity": -5.103817901912557e-7,
+            "velocityX": 3.6707033828010953,
+            "velocityY": 0.9146449396035711,
+            "timestamp": 1.1821537032392817
+        },
+        {
+            "x": 4.511087272886861,
+            "y": 6.673116611093223,
+            "heading": 0.5147553358823933,
+            "angularVelocity": -5.103817929337743e-7,
+            "velocityX": 3.6707033828010958,
+            "velocityY": 0.9146449396035705,
+            "timestamp": 1.26997078541036
+        },
+        {
+            "x": 4.833437733479959,
+            "y": 6.753438060911751,
+            "heading": 0.5147552910621531,
+            "angularVelocity": -5.103817959943963e-7,
+            "velocityX": 3.6707033828010953,
+            "velocityY": 0.9146449396035703,
+            "timestamp": 1.357787867581438
+        },
+        {
+            "x": 5.155788194073057,
+            "y": 6.833759510730278,
+            "heading": 0.5147552462419127,
+            "angularVelocity": -5.10381799453682e-7,
+            "velocityX": 3.670703382801095,
+            "velocityY": 0.9146449396035702,
+            "timestamp": 1.4456049497525163
+        },
+        {
+            "x": 5.4781386546661555,
+            "y": 6.914080960548806,
+            "heading": 0.514755201421672,
+            "angularVelocity": -5.10381803359495e-7,
+            "velocityX": 3.670703382801095,
+            "velocityY": 0.9146449396035701,
+            "timestamp": 1.5334220319235945
+        },
+        {
+            "x": 5.8004891152592535,
+            "y": 6.994402410367334,
+            "heading": 0.5147551566014309,
+            "angularVelocity": -5.1038180633665e-7,
+            "velocityX": 3.670703382801095,
+            "velocityY": 0.9146449396035699,
+            "timestamp": 1.6212391140946727
+        },
+        {
+            "x": 6.1228395758523515,
+            "y": 7.074723860185862,
+            "heading": 0.5147551117811896,
+            "angularVelocity": -5.103818092114042e-7,
+            "velocityX": 3.670703382801095,
+            "velocityY": 0.9146449396035696,
+            "timestamp": 1.709056196265751
+        },
+        {
+            "x": 6.4451900364454335,
+            "y": 7.1550453100044535,
+            "heading": 0.514755066960948,
+            "angularVelocity": -5.10381812997418e-7,
+            "velocityX": 3.6707033828009097,
+            "velocityY": 0.9146449396043118,
+            "timestamp": 1.796873278436829
+        },
+        {
+            "x": 6.767540493462307,
+            "y": 7.235366774175193,
+            "heading": 0.5147550221406476,
+            "angularVelocity": -5.103824823798507e-7,
+            "velocityX": 3.670703342077523,
+            "velocityY": 0.9146451030365995,
+            "timestamp": 1.8846903606079073
+        },
+        {
+            "x": 7.069460815974501,
+            "y": 7.313239150235045,
+            "heading": 0.4557447568037101,
+            "angularVelocity": -0.6719679574638842,
+            "velocityX": 3.438059145759549,
+            "velocityY": 0.8867565869263073,
+            "timestamp": 1.9725074427789855
+        },
+        {
+            "x": 7.3146543806816275,
+            "y": 7.37536322701953,
+            "heading": 0.2979903486408247,
+            "angularVelocity": -1.7963977424752164,
+            "velocityX": 2.7920941876599903,
+            "velocityY": 0.7074258817146762,
+            "timestamp": 2.0603245249500635
+        },
+        {
+            "x": 7.497733370662191,
+            "y": 7.421431644323229,
+            "heading": 0.15573272655395068,
+            "angularVelocity": -1.6199310950657548,
+            "velocityX": 2.084776508787937,
+            "velocityY": 0.5245951717451859,
+            "timestamp": 2.1481416071211417
+        },
+        {
+            "x": 7.619355739034295,
+            "y": 7.45187793306489,
+            "heading": 0.05195662802885072,
+            "angularVelocity": -1.1817302050975884,
+            "velocityX": 1.3849511435049664,
+            "velocityY": 0.34670121107359286,
+            "timestamp": 2.23595868929222
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -0.003,
+            "angularVelocity": -0.6258079484101811,
+            "velocityX": 0.6905747659386144,
+            "velocityY": 0.1721996058312404,
+            "timestamp": 2.323775771463298
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -0.003,
+            "angularVelocity": 0,
+            "velocityX": 2.3316560699920504e-34,
+            "velocityY": 0,
+            "timestamp": 2.4115928536343763
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.traj
index b2886244..0c5693aa 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.traj	
@@ -4,271 +4,325 @@
             "x": 0.71,
             "y": 6.72,
             "heading": 1.04,
-            "angularVelocity": 0,
-            "velocityX": -1.8098951472235817e-30,
-            "velocityY": 0,
+            "angularVelocity": 1.821187546700747e-33,
+            "velocityX": -3.3437580769683535e-34,
+            "velocityY": -3.622513119436198e-34,
             "timestamp": 0
         },
         {
-            "x": 0.7613816362208097,
-            "y": 6.725489442192221,
-            "heading": 0.9931061799728784,
-            "angularVelocity": -0.5863053623193526,
-            "velocityX": 0.6424157559264662,
-            "velocityY": 0.06863355110715653,
-            "timestamp": 0.07998190540440472
-        },
-        {
-            "x": 0.8643774617687672,
-            "y": 6.736651681634022,
-            "heading": 0.9029654144342619,
-            "angularVelocity": -1.127014480123304,
-            "velocityX": 1.287739083323781,
-            "velocityY": 0.1395595589447693,
-            "timestamp": 0.15996381080880945
-        },
-        {
-            "x": 1.019343320389449,
-            "y": 6.753669544326843,
-            "heading": 0.7755395878520636,
-            "angularVelocity": -1.5931831823449016,
-            "velocityX": 1.9375114638385158,
-            "velocityY": 0.21277140881771536,
-            "timestamp": 0.23994571621321417
-        },
-        {
-            "x": 1.2269178654194646,
-            "y": 6.7767659053404685,
-            "heading": 0.62538736801047,
-            "angularVelocity": -1.8773273665136312,
-            "velocityX": 2.5952688171215317,
-            "velocityY": 0.2887698273359004,
-            "timestamp": 0.3199276216176189
-        },
-        {
-            "x": 1.4861217663901574,
-            "y": 6.806716511012647,
-            "heading": 0.5166970392894589,
-            "angularVelocity": -1.3589364765874345,
-            "velocityX": 3.240781770077933,
-            "velocityY": 0.37446726882464465,
-            "timestamp": 0.39990952702202365
-        },
-        {
-            "x": 1.7870125383049598,
-            "y": 6.838519066862702,
-            "heading": 0.5166969975980924,
-            "angularVelocity": -5.212599803241117e-7,
-            "velocityX": 3.76198554402321,
-            "velocityY": 0.39762188321538583,
-            "timestamp": 0.4798914324264284
-        },
-        {
-            "x": 2.0879033719171214,
-            "y": 6.870321038982823,
-            "heading": 0.5166969559088411,
-            "angularVelocity": -5.212335340785354e-7,
-            "velocityX": 3.761986315414672,
-            "velocityY": 0.39761458494048757,
-            "timestamp": 0.5598733378308332
-        },
-        {
-            "x": 2.3887942055296523,
-            "y": 6.902123011099449,
-            "heading": 0.5166969142195897,
-            "angularVelocity": -5.212335365150009e-7,
-            "velocityX": 3.7619863154192914,
-            "velocityY": 0.3976145848967806,
-            "timestamp": 0.6398552432352379
-        },
-        {
-            "x": 2.689685039142183,
-            "y": 6.933924983216074,
-            "heading": 0.516696872530338,
-            "angularVelocity": -5.212335398189914e-7,
-            "velocityX": 3.7619863154192914,
-            "velocityY": 0.3976145848967804,
-            "timestamp": 0.7198371486396427
-        },
-        {
-            "x": 2.990575872754714,
-            "y": 6.9657269553327,
-            "heading": 0.5166968308410861,
-            "angularVelocity": -5.212335433606452e-7,
-            "velocityX": 3.7619863154192914,
-            "velocityY": 0.39761458489678053,
-            "timestamp": 0.7998190540440474
-        },
-        {
-            "x": 3.291466706367245,
-            "y": 6.997528927449325,
-            "heading": 0.5166967891518339,
-            "angularVelocity": -5.212335460884143e-7,
-            "velocityX": 3.761986315419291,
-            "velocityY": 0.3976145848967808,
-            "timestamp": 0.8798009594484522
-        },
-        {
-            "x": 3.5923575399797762,
-            "y": 7.0293308995659505,
-            "heading": 0.5166967474625817,
-            "angularVelocity": -5.212335479348436e-7,
-            "velocityX": 3.761986315419291,
-            "velocityY": 0.3976145848967805,
-            "timestamp": 0.9597828648528569
-        },
-        {
-            "x": 3.893248373592307,
-            "y": 7.0611328716825765,
-            "heading": 0.516696705773329,
-            "angularVelocity": -5.212335516712931e-7,
-            "velocityX": 3.7619863154192905,
-            "velocityY": 0.3976145848967809,
-            "timestamp": 1.0397647702572617
-        },
-        {
-            "x": 4.194139207204837,
-            "y": 7.092934843799203,
-            "heading": 0.5166966640840763,
-            "angularVelocity": -5.212335535473894e-7,
-            "velocityX": 3.7619863154192905,
-            "velocityY": 0.3976145848967809,
-            "timestamp": 1.1197466756616663
-        },
-        {
-            "x": 4.495030040817368,
-            "y": 7.1247368159158295,
-            "heading": 0.5166966223948233,
-            "angularVelocity": -5.212335568545237e-7,
-            "velocityX": 3.7619863154192905,
-            "velocityY": 0.3976145848967808,
-            "timestamp": 1.199728581066071
-        },
-        {
-            "x": 4.7959208744299,
-            "y": 7.156538788032456,
-            "heading": 0.51669658070557,
-            "angularVelocity": -5.212335599678648e-7,
-            "velocityX": 3.7619863154192905,
-            "velocityY": 0.397614584896781,
-            "timestamp": 1.2797104864704756
-        },
-        {
-            "x": 5.096811708042431,
-            "y": 7.1883407601490825,
-            "heading": 0.5166965390163165,
-            "angularVelocity": -5.212335628762146e-7,
-            "velocityX": 3.7619863154192905,
-            "velocityY": 0.3976145848967809,
-            "timestamp": 1.3596923918748802
-        },
-        {
-            "x": 5.3977025416549615,
-            "y": 7.220142732265709,
-            "heading": 0.5166964973270628,
-            "angularVelocity": -5.212335655841188e-7,
-            "velocityX": 3.76198631541929,
-            "velocityY": 0.3976145848967813,
-            "timestamp": 1.4396742972792849
-        },
-        {
-            "x": 5.698593375267492,
-            "y": 7.251944704382335,
-            "heading": 0.5166964556378089,
-            "angularVelocity": -5.212335680260379e-7,
-            "velocityX": 3.7619863154192896,
-            "velocityY": 0.3976145848967809,
-            "timestamp": 1.5196562026836895
-        },
-        {
-            "x": 5.999484208880023,
-            "y": 7.283746676498962,
-            "heading": 0.5166964139485547,
-            "angularVelocity": -5.212335712969897e-7,
-            "velocityX": 3.7619863154192896,
-            "velocityY": 0.3976145848967812,
-            "timestamp": 1.5996381080880941
-        },
-        {
-            "x": 6.300375042492554,
-            "y": 7.315548648615588,
-            "heading": 0.5166963722593003,
-            "angularVelocity": -5.21233573548864e-7,
-            "velocityX": 3.7619863154192896,
-            "velocityY": 0.39761458489678125,
-            "timestamp": 1.6796200134924988
-        },
-        {
-            "x": 6.6012658761048035,
-            "y": 7.347350620734872,
-            "heading": 0.5166963305700458,
-            "angularVelocity": -5.212335762453642e-7,
-            "velocityX": 3.7619863154157773,
-            "velocityY": 0.3976145849300086,
-            "timestamp": 1.7596019188969034
-        },
-        {
-            "x": 6.902156662813089,
-            "y": 7.3791530366221005,
-            "heading": 0.5166962888792155,
-            "angularVelocity": -5.212532766918364e-7,
-            "velocityX": 3.761985728983584,
-            "velocityY": 0.3976201332842598,
-            "timestamp": 1.839583824301308
-        },
-        {
-            "x": 7.162091022778527,
-            "y": 7.409610892063227,
-            "heading": 0.4051307797997224,
-            "angularVelocity": -1.3948843618490365,
-            "velocityX": 3.249914573191993,
-            "velocityY": 0.3808093253983604,
-            "timestamp": 1.9195657297057127
-        },
-        {
-            "x": 7.370036606479622,
-            "y": 7.432974685540162,
-            "heading": 0.25885269617169443,
-            "angularVelocity": -1.8288897080960538,
-            "velocityX": 2.5999078497777726,
-            "velocityY": 0.2921134894049214,
-            "timestamp": 1.9995476351101173
-        },
-        {
-            "x": 7.525326975450784,
-            "y": 7.45013556763307,
-            "heading": 0.1335302309738367,
-            "angularVelocity": -1.5668852169025227,
-            "velocityX": 1.9415687609088876,
-            "velocityY": 0.21455955576625652,
-            "timestamp": 2.079529540514522
-        },
-        {
-            "x": 7.628525640862077,
-            "y": 7.461415071089343,
-            "heading": 0.043910704401711645,
-            "angularVelocity": -1.1204975190199653,
-            "velocityX": 1.2902751552304177,
-            "velocityY": 0.14102569073894916,
-            "timestamp": 2.159511445918927
+            "x": 0.7214806358425874,
+            "y": 6.713430005414815,
+            "heading": 1.0269113199415918,
+            "angularVelocity": -0.32293003145597954,
+            "velocityX": 0.28325561303637664,
+            "velocityY": -0.1620979769229106,
+            "timestamp": 0.04053100914583816
+        },
+        {
+            "x": 0.7444896856754503,
+            "y": 6.700284020801432,
+            "heading": 1.001394622951622,
+            "angularVelocity": -0.6295598734824499,
+            "velocityX": 0.5676900308618518,
+            "velocityY": -0.32434387621788874,
+            "timestamp": 0.08106201829167632
+        },
+        {
+            "x": 0.7790845165676208,
+            "y": 6.680557699463914,
+            "heading": 0.9642667952582056,
+            "angularVelocity": -0.916035116713321,
+            "velocityX": 0.8535398358252526,
+            "velocityY": -0.4866970192264045,
+            "timestamp": 0.12159302743751449
+        },
+        {
+            "x": 0.8253351719483972,
+            "y": 6.654251504147771,
+            "heading": 0.9165522532050936,
+            "angularVelocity": -1.177235481145466,
+            "velocityX": 1.141117785011419,
+            "velocityY": -0.6490387451614623,
+            "timestamp": 0.16212403658335264
+        },
+        {
+            "x": 0.883327254582786,
+            "y": 6.621374965178265,
+            "heading": 0.8595502901552765,
+            "angularVelocity": -1.4063790724951688,
+            "velocityX": 1.4308077656226736,
+            "velocityY": -0.8111453344576391,
+            "timestamp": 0.2026550457291908
+        },
+        {
+            "x": 0.953164245540845,
+            "y": 6.581952530815573,
+            "heading": 0.7949436194315174,
+            "angularVelocity": -1.5940059743218395,
+            "velocityX": 1.7230508795567456,
+            "velocityY": -0.9726487248526893,
+            "timestamp": 0.24318605487502895
+        },
+        {
+            "x": 1.0349687566216221,
+            "y": 6.5360339228116,
+            "heading": 0.7250282822193501,
+            "angularVelocity": -1.724983874953593,
+            "velocityX": 2.0183191291001217,
+            "velocityY": -1.132925356947043,
+            "timestamp": 0.2837170640208671
+        },
+        {
+            "x": 1.1288774102342096,
+            "y": 6.483723562804198,
+            "heading": 0.6532984652847575,
+            "angularVelocity": -1.7697515666707262,
+            "velocityX": 2.316958190571729,
+            "velocityY": -1.2906256495904058,
+            "timestamp": 0.3242480731667053
+        },
+        {
+            "x": 1.2349754296776467,
+            "y": 6.425294263584176,
+            "heading": 0.5860844259008366,
+            "angularVelocity": -1.6583361924711089,
+            "velocityX": 2.6176999211067367,
+            "velocityY": -1.4415949775586157,
+            "timestamp": 0.36477908231254347
+        },
+        {
+            "x": 1.3525875597286727,
+            "y": 6.361480505266573,
+            "heading": 0.5370018573395918,
+            "angularVelocity": -1.2109880705076077,
+            "velocityX": 2.901781439189822,
+            "velocityY": -1.5744428688658776,
+            "timestamp": 0.40531009145838165
+        },
+        {
+            "x": 1.481480430033789,
+            "y": 6.296375538238497,
+            "heading": 0.5147559483468728,
+            "angularVelocity": -0.5488614633964374,
+            "velocityX": 3.1801051348446676,
+            "velocityY": -1.6063001736230227,
+            "timestamp": 0.44584110060421983
+        },
+        {
+            "x": 1.6221023727568105,
+            "y": 6.235265511699909,
+            "heading": 0.514755821640399,
+            "angularVelocity": -0.0000031261613391085408,
+            "velocityX": 3.469490291175273,
+            "velocityY": -1.5077351348125194,
+            "timestamp": 0.486372109750058
+        },
+        {
+            "x": 1.7676170388967807,
+            "y": 6.186949296866798,
+            "heading": 0.5147557675556996,
+            "angularVelocity": -0.0000013344029787740425,
+            "velocityX": 3.5902058499550584,
+            "velocityY": -1.1920802331681561,
+            "timestamp": 0.5269031188958961
+        },
+        {
+            "x": 1.9168630838394165,
+            "y": 6.151812553405762,
+            "heading": 0.5147557009631692,
+            "angularVelocity": -0.000001643002030243893,
+            "velocityX": 3.6822681716515033,
+            "velocityY": -0.866910155989649,
+            "timestamp": 0.5674341280417343
+        },
+        {
+            "x": 2.2488381415683802,
+            "y": 6.139406390407425,
+            "heading": 0.5147556500188685,
+            "angularVelocity": -5.801183495483463e-7,
+            "velocityX": 3.780301616970564,
+            "velocityY": -0.14127277622555945,
+            "timestamp": 0.6552512102128125
+        },
+        {
+            "x": 2.57698446655607,
+            "y": 6.191188083838006,
+            "heading": 0.5147556048038259,
+            "angularVelocity": -5.148775328112685e-7,
+            "velocityX": 3.7367026650739894,
+            "velocityY": 0.58965399612918,
+            "timestamp": 0.7430682923838907
+        },
+        {
+            "x": 2.8993349699211755,
+            "y": 6.271509362001365,
+            "heading": 0.5147555599835898,
+            "angularVelocity": -5.103817495962876e-7,
+            "velocityX": 3.6707038698590346,
+            "velocityY": 0.9146429849136081,
+            "timestamp": 0.8308853745549689
+        },
+        {
+            "x": 3.221685430514468,
+            "y": 6.351830811819113,
+            "heading": 0.514755515163351,
+            "angularVelocity": -5.103817801462039e-7,
+            "velocityX": 3.6707033828033078,
+            "velocityY": 0.9146449395946943,
+            "timestamp": 0.9187024567260471
+        },
+        {
+            "x": 3.5440358911075664,
+            "y": 6.432152261637641,
+            "heading": 0.514755470343112,
+            "angularVelocity": -5.103817833267703e-7,
+            "velocityX": 3.670703382801096,
+            "velocityY": 0.9146449396035715,
+            "timestamp": 1.0065195388971253
+        },
+        {
+            "x": 3.8663863517006654,
+            "y": 6.512473711456168,
+            "heading": 0.5147554255228727,
+            "angularVelocity": -5.103817863197995e-7,
+            "velocityX": 3.6707033828010953,
+            "velocityY": 0.9146449396035711,
+            "timestamp": 1.0943366210682035
+        },
+        {
+            "x": 4.188736812293763,
+            "y": 6.592795161274696,
+            "heading": 0.5147553807026332,
+            "angularVelocity": -5.103817901912557e-7,
+            "velocityX": 3.6707033828010953,
+            "velocityY": 0.9146449396035711,
+            "timestamp": 1.1821537032392817
+        },
+        {
+            "x": 4.511087272886861,
+            "y": 6.673116611093223,
+            "heading": 0.5147553358823933,
+            "angularVelocity": -5.103817929337743e-7,
+            "velocityX": 3.6707033828010958,
+            "velocityY": 0.9146449396035705,
+            "timestamp": 1.26997078541036
+        },
+        {
+            "x": 4.833437733479959,
+            "y": 6.753438060911751,
+            "heading": 0.5147552910621531,
+            "angularVelocity": -5.103817959943963e-7,
+            "velocityX": 3.6707033828010953,
+            "velocityY": 0.9146449396035703,
+            "timestamp": 1.357787867581438
+        },
+        {
+            "x": 5.155788194073057,
+            "y": 6.833759510730278,
+            "heading": 0.5147552462419127,
+            "angularVelocity": -5.10381799453682e-7,
+            "velocityX": 3.670703382801095,
+            "velocityY": 0.9146449396035702,
+            "timestamp": 1.4456049497525163
+        },
+        {
+            "x": 5.4781386546661555,
+            "y": 6.914080960548806,
+            "heading": 0.514755201421672,
+            "angularVelocity": -5.10381803359495e-7,
+            "velocityX": 3.670703382801095,
+            "velocityY": 0.9146449396035701,
+            "timestamp": 1.5334220319235945
+        },
+        {
+            "x": 5.8004891152592535,
+            "y": 6.994402410367334,
+            "heading": 0.5147551566014309,
+            "angularVelocity": -5.1038180633665e-7,
+            "velocityX": 3.670703382801095,
+            "velocityY": 0.9146449396035699,
+            "timestamp": 1.6212391140946727
+        },
+        {
+            "x": 6.1228395758523515,
+            "y": 7.074723860185862,
+            "heading": 0.5147551117811896,
+            "angularVelocity": -5.103818092114042e-7,
+            "velocityX": 3.670703382801095,
+            "velocityY": 0.9146449396035696,
+            "timestamp": 1.709056196265751
+        },
+        {
+            "x": 6.4451900364454335,
+            "y": 7.1550453100044535,
+            "heading": 0.514755066960948,
+            "angularVelocity": -5.10381812997418e-7,
+            "velocityX": 3.6707033828009097,
+            "velocityY": 0.9146449396043118,
+            "timestamp": 1.796873278436829
+        },
+        {
+            "x": 6.767540493462307,
+            "y": 7.235366774175193,
+            "heading": 0.5147550221406476,
+            "angularVelocity": -5.103824823798507e-7,
+            "velocityX": 3.670703342077523,
+            "velocityY": 0.9146451030365995,
+            "timestamp": 1.8846903606079073
+        },
+        {
+            "x": 7.069460815974501,
+            "y": 7.313239150235045,
+            "heading": 0.4557447568037101,
+            "angularVelocity": -0.6719679574638842,
+            "velocityX": 3.438059145759549,
+            "velocityY": 0.8867565869263073,
+            "timestamp": 1.9725074427789855
+        },
+        {
+            "x": 7.3146543806816275,
+            "y": 7.37536322701953,
+            "heading": 0.2979903486408247,
+            "angularVelocity": -1.7963977424752164,
+            "velocityX": 2.7920941876599903,
+            "velocityY": 0.7074258817146762,
+            "timestamp": 2.0603245249500635
+        },
+        {
+            "x": 7.497733370662191,
+            "y": 7.421431644323229,
+            "heading": 0.15573272655395068,
+            "angularVelocity": -1.6199310950657548,
+            "velocityX": 2.084776508787937,
+            "velocityY": 0.5245951717451859,
+            "timestamp": 2.1481416071211417
+        },
+        {
+            "x": 7.619355739034295,
+            "y": 7.45187793306489,
+            "heading": 0.05195662802885072,
+            "angularVelocity": -1.1817302050975884,
+            "velocityX": 1.3849511435049664,
+            "velocityY": 0.34670121107359286,
+            "timestamp": 2.23595868929222
         },
         {
             "x": 7.68,
             "y": 7.467,
             "heading": -0.003,
-            "angularVelocity": -0.5865164647493907,
-            "velocityX": 0.6435750546033896,
-            "velocityY": 0.0698274051164111,
-            "timestamp": 2.2394933513233317
+            "angularVelocity": -0.6258079484101811,
+            "velocityX": 0.6905747659386144,
+            "velocityY": 0.1721996058312404,
+            "timestamp": 2.323775771463298
         },
         {
             "x": 7.68,
             "y": 7.467,
             "heading": -0.003,
             "angularVelocity": 0,
-            "velocityX": 4.312091780947568e-36,
+            "velocityX": 2.3316560699920504e-34,
             "velocityY": 0,
-            "timestamp": 2.3194752567277366
+            "timestamp": 2.4115928536343763
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.1.traj
new file mode 100644
index 00000000..38a8bead
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.1.traj	
@@ -0,0 +1,364 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": 1.04,
+            "angularVelocity": -7.919640222280865e-31,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7245966822673344,
+            "y": 6.714849739719678,
+            "heading": 1.024358911223375,
+            "angularVelocity": -0.35643014194665995,
+            "velocityX": 0.33263013891161514,
+            "velocityY": -0.1173644641361684,
+            "timestamp": 0.04388262084457728
+        },
+        {
+            "x": 0.7538441442080397,
+            "y": 6.704534883949825,
+            "heading": 0.9938971402916456,
+            "angularVelocity": -0.6941648047781096,
+            "velocityX": 0.6664930530083778,
+            "velocityY": -0.23505559994630235,
+            "timestamp": 0.08776524168915456
+        },
+        {
+            "x": 0.7978084886925392,
+            "y": 6.689039857166789,
+            "heading": 0.9496619935965729,
+            "angularVelocity": -1.0080333818652591,
+            "velocityX": 1.0018623235886674,
+            "velocityY": -0.3531016717960754,
+            "timestamp": 0.13164786253373184
+        },
+        {
+            "x": 0.856569122575896,
+            "y": 6.668349449234606,
+            "heading": 0.8929800253175195,
+            "angularVelocity": -1.291672356576085,
+            "velocityX": 1.3390411227143098,
+            "velocityY": -0.4714943532072734,
+            "timestamp": 0.17553048337830912
+        },
+        {
+            "x": 0.9302192448226602,
+            "y": 6.642448160442897,
+            "heading": 0.8255526698192454,
+            "angularVelocity": -1.5365389350167389,
+            "velocityX": 1.6783437458676926,
+            "velocityY": -0.5902402430203169,
+            "timestamp": 0.2194131042228864
+        },
+        {
+            "x": 1.018865391503866,
+            "y": 6.6113159895257,
+            "heading": 0.7496899973128085,
+            "angularVelocity": -1.728763484184805,
+            "velocityX": 2.020074119893375,
+            "velocityY": -0.7094419229748447,
+            "timestamp": 0.2632957250674637
+        },
+        {
+            "x": 1.122620892464334,
+            "y": 6.574924283447733,
+            "heading": 0.6689630682339129,
+            "angularVelocity": -1.839610477341524,
+            "velocityX": 2.364387061747502,
+            "velocityY": -0.8292965501492129,
+            "timestamp": 0.30717834591204096
+        },
+        {
+            "x": 1.2415335684286195,
+            "y": 6.533261700289126,
+            "heading": 0.5901648629283005,
+            "angularVelocity": -1.7956585953398425,
+            "velocityX": 2.7097897453635067,
+            "velocityY": -0.9494096377266702,
+            "timestamp": 0.35106096675661824
+        },
+        {
+            "x": 1.3748018786634473,
+            "y": 6.486525528750173,
+            "heading": 0.5293493772235701,
+            "angularVelocity": -1.3858672188271242,
+            "velocityX": 3.03692686694496,
+            "velocityY": -1.0650268976507753,
+            "timestamp": 0.3949435876011955
+        },
+        {
+            "x": 1.5197726766571482,
+            "y": 6.4335443318697845,
+            "heading": 0.5007639504532418,
+            "angularVelocity": -0.6514065527572436,
+            "velocityX": 3.3036039143420037,
+            "velocityY": -1.2073389387557742,
+            "timestamp": 0.4388262084457728
+        },
+        {
+            "x": 1.6765216724762246,
+            "y": 6.378886133078717,
+            "heading": 0.5007639037196894,
+            "angularVelocity": -0.0000010649672122148667,
+            "velocityX": 3.572006247627606,
+            "velocityY": -1.2455545666831078,
+            "timestamp": 0.4827088292903501
+        },
+        {
+            "x": 1.8335562881896872,
+            "y": 6.325054024772622,
+            "heading": 0.5007638569717894,
+            "angularVelocity": -0.0000010652941670826544,
+            "velocityX": 3.5785149722331555,
+            "velocityY": -1.2267295633425939,
+            "timestamp": 0.5265914501349274
+        },
+        {
+            "x": 1.9950553751457016,
+            "y": 6.286637737718841,
+            "heading": 0.5007638092073705,
+            "angularVelocity": -0.0000010884586644439279,
+            "velocityX": 3.6802516314615477,
+            "velocityY": -0.8754328322739401,
+            "timestamp": 0.5704740709795046
+        },
+        {
+            "x": 2.159510374069214,
+            "y": 6.264003276824951,
+            "heading": 0.500763757205963,
+            "angularVelocity": -0.0000011850114334401085,
+            "velocityX": 3.7476111444210547,
+            "velocityY": -0.5157955577412445,
+            "timestamp": 0.6143566918240819
+        },
+        {
+            "x": 2.4127218287177645,
+            "y": 6.26676408374459,
+            "heading": 0.5007637073683532,
+            "angularVelocity": -7.445219928061725e-7,
+            "velocityX": 3.7827154561393628,
+            "velocityY": 0.04124358046976413,
+            "timestamp": 0.681295761808209
+        },
+        {
+            "x": 2.6627712176567337,
+            "y": 6.306751164644663,
+            "heading": 0.5007636590304977,
+            "angularVelocity": -7.221172264287654e-7,
+            "velocityX": 3.7354774871878274,
+            "velocityY": 0.5973653489565757,
+            "timestamp": 0.7482348317923362
+        },
+        {
+            "x": 2.9117460938747426,
+            "y": 6.352959114176208,
+            "heading": 0.5007636106421453,
+            "angularVelocity": -7.22871595873179e-7,
+            "velocityX": 3.719425386056917,
+            "velocityY": 0.6902986483459334,
+            "timestamp": 0.8151739017764633
+        },
+        {
+            "x": 3.160720959556854,
+            "y": 6.399167120476624,
+            "heading": 0.5007635622537979,
+            "angularVelocity": -7.228715220114282e-7,
+            "velocityX": 3.719425228661581,
+            "velocityY": 0.6902994964137207,
+            "timestamp": 0.8821129717605904
+        },
+        {
+            "x": 3.409695827610573,
+            "y": 6.445375113998521,
+            "heading": 0.5007635138654579,
+            "angularVelocity": -7.228714109969877e-7,
+            "velocityX": 3.719425264090935,
+            "velocityY": 0.6902993055159711,
+            "timestamp": 0.9490520417447176
+        },
+        {
+            "x": 3.658670698533997,
+            "y": 6.491583092058085,
+            "heading": 0.5007634654771249,
+            "angularVelocity": -7.228713077638952e-7,
+            "velocityX": 3.719425306961334,
+            "velocityY": 0.6902990745248123,
+            "timestamp": 1.0159911117288447
+        },
+        {
+            "x": 3.9076455729725814,
+            "y": 6.537791051177508,
+            "heading": 0.5007634170887977,
+            "angularVelocity": -7.228712197353323e-7,
+            "velocityX": 3.7194253594741626,
+            "velocityY": 0.6902987915788225,
+            "timestamp": 1.082930181712972
+        },
+        {
+            "x": 4.156620451761658,
+            "y": 6.583998986855888,
+            "heading": 0.5007633687004744,
+            "angularVelocity": -7.228711615866084e-7,
+            "velocityX": 3.7194254244659963,
+            "velocityY": 0.6902984413940524,
+            "timestamp": 1.1498692516970992
+        },
+        {
+            "x": 4.405595335954841,
+            "y": 6.630206893416186,
+            "heading": 0.5007633203121516,
+            "angularVelocity": -7.22871154439704e-7,
+            "velocityX": 3.719425505197721,
+            "velocityY": 0.6902980064002329,
+            "timestamp": 1.2168083216812264
+        },
+        {
+            "x": 4.65457022677848,
+            "y": 6.676414764250624,
+            "heading": 0.5007632719238245,
+            "angularVelocity": -7.228712197620323e-7,
+            "velocityX": 3.719425604249829,
+            "velocityY": 0.690297472693087,
+            "timestamp": 1.2837473916653537
+        },
+        {
+            "x": 4.903545125380086,
+            "y": 6.722622593176218,
+            "heading": 0.5007632235354964,
+            "angularVelocity": -7.228712343344299e-7,
+            "velocityX": 3.7194257204445353,
+            "velocityY": 0.69029684661859,
+            "timestamp": 1.350686461649481
+        },
+        {
+            "x": 5.152520035964963,
+            "y": 6.768830357534066,
+            "heading": 0.500763175147177,
+            "angularVelocity": -7.228711020086952e-7,
+            "velocityX": 3.7194258994621494,
+            "velocityY": 0.6902958820437133,
+            "timestamp": 1.4176255316336082
+        },
+        {
+            "x": 5.401781723000098,
+            "y": 6.813465264887065,
+            "heading": 0.5007631267776413,
+            "angularVelocity": -7.225904944160688e-7,
+            "velocityX": 3.7237100410005364,
+            "velocityY": 0.6667990362518919,
+            "timestamp": 1.4845646016177354
+        },
+        {
+            "x": 5.654897775782049,
+            "y": 6.8209436894170015,
+            "heading": 0.5007630775196736,
+            "angularVelocity": -7.358627453849351e-7,
+            "velocityX": 3.781290251597022,
+            "velocityY": 0.11171987498067794,
+            "timestamp": 1.5515036716018626
+        },
+        {
+            "x": 5.9063591957092285,
+            "y": 6.791097164154053,
+            "heading": 0.500763020915307,
+            "angularVelocity": -8.456102929151622e-7,
+            "velocityX": 3.756571759763139,
+            "velocityY": -0.44587600739018746,
+            "timestamp": 1.6184427415859899
+        },
+        {
+            "x": 6.200798567763329,
+            "y": 6.700713487178965,
+            "heading": 0.5007629755779084,
+            "angularVelocity": -5.568470838728409e-7,
+            "velocityX": 3.616389792279645,
+            "velocityY": -1.1101185433221765,
+            "timestamp": 1.6998607824959264
+        },
+        {
+            "x": 6.474342410112796,
+            "y": 6.559159535827226,
+            "heading": 0.5007629354438458,
+            "angularVelocity": -4.929382012989922e-7,
+            "velocityX": 3.3597448340971368,
+            "velocityY": -1.7386066990769444,
+            "timestamp": 1.781278823405863
+        },
+        {
+            "x": 6.734766818786864,
+            "y": 6.394712333057686,
+            "heading": 0.5007628957809265,
+            "angularVelocity": -4.871514819255018e-7,
+            "velocityX": 3.1986081433983946,
+            "velocityY": -2.0197882549329598,
+            "timestamp": 1.8626968643157995
+        },
+        {
+            "x": 6.9951910734023155,
+            "y": 6.230264886316009,
+            "heading": 0.5007628561176116,
+            "angularVelocity": -4.871563403692846e-7,
+            "velocityX": 3.198606251205787,
+            "velocityY": -2.019791251469529,
+            "timestamp": 1.944114905225736
+        },
+        {
+            "x": 7.223286683326485,
+            "y": 6.0852317833380125,
+            "heading": 0.4070504938542789,
+            "angularVelocity": -1.1510024217727797,
+            "velocityX": 2.801536457705794,
+            "velocityY": -1.781338648745319,
+            "timestamp": 2.0255329461356726
+        },
+        {
+            "x": 7.40663062232063,
+            "y": 5.969243166944327,
+            "heading": 0.26222076447726717,
+            "angularVelocity": -1.7788407551738947,
+            "velocityX": 2.2518834516904875,
+            "velocityY": -1.4246058379369684,
+            "timestamp": 2.106950987045609
+        },
+        {
+            "x": 7.543566910533025,
+            "y": 5.882836756425157,
+            "heading": 0.1356651998410208,
+            "angularVelocity": -1.5543921620054602,
+            "velocityX": 1.6818912206923584,
+            "velocityY": -1.061268602799615,
+            "timestamp": 2.1883690279555457
+        },
+        {
+            "x": 7.634586620516168,
+            "y": 5.825537139087579,
+            "heading": 0.04464428184824825,
+            "angularVelocity": -1.1179453223820228,
+            "velocityX": 1.1179304852572838,
+            "velocityY": -0.7037705242866615,
+            "timestamp": 2.269787068865482
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -0.003,
+            "angularVelocity": -0.5851808925364741,
+            "velocityX": 0.5577803024524691,
+            "velocityY": -0.35050142165846687,
+            "timestamp": 2.3512051097754187
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -0.003,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 2.4326231506853553
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.traj
index 8c7890ab..38a8bead 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.traj	
@@ -4,316 +4,361 @@
             "x": 0.71,
             "y": 6.72,
             "heading": 1.04,
-            "angularVelocity": 0,
-            "velocityX": 2.674890564447798e-33,
-            "velocityY": 3.559444452002106e-32,
+            "angularVelocity": -7.919640222280865e-31,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.744719766454751,
-            "y": 6.720759968199705,
-            "heading": 1.006760857121406,
-            "angularVelocity": -0.5061419234074507,
-            "velocityX": 0.5286878015432446,
-            "velocityY": 0.011572252862695013,
-            "timestamp": 0.06567158605400746
-        },
-        {
-            "x": 0.8143576636925598,
-            "y": 6.722372451876338,
-            "heading": 0.9431499891720047,
-            "angularVelocity": -0.9686208567749168,
-            "velocityX": 1.0603961533765867,
-            "velocityY": 0.024553749551892837,
-            "timestamp": 0.1313431721080149
-        },
-        {
-            "x": 0.9191612228651693,
-            "y": 6.724923861576486,
-            "heading": 0.8530685118915011,
-            "angularVelocity": -1.3716963864162066,
-            "velocityX": 1.5958737327650472,
-            "velocityY": 0.03885104431723037,
-            "timestamp": 0.19701475816202235
-        },
-        {
-            "x": 1.059454644842809,
-            "y": 6.7284663342548425,
-            "heading": 0.7426650921374723,
-            "angularVelocity": -1.6811444094442096,
-            "velocityX": 2.1362880114128,
-            "velocityY": 0.05394224338464699,
-            "timestamp": 0.2626863442160298
-        },
-        {
-            "x": 1.2356244745863587,
-            "y": 6.733039509062333,
-            "heading": 0.6243118436370656,
-            "angularVelocity": -1.8021987226420155,
-            "velocityX": 2.682588320596835,
-            "velocityY": 0.06963703912619325,
-            "timestamp": 0.3283579302700373
-        },
-        {
-            "x": 1.4470569399809903,
-            "y": 6.738777367450543,
-            "heading": 0.5308220213971302,
-            "angularVelocity": -1.4235962287107038,
-            "velocityX": 3.219542546463745,
-            "velocityY": 0.08737200870294055,
-            "timestamp": 0.39402951632404476
-        },
-        {
-            "x": 1.688226065456379,
-            "y": 6.748371696739962,
-            "heading": 0.5126606987274956,
-            "angularVelocity": -0.2765476480908373,
-            "velocityX": 3.6723511638207493,
-            "velocityY": 0.14609559272058092,
-            "timestamp": 0.45970110237805223
-        },
-        {
-            "x": 1.9365780121804386,
-            "y": 6.754665679712547,
-            "heading": 0.5126606479364623,
-            "angularVelocity": -7.734095747362075e-7,
-            "velocityX": 3.7817260347544686,
-            "velocityY": 0.09584027660688858,
-            "timestamp": 0.5253726884320596
-        },
-        {
-            "x": 2.1849299738494317,
-            "y": 6.760959072962077,
-            "heading": 0.5126605971462416,
-            "angularVelocity": -7.733971982996713e-7,
-            "velocityX": 3.7817262623252414,
-            "velocityY": 0.09583129672545204,
-            "timestamp": 0.5910442744860671
-        },
-        {
-            "x": 2.433281935680659,
-            "y": 6.767252459809602,
-            "heading": 0.5126605463560266,
-            "angularVelocity": -7.733971099299294e-7,
-            "velocityX": 3.7817262647956325,
-            "velocityY": 0.09583119924030911,
-            "timestamp": 0.6567158605400746
-        },
-        {
-            "x": 2.6816338977021794,
-            "y": 6.773545839147862,
-            "heading": 0.5126604955658177,
-            "angularVelocity": -7.733970196193196e-7,
-            "velocityX": 3.78172626769327,
-            "velocityY": 0.09583108489457716,
-            "timestamp": 0.722387446594082
-        },
-        {
-            "x": 2.9299858599438156,
-            "y": 6.779839209799928,
-            "heading": 0.5126604447756148,
-            "angularVelocity": -7.733969286509421e-7,
-            "velocityX": 3.7817262710450463,
-            "velocityY": 0.09583095262742326,
-            "timestamp": 0.7880590326480895
-        },
-        {
-            "x": 3.1783378224424883,
-            "y": 6.786132570308856,
-            "heading": 0.5126603939854177,
-            "angularVelocity": -7.733968385113031e-7,
-            "velocityX": 3.781726274959007,
-            "velocityY": 0.09583079817497217,
-            "timestamp": 0.853730618702097
-        },
-        {
-            "x": 3.426689785244194,
-            "y": 6.792425918859452,
-            "heading": 0.5126603431952265,
-            "angularVelocity": -7.733967515033378e-7,
-            "velocityX": 3.7817262795733857,
-            "velocityY": 0.09583061608201786,
-            "timestamp": 0.9194022047561045
-        },
-        {
-            "x": 3.6750417484067377,
-            "y": 6.798719253170605,
-            "heading": 0.5126602924050404,
-            "angularVelocity": -7.733966712943809e-7,
-            "velocityX": 3.781726285067957,
-            "velocityY": 0.09583039925395942,
-            "timestamp": 0.9850737908101119
-        },
-        {
-            "x": 3.9233937120031053,
-            "y": 6.805012570362013,
-            "heading": 0.5126602416148589,
-            "angularVelocity": -7.733966025007139e-7,
-            "velocityX": 3.7817262916739214,
-            "velocityY": 0.095830138566711,
-            "timestamp": 1.0507453768641193
-        },
-        {
-            "x": 4.171745676125189,
-            "y": 6.811305866807266,
-            "heading": 0.5126601908246804,
-            "angularVelocity": -7.733965561950186e-7,
-            "velocityX": 3.7817262996791623,
-            "velocityY": 0.09582982265909266,
-            "timestamp": 1.1164169629181266
-        },
-        {
-            "x": 4.420097640886595,
-            "y": 6.817599138023107,
-            "heading": 0.5126601400345029,
-            "angularVelocity": -7.733965406200526e-7,
-            "velocityX": 3.781726309414286,
-            "velocityY": 0.09582943848362173,
-            "timestamp": 1.182088548972134
-        },
-        {
-            "x": 4.6684496064150185,
-            "y": 6.8238923789700765,
-            "heading": 0.5126600892443262,
-            "angularVelocity": -7.73396529455525e-7,
-            "velocityX": 3.7817263210938923,
-            "velocityY": 0.09582897757087808,
-            "timestamp": 1.2477601350261414
-        },
-        {
-            "x": 4.916801572841165,
-            "y": 6.830185584489968,
-            "heading": 0.5126600384541553,
-            "angularVelocity": -7.733964406232834e-7,
-            "velocityX": 3.7817263347637717,
-            "velocityY": 0.09582843811261686,
-            "timestamp": 1.3134317210801487
-        },
-        {
-            "x": 5.165153539254683,
-            "y": 6.8364787905062485,
-            "heading": 0.5126599876639959,
-            "angularVelocity": -7.733962639482034e-7,
-            "velocityX": 3.781726334571479,
-            "velocityY": 0.09582844567126618,
-            "timestamp": 1.379103307134156
-        },
-        {
-            "x": 5.413504500375102,
-            "y": 6.842811545538402,
-            "heading": 0.5126599368726894,
-            "angularVelocity": -7.734137320837285e-7,
-            "velocityX": 3.781711026686433,
-            "velocityY": 0.09643066982787928,
-            "timestamp": 1.4447748931881634
-        },
-        {
-            "x": 5.661808131063952,
-            "y": 6.834835924305975,
-            "heading": 0.5126598860199024,
-            "angularVelocity": -7.743499133059843e-7,
-            "velocityX": 3.780990312684674,
-            "velocityY": -0.12144706275568991,
-            "timestamp": 1.5104464792421708
+            "x": 0.7245966822673344,
+            "y": 6.714849739719678,
+            "heading": 1.024358911223375,
+            "angularVelocity": -0.35643014194665995,
+            "velocityX": 0.33263013891161514,
+            "velocityY": -0.1173644641361684,
+            "timestamp": 0.04388262084457728
+        },
+        {
+            "x": 0.7538441442080397,
+            "y": 6.704534883949825,
+            "heading": 0.9938971402916456,
+            "angularVelocity": -0.6941648047781096,
+            "velocityX": 0.6664930530083778,
+            "velocityY": -0.23505559994630235,
+            "timestamp": 0.08776524168915456
+        },
+        {
+            "x": 0.7978084886925392,
+            "y": 6.689039857166789,
+            "heading": 0.9496619935965729,
+            "angularVelocity": -1.0080333818652591,
+            "velocityX": 1.0018623235886674,
+            "velocityY": -0.3531016717960754,
+            "timestamp": 0.13164786253373184
+        },
+        {
+            "x": 0.856569122575896,
+            "y": 6.668349449234606,
+            "heading": 0.8929800253175195,
+            "angularVelocity": -1.291672356576085,
+            "velocityX": 1.3390411227143098,
+            "velocityY": -0.4714943532072734,
+            "timestamp": 0.17553048337830912
+        },
+        {
+            "x": 0.9302192448226602,
+            "y": 6.642448160442897,
+            "heading": 0.8255526698192454,
+            "angularVelocity": -1.5365389350167389,
+            "velocityX": 1.6783437458676926,
+            "velocityY": -0.5902402430203169,
+            "timestamp": 0.2194131042228864
+        },
+        {
+            "x": 1.018865391503866,
+            "y": 6.6113159895257,
+            "heading": 0.7496899973128085,
+            "angularVelocity": -1.728763484184805,
+            "velocityX": 2.020074119893375,
+            "velocityY": -0.7094419229748447,
+            "timestamp": 0.2632957250674637
+        },
+        {
+            "x": 1.122620892464334,
+            "y": 6.574924283447733,
+            "heading": 0.6689630682339129,
+            "angularVelocity": -1.839610477341524,
+            "velocityX": 2.364387061747502,
+            "velocityY": -0.8292965501492129,
+            "timestamp": 0.30717834591204096
+        },
+        {
+            "x": 1.2415335684286195,
+            "y": 6.533261700289126,
+            "heading": 0.5901648629283005,
+            "angularVelocity": -1.7956585953398425,
+            "velocityX": 2.7097897453635067,
+            "velocityY": -0.9494096377266702,
+            "timestamp": 0.35106096675661824
+        },
+        {
+            "x": 1.3748018786634473,
+            "y": 6.486525528750173,
+            "heading": 0.5293493772235701,
+            "angularVelocity": -1.3858672188271242,
+            "velocityX": 3.03692686694496,
+            "velocityY": -1.0650268976507753,
+            "timestamp": 0.3949435876011955
+        },
+        {
+            "x": 1.5197726766571482,
+            "y": 6.4335443318697845,
+            "heading": 0.5007639504532418,
+            "angularVelocity": -0.6514065527572436,
+            "velocityX": 3.3036039143420037,
+            "velocityY": -1.2073389387557742,
+            "timestamp": 0.4388262084457728
+        },
+        {
+            "x": 1.6765216724762246,
+            "y": 6.378886133078717,
+            "heading": 0.5007639037196894,
+            "angularVelocity": -0.0000010649672122148667,
+            "velocityX": 3.572006247627606,
+            "velocityY": -1.2455545666831078,
+            "timestamp": 0.4827088292903501
+        },
+        {
+            "x": 1.8335562881896872,
+            "y": 6.325054024772622,
+            "heading": 0.5007638569717894,
+            "angularVelocity": -0.0000010652941670826544,
+            "velocityX": 3.5785149722331555,
+            "velocityY": -1.2267295633425939,
+            "timestamp": 0.5265914501349274
+        },
+        {
+            "x": 1.9950553751457016,
+            "y": 6.286637737718841,
+            "heading": 0.5007638092073705,
+            "angularVelocity": -0.0000010884586644439279,
+            "velocityX": 3.6802516314615477,
+            "velocityY": -0.8754328322739401,
+            "timestamp": 0.5704740709795046
+        },
+        {
+            "x": 2.159510374069214,
+            "y": 6.264003276824951,
+            "heading": 0.500763757205963,
+            "angularVelocity": -0.0000011850114334401085,
+            "velocityX": 3.7476111444210547,
+            "velocityY": -0.5157955577412445,
+            "timestamp": 0.6143566918240819
+        },
+        {
+            "x": 2.4127218287177645,
+            "y": 6.26676408374459,
+            "heading": 0.5007637073683532,
+            "angularVelocity": -7.445219928061725e-7,
+            "velocityX": 3.7827154561393628,
+            "velocityY": 0.04124358046976413,
+            "timestamp": 0.681295761808209
+        },
+        {
+            "x": 2.6627712176567337,
+            "y": 6.306751164644663,
+            "heading": 0.5007636590304977,
+            "angularVelocity": -7.221172264287654e-7,
+            "velocityX": 3.7354774871878274,
+            "velocityY": 0.5973653489565757,
+            "timestamp": 0.7482348317923362
+        },
+        {
+            "x": 2.9117460938747426,
+            "y": 6.352959114176208,
+            "heading": 0.5007636106421453,
+            "angularVelocity": -7.22871595873179e-7,
+            "velocityX": 3.719425386056917,
+            "velocityY": 0.6902986483459334,
+            "timestamp": 0.8151739017764633
+        },
+        {
+            "x": 3.160720959556854,
+            "y": 6.399167120476624,
+            "heading": 0.5007635622537979,
+            "angularVelocity": -7.228715220114282e-7,
+            "velocityX": 3.719425228661581,
+            "velocityY": 0.6902994964137207,
+            "timestamp": 0.8821129717605904
+        },
+        {
+            "x": 3.409695827610573,
+            "y": 6.445375113998521,
+            "heading": 0.5007635138654579,
+            "angularVelocity": -7.228714109969877e-7,
+            "velocityX": 3.719425264090935,
+            "velocityY": 0.6902993055159711,
+            "timestamp": 0.9490520417447176
+        },
+        {
+            "x": 3.658670698533997,
+            "y": 6.491583092058085,
+            "heading": 0.5007634654771249,
+            "angularVelocity": -7.228713077638952e-7,
+            "velocityX": 3.719425306961334,
+            "velocityY": 0.6902990745248123,
+            "timestamp": 1.0159911117288447
+        },
+        {
+            "x": 3.9076455729725814,
+            "y": 6.537791051177508,
+            "heading": 0.5007634170887977,
+            "angularVelocity": -7.228712197353323e-7,
+            "velocityX": 3.7194253594741626,
+            "velocityY": 0.6902987915788225,
+            "timestamp": 1.082930181712972
+        },
+        {
+            "x": 4.156620451761658,
+            "y": 6.583998986855888,
+            "heading": 0.5007633687004744,
+            "angularVelocity": -7.228711615866084e-7,
+            "velocityX": 3.7194254244659963,
+            "velocityY": 0.6902984413940524,
+            "timestamp": 1.1498692516970992
+        },
+        {
+            "x": 4.405595335954841,
+            "y": 6.630206893416186,
+            "heading": 0.5007633203121516,
+            "angularVelocity": -7.22871154439704e-7,
+            "velocityX": 3.719425505197721,
+            "velocityY": 0.6902980064002329,
+            "timestamp": 1.2168083216812264
+        },
+        {
+            "x": 4.65457022677848,
+            "y": 6.676414764250624,
+            "heading": 0.5007632719238245,
+            "angularVelocity": -7.228712197620323e-7,
+            "velocityX": 3.719425604249829,
+            "velocityY": 0.690297472693087,
+            "timestamp": 1.2837473916653537
+        },
+        {
+            "x": 4.903545125380086,
+            "y": 6.722622593176218,
+            "heading": 0.5007632235354964,
+            "angularVelocity": -7.228712343344299e-7,
+            "velocityX": 3.7194257204445353,
+            "velocityY": 0.69029684661859,
+            "timestamp": 1.350686461649481
+        },
+        {
+            "x": 5.152520035964963,
+            "y": 6.768830357534066,
+            "heading": 0.500763175147177,
+            "angularVelocity": -7.228711020086952e-7,
+            "velocityX": 3.7194258994621494,
+            "velocityY": 0.6902958820437133,
+            "timestamp": 1.4176255316336082
+        },
+        {
+            "x": 5.401781723000098,
+            "y": 6.813465264887065,
+            "heading": 0.5007631267776413,
+            "angularVelocity": -7.225904944160688e-7,
+            "velocityX": 3.7237100410005364,
+            "velocityY": 0.6667990362518919,
+            "timestamp": 1.4845646016177354
+        },
+        {
+            "x": 5.654897775782049,
+            "y": 6.8209436894170015,
+            "heading": 0.5007630775196736,
+            "angularVelocity": -7.358627453849351e-7,
+            "velocityX": 3.781290251597022,
+            "velocityY": 0.11171987498067794,
+            "timestamp": 1.5515036716018626
         },
         {
             "x": 5.9063591957092285,
             "y": 6.791097164154053,
-            "heading": 0.5126598314406843,
-            "angularVelocity": -8.310933457001734e-7,
-            "velocityX": 3.723848917614642,
-            "velocityY": -0.6660225948568639,
-            "timestamp": 1.5761180652961782
-        },
-        {
-            "x": 6.194622556481831,
-            "y": 6.683762426219015,
-            "heading": 0.5126597887806125,
-            "angularVelocity": -5.246475178274693e-7,
-            "velocityX": 3.5451571072929595,
-            "velocityY": -1.3200377183874616,
-            "timestamp": 1.65742993725234
-        },
-        {
-            "x": 6.459547388261047,
-            "y": 6.527457130068993,
-            "heading": 0.5126597482300577,
-            "angularVelocity": -4.987039864896594e-7,
-            "velocityX": 3.2581322432449182,
-            "velocityY": -1.9222936625379825,
-            "timestamp": 1.7387418092085019
-        },
-        {
-            "x": 6.723521508223802,
-            "y": 6.3695515377198895,
-            "heading": 0.5126597076877473,
-            "angularVelocity": -4.98602596336278e-7,
-            "velocityX": 3.2464400783305223,
-            "velocityY": -1.9419746286809643,
-            "timestamp": 1.8200536811646637
-        },
-        {
-            "x": 6.987495332330779,
-            "y": 6.211645450805702,
-            "heading": 0.5126596671480953,
-            "angularVelocity": -4.985699012832353e-7,
-            "velocityX": 3.246436439802837,
-            "velocityY": -1.9419807109984206,
-            "timestamp": 1.9013655531208256
-        },
-        {
-            "x": 7.218138260463332,
-            "y": 6.072816355413413,
-            "heading": 0.4174347514218684,
-            "angularVelocity": -1.1711071635093024,
-            "velocityX": 2.8365221779297074,
-            "velocityY": -1.7073656288101424,
-            "timestamp": 1.9826774250769874
-        },
-        {
-            "x": 7.40359391202109,
-            "y": 5.961763553999033,
-            "heading": 0.26911254592387807,
-            "angularVelocity": -1.8241150022712385,
-            "velocityX": 2.2807942689821186,
-            "velocityY": -1.365763679308134,
-            "timestamp": 2.063989297033149
-        },
-        {
-            "x": 7.542069024042313,
-            "y": 5.879081457057784,
-            "heading": 0.13934312775190755,
-            "angularVelocity": -1.595946754760921,
-            "velocityX": 1.7030122255201257,
-            "velocityY": -1.0168514750927211,
-            "timestamp": 2.145301168989311
-        },
-        {
-            "x": 7.63409233729861,
-            "y": 5.824280880315082,
-            "heading": 0.04592275308495019,
-            "angularVelocity": -1.1489143272623823,
-            "velocityX": 1.1317328090287153,
-            "velocityY": -0.6739554191077514,
-            "timestamp": 2.2266130409454727
+            "heading": 0.500763020915307,
+            "angularVelocity": -8.456102929151622e-7,
+            "velocityX": 3.756571759763139,
+            "velocityY": -0.44587600739018746,
+            "timestamp": 1.6184427415859899
+        },
+        {
+            "x": 6.200798567763329,
+            "y": 6.700713487178965,
+            "heading": 0.5007629755779084,
+            "angularVelocity": -5.568470838728409e-7,
+            "velocityX": 3.616389792279645,
+            "velocityY": -1.1101185433221765,
+            "timestamp": 1.6998607824959264
+        },
+        {
+            "x": 6.474342410112796,
+            "y": 6.559159535827226,
+            "heading": 0.5007629354438458,
+            "angularVelocity": -4.929382012989922e-7,
+            "velocityX": 3.3597448340971368,
+            "velocityY": -1.7386066990769444,
+            "timestamp": 1.781278823405863
+        },
+        {
+            "x": 6.734766818786864,
+            "y": 6.394712333057686,
+            "heading": 0.5007628957809265,
+            "angularVelocity": -4.871514819255018e-7,
+            "velocityX": 3.1986081433983946,
+            "velocityY": -2.0197882549329598,
+            "timestamp": 1.8626968643157995
+        },
+        {
+            "x": 6.9951910734023155,
+            "y": 6.230264886316009,
+            "heading": 0.5007628561176116,
+            "angularVelocity": -4.871563403692846e-7,
+            "velocityX": 3.198606251205787,
+            "velocityY": -2.019791251469529,
+            "timestamp": 1.944114905225736
+        },
+        {
+            "x": 7.223286683326485,
+            "y": 6.0852317833380125,
+            "heading": 0.4070504938542789,
+            "angularVelocity": -1.1510024217727797,
+            "velocityX": 2.801536457705794,
+            "velocityY": -1.781338648745319,
+            "timestamp": 2.0255329461356726
+        },
+        {
+            "x": 7.40663062232063,
+            "y": 5.969243166944327,
+            "heading": 0.26222076447726717,
+            "angularVelocity": -1.7788407551738947,
+            "velocityX": 2.2518834516904875,
+            "velocityY": -1.4246058379369684,
+            "timestamp": 2.106950987045609
+        },
+        {
+            "x": 7.543566910533025,
+            "y": 5.882836756425157,
+            "heading": 0.1356651998410208,
+            "angularVelocity": -1.5543921620054602,
+            "velocityX": 1.6818912206923584,
+            "velocityY": -1.061268602799615,
+            "timestamp": 2.1883690279555457
+        },
+        {
+            "x": 7.634586620516168,
+            "y": 5.825537139087579,
+            "heading": 0.04464428184824825,
+            "angularVelocity": -1.1179453223820228,
+            "velocityX": 1.1179304852572838,
+            "velocityY": -0.7037705242866615,
+            "timestamp": 2.269787068865482
         },
         {
             "x": 7.68,
             "y": 5.797,
             "heading": -0.003,
-            "angularVelocity": -0.6016680210157614,
-            "velocityX": 0.5645874531846384,
-            "velocityY": -0.3355091902172136,
-            "timestamp": 2.3079249129016346
+            "angularVelocity": -0.5851808925364741,
+            "velocityX": 0.5577803024524691,
+            "velocityY": -0.35050142165846687,
+            "timestamp": 2.3512051097754187
         },
         {
             "x": 7.68,
             "y": 5.797,
             "heading": -0.003,
-            "angularVelocity": 3.440382093642737e-30,
+            "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": 2.1122019835026996e-31,
-            "timestamp": 2.3892367848577964
+            "velocityY": 0,
+            "timestamp": 2.4326231506853553
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.1.traj
new file mode 100644
index 00000000..78068220
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.1.traj	
@@ -0,0 +1,670 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": 1.04,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7276374646295759,
+            "y": 6.716389445106627,
+            "heading": 1.0769704869840742,
+            "angularVelocity": 0.7463572831464532,
+            "velocityX": 0.35606374858489526,
+            "velocityY": -0.07288959818107699,
+            "timestamp": 0.0495345698619811
+        },
+        {
+            "x": 0.7632753901023804,
+            "y": 6.70941368019147,
+            "heading": 1.1486786527951847,
+            "angularVelocity": 1.4476388108510687,
+            "velocityX": 0.7194556361777777,
+            "velocityY": -0.1408261933931805,
+            "timestamp": 0.0990691397239622
+        },
+        {
+            "x": 0.8173493088554353,
+            "y": 6.699320044331327,
+            "heading": 1.251948933720564,
+            "angularVelocity": 2.084812308111838,
+            "velocityX": 1.0916400183503512,
+            "velocityY": -0.20376952677844481,
+            "timestamp": 0.1486037095859433
+        },
+        {
+            "x": 0.8904201618241163,
+            "y": 6.686277798850656,
+            "heading": 1.3814946265878076,
+            "angularVelocity": 2.615258257579252,
+            "velocityX": 1.4751486320027993,
+            "velocityY": -0.263295825859426,
+            "timestamp": 0.1981382794479244
+        },
+        {
+            "x": 0.9831303708035419,
+            "y": 6.670301294322549,
+            "heading": 1.5290667264747733,
+            "angularVelocity": 2.9791739445430228,
+            "velocityX": 1.871626406322422,
+            "velocityY": -0.32253241669944677,
+            "timestamp": 0.2476728493099055
+        },
+        {
+            "x": 1.0960137924135722,
+            "y": 6.651369432356597,
+            "heading": 1.6827078385240544,
+            "angularVelocity": 3.1016946846903584,
+            "velocityX": 2.278881636090858,
+            "velocityY": -0.3821949401851956,
+            "timestamp": 0.2972074191718866
+        },
+        {
+            "x": 1.2286250246243187,
+            "y": 6.629796552300239,
+            "heading": 1.8192744268134131,
+            "angularVelocity": 2.7569955421025307,
+            "velocityX": 2.677145124715176,
+            "velocityY": -0.4355116056504451,
+            "timestamp": 0.34674198903386766
+        },
+        {
+            "x": 1.37806258880499,
+            "y": 6.607677950088394,
+            "heading": 1.9114236020956594,
+            "angularVelocity": 1.8603003021733433,
+            "velocityX": 3.0168337909701455,
+            "velocityY": -0.446528601597435,
+            "timestamp": 0.3962765588958488
+        },
+        {
+            "x": 1.5443963470492121,
+            "y": 6.584716640433465,
+            "heading": 1.9605466016146158,
+            "angularVelocity": 0.991691250288047,
+            "velocityX": 3.3579328276762688,
+            "velocityY": -0.4635411131687398,
+            "timestamp": 0.4458111287578299
+        },
+        {
+            "x": 1.7276120279383835,
+            "y": 6.5605525122280195,
+            "heading": 1.9670840049159166,
+            "angularVelocity": 0.13197658361397824,
+            "velocityX": 3.698743754106468,
+            "velocityY": -0.4878235194494175,
+            "timestamp": 0.49534569861981104
+        },
+        {
+            "x": 1.9125583307039895,
+            "y": 6.530411390267246,
+            "heading": 1.9670841172507754,
+            "angularVelocity": 0.0000022678072915558937,
+            "velocityX": 3.7336814124171944,
+            "velocityY": -0.6084865992338514,
+            "timestamp": 0.5448802684817922
+        },
+        {
+            "x": 2.0974975773537823,
+            "y": 6.500227003772903,
+            "heading": 1.967084229554738,
+            "angularVelocity": 0.0000022671835628273576,
+            "velocityX": 3.733538964104325,
+            "velocityY": -0.6093600202458971,
+            "timestamp": 0.5944148383437733
+        },
+        {
+            "x": 2.2824570922079714,
+            "y": 6.470167063452025,
+            "heading": 1.9670843418904687,
+            "angularVelocity": 0.0000022678248970866596,
+            "velocityX": 3.7339481370144885,
+            "velocityY": -0.6068477107210625,
+            "timestamp": 0.6439494082057544
+        },
+        {
+            "x": 2.4663667602317028,
+            "y": 6.434238576599238,
+            "heading": 1.9670844533817493,
+            "angularVelocity": 0.000002250777202166417,
+            "velocityX": 3.712753911780929,
+            "velocityY": -0.7253214664890316,
+            "timestamp": 0.6934839780677355
+        },
+        {
+            "x": 2.6453124624138216,
+            "y": 6.3786323067408315,
+            "heading": 1.96708456679077,
+            "angularVelocity": 0.000002289492388953058,
+            "velocityX": 3.6125417598312857,
+            "velocityY": -1.1225750019579073,
+            "timestamp": 0.7430185479297167
+        },
+        {
+            "x": 2.817133903503418,
+            "y": 6.303859233856201,
+            "heading": 1.96708468933932,
+            "angularVelocity": 0.0000024740004860391374,
+            "velocityX": 3.468717737283832,
+            "velocityY": -1.5095129137971557,
+            "timestamp": 0.7925531177916978
+        },
+        {
+            "x": 2.91295588800905,
+            "y": 6.254800700059255,
+            "heading": 1.967084804070116,
+            "angularVelocity": 0.000004031753460568297,
+            "velocityX": 3.367279151013272,
+            "velocityY": -1.7239653184522483,
+            "timestamp": 0.8210099163546827
+        },
+        {
+            "x": 3.0055158966135553,
+            "y": 6.199834910977839,
+            "heading": 1.9670849118890987,
+            "angularVelocity": 0.000003788865518014884,
+            "velocityX": 3.2526500969440626,
+            "velocityY": -1.9315521020251178,
+            "timestamp": 0.8494667149176676
+        },
+        {
+            "x": 3.0944547185216633,
+            "y": 6.139184440666953,
+            "heading": 1.96708501618758,
+            "angularVelocity": 0.0000036651516103387324,
+            "velocityX": 3.125398020841055,
+            "velocityY": -2.13131741353383,
+            "timestamp": 0.8779235134806525
+        },
+        {
+            "x": 3.179446617500541,
+            "y": 6.0731165142841,
+            "heading": 1.9670851198382022,
+            "angularVelocity": 0.0000036423852113204396,
+            "velocityX": 2.986699251881383,
+            "velocityY": -2.3216921691473744,
+            "timestamp": 0.9063803120436373
+        },
+        {
+            "x": 3.2604149841370136,
+            "y": 6.002174866532259,
+            "heading": 1.9670852253780247,
+            "angularVelocity": 0.0000037087735735672327,
+            "velocityX": 2.845308352497901,
+            "velocityY": -2.4929595504099007,
+            "timestamp": 0.9348371106066222
+        },
+        {
+            "x": 3.3400950350688365,
+            "y": 5.929789210280793,
+            "heading": 1.9670853319364994,
+            "angularVelocity": 0.000003744570018753785,
+            "velocityX": 2.8000356665421786,
+            "velocityY": -2.5437034349136662,
+            "timestamp": 0.9632939091696071
+        },
+        {
+            "x": 3.4197035202865202,
+            "y": 5.857324854756433,
+            "heading": 1.9670854385695549,
+            "angularVelocity": 0.0000037471908580905978,
+            "velocityX": 2.7975207766800336,
+            "velocityY": -2.5464690050744796,
+            "timestamp": 0.991750707732592
+        },
+        {
+            "x": 3.4993494510128746,
+            "y": 5.7849016577830055,
+            "heading": 1.9670855451672207,
+            "angularVelocity": 0.0000037459472354931967,
+            "velocityX": 2.7988366488248966,
+            "velocityY": -2.5450226529586133,
+            "timestamp": 1.0202075062955769
+        },
+        {
+            "x": 3.579011291529686,
+            "y": 5.712495961052324,
+            "heading": 1.96708565174968,
+            "angularVelocity": 0.0000037454128599628807,
+            "velocityX": 2.7993957345647376,
+            "velocityY": -2.544407677155567,
+            "timestamp": 1.0486643048585618
+        },
+        {
+            "x": 3.658639843469943,
+            "y": 5.640053656617691,
+            "heading": 1.9670857583635384,
+            "angularVelocity": 0.000003746516260337957,
+            "velocityX": 2.7982259411140946,
+            "velocityY": -2.545694108020349,
+            "timestamp": 1.0771211034215467
+        },
+        {
+            "x": 3.7382184197836335,
+            "y": 5.567556457074997,
+            "heading": 1.9670858650249143,
+            "angularVelocity": 0.0000037481860642492875,
+            "velocityX": 2.796469748251449,
+            "velocityY": -2.547623176320097,
+            "timestamp": 1.1055779019845315
+        },
+        {
+            "x": 3.81773211309123,
+            "y": 5.494988100908759,
+            "heading": 1.967085971748553,
+            "angularVelocity": 0.0000037503740493629023,
+            "velocityX": 2.794189695354055,
+            "velocityY": -2.5501236903290603,
+            "timestamp": 1.1340347005475164
+        },
+        {
+            "x": 3.8971703522513814,
+            "y": 5.422337155346324,
+            "heading": 1.967086078550155,
+            "angularVelocity": 0.000003753113746455045,
+            "velocityX": 2.7915381621131705,
+            "velocityY": -2.5530259632560406,
+            "timestamp": 1.1624914991105013
+        },
+        {
+            "x": 3.9762575655393007,
+            "y": 5.349304242205494,
+            "heading": 1.9670861858107123,
+            "angularVelocity": 0.0000037692418878753097,
+            "velocityX": 2.779202766357265,
+            "velocityY": -2.566448681118608,
+            "timestamp": 1.1909482976734862
+        },
+        {
+            "x": 4.0513085153658634,
+            "y": 5.27212945123473,
+            "heading": 1.9670862984844089,
+            "angularVelocity": 0.000003959464953334102,
+            "velocityX": 2.637364482882678,
+            "velocityY": -2.7119983577895854,
+            "timestamp": 1.219405096236471
+        },
+        {
+            "x": 4.121436785634375,
+            "y": 5.190455596279305,
+            "heading": 1.9670864209307273,
+            "angularVelocity": 0.000004302884530781832,
+            "velocityX": 2.4643766625150745,
+            "velocityY": -2.8700999086264516,
+            "timestamp": 1.247861894799456
+        },
+        {
+            "x": 4.18632086285232,
+            "y": 5.104556536444466,
+            "heading": 1.9670865586915784,
+            "angularVelocity": 0.0000048410523378145445,
+            "velocityX": 2.280090540555213,
+            "velocityY": -3.0185777800939864,
+            "timestamp": 1.2763186933624409
+        },
+        {
+            "x": 4.245696839289754,
+            "y": 5.014761859934199,
+            "heading": 1.9670867202387443,
+            "angularVelocity": 0.000005676926915437751,
+            "velocityX": 2.086530440380401,
+            "velocityY": -3.1554735966350926,
+            "timestamp": 1.3047754919254257
+        },
+        {
+            "x": 4.299328195356381,
+            "y": 4.921422294262094,
+            "heading": 1.967086761089818,
+            "angularVelocity": 0.0000014355470678059053,
+            "velocityX": 1.8846588082611082,
+            "velocityY": -3.2800445020371574,
+            "timestamp": 1.3332322904884106
+        },
+        {
+            "x": 4.347003427577455,
+            "y": 4.824904615507251,
+            "heading": 1.9670865846023797,
+            "angularVelocity": -0.000006201942843803764,
+            "velocityX": 1.6753547351988247,
+            "velocityY": -3.391726533861692,
+            "timestamp": 1.3616890890513955
+        },
+        {
+            "x": 4.391601867669633,
+            "y": 4.730655732437251,
+            "heading": 1.956126696369797,
+            "angularVelocity": -0.3851412943835458,
+            "velocityX": 1.5672332217368503,
+            "velocityY": -3.3119988132677265,
+            "timestamp": 1.3901458876143804
+        },
+        {
+            "x": 4.4407405541886815,
+            "y": 4.641134625282056,
+            "heading": 1.9393492777589223,
+            "angularVelocity": -0.5895750561568437,
+            "velocityX": 1.7267819642570823,
+            "velocityY": -3.145860099373803,
+            "timestamp": 1.4186026861773653
+        },
+        {
+            "x": 4.494270192698628,
+            "y": 4.5565595160525465,
+            "heading": 1.9177097235934124,
+            "angularVelocity": -0.7604353004627978,
+            "velocityX": 1.8810843528824521,
+            "velocityY": -2.97205284854274,
+            "timestamp": 1.4470594847403502
+        },
+        {
+            "x": 4.552047682392307,
+            "y": 4.477111099902014,
+            "heading": 1.8920595280024661,
+            "angularVelocity": -0.901373200296037,
+            "velocityX": 2.030358037844879,
+            "velocityY": -2.791895791605024,
+            "timestamp": 1.475516283303335
+        },
+        {
+            "x": 4.613941492209361,
+            "y": 4.402936898219151,
+            "heading": 1.8631628723317806,
+            "angularVelocity": -1.0154570130820861,
+            "velocityX": 2.175009591471355,
+            "velocityY": -2.6065546874046253,
+            "timestamp": 1.50397308186632
+        },
+        {
+            "x": 4.679832286793745,
+            "y": 4.334157239741354,
+            "heading": 1.831710656249818,
+            "angularVelocity": -1.105261929317901,
+            "velocityX": 2.315467582853696,
+            "velocityY": -2.416985112558446,
+            "timestamp": 1.5324298804293048
+        },
+        {
+            "x": 4.749611538460051,
+            "y": 4.270871230504256,
+            "heading": 1.798335587748108,
+            "angularVelocity": -1.1728328619898605,
+            "velocityX": 2.4521118042094026,
+            "velocityY": -2.223932853761988,
+            "timestamp": 1.5608866789922897
+        },
+        {
+            "x": 4.82317903379308,
+            "y": 4.213162042716923,
+            "heading": 1.7636285592897518,
+            "angularVelocity": -1.2196392500559503,
+            "velocityX": 2.585234427203579,
+            "velocityY": -2.0279578414139063,
+            "timestamp": 1.5893434775552746
+        },
+        {
+            "x": 4.900439739227295,
+            "y": 4.161101341247559,
+            "heading": 1.7281561592052472,
+            "angularVelocity": -1.2465351647339973,
+            "velocityX": 2.715017476867699,
+            "velocityY": -1.8294644548346262,
+            "timestamp": 1.6178002761182595
+        },
+        {
+            "x": 5.010449341197396,
+            "y": 4.1015961278120825,
+            "heading": 1.6804127870661807,
+            "angularVelocity": -1.251568549956003,
+            "velocityX": 2.8838465288429167,
+            "velocityY": -1.5598993191587458,
+            "timestamp": 1.6559471056633712
+        },
+        {
+            "x": 5.126648667318374,
+            "y": 4.05252161275018,
+            "heading": 1.6338488947780365,
+            "angularVelocity": -1.2206490773575045,
+            "velocityX": 3.0461070423551457,
+            "velocityY": -1.2864637939008783,
+            "timestamp": 1.694093935208483
+        },
+        {
+            "x": 5.248756740712563,
+            "y": 4.014017499979329,
+            "heading": 1.589989878848282,
+            "angularVelocity": -1.1497421005375177,
+            "velocityX": 3.2010018879727395,
+            "velocityY": -1.0093660005294856,
+            "timestamp": 1.7322407647535947
+        },
+        {
+            "x": 5.376454980725802,
+            "y": 3.9862068849459353,
+            "heading": 1.5506775545962859,
+            "angularVelocity": -1.0305528590747366,
+            "velocityX": 3.347545301559156,
+            "velocityY": -0.7290413217820071,
+            "timestamp": 1.7703875942987064
+        },
+        {
+            "x": 5.509414833155046,
+            "y": 3.969152206173251,
+            "heading": 1.5183946275971996,
+            "angularVelocity": -0.8462807364186931,
+            "velocityX": 3.485475831533742,
+            "velocityY": -0.4470798484702507,
+            "timestamp": 1.8085344238438181
+        },
+        {
+            "x": 5.647435915880888,
+            "y": 3.96261903739833,
+            "heading": 1.4980750155673637,
+            "angularVelocity": -0.5326684359468331,
+            "velocityX": 3.618153444771329,
+            "velocityY": -0.17126374204166636,
+            "timestamp": 1.8466812533889299
+        },
+        {
+            "x": 5.791726765964232,
+            "y": 3.964767379969892,
+            "heading": 1.498073903211314,
+            "angularVelocity": -0.00002915985583373997,
+            "velocityX": 3.7825122507942877,
+            "velocityY": 0.056317722783366074,
+            "timestamp": 1.8848280829340416
+        },
+        {
+            "x": 5.935369435750422,
+            "y": 3.9785994401105067,
+            "heading": 1.4980736722429098,
+            "angularVelocity": -0.00000605472084526459,
+            "velocityX": 3.765520529467262,
+            "velocityY": 0.3626005176676574,
+            "timestamp": 1.9229749124791533
+        },
+        {
+            "x": 6.078764518036592,
+            "y": 3.9947980029743344,
+            "heading": 1.498073449658831,
+            "angularVelocity": -0.000005834929965972279,
+            "velocityX": 3.7590301473577066,
+            "velocityY": 0.42463719939010114,
+            "timestamp": 1.961121742024265
+        },
+        {
+            "x": 6.222150100858587,
+            "y": 4.0110804389753145,
+            "heading": 1.4980732273393433,
+            "angularVelocity": -0.000005827993841710607,
+            "velocityX": 3.7587811236710817,
+            "velocityY": 0.4268358915939984,
+            "timestamp": 1.9992685715693768
+        },
+        {
+            "x": 6.3655338107868324,
+            "y": 4.0273793596330805,
+            "heading": 1.4980730050704238,
+            "angularVelocity": -0.000005826668223171757,
+            "velocityX": 3.758732026701189,
+            "velocityY": 0.42726802860026514,
+            "timestamp": 2.0374154011144885
+        },
+        {
+            "x": 6.508922861230399,
+            "y": 4.043631229943055,
+            "heading": 1.4980727826574463,
+            "angularVelocity": -0.000005830444635518525,
+            "velocityX": 3.7588720256305224,
+            "velocityY": 0.42603462734214087,
+            "timestamp": 2.0755622306596
+        },
+        {
+            "x": 6.6523224677448685,
+            "y": 4.059789689420356,
+            "heading": 1.4980725589565675,
+            "angularVelocity": -0.000005864206314008162,
+            "velocityX": 3.759148747731203,
+            "velocityY": 0.4235859092458616,
+            "timestamp": 2.1137090602047115
+        },
+        {
+            "x": 6.792869039761006,
+            "y": 4.068475137054533,
+            "heading": 1.4874839096219183,
+            "angularVelocity": -0.2775761304689174,
+            "velocityX": 3.684357879593819,
+            "velocityY": 0.2276846526483585,
+            "timestamp": 2.151855889749823
+        },
+        {
+            "x": 6.923724640852256,
+            "y": 4.075654653912489,
+            "heading": 1.4487910280540974,
+            "angularVelocity": -1.014314479844837,
+            "velocityX": 3.430313938317647,
+            "velocityY": 0.18820743280285396,
+            "timestamp": 2.1900027192949345
+        },
+        {
+            "x": 7.04453293582936,
+            "y": 4.081916427546979,
+            "heading": 1.3838287724616933,
+            "angularVelocity": -1.7029529417506197,
+            "velocityX": 3.1669288488112004,
+            "velocityY": 0.1641492545881335,
+            "timestamp": 2.228149548840046
+        },
+        {
+            "x": 7.155015675902827,
+            "y": 4.087769066352678,
+            "heading": 1.2944912667640571,
+            "angularVelocity": -2.3419378953075096,
+            "velocityX": 2.896249606872667,
+            "velocityY": 0.1534239902944478,
+            "timestamp": 2.2662963783851575
+        },
+        {
+            "x": 7.255027442080941,
+            "y": 4.093665982164644,
+            "heading": 1.1821266374518384,
+            "angularVelocity": -2.945582389205927,
+            "velocityX": 2.621758279016409,
+            "velocityY": 0.15458468980091475,
+            "timestamp": 2.304443207930269
+        },
+        {
+            "x": 7.344594486390613,
+            "y": 4.100041344791564,
+            "heading": 1.0471923484078691,
+            "angularVelocity": -3.5372346969048754,
+            "velocityX": 2.3479551348760803,
+            "velocityY": 0.16712693303897977,
+            "timestamp": 2.3425900374753805
+        },
+        {
+            "x": 7.423972884493799,
+            "y": 4.107363761305008,
+            "heading": 0.8891062073498277,
+            "angularVelocity": -4.144148883225662,
+            "velocityX": 2.080864885752389,
+            "velocityY": 0.19195347557968287,
+            "timestamp": 2.380736867020492
+        },
+        {
+            "x": 7.492968640563269,
+            "y": 4.113132211800853,
+            "heading": 0.7092469399580156,
+            "angularVelocity": -4.714920467480658,
+            "velocityX": 1.8086891333366921,
+            "velocityY": 0.1512170359820358,
+            "timestamp": 2.4188836965656035
+        },
+        {
+            "x": 7.549873361136887,
+            "y": 4.117954510008317,
+            "heading": 0.5306887118642236,
+            "angularVelocity": -4.680814374954733,
+            "velocityX": 1.4917287033341673,
+            "velocityY": 0.12641412838299443,
+            "timestamp": 2.457030526110715
+        },
+        {
+            "x": 7.595282353021441,
+            "y": 4.121668824298383,
+            "heading": 0.3658074925223697,
+            "angularVelocity": -4.322278451654422,
+            "velocityX": 1.1903739426332636,
+            "velocityY": 0.09736888581300335,
+            "timestamp": 2.4951773556558265
+        },
+        {
+            "x": 7.630209510034256,
+            "y": 4.124185175295285,
+            "heading": 0.22438733020455492,
+            "angularVelocity": -3.7072586111122043,
+            "velocityX": 0.9155978997292245,
+            "velocityY": 0.06596487904592362,
+            "timestamp": 2.533324185200938
+        },
+        {
+            "x": 7.65555835714788,
+            "y": 4.125725160446138,
+            "heading": 0.1131980096846438,
+            "angularVelocity": -2.91477225881568,
+            "velocityX": 0.6645073107231708,
+            "velocityY": 0.04036993818976583,
+            "timestamp": 2.5714710147460496
+        },
+        {
+            "x": 7.671987700512514,
+            "y": 4.126592780034806,
+            "heading": 0.03644789338444829,
+            "angularVelocity": -2.011965796774904,
+            "velocityX": 0.43068699445162756,
+            "velocityY": 0.02274421227203338,
+            "timestamp": 2.609617844291161
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -0.003,
+            "angularVelocity": -1.0341067358634992,
+            "velocityX": 0.21003841165919293,
+            "velocityY": 0.010675067103991773,
+            "timestamp": 2.6477646738362726
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -0.003,
+            "angularVelocity": 1.475285929099793e-30,
+            "velocityX": 9.681452358723228e-32,
+            "velocityY": 1.6696229962948817e-32,
+            "timestamp": 2.685911503381384
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.traj
index 463b42ef..78068220 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.traj	
@@ -6,521 +6,665 @@
             "heading": 1.04,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": 1.3928609100313025e-33,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.729279758485921,
-            "y": 6.711628327586588,
-            "heading": 1.093428142295198,
-            "angularVelocity": 0.9770378814200095,
-            "velocityX": 0.352568020824986,
-            "velocityY": -0.15309237280887605,
-            "timestamp": 0.05468379815278649
-        },
-        {
-            "x": 0.7686493967347447,
-            "y": 6.69501140584797,
-            "heading": 1.1949690599508662,
-            "angularVelocity": 1.8568739020644303,
-            "velocityX": 0.7199506906748694,
-            "velocityY": -0.30387285265355396,
-            "timestamp": 0.10936759630557298
-        },
-        {
-            "x": 0.8290790315170126,
-            "y": 6.670059182269693,
-            "heading": 1.3362954219692313,
-            "angularVelocity": 2.584428419245852,
-            "velocityX": 1.1050738394839994,
-            "velocityY": -0.4563001185206737,
-            "timestamp": 0.16405139445835948
-        },
-        {
-            "x": 0.9115429372168562,
-            "y": 6.636285478537955,
-            "heading": 1.5055888656223098,
-            "angularVelocity": 3.0958611027725738,
-            "velocityX": 1.508013497333151,
-            "velocityY": -0.6176181039468854,
-            "timestamp": 0.21873519261114596
-        },
-        {
-            "x": 1.0168329273736771,
-            "y": 6.593191021214006,
-            "heading": 1.6881423696003794,
-            "angularVelocity": 3.3383471913932383,
-            "velocityX": 1.9254330114861617,
-            "velocityY": -0.7880662788554578,
-            "timestamp": 0.27341899076393245
-        },
-        {
-            "x": 1.1449422871140338,
-            "y": 6.540878714080721,
-            "heading": 1.8564615478628756,
-            "angularVelocity": 3.078044758197163,
-            "velocityX": 2.3427297310698694,
-            "velocityY": -0.9566326572109004,
-            "timestamp": 0.32810278891671896
-        },
-        {
-            "x": 1.2912409780613738,
-            "y": 6.479735868066328,
-            "heading": 1.974688906048384,
-            "angularVelocity": 2.1620180415263293,
-            "velocityX": 2.675357160425865,
-            "velocityY": -1.1181162991560991,
-            "timestamp": 0.38278658706950547
-        },
-        {
-            "x": 1.4553681499836075,
-            "y": 6.408144093279595,
-            "heading": 2.0430197361851268,
-            "angularVelocity": 1.2495626208301445,
-            "velocityX": 3.001385738855639,
-            "velocityY": -1.3091953595963746,
-            "timestamp": 0.437470385222292
-        },
-        {
-            "x": 1.6374055328463037,
-            "y": 6.326368400043195,
-            "heading": 2.0610350683831213,
-            "angularVelocity": 0.3294455178051113,
-            "velocityX": 3.328908909254691,
-            "velocityY": -1.4954281889476506,
-            "timestamp": 0.4921541833750785
-        },
-        {
-            "x": 1.8283093127689483,
-            "y": 6.246687216839916,
-            "heading": 2.061035169590908,
-            "angularVelocity": 0.000001850781943801841,
-            "velocityX": 3.4910482880003957,
-            "velocityY": -1.45712598420193,
-            "timestamp": 0.546837981527865
-        },
-        {
-            "x": 2.01921313853121,
-            "y": 6.167006143460992,
-            "heading": 2.0610352707983473,
-            "angularVelocity": 0.0000018507755948234292,
-            "velocityX": 3.491049126267287,
-            "velocityY": -1.4571239758492094,
-            "timestamp": 0.6015217796806515
-        },
-        {
-            "x": 2.210116959480156,
-            "y": 6.087325058550032,
-            "heading": 2.061035372005735,
-            "angularVelocity": 0.000001850774661522582,
-            "velocityX": 3.4910490382464,
-            "velocityY": -1.4571241867349893,
-            "timestamp": 0.6562055778334379
-        },
-        {
-            "x": 2.40102077504018,
-            "y": 6.007643960727976,
-            "heading": 2.0610354732130722,
-            "angularVelocity": 0.0000018507737318277825,
-            "velocityX": 3.49104893969945,
-            "velocityY": -1.4571244228395928,
-            "timestamp": 0.7108893759862244
-        },
-        {
-            "x": 2.5919245845305627,
-            "y": 5.927962848363926,
-            "heading": 2.0610355744203592,
-            "angularVelocity": 0.0000018507728139380304,
-            "velocityX": 3.491048828704212,
-            "velocityY": -1.457124688768356,
-            "timestamp": 0.7655731741390108
-        },
-        {
-            "x": 2.78282838714305,
-            "y": 5.848281719521431,
-            "heading": 2.0610356756275974,
-            "angularVelocity": 0.0000018507719179726602,
-            "velocityX": 3.4910487029284663,
-            "velocityY": -1.457124990108895,
-            "timestamp": 0.8202569722917973
-        },
-        {
-            "x": 2.973732181912291,
-            "y": 5.768600571887663,
-            "heading": 2.061035776834788,
-            "angularVelocity": 0.0000018507710613284672,
-            "velocityX": 3.491048559499401,
-            "velocityY": -1.4571253337439838,
-            "timestamp": 0.8749407704445837
-        },
-        {
-            "x": 3.16463596766188,
-            "y": 5.688919402644143,
-            "heading": 2.061035878041936,
-            "angularVelocity": 0.000001850770272196717,
-            "velocityX": 3.4910483945574473,
-            "velocityY": -1.4571257289205004,
-            "timestamp": 0.9296245685973702
-        },
-        {
-            "x": 3.3555397425941185,
-            "y": 5.609238207483871,
-            "heading": 2.061035979249042,
-            "angularVelocity": 0.000001850769506382635,
-            "velocityX": 3.4910481967410822,
-            "velocityY": -1.457126202858873,
-            "timestamp": 0.9843083667501566
-        },
-        {
-            "x": 3.546443239175115,
-            "y": 5.529556345440524,
-            "heading": 2.0610360804562036,
-            "angularVelocity": 0.0000018507705174022697,
-            "velocityX": 3.491043106545231,
-            "velocityY": -1.4571383981177848,
-            "timestamp": 1.0389921649029432
-        },
-        {
-            "x": 3.7301891812097523,
-            "y": 5.434525969364101,
-            "heading": 2.0610361860102304,
-            "angularVelocity": 0.0000019302614304565207,
-            "velocityX": 3.360153249071168,
-            "velocityY": -1.7378159397580357,
-            "timestamp": 1.0936759630557298
-        },
-        {
-            "x": 3.9011674048607876,
-            "y": 5.318079465137689,
-            "heading": 2.0610363082632275,
-            "angularVelocity": 0.0000022356347107367405,
-            "velocityX": 3.126670593972313,
-            "velocityY": -2.129451650396711,
-            "timestamp": 1.1483597612085163
-        },
-        {
-            "x": 4.05689386248419,
-            "y": 5.181908226701039,
-            "heading": 2.0610364709885136,
-            "angularVelocity": 0.0000029757495174652245,
-            "velocityX": 2.8477622784778513,
-            "velocityY": -2.490156920998575,
-            "timestamp": 1.203043559361303
-        },
-        {
-            "x": 4.195106185477333,
-            "y": 5.027990436285506,
-            "heading": 2.061036659879246,
-            "angularVelocity": 0.0000034542357869786317,
-            "velocityX": 2.52748213660978,
-            "velocityY": -2.814687267798894,
-            "timestamp": 1.2577273575140895
-        },
-        {
-            "x": 4.313711649853664,
-            "y": 4.858669951905228,
-            "heading": 2.0606108116890636,
-            "angularVelocity": -0.007787465475470693,
-            "velocityX": 2.168932451344132,
-            "velocityY": -3.096355595256869,
-            "timestamp": 1.312411155666876
-        },
-        {
-            "x": 4.40996698448611,
-            "y": 4.67856050613378,
-            "heading": 2.051588761211157,
-            "angularVelocity": -0.16498580535132465,
-            "velocityX": 1.7602166982532683,
-            "velocityY": -3.293652815925874,
-            "timestamp": 1.3670949538196626
-        },
-        {
-            "x": 4.510742037442817,
-            "y": 4.51918682060045,
-            "heading": 1.996778499947209,
-            "angularVelocity": -1.002312624862085,
-            "velocityX": 1.8428685709639585,
-            "velocityY": -2.9144589607334965,
-            "timestamp": 1.4217787519724492
-        },
-        {
-            "x": 4.62612450472694,
-            "y": 4.379617585881877,
-            "heading": 1.9276644376664414,
-            "angularVelocity": -1.263885549567404,
-            "velocityX": 2.1099936577511538,
-            "velocityY": -2.5522959163995464,
-            "timestamp": 1.4764625501252358
-        },
-        {
-            "x": 4.756154634809633,
-            "y": 4.26013511834241,
-            "heading": 1.8517335763640235,
-            "angularVelocity": -1.3885440270675238,
-            "velocityX": 2.3778547664042544,
-            "velocityY": -2.1849701662205963,
-            "timestamp": 1.5311463482780223
+            "x": 0.7276374646295759,
+            "y": 6.716389445106627,
+            "heading": 1.0769704869840742,
+            "angularVelocity": 0.7463572831464532,
+            "velocityX": 0.35606374858489526,
+            "velocityY": -0.07288959818107699,
+            "timestamp": 0.0495345698619811
+        },
+        {
+            "x": 0.7632753901023804,
+            "y": 6.70941368019147,
+            "heading": 1.1486786527951847,
+            "angularVelocity": 1.4476388108510687,
+            "velocityX": 0.7194556361777777,
+            "velocityY": -0.1408261933931805,
+            "timestamp": 0.0990691397239622
+        },
+        {
+            "x": 0.8173493088554353,
+            "y": 6.699320044331327,
+            "heading": 1.251948933720564,
+            "angularVelocity": 2.084812308111838,
+            "velocityX": 1.0916400183503512,
+            "velocityY": -0.20376952677844481,
+            "timestamp": 0.1486037095859433
+        },
+        {
+            "x": 0.8904201618241163,
+            "y": 6.686277798850656,
+            "heading": 1.3814946265878076,
+            "angularVelocity": 2.615258257579252,
+            "velocityX": 1.4751486320027993,
+            "velocityY": -0.263295825859426,
+            "timestamp": 0.1981382794479244
+        },
+        {
+            "x": 0.9831303708035419,
+            "y": 6.670301294322549,
+            "heading": 1.5290667264747733,
+            "angularVelocity": 2.9791739445430228,
+            "velocityX": 1.871626406322422,
+            "velocityY": -0.32253241669944677,
+            "timestamp": 0.2476728493099055
+        },
+        {
+            "x": 1.0960137924135722,
+            "y": 6.651369432356597,
+            "heading": 1.6827078385240544,
+            "angularVelocity": 3.1016946846903584,
+            "velocityX": 2.278881636090858,
+            "velocityY": -0.3821949401851956,
+            "timestamp": 0.2972074191718866
+        },
+        {
+            "x": 1.2286250246243187,
+            "y": 6.629796552300239,
+            "heading": 1.8192744268134131,
+            "angularVelocity": 2.7569955421025307,
+            "velocityX": 2.677145124715176,
+            "velocityY": -0.4355116056504451,
+            "timestamp": 0.34674198903386766
+        },
+        {
+            "x": 1.37806258880499,
+            "y": 6.607677950088394,
+            "heading": 1.9114236020956594,
+            "angularVelocity": 1.8603003021733433,
+            "velocityX": 3.0168337909701455,
+            "velocityY": -0.446528601597435,
+            "timestamp": 0.3962765588958488
+        },
+        {
+            "x": 1.5443963470492121,
+            "y": 6.584716640433465,
+            "heading": 1.9605466016146158,
+            "angularVelocity": 0.991691250288047,
+            "velocityX": 3.3579328276762688,
+            "velocityY": -0.4635411131687398,
+            "timestamp": 0.4458111287578299
+        },
+        {
+            "x": 1.7276120279383835,
+            "y": 6.5605525122280195,
+            "heading": 1.9670840049159166,
+            "angularVelocity": 0.13197658361397824,
+            "velocityX": 3.698743754106468,
+            "velocityY": -0.4878235194494175,
+            "timestamp": 0.49534569861981104
+        },
+        {
+            "x": 1.9125583307039895,
+            "y": 6.530411390267246,
+            "heading": 1.9670841172507754,
+            "angularVelocity": 0.0000022678072915558937,
+            "velocityX": 3.7336814124171944,
+            "velocityY": -0.6084865992338514,
+            "timestamp": 0.5448802684817922
+        },
+        {
+            "x": 2.0974975773537823,
+            "y": 6.500227003772903,
+            "heading": 1.967084229554738,
+            "angularVelocity": 0.0000022671835628273576,
+            "velocityX": 3.733538964104325,
+            "velocityY": -0.6093600202458971,
+            "timestamp": 0.5944148383437733
+        },
+        {
+            "x": 2.2824570922079714,
+            "y": 6.470167063452025,
+            "heading": 1.9670843418904687,
+            "angularVelocity": 0.0000022678248970866596,
+            "velocityX": 3.7339481370144885,
+            "velocityY": -0.6068477107210625,
+            "timestamp": 0.6439494082057544
+        },
+        {
+            "x": 2.4663667602317028,
+            "y": 6.434238576599238,
+            "heading": 1.9670844533817493,
+            "angularVelocity": 0.000002250777202166417,
+            "velocityX": 3.712753911780929,
+            "velocityY": -0.7253214664890316,
+            "timestamp": 0.6934839780677355
+        },
+        {
+            "x": 2.6453124624138216,
+            "y": 6.3786323067408315,
+            "heading": 1.96708456679077,
+            "angularVelocity": 0.000002289492388953058,
+            "velocityX": 3.6125417598312857,
+            "velocityY": -1.1225750019579073,
+            "timestamp": 0.7430185479297167
+        },
+        {
+            "x": 2.817133903503418,
+            "y": 6.303859233856201,
+            "heading": 1.96708468933932,
+            "angularVelocity": 0.0000024740004860391374,
+            "velocityX": 3.468717737283832,
+            "velocityY": -1.5095129137971557,
+            "timestamp": 0.7925531177916978
+        },
+        {
+            "x": 2.91295588800905,
+            "y": 6.254800700059255,
+            "heading": 1.967084804070116,
+            "angularVelocity": 0.000004031753460568297,
+            "velocityX": 3.367279151013272,
+            "velocityY": -1.7239653184522483,
+            "timestamp": 0.8210099163546827
+        },
+        {
+            "x": 3.0055158966135553,
+            "y": 6.199834910977839,
+            "heading": 1.9670849118890987,
+            "angularVelocity": 0.000003788865518014884,
+            "velocityX": 3.2526500969440626,
+            "velocityY": -1.9315521020251178,
+            "timestamp": 0.8494667149176676
+        },
+        {
+            "x": 3.0944547185216633,
+            "y": 6.139184440666953,
+            "heading": 1.96708501618758,
+            "angularVelocity": 0.0000036651516103387324,
+            "velocityX": 3.125398020841055,
+            "velocityY": -2.13131741353383,
+            "timestamp": 0.8779235134806525
+        },
+        {
+            "x": 3.179446617500541,
+            "y": 6.0731165142841,
+            "heading": 1.9670851198382022,
+            "angularVelocity": 0.0000036423852113204396,
+            "velocityX": 2.986699251881383,
+            "velocityY": -2.3216921691473744,
+            "timestamp": 0.9063803120436373
+        },
+        {
+            "x": 3.2604149841370136,
+            "y": 6.002174866532259,
+            "heading": 1.9670852253780247,
+            "angularVelocity": 0.0000037087735735672327,
+            "velocityX": 2.845308352497901,
+            "velocityY": -2.4929595504099007,
+            "timestamp": 0.9348371106066222
+        },
+        {
+            "x": 3.3400950350688365,
+            "y": 5.929789210280793,
+            "heading": 1.9670853319364994,
+            "angularVelocity": 0.000003744570018753785,
+            "velocityX": 2.8000356665421786,
+            "velocityY": -2.5437034349136662,
+            "timestamp": 0.9632939091696071
+        },
+        {
+            "x": 3.4197035202865202,
+            "y": 5.857324854756433,
+            "heading": 1.9670854385695549,
+            "angularVelocity": 0.0000037471908580905978,
+            "velocityX": 2.7975207766800336,
+            "velocityY": -2.5464690050744796,
+            "timestamp": 0.991750707732592
+        },
+        {
+            "x": 3.4993494510128746,
+            "y": 5.7849016577830055,
+            "heading": 1.9670855451672207,
+            "angularVelocity": 0.0000037459472354931967,
+            "velocityX": 2.7988366488248966,
+            "velocityY": -2.5450226529586133,
+            "timestamp": 1.0202075062955769
+        },
+        {
+            "x": 3.579011291529686,
+            "y": 5.712495961052324,
+            "heading": 1.96708565174968,
+            "angularVelocity": 0.0000037454128599628807,
+            "velocityX": 2.7993957345647376,
+            "velocityY": -2.544407677155567,
+            "timestamp": 1.0486643048585618
+        },
+        {
+            "x": 3.658639843469943,
+            "y": 5.640053656617691,
+            "heading": 1.9670857583635384,
+            "angularVelocity": 0.000003746516260337957,
+            "velocityX": 2.7982259411140946,
+            "velocityY": -2.545694108020349,
+            "timestamp": 1.0771211034215467
+        },
+        {
+            "x": 3.7382184197836335,
+            "y": 5.567556457074997,
+            "heading": 1.9670858650249143,
+            "angularVelocity": 0.0000037481860642492875,
+            "velocityX": 2.796469748251449,
+            "velocityY": -2.547623176320097,
+            "timestamp": 1.1055779019845315
+        },
+        {
+            "x": 3.81773211309123,
+            "y": 5.494988100908759,
+            "heading": 1.967085971748553,
+            "angularVelocity": 0.0000037503740493629023,
+            "velocityX": 2.794189695354055,
+            "velocityY": -2.5501236903290603,
+            "timestamp": 1.1340347005475164
+        },
+        {
+            "x": 3.8971703522513814,
+            "y": 5.422337155346324,
+            "heading": 1.967086078550155,
+            "angularVelocity": 0.000003753113746455045,
+            "velocityX": 2.7915381621131705,
+            "velocityY": -2.5530259632560406,
+            "timestamp": 1.1624914991105013
+        },
+        {
+            "x": 3.9762575655393007,
+            "y": 5.349304242205494,
+            "heading": 1.9670861858107123,
+            "angularVelocity": 0.0000037692418878753097,
+            "velocityX": 2.779202766357265,
+            "velocityY": -2.566448681118608,
+            "timestamp": 1.1909482976734862
+        },
+        {
+            "x": 4.0513085153658634,
+            "y": 5.27212945123473,
+            "heading": 1.9670862984844089,
+            "angularVelocity": 0.000003959464953334102,
+            "velocityX": 2.637364482882678,
+            "velocityY": -2.7119983577895854,
+            "timestamp": 1.219405096236471
+        },
+        {
+            "x": 4.121436785634375,
+            "y": 5.190455596279305,
+            "heading": 1.9670864209307273,
+            "angularVelocity": 0.000004302884530781832,
+            "velocityX": 2.4643766625150745,
+            "velocityY": -2.8700999086264516,
+            "timestamp": 1.247861894799456
+        },
+        {
+            "x": 4.18632086285232,
+            "y": 5.104556536444466,
+            "heading": 1.9670865586915784,
+            "angularVelocity": 0.0000048410523378145445,
+            "velocityX": 2.280090540555213,
+            "velocityY": -3.0185777800939864,
+            "timestamp": 1.2763186933624409
+        },
+        {
+            "x": 4.245696839289754,
+            "y": 5.014761859934199,
+            "heading": 1.9670867202387443,
+            "angularVelocity": 0.000005676926915437751,
+            "velocityX": 2.086530440380401,
+            "velocityY": -3.1554735966350926,
+            "timestamp": 1.3047754919254257
+        },
+        {
+            "x": 4.299328195356381,
+            "y": 4.921422294262094,
+            "heading": 1.967086761089818,
+            "angularVelocity": 0.0000014355470678059053,
+            "velocityX": 1.8846588082611082,
+            "velocityY": -3.2800445020371574,
+            "timestamp": 1.3332322904884106
+        },
+        {
+            "x": 4.347003427577455,
+            "y": 4.824904615507251,
+            "heading": 1.9670865846023797,
+            "angularVelocity": -0.000006201942843803764,
+            "velocityX": 1.6753547351988247,
+            "velocityY": -3.391726533861692,
+            "timestamp": 1.3616890890513955
+        },
+        {
+            "x": 4.391601867669633,
+            "y": 4.730655732437251,
+            "heading": 1.956126696369797,
+            "angularVelocity": -0.3851412943835458,
+            "velocityX": 1.5672332217368503,
+            "velocityY": -3.3119988132677265,
+            "timestamp": 1.3901458876143804
+        },
+        {
+            "x": 4.4407405541886815,
+            "y": 4.641134625282056,
+            "heading": 1.9393492777589223,
+            "angularVelocity": -0.5895750561568437,
+            "velocityX": 1.7267819642570823,
+            "velocityY": -3.145860099373803,
+            "timestamp": 1.4186026861773653
+        },
+        {
+            "x": 4.494270192698628,
+            "y": 4.5565595160525465,
+            "heading": 1.9177097235934124,
+            "angularVelocity": -0.7604353004627978,
+            "velocityX": 1.8810843528824521,
+            "velocityY": -2.97205284854274,
+            "timestamp": 1.4470594847403502
+        },
+        {
+            "x": 4.552047682392307,
+            "y": 4.477111099902014,
+            "heading": 1.8920595280024661,
+            "angularVelocity": -0.901373200296037,
+            "velocityX": 2.030358037844879,
+            "velocityY": -2.791895791605024,
+            "timestamp": 1.475516283303335
+        },
+        {
+            "x": 4.613941492209361,
+            "y": 4.402936898219151,
+            "heading": 1.8631628723317806,
+            "angularVelocity": -1.0154570130820861,
+            "velocityX": 2.175009591471355,
+            "velocityY": -2.6065546874046253,
+            "timestamp": 1.50397308186632
+        },
+        {
+            "x": 4.679832286793745,
+            "y": 4.334157239741354,
+            "heading": 1.831710656249818,
+            "angularVelocity": -1.105261929317901,
+            "velocityX": 2.315467582853696,
+            "velocityY": -2.416985112558446,
+            "timestamp": 1.5324298804293048
+        },
+        {
+            "x": 4.749611538460051,
+            "y": 4.270871230504256,
+            "heading": 1.798335587748108,
+            "angularVelocity": -1.1728328619898605,
+            "velocityX": 2.4521118042094026,
+            "velocityY": -2.223932853761988,
+            "timestamp": 1.5608866789922897
+        },
+        {
+            "x": 4.82317903379308,
+            "y": 4.213162042716923,
+            "heading": 1.7636285592897518,
+            "angularVelocity": -1.2196392500559503,
+            "velocityX": 2.585234427203579,
+            "velocityY": -2.0279578414139063,
+            "timestamp": 1.5893434775552746
         },
         {
             "x": 4.900439739227295,
             "y": 4.161101341247559,
-            "heading": 1.7737408289368037,
-            "angularVelocity": -1.4262496399629772,
-            "velocityX": 2.638534799915143,
-            "velocityY": -1.8110259426046127,
-            "timestamp": 1.5858301464308089
-        },
-        {
-            "x": 5.008276114908647,
-            "y": 4.1019129343639005,
-            "heading": 1.719841693932476,
-            "angularVelocity": -1.4074809230288232,
-            "velocityX": 2.815956908545699,
-            "velocityY": -1.5456009367595303,
-            "timestamp": 1.6241248999315097
-        },
-        {
-            "x": 5.122676890234505,
-            "y": 4.053024888243358,
-            "heading": 1.6679900254477509,
-            "angularVelocity": -1.354014943163857,
-            "velocityX": 2.987374636678728,
-            "velocityY": -1.276625167978901,
-            "timestamp": 1.6624196534322104
-        },
-        {
-            "x": 5.243382451295884,
-            "y": 4.014566134854959,
-            "heading": 1.6197010617895011,
-            "angularVelocity": -1.260981185252083,
-            "velocityX": 3.1520130051018556,
-            "velocityY": -1.0042825680467018,
-            "timestamp": 1.7007144069329112
-        },
-        {
-            "x": 5.370099790784461,
-            "y": 3.9866453379407707,
-            "heading": 1.576858310766692,
-            "angularVelocity": -1.1187629402556432,
-            "velocityX": 3.3090000040412137,
-            "velocityY": -0.7291024059908604,
-            "timestamp": 1.739009160433612
-        },
-        {
-            "x": 5.502536097274536,
-            "y": 3.9692960416426737,
-            "heading": 1.5420636410872666,
-            "angularVelocity": -0.9086014792806747,
-            "velocityX": 3.4583407486263615,
-            "velocityY": -0.45304629778540073,
-            "timestamp": 1.7773039139343128
-        },
-        {
-            "x": 5.640613600815518,
-            "y": 3.9622331858505495,
-            "heading": 1.5196895147171037,
-            "angularVelocity": -0.5842608797508935,
-            "velocityX": 3.6056506680074274,
-            "velocityY": -0.18443403198808295,
-            "timestamp": 1.8155986674350135
-        },
-        {
-            "x": 5.785301546169388,
-            "y": 3.963377277975263,
-            "heading": 1.519119424890785,
-            "angularVelocity": -0.014886891132695255,
-            "velocityX": 3.778270706226711,
-            "velocityY": 0.029875949578638192,
-            "timestamp": 1.8538934209357143
-        },
-        {
-            "x": 5.929550760809323,
-            "y": 3.9767385598159253,
-            "heading": 1.5191191822149777,
-            "angularVelocity": -0.000006337051042848807,
-            "velocityX": 3.7668140268168893,
-            "velocityY": 0.3489063283934661,
-            "timestamp": 1.892188174436415
-        },
-        {
-            "x": 6.073462686179593,
-            "y": 3.9933433315789473,
-            "heading": 1.5191189525644635,
-            "angularVelocity": -0.0000059969184602345635,
-            "velocityX": 3.7580063119517533,
-            "velocityY": 0.4336043516435729,
-            "timestamp": 1.9304829279371158
-        },
-        {
-            "x": 6.217374593429904,
-            "y": 4.009948260386706,
-            "heading": 1.519118722914699,
-            "angularVelocity": -0.000005996898884562842,
-            "velocityX": 3.7580058387809343,
-            "velocityY": 0.43360845259010783,
-            "timestamp": 1.9687776814378166
-        },
-        {
-            "x": 6.361286503020876,
-            "y": 4.026553168908082,
-            "heading": 1.5191184932648618,
-            "angularVelocity": -0.000005996900776462565,
-            "velocityX": 3.7580058999032113,
-            "velocityY": 0.4336079228469906,
-            "timestamp": 2.0070724349385176
-        },
-        {
-            "x": 6.5051984133944005,
-            "y": 4.04315807064667,
-            "heading": 1.5191182636148604,
-            "angularVelocity": -0.000005996905070522029,
-            "velocityX": 3.7580059203381437,
-            "velocityY": 0.4336077457264339,
-            "timestamp": 2.0453671884392186
-        },
-        {
-            "x": 6.6491104352583505,
-            "y": 4.059762006067292,
-            "heading": 1.5191180339567425,
-            "angularVelocity": -0.000005997117020238007,
-            "velocityX": 3.7580088317141747,
-            "velocityY": 0.4335825120357026,
-            "timestamp": 2.0836619419399196
-        },
-        {
-            "x": 6.790280334982046,
-            "y": 4.068588148423426,
-            "heading": 1.5085875786821552,
-            "angularVelocity": -0.27498428144717973,
-            "velocityX": 3.6864031445224414,
-            "velocityY": 0.23047915312922457,
-            "timestamp": 2.1219566954406206
-        },
-        {
-            "x": 6.921716704437578,
-            "y": 4.075833761978748,
-            "heading": 1.4694677109965053,
-            "angularVelocity": -1.0215464028234083,
-            "velocityX": 3.43222915517987,
-            "velocityY": 0.18920642889606504,
-            "timestamp": 2.1602514489413216
-        },
-        {
-            "x": 7.043039125371844,
-            "y": 4.08211395301362,
-            "heading": 1.4037305732475935,
-            "angularVelocity": -1.716609502335841,
-            "velocityX": 3.1681212135768497,
-            "velocityY": 0.16399612115943957,
-            "timestamp": 2.1985462024420226
-        },
-        {
-            "x": 7.153946697571625,
-            "y": 4.087967374487544,
-            "heading": 1.3134036899027293,
-            "angularVelocity": -2.3587273735346383,
-            "velocityX": 2.896155793188521,
-            "velocityY": 0.1528517861805002,
-            "timestamp": 2.2368409559427236
-        },
-        {
-            "x": 7.254278338500056,
-            "y": 4.0938696715910385,
-            "heading": 1.199938125135736,
-            "angularVelocity": -2.9629532610757465,
-            "velocityX": 2.619983986229206,
-            "velocityY": 0.1541280871121784,
-            "timestamp": 2.2751357094434246
-        },
-        {
-            "x": 7.34404947628579,
-            "y": 4.100268813517142,
-            "heading": 1.0638507127340207,
-            "angularVelocity": -3.5536829450860603,
-            "velocityX": 2.344215057660344,
-            "velocityY": 0.16710231405423262,
-            "timestamp": 2.3134304629441256
-        },
-        {
-            "x": 7.4235066081317465,
-            "y": 4.107648882181887,
-            "heading": 0.9045749783818139,
-            "angularVelocity": -4.159205107542786,
-            "velocityX": 2.0748829691383666,
-            "velocityY": 0.192717487125464,
-            "timestamp": 2.3517252164448266
-        },
-        {
-            "x": 7.492760083799764,
-            "y": 4.113297423743801,
-            "heading": 0.7219171880631509,
-            "angularVelocity": -4.769786292404788,
-            "velocityX": 1.808432470175071,
-            "velocityY": 0.1475017083426549,
-            "timestamp": 2.3900199699455276
-        },
-        {
-            "x": 7.549820937161571,
-            "y": 4.1180620764150975,
-            "heading": 0.5402485968001275,
-            "angularVelocity": -4.7439551023536115,
-            "velocityX": 1.4900436259698675,
-            "velocityY": 0.1244205076606485,
-            "timestamp": 2.4283147234462286
-        },
-        {
-            "x": 7.595294548959434,
-            "y": 4.121747510359626,
-            "heading": 0.3723680212509228,
-            "angularVelocity": -4.383905370904984,
-            "velocityX": 1.1874632329734431,
-            "velocityY": 0.09623861254154589,
-            "timestamp": 2.4666094769469296
-        },
-        {
-            "x": 7.630234770559726,
-            "y": 4.124236647711556,
-            "heading": 0.22836852122657442,
-            "angularVelocity": -3.7602931697083015,
-            "velocityX": 0.912402311184801,
-            "velocityY": 0.06499943528517171,
-            "timestamp": 2.5049042304476306
-        },
-        {
-            "x": 7.655576852148959,
-            "y": 4.125750923706981,
-            "heading": 0.11519495737029227,
-            "angularVelocity": -2.9553281718920323,
-            "velocityX": 0.6617637998053468,
-            "velocityY": 0.03954264897925492,
-            "timestamp": 2.5431989839483315
-        },
-        {
-            "x": 7.671995118301485,
-            "y": 4.1266005721732935,
-            "heading": 0.03711275718074835,
-            "angularVelocity": -2.0389790520029063,
-            "velocityX": 0.4287340863083462,
-            "velocityY": 0.02218707234390751,
-            "timestamp": 2.5814937374490325
+            "heading": 1.7281561592052472,
+            "angularVelocity": -1.2465351647339973,
+            "velocityX": 2.715017476867699,
+            "velocityY": -1.8294644548346262,
+            "timestamp": 1.6178002761182595
+        },
+        {
+            "x": 5.010449341197396,
+            "y": 4.1015961278120825,
+            "heading": 1.6804127870661807,
+            "angularVelocity": -1.251568549956003,
+            "velocityX": 2.8838465288429167,
+            "velocityY": -1.5598993191587458,
+            "timestamp": 1.6559471056633712
+        },
+        {
+            "x": 5.126648667318374,
+            "y": 4.05252161275018,
+            "heading": 1.6338488947780365,
+            "angularVelocity": -1.2206490773575045,
+            "velocityX": 3.0461070423551457,
+            "velocityY": -1.2864637939008783,
+            "timestamp": 1.694093935208483
+        },
+        {
+            "x": 5.248756740712563,
+            "y": 4.014017499979329,
+            "heading": 1.589989878848282,
+            "angularVelocity": -1.1497421005375177,
+            "velocityX": 3.2010018879727395,
+            "velocityY": -1.0093660005294856,
+            "timestamp": 1.7322407647535947
+        },
+        {
+            "x": 5.376454980725802,
+            "y": 3.9862068849459353,
+            "heading": 1.5506775545962859,
+            "angularVelocity": -1.0305528590747366,
+            "velocityX": 3.347545301559156,
+            "velocityY": -0.7290413217820071,
+            "timestamp": 1.7703875942987064
+        },
+        {
+            "x": 5.509414833155046,
+            "y": 3.969152206173251,
+            "heading": 1.5183946275971996,
+            "angularVelocity": -0.8462807364186931,
+            "velocityX": 3.485475831533742,
+            "velocityY": -0.4470798484702507,
+            "timestamp": 1.8085344238438181
+        },
+        {
+            "x": 5.647435915880888,
+            "y": 3.96261903739833,
+            "heading": 1.4980750155673637,
+            "angularVelocity": -0.5326684359468331,
+            "velocityX": 3.618153444771329,
+            "velocityY": -0.17126374204166636,
+            "timestamp": 1.8466812533889299
+        },
+        {
+            "x": 5.791726765964232,
+            "y": 3.964767379969892,
+            "heading": 1.498073903211314,
+            "angularVelocity": -0.00002915985583373997,
+            "velocityX": 3.7825122507942877,
+            "velocityY": 0.056317722783366074,
+            "timestamp": 1.8848280829340416
+        },
+        {
+            "x": 5.935369435750422,
+            "y": 3.9785994401105067,
+            "heading": 1.4980736722429098,
+            "angularVelocity": -0.00000605472084526459,
+            "velocityX": 3.765520529467262,
+            "velocityY": 0.3626005176676574,
+            "timestamp": 1.9229749124791533
+        },
+        {
+            "x": 6.078764518036592,
+            "y": 3.9947980029743344,
+            "heading": 1.498073449658831,
+            "angularVelocity": -0.000005834929965972279,
+            "velocityX": 3.7590301473577066,
+            "velocityY": 0.42463719939010114,
+            "timestamp": 1.961121742024265
+        },
+        {
+            "x": 6.222150100858587,
+            "y": 4.0110804389753145,
+            "heading": 1.4980732273393433,
+            "angularVelocity": -0.000005827993841710607,
+            "velocityX": 3.7587811236710817,
+            "velocityY": 0.4268358915939984,
+            "timestamp": 1.9992685715693768
+        },
+        {
+            "x": 6.3655338107868324,
+            "y": 4.0273793596330805,
+            "heading": 1.4980730050704238,
+            "angularVelocity": -0.000005826668223171757,
+            "velocityX": 3.758732026701189,
+            "velocityY": 0.42726802860026514,
+            "timestamp": 2.0374154011144885
+        },
+        {
+            "x": 6.508922861230399,
+            "y": 4.043631229943055,
+            "heading": 1.4980727826574463,
+            "angularVelocity": -0.000005830444635518525,
+            "velocityX": 3.7588720256305224,
+            "velocityY": 0.42603462734214087,
+            "timestamp": 2.0755622306596
+        },
+        {
+            "x": 6.6523224677448685,
+            "y": 4.059789689420356,
+            "heading": 1.4980725589565675,
+            "angularVelocity": -0.000005864206314008162,
+            "velocityX": 3.759148747731203,
+            "velocityY": 0.4235859092458616,
+            "timestamp": 2.1137090602047115
+        },
+        {
+            "x": 6.792869039761006,
+            "y": 4.068475137054533,
+            "heading": 1.4874839096219183,
+            "angularVelocity": -0.2775761304689174,
+            "velocityX": 3.684357879593819,
+            "velocityY": 0.2276846526483585,
+            "timestamp": 2.151855889749823
+        },
+        {
+            "x": 6.923724640852256,
+            "y": 4.075654653912489,
+            "heading": 1.4487910280540974,
+            "angularVelocity": -1.014314479844837,
+            "velocityX": 3.430313938317647,
+            "velocityY": 0.18820743280285396,
+            "timestamp": 2.1900027192949345
+        },
+        {
+            "x": 7.04453293582936,
+            "y": 4.081916427546979,
+            "heading": 1.3838287724616933,
+            "angularVelocity": -1.7029529417506197,
+            "velocityX": 3.1669288488112004,
+            "velocityY": 0.1641492545881335,
+            "timestamp": 2.228149548840046
+        },
+        {
+            "x": 7.155015675902827,
+            "y": 4.087769066352678,
+            "heading": 1.2944912667640571,
+            "angularVelocity": -2.3419378953075096,
+            "velocityX": 2.896249606872667,
+            "velocityY": 0.1534239902944478,
+            "timestamp": 2.2662963783851575
+        },
+        {
+            "x": 7.255027442080941,
+            "y": 4.093665982164644,
+            "heading": 1.1821266374518384,
+            "angularVelocity": -2.945582389205927,
+            "velocityX": 2.621758279016409,
+            "velocityY": 0.15458468980091475,
+            "timestamp": 2.304443207930269
+        },
+        {
+            "x": 7.344594486390613,
+            "y": 4.100041344791564,
+            "heading": 1.0471923484078691,
+            "angularVelocity": -3.5372346969048754,
+            "velocityX": 2.3479551348760803,
+            "velocityY": 0.16712693303897977,
+            "timestamp": 2.3425900374753805
+        },
+        {
+            "x": 7.423972884493799,
+            "y": 4.107363761305008,
+            "heading": 0.8891062073498277,
+            "angularVelocity": -4.144148883225662,
+            "velocityX": 2.080864885752389,
+            "velocityY": 0.19195347557968287,
+            "timestamp": 2.380736867020492
+        },
+        {
+            "x": 7.492968640563269,
+            "y": 4.113132211800853,
+            "heading": 0.7092469399580156,
+            "angularVelocity": -4.714920467480658,
+            "velocityX": 1.8086891333366921,
+            "velocityY": 0.1512170359820358,
+            "timestamp": 2.4188836965656035
+        },
+        {
+            "x": 7.549873361136887,
+            "y": 4.117954510008317,
+            "heading": 0.5306887118642236,
+            "angularVelocity": -4.680814374954733,
+            "velocityX": 1.4917287033341673,
+            "velocityY": 0.12641412838299443,
+            "timestamp": 2.457030526110715
+        },
+        {
+            "x": 7.595282353021441,
+            "y": 4.121668824298383,
+            "heading": 0.3658074925223697,
+            "angularVelocity": -4.322278451654422,
+            "velocityX": 1.1903739426332636,
+            "velocityY": 0.09736888581300335,
+            "timestamp": 2.4951773556558265
+        },
+        {
+            "x": 7.630209510034256,
+            "y": 4.124185175295285,
+            "heading": 0.22438733020455492,
+            "angularVelocity": -3.7072586111122043,
+            "velocityX": 0.9155978997292245,
+            "velocityY": 0.06596487904592362,
+            "timestamp": 2.533324185200938
+        },
+        {
+            "x": 7.65555835714788,
+            "y": 4.125725160446138,
+            "heading": 0.1131980096846438,
+            "angularVelocity": -2.91477225881568,
+            "velocityX": 0.6645073107231708,
+            "velocityY": 0.04036993818976583,
+            "timestamp": 2.5714710147460496
+        },
+        {
+            "x": 7.671987700512514,
+            "y": 4.126592780034806,
+            "heading": 0.03644789338444829,
+            "angularVelocity": -2.011965796774904,
+            "velocityX": 0.43068699445162756,
+            "velocityY": 0.02274421227203338,
+            "timestamp": 2.609617844291161
         },
         {
             "x": 7.68,
             "y": 4.127,
             "heading": -0.003,
-            "angularVelocity": -1.047473962197309,
-            "velocityX": 0.20903337838088354,
-            "velocityY": 0.010430353774151994,
-            "timestamp": 2.6197884909497335
+            "angularVelocity": -1.0341067358634992,
+            "velocityX": 0.21003841165919293,
+            "velocityY": 0.010675067103991773,
+            "timestamp": 2.6477646738362726
         },
         {
             "x": 7.68,
             "y": 4.127,
             "heading": -0.003,
-            "angularVelocity": 7.250240493925158e-33,
-            "velocityX": 3.887711813813661e-34,
-            "velocityY": -4.900942603355191e-34,
-            "timestamp": 2.6580832444504345
+            "angularVelocity": 1.475285929099793e-30,
+            "velocityX": 9.681452358723228e-32,
+            "velocityY": 1.6696229962948817e-32,
+            "timestamp": 2.685911503381384
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.1.traj
new file mode 100644
index 00000000..f0cd1af3
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.1.traj	
@@ -0,0 +1,454 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": 1.04,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7232220177061155,
+            "y": 6.714742544338994,
+            "heading": 0.9631209877994957,
+            "angularVelocity": -1.4677311041544436,
+            "velocityX": 0.25242736725510606,
+            "velocityY": -0.10037240309808587,
+            "timestamp": 0.052379493752565884
+        },
+        {
+            "x": 0.7500587763634208,
+            "y": 6.703146440373859,
+            "heading": 0.8150187561291465,
+            "angularVelocity": -2.8274849766583348,
+            "velocityX": 0.5123523870635092,
+            "velocityY": -0.22138633145086953,
+            "timestamp": 0.10475898750513177
+        },
+        {
+            "x": 0.7962886642360024,
+            "y": 6.691501707403842,
+            "heading": 0.6243144861489608,
+            "angularVelocity": -3.6408192656662295,
+            "velocityX": 0.882595163881611,
+            "velocityY": -0.22231472921494633,
+            "timestamp": 0.15713848125769764
+        },
+        {
+            "x": 0.8619102089829233,
+            "y": 6.679485060515379,
+            "heading": 0.3880805528376709,
+            "angularVelocity": -4.510046134222473,
+            "velocityX": 1.2528098315899883,
+            "velocityY": -0.22941510174244611,
+            "timestamp": 0.20951797501026354
+        },
+        {
+            "x": 0.948820798934311,
+            "y": 6.665315928828611,
+            "heading": 0.12168049392786562,
+            "angularVelocity": -5.0859609328842605,
+            "velocityX": 1.6592483761286883,
+            "velocityY": -0.2705091376732434,
+            "timestamp": 0.26189746876282943
+        },
+        {
+            "x": 1.058148493976547,
+            "y": 6.648822752325803,
+            "heading": -0.13051602501618126,
+            "angularVelocity": -4.814794891592335,
+            "velocityX": 2.0872232091185596,
+            "velocityY": -0.3148785015128142,
+            "timestamp": 0.31427696251539533
+        },
+        {
+            "x": 1.1845022977802875,
+            "y": 6.626138581630819,
+            "heading": -0.32549006587483453,
+            "angularVelocity": -3.722335343286937,
+            "velocityX": 2.41227615525686,
+            "velocityY": -0.43307350014000373,
+            "timestamp": 0.36665645626796123
+        },
+        {
+            "x": 1.3283887493945847,
+            "y": 6.598463049204702,
+            "heading": -0.4648019456307372,
+            "angularVelocity": -2.6596644941624366,
+            "velocityX": 2.7469996616232804,
+            "velocityY": -0.5283657867495356,
+            "timestamp": 0.4190359500205271
+        },
+        {
+            "x": 1.4904287339592295,
+            "y": 6.567163978467113,
+            "heading": -0.552248498766723,
+            "angularVelocity": -1.6694806854963555,
+            "velocityX": 3.0935767598308836,
+            "velocityY": -0.5975443536251414,
+            "timestamp": 0.471415443773093
+        },
+        {
+            "x": 1.6709503203675815,
+            "y": 6.533637134258406,
+            "heading": -0.590326905081816,
+            "angularVelocity": -0.7269716369343046,
+            "velocityX": 3.4464171658686396,
+            "velocityY": -0.6400757587899589,
+            "timestamp": 0.5237949375256589
+        },
+        {
+            "x": 1.8675588987397893,
+            "y": 6.50898247632772,
+            "heading": -0.5903272348122566,
+            "angularVelocity": -0.000006295029161022994,
+            "velocityX": 3.7535410193340524,
+            "velocityY": -0.4706929403928793,
+            "timestamp": 0.5761744312782248
+        },
+        {
+            "x": 2.0641677208301488,
+            "y": 6.4843297618820115,
+            "heading": -0.5903275645018402,
+            "angularVelocity": -0.000006294249141189924,
+            "velocityX": 3.7535456722646963,
+            "velocityY": -0.47065583646465164,
+            "timestamp": 0.6285539250307907
+        },
+        {
+            "x": 2.2607765429184905,
+            "y": 6.459677047420138,
+            "heading": -0.5903278941913652,
+            "angularVelocity": -0.000006294248023551401,
+            "velocityX": 3.753545672226176,
+            "velocityY": -0.4706558367732492,
+            "timestamp": 0.6809334187833566
+        },
+        {
+            "x": 2.4573850313302983,
+            "y": 6.435021672008896,
+            "heading": -0.5903282238892777,
+            "angularVelocity": -0.000006294408153628446,
+            "velocityX": 3.753539301860412,
+            "velocityY": -0.4707066381303828,
+            "timestamp": 0.7333129125359225
+        },
+        {
+            "x": 2.6504394143479257,
+            "y": 6.390381144575869,
+            "heading": -0.5903286607455448,
+            "angularVelocity": -0.000008340215525789833,
+            "velocityX": 3.685686309409395,
+            "velocityY": -0.8522519832647445,
+            "timestamp": 0.7856924062884884
+        },
+        {
+            "x": 2.837061882019043,
+            "y": 6.323787212371826,
+            "heading": -0.5903296599203023,
+            "angularVelocity": -0.000019075685653139892,
+            "velocityX": 3.56289177884571,
+            "velocityY": -1.2713741090859834,
+            "timestamp": 0.8380719000410543
+        },
+        {
+            "x": 3.013317183081598,
+            "y": 6.237060061778313,
+            "heading": -0.5903324266212199,
+            "angularVelocity": -0.000053280252957349455,
+            "velocityX": 3.394268952515208,
+            "velocityY": -1.6701640905268176,
+            "timestamp": 0.8899992232815851
+        },
+        {
+            "x": 3.177545337709341,
+            "y": 6.131367851066497,
+            "heading": -0.5941789632152575,
+            "angularVelocity": -0.0740753875608063,
+            "velocityX": 3.1626539628670427,
+            "velocityY": -2.0353872319249358,
+            "timestamp": 0.9419265465221159
+        },
+        {
+            "x": 3.3316611475687807,
+            "y": 6.009565055315307,
+            "heading": -0.5941812553691556,
+            "angularVelocity": -0.00004414157624491074,
+            "velocityX": 2.967913619301448,
+            "velocityY": -2.3456397932739814,
+            "timestamp": 0.9938538697626468
+        },
+        {
+            "x": 3.48577581317155,
+            "y": 5.887760811491488,
+            "heading": -0.5941835469667753,
+            "angularVelocity": -0.00004413086361592441,
+            "velocityX": 2.9678915835676754,
+            "velocityY": -2.3456676797995843,
+            "timestamp": 1.0457811930031775
+        },
+        {
+            "x": 3.639890478746931,
+            "y": 5.7659565676417115,
+            "heading": -0.5941858385862709,
+            "angularVelocity": -0.00004413128489121798,
+            "velocityX": 2.967891583040235,
+            "velocityY": -2.3456676802994654,
+            "timestamp": 1.0977085162437084
+        },
+        {
+            "x": 3.7940051443206335,
+            "y": 5.644152323798514,
+            "heading": -0.594188130227655,
+            "angularVelocity": -0.0000441317064133718,
+            "velocityX": 2.9678915830079062,
+            "velocityY": -2.3456676801727796,
+            "timestamp": 1.1496358394842392
+        },
+        {
+            "x": 3.948119809892657,
+            "y": 5.522348079961896,
+            "heading": -0.5941904218909294,
+            "angularVelocity": -0.0000441321279705331,
+            "velocityX": 2.9678915829755823,
+            "velocityY": -2.345667680046072,
+            "timestamp": 1.20156316272477
+        },
+        {
+            "x": 4.1022344754630025,
+            "y": 5.400543836131857,
+            "heading": -0.5941927135760963,
+            "angularVelocity": -0.00004413254956830847,
+            "velocityX": 2.9678915829432535,
+            "velocityY": -2.3456676799193525,
+            "timestamp": 1.2534904859653009
+        },
+        {
+            "x": 4.256349141031668,
+            "y": 5.2787395923083995,
+            "heading": -0.5941950052831575,
+            "angularVelocity": -0.000044132971203721087,
+            "velocityX": 2.9678915829109185,
+            "velocityY": -2.3456676797926255,
+            "timestamp": 1.3054178092058317
+        },
+        {
+            "x": 4.410463806598655,
+            "y": 5.1569353484915235,
+            "heading": -0.594197297012115,
+            "angularVelocity": -0.00004413339287361589,
+            "velocityX": 2.9678915828785764,
+            "velocityY": -2.345667679665889,
+            "timestamp": 1.3573451324463626
+        },
+        {
+            "x": 4.564578472163962,
+            "y": 5.035131104681229,
+            "heading": -0.5941995887629704,
+            "angularVelocity": -0.000044133814578891406,
+            "velocityX": 2.9678915828462293,
+            "velocityY": -2.3456676795391433,
+            "timestamp": 1.4092724556868934
+        },
+        {
+            "x": 4.718693137727589,
+            "y": 4.913326860877516,
+            "heading": -0.5942018805357254,
+            "angularVelocity": -0.00004413423631330234,
+            "velocityX": 2.967891582813878,
+            "velocityY": -2.3456676794123914,
+            "timestamp": 1.4611997789274243
+        },
+        {
+            "x": 4.872807803289535,
+            "y": 4.791522617080385,
+            "heading": -0.5942041723303811,
+            "angularVelocity": -0.00004413465807134274,
+            "velocityX": 2.9678915827815215,
+            "velocityY": -2.3456676792856337,
+            "timestamp": 1.513127102167955
+        },
+        {
+            "x": 5.026922468849802,
+            "y": 4.669718373289836,
+            "heading": -0.5942064641469391,
+            "angularVelocity": -0.000044135079855748624,
+            "velocityX": 2.9678915827491625,
+            "velocityY": -2.3456676791588693,
+            "timestamp": 1.565054425408486
+        },
+        {
+            "x": 5.181037134408388,
+            "y": 4.5479141295058705,
+            "heading": -0.5942087559854001,
+            "angularVelocity": -0.000044135501659061016,
+            "velocityX": 2.967891582716799,
+            "velocityY": -2.345667679032102,
+            "timestamp": 1.6169817486490168
+        },
+        {
+            "x": 5.335151799963721,
+            "y": 4.4261098857264995,
+            "heading": -0.594211047845765,
+            "angularVelocity": -0.00004413592347400816,
+            "velocityX": 2.967891582654166,
+            "velocityY": -2.345667678943628,
+            "timestamp": 1.6689090718895476
+        },
+        {
+            "x": 5.489266395568848,
+            "y": 4.304305553436279,
+            "heading": -0.5942133396987443,
+            "angularVelocity": -0.000044135781248471857,
+            "velocityX": 2.9678902355751515,
+            "velocityY": -2.345669383457615,
+            "timestamp": 1.7208363951300785
+        },
+        {
+            "x": 5.673870655984677,
+            "y": 4.147383169820797,
+            "heading": -0.5942132891163008,
+            "angularVelocity": 7.897648233188991e-7,
+            "velocityX": 2.882303439061699,
+            "velocityY": -2.450094732060046,
+            "timestamp": 1.7848838711570736
+        },
+        {
+            "x": 5.858474845870276,
+            "y": 3.9904607032332415,
+            "heading": -0.5942132385338614,
+            "angularVelocity": 7.897647575237561e-7,
+            "velocityX": 2.882302337843733,
+            "velocityY": -2.450096027537681,
+            "timestamp": 1.8489313471840687
+        },
+        {
+            "x": 6.043079035755059,
+            "y": 3.833538236644727,
+            "heading": -0.5942131879514223,
+            "angularVelocity": 7.897647543646741e-7,
+            "velocityX": 2.882302337831004,
+            "velocityY": -2.4500960275526547,
+            "timestamp": 1.9129788232110638
+        },
+        {
+            "x": 6.227683225639842,
+            "y": 3.676615770056212,
+            "heading": -0.5942131373689834,
+            "angularVelocity": 7.897647493463809e-7,
+            "velocityX": 2.8823023378310055,
+            "velocityY": -2.450096027552654,
+            "timestamp": 1.977026299238059
+        },
+        {
+            "x": 6.412287415524625,
+            "y": 3.5196933034676974,
+            "heading": -0.5942130867865447,
+            "angularVelocity": 7.89764745190247e-7,
+            "velocityX": 2.882302337831007,
+            "velocityY": -2.4500960275526538,
+            "timestamp": 2.041073775265054
+        },
+        {
+            "x": 6.596891605409407,
+            "y": 3.362770836879183,
+            "heading": -0.5942130362041063,
+            "angularVelocity": 7.897647430611541e-7,
+            "velocityX": 2.882302337831007,
+            "velocityY": -2.450096027552653,
+            "timestamp": 2.105121251292049
+        },
+        {
+            "x": 6.781495795294782,
+            "y": 3.2058483702913625,
+            "heading": -0.5942129856216681,
+            "angularVelocity": 7.897647381910717e-7,
+            "velocityX": 2.8823023378402177,
+            "velocityY": -2.4500960275418184,
+            "timestamp": 2.169168727319044
+        },
+        {
+            "x": 6.966100036209046,
+            "y": 3.048925963734244,
+            "heading": -0.5942129350390435,
+            "angularVelocity": 7.897676514556789e-7,
+            "velocityX": 2.882303134575621,
+            "velocityY": -2.4500950902573897,
+            "timestamp": 2.233216203346039
+        },
+        {
+            "x": 7.143621774279278,
+            "y": 2.9040156366918444,
+            "heading": -0.5543160517170741,
+            "angularVelocity": 0.6229267068253146,
+            "velocityX": 2.771721058850299,
+            "velocityY": -2.262545474568311,
+            "timestamp": 2.2972636793730343
+        },
+        {
+            "x": 7.2980980894932665,
+            "y": 2.7758641788664877,
+            "heading": -0.43954991420980805,
+            "angularVelocity": 1.7918916501704738,
+            "velocityX": 2.411903244225882,
+            "velocityY": -2.000882248214473,
+            "timestamp": 2.3613111554000294
+        },
+        {
+            "x": 7.426195766165712,
+            "y": 2.6690947773000513,
+            "heading": -0.31008673137290993,
+            "angularVelocity": 2.0213627588124035,
+            "velocityX": 2.000042540606181,
+            "velocityY": -1.667035271170323,
+            "timestamp": 2.4253586314270246
+        },
+        {
+            "x": 7.528097771332223,
+            "y": 2.5840084569104262,
+            "heading": -0.19375204823220424,
+            "angularVelocity": 1.816382008428752,
+            "velocityX": 1.591038577750685,
+            "velocityY": -1.3284882663254731,
+            "timestamp": 2.4894061074540197
+        },
+        {
+            "x": 7.6042028509496316,
+            "y": 2.5203982409108687,
+            "heading": -0.10083265861354884,
+            "angularVelocity": 1.4507892485800875,
+            "velocityX": 1.1882604021012733,
+            "velocityY": -0.9931728765197068,
+            "timestamp": 2.553453583481015
+        },
+        {
+            "x": 7.654775762663328,
+            "y": 2.4780994279137802,
+            "heading": -0.036281315483046445,
+            "angularVelocity": 1.0078670875850742,
+            "velocityX": 0.7896159981757911,
+            "velocityY": -0.660429038284975,
+            "timestamp": 2.61750105950801
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -0.003,
+            "angularVelocity": 0.5196350824038538,
+            "velocityX": 0.39383655533967327,
+            "velocityY": -0.32943418261927393,
+            "timestamp": 2.681548535535005
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -0.003,
+            "angularVelocity": 1.9104375826405664e-34,
+            "velocityX": -1.672523979523505e-35,
+            "velocityY": 0,
+            "timestamp": 2.745596011562
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.traj
index ca6ee59b..f0cd1af3 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.traj	
@@ -10,391 +10,445 @@
             "timestamp": 0
         },
         {
-            "x": 0.7271139423991175,
-            "y": 6.709732017673653,
-            "heading": 0.9305052411067223,
-            "angularVelocity": -1.767895408627961,
-            "velocityX": 0.2763206248110281,
-            "velocityY": -0.16578618916650492,
-            "timestamp": 0.06193508867035021
-        },
-        {
-            "x": 0.7615532933104725,
-            "y": 6.686914726598375,
-            "heading": 0.7206788937202419,
-            "angularVelocity": -3.387842851138587,
-            "velocityX": 0.5560555680263672,
-            "velocityY": -0.36840652956392367,
-            "timestamp": 0.12387017734070042
-        },
-        {
-            "x": 0.8202948153873781,
-            "y": 6.650989300229197,
-            "heading": 0.4540271404715134,
-            "angularVelocity": -4.305342237709284,
-            "velocityX": 0.9484368770271357,
-            "velocityY": -0.5800496477915925,
-            "timestamp": 0.18580526601105063
-        },
-        {
-            "x": 0.9068549514543394,
-            "y": 6.604915057765644,
-            "heading": 0.1437481054753499,
-            "angularVelocity": -5.009745552276917,
-            "velocityX": 1.3975944480789861,
-            "velocityY": -0.7439117865606645,
-            "timestamp": 0.24774035468140085
-        },
-        {
-            "x": 1.022793030858839,
-            "y": 6.550181878656492,
-            "heading": -0.13251218552677876,
-            "angularVelocity": -4.460481076769346,
-            "velocityX": 1.871928851536495,
-            "velocityY": -0.8837184265686551,
-            "timestamp": 0.30967544335175107
-        },
-        {
-            "x": 1.1650239086947571,
-            "y": 6.483494014593417,
-            "heading": -0.35594040398018906,
-            "angularVelocity": -3.6074577957352734,
-            "velocityX": 2.296450701684509,
-            "velocityY": -1.0767380090149254,
-            "timestamp": 0.37161053202210126
-        },
-        {
-            "x": 1.3271753983931303,
-            "y": 6.398307258999271,
-            "heading": -0.5136957877872723,
-            "angularVelocity": -2.5471083870846947,
-            "velocityX": 2.61808763302859,
-            "velocityY": -1.3754199343695408,
-            "timestamp": 0.43354562069245145
-        },
-        {
-            "x": 1.510544798478812,
-            "y": 6.2989823057862875,
-            "heading": -0.5925289533905876,
-            "angularVelocity": -1.2728352747326335,
-            "velocityX": 2.96067066379232,
-            "velocityY": -1.6036943733404823,
-            "timestamp": 0.49548070936280164
-        },
-        {
-            "x": 1.7178378905511122,
-            "y": 6.190936000456915,
-            "heading": -0.5942013489806535,
-            "angularVelocity": -0.027002392762643195,
-            "velocityX": 3.3469410720572084,
-            "velocityY": -1.7445087695675872,
-            "timestamp": 0.5574157980331519
-        },
-        {
-            "x": 1.927701821836241,
-            "y": 6.086763021965953,
-            "heading": -0.5942019920614379,
-            "angularVelocity": -0.000010383141417040622,
-            "velocityX": 3.388449678374247,
-            "velocityY": -1.6819702809408126,
-            "timestamp": 0.6193508867035021
-        },
-        {
-            "x": 2.1375657815463693,
-            "y": 5.982590100737573,
-            "heading": -0.594202635139582,
-            "angularVelocity": -0.0000103830987882079,
-            "velocityX": 3.3884501373224802,
-            "velocityY": -1.6819693563827811,
-            "timestamp": 0.6812859753738524
-        },
-        {
-            "x": 2.347429741257157,
-            "y": 5.878417179509917,
-            "heading": -0.5942032782166262,
-            "angularVelocity": -0.000010383081029176535,
-            "velocityX": 3.3884501373331317,
-            "velocityY": -1.6819693563710962,
-            "timestamp": 0.7432210640442026
-        },
-        {
-            "x": 2.5572937009682177,
-            "y": 5.774244258282216,
-            "heading": -0.5942039212925836,
-            "angularVelocity": -0.000010383063479788062,
-            "velocityX": 3.3884501373375415,
-            "velocityY": -1.6819693563718265,
-            "timestamp": 0.8051561527145529
-        },
-        {
-            "x": 2.7671576606795463,
-            "y": 5.670071337054474,
-            "heading": -0.594204564367474,
-            "angularVelocity": -0.00001038304625561267,
-            "velocityX": 3.388450137341862,
-            "velocityY": -1.68196935637249,
-            "timestamp": 0.8670912413849031
-        },
-        {
-            "x": 2.977021620391137,
-            "y": 5.5658984158266955,
-            "heading": -0.5942052074413191,
-            "angularVelocity": -0.000010383029375253952,
-            "velocityX": 3.388450137346089,
-            "velocityY": -1.6819693563730826,
-            "timestamp": 0.9290263300552534
-        },
-        {
-            "x": 3.186885580102983,
-            "y": 5.461725494598884,
-            "heading": -0.5942058505141404,
-            "angularVelocity": -0.000010383012846871076,
-            "velocityX": 3.3884501373502207,
-            "velocityY": -1.6819693563736025,
-            "timestamp": 0.9909614187256036
-        },
-        {
-            "x": 3.3967495398150795,
-            "y": 5.357552573371046,
-            "heading": -0.594206493585961,
-            "angularVelocity": -0.000010382996688205053,
-            "velocityX": 3.3884501373542517,
-            "velocityY": -1.681969356374048,
-            "timestamp": 1.0528965073959538
-        },
-        {
-            "x": 3.6066134995274184,
-            "y": 5.253379652143185,
-            "heading": -0.5942071366568044,
-            "angularVelocity": -0.000010382980910469912,
-            "velocityX": 3.38845013735818,
-            "velocityY": -1.6819693563744145,
-            "timestamp": 1.114831596066304
-        },
-        {
-            "x": 3.8164774592399944,
-            "y": 5.149206730915306,
-            "heading": -0.5942077797266946,
-            "angularVelocity": -0.000010382965523044611,
-            "velocityX": 3.388450137362003,
-            "velocityY": -1.681969356374701,
-            "timestamp": 1.1767666847366542
-        },
-        {
-            "x": 4.026341418952801,
-            "y": 5.045033809687414,
-            "heading": -0.594208422795657,
-            "angularVelocity": -0.00001038295054018878,
-            "velocityX": 3.388450137365716,
-            "velocityY": -1.6819693563749047,
-            "timestamp": 1.2387017734070045
-        },
-        {
-            "x": 4.236205378665829,
-            "y": 4.940860888459515,
-            "heading": -0.594209065863717,
-            "angularVelocity": -0.000010382935971395078,
-            "velocityX": 3.388450137369317,
-            "velocityY": -1.681969356375023,
-            "timestamp": 1.3006368620773547
-        },
-        {
-            "x": 4.446069338379075,
-            "y": 4.836687967231613,
-            "heading": -0.5942097089309012,
-            "angularVelocity": -0.000010382921828372961,
-            "velocityX": 3.3884501373728035,
-            "velocityY": -1.6819693563750544,
-            "timestamp": 1.362571950747705
-        },
-        {
-            "x": 4.6559332980925285,
-            "y": 4.732515046003717,
-            "heading": -0.5942103519972364,
-            "angularVelocity": -0.000010382908121631514,
-            "velocityX": 3.388450137376173,
-            "velocityY": -1.6819693563749973,
-            "timestamp": 1.4245070394180552
-        },
-        {
-            "x": 4.865797257806183,
-            "y": 4.628342124775828,
-            "heading": -0.5942109950627501,
-            "angularVelocity": -0.00001038289485891648,
-            "velocityX": 3.388450137379422,
-            "velocityY": -1.6819693563748486,
-            "timestamp": 1.4864421280884055
-        },
-        {
-            "x": 5.075661217518705,
-            "y": 4.524169203545277,
-            "heading": -0.594211638127464,
-            "angularVelocity": -0.000010382881945026341,
-            "velocityX": 3.3884501373611275,
-            "velocityY": -1.6819693564178315,
-            "timestamp": 1.5483772167587557
-        },
-        {
-            "x": 5.285525078653758,
-            "y": 4.419996083725492,
-            "heading": -0.5942122811951368,
-            "angularVelocity": -0.000010382929719206097,
-            "velocityX": 3.3884485457355824,
-            "velocityY": -1.6819725628269917,
-            "timestamp": 1.610312305429106
+            "x": 0.7232220177061155,
+            "y": 6.714742544338994,
+            "heading": 0.9631209877994957,
+            "angularVelocity": -1.4677311041544436,
+            "velocityX": 0.25242736725510606,
+            "velocityY": -0.10037240309808587,
+            "timestamp": 0.052379493752565884
+        },
+        {
+            "x": 0.7500587763634208,
+            "y": 6.703146440373859,
+            "heading": 0.8150187561291465,
+            "angularVelocity": -2.8274849766583348,
+            "velocityX": 0.5123523870635092,
+            "velocityY": -0.22138633145086953,
+            "timestamp": 0.10475898750513177
+        },
+        {
+            "x": 0.7962886642360024,
+            "y": 6.691501707403842,
+            "heading": 0.6243144861489608,
+            "angularVelocity": -3.6408192656662295,
+            "velocityX": 0.882595163881611,
+            "velocityY": -0.22231472921494633,
+            "timestamp": 0.15713848125769764
+        },
+        {
+            "x": 0.8619102089829233,
+            "y": 6.679485060515379,
+            "heading": 0.3880805528376709,
+            "angularVelocity": -4.510046134222473,
+            "velocityX": 1.2528098315899883,
+            "velocityY": -0.22941510174244611,
+            "timestamp": 0.20951797501026354
+        },
+        {
+            "x": 0.948820798934311,
+            "y": 6.665315928828611,
+            "heading": 0.12168049392786562,
+            "angularVelocity": -5.0859609328842605,
+            "velocityX": 1.6592483761286883,
+            "velocityY": -0.2705091376732434,
+            "timestamp": 0.26189746876282943
+        },
+        {
+            "x": 1.058148493976547,
+            "y": 6.648822752325803,
+            "heading": -0.13051602501618126,
+            "angularVelocity": -4.814794891592335,
+            "velocityX": 2.0872232091185596,
+            "velocityY": -0.3148785015128142,
+            "timestamp": 0.31427696251539533
+        },
+        {
+            "x": 1.1845022977802875,
+            "y": 6.626138581630819,
+            "heading": -0.32549006587483453,
+            "angularVelocity": -3.722335343286937,
+            "velocityX": 2.41227615525686,
+            "velocityY": -0.43307350014000373,
+            "timestamp": 0.36665645626796123
+        },
+        {
+            "x": 1.3283887493945847,
+            "y": 6.598463049204702,
+            "heading": -0.4648019456307372,
+            "angularVelocity": -2.6596644941624366,
+            "velocityX": 2.7469996616232804,
+            "velocityY": -0.5283657867495356,
+            "timestamp": 0.4190359500205271
+        },
+        {
+            "x": 1.4904287339592295,
+            "y": 6.567163978467113,
+            "heading": -0.552248498766723,
+            "angularVelocity": -1.6694806854963555,
+            "velocityX": 3.0935767598308836,
+            "velocityY": -0.5975443536251414,
+            "timestamp": 0.471415443773093
+        },
+        {
+            "x": 1.6709503203675815,
+            "y": 6.533637134258406,
+            "heading": -0.590326905081816,
+            "angularVelocity": -0.7269716369343046,
+            "velocityX": 3.4464171658686396,
+            "velocityY": -0.6400757587899589,
+            "timestamp": 0.5237949375256589
+        },
+        {
+            "x": 1.8675588987397893,
+            "y": 6.50898247632772,
+            "heading": -0.5903272348122566,
+            "angularVelocity": -0.000006295029161022994,
+            "velocityX": 3.7535410193340524,
+            "velocityY": -0.4706929403928793,
+            "timestamp": 0.5761744312782248
+        },
+        {
+            "x": 2.0641677208301488,
+            "y": 6.4843297618820115,
+            "heading": -0.5903275645018402,
+            "angularVelocity": -0.000006294249141189924,
+            "velocityX": 3.7535456722646963,
+            "velocityY": -0.47065583646465164,
+            "timestamp": 0.6285539250307907
+        },
+        {
+            "x": 2.2607765429184905,
+            "y": 6.459677047420138,
+            "heading": -0.5903278941913652,
+            "angularVelocity": -0.000006294248023551401,
+            "velocityX": 3.753545672226176,
+            "velocityY": -0.4706558367732492,
+            "timestamp": 0.6809334187833566
+        },
+        {
+            "x": 2.4573850313302983,
+            "y": 6.435021672008896,
+            "heading": -0.5903282238892777,
+            "angularVelocity": -0.000006294408153628446,
+            "velocityX": 3.753539301860412,
+            "velocityY": -0.4707066381303828,
+            "timestamp": 0.7333129125359225
+        },
+        {
+            "x": 2.6504394143479257,
+            "y": 6.390381144575869,
+            "heading": -0.5903286607455448,
+            "angularVelocity": -0.000008340215525789833,
+            "velocityX": 3.685686309409395,
+            "velocityY": -0.8522519832647445,
+            "timestamp": 0.7856924062884884
+        },
+        {
+            "x": 2.837061882019043,
+            "y": 6.323787212371826,
+            "heading": -0.5903296599203023,
+            "angularVelocity": -0.000019075685653139892,
+            "velocityX": 3.56289177884571,
+            "velocityY": -1.2713741090859834,
+            "timestamp": 0.8380719000410543
+        },
+        {
+            "x": 3.013317183081598,
+            "y": 6.237060061778313,
+            "heading": -0.5903324266212199,
+            "angularVelocity": -0.000053280252957349455,
+            "velocityX": 3.394268952515208,
+            "velocityY": -1.6701640905268176,
+            "timestamp": 0.8899992232815851
+        },
+        {
+            "x": 3.177545337709341,
+            "y": 6.131367851066497,
+            "heading": -0.5941789632152575,
+            "angularVelocity": -0.0740753875608063,
+            "velocityX": 3.1626539628670427,
+            "velocityY": -2.0353872319249358,
+            "timestamp": 0.9419265465221159
+        },
+        {
+            "x": 3.3316611475687807,
+            "y": 6.009565055315307,
+            "heading": -0.5941812553691556,
+            "angularVelocity": -0.00004414157624491074,
+            "velocityX": 2.967913619301448,
+            "velocityY": -2.3456397932739814,
+            "timestamp": 0.9938538697626468
+        },
+        {
+            "x": 3.48577581317155,
+            "y": 5.887760811491488,
+            "heading": -0.5941835469667753,
+            "angularVelocity": -0.00004413086361592441,
+            "velocityX": 2.9678915835676754,
+            "velocityY": -2.3456676797995843,
+            "timestamp": 1.0457811930031775
+        },
+        {
+            "x": 3.639890478746931,
+            "y": 5.7659565676417115,
+            "heading": -0.5941858385862709,
+            "angularVelocity": -0.00004413128489121798,
+            "velocityX": 2.967891583040235,
+            "velocityY": -2.3456676802994654,
+            "timestamp": 1.0977085162437084
+        },
+        {
+            "x": 3.7940051443206335,
+            "y": 5.644152323798514,
+            "heading": -0.594188130227655,
+            "angularVelocity": -0.0000441317064133718,
+            "velocityX": 2.9678915830079062,
+            "velocityY": -2.3456676801727796,
+            "timestamp": 1.1496358394842392
+        },
+        {
+            "x": 3.948119809892657,
+            "y": 5.522348079961896,
+            "heading": -0.5941904218909294,
+            "angularVelocity": -0.0000441321279705331,
+            "velocityX": 2.9678915829755823,
+            "velocityY": -2.345667680046072,
+            "timestamp": 1.20156316272477
+        },
+        {
+            "x": 4.1022344754630025,
+            "y": 5.400543836131857,
+            "heading": -0.5941927135760963,
+            "angularVelocity": -0.00004413254956830847,
+            "velocityX": 2.9678915829432535,
+            "velocityY": -2.3456676799193525,
+            "timestamp": 1.2534904859653009
+        },
+        {
+            "x": 4.256349141031668,
+            "y": 5.2787395923083995,
+            "heading": -0.5941950052831575,
+            "angularVelocity": -0.000044132971203721087,
+            "velocityX": 2.9678915829109185,
+            "velocityY": -2.3456676797926255,
+            "timestamp": 1.3054178092058317
+        },
+        {
+            "x": 4.410463806598655,
+            "y": 5.1569353484915235,
+            "heading": -0.594197297012115,
+            "angularVelocity": -0.00004413339287361589,
+            "velocityX": 2.9678915828785764,
+            "velocityY": -2.345667679665889,
+            "timestamp": 1.3573451324463626
+        },
+        {
+            "x": 4.564578472163962,
+            "y": 5.035131104681229,
+            "heading": -0.5941995887629704,
+            "angularVelocity": -0.000044133814578891406,
+            "velocityX": 2.9678915828462293,
+            "velocityY": -2.3456676795391433,
+            "timestamp": 1.4092724556868934
+        },
+        {
+            "x": 4.718693137727589,
+            "y": 4.913326860877516,
+            "heading": -0.5942018805357254,
+            "angularVelocity": -0.00004413423631330234,
+            "velocityX": 2.967891582813878,
+            "velocityY": -2.3456676794123914,
+            "timestamp": 1.4611997789274243
+        },
+        {
+            "x": 4.872807803289535,
+            "y": 4.791522617080385,
+            "heading": -0.5942041723303811,
+            "angularVelocity": -0.00004413465807134274,
+            "velocityX": 2.9678915827815215,
+            "velocityY": -2.3456676792856337,
+            "timestamp": 1.513127102167955
+        },
+        {
+            "x": 5.026922468849802,
+            "y": 4.669718373289836,
+            "heading": -0.5942064641469391,
+            "angularVelocity": -0.000044135079855748624,
+            "velocityX": 2.9678915827491625,
+            "velocityY": -2.3456676791588693,
+            "timestamp": 1.565054425408486
+        },
+        {
+            "x": 5.181037134408388,
+            "y": 4.5479141295058705,
+            "heading": -0.5942087559854001,
+            "angularVelocity": -0.000044135501659061016,
+            "velocityX": 2.967891582716799,
+            "velocityY": -2.345667679032102,
+            "timestamp": 1.6169817486490168
+        },
+        {
+            "x": 5.335151799963721,
+            "y": 4.4261098857264995,
+            "heading": -0.594211047845765,
+            "angularVelocity": -0.00004413592347400816,
+            "velocityX": 2.967891582654166,
+            "velocityY": -2.345667678943628,
+            "timestamp": 1.6689090718895476
         },
         {
             "x": 5.489266395568848,
             "y": 4.304305553436279,
             "heading": -0.5942133396987443,
-            "angularVelocity": -0.000017090531880803138,
-            "velocityX": 3.2895943364108793,
-            "velocityY": -1.8679319392755818,
-            "timestamp": 1.6722473940994562
-        },
-        {
-            "x": 5.681024172067428,
-            "y": 4.156179263559471,
-            "heading": -0.5942132880529166,
-            "angularVelocity": 8.063055616728172e-7,
-            "velocityX": 2.9937628707653796,
-            "velocityY": -2.312578894659331,
-            "timestamp": 1.7362998206864544
-        },
-        {
-            "x": 5.864967854996084,
-            "y": 3.998454299605836,
-            "heading": -0.5942132372714615,
-            "angularVelocity": 7.928107921043856e-7,
-            "velocityX": 2.871767593048443,
-            "velocityY": -2.4624354198261003,
-            "timestamp": 1.8003522472734526
-        },
-        {
-            "x": 6.0489114264021975,
-            "y": 3.840729205591288,
-            "heading": -0.5942131864900138,
-            "angularVelocity": 7.928106775338953e-7,
-            "velocityX": 2.871765851934987,
-            "velocityY": -2.4624374503645132,
-            "timestamp": 1.8644046738604507
-        },
-        {
-            "x": 6.232854997807022,
-            "y": 3.683004111575237,
-            "heading": -0.5942131357085663,
-            "angularVelocity": 7.928106743629292e-7,
-            "velocityX": 2.87176585191486,
-            "velocityY": -2.462437450387986,
-            "timestamp": 1.928457100447449
-        },
-        {
-            "x": 6.4167985692118465,
-            "y": 3.5252790175591864,
-            "heading": -0.594213084927119,
-            "angularVelocity": 7.928106702567426e-7,
-            "velocityX": 2.871765851914861,
-            "velocityY": -2.462437450387986,
-            "timestamp": 1.992509527034447
-        },
-        {
-            "x": 6.6007421406166715,
-            "y": 3.367553923543135,
-            "heading": -0.5942130341456721,
-            "angularVelocity": 7.928106666471698e-7,
-            "velocityX": 2.8717658519148612,
-            "velocityY": -2.462437450387985,
-            "timestamp": 2.0565619536214452
-        },
-        {
-            "x": 6.784685712022088,
-            "y": 3.2098288295277735,
-            "heading": -0.5942129833642253,
-            "angularVelocity": 7.928106614555171e-7,
-            "velocityX": 2.8717658519240943,
-            "velocityY": -2.4624374503772177,
-            "timestamp": 2.120614380208443
-        },
-        {
-            "x": 6.968629334581004,
-            "y": 3.052103795169223,
-            "heading": -0.594212932582592,
-            "angularVelocity": 7.928135744203515e-7,
-            "velocityX": 2.871766650543336,
-            "velocityY": -2.462436519002493,
-            "timestamp": 2.184666806795441
-        },
-        {
-            "x": 7.14554518368725,
-            "y": 2.9064126516878526,
-            "heading": -0.554455950225117,
-            "angularVelocity": 0.6206943979472176,
-            "velocityX": 2.7620475684235433,
-            "velocityY": -2.2745608752775093,
-            "timestamp": 2.248719233382439
-        },
-        {
-            "x": 7.299470766691596,
-            "y": 2.7775737815312107,
-            "heading": -0.43989859569345574,
-            "angularVelocity": 1.7884935924490803,
-            "velocityX": 2.4031186827134454,
-            "velocityY": -2.0114596280228136,
-            "timestamp": 2.312771659969437
-        },
-        {
-            "x": 7.4271138269599435,
-            "y": 2.670230084319628,
-            "heading": -0.3103960824325961,
-            "angularVelocity": 2.021820564205559,
-            "velocityX": 1.9927903917110543,
-            "velocityY": -1.6758724521667427,
-            "timestamp": 2.376824086556435
-        },
-        {
-            "x": 7.52865057013146,
-            "y": 2.5846878922538004,
-            "heading": -0.1939655985882249,
-            "angularVelocity": 1.8177372825404448,
-            "velocityX": 1.5852130603296692,
-            "velocityY": -1.3355027533522315,
-            "timestamp": 2.440876513143433
-        },
-        {
-            "x": 7.604480145165485,
-            "y": 2.520737422138354,
-            "heading": -0.10094853997619462,
-            "angularVelocity": 1.4522019471922978,
-            "velocityX": 1.1838673267285131,
-            "velocityY": -0.9984082340516305,
-            "timestamp": 2.504928939730431
-        },
-        {
-            "x": 7.654868393442648,
-            "y": 2.4782123742697575,
-            "heading": -0.03632146129528005,
-            "angularVelocity": 1.008971589751342,
-            "velocityX": 0.7866719648587251,
-            "velocityY": -0.6639100208145539,
-            "timestamp": 2.568981366317429
+            "angularVelocity": -0.000044135781248471857,
+            "velocityX": 2.9678902355751515,
+            "velocityY": -2.345669383457615,
+            "timestamp": 1.7208363951300785
+        },
+        {
+            "x": 5.673870655984677,
+            "y": 4.147383169820797,
+            "heading": -0.5942132891163008,
+            "angularVelocity": 7.897648233188991e-7,
+            "velocityX": 2.882303439061699,
+            "velocityY": -2.450094732060046,
+            "timestamp": 1.7848838711570736
+        },
+        {
+            "x": 5.858474845870276,
+            "y": 3.9904607032332415,
+            "heading": -0.5942132385338614,
+            "angularVelocity": 7.897647575237561e-7,
+            "velocityX": 2.882302337843733,
+            "velocityY": -2.450096027537681,
+            "timestamp": 1.8489313471840687
+        },
+        {
+            "x": 6.043079035755059,
+            "y": 3.833538236644727,
+            "heading": -0.5942131879514223,
+            "angularVelocity": 7.897647543646741e-7,
+            "velocityX": 2.882302337831004,
+            "velocityY": -2.4500960275526547,
+            "timestamp": 1.9129788232110638
+        },
+        {
+            "x": 6.227683225639842,
+            "y": 3.676615770056212,
+            "heading": -0.5942131373689834,
+            "angularVelocity": 7.897647493463809e-7,
+            "velocityX": 2.8823023378310055,
+            "velocityY": -2.450096027552654,
+            "timestamp": 1.977026299238059
+        },
+        {
+            "x": 6.412287415524625,
+            "y": 3.5196933034676974,
+            "heading": -0.5942130867865447,
+            "angularVelocity": 7.89764745190247e-7,
+            "velocityX": 2.882302337831007,
+            "velocityY": -2.4500960275526538,
+            "timestamp": 2.041073775265054
+        },
+        {
+            "x": 6.596891605409407,
+            "y": 3.362770836879183,
+            "heading": -0.5942130362041063,
+            "angularVelocity": 7.897647430611541e-7,
+            "velocityX": 2.882302337831007,
+            "velocityY": -2.450096027552653,
+            "timestamp": 2.105121251292049
+        },
+        {
+            "x": 6.781495795294782,
+            "y": 3.2058483702913625,
+            "heading": -0.5942129856216681,
+            "angularVelocity": 7.897647381910717e-7,
+            "velocityX": 2.8823023378402177,
+            "velocityY": -2.4500960275418184,
+            "timestamp": 2.169168727319044
+        },
+        {
+            "x": 6.966100036209046,
+            "y": 3.048925963734244,
+            "heading": -0.5942129350390435,
+            "angularVelocity": 7.897676514556789e-7,
+            "velocityX": 2.882303134575621,
+            "velocityY": -2.4500950902573897,
+            "timestamp": 2.233216203346039
+        },
+        {
+            "x": 7.143621774279278,
+            "y": 2.9040156366918444,
+            "heading": -0.5543160517170741,
+            "angularVelocity": 0.6229267068253146,
+            "velocityX": 2.771721058850299,
+            "velocityY": -2.262545474568311,
+            "timestamp": 2.2972636793730343
+        },
+        {
+            "x": 7.2980980894932665,
+            "y": 2.7758641788664877,
+            "heading": -0.43954991420980805,
+            "angularVelocity": 1.7918916501704738,
+            "velocityX": 2.411903244225882,
+            "velocityY": -2.000882248214473,
+            "timestamp": 2.3613111554000294
+        },
+        {
+            "x": 7.426195766165712,
+            "y": 2.6690947773000513,
+            "heading": -0.31008673137290993,
+            "angularVelocity": 2.0213627588124035,
+            "velocityX": 2.000042540606181,
+            "velocityY": -1.667035271170323,
+            "timestamp": 2.4253586314270246
+        },
+        {
+            "x": 7.528097771332223,
+            "y": 2.5840084569104262,
+            "heading": -0.19375204823220424,
+            "angularVelocity": 1.816382008428752,
+            "velocityX": 1.591038577750685,
+            "velocityY": -1.3284882663254731,
+            "timestamp": 2.4894061074540197
+        },
+        {
+            "x": 7.6042028509496316,
+            "y": 2.5203982409108687,
+            "heading": -0.10083265861354884,
+            "angularVelocity": 1.4507892485800875,
+            "velocityX": 1.1882604021012733,
+            "velocityY": -0.9931728765197068,
+            "timestamp": 2.553453583481015
+        },
+        {
+            "x": 7.654775762663328,
+            "y": 2.4780994279137802,
+            "heading": -0.036281315483046445,
+            "angularVelocity": 1.0078670875850742,
+            "velocityX": 0.7896159981757911,
+            "velocityY": -0.660429038284975,
+            "timestamp": 2.61750105950801
         },
         {
             "x": 7.68,
             "y": 2.457,
             "heading": -0.003,
-            "angularVelocity": 0.5202216851226045,
-            "velocityX": 0.39235994475896174,
-            "velocityY": -0.3311720632620588,
-            "timestamp": 2.6330337929044267
+            "angularVelocity": 0.5196350824038538,
+            "velocityX": 0.39383655533967327,
+            "velocityY": -0.32943418261927393,
+            "timestamp": 2.681548535535005
         },
         {
             "x": 7.68,
             "y": 2.457,
             "heading": -0.003,
-            "angularVelocity": -9.133242422473854e-34,
-            "velocityX": 1.8081868884139647e-34,
+            "angularVelocity": 1.9104375826405664e-34,
+            "velocityX": -1.672523979523505e-35,
             "velocityY": 0,
-            "timestamp": 2.6970862194914247
+            "timestamp": 2.745596011562
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.1.traj
new file mode 100644
index 00000000..00f6917b
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.1.traj	
@@ -0,0 +1,859 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": 1.04,
+            "angularVelocity": 0,
+            "velocityX": 1.8950109818726043e-31,
+            "velocityY": 4.417214679587952e-30,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7239179438589063,
+            "y": 6.714864648059082,
+            "heading": 0.9709045474564667,
+            "angularVelocity": -1.3401351995297424,
+            "velocityX": 0.269944341976283,
+            "velocityY": -0.09960229862677873,
+            "timestamp": 0.051558568544241005
+        },
+        {
+            "x": 0.7525251092531191,
+            "y": 6.703796484721446,
+            "heading": 0.8397844996535714,
+            "angularVelocity": -2.5431281648227726,
+            "velocityX": 0.5548479370541143,
+            "velocityY": -0.21467165691630782,
+            "timestamp": 0.10311713708848201
+        },
+        {
+            "x": 0.8002738106410594,
+            "y": 6.689991944618376,
+            "heading": 0.6713194026020708,
+            "angularVelocity": -3.2674510136339485,
+            "velocityX": 0.9261060331222868,
+            "velocityY": -0.26774482870254956,
+            "timestamp": 0.154675705632723
+        },
+        {
+            "x": 0.8672232338097706,
+            "y": 6.675504894458598,
+            "heading": 0.46260124525274376,
+            "angularVelocity": -4.048175952947979,
+            "velocityX": 1.2985120622830493,
+            "velocityY": -0.28098239669505554,
+            "timestamp": 0.20623427417696402
+        },
+        {
+            "x": 0.954736838142347,
+            "y": 6.658878661036629,
+            "heading": 0.2244001490575843,
+            "angularVelocity": -4.620009882366156,
+            "velocityX": 1.6973629564117712,
+            "velocityY": -0.3224727507251158,
+            "timestamp": 0.257792842721205
+        },
+        {
+            "x": 1.0641109683124677,
+            "y": 6.639652990231364,
+            "heading": -0.004541406089891365,
+            "angularVelocity": -4.440417211175481,
+            "velocityX": 2.12135699765032,
+            "velocityY": -0.37288992592389836,
+            "timestamp": 0.309351411265446
+        },
+        {
+            "x": 1.1925189459621883,
+            "y": 6.613890706063681,
+            "heading": -0.19800120779802596,
+            "angularVelocity": -3.752233763861444,
+            "velocityX": 2.4905264299488024,
+            "velocityY": -0.49967027586341123,
+            "timestamp": 0.36090997980968703
+        },
+        {
+            "x": 1.3374709433458876,
+            "y": 6.583331183982878,
+            "heading": -0.33396197230189334,
+            "angularVelocity": -2.6370158897471825,
+            "velocityX": 2.811404611812854,
+            "velocityY": -0.5927147115125508,
+            "timestamp": 0.41246854835392804
+        },
+        {
+            "x": 1.49983426972339,
+            "y": 6.549745693102723,
+            "heading": -0.41661329783619183,
+            "angularVelocity": -1.6030570255902088,
+            "velocityX": 3.149104619501603,
+            "velocityY": -0.6514046419107491,
+            "timestamp": 0.46402711689816906
+        },
+        {
+            "x": 1.6800135166896315,
+            "y": 6.515230594671984,
+            "heading": -0.44907632730235497,
+            "angularVelocity": -0.6296340333474829,
+            "velocityX": 3.4946518503843107,
+            "velocityY": -0.6694347691375233,
+            "timestamp": 0.51558568544241
+        },
+        {
+            "x": 1.8735253740457867,
+            "y": 6.490840556482913,
+            "heading": -0.4490767643443874,
+            "angularVelocity": -0.00000847661300213044,
+            "velocityX": 3.7532434049278374,
+            "velocityY": -0.47305499119601163,
+            "timestamp": 0.567144253986651
+        },
+        {
+            "x": 2.067041889759694,
+            "y": 6.46648750315131,
+            "heading": -0.4490772002167485,
+            "angularVelocity": -0.000008453926735380078,
+            "velocityX": 3.753333755723225,
+            "velocityY": -0.47233765442907966,
+            "timestamp": 0.618702822530892
+        },
+        {
+            "x": 2.2605372007770526,
+            "y": 6.441966542289934,
+            "heading": -0.4490776373561405,
+            "angularVelocity": -0.0000084785013311663,
+            "velocityX": 3.7529224817654048,
+            "velocityY": -0.4755942911914249,
+            "timestamp": 0.670261391075133
+        },
+        {
+            "x": 2.45117838314973,
+            "y": 6.400765025555903,
+            "heading": -0.4490787415251664,
+            "angularVelocity": -0.000021415820050526827,
+            "velocityX": 3.6975654630332984,
+            "velocityY": -0.7991206485676197,
+            "timestamp": 0.7218199596193741
+        },
+        {
+            "x": 2.6301938207404594,
+            "y": 6.341459154774922,
+            "heading": -0.46856582254307677,
+            "angularVelocity": -0.377960086337202,
+            "velocityX": 3.472079280806918,
+            "velocityY": -1.150262166999653,
+            "timestamp": 0.7733785281636151
+        },
+        {
+            "x": 2.797205924987793,
+            "y": 6.264003276824951,
+            "heading": -0.5033519686488042,
+            "angularVelocity": -0.6746918521648854,
+            "velocityX": 3.239269610517634,
+            "velocityY": -1.502289146830184,
+            "timestamp": 0.8249370967078561
+        },
+        {
+            "x": 2.911639196984497,
+            "y": 6.1983709493948735,
+            "heading": -0.5311684488221697,
+            "angularVelocity": -0.7502107513814861,
+            "velocityX": 3.0862665022221747,
+            "velocityY": -1.770104534065962,
+            "timestamp": 0.8620153178901536
+        },
+        {
+            "x": 3.0203603627243916,
+            "y": 6.122823189814021,
+            "heading": -0.5565632404510816,
+            "angularVelocity": -0.6848977868761134,
+            "velocityX": 2.9322109387469486,
+            "velocityY": -2.037523839376905,
+            "timestamp": 0.8990935390724512
+        },
+        {
+            "x": 3.1248710366711436,
+            "y": 6.036966714195615,
+            "heading": -0.5720315951438852,
+            "angularVelocity": -0.41718168240112047,
+            "velocityX": 2.8186539325316726,
+            "velocityY": -2.3155500150894683,
+            "timestamp": 0.9361717602547487
+        },
+        {
+            "x": 3.2269084939172057,
+            "y": 5.940732357266897,
+            "heading": -0.5720474132070273,
+            "angularVelocity": -0.00042661332192557474,
+            "velocityX": 2.7519512531259087,
+            "velocityY": -2.5954415789079244,
+            "timestamp": 0.9732499814370462
+        },
+        {
+            "x": 3.3218654576085616,
+            "y": 5.837498151619579,
+            "heading": -0.5720480482162372,
+            "angularVelocity": -0.000017126204806487387,
+            "velocityX": 2.560990270400005,
+            "velocityY": -2.7842275695894796,
+            "timestamp": 1.0103282026193439
+        },
+        {
+            "x": 3.4166998907386708,
+            "y": 5.734151371991349,
+            "heading": -0.5720486788754773,
+            "angularVelocity": -0.00001700888607859636,
+            "velocityX": 2.557685619889607,
+            "velocityY": -2.7872636910247786,
+            "timestamp": 1.0474064238016414
+        },
+        {
+            "x": 3.5114576080251196,
+            "y": 5.630734246765037,
+            "heading": -0.5720493072137337,
+            "angularVelocity": -0.000016946289124090365,
+            "velocityX": 2.555616592803881,
+            "velocityY": -2.7891609125222083,
+            "timestamp": 1.084484644983939
+        },
+        {
+            "x": 3.6061908144763444,
+            "y": 5.527294668181728,
+            "heading": -0.572049934817026,
+            "angularVelocity": -0.00001692646713700802,
+            "velocityX": 2.5549555353779705,
+            "velocityY": -2.7897664797435726,
+            "timestamp": 1.1215628661662365
+        },
+        {
+            "x": 3.7009275327159292,
+            "y": 5.423858305931887,
+            "heading": -0.5720505625260676,
+            "angularVelocity": -0.00001692931919251171,
+            "velocityX": 2.555050248355605,
+            "velocityY": -2.789679735188215,
+            "timestamp": 1.158641087348534
+        },
+        {
+            "x": 3.7956729593070784,
+            "y": 5.320429920428362,
+            "heading": -0.5720511904967662,
+            "angularVelocity": -0.000016936376088998586,
+            "velocityX": 2.5552851126613465,
+            "velocityY": -2.7894646022814182,
+            "timestamp": 1.1957193085308315
+        },
+        {
+            "x": 3.890467288385621,
+            "y": 5.217046353888283,
+            "heading": -0.5720518199780259,
+            "angularVelocity": -0.000016977115933005807,
+            "velocityX": 2.556604013243175,
+            "velocityY": -2.7882558343819337,
+            "timestamp": 1.232797529713129
+        },
+        {
+            "x": 3.9877865474692196,
+            "y": 5.11603605874111,
+            "heading": -0.5720525488313646,
+            "angularVelocity": -0.00001965718190938232,
+            "velocityX": 2.624701400996587,
+            "velocityY": -2.7242486809308257,
+            "timestamp": 1.2698757508954266
+        },
+        {
+            "x": 4.092995643615723,
+            "y": 5.023272514343262,
+            "heading": -0.5720542830492642,
+            "angularVelocity": -0.000046771874277348444,
+            "velocityX": 2.8374903863181586,
+            "velocityY": -2.5018337298792988,
+            "timestamp": 1.3069539720777241
+        },
+        {
+            "x": 4.14638765114505,
+            "y": 4.979871763007162,
+            "heading": -0.5720542261628284,
+            "angularVelocity": 0.000003127581707131923,
+            "velocityX": 2.935460161172297,
+            "velocityY": -2.386146961079825,
+            "timestamp": 1.3251426050680648
+        },
+        {
+            "x": 4.201473544713913,
+            "y": 4.938642209546033,
+            "heading": -0.5720541700948967,
+            "angularVelocity": 0.000003082580845535541,
+            "velocityX": 3.0285889873152927,
+            "velocityY": -2.266775819987239,
+            "timestamp": 1.3433312380584055
+        },
+        {
+            "x": 4.258151834683336,
+            "y": 4.899630631332575,
+            "heading": -0.5720541145494444,
+            "angularVelocity": 0.0000030538552492495714,
+            "velocityX": 3.11613797471883,
+            "velocityY": -2.1448328873407987,
+            "timestamp": 1.3615198710487462
+        },
+        {
+            "x": 4.316201884310526,
+            "y": 4.862691173522813,
+            "heading": -0.5720540592428014,
+            "angularVelocity": 0.000003040725654248182,
+            "velocityX": 3.1915564879460585,
+            "velocityY": -2.030908965471825,
+            "timestamp": 1.3797085040390868
+        },
+        {
+            "x": 4.374575644994192,
+            "y": 4.826265415180517,
+            "heading": -0.5720540039764063,
+            "angularVelocity": 0.0000030385128512512867,
+            "velocityX": 3.209353925315247,
+            "velocityY": -2.002666080603007,
+            "timestamp": 1.3978971370294275
+        },
+        {
+            "x": 4.432961770826435,
+            "y": 4.789859479973715,
+            "heading": -0.572053948711591,
+            "angularVelocity": 0.000003038425994779569,
+            "velocityX": 3.210033753678121,
+            "velocityY": -2.0015762166465794,
+            "timestamp": 1.4160857700197682
+        },
+        {
+            "x": 4.491358693973173,
+            "y": 4.753470866702408,
+            "heading": -0.5720538934471577,
+            "angularVelocity": 0.000003038404992596871,
+            "velocityX": 3.21062738347468,
+            "velocityY": -2.000623867148186,
+            "timestamp": 1.434274403010109
+        },
+        {
+            "x": 4.549763643159385,
+            "y": 4.7170951368878224,
+            "heading": -0.5720538381830028,
+            "angularVelocity": 0.0000030383896855688938,
+            "velocityX": 3.2110686502526127,
+            "velocityY": -1.9999155425186408,
+            "timestamp": 1.4524630360004496
+        },
+        {
+            "x": 4.608173712818011,
+            "y": 4.680727629815019,
+            "heading": -0.5720537829190162,
+            "angularVelocity": 0.0000030383804361409647,
+            "velocityX": 3.211350170717194,
+            "velocityY": -1.9994634611677902,
+            "timestamp": 1.4706516689907903
+        },
+        {
+            "x": 4.666585738503753,
+            "y": 4.644363264529719,
+            "heading": -0.5720537276550893,
+            "angularVelocity": 0.0000030383771462609746,
+            "velocityX": 3.211457711899863,
+            "velocityY": -1.999290727599226,
+            "timestamp": 1.488840301981131
+        },
+        {
+            "x": 4.724997877507934,
+            "y": 4.607999081268276,
+            "heading": -0.5720536723911652,
+            "angularVelocity": 0.0000030383769981998487,
+            "velocityX": 3.2114639420784044,
+            "velocityY": -1.9992807200388194,
+            "timestamp": 1.5070289349714716
+        },
+        {
+            "x": 4.783409732751982,
+            "y": 4.571634442201932,
+            "heading": -0.5720536171272324,
+            "angularVelocity": 0.0000030383774810111486,
+            "velocityX": 3.2114483411183334,
+            "velocityY": -1.9993057799158553,
+            "timestamp": 1.5252175679618123
+        },
+        {
+            "x": 4.841821302805823,
+            "y": 4.535269345043953,
+            "heading": -0.5720535618632904,
+            "angularVelocity": 0.0000030383779810703436,
+            "velocityX": 3.211432661533899,
+            "velocityY": -1.9993309655158582,
+            "timestamp": 1.543406200952153
+        },
+        {
+            "x": 4.900232750957554,
+            "y": 4.4989040520829935,
+            "heading": -0.5720535065993444,
+            "angularVelocity": 0.0000030383782054284467,
+            "velocityX": 3.2114259594301156,
+            "velocityY": -1.9993417306445722,
+            "timestamp": 1.5615948339424937
+        },
+        {
+            "x": 4.958643009851236,
+            "y": 4.46253684895675,
+            "heading": -0.5720534513353597,
+            "angularVelocity": 0.000003038380333213315,
+            "velocityX": 3.2113605747448064,
+            "velocityY": -1.999446750371714,
+            "timestamp": 1.5797834669328343
+        },
+        {
+            "x": 5.017052204377269,
+            "y": 4.426167936377253,
+            "heading": -0.5720533960713398,
+            "angularVelocity": 0.0000030383822663214653,
+            "velocityX": 3.21130205645789,
+            "velocityY": -1.999540735073477,
+            "timestamp": 1.597972099923175
+        },
+        {
+            "x": 5.075461342766336,
+            "y": 4.389798933639636,
+            "heading": -0.572053340807318,
+            "angularVelocity": 0.0000030383823681408014,
+            "velocityX": 3.211298970081695,
+            "velocityY": -1.999545691911999,
+            "timestamp": 1.6161607329135157
+        },
+        {
+            "x": 5.133870231273224,
+            "y": 4.35342952959225,
+            "heading": -0.5720532855432875,
+            "angularVelocity": 0.0000030383828455618877,
+            "velocityX": 3.211285231711017,
+            "velocityY": -1.9995677556796618,
+            "timestamp": 1.6343493659038564
+        },
+        {
+            "x": 5.192278843825858,
+            "y": 4.317059682370623,
+            "heading": -0.5720532302792477,
+            "angularVelocity": 0.0000030383833608448334,
+            "velocityX": 3.2112700599133515,
+            "velocityY": -1.9995921211310104,
+            "timestamp": 1.652537998894197
+        },
+        {
+            "x": 5.250688378518623,
+            "y": 4.28069131610433,
+            "heading": -0.5720531750152431,
+            "angularVelocity": 0.000003038381418986995,
+            "velocityX": 3.211320758617258,
+            "velocityY": -1.9995106991068379,
+            "timestamp": 1.6707266318845377
+        },
+        {
+            "x": 5.309100997862405,
+            "y": 4.2443279043993805,
+            "heading": -0.5720531197513381,
+            "angularVelocity": 0.0000030383759466885435,
+            "velocityX": 3.2114903508569643,
+            "velocityY": -1.9992383003329772,
+            "timestamp": 1.6889152648748784
+        },
+        {
+            "x": 5.367519658270811,
+            "y": 4.20797419858894,
+            "heading": -0.5720530644876303,
+            "angularVelocity": 0.0000030383651093388606,
+            "velocityX": 3.2118224849224113,
+            "velocityY": -1.998704676145307,
+            "timestamp": 1.707103897865219
+        },
+        {
+            "x": 5.425947098435591,
+            "y": 4.171634605233085,
+            "heading": -0.5720530092241862,
+            "angularVelocity": 0.000003038350601996349,
+            "velocityX": 3.212305190598148,
+            "velocityY": -1.9979287819592548,
+            "timestamp": 1.7252925308555598
+        },
+        {
+            "x": 5.484385608043213,
+            "y": 4.135312815587752,
+            "heading": -0.5720529539612775,
+            "angularVelocity": 0.0000030383211709796797,
+            "velocityX": 3.2129137818460802,
+            "velocityY": -1.9969499447620875,
+            "timestamp": 1.7434811638459005
+        },
+        {
+            "x": 5.542805855367704,
+            "y": 4.098961660035809,
+            "heading": -0.5720528986974772,
+            "angularVelocity": 0.0000030383701899944643,
+            "velocityX": 3.21190973260657,
+            "velocityY": -1.9985644644794618,
+            "timestamp": 1.7616697968362411
+        },
+        {
+            "x": 5.601240344083477,
+            "y": 4.062633401947461,
+            "heading": -0.5720528434300965,
+            "angularVelocity": 0.000003038567036318681,
+            "velocityX": 3.2126927156544984,
+            "velocityY": -1.9973055758301874,
+            "timestamp": 1.7798584298265818
+        },
+        {
+            "x": 5.6606145093793865,
+            "y": 4.027862145195579,
+            "heading": -0.5720527881030576,
+            "angularVelocity": 0.0000030418470110587657,
+            "velocityX": 3.2643555635790893,
+            "velocityY": -1.9117025875630587,
+            "timestamp": 1.7980470628169225
+        },
+        {
+            "x": 5.721296662408543,
+            "y": 3.9954274711883437,
+            "heading": -0.5720527324898503,
+            "angularVelocity": 0.0000030575803747562297,
+            "velocityX": 3.3362679351103024,
+            "velocityY": -1.7832386867362542,
+            "timestamp": 1.8162356958072632
+        },
+        {
+            "x": 5.7832255363464355,
+            "y": 3.9654417037963863,
+            "heading": -0.5720526754836338,
+            "angularVelocity": 0.0000031341671777608764,
+            "velocityX": 3.404811893823314,
+            "velocityY": -1.6485992876907705,
+            "timestamp": 1.8344243287976039
+        },
+        {
+            "x": 5.946280542451374,
+            "y": 3.905964298639421,
+            "heading": -0.5720526220643124,
+            "angularVelocity": 0.0000011643081031480314,
+            "velocityX": 3.5538876198319724,
+            "velocityY": -1.296347894456755,
+            "timestamp": 1.880305072239023
+        },
+        {
+            "x": 6.113507668800577,
+            "y": 3.8594929620139826,
+            "heading": -0.5720525704992387,
+            "angularVelocity": 0.0000011238935971124524,
+            "velocityX": 3.6448216355273746,
+            "velocityY": -1.0128723542630858,
+            "timestamp": 1.9261858156804423
+        },
+        {
+            "x": 6.280741389533688,
+            "y": 3.813045361829368,
+            "heading": -0.5720525189366377,
+            "angularVelocity": 0.000001123839702331554,
+            "velocityX": 3.64496536431679,
+            "velocityY": -1.0123550034458102,
+            "timestamp": 1.9720665591218616
+        },
+        {
+            "x": 6.445214711488264,
+            "y": 3.7576105066241974,
+            "heading": -0.5720524661781627,
+            "angularVelocity": 0.000001149904532146113,
+            "velocityX": 3.584800716329628,
+            "velocityY": -1.2082379457532038,
+            "timestamp": 2.0179473025632806
+        },
+        {
+            "x": 6.603250853740864,
+            "y": 3.685853457646542,
+            "heading": -0.5720524095450499,
+            "angularVelocity": 0.0000012343547357711312,
+            "velocityX": 3.444498288357164,
+            "velocityY": -1.563990545825532,
+            "timestamp": 2.0638280460046996
+        },
+        {
+            "x": 6.753231641982998,
+            "y": 3.5985021065376834,
+            "heading": -0.5720523460454168,
+            "angularVelocity": 0.0000013840149106858422,
+            "velocityX": 3.268926721589291,
+            "velocityY": -1.9038782843733606,
+            "timestamp": 2.1097087894461186
+        },
+        {
+            "x": 6.893623224143754,
+            "y": 3.4964496816334383,
+            "heading": -0.5720522715269948,
+            "angularVelocity": 0.000001624176426799149,
+            "velocityX": 3.059923872852999,
+            "velocityY": -2.2242975429267773,
+            "timestamp": 2.1555895328875376
+        },
+        {
+            "x": 7.022989830955657,
+            "y": 3.3807398195880776,
+            "heading": -0.5720521794208858,
+            "angularVelocity": 0.000002007511259312441,
+            "velocityX": 2.819627519265176,
+            "velocityY": -2.521970076471132,
+            "timestamp": 2.2014702763289566
+        },
+        {
+            "x": 7.140008449554443,
+            "y": 3.2525558471679688,
+            "heading": -0.5720520666435536,
+            "angularVelocity": 0.0000024580537218711263,
+            "velocityX": 2.550495258387596,
+            "velocityY": -2.7938512501200163,
+            "timestamp": 2.2473510197703757
+        },
+        {
+            "x": 7.220333202972963,
+            "y": 3.149893277120657,
+            "heading": -0.57205195105401,
+            "angularVelocity": 0.0000033545158672128766,
+            "velocityX": 2.3310989161613462,
+            "velocityY": -2.9793630927244275,
+            "timestamp": 2.28180891122307
+        },
+        {
+            "x": 7.292634698776439,
+            "y": 3.0414305376048008,
+            "heading": -0.5720518635503085,
+            "angularVelocity": 0.000002539438655350069,
+            "velocityX": 2.098256531535021,
+            "velocityY": -3.1476893954673923,
+            "timestamp": 2.3162668026757647
+        },
+        {
+            "x": 7.356495877093559,
+            "y": 2.9277932908128785,
+            "heading": -0.5720517925210026,
+            "angularVelocity": 0.000002061336399152925,
+            "velocityX": 1.853310682252877,
+            "velocityY": -3.297858400531866,
+            "timestamp": 2.350724694128459
+        },
+        {
+            "x": 7.411548371625277,
+            "y": 2.8096370468188216,
+            "heading": -0.5720517318127031,
+            "angularVelocity": 0.0000017618112132015592,
+            "velocityX": 1.5976745009881768,
+            "velocityY": -3.4290038946879324,
+            "timestamp": 2.3851825855811537
+        },
+        {
+            "x": 7.457474639290446,
+            "y": 2.6876433834723,
+            "heading": -0.5720516777421675,
+            "angularVelocity": 0.0000015691771403552433,
+            "velocityX": 1.332822924707698,
+            "velocityY": -3.540369366882859,
+            "timestamp": 2.419640477033848
+        },
+        {
+            "x": 7.49400980759948,
+            "y": 2.5625160164724585,
+            "heading": -0.5720516278694773,
+            "angularVelocity": 0.0000014473517720699968,
+            "velocityX": 1.0602845028769212,
+            "velocityY": -3.631312356172488,
+            "timestamp": 2.4540983684865427
+        },
+        {
+            "x": 7.520944004354476,
+            "y": 2.4349769014207707,
+            "heading": -0.572051580412575,
+            "angularVelocity": 0.0000013772433620936625,
+            "velocityX": 0.7816553950223419,
+            "velocityY": -3.7013035236575447,
+            "timestamp": 2.4885562599392372
+        },
+        {
+            "x": 7.53814696549347,
+            "y": 2.3057649240296647,
+            "heading": -0.5720515339265989,
+            "angularVelocity": 0.0000013490661810968306,
+            "velocityX": 0.4992459031544392,
+            "velocityY": -3.7498515418008185,
+            "timestamp": 2.5230141513919317
+        },
+        {
+            "x": 7.550702118784975,
+            "y": 2.1760188446169613,
+            "heading": -0.5720514875103294,
+            "angularVelocity": 0.0000013470432314212435,
+            "velocityX": 0.3643622044850161,
+            "velocityY": -3.7653516783177263,
+            "timestamp": 2.5574720428446263
+        },
+        {
+            "x": 7.56324456343349,
+            "y": 2.046271536053824,
+            "heading": -0.5720514410938347,
+            "angularVelocity": 0.000001347049770461967,
+            "velocityX": 0.36399338786688684,
+            "velocityY": -3.765387349404451,
+            "timestamp": 2.5919299342973208
+        },
+        {
+            "x": 7.57578727298306,
+            "y": 1.916524253098162,
+            "heading": -0.5720513946773481,
+            "angularVelocity": 0.0000013470495296572014,
+            "velocityX": 0.3640010755350196,
+            "velocityY": -3.7653866062523553,
+            "timestamp": 2.6263878257500153
+        },
+        {
+            "x": 7.588311679081848,
+            "y": 1.7867752020286953,
+            "heading": -0.572051348260254,
+            "angularVelocity": 0.0000013470671654480904,
+            "velocityX": 0.3634698923895803,
+            "velocityY": -3.765437918557532,
+            "timestamp": 2.66084571720271
+        },
+        {
+            "x": 7.600787981664821,
+            "y": 1.657021516812979,
+            "heading": -0.5720513014102728,
+            "angularVelocity": 0.0000013596299477389166,
+            "velocityX": 0.3620738837176714,
+            "velocityY": -3.7655724057824624,
+            "timestamp": 2.6953036086554043
+        },
+        {
+            "x": 7.6103162713524775,
+            "y": 1.534453918276911,
+            "heading": -0.5536995765422551,
+            "angularVelocity": 0.5325840930573226,
+            "velocityX": 0.27651981261615693,
+            "velocityY": -3.557025498914872,
+            "timestamp": 2.729761500108099
+        },
+        {
+            "x": 7.620120451226168,
+            "y": 1.4200989858077522,
+            "heading": -0.5149948955625814,
+            "angularVelocity": 1.1232457747100673,
+            "velocityX": 0.2845264019455608,
+            "velocityY": -3.3186863051722235,
+            "timestamp": 2.7642193915607933
+        },
+        {
+            "x": 7.630164619508576,
+            "y": 1.3139569074919104,
+            "heading": -0.4555658179030021,
+            "angularVelocity": 1.7246870064917483,
+            "velocityX": 0.29149108836702503,
+            "velocityY": -3.080341652987332,
+            "timestamp": 2.798677283013488
+        },
+        {
+            "x": 7.6393124908526255,
+            "y": 1.2174783858270968,
+            "heading": -0.38913465703239974,
+            "angularVelocity": 1.9278939618746356,
+            "velocityX": 0.26547971911102464,
+            "velocityY": -2.799896267514583,
+            "timestamp": 2.8331351744661823
+        },
+        {
+            "x": 7.647521237661079,
+            "y": 1.1308545482085492,
+            "heading": -0.3222243620613333,
+            "angularVelocity": 1.9417988782892024,
+            "velocityX": 0.23822545322294675,
+            "velocityY": -2.5139041875930466,
+            "timestamp": 2.867593065918877
+        },
+        {
+            "x": 7.654790830767336,
+            "y": 1.0540553472510912,
+            "heading": -0.25810310965019956,
+            "angularVelocity": 1.8608582739068054,
+            "velocityX": 0.21097034089302885,
+            "velocityY": -2.2287841106847384,
+            "timestamp": 2.9020509573715714
+        },
+        {
+            "x": 7.661128119511832,
+            "y": 0.9870214726082436,
+            "heading": -0.19877373026472553,
+            "angularVelocity": 1.7217936700197296,
+            "velocityX": 0.18391400278139633,
+            "velocityY": -1.9453852750938303,
+            "timestamp": 2.936508848824266
+        },
+        {
+            "x": 7.6665422514065025,
+            "y": 0.9296939844226774,
+            "heading": -0.14563708266620207,
+            "angularVelocity": 1.5420748443496368,
+            "velocityX": 0.15712313395848782,
+            "velocityY": -1.6636969288812407,
+            "timestamp": 2.9709667402769604
+        },
+        {
+            "x": 7.6710416569080175,
+            "y": 0.8820195033603357,
+            "heading": -0.09974947152576126,
+            "angularVelocity": 1.331701076470322,
+            "velocityX": 0.1305769248148914,
+            "velocityY": -1.3835577004993256,
+            "timestamp": 3.005424631729655
+        },
+        {
+            "x": 7.674632846509551,
+            "y": 0.8439509935506372,
+            "heading": -0.06193916709248371,
+            "angularVelocity": 1.0972901370124863,
+            "velocityX": 0.10421965622732904,
+            "velocityY": -1.1047834967488865,
+            "timestamp": 3.0398825231823494
+        },
+        {
+            "x": 7.677320522590369,
+            "y": 0.8154475641909131,
+            "heading": -0.032869067302209315,
+            "angularVelocity": 0.8436412840348516,
+            "velocityX": 0.07799885505174348,
+            "velocityY": -0.8271959820541263,
+            "timestamp": 3.074340414635044
+        },
+        {
+            "x": 7.6791083601324726,
+            "y": 0.7964739655020694,
+            "heading": -0.013076341373692615,
+            "angularVelocity": 0.5744032816313593,
+            "velocityX": 0.05188470526556513,
+            "velocityY": -0.5506314486738934,
+            "timestamp": 3.1087983060877384
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -0.003,
+            "angularVelocity": 0.2924247813459689,
+            "velocityX": 0.02587621673680596,
+            "velocityY": -0.2749432743171821,
+            "timestamp": 3.143256197540433
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -0.003,
+            "angularVelocity": -5.23227119774542e-33,
+            "velocityX": 0,
+            "velocityY": -1.249481443135334e-33,
+            "timestamp": 3.1777140889931275
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.traj
index 6231149d..00f6917b 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.traj	
@@ -5,801 +5,855 @@
             "y": 6.72,
             "heading": 1.04,
             "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
+            "velocityX": 1.8950109818726043e-31,
+            "velocityY": 4.417214679587952e-30,
             "timestamp": 0
         },
         {
-            "x": 0.7237404107060486,
-            "y": 6.712071337633356,
-            "heading": 0.9558489426883627,
-            "angularVelocity": -1.537881157714612,
-            "velocityX": 0.25110936688338653,
-            "velocityY": -0.14489824428928566,
-            "timestamp": 0.05471882979351295
-        },
-        {
-            "x": 0.7514918072858734,
-            "y": 6.694799584586779,
-            "heading": 0.7935491608961526,
-            "angularVelocity": -2.966068214628546,
-            "velocityX": 0.5071635611460984,
-            "velocityY": -0.31564551200661234,
-            "timestamp": 0.1094376595870259
-        },
-        {
-            "x": 0.7959632333479747,
-            "y": 6.666655428800684,
-            "heading": 0.574279005326375,
-            "angularVelocity": -4.007215731718184,
-            "velocityX": 0.8127261900504715,
-            "velocityY": -0.514341331718894,
-            "timestamp": 0.16415648938053887
-        },
-        {
-            "x": 0.8617224500002636,
-            "y": 6.63045184710829,
-            "heading": 0.31905346805964874,
-            "angularVelocity": -4.664309127769047,
-            "velocityX": 1.2017657705846174,
-            "velocityY": -0.6616293116832328,
-            "timestamp": 0.2188753191740518
-        },
-        {
-            "x": 0.9507709996198539,
-            "y": 6.585947725507511,
-            "heading": 0.05142997722620149,
-            "angularVelocity": -4.890884762034389,
-            "velocityX": 1.627384027685255,
-            "velocityY": -0.8133237090178916,
-            "timestamp": 0.2735941489675648
-        },
-        {
-            "x": 1.0614190010918925,
-            "y": 6.535018660742887,
-            "heading": -0.17495854287380355,
-            "angularVelocity": -4.137305584828904,
-            "velocityX": 2.0221192940269503,
-            "velocityY": -0.9307411170306262,
-            "timestamp": 0.32831297876107773
-        },
-        {
-            "x": 1.1921384640832198,
-            "y": 6.474205616338843,
-            "heading": -0.35862575739678126,
-            "angularVelocity": -3.356563274764148,
-            "velocityX": 2.388930163247485,
-            "velocityY": -1.111373262796896,
-            "timestamp": 0.3830318085545907
-        },
-        {
-            "x": 1.3384031857193457,
-            "y": 6.398894346098027,
-            "heading": -0.49152044440666937,
-            "angularVelocity": -2.4286829142980233,
-            "velocityX": 2.6730235677201417,
-            "velocityY": -1.376331886573771,
-            "timestamp": 0.4377506383481036
-        },
-        {
-            "x": 1.500957547301863,
-            "y": 6.312259668262383,
-            "heading": -0.5624501430724521,
-            "angularVelocity": -1.2962575942036991,
-            "velocityX": 2.9707207225727275,
-            "velocityY": -1.5832699303433067,
-            "timestamp": 0.4924694681416166
-        },
-        {
-            "x": 1.6817069101290123,
-            "y": 6.217739107445247,
-            "heading": -0.5720464000476684,
-            "angularVelocity": -0.17537394369443057,
-            "velocityX": 3.3032388212471107,
-            "velocityY": -1.7273863709040396,
-            "timestamp": 0.5471882979351296
-        },
-        {
-            "x": 1.8673643551319996,
-            "y": 6.126199810501224,
-            "heading": -0.5720469984173726,
-            "angularVelocity": -0.000010935352720012522,
-            "velocityX": 3.392935223643913,
-            "velocityY": -1.6729030443351731,
-            "timestamp": 0.6019071277286425
-        },
-        {
-            "x": 2.0530162492759167,
-            "y": 6.034649256644459,
-            "heading": -0.5720475972653951,
-            "angularVelocity": -0.000010944094102204816,
-            "velocityX": 3.3928337803365705,
-            "velocityY": -1.6731087671691436,
-            "timestamp": 0.6566259575221555
-        },
-        {
-            "x": 2.23866813267496,
-            "y": 5.943098680998449,
-            "heading": -0.5720481961130315,
-            "angularVelocity": -0.000010944087046546937,
-            "velocityX": 3.3928335839713664,
-            "velocityY": -1.673109165372953,
-            "timestamp": 0.7113447873156684
-        },
-        {
-            "x": 2.4243200161477754,
-            "y": 5.851548105501653,
-            "heading": -0.5720487949598734,
-            "angularVelocity": -0.000010944072528266654,
-            "velocityX": 3.392833585319575,
-            "velocityY": -1.673109162646019,
-            "timestamp": 0.7660636171091814
-        },
-        {
-            "x": 2.6099718996232633,
-            "y": 5.759997530009892,
-            "heading": -0.5720493938059237,
-            "angularVelocity": -0.000010944058060277418,
-            "velocityX": 3.3928335853684146,
-            "velocityY": -1.673109162553994,
-            "timestamp": 0.8207824469026943
-        },
-        {
-            "x": 2.7956237830994004,
-            "y": 5.668446954519065,
-            "heading": -0.5720499926511823,
-            "angularVelocity": -0.000010944043595406558,
-            "velocityX": 3.3928335853802802,
-            "velocityY": -1.6731091625369476,
-            "timestamp": 0.8755012766962073
-        },
-        {
-            "x": 2.9812756665756717,
-            "y": 5.576896379028125,
-            "heading": -0.5720505914956495,
-            "angularVelocity": -0.000010944029129446314,
-            "velocityX": 3.3928335853827263,
-            "velocityY": -1.6731091625390042,
-            "timestamp": 0.9302201064897202
-        },
-        {
-            "x": 3.166927550036217,
-            "y": 5.485345803504912,
-            "heading": -0.5720511903393257,
-            "angularVelocity": -0.000010944014673786385,
-            "velocityX": 3.392833585095338,
-            "velocityY": -1.673109163128796,
-            "timestamp": 0.9849389362832331
-        },
-        {
-            "x": 3.3525794334560626,
-            "y": 5.393795227898782,
-            "heading": -0.5720517891822119,
-            "angularVelocity": -0.000010944000236842356,
-            "velocityX": 3.392833584351539,
-            "velocityY": -1.6731091646441256,
-            "timestamp": 1.039657766076746
-        },
-        {
-            "x": 3.538231313226632,
-            "y": 5.302244644892125,
-            "heading": -0.5720523880244444,
-            "angularVelocity": -0.000010943988288632917,
-            "velocityX": 3.392833517660122,
-            "velocityY": -1.673109299890567,
-            "timestamp": 1.0943765958702592
-        },
-        {
-            "x": 3.723883292814953,
-            "y": 5.21069426429933,
-            "heading": -0.5720529868621156,
-            "angularVelocity": -0.000010943904932797782,
-            "velocityX": 3.3928353418539645,
-            "velocityY": -1.673105600727685,
-            "timestamp": 1.1490954256637722
-        },
-        {
-            "x": 3.909492929164672,
-            "y": 5.1190580682844296,
-            "heading": -0.5720535873273735,
-            "angularVelocity": -0.000010973649477031932,
-            "velocityX": 3.3920615088102064,
-            "velocityY": -1.6746738985594591,
-            "timestamp": 1.2038142554572853
+            "x": 0.7239179438589063,
+            "y": 6.714864648059082,
+            "heading": 0.9709045474564667,
+            "angularVelocity": -1.3401351995297424,
+            "velocityX": 0.269944341976283,
+            "velocityY": -0.09960229862677873,
+            "timestamp": 0.051558568544241005
+        },
+        {
+            "x": 0.7525251092531191,
+            "y": 6.703796484721446,
+            "heading": 0.8397844996535714,
+            "angularVelocity": -2.5431281648227726,
+            "velocityX": 0.5548479370541143,
+            "velocityY": -0.21467165691630782,
+            "timestamp": 0.10311713708848201
+        },
+        {
+            "x": 0.8002738106410594,
+            "y": 6.689991944618376,
+            "heading": 0.6713194026020708,
+            "angularVelocity": -3.2674510136339485,
+            "velocityX": 0.9261060331222868,
+            "velocityY": -0.26774482870254956,
+            "timestamp": 0.154675705632723
+        },
+        {
+            "x": 0.8672232338097706,
+            "y": 6.675504894458598,
+            "heading": 0.46260124525274376,
+            "angularVelocity": -4.048175952947979,
+            "velocityX": 1.2985120622830493,
+            "velocityY": -0.28098239669505554,
+            "timestamp": 0.20623427417696402
+        },
+        {
+            "x": 0.954736838142347,
+            "y": 6.658878661036629,
+            "heading": 0.2244001490575843,
+            "angularVelocity": -4.620009882366156,
+            "velocityX": 1.6973629564117712,
+            "velocityY": -0.3224727507251158,
+            "timestamp": 0.257792842721205
+        },
+        {
+            "x": 1.0641109683124677,
+            "y": 6.639652990231364,
+            "heading": -0.004541406089891365,
+            "angularVelocity": -4.440417211175481,
+            "velocityX": 2.12135699765032,
+            "velocityY": -0.37288992592389836,
+            "timestamp": 0.309351411265446
+        },
+        {
+            "x": 1.1925189459621883,
+            "y": 6.613890706063681,
+            "heading": -0.19800120779802596,
+            "angularVelocity": -3.752233763861444,
+            "velocityX": 2.4905264299488024,
+            "velocityY": -0.49967027586341123,
+            "timestamp": 0.36090997980968703
+        },
+        {
+            "x": 1.3374709433458876,
+            "y": 6.583331183982878,
+            "heading": -0.33396197230189334,
+            "angularVelocity": -2.6370158897471825,
+            "velocityX": 2.811404611812854,
+            "velocityY": -0.5927147115125508,
+            "timestamp": 0.41246854835392804
+        },
+        {
+            "x": 1.49983426972339,
+            "y": 6.549745693102723,
+            "heading": -0.41661329783619183,
+            "angularVelocity": -1.6030570255902088,
+            "velocityX": 3.149104619501603,
+            "velocityY": -0.6514046419107491,
+            "timestamp": 0.46402711689816906
+        },
+        {
+            "x": 1.6800135166896315,
+            "y": 6.515230594671984,
+            "heading": -0.44907632730235497,
+            "angularVelocity": -0.6296340333474829,
+            "velocityX": 3.4946518503843107,
+            "velocityY": -0.6694347691375233,
+            "timestamp": 0.51558568544241
+        },
+        {
+            "x": 1.8735253740457867,
+            "y": 6.490840556482913,
+            "heading": -0.4490767643443874,
+            "angularVelocity": -0.00000847661300213044,
+            "velocityX": 3.7532434049278374,
+            "velocityY": -0.47305499119601163,
+            "timestamp": 0.567144253986651
+        },
+        {
+            "x": 2.067041889759694,
+            "y": 6.46648750315131,
+            "heading": -0.4490772002167485,
+            "angularVelocity": -0.000008453926735380078,
+            "velocityX": 3.753333755723225,
+            "velocityY": -0.47233765442907966,
+            "timestamp": 0.618702822530892
+        },
+        {
+            "x": 2.2605372007770526,
+            "y": 6.441966542289934,
+            "heading": -0.4490776373561405,
+            "angularVelocity": -0.0000084785013311663,
+            "velocityX": 3.7529224817654048,
+            "velocityY": -0.4755942911914249,
+            "timestamp": 0.670261391075133
+        },
+        {
+            "x": 2.45117838314973,
+            "y": 6.400765025555903,
+            "heading": -0.4490787415251664,
+            "angularVelocity": -0.000021415820050526827,
+            "velocityX": 3.6975654630332984,
+            "velocityY": -0.7991206485676197,
+            "timestamp": 0.7218199596193741
+        },
+        {
+            "x": 2.6301938207404594,
+            "y": 6.341459154774922,
+            "heading": -0.46856582254307677,
+            "angularVelocity": -0.377960086337202,
+            "velocityX": 3.472079280806918,
+            "velocityY": -1.150262166999653,
+            "timestamp": 0.7733785281636151
+        },
+        {
+            "x": 2.797205924987793,
+            "y": 6.264003276824951,
+            "heading": -0.5033519686488042,
+            "angularVelocity": -0.6746918521648854,
+            "velocityX": 3.239269610517634,
+            "velocityY": -1.502289146830184,
+            "timestamp": 0.8249370967078561
+        },
+        {
+            "x": 2.911639196984497,
+            "y": 6.1983709493948735,
+            "heading": -0.5311684488221697,
+            "angularVelocity": -0.7502107513814861,
+            "velocityX": 3.0862665022221747,
+            "velocityY": -1.770104534065962,
+            "timestamp": 0.8620153178901536
+        },
+        {
+            "x": 3.0203603627243916,
+            "y": 6.122823189814021,
+            "heading": -0.5565632404510816,
+            "angularVelocity": -0.6848977868761134,
+            "velocityX": 2.9322109387469486,
+            "velocityY": -2.037523839376905,
+            "timestamp": 0.8990935390724512
+        },
+        {
+            "x": 3.1248710366711436,
+            "y": 6.036966714195615,
+            "heading": -0.5720315951438852,
+            "angularVelocity": -0.41718168240112047,
+            "velocityX": 2.8186539325316726,
+            "velocityY": -2.3155500150894683,
+            "timestamp": 0.9361717602547487
+        },
+        {
+            "x": 3.2269084939172057,
+            "y": 5.940732357266897,
+            "heading": -0.5720474132070273,
+            "angularVelocity": -0.00042661332192557474,
+            "velocityX": 2.7519512531259087,
+            "velocityY": -2.5954415789079244,
+            "timestamp": 0.9732499814370462
+        },
+        {
+            "x": 3.3218654576085616,
+            "y": 5.837498151619579,
+            "heading": -0.5720480482162372,
+            "angularVelocity": -0.000017126204806487387,
+            "velocityX": 2.560990270400005,
+            "velocityY": -2.7842275695894796,
+            "timestamp": 1.0103282026193439
+        },
+        {
+            "x": 3.4166998907386708,
+            "y": 5.734151371991349,
+            "heading": -0.5720486788754773,
+            "angularVelocity": -0.00001700888607859636,
+            "velocityX": 2.557685619889607,
+            "velocityY": -2.7872636910247786,
+            "timestamp": 1.0474064238016414
+        },
+        {
+            "x": 3.5114576080251196,
+            "y": 5.630734246765037,
+            "heading": -0.5720493072137337,
+            "angularVelocity": -0.000016946289124090365,
+            "velocityX": 2.555616592803881,
+            "velocityY": -2.7891609125222083,
+            "timestamp": 1.084484644983939
+        },
+        {
+            "x": 3.6061908144763444,
+            "y": 5.527294668181728,
+            "heading": -0.572049934817026,
+            "angularVelocity": -0.00001692646713700802,
+            "velocityX": 2.5549555353779705,
+            "velocityY": -2.7897664797435726,
+            "timestamp": 1.1215628661662365
+        },
+        {
+            "x": 3.7009275327159292,
+            "y": 5.423858305931887,
+            "heading": -0.5720505625260676,
+            "angularVelocity": -0.00001692931919251171,
+            "velocityX": 2.555050248355605,
+            "velocityY": -2.789679735188215,
+            "timestamp": 1.158641087348534
+        },
+        {
+            "x": 3.7956729593070784,
+            "y": 5.320429920428362,
+            "heading": -0.5720511904967662,
+            "angularVelocity": -0.000016936376088998586,
+            "velocityX": 2.5552851126613465,
+            "velocityY": -2.7894646022814182,
+            "timestamp": 1.1957193085308315
+        },
+        {
+            "x": 3.890467288385621,
+            "y": 5.217046353888283,
+            "heading": -0.5720518199780259,
+            "angularVelocity": -0.000016977115933005807,
+            "velocityX": 2.556604013243175,
+            "velocityY": -2.7882558343819337,
+            "timestamp": 1.232797529713129
+        },
+        {
+            "x": 3.9877865474692196,
+            "y": 5.11603605874111,
+            "heading": -0.5720525488313646,
+            "angularVelocity": -0.00001965718190938232,
+            "velocityX": 2.624701400996587,
+            "velocityY": -2.7242486809308257,
+            "timestamp": 1.2698757508954266
         },
         {
             "x": 4.092995643615723,
             "y": 5.023272514343262,
             "heading": -0.5720542830492642,
-            "angularVelocity": -0.000012714487741704976,
-            "velocityX": 3.353556995709023,
-            "velocityY": -1.7505044296932655,
-            "timestamp": 1.2585330852507983
-        },
-        {
-            "x": 4.152699494738863,
-            "y": 4.989124335806784,
-            "heading": -0.5720542274621303,
-            "angularVelocity": 0.0000030573365885939726,
-            "velocityX": 3.283759307267904,
-            "velocityY": -1.8781769850015708,
-            "timestamp": 1.2767146404552234
-        },
-        {
-            "x": 4.211236123155136,
-            "y": 4.953011857528808,
-            "heading": -0.5720541720789936,
-            "angularVelocity": 0.000003046116576982166,
-            "velocityX": 3.2195611298433677,
-            "velocityY": -1.9862150334199096,
-            "timestamp": 1.2948961956596485
-        },
-        {
-            "x": 4.269217408443118,
-            "y": 4.916014310130808,
-            "heading": -0.5720541167402081,
-            "angularVelocity": 0.000003043677217870583,
-            "velocityX": 3.1890168160017764,
-            "velocityY": -2.034894539109796,
-            "timestamp": 1.3130777508640736
-        },
-        {
-            "x": 4.327172181408397,
-            "y": 4.8789752463128,
-            "heading": -0.5720540614037476,
-            "angularVelocity": 0.000003043549349895456,
-            "velocityX": 3.187558617163689,
-            "velocityY": -2.0371779752347874,
-            "timestamp": 1.3312593060684987
-        },
-        {
-            "x": 4.385108415981365,
-            "y": 4.841907191568928,
-            "heading": -0.5720540060672449,
-            "angularVelocity": 0.0000030435516711711228,
-            "velocityX": 3.186538990839725,
-            "velocityY": -2.0387724992190503,
-            "timestamp": 1.3494408612729238
-        },
-        {
-            "x": 4.443034277698237,
-            "y": 4.804822929374826,
-            "heading": -0.5720539507307192,
-            "angularVelocity": 0.000003043552934036031,
-            "velocityX": 3.1859684755020403,
-            "velocityY": -2.0396639218758956,
-            "timestamp": 1.367622416477349
-        },
-        {
-            "x": 4.500957639641057,
-            "y": 4.767734762799275,
-            "heading": -0.5720538953941279,
-            "angularVelocity": 0.0000030435565398491907,
-            "velocityX": 3.1858309859372986,
-            "velocityY": -2.039878665966735,
-            "timestamp": 1.385803971681774
-        },
-        {
-            "x": 4.558883202891902,
-            "y": 4.7306500344023386,
-            "heading": -0.5720538400575734,
-            "angularVelocity": 0.000003043554516316428,
-            "velocityX": 3.1859520596302042,
-            "velocityY": -2.03968956340728,
-            "timestamp": 1.4039855268861992
-        },
-        {
-            "x": 4.616809360440233,
-            "y": 4.693566234312182,
-            "heading": -0.572053784721022,
-            "angularVelocity": 0.000003043554346667464,
-            "velocityX": 3.1859847464654854,
-            "velocityY": -2.039638505794633,
-            "timestamp": 1.4221670820906243
-        },
-        {
-            "x": 4.674735447300298,
-            "y": 4.656482323806888,
-            "heading": -0.5720537293844703,
-            "angularVelocity": 0.00000304355436453364,
-            "velocityX": 3.185980858554995,
-            "velocityY": -2.039644578714451,
-            "timestamp": 1.4403486372950494
-        },
-        {
-            "x": 4.732661331177625,
-            "y": 4.619398096238942,
-            "heading": -0.5720536740479176,
-            "angularVelocity": 0.0000030435544255368,
-            "velocityX": 3.1859696943429383,
-            "velocityY": -2.0396620174125277,
-            "timestamp": 1.4585301924994745
-        },
-        {
-            "x": 4.790587117727679,
-            "y": 4.582313716645222,
-            "heading": -0.5720536187113643,
-            "angularVelocity": 0.0000030435544545803464,
-            "velocityX": 3.1859643412654055,
-            "velocityY": -2.039670378951063,
-            "timestamp": 1.4767117477038996
-        },
-        {
-            "x": 4.848512993160408,
-            "y": 4.5452294758867104,
-            "heading": -0.5720535633748114,
-            "angularVelocity": 0.0000030435544295946876,
-            "velocityX": 3.1859692298833355,
-            "velocityY": -2.0396627429041696,
-            "timestamp": 1.4948933029083247
-        },
-        {
-            "x": 4.906438819485911,
-            "y": 4.508145158422396,
-            "heading": -0.5720535080382583,
-            "angularVelocity": 0.0000030435544442083865,
-            "velocityX": 3.1859665289468815,
-            "velocityY": -2.039666961784329,
-            "timestamp": 1.5130748581127498
-        },
-        {
-            "x": 4.9643646433421065,
-            "y": 4.471060837101057,
-            "heading": -0.5720534527017052,
-            "angularVelocity": 0.0000030435544458805193,
-            "velocityX": 3.185966393132936,
-            "velocityY": -2.039667173923745,
-            "timestamp": 1.5312564133171749
-        },
-        {
-            "x": 5.022290480818567,
-            "y": 4.433976537054754,
-            "heading": -0.572053397365152,
-            "angularVelocity": 0.0000030435544415454932,
-            "velocityX": 3.185967142258403,
-            "velocityY": -2.039666003779759,
-            "timestamp": 1.5494379685216
-        },
-        {
-            "x": 5.080216347467466,
-            "y": 4.396892282576102,
-            "heading": -0.572053342028599,
-            "angularVelocity": 0.000003043554433808661,
-            "velocityX": 3.1859687467658464,
-            "velocityY": -2.039663497522643,
-            "timestamp": 1.567619523726025
-        },
-        {
-            "x": 5.138142382220086,
-            "y": 4.359808290677983,
-            "heading": -0.5720532866920471,
-            "angularVelocity": 0.000003043554377469469,
-            "velocityX": 3.185977992604117,
-            "velocityY": -2.039649055384386,
-            "timestamp": 1.5858010789304502
-        },
-        {
-            "x": 5.196068138494407,
-            "y": 4.322723863792993,
-            "heading": -0.5720532313554916,
-            "angularVelocity": 0.000003043554576021048,
-            "velocityX": 3.1859626760759787,
-            "velocityY": -2.0396729800088242,
-            "timestamp": 1.6039826341348753
-        },
-        {
-            "x": 5.253993690845546,
-            "y": 4.285639118381674,
-            "heading": -0.5720531760189423,
-            "angularVelocity": 0.0000030435542278177066,
-            "velocityX": 3.1859514601384813,
-            "velocityY": -2.0396904992104243,
-            "timestamp": 1.6221641893393004
-        },
-        {
-            "x": 5.3119198680466875,
-            "y": 4.248555348991913,
-            "heading": -0.5720531206823879,
-            "angularVelocity": 0.000003043554518420827,
-            "velocityX": 3.1859858273860655,
-            "velocityY": -2.0396368172479113,
-            "timestamp": 1.6403457445437255
-        },
-        {
-            "x": 5.369845451022558,
-            "y": 4.211470651416335,
-            "heading": -0.5720530653458327,
-            "angularVelocity": 0.0000030435545594837867,
-            "velocityX": 3.1859531445232334,
-            "velocityY": -2.039687868206526,
-            "timestamp": 1.6585272997481506
-        },
-        {
-            "x": 5.427769955136907,
-            "y": 4.17438426873464,
-            "heading": -0.5720530100092767,
-            "angularVelocity": 0.0000030435545998300077,
-            "velocityX": 3.185893806281465,
-            "velocityY": -2.039780550383344,
-            "timestamp": 1.6767088549525757
-        },
-        {
-            "x": 5.485698211873678,
-            "y": 4.137303747848898,
-            "heading": -0.5720529546727399,
-            "angularVelocity": 0.000003043553546723704,
-            "velocityX": 3.1861002035013715,
-            "velocityY": -2.0394581469404574,
-            "timestamp": 1.6948904101570008
-        },
-        {
-            "x": 5.5436372749681775,
-            "y": 4.100240114388813,
-            "heading": -0.5720528993360909,
-            "angularVelocity": 0.0000030435597092212927,
-            "velocityX": 3.186694561772871,
-            "velocityY": -2.0385293250953618,
-            "timestamp": 1.713071965361426
-        },
-        {
-            "x": 5.601833960036281,
-            "y": 4.063582321171135,
-            "heading": -0.5720528439855975,
-            "angularVelocity": 0.0000030443211669066955,
-            "velocityX": 3.2008639752640304,
-            "velocityY": -2.0162077889097265,
-            "timestamp": 1.731253520565851
-        },
-        {
-            "x": 5.661045954546234,
-            "y": 4.0285882123488195,
-            "heading": -0.5720527885077281,
-            "angularVelocity": 0.000003051326955110969,
-            "velocityX": 3.2567068022627845,
-            "velocityY": -1.9247038236737246,
-            "timestamp": 1.7494350757702761
-        },
-        {
-            "x": 5.721517200862813,
-            "y": 3.9958181607132874,
-            "heading": -0.5720527326566293,
-            "angularVelocity": 0.0000030718548612359243,
-            "velocityX": 3.3259666533847816,
-            "velocityY": -1.8023789091228484,
-            "timestamp": 1.7676166309747012
+            "angularVelocity": -0.000046771874277348444,
+            "velocityX": 2.8374903863181586,
+            "velocityY": -2.5018337298792988,
+            "timestamp": 1.3069539720777241
+        },
+        {
+            "x": 4.14638765114505,
+            "y": 4.979871763007162,
+            "heading": -0.5720542261628284,
+            "angularVelocity": 0.000003127581707131923,
+            "velocityX": 2.935460161172297,
+            "velocityY": -2.386146961079825,
+            "timestamp": 1.3251426050680648
+        },
+        {
+            "x": 4.201473544713913,
+            "y": 4.938642209546033,
+            "heading": -0.5720541700948967,
+            "angularVelocity": 0.000003082580845535541,
+            "velocityX": 3.0285889873152927,
+            "velocityY": -2.266775819987239,
+            "timestamp": 1.3433312380584055
+        },
+        {
+            "x": 4.258151834683336,
+            "y": 4.899630631332575,
+            "heading": -0.5720541145494444,
+            "angularVelocity": 0.0000030538552492495714,
+            "velocityX": 3.11613797471883,
+            "velocityY": -2.1448328873407987,
+            "timestamp": 1.3615198710487462
+        },
+        {
+            "x": 4.316201884310526,
+            "y": 4.862691173522813,
+            "heading": -0.5720540592428014,
+            "angularVelocity": 0.000003040725654248182,
+            "velocityX": 3.1915564879460585,
+            "velocityY": -2.030908965471825,
+            "timestamp": 1.3797085040390868
+        },
+        {
+            "x": 4.374575644994192,
+            "y": 4.826265415180517,
+            "heading": -0.5720540039764063,
+            "angularVelocity": 0.0000030385128512512867,
+            "velocityX": 3.209353925315247,
+            "velocityY": -2.002666080603007,
+            "timestamp": 1.3978971370294275
+        },
+        {
+            "x": 4.432961770826435,
+            "y": 4.789859479973715,
+            "heading": -0.572053948711591,
+            "angularVelocity": 0.000003038425994779569,
+            "velocityX": 3.210033753678121,
+            "velocityY": -2.0015762166465794,
+            "timestamp": 1.4160857700197682
+        },
+        {
+            "x": 4.491358693973173,
+            "y": 4.753470866702408,
+            "heading": -0.5720538934471577,
+            "angularVelocity": 0.000003038404992596871,
+            "velocityX": 3.21062738347468,
+            "velocityY": -2.000623867148186,
+            "timestamp": 1.434274403010109
+        },
+        {
+            "x": 4.549763643159385,
+            "y": 4.7170951368878224,
+            "heading": -0.5720538381830028,
+            "angularVelocity": 0.0000030383896855688938,
+            "velocityX": 3.2110686502526127,
+            "velocityY": -1.9999155425186408,
+            "timestamp": 1.4524630360004496
+        },
+        {
+            "x": 4.608173712818011,
+            "y": 4.680727629815019,
+            "heading": -0.5720537829190162,
+            "angularVelocity": 0.0000030383804361409647,
+            "velocityX": 3.211350170717194,
+            "velocityY": -1.9994634611677902,
+            "timestamp": 1.4706516689907903
+        },
+        {
+            "x": 4.666585738503753,
+            "y": 4.644363264529719,
+            "heading": -0.5720537276550893,
+            "angularVelocity": 0.0000030383771462609746,
+            "velocityX": 3.211457711899863,
+            "velocityY": -1.999290727599226,
+            "timestamp": 1.488840301981131
+        },
+        {
+            "x": 4.724997877507934,
+            "y": 4.607999081268276,
+            "heading": -0.5720536723911652,
+            "angularVelocity": 0.0000030383769981998487,
+            "velocityX": 3.2114639420784044,
+            "velocityY": -1.9992807200388194,
+            "timestamp": 1.5070289349714716
+        },
+        {
+            "x": 4.783409732751982,
+            "y": 4.571634442201932,
+            "heading": -0.5720536171272324,
+            "angularVelocity": 0.0000030383774810111486,
+            "velocityX": 3.2114483411183334,
+            "velocityY": -1.9993057799158553,
+            "timestamp": 1.5252175679618123
+        },
+        {
+            "x": 4.841821302805823,
+            "y": 4.535269345043953,
+            "heading": -0.5720535618632904,
+            "angularVelocity": 0.0000030383779810703436,
+            "velocityX": 3.211432661533899,
+            "velocityY": -1.9993309655158582,
+            "timestamp": 1.543406200952153
+        },
+        {
+            "x": 4.900232750957554,
+            "y": 4.4989040520829935,
+            "heading": -0.5720535065993444,
+            "angularVelocity": 0.0000030383782054284467,
+            "velocityX": 3.2114259594301156,
+            "velocityY": -1.9993417306445722,
+            "timestamp": 1.5615948339424937
+        },
+        {
+            "x": 4.958643009851236,
+            "y": 4.46253684895675,
+            "heading": -0.5720534513353597,
+            "angularVelocity": 0.000003038380333213315,
+            "velocityX": 3.2113605747448064,
+            "velocityY": -1.999446750371714,
+            "timestamp": 1.5797834669328343
+        },
+        {
+            "x": 5.017052204377269,
+            "y": 4.426167936377253,
+            "heading": -0.5720533960713398,
+            "angularVelocity": 0.0000030383822663214653,
+            "velocityX": 3.21130205645789,
+            "velocityY": -1.999540735073477,
+            "timestamp": 1.597972099923175
+        },
+        {
+            "x": 5.075461342766336,
+            "y": 4.389798933639636,
+            "heading": -0.572053340807318,
+            "angularVelocity": 0.0000030383823681408014,
+            "velocityX": 3.211298970081695,
+            "velocityY": -1.999545691911999,
+            "timestamp": 1.6161607329135157
+        },
+        {
+            "x": 5.133870231273224,
+            "y": 4.35342952959225,
+            "heading": -0.5720532855432875,
+            "angularVelocity": 0.0000030383828455618877,
+            "velocityX": 3.211285231711017,
+            "velocityY": -1.9995677556796618,
+            "timestamp": 1.6343493659038564
+        },
+        {
+            "x": 5.192278843825858,
+            "y": 4.317059682370623,
+            "heading": -0.5720532302792477,
+            "angularVelocity": 0.0000030383833608448334,
+            "velocityX": 3.2112700599133515,
+            "velocityY": -1.9995921211310104,
+            "timestamp": 1.652537998894197
+        },
+        {
+            "x": 5.250688378518623,
+            "y": 4.28069131610433,
+            "heading": -0.5720531750152431,
+            "angularVelocity": 0.000003038381418986995,
+            "velocityX": 3.211320758617258,
+            "velocityY": -1.9995106991068379,
+            "timestamp": 1.6707266318845377
+        },
+        {
+            "x": 5.309100997862405,
+            "y": 4.2443279043993805,
+            "heading": -0.5720531197513381,
+            "angularVelocity": 0.0000030383759466885435,
+            "velocityX": 3.2114903508569643,
+            "velocityY": -1.9992383003329772,
+            "timestamp": 1.6889152648748784
+        },
+        {
+            "x": 5.367519658270811,
+            "y": 4.20797419858894,
+            "heading": -0.5720530644876303,
+            "angularVelocity": 0.0000030383651093388606,
+            "velocityX": 3.2118224849224113,
+            "velocityY": -1.998704676145307,
+            "timestamp": 1.707103897865219
+        },
+        {
+            "x": 5.425947098435591,
+            "y": 4.171634605233085,
+            "heading": -0.5720530092241862,
+            "angularVelocity": 0.000003038350601996349,
+            "velocityX": 3.212305190598148,
+            "velocityY": -1.9979287819592548,
+            "timestamp": 1.7252925308555598
+        },
+        {
+            "x": 5.484385608043213,
+            "y": 4.135312815587752,
+            "heading": -0.5720529539612775,
+            "angularVelocity": 0.0000030383211709796797,
+            "velocityX": 3.2129137818460802,
+            "velocityY": -1.9969499447620875,
+            "timestamp": 1.7434811638459005
+        },
+        {
+            "x": 5.542805855367704,
+            "y": 4.098961660035809,
+            "heading": -0.5720528986974772,
+            "angularVelocity": 0.0000030383701899944643,
+            "velocityX": 3.21190973260657,
+            "velocityY": -1.9985644644794618,
+            "timestamp": 1.7616697968362411
+        },
+        {
+            "x": 5.601240344083477,
+            "y": 4.062633401947461,
+            "heading": -0.5720528434300965,
+            "angularVelocity": 0.000003038567036318681,
+            "velocityX": 3.2126927156544984,
+            "velocityY": -1.9973055758301874,
+            "timestamp": 1.7798584298265818
+        },
+        {
+            "x": 5.6606145093793865,
+            "y": 4.027862145195579,
+            "heading": -0.5720527881030576,
+            "angularVelocity": 0.0000030418470110587657,
+            "velocityX": 3.2643555635790893,
+            "velocityY": -1.9117025875630587,
+            "timestamp": 1.7980470628169225
+        },
+        {
+            "x": 5.721296662408543,
+            "y": 3.9954274711883437,
+            "heading": -0.5720527324898503,
+            "angularVelocity": 0.0000030575803747562297,
+            "velocityX": 3.3362679351103024,
+            "velocityY": -1.7832386867362542,
+            "timestamp": 1.8162356958072632
         },
         {
             "x": 5.7832255363464355,
             "y": 3.9654417037963863,
-            "heading": -0.572052675208486,
-            "angularVelocity": 0.000003159693577578397,
-            "velocityX": 3.3940075416988695,
-            "velocityY": -1.670729295450284,
-            "timestamp": 1.7857981861791263
-        },
-        {
-            "x": 5.945861586676733,
-            "y": 3.9048707731215235,
-            "heading": -0.5720526214657033,
-            "angularVelocity": 0.0000011714587578241761,
-            "velocityX": 3.5450606755342,
-            "velocityY": -1.3202953710429959,
-            "timestamp": 1.831674989206145
-        },
-        {
-            "x": 6.113092819895856,
-            "y": 3.858469939627616,
-            "heading": -0.5720525698257588,
-            "angularVelocity": 0.0000011256221281562495,
-            "velocityX": 3.6452242132183756,
-            "velocityY": -1.0114225585101817,
-            "timestamp": 1.8775517922331635
-        },
-        {
-            "x": 6.280428909416468,
-            "y": 3.8124486851902244,
-            "heading": -0.5720525182264563,
-            "angularVelocity": 0.0000011247362300949913,
-            "velocityX": 3.6475098193329205,
-            "velocityY": -1.0031486808299002,
-            "timestamp": 1.923428595260182
-        },
-        {
-            "x": 6.444900242470224,
-            "y": 3.7570546132296903,
-            "heading": -0.5720524653781475,
-            "angularVelocity": 0.000001151961455361842,
-            "velocityX": 3.585065266142057,
-            "velocityY": -1.2074527496557126,
-            "timestamp": 1.9693053982872004
-        },
-        {
-            "x": 6.602974533610246,
-            "y": 3.685417746961916,
-            "heading": -0.5720524086130027,
-            "angularVelocity": 0.0000012373387213911858,
-            "velocityX": 3.445625691199315,
-            "velocityY": -1.561505195243581,
-            "timestamp": 2.015182201314219
-        },
-        {
-            "x": 6.753006171084989,
-            "y": 3.5981834135738024,
-            "heading": -0.5720523449049553,
-            "angularVelocity": 0.0000013886766999770178,
-            "velocityX": 3.27031588025751,
-            "velocityY": -1.9014911160397099,
-            "timestamp": 2.0610590043412373
-        },
-        {
-            "x": 6.893461047058093,
-            "y": 3.4962434932531963,
-            "heading": -0.5720522700606003,
-            "angularVelocity": 0.0000016314204553722347,
-            "velocityX": 3.0615663408441725,
-            "velocityY": -2.2220362709370285,
-            "timestamp": 2.106935807368256
-        },
-        {
-            "x": 7.0229029918463715,
-            "y": 3.380640293435665,
-            "heading": -0.5720521774325067,
-            "angularVelocity": 0.000002019061650871652,
-            "velocityX": 2.8215118806773165,
-            "velocityY": -2.519861720737319,
-            "timestamp": 2.1528126103952743
+            "heading": -0.5720526754836338,
+            "angularVelocity": 0.0000031341671777608764,
+            "velocityX": 3.404811893823314,
+            "velocityY": -1.6485992876907705,
+            "timestamp": 1.8344243287976039
+        },
+        {
+            "x": 5.946280542451374,
+            "y": 3.905964298639421,
+            "heading": -0.5720526220643124,
+            "angularVelocity": 0.0000011643081031480314,
+            "velocityX": 3.5538876198319724,
+            "velocityY": -1.296347894456755,
+            "timestamp": 1.880305072239023
+        },
+        {
+            "x": 6.113507668800577,
+            "y": 3.8594929620139826,
+            "heading": -0.5720525704992387,
+            "angularVelocity": 0.0000011238935971124524,
+            "velocityX": 3.6448216355273746,
+            "velocityY": -1.0128723542630858,
+            "timestamp": 1.9261858156804423
+        },
+        {
+            "x": 6.280741389533688,
+            "y": 3.813045361829368,
+            "heading": -0.5720525189366377,
+            "angularVelocity": 0.000001123839702331554,
+            "velocityX": 3.64496536431679,
+            "velocityY": -1.0123550034458102,
+            "timestamp": 1.9720665591218616
+        },
+        {
+            "x": 6.445214711488264,
+            "y": 3.7576105066241974,
+            "heading": -0.5720524661781627,
+            "angularVelocity": 0.000001149904532146113,
+            "velocityX": 3.584800716329628,
+            "velocityY": -1.2082379457532038,
+            "timestamp": 2.0179473025632806
+        },
+        {
+            "x": 6.603250853740864,
+            "y": 3.685853457646542,
+            "heading": -0.5720524095450499,
+            "angularVelocity": 0.0000012343547357711312,
+            "velocityX": 3.444498288357164,
+            "velocityY": -1.563990545825532,
+            "timestamp": 2.0638280460046996
+        },
+        {
+            "x": 6.753231641982998,
+            "y": 3.5985021065376834,
+            "heading": -0.5720523460454168,
+            "angularVelocity": 0.0000013840149106858422,
+            "velocityX": 3.268926721589291,
+            "velocityY": -1.9038782843733606,
+            "timestamp": 2.1097087894461186
+        },
+        {
+            "x": 6.893623224143754,
+            "y": 3.4964496816334383,
+            "heading": -0.5720522715269948,
+            "angularVelocity": 0.000001624176426799149,
+            "velocityX": 3.059923872852999,
+            "velocityY": -2.2242975429267773,
+            "timestamp": 2.1555895328875376
+        },
+        {
+            "x": 7.022989830955657,
+            "y": 3.3807398195880776,
+            "heading": -0.5720521794208858,
+            "angularVelocity": 0.000002007511259312441,
+            "velocityX": 2.819627519265176,
+            "velocityY": -2.521970076471132,
+            "timestamp": 2.2014702763289566
         },
         {
             "x": 7.140008449554443,
             "y": 3.2525558471679688,
-            "heading": -0.5720520639012779,
-            "angularVelocity": 0.0000024746979161572956,
-            "velocityX": 2.5526071997441684,
-            "velocityY": -2.7919217952549285,
-            "timestamp": 2.1986894134222927
-        },
-        {
-            "x": 7.220415929770595,
-            "y": 3.149946383889573,
-            "heading": -0.5720519473102574,
-            "angularVelocity": 0.000003383341158681086,
-            "velocityX": 2.333335244140948,
-            "velocityY": -2.977611988413324,
-            "timestamp": 2.2331497338662096
-        },
-        {
-            "x": 7.2928033634314815,
-            "y": 3.0415299314010706,
-            "heading": -0.5720518591979534,
-            "angularVelocity": 0.000002556920622107498,
-            "velocityX": 2.100602453152653,
-            "velocityY": -3.1461243276865574,
-            "timestamp": 2.2676100543101265
-        },
-        {
-            "x": 7.356753135957797,
-            "y": 2.927931973099431,
-            "heading": -0.5720517877574303,
-            "angularVelocity": 0.000002073124165462267,
-            "velocityX": 1.855750953634494,
-            "velocityY": -3.2964858375736252,
-            "timestamp": 2.3020703747540434
-        },
-        {
-            "x": 7.411896317838347,
-            "y": 2.8098078848104033,
-            "heading": -0.5720517267503814,
-            "angularVelocity": 0.0000017703564026272917,
-            "velocityX": 1.6001935318717442,
-            "velocityY": -3.427829073187373,
-            "timestamp": 2.3365306951979603
-        },
-        {
-            "x": 7.457914797691639,
-            "y": 2.687839154921363,
-            "heading": -0.572051672451037,
-            "angularVelocity": 0.0000015757063075533039,
-            "velocityX": 1.3354048732140973,
-            "velocityY": -3.5393962771625227,
-            "timestamp": 2.370991015641877
-        },
-        {
-            "x": 7.494543133171347,
-            "y": 2.5627294544923394,
-            "heading": -0.5720516223974168,
-            "angularVelocity": 0.000001452500141635371,
-            "velocityX": 1.0629133742199746,
-            "velocityY": -3.630543733121263,
-            "timestamp": 2.405451336085794
-        },
-        {
-            "x": 7.521592976236692,
-            "y": 2.435205423722826,
-            "heading": -0.572051574793162,
-            "angularVelocity": 0.0000013814222897397843,
-            "velocityX": 0.7849562255098341,
-            "velocityY": -3.70060490229679,
-            "timestamp": 2.439911656529711
-        },
-        {
-            "x": 7.539069360193098,
-            "y": 2.3060208712744585,
-            "heading": -0.572051528188319,
-            "angularVelocity": 0.000001352420480159191,
-            "velocityX": 0.5071451376970121,
-            "velocityY": -3.748791386272309,
-            "timestamp": 2.474371976973628
-        },
-        {
-            "x": 7.551504596507269,
-            "y": 2.176254012981795,
-            "heading": -0.5720514816592456,
-            "angularVelocity": 0.0000013502217280486013,
-            "velocityX": 0.3608566651205708,
-            "velocityY": -3.7656892513183533,
-            "timestamp": 2.5088322974175448
-        },
-        {
-            "x": 7.5639699142109125,
-            "y": 2.0464900408376128,
-            "heading": -0.5720514351306527,
-            "angularVelocity": 0.0000013502077822692335,
-            "velocityX": 0.36172959342888805,
-            "velocityY": -3.765605498514506,
-            "timestamp": 2.5432926178614617
-        },
-        {
-            "x": 7.576441327813496,
-            "y": 1.916726654418564,
-            "heading": -0.5720513886022516,
-            "angularVelocity": 0.00000135020222116771,
-            "velocityX": 0.36190648960876737,
-            "velocityY": -3.7655885014252446,
-            "timestamp": 2.5777529383053785
-        },
-        {
-            "x": 7.58884833483585,
-            "y": 1.786957094038731,
-            "heading": -0.5720513420716767,
-            "angularVelocity": 0.000001350265295572762,
-            "velocityX": 0.36003748258495927,
-            "velocityY": -3.765767662871606,
-            "timestamp": 2.6122132587492954
-        },
-        {
-            "x": 7.601019357724818,
-            "y": 1.6571651890693204,
-            "heading": -0.5720512933827582,
-            "angularVelocity": 0.0000014128980208836598,
-            "velocityX": 0.3531894866947264,
-            "velocityY": -3.7664160778959355,
-            "timestamp": 2.6466735791932123
-        },
-        {
-            "x": 7.610550391987499,
-            "y": 1.5345810963054864,
-            "heading": -0.5537181215211259,
-            "angularVelocity": 0.5320081654919462,
-            "velocityX": 0.2765799661741953,
-            "velocityY": -3.5572534203020942,
-            "timestamp": 2.6811338996371292
-        },
-        {
-            "x": 7.620334086621719,
-            "y": 1.4202071871878734,
-            "heading": -0.5150322166693104,
-            "angularVelocity": 1.122621738667962,
-            "velocityX": 0.2839118878810709,
-            "velocityY": -3.319003063356803,
-            "timestamp": 2.715594220081046
-        },
-        {
-            "x": 7.630352820350644,
-            "y": 1.314046573370508,
-            "heading": -0.4556179078059868,
-            "angularVelocity": 1.7241368651811046,
-            "velocityX": 0.290732459821803,
-            "velocityY": -3.0806624096878554,
-            "timestamp": 2.750054540524963
-        },
-        {
-            "x": 7.63947235322732,
-            "y": 1.2175511973082003,
-            "heading": -0.3892109145347841,
-            "angularVelocity": 1.9270567544269157,
-            "velocityX": 0.2646386556821465,
-            "velocityY": -2.8001880080992603,
-            "timestamp": 2.78451486096888
-        },
-        {
-            "x": 7.647652812069227,
-            "y": 1.1309118710627388,
-            "heading": -0.32231407882554747,
-            "angularVelocity": 1.9412714347239703,
-            "velocityX": 0.23738777633239685,
-            "velocityY": -2.5141764536539966,
-            "timestamp": 2.818975181412797
-        },
-        {
-            "x": 7.654895533140534,
-            "y": 1.0540989766744202,
-            "heading": -0.25819483301023066,
-            "angularVelocity": 1.8606688791433752,
-            "velocityX": 0.21017567387602423,
-            "velocityY": -2.22902437930995,
-            "timestamp": 2.8534355018567137
-        },
-        {
-            "x": 7.66120815675186,
-            "y": 0.9870533783479266,
-            "heading": -0.19885832997172487,
-            "angularVelocity": 1.7218790270703905,
-            "velocityX": 0.18318528469800885,
-            "velocityY": -1.9455883596791401,
-            "timestamp": 2.8878958223006306
-        },
-        {
-            "x": 7.666600355589264,
-            "y": 0.929716153410391,
-            "heading": -0.14570825222338094,
-            "angularVelocity": 1.542355876662632,
-            "velocityX": 0.15647558606309142,
-            "velocityY": -1.663862210186161,
-            "timestamp": 2.9223561427445475
-        },
-        {
-            "x": 7.671080940668327,
-            "y": 0.8820338579186632,
-            "heading": -0.09980369912266952,
-            "angularVelocity": 1.3320988461333123,
-            "velocityX": 0.1300215732566484,
-            "velocityY": -1.383686944215496,
-            "timestamp": 2.9568164631884644
-        },
-        {
-            "x": 7.674656709905022,
-            "y": 0.8439593502971015,
-            "heading": -0.06197555143316066,
-            "angularVelocity": 1.0977305841102087,
-            "velocityX": 0.10376482837700403,
-            "velocityY": -1.1048796741031013,
-            "timestamp": 2.9912767836323813
-        },
-        {
-            "x": 7.67733258738128,
-            "y": 0.8154516156506164,
-            "heading": -0.032889080662387146,
-            "angularVelocity": 0.8440568861838474,
-            "velocityX": 0.07765097485389878,
-            "velocityY": -0.8272626104241091,
-            "timestamp": 3.025737104076298
-        },
-        {
-            "x": 7.679112422858347,
-            "y": 0.7964752743679322,
-            "heading": -0.013083588528854375,
-            "angularVelocity": 0.5747332548975859,
-            "velocityX": 0.05164883710108163,
-            "velocityY": -0.5506722235380279,
-            "timestamp": 3.060197424520215
+            "heading": -0.5720520666435536,
+            "angularVelocity": 0.0000024580537218711263,
+            "velocityX": 2.550495258387596,
+            "velocityY": -2.7938512501200163,
+            "timestamp": 2.2473510197703757
+        },
+        {
+            "x": 7.220333202972963,
+            "y": 3.149893277120657,
+            "heading": -0.57205195105401,
+            "angularVelocity": 0.0000033545158672128766,
+            "velocityX": 2.3310989161613462,
+            "velocityY": -2.9793630927244275,
+            "timestamp": 2.28180891122307
+        },
+        {
+            "x": 7.292634698776439,
+            "y": 3.0414305376048008,
+            "heading": -0.5720518635503085,
+            "angularVelocity": 0.000002539438655350069,
+            "velocityX": 2.098256531535021,
+            "velocityY": -3.1476893954673923,
+            "timestamp": 2.3162668026757647
+        },
+        {
+            "x": 7.356495877093559,
+            "y": 2.9277932908128785,
+            "heading": -0.5720517925210026,
+            "angularVelocity": 0.000002061336399152925,
+            "velocityX": 1.853310682252877,
+            "velocityY": -3.297858400531866,
+            "timestamp": 2.350724694128459
+        },
+        {
+            "x": 7.411548371625277,
+            "y": 2.8096370468188216,
+            "heading": -0.5720517318127031,
+            "angularVelocity": 0.0000017618112132015592,
+            "velocityX": 1.5976745009881768,
+            "velocityY": -3.4290038946879324,
+            "timestamp": 2.3851825855811537
+        },
+        {
+            "x": 7.457474639290446,
+            "y": 2.6876433834723,
+            "heading": -0.5720516777421675,
+            "angularVelocity": 0.0000015691771403552433,
+            "velocityX": 1.332822924707698,
+            "velocityY": -3.540369366882859,
+            "timestamp": 2.419640477033848
+        },
+        {
+            "x": 7.49400980759948,
+            "y": 2.5625160164724585,
+            "heading": -0.5720516278694773,
+            "angularVelocity": 0.0000014473517720699968,
+            "velocityX": 1.0602845028769212,
+            "velocityY": -3.631312356172488,
+            "timestamp": 2.4540983684865427
+        },
+        {
+            "x": 7.520944004354476,
+            "y": 2.4349769014207707,
+            "heading": -0.572051580412575,
+            "angularVelocity": 0.0000013772433620936625,
+            "velocityX": 0.7816553950223419,
+            "velocityY": -3.7013035236575447,
+            "timestamp": 2.4885562599392372
+        },
+        {
+            "x": 7.53814696549347,
+            "y": 2.3057649240296647,
+            "heading": -0.5720515339265989,
+            "angularVelocity": 0.0000013490661810968306,
+            "velocityX": 0.4992459031544392,
+            "velocityY": -3.7498515418008185,
+            "timestamp": 2.5230141513919317
+        },
+        {
+            "x": 7.550702118784975,
+            "y": 2.1760188446169613,
+            "heading": -0.5720514875103294,
+            "angularVelocity": 0.0000013470432314212435,
+            "velocityX": 0.3643622044850161,
+            "velocityY": -3.7653516783177263,
+            "timestamp": 2.5574720428446263
+        },
+        {
+            "x": 7.56324456343349,
+            "y": 2.046271536053824,
+            "heading": -0.5720514410938347,
+            "angularVelocity": 0.000001347049770461967,
+            "velocityX": 0.36399338786688684,
+            "velocityY": -3.765387349404451,
+            "timestamp": 2.5919299342973208
+        },
+        {
+            "x": 7.57578727298306,
+            "y": 1.916524253098162,
+            "heading": -0.5720513946773481,
+            "angularVelocity": 0.0000013470495296572014,
+            "velocityX": 0.3640010755350196,
+            "velocityY": -3.7653866062523553,
+            "timestamp": 2.6263878257500153
+        },
+        {
+            "x": 7.588311679081848,
+            "y": 1.7867752020286953,
+            "heading": -0.572051348260254,
+            "angularVelocity": 0.0000013470671654480904,
+            "velocityX": 0.3634698923895803,
+            "velocityY": -3.765437918557532,
+            "timestamp": 2.66084571720271
+        },
+        {
+            "x": 7.600787981664821,
+            "y": 1.657021516812979,
+            "heading": -0.5720513014102728,
+            "angularVelocity": 0.0000013596299477389166,
+            "velocityX": 0.3620738837176714,
+            "velocityY": -3.7655724057824624,
+            "timestamp": 2.6953036086554043
+        },
+        {
+            "x": 7.6103162713524775,
+            "y": 1.534453918276911,
+            "heading": -0.5536995765422551,
+            "angularVelocity": 0.5325840930573226,
+            "velocityX": 0.27651981261615693,
+            "velocityY": -3.557025498914872,
+            "timestamp": 2.729761500108099
+        },
+        {
+            "x": 7.620120451226168,
+            "y": 1.4200989858077522,
+            "heading": -0.5149948955625814,
+            "angularVelocity": 1.1232457747100673,
+            "velocityX": 0.2845264019455608,
+            "velocityY": -3.3186863051722235,
+            "timestamp": 2.7642193915607933
+        },
+        {
+            "x": 7.630164619508576,
+            "y": 1.3139569074919104,
+            "heading": -0.4555658179030021,
+            "angularVelocity": 1.7246870064917483,
+            "velocityX": 0.29149108836702503,
+            "velocityY": -3.080341652987332,
+            "timestamp": 2.798677283013488
+        },
+        {
+            "x": 7.6393124908526255,
+            "y": 1.2174783858270968,
+            "heading": -0.38913465703239974,
+            "angularVelocity": 1.9278939618746356,
+            "velocityX": 0.26547971911102464,
+            "velocityY": -2.799896267514583,
+            "timestamp": 2.8331351744661823
+        },
+        {
+            "x": 7.647521237661079,
+            "y": 1.1308545482085492,
+            "heading": -0.3222243620613333,
+            "angularVelocity": 1.9417988782892024,
+            "velocityX": 0.23822545322294675,
+            "velocityY": -2.5139041875930466,
+            "timestamp": 2.867593065918877
+        },
+        {
+            "x": 7.654790830767336,
+            "y": 1.0540553472510912,
+            "heading": -0.25810310965019956,
+            "angularVelocity": 1.8608582739068054,
+            "velocityX": 0.21097034089302885,
+            "velocityY": -2.2287841106847384,
+            "timestamp": 2.9020509573715714
+        },
+        {
+            "x": 7.661128119511832,
+            "y": 0.9870214726082436,
+            "heading": -0.19877373026472553,
+            "angularVelocity": 1.7217936700197296,
+            "velocityX": 0.18391400278139633,
+            "velocityY": -1.9453852750938303,
+            "timestamp": 2.936508848824266
+        },
+        {
+            "x": 7.6665422514065025,
+            "y": 0.9296939844226774,
+            "heading": -0.14563708266620207,
+            "angularVelocity": 1.5420748443496368,
+            "velocityX": 0.15712313395848782,
+            "velocityY": -1.6636969288812407,
+            "timestamp": 2.9709667402769604
+        },
+        {
+            "x": 7.6710416569080175,
+            "y": 0.8820195033603357,
+            "heading": -0.09974947152576126,
+            "angularVelocity": 1.331701076470322,
+            "velocityX": 0.1305769248148914,
+            "velocityY": -1.3835577004993256,
+            "timestamp": 3.005424631729655
+        },
+        {
+            "x": 7.674632846509551,
+            "y": 0.8439509935506372,
+            "heading": -0.06193916709248371,
+            "angularVelocity": 1.0972901370124863,
+            "velocityX": 0.10421965622732904,
+            "velocityY": -1.1047834967488865,
+            "timestamp": 3.0398825231823494
+        },
+        {
+            "x": 7.677320522590369,
+            "y": 0.8154475641909131,
+            "heading": -0.032869067302209315,
+            "angularVelocity": 0.8436412840348516,
+            "velocityX": 0.07799885505174348,
+            "velocityY": -0.8271959820541263,
+            "timestamp": 3.074340414635044
+        },
+        {
+            "x": 7.6791083601324726,
+            "y": 0.7964739655020694,
+            "heading": -0.013076341373692615,
+            "angularVelocity": 0.5744032816313593,
+            "velocityX": 0.05188470526556513,
+            "velocityY": -0.5506314486738934,
+            "timestamp": 3.1087983060877384
         },
         {
             "x": 7.68,
             "y": 0.787,
             "heading": -0.003,
-            "angularVelocity": 0.2926144736603049,
-            "velocityX": 0.025756497043992084,
-            "velocityY": -0.27496187632245256,
-            "timestamp": 3.094657744964132
+            "angularVelocity": 0.2924247813459689,
+            "velocityX": 0.02587621673680596,
+            "velocityY": -0.2749432743171821,
+            "timestamp": 3.143256197540433
         },
         {
             "x": 7.68,
             "y": 0.787,
             "heading": -0.003,
-            "angularVelocity": 9.445490920898867e-33,
+            "angularVelocity": -5.23227119774542e-33,
             "velocityX": 0,
-            "velocityY": 3.8255947334103544e-33,
-            "timestamp": 3.129118065408049
+            "velocityY": -1.249481443135334e-33,
+            "timestamp": 3.1777140889931275
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj
new file mode 100644
index 00000000..59066a5b
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj	
@@ -0,0 +1,121 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": 1.04,
+            "angularVelocity": -3.0520704892108665e-32,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7541195079597249,
+            "y": 6.720987197392724,
+            "heading": 1.0162961519009226,
+            "angularVelocity": -0.3240021115694771,
+            "velocityX": 0.6030587810307456,
+            "velocityY": 0.013493760103527496,
+            "timestamp": 0.07315954820244353
+        },
+        {
+            "x": 0.8424136303815098,
+            "y": 6.722972492793459,
+            "heading": 0.9702543819568796,
+            "angularVelocity": -0.6293337107086672,
+            "velocityX": 1.2068707993857855,
+            "velocityY": 0.027136518055622637,
+            "timestamp": 0.14631909640488705
+        },
+        {
+            "x": 0.9749626751701804,
+            "y": 6.7259693430054925,
+            "heading": 0.9040114694947134,
+            "angularVelocity": -0.9054581949963141,
+            "velocityX": 1.8117805268819878,
+            "velocityY": 0.0409632137659495,
+            "timestamp": 0.21947864460733058
+        },
+        {
+            "x": 1.1518931657803464,
+            "y": 6.729992727527654,
+            "heading": 0.8214709979365993,
+            "angularVelocity": -1.1282255506783914,
+            "velocityX": 2.4184196725842533,
+            "velocityY": 0.0549946605880228,
+            "timestamp": 0.2926381928097741
+        },
+        {
+            "x": 1.3734034446931793,
+            "y": 6.735064446892539,
+            "heading": 0.7325177816386138,
+            "angularVelocity": -1.215879792639604,
+            "velocityX": 3.0277699132296103,
+            "velocityY": 0.06932409356680827,
+            "timestamp": 0.3657977410122176
+        },
+        {
+            "x": 1.6366130282089124,
+            "y": 6.741790657706068,
+            "heading": 0.6993734786052862,
+            "angularVelocity": -0.4530413848603092,
+            "velocityX": 3.597747525550208,
+            "velocityY": 0.09193893317775545,
+            "timestamp": 0.43895728921466115
+        },
+        {
+            "x": 1.858120509684905,
+            "y": 6.746838490569188,
+            "heading": 0.6098796286126051,
+            "angularVelocity": -1.2232695825988174,
+            "velocityX": 3.0277316757485147,
+            "velocityY": 0.0689975948068392,
+            "timestamp": 0.5121168374171047
+        },
+        {
+            "x": 2.035048195511774,
+            "y": 6.750890505964207,
+            "heading": 0.5269052635268205,
+            "angularVelocity": -1.1341563353588457,
+            "velocityX": 2.418381334686234,
+            "velocityY": 0.0553860089978706,
+            "timestamp": 0.5852763856195482
+        },
+        {
+            "x": 2.1675926440263233,
+            "y": 6.753939666061267,
+            "heading": 0.4602292179018529,
+            "angularVelocity": -0.9113785864349585,
+            "velocityX": 1.811717701533345,
+            "velocityY": 0.041678224811154865,
+            "timestamp": 0.6584359338219917
+        },
+        {
+            "x": 2.2558825150080364,
+            "y": 6.755978237986949,
+            "heading": 0.41386063697230147,
+            "angularVelocity": -0.6338008102679221,
+            "velocityX": 1.206812687489562,
+            "velocityY": 0.027864741865846625,
+            "timestamp": 0.7315954820244353
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": -0.3261452203924221,
+            "velocityX": 0.6030311295783809,
+            "velocityY": 0.013966215458624026,
+            "timestamp": 0.8047550302268788
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 9.371559495672325e-33,
+            "velocityX": 2.5226214414524116e-33,
+            "velocityY": -5.278780681595662e-33,
+            "timestamp": 0.8779145784293223
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.1.traj
new file mode 100644
index 00000000..fb2e2531
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": 1.04,
+            "angularVelocity": 3.547724392998225e-32,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.737435339105297,
+            "y": 6.6995140898970185,
+            "heading": 1.00766177496316,
+            "angularVelocity": -0.49661186995303425,
+            "velocityX": 0.4213191985755434,
+            "velocityY": -0.3145981609176562,
+            "timestamp": 0.06511770457661178
+        },
+        {
+            "x": 0.7924238483684921,
+            "y": 6.658398285939555,
+            "heading": 0.945783535120445,
+            "angularVelocity": -0.95025216636614,
+            "velocityX": 0.8444479058456402,
+            "velocityY": -0.6314074524707902,
+            "timestamp": 0.13023540915322357
+        },
+        {
+            "x": 0.8751312092590968,
+            "y": 6.596482141852294,
+            "heading": 0.8584519121155169,
+            "angularVelocity": -1.3411348506945964,
+            "velocityX": 1.2701209514118896,
+            "velocityY": -0.9508342545216032,
+            "timestamp": 0.19535311372983535
+        },
+        {
+            "x": 0.9857818913354864,
+            "y": 6.513571279138628,
+            "heading": 0.7519056782658311,
+            "angularVelocity": -1.6362099146835547,
+            "velocityX": 1.6992411325894867,
+            "velocityY": -1.273246089565702,
+            "timestamp": 0.26047081830644714
+        },
+        {
+            "x": 1.1246151480908613,
+            "y": 6.409448227792029,
+            "heading": 0.6367147770436085,
+            "angularVelocity": -1.7689644002530829,
+            "velocityX": 2.132035483407367,
+            "velocityY": -1.5989975694566836,
+            "timestamp": 0.32558852288305895
+        },
+        {
+            "x": 1.2913898404503776,
+            "y": 6.284148794801154,
+            "heading": 0.5378900995711877,
+            "angularVelocity": -1.5176314662036392,
+            "velocityX": 2.5611267080722016,
+            "velocityY": -1.9241991683453519,
+            "timestamp": 0.39070622745967076
+        },
+        {
+            "x": 1.479902092849603,
+            "y": 6.139172935751937,
+            "heading": 0.5154398140678585,
+            "angularVelocity": -0.3447646941687853,
+            "velocityX": 2.8949462150871557,
+            "velocityY": -2.2263662392867607,
+            "timestamp": 0.4558239320362826
+        },
+        {
+            "x": 1.6682099030328927,
+            "y": 5.994133301373275,
+            "heading": 0.4928532472983225,
+            "angularVelocity": -0.34685753922672635,
+            "velocityX": 2.8918066355017875,
+            "velocityY": -2.2273456246913312,
+            "timestamp": 0.5209416366128944
+        },
+        {
+            "x": 1.8350205910276742,
+            "y": 5.8693381796986746,
+            "heading": 0.39723540571375665,
+            "angularVelocity": -1.4683847074503305,
+            "velocityX": 2.561679486083952,
+            "velocityY": -1.9164545569596636,
+            "timestamp": 0.5860593411895062
+        },
+        {
+            "x": 1.9739536597530634,
+            "y": 5.765582600106914,
+            "heading": 0.2820330722411117,
+            "angularVelocity": -1.7691399631126123,
+            "velocityX": 2.1335682765342634,
+            "velocityY": -1.5933543767607987,
+            "timestamp": 0.651177045766118
+        },
+        {
+            "x": 2.08468090414567,
+            "y": 5.683000750382208,
+            "heading": 0.1756895135649168,
+            "angularVelocity": -1.6330974712273538,
+            "velocityX": 1.7004168852778658,
+            "velocityY": -1.2681935007022147,
+            "timestamp": 0.7162947503427298
+        },
+        {
+            "x": 2.167467858188685,
+            "y": 5.6213477608369935,
+            "heading": 0.08927498136102419,
+            "angularVelocity": -1.3270512645639152,
+            "velocityX": 1.2713432480657338,
+            "velocityY": -0.946793041094972,
+            "timestamp": 0.7814124549193416
+        },
+        {
+            "x": 2.2225251033432207,
+            "y": 5.5804064125777195,
+            "heading": 0.028526912579098198,
+            "angularVelocity": -0.9328963478811614,
+            "velocityX": 0.8455034696402836,
+            "velocityY": -0.628728370041152,
+            "timestamp": 0.8465301594959534
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -0.003,
+            "angularVelocity": -0.4841527013902356,
+            "velocityX": 0.42192667624600166,
+            "velocityY": -0.31337733279145685,
+            "timestamp": 0.9116478640725653
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -0.003,
+            "angularVelocity": -3.2592320489843035e-28,
+            "velocityX": -3.6408651267401044e-33,
+            "velocityY": 2.9208085569078165e-32,
+            "timestamp": 0.9767655686491771
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.1.traj
new file mode 100644
index 00000000..331d33ff
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.1.traj	
@@ -0,0 +1,238 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 6.72,
+            "heading": 1.04,
+            "angularVelocity": -9.490919672428962e-31,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7219893017338576,
+            "y": 6.7135407128428675,
+            "heading": 1.0202523530723182,
+            "angularVelocity": -0.4720627806088485,
+            "velocityX": 0.2866013928023673,
+            "velocityY": -0.15440771588183827,
+            "timestamp": 0.041832670862584576
+        },
+        {
+            "x": 0.7460365692584342,
+            "y": 6.700508326600243,
+            "heading": 0.982041462546113,
+            "angularVelocity": -0.9134222065746536,
+            "velocityX": 0.574844183475,
+            "velocityY": -0.3115360787128853,
+            "timestamp": 0.08366534172516915
+        },
+        {
+            "x": 0.7822226491271561,
+            "y": 6.680770273598179,
+            "heading": 0.9269846511448561,
+            "angularVelocity": -1.3161199193358153,
+            "velocityX": 0.8650195916869635,
+            "velocityY": -0.4718334401092994,
+            "timestamp": 0.12549801258775373
+        },
+        {
+            "x": 0.830641015185364,
+            "y": 6.654177161634895,
+            "heading": 0.8571062408800131,
+            "angularVelocity": -1.6704266981755755,
+            "velocityX": 1.1574294698338659,
+            "velocityY": -0.6357019863882251,
+            "timestamp": 0.1673306834503383
+        },
+        {
+            "x": 0.8913887859512984,
+            "y": 6.620557389613832,
+            "heading": 0.7749072042273928,
+            "angularVelocity": -1.9649483276514406,
+            "velocityX": 1.4521609429503868,
+            "velocityY": -0.8036726158720227,
+            "timestamp": 0.20916335431292288
+        },
+        {
+            "x": 0.9645435989149714,
+            "y": 6.579696019447196,
+            "heading": 0.6835223857085977,
+            "angularVelocity": -2.1845322479884017,
+            "velocityX": 1.7487483217121933,
+            "velocityY": -0.976781288979697,
+            "timestamp": 0.25099602517550745
+        },
+        {
+            "x": 1.05011662601444,
+            "y": 6.531295841858201,
+            "heading": 0.5872583086768085,
+            "angularVelocity": -2.301169756719744,
+            "velocityX": 2.0456027629834046,
+            "velocityY": -1.1569946788233771,
+            "timestamp": 0.29282869603809203
+        },
+        {
+            "x": 1.147914322294414,
+            "y": 6.47495195963137,
+            "heading": 0.4931548271581024,
+            "angularVelocity": -2.249521237308087,
+            "velocityX": 2.337830558350811,
+            "velocityY": -1.3468870398426824,
+            "timestamp": 0.3346613669006766
+        },
+        {
+            "x": 1.256785786947157,
+            "y": 6.410210097873117,
+            "heading": 0.4149981322548756,
+            "angularVelocity": -1.8683171141513,
+            "velocityX": 2.602546344945923,
+            "velocityY": -1.5476387336328392,
+            "timestamp": 0.3764940377632612
+        },
+        {
+            "x": 1.3739249067400805,
+            "y": 6.336279355727049,
+            "heading": 0.36626571694971094,
+            "angularVelocity": -1.1649367420322734,
+            "velocityX": 2.800182665307358,
+            "velocityY": -1.7672967233894428,
+            "timestamp": 0.41832670862584576
+        },
+        {
+            "x": 1.4978217219933585,
+            "y": 6.2516824812952025,
+            "heading": 0.34588578820783056,
+            "angularVelocity": -0.48717732627787996,
+            "velocityX": 2.961723760365251,
+            "velocityY": -2.022268066740437,
+            "timestamp": 0.46015937948843033
+        },
+        {
+            "x": 1.6219909683827523,
+            "y": 6.153574664712983,
+            "heading": 0.34588517926846474,
+            "angularVelocity": -0.000014556550044979093,
+            "velocityX": 2.9682361615699526,
+            "velocityY": -2.345243910065211,
+            "timestamp": 0.5019920503510149
+        },
+        {
+            "x": 1.7365961394307947,
+            "y": 6.044446925458561,
+            "heading": 0.3458850779757818,
+            "angularVelocity": -0.0000024213773805789224,
+            "velocityX": 2.7396092261115483,
+            "velocityY": -2.608672528056193,
+            "timestamp": 0.5438247212135995
+        },
+        {
+            "x": 1.840662717819214,
+            "y": 5.925227165222168,
+            "heading": 0.34588493628260575,
+            "angularVelocity": -0.000003387141510558926,
+            "velocityX": 2.4876866870453034,
+            "velocityY": -2.849919877887629,
+            "timestamp": 0.5856573920761841
+        },
+        {
+            "x": 1.973177224121443,
+            "y": 5.721056374144466,
+            "heading": 0.3458848333105023,
+            "angularVelocity": -0.0000016003693825600053,
+            "velocityX": 2.0595107939713087,
+            "velocityY": -3.173176731904646,
+            "timestamp": 0.6500001023402711
+        },
+        {
+            "x": 2.0754764812101274,
+            "y": 5.5001929040123185,
+            "heading": 0.34588474927124485,
+            "angularVelocity": -0.0000013061193280868856,
+            "velocityX": 1.5899121542881502,
+            "velocityY": -3.432610613162356,
+            "timestamp": 0.7143428126043581
+        },
+        {
+            "x": 2.145503113949715,
+            "y": 5.26707902131208,
+            "heading": 0.3458846699393237,
+            "angularVelocity": -0.0000012329589594933622,
+            "velocityX": 1.0883382507855357,
+            "velocityY": -3.6230037830772255,
+            "timestamp": 0.7786855228684451
+        },
+        {
+            "x": 2.1857192532601815,
+            "y": 5.0438534457178905,
+            "heading": 0.2952188838034157,
+            "angularVelocity": -0.787436306738375,
+            "velocityX": 0.6250302348985738,
+            "velocityY": -3.4693219275035725,
+            "timestamp": 0.8430282331325321
+        },
+        {
+            "x": 2.219169026901941,
+            "y": 4.846871449995286,
+            "heading": 0.15700289500909007,
+            "angularVelocity": -2.1481219586034865,
+            "velocityX": 0.5198688943072862,
+            "velocityY": -3.0614500836865797,
+            "timestamp": 0.9073709433966191
+        },
+        {
+            "x": 2.2467778858930516,
+            "y": 4.683227688496762,
+            "heading": -0.005808843023566441,
+            "angularVelocity": -2.5303835875804483,
+            "velocityX": 0.42909070627880197,
+            "velocityY": -2.54331471004047,
+            "timestamp": 0.9717136536607061
+        },
+        {
+            "x": 2.268435157539554,
+            "y": 4.553295114204883,
+            "heading": -0.15269665079354766,
+            "angularVelocity": -2.282897427962143,
+            "velocityX": 0.33659246801420317,
+            "velocityY": -2.019382984623824,
+            "timestamp": 1.036056363924793
+        },
+        {
+            "x": 2.2843848876654955,
+            "y": 4.456384809980395,
+            "heading": -0.26981466762495615,
+            "angularVelocity": -1.820222001073865,
+            "velocityX": 0.24788713531789203,
+            "velocityY": -1.5061582551734227,
+            "timestamp": 1.10039907418888
+        },
+        {
+            "x": 2.2948521877927464,
+            "y": 4.392049751527424,
+            "heading": -0.3510408876957304,
+            "angularVelocity": -1.2623997300920147,
+            "velocityX": 0.16268043550387537,
+            "velocityY": -0.9998810772644539,
+            "timestamp": 1.164741784452967
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.393,
+            "angularVelocity": -0.6521191310103414,
+            "velocityX": 0.0800061450027687,
+            "velocityY": -0.498110063997614,
+            "timestamp": 1.229084494717054
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.393,
+            "angularVelocity": -1.3043243176582522e-30,
+            "velocityX": 1.4896025231223568e-31,
+            "velocityY": 2.1651038615450103e-31,
+            "timestamp": 1.293427204981141
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.traj
index dd55da0b..331d33ff 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.traj	
@@ -4,190 +4,235 @@
             "x": 0.71,
             "y": 6.72,
             "heading": 1.04,
-            "angularVelocity": 0,
+            "angularVelocity": -9.490919672428962e-31,
             "velocityX": 0,
             "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.7269019070910594,
-            "y": 6.695506477136938,
-            "heading": 1.0019343442516249,
-            "angularVelocity": -0.6189075555866055,
-            "velocityX": 0.2748072454505401,
-            "velocityY": -0.3982389391394925,
-            "timestamp": 0.06150459047522247
-        },
-        {
-            "x": 0.7606735605009036,
-            "y": 6.646281390208111,
-            "heading": 0.9275075561119867,
-            "angularVelocity": -1.2101013528351434,
-            "velocityX": 0.5490915905447641,
-            "velocityY": -0.8003481780543988,
-            "timestamp": 0.12300918095044494
-        },
-        {
-            "x": 0.8112865108119746,
-            "y": 6.571983401826325,
-            "heading": 0.8196848361236891,
-            "angularVelocity": -1.7530841056771946,
-            "velocityX": 0.8229133780097379,
-            "velocityY": -1.2080072041406207,
-            "timestamp": 0.1845137714256674
-        },
-        {
-            "x": 0.8787770707149783,
-            "y": 6.472119277485466,
-            "heading": 0.6843886163577808,
-            "angularVelocity": -2.199774337501089,
-            "velocityX": 1.0973255716610744,
-            "velocityY": -1.6236857049083067,
-            "timestamp": 0.24601836190088988
-        },
-        {
-            "x": 0.9632585614164454,
-            "y": 6.3460508217508655,
-            "heading": 0.5331826539280757,
-            "angularVelocity": -2.4584500321259677,
-            "velocityX": 1.373580249030372,
-            "velocityY": -2.0497405927024026,
-            "timestamp": 0.3075229523761124
-        },
-        {
-            "x": 1.0643372089754837,
-            "y": 6.193350092274746,
-            "heading": 0.39338059647753487,
-            "angularVelocity": -2.273034522632274,
-            "velocityX": 1.6434325759759127,
-            "velocityY": -2.4827533733053033,
-            "timestamp": 0.3690275428513349
-        },
-        {
-            "x": 1.1776993188507112,
-            "y": 6.018028259717655,
-            "heading": 0.3265019863738018,
-            "angularVelocity": -1.087375911082207,
-            "velocityX": 1.843148763357691,
-            "velocityY": -2.85054873469518,
-            "timestamp": 0.4305321333265574
-        },
-        {
-            "x": 1.3085730997897271,
-            "y": 5.825657422218107,
-            "heading": 0.3265019166878518,
-            "angularVelocity": -0.0000011330203075824657,
-            "velocityX": 2.1278701301448275,
-            "velocityY": -3.1277476366100623,
-            "timestamp": 0.4920367238017799
-        },
-        {
-            "x": 1.4394483094166801,
-            "y": 5.633287556689,
-            "heading": 0.32650184700859936,
-            "angularVelocity": -0.00000113291141146818,
-            "velocityX": 2.1278933591091382,
-            "velocityY": -3.127731833392264,
-            "timestamp": 0.5535413142770024
-        },
-        {
-            "x": 1.5703235190623746,
-            "y": 5.4409176911726425,
-            "heading": 0.32650177732934843,
-            "angularVelocity": -0.0000011329113877746253,
-            "velocityX": 2.127893359413853,
-            "velocityY": -3.1277318331849595,
-            "timestamp": 0.6150459047522249
-        },
-        {
-            "x": 1.7011987286842072,
-            "y": 5.24854782564005,
-            "heading": 0.32650170765009895,
-            "angularVelocity": -0.0000011329113643360664,
-            "velocityX": 2.127893359025885,
-            "velocityY": -3.1277318334489093,
-            "timestamp": 0.6765504952274474
-        },
-        {
-            "x": 1.8320721192807423,
-            "y": 5.056176722584032,
-            "heading": 0.32650163796198384,
-            "angularVelocity": -0.000001133055510645955,
-            "velocityX": 2.1278637835863385,
-            "velocityY": -3.1277519542791743,
-            "timestamp": 0.7380550857026699
-        },
-        {
-            "x": 1.9454000953072241,
-            "y": 4.881259770298511,
-            "heading": 0.26077643177319376,
-            "angularVelocity": -1.0686227756490447,
-            "velocityX": 1.8425937828516514,
-            "velocityY": -2.843965807007333,
-            "timestamp": 0.7995596761778924
-        },
-        {
-            "x": 2.046443192812682,
-            "y": 4.7300582879948045,
-            "heading": 0.13106133632258532,
-            "angularVelocity": -2.1090311218780498,
-            "velocityX": 1.6428545694676804,
-            "velocityY": -2.4583771899858244,
-            "timestamp": 0.8610642666531149
-        },
-        {
-            "x": 2.131123897414369,
-            "y": 4.604963233646066,
-            "heading": -0.022718028385538325,
-            "angularVelocity": -2.50029084853552,
-            "velocityX": 1.376819257674782,
-            "velocityY": -2.0339141092100017,
-            "timestamp": 0.9225688571283374
-        },
-        {
-            "x": 2.198697305455373,
-            "y": 4.506052899677577,
-            "heading": -0.16200255999945734,
-            "angularVelocity": -2.264620096446796,
-            "velocityX": 1.0986725953118257,
-            "velocityY": -1.6081780758842066,
-            "timestamp": 0.9840734476035599
-        },
-        {
-            "x": 2.249342546290196,
-            "y": 4.432626729162034,
-            "heading": -0.2740984297313893,
-            "angularVelocity": -1.822561029442031,
-            "velocityX": 0.8234383879887088,
-            "velocityY": -1.1938323619132651,
-            "timestamp": 1.0455780380787822
-        },
-        {
-            "x": 2.283112085041758,
-            "y": 4.38410205128396,
-            "heading": -0.35252590091377134,
-            "angularVelocity": -1.2751482544051254,
-            "velocityX": 0.549057208423595,
-            "velocityY": -0.788960263016835,
-            "timestamp": 1.1070826285540047
+            "x": 0.7219893017338576,
+            "y": 6.7135407128428675,
+            "heading": 1.0202523530723182,
+            "angularVelocity": -0.4720627806088485,
+            "velocityX": 0.2866013928023673,
+            "velocityY": -0.15440771588183827,
+            "timestamp": 0.041832670862584576
+        },
+        {
+            "x": 0.7460365692584342,
+            "y": 6.700508326600243,
+            "heading": 0.982041462546113,
+            "angularVelocity": -0.9134222065746536,
+            "velocityX": 0.574844183475,
+            "velocityY": -0.3115360787128853,
+            "timestamp": 0.08366534172516915
+        },
+        {
+            "x": 0.7822226491271561,
+            "y": 6.680770273598179,
+            "heading": 0.9269846511448561,
+            "angularVelocity": -1.3161199193358153,
+            "velocityX": 0.8650195916869635,
+            "velocityY": -0.4718334401092994,
+            "timestamp": 0.12549801258775373
+        },
+        {
+            "x": 0.830641015185364,
+            "y": 6.654177161634895,
+            "heading": 0.8571062408800131,
+            "angularVelocity": -1.6704266981755755,
+            "velocityX": 1.1574294698338659,
+            "velocityY": -0.6357019863882251,
+            "timestamp": 0.1673306834503383
+        },
+        {
+            "x": 0.8913887859512984,
+            "y": 6.620557389613832,
+            "heading": 0.7749072042273928,
+            "angularVelocity": -1.9649483276514406,
+            "velocityX": 1.4521609429503868,
+            "velocityY": -0.8036726158720227,
+            "timestamp": 0.20916335431292288
+        },
+        {
+            "x": 0.9645435989149714,
+            "y": 6.579696019447196,
+            "heading": 0.6835223857085977,
+            "angularVelocity": -2.1845322479884017,
+            "velocityX": 1.7487483217121933,
+            "velocityY": -0.976781288979697,
+            "timestamp": 0.25099602517550745
+        },
+        {
+            "x": 1.05011662601444,
+            "y": 6.531295841858201,
+            "heading": 0.5872583086768085,
+            "angularVelocity": -2.301169756719744,
+            "velocityX": 2.0456027629834046,
+            "velocityY": -1.1569946788233771,
+            "timestamp": 0.29282869603809203
+        },
+        {
+            "x": 1.147914322294414,
+            "y": 6.47495195963137,
+            "heading": 0.4931548271581024,
+            "angularVelocity": -2.249521237308087,
+            "velocityX": 2.337830558350811,
+            "velocityY": -1.3468870398426824,
+            "timestamp": 0.3346613669006766
+        },
+        {
+            "x": 1.256785786947157,
+            "y": 6.410210097873117,
+            "heading": 0.4149981322548756,
+            "angularVelocity": -1.8683171141513,
+            "velocityX": 2.602546344945923,
+            "velocityY": -1.5476387336328392,
+            "timestamp": 0.3764940377632612
+        },
+        {
+            "x": 1.3739249067400805,
+            "y": 6.336279355727049,
+            "heading": 0.36626571694971094,
+            "angularVelocity": -1.1649367420322734,
+            "velocityX": 2.800182665307358,
+            "velocityY": -1.7672967233894428,
+            "timestamp": 0.41832670862584576
+        },
+        {
+            "x": 1.4978217219933585,
+            "y": 6.2516824812952025,
+            "heading": 0.34588578820783056,
+            "angularVelocity": -0.48717732627787996,
+            "velocityX": 2.961723760365251,
+            "velocityY": -2.022268066740437,
+            "timestamp": 0.46015937948843033
+        },
+        {
+            "x": 1.6219909683827523,
+            "y": 6.153574664712983,
+            "heading": 0.34588517926846474,
+            "angularVelocity": -0.000014556550044979093,
+            "velocityX": 2.9682361615699526,
+            "velocityY": -2.345243910065211,
+            "timestamp": 0.5019920503510149
+        },
+        {
+            "x": 1.7365961394307947,
+            "y": 6.044446925458561,
+            "heading": 0.3458850779757818,
+            "angularVelocity": -0.0000024213773805789224,
+            "velocityX": 2.7396092261115483,
+            "velocityY": -2.608672528056193,
+            "timestamp": 0.5438247212135995
+        },
+        {
+            "x": 1.840662717819214,
+            "y": 5.925227165222168,
+            "heading": 0.34588493628260575,
+            "angularVelocity": -0.000003387141510558926,
+            "velocityX": 2.4876866870453034,
+            "velocityY": -2.849919877887629,
+            "timestamp": 0.5856573920761841
+        },
+        {
+            "x": 1.973177224121443,
+            "y": 5.721056374144466,
+            "heading": 0.3458848333105023,
+            "angularVelocity": -0.0000016003693825600053,
+            "velocityX": 2.0595107939713087,
+            "velocityY": -3.173176731904646,
+            "timestamp": 0.6500001023402711
+        },
+        {
+            "x": 2.0754764812101274,
+            "y": 5.5001929040123185,
+            "heading": 0.34588474927124485,
+            "angularVelocity": -0.0000013061193280868856,
+            "velocityX": 1.5899121542881502,
+            "velocityY": -3.432610613162356,
+            "timestamp": 0.7143428126043581
+        },
+        {
+            "x": 2.145503113949715,
+            "y": 5.26707902131208,
+            "heading": 0.3458846699393237,
+            "angularVelocity": -0.0000012329589594933622,
+            "velocityX": 1.0883382507855357,
+            "velocityY": -3.6230037830772255,
+            "timestamp": 0.7786855228684451
+        },
+        {
+            "x": 2.1857192532601815,
+            "y": 5.0438534457178905,
+            "heading": 0.2952188838034157,
+            "angularVelocity": -0.787436306738375,
+            "velocityX": 0.6250302348985738,
+            "velocityY": -3.4693219275035725,
+            "timestamp": 0.8430282331325321
+        },
+        {
+            "x": 2.219169026901941,
+            "y": 4.846871449995286,
+            "heading": 0.15700289500909007,
+            "angularVelocity": -2.1481219586034865,
+            "velocityX": 0.5198688943072862,
+            "velocityY": -3.0614500836865797,
+            "timestamp": 0.9073709433966191
+        },
+        {
+            "x": 2.2467778858930516,
+            "y": 4.683227688496762,
+            "heading": -0.005808843023566441,
+            "angularVelocity": -2.5303835875804483,
+            "velocityX": 0.42909070627880197,
+            "velocityY": -2.54331471004047,
+            "timestamp": 0.9717136536607061
+        },
+        {
+            "x": 2.268435157539554,
+            "y": 4.553295114204883,
+            "heading": -0.15269665079354766,
+            "angularVelocity": -2.282897427962143,
+            "velocityX": 0.33659246801420317,
+            "velocityY": -2.019382984623824,
+            "timestamp": 1.036056363924793
+        },
+        {
+            "x": 2.2843848876654955,
+            "y": 4.456384809980395,
+            "heading": -0.26981466762495615,
+            "angularVelocity": -1.820222001073865,
+            "velocityX": 0.24788713531789203,
+            "velocityY": -1.5061582551734227,
+            "timestamp": 1.10039907418888
+        },
+        {
+            "x": 2.2948521877927464,
+            "y": 4.392049751527424,
+            "heading": -0.3510408876957304,
+            "angularVelocity": -1.2623997300920147,
+            "velocityX": 0.16268043550387537,
+            "velocityY": -0.9998810772644539,
+            "timestamp": 1.164741784452967
         },
         {
             "x": 2.3,
             "y": 4.36,
             "heading": -0.393,
-            "angularVelocity": -0.6580663129938888,
-            "velocityX": 0.2745797480766085,
-            "velocityY": -0.39187402269867344,
-            "timestamp": 1.1685872190292272
+            "angularVelocity": -0.6521191310103414,
+            "velocityX": 0.0800061450027687,
+            "velocityY": -0.498110063997614,
+            "timestamp": 1.229084494717054
         },
         {
             "x": 2.3,
             "y": 4.36,
             "heading": -0.393,
-            "angularVelocity": 0,
-            "velocityX": 1.1335977805694889e-32,
-            "velocityY": 0,
-            "timestamp": 1.2300918095044497
+            "angularVelocity": -1.3043243176582522e-30,
+            "velocityX": 1.4896025231223568e-31,
+            "velocityY": 2.1651038615450103e-31,
+            "timestamp": 1.293427204981141
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C2.1.traj
new file mode 100644
index 00000000..50686485
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C2.1.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -5.382102664363315e-41,
+            "angularVelocity": -2.8652892976895567e-45,
+            "velocityX": 1.592735858156421e-40,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 7.383499994424153,
+            "heading": 1.659495816830368e-21,
+            "angularVelocity": 1.658316243120447e-20,
+            "velocityX": 2.780383744489643e-33,
+            "velocityY": -0.8344065481228213,
+            "timestamp": 0.10007112931184184
+        },
+        {
+            "x": 7.68,
+            "y": 7.216499986370153,
+            "heading": 4.9512387638524226e-21,
+            "angularVelocity": 3.289403167370527e-20,
+            "velocityX": 3.618624574631574e-33,
+            "velocityY": -1.668813065290736,
+            "timestamp": 0.20014225862368368
+        },
+        {
+            "x": 7.68,
+            "y": 6.9659999820333836,
+            "heading": 9.801760970151702e-21,
+            "angularVelocity": 4.8470744390330394e-20,
+            "velocityX": 3.439292531132709e-33,
+            "velocityY": -2.503219520548835,
+            "timestamp": 0.30021338793552554
+        },
+        {
+            "x": 7.68,
+            "y": 6.632,
+            "heading": 1.506109654033251e-20,
+            "angularVelocity": 5.255597191401202e-20,
+            "velocityX": 4.639675238422859e-33,
+            "velocityY": -3.3376257900775017,
+            "timestamp": 0.40028451724736736
+        },
+        {
+            "x": 7.68,
+            "y": 6.298000017966616,
+            "heading": 5.3586032763760104e-21,
+            "angularVelocity": -9.69559666912617e-20,
+            "velocityX": 4.935439546802769e-33,
+            "velocityY": -3.3376257900775017,
+            "timestamp": 0.5003556465592092
+        },
+        {
+            "x": 7.68,
+            "y": 6.047500013629846,
+            "heading": 1.8760590350448333e-21,
+            "angularVelocity": -3.480068842301423e-20,
+            "velocityX": 4.701730454770122e-33,
+            "velocityY": -2.5032195205488352,
+            "timestamp": 0.600426775871051
+        },
+        {
+            "x": 7.68,
+            "y": 5.880500005575846,
+            "heading": 3.168722428707802e-22,
+            "angularVelocity": -1.5580785203142817e-20,
+            "velocityX": 3.49539235617723e-33,
+            "velocityY": -1.668813065290736,
+            "timestamp": 0.7004979051828928
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 5.881496870004905e-41,
+            "angularVelocity": -3.1664700918333573e-21,
+            "velocityX": 3.364378386644386e-33,
+            "velocityY": -0.8344065481228213,
+            "timestamp": 0.8005690344947346
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 9.145026397391945e-42,
+            "angularVelocity": -4.963805231001231e-40,
+            "velocityX": -4.870998572760777e-42,
+            "velocityY": 3.489748854003666e-39,
+            "timestamp": 0.9006401638065764
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C3.1.traj
new file mode 100644
index 00000000..688fbdaa
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C3.1.traj	
@@ -0,0 +1,139 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 1.0127320738607452e-39,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 7.390554627994461,
+            "heading": -1.4011773737891353e-18,
+            "angularVelocity": -1.4633626501290335e-17,
+            "velocityX": 9.653222160408549e-34,
+            "velocityY": -0.7983807350946673,
+            "timestamp": 0.09575052183150974
+        },
+        {
+            "x": 7.68,
+            "y": 7.237663887017311,
+            "heading": -4.2066729097937526e-18,
+            "angularVelocity": -2.93000547907741e-17,
+            "velocityX": 5.229262005564601e-34,
+            "velocityY": -1.5967614385035824,
+            "timestamp": 0.19150104366301948
+        },
+        {
+            "x": 7.68,
+            "y": 7.0083277831364,
+            "heading": -8.423669065336014e-18,
+            "angularVelocity": -4.4041495279144365e-17,
+            "velocityX": -1.080014219031917e-33,
+            "velocityY": -2.3951420785410225,
+            "timestamp": 0.28725156549452924
+        },
+        {
+            "x": 7.68,
+            "y": 6.702546334555254,
+            "heading": -1.4072329853209785e-17,
+            "angularVelocity": -5.899352482411367e-17,
+            "velocityX": -1.5994734307756055e-33,
+            "velocityY": -3.1935225284643685,
+            "timestamp": 0.38300208732603896
+        },
+        {
+            "x": 7.68,
+            "y": 6.340327800733179,
+            "heading": -3.9014262815434015e-17,
+            "angularVelocity": -2.604887418380216e-16,
+            "velocityX": 4.89416509473128e-33,
+            "velocityY": -3.7829405719528495,
+            "timestamp": 0.4787526091575487
+        },
+        {
+            "x": 7.68,
+            "y": 5.978109266911059,
+            "heading": -3.149786356993182e-17,
+            "angularVelocity": 7.849982539759592e-17,
+            "velocityX": -6.0692119772378415e-33,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.5745031309890585
+        },
+        {
+            "x": 7.68,
+            "y": 5.61589073308894,
+            "heading": -3.435394827038493e-17,
+            "angularVelocity": -2.98283982773363e-17,
+            "velocityX": 7.480170031140856e-33,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.6702536528205683
+        },
+        {
+            "x": 7.68,
+            "y": 5.253672199266821,
+            "heading": -8.183196831726821e-18,
+            "angularVelocity": 2.7332228522691796e-16,
+            "velocityX": -1.1202872015905622e-32,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.766004174652078
+        },
+        {
+            "x": 7.68,
+            "y": 4.891453665444746,
+            "heading": 1.4014689490668664e-17,
+            "angularVelocity": 2.318304474774222e-16,
+            "velocityX": 5.155427490211361e-33,
+            "velocityY": -3.7829405719528495,
+            "timestamp": 0.8617546964835878
+        },
+        {
+            "x": 7.68,
+            "y": 4.5856722168636,
+            "heading": 8.395813027039913e-18,
+            "angularVelocity": -5.868246309956073e-17,
+            "velocityX": -4.168254505442575e-33,
+            "velocityY": -3.1935225284643685,
+            "timestamp": 0.9575052183150976
+        },
+        {
+            "x": 7.68,
+            "y": 4.356336112982689,
+            "heading": 4.1945848336017725e-18,
+            "angularVelocity": -4.387681773072387e-17,
+            "velocityX": 2.1988846402360483e-33,
+            "velocityY": -2.3951420785410225,
+            "timestamp": 1.0532557401466074
+        },
+        {
+            "x": 7.68,
+            "y": 4.203445372005539,
+            "heading": 1.3975677775871127e-18,
+            "angularVelocity": -2.9211507184809557e-17,
+            "velocityX": 3.5559383909284404e-33,
+            "velocityY": -1.5967614385035824,
+            "timestamp": 1.1490062619781172
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": -1.4595928576161535e-17,
+            "velocityX": -6.471812078129625e-34,
+            "velocityY": -0.7983807350946673,
+            "timestamp": 1.244756783809627
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 8.546727720642512e-43,
+            "velocityY": 6.233759417908357e-39,
+            "timestamp": 1.3405073056411367
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.1.traj
new file mode 100644
index 00000000..d59fe62f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.1.traj	
@@ -0,0 +1,175 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 3.5626996813915863e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 7.385155846048237,
+            "heading": 1.781123128781041e-18,
+            "angularVelocity": 1.7977717932873702e-17,
+            "velocityX": 2.837123063321612e-19,
+            "velocityY": -0.8260917454276266,
+            "timestamp": 0.099073927811004
+        },
+        {
+            "x": 7.68,
+            "y": 7.22146754125807,
+            "heading": 5.344190150368438e-18,
+            "angularVelocity": 3.5963720225105045e-17,
+            "velocityX": 5.673750850451309e-19,
+            "velocityY": -1.65218345943065,
+            "timestamp": 0.198147855622008
+        },
+        {
+            "x": 7.68,
+            "y": 6.975935091856216,
+            "heading": 1.0690981078424305e-17,
+            "angularVelocity": 5.396768903659916e-17,
+            "velocityX": 8.509388117154945e-19,
+            "velocityY": -2.47827511058448,
+            "timestamp": 0.29722178343301203
+        },
+        {
+            "x": 7.68,
+            "y": 6.648558516522808,
+            "heading": 1.7824905328944302e-17,
+            "angularVelocity": 7.200607069613354e-17,
+            "velocityX": 1.134205375948437e-18,
+            "velocityY": -3.304366573190878,
+            "timestamp": 0.396295711244016
+        },
+        {
+            "x": 7.68,
+            "y": 6.273767735073314,
+            "heading": 3.2118763957552995e-17,
+            "angularVelocity": 1.4427467391699638e-16,
+            "velocityX": 1.2981920193437937e-18,
+            "velocityY": -3.782940575087071,
+            "timestamp": 0.49536963905502
+        },
+        {
+            "x": 7.68,
+            "y": 5.898976953623795,
+            "heading": 3.194072142377472e-17,
+            "angularVelocity": -1.7970674806511035e-18,
+            "velocityX": 1.2970624694923187e-18,
+            "velocityY": -3.7829405750873115,
+            "timestamp": 0.5944435668660241
+        },
+        {
+            "x": 7.68,
+            "y": 5.524186172174278,
+            "heading": 3.523836351949418e-17,
+            "angularVelocity": 3.3284660945411024e-17,
+            "velocityX": 1.2988038949897902e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.6935174946770281
+        },
+        {
+            "x": 7.68,
+            "y": 5.149395390724759,
+            "heading": 4.1591047407718544e-17,
+            "angularVelocity": 6.412064231794501e-17,
+            "velocityX": 1.299093645390414e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.792591422488032
+        },
+        {
+            "x": 7.68,
+            "y": 4.774604609275242,
+            "heading": 3.0405238206332753e-17,
+            "angularVelocity": -1.1290366142265185e-16,
+            "velocityX": 1.2929821768433727e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.891665350299036
+        },
+        {
+            "x": 7.68,
+            "y": 4.3998138278257235,
+            "heading": 3.8390344063969795e-17,
+            "angularVelocity": 8.059744913789504e-17,
+            "velocityX": 1.304793755296235e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.99073927811004
+        },
+        {
+            "x": 7.68,
+            "y": 4.025023046376205,
+            "heading": 1.0979594277826173e-17,
+            "angularVelocity": -2.766696586253564e-16,
+            "velocityX": 1.307186742413441e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 1.0898132059210441
+        },
+        {
+            "x": 7.68,
+            "y": 3.650232264926687,
+            "heading": -2.14668342522941e-17,
+            "angularVelocity": -3.2749714528425564e-16,
+            "velocityX": 1.3056923664932481e-18,
+            "velocityY": -3.7829405750873115,
+            "timestamp": 1.1888871337320481
+        },
+        {
+            "x": 7.68,
+            "y": 3.2754414834771928,
+            "heading": -1.7689075007663427e-17,
+            "angularVelocity": 3.813071035032691e-17,
+            "velocityX": 1.3012288375156972e-18,
+            "velocityY": -3.782940575087071,
+            "timestamp": 1.2879610615430521
+        },
+        {
+            "x": 7.68,
+            "y": 2.948064908143785,
+            "heading": -1.0623620375651477e-17,
+            "angularVelocity": 7.131497447110055e-17,
+            "velocityX": 1.136256754825581e-18,
+            "velocityY": -3.304366573190878,
+            "timestamp": 1.3870349893540561
+        },
+        {
+            "x": 7.68,
+            "y": 2.70253245874193,
+            "heading": -5.31498515459649e-18,
+            "angularVelocity": 5.35825654501949e-17,
+            "velocityX": 8.521926280380639e-19,
+            "velocityY": -2.4782751105844802,
+            "timestamp": 1.48610891716506
+        },
+        {
+            "x": 7.68,
+            "y": 2.538844153951763,
+            "heading": -1.7723662552803497e-18,
+            "angularVelocity": 3.575732766130284e-17,
+            "velocityX": 5.681284366395801e-19,
+            "velocityY": -1.6521834594306501,
+            "timestamp": 1.585182844976064
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": 1.7889330669293064e-17,
+            "velocityX": 2.8406422370407584e-19,
+            "velocityY": -0.8260917454276266,
+            "timestamp": 1.684256772787068
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 1.0461754707969188e-34,
+            "velocityY": 0,
+            "timestamp": 1.783330700598072
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.1.traj
new file mode 100644
index 00000000..6f64999b
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.1.traj	
@@ -0,0 +1,220 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -8.452820466081124e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 7.3890447295128565,
+            "heading": -6.111572142469688e-20,
+            "angularVelocity": -6.320692328199756e-19,
+            "velocityX": -1.9797111149811777e-21,
+            "velocityY": -0.8062267267095136,
+            "timestamp": 0.09669149868710691
+        },
+        {
+            "x": 7.68,
+            "y": 7.2331341916331855,
+            "heading": -1.8226742072400974e-19,
+            "angularVelocity": -1.2529715733337834e-18,
+            "velocityX": -3.753564400558873e-21,
+            "velocityY": -1.6124534214139752,
+            "timestamp": 0.19338299737421383
+        },
+        {
+            "x": 7.68,
+            "y": 6.999268392550218,
+            "heading": -3.611632829024713e-19,
+            "angularVelocity": -1.850171571559818e-18,
+            "velocityX": -5.1157697830729356e-21,
+            "velocityY": -2.418680052108357,
+            "timestamp": 0.29007449606132074
+        },
+        {
+            "x": 7.68,
+            "y": 6.687447350831621,
+            "heading": -5.912144198792796e-19,
+            "angularVelocity": -2.3792281637439373e-18,
+            "velocityX": -5.242727749103024e-21,
+            "velocityY": -3.224906490772764,
+            "timestamp": 0.38676599474842766
+        },
+        {
+            "x": 7.68,
+            "y": 6.321669157855709,
+            "heading": -4.100561450064452e-17,
+            "angularVelocity": -4.179726307846966e-16,
+            "velocityX": -1.166132844762965e-20,
+            "velocityY": -3.782940568121389,
+            "timestamp": 0.48345749343553457
+        },
+        {
+            "x": 7.68,
+            "y": 5.955890964879757,
+            "heading": -6.040572418063157e-17,
+            "angularVelocity": -2.0063924898701218e-16,
+            "velocityX": -1.9392305823460385e-22,
+            "velocityY": -3.782940568121787,
+            "timestamp": 0.5801489921226415
+        },
+        {
+            "x": 7.68,
+            "y": 5.590112771903805,
+            "heading": -8.327536807485113e-17,
+            "angularVelocity": -2.365217646302656e-16,
+            "velocityX": 3.675624932403311e-22,
+            "velocityY": -3.782940568121787,
+            "timestamp": 0.6768404908097484
+        },
+        {
+            "x": 7.68,
+            "y": 5.224334578927854,
+            "heading": -7.575439540555723e-17,
+            "angularVelocity": 7.778318436801094e-17,
+            "velocityX": -2.1057162419585226e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 0.7735319894968553
+        },
+        {
+            "x": 7.68,
+            "y": 4.858556385951902,
+            "heading": -4.8431591576069396e-17,
+            "angularVelocity": 2.82577105541659e-16,
+            "velocityX": 1.016892835365518e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 0.8702234881839622
+        },
+        {
+            "x": 7.68,
+            "y": 4.49277819297595,
+            "heading": -7.892411277004063e-17,
+            "angularVelocity": -3.1535886409915893e-16,
+            "velocityX": 1.448731313682566e-20,
+            "velocityY": -3.782940568121787,
+            "timestamp": 0.9669149868710691
+        },
+        {
+            "x": 7.68,
+            "y": 4.126999999999999,
+            "heading": -8.668140475632877e-17,
+            "angularVelocity": -8.022723912254832e-17,
+            "velocityX": 2.655349226445793e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 1.063606485558176
+        },
+        {
+            "x": 7.68,
+            "y": 3.7612218070240475,
+            "heading": -4.236036305527668e-17,
+            "angularVelocity": 4.583757859030679e-16,
+            "velocityX": 7.500561292003624e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 1.160297984245283
+        },
+        {
+            "x": 7.68,
+            "y": 3.395443614048096,
+            "heading": -2.060715967072721e-17,
+            "angularVelocity": 2.249753461257506e-16,
+            "velocityX": 1.3311646441840309e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 1.25698948293239
+        },
+        {
+            "x": 7.68,
+            "y": 3.0296654210721448,
+            "heading": -1.7378554757194933e-17,
+            "angularVelocity": 3.3390783650741015e-17,
+            "velocityX": 1.5677048108756238e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 1.353680981619497
+        },
+        {
+            "x": 7.68,
+            "y": 2.663887228096194,
+            "heading": -2.7058170522872628e-18,
+            "angularVelocity": 1.5174796030816139e-16,
+            "velocityX": 2.708497475030379e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 1.450372480306604
+        },
+        {
+            "x": 7.68,
+            "y": 2.2981090351202424,
+            "heading": -9.881529062229235e-18,
+            "angularVelocity": -7.421243963841227e-17,
+            "velocityX": 2.3345387345088647e-21,
+            "velocityY": -3.782940568121787,
+            "timestamp": 1.547063978993711
+        },
+        {
+            "x": 7.68,
+            "y": 1.9323308421442913,
+            "heading": -6.5997498705085955e-18,
+            "angularVelocity": 3.39407211211041e-17,
+            "velocityX": -6.072343928076645e-20,
+            "velocityY": -3.782940568121787,
+            "timestamp": 1.643755477680818
+        },
+        {
+            "x": 7.68,
+            "y": 1.5665526491683786,
+            "heading": 6.487893960505722e-19,
+            "angularVelocity": 7.496563157826373e-17,
+            "velocityX": 5.228734565126193e-20,
+            "velocityY": -3.782940568121389,
+            "timestamp": 1.7404469763679251
+        },
+        {
+            "x": 7.68,
+            "y": 1.254731607449781,
+            "heading": 3.9009642634108723e-19,
+            "angularVelocity": -2.6754468948553036e-18,
+            "velocityX": 1.8077268140623735e-21,
+            "velocityY": -3.224906490772764,
+            "timestamp": 1.8371384750550321
+        },
+        {
+            "x": 7.68,
+            "y": 1.020865808366814,
+            "heading": 1.9475770164363563e-19,
+            "angularVelocity": -2.020226466917104e-18,
+            "velocityX": 1.3557598034014801e-21,
+            "velocityY": -2.418680052108357,
+            "timestamp": 1.9338299737421392
+        },
+        {
+            "x": 7.68,
+            "y": 0.8649552704871435,
+            "heading": 6.488190339479676e-20,
+            "angularVelocity": -1.3431976959152128e-18,
+            "velocityX": 9.038296351119003e-22,
+            "velocityY": -1.6124534214139752,
+            "timestamp": 2.030521472429246
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": -6.710197305720325e-19,
+            "velocityX": 4.519117474728098e-22,
+            "velocityY": -0.8062267267095136,
+            "timestamp": 2.127212971116353
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 1.0656726462878492e-32,
+            "velocityY": 0,
+            "timestamp": 2.2239044698034602
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.1.traj
new file mode 100644
index 00000000..ce3947a6
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -7.649625564275916e-33,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.617797764168564,
+            "y": 7.446201717117849,
+            "heading": 0.013442479376395878,
+            "angularVelocity": 0.15144225686963014,
+            "velocityX": -0.7007670767707749,
+            "velocityY": -0.23431234749607086,
+            "timestamp": 0.08876306820531243
+        },
+        {
+            "x": 7.493368305965553,
+            "y": 7.404602586388856,
+            "heading": 0.03870227011836747,
+            "angularVelocity": 0.28457545746724416,
+            "velocityX": -1.401815650572125,
+            "velocityY": -0.4686535917478179,
+            "timestamp": 0.17752613641062487
+        },
+        {
+            "x": 7.30667604944407,
+            "y": 7.3422008161122045,
+            "heading": 0.0730276629215222,
+            "angularVelocity": 0.38670804756608035,
+            "velocityX": -2.1032650210965835,
+            "velocityY": -0.7030150212056434,
+            "timestamp": 0.2662892046159373
+        },
+        {
+            "x": 7.057674574154849,
+            "y": 7.259002273186937,
+            "heading": 0.11075765125141449,
+            "angularVelocity": 0.42506403948785515,
+            "velocityX": -2.8052373619302045,
+            "velocityY": -0.9373103544914424,
+            "timestamp": 0.35505227282124974
+        },
+        {
+            "x": 6.746464870184761,
+            "y": 7.155111739606062,
+            "heading": 0.13339737013954753,
+            "angularVelocity": 0.25505786749611997,
+            "velocityX": -3.5060719538216505,
+            "velocityY": -1.170425219424191,
+            "timestamp": 0.44381534102656217
+        },
+        {
+            "x": 6.428047828205506,
+            "y": 7.048517057389171,
+            "heading": 0.13339738106725169,
+            "angularVelocity": 1.2311157711164836e-7,
+            "velocityX": -3.5872694400628884,
+            "velocityY": -1.2008900139638257,
+            "timestamp": 0.5325784092318746
+        },
+        {
+            "x": 6.109630786224927,
+            "y": 6.941922375176235,
+            "heading": 0.1333973919949535,
+            "angularVelocity": 1.23111550991103e-7,
+            "velocityX": -3.587269440077816,
+            "velocityY": -1.200890013919235,
+            "timestamp": 0.621341477437187
+        },
+        {
+            "x": 5.798381397539494,
+            "y": 6.838018370585249,
+            "heading": 0.15626352052401435,
+            "angularVelocity": 0.2576085864583032,
+            "velocityX": -3.5065190397107555,
+            "velocityY": -1.1705769831075976,
+            "timestamp": 0.7101045456424995
+        },
+        {
+            "x": 5.5493631968567065,
+            "y": 6.754814060290535,
+            "heading": 0.1928079538664876,
+            "angularVelocity": 0.4117076401528937,
+            "velocityX": -2.8054257893230465,
+            "velocityY": -0.9373753293689796,
+            "timestamp": 0.7988676138478119
+        },
+        {
+            "x": 5.362653942099421,
+            "y": 6.692406067264648,
+            "heading": 0.22585011955614995,
+            "angularVelocity": 0.37225127926004886,
+            "velocityX": -2.1034565223155943,
+            "velocityY": -0.703085126367409,
+            "timestamp": 0.8876306820531243
+        },
+        {
+            "x": 5.238210383284219,
+            "y": 6.650801525067523,
+            "heading": 0.25010704549903756,
+            "angularVelocity": 0.2732772360607878,
+            "velocityX": -1.4019745073178984,
+            "velocityY": -0.4687145570597535,
+            "timestamp": 0.9763937502584368
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.263,
+            "angularVelocity": 0.1452513388947291,
+            "velocityX": -0.7008588655398115,
+            "velocityY": -0.2343488737838604,
+            "timestamp": 1.0651568184637492
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.263,
+            "angularVelocity": -2.2743517080702824e-31,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 1.1539198866690616
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.1.traj
new file mode 100644
index 00000000..f95228f2
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -6.443666081756986e-29,
+            "angularVelocity": 1.4558689567742243e-28,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.593939912495806,
+            "y": 7.445359803187238,
+            "heading": -0.04618112566514864,
+            "angularVelocity": -0.44533178373248467,
+            "velocityX": -0.8298908206418381,
+            "velocityY": -0.2086797865609081,
+            "timestamp": 0.10370049332228601
+        },
+        {
+            "x": 7.421570715628637,
+            "y": 7.402025139488172,
+            "heading": -0.13159426902714605,
+            "angularVelocity": -0.823652237569843,
+            "velocityX": -1.662182997832739,
+            "velocityY": -0.41788290788915955,
+            "timestamp": 0.20740098664457202
+        },
+        {
+            "x": 7.162505124622784,
+            "y": 7.336932458231965,
+            "heading": -0.24235041158328322,
+            "angularVelocity": -1.0680387239038096,
+            "velocityX": -2.4982098224038025,
+            "velocityY": -0.627698858229146,
+            "timestamp": 0.311101479966858
+        },
+        {
+            "x": 6.816624735331939,
+            "y": 7.250238713519174,
+            "heading": -0.3315039987658666,
+            "angularVelocity": -0.8597219196007858,
+            "velocityX": -3.3353784365894983,
+            "velocityY": -0.8360012757447058,
+            "timestamp": 0.41480197328914403
+        },
+        {
+            "x": 6.436297385949381,
+            "y": 7.154089090014683,
+            "heading": -0.3315040257643058,
+            "angularVelocity": -2.603501519346576e-7,
+            "velocityX": -3.6675558350581356,
+            "velocityY": -0.927185786914993,
+            "timestamp": 0.5185024666114301
+        },
+        {
+            "x": 6.055970043958053,
+            "y": 7.05793943727301,
+            "heading": -0.33150405276269507,
+            "angularVelocity": -2.60349670609762e-7,
+            "velocityX": -3.6675557637833287,
+            "velocityY": -0.9271860688537472,
+            "timestamp": 0.6222029599337161
+        },
+        {
+            "x": 5.675642817406181,
+            "y": 6.961789327902759,
+            "heading": -0.33150407976108204,
+            "angularVelocity": -2.6034964813703677e-7,
+            "velocityX": -3.6675546505826837,
+            "velocityY": -0.9271904721942118,
+            "timestamp": 0.7259034532560021
+        },
+        {
+            "x": 5.306931756724127,
+            "y": 6.827827408155551,
+            "heading": -0.331504107569482,
+            "angularVelocity": -2.6816073038858223e-7,
+            "velocityX": -3.5555381548297533,
+            "velocityY": -1.2918156457642633,
+            "timestamp": 0.8296039465782882
+        },
+        {
+            "x": 4.97827117667767,
+            "y": 6.613640897314209,
+            "heading": -0.3315041419146958,
+            "angularVelocity": -3.311962425493206e-7,
+            "velocityX": -3.1693251354651193,
+            "velocityY": -2.065433866121419,
+            "timestamp": 0.9333044399005742
+        },
+        {
+            "x": 4.706831845168329,
+            "y": 6.330419557819514,
+            "heading": -0.3315041418010325,
+            "angularVelocity": 1.0960729474265148e-9,
+            "velocityX": -2.6175317282796997,
+            "velocityY": -2.7311474653691747,
+            "timestamp": 1.0370049332228601
+        },
+        {
+            "x": 4.518922560808715,
+            "y": 6.028421366315536,
+            "heading": -0.2325817741817243,
+            "angularVelocity": 0.9539237900428917,
+            "velocityX": -1.8120384806234937,
+            "velocityY": -2.912215572257732,
+            "timestamp": 1.1407054265451462
+        },
+        {
+            "x": 4.388098262013575,
+            "y": 5.795527878516759,
+            "heading": -0.12496957799500227,
+            "angularVelocity": 1.0377211596504115,
+            "velocityX": -1.2615590784949995,
+            "velocityY": -2.245828157007679,
+            "timestamp": 1.2444059198674322
+        },
+        {
+            "x": 4.304052100915554,
+            "y": 5.638778154553602,
+            "heading": -0.0435889655527213,
+            "angularVelocity": 0.7847659141732163,
+            "velocityX": -0.8104702148022851,
+            "velocityY": -1.5115619891604988,
+            "timestamp": 1.3481064131897182
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 7.875266725539043e-28,
+            "angularVelocity": 0.42033518025077415,
+            "velocityX": -0.39587179964482383,
+            "velocityY": -0.7596700076322139,
+            "timestamp": 1.4518069065120043
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": -1.3117957600781845e-26,
+            "velocityX": -2.8963805377195657e-28,
+            "velocityY": 0,
+            "timestamp": 1.5555073998342903
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S3.1.traj
new file mode 100644
index 00000000..f3e004b9
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S3.1.traj	
@@ -0,0 +1,265 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 3.237981257278863e-31,
+            "velocityX": 3.6994687797817426e-32,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.663124715083084,
+            "y": 7.408544235319849,
+            "heading": 0.027485198347684953,
+            "angularVelocity": 0.3204918925418427,
+            "velocityX": -0.19677471240300645,
+            "velocityY": -0.6816250119548423,
+            "timestamp": 0.08575941852911824
+        },
+        {
+            "x": 7.629347887601235,
+            "y": 7.291532761279695,
+            "heading": 0.07929730291499706,
+            "angularVelocity": 0.6041564350126787,
+            "velocityX": -0.3938556028149893,
+            "velocityY": -1.3644154315298311,
+            "timestamp": 0.17151883705823648
+        },
+        {
+            "x": 7.578636110956943,
+            "y": 7.115823334781324,
+            "heading": 0.15015093083574435,
+            "angularVelocity": 0.8261906288076145,
+            "velocityX": -0.5913260317532774,
+            "velocityY": -2.048864480566788,
+            "timestamp": 0.25727825558735473
+        },
+        {
+            "x": 7.510961813450331,
+            "y": 6.881225838812792,
+            "heading": 0.22908991381513497,
+            "angularVelocity": 0.9204701283345157,
+            "velocityX": -0.7891179612374983,
+            "velocityY": -2.7355303941208287,
+            "timestamp": 0.34303767411647296
+        },
+        {
+            "x": 7.426596388399054,
+            "y": 6.588221528517866,
+            "heading": 0.2780284674480044,
+            "angularVelocity": 0.5706493172671544,
+            "velocityX": -0.9837453016618997,
+            "velocityY": -3.4165846191627605,
+            "timestamp": 0.4287970926455912
+        },
+        {
+            "x": 7.336364964950643,
+            "y": 6.276599275391096,
+            "heading": 0.2780284905344079,
+            "angularVelocity": 2.691996271350766e-7,
+            "velocityX": -1.0521459333096546,
+            "velocityY": -3.6336796409243712,
+            "timestamp": 0.5145565111747095
+        },
+        {
+            "x": 7.246133490581996,
+            "y": 5.964977037008454,
+            "heading": 0.27802851362075814,
+            "angularVelocity": 2.6919900573505555e-7,
+            "velocityX": -1.0521465270664079,
+            "velocityY": -3.633679469000075,
+            "timestamp": 0.6003159297038277
+        },
+        {
+            "x": 7.155901958204915,
+            "y": 5.653354815422387,
+            "heading": 0.2780285367070952,
+            "angularVelocity": 2.6919885218329885e-7,
+            "velocityX": -1.0521472034753196,
+            "velocityY": -3.633679273143183,
+            "timestamp": 0.686075348232946
+        },
+        {
+            "x": 7.06567006903972,
+            "y": 5.341732697146042,
+            "heading": 0.27802855979342844,
+            "angularVelocity": 2.6919880800587287e-7,
+            "velocityX": -1.052151363812688,
+            "velocityY": -3.6336780684973866,
+            "timestamp": 0.7718347667620643
+        },
+        {
+            "x": 6.941702537617662,
+            "y": 5.041929112902265,
+            "heading": 0.2780285835706332,
+            "angularVelocity": 2.772547333115093e-7,
+            "velocityX": -1.445526725207054,
+            "velocityY": -3.495867735413615,
+            "timestamp": 0.8575941852911826
+        },
+        {
+            "x": 6.763533066668612,
+            "y": 4.770809627708675,
+            "heading": 0.2780286115416881,
+            "angularVelocity": 3.2615723612466423e-7,
+            "velocityX": -2.0775498948673006,
+            "velocityY": -3.1613960290733045,
+            "timestamp": 0.9433536038203009
+        },
+        {
+            "x": 6.537527544905599,
+            "y": 4.538061625407243,
+            "heading": 0.2780286466069666,
+            "angularVelocity": 4.0887962009830865e-7,
+            "velocityX": -2.6353434484432303,
+            "velocityY": -2.713964323608448,
+            "timestamp": 1.0291130223494191
+        },
+        {
+            "x": 6.274440682462666,
+            "y": 4.354088811409382,
+            "heading": 0.26884633764727317,
+            "angularVelocity": -0.10707055991260324,
+            "velocityX": -3.0677314160380695,
+            "velocityY": -2.145219932145364,
+            "timestamp": 1.1148724408785373
+        },
+        {
+            "x": 5.982323590323029,
+            "y": 4.22411616098336,
+            "heading": 0.2547621599051439,
+            "angularVelocity": -0.16422893232822805,
+            "velocityX": -3.40623918806603,
+            "velocityY": -1.5155495764222278,
+            "timestamp": 1.2006318594076555
+        },
+        {
+            "x": 5.6751302181547745,
+            "y": 4.153390532297334,
+            "heading": 0.22346642432676106,
+            "angularVelocity": -0.3649247641266729,
+            "velocityX": -3.582036555716069,
+            "velocityY": -0.8246980902979518,
+            "timestamp": 1.2863912779367737
+        },
+        {
+            "x": 5.366531848907471,
+            "y": 4.105798244476318,
+            "heading": 0.18319904977282217,
+            "angularVelocity": -0.4695388010388101,
+            "velocityX": -3.5984195618411468,
+            "velocityY": -0.5549511486584576,
+            "timestamp": 1.3721506964658918
+        },
+        {
+            "x": 5.15360011207683,
+            "y": 4.041515222282991,
+            "heading": 0.15622154454817708,
+            "angularVelocity": -0.44121073940112593,
+            "velocityX": -3.482448368249538,
+            "velocityY": -1.051333676582719,
+            "timestamp": 1.4332949588681712
+        },
+        {
+            "x": 4.961585993321137,
+            "y": 3.955708543808847,
+            "heading": 0.09940090208560654,
+            "angularVelocity": -0.9292882149552651,
+            "velocityX": -3.1403456548775974,
+            "velocityY": -1.403348001969601,
+            "timestamp": 1.4944392212704505
+        },
+        {
+            "x": 4.792433392014809,
+            "y": 3.849704139398821,
+            "heading": 0.01980990286006859,
+            "angularVelocity": -1.301692032882722,
+            "velocityX": -2.766450925410501,
+            "velocityY": -1.7336770490843985,
+            "timestamp": 1.5555834836727298
+        },
+        {
+            "x": 4.64716063510186,
+            "y": 3.7242164869664776,
+            "heading": -0.07646566712737501,
+            "angularVelocity": -1.5745642551712946,
+            "velocityX": -2.3759016987918233,
+            "velocityY": -2.0523209783239644,
+            "timestamp": 1.6167277460750091
+        },
+        {
+            "x": 4.526373969972362,
+            "y": 3.5797426645365276,
+            "heading": -0.18496768957846801,
+            "angularVelocity": -1.7745250034621045,
+            "velocityX": -1.975437438999914,
+            "velocityY": -2.362835313629761,
+            "timestamp": 1.6778720084772885
+        },
+        {
+            "x": 4.435318488505818,
+            "y": 3.4263170797911884,
+            "heading": -0.3033986089581413,
+            "angularVelocity": -1.936909772506439,
+            "velocityX": -1.489190937777188,
+            "velocityY": -2.5092392763841707,
+            "timestamp": 1.7390162708795678
+        },
+        {
+            "x": 4.37042278768838,
+            "y": 3.2883178140362905,
+            "heading": -0.39683380701899557,
+            "angularVelocity": -1.5281106417823267,
+            "velocityX": -1.0613538910728308,
+            "velocityY": -2.2569454652502787,
+            "timestamp": 1.800160533281847
+        },
+        {
+            "x": 4.3241368483863285,
+            "y": 3.1745112953889034,
+            "heading": -0.4672868225779202,
+            "angularVelocity": -1.1522424638210744,
+            "velocityX": -0.7569956277749857,
+            "velocityY": -1.8612787884925825,
+            "timestamp": 1.8613047956841264
+        },
+        {
+            "x": 4.292271472981756,
+            "y": 3.08778416242541,
+            "heading": -0.5177338397629753,
+            "angularVelocity": -0.8250490757931602,
+            "velocityX": -0.5211507041318677,
+            "velocityY": -1.418401818193496,
+            "timestamp": 1.9224490580864058
+        },
+        {
+            "x": 4.272405555044274,
+            "y": 3.0293931910282037,
+            "heading": -0.5502025950282511,
+            "angularVelocity": -0.5310188395381721,
+            "velocityX": -0.3249024055075056,
+            "velocityY": -0.9549705745576195,
+            "timestamp": 1.983593320488685
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -0.25836283489389134,
+            "velocityX": -0.15382563587721435,
+            "velocityY": -0.48071871134564137,
+            "timestamp": 2.0447375828909644
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 0,
+            "velocityX": 3.3381626772503296e-32,
+            "velocityY": -2.986960154995716e-30,
+            "timestamp": 2.105881845293244
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S4.1.traj
new file mode 100644
index 00000000..6d9a6add
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S4.1.traj	
@@ -0,0 +1,301 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 9.776523821034258e-32,
+            "velocityX": -4.560355515853987e-33,
+            "velocityY": 2.3850698853192112e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 7.67836355889096,
+            "y": 7.400720406813562,
+            "heading": -0.028945523101477338,
+            "angularVelocity": -0.3234212568317029,
+            "velocityX": -0.018284687354286195,
+            "velocityY": -0.7405714954779761,
+            "timestamp": 0.0894978993806123
+        },
+        {
+            "x": 7.675074058245281,
+            "y": 7.268033905727148,
+            "heading": -0.0827237859595526,
+            "angularVelocity": -0.6008885485609575,
+            "velocityX": -0.03675505982201489,
+            "velocityY": -1.4825655351097347,
+            "timestamp": 0.1789957987612246
+        },
+        {
+            "x": 7.670107822881543,
+            "y": 7.068767684711926,
+            "heading": -0.1543450175390465,
+            "angularVelocity": -0.8002560068466026,
+            "velocityX": -0.05548996566520156,
+            "velocityY": -2.226490480718353,
+            "timestamp": 0.2684936981418369
+        },
+        {
+            "x": 7.663417838093003,
+            "y": 6.802742204879413,
+            "heading": -0.22912917227010834,
+            "angularVelocity": -0.8355967597967521,
+            "velocityX": -0.07475018782384167,
+            "velocityY": -2.9724214945109684,
+            "timestamp": 0.3579915975224492
+        },
+        {
+            "x": 7.654753648982881,
+            "y": 6.471615180012679,
+            "heading": -0.2502157544796387,
+            "angularVelocity": -0.23560980040272358,
+            "velocityX": -0.0968088543986332,
+            "velocityY": -3.6998301318619107,
+            "timestamp": 0.4474894969030615
+        },
+        {
+            "x": 7.646619499365586,
+            "y": 6.133147678353669,
+            "heading": -0.250215779137968,
+            "angularVelocity": -2.7551852588992195e-7,
+            "velocityX": -0.09088648642693918,
+            "velocityY": -3.78184855735655,
+            "timestamp": 0.5369873962836738
+        },
+        {
+            "x": 7.638485287213415,
+            "y": 5.7946801781975115,
+            "heading": -0.25021580379628167,
+            "angularVelocity": -2.755183512776458e-7,
+            "velocityX": -0.09088718515702116,
+            "velocityY": -3.7818485405645093,
+            "timestamp": 0.626485295664286
+        },
+        {
+            "x": 7.630351002745582,
+            "y": 5.456212679779282,
+            "heading": -0.2502158284545949,
+            "angularVelocity": -2.7551834628402704e-7,
+            "velocityX": -0.09088799317222593,
+            "velocityY": -3.781848521145859,
+            "timestamp": 0.7159831950448983
+        },
+        {
+            "x": 7.622216641973054,
+            "y": 5.117745183194865,
+            "heading": -0.25021585311291117,
+            "angularVelocity": -2.7551838019723297e-7,
+            "velocityX": -0.09088884575864668,
+            "velocityY": -3.781848500655846,
+            "timestamp": 0.8054810944255105
+        },
+        {
+            "x": 7.614082202627753,
+            "y": 4.779277688498788,
+            "heading": -0.25021587777122717,
+            "angularVelocity": -2.755183772950637e-7,
+            "velocityX": -0.09088972368733664,
+            "velocityY": -3.7818484795565834,
+            "timestamp": 0.8949789938061228
+        },
+        {
+            "x": 7.605947683187911,
+            "y": 4.440810195727643,
+            "heading": -0.2502159024295383,
+            "angularVelocity": -2.755183223713384e-7,
+            "velocityX": -0.09089061861940764,
+            "velocityY": -3.7818484580484664,
+            "timestamp": 0.984476893186735
+        },
+        {
+            "x": 7.597813082423135,
+            "y": 4.1023427049110195,
+            "heading": -0.25021592708784146,
+            "angularVelocity": -2.755182337798815e-7,
+            "velocityX": -0.09089152729921718,
+            "velocityY": -3.781848436209727,
+            "timestamp": 1.0739747925673473
+        },
+        {
+            "x": 7.589678399478048,
+            "y": 3.7638752160694957,
+            "heading": -0.2502159517461411,
+            "angularVelocity": -2.755181940361221e-7,
+            "velocityX": -0.09089244553653594,
+            "velocityY": -3.781848414141052,
+            "timestamp": 1.1634726919479597
+        },
+        {
+            "x": 7.581543633206817,
+            "y": 3.4254077292306317,
+            "heading": -0.2502159764044425,
+            "angularVelocity": -2.7551821391136536e-7,
+            "velocityX": -0.09089337657676429,
+            "velocityY": -3.7818483917644334,
+            "timestamp": 1.252970591328572
+        },
+        {
+            "x": 7.573407384740196,
+            "y": 3.0869402780182864,
+            "heading": -0.2502160010627749,
+            "angularVelocity": -2.7551856112041726e-7,
+            "velocityX": -0.09090993780780313,
+            "velocityY": -3.7818479936934355,
+            "timestamp": 1.3424684907091844
+        },
+        {
+            "x": 7.507089727245593,
+            "y": 2.754933679576005,
+            "heading": -0.25021602759782946,
+            "angularVelocity": -2.964880153822211e-7,
+            "velocityX": -0.7409968049928212,
+            "velocityY": -3.709658000243552,
+            "timestamp": 1.4319663900897968
+        },
+        {
+            "x": 7.3768883522731565,
+            "y": 2.4424052627551904,
+            "heading": -0.2502160612558337,
+            "angularVelocity": -3.7607591242222915e-7,
+            "velocityX": -1.454798111168213,
+            "velocityY": -3.492019577931195,
+            "timestamp": 1.5214642894704091
+        },
+        {
+            "x": 7.1878697455519145,
+            "y": 2.1615167326419513,
+            "heading": -0.2502161148657548,
+            "angularVelocity": -5.990075908840489e-7,
+            "velocityX": -2.111989309574672,
+            "velocityY": -3.1384929932119427,
+            "timestamp": 1.6109621888510215
+        },
+        {
+            "x": 6.94738938516625,
+            "y": 1.9231986374023808,
+            "heading": -0.25021645217340494,
+            "angularVelocity": -0.000003768889018580663,
+            "velocityX": -2.686994466350113,
+            "velocityY": -2.6628345121941095,
+            "timestamp": 1.7004600882316339
+        },
+        {
+            "x": 6.667655944824219,
+            "y": 1.738860011100769,
+            "heading": -0.25893161706240975,
+            "angularVelocity": -0.09737842954215001,
+            "velocityX": -3.125586659329227,
+            "velocityY": -2.059697798242895,
+            "timestamp": 1.7899579876122462
+        },
+        {
+            "x": 6.480451878153193,
+            "y": 1.646736643588724,
+            "heading": -0.268441983388845,
+            "angularVelocity": -0.16928620019223647,
+            "velocityX": -3.3322654479868237,
+            "velocityY": -1.6398122111953808,
+            "timestamp": 1.846137205502623
+        },
+        {
+            "x": 6.284980922842322,
+            "y": 1.5795583069683912,
+            "heading": -0.28291661109989463,
+            "angularVelocity": -0.25765092955355146,
+            "velocityX": -3.47941752575298,
+            "velocityY": -1.1957862558966221,
+            "timestamp": 1.9023164233929999
+        },
+        {
+            "x": 6.086191677834544,
+            "y": 1.5383458560749974,
+            "heading": -0.3070406119029933,
+            "angularVelocity": -0.429411474723066,
+            "velocityX": -3.53848366838567,
+            "velocityY": -0.7335889042423714,
+            "timestamp": 1.9584956412833767
+        },
+        {
+            "x": 5.891572555778559,
+            "y": 1.5225106097486452,
+            "heading": -0.35143250880916643,
+            "angularVelocity": -0.7901836047765775,
+            "velocityX": -3.4642547433776456,
+            "velocityY": -0.2818701811985366,
+            "timestamp": 2.0146748591737533
+        },
+        {
+            "x": 5.713348199791213,
+            "y": 1.5270062964148978,
+            "heading": -0.407385332387855,
+            "angularVelocity": -0.9959701412692558,
+            "velocityX": -3.172425011952234,
+            "velocityY": 0.08002401662169539,
+            "timestamp": 2.07085407706413
+        },
+        {
+            "x": 5.558872974287929,
+            "y": 1.5428236796222032,
+            "heading": -0.4653888399418059,
+            "angularVelocity": -1.0324726781908529,
+            "velocityX": -2.7496862951120815,
+            "velocityY": 0.2815522145247918,
+            "timestamp": 2.1270332949545065
+        },
+        {
+            "x": 5.4303297125098124,
+            "y": 1.5629685460150702,
+            "heading": -0.519265179295127,
+            "angularVelocity": -0.9590083553396718,
+            "velocityX": -2.2880927610801525,
+            "velocityY": 0.35858217948453525,
+            "timestamp": 2.183212512844883
+        },
+        {
+            "x": 5.3280145107455015,
+            "y": 1.583083454211334,
+            "heading": -0.5654730488836437,
+            "angularVelocity": -0.8225082392332849,
+            "velocityX": -1.8212286608183241,
+            "velocityY": 0.3580489182942135,
+            "timestamp": 2.2393917307352598
+        },
+        {
+            "x": 5.251723840078425,
+            "y": 1.6004232571954151,
+            "heading": -0.6018676415285119,
+            "angularVelocity": -0.6478301765590008,
+            "velocityX": -1.3579874112157035,
+            "velocityY": 0.3086515554188953,
+            "timestamp": 2.2955709486256364
+        },
+        {
+            "x": 5.20115218978415,
+            "y": 1.613158164546165,
+            "heading": -0.6270407807035692,
+            "angularVelocity": -0.44808632302748147,
+            "velocityX": -0.9001843064628601,
+            "velocityY": 0.22668359989631479,
+            "timestamp": 2.351750166516013
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -0.2306763921441399,
+            "velocityX": -0.44771342016952703,
+            "velocityY": 0.12178587938311458,
+            "timestamp": 2.4079293844063896
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 3.4924263364045203e-31,
+            "velocityX": 0,
+            "velocityY": 4.738614736798697e-32,
+            "timestamp": 2.464108602296766
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.1.traj
new file mode 100644
index 00000000..1a381a4d
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 3.136513796099116e-33,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.613270105003341,
+            "y": 7.458186274184093,
+            "heading": 0.022686687743237347,
+            "angularVelocity": 0.25194782208363986,
+            "velocityX": -0.7410712353675238,
+            "velocityY": -0.0978811472565877,
+            "timestamp": 0.09004518298914281
+        },
+        {
+            "x": 7.479732366880497,
+            "y": 7.440537508755927,
+            "heading": 0.06475240251532689,
+            "angularVelocity": 0.4671622998107697,
+            "velocityX": -1.483008126475186,
+            "velocityY": -0.19599899564082535,
+            "timestamp": 0.18009036597828562
+        },
+        {
+            "x": 7.2792817573683966,
+            "y": 7.414022951557291,
+            "heading": 0.12053408331695856,
+            "angularVelocity": 0.6194854510802376,
+            "velocityX": -2.226111412714547,
+            "velocityY": -0.2944583632178676,
+            "timestamp": 0.2701355489674284
+        },
+        {
+            "x": 7.01181645042657,
+            "y": 7.378594749595234,
+            "heading": 0.17807816149187164,
+            "angularVelocity": 0.6390578181383835,
+            "velocityX": -2.9703455316879728,
+            "velocityY": -0.39344916391950197,
+            "timestamp": 0.36018073195657124
+        },
+        {
+            "x": 6.678421151155383,
+            "y": 7.334256434894407,
+            "heading": 0.1920911587056688,
+            "angularVelocity": 0.15562184170903165,
+            "velocityX": -3.7025334193766524,
+            "velocityY": -0.49240073959507324,
+            "timestamp": 0.45022591494571407
+        },
+        {
+            "x": 6.340697514229505,
+            "y": 7.289811670135368,
+            "heading": 0.1920911781237931,
+            "angularVelocity": 2.1564867368321945e-7,
+            "velocityX": -3.7506019279965175,
+            "velocityY": -0.49358292452353475,
+            "timestamp": 0.5402710979348568
+        },
+        {
+            "x": 6.002973877037569,
+            "y": 7.245366907398008,
+            "heading": 0.19209119754191162,
+            "angularVelocity": 2.1564861007875057e-7,
+            "velocityX": -3.750601930951228,
+            "velocityY": -0.49358290207171196,
+            "timestamp": 0.6303162809239996
+        },
+        {
+            "x": 5.665250239845632,
+            "y": 7.200922144660658,
+            "heading": 0.19209121696003015,
+            "angularVelocity": 2.1564860966767442e-7,
+            "velocityX": -3.7506019309512406,
+            "velocityY": -0.493582902071613,
+            "timestamp": 0.7203614639131424
+        },
+        {
+            "x": 5.327526602653695,
+            "y": 7.156477381923307,
+            "heading": 0.19209123637814873,
+            "angularVelocity": 2.1564861026621434e-7,
+            "velocityX": -3.7506019309512415,
+            "velocityY": -0.493582902071613,
+            "timestamp": 0.8104066469022851
+        },
+        {
+            "x": 4.989802965461758,
+            "y": 7.112032619185957,
+            "heading": 0.19209125579626718,
+            "angularVelocity": 2.156486089426075e-7,
+            "velocityX": -3.7506019309512415,
+            "velocityY": -0.49358290207161315,
+            "timestamp": 0.9004518298914279
+        },
+        {
+            "x": 4.652079328269822,
+            "y": 7.067587856448606,
+            "heading": 0.19209127521438563,
+            "angularVelocity": 2.156486089612396e-7,
+            "velocityX": -3.750601930951241,
+            "velocityY": -0.4935829020716131,
+            "timestamp": 0.9904970128805707
+        },
+        {
+            "x": 4.314355691077885,
+            "y": 7.023143093711255,
+            "heading": 0.19209129463250418,
+            "angularVelocity": 2.1564860999442945e-7,
+            "velocityX": -3.7506019309512415,
+            "velocityY": -0.4935829020716131,
+            "timestamp": 1.0805421958697135
+        },
+        {
+            "x": 3.976632053885947,
+            "y": 6.978698330973905,
+            "heading": 0.1920913140506227,
+            "angularVelocity": 2.1564860984546372e-7,
+            "velocityX": -3.750601930951241,
+            "velocityY": -0.49358290207161315,
+            "timestamp": 1.1705873788588563
+        },
+        {
+            "x": 3.638908416694012,
+            "y": 6.934253568236545,
+            "heading": 0.19209133346874124,
+            "angularVelocity": 2.1564860987115903e-7,
+            "velocityX": -3.750601930951227,
+            "velocityY": -0.4935829020717237,
+            "timestamp": 1.2606325618479992
+        },
+        {
+            "x": 3.3011847797995815,
+            "y": 6.8898088032385445,
+            "heading": 0.19209135288686577,
+            "angularVelocity": 2.1564867640744468e-7,
+            "velocityX": -3.7506019276472724,
+            "velocityY": -0.4935829271773418,
+            "timestamp": 1.350677744837142
+        },
+        {
+            "x": 2.967975868183031,
+            "y": 6.845440652800829,
+            "heading": 0.2065131865620181,
+            "angularVelocity": 0.16016218965195775,
+            "velocityX": -3.7004634846122446,
+            "velocityY": -0.492732081438091,
+            "timestamp": 1.440722927826285
+        },
+        {
+            "x": 2.7005740636525677,
+            "y": 6.810001078107782,
+            "heading": 0.266815731322117,
+            "angularVelocity": 0.6696920674520688,
+            "velocityX": -2.9696403033875236,
+            "velocityY": -0.39357546419024264,
+            "timestamp": 1.5307681108154279
+        },
+        {
+            "x": 2.5001864407535797,
+            "y": 6.78347721885534,
+            "heading": 0.32524318999567287,
+            "angularVelocity": 0.6488682318586739,
+            "velocityX": -2.2254119126300136,
+            "velocityY": -0.29456166750908425,
+            "timestamp": 1.6208132938045707
+        },
+        {
+            "x": 2.366700230698382,
+            "y": 6.765819830273621,
+            "heading": 0.3692681816828941,
+            "angularVelocity": 0.48892111966199864,
+            "velocityX": -1.4824358796771253,
+            "velocityY": -0.19609476038099796,
+            "timestamp": 1.7108584767937136
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0.26355455704906927,
+            "velocityX": -0.740741797442122,
+            "velocityY": -0.09794894053004785,
+            "timestamp": 1.8009036597828565
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0,
+            "velocityX": 6.927431243507319e-35,
+            "velocityY": 0,
+            "timestamp": 1.8909488427719994
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.1.traj
new file mode 100644
index 00000000..28a53b93
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.1.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -2.4374604803646449e-32,
+            "velocityY": 2.350988701644575e-38,
+            "timestamp": 0
+        },
+        {
+            "x": 7.602872461914855,
+            "y": 7.439913035887961,
+            "heading": 5.3325436298288445e-18,
+            "angularVelocity": 5.3856284215509377e-17,
+            "velocityX": -0.7789533333445217,
+            "velocityY": -0.2735661154121556,
+            "timestamp": 0.09901432445765257
+        },
+        {
+            "x": 7.448617388685645,
+            "y": 7.385739108696781,
+            "heading": -4.4022234029758545e-18,
+            "angularVelocity": -9.831675377471393e-17,
+            "velocityX": -1.557906636985473,
+            "velocityY": -0.5471322203925048,
+            "timestamp": 0.19802864891530514
+        },
+        {
+            "x": 7.217234786194527,
+            "y": 7.304478220492258,
+            "heading": -7.053417683545123e-18,
+            "angularVelocity": -2.6775864730895626e-17,
+            "velocityX": -2.3368598812192953,
+            "velocityY": -0.8206983045092451,
+            "timestamp": 0.2970429733729577
+        },
+        {
+            "x": 6.9087246720879545,
+            "y": 7.196130377471774,
+            "heading": 2.6673179344101e-18,
+            "angularVelocity": 9.817504407722252e-17,
+            "velocityX": -3.1158129472318725,
+            "velocityY": -1.0942643260352094,
+            "timestamp": 0.3960572978306103
+        },
+        {
+            "x": 6.5553201862538,
+            "y": 7.072015763386003,
+            "heading": 8.41895708879283e-18,
+            "angularVelocity": 5.808896021749495e-17,
+            "velocityX": -3.5692258445423475,
+            "velocityY": -1.2535015995473784,
+            "timestamp": 0.49507162228826285
+        },
+        {
+            "x": 6.2019157004196215,
+            "y": 6.9479011493002245,
+            "heading": -3.69857396739816e-19,
+            "angularVelocity": -8.876306064099344e-17,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.2535015995474599,
+            "timestamp": 0.5940859467459154
+        },
+        {
+            "x": 5.848511214585444,
+            "y": 6.823786535214446,
+            "heading": -8.032367976884792e-18,
+            "angularVelocity": -7.738789939855791e-17,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.2535015995474599,
+            "timestamp": 0.693100271203568
+        },
+        {
+            "x": 5.495106728751266,
+            "y": 6.699671921128667,
+            "heading": -3.490709714785344e-17,
+            "angularVelocity": -2.714226382747745e-16,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.2535015995474599,
+            "timestamp": 0.7921145956612206
+        },
+        {
+            "x": 5.141702242917089,
+            "y": 6.575557307042889,
+            "heading": -1.2741337636064699e-17,
+            "angularVelocity": 2.238641694845762e-16,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.2535015995474599,
+            "timestamp": 0.8911289201188731
+        },
+        {
+            "x": 4.788297757082911,
+            "y": 6.4514426929571105,
+            "heading": -2.2645871045802615e-17,
+            "angularVelocity": -1.0003131833516608e-16,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.2535015995474597,
+            "timestamp": 0.9901432445765257
+        },
+        {
+            "x": 4.4348932712487334,
+            "y": 6.327328078871332,
+            "heading": -7.505924377434873e-18,
+            "angularVelocity": 1.529066299375997e-16,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.25350159954746,
+            "timestamp": 1.0891575690341782
+        },
+        {
+            "x": 4.081488785414556,
+            "y": 6.203213464785553,
+            "heading": -3.3249179069580955e-17,
+            "angularVelocity": -2.5999525658031657e-16,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.25350159954746,
+            "timestamp": 1.1881718934918308
+        },
+        {
+            "x": 3.728084299580378,
+            "y": 6.079098850699775,
+            "heading": -3.31250701341244e-17,
+            "angularVelocity": 1.2534442479544336e-18,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.2535015995474597,
+            "timestamp": 1.2871862179494835
+        },
+        {
+            "x": 3.3746798137461993,
+            "y": 5.9549842366139965,
+            "heading": -3.474935359347948e-17,
+            "angularVelocity": -1.6404530025230332e-17,
+            "velocityX": -3.569225844542579,
+            "velocityY": -1.2535015995474599,
+            "timestamp": 1.3862005424071362
+        },
+        {
+            "x": 3.021275327912045,
+            "y": 5.830869622528225,
+            "heading": -2.4652412430964293e-17,
+            "angularVelocity": 1.0197454982216833e-16,
+            "velocityX": -3.5692258445423475,
+            "velocityY": -1.2535015995473784,
+            "timestamp": 1.4852148668647889
+        },
+        {
+            "x": 2.7127652138054734,
+            "y": 5.722521779507741,
+            "heading": 1.414690860074665e-18,
+            "angularVelocity": 2.632659789801892e-16,
+            "velocityX": -3.115812947231872,
+            "velocityY": -1.0942643260352094,
+            "timestamp": 1.5842291913224416
+        },
+        {
+            "x": 2.4813826113143547,
+            "y": 5.641260891303218,
+            "heading": 8.307803002216867e-18,
+            "angularVelocity": 6.961732115429119e-17,
+            "velocityX": -2.336859881219295,
+            "velocityY": -0.8206983045092451,
+            "timestamp": 1.6832435157800942
+        },
+        {
+            "x": 2.3271275380851444,
+            "y": 5.587086964112038,
+            "heading": -3.972937436005726e-18,
+            "angularVelocity": -1.2402993582700068e-16,
+            "velocityX": -1.557906636985473,
+            "velocityY": -0.5471322203925048,
+            "timestamp": 1.782257840237747
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 4.012487544797503e-17,
+            "velocityX": -0.7789533333445219,
+            "velocityY": -0.2735661154121556,
+            "timestamp": 1.8812721646953996
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -5.265297803216144e-35,
+            "velocityY": 0,
+            "timestamp": 1.9802864891530523
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.1.traj
new file mode 100644
index 00000000..d8ed1eba
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.1.traj	
@@ -0,0 +1,346 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": -3.2387949852399556e-31,
+            "velocityX": -1.054254683790836e-34,
+            "velocityY": 3.285843466136746e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 7.625901434953002,
+            "y": 7.454866143797358,
+            "heading": -0.004441360542181856,
+            "angularVelocity": -0.05445952619049241,
+            "velocityX": -0.6633512843785839,
+            "velocityY": -0.14878415147416332,
+            "timestamp": 0.08155341871038292
+        },
+        {
+            "x": 7.517704306524476,
+            "y": 7.430598431564336,
+            "heading": -0.01332401825146925,
+            "angularVelocity": -0.10891827528203153,
+            "velocityX": -1.3267025483353667,
+            "velocityY": -0.29756830083607516,
+            "timestamp": 0.16310683742076584
+        },
+        {
+            "x": 7.355408617374589,
+            "y": 7.3941968638363065,
+            "heading": -0.02664786051722782,
+            "angularVelocity": -0.1633756435530466,
+            "velocityX": -1.9900537796734206,
+            "velocityY": -0.4463524436333081,
+            "timestamp": 0.24466025613114878
+        },
+        {
+            "x": 7.139014373014635,
+            "y": 7.345661441877259,
+            "heading": -0.044412717144295275,
+            "angularVelocity": -0.21783092490771921,
+            "velocityX": -2.653404943432558,
+            "velocityY": -0.5951365709316123,
+            "timestamp": 0.3262136748415317
+        },
+        {
+            "x": 6.868521591139737,
+            "y": 7.284992169702681,
+            "heading": -0.06661833727396635,
+            "angularVelocity": -0.2722831302576181,
+            "velocityX": -3.3167558902158003,
+            "velocityY": -0.7439206489924082,
+            "timestamp": 0.4077670935519146
+        },
+        {
+            "x": 6.5674889621165065,
+            "y": 7.217472965131769,
+            "heading": -0.06661834019422361,
+            "angularVelocity": -3.5807907402848094e-8,
+            "velocityX": -3.691232492561401,
+            "velocityY": -0.827913846391786,
+            "timestamp": 0.4893205122622975
+        },
+        {
+            "x": 6.266456335099212,
+            "y": 7.149953751617259,
+            "heading": -0.06661834311445712,
+            "angularVelocity": -3.5807616132797033e-8,
+            "velocityX": -3.6912324679648125,
+            "velocityY": -0.8279139560572942,
+            "timestamp": 0.5708739309726805
+        },
+        {
+            "x": 5.9654237103471415,
+            "y": 7.082434528003299,
+            "heading": -0.06661834603468975,
+            "angularVelocity": -3.5807605171686195e-8,
+            "velocityX": -3.6912324401888563,
+            "velocityY": -0.8279140798957559,
+            "timestamp": 0.6524273496830634
+        },
+        {
+            "x": 5.664391088171292,
+            "y": 7.014915292903322,
+            "heading": -0.06661834895492107,
+            "angularVelocity": -3.580758928944026e-8,
+            "velocityX": -3.691232408599494,
+            "velocityY": -0.8279142207361639,
+            "timestamp": 0.7339807683934463
+        },
+        {
+            "x": 5.363358468935584,
+            "y": 6.947396044694795,
+            "heading": -0.06661835187515067,
+            "angularVelocity": -3.580756830329485e-8,
+            "velocityX": -3.6912323725477565,
+            "velocityY": -0.8279143814719224,
+            "timestamp": 0.8155341871038292
+        },
+        {
+            "x": 5.062325853062933,
+            "y": 6.879876781492171,
+            "heading": -0.06661835479537813,
+            "angularVelocity": -3.58075419139389e-8,
+            "velocityX": -3.691232331310291,
+            "velocityY": -0.8279145653280606,
+            "timestamp": 0.8970876058142121
+        },
+        {
+            "x": 4.761293241038404,
+            "y": 6.812357501132806,
+            "heading": -0.06661835771560275,
+            "angularVelocity": -3.580750706252144e-8,
+            "velocityX": -3.691232284125005,
+            "velocityY": -0.8279147757023306,
+            "timestamp": 0.978641024524595
+        },
+        {
+            "x": 4.46026063340559,
+            "y": 6.744838201193102,
+            "heading": -0.06661836063582369,
+            "angularVelocity": -3.580746208542298e-8,
+            "velocityX": -3.6912322302742044,
+            "velocityY": -0.8279150157945213,
+            "timestamp": 1.060194443234978
+        },
+        {
+            "x": 4.159228030751222,
+            "y": 6.677318879057164,
+            "heading": -0.06661836355604021,
+            "angularVelocity": -3.5807407853670686e-8,
+            "velocityX": -3.691232169229015,
+            "velocityY": -0.8279152879625719,
+            "timestamp": 1.141747861945361
+        },
+        {
+            "x": 3.8581954336724973,
+            "y": 6.609799532062401,
+            "heading": -0.06661836647625141,
+            "angularVelocity": -3.580734253915704e-8,
+            "velocityX": -3.6912321008610123,
+            "velocityY": -0.8279155927790207,
+            "timestamp": 1.223301280655744
+        },
+        {
+            "x": 3.557162842693998,
+            "y": 6.542280157870002,
+            "heading": -0.0666183693964589,
+            "angularVelocity": -3.580729730790545e-8,
+            "velocityX": -3.691232026060653,
+            "velocityY": -0.8279159262737572,
+            "timestamp": 1.304854699366127
+        },
+        {
+            "x": 3.256130258261278,
+            "y": 6.474760754493496,
+            "heading": -0.06661837231666672,
+            "angularVelocity": -3.580730095676624e-8,
+            "velocityX": -3.6912319457969542,
+            "velocityY": -0.8279162841264173,
+            "timestamp": 1.38640811807651
+        },
+        {
+            "x": 2.9550976753234863,
+            "y": 6.407241344451904,
+            "heading": -0.06661837523687889,
+            "angularVelocity": -3.580735442493409e-8,
+            "velocityX": -3.6912319274662786,
+            "velocityY": -0.8279163658530455,
+            "timestamp": 1.467961536786893
+        },
+        {
+            "x": 2.779035844872165,
+            "y": 6.36860208155972,
+            "heading": -0.0666183783001983,
+            "angularVelocity": -6.428976142053906e-8,
+            "velocityX": -3.695002547647375,
+            "velocityY": -0.8109206547436999,
+            "timestamp": 1.5156101725269335
+        },
+        {
+            "x": 2.608054120329012,
+            "y": 6.311726454658268,
+            "heading": -0.06677425221357433,
+            "angularVelocity": -0.0032713195629861798,
+            "velocityX": -3.5883865694652988,
+            "velocityY": -1.1936464920308683,
+            "timestamp": 1.563258808266974
+        },
+        {
+            "x": 2.4510058546542055,
+            "y": 6.242296424291011,
+            "heading": -0.07609605863752185,
+            "angularVelocity": -0.19563637614744786,
+            "velocityX": -3.29596562914219,
+            "velocityY": -1.457125252146435,
+            "timestamp": 1.6109074440070146
+        },
+        {
+            "x": 2.3100257060565728,
+            "y": 6.162946485717711,
+            "heading": -0.09018973905193617,
+            "angularVelocity": -0.2957835034721525,
+            "velocityX": -2.9587447029271896,
+            "velocityY": -1.6653139663050547,
+            "timestamp": 1.658556079747055
+        },
+        {
+            "x": 2.1858070710130613,
+            "y": 6.074835566620609,
+            "heading": -0.10721183288872499,
+            "angularVelocity": -0.3572419980708675,
+            "velocityX": -2.6069714927652616,
+            "velocityY": -1.8491803118526937,
+            "timestamp": 1.7062047154870956
+        },
+        {
+            "x": 2.078679040228791,
+            "y": 5.978601109574319,
+            "heading": -0.12617474237211068,
+            "angularVelocity": -0.3979738179094536,
+            "velocityX": -2.248291669223545,
+            "velocityY": -2.0196686757492435,
+            "timestamp": 1.7538533512271361
+        },
+        {
+            "x": 1.9888310933691378,
+            "y": 5.874644187394139,
+            "heading": -0.14646656775118436,
+            "angularVelocity": -0.42586372231046465,
+            "velocityX": -1.8856352435727148,
+            "velocityY": -2.181739740617511,
+            "timestamp": 1.8015019869671767
+        },
+        {
+            "x": 1.9163856506347656,
+            "y": 5.763239860534668,
+            "heading": -0.16767175460275374,
+            "angularVelocity": -0.44503240276194317,
+            "velocityX": -1.5204095900989456,
+            "velocityY": -2.3380381227986136,
+            "timestamp": 1.8491506227072172
+        },
+        {
+            "x": 1.8485889050766984,
+            "y": 5.589343182132367,
+            "heading": -0.1992083750464638,
+            "angularVelocity": -0.46340502818407264,
+            "velocityX": -0.9962181217957636,
+            "velocityY": -2.555270476750044,
+            "timestamp": 1.9172047406506962
+        },
+        {
+            "x": 1.817164507457459,
+            "y": 5.402475290970386,
+            "heading": -0.23166704244574374,
+            "angularVelocity": -0.47695375945076895,
+            "velocityX": -0.4617560049104581,
+            "velocityY": -2.7458719150120894,
+            "timestamp": 1.9852588585941753
+        },
+        {
+            "x": 1.8230483770902466,
+            "y": 5.205640044676734,
+            "heading": -0.2645113992669886,
+            "angularVelocity": -0.4826211523089798,
+            "velocityX": 0.08645868627131759,
+            "velocityY": -2.8923341047058035,
+            "timestamp": 2.0533129765376543
+        },
+        {
+            "x": 1.8673165212991787,
+            "y": 5.004575832748109,
+            "heading": -0.29674148680613366,
+            "angularVelocity": -0.4735949640250024,
+            "velocityX": 0.6504844313118886,
+            "velocityY": -2.9544753205907597,
+            "timestamp": 2.1213670944811334
+        },
+        {
+            "x": 1.9491517735306563,
+            "y": 4.812420400099087,
+            "heading": -0.3261812067352161,
+            "angularVelocity": -0.43259277790693457,
+            "velocityX": 1.202502577425556,
+            "velocityY": -2.823568043429816,
+            "timestamp": 2.1894212124246124
+        },
+        {
+            "x": 2.050948548896826,
+            "y": 4.653276186897604,
+            "heading": -0.34937275798086526,
+            "angularVelocity": -0.34078101291423535,
+            "velocityX": 1.4958209501847175,
+            "velocityY": -2.338494980328238,
+            "timestamp": 2.2574753303680914
+        },
+        {
+            "x": 2.14520275900139,
+            "y": 4.5319666650276815,
+            "heading": -0.36647677931046935,
+            "angularVelocity": -0.2513297041615066,
+            "velocityX": 1.3849890785871777,
+            "velocityY": -1.7825449147790617,
+            "timestamp": 2.3255294483115705
+        },
+        {
+            "x": 2.2207736095496604,
+            "y": 4.444424274499807,
+            "heading": -0.37856224680514944,
+            "angularVelocity": -0.17758613086130492,
+            "velocityX": 1.1104522816836297,
+            "velocityY": -1.2863643402237204,
+            "timestamp": 2.3935835662550495
+        },
+        {
+            "x": 2.273137984258771,
+            "y": 4.387723780671385,
+            "heading": -0.38627299411990695,
+            "angularVelocity": -0.113303170297753,
+            "velocityX": 0.7694519639887757,
+            "velocityY": -0.8331677133117752,
+            "timestamp": 2.4616376841985286
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.054765324902410414,
+            "velocityX": 0.3947155080835744,
+            "velocityY": -0.4073784439379148,
+            "timestamp": 2.5296918021420076
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 3.1303206075717874e-30,
+            "velocityX": 0,
+            "velocityY": -1.5697460792586675e-31,
+            "timestamp": 2.5977459200854867
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.traj
index 96b5b24b..d8ed1eba 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.traj	
@@ -4,271 +4,343 @@
             "x": 7.68,
             "y": 7.467,
             "heading": 0,
-            "angularVelocity": -1.8418609272007183e-28,
-            "velocityX": -1.1316944174618986e-32,
-            "velocityY": -1.0903166764521522e-29,
+            "angularVelocity": -3.2387949852399556e-31,
+            "velocityX": -1.054254683790836e-34,
+            "velocityY": 3.285843466136746e-33,
             "timestamp": 0
         },
         {
-            "x": 7.615058283165484,
-            "y": 7.4500526040017725,
-            "heading": -0.011528934363875379,
-            "angularVelocity": -0.12842736126317786,
-            "velocityX": -0.7234227436435784,
-            "velocityY": -0.18878668917675853,
-            "timestamp": 0.08977007898235294
-        },
-        {
-            "x": 7.485153650546927,
-            "y": 7.41615410854214,
-            "heading": -0.0328638632229319,
-            "angularVelocity": -0.23766191476385995,
-            "velocityX": -1.4470816344507456,
-            "velocityY": -0.3776146333375848,
-            "timestamp": 0.17954015796470588
-        },
-        {
-            "x": 7.290257872847477,
-            "y": 7.365300387326372,
-            "heading": -0.06109799257845879,
-            "angularVelocity": -0.3145160355888266,
-            "velocityX": -2.17105498746146,
-            "velocityY": -0.5664885426438214,
-            "timestamp": 0.2693102369470588
-        },
-        {
-            "x": 7.030342832843462,
-            "y": 7.297491054945299,
-            "heading": -0.0902650461777,
-            "angularVelocity": -0.32490840968750134,
-            "velocityX": -2.895341554228826,
-            "velocityY": -0.7553667452421856,
-            "timestamp": 0.35908031592941175
-        },
-        {
-            "x": 6.705594231461252,
-            "y": 7.212800171241254,
-            "heading": -0.10097705422378679,
-            "angularVelocity": -0.11932715407194204,
-            "velocityX": -3.617559492690666,
-            "velocityY": -0.9434199531077018,
-            "timestamp": 0.4488503949117647
-        },
-        {
-            "x": 6.377014064882571,
-            "y": 7.1270111914036525,
-            "heading": -0.10097706428057987,
-            "angularVelocity": -1.120283753063034e-7,
-            "velocityX": -3.6602414780458985,
-            "velocityY": -0.9556522708907337,
-            "timestamp": 0.5386204738941176
-        },
-        {
-            "x": 6.048433901032778,
-            "y": 7.0412222011141505,
-            "heading": -0.10097707433737516,
-            "angularVelocity": -1.1202840770590317e-7,
-            "velocityX": -3.660241447647229,
-            "velocityY": -0.9556523873204973,
-            "timestamp": 0.6283905528764705
-        },
-        {
-            "x": 5.719853740215168,
-            "y": 6.955433199211206,
-            "heading": -0.10097708439415136,
-            "angularVelocity": -1.1202819821797078e-7,
-            "velocityX": -3.6602414138700015,
-            "velocityY": -0.9556525166893554,
-            "timestamp": 0.7181606318588234
-        },
-        {
-            "x": 5.391273582926602,
-            "y": 6.86964418379178,
-            "heading": -0.100977094450907,
-            "angularVelocity": -1.1202797265919916e-7,
-            "velocityX": -3.660241374557919,
-            "velocityY": -0.9556526672572584,
-            "timestamp": 0.8079307108411763
-        },
-        {
-            "x": 5.062693429765362,
-            "y": 6.783855152564382,
-            "heading": -0.10097710450764096,
-            "angularVelocity": -1.1202773594839044e-7,
-            "velocityX": -3.6602413285812334,
-            "velocityY": -0.9556528433514181,
-            "timestamp": 0.8977007898235292
-        },
-        {
-            "x": 4.734113281444647,
-            "y": 6.698066102797359,
-            "heading": -0.10097711456435234,
-            "angularVelocity": -1.1202748993058552e-7,
-            "velocityX": -3.6602412746598207,
-            "velocityY": -0.9556530498750426,
-            "timestamp": 0.9874708688058821
-        },
-        {
-            "x": 4.4055331387963745,
-            "y": 6.612277031304317,
-            "heading": -0.10097712462104143,
-            "angularVelocity": -1.1202724880866421e-7,
-            "velocityX": -3.660241211471189,
-            "velocityY": -0.9556532918937597,
-            "timestamp": 1.077240947788235
-        },
-        {
-            "x": 4.076953002755506,
-            "y": 6.526487934504167,
-            "heading": -0.10097713467770983,
-            "angularVelocity": -1.1202702659530083e-7,
-            "velocityX": -3.6602411378675055,
-            "velocityY": -0.9556535738043043,
-            "timestamp": 1.167011026770588
-        },
-        {
-            "x": 3.748372874246301,
-            "y": 6.440698808855463,
-            "heading": -0.10097714473442117,
-            "angularVelocity": -1.1202751392033635e-7,
-            "velocityX": -3.6602410539684915,
-            "velocityY": -0.9556538951629009,
-            "timestamp": 1.256781105752941
-        },
-        {
-            "x": 3.4197928479820465,
-            "y": 6.354909291415878,
-            "heading": -0.10097715479114391,
-            "angularVelocity": -1.120276421337113e-7,
-            "velocityX": -3.6602399150000573,
-            "velocityY": -0.9556582595606669,
-            "timestamp": 1.346551184735294
-        },
-        {
-            "x": 3.1047619528127512,
-            "y": 6.228101525660344,
-            "heading": -0.10097716526412484,
-            "angularVelocity": -1.1666457370126234e-7,
-            "velocityX": -3.5093084326123236,
-            "velocityY": -1.412583871971558,
-            "timestamp": 1.436321263717647
-        },
-        {
-            "x": 2.8208656132222933,
-            "y": 6.041748259045348,
-            "heading": -0.1009771779112871,
-            "angularVelocity": -1.4088397962304153e-7,
-            "velocityX": -3.1624828986324123,
-            "velocityY": -2.0758950947495576,
-            "timestamp": 1.5260913427
-        },
-        {
-            "x": 2.5792187192315943,
-            "y": 5.803145197833315,
-            "heading": -0.10097728674036179,
-            "angularVelocity": -0.0000012123108078932896,
-            "velocityX": -2.691842278966871,
-            "velocityY": -2.65793529332427,
-            "timestamp": 1.615861421682353
-        },
-        {
-            "x": 2.3963290188287294,
-            "y": 5.533479989641919,
-            "heading": -0.13644335482170092,
-            "angularVelocity": -0.3950767169047361,
-            "velocityX": -2.037312459521214,
-            "velocityY": -3.003954226707092,
-            "timestamp": 1.705631500664706
-        },
-        {
-            "x": 2.279822826385498,
-            "y": 5.254662990570068,
-            "heading": -0.18962986506951118,
-            "angularVelocity": -0.5924748073105779,
-            "velocityX": -1.2978287839789375,
-            "velocityY": -3.105901233821009,
-            "timestamp": 1.795401579647059
-        },
-        {
-            "x": 2.2328990354823257,
-            "y": 5.0773593431323425,
-            "heading": -0.225508532252886,
-            "angularVelocity": -0.6284306472549296,
-            "velocityX": -0.8218908506157174,
-            "velocityY": -3.1055514229095325,
-            "timestamp": 1.8524940644563022
-        },
-        {
-            "x": 2.2122650091837905,
-            "y": 4.906948206481987,
-            "heading": -0.2618449581428327,
-            "angularVelocity": -0.636448492486558,
-            "velocityX": -0.3614140524419242,
-            "velocityY": -2.9848260628208765,
-            "timestamp": 1.9095865492655453
-        },
-        {
-            "x": 2.2138710973194917,
-            "y": 4.752151212947765,
-            "heading": -0.29639527524079207,
-            "angularVelocity": -0.6051640108688915,
-            "velocityX": 0.02813134059521416,
-            "velocityY": -2.7113374737719043,
-            "timestamp": 1.9666790340747884
-        },
-        {
-            "x": 2.229883400142308,
-            "y": 4.620372877512837,
-            "heading": -0.32691110500805654,
-            "angularVelocity": -0.5344981895462751,
-            "velocityX": 0.280462531565201,
-            "velocityY": -2.308155545781648,
-            "timestamp": 2.0237715188840317
-        },
-        {
-            "x": 2.251883940293385,
-            "y": 4.51506115985361,
-            "heading": -0.3519685302004407,
-            "angularVelocity": -0.4388918309689961,
-            "velocityX": 0.38534914402167997,
-            "velocityY": -1.844581086479138,
-            "timestamp": 2.080864003693275
-        },
-        {
-            "x": 2.273515445313811,
-            "y": 4.436872238113893,
-            "heading": -0.37095335253129014,
-            "angularVelocity": -0.33252751906728534,
-            "velocityX": 0.3788853312819565,
-            "velocityY": -1.3695133781784317,
-            "timestamp": 2.1379564885025184
-        },
-        {
-            "x": 2.2905088224909966,
-            "y": 4.385405157743103,
-            "heading": -0.3836505931421081,
-            "angularVelocity": -0.22239775783770355,
-            "velocityX": 0.2976464806908316,
-            "velocityY": -0.9014685653065743,
-            "timestamp": 2.1950489733117617
+            "x": 7.625901434953002,
+            "y": 7.454866143797358,
+            "heading": -0.004441360542181856,
+            "angularVelocity": -0.05445952619049241,
+            "velocityX": -0.6633512843785839,
+            "velocityY": -0.14878415147416332,
+            "timestamp": 0.08155341871038292
+        },
+        {
+            "x": 7.517704306524476,
+            "y": 7.430598431564336,
+            "heading": -0.01332401825146925,
+            "angularVelocity": -0.10891827528203153,
+            "velocityX": -1.3267025483353667,
+            "velocityY": -0.29756830083607516,
+            "timestamp": 0.16310683742076584
+        },
+        {
+            "x": 7.355408617374589,
+            "y": 7.3941968638363065,
+            "heading": -0.02664786051722782,
+            "angularVelocity": -0.1633756435530466,
+            "velocityX": -1.9900537796734206,
+            "velocityY": -0.4463524436333081,
+            "timestamp": 0.24466025613114878
+        },
+        {
+            "x": 7.139014373014635,
+            "y": 7.345661441877259,
+            "heading": -0.044412717144295275,
+            "angularVelocity": -0.21783092490771921,
+            "velocityX": -2.653404943432558,
+            "velocityY": -0.5951365709316123,
+            "timestamp": 0.3262136748415317
+        },
+        {
+            "x": 6.868521591139737,
+            "y": 7.284992169702681,
+            "heading": -0.06661833727396635,
+            "angularVelocity": -0.2722831302576181,
+            "velocityX": -3.3167558902158003,
+            "velocityY": -0.7439206489924082,
+            "timestamp": 0.4077670935519146
+        },
+        {
+            "x": 6.5674889621165065,
+            "y": 7.217472965131769,
+            "heading": -0.06661834019422361,
+            "angularVelocity": -3.5807907402848094e-8,
+            "velocityX": -3.691232492561401,
+            "velocityY": -0.827913846391786,
+            "timestamp": 0.4893205122622975
+        },
+        {
+            "x": 6.266456335099212,
+            "y": 7.149953751617259,
+            "heading": -0.06661834311445712,
+            "angularVelocity": -3.5807616132797033e-8,
+            "velocityX": -3.6912324679648125,
+            "velocityY": -0.8279139560572942,
+            "timestamp": 0.5708739309726805
+        },
+        {
+            "x": 5.9654237103471415,
+            "y": 7.082434528003299,
+            "heading": -0.06661834603468975,
+            "angularVelocity": -3.5807605171686195e-8,
+            "velocityX": -3.6912324401888563,
+            "velocityY": -0.8279140798957559,
+            "timestamp": 0.6524273496830634
+        },
+        {
+            "x": 5.664391088171292,
+            "y": 7.014915292903322,
+            "heading": -0.06661834895492107,
+            "angularVelocity": -3.580758928944026e-8,
+            "velocityX": -3.691232408599494,
+            "velocityY": -0.8279142207361639,
+            "timestamp": 0.7339807683934463
+        },
+        {
+            "x": 5.363358468935584,
+            "y": 6.947396044694795,
+            "heading": -0.06661835187515067,
+            "angularVelocity": -3.580756830329485e-8,
+            "velocityX": -3.6912323725477565,
+            "velocityY": -0.8279143814719224,
+            "timestamp": 0.8155341871038292
+        },
+        {
+            "x": 5.062325853062933,
+            "y": 6.879876781492171,
+            "heading": -0.06661835479537813,
+            "angularVelocity": -3.58075419139389e-8,
+            "velocityX": -3.691232331310291,
+            "velocityY": -0.8279145653280606,
+            "timestamp": 0.8970876058142121
+        },
+        {
+            "x": 4.761293241038404,
+            "y": 6.812357501132806,
+            "heading": -0.06661835771560275,
+            "angularVelocity": -3.580750706252144e-8,
+            "velocityX": -3.691232284125005,
+            "velocityY": -0.8279147757023306,
+            "timestamp": 0.978641024524595
+        },
+        {
+            "x": 4.46026063340559,
+            "y": 6.744838201193102,
+            "heading": -0.06661836063582369,
+            "angularVelocity": -3.580746208542298e-8,
+            "velocityX": -3.6912322302742044,
+            "velocityY": -0.8279150157945213,
+            "timestamp": 1.060194443234978
+        },
+        {
+            "x": 4.159228030751222,
+            "y": 6.677318879057164,
+            "heading": -0.06661836355604021,
+            "angularVelocity": -3.5807407853670686e-8,
+            "velocityX": -3.691232169229015,
+            "velocityY": -0.8279152879625719,
+            "timestamp": 1.141747861945361
+        },
+        {
+            "x": 3.8581954336724973,
+            "y": 6.609799532062401,
+            "heading": -0.06661836647625141,
+            "angularVelocity": -3.580734253915704e-8,
+            "velocityX": -3.6912321008610123,
+            "velocityY": -0.8279155927790207,
+            "timestamp": 1.223301280655744
+        },
+        {
+            "x": 3.557162842693998,
+            "y": 6.542280157870002,
+            "heading": -0.0666183693964589,
+            "angularVelocity": -3.580729730790545e-8,
+            "velocityX": -3.691232026060653,
+            "velocityY": -0.8279159262737572,
+            "timestamp": 1.304854699366127
+        },
+        {
+            "x": 3.256130258261278,
+            "y": 6.474760754493496,
+            "heading": -0.06661837231666672,
+            "angularVelocity": -3.580730095676624e-8,
+            "velocityX": -3.6912319457969542,
+            "velocityY": -0.8279162841264173,
+            "timestamp": 1.38640811807651
+        },
+        {
+            "x": 2.9550976753234863,
+            "y": 6.407241344451904,
+            "heading": -0.06661837523687889,
+            "angularVelocity": -3.580735442493409e-8,
+            "velocityX": -3.6912319274662786,
+            "velocityY": -0.8279163658530455,
+            "timestamp": 1.467961536786893
+        },
+        {
+            "x": 2.779035844872165,
+            "y": 6.36860208155972,
+            "heading": -0.0666183783001983,
+            "angularVelocity": -6.428976142053906e-8,
+            "velocityX": -3.695002547647375,
+            "velocityY": -0.8109206547436999,
+            "timestamp": 1.5156101725269335
+        },
+        {
+            "x": 2.608054120329012,
+            "y": 6.311726454658268,
+            "heading": -0.06677425221357433,
+            "angularVelocity": -0.0032713195629861798,
+            "velocityX": -3.5883865694652988,
+            "velocityY": -1.1936464920308683,
+            "timestamp": 1.563258808266974
+        },
+        {
+            "x": 2.4510058546542055,
+            "y": 6.242296424291011,
+            "heading": -0.07609605863752185,
+            "angularVelocity": -0.19563637614744786,
+            "velocityX": -3.29596562914219,
+            "velocityY": -1.457125252146435,
+            "timestamp": 1.6109074440070146
+        },
+        {
+            "x": 2.3100257060565728,
+            "y": 6.162946485717711,
+            "heading": -0.09018973905193617,
+            "angularVelocity": -0.2957835034721525,
+            "velocityX": -2.9587447029271896,
+            "velocityY": -1.6653139663050547,
+            "timestamp": 1.658556079747055
+        },
+        {
+            "x": 2.1858070710130613,
+            "y": 6.074835566620609,
+            "heading": -0.10721183288872499,
+            "angularVelocity": -0.3572419980708675,
+            "velocityX": -2.6069714927652616,
+            "velocityY": -1.8491803118526937,
+            "timestamp": 1.7062047154870956
+        },
+        {
+            "x": 2.078679040228791,
+            "y": 5.978601109574319,
+            "heading": -0.12617474237211068,
+            "angularVelocity": -0.3979738179094536,
+            "velocityX": -2.248291669223545,
+            "velocityY": -2.0196686757492435,
+            "timestamp": 1.7538533512271361
+        },
+        {
+            "x": 1.9888310933691378,
+            "y": 5.874644187394139,
+            "heading": -0.14646656775118436,
+            "angularVelocity": -0.42586372231046465,
+            "velocityX": -1.8856352435727148,
+            "velocityY": -2.181739740617511,
+            "timestamp": 1.8015019869671767
+        },
+        {
+            "x": 1.9163856506347656,
+            "y": 5.763239860534668,
+            "heading": -0.16767175460275374,
+            "angularVelocity": -0.44503240276194317,
+            "velocityX": -1.5204095900989456,
+            "velocityY": -2.3380381227986136,
+            "timestamp": 1.8491506227072172
+        },
+        {
+            "x": 1.8485889050766984,
+            "y": 5.589343182132367,
+            "heading": -0.1992083750464638,
+            "angularVelocity": -0.46340502818407264,
+            "velocityX": -0.9962181217957636,
+            "velocityY": -2.555270476750044,
+            "timestamp": 1.9172047406506962
+        },
+        {
+            "x": 1.817164507457459,
+            "y": 5.402475290970386,
+            "heading": -0.23166704244574374,
+            "angularVelocity": -0.47695375945076895,
+            "velocityX": -0.4617560049104581,
+            "velocityY": -2.7458719150120894,
+            "timestamp": 1.9852588585941753
+        },
+        {
+            "x": 1.8230483770902466,
+            "y": 5.205640044676734,
+            "heading": -0.2645113992669886,
+            "angularVelocity": -0.4826211523089798,
+            "velocityX": 0.08645868627131759,
+            "velocityY": -2.8923341047058035,
+            "timestamp": 2.0533129765376543
+        },
+        {
+            "x": 1.8673165212991787,
+            "y": 5.004575832748109,
+            "heading": -0.29674148680613366,
+            "angularVelocity": -0.4735949640250024,
+            "velocityX": 0.6504844313118886,
+            "velocityY": -2.9544753205907597,
+            "timestamp": 2.1213670944811334
+        },
+        {
+            "x": 1.9491517735306563,
+            "y": 4.812420400099087,
+            "heading": -0.3261812067352161,
+            "angularVelocity": -0.43259277790693457,
+            "velocityX": 1.202502577425556,
+            "velocityY": -2.823568043429816,
+            "timestamp": 2.1894212124246124
+        },
+        {
+            "x": 2.050948548896826,
+            "y": 4.653276186897604,
+            "heading": -0.34937275798086526,
+            "angularVelocity": -0.34078101291423535,
+            "velocityX": 1.4958209501847175,
+            "velocityY": -2.338494980328238,
+            "timestamp": 2.2574753303680914
+        },
+        {
+            "x": 2.14520275900139,
+            "y": 4.5319666650276815,
+            "heading": -0.36647677931046935,
+            "angularVelocity": -0.2513297041615066,
+            "velocityX": 1.3849890785871777,
+            "velocityY": -1.7825449147790617,
+            "timestamp": 2.3255294483115705
+        },
+        {
+            "x": 2.2207736095496604,
+            "y": 4.444424274499807,
+            "heading": -0.37856224680514944,
+            "angularVelocity": -0.17758613086130492,
+            "velocityX": 1.1104522816836297,
+            "velocityY": -1.2863643402237204,
+            "timestamp": 2.3935835662550495
+        },
+        {
+            "x": 2.273137984258771,
+            "y": 4.387723780671385,
+            "heading": -0.38627299411990695,
+            "angularVelocity": -0.113303170297753,
+            "velocityX": 0.7694519639887757,
+            "velocityY": -0.8331677133117752,
+            "timestamp": 2.4616376841985286
         },
         {
             "x": 2.3,
             "y": 4.36,
             "heading": -0.39,
-            "angularVelocity": -0.11121265573212626,
-            "velocityX": 0.16624215149881386,
-            "velocityY": -0.4449825196428863,
-            "timestamp": 2.252141458121005
+            "angularVelocity": -0.054765324902410414,
+            "velocityX": 0.3947155080835744,
+            "velocityY": -0.4073784439379148,
+            "timestamp": 2.5296918021420076
         },
         {
             "x": 2.3,
             "y": 4.36,
             "heading": -0.39,
-            "angularVelocity": -5.463060715454085e-29,
+            "angularVelocity": 3.1303206075717874e-30,
             "velocityX": 0,
-            "velocityY": -5.448481045128469e-30,
-            "timestamp": 2.3092339429302484
+            "velocityY": -1.5697460792586675e-31,
+            "timestamp": 2.5977459200854867
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.1.traj
new file mode 100644
index 00000000..90a593eb
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.1.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -3.817834980565894e-40,
+            "angularVelocity": -1.9855221195343998e-39,
+            "velocityX": -9.621145750064654e-40,
+            "velocityY": 1.4575297691012433e-39,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 5.880500005575846,
+            "heading": 3.479114156010015e-22,
+            "angularVelocity": 3.4766412561552746e-21,
+            "velocityX": 2.7068848151306367e-33,
+            "velocityY": 0.8344065481228213,
+            "timestamp": 0.10007112931184182
+        },
+        {
+            "x": 7.68,
+            "y": 6.047500013629847,
+            "heading": 1.081691491825702e-21,
+            "angularVelocity": 7.332585052389997e-21,
+            "velocityX": 5.1589068200097806e-33,
+            "velocityY": 1.6688130652907358,
+            "timestamp": 0.20014225862368365
+        },
+        {
+            "x": 7.68,
+            "y": 6.298000017966616,
+            "heading": 2.2511586271387426e-21,
+            "angularVelocity": 1.1686358860616172e-20,
+            "velocityX": 5.8498916949463956e-33,
+            "velocityY": 2.503219520548835,
+            "timestamp": 0.3002133879355255
+        },
+        {
+            "x": 7.68,
+            "y": 6.632,
+            "heading": 4.481673966100434e-21,
+            "angularVelocity": 2.228929929167172e-20,
+            "velocityX": 6.576791251337367e-33,
+            "velocityY": 3.3376257900775013,
+            "timestamp": 0.4002845172473673
+        },
+        {
+            "x": 7.68,
+            "y": 6.9659999820333836,
+            "heading": 1.4391637904192512e-21,
+            "angularVelocity": -3.0403475801105514e-20,
+            "velocityX": 1.0509973010204301e-32,
+            "velocityY": 3.3376257900775013,
+            "timestamp": 0.5003556465592092
+        },
+        {
+            "x": 7.68,
+            "y": 7.216499986370152,
+            "heading": 4.7457500532831525e-22,
+            "angularVelocity": -9.63903176962576e-21,
+            "velocityX": 7.983324746463204e-33,
+            "velocityY": 2.503219520548835,
+            "timestamp": 0.600426775871051
+        },
+        {
+            "x": 7.68,
+            "y": 7.383499994424153,
+            "heading": 1.0909640932435146e-22,
+            "angularVelocity": -3.652188228166023e-21,
+            "velocityX": 7.341551448907972e-33,
+            "velocityY": 1.6688130652907358,
+            "timestamp": 0.7004979051828928
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -9.42965601107167e-40,
+            "angularVelocity": -1.09018866193586e-21,
+            "velocityX": 7.44470103227488e-33,
+            "velocityY": 0.8344065481228213,
+            "timestamp": 0.8005690344947346
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 8.249839679439821e-43,
+            "angularVelocity": 9.431229878048832e-39,
+            "velocityX": 1.012759498983901e-41,
+            "velocityY": 5.142787784847508e-39,
+            "timestamp": 0.9006401638065764
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.1.traj
new file mode 100644
index 00000000..74357de2
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.1.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -5.382102664363315e-41,
+            "angularVelocity": -2.8652892976895567e-45,
+            "velocityX": 1.592735858156421e-40,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 5.713499994424153,
+            "heading": 1.659495816830368e-21,
+            "angularVelocity": 1.658316243120447e-20,
+            "velocityX": 2.780383744489643e-33,
+            "velocityY": -0.8344065481228213,
+            "timestamp": 0.10007112931184184
+        },
+        {
+            "x": 7.68,
+            "y": 5.546499986370153,
+            "heading": 4.9512387638524226e-21,
+            "angularVelocity": 3.289403167370527e-20,
+            "velocityX": 3.618624574631574e-33,
+            "velocityY": -1.668813065290736,
+            "timestamp": 0.20014225862368368
+        },
+        {
+            "x": 7.68,
+            "y": 5.295999982033384,
+            "heading": 9.801760970151702e-21,
+            "angularVelocity": 4.8470744390330394e-20,
+            "velocityX": 3.439292531132709e-33,
+            "velocityY": -2.503219520548835,
+            "timestamp": 0.30021338793552554
+        },
+        {
+            "x": 7.68,
+            "y": 4.962,
+            "heading": 1.506109654033251e-20,
+            "angularVelocity": 5.255597191401202e-20,
+            "velocityX": 4.639675238422859e-33,
+            "velocityY": -3.3376257900775017,
+            "timestamp": 0.40028451724736736
+        },
+        {
+            "x": 7.68,
+            "y": 4.628000017966616,
+            "heading": 5.3586032763760104e-21,
+            "angularVelocity": -9.69559666912617e-20,
+            "velocityX": 4.935439546802769e-33,
+            "velocityY": -3.3376257900775017,
+            "timestamp": 0.5003556465592092
+        },
+        {
+            "x": 7.68,
+            "y": 4.377500013629846,
+            "heading": 1.8760590350448333e-21,
+            "angularVelocity": -3.480068842301423e-20,
+            "velocityX": 4.701730454770122e-33,
+            "velocityY": -2.5032195205488352,
+            "timestamp": 0.600426775871051
+        },
+        {
+            "x": 7.68,
+            "y": 4.210500005575846,
+            "heading": 3.168722428707802e-22,
+            "angularVelocity": -1.5580785203142817e-20,
+            "velocityX": 3.49539235617723e-33,
+            "velocityY": -1.668813065290736,
+            "timestamp": 0.7004979051828928
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 5.881496870004905e-41,
+            "angularVelocity": -3.1664700918333573e-21,
+            "velocityX": 3.364378386644386e-33,
+            "velocityY": -0.8344065481228213,
+            "timestamp": 0.8005690344947346
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 9.145026397391945e-42,
+            "angularVelocity": -4.963805231001231e-40,
+            "velocityX": -4.870998572760777e-42,
+            "velocityY": 3.489748854003666e-39,
+            "timestamp": 0.9006401638065764
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.1.traj
new file mode 100644
index 00000000..bcede3cb
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.1.traj	
@@ -0,0 +1,139 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 7.067561628822357e-40,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 5.720554627994461,
+            "heading": -3.8243496398495774e-19,
+            "angularVelocity": -3.994077070459528e-18,
+            "velocityX": -4.592295404090674e-34,
+            "velocityY": -0.7983807350946677,
+            "timestamp": 0.09575052183150977
+        },
+        {
+            "x": 7.68,
+            "y": 5.567663887017311,
+            "heading": -1.1460790562861162e-18,
+            "angularVelocity": -7.975351749671857e-18,
+            "velocityX": 3.216033275420413e-35,
+            "velocityY": -1.5967614385035829,
+            "timestamp": 0.19150104366301954
+        },
+        {
+            "x": 7.68,
+            "y": 5.3383277831364,
+            "heading": -2.289011643788579e-18,
+            "angularVelocity": -1.1936567706665239e-17,
+            "velocityX": -6.99395718741277e-36,
+            "velocityY": -2.3951420785410233,
+            "timestamp": 0.2872515654945293
+        },
+        {
+            "x": 7.68,
+            "y": 5.032546334555254,
+            "heading": -3.801510097022925e-18,
+            "angularVelocity": -1.5796242396621782e-17,
+            "velocityX": -1.9389720200265424e-34,
+            "velocityY": -3.1935225284643693,
+            "timestamp": 0.3830020873260391
+        },
+        {
+            "x": 7.68,
+            "y": 4.670327800733179,
+            "heading": -7.244345840319028e-19,
+            "angularVelocity": 3.2136383741131935e-17,
+            "velocityX": -8.432373407035536e-35,
+            "velocityY": -3.7829405719528495,
+            "timestamp": 0.47875260915754886
+        },
+        {
+            "x": 7.68,
+            "y": 4.308109266911059,
+            "heading": -3.757861819981246e-18,
+            "angularVelocity": -3.168052954622205e-17,
+            "velocityX": -1.6004910806259417e-34,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.5745031309890586
+        },
+        {
+            "x": 7.68,
+            "y": 3.9458907330889397,
+            "heading": -5.3976563030855884e-18,
+            "angularVelocity": -1.712569761227889e-17,
+            "velocityX": -2.8603575042124878e-33,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.6702536528205684
+        },
+        {
+            "x": 7.68,
+            "y": 3.5836721992668203,
+            "heading": -4.35645304230582e-18,
+            "angularVelocity": 1.0874126227865062e-17,
+            "velocityX": -1.9073271103237224e-34,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.7660041746520782
+        },
+        {
+            "x": 7.68,
+            "y": 3.221453665444745,
+            "heading": 3.661990694649205e-18,
+            "angularVelocity": 8.37430813282138e-17,
+            "velocityX": -3.48109229998139e-33,
+            "velocityY": -3.7829405719528495,
+            "timestamp": 0.8617546964835879
+        },
+        {
+            "x": 7.68,
+            "y": 2.915672216863599,
+            "heading": 2.222559506115206e-18,
+            "angularVelocity": -1.503314198751682e-17,
+            "velocityX": 7.233727398507661e-34,
+            "velocityY": -3.1935225284643693,
+            "timestamp": 0.9575052183150977
+        },
+        {
+            "x": 7.68,
+            "y": 2.686336112982689,
+            "heading": 1.1174854461234148e-18,
+            "angularVelocity": -1.1541180553049724e-17,
+            "velocityX": 7.218959232022663e-34,
+            "velocityY": -2.3951420785410233,
+            "timestamp": 1.0532557401466074
+        },
+        {
+            "x": 7.68,
+            "y": 2.5334453720055388,
+            "heading": 3.7383227049768543e-19,
+            "angularVelocity": -7.766570474567622e-18,
+            "velocityX": 1.7872274250399634e-33,
+            "velocityY": -1.5967614385035829,
+            "timestamp": 1.1490062619781172
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": -3.904232200157263e-18,
+            "velocityX": 6.089132400706592e-33,
+            "velocityY": -0.7983807350946677,
+            "timestamp": 1.244756783809627
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 3.5763678522980893e-43,
+            "velocityY": -5.882608067207678e-38,
+            "timestamp": 1.3405073056411367
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.1.traj
new file mode 100644
index 00000000..3fd40c6e
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.1.traj	
@@ -0,0 +1,175 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -1.2882449649302714e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 5.715155846048237,
+            "heading": -2.6675793503802873e-18,
+            "angularVelocity": -2.692513973443352e-17,
+            "velocityX": 2.411941793716939e-19,
+            "velocityY": -0.8260917454276263,
+            "timestamp": 0.09907392781100396
+        },
+        {
+            "x": 7.68,
+            "y": 5.55146754125807,
+            "heading": -8.002991694377614e-18,
+            "angularVelocity": -5.385283961043983e-17,
+            "velocityX": 4.822926900034719e-19,
+            "velocityY": -1.6521834594306495,
+            "timestamp": 0.19814785562200793
+        },
+        {
+            "x": 7.68,
+            "y": 5.305935091856216,
+            "heading": -1.6007101079545214e-17,
+            "angularVelocity": -8.07892607262788e-17,
+            "velocityX": 7.23199861883135e-19,
+            "velocityY": -2.478275110584479,
+            "timestamp": 0.29722178343301187
+        },
+        {
+            "x": 7.68,
+            "y": 4.978558516522808,
+            "heading": -2.6680861930648114e-17,
+            "angularVelocity": -1.0773531530269989e-16,
+            "velocityX": 9.635330187135107e-19,
+            "velocityY": -3.3043665731908765,
+            "timestamp": 0.39629571124401586
+        },
+        {
+            "x": 7.68,
+            "y": 4.603767735073314,
+            "heading": -3.033567109313287e-17,
+            "angularVelocity": -3.688971703490728e-17,
+            "velocityX": 1.1102275506466142e-18,
+            "velocityY": -3.782940575087071,
+            "timestamp": 0.49536963905501985
+        },
+        {
+            "x": 7.68,
+            "y": 4.228976953623796,
+            "heading": -3.5529792774719166e-17,
+            "angularVelocity": -5.2426726146536295e-17,
+            "velocityX": 1.1087814850277944e-18,
+            "velocityY": -3.7829405750873115,
+            "timestamp": 0.5944435668660238
+        },
+        {
+            "x": 7.68,
+            "y": 3.8541861721742774,
+            "heading": -4.8775545061581196e-17,
+            "angularVelocity": -1.3369564101798844e-16,
+            "velocityX": 1.1066129122730268e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.6935174946770278
+        },
+        {
+            "x": 7.68,
+            "y": 3.4793953907247595,
+            "heading": -3.3383639047364345e-17,
+            "angularVelocity": 1.5535778538404392e-16,
+            "velocityX": 1.113993391664377e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.7925914224880318
+        },
+        {
+            "x": 7.68,
+            "y": 3.104604609275241,
+            "heading": -4.339521377299963e-18,
+            "angularVelocity": 2.9315601300747485e-16,
+            "velocityX": 1.114171335036007e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.8916653502990358
+        },
+        {
+            "x": 7.68,
+            "y": 2.7298138278257227,
+            "heading": 1.3174711336089981e-17,
+            "angularVelocity": 1.7677943229992181e-16,
+            "velocityX": 1.1158489583207238e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 0.9907392781100398
+        },
+        {
+            "x": 7.68,
+            "y": 2.3550230463762043,
+            "heading": 2.965421117619106e-17,
+            "angularVelocity": 1.6633538413393434e-16,
+            "velocityX": 1.1181357718618657e-18,
+            "velocityY": -3.782940575087312,
+            "timestamp": 1.0898132059210437
+        },
+        {
+            "x": 7.68,
+            "y": 1.9802322649266861,
+            "heading": 2.902288280090785e-17,
+            "angularVelocity": -6.372295813531433e-18,
+            "velocityX": 1.121394742098521e-18,
+            "velocityY": -3.7829405750873115,
+            "timestamp": 1.1888871337320477
+        },
+        {
+            "x": 7.68,
+            "y": 1.605441483477192,
+            "heading": 2.667314494747493e-17,
+            "angularVelocity": -2.3717015216307205e-17,
+            "velocityX": 1.1050727300415562e-18,
+            "velocityY": -3.782940575087071,
+            "timestamp": 1.2879610615430517
+        },
+        {
+            "x": 7.68,
+            "y": 1.2780649081437845,
+            "heading": 1.6004201006274906e-17,
+            "angularVelocity": -1.0768669595616384e-16,
+            "velocityX": 9.663666503471993e-19,
+            "velocityY": -3.304366573190877,
+            "timestamp": 1.3870349893540557
+        },
+        {
+            "x": 7.68,
+            "y": 1.0325324587419298,
+            "heading": 8.002125258512226e-18,
+            "angularVelocity": -8.076873426351199e-17,
+            "velocityX": 7.247750412790524e-19,
+            "velocityY": -2.478275110584479,
+            "timestamp": 1.4861089171650597
+        },
+        {
+            "x": 7.68,
+            "y": 0.868844153951763,
+            "heading": 2.6673738748743342e-18,
+            "angularVelocity": -5.384616822543564e-17,
+            "velocityX": 4.831833763651305e-19,
+            "velocityY": -1.6521834594306495,
+            "timestamp": 1.5851828449760637
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": -2.6923065773077948e-17,
+            "velocityX": 2.415916928361511e-19,
+            "velocityY": -0.8260917454276263,
+            "timestamp": 1.6842567727870676
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -1.1487176577652646e-33,
+            "velocityY": 0,
+            "timestamp": 1.7833307005980716
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.1.traj
new file mode 100644
index 00000000..6cbc4c1d
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 7.776001513534033e-33,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.620476769374415,
+            "y": 5.82809535402629,
+            "heading": -0.014618874829729222,
+            "angularVelocity": -0.16274311325472623,
+            "velocityX": -0.6626362135126213,
+            "velocityY": 0.34616581514913597,
+            "timestamp": 0.0898279167539809
+        },
+        {
+            "x": 7.5013982904113785,
+            "y": 5.890300493780534,
+            "heading": -0.04163561529349077,
+            "angularVelocity": -0.30076107116849365,
+            "velocityX": -1.3256288608937272,
+            "velocityY": 0.6924922897255835,
+            "timestamp": 0.1796558335079618
+        },
+        {
+            "x": 7.3227216232188646,
+            "y": 5.983633458070523,
+            "heading": -0.07726722819497155,
+            "angularVelocity": -0.39666524827763777,
+            "velocityX": -1.9890995321851956,
+            "velocityY": 1.0390195794655717,
+            "timestamp": 0.2694837502619427
+        },
+        {
+            "x": 7.084404558965627,
+            "y": 6.108106280619183,
+            "heading": -0.11365614248079976,
+            "angularVelocity": -0.40509582767559393,
+            "velocityX": -2.6530400889284445,
+            "velocityY": 1.3856808333823862,
+            "timestamp": 0.3593116670159236
+        },
+        {
+            "x": 6.786764320803032,
+            "y": 6.263508027124416,
+            "heading": -0.12453213515982267,
+            "angularVelocity": -0.12107586452004557,
+            "velocityX": -3.313449191722499,
+            "velocityY": 1.7299938829800985,
+            "timestamp": 0.4491395837699045
+        },
+        {
+            "x": 6.485598483527544,
+            "y": 6.42090073842616,
+            "heading": -0.1245321478638912,
+            "angularVelocity": -1.4142672994383305e-7,
+            "velocityX": -3.3526975594938513,
+            "velocityY": 1.7521580928210598,
+            "timestamp": 0.5389675005238854
+        },
+        {
+            "x": 6.164419225817841,
+            "y": 6.531883674964022,
+            "heading": -0.12453214704403566,
+            "angularVelocity": 9.126957138704023e-9,
+            "velocityX": -3.575494894191328,
+            "velocityY": 1.2355060714790946,
+            "timestamp": 0.6287954172778664
+        },
+        {
+            "x": 5.838530026083828,
+            "y": 6.576482035403971,
+            "heading": -0.08897109734241883,
+            "angularVelocity": 0.39587971074750417,
+            "velocityX": -3.62792783702814,
+            "velocityY": 0.4964866385813549,
+            "timestamp": 0.7186233340318473
+        },
+        {
+            "x": 5.57314678987786,
+            "y": 6.601508855183474,
+            "heading": 0.02708371106201491,
+            "angularVelocity": 1.2919681608812392,
+            "velocityX": -2.9543514510393742,
+            "velocityY": 0.27860848479928674,
+            "timestamp": 0.8084512507858282
+        },
+        {
+            "x": 5.3743330310505755,
+            "y": 6.616929267325104,
+            "heading": 0.1370261836602847,
+            "angularVelocity": 1.2239232141983023,
+            "velocityX": -2.213273623742076,
+            "velocityY": 0.17166614454461948,
+            "timestamp": 0.8982791675398092
+        },
+        {
+            "x": 5.242037120273562,
+            "y": 6.625930657483227,
+            "heading": 0.21902488355780253,
+            "angularVelocity": 0.9128420524556349,
+            "velocityX": -1.4727705546076917,
+            "velocityY": 0.10020704568687104,
+            "timestamp": 0.9881070842937901
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.263,
+            "angularVelocity": 0.48954843918551255,
+            "velocityX": -0.7351514168409599,
+            "velocityY": 0.0453015350218741,
+            "timestamp": 1.077935001047771
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.263,
+            "angularVelocity": -4.780109867389741e-33,
+            "velocityX": 0,
+            "velocityY": -5.233682494366388e-34,
+            "timestamp": 1.167762917801752
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.1.traj
new file mode 100644
index 00000000..4e0731ce
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.1.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 1.3211551091100433e-31,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6375302942972665,
+            "y": 5.822387711771769,
+            "heading": 0.010500881646553203,
+            "angularVelocity": 0.13619602846032355,
+            "velocityX": -0.5508304389364825,
+            "velocityY": 0.32927763890619355,
+            "timestamp": 0.07710123243140166
+        },
+        {
+            "x": 7.5525908811480225,
+            "y": 5.873163180716977,
+            "heading": 0.031500279151306286,
+            "angularVelocity": 0.27236137273728567,
+            "velocityX": -1.1016609004897018,
+            "velocityY": 0.6585558666702785,
+            "timestamp": 0.1542024648628033
+        },
+        {
+            "x": 7.4251817593524025,
+            "y": 5.949326484276257,
+            "heading": 0.06299451206522548,
+            "angularVelocity": 0.40847898173275193,
+            "velocityX": -1.6524913776051289,
+            "velocityY": 0.987835098836381,
+            "timestamp": 0.23130369729420497
+        },
+        {
+            "x": 7.255302939480135,
+            "y": 6.050877733979633,
+            "heading": 0.10497913316622995,
+            "angularVelocity": 0.5445389104299847,
+            "velocityX": -2.203321717631576,
+            "velocityY": 1.3171157775425744,
+            "timestamp": 0.3084049297256066
+        },
+        {
+            "x": 7.042954462706059,
+            "y": 6.177817072431174,
+            "heading": 0.1574496496087124,
+            "angularVelocity": 0.6805405670936119,
+            "velocityX": -2.754151523621958,
+            "velocityY": 1.6463983058180132,
+            "timestamp": 0.3855061621570083
+        },
+        {
+            "x": 6.792607460054783,
+            "y": 6.3274746833219355,
+            "heading": 0.15744965721454035,
+            "angularVelocity": 9.864729365588338e-8,
+            "velocityX": -3.246990933303352,
+            "velocityY": 1.941053419916652,
+            "timestamp": 0.46260739458840994
+        },
+        {
+            "x": 6.542259846259959,
+            "y": 6.4771312718889495,
+            "heading": 0.15744966481969572,
+            "angularVelocity": 9.863857053631023e-8,
+            "velocityX": -3.246998859811527,
+            "velocityY": 1.9410401604172332,
+            "timestamp": 0.5397086270198116
+        },
+        {
+            "x": 6.273256747030451,
+            "y": 6.589862608843688,
+            "heading": 0.1574496727775284,
+            "angularVelocity": 1.0321278187268036e-7,
+            "velocityX": -3.4889597837342676,
+            "velocityY": 1.4621210764047974,
+            "timestamp": 0.6168098594512132
+        },
+        {
+            "x": 5.98904964850956,
+            "y": 6.655416753006686,
+            "heading": 0.15744969119167346,
+            "angularVelocity": 2.3883074855376557e-7,
+            "velocityX": -3.686155065987494,
+            "velocityY": 0.8502347121535809,
+            "timestamp": 0.6939110918826149
+        },
+        {
+            "x": 5.6978465137603145,
+            "y": 6.6719003531074685,
+            "heading": 0.15744999446473232,
+            "angularVelocity": 0.00000393343983387992,
+            "velocityX": -3.7768933850484796,
+            "velocityY": 0.21379165521703997,
+            "timestamp": 0.7710123243140166
+        },
+        {
+            "x": 5.40805721282959,
+            "y": 6.638837814331055,
+            "heading": 0.15745035498223803,
+            "angularVelocity": 0.00000467589809324531,
+            "velocityX": -3.7585560151526085,
+            "velocityY": -0.428819848059242,
+            "timestamp": 0.8481135567454182
+        },
+        {
+            "x": 5.104307335648147,
+            "y": 6.562247743535093,
+            "heading": 0.18966206475475966,
+            "angularVelocity": 0.37702586381684944,
+            "velocityX": -3.555277277652879,
+            "velocityY": -0.896457772827513,
+            "timestamp": 0.9335498964342235
+        },
+        {
+            "x": 4.836190414730584,
+            "y": 6.438293970082571,
+            "heading": 0.27176288986041636,
+            "angularVelocity": 0.9609590650149814,
+            "velocityX": -3.1382070193334055,
+            "velocityY": -1.4508319750590115,
+            "timestamp": 1.0189862361230289
+        },
+        {
+            "x": 4.616497381460216,
+            "y": 6.278221072521664,
+            "heading": 0.3274582685749672,
+            "angularVelocity": 0.6518933151562509,
+            "velocityX": -2.5714237532949347,
+            "velocityY": -1.8735926438791781,
+            "timestamp": 1.1044225758118342
+        },
+        {
+            "x": 4.449006665280017,
+            "y": 6.09487376587715,
+            "heading": 0.3096216512620487,
+            "angularVelocity": -0.2087708506460702,
+            "velocityX": -1.960415401575849,
+            "velocityY": -2.1460107878256482,
+            "timestamp": 1.1898589155006396
+        },
+        {
+            "x": 4.339814814870372,
+            "y": 5.903698766708507,
+            "heading": 0.23521329843339078,
+            "angularVelocity": -0.8709215902704173,
+            "velocityX": -1.2780492564097095,
+            "velocityY": -2.237630964352897,
+            "timestamp": 1.275295255189445
+        },
+        {
+            "x": 4.286146359443328,
+            "y": 5.736442488850756,
+            "heading": 0.1356278137676922,
+            "angularVelocity": -1.1656103834554516,
+            "velocityX": -0.628168945703036,
+            "velocityY": -1.957671389796991,
+            "timestamp": 1.3607315948782504
+        },
+        {
+            "x": 4.266951931468271,
+            "y": 5.619203136466468,
+            "heading": 0.049466079264437586,
+            "angularVelocity": -1.0084904715849423,
+            "velocityX": -0.2246635102225946,
+            "velocityY": -1.372242219309977,
+            "timestamp": 1.4461679345670557
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 4.182976778726496e-34,
+            "angularVelocity": -0.578981724224301,
+            "velocityX": -0.046255861178807596,
+            "velocityY": -0.6929502912005617,
+            "timestamp": 1.531604274255861
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -6.30298269203181e-33,
+            "timestamp": 1.6170406139446665
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.1.traj
new file mode 100644
index 00000000..90749c58
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.1.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 7.446350164213638e-30,
+            "velocityX": -3.476389258222187e-32,
+            "velocityY": 2.4606016306403766e-37,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6436015698643995,
+            "y": 5.74806653588022,
+            "heading": -0.0038807165751560974,
+            "angularVelocity": -0.045372809336175934,
+            "velocityX": -0.4255654848002544,
+            "velocityY": -0.572123393880186,
+            "timestamp": 0.08552956345292874
+        },
+        {
+            "x": 7.570804707716712,
+            "y": 5.6501996114987145,
+            "heading": -0.011642164214821673,
+            "angularVelocity": -0.09074578807989701,
+            "velocityX": -0.8511309915401587,
+            "velocityY": -1.1442467426525054,
+            "timestamp": 0.17105912690585748
+        },
+        {
+            "x": 7.461609412342726,
+            "y": 5.503399231866818,
+            "heading": -0.02328436718357624,
+            "angularVelocity": -0.136119050521775,
+            "velocityX": -1.2766965124764296,
+            "velocityY": -1.7163700328330225,
+            "timestamp": 0.25658869035878623
+        },
+        {
+            "x": 7.3160156842382476,
+            "y": 5.307665404737857,
+            "heading": -0.038807352623976644,
+            "angularVelocity": -0.18149263031072865,
+            "velocityX": -1.7022620276158245,
+            "velocityY": -2.288493232362672,
+            "timestamp": 0.34211825381171496
+        },
+        {
+            "x": 7.134023531165023,
+            "y": 5.062998148299065,
+            "heading": -0.05821111114579378,
+            "angularVelocity": -0.22686610031040333,
+            "velocityX": -2.127827452005934,
+            "velocityY": -2.8606162192496707,
+            "timestamp": 0.4276478172646437
+        },
+        {
+            "x": 6.9409164317403365,
+            "y": 4.803390071813602,
+            "heading": -0.058211113467083825,
+            "angularVelocity": -2.714020682501112e-8,
+            "velocityX": -2.2577818900123794,
+            "velocityY": -3.0353022511138943,
+            "timestamp": 0.5131773807175725
+        },
+        {
+            "x": 6.70968017178257,
+            "y": 4.577079452070077,
+            "heading": -0.05821111591888997,
+            "angularVelocity": -2.866618337367041e-8,
+            "velocityX": -2.7035828387575864,
+            "velocityY": -2.645992924634468,
+            "timestamp": 0.5987069441705012
+        },
+        {
+            "x": 6.4400791107327935,
+            "y": 4.398188508585333,
+            "heading": -0.05821111892081074,
+            "angularVelocity": -3.509804859460385e-8,
+            "velocityX": -3.152138864804934,
+            "velocityY": -2.091568532127445,
+            "timestamp": 0.6842365076234299
+        },
+        {
+            "x": 6.141694589328426,
+            "y": 4.273075039260189,
+            "heading": -0.058211144902897714,
+            "angularVelocity": -3.0377902004237926e-7,
+            "velocityX": -3.488671160686832,
+            "velocityY": -1.462809632999017,
+            "timestamp": 0.7697660710763586
+        },
+        {
+            "x": 5.825131041705898,
+            "y": 4.206185466847777,
+            "heading": -0.05821124348692034,
+            "angularVelocity": -0.000001152630957627144,
+            "velocityX": -3.7012178578082944,
+            "velocityY": -0.7820637649954203,
+            "timestamp": 0.8552956345292874
+        },
+        {
+            "x": 5.503318785454965,
+            "y": 4.1726658015863975,
+            "heading": -0.058211338856237596,
+            "angularVelocity": -0.0000011150450605169934,
+            "velocityX": -3.76258504380235,
+            "velocityY": -0.39190735820634004,
+            "timestamp": 0.9408251979822161
+        },
+        {
+            "x": 5.1935160355708865,
+            "y": 4.079344267897855,
+            "heading": -0.0582116136415855,
+            "angularVelocity": -0.0000032127528402629677,
+            "velocityX": -3.622171532005757,
+            "velocityY": -1.091102654112135,
+            "timestamp": 1.026354761435145
+        },
+        {
+            "x": 4.918641828284195,
+            "y": 3.9369980738928456,
+            "heading": -0.09326111746178506,
+            "angularVelocity": -0.4097940221510551,
+            "velocityX": -3.2137917719873403,
+            "velocityY": -1.664292301495839,
+            "timestamp": 1.1118843248880736
+        },
+        {
+            "x": 4.689025118769517,
+            "y": 3.755353610578967,
+            "heading": -0.16867561276178636,
+            "angularVelocity": -0.8817360016283226,
+            "velocityX": -2.6846472756878708,
+            "velocityY": -2.1237623107224977,
+            "timestamp": 1.1974138883410024
+        },
+        {
+            "x": 4.510985991586203,
+            "y": 3.542062267326439,
+            "heading": -0.2715725395002121,
+            "angularVelocity": -1.2030568447254515,
+            "velocityX": -2.0816092120158958,
+            "velocityY": -2.493773318156982,
+            "timestamp": 1.282943451793931
+        },
+        {
+            "x": 4.39344009542189,
+            "y": 3.3364721307805043,
+            "heading": -0.3792171957634167,
+            "angularVelocity": -1.2585666513128753,
+            "velocityX": -1.3743305989045989,
+            "velocityY": -2.4037318588569803,
+            "timestamp": 1.3684730152468598
+        },
+        {
+            "x": 4.321460732883776,
+            "y": 3.1711461674458645,
+            "heading": -0.4697306425856333,
+            "angularVelocity": -1.0582708851545903,
+            "velocityX": -0.8415728975131295,
+            "velocityY": -1.932968632835679,
+            "timestamp": 1.4540025786997886
+        },
+        {
+            "x": 4.280782976343733,
+            "y": 3.057632956228743,
+            "heading": -0.5333100471714295,
+            "angularVelocity": -0.7433617338733072,
+            "velocityX": -0.475598785938263,
+            "velocityY": -1.3271809960727023,
+            "timestamp": 1.5395321421527173
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -0.38220647351440606,
+            "velocityX": -0.20791613596297567,
+            "velocityY": -0.6738366700592523,
+            "timestamp": 1.625061705605646
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -1.9453350650223036e-32,
+            "velocityX": 0,
+            "velocityY": 3.675541041669649e-33,
+            "timestamp": 1.7105912690585747
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S4.1.traj
new file mode 100644
index 00000000..4efefc66
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S4.1.traj	
@@ -0,0 +1,256 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": -1.007826409753455e-39,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.661226431831969,
+            "y": 5.757681823957774,
+            "heading": -0.0001846118691686639,
+            "angularVelocity": -0.0025538708536768893,
+            "velocityX": -0.25970848342391806,
+            "velocityY": -0.5439170529292338,
+            "timestamp": 0.07228708096295171
+        },
+        {
+            "x": 7.623679292701499,
+            "y": 5.679045474684997,
+            "heading": -0.0005538375109882563,
+            "angularVelocity": -0.005107768039614528,
+            "velocityX": -0.5194170055049473,
+            "velocityY": -1.0878340669625215,
+            "timestamp": 0.14457416192590342
+        },
+        {
+            "x": 7.567358579979895,
+            "y": 5.561090955652203,
+            "heading": -0.0011076778331591435,
+            "angularVelocity": -0.0076616777824344475,
+            "velocityX": -0.779125563950612,
+            "velocityY": -1.6317510329853882,
+            "timestamp": 0.2168612428888551
+        },
+        {
+            "x": 7.492264291454711,
+            "y": 5.403818271607319,
+            "heading": -0.001846132228095339,
+            "angularVelocity": -0.010215579120073546,
+            "velocityX": -1.0388341530026832,
+            "velocityY": -2.175667933326667,
+            "timestamp": 0.28914832385180683
+        },
+        {
+            "x": 7.39839642615194,
+            "y": 5.207227430397753,
+            "heading": -0.0027691974892554232,
+            "angularVelocity": -0.012769436099282666,
+            "velocityX": -1.298542755528879,
+            "velocityY": -2.719584725109078,
+            "timestamp": 0.36143540481475855
+        },
+        {
+            "x": 7.285754988600711,
+            "y": 4.97131845200793,
+            "heading": -0.0038768638943260385,
+            "angularVelocity": -0.015323158582628504,
+            "velocityX": -1.5582512954003445,
+            "velocityY": -3.263501240432299,
+            "timestamp": 0.4337224857777103
+        },
+        {
+            "x": 7.167927209247298,
+            "y": 4.7245478329210355,
+            "heading": -0.0038768640537450113,
+            "angularVelocity": -2.205359112715029e-9,
+            "velocityX": -1.629997750411335,
+            "velocityY": -3.4137582511232942,
+            "timestamp": 0.506009566740662
+        },
+        {
+            "x": 7.050099366875978,
+            "y": 4.477777243923851,
+            "heading": -0.003876864213154645,
+            "angularVelocity": -2.2052299170164517e-9,
+            "velocityX": -1.6299986221840805,
+            "velocityY": -3.413757834870371,
+            "timestamp": 0.5782966477036137
+        },
+        {
+            "x": 6.932271561638543,
+            "y": 4.2310066371960025,
+            "heading": -0.0038768643725604667,
+            "angularVelocity": -2.2051771914474314e-9,
+            "velocityX": -1.629998108483918,
+            "velocityY": -3.413758080151585,
+            "timestamp": 0.6505837286665653
+        },
+        {
+            "x": 6.8252544845676155,
+            "y": 3.9793591274718993,
+            "heading": -0.0038768645333797187,
+            "angularVelocity": -2.2247301918048757e-9,
+            "velocityX": -1.4804454080221456,
+            "velocityY": -3.4812238421008592,
+            "timestamp": 0.722870809629517
+        },
+        {
+            "x": 6.7595626994044276,
+            "y": 3.7139091088268694,
+            "heading": -0.003876864707631206,
+            "angularVelocity": -2.410548124556829e-9,
+            "velocityX": -0.9087624550347577,
+            "velocityY": -3.6721640313720423,
+            "timestamp": 0.7951578905924687
+        },
+        {
+            "x": 6.7368645668029785,
+            "y": 3.4413950443267822,
+            "heading": -0.0038768649214938546,
+            "angularVelocity": -2.9585182521427424e-9,
+            "velocityX": -0.31399985030634436,
+            "velocityY": -3.7698861382956523,
+            "timestamp": 0.8674449715554203
+        },
+        {
+            "x": 6.763602448674763,
+            "y": 3.1456190677825386,
+            "heading": -0.0038769163309766456,
+            "angularVelocity": -6.548510424231184e-7,
+            "velocityX": 0.340585605327871,
+            "velocityY": -3.7675774205236117,
+            "timestamp": 0.9459505849856912
+        },
+        {
+            "x": 6.763837716877819,
+            "y": 2.855181760345028,
+            "heading": -0.02573615579182971,
+            "angularVelocity": -0.27844173818562123,
+            "velocityX": 0.0029968328731749647,
+            "velocityY": -3.699573759722024,
+            "timestamp": 1.0244561984159621
+        },
+        {
+            "x": 6.746691140594586,
+            "y": 2.576419895706607,
+            "heading": -0.08408576500639096,
+            "angularVelocity": -0.7432539746522423,
+            "velocityX": -0.2184121049950426,
+            "velocityY": -3.5508526391685153,
+            "timestamp": 1.102961811846233
+        },
+        {
+            "x": 6.69086397201793,
+            "y": 2.322743038831463,
+            "heading": -0.2046015958190934,
+            "angularVelocity": -1.5351237388870964,
+            "velocityX": -0.7111232705192682,
+            "velocityY": -3.2313212494092722,
+            "timestamp": 1.1814674252765038
+        },
+        {
+            "x": 6.586917702656428,
+            "y": 2.0871040226876985,
+            "heading": -0.32491541634118276,
+            "angularVelocity": -1.5325505433946174,
+            "velocityX": -1.324061615719081,
+            "velocityY": -3.001556269005664,
+            "timestamp": 1.2599730387067747
+        },
+        {
+            "x": 6.439847045017927,
+            "y": 1.879401635438828,
+            "heading": -0.448122685606705,
+            "angularVelocity": -1.5694071274910255,
+            "velocityX": -1.8733775995410846,
+            "velocityY": -2.6457010928696563,
+            "timestamp": 1.3384786521370455
+        },
+        {
+            "x": 6.25456019704424,
+            "y": 1.7060438180074393,
+            "heading": -0.5681346722700569,
+            "angularVelocity": -1.528705801018258,
+            "velocityX": -2.3601732395640638,
+            "velocityY": -2.2082219328859343,
+            "timestamp": 1.4169842655673164
+        },
+        {
+            "x": 6.049779045368227,
+            "y": 1.57972317182915,
+            "heading": -0.6633309119306381,
+            "angularVelocity": -1.2126042393788183,
+            "velocityX": -2.6084905617341763,
+            "velocityY": -1.6090651439911137,
+            "timestamp": 1.4954898789975872
+        },
+        {
+            "x": 5.840944041976222,
+            "y": 1.503518626439596,
+            "heading": -0.7208467997653518,
+            "angularVelocity": -0.7326340795464229,
+            "velocityX": -2.660128292322611,
+            "velocityY": -0.9706891273098467,
+            "timestamp": 1.573995492427858
+        },
+        {
+            "x": 5.636524773547887,
+            "y": 1.4770268115818306,
+            "heading": -0.7348631463149735,
+            "angularVelocity": -0.17853941822989652,
+            "velocityX": -2.603880913686534,
+            "velocityY": -0.33745121782018417,
+            "timestamp": 1.652501105858129
+        },
+        {
+            "x": 5.451777868175388,
+            "y": 1.4973120784666072,
+            "heading": -0.7197568228935092,
+            "angularVelocity": 0.19242348109134802,
+            "velocityX": -2.353295481686686,
+            "velocityY": 0.25839256581051945,
+            "timestamp": 1.7310067192883998
+        },
+        {
+            "x": 5.309771976878378,
+            "y": 1.5457401851903851,
+            "heading": -0.687868735789328,
+            "angularVelocity": 0.4061886241103559,
+            "velocityX": -1.808862896449324,
+            "velocityY": 0.6168744451222204,
+            "timestamp": 1.8095123327186706
+        },
+        {
+            "x": 5.2191255322417085,
+            "y": 1.592492981200916,
+            "heading": -0.6576952711638115,
+            "angularVelocity": 0.38434786134518467,
+            "velocityX": -1.154649211386418,
+            "velocityY": 0.5955344333696231,
+            "timestamp": 1.8880179461489415
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 0.22540134890517805,
+            "velocityX": -0.5493305555788384,
+            "velocityY": 0.35038282738234927,
+            "timestamp": 1.9665235595792123
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 0,
+            "velocityX": 6.617232561290069e-35,
+            "velocityY": 1.0472745338276337e-32,
+            "timestamp": 2.045029173009483
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.1.traj
new file mode 100644
index 00000000..0428af29
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 2.477402495362019e-32,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.61783610738437,
+            "y": 5.830159445563511,
+            "heading": -0.009839426193567341,
+            "angularVelocity": -0.10699963479066096,
+            "velocityX": -0.6760062707098423,
+            "velocityY": 0.3605950687933442,
+            "timestamp": 0.09195756801242966
+        },
+        {
+            "x": 7.493508328314676,
+            "y": 5.8964783245931205,
+            "heading": -0.029519357600464063,
+            "angularVelocity": -0.21401100346886773,
+            "velocityX": -1.3520124744152573,
+            "velocityY": 0.7211900060324206,
+            "timestamp": 0.18391513602485932
+        },
+        {
+            "x": 7.307016677169599,
+            "y": 5.995956620127639,
+            "heading": -0.05904164496194892,
+            "angularVelocity": -0.32104249818236175,
+            "velocityX": -2.028018521758536,
+            "velocityY": 1.0817847588256284,
+            "timestamp": 0.275872704037289
+        },
+        {
+            "x": 7.058361185168299,
+            "y": 6.1285943089313815,
+            "heading": -0.09840880514573262,
+            "angularVelocity": -0.42810136277704247,
+            "velocityX": -2.7040242296065236,
+            "velocityY": 1.4423792589405482,
+            "timestamp": 0.36783027204971863
+        },
+        {
+            "x": 6.751428461707863,
+            "y": 6.292319225940392,
+            "heading": -0.09840881013370367,
+            "angularVelocity": -5.424209394751822e-8,
+            "velocityX": -3.3377646896767494,
+            "velocityY": 1.7804398327158815,
+            "timestamp": 0.4597878400621483
+        },
+        {
+            "x": 6.443018732830252,
+            "y": 6.45324462570272,
+            "heading": -0.09840881513118924,
+            "angularVelocity": -5.434556026601013e-8,
+            "velocityX": -3.3538265043712756,
+            "velocityY": 1.7499962563230937,
+            "timestamp": 0.551745408074578
+        },
+        {
+            "x": 6.108495593860041,
+            "y": 6.548679499028843,
+            "heading": -0.09840879814777771,
+            "angularVelocity": 1.8468748020115896e-7,
+            "velocityX": -3.637798891386455,
+            "velocityY": 1.0378142374668238,
+            "timestamp": 0.6437029760870077
+        },
+        {
+            "x": 5.7623127419169835,
+            "y": 6.582899007638951,
+            "heading": -0.09840874396641834,
+            "angularVelocity": 5.891995683553032e-7,
+            "velocityX": -3.764593381767812,
+            "velocityY": 0.37212281000605224,
+            "timestamp": 0.7356605440994373
+        },
+        {
+            "x": 5.416121749902788,
+            "y": 6.617036066627776,
+            "heading": -0.09840868977891702,
+            "angularVelocity": 5.892663593006056e-7,
+            "velocityX": -3.7646819016288298,
+            "velocityY": 0.37122620494064046,
+            "timestamp": 0.8276181121118669
+        },
+        {
+            "x": 5.068594601971295,
+            "y": 6.632477066079001,
+            "heading": -0.0984086463109951,
+            "angularVelocity": 4.72695427552955e-7,
+            "velocityX": -3.7792120370616824,
+            "velocityY": 0.1679143955735957,
+            "timestamp": 0.9195756801242965
+        },
+        {
+            "x": 4.721067428734796,
+            "y": 6.647917495985935,
+            "heading": -0.09840860284326601,
+            "angularVelocity": 4.7269333042542933e-7,
+            "velocityX": -3.7792123122430223,
+            "velocityY": 0.16790820201819795,
+            "timestamp": 1.011533248136726
+        },
+        {
+            "x": 4.3735402523552525,
+            "y": 6.663357855150138,
+            "heading": -0.09840855937552709,
+            "angularVelocity": 4.726934374928119e-7,
+            "velocityX": -3.779212346422309,
+            "velocityY": 0.16790743272066663,
+            "timestamp": 1.1034908161491557
+        },
+        {
+            "x": 4.026013074283842,
+            "y": 6.678798176234082,
+            "heading": -0.09840851590777203,
+            "angularVelocity": 4.7269361285314424e-7,
+            "velocityX": -3.779212364820652,
+            "velocityY": 0.1679070186138197,
+            "timestamp": 1.1954483841615853
+        },
+        {
+            "x": 3.6784858950907164,
+            "y": 6.69423847207055,
+            "heading": -0.09840847244000124,
+            "angularVelocity": 4.7269378404370134e-7,
+            "velocityX": -3.7792123770188395,
+            "velocityY": 0.16790674405809006,
+            "timestamp": 1.2874059521740149
+        },
+        {
+            "x": 3.330958715199463,
+            "y": 6.709678752193432,
+            "heading": -0.09840842897219951,
+            "angularVelocity": 4.7269412044437995e-7,
+            "velocityX": -3.7792123846106827,
+            "velocityY": 0.16790657317944163,
+            "timestamp": 1.3793635201864445
+        },
+        {
+            "x": 2.991492162018093,
+            "y": 6.7253542779205,
+            "heading": -0.07153342912076269,
+            "angularVelocity": 0.2922543563549246,
+            "velocityX": -3.6915564484642,
+            "velocityY": 0.17046477050097905,
+            "timestamp": 1.471321088198874
+        },
+        {
+            "x": 2.713831676976805,
+            "y": 6.738399816428541,
+            "heading": 0.08699262812295999,
+            "angularVelocity": 1.7239044123295555,
+            "velocityX": -3.0194413689122035,
+            "velocityY": 0.14186476208546492,
+            "timestamp": 1.5632786562113037
+        },
+        {
+            "x": 2.5064819943185284,
+            "y": 6.7478572981535265,
+            "heading": 0.2316797715135602,
+            "angularVelocity": 1.5734120259796696,
+            "velocityX": -2.2548408699787488,
+            "velocityY": 0.10284614882059591,
+            "timestamp": 1.6552362242237333
+        },
+        {
+            "x": 2.368710598807467,
+            "y": 6.7539967145328434,
+            "heading": 0.3372042735506754,
+            "angularVelocity": 1.147534719739997,
+            "velocityX": -1.4982061671361233,
+            "velocityY": 0.06676357924654308,
+            "timestamp": 1.747193792236163
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0.6067551334304849,
+            "velocityX": -0.7471989559160562,
+            "velocityY": 0.03265947036300673,
+            "timestamp": 1.8391513602485925
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0,
+            "velocityX": -4.676461204476467e-32,
+            "velocityY": 0,
+            "timestamp": 1.931108928261022
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.1.traj
new file mode 100644
index 00000000..5429265f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.1.traj	
@@ -0,0 +1,184 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -7.048442713490885e-38,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.598761685357721,
+            "y": 5.839328892781844,
+            "heading": -0.010877614975927053,
+            "angularVelocity": -0.10375029848266179,
+            "velocityX": -0.774848108801196,
+            "velocityY": 0.4037314494285639,
+            "timestamp": 0.10484418006512992
+        },
+        {
+            "x": 7.436285061134455,
+            "y": 5.923986667743732,
+            "heading": -0.03263383242489801,
+            "angularVelocity": -0.20751001567713226,
+            "velocityX": -1.5496961693280003,
+            "velocityY": 0.8074627977375359,
+            "timestamp": 0.20968836013025985
+        },
+        {
+            "x": 7.192570139480125,
+            "y": 6.050973310343331,
+            "heading": -0.06527034306657675,
+            "angularVelocity": -0.31128585889464805,
+            "velocityX": -2.3245441139692504,
+            "velocityY": 1.2111940073422762,
+            "timestamp": 0.3145325401953898
+        },
+        {
+            "x": 6.867616948374112,
+            "y": 6.220288799737083,
+            "heading": -0.10878948434468085,
+            "angularVelocity": -0.41508399656585776,
+            "velocityX": -3.0993917917441864,
+            "velocityY": 1.6149250181418864,
+            "timestamp": 0.4193767202605197
+        },
+        {
+            "x": 6.5158804421759,
+            "y": 6.403559919066418,
+            "heading": -0.10878948853448296,
+            "angularVelocity": -3.996218113987742e-8,
+            "velocityX": -3.3548500830442913,
+            "velocityY": 1.7480333120587623,
+            "timestamp": 0.5242209003256496
+        },
+        {
+            "x": 6.14377891604402,
+            "y": 6.540845489176542,
+            "heading": -0.10878948531130445,
+            "angularVelocity": 3.074256021150283e-8,
+            "velocityX": -3.549090907103546,
+            "velocityY": 1.309424805695852,
+            "timestamp": 0.6290650803907796
+        },
+        {
+            "x": 5.750100268474419,
+            "y": 6.5890532439035745,
+            "heading": -0.10878941324219087,
+            "angularVelocity": 6.873926003335433e-7,
+            "velocityX": -3.754892711498581,
+            "velocityY": 0.4598038221776824,
+            "timestamp": 0.7339092604559095
+        },
+        {
+            "x": 5.35529697720587,
+            "y": 6.626964014921626,
+            "heading": -0.10878934177449004,
+            "angularVelocity": 6.816563474487017e-7,
+            "velocityX": -3.7656195224503146,
+            "velocityY": 0.3615915637329853,
+            "timestamp": 0.8387534405210395
+        },
+        {
+            "x": 4.9606240931120595,
+            "y": 6.587718813133039,
+            "heading": -0.10878931155021784,
+            "angularVelocity": 2.882780157548741e-7,
+            "velocityX": -3.7643757035310585,
+            "velocityY": -0.3743193161910223,
+            "timestamp": 0.9435976205861695
+        },
+        {
+            "x": 4.585497747567555,
+            "y": 6.458927912145357,
+            "heading": -0.10878930710495487,
+            "angularVelocity": 4.239875765382173e-8,
+            "velocityX": -3.5779415253328617,
+            "velocityY": -1.2284029586351546,
+            "timestamp": 1.0484418006512994
+        },
+        {
+            "x": 4.215349929173217,
+            "y": 6.316458743577884,
+            "heading": -0.10878930270384918,
+            "angularVelocity": 4.197758707259389e-8,
+            "velocityX": -3.530456513317188,
+            "velocityY": -1.3588657804273945,
+            "timestamp": 1.1532859807164293
+        },
+        {
+            "x": 3.8452021463353314,
+            "y": 6.173989482631574,
+            "heading": -0.10878929830273087,
+            "angularVelocity": 4.197770719831182e-8,
+            "velocityX": -3.530456174181034,
+            "velocityY": -1.3588666615333942,
+            "timestamp": 1.2581301607815591
+        },
+        {
+            "x": 3.475054376591526,
+            "y": 6.0315201876656985,
+            "heading": -0.10878929390160265,
+            "angularVelocity": 4.197780205439048e-8,
+            "velocityX": -3.530456049290165,
+            "velocityY": -1.3588669860107905,
+            "timestamp": 1.362974340846689
+        },
+        {
+            "x": 3.104906616234201,
+            "y": 5.889050868312964,
+            "heading": -0.10878928950045777,
+            "angularVelocity": 4.19779608468647e-8,
+            "velocityX": -3.530455959762256,
+            "velocityY": -1.358867218611777,
+            "timestamp": 1.4678185209118189
+        },
+        {
+            "x": 2.7629439773689555,
+            "y": 5.757430528072475,
+            "heading": -0.06527490613894259,
+            "angularVelocity": 0.4150386157294016,
+            "velocityX": -3.2616272896866185,
+            "velocityY": -1.2553900479618894,
+            "timestamp": 1.5726627009769487
+        },
+        {
+            "x": 2.506471990883214,
+            "y": 5.658715266844828,
+            "heading": -0.03263773447849684,
+            "angularVelocity": 0.311292163667756,
+            "velocityX": -2.446220537243172,
+            "velocityY": -0.9415425936501655,
+            "timestamp": 1.6775068810420786
+        },
+        {
+            "x": 2.335490664707938,
+            "y": 5.592905089907167,
+            "heading": -0.010879217132893882,
+            "angularVelocity": 0.20753195200808147,
+            "velocityX": -1.6308137091544919,
+            "velocityY": -0.6276950890052175,
+            "timestamp": 1.7823510611072084
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0.10376557979790346,
+            "velocityX": -0.8154068700316104,
+            "velocityY": -0.3138475582214142,
+            "timestamp": 1.8871952411723383
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 2.158258486540926e-34,
+            "velocityY": 0,
+            "timestamp": 1.9920394212374681
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.1.traj
new file mode 100644
index 00000000..cf63c7e3
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.1.traj	
@@ -0,0 +1,346 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": -2.981242670977675e-31,
+            "velocityX": -5.763030958279131e-32,
+            "velocityY": 3.5928963704249306e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6464505983184665,
+            "y": 5.761517456765686,
+            "heading": 0.04441523610418216,
+            "angularVelocity": 0.571175712894524,
+            "velocityX": -0.43144211544178024,
+            "velocityY": -0.4563021319898165,
+            "timestamp": 0.07776107264628128
+        },
+        {
+            "x": 7.57910774134765,
+            "y": 5.690410071738714,
+            "heading": 0.1294441379757556,
+            "angularVelocity": 1.0934635927458562,
+            "velocityX": -0.8660227370723712,
+            "velocityY": -0.914434209908928,
+            "timestamp": 0.15552214529256256
+        },
+        {
+            "x": 7.4776816969238515,
+            "y": 5.583411802725407,
+            "heading": 0.24873158333085876,
+            "angularVelocity": 1.5340252043296483,
+            "velocityX": -1.3043292867777738,
+            "velocityY": -1.3759875651409672,
+            "timestamp": 0.23328321793884382
+        },
+        {
+            "x": 7.341761998810456,
+            "y": 5.440105324158156,
+            "heading": 0.38885380810835574,
+            "angularVelocity": 1.8019584865410045,
+            "velocityX": -1.747914393255167,
+            "velocityY": -1.842907687489359,
+            "timestamp": 0.3110442905851251
+        },
+        {
+            "x": 7.171108690296084,
+            "y": 5.260470754663091,
+            "heading": 0.5147184350397563,
+            "angularVelocity": 1.6186071339824826,
+            "velocityX": -2.1945853202236263,
+            "velocityY": -2.3100834824152265,
+            "timestamp": 0.3888053632314064
+        },
+        {
+            "x": 6.970033766909185,
+            "y": 5.052418524467747,
+            "heading": 0.5282282166004705,
+            "angularVelocity": 0.17373450623768458,
+            "velocityX": -2.585804394720051,
+            "velocityY": -2.67553189681076,
+            "timestamp": 0.4665664358776877
+        },
+        {
+            "x": 6.768438732548067,
+            "y": 4.838192511419719,
+            "heading": 0.5282282675213732,
+            "angularVelocity": 6.548379670470849e-7,
+            "velocityX": -2.592492972391604,
+            "velocityY": -2.7549261572367425,
+            "timestamp": 0.5443275085239689
+        },
+        {
+            "x": 6.5423880710685225,
+            "y": 4.649952125007829,
+            "heading": 0.5282283002658353,
+            "angularVelocity": 4.210906690145718e-7,
+            "velocityX": -2.9069900116707847,
+            "velocityY": -2.42075346964613,
+            "timestamp": 0.6220885811702502
+        },
+        {
+            "x": 6.292219183977312,
+            "y": 4.49519788912002,
+            "heading": 0.5282283052888641,
+            "angularVelocity": 6.459567356551782e-8,
+            "velocityX": -3.217148099656167,
+            "velocityY": -1.9901247580747212,
+            "timestamp": 0.6998496538165315
+        },
+        {
+            "x": 6.0420493950115075,
+            "y": 4.3404451111724525,
+            "heading": 0.5282283103117955,
+            "angularVelocity": 6.459441933764687e-8,
+            "velocityX": -3.217159697677684,
+            "velocityY": -1.990106009101862,
+            "timestamp": 0.7776107264628128
+        },
+        {
+            "x": 5.780250458218128,
+            "y": 4.206300927357815,
+            "heading": 0.5282283154384368,
+            "angularVelocity": 6.592812032299238e-8,
+            "velocityX": -3.366709432935023,
+            "velocityY": -1.7250814482051704,
+            "timestamp": 0.8553717991090941
+        },
+        {
+            "x": 5.499389832161033,
+            "y": 4.11883296395985,
+            "heading": 0.5282284192975907,
+            "angularVelocity": 0.0000013356188425542589,
+            "velocityX": -3.611840944307328,
+            "velocityY": -1.1248296920470104,
+            "timestamp": 0.9331328717553754
+        },
+        {
+            "x": 5.2375954046767585,
+            "y": 3.9846908767107623,
+            "heading": 0.5282448470390027,
+            "angularVelocity": 0.0002112591919891304,
+            "velocityX": -3.3666514436488617,
+            "velocityY": -1.7250544865716144,
+            "timestamp": 1.0108939444016567
+        },
+        {
+            "x": 4.9990709441074195,
+            "y": 3.8334805845295827,
+            "heading": 0.56802081206443,
+            "angularVelocity": 0.5115151279660829,
+            "velocityX": -3.067401881842984,
+            "velocityY": -1.944549978997939,
+            "timestamp": 1.0886550170479379
+        },
+        {
+            "x": 4.779271759525853,
+            "y": 3.6735037985529155,
+            "heading": 0.6417219019727908,
+            "angularVelocity": 0.9477890080506808,
+            "velocityX": -2.826596613210087,
+            "velocityY": -2.057286255609729,
+            "timestamp": 1.166416089694219
+        },
+        {
+            "x": 4.5625385904493,
+            "y": 3.4912294193947493,
+            "heading": 0.6783171685956464,
+            "angularVelocity": 0.47061164895862556,
+            "velocityX": -2.7871679453603333,
+            "velocityY": -2.3440311836655594,
+            "timestamp": 1.2441771623405002
+        },
+        {
+            "x": 4.372028277853454,
+            "y": 3.267088484433369,
+            "heading": 0.6783172621445687,
+            "angularVelocity": 0.0000012030302442626518,
+            "velocityX": -2.4499445045265382,
+            "velocityY": -2.8824311102413667,
+            "timestamp": 1.3219382349867814
+        },
+        {
+            "x": 4.1999208824099785,
+            "y": 3.028525386131778,
+            "heading": 0.678317151805174,
+            "angularVelocity": -0.0000014189541221835924,
+            "velocityX": -2.213284739864076,
+            "velocityY": -3.067898759405809,
+            "timestamp": 1.3996993076330626
+        },
+        {
+            "x": 4.015537599692405,
+            "y": 2.8383590216563426,
+            "heading": 0.6532286408155417,
+            "angularVelocity": -0.32263586568249475,
+            "velocityX": -2.371151482905778,
+            "velocityY": -2.4455213644037763,
+            "timestamp": 1.4774603802793438
+        },
+        {
+            "x": 3.8266113642227695,
+            "y": 2.6983370404519986,
+            "heading": 0.6187227841336836,
+            "angularVelocity": -0.4437420357974964,
+            "velocityX": -2.429573423311953,
+            "velocityY": -1.8006693637223163,
+            "timestamp": 1.555221452925625
+        },
+        {
+            "x": 3.635277950307217,
+            "y": 2.6086556112169066,
+            "heading": 0.5790475746735078,
+            "angularVelocity": -0.5102194209775099,
+            "velocityX": -2.460529509229992,
+            "velocityY": -1.1532946522360021,
+            "timestamp": 1.6329825255719062
+        },
+        {
+            "x": 3.4425296783447266,
+            "y": 2.5693652629852295,
+            "heading": 0.5361762744054714,
+            "angularVelocity": -0.5513208448531661,
+            "velocityX": -2.4787244491760223,
+            "velocityY": -0.5052701421751292,
+            "timestamp": 1.7107435982181873
+        },
+        {
+            "x": 3.273952117174966,
+            "y": 2.5733990398146904,
+            "heading": 0.49726118807033537,
+            "angularVelocity": -0.5743772146540196,
+            "velocityX": -2.4881638242809143,
+            "velocityY": 0.05953756545439798,
+            "timestamp": 1.7784953915385004
+        },
+        {
+            "x": 3.1059881626789445,
+            "y": 2.6157002802478813,
+            "heading": 0.456882179671172,
+            "angularVelocity": -0.5959843484623638,
+            "velocityX": -2.4791071389347814,
+            "velocityY": 0.6243560260198714,
+            "timestamp": 1.8462471848588136
+        },
+        {
+            "x": 2.9403803042277463,
+            "y": 2.6962017103764078,
+            "heading": 0.41517848324514556,
+            "angularVelocity": -0.6155364217277942,
+            "velocityX": -2.444331734043506,
+            "velocityY": 1.1881815400506988,
+            "timestamp": 1.9139989781791267
+        },
+        {
+            "x": 2.7796953039379972,
+            "y": 2.8146585273254012,
+            "heading": 0.3723609476398507,
+            "angularVelocity": -0.6319764172568042,
+            "velocityX": -2.3716715442508174,
+            "velocityY": 1.7483938231562413,
+            "timestamp": 1.9817507714994398
+        },
+        {
+            "x": 2.628009703912687,
+            "y": 2.970316258543535,
+            "heading": 0.3287788631859365,
+            "angularVelocity": -0.6432609724124856,
+            "velocityX": -2.23884258396202,
+            "velocityY": 2.2974702748047298,
+            "timestamp": 2.0495025648197527
+        },
+        {
+            "x": 2.4923790399755665,
+            "y": 3.160718496478581,
+            "heading": 0.28508218813026037,
+            "angularVelocity": -0.6449523018392979,
+            "velocityX": -2.001875630005736,
+            "velocityY": 2.8102907480968544,
+            "timestamp": 2.1172543581400656
+        },
+        {
+            "x": 2.384956097652455,
+            "y": 3.376988776463332,
+            "heading": 0.24257394609876834,
+            "angularVelocity": -0.627411319291935,
+            "velocityX": -1.5855365158416335,
+            "velocityY": 3.192096760631575,
+            "timestamp": 2.1850061514603785
+        },
+        {
+            "x": 2.3144199991578422,
+            "y": 3.6018152629752547,
+            "heading": 0.17996830737750358,
+            "angularVelocity": -0.9240440090682317,
+            "velocityX": -1.0410956675513523,
+            "velocityY": 3.3183842890918167,
+            "timestamp": 2.2527579447806914
+        },
+        {
+            "x": 2.277461106698391,
+            "y": 3.8129334421080285,
+            "heading": 0.07321465766316056,
+            "angularVelocity": -1.57565791963083,
+            "velocityX": -0.5455042685692306,
+            "velocityY": 3.1160530044520236,
+            "timestamp": 2.3205097381010042
+        },
+        {
+            "x": 2.2642620699526455,
+            "y": 3.9942911908489713,
+            "heading": -0.04721496632936169,
+            "angularVelocity": -1.7775119755599884,
+            "velocityX": -0.1948145738865332,
+            "velocityY": 2.676796286166625,
+            "timestamp": 2.388261531421317
+        },
+        {
+            "x": 2.2685016834700633,
+            "y": 4.1415779064179725,
+            "heading": -0.1677701441610935,
+            "angularVelocity": -1.7793651197065368,
+            "velocityX": 0.06257566493294611,
+            "velocityY": 2.173916118687331,
+            "timestamp": 2.45601332474163
+        },
+        {
+            "x": 2.2803498467055876,
+            "y": 4.251629868834139,
+            "heading": -0.2721018795383186,
+            "angularVelocity": -1.5399110527447033,
+            "velocityX": 0.17487600925202312,
+            "velocityY": 1.624340213341216,
+            "timestamp": 2.523765118061943
+        },
+        {
+            "x": 2.2924250535428135,
+            "y": 4.3241863896986095,
+            "heading": -0.3487735727111741,
+            "angularVelocity": -1.1316555535345214,
+            "velocityX": 0.17822711762236845,
+            "velocityY": 1.0709166105971757,
+            "timestamp": 2.591516911382256
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.6084920452794201,
+            "velocityX": 0.1118043683563332,
+            "velocityY": 0.5286001823165565,
+            "timestamp": 2.6592687047025687
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -7.720140427747849e-31,
+            "velocityX": 0,
+            "velocityY": -1.167304580662352e-31,
+            "timestamp": 2.7270204980228816
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.1.traj
new file mode 100644
index 00000000..57334e24
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.1.traj	
@@ -0,0 +1,139 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": -2.1749467140472343e-39,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 4.203445372005539,
+            "heading": -7.837837605587582e-19,
+            "angularVelocity": -8.185686568731239e-18,
+            "velocityX": 1.377912473786286e-33,
+            "velocityY": 0.7983807350946673,
+            "timestamp": 0.09575052183150974
+        },
+        {
+            "x": 7.68,
+            "y": 4.356336112982689,
+            "heading": -2.3565356003209568e-18,
+            "angularVelocity": -1.6425517161035916e-17,
+            "velocityX": 3.260516050321855e-34,
+            "velocityY": 1.5967614385035824,
+            "timestamp": 0.19150104366301948
+        },
+        {
+            "x": 7.68,
+            "y": 4.5856722168636,
+            "heading": -4.728579560129296e-18,
+            "angularVelocity": -2.4773170043438095e-17,
+            "velocityX": -1.693775140889528e-33,
+            "velocityY": 2.3951420785410225,
+            "timestamp": 0.28725156549452924
+        },
+        {
+            "x": 7.68,
+            "y": 4.891453665444746,
+            "heading": -7.932188825954499e-18,
+            "angularVelocity": -3.345787785122934e-17,
+            "velocityX": -9.607950389092107e-34,
+            "velocityY": 3.1935225284643685,
+            "timestamp": 0.38300208732603896
+        },
+        {
+            "x": 7.68,
+            "y": 5.253672199266821,
+            "heading": -4.35728250993897e-17,
+            "angularVelocity": -3.722239377050204e-16,
+            "velocityX": -3.660377688643743e-33,
+            "velocityY": 3.7829405719528495,
+            "timestamp": 0.4787526091575487
+        },
+        {
+            "x": 7.68,
+            "y": 5.61589073308894,
+            "heading": -3.405116375197899e-17,
+            "angularVelocity": 9.944239639932065e-17,
+            "velocityX": -5.791062266320423e-33,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.5745031309890585
+        },
+        {
+            "x": 7.68,
+            "y": 5.978109266911059,
+            "heading": 5.876472199762529e-18,
+            "angularVelocity": 4.169965362904391e-16,
+            "velocityX": -1.6171107443968654e-32,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.6702536528205683
+        },
+        {
+            "x": 7.68,
+            "y": 6.340327800733179,
+            "heading": 2.547705878984039e-17,
+            "angularVelocity": 2.047047495424479e-16,
+            "velocityX": -4.3724642588945734e-33,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.766004174652078
+        },
+        {
+            "x": 7.68,
+            "y": 6.702546334555254,
+            "heading": 7.687233753276096e-18,
+            "angularVelocity": -1.8579350479017399e-16,
+            "velocityX": -2.6103711792186515e-33,
+            "velocityY": 3.7829405719528495,
+            "timestamp": 0.8617546964835878
+        },
+        {
+            "x": 7.68,
+            "y": 7.0083277831364,
+            "heading": 4.613320516549708e-18,
+            "angularVelocity": -3.210335754994756e-17,
+            "velocityX": 1.2273832122580678e-32,
+            "velocityY": 3.1935225284643685,
+            "timestamp": 0.9575052183150976
+        },
+        {
+            "x": 7.68,
+            "y": 7.237663887017311,
+            "heading": 2.3068261479777784e-18,
+            "angularVelocity": -2.4088582761110343e-17,
+            "velocityX": 8.179933156696061e-33,
+            "velocityY": 2.3951420785410225,
+            "timestamp": 1.0532557401466074
+        },
+        {
+            "x": 7.68,
+            "y": 7.390554627994461,
+            "heading": 7.689558058679801e-19,
+            "angularVelocity": -1.6061221530256843e-17,
+            "velocityX": 9.843160057308665e-33,
+            "velocityY": 1.5967614385035824,
+            "timestamp": 1.1490062619781172
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": -8.030826271263815e-18,
+            "velocityX": 3.1854559448557055e-33,
+            "velocityY": 0.7983807350946673,
+            "timestamp": 1.244756783809627
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 2.257351446612169e-43,
+            "velocityY": -1.1946741683702123e-38,
+            "timestamp": 1.3405073056411367
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C2.1.traj
new file mode 100644
index 00000000..5ffe4bce
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C2.1.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -3.817834980565894e-40,
+            "angularVelocity": -1.9855221195343998e-39,
+            "velocityX": -9.621145750064654e-40,
+            "velocityY": 1.4575297691012433e-39,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 4.210500005575846,
+            "heading": 3.479114156010015e-22,
+            "angularVelocity": 3.4766412561552746e-21,
+            "velocityX": 2.7068848151306367e-33,
+            "velocityY": 0.8344065481228213,
+            "timestamp": 0.10007112931184182
+        },
+        {
+            "x": 7.68,
+            "y": 4.377500013629847,
+            "heading": 1.081691491825702e-21,
+            "angularVelocity": 7.332585052389997e-21,
+            "velocityX": 5.1589068200097806e-33,
+            "velocityY": 1.6688130652907358,
+            "timestamp": 0.20014225862368365
+        },
+        {
+            "x": 7.68,
+            "y": 4.628000017966616,
+            "heading": 2.2511586271387426e-21,
+            "angularVelocity": 1.1686358860616172e-20,
+            "velocityX": 5.8498916949463956e-33,
+            "velocityY": 2.503219520548835,
+            "timestamp": 0.3002133879355255
+        },
+        {
+            "x": 7.68,
+            "y": 4.962,
+            "heading": 4.481673966100434e-21,
+            "angularVelocity": 2.228929929167172e-20,
+            "velocityX": 6.576791251337367e-33,
+            "velocityY": 3.3376257900775013,
+            "timestamp": 0.4002845172473673
+        },
+        {
+            "x": 7.68,
+            "y": 5.295999982033384,
+            "heading": 1.4391637904192512e-21,
+            "angularVelocity": -3.0403475801105514e-20,
+            "velocityX": 1.0509973010204301e-32,
+            "velocityY": 3.3376257900775013,
+            "timestamp": 0.5003556465592092
+        },
+        {
+            "x": 7.68,
+            "y": 5.5464999863701525,
+            "heading": 4.7457500532831525e-22,
+            "angularVelocity": -9.63903176962576e-21,
+            "velocityX": 7.983324746463204e-33,
+            "velocityY": 2.503219520548835,
+            "timestamp": 0.600426775871051
+        },
+        {
+            "x": 7.68,
+            "y": 5.713499994424153,
+            "heading": 1.0909640932435146e-22,
+            "angularVelocity": -3.652188228166023e-21,
+            "velocityX": 7.341551448907972e-33,
+            "velocityY": 1.6688130652907358,
+            "timestamp": 0.7004979051828928
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -9.42965601107167e-40,
+            "angularVelocity": -1.09018866193586e-21,
+            "velocityX": 7.44470103227488e-33,
+            "velocityY": 0.8344065481228213,
+            "timestamp": 0.8005690344947346
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 8.249839679439821e-43,
+            "angularVelocity": 9.431229878048832e-39,
+            "velocityX": 1.012759498983901e-41,
+            "velocityY": 5.142787784847508e-39,
+            "timestamp": 0.9006401638065764
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.1.traj
new file mode 100644
index 00000000..1862a6fc
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.1.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -1.6773104267984464e-40,
+            "angularVelocity": -1.9855220924111835e-39,
+            "velocityX": -5.3948128499400994e-40,
+            "velocityY": -2.7550648847397363e-40,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 4.043499994424153,
+            "heading": -2.181197470484939e-22,
+            "angularVelocity": -2.1796471324426728e-21,
+            "velocityX": 2.2751291494664054e-33,
+            "velocityY": -0.8344065481228213,
+            "timestamp": 0.10007112931184182
+        },
+        {
+            "x": 7.68,
+            "y": 3.8764999863701526,
+            "heading": -1.0976856482379068e-21,
+            "angularVelocity": -8.789407177499062e-21,
+            "velocityX": 3.967849397509383e-33,
+            "velocityY": -1.668813065290736,
+            "timestamp": 0.20014225862368365
+        },
+        {
+            "x": 7.68,
+            "y": 3.6259999820333837,
+            "heading": -3.357689035831156e-21,
+            "angularVelocity": -2.2583970074080117e-20,
+            "velocityX": 6.1318806903820435e-33,
+            "velocityY": -2.503219520548835,
+            "timestamp": 0.3002133879355255
+        },
+        {
+            "x": 7.68,
+            "y": 3.2919999999999994,
+            "heading": -1.0603568936345678e-20,
+            "angularVelocity": -7.2407296109284e-20,
+            "velocityX": 8.195284274358776e-33,
+            "velocityY": -3.3376257900775017,
+            "timestamp": 0.4002845172473673
+        },
+        {
+            "x": 7.68,
+            "y": 2.9580000179666155,
+            "heading": -4.785144304302103e-21,
+            "angularVelocity": 5.814288954429956e-20,
+            "velocityX": 7.586727405106972e-33,
+            "velocityY": -3.3376257900775013,
+            "timestamp": 0.5003556465592092
+        },
+        {
+            "x": 7.68,
+            "y": 2.7075000136298466,
+            "heading": -2.6182090767908737e-21,
+            "angularVelocity": 2.165395003165498e-20,
+            "velocityX": 5.326134827450793e-33,
+            "velocityY": -2.503219520548835,
+            "timestamp": 0.600426775871051
+        },
+        {
+            "x": 7.68,
+            "y": 2.540500005575846,
+            "heading": -9.668053802454109e-22,
+            "angularVelocity": 1.6502299039719674e-20,
+            "velocityX": 3.4348416075821124e-33,
+            "velocityY": -1.6688130652907358,
+            "timestamp": 0.7004979051828928
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 1.8181769494337196e-40,
+            "angularVelocity": 9.661181877631647e-21,
+            "velocityX": 1.7280403247932335e-33,
+            "velocityY": -0.8344065481228213,
+            "timestamp": 0.8005690344947346
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -1.6883012335149235e-41,
+            "angularVelocity": -1.9855220810412424e-39,
+            "velocityX": -1.2101303998889038e-42,
+            "velocityY": -7.163168700323315e-39,
+            "timestamp": 0.9006401638065764
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.1.traj
new file mode 100644
index 00000000..9c555123
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.1.traj	
@@ -0,0 +1,139 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": -1.1842498081533988e-39,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 4.050554627994461,
+            "heading": 1.1070575354189048e-18,
+            "angularVelocity": 1.156189558439314e-17,
+            "velocityX": 1.1339790734570631e-33,
+            "velocityY": -0.7983807350946678,
+            "timestamp": 0.09575052183150978
+        },
+        {
+            "x": 7.68,
+            "y": 3.897663887017311,
+            "heading": 3.320268139408775e-18,
+            "angularVelocity": 2.3114345091427606e-17,
+            "velocityX": 5.50963145740529e-34,
+            "velocityY": -1.596761438503583,
+            "timestamp": 0.19150104366301957
+        },
+        {
+            "x": 7.68,
+            "y": 3.6683277831364007,
+            "heading": 6.638014774282681e-18,
+            "angularVelocity": 3.4649906568297676e-17,
+            "velocityX": 4.767145470149766e-34,
+            "velocityY": -2.3951420785410233,
+            "timestamp": 0.28725156549452935
+        },
+        {
+            "x": 7.68,
+            "y": 3.362546334555254,
+            "heading": 1.1056344671920077e-17,
+            "angularVelocity": 4.614418609717799e-17,
+            "velocityX": -5.447572473811757e-34,
+            "velocityY": -3.19352252846437,
+            "timestamp": 0.38300208732603913
+        },
+        {
+            "x": 7.68,
+            "y": 3.0003278007331793,
+            "heading": 2.1759975564527066e-17,
+            "angularVelocity": 1.11786658577609e-16,
+            "velocityX": -1.9220305893434716e-33,
+            "velocityY": -3.7829405719528495,
+            "timestamp": 0.4787526091575489
+        },
+        {
+            "x": 7.68,
+            "y": 2.63810926691106,
+            "heading": -1.7246492437320545e-17,
+            "angularVelocity": -4.0737603572006183e-16,
+            "velocityX": 5.877602560652089e-33,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.5745031309890587
+        },
+        {
+            "x": 7.68,
+            "y": 2.27589073308894,
+            "heading": -9.722837303851897e-18,
+            "angularVelocity": 7.857560449339153e-17,
+            "velocityX": 3.350655179860002e-33,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.6702536528205685
+        },
+        {
+            "x": 7.68,
+            "y": 1.9136721992668206,
+            "heading": -4.541786403704224e-17,
+            "angularVelocity": -3.727919811862982e-16,
+            "velocityX": 1.4553646654154268e-32,
+            "velocityY": -3.782940571953311,
+            "timestamp": 0.7660041746520783
+        },
+        {
+            "x": 7.68,
+            "y": 1.5514536654447455,
+            "heading": -1.11893051502424e-17,
+            "angularVelocity": 3.5747647356983727e-16,
+            "velocityX": -5.35720215261278e-33,
+            "velocityY": -3.7829405719528495,
+            "timestamp": 0.861754696483588
+        },
+        {
+            "x": 7.68,
+            "y": 1.2456722168635992,
+            "heading": -6.7032993083141955e-18,
+            "angularVelocity": 4.6850980602319425e-17,
+            "velocityX": -4.3527390380082413e-33,
+            "velocityY": -3.19352252846437,
+            "timestamp": 0.9575052183150978
+        },
+        {
+            "x": 7.68,
+            "y": 1.016336112982689,
+            "heading": -3.348658686079055e-18,
+            "angularVelocity": 3.5035220254491646e-17,
+            "velocityX": -6.634490282826358e-33,
+            "velocityY": -2.395142078541024,
+            "timestamp": 1.0532557401466076
+        },
+        {
+            "x": 7.68,
+            "y": 0.8634453720055388,
+            "heading": -1.115588951241814e-18,
+            "angularVelocity": 2.3321750022200153e-17,
+            "velocityX": -4.820835446954302e-33,
+            "velocityY": -1.596761438503583,
+            "timestamp": 1.1490062619781174
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 1.1650996045214628e-17,
+            "velocityX": -3.709395222464697e-33,
+            "velocityY": -0.7983807350946678,
+            "timestamp": 1.2447567838096272
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 6.978360472267901e-43,
+            "velocityY": -3.6401965239950315e-38,
+            "timestamp": 1.340507305641137
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.1.traj
new file mode 100644
index 00000000..e393ae58
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.1.traj	
@@ -0,0 +1,157 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -7.709535672280055e-33,
+            "angularVelocity": 3.0776034223095e-32,
+            "velocityX": -2.0787724059290825e-32,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.653625318689537,
+            "y": 4.193887618013532,
+            "heading": -0.05699658004084171,
+            "angularVelocity": -0.6065329552716214,
+            "velocityX": -0.28066795214939116,
+            "velocityY": 0.7117891037630026,
+            "timestamp": 0.0939711182145365
+        },
+        {
+            "x": 7.600609429037226,
+            "y": 4.328102182971276,
+            "heading": -0.1617251039431606,
+            "angularVelocity": -1.1144756590341216,
+            "velocityX": -0.5641721697008566,
+            "velocityY": 1.4282533560080801,
+            "timestamp": 0.187942236429073
+        },
+        {
+            "x": 7.520673672603614,
+            "y": 4.5302373715153195,
+            "heading": -0.299017155202882,
+            "angularVelocity": -1.4610026343017766,
+            "velocityX": -0.8506417498525306,
+            "velocityY": 2.151035258328725,
+            "timestamp": 0.2819133546436095
+        },
+        {
+            "x": 7.413680530437971,
+            "y": 4.80084724043498,
+            "heading": -0.43363333911195256,
+            "angularVelocity": -1.4325272111984575,
+            "velocityX": -1.1385747472045327,
+            "velocityY": 2.8797131933862543,
+            "timestamp": 0.375884472858146
+        },
+        {
+            "x": 7.279380160390009,
+            "y": 5.1284214711267,
+            "heading": -0.4371879316114584,
+            "angularVelocity": -0.037826436111898676,
+            "velocityX": -1.4291664566697349,
+            "velocityY": 3.4859032957750506,
+            "timestamp": 0.46985559107268254
+        },
+        {
+            "x": 7.147763767829931,
+            "y": 5.458645997615663,
+            "heading": -0.43718797458387054,
+            "angularVelocity": -4.5729382513446123e-7,
+            "velocityX": -1.4006047289934083,
+            "velocityY": 3.514106597466034,
+            "timestamp": 0.563826709287219
+        },
+        {
+            "x": 6.993957305230812,
+            "y": 5.779137314807096,
+            "heading": -0.43718801851214967,
+            "angularVelocity": -4.6746574877776706e-7,
+            "velocityX": -1.6367418577267254,
+            "velocityY": 3.410529993478958,
+            "timestamp": 0.6577978275017555
+        },
+        {
+            "x": 6.777425503833482,
+            "y": 6.0610683140345785,
+            "heading": -0.43718807107725144,
+            "angularVelocity": -5.593750798607861e-7,
+            "velocityX": -2.3042377861566687,
+            "velocityY": 3.000187766030755,
+            "timestamp": 0.751768945716292
+        },
+        {
+            "x": 6.507457353311083,
+            "y": 6.292343710685516,
+            "heading": -0.4371880918313372,
+            "angularVelocity": -2.2085600557820128e-7,
+            "velocityX": -2.87288430372895,
+            "velocityY": 2.4611327506280603,
+            "timestamp": 0.8457400639308286
+        },
+        {
+            "x": 6.1957042493791805,
+            "y": 6.462993619393473,
+            "heading": -0.4369141825473297,
+            "angularVelocity": 0.0029148241418374306,
+            "velocityX": -3.3175417070186306,
+            "velocityY": 1.8159825268692036,
+            "timestamp": 0.9397111821453651
+        },
+        {
+            "x": 5.885878112264826,
+            "y": 6.564806359719212,
+            "heading": -0.3410861416211598,
+            "angularVelocity": 1.0197605684269313,
+            "velocityX": -3.297035759508787,
+            "velocityY": 1.083447151211916,
+            "timestamp": 1.0336823003599016
+        },
+        {
+            "x": 5.602663899704114,
+            "y": 6.605182844765102,
+            "heading": -0.13545254769192583,
+            "angularVelocity": 2.1882637754695176,
+            "velocityX": -3.0138431673669435,
+            "velocityY": 0.4296690920896744,
+            "timestamp": 1.127653418574438
+        },
+        {
+            "x": 5.388992479168701,
+            "y": 6.621862905384601,
+            "heading": 0.0527880784098822,
+            "angularVelocity": 2.0031753338515554,
+            "velocityX": -2.2737988500636965,
+            "velocityY": 0.17750199142483714,
+            "timestamp": 1.2216245367889744
+        },
+        {
+            "x": 5.246852509457665,
+            "y": 6.628278045699216,
+            "heading": 0.19017469868317557,
+            "angularVelocity": 1.4620089968455965,
+            "velocityX": -1.5125920858633375,
+            "velocityY": 0.06826714884852915,
+            "timestamp": 1.3155956550035108
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.263,
+            "angularVelocity": 0.7749753615846516,
+            "velocityX": -0.7539817638001085,
+            "velocityY": 0.01832429296896703,
+            "timestamp": 1.4095667732180472
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.263,
+            "angularVelocity": 6.971645129510918e-32,
+            "velocityX": 3.634932162806737e-32,
+            "velocityY": 0,
+            "timestamp": 1.5035378914325837
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S2.1.traj
new file mode 100644
index 00000000..67cbd06a
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S2.1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 1.358270377512338e-32,
+            "angularVelocity": -4.2989327725546786e-32,
+            "velocityX": 9.28706448380456e-34,
+            "velocityY": 4.1113328098198935e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 7.585313636391529,
+            "y": 4.127746947940637,
+            "heading": -0.08924586799649764,
+            "angularVelocity": -0.8241960155421162,
+            "velocityX": -0.8744396280098535,
+            "velocityY": 0.006898151480975746,
+            "timestamp": 0.10828233370892486
+        },
+        {
+            "x": 7.394793136195202,
+            "y": 4.129317895600586,
+            "heading": -0.2483761167773939,
+            "angularVelocity": -1.4695864351119023,
+            "velocityX": -1.7594790735529262,
+            "velocityY": 0.014507885138232623,
+            "timestamp": 0.21656466741784972
+        },
+        {
+            "x": 7.106938899563738,
+            "y": 4.131715387581193,
+            "heading": -0.440143741107867,
+            "angularVelocity": -1.7709964106053178,
+            "velocityX": -2.6583674988502586,
+            "velocityY": 0.022141118486160887,
+            "timestamp": 0.32484700112677456
+        },
+        {
+            "x": 6.726884682874397,
+            "y": 4.136782702937484,
+            "heading": -0.5152103460374668,
+            "angularVelocity": -0.6932488648738155,
+            "velocityX": -3.509845084341914,
+            "velocityY": 0.04679724921622947,
+            "timestamp": 0.43312933483569943
+        },
+        {
+            "x": 6.317264615210477,
+            "y": 4.138917325587368,
+            "heading": -0.515210397277194,
+            "angularVelocity": -4.7320486478177106e-7,
+            "velocityX": -3.782889171607859,
+            "velocityY": 0.019713489511795114,
+            "timestamp": 0.5414116685446243
+        },
+        {
+            "x": 5.907644548289307,
+            "y": 4.14105209076814,
+            "heading": -0.5152104485168384,
+            "angularVelocity": -4.732041015646636e-7,
+            "velocityX": -3.7828891647484775,
+            "velocityY": 0.019714805801191258,
+            "timestamp": 0.6496940022535491
+        },
+        {
+            "x": 5.500258277815471,
+            "y": 4.183825709430579,
+            "heading": -0.5152105024899405,
+            "angularVelocity": -4.984479029228616e-7,
+            "velocityX": -3.762259793632967,
+            "velocityY": 0.3950193646307994,
+            "timestamp": 0.7579763359624739
+        },
+        {
+            "x": 5.114610765355458,
+            "y": 4.3219164843017905,
+            "heading": -0.5152109227554116,
+            "angularVelocity": -0.000003881200715936794,
+            "velocityX": -3.5614998241234512,
+            "velocityY": 1.2752844359857862,
+            "timestamp": 0.8662586696713988
+        },
+        {
+            "x": 4.790273412388094,
+            "y": 4.533322369220829,
+            "heading": -0.5901314422767667,
+            "angularVelocity": -0.6918997490647923,
+            "velocityX": -2.9952933397170605,
+            "velocityY": 1.952358041038544,
+            "timestamp": 0.9745410033803236
+        },
+        {
+            "x": 4.547771251602889,
+            "y": 4.790487049727204,
+            "heading": -0.7662492650374113,
+            "angularVelocity": -1.6264686650923976,
+            "velocityX": -2.239535780943542,
+            "velocityY": 2.3749458632620883,
+            "timestamp": 1.0828233370892484
+        },
+        {
+            "x": 4.394005608234175,
+            "y": 5.057835455573767,
+            "heading": -0.8024780004938983,
+            "angularVelocity": -0.3345766037318139,
+            "velocityX": -1.4200436775037824,
+            "velocityY": 2.468993756315091,
+            "timestamp": 1.1911056707981733
+        },
+        {
+            "x": 4.299277084870805,
+            "y": 5.304155354811625,
+            "heading": -0.5153879845736841,
+            "angularVelocity": 2.651309831315093,
+            "velocityX": -0.8748289782709269,
+            "velocityY": 2.2747930414945974,
+            "timestamp": 1.2993880045070982
+        },
+        {
+            "x": 4.268346904630433,
+            "y": 5.476762087961931,
+            "heading": -0.20205360473699635,
+            "angularVelocity": 2.893679597624539,
+            "velocityX": -0.2856438273995415,
+            "velocityY": 1.5940433424190128,
+            "timestamp": 1.4076703382160232
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 2.864290108945775e-30,
+            "angularVelocity": 1.8659886411401074,
+            "velocityX": -0.04937928882107947,
+            "velocityY": 0.7687118404911842,
+            "timestamp": 1.515952671924948
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": -1.339793214188515e-31,
+            "velocityX": -1.755749143439243e-31,
+            "velocityY": 0,
+            "timestamp": 1.624235005633873
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S3.1.traj
new file mode 100644
index 00000000..c2db4e34
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S3.1.traj	
@@ -0,0 +1,166 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 4.8594393424661863e-32,
+            "velocityX": 8.070887902586468e-34,
+            "velocityY": -1.5484627527783442e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.613090966537703,
+            "y": 4.130415229492922,
+            "heading": -0.02261774188189381,
+            "angularVelocity": -0.25176724339812206,
+            "velocityX": -0.7447915447554241,
+            "velocityY": 0.038016302406854975,
+            "timestamp": 0.08983591977315897
+        },
+        {
+            "x": 7.479193313628693,
+            "y": 4.137259099498979,
+            "heading": -0.06454119971556918,
+            "angularVelocity": -0.46666698504702897,
+            "velocityX": -1.4904689933291897,
+            "velocityY": 0.0761818883097586,
+            "timestamp": 0.17967183954631794
+        },
+        {
+            "x": 7.278200146895047,
+            "y": 4.147551727639681,
+            "heading": -0.12012980810986089,
+            "angularVelocity": -0.6187793093216658,
+            "velocityX": -2.237336326507015,
+            "velocityY": 0.11457141160316474,
+            "timestamp": 0.2695077593194769
+        },
+        {
+            "x": 7.010006404514947,
+            "y": 4.161329433657577,
+            "heading": -0.1775477987284744,
+            "angularVelocity": -0.6391429036434741,
+            "velocityX": -2.985373145364283,
+            "velocityY": 0.15336522465649807,
+            "timestamp": 0.35934367909263587
+        },
+        {
+            "x": 6.675632733992502,
+            "y": 4.178685588756415,
+            "heading": -0.19285230682653456,
+            "angularVelocity": -0.17036067687364617,
+            "velocityX": -3.7220487235639528,
+            "velocityY": 0.19319839038333309,
+            "timestamp": 0.44917959886579484
+        },
+        {
+            "x": 6.3362226416697665,
+            "y": 4.195852203162663,
+            "heading": -0.19285232652882817,
+            "angularVelocity": -2.193200640663355e-7,
+            "velocityX": -3.7781111740035236,
+            "velocityY": 0.1910885362012055,
+            "timestamp": 0.5390155186389538
+        },
+        {
+            "x": 5.996812544590127,
+            "y": 4.213018723498308,
+            "heading": -0.19285234623213549,
+            "angularVelocity": -2.1933156264800868e-7,
+            "velocityX": -3.7781112269558137,
+            "velocityY": 0.1910874890356692,
+            "timestamp": 0.6288514384121128
+        },
+        {
+            "x": 5.65703115405788,
+            "y": 4.206499194085455,
+            "heading": -0.1928524148916534,
+            "angularVelocity": -7.642946895727943e-7,
+            "velocityX": -3.782244244720454,
+            "velocityY": -0.07257152185432766,
+            "timestamp": 0.7186873581852717
+        },
+        {
+            "x": 5.325195518902672,
+            "y": 4.133157682156235,
+            "heading": -0.192852601151229,
+            "angularVelocity": -0.0000020733763948999505,
+            "velocityX": -3.6937968241559735,
+            "velocityY": -0.8163940672507154,
+            "timestamp": 0.8085232779584307
+        },
+        {
+            "x": 5.022921580906399,
+            "y": 3.999719932132448,
+            "heading": -0.21619143682617745,
+            "angularVelocity": -0.2597940304978627,
+            "velocityX": -3.364733602738329,
+            "velocityY": -1.485349628067107,
+            "timestamp": 0.8983591977315897
+        },
+        {
+            "x": 4.76405537852928,
+            "y": 3.8156128777716756,
+            "heading": -0.2707600447886353,
+            "angularVelocity": -0.6074252715762222,
+            "velocityX": -2.881544520603651,
+            "velocityY": -2.0493701720293274,
+            "timestamp": 0.9881951175047486
+        },
+        {
+            "x": 4.5584097754694755,
+            "y": 3.590752248351908,
+            "heading": -0.3464948618413025,
+            "angularVelocity": -0.8430349156856024,
+            "velocityX": -2.2891244791432657,
+            "velocityY": -2.5030147182477354,
+            "timestamp": 1.0780310372779076
+        },
+        {
+            "x": 4.420003796304514,
+            "y": 3.3682412112408735,
+            "heading": -0.42679529224450924,
+            "angularVelocity": -0.8938566066393836,
+            "velocityX": -1.5406529984228516,
+            "velocityY": -2.476860454851527,
+            "timestamp": 1.1678669570510665
+        },
+        {
+            "x": 4.3340794956684014,
+            "y": 3.187695881574611,
+            "heading": -0.4942860320434666,
+            "angularVelocity": -0.7512667535241839,
+            "velocityX": -0.9564581834687845,
+            "velocityY": -2.009723172222817,
+            "timestamp": 1.2577028768242253
+        },
+        {
+            "x": 4.284833149556787,
+            "y": 3.063288277698013,
+            "heading": -0.5416494132844137,
+            "angularVelocity": -0.5272209752980105,
+            "velocityX": -0.5481810197605272,
+            "velocityY": -1.3848314147664058,
+            "timestamp": 1.3475387965973842
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -0.27105624093568903,
+            "velocityX": -0.24303362855860308,
+            "velocityY": -0.7044874461979944,
+            "timestamp": 1.437374716370543
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 6.221864662981848e-31,
+            "velocityX": 0,
+            "velocityY": 2.6923638006152617e-32,
+            "timestamp": 1.527210636143702
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S4.1.traj
new file mode 100644
index 00000000..2274eedc
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S4.1.traj	
@@ -0,0 +1,238 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 3.185769476987751e-31,
+            "velocityX": 7.333541525795099e-32,
+            "velocityY": -1.3517842061565195e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 7.679078910576486,
+            "y": 4.080614551713116,
+            "heading": -0.023748331346269917,
+            "angularVelocity": -0.3167839884646853,
+            "velocityX": -0.012286605616975374,
+            "velocityY": -0.6187452541731726,
+            "timestamp": 0.07496695606800095
+        },
+        {
+            "x": 7.6772274783441,
+            "y": 3.987757014244699,
+            "heading": -0.06889026621284731,
+            "angularVelocity": -0.6021577670253255,
+            "velocityX": -0.024696644088144176,
+            "velocityY": -1.2386462294692595,
+            "timestamp": 0.1499339121360019
+        },
+        {
+            "x": 7.674435217742111,
+            "y": 3.848312298051719,
+            "heading": -0.1318457680879735,
+            "angularVelocity": -0.8397766853177616,
+            "velocityX": -0.037246551660047825,
+            "velocityY": -1.8600824083946148,
+            "timestamp": 0.22490086820400285
+        },
+        {
+            "x": 7.670687411814683,
+            "y": 3.6621317365631327,
+            "heading": -0.20653378464827088,
+            "angularVelocity": -0.996279167218362,
+            "velocityX": -0.049992771802326974,
+            "velocityY": -2.483501681990474,
+            "timestamp": 0.2998678242720038
+        },
+        {
+            "x": 7.665952301925262,
+            "y": 3.4291021726627307,
+            "heading": -0.27999342706200125,
+            "angularVelocity": -0.9798936260280143,
+            "velocityX": -0.06316262707958344,
+            "velocityY": -3.1084303821676644,
+            "timestamp": 0.3748347803400048
+        },
+        {
+            "x": 7.659724245559776,
+            "y": 3.1517739981457797,
+            "heading": -0.2973184560258276,
+            "angularVelocity": -0.23110220652567398,
+            "velocityX": -0.0830773542389579,
+            "velocityY": -3.699338869586643,
+            "timestamp": 0.44980173640800575
+        },
+        {
+            "x": 7.61848609595722,
+            "y": 2.8711927485616604,
+            "heading": -0.2973184850947423,
+            "angularVelocity": -3.8775636846908307e-7,
+            "velocityX": -0.5500843540339733,
+            "velocityY": -3.742732322352917,
+            "timestamp": 0.5247686924760067
+        },
+        {
+            "x": 7.5316071002697536,
+            "y": 2.6012326390018865,
+            "heading": -0.2973185190564447,
+            "angularVelocity": -4.530222940715824e-7,
+            "velocityX": -1.1588972027711093,
+            "velocityY": -3.6010547008857787,
+            "timestamp": 0.5997356485440076
+        },
+        {
+            "x": 7.401459139481823,
+            "y": 2.3492645367371554,
+            "heading": -0.29731856454052086,
+            "angularVelocity": -6.067216612944958e-7,
+            "velocityX": -1.7360710320141561,
+            "velocityY": -3.3610555300681284,
+            "timestamp": 0.6747026046120086
+        },
+        {
+            "x": 7.231595664022054,
+            "y": 2.1221680676798096,
+            "heading": -0.2973186386883636,
+            "angularVelocity": -9.89073675332688e-7,
+            "velocityX": -2.265844638345654,
+            "velocityY": -3.0292875817359826,
+            "timestamp": 0.7496695606800096
+        },
+        {
+            "x": 7.026654548839687,
+            "y": 1.9261437915792998,
+            "heading": -0.2973188151543491,
+            "angularVelocity": -0.000002353916908579506,
+            "velocityX": -2.7337526549226907,
+            "velocityY": -2.614809062311352,
+            "timestamp": 0.8246365167480105
+        },
+        {
+            "x": 6.792231559753418,
+            "y": 1.766543984413147,
+            "heading": -0.2973195709317279,
+            "angularVelocity": -0.00001008147347055336,
+            "velocityX": -3.127017573897922,
+            "velocityY": -2.1289354075064555,
+            "timestamp": 0.8996034728160115
+        },
+        {
+            "x": 6.610935711613446,
+            "y": 1.6724691195207,
+            "heading": -0.2973321063250889,
+            "angularVelocity": -0.00023216320074468077,
+            "velocityX": -3.3577106919370485,
+            "velocityY": -1.7423243992220112,
+            "timestamp": 0.9535973587619662
+        },
+        {
+            "x": 6.419882816475647,
+            "y": 1.600658398177247,
+            "heading": -0.29770902924967546,
+            "angularVelocity": -0.006980844552986395,
+            "velocityX": -3.538417207626563,
+            "velocityY": -1.329978757508455,
+            "timestamp": 1.007591244707921
+        },
+        {
+            "x": 6.223144239006287,
+            "y": 1.5524565272909876,
+            "heading": -0.30199019992690734,
+            "angularVelocity": -0.07928991592723345,
+            "velocityX": -3.6437195438439516,
+            "velocityY": -0.892728316211746,
+            "timestamp": 1.0615851306538757
+        },
+        {
+            "x": 6.027009837481886,
+            "y": 1.528287215105613,
+            "heading": -0.31920772077283077,
+            "angularVelocity": -0.31887908314582397,
+            "velocityX": -3.63252983348375,
+            "velocityY": -0.44763053745692566,
+            "timestamp": 1.1155790165998305
+        },
+        {
+            "x": 5.840489459577601,
+            "y": 1.5253276945335545,
+            "heading": -0.3622266817701244,
+            "angularVelocity": -0.7967376350788925,
+            "velocityX": -3.454472198777908,
+            "velocityY": -0.05481214252704887,
+            "timestamp": 1.1695729025457853
+        },
+        {
+            "x": 5.673855769295343,
+            "y": 1.5359985529910274,
+            "heading": -0.41631972448131044,
+            "angularVelocity": -1.0018364443214895,
+            "velocityX": -3.086158504113808,
+            "velocityY": 0.19763086635687102,
+            "timestamp": 1.22356678849174
+        },
+        {
+            "x": 5.530708201179271,
+            "y": 1.5529237053226916,
+            "heading": -0.47196047387840134,
+            "angularVelocity": -1.0305009247305976,
+            "velocityX": -2.651181066303626,
+            "velocityY": 0.3134642383140704,
+            "timestamp": 1.2775606744376948
+        },
+        {
+            "x": 5.411784437145291,
+            "y": 1.57153818466476,
+            "heading": -0.5236032237572267,
+            "angularVelocity": -0.956455512954414,
+            "velocityX": -2.2025413053807186,
+            "velocityY": 0.34475161429762524,
+            "timestamp": 1.3315545603836496
+        },
+        {
+            "x": 5.3170603467926,
+            "y": 1.5890782395606275,
+            "heading": -0.5679992830659408,
+            "angularVelocity": -0.8222423433861833,
+            "velocityX": -1.754348454332506,
+            "velocityY": 0.32485261226475975,
+            "timestamp": 1.3855484463296044
+        },
+        {
+            "x": 5.246334967071984,
+            "y": 1.6037618746917708,
+            "heading": -0.6030801100679523,
+            "angularVelocity": -0.6497185077054303,
+            "velocityX": -1.309877562644932,
+            "velocityY": 0.2719499601462664,
+            "timestamp": 1.4395423322755592
+        },
+        {
+            "x": 5.199384048260516,
+            "y": 1.6143645569232876,
+            "heading": -0.6274260701015184,
+            "angularVelocity": -0.45090216432919045,
+            "velocityX": -0.8695599138477248,
+            "velocityY": 0.19636820069091412,
+            "timestamp": 1.493536218221514
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -0.23287692075121041,
+            "velocityX": -0.4330869662524824,
+            "velocityY": 0.10437187429616973,
+            "timestamp": 1.5475301041674687
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 0,
+            "velocityX": 3.9288115092123874e-33,
+            "velocityY": 2.0888895254257856e-32,
+            "timestamp": 1.6015239901134235
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.1.traj
new file mode 100644
index 00000000..93fe8a43
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.1.traj	
@@ -0,0 +1,265 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": -6.709404579113994e-34,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.616979036560275,
+            "y": 4.122277530863763,
+            "heading": 0.08798648745533431,
+            "angularVelocity": 0.9786973137324638,
+            "velocityX": -0.7009990898727472,
+            "velocityY": -0.05252929161611988,
+            "timestamp": 0.08990163375413819
+        },
+        {
+            "x": 7.489636713382173,
+            "y": 4.1124897070611155,
+            "heading": 0.24774232666841967,
+            "angularVelocity": 1.7770070747544424,
+            "velocityX": -1.4164628367750984,
+            "velocityY": -0.10887259100780497,
+            "timestamp": 0.17980326750827638
+        },
+        {
+            "x": 7.296297558859626,
+            "y": 4.097684850959325,
+            "heading": 0.45307882023210666,
+            "angularVelocity": 2.2840129260079944,
+            "velocityX": -2.150563304013888,
+            "velocityY": -0.16467838773963095,
+            "timestamp": 0.2697049012624146
+        },
+        {
+            "x": 7.035802443968742,
+            "y": 4.077851050872189,
+            "heading": 0.6494071349161524,
+            "angularVelocity": 2.183812534719467,
+            "velocityX": -2.8975570744718895,
+            "velocityY": -0.22061668135394896,
+            "timestamp": 0.35960653501655276
+        },
+        {
+            "x": 6.7206589840239435,
+            "y": 4.046579170766799,
+            "heading": 0.7075219081795038,
+            "angularVelocity": 0.6464262198202391,
+            "velocityX": -3.505425283000413,
+            "velocityY": -0.34784551514284123,
+            "timestamp": 0.44950816877069094
+        },
+        {
+            "x": 6.381493052293109,
+            "y": 4.021491444364005,
+            "heading": 0.7075219804762528,
+            "angularVelocity": 8.041761424949666e-7,
+            "velocityX": -3.7726336838147025,
+            "velocityY": -0.27905751380892024,
+            "timestamp": 0.5394098025248292
+        },
+        {
+            "x": 6.042319780035644,
+            "y": 3.9965031541240665,
+            "heading": 0.7075220527697934,
+            "angularVelocity": 8.041404542109454e-7,
+            "velocityX": -3.7727153344624633,
+            "velocityY": -0.27795145868295346,
+            "timestamp": 0.6293114362789674
+        },
+        {
+            "x": 5.7039977727154,
+            "y": 4.0311606356057235,
+            "heading": 0.7075221294336903,
+            "angularVelocity": 8.52753100011537e-7,
+            "velocityX": -3.7632464861037187,
+            "velocityY": 0.3855044678769589,
+            "timestamp": 0.7192130700331055
+        },
+        {
+            "x": 5.379151739194528,
+            "y": 4.1318481159369504,
+            "heading": 0.707522232494757,
+            "angularVelocity": 0.0000011463759035114744,
+            "velocityX": -3.6133496128586025,
+            "velocityY": 1.1199738661767524,
+            "timestamp": 0.8091147037872437
+        },
+        {
+            "x": 5.0805369696371825,
+            "y": 4.294612195205207,
+            "heading": 0.7075222695179495,
+            "angularVelocity": 4.1181890719021464e-7,
+            "velocityX": -3.3215722238596133,
+            "velocityY": 1.8104685362379804,
+            "timestamp": 0.8990163375413819
+        },
+        {
+            "x": 4.819878772182475,
+            "y": 4.513061803267287,
+            "heading": 0.7075222309589103,
+            "angularVelocity": -4.2890254069180694e-7,
+            "velocityX": -2.899371085597298,
+            "velocityY": 2.429873617864304,
+            "timestamp": 0.9889179712955201
+        },
+        {
+            "x": 4.607411800528633,
+            "y": 4.7786191711792085,
+            "heading": 0.7075221980431953,
+            "angularVelocity": -3.6613033230177587e-7,
+            "velocityX": -2.363327147478702,
+            "velocityY": 2.953865873429686,
+            "timestamp": 1.0788196050496583
+        },
+        {
+            "x": 4.4192729102850965,
+            "y": 5.061932933767741,
+            "heading": 0.7075221661049191,
+            "angularVelocity": -3.552580165687493e-7,
+            "velocityX": -2.0927193687943375,
+            "velocityY": 3.151375016869355,
+            "timestamp": 1.1687212388037966
+        },
+        {
+            "x": 4.23999192605832,
+            "y": 5.350933382498888,
+            "heading": 0.7075221408571981,
+            "angularVelocity": -2.808371773455553e-7,
+            "velocityX": -1.9941905028900322,
+            "velocityY": 3.2146295530233306,
+            "timestamp": 1.258622872557935
+        },
+        {
+            "x": 4.059747399496467,
+            "y": 5.639333870205677,
+            "heading": 0.7075221155890865,
+            "angularVelocity": -2.8106398720718004e-7,
+            "velocityX": -2.0049082428773954,
+            "velocityY": 3.2079560255325883,
+            "timestamp": 1.3485245063120732
+        },
+        {
+            "x": 3.82761549949646,
+            "y": 5.887885093688965,
+            "heading": 0.7075220884594695,
+            "angularVelocity": -3.017700109769218e-7,
+            "velocityX": -2.5820654231362323,
+            "velocityY": 2.764701964849852,
+            "timestamp": 1.4384261400662115
+        },
+        {
+            "x": 3.6506709942338365,
+            "y": 6.0327931031008335,
+            "heading": 0.7075220641283702,
+            "angularVelocity": -4.024467762457879e-7,
+            "velocityX": -2.92673770558828,
+            "velocityY": 2.3968403786158894,
+            "timestamp": 1.4988840707255708
+        },
+        {
+            "x": 3.4560308994306905,
+            "y": 6.152888579058325,
+            "heading": 0.7075220417785688,
+            "angularVelocity": -3.696752622643165e-7,
+            "velocityX": -3.2194303159267204,
+            "velocityY": 1.9864304756722417,
+            "timestamp": 1.5593420013849302
+        },
+        {
+            "x": 3.2531901663020784,
+            "y": 6.258545225655943,
+            "heading": 0.7075220198614292,
+            "angularVelocity": -3.625188534499236e-7,
+            "velocityX": -3.3550723770436006,
+            "velocityY": 1.7476060699673126,
+            "timestamp": 1.6197999320442895
+        },
+        {
+            "x": 3.0503686595470367,
+            "y": 6.36423877502023,
+            "heading": 0.7075219979437177,
+            "angularVelocity": -3.625283082596746e-7,
+            "velocityX": -3.354754364616084,
+            "velocityY": 1.7482164574835888,
+            "timestamp": 1.6802578627036489
+        },
+        {
+            "x": 2.862919086961248,
+            "y": 6.462612745515211,
+            "heading": 0.6632456903097049,
+            "angularVelocity": -0.7323490425675934,
+            "velocityX": -3.1004960067524197,
+            "velocityY": 1.6271474961510723,
+            "timestamp": 1.7407157933630082
+        },
+        {
+            "x": 2.701858233172079,
+            "y": 6.5469153850893225,
+            "heading": 0.5979529513983839,
+            "angularVelocity": -1.0799697938593102,
+            "velocityX": -2.6640153249158556,
+            "velocityY": 1.394401671618308,
+            "timestamp": 1.8011737240223675
+        },
+        {
+            "x": 2.567771439572364,
+            "y": 6.617045170730925,
+            "heading": 0.5348900965776917,
+            "angularVelocity": -1.0430865584205178,
+            "velocityX": -2.2178528463909877,
+            "velocityY": 1.1599766131037368,
+            "timestamp": 1.8616316546817269
+        },
+        {
+            "x": 2.4605956436791856,
+            "y": 6.67307612750581,
+            "heading": 0.4804771381393031,
+            "angularVelocity": -0.9000135771262476,
+            "velocityX": -1.7727334482731019,
+            "velocityY": 0.9267759607999744,
+            "timestamp": 1.9220895853410862
+        },
+        {
+            "x": 2.380269344431837,
+            "y": 6.715058292556038,
+            "heading": 0.43766723065506036,
+            "angularVelocity": -0.7080941576620494,
+            "velocityX": -1.3286313039718116,
+            "velocityY": 0.694402944201169,
+            "timestamp": 1.9825475160004455
+        },
+        {
+            "x": 2.3267482247734668,
+            "y": 6.743025081240933,
+            "heading": 0.4081449342684466,
+            "angularVelocity": -0.48831139380108807,
+            "velocityX": -0.8852621827225426,
+            "velocityY": 0.462582631920201,
+            "timestamp": 2.0430054466598047
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": -0.2505036825320822,
+            "velocityX": -0.4424270642703138,
+            "velocityY": 0.23115112619031306,
+            "timestamp": 2.103463377319164
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -2.9211624967517807e-31,
+            "timestamp": 2.1639213079785233
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.traj
index 47660d82..93fe8a43 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.traj	
@@ -4,217 +4,262 @@
             "x": 7.68,
             "y": 4.127,
             "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 1.832751405105417e-34,
+            "angularVelocity": -6.709404579113994e-34,
+            "velocityX": 0,
             "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 7.608571974046609,
-            "y": 4.129750388677263,
-            "heading": -0.02440208863451916,
-            "angularVelocity": -0.26295738388657636,
-            "velocityX": -0.7697098032139116,
-            "velocityY": 0.029638242122484983,
-            "timestamp": 0.09279864392416992
-        },
-        {
-            "x": 7.46571594445247,
-            "y": 4.135252523590067,
-            "heading": -0.07320447592180841,
-            "angularVelocity": -0.5258954788948677,
-            "velocityX": -1.5394193659863489,
-            "velocityY": 0.05929111331948823,
-            "timestamp": 0.18559728784833984
-        },
-        {
-            "x": 7.25143160309568,
-            "y": 4.143508368339934,
-            "heading": -0.1463869418885076,
-            "angularVelocity": -0.7886156830752087,
-            "velocityX": -2.3091322490864354,
-            "velocityY": 0.08896514432486853,
-            "timestamp": 0.27839593177250976
-        },
-        {
-            "x": 6.965717910609067,
-            "y": 4.154520147774138,
-            "heading": -0.24389107124776152,
-            "angularVelocity": -1.050706402983198,
-            "velocityX": -3.078856332427471,
-            "velocityY": 0.1186631503279318,
-            "timestamp": 0.3711945756966797
-        },
-        {
-            "x": 6.614926201978592,
-            "y": 4.168029747138803,
-            "heading": -0.24389108357148506,
-            "angularVelocity": -1.3280068591073525e-7,
-            "velocityX": -3.780138305869253,
-            "velocityY": 0.1455797067002448,
-            "timestamp": 0.4639932196208496
-        },
-        {
-            "x": 6.264134491970206,
-            "y": 4.181539310738243,
-            "heading": -0.2438910958936344,
-            "angularVelocity": -1.3278372185354406e-7,
-            "velocityX": -3.7801383207176356,
-            "velocityY": 0.14557932129349355,
-            "timestamp": 0.5567918635450195
-        },
-        {
-            "x": 5.913342805476888,
-            "y": 4.195049484934711,
-            "heading": -0.2438911082157909,
-            "angularVelocity": -1.32783799697389e-7,
-            "velocityX": -3.780138067318815,
-            "velocityY": 0.14558590109896277,
-            "timestamp": 0.6495905074691894
-        },
-        {
-            "x": 5.568285864608835,
-            "y": 4.259648543779433,
-            "heading": -0.24389114467773698,
-            "angularVelocity": -3.929146432741106e-7,
-            "velocityX": -3.718340336417024,
-            "velocityY": 0.6961207202291138,
-            "timestamp": 0.7423891513933594
-        },
-        {
-            "x": 5.223229300660808,
-            "y": 4.324249615558004,
-            "heading": -0.2438911811402195,
-            "angularVelocity": -3.929204239211486e-7,
-            "velocityX": -3.7183362747195923,
-            "velocityY": 0.6961424116429128,
-            "timestamp": 0.8351877953175293
-        },
-        {
-            "x": 4.889178228675737,
-            "y": 4.432171862538056,
-            "heading": -0.2438912192984801,
-            "angularVelocity": -4.111941617608203e-7,
-            "velocityX": -3.5997408783046705,
-            "velocityY": 1.1629722420106654,
-            "timestamp": 0.9279864392416992
-        },
-        {
-            "x": 4.584073695522117,
-            "y": 4.605805251226935,
-            "heading": -0.24389116821114407,
-            "angularVelocity": 5.505181312773355e-7,
-            "velocityX": -3.287812410307864,
-            "velocityY": 1.8710767889103372,
-            "timestamp": 1.0207850831658691
-        },
-        {
-            "x": 4.320679858830311,
-            "y": 4.837885010533044,
-            "heading": -0.24389110572504064,
-            "angularVelocity": 6.733514712028696e-7,
-            "velocityX": -2.8383371303038154,
-            "velocityY": 2.5008960205899418,
-            "timestamp": 1.113583727090039
-        },
-        {
-            "x": 4.066259556785684,
-            "y": 5.079768532204951,
-            "heading": -0.24389104453640137,
-            "angularVelocity": 6.593699721479525e-7,
-            "velocityX": -2.7416381456233947,
-            "velocityY": 2.606541555386963,
-            "timestamp": 1.206382371014209
-        },
-        {
-            "x": 3.8118393280605467,
-            "y": 5.321652130996455,
-            "heading": -0.243890983347767,
-            "angularVelocity": 6.593699195322184e-7,
-            "velocityX": -2.741637355531139,
-            "velocityY": 2.606542386429263,
-            "timestamp": 1.299181014938379
-        },
-        {
-            "x": 3.557419123484207,
-            "y": 5.563535755188363,
-            "heading": -0.24389092215913197,
-            "angularVelocity": 6.593699267467298e-7,
-            "velocityX": -2.7416370953032203,
-            "velocityY": 2.606542660144498,
-            "timestamp": 1.3919796588625488
-        },
-        {
-            "x": 3.3029989365055474,
-            "y": 5.805419397890013,
-            "heading": -0.24389086097049373,
-            "angularVelocity": 6.593699609263459e-7,
-            "velocityX": -2.7416369056702816,
-            "velocityY": 2.606542859605842,
-            "timestamp": 1.4847783027867187
-        },
-        {
-            "x": 3.0485787435735707,
-            "y": 6.04730303432977,
-            "heading": -0.24389079978182,
-            "angularVelocity": 6.593703437871394e-7,
-            "velocityX": -2.741636969823341,
-            "velocityY": 2.6065427921275584,
-            "timestamp": 1.5775769467108887
-        },
-        {
-            "x": 2.7984729856087975,
-            "y": 6.275491677578651,
-            "heading": -0.21231080129448254,
-            "angularVelocity": 0.3403066807004949,
-            "velocityX": -2.6951445343225506,
-            "velocityY": 2.45896527793385,
-            "timestamp": 1.6703755906350586
-        },
-        {
-            "x": 2.596978431795094,
-            "y": 6.469397893884434,
-            "heading": -0.03732218251039644,
-            "angularVelocity": 1.8856807748945335,
-            "velocityX": -2.171309248638947,
-            "velocityY": 2.089537175394848,
-            "timestamp": 1.7631742345592285
-        },
-        {
-            "x": 2.4472498512530994,
-            "y": 6.613867328015145,
-            "heading": 0.15789573469158452,
-            "angularVelocity": 2.1036720898801096,
-            "velocityX": -1.6134781092743464,
-            "velocityY": 1.5568054447946784,
-            "timestamp": 1.8559728784833984
-        },
-        {
-            "x": 2.3487140682465175,
-            "y": 6.7094536627437265,
-            "heading": 0.3099829221841124,
-            "angularVelocity": 1.6388945038552638,
-            "velocityX": -1.0618235228426263,
-            "velocityY": 1.0300402105734285,
-            "timestamp": 1.9487715224075683
+            "x": 7.616979036560275,
+            "y": 4.122277530863763,
+            "heading": 0.08798648745533431,
+            "angularVelocity": 0.9786973137324638,
+            "velocityX": -0.7009990898727472,
+            "velocityY": -0.05252929161611988,
+            "timestamp": 0.08990163375413819
+        },
+        {
+            "x": 7.489636713382173,
+            "y": 4.1124897070611155,
+            "heading": 0.24774232666841967,
+            "angularVelocity": 1.7770070747544424,
+            "velocityX": -1.4164628367750984,
+            "velocityY": -0.10887259100780497,
+            "timestamp": 0.17980326750827638
+        },
+        {
+            "x": 7.296297558859626,
+            "y": 4.097684850959325,
+            "heading": 0.45307882023210666,
+            "angularVelocity": 2.2840129260079944,
+            "velocityX": -2.150563304013888,
+            "velocityY": -0.16467838773963095,
+            "timestamp": 0.2697049012624146
+        },
+        {
+            "x": 7.035802443968742,
+            "y": 4.077851050872189,
+            "heading": 0.6494071349161524,
+            "angularVelocity": 2.183812534719467,
+            "velocityX": -2.8975570744718895,
+            "velocityY": -0.22061668135394896,
+            "timestamp": 0.35960653501655276
+        },
+        {
+            "x": 6.7206589840239435,
+            "y": 4.046579170766799,
+            "heading": 0.7075219081795038,
+            "angularVelocity": 0.6464262198202391,
+            "velocityX": -3.505425283000413,
+            "velocityY": -0.34784551514284123,
+            "timestamp": 0.44950816877069094
+        },
+        {
+            "x": 6.381493052293109,
+            "y": 4.021491444364005,
+            "heading": 0.7075219804762528,
+            "angularVelocity": 8.041761424949666e-7,
+            "velocityX": -3.7726336838147025,
+            "velocityY": -0.27905751380892024,
+            "timestamp": 0.5394098025248292
+        },
+        {
+            "x": 6.042319780035644,
+            "y": 3.9965031541240665,
+            "heading": 0.7075220527697934,
+            "angularVelocity": 8.041404542109454e-7,
+            "velocityX": -3.7727153344624633,
+            "velocityY": -0.27795145868295346,
+            "timestamp": 0.6293114362789674
+        },
+        {
+            "x": 5.7039977727154,
+            "y": 4.0311606356057235,
+            "heading": 0.7075221294336903,
+            "angularVelocity": 8.52753100011537e-7,
+            "velocityX": -3.7632464861037187,
+            "velocityY": 0.3855044678769589,
+            "timestamp": 0.7192130700331055
+        },
+        {
+            "x": 5.379151739194528,
+            "y": 4.1318481159369504,
+            "heading": 0.707522232494757,
+            "angularVelocity": 0.0000011463759035114744,
+            "velocityX": -3.6133496128586025,
+            "velocityY": 1.1199738661767524,
+            "timestamp": 0.8091147037872437
+        },
+        {
+            "x": 5.0805369696371825,
+            "y": 4.294612195205207,
+            "heading": 0.7075222695179495,
+            "angularVelocity": 4.1181890719021464e-7,
+            "velocityX": -3.3215722238596133,
+            "velocityY": 1.8104685362379804,
+            "timestamp": 0.8990163375413819
+        },
+        {
+            "x": 4.819878772182475,
+            "y": 4.513061803267287,
+            "heading": 0.7075222309589103,
+            "angularVelocity": -4.2890254069180694e-7,
+            "velocityX": -2.899371085597298,
+            "velocityY": 2.429873617864304,
+            "timestamp": 0.9889179712955201
+        },
+        {
+            "x": 4.607411800528633,
+            "y": 4.7786191711792085,
+            "heading": 0.7075221980431953,
+            "angularVelocity": -3.6613033230177587e-7,
+            "velocityX": -2.363327147478702,
+            "velocityY": 2.953865873429686,
+            "timestamp": 1.0788196050496583
+        },
+        {
+            "x": 4.4192729102850965,
+            "y": 5.061932933767741,
+            "heading": 0.7075221661049191,
+            "angularVelocity": -3.552580165687493e-7,
+            "velocityX": -2.0927193687943375,
+            "velocityY": 3.151375016869355,
+            "timestamp": 1.1687212388037966
+        },
+        {
+            "x": 4.23999192605832,
+            "y": 5.350933382498888,
+            "heading": 0.7075221408571981,
+            "angularVelocity": -2.808371773455553e-7,
+            "velocityX": -1.9941905028900322,
+            "velocityY": 3.2146295530233306,
+            "timestamp": 1.258622872557935
+        },
+        {
+            "x": 4.059747399496467,
+            "y": 5.639333870205677,
+            "heading": 0.7075221155890865,
+            "angularVelocity": -2.8106398720718004e-7,
+            "velocityX": -2.0049082428773954,
+            "velocityY": 3.2079560255325883,
+            "timestamp": 1.3485245063120732
+        },
+        {
+            "x": 3.82761549949646,
+            "y": 5.887885093688965,
+            "heading": 0.7075220884594695,
+            "angularVelocity": -3.017700109769218e-7,
+            "velocityX": -2.5820654231362323,
+            "velocityY": 2.764701964849852,
+            "timestamp": 1.4384261400662115
+        },
+        {
+            "x": 3.6506709942338365,
+            "y": 6.0327931031008335,
+            "heading": 0.7075220641283702,
+            "angularVelocity": -4.024467762457879e-7,
+            "velocityX": -2.92673770558828,
+            "velocityY": 2.3968403786158894,
+            "timestamp": 1.4988840707255708
+        },
+        {
+            "x": 3.4560308994306905,
+            "y": 6.152888579058325,
+            "heading": 0.7075220417785688,
+            "angularVelocity": -3.696752622643165e-7,
+            "velocityX": -3.2194303159267204,
+            "velocityY": 1.9864304756722417,
+            "timestamp": 1.5593420013849302
+        },
+        {
+            "x": 3.2531901663020784,
+            "y": 6.258545225655943,
+            "heading": 0.7075220198614292,
+            "angularVelocity": -3.625188534499236e-7,
+            "velocityX": -3.3550723770436006,
+            "velocityY": 1.7476060699673126,
+            "timestamp": 1.6197999320442895
+        },
+        {
+            "x": 3.0503686595470367,
+            "y": 6.36423877502023,
+            "heading": 0.7075219979437177,
+            "angularVelocity": -3.625283082596746e-7,
+            "velocityX": -3.354754364616084,
+            "velocityY": 1.7482164574835888,
+            "timestamp": 1.6802578627036489
+        },
+        {
+            "x": 2.862919086961248,
+            "y": 6.462612745515211,
+            "heading": 0.6632456903097049,
+            "angularVelocity": -0.7323490425675934,
+            "velocityX": -3.1004960067524197,
+            "velocityY": 1.6271474961510723,
+            "timestamp": 1.7407157933630082
+        },
+        {
+            "x": 2.701858233172079,
+            "y": 6.5469153850893225,
+            "heading": 0.5979529513983839,
+            "angularVelocity": -1.0799697938593102,
+            "velocityX": -2.6640153249158556,
+            "velocityY": 1.394401671618308,
+            "timestamp": 1.8011737240223675
+        },
+        {
+            "x": 2.567771439572364,
+            "y": 6.617045170730925,
+            "heading": 0.5348900965776917,
+            "angularVelocity": -1.0430865584205178,
+            "velocityX": -2.2178528463909877,
+            "velocityY": 1.1599766131037368,
+            "timestamp": 1.8616316546817269
+        },
+        {
+            "x": 2.4605956436791856,
+            "y": 6.67307612750581,
+            "heading": 0.4804771381393031,
+            "angularVelocity": -0.9000135771262476,
+            "velocityX": -1.7727334482731019,
+            "velocityY": 0.9267759607999744,
+            "timestamp": 1.9220895853410862
+        },
+        {
+            "x": 2.380269344431837,
+            "y": 6.715058292556038,
+            "heading": 0.43766723065506036,
+            "angularVelocity": -0.7080941576620494,
+            "velocityX": -1.3286313039718116,
+            "velocityY": 0.694402944201169,
+            "timestamp": 1.9825475160004455
+        },
+        {
+            "x": 2.3267482247734668,
+            "y": 6.743025081240933,
+            "heading": 0.4081449342684466,
+            "angularVelocity": -0.48831139380108807,
+            "velocityX": -0.8852621827225426,
+            "velocityY": 0.462582631920201,
+            "timestamp": 2.0430054466598047
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.393,
-            "angularVelocity": 0.8945936525078311,
-            "velocityX": -0.5249437511858832,
-            "velocityY": 0.5123602592202229,
-            "timestamp": 2.0415701663317383
+            "angularVelocity": -0.2505036825320822,
+            "velocityX": -0.4424270642703138,
+            "velocityY": 0.23115112619031306,
+            "timestamp": 2.103463377319164
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.393,
             "angularVelocity": 0,
-            "velocityX": 1.4564927652554838e-31,
-            "velocityY": 0,
-            "timestamp": 2.134368810255908
+            "velocityX": 0,
+            "velocityY": -2.9211624967517807e-31,
+            "timestamp": 2.1639213079785233
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.1.traj
new file mode 100644
index 00000000..2f31d71f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.1.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": -8.248091288877123e-33,
+            "velocityX": -1.7499589770527321e-34,
+            "velocityY": 1.243057151683689e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 7.599512207946833,
+            "y": 4.126338330595212,
+            "heading": -0.05672955044148203,
+            "angularVelocity": -0.5720722032058021,
+            "velocityX": -0.8116550928518662,
+            "velocityY": -0.0066724074357217035,
+            "timestamp": 0.09916501819801522
+        },
+        {
+            "x": 7.438184902121127,
+            "y": 4.125091465076226,
+            "heading": -0.16291927611759752,
+            "angularVelocity": -1.070838563898341,
+            "velocityX": -1.6268570183042166,
+            "velocityY": -0.01257364281924666,
+            "timestamp": 0.19833003639603045
+        },
+        {
+            "x": 7.19545269041534,
+            "y": 4.123364615129754,
+            "heading": -0.30423484322627714,
+            "angularVelocity": -1.4250546178138856,
+            "velocityX": -2.4477604715515007,
+            "velocityY": -0.017413902380612675,
+            "timestamp": 0.2974950545940457
+        },
+        {
+            "x": 6.870785634071806,
+            "y": 4.1216134281783505,
+            "heading": -0.4343749203987102,
+            "angularVelocity": -1.3123587282822462,
+            "velocityX": -3.274007933878773,
+            "velocityY": -0.01765932163604032,
+            "timestamp": 0.3966600727920609
+        },
+        {
+            "x": 6.495672194218458,
+            "y": 4.117557524267507,
+            "heading": -0.4343749552288927,
+            "angularVelocity": -3.5123456983963544e-7,
+            "velocityX": -3.782719417287976,
+            "velocityY": -0.040900551268437134,
+            "timestamp": 0.4958250909900761
+        },
+        {
+            "x": 6.120558752684864,
+            "y": 4.113501775750749,
+            "heading": -0.43437499005887026,
+            "angularVelocity": -3.512325030859258e-7,
+            "velocityX": -3.782719434231919,
+            "velocityY": -0.04089898424321494,
+            "timestamp": 0.5949901091880914
+        },
+        {
+            "x": 5.747019040696225,
+            "y": 4.148065138700183,
+            "heading": -0.43437502656757854,
+            "angularVelocity": -3.681611615448704e-7,
+            "velocityX": -3.766849628794958,
+            "velocityY": 0.3485439076955189,
+            "timestamp": 0.6941551273861066
+        },
+        {
+            "x": 5.389911454629062,
+            "y": 4.262959433025878,
+            "heading": -0.434375162279621,
+            "angularVelocity": -0.0000013685475476133038,
+            "velocityX": -3.601144764115114,
+            "velocityY": 1.1586171859140066,
+            "timestamp": 0.7933201455841219
+        },
+        {
+            "x": 5.045913588511523,
+            "y": 4.412598985545015,
+            "heading": -0.4343752985502662,
+            "angularVelocity": -0.0000013741806101348962,
+            "velocityX": -3.468943709873945,
+            "velocityY": 1.5089953618556682,
+            "timestamp": 0.8924851637821372
+        },
+        {
+            "x": 4.713243052034498,
+            "y": 4.585968073714987,
+            "heading": -0.43437543856751193,
+            "angularVelocity": -0.0000014119620841874337,
+            "velocityX": -3.354716638207433,
+            "velocityY": 1.7482887748155678,
+            "timestamp": 0.9916501819801524
+        },
+        {
+            "x": 4.426185676495707,
+            "y": 4.82747296096862,
+            "heading": -0.4343754384530987,
+            "angularVelocity": 1.153766536466057e-9,
+            "velocityX": -2.894744343873237,
+            "velocityY": 2.4353838847827403,
+            "timestamp": 1.0908152001781677
+        },
+        {
+            "x": 4.09351517988242,
+            "y": 5.000842166204109,
+            "heading": -0.4343753660253311,
+            "angularVelocity": 7.303761838407394e-7,
+            "velocityX": -3.354716236213479,
+            "velocityY": 1.748289955327798,
+            "timestamp": 1.189980218376183
+        },
+        {
+            "x": 3.752063829141965,
+            "y": 5.156204697333757,
+            "heading": -0.43437529472941433,
+            "angularVelocity": 7.189623727917814e-7,
+            "velocityX": -3.4432641363372296,
+            "velocityY": 1.5667070298864525,
+            "timestamp": 1.2891452365741982
+        },
+        {
+            "x": 3.4085469650969755,
+            "y": 5.306945220863998,
+            "heading": -0.43437522535251666,
+            "angularVelocity": 6.996105981774619e-7,
+            "velocityX": -3.46409318817495,
+            "velocityY": 1.5200977750968405,
+            "timestamp": 1.3883102547722135
+        },
+        {
+            "x": 3.0417944650952866,
+            "y": 5.3858068126004595,
+            "heading": -0.43437520041032157,
+            "angularVelocity": 2.5152211488411685e-7,
+            "velocityX": -3.6984060172242224,
+            "velocityY": 0.7952561615930772,
+            "timestamp": 1.4874752729702287
+        },
+        {
+            "x": 2.7246409461958954,
+            "y": 5.456187357907523,
+            "heading": -0.2777696543737322,
+            "angularVelocity": 1.5792418423589194,
+            "velocityX": -3.1982399102281316,
+            "velocityY": 0.7097315826285274,
+            "timestamp": 1.586640291168244
+        },
+        {
+            "x": 2.4871805723397817,
+            "y": 5.508279050924135,
+            "heading": -0.14311381126711797,
+            "angularVelocity": 1.3578966207390795,
+            "velocityX": -2.394598197742947,
+            "velocityY": 0.5253031155865293,
+            "timestamp": 1.6858053093662593
+        },
+        {
+            "x": 2.3290265259060328,
+            "y": 5.5428007138640085,
+            "heading": -0.04864902301750531,
+            "angularVelocity": 0.9526019353012469,
+            "velocityX": -1.594857231992267,
+            "velocityY": 0.3481233964071807,
+            "timestamp": 1.7849703275642745
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -2.5681051863102055e-32,
+            "angularVelocity": 0.490586538494469,
+            "velocityX": -0.7969193909512572,
+            "velocityY": 0.1734410626703791,
+            "timestamp": 1.8841353457622898
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 1.14717044306562e-31,
+            "velocityX": 0,
+            "velocityY": 5.846493116157662e-32,
+            "timestamp": 1.983300363960305
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W3.1.traj
new file mode 100644
index 00000000..3874f8b2
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W3.1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 5.58164160972529e-32,
+            "velocityX": 1.1273756142039836e-32,
+            "velocityY": 2.4507602066878393e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.588395006557631,
+            "y": 4.128528104169273,
+            "heading": -0.06094869084906696,
+            "angularVelocity": -0.5766457987157873,
+            "velocityX": -0.8666902254019087,
+            "velocityY": 0.01445765014696692,
+            "timestamp": 0.10569519622756653
+        },
+        {
+            "x": 7.4046836501246664,
+            "y": 4.131675342578391,
+            "heading": -0.17161815397097052,
+            "angularVelocity": -1.0470623743734504,
+            "velocityX": -1.7381239922903071,
+            "velocityY": 0.029776551077511915,
+            "timestamp": 0.21139039245513305
+        },
+        {
+            "x": 7.128148507793846,
+            "y": 4.136585421276787,
+            "heading": -0.30937735862178317,
+            "angularVelocity": -1.3033629679272352,
+            "velocityX": -2.6163454177749763,
+            "velocityY": 0.0464550790730766,
+            "timestamp": 0.3170855886826996
+        },
+        {
+            "x": 6.759745872373483,
+            "y": 4.143898882898015,
+            "heading": -0.3925495634756188,
+            "angularVelocity": -0.7869061965196801,
+            "velocityX": -3.485519196418107,
+            "velocityY": 0.06919388848555069,
+            "timestamp": 0.4227807849102661
+        },
+        {
+            "x": 6.3599483672427946,
+            "y": 4.149634642956697,
+            "heading": -0.39254959749689916,
+            "angularVelocity": -3.2188104690533836e-7,
+            "velocityX": -3.782551330619671,
+            "velocityY": 0.05426698907234567,
+            "timestamp": 0.5284759811378327
+        },
+        {
+            "x": 5.960150865134662,
+            "y": 4.155370613693523,
+            "heading": -0.39254963151811834,
+            "angularVelocity": -3.2188046698591794e-7,
+            "velocityX": -3.7825513020227493,
+            "velocityY": 0.05426898233365955,
+            "timestamp": 0.6341711773653992
+        },
+        {
+            "x": 5.56430885680334,
+            "y": 4.211762521698523,
+            "heading": -0.3925496691964927,
+            "angularVelocity": -3.5648142721372326e-7,
+            "velocityX": -3.745127711188092,
+            "velocityY": 0.5335333110464675,
+            "timestamp": 0.7398663735929657
+        },
+        {
+            "x": 5.196451370697568,
+            "y": 4.356905683947299,
+            "heading": -0.407192768864793,
+            "angularVelocity": -0.13854082485237101,
+            "velocityX": -3.4803614472105,
+            "velocityY": 1.3732238306864724,
+            "timestamp": 0.8455615698205322
+        },
+        {
+            "x": 4.853119399386178,
+            "y": 4.529141900436451,
+            "heading": -0.46036689242459117,
+            "angularVelocity": -0.5030893120753747,
+            "velocityX": -3.2483214333807724,
+            "velocityY": 1.6295557663597022,
+            "timestamp": 0.9512567660480987
+        },
+        {
+            "x": 4.514017884763102,
+            "y": 4.740993233583884,
+            "heading": -0.4603674462778229,
+            "angularVelocity": -0.000005240098429403888,
+            "velocityX": -3.208296372268186,
+            "velocityY": 2.0043610372916896,
+            "timestamp": 1.0569519622756653
+        },
+        {
+            "x": 4.21750261375463,
+            "y": 5.009227519029098,
+            "heading": -0.46036738913342323,
+            "angularVelocity": 5.406527608893175e-7,
+            "velocityX": -2.8053807702864813,
+            "velocityY": 2.537809616888297,
+            "timestamp": 1.1626471585032319
+        },
+        {
+            "x": 3.866969646487487,
+            "y": 5.201574981603133,
+            "heading": -0.46036721071098435,
+            "angularVelocity": 0.0000016880846552508654,
+            "velocityX": -3.3164512653198517,
+            "velocityY": 1.8198316426784653,
+            "timestamp": 1.2683423547307984
+        },
+        {
+            "x": 3.5045677865422378,
+            "y": 5.303672838764994,
+            "heading": -0.3887465759877037,
+            "angularVelocity": 0.6776148517580518,
+            "velocityX": -3.4287448519891264,
+            "velocityY": 0.9659649710289621,
+            "timestamp": 1.3740375509583649
+        },
+        {
+            "x": 3.16557460185915,
+            "y": 5.315860378892371,
+            "heading": -0.29320626763004903,
+            "angularVelocity": 0.9039229006392311,
+            "velocityX": -3.207271444515043,
+            "velocityY": 0.1153083636945626,
+            "timestamp": 1.4797327471859314
+        },
+        {
+            "x": 2.8681536233597122,
+            "y": 5.2454821587095966,
+            "heading": -0.23849853505250618,
+            "angularVelocity": 0.5175990445181124,
+            "velocityX": -2.8139498209462337,
+            "velocityY": -0.6658601591622668,
+            "timestamp": 1.585427943413498
+        },
+        {
+            "x": 2.618332286986499,
+            "y": 5.0970676603891505,
+            "heading": -0.24744806192055374,
+            "angularVelocity": -0.08467297651615932,
+            "velocityX": -2.3636016137889215,
+            "velocityY": -1.4041744905880433,
+            "timestamp": 1.6911231396410644
+        },
+        {
+            "x": 2.4325478975720265,
+            "y": 4.8813111498736355,
+            "heading": -0.27927376577641677,
+            "angularVelocity": -0.3011083283987745,
+            "velocityX": -1.7577373054350616,
+            "velocityY": -2.0413085761341616,
+            "timestamp": 1.796818335868631
+        },
+        {
+            "x": 2.3348070320385017,
+            "y": 4.635496280912222,
+            "heading": -0.32770640318337707,
+            "angularVelocity": -0.4582293153861289,
+            "velocityX": -0.9247427416008974,
+            "velocityY": -2.3256957528340565,
+            "timestamp": 1.9025135320961974
+        },
+        {
+            "x": 2.305548232287739,
+            "y": 4.452786448669473,
+            "heading": -0.3683698320727586,
+            "angularVelocity": -0.3847235289845278,
+            "velocityX": -0.2768224176221525,
+            "velocityY": -1.72864840374928,
+            "timestamp": 2.008208728323764
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.2046466509288718,
+            "velocityX": -0.05249275734153,
+            "velocityY": -0.8778681716972112,
+            "timestamp": 2.1139039245513307
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 5.518495272475637e-32,
+            "velocityX": 0,
+            "velocityY": 3.7393231194786592e-34,
+            "timestamp": 2.219599120778897
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.1.traj
new file mode 100644
index 00000000..1bf739bf
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.1.traj	
@@ -0,0 +1,175 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -2.2542235108498663e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 2.538844153951763,
+            "heading": 2.1223723168001845e-18,
+            "angularVelocity": 2.1422107346760384e-17,
+            "velocityX": -4.524824041974102e-19,
+            "velocityY": 0.8260917454276265,
+            "timestamp": 0.09907392781100399
+        },
+        {
+            "x": 7.68,
+            "y": 2.70253245874193,
+            "heading": 6.368502121517421e-18,
+            "angularVelocity": 4.285819588010552e-17,
+            "velocityX": -9.048618887508432e-19,
+            "velocityY": 1.6521834594306501,
+            "timestamp": 0.19814785562200798
+        },
+        {
+            "x": 7.68,
+            "y": 2.9480649081437846,
+            "heading": 1.2741259045974704e-17,
+            "angularVelocity": 6.432324896396774e-17,
+            "velocityX": -1.3570355356096521e-18,
+            "velocityY": 2.47827511058448,
+            "timestamp": 0.297221783433012
+        },
+        {
+            "x": 7.68,
+            "y": 3.2754414834771923,
+            "heading": 2.1247069179422305e-17,
+            "angularVelocity": 8.585316360490521e-17,
+            "velocityX": -1.8085916683463265e-18,
+            "velocityY": 3.3043665731908773,
+            "timestamp": 0.39629571124401597
+        },
+        {
+            "x": 7.68,
+            "y": 3.6502322649266867,
+            "heading": 5.059469084858907e-18,
+            "angularVelocity": -1.633891019789118e-16,
+            "velocityX": -2.0715339797290713e-18,
+            "velocityY": 3.782940575087071,
+            "timestamp": 0.49536963905501996
+        },
+        {
+            "x": 7.68,
+            "y": 4.025023046376204,
+            "heading": -4.370025378852898e-19,
+            "angularVelocity": -5.5478487063848375e-17,
+            "velocityX": -2.0691441879135133e-18,
+            "velocityY": 3.7829405750873115,
+            "timestamp": 0.594443566866024
+        },
+        {
+            "x": 7.68,
+            "y": 4.3998138278257235,
+            "heading": 3.678666355048194e-17,
+            "angularVelocity": 3.7571606285132846e-16,
+            "velocityX": -2.072328507072934e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.693517494677028
+        },
+        {
+            "x": 7.68,
+            "y": 4.774604609275241,
+            "heading": 5.708604515927294e-17,
+            "angularVelocity": 2.04891257037182e-16,
+            "velocityX": -2.07516702935077e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.7925914224880319
+        },
+        {
+            "x": 7.68,
+            "y": 5.14939539072476,
+            "heading": 3.2711500565517004e-17,
+            "angularVelocity": -2.460238039643844e-16,
+            "velocityX": -2.078706211560206e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.8916653502990359
+        },
+        {
+            "x": 7.68,
+            "y": 5.524186172174278,
+            "heading": 3.4723638610714384e-17,
+            "angularVelocity": 2.0309460720999983e-17,
+            "velocityX": -2.0667219645048642e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.9907392781100399
+        },
+        {
+            "x": 7.68,
+            "y": 5.898976953623796,
+            "heading": 1.3963468905806427e-18,
+            "angularVelocity": -3.363881139719203e-16,
+            "velocityX": -2.078319856885538e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 1.089813205921044
+        },
+        {
+            "x": 7.68,
+            "y": 6.273767735073314,
+            "heading": -1.338725801529433e-17,
+            "angularVelocity": -1.492179146704526e-16,
+            "velocityX": -2.0709974384556896e-18,
+            "velocityY": 3.7829405750873115,
+            "timestamp": 1.188887133732048
+        },
+        {
+            "x": 7.68,
+            "y": 6.648558516522808,
+            "heading": -2.1247111946894708e-17,
+            "angularVelocity": -7.933322222365232e-17,
+            "velocityX": -2.070808166139798e-18,
+            "velocityY": 3.782940575087071,
+            "timestamp": 1.287961061543052
+        },
+        {
+            "x": 7.68,
+            "y": 6.975935091856216,
+            "heading": -1.273985947141283e-17,
+            "angularVelocity": 8.586772184549593e-17,
+            "velocityX": -1.8108827209778567e-18,
+            "velocityY": 3.304366573190878,
+            "timestamp": 1.387034989354056
+        },
+        {
+            "x": 7.68,
+            "y": 7.22146754125807,
+            "heading": -6.367750753434283e-18,
+            "angularVelocity": 6.431670631033216e-17,
+            "velocityX": -1.3581621409711223e-18,
+            "velocityY": 2.47827511058448,
+            "timestamp": 1.4861089171650599
+        },
+        {
+            "x": 7.68,
+            "y": 7.385155846048237,
+            "heading": -2.122107432521882e-18,
+            "angularVelocity": 4.285328556951628e-17,
+            "velocityX": -9.05441456368543e-19,
+            "velocityY": 1.65218345943065,
+            "timestamp": 1.5851828449760639
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 2.141943374454498e-17,
+            "velocityX": -4.527207369006345e-19,
+            "velocityY": 0.8260917454276265,
+            "timestamp": 1.6842567727870679
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -3.1368402443636883e-34,
+            "velocityY": 0,
+            "timestamp": 1.7833307005980719
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.1.traj
new file mode 100644
index 00000000..0dd47978
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.1.traj	
@@ -0,0 +1,139 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": -1.7120676318191985e-39,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 2.5334453720055388,
+            "heading": -1.6066276958150233e-18,
+            "angularVelocity": -1.677931007769515e-17,
+            "velocityX": 1.4398812722194594e-33,
+            "velocityY": 0.7983807350946674,
+            "timestamp": 0.09575052183150976
+        },
+        {
+            "x": 7.68,
+            "y": 2.686336112982689,
+            "heading": -4.816833322743681e-18,
+            "angularVelocity": -3.3526769001814837e-17,
+            "velocityX": 1.5865173753469157e-33,
+            "velocityY": 1.5967614385035827,
+            "timestamp": 0.1915010436630195
+        },
+        {
+            "x": 7.68,
+            "y": 2.915672216863599,
+            "heading": -9.624489104750263e-18,
+            "angularVelocity": -5.0210230614541765e-17,
+            "velocityX": 1.0182538660036936e-34,
+            "velocityY": 2.395142078541023,
+            "timestamp": 0.28725156549452924
+        },
+        {
+            "x": 7.68,
+            "y": 3.221453665444745,
+            "heading": -1.6009153132358185e-17,
+            "angularVelocity": -6.66802008672839e-17,
+            "velocityX": 3.094783239015371e-34,
+            "velocityY": 3.193522528464369,
+            "timestamp": 0.383002087326039
+        },
+        {
+            "x": 7.68,
+            "y": 3.5836721992668203,
+            "heading": -3.473454086768081e-17,
+            "angularVelocity": -1.9556434134398854e-16,
+            "velocityX": 3.082893791022991e-33,
+            "velocityY": 3.7829405719528495,
+            "timestamp": 0.4787526091575488
+        },
+        {
+            "x": 7.68,
+            "y": 3.9458907330889397,
+            "heading": -3.402618832360637e-17,
+            "angularVelocity": 7.397897479044197e-18,
+            "velocityX": 3.015867889332214e-33,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.5745031309890586
+        },
+        {
+            "x": 7.68,
+            "y": 4.308109266911059,
+            "heading": -1.7375698441954866e-17,
+            "angularVelocity": 1.7389450796885638e-16,
+            "velocityX": 5.330140345912211e-33,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.6702536528205684
+        },
+        {
+            "x": 7.68,
+            "y": 4.670327800733179,
+            "heading": 7.579747606314133e-18,
+            "angularVelocity": 2.6062986990486198e-16,
+            "velocityX": 7.600730979130334e-33,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.7660041746520782
+        },
+        {
+            "x": 7.68,
+            "y": 5.032546334555254,
+            "heading": 1.6243735534665814e-17,
+            "angularVelocity": 9.048502047432954e-17,
+            "velocityX": -2.799411465702586e-33,
+            "velocityY": 3.7829405719528495,
+            "timestamp": 0.8617546964835879
+        },
+        {
+            "x": 7.68,
+            "y": 5.3383277831364,
+            "heading": 9.733952786983855e-18,
+            "angularVelocity": -6.798691664538921e-17,
+            "velocityX": -8.030123288193386e-33,
+            "velocityY": 3.193522528464369,
+            "timestamp": 0.9575052183150977
+        },
+        {
+            "x": 7.68,
+            "y": 5.567663887017311,
+            "heading": 4.864296415465134e-18,
+            "angularVelocity": -5.08577528116881e-17,
+            "velocityX": -5.179461626817983e-33,
+            "velocityY": 2.395142078541023,
+            "timestamp": 1.0532557401466074
+        },
+        {
+            "x": 7.68,
+            "y": 5.720554627994461,
+            "heading": 1.6208272352196832e-18,
+            "angularVelocity": -3.3874167136237696e-17,
+            "velocityX": -4.721587370568867e-33,
+            "velocityY": 1.5967614385035827,
+            "timestamp": 1.1490062619781172
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": -1.69276073284528e-17,
+            "velocityX": -2.4599412486527172e-33,
+            "velocityY": 0.7983807350946674,
+            "timestamp": 1.244756783809627
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -3.907521125363084e-43,
+            "velocityY": 8.448865646535191e-39,
+            "timestamp": 1.3405073056411367
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.1.traj
new file mode 100644
index 00000000..14b92c55
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.1.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -2.24426408200091e-39,
+            "angularVelocity": 3.072610310721988e-47,
+            "velocityX": -5.239815815905201e-40,
+            "velocityY": -4.5917748078995606e-40,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 2.540500005575846,
+            "heading": -7.160737227180709e-23,
+            "angularVelocity": -7.15564807370433e-22,
+            "velocityX": 1.97902852084548e-33,
+            "velocityY": 0.8344065481228216,
+            "timestamp": 0.10007112931184187
+        },
+        {
+            "x": 7.68,
+            "y": 2.7075000136298466,
+            "heading": 1.9234971876596208e-22,
+            "angularVelocity": 2.6376946581601568e-21,
+            "velocityX": 2.759296119129438e-33,
+            "velocityY": 1.6688130652907365,
+            "timestamp": 0.20014225862368373
+        },
+        {
+            "x": 7.68,
+            "y": 2.9580000179666155,
+            "heading": 1.2715262688933367e-21,
+            "angularVelocity": 1.0784094425251139e-20,
+            "velocityX": 3.360924516770788e-33,
+            "velocityY": 2.5032195205488357,
+            "timestamp": 0.3002133879355256
+        },
+        {
+            "x": 7.68,
+            "y": 3.2919999999999994,
+            "heading": 4.5990005279924634e-21,
+            "angularVelocity": 3.325109025879841e-20,
+            "velocityX": 4.212493187097162e-33,
+            "velocityY": 3.3376257900775026,
+            "timestamp": 0.40028451724736747
+        },
+        {
+            "x": 7.68,
+            "y": 3.6259999820333837,
+            "heading": 2.8774590551268706e-21,
+            "angularVelocity": -1.7203177491039364e-20,
+            "velocityX": -1.8223734723751632e-33,
+            "velocityY": 3.3376257900775026,
+            "timestamp": 0.5003556465592094
+        },
+        {
+            "x": 7.68,
+            "y": 3.8764999863701526,
+            "heading": 1.4966782173181084e-21,
+            "angularVelocity": -1.3797993485663648e-20,
+            "velocityX": -1.777649344678044e-33,
+            "velocityY": 2.5032195205488357,
+            "timestamp": 0.6004267758710513
+        },
+        {
+            "x": 7.68,
+            "y": 4.043499994424153,
+            "heading": 5.147844205579642e-22,
+            "angularVelocity": -9.81195850164629e-21,
+            "velocityX": -2.007686184897088e-33,
+            "velocityY": 1.6688130652907365,
+            "timestamp": 0.7004979051828932
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -4.8498876719579466e-42,
+            "angularVelocity": -5.144185056490002e-21,
+            "velocityX": -2.233495211598662e-33,
+            "velocityY": 0.8344065481228216,
+            "timestamp": 0.8005690344947352
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 9.449822208481667e-41,
+            "angularVelocity": 9.927610425813228e-40,
+            "velocityX": 1.9465664275137074e-41,
+            "velocityY": 5.142787784847508e-39,
+            "timestamp": 0.9006401638065771
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C5.1.traj
new file mode 100644
index 00000000..27f393e3
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C5.1.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -4.712717929333765e-40,
+            "angularVelocity": 9.927649781151023e-40,
+            "velocityX": 1.7772849076267516e-40,
+            "velocityY": -4.543888050052464e-40,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 2.3734999944241535,
+            "heading": 2.8227518572323466e-21,
+            "angularVelocity": 2.820745460648545e-20,
+            "velocityX": 2.2465911667965736e-32,
+            "velocityY": -0.8344065481228212,
+            "timestamp": 0.10007112931184181
+        },
+        {
+            "x": 7.68,
+            "y": 2.206499986370153,
+            "heading": 8.436925006375396e-21,
+            "angularVelocity": 5.6101826293588e-20,
+            "velocityX": 4.4654374072917263e-32,
+            "velocityY": -1.6688130652907358,
+            "timestamp": 0.20014225862368362
+        },
+        {
+            "x": 7.68,
+            "y": 1.955999982033384,
+            "heading": 1.682221146414935e-20,
+            "angularVelocity": 8.379326253074398e-20,
+            "velocityX": 6.797738711780288e-32,
+            "velocityY": -2.503219520548835,
+            "timestamp": 0.30021338793552543
+        },
+        {
+            "x": 7.68,
+            "y": 1.6219999999999999,
+            "heading": 2.78675063552584e-20,
+            "angularVelocity": 1.1037443968745167e-19,
+            "velocityX": 8.936946075377416e-32,
+            "velocityY": -3.3376257900775013,
+            "timestamp": 0.40028451724736724
+        },
+        {
+            "x": 7.68,
+            "y": 1.288000017966616,
+            "heading": 1.0287640204896551e-20,
+            "angularVelocity": -1.75673704816788e-19,
+            "velocityX": 9.060995032099377e-32,
+            "velocityY": -3.3376257900775013,
+            "timestamp": 0.500355646559209
+        },
+        {
+            "x": 7.68,
+            "y": 1.037500013629847,
+            "heading": 3.2969803204781124e-21,
+            "angularVelocity": -6.985690973056607e-20,
+            "velocityX": 6.916062716279199e-32,
+            "velocityY": -2.503219520548835,
+            "timestamp": 0.6004267758710509
+        },
+        {
+            "x": 7.68,
+            "y": 0.8705000055758465,
+            "heading": 6.18120991020044e-22,
+            "angularVelocity": -2.676955216055553e-20,
+            "velocityX": 4.34564572466329e-32,
+            "velocityY": -1.6688130652907358,
+            "timestamp": 0.7004979051828927
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -2.2737142324276756e-40,
+            "angularVelocity": -6.1768164103594875e-21,
+            "velocityX": 2.131975080632403e-32,
+            "velocityY": -0.8344065481228212,
+            "timestamp": 0.8005690344947345
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 2.0993519451368808e-41,
+            "angularVelocity": 2.4819025971975228e-39,
+            "velocityX": -8.732390511178075e-43,
+            "velocityY": -2.571393892423754e-39,
+            "timestamp": 0.9006401638065763
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S1.1.traj
new file mode 100644
index 00000000..d3b44a34
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S1.1.traj	
@@ -0,0 +1,256 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": -3.908498355076501e-28,
+            "velocityX": -5.285170566576542e-31,
+            "velocityY": -1.5421992925545815e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 7.644672083885401,
+            "y": 2.5062543236054426,
+            "heading": 0.0003371503707738104,
+            "angularVelocity": 0.003954317681676867,
+            "velocityX": -0.4143486570570975,
+            "velocityY": 0.5776865743787781,
+            "timestamp": 0.08526132645270777
+        },
+        {
+            "x": 7.574016248997627,
+            "y": 2.604762966378743,
+            "heading": 0.0010114541718947692,
+            "angularVelocity": 0.00790867124800936,
+            "velocityX": -0.8286973452956875,
+            "velocityY": 1.1553730967106797,
+            "timestamp": 0.17052265290541554
+        },
+        {
+            "x": 7.4680324930779305,
+            "y": 2.7525259224839496,
+            "heading": 0.0020229127878395882,
+            "angularVelocity": 0.01186304105238048,
+            "velocityX": -1.243046060026343,
+            "velocityY": 1.73305955059478,
+            "timestamp": 0.2557839793581233
+        },
+        {
+            "x": 7.326720815099078,
+            "y": 2.949543182754876,
+            "heading": 0.0033715247016779517,
+            "angularVelocity": 0.015817393065211405,
+            "velocityX": -1.657394786805044,
+            "velocityY": 2.310745896971908,
+            "timestamp": 0.3410453058108311
+        },
+        {
+            "x": 7.1500812197312955,
+            "y": 3.1958147252550657,
+            "heading": 0.005057281636821727,
+            "angularVelocity": 0.01977164800486184,
+            "velocityX": -2.0717434588083226,
+            "velocityY": 2.8884319860640817,
+            "timestamp": 0.4263066322635389
+        },
+        {
+            "x": 6.957409028465344,
+            "y": 3.454481547786369,
+            "heading": 0.005057281852786698,
+            "angularVelocity": 2.532976909732762e-9,
+            "velocityX": -2.2597841164587846,
+            "velocityY": 3.0338118499096547,
+            "timestamp": 0.5115679587162466
+        },
+        {
+            "x": 6.719745009541024,
+            "y": 3.6725340903222867,
+            "heading": 0.005057282088140234,
+            "angularVelocity": 2.760378532567681e-9,
+            "velocityX": -2.787477380570894,
+            "velocityY": 2.5574612970252826,
+            "timestamp": 0.5968292851689544
+        },
+        {
+            "x": 6.445480997294438,
+            "y": 3.842270194716782,
+            "heading": 0.005057282389511224,
+            "angularVelocity": 3.534673960970524e-9,
+            "velocityX": -3.2167457821427474,
+            "velocityY": 1.9907748501725318,
+            "timestamp": 0.6820906116216622
+        },
+        {
+            "x": 6.1443030532206215,
+            "y": 3.9576952313720115,
+            "heading": 0.0050573451663417065,
+            "angularVelocity": 7.362872839766494e-7,
+            "velocityX": -3.5324097877003195,
+            "velocityY": 1.3537795089212774,
+            "timestamp": 0.76735193807437
+        },
+        {
+            "x": 5.8268478532983945,
+            "y": 4.0147328665402,
+            "heading": 0.005057406885810731,
+            "angularVelocity": 7.238858646916699e-7,
+            "velocityX": -3.723319975538169,
+            "velocityY": 0.6689742881224171,
+            "timestamp": 0.8526132645270778
+        },
+        {
+            "x": 5.505959202112108,
+            "y": 4.047314610372366,
+            "heading": 0.005057466820580134,
+            "angularVelocity": 7.029537528496964e-7,
+            "velocityX": -3.7635897133768776,
+            "velocityY": 0.3821397717793548,
+            "timestamp": 0.9378745909797855
+        },
+        {
+            "x": 5.196832806272751,
+            "y": 4.139357882135735,
+            "heading": 0.005057539347948128,
+            "angularVelocity": 8.506478964233103e-7,
+            "velocityX": -3.625634372587421,
+            "velocityY": 1.0795430424660248,
+            "timestamp": 1.0231359174324932
+        },
+        {
+            "x": 4.911616541791353,
+            "y": 4.287091740854739,
+            "heading": 0.008468262161445878,
+            "angularVelocity": 0.040003163864724633,
+            "velocityX": -3.3452008823671444,
+            "velocityY": 1.7327182776242007,
+            "timestamp": 1.108397243885201
+        },
+        {
+            "x": 4.672649683479067,
+            "y": 4.471709481894194,
+            "heading": 0.05890642847923189,
+            "angularVelocity": 0.5915714476464851,
+            "velocityX": -2.8027579238375626,
+            "velocityY": 2.165316313039838,
+            "timestamp": 1.1936585703379088
+        },
+        {
+            "x": 4.485112052095124,
+            "y": 4.685980588264911,
+            "heading": 0.1533943172973333,
+            "angularVelocity": 1.1082150929337078,
+            "velocityX": -2.1995626761432403,
+            "velocityY": 2.513110167122247,
+            "timestamp": 1.2789198967906166
+        },
+        {
+            "x": 4.353217601776123,
+            "y": 4.923890113830566,
+            "heading": 0.2635187752392858,
+            "angularVelocity": 1.291610892363634,
+            "velocityX": -1.5469434479432043,
+            "velocityY": 2.7903568413003317,
+            "timestamp": 1.3641812232433244
+        },
+        {
+            "x": 4.2800604900800625,
+            "y": 5.16873168779887,
+            "heading": 0.3704891219101591,
+            "angularVelocity": 1.3052114311350342,
+            "velocityX": -0.8926352155228909,
+            "velocityY": 2.987463639297507,
+            "timestamp": 1.4461375592962549
+        },
+        {
+            "x": 4.26199164107549,
+            "y": 5.42343977988949,
+            "heading": 0.46980573799025893,
+            "angularVelocity": 1.2118235253560785,
+            "velocityX": -0.22046921415441664,
+            "velocityY": 3.107851135829132,
+            "timestamp": 1.5280938953491854
+        },
+        {
+            "x": 4.2996600311730235,
+            "y": 5.679753559001703,
+            "heading": 0.550426603173402,
+            "angularVelocity": 0.9837050930723119,
+            "velocityX": 0.45961535021819666,
+            "velocityY": 3.127443117352554,
+            "timestamp": 1.610050231402116
+        },
+        {
+            "x": 4.391931306983731,
+            "y": 5.927283560440951,
+            "heading": 0.5996302346047028,
+            "angularVelocity": 0.6003639718722656,
+            "velocityX": 1.1258589665496648,
+            "velocityY": 3.0202668074301977,
+            "timestamp": 1.6920065674550464
+        },
+        {
+            "x": 4.533405002431738,
+            "y": 6.1528417215915425,
+            "heading": 0.5943564820218653,
+            "angularVelocity": -0.06434832053139185,
+            "velocityX": 1.7262081525536632,
+            "velocityY": 2.752174780055045,
+            "timestamp": 1.773962903507977
+        },
+        {
+            "x": 4.71098037645994,
+            "y": 6.338671203945781,
+            "heading": 0.5303639205613101,
+            "angularVelocity": -0.7808128638054942,
+            "velocityX": 2.166707085507742,
+            "velocityY": 2.2674205717817246,
+            "timestamp": 1.8559192395609074
+        },
+        {
+            "x": 4.8864330916758485,
+            "y": 6.469038507428809,
+            "heading": 0.43960837441640055,
+            "angularVelocity": -1.107364610427659,
+            "velocityX": 2.1408072110807552,
+            "velocityY": 1.5906921875921145,
+            "timestamp": 1.937875575613838
+        },
+        {
+            "x": 5.028345407892232,
+            "y": 6.554592883552385,
+            "heading": 0.3555321434700494,
+            "angularVelocity": -1.0258661501483803,
+            "velocityX": 1.7315600361235026,
+            "velocityY": 1.0439019146530064,
+            "timestamp": 2.0198319116667687
+        },
+        {
+            "x": 5.126173645194142,
+            "y": 6.6061448414321,
+            "heading": 0.29324170563915075,
+            "angularVelocity": -0.760044199524953,
+            "velocityX": 1.1936628943333356,
+            "velocityY": 0.6290173568329146,
+            "timestamp": 2.1017882477196994
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": -0.405602632332653,
+            "velocityX": 0.6079622053097948,
+            "velocityY": 0.29107155976928517,
+            "timestamp": 2.18374458377263
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": 0,
+            "velocityX": 1.6766347007166246e-32,
+            "velocityY": 1.3162553729765107e-31,
+            "timestamp": 2.265700919825561
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S1.traj
new file mode 100644
index 00000000..d3b44a34
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S1.traj	
@@ -0,0 +1,256 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": -3.908498355076501e-28,
+            "velocityX": -5.285170566576542e-31,
+            "velocityY": -1.5421992925545815e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 7.644672083885401,
+            "y": 2.5062543236054426,
+            "heading": 0.0003371503707738104,
+            "angularVelocity": 0.003954317681676867,
+            "velocityX": -0.4143486570570975,
+            "velocityY": 0.5776865743787781,
+            "timestamp": 0.08526132645270777
+        },
+        {
+            "x": 7.574016248997627,
+            "y": 2.604762966378743,
+            "heading": 0.0010114541718947692,
+            "angularVelocity": 0.00790867124800936,
+            "velocityX": -0.8286973452956875,
+            "velocityY": 1.1553730967106797,
+            "timestamp": 0.17052265290541554
+        },
+        {
+            "x": 7.4680324930779305,
+            "y": 2.7525259224839496,
+            "heading": 0.0020229127878395882,
+            "angularVelocity": 0.01186304105238048,
+            "velocityX": -1.243046060026343,
+            "velocityY": 1.73305955059478,
+            "timestamp": 0.2557839793581233
+        },
+        {
+            "x": 7.326720815099078,
+            "y": 2.949543182754876,
+            "heading": 0.0033715247016779517,
+            "angularVelocity": 0.015817393065211405,
+            "velocityX": -1.657394786805044,
+            "velocityY": 2.310745896971908,
+            "timestamp": 0.3410453058108311
+        },
+        {
+            "x": 7.1500812197312955,
+            "y": 3.1958147252550657,
+            "heading": 0.005057281636821727,
+            "angularVelocity": 0.01977164800486184,
+            "velocityX": -2.0717434588083226,
+            "velocityY": 2.8884319860640817,
+            "timestamp": 0.4263066322635389
+        },
+        {
+            "x": 6.957409028465344,
+            "y": 3.454481547786369,
+            "heading": 0.005057281852786698,
+            "angularVelocity": 2.532976909732762e-9,
+            "velocityX": -2.2597841164587846,
+            "velocityY": 3.0338118499096547,
+            "timestamp": 0.5115679587162466
+        },
+        {
+            "x": 6.719745009541024,
+            "y": 3.6725340903222867,
+            "heading": 0.005057282088140234,
+            "angularVelocity": 2.760378532567681e-9,
+            "velocityX": -2.787477380570894,
+            "velocityY": 2.5574612970252826,
+            "timestamp": 0.5968292851689544
+        },
+        {
+            "x": 6.445480997294438,
+            "y": 3.842270194716782,
+            "heading": 0.005057282389511224,
+            "angularVelocity": 3.534673960970524e-9,
+            "velocityX": -3.2167457821427474,
+            "velocityY": 1.9907748501725318,
+            "timestamp": 0.6820906116216622
+        },
+        {
+            "x": 6.1443030532206215,
+            "y": 3.9576952313720115,
+            "heading": 0.0050573451663417065,
+            "angularVelocity": 7.362872839766494e-7,
+            "velocityX": -3.5324097877003195,
+            "velocityY": 1.3537795089212774,
+            "timestamp": 0.76735193807437
+        },
+        {
+            "x": 5.8268478532983945,
+            "y": 4.0147328665402,
+            "heading": 0.005057406885810731,
+            "angularVelocity": 7.238858646916699e-7,
+            "velocityX": -3.723319975538169,
+            "velocityY": 0.6689742881224171,
+            "timestamp": 0.8526132645270778
+        },
+        {
+            "x": 5.505959202112108,
+            "y": 4.047314610372366,
+            "heading": 0.005057466820580134,
+            "angularVelocity": 7.029537528496964e-7,
+            "velocityX": -3.7635897133768776,
+            "velocityY": 0.3821397717793548,
+            "timestamp": 0.9378745909797855
+        },
+        {
+            "x": 5.196832806272751,
+            "y": 4.139357882135735,
+            "heading": 0.005057539347948128,
+            "angularVelocity": 8.506478964233103e-7,
+            "velocityX": -3.625634372587421,
+            "velocityY": 1.0795430424660248,
+            "timestamp": 1.0231359174324932
+        },
+        {
+            "x": 4.911616541791353,
+            "y": 4.287091740854739,
+            "heading": 0.008468262161445878,
+            "angularVelocity": 0.040003163864724633,
+            "velocityX": -3.3452008823671444,
+            "velocityY": 1.7327182776242007,
+            "timestamp": 1.108397243885201
+        },
+        {
+            "x": 4.672649683479067,
+            "y": 4.471709481894194,
+            "heading": 0.05890642847923189,
+            "angularVelocity": 0.5915714476464851,
+            "velocityX": -2.8027579238375626,
+            "velocityY": 2.165316313039838,
+            "timestamp": 1.1936585703379088
+        },
+        {
+            "x": 4.485112052095124,
+            "y": 4.685980588264911,
+            "heading": 0.1533943172973333,
+            "angularVelocity": 1.1082150929337078,
+            "velocityX": -2.1995626761432403,
+            "velocityY": 2.513110167122247,
+            "timestamp": 1.2789198967906166
+        },
+        {
+            "x": 4.353217601776123,
+            "y": 4.923890113830566,
+            "heading": 0.2635187752392858,
+            "angularVelocity": 1.291610892363634,
+            "velocityX": -1.5469434479432043,
+            "velocityY": 2.7903568413003317,
+            "timestamp": 1.3641812232433244
+        },
+        {
+            "x": 4.2800604900800625,
+            "y": 5.16873168779887,
+            "heading": 0.3704891219101591,
+            "angularVelocity": 1.3052114311350342,
+            "velocityX": -0.8926352155228909,
+            "velocityY": 2.987463639297507,
+            "timestamp": 1.4461375592962549
+        },
+        {
+            "x": 4.26199164107549,
+            "y": 5.42343977988949,
+            "heading": 0.46980573799025893,
+            "angularVelocity": 1.2118235253560785,
+            "velocityX": -0.22046921415441664,
+            "velocityY": 3.107851135829132,
+            "timestamp": 1.5280938953491854
+        },
+        {
+            "x": 4.2996600311730235,
+            "y": 5.679753559001703,
+            "heading": 0.550426603173402,
+            "angularVelocity": 0.9837050930723119,
+            "velocityX": 0.45961535021819666,
+            "velocityY": 3.127443117352554,
+            "timestamp": 1.610050231402116
+        },
+        {
+            "x": 4.391931306983731,
+            "y": 5.927283560440951,
+            "heading": 0.5996302346047028,
+            "angularVelocity": 0.6003639718722656,
+            "velocityX": 1.1258589665496648,
+            "velocityY": 3.0202668074301977,
+            "timestamp": 1.6920065674550464
+        },
+        {
+            "x": 4.533405002431738,
+            "y": 6.1528417215915425,
+            "heading": 0.5943564820218653,
+            "angularVelocity": -0.06434832053139185,
+            "velocityX": 1.7262081525536632,
+            "velocityY": 2.752174780055045,
+            "timestamp": 1.773962903507977
+        },
+        {
+            "x": 4.71098037645994,
+            "y": 6.338671203945781,
+            "heading": 0.5303639205613101,
+            "angularVelocity": -0.7808128638054942,
+            "velocityX": 2.166707085507742,
+            "velocityY": 2.2674205717817246,
+            "timestamp": 1.8559192395609074
+        },
+        {
+            "x": 4.8864330916758485,
+            "y": 6.469038507428809,
+            "heading": 0.43960837441640055,
+            "angularVelocity": -1.107364610427659,
+            "velocityX": 2.1408072110807552,
+            "velocityY": 1.5906921875921145,
+            "timestamp": 1.937875575613838
+        },
+        {
+            "x": 5.028345407892232,
+            "y": 6.554592883552385,
+            "heading": 0.3555321434700494,
+            "angularVelocity": -1.0258661501483803,
+            "velocityX": 1.7315600361235026,
+            "velocityY": 1.0439019146530064,
+            "timestamp": 2.0198319116667687
+        },
+        {
+            "x": 5.126173645194142,
+            "y": 6.6061448414321,
+            "heading": 0.29324170563915075,
+            "angularVelocity": -0.760044199524953,
+            "velocityX": 1.1936628943333356,
+            "velocityY": 0.6290173568329146,
+            "timestamp": 2.1017882477196994
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": -0.405602632332653,
+            "velocityX": 0.6079622053097948,
+            "velocityY": 0.29107155976928517,
+            "timestamp": 2.18374458377263
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": 0,
+            "velocityX": 1.6766347007166246e-32,
+            "velocityY": 1.3162553729765107e-31,
+            "timestamp": 2.265700919825561
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S2.1.traj
new file mode 100644
index 00000000..3bd7e899
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S2.1.traj	
@@ -0,0 +1,211 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": -1.8537798939082814e-32,
+            "velocityX": 2.7477856809061307e-34,
+            "velocityY": 6.925951302123011e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6474171775045905,
+            "y": 2.4907449431726314,
+            "heading": -0.04287864331544942,
+            "angularVelocity": -0.5625230567182208,
+            "velocityX": -0.42745263118016213,
+            "velocityY": 0.4426984418000905,
+            "timestamp": 0.07622557476240162
+        },
+        {
+            "x": 7.582037622461459,
+            "y": 2.5583409566544844,
+            "heading": -0.12553871024043178,
+            "angularVelocity": -1.084413822823079,
+            "velocityX": -0.8577115390329612,
+            "velocityY": 0.8867891608892778,
+            "timestamp": 0.15245114952480324
+        },
+        {
+            "x": 7.483606625879984,
+            "y": 2.659997722482693,
+            "heading": -0.2429013405408753,
+            "angularVelocity": -1.5396752424139517,
+            "velocityX": -1.2913119630555505,
+            "velocityY": 1.333630689503849,
+            "timestamp": 0.22867672428720487
+        },
+        {
+            "x": 7.3517522665520065,
+            "y": 2.7960803575645476,
+            "heading": -0.3842395530561246,
+            "angularVelocity": -1.8542098627108627,
+            "velocityX": -1.7297916052318956,
+            "velocityY": 1.785262170944986,
+            "timestamp": 0.3049022990496065
+        },
+        {
+            "x": 7.186045392769314,
+            "y": 2.9668045775727996,
+            "heading": -0.5218107815453775,
+            "angularVelocity": -1.8047909631126855,
+            "velocityX": -2.173901270002986,
+            "velocityY": 2.2397236169147496,
+            "timestamp": 0.3811278738120081
+        },
+        {
+            "x": 6.990773522032128,
+            "y": 3.1645972581511623,
+            "heading": -0.5514162056749212,
+            "angularVelocity": -0.38839227151549954,
+            "velocityX": -2.5617631791673237,
+            "velocityY": 2.59483357383517,
+            "timestamp": 0.4573534485744097
+        },
+        {
+            "x": 6.791088913724043,
+            "y": 3.3726253897030523,
+            "heading": -0.5514162553808045,
+            "angularVelocity": -6.520893213469087e-7,
+            "velocityX": -2.619653691435051,
+            "velocityY": 2.729112009982516,
+            "timestamp": 0.5335790233368113
+        },
+        {
+            "x": 6.578307530314685,
+            "y": 3.567237130643777,
+            "heading": -0.5514163004289083,
+            "angularVelocity": -5.90984115130558e-7,
+            "velocityX": -2.7914697143656473,
+            "velocityY": 2.55310296507882,
+            "timestamp": 0.609804598099213
+        },
+        {
+            "x": 6.335948512349157,
+            "y": 3.7234797461898013,
+            "heading": -0.551416294920752,
+            "angularVelocity": 7.226126339733625e-8,
+            "velocityX": -3.179497415676709,
+            "velocityY": 2.0497400778287207,
+            "timestamp": 0.6860301728616146
+        },
+        {
+            "x": 6.088743524617576,
+            "y": 3.871936280628758,
+            "heading": -0.5514162894475348,
+            "angularVelocity": 7.180289728327137e-8,
+            "velocityX": -3.2430714822699573,
+            "velocityY": 1.9475948184280927,
+            "timestamp": 0.7622557476240163
+        },
+        {
+            "x": 5.841537785943911,
+            "y": 4.020391564616265,
+            "heading": -0.5514162839746023,
+            "angularVelocity": 7.179916458555982e-8,
+            "velocityX": -3.2430813338464923,
+            "velocityY": 1.947578413809903,
+            "timestamp": 0.8384813223864179
+        },
+        {
+            "x": 5.581140041351318,
+            "y": 4.144257068634033,
+            "heading": -0.5514162991479159,
+            "angularVelocity": -1.9905804152585698e-7,
+            "velocityX": -3.416146685731968,
+            "velocityY": 1.6249861598795787,
+            "timestamp": 0.9147068971488196
+        },
+        {
+            "x": 5.261335999197585,
+            "y": 4.231658536911035,
+            "heading": -0.5514164840698423,
+            "angularVelocity": -0.000002110046686592666,
+            "velocityX": -3.6491154543506057,
+            "velocityY": 0.9972921119903242,
+            "timestamp": 1.0023456814389826
+        },
+        {
+            "x": 4.96430123021485,
+            "y": 4.378912897735688,
+            "heading": -0.551417377431188,
+            "angularVelocity": -0.000010193675698949444,
+            "velocityX": -3.3893072728996803,
+            "velocityY": 1.6802419387415284,
+            "timestamp": 1.0899844657291458
+        },
+        {
+            "x": 4.714895393240272,
+            "y": 4.564810737884653,
+            "heading": -0.6189140340648516,
+            "angularVelocity": -0.7701687920520328,
+            "velocityX": -2.8458386203626582,
+            "velocityY": 2.121182324180531,
+            "timestamp": 1.177623250019309
+        },
+        {
+            "x": 4.521287798442847,
+            "y": 4.7820097530438215,
+            "heading": -0.6965700023585185,
+            "angularVelocity": -0.8860913455457793,
+            "velocityX": -2.209154273026079,
+            "velocityY": 2.4783435429688825,
+            "timestamp": 1.265262034309472
+        },
+        {
+            "x": 4.386338131009024,
+            "y": 5.001818806087968,
+            "heading": -0.6848521841568085,
+            "angularVelocity": 0.1337058506301677,
+            "velocityX": -1.539839564490294,
+            "velocityY": 2.5081253103235808,
+            "timestamp": 1.3529008185996352
+        },
+        {
+            "x": 4.301033328473477,
+            "y": 5.211510806429668,
+            "heading": -0.519384386353178,
+            "angularVelocity": 1.888065873389841,
+            "velocityX": -0.9733681637243018,
+            "velocityY": 2.3926849515327757,
+            "timestamp": 1.4405396028897983
+        },
+        {
+            "x": 4.267176527868507,
+            "y": 5.385808771912563,
+            "heading": -0.30251041793580363,
+            "angularVelocity": 2.474634605830756,
+            "velocityX": -0.38632211616348305,
+            "velocityY": 1.988822265103685,
+            "timestamp": 1.5281783871799615
+        },
+        {
+            "x": 4.261077190237836,
+            "y": 5.502861748658668,
+            "heading": -0.11181365176971161,
+            "angularVelocity": 2.175940340919318,
+            "velocityX": -0.06959632861263781,
+            "velocityY": 1.335629854911658,
+            "timestamp": 1.6158171714701246
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 9.015876776981611e-33,
+            "angularVelocity": 1.275846677647969,
+            "velocityX": 0.021940169272516404,
+            "velocityY": 0.6519744860010044,
+            "timestamp": 1.7034559557602877
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": -9.923717904588528e-32,
+            "velocityX": -3.152679648787722e-32,
+            "velocityY": -1.9738855170003778e-32,
+            "timestamp": 1.7910947400504509
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S2.traj
new file mode 100644
index 00000000..3bd7e899
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S2.traj	
@@ -0,0 +1,211 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": -1.8537798939082814e-32,
+            "velocityX": 2.7477856809061307e-34,
+            "velocityY": 6.925951302123011e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6474171775045905,
+            "y": 2.4907449431726314,
+            "heading": -0.04287864331544942,
+            "angularVelocity": -0.5625230567182208,
+            "velocityX": -0.42745263118016213,
+            "velocityY": 0.4426984418000905,
+            "timestamp": 0.07622557476240162
+        },
+        {
+            "x": 7.582037622461459,
+            "y": 2.5583409566544844,
+            "heading": -0.12553871024043178,
+            "angularVelocity": -1.084413822823079,
+            "velocityX": -0.8577115390329612,
+            "velocityY": 0.8867891608892778,
+            "timestamp": 0.15245114952480324
+        },
+        {
+            "x": 7.483606625879984,
+            "y": 2.659997722482693,
+            "heading": -0.2429013405408753,
+            "angularVelocity": -1.5396752424139517,
+            "velocityX": -1.2913119630555505,
+            "velocityY": 1.333630689503849,
+            "timestamp": 0.22867672428720487
+        },
+        {
+            "x": 7.3517522665520065,
+            "y": 2.7960803575645476,
+            "heading": -0.3842395530561246,
+            "angularVelocity": -1.8542098627108627,
+            "velocityX": -1.7297916052318956,
+            "velocityY": 1.785262170944986,
+            "timestamp": 0.3049022990496065
+        },
+        {
+            "x": 7.186045392769314,
+            "y": 2.9668045775727996,
+            "heading": -0.5218107815453775,
+            "angularVelocity": -1.8047909631126855,
+            "velocityX": -2.173901270002986,
+            "velocityY": 2.2397236169147496,
+            "timestamp": 0.3811278738120081
+        },
+        {
+            "x": 6.990773522032128,
+            "y": 3.1645972581511623,
+            "heading": -0.5514162056749212,
+            "angularVelocity": -0.38839227151549954,
+            "velocityX": -2.5617631791673237,
+            "velocityY": 2.59483357383517,
+            "timestamp": 0.4573534485744097
+        },
+        {
+            "x": 6.791088913724043,
+            "y": 3.3726253897030523,
+            "heading": -0.5514162553808045,
+            "angularVelocity": -6.520893213469087e-7,
+            "velocityX": -2.619653691435051,
+            "velocityY": 2.729112009982516,
+            "timestamp": 0.5335790233368113
+        },
+        {
+            "x": 6.578307530314685,
+            "y": 3.567237130643777,
+            "heading": -0.5514163004289083,
+            "angularVelocity": -5.90984115130558e-7,
+            "velocityX": -2.7914697143656473,
+            "velocityY": 2.55310296507882,
+            "timestamp": 0.609804598099213
+        },
+        {
+            "x": 6.335948512349157,
+            "y": 3.7234797461898013,
+            "heading": -0.551416294920752,
+            "angularVelocity": 7.226126339733625e-8,
+            "velocityX": -3.179497415676709,
+            "velocityY": 2.0497400778287207,
+            "timestamp": 0.6860301728616146
+        },
+        {
+            "x": 6.088743524617576,
+            "y": 3.871936280628758,
+            "heading": -0.5514162894475348,
+            "angularVelocity": 7.180289728327137e-8,
+            "velocityX": -3.2430714822699573,
+            "velocityY": 1.9475948184280927,
+            "timestamp": 0.7622557476240163
+        },
+        {
+            "x": 5.841537785943911,
+            "y": 4.020391564616265,
+            "heading": -0.5514162839746023,
+            "angularVelocity": 7.179916458555982e-8,
+            "velocityX": -3.2430813338464923,
+            "velocityY": 1.947578413809903,
+            "timestamp": 0.8384813223864179
+        },
+        {
+            "x": 5.581140041351318,
+            "y": 4.144257068634033,
+            "heading": -0.5514162991479159,
+            "angularVelocity": -1.9905804152585698e-7,
+            "velocityX": -3.416146685731968,
+            "velocityY": 1.6249861598795787,
+            "timestamp": 0.9147068971488196
+        },
+        {
+            "x": 5.261335999197585,
+            "y": 4.231658536911035,
+            "heading": -0.5514164840698423,
+            "angularVelocity": -0.000002110046686592666,
+            "velocityX": -3.6491154543506057,
+            "velocityY": 0.9972921119903242,
+            "timestamp": 1.0023456814389826
+        },
+        {
+            "x": 4.96430123021485,
+            "y": 4.378912897735688,
+            "heading": -0.551417377431188,
+            "angularVelocity": -0.000010193675698949444,
+            "velocityX": -3.3893072728996803,
+            "velocityY": 1.6802419387415284,
+            "timestamp": 1.0899844657291458
+        },
+        {
+            "x": 4.714895393240272,
+            "y": 4.564810737884653,
+            "heading": -0.6189140340648516,
+            "angularVelocity": -0.7701687920520328,
+            "velocityX": -2.8458386203626582,
+            "velocityY": 2.121182324180531,
+            "timestamp": 1.177623250019309
+        },
+        {
+            "x": 4.521287798442847,
+            "y": 4.7820097530438215,
+            "heading": -0.6965700023585185,
+            "angularVelocity": -0.8860913455457793,
+            "velocityX": -2.209154273026079,
+            "velocityY": 2.4783435429688825,
+            "timestamp": 1.265262034309472
+        },
+        {
+            "x": 4.386338131009024,
+            "y": 5.001818806087968,
+            "heading": -0.6848521841568085,
+            "angularVelocity": 0.1337058506301677,
+            "velocityX": -1.539839564490294,
+            "velocityY": 2.5081253103235808,
+            "timestamp": 1.3529008185996352
+        },
+        {
+            "x": 4.301033328473477,
+            "y": 5.211510806429668,
+            "heading": -0.519384386353178,
+            "angularVelocity": 1.888065873389841,
+            "velocityX": -0.9733681637243018,
+            "velocityY": 2.3926849515327757,
+            "timestamp": 1.4405396028897983
+        },
+        {
+            "x": 4.267176527868507,
+            "y": 5.385808771912563,
+            "heading": -0.30251041793580363,
+            "angularVelocity": 2.474634605830756,
+            "velocityX": -0.38632211616348305,
+            "velocityY": 1.988822265103685,
+            "timestamp": 1.5281783871799615
+        },
+        {
+            "x": 4.261077190237836,
+            "y": 5.502861748658668,
+            "heading": -0.11181365176971161,
+            "angularVelocity": 2.175940340919318,
+            "velocityX": -0.06959632861263781,
+            "velocityY": 1.335629854911658,
+            "timestamp": 1.6158171714701246
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 9.015876776981611e-33,
+            "angularVelocity": 1.275846677647969,
+            "velocityX": 0.021940169272516404,
+            "velocityY": 0.6519744860010044,
+            "timestamp": 1.7034559557602877
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": -9.923717904588528e-32,
+            "velocityX": -3.152679648787722e-32,
+            "velocityY": -1.9738855170003778e-32,
+            "timestamp": 1.7910947400504509
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S3.1.traj
new file mode 100644
index 00000000..80fc230a
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S3.1.traj	
@@ -0,0 +1,229 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 7.203126847267154e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6459509070752,
+            "y": 2.420050479995149,
+            "heading": -0.026297254997510774,
+            "angularVelocity": -0.33695614983619343,
+            "velocityX": -0.4362832264603014,
+            "velocityY": -0.47344743777665743,
+            "timestamp": 0.078043552581802
+        },
+        {
+            "x": 7.57752123433784,
+            "y": 2.3463325784745406,
+            "heading": -0.07655763531528098,
+            "angularVelocity": -0.6440042598660941,
+            "velocityX": -0.8768139131756897,
+            "velocityY": -0.9445738831958564,
+            "timestamp": 0.156087105163604
+        },
+        {
+            "x": 7.474195561438856,
+            "y": 2.236153787211583,
+            "heading": -0.14701004173730176,
+            "angularVelocity": -0.9027319245646475,
+            "velocityX": -1.3239488654839289,
+            "velocityY": -1.4117603263569152,
+            "timestamp": 0.23413065774540598
+        },
+        {
+            "x": 7.33507656702923,
+            "y": 2.0901340736402396,
+            "heading": -0.23055657416165956,
+            "angularVelocity": -1.0705116522834195,
+            "velocityX": -1.7825815177216175,
+            "velocityY": -1.8710029046700958,
+            "timestamp": 0.312174210327208
+        },
+        {
+            "x": 7.158294386463989,
+            "y": 1.910067298253472,
+            "heading": -0.30975533817585227,
+            "angularVelocity": -1.014802137964151,
+            "velocityX": -2.2651734155741057,
+            "velocityY": -2.3072601057983104,
+            "timestamp": 0.39021776290901
+        },
+        {
+            "x": 6.940360292129115,
+            "y": 1.7108993896694566,
+            "heading": -0.30975560484167247,
+            "angularVelocity": -0.00000341688469228089,
+            "velocityX": -2.7924676302559823,
+            "velocityY": -2.552009768843728,
+            "timestamp": 0.468261315490812
+        },
+        {
+            "x": 6.6915168927330235,
+            "y": 1.5520279357314815,
+            "heading": -0.30975564017638474,
+            "angularVelocity": -4.5275632793576304e-7,
+            "velocityX": -3.1885196299240257,
+            "velocityY": -2.035676858398594,
+            "timestamp": 0.546304868072614
+        },
+        {
+            "x": 6.419127595403782,
+            "y": 1.43815413781978,
+            "heading": -0.30975568823518523,
+            "angularVelocity": -6.157946287917162e-7,
+            "velocityX": -3.490221655962278,
+            "velocityY": -1.4591057703627237,
+            "timestamp": 0.624348420654416
+        },
+        {
+            "x": 6.131252530334066,
+            "y": 1.37264761019134,
+            "heading": -0.30975577587401576,
+            "angularVelocity": -0.0000011229477336220622,
+            "velocityX": -3.6886463461279972,
+            "velocityY": -0.8393586076157934,
+            "timestamp": 0.7023919732362179
+        },
+        {
+            "x": 5.83657654735769,
+            "y": 1.3574693553002635,
+            "heading": -0.31020540722452916,
+            "angularVelocity": -0.005761287587978277,
+            "velocityX": -3.7757889438387497,
+            "velocityY": -0.1944844178532558,
+            "timestamp": 0.7804355258180199
+        },
+        {
+            "x": 5.546474933624268,
+            "y": 1.3928169012069702,
+            "heading": -0.3189695864044355,
+            "angularVelocity": -0.11229856778804584,
+            "velocityX": -3.717175911864151,
+            "velocityY": 0.4529207697160109,
+            "timestamp": 0.8584790783998218
+        },
+        {
+            "x": 5.303276771996363,
+            "y": 1.4607318697473424,
+            "heading": -0.3307229091538368,
+            "angularVelocity": -0.17360472172913416,
+            "velocityX": -3.5922053767043294,
+            "velocityY": 1.0031511484968414,
+            "timestamp": 0.9261807090485242
+        },
+        {
+            "x": 5.073563214559545,
+            "y": 1.564404927734292,
+            "heading": -0.34378229938421223,
+            "angularVelocity": -0.1928962434919207,
+            "velocityX": -3.3930284283517023,
+            "velocityY": 1.5313229089691542,
+            "timestamp": 0.9938823396972266
+        },
+        {
+            "x": 4.862974125111252,
+            "y": 1.7011642845126724,
+            "heading": -0.35827284529055603,
+            "angularVelocity": -0.2140354045770704,
+            "velocityX": -3.1105467834446325,
+            "velocityY": 2.020030469989471,
+            "timestamp": 1.0615839703459289
+        },
+        {
+            "x": 4.677671719496688,
+            "y": 1.8665243414779629,
+            "heading": -0.3791415933598452,
+            "angularVelocity": -0.30824587043792007,
+            "velocityX": -2.737044940262413,
+            "velocityY": 2.442482631227878,
+            "timestamp": 1.1292856009946313
+        },
+        {
+            "x": 4.523629451068858,
+            "y": 2.053477766497112,
+            "heading": -0.41478083919993697,
+            "angularVelocity": -0.5264163580502632,
+            "velocityX": -2.27531105162187,
+            "velocityY": 2.761431640977936,
+            "timestamp": 1.1969872316433336
+        },
+        {
+            "x": 4.406611773563404,
+            "y": 2.2498215105595722,
+            "heading": -0.45494733767836987,
+            "angularVelocity": -0.5932870167770028,
+            "velocityX": -1.7284321867022447,
+            "velocityY": 2.900133160474012,
+            "timestamp": 1.264688862292036
+        },
+        {
+            "x": 4.327688857963163,
+            "y": 2.4438365235550945,
+            "heading": -0.48799769437613305,
+            "angularVelocity": -0.4881766713911416,
+            "velocityX": -1.1657461547677304,
+            "velocityY": 2.865736188870776,
+            "timestamp": 1.3323904929407384
+        },
+        {
+            "x": 4.28360260298108,
+            "y": 2.622213717107768,
+            "heading": -0.5154307830643909,
+            "angularVelocity": -0.40520573028252516,
+            "velocityX": -0.6511845366152605,
+            "velocityY": 2.634754759132393,
+            "timestamp": 1.4000921235894408
+        },
+        {
+            "x": 4.264391797811532,
+            "y": 2.7716303897908317,
+            "heading": -0.5365689504315967,
+            "angularVelocity": -0.3122253801676767,
+            "velocityX": -0.283756904899847,
+            "velocityY": 2.2069877970647664,
+            "timestamp": 1.4677937542381432
+        },
+        {
+            "x": 4.259328393836262,
+            "y": 2.8855840206662466,
+            "heading": -0.5517027167443236,
+            "angularVelocity": -0.22353621571242488,
+            "velocityX": -0.07478998551086755,
+            "velocityY": 1.6831740946789628,
+            "timestamp": 1.5354953848868456
+        },
+        {
+            "x": 4.260599830778067,
+            "y": 2.9618797181043535,
+            "heading": -0.5613571988137664,
+            "angularVelocity": -0.1426033904496253,
+            "velocityX": 0.018780004700190214,
+            "velocityY": 1.1269403219251917,
+            "timestamp": 1.603197015535548
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -0.0685773908510417,
+            "velocityX": 0.035452162657366115,
+            "velocityY": 0.5630629798778553,
+            "timestamp": 1.6708986461842503
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 0,
+            "velocityX": 1.1764020723168042e-36,
+            "velocityY": 0,
+            "timestamp": 1.7386002768329527
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.1.traj
new file mode 100644
index 00000000..e5689e4c
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.1.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": 1.0550386775092505e-35,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6504501923310215,
+            "y": 2.437078665707293,
+            "heading": -0.0029944704440250615,
+            "angularVelocity": -0.0457972069493345,
+            "velocityX": -0.45193254781641656,
+            "velocityY": -0.30467539632270557,
+            "timestamp": 0.0653854381848624
+        },
+        {
+            "x": 7.5913505770823395,
+            "y": 2.3972359988448533,
+            "heading": -0.008983367804687599,
+            "angularVelocity": -0.09159374819405963,
+            "velocityX": -0.9038650942674934,
+            "velocityY": -0.609350766294381,
+            "timestamp": 0.1307708763697248
+        },
+        {
+            "x": 7.50270115478166,
+            "y": 2.337472001372052,
+            "heading": -0.017966623739921285,
+            "angularVelocity": -0.13738924422033513,
+            "velocityX": -1.3557976326478658,
+            "velocityY": -0.91402610629957,
+            "timestamp": 0.19615631455458718
+        },
+        {
+            "x": 7.384501926877228,
+            "y": 2.2577866758321745,
+            "heading": -0.02994413949472175,
+            "angularVelocity": -0.18318322989496186,
+            "velocityX": -1.8077301488788986,
+            "velocityY": -1.2187014074079496,
+            "timestamp": 0.2615417527394496
+        },
+        {
+            "x": 7.2367528971895485,
+            "y": 2.1581800263627264,
+            "heading": -0.044915766222073404,
+            "angularVelocity": -0.2289749391144676,
+            "velocityX": -2.25966260667937,
+            "velocityY": -1.5233766452376303,
+            "timestamp": 0.326927190924312
+        },
+        {
+            "x": 7.059454079201559,
+            "y": 2.0386520636124454,
+            "heading": -0.06288119646669635,
+            "angularVelocity": -0.2747619461359239,
+            "velocityX": -2.711594858272837,
+            "velocityY": -1.82805172020631,
+            "timestamp": 0.3923126291091744
+        },
+        {
+            "x": 6.854358935700705,
+            "y": 1.900385597577332,
+            "heading": -0.06288119947800026,
+            "angularVelocity": -4.6054656889756363e-8,
+            "velocityX": -3.136709781174733,
+            "velocityY": -2.114636987584247,
+            "timestamp": 0.45769806729403684
+        },
+        {
+            "x": 6.636845838072133,
+            "y": 1.782615967982618,
+            "heading": -0.06288120256062167,
+            "angularVelocity": -4.714538109405926e-8,
+            "velocityX": -3.3266290425951324,
+            "velocityY": -1.801159904469057,
+            "timestamp": 0.5230835054788993
+        },
+        {
+            "x": 6.4046630859375,
+            "y": 1.6973352432250977,
+            "heading": -0.06288120593828476,
+            "angularVelocity": -5.1657726572043936e-8,
+            "velocityX": -3.5509856411482486,
+            "velocityY": -1.3042770244409516,
+            "timestamp": 0.5884689436637617
+        },
+        {
+            "x": 6.207746736822867,
+            "y": 1.6503996785686041,
+            "heading": -0.06288121580879107,
+            "angularVelocity": -1.8445407171610623e-7,
+            "velocityX": -3.679854027466254,
+            "velocityY": -0.8771035386810692,
+            "timestamp": 0.6419809437716079
+        },
+        {
+            "x": 6.009908768236521,
+            "y": 1.6271742750630454,
+            "heading": -0.0724170210773865,
+            "angularVelocity": -0.17819938050114592,
+            "velocityX": -3.697076696584537,
+            "velocityY": -0.43402234001253925,
+            "timestamp": 0.695492943879454
+        },
+        {
+            "x": 5.8260492909953046,
+            "y": 1.6186155743000838,
+            "heading": -0.12620190803666176,
+            "angularVelocity": -1.0050995449783064,
+            "velocityX": -3.43585507681777,
+            "velocityY": -0.1599398405163814,
+            "timestamp": 0.7490049439873002
+        },
+        {
+            "x": 5.6625912601386625,
+            "y": 1.614879890442449,
+            "heading": -0.22199295256207294,
+            "angularVelocity": -1.7900852954918034,
+            "velocityX": -3.0546051451490293,
+            "velocityY": -0.06981020799271546,
+            "timestamp": 0.8025169440951464
+        },
+        {
+            "x": 5.5227239807129855,
+            "y": 1.6141995251565013,
+            "heading": -0.32512734843535046,
+            "angularVelocity": -1.9273134187738115,
+            "velocityX": -2.61375540334492,
+            "velocityY": -0.012714256327113658,
+            "timestamp": 0.8560289442029926
+        },
+        {
+            "x": 5.406619958232834,
+            "y": 1.6150039384726416,
+            "heading": -0.42188556792063076,
+            "angularVelocity": -1.8081592781110263,
+            "velocityX": -2.169681982474204,
+            "velocityY": 0.015032391136922594,
+            "timestamp": 0.9095409443108388
+        },
+        {
+            "x": 5.314084878105213,
+            "y": 1.6164421457440925,
+            "heading": -0.5051954003859285,
+            "angularVelocity": -1.5568439284160218,
+            "velocityX": -1.7292397955809797,
+            "velocityY": 0.026876350511144828,
+            "timestamp": 0.9630529444186849
+        },
+        {
+            "x": 5.244912091070787,
+            "y": 1.617980616061371,
+            "heading": -0.5709381839957368,
+            "angularVelocity": -1.2285615091439794,
+            "velocityX": -1.2926593454742539,
+            "velocityY": 0.028750005871169333,
+            "timestamp": 1.016564944526531
+        },
+        {
+            "x": 5.198930392622704,
+            "y": 1.6192554664106382,
+            "heading": -0.6164892293058035,
+            "angularVelocity": -0.851230475748707,
+            "velocityX": -0.8592782619863396,
+            "velocityY": 0.023823634823929046,
+            "timestamp": 1.0700769446343772
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -0.4393551100092281,
+            "velocityX": -0.42850935447171357,
+            "velocityY": 0.013913394899485362,
+            "timestamp": 1.1235889447422234
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 6.2403191565519e-36,
+            "timestamp": 1.1771009448500696
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.1.traj
new file mode 100644
index 00000000..f850aede
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.1.traj	
@@ -0,0 +1,238 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": 2.322866633708497e-28,
+            "velocityX": -2.1618763976557105e-31,
+            "velocityY": -1.247476705840714e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 7.635666877682845,
+            "y": 2.515362381859407,
+            "heading": -0.0008300252680408195,
+            "angularVelocity": -0.00885317162168204,
+            "velocityX": -0.4728636048938783,
+            "velocityY": 0.6225017511467659,
+            "timestamp": 0.09375456655646779
+        },
+        {
+            "x": 7.547000631163484,
+            "y": 2.632087139923421,
+            "heading": -0.0024900693866490363,
+            "angularVelocity": -0.01770627479364842,
+            "velocityX": -0.9457272298940119,
+            "velocityY": 1.2450034419786018,
+            "timestamp": 0.18750913311293557
+        },
+        {
+            "x": 7.4140012603329,
+            "y": 2.8071742656560237,
+            "heading": -0.004980126198969091,
+            "angularVelocity": -0.026559312295687615,
+            "velocityX": -1.418590856056911,
+            "velocityY": 1.867505041764004,
+            "timestamp": 0.28126369966940334
+        },
+        {
+            "x": 7.236668772838196,
+            "y": 3.0406237394955027,
+            "heading": -0.008300187115435396,
+            "angularVelocity": -0.035412258180156565,
+            "velocityX": -1.891454400654691,
+            "velocityY": 2.4900064329013105,
+            "timestamp": 0.37501826622587114
+        },
+        {
+            "x": 7.022136423676778,
+            "y": 3.3230511197393278,
+            "heading": -0.008300187546286333,
+            "angularVelocity": -4.59551947810532e-9,
+            "velocityX": -2.2882335980104607,
+            "velocityY": 3.012411988206685,
+            "timestamp": 0.46877283278233894
+        },
+        {
+            "x": 6.786560921928144,
+            "y": 3.5881804025568163,
+            "heading": -0.008300187986713265,
+            "angularVelocity": -4.6976584720676315e-9,
+            "velocityX": -2.51268295936015,
+            "velocityY": 2.827907936172931,
+            "timestamp": 0.5625273993388067
+        },
+        {
+            "x": 6.5015204114211205,
+            "y": 3.799228403383322,
+            "heading": -0.008300188501414158,
+            "angularVelocity": -5.489875460815635e-9,
+            "velocityX": -3.0402840200359327,
+            "velocityY": 2.251069026055312,
+            "timestamp": 0.6562819658952744
+        },
+        {
+            "x": 6.179186778264032,
+            "y": 3.9471823835437703,
+            "heading": -0.008300169798343478,
+            "angularVelocity": 1.9948970346527163e-7,
+            "velocityX": -3.4380579527606097,
+            "velocityY": 1.5780989192812942,
+            "timestamp": 0.7500365324517422
+        },
+        {
+            "x": 5.833324835964247,
+            "y": 4.025724182042927,
+            "heading": -0.008299866211032016,
+            "angularVelocity": 0.0000032381069276099122,
+            "velocityX": -3.689014359545628,
+            "velocityY": 0.8377383778085268,
+            "timestamp": 0.8437910990082099
+        },
+        {
+            "x": 5.481092764022396,
+            "y": 4.0672194252777,
+            "heading": -0.008299498284594501,
+            "angularVelocity": 0.000003924357511610969,
+            "velocityX": -3.7569590994770814,
+            "velocityY": 0.44259436909433375,
+            "timestamp": 0.9375456655646777
+        },
+        {
+            "x": 5.14904862351455,
+            "y": 4.179095043376677,
+            "heading": 0.0033651913425959347,
+            "angularVelocity": 0.12441729566489874,
+            "velocityX": -3.5416316527670983,
+            "velocityY": 1.1932818017093059,
+            "timestamp": 1.0313002321211455
+        },
+        {
+            "x": 4.858987953002521,
+            "y": 4.350333701259038,
+            "heading": 0.047887100206889445,
+            "angularVelocity": 0.474877229979814,
+            "velocityX": -3.093829785211866,
+            "velocityY": 1.8264567174893411,
+            "timestamp": 1.1250547986776134
+        },
+        {
+            "x": 4.6181973815778665,
+            "y": 4.575631660267378,
+            "heading": 0.10901822024185409,
+            "angularVelocity": 0.652033519862158,
+            "velocityX": -2.5683076597621346,
+            "velocityY": 2.403061176467013,
+            "timestamp": 1.2188093652340812
+        },
+        {
+            "x": 4.427445933507109,
+            "y": 4.854263962903063,
+            "heading": 0.15298068479753676,
+            "angularVelocity": 0.46891011467909954,
+            "velocityX": -2.0345830083473238,
+            "velocityY": 2.971933132108998,
+            "timestamp": 1.312563931790549
+        },
+        {
+            "x": 4.19709169118396,
+            "y": 5.123942070734021,
+            "heading": 0.15298069409509907,
+            "angularVelocity": 9.916916750047786e-8,
+            "velocityX": -2.4569922381797515,
+            "velocityY": 2.8764263729866686,
+            "timestamp": 1.406318498347017
+        },
+        {
+            "x": 3.9282946631899107,
+            "y": 5.355323824233057,
+            "heading": 0.15298070467968702,
+            "angularVelocity": 1.1289677246612154e-7,
+            "velocityX": -2.8670286458223346,
+            "velocityY": 2.4679518235485323,
+            "timestamp": 1.5000730649034848
+        },
+        {
+            "x": 3.6594937496643456,
+            "y": 5.586701063819283,
+            "heading": 0.1529807152642756,
+            "angularVelocity": 1.1289677885329184e-7,
+            "velocityX": -2.867070089473112,
+            "velocityY": 2.4679036774904017,
+            "timestamp": 1.5938276314599527
+        },
+        {
+            "x": 3.3906928188108107,
+            "y": 5.818078283274842,
+            "heading": 0.1529807258488866,
+            "angularVelocity": 1.1289701863908675e-7,
+            "velocityX": -2.8670702742957928,
+            "velocityY": 2.467903462773746,
+            "timestamp": 1.6875821980164205
+        },
+        {
+            "x": 3.1218918754104785,
+            "y": 6.049455488154229,
+            "heading": 0.15298073643351034,
+            "angularVelocity": 1.1289715379702627e-7,
+            "velocityX": -2.867070408121775,
+            "velocityY": 2.467903307302158,
+            "timestamp": 1.7813367645728884
+        },
+        {
+            "x": 2.853090944613356,
+            "y": 6.280832707674931,
+            "heading": 0.15298074701856604,
+            "angularVelocity": 1.1290176153813175e-7,
+            "velocityX": -2.8670702736940825,
+            "velocityY": 2.4679034634685753,
+            "timestamp": 1.8750913311293562
+        },
+        {
+            "x": 2.631853660621228,
+            "y": 6.471299719512446,
+            "heading": 0.24894777935018825,
+            "angularVelocity": 1.0235984854542721,
+            "velocityX": -2.3597494193403232,
+            "velocityY": 2.0315491696374846,
+            "timestamp": 1.968845897685824
+        },
+        {
+            "x": 2.465926649664366,
+            "y": 6.614149879341798,
+            "heading": 0.32096506390227886,
+            "angularVelocity": 0.7681469521670183,
+            "velocityX": -1.7698019099360354,
+            "velocityY": 1.52366082075921,
+            "timestamp": 2.062600464242292
+        },
+        {
+            "x": 2.355308907973467,
+            "y": 6.709383273555383,
+            "heading": 0.3689895410149908,
+            "angularVelocity": 0.5122361382129277,
+            "velocityX": -1.1798651068828128,
+            "velocityY": 1.0157733933549364,
+            "timestamp": 2.1563550307987596
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0.25609908793666974,
+            "velocityX": -0.5899329494543092,
+            "velocityY": 0.5078870095990298,
+            "timestamp": 2.250109597355227
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0,
+            "velocityX": -3.330818616870831e-32,
+            "velocityY": 5.736512740310497e-33,
+            "timestamp": 2.343864163911695
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.1.traj
new file mode 100644
index 00000000..086204d2
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.1.traj	
@@ -0,0 +1,319 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": -1.0202939485165084e-30,
+            "velocityX": 2.0196671556133315e-31,
+            "velocityY": 6.267725686062039e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.650233104605526,
+            "y": 2.498844879842391,
+            "heading": 0.006167890922909293,
+            "angularVelocity": 0.07857204140894247,
+            "velocityX": -0.37919700052787514,
+            "velocityY": 0.5330570324317279,
+            "timestamp": 0.07849981764896931
+        },
+        {
+            "x": 7.590699315345095,
+            "y": 2.5825346366761623,
+            "heading": 0.018503878854508167,
+            "angularVelocity": 0.15714670812054757,
+            "velocityX": -0.758393981584137,
+            "velocityY": 1.066114028544761,
+            "timestamp": 0.15699963529793862
+        },
+        {
+            "x": 7.501398636107905,
+            "y": 2.7080692662264956,
+            "heading": 0.037008343975294504,
+            "angularVelocity": 0.23572621790707007,
+            "velocityX": -1.1375909130963788,
+            "velocityY": 1.599170970201378,
+            "timestamp": 0.23549945294690794
+        },
+        {
+            "x": 7.382331074013197,
+            "y": 2.875448760857071,
+            "heading": 0.061681864177524894,
+            "angularVelocity": 0.31431308939549074,
+            "velocityX": -1.5167877539174253,
+            "velocityY": 2.132227814579811,
+            "timestamp": 0.31399927059587723
+        },
+        {
+            "x": 7.2334966444595095,
+            "y": 3.084673101412061,
+            "heading": 0.09252521384117814,
+            "angularVelocity": 0.39290982561993243,
+            "velocityX": -1.8959843985782956,
+            "velocityY": 2.665284414934403,
+            "timestamp": 0.39249908824484653
+        },
+        {
+            "x": 7.061360166565958,
+            "y": 3.326653173894572,
+            "heading": 0.09252521821441809,
+            "angularVelocity": 5.5710192458533506e-8,
+            "velocityX": -2.192826468251186,
+            "velocityY": 3.0825558546464755,
+            "timestamp": 0.4709989058938158
+        },
+        {
+            "x": 6.8697148114409385,
+            "y": 3.553495365981472,
+            "heading": 0.09252522268181232,
+            "angularVelocity": 5.690961282999965e-8,
+            "velocityX": -2.4413477746153216,
+            "velocityY": 2.8897161659824335,
+            "timestamp": 0.5494987235427852
+        },
+        {
+            "x": 6.64183630533757,
+            "y": 3.743907306900521,
+            "heading": 0.09252522772743905,
+            "angularVelocity": 6.427564912453075e-8,
+            "velocityX": -2.902917649087824,
+            "velocityY": 2.425635455237886,
+            "timestamp": 0.6279985411917545
+        },
+        {
+            "x": 6.384546476442235,
+            "y": 3.8921883279204876,
+            "heading": 0.09252523431940601,
+            "angularVelocity": 8.397429651316677e-8,
+            "velocityX": -3.277585051800601,
+            "velocityY": 1.8889345919635134,
+            "timestamp": 0.7064983588407238
+        },
+        {
+            "x": 6.105547891541977,
+            "y": 3.9938992218783795,
+            "heading": 0.09252527901816404,
+            "angularVelocity": 5.694122529822745e-7,
+            "velocityX": -3.5541303566827107,
+            "velocityY": 1.2956831876058104,
+            "timestamp": 0.7849981764896931
+        },
+        {
+            "x": 5.813193382307703,
+            "y": 4.045994945482373,
+            "heading": 0.09252629740574593,
+            "angularVelocity": 0.000012973120350997966,
+            "velocityX": -3.7242699153978625,
+            "velocityY": 0.6636413327346956,
+            "timestamp": 0.8634979941386624
+        },
+        {
+            "x": 5.523572440187452,
+            "y": 4.068080526231475,
+            "heading": 0.11166809511552328,
+            "angularVelocity": 0.24384512325081395,
+            "velocityX": -3.6894473235002847,
+            "velocityY": 0.28134563124547074,
+            "timestamp": 0.9419978117876316
+        },
+        {
+            "x": 5.248612904615211,
+            "y": 4.139233352038044,
+            "heading": 0.1457449942262438,
+            "angularVelocity": 0.43410163400767243,
+            "velocityX": -3.502677379478612,
+            "velocityY": 0.9064075298205869,
+            "timestamp": 1.020497629436601
+        },
+        {
+            "x": 4.992805004119873,
+            "y": 4.258058071136475,
+            "heading": 0.1827080793073441,
+            "angularVelocity": 0.47086841967443066,
+            "velocityX": -3.258706939158039,
+            "velocityY": 1.513694205377446,
+            "timestamp": 1.0989974470855703
+        },
+        {
+            "x": 4.782367562736006,
+            "y": 4.400850786547769,
+            "heading": 0.2087590252622848,
+            "angularVelocity": 0.37195010692359587,
+            "velocityX": -3.004582979783548,
+            "velocityY": 2.0387653429944033,
+            "timestamp": 1.1690362653840851
+        },
+        {
+            "x": 4.593366685154892,
+            "y": 4.57831882888555,
+            "heading": 0.22294244363545143,
+            "angularVelocity": 0.20250796226614393,
+            "velocityX": -2.6985160825467664,
+            "velocityY": 2.5338526070127387,
+            "timestamp": 1.2390750836825999
+        },
+        {
+            "x": 4.4303113780727985,
+            "y": 4.787155425256865,
+            "heading": 0.2229432862167668,
+            "angularVelocity": 0.000012030204616055241,
+            "velocityX": -2.3280705049466333,
+            "velocityY": 2.9817264403466797,
+            "timestamp": 1.3091139019811147
+        },
+        {
+            "x": 4.301341913399859,
+            "y": 5.018600438478609,
+            "heading": 0.22294334445620553,
+            "angularVelocity": 8.315308587772638e-7,
+            "velocityX": -1.8413997809506994,
+            "velocityY": 3.3045248170136206,
+            "timestamp": 1.3791527202796294
+        },
+        {
+            "x": 4.209527987601824,
+            "y": 5.267136397095996,
+            "heading": 0.22294334228224533,
+            "angularVelocity": -3.103936119065208e-8,
+            "velocityX": -1.3109005552544912,
+            "velocityY": 3.5485458586481258,
+            "timestamp": 1.4491915385781442
+        },
+        {
+            "x": 4.095163246631834,
+            "y": 5.506135613505712,
+            "heading": 0.22294334005402738,
+            "angularVelocity": -3.181404259759038e-8,
+            "velocityX": -1.6328765068917115,
+            "velocityY": 3.412382193414732,
+            "timestamp": 1.519230356876659
+        },
+        {
+            "x": 3.9453760475707735,
+            "y": 5.724684722787353,
+            "heading": 0.22294333755566106,
+            "angularVelocity": -3.567116580835512e-8,
+            "velocityX": -2.138631157692488,
+            "velocityY": 3.120399723908581,
+            "timestamp": 1.5892691751751737
+        },
+        {
+            "x": 3.7637358458155963,
+            "y": 5.917575181509908,
+            "heading": 0.22294327041118944,
+            "angularVelocity": -9.586751071941434e-7,
+            "velocityX": -2.5934218504529762,
+            "velocityY": 2.7540507308438293,
+            "timestamp": 1.6593079934736885
+        },
+        {
+            "x": 3.5705120272666107,
+            "y": 6.071441420591724,
+            "heading": 0.20870193370475335,
+            "angularVelocity": -0.20333490844666557,
+            "velocityX": -2.7588103746352264,
+            "velocityY": 2.1968708613263175,
+            "timestamp": 1.7293468117722033
+        },
+        {
+            "x": 3.3793495617108644,
+            "y": 6.184488312917552,
+            "heading": 0.18879422691132347,
+            "angularVelocity": -0.2842381878658059,
+            "velocityX": -2.7293787959290245,
+            "velocityY": 1.614060532031302,
+            "timestamp": 1.799385630070718
+        },
+        {
+            "x": 3.1945436254003248,
+            "y": 6.257137552443532,
+            "heading": 0.1660945073730067,
+            "angularVelocity": -0.3241019778712817,
+            "velocityX": -2.638621564444474,
+            "velocityY": 1.037271063258847,
+            "timestamp": 1.8694244483692328
+        },
+        {
+            "x": 3.018114205815821,
+            "y": 6.289753496686376,
+            "heading": 0.14197845255388317,
+            "angularVelocity": -0.34432412489220054,
+            "velocityX": -2.519023362625089,
+            "velocityY": 0.46568381699052885,
+            "timestamp": 1.9394632666677476
+        },
+        {
+            "x": 2.851226329803467,
+            "y": 6.282596111297607,
+            "heading": 0.11724722927389757,
+            "angularVelocity": -0.3531073750355408,
+            "velocityX": -2.382791144492779,
+            "velocityY": -0.10219169258892027,
+            "timestamp": 2.0095020849662624
+        },
+        {
+            "x": 2.661249908723212,
+            "y": 6.213695182206588,
+            "heading": 0.08663876117142615,
+            "angularVelocity": -0.3547239131111928,
+            "velocityX": -2.2016514926122546,
+            "velocityY": -0.7984982163199495,
+            "timestamp": 2.0957902289879784
+        },
+        {
+            "x": 2.4936356536291973,
+            "y": 6.0868793491721815,
+            "heading": 0.056963129639284946,
+            "angularVelocity": -0.3439131976774355,
+            "velocityX": -1.9424946149239757,
+            "velocityY": -1.4696785343117862,
+            "timestamp": 2.1820783730096944
+        },
+        {
+            "x": 2.3641635269275443,
+            "y": 5.911089762977232,
+            "heading": 0.030889400871559385,
+            "angularVelocity": -0.3021704669089439,
+            "velocityX": -1.500462527842606,
+            "velocityY": -2.037239161740341,
+            "timestamp": 2.2683665170314105
+        },
+        {
+            "x": 2.2965373597407854,
+            "y": 5.740054606624193,
+            "heading": 0.01408623201300084,
+            "angularVelocity": -0.19473322840668433,
+            "velocityX": -0.7837249016474405,
+            "velocityY": -1.9821397051956142,
+            "timestamp": 2.3546546610531265
+        },
+        {
+            "x": 2.263390196849459,
+            "y": 5.620608972310546,
+            "heading": 0.004408633441468959,
+            "angularVelocity": -0.11215444116055903,
+            "velocityX": -0.38414504410927525,
+            "velocityY": -1.384264729156902,
+            "timestamp": 2.4409428050748425
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -1.7581112791577623e-31,
+            "angularVelocity": -0.05109199521503166,
+            "velocityX": -0.1551800308289656,
+            "velocityY": -0.7024020854510996,
+            "timestamp": 2.5272309490965585
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 5.961493131289069e-30,
+            "velocityX": -9.776555553264502e-32,
+            "velocityY": 0,
+            "timestamp": 2.6135190931182746
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.traj
index f49877f8..086204d2 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.traj	
@@ -4,253 +4,316 @@
             "x": 7.68,
             "y": 2.457,
             "heading": 0,
-            "angularVelocity": 6.426641305668581e-31,
-            "velocityX": -5.011351780601486e-32,
-            "velocityY": -1.0551805148640748e-32,
+            "angularVelocity": -1.0202939485165084e-30,
+            "velocityX": 2.0196671556133315e-31,
+            "velocityY": 6.267725686062039e-32,
             "timestamp": 0
         },
         {
-            "x": 7.6491080594765135,
-            "y": 2.4958059548034544,
-            "heading": -0.02134777834454554,
-            "angularVelocity": -0.2757848804434067,
-            "velocityX": -0.3990827516771383,
-            "velocityY": 0.5013212819261742,
-            "timestamp": 0.07740735572675958
-        },
-        {
-            "x": 7.587308326375472,
-            "y": 2.5734484495799954,
-            "heading": -0.06302080337998518,
-            "angularVelocity": -0.538360013001088,
-            "velocityX": -0.7983702907923742,
-            "velocityY": 1.0030376835324535,
-            "timestamp": 0.15481471145351916
-        },
-        {
-            "x": 7.4945787588029695,
-            "y": 2.6899753845455243,
-            "heading": -0.12336744193035674,
-            "angularVelocity": -0.7795982433941759,
-            "velocityX": -1.1979425818371583,
-            "velocityY": 1.5053728921687748,
-            "timestamp": 0.23222206718027874
-        },
-        {
-            "x": 7.370887761034633,
-            "y": 2.845471450080925,
-            "heading": -0.19922081386712712,
-            "angularVelocity": -0.9799245979222482,
-            "velocityX": -1.5979230475841766,
-            "velocityY": 2.0088021877957765,
-            "timestamp": 0.3096294229070383
-        },
-        {
-            "x": 7.216202117777622,
-            "y": 3.0401051125017453,
-            "heading": -0.28194072984640106,
-            "angularVelocity": -1.0686312069781692,
-            "velocityX": -1.9983326107022177,
-            "velocityY": 2.5144078439762083,
-            "timestamp": 0.38703677863379793
-        },
-        {
-            "x": 7.032473858656331,
-            "y": 3.268122248690632,
-            "heading": -0.2819407809538647,
-            "angularVelocity": -6.602404015783628e-7,
-            "velocityX": -2.3735245493959742,
-            "velocityY": 2.9456778887237176,
-            "timestamp": 0.46444413436055754
-        },
-        {
-            "x": 6.834683469529352,
-            "y": 3.4840547774927524,
-            "heading": -0.28194080060331267,
-            "angularVelocity": -2.538447128540298e-7,
-            "velocityX": -2.5551885511392456,
-            "velocityY": 2.7895608469606037,
-            "timestamp": 0.5418514900873171
-        },
-        {
-            "x": 6.603064880376339,
-            "y": 3.663221523268011,
-            "heading": -0.2819408226641663,
-            "angularVelocity": -2.8499686418440244e-7,
-            "velocityX": -2.992203867170001,
-            "velocityY": 2.314595868740169,
-            "timestamp": 0.6192588458140768
-        },
-        {
-            "x": 6.344360158554223,
-            "y": 3.8004066501958644,
-            "heading": -0.2819407635550332,
-            "angularVelocity": 7.636113202840995e-7,
-            "velocityX": -3.342120647233029,
-            "velocityY": 1.7722492344539278,
-            "timestamp": 0.6966662015408364
-        },
-        {
-            "x": 6.0661003337524155,
-            "y": 3.891617196013079,
-            "heading": -0.28194070942615473,
-            "angularVelocity": 6.992730598438293e-7,
-            "velocityX": -3.59474654817093,
-            "velocityY": 1.1783188427102327,
-            "timestamp": 0.774073557267596
-        },
-        {
-            "x": 5.7830200242104235,
-            "y": 3.966540038544743,
-            "heading": -0.2819406560965727,
-            "angularVelocity": 6.889472132026456e-7,
-            "velocityX": -3.6570207945254936,
-            "velocityY": 0.967903396624646,
-            "timestamp": 0.8514809129943556
-        },
-        {
-            "x": 5.499939604590639,
-            "y": 4.041462465169378,
-            "heading": -0.2819406027672389,
-            "angularVelocity": 6.889440059098256e-7,
-            "velocityX": -3.657022216584049,
-            "velocityY": 0.9678980236594484,
-            "timestamp": 0.9288882687211152
-        },
-        {
-            "x": 5.234359644348809,
-            "y": 4.1279159927443425,
-            "heading": -0.3272205989423401,
-            "angularVelocity": -0.5849572789301486,
-            "velocityX": -3.430939576069496,
-            "velocityY": 1.1168644990295,
-            "timestamp": 1.0062956244478747
+            "x": 7.650233104605526,
+            "y": 2.498844879842391,
+            "heading": 0.006167890922909293,
+            "angularVelocity": 0.07857204140894247,
+            "velocityX": -0.37919700052787514,
+            "velocityY": 0.5330570324317279,
+            "timestamp": 0.07849981764896931
+        },
+        {
+            "x": 7.590699315345095,
+            "y": 2.5825346366761623,
+            "heading": 0.018503878854508167,
+            "angularVelocity": 0.15714670812054757,
+            "velocityX": -0.758393981584137,
+            "velocityY": 1.066114028544761,
+            "timestamp": 0.15699963529793862
+        },
+        {
+            "x": 7.501398636107905,
+            "y": 2.7080692662264956,
+            "heading": 0.037008343975294504,
+            "angularVelocity": 0.23572621790707007,
+            "velocityX": -1.1375909130963788,
+            "velocityY": 1.599170970201378,
+            "timestamp": 0.23549945294690794
+        },
+        {
+            "x": 7.382331074013197,
+            "y": 2.875448760857071,
+            "heading": 0.061681864177524894,
+            "angularVelocity": 0.31431308939549074,
+            "velocityX": -1.5167877539174253,
+            "velocityY": 2.132227814579811,
+            "timestamp": 0.31399927059587723
+        },
+        {
+            "x": 7.2334966444595095,
+            "y": 3.084673101412061,
+            "heading": 0.09252521384117814,
+            "angularVelocity": 0.39290982561993243,
+            "velocityX": -1.8959843985782956,
+            "velocityY": 2.665284414934403,
+            "timestamp": 0.39249908824484653
+        },
+        {
+            "x": 7.061360166565958,
+            "y": 3.326653173894572,
+            "heading": 0.09252521821441809,
+            "angularVelocity": 5.5710192458533506e-8,
+            "velocityX": -2.192826468251186,
+            "velocityY": 3.0825558546464755,
+            "timestamp": 0.4709989058938158
+        },
+        {
+            "x": 6.8697148114409385,
+            "y": 3.553495365981472,
+            "heading": 0.09252522268181232,
+            "angularVelocity": 5.690961282999965e-8,
+            "velocityX": -2.4413477746153216,
+            "velocityY": 2.8897161659824335,
+            "timestamp": 0.5494987235427852
+        },
+        {
+            "x": 6.64183630533757,
+            "y": 3.743907306900521,
+            "heading": 0.09252522772743905,
+            "angularVelocity": 6.427564912453075e-8,
+            "velocityX": -2.902917649087824,
+            "velocityY": 2.425635455237886,
+            "timestamp": 0.6279985411917545
+        },
+        {
+            "x": 6.384546476442235,
+            "y": 3.8921883279204876,
+            "heading": 0.09252523431940601,
+            "angularVelocity": 8.397429651316677e-8,
+            "velocityX": -3.277585051800601,
+            "velocityY": 1.8889345919635134,
+            "timestamp": 0.7064983588407238
+        },
+        {
+            "x": 6.105547891541977,
+            "y": 3.9938992218783795,
+            "heading": 0.09252527901816404,
+            "angularVelocity": 5.694122529822745e-7,
+            "velocityX": -3.5541303566827107,
+            "velocityY": 1.2956831876058104,
+            "timestamp": 0.7849981764896931
+        },
+        {
+            "x": 5.813193382307703,
+            "y": 4.045994945482373,
+            "heading": 0.09252629740574593,
+            "angularVelocity": 0.000012973120350997966,
+            "velocityX": -3.7242699153978625,
+            "velocityY": 0.6636413327346956,
+            "timestamp": 0.8634979941386624
+        },
+        {
+            "x": 5.523572440187452,
+            "y": 4.068080526231475,
+            "heading": 0.11166809511552328,
+            "angularVelocity": 0.24384512325081395,
+            "velocityX": -3.6894473235002847,
+            "velocityY": 0.28134563124547074,
+            "timestamp": 0.9419978117876316
+        },
+        {
+            "x": 5.248612904615211,
+            "y": 4.139233352038044,
+            "heading": 0.1457449942262438,
+            "angularVelocity": 0.43410163400767243,
+            "velocityX": -3.502677379478612,
+            "velocityY": 0.9064075298205869,
+            "timestamp": 1.020497629436601
         },
         {
             "x": 4.992805004119873,
             "y": 4.258058071136475,
-            "heading": -0.38392632852173914,
-            "angularVelocity": -0.7325625458589549,
-            "velocityX": -3.120564421314147,
-            "velocityY": 1.6812624222886623,
-            "timestamp": 1.0837029801746343
-        },
-        {
-            "x": 4.760679474369801,
-            "y": 4.451318301231298,
-            "heading": -0.43925940371465827,
-            "angularVelocity": -0.650567727765224,
-            "velocityX": -2.7291701738847216,
-            "velocityY": 2.272219071899485,
-            "timestamp": 1.1687565000948184
-        },
-        {
-            "x": 4.558064597648056,
-            "y": 4.695759350557987,
-            "heading": -0.45036550730749886,
-            "angularVelocity": -0.13057782444839394,
-            "velocityX": -2.3822044862091225,
-            "velocityY": 2.8739674684372165,
-            "timestamp": 1.2538100200150024
-        },
-        {
-            "x": 4.30999794561686,
-            "y": 4.900667947770545,
-            "heading": -0.4503653988140509,
-            "angularVelocity": 0.0000012755903345298267,
-            "velocityX": -2.9165947777821004,
-            "velocityY": 2.4091724528843765,
-            "timestamp": 1.3388635399351865
-        },
-        {
-            "x": 4.028045524182051,
-            "y": 5.05567579900462,
-            "heading": -0.4503653036134663,
-            "angularVelocity": 0.0000011193021141522574,
-            "velocityX": -3.3150000340890857,
-            "velocityY": 1.8224742653747859,
-            "timestamp": 1.4239170598553705
-        },
-        {
-            "x": 3.731575050469511,
-            "y": 5.180695241574213,
-            "heading": -0.45036521232637566,
-            "angularVelocity": 0.000001073289978617534,
-            "velocityX": -3.485693172848094,
-            "velocityY": 1.4698914599542185,
-            "timestamp": 1.5089705797755546
-        },
-        {
-            "x": 3.4348291016010033,
-            "y": 5.305059402921897,
-            "heading": -0.4503651210307298,
-            "angularVelocity": 0.000001073390565931622,
-            "velocityX": -3.488932017711167,
-            "velocityY": 1.4621871201127652,
-            "timestamp": 1.5940240996957387
-        },
-        {
-            "x": 3.120089238118937,
-            "y": 5.371868296261872,
-            "heading": -0.45036508370615375,
-            "angularVelocity": 4.3883634791364236e-7,
-            "velocityX": -3.7004919229372337,
-            "velocityY": 0.785492398232055,
-            "timestamp": 1.6790776196159227
-        },
-        {
-            "x": 2.8304866761911076,
-            "y": 5.435580552385651,
-            "heading": -0.37567464629819297,
-            "angularVelocity": 0.8781580994889028,
-            "velocityX": -3.404945053415936,
-            "velocityY": 0.7490842963759552,
-            "timestamp": 1.7641311395361068
-        },
-        {
-            "x": 2.5976382602327472,
-            "y": 5.485810295855226,
-            "heading": -0.2445738789443125,
-            "angularVelocity": 1.5413914377313083,
-            "velocityX": -2.7376693660283826,
-            "velocityY": 0.5905663106789283,
-            "timestamp": 1.8491846594562908
-        },
-        {
-            "x": 2.4235493783626687,
-            "y": 5.523078200691987,
-            "heading": -0.12850773623159037,
-            "angularVelocity": 1.3646248012033382,
-            "velocityX": -2.0468157230112984,
-            "velocityY": 0.43817004718549557,
-            "timestamp": 1.934238179376475
-        },
-        {
-            "x": 2.3077761259926057,
-            "y": 5.547737838884683,
-            "heading": -0.04437897835044122,
-            "angularVelocity": 0.9891272925576834,
-            "velocityX": -1.3611812007158846,
-            "velocityY": 0.2899308366756611,
-            "timestamp": 2.0192916992966587
+            "heading": 0.1827080793073441,
+            "angularVelocity": 0.47086841967443066,
+            "velocityX": -3.258706939158039,
+            "velocityY": 1.513694205377446,
+            "timestamp": 1.0989974470855703
+        },
+        {
+            "x": 4.782367562736006,
+            "y": 4.400850786547769,
+            "heading": 0.2087590252622848,
+            "angularVelocity": 0.37195010692359587,
+            "velocityX": -3.004582979783548,
+            "velocityY": 2.0387653429944033,
+            "timestamp": 1.1690362653840851
+        },
+        {
+            "x": 4.593366685154892,
+            "y": 4.57831882888555,
+            "heading": 0.22294244363545143,
+            "angularVelocity": 0.20250796226614393,
+            "velocityX": -2.6985160825467664,
+            "velocityY": 2.5338526070127387,
+            "timestamp": 1.2390750836825999
+        },
+        {
+            "x": 4.4303113780727985,
+            "y": 4.787155425256865,
+            "heading": 0.2229432862167668,
+            "angularVelocity": 0.000012030204616055241,
+            "velocityX": -2.3280705049466333,
+            "velocityY": 2.9817264403466797,
+            "timestamp": 1.3091139019811147
+        },
+        {
+            "x": 4.301341913399859,
+            "y": 5.018600438478609,
+            "heading": 0.22294334445620553,
+            "angularVelocity": 8.315308587772638e-7,
+            "velocityX": -1.8413997809506994,
+            "velocityY": 3.3045248170136206,
+            "timestamp": 1.3791527202796294
+        },
+        {
+            "x": 4.209527987601824,
+            "y": 5.267136397095996,
+            "heading": 0.22294334228224533,
+            "angularVelocity": -3.103936119065208e-8,
+            "velocityX": -1.3109005552544912,
+            "velocityY": 3.5485458586481258,
+            "timestamp": 1.4491915385781442
+        },
+        {
+            "x": 4.095163246631834,
+            "y": 5.506135613505712,
+            "heading": 0.22294334005402738,
+            "angularVelocity": -3.181404259759038e-8,
+            "velocityX": -1.6328765068917115,
+            "velocityY": 3.412382193414732,
+            "timestamp": 1.519230356876659
+        },
+        {
+            "x": 3.9453760475707735,
+            "y": 5.724684722787353,
+            "heading": 0.22294333755566106,
+            "angularVelocity": -3.567116580835512e-8,
+            "velocityX": -2.138631157692488,
+            "velocityY": 3.120399723908581,
+            "timestamp": 1.5892691751751737
+        },
+        {
+            "x": 3.7637358458155963,
+            "y": 5.917575181509908,
+            "heading": 0.22294327041118944,
+            "angularVelocity": -9.586751071941434e-7,
+            "velocityX": -2.5934218504529762,
+            "velocityY": 2.7540507308438293,
+            "timestamp": 1.6593079934736885
+        },
+        {
+            "x": 3.5705120272666107,
+            "y": 6.071441420591724,
+            "heading": 0.20870193370475335,
+            "angularVelocity": -0.20333490844666557,
+            "velocityX": -2.7588103746352264,
+            "velocityY": 2.1968708613263175,
+            "timestamp": 1.7293468117722033
+        },
+        {
+            "x": 3.3793495617108644,
+            "y": 6.184488312917552,
+            "heading": 0.18879422691132347,
+            "angularVelocity": -0.2842381878658059,
+            "velocityX": -2.7293787959290245,
+            "velocityY": 1.614060532031302,
+            "timestamp": 1.799385630070718
+        },
+        {
+            "x": 3.1945436254003248,
+            "y": 6.257137552443532,
+            "heading": 0.1660945073730067,
+            "angularVelocity": -0.3241019778712817,
+            "velocityX": -2.638621564444474,
+            "velocityY": 1.037271063258847,
+            "timestamp": 1.8694244483692328
+        },
+        {
+            "x": 3.018114205815821,
+            "y": 6.289753496686376,
+            "heading": 0.14197845255388317,
+            "angularVelocity": -0.34432412489220054,
+            "velocityX": -2.519023362625089,
+            "velocityY": 0.46568381699052885,
+            "timestamp": 1.9394632666677476
+        },
+        {
+            "x": 2.851226329803467,
+            "y": 6.282596111297607,
+            "heading": 0.11724722927389757,
+            "angularVelocity": -0.3531073750355408,
+            "velocityX": -2.382791144492779,
+            "velocityY": -0.10219169258892027,
+            "timestamp": 2.0095020849662624
+        },
+        {
+            "x": 2.661249908723212,
+            "y": 6.213695182206588,
+            "heading": 0.08663876117142615,
+            "angularVelocity": -0.3547239131111928,
+            "velocityX": -2.2016514926122546,
+            "velocityY": -0.7984982163199495,
+            "timestamp": 2.0957902289879784
+        },
+        {
+            "x": 2.4936356536291973,
+            "y": 6.0868793491721815,
+            "heading": 0.056963129639284946,
+            "angularVelocity": -0.3439131976774355,
+            "velocityX": -1.9424946149239757,
+            "velocityY": -1.4696785343117862,
+            "timestamp": 2.1820783730096944
+        },
+        {
+            "x": 2.3641635269275443,
+            "y": 5.911089762977232,
+            "heading": 0.030889400871559385,
+            "angularVelocity": -0.3021704669089439,
+            "velocityX": -1.500462527842606,
+            "velocityY": -2.037239161740341,
+            "timestamp": 2.2683665170314105
+        },
+        {
+            "x": 2.2965373597407854,
+            "y": 5.740054606624193,
+            "heading": 0.01408623201300084,
+            "angularVelocity": -0.19473322840668433,
+            "velocityX": -0.7837249016474405,
+            "velocityY": -1.9821397051956142,
+            "timestamp": 2.3546546610531265
+        },
+        {
+            "x": 2.263390196849459,
+            "y": 5.620608972310546,
+            "heading": 0.004408633441468959,
+            "angularVelocity": -0.11215444116055903,
+            "velocityX": -0.38414504410927525,
+            "velocityY": -1.384264729156902,
+            "timestamp": 2.4409428050748425
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 1.7803711242706045e-30,
-            "angularVelocity": 0.521777092730431,
-            "velocityX": -0.6792914161204248,
-            "velocityY": 0.14416994295838573,
-            "timestamp": 2.104345219216843
+            "heading": -1.7581112791577623e-31,
+            "angularVelocity": -0.05109199521503166,
+            "velocityX": -0.1551800308289656,
+            "velocityY": -0.7024020854510996,
+            "timestamp": 2.5272309490965585
         },
         {
             "x": 2.25,
             "y": 5.56,
             "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 1.3562125763475036e-31,
-            "velocityY": 2.290518308039125e-31,
-            "timestamp": 2.189398739137027
+            "angularVelocity": 5.961493131289069e-30,
+            "velocityX": -9.776555553264502e-32,
+            "velocityY": 0,
+            "timestamp": 2.6135190931182746
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W3.1.traj
new file mode 100644
index 00000000..15a1cd3f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W3.1.traj	
@@ -0,0 +1,274 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": -4.62162066936404e-33,
+            "velocityX": 3.0688151507515204e-31,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.623584376302727,
+            "y": 2.4297857949616017,
+            "heading": 0.033539307380936634,
+            "angularVelocity": 0.38483343876921716,
+            "velocityX": -0.6473186288895079,
+            "velocityY": -0.3122585684118786,
+            "timestamp": 0.08715278871867994
+        },
+        {
+            "x": 7.510615150776605,
+            "y": 2.375312721693156,
+            "heading": 0.0969954791736376,
+            "angularVelocity": 0.7281025968948721,
+            "velocityX": -1.2962204329545195,
+            "velocityY": -0.6250296068468768,
+            "timestamp": 0.1743055774373599
+        },
+        {
+            "x": 7.340880293995689,
+            "y": 2.2935273668964844,
+            "heading": 0.1838752961952568,
+            "angularVelocity": 0.9968678948651678,
+            "velocityX": -1.9475550843106406,
+            "velocityY": -0.9384135149210906,
+            "timestamp": 0.26145836615603985
+        },
+        {
+            "x": 7.114069036249808,
+            "y": 2.1844157133503646,
+            "heading": 0.27996244701307155,
+            "angularVelocity": 1.1025137833279668,
+            "velocityX": -2.6024555390648842,
+            "velocityY": -1.251958258023393,
+            "timestamp": 0.3486111548747198
+        },
+        {
+            "x": 6.831104073589339,
+            "y": 2.0490142604857513,
+            "heading": 0.32912237638568254,
+            "angularVelocity": 0.5640660510737534,
+            "velocityX": -3.246768885088134,
+            "velocityY": -1.5536101007814562,
+            "timestamp": 0.4357639435933997
+        },
+        {
+            "x": 6.534528280813305,
+            "y": 1.9049975621731787,
+            "heading": 0.3291224030694369,
+            "angularVelocity": 3.0617212321522066e-7,
+            "velocityX": -3.4029409401155197,
+            "velocityY": -1.6524623070575855,
+            "timestamp": 0.5229167323120797
+        },
+        {
+            "x": 6.237952262222522,
+            "y": 1.7609813288852085,
+            "heading": 0.3291224297529421,
+            "angularVelocity": 3.0616926449614015e-7,
+            "velocityX": -3.402943531137019,
+            "velocityY": -1.6524569713178037,
+            "timestamp": 0.6100695210307596
+        },
+        {
+            "x": 5.936787262454918,
+            "y": 1.6268259970502223,
+            "heading": 0.32912245667190637,
+            "angularVelocity": 3.0887094574020004e-7,
+            "velocityX": -3.4555979699024117,
+            "velocityY": -1.5393119808021931,
+            "timestamp": 0.6972223097494396
+        },
+        {
+            "x": 5.615527521313305,
+            "y": 1.5527301986214108,
+            "heading": 0.32912248658360005,
+            "angularVelocity": 3.4320982850713803e-7,
+            "velocityX": -3.6861670850098154,
+            "velocityY": -0.850182759704744,
+            "timestamp": 0.7843750984681195
+        },
+        {
+            "x": 5.286027461953728,
+            "y": 1.5414290964422628,
+            "heading": 0.32912252585289026,
+            "angularVelocity": 4.50579847209181e-7,
+            "velocityX": -3.780717337951944,
+            "velocityY": -0.12967000075725457,
+            "timestamp": 0.8715278871867994
+        },
+        {
+            "x": 4.960446284223111,
+            "y": 1.593339788205863,
+            "heading": 0.3291233220408656,
+            "angularVelocity": 0.000009135542155699871,
+            "velocityX": -3.7357516898462033,
+            "velocityY": 0.5956285797252272,
+            "timestamp": 0.9586806759054793
+        },
+        {
+            "x": 4.650797848549272,
+            "y": 1.7065466836899894,
+            "heading": 0.3291235959637348,
+            "angularVelocity": 0.0000031430189815062194,
+            "velocityX": -3.5529377800330906,
+            "velocityY": 1.2989474823295228,
+            "timestamp": 1.0458334646241594
+        },
+        {
+            "x": 4.368508202692925,
+            "y": 1.8768714467040708,
+            "heading": 0.32912381701495697,
+            "angularVelocity": 0.000002536364302217629,
+            "velocityX": -3.2390202310972316,
+            "velocityY": 1.9543237286860882,
+            "timestamp": 1.1329862533428394
+        },
+        {
+            "x": 4.092384496633659,
+            "y": 2.0570204638146983,
+            "heading": 0.3291240376025158,
+            "angularVelocity": 0.000002531044182467546,
+            "velocityX": -3.1682716080441704,
+            "velocityY": 2.067048223690569,
+            "timestamp": 1.2201390420615195
+        },
+        {
+            "x": 3.8162567615509033,
+            "y": 2.2371633052825928,
+            "heading": 0.32912425818941665,
+            "angularVelocity": 0.0000025310366317804206,
+            "velocityX": -3.1683178374712258,
+            "velocityY": 2.066977363735053,
+            "timestamp": 1.3072918307801995
+        },
+        {
+            "x": 3.558387727399027,
+            "y": 2.3518646644990793,
+            "heading": 0.32912440943088817,
+            "angularVelocity": 0.0000020272145178599712,
+            "velocityX": -3.4564319203160125,
+            "velocityY": 1.5374371746628512,
+            "timestamp": 1.3818973893726278
+        },
+        {
+            "x": 3.300516056227942,
+            "y": 2.466560095044955,
+            "heading": 0.3291245606718786,
+            "angularVelocity": 0.000002027208070545675,
+            "velocityX": -3.4564672664652627,
+            "velocityY": 1.5373577077876843,
+            "timestamp": 1.4565029479650562
+        },
+        {
+            "x": 3.052458337002912,
+            "y": 2.601171414843865,
+            "heading": 0.329124716605593,
+            "angularVelocity": 0.0000020901085294853097,
+            "velocityX": -3.3249227524744356,
+            "velocityY": 1.804306841723335,
+            "timestamp": 1.5311085065574845
+        },
+        {
+            "x": 2.829814908557799,
+            "y": 2.7746151638451657,
+            "heading": 0.3291249161701291,
+            "angularVelocity": 0.000002674928516490017,
+            "velocityX": -2.984273995740964,
+            "velocityY": 2.3248100044237634,
+            "timestamp": 1.6057140651499129
+        },
+        {
+            "x": 2.638606432152533,
+            "y": 2.982201542308141,
+            "heading": 0.3291253317978473,
+            "angularVelocity": 0.000005571002027267265,
+            "velocityX": -2.562925337103115,
+            "velocityY": 2.7824519027734036,
+            "timestamp": 1.6803196237423412
+        },
+        {
+            "x": 2.4840036736729245,
+            "y": 3.218316897029485,
+            "heading": 0.3291273340604081,
+            "angularVelocity": 0.000026837980957261232,
+            "velocityX": -2.0722686271167303,
+            "velocityY": 3.1648493647939433,
+            "timestamp": 1.7549251823347696
+        },
+        {
+            "x": 2.3733884856918945,
+            "y": 3.4659660107751304,
+            "heading": 0.2954274954004952,
+            "angularVelocity": -0.4517068070492687,
+            "velocityX": -1.4826668423638905,
+            "velocityY": 3.31944587532086,
+            "timestamp": 1.829530740927198
+        },
+        {
+            "x": 2.305023098579148,
+            "y": 3.7064756804968377,
+            "heading": 0.19598318894217118,
+            "angularVelocity": -1.3329342790875696,
+            "velocityX": -0.9163578210871337,
+            "velocityY": 3.2237500028063106,
+            "timestamp": 1.9041362995196263
+        },
+        {
+            "x": 2.272475779606133,
+            "y": 3.9200958593762563,
+            "heading": 0.053473340322528436,
+            "angularVelocity": -1.9101773555262378,
+            "velocityX": -0.43625863256144176,
+            "velocityY": 2.8633279196585013,
+            "timestamp": 1.9787418581120546
+        },
+        {
+            "x": 2.2677016800416907,
+            "y": 4.09662450199971,
+            "heading": -0.09830221598718161,
+            "angularVelocity": -2.034373298360552,
+            "velocityX": -0.06399120460344819,
+            "velocityY": 2.3661593848232187,
+            "timestamp": 2.053347416704483
+        },
+        {
+            "x": 2.2778239174156503,
+            "y": 4.229436050413757,
+            "heading": -0.23380831521279008,
+            "angularVelocity": -1.8163003103546322,
+            "velocityX": 0.13567671853055638,
+            "velocityY": 1.7801830174558415,
+            "timestamp": 2.1279529752969113
+        },
+        {
+            "x": 2.29107059750262,
+            "y": 4.316936633046431,
+            "heading": -0.33501872696397506,
+            "angularVelocity": -1.3566068488823915,
+            "velocityX": 0.1775562081015563,
+            "velocityY": 1.1728426712906341,
+            "timestamp": 2.2025585338893396
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.7369594715641588,
+            "velocityX": 0.11968816621507666,
+            "velocityY": 0.5772139203303202,
+            "timestamp": 2.277164092481768
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -8.092865962806098e-32,
+            "velocityX": 0,
+            "velocityY": 4.648171279746289e-33,
+            "timestamp": 2.3517696510741963
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.1.traj
new file mode 100644
index 00000000..8293ba75
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.1.traj	
@@ -0,0 +1,220 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -1.1520076473353784e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 0.8649552704871435,
+            "heading": -1.2072196888077206e-18,
+            "angularVelocity": -1.2485272283928241e-17,
+            "velocityX": -1.2865550388289577e-22,
+            "velocityY": 0.8062267267095138,
+            "timestamp": 0.09669149868710694
+        },
+        {
+            "x": 7.68,
+            "y": 1.0208658083668138,
+            "heading": -3.6224298175784965e-18,
+            "angularVelocity": -2.497851580797786e-17,
+            "velocityX": -3.7534290948711127e-22,
+            "velocityY": 1.6124534214139756,
+            "timestamp": 0.19338299737421388
+        },
+        {
+            "x": 7.68,
+            "y": 1.254731607449781,
+            "heading": -7.246204108405477e-18,
+            "angularVelocity": -3.7477692864160655e-17,
+            "velocityX": -8.581224837351263e-22,
+            "velocityY": 2.4186800521083573,
+            "timestamp": 0.29007449606132085
+        },
+        {
+            "x": 7.68,
+            "y": 1.5665526491683786,
+            "heading": -1.208276035533523e-17,
+            "angularVelocity": -5.0020491067317066e-17,
+            "velocityX": -2.0493771641846554e-21,
+            "velocityY": 3.2249064907727645,
+            "timestamp": 0.38676599474842777
+        },
+        {
+            "x": 7.68,
+            "y": 1.9323308421442915,
+            "heading": -4.9593132234185194e-18,
+            "angularVelocity": 7.367190734076976e-17,
+            "velocityX": 8.922575847291113e-21,
+            "velocityY": 3.782940568121389,
+            "timestamp": 0.4834574934355347
+        },
+        {
+            "x": 7.68,
+            "y": 2.298109035120243,
+            "heading": 4.533704301231546e-17,
+            "angularVelocity": 5.201735097557347e-16,
+            "velocityX": 4.097272417616201e-22,
+            "velocityY": 3.782940568121787,
+            "timestamp": 0.5801489921226416
+        },
+        {
+            "x": 7.68,
+            "y": 2.6638872280961943,
+            "heading": 2.5191507323029372e-17,
+            "angularVelocity": -2.083485721374208e-16,
+            "velocityX": 1.8682331175835897e-21,
+            "velocityY": 3.782940568121787,
+            "timestamp": 0.6768404908097485
+        },
+        {
+            "x": 7.68,
+            "y": 3.0296654210721456,
+            "heading": 5.2668856754382746e-17,
+            "angularVelocity": 2.8417544256148633e-16,
+            "velocityX": 2.5170076181953607e-21,
+            "velocityY": 3.782940568121787,
+            "timestamp": 0.7735319894968554
+        },
+        {
+            "x": 7.68,
+            "y": 3.395443614048097,
+            "heading": 4.929611386988728e-17,
+            "angularVelocity": -3.48814831737134e-17,
+            "velocityX": 3.0934195236040766e-21,
+            "velocityY": 3.782940568121787,
+            "timestamp": 0.8702234881839623
+        },
+        {
+            "x": 7.68,
+            "y": 3.7612218070240484,
+            "heading": 4.8005098046144365e-17,
+            "angularVelocity": -1.3351906227498234e-17,
+            "velocityX": -1.4483318353601114e-20,
+            "velocityY": 3.782940568121787,
+            "timestamp": 0.9669149868710692
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.8136822799879804e-17,
+            "angularVelocity": -1.0205938867695289e-16,
+            "velocityX": -1.0465805642283078e-22,
+            "velocityY": 3.782940568121787,
+            "timestamp": 1.0636064855581762
+        },
+        {
+            "x": 7.68,
+            "y": 4.492778192975952,
+            "heading": 4.310486593468924e-17,
+            "angularVelocity": 5.1380350929111975e-17,
+            "velocityX": 2.042774398860212e-21,
+            "velocityY": 3.782940568121787,
+            "timestamp": 1.1602979842452832
+        },
+        {
+            "x": 7.68,
+            "y": 4.8585563859519025,
+            "heading": 1.5339712420766394e-17,
+            "angularVelocity": -2.8715196155735124e-16,
+            "velocityX": 3.895366641644577e-22,
+            "velocityY": 3.782940568121787,
+            "timestamp": 1.2569894829323902
+        },
+        {
+            "x": 7.68,
+            "y": 5.224334578927854,
+            "heading": -2.710858238227145e-17,
+            "angularVelocity": -4.390075175101735e-16,
+            "velocityX": -3.863371094269421e-21,
+            "velocityY": 3.782940568121787,
+            "timestamp": 1.3536809816194972
+        },
+        {
+            "x": 7.68,
+            "y": 5.590112771903805,
+            "heading": -3.378098304971619e-17,
+            "angularVelocity": -6.900710774001486e-17,
+            "velocityX": -4.042329982282679e-22,
+            "velocityY": 3.782940568121787,
+            "timestamp": 1.4503724803066043
+        },
+        {
+            "x": 7.68,
+            "y": 5.955890964879757,
+            "heading": -3.941496183315801e-17,
+            "angularVelocity": -5.826757119208287e-17,
+            "velocityX": 2.6937052538433574e-21,
+            "velocityY": 3.782940568121787,
+            "timestamp": 1.5470639789937113
+        },
+        {
+            "x": 7.68,
+            "y": 6.321669157855708,
+            "heading": -1.9923808577496458e-17,
+            "angularVelocity": 2.0158083720198419e-16,
+            "velocityX": -4.499746264941967e-21,
+            "velocityY": 3.782940568121787,
+            "timestamp": 1.6437554776808183
+        },
+        {
+            "x": 7.68,
+            "y": 6.68744735083162,
+            "heading": 1.1959451428437676e-17,
+            "angularVelocity": 3.297421225035248e-16,
+            "velocityX": 5.431014750114918e-21,
+            "velocityY": 3.782940568121389,
+            "timestamp": 1.7404469763679253
+        },
+        {
+            "x": 7.68,
+            "y": 6.999268392550218,
+            "heading": 7.18685516064291e-18,
+            "angularVelocity": -4.935900605111643e-17,
+            "velocityX": -2.4046856363551417e-22,
+            "velocityY": 3.2249064907727645,
+            "timestamp": 1.8371384750550324
+        },
+        {
+            "x": 7.68,
+            "y": 7.2331341916331855,
+            "heading": 3.596188330325507e-18,
+            "angularVelocity": -3.713528985888002e-17,
+            "velocityX": -1.8035546521541034e-22,
+            "velocityY": 2.4186800521083573,
+            "timestamp": 1.9338299737421394
+        },
+        {
+            "x": 7.68,
+            "y": 7.3890447295128565,
+            "heading": 1.199348678010309e-18,
+            "angularVelocity": -2.4788525206448663e-17,
+            "velocityX": -1.2023814854884031e-22,
+            "velocityY": 1.6124534214139756,
+            "timestamp": 2.030521472429246
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": -1.2403868947555692e-17,
+            "velocityX": -6.011942579686287e-23,
+            "velocityY": 0.8062267267095138,
+            "timestamp": 2.127212971116353
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -2.719452927302828e-33,
+            "velocityY": 0,
+            "timestamp": 2.2239044698034602
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.1.traj
new file mode 100644
index 00000000..f16ed7e1
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.1.traj	
@@ -0,0 +1,175 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.5195168592304314e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 0.8688441539517631,
+            "heading": 1.4487899486085006e-18,
+            "angularVelocity": 1.4623322004407643e-17,
+            "velocityX": 2.341988908112997e-19,
+            "velocityY": 0.8260917454276263,
+            "timestamp": 0.09907392781100396
+        },
+        {
+            "x": 7.68,
+            "y": 1.03253245874193,
+            "heading": 4.3483011920968684e-18,
+            "angularVelocity": 2.926613800008521e-17,
+            "velocityX": 4.684469740721679e-19,
+            "velocityY": 1.6521834594306495,
+            "timestamp": 0.19814785562200793
+        },
+        {
+            "x": 7.68,
+            "y": 1.2780649081437847,
+            "heading": 8.701494230539972e-18,
+            "angularVelocity": 4.393883572228609e-17,
+            "velocityX": 7.027934476495029e-19,
+            "velocityY": 2.4782751105844794,
+            "timestamp": 0.29722178343301187
+        },
+        {
+            "x": 7.68,
+            "y": 1.6054414834771922,
+            "heading": 1.4516368843485863e-17,
+            "angularVelocity": 5.869227900187472e-17,
+            "velocityX": 9.37435089599571e-19,
+            "velocityY": 3.304366573190877,
+            "timestamp": 0.39629571124401586
+        },
+        {
+            "x": 7.68,
+            "y": 1.9802322649266864,
+            "heading": 6.647154079192576e-18,
+            "angularVelocity": -7.942770553424421e-17,
+            "velocityX": 1.0709424285811041e-18,
+            "velocityY": 3.782940575087071,
+            "timestamp": 0.49536963905501985
+        },
+        {
+            "x": 7.68,
+            "y": 2.3550230463762047,
+            "heading": -4.11606275908414e-17,
+            "angularVelocity": -4.825465460619937e-16,
+            "velocityX": 1.073083844244948e-18,
+            "velocityY": 3.7829405750873115,
+            "timestamp": 0.5944435668660238
+        },
+        {
+            "x": 7.68,
+            "y": 2.729813827825723,
+            "heading": -7.635066611173202e-17,
+            "angularVelocity": -3.5518969822232415e-16,
+            "velocityX": 1.0738946259920485e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.6935174946770278
+        },
+        {
+            "x": 7.68,
+            "y": 3.1046046092752415,
+            "heading": -5.864574302033697e-17,
+            "angularVelocity": 1.787041604453991e-16,
+            "velocityX": 1.0691415340399175e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.7925914224880318
+        },
+        {
+            "x": 7.68,
+            "y": 3.4793953907247595,
+            "heading": -2.6391173603704442e-17,
+            "angularVelocity": 3.255606205313886e-16,
+            "velocityX": 1.0732289061667476e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.8916653502990358
+        },
+        {
+            "x": 7.68,
+            "y": 3.854186172174278,
+            "heading": -6.511485011951871e-17,
+            "angularVelocity": -3.9085637736788184e-16,
+            "velocityX": 1.0716697150075795e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 0.9907392781100398
+        },
+        {
+            "x": 7.68,
+            "y": 4.228976953623796,
+            "heading": -6.241329479526944e-17,
+            "angularVelocity": 2.7268075304360027e-17,
+            "velocityX": 1.0686111075844087e-18,
+            "velocityY": 3.782940575087312,
+            "timestamp": 1.0898132059210437
+        },
+        {
+            "x": 7.68,
+            "y": 4.603767735073314,
+            "heading": -2.2737833863559818e-17,
+            "angularVelocity": 4.004631875225205e-16,
+            "velocityX": 1.0710197916722359e-18,
+            "velocityY": 3.7829405750873115,
+            "timestamp": 1.1888871337320477
+        },
+        {
+            "x": 7.68,
+            "y": 4.978558516522808,
+            "heading": -1.448944579275074e-17,
+            "angularVelocity": 8.325488100707829e-17,
+            "velocityX": 1.0714504382145178e-18,
+            "velocityY": 3.782940575087071,
+            "timestamp": 1.2879610615430517
+        },
+        {
+            "x": 7.68,
+            "y": 5.305935091856216,
+            "heading": -8.687037170545135e-18,
+            "angularVelocity": 5.856645386228716e-17,
+            "velocityX": 9.36753995288457e-19,
+            "velocityY": 3.304366573190877,
+            "timestamp": 1.3870349893540557
+        },
+        {
+            "x": 7.68,
+            "y": 5.55146754125807,
+            "heading": -4.341905072762387e-18,
+            "angularVelocity": 4.385747283685971e-17,
+            "velocityX": 7.025655481927048e-19,
+            "velocityY": 2.4782751105844794,
+            "timestamp": 1.4861089171650597
+        },
+        {
+            "x": 7.68,
+            "y": 5.715155846048237,
+            "heading": -1.4469357825353783e-18,
+            "angularVelocity": 2.9220293918225814e-17,
+            "velocityX": 4.683770471216647e-19,
+            "velocityY": 1.6521834594306495,
+            "timestamp": 1.5851828449760637
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 1.460460702963924e-17,
+            "velocityX": 2.341885280588165e-19,
+            "velocityY": 0.8260917454276263,
+            "timestamp": 1.6842567727870676
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 3.1812192470207854e-34,
+            "velocityY": 0,
+            "timestamp": 1.7833307005980716
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C3.1.traj
new file mode 100644
index 00000000..864eb2e4
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C3.1.traj	
@@ -0,0 +1,139 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 2.417290808823166e-40,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 0.8634453720055388,
+            "heading": 1.009810999063182e-18,
+            "angularVelocity": 1.0546271494620871e-17,
+            "velocityX": -3.226662245674165e-34,
+            "velocityY": 0.7983807350946677,
+            "timestamp": 0.09575052183150978
+        },
+        {
+            "x": 7.68,
+            "y": 1.0163361129826893,
+            "heading": 3.0323418642587253e-18,
+            "angularVelocity": 2.112292263585315e-17,
+            "velocityX": -6.037770263383471e-34,
+            "velocityY": 1.596761438503583,
+            "timestamp": 0.19150104366301957
+        },
+        {
+            "x": 7.68,
+            "y": 1.2456722168635992,
+            "heading": 6.073900773319643e-18,
+            "angularVelocity": 3.1765455168164525e-17,
+            "velocityX": -4.821612934727609e-34,
+            "velocityY": 2.3951420785410233,
+            "timestamp": 0.28725156549452935
+        },
+        {
+            "x": 7.68,
+            "y": 1.5514536654447455,
+            "heading": 1.0152788689831222e-17,
+            "angularVelocity": 4.2599119433487623e-17,
+            "velocityX": -1.2140491970354773e-33,
+            "velocityY": 3.19352252846437,
+            "timestamp": 0.38300208732603913
+        },
+        {
+            "x": 7.68,
+            "y": 1.9136721992668209,
+            "heading": 4.411917253468209e-18,
+            "angularVelocity": -5.995655508243724e-17,
+            "velocityX": 4.90795348832912e-33,
+            "velocityY": 3.7829405719528495,
+            "timestamp": 0.4787526091575489
+        },
+        {
+            "x": 7.68,
+            "y": 2.27589073308894,
+            "heading": 1.2796107303848674e-17,
+            "angularVelocity": 8.756286535058141e-17,
+            "velocityX": -1.5435756690671252e-33,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.5745031309890587
+        },
+        {
+            "x": 7.68,
+            "y": 2.63810926691106,
+            "heading": 6.994895399550348e-18,
+            "angularVelocity": -6.058673930264937e-17,
+            "velocityX": 4.776897057793067e-34,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.6702536528205685
+        },
+        {
+            "x": 7.68,
+            "y": 3.0003278007331793,
+            "heading": -9.534020306729642e-18,
+            "angularVelocity": -1.7262481070720064e-16,
+            "velocityX": 2.9275045647973183e-33,
+            "velocityY": 3.782940571953311,
+            "timestamp": 0.7660041746520783
+        },
+        {
+            "x": 7.68,
+            "y": 3.3625463345552546,
+            "heading": -1.0013819273727212e-17,
+            "angularVelocity": -5.010927959660929e-18,
+            "velocityX": 2.237784317663532e-33,
+            "velocityY": 3.7829405719528495,
+            "timestamp": 0.861754696483588
+        },
+        {
+            "x": 7.68,
+            "y": 3.6683277831364007,
+            "heading": -6.0064636534747415e-18,
+            "angularVelocity": 4.185204993640188e-17,
+            "velocityX": -2.2313398142102424e-33,
+            "velocityY": 3.19352252846437,
+            "timestamp": 0.9575052183150978
+        },
+        {
+            "x": 7.68,
+            "y": 3.897663887017311,
+            "heading": -3.002975312877452e-18,
+            "angularVelocity": 3.136785349810471e-17,
+            "velocityX": -2.8833027864886262e-33,
+            "velocityY": 2.3951420785410233,
+            "timestamp": 1.0532557401466076
+        },
+        {
+            "x": 7.68,
+            "y": 4.050554627994461,
+            "heading": -1.0009557447862449e-18,
+            "angularVelocity": 2.0908706606963636e-17,
+            "velocityX": -3.742421273800505e-33,
+            "velocityY": 1.596761438503583,
+            "timestamp": 1.1490062619781174
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 1.0453788927770355e-17,
+            "velocityX": -1.5032681111068507e-33,
+            "velocityY": 0.7983807350946677,
+            "timestamp": 1.2447567838096272
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 9.665729080515376e-43,
+            "velocityY": 2.80463986608038e-38,
+            "timestamp": 1.340507305641137
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.1.traj
new file mode 100644
index 00000000..17170fc9
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.1.traj	
@@ -0,0 +1,94 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 9.069206296242399e-40,
+            "angularVelocity": -9.927610039652183e-40,
+            "velocityX": -8.253947176298157e-40,
+            "velocityY": 1.8367099231598242e-40,
+            "timestamp": 0
+        },
+        {
+            "x": 7.68,
+            "y": 0.8705000055758465,
+            "heading": 6.490773345690525e-22,
+            "angularVelocity": 6.486159683422905e-21,
+            "velocityX": -7.132918469580153e-33,
+            "velocityY": 0.8344065481228213,
+            "timestamp": 0.10007112931184182
+        },
+        {
+            "x": 7.68,
+            "y": 1.037500013629847,
+            "heading": 2.023593010760001e-21,
+            "angularVelocity": 1.3735386658326633e-20,
+            "velocityX": -1.5648094960655976e-32,
+            "velocityY": 1.668813065290736,
+            "timestamp": 0.20014225862368365
+        },
+        {
+            "x": 7.68,
+            "y": 1.2880000179666162,
+            "heading": 3.787875676222919e-21,
+            "angularVelocity": 1.7630286060953193e-20,
+            "velocityX": -2.3089760494009302e-32,
+            "velocityY": 2.503219520548835,
+            "timestamp": 0.3002133879355255
+        },
+        {
+            "x": 7.68,
+            "y": 1.6219999999999999,
+            "heading": 5.860880748165694e-21,
+            "angularVelocity": 2.0715315514205814e-20,
+            "velocityX": -3.0704903308303665e-32,
+            "velocityY": 3.3376257900775013,
+            "timestamp": 0.4002845172473673
+        },
+        {
+            "x": 7.68,
+            "y": 1.9559999820333838,
+            "heading": 3.934658943161884e-21,
+            "angularVelocity": -1.9248526176341983e-20,
+            "velocityX": -4.040562356218352e-32,
+            "velocityY": 3.3376257900775017,
+            "timestamp": 0.5003556465592092
+        },
+        {
+            "x": 7.68,
+            "y": 2.206499986370153,
+            "heading": 1.6176834885162177e-21,
+            "angularVelocity": -2.3153285409554772e-20,
+            "velocityX": -3.08903871620124e-32,
+            "velocityY": 2.503219520548835,
+            "timestamp": 0.600426775871051
+        },
+        {
+            "x": 7.68,
+            "y": 2.3734999944241535,
+            "heading": 4.992981261257204e-22,
+            "angularVelocity": -1.1175904096118944e-20,
+            "velocityX": -2.3331216062245415e-32,
+            "velocityY": 1.668813065290736,
+            "timestamp": 0.7004979051828928
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 1.7562757360140444e-40,
+            "angularVelocity": -4.9894322348928345e-21,
+            "velocityX": -1.3647931662662733e-32,
+            "velocityY": 0.8344065481228213,
+            "timestamp": 0.8005690344947346
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -7.274275970183695e-41,
+            "angularVelocity": -2.4819026120794767e-39,
+            "velocityX": -8.637198685981915e-42,
+            "velocityY": 5.142787784847508e-39,
+            "timestamp": 0.9006401638065764
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S1.1.traj
new file mode 100644
index 00000000..73f0ef42
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S1.1.traj	
@@ -0,0 +1,292 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 4.273608378959021e-31,
+            "velocityX": -7.420825794722196e-33,
+            "velocityY": 7.583788429881001e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 7.678491547493202,
+            "y": 0.8436471873828707,
+            "heading": -0.006289831251442517,
+            "angularVelocity": -0.0762785379812196,
+            "velocityX": -0.018293424295956956,
+            "velocityY": 0.686976241742982,
+            "timestamp": 0.08245872846948342
+        },
+        {
+            "x": 7.67547464227306,
+            "y": 0.9569415605229419,
+            "heading": -0.018869443032064528,
+            "angularVelocity": -0.1525564608394078,
+            "velocityX": -0.03658685109677269,
+            "velocityY": 1.3739524637710059,
+            "timestamp": 0.16491745693896684
+        },
+        {
+            "x": 7.670949285584863,
+            "y": 1.1268831170081794,
+            "heading": -0.03773871517098893,
+            "angularVelocity": -0.22883292635185057,
+            "velocityX": -0.05488026279561822,
+            "velocityY": 2.060928656547622,
+            "timestamp": 0.24737618540845024
+        },
+        {
+            "x": 7.664915480002808,
+            "y": 1.3534718520373763,
+            "heading": -0.06289740953278049,
+            "angularVelocity": -0.3051065039294483,
+            "velocityX": -0.07317364327643945,
+            "velocityY": 2.7479047910986583,
+            "timestamp": 0.3298349138779337
+        },
+        {
+            "x": 7.657373229393316,
+            "y": 1.6367077493690674,
+            "heading": -0.09434508673453766,
+            "angularVelocity": -0.38137475298804396,
+            "velocityX": -0.09146697686811679,
+            "velocityY": 3.4348807286849197,
+            "timestamp": 0.4122936423474171
+        },
+        {
+            "x": 7.649069464629009,
+            "y": 1.9485336685806538,
+            "heading": -0.09434509076634809,
+            "angularVelocity": -4.889488962341613e-8,
+            "velocityX": -0.10070207142933957,
+            "velocityY": 3.781599898511507,
+            "timestamp": 0.49475237081690054
+        },
+        {
+            "x": 7.6407656424773585,
+            "y": 2.260359586264075,
+            "heading": -0.09434509479812381,
+            "angularVelocity": -4.889446872332144e-8,
+            "velocityX": -0.1007027673816724,
+            "velocityY": 3.781599879979021,
+            "timestamp": 0.5772110992863839
+        },
+        {
+            "x": 7.63246175736497,
+            "y": 2.5721855022708704,
+            "heading": -0.09434509882989094,
+            "angularVelocity": -4.889436444733497e-8,
+            "velocityX": -0.10070353092409444,
+            "velocityY": 3.7815998596461093,
+            "timestamp": 0.6596698277558674
+        },
+        {
+            "x": 7.6241578045918486,
+            "y": 2.8840114164758646,
+            "heading": -0.09434510286165439,
+            "angularVelocity": -4.8894319896339695e-8,
+            "velocityX": -0.10070435146468282,
+            "velocityY": 3.781599837795173,
+            "timestamp": 0.7421285562253508
+        },
+        {
+            "x": 7.615853780314312,
+            "y": 3.195837328776685,
+            "heading": -0.09434510689341911,
+            "angularVelocity": -4.889433549543518e-8,
+            "velocityX": -0.10070521861867329,
+            "velocityY": 3.78159981470272,
+            "timestamp": 0.8245872846948342
+        },
+        {
+            "x": 7.607549681647345,
+            "y": 3.5076632390964844,
+            "heading": -0.09434511092518724,
+            "angularVelocity": -4.889437661104965e-8,
+            "velocityX": -0.10070612076003535,
+            "velocityY": 3.7815997906783263,
+            "timestamp": 0.9070460131643177
+        },
+        {
+            "x": 7.599245507241111,
+            "y": 3.819489147399297,
+            "heading": -0.0943451149569584,
+            "angularVelocity": -4.889441348359375e-8,
+            "velocityX": -0.10070703927126574,
+            "velocityY": 3.7815997662177643,
+            "timestamp": 0.9895047416338011
+        },
+        {
+            "x": 7.590941256893013,
+            "y": 4.131315053679708,
+            "heading": -0.09434511898873191,
+            "angularVelocity": -4.88944418703562e-8,
+            "velocityX": -0.10070796023943433,
+            "velocityY": 3.781599741691539,
+            "timestamp": 1.0719634701032845
+        },
+        {
+            "x": 7.582636930618974,
+            "y": 4.443140957938123,
+            "heading": -0.09434512302050634,
+            "angularVelocity": -4.889445311998471e-8,
+            "velocityX": -0.10070888101448454,
+            "velocityY": 3.781599717170225,
+            "timestamp": 1.1544221985727678
+        },
+        {
+            "x": 7.574332505518219,
+            "y": 4.754966859564639,
+            "heading": -0.09434512705227821,
+            "angularVelocity": -4.889442208864286e-8,
+            "velocityX": -0.10071007951363715,
+            "velocityY": 3.78159968525245,
+            "timestamp": 1.2368809270422512
+        },
+        {
+            "x": 7.562081052721255,
+            "y": 5.066662637971005,
+            "heading": -0.09434513109398723,
+            "angularVelocity": -4.901493268640619e-8,
+            "velocityX": -0.1485767853126217,
+            "velocityY": 3.7800216446669976,
+            "timestamp": 1.3193396555117345
+        },
+        {
+            "x": 7.493618412578439,
+            "y": 5.370993415792126,
+            "heading": -0.09434513545729191,
+            "angularVelocity": -5.29150129274595e-8,
+            "velocityX": -0.8302655329951328,
+            "velocityY": 3.6907042282825056,
+            "timestamp": 1.4017983839812178
+        },
+        {
+            "x": 7.37120325739307,
+            "y": 5.657906089273735,
+            "heading": -0.09434514092208332,
+            "angularVelocity": -6.627304961707395e-8,
+            "velocityX": -1.4845627316539582,
+            "velocityY": 3.479470018601967,
+            "timestamp": 1.4842571124507011
+        },
+        {
+            "x": 7.198879241943359,
+            "y": 5.9179229736328125,
+            "heading": -0.09434514894422652,
+            "angularVelocity": -9.728676819194244e-8,
+            "velocityX": -2.089821400938593,
+            "velocityY": 3.153297281988846,
+            "timestamp": 1.5667158409201845
+        },
+        {
+            "x": 7.016660952404083,
+            "y": 6.116497130944734,
+            "heading": -0.09434515627038953,
+            "angularVelocity": -1.028329950777357e-7,
+            "velocityX": -2.5576898068579945,
+            "velocityY": 2.7872673997010713,
+            "timestamp": 1.6379591516026175
+        },
+        {
+            "x": 6.805603401067166,
+            "y": 6.284097595272953,
+            "heading": -0.09434516139165254,
+            "angularVelocity": -7.188412437441302e-8,
+            "velocityX": -2.9624893806199624,
+            "velocityY": 2.352508084236811,
+            "timestamp": 1.7092024622850506
+        },
+        {
+            "x": 6.57091097390028,
+            "y": 6.416591642615984,
+            "heading": -0.09434516555011045,
+            "angularVelocity": -5.83698017255904e-8,
+            "velocityX": -3.2942380824078485,
+            "velocityY": 1.8597401787463752,
+            "timestamp": 1.7804457729674836
+        },
+        {
+            "x": 6.318370883291981,
+            "y": 6.510712371224652,
+            "heading": -0.09434516933867851,
+            "angularVelocity": -5.317787750894471e-8,
+            "velocityX": -3.544755124224867,
+            "velocityY": 1.3211167154795889,
+            "timestamp": 1.8516890836499167
+        },
+        {
+            "x": 6.054210380502568,
+            "y": 6.5641393107028225,
+            "heading": -0.09434515209433943,
+            "angularVelocity": 2.420485363909629e-7,
+            "velocityX": -3.7078639420184567,
+            "velocityY": 0.7499221887135695,
+            "timestamp": 1.9229323943323497
+        },
+        {
+            "x": 5.803536575930751,
+            "y": 6.585943895129495,
+            "heading": -0.03682720962895,
+            "angularVelocity": 0.8073451656643992,
+            "velocityX": -3.5185591765828246,
+            "velocityY": 0.30605798941413415,
+            "timestamp": 1.9941757050147828
+        },
+        {
+            "x": 5.594045383796189,
+            "y": 6.601646508747958,
+            "heading": 0.04817170409455108,
+            "angularVelocity": 1.1930792225867084,
+            "velocityX": -2.9405033276509123,
+            "velocityY": 0.2204082526211729,
+            "timestamp": 2.065419015697216
+        },
+        {
+            "x": 5.426658878791764,
+            "y": 6.613398993876379,
+            "heading": 0.12734083591800166,
+            "angularVelocity": 1.111250039689289,
+            "velocityX": -2.3495048644012306,
+            "velocityY": 0.16496264724146908,
+            "timestamp": 2.136662326379649
+        },
+        {
+            "x": 5.301258489415666,
+            "y": 6.621853943751229,
+            "heading": 0.1915936210653456,
+            "angularVelocity": 0.9018781487254319,
+            "velocityX": -1.7601707188351954,
+            "velocityY": 0.11867710517466466,
+            "timestamp": 2.207905637062082
+        },
+        {
+            "x": 5.217732575552286,
+            "y": 6.627325619944874,
+            "heading": 0.2366430637253094,
+            "angularVelocity": 0.6323322460514409,
+            "velocityX": -1.1724035992052257,
+            "velocityY": 0.07680266597989953,
+            "timestamp": 2.279148947744515
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": 0.32784742947732926,
+            "velocityX": -0.5857753542407405,
+            "velocityY": 0.037538682993647744,
+            "timestamp": 2.350392258426948
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": -8.68876182413906e-32,
+            "velocityX": 0,
+            "velocityY": -2.1409750340846636e-32,
+            "timestamp": 2.421635569109381
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S1.traj
new file mode 100644
index 00000000..73f0ef42
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S1.traj	
@@ -0,0 +1,292 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 4.273608378959021e-31,
+            "velocityX": -7.420825794722196e-33,
+            "velocityY": 7.583788429881001e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 7.678491547493202,
+            "y": 0.8436471873828707,
+            "heading": -0.006289831251442517,
+            "angularVelocity": -0.0762785379812196,
+            "velocityX": -0.018293424295956956,
+            "velocityY": 0.686976241742982,
+            "timestamp": 0.08245872846948342
+        },
+        {
+            "x": 7.67547464227306,
+            "y": 0.9569415605229419,
+            "heading": -0.018869443032064528,
+            "angularVelocity": -0.1525564608394078,
+            "velocityX": -0.03658685109677269,
+            "velocityY": 1.3739524637710059,
+            "timestamp": 0.16491745693896684
+        },
+        {
+            "x": 7.670949285584863,
+            "y": 1.1268831170081794,
+            "heading": -0.03773871517098893,
+            "angularVelocity": -0.22883292635185057,
+            "velocityX": -0.05488026279561822,
+            "velocityY": 2.060928656547622,
+            "timestamp": 0.24737618540845024
+        },
+        {
+            "x": 7.664915480002808,
+            "y": 1.3534718520373763,
+            "heading": -0.06289740953278049,
+            "angularVelocity": -0.3051065039294483,
+            "velocityX": -0.07317364327643945,
+            "velocityY": 2.7479047910986583,
+            "timestamp": 0.3298349138779337
+        },
+        {
+            "x": 7.657373229393316,
+            "y": 1.6367077493690674,
+            "heading": -0.09434508673453766,
+            "angularVelocity": -0.38137475298804396,
+            "velocityX": -0.09146697686811679,
+            "velocityY": 3.4348807286849197,
+            "timestamp": 0.4122936423474171
+        },
+        {
+            "x": 7.649069464629009,
+            "y": 1.9485336685806538,
+            "heading": -0.09434509076634809,
+            "angularVelocity": -4.889488962341613e-8,
+            "velocityX": -0.10070207142933957,
+            "velocityY": 3.781599898511507,
+            "timestamp": 0.49475237081690054
+        },
+        {
+            "x": 7.6407656424773585,
+            "y": 2.260359586264075,
+            "heading": -0.09434509479812381,
+            "angularVelocity": -4.889446872332144e-8,
+            "velocityX": -0.1007027673816724,
+            "velocityY": 3.781599879979021,
+            "timestamp": 0.5772110992863839
+        },
+        {
+            "x": 7.63246175736497,
+            "y": 2.5721855022708704,
+            "heading": -0.09434509882989094,
+            "angularVelocity": -4.889436444733497e-8,
+            "velocityX": -0.10070353092409444,
+            "velocityY": 3.7815998596461093,
+            "timestamp": 0.6596698277558674
+        },
+        {
+            "x": 7.6241578045918486,
+            "y": 2.8840114164758646,
+            "heading": -0.09434510286165439,
+            "angularVelocity": -4.8894319896339695e-8,
+            "velocityX": -0.10070435146468282,
+            "velocityY": 3.781599837795173,
+            "timestamp": 0.7421285562253508
+        },
+        {
+            "x": 7.615853780314312,
+            "y": 3.195837328776685,
+            "heading": -0.09434510689341911,
+            "angularVelocity": -4.889433549543518e-8,
+            "velocityX": -0.10070521861867329,
+            "velocityY": 3.78159981470272,
+            "timestamp": 0.8245872846948342
+        },
+        {
+            "x": 7.607549681647345,
+            "y": 3.5076632390964844,
+            "heading": -0.09434511092518724,
+            "angularVelocity": -4.889437661104965e-8,
+            "velocityX": -0.10070612076003535,
+            "velocityY": 3.7815997906783263,
+            "timestamp": 0.9070460131643177
+        },
+        {
+            "x": 7.599245507241111,
+            "y": 3.819489147399297,
+            "heading": -0.0943451149569584,
+            "angularVelocity": -4.889441348359375e-8,
+            "velocityX": -0.10070703927126574,
+            "velocityY": 3.7815997662177643,
+            "timestamp": 0.9895047416338011
+        },
+        {
+            "x": 7.590941256893013,
+            "y": 4.131315053679708,
+            "heading": -0.09434511898873191,
+            "angularVelocity": -4.88944418703562e-8,
+            "velocityX": -0.10070796023943433,
+            "velocityY": 3.781599741691539,
+            "timestamp": 1.0719634701032845
+        },
+        {
+            "x": 7.582636930618974,
+            "y": 4.443140957938123,
+            "heading": -0.09434512302050634,
+            "angularVelocity": -4.889445311998471e-8,
+            "velocityX": -0.10070888101448454,
+            "velocityY": 3.781599717170225,
+            "timestamp": 1.1544221985727678
+        },
+        {
+            "x": 7.574332505518219,
+            "y": 4.754966859564639,
+            "heading": -0.09434512705227821,
+            "angularVelocity": -4.889442208864286e-8,
+            "velocityX": -0.10071007951363715,
+            "velocityY": 3.78159968525245,
+            "timestamp": 1.2368809270422512
+        },
+        {
+            "x": 7.562081052721255,
+            "y": 5.066662637971005,
+            "heading": -0.09434513109398723,
+            "angularVelocity": -4.901493268640619e-8,
+            "velocityX": -0.1485767853126217,
+            "velocityY": 3.7800216446669976,
+            "timestamp": 1.3193396555117345
+        },
+        {
+            "x": 7.493618412578439,
+            "y": 5.370993415792126,
+            "heading": -0.09434513545729191,
+            "angularVelocity": -5.29150129274595e-8,
+            "velocityX": -0.8302655329951328,
+            "velocityY": 3.6907042282825056,
+            "timestamp": 1.4017983839812178
+        },
+        {
+            "x": 7.37120325739307,
+            "y": 5.657906089273735,
+            "heading": -0.09434514092208332,
+            "angularVelocity": -6.627304961707395e-8,
+            "velocityX": -1.4845627316539582,
+            "velocityY": 3.479470018601967,
+            "timestamp": 1.4842571124507011
+        },
+        {
+            "x": 7.198879241943359,
+            "y": 5.9179229736328125,
+            "heading": -0.09434514894422652,
+            "angularVelocity": -9.728676819194244e-8,
+            "velocityX": -2.089821400938593,
+            "velocityY": 3.153297281988846,
+            "timestamp": 1.5667158409201845
+        },
+        {
+            "x": 7.016660952404083,
+            "y": 6.116497130944734,
+            "heading": -0.09434515627038953,
+            "angularVelocity": -1.028329950777357e-7,
+            "velocityX": -2.5576898068579945,
+            "velocityY": 2.7872673997010713,
+            "timestamp": 1.6379591516026175
+        },
+        {
+            "x": 6.805603401067166,
+            "y": 6.284097595272953,
+            "heading": -0.09434516139165254,
+            "angularVelocity": -7.188412437441302e-8,
+            "velocityX": -2.9624893806199624,
+            "velocityY": 2.352508084236811,
+            "timestamp": 1.7092024622850506
+        },
+        {
+            "x": 6.57091097390028,
+            "y": 6.416591642615984,
+            "heading": -0.09434516555011045,
+            "angularVelocity": -5.83698017255904e-8,
+            "velocityX": -3.2942380824078485,
+            "velocityY": 1.8597401787463752,
+            "timestamp": 1.7804457729674836
+        },
+        {
+            "x": 6.318370883291981,
+            "y": 6.510712371224652,
+            "heading": -0.09434516933867851,
+            "angularVelocity": -5.317787750894471e-8,
+            "velocityX": -3.544755124224867,
+            "velocityY": 1.3211167154795889,
+            "timestamp": 1.8516890836499167
+        },
+        {
+            "x": 6.054210380502568,
+            "y": 6.5641393107028225,
+            "heading": -0.09434515209433943,
+            "angularVelocity": 2.420485363909629e-7,
+            "velocityX": -3.7078639420184567,
+            "velocityY": 0.7499221887135695,
+            "timestamp": 1.9229323943323497
+        },
+        {
+            "x": 5.803536575930751,
+            "y": 6.585943895129495,
+            "heading": -0.03682720962895,
+            "angularVelocity": 0.8073451656643992,
+            "velocityX": -3.5185591765828246,
+            "velocityY": 0.30605798941413415,
+            "timestamp": 1.9941757050147828
+        },
+        {
+            "x": 5.594045383796189,
+            "y": 6.601646508747958,
+            "heading": 0.04817170409455108,
+            "angularVelocity": 1.1930792225867084,
+            "velocityX": -2.9405033276509123,
+            "velocityY": 0.2204082526211729,
+            "timestamp": 2.065419015697216
+        },
+        {
+            "x": 5.426658878791764,
+            "y": 6.613398993876379,
+            "heading": 0.12734083591800166,
+            "angularVelocity": 1.111250039689289,
+            "velocityX": -2.3495048644012306,
+            "velocityY": 0.16496264724146908,
+            "timestamp": 2.136662326379649
+        },
+        {
+            "x": 5.301258489415666,
+            "y": 6.621853943751229,
+            "heading": 0.1915936210653456,
+            "angularVelocity": 0.9018781487254319,
+            "velocityX": -1.7601707188351954,
+            "velocityY": 0.11867710517466466,
+            "timestamp": 2.207905637062082
+        },
+        {
+            "x": 5.217732575552286,
+            "y": 6.627325619944874,
+            "heading": 0.2366430637253094,
+            "angularVelocity": 0.6323322460514409,
+            "velocityX": -1.1724035992052257,
+            "velocityY": 0.07680266597989953,
+            "timestamp": 2.279148947744515
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": 0.32784742947732926,
+            "velocityX": -0.5857753542407405,
+            "velocityY": 0.037538682993647744,
+            "timestamp": 2.350392258426948
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": -8.68876182413906e-32,
+            "velocityX": 0,
+            "velocityY": -2.1409750340846636e-32,
+            "timestamp": 2.421635569109381
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S2.1.traj
new file mode 100644
index 00000000..dfbc040a
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S2.1.traj	
@@ -0,0 +1,247 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": -1.3156383032943886e-37,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.664225467591612,
+            "y": 0.8361290574305452,
+            "heading": -0.011405957532827086,
+            "angularVelocity": -0.14485338302751044,
+            "velocityX": -0.20033341159264173,
+            "velocityY": 0.6239292188565086,
+            "timestamp": 0.07874139557141634
+        },
+        {
+            "x": 7.632676408078482,
+            "y": 0.9343872155736361,
+            "heading": -0.03421427612768491,
+            "angularVelocity": -0.2896610915940815,
+            "velocityX": -0.4006667558300489,
+            "velocityY": 1.2478589873857826,
+            "timestamp": 0.15748279114283267
+        },
+        {
+            "x": 7.585352811707443,
+            "y": 1.0817745497576996,
+            "heading": -0.06841862032572539,
+            "angularVelocity": -0.4343883410984004,
+            "velocityX": -0.6010002239307173,
+            "velocityY": 1.8717897125710332,
+            "timestamp": 0.236224186714249
+        },
+        {
+            "x": 7.522254637419436,
+            "y": 1.278291160108196,
+            "heading": -0.11400992700252059,
+            "angularVelocity": -0.5790004907322871,
+            "velocityX": -0.8013342134732577,
+            "velocityY": 2.4957217093296378,
+            "timestamp": 0.31496558228566535
+        },
+        {
+            "x": 7.443381794868827,
+            "y": 1.5239371518673812,
+            "heading": -0.1709767215367488,
+            "angularVelocity": -0.723466914966739,
+            "velocityX": -1.0016693503872847,
+            "velocityY": 3.1196550426438785,
+            "timestamp": 0.3937069778570817
+        },
+        {
+            "x": 7.3523133020023,
+            "y": 1.807548605142893,
+            "heading": -0.17097672945138376,
+            "angularVelocity": -1.00514283666892e-7,
+            "velocityX": -1.1565516740674315,
+            "velocityY": 3.6018088226323544,
+            "timestamp": 0.47244837342849805
+        },
+        {
+            "x": 7.26124476293705,
+            "y": 2.0911600435840003,
+            "heading": -0.17097673736581726,
+            "angularVelocity": -1.0051172494783456e-7,
+            "velocityX": -1.1565522607819834,
+            "velocityY": 3.6018086342383846,
+            "timestamp": 0.5511897689999143
+        },
+        {
+            "x": 7.170176175790629,
+            "y": 2.3747714665860937,
+            "heading": -0.17097674528023646,
+            "angularVelocity": -1.0051154369078753e-7,
+            "velocityX": -1.1565528714032516,
+            "velocityY": 3.601808438165991,
+            "timestamp": 0.6299311645713307
+        },
+        {
+            "x": 7.079107531002594,
+            "y": 2.6583828710792723,
+            "heading": -0.17097675319464586,
+            "angularVelocity": -1.0051141913755183e-7,
+            "velocityX": -1.1565536034402273,
+            "velocityY": 3.601808203106465,
+            "timestamp": 0.708672560142747
+        },
+        {
+            "x": 6.988038816793172,
+            "y": 2.9419942532809915,
+            "heading": -0.17097676110905413,
+            "angularVelocity": -1.0051140450859988e-7,
+            "velocityX": -1.1565544850779974,
+            "velocityY": 3.601807920009381,
+            "timestamp": 0.7874139557141634
+        },
+        {
+            "x": 6.896968817946089,
+            "y": 3.2256052229773635,
+            "heading": -0.17097676902348397,
+            "angularVelocity": -1.0051167869642574e-7,
+            "velocityX": -1.156570799719756,
+            "velocityY": 3.6018026812738513,
+            "timestamp": 0.8661553512855797
+        },
+        {
+            "x": 6.765153291475094,
+            "y": 3.4927261567236307,
+            "heading": -0.1709767773203105,
+            "angularVelocity": -1.053680400636008e-7,
+            "velocityX": -1.6740308641271406,
+            "velocityY": 3.392382517579277,
+            "timestamp": 0.9448967468569961
+        },
+        {
+            "x": 6.589137393143544,
+            "y": 3.7330328933669676,
+            "heading": -0.17097678717611917,
+            "angularVelocity": -1.2516680215717606e-7,
+            "velocityX": -2.235366760446956,
+            "velocityY": 3.0518475688608455,
+            "timestamp": 1.0236381424284124
+        },
+        {
+            "x": 6.374222917834714,
+            "y": 3.939286833341542,
+            "heading": -0.17097680095561982,
+            "angularVelocity": -1.7499690668870768e-7,
+            "velocityX": -2.7293709204570815,
+            "velocityY": 2.6193838511219654,
+            "timestamp": 1.1023795379998287
+        },
+        {
+            "x": 6.126883506774902,
+            "y": 4.1052751541137695,
+            "heading": -0.17097682837855596,
+            "angularVelocity": -3.482658130630958e-7,
+            "velocityX": -3.1411611296053437,
+            "velocityY": 2.108018527836233,
+            "timestamp": 1.181120933571245
+        },
+        {
+            "x": 5.792856415059812,
+            "y": 4.239084767822678,
+            "heading": -0.17097684795948573,
+            "angularVelocity": -2.0585572281638246e-7,
+            "velocityX": -3.5116508401962974,
+            "velocityY": 1.4067500932170736,
+            "timestamp": 1.2762406100184678
+        },
+        {
+            "x": 5.438271606876556,
+            "y": 4.300308177252639,
+            "heading": -0.1709790019096565,
+            "angularVelocity": -0.000022644633069121558,
+            "velocityX": -3.7277755920458318,
+            "velocityY": 0.6436461068486719,
+            "timestamp": 1.3713602864656906
+        },
+        {
+            "x": 5.110881399181515,
+            "y": 4.377812934831655,
+            "heading": -0.2483895057711766,
+            "angularVelocity": -0.8138221948691259,
+            "velocityX": -3.4418768011337173,
+            "velocityY": 0.8148130909803968,
+            "timestamp": 1.4664799629129135
+        },
+        {
+            "x": 4.831528638348494,
+            "y": 4.508515906317057,
+            "heading": -0.41196599842011083,
+            "angularVelocity": -1.7196914325049715,
+            "velocityX": -2.936855667165985,
+            "velocityY": 1.374089740075198,
+            "timestamp": 1.5615996393601363
+        },
+        {
+            "x": 4.597313308728367,
+            "y": 4.699607232649008,
+            "heading": -0.5851564423497488,
+            "angularVelocity": -1.8207635938052467,
+            "velocityX": -2.4623226063019743,
+            "velocityY": 2.0089568580269215,
+            "timestamp": 1.6567193158073592
+        },
+        {
+            "x": 4.422373656654268,
+            "y": 4.922240435570765,
+            "heading": -0.6321779806524386,
+            "angularVelocity": -0.49434081421397164,
+            "velocityX": -1.8391531448402672,
+            "velocityY": 2.340558875274187,
+            "timestamp": 1.751838992254582
+        },
+        {
+            "x": 4.3095074978210945,
+            "y": 5.147658123857161,
+            "heading": -0.526303235570326,
+            "angularVelocity": 1.11306881012003,
+            "velocityX": -1.1865700457443962,
+            "velocityY": 2.3698323701875594,
+            "timestamp": 1.846958668701805
+        },
+        {
+            "x": 4.263497791071102,
+            "y": 5.351217934034869,
+            "heading": -0.3174810959543699,
+            "angularVelocity": 2.1953621733755666,
+            "velocityX": -0.48370335632419137,
+            "velocityY": 2.1400389254967007,
+            "timestamp": 1.9420783451490278
+        },
+        {
+            "x": 4.258299402660224,
+            "y": 5.491389588363689,
+            "heading": -0.11860771311510487,
+            "angularVelocity": 2.090770177814994,
+            "velocityX": -0.05465103125915711,
+            "velocityY": 1.4736346838458172,
+            "timestamp": 2.0371980215962506
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 6.542074475732666e-36,
+            "angularVelocity": 1.2469314188732994,
+            "velocityX": 0.04941771792488758,
+            "velocityY": 0.7213061923563153,
+            "timestamp": 2.1323176980434737
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -6.642487421560091e-37,
+            "velocityY": 5.186129384888016e-33,
+            "timestamp": 2.227437374490697
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S2.traj
new file mode 100644
index 00000000..dfbc040a
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S2.traj	
@@ -0,0 +1,247 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": -1.3156383032943886e-37,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.664225467591612,
+            "y": 0.8361290574305452,
+            "heading": -0.011405957532827086,
+            "angularVelocity": -0.14485338302751044,
+            "velocityX": -0.20033341159264173,
+            "velocityY": 0.6239292188565086,
+            "timestamp": 0.07874139557141634
+        },
+        {
+            "x": 7.632676408078482,
+            "y": 0.9343872155736361,
+            "heading": -0.03421427612768491,
+            "angularVelocity": -0.2896610915940815,
+            "velocityX": -0.4006667558300489,
+            "velocityY": 1.2478589873857826,
+            "timestamp": 0.15748279114283267
+        },
+        {
+            "x": 7.585352811707443,
+            "y": 1.0817745497576996,
+            "heading": -0.06841862032572539,
+            "angularVelocity": -0.4343883410984004,
+            "velocityX": -0.6010002239307173,
+            "velocityY": 1.8717897125710332,
+            "timestamp": 0.236224186714249
+        },
+        {
+            "x": 7.522254637419436,
+            "y": 1.278291160108196,
+            "heading": -0.11400992700252059,
+            "angularVelocity": -0.5790004907322871,
+            "velocityX": -0.8013342134732577,
+            "velocityY": 2.4957217093296378,
+            "timestamp": 0.31496558228566535
+        },
+        {
+            "x": 7.443381794868827,
+            "y": 1.5239371518673812,
+            "heading": -0.1709767215367488,
+            "angularVelocity": -0.723466914966739,
+            "velocityX": -1.0016693503872847,
+            "velocityY": 3.1196550426438785,
+            "timestamp": 0.3937069778570817
+        },
+        {
+            "x": 7.3523133020023,
+            "y": 1.807548605142893,
+            "heading": -0.17097672945138376,
+            "angularVelocity": -1.00514283666892e-7,
+            "velocityX": -1.1565516740674315,
+            "velocityY": 3.6018088226323544,
+            "timestamp": 0.47244837342849805
+        },
+        {
+            "x": 7.26124476293705,
+            "y": 2.0911600435840003,
+            "heading": -0.17097673736581726,
+            "angularVelocity": -1.0051172494783456e-7,
+            "velocityX": -1.1565522607819834,
+            "velocityY": 3.6018086342383846,
+            "timestamp": 0.5511897689999143
+        },
+        {
+            "x": 7.170176175790629,
+            "y": 2.3747714665860937,
+            "heading": -0.17097674528023646,
+            "angularVelocity": -1.0051154369078753e-7,
+            "velocityX": -1.1565528714032516,
+            "velocityY": 3.601808438165991,
+            "timestamp": 0.6299311645713307
+        },
+        {
+            "x": 7.079107531002594,
+            "y": 2.6583828710792723,
+            "heading": -0.17097675319464586,
+            "angularVelocity": -1.0051141913755183e-7,
+            "velocityX": -1.1565536034402273,
+            "velocityY": 3.601808203106465,
+            "timestamp": 0.708672560142747
+        },
+        {
+            "x": 6.988038816793172,
+            "y": 2.9419942532809915,
+            "heading": -0.17097676110905413,
+            "angularVelocity": -1.0051140450859988e-7,
+            "velocityX": -1.1565544850779974,
+            "velocityY": 3.601807920009381,
+            "timestamp": 0.7874139557141634
+        },
+        {
+            "x": 6.896968817946089,
+            "y": 3.2256052229773635,
+            "heading": -0.17097676902348397,
+            "angularVelocity": -1.0051167869642574e-7,
+            "velocityX": -1.156570799719756,
+            "velocityY": 3.6018026812738513,
+            "timestamp": 0.8661553512855797
+        },
+        {
+            "x": 6.765153291475094,
+            "y": 3.4927261567236307,
+            "heading": -0.1709767773203105,
+            "angularVelocity": -1.053680400636008e-7,
+            "velocityX": -1.6740308641271406,
+            "velocityY": 3.392382517579277,
+            "timestamp": 0.9448967468569961
+        },
+        {
+            "x": 6.589137393143544,
+            "y": 3.7330328933669676,
+            "heading": -0.17097678717611917,
+            "angularVelocity": -1.2516680215717606e-7,
+            "velocityX": -2.235366760446956,
+            "velocityY": 3.0518475688608455,
+            "timestamp": 1.0236381424284124
+        },
+        {
+            "x": 6.374222917834714,
+            "y": 3.939286833341542,
+            "heading": -0.17097680095561982,
+            "angularVelocity": -1.7499690668870768e-7,
+            "velocityX": -2.7293709204570815,
+            "velocityY": 2.6193838511219654,
+            "timestamp": 1.1023795379998287
+        },
+        {
+            "x": 6.126883506774902,
+            "y": 4.1052751541137695,
+            "heading": -0.17097682837855596,
+            "angularVelocity": -3.482658130630958e-7,
+            "velocityX": -3.1411611296053437,
+            "velocityY": 2.108018527836233,
+            "timestamp": 1.181120933571245
+        },
+        {
+            "x": 5.792856415059812,
+            "y": 4.239084767822678,
+            "heading": -0.17097684795948573,
+            "angularVelocity": -2.0585572281638246e-7,
+            "velocityX": -3.5116508401962974,
+            "velocityY": 1.4067500932170736,
+            "timestamp": 1.2762406100184678
+        },
+        {
+            "x": 5.438271606876556,
+            "y": 4.300308177252639,
+            "heading": -0.1709790019096565,
+            "angularVelocity": -0.000022644633069121558,
+            "velocityX": -3.7277755920458318,
+            "velocityY": 0.6436461068486719,
+            "timestamp": 1.3713602864656906
+        },
+        {
+            "x": 5.110881399181515,
+            "y": 4.377812934831655,
+            "heading": -0.2483895057711766,
+            "angularVelocity": -0.8138221948691259,
+            "velocityX": -3.4418768011337173,
+            "velocityY": 0.8148130909803968,
+            "timestamp": 1.4664799629129135
+        },
+        {
+            "x": 4.831528638348494,
+            "y": 4.508515906317057,
+            "heading": -0.41196599842011083,
+            "angularVelocity": -1.7196914325049715,
+            "velocityX": -2.936855667165985,
+            "velocityY": 1.374089740075198,
+            "timestamp": 1.5615996393601363
+        },
+        {
+            "x": 4.597313308728367,
+            "y": 4.699607232649008,
+            "heading": -0.5851564423497488,
+            "angularVelocity": -1.8207635938052467,
+            "velocityX": -2.4623226063019743,
+            "velocityY": 2.0089568580269215,
+            "timestamp": 1.6567193158073592
+        },
+        {
+            "x": 4.422373656654268,
+            "y": 4.922240435570765,
+            "heading": -0.6321779806524386,
+            "angularVelocity": -0.49434081421397164,
+            "velocityX": -1.8391531448402672,
+            "velocityY": 2.340558875274187,
+            "timestamp": 1.751838992254582
+        },
+        {
+            "x": 4.3095074978210945,
+            "y": 5.147658123857161,
+            "heading": -0.526303235570326,
+            "angularVelocity": 1.11306881012003,
+            "velocityX": -1.1865700457443962,
+            "velocityY": 2.3698323701875594,
+            "timestamp": 1.846958668701805
+        },
+        {
+            "x": 4.263497791071102,
+            "y": 5.351217934034869,
+            "heading": -0.3174810959543699,
+            "angularVelocity": 2.1953621733755666,
+            "velocityX": -0.48370335632419137,
+            "velocityY": 2.1400389254967007,
+            "timestamp": 1.9420783451490278
+        },
+        {
+            "x": 4.258299402660224,
+            "y": 5.491389588363689,
+            "heading": -0.11860771311510487,
+            "angularVelocity": 2.090770177814994,
+            "velocityX": -0.05465103125915711,
+            "velocityY": 1.4736346838458172,
+            "timestamp": 2.0371980215962506
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 6.542074475732666e-36,
+            "angularVelocity": 1.2469314188732994,
+            "velocityX": 0.04941771792488758,
+            "velocityY": 0.7213061923563153,
+            "timestamp": 2.1323176980434737
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -6.642487421560091e-37,
+            "velocityY": 5.186129384888016e-33,
+            "timestamp": 2.227437374490697
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.1.traj
new file mode 100644
index 00000000..d9cc7c97
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.1.traj	
@@ -0,0 +1,247 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": -3.982415645335998e-31,
+            "velocityX": 3.346280928312519e-33,
+            "velocityY": 3.541078180282768e-38,
+            "timestamp": 0
+        },
+        {
+            "x": 7.629395539536042,
+            "y": 0.7881897280541793,
+            "heading": -0.02207380596023684,
+            "angularVelocity": -0.2822732032732244,
+            "velocityX": -0.6471146471374848,
+            "velocityY": 0.015213885156899113,
+            "timestamp": 0.0782001468948451
+        },
+        {
+            "x": 7.528184091504648,
+            "y": 0.7905692108612611,
+            "heading": -0.06614402373093474,
+            "angularVelocity": -0.5635567133951742,
+            "velocityX": -1.2942616101155584,
+            "velocityY": 0.030428111729043,
+            "timestamp": 0.1564002937896902
+        },
+        {
+            "x": 7.376360973312952,
+            "y": 0.7941378827255743,
+            "heading": -0.13206235338942335,
+            "angularVelocity": -0.8429438086302324,
+            "velocityX": -1.9414684526853105,
+            "velocityY": 0.04563510435477082,
+            "timestamp": 0.23460044068453528
+        },
+        {
+            "x": 7.173916466766183,
+            "y": 0.7988917309385671,
+            "heading": -0.21951772550448787,
+            "angularVelocity": -1.1183530413716716,
+            "velocityX": -2.588799568612108,
+            "velocityY": 0.060790783668257076,
+            "timestamp": 0.3128005875793804
+        },
+        {
+            "x": 6.920824289019536,
+            "y": 0.8048108023879232,
+            "heading": -0.3276760191057486,
+            "angularVelocity": -1.383095785572821,
+            "velocityX": -3.236466781667369,
+            "velocityY": 0.07569130857196414,
+            "timestamp": 0.3910007344742255
+        },
+        {
+            "x": 6.625087122175782,
+            "y": 0.8120803100358527,
+            "heading": -0.3276760358371292,
+            "angularVelocity": -2.1395587179409026e-7,
+            "velocityX": -3.781798098684327,
+            "velocityY": 0.09296028123113304,
+            "timestamp": 0.4692008813690706
+        },
+        {
+            "x": 6.329343587367458,
+            "y": 0.8190859886904246,
+            "heading": -0.3276760513572638,
+            "angularVelocity": -1.9846682128968708e-7,
+            "velocityX": -3.78187953030339,
+            "velocityY": 0.08958651527359804,
+            "timestamp": 0.5474010282639157
+        },
+        {
+            "x": 6.033600055760057,
+            "y": 0.826091802470254,
+            "heading": -0.3276760668773978,
+            "angularVelocity": -1.9846681457625392e-7,
+            "velocityX": -3.7818794893709464,
+            "velocityY": 0.0895882432148227,
+            "timestamp": 0.6256011751587608
+        },
+        {
+            "x": 5.7383102527596765,
+            "y": 0.8439033174946344,
+            "heading": -0.3276760825327013,
+            "angularVelocity": -2.0019532088764418e-7,
+            "velocityX": -3.7760773441696487,
+            "velocityY": 0.22776830647924015,
+            "timestamp": 0.7038013220536059
+        },
+        {
+            "x": 5.450465299571764,
+            "y": 0.9121572529445121,
+            "heading": -0.32767609968005607,
+            "angularVelocity": -2.1927522433491614e-7,
+            "velocityX": -3.680874839978332,
+            "velocityY": 0.8728108342551084,
+            "timestamp": 0.782001468948451
+        },
+        {
+            "x": 5.178616904201158,
+            "y": 1.028826716826592,
+            "heading": -0.32767612118071976,
+            "angularVelocity": -2.749440322201668e-7,
+            "velocityX": -3.4763156613508643,
+            "velocityY": 1.4919340757663395,
+            "timestamp": 0.8602016158432961
+        },
+        {
+            "x": 4.930841529412244,
+            "y": 1.190445624246428,
+            "heading": -0.3276761534331376,
+            "angularVelocity": -4.1243423626599064e-7,
+            "velocityX": -3.1684771017365216,
+            "velocityY": 2.0667340642903906,
+            "timestamp": 0.9384017627381412
+        },
+        {
+            "x": 4.7145004488669215,
+            "y": 1.3922123936122235,
+            "heading": -0.32767621878386327,
+            "angularVelocity": -8.356854591223944e-7,
+            "velocityX": -2.7665047846524513,
+            "velocityY": 2.5801328690255976,
+            "timestamp": 1.0166019096329864
+        },
+        {
+            "x": 4.536026477813721,
+            "y": 1.628126621246338,
+            "heading": -0.327696539002069,
+            "angularVelocity": -0.00025984882823803823,
+            "velocityX": -2.282271557538034,
+            "velocityY": 3.016800313073996,
+            "timestamp": 1.0948020565278314
+        },
+        {
+            "x": 4.4347589269746495,
+            "y": 1.8009845745755027,
+            "heading": -0.3311064514524199,
+            "angularVelocity": -0.06394544615832723,
+            "velocityX": -1.899051313343304,
+            "velocityY": 3.241572651564751,
+            "timestamp": 1.1481273933038434
+        },
+        {
+            "x": 4.355560152796487,
+            "y": 1.9823487857199633,
+            "heading": -0.3404702376957046,
+            "angularVelocity": -0.17559732033810904,
+            "velocityX": -1.4851996999249673,
+            "velocityY": 3.4010889027473166,
+            "timestamp": 1.2014527300798554
+        },
+        {
+            "x": 4.29961502910342,
+            "y": 2.1668323093896307,
+            "heading": -0.3620606217709223,
+            "angularVelocity": -0.40488040734269476,
+            "velocityX": -1.049128370778876,
+            "velocityY": 3.4595847832064672,
+            "timestamp": 1.2547780668558675
+        },
+        {
+            "x": 4.26624261518445,
+            "y": 2.3445509425061104,
+            "heading": -0.3930653835662206,
+            "angularVelocity": -0.5814264600999738,
+            "velocityX": -0.6258265945712352,
+            "velocityY": 3.332724064417915,
+            "timestamp": 1.3081034036318795
+        },
+        {
+            "x": 4.250245555353669,
+            "y": 2.506179624728893,
+            "heading": -0.42827286207411436,
+            "angularVelocity": -0.6602392152823773,
+            "velocityX": -0.2999898509389163,
+            "velocityY": 3.0309922448647635,
+            "timestamp": 1.3614287404078915
+        },
+        {
+            "x": 4.245253885327146,
+            "y": 2.646807044638539,
+            "heading": -0.46326114250173545,
+            "angularVelocity": -0.6561286349527023,
+            "velocityX": -0.09360784813172378,
+            "velocityY": 2.6371595270071015,
+            "timestamp": 1.4147540771839036
+        },
+        {
+            "x": 4.246491895421447,
+            "y": 2.764573131222025,
+            "heading": -0.49520995766037457,
+            "angularVelocity": -0.5991301150594935,
+            "velocityX": 0.023216170194527277,
+            "velocityY": 2.2084452476729117,
+            "timestamp": 1.4680794139599156
+        },
+        {
+            "x": 4.250760184591696,
+            "y": 2.858861309855261,
+            "heading": -0.522398988898447,
+            "angularVelocity": -0.5098707834110707,
+            "velocityX": 0.08004242313776105,
+            "velocityY": 1.7681684605074017,
+            "timestamp": 1.5214047507359276
+        },
+        {
+            "x": 4.255888225984497,
+            "y": 2.929511890736146,
+            "heading": -0.5437259487715407,
+            "angularVelocity": -0.3999404628668233,
+            "velocityX": 0.09616519468543994,
+            "velocityY": 1.324897040550944,
+            "timestamp": 1.5747300875119397
+        },
+        {
+            "x": 4.260343621579396,
+            "y": 2.9765343465211713,
+            "heading": -0.5584397105575287,
+            "angularVelocity": -0.2759244043319095,
+            "velocityX": 0.08355119468920533,
+            "velocityY": 0.8818032595380478,
+            "timestamp": 1.6280554242879517
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -0.14177668439456792,
+            "velocityX": 0.04981456435476623,
+            "velocityY": 0.4400469813703916,
+            "timestamp": 1.6813807610639637
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 0,
+            "velocityX": 3.549235715029611e-32,
+            "velocityY": -9.598836837089386e-32,
+            "timestamp": 1.7347060978399758
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.1.traj
new file mode 100644
index 00000000..773c6f5e
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 2.8467808772943154e-31,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 7.633526252649942,
+            "y": 0.8024218861992577,
+            "heading": -0.02675508445787236,
+            "angularVelocity": -0.3471453590528024,
+            "velocityX": -0.602993637929559,
+            "velocityY": 0.20009790028293237,
+            "timestamp": 0.07707170428800919
+        },
+        {
+            "x": 7.540488865885011,
+            "y": 0.8333059401759839,
+            "heading": -0.07783349665964928,
+            "angularVelocity": -0.6627388439588938,
+            "velocityX": -1.2071536191448495,
+            "velocityY": 0.400718451240102,
+            "timestamp": 0.15414340857601838
+        },
+        {
+            "x": 7.400762361365792,
+            "y": 0.8797032018771439,
+            "heading": -0.14939031986058435,
+            "angularVelocity": -0.9284448016555393,
+            "velocityX": -1.8129416730824437,
+            "velocityY": 0.6020012419574645,
+            "timestamp": 0.23121511286402757
+        },
+        {
+            "x": 7.214174023420098,
+            "y": 0.9416744881041137,
+            "heading": -0.23458534610519022,
+            "angularVelocity": -1.105399537114693,
+            "velocityX": -2.4209707008480317,
+            "velocityY": 0.8040731264406625,
+            "timestamp": 0.30828681715203676
+        },
+        {
+            "x": 6.98058449689491,
+            "y": 1.0192605514016468,
+            "heading": -0.3182722929391017,
+            "angularVelocity": -1.085832311702643,
+            "velocityX": -3.030807852026836,
+            "velocityY": 1.0066737723562156,
+            "timestamp": 0.38535852144004595
+        },
+        {
+            "x": 6.7044949607857305,
+            "y": 1.1109419048147462,
+            "heading": -0.3204977394277859,
+            "angularVelocity": -0.02887501333002772,
+            "velocityX": -3.5822425189594846,
+            "velocityY": 1.189559180766205,
+            "timestamp": 0.46243022572805514
+        },
+        {
+            "x": 6.427835578289735,
+            "y": 1.202949830409229,
+            "heading": -0.32049776831244,
+            "angularVelocity": -3.7477637567079044e-7,
+            "velocityX": -3.5896362361749183,
+            "velocityY": 1.1937964320946928,
+            "timestamp": 0.5395019300160644
+        },
+        {
+            "x": 6.1511761970625,
+            "y": 1.294957759818759,
+            "heading": -0.3204977971970938,
+            "angularVelocity": -3.7477637370825724e-7,
+            "velocityX": -3.589636219712834,
+            "velocityY": 1.1937964815946673,
+            "timestamp": 0.6165736343040735
+        },
+        {
+            "x": 5.87524941446386,
+            "y": 1.3871813457379307,
+            "heading": -0.32266464886855784,
+            "angularVelocity": -0.028114749653993878,
+            "velocityX": -3.5801308034856563,
+            "velocityY": 1.1965946098005253,
+            "timestamp": 0.6936453385920827
+        },
+        {
+            "x": 5.641703675217654,
+            "y": 1.4649149367510799,
+            "heading": -0.4060390277887848,
+            "angularVelocity": -1.081776764773039,
+            "velocityX": -3.030239715129037,
+            "velocityY": 1.0085879342004245,
+            "timestamp": 0.7707170428800918
+        },
+        {
+            "x": 5.455154020778328,
+            "y": 1.5270189169325792,
+            "heading": -0.49098864699750666,
+            "angularVelocity": -1.1022153979010723,
+            "velocityX": -2.420468785044752,
+            "velocityY": 0.8057948212669979,
+            "timestamp": 0.8477887471681009
+        },
+        {
+            "x": 5.315462796839212,
+            "y": 1.573537321073397,
+            "heading": -0.5623285861822422,
+            "angularVelocity": -0.9256307466375128,
+            "velocityX": -1.8124839100106522,
+            "velocityY": 0.6035730566821683,
+            "timestamp": 0.9248604514561101
+        },
+        {
+            "x": 5.222455052636063,
+            "y": 1.6045200564296747,
+            "heading": -0.613275527154411,
+            "angularVelocity": -0.6610330139033284,
+            "velocityX": -1.2067690089684353,
+            "velocityY": 0.40199883527290925,
+            "timestamp": 1.0019321557441192
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -0.34674817551357506,
+            "velocityX": -0.602751075316368,
+            "velocityY": 0.2008511906325378,
+            "timestamp": 1.0790038600321283
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 8.760742141155177e-29,
+            "velocityX": 3.313743560366677e-30,
+            "velocityY": 1.2206105198294578e-30,
+            "timestamp": 1.1560755643201375
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.1.traj
new file mode 100644
index 00000000..bec33f37
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.1.traj	
@@ -0,0 +1,310 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": -3.516607019090483e-32,
+            "velocityX": -4.092268936373955e-33,
+            "velocityY": -4.396806616029776e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 7.6580594556627535,
+            "y": 0.8642766421574168,
+            "heading": -0.06977499787943942,
+            "angularVelocity": -0.7009716360726537,
+            "velocityX": -0.22041848409624198,
+            "velocityY": 0.7763344454252913,
+            "timestamp": 0.09954040119279155
+        },
+        {
+            "x": 7.613877684624128,
+            "y": 1.0194223152597515,
+            "heading": -0.19807907998906352,
+            "angularVelocity": -1.2889648883484262,
+            "velocityX": -0.4438576749661031,
+            "velocityY": 1.558620130552277,
+            "timestamp": 0.1990808023855831
+        },
+        {
+            "x": 7.54706531781781,
+            "y": 1.253315507091588,
+            "heading": -0.3641253773176819,
+            "angularVelocity": -1.6681296773861405,
+            "velocityX": -0.6712085344815377,
+            "velocityY": 2.349731255139591,
+            "timestamp": 0.2986212035783746
+        },
+        {
+            "x": 7.457648537893544,
+            "y": 1.5658706931334108,
+            "heading": -0.49008580394214096,
+            "angularVelocity": -1.265420122031624,
+            "velocityX": -0.8982963585919527,
+            "velocityY": 3.139983185686188,
+            "timestamp": 0.3981616047711662
+        },
+        {
+            "x": 7.353528135913809,
+            "y": 1.9277448806018234,
+            "heading": -0.490085841920944,
+            "angularVelocity": -3.8154159059291386e-7,
+            "velocityX": -1.0460114760646044,
+            "velocityY": 3.6354503611807685,
+            "timestamp": 0.49770200596395775
+        },
+        {
+            "x": 7.249407681423134,
+            "y": 2.2896190529615703,
+            "heading": -0.4900858798995876,
+            "angularVelocity": -3.8153998933914064e-7,
+            "velocityX": -1.0460120035985523,
+            "velocityY": 3.635450209396512,
+            "timestamp": 0.5972424071567493
+        },
+        {
+            "x": 7.145287117295142,
+            "y": 2.651493193775847,
+            "heading": -0.4900859178782245,
+            "angularVelocity": -3.8153992177326955e-7,
+            "velocityX": -1.0460131050338848,
+            "velocityY": 3.6354498924852887,
+            "timestamp": 0.6967828083495408
+        },
+        {
+            "x": 7.025437006860337,
+            "y": 3.00846649318931,
+            "heading": -0.49008595631381496,
+            "angularVelocity": -3.8613055604326664e-7,
+            "velocityX": -1.204034834083884,
+            "velocityY": 3.586215196401226,
+            "timestamp": 0.7963232095423323
+        },
+        {
+            "x": 6.830624533504276,
+            "y": 3.3307118703307705,
+            "heading": -0.49008600153372534,
+            "angularVelocity": -4.542870012894294e-7,
+            "velocityX": -1.9571196320451347,
+            "velocityY": 3.237332513029861,
+            "timestamp": 0.8958636107351238
+        },
+        {
+            "x": 6.570226686668516,
+            "y": 3.602717269957059,
+            "heading": -0.4900860357215963,
+            "angularVelocity": -3.4345723556551647e-7,
+            "velocityX": -2.6160015804177474,
+            "velocityY": 2.7326130532612987,
+            "timestamp": 0.9954040119279153
+        },
+        {
+            "x": 6.270670526052627,
+            "y": 3.8032215967371097,
+            "heading": -0.4416564414377051,
+            "angularVelocity": 0.4865320382835506,
+            "velocityX": -3.009392739292897,
+            "velocityY": 2.0143009710369753,
+            "timestamp": 1.094944413120707
+        },
+        {
+            "x": 5.9498699270968665,
+            "y": 3.9255658761230467,
+            "heading": -0.3340733830984328,
+            "angularVelocity": 1.0807979177309486,
+            "velocityX": -3.222818022748657,
+            "velocityY": 1.229091684581206,
+            "timestamp": 1.1944848143134985
+        },
+        {
+            "x": 5.637666976812727,
+            "y": 3.9694128449739723,
+            "heading": -0.13726885518093873,
+            "angularVelocity": 1.977132154976145,
+            "velocityX": -3.136444564649281,
+            "velocityY": 0.4404941945733328,
+            "timestamp": 1.2940252155062901
+        },
+        {
+            "x": 5.333615486534618,
+            "y": 3.942111372133816,
+            "heading": 0.08955200005531357,
+            "angularVelocity": 2.278681344642582,
+            "velocityX": -3.0545535946677287,
+            "velocityY": -0.2742752943830166,
+            "timestamp": 1.3935656166990817
+        },
+        {
+            "x": 5.078082684497777,
+            "y": 3.9650753252247246,
+            "heading": 0.19884455131252843,
+            "angularVelocity": 1.0979717777662474,
+            "velocityX": -2.5671265031564414,
+            "velocityY": 0.2306998245509574,
+            "timestamp": 1.4931060178918734
+        },
+        {
+            "x": 4.8543877601623535,
+            "y": 4.064273357391357,
+            "heading": 0.3064368289582943,
+            "angularVelocity": 1.0808905364705061,
+            "velocityX": -2.2472777048805277,
+            "velocityY": 0.9965605018459247,
+            "timestamp": 1.592646419084665
+        },
+        {
+            "x": 4.700013932982481,
+            "y": 4.187105651970831,
+            "heading": 0.3888797273509616,
+            "angularVelocity": 1.0676410431648378,
+            "velocityX": -1.99915137751061,
+            "velocityY": 1.5906864226746236,
+            "timestamp": 1.6698660978526145
+        },
+        {
+            "x": 4.564169886311024,
+            "y": 4.35607150380987,
+            "heading": 0.4692148602751942,
+            "angularVelocity": 1.040345339504005,
+            "velocityX": -1.7591894817340108,
+            "velocityY": 2.1881190719116157,
+            "timestamp": 1.747085776620564
+        },
+        {
+            "x": 4.445019228108457,
+            "y": 4.571847534724939,
+            "heading": 0.5443267073647724,
+            "angularVelocity": 0.9727034389160626,
+            "velocityX": -1.543008985580261,
+            "velocityY": 2.7943140188848803,
+            "timestamp": 1.8243054553885136
+        },
+        {
+            "x": 4.302573716554072,
+            "y": 4.826880596020871,
+            "heading": 0.5443268349443496,
+            "angularVelocity": 0.0000016521640508021742,
+            "velocityX": -1.8446788930894609,
+            "velocityY": 3.302695185541032,
+            "timestamp": 1.9015251341564632
+        },
+        {
+            "x": 4.139811862406642,
+            "y": 5.06945260129317,
+            "heading": 0.544326895208066,
+            "angularVelocity": 7.80419154390702e-7,
+            "velocityX": -2.1077768872432143,
+            "velocityY": 3.141323677365259,
+            "timestamp": 1.9787448129244127
+        },
+        {
+            "x": 3.9382711614032555,
+            "y": 5.28090933180296,
+            "heading": 0.5443268879285122,
+            "angularVelocity": -9.42707078520697e-8,
+            "velocityX": -2.60996554529876,
+            "velocityY": 2.738378790013248,
+            "timestamp": 2.0559644916923623
+        },
+        {
+            "x": 3.721249786731846,
+            "y": 5.476445328956613,
+            "heading": 0.5443268807879367,
+            "angularVelocity": -9.247093001843253e-8,
+            "velocityX": -2.810441303745562,
+            "velocityY": 2.53220422919983,
+            "timestamp": 2.1331841704603116
+        },
+        {
+            "x": 3.5042282470745802,
+            "y": 5.671981142995692,
+            "heading": 0.5443268736473624,
+            "angularVelocity": -9.247091428352055e-8,
+            "velocityX": -2.810443440323428,
+            "velocityY": 2.53220185785385,
+            "timestamp": 2.210403849228261
+        },
+        {
+            "x": 3.287206691377032,
+            "y": 5.867516939231946,
+            "heading": 0.5443268665067905,
+            "angularVelocity": -9.247088327815795e-8,
+            "velocityX": -2.810443648046155,
+            "velocityY": 2.5322016273060566,
+            "timestamp": 2.2876235279962103
+        },
+        {
+            "x": 3.0701851252220784,
+            "y": 6.063052723861702,
+            "heading": 0.5443268593662193,
+            "angularVelocity": -9.247087442211068e-8,
+            "velocityX": -2.8104437834702534,
+            "velocityY": 2.532201477001163,
+            "timestamp": 2.3648432067641596
+        },
+        {
+            "x": 2.853163562032648,
+            "y": 6.258588511782302,
+            "heading": 0.5443268522252032,
+            "angularVelocity": -9.247663534512907e-8,
+            "velocityX": -2.8104437450665345,
+            "velocityY": 2.5322015196178005,
+            "timestamp": 2.442062885532109
+        },
+        {
+            "x": 2.6687757934039245,
+            "y": 6.424725614600548,
+            "heading": 0.4938774787934042,
+            "angularVelocity": -0.6533227570578579,
+            "velocityX": -2.3878339248576954,
+            "velocityY": 2.1514865830703513,
+            "timestamp": 2.5192825643000583
+        },
+        {
+            "x": 2.521265507868239,
+            "y": 6.557635351096379,
+            "heading": 0.4535239115954702,
+            "angularVelocity": -0.522581391709739,
+            "velocityX": -1.9102680545844264,
+            "velocityY": 1.7211899688838848,
+            "timestamp": 2.5965022430680076
+        },
+        {
+            "x": 2.4106327602038906,
+            "y": 6.6573176734827975,
+            "heading": 0.42326153130587874,
+            "angularVelocity": -0.391899846935805,
+            "velocityX": -1.432701474928527,
+            "velocityY": 1.2908927358526117,
+            "timestamp": 2.673721921835957
+        },
+        {
+            "x": 2.336877585524826,
+            "y": 6.723772559032594,
+            "heading": 0.40308732310971707,
+            "angularVelocity": -0.2612573442164476,
+            "velocityX": -0.9551344405446706,
+            "velocityY": 0.8605952085024652,
+            "timestamp": 2.7509416006039062
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": -0.13063150832354717,
+            "velocityX": -0.4775671967717696,
+            "velocityY": 0.43029758084408004,
+            "timestamp": 2.8281612793718556
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": -5.871044564465464e-32,
+            "velocityX": 0,
+            "velocityY": -1.090928833064942e-33,
+            "timestamp": 2.905380958139805
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.1.traj
new file mode 100644
index 00000000..50d9a810
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.1.traj	
@@ -0,0 +1,346 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": -1.6344658510237988e-32,
+            "velocityX": 1.0699335804910329e-32,
+            "velocityY": 2.3378742346984854e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 7.662237951731404,
+            "y": 0.8490986120749341,
+            "heading": -0.06160217222995063,
+            "angularVelocity": -0.6883164695444892,
+            "velocityX": -0.19846557213082008,
+            "velocityY": 0.6938634772078657,
+            "timestamp": 0.0894968738300239
+        },
+        {
+            "x": 7.626433684211601,
+            "y": 0.9738312717265619,
+            "heading": -0.1754105069837687,
+            "angularVelocity": -1.271645923297472,
+            "velocityX": -0.4000616556484858,
+            "velocityY": 1.3937096829608293,
+            "timestamp": 0.1789937476600478
+        },
+        {
+            "x": 7.572249558904937,
+            "y": 1.161886141289839,
+            "heading": -0.32706746606296133,
+            "angularVelocity": -1.6945503523086813,
+            "velocityX": -0.6054303685464192,
+            "velocityY": 2.101245121929494,
+            "timestamp": 0.2684906214900717
+        },
+        {
+            "x": 7.499449827352778,
+            "y": 1.4140559658703253,
+            "heading": -0.4849879770703222,
+            "angularVelocity": -1.7645366173045323,
+            "velocityX": -0.8134332344437395,
+            "velocityY": 2.8176383575075215,
+            "timestamp": 0.3579874953200956
+        },
+        {
+            "x": 7.40801256781841,
+            "y": 1.7212752924886132,
+            "heading": -0.5291211000298751,
+            "angularVelocity": -0.4931247435901767,
+            "velocityX": -1.0216810445024997,
+            "velocityY": 3.432738077553094,
+            "timestamp": 0.4474843691501195
+        },
+        {
+            "x": 7.313226703333856,
+            "y": 2.0462974852478544,
+            "heading": -0.5291211457832385,
+            "angularVelocity": -5.112286210233085e-7,
+            "velocityX": -1.0590969318612773,
+            "velocityY": 3.631659731227445,
+            "timestamp": 0.5369812429801434
+        },
+        {
+            "x": 7.218440797992873,
+            "y": 2.371319666092203,
+            "heading": -0.5291211915365511,
+            "angularVelocity": -5.112280543858435e-7,
+            "velocityX": -1.059097388373643,
+            "velocityY": 3.6316595980955055,
+            "timestamp": 0.6264781168101673
+        },
+        {
+            "x": 7.1236547448927325,
+            "y": 2.6963418038456624,
+            "heading": -0.5291212372898596,
+            "angularVelocity": -5.112280079310825e-7,
+            "velocityX": -1.0590990393716089,
+            "velocityY": 3.631659116616238,
+            "timestamp": 0.7159749906401912
+        },
+        {
+            "x": 7.010927268830994,
+            "y": 3.0155851307576884,
+            "heading": -0.5291212838180788,
+            "angularVelocity": -5.198865285819975e-7,
+            "velocityX": -1.2595688680236534,
+            "velocityY": 3.567089142335248,
+            "timestamp": 0.8054718644702151
+        },
+        {
+            "x": 6.837725577807192,
+            "y": 3.30648882172331,
+            "heading": -0.5291213383056133,
+            "angularVelocity": -6.088205344213367e-7,
+            "velocityX": -1.9352820228419736,
+            "velocityY": 3.2504341047500516,
+            "timestamp": 0.894968738300239
+        },
+        {
+            "x": 6.610788937088648,
+            "y": 3.5577327922782125,
+            "heading": -0.5291214021937537,
+            "angularVelocity": -7.138589055640395e-7,
+            "velocityX": -2.535693494161062,
+            "velocityY": 2.8072932584447,
+            "timestamp": 0.984465612130263
+        },
+        {
+            "x": 6.351860794366729,
+            "y": 3.7516776606884883,
+            "heading": -0.4841599241371699,
+            "angularVelocity": 0.5023804310972586,
+            "velocityX": -2.8931529297178065,
+            "velocityY": 2.167057463690003,
+            "timestamp": 1.0739624859602868
+        },
+        {
+            "x": 6.072750894879938,
+            "y": 3.8834732684056306,
+            "heading": -0.3889789775410024,
+            "angularVelocity": 1.0635114113252733,
+            "velocityX": -3.1186552953445856,
+            "velocityY": 1.4726280603661466,
+            "timestamp": 1.1634593597903107
+        },
+        {
+            "x": 5.7916594444015015,
+            "y": 3.950870142776961,
+            "heading": -0.22937923493241555,
+            "angularVelocity": 1.783299636942698,
+            "velocityX": -3.1407963032574324,
+            "velocityY": 0.7530640064515952,
+            "timestamp": 1.2529562336203346
+        },
+        {
+            "x": 5.498028130376245,
+            "y": 3.9763121038871923,
+            "heading": -0.08643118620464799,
+            "angularVelocity": 1.5972406924431666,
+            "velocityX": -3.280911404603181,
+            "velocityY": 0.28427765151386064,
+            "timestamp": 1.3424531074503585
+        },
+        {
+            "x": 5.202994132142315,
+            "y": 4.066458551574186,
+            "heading": -0.003961984667318494,
+            "angularVelocity": 0.9214757790753437,
+            "velocityX": -3.2965844013084786,
+            "velocityY": 1.0072580619765965,
+            "timestamp": 1.4319499812803824
+        },
+        {
+            "x": 4.937438011169434,
+            "y": 4.2165327072143555,
+            "heading": 0.07947932900863551,
+            "angularVelocity": 0.9323377466170348,
+            "velocityX": -2.967211139433056,
+            "velocityY": 1.6768647799385226,
+            "timestamp": 1.5214468551104063
+        },
+        {
+            "x": 4.74667334234301,
+            "y": 4.373655515147294,
+            "heading": 0.13953709439935258,
+            "angularVelocity": 0.8363208396611413,
+            "velocityX": -2.6564502853656404,
+            "velocityY": 2.187978154123957,
+            "timestamp": 1.5932587274715584
+        },
+        {
+            "x": 4.579753248112816,
+            "y": 4.566217048926455,
+            "heading": 0.1819755811120341,
+            "angularVelocity": 0.5909675561619205,
+            "velocityX": -2.324408050394913,
+            "velocityY": 2.681472122196475,
+            "timestamp": 1.6650705998327104
+        },
+        {
+            "x": 4.438339398678278,
+            "y": 4.792399271574889,
+            "heading": 0.19511904158847473,
+            "angularVelocity": 0.18302628861061485,
+            "velocityX": -1.9692266025783034,
+            "velocityY": 3.1496494271996025,
+            "timestamp": 1.7368824721938625
+        },
+        {
+            "x": 4.33242543936342,
+            "y": 5.0425620352859015,
+            "heading": 0.1951190405932856,
+            "angularVelocity": -1.3858280983712203e-8,
+            "velocityX": -1.4748809052380785,
+            "velocityY": 3.483585032470794,
+            "timestamp": 1.8086943445550145
+        },
+        {
+            "x": 4.2346577127829645,
+            "y": 5.296019422041832,
+            "heading": 0.19511903926490842,
+            "angularVelocity": -1.849801636869094e-8,
+            "velocityX": -1.3614423822396335,
+            "velocityY": 3.5294635611400946,
+            "timestamp": 1.8805062169161666
+        },
+        {
+            "x": 4.124562899973198,
+            "y": 5.544370599514641,
+            "heading": 0.19511903792287785,
+            "angularVelocity": -1.8688143477674107e-8,
+            "velocityX": -1.533100435761989,
+            "velocityY": 3.4583581977060263,
+            "timestamp": 1.9523180892773186
+        },
+        {
+            "x": 3.9766612552398675,
+            "y": 5.772239592130343,
+            "heading": 0.1951190364645304,
+            "angularVelocity": -2.030788798410176e-8,
+            "velocityX": -2.059570929852819,
+            "velocityY": 3.1731381611903378,
+            "timestamp": 2.024129961638471
+        },
+        {
+            "x": 3.7946576924754596,
+            "y": 5.973917226463319,
+            "heading": 0.19511901792814293,
+            "angularVelocity": -2.581242745470272e-7,
+            "velocityX": -2.534449482797023,
+            "velocityY": 2.8084163203364336,
+            "timestamp": 2.095941833999623
+        },
+        {
+            "x": 3.5938514397947183,
+            "y": 6.137095652578835,
+            "heading": 0.18216101901891746,
+            "angularVelocity": -0.1804436854683003,
+            "velocityX": -2.7962820920593496,
+            "velocityY": 2.272304296633136,
+            "timestamp": 2.167753706360775
+        },
+        {
+            "x": 3.3950104775935763,
+            "y": 6.257338696148904,
+            "heading": 0.16464788328752675,
+            "angularVelocity": -0.2438752138826665,
+            "velocityX": -2.768914883616214,
+            "velocityY": 1.6744173298441403,
+            "timestamp": 2.239565578721927
+        },
+        {
+            "x": 3.203569657298003,
+            "y": 6.335228188048846,
+            "heading": 0.14501288414051958,
+            "angularVelocity": -0.273422743362824,
+            "velocityX": -2.6658658798476953,
+            "velocityY": 1.0846325174230818,
+            "timestamp": 2.311377451083079
+        },
+        {
+            "x": 3.0218761949257575,
+            "y": 6.3712387750954615,
+            "heading": 0.12433283555566121,
+            "angularVelocity": -0.2879753431418064,
+            "velocityX": -2.530131249864694,
+            "velocityY": 0.5014572919852904,
+            "timestamp": 2.383189323444231
+        },
+        {
+            "x": 2.851226329803467,
+            "y": 6.365692615509033,
+            "heading": 0.10320964185538832,
+            "angularVelocity": -0.2941462603013797,
+            "velocityX": -2.3763461320722596,
+            "velocityY": -0.07723179195964242,
+            "timestamp": 2.4550011958053832
+        },
+        {
+            "x": 2.6537141886903095,
+            "y": 6.292049225449601,
+            "heading": 0.07629879670595036,
+            "angularVelocity": -0.2952020622183997,
+            "velocityX": -2.1666354603877345,
+            "velocityY": -0.8078408720936208,
+            "timestamp": 2.5461619585290816
+        },
+        {
+            "x": 2.4816525262537397,
+            "y": 6.153957047067382,
+            "heading": 0.05014624127109134,
+            "angularVelocity": -0.2868839032657673,
+            "velocityX": -1.8874530806425645,
+            "velocityY": -1.5148203487586651,
+            "timestamp": 2.63732272125278
+        },
+        {
+            "x": 2.3499481448537543,
+            "y": 5.9595438725680525,
+            "heading": 0.026875208437583037,
+            "angularVelocity": -0.25527466135886906,
+            "velocityX": -1.4447485679685645,
+            "velocityY": -2.1326409377308697,
+            "timestamp": 2.728483483976478
+        },
+        {
+            "x": 2.2874888909600317,
+            "y": 5.764180997032623,
+            "heading": 0.012070368781366221,
+            "angularVelocity": -0.1624036396129025,
+            "velocityX": -0.6851550165615855,
+            "velocityY": -2.1430588083994038,
+            "timestamp": 2.8196442467001765
+        },
+        {
+            "x": 2.260118679493569,
+            "y": 5.628541423814637,
+            "heading": 0.0037496226116310134,
+            "angularVelocity": -0.09127552162935264,
+            "velocityX": -0.3002411415689918,
+            "velocityY": -1.487916173201615,
+            "timestamp": 2.910805009423875
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -4.5794503744404666e-32,
+            "angularVelocity": -0.04113197936919252,
+            "velocityX": -0.11099818815950499,
+            "velocityY": -0.7518741810265671,
+            "timestamp": 3.001965772147573
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": -2.1498446437166114e-31,
+            "velocityX": 0,
+            "velocityY": -6.253874378421497e-34,
+            "timestamp": 3.0931265348712715
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.traj
index d94da80a..50d9a810 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.traj	
@@ -4,280 +4,343 @@
             "x": 7.68,
             "y": 0.787,
             "heading": 0,
-            "angularVelocity": -2.906139149989869e-32,
-            "velocityX": 1.181772084740038e-33,
-            "velocityY": 2.7502272506846476e-34,
+            "angularVelocity": -1.6344658510237988e-32,
+            "velocityX": 1.0699335804910329e-32,
+            "velocityY": 2.3378742346984854e-33,
             "timestamp": 0
         },
         {
-            "x": 7.666043555718406,
-            "y": 0.8524996120976488,
-            "heading": 0.004999198621269784,
-            "angularVelocity": 0.05577597491216818,
-            "velocityX": -0.15571181404981524,
-            "velocityY": 0.7307780702234378,
-            "timestamp": 0.08962996396104499
-        },
-        {
-            "x": 7.638131101049187,
-            "y": 0.9835016263770707,
-            "heading": 0.014524734067527434,
-            "angularVelocity": 0.10627623871853438,
-            "velocityX": -0.31141878715192206,
-            "velocityY": 1.4615872693685117,
-            "timestamp": 0.17925992792208997
-        },
-        {
-            "x": 7.596263489657983,
-            "y": 1.180010140566484,
-            "heading": 0.02778512041931341,
-            "angularVelocity": 0.14794590743726296,
-            "velocityX": -0.46711623592083296,
-            "velocityY": 2.1924421867983774,
-            "timestamp": 0.26888989188313495
-        },
-        {
-            "x": 7.540442918712468,
-            "y": 1.4420312351509346,
-            "heading": 0.04318451816280708,
-            "angularVelocity": 0.17181082154832236,
-            "velocityX": -0.6227891709269913,
-            "velocityY": 2.9233649440976053,
-            "timestamp": 0.35851985584417995
-        },
-        {
-            "x": 7.470680035396563,
-            "y": 1.7695653388184471,
-            "heading": 0.05582613302662815,
-            "angularVelocity": 0.1410422843561041,
-            "velocityX": -0.7783433154812639,
-            "velocityY": 3.654292484261909,
-            "timestamp": 0.44814981980522495
-        },
-        {
-            "x": 7.399999368806171,
-            "y": 2.101181376235394,
-            "heading": 0.05582613685023459,
-            "angularVelocity": 4.265991273772723e-8,
-            "velocityX": -0.7885830080341364,
-            "velocityY": 3.6998345504308543,
-            "timestamp": 0.5377797837662699
-        },
-        {
-            "x": 7.329318638814834,
-            "y": 2.4327974001390613,
-            "heading": 0.05582614067384277,
-            "angularVelocity": 4.265993214747779e-8,
-            "velocityX": -0.788583715397405,
-            "velocityY": 3.6998343996634295,
-            "timestamp": 0.6274097477273148
-        },
-        {
-            "x": 7.258637432835217,
-            "y": 2.764413322590103,
-            "heading": 0.05582614449746126,
-            "angularVelocity": 4.266004720083137e-8,
-            "velocityX": -0.7885890259906463,
-            "velocityY": 3.699833267758185,
-            "timestamp": 0.7170397116883598
-        },
-        {
-            "x": 7.145396314308057,
-            "y": 3.084009006822374,
-            "heading": 0.05582614848984762,
-            "angularVelocity": 4.4542987394171874e-8,
-            "velocityX": -1.2634292542656518,
-            "velocityY": 3.5657236721770227,
-            "timestamp": 0.8066696756494047
-        },
-        {
-            "x": 6.971535677728531,
-            "y": 3.375105946924151,
-            "heading": 0.05582615332930866,
-            "angularVelocity": 5.39937854332775e-8,
-            "velocityX": -1.9397601973274063,
-            "velocityY": 3.2477636633692444,
-            "timestamp": 0.8962996396104497
-        },
-        {
-            "x": 6.743840876905014,
-            "y": 3.626342927147427,
-            "heading": 0.0558261606002303,
-            "angularVelocity": 8.112155042111421e-8,
-            "velocityX": -2.5403870621043505,
-            "velocityY": 2.8030467615993793,
-            "timestamp": 0.9859296035714946
-        },
-        {
-            "x": 6.471198532281258,
-            "y": 3.8279144449501397,
-            "heading": 0.0558261762788256,
-            "angularVelocity": 1.7492582391526053e-7,
-            "velocityX": -3.041866052096724,
-            "velocityY": 2.2489300329331785,
-            "timestamp": 1.0755595675325396
-        },
-        {
-            "x": 6.164249513947577,
-            "y": 3.9719534129902763,
-            "heading": 0.05582625958870765,
-            "angularVelocity": 9.294869524458774e-7,
-            "velocityX": -3.4246250335109787,
-            "velocityY": 1.6070403431461682,
-            "timestamp": 1.1651895314935845
-        },
-        {
-            "x": 5.836252922691461,
-            "y": 4.05240893270077,
-            "heading": 0.05954498226559844,
-            "angularVelocity": 0.04148972634315725,
-            "velocityX": -3.6594524505071675,
-            "velocityY": 0.8976408798452788,
-            "timestamp": 1.2548194954546295
-        },
-        {
-            "x": 5.497492188342187,
-            "y": 4.066758597782368,
-            "heading": 0.05954602093201425,
-            "angularVelocity": 0.000011588383726853398,
-            "velocityX": -3.779547813904135,
-            "velocityY": 0.16009897190000646,
-            "timestamp": 1.3444494594156744
-        },
-        {
-            "x": 5.204246867986104,
-            "y": 4.110932074419811,
-            "heading": 0.17401114275805463,
-            "angularVelocity": 1.2770854384789139,
-            "velocityX": -3.2717331057226993,
-            "velocityY": 0.4928427356797903,
-            "timestamp": 1.4340794233767193
+            "x": 7.662237951731404,
+            "y": 0.8490986120749341,
+            "heading": -0.06160217222995063,
+            "angularVelocity": -0.6883164695444892,
+            "velocityX": -0.19846557213082008,
+            "velocityY": 0.6938634772078657,
+            "timestamp": 0.0894968738300239
+        },
+        {
+            "x": 7.626433684211601,
+            "y": 0.9738312717265619,
+            "heading": -0.1754105069837687,
+            "angularVelocity": -1.271645923297472,
+            "velocityX": -0.4000616556484858,
+            "velocityY": 1.3937096829608293,
+            "timestamp": 0.1789937476600478
+        },
+        {
+            "x": 7.572249558904937,
+            "y": 1.161886141289839,
+            "heading": -0.32706746606296133,
+            "angularVelocity": -1.6945503523086813,
+            "velocityX": -0.6054303685464192,
+            "velocityY": 2.101245121929494,
+            "timestamp": 0.2684906214900717
+        },
+        {
+            "x": 7.499449827352778,
+            "y": 1.4140559658703253,
+            "heading": -0.4849879770703222,
+            "angularVelocity": -1.7645366173045323,
+            "velocityX": -0.8134332344437395,
+            "velocityY": 2.8176383575075215,
+            "timestamp": 0.3579874953200956
+        },
+        {
+            "x": 7.40801256781841,
+            "y": 1.7212752924886132,
+            "heading": -0.5291211000298751,
+            "angularVelocity": -0.4931247435901767,
+            "velocityX": -1.0216810445024997,
+            "velocityY": 3.432738077553094,
+            "timestamp": 0.4474843691501195
+        },
+        {
+            "x": 7.313226703333856,
+            "y": 2.0462974852478544,
+            "heading": -0.5291211457832385,
+            "angularVelocity": -5.112286210233085e-7,
+            "velocityX": -1.0590969318612773,
+            "velocityY": 3.631659731227445,
+            "timestamp": 0.5369812429801434
+        },
+        {
+            "x": 7.218440797992873,
+            "y": 2.371319666092203,
+            "heading": -0.5291211915365511,
+            "angularVelocity": -5.112280543858435e-7,
+            "velocityX": -1.059097388373643,
+            "velocityY": 3.6316595980955055,
+            "timestamp": 0.6264781168101673
+        },
+        {
+            "x": 7.1236547448927325,
+            "y": 2.6963418038456624,
+            "heading": -0.5291212372898596,
+            "angularVelocity": -5.112280079310825e-7,
+            "velocityX": -1.0590990393716089,
+            "velocityY": 3.631659116616238,
+            "timestamp": 0.7159749906401912
+        },
+        {
+            "x": 7.010927268830994,
+            "y": 3.0155851307576884,
+            "heading": -0.5291212838180788,
+            "angularVelocity": -5.198865285819975e-7,
+            "velocityX": -1.2595688680236534,
+            "velocityY": 3.567089142335248,
+            "timestamp": 0.8054718644702151
+        },
+        {
+            "x": 6.837725577807192,
+            "y": 3.30648882172331,
+            "heading": -0.5291213383056133,
+            "angularVelocity": -6.088205344213367e-7,
+            "velocityX": -1.9352820228419736,
+            "velocityY": 3.2504341047500516,
+            "timestamp": 0.894968738300239
+        },
+        {
+            "x": 6.610788937088648,
+            "y": 3.5577327922782125,
+            "heading": -0.5291214021937537,
+            "angularVelocity": -7.138589055640395e-7,
+            "velocityX": -2.535693494161062,
+            "velocityY": 2.8072932584447,
+            "timestamp": 0.984465612130263
+        },
+        {
+            "x": 6.351860794366729,
+            "y": 3.7516776606884883,
+            "heading": -0.4841599241371699,
+            "angularVelocity": 0.5023804310972586,
+            "velocityX": -2.8931529297178065,
+            "velocityY": 2.167057463690003,
+            "timestamp": 1.0739624859602868
+        },
+        {
+            "x": 6.072750894879938,
+            "y": 3.8834732684056306,
+            "heading": -0.3889789775410024,
+            "angularVelocity": 1.0635114113252733,
+            "velocityX": -3.1186552953445856,
+            "velocityY": 1.4726280603661466,
+            "timestamp": 1.1634593597903107
+        },
+        {
+            "x": 5.7916594444015015,
+            "y": 3.950870142776961,
+            "heading": -0.22937923493241555,
+            "angularVelocity": 1.783299636942698,
+            "velocityX": -3.1407963032574324,
+            "velocityY": 0.7530640064515952,
+            "timestamp": 1.2529562336203346
+        },
+        {
+            "x": 5.498028130376245,
+            "y": 3.9763121038871923,
+            "heading": -0.08643118620464799,
+            "angularVelocity": 1.5972406924431666,
+            "velocityX": -3.280911404603181,
+            "velocityY": 0.28427765151386064,
+            "timestamp": 1.3424531074503585
+        },
+        {
+            "x": 5.202994132142315,
+            "y": 4.066458551574186,
+            "heading": -0.003961984667318494,
+            "angularVelocity": 0.9214757790753437,
+            "velocityX": -3.2965844013084786,
+            "velocityY": 1.0072580619765965,
+            "timestamp": 1.4319499812803824
         },
         {
             "x": 4.937438011169434,
             "y": 4.2165327072143555,
-            "heading": 0.3021760231951433,
-            "angularVelocity": 1.4299334148208713,
-            "velocityX": -2.976781926774297,
-            "velocityY": 1.1781844834885962,
-            "timestamp": 1.5237093873377643
-        },
-        {
-            "x": 4.703348327317259,
-            "y": 4.378813716693983,
-            "heading": 0.4253911826920849,
-            "angularVelocity": 1.3960058537628992,
-            "velocityX": -2.6521945051027034,
-            "velocityY": 1.8386149895276152,
-            "timestamp": 1.61197202546698
-        },
-        {
-            "x": 4.500555108729279,
-            "y": 4.597666686435954,
-            "heading": 0.5261840356750895,
-            "angularVelocity": 1.1419651068603307,
-            "velocityX": -2.297611117074157,
-            "velocityY": 2.479565242787912,
-            "timestamp": 1.7002346635961958
-        },
-        {
-            "x": 4.326794639818804,
-            "y": 4.871425359866347,
-            "heading": 0.5588913487111756,
-            "angularVelocity": 0.37056804248478087,
-            "velocityX": -1.968675224233522,
-            "velocityY": 3.1016371052677254,
-            "timestamp": 1.7884973017254115
-        },
-        {
-            "x": 4.0969805858398445,
-            "y": 5.11364296690146,
-            "heading": 0.5588923269288569,
-            "angularVelocity": 0.00001108303243636796,
-            "velocityX": -2.603752378696331,
-            "velocityY": 2.744282429905488,
-            "timestamp": 1.8767599398546273
-        },
-        {
-            "x": 3.8246168380678105,
-            "y": 5.306780372295281,
-            "heading": 0.5588922897097742,
-            "angularVelocity": -4.2168559122096544e-7,
-            "velocityX": -3.0858328455274044,
-            "velocityY": 2.188212470049549,
-            "timestamp": 1.965022577983843
-        },
-        {
-            "x": 3.520011793550478,
-            "y": 5.443527703198532,
-            "heading": 0.5588922240132788,
-            "angularVelocity": -7.44329614921164e-7,
-            "velocityX": -3.451121006278936,
-            "velocityY": 1.549322950250525,
-            "timestamp": 2.053285216113059
-        },
-        {
-            "x": 3.194693864220709,
-            "y": 5.518709582083228,
-            "heading": 0.5588921711301718,
-            "angularVelocity": -5.991562005821763e-7,
-            "velocityX": -3.6857943091787817,
-            "velocityY": 0.851797323060201,
-            "timestamp": 2.1415478542422743
-        },
-        {
-            "x": 2.8828791952129387,
-            "y": 5.532174882793972,
-            "heading": 0.5041280478943673,
-            "angularVelocity": -0.6204677811196886,
-            "velocityX": -3.5328047701370098,
-            "velocityY": 0.15255946339412532,
-            "timestamp": 2.22981049237149
-        },
-        {
-            "x": 2.6283322264519793,
-            "y": 5.543894951157175,
-            "heading": 0.3346872638422791,
-            "angularVelocity": -1.9197339626765784,
-            "velocityX": -2.883971906530878,
-            "velocityY": 0.132786291137652,
-            "timestamp": 2.3180731305007054
-        },
-        {
-            "x": 2.4385492640365714,
-            "y": 5.5521384887253,
-            "heading": 0.1780668377028913,
-            "angularVelocity": -1.7744815865360473,
-            "velocityX": -2.150207227406541,
-            "velocityY": 0.09339781523475092,
-            "timestamp": 2.406335768629921
-        },
-        {
-            "x": 2.31267915340354,
-            "y": 5.55742889143524,
-            "heading": 0.062002293486176877,
-            "angularVelocity": -1.3149906537667377,
-            "velocityX": -1.4260859781775244,
-            "velocityY": 0.0599393222554144,
-            "timestamp": 2.4945984067591365
+            "heading": 0.07947932900863551,
+            "angularVelocity": 0.9323377466170348,
+            "velocityX": -2.967211139433056,
+            "velocityY": 1.6768647799385226,
+            "timestamp": 1.5214468551104063
+        },
+        {
+            "x": 4.74667334234301,
+            "y": 4.373655515147294,
+            "heading": 0.13953709439935258,
+            "angularVelocity": 0.8363208396611413,
+            "velocityX": -2.6564502853656404,
+            "velocityY": 2.187978154123957,
+            "timestamp": 1.5932587274715584
+        },
+        {
+            "x": 4.579753248112816,
+            "y": 4.566217048926455,
+            "heading": 0.1819755811120341,
+            "angularVelocity": 0.5909675561619205,
+            "velocityX": -2.324408050394913,
+            "velocityY": 2.681472122196475,
+            "timestamp": 1.6650705998327104
+        },
+        {
+            "x": 4.438339398678278,
+            "y": 4.792399271574889,
+            "heading": 0.19511904158847473,
+            "angularVelocity": 0.18302628861061485,
+            "velocityX": -1.9692266025783034,
+            "velocityY": 3.1496494271996025,
+            "timestamp": 1.7368824721938625
+        },
+        {
+            "x": 4.33242543936342,
+            "y": 5.0425620352859015,
+            "heading": 0.1951190405932856,
+            "angularVelocity": -1.3858280983712203e-8,
+            "velocityX": -1.4748809052380785,
+            "velocityY": 3.483585032470794,
+            "timestamp": 1.8086943445550145
+        },
+        {
+            "x": 4.2346577127829645,
+            "y": 5.296019422041832,
+            "heading": 0.19511903926490842,
+            "angularVelocity": -1.849801636869094e-8,
+            "velocityX": -1.3614423822396335,
+            "velocityY": 3.5294635611400946,
+            "timestamp": 1.8805062169161666
+        },
+        {
+            "x": 4.124562899973198,
+            "y": 5.544370599514641,
+            "heading": 0.19511903792287785,
+            "angularVelocity": -1.8688143477674107e-8,
+            "velocityX": -1.533100435761989,
+            "velocityY": 3.4583581977060263,
+            "timestamp": 1.9523180892773186
+        },
+        {
+            "x": 3.9766612552398675,
+            "y": 5.772239592130343,
+            "heading": 0.1951190364645304,
+            "angularVelocity": -2.030788798410176e-8,
+            "velocityX": -2.059570929852819,
+            "velocityY": 3.1731381611903378,
+            "timestamp": 2.024129961638471
+        },
+        {
+            "x": 3.7946576924754596,
+            "y": 5.973917226463319,
+            "heading": 0.19511901792814293,
+            "angularVelocity": -2.581242745470272e-7,
+            "velocityX": -2.534449482797023,
+            "velocityY": 2.8084163203364336,
+            "timestamp": 2.095941833999623
+        },
+        {
+            "x": 3.5938514397947183,
+            "y": 6.137095652578835,
+            "heading": 0.18216101901891746,
+            "angularVelocity": -0.1804436854683003,
+            "velocityX": -2.7962820920593496,
+            "velocityY": 2.272304296633136,
+            "timestamp": 2.167753706360775
+        },
+        {
+            "x": 3.3950104775935763,
+            "y": 6.257338696148904,
+            "heading": 0.16464788328752675,
+            "angularVelocity": -0.2438752138826665,
+            "velocityX": -2.768914883616214,
+            "velocityY": 1.6744173298441403,
+            "timestamp": 2.239565578721927
+        },
+        {
+            "x": 3.203569657298003,
+            "y": 6.335228188048846,
+            "heading": 0.14501288414051958,
+            "angularVelocity": -0.273422743362824,
+            "velocityX": -2.6658658798476953,
+            "velocityY": 1.0846325174230818,
+            "timestamp": 2.311377451083079
+        },
+        {
+            "x": 3.0218761949257575,
+            "y": 6.3712387750954615,
+            "heading": 0.12433283555566121,
+            "angularVelocity": -0.2879753431418064,
+            "velocityX": -2.530131249864694,
+            "velocityY": 0.5014572919852904,
+            "timestamp": 2.383189323444231
+        },
+        {
+            "x": 2.851226329803467,
+            "y": 6.365692615509033,
+            "heading": 0.10320964185538832,
+            "angularVelocity": -0.2941462603013797,
+            "velocityX": -2.3763461320722596,
+            "velocityY": -0.07723179195964242,
+            "timestamp": 2.4550011958053832
+        },
+        {
+            "x": 2.6537141886903095,
+            "y": 6.292049225449601,
+            "heading": 0.07629879670595036,
+            "angularVelocity": -0.2952020622183997,
+            "velocityX": -2.1666354603877345,
+            "velocityY": -0.8078408720936208,
+            "timestamp": 2.5461619585290816
+        },
+        {
+            "x": 2.4816525262537397,
+            "y": 6.153957047067382,
+            "heading": 0.05014624127109134,
+            "angularVelocity": -0.2868839032657673,
+            "velocityX": -1.8874530806425645,
+            "velocityY": -1.5148203487586651,
+            "timestamp": 2.63732272125278
+        },
+        {
+            "x": 2.3499481448537543,
+            "y": 5.9595438725680525,
+            "heading": 0.026875208437583037,
+            "angularVelocity": -0.25527466135886906,
+            "velocityX": -1.4447485679685645,
+            "velocityY": -2.1326409377308697,
+            "timestamp": 2.728483483976478
+        },
+        {
+            "x": 2.2874888909600317,
+            "y": 5.764180997032623,
+            "heading": 0.012070368781366221,
+            "angularVelocity": -0.1624036396129025,
+            "velocityX": -0.6851550165615855,
+            "velocityY": -2.1430588083994038,
+            "timestamp": 2.8196442467001765
+        },
+        {
+            "x": 2.260118679493569,
+            "y": 5.628541423814637,
+            "heading": 0.0037496226116310134,
+            "angularVelocity": -0.09127552162935264,
+            "velocityX": -0.3002411415689918,
+            "velocityY": -1.487916173201615,
+            "timestamp": 2.910805009423875
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 9.107235017738231e-34,
-            "angularVelocity": -0.7024749633633888,
-            "velocityX": -0.7101436659051381,
-            "velocityY": 0.029130202985730554,
-            "timestamp": 2.582861044888352
+            "heading": -4.5794503744404666e-32,
+            "angularVelocity": -0.04113197936919252,
+            "velocityX": -0.11099818815950499,
+            "velocityY": -0.7518741810265671,
+            "timestamp": 3.001965772147573
         },
         {
             "x": 2.25,
             "y": 5.56,
             "heading": 0,
-            "angularVelocity": -2.2232006496445675e-32,
+            "angularVelocity": -2.1498446437166114e-31,
             "velocityX": 0,
-            "velocityY": -2.9660881962078245e-33,
-            "timestamp": 2.6711236830175675
+            "velocityY": -6.253874378421497e-34,
+            "timestamp": 3.0931265348712715
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W3.1.traj
new file mode 100644
index 00000000..62c8640c
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W3.1.traj	
@@ -0,0 +1,292 @@
+{
+    "samples": [
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": -1.5069359770438582e-30,
+            "velocityX": 5.69693995403372e-32,
+            "velocityY": 3.042278119963715e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 7.627290492953592,
+            "y": 0.8035142159340071,
+            "heading": 0.015175150751654183,
+            "angularVelocity": 0.18617297608634506,
+            "velocityX": -0.6466549133823012,
+            "velocityY": 0.20260100070713255,
+            "timestamp": 0.08151102845675925
+        },
+        {
+            "x": 7.521871327482097,
+            "y": 0.8365426259859824,
+            "heading": 0.045515680376982795,
+            "angularVelocity": 0.3722260680519286,
+            "velocityX": -1.2933116839203953,
+            "velocityY": 0.4052017337690261,
+            "timestamp": 0.1630220569135185
+        },
+        {
+            "x": 7.363742244741542,
+            "y": 0.8860852747673063,
+            "heading": 0.09100430929463568,
+            "angularVelocity": 0.5580671693007231,
+            "velocityX": -1.9399716300284648,
+            "velocityY": 0.6078030141357551,
+            "timestamp": 0.24453308537027776
+        },
+        {
+            "x": 7.152902908699907,
+            "y": 0.95214234591484,
+            "heading": 0.15161661460632753,
+            "angularVelocity": 0.7436086436309897,
+            "velocityX": -2.5866356986709165,
+            "velocityY": 0.8104065474106494,
+            "timestamp": 0.326044113827037
+        },
+        {
+            "x": 6.889352971860368,
+            "y": 1.0347142415767305,
+            "heading": 0.22732297101557483,
+            "angularVelocity": 0.9287866665726976,
+            "velocityX": -3.233304030501219,
+            "velocityY": 1.0130150143510155,
+            "timestamp": 0.4075551422837963
+        },
+        {
+            "x": 6.595109187456524,
+            "y": 1.1269159867790934,
+            "heading": 0.22732298101704992,
+            "angularVelocity": 1.22700882310899e-7,
+            "velocityX": -3.6098647014365244,
+            "velocityY": 1.1311566906713055,
+            "timestamp": 0.4890661707405556
+        },
+        {
+            "x": 6.300865420236581,
+            "y": 1.219117786820698,
+            "heading": 0.2273229910184091,
+            "angularVelocity": 1.2269945998464997e-7,
+            "velocityX": -3.6098644906196586,
+            "velocityY": 1.131157363454394,
+            "timestamp": 0.5705771991973149
+        },
+        {
+            "x": 6.006621672014901,
+            "y": 1.3113196474914972,
+            "heading": 0.2273230010197575,
+            "angularVelocity": 1.2269932759504928e-7,
+            "velocityX": -3.6098642575436695,
+            "velocityY": 1.1311581072702415,
+            "timestamp": 0.6520882276540741
+        },
+        {
+            "x": 5.7123779465456614,
+            "y": 1.4035215807721424,
+            "heading": 0.22732301102110228,
+            "angularVelocity": 1.2269928353992546e-7,
+            "velocityX": -3.6098639784103828,
+            "velocityY": 1.131158998068056,
+            "timestamp": 0.7335992561108334
+        },
+        {
+            "x": 5.418134249112936,
+            "y": 1.4957236035255936,
+            "heading": 0.2273230210224565,
+            "angularVelocity": 1.2269939906393907e-7,
+            "velocityX": -3.60986363445063,
+            "velocityY": 1.1311600957453605,
+            "timestamp": 0.8151102845675927
+        },
+        {
+            "x": 5.123890658648026,
+            "y": 1.5879259676440571,
+            "heading": 0.22732303102385681,
+            "angularVelocity": 1.2269996504792678e-7,
+            "velocityX": -3.609862322139673,
+            "velocityY": 1.1311642837064038,
+            "timestamp": 0.896621313024352
+        },
+        {
+            "x": 4.838467298400658,
+            "y": 1.7046053225590203,
+            "heading": 0.22732304878104165,
+            "angularVelocity": 2.1785008908970575e-7,
+            "velocityX": -3.5016532811726435,
+            "velocityY": 1.4314548242617389,
+            "timestamp": 0.9781323414811113
+        },
+        {
+            "x": 4.5530441892872355,
+            "y": 1.8212852918009321,
+            "heading": 0.2273230665381978,
+            "angularVelocity": 2.1784973753126392e-7,
+            "velocityX": -3.5016502001914627,
+            "velocityY": 1.4314623609958343,
+            "timestamp": 1.0596433699378704
+        },
+        {
+            "x": 4.267621124217833,
+            "y": 1.9379653687834795,
+            "heading": 0.2273230842953322,
+            "angularVelocity": 2.1784947052459537e-7,
+            "velocityX": -3.5016496598471556,
+            "velocityY": 1.4314636827879597,
+            "timestamp": 1.1411543983946297
+        },
+        {
+            "x": 3.9821980933555543,
+            "y": 2.054645529443536,
+            "heading": 0.22732310205245027,
+            "angularVelocity": 2.1784927045604472e-7,
+            "velocityX": -3.5016492401846184,
+            "velocityY": 1.4314647093669521,
+            "timestamp": 1.222665426851389
+        },
+        {
+            "x": 3.6967750938784922,
+            "y": 2.171325766878081,
+            "heading": 0.22732311980956,
+            "angularVelocity": 2.1784916783717796e-7,
+            "velocityX": -3.501648855142053,
+            "velocityY": 1.4314656512577588,
+            "timestamp": 1.3041764553081483
+        },
+        {
+            "x": 3.411352191599502,
+            "y": 2.288006242078097,
+            "heading": 0.2273231375666713,
+            "angularVelocity": 2.178491871140625e-7,
+            "velocityX": -3.501647662689034,
+            "velocityY": 1.4314685682307748,
+            "timestamp": 1.3856874837649076
+        },
+        {
+            "x": 3.135544385824121,
+            "y": 2.425885248705818,
+            "heading": 0.22732315564878777,
+            "angularVelocity": 2.218364413735132e-7,
+            "velocityX": -3.3836869807340535,
+            "velocityY": 1.6915380561155315,
+            "timestamp": 1.4671985122216669
+        },
+        {
+            "x": 2.8888590335845947,
+            "y": 2.6108903884887695,
+            "heading": 0.22732317584620773,
+            "angularVelocity": 2.4778757367040615e-7,
+            "velocityX": -3.026404609412922,
+            "velocityY": 2.2696945834893776,
+            "timestamp": 1.5487095406784261
+        },
+        {
+            "x": 2.714424659127437,
+            "y": 2.7861164379371415,
+            "heading": 0.22732319539367835,
+            "angularVelocity": 2.990800632496739e-7,
+            "velocityX": -2.668879509458687,
+            "velocityY": 2.6809922892290525,
+            "timestamp": 1.6140681954393303
+        },
+        {
+            "x": 2.566977650329029,
+            "y": 2.984587845024715,
+            "heading": 0.2273232142228265,
+            "angularVelocity": 2.880895914276183e-7,
+            "velocityX": -2.2559676195571057,
+            "velocityY": 3.0366507360596735,
+            "timestamp": 1.6794268502002345
+        },
+        {
+            "x": 2.4495779823740462,
+            "y": 3.20218572489824,
+            "heading": 0.22732323403642601,
+            "angularVelocity": 3.0315188694924184e-7,
+            "velocityX": -1.7962375202558922,
+            "velocityY": 3.3292894517113587,
+            "timestamp": 1.7447855049611387
+        },
+        {
+            "x": 2.3646621591221844,
+            "y": 3.4343941523127826,
+            "heading": 0.22732276450933128,
+            "angularVelocity": -0.000007183855179950579,
+            "velocityX": -1.2992284428512828,
+            "velocityY": 3.5528336417572115,
+            "timestamp": 1.810144159722043
+        },
+        {
+            "x": 2.3096843159184894,
+            "y": 3.656196634862642,
+            "heading": 0.16843360905648927,
+            "angularVelocity": -0.9010154151467895,
+            "velocityX": -0.8411715847703609,
+            "velocityY": 3.3936206820850954,
+            "timestamp": 1.875502814482947
+        },
+        {
+            "x": 2.277656929328602,
+            "y": 3.853541817964372,
+            "heading": 0.0672675417750508,
+            "angularVelocity": -1.547860304829062,
+            "velocityX": -0.49002518039951914,
+            "velocityY": 3.0194192922676324,
+            "timestamp": 1.9408614692438513
+        },
+        {
+            "x": 2.265357919525011,
+            "y": 4.021569174077037,
+            "heading": -0.049806740829473055,
+            "angularVelocity": -1.791259061753831,
+            "velocityX": -0.188177217670429,
+            "velocityY": 2.570850895376378,
+            "timestamp": 2.0062201240047557
+        },
+        {
+            "x": 2.269649055536998,
+            "y": 4.158080976089645,
+            "heading": -0.1685192187929243,
+            "angularVelocity": -1.8163237661135343,
+            "velocityX": 0.06565520706762865,
+            "velocityY": 2.088656850603768,
+            "timestamp": 2.07157877876566
+        },
+        {
+            "x": 2.2810300353091644,
+            "y": 4.259951613943982,
+            "heading": -0.2719091294332062,
+            "angularVelocity": -1.581885536330179,
+            "velocityX": 0.1741311814602323,
+            "velocityY": 1.5586403702310236,
+            "timestamp": 2.1369374335265645
+        },
+        {
+            "x": 2.2926691286250658,
+            "y": 4.326989538481201,
+            "heading": -0.34849979264864867,
+            "angularVelocity": -1.1718518916219969,
+            "velocityX": 0.1780803683686499,
+            "velocityY": 1.0256931508529106,
+            "timestamp": 2.202296088287469
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.6349611616573703,
+            "velocityX": 0.11216374329844404,
+            "velocityY": 0.5050664160631704,
+            "timestamp": 2.2676547430483733
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -1.761626207104486e-30,
+            "velocityX": 0,
+            "velocityY": -1.839087679439853e-31,
+            "timestamp": 2.3330133978092777
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.1.traj
new file mode 100644
index 00000000..1818c169
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.1.traj	
@@ -0,0 +1,265 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": -1.4340376968599448e-32,
+            "velocityX": -4.9971810322519555e-29,
+            "velocityY": 8.414879145245036e-29,
+            "timestamp": 0
+        },
+        {
+            "x": 1.3816327251489127,
+            "y": 5.576194684745321,
+            "heading": 4.506697153639657e-18,
+            "angularVelocity": 6.903205799499945e-17,
+            "velocityX": 0.4845393494564494,
+            "velocityY": 0.24806468536018447,
+            "timestamp": 0.06528412023584423
+        },
+        {
+            "x": 1.4448981743168023,
+            "y": 5.608584053657482,
+            "heading": 9.944941296273304e-18,
+            "angularVelocity": 8.330117912909241e-17,
+            "velocityX": 0.9690786816049286,
+            "velocityY": 0.4961293618593833,
+            "timestamp": 0.13056824047168847
+        },
+        {
+            "x": 1.5397963458087682,
+            "y": 5.657168105868761,
+            "heading": 6.849951425963079e-18,
+            "angularVelocity": -4.74080026246192e-17,
+            "velocityX": 1.4536179877914959,
+            "velocityY": 0.7441940250671262,
+            "timestamp": 0.1958523607075327
+        },
+        {
+            "x": 1.666327236799984,
+            "y": 5.721946839932961,
+            "heading": -2.8668788112285783e-18,
+            "angularVelocity": -1.4883910439092565e-16,
+            "velocityX": 1.9381572507083278,
+            "velocityY": 0.9922586661225022,
+            "timestamp": 0.26113648094337694
+        },
+        {
+            "x": 1.8244908416408254,
+            "y": 5.802920252957701,
+            "heading": -1.6578007712929266e-17,
+            "angularVelocity": -2.1002241242045617e-16,
+            "velocityX": 2.4226964270861338,
+            "velocityY": 1.2403232628733727,
+            "timestamp": 0.32642060117922117
+        },
+        {
+            "x": 2.0142871433826555,
+            "y": 5.900088336265959,
+            "heading": -2.5925213294324244e-17,
+            "angularVelocity": -1.4317730798527664e-16,
+            "velocityX": 2.9072353438504677,
+            "velocityY": 1.4883877267125722,
+            "timestamp": 0.3917047214150654
+        },
+        {
+            "x": 2.234118517125294,
+            "y": 6.012633170077576,
+            "heading": -1.2540245641654488e-17,
+            "angularVelocity": 2.0502639235211304e-16,
+            "velocityX": 3.3673023845382315,
+            "velocityY": 1.723923572915424,
+            "timestamp": 0.45698884165090964
+        },
+        {
+            "x": 2.4539498908690707,
+            "y": 6.125178003888657,
+            "heading": 1.1802241639861868e-17,
+            "angularVelocity": 3.728699597195476e-16,
+            "velocityX": 3.3673023845556527,
+            "velocityY": 1.72392357290723,
+            "timestamp": 0.5222729618867539
+        },
+        {
+            "x": 2.6737813045614214,
+            "y": 6.237722759669026,
+            "heading": 3.649409202314283e-17,
+            "angularVelocity": 3.78221385080107e-16,
+            "velocityX": 3.367302996474358,
+            "velocityY": 1.7239223776592436,
+            "timestamp": 0.5875570821225982
+        },
+        {
+            "x": 2.896845817565918,
+            "y": 6.343715190887451,
+            "heading": 3.1275322529875387e-17,
+            "angularVelocity": -7.99393399896351e-17,
+            "velocityX": 3.416826514605052,
+            "velocityY": 1.6235560935112305,
+            "timestamp": 0.6528412023584425
+        },
+        {
+            "x": 3.2165948847884085,
+            "y": 6.4256885407325814,
+            "heading": 6.106570262583634e-20,
+            "angularVelocity": -3.577262143903813e-16,
+            "velocityX": 3.664435260812148,
+            "velocityY": 0.9394430333407913,
+            "timestamp": 0.7400985952347529
+        },
+        {
+            "x": 3.5380520485012785,
+            "y": 6.500684460821759,
+            "heading": -4.2865718547534536e-17,
+            "angularVelocity": -4.919558421997663e-16,
+            "velocityX": 3.6840106392881062,
+            "velocityY": 0.8594792672236512,
+            "timestamp": 0.8273559881110633
+        },
+        {
+            "x": 3.8595092199655934,
+            "y": 6.5756803476856875,
+            "heading": -3.198465678230611e-17,
+            "angularVelocity": 1.2470074347342177e-16,
+            "velocityX": 3.684010728122356,
+            "velocityY": 0.8594788864507628,
+            "timestamp": 0.9146133809873737
+        },
+        {
+            "x": 4.180966391429945,
+            "y": 6.6506762345494606,
+            "heading": 1.830329484329921e-18,
+            "angularVelocity": 3.8753147618851267e-16,
+            "velocityX": 3.6840107281227708,
+            "velocityY": 0.8594788864489847,
+            "timestamp": 1.001870773863684
+        },
+        {
+            "x": 4.502423562894296,
+            "y": 6.725672121413234,
+            "heading": 1.519427405027002e-17,
+            "angularVelocity": 1.531554419106218e-16,
+            "velocityX": 3.684010728122771,
+            "velocityY": 0.8594788864489847,
+            "timestamp": 1.0891281667399946
+        },
+        {
+            "x": 4.823880734358647,
+            "y": 6.800668008277006,
+            "heading": 2.800360653678013e-17,
+            "angularVelocity": 1.4679939507274274e-16,
+            "velocityX": 3.6840107281227708,
+            "velocityY": 0.8594788864489846,
+            "timestamp": 1.176385559616305
+        },
+        {
+            "x": 5.1453379058229975,
+            "y": 6.875663895140779,
+            "heading": -9.132380596349043e-18,
+            "angularVelocity": -4.2559129844470954e-16,
+            "velocityX": 3.6840107281227708,
+            "velocityY": 0.8594788864489846,
+            "timestamp": 1.2636429524926156
+        },
+        {
+            "x": 5.466795077287348,
+            "y": 6.950659782004552,
+            "heading": 1.9053427809876355e-18,
+            "angularVelocity": 1.2649613980635305e-16,
+            "velocityX": 3.6840107281227708,
+            "velocityY": 0.8594788864489846,
+            "timestamp": 1.3509003453689261
+        },
+        {
+            "x": 5.788252248751699,
+            "y": 7.025655668868324,
+            "heading": -3.1119644574102345e-18,
+            "angularVelocity": -5.750008191867264e-17,
+            "velocityX": 3.684010728122771,
+            "velocityY": 0.8594788864489846,
+            "timestamp": 1.4381577382452366
+        },
+        {
+            "x": 6.10970942021605,
+            "y": 7.100651555732097,
+            "heading": 3.013239534330982e-17,
+            "angularVelocity": 3.80991898964227e-16,
+            "velocityX": 3.6840107281227708,
+            "velocityY": 0.8594788864489846,
+            "timestamp": 1.5254151311215471
+        },
+        {
+            "x": 6.431166591680401,
+            "y": 7.17564744259587,
+            "heading": 3.192919981892433e-17,
+            "angularVelocity": 2.0592002767735807e-17,
+            "velocityX": 3.6840107281227708,
+            "velocityY": 0.8594788864489847,
+            "timestamp": 1.6126725239978577
+        },
+        {
+            "x": 6.752623763144745,
+            "y": 7.250643329459641,
+            "heading": 5.4408724078031107e-17,
+            "angularVelocity": 2.576231481446227e-16,
+            "velocityX": 3.6840107281226917,
+            "velocityY": 0.8594788864489661,
+            "timestamp": 1.6999299168741682
+        },
+        {
+            "x": 7.061749152944237,
+            "y": 7.322762214393876,
+            "heading": 4.954044927264927e-17,
+            "angularVelocity": -5.579212125566035e-17,
+            "velocityX": 3.5426842312110383,
+            "velocityY": 0.8265074460391663,
+            "timestamp": 1.7871873097504787
+        },
+        {
+            "x": 7.309049484595465,
+            "y": 7.380457326963315,
+            "heading": 3.4625344335160436e-17,
+            "angularVelocity": -1.7093228018551423e-16,
+            "velocityX": 2.834147612017054,
+            "velocityY": 0.6612060098016332,
+            "timestamp": 1.8744447026267892
+        },
+        {
+            "x": 7.49452473986686,
+            "y": 7.4237286629145345,
+            "heading": 2.1983691983745354e-17,
+            "angularVelocity": -1.448777241544276e-16,
+            "velocityX": 2.125610783882924,
+            "velocityY": 0.49590452481841046,
+            "timestamp": 1.9617020955030997
+        },
+        {
+            "x": 7.618174912681235,
+            "y": 7.452576220829731,
+            "heading": 1.0249539478821592e-17,
+            "angularVelocity": -1.3447746039961902e-16,
+            "velocityX": 1.4170738861021466,
+            "velocityY": 0.3306030235866404,
+            "timestamp": 2.04895948837941
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": -1.174632794233726e-16,
+            "velocityX": 0.7085369534980623,
+            "velocityY": 0.16530151423060047,
+            "timestamp": 2.1362168812557205
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -1.0281234266250986e-33,
+            "velocityY": 0,
+            "timestamp": 2.223474274132031
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.traj
index 1fe0cdb8..1818c169 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.traj	
@@ -4,217 +4,262 @@
             "x": 1.35,
             "y": 5.56,
             "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -2.1086675817598662e-32,
-            "velocityY": -7.052966104933725e-38,
+            "angularVelocity": -1.4340376968599448e-32,
+            "velocityX": -4.9971810322519555e-29,
+            "velocityY": 8.414879145245036e-29,
             "timestamp": 0
         },
         {
-            "x": 1.4233949672121562,
-            "y": 5.582111248416047,
-            "heading": 6.989955400155978e-19,
-            "angularVelocity": 7.290266672073875e-18,
-            "velocityX": 0.7654825584964474,
-            "velocityY": 0.23061220206204183,
-            "timestamp": 0.09588065253415788
-        },
-        {
-            "x": 1.5701848986828006,
-            "y": 5.626333744358309,
-            "heading": -5.64955528358279e-19,
-            "angularVelocity": -1.3182545571253806e-17,
-            "velocityX": 1.530965086187225,
-            "velocityY": 0.4612243948434498,
-            "timestamp": 0.19176130506831576
-        },
-        {
-            "x": 1.7903697885046006,
-            "y": 5.69266748604712,
-            "heading": -1.1328157855510893e-18,
-            "angularVelocity": -5.922574643212496e-18,
-            "velocityX": 2.2964475522666925,
-            "velocityY": 0.6918365690635988,
-            "timestamp": 0.28764195760247363
-        },
-        {
-            "x": 2.08394961895559,
-            "y": 5.781112468143492,
-            "heading": -1.2347885191323161e-17,
-            "angularVelocity": -1.1696905584501664e-16,
-            "velocityX": 3.061929833512548,
-            "velocityY": 0.9224486876000673,
-            "timestamp": 0.3835226101366315
-        },
-        {
-            "x": 2.4312425305333245,
-            "y": 5.885739258408696,
-            "heading": -1.8571615119787707e-17,
-            "angularVelocity": -6.491121789400923e-17,
-            "velocityX": 3.6221375470302584,
-            "velocityY": 1.0912190050847865,
-            "timestamp": 0.4794032626707894
-        },
-        {
-            "x": 2.7785354421111035,
-            "y": 5.990366048673914,
-            "heading": -2.5105020741934312e-17,
-            "angularVelocity": -6.814102166319416e-17,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849244,
-            "timestamp": 0.5752839152049473
-        },
-        {
-            "x": 3.125828353688883,
-            "y": 6.094992838939131,
-            "heading": -3.974939977960392e-17,
-            "angularVelocity": -1.527354961675144e-16,
-            "velocityX": 3.622137547030716,
-            "velocityY": 1.0912190050849244,
-            "timestamp": 0.6711645677391052
-        },
-        {
-            "x": 3.473121265266662,
-            "y": 6.199619629204348,
-            "heading": -4.6160884572564577e-17,
-            "angularVelocity": -6.68694321901595e-17,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849242,
-            "timestamp": 0.767045220273263
-        },
-        {
-            "x": 3.8204141768444413,
-            "y": 6.304246419469565,
-            "heading": -5.207468497060529e-17,
-            "angularVelocity": -6.167876669314405e-17,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849244,
-            "timestamp": 0.862925872807421
-        },
-        {
-            "x": 4.16770708842222,
-            "y": 6.408873209734782,
-            "heading": -3.0597254964935646e-17,
-            "angularVelocity": 2.24001708769928e-16,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849242,
-            "timestamp": 0.9588065253415788
-        },
-        {
-            "x": 4.515,
-            "y": 6.5135,
-            "heading": -2.0783137065340813e-18,
-            "angularVelocity": 2.974420855995074e-16,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849244,
-            "timestamp": 1.0546871778757367
-        },
-        {
-            "x": 4.862292911577779,
-            "y": 6.618126790265217,
-            "heading": -2.7789619178473824e-17,
-            "angularVelocity": -2.681594752696605e-16,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849244,
-            "timestamp": 1.1505678304098945
-        },
-        {
-            "x": 5.209585823155558,
-            "y": 6.722753580530434,
-            "heading": 2.2712942266126744e-18,
-            "angularVelocity": 3.135242889004865e-16,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849242,
-            "timestamp": 1.2464484829440523
-        },
-        {
-            "x": 5.5568787347333375,
-            "y": 6.827380370795651,
-            "heading": 3.7041174502192387e-17,
-            "angularVelocity": 3.6263708429820527e-16,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849244,
-            "timestamp": 1.34232913547821
-        },
-        {
-            "x": 5.9041716463111165,
-            "y": 6.932007161060868,
-            "heading": 6.494346998398645e-17,
-            "angularVelocity": 2.910106965725311e-16,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849244,
-            "timestamp": 1.4382097880123679
-        },
-        {
-            "x": 6.251464557888895,
-            "y": 7.036633951326086,
-            "heading": 5.759102638783417e-17,
-            "angularVelocity": -7.668328700140211e-17,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849244,
-            "timestamp": 1.5340904405465257
-        },
-        {
-            "x": 6.598757469466675,
-            "y": 7.141260741591303,
-            "heading": 2.5440135069531877e-17,
-            "angularVelocity": -3.353219911269207e-16,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849244,
-            "timestamp": 1.6299710930806834
-        },
-        {
-            "x": 6.94605038104441,
-            "y": 7.245887531856507,
-            "heading": 2.2711688502269004e-17,
-            "angularVelocity": -2.84566958514003e-17,
-            "velocityX": 3.6221375470302584,
-            "velocityY": 1.0912190050847868,
-            "timestamp": 1.7258517456148412
-        },
-        {
-            "x": 7.239630211495399,
-            "y": 7.334332513952879,
-            "heading": 1.1340524791180054e-17,
-            "angularVelocity": -1.1859706178712186e-16,
-            "velocityX": 3.061929833512548,
-            "velocityY": 0.9224486876000674,
-            "timestamp": 1.821732398148999
-        },
-        {
-            "x": 7.4598151013172,
-            "y": 7.40066625564169,
-            "heading": 5.896173994762203e-18,
-            "angularVelocity": -5.67825796795303e-17,
-            "velocityX": 2.2964475522666925,
-            "velocityY": 0.6918365690635988,
-            "timestamp": 1.9176130506831568
-        },
-        {
-            "x": 7.606605032787844,
-            "y": 7.444888751583952,
-            "heading": 1.9652682638036787e-18,
-            "angularVelocity": -4.099790316709085e-17,
-            "velocityX": 1.530965086187225,
-            "velocityY": 0.46122439484344985,
-            "timestamp": 2.0134937032173146
+            "x": 1.3816327251489127,
+            "y": 5.576194684745321,
+            "heading": 4.506697153639657e-18,
+            "angularVelocity": 6.903205799499945e-17,
+            "velocityX": 0.4845393494564494,
+            "velocityY": 0.24806468536018447,
+            "timestamp": 0.06528412023584423
+        },
+        {
+            "x": 1.4448981743168023,
+            "y": 5.608584053657482,
+            "heading": 9.944941296273304e-18,
+            "angularVelocity": 8.330117912909241e-17,
+            "velocityX": 0.9690786816049286,
+            "velocityY": 0.4961293618593833,
+            "timestamp": 0.13056824047168847
+        },
+        {
+            "x": 1.5397963458087682,
+            "y": 5.657168105868761,
+            "heading": 6.849951425963079e-18,
+            "angularVelocity": -4.74080026246192e-17,
+            "velocityX": 1.4536179877914959,
+            "velocityY": 0.7441940250671262,
+            "timestamp": 0.1958523607075327
+        },
+        {
+            "x": 1.666327236799984,
+            "y": 5.721946839932961,
+            "heading": -2.8668788112285783e-18,
+            "angularVelocity": -1.4883910439092565e-16,
+            "velocityX": 1.9381572507083278,
+            "velocityY": 0.9922586661225022,
+            "timestamp": 0.26113648094337694
+        },
+        {
+            "x": 1.8244908416408254,
+            "y": 5.802920252957701,
+            "heading": -1.6578007712929266e-17,
+            "angularVelocity": -2.1002241242045617e-16,
+            "velocityX": 2.4226964270861338,
+            "velocityY": 1.2403232628733727,
+            "timestamp": 0.32642060117922117
+        },
+        {
+            "x": 2.0142871433826555,
+            "y": 5.900088336265959,
+            "heading": -2.5925213294324244e-17,
+            "angularVelocity": -1.4317730798527664e-16,
+            "velocityX": 2.9072353438504677,
+            "velocityY": 1.4883877267125722,
+            "timestamp": 0.3917047214150654
+        },
+        {
+            "x": 2.234118517125294,
+            "y": 6.012633170077576,
+            "heading": -1.2540245641654488e-17,
+            "angularVelocity": 2.0502639235211304e-16,
+            "velocityX": 3.3673023845382315,
+            "velocityY": 1.723923572915424,
+            "timestamp": 0.45698884165090964
+        },
+        {
+            "x": 2.4539498908690707,
+            "y": 6.125178003888657,
+            "heading": 1.1802241639861868e-17,
+            "angularVelocity": 3.728699597195476e-16,
+            "velocityX": 3.3673023845556527,
+            "velocityY": 1.72392357290723,
+            "timestamp": 0.5222729618867539
+        },
+        {
+            "x": 2.6737813045614214,
+            "y": 6.237722759669026,
+            "heading": 3.649409202314283e-17,
+            "angularVelocity": 3.78221385080107e-16,
+            "velocityX": 3.367302996474358,
+            "velocityY": 1.7239223776592436,
+            "timestamp": 0.5875570821225982
+        },
+        {
+            "x": 2.896845817565918,
+            "y": 6.343715190887451,
+            "heading": 3.1275322529875387e-17,
+            "angularVelocity": -7.99393399896351e-17,
+            "velocityX": 3.416826514605052,
+            "velocityY": 1.6235560935112305,
+            "timestamp": 0.6528412023584425
+        },
+        {
+            "x": 3.2165948847884085,
+            "y": 6.4256885407325814,
+            "heading": 6.106570262583634e-20,
+            "angularVelocity": -3.577262143903813e-16,
+            "velocityX": 3.664435260812148,
+            "velocityY": 0.9394430333407913,
+            "timestamp": 0.7400985952347529
+        },
+        {
+            "x": 3.5380520485012785,
+            "y": 6.500684460821759,
+            "heading": -4.2865718547534536e-17,
+            "angularVelocity": -4.919558421997663e-16,
+            "velocityX": 3.6840106392881062,
+            "velocityY": 0.8594792672236512,
+            "timestamp": 0.8273559881110633
+        },
+        {
+            "x": 3.8595092199655934,
+            "y": 6.5756803476856875,
+            "heading": -3.198465678230611e-17,
+            "angularVelocity": 1.2470074347342177e-16,
+            "velocityX": 3.684010728122356,
+            "velocityY": 0.8594788864507628,
+            "timestamp": 0.9146133809873737
+        },
+        {
+            "x": 4.180966391429945,
+            "y": 6.6506762345494606,
+            "heading": 1.830329484329921e-18,
+            "angularVelocity": 3.8753147618851267e-16,
+            "velocityX": 3.6840107281227708,
+            "velocityY": 0.8594788864489847,
+            "timestamp": 1.001870773863684
+        },
+        {
+            "x": 4.502423562894296,
+            "y": 6.725672121413234,
+            "heading": 1.519427405027002e-17,
+            "angularVelocity": 1.531554419106218e-16,
+            "velocityX": 3.684010728122771,
+            "velocityY": 0.8594788864489847,
+            "timestamp": 1.0891281667399946
+        },
+        {
+            "x": 4.823880734358647,
+            "y": 6.800668008277006,
+            "heading": 2.800360653678013e-17,
+            "angularVelocity": 1.4679939507274274e-16,
+            "velocityX": 3.6840107281227708,
+            "velocityY": 0.8594788864489846,
+            "timestamp": 1.176385559616305
+        },
+        {
+            "x": 5.1453379058229975,
+            "y": 6.875663895140779,
+            "heading": -9.132380596349043e-18,
+            "angularVelocity": -4.2559129844470954e-16,
+            "velocityX": 3.6840107281227708,
+            "velocityY": 0.8594788864489846,
+            "timestamp": 1.2636429524926156
+        },
+        {
+            "x": 5.466795077287348,
+            "y": 6.950659782004552,
+            "heading": 1.9053427809876355e-18,
+            "angularVelocity": 1.2649613980635305e-16,
+            "velocityX": 3.6840107281227708,
+            "velocityY": 0.8594788864489846,
+            "timestamp": 1.3509003453689261
+        },
+        {
+            "x": 5.788252248751699,
+            "y": 7.025655668868324,
+            "heading": -3.1119644574102345e-18,
+            "angularVelocity": -5.750008191867264e-17,
+            "velocityX": 3.684010728122771,
+            "velocityY": 0.8594788864489846,
+            "timestamp": 1.4381577382452366
+        },
+        {
+            "x": 6.10970942021605,
+            "y": 7.100651555732097,
+            "heading": 3.013239534330982e-17,
+            "angularVelocity": 3.80991898964227e-16,
+            "velocityX": 3.6840107281227708,
+            "velocityY": 0.8594788864489846,
+            "timestamp": 1.5254151311215471
+        },
+        {
+            "x": 6.431166591680401,
+            "y": 7.17564744259587,
+            "heading": 3.192919981892433e-17,
+            "angularVelocity": 2.0592002767735807e-17,
+            "velocityX": 3.6840107281227708,
+            "velocityY": 0.8594788864489847,
+            "timestamp": 1.6126725239978577
+        },
+        {
+            "x": 6.752623763144745,
+            "y": 7.250643329459641,
+            "heading": 5.4408724078031107e-17,
+            "angularVelocity": 2.576231481446227e-16,
+            "velocityX": 3.6840107281226917,
+            "velocityY": 0.8594788864489661,
+            "timestamp": 1.6999299168741682
+        },
+        {
+            "x": 7.061749152944237,
+            "y": 7.322762214393876,
+            "heading": 4.954044927264927e-17,
+            "angularVelocity": -5.579212125566035e-17,
+            "velocityX": 3.5426842312110383,
+            "velocityY": 0.8265074460391663,
+            "timestamp": 1.7871873097504787
+        },
+        {
+            "x": 7.309049484595465,
+            "y": 7.380457326963315,
+            "heading": 3.4625344335160436e-17,
+            "angularVelocity": -1.7093228018551423e-16,
+            "velocityX": 2.834147612017054,
+            "velocityY": 0.6612060098016332,
+            "timestamp": 1.8744447026267892
+        },
+        {
+            "x": 7.49452473986686,
+            "y": 7.4237286629145345,
+            "heading": 2.1983691983745354e-17,
+            "angularVelocity": -1.448777241544276e-16,
+            "velocityX": 2.125610783882924,
+            "velocityY": 0.49590452481841046,
+            "timestamp": 1.9617020955030997
+        },
+        {
+            "x": 7.618174912681235,
+            "y": 7.452576220829731,
+            "heading": 1.0249539478821592e-17,
+            "angularVelocity": -1.3447746039961902e-16,
+            "velocityX": 1.4170738861021466,
+            "velocityY": 0.3306030235866404,
+            "timestamp": 2.04895948837941
         },
         {
             "x": 7.68,
             "y": 7.467,
             "heading": 0,
-            "angularVelocity": -2.0497026255693862e-17,
-            "velocityX": 0.7654825584964475,
-            "velocityY": 0.23061220206204183,
-            "timestamp": 2.1093743557514726
+            "angularVelocity": -1.174632794233726e-16,
+            "velocityX": 0.7085369534980623,
+            "velocityY": 0.16530151423060047,
+            "timestamp": 2.1362168812557205
         },
         {
             "x": 7.68,
             "y": 7.467,
             "heading": 0,
             "angularVelocity": 0,
-            "velocityX": 2.8084467268312453e-34,
+            "velocityX": -1.0281234266250986e-33,
             "velocityY": 0,
-            "timestamp": 2.2052550082856306
+            "timestamp": 2.223474274132031
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.1.traj
new file mode 100644
index 00000000..51c46220
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.1.traj	
@@ -0,0 +1,265 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": -1.3272791352480801e-30,
+            "velocityX": -6.138871086325045e-33,
+            "velocityY": -8.954734510196666e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 1.3835743361511121,
+            "y": 5.575814713247826,
+            "heading": -0.005199936040550204,
+            "angularVelocity": -0.07791207752010428,
+            "velocityX": 0.5030535492153302,
+            "velocityY": 0.23695621540617479,
+            "timestamp": 0.06674107796969504
+        },
+        {
+            "x": 1.4507230090250494,
+            "y": 5.607444140582854,
+            "heading": -0.015599510401707435,
+            "angularVelocity": -0.15581969422009245,
+            "velocityX": 1.0061071069967922,
+            "velocityY": 0.4739124433888902,
+            "timestamp": 0.13348215593939008
+        },
+        {
+            "x": 1.5514460199136038,
+            "y": 5.654888284136176,
+            "heading": -0.03119820909467289,
+            "angularVelocity": -0.23371960968398373,
+            "velocityX": 1.5091606841335288,
+            "velocityY": 0.7108687033024069,
+            "timestamp": 0.20022323390908514
+        },
+        {
+            "x": 1.6857433700165965,
+            "y": 5.718147147394209,
+            "heading": -0.051995311102797384,
+            "angularVelocity": -0.3116087219563307,
+            "velocityX": 2.0122142792475226,
+            "velocityY": 0.9478250154538677,
+            "timestamp": 0.26696431187878017
+        },
+        {
+            "x": 1.853615058416753,
+            "y": 5.79722073482494,
+            "heading": -0.07798989994858517,
+            "angularVelocity": -0.3894841023932979,
+            "velocityX": 2.515267860617741,
+            "velocityY": 1.184781394550373,
+            "timestamp": 0.3337053898484752
+        },
+        {
+            "x": 2.0550610733526136,
+            "y": 5.892109048523229,
+            "heading": -0.10918084380142022,
+            "angularVelocity": -0.4673425242996203,
+            "velocityX": 3.0183212657627503,
+            "velocityY": 1.4217378050347718,
+            "timestamp": 0.4004464678181702
+        },
+        {
+            "x": 2.283467758752316,
+            "y": 5.999697539336407,
+            "heading": -0.1091808488901602,
+            "angularVelocity": -7.624599630551263e-8,
+            "velocityX": 3.422280435797193,
+            "velocityY": 1.6120280655645223,
+            "timestamp": 0.46718754578786525
+        },
+        {
+            "x": 2.511874447545338,
+            "y": 6.107286022946165,
+            "heading": -0.10918085397868219,
+            "angularVelocity": -7.624272995870229e-8,
+            "velocityX": 3.4222804866402323,
+            "velocityY": 1.6120279576336907,
+            "timestamp": 0.5339286237575603
+        },
+        {
+            "x": 2.7402812851669234,
+            "y": 6.214874190597382,
+            "heading": -0.10918085906719958,
+            "angularVelocity": -7.624266119750657e-8,
+            "velocityX": 3.4222827165797036,
+            "velocityY": 1.612023223539615,
+            "timestamp": 0.6006697017272554
+        },
+        {
+            "x": 2.976557731628418,
+            "y": 6.303859233856201,
+            "heading": -0.10918086426900071,
+            "angularVelocity": -7.794002274324629e-8,
+            "velocityX": 3.5401952388119984,
+            "velocityY": 1.3332874740084952,
+            "timestamp": 0.6674107796969505
+        },
+        {
+            "x": 3.298504794987031,
+            "y": 6.35898613680169,
+            "heading": -0.1091808690742104,
+            "angularVelocity": -5.565219068812679e-8,
+            "velocityX": 3.7286737752062145,
+            "velocityY": 0.6384597367556102,
+            "timestamp": 0.753754365316963
+        },
+        {
+            "x": 3.623164348220099,
+            "y": 6.394833894219333,
+            "heading": -0.10918087380844538,
+            "angularVelocity": -5.483018746401896e-8,
+            "velocityX": 3.760088846227146,
+            "velocityY": 0.4151756863029092,
+            "timestamp": 0.8400979509369756
+        },
+        {
+            "x": 3.94782391548866,
+            "y": 6.430681524522641,
+            "heading": -0.10918087854267373,
+            "angularVelocity": -5.483011064647812e-8,
+            "velocityX": 3.7600890087811236,
+            "velocityY": 0.41517421411094135,
+            "timestamp": 0.9264415365569881
+        },
+        {
+            "x": 4.272483486014202,
+            "y": 6.466529125328635,
+            "heading": -0.10918088327689503,
+            "angularVelocity": -5.483002882633892e-8,
+            "velocityX": 3.7600890465022867,
+            "velocityY": 0.4151738724837589,
+            "timestamp": 1.0127851221770008
+        },
+        {
+            "x": 4.597143061517746,
+            "y": 6.502376681050562,
+            "heading": -0.10918088801110931,
+            "angularVelocity": -5.482994771911856e-8,
+            "velocityX": 3.760089104155704,
+            "velocityY": 0.4151733503364993,
+            "timestamp": 1.0991287077970133
+        },
+        {
+            "x": 4.921802670469747,
+            "y": 6.538223933839528,
+            "heading": -0.10918089274532473,
+            "angularVelocity": -5.482996090104271e-8,
+            "velocityX": 3.76008949154355,
+            "velocityY": 0.41516984187713296,
+            "timestamp": 1.1854722934170259
+        },
+        {
+            "x": 5.24839709757225,
+            "y": 6.543220677288062,
+            "heading": -0.10918090874246278,
+            "angularVelocity": -1.8527303358226695e-7,
+            "velocityX": 3.782497851546312,
+            "velocityY": 0.05787046498769136,
+            "timestamp": 1.2718158790370384
+        },
+        {
+            "x": 5.573089202193745,
+            "y": 6.507668986743235,
+            "heading": -0.10918094712419395,
+            "angularVelocity": -4.4452324855688095e-7,
+            "velocityX": 3.760465844566904,
+            "velocityY": -0.4117467474802974,
+            "timestamp": 1.358159464657051
+        },
+        {
+            "x": 5.897772644305168,
+            "y": 6.472038270745821,
+            "heading": -0.10918098550925279,
+            "angularVelocity": -4.4456178841484606e-7,
+            "velocityX": 3.76036551852636,
+            "velocityY": -0.4126619915255808,
+            "timestamp": 1.4445030502770635
+        },
+        {
+            "x": 6.218673539970231,
+            "y": 6.411116178321686,
+            "heading": -0.10918101517811797,
+            "angularVelocity": -3.436140042414391e-7,
+            "velocityX": 3.716557441537206,
+            "velocityY": -0.7055775132184731,
+            "timestamp": 1.530846635897076
+        },
+        {
+            "x": 6.522221927429097,
+            "y": 6.290504141989104,
+            "heading": -0.10918101099769996,
+            "angularVelocity": 4.8416080721557094e-8,
+            "velocityX": 3.5155870037033834,
+            "velocityY": -1.3968847305389978,
+            "timestamp": 1.6171902215170886
+        },
+        {
+            "x": 6.822696463094792,
+            "y": 6.162426273800445,
+            "heading": -0.10918100683974102,
+            "angularVelocity": 4.8155967911031915e-8,
+            "velocityX": 3.479986770390183,
+            "velocityY": -1.4833512792984263,
+            "timestamp": 1.7035338071371011
+        },
+        {
+            "x": 7.1084642887127405,
+            "y": 6.040617535278895,
+            "heading": -0.0727866812513063,
+            "angularVelocity": 0.42150584003543307,
+            "velocityX": 3.309659004382518,
+            "velocityY": -1.4107444999749623,
+            "timestamp": 1.7898773927571137
+        },
+        {
+            "x": 7.337078567452098,
+            "y": 5.943170529564944,
+            "heading": -0.043671737033755105,
+            "angularVelocity": 0.33719869297161775,
+            "velocityX": 2.6477274148129606,
+            "velocityY": -1.1285957725082563,
+            "timestamp": 1.8762209783771262
+        },
+        {
+            "x": 7.508539282574383,
+            "y": 5.870085268029055,
+            "heading": -0.021835852662427283,
+            "angularVelocity": 0.2528952696894573,
+            "velocityX": 1.985795631384405,
+            "velocityY": -0.8464469133529862,
+            "timestamp": 1.9625645639971387
+        },
+        {
+            "x": 7.622846427592322,
+            "y": 5.821361756830362,
+            "heading": -0.007278657657073996,
+            "angularVelocity": 0.16859613717477176,
+            "velocityX": 1.323863772822564,
+            "velocityY": -0.564297982864862,
+            "timestamp": 2.0489081496171515
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 0.08429876527373403,
+            "velocityX": 0.6619318852381739,
+            "velocityY": -0.282149005689615,
+            "timestamp": 2.1352517352371643
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -1.8142028796058936e-31,
+            "velocityY": -1.068352585526189e-31,
+            "timestamp": 2.221595320857177
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.traj
index a76ce3f5..51c46220 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.traj	
@@ -4,208 +4,262 @@
             "x": 1.35,
             "y": 5.56,
             "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 4.006684802425094e-34,
-            "velocityY": 0,
+            "angularVelocity": -1.3272791352480801e-30,
+            "velocityX": -6.138871086325045e-33,
+            "velocityY": -8.954734510196666e-33,
             "timestamp": 0
         },
         {
-            "x": 1.4307759237486215,
-            "y": 5.581186236587336,
-            "heading": -0.002104963011526108,
-            "angularVelocity": -0.021033383553861286,
-            "velocityX": 0.8071357913745165,
-            "velocityY": 0.21169884590096585,
-            "timestamp": 0.1000772417873624
-        },
-        {
-            "x": 1.592327768448065,
-            "y": 5.623558708255768,
-            "heading": -0.006314883423264977,
-            "angularVelocity": -0.04206671103789844,
-            "velocityX": 1.6142715547926287,
-            "velocityY": 0.42339767675114676,
-            "timestamp": 0.2001544835747248
-        },
-        {
-            "x": 1.8346555283201378,
-            "y": 5.6871174126709505,
-            "heading": -0.012629752552513055,
-            "angularVelocity": -0.06309995176191506,
-            "velocityX": 2.4214072604734147,
-            "velocityY": 0.6350964842758995,
-            "timestamp": 0.3002317253620872
-        },
-        {
-            "x": 2.1577591856288034,
-            "y": 5.77186234429474,
-            "heading": -0.02104955396970995,
-            "angularVelocity": -0.08413302831713466,
-            "velocityX": 3.228542788930726,
-            "velocityY": 0.8467952364619561,
-            "timestamp": 0.4003089671494496
-        },
-        {
-            "x": 2.5239589562380984,
-            "y": 5.867910684343787,
-            "heading": -0.021049554851793276,
-            "angularVelocity": -8.814025154574047e-9,
-            "velocityX": 3.6591712967806616,
-            "velocityY": 0.9597420785549242,
-            "timestamp": 0.500386208936812
-        },
-        {
-            "x": 2.8901587297210503,
-            "y": 5.963959013436644,
-            "heading": -0.021049555733868682,
-            "angularVelocity": -8.813945991956408e-9,
-            "velocityX": 3.659171325495056,
-            "velocityY": 0.9597419690775807,
-            "timestamp": 0.6004634507241744
-        },
-        {
-            "x": 3.2563585069426413,
-            "y": 6.060007328275347,
-            "heading": -0.02104955661593894,
-            "angularVelocity": -8.813894597525165e-9,
-            "velocityX": 3.659171362852588,
-            "velocityY": 0.9597418266460623,
-            "timestamp": 0.7005406925115368
-        },
-        {
-            "x": 3.622558289188601,
-            "y": 6.15605562395784,
-            "heading": -0.021049557498003604,
-            "angularVelocity": -8.813838677794314e-9,
-            "velocityX": 3.6591714130574977,
-            "velocityY": 0.9597416352318217,
-            "timestamp": 0.8006179342988993
-        },
-        {
-            "x": 3.9887580784892815,
-            "y": 6.252103892743073,
-            "heading": -0.02104955838006166,
-            "angularVelocity": -8.81377261667878e-9,
-            "velocityX": 3.659171483550255,
-            "velocityY": 0.9597413664668089,
-            "timestamp": 0.9006951760862617
-        },
-        {
-            "x": 4.354957878489299,
-            "y": 6.3481521207353335,
-            "heading": -0.021049559262112345,
-            "angularVelocity": -8.813699036774153e-9,
-            "velocityX": 3.6591715904610522,
-            "velocityY": 0.9597409588519333,
-            "timestamp": 1.0007724178736241
-        },
-        {
-            "x": 4.721157716449094,
-            "y": 6.444200203999558,
-            "heading": -0.021049560144156942,
-            "angularVelocity": -8.81363814954091e-9,
-            "velocityX": 3.659171969765827,
-            "velocityY": 0.9597395126886197,
-            "timestamp": 1.1008496596609865
-        },
-        {
-            "x": 5.092065894328832,
-            "y": 6.520059807846628,
-            "heading": -0.02104956519242719,
-            "angularVelocity": -5.0443738888274115e-8,
-            "velocityX": 3.70621902897584,
-            "velocityY": 0.7580105375830706,
-            "timestamp": 1.2009269014483488
-        },
-        {
-            "x": 5.47058189966855,
-            "y": 6.512767035208857,
-            "heading": -0.021049620221549888,
-            "angularVelocity": -5.498665002090286e-7,
-            "velocityX": 3.7822385847120277,
-            "velocityY": -0.07287143917560433,
-            "timestamp": 1.3010041432357111
-        },
-        {
-            "x": 5.849069453983326,
-            "y": 6.50412284194313,
-            "heading": -0.02104967520004647,
-            "angularVelocity": -5.493606298291521e-7,
-            "velocityX": 3.7819542940538056,
-            "velocityY": -0.08637521489745259,
-            "timestamp": 1.4010813850230734
-        },
-        {
-            "x": 6.2213754396920145,
-            "y": 6.435451104158958,
-            "heading": -0.021049699334872447,
-            "angularVelocity": -2.4116198195152956e-7,
-            "velocityX": 3.720186318681132,
-            "velocityY": -0.6861873544644725,
-            "timestamp": 1.5011586268104358
-        },
-        {
-            "x": 6.5695694546238315,
-            "y": 6.286829159405189,
-            "heading": -0.02104969841623417,
-            "angularVelocity": 9.179292491243133e-9,
-            "velocityX": 3.4792527123362977,
-            "velocityY": -1.4850723511100814,
-            "timestamp": 1.601235868597798
-        },
-        {
-            "x": 6.915952468622566,
-            "y": 6.134033995374984,
-            "heading": -0.021049697500180466,
-            "angularVelocity": 9.153466759383175e-9,
-            "velocityX": 3.4611566807037675,
-            "velocityY": -1.5267723340622583,
-            "timestamp": 1.7013131103851604
-        },
-        {
-            "x": 7.221571466882516,
-            "y": 5.999220411113471,
-            "heading": -0.012629808861323527,
-            "angularVelocity": 0.0841338998605394,
-            "velocityX": 3.053831148837094,
-            "velocityY": -1.347095322110863,
-            "timestamp": 1.8013903521725227
-        },
-        {
-            "x": 7.450785730315982,
-            "y": 5.898110210460681,
-            "heading": -0.0063148981970481905,
-            "angularVelocity": 0.0631003667916113,
-            "velocityX": 2.290373508899117,
-            "velocityY": -1.0103216160535444,
-            "timestamp": 1.901467593959885
-        },
-        {
-            "x": 7.603595242863294,
-            "y": 5.830703404850431,
-            "heading": -0.0021049639599730564,
-            "angularVelocity": 0.04206684918455414,
-            "velocityX": 1.5269157084883698,
-            "velocityY": -0.673547795746317,
-            "timestamp": 2.0015448357472474
+            "x": 1.3835743361511121,
+            "y": 5.575814713247826,
+            "heading": -0.005199936040550204,
+            "angularVelocity": -0.07791207752010428,
+            "velocityX": 0.5030535492153302,
+            "velocityY": 0.23695621540617479,
+            "timestamp": 0.06674107796969504
+        },
+        {
+            "x": 1.4507230090250494,
+            "y": 5.607444140582854,
+            "heading": -0.015599510401707435,
+            "angularVelocity": -0.15581969422009245,
+            "velocityX": 1.0061071069967922,
+            "velocityY": 0.4739124433888902,
+            "timestamp": 0.13348215593939008
+        },
+        {
+            "x": 1.5514460199136038,
+            "y": 5.654888284136176,
+            "heading": -0.03119820909467289,
+            "angularVelocity": -0.23371960968398373,
+            "velocityX": 1.5091606841335288,
+            "velocityY": 0.7108687033024069,
+            "timestamp": 0.20022323390908514
+        },
+        {
+            "x": 1.6857433700165965,
+            "y": 5.718147147394209,
+            "heading": -0.051995311102797384,
+            "angularVelocity": -0.3116087219563307,
+            "velocityX": 2.0122142792475226,
+            "velocityY": 0.9478250154538677,
+            "timestamp": 0.26696431187878017
+        },
+        {
+            "x": 1.853615058416753,
+            "y": 5.79722073482494,
+            "heading": -0.07798989994858517,
+            "angularVelocity": -0.3894841023932979,
+            "velocityX": 2.515267860617741,
+            "velocityY": 1.184781394550373,
+            "timestamp": 0.3337053898484752
+        },
+        {
+            "x": 2.0550610733526136,
+            "y": 5.892109048523229,
+            "heading": -0.10918084380142022,
+            "angularVelocity": -0.4673425242996203,
+            "velocityX": 3.0183212657627503,
+            "velocityY": 1.4217378050347718,
+            "timestamp": 0.4004464678181702
+        },
+        {
+            "x": 2.283467758752316,
+            "y": 5.999697539336407,
+            "heading": -0.1091808488901602,
+            "angularVelocity": -7.624599630551263e-8,
+            "velocityX": 3.422280435797193,
+            "velocityY": 1.6120280655645223,
+            "timestamp": 0.46718754578786525
+        },
+        {
+            "x": 2.511874447545338,
+            "y": 6.107286022946165,
+            "heading": -0.10918085397868219,
+            "angularVelocity": -7.624272995870229e-8,
+            "velocityX": 3.4222804866402323,
+            "velocityY": 1.6120279576336907,
+            "timestamp": 0.5339286237575603
+        },
+        {
+            "x": 2.7402812851669234,
+            "y": 6.214874190597382,
+            "heading": -0.10918085906719958,
+            "angularVelocity": -7.624266119750657e-8,
+            "velocityX": 3.4222827165797036,
+            "velocityY": 1.612023223539615,
+            "timestamp": 0.6006697017272554
+        },
+        {
+            "x": 2.976557731628418,
+            "y": 6.303859233856201,
+            "heading": -0.10918086426900071,
+            "angularVelocity": -7.794002274324629e-8,
+            "velocityX": 3.5401952388119984,
+            "velocityY": 1.3332874740084952,
+            "timestamp": 0.6674107796969505
+        },
+        {
+            "x": 3.298504794987031,
+            "y": 6.35898613680169,
+            "heading": -0.1091808690742104,
+            "angularVelocity": -5.565219068812679e-8,
+            "velocityX": 3.7286737752062145,
+            "velocityY": 0.6384597367556102,
+            "timestamp": 0.753754365316963
+        },
+        {
+            "x": 3.623164348220099,
+            "y": 6.394833894219333,
+            "heading": -0.10918087380844538,
+            "angularVelocity": -5.483018746401896e-8,
+            "velocityX": 3.760088846227146,
+            "velocityY": 0.4151756863029092,
+            "timestamp": 0.8400979509369756
+        },
+        {
+            "x": 3.94782391548866,
+            "y": 6.430681524522641,
+            "heading": -0.10918087854267373,
+            "angularVelocity": -5.483011064647812e-8,
+            "velocityX": 3.7600890087811236,
+            "velocityY": 0.41517421411094135,
+            "timestamp": 0.9264415365569881
+        },
+        {
+            "x": 4.272483486014202,
+            "y": 6.466529125328635,
+            "heading": -0.10918088327689503,
+            "angularVelocity": -5.483002882633892e-8,
+            "velocityX": 3.7600890465022867,
+            "velocityY": 0.4151738724837589,
+            "timestamp": 1.0127851221770008
+        },
+        {
+            "x": 4.597143061517746,
+            "y": 6.502376681050562,
+            "heading": -0.10918088801110931,
+            "angularVelocity": -5.482994771911856e-8,
+            "velocityX": 3.760089104155704,
+            "velocityY": 0.4151733503364993,
+            "timestamp": 1.0991287077970133
+        },
+        {
+            "x": 4.921802670469747,
+            "y": 6.538223933839528,
+            "heading": -0.10918089274532473,
+            "angularVelocity": -5.482996090104271e-8,
+            "velocityX": 3.76008949154355,
+            "velocityY": 0.41516984187713296,
+            "timestamp": 1.1854722934170259
+        },
+        {
+            "x": 5.24839709757225,
+            "y": 6.543220677288062,
+            "heading": -0.10918090874246278,
+            "angularVelocity": -1.8527303358226695e-7,
+            "velocityX": 3.782497851546312,
+            "velocityY": 0.05787046498769136,
+            "timestamp": 1.2718158790370384
+        },
+        {
+            "x": 5.573089202193745,
+            "y": 6.507668986743235,
+            "heading": -0.10918094712419395,
+            "angularVelocity": -4.4452324855688095e-7,
+            "velocityX": 3.760465844566904,
+            "velocityY": -0.4117467474802974,
+            "timestamp": 1.358159464657051
+        },
+        {
+            "x": 5.897772644305168,
+            "y": 6.472038270745821,
+            "heading": -0.10918098550925279,
+            "angularVelocity": -4.4456178841484606e-7,
+            "velocityX": 3.76036551852636,
+            "velocityY": -0.4126619915255808,
+            "timestamp": 1.4445030502770635
+        },
+        {
+            "x": 6.218673539970231,
+            "y": 6.411116178321686,
+            "heading": -0.10918101517811797,
+            "angularVelocity": -3.436140042414391e-7,
+            "velocityX": 3.716557441537206,
+            "velocityY": -0.7055775132184731,
+            "timestamp": 1.530846635897076
+        },
+        {
+            "x": 6.522221927429097,
+            "y": 6.290504141989104,
+            "heading": -0.10918101099769996,
+            "angularVelocity": 4.8416080721557094e-8,
+            "velocityX": 3.5155870037033834,
+            "velocityY": -1.3968847305389978,
+            "timestamp": 1.6171902215170886
+        },
+        {
+            "x": 6.822696463094792,
+            "y": 6.162426273800445,
+            "heading": -0.10918100683974102,
+            "angularVelocity": 4.8155967911031915e-8,
+            "velocityX": 3.479986770390183,
+            "velocityY": -1.4833512792984263,
+            "timestamp": 1.7035338071371011
+        },
+        {
+            "x": 7.1084642887127405,
+            "y": 6.040617535278895,
+            "heading": -0.0727866812513063,
+            "angularVelocity": 0.42150584003543307,
+            "velocityX": 3.309659004382518,
+            "velocityY": -1.4107444999749623,
+            "timestamp": 1.7898773927571137
+        },
+        {
+            "x": 7.337078567452098,
+            "y": 5.943170529564944,
+            "heading": -0.043671737033755105,
+            "angularVelocity": 0.33719869297161775,
+            "velocityX": 2.6477274148129606,
+            "velocityY": -1.1285957725082563,
+            "timestamp": 1.8762209783771262
+        },
+        {
+            "x": 7.508539282574383,
+            "y": 5.870085268029055,
+            "heading": -0.021835852662427283,
+            "angularVelocity": 0.2528952696894573,
+            "velocityX": 1.985795631384405,
+            "velocityY": -0.8464469133529862,
+            "timestamp": 1.9625645639971387
+        },
+        {
+            "x": 7.622846427592322,
+            "y": 5.821361756830362,
+            "heading": -0.007278657657073996,
+            "angularVelocity": 0.16859613717477176,
+            "velocityX": 1.323863772822564,
+            "velocityY": -0.564297982864862,
+            "timestamp": 2.0489081496171515
         },
         {
             "x": 7.68,
             "y": 5.797,
             "heading": 0,
-            "angularVelocity": 0.02103339303101045,
-            "velocityX": 0.7634578628680123,
-            "velocityY": -0.33677391831044023,
-            "timestamp": 2.1016220775346097
+            "angularVelocity": 0.08429876527373403,
+            "velocityX": 0.6619318852381739,
+            "velocityY": -0.282149005689615,
+            "timestamp": 2.1352517352371643
         },
         {
             "x": 7.68,
             "y": 5.797,
             "heading": 0,
             "angularVelocity": 0,
-            "velocityX": 1.7450911582816108e-33,
-            "velocityY": 0,
-            "timestamp": 2.201699319321972
+            "velocityX": -1.8142028796058936e-31,
+            "velocityY": -1.068352585526189e-31,
+            "timestamp": 2.221595320857177
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.1.traj
new file mode 100644
index 00000000..b656a1eb
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.1.traj	
@@ -0,0 +1,274 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": -7.013886661360986e-29,
+            "velocityX": -3.078938571640024e-30,
+            "velocityY": -8.674299238088502e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 1.3762618085283742,
+            "y": 5.591108793803365,
+            "heading": -0.012389964804724785,
+            "angularVelocity": -0.17699325527244014,
+            "velocityX": 0.3751554628363854,
+            "velocityY": 0.44439566776177375,
+            "timestamp": 0.07000246865606696
+        },
+        {
+            "x": 1.4301971031295795,
+            "y": 5.652094846604856,
+            "heading": -0.03662832018577037,
+            "angularVelocity": -0.3462500086931304,
+            "velocityX": 0.7704770365480613,
+            "velocityY": 0.871198601597385,
+            "timestamp": 0.14000493731213393
+        },
+        {
+            "x": 1.513800165351683,
+            "y": 5.741006259883096,
+            "heading": -0.07186447572819399,
+            "angularVelocity": -0.5033558989907949,
+            "velocityX": 1.1942873419625006,
+            "velocityY": 1.2701182541879128,
+            "timestamp": 0.2100074059682009
+        },
+        {
+            "x": 1.6300142433743692,
+            "y": 5.854390700910866,
+            "heading": -0.1166088429554257,
+            "angularVelocity": -0.6391827043487479,
+            "velocityX": 1.660142567166188,
+            "velocityY": 1.6197206070673247,
+            "timestamp": 0.28000987462426785
+        },
+        {
+            "x": 1.7831654317988928,
+            "y": 5.985149557302319,
+            "heading": -0.16783055719619017,
+            "angularVelocity": -0.7317129698935645,
+            "velocityX": 2.187796964375065,
+            "velocityY": 1.8679177877841964,
+            "timestamp": 0.3500123432803348
+        },
+        {
+            "x": 1.9771685219359858,
+            "y": 6.116694524674393,
+            "heading": -0.21853613173426145,
+            "angularVelocity": -0.7243398055979559,
+            "velocityX": 2.771374979505968,
+            "velocityY": 1.8791475486148668,
+            "timestamp": 0.4200148119364018
+        },
+        {
+            "x": 2.202537147154592,
+            "y": 6.2222527712605,
+            "heading": -0.25772100339188303,
+            "angularVelocity": -0.5597641398939388,
+            "velocityX": 3.2194382504688988,
+            "velocityY": 1.507921772083263,
+            "timestamp": 0.49001728059246874
+        },
+        {
+            "x": 2.4433286317618386,
+            "y": 6.289980043332844,
+            "heading": -0.29446027058227175,
+            "angularVelocity": -0.5248281652898821,
+            "velocityX": 3.4397570432873823,
+            "velocityY": 0.9674983378806481,
+            "timestamp": 0.5600197492485357
+        },
+        {
+            "x": 2.6903031212328568,
+            "y": 6.317347154244271,
+            "heading": -0.33666392758398794,
+            "angularVelocity": -0.6028881239905549,
+            "velocityX": 3.528082569267318,
+            "velocityY": 0.39094494004013314,
+            "timestamp": 0.6300222179046027
+        },
+        {
+            "x": 2.936701774597168,
+            "y": 6.303859233856201,
+            "heading": -0.37968716420906834,
+            "angularVelocity": -0.6145959914234042,
+            "velocityX": 3.5198566292706706,
+            "velocityY": -0.1926777818987785,
+            "timestamp": 0.7000246865606696
+        },
+        {
+            "x": 3.257320421397908,
+            "y": 6.21346874174157,
+            "heading": -0.4320815750431537,
+            "angularVelocity": -0.5604086145235933,
+            "velocityX": 3.429324784540344,
+            "velocityY": -0.9668132467920023,
+            "timestamp": 0.7935179153869726
+        },
+        {
+            "x": 3.5608980813516515,
+            "y": 6.0524118876009405,
+            "heading": -0.46560329407698275,
+            "angularVelocity": -0.35854702479102396,
+            "velocityX": 3.247055040935253,
+            "velocityY": -1.7226579524796546,
+            "timestamp": 0.8870111442132755
+        },
+        {
+            "x": 3.8328887233978395,
+            "y": 5.82633673900318,
+            "heading": -0.46560402675062595,
+            "angularVelocity": -0.00000783664977303189,
+            "velocityX": 2.909201505399863,
+            "velocityY": -2.418091143453467,
+            "timestamp": 0.9805043730395785
+        },
+        {
+            "x": 4.052764522028411,
+            "y": 5.549310224236533,
+            "heading": -0.46560404923284493,
+            "angularVelocity": -2.404689503787649e-7,
+            "velocityX": 2.3517831332905046,
+            "velocityY": -2.963065007427709,
+            "timestamp": 1.0739976018658814
+        },
+        {
+            "x": 4.211188193978367,
+            "y": 5.233096683639544,
+            "heading": -0.46560406449531083,
+            "angularVelocity": -1.6324674914477828e-7,
+            "velocityX": 1.6944935364707439,
+            "velocityY": -3.382207937052538,
+            "timestamp": 1.1674908306921843
+        },
+        {
+            "x": 4.383758324037826,
+            "y": 4.9243757535417085,
+            "heading": -0.4656040804041062,
+            "angularVelocity": -1.701598650622723e-7,
+            "velocityX": 1.8458035113972522,
+            "velocityY": -3.302067261698653,
+            "timestamp": 1.2609840595184871
+        },
+        {
+            "x": 4.615944430524366,
+            "y": 4.657582435311582,
+            "heading": -0.46560407064369363,
+            "angularVelocity": 1.0439700291593356e-7,
+            "velocityX": 2.4834537153263514,
+            "velocityY": -2.8536111286282813,
+            "timestamp": 1.35447728834479
+        },
+        {
+            "x": 4.89788653877274,
+            "y": 4.444046330431443,
+            "heading": -0.46560381887866387,
+            "angularVelocity": 0.0000026928691303704606,
+            "velocityX": 3.015642007317779,
+            "velocityY": -2.2839740113892115,
+            "timestamp": 1.4479705171710928
+        },
+        {
+            "x": 5.217611687922929,
+            "y": 4.2928350259750445,
+            "heading": -0.4656035898817651,
+            "angularVelocity": 0.000002449342071651111,
+            "velocityX": 3.4197679678406536,
+            "velocityY": -1.6173503295872917,
+            "timestamp": 1.5414637459973957
+        },
+        {
+            "x": 5.553860912374937,
+            "y": 4.183174208089645,
+            "heading": -0.46560339968651515,
+            "angularVelocity": 0.0000020343211207896953,
+            "velocityX": 3.5965088453273077,
+            "velocityY": -1.1729279142677982,
+            "timestamp": 1.6349569748236985
+        },
+        {
+            "x": 5.905448307826856,
+            "y": 4.144763712168745,
+            "heading": -0.4656033544319255,
+            "angularVelocity": 4.840413598760029e-7,
+            "velocityX": 3.7605653357540834,
+            "velocityY": -0.4108371954108264,
+            "timestamp": 1.7284502036500013
+        },
+        {
+            "x": 6.2591141169961295,
+            "y": 4.141672024564684,
+            "heading": -0.4656033129772286,
+            "angularVelocity": 4.433978530009588e-7,
+            "velocityX": 3.7827959693886872,
+            "velocityY": -0.0330685723754928,
+            "timestamp": 1.8219434324763042
+        },
+        {
+            "x": 6.612779928543081,
+            "y": 4.138580608962963,
+            "heading": -0.465603271522502,
+            "angularVelocity": 4.433981701164172e-7,
+            "velocityX": 3.7827959948202516,
+            "velocityY": -0.03306566304887272,
+            "timestamp": 1.915436661302607
+        },
+        {
+            "x": 6.963404638780936,
+            "y": 4.132792400258934,
+            "heading": -0.45779034574220756,
+            "angularVelocity": 0.08356675535090403,
+            "velocityX": 3.7502684915210915,
+            "velocityY": -0.061910458935820756,
+            "timestamp": 2.00892989012891
+        },
+        {
+            "x": 7.251193869015599,
+            "y": 4.13050050188738,
+            "heading": -0.30806732380169355,
+            "angularVelocity": 1.6014317167147898,
+            "velocityX": 3.0781826004676223,
+            "velocityY": -0.024514057331484977,
+            "timestamp": 2.1024231189552127
+        },
+        {
+            "x": 7.466125497714746,
+            "y": 4.12875254378885,
+            "heading": -0.16499798934349721,
+            "angularVelocity": 1.5302641298654869,
+            "velocityX": 2.29890048078732,
+            "velocityY": -0.018696092973500365,
+            "timestamp": 2.1959163477815156
+        },
+        {
+            "x": 7.608856917870355,
+            "y": 4.12758509655783,
+            "heading": -0.05770853615881093,
+            "angularVelocity": 1.147563888118738,
+            "velocityX": 1.5266498114080966,
+            "velocityY": -0.012486970935497041,
+            "timestamp": 2.2894095766078184
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -1.6446149791629635e-35,
+            "angularVelocity": 0.6172482957672322,
+            "velocityX": 0.7609436856846457,
+            "velocityY": -0.0062581704063020965,
+            "timestamp": 2.3829028054341213
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": -1.0953722106158818e-30,
+            "velocityX": 0,
+            "velocityY": -1.282624680344039e-29,
+            "timestamp": 2.476396034260424
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.traj
index 4db64f81..b656a1eb 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.traj	
@@ -4,208 +4,271 @@
             "x": 1.35,
             "y": 5.56,
             "heading": 0,
-            "angularVelocity": 2.6743025901632004e-33,
-            "velocityX": 2.068786142168461e-33,
-            "velocityY": 0,
+            "angularVelocity": -7.013886661360986e-29,
+            "velocityX": -3.078938571640024e-30,
+            "velocityY": -8.674299238088502e-31,
             "timestamp": 0
         },
         {
-            "x": 1.4007220754094205,
-            "y": 5.545584664540576,
-            "heading": -0.37232774466736046,
-            "angularVelocity": -3.437582019103784,
-            "velocityX": 0.4683005682394807,
-            "velocityY": -0.13309214444637307,
-            "timestamp": 0.10831094141120465
-        },
-        {
-            "x": 1.5327694511201624,
-            "y": 5.533363500300387,
-            "heading": -0.9362708794209632,
-            "angularVelocity": -5.206705134364786,
-            "velocityX": 1.2191508446909478,
-            "velocityY": -0.11283406903270367,
-            "timestamp": 0.2166218828224093
-        },
-        {
-            "x": 1.760106911270163,
-            "y": 5.516367699163042,
-            "heading": -1.4218044496098983,
-            "angularVelocity": -4.482774905866594,
-            "velocityX": 2.0989334705060814,
-            "velocityY": -0.1569167520465022,
-            "timestamp": 0.3249328242336139
-        },
-        {
-            "x": 2.067387230758427,
-            "y": 5.467280597023701,
-            "heading": -1.7292049865361503,
-            "angularVelocity": -2.838130044121764,
-            "velocityX": 2.83702011527781,
-            "velocityY": -0.453205387191498,
-            "timestamp": 0.4332437656448186
-        },
-        {
-            "x": 2.4488593918735733,
-            "y": 5.393167697716653,
-            "heading": -1.7983473850598053,
-            "angularVelocity": -0.6383694723984937,
-            "velocityX": 3.5220094677866336,
-            "velocityY": -0.6842605035226905,
-            "timestamp": 0.5415547070560233
-        },
-        {
-            "x": 2.8531804118778052,
-            "y": 5.326790341993422,
-            "heading": -1.7983490370676205,
-            "angularVelocity": -0.000015252455509683049,
-            "velocityX": 3.7329656148885193,
-            "velocityY": -0.6128407237384188,
-            "timestamp": 0.6498656484672279
-        },
-        {
-            "x": 3.257501404598323,
-            "y": 5.2604128200678275,
-            "heading": -1.7983506890680847,
-            "angularVelocity": -0.00001525238764202395,
-            "velocityX": 3.732965362986781,
-            "velocityY": -0.6128422582312392,
-            "timestamp": 0.7581765898784326
-        },
-        {
-            "x": 3.6466232677858144,
-            "y": 5.172310745117625,
-            "heading": -1.8337945854134767,
-            "angularVelocity": -0.32724206699329256,
-            "velocityX": 3.5926367005728674,
-            "velocityY": -0.8134180517896293,
-            "timestamp": 0.8664875312896373
-        },
-        {
-            "x": 4.014238623942389,
-            "y": 5.062846847355338,
-            "heading": -1.917513965365452,
-            "angularVelocity": -0.7729540419571551,
-            "velocityX": 3.394074055370967,
-            "velocityY": -1.0106448742487064,
-            "timestamp": 0.974798472700842
-        },
-        {
-            "x": 4.354761888880782,
-            "y": 4.880183720994759,
-            "heading": -1.9914398534549198,
-            "angularVelocity": -0.6825338892476871,
-            "velocityX": 3.143941512294591,
-            "velocityY": -1.6864697507068689,
-            "timestamp": 1.0831094141120465
-        },
-        {
-            "x": 4.661144802240214,
-            "y": 4.608133464126214,
-            "heading": -1.9914398486412812,
-            "angularVelocity": 4.444277523534959e-8,
-            "velocityX": 2.8287346538355926,
-            "velocityY": -2.5117523061285136,
-            "timestamp": 1.1914203555232512
-        },
-        {
-            "x": 5.019380877625839,
-            "y": 4.418922250546549,
-            "heading": -1.9767149939198712,
-            "angularVelocity": 0.13594983599585594,
-            "velocityX": 3.3074781801182462,
-            "velocityY": -1.7469261287400477,
-            "timestamp": 1.2997312969344559
-        },
-        {
-            "x": 5.376950498025801,
-            "y": 4.3069722509438675,
-            "heading": -1.8614424797513431,
-            "angularVelocity": 1.0642739566900585,
-            "velocityX": 3.3013250161167162,
-            "velocityY": -1.0335982509621156,
-            "timestamp": 1.4080422383456606
-        },
-        {
-            "x": 5.749629435463048,
-            "y": 4.240661073830716,
-            "heading": -1.7582912287295662,
-            "angularVelocity": 0.9523622422425545,
-            "velocityX": 3.4408244687151703,
-            "velocityY": -0.6122297179691145,
-            "timestamp": 1.5163531797568652
-        },
-        {
-            "x": 6.158173088993434,
-            "y": 4.209455444526307,
-            "heading": -1.7582906674190417,
-            "angularVelocity": 0.000005182399092709644,
-            "velocityX": 3.771951828756997,
-            "velocityY": -0.2881115139230142,
-            "timestamp": 1.62466412116807
-        },
-        {
-            "x": 6.566716775041419,
-            "y": 4.178250240929697,
-            "heading": -1.7582901061123783,
-            "angularVelocity": 0.000005182363444203767,
-            "velocityX": 3.771952128981517,
-            "velocityY": -0.2881075835001592,
-            "timestamp": 1.7329750625792746
-        },
-        {
-            "x": 6.948239527399369,
-            "y": 4.140315578246664,
-            "heading": -1.6699172621402418,
-            "angularVelocity": 0.8159179748667066,
-            "velocityX": 3.5224765604196056,
-            "velocityY": -0.35023850950582514,
-            "timestamp": 1.8412860039904793
-        },
-        {
-            "x": 7.2588942750640015,
-            "y": 4.14084991195257,
-            "heading": -1.390296709754989,
-            "angularVelocity": 2.5816464037891405,
-            "velocityX": 2.86817512263351,
-            "velocityY": 0.004933330824608595,
-            "timestamp": 1.949596945401684
-        },
-        {
-            "x": 7.4846066646178775,
-            "y": 4.133171401549216,
-            "heading": -0.9384850139510863,
-            "angularVelocity": 4.17143171241205,
-            "velocityX": 2.083929717653862,
-            "velocityY": -0.07089321081793584,
-            "timestamp": 2.0579078868128886
-        },
-        {
-            "x": 7.620450068903259,
-            "y": 4.128683952785512,
-            "heading": -0.34333254233125515,
-            "angularVelocity": 5.494850879010672,
-            "velocityX": 1.2541983525897882,
-            "velocityY": -0.04143116757398245,
-            "timestamp": 2.166218828224093
+            "x": 1.3762618085283742,
+            "y": 5.591108793803365,
+            "heading": -0.012389964804724785,
+            "angularVelocity": -0.17699325527244014,
+            "velocityX": 0.3751554628363854,
+            "velocityY": 0.44439566776177375,
+            "timestamp": 0.07000246865606696
+        },
+        {
+            "x": 1.4301971031295795,
+            "y": 5.652094846604856,
+            "heading": -0.03662832018577037,
+            "angularVelocity": -0.3462500086931304,
+            "velocityX": 0.7704770365480613,
+            "velocityY": 0.871198601597385,
+            "timestamp": 0.14000493731213393
+        },
+        {
+            "x": 1.513800165351683,
+            "y": 5.741006259883096,
+            "heading": -0.07186447572819399,
+            "angularVelocity": -0.5033558989907949,
+            "velocityX": 1.1942873419625006,
+            "velocityY": 1.2701182541879128,
+            "timestamp": 0.2100074059682009
+        },
+        {
+            "x": 1.6300142433743692,
+            "y": 5.854390700910866,
+            "heading": -0.1166088429554257,
+            "angularVelocity": -0.6391827043487479,
+            "velocityX": 1.660142567166188,
+            "velocityY": 1.6197206070673247,
+            "timestamp": 0.28000987462426785
+        },
+        {
+            "x": 1.7831654317988928,
+            "y": 5.985149557302319,
+            "heading": -0.16783055719619017,
+            "angularVelocity": -0.7317129698935645,
+            "velocityX": 2.187796964375065,
+            "velocityY": 1.8679177877841964,
+            "timestamp": 0.3500123432803348
+        },
+        {
+            "x": 1.9771685219359858,
+            "y": 6.116694524674393,
+            "heading": -0.21853613173426145,
+            "angularVelocity": -0.7243398055979559,
+            "velocityX": 2.771374979505968,
+            "velocityY": 1.8791475486148668,
+            "timestamp": 0.4200148119364018
+        },
+        {
+            "x": 2.202537147154592,
+            "y": 6.2222527712605,
+            "heading": -0.25772100339188303,
+            "angularVelocity": -0.5597641398939388,
+            "velocityX": 3.2194382504688988,
+            "velocityY": 1.507921772083263,
+            "timestamp": 0.49001728059246874
+        },
+        {
+            "x": 2.4433286317618386,
+            "y": 6.289980043332844,
+            "heading": -0.29446027058227175,
+            "angularVelocity": -0.5248281652898821,
+            "velocityX": 3.4397570432873823,
+            "velocityY": 0.9674983378806481,
+            "timestamp": 0.5600197492485357
+        },
+        {
+            "x": 2.6903031212328568,
+            "y": 6.317347154244271,
+            "heading": -0.33666392758398794,
+            "angularVelocity": -0.6028881239905549,
+            "velocityX": 3.528082569267318,
+            "velocityY": 0.39094494004013314,
+            "timestamp": 0.6300222179046027
+        },
+        {
+            "x": 2.936701774597168,
+            "y": 6.303859233856201,
+            "heading": -0.37968716420906834,
+            "angularVelocity": -0.6145959914234042,
+            "velocityX": 3.5198566292706706,
+            "velocityY": -0.1926777818987785,
+            "timestamp": 0.7000246865606696
+        },
+        {
+            "x": 3.257320421397908,
+            "y": 6.21346874174157,
+            "heading": -0.4320815750431537,
+            "angularVelocity": -0.5604086145235933,
+            "velocityX": 3.429324784540344,
+            "velocityY": -0.9668132467920023,
+            "timestamp": 0.7935179153869726
+        },
+        {
+            "x": 3.5608980813516515,
+            "y": 6.0524118876009405,
+            "heading": -0.46560329407698275,
+            "angularVelocity": -0.35854702479102396,
+            "velocityX": 3.247055040935253,
+            "velocityY": -1.7226579524796546,
+            "timestamp": 0.8870111442132755
+        },
+        {
+            "x": 3.8328887233978395,
+            "y": 5.82633673900318,
+            "heading": -0.46560402675062595,
+            "angularVelocity": -0.00000783664977303189,
+            "velocityX": 2.909201505399863,
+            "velocityY": -2.418091143453467,
+            "timestamp": 0.9805043730395785
+        },
+        {
+            "x": 4.052764522028411,
+            "y": 5.549310224236533,
+            "heading": -0.46560404923284493,
+            "angularVelocity": -2.404689503787649e-7,
+            "velocityX": 2.3517831332905046,
+            "velocityY": -2.963065007427709,
+            "timestamp": 1.0739976018658814
+        },
+        {
+            "x": 4.211188193978367,
+            "y": 5.233096683639544,
+            "heading": -0.46560406449531083,
+            "angularVelocity": -1.6324674914477828e-7,
+            "velocityX": 1.6944935364707439,
+            "velocityY": -3.382207937052538,
+            "timestamp": 1.1674908306921843
+        },
+        {
+            "x": 4.383758324037826,
+            "y": 4.9243757535417085,
+            "heading": -0.4656040804041062,
+            "angularVelocity": -1.701598650622723e-7,
+            "velocityX": 1.8458035113972522,
+            "velocityY": -3.302067261698653,
+            "timestamp": 1.2609840595184871
+        },
+        {
+            "x": 4.615944430524366,
+            "y": 4.657582435311582,
+            "heading": -0.46560407064369363,
+            "angularVelocity": 1.0439700291593356e-7,
+            "velocityX": 2.4834537153263514,
+            "velocityY": -2.8536111286282813,
+            "timestamp": 1.35447728834479
+        },
+        {
+            "x": 4.89788653877274,
+            "y": 4.444046330431443,
+            "heading": -0.46560381887866387,
+            "angularVelocity": 0.0000026928691303704606,
+            "velocityX": 3.015642007317779,
+            "velocityY": -2.2839740113892115,
+            "timestamp": 1.4479705171710928
+        },
+        {
+            "x": 5.217611687922929,
+            "y": 4.2928350259750445,
+            "heading": -0.4656035898817651,
+            "angularVelocity": 0.000002449342071651111,
+            "velocityX": 3.4197679678406536,
+            "velocityY": -1.6173503295872917,
+            "timestamp": 1.5414637459973957
+        },
+        {
+            "x": 5.553860912374937,
+            "y": 4.183174208089645,
+            "heading": -0.46560339968651515,
+            "angularVelocity": 0.0000020343211207896953,
+            "velocityX": 3.5965088453273077,
+            "velocityY": -1.1729279142677982,
+            "timestamp": 1.6349569748236985
+        },
+        {
+            "x": 5.905448307826856,
+            "y": 4.144763712168745,
+            "heading": -0.4656033544319255,
+            "angularVelocity": 4.840413598760029e-7,
+            "velocityX": 3.7605653357540834,
+            "velocityY": -0.4108371954108264,
+            "timestamp": 1.7284502036500013
+        },
+        {
+            "x": 6.2591141169961295,
+            "y": 4.141672024564684,
+            "heading": -0.4656033129772286,
+            "angularVelocity": 4.433978530009588e-7,
+            "velocityX": 3.7827959693886872,
+            "velocityY": -0.0330685723754928,
+            "timestamp": 1.8219434324763042
+        },
+        {
+            "x": 6.612779928543081,
+            "y": 4.138580608962963,
+            "heading": -0.465603271522502,
+            "angularVelocity": 4.433981701164172e-7,
+            "velocityX": 3.7827959948202516,
+            "velocityY": -0.03306566304887272,
+            "timestamp": 1.915436661302607
+        },
+        {
+            "x": 6.963404638780936,
+            "y": 4.132792400258934,
+            "heading": -0.45779034574220756,
+            "angularVelocity": 0.08356675535090403,
+            "velocityX": 3.7502684915210915,
+            "velocityY": -0.061910458935820756,
+            "timestamp": 2.00892989012891
+        },
+        {
+            "x": 7.251193869015599,
+            "y": 4.13050050188738,
+            "heading": -0.30806732380169355,
+            "angularVelocity": 1.6014317167147898,
+            "velocityX": 3.0781826004676223,
+            "velocityY": -0.024514057331484977,
+            "timestamp": 2.1024231189552127
+        },
+        {
+            "x": 7.466125497714746,
+            "y": 4.12875254378885,
+            "heading": -0.16499798934349721,
+            "angularVelocity": 1.5302641298654869,
+            "velocityX": 2.29890048078732,
+            "velocityY": -0.018696092973500365,
+            "timestamp": 2.1959163477815156
+        },
+        {
+            "x": 7.608856917870355,
+            "y": 4.12758509655783,
+            "heading": -0.05770853615881093,
+            "angularVelocity": 1.147563888118738,
+            "velocityX": 1.5266498114080966,
+            "velocityY": -0.012486970935497041,
+            "timestamp": 2.2894095766078184
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 9.09513036114211e-34,
-            "angularVelocity": 3.1698786646843584,
-            "velocityX": 0.5498053134877472,
-            "velocityY": -0.015547393121808115,
-            "timestamp": 2.2745297696352975
+            "heading": -1.6446149791629635e-35,
+            "angularVelocity": 0.6172482957672322,
+            "velocityX": 0.7609436856846457,
+            "velocityY": -0.0062581704063020965,
+            "timestamp": 2.3829028054341213
         },
         {
             "x": 7.68,
             "y": 4.127,
             "heading": 0,
-            "angularVelocity": -7.032634787663221e-33,
+            "angularVelocity": -1.0953722106158818e-30,
             "velocityX": 0,
-            "velocityY": -1.950631326224301e-32,
-            "timestamp": 2.382840711046502
+            "velocityY": -1.282624680344039e-29,
+            "timestamp": 2.476396034260424
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.1.traj
new file mode 100644
index 00000000..0d7f85e3
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.1.traj	
@@ -0,0 +1,1453 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 3.439885530040909e-30,
+            "timestamp": 0
+        },
+        {
+            "x": 1.3518715996750759,
+            "y": 5.562488632849977,
+            "heading": -0.00039025483625088287,
+            "angularVelocity": -0.020188245249371933,
+            "velocityX": 0.09681959001984766,
+            "velocityY": 0.12873928941829918,
+            "timestamp": 0.019330795293514086
+        },
+        {
+            "x": 1.3556369831983786,
+            "y": 5.567448998179342,
+            "heading": -0.001175615506548702,
+            "angularVelocity": -0.04062743712160119,
+            "velocityX": 0.19478678792724713,
+            "velocityY": 0.256604306964468,
+            "timestamp": 0.03866159058702817
+        },
+        {
+            "x": 1.3613203761105706,
+            "y": 5.574862277415471,
+            "heading": -0.0023613813379046128,
+            "angularVelocity": -0.061340768103509684,
+            "velocityX": 0.2940071955600798,
+            "velocityY": 0.38349582226532697,
+            "timestamp": 0.05799238588054226
+        },
+        {
+            "x": 1.3689483300707626,
+            "y": 5.58470739953387,
+            "heading": -0.003953362765501964,
+            "angularVelocity": -0.08235467829569511,
+            "velocityX": 0.39460114518679135,
+            "velocityY": 0.5092973138928011,
+            "timestamp": 0.07732318117405634
+        },
+        {
+            "x": 1.3785500603498269,
+            "y": 5.596960627127346,
+            "heading": -0.0059579558445008625,
+            "angularVelocity": -0.10369946236363117,
+            "velocityX": 0.4967064279184545,
+            "velocityY": 0.6338708473927627,
+            "timestamp": 0.09665397646757043
+        },
+        {
+            "x": 1.3901578478597558,
+            "y": 5.611595038412612,
+            "heading": -0.008382231168191269,
+            "angularVelocity": -0.12541001479146147,
+            "velocityX": 0.6004816322183893,
+            "velocityY": 0.7570516920313731,
+            "timestamp": 0.11598477176108452
+        },
+        {
+            "x": 1.4038075203372553,
+            "y": 5.62857987161075,
+            "heading": -0.01123404053815137,
+            "angularVelocity": -0.14752674820972528,
+            "velocityX": 0.7061102386242472,
+            "velocityY": 0.8786412012669059,
+            "timestamp": 0.1353155670545986
+        },
+        {
+            "x": 1.4195390306280726,
+            "y": 5.647879684835849,
+            "heading": -0.014522145551621221,
+            "angularVelocity": -0.17009672719326588,
+            "velocityX": 0.8138056428591633,
+            "velocityY": 0.9983972688167116,
+            "timestamp": 0.1546463623481127
+        },
+        {
+            "x": 1.4373971536587393,
+            "y": 5.669453265066441,
+            "heading": -0.018256373225668265,
+            "angularVelocity": -0.19317506689959266,
+            "velocityX": 0.923817295642181,
+            "velocityY": 1.116021348476537,
+            "timestamp": 0.17397715764162677
+        },
+        {
+            "x": 1.457432326992564,
+            "y": 5.693252190583062,
+            "heading": -0.022447804782031403,
+            "angularVelocity": -0.21682664850159125,
+            "velocityX": 1.0364381304346626,
+            "velocityY": 1.2311405276019616,
+            "timestamp": 0.19330795293514086
+        },
+        {
+            "x": 1.47970166115617,
+            "y": 5.719218907072145,
+            "heading": -0.027109004507978946,
+            "angularVelocity": -0.24112819235695526,
+            "velocityX": 1.1520133458284385,
+            "velocityY": 1.3432823686149544,
+            "timestamp": 0.21263874822865494
+        },
+        {
+            "x": 1.5042701411060604,
+            "y": 5.747284109919373,
+            "heading": -0.03225429550223853,
+            "angularVelocity": -0.2661706834165162,
+            "velocityX": 1.2709502933970824,
+            "velocityY": 1.4518390175412745,
+            "timestamp": 0.23196954352216903
+        },
+        {
+            "x": 1.5312120197646597,
+            "y": 5.777363120811461,
+            "heading": -0.037900086436595266,
+            "angularVelocity": -0.2920620103121596,
+            "velocityX": 1.3937284136281272,
+            "velocityY": 1.5560151786502046,
+            "timestamp": 0.25130033881568314
+        },
+        {
+            "x": 1.5606123466917503,
+            "y": 5.809350786193417,
+            "heading": -0.044065244214455006,
+            "angularVelocity": -0.3189293396494718,
+            "velocityX": 1.5209062266028461,
+            "velocityY": 1.6547516486654796,
+            "timestamp": 0.27063113410919726
+        },
+        {
+            "x": 1.5925684302904153,
+            "y": 5.843114184319679,
+            "heading": -0.050771482089097104,
+            "angularVelocity": -0.34691991575183656,
+            "velocityX": 1.653117893674399,
+            "velocityY": 1.7466119532904336,
+            "timestamp": 0.28996192940271137
+        },
+        {
+            "x": 1.6271906907680806,
+            "y": 5.8784820940308995,
+            "heading": -0.058043668844819325,
+            "angularVelocity": -0.37619697717051565,
+            "velocityX": 1.791041700663091,
+            "velocityY": 1.8296148282676763,
+            "timestamp": 0.3092927246962255
+        },
+        {
+            "x": 1.6646015916598045,
+            "y": 5.915229821006168,
+            "heading": -0.06590980894370155,
+            "angularVelocity": -0.4069227354304089,
+            "velocityX": 1.935300660096276,
+            "velocityY": 1.9009940572697726,
+            "timestamp": 0.3286235199897396
+        },
+        {
+            "x": 1.7049297157110095,
+            "y": 5.953057987948439,
+            "heading": -0.07440007857488001,
+            "angularVelocity": -0.43920953598980744,
+            "velocityX": 2.086211324411271,
+            "velocityY": 1.9568862205562443,
+            "timestamp": 0.3479543152832537
+        },
+        {
+            "x": 1.7482931222517035,
+            "y": 5.991565656180575,
+            "heading": -0.08354352935308812,
+            "angularVelocity": -0.47299920357005454,
+            "velocityX": 2.2432293075517364,
+            "velocityY": 1.9920374535783947,
+            "timestamp": 0.3672851105767678
+        },
+        {
+            "x": 1.7947629094651933,
+            "y": 6.030225122456552,
+            "heading": -0.09335994332155541,
+            "angularVelocity": -0.5078122146252688,
+            "velocityX": 2.4039252657691588,
+            "velocityY": 1.9998901074156783,
+            "timestamp": 0.38661590587028194
+        },
+        {
+            "x": 1.844301928458138,
+            "y": 6.068381679285448,
+            "heading": -0.10384499839936924,
+            "angularVelocity": -0.5424016404194022,
+            "velocityX": 2.562699477220483,
+            "velocityY": 1.9738741344852324,
+            "timestamp": 0.40594670116379605
+        },
+        {
+            "x": 1.8967008920878712,
+            "y": 6.105314232951604,
+            "heading": -0.1149551002367539,
+            "angularVelocity": -0.5747358899978738,
+            "velocityX": 2.710647070341413,
+            "velocityY": 1.9105553137044158,
+            "timestamp": 0.42527749645731017
+        },
+        {
+            "x": 1.951571944971935,
+            "y": 6.140356350737874,
+            "heading": -0.126607814812745,
+            "angularVelocity": -0.6028057510857173,
+            "velocityX": 2.83853054418689,
+            "velocityY": 1.8127613093097652,
+            "timestamp": 0.4446082917508243
+        },
+        {
+            "x": 2.008427579479744,
+            "y": 6.172999646985284,
+            "heading": -0.13870025114422718,
+            "angularVelocity": -0.6255529660251172,
+            "velocityX": 2.9411947953783932,
+            "velocityY": 1.6886680424557017,
+            "timestamp": 0.4639390870443384
+        },
+        {
+            "x": 2.066782288063465,
+            "y": 6.202913115188225,
+            "heading": -0.15112974078850008,
+            "angularVelocity": -0.6429890470384952,
+            "velocityX": 3.018743290054933,
+            "velocityY": 1.5474515015415655,
+            "timestamp": 0.4832698823378525
+        },
+        {
+            "x": 2.126212031523665,
+            "y": 6.229902950259444,
+            "heading": -0.1638060412217236,
+            "angularVelocity": -0.6557567984529158,
+            "velocityX": 3.074355842986947,
+            "velocityY": 1.3962092434072775,
+            "timestamp": 0.5026006776313666
+        },
+        {
+            "x": 2.1863688602207922,
+            "y": 6.253863406934038,
+            "heading": -0.17665473442964857,
+            "angularVelocity": -0.6646748368514358,
+            "velocityX": 3.111968637798953,
+            "velocityY": 1.2394966844759612,
+            "timestamp": 0.5219314729248807
+        },
+        {
+            "x": 2.246973575487724,
+            "y": 6.2747405977079325,
+            "heading": -0.1896159238006329,
+            "angularVelocity": -0.6704943678821678,
+            "velocityX": 3.1351382261683427,
+            "velocityY": 1.0799964748940398,
+            "timestamp": 0.5412622682183948
+        },
+        {
+            "x": 2.30780256528041,
+            "y": 6.292510136082229,
+            "heading": -0.20264154258755296,
+            "angularVelocity": -0.6738273614273119,
+            "velocityX": 3.146740155750102,
+            "velocityY": 0.9192347290676759,
+            "timestamp": 0.560593063511909
+        },
+        {
+            "x": 2.3686753728703707,
+            "y": 6.307164203375093,
+            "heading": -0.2156927432279827,
+            "angularVelocity": -0.6751507344764429,
+            "velocityX": 3.1490068911124522,
+            "velocityY": 0.7580685155659881,
+            "timestamp": 0.5799238588054231
+        },
+        {
+            "x": 2.4294446969864714,
+            "y": 6.318704202261767,
+            "heading": -0.228737776875863,
+            "angularVelocity": -0.6748317102223548,
+            "velocityX": 3.143653594867338,
+            "velocityY": 0.5969748637576952,
+            "timestamp": 0.5992546540989372
+        },
+        {
+            "x": 2.4899887888473393,
+            "y": 6.32713657767754,
+            "heading": -0.24175037423147705,
+            "angularVelocity": -0.6731537506881662,
+            "velocityX": 3.132002121049924,
+            "velocityY": 0.43621461443967424,
+            "timestamp": 0.6185854493924513
+        },
+        {
+            "x": 2.550205774126716,
+            "y": 6.332470420209207,
+            "heading": -0.25470853343764965,
+            "angularVelocity": -0.6703376146412596,
+            "velocityX": 3.1150805936877464,
+            "velocityY": 0.2759246296222352,
+            "timestamp": 0.6379162446859654
+        },
+        {
+            "x": 2.610009417191064,
+            "y": 6.334716080800322,
+            "heading": -0.26759361438505785,
+            "angularVelocity": -0.6665572084213006,
+            "velocityX": 3.09369801688466,
+            "velocityY": 0.11617010873154458,
+            "timestamp": 0.6572470399794795
+        },
+        {
+            "x": 2.669325940254228,
+            "y": 6.333884367249437,
+            "heading": -0.28038965768785973,
+            "angularVelocity": -0.6619512083445117,
+            "velocityX": 3.0684988466597254,
+            "velocityY": -0.04302531469893782,
+            "timestamp": 0.6765778352729936
+        },
+        {
+            "x": 2.7280916104140402,
+            "y": 6.329986080406231,
+            "heading": -0.2930828674707726,
+            "angularVelocity": -0.656631534822156,
+            "velocityX": 3.040002714194029,
+            "velocityY": -0.2016620001410218,
+            "timestamp": 0.6959086305665078
+        },
+        {
+            "x": 2.7862508879843872,
+            "y": 6.323031751356315,
+            "heading": -0.3056612140555153,
+            "angularVelocity": -0.650689554866023,
+            "velocityX": 3.0086334621659443,
+            "velocityY": -0.35975390274029156,
+            "timestamp": 0.7152394258600219
+        },
+        {
+            "x": 2.843754988622848,
+            "y": 6.3130314987287885,
+            "heading": -0.31811412514136855,
+            "angularVelocity": -0.6442006599713835,
+            "velocityX": 2.974740550780866,
+            "velocityY": -0.5173223592555559,
+            "timestamp": 0.734570221153536
+        },
+        {
+            "x": 2.9005607537314586,
+            "y": 6.299994958196607,
+            "heading": -0.3304322429824912,
+            "angularVelocity": -0.6372276801904713,
+            "velocityX": 2.9386150050261657,
+            "velocityY": -0.6743923534566658,
+            "timestamp": 0.7539010164470501
+        },
+        {
+            "x": 2.956629753112793,
+            "y": 6.283931255340576,
+            "heading": -0.34260723133624194,
+            "angularVelocity": -0.6298234588328613,
+            "velocityX": 2.9005014294547,
+            "velocityY": -0.8309902728844937,
+            "timestamp": 0.7732318117405642
+        },
+        {
+            "x": 2.9906169983281425,
+            "y": 6.272983358953205,
+            "heading": -0.34999362222052477,
+            "angularVelocity": -0.625060967472129,
+            "velocityX": 2.8761137487624757,
+            "velocityY": -0.9264474104986867,
+            "timestamp": 0.7850488844241373
+        },
+        {
+            "x": 3.024312445989771,
+            "y": 6.26090838895792,
+            "heading": -0.35731821138357023,
+            "angularVelocity": -0.6198311002374715,
+            "velocityX": 2.8514208691014065,
+            "velocityY": -1.0218241284130187,
+            "timestamp": 0.7968659571077105
+        },
+        {
+            "x": 3.0577122238990846,
+            "y": 6.2477073818247915,
+            "heading": -0.36457506252100974,
+            "angularVelocity": -0.6140988831800235,
+            "velocityX": 2.8264003111145226,
+            "velocityY": -1.1171131367820717,
+            "timestamp": 0.8086830297912836
+        },
+        {
+            "x": 3.090812163934883,
+            "y": 6.233381471982772,
+            "heading": -0.3717577779780659,
+            "angularVelocity": -0.6078252752934934,
+            "velocityX": 2.8010270328463904,
+            "velocityY": -1.2123061459996218,
+            "timestamp": 0.8205001024748567
+        },
+        {
+            "x": 3.1236077670194575,
+            "y": 6.217931905831193,
+            "heading": -0.3788594429566703,
+            "angularVelocity": -0.6009665141923228,
+            "velocityX": 2.7752730276563167,
+            "velocityY": -1.307393680759415,
+            "timestamp": 0.8323171751584298
+        },
+        {
+            "x": 3.1560941623709735,
+            "y": 6.201360058471141,
+            "heading": -0.3858725603738981,
+            "angularVelocity": -0.5934733249949906,
+            "velocityX": 2.749106840704776,
+            "velocityY": -1.4023648498911914,
+            "timestamp": 0.844134247842003
+        },
+        {
+            "x": 3.188266059838492,
+            "y": 6.1836674538325775,
+            "heading": -0.3927889743327873,
+            "angularVelocity": -0.5852899566661331,
+            "velocityX": 2.722492983583009,
+            "velocityY": -1.4972070590002187,
+            "timestamp": 0.8559513205255761
+        },
+        {
+            "x": 3.2201176938016274,
+            "y": 6.164855789080512,
+            "heading": -0.399599779609834,
+            "angularVelocity": -0.576352998701121,
+            "velocityX": 2.695391220484986,
+            "velocityY": -1.5919056483603757,
+            "timestamp": 0.8677683932091492
+        },
+        {
+            "x": 3.251642756703662,
+            "y": 6.144926964467571,
+            "heading": -0.40629521382129,
+            "angularVelocity": -0.5665899153487628,
+            "velocityX": 2.6677556909553144,
+            "velocityY": -1.686443432022221,
+            "timestamp": 0.8795854658927224
+        },
+        {
+            "x": 3.282834319738247,
+            "y": 6.123883120194607,
+            "heading": -0.41286452792903033,
+            "angularVelocity": -0.5559172126335613,
+            "velocityX": 2.6395338227837533,
+            "velocityY": -1.7808001047684463,
+            "timestamp": 0.8914025385762955
+        },
+        {
+            "x": 3.3136847374734915,
+            "y": 6.101726682396313,
+            "heading": -0.4192958293777808,
+            "angularVelocity": -0.5442381223304499,
+            "velocityX": 2.6106649727330486,
+            "velocityY": -1.874951469926556,
+            "timestamp": 0.9032196112598686
+        },
+        {
+            "x": 3.3441855321977045,
+            "y": 6.078460421163092,
+            "heading": -0.42557589025707343,
+            "angularVelocity": -0.5314396422408687,
+            "velocityX": 2.5810787105179256,
+            "velocityY": -1.9688684208198879,
+            "timestamp": 0.9150366839434417
+        },
+        {
+            "x": 3.37432725239814,
+            "y": 6.0540875246663015,
+            "heading": -0.4316899102075926,
+            "angularVelocity": -0.5173887065125765,
+            "velocityX": 2.5506926298537165,
+            "velocityY": -2.0625155780475546,
+            "timestamp": 0.9268537566270149
+        },
+        {
+            "x": 3.4040992978747684,
+            "y": 6.028611695167346,
+            "heading": -0.43762121996231873,
+            "angularVelocity": -0.5019271619587302,
+            "velocityX": 2.519409525001445,
+            "velocityY": -2.1558494375996973,
+            "timestamp": 0.938670829310588
+        },
+        {
+            "x": 3.4334897013019283,
+            "y": 6.00203727528326,
+            "heading": -0.44335090582849124,
+            "angularVelocity": -0.4848650778070557,
+            "velocityX": 2.48711370524239,
+            "velocityY": -2.2488158104525207,
+            "timestamp": 0.9504879019941611
+        },
+        {
+            "x": 3.462484852215012,
+            "y": 5.974369416891247,
+            "heading": -0.4488573271132443,
+            "angularVelocity": -0.46597168623725893,
+            "velocityX": 2.4536661226929253,
+            "velocityY": -2.341346214318684,
+            "timestamp": 0.9623049746777342
+        },
+        {
+            "x": 3.4910691438628887,
+            "y": 5.945614311410449,
+            "heading": -0.45411548589649775,
+            "angularVelocity": -0.4449628875146618,
+            "velocityX": 2.4188978449470153,
+            "velocityY": -2.4333526797013256,
+            "timestamp": 0.9741220473613074
+        },
+        {
+            "x": 3.5192245152965964,
+            "y": 5.915779510545288,
+            "heading": -0.4590961890390477,
+            "angularVelocity": -0.42148366824158007,
+            "velocityX": 2.3826011896200363,
+            "velocityY": -2.524720094735007,
+            "timestamp": 0.9859391200448805
+        },
+        {
+            "x": 3.5469298492407,
+            "y": 5.884874383909108,
+            "heading": -0.46376491150225324,
+            "angularVelocity": -0.39508282535110545,
+            "velocityX": 2.3445175201991417,
+            "velocityY": -2.6152946219194915,
+            "timestamp": 0.9977561927284536
+        },
+        {
+            "x": 3.5741601690568516,
+            "y": 5.852910789899007,
+            "heading": -0.46808022074004524,
+            "angularVelocity": -0.36517582258680764,
+            "velocityX": 2.304320244556386,
+            "velocityY": -2.7048656520944543,
+            "timestamp": 1.0095732654120266
+        },
+        {
+            "x": 3.6008855536785225,
+            "y": 5.819904089620565,
+            "heading": -0.4719915430906053,
+            "angularVelocity": -0.3309891083260508,
+            "velocityX": 2.2615909487314654,
+            "velocityY": -2.7931367744165776,
+            "timestamp": 1.0213903380955998
+        },
+        {
+            "x": 3.62706965768041,
+            "y": 5.785874731899473,
+            "heading": -0.4754359323425916,
+            "angularVelocity": -0.2914756762708573,
+            "velocityX": 2.2157859821143067,
+            "velocityY": -2.8796774490856496,
+            "timestamp": 1.0332074107791729
+        },
+        {
+            "x": 3.652667693323084,
+            "y": 5.750850822222342,
+            "heading": -0.4783333470461038,
+            "angularVelocity": -0.24518887046705806,
+            "velocityX": 2.1661909280000957,
+            "velocityY": -2.9638397439848143,
+            "timestamp": 1.045024483462746
+        },
+        {
+            "x": 3.677623743737937,
+            "y": 5.7148724362586085,
+            "heading": -0.48057991095642455,
+            "angularVelocity": -0.1901117112907446,
+            "velocityX": 2.1118640024567648,
+            "velocityY": -3.044610702424323,
+            "timestamp": 1.0568415561463191
+        },
+        {
+            "x": 3.701867477642102,
+            "y": 5.67799904418439,
+            "heading": -0.48203966919964636,
+            "angularVelocity": -0.12352959843022769,
+            "velocityX": 2.0515854097999866,
+            "velocityY": -3.120349096733213,
+            "timestamp": 1.0686586288298923
+        },
+        {
+            "x": 3.725329217347855,
+            "y": 5.640287080856699,
+            "heading": -0.4827696693750387,
+            "angularVelocity": -0.06177504318881954,
+            "velocityX": 1.9854104594250916,
+            "velocityY": -3.191311785711063,
+            "timestamp": 1.0804757015134654
+        },
+        {
+            "x": 3.74792659306077,
+            "y": 5.60180905514361,
+            "heading": -0.48297166201638797,
+            "angularVelocity": -0.017093289239954308,
+            "velocityX": 1.9122651030425624,
+            "velocityY": -3.2561385330714976,
+            "timestamp": 1.0922927741970385
+        },
+        {
+            "x": 3.769555777117098,
+            "y": 5.56268673154527,
+            "heading": -0.48297173035274543,
+            "angularVelocity": -0.000005782849890954889,
+            "velocityX": 1.8303335043707702,
+            "velocityY": -3.310661163379628,
+            "timestamp": 1.1041098468806116
+        },
+        {
+            "x": 3.7901587344932506,
+            "y": 5.52301434523992,
+            "heading": -0.48297173483145855,
+            "angularVelocity": -3.790036003454015e-7,
+            "velocityX": 1.743490788949144,
+            "velocityY": -3.3572092994315077,
+            "timestamp": 1.1159269195641848
+        },
+        {
+            "x": 3.809721481337777,
+            "y": 5.482818833345491,
+            "heading": -0.4829717389588181,
+            "angularVelocity": -3.4927089311686727e-7,
+            "velocityX": 1.6554647135005363,
+            "velocityY": -3.401477926957743,
+            "timestamp": 1.127743992247758
+        },
+        {
+            "x": 3.8282308263038667,
+            "y": 5.442127501029994,
+            "heading": -0.482971742791846,
+            "angularVelocity": -3.2436357183409427e-7,
+            "velocityX": 1.5663223424036268,
+            "velocityY": -3.443435900336205,
+            "timestamp": 1.139561064931331
+        },
+        {
+            "x": 3.8456768429335884,
+            "y": 5.400969063215312,
+            "heading": -0.48297174636762497,
+            "angularVelocity": -3.0259430956931755e-7,
+            "velocityX": 1.4763399614162735,
+            "velocityY": -3.482963921504598,
+            "timestamp": 1.1513781376149042
+        },
+        {
+            "x": 3.863665898181474,
+            "y": 5.360045056052632,
+            "heading": -0.48297175000247344,
+            "angularVelocity": -3.075929693793739e-7,
+            "velocityX": 1.5222936957043627,
+            "velocityY": -3.463125619898205,
+            "timestamp": 1.1631952102984773
+        },
+        {
+            "x": 3.8827072670006824,
+            "y": 5.319599949808908,
+            "heading": -0.4829717539017329,
+            "angularVelocity": -3.299683104273868e-7,
+            "velocityX": 1.6113439706329509,
+            "velocityY": -3.4225994310711667,
+            "timestamp": 1.1750122829820504
+        },
+        {
+            "x": 3.902794276163034,
+            "y": 5.279663847505986,
+            "heading": -0.48297175811002735,
+            "angularVelocity": -3.5611987502134074e-7,
+            "velocityX": 1.6998295347945822,
+            "velocityY": -3.379525824397886,
+            "timestamp": 1.1868293556656235
+        },
+        {
+            "x": 3.92391456197569,
+            "y": 5.240264436686968,
+            "heading": -0.48297176267932185,
+            "angularVelocity": -3.8666890356003695e-7,
+            "velocityX": 1.78726884214022,
+            "velocityY": -3.3341092057245874,
+            "timestamp": 1.1986464283491967
+        },
+        {
+            "x": 3.9460538007835475,
+            "y": 5.201428448582004,
+            "heading": -0.4829717676728866,
+            "angularVelocity": -4.225720568590589e-7,
+            "velocityX": 1.8734960341433124,
+            "velocityY": -3.286430501434571,
+            "timestamp": 1.2104635010327698
+        },
+        {
+            "x": 3.969196975663963,
+            "y": 5.163182231498957,
+            "heading": -0.4829717731698369,
+            "angularVelocity": -4.651702214495778e-7,
+            "velocityX": 1.9584524441985565,
+            "velocityY": -3.2365221156854496,
+            "timestamp": 1.222280573716343
+        },
+        {
+            "x": 3.993328387583419,
+            "y": 5.125551733327025,
+            "heading": -0.48297177927069973,
+            "angularVelocity": -5.162753076431712e-7,
+            "velocityX": 2.0420803498146496,
+            "velocityY": -3.184417933236803,
+            "timestamp": 1.234097646399916
+        },
+        {
+            "x": 4.018431663513184,
+            "y": 5.088562488555908,
+            "heading": -0.4829717875114574,
+            "angularVelocity": -6.973603296302728e-7,
+            "velocityX": 2.124322715274553,
+            "velocityY": -3.1301529373289303,
+            "timestamp": 1.2459147190834892
+        },
+        {
+            "x": 4.068270320597518,
+            "y": 5.022262107474161,
+            "heading": -0.48297170333499495,
+            "angularVelocity": 0.000003839169547390073,
+            "velocityX": 2.2730707479901486,
+            "velocityY": -3.023866725833157,
+            "timestamp": 1.2678404145896236
+        },
+        {
+            "x": 4.121253956507243,
+            "y": 4.9584470041490425,
+            "heading": -0.48297162876403243,
+            "angularVelocity": 0.0000034010762602073378,
+            "velocityX": 2.4165087896481814,
+            "velocityY": -2.910516718033651,
+            "timestamp": 1.289766110095758
+        },
+        {
+            "x": 4.177258824192737,
+            "y": 4.89726621863139,
+            "heading": -0.4829715614847619,
+            "angularVelocity": 0.0000030685124925835,
+            "velocityX": 2.5543029031769633,
+            "velocityY": -2.7903692040481487,
+            "timestamp": 1.3116918056018925
+        },
+        {
+            "x": 4.236154120064876,
+            "y": 4.83886263701252,
+            "heading": -0.482971499836949,
+            "angularVelocity": 0.000002811669663014854,
+            "velocityX": 2.686131249777681,
+            "velocityY": -2.6637048572771733,
+            "timestamp": 1.333617501108027
+        },
+        {
+            "x": 4.2978022891900425,
+            "y": 4.7833726571672255,
+            "heading": -0.48297144258671054,
+            "angularVelocity": 0.0000026111025032599054,
+            "velocityX": 2.811685910164931,
+            "velocityY": -2.530819596111362,
+            "timestamp": 1.3555431966141613
+        },
+        {
+            "x": 4.362059346053497,
+            "y": 4.730925869094327,
+            "heading": -0.48297138878758666,
+            "angularVelocity": 0.0000024537020442790352,
+            "velocityX": 2.9306735946176077,
+            "velocityY": -2.392023918156931,
+            "timestamp": 1.3774688921202958
+        },
+        {
+            "x": 4.428775209886728,
+            "y": 4.681644750011071,
+            "heading": -0.4829713376913353,
+            "angularVelocity": 0.000002330427847533908,
+            "velocityX": 3.042816307221113,
+            "velocityY": -2.247642227334136,
+            "timestamp": 1.3993945876264302
+        },
+        {
+            "x": 4.497794001617253,
+            "y": 4.635644296035039,
+            "heading": -0.4829712886902182,
+            "angularVelocity": 0.000002234871730357886,
+            "velocityX": 3.147849595521449,
+            "velocityY": -2.098015726031637,
+            "timestamp": 1.4213202831325646
+        },
+        {
+            "x": 4.568948026555497,
+            "y": 4.593021059923411,
+            "heading": -0.4829712412701429,
+            "angularVelocity": 0.0000021627626491587156,
+            "velocityX": 3.2452345659144926,
+            "velocityY": -1.9439855898636562,
+            "timestamp": 1.443245978638699
+        },
+        {
+            "x": 4.64205867202195,
+            "y": 4.55384875924605,
+            "heading": -0.4829711949807044,
+            "angularVelocity": 0.000002111195903735364,
+            "velocityX": 3.3344732643018493,
+            "velocityY": -1.7865932994667033,
+            "timestamp": 1.4651716741448335
+        },
+        {
+            "x": 4.71691342103475,
+            "y": 4.5181219873551415,
+            "heading": -0.4829711494130406,
+            "angularVelocity": 0.000002078276777149451,
+            "velocityX": 3.414019363346236,
+            "velocityY": -1.6294475986389252,
+            "timestamp": 1.487097369650968
+        },
+        {
+            "x": 4.7930985438005,
+            "y": 4.485327738304056,
+            "heading": -0.4829711041848689,
+            "angularVelocity": 0.000002062793021590678,
+            "velocityX": 3.4746958309446994,
+            "velocityY": -1.4956993743693363,
+            "timestamp": 1.5090230651571024
+        },
+        {
+            "x": 4.8695687201769084,
+            "y": 4.453203793512952,
+            "heading": -0.4829710590084327,
+            "angularVelocity": 0.0000020604334399265313,
+            "velocityX": 3.48769672346274,
+            "velocityY": -1.4651277439372516,
+            "timestamp": 1.5309487606632368
+        },
+        {
+            "x": 4.94605170532014,
+            "y": 4.421110356684282,
+            "heading": -0.48297101383350444,
+            "angularVelocity": 0.0000020603646626736416,
+            "velocityX": 3.4882809132254033,
+            "velocityY": -1.463736318862417,
+            "timestamp": 1.5528744561693713
+        },
+        {
+            "x": 5.022532332392591,
+            "y": 4.389011300831823,
+            "heading": -0.48297096865831407,
+            "angularVelocity": 0.000002060376618948169,
+            "velocityX": 3.4881733649487248,
+            "velocityY": -1.463992594598631,
+            "timestamp": 1.5748001516755057
+        },
+        {
+            "x": 5.099012556138694,
+            "y": 4.356911284014184,
+            "heading": -0.48297092348307896,
+            "angularVelocity": 0.000002060378658787562,
+            "velocityX": 3.488154969803465,
+            "velocityY": -1.4640364228636573,
+            "timestamp": 1.5967258471816401
+        },
+        {
+            "x": 5.175492855161546,
+            "y": 4.324811446548553,
+            "heading": -0.48297087830785207,
+            "angularVelocity": 0.0000020603782854914656,
+            "velocityX": 3.48815840306965,
+            "velocityY": -1.46402824287194,
+            "timestamp": 1.6186515426877746
+        },
+        {
+            "x": 5.2519731561146505,
+            "y": 4.292711613681912,
+            "heading": -0.48297083313262523,
+            "angularVelocity": 0.0000020603782806948455,
+            "velocityX": 3.488158491105669,
+            "velocityY": -1.464028033118535,
+            "timestamp": 1.640577238193909
+        },
+        {
+            "x": 5.328453454863713,
+            "y": 4.260611775564003,
+            "heading": -0.48297078795739806,
+            "angularVelocity": 0.0000020603782977173096,
+            "velocityX": 3.488158390582454,
+            "velocityY": -1.4640282726214169,
+            "timestamp": 1.6625029337000434
+        },
+        {
+            "x": 5.404933754197733,
+            "y": 4.228511938839819,
+            "heading": -0.4829707427821709,
+            "angularVelocity": 0.000002060378298482567,
+            "velocityX": 3.4881584172616003,
+            "velocityY": -1.4640282090556718,
+            "timestamp": 1.6844286292061779
+        },
+        {
+            "x": 5.481414053769572,
+            "y": 4.196412102682258,
+            "heading": -0.48297069760694356,
+            "angularVelocity": 0.000002060378303797646,
+            "velocityX": 3.488158428108085,
+            "velocityY": -1.4640281832128117,
+            "timestamp": 1.7063543247123123
+        },
+        {
+            "x": 5.5578943539285115,
+            "y": 4.164312267923512,
+            "heading": -0.4829706524317162,
+            "angularVelocity": 0.000002060378305885037,
+            "velocityX": 3.488158454884962,
+            "velocityY": -1.4640281194148257,
+            "timestamp": 1.7282800202184467
+        },
+        {
+            "x": 5.634374653438418,
+            "y": 4.132212431618396,
+            "heading": -0.4829706072564887,
+            "angularVelocity": 0.0000020603783152723795,
+            "velocityX": 3.4881584252834856,
+            "velocityY": -1.4640281899425134,
+            "timestamp": 1.7502057157245812
+        },
+        {
+            "x": 5.71085495402441,
+            "y": 4.100112597877132,
+            "heading": -0.4829705620812611,
+            "angularVelocity": 0.0000020603783147781934,
+            "velocityX": 3.4881584743622165,
+            "velocityY": -1.4640280730089004,
+            "timestamp": 1.7721314112307156
+        },
+        {
+            "x": 5.7873352532620705,
+            "y": 4.068012760923364,
+            "heading": -0.4829705169060333,
+            "angularVelocity": 0.0000020603783267765002,
+            "velocityX": 3.488158412866673,
+            "velocityY": -1.4640282195266534,
+            "timestamp": 1.79405710673685
+        },
+        {
+            "x": 5.863815552376704,
+            "y": 4.035912923676468,
+            "heading": -0.4829704717308054,
+            "angularVelocity": 0.000002060378332596705,
+            "velocityX": 3.4881584072556215,
+            "velocityY": -1.4640282328957663,
+            "timestamp": 1.8159828022429845
+        },
+        {
+            "x": 5.940295849970619,
+            "y": 4.00381308280633,
+            "heading": -0.48297042655557715,
+            "angularVelocity": 0.0000020603783450065593,
+            "velocityX": 3.4881583378978336,
+            "velocityY": -1.4640283981467839,
+            "timestamp": 1.837908497749119
+        },
+        {
+            "x": 6.016776146567219,
+            "y": 3.971713239559965,
+            "heading": -0.4829703813803487,
+            "angularVelocity": 0.0000020603783564721777,
+            "velocityX": 3.4881582924116907,
+            "velocityY": -1.4640285065230472,
+            "timestamp": 1.8598341932552533
+        },
+        {
+            "x": 6.093256433235532,
+            "y": 3.939613372658632,
+            "heading": -0.48297033620511903,
+            "angularVelocity": 0.000002060378411227836,
+            "velocityX": 3.4881578395965716,
+            "velocityY": -1.464029585392675,
+            "timestamp": 1.8817598887613878
+        },
+        {
+            "x": 6.169736625426417,
+            "y": 3.9075132806585757,
+            "heading": -0.4829702910298788,
+            "angularVelocity": 0.0000020603788925938424,
+            "velocityX": 3.488153530614592,
+            "velocityY": -1.4640398518273146,
+            "timestamp": 1.9036855842675222
+        },
+        {
+            "x": 6.2462171159811755,
+            "y": 3.8754138995336747,
+            "heading": -0.4829702458546715,
+            "angularVelocity": 0.0000020603773887814844,
+            "velocityX": 3.4881671385698874,
+            "velocityY": -1.4640074298145878,
+            "timestamp": 1.9256112797736566
+        },
+        {
+            "x": 6.322701012459848,
+            "y": 3.843322634623867,
+            "heading": -0.4829702006798376,
+            "angularVelocity": 0.0000020603603619090433,
+            "velocityX": 3.4883224779483077,
+            "velocityY": -1.4636372607092853,
+            "timestamp": 1.947536975279791
+        },
+        {
+            "x": 6.399174860669393,
+            "y": 3.811207431927173,
+            "heading": -0.4829701555038708,
+            "angularVelocity": 0.000002060412030184656,
+            "velocityX": 3.4878641905846774,
+            "velocityY": -1.4647290293616972,
+            "timestamp": 1.9694626707859255
+        },
+        {
+            "x": 6.47530722887592,
+            "y": 3.7782908978955247,
+            "heading": -0.4829701102655904,
+            "angularVelocity": 0.0000020632540676234596,
+            "velocityX": 3.4722897700184046,
+            "velocityY": -1.501276619595699,
+            "timestamp": 1.99138836629206
+        },
+        {
+            "x": 6.550061088980348,
+            "y": 3.742353506827061,
+            "heading": -0.48297006466550657,
+            "angularVelocity": 0.0000020797554102610617,
+            "velocityX": 3.409417962749362,
+            "velocityY": -1.6390536418100325,
+            "timestamp": 2.0133140617981944
+        },
+        {
+            "x": 6.623057371447113,
+            "y": 3.702968505083503,
+            "heading": -0.482970018320009,
+            "angularVelocity": 0.0000021137526750992832,
+            "velocityX": 3.3292573294334487,
+            "velocityY": -1.796294294637834,
+            "timestamp": 2.035239757304329
+        },
+        {
+            "x": 6.694087047655727,
+            "y": 3.6601383683558693,
+            "heading": -0.48296997081845616,
+            "angularVelocity": 0.0000021664787259977457,
+            "velocityX": 3.2395631960090396,
+            "velocityY": -1.9534220346951927,
+            "timestamp": 2.0571654528104633
+        },
+        {
+            "x": 6.762971951373863,
+            "y": 3.6139376604858007,
+            "heading": -0.48296992170702124,
+            "angularVelocity": 0.000002239903173580672,
+            "velocityX": 3.141743152404488,
+            "velocityY": -2.1071490232631853,
+            "timestamp": 2.0790911483165977
+        },
+        {
+            "x": 6.829544518243924,
+            "y": 3.564463136615443,
+            "heading": -0.4829698704667692,
+            "angularVelocity": 0.0000023369955126814126,
+            "velocityX": 3.0362807351500685,
+            "velocityY": -2.256463146473092,
+            "timestamp": 2.101016843822732
+        },
+        {
+            "x": 6.893649101257324,
+            "y": 3.5118300914764404,
+            "heading": -0.48296981752333895,
+            "angularVelocity": 0.0000024146750677216304,
+            "velocityX": 2.923719477698919,
+            "velocityY": -2.400518839837981,
+            "timestamp": 2.1229425393288666
+        },
+        {
+            "x": 6.935492251520178,
+            "y": 3.475140361342702,
+            "heading": -0.48296976475625003,
+            "angularVelocity": 0.000003586928740757824,
+            "velocityX": 2.8443562341956334,
+            "velocityY": -2.4940441143007757,
+            "timestamp": 2.1376534780501846
+        },
+        {
+            "x": 6.976123919914837,
+            "y": 3.4371133556518583,
+            "heading": -0.4829697143572031,
+            "angularVelocity": 0.0000034259572361559534,
+            "velocityX": 2.7620037826529793,
+            "velocityY": -2.5849475965623756,
+            "timestamp": 2.1523644167715026
+        },
+        {
+            "x": 7.015501922342544,
+            "y": 3.3977895885157845,
+            "heading": -0.4829696659490819,
+            "angularVelocity": 0.0000032906208164279545,
+            "velocityX": 2.676783798347444,
+            "velocityY": -2.673097066136968,
+            "timestamp": 2.1670753554928206
+        },
+        {
+            "x": 7.053586816421239,
+            "y": 3.357212172853621,
+            "heading": -0.4829696191989941,
+            "angularVelocity": 0.000003177913298539426,
+            "velocityX": 2.5888826539333314,
+            "velocityY": -2.758315864871489,
+            "timestamp": 2.1817862942141386
+        },
+        {
+            "x": 7.090341266441407,
+            "y": 3.315425855846777,
+            "heading": -0.48296957381059136,
+            "angularVelocity": 0.0000030853505387280562,
+            "velocityX": 2.4984435539051053,
+            "velocityY": -2.8404929011286835,
+            "timestamp": 2.1964972329354566
+        },
+        {
+            "x": 7.125730460448936,
+            "y": 3.2724771552259067,
+            "heading": -0.48296952951403316,
+            "angularVelocity": 0.000003011130632666104,
+            "velocityX": 2.405638054643597,
+            "velocityY": -2.9195078189422343,
+            "timestamp": 2.2112081716567746
+        },
+        {
+            "x": 7.1597225937186835,
+            "y": 3.228414468562161,
+            "heading": -0.48296947336189594,
+            "angularVelocity": 0.000003817032908672554,
+            "velocityX": 2.3106705774318574,
+            "velocityY": -2.9952328330951756,
+            "timestamp": 2.2259191103780926
+        },
+        {
+            "x": 7.192119389825632,
+            "y": 3.1836076694729414,
+            "heading": -0.4820443638475483,
+            "angularVelocity": 0.06288582475110666,
+            "velocityX": 2.2022249375566023,
+            "velocityY": -3.0458150861769275,
+            "timestamp": 2.2406300490994107
+        },
+        {
+            "x": 7.223229630823693,
+            "y": 3.1396490395754038,
+            "heading": -0.47743685492285376,
+            "angularVelocity": 0.3132029173651504,
+            "velocityX": 2.11476926030427,
+            "velocityY": -2.988159405071387,
+            "timestamp": 2.2553409878207287
+        },
+        {
+            "x": 7.253205323915578,
+            "y": 3.0967066455951215,
+            "heading": -0.4690721734359265,
+            "angularVelocity": 0.5686028366637977,
+            "velocityX": 2.0376465200313345,
+            "velocityY": -2.9190791147849615,
+            "timestamp": 2.2700519265420467
+        },
+        {
+            "x": 7.28210093319025,
+            "y": 3.0548329265465135,
+            "heading": -0.45697001578434165,
+            "angularVelocity": 0.8226638612835941,
+            "velocityX": 1.9642260648397363,
+            "velocityY": -2.8464341971547444,
+            "timestamp": 2.2847628652633647
+        },
+        {
+            "x": 7.309941019631162,
+            "y": 3.0140523923628573,
+            "heading": -0.44116512849373885,
+            "angularVelocity": 1.0743629342769347,
+            "velocityX": 1.892475182468747,
+            "velocityY": -2.7721231769227326,
+            "timestamp": 2.2994738039846827
+        },
+        {
+            "x": 7.336737361434823,
+            "y": 2.9743781069216433,
+            "heading": -0.42169586611687776,
+            "angularVelocity": 1.3234547941286585,
+            "velocityX": 1.8215249421732278,
+            "velocityY": -2.696924118358335,
+            "timestamp": 2.3141847427060007
+        },
+        {
+            "x": 7.362502163456035,
+            "y": 2.936038971640856,
+            "heading": -0.39992762164634665,
+            "angularVelocity": 1.479731843283817,
+            "velocityX": 1.7514043467447793,
+            "velocityY": -2.606165113394762,
+            "timestamp": 2.3288956814273187
+        },
+        {
+            "x": 7.3872337303102205,
+            "y": 2.8991268417513614,
+            "heading": -0.37676830696947144,
+            "angularVelocity": 1.5742921043722566,
+            "velocityX": 1.6811685047907687,
+            "velocityY": -2.5091620996289765,
+            "timestamp": 2.3436066201486367
+        },
+        {
+            "x": 7.410930819346905,
+            "y": 2.863676250815302,
+            "heading": -0.3528129850042874,
+            "angularVelocity": 1.6284019952084616,
+            "velocityX": 1.6108481916483202,
+            "velocityY": -2.4098116107768903,
+            "timestamp": 2.3583175588699548
+        },
+        {
+            "x": 7.433596368468817,
+            "y": 2.8297026440026514,
+            "heading": -0.328491545307115,
+            "angularVelocity": 1.6532894438563033,
+            "velocityX": 1.5407275872250867,
+            "velocityY": -2.3094112113606737,
+            "timestamp": 2.3730284975912728
+        },
+        {
+            "x": 7.455235422231159,
+            "y": 2.7972130374670496,
+            "heading": -0.30412705199668255,
+            "angularVelocity": 1.6562160832826247,
+            "velocityX": 1.4709498946511312,
+            "velocityY": -2.208533877482683,
+            "timestamp": 2.387739436312591
+        },
+        {
+            "x": 7.475853646381716,
+            "y": 2.766210258525358,
+            "heading": -0.2799689621545816,
+            "angularVelocity": 1.6421854716240782,
+            "velocityX": 1.4015573405031783,
+            "velocityY": -2.1074643521399103,
+            "timestamp": 2.402450375033909
+        },
+        {
+            "x": 7.495456618827617,
+            "y": 2.736694950498885,
+            "heading": -0.2562142168141467,
+            "angularVelocity": 1.6147674727249637,
+            "velocityX": 1.332543953669861,
+            "velocityY": -2.006351095984268,
+            "timestamp": 2.417161313755227
+        },
+        {
+            "x": 7.514049552575163,
+            "y": 2.7086666100983954,
+            "heading": -0.23302134486774898,
+            "angularVelocity": 1.576573214378762,
+            "velocityX": 1.2638849294234806,
+            "velocityY": -1.9052720517334563,
+            "timestamp": 2.431872252476545
+        },
+        {
+            "x": 7.5316372223467996,
+            "y": 2.682124150428342,
+            "heading": -0.2105202201245662,
+            "angularVelocity": 1.5295505724985052,
+            "velocityX": 1.1955504747055303,
+            "velocityY": -1.804266890976241,
+            "timestamp": 2.446583191197863
+        },
+        {
+            "x": 7.5482239779420395,
+            "y": 2.6570662133282936,
+            "heading": -0.18881898554249216,
+            "angularVelocity": 1.4751767370647726,
+            "velocityX": 1.127511704688336,
+            "velocityY": -1.7033540533846026,
+            "timestamp": 2.461294129919181
+        },
+        {
+            "x": 7.563813788966016,
+            "y": 2.6334913430066904,
+            "heading": -0.16800906628713844,
+            "angularVelocity": 1.414588127214289,
+            "velocityX": 1.0597427750401904,
+            "velocityY": -1.602540175593351,
+            "timestamp": 2.476005068640499
+        },
+        {
+            "x": 7.5784102960217234,
+            "y": 2.611398080774854,
+            "heading": -0.1481688563613113,
+            "angularVelocity": 1.3486705574455213,
+            "velocityX": 0.9922213213052737,
+            "velocityY": -1.5018254545389353,
+            "timestamp": 2.490716007361817
+        },
+        {
+            "x": 7.5920168580101715,
+            "y": 2.5907850143215074,
+            "heading": -0.1293664624719475,
+            "angularVelocity": 1.2781233234366292,
+            "velocityX": 0.9249281943327172,
+            "velocityY": -1.401206737641836,
+            "timestamp": 2.505426946083135
+        },
+        {
+            "x": 7.604636591974509,
+            "y": 2.571650800836196,
+            "heading": -0.1116617644111604,
+            "angularVelocity": 1.2035056631111207,
+            "velocityX": 0.8578469534407521,
+            "velocityY": -1.3006793004707733,
+            "timestamp": 2.520137884804453
+        },
+        {
+            "x": 7.616272404986451,
+            "y": 2.5539941753706046,
+            "heading": -0.09510797189421191,
+            "angularVelocity": 1.1252709857977845,
+            "velocityX": 0.790963325479677,
+            "velocityY": -1.20023785021998,
+            "timestamp": 2.534848823525771
+        },
+        {
+            "x": 7.626927018849464,
+            "y": 2.537813951229366,
+            "heading": -0.07975280576069126,
+            "angularVelocity": 1.0437924067530058,
+            "velocityX": 0.7242647165386202,
+            "velocityY": -1.0998770675179592,
+            "timestamp": 2.549559762247089
+        },
+        {
+            "x": 7.636602988821212,
+            "y": 2.523109016437205,
+            "heading": -0.0656393963876239,
+            "angularVelocity": 0.9593819701400241,
+            "velocityX": 0.6577398053956987,
+            "velocityY": -0.9995918731448548,
+            "timestamp": 2.564270700968407
+        },
+        {
+            "x": 7.64530271759392,
+            "y": 2.509878328655525,
+            "heading": -0.05280696792868869,
+            "angularVelocity": 0.8723052078477697,
+            "velocityX": 0.5913782211668082,
+            "velocityY": -0.899377533434203,
+            "timestamp": 2.578981639689725
+        },
+        {
+            "x": 7.653028465638572,
+            "y": 2.498120909886367,
+            "heading": -0.04129135982777762,
+            "angularVelocity": 0.7827922010322448,
+            "velocityX": 0.5251702961317726,
+            "velocityY": -0.799229674726393,
+            "timestamp": 2.593692578411043
+        },
+        {
+            "x": 7.6597823588265515,
+            "y": 2.48783584165749,
+            "heading": -0.03112542462311605,
+            "angularVelocity": 0.6910459894669937,
+            "velocityX": 0.4591068806636599,
+            "velocityY": -0.6991442506638482,
+            "timestamp": 2.608403517132361
+        },
+        {
+            "x": 7.665566394043072,
+            "y": 2.4790222609826817,
+            "heading": -0.02233933188139214,
+            "angularVelocity": 0.5972489525084974,
+            "velocityX": 0.39317920671771145,
+            "velocityY": -0.5991174894934748,
+            "timestamp": 2.623114455853679
+        },
+        {
+            "x": 7.670382443324971,
+            "y": 2.471679357147111,
+            "heading": -0.014960801225629315,
+            "angularVelocity": 0.5015676290643761,
+            "velocityX": 0.3273787875222405,
+            "velocityY": -0.4991458379831521,
+            "timestamp": 2.637825394574997
+        },
+        {
+            "x": 7.674232256901065,
+            "y": 2.4658063692226495,
+            "heading": -0.00901528221740954,
+            "angularVelocity": 0.40415633025538694,
+            "velocityX": 0.2616973429788216,
+            "velocityY": -0.399225911800697,
+            "timestamp": 2.652536333296315
+        },
+        {
+            "x": 7.677117465389097,
+            "y": 2.461402584136771,
+            "heading": -0.004526094899743595,
+            "angularVelocity": 0.30515981357196037,
+            "velocityX": 0.19612674233020058,
+            "velocityY": -0.2993544578835629,
+            "timestamp": 2.667247272017633
+        },
+        {
+            "x": 7.679039581308467,
+            "y": 2.45846733507745,
+            "heading": -0.001514541807715108,
+            "angularVelocity": 0.20471522239871504,
+            "velocityX": 0.13065895764927307,
+            "velocityY": -0.1995283315991124,
+            "timestamp": 2.681958210738951
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 7.235538726171091e-30,
+            "angularVelocity": 0.10295344412796348,
+            "velocityX": 0.06528602353168424,
+            "velocityY": -0.09974448981448966,
+            "timestamp": 2.696669149460269
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 1.9723672087239994e-30,
+            "angularVelocity": -3.5777263674767652e-28,
+            "velocityX": 6.106461964279306e-29,
+            "velocityY": 3.383375524617378e-29,
+            "timestamp": 2.711380088181587
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.traj
index 1f520266..0d7f85e3 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.traj	
@@ -3,1091 +3,1451 @@
         {
             "x": 1.35,
             "y": 5.56,
-            "heading": -1.9296664556128306e-33,
+            "heading": 0,
             "angularVelocity": 0,
-            "velocityX": -7.813211441980168e-34,
-            "velocityY": -2.2407271587906947e-32,
+            "velocityX": 0,
+            "velocityY": 3.439885530040909e-30,
             "timestamp": 0
         },
         {
-            "x": 1.35439418502164,
-            "y": 5.559271848784049,
-            "heading": -0.0037613483747314606,
-            "angularVelocity": -0.16062668985124567,
-            "velocityX": 0.18765169410035007,
-            "velocityY": -0.031095370031394967,
-            "timestamp": 0.023416708507251784
-        },
-        {
-            "x": 1.3631871179840578,
-            "y": 5.557813607446842,
-            "heading": -0.011205273313316288,
-            "angularVelocity": -0.317889464963847,
-            "velocityX": 0.37549824560930783,
-            "velocityY": -0.06227354014150002,
-            "timestamp": 0.04683341701450357
-        },
-        {
-            "x": 1.3763837173940232,
-            "y": 5.5556231238892515,
-            "heading": -0.02224504855076525,
-            "angularVelocity": -0.47144863395424863,
-            "velocityX": 0.563554839736692,
-            "velocityY": -0.09354361467639355,
-            "timestamp": 0.07025012552175536
-        },
-        {
-            "x": 1.3939893513502628,
-            "y": 5.552698044632534,
-            "heading": -0.03678367986427699,
-            "angularVelocity": -0.6208657083043512,
-            "velocityX": 0.7518406761047259,
-            "velocityY": -0.12491419346204678,
-            "timestamp": 0.09366683402900713
-        },
-        {
-            "x": 1.4160099408355828,
-            "y": 5.549035829570074,
-            "heading": -0.0547111148835232,
-            "angularVelocity": -0.7655830456997818,
-            "velocityX": 0.9403793653792271,
-            "velocityY": -0.15639324635770782,
-            "timestamp": 0.11708354253625891
-        },
-        {
-            "x": 1.4424520698548373,
-            "y": 5.544633771060564,
-            "heading": -0.07590088077413953,
-            "angularVelocity": -0.9048994176125892,
-            "velocityX": 1.129199221618411,
-            "velocityY": -0.18798792785714832,
-            "timestamp": 0.1405002510435107
-        },
-        {
-            "x": 1.4733230980825243,
-            "y": 5.539489016814175,
-            "heading": -0.10020600379509263,
-            "angularVelocity": -1.0379393420482537,
-            "velocityX": 1.3183333694454236,
-            "velocityY": -0.2197044151102235,
-            "timestamp": 0.16391695955076246
-        },
-        {
-            "x": 1.5086312686123933,
-            "y": 5.533598592872994,
-            "heading": -0.12745397379473564,
-            "angularVelocity": -1.163612298082128,
-            "velocityX": 1.5078195348818708,
-            "velocityY": -0.25154790389774145,
-            "timestamp": 0.18733366805801424
-        },
-        {
-            "x": 1.5483857981367395,
-            "y": 5.526959418437362,
-            "heading": -0.15744034724886685,
-            "angularVelocity": -1.2805545854082991,
-            "velocityX": 1.6976992950155392,
-            "velocityY": -0.28352295684851286,
-            "timestamp": 0.21075037656526602
-        },
-        {
-            "x": 1.5925969268012463,
-            "y": 5.5195682985391485,
-            "heading": -0.18992026053497643,
-            "angularVelocity": -1.3870400819163256,
-            "velocityX": 1.888016355962865,
-            "velocityY": -0.31563444947560276,
-            "timestamp": 0.2341670850725178
-        },
-        {
-            "x": 1.6412758830317984,
-            "y": 5.511421875068327,
-            "heading": -0.22459650029504458,
-            "angularVelocity": -1.4808332157070316,
-            "velocityX": 2.0788129217851563,
-            "velocityY": -0.34788934868040844,
-            "timestamp": 0.25758379357976957
-        },
-        {
-            "x": 1.6944346649736404,
-            "y": 5.502516516620167,
-            "heading": -0.26110152083126115,
-            "angularVelocity": -1.5589304758570732,
-            "velocityX": 2.2701218630013136,
-            "velocityY": -0.38029932539011047,
-            "timestamp": 0.2810005020870214
-        },
-        {
-            "x": 1.7520853949904163,
-            "y": 5.492848149517658,
-            "heading": -0.298968117565775,
-            "angularVelocity": -1.6170759747377448,
-            "velocityX": 2.4619484843022064,
-            "velocityY": -0.4128832666435571,
-            "timestamp": 0.3044172105942732
-        },
-        {
-            "x": 1.8142385663960479,
-            "y": 5.482412126535505,
-            "heading": -0.337577206769387,
-            "angularVelocity": -1.6487837815317001,
-            "velocityX": 2.654223217852448,
-            "velocityY": -0.44566566556215986,
-            "timestamp": 0.327833919101525
-        },
-        {
-            "x": 1.8808979849725118,
-            "y": 5.471203531590798,
-            "heading": -0.3760544549024592,
-            "angularVelocity": -1.6431535679387552,
-            "velocityX": 2.8466604756095246,
-            "velocityY": -0.47865800358925525,
-            "timestamp": 0.3512506276087768
-        },
-        {
-            "x": 1.9520436894250033,
-            "y": 5.459218834920382,
-            "heading": -0.41303307424029884,
-            "angularVelocity": -1.5791552995754587,
-            "velocityX": 3.0382452952539354,
-            "velocityY": -0.5118010785637118,
-            "timestamp": 0.3746673361160286
-        },
-        {
-            "x": 2.027554046757936,
-            "y": 5.446452385802885,
-            "heading": -0.44595627669268334,
-            "angularVelocity": -1.4059705462956014,
-            "velocityX": 3.2246358325529645,
-            "velocityY": -0.5451854650516427,
-            "timestamp": 0.3980840446232804
-        },
-        {
-            "x": 2.106704107988529,
-            "y": 5.4327632732947455,
-            "heading": -0.46854805770485686,
-            "angularVelocity": -0.9647718425148408,
-            "velocityX": 3.3800677497471567,
-            "velocityY": -0.5845873899786794,
-            "timestamp": 0.4215007531305322
-        },
-        {
-            "x": 2.1895060473348913,
-            "y": 5.418170740153337,
-            "heading": -0.4808901513114338,
-            "angularVelocity": -0.5270635539044761,
-            "velocityX": 3.5360195614476866,
-            "velocityY": -0.6231675616108724,
-            "timestamp": 0.444917461637784
-        },
-        {
-            "x": 2.275961949822614,
-            "y": 5.402702256569038,
-            "heading": -0.4829709400280361,
-            "angularVelocity": -0.0888591458512548,
-            "velocityX": 3.6920604132278863,
-            "velocityY": -0.6605746311236733,
-            "timestamp": 0.4683341701450358
-        },
-        {
-            "x": 2.3633379318433296,
-            "y": 5.388122823546896,
-            "heading": -0.48297098226531077,
-            "angularVelocity": -0.0000018037238101032338,
-            "velocityX": 3.7313519956767345,
-            "velocityY": -0.6226081269122786,
-            "timestamp": 0.4917508786522876
-        },
-        {
-            "x": 2.4507217300981603,
-            "y": 5.373590311340196,
-            "heading": -0.48297102439627604,
-            "angularVelocity": -0.0000017991839149984119,
-            "velocityX": 3.7316857844376363,
-            "velocityY": -0.6206043946012527,
-            "timestamp": 0.5151675871595394
-        },
-        {
-            "x": 2.5381084152434026,
-            "y": 5.359075168632344,
-            "heading": -0.4829710665257657,
-            "angularVelocity": -0.0000017991208983915868,
-            "velocityX": 3.7318090677936033,
-            "velocityY": -0.6198626379688947,
-            "timestamp": 0.5385842956667911
-        },
-        {
-            "x": 2.6254950830921073,
-            "y": 5.344559921791865,
-            "heading": -0.482971108655264,
-            "angularVelocity": -0.0000017991212673817243,
-            "velocityX": 3.7318083291527238,
-            "velocityY": -0.6198670849061145,
-            "timestamp": 0.5620010041740429
-        },
-        {
-            "x": 2.712881673840345,
-            "y": 5.330044210788612,
-            "heading": -0.4829711507848006,
-            "angularVelocity": -0.0000017991229009417428,
-            "velocityX": 3.731805036611997,
-            "velocityY": -0.6198869067684731,
-            "timestamp": 0.5854177126812946
-        },
-        {
-            "x": 2.800268245782709,
-            "y": 5.315528386572539,
-            "heading": -0.4829711929143463,
-            "angularVelocity": -0.0000017991232935293638,
-            "velocityX": 3.7318042335155903,
-            "velocityY": -0.6198917414709255,
-            "timestamp": 0.6088344211885464
-        },
-        {
-            "x": 2.8876548130761686,
-            "y": 5.301012534370082,
-            "heading": -0.4829712350438941,
-            "angularVelocity": -0.0000017991233825733017,
-            "velocityX": 3.731804034986268,
-            "velocityY": -0.6198929366167053,
-            "timestamp": 0.6322511296957981
-        },
-        {
-            "x": 2.975041345179212,
-            "y": 5.286496470320027,
-            "heading": -0.48297127717345933,
-            "angularVelocity": -0.000001799124124149535,
-            "velocityX": 3.7318025321954105,
-            "velocityY": -0.6199019834729332,
-            "timestamp": 0.6556678382030499
-        },
-        {
-            "x": 3.0624277772479402,
-            "y": 5.271979804077107,
-            "heading": -0.4829713193030743,
-            "angularVelocity": -0.0000017991262516148556,
-            "velocityX": 3.7317982602749975,
-            "velocityY": -0.6199276998481766,
-            "timestamp": 0.6790845467103016
-        },
-        {
-            "x": 3.149814115388955,
-            "y": 5.257462572426413,
-            "heading": -0.482971361432736,
-            "angularVelocity": -0.0000017991282469783394,
-            "velocityX": 3.731794249134225,
-            "velocityY": -0.619951845332987,
-            "timestamp": 0.7025012552175534
-        },
-        {
-            "x": 3.2372009926614522,
-            "y": 5.242948586436876,
-            "heading": -0.48297140356212753,
-            "angularVelocity": -0.0000017991167072996656,
-            "velocityX": 3.7318172725014778,
-            "velocityY": -0.6198132408343549,
-            "timestamp": 0.7259179637248051
-        },
-        {
-            "x": 3.3245908005140246,
-            "y": 5.228452256133779,
-            "heading": -0.4829714456900571,
-            "angularVelocity": -0.0000017990542795775989,
-            "velocityX": 3.7319424216049217,
-            "velocityY": -0.6190592626894866,
-            "timestamp": 0.7493346722320569
-        },
-        {
-            "x": 3.4119833565424647,
-            "y": 5.213972502683899,
-            "heading": -0.48297148781659605,
-            "angularVelocity": -0.0000017989948869691892,
-            "velocityX": 3.7320597812190472,
-            "velocityY": -0.6183513556300411,
-            "timestamp": 0.7727513807393086
-        },
-        {
-            "x": 3.4993677183765524,
-            "y": 5.199443379550061,
-            "heading": -0.48297152994719206,
-            "angularVelocity": -0.0000017991681461203675,
-            "velocityX": 3.731709851836638,
-            "velocityY": -0.6204596657670539,
-            "timestamp": 0.7961680892465604
-        },
-        {
-            "x": 3.5867330233447126,
-            "y": 5.184800101210533,
-            "heading": -0.48297157208742414,
-            "angularVelocity": -0.0000017995796480013664,
-            "velocityX": 3.7308960369522555,
-            "velocityY": -0.6253346124593939,
-            "timestamp": 0.8195847977538121
-        },
-        {
-            "x": 3.6740864894013328,
-            "y": 5.170086363489935,
-            "heading": -0.48297161423379187,
-            "angularVelocity": -0.0000017998416699478457,
-            "velocityX": 3.7303904615618975,
-            "velocityY": -0.6283435486246598,
-            "timestamp": 0.8430015062610638
-        },
-        {
-            "x": 3.7614250146254347,
-            "y": 5.155284197469098,
-            "heading": -0.482971656398251,
-            "angularVelocity": -0.0000018006142553635733,
-            "velocityX": 3.729752420031712,
-            "velocityY": -0.6321198393988197,
-            "timestamp": 0.8664182147683156
-        },
-        {
-            "x": 3.848175799548237,
-            "y": 5.137355912505346,
-            "heading": -0.48297169899264486,
-            "angularVelocity": -0.000001818974422860047,
-            "velocityX": 3.704653234929829,
-            "velocityY": -0.7656193421980864,
-            "timestamp": 0.8898349232755673
-        },
-        {
-            "x": 3.9339293985898425,
-            "y": 5.115142351916783,
-            "heading": -0.48297174253890224,
-            "angularVelocity": -0.0000018596233270255789,
-            "velocityX": 3.6620688605764276,
-            "velocityY": -0.9486201095127581,
-            "timestamp": 0.9132516317828191
+            "x": 1.3518715996750759,
+            "y": 5.562488632849977,
+            "heading": -0.00039025483625088287,
+            "angularVelocity": -0.020188245249371933,
+            "velocityX": 0.09681959001984766,
+            "velocityY": 0.12873928941829918,
+            "timestamp": 0.019330795293514086
+        },
+        {
+            "x": 1.3556369831983786,
+            "y": 5.567448998179342,
+            "heading": -0.001175615506548702,
+            "angularVelocity": -0.04062743712160119,
+            "velocityX": 0.19478678792724713,
+            "velocityY": 0.256604306964468,
+            "timestamp": 0.03866159058702817
+        },
+        {
+            "x": 1.3613203761105706,
+            "y": 5.574862277415471,
+            "heading": -0.0023613813379046128,
+            "angularVelocity": -0.061340768103509684,
+            "velocityX": 0.2940071955600798,
+            "velocityY": 0.38349582226532697,
+            "timestamp": 0.05799238588054226
+        },
+        {
+            "x": 1.3689483300707626,
+            "y": 5.58470739953387,
+            "heading": -0.003953362765501964,
+            "angularVelocity": -0.08235467829569511,
+            "velocityX": 0.39460114518679135,
+            "velocityY": 0.5092973138928011,
+            "timestamp": 0.07732318117405634
+        },
+        {
+            "x": 1.3785500603498269,
+            "y": 5.596960627127346,
+            "heading": -0.0059579558445008625,
+            "angularVelocity": -0.10369946236363117,
+            "velocityX": 0.4967064279184545,
+            "velocityY": 0.6338708473927627,
+            "timestamp": 0.09665397646757043
+        },
+        {
+            "x": 1.3901578478597558,
+            "y": 5.611595038412612,
+            "heading": -0.008382231168191269,
+            "angularVelocity": -0.12541001479146147,
+            "velocityX": 0.6004816322183893,
+            "velocityY": 0.7570516920313731,
+            "timestamp": 0.11598477176108452
+        },
+        {
+            "x": 1.4038075203372553,
+            "y": 5.62857987161075,
+            "heading": -0.01123404053815137,
+            "angularVelocity": -0.14752674820972528,
+            "velocityX": 0.7061102386242472,
+            "velocityY": 0.8786412012669059,
+            "timestamp": 0.1353155670545986
+        },
+        {
+            "x": 1.4195390306280726,
+            "y": 5.647879684835849,
+            "heading": -0.014522145551621221,
+            "angularVelocity": -0.17009672719326588,
+            "velocityX": 0.8138056428591633,
+            "velocityY": 0.9983972688167116,
+            "timestamp": 0.1546463623481127
+        },
+        {
+            "x": 1.4373971536587393,
+            "y": 5.669453265066441,
+            "heading": -0.018256373225668265,
+            "angularVelocity": -0.19317506689959266,
+            "velocityX": 0.923817295642181,
+            "velocityY": 1.116021348476537,
+            "timestamp": 0.17397715764162677
+        },
+        {
+            "x": 1.457432326992564,
+            "y": 5.693252190583062,
+            "heading": -0.022447804782031403,
+            "angularVelocity": -0.21682664850159125,
+            "velocityX": 1.0364381304346626,
+            "velocityY": 1.2311405276019616,
+            "timestamp": 0.19330795293514086
+        },
+        {
+            "x": 1.47970166115617,
+            "y": 5.719218907072145,
+            "heading": -0.027109004507978946,
+            "angularVelocity": -0.24112819235695526,
+            "velocityX": 1.1520133458284385,
+            "velocityY": 1.3432823686149544,
+            "timestamp": 0.21263874822865494
+        },
+        {
+            "x": 1.5042701411060604,
+            "y": 5.747284109919373,
+            "heading": -0.03225429550223853,
+            "angularVelocity": -0.2661706834165162,
+            "velocityX": 1.2709502933970824,
+            "velocityY": 1.4518390175412745,
+            "timestamp": 0.23196954352216903
+        },
+        {
+            "x": 1.5312120197646597,
+            "y": 5.777363120811461,
+            "heading": -0.037900086436595266,
+            "angularVelocity": -0.2920620103121596,
+            "velocityX": 1.3937284136281272,
+            "velocityY": 1.5560151786502046,
+            "timestamp": 0.25130033881568314
+        },
+        {
+            "x": 1.5606123466917503,
+            "y": 5.809350786193417,
+            "heading": -0.044065244214455006,
+            "angularVelocity": -0.3189293396494718,
+            "velocityX": 1.5209062266028461,
+            "velocityY": 1.6547516486654796,
+            "timestamp": 0.27063113410919726
+        },
+        {
+            "x": 1.5925684302904153,
+            "y": 5.843114184319679,
+            "heading": -0.050771482089097104,
+            "angularVelocity": -0.34691991575183656,
+            "velocityX": 1.653117893674399,
+            "velocityY": 1.7466119532904336,
+            "timestamp": 0.28996192940271137
+        },
+        {
+            "x": 1.6271906907680806,
+            "y": 5.8784820940308995,
+            "heading": -0.058043668844819325,
+            "angularVelocity": -0.37619697717051565,
+            "velocityX": 1.791041700663091,
+            "velocityY": 1.8296148282676763,
+            "timestamp": 0.3092927246962255
+        },
+        {
+            "x": 1.6646015916598045,
+            "y": 5.915229821006168,
+            "heading": -0.06590980894370155,
+            "angularVelocity": -0.4069227354304089,
+            "velocityX": 1.935300660096276,
+            "velocityY": 1.9009940572697726,
+            "timestamp": 0.3286235199897396
+        },
+        {
+            "x": 1.7049297157110095,
+            "y": 5.953057987948439,
+            "heading": -0.07440007857488001,
+            "angularVelocity": -0.43920953598980744,
+            "velocityX": 2.086211324411271,
+            "velocityY": 1.9568862205562443,
+            "timestamp": 0.3479543152832537
+        },
+        {
+            "x": 1.7482931222517035,
+            "y": 5.991565656180575,
+            "heading": -0.08354352935308812,
+            "angularVelocity": -0.47299920357005454,
+            "velocityX": 2.2432293075517364,
+            "velocityY": 1.9920374535783947,
+            "timestamp": 0.3672851105767678
+        },
+        {
+            "x": 1.7947629094651933,
+            "y": 6.030225122456552,
+            "heading": -0.09335994332155541,
+            "angularVelocity": -0.5078122146252688,
+            "velocityX": 2.4039252657691588,
+            "velocityY": 1.9998901074156783,
+            "timestamp": 0.38661590587028194
+        },
+        {
+            "x": 1.844301928458138,
+            "y": 6.068381679285448,
+            "heading": -0.10384499839936924,
+            "angularVelocity": -0.5424016404194022,
+            "velocityX": 2.562699477220483,
+            "velocityY": 1.9738741344852324,
+            "timestamp": 0.40594670116379605
+        },
+        {
+            "x": 1.8967008920878712,
+            "y": 6.105314232951604,
+            "heading": -0.1149551002367539,
+            "angularVelocity": -0.5747358899978738,
+            "velocityX": 2.710647070341413,
+            "velocityY": 1.9105553137044158,
+            "timestamp": 0.42527749645731017
+        },
+        {
+            "x": 1.951571944971935,
+            "y": 6.140356350737874,
+            "heading": -0.126607814812745,
+            "angularVelocity": -0.6028057510857173,
+            "velocityX": 2.83853054418689,
+            "velocityY": 1.8127613093097652,
+            "timestamp": 0.4446082917508243
+        },
+        {
+            "x": 2.008427579479744,
+            "y": 6.172999646985284,
+            "heading": -0.13870025114422718,
+            "angularVelocity": -0.6255529660251172,
+            "velocityX": 2.9411947953783932,
+            "velocityY": 1.6886680424557017,
+            "timestamp": 0.4639390870443384
+        },
+        {
+            "x": 2.066782288063465,
+            "y": 6.202913115188225,
+            "heading": -0.15112974078850008,
+            "angularVelocity": -0.6429890470384952,
+            "velocityX": 3.018743290054933,
+            "velocityY": 1.5474515015415655,
+            "timestamp": 0.4832698823378525
+        },
+        {
+            "x": 2.126212031523665,
+            "y": 6.229902950259444,
+            "heading": -0.1638060412217236,
+            "angularVelocity": -0.6557567984529158,
+            "velocityX": 3.074355842986947,
+            "velocityY": 1.3962092434072775,
+            "timestamp": 0.5026006776313666
+        },
+        {
+            "x": 2.1863688602207922,
+            "y": 6.253863406934038,
+            "heading": -0.17665473442964857,
+            "angularVelocity": -0.6646748368514358,
+            "velocityX": 3.111968637798953,
+            "velocityY": 1.2394966844759612,
+            "timestamp": 0.5219314729248807
+        },
+        {
+            "x": 2.246973575487724,
+            "y": 6.2747405977079325,
+            "heading": -0.1896159238006329,
+            "angularVelocity": -0.6704943678821678,
+            "velocityX": 3.1351382261683427,
+            "velocityY": 1.0799964748940398,
+            "timestamp": 0.5412622682183948
+        },
+        {
+            "x": 2.30780256528041,
+            "y": 6.292510136082229,
+            "heading": -0.20264154258755296,
+            "angularVelocity": -0.6738273614273119,
+            "velocityX": 3.146740155750102,
+            "velocityY": 0.9192347290676759,
+            "timestamp": 0.560593063511909
+        },
+        {
+            "x": 2.3686753728703707,
+            "y": 6.307164203375093,
+            "heading": -0.2156927432279827,
+            "angularVelocity": -0.6751507344764429,
+            "velocityX": 3.1490068911124522,
+            "velocityY": 0.7580685155659881,
+            "timestamp": 0.5799238588054231
+        },
+        {
+            "x": 2.4294446969864714,
+            "y": 6.318704202261767,
+            "heading": -0.228737776875863,
+            "angularVelocity": -0.6748317102223548,
+            "velocityX": 3.143653594867338,
+            "velocityY": 0.5969748637576952,
+            "timestamp": 0.5992546540989372
+        },
+        {
+            "x": 2.4899887888473393,
+            "y": 6.32713657767754,
+            "heading": -0.24175037423147705,
+            "angularVelocity": -0.6731537506881662,
+            "velocityX": 3.132002121049924,
+            "velocityY": 0.43621461443967424,
+            "timestamp": 0.6185854493924513
+        },
+        {
+            "x": 2.550205774126716,
+            "y": 6.332470420209207,
+            "heading": -0.25470853343764965,
+            "angularVelocity": -0.6703376146412596,
+            "velocityX": 3.1150805936877464,
+            "velocityY": 0.2759246296222352,
+            "timestamp": 0.6379162446859654
+        },
+        {
+            "x": 2.610009417191064,
+            "y": 6.334716080800322,
+            "heading": -0.26759361438505785,
+            "angularVelocity": -0.6665572084213006,
+            "velocityX": 3.09369801688466,
+            "velocityY": 0.11617010873154458,
+            "timestamp": 0.6572470399794795
+        },
+        {
+            "x": 2.669325940254228,
+            "y": 6.333884367249437,
+            "heading": -0.28038965768785973,
+            "angularVelocity": -0.6619512083445117,
+            "velocityX": 3.0684988466597254,
+            "velocityY": -0.04302531469893782,
+            "timestamp": 0.6765778352729936
+        },
+        {
+            "x": 2.7280916104140402,
+            "y": 6.329986080406231,
+            "heading": -0.2930828674707726,
+            "angularVelocity": -0.656631534822156,
+            "velocityX": 3.040002714194029,
+            "velocityY": -0.2016620001410218,
+            "timestamp": 0.6959086305665078
+        },
+        {
+            "x": 2.7862508879843872,
+            "y": 6.323031751356315,
+            "heading": -0.3056612140555153,
+            "angularVelocity": -0.650689554866023,
+            "velocityX": 3.0086334621659443,
+            "velocityY": -0.35975390274029156,
+            "timestamp": 0.7152394258600219
+        },
+        {
+            "x": 2.843754988622848,
+            "y": 6.3130314987287885,
+            "heading": -0.31811412514136855,
+            "angularVelocity": -0.6442006599713835,
+            "velocityX": 2.974740550780866,
+            "velocityY": -0.5173223592555559,
+            "timestamp": 0.734570221153536
+        },
+        {
+            "x": 2.9005607537314586,
+            "y": 6.299994958196607,
+            "heading": -0.3304322429824912,
+            "angularVelocity": -0.6372276801904713,
+            "velocityX": 2.9386150050261657,
+            "velocityY": -0.6743923534566658,
+            "timestamp": 0.7539010164470501
+        },
+        {
+            "x": 2.956629753112793,
+            "y": 6.283931255340576,
+            "heading": -0.34260723133624194,
+            "angularVelocity": -0.6298234588328613,
+            "velocityX": 2.9005014294547,
+            "velocityY": -0.8309902728844937,
+            "timestamp": 0.7732318117405642
+        },
+        {
+            "x": 2.9906169983281425,
+            "y": 6.272983358953205,
+            "heading": -0.34999362222052477,
+            "angularVelocity": -0.625060967472129,
+            "velocityX": 2.8761137487624757,
+            "velocityY": -0.9264474104986867,
+            "timestamp": 0.7850488844241373
+        },
+        {
+            "x": 3.024312445989771,
+            "y": 6.26090838895792,
+            "heading": -0.35731821138357023,
+            "angularVelocity": -0.6198311002374715,
+            "velocityX": 2.8514208691014065,
+            "velocityY": -1.0218241284130187,
+            "timestamp": 0.7968659571077105
+        },
+        {
+            "x": 3.0577122238990846,
+            "y": 6.2477073818247915,
+            "heading": -0.36457506252100974,
+            "angularVelocity": -0.6140988831800235,
+            "velocityX": 2.8264003111145226,
+            "velocityY": -1.1171131367820717,
+            "timestamp": 0.8086830297912836
+        },
+        {
+            "x": 3.090812163934883,
+            "y": 6.233381471982772,
+            "heading": -0.3717577779780659,
+            "angularVelocity": -0.6078252752934934,
+            "velocityX": 2.8010270328463904,
+            "velocityY": -1.2123061459996218,
+            "timestamp": 0.8205001024748567
+        },
+        {
+            "x": 3.1236077670194575,
+            "y": 6.217931905831193,
+            "heading": -0.3788594429566703,
+            "angularVelocity": -0.6009665141923228,
+            "velocityX": 2.7752730276563167,
+            "velocityY": -1.307393680759415,
+            "timestamp": 0.8323171751584298
+        },
+        {
+            "x": 3.1560941623709735,
+            "y": 6.201360058471141,
+            "heading": -0.3858725603738981,
+            "angularVelocity": -0.5934733249949906,
+            "velocityX": 2.749106840704776,
+            "velocityY": -1.4023648498911914,
+            "timestamp": 0.844134247842003
+        },
+        {
+            "x": 3.188266059838492,
+            "y": 6.1836674538325775,
+            "heading": -0.3927889743327873,
+            "angularVelocity": -0.5852899566661331,
+            "velocityX": 2.722492983583009,
+            "velocityY": -1.4972070590002187,
+            "timestamp": 0.8559513205255761
+        },
+        {
+            "x": 3.2201176938016274,
+            "y": 6.164855789080512,
+            "heading": -0.399599779609834,
+            "angularVelocity": -0.576352998701121,
+            "velocityX": 2.695391220484986,
+            "velocityY": -1.5919056483603757,
+            "timestamp": 0.8677683932091492
+        },
+        {
+            "x": 3.251642756703662,
+            "y": 6.144926964467571,
+            "heading": -0.40629521382129,
+            "angularVelocity": -0.5665899153487628,
+            "velocityX": 2.6677556909553144,
+            "velocityY": -1.686443432022221,
+            "timestamp": 0.8795854658927224
+        },
+        {
+            "x": 3.282834319738247,
+            "y": 6.123883120194607,
+            "heading": -0.41286452792903033,
+            "angularVelocity": -0.5559172126335613,
+            "velocityX": 2.6395338227837533,
+            "velocityY": -1.7808001047684463,
+            "timestamp": 0.8914025385762955
+        },
+        {
+            "x": 3.3136847374734915,
+            "y": 6.101726682396313,
+            "heading": -0.4192958293777808,
+            "angularVelocity": -0.5442381223304499,
+            "velocityX": 2.6106649727330486,
+            "velocityY": -1.874951469926556,
+            "timestamp": 0.9032196112598686
+        },
+        {
+            "x": 3.3441855321977045,
+            "y": 6.078460421163092,
+            "heading": -0.42557589025707343,
+            "angularVelocity": -0.5314396422408687,
+            "velocityX": 2.5810787105179256,
+            "velocityY": -1.9688684208198879,
+            "timestamp": 0.9150366839434417
+        },
+        {
+            "x": 3.37432725239814,
+            "y": 6.0540875246663015,
+            "heading": -0.4316899102075926,
+            "angularVelocity": -0.5173887065125765,
+            "velocityX": 2.5506926298537165,
+            "velocityY": -2.0625155780475546,
+            "timestamp": 0.9268537566270149
+        },
+        {
+            "x": 3.4040992978747684,
+            "y": 6.028611695167346,
+            "heading": -0.43762121996231873,
+            "angularVelocity": -0.5019271619587302,
+            "velocityX": 2.519409525001445,
+            "velocityY": -2.1558494375996973,
+            "timestamp": 0.938670829310588
+        },
+        {
+            "x": 3.4334897013019283,
+            "y": 6.00203727528326,
+            "heading": -0.44335090582849124,
+            "angularVelocity": -0.4848650778070557,
+            "velocityX": 2.48711370524239,
+            "velocityY": -2.2488158104525207,
+            "timestamp": 0.9504879019941611
+        },
+        {
+            "x": 3.462484852215012,
+            "y": 5.974369416891247,
+            "heading": -0.4488573271132443,
+            "angularVelocity": -0.46597168623725893,
+            "velocityX": 2.4536661226929253,
+            "velocityY": -2.341346214318684,
+            "timestamp": 0.9623049746777342
+        },
+        {
+            "x": 3.4910691438628887,
+            "y": 5.945614311410449,
+            "heading": -0.45411548589649775,
+            "angularVelocity": -0.4449628875146618,
+            "velocityX": 2.4188978449470153,
+            "velocityY": -2.4333526797013256,
+            "timestamp": 0.9741220473613074
+        },
+        {
+            "x": 3.5192245152965964,
+            "y": 5.915779510545288,
+            "heading": -0.4590961890390477,
+            "angularVelocity": -0.42148366824158007,
+            "velocityX": 2.3826011896200363,
+            "velocityY": -2.524720094735007,
+            "timestamp": 0.9859391200448805
+        },
+        {
+            "x": 3.5469298492407,
+            "y": 5.884874383909108,
+            "heading": -0.46376491150225324,
+            "angularVelocity": -0.39508282535110545,
+            "velocityX": 2.3445175201991417,
+            "velocityY": -2.6152946219194915,
+            "timestamp": 0.9977561927284536
+        },
+        {
+            "x": 3.5741601690568516,
+            "y": 5.852910789899007,
+            "heading": -0.46808022074004524,
+            "angularVelocity": -0.36517582258680764,
+            "velocityX": 2.304320244556386,
+            "velocityY": -2.7048656520944543,
+            "timestamp": 1.0095732654120266
+        },
+        {
+            "x": 3.6008855536785225,
+            "y": 5.819904089620565,
+            "heading": -0.4719915430906053,
+            "angularVelocity": -0.3309891083260508,
+            "velocityX": 2.2615909487314654,
+            "velocityY": -2.7931367744165776,
+            "timestamp": 1.0213903380955998
+        },
+        {
+            "x": 3.62706965768041,
+            "y": 5.785874731899473,
+            "heading": -0.4754359323425916,
+            "angularVelocity": -0.2914756762708573,
+            "velocityX": 2.2157859821143067,
+            "velocityY": -2.8796774490856496,
+            "timestamp": 1.0332074107791729
+        },
+        {
+            "x": 3.652667693323084,
+            "y": 5.750850822222342,
+            "heading": -0.4783333470461038,
+            "angularVelocity": -0.24518887046705806,
+            "velocityX": 2.1661909280000957,
+            "velocityY": -2.9638397439848143,
+            "timestamp": 1.045024483462746
+        },
+        {
+            "x": 3.677623743737937,
+            "y": 5.7148724362586085,
+            "heading": -0.48057991095642455,
+            "angularVelocity": -0.1901117112907446,
+            "velocityX": 2.1118640024567648,
+            "velocityY": -3.044610702424323,
+            "timestamp": 1.0568415561463191
+        },
+        {
+            "x": 3.701867477642102,
+            "y": 5.67799904418439,
+            "heading": -0.48203966919964636,
+            "angularVelocity": -0.12352959843022769,
+            "velocityX": 2.0515854097999866,
+            "velocityY": -3.120349096733213,
+            "timestamp": 1.0686586288298923
+        },
+        {
+            "x": 3.725329217347855,
+            "y": 5.640287080856699,
+            "heading": -0.4827696693750387,
+            "angularVelocity": -0.06177504318881954,
+            "velocityX": 1.9854104594250916,
+            "velocityY": -3.191311785711063,
+            "timestamp": 1.0804757015134654
+        },
+        {
+            "x": 3.74792659306077,
+            "y": 5.60180905514361,
+            "heading": -0.48297166201638797,
+            "angularVelocity": -0.017093289239954308,
+            "velocityX": 1.9122651030425624,
+            "velocityY": -3.2561385330714976,
+            "timestamp": 1.0922927741970385
+        },
+        {
+            "x": 3.769555777117098,
+            "y": 5.56268673154527,
+            "heading": -0.48297173035274543,
+            "angularVelocity": -0.000005782849890954889,
+            "velocityX": 1.8303335043707702,
+            "velocityY": -3.310661163379628,
+            "timestamp": 1.1041098468806116
+        },
+        {
+            "x": 3.7901587344932506,
+            "y": 5.52301434523992,
+            "heading": -0.48297173483145855,
+            "angularVelocity": -3.790036003454015e-7,
+            "velocityX": 1.743490788949144,
+            "velocityY": -3.3572092994315077,
+            "timestamp": 1.1159269195641848
+        },
+        {
+            "x": 3.809721481337777,
+            "y": 5.482818833345491,
+            "heading": -0.4829717389588181,
+            "angularVelocity": -3.4927089311686727e-7,
+            "velocityX": 1.6554647135005363,
+            "velocityY": -3.401477926957743,
+            "timestamp": 1.127743992247758
+        },
+        {
+            "x": 3.8282308263038667,
+            "y": 5.442127501029994,
+            "heading": -0.482971742791846,
+            "angularVelocity": -3.2436357183409427e-7,
+            "velocityX": 1.5663223424036268,
+            "velocityY": -3.443435900336205,
+            "timestamp": 1.139561064931331
+        },
+        {
+            "x": 3.8456768429335884,
+            "y": 5.400969063215312,
+            "heading": -0.48297174636762497,
+            "angularVelocity": -3.0259430956931755e-7,
+            "velocityX": 1.4763399614162735,
+            "velocityY": -3.482963921504598,
+            "timestamp": 1.1513781376149042
+        },
+        {
+            "x": 3.863665898181474,
+            "y": 5.360045056052632,
+            "heading": -0.48297175000247344,
+            "angularVelocity": -3.075929693793739e-7,
+            "velocityX": 1.5222936957043627,
+            "velocityY": -3.463125619898205,
+            "timestamp": 1.1631952102984773
+        },
+        {
+            "x": 3.8827072670006824,
+            "y": 5.319599949808908,
+            "heading": -0.4829717539017329,
+            "angularVelocity": -3.299683104273868e-7,
+            "velocityX": 1.6113439706329509,
+            "velocityY": -3.4225994310711667,
+            "timestamp": 1.1750122829820504
+        },
+        {
+            "x": 3.902794276163034,
+            "y": 5.279663847505986,
+            "heading": -0.48297175811002735,
+            "angularVelocity": -3.5611987502134074e-7,
+            "velocityX": 1.6998295347945822,
+            "velocityY": -3.379525824397886,
+            "timestamp": 1.1868293556656235
+        },
+        {
+            "x": 3.92391456197569,
+            "y": 5.240264436686968,
+            "heading": -0.48297176267932185,
+            "angularVelocity": -3.8666890356003695e-7,
+            "velocityX": 1.78726884214022,
+            "velocityY": -3.3341092057245874,
+            "timestamp": 1.1986464283491967
+        },
+        {
+            "x": 3.9460538007835475,
+            "y": 5.201428448582004,
+            "heading": -0.4829717676728866,
+            "angularVelocity": -4.225720568590589e-7,
+            "velocityX": 1.8734960341433124,
+            "velocityY": -3.286430501434571,
+            "timestamp": 1.2104635010327698
+        },
+        {
+            "x": 3.969196975663963,
+            "y": 5.163182231498957,
+            "heading": -0.4829717731698369,
+            "angularVelocity": -4.651702214495778e-7,
+            "velocityX": 1.9584524441985565,
+            "velocityY": -3.2365221156854496,
+            "timestamp": 1.222280573716343
+        },
+        {
+            "x": 3.993328387583419,
+            "y": 5.125551733327025,
+            "heading": -0.48297177927069973,
+            "angularVelocity": -5.162753076431712e-7,
+            "velocityX": 2.0420803498146496,
+            "velocityY": -3.184417933236803,
+            "timestamp": 1.234097646399916
         },
         {
             "x": 4.018431663513184,
             "y": 5.088562488555908,
             "heading": -0.4829717875114574,
-            "angularVelocity": -0.000001920532733427299,
-            "velocityX": 3.6086312001179417,
-            "velocityY": -1.135081104701801,
-            "timestamp": 0.9366683402900708
-        },
-        {
-            "x": 4.095489617333613,
-            "y": 5.060208566496142,
-            "heading": -0.48297174072333765,
-            "angularVelocity": 0.00000215563208981735,
-            "velocityX": 3.550230245224857,
-            "velocityY": -1.306327856847248,
-            "timestamp": 0.9583733984124503
-        },
-        {
-            "x": 4.1711135466589555,
-            "y": 5.028225843813575,
-            "heading": -0.482971694875483,
-            "angularVelocity": 0.0000021123119922022846,
-            "velocityX": 3.4841615672696387,
-            "velocityY": -1.4735147218783384,
-            "timestamp": 0.9800784565348297
-        },
-        {
-            "x": 4.245175973739862,
-            "y": 4.9927768637698655,
-            "heading": -0.48297164958118555,
-            "angularVelocity": 0.0000020868083942836762,
-            "velocityX": 3.4122197076519116,
-            "velocityY": -1.633212859593547,
-            "timestamp": 1.0017835146572092
-        },
-        {
-            "x": 4.317835043195099,
-            "y": 4.954533786792328,
-            "heading": -0.4829716044969262,
-            "angularVelocity": 0.0000020771314744606885,
-            "velocityX": 3.3475639201504173,
-            "velocityY": -1.7619430808199938,
-            "timestamp": 1.0234885727795886
-        },
-        {
-            "x": 4.39018173013391,
-            "y": 4.915702997439957,
-            "heading": -0.48297155943721365,
-            "angularVelocity": 0.000002076000546478681,
-            "velocityX": 3.333171767193961,
-            "velocityY": -1.789020288884605,
-            "timestamp": 1.045193630901968
-        },
-        {
-            "x": 4.462524157787397,
-            "y": 4.876864273483723,
-            "heading": -0.4829715143782692,
-            "angularVelocity": 0.000002075965162802355,
-            "velocityX": 3.3329755325063237,
-            "velocityY": -1.7893858536194671,
-            "timestamp": 1.0668986890243475
-        },
-        {
-            "x": 4.534839835738717,
-            "y": 4.8379757656949245,
-            "heading": -0.48297146931947993,
-            "angularVelocity": 0.0000020759580074306272,
-            "velocityX": 3.331743114605303,
-            "velocityY": -1.7916795048180625,
-            "timestamp": 1.088603747146727
-        },
-        {
-            "x": 4.60713879668486,
-            "y": 4.7990561876016065,
-            "heading": -0.48297142426091105,
-            "angularVelocity": 0.000002075947856864141,
-            "velocityX": 3.3309729252282607,
-            "velocityY": -1.793110982419974,
-            "timestamp": 1.1103088052691064
-        },
-        {
-            "x": 4.679445820973998,
-            "y": 4.760151592099993,
-            "heading": -0.48297137920221006,
-            "angularVelocity": 0.0000020759539417382525,
-            "velocityX": 3.331344421260875,
-            "velocityY": -1.7924207013070022,
-            "timestamp": 1.1320138633914858
-        },
-        {
-            "x": 4.75175835900604,
-            "y": 4.72125724602898,
-            "heading": -0.4829713341434503,
-            "angularVelocity": 0.000002075956651977989,
-            "velocityX": 3.3315984515826855,
-            "velocityY": -1.791948487386363,
-            "timestamp": 1.1537189215138652
-        },
-        {
-            "x": 4.824072564767644,
-            "y": 4.68236600077068,
-            "heading": -0.48297128908467135,
-            "angularVelocity": 0.0000020759575350386475,
-            "velocityX": 3.331675287569862,
-            "velocityY": -1.7918056260909452,
-            "timestamp": 1.1754239796362447
-        },
-        {
-            "x": 4.8963869623773055,
-            "y": 4.643475112237639,
-            "heading": -0.48297124402589003,
-            "angularVelocity": 0.000002075957644932215,
-            "velocityX": 3.3316841264339563,
-            "velocityY": -1.79178919096958,
-            "timestamp": 1.1971290377586241
-        },
-        {
-            "x": 4.968701267365325,
-            "y": 4.604584051483172,
-            "heading": -0.4829711989671097,
-            "angularVelocity": 0.000002075957594698055,
-            "velocityX": 3.331679859150302,
-            "velocityY": -1.7917971255911598,
-            "timestamp": 1.2188340958810036
-        },
-        {
-            "x": 5.0410154847403765,
-            "y": 4.565692827821048,
-            "heading": -0.4829711539083305,
-            "angularVelocity": 0.0000020759575472868184,
-            "velocityX": 3.331675822627313,
-            "velocityY": -1.7918046311069282,
-            "timestamp": 1.240539154003383
-        },
-        {
-            "x": 5.113329654064444,
-            "y": 4.526801514813212,
-            "heading": -0.4829711088495519,
-            "angularVelocity": 0.000002075957520958662,
-            "velocityX": 3.331673608812237,
-            "velocityY": -1.7918087474613025,
-            "timestamp": 1.2622442121257624
-        },
-        {
-            "x": 5.185643801945598,
-            "y": 4.487910161934636,
-            "heading": -0.48297106379077354,
-            "angularVelocity": 0.0000020759575090327586,
-            "velocityX": 3.3316726208897984,
-            "velocityY": -1.7918105843944965,
-            "timestamp": 1.2839492702481419
-        },
-        {
-            "x": 5.257957938797454,
-            "y": 4.4490187885483135,
-            "heading": -0.48297101873199527,
-            "angularVelocity": 0.0000020759575026231074,
-            "velocityX": 3.331672112745739,
-            "velocityY": -1.7918115292316654,
-            "timestamp": 1.3056543283705213
-        },
-        {
-            "x": 5.330272065176824,
-            "y": 4.410127395689581,
-            "heading": -0.4829709736732172,
-            "angularVelocity": 0.000002075957497099591,
-            "velocityX": 3.3316716302550873,
-            "velocityY": -1.7918124263686461,
-            "timestamp": 1.3273593864929008
-        },
-        {
-            "x": 5.402586187963667,
-            "y": 4.371235996150952,
-            "heading": -0.4829709286144391,
-            "angularVelocity": 0.00000207595749402331,
-            "velocityX": 3.3316714647394585,
-            "velocityY": -1.7918127341261798,
-            "timestamp": 1.3490644446152802
-        },
-        {
-            "x": 5.474900315939471,
-            "y": 4.332344606260608,
-            "heading": -0.482970883555661,
-            "angularVelocity": 0.0000020759574957870433,
-            "velocityX": 3.331671703806323,
-            "velocityY": -1.7918122896084423,
-            "timestamp": 1.3707695027376596
-        },
-        {
-            "x": 5.547214451711641,
-            "y": 4.293453230866729,
-            "heading": -0.4829708384968828,
-            "angularVelocity": 0.0000020759574997718594,
-            "velocityX": 3.331672063002153,
-            "velocityY": -1.7918116217242377,
-            "timestamp": 1.392474560860039
-        },
-        {
-            "x": 5.619528591620017,
-            "y": 4.254561863163663,
-            "heading": -0.4829707934381046,
-            "angularVelocity": 0.0000020759575011693265,
-            "velocityX": 3.3316722535663335,
-            "velocityY": -1.7918112673914555,
-            "timestamp": 1.4141796189824185
-        },
-        {
-            "x": 5.6918427353771435,
-            "y": 4.215670502616919,
-            "heading": -0.4829707483793263,
-            "angularVelocity": 0.000002075957502624911,
-            "velocityX": 3.3316724308867816,
-            "velocityY": -1.7918109376838844,
-            "timestamp": 1.435884677104798
-        },
-        {
-            "x": 5.764156898177578,
-            "y": 4.176779177479102,
-            "heading": -0.4829707033205479,
-            "angularVelocity": 0.0000020759575111736717,
-            "velocityX": 3.3316733082540817,
-            "velocityY": -1.7918093063163627,
-            "timestamp": 1.4575897352271774
-        },
-        {
-            "x": 5.836471080177112,
-            "y": 4.137887888039928,
-            "heading": -0.48297065826176927,
-            "angularVelocity": 0.0000020759575207599675,
-            "velocityX": 3.331674192798964,
-            "velocityY": -1.7918076616010294,
-            "timestamp": 1.4792947933495568
-        },
-        {
-            "x": 5.908785251268908,
-            "y": 4.098996578318978,
-            "heading": -0.48297061320299073,
-            "angularVelocity": 0.0000020759575149169034,
-            "velocityX": 3.331673690255448,
-            "velocityY": -1.7918085960272527,
-            "timestamp": 1.5009998514719363
-        },
-        {
-            "x": 5.981099339043632,
-            "y": 4.0601051136793025,
-            "heading": -0.48297056814421313,
-            "angularVelocity": 0.000002075957470564626,
-            "velocityX": 3.3316698516536873,
-            "velocityY": -1.7918157334753977,
-            "timestamp": 1.5227049095943157
-        },
-        {
-            "x": 6.053413282760847,
-            "y": 4.021213381182831,
-            "heading": -0.48297052308543725,
-            "angularVelocity": 0.00000207595739406405,
-            "velocityX": 3.3316632146058507,
-            "velocityY": -1.7918280742300665,
-            "timestamp": 1.5444099677166951
-        },
-        {
-            "x": 6.125727369888532,
-            "y": 3.982321915340118,
-            "heading": -0.4829704780266597,
-            "angularVelocity": 0.0000020759574679524,
-            "velocityX": 3.3316698218431826,
-            "velocityY": -1.7918157889019397,
-            "timestamp": 1.5661150258390746
-        },
-        {
-            "x": 6.198042995964267,
-            "y": 3.9434333111252817,
-            "heading": -0.4829704329678644,
-            "angularVelocity": 0.000002075958288103883,
-            "velocityX": 3.3317407245813113,
-            "velocityY": -1.7916839473808401,
-            "timestamp": 1.587820083961454
-        },
-        {
-            "x": 6.270359121166566,
-            "y": 3.9045456350763237,
-            "heading": -0.4829703879090624,
-            "angularVelocity": 0.0000020759585995923127,
-            "velocityX": 3.331763720444871,
-            "velocityY": -1.791641184728093,
-            "timestamp": 1.6095251420838335
-        },
-        {
-            "x": 6.342677550055512,
-            "y": 3.8656622432711547,
-            "heading": -0.48297034285023976,
-            "angularVelocity": 0.0000020759595452668008,
-            "velocityX": 3.331869856379567,
-            "velocityY": -1.791443800149147,
-            "timestamp": 1.631230200206213
-        },
-        {
-            "x": 6.414977737835259,
-            "y": 3.8267449442462484,
-            "heading": -0.4829702977916256,
-            "angularVelocity": 0.0000020759499429522823,
-            "velocityX": 3.3310294481622025,
-            "velocityY": -1.7930059807042389,
-            "timestamp": 1.6529352583285923
-        },
-        {
-            "x": 6.487268603977522,
-            "y": 3.787810332568674,
-            "heading": -0.4829702527330872,
-            "angularVelocity": 0.00000207594645249869,
-            "velocityX": 3.330599979721225,
-            "velocityY": -1.793803612876013,
-            "timestamp": 1.6746403164509718
-        },
-        {
-            "x": 6.559543297837492,
-            "y": 3.7488457083598847,
-            "heading": -0.4829702076720725,
-            "angularVelocity": 0.000002076060537305802,
-            "velocityX": 3.3298548869330626,
-            "velocityY": -1.7951863565208734,
-            "timestamp": 1.6963453745733512
-        },
-        {
-            "x": 6.630628961599716,
-            "y": 3.7077518923991635,
-            "heading": -0.48297016252441444,
-            "angularVelocity": 0.000002080052392839484,
-            "velocityX": 3.2750736423474676,
-            "velocityY": -1.893282926450661,
-            "timestamp": 1.7180504326957307
-        },
-        {
-            "x": 6.699799614538253,
-            "y": 3.6635105764693066,
-            "heading": -0.4829701170038858,
-            "angularVelocity": 0.0000020972313632639956,
-            "velocityX": 3.186844861162469,
-            "velocityY": -2.0382952066016227,
-            "timestamp": 1.73975549081811
-        },
-        {
-            "x": 6.766806665010948,
-            "y": 3.616056089090901,
-            "heading": -0.48297007073382403,
-            "angularVelocity": 0.000002131764013783056,
-            "velocityX": 3.0871629135886995,
-            "velocityY": -2.1863331169555953,
-            "timestamp": 1.7614605489404895
-        },
-        {
-            "x": 6.831474779993965,
-            "y": 3.565460555976799,
-            "heading": -0.48297002329923955,
-            "angularVelocity": 0.0000021854161466442962,
-            "velocityX": 2.9794029860873867,
-            "velocityY": -2.331048036306895,
-            "timestamp": 1.783165607062869
+            "angularVelocity": -6.973603296302728e-7,
+            "velocityX": 2.124322715274553,
+            "velocityY": -3.1301529373289303,
+            "timestamp": 1.2459147190834892
+        },
+        {
+            "x": 4.068270320597518,
+            "y": 5.022262107474161,
+            "heading": -0.48297170333499495,
+            "angularVelocity": 0.000003839169547390073,
+            "velocityX": 2.2730707479901486,
+            "velocityY": -3.023866725833157,
+            "timestamp": 1.2678404145896236
+        },
+        {
+            "x": 4.121253956507243,
+            "y": 4.9584470041490425,
+            "heading": -0.48297162876403243,
+            "angularVelocity": 0.0000034010762602073378,
+            "velocityX": 2.4165087896481814,
+            "velocityY": -2.910516718033651,
+            "timestamp": 1.289766110095758
+        },
+        {
+            "x": 4.177258824192737,
+            "y": 4.89726621863139,
+            "heading": -0.4829715614847619,
+            "angularVelocity": 0.0000030685124925835,
+            "velocityX": 2.5543029031769633,
+            "velocityY": -2.7903692040481487,
+            "timestamp": 1.3116918056018925
+        },
+        {
+            "x": 4.236154120064876,
+            "y": 4.83886263701252,
+            "heading": -0.482971499836949,
+            "angularVelocity": 0.000002811669663014854,
+            "velocityX": 2.686131249777681,
+            "velocityY": -2.6637048572771733,
+            "timestamp": 1.333617501108027
+        },
+        {
+            "x": 4.2978022891900425,
+            "y": 4.7833726571672255,
+            "heading": -0.48297144258671054,
+            "angularVelocity": 0.0000026111025032599054,
+            "velocityX": 2.811685910164931,
+            "velocityY": -2.530819596111362,
+            "timestamp": 1.3555431966141613
+        },
+        {
+            "x": 4.362059346053497,
+            "y": 4.730925869094327,
+            "heading": -0.48297138878758666,
+            "angularVelocity": 0.0000024537020442790352,
+            "velocityX": 2.9306735946176077,
+            "velocityY": -2.392023918156931,
+            "timestamp": 1.3774688921202958
+        },
+        {
+            "x": 4.428775209886728,
+            "y": 4.681644750011071,
+            "heading": -0.4829713376913353,
+            "angularVelocity": 0.000002330427847533908,
+            "velocityX": 3.042816307221113,
+            "velocityY": -2.247642227334136,
+            "timestamp": 1.3993945876264302
+        },
+        {
+            "x": 4.497794001617253,
+            "y": 4.635644296035039,
+            "heading": -0.4829712886902182,
+            "angularVelocity": 0.000002234871730357886,
+            "velocityX": 3.147849595521449,
+            "velocityY": -2.098015726031637,
+            "timestamp": 1.4213202831325646
+        },
+        {
+            "x": 4.568948026555497,
+            "y": 4.593021059923411,
+            "heading": -0.4829712412701429,
+            "angularVelocity": 0.0000021627626491587156,
+            "velocityX": 3.2452345659144926,
+            "velocityY": -1.9439855898636562,
+            "timestamp": 1.443245978638699
+        },
+        {
+            "x": 4.64205867202195,
+            "y": 4.55384875924605,
+            "heading": -0.4829711949807044,
+            "angularVelocity": 0.000002111195903735364,
+            "velocityX": 3.3344732643018493,
+            "velocityY": -1.7865932994667033,
+            "timestamp": 1.4651716741448335
+        },
+        {
+            "x": 4.71691342103475,
+            "y": 4.5181219873551415,
+            "heading": -0.4829711494130406,
+            "angularVelocity": 0.000002078276777149451,
+            "velocityX": 3.414019363346236,
+            "velocityY": -1.6294475986389252,
+            "timestamp": 1.487097369650968
+        },
+        {
+            "x": 4.7930985438005,
+            "y": 4.485327738304056,
+            "heading": -0.4829711041848689,
+            "angularVelocity": 0.000002062793021590678,
+            "velocityX": 3.4746958309446994,
+            "velocityY": -1.4956993743693363,
+            "timestamp": 1.5090230651571024
+        },
+        {
+            "x": 4.8695687201769084,
+            "y": 4.453203793512952,
+            "heading": -0.4829710590084327,
+            "angularVelocity": 0.0000020604334399265313,
+            "velocityX": 3.48769672346274,
+            "velocityY": -1.4651277439372516,
+            "timestamp": 1.5309487606632368
+        },
+        {
+            "x": 4.94605170532014,
+            "y": 4.421110356684282,
+            "heading": -0.48297101383350444,
+            "angularVelocity": 0.0000020603646626736416,
+            "velocityX": 3.4882809132254033,
+            "velocityY": -1.463736318862417,
+            "timestamp": 1.5528744561693713
+        },
+        {
+            "x": 5.022532332392591,
+            "y": 4.389011300831823,
+            "heading": -0.48297096865831407,
+            "angularVelocity": 0.000002060376618948169,
+            "velocityX": 3.4881733649487248,
+            "velocityY": -1.463992594598631,
+            "timestamp": 1.5748001516755057
+        },
+        {
+            "x": 5.099012556138694,
+            "y": 4.356911284014184,
+            "heading": -0.48297092348307896,
+            "angularVelocity": 0.000002060378658787562,
+            "velocityX": 3.488154969803465,
+            "velocityY": -1.4640364228636573,
+            "timestamp": 1.5967258471816401
+        },
+        {
+            "x": 5.175492855161546,
+            "y": 4.324811446548553,
+            "heading": -0.48297087830785207,
+            "angularVelocity": 0.0000020603782854914656,
+            "velocityX": 3.48815840306965,
+            "velocityY": -1.46402824287194,
+            "timestamp": 1.6186515426877746
+        },
+        {
+            "x": 5.2519731561146505,
+            "y": 4.292711613681912,
+            "heading": -0.48297083313262523,
+            "angularVelocity": 0.0000020603782806948455,
+            "velocityX": 3.488158491105669,
+            "velocityY": -1.464028033118535,
+            "timestamp": 1.640577238193909
+        },
+        {
+            "x": 5.328453454863713,
+            "y": 4.260611775564003,
+            "heading": -0.48297078795739806,
+            "angularVelocity": 0.0000020603782977173096,
+            "velocityX": 3.488158390582454,
+            "velocityY": -1.4640282726214169,
+            "timestamp": 1.6625029337000434
+        },
+        {
+            "x": 5.404933754197733,
+            "y": 4.228511938839819,
+            "heading": -0.4829707427821709,
+            "angularVelocity": 0.000002060378298482567,
+            "velocityX": 3.4881584172616003,
+            "velocityY": -1.4640282090556718,
+            "timestamp": 1.6844286292061779
+        },
+        {
+            "x": 5.481414053769572,
+            "y": 4.196412102682258,
+            "heading": -0.48297069760694356,
+            "angularVelocity": 0.000002060378303797646,
+            "velocityX": 3.488158428108085,
+            "velocityY": -1.4640281832128117,
+            "timestamp": 1.7063543247123123
+        },
+        {
+            "x": 5.5578943539285115,
+            "y": 4.164312267923512,
+            "heading": -0.4829706524317162,
+            "angularVelocity": 0.000002060378305885037,
+            "velocityX": 3.488158454884962,
+            "velocityY": -1.4640281194148257,
+            "timestamp": 1.7282800202184467
+        },
+        {
+            "x": 5.634374653438418,
+            "y": 4.132212431618396,
+            "heading": -0.4829706072564887,
+            "angularVelocity": 0.0000020603783152723795,
+            "velocityX": 3.4881584252834856,
+            "velocityY": -1.4640281899425134,
+            "timestamp": 1.7502057157245812
+        },
+        {
+            "x": 5.71085495402441,
+            "y": 4.100112597877132,
+            "heading": -0.4829705620812611,
+            "angularVelocity": 0.0000020603783147781934,
+            "velocityX": 3.4881584743622165,
+            "velocityY": -1.4640280730089004,
+            "timestamp": 1.7721314112307156
+        },
+        {
+            "x": 5.7873352532620705,
+            "y": 4.068012760923364,
+            "heading": -0.4829705169060333,
+            "angularVelocity": 0.0000020603783267765002,
+            "velocityX": 3.488158412866673,
+            "velocityY": -1.4640282195266534,
+            "timestamp": 1.79405710673685
+        },
+        {
+            "x": 5.863815552376704,
+            "y": 4.035912923676468,
+            "heading": -0.4829704717308054,
+            "angularVelocity": 0.000002060378332596705,
+            "velocityX": 3.4881584072556215,
+            "velocityY": -1.4640282328957663,
+            "timestamp": 1.8159828022429845
+        },
+        {
+            "x": 5.940295849970619,
+            "y": 4.00381308280633,
+            "heading": -0.48297042655557715,
+            "angularVelocity": 0.0000020603783450065593,
+            "velocityX": 3.4881583378978336,
+            "velocityY": -1.4640283981467839,
+            "timestamp": 1.837908497749119
+        },
+        {
+            "x": 6.016776146567219,
+            "y": 3.971713239559965,
+            "heading": -0.4829703813803487,
+            "angularVelocity": 0.0000020603783564721777,
+            "velocityX": 3.4881582924116907,
+            "velocityY": -1.4640285065230472,
+            "timestamp": 1.8598341932552533
+        },
+        {
+            "x": 6.093256433235532,
+            "y": 3.939613372658632,
+            "heading": -0.48297033620511903,
+            "angularVelocity": 0.000002060378411227836,
+            "velocityX": 3.4881578395965716,
+            "velocityY": -1.464029585392675,
+            "timestamp": 1.8817598887613878
+        },
+        {
+            "x": 6.169736625426417,
+            "y": 3.9075132806585757,
+            "heading": -0.4829702910298788,
+            "angularVelocity": 0.0000020603788925938424,
+            "velocityX": 3.488153530614592,
+            "velocityY": -1.4640398518273146,
+            "timestamp": 1.9036855842675222
+        },
+        {
+            "x": 6.2462171159811755,
+            "y": 3.8754138995336747,
+            "heading": -0.4829702458546715,
+            "angularVelocity": 0.0000020603773887814844,
+            "velocityX": 3.4881671385698874,
+            "velocityY": -1.4640074298145878,
+            "timestamp": 1.9256112797736566
+        },
+        {
+            "x": 6.322701012459848,
+            "y": 3.843322634623867,
+            "heading": -0.4829702006798376,
+            "angularVelocity": 0.0000020603603619090433,
+            "velocityX": 3.4883224779483077,
+            "velocityY": -1.4636372607092853,
+            "timestamp": 1.947536975279791
+        },
+        {
+            "x": 6.399174860669393,
+            "y": 3.811207431927173,
+            "heading": -0.4829701555038708,
+            "angularVelocity": 0.000002060412030184656,
+            "velocityX": 3.4878641905846774,
+            "velocityY": -1.4647290293616972,
+            "timestamp": 1.9694626707859255
+        },
+        {
+            "x": 6.47530722887592,
+            "y": 3.7782908978955247,
+            "heading": -0.4829701102655904,
+            "angularVelocity": 0.0000020632540676234596,
+            "velocityX": 3.4722897700184046,
+            "velocityY": -1.501276619595699,
+            "timestamp": 1.99138836629206
+        },
+        {
+            "x": 6.550061088980348,
+            "y": 3.742353506827061,
+            "heading": -0.48297006466550657,
+            "angularVelocity": 0.0000020797554102610617,
+            "velocityX": 3.409417962749362,
+            "velocityY": -1.6390536418100325,
+            "timestamp": 2.0133140617981944
+        },
+        {
+            "x": 6.623057371447113,
+            "y": 3.702968505083503,
+            "heading": -0.482970018320009,
+            "angularVelocity": 0.0000021137526750992832,
+            "velocityX": 3.3292573294334487,
+            "velocityY": -1.796294294637834,
+            "timestamp": 2.035239757304329
+        },
+        {
+            "x": 6.694087047655727,
+            "y": 3.6601383683558693,
+            "heading": -0.48296997081845616,
+            "angularVelocity": 0.0000021664787259977457,
+            "velocityX": 3.2395631960090396,
+            "velocityY": -1.9534220346951927,
+            "timestamp": 2.0571654528104633
+        },
+        {
+            "x": 6.762971951373863,
+            "y": 3.6139376604858007,
+            "heading": -0.48296992170702124,
+            "angularVelocity": 0.000002239903173580672,
+            "velocityX": 3.141743152404488,
+            "velocityY": -2.1071490232631853,
+            "timestamp": 2.0790911483165977
+        },
+        {
+            "x": 6.829544518243924,
+            "y": 3.564463136615443,
+            "heading": -0.4829698704667692,
+            "angularVelocity": 0.0000023369955126814126,
+            "velocityX": 3.0362807351500685,
+            "velocityY": -2.256463146473092,
+            "timestamp": 2.101016843822732
         },
         {
             "x": 6.893649101257324,
             "y": 3.5118300914764404,
-            "heading": -0.48296997476148124,
-            "angularVelocity": 0.00000223624180421772,
-            "velocityX": 2.8645083976649595,
-            "velocityY": -2.470874033046681,
-            "timestamp": 1.8048706651852484
-        },
-        {
-            "x": 6.934561334359113,
-            "y": 3.474160479757131,
-            "heading": -0.4829699264500687,
-            "angularVelocity": 0.0000032862628672599958,
-            "velocityX": 2.782952215912443,
-            "velocityY": -2.5623810156238895,
-            "timestamp": 1.8195716840772946
-        },
-        {
-            "x": 6.974231870495054,
-            "y": 3.435185370851295,
-            "heading": -0.4829698798888691,
-            "angularVelocity": 0.0000031672090196575476,
-            "velocityX": 2.6984888889168737,
-            "velocityY": -2.6511841928806597,
-            "timestamp": 1.8342727029693409
-        },
-        {
-            "x": 7.012622614846585,
-            "y": 3.394949051793438,
-            "heading": -0.482969834780297,
-            "angularVelocity": 0.0000030683976682796836,
-            "velocityX": 2.611434257274787,
-            "velocityY": -2.736974855506367,
-            "timestamp": 1.848973721861387
-        },
-        {
-            "x": 7.049699862488227,
-            "y": 3.3534992220248374,
-            "heading": -0.4829697908553284,
-            "angularVelocity": 0.0000029878860039287864,
-            "velocityX": 2.5220869324714577,
-            "velocityY": -2.8195208830745364,
-            "timestamp": 1.8636747407534333
-        },
-        {
-            "x": 7.085438739774718,
-            "y": 3.310890028476691,
-            "heading": -0.48296974786836877,
-            "angularVelocity": 0.0000029240802913465185,
-            "velocityX": 2.4310476402306533,
-            "velocityY": -2.8983837012274143,
-            "timestamp": 1.8783757596454795
-        },
-        {
-            "x": 7.119836677140904,
-            "y": 3.2671911422114968,
-            "heading": -0.4829697055890564,
-            "angularVelocity": 0.0000028759443619428334,
-            "velocityX": 2.339833559754191,
-            "velocityY": -2.9725073198044742,
-            "timestamp": 1.8930767785375258
-        },
-        {
-            "x": 7.1529563800127525,
-            "y": 3.222515689615774,
-            "heading": -0.48296966398118896,
-            "angularVelocity": 0.000002830270996857658,
-            "velocityX": 2.2528848588700137,
-            "velocityY": -3.0389357991976094,
-            "timestamp": 1.907777797429572
-        },
-        {
-            "x": 7.184894346745796,
-            "y": 3.177381185858669,
-            "heading": -0.4821430167851757,
-            "angularVelocity": 0.056230605652804695,
-            "velocityX": 2.17250021699707,
-            "velocityY": -3.0701616050246474,
-            "timestamp": 1.9224788163216182
-        },
-        {
-            "x": 7.2158791345376,
-            "y": 3.1333943550888805,
-            "heading": -0.47750834116458535,
-            "angularVelocity": 0.31526220424747137,
-            "velocityX": 2.107662606199986,
-            "velocityY": -2.992094023740475,
-            "timestamp": 1.9371798352136644
-        },
-        {
-            "x": 7.245895346179036,
-            "y": 3.090550131820991,
-            "heading": -0.46908945700490695,
-            "angularVelocity": 0.5726735147747635,
-            "velocityX": 2.041777638805421,
-            "velocityY": -2.9143710094182476,
-            "timestamp": 1.9518808541057107
-        },
-        {
-            "x": 7.274933319615603,
-            "y": 3.048846636162261,
-            "heading": -0.45692051225089847,
-            "angularVelocity": 0.827761996863506,
-            "velocityX": 1.9752354343465999,
-            "velocityY": -2.8367758700924766,
-            "timestamp": 1.9665818729977569
-        },
-        {
-            "x": 7.3029845213490745,
-            "y": 3.008282422680816,
-            "heading": -0.4410407652781448,
-            "angularVelocity": 1.08018002625281,
-            "velocityX": 1.90811276003785,
-            "velocityY": -2.7592790526506894,
-            "timestamp": 1.981282891889803
-        },
-        {
-            "x": 7.330040747407425,
-            "y": 2.9688555797343485,
-            "heading": -0.42149121219843927,
-            "angularVelocity": 1.3298093977882255,
-            "velocityX": 1.8404320310743014,
-            "velocityY": -2.6819122698903146,
-            "timestamp": 1.9959839107818493
-        },
-        {
-            "x": 7.356112777651364,
-            "y": 2.9307983654229495,
-            "heading": -0.3996724931243264,
-            "angularVelocity": 1.4841637327544344,
-            "velocityX": 1.7734845751435842,
-            "velocityY": -2.5887467114262686,
-            "timestamp": 2.0106849296738956
-        },
-        {
-            "x": 7.38118332940277,
-            "y": 2.8941879380599844,
-            "heading": -0.37646864699164706,
-            "angularVelocity": 1.578383532670194,
-            "velocityX": 1.7053615083080305,
-            "velocityY": -2.4903326518935414,
-            "timestamp": 2.025385948565942
-        },
-        {
-            "x": 7.40523963577506,
-            "y": 2.8590504496296347,
-            "heading": -0.3524751392807844,
-            "angularVelocity": 1.6320982842790503,
-            "velocityX": 1.6363700059799433,
-            "velocityY": -2.390139669119207,
-            "timestamp": 2.040086967457988
-        },
-        {
-            "x": 7.428276340738969,
-            "y": 2.825395406477394,
-            "heading": -0.3281233875567818,
-            "angularVelocity": 1.656466936259613,
-            "velocityX": 1.5670141731720877,
-            "velocityY": -2.289300040995877,
-            "timestamp": 2.0547879863500342
-        },
-        {
-            "x": 7.450292281624529,
-            "y": 2.793225377698084,
-            "heading": -0.3037369697425905,
-            "angularVelocity": 1.658825010243686,
-            "velocityX": 1.497579252650967,
-            "velocityY": -2.1882856566299744,
-            "timestamp": 2.0694890052420805
-        },
-        {
-            "x": 7.471288319158757,
-            "y": 2.7625397684205146,
-            "heading": -0.27956516086770095,
-            "angularVelocity": 1.6442267745106667,
-            "velocityX": 1.4282028809299567,
-            "velocityY": -2.0873117368873455,
-            "timestamp": 2.0841900241341267
-        },
-        {
-            "x": 7.491266209557209,
-            "y": 2.733336527235843,
-            "heading": -0.2558043926700512,
-            "angularVelocity": 1.6162667616531794,
-            "velocityX": 1.3589459713748013,
-            "velocityY": -1.9864773590946931,
-            "timestamp": 2.098891043026173
-        },
-        {
-            "x": 7.510228057377649,
-            "y": 2.7056129838220437,
-            "heading": -0.2326125564520332,
-            "angularVelocity": 1.577566588297197,
-            "velocityX": 1.28983221909181,
-            "velocityY": -1.885824623271541,
-            "timestamp": 2.113592061918219
-        },
-        {
-            "x": 7.528176060430696,
-            "y": 2.679366275566135,
-            "heading": -0.21011884554685348,
-            "angularVelocity": 1.5300783619392286,
-            "velocityX": 1.2208679673730278,
-            "velocityY": -1.7853666095287006,
-            "timestamp": 2.1282930808102654
-        },
-        {
-            "x": 7.545112396890263,
-            "y": 2.654593565371254,
-            "heading": -0.18843070859149175,
-            "angularVelocity": 1.4752812110931899,
-            "velocityX": 1.152051880480808,
-            "velocityY": -1.6851015821961506,
-            "timestamp": 2.1429940997023116
-        },
-        {
-            "x": 7.561039180049459,
-            "y": 2.631292148556631,
-            "heading": -0.1676388680105614,
-            "angularVelocity": 1.4143128944742427,
-            "velocityX": 1.0833795450607198,
-            "velocityY": -1.5850205340005987,
-            "timestamp": 2.157695118594358
-        },
-        {
-            "x": 7.575958444025175,
-            "y": 2.6094594995019675,
-            "heading": -0.14782100448684538,
-            "angularVelocity": 1.3480605439149753,
-            "velocityX": 1.0148455753489192,
-            "velocityY": -1.485111284801969,
-            "timestamp": 2.172396137486404
-        },
-        {
-            "x": 7.589872142500143,
-            "y": 2.5890932855015207,
-            "heading": -0.12904449811203259,
-            "angularVelocity": 1.2772248313327248,
-            "velocityX": 0.9464445000131985,
-            "velocityY": -1.3853607120705473,
-            "timestamp": 2.1870971563784503
-        },
-        {
-            "x": 7.602782151943804,
-            "y": 2.570191363211945,
-            "heading": -0.11136848807718236,
-            "angularVelocity": 1.2023663233582889,
-            "velocityX": 0.8781710668125251,
-            "velocityY": -1.2857559349035097,
-            "timestamp": 2.2017981752704965
-        },
-        {
-            "x": 7.614690275426409,
-            "y": 2.552751766479238,
-            "heading": -0.09484543137092857,
-            "angularVelocity": 1.1239395600799826,
-            "velocityX": 0.8100202829510676,
-            "velocityY": -1.186284900439311,
-            "timestamp": 2.2164991941625427
-        },
-        {
-            "x": 7.62559824546286,
-            "y": 2.536772690588083,
-            "heading": -0.07952228805027751,
-            "angularVelocity": 1.042318456507909,
-            "velocityX": 0.7419873490775946,
-            "velocityY": -1.086936627215761,
-            "timestamp": 2.231200213054589
-        },
-        {
-            "x": 7.635507725447758,
-            "y": 2.522252475791086,
-            "heading": -0.06544142530276546,
-            "angularVelocity": 0.9578154310876013,
-            "velocityX": 0.6740675634570735,
-            "velocityY": -0.9877012541528096,
-            "timestamp": 2.245901231946635
-        },
-        {
-            "x": 7.644420309753795,
-            "y": 2.509189591670685,
-            "heading": -0.05264130826624494,
-            "angularVelocity": 0.8706959109783889,
-            "velocityX": 0.6062562310465265,
-            "velocityY": -0.8885699839124228,
-            "timestamp": 2.2606022508386814
-        },
-        {
-            "x": 7.652337522761704,
-            "y": 2.497582623097395,
-            "heading": -0.04115702850586921,
-            "angularVelocity": 0.7811893750159932,
-            "velocityX": 0.538548590818527,
-            "velocityY": -0.7895349743118973,
-            "timestamp": 2.2753032697307276
-        },
-        {
-            "x": 7.6592608171290975,
-            "y": 2.487430258069545,
-            "heading": -0.031020708755951136,
-            "angularVelocity": 0.6894977704846257,
-            "velocityX": 0.4709397639872257,
-            "velocityY": -0.6905892103399419,
-            "timestamp": 2.290004288622774
-        },
-        {
-            "x": 7.665191571570965,
-            "y": 2.4787312774326344,
-            "heading": -0.022261813501037925,
-            "angularVelocity": 0.5958019181685587,
-            "velocityX": 0.40342472079099084,
-            "velocityY": -0.5917263763001924,
-            "timestamp": 2.30470530751482
-        },
-        {
-            "x": 7.670131088361328,
-            "y": 2.4714845463094046,
-            "heading": -0.014907388223763604,
-            "angularVelocity": 0.5002663646159557,
-            "velocityX": 0.33599826152420176,
-            "velocityY": -0.49294073944433947,
-            "timestamp": 2.3194063264068663
-        },
-        {
-            "x": 7.674080590697013,
-            "y": 2.465689006980458,
-            "heading": -0.008982245050778671,
-            "angularVelocity": 0.40304302827545885,
-            "velocityX": 0.26865500715889973,
-            "velocityY": -0.3942270513020354,
-            "timestamp": 2.3341073452989125
-        },
-        {
-            "x": 7.677041220003002,
-            "y": 2.461343672911727,
-            "heading": -0.0045091086563273375,
-            "angularVelocity": 0.304273902870198,
-            "velocityX": 0.20138939536970257,
-            "velocityY": -0.29558046966947404,
-            "timestamp": 2.3488083641909587
-        },
-        {
-            "x": 7.679014033209882,
-            "y": 2.458447623611078,
-            "heading": -0.00150873334272897,
-            "angularVelocity": 0.20409301801671295,
-            "velocityX": 0.1341956786374654,
-            "velocityY": -0.1969965022095263,
-            "timestamp": 2.363509383083005
+            "heading": -0.48296981752333895,
+            "angularVelocity": 0.0000024146750677216304,
+            "velocityX": 2.923719477698919,
+            "velocityY": -2.400518839837981,
+            "timestamp": 2.1229425393288666
+        },
+        {
+            "x": 6.935492251520178,
+            "y": 3.475140361342702,
+            "heading": -0.48296976475625003,
+            "angularVelocity": 0.000003586928740757824,
+            "velocityX": 2.8443562341956334,
+            "velocityY": -2.4940441143007757,
+            "timestamp": 2.1376534780501846
+        },
+        {
+            "x": 6.976123919914837,
+            "y": 3.4371133556518583,
+            "heading": -0.4829697143572031,
+            "angularVelocity": 0.0000034259572361559534,
+            "velocityX": 2.7620037826529793,
+            "velocityY": -2.5849475965623756,
+            "timestamp": 2.1523644167715026
+        },
+        {
+            "x": 7.015501922342544,
+            "y": 3.3977895885157845,
+            "heading": -0.4829696659490819,
+            "angularVelocity": 0.0000032906208164279545,
+            "velocityX": 2.676783798347444,
+            "velocityY": -2.673097066136968,
+            "timestamp": 2.1670753554928206
+        },
+        {
+            "x": 7.053586816421239,
+            "y": 3.357212172853621,
+            "heading": -0.4829696191989941,
+            "angularVelocity": 0.000003177913298539426,
+            "velocityX": 2.5888826539333314,
+            "velocityY": -2.758315864871489,
+            "timestamp": 2.1817862942141386
+        },
+        {
+            "x": 7.090341266441407,
+            "y": 3.315425855846777,
+            "heading": -0.48296957381059136,
+            "angularVelocity": 0.0000030853505387280562,
+            "velocityX": 2.4984435539051053,
+            "velocityY": -2.8404929011286835,
+            "timestamp": 2.1964972329354566
+        },
+        {
+            "x": 7.125730460448936,
+            "y": 3.2724771552259067,
+            "heading": -0.48296952951403316,
+            "angularVelocity": 0.000003011130632666104,
+            "velocityX": 2.405638054643597,
+            "velocityY": -2.9195078189422343,
+            "timestamp": 2.2112081716567746
+        },
+        {
+            "x": 7.1597225937186835,
+            "y": 3.228414468562161,
+            "heading": -0.48296947336189594,
+            "angularVelocity": 0.000003817032908672554,
+            "velocityX": 2.3106705774318574,
+            "velocityY": -2.9952328330951756,
+            "timestamp": 2.2259191103780926
+        },
+        {
+            "x": 7.192119389825632,
+            "y": 3.1836076694729414,
+            "heading": -0.4820443638475483,
+            "angularVelocity": 0.06288582475110666,
+            "velocityX": 2.2022249375566023,
+            "velocityY": -3.0458150861769275,
+            "timestamp": 2.2406300490994107
+        },
+        {
+            "x": 7.223229630823693,
+            "y": 3.1396490395754038,
+            "heading": -0.47743685492285376,
+            "angularVelocity": 0.3132029173651504,
+            "velocityX": 2.11476926030427,
+            "velocityY": -2.988159405071387,
+            "timestamp": 2.2553409878207287
+        },
+        {
+            "x": 7.253205323915578,
+            "y": 3.0967066455951215,
+            "heading": -0.4690721734359265,
+            "angularVelocity": 0.5686028366637977,
+            "velocityX": 2.0376465200313345,
+            "velocityY": -2.9190791147849615,
+            "timestamp": 2.2700519265420467
+        },
+        {
+            "x": 7.28210093319025,
+            "y": 3.0548329265465135,
+            "heading": -0.45697001578434165,
+            "angularVelocity": 0.8226638612835941,
+            "velocityX": 1.9642260648397363,
+            "velocityY": -2.8464341971547444,
+            "timestamp": 2.2847628652633647
+        },
+        {
+            "x": 7.309941019631162,
+            "y": 3.0140523923628573,
+            "heading": -0.44116512849373885,
+            "angularVelocity": 1.0743629342769347,
+            "velocityX": 1.892475182468747,
+            "velocityY": -2.7721231769227326,
+            "timestamp": 2.2994738039846827
+        },
+        {
+            "x": 7.336737361434823,
+            "y": 2.9743781069216433,
+            "heading": -0.42169586611687776,
+            "angularVelocity": 1.3234547941286585,
+            "velocityX": 1.8215249421732278,
+            "velocityY": -2.696924118358335,
+            "timestamp": 2.3141847427060007
+        },
+        {
+            "x": 7.362502163456035,
+            "y": 2.936038971640856,
+            "heading": -0.39992762164634665,
+            "angularVelocity": 1.479731843283817,
+            "velocityX": 1.7514043467447793,
+            "velocityY": -2.606165113394762,
+            "timestamp": 2.3288956814273187
+        },
+        {
+            "x": 7.3872337303102205,
+            "y": 2.8991268417513614,
+            "heading": -0.37676830696947144,
+            "angularVelocity": 1.5742921043722566,
+            "velocityX": 1.6811685047907687,
+            "velocityY": -2.5091620996289765,
+            "timestamp": 2.3436066201486367
+        },
+        {
+            "x": 7.410930819346905,
+            "y": 2.863676250815302,
+            "heading": -0.3528129850042874,
+            "angularVelocity": 1.6284019952084616,
+            "velocityX": 1.6108481916483202,
+            "velocityY": -2.4098116107768903,
+            "timestamp": 2.3583175588699548
+        },
+        {
+            "x": 7.433596368468817,
+            "y": 2.8297026440026514,
+            "heading": -0.328491545307115,
+            "angularVelocity": 1.6532894438563033,
+            "velocityX": 1.5407275872250867,
+            "velocityY": -2.3094112113606737,
+            "timestamp": 2.3730284975912728
+        },
+        {
+            "x": 7.455235422231159,
+            "y": 2.7972130374670496,
+            "heading": -0.30412705199668255,
+            "angularVelocity": 1.6562160832826247,
+            "velocityX": 1.4709498946511312,
+            "velocityY": -2.208533877482683,
+            "timestamp": 2.387739436312591
+        },
+        {
+            "x": 7.475853646381716,
+            "y": 2.766210258525358,
+            "heading": -0.2799689621545816,
+            "angularVelocity": 1.6421854716240782,
+            "velocityX": 1.4015573405031783,
+            "velocityY": -2.1074643521399103,
+            "timestamp": 2.402450375033909
+        },
+        {
+            "x": 7.495456618827617,
+            "y": 2.736694950498885,
+            "heading": -0.2562142168141467,
+            "angularVelocity": 1.6147674727249637,
+            "velocityX": 1.332543953669861,
+            "velocityY": -2.006351095984268,
+            "timestamp": 2.417161313755227
+        },
+        {
+            "x": 7.514049552575163,
+            "y": 2.7086666100983954,
+            "heading": -0.23302134486774898,
+            "angularVelocity": 1.576573214378762,
+            "velocityX": 1.2638849294234806,
+            "velocityY": -1.9052720517334563,
+            "timestamp": 2.431872252476545
+        },
+        {
+            "x": 7.5316372223467996,
+            "y": 2.682124150428342,
+            "heading": -0.2105202201245662,
+            "angularVelocity": 1.5295505724985052,
+            "velocityX": 1.1955504747055303,
+            "velocityY": -1.804266890976241,
+            "timestamp": 2.446583191197863
+        },
+        {
+            "x": 7.5482239779420395,
+            "y": 2.6570662133282936,
+            "heading": -0.18881898554249216,
+            "angularVelocity": 1.4751767370647726,
+            "velocityX": 1.127511704688336,
+            "velocityY": -1.7033540533846026,
+            "timestamp": 2.461294129919181
+        },
+        {
+            "x": 7.563813788966016,
+            "y": 2.6334913430066904,
+            "heading": -0.16800906628713844,
+            "angularVelocity": 1.414588127214289,
+            "velocityX": 1.0597427750401904,
+            "velocityY": -1.602540175593351,
+            "timestamp": 2.476005068640499
+        },
+        {
+            "x": 7.5784102960217234,
+            "y": 2.611398080774854,
+            "heading": -0.1481688563613113,
+            "angularVelocity": 1.3486705574455213,
+            "velocityX": 0.9922213213052737,
+            "velocityY": -1.5018254545389353,
+            "timestamp": 2.490716007361817
+        },
+        {
+            "x": 7.5920168580101715,
+            "y": 2.5907850143215074,
+            "heading": -0.1293664624719475,
+            "angularVelocity": 1.2781233234366292,
+            "velocityX": 0.9249281943327172,
+            "velocityY": -1.401206737641836,
+            "timestamp": 2.505426946083135
+        },
+        {
+            "x": 7.604636591974509,
+            "y": 2.571650800836196,
+            "heading": -0.1116617644111604,
+            "angularVelocity": 1.2035056631111207,
+            "velocityX": 0.8578469534407521,
+            "velocityY": -1.3006793004707733,
+            "timestamp": 2.520137884804453
+        },
+        {
+            "x": 7.616272404986451,
+            "y": 2.5539941753706046,
+            "heading": -0.09510797189421191,
+            "angularVelocity": 1.1252709857977845,
+            "velocityX": 0.790963325479677,
+            "velocityY": -1.20023785021998,
+            "timestamp": 2.534848823525771
+        },
+        {
+            "x": 7.626927018849464,
+            "y": 2.537813951229366,
+            "heading": -0.07975280576069126,
+            "angularVelocity": 1.0437924067530058,
+            "velocityX": 0.7242647165386202,
+            "velocityY": -1.0998770675179592,
+            "timestamp": 2.549559762247089
+        },
+        {
+            "x": 7.636602988821212,
+            "y": 2.523109016437205,
+            "heading": -0.0656393963876239,
+            "angularVelocity": 0.9593819701400241,
+            "velocityX": 0.6577398053956987,
+            "velocityY": -0.9995918731448548,
+            "timestamp": 2.564270700968407
+        },
+        {
+            "x": 7.64530271759392,
+            "y": 2.509878328655525,
+            "heading": -0.05280696792868869,
+            "angularVelocity": 0.8723052078477697,
+            "velocityX": 0.5913782211668082,
+            "velocityY": -0.899377533434203,
+            "timestamp": 2.578981639689725
+        },
+        {
+            "x": 7.653028465638572,
+            "y": 2.498120909886367,
+            "heading": -0.04129135982777762,
+            "angularVelocity": 0.7827922010322448,
+            "velocityX": 0.5251702961317726,
+            "velocityY": -0.799229674726393,
+            "timestamp": 2.593692578411043
+        },
+        {
+            "x": 7.6597823588265515,
+            "y": 2.48783584165749,
+            "heading": -0.03112542462311605,
+            "angularVelocity": 0.6910459894669937,
+            "velocityX": 0.4591068806636599,
+            "velocityY": -0.6991442506638482,
+            "timestamp": 2.608403517132361
+        },
+        {
+            "x": 7.665566394043072,
+            "y": 2.4790222609826817,
+            "heading": -0.02233933188139214,
+            "angularVelocity": 0.5972489525084974,
+            "velocityX": 0.39317920671771145,
+            "velocityY": -0.5991174894934748,
+            "timestamp": 2.623114455853679
+        },
+        {
+            "x": 7.670382443324971,
+            "y": 2.471679357147111,
+            "heading": -0.014960801225629315,
+            "angularVelocity": 0.5015676290643761,
+            "velocityX": 0.3273787875222405,
+            "velocityY": -0.4991458379831521,
+            "timestamp": 2.637825394574997
+        },
+        {
+            "x": 7.674232256901065,
+            "y": 2.4658063692226495,
+            "heading": -0.00901528221740954,
+            "angularVelocity": 0.40415633025538694,
+            "velocityX": 0.2616973429788216,
+            "velocityY": -0.399225911800697,
+            "timestamp": 2.652536333296315
+        },
+        {
+            "x": 7.677117465389097,
+            "y": 2.461402584136771,
+            "heading": -0.004526094899743595,
+            "angularVelocity": 0.30515981357196037,
+            "velocityX": 0.19612674233020058,
+            "velocityY": -0.2993544578835629,
+            "timestamp": 2.667247272017633
+        },
+        {
+            "x": 7.679039581308467,
+            "y": 2.45846733507745,
+            "heading": -0.001514541807715108,
+            "angularVelocity": 0.20471522239871504,
+            "velocityX": 0.13065895764927307,
+            "velocityY": -0.1995283315991124,
+            "timestamp": 2.681958210738951
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": -1.1328442579469643e-34,
-            "angularVelocity": 0.10262780789604178,
-            "velocityX": 0.06706792211866815,
-            "velocityY": -0.0984709714141921,
-            "timestamp": 2.378210401975051
+            "heading": 7.235538726171091e-30,
+            "angularVelocity": 0.10295344412796348,
+            "velocityX": 0.06528602353168424,
+            "velocityY": -0.09974448981448966,
+            "timestamp": 2.696669149460269
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": -2.155134012002005e-33,
-            "angularVelocity": -1.3889170145055665e-31,
-            "velocityX": 1.4892305241424507e-32,
-            "velocityY": 6.576507695252416e-33,
-            "timestamp": 2.3929114208670974
+            "heading": 1.9723672087239994e-30,
+            "angularVelocity": -3.5777263674767652e-28,
+            "velocityX": 6.106461964279306e-29,
+            "velocityY": 3.383375524617378e-29,
+            "timestamp": 2.711380088181587
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.1.traj
new file mode 100644
index 00000000..39d8ef61
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.1.traj	
@@ -0,0 +1,625 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -4.1566893564125267e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 1.3546830312136449,
+            "y": 5.550921973415328,
+            "heading": -1.4050458772244498e-18,
+            "angularVelocity": -4.014308801297789e-17,
+            "velocityX": 0.13379730014070312,
+            "velocityY": -0.25936522568857817,
+            "timestamp": 0.03500093954603885
+        },
+        {
+            "x": 1.3640490935091651,
+            "y": 5.532765920501419,
+            "heading": -3.603876179363921e-18,
+            "angularVelocity": -6.28220327965604e-17,
+            "velocityX": 0.26759459651667833,
+            "velocityY": -0.5187304440792354,
+            "timestamp": 0.0700018790920777
+        },
+        {
+            "x": 1.3780981867284385,
+            "y": 5.505531841564791,
+            "heading": -4.091508575199503e-18,
+            "angularVelocity": -1.3931950355515604e-17,
+            "velocityX": 0.4013918883749871,
+            "velocityY": -0.7780956537124009,
+            "timestamp": 0.10500281863811654
+        },
+        {
+            "x": 1.3968303106782045,
+            "y": 5.469219736980081,
+            "heading": -4.985304542720085e-18,
+            "angularVelocity": -2.5536326104216816e-17,
+            "velocityX": 0.5351891747117141,
+            "velocityY": -1.0374608526419855,
+            "timestamp": 0.1400037581841554
+        },
+        {
+            "x": 1.4202454651168877,
+            "y": 5.423829607215581,
+            "heading": -4.3424315250592746e-18,
+            "angularVelocity": 1.8367331153854942e-17,
+            "velocityX": 0.6689864541464801,
+            "velocityY": -1.2968260381921248,
+            "timestamp": 0.17500469773019423
+        },
+        {
+            "x": 1.4483436497338926,
+            "y": 5.369361452873384,
+            "heading": -2.0560193041515625e-18,
+            "angularVelocity": 6.532433512064817e-17,
+            "velocityX": 0.8027837247073224,
+            "velocityY": -1.5561912065401715,
+            "timestamp": 0.21000563727623306
+        },
+        {
+            "x": 1.4811248641150936,
+            "y": 5.305815274756268,
+            "heading": 2.1208651292681738e-19,
+            "angularVelocity": 6.480130833433249e-17,
+            "velocityX": 0.9365809834363137,
+            "velocityY": -1.815556351952184,
+            "timestamp": 0.2450065768222719
+        },
+        {
+            "x": 1.518589107680717,
+            "y": 5.233191073988126,
+            "heading": 3.0112738874394488e-18,
+            "angularVelocity": 7.99746695569e-17,
+            "velocityX": 1.0703782256008032,
+            "velocityY": -2.074921465253921,
+            "timestamp": 0.2800075163683107
+        },
+        {
+            "x": 1.5607363795611104,
+            "y": 5.151488852254779,
+            "heading": 5.4707987602321704e-18,
+            "angularVelocity": 7.027028770373186e-17,
+            "velocityX": 1.2041754429187383,
+            "velocityY": -2.334286530390624,
+            "timestamp": 0.31500845591434956
+        },
+        {
+            "x": 1.6075666783068712,
+            "y": 5.060708612365892,
+            "heading": 7.441304754324509e-18,
+            "angularVelocity": 5.629869559433992e-17,
+            "velocityX": 1.3379726188262824,
+            "velocityY": -2.5936515152532893,
+            "timestamp": 0.3500093954603884
+        },
+        {
+            "x": 1.6590800010192646,
+            "y": 4.9608503599406655,
+            "heading": 8.015191331788108e-18,
+            "angularVelocity": 1.639633984973558e-17,
+            "velocityX": 1.4717697119150586,
+            "velocityY": -2.853016339571725,
+            "timestamp": 0.38501033500642723
+        },
+        {
+            "x": 1.7152763390026586,
+            "y": 4.851914111835609,
+            "heading": 8.871725964928404e-18,
+            "angularVelocity": 2.4471795525975083e-17,
+            "velocityX": 1.6055665565638684,
+            "velocityY": -3.112380682288518,
+            "timestamp": 0.42001127455246606
+        },
+        {
+            "x": 1.775979033805623,
+            "y": 4.734242319347404,
+            "heading": 1.526334137039101e-17,
+            "angularVelocity": 1.8261268092684144e-16,
+            "velocityX": 1.7343161523752855,
+            "velocityY": -3.361960964887902,
+            "timestamp": 0.4550122140985049
+        },
+        {
+            "x": 1.8366817286107127,
+            "y": 4.616570526856247,
+            "heading": 2.6738432547349554e-17,
+            "angularVelocity": 3.2785094702569765e-16,
+            "velocityX": 1.734316152436003,
+            "velocityY": -3.3619609649722197,
+            "timestamp": 0.49001315364454373
+        },
+        {
+            "x": 1.8973844234157844,
+            "y": 4.498898734365081,
+            "heading": 1.9852147320466475e-17,
+            "angularVelocity": -1.967457250062152e-16,
+            "velocityX": 1.7343161524354893,
+            "velocityY": -3.3619609649724835,
+            "timestamp": 0.5250140931905826
+        },
+        {
+            "x": 1.9580871182208561,
+            "y": 4.381226941873916,
+            "heading": 2.777024505719557e-17,
+            "angularVelocity": 2.2622529107579697e-16,
+            "velocityX": 1.7343161524354895,
+            "velocityY": -3.361960964972483,
+            "timestamp": 0.5600150327366215
+        },
+        {
+            "x": 2.018789813025928,
+            "y": 4.26355514938275,
+            "heading": 1.7414652311267274e-17,
+            "angularVelocity": -2.9586613617347484e-16,
+            "velocityX": 1.7343161524354893,
+            "velocityY": -3.361960964972483,
+            "timestamp": 0.5950159722826603
+        },
+        {
+            "x": 2.0794925078309996,
+            "y": 4.145883356891583,
+            "heading": 6.771469247853252e-18,
+            "angularVelocity": -3.040827818183264e-16,
+            "velocityX": 1.7343161524354895,
+            "velocityY": -3.361960964972483,
+            "timestamp": 0.6300169118286992
+        },
+        {
+            "x": 2.140195202636071,
+            "y": 4.028211564400419,
+            "heading": 1.7156471914154247e-17,
+            "angularVelocity": 2.9670636674740876e-16,
+            "velocityX": 1.7343161524354882,
+            "velocityY": -3.361960964972482,
+            "timestamp": 0.6650178513747381
+        },
+        {
+            "x": 2.2008978974411426,
+            "y": 3.910539771909253,
+            "heading": 9.427453062324153e-18,
+            "angularVelocity": -2.208231822366884e-16,
+            "velocityX": 1.734316152435471,
+            "velocityY": -3.361960964972468,
+            "timestamp": 0.700018790920777
+        },
+        {
+            "x": 2.261600592246207,
+            "y": 3.7928679794180944,
+            "heading": -2.0403441270686145e-18,
+            "angularVelocity": -3.2764255297515643e-16,
+            "velocityX": 1.7343161524352513,
+            "velocityY": -3.3619609649722837,
+            "timestamp": 0.7350197304668159
+        },
+        {
+            "x": 2.322303287051213,
+            "y": 3.6751961869269927,
+            "heading": 3.357966916114392e-18,
+            "angularVelocity": 1.5423331811082687e-16,
+            "velocityX": 1.7343161524334347,
+            "velocityY": -3.3619609649707387,
+            "timestamp": 0.7700206700128548
+        },
+        {
+            "x": 2.383005981879539,
+            "y": 3.55752439444761,
+            "heading": 1.6083387472754582e-17,
+            "angularVelocity": 3.6357368464068287e-16,
+            "velocityX": 1.7343161531001825,
+            "velocityY": -3.361960964635663,
+            "timestamp": 0.8050216095588937
+        },
+        {
+            "x": 2.4437089810285184,
+            "y": 3.439852758955193,
+            "heading": 2.2946423291224054e-17,
+            "angularVelocity": 1.9608147402560587e-16,
+            "velocityX": 1.7343248477441577,
+            "velocityY": -3.3619564794134043,
+            "timestamp": 0.8400225491049326
+        },
+        {
+            "x": 2.507799181067132,
+            "y": 3.323991142238629,
+            "heading": 9.669545465348722e-18,
+            "angularVelocity": -3.793291836756151e-16,
+            "velocityX": 1.8310994181462568,
+            "velocityY": -3.3102430454713967,
+            "timestamp": 0.8750234886509715
+        },
+        {
+            "x": 2.5806292795037855,
+            "y": 3.213414270581727,
+            "heading": 3.38196516760506e-17,
+            "angularVelocity": 6.899845125281262e-16,
+            "velocityX": 2.0808040978720626,
+            "velocityY": -3.159254382625257,
+            "timestamp": 0.9100244281970103
+        },
+        {
+            "x": 2.661766753531331,
+            "y": 3.1087807297901673,
+            "heading": 5.146705636243318e-17,
+            "angularVelocity": 5.041980276892245e-16,
+            "velocityX": 2.3181513147947848,
+            "velocityY": -2.9894494876003863,
+            "timestamp": 0.9450253677430492
+        },
+        {
+            "x": 2.7507288455963135,
+            "y": 3.0107133388519287,
+            "heading": 6.422105835490463e-17,
+            "angularVelocity": 3.643902751722035e-16,
+            "velocityX": 2.5417058290140595,
+            "velocityY": -2.8018502420229328,
+            "timestamp": 0.9800263072890881
+        },
+        {
+            "x": 2.9215337593923705,
+            "y": 2.8658352980219064,
+            "heading": 5.1318206888106e-17,
+            "angularVelocity": -2.1793112103404205e-16,
+            "velocityX": 2.884920936848368,
+            "velocityY": -2.4470121145413257,
+            "timestamp": 1.0392324060123015
+        },
+        {
+            "x": 3.109750180151373,
+            "y": 2.744432888397421,
+            "heading": 6.648964720130561e-17,
+            "angularVelocity": 2.5624793121609224e-16,
+            "velocityX": 3.179003934013426,
+            "velocityY": -2.0505051378665997,
+            "timestamp": 1.098438504735515
+        },
+        {
+            "x": 3.309758578030313,
+            "y": 2.6436330516151556,
+            "heading": 6.809268296964168e-17,
+            "angularVelocity": 2.7075517621497836e-17,
+            "velocityX": 3.3781722186064402,
+            "velocityY": -1.702524553319066,
+            "timestamp": 1.1576446034587284
+        },
+        {
+            "x": 3.5097675494851996,
+            "y": 2.542834352942729,
+            "heading": 6.982732716570642e-17,
+            "angularVelocity": 2.929840393934814e-17,
+            "velocityX": 3.378181906393955,
+            "velocityY": -1.702505330465446,
+            "timestamp": 1.216850702181942
+        },
+        {
+            "x": 3.709776520948374,
+            "y": 2.4420356542872104,
+            "heading": 8.871000937094426e-17,
+            "angularVelocity": 3.189313704575333e-16,
+            "velocityX": 3.3781819065336705,
+            "velocityY": -1.702505330180368,
+            "timestamp": 1.2760568009051554
+        },
+        {
+            "x": 3.9097854924115545,
+            "y": 2.341236955631689,
+            "heading": 7.04137580064196e-17,
+            "angularVelocity": -3.090264644873572e-16,
+            "velocityX": 3.3781819065337895,
+            "velocityY": -1.7025053301803954,
+            "timestamp": 1.3352628996283689
+        },
+        {
+            "x": 4.109794463874735,
+            "y": 2.240438256976168,
+            "heading": 6.635408667627181e-17,
+            "angularVelocity": -6.856846534504647e-17,
+            "velocityX": 3.3781819065337926,
+            "velocityY": -1.7025053301803963,
+            "timestamp": 1.3944689983515823
+        },
+        {
+            "x": 4.3098034353379155,
+            "y": 2.1396395583206465,
+            "heading": 8.756379552984881e-17,
+            "angularVelocity": 3.5823520399025277e-16,
+            "velocityX": 3.3781819065337926,
+            "velocityY": -1.7025053301803963,
+            "timestamp": 1.4536750970747958
+        },
+        {
+            "x": 4.509812406801097,
+            "y": 2.0388408596651253,
+            "heading": 7.076319345079811e-17,
+            "angularVelocity": -2.837647208878057e-16,
+            "velocityX": 3.3781819065337926,
+            "velocityY": -1.7025053301803963,
+            "timestamp": 1.5128811957980093
+        },
+        {
+            "x": 4.709821378264278,
+            "y": 1.9380421610096037,
+            "heading": 6.652684249010322e-17,
+            "angularVelocity": -7.15526111686399e-17,
+            "velocityX": 3.378181906533802,
+            "velocityY": -1.7025053301803985,
+            "timestamp": 1.5720872945212228
+        },
+        {
+            "x": 4.9098303497278835,
+            "y": 1.837243462354927,
+            "heading": 7.855425123263822e-17,
+            "angularVelocity": 2.0314476045367986e-16,
+            "velocityX": 3.378181906540968,
+            "velocityY": -1.7025053301661324,
+            "timestamp": 1.6312933932444362
+        },
+        {
+            "x": 5.109839349236117,
+            "y": 1.7364448193478421,
+            "heading": 7.31685607290898e-17,
+            "angularVelocity": -9.096513061477316e-17,
+            "velocityX": 3.378182380218786,
+            "velocityY": -1.7025043902702668,
+            "timestamp": 1.6904994919676497
+        },
+        {
+            "x": 5.311619758605957,
+            "y": 1.6392407417297363,
+            "heading": 6.073866217998324e-17,
+            "angularVelocity": -2.0994287441931112e-16,
+            "velocityX": 3.4081017618384997,
+            "velocityY": -1.6417916348765775,
+            "timestamp": 1.7497055906908632
+        },
+        {
+            "x": 5.430192834880249,
+            "y": 1.5926107881863976,
+            "heading": 7.355026472308393e-17,
+            "angularVelocity": 3.8038299911394555e-16,
+            "velocityX": 3.5204949744296554,
+            "velocityY": -1.3844670499027307,
+            "timestamp": 1.78338638760593
+        },
+        {
+            "x": 5.550146685206584,
+            "y": 1.5496573158737061,
+            "heading": 8.480161924468705e-17,
+            "angularVelocity": 3.3405844137167443e-16,
+            "velocityX": 3.5614908586879754,
+            "velocityY": -1.2753104512640399,
+            "timestamp": 1.8170671845209967
+        },
+        {
+            "x": 5.670100671705041,
+            "y": 1.506704223843596,
+            "heading": 8.467696633181045e-17,
+            "angularVelocity": -3.701008417078351e-18,
+            "velocityX": 3.5614949017075612,
+            "velocityY": -1.2752991604806918,
+            "timestamp": 1.8507479814360634
+        },
+        {
+            "x": 5.790054658213967,
+            "y": 1.4637511318444054,
+            "heading": 8.225246246818014e-17,
+            "angularVelocity": -7.198475350046388e-17,
+            "velocityX": 3.561494902018399,
+            "velocityY": -1.275299159562779,
+            "timestamp": 1.8844287783511302
+        },
+        {
+            "x": 5.910008644722922,
+            "y": 1.4207980398452125,
+            "heading": 9.443730259968232e-17,
+            "angularVelocity": 3.617741041647218e-16,
+            "velocityX": 3.5614949020192235,
+            "velocityY": -1.2752991595628402,
+            "timestamp": 1.918109575266197
+        },
+        {
+            "x": 6.02996263123188,
+            "y": 1.3778449478460182,
+            "heading": 8.582959689044548e-17,
+            "angularVelocity": -2.555671628240573e-16,
+            "velocityX": 3.5614949020193487,
+            "velocityY": -1.2752991595628858,
+            "timestamp": 1.9517903721812637
+        },
+        {
+            "x": 6.149916617740839,
+            "y": 1.3348918558468235,
+            "heading": 7.701354643967334e-17,
+            "angularVelocity": -2.6175302422337863e-16,
+            "velocityX": 3.56149490201936,
+            "velocityY": -1.2752991595628895,
+            "timestamp": 1.9854711690963305
+        },
+        {
+            "x": 6.269870604249798,
+            "y": 1.291938763847629,
+            "heading": 8.367010625720842e-17,
+            "angularVelocity": 1.9763664839406868e-16,
+            "velocityX": 3.5614949020193607,
+            "velocityY": -1.27529915956289,
+            "timestamp": 2.019151966011397
+        },
+        {
+            "x": 6.3898245907587565,
+            "y": 1.2489856718484342,
+            "heading": 7.968637324735258e-17,
+            "angularVelocity": -1.1827906031741492e-16,
+            "velocityX": 3.5614949020193607,
+            "velocityY": -1.2752991595628902,
+            "timestamp": 2.0528327629264638
+        },
+        {
+            "x": 6.509778577267715,
+            "y": 1.2060325798492397,
+            "heading": 7.914041276645377e-17,
+            "angularVelocity": -1.6209844508000374e-17,
+            "velocityX": 3.561494902019361,
+            "velocityY": -1.27529915956289,
+            "timestamp": 2.0865135598415305
+        },
+        {
+            "x": 6.629732563776674,
+            "y": 1.1630794878500452,
+            "heading": 7.575912442206911e-17,
+            "angularVelocity": -1.0039217162560839e-16,
+            "velocityX": 3.561494902019361,
+            "velocityY": -1.27529915956289,
+            "timestamp": 2.1201943567565973
+        },
+        {
+            "x": 6.749686550285633,
+            "y": 1.1201263958508503,
+            "heading": 6.889954024340196e-17,
+            "angularVelocity": -2.0366454499176497e-16,
+            "velocityX": 3.5614949020193576,
+            "velocityY": -1.2752991595628989,
+            "timestamp": 2.153875153671664
+        },
+        {
+            "x": 6.869640536794461,
+            "y": 1.077173303851719,
+            "heading": 8.054846416919904e-17,
+            "angularVelocity": 3.458624792985843e-16,
+            "velocityX": 3.5614949020154905,
+            "velocityY": -1.275299159561009,
+            "timestamp": 2.187555950586731
+        },
+        {
+            "x": 6.985406128017608,
+            "y": 1.0357199913369377,
+            "heading": 6.97905590140722e-17,
+            "angularVelocity": -3.1940767538205783e-16,
+            "velocityX": 3.437139314579595,
+            "velocityY": -1.2307699434580792,
+            "timestamp": 2.2212367475017976
+        },
+        {
+            "x": 7.092266700241795,
+            "y": 0.9974553856875888,
+            "heading": 5.947830002893604e-17,
+            "angularVelocity": -3.0617621445548207e-16,
+            "velocityX": 3.172744768885924,
+            "velocityY": -1.1360956139440452,
+            "timestamp": 2.2549175444168643
+        },
+        {
+            "x": 7.190222235939473,
+            "y": 0.9623794931799317,
+            "heading": 5.044588900647289e-17,
+            "angularVelocity": -2.6817687744625345e-16,
+            "velocityX": 2.9083497027904013,
+            "velocityY": -1.0414210980846788,
+            "timestamp": 2.288598341331931
+        },
+        {
+            "x": 7.279272729268112,
+            "y": 0.930492315906057,
+            "heading": 4.27627289839913e-17,
+            "angularVelocity": -2.281169282784766e-16,
+            "velocityX": 2.6439544632272356,
+            "velocityY": -0.9467465201100714,
+            "timestamp": 2.322279138246998
+        },
+        {
+            "x": 7.35941817730645,
+            "y": 0.9017938549120106,
+            "heading": 3.626946964953948e-17,
+            "angularVelocity": -1.9278817213060433e-16,
+            "velocityX": 2.379559136930222,
+            "velocityY": -0.8520719110778341,
+            "timestamp": 2.3559599351620646
+        },
+        {
+            "x": 7.430658578301727,
+            "y": 0.8762841108254199,
+            "heading": 2.9442071838197796e-17,
+            "angularVelocity": -2.0270891340137855e-16,
+            "velocityX": 2.1151637585928986,
+            "velocityY": -0.7573972834110186,
+            "timestamp": 2.3896407320771313
+        },
+        {
+            "x": 7.492993931085436,
+            "y": 0.8539630840647031,
+            "heading": 2.3413209096667504e-17,
+            "angularVelocity": -1.790000012026981e-16,
+            "velocityX": 1.850768345562039,
+            "velocityY": -0.662722643321152,
+            "timestamp": 2.423321528992198
+        },
+        {
+            "x": 7.546424234822929,
+            "y": 0.8348307749287305,
+            "heading": 1.7731266545962487e-17,
+            "angularVelocity": -1.6869976443484408e-16,
+            "velocityX": 1.5863729077500839,
+            "velocityY": -0.5680479943576784,
+            "timestamp": 2.457002325907265
+        },
+        {
+            "x": 7.590949488888224,
+            "y": 0.8188871836416546,
+            "heading": 1.2363765500283435e-17,
+            "angularVelocity": -1.593638362048541e-16,
+            "velocityX": 1.321977451352309,
+            "velocityY": -0.4733733387389999,
+            "timestamp": 2.4906831228223316
+        },
+        {
+            "x": 7.626569692794441,
+            "y": 0.8061323103778162,
+            "heading": 7.88828312817125e-18,
+            "angularVelocity": -1.3287934794211324e-16,
+            "velocityX": 1.057581980498898,
+            "velocityY": -0.37869867794405154,
+            "timestamp": 2.5243639197373984
+        },
+        {
+            "x": 7.653284846152079,
+            "y": 0.7965661552766882,
+            "heading": 4.236479358871306e-18,
+            "angularVelocity": -1.0842391112822455e-16,
+            "velocityX": 0.7931864980809786,
+            "velocityY": -0.28402401300808766,
+            "timestamp": 2.558044716652465
+        },
+        {
+            "x": 7.671094948642455,
+            "y": 0.7901887184523844,
+            "heading": 1.249730071610374e-18,
+            "angularVelocity": -8.867810508445172e-17,
+            "velocityX": 0.5287910062011898,
+            "velocityY": -0.18934934468402032,
+            "timestamp": 2.591725513567532
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -2.906959217388984e-34,
+            "angularVelocity": -3.7105121777105e-17,
+            "velocityX": 0.26439550643651005,
+            "velocityY": -0.09467467353653367,
+            "timestamp": 2.6254063104825986
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -4.078160951605287e-34,
+            "angularVelocity": -3.477050502256718e-33,
+            "velocityX": 7.685636324015334e-33,
+            "velocityY": -2.3747808746440768e-32,
+            "timestamp": 2.6590871073976654
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.1.traj
new file mode 100644
index 00000000..641af2c0
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.1.traj	
@@ -0,0 +1,112 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": -1.3982338019676807e-32,
+            "velocityX": 1.9225496754980614e-32,
+            "velocityY": -8.567103847838605e-38,
+            "timestamp": 0
+        },
+        {
+            "x": 1.3816658535274804,
+            "y": 5.599900984360149,
+            "heading": 0.013082876743969601,
+            "angularVelocity": 0.16716651866961724,
+            "velocityX": 0.4046105912700521,
+            "velocityY": 0.509835013927756,
+            "timestamp": 0.07826254233257365
+        },
+        {
+            "x": 1.4449977124682947,
+            "y": 5.679702872674784,
+            "heading": 0.039246215534929935,
+            "angularVelocity": 0.3343021835373132,
+            "velocityX": 0.8092231232623148,
+            "velocityY": 1.0196690004717623,
+            "timestamp": 0.1565250846651473
+        },
+        {
+            "x": 1.5399958043003432,
+            "y": 5.799405527723277,
+            "heading": 0.07848745324353076,
+            "angularVelocity": 0.501405098007769,
+            "velocityX": 1.2138385618545031,
+            "velocityY": 1.5295012336785618,
+            "timestamp": 0.23478762699772096
+        },
+        {
+            "x": 1.6666604122853452,
+            "y": 5.959008727776544,
+            "heading": 0.1308059995871385,
+            "angularVelocity": 0.6685004701391144,
+            "velocityX": 1.6184576198246376,
+            "velocityY": 2.039330633740969,
+            "timestamp": 0.3130501693302946
+        },
+        {
+            "x": 1.8249918316073899,
+            "y": 6.158512135429991,
+            "heading": 0.19620604644996878,
+            "angularVelocity": 0.8356494040905814,
+            "velocityX": 2.023080449510844,
+            "velocityY": 2.5491557226145924,
+            "timestamp": 0.3913127116628683
+        },
+        {
+            "x": 1.9833278311922586,
+            "y": 6.358008426099132,
+            "heading": 0.26178707056088246,
+            "angularVelocity": 0.8379618417228061,
+            "velocityX": 2.023138973840461,
+            "velocityY": 2.549064785314409,
+            "timestamp": 0.469575253995442
+        },
+        {
+            "x": 2.109996681019138,
+            "y": 6.517605179473628,
+            "heading": 0.31426571552794874,
+            "angularVelocity": 0.6705461310477281,
+            "velocityX": 1.618511819979031,
+            "velocityY": 2.039248261272889,
+            "timestamp": 0.5478377963280157
+        },
+        {
+            "x": 2.204998332006287,
+            "y": 6.637302612224517,
+            "heading": 0.35363163522076196,
+            "angularVelocity": 0.5029982226430836,
+            "velocityX": 1.2138840389754155,
+            "velocityY": 1.5294345057465644,
+            "timestamp": 0.6261003386605893
+        },
+        {
+            "x": 2.268332773183591,
+            "y": 6.717100865272008,
+            "heading": 0.3798775217934225,
+            "angularVelocity": 0.3353569382033373,
+            "velocityX": 0.8092561178011136,
+            "velocityY": 1.0196225508288703,
+            "timestamp": 0.704362880993163
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0.1676725265429579,
+            "velocityX": 0.40462813847574725,
+            "velocityY": 0.509811380244222,
+            "timestamp": 0.7826254233257367
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 9.092584352802723e-32,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0.8608879656583104
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.1.traj
new file mode 100644
index 00000000..bb0e9680
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.1.traj	
@@ -0,0 +1,76 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 7.039472547405692e-42,
+            "angularVelocity": 1.0705191064262213e-40,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 1.4250000045295101,
+            "y": 5.56,
+            "heading": 8.194631828276944e-23,
+            "angularVelocity": 8.6403876700238e-22,
+            "velocityX": 0.7907971475606789,
+            "velocityY": 8.424071056371489e-35,
+            "timestamp": 0.0948410155004449
+        },
+        {
+            "x": 1.5750000090590195,
+            "y": 5.56,
+            "heading": 2.6954329855279996e-22,
+            "angularVelocity": 1.978015178210688e-21,
+            "velocityX": 1.5815942473623736,
+            "velocityY": 1.2488550168110521e-34,
+            "timestamp": 0.1896820310008898
+        },
+        {
+            "x": 1.7999999999999998,
+            "y": 5.56,
+            "heading": 5.87848829010012e-22,
+            "angularVelocity": 3.3562006983676933e-21,
+            "velocityX": 2.3723912038871497,
+            "velocityY": 2.135553914770072e-34,
+            "timestamp": 0.2845230465013347
+        },
+        {
+            "x": 2.0249999909409806,
+            "y": 5.56,
+            "heading": 4.475372520266276e-22,
+            "angularVelocity": -1.4794393741169351e-21,
+            "velocityX": 2.37239120388715,
+            "velocityY": 2.583104828416982e-34,
+            "timestamp": 0.3793640620017796
+        },
+        {
+            "x": 2.17499999547049,
+            "y": 5.56,
+            "heading": 1.6184867431561565e-22,
+            "angularVelocity": -3.012289189189882e-21,
+            "velocityX": 1.5815942473623736,
+            "velocityY": 1.9272829246954695e-34,
+            "timestamp": 0.47420507750222446
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 1.246024226890136e-41,
+            "angularVelocity": -1.7065260802739357e-21,
+            "velocityX": 0.7907971475606789,
+            "velocityY": -3.1542839688982834e-34,
+            "timestamp": 0.5690460930026694
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 7.131086050293936e-41,
+            "angularVelocity": 6.207915462550707e-40,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0.6638871085031143
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.1.traj
new file mode 100644
index 00000000..aceaac81
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.1.traj	
@@ -0,0 +1,112 @@
+{
+    "samples": [
+        {
+            "x": 1.35,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 5.56203910119815e-35,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 1.3816658774685813,
+            "y": 5.519999060819193,
+            "heading": -0.012983997100427386,
+            "angularVelocity": -0.16577951920330813,
+            "velocityX": 0.4043095436088268,
+            "velocityY": -0.5107315115509736,
+            "timestamp": 0.07832087559926142
+        },
+        {
+            "x": 1.4449977779940397,
+            "y": 5.439997258819368,
+            "heading": -0.038949622534069785,
+            "angularVelocity": -0.33152879401526647,
+            "velocityX": 0.8086209460872757,
+            "velocityY": -1.0214620481155445,
+            "timestamp": 0.15664175119852283
+        },
+        {
+            "x": 1.5399959199593471,
+            "y": 5.3199947242673264,
+            "heading": -0.07789432529768456,
+            "angularVelocity": -0.497245497648426,
+            "velocityX": 1.2129351368768808,
+            "velocityY": -1.5321909214351659,
+            "timestamp": 0.23496262679778424
+        },
+        {
+            "x": 1.6666605759505058,
+            "y": 5.159991667949077,
+            "heading": -0.1298174067167751,
+            "angularVelocity": -0.662953280613991,
+            "velocityX": 1.6172528080412967,
+            "velocityY": -2.042917103441561,
+            "timestamp": 0.31328350239704567
+        },
+        {
+            "x": 1.8249920311985446,
+            "y": 4.959988411604181,
+            "heading": -0.19472272142711608,
+            "angularVelocity": -0.828710279522869,
+            "velocityX": 2.0215741210320677,
+            "velocityY": -2.5536391774810774,
+            "timestamp": 0.3916043779963071
+        },
+        {
+            "x": 1.9833279596837488,
+            "y": 4.759991951944026,
+            "heading": -0.259798921868918,
+            "angularVelocity": -0.8308921464919682,
+            "velocityX": 2.021631235270526,
+            "velocityY": -2.553552397491847,
+            "timestamp": 0.4699252535955685
+        },
+        {
+            "x": 2.1099967561163506,
+            "y": 4.599995051908685,
+            "heading": -0.3118731002379449,
+            "angularVelocity": -0.6648824846579983,
+            "velocityX": 1.6173056731479674,
+            "velocityY": -2.042838500095239,
+            "timestamp": 0.54824612919483
+        },
+        {
+            "x": 2.2049983692603425,
+            "y": 4.4799975041635465,
+            "heading": -0.3509353765811358,
+            "angularVelocity": -0.4987466757018699,
+            "velocityX": 1.2129794568446812,
+            "velocityY": -1.532127249944463,
+            "timestamp": 0.6265670047940913
+        },
+        {
+            "x": 2.268332785802464,
+            "y": 4.399999173265512,
+            "heading": -0.3769787554180828,
+            "angularVelocity": -0.33252154853581545,
+            "velocityX": 0.808653070557838,
+            "velocityY": -1.021417729129522,
+            "timestamp": 0.7048878803933527
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.16625509460009477,
+            "velocityX": 0.40432661094808764,
+            "velocityY": -0.5107089643656818,
+            "timestamp": 0.7832087559926141
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -5.703180031220666e-32,
+            "velocityX": 1.0257416304055705e-32,
+            "velocityY": 0,
+            "timestamp": 0.8615296315918755
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.1.traj
new file mode 100644
index 00000000..66955466
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.238209882810899,
+            "y": 6.650800869780916,
+            "heading": 0.2470077179050236,
+            "angularVelocity": -0.14636941117769697,
+            "velocityX": 0.7008486923098889,
+            "velocityY": 0.23433997503541223,
+            "timestamp": 0.08876364255722656
+        },
+        {
+            "x": 5.362652939631404,
+            "y": 6.692404642961414,
+            "heading": 0.22258646311132924,
+            "angularVelocity": -0.2751267759001645,
+            "velocityX": 1.401959780326283,
+            "velocityY": 0.4687028605622118,
+            "timestamp": 0.17752728511445312
+        },
+        {
+            "x": 5.549362223961953,
+            "y": 6.754812642679732,
+            "heading": 0.18937073322571307,
+            "angularVelocity": -0.37420422291202055,
+            "velocityX": 2.1034432449090223,
+            "velocityY": 0.7030806523975865,
+            "timestamp": 0.26629092767167967
+        },
+        {
+            "x": 5.798381050208957,
+            "y": 6.8380191875714225,
+            "heading": 0.15275465831989177,
+            "angularVelocity": -0.4125120809759026,
+            "velocityX": 2.8054146841303256,
+            "velocityY": 0.9373944386978721,
+            "timestamp": 0.35505457022890624
+        },
+        {
+            "x": 6.109622945204874,
+            "y": 6.941929047341865,
+            "heading": 0.1302624797347834,
+            "angularVelocity": -0.25339404667863696,
+            "velocityX": 3.506411927553799,
+            "velocityY": 1.1706353725121932,
+            "timestamp": 0.4438182127861328
+        },
+        {
+            "x": 6.4280450770381,
+            "y": 7.048515369175043,
+            "heading": 0.1302624686930749,
+            "angularVelocity": -1.2439475582812342e-7,
+            "velocityX": 3.587303570016191,
+            "velocityY": 1.2007880564873126,
+            "timestamp": 0.5325818553433593
+        },
+        {
+            "x": 6.746467208870004,
+            "y": 7.1551016910121685,
+            "heading": 0.13026245765136438,
+            "angularVelocity": -1.2439477842907252e-7,
+            "velocityX": 3.5873035700012967,
+            "velocityY": 1.2007880565318134,
+            "timestamp": 0.6213454979005859
+        },
+        {
+            "x": 7.057671454315644,
+            "y": 7.258999412253756,
+            "heading": 0.10797351007767278,
+            "angularVelocity": -0.2511044717394208,
+            "velocityX": 3.5059877724700192,
+            "velocityY": 1.1704986213770205,
+            "timestamp": 0.7101091404578125
+        },
+        {
+            "x": 7.3066745305511045,
+            "y": 7.342200909102172,
+            "heading": 0.07025614763700679,
+            "angularVelocity": -0.4249190474112408,
+            "velocityX": 2.8052372464872497,
+            "velocityY": 0.9373375680789704,
+            "timestamp": 0.7988727830150391
+        },
+        {
+            "x": 7.49336787770315,
+            "y": 7.4046033027166684,
+            "heading": 0.03585076588111199,
+            "angularVelocity": -0.3876066908033862,
+            "velocityX": 2.103263698666567,
+            "velocityY": 0.7030174947390144,
+            "timestamp": 0.8876364255722656
+        },
+        {
+            "x": 7.617797738081694,
+            "y": 7.446202129881464,
+            "heading": 0.010501179707511226,
+            "angularVelocity": -0.2855852401136722,
+            "velocityX": 1.4018111108759532,
+            "velocityY": 0.4686471393732002,
+            "timestamp": 0.9764000681294922
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -0.003,
+            "angularVelocity": -0.15210258748116404,
+            "velocityX": 0.7007628362953491,
+            "velocityY": 0.23430618121741137,
+            "timestamp": 1.0651637106867187
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -0.003,
+            "angularVelocity": -3.7910464085672074e-31,
+            "velocityX": 1.2889893239032156e-32,
+            "velocityY": 1.1630630989344696e-31,
+            "timestamp": 1.1539273532439451
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.1.traj
new file mode 100644
index 00000000..63556f1f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.242063425541118,
+            "y": 6.625917379024557,
+            "heading": 0.21680591849920633,
+            "angularVelocity": -0.48090771817524164,
+            "velocityX": 0.735526954803639,
+            "velocityY": -0.04545446665971497,
+            "timestamp": 0.08981781715770647
+        },
+        {
+            "x": 5.374390185654174,
+            "y": 6.616899293663565,
+            "heading": 0.13592925245739512,
+            "angularVelocity": -0.9004523668149724,
+            "velocityX": 1.473279626476675,
+            "velocityY": -0.10040419202302918,
+            "timestamp": 0.17963563431541293
+        },
+        {
+            "x": 5.573225936717345,
+            "y": 6.601485332040172,
+            "heading": 0.027174297785337913,
+            "angularVelocity": -1.2108394315695736,
+            "velocityX": 2.2137673499017074,
+            "velocityY": -0.1716136298027423,
+            "timestamp": 0.2694534514731194
+        },
+        {
+            "x": 5.8386201791233034,
+            "y": 6.576536765329541,
+            "heading": -0.0884788299359431,
+            "angularVelocity": -1.287641265186969,
+            "velocityX": 2.9548061932964313,
+            "velocityY": -0.27776857087079165,
+            "timestamp": 0.35927126863082587
+        },
+        {
+            "x": 6.16462893780028,
+            "y": 6.53182752327052,
+            "heading": -0.12460373299787371,
+            "angularVelocity": -0.4022019706680331,
+            "velocityX": 3.6296669078981743,
+            "velocityY": -0.49777698316268804,
+            "timestamp": 0.44908908578853235
+        },
+        {
+            "x": 6.485747891959683,
+            "y": 6.420787077377064,
+            "heading": -0.12460373462910332,
+            "angularVelocity": -1.816153684215185e-8,
+            "velocityX": 3.5752255434527758,
+            "velocityY": -1.2362852873443355,
+            "timestamp": 0.5389069029462388
+        },
+        {
+            "x": 6.7868856142210525,
+            "y": 6.263423057279399,
+            "heading": -0.12460372221510443,
+            "angularVelocity": 1.382130993675135e-7,
+            "velocityX": 3.352761532075727,
+            "velocityY": -1.7520356770790513,
+            "timestamp": 0.6287247201039452
+        },
+        {
+            "x": 7.0844936481565455,
+            "y": 6.108042080528118,
+            "heading": -0.11377963349551397,
+            "angularVelocity": 0.12051159850149781,
+            "velocityX": 3.313463223148008,
+            "velocityY": -1.7299571696165401,
+            "timestamp": 0.7185425372616516
+        },
+        {
+            "x": 7.322771112714566,
+            "y": 5.983596379959823,
+            "heading": -0.07830444960996434,
+            "angularVelocity": 0.39496822577262786,
+            "velocityX": 2.652897521876331,
+            "velocityY": -1.3855346801602506,
+            "timestamp": 0.808360354419358
+        },
+        {
+            "x": 7.501421172621153,
+            "y": 5.8902827497271355,
+            "heading": -0.04357887494407857,
+            "angularVelocity": 0.3866223402525239,
+            "velocityX": 1.9890269610193678,
+            "velocityY": -1.038921153793384,
+            "timestamp": 0.8981781715770645
+        },
+        {
+            "x": 7.62048387761172,
+            "y": 5.828089668977501,
+            "heading": -0.01724918093308134,
+            "angularVelocity": 0.29314555668577735,
+            "velocityX": 1.3256022998366965,
+            "velocityY": -0.6924358965486016,
+            "timestamp": 0.9879959887347709
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -0.003,
+            "angularVelocity": 0.15864537108558247,
+            "velocityX": 0.6626315832612425,
+            "velocityY": -0.346141444552285,
+            "timestamp": 1.0778138058924773
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -0.003,
+            "angularVelocity": 0,
+            "velocityX": 4.386752331989148e-33,
+            "velocityY": -2.33652586669764e-33,
+            "timestamp": 1.1676316230501838
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3.1.traj
new file mode 100644
index 00000000..b1b40339
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3.1.traj	
@@ -0,0 +1,157 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.246662494261999,
+            "y": 6.628692732737936,
+            "heading": 0.18398762968545185,
+            "angularVelocity": -0.8088463398355284,
+            "velocityX": 0.7519183997414132,
+            "velocityY": -0.013910608703967415,
+            "timestamp": 0.09397628025368156
+        },
+        {
+            "x": 5.3885394117424426,
+            "y": 6.6228569584156665,
+            "heading": 0.04133864379940616,
+            "angularVelocity": -1.5179254328962157,
+            "velocityX": 1.509709866122152,
+            "velocityY": -0.0620983753189278,
+            "timestamp": 0.18795256050736311
+        },
+        {
+            "x": 5.602094427902648,
+            "y": 6.606624752166298,
+            "heading": -0.14949344735311892,
+            "angularVelocity": -2.0306410366252936,
+            "velocityX": 2.272435295201409,
+            "velocityY": -0.172726630651381,
+            "timestamp": 0.2819288407610447
+        },
+        {
+            "x": 5.885007714232348,
+            "y": 6.565697735942791,
+            "heading": -0.3456589598447775,
+            "angularVelocity": -2.087393882393782,
+            "velocityX": 3.010475468554385,
+            "velocityY": -0.4355036836213049,
+            "timestamp": 0.37590512101472623
+        },
+        {
+            "x": 6.196762938930662,
+            "y": 6.464967292979978,
+            "heading": -0.4305176558630451,
+            "angularVelocity": -0.9029799412064233,
+            "velocityX": 3.3173820442430326,
+            "velocityY": -1.0718709305252299,
+            "timestamp": 0.4698814012684078
+        },
+        {
+            "x": 6.507653773495931,
+            "y": 6.294193106326001,
+            "heading": -0.4331106719684992,
+            "angularVelocity": -0.027592240280786797,
+            "velocityX": 3.3081840835372915,
+            "velocityY": -1.8172052159649787,
+            "timestamp": 0.5638576815220894
+        },
+        {
+            "x": 6.777249226823148,
+            "y": 6.0624534109685,
+            "heading": -0.433110646067504,
+            "angularVelocity": 2.7561204856502443e-7,
+            "velocityX": 2.8687606340606973,
+            "velocityY": -2.465938157287546,
+            "timestamp": 0.6578339617757709
+        },
+        {
+            "x": 6.99331744885121,
+            "y": 5.780142381909384,
+            "heading": -0.4331105937998759,
+            "angularVelocity": 5.561789419211182e-7,
+            "velocityX": 2.2991782761011916,
+            "velocityY": -3.004066859180208,
+            "timestamp": 0.7518102420294525
+        },
+        {
+            "x": 7.1465883664001515,
+            "y": 5.459372959434879,
+            "heading": -0.43311054984028735,
+            "angularVelocity": 4.677732342413875e-7,
+            "velocityX": 1.6309532270823943,
+            "velocityY": -3.4133019694822235,
+            "timestamp": 0.845786522283134
+        },
+        {
+            "x": 7.2797482024400875,
+            "y": 5.1297467242247805,
+            "heading": -0.4331105067287741,
+            "angularVelocity": 4.5874887908824285e-7,
+            "velocityX": 1.4169515507581365,
+            "velocityY": -3.507547163180951,
+            "timestamp": 0.9397628025368155
+        },
+        {
+            "x": 7.407925031698652,
+            "y": 4.799268903667217,
+            "heading": -0.4305589051420175,
+            "angularVelocity": 0.027151549091629798,
+            "velocityX": 1.3639274603395894,
+            "velocityY": -3.5166088683810885,
+            "timestamp": 1.033739082790497
+        },
+        {
+            "x": 7.517390810231398,
+            "y": 4.529437032753176,
+            "heading": -0.29559925755962413,
+            "angularVelocity": 1.4361033147734772,
+            "velocityX": 1.164823487770018,
+            "velocityY": -2.871276349581525,
+            "timestamp": 1.1277153630441787
+        },
+        {
+            "x": 7.59899827381295,
+            "y": 4.327770783855267,
+            "heading": -0.16069313013903275,
+            "angularVelocity": 1.4355338076419173,
+            "velocityX": 0.8683836321384404,
+            "velocityY": -2.1459271249460685,
+            "timestamp": 1.2216916432978604
+        },
+        {
+            "x": 7.653087401223331,
+            "y": 4.193799869992724,
+            "heading": -0.05829734753961687,
+            "angularVelocity": 1.0895917812772173,
+            "velocityX": 0.5755614849233384,
+            "velocityY": -1.4255822160751557,
+            "timestamp": 1.315667923551542
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -0.003,
+            "angularVelocity": 0.5884181347713066,
+            "velocityX": 0.28637650590148805,
+            "velocityY": -0.7108162805806236,
+            "timestamp": 1.4096442038052237
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -0.003,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.8051389340682807e-33,
+            "timestamp": 1.5036204840589054
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C4.1.traj
new file mode 100644
index 00000000..3f479acb
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C4.1.traj	
@@ -0,0 +1,247 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": 0,
+            "velocityX": -1.7123315632321391e-31,
+            "velocityY": 4.571975133912053e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 5.2081944432473115,
+            "y": 6.63354239454729,
+            "heading": 0.2524699257138384,
+            "angularVelocity": -0.12069194203891866,
+            "velocityX": 0.5160121574790498,
+            "velocityY": 0.0567774581143745,
+            "timestamp": 0.062390861883169
+        },
+        {
+            "x": 5.2726441787772975,
+            "y": 6.640106281427696,
+            "heading": 0.23785051964847434,
+            "angularVelocity": -0.2343196683632906,
+            "velocityX": 1.0329996025807835,
+            "velocityY": 0.1052059016703473,
+            "timestamp": 0.124781723766338
+        },
+        {
+            "x": 5.369418980987113,
+            "y": 6.648966513737627,
+            "heading": 0.21675536507543205,
+            "angularVelocity": -0.33811288923278293,
+            "velocityX": 1.551105390898958,
+            "velocityY": 0.14201169918963874,
+            "timestamp": 0.187172585649507
+        },
+        {
+            "x": 5.498590594297912,
+            "y": 6.659046571836944,
+            "heading": 0.19009632617593986,
+            "angularVelocity": -0.42729076173705777,
+            "velocityX": 2.070361097955043,
+            "velocityY": 0.16156305258605722,
+            "timestamp": 0.249563447532676
+        },
+        {
+            "x": 5.6601948909824875,
+            "y": 6.668590441412665,
+            "heading": 0.15936442465931713,
+            "angularVelocity": -0.49257055583183496,
+            "velocityX": 2.5901917653772815,
+            "velocityY": 0.15296902924013772,
+            "timestamp": 0.311954309415845
+        },
+        {
+            "x": 5.854024302844021,
+            "y": 6.674275452040902,
+            "heading": 0.12740102584180835,
+            "angularVelocity": -0.512308980077189,
+            "velocityX": 3.106695532183737,
+            "velocityY": 0.09111928344386595,
+            "timestamp": 0.374345171299014
+        },
+        {
+            "x": 6.078018113514707,
+            "y": 6.668089747759068,
+            "heading": 0.10126004011963459,
+            "angularVelocity": -0.4189874115076864,
+            "velocityX": 3.590170161299073,
+            "velocityY": -0.09914439543117176,
+            "timestamp": 0.436736033182183
+        },
+        {
+            "x": 6.311203816285634,
+            "y": 6.63161682590639,
+            "heading": 0.10125992071184643,
+            "angularVelocity": -0.000001913866624365403,
+            "velocityX": 3.737497699704501,
+            "velocityY": -0.584587562213517,
+            "timestamp": 0.499126895065352
+        },
+        {
+            "x": 6.53718081427167,
+            "y": 6.563497401833266,
+            "heading": 0.10125989664992657,
+            "angularVelocity": -3.856641685769623e-7,
+            "velocityX": 3.621956664249834,
+            "velocityY": -1.0918173273625358,
+            "timestamp": 0.561517756948521
+        },
+        {
+            "x": 6.7516755548198235,
+            "y": 6.465019697743108,
+            "heading": 0.10125985817262735,
+            "angularVelocity": -6.167136938553595e-7,
+            "velocityX": 3.4379191771674527,
+            "velocityY": -1.5783994821960954,
+            "timestamp": 0.62390861883169
+        },
+        {
+            "x": 6.950631618499756,
+            "y": 6.338046073913574,
+            "heading": 0.10125971935519859,
+            "angularVelocity": -0.000002224964167076665,
+            "velocityX": 3.1888654471946203,
+            "velocityY": -2.035131748416977,
+            "timestamp": 0.686299480714859
+        },
+        {
+            "x": 7.196058677643184,
+            "y": 6.101503366378845,
+            "heading": 0.10125966337035743,
+            "angularVelocity": -6.213285755242982e-7,
+            "velocityX": 2.723788105663008,
+            "velocityY": -2.625188174087521,
+            "timestamp": 0.7764045291337008
+        },
+        {
+            "x": 7.389899318639429,
+            "y": 5.821123760827136,
+            "heading": 0.10125964077104323,
+            "angularVelocity": -2.508107435019506e-7,
+            "velocityX": 2.1512739230237647,
+            "velocityY": -3.1116969633976366,
+            "timestamp": 0.8665095775525427
+        },
+        {
+            "x": 7.524507781478929,
+            "y": 5.507966472295818,
+            "heading": 0.101259626680438,
+            "angularVelocity": -1.5637975290365853e-7,
+            "velocityX": 1.4939058931947202,
+            "velocityY": -3.475468844716046,
+            "timestamp": 0.9566146259713845
+        },
+        {
+            "x": 7.594574766806078,
+            "y": 5.174383579102459,
+            "heading": 0.10125961555168732,
+            "angularVelocity": -1.235086255399969e-7,
+            "velocityX": 0.7776144240159958,
+            "velocityY": -3.702155418004344,
+            "timestamp": 1.0467196743902263
+        },
+        {
+            "x": 7.605242469312774,
+            "y": 4.833688510035603,
+            "heading": 0.10125960514477245,
+            "angularVelocity": -1.1549757817967306e-7,
+            "velocityX": 0.11839184034432584,
+            "velocityY": -3.781087464524514,
+            "timestamp": 1.136824722809068
+        },
+        {
+            "x": 7.615908675824772,
+            "y": 4.492993394135096,
+            "heading": 0.10125959473787158,
+            "angularVelocity": -1.1549742264017462e-7,
+            "velocityX": 0.11837523756082047,
+            "velocityY": -3.781087984291716,
+            "timestamp": 1.2269297712279097
+        },
+        {
+            "x": 7.6265748461946865,
+            "y": 4.152298277103123,
+            "heading": 0.10125958433097336,
+            "angularVelocity": -1.1549739343149193e-7,
+            "velocityX": 0.11837483645025074,
+            "velocityY": -3.781087996848918,
+            "timestamp": 1.3170348196467514
+        },
+        {
+            "x": 7.637240986091898,
+            "y": 3.8116031591171406,
+            "heading": 0.10125957392407539,
+            "angularVelocity": -1.1549739036497336e-7,
+            "velocityX": 0.11837449825941278,
+            "velocityY": -3.78108800743665,
+            "timestamp": 1.407139868065593
+        },
+        {
+            "x": 7.647907101492937,
+            "y": 3.470908040364265,
+            "heading": 0.10125956351717343,
+            "angularVelocity": -1.1549743491773867e-7,
+            "velocityX": 0.11837422639702805,
+            "velocityY": -3.781088015947766,
+            "timestamp": 1.4972449164844348
+        },
+        {
+            "x": 7.658649980665503,
+            "y": 3.1330359890940187,
+            "heading": 0.09207038783752043,
+            "angularVelocity": -0.10198291706072676,
+            "velocityX": 0.11922616280763662,
+            "velocityY": -3.7497571689844853,
+            "timestamp": 1.5873499649032765
+        },
+        {
+            "x": 7.667206120655169,
+            "y": 2.862563871388905,
+            "heading": 0.061604548821908564,
+            "angularVelocity": -0.33811467337536444,
+            "velocityX": 0.09495738740298482,
+            "velocityY": -3.0017421049246735,
+            "timestamp": 1.6774550133221182
+        },
+        {
+            "x": 7.673608699461006,
+            "y": 2.659753859006467,
+            "heading": 0.031813701428875164,
+            "angularVelocity": -0.33062351017837394,
+            "velocityX": 0.07105682665061783,
+            "velocityY": -2.250817417462574,
+            "timestamp": 1.76756006174096
+        },
+        {
+            "x": 7.677870998153504,
+            "y": 2.524576540197495,
+            "heading": 0.009226184845951672,
+            "angularVelocity": -0.25067981183391225,
+            "velocityX": 0.04730366130750142,
+            "velocityY": -1.5002191462193974,
+            "timestamp": 1.8576651101598016
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -0.003,
+            "angularVelocity": -0.1356881224803119,
+            "velocityX": 0.023627997363707604,
+            "velocityY": -0.7499750722442785,
+            "timestamp": 1.9477701585786433
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -0.003,
+            "angularVelocity": 0,
+            "velocityX": -1.899719241029588e-32,
+            "velocityY": -4.254972095731821e-34,
+            "timestamp": 2.037875206997485
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C5.1.traj
new file mode 100644
index 00000000..7f9f6ab6
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C5.1.traj	
@@ -0,0 +1,292 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": -8.010592837331029e-32,
+            "velocityX": 9.396271853657066e-34,
+            "velocityY": -2.4298772375808502e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 5.207063492876026,
+            "y": 6.6289176408913075,
+            "heading": 0.24374543730705361,
+            "angularVelocity": -0.264834788594711,
+            "velocityX": 0.5061159579768882,
+            "velocityY": -0.01763482359684202,
+            "timestamp": 0.06137623678217253
+        },
+        {
+            "x": 5.269220913885746,
+            "y": 6.626580685647671,
+            "heading": 0.2121773997323976,
+            "angularVelocity": -0.514336479877237,
+            "velocityX": 1.0127277961065038,
+            "velocityY": -0.03807589657100854,
+            "timestamp": 0.12275247356434506
+        },
+        {
+            "x": 5.36250981468421,
+            "y": 6.6227475288112485,
+            "heading": 0.16662318019476377,
+            "angularVelocity": -0.7422126530713462,
+            "velocityX": 1.5199514615005028,
+            "velocityY": -0.06245343535846346,
+            "timestamp": 0.1841287103465176
+        },
+        {
+            "x": 5.486975524936631,
+            "y": 6.617054798693029,
+            "heading": 0.10910232843404971,
+            "angularVelocity": -0.9371844019185364,
+            "velocityX": 2.0279136808950478,
+            "velocityY": -0.0927513711605005,
+            "timestamp": 0.24550494712869012
+        },
+        {
+            "x": 5.642662315095453,
+            "y": 6.608892934358269,
+            "heading": 0.04300979987381644,
+            "angularVelocity": -1.076842309423355,
+            "velocityX": 2.536597196588689,
+            "velocityY": -0.1329808532205177,
+            "timestamp": 0.30688118391086266
+        },
+        {
+            "x": 5.829535978248522,
+            "y": 6.597025931717244,
+            "heading": -0.024847542112422006,
+            "angularVelocity": -1.1055963275666998,
+            "velocityX": 3.0447233807490215,
+            "velocityY": -0.19334848897856358,
+            "timestamp": 0.3682574206930352
+        },
+        {
+            "x": 6.046434546533161,
+            "y": 6.5777851666622515,
+            "heading": -0.07267029178585922,
+            "angularVelocity": -0.7791737027338695,
+            "velocityX": 3.533917679808606,
+            "velocityY": -0.31348883645767445,
+            "timestamp": 0.4296336574752077
+        },
+        {
+            "x": 6.274079716361941,
+            "y": 6.532107529947078,
+            "heading": -0.07267036448034916,
+            "angularVelocity": -0.0000011844077407082938,
+            "velocityX": 3.709011528952235,
+            "velocityY": -0.7442234830605138,
+            "timestamp": 0.49100989425738023
+        },
+        {
+            "x": 6.493476634626264,
+            "y": 6.456122214314626,
+            "heading": -0.07267035803800323,
+            "angularVelocity": 1.049648246960038e-7,
+            "velocityX": 3.5746231728571716,
+            "velocityY": -1.238025001469686,
+            "timestamp": 0.5523861310395528
+        },
+        {
+            "x": 6.700610073343003,
+            "y": 6.351219816335689,
+            "heading": -0.07267035070446132,
+            "angularVelocity": 1.1948503655702063e-7,
+            "velocityX": 3.3748149051866063,
+            "velocityY": -1.7091695985083282,
+            "timestamp": 0.6137623678217253
+        },
+        {
+            "x": 6.891689244621814,
+            "y": 6.21932014936447,
+            "heading": -0.07267034149477326,
+            "angularVelocity": 1.5005299351249212e-7,
+            "velocityX": 3.1132435173071933,
+            "velocityY": -2.1490347712151214,
+            "timestamp": 0.6751386046038979
+        },
+        {
+            "x": 7.0632171630859375,
+            "y": 6.06283712387085,
+            "heading": -0.07267032569899272,
+            "angularVelocity": 2.573598736854518e-7,
+            "velocityX": 2.7946959190881038,
+            "velocityY": -2.549570219643589,
+            "timestamp": 0.7365148413860704
+        },
+        {
+            "x": 7.263317671759933,
+            "y": 5.7907628781558955,
+            "heading": -0.07267031513885969,
+            "angularVelocity": 1.182832965996254e-7,
+            "velocityX": 2.24131152203967,
+            "velocityY": -3.0474842158680824,
+            "timestamp": 0.8257931530827405
+        },
+        {
+            "x": 7.406264443711533,
+            "y": 5.484771313458312,
+            "heading": -0.07267030869611313,
+            "angularVelocity": 7.216474459034548e-8,
+            "velocityX": 1.6011365944875051,
+            "velocityY": -3.4273896860551396,
+            "timestamp": 0.9150714647794106
+        },
+        {
+            "x": 7.486522259331217,
+            "y": 5.1567114044359705,
+            "heading": -0.07267030368970051,
+            "angularVelocity": 5.607647052205946e-8,
+            "velocityX": 0.8989620669840581,
+            "velocityY": -3.6745756364317224,
+            "timestamp": 1.0043497764760807
+        },
+        {
+            "x": 7.50149841248159,
+            "y": 4.81930906784098,
+            "heading": -0.07267029905812865,
+            "angularVelocity": 5.187790600157932e-8,
+            "velocityX": 0.1677468230047605,
+            "velocityY": -3.7792195011633143,
+            "timestamp": 1.093628088172751
+        },
+        {
+            "x": 7.516449925771596,
+            "y": 4.481905638472984,
+            "heading": -0.0726702944266145,
+            "angularVelocity": 5.1877259457701335e-8,
+            "velocityX": 0.16747083368699095,
+            "velocityY": -3.7792317412357557,
+            "timestamp": 1.1829063998694211
+        },
+        {
+            "x": 7.531401402222247,
+            "y": 4.144502207472513,
+            "heading": -0.07267028979509418,
+            "angularVelocity": 5.187732873980297e-8,
+            "velocityX": 0.16747042105201052,
+            "velocityY": -3.779231759520992,
+            "timestamp": 1.2721847115660914
+        },
+        {
+            "x": 7.546352847973806,
+            "y": 3.807098775111667,
+            "heading": -0.07267028516356967,
+            "angularVelocity": 5.1877375603275234e-8,
+            "velocityX": 0.16747007719365256,
+            "velocityY": -3.7792317747584647,
+            "timestamp": 1.3614630232627616
+        },
+        {
+            "x": 7.561304268557873,
+            "y": 3.4696953416355707,
+            "heading": -0.07267028053204275,
+            "angularVelocity": 5.1877402813012475e-8,
+            "velocityX": 0.16746979529438435,
+            "velocityY": -3.7792317872503007,
+            "timestamp": 1.4507413349594318
+        },
+        {
+            "x": 7.576255668582546,
+            "y": 3.132291907248425,
+            "heading": -0.07267027590051456,
+            "angularVelocity": 5.1877416880821484e-8,
+            "velocityX": 0.16746956501007867,
+            "velocityY": -3.779231797454896,
+            "timestamp": 1.540019646656102
+        },
+        {
+            "x": 7.591207051730301,
+            "y": 2.794888472113414,
+            "heading": -0.07267027126898577,
+            "angularVelocity": 5.1877423431260937e-8,
+            "velocityX": 0.16746937597291248,
+            "velocityY": -3.779231805831691,
+            "timestamp": 1.6292979583527722
+        },
+        {
+            "x": 7.60615842090353,
+            "y": 2.457485036359151,
+            "heading": -0.07267026663745696,
+            "angularVelocity": 5.1877423851232326e-8,
+            "velocityX": 0.16746921944524512,
+            "velocityY": -3.779231812767884,
+            "timestamp": 1.7185762700494425
+        },
+        {
+            "x": 7.621109778388665,
+            "y": 2.1200816000869556,
+            "heading": -0.07267026200592838,
+            "angularVelocity": 5.187742117887202e-8,
+            "velocityX": 0.1674690885277524,
+            "velocityY": -3.779231818569208,
+            "timestamp": 1.8078545817461127
+        },
+        {
+            "x": 7.636061125621916,
+            "y": 1.7826781633604734,
+            "heading": -0.07267025737439563,
+            "angularVelocity": 5.1877468094637746e-8,
+            "velocityX": 0.16746897369713376,
+            "velocityY": -3.7792318236576405,
+            "timestamp": 1.897132893442783
+        },
+        {
+            "x": 7.6506850878742485,
+            "y": 1.4507667606163608,
+            "heading": -0.05536549896524321,
+            "angularVelocity": 0.19382936438075055,
+            "velocityX": 0.16380195788222543,
+            "velocityY": -3.7177159428351065,
+            "timestamp": 1.9864112051394531
+        },
+        {
+            "x": 7.662404359721407,
+            "y": 1.1852476637582392,
+            "heading": -0.0353858682020255,
+            "angularVelocity": 0.2237904187872895,
+            "velocityX": 0.13126672788097452,
+            "velocityY": -2.974060461181691,
+            "timestamp": 2.0756895168361233
+        },
+        {
+            "x": 7.6711999602966845,
+            "y": 0.9861189087552604,
+            "heading": -0.018423007440779238,
+            "angularVelocity": 0.18999979321832794,
+            "velocityX": 0.0985188945458782,
+            "velocityY": -2.2304269784976873,
+            "timestamp": 2.1649678285327933
+        },
+        {
+            "x": 7.677066105120016,
+            "y": 0.8533716446291538,
+            "heading": -0.006322956528169105,
+            "angularVelocity": 0.13553180702747777,
+            "velocityX": 0.06570626966225444,
+            "velocityY": -1.4868926349898441,
+            "timestamp": 2.2542461402294633
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -2.444948224573009e-31,
+            "angularVelocity": 0.0708229849781762,
+            "velocityX": 0.03286234724008497,
+            "velocityY": -0.7434240563896022,
+            "timestamp": 2.3435244519261333
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 1.883272219480887e-30,
+            "velocityX": 0,
+            "velocityY": -3.281240907044904e-32,
+            "timestamp": 2.4328027636228033
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj
new file mode 100644
index 00000000..462787d1
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": -5.95142511939464e-31,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.1029790507443495,
+            "y": 6.633224483607701,
+            "heading": 0.26650125878935643,
+            "angularVelocity": 0.06942713064122713,
+            "velocityX": -0.7797928290155524,
+            "velocityY": 0.034434353705267144,
+            "timestamp": 0.09364147314336706
+        },
+        {
+            "x": 4.9569371541735245,
+            "y": 6.639673450192742,
+            "heading": 0.2795035687162854,
+            "angularVelocity": 0.13885204376302582,
+            "velocityX": -1.559585637308702,
+            "velocityY": 0.06886870067889261,
+            "timestamp": 0.18728294628673411
+        },
+        {
+            "x": 4.737874314376445,
+            "y": 6.649346898931416,
+            "heading": 0.2990065710588269,
+            "angularVelocity": 0.20827312608258508,
+            "velocityX": -2.339378401936154,
+            "velocityY": 0.1033030388561254,
+            "timestamp": 0.28092441943010116
+        },
+        {
+            "x": 4.445790546320906,
+            "y": 6.66224482863892,
+            "heading": 0.3250097249050781,
+            "angularVelocity": 0.2776884319880837,
+            "velocityX": -3.119171006722006,
+            "velocityY": 0.13773736438080994,
+            "timestamp": 0.37456589257346823
+        },
+        {
+            "x": 4.09189529815297,
+            "y": 6.677872415392626,
+            "heading": 0.32500972789735083,
+            "angularVelocity": 3.195452790688951e-8,
+            "velocityX": -3.7792575905562438,
+            "velocityY": 0.16688745092443205,
+            "timestamp": 0.4682073657168353
+        },
+        {
+            "x": 3.7380000499890724,
+            "y": 6.693500002240241,
+            "heading": 0.32500973088947493,
+            "angularVelocity": 3.1952970747065184e-8,
+            "velocityX": -3.7792575905130557,
+            "velocityY": 0.16688745192729154,
+            "timestamp": 0.5618488388602023
+        },
+        {
+            "x": 3.384104801825175,
+            "y": 6.7091275890878554,
+            "heading": 0.3250097338815991,
+            "angularVelocity": 3.195297127919844e-8,
+            "velocityX": -3.7792575905130557,
+            "velocityY": 0.1668874519272919,
+            "timestamp": 0.6554903120035693
+        },
+        {
+            "x": 3.030209553657211,
+            "y": 6.724755175840924,
+            "heading": 0.32500973687387263,
+            "angularVelocity": 3.195453683514771e-8,
+            "velocityX": -3.7792575905565435,
+            "velocityY": 0.16688745091763021,
+            "timestamp": 0.7491317851469363
+        },
+        {
+            "x": 2.7381257502813003,
+            "y": 6.7376531044804615,
+            "heading": 0.35100602630019134,
+            "angularVelocity": 0.2776151266490911,
+            "velocityX": -3.119171383909283,
+            "velocityY": 0.13773735297596457,
+            "timestamp": 0.8427732582903034
+        },
+        {
+            "x": 2.5190628797280783,
+            "y": 6.747326551971013,
+            "heading": 0.37050304728737704,
+            "angularVelocity": 0.20820925101574836,
+            "velocityX": -2.3393787303818834,
+            "velocityY": 0.10330302552738789,
+            "timestamp": 0.9364147314336704
+        },
+        {
+            "x": 2.3730209608288053,
+            "y": 6.7537755172838185,
+            "heading": 0.3835010053218344,
+            "angularVelocity": 0.13880556977745576,
+            "velocityX": -1.559585875754842,
+            "velocityY": 0.06886868709264785,
+            "timestamp": 1.0300562045770374
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 0.06940295213233785,
+            "velocityX": -0.7797929526056125,
+            "velocityY": 0.03443434418469861,
+            "timestamp": 1.1236976777204044
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": -1.6940949553652625e-30,
+            "velocityX": -1.6586267397188983e-32,
+            "velocityY": 0,
+            "timestamp": 1.2173391508637714
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj
new file mode 100644
index 00000000..53d96783
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 3.363577949177338e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 5.118924970919473,
+            "y": 6.609128397315466,
+            "heading": 0.25122660793754975,
+            "angularVelocity": -0.10272459100864365,
+            "velocityX": -0.6682716305586017,
+            "velocityY": -0.24437832416494054,
+            "timestamp": 0.08540693106008139
+        },
+        {
+            "x": 5.0047749070529814,
+            "y": 6.567385203608531,
+            "heading": 0.23368024533248913,
+            "angularVelocity": -0.20544424658833893,
+            "velocityX": -1.3365433279201937,
+            "velocityY": -0.48875651178205104,
+            "timestamp": 0.17081386212016278
+        },
+        {
+            "x": 4.833549798383878,
+            "y": 6.504770434941582,
+            "heading": 0.20736177398264571,
+            "angularVelocity": -0.30815381167752187,
+            "velocityX": -2.004815142563213,
+            "velocityY": -0.7331345113302418,
+            "timestamp": 0.25622079318024416
+        },
+        {
+            "x": 4.605249631158782,
+            "y": 6.421284110658292,
+            "heading": 0.17227269285622732,
+            "angularVelocity": -0.4108458258702004,
+            "velocityX": -2.6730871182397737,
+            "velocityY": -0.977512284390117,
+            "timestamp": 0.34162772424032556
+        },
+        {
+            "x": 4.319874394275373,
+            "y": 6.316926253811125,
+            "heading": 0.12841549400557886,
+            "angularVelocity": -0.5135086614902031,
+            "velocityX": -3.3413592239095435,
+            "velocityY": -1.2218897875367427,
+            "timestamp": 0.42703465530040696
+        },
+        {
+            "x": 4.016437653324315,
+            "y": 6.205962904892466,
+            "heading": 0.1284154887581796,
+            "angularVelocity": -6.143996998796484e-8,
+            "velocityX": -3.552835082407984,
+            "velocityY": -1.2992311928477889,
+            "timestamp": 0.5124415863604883
+        },
+        {
+            "x": 3.713000912388581,
+            "y": 6.094999555931864,
+            "heading": 0.12841548351080107,
+            "angularVelocity": -6.143972697353832e-8,
+            "velocityX": -3.5528350822285746,
+            "velocityY": -1.299231193338901,
+            "timestamp": 0.5978485174205697
+        },
+        {
+            "x": 3.4095641714528466,
+            "y": 5.98403620697126,
+            "heading": 0.12841547826342262,
+            "angularVelocity": -6.143972595662007e-8,
+            "velocityX": -3.5528350822285746,
+            "velocityY": -1.299231193338901,
+            "timestamp": 0.683255448480651
+        },
+        {
+            "x": 3.106127430499178,
+            "y": 5.873072858059743,
+            "heading": 0.12841547301602332,
+            "angularVelocity": -6.143997022352915e-8,
+            "velocityX": -3.55283508243856,
+            "velocityY": -1.2992311927641775,
+            "timestamp": 0.7686623795407324
+        },
+        {
+            "x": 2.820751661400481,
+            "y": 5.768715245814204,
+            "heading": 0.08460804498823243,
+            "angularVelocity": -0.5129259122655233,
+            "velocityX": -3.3413654554329186,
+            "velocityY": -1.2218869235814813,
+            "timestamp": 0.8540693106008137
+        },
+        {
+            "x": 2.5924510107246115,
+            "y": 5.685229149936981,
+            "heading": 0.04956396734437731,
+            "angularVelocity": -0.41031889577208425,
+            "velocityX": -2.6730927787964465,
+            "velocityY": -0.977509610063062,
+            "timestamp": 0.939476241660895
+        },
+        {
+            "x": 2.4212255084791887,
+            "y": 5.622614575599846,
+            "heading": 0.02328185696555881,
+            "angularVelocity": -0.30772807373595673,
+            "velocityX": -2.00481975081121,
+            "velocityY": -0.7331322359901695,
+            "timestamp": 1.0248831727209764
+        },
+        {
+            "x": 2.307075169405903,
+            "y": 5.580871525201533,
+            "heading": 0.005760683198102726,
+            "angularVelocity": -0.20514931926462068,
+            "velocityX": -1.3365465502206675,
+            "velocityY": -0.48875483383130996,
+            "timestamp": 1.1102901037810577
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -0.003,
+            "angularVelocity": -0.10257578734376759,
+            "velocityX": -0.6682732735795421,
+            "velocityY": -0.24437741694350015,
+            "timestamp": 1.195697034841139
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -0.003,
+            "angularVelocity": 2.4865352537032555e-34,
+            "velocityX": 0,
+            "velocityY": -6.568641934425214e-34,
+            "timestamp": 1.2811039659012204
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.1.traj
new file mode 100644
index 00000000..0a2ace26
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.1.traj	
@@ -0,0 +1,283 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": 1.3458298549409377e-33,
+            "velocityX": 0,
+            "velocityY": 3.5102521636207017e-36,
+            "timestamp": 0
+        },
+        {
+            "x": 5.139424093284861,
+            "y": 6.625864418761083,
+            "heading": 0.2540205855596587,
+            "angularVelocity": -0.08994806739995133,
+            "velocityX": -0.5502097496757357,
+            "velocityY": -0.06221136596695031,
+            "timestamp": 0.06647629696982885
+        },
+        {
+            "x": 5.066272282481334,
+            "y": 6.617593263412388,
+            "heading": 0.24206149463416465,
+            "angularVelocity": -0.17990007672842862,
+            "velocityX": -1.1004194598373498,
+            "velocityY": -0.12442262469054285,
+            "timestamp": 0.1329525939396577
+        },
+        {
+            "x": 4.956544571814103,
+            "y": 6.605186545652003,
+            "heading": 0.22412231737841096,
+            "angularVelocity": -0.26985825133875346,
+            "velocityX": -1.6506291064472642,
+            "velocityY": -0.1866337074403447,
+            "timestamp": 0.19942889090948654
+        },
+        {
+            "x": 4.810240966932036,
+            "y": 6.588644282109891,
+            "heading": 0.2002025750538153,
+            "angularVelocity": -0.35982362759243297,
+            "velocityX": -2.2008386680814915,
+            "velocityY": -0.24884454002634754,
+            "timestamp": 0.2659051878793154
+        },
+        {
+            "x": 4.627361475577568,
+            "y": 6.567966494763936,
+            "heading": 0.17030184062671833,
+            "angularVelocity": -0.4497954276945935,
+            "velocityX": -2.7510481132465947,
+            "velocityY": -0.31105504200002715,
+            "timestamp": 0.3323814848491442
+        },
+        {
+            "x": 4.407906114797318,
+            "y": 6.543153211904946,
+            "heading": 0.13441995513073204,
+            "angularVelocity": -0.5397696191211097,
+            "velocityX": -3.3012573019801734,
+            "velocityY": -0.37326511839627413,
+            "timestamp": 0.398857781818973
+        },
+        {
+            "x": 4.158022417123658,
+            "y": 6.514899899511502,
+            "heading": 0.134419949269586,
+            "angularVelocity": -8.816896134746106e-8,
+            "velocityX": -3.758989430278769,
+            "velocityY": -0.4250133307856765,
+            "timestamp": 0.46533407878880184
+        },
+        {
+            "x": 3.908138721969202,
+            "y": 6.486646564834816,
+            "heading": 0.13441994340875443,
+            "angularVelocity": -8.816423004519242e-8,
+            "velocityX": -3.758989392382515,
+            "velocityY": -0.42501366599148993,
+            "timestamp": 0.5318103757586307
+        },
+        {
+            "x": 3.6582550295868868,
+            "y": 6.45839320564021,
+            "heading": 0.1344199375479281,
+            "angularVelocity": -8.816415176708587e-8,
+            "velocityX": -3.7589893506813095,
+            "velocityY": -0.42501403481347844,
+            "timestamp": 0.5982866727284595
+        },
+        {
+            "x": 3.4083713401101514,
+            "y": 6.430139820747532,
+            "heading": 0.13441993168710784,
+            "angularVelocity": -8.816406061478911e-8,
+            "velocityX": -3.758989306972801,
+            "velocityY": -0.42501442138844536,
+            "timestamp": 0.6647629696982883
+        },
+        {
+            "x": 3.1584877665624913,
+            "y": 6.401885410552911,
+            "heading": 0.1344199258262894,
+            "angularVelocity": -8.816403354486871e-8,
+            "velocityX": -3.7589875630568486,
+            "velocityY": -0.4250298449602976,
+            "timestamp": 0.7312392666681171
+        },
+        {
+            "x": 2.9135491847991943,
+            "y": 6.344918727874756,
+            "heading": 0.13441991981204132,
+            "angularVelocity": -9.047206787345137e-8,
+            "velocityX": -3.684600269994962,
+            "velocityY": -0.8569472921154051,
+            "timestamp": 0.7977155636379459
+        },
+        {
+            "x": 2.741853960900858,
+            "y": 6.285250221506208,
+            "heading": 0.13441991359516897,
+            "angularVelocity": -1.293850671116252e-7,
+            "velocityX": -3.573307735472737,
+            "velocityY": -1.2418163448570063,
+            "timestamp": 0.8457649443822611
+        },
+        {
+            "x": 2.5774323687799447,
+            "y": 6.207758184927692,
+            "heading": 0.1344190973329971,
+            "angularVelocity": -0.000016987985264431854,
+            "velocityX": -3.4219294728447913,
+            "velocityY": -1.6127582786315857,
+            "timestamp": 0.8938143251265763
+        },
+        {
+            "x": 2.4266376481277105,
+            "y": 6.11739996688274,
+            "heading": 0.11881021575379135,
+            "angularVelocity": -0.32485083756365457,
+            "velocityX": -3.1383280765814066,
+            "velocityY": -1.8805282533353447,
+            "timestamp": 0.9418637058708915
+        },
+        {
+            "x": 2.292578699649266,
+            "y": 6.017904789932588,
+            "heading": 0.09375834131328029,
+            "angularVelocity": -0.5213776754755571,
+            "velocityX": -2.7900244790211017,
+            "velocityY": -2.0706859361954097,
+            "timestamp": 0.9899130866152067
+        },
+        {
+            "x": 2.176263979579871,
+            "y": 5.911118232592638,
+            "heading": 0.0629895244075883,
+            "angularVelocity": -0.6403582403990148,
+            "velocityX": -2.420732968200766,
+            "velocityY": -2.2224335815729503,
+            "timestamp": 1.037962467359522
+        },
+        {
+            "x": 2.0781210346390075,
+            "y": 5.798061969403822,
+            "heading": 0.02853978404947447,
+            "angularVelocity": -0.7169653349214,
+            "velocityX": -2.0425433880846535,
+            "velocityY": -2.3529182153339443,
+            "timestamp": 1.0860118481038372
+        },
+        {
+            "x": 1.9983746548109367,
+            "y": 5.679376225369417,
+            "heading": -0.008333608258354223,
+            "angularVelocity": -0.7674061920598474,
+            "velocityX": -1.659675496182245,
+            "velocityY": -2.470078535787315,
+            "timestamp": 1.1340612288481524
+        },
+        {
+            "x": 1.9371600151062012,
+            "y": 5.555497646331787,
+            "heading": -0.04678243253848999,
+            "angularVelocity": -0.8001939605576434,
+            "velocityX": -1.273994352403345,
+            "velocityY": -2.5781514167024167,
+            "timestamp": 1.1821106095924676
+        },
+        {
+            "x": 1.8879136197731217,
+            "y": 5.371859224657917,
+            "heading": -0.10269858839018778,
+            "angularVelocity": -0.8284337962966012,
+            "velocityX": -0.7296170063605633,
+            "velocityY": -2.720721274486196,
+            "timestamp": 1.2496068369333404
+        },
+        {
+            "x": 1.8760341540552343,
+            "y": 5.181395172179021,
+            "heading": -0.15967327513417093,
+            "angularVelocity": -0.8441166119736836,
+            "velocityX": -0.17600192167619955,
+            "velocityY": -2.821847382921196,
+            "timestamp": 1.3171030642742134
+        },
+        {
+            "x": 1.9020919030528052,
+            "y": 4.989002664772072,
+            "heading": -0.21623702759974286,
+            "angularVelocity": -0.8380283564577732,
+            "velocityX": 0.3860623033932351,
+            "velocityY": -2.8504186824445474,
+            "timestamp": 1.3845992916150864
+        },
+        {
+            "x": 1.9652989688718845,
+            "y": 4.8044863939922715,
+            "heading": -0.26949747833980514,
+            "angularVelocity": -0.7890878177691865,
+            "velocityX": 0.9364533146403533,
+            "velocityY": -2.7337271733418085,
+            "timestamp": 1.4520955189559595
+        },
+        {
+            "x": 2.0553904334698565,
+            "y": 4.6466969161112965,
+            "heading": -0.31411027699435934,
+            "angularVelocity": -0.6609672927236123,
+            "velocityX": 1.3347629659801235,
+            "velocityY": -2.337752554436838,
+            "timestamp": 1.5195917462968325
+        },
+        {
+            "x": 2.145781200762326,
+            "y": 4.526760098072049,
+            "heading": -0.34739291091529667,
+            "angularVelocity": -0.49310361826375293,
+            "velocityX": 1.3391973278146792,
+            "velocityY": -1.7769410641803345,
+            "timestamp": 1.5870879736377055
+        },
+        {
+            "x": 2.220455144634577,
+            "y": 4.441296194130274,
+            "heading": -0.37087004938287466,
+            "angularVelocity": -0.3478288993696278,
+            "velocityX": 1.1063424848196184,
+            "velocityY": -1.2662026798943848,
+            "timestamp": 1.6545842009785785
+        },
+        {
+            "x": 2.2728997785605394,
+            "y": 4.3865419949716715,
+            "heading": -0.3858025738529217,
+            "angularVelocity": -0.22123494983851594,
+            "velocityX": 0.7770009671963372,
+            "velocityY": -0.8112186608901896,
+            "timestamp": 1.7220804283194515
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.393,
+            "angularVelocity": -0.1066344954471229,
+            "velocityX": 0.4015072028040527,
+            "velocityY": -0.3932367188113757,
+            "timestamp": 1.7895766556603245
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.393,
+            "angularVelocity": 8.903141052781706e-32,
+            "velocityX": 0,
+            "velocityY": -2.026142404451774e-33,
+            "timestamp": 1.8570728830011975
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.traj
index 359cb1db..0a2ace26 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.traj	
@@ -4,172 +4,280 @@
             "x": 5.176,
             "y": 6.63,
             "heading": 0.26,
-            "angularVelocity": 2.521971410376041e-31,
-            "velocityX": -3.46085485178236e-32,
-            "velocityY": -2.8734015412732535e-37,
+            "angularVelocity": 1.3458298549409377e-33,
+            "velocityX": 0,
+            "velocityY": 3.5102521636207017e-36,
             "timestamp": 0
         },
         {
-            "x": 5.132269964046874,
-            "y": 6.606195610268081,
-            "heading": 0.16914444930535336,
-            "angularVelocity": -1.1148212857556328,
-            "velocityX": -0.5365789380469493,
-            "velocityY": -0.29208606589986635,
-            "timestamp": 0.08149786145594107
-        },
-        {
-            "x": 5.043666795437562,
-            "y": 6.5570487224925476,
-            "heading": 0.005335724612659364,
-            "angularVelocity": -2.0099757437347154,
-            "velocityX": -1.0871839705537814,
-            "velocityY": -0.6030451216453527,
-            "timestamp": 0.16299572291188213
-        },
-        {
-            "x": 4.909391168326261,
-            "y": 6.4806746643853055,
-            "heading": -0.2131663273161798,
-            "angularVelocity": -2.681077123072297,
-            "velocityX": -1.647596939508567,
-            "velocityY": -0.9371295975481627,
-            "timestamp": 0.2444935843678232
-        },
-        {
-            "x": 4.728031239930722,
-            "y": 6.375889843725219,
-            "heading": -0.4550458803770597,
-            "angularVelocity": -2.967925154596154,
-            "velocityX": -2.225333587355345,
-            "velocityY": -1.2857370584715861,
-            "timestamp": 0.32599144582376427
-        },
-        {
-            "x": 4.50844214888531,
-            "y": 6.243100931440797,
-            "heading": -0.5880935326141025,
-            "angularVelocity": -1.6325293677671568,
-            "velocityX": -2.6944153763362864,
-            "velocityY": -1.6293545611158133,
-            "timestamp": 0.40748930727970534
-        },
-        {
-            "x": 4.251218650398233,
-            "y": 6.083818287155582,
-            "heading": -0.6031902279140122,
-            "angularVelocity": -0.1852403858237583,
-            "velocityX": -3.15619936390767,
-            "velocityY": -1.9544395575499292,
-            "timestamp": 0.4889871687356464
-        },
-        {
-            "x": 3.9855391766757995,
-            "y": 5.92740773635831,
-            "heading": -0.6031903133870022,
-            "angularVelocity": -0.0000010487758630839914,
-            "velocityX": -3.259956383837921,
-            "velocityY": -1.9191982219291623,
-            "timestamp": 0.5704850301915875
-        },
-        {
-            "x": 3.719859695364488,
-            "y": 5.770997198451498,
-            "heading": -0.603190398859952,
-            "angularVelocity": -0.0000010487753700599598,
-            "velocityX": -3.259956476955436,
-            "velocityY": -1.919198063759863,
-            "timestamp": 0.6519828916475285
-        },
-        {
-            "x": 3.4541802321431443,
-            "y": 5.614586629816999,
-            "heading": -0.6031904843329001,
-            "angularVelocity": -0.000001048775349610677,
-            "velocityX": -3.259956254986808,
-            "velocityY": -1.9191984407966034,
-            "timestamp": 0.7334807531034696
-        },
-        {
-            "x": 3.1885009063173535,
-            "y": 5.458175827802239,
-            "heading": -0.6031905698058837,
-            "angularVelocity": -0.000001048775783641969,
-            "velocityX": -3.259954569107567,
-            "velocityY": -1.9192013044332228,
-            "timestamp": 0.8149786145594107
-        },
-        {
-            "x": 2.937386380849553,
-            "y": 5.279314604480798,
-            "heading": -0.603190661977047,
-            "angularVelocity": -0.0000011309641953921016,
-            "velocityX": -3.0812406728433848,
-            "velocityY": -2.1946738248847923,
-            "timestamp": 0.8964764760153517
-        },
-        {
-            "x": 2.722322578778117,
-            "y": 5.058413395221267,
-            "heading": -0.6031910253459357,
-            "angularVelocity": -0.00000445863096683889,
-            "velocityX": -2.6388889012468426,
-            "velocityY": -2.710515408787179,
-            "timestamp": 0.9779743374712928
-        },
-        {
-            "x": 2.561151389441887,
-            "y": 4.8410288515668665,
-            "heading": -0.5595876383179096,
-            "angularVelocity": 0.5350249227287813,
-            "velocityX": -1.9776124975176395,
-            "velocityY": -2.667365005300435,
-            "timestamp": 1.0594721989272338
-        },
-        {
-            "x": 2.4444023928247787,
-            "y": 4.656624455699537,
-            "heading": -0.5072145719014195,
-            "angularVelocity": 0.6426311743750899,
-            "velocityX": -1.432540615562337,
-            "velocityY": -2.262689996681954,
-            "timestamp": 1.1409700603831747
-        },
-        {
-            "x": 2.367465069003143,
-            "y": 4.510717874734717,
-            "heading": -0.4552059179368463,
-            "angularVelocity": 0.6381597386170672,
-            "velocityX": -0.9440410146618312,
-            "velocityY": -1.7903117745451358,
-            "timestamp": 1.2224679218391157
-        },
-        {
-            "x": 2.321242179529733,
-            "y": 4.410775659264178,
-            "heading": -0.4150276653385325,
-            "angularVelocity": 0.49299763061923907,
-            "velocityX": -0.5671669004271778,
-            "velocityY": -1.2263170307182782,
-            "timestamp": 1.3039657832950566
+            "x": 5.139424093284861,
+            "y": 6.625864418761083,
+            "heading": 0.2540205855596587,
+            "angularVelocity": -0.08994806739995133,
+            "velocityX": -0.5502097496757357,
+            "velocityY": -0.06221136596695031,
+            "timestamp": 0.06647629696982885
+        },
+        {
+            "x": 5.066272282481334,
+            "y": 6.617593263412388,
+            "heading": 0.24206149463416465,
+            "angularVelocity": -0.17990007672842862,
+            "velocityX": -1.1004194598373498,
+            "velocityY": -0.12442262469054285,
+            "timestamp": 0.1329525939396577
+        },
+        {
+            "x": 4.956544571814103,
+            "y": 6.605186545652003,
+            "heading": 0.22412231737841096,
+            "angularVelocity": -0.26985825133875346,
+            "velocityX": -1.6506291064472642,
+            "velocityY": -0.1866337074403447,
+            "timestamp": 0.19942889090948654
+        },
+        {
+            "x": 4.810240966932036,
+            "y": 6.588644282109891,
+            "heading": 0.2002025750538153,
+            "angularVelocity": -0.35982362759243297,
+            "velocityX": -2.2008386680814915,
+            "velocityY": -0.24884454002634754,
+            "timestamp": 0.2659051878793154
+        },
+        {
+            "x": 4.627361475577568,
+            "y": 6.567966494763936,
+            "heading": 0.17030184062671833,
+            "angularVelocity": -0.4497954276945935,
+            "velocityX": -2.7510481132465947,
+            "velocityY": -0.31105504200002715,
+            "timestamp": 0.3323814848491442
+        },
+        {
+            "x": 4.407906114797318,
+            "y": 6.543153211904946,
+            "heading": 0.13441995513073204,
+            "angularVelocity": -0.5397696191211097,
+            "velocityX": -3.3012573019801734,
+            "velocityY": -0.37326511839627413,
+            "timestamp": 0.398857781818973
+        },
+        {
+            "x": 4.158022417123658,
+            "y": 6.514899899511502,
+            "heading": 0.134419949269586,
+            "angularVelocity": -8.816896134746106e-8,
+            "velocityX": -3.758989430278769,
+            "velocityY": -0.4250133307856765,
+            "timestamp": 0.46533407878880184
+        },
+        {
+            "x": 3.908138721969202,
+            "y": 6.486646564834816,
+            "heading": 0.13441994340875443,
+            "angularVelocity": -8.816423004519242e-8,
+            "velocityX": -3.758989392382515,
+            "velocityY": -0.42501366599148993,
+            "timestamp": 0.5318103757586307
+        },
+        {
+            "x": 3.6582550295868868,
+            "y": 6.45839320564021,
+            "heading": 0.1344199375479281,
+            "angularVelocity": -8.816415176708587e-8,
+            "velocityX": -3.7589893506813095,
+            "velocityY": -0.42501403481347844,
+            "timestamp": 0.5982866727284595
+        },
+        {
+            "x": 3.4083713401101514,
+            "y": 6.430139820747532,
+            "heading": 0.13441993168710784,
+            "angularVelocity": -8.816406061478911e-8,
+            "velocityX": -3.758989306972801,
+            "velocityY": -0.42501442138844536,
+            "timestamp": 0.6647629696982883
+        },
+        {
+            "x": 3.1584877665624913,
+            "y": 6.401885410552911,
+            "heading": 0.1344199258262894,
+            "angularVelocity": -8.816403354486871e-8,
+            "velocityX": -3.7589875630568486,
+            "velocityY": -0.4250298449602976,
+            "timestamp": 0.7312392666681171
+        },
+        {
+            "x": 2.9135491847991943,
+            "y": 6.344918727874756,
+            "heading": 0.13441991981204132,
+            "angularVelocity": -9.047206787345137e-8,
+            "velocityX": -3.684600269994962,
+            "velocityY": -0.8569472921154051,
+            "timestamp": 0.7977155636379459
+        },
+        {
+            "x": 2.741853960900858,
+            "y": 6.285250221506208,
+            "heading": 0.13441991359516897,
+            "angularVelocity": -1.293850671116252e-7,
+            "velocityX": -3.573307735472737,
+            "velocityY": -1.2418163448570063,
+            "timestamp": 0.8457649443822611
+        },
+        {
+            "x": 2.5774323687799447,
+            "y": 6.207758184927692,
+            "heading": 0.1344190973329971,
+            "angularVelocity": -0.000016987985264431854,
+            "velocityX": -3.4219294728447913,
+            "velocityY": -1.6127582786315857,
+            "timestamp": 0.8938143251265763
+        },
+        {
+            "x": 2.4266376481277105,
+            "y": 6.11739996688274,
+            "heading": 0.11881021575379135,
+            "angularVelocity": -0.32485083756365457,
+            "velocityX": -3.1383280765814066,
+            "velocityY": -1.8805282533353447,
+            "timestamp": 0.9418637058708915
+        },
+        {
+            "x": 2.292578699649266,
+            "y": 6.017904789932588,
+            "heading": 0.09375834131328029,
+            "angularVelocity": -0.5213776754755571,
+            "velocityX": -2.7900244790211017,
+            "velocityY": -2.0706859361954097,
+            "timestamp": 0.9899130866152067
+        },
+        {
+            "x": 2.176263979579871,
+            "y": 5.911118232592638,
+            "heading": 0.0629895244075883,
+            "angularVelocity": -0.6403582403990148,
+            "velocityX": -2.420732968200766,
+            "velocityY": -2.2224335815729503,
+            "timestamp": 1.037962467359522
+        },
+        {
+            "x": 2.0781210346390075,
+            "y": 5.798061969403822,
+            "heading": 0.02853978404947447,
+            "angularVelocity": -0.7169653349214,
+            "velocityX": -2.0425433880846535,
+            "velocityY": -2.3529182153339443,
+            "timestamp": 1.0860118481038372
+        },
+        {
+            "x": 1.9983746548109367,
+            "y": 5.679376225369417,
+            "heading": -0.008333608258354223,
+            "angularVelocity": -0.7674061920598474,
+            "velocityX": -1.659675496182245,
+            "velocityY": -2.470078535787315,
+            "timestamp": 1.1340612288481524
+        },
+        {
+            "x": 1.9371600151062012,
+            "y": 5.555497646331787,
+            "heading": -0.04678243253848999,
+            "angularVelocity": -0.8001939605576434,
+            "velocityX": -1.273994352403345,
+            "velocityY": -2.5781514167024167,
+            "timestamp": 1.1821106095924676
+        },
+        {
+            "x": 1.8879136197731217,
+            "y": 5.371859224657917,
+            "heading": -0.10269858839018778,
+            "angularVelocity": -0.8284337962966012,
+            "velocityX": -0.7296170063605633,
+            "velocityY": -2.720721274486196,
+            "timestamp": 1.2496068369333404
+        },
+        {
+            "x": 1.8760341540552343,
+            "y": 5.181395172179021,
+            "heading": -0.15967327513417093,
+            "angularVelocity": -0.8441166119736836,
+            "velocityX": -0.17600192167619955,
+            "velocityY": -2.821847382921196,
+            "timestamp": 1.3171030642742134
+        },
+        {
+            "x": 1.9020919030528052,
+            "y": 4.989002664772072,
+            "heading": -0.21623702759974286,
+            "angularVelocity": -0.8380283564577732,
+            "velocityX": 0.3860623033932351,
+            "velocityY": -2.8504186824445474,
+            "timestamp": 1.3845992916150864
+        },
+        {
+            "x": 1.9652989688718845,
+            "y": 4.8044863939922715,
+            "heading": -0.26949747833980514,
+            "angularVelocity": -0.7890878177691865,
+            "velocityX": 0.9364533146403533,
+            "velocityY": -2.7337271733418085,
+            "timestamp": 1.4520955189559595
+        },
+        {
+            "x": 2.0553904334698565,
+            "y": 4.6466969161112965,
+            "heading": -0.31411027699435934,
+            "angularVelocity": -0.6609672927236123,
+            "velocityX": 1.3347629659801235,
+            "velocityY": -2.337752554436838,
+            "timestamp": 1.5195917462968325
+        },
+        {
+            "x": 2.145781200762326,
+            "y": 4.526760098072049,
+            "heading": -0.34739291091529667,
+            "angularVelocity": -0.49310361826375293,
+            "velocityX": 1.3391973278146792,
+            "velocityY": -1.7769410641803345,
+            "timestamp": 1.5870879736377055
+        },
+        {
+            "x": 2.220455144634577,
+            "y": 4.441296194130274,
+            "heading": -0.37087004938287466,
+            "angularVelocity": -0.3478288993696278,
+            "velocityX": 1.1063424848196184,
+            "velocityY": -1.2662026798943848,
+            "timestamp": 1.6545842009785785
+        },
+        {
+            "x": 2.2728997785605394,
+            "y": 4.3865419949716715,
+            "heading": -0.3858025738529217,
+            "angularVelocity": -0.22123494983851594,
+            "velocityX": 0.7770009671963372,
+            "velocityY": -0.8112186608901896,
+            "timestamp": 1.7220804283194515
         },
         {
             "x": 2.3,
             "y": 4.36,
             "heading": -0.393,
-            "angularVelocity": 0.27028519454392347,
-            "velocityX": -0.26064707895700284,
-            "velocityY": -0.6230305723006956,
-            "timestamp": 1.3854636447509976
+            "angularVelocity": -0.1066344954471229,
+            "velocityX": 0.4015072028040527,
+            "velocityY": -0.3932367188113757,
+            "timestamp": 1.7895766556603245
         },
         {
             "x": 2.3,
             "y": 4.36,
             "heading": -0.393,
-            "angularVelocity": -9.678618240747916e-31,
+            "angularVelocity": 8.903141052781706e-32,
             "velocityX": 0,
-            "velocityY": -3.108003047332484e-31,
-            "timestamp": 1.4669615062069385
+            "velocityY": -2.026142404451774e-33,
+            "timestamp": 1.8570728830011975
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.1.traj
new file mode 100644
index 00000000..29c2d4bc
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 5.380258045140596e-28,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.304265282414897,
+            "y": 5.638698322322049,
+            "heading": -0.04261698283734242,
+            "angularVelocity": -0.41098530784979936,
+            "velocityX": 0.3979499173257931,
+            "velocityY": 0.7589428456312429,
+            "timestamp": 0.10369466261532896
+        },
+        {
+            "x": 4.38866009880019,
+            "y": 5.795292929312062,
+            "heading": -0.1228057028496872,
+            "angularVelocity": -0.7733157907057984,
+            "velocityX": 0.8138781134605525,
+            "velocityY": 1.510151082422871,
+            "timestamp": 0.20738932523065792
+        },
+        {
+            "x": 4.519821720835651,
+            "y": 6.028001091828693,
+            "heading": -0.23019023538729733,
+            "angularVelocity": -1.0355839908170448,
+            "velocityX": 1.264883058851596,
+            "velocityY": 2.2441672179396264,
+            "timestamp": 0.3110839878459869
+        },
+        {
+            "x": 4.707676475524912,
+            "y": 6.330157785456564,
+            "heading": -0.33387571773962255,
+            "angularVelocity": -0.9999114683169592,
+            "velocityX": 1.8116145031122473,
+            "velocityY": 2.9139078715050806,
+            "timestamp": 0.41477865046131585
+        },
+        {
+            "x": 4.978690211282069,
+            "y": 6.613755936139097,
+            "heading": -0.3338757185080882,
+            "angularVelocity": -7.410850442524386e-9,
+            "velocityX": 2.6135745941189206,
+            "velocityY": 2.7349348898948014,
+            "timestamp": 0.5184733130766448
+        },
+        {
+            "x": 5.307019127730767,
+            "y": 6.828410259978494,
+            "heading": -0.3338757060420003,
+            "angularVelocity": 1.2021918642580205e-7,
+            "velocityX": 3.166304881734213,
+            "velocityY": 2.070061451819255,
+            "timestamp": 0.6221679756919738
+        },
+        {
+            "x": 5.675511606586513,
+            "y": 6.962908031631864,
+            "heading": -0.3338756959696621,
+            "angularVelocity": 9.713458668099172e-8,
+            "velocityX": 3.5536301441350284,
+            "velocityY": 1.2970558779125847,
+            "timestamp": 0.7258626383073028
+        },
+        {
+            "x": 6.055844888125957,
+            "y": 7.058944236891981,
+            "heading": -0.3338756861984713,
+            "angularVelocity": 9.423041207795053e-8,
+            "velocityX": 3.6678192680981745,
+            "velocityY": 0.9261441508940896,
+            "timestamp": 0.8295573009226318
+        },
+        {
+            "x": 6.436176952400414,
+            "y": 7.154985262770466,
+            "heading": -0.3338756764272739,
+            "angularVelocity": 9.423047569354886e-8,
+            "velocityX": 3.667807529162388,
+            "velocityY": 0.9261906394812687,
+            "timestamp": 0.9332519635379608
+        },
+        {
+            "x": 6.8165090646020285,
+            "y": 7.2510260988513195,
+            "heading": -0.33387566665551793,
+            "angularVelocity": 9.423586238606553e-8,
+            "velocityX": 3.6678079913574164,
+            "velocityY": 0.9261888091302356,
+            "timestamp": 1.0369466261532898
+        },
+        {
+            "x": 7.162467192455402,
+            "y": 7.33732437680055,
+            "heading": -0.24283297058924916,
+            "angularVelocity": 0.8779882567727307,
+            "velocityX": 3.336315670718354,
+            "velocityY": 0.8322345217454595,
+            "timestamp": 1.1406412887686188
+        },
+        {
+            "x": 7.421550289934561,
+            "y": 7.4021983509049765,
+            "heading": -0.1315877257451376,
+            "angularVelocity": 1.0728155339758783,
+            "velocityX": 2.4985191228237835,
+            "velocityY": 0.6256250077700213,
+            "timestamp": 1.2443359513839478
+        },
+        {
+            "x": 7.593930847137365,
+            "y": 7.445412549605325,
+            "heading": -0.046073342510862675,
+            "angularVelocity": 0.8246748779298502,
+            "velocityX": 1.662386017323544,
+            "velocityY": 0.41674467721309977,
+            "timestamp": 1.3480306139992768
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0.44431739637147044,
+            "velocityX": 0.8300249086292989,
+            "velocityY": 0.20818285001567757,
+            "timestamp": 1.4517252766146058
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 6.677691966309128e-30,
+            "velocityY": 0,
+            "timestamp": 1.5554199392299348
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2.1.traj
new file mode 100644
index 00000000..05a18e04
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2.1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.272659932573731,
+            "y": 5.605321030989923,
+            "heading": -5.269773313916661e-8,
+            "angularVelocity": -7.069117507058631e-7,
+            "velocityX": 0.1295790656325301,
+            "velocityY": 0.6079397351586845,
+            "timestamp": 0.07454855863477708
+        },
+        {
+            "x": 4.294668340488188,
+            "y": 5.695305527660525,
+            "heading": -1.390931476532318e-7,
+            "angularVelocity": -0.0000011589489187991593,
+            "velocityX": 0.2952224471421504,
+            "velocityY": 1.2070588395619357,
+            "timestamp": 0.14909711726955416
+        },
+        {
+            "x": 4.333068049168991,
+            "y": 5.8286332482919585,
+            "heading": -2.3393980900082764e-7,
+            "angularVelocity": -0.0000012723197345504802,
+            "velocityX": 0.5150965944736963,
+            "velocityY": 1.7884681217524945,
+            "timestamp": 0.22364567590433124
+        },
+        {
+            "x": 4.394361213548298,
+            "y": 6.0022499148295365,
+            "heading": -3.0377830712505715e-7,
+            "angularVelocity": -9.368513338342214e-7,
+            "velocityX": 0.8221911395208825,
+            "velocityY": 2.3289070872699873,
+            "timestamp": 0.2981942345391083
+        },
+        {
+            "x": 4.489214393007107,
+            "y": 6.207820209268236,
+            "heading": -3.104589480689307e-7,
+            "angularVelocity": -8.962358439949817e-8,
+            "velocityX": 1.2723677183517785,
+            "velocityY": 2.75753546703125,
+            "timestamp": 0.37274279317388537
+        },
+        {
+            "x": 4.629403012529928,
+            "y": 6.42298249437822,
+            "heading": -2.4199647648725644e-7,
+            "angularVelocity": 9.18393943226279e-7,
+            "velocityX": 1.880500737963715,
+            "velocityY": 2.8862031546566858,
+            "timestamp": 0.4472913518086624
+        },
+        {
+            "x": 4.811531801184116,
+            "y": 6.618438666310206,
+            "heading": -1.472447710538824e-7,
+            "angularVelocity": 0.0000012710752897138218,
+            "velocityX": 2.443089336474201,
+            "velocityY": 2.6218638627509074,
+            "timestamp": 0.5218399104434395
+        },
+        {
+            "x": 5.024847227590037,
+            "y": 6.779620899070013,
+            "heading": -6.254572131926062e-8,
+            "angularVelocity": 0.000001136235548213432,
+            "velocityX": 2.861429252247402,
+            "velocityY": 2.1621106520395927,
+            "timestamp": 0.5963884690782165
+        },
+        {
+            "x": 5.261847496032715,
+            "y": 6.900974273681641,
+            "heading": 1.6289151195668303e-9,
+            "angularVelocity": 8.609140934997053e-7,
+            "velocityX": 3.1791395133448552,
+            "velocityY": 1.6278433384167708,
+            "timestamp": 0.6709370277129936
+        },
+        {
+            "x": 5.5631639462020495,
+            "y": 6.984947323321856,
+            "heading": 4.965855378058324e-8,
+            "angularVelocity": 5.539886921154719e-7,
+            "velocityX": 3.4757171947403775,
+            "velocityY": 0.9686380297960585,
+            "timestamp": 0.7576289054784
+        },
+        {
+            "x": 5.883696640724643,
+            "y": 7.009274337947639,
+            "heading": 7.188189606013307e-8,
+            "angularVelocity": 2.563216327096045e-7,
+            "velocityX": 3.697378610459799,
+            "velocityY": 0.2806146929817695,
+            "timestamp": 0.8443207832438064
+        },
+        {
+            "x": 6.209423466668327,
+            "y": 6.971151903870608,
+            "heading": 7.193737396357795e-8,
+            "angularVelocity": 6.438452889791163e-10,
+            "velocityX": 3.7572934667332714,
+            "velocityY": -0.4397463179458525,
+            "timestamp": 0.9310126610092129
+        },
+        {
+            "x": 6.521952745942683,
+            "y": 6.871769894938554,
+            "heading": 7.193784305655878e-8,
+            "angularVelocity": 5.630908705768223e-12,
+            "velocityX": 3.6050583669434406,
+            "velocityY": -1.1463820079014435,
+            "timestamp": 1.0177045387746193
+        },
+        {
+            "x": 6.809873374118548,
+            "y": 6.714756988695251,
+            "heading": 7.193831945924844e-8,
+            "angularVelocity": 5.91084758847972e-12,
+            "velocityX": 3.321194967875227,
+            "velocityY": -1.8111605177424126,
+            "timestamp": 1.1043964165400257
+        },
+        {
+            "x": 7.062672913559571,
+            "y": 6.505846154651799,
+            "heading": 7.193893641048392e-8,
+            "angularVelocity": 7.48396872358976e-12,
+            "velocityX": 2.9160694861627596,
+            "velocityY": -2.4098086166543093,
+            "timestamp": 1.1910882943054322
+        },
+        {
+            "x": 7.268448732261103,
+            "y": 6.269564240975314,
+            "heading": 6.958123303548702e-8,
+            "angularVelocity": -2.7156604096657526e-8,
+            "velocityX": 2.3736458816624144,
+            "velocityY": -2.7255369226670942,
+            "timestamp": 1.2777801720708386
+        },
+        {
+            "x": 7.433069281193563,
+            "y": 6.080538590855167,
+            "heading": 5.4913746267249155e-8,
+            "angularVelocity": -1.6915879682512025e-7,
+            "velocityX": 1.8989154825033356,
+            "velocityY": -2.180430912690726,
+            "timestamp": 1.364472049836245
+        },
+        {
+            "x": 7.556534655605292,
+            "y": 5.9387693118426395,
+            "heading": 3.4027865091828515e-8,
+            "angularVelocity": -2.408962130800618e-7,
+            "velocityX": 1.424186181835385,
+            "velocityY": -1.635323662267605,
+            "timestamp": 1.4511639276016515
+        },
+        {
+            "x": 7.638844889158882,
+            "y": 5.8442564415645775,
+            "heading": 1.3493455787871544e-8,
+            "angularVelocity": -2.368497714278514e-7,
+            "velocityX": 0.9494572693998051,
+            "velocityY": -1.0902159778725726,
+            "timestamp": 1.537855805367058
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -2.563219583366834e-29,
+            "angularVelocity": -1.5563981959470815e-7,
+            "velocityX": 0.4747285662416774,
+            "velocityY": -0.5451080630467725,
+            "timestamp": 1.6245476831324643
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 4.328374921762355e-28,
+            "velocityX": 0,
+            "velocityY": 6.636862452110489e-29,
+            "timestamp": 1.7112395608978708
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.1.traj
new file mode 100644
index 00000000..2b663e69
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": -9.522541691773037e-34,
+            "velocityX": 0,
+            "velocityY": -4.2308696063129985e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 4.271262994735721,
+            "y": 5.464085078742271,
+            "heading": 0.013066573413208545,
+            "angularVelocity": 0.12156077000957308,
+            "velocityX": 0.0768721814736735,
+            "velocityY": -0.8923144052220247,
+            "timestamp": 0.10749005137249076
+        },
+        {
+            "x": 4.301424317332477,
+            "y": 5.27437671657762,
+            "heading": 0.033784324267673776,
+            "angularVelocity": 0.192741101059399,
+            "velocityX": 0.2805964106597774,
+            "velocityY": -1.7648922829820315,
+            "timestamp": 0.21498010274498153
+        },
+        {
+            "x": 4.379013311524821,
+            "y": 5.000817118183451,
+            "heading": 0.051063906614390905,
+            "angularVelocity": 0.16075517804747627,
+            "velocityX": 0.7218248870629933,
+            "velocityY": -2.544975975927198,
+            "timestamp": 0.32247015411747226
+        },
+        {
+            "x": 4.5426840546799285,
+            "y": 4.684552935799795,
+            "heading": 0.043724640930798246,
+            "angularVelocity": -0.06827855778168279,
+            "velocityX": 1.5226594560638123,
+            "velocityY": -2.9422646872470954,
+            "timestamp": 0.42996020548996305
+        },
+        {
+            "x": 4.7987371937718075,
+            "y": 4.3933637164950845,
+            "heading": -0.0025480008668042047,
+            "angularVelocity": -0.4304830187237655,
+            "velocityX": 2.382110119238521,
+            "velocityY": -2.7089876280330105,
+            "timestamp": 0.5374502568624538
+        },
+        {
+            "x": 5.129552682758233,
+            "y": 4.161939062753506,
+            "heading": -0.00976861092561694,
+            "angularVelocity": -0.06717468236935505,
+            "velocityX": 3.0776382070935524,
+            "velocityY": -2.152986725623672,
+            "timestamp": 0.6449403082349446
+        },
+        {
+            "x": 5.508331065778466,
+            "y": 4.0140416634822165,
+            "heading": -0.009768639154040644,
+            "angularVelocity": -2.6261429169311795e-7,
+            "velocityX": 3.5238459576843355,
+            "velocityY": -1.3759170954228361,
+            "timestamp": 0.7524303596074354
+        },
+        {
+            "x": 5.911272135069668,
+            "y": 3.959404581326429,
+            "heading": -0.009768652411608816,
+            "angularVelocity": -1.233376298753571e-7,
+            "velocityX": 3.7486359355701584,
+            "velocityY": -0.5082989677477323,
+            "timestamp": 0.8599204109799262
+        },
+        {
+            "x": 6.316087697151271,
+            "y": 3.997759236961094,
+            "heading": -0.009768652034230711,
+            "angularVelocity": 3.51081891151765e-9,
+            "velocityX": 3.7660746916826353,
+            "velocityY": 0.3568205163634492,
+            "timestamp": 0.967410462352417
+        },
+        {
+            "x": 6.720902827559142,
+            "y": 4.036118448432054,
+            "heading": -0.009768651656829377,
+            "angularVelocity": 3.5110350109919825e-9,
+            "velocityX": 3.766070675741356,
+            "velocityY": 0.35686290015837574,
+            "timestamp": 1.0749005137249077
+        },
+        {
+            "x": 7.104541677751467,
+            "y": 4.072471064803872,
+            "heading": -0.005861206189315246,
+            "angularVelocity": 0.03635169411142489,
+            "velocityX": 3.569063790497984,
+            "velocityY": 0.3381951716242375,
+            "timestamp": 1.1823905650973985
+        },
+        {
+            "x": 7.392270834070994,
+            "y": 4.099735530737146,
+            "heading": -0.0029306066828158664,
+            "angularVelocity": 0.027263913907193366,
+            "velocityX": 2.6767980166131413,
+            "velocityY": 0.2536464127158502,
+            "timestamp": 1.2898806164698893
+        },
+        {
+            "x": 7.584090276967534,
+            "y": 4.117911843086064,
+            "heading": -0.0009768696053991616,
+            "angularVelocity": 0.018175980497453854,
+            "velocityX": 1.7845320608492279,
+            "velocityY": 0.16909762454137037,
+            "timestamp": 1.39737066784238
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -6.809535330810639e-32,
+            "angularVelocity": 0.009088000172350516,
+            "velocityX": 0.8922660451626774,
+            "velocityY": 0.08454881915017429,
+            "timestamp": 1.5048607192148709
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": -9.489213882041181e-31,
+            "velocityX": 2.198140684032685e-33,
+            "velocityY": -6.756921472052438e-32,
+            "timestamp": 1.6123507705873616
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C4.1.traj
new file mode 100644
index 00000000..c13783bc
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C4.1.traj	
@@ -0,0 +1,166 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 7.441191049306378e-32,
+            "velocityX": -6.15829104129807e-34,
+            "velocityY": 2.4252394047543234e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 4.262142393092874,
+            "y": 5.471176724349191,
+            "heading": -0.06580713819401311,
+            "angularVelocity": -0.6311237133024392,
+            "velocityX": -0.008224883662067025,
+            "velocityY": -0.8518601035521746,
+            "timestamp": 0.10426979181255681
+        },
+        {
+            "x": 4.27936690849282,
+            "y": 5.294863623901166,
+            "heading": -0.18680392528158013,
+            "angularVelocity": -1.1604203382805245,
+            "velocityX": 0.16519180771848577,
+            "velocityY": -1.6909317395107013,
+            "timestamp": 0.20853958362511363
+        },
+        {
+            "x": 4.348116425232891,
+            "y": 5.0445566747334585,
+            "heading": -0.3412830267774945,
+            "angularVelocity": -1.4815326549574157,
+            "velocityX": 0.6593426105967491,
+            "velocityY": -2.400570144205118,
+            "timestamp": 0.31280937543767046
+        },
+        {
+            "x": 4.502267029635194,
+            "y": 4.763921050939977,
+            "heading": -0.4878370803946758,
+            "angularVelocity": -1.4055274405902505,
+            "velocityX": 1.4783822018117698,
+            "velocityY": -2.6914374615609895,
+            "timestamp": 0.41707916725022726
+        },
+        {
+            "x": 4.740029978369904,
+            "y": 4.496898841850153,
+            "heading": -0.5207249369577782,
+            "angularVelocity": -0.31541116551017917,
+            "velocityX": 2.280266840487519,
+            "velocityY": -2.5608779345205184,
+            "timestamp": 0.5213489590627841
+        },
+        {
+            "x": 5.0582868126043,
+            "y": 4.2697391199027495,
+            "heading": -0.509978725166827,
+            "angularVelocity": 0.10306160206274768,
+            "velocityX": 3.0522438829312937,
+            "velocityY": -2.1785765368723764,
+            "timestamp": 0.6256187508753409
+        },
+        {
+            "x": 5.423286105863135,
+            "y": 4.1201949004052105,
+            "heading": -0.5099785687835147,
+            "angularVelocity": 0.0000014997949991595825,
+            "velocityX": 3.5005276879710774,
+            "velocityY": -1.4342046425715584,
+            "timestamp": 0.7298885426878977
+        },
+        {
+            "x": 5.805099761015269,
+            "y": 4.021168099470067,
+            "heading": -0.5099784851647622,
+            "angularVelocity": 8.019460959727046e-7,
+            "velocityX": 3.6617859162748747,
+            "velocityY": -0.9497170677501874,
+            "timestamp": 0.8341583345004545
+        },
+        {
+            "x": 6.154222032422552,
+            "y": 3.8375880828272497,
+            "heading": -0.5099784796190555,
+            "angularVelocity": 5.318612994948929e-8,
+            "velocityX": 3.348259024386382,
+            "velocityY": -1.7606251384181746,
+            "timestamp": 0.9384281263130113
+        },
+        {
+            "x": 6.493379431179238,
+            "y": 3.636192348012897,
+            "heading": -0.5099784741544249,
+            "angularVelocity": 5.240856867670856e-8,
+            "velocityX": 3.2526908595576858,
+            "velocityY": -1.9314868795019449,
+            "timestamp": 1.0426979181255682
+        },
+        {
+            "x": 6.795285003697281,
+            "y": 3.3823400148372036,
+            "heading": -0.5099784394461939,
+            "angularVelocity": 3.32869476219209e-7,
+            "velocityX": 2.895427019368865,
+            "velocityY": -2.4345721686299853,
+            "timestamp": 1.146967709938125
+        },
+        {
+            "x": 7.066681771760518,
+            "y": 3.0961027469536373,
+            "heading": -0.509978390292543,
+            "angularVelocity": 4.714083530965781e-7,
+            "velocityX": 2.6028321659174227,
+            "velocityY": -2.7451600593787298,
+            "timestamp": 1.2512375017506818
+        },
+        {
+            "x": 7.312943361341716,
+            "y": 2.8410544331524363,
+            "heading": -0.40533862017886096,
+            "angularVelocity": 1.0035482788897323,
+            "velocityX": 2.361773101301441,
+            "velocityY": -2.446042227260748,
+            "timestamp": 1.3555072935632386
+        },
+        {
+            "x": 7.49731350893092,
+            "y": 2.6484381177603322,
+            "heading": -0.22178416093118458,
+            "angularVelocity": 1.7603800300824186,
+            "velocityX": 1.768202893515326,
+            "velocityY": -1.8472878102448496,
+            "timestamp": 1.4597770853757954
+        },
+        {
+            "x": 7.619307085639959,
+            "y": 2.520663921373434,
+            "heading": -0.0776901780416505,
+            "angularVelocity": 1.3819341190262298,
+            "velocityX": 1.1699800545142,
+            "velocityY": -1.225419118670484,
+            "timestamp": 1.5640468771883522
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 1.429761379305419e-34,
+            "angularVelocity": 0.7450880709660586,
+            "velocityX": 0.5820757220763062,
+            "velocityY": -0.6105691808408042,
+            "timestamp": 1.668316669000909
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": -5.61453765178046e-32,
+            "velocityX": 0,
+            "velocityY": 2.318867528557263e-33,
+            "timestamp": 1.7725864608134658
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C5.1.traj
new file mode 100644
index 00000000..004abebc
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C5.1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": -1.2613927548149136e-32,
+            "velocityX": 4.983015637334932e-33,
+            "velocityY": -3.6905945238681026e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 4.258006222511338,
+            "y": 5.473607841813564,
+            "heading": -0.06530003091818118,
+            "angularVelocity": -0.6342708718414432,
+            "velocityX": -0.04850545332028736,
+            "velocityY": -0.8391424739420397,
+            "timestamp": 0.10295290831912149
+        },
+        {
+            "x": 4.271377859079598,
+            "y": 5.302453682559796,
+            "heading": -0.1916286288786279,
+            "angularVelocity": -1.2270522515873779,
+            "velocityX": 0.12988109599401854,
+            "velocityY": -1.662450940416803,
+            "timestamp": 0.20590581663824298
+        },
+        {
+            "x": 4.340766271943036,
+            "y": 5.0642424556348775,
+            "heading": -0.3653686640748466,
+            "angularVelocity": -1.6875680156375907,
+            "velocityX": 0.673982056420939,
+            "velocityY": -2.3137882242873524,
+            "timestamp": 0.30885872495736444
+        },
+        {
+            "x": 4.494848034080926,
+            "y": 4.801255485663876,
+            "heading": -0.5565822720790902,
+            "angularVelocity": -1.8572919514963226,
+            "velocityX": 1.4966236957608356,
+            "velocityY": -2.5544394448365244,
+            "timestamp": 0.41181163327648596
+        },
+        {
+            "x": 4.73233691115851,
+            "y": 4.546498868370588,
+            "heading": -0.6490495118957932,
+            "angularVelocity": -0.8981508276588281,
+            "velocityX": 2.3067719111095206,
+            "velocityY": -2.4744965582091383,
+            "timestamp": 0.5147645415956075
+        },
+        {
+            "x": 5.049349589860016,
+            "y": 4.3202563768587865,
+            "heading": -0.6490493910522949,
+            "angularVelocity": 0.0000011737744981731646,
+            "velocityX": 3.079200810130272,
+            "velocityY": -2.1975337579636096,
+            "timestamp": 0.617717449914729
+        },
+        {
+            "x": 5.4092082222704905,
+            "y": 4.171311637049865,
+            "heading": -0.6490493274377749,
+            "angularVelocity": 6.178992034513071e-7,
+            "velocityX": 3.4953712166637025,
+            "velocityY": -1.446726879703489,
+            "timestamp": 0.7206703582338505
+        },
+        {
+            "x": 5.789554256648214,
+            "y": 4.087528164867666,
+            "heading": -0.6490492895940018,
+            "angularVelocity": 3.675833320529158e-7,
+            "velocityX": 3.6943690138288297,
+            "velocityY": -0.8138038405141147,
+            "timestamp": 0.823623266552972
+        },
+        {
+            "x": 6.141218018459777,
+            "y": 3.9201502365347105,
+            "heading": -0.6490493503243893,
+            "angularVelocity": -5.898851085689342e-7,
+            "velocityX": 3.4157729738097102,
+            "velocityY": -1.6257717345306835,
+            "timestamp": 0.9265761748720935
+        },
+        {
+            "x": 6.456138339829479,
+            "y": 3.69100407104571,
+            "heading": -0.6490494074378677,
+            "angularVelocity": -5.547534235510974e-7,
+            "velocityX": 3.0588773693847333,
+            "velocityY": -2.2257376622981755,
+            "timestamp": 1.029529083191215
+        },
+        {
+            "x": 6.721775326533507,
+            "y": 3.406188756476768,
+            "heading": -0.6490494689481024,
+            "angularVelocity": -5.974599048336077e-7,
+            "velocityX": 2.580179531020508,
+            "velocityY": -2.7664620574496457,
+            "timestamp": 1.1324819915103363
+        },
+        {
+            "x": 6.916359430113237,
+            "y": 3.0688168532177102,
+            "heading": -0.6490494077682868,
+            "angularVelocity": 5.942504852606566e-7,
+            "velocityX": 1.8900301774533879,
+            "velocityY": -3.276953597204961,
+            "timestamp": 1.2354348998294578
+        },
+        {
+            "x": 7.040594979350531,
+            "y": 2.699698573462511,
+            "heading": -0.6490493528706274,
+            "angularVelocity": 5.332307776860117e-7,
+            "velocityX": 1.206722095234099,
+            "velocityY": -3.585311826364818,
+            "timestamp": 1.3383878081485792
+        },
+        {
+            "x": 7.164812386541997,
+            "y": 2.330574188065911,
+            "heading": -0.649049297974002,
+            "angularVelocity": 5.332207349098274e-7,
+            "velocityX": 1.2065458783003051,
+            "velocityY": -3.585371131551044,
+            "timestamp": 1.4413407164677006
+        },
+        {
+            "x": 7.289029734328084,
+            "y": 1.9614497826782673,
+            "heading": -0.6490492430773671,
+            "angularVelocity": 5.332208261000816e-7,
+            "velocityX": 1.206545301285243,
+            "velocityY": -3.5853713257276327,
+            "timestamp": 1.544293624786822
+        },
+        {
+            "x": 7.413246520819429,
+            "y": 1.5923251884092744,
+            "heading": -0.6490491881699392,
+            "angularVelocity": 5.333256623024379e-7,
+            "velocityX": 1.2065398493290926,
+            "velocityY": -3.585373160365936,
+            "timestamp": 1.6472465331059434
+        },
+        {
+            "x": 7.520074458284658,
+            "y": 1.2704783284902474,
+            "heading": -0.5256216946651037,
+            "angularVelocity": 1.1988733054752465,
+            "velocityX": 1.037638850707321,
+            "velocityY": -3.1261560763432104,
+            "timestamp": 1.7501994414250648
+        },
+        {
+            "x": 7.601104601009551,
+            "y": 1.0271731610238788,
+            "heading": -0.2913266412644364,
+            "angularVelocity": 2.275749730881093,
+            "velocityX": 0.7870602593733868,
+            "velocityY": -2.3632665792422265,
+            "timestamp": 1.8531523497441862
+        },
+        {
+            "x": 7.654009357663848,
+            "y": 0.8666565360101991,
+            "heading": -0.10310334462657153,
+            "angularVelocity": 1.828246522715339,
+            "velocityX": 0.5138733574219042,
+            "velocityY": -1.5591266690216163,
+            "timestamp": 1.9561052580633076
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -1.429149737287701e-32,
+            "angularVelocity": 1.0014612147428026,
+            "velocityX": 0.2524517545010862,
+            "velocityY": -0.7737181718391956,
+            "timestamp": 2.059058166382429
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": -8.032741852653566e-32,
+            "velocityX": 0,
+            "velocityY": 2.733638748586157e-32,
+            "timestamp": 2.1620110747015504
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj
new file mode 100644
index 00000000..c913a793
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 8.330960047205791e-35,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.215224456008129,
+            "y": 5.5891314798928855,
+            "heading": 0.013109691961439362,
+            "angularVelocity": 0.15985781811927008,
+            "velocityX": -0.5825685488618567,
+            "velocityY": 0.35522534228568337,
+            "timestamp": 0.08200845048227925
+        },
+        {
+            "x": 4.11967337131145,
+            "y": 5.647394543004508,
+            "heading": 0.03932481805987993,
+            "angularVelocity": 0.31966371690079975,
+            "velocityX": -1.1651370576417237,
+            "velocityY": 0.7104519445128596,
+            "timestamp": 0.1640169009645585
+        },
+        {
+            "x": 3.976346757700282,
+            "y": 5.734789362523958,
+            "heading": 0.07863903763442207,
+            "angularVelocity": 0.47939229851730114,
+            "velocityX": -1.7477054226519961,
+            "velocityY": 1.0656806585844962,
+            "timestamp": 0.24602535144683776
+        },
+        {
+            "x": 3.7852446629452063,
+            "y": 5.851316183985278,
+            "heading": 0.13104540832787342,
+            "angularVelocity": 0.6390362259652201,
+            "velocityX": -2.3302732051542674,
+            "velocityY": 1.4209123666652002,
+            "timestamp": 0.328033801929117
+        },
+        {
+            "x": 3.5463672091086966,
+            "y": 5.996975313640393,
+            "heading": 0.19653856458379848,
+            "angularVelocity": 0.7986147265406159,
+            "velocityX": -2.912839499243156,
+            "velocityY": 1.77614780914058,
+            "timestamp": 0.41004225241139625
+        },
+        {
+            "x": 3.281494446824011,
+            "y": 6.15848998396632,
+            "heading": 0.19653857286419524,
+            "angularVelocity": 1.009700426817188e-7,
+            "velocityX": -3.229822789322431,
+            "velocityY": 1.9694881366015824,
+            "timestamp": 0.4920507028936755
+        },
+        {
+            "x": 3.016621684855716,
+            "y": 6.320004654811106,
+            "heading": 0.1965385811445921,
+            "angularVelocity": 1.0097004416983622e-7,
+            "velocityX": -3.229822785464406,
+            "velocityY": 1.969488142928474,
+            "timestamp": 0.5740591533759547
+        },
+        {
+            "x": 2.77774755563319,
+            "y": 6.465669559960414,
+            "heading": 0.2620174097526663,
+            "angularVelocity": 0.7984400171324189,
+            "velocityX": -2.9127989593480157,
+            "velocityY": 1.7762182347389206,
+            "timestamp": 0.6560676038582339
+        },
+        {
+            "x": 2.5866484397575076,
+            "y": 6.582201656937504,
+            "heading": 0.3144069758542471,
+            "angularVelocity": 0.6388313130352502,
+            "velocityX": -2.330236881100143,
+            "velocityY": 1.4209766955939638,
+            "timestamp": 0.7380760543405132
+        },
+        {
+            "x": 2.443324199843316,
+            "y": 6.6696008076986475,
+            "heading": 0.35370275100271265,
+            "angularVelocity": 0.4791673896698825,
+            "velocityX": -1.7476764781107736,
+            "velocityY": 1.0657334731623573,
+            "timestamp": 0.8200845048227924
+        },
+        {
+            "x": 2.347774735979553,
+            "y": 6.727866934294423,
+            "heading": 0.37990105353306003,
+            "angularVelocity": 0.3194585725773273,
+            "velocityX": -1.1651172934234333,
+            "velocityY": 0.7104893002260324,
+            "timestamp": 0.9020929553050716
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0.15972678900658524,
+            "velocityX": -0.5825586960684825,
+            "velocityY": 0.3552446794720561,
+            "timestamp": 0.9841014057873508
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": -4.59054414969708e-34,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 1.06610985626963
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj
new file mode 100644
index 00000000..a4697851
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 4.181096344253034,
+            "y": 5.56,
+            "heading": 2.5710705899848466e-19,
+            "angularVelocity": 2.5941602968880693e-18,
+            "velocityX": -0.8263919813283321,
+            "velocityY": -1.4479549544654444e-34,
+            "timestamp": 0.099109935233537
+        },
+        {
+            "x": 4.017289035836238,
+            "y": 5.56,
+            "heading": 7.7182898862159e-19,
+            "angularVelocity": 5.193444314378615e-18,
+            "velocityX": -1.6527839316089838,
+            "velocityY": -7.36495654370163e-34,
+            "timestamp": 0.198219870467074
+        },
+        {
+            "x": 3.771578080903875,
+            "y": 5.56,
+            "heading": 1.545129672215716e-18,
+            "angularVelocity": 7.802453727507411e-18,
+            "velocityX": -2.479175819794286,
+            "velocityY": -8.54847043233614e-34,
+            "timestamp": 0.297329805700611
+        },
+        {
+            "x": 3.4439634979187304,
+            "y": 5.56,
+            "heading": 2.5806642382479214e-18,
+            "angularVelocity": 1.0448342676399075e-17,
+            "velocityX": -3.3055675216936846,
+            "velocityY": -5.0910568375180236e-34,
+            "timestamp": 0.396439740934148
+        },
+        {
+            "x": 3.0690365020812695,
+            "y": 5.56,
+            "heading": -2.5850068638158606e-18,
+            "angularVelocity": -5.212061828001078e-17,
+            "velocityX": -3.7829405796099502,
+            "velocityY": -4.536187828596351e-34,
+            "timestamp": 0.495549676167685
+        },
+        {
+            "x": 2.741421919096125,
+            "y": 5.56,
+            "heading": -1.5478395588580842e-18,
+            "angularVelocity": 1.0464816695997814e-17,
+            "velocityX": -3.305567521693684,
+            "velocityY": -2.872506388563931e-33,
+            "timestamp": 0.594659611401222
+        },
+        {
+            "x": 2.4957109641637625,
+            "y": 5.56,
+            "heading": -7.728706253244835e-19,
+            "angularVelocity": 7.819286045545264e-18,
+            "velocityX": -2.479175819794286,
+            "velocityY": -1.6595590840772702e-33,
+            "timestamp": 0.6937695466347591
+        },
+        {
+            "x": 2.3319036557469652,
+            "y": 5.56,
+            "heading": -2.5738443375571516e-19,
+            "angularVelocity": 5.201155568994596e-18,
+            "velocityX": -1.6527839316089838,
+            "velocityY": -1.4347514653062086e-33,
+            "timestamp": 0.792879481868296
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 2.5969589542999405e-18,
+            "velocityX": -0.8263919813283321,
+            "velocityY": 2.9921012122849815e-33,
+            "timestamp": 0.891989417101833
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 1.808288986442481e-43,
+            "timestamp": 0.9910993523353699
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj
new file mode 100644
index 00000000..4a59e123
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj	
@@ -0,0 +1,130 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 9.014129666558106e-32,
+            "angularVelocity": 9.039417129994225e-31,
+            "velocityX": 0,
+            "velocityY": -2.779554313855136e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 4.206523478699805,
+            "y": 5.55440930401173,
+            "heading": -0.10760119525493232,
+            "angularVelocity": -1.2344158099898592,
+            "velocityX": -0.6479064718660169,
+            "velocityY": -0.06413723844253004,
+            "timestamp": 0.08716770668695249
+        },
+        {
+            "x": 4.091196989898444,
+            "y": 5.541197271810027,
+            "heading": -0.295913717265277,
+            "angularVelocity": -2.1603473254910437,
+            "velocityX": -1.3230414471673093,
+            "velocityY": -0.15157026270235532,
+            "timestamp": 0.17433541337390498
+        },
+        {
+            "x": 3.9151950960136452,
+            "y": 5.516370757100726,
+            "heading": -0.5340653762567812,
+            "angularVelocity": -2.7321088054637612,
+            "velocityX": -2.019118095155108,
+            "velocityY": -0.2848132141236808,
+            "timestamp": 0.26150312006085746
+        },
+        {
+            "x": 3.678758800597895,
+            "y": 5.473177706772957,
+            "heading": -0.7546654655667131,
+            "angularVelocity": -2.530754767957573,
+            "velocityX": -2.712429917020411,
+            "velocityY": -0.4955166536948123,
+            "timestamp": 0.34867082674780997
+        },
+        {
+            "x": 3.3968844521889907,
+            "y": 5.398848640381046,
+            "heading": -0.8527884173152881,
+            "angularVelocity": -1.1256800881657552,
+            "velocityX": -3.233701552126478,
+            "velocityY": -0.8527133409492049,
+            "timestamp": 0.4358385334347625
+        },
+        {
+            "x": 3.098615494079836,
+            "y": 5.268356714792353,
+            "heading": -0.864385170870599,
+            "angularVelocity": -0.1330395624260059,
+            "velocityX": -3.421782784538955,
+            "velocityY": -1.4970214377364781,
+            "timestamp": 0.5230062401217149
+        },
+        {
+            "x": 2.832096469446971,
+            "y": 5.0836195976119996,
+            "heading": -0.8474300397750907,
+            "angularVelocity": 0.19451161146636953,
+            "velocityX": -3.0575431517318843,
+            "velocityY": -2.1193297862453186,
+            "timestamp": 0.6101739468086674
+        },
+        {
+            "x": 2.620164345186853,
+            "y": 4.879059766080367,
+            "heading": -0.7372424721392209,
+            "angularVelocity": 1.2640870320425897,
+            "velocityX": -2.431314672774813,
+            "velocityY": -2.346738709855847,
+            "timestamp": 0.6973416534956199
+        },
+        {
+            "x": 2.469226841034766,
+            "y": 4.6911608685478585,
+            "heading": -0.6128307853924054,
+            "angularVelocity": 1.427268095897297,
+            "velocityX": -1.7315759458276552,
+            "velocityY": -2.155602168212534,
+            "timestamp": 0.7845093601825724
+        },
+        {
+            "x": 2.3748885049182054,
+            "y": 4.5313089505917965,
+            "heading": -0.5057978702393489,
+            "angularVelocity": 1.2278964219794164,
+            "velocityX": -1.0822624536327627,
+            "velocityY": -1.8338433352404204,
+            "timestamp": 0.871677066869525
+        },
+        {
+            "x": 2.3223904484665,
+            "y": 4.418253055823363,
+            "heading": -0.42949016513376354,
+            "angularVelocity": 0.87541255822675,
+            "velocityX": -0.6022649722820163,
+            "velocityY": -1.2969928780443263,
+            "timestamp": 0.9588447735564775
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 0.4530366420626772,
+            "velocityX": -0.256866324898396,
+            "velocityY": -0.6682871218875669,
+            "timestamp": 1.0460124802434299
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -3.4100744325418914e-31,
+            "velocityX": -9.400788860397695e-33,
+            "velocityY": 0,
+            "timestamp": 1.1331801869303824
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C1.1.traj
new file mode 100644
index 00000000..97fb7eaf
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C1.1.traj	
@@ -0,0 +1,220 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 3.2688003829398086e-32,
+            "velocityX": -2.078713781970587e-32,
+            "velocityY": -4.638967376158192e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 4.29154268151456,
+            "y": 3.0623077457400183,
+            "heading": -0.5127665183876435,
+            "angularVelocity": 0.580669777278592,
+            "velocityX": 0.31134301225465,
+            "velocityY": 0.6796516730776717,
+            "timestamp": 0.09167599846825866
+        },
+        {
+            "x": 4.3492977760941525,
+            "y": 3.186661868715021,
+            "heading": -0.4070750475472326,
+            "angularVelocity": 1.1528804987818582,
+            "velocityX": 0.6299914431757085,
+            "velocityY": 1.3564523436094174,
+            "timestamp": 0.18335199693651733
+        },
+        {
+            "x": 4.438053987492854,
+            "y": 3.3723523918382017,
+            "heading": -0.25179906449139605,
+            "angularVelocity": 1.6937473891773143,
+            "velocityX": 0.968151019696088,
+            "velocityY": 2.025508597950785,
+            "timestamp": 0.27502799540477596
+        },
+        {
+            "x": 4.5709698511757795,
+            "y": 3.6117104026755453,
+            "heading": -0.06452556421817643,
+            "angularVelocity": 2.042775681773022,
+            "velocityX": 1.449843643960376,
+            "velocityY": 2.6109125053076734,
+            "timestamp": 0.36670399387303465
+        },
+        {
+            "x": 4.761013106791701,
+            "y": 3.810602792551275,
+            "heading": 0.11153690871840848,
+            "angularVelocity": 1.9204860146414893,
+            "velocityX": 2.0729881189319217,
+            "velocityY": 2.1695143025313537,
+            "timestamp": 0.45837999234129334
+        },
+        {
+            "x": 5.007328053154023,
+            "y": 3.968591564774131,
+            "heading": 0.25752334504560864,
+            "angularVelocity": 1.5924171949733021,
+            "velocityX": 2.6867986220799525,
+            "velocityY": 1.7233384404049508,
+            "timestamp": 0.550055990809552
+        },
+        {
+            "x": 5.305296422655045,
+            "y": 4.081628168088871,
+            "heading": 0.35020186087766364,
+            "angularVelocity": 1.0109354398157564,
+            "velocityX": 3.2502331524012615,
+            "velocityY": 1.2330010602925316,
+            "timestamp": 0.6417319892778107
+        },
+        {
+            "x": 5.6446965706540295,
+            "y": 4.152909996485894,
+            "heading": 0.3502021264888923,
+            "angularVelocity": 0.0000028972820926514052,
+            "velocityX": 3.702170182705961,
+            "velocityY": 0.7775407913523397,
+            "timestamp": 0.7334079877460694
+        },
+        {
+            "x": 5.981889969698284,
+            "y": 4.233991234618027,
+            "heading": 0.3502024148432213,
+            "angularVelocity": 0.0000031453633856654694,
+            "velocityX": 3.6780990082262504,
+            "velocityY": 0.884432561268565,
+            "timestamp": 0.8250839862143281
+        },
+        {
+            "x": 6.295899946950055,
+            "y": 4.381202864314355,
+            "heading": 0.350202827556894,
+            "angularVelocity": 0.000004501872678764248,
+            "velocityX": 3.425214696303447,
+            "velocityY": 1.6057815803042044,
+            "timestamp": 0.9167599846825868
+        },
+        {
+            "x": 6.57390491063703,
+            "y": 4.588535293884794,
+            "heading": 0.350203324900352,
+            "angularVelocity": 0.0000054250127210279085,
+            "velocityX": 3.0324727118541346,
+            "velocityY": 2.2615780906081357,
+            "timestamp": 1.0084359831508454
+        },
+        {
+            "x": 6.804553755441848,
+            "y": 4.847523149382792,
+            "heading": 0.35020327996165895,
+            "angularVelocity": -4.901903857536901e-7,
+            "velocityX": 2.5159130923965525,
+            "velocityY": 2.825034467311176,
+            "timestamp": 1.100111981619104
+        },
+        {
+            "x": 6.978428828519116,
+            "y": 5.147591577808381,
+            "heading": 0.35020324373344786,
+            "angularVelocity": -3.9517661894346874e-7,
+            "velocityX": 1.8966258997164847,
+            "velocityY": 3.273140554116584,
+            "timestamp": 1.1917879800873628
+        },
+        {
+            "x": 7.088431556948234,
+            "y": 5.476488224562697,
+            "heading": 0.3502032113343976,
+            "angularVelocity": -3.5340820734555857e-7,
+            "velocityX": 1.1999076123202526,
+            "velocityY": 3.587598196360961,
+            "timestamp": 1.2834639785556214
+        },
+        {
+            "x": 7.187536215850895,
+            "y": 5.808831226900728,
+            "heading": 0.3502031791295722,
+            "angularVelocity": -3.5128960565806065e-7,
+            "velocityX": 1.0810316828670656,
+            "velocityY": 3.6251909757285055,
+            "timestamp": 1.3751399770238801
+        },
+        {
+            "x": 7.286640767886556,
+            "y": 6.141174261106461,
+            "heading": 0.3502031469247365,
+            "angularVelocity": -3.512897194087527e-7,
+            "velocityX": 1.0810305171639119,
+            "velocityY": 3.6251913233407715,
+            "timestamp": 1.4668159754921388
+        },
+        {
+            "x": 7.3857452620780215,
+            "y": 6.473517312561292,
+            "heading": 0.350203114719879,
+            "angularVelocity": -3.512899557892109e-7,
+            "velocityX": 1.0810298862005645,
+            "velocityY": 3.625191511493593,
+            "timestamp": 1.5584919739603975
+        },
+        {
+            "x": 7.481976228665169,
+            "y": 6.80241098377653,
+            "heading": 0.33998449018149474,
+            "angularVelocity": -0.11146455679915658,
+            "velocityX": 1.0496855032395944,
+            "velocityY": 3.5875657392388547,
+            "timestamp": 1.6501679724286562
+        },
+        {
+            "x": 7.561455250393006,
+            "y": 7.068914029375232,
+            "heading": 0.2316909043200923,
+            "angularVelocity": -1.1812643185871456,
+            "velocityX": 0.8669556160367877,
+            "velocityY": 2.907010014087551,
+            "timestamp": 1.741843970896915
+        },
+        {
+            "x": 7.620860593043084,
+            "y": 7.268277067392774,
+            "heading": 0.12481105079738766,
+            "angularVelocity": -1.1658433538600752,
+            "velocityX": 0.6479923168837385,
+            "velocityY": 2.1746481232660675,
+            "timestamp": 1.8335199693651736
+        },
+        {
+            "x": 7.66032628893014,
+            "y": 7.4008489817464715,
+            "heading": 0.04379045785830094,
+            "angularVelocity": -0.8837710446877722,
+            "velocityX": 0.430491039600946,
+            "velocityY": 1.4460918513977026,
+            "timestamp": 1.9251959678334323
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 6.52409657127002e-31,
+            "angularVelocity": -0.4776654586801488,
+            "velocityX": 0.21460045593800534,
+            "velocityY": 0.7215740145599008,
+            "timestamp": 2.0168719663016907
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": -6.745238901014272e-30,
+            "velocityX": 0,
+            "velocityY": 3.373152313643601e-31,
+            "timestamp": 2.1085479647699494
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.1.traj
new file mode 100644
index 00000000..69777c27
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.1.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -2.8196964588027846e-31,
+            "velocityX": -1.564828148658428e-31,
+            "velocityY": 3.07478616058615e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 4.294807569207289,
+            "y": 3.0477321348748183,
+            "heading": -0.5960373953696199,
+            "angularVelocity": -0.36026847684168845,
+            "velocityX": 0.381499939303545,
+            "velocityY": 0.5724991570056517,
+            "timestamp": 0.08337503084628527
+        },
+        {
+            "x": 4.359939756522268,
+            "y": 3.142193816557181,
+            "heading": -0.6551934033575421,
+            "angularVelocity": -0.7095170746861289,
+            "velocityX": 0.7811953609355742,
+            "velocityY": 1.1329732741750633,
+            "timestamp": 0.16675006169257053
+        },
+        {
+            "x": 4.461050205756915,
+            "y": 3.2814565261988764,
+            "heading": -0.7417418377173162,
+            "angularVelocity": -1.038061797174498,
+            "velocityX": 1.2127185826300881,
+            "velocityY": 1.6703167390540123,
+            "timestamp": 0.2501250925388558
+        },
+        {
+            "x": 4.603825208189836,
+            "y": 3.4604941356958814,
+            "heading": -0.8514332359801334,
+            "angularVelocity": -1.315638472926628,
+            "velocityX": 1.7124431737380506,
+            "velocityY": 2.147376830685539,
+            "timestamp": 0.33350012338514107
+        },
+        {
+            "x": 4.8030511460470855,
+            "y": 3.6526461815261504,
+            "heading": -0.9644207302174959,
+            "angularVelocity": -1.3551718433024906,
+            "velocityX": 2.3895156119888465,
+            "velocityY": 2.3046713611960277,
+            "timestamp": 0.41687515423142635
+        },
+        {
+            "x": 5.051097476442208,
+            "y": 3.8467657037250893,
+            "heading": -0.9658198239131095,
+            "angularVelocity": -0.016780727772000164,
+            "velocityX": 2.975067329839247,
+            "velocityY": 2.328269269930804,
+            "timestamp": 0.5002501850777116
+        },
+        {
+            "x": 5.320526470824812,
+            "y": 4.010737925204951,
+            "heading": -0.965820109061322,
+            "angularVelocity": -0.000003420067250854742,
+            "velocityX": 3.2315309709370754,
+            "velocityY": 1.9666825884858703,
+            "timestamp": 0.5836252159239969
+        },
+        {
+            "x": 5.614243645591117,
+            "y": 4.1256688963704224,
+            "heading": -0.9658201941489747,
+            "angularVelocity": -0.000001020541184209393,
+            "velocityX": 3.5228433715103167,
+            "velocityY": 1.37848190278171,
+            "timestamp": 0.6670002467702821
+        },
+        {
+            "x": 5.906557788271019,
+            "y": 4.244122886725936,
+            "heading": -0.965820105112684,
+            "angularVelocity": 0.0000010679011417361832,
+            "velocityX": 3.506015406684877,
+            "velocityY": 1.4207369898777267,
+            "timestamp": 0.7503752776165674
+        },
+        {
+            "x": 6.172253111887642,
+            "y": 4.41406398245588,
+            "heading": -0.9657938677546691,
+            "angularVelocity": 0.0003146908342779248,
+            "velocityX": 3.1867493291423483,
+            "velocityY": 2.0382732576526004,
+            "timestamp": 0.8337503084628527
+        },
+        {
+            "x": 6.426785390452526,
+            "y": 4.5994304003649855,
+            "heading": -0.9640144900160125,
+            "angularVelocity": 0.021341854037057975,
+            "velocityX": 3.0528597828545716,
+            "velocityY": 2.2232845496735987,
+            "timestamp": 0.917125339309138
+        },
+        {
+            "x": 6.681598941757368,
+            "y": 4.785246665794811,
+            "heading": -0.9639002296367303,
+            "angularVelocity": 0.0013704388246982366,
+            "velocityX": 3.0562333676929114,
+            "velocityY": 2.2286800201898096,
+            "timestamp": 1.0005003701554231
+        },
+        {
+            "x": 6.9014822245071255,
+            "y": 5.01136660335564,
+            "heading": -0.9639000238890051,
+            "angularVelocity": 0.000002467737919950576,
+            "velocityX": 2.6372797769052228,
+            "velocityY": 2.7120822057351384,
+            "timestamp": 1.0838754010017084
+        },
+        {
+            "x": 7.122012116423131,
+            "y": 5.23107273201418,
+            "heading": -0.9515772318649645,
+            "angularVelocity": 0.1477995498047812,
+            "velocityX": 2.645035206314798,
+            "velocityY": 2.635154990990089,
+            "timestamp": 1.1672504318479937
+        },
+        {
+            "x": 7.311148990719613,
+            "y": 5.417840497645029,
+            "heading": -0.7917020160601519,
+            "angularVelocity": 1.9175431083146124,
+            "velocityX": 2.2685073981583956,
+            "velocityY": 2.2400923122317544,
+            "timestamp": 1.250625462694279
+        },
+        {
+            "x": 7.462007390054419,
+            "y": 5.572027096128993,
+            "heading": -0.5225549044290918,
+            "angularVelocity": 3.228150069620655,
+            "velocityX": 1.8093954245478587,
+            "velocityY": 1.8493138403538476,
+            "timestamp": 1.3340004935405643
+        },
+        {
+            "x": 7.571928087470151,
+            "y": 5.685474744918936,
+            "heading": -0.27088873612536774,
+            "angularVelocity": 3.018483660506339,
+            "velocityX": 1.318388686636741,
+            "velocityY": 1.3606909363440005,
+            "timestamp": 1.4173755243868496
+        },
+        {
+            "x": 7.6442805622690475,
+            "y": 5.7599789636443175,
+            "heading": -0.0924144798498188,
+            "angularVelocity": 2.1406199729580164,
+            "velocityX": 0.8677954786282445,
+            "velocityY": 0.8936034921863096,
+            "timestamp": 1.5007505552331348
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -2.5332548087748344e-31,
+            "angularVelocity": 1.1084191383412998,
+            "velocityX": 0.42841888474747514,
+            "velocityY": 0.4440302567795869,
+            "timestamp": 1.5841255860794201
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 5.737670210968143e-33,
+            "velocityX": 0,
+            "velocityY": -3.2710687085311585e-31,
+            "timestamp": 1.6675006169257054
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.1.traj
new file mode 100644
index 00000000..9d039264
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.1.traj	
@@ -0,0 +1,166 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -1.0027901855857707e-30,
+            "velocityX": -5.17158696002297e-32,
+            "velocityY": 1.9371271312239428e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 4.300239460670815,
+            "y": 3.0530479185441526,
+            "heading": -0.6028935811234369,
+            "angularVelocity": -0.4158951640704737,
+            "velocityX": 0.4197942063083121,
+            "velocityY": 0.5980003056006388,
+            "timestamp": 0.08870884855296232
+        },
+        {
+            "x": 4.375862012794379,
+            "y": 3.1584586937478076,
+            "heading": -0.6740120078870278,
+            "angularVelocity": -0.8017061197804932,
+            "velocityX": 0.8524803709791626,
+            "velocityY": 1.1882780232540273,
+            "timestamp": 0.17741769710592464
+        },
+        {
+            "x": 4.491898262382256,
+            "y": 3.3149103934712074,
+            "heading": -0.7745169367898204,
+            "angularVelocity": -1.1329752391362358,
+            "velocityX": 1.3080572172977813,
+            "velocityY": 1.763653821185524,
+            "timestamp": 0.26612654565888694
+        },
+        {
+            "x": 4.65291407862912,
+            "y": 3.5188640922932937,
+            "heading": -0.893163244153065,
+            "angularVelocity": -1.3374799616794542,
+            "velocityX": 1.8151043427277864,
+            "velocityY": 2.299135905256609,
+            "timestamp": 0.3548353942118493
+        },
+        {
+            "x": 4.870914712338585,
+            "y": 3.7498942741357597,
+            "heading": -0.9455687285938831,
+            "angularVelocity": -0.5907582534963245,
+            "velocityX": 2.457484650804715,
+            "velocityY": 2.6043645657798487,
+            "timestamp": 0.44354424276481164
+        },
+        {
+            "x": 5.142859309500119,
+            "y": 3.9465163862068953,
+            "heading": -0.9455688623878027,
+            "angularVelocity": -0.0000015082364580335284,
+            "velocityX": 3.065585920655626,
+            "velocityY": 2.216488155110533,
+            "timestamp": 0.532253091317774
+        },
+        {
+            "x": 5.447866241414174,
+            "y": 4.086462405550872,
+            "heading": -0.9455689961292948,
+            "angularVelocity": -0.0000015076454528851402,
+            "velocityX": 3.438292085732067,
+            "velocityY": 1.5775880492961702,
+            "timestamp": 0.6209619398707363
+        },
+        {
+            "x": 5.774274975125892,
+            "y": 4.16438201701327,
+            "heading": -0.9455689173593601,
+            "angularVelocity": 8.879602884106285e-7,
+            "velocityX": 3.6795510147653454,
+            "velocityY": 0.8783747363813094,
+            "timestamp": 0.7096707884236987
+        },
+        {
+            "x": 6.109606649873876,
+            "y": 4.1772967927030695,
+            "heading": -0.9455688167433821,
+            "angularVelocity": 0.0000011342270768412302,
+            "velocityX": 3.780137835379285,
+            "velocityY": 0.14558610443566755,
+            "timestamp": 0.798379636976661
+        },
+        {
+            "x": 6.445060320326861,
+            "y": 4.168079453807152,
+            "heading": -0.9455687201101868,
+            "angularVelocity": 0.0000010893298350239914,
+            "velocityX": 3.781513072539862,
+            "velocityY": -0.10390551840399499,
+            "timestamp": 0.8870884855296234
+        },
+        {
+            "x": 6.780513985890507,
+            "y": 4.158861936977066,
+            "heading": -0.9455686234766241,
+            "angularVelocity": 0.0000010893339756447378,
+            "velocityX": 3.781513017423149,
+            "velocityY": -0.10390752422611307,
+            "timestamp": 0.9757973340825857
+        },
+        {
+            "x": 7.079210544299498,
+            "y": 4.144050975293544,
+            "heading": -0.856180476499644,
+            "angularVelocity": 1.0076576174203435,
+            "velocityX": 3.3671563015572095,
+            "velocityY": -0.16696149172400718,
+            "timestamp": 1.064506182635548
+        },
+        {
+            "x": 7.324091725395792,
+            "y": 4.136558502839443,
+            "heading": -0.6317167360096363,
+            "angularVelocity": 2.5303421716267107,
+            "velocityX": 2.760504561730298,
+            "velocityY": -0.08446138774564337,
+            "timestamp": 1.1532150311885103
+        },
+        {
+            "x": 7.50528856962614,
+            "y": 4.13147246408345,
+            "heading": -0.35065072299807204,
+            "angularVelocity": 3.1684101146207286,
+            "velocityX": 2.042601692909661,
+            "velocityY": -0.05733406350050504,
+            "timestamp": 1.2419238797414727
+        },
+        {
+            "x": 7.622685008969739,
+            "y": 4.128418008534293,
+            "heading": -0.12509027443555182,
+            "angularVelocity": 2.5427051781407424,
+            "velocityX": 1.323390408720159,
+            "velocityY": -0.03443236609404116,
+            "timestamp": 1.330632728294435
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.405276849475189e-32,
+            "angularVelocity": 1.4101217237744719,
+            "velocityX": 0.6461022994345517,
+            "velocityY": -0.01598497283444213,
+            "timestamp": 1.4193415768473974
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": -2.3485646679922316e-31,
+            "velocityX": 0,
+            "velocityY": -2.6996723431981715e-31,
+            "timestamp": 1.5080504254003597
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4.1.traj
new file mode 100644
index 00000000..0b021e4e
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4.1.traj	
@@ -0,0 +1,238 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 0,
+            "velocityX": 5.832812358477626e-33,
+            "velocityY": -2.114400767819492e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 4.2637473414422145,
+            "y": 2.974020167404826,
+            "heading": -0.5570803398431683,
+            "angularVelocity": 0.15939480257853877,
+            "velocityX": 0.013355031418934325,
+            "velocityY": -0.46426099366130524,
+            "timestamp": 0.05595954204614445
+        },
+        {
+            "x": 4.266239342094047,
+            "y": 2.922097369670203,
+            "heading": -0.5396590874905302,
+            "angularVelocity": 0.31131870840312753,
+            "velocityX": 0.04453218451605929,
+            "velocityY": -0.9278631639230981,
+            "timestamp": 0.1119190840922889
+        },
+        {
+            "x": 4.271816064940073,
+            "y": 2.844342168020101,
+            "heading": -0.5143157133438189,
+            "angularVelocity": 0.4528874472527088,
+            "velocityX": 0.0996563345966598,
+            "velocityY": -1.3894895992176792,
+            "timestamp": 0.16787862613843335
+        },
+        {
+            "x": 4.282354343554946,
+            "y": 2.7410289237168692,
+            "heading": -0.48190029722305777,
+            "angularVelocity": 0.5792652143941603,
+            "velocityX": 0.18831960072480589,
+            "velocityY": -1.8462131841257783,
+            "timestamp": 0.2238381681845778
+        },
+        {
+            "x": 4.3006106845058465,
+            "y": 2.6128237251760056,
+            "heading": -0.4437500478548854,
+            "angularVelocity": 0.6817469902936669,
+            "velocityX": 0.32624178617911886,
+            "velocityY": -2.2910337335345985,
+            "timestamp": 0.27979771023072225
+        },
+        {
+            "x": 4.330801306241698,
+            "y": 2.4614150778969766,
+            "heading": -0.40214700400499553,
+            "angularVelocity": 0.7434486117771366,
+            "velocityX": 0.5395080201148661,
+            "velocityY": -2.705680599640681,
+            "timestamp": 0.3357572522768667
+        },
+        {
+            "x": 4.379172513113433,
+            "y": 2.2912661424924936,
+            "heading": -0.3611794534128463,
+            "angularVelocity": 0.7320923133783194,
+            "velocityX": 0.864396045840541,
+            "velocityY": -3.0405705476320635,
+            "timestamp": 0.39171679432301115
+        },
+        {
+            "x": 4.451949891834961,
+            "y": 2.111933259909663,
+            "heading": -0.32494756197390784,
+            "angularVelocity": 0.6474658318159084,
+            "velocityX": 1.3005356380778725,
+            "velocityY": -3.2046881733762564,
+            "timestamp": 0.4476763363691556
+        },
+        {
+            "x": 4.550285732793113,
+            "y": 1.9373903674143325,
+            "heading": -0.2971520965733793,
+            "angularVelocity": 0.49670644869849423,
+            "velocityX": 1.7572667209653445,
+            "velocityY": -3.1190907951212283,
+            "timestamp": 0.5036358784153001
+        },
+        {
+            "x": 4.671000177640236,
+            "y": 1.7762856565305696,
+            "heading": -0.2715476133805437,
+            "angularVelocity": 0.457553479828869,
+            "velocityX": 2.157173565637473,
+            "velocityY": -2.878949773229245,
+            "timestamp": 0.5595954204614446
+        },
+        {
+            "x": 4.810884164899462,
+            "y": 1.6328902215117949,
+            "heading": -0.24301377044973924,
+            "angularVelocity": 0.5099012945331528,
+            "velocityX": 2.4997343106181424,
+            "velocityY": -2.5624840693036233,
+            "timestamp": 0.6155549625075891
+        },
+        {
+            "x": 4.966899091534391,
+            "y": 1.5100232904396305,
+            "heading": -0.21315700918191657,
+            "angularVelocity": 0.5335419157506132,
+            "velocityX": 2.7879950573269263,
+            "velocityY": -2.1956386092446096,
+            "timestamp": 0.6715145045537336
+        },
+        {
+            "x": 5.136752128601074,
+            "y": 1.409298062324524,
+            "heading": -0.1837509654270181,
+            "angularVelocity": 0.5254875697633724,
+            "velocityX": 3.035282828558936,
+            "velocityY": -1.7999651968567865,
+            "timestamp": 0.7274740465998781
+        },
+        {
+            "x": 5.4382274814533025,
+            "y": 1.3074419969375322,
+            "heading": -0.14111678525881038,
+            "angularVelocity": 0.4782734646509064,
+            "velocityX": 3.3819733590888914,
+            "velocityY": -1.142629061849971,
+            "timestamp": 0.8166158939305274
+        },
+        {
+            "x": 5.764139683697637,
+            "y": 1.267066681764717,
+            "heading": -0.11133632809533998,
+            "angularVelocity": 0.3340794257159682,
+            "velocityX": 3.6561077877984602,
+            "velocityY": -0.4529333459183563,
+            "timestamp": 0.9057577412611766
+        },
+        {
+            "x": 6.100462721846291,
+            "y": 1.2915989985388854,
+            "heading": -0.11133120767369757,
+            "angularVelocity": 0.000057441278584387276,
+            "velocityX": 3.7728973340784897,
+            "velocityY": 0.2752053890377148,
+            "timestamp": 0.9948995885918258
+        },
+        {
+            "x": 6.42549869748969,
+            "y": 1.3814198753662668,
+            "heading": -0.1113311879575033,
+            "angularVelocity": 2.2117776172639892e-7,
+            "velocityX": 3.6462782113742715,
+            "velocityY": 1.0076174043625032,
+            "timestamp": 1.084041435922475
+        },
+        {
+            "x": 6.7266979846048365,
+            "y": 1.533061465523067,
+            "heading": -0.11133117504978503,
+            "angularVelocity": 1.4479976182990285e-7,
+            "velocityX": 3.3788764327254777,
+            "velocityY": 1.7011268522889103,
+            "timestamp": 1.1731832832531242
+        },
+        {
+            "x": 6.992432961420095,
+            "y": 1.740669527522903,
+            "heading": -0.11133116451645807,
+            "angularVelocity": 1.1816366034373416e-7,
+            "velocityX": 2.981035111708839,
+            "velocityY": 2.3289629754897407,
+            "timestamp": 1.2623251305837733
+        },
+        {
+            "x": 7.221595592407175,
+            "y": 1.979432543612985,
+            "heading": -0.09587073631794915,
+            "angularVelocity": 0.17343625537800947,
+            "velocityX": 2.5707637641507293,
+            "velocityY": 2.678461611912118,
+            "timestamp": 1.3514669779144224
+        },
+        {
+            "x": 7.404993837760709,
+            "y": 2.170501480308595,
+            "heading": -0.06411284637687509,
+            "angularVelocity": 0.3562624164974272,
+            "velocityX": 2.0573754173309005,
+            "velocityY": 2.1434258142181197,
+            "timestamp": 1.4406088252450715
+        },
+        {
+            "x": 7.54251389523076,
+            "y": 2.3137697288578556,
+            "heading": -0.0342508837604198,
+            "angularVelocity": 0.33499376006509896,
+            "velocityX": 1.5427104282454527,
+            "velocityY": 1.6071940714649837,
+            "timestamp": 1.5297506725757206
+        },
+        {
+            "x": 7.634176122098128,
+            "y": 2.4092619199619465,
+            "heading": -0.011960250388889534,
+            "angularVelocity": 0.25005801471490263,
+            "velocityX": 1.028273808679041,
+            "velocityY": 1.0712386377846093,
+            "timestamp": 1.6188925199063697
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 9.313254165846089e-33,
+            "angularVelocity": 0.1341709954082624,
+            "velocityX": 0.5140557355952123,
+            "velocityY": 0.5355294002487934,
+            "timestamp": 1.7080343672370188
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": -1.1876173054192403e-31,
+            "velocityX": 4.5955148793706025e-32,
+            "velocityY": 0,
+            "timestamp": 1.797176214567668
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.1.traj
new file mode 100644
index 00000000..d933d22a
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.1.traj	
@@ -0,0 +1,184 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -2.2118032332823513e-27,
+            "velocityX": -2.2398078489200873e-28,
+            "velocityY": 7.32316842173409e-29,
+            "timestamp": 0
+        },
+        {
+            "x": 4.257275610783411,
+            "y": 2.9390850995701614,
+            "heading": -0.6153074756658753,
+            "angularVelocity": -0.5685220144659363,
+            "velocityX": -0.06600299944485946,
+            "velocityY": -0.7023572274929988,
+            "timestamp": 0.0867292284401611
+        },
+        {
+            "x": 4.259999029271463,
+            "y": 2.8165594202827866,
+            "heading": -0.6936339270516045,
+            "angularVelocity": -0.9031148183195571,
+            "velocityX": 0.03140139186100546,
+            "velocityY": -1.4127380295088343,
+            "timestamp": 0.1734584568803222
+        },
+        {
+            "x": 4.293815085802935,
+            "y": 2.6396142169455077,
+            "heading": -0.7636289676296801,
+            "angularVelocity": -0.8070524993355632,
+            "velocityX": 0.3899038091270867,
+            "velocityY": -2.0402026689233352,
+            "timestamp": 0.2601876853204833
+        },
+        {
+            "x": 4.379794167011224,
+            "y": 2.432035667571809,
+            "heading": -0.7730870281638328,
+            "angularVelocity": -0.10905274616477362,
+            "velocityX": 0.9913506986587586,
+            "velocityY": -2.3934093858210397,
+            "timestamp": 0.3469169137606444
+        },
+        {
+            "x": 4.522000800858502,
+            "y": 2.2059673134468256,
+            "heading": -0.7069342437539652,
+            "angularVelocity": 0.7627507542686203,
+            "velocityX": 1.6396621577856372,
+            "velocityY": -2.6065993920487904,
+            "timestamp": 0.43364614220080544
+        },
+        {
+            "x": 4.726803922559788,
+            "y": 1.9764479224139355,
+            "heading": -0.6474090008701758,
+            "angularVelocity": 0.6863342837743838,
+            "velocityX": 2.3614083208706247,
+            "velocityY": -2.6463903249323897,
+            "timestamp": 0.5203753706409665
+        },
+        {
+            "x": 4.985426543182091,
+            "y": 1.774560944536545,
+            "heading": -0.6474088034070749,
+            "angularVelocity": 0.000002276776866191791,
+            "velocityX": 2.9819545875556583,
+            "velocityY": -2.3277847792302735,
+            "timestamp": 0.6071045990811276
+        },
+        {
+            "x": 5.274431548584119,
+            "y": 1.6192543598542732,
+            "heading": -0.6474086941993331,
+            "angularVelocity": 0.000001259180368575944,
+            "velocityX": 3.3322676864516145,
+            "velocityY": -1.7907064028526984,
+            "timestamp": 0.6938338275212886
+        },
+        {
+            "x": 5.576835908979959,
+            "y": 1.4919922741488194,
+            "heading": -0.6474086295363626,
+            "angularVelocity": 7.455729944345581e-7,
+            "velocityX": 3.4867641028824017,
+            "velocityY": -1.4673494506325018,
+            "timestamp": 0.7805630559614497
+        },
+        {
+            "x": 5.888110144336631,
+            "y": 1.388298557512795,
+            "heading": -0.6474085736154427,
+            "angularVelocity": 6.447759407143774e-7,
+            "velocityX": 3.589034988031027,
+            "velocityY": -1.1956028953671916,
+            "timestamp": 0.8672922844016108
+        },
+        {
+            "x": 6.199390629137113,
+            "y": 1.2846236026664921,
+            "heading": -0.6474085176958322,
+            "angularVelocity": 6.447608435198244e-7,
+            "velocityX": 3.5891070449826046,
+            "velocityY": -1.1953865693365275,
+            "timestamp": 0.9540215128417718
+        },
+        {
+            "x": 6.510671944712167,
+            "y": 1.1809511422971397,
+            "heading": -0.6474084617763125,
+            "angularVelocity": 6.447597973785494e-7,
+            "velocityX": 3.589116623928252,
+            "velocityY": -1.1953578076724478,
+            "timestamp": 1.040750741281933
+        },
+        {
+            "x": 6.8219538378473334,
+            "y": 1.0772804160640193,
+            "heading": -0.6474084058668956,
+            "angularVelocity": 6.446433092317229e-7,
+            "velocityX": 3.5891232832762197,
+            "velocityY": -1.1953378128419065,
+            "timestamp": 1.127479969722094
+        },
+        {
+            "x": 7.104843996542823,
+            "y": 0.9795715918943764,
+            "heading": -0.5659567926753784,
+            "angularVelocity": 0.9391483662017565,
+            "velocityX": 3.2617626581409067,
+            "velocityY": -1.126596257420401,
+            "timestamp": 1.2142091981622551
+        },
+        {
+            "x": 7.336171547926519,
+            "y": 0.9014041069183023,
+            "heading": -0.36771202193482067,
+            "angularVelocity": 2.2857896271651064,
+            "velocityX": 2.6672386638755765,
+            "velocityY": -0.9012819136285574,
+            "timestamp": 1.3009384266024162
+        },
+        {
+            "x": 7.508580909570239,
+            "y": 0.8437450323938049,
+            "heading": -0.19262457792893875,
+            "angularVelocity": 2.018782446873579,
+            "velocityX": 1.9879037868147627,
+            "velocityY": -0.6648171044699364,
+            "timestamp": 1.3876676550425773
+        },
+        {
+            "x": 7.622996353926532,
+            "y": 0.8057914476404057,
+            "heading": -0.06643070405801102,
+            "angularVelocity": 1.4550328204291798,
+            "velocityX": 1.3192258989739947,
+            "velocityY": -0.4376100818144245,
+            "timestamp": 1.4743968834827383
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -1.2156690964354266e-28,
+            "angularVelocity": 0.7659552062526378,
+            "velocityX": 0.6572599237729437,
+            "velocityY": -0.21666799046150395,
+            "timestamp": 1.5611261119228994
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 1.25023472254267e-27,
+            "velocityX": 0,
+            "velocityY": -4.9030509543456705e-29,
+            "timestamp": 1.6478553403630605
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W1.1.traj
new file mode 100644
index 00000000..44ee68f9
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W1.1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -1.6794679447375943e-30,
+            "velocityX": 6.742046899995187e-33,
+            "velocityY": 9.708842875589211e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 4.290023963257714,
+            "y": 3.0497874441329906,
+            "heading": -0.5200869306801483,
+            "angularVelocity": 0.5503538158132694,
+            "velocityX": 0.3239326300245928,
+            "velocityY": 0.5967954280576505,
+            "timestamp": 0.08342464065958187
+        },
+        {
+            "x": 4.342730369898429,
+            "y": 3.150178003649595,
+            "heading": -0.42997936088427535,
+            "angularVelocity": 1.0801073769506666,
+            "velocityX": 0.6317846408926776,
+            "velocityY": 1.2033681982072046,
+            "timestamp": 0.16684928131916374
+        },
+        {
+            "x": 4.418537982231504,
+            "y": 3.3026029150734924,
+            "heading": -0.2990998156933407,
+            "angularVelocity": 1.5688355880967437,
+            "velocityX": 0.9086957010988046,
+            "velocityY": 1.8270970089745318,
+            "timestamp": 0.2502739219787456
+        },
+        {
+            "x": 4.5104462205151705,
+            "y": 3.5100667276192694,
+            "heading": -0.13617594937576347,
+            "angularVelocity": 1.9529465758491624,
+            "velocityX": 1.1016917490685052,
+            "velocityY": 2.4868409489750483,
+            "timestamp": 0.3336985626383275
+        },
+        {
+            "x": 4.573472263152817,
+            "y": 3.7674681923433693,
+            "heading": 0.004287761033867021,
+            "angularVelocity": 1.6837196935950753,
+            "velocityX": 0.7554847361563879,
+            "velocityY": 3.0854369007645883,
+            "timestamp": 0.41712320329790936
+        },
+        {
+            "x": 4.6071871310155705,
+            "y": 4.066840423815047,
+            "heading": 0.04705620405427594,
+            "angularVelocity": 0.5126596013152507,
+            "velocityX": 0.40413560785150865,
+            "velocityY": 3.5885348633778387,
+            "timestamp": 0.5005478439574912
+        },
+        {
+            "x": 4.587417601167403,
+            "y": 4.370555626705338,
+            "heading": 0.08458578048595149,
+            "angularVelocity": 0.4498620088136386,
+            "velocityX": -0.23697470785444333,
+            "velocityY": 3.6405934804036506,
+            "timestamp": 0.5839724846170731
+        },
+        {
+            "x": 4.510022832963655,
+            "y": 4.67116829443229,
+            "heading": 0.1002321163817716,
+            "angularVelocity": 0.1875505338964035,
+            "velocityX": -0.9277207260569706,
+            "velocityY": 3.603403806719601,
+            "timestamp": 0.667397125276655
+        },
+        {
+            "x": 4.376634060333898,
+            "y": 4.957183607465761,
+            "heading": 0.10023219176740035,
+            "angularVelocity": 9.036374403612271e-7,
+            "velocityX": -1.5989133615097586,
+            "velocityY": 3.4284272700744434,
+            "timestamp": 0.7508217659362368
+        },
+        {
+            "x": 4.19313075849175,
+            "y": 5.213940045953902,
+            "heading": 0.10023221692184411,
+            "angularVelocity": 3.015229499928316e-7,
+            "velocityX": -2.1996295146291525,
+            "velocityY": 3.0777050576201734,
+            "timestamp": 0.8342464065958187
+        },
+        {
+            "x": 3.9657176811920825,
+            "y": 5.432756463781841,
+            "heading": 0.10023223856547658,
+            "angularVelocity": 2.5943932502769966e-7,
+            "velocityX": -2.725970115084308,
+            "velocityY": 2.6229231087830502,
+            "timestamp": 0.9176710472554006
+        },
+        {
+            "x": 3.7186426713834653,
+            "y": 5.629101242954768,
+            "heading": 0.10023225962798245,
+            "angularVelocity": 2.524734383899783e-7,
+            "velocityX": -2.961655068037016,
+            "velocityY": 2.3535585843770224,
+            "timestamp": 1.0010956879149824
+        },
+        {
+            "x": 3.4715770348675625,
+            "y": 5.825457816649942,
+            "heading": 0.10023228069061084,
+            "angularVelocity": 2.5247490699416256e-7,
+            "velocityX": -2.961542711631996,
+            "velocityY": 2.353699963736333,
+            "timestamp": 1.0845203285745642
+        },
+        {
+            "x": 3.224511384705754,
+            "y": 6.021814373175139,
+            "heading": 0.10023230175325984,
+            "angularVelocity": 2.5247515406639525e-7,
+            "velocityX": -2.96154287520364,
+            "velocityY": 2.353699757922098,
+            "timestamp": 1.167944969234146
+        },
+        {
+            "x": 2.9774457295747583,
+            "y": 6.218170923447359,
+            "heading": 0.10023232281654228,
+            "angularVelocity": 2.524827469327523e-7,
+            "velocityX": -2.9615429347686173,
+            "velocityY": 2.3536996829685104,
+            "timestamp": 1.2513696098937277
+        },
+        {
+            "x": 2.751412346836958,
+            "y": 6.39797212959724,
+            "heading": 0.1658210049038553,
+            "angularVelocity": 0.7862027521934517,
+            "velocityX": -2.7094319010631414,
+            "velocityY": 2.155252989144637,
+            "timestamp": 1.3347942505533095
+        },
+        {
+            "x": 2.5706626829115913,
+            "y": 6.54172747182469,
+            "heading": 0.24638520673123038,
+            "angularVelocity": 0.9657123026291593,
+            "velocityX": -2.166622025534686,
+            "velocityY": 1.723176043563091,
+            "timestamp": 1.4182188912128912
+        },
+        {
+            "x": 2.435257350873027,
+            "y": 6.649419264379792,
+            "heading": 0.31518856404188306,
+            "angularVelocity": 0.8247366337651717,
+            "velocityX": -1.6230855891977036,
+            "velocityY": 1.2908871012647558,
+            "timestamp": 1.501643531872473
+        },
+        {
+            "x": 2.3450659538089185,
+            "y": 6.721154353305962,
+            "heading": 0.3643262231924431,
+            "angularVelocity": 0.5890065424563048,
+            "velocityX": -1.0811122031935256,
+            "velocityY": 0.8598789081859941,
+            "timestamp": 1.5850681725320548
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 0.3077481257883978,
+            "velocityX": -0.5401995555822935,
+            "velocityY": 0.42967696846676384,
+            "timestamp": 1.6684928131916366
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": -2.8369790280137563e-29,
+            "velocityX": 0,
+            "velocityY": -1.9155196850548007e-31,
+            "timestamp": 1.7519174538512183
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.1.traj
new file mode 100644
index 00000000..01699583
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.1.traj	
@@ -0,0 +1,157 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.1175745999315144e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 4.331197427717942,
+            "y": 3.064203342806604,
+            "heading": -0.5918486780870994,
+            "angularVelocity": -0.24352382070717674,
+            "velocityX": 0.642497001367488,
+            "velocityY": 0.604868198278966,
+            "timestamp": 0.10614435176007272
+        },
+        {
+            "x": 4.457228812945604,
+            "y": 3.20188389088458,
+            "heading": -0.6500618235234895,
+            "angularVelocity": -0.5484337552691863,
+            "velocityX": 1.1873583769444531,
+            "velocityY": 1.2971066834454588,
+            "timestamp": 0.21228870352014545
+        },
+        {
+            "x": 4.610677951553499,
+            "y": 3.4285671398637003,
+            "heading": -0.7523465196948876,
+            "angularVelocity": -0.9636376733695737,
+            "velocityX": 1.4456646638603075,
+            "velocityY": 2.1356129197671536,
+            "timestamp": 0.31843305528021815
+        },
+        {
+            "x": 4.708912918877337,
+            "y": 3.730071149255109,
+            "heading": -0.9027593729452231,
+            "angularVelocity": -1.4170594172577993,
+            "velocityX": 0.9254846413861598,
+            "velocityY": 2.840509215911215,
+            "timestamp": 0.4245774070402909
+        },
+        {
+            "x": 4.717930123488916,
+            "y": 4.059451821332611,
+            "heading": -1.0196863730682848,
+            "angularVelocity": -1.1015847587195386,
+            "velocityX": 0.08495227925044571,
+            "velocityY": 3.10313894819415,
+            "timestamp": 0.5307217588003637
+        },
+        {
+            "x": 4.639893034223715,
+            "y": 4.389369622514019,
+            "heading": -1.0106655427651627,
+            "angularVelocity": 0.08498643737080536,
+            "velocityX": -0.7351977563685651,
+            "velocityY": 3.1081993126412395,
+            "timestamp": 0.6368661105604364
+        },
+        {
+            "x": 4.471904603111427,
+            "y": 4.710090421849215,
+            "heading": -0.9107146957571205,
+            "angularVelocity": 0.9416501712118399,
+            "velocityX": -1.5826412647185075,
+            "velocityY": 3.021553139823678,
+            "timestamp": 0.7430104623205092
+        },
+        {
+            "x": 4.218561716713538,
+            "y": 4.992188206454831,
+            "heading": -0.8355436069817835,
+            "angularVelocity": 0.7081967860640641,
+            "velocityX": -2.3867768957743736,
+            "velocityY": 2.657680601255776,
+            "timestamp": 0.8491548140805819
+        },
+        {
+            "x": 3.8896224481808446,
+            "y": 5.222473200709689,
+            "heading": -0.83554222765338,
+            "angularVelocity": 0.000012994835623464205,
+            "velocityX": -3.098980426921093,
+            "velocityY": 2.169545439171289,
+            "timestamp": 0.9552991658406547
+        },
+        {
+            "x": 3.516178298161353,
+            "y": 5.370026719708121,
+            "heading": -0.8355421446101063,
+            "angularVelocity": 7.823616840140413e-7,
+            "velocityX": -3.518266811442025,
+            "velocityY": 1.390121250464274,
+            "timestamp": 1.0614435176007273
+        },
+        {
+            "x": 3.1189612059977283,
+            "y": 5.42877328608909,
+            "heading": -0.8355420737846555,
+            "angularVelocity": 6.672559542292647e-7,
+            "velocityX": -3.742234848835754,
+            "velocityY": 0.55345918465599,
+            "timestamp": 1.1675878693608
+        },
+        {
+            "x": 2.774416222135212,
+            "y": 5.484170620762628,
+            "heading": -0.7035735462776811,
+            "angularVelocity": 1.243292980914089,
+            "velocityX": -3.2460039384980304,
+            "velocityY": 0.5219056290320314,
+            "timestamp": 1.2737322211208726
+        },
+        {
+            "x": 2.5095166350816602,
+            "y": 5.523131311595933,
+            "heading": -0.3884677902217159,
+            "angularVelocity": 2.9686530732056857,
+            "velocityX": -2.4956541036901063,
+            "velocityY": 0.3670538298766209,
+            "timestamp": 1.3798765728809452
+        },
+        {
+            "x": 2.335800273937377,
+            "y": 5.547984865708467,
+            "heading": -0.13742924633332027,
+            "angularVelocity": 2.365067379711733,
+            "velocityX": -1.6366048523895966,
+            "velocityY": 0.23414862590816604,
+            "timestamp": 1.4860209246410179
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 1.2947391364164484,
+            "velocityX": -0.8083357476365635,
+            "velocityY": 0.11319617193282665,
+            "timestamp": 1.5921652764010905
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": -1.986468774957569e-34,
+            "velocityY": 0,
+            "timestamp": 1.6983096281611632
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.1.traj
new file mode 100644
index 00000000..26f24eb2
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.1.traj	
@@ -0,0 +1,121 @@
+{
+    "samples": [
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 0,
+            "velocityX": -3.809622996347125e-33,
+            "velocityY": 4.7029981856297106e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 4.1621681366579715,
+            "y": 2.9362991956112916,
+            "heading": -0.5618109581410129,
+            "angularVelocity": 0.03502487973446616,
+            "velocityX": -0.8430624486077395,
+            "velocityY": -0.5326069988815002,
+            "timestamp": 0.11960189130537771
+        },
+        {
+            "x": 3.949371568032143,
+            "y": 2.832059073063443,
+            "heading": -0.5332490097615643,
+            "angularVelocity": 0.2388085010004113,
+            "velocityX": -1.7792073879709687,
+            "velocityY": -0.8715591485229383,
+            "timestamp": 0.23920378261075542
+        },
+        {
+            "x": 3.6196334559490815,
+            "y": 2.7428786025637635,
+            "heading": -0.4400400521459035,
+            "angularVelocity": 0.7793267865446122,
+            "velocityX": -2.756964028613438,
+            "velocityY": -0.7456443165432319,
+            "timestamp": 0.35880567391613316
+        },
+        {
+            "x": 3.2124362068162218,
+            "y": 2.7443781288173086,
+            "heading": -0.34020495702705544,
+            "angularVelocity": 0.8347283979314292,
+            "velocityX": -3.4046054346513173,
+            "velocityY": 0.012537646664080927,
+            "timestamp": 0.47840756522151084
+        },
+        {
+            "x": 2.829910158877178,
+            "y": 2.8625567119312745,
+            "heading": -0.23363822858373923,
+            "angularVelocity": 0.891012067453128,
+            "velocityX": -3.1983277502054186,
+            "velocityY": 0.9880996180254248,
+            "timestamp": 0.5980094565268885
+        },
+        {
+            "x": 2.5228974587582895,
+            "y": 3.073051960500077,
+            "heading": -0.12255766699698727,
+            "angularVelocity": 0.9287525504349555,
+            "velocityX": -2.566955227614215,
+            "velocityY": 1.7599658857512914,
+            "timestamp": 0.7176113478322662
+        },
+        {
+            "x": 2.307806503132395,
+            "y": 3.359545759981809,
+            "heading": -0.008191563913849259,
+            "angularVelocity": 0.9562231987710198,
+            "velocityX": -1.7983909224036065,
+            "velocityY": 2.395395226236259,
+            "timestamp": 0.8372132391376439
+        },
+        {
+            "x": 2.197079056424326,
+            "y": 3.692587049160117,
+            "heading": -0.017161158826022753,
+            "angularVelocity": -0.07499542703130056,
+            "velocityX": -0.9258001315827844,
+            "velocityY": 2.7845821294578066,
+            "timestamp": 0.9568151304430216
+        },
+        {
+            "x": 2.2005818293568207,
+            "y": 4.024472502975606,
+            "heading": -0.15274865723996348,
+            "angularVelocity": -1.133656808718376,
+            "velocityX": 0.029286935969546842,
+            "velocityY": 2.774918107006309,
+            "timestamp": 1.0764170217483993
+        },
+        {
+            "x": 2.2590478504225837,
+            "y": 4.250579625353832,
+            "heading": -0.3027008584759083,
+            "angularVelocity": -1.2537611203242134,
+            "velocityX": 0.48883859968803833,
+            "velocityY": 1.8904978835235247,
+            "timestamp": 1.196018913053777
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.729914389908579,
+            "velocityX": 0.3424038627687911,
+            "velocityY": 0.914871608232253,
+            "timestamp": 1.3156208043591548
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 3.1982065543283105e-28,
+            "velocityX": 4.1170744495691044e-30,
+            "velocityY": 1.8090480101607624e-30,
+            "timestamp": 1.4352226956645326
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C1.1.traj
new file mode 100644
index 00000000..f8bd91fc
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C1.1.traj	
@@ -0,0 +1,292 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 1.1411819845129358e-30,
+            "velocityX": 2.0257584812603577e-32,
+            "velocityY": 3.042151659469263e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 5.128157808100627,
+            "y": 1.6381505638691025,
+            "heading": -0.6351424860741187,
+            "angularVelocity": 0.06199634399134786,
+            "velocityX": -0.6106088487961494,
+            "velocityY": 0.23165524966799825,
+            "timestamp": 0.07835161903352221
+        },
+        {
+            "x": 5.034138141125347,
+            "y": 1.6783506940439978,
+            "heading": -0.625731099901857,
+            "angularVelocity": 0.12011731586854772,
+            "velocityX": -1.199970953185457,
+            "velocityY": 0.5130733821555872,
+            "timestamp": 0.15670323806704442
+        },
+        {
+            "x": 4.897649658060979,
+            "y": 1.7471051706265737,
+            "heading": -0.6123788413002932,
+            "angularVelocity": 0.1704145844880519,
+            "velocityX": -1.74199952404266,
+            "velocityY": 0.8775118808095047,
+            "timestamp": 0.23505485710056662
+        },
+        {
+            "x": 4.7291295198915435,
+            "y": 1.855780594555545,
+            "heading": -0.5966040827715752,
+            "angularVelocity": 0.20133289807283902,
+            "velocityX": -2.150818837544829,
+            "velocityY": 1.3870220586313022,
+            "timestamp": 0.31340647613408884
+        },
+        {
+            "x": 4.559409610033689,
+            "y": 2.0156280635815147,
+            "heading": -0.5822183456142529,
+            "angularVelocity": 0.18360484869071408,
+            "velocityX": -2.1661314973624393,
+            "velocityY": 2.0401297509574126,
+            "timestamp": 0.39175809516761106
+        },
+        {
+            "x": 4.423281260574217,
+            "y": 2.2140742145732073,
+            "heading": -0.5727758831618049,
+            "angularVelocity": 0.1205139417528553,
+            "velocityX": -1.7374031467203144,
+            "velocityY": 2.532763884646578,
+            "timestamp": 0.4701097142011333
+        },
+        {
+            "x": 4.3324586171803325,
+            "y": 2.4362881098121476,
+            "heading": -0.5691499320560302,
+            "angularVelocity": 0.04627793465535804,
+            "velocityX": -1.1591674111421555,
+            "velocityY": 2.8361110846205655,
+            "timestamp": 0.5484613332346555
+        },
+        {
+            "x": 4.290356765831772,
+            "y": 2.674111890908862,
+            "heading": -0.5714790836196856,
+            "angularVelocity": -0.02972691046318988,
+            "velocityX": -0.5373450079001806,
+            "velocityY": 3.0353397164002804,
+            "timestamp": 0.6268129522681777
+        },
+        {
+            "x": 4.298268630722052,
+            "y": 2.9211188280591673,
+            "heading": -0.5947192771376703,
+            "angularVelocity": -0.29661408155511804,
+            "velocityX": 0.1009789585444941,
+            "velocityY": 3.1525441362561364,
+            "timestamp": 0.7051645713016998
+        },
+        {
+            "x": 4.356924477851345,
+            "y": 3.1722484967201265,
+            "heading": -0.6368890523591307,
+            "angularVelocity": -0.5382119188043637,
+            "velocityX": 0.7486232939768359,
+            "velocityY": 3.2051624683532776,
+            "timestamp": 0.783516190335222
+        },
+        {
+            "x": 4.466386404708156,
+            "y": 3.418800428561669,
+            "heading": -0.694172117002664,
+            "angularVelocity": -0.7311025011368958,
+            "velocityX": 1.3970601782967371,
+            "velocityY": 3.1467369134523744,
+            "timestamp": 0.8618678093687442
+        },
+        {
+            "x": 4.622742851027307,
+            "y": 3.644917946871822,
+            "heading": -0.7581397869413499,
+            "angularVelocity": -0.8164179723116849,
+            "velocityX": 1.9955739045067467,
+            "velocityY": 2.8859329404975034,
+            "timestamp": 0.9402194284022664
+        },
+        {
+            "x": 4.8130531116785615,
+            "y": 3.833426042269654,
+            "heading": -0.8482959054101304,
+            "angularVelocity": -1.1506605681014435,
+            "velocityX": 2.4289256941816704,
+            "velocityY": 2.4059246984696085,
+            "timestamp": 1.0185710474357885
+        },
+        {
+            "x": 5.034329891204834,
+            "y": 3.981222629547119,
+            "heading": -0.9454274313666489,
+            "angularVelocity": -1.2396875412997066,
+            "velocityX": 2.824150697276595,
+            "velocityY": 1.8863246107809373,
+            "timestamp": 1.0969226664693108
+        },
+        {
+            "x": 5.3307679727813255,
+            "y": 4.095215339701575,
+            "heading": -1.0325645415374454,
+            "angularVelocity": -0.9491279076997933,
+            "velocityX": 3.2289073573561264,
+            "velocityY": 1.241648503948074,
+            "timestamp": 1.1887302189223545
+        },
+        {
+            "x": 5.6706736122505745,
+            "y": 4.158722341660029,
+            "heading": -1.0366531166779425,
+            "angularVelocity": -0.04453419170049298,
+            "velocityX": 3.7023712144281196,
+            "velocityY": 0.691740496958957,
+            "timestamp": 1.2805377713753983
+        },
+        {
+            "x": 5.985432145120126,
+            "y": 4.2874622186845945,
+            "heading": -1.0150469515971576,
+            "angularVelocity": 0.23534191363870294,
+            "velocityX": 3.4284601262030105,
+            "velocityY": 1.402279807975625,
+            "timestamp": 1.372345323828442
+        },
+        {
+            "x": 6.260473058973373,
+            "y": 4.473603429076733,
+            "heading": -0.9641067919300658,
+            "angularVelocity": 0.5548580515001295,
+            "velocityX": 2.9958419160985765,
+            "velocityY": 2.027515225257119,
+            "timestamp": 1.4641528762814857
+        },
+        {
+            "x": 6.487005993334624,
+            "y": 4.6803674717401,
+            "heading": -0.8309910438969466,
+            "angularVelocity": 1.4499433268434323,
+            "velocityX": 2.4674760224886074,
+            "velocityY": 2.2521463337029823,
+            "timestamp": 1.5559604287345294
+        },
+        {
+            "x": 6.71888754498383,
+            "y": 4.9198952355114685,
+            "heading": -0.7843851972685187,
+            "angularVelocity": 0.507647196588375,
+            "velocityX": 2.5257350343568326,
+            "velocityY": 2.609020253468568,
+            "timestamp": 1.6477679811875732
+        },
+        {
+            "x": 6.906293835583718,
+            "y": 5.2122954621107285,
+            "heading": -0.7843850302000089,
+            "angularVelocity": 0.0000018197686939661552,
+            "velocityX": 2.0412949217411986,
+            "velocityY": 3.1849256274293083,
+            "timestamp": 1.739575533640617
+        },
+        {
+            "x": 7.030998336898649,
+            "y": 5.536437116039629,
+            "heading": -0.7843849409482478,
+            "angularVelocity": 9.721614257680115e-7,
+            "velocityX": 1.3583250830993825,
+            "velocityY": 3.530664365490929,
+            "timestamp": 1.8313830860936606
+        },
+        {
+            "x": 7.139916048661206,
+            "y": 5.86621879705104,
+            "heading": -0.7843848550588985,
+            "angularVelocity": 9.355368597772783e-7,
+            "velocityX": 1.1863698448802928,
+            "velocityY": 3.592097514854049,
+            "timestamp": 1.9231906385467044
+        },
+        {
+            "x": 7.248833621341376,
+            "y": 6.196000523997438,
+            "heading": -0.7843847691695015,
+            "angularVelocity": 9.355373786921671e-7,
+            "velocityX": 1.1863683299462482,
+            "velocityY": 3.592098015194004,
+            "timestamp": 2.0149981909997483
+        },
+        {
+            "x": 7.357751176995467,
+            "y": 6.5257822565670125,
+            "heading": -0.7843846832799689,
+            "angularVelocity": 9.355388566444766e-7,
+            "velocityX": 1.1863681444922285,
+            "velocityY": 3.5920980764436137,
+            "timestamp": 2.1068057434527923
+        },
+        {
+            "x": 7.4669549324183055,
+            "y": 6.834400664448086,
+            "heading": -0.7318192896940902,
+            "angularVelocity": 0.5725606682823186,
+            "velocityX": 1.189485532562175,
+            "velocityY": 3.361579735380924,
+            "timestamp": 2.198613295905836
+        },
+        {
+            "x": 7.555413306559794,
+            "y": 7.089894078912264,
+            "heading": -0.5139853289463429,
+            "angularVelocity": 2.3727237566774426,
+            "velocityX": 0.9635195773978621,
+            "velocityY": 2.782923709842436,
+            "timestamp": 2.29042084835888
+        },
+        {
+            "x": 7.619158448834915,
+            "y": 7.279787689667393,
+            "heading": -0.2782484542424907,
+            "angularVelocity": 2.5677285626846773,
+            "velocityX": 0.6943344046528581,
+            "velocityY": 2.0683876835977384,
+            "timestamp": 2.382228400811924
+        },
+        {
+            "x": 7.660128944224933,
+            "y": 7.404974004983973,
+            "heading": -0.09793823848426357,
+            "angularVelocity": 1.964001990472943,
+            "velocityX": 0.44626497815605787,
+            "velocityY": 1.3635731698719276,
+            "timestamp": 2.474035953264968
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 2.533782828853193e-32,
+            "angularVelocity": 1.0667775783953661,
+            "velocityX": 0.21644249567844834,
+            "velocityY": 0.6756088508922024,
+            "timestamp": 2.565843505718012
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 0,
+            "angularVelocity": 2.4912680070874024e-31,
+            "velocityX": 0,
+            "velocityY": -3.645397861822831e-32,
+            "timestamp": 2.657651058171056
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C2.1.traj
new file mode 100644
index 00000000..0e0e8d38
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C2.1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -2.491480567253294e-33,
+            "velocityX": -2.0103104784295923e-34,
+            "velocityY": 1.605618083193578e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 5.240158813928264,
+            "y": 1.6204190327819517,
+            "heading": -0.5237338108165747,
+            "angularVelocity": 1.2526389382742513,
+            "velocityX": 0.6912398963489377,
+            "velocityY": 0.004514612397400031,
+            "timestamp": 0.0928170006782674
+        },
+        {
+            "x": 5.369626464101249,
+            "y": 1.6247166229091008,
+            "heading": -0.2974184918320366,
+            "angularVelocity": 2.4382959730515035,
+            "velocityX": 1.394870004707003,
+            "velocityY": 0.046301756097956205,
+            "timestamp": 0.1856340013565348
+        },
+        {
+            "x": 5.5674551093283755,
+            "y": 1.639197763374398,
+            "heading": 0.0009013327241277373,
+            "angularVelocity": 3.214064475001015,
+            "velocityX": 2.1313837312289596,
+            "velocityY": 0.15601819019657054,
+            "timestamp": 0.2784510020348022
+        },
+        {
+            "x": 5.831585704851482,
+            "y": 1.6774487923340957,
+            "heading": 0.2480008567342784,
+            "angularVelocity": 2.6622226769282764,
+            "velocityX": 2.8457135394696165,
+            "velocityY": 0.41211231434085943,
+            "timestamp": 0.3712680027130696
+        },
+        {
+            "x": 6.141513989428359,
+            "y": 1.75469627200855,
+            "heading": 0.3453177753240257,
+            "angularVelocity": 1.0484816130514494,
+            "velocityX": 3.339132726893265,
+            "velocityY": 0.8322557194259916,
+            "timestamp": 0.46408500339133696
+        },
+        {
+            "x": 6.452443961209774,
+            "y": 1.9008832260175597,
+            "heading": 0.36957097138497697,
+            "angularVelocity": 0.2613012258930921,
+            "velocityX": 3.3499247929718696,
+            "velocityY": 1.5750019171136467,
+            "timestamp": 0.5569020040696043
+        },
+        {
+            "x": 6.733109637262523,
+            "y": 2.111863899724239,
+            "heading": 0.36957109822659895,
+            "angularVelocity": 0.0000013665774702958542,
+            "velocityX": 3.023860650546378,
+            "velocityY": 2.273082217319261,
+            "timestamp": 0.6497190047478717
+        },
+        {
+            "x": 6.964965499466835,
+            "y": 2.3755472934997988,
+            "heading": 0.36957101579118784,
+            "angularVelocity": -8.881499138395027e-7,
+            "velocityX": 2.4979891669630336,
+            "velocityY": 2.840895437782658,
+            "timestamp": 0.7425360054261391
+        },
+        {
+            "x": 7.138307566970259,
+            "y": 2.680897277378083,
+            "heading": 0.36957096673523665,
+            "angularVelocity": -5.285233395413412e-7,
+            "velocityX": 1.867568077364202,
+            "velocityY": 3.289806626446832,
+            "timestamp": 0.8353530061044064
+        },
+        {
+            "x": 7.245881008413397,
+            "y": 3.0151337865477195,
+            "heading": 0.3695709277774119,
+            "angularVelocity": -4.197272531605919e-7,
+            "velocityX": 1.1589842448800958,
+            "velocityY": 3.6010268240427665,
+            "timestamp": 0.9281700067826738
+        },
+        {
+            "x": 7.300266060372569,
+            "y": 3.36201756600148,
+            "heading": 0.3695708905369613,
+            "angularVelocity": -4.012244559973519e-7,
+            "velocityX": 0.5859384763755527,
+            "velocityY": 3.7372871017042186,
+            "timestamp": 1.0209870074609413
+        },
+        {
+            "x": 7.354650396010165,
+            "y": 3.708901457760651,
+            "heading": 0.3695708532965109,
+            "angularVelocity": -4.0122445392101915e-7,
+            "velocityX": 0.5859307588068667,
+            "velocityY": 3.7372883116701696,
+            "timestamp": 1.1138040081392087
+        },
+        {
+            "x": 7.409034640641628,
+            "y": 4.055785363787717,
+            "heading": 0.36957081605605097,
+            "angularVelocity": -4.012245569815441e-7,
+            "velocityX": 0.5859297783169686,
+            "velocityY": 3.737288465390881,
+            "timestamp": 1.206621008817476
+        },
+        {
+            "x": 7.463418795891099,
+            "y": 4.402669283828022,
+            "heading": 0.36957077881557854,
+            "angularVelocity": -4.0122469066112833e-7,
+            "velocityX": 0.5859288153253637,
+            "velocityY": 3.7372886163679513,
+            "timestamp": 1.2994380094957434
+        },
+        {
+            "x": 7.517802854177964,
+            "y": 4.7495532190700205,
+            "heading": 0.3695707415750949,
+            "angularVelocity": -4.0122481249990114e-7,
+            "velocityX": 0.5859277706610763,
+            "velocityY": 3.737288780149305,
+            "timestamp": 1.3922550101740108
+        },
+        {
+            "x": 7.56989445358868,
+            "y": 5.095450165697103,
+            "heading": 0.36620989603022114,
+            "angularVelocity": -0.036209374579162214,
+            "velocityX": 0.5612290747390308,
+            "velocityY": 3.7266550750338174,
+            "timestamp": 1.4850720108522781
+        },
+        {
+            "x": 7.614129631330889,
+            "y": 5.376850109081837,
+            "heading": 0.24917441522367106,
+            "angularVelocity": -1.2609272003114111,
+            "velocityX": 0.4765848650458117,
+            "velocityY": 3.0317715647821197,
+            "timestamp": 1.5778890115305455
+        },
+        {
+            "x": 7.6471559939174325,
+            "y": 5.587299345919279,
+            "heading": 0.1342077337923999,
+            "angularVelocity": -1.2386381868746383,
+            "velocityX": 0.35582234229936965,
+            "velocityY": 2.267356575838128,
+            "timestamp": 1.6707060122088129
+        },
+        {
+            "x": 7.669079055841979,
+            "y": 5.72720580380319,
+            "heading": 0.04709984861251457,
+            "angularVelocity": -0.9384906271840042,
+            "velocityX": 0.23619662092442378,
+            "velocityY": 1.5073365532341503,
+            "timestamp": 1.7635230128870802
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -9.59663427624948e-34,
+            "angularVelocity": -0.5074485090913172,
+            "velocityX": 0.11766103276571077,
+            "velocityY": 0.751954875580805,
+            "timestamp": 1.8563400135653476
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 0,
+            "angularVelocity": 1.9250732473517817e-32,
+            "velocityX": 0,
+            "velocityY": 2.1529996497623123e-34,
+            "timestamp": 1.949157014243615
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3.1.traj
new file mode 100644
index 00000000..4d225c1e
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3.1.traj	
@@ -0,0 +1,238 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 0,
+            "velocityX": 2.653642496841617e-31,
+            "velocityY": -1.2761459725684476e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 5.194122324075795,
+            "y": 1.6116215778070426,
+            "heading": -0.6335243646936302,
+            "angularVelocity": 0.13206571622689564,
+            "velocityX": 0.3695911822570569,
+            "velocityY": -0.1708716250073132,
+            "timestamp": 0.049033431926404414
+        },
+        {
+            "x": 5.2306889078335095,
+            "y": 1.5955898422898989,
+            "heading": -0.6207226842720189,
+            "angularVelocity": 0.2610806529090014,
+            "velocityX": 0.745747999295838,
+            "velocityY": -0.3269551994890449,
+            "timestamp": 0.09806686385280883
+        },
+        {
+            "x": 5.286073830286703,
+            "y": 1.5728623577205654,
+            "heading": -0.6018022859553105,
+            "angularVelocity": 0.38586730672362957,
+            "velocityX": 1.1295338767299938,
+            "velocityY": -0.4635099701654054,
+            "timestamp": 0.14710029577921324
+        },
+        {
+            "x": 5.360700050095615,
+            "y": 1.5447484727758143,
+            "heading": -0.577064216904969,
+            "angularVelocity": 0.5045143298894789,
+            "velocityX": 1.5219456782249914,
+            "velocityY": -0.5733615584348081,
+            "timestamp": 0.19613372770561766
+        },
+        {
+            "x": 5.45500431506412,
+            "y": 1.513111103841015,
+            "heading": -0.5469692052395991,
+            "angularVelocity": 0.6137651492712723,
+            "velocityX": 1.9232646230039372,
+            "velocityY": -0.6452203668359155,
+            "timestamp": 0.24516715963202207
+        },
+        {
+            "x": 5.5692993756365,
+            "y": 1.480706991826158,
+            "heading": -0.5122625268421303,
+            "angularVelocity": 0.7078166270271339,
+            "velocityX": 2.330961878090668,
+            "velocityY": -0.6608575158164777,
+            "timestamp": 0.2942005915584265
+        },
+        {
+            "x": 5.703331364670941,
+            "y": 1.4516882815399292,
+            "heading": -0.47420565589215874,
+            "angularVelocity": 0.7761412867722233,
+            "velocityX": 2.733481703578104,
+            "velocityY": -0.5918147913809916,
+            "timestamp": 0.3432340234848309
+        },
+        {
+            "x": 5.855159622936256,
+            "y": 1.4318923943368613,
+            "heading": -0.4349045247507558,
+            "angularVelocity": 0.8015170384282412,
+            "velocityX": 3.096423242273411,
+            "velocityY": -0.40372224470801127,
+            "timestamp": 0.3922674554112353
+        },
+        {
+            "x": 6.019815732483991,
+            "y": 1.4274954608427641,
+            "heading": -0.3972296573283927,
+            "angularVelocity": 0.768350611865634,
+            "velocityX": 3.358037630221609,
+            "velocityY": -0.08967215471871418,
+            "timestamp": 0.4413008873376397
+        },
+        {
+            "x": 6.190507691154086,
+            "y": 1.4421792499476667,
+            "heading": -0.36375877276285506,
+            "angularVelocity": 0.6826135404074718,
+            "velocityX": 3.4811342376834093,
+            "velocityY": 0.2994648452702519,
+            "timestamp": 0.49033431926404414
+        },
+        {
+            "x": 6.361514712702491,
+            "y": 1.4768435219754552,
+            "heading": -0.33601787986089265,
+            "angularVelocity": 0.5657546659914753,
+            "velocityX": 3.487559708345244,
+            "velocityY": 0.7069517809772359,
+            "timestamp": 0.5393677511904486
+        },
+        {
+            "x": 6.529238224029541,
+            "y": 1.531233787536621,
+            "heading": -0.3133159497925571,
+            "angularVelocity": 0.4629888052692109,
+            "velocityX": 3.4205949846286416,
+            "velocityY": 1.1092485968081267,
+            "timestamp": 0.588401183116853
+        },
+        {
+            "x": 6.794330666624461,
+            "y": 1.6764087741891647,
+            "heading": -0.29089151826229404,
+            "angularVelocity": 0.27241251742532613,
+            "velocityX": 3.2203491776466797,
+            "velocityY": 1.7635891250039288,
+            "timestamp": 0.6707191020664587
+        },
+        {
+            "x": 7.034738463760469,
+            "y": 1.8721289187979775,
+            "heading": -0.28740556521383775,
+            "angularVelocity": 0.042347438965477144,
+            "velocityX": 2.9204795286821774,
+            "velocityY": 2.377612883150898,
+            "timestamp": 0.7530370210160644
+        },
+        {
+            "x": 7.236654527584481,
+            "y": 2.109198968129657,
+            "heading": -0.2874055261243408,
+            "angularVelocity": 4.748601214403234e-7,
+            "velocityX": 2.452881054338489,
+            "velocityY": 2.879932490479633,
+            "timestamp": 0.8353549399656701
+        },
+        {
+            "x": 7.392410351066591,
+            "y": 2.3788514084218195,
+            "heading": -0.2874055078665042,
+            "angularVelocity": 2.2179662469221753e-7,
+            "velocityX": 1.8921253776776576,
+            "velocityY": 3.2757441360636306,
+            "timestamp": 0.9176728589152758
+        },
+        {
+            "x": 7.49687844031107,
+            "y": 2.6722091028580075,
+            "heading": -0.28740549372573876,
+            "angularVelocity": 1.7178234776742224e-7,
+            "velocityX": 1.2690807855393922,
+            "velocityY": 3.5637161164844247,
+            "timestamp": 0.9999907778648816
+        },
+        {
+            "x": 7.546620338444338,
+            "y": 2.979614475892834,
+            "heading": -0.28740548096651763,
+            "angularVelocity": 1.5499931641741705e-7,
+            "velocityX": 0.6042657390764874,
+            "velocityY": 3.7343676438541644,
+            "timestamp": 1.0823086968144873
+        },
+        {
+            "x": 7.582493232504898,
+            "y": 3.288945132948743,
+            "heading": -0.2874054683444637,
+            "angularVelocity": 1.5333300623146833e-7,
+            "velocityX": 0.43578475401492967,
+            "velocityY": 3.7577560390622633,
+            "timestamp": 1.164626615764093
+        },
+        {
+            "x": 7.6150468277369106,
+            "y": 3.5683420563080905,
+            "heading": -0.1936159509616472,
+            "angularVelocity": 1.1393572454157397,
+            "velocityX": 0.39546183440153926,
+            "velocityY": 3.3941203437178378,
+            "timestamp": 1.2469445347136987
+        },
+        {
+            "x": 7.641044736890772,
+            "y": 3.791820131624945,
+            "heading": -0.11680359430817472,
+            "angularVelocity": 0.9331183007718732,
+            "velocityX": 0.31582320697043825,
+            "velocityY": 2.714816873027222,
+            "timestamp": 1.3292624536633044
+        },
+        {
+            "x": 7.660527962829294,
+            "y": 3.959415202649684,
+            "heading": -0.05861701943494889,
+            "angularVelocity": 0.7068518691380767,
+            "velocityX": 0.23668268327338735,
+            "velocityY": 2.035948833052459,
+            "timestamp": 1.4115803726129101
+        },
+        {
+            "x": 7.673510674167818,
+            "y": 4.0711397209924485,
+            "heading": -0.019593791766857216,
+            "angularVelocity": 0.4740550801809447,
+            "velocityX": 0.1577142802460941,
+            "velocityY": 1.3572320555281903,
+            "timestamp": 1.4938982915625159
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -2.071552209824535e-33,
+            "angularVelocity": 0.23802583953553896,
+            "velocityX": 0.07883248161483059,
+            "velocityY": 0.6785919726876813,
+            "timestamp": 1.5762162105121216
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": -4.785738309812717e-31,
+            "velocityX": -1.2484093100405624e-30,
+            "velocityY": 4.284452606087597e-31,
+            "timestamp": 1.6585341294617273
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.1.traj
new file mode 100644
index 00000000..751754fd
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.222401190948388,
+            "y": 1.635609178560311,
+            "heading": -0.6120137040899031,
+            "angularVelocity": 0.36292240482976423,
+            "velocityX": 0.6017242103081577,
+            "velocityY": 0.20241766322785323,
+            "timestamp": 0.07711371780208913
+        },
+        {
+            "x": 5.315327100770923,
+            "y": 1.6668589219810437,
+            "heading": -0.5590312951883875,
+            "angularVelocity": 0.6870685321838068,
+            "velocityX": 1.205050313629373,
+            "velocityY": 0.4052423396435706,
+            "timestamp": 0.15422743560417826
+        },
+        {
+            "x": 5.454938302762681,
+            "y": 1.713800132384211,
+            "heading": -0.4856728473163301,
+            "angularVelocity": 0.9513021802467544,
+            "velocityX": 1.8104587091763231,
+            "velocityY": 0.6087271077195515,
+            "timestamp": 0.23134115340626737
+        },
+        {
+            "x": 5.641435450684747,
+            "y": 1.7765099385015581,
+            "heading": -0.4002091399727602,
+            "angularVelocity": 1.1082815065785667,
+            "velocityX": 2.418469154874715,
+            "velocityY": 0.8132120704942623,
+            "timestamp": 0.3084548712083565
+        },
+        {
+            "x": 5.8748863814192855,
+            "y": 1.8550365409977465,
+            "heading": -0.32154781798640775,
+            "angularVelocity": 1.0200691164734093,
+            "velocityX": 3.0273592998548686,
+            "velocityY": 1.0183220928048784,
+            "timestamp": 0.38556858901044566
+        },
+        {
+            "x": 6.151206632813102,
+            "y": 1.945953456728306,
+            "heading": -0.3195100296755088,
+            "angularVelocity": 0.026425756259478697,
+            "velocityX": 3.583282705976986,
+            "velocityY": 1.178997956808355,
+            "timestamp": 0.4626823068125348
+        },
+        {
+            "x": 6.4277763897122595,
+            "y": 2.0387313847088295,
+            "heading": -0.31950999788997536,
+            "angularVelocity": 4.121903903341971e-7,
+            "velocityX": 3.5865182587742526,
+            "velocityY": 1.2031313056210848,
+            "timestamp": 0.539796024614624
+        },
+        {
+            "x": 6.7043461469693755,
+            "y": 2.1315093116222874,
+            "heading": -0.31950996610444216,
+            "angularVelocity": 4.121903870025697e-7,
+            "velocityX": 3.5865182634161936,
+            "velocityY": 1.2031312917835268,
+            "timestamp": 0.616909742416713
+        },
+        {
+            "x": 6.980831721332989,
+            "y": 2.2223162260189957,
+            "heading": -0.31777519026878787,
+            "angularVelocity": 0.022496332495664188,
+            "velocityX": 3.5854265913259273,
+            "velocityY": 1.1775714747635835,
+            "timestamp": 0.6940234602188021
+        },
+        {
+            "x": 7.214355031558839,
+            "y": 2.3007720927806434,
+            "heading": -0.23870969056825803,
+            "angularVelocity": 1.0253104370281303,
+            "velocityX": 3.028297907062184,
+            "velocityY": 1.0174048016074528,
+            "timestamp": 0.7711371780208912
+        },
+        {
+            "x": 7.400917361819277,
+            "y": 2.3634042679352882,
+            "heading": -0.15373574527352316,
+            "angularVelocity": 1.1019303402387481,
+            "velocityX": 2.419314430400663,
+            "velocityY": 0.8122053629341165,
+            "timestamp": 0.8482508958229803
+        },
+        {
+            "x": 7.540592119323217,
+            "y": 2.4102654434053137,
+            "heading": -0.08076255152981597,
+            "angularVelocity": 0.9463062581289249,
+            "velocityX": 1.8112828882458099,
+            "velocityY": 0.6076892258040765,
+            "timestamp": 0.9253646136250694
+        },
+        {
+            "x": 7.633568850123268,
+            "y": 2.4414404537843537,
+            "heading": -0.02793953661151031,
+            "angularVelocity": 0.6850015331107969,
+            "velocityX": 1.2057093530190521,
+            "velocityY": 0.4042732119212608,
+            "timestamp": 1.0024783314271586
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -3.3818227179407965e-32,
+            "angularVelocity": 0.3623160367292381,
+            "velocityX": 0.6021127135368662,
+            "velocityY": 0.20177403786417583,
+            "timestamp": 1.0795920492292477
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -1.7311819232040874e-32,
+            "angularVelocity": 2.140534191184349e-31,
+            "velocityX": 0,
+            "velocityY": -2.0522160776390996e-31,
+            "timestamp": 1.1567057670313368
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.1.traj
new file mode 100644
index 00000000..0bd0e944
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.22245562506892,
+            "y": 1.6045082039129626,
+            "heading": -0.6133597798196372,
+            "angularVelocity": 0.3456549186542876,
+            "velocityX": 0.6027583554308628,
+            "velocityY": -0.20100492713723658,
+            "timestamp": 0.07707172310487943
+        },
+        {
+            "x": 5.31546298986369,
+            "y": 1.573504853521539,
+            "heading": -0.5625082909022232,
+            "angularVelocity": 0.6597943690478437,
+            "velocityX": 1.2067637915426601,
+            "velocityY": -0.4022662157070755,
+            "timestamp": 0.15414344620975887
+        },
+        {
+            "x": 5.455153171011431,
+            "y": 1.526958597742617,
+            "heading": -0.4912836919139272,
+            "angularVelocity": 0.924133989989731,
+            "velocityX": 1.8124699373549893,
+            "velocityY": -0.6039342822993798,
+            "timestamp": 0.2312151693146383
+        },
+        {
+            "x": 5.641701159953251,
+            "y": 1.4648201004985182,
+            "heading": -0.406509898431932,
+            "angularVelocity": 1.0999338027856826,
+            "velocityX": 2.420446584384324,
+            "velocityY": -0.8062424809101578,
+            "timestamp": 0.30828689241951773
+        },
+        {
+            "x": 5.875244890448534,
+            "y": 1.3870507382512116,
+            "heading": -0.323292875073049,
+            "angularVelocity": 1.0797348236997513,
+            "velocityX": 3.03021291190643,
+            "velocityY": -1.0090518171168696,
+            "timestamp": 0.38535861552439715
+        },
+        {
+            "x": 6.151315012657669,
+            "y": 1.295160148354896,
+            "heading": -0.32123146568205607,
+            "angularVelocity": 0.026746636872095383,
+            "velocityX": 3.5819897504232276,
+            "velocityY": -1.192273718485189,
+            "timestamp": 0.46243033862927657
+        },
+        {
+            "x": 6.427941847872596,
+            "y": 1.2030541885235448,
+            "heading": -0.3212314367193528,
+            "angularVelocity": 3.757889680371681e-7,
+            "velocityX": 3.5892130611702004,
+            "velocityY": -1.1950681277232245,
+            "timestamp": 0.539502061734156
+        },
+        {
+            "x": 6.704568684105482,
+            "y": 1.1109482317494896,
+            "heading": -0.3212314077566493,
+            "angularVelocity": 3.7578897090221527e-7,
+            "velocityX": 3.589213074378161,
+            "velocityY": -1.1950680880550335,
+            "timestamp": 0.6165737848390355
+        },
+        {
+            "x": 6.9807077062893965,
+            "y": 1.0195126541212949,
+            "heading": -0.3189025007582472,
+            "angularVelocity": 0.030217398866675224,
+            "velocityX": 3.582883722583232,
+            "velocityY": -1.1863699674103443,
+            "timestamp": 0.6936455079439149
+        },
+        {
+            "x": 7.214256981896637,
+            "y": 0.9418352430707998,
+            "heading": -0.2350676331360113,
+            "angularVelocity": 1.0877513080660361,
+            "velocityX": 3.0302848593306506,
+            "velocityY": -1.0078587570280646,
+            "timestamp": 0.7707172310487943
+        },
+        {
+            "x": 7.4008131962231865,
+            "y": 0.8797969152412516,
+            "heading": -0.14969687190170503,
+            "angularVelocity": 1.1076794159400525,
+            "velocityX": 2.4205533081527695,
+            "velocityY": -0.8049427900441033,
+            "timestamp": 0.8477889541536737
+        },
+        {
+            "x": 7.54051565231881,
+            "y": 0.8333524229780926,
+            "heading": -0.0780215306756512,
+            "angularVelocity": 0.9299823377312819,
+            "velocityX": 1.812629203910703,
+            "velocityY": -0.6026139080860716,
+            "timestamp": 0.9248606772585531
+        },
+        {
+            "x": 7.633536087871107,
+            "y": 0.802437413208921,
+            "heading": -0.026843150662094223,
+            "angularVelocity": 0.6640357572376224,
+            "velocityX": 1.2069333836706095,
+            "velocityY": -0.401120002560503,
+            "timestamp": 1.0019324003634327
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 3.8658238709310755e-33,
+            "angularVelocity": 0.3482879268907221,
+            "velocityX": 0.6028658794310917,
+            "velocityY": -0.20029931325025216,
+            "timestamp": 1.079004123468312
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 4.381697144346615e-34,
+            "angularVelocity": -4.4473487408728983e-32,
+            "velocityX": 0,
+            "velocityY": -1.5918634852021244e-32,
+            "timestamp": 1.1560758465731915
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W1.1.traj
new file mode 100644
index 00000000..90e7d73f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W1.1.traj	
@@ -0,0 +1,247 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 0,
+            "velocityX": -4.2485019622555225e-35,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 5.123544064624381,
+            "y": 1.640479835564366,
+            "heading": -0.5261105113915723,
+            "angularVelocity": 1.3044458865146935,
+            "velocityX": -0.6008098724480858,
+            "velocityY": 0.23456806756139406,
+            "timestamp": 0.08730871076049408
+        },
+        {
+            "x": 5.016418569113309,
+            "y": 1.6815339921073649,
+            "heading": -0.31680443246750034,
+            "angularVelocity": 2.397310384049101,
+            "velocityX": -1.2269737415426907,
+            "velocityY": 0.4702183342922002,
+            "timestamp": 0.17461742152098816
+        },
+        {
+            "x": 4.8522086939147036,
+            "y": 1.7459427773647251,
+            "heading": -0.051891218250848345,
+            "angularVelocity": 3.0342128741697256,
+            "velocityX": -1.8807960141464901,
+            "velocityY": 0.7377131639710823,
+            "timestamp": 0.26192613228148226
+        },
+        {
+            "x": 4.632595451769563,
+            "y": 1.839668047804236,
+            "heading": 0.17554509517769362,
+            "angularVelocity": 2.6049670353333583,
+            "velocityX": -2.515364620920639,
+            "velocityY": 1.073492777789593,
+            "timestamp": 0.34923484304197633
+        },
+        {
+            "x": 4.3704901470738005,
+            "y": 1.9627849470607086,
+            "heading": 0.27538033292006053,
+            "angularVelocity": 1.1434739658020574,
+            "velocityX": -3.002052171114658,
+            "velocityY": 1.4101330575617794,
+            "timestamp": 0.4365435538024704
+        },
+        {
+            "x": 4.075718453010027,
+            "y": 2.1117712847634245,
+            "heading": 0.2753804447713452,
+            "angularVelocity": 0.0000012811010915301473,
+            "velocityX": -3.3762002839830365,
+            "velocityY": 1.7064315393617047,
+            "timestamp": 0.5238522645629645
+        },
+        {
+            "x": 3.780946849777969,
+            "y": 2.260757802178556,
+            "heading": 0.2753805566219007,
+            "angularVelocity": 0.0000012810927397041138,
+            "velocityX": -3.3761992436319233,
+            "velocityY": 1.7064335977177825,
+            "timestamp": 0.6111609753234586
+        },
+        {
+            "x": 3.4861752863833098,
+            "y": 2.409744398412427,
+            "heading": 0.27538066847245973,
+            "angularVelocity": 0.0000012810927810381003,
+            "velocityX": -3.3761987873498507,
+            "velocityY": 1.7064345004769594,
+            "timestamp": 0.6984696860839527
+        },
+        {
+            "x": 3.191403823869153,
+            "y": 2.5587311942392557,
+            "heading": 0.2753807803230405,
+            "angularVelocity": 0.000001281093028963251,
+            "velocityX": -3.3761976319038296,
+            "velocityY": 1.7064367865370367,
+            "timestamp": 0.7857783968444467
+        },
+        {
+            "x": 2.907963654667952,
+            "y": 2.72828634801538,
+            "heading": 0.27538089480545036,
+            "angularVelocity": 0.0000013112369766496227,
+            "velocityX": -3.246413407463273,
+            "velocityY": 1.9420187550501031,
+            "timestamp": 0.8730871076049408
+        },
+        {
+            "x": 2.662249448997808,
+            "y": 2.9489938499602837,
+            "heading": 0.27538103946019826,
+            "angularVelocity": 0.000001656819194794751,
+            "velocityX": -2.81431490088302,
+            "velocityY": 2.527897846875211,
+            "timestamp": 0.9603958183654349
+        },
+        {
+            "x": 2.463361015566044,
+            "y": 3.212680537293485,
+            "heading": 0.27538116270778873,
+            "angularVelocity": 0.0000014116299433562534,
+            "velocityX": -2.277990726232991,
+            "velocityY": 3.0201647125055904,
+            "timestamp": 1.047704529125929
+        },
+        {
+            "x": 2.3186638703555946,
+            "y": 3.509581240155329,
+            "heading": 0.2753811333053821,
+            "angularVelocity": -3.3676372418890626e-7,
+            "velocityX": -1.657304797540575,
+            "velocityY": 3.4005851223287915,
+            "timestamp": 1.1350132398864232
+        },
+        {
+            "x": 2.233516432992889,
+            "y": 3.8287006658490608,
+            "heading": 0.2753811088610493,
+            "angularVelocity": -2.799758765995743e-7,
+            "velocityX": -0.9752456154836949,
+            "velocityY": 3.655069728026828,
+            "timestamp": 1.2223219506469174
+        },
+        {
+            "x": 2.1898375161467403,
+            "y": 4.15608338798651,
+            "heading": 0.27538108549661844,
+            "angularVelocity": -2.6760709935894875e-7,
+            "velocityX": -0.5002813174732252,
+            "velocityY": 3.749714310127982,
+            "timestamp": 1.3096306614074116
+        },
+        {
+            "x": 2.1796859113101297,
+            "y": 4.486211000706444,
+            "heading": 0.2753810837562719,
+            "angularVelocity": -1.9933252293602923e-8,
+            "velocityX": -0.11627253166592438,
+            "velocityY": 3.78115321878412,
+            "timestamp": 1.3969393721679058
+        },
+        {
+            "x": 2.197179274626327,
+            "y": 4.816031069152981,
+            "heading": 0.2753810893961792,
+            "angularVelocity": 6.459730400705482e-8,
+            "velocityX": 0.20036217650934227,
+            "velocityY": 3.7776307263464504,
+            "timestamp": 1.4842480829284
+        },
+        {
+            "x": 2.214672942788018,
+            "y": 5.1458511214306375,
+            "heading": 0.27538109503608554,
+            "angularVelocity": 6.459729260879602e-8,
+            "velocityX": 0.20036566809100828,
+            "velocityY": 3.777630541154376,
+            "timestamp": 1.5715567936888941
+        },
+        {
+            "x": 2.2321666974024836,
+            "y": 5.475671169122818,
+            "heading": 0.2753811006759881,
+            "angularVelocity": 6.459724966098765e-8,
+            "velocityX": 0.20036665828744704,
+            "velocityY": 3.7776304886341237,
+            "timestamp": 1.6588655044493883
+        },
+        {
+            "x": 2.2496605217353887,
+            "y": 5.805491213117091,
+            "heading": 0.2753811063158993,
+            "angularVelocity": 6.459734855412882e-8,
+            "velocityX": 0.20036745681532722,
+            "velocityY": 3.7776304462797263,
+            "timestamp": 1.7461742152098825
+        },
+        {
+            "x": 2.2664262161021513,
+            "y": 6.122693971906954,
+            "heading": 0.30917552472065174,
+            "angularVelocity": 0.3870681185232194,
+            "velocityX": 0.19202773950877436,
+            "velocityY": 3.633116971112041,
+            "timestamp": 1.8334829259703767
+        },
+        {
+            "x": 2.279851444819284,
+            "y": 6.376429629998848,
+            "heading": 0.34011357351362637,
+            "angularVelocity": 0.35435237244361656,
+            "velocityX": 0.15376734578020115,
+            "velocityY": 2.9061894956614607,
+            "timestamp": 1.9207916367308708
+        },
+        {
+            "x": 2.289924296428913,
+            "y": 6.566719611746641,
+            "heading": 0.3645867540299274,
+            "angularVelocity": 0.2803062867740199,
+            "velocityX": 0.11537052284807026,
+            "velocityY": 2.179507406423607,
+            "timestamp": 2.008100347491365
+        },
+        {
+            "x": 2.296641079194263,
+            "y": 6.6935744437684805,
+            "heading": 0.3814115840440414,
+            "angularVelocity": 0.192705056203017,
+            "velocityX": 0.07693141619941274,
+            "velocityY": 1.4529458849739352,
+            "timestamp": 2.095409058251859
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 0.09836837448577633,
+            "velocityX": 0.03847177190545204,
+            "velocityY": 0.7264516412973858,
+            "timestamp": 2.182717769012353
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 2.6936232860428826e-33,
+            "velocityX": 0,
+            "velocityY": 5.62663175256514e-35,
+            "timestamp": 2.2700264797728473
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W2.1.traj
new file mode 100644
index 00000000..97118e90
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W2.1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -2.256840111784074e-31,
+            "velocityX": -2.1308644711626517e-33,
+            "velocityY": -1.0217798620245829e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 5.1132615869041915,
+            "y": 1.6556053336408048,
+            "heading": -0.6461329738139044,
+            "angularVelocity": -0.06592692141286781,
+            "velocityX": -0.6744118848768044,
+            "velocityY": 0.3827425493802767,
+            "timestamp": 0.09302684976743651
+        },
+        {
+            "x": 4.987784771636742,
+            "y": 1.7268160147391884,
+            "heading": -0.6583988506620269,
+            "angularVelocity": -0.131853081973501,
+            "velocityX": -1.348823652323362,
+            "velocityY": 0.7654852472851404,
+            "timestamp": 0.18605369953487302
+        },
+        {
+            "x": 4.799569566809376,
+            "y": 1.8336320552005005,
+            "heading": -0.6767974581881981,
+            "angularVelocity": -0.19777738977689827,
+            "velocityX": -2.02323528419909,
+            "velocityY": 1.148228073167565,
+            "timestamp": 0.2790805493023095
+        },
+        {
+            "x": 4.54861599428641,
+            "y": 1.9760534609244509,
+            "heading": -0.7013284716868837,
+            "angularVelocity": -0.26369820713065356,
+            "velocityX": -2.697646681042515,
+            "velocityY": 1.5309709624694186,
+            "timestamp": 0.37210739906974605
+        },
+        {
+            "x": 4.242554383646119,
+            "y": 2.1497500028424636,
+            "heading": -0.7013284745367188,
+            "angularVelocity": -3.063454187141868e-8,
+            "velocityX": -3.29003520387323,
+            "velocityY": 1.8671656876724017,
+            "timestamp": 0.4651342488371826
+        },
+        {
+            "x": 3.9364928269084327,
+            "y": 2.3234466397396827,
+            "heading": -0.7013284773864732,
+            "angularVelocity": -3.0633677229161635e-8,
+            "velocityX": -3.290034624442601,
+            "velocityY": 1.8671667086594281,
+            "timestamp": 0.558161098604619
+        },
+        {
+            "x": 3.630431318940711,
+            "y": 2.497143362571822,
+            "heading": -0.7013284802362207,
+            "angularVelocity": -3.063360291364155e-8,
+            "velocityX": -3.2900341001857365,
+            "velocityY": 1.867167632424124,
+            "timestamp": 0.6511879483720555
+        },
+        {
+            "x": 3.3243698648507793,
+            "y": 2.6708401803390394,
+            "heading": -0.7013284830859674,
+            "angularVelocity": -3.063359264364299e-8,
+            "velocityX": -3.2900335210218676,
+            "velocityY": 1.8671686529367946,
+            "timestamp": 0.744214798139492
+        },
+        {
+            "x": 3.0183085209148324,
+            "y": 2.844537192202297,
+            "heading": -0.7013284859357423,
+            "angularVelocity": -3.063389607722874e-8,
+            "velocityX": -3.290032336912287,
+            "velocityY": 1.8671707393886097,
+            "timestamp": 0.8372416479069285
+        },
+        {
+            "x": 2.714360997870643,
+            "y": 3.021907148464231,
+            "heading": -0.7013284887932081,
+            "angularVelocity": -3.071657002046057e-8,
+            "velocityX": -3.2673096402172703,
+            "velocityY": 1.9066533662630898,
+            "timestamp": 0.9302684976743649
+        },
+        {
+            "x": 2.4529780195268613,
+            "y": 3.2575406255854364,
+            "heading": -0.7013284919399162,
+            "angularVelocity": -3.382580562568125e-8,
+            "velocityX": -2.8097584621776335,
+            "velocityY": 2.532962018065539,
+            "timestamp": 1.0232953474418014
+        },
+        {
+            "x": 2.2451503238556145,
+            "y": 3.5415333045009083,
+            "heading": -0.7013284961513817,
+            "angularVelocity": -4.527150531202637e-8,
+            "velocityX": -2.2340614155032408,
+            "velocityY": 3.0528033532839474,
+            "timestamp": 1.1163221972092379
+        },
+        {
+            "x": 2.099615704405981,
+            "y": 3.861945316168419,
+            "heading": -0.7013284332934493,
+            "angularVelocity": 6.756966674200994e-7,
+            "velocityX": -1.564436717071087,
+            "velocityY": 3.444296055047848,
+            "timestamp": 1.2093490469766743
+        },
+        {
+            "x": 2.0224929199626733,
+            "y": 4.205305507968674,
+            "heading": -0.7013282219960775,
+            "angularVelocity": 0.0000022713589924444385,
+            "velocityX": -0.8290379028862299,
+            "velocityY": 3.6909794608614943,
+            "timestamp": 1.3023758967441108
+        },
+        {
+            "x": 2.0170244087221176,
+            "y": 4.5571780060792255,
+            "heading": -0.7013280555552984,
+            "angularVelocity": 0.0000017891692504432307,
+            "velocityX": -0.05878422470745493,
+            "velocityY": 3.782483218449498,
+            "timestamp": 1.3954027465115473
+        },
+        {
+            "x": 2.079587370397571,
+            "y": 4.887709205686539,
+            "heading": -0.661535006789308,
+            "angularVelocity": 0.4277587477752017,
+            "velocityX": 0.6725258549747536,
+            "velocityY": 3.5530731227987227,
+            "timestamp": 1.4884295962789837
+        },
+        {
+            "x": 2.143441139356313,
+            "y": 5.160361503306635,
+            "heading": -0.462273406431507,
+            "angularVelocity": 2.1419794484704924,
+            "velocityX": 0.6864014971846709,
+            "velocityY": 2.9308989641347156,
+            "timestamp": 1.5814564460464202
+        },
+        {
+            "x": 2.1958862911911914,
+            "y": 5.3618445595482385,
+            "heading": -0.25085794958853325,
+            "angularVelocity": 2.272628358065485,
+            "velocityX": 0.5637636012182422,
+            "velocityY": 2.165859176628076,
+            "timestamp": 1.6744832958138567
+        },
+        {
+            "x": 2.231834886718389,
+            "y": 5.494391410801975,
+            "heading": -0.08836645960984135,
+            "angularVelocity": 1.7467160328971074,
+            "velocityX": 0.38643247209883785,
+            "velocityY": 1.4248236029178551,
+            "timestamp": 1.7675101455812932
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -3.8128513304390615e-33,
+            "angularVelocity": 0.9499027413134385,
+            "velocityX": 0.19526742362042082,
+            "velocityY": 0.7052650859622128,
+            "timestamp": 1.8605369953487296
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 5.82518331797089e-32,
+            "velocityX": 0,
+            "velocityY": -4.1086986258215814e-33,
+            "timestamp": 1.953563845116166
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.1.traj
new file mode 100644
index 00000000..8a67252f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.1.traj	
@@ -0,0 +1,256 @@
+{
+    "samples": [
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -2.137980318394988e-34,
+            "velocityX": 1.3258659962604387e-36,
+            "velocityY": 7.61509960712835e-36,
+            "timestamp": 0
+        },
+        {
+            "x": 5.139985955005962,
+            "y": 1.6266018860262446,
+            "heading": -0.6327443563347177,
+            "angularVelocity": 0.10940864810395588,
+            "velocityX": -0.5430597418126536,
+            "velocityY": 0.09955056482775779,
+            "timestamp": 0.06631691178916982
+        },
+        {
+            "x": 5.067957879081044,
+            "y": 1.6398056681484225,
+            "heading": -0.6182319813670383,
+            "angularVelocity": 0.2188336968074761,
+            "velocityX": -1.0861192715653594,
+            "velocityY": 0.19910128149746392,
+            "timestamp": 0.13263382357833964
+        },
+        {
+            "x": 4.959915797050431,
+            "y": 1.6596113536414707,
+            "heading": -0.5964609094580124,
+            "angularVelocity": 0.328288385596709,
+            "velocityX": -1.6291784269764191,
+            "velocityY": 0.2986521078667379,
+            "timestamp": 0.19895073536750946
+        },
+        {
+            "x": 4.815859744494909,
+            "y": 1.686018943789264,
+            "heading": -0.5674282989321845,
+            "angularVelocity": 0.43778592432238833,
+            "velocityX": -2.172237045860861,
+            "velocityY": 0.39820295359570684,
+            "timestamp": 0.2652676471566793
+        },
+        {
+            "x": 4.635789767966758,
+            "y": 1.719028429378711,
+            "heading": -0.5311304969514992,
+            "angularVelocity": 0.5473385445944894,
+            "velocityX": -2.7152949627783816,
+            "velocityY": 0.49775366039945246,
+            "timestamp": 0.3315845589458491
+        },
+        {
+            "x": 4.41970592970577,
+            "y": 1.758639783689655,
+            "heading": -0.48756329690665473,
+            "angularVelocity": 0.6569545967904882,
+            "velocityX": -3.258351941175237,
+            "velocityY": 0.5973039642870227,
+            "timestamp": 0.3979014707350189
+        },
+        {
+            "x": 4.172944378919847,
+            "y": 1.8038720825822125,
+            "heading": -0.4875632898742738,
+            "angularVelocity": 1.0604204507812919e-7,
+            "velocityX": -3.7209445393116334,
+            "velocityY": 0.6820628052819626,
+            "timestamp": 0.46421838252418873
+        },
+        {
+            "x": 3.9261828364502485,
+            "y": 1.849104426845956,
+            "heading": -0.4875632828424199,
+            "angularVelocity": 1.0603409772331237e-7,
+            "velocityX": -3.7209444139088643,
+            "velocityY": 0.6820634894390606,
+            "timestamp": 0.5305352943133586
+        },
+        {
+            "x": 3.6794213056437455,
+            "y": 1.8943368347367484,
+            "heading": -0.487563275810563,
+            "angularVelocity": 1.0603414195839125e-7,
+            "velocityX": -3.720944238039768,
+            "velocityY": 0.6820644488783205,
+            "timestamp": 0.5968522061025283
+        },
+        {
+            "x": 3.4326599282156307,
+            "y": 1.9395700793621615,
+            "heading": -0.4875632687787025,
+            "angularVelocity": 1.0603419804191013e-7,
+            "velocityX": -3.720941925230195,
+            "velocityY": 0.6820770660916139,
+            "timestamp": 0.6631691178916981
+        },
+        {
+            "x": 3.192328571865682,
+            "y": 2.011528836252569,
+            "heading": -0.4875632615410612,
+            "angularVelocity": 1.0913718718412795e-7,
+            "velocityX": -3.6239829308396314,
+            "velocityY": 1.0850740022269671,
+            "timestamp": 0.7294860296808678
+        },
+        {
+            "x": 2.965054771843869,
+            "y": 2.1177542675713954,
+            "heading": -0.48756325344928275,
+            "angularVelocity": 1.2201681646141545e-7,
+            "velocityX": -3.4270866041583203,
+            "velocityY": 1.6017849512735258,
+            "timestamp": 0.7958029414700376
+        },
+        {
+            "x": 2.7556945515371734,
+            "y": 2.2559769801698577,
+            "heading": -0.48756324342419455,
+            "angularVelocity": 1.5116940667572282e-7,
+            "velocityX": -3.156965767228115,
+            "velocityY": 2.084275471660842,
+            "timestamp": 0.8621198532592074
+        },
+        {
+            "x": 2.5687211592217127,
+            "y": 2.4232437322251608,
+            "heading": -0.4875632292213124,
+            "angularVelocity": 2.1416682100483036e-7,
+            "velocityX": -2.8193923280063653,
+            "velocityY": 2.5222337340898235,
+            "timestamp": 0.9284367650483771
+        },
+        {
+            "x": 2.4081295337898796,
+            "y": 2.6159806661875145,
+            "heading": -0.48756314249031313,
+            "angularVelocity": 0.0000013078262679985986,
+            "velocityX": -2.421578766248568,
+            "velocityY": 2.906301405817713,
+            "timestamp": 0.9947536768375469
+        },
+        {
+            "x": 2.2783797783684188,
+            "y": 2.8285106690600896,
+            "heading": -0.4828540771785898,
+            "angularVelocity": 0.07100851328382284,
+            "velocityX": -1.9565108193510647,
+            "velocityY": 3.2047632668456583,
+            "timestamp": 1.0610705886267167
+        },
+        {
+            "x": 2.1829307079315186,
+            "y": 3.053825616836548,
+            "heading": -0.47038335932986286,
+            "angularVelocity": 0.18804732476646344,
+            "velocityX": -1.4392870213912456,
+            "velocityY": 3.3975488559051734,
+            "timestamp": 1.1273875004158864
+        },
+        {
+            "x": 2.124568521000596,
+            "y": 3.2749812318761844,
+            "heading": -0.4550563591461243,
+            "angularVelocity": 0.24322153086635045,
+            "velocityX": -0.9261395106595871,
+            "velocityY": 3.5094804335357668,
+            "timestamp": 1.1904041261609186
+        },
+        {
+            "x": 2.099309698866093,
+            "y": 3.4965263857952165,
+            "heading": -0.4375068558264715,
+            "angularVelocity": 0.2784900510328627,
+            "velocityX": -0.40082790590377965,
+            "velocityY": 3.5156619590425686,
+            "timestamp": 1.2534207519059508
+        },
+        {
+            "x": 2.105558929434475,
+            "y": 3.707892405315209,
+            "heading": -0.4199092504656253,
+            "angularVelocity": 0.2792533740547583,
+            "velocityX": 0.09916796550908585,
+            "velocityY": 3.354131025281934,
+            "timestamp": 1.316437377650983
+        },
+        {
+            "x": 2.135680280502732,
+            "y": 3.8963278332771982,
+            "heading": -0.40525850037923283,
+            "angularVelocity": 0.23249023433387317,
+            "velocityX": 0.4779905415775436,
+            "velocityY": 2.990249410122441,
+            "timestamp": 1.3794540033960152
+        },
+        {
+            "x": 2.177481885950485,
+            "y": 4.0538159629335215,
+            "heading": -0.39590861500997965,
+            "angularVelocity": 0.14837172347315433,
+            "velocityX": 0.6633424902324003,
+            "velocityY": 2.4991520538330105,
+            "timestamp": 1.4424706291410474
+        },
+        {
+            "x": 2.2205433619064863,
+            "y": 4.17824561445827,
+            "heading": -0.39150641146352744,
+            "angularVelocity": 0.06985781124276201,
+            "velocityX": 0.6833351587282585,
+            "velocityY": 1.9745527478446014,
+            "timestamp": 1.5054872548860796
+        },
+        {
+            "x": 2.2579292824582606,
+            "y": 4.270063380418578,
+            "heading": -0.38992694464863337,
+            "angularVelocity": 0.0250642873403699,
+            "velocityX": 0.5932707457717501,
+            "velocityY": 1.4570403425249505,
+            "timestamp": 1.5685038806311118
+        },
+        {
+            "x": 2.285342089257776,
+            "y": 4.3303097045050025,
+            "heading": -0.38974725509942826,
+            "angularVelocity": 0.002851462563739305,
+            "velocityX": 0.43500911823537725,
+            "velocityY": 0.9560385592555154,
+            "timestamp": 1.631520506376144
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.004010765374750144,
+            "velocityX": 0.23260386555018367,
+            "velocityY": 0.47115019479344145,
+            "timestamp": 1.6945371321211762
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -1.6105528525551922e-32,
+            "velocityX": 0,
+            "velocityY": -4.5276793712997926e-36,
+            "timestamp": 1.7575537578662084
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.1.traj
new file mode 100644
index 00000000..b5a8c20b
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.1.traj	
@@ -0,0 +1,427 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": -1.04,
+            "angularVelocity": -3.8536087109009435e-31,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7208977987123659,
+            "y": 4.366230614894293,
+            "heading": -1.0125695703411368,
+            "angularVelocity": 0.6607032256639885,
+            "velocityX": 0.262489900865184,
+            "velocityY": 0.1500737469179648,
+            "timestamp": 0.04151702094612918
+        },
+        {
+            "x": 0.7428348149738224,
+            "y": 4.378911218024912,
+            "heading": -0.9596996326030717,
+            "angularVelocity": 1.2734521055122956,
+            "velocityX": 0.5283860874778604,
+            "velocityY": 0.30543143129192235,
+            "timestamp": 0.08303404189225835
+        },
+        {
+            "x": 0.7759938113776359,
+            "y": 4.398288777276395,
+            "heading": -0.8840273814949616,
+            "angularVelocity": 1.8226801775305617,
+            "velocityX": 0.7986843865038376,
+            "velocityY": 0.46673770925264374,
+            "timestamp": 0.12455106283838753
+        },
+        {
+            "x": 0.8206097262268935,
+            "y": 4.424610495629411,
+            "heading": -0.7889470653064761,
+            "angularVelocity": 2.2901526656373563,
+            "velocityX": 1.0746415285232955,
+            "velocityY": 0.6339982434446707,
+            "timestamp": 0.1660680837845167
+        },
+        {
+            "x": 0.876940069443398,
+            "y": 4.458114052595694,
+            "heading": -0.6785120124719297,
+            "angularVelocity": 2.6599946315634684,
+            "velocityX": 1.3568011849799015,
+            "velocityY": 0.8069836467704494,
+            "timestamp": 0.2075851047306459
+        },
+        {
+            "x": 0.9451906760990277,
+            "y": 4.499074634849957,
+            "heading": -0.5574056685886438,
+            "angularVelocity": 2.9170287540888538,
+            "velocityX": 1.6439186892586013,
+            "velocityY": 0.9865973357595108,
+            "timestamp": 0.2491021256767751
+        },
+        {
+            "x": 1.0254198074155758,
+            "y": 4.547882919999799,
+            "heading": -0.43194974381010975,
+            "angularVelocity": 3.0217949631302035,
+            "velocityX": 1.9324395028417536,
+            "velocityY": 1.175621083531942,
+            "timestamp": 0.2906191466229043
+        },
+        {
+            "x": 1.117246624722019,
+            "y": 4.6049782109254505,
+            "heading": -0.31304235816438886,
+            "angularVelocity": 2.8640635319318393,
+            "velocityX": 2.211787243252437,
+            "velocityY": 1.3752261030390898,
+            "timestamp": 0.33213616756903347
+        },
+        {
+            "x": 1.217280699738284,
+            "y": 4.671027805780365,
+            "heading": -0.22202476317889255,
+            "angularVelocity": 2.1922959044630015,
+            "velocityX": 2.409471410436724,
+            "velocityY": 1.5909040039398776,
+            "timestamp": 0.37365318851516266
+        },
+        {
+            "x": 1.3245506705159111,
+            "y": 4.7468577293550815,
+            "heading": -0.15914757808634442,
+            "angularVelocity": 1.5144917351940563,
+            "velocityX": 2.5837588616171234,
+            "velocityY": 1.8264779564293179,
+            "timestamp": 0.41517020946129185
+        },
+        {
+            "x": 1.4375421795348913,
+            "y": 4.833718356767073,
+            "heading": -0.12357000071830693,
+            "angularVelocity": 0.8569395529247944,
+            "velocityX": 2.7215707303608654,
+            "velocityY": 2.0921690774675916,
+            "timestamp": 0.45668723040742104
+        },
+        {
+            "x": 1.5526379689004826,
+            "y": 4.933380534066268,
+            "heading": -0.11179394732799099,
+            "angularVelocity": 0.2836439879911631,
+            "velocityX": 2.7722554928653214,
+            "velocityY": 2.4005136936152596,
+            "timestamp": 0.49820425135355023
+        },
+        {
+            "x": 1.6621482250840773,
+            "y": 5.045879001435295,
+            "heading": -0.11165007196252263,
+            "angularVelocity": 0.0034654549371103873,
+            "velocityX": 2.6377195108883376,
+            "velocityY": 2.709695079403094,
+            "timestamp": 0.5397212722996794
+        },
+        {
+            "x": 1.760950803756714,
+            "y": 5.167963981628418,
+            "heading": -0.11164985662454338,
+            "angularVelocity": 0.000005186739663230103,
+            "velocityX": 2.3798089655981185,
+            "velocityY": 2.940600684033376,
+            "timestamp": 0.5812382932458086
+        },
+        {
+            "x": 1.8636898338676644,
+            "y": 5.328509480252694,
+            "heading": -0.11164971710954129,
+            "angularVelocity": 0.000002768959602908003,
+            "velocityX": 2.039065475146862,
+            "velocityY": 3.1863526751214604,
+            "timestamp": 0.6316236444586746
+        },
+        {
+            "x": 1.9479932631191832,
+            "y": 5.499457142063362,
+            "heading": -0.11164961128911181,
+            "angularVelocity": 0.0000021002221265930453,
+            "velocityX": 1.6731733970781186,
+            "velocityY": 3.3928048072606183,
+            "timestamp": 0.6820089956715406
+        },
+        {
+            "x": 2.043351099956472,
+            "y": 5.66449366180702,
+            "heading": -0.11164949905977092,
+            "angularVelocity": 0.000002227420038927282,
+            "velocityX": 1.8925706488092502,
+            "velocityY": 3.275486143730564,
+            "timestamp": 0.7323943468844066
+        },
+        {
+            "x": 2.1564178056024077,
+            "y": 5.817933945525861,
+            "heading": -0.11163496800446338,
+            "angularVelocity": 0.0002883986749930945,
+            "velocityX": 2.244039247910364,
+            "velocityY": 3.0453352020399613,
+            "timestamp": 0.7827796980972727
+        },
+        {
+            "x": 2.2829725450073877,
+            "y": 5.955279135270152,
+            "heading": -0.10213363222908782,
+            "angularVelocity": 0.1885733760372676,
+            "velocityX": 2.5117367718340784,
+            "velocityY": 2.7258952540120527,
+            "timestamp": 0.8331650493101387
+        },
+        {
+            "x": 2.421245283059438,
+            "y": 6.075180412328107,
+            "heading": -0.08356505866258204,
+            "angularVelocity": 0.36853119252760197,
+            "velocityX": 2.7443043409080725,
+            "velocityY": 2.379685249204375,
+            "timestamp": 0.8835504005230047
+        },
+        {
+            "x": 2.570177190376964,
+            "y": 6.176890921850726,
+            "heading": -0.058269582676031995,
+            "angularVelocity": 0.5020402830847994,
+            "velocityX": 2.9558572825813,
+            "velocityY": 2.018652387526583,
+            "timestamp": 0.9339357517358707
+        },
+        {
+            "x": 2.728959152818659,
+            "y": 6.259893890741026,
+            "heading": -0.029260248722302817,
+            "angularVelocity": 0.5757493647196714,
+            "velocityX": 3.151351704820358,
+            "velocityY": 1.6473631103484576,
+            "timestamp": 0.9843211029487368
+        },
+        {
+            "x": 2.896845817565918,
+            "y": 6.323787212371826,
+            "heading": -5.388333556894659e-33,
+            "angularVelocity": 0.580729279745141,
+            "velocityX": 3.332053081047339,
+            "velocityY": 1.2680932075002167,
+            "timestamp": 1.034706454161603
+        },
+        {
+            "x": 3.0174812814006593,
+            "y": 6.35877580192698,
+            "heading": 0.017969633030989543,
+            "angularVelocity": 0.512929568417621,
+            "velocityX": 3.443447970619488,
+            "velocityY": 0.9987227956847937,
+            "timestamp": 1.069739788442565
+        },
+        {
+            "x": 3.141783379846136,
+            "y": 6.384241994405335,
+            "heading": 0.0331535119160805,
+            "angularVelocity": 0.43341232568386434,
+            "velocityX": 3.5481092792260864,
+            "velocityY": 0.7269131814328351,
+            "timestamp": 1.1047731227235271
+        },
+        {
+            "x": 3.269386252376355,
+            "y": 6.400068652575719,
+            "heading": 0.0449202302050253,
+            "angularVelocity": 0.3358720639272233,
+            "velocityX": 3.642327376147768,
+            "velocityY": 0.45175997361343984,
+            "timestamp": 1.1398064570044892
+        },
+        {
+            "x": 3.3996512457384864,
+            "y": 6.406094302159117,
+            "heading": 0.052174145586997546,
+            "angularVelocity": 0.20705752202617325,
+            "velocityX": 3.7183155995559303,
+            "velocityY": 0.17199760476778314,
+            "timestamp": 1.1748397912854514
+        },
+        {
+            "x": 3.531556253779136,
+            "y": 6.402132340099008,
+            "heading": 0.05397985174916118,
+            "angularVelocity": 0.05154251518314789,
+            "velocityX": 3.7651285767486513,
+            "velocityY": -0.11309120703085081,
+            "timestamp": 1.2098731255664135
+        },
+        {
+            "x": 3.663324219452337,
+            "y": 6.387950136374773,
+            "heading": 0.05397986327261661,
+            "angularVelocity": 3.2892831051068895e-7,
+            "velocityX": 3.7612168061547906,
+            "velocityY": -0.4048202666228151,
+            "timestamp": 1.2449064598473756
+        },
+        {
+            "x": 3.793605089187622,
+            "y": 6.363643169403076,
+            "heading": 0.053979860367415415,
+            "angularVelocity": -8.292676841260746e-8,
+            "velocityX": 3.718768778627541,
+            "velocityY": -0.6938239671072872,
+            "timestamp": 1.2799397941283377
+        },
+        {
+            "x": 4.111061973016203,
+            "y": 6.363890421291427,
+            "heading": 0.05397985774152245,
+            "angularVelocity": -3.129115871783651e-8,
+            "velocityX": 3.7829393158349456,
+            "velocityY": 0.002946349368989899,
+            "timestamp": 1.3638578423242849
+        },
+        {
+            "x": 4.423042820635495,
+            "y": 6.422600616815994,
+            "heading": 0.053979855524806185,
+            "angularVelocity": -2.641525047123925e-8,
+            "velocityX": 3.7176847451316233,
+            "velocityY": 0.6996134536813446,
+            "timestamp": 1.447775890520232
+        },
+        {
+            "x": 4.7253380303410815,
+            "y": 6.519536384153899,
+            "heading": 0.05397985339265137,
+            "angularVelocity": -2.5407583503873633e-8,
+            "velocityX": 3.602266928321921,
+            "velocityY": 1.1551241886794503,
+            "timestamp": 1.5316939387161792
+        },
+        {
+            "x": 5.027633110558163,
+            "y": 6.616472555302148,
+            "heading": 0.053979851260496835,
+            "angularVelocity": -2.5407580227573106e-8,
+            "velocityX": 3.6022653852867035,
+            "velocityY": 1.1551290006400656,
+            "timestamp": 1.6156119869121264
+        },
+        {
+            "x": 5.329928190774575,
+            "y": 6.71340872645249,
+            "heading": 0.05397984912834238,
+            "angularVelocity": -2.540757910781892e-8,
+            "velocityX": 3.602265385278706,
+            "velocityY": 1.1551290006650068,
+            "timestamp": 1.6995300351080735
+        },
+        {
+            "x": 5.632223270990985,
+            "y": 6.810344897602833,
+            "heading": 0.053979846996187915,
+            "angularVelocity": -2.5407579299100972e-8,
+            "velocityX": 3.602265385278706,
+            "velocityY": 1.155129000665007,
+            "timestamp": 1.7834480833040207
+        },
+        {
+            "x": 5.934518351207396,
+            "y": 6.907281068753174,
+            "heading": 0.05397984486403343,
+            "angularVelocity": -2.5407579624099417e-8,
+            "velocityX": 3.602265385278706,
+            "velocityY": 1.1551290006650068,
+            "timestamp": 1.8673661314999679
+        },
+        {
+            "x": 6.236813431423807,
+            "y": 7.004217239903516,
+            "heading": 0.053979842731878905,
+            "angularVelocity": -2.5407579955507513e-8,
+            "velocityX": 3.602265385278706,
+            "velocityY": 1.1551290006650072,
+            "timestamp": 1.951284179695915
+        },
+        {
+            "x": 6.539108511640218,
+            "y": 7.1011534110538586,
+            "heading": 0.053979840599724424,
+            "angularVelocity": -2.5407579566600852e-8,
+            "velocityX": 3.6022653852787063,
+            "velocityY": 1.155129000665007,
+            "timestamp": 2.0352022278918622
+        },
+        {
+            "x": 6.841403591857615,
+            "y": 7.198089582201063,
+            "heading": 0.05397983846756001,
+            "angularVelocity": -2.5407697930922655e-8,
+            "velocityX": 3.6022653852904623,
+            "velocityY": 1.1551290006276251,
+            "timestamp": 2.119120276087809
+        },
+        {
+            "x": 7.120935706136836,
+            "y": 7.287726381323771,
+            "heading": 0.035986533919804255,
+            "angularVelocity": -0.21441519356800662,
+            "velocityX": 3.331013057245073,
+            "velocityY": 1.0681468533849583,
+            "timestamp": 2.203038324283756
+        },
+        {
+            "x": 7.344561416731701,
+            "y": 7.359435826630496,
+            "heading": 0.021591909406666148,
+            "angularVelocity": -0.17153192695269756,
+            "velocityX": 2.664810674250942,
+            "velocityY": 0.8545175543082761,
+            "timestamp": 2.286956372479703
+        },
+        {
+            "x": 7.512280706030795,
+            "y": 7.413217912582047,
+            "heading": 0.010795955954566746,
+            "angularVelocity": -0.12864876727003446,
+            "velocityX": 1.9986080813923648,
+            "velocityY": 0.6408881892244497,
+            "timestamp": 2.37087442067565
+        },
+        {
+            "x": 7.624093568104708,
+            "y": 7.449072637343766,
+            "heading": 0.0035986551256689905,
+            "angularVelocity": -0.08576582729978673,
+            "velocityX": 1.3324054178766367,
+            "velocityY": 0.42725880227814755,
+            "timestamp": 2.454792468871597
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -6.709954361314771e-31,
+            "angularVelocity": -0.04288296979061891,
+            "velocityX": 0.6662027191665668,
+            "velocityY": 0.21362940442053102,
+            "timestamp": 2.538710517067544
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 6.378058636012386e-33,
+            "angularVelocity": 8.071845383822447e-30,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 2.622628565263491
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.traj
index a684ff7a..b5a8c20b 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.traj	
@@ -4,613 +4,424 @@
             "x": 0.71,
             "y": 4.36,
             "heading": -1.04,
-            "angularVelocity": 0,
+            "angularVelocity": -3.8536087109009435e-31,
             "velocityX": 0,
-            "velocityY": 5.951430910881942e-24,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.7172662637575132,
-            "y": 4.364294720467062,
-            "heading": -1.0315470131742317,
-            "angularVelocity": 0.26098153509522004,
-            "velocityX": 0.22434208273715575,
-            "velocityY": 0.13259724206381326,
-            "timestamp": 0.03238921413610362
-        },
-        {
-            "x": 0.7318144654254066,
-            "y": 4.3728967855708385,
-            "heading": -1.0149312072454106,
-            "angularVelocity": 0.5130042939294357,
-            "velocityX": 0.4491680967238058,
-            "velocityY": 0.2655842487449578,
-            "timestamp": 0.06477842827220724
-        },
-        {
-            "x": 0.7536624687467911,
-            "y": 4.385820612299006,
-            "heading": -0.9904958605622504,
-            "angularVelocity": 0.7544285137786853,
-            "velocityX": 0.6745456444104029,
-            "velocityY": 0.39901637235962667,
-            "timestamp": 0.09716764240831086
-        },
-        {
-            "x": 0.7828310955502448,
-            "y": 4.4030821899572965,
-            "heading": -0.9586530378385134,
-            "angularVelocity": 0.9831304516969581,
-            "velocityX": 0.9005660551343861,
-            "velocityY": 0.5329421574032179,
-            "timestamp": 0.12955685654441448
-        },
-        {
-            "x": 0.8193448564336872,
-            "y": 4.424698840047832,
-            "heading": -0.919902879162077,
-            "angularVelocity": 1.1963908266993857,
-            "velocityX": 1.1273432177146,
-            "velocityY": 0.6674027347406944,
-            "timestamp": 0.1619460706805181
-        },
-        {
-            "x": 0.8632324943492647,
-            "y": 4.450689040734006,
-            "heading": -0.8748582540701377,
-            "angularVelocity": 1.3907291761589549,
-            "velocityX": 1.3550078038681697,
-            "velocityY": 0.8024338156819394,
-            "timestamp": 0.19433528481662174
-        },
-        {
-            "x": 0.9145271224696989,
-            "y": 4.481072488182425,
-            "heading": -0.8242801160685745,
-            "angularVelocity": 1.5615734852049024,
-            "velocityX": 1.5836947418633758,
-            "velocityY": 0.9380730054376618,
-            "timestamp": 0.22672449895272537
-        },
-        {
-            "x": 0.9732655893831887,
-            "y": 4.515870568727518,
-            "heading": -0.7691375276578237,
-            "angularVelocity": 1.7024984977725797,
-            "velocityX": 1.8135193606940647,
-            "velocityY": 1.0743724870528544,
-            "timestamp": 0.25911371308882897
-        },
-        {
-            "x": 1.0394861119792222,
-            "y": 4.555106942812012,
-            "heading": -0.7107271690719036,
-            "angularVelocity": 1.8033891881560444,
-            "velocityX": 2.044523905944935,
-            "velocityY": 1.2114024724285466,
-            "timestamp": 0.2915029272249326
-        },
-        {
-            "x": 1.1132202582415271,
-            "y": 4.598805444215395,
-            "heading": -0.6509416110612904,
-            "angularVelocity": 1.8458477491731227,
-            "velocityX": 2.276503096137642,
-            "velocityY": 1.3491683132463759,
-            "timestamp": 0.3238921413610362
-        },
-        {
-            "x": 1.194458181637227,
-            "y": 4.646971254862031,
-            "heading": -0.5929406477835183,
-            "angularVelocity": 1.790749322723445,
-            "velocityX": 2.508178279791774,
-            "velocityY": 1.48709414326129,
-            "timestamp": 0.35628135549713985
-        },
-        {
-            "x": 1.2829200414273145,
-            "y": 4.699514481647374,
-            "heading": -0.5430956567390244,
-            "angularVelocity": 1.5389379574026985,
-            "velocityX": 2.7312135273909255,
-            "velocityY": 1.6222445708176134,
-            "timestamp": 0.3886705696332435
-        },
-        {
-            "x": 1.3771379621947135,
-            "y": 4.756486240363935,
-            "heading": -0.5116699283377882,
-            "angularVelocity": 0.9702528832339478,
-            "velocityX": 2.908928891311882,
-            "velocityY": 1.758973171660129,
-            "timestamp": 0.4210597837693471
-        },
-        {
-            "x": 1.4771406301349097,
-            "y": 4.8178582459332,
-            "heading": -0.4985254849792972,
-            "angularVelocity": 0.40582779511896505,
-            "velocityX": 3.087529926474048,
-            "velocityY": 1.894828485537579,
-            "timestamp": 0.45344899790545073
-        },
-        {
-            "x": 1.5826858211522454,
-            "y": 4.880091207833619,
-            "heading": -0.49852543793275184,
-            "angularVelocity": 0.000001452537415626245,
-            "velocityX": 3.2586524197166784,
-            "velocityY": 1.9214100607352578,
-            "timestamp": 0.48583821204155436
-        },
-        {
-            "x": 1.6882312828144117,
-            "y": 4.9423237107273525,
-            "heading": -0.4985253908851185,
-            "angularVelocity": 0.0000014525710071923091,
-            "velocityX": 3.2586607757338757,
-            "velocityY": 1.9213958891446032,
-            "timestamp": 0.5182274261776579
-        },
-        {
-            "x": 1.7937767445018398,
-            "y": 5.004556213578243,
-            "heading": -0.49852534383749064,
-            "angularVelocity": 0.0000014525708350435827,
-            "velocityX": 3.2586607765138327,
-            "velocityY": 1.921395887821837,
-            "timestamp": 0.5506166403137616
-        },
-        {
-            "x": 1.8993222061892705,
-            "y": 5.06678871642913,
-            "heading": -0.49852529678986307,
-            "angularVelocity": 0.0000014525708270038968,
-            "velocityX": 3.2586607765139086,
-            "velocityY": 1.9213958878217097,
-            "timestamp": 0.5830058544498652
-        },
-        {
-            "x": 2.0048676678767015,
-            "y": 5.129021219280016,
-            "heading": -0.4985252497422358,
-            "angularVelocity": 0.0000014525708183379938,
-            "velocityX": 3.258660776513909,
-            "velocityY": 1.9213958878217101,
-            "timestamp": 0.6153950685859688
-        },
-        {
-            "x": 2.1104131295641317,
-            "y": 5.191253722130902,
-            "heading": -0.49852520269460865,
-            "angularVelocity": 0.000001452570810915054,
-            "velocityX": 3.2586607765139104,
-            "velocityY": 1.9213958878217086,
-            "timestamp": 0.6477842827220724
-        },
-        {
-            "x": 2.2159585912515625,
-            "y": 5.253486224981789,
-            "heading": -0.4985251556469819,
-            "angularVelocity": 0.0000014525708028649706,
-            "velocityX": 3.2586607765139086,
-            "velocityY": 1.9213958878217123,
-            "timestamp": 0.6801734968581761
-        },
-        {
-            "x": 2.321504052938993,
-            "y": 5.315718727832675,
-            "heading": -0.4985251085993554,
-            "angularVelocity": 0.000001452570794356237,
-            "velocityX": 3.2586607765139117,
-            "velocityY": 1.9213958878217081,
-            "timestamp": 0.7125627109942797
-        },
-        {
-            "x": 2.427049514626424,
-            "y": 5.3779512306835615,
-            "heading": -0.49852506155172915,
-            "angularVelocity": 0.0000014525707870212498,
-            "velocityX": 3.258660776513912,
-            "velocityY": 1.9213958878217077,
-            "timestamp": 0.7449519251303833
-        },
-        {
-            "x": 2.5325949763138547,
-            "y": 5.440183733534448,
-            "heading": -0.4985250145041031,
-            "angularVelocity": 0.000001452570779107594,
-            "velocityX": 3.258660776513913,
-            "velocityY": 1.9213958878217068,
-            "timestamp": 0.777341139266487
-        },
-        {
-            "x": 2.6381404380012854,
-            "y": 5.502416236385335,
-            "heading": -0.49852496745647734,
-            "angularVelocity": 0.0000014525707705186945,
-            "velocityX": 3.258660776513915,
-            "velocityY": 1.9213958878217055,
-            "timestamp": 0.8097303534025906
-        },
-        {
-            "x": 2.743685899688716,
-            "y": 5.564648739236221,
-            "heading": -0.4985249204088519,
-            "angularVelocity": 0.00000145257076360687,
-            "velocityX": 3.2586607765139153,
-            "velocityY": 1.9213958878217066,
-            "timestamp": 0.8421195675386942
-        },
-        {
-            "x": 2.849231361376147,
-            "y": 5.626881242087108,
-            "heading": -0.4985248733612266,
-            "angularVelocity": 0.0000014525707544682094,
-            "velocityX": 3.2586607765139153,
-            "velocityY": 1.9213958878217066,
-            "timestamp": 0.8745087816747978
-        },
-        {
-            "x": 2.954776823063578,
-            "y": 5.689113744937994,
-            "heading": -0.4985248263136016,
-            "angularVelocity": 0.0000014525707471414822,
-            "velocityX": 3.2586607765139166,
-            "velocityY": 1.921395887821705,
-            "timestamp": 0.9068979958109015
-        },
-        {
-            "x": 3.0603222847510096,
-            "y": 5.751346247788881,
-            "heading": -0.49852477926597694,
-            "angularVelocity": 0.0000014525707385370494,
-            "velocityX": 3.258660776513917,
-            "velocityY": 1.9213958878217057,
-            "timestamp": 0.9392872099470051
-        },
-        {
-            "x": 3.1658677464384404,
-            "y": 5.813578750639766,
-            "heading": -0.4985247322183525,
-            "angularVelocity": 0.0000014525707310188908,
-            "velocityX": 3.2586607765139175,
-            "velocityY": 1.9213958878217063,
-            "timestamp": 0.9716764240831087
-        },
-        {
-            "x": 3.271413208125871,
-            "y": 5.875811253490653,
-            "heading": -0.4985246851707283,
-            "angularVelocity": 0.0000014525707228586851,
-            "velocityX": 3.258660776513917,
-            "velocityY": 1.921395887821708,
-            "timestamp": 1.0040656382192124
-        },
-        {
-            "x": 3.376958669813326,
-            "y": 5.938043756341499,
-            "heading": -0.4985246381231044,
-            "angularVelocity": 0.000001452570714543944,
-            "velocityX": 3.258660776514645,
-            "velocityY": 1.921395887820474,
-            "timestamp": 1.0364548523553159
-        },
-        {
-            "x": 3.482504131751534,
-            "y": 6.000276258767071,
-            "heading": -0.49852459107548075,
-            "angularVelocity": 0.0000014525707064344823,
-            "velocityX": 3.258660784256519,
-            "velocityY": 1.9213958746903632,
-            "timestamp": 1.0688440664914194
-        },
-        {
-            "x": 3.588052280044538,
-            "y": 6.0625042049472775,
-            "heading": -0.4985245440280508,
-            "angularVelocity": 0.0000014525647243024202,
-            "velocityX": 3.258743724052004,
-            "velocityY": 1.921255203004168,
-            "timestamp": 1.101233280627523
-        },
-        {
-            "x": 3.69740891456604,
-            "y": 6.1177659034729,
-            "heading": -0.4985244969178424,
-            "angularVelocity": 0.0000014545029788824699,
-            "velocityX": 3.3763287390046313,
-            "velocityY": 1.7061759601022248,
-            "timestamp": 1.1336224947636264
-        },
-        {
-            "x": 3.8477077097405896,
-            "y": 6.176720729441604,
-            "heading": -0.4985244519972541,
-            "angularVelocity": 0.0000010525498975792195,
-            "velocityX": 3.5217032391581933,
-            "velocityY": 1.3813909907718895,
-            "timestamp": 1.1763003653755966
-        },
-        {
-            "x": 4.000682189533232,
-            "y": 6.228336695265556,
-            "heading": -0.49852440748602983,
-            "angularVelocity": 0.000001042957945295385,
-            "velocityX": 3.58439813418752,
-            "velocityY": 1.2094316113670969,
-            "timestamp": 1.2189782359875667
-        },
-        {
-            "x": 4.153656816760498,
-            "y": 6.279952224134777,
-            "heading": -0.4985243629748153,
-            "angularVelocity": 0.00000104295771772215,
-            "velocityX": 3.5844015887793454,
-            "velocityY": 1.2094213729291292,
-            "timestamp": 1.2616561065995369
-        },
-        {
-            "x": 4.306631443993679,
-            "y": 6.331567752986466,
-            "heading": -0.4985243184636008,
-            "angularVelocity": 0.0000010429577168479619,
-            "velocityX": 3.584401588917953,
-            "velocityY": 1.209421372518333,
-            "timestamp": 1.304333977211507
-        },
-        {
-            "x": 4.459606071226857,
-            "y": 6.38318328183816,
-            "heading": -0.4985242739523863,
-            "angularVelocity": 0.000001042957716433214,
-            "velocityX": 3.584401588917909,
-            "velocityY": 1.2094213725184633,
-            "timestamp": 1.3470118478234772
-        },
-        {
-            "x": 4.612580698460036,
-            "y": 6.434798810689855,
-            "heading": -0.4985242294411719,
-            "angularVelocity": 0.0000010429577140470852,
-            "velocityX": 3.5844015889179057,
-            "velocityY": 1.209421372518471,
-            "timestamp": 1.3896897184354473
-        },
-        {
-            "x": 4.765555325693214,
-            "y": 6.4864143395415494,
-            "heading": -0.49852418492995754,
-            "angularVelocity": 0.0000010429577143791187,
-            "velocityX": 3.584401588917906,
-            "velocityY": 1.2094213725184708,
-            "timestamp": 1.4323675890474175
-        },
-        {
-            "x": 4.918529952926393,
-            "y": 6.5380298683932425,
-            "heading": -0.4985241404187432,
-            "angularVelocity": 0.0000010429577134079309,
-            "velocityX": 3.584401588917911,
-            "velocityY": 1.2094213725184557,
-            "timestamp": 1.4750454596593876
-        },
-        {
-            "x": 5.071504580159572,
-            "y": 6.589645397244938,
-            "heading": -0.49852409590752894,
-            "angularVelocity": 0.000001042957711441133,
-            "velocityX": 3.584401588917906,
-            "velocityY": 1.20942137251847,
-            "timestamp": 1.5177233302713578
-        },
-        {
-            "x": 5.224479207392751,
-            "y": 6.641260926096632,
-            "heading": -0.49852405139631467,
-            "angularVelocity": 0.0000010429577109856096,
-            "velocityX": 3.5844015889179066,
-            "velocityY": 1.2094213725184684,
-            "timestamp": 1.560401200883328
-        },
-        {
-            "x": 5.377453834625929,
-            "y": 6.692876454948327,
-            "heading": -0.4985240068851004,
-            "angularVelocity": 0.000001042957710628888,
-            "velocityX": 3.584401588917908,
-            "velocityY": 1.209421372518467,
-            "timestamp": 1.603079071495298
-        },
-        {
-            "x": 5.530428461859109,
-            "y": 6.744491983800022,
-            "heading": -0.4985239623738862,
-            "angularVelocity": 0.0000010429577097054993,
-            "velocityX": 3.5844015889179075,
-            "velocityY": 1.2094213725184686,
-            "timestamp": 1.6457569421072682
-        },
-        {
-            "x": 5.683403089092287,
-            "y": 6.796107512651717,
-            "heading": -0.4985239178626721,
-            "angularVelocity": 0.0000010429577084041104,
-            "velocityX": 3.584401588917908,
-            "velocityY": 1.2094213725184642,
-            "timestamp": 1.6884348127192383
-        },
-        {
-            "x": 5.836377716325465,
-            "y": 6.847723041503412,
-            "heading": -0.4985238733514579,
-            "angularVelocity": 0.000001042957708437487,
-            "velocityX": 3.5844015889179084,
-            "velocityY": 1.209421372518465,
-            "timestamp": 1.7311126833312085
-        },
-        {
-            "x": 5.989352343558645,
-            "y": 6.899338570355106,
-            "heading": -0.4985238288402439,
-            "angularVelocity": 0.0000010429577061050693,
-            "velocityX": 3.5844015889179084,
-            "velocityY": 1.2094213725184642,
-            "timestamp": 1.7737905539431786
-        },
-        {
-            "x": 6.142326970791823,
-            "y": 6.9509540992067995,
-            "heading": -0.49852378432902983,
-            "angularVelocity": 0.0000010429577060929704,
-            "velocityX": 3.584401588917909,
-            "velocityY": 1.2094213725184646,
-            "timestamp": 1.8164684245551488
-        },
-        {
-            "x": 6.295301598025002,
-            "y": 7.002569628058494,
-            "heading": -0.4985237398178158,
-            "angularVelocity": 0.0000010429577057751362,
-            "velocityX": 3.58440158891791,
-            "velocityY": 1.2094213725184613,
-            "timestamp": 1.859146295167119
-        },
-        {
-            "x": 6.44827622525818,
-            "y": 7.054185156910189,
-            "heading": -0.49852369530660184,
-            "angularVelocity": 0.0000010429577050460837,
-            "velocityX": 3.5844015889179097,
-            "velocityY": 1.2094213725184615,
-            "timestamp": 1.901824165779089
-        },
-        {
-            "x": 6.60125085249139,
-            "y": 7.105800685761796,
-            "heading": -0.4985236507953879,
-            "angularVelocity": 0.0000010429577031279755,
-            "velocityX": 3.584401588918602,
-            "velocityY": 1.2094213725164094,
-            "timestamp": 1.9445020363910592
-        },
-        {
-            "x": 6.754225480461122,
-            "y": 7.15741621243045,
-            "heading": -0.49852360628410947,
-            "angularVelocity": 0.000001042959214800434,
-            "velocityX": 3.584401606176385,
-            "velocityY": 1.209421321366894,
-            "timestamp": 1.9871799070030294
-        },
-        {
-            "x": 6.905473027890085,
-            "y": 7.208390081717835,
-            "heading": -0.49401401466016337,
-            "angularVelocity": 0.10566580664128356,
-            "velocityX": 3.543933782547739,
-            "velocityY": 1.1943864245440503,
-            "timestamp": 2.0298577776149997
-        },
-        {
-            "x": 7.045508663452743,
-            "y": 7.253974234854833,
-            "heading": -0.45930885159823864,
-            "angularVelocity": 0.8131887220303519,
-            "velocityX": 3.2812235839006783,
-            "velocityY": 1.0680981146283737,
-            "timestamp": 2.07253564822697
-        },
-        {
-            "x": 7.17296615196941,
-            "y": 7.296903697598188,
-            "heading": -0.39602280772923965,
-            "angularVelocity": 1.4828772607799376,
-            "velocityX": 2.986500654531669,
-            "velocityY": 1.0058951425592673,
-            "timestamp": 2.1152135188389405
-        },
-        {
-            "x": 7.286216905364156,
-            "y": 7.335009151903716,
-            "heading": -0.3243524220820172,
-            "angularVelocity": 1.6793336832302157,
-            "velocityX": 2.6536177126649325,
-            "velocityY": 0.8928621264164087,
-            "timestamp": 2.157891389450911
-        },
-        {
-            "x": 7.38508554947564,
-            "y": 7.368236895639727,
-            "heading": -0.2530399098538711,
-            "angularVelocity": 1.6709482269282783,
-            "velocityX": 2.31662551795061,
-            "velocityY": 0.7785707969856192,
-            "timestamp": 2.2005692600628812
-        },
-        {
-            "x": 7.469627579006154,
-            "y": 7.396613050645165,
-            "heading": -0.18659143584410423,
-            "angularVelocity": 1.5569772591027433,
-            "velocityX": 1.9809336388681473,
-            "velocityY": 0.6648915374301301,
-            "timestamp": 2.2432471306748516
-        },
-        {
-            "x": 7.539923583097788,
-            "y": 7.420174905803342,
-            "heading": -0.12774543015301906,
-            "angularVelocity": 1.3788411850749698,
-            "velocityX": 1.6471300719468636,
-            "velocityY": 0.5520860066427349,
-            "timestamp": 2.285925001286822
-        },
-        {
-            "x": 7.596048224439389,
-            "y": 7.4389607760624,
-            "heading": -0.07839593087005978,
-            "angularVelocity": 1.1563252471438406,
-            "velocityX": 1.315075952403783,
-            "velocityY": 0.4401782467044836,
-            "timestamp": 2.3286028718987923
-        },
-        {
-            "x": 7.638066692552635,
-            "y": 7.453006402993039,
-            "heading": -0.03996068961729096,
-            "angularVelocity": 0.9005894788478175,
-            "velocityX": 0.9845493111706484,
-            "velocityY": 0.3291079599154316,
-            "timestamp": 2.3712807425107627
-        },
-        {
-            "x": 7.666034942577791,
-            "y": 7.46234355914255,
-            "heading": -0.013541291231896178,
-            "angularVelocity": 0.6190420938664342,
-            "velocityX": 0.655333774251397,
-            "velocityY": 0.2187821467102802,
-            "timestamp": 2.413958613122733
+            "x": 0.7208977987123659,
+            "y": 4.366230614894293,
+            "heading": -1.0125695703411368,
+            "angularVelocity": 0.6607032256639885,
+            "velocityX": 0.262489900865184,
+            "velocityY": 0.1500737469179648,
+            "timestamp": 0.04151702094612918
+        },
+        {
+            "x": 0.7428348149738224,
+            "y": 4.378911218024912,
+            "heading": -0.9596996326030717,
+            "angularVelocity": 1.2734521055122956,
+            "velocityX": 0.5283860874778604,
+            "velocityY": 0.30543143129192235,
+            "timestamp": 0.08303404189225835
+        },
+        {
+            "x": 0.7759938113776359,
+            "y": 4.398288777276395,
+            "heading": -0.8840273814949616,
+            "angularVelocity": 1.8226801775305617,
+            "velocityX": 0.7986843865038376,
+            "velocityY": 0.46673770925264374,
+            "timestamp": 0.12455106283838753
+        },
+        {
+            "x": 0.8206097262268935,
+            "y": 4.424610495629411,
+            "heading": -0.7889470653064761,
+            "angularVelocity": 2.2901526656373563,
+            "velocityX": 1.0746415285232955,
+            "velocityY": 0.6339982434446707,
+            "timestamp": 0.1660680837845167
+        },
+        {
+            "x": 0.876940069443398,
+            "y": 4.458114052595694,
+            "heading": -0.6785120124719297,
+            "angularVelocity": 2.6599946315634684,
+            "velocityX": 1.3568011849799015,
+            "velocityY": 0.8069836467704494,
+            "timestamp": 0.2075851047306459
+        },
+        {
+            "x": 0.9451906760990277,
+            "y": 4.499074634849957,
+            "heading": -0.5574056685886438,
+            "angularVelocity": 2.9170287540888538,
+            "velocityX": 1.6439186892586013,
+            "velocityY": 0.9865973357595108,
+            "timestamp": 0.2491021256767751
+        },
+        {
+            "x": 1.0254198074155758,
+            "y": 4.547882919999799,
+            "heading": -0.43194974381010975,
+            "angularVelocity": 3.0217949631302035,
+            "velocityX": 1.9324395028417536,
+            "velocityY": 1.175621083531942,
+            "timestamp": 0.2906191466229043
+        },
+        {
+            "x": 1.117246624722019,
+            "y": 4.6049782109254505,
+            "heading": -0.31304235816438886,
+            "angularVelocity": 2.8640635319318393,
+            "velocityX": 2.211787243252437,
+            "velocityY": 1.3752261030390898,
+            "timestamp": 0.33213616756903347
+        },
+        {
+            "x": 1.217280699738284,
+            "y": 4.671027805780365,
+            "heading": -0.22202476317889255,
+            "angularVelocity": 2.1922959044630015,
+            "velocityX": 2.409471410436724,
+            "velocityY": 1.5909040039398776,
+            "timestamp": 0.37365318851516266
+        },
+        {
+            "x": 1.3245506705159111,
+            "y": 4.7468577293550815,
+            "heading": -0.15914757808634442,
+            "angularVelocity": 1.5144917351940563,
+            "velocityX": 2.5837588616171234,
+            "velocityY": 1.8264779564293179,
+            "timestamp": 0.41517020946129185
+        },
+        {
+            "x": 1.4375421795348913,
+            "y": 4.833718356767073,
+            "heading": -0.12357000071830693,
+            "angularVelocity": 0.8569395529247944,
+            "velocityX": 2.7215707303608654,
+            "velocityY": 2.0921690774675916,
+            "timestamp": 0.45668723040742104
+        },
+        {
+            "x": 1.5526379689004826,
+            "y": 4.933380534066268,
+            "heading": -0.11179394732799099,
+            "angularVelocity": 0.2836439879911631,
+            "velocityX": 2.7722554928653214,
+            "velocityY": 2.4005136936152596,
+            "timestamp": 0.49820425135355023
+        },
+        {
+            "x": 1.6621482250840773,
+            "y": 5.045879001435295,
+            "heading": -0.11165007196252263,
+            "angularVelocity": 0.0034654549371103873,
+            "velocityX": 2.6377195108883376,
+            "velocityY": 2.709695079403094,
+            "timestamp": 0.5397212722996794
+        },
+        {
+            "x": 1.760950803756714,
+            "y": 5.167963981628418,
+            "heading": -0.11164985662454338,
+            "angularVelocity": 0.000005186739663230103,
+            "velocityX": 2.3798089655981185,
+            "velocityY": 2.940600684033376,
+            "timestamp": 0.5812382932458086
+        },
+        {
+            "x": 1.8636898338676644,
+            "y": 5.328509480252694,
+            "heading": -0.11164971710954129,
+            "angularVelocity": 0.000002768959602908003,
+            "velocityX": 2.039065475146862,
+            "velocityY": 3.1863526751214604,
+            "timestamp": 0.6316236444586746
+        },
+        {
+            "x": 1.9479932631191832,
+            "y": 5.499457142063362,
+            "heading": -0.11164961128911181,
+            "angularVelocity": 0.0000021002221265930453,
+            "velocityX": 1.6731733970781186,
+            "velocityY": 3.3928048072606183,
+            "timestamp": 0.6820089956715406
+        },
+        {
+            "x": 2.043351099956472,
+            "y": 5.66449366180702,
+            "heading": -0.11164949905977092,
+            "angularVelocity": 0.000002227420038927282,
+            "velocityX": 1.8925706488092502,
+            "velocityY": 3.275486143730564,
+            "timestamp": 0.7323943468844066
+        },
+        {
+            "x": 2.1564178056024077,
+            "y": 5.817933945525861,
+            "heading": -0.11163496800446338,
+            "angularVelocity": 0.0002883986749930945,
+            "velocityX": 2.244039247910364,
+            "velocityY": 3.0453352020399613,
+            "timestamp": 0.7827796980972727
+        },
+        {
+            "x": 2.2829725450073877,
+            "y": 5.955279135270152,
+            "heading": -0.10213363222908782,
+            "angularVelocity": 0.1885733760372676,
+            "velocityX": 2.5117367718340784,
+            "velocityY": 2.7258952540120527,
+            "timestamp": 0.8331650493101387
+        },
+        {
+            "x": 2.421245283059438,
+            "y": 6.075180412328107,
+            "heading": -0.08356505866258204,
+            "angularVelocity": 0.36853119252760197,
+            "velocityX": 2.7443043409080725,
+            "velocityY": 2.379685249204375,
+            "timestamp": 0.8835504005230047
+        },
+        {
+            "x": 2.570177190376964,
+            "y": 6.176890921850726,
+            "heading": -0.058269582676031995,
+            "angularVelocity": 0.5020402830847994,
+            "velocityX": 2.9558572825813,
+            "velocityY": 2.018652387526583,
+            "timestamp": 0.9339357517358707
+        },
+        {
+            "x": 2.728959152818659,
+            "y": 6.259893890741026,
+            "heading": -0.029260248722302817,
+            "angularVelocity": 0.5757493647196714,
+            "velocityX": 3.151351704820358,
+            "velocityY": 1.6473631103484576,
+            "timestamp": 0.9843211029487368
+        },
+        {
+            "x": 2.896845817565918,
+            "y": 6.323787212371826,
+            "heading": -5.388333556894659e-33,
+            "angularVelocity": 0.580729279745141,
+            "velocityX": 3.332053081047339,
+            "velocityY": 1.2680932075002167,
+            "timestamp": 1.034706454161603
+        },
+        {
+            "x": 3.0174812814006593,
+            "y": 6.35877580192698,
+            "heading": 0.017969633030989543,
+            "angularVelocity": 0.512929568417621,
+            "velocityX": 3.443447970619488,
+            "velocityY": 0.9987227956847937,
+            "timestamp": 1.069739788442565
+        },
+        {
+            "x": 3.141783379846136,
+            "y": 6.384241994405335,
+            "heading": 0.0331535119160805,
+            "angularVelocity": 0.43341232568386434,
+            "velocityX": 3.5481092792260864,
+            "velocityY": 0.7269131814328351,
+            "timestamp": 1.1047731227235271
+        },
+        {
+            "x": 3.269386252376355,
+            "y": 6.400068652575719,
+            "heading": 0.0449202302050253,
+            "angularVelocity": 0.3358720639272233,
+            "velocityX": 3.642327376147768,
+            "velocityY": 0.45175997361343984,
+            "timestamp": 1.1398064570044892
+        },
+        {
+            "x": 3.3996512457384864,
+            "y": 6.406094302159117,
+            "heading": 0.052174145586997546,
+            "angularVelocity": 0.20705752202617325,
+            "velocityX": 3.7183155995559303,
+            "velocityY": 0.17199760476778314,
+            "timestamp": 1.1748397912854514
+        },
+        {
+            "x": 3.531556253779136,
+            "y": 6.402132340099008,
+            "heading": 0.05397985174916118,
+            "angularVelocity": 0.05154251518314789,
+            "velocityX": 3.7651285767486513,
+            "velocityY": -0.11309120703085081,
+            "timestamp": 1.2098731255664135
+        },
+        {
+            "x": 3.663324219452337,
+            "y": 6.387950136374773,
+            "heading": 0.05397986327261661,
+            "angularVelocity": 3.2892831051068895e-7,
+            "velocityX": 3.7612168061547906,
+            "velocityY": -0.4048202666228151,
+            "timestamp": 1.2449064598473756
+        },
+        {
+            "x": 3.793605089187622,
+            "y": 6.363643169403076,
+            "heading": 0.053979860367415415,
+            "angularVelocity": -8.292676841260746e-8,
+            "velocityX": 3.718768778627541,
+            "velocityY": -0.6938239671072872,
+            "timestamp": 1.2799397941283377
+        },
+        {
+            "x": 4.111061973016203,
+            "y": 6.363890421291427,
+            "heading": 0.05397985774152245,
+            "angularVelocity": -3.129115871783651e-8,
+            "velocityX": 3.7829393158349456,
+            "velocityY": 0.002946349368989899,
+            "timestamp": 1.3638578423242849
+        },
+        {
+            "x": 4.423042820635495,
+            "y": 6.422600616815994,
+            "heading": 0.053979855524806185,
+            "angularVelocity": -2.641525047123925e-8,
+            "velocityX": 3.7176847451316233,
+            "velocityY": 0.6996134536813446,
+            "timestamp": 1.447775890520232
+        },
+        {
+            "x": 4.7253380303410815,
+            "y": 6.519536384153899,
+            "heading": 0.05397985339265137,
+            "angularVelocity": -2.5407583503873633e-8,
+            "velocityX": 3.602266928321921,
+            "velocityY": 1.1551241886794503,
+            "timestamp": 1.5316939387161792
+        },
+        {
+            "x": 5.027633110558163,
+            "y": 6.616472555302148,
+            "heading": 0.053979851260496835,
+            "angularVelocity": -2.5407580227573106e-8,
+            "velocityX": 3.6022653852867035,
+            "velocityY": 1.1551290006400656,
+            "timestamp": 1.6156119869121264
+        },
+        {
+            "x": 5.329928190774575,
+            "y": 6.71340872645249,
+            "heading": 0.05397984912834238,
+            "angularVelocity": -2.540757910781892e-8,
+            "velocityX": 3.602265385278706,
+            "velocityY": 1.1551290006650068,
+            "timestamp": 1.6995300351080735
+        },
+        {
+            "x": 5.632223270990985,
+            "y": 6.810344897602833,
+            "heading": 0.053979846996187915,
+            "angularVelocity": -2.5407579299100972e-8,
+            "velocityX": 3.602265385278706,
+            "velocityY": 1.155129000665007,
+            "timestamp": 1.7834480833040207
+        },
+        {
+            "x": 5.934518351207396,
+            "y": 6.907281068753174,
+            "heading": 0.05397984486403343,
+            "angularVelocity": -2.5407579624099417e-8,
+            "velocityX": 3.602265385278706,
+            "velocityY": 1.1551290006650068,
+            "timestamp": 1.8673661314999679
+        },
+        {
+            "x": 6.236813431423807,
+            "y": 7.004217239903516,
+            "heading": 0.053979842731878905,
+            "angularVelocity": -2.5407579955507513e-8,
+            "velocityX": 3.602265385278706,
+            "velocityY": 1.1551290006650072,
+            "timestamp": 1.951284179695915
+        },
+        {
+            "x": 6.539108511640218,
+            "y": 7.1011534110538586,
+            "heading": 0.053979840599724424,
+            "angularVelocity": -2.5407579566600852e-8,
+            "velocityX": 3.6022653852787063,
+            "velocityY": 1.155129000665007,
+            "timestamp": 2.0352022278918622
+        },
+        {
+            "x": 6.841403591857615,
+            "y": 7.198089582201063,
+            "heading": 0.05397983846756001,
+            "angularVelocity": -2.5407697930922655e-8,
+            "velocityX": 3.6022653852904623,
+            "velocityY": 1.1551290006276251,
+            "timestamp": 2.119120276087809
+        },
+        {
+            "x": 7.120935706136836,
+            "y": 7.287726381323771,
+            "heading": 0.035986533919804255,
+            "angularVelocity": -0.21441519356800662,
+            "velocityX": 3.331013057245073,
+            "velocityY": 1.0681468533849583,
+            "timestamp": 2.203038324283756
+        },
+        {
+            "x": 7.344561416731701,
+            "y": 7.359435826630496,
+            "heading": 0.021591909406666148,
+            "angularVelocity": -0.17153192695269756,
+            "velocityX": 2.664810674250942,
+            "velocityY": 0.8545175543082761,
+            "timestamp": 2.286956372479703
+        },
+        {
+            "x": 7.512280706030795,
+            "y": 7.413217912582047,
+            "heading": 0.010795955954566746,
+            "angularVelocity": -0.12864876727003446,
+            "velocityX": 1.9986080813923648,
+            "velocityY": 0.6408881892244497,
+            "timestamp": 2.37087442067565
+        },
+        {
+            "x": 7.624093568104708,
+            "y": 7.449072637343766,
+            "heading": 0.0035986551256689905,
+            "angularVelocity": -0.08576582729978673,
+            "velocityX": 1.3324054178766367,
+            "velocityY": 0.42725880227814755,
+            "timestamp": 2.454792468871597
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0.31729069510085045,
-            "velocityX": 0.3272201077973249,
-            "velocityY": 0.10910668200355174,
-            "timestamp": 2.4566364837347034
+            "heading": -6.709954361314771e-31,
+            "angularVelocity": -0.04288296979061891,
+            "velocityX": 0.6662027191665668,
+            "velocityY": 0.21362940442053102,
+            "timestamp": 2.538710517067544
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 5.5875375349932e-23,
-            "velocityY": -3.709612464642882e-23,
-            "timestamp": 2.499314354346674
+            "heading": 6.378058636012386e-33,
+            "angularVelocity": 8.071845383822447e-30,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 2.622628565263491
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.1.traj
new file mode 100644
index 00000000..56b4df9b
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.1.traj	
@@ -0,0 +1,661 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": -1.04,
+            "angularVelocity": 0,
+            "velocityX": -4.815368277433195e-31,
+            "velocityY": 1.7671411030208604e-30,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7256756209676614,
+            "y": 4.374259536946564,
+            "heading": -0.9853384433657985,
+            "angularVelocity": 0.9927336490852509,
+            "velocityX": 0.2846921559393299,
+            "velocityY": 0.25897400328871373,
+            "timestamp": 0.05506165393280401
+        },
+        {
+            "x": 0.7571509327835594,
+            "y": 4.40368458563067,
+            "heading": -0.8810495211913171,
+            "angularVelocity": 1.8940390403219183,
+            "velocityX": 0.5716376019938672,
+            "velocityY": 0.5344018310849077,
+            "timestamp": 0.11012330786560802
+        },
+        {
+            "x": 0.8047782151983651,
+            "y": 4.4492655723711865,
+            "heading": -0.7351323547259855,
+            "angularVelocity": 2.6500687146592434,
+            "velocityX": 0.8649809624852585,
+            "velocityY": 0.8278172463956943,
+            "timestamp": 0.16518496179841202
+        },
+        {
+            "x": 0.8693373364829333,
+            "y": 4.511878147553763,
+            "heading": -0.5600607568180229,
+            "angularVelocity": 3.179555741677643,
+            "velocityX": 1.1724878690220806,
+            "velocityY": 1.1371357507548814,
+            "timestamp": 0.22024661573121604
+        },
+        {
+            "x": 0.9516435423656624,
+            "y": 4.592143379904835,
+            "heading": -0.3723491539523138,
+            "angularVelocity": 3.4091166802965285,
+            "velocityX": 1.4948008278694735,
+            "velocityY": 1.4577337696599522,
+            "timestamp": 0.27530826966402006
+        },
+        {
+            "x": 1.0514451430006502,
+            "y": 4.68995617229961,
+            "heading": -0.20247463803020274,
+            "angularVelocity": 3.085169147472857,
+            "velocityX": 1.8125427317637794,
+            "velocityY": 1.7764230713854863,
+            "timestamp": 0.33036992359682404
+        },
+        {
+            "x": 1.163482365369112,
+            "y": 4.8045671318094305,
+            "heading": -0.08453465328971378,
+            "angularVelocity": 2.1419622607883397,
+            "velocityX": 2.034759480799993,
+            "velocityY": 2.0815023037586085,
+            "timestamp": 0.38543157752962803
+        },
+        {
+            "x": 1.290791904505612,
+            "y": 4.932877435439672,
+            "heading": -0.017113580237308034,
+            "angularVelocity": 1.2244650902572813,
+            "velocityX": 2.3121270438301877,
+            "velocityY": 2.330302387705078,
+            "timestamp": 0.440493231462432
+        },
+        {
+            "x": 1.4344972301022125,
+            "y": 5.07384452901374,
+            "heading": -0.000001124802148976757,
+            "angularVelocity": 0.3107871669853269,
+            "velocityX": 2.6098984562178145,
+            "velocityY": 2.560168166154319,
+            "timestamp": 0.495554885395236
+        },
+        {
+            "x": 1.5830888632571813,
+            "y": 5.219814728179112,
+            "heading": -9.965376040774715e-7,
+            "angularVelocity": 0.0000023294713396498257,
+            "velocityX": 2.6986409332390493,
+            "velocityY": 2.6510318659074916,
+            "timestamp": 0.55061653932804
+        },
+        {
+            "x": 1.7316392407262138,
+            "y": 5.365826918457485,
+            "heading": -8.792165857555993e-7,
+            "angularVelocity": 0.000002130720927018926,
+            "velocityX": 2.697891669756242,
+            "velocityY": 2.6517944858193996,
+            "timestamp": 0.605678193260844
+        },
+        {
+            "x": 1.8801815584157808,
+            "y": 5.511847308171012,
+            "heading": -7.618956854973843e-7,
+            "angularVelocity": 0.000002130718782810582,
+            "velocityX": 2.69774529239705,
+            "velocityY": 2.6519433995297197,
+            "timestamp": 0.6607398471936481
+        },
+        {
+            "x": 2.0287247432411455,
+            "y": 5.657866815749449,
+            "heading": -6.445747704723799e-7,
+            "angularVelocity": 0.000002130719050997138,
+            "velocityX": 2.697761040854699,
+            "velocityY": 2.651927378662928,
+            "timestamp": 0.7158015011264521
+        },
+        {
+            "x": 2.1772589110251723,
+            "y": 5.8038954956427204,
+            "heading": -5.272539834240943e-7,
+            "angularVelocity": 0.000002130716726753334,
+            "velocityX": 2.6975972782370956,
+            "velocityY": 2.652093961273629,
+            "timestamp": 0.7708631550592562
+        },
+        {
+            "x": 2.325792733071576,
+            "y": 5.949924527226785,
+            "heading": -4.0993217710172413e-7,
+            "angularVelocity": 0.000002130735238257545,
+            "velocityX": 2.6975909991452314,
+            "velocityY": 2.6521003484836045,
+            "timestamp": 0.8259248089920602
+        },
+        {
+            "x": 2.481459175645397,
+            "y": 6.088325151306142,
+            "heading": -2.908915465695403e-7,
+            "angularVelocity": 0.0000021619515945987695,
+            "velocityX": 2.8271297982536217,
+            "velocityY": 2.5135573342462116,
+            "timestamp": 0.8809864629248643
+        },
+        {
+            "x": 2.6527099574524438,
+            "y": 6.206899777175588,
+            "heading": -1.5899541452377036e-7,
+            "angularVelocity": 0.0000023954262651640655,
+            "velocityX": 3.1101641446636745,
+            "velocityY": 2.1534882699533155,
+            "timestamp": 0.9360481168576683
+        },
+        {
+            "x": 2.837061882019043,
+            "y": 6.303859233856201,
+            "heading": 0,
+            "angularVelocity": 0.0000028875887873359224,
+            "velocityX": 3.348100018792042,
+            "velocityY": 1.760925249319746,
+            "timestamp": 0.9911097707904724
+        },
+        {
+            "x": 2.932152988543463,
+            "y": 6.346714053639414,
+            "heading": 2.0172187169325966e-12,
+            "angularVelocity": 7.316288159012044e-11,
+            "velocityX": 3.4488770423117883,
+            "velocityY": 1.5543094355402358,
+            "timestamp": 1.018681384784641
+        },
+        {
+            "x": 3.0296712234656686,
+            "y": 6.383714706348048,
+            "heading": 2.030359324162707e-12,
+            "angularVelocity": 4.765991109637323e-13,
+            "velocityX": 3.5369070139469976,
+            "velocityY": 1.3419835602475594,
+            "timestamp": 1.0462529987788096
+        },
+        {
+            "x": 3.129255935132477,
+            "y": 6.414726637699066,
+            "heading": 2.0283850653761223e-12,
+            "angularVelocity": -7.160477709378183e-14,
+            "velocityX": 3.611856443643096,
+            "velocityY": 1.1247775106178561,
+            "timestamp": 1.0738246127729782
+        },
+        {
+            "x": 3.2305380260225394,
+            "y": 6.439642353966136,
+            "heading": 2.0263773134602995e-12,
+            "angularVelocity": -7.281954448876825e-14,
+            "velocityX": 3.6734190066355246,
+            "velocityY": 0.9036727510191612,
+            "timestamp": 1.1013962267671469
+        },
+        {
+            "x": 3.3331381427053266,
+            "y": 6.458405909393185,
+            "heading": 2.0244535837319738e-12,
+            "angularVelocity": -6.977212868461473e-14,
+            "velocityX": 3.7212227294451083,
+            "velocityY": 0.6805388843797584,
+            "timestamp": 1.1289678407613155
+        },
+        {
+            "x": 3.43663787164561,
+            "y": 6.471315638605137,
+            "heading": 2.022533482460756e-12,
+            "angularVelocity": -6.96405259775829e-14,
+            "velocityX": 3.753850933863969,
+            "velocityY": 0.4682253717820687,
+            "timestamp": 1.1565394547554841
+        },
+        {
+            "x": 3.5403567621307404,
+            "y": 6.4823268492843,
+            "heading": 2.0206101184182045e-12,
+            "angularVelocity": -6.975886505788547e-14,
+            "velocityX": 3.7617997447283904,
+            "velocityY": 0.39936764973541705,
+            "timestamp": 1.1841110687496528
+        },
+        {
+            "x": 3.6440832149520914,
+            "y": 6.493266592802732,
+            "heading": 2.018711397348351e-12,
+            "angularVelocity": -6.886508434036338e-14,
+            "velocityX": 3.7620740245091877,
+            "velocityY": 0.3967755939732401,
+            "timestamp": 1.2116826827438214
+        },
+        {
+            "x": 3.747806936839693,
+            "y": 6.504232199027323,
+            "heading": 2.016809036122773e-12,
+            "angularVelocity": -6.89971097222047e-14,
+            "velocityX": 3.7619749757661194,
+            "velocityY": 0.39771361326329024,
+            "timestamp": 1.23925429673799
+        },
+        {
+            "x": 3.8515283310140283,
+            "y": 6.515219800794399,
+            "heading": 2.0149121825042556e-12,
+            "angularVelocity": -6.87973533298754e-14,
+            "velocityX": 3.761890551500436,
+            "velocityY": 0.3985113736873028,
+            "timestamp": 1.2668259107321587
+        },
+        {
+            "x": 3.9552475387122907,
+            "y": 6.52622802304771,
+            "heading": 2.0130014782113644e-12,
+            "angularVelocity": -6.92997060168648e-14,
+            "velocityX": 3.7618112497917684,
+            "velocityY": 0.3992592619503583,
+            "timestamp": 1.2943975247263273
+        },
+        {
+            "x": 4.058964880445898,
+            "y": 6.537253812271398,
+            "heading": 2.011079593605957e-12,
+            "angularVelocity": -6.970520685257517e-14,
+            "velocityX": 3.7617435727707904,
+            "velocityY": 0.3998964016833969,
+            "timestamp": 1.321969138720496
+        },
+        {
+            "x": 4.162680825175896,
+            "y": 6.5482927349999205,
+            "heading": 2.009190619489212e-12,
+            "angularVelocity": -6.851157050326969e-14,
+            "velocityX": 3.761692904585482,
+            "velocityY": 0.4003727432280311,
+            "timestamp": 1.3495407527146646
+        },
+        {
+            "x": 4.2663959076467215,
+            "y": 6.559339756077265,
+            "heading": 2.007295781849634e-12,
+            "angularVelocity": -6.87242357042844e-14,
+            "velocityX": 3.7616616311467697,
+            "velocityY": 0.40066646375103865,
+            "timestamp": 1.3771123667088332
+        },
+        {
+            "x": 4.370110729285335,
+            "y": 6.570389225581033,
+            "heading": 2.0053851186045697e-12,
+            "angularVelocity": -6.929821753536312e-14,
+            "velocityX": 3.761652170976742,
+            "velocityY": 0.40075526616572615,
+            "timestamp": 1.4046839807030018
+        },
+        {
+            "x": 4.473826107172322,
+            "y": 6.581433472559441,
+            "heading": 2.003480405786031e-12,
+            "angularVelocity": -6.908240040814547e-14,
+            "velocityX": 3.7616723456572294,
+            "velocityY": 0.4005658493780133,
+            "timestamp": 1.4322555946971705
+        },
+        {
+            "x": 4.577543170052599,
+            "y": 6.592461884602437,
+            "heading": 2.001567952942652e-12,
+            "angularVelocity": -6.936312480979614e-14,
+            "velocityX": 3.7617334589969245,
+            "velocityY": 0.399991529116124,
+            "timestamp": 1.459827208691339
+        },
+        {
+            "x": 4.681263099195036,
+            "y": 6.603463307611616,
+            "heading": 1.9996669629945417e-12,
+            "angularVelocity": -6.894737463787157e-14,
+            "velocityX": 3.761837415995553,
+            "velocityY": 0.3990126587978969,
+            "timestamp": 1.4873988226855077
+        },
+        {
+            "x": 4.784986899417181,
+            "y": 6.61442817334549,
+            "heading": 1.997767883198675e-12,
+            "angularVelocity": -6.88780949705506e-14,
+            "velocityX": 3.7619778169075735,
+            "velocityY": 0.3976867561604882,
+            "timestamp": 1.5149704366796763
+        },
+        {
+            "x": 4.888715211119033,
+            "y": 6.625350277801733,
+            "heading": 1.9958591963322718e-12,
+            "angularVelocity": -6.922653588980761e-14,
+            "velocityX": 3.762141444605259,
+            "velocityY": 0.39613583941828256,
+            "timestamp": 1.542542050673845
+        },
+        {
+            "x": 4.992447807319009,
+            "y": 6.636231614926857,
+            "heading": 1.9939696519007763e-12,
+            "angularVelocity": -6.853225532454514e-14,
+            "velocityX": 3.7622968398643226,
+            "velocityY": 0.3946572415062904,
+            "timestamp": 1.5701136646680136
+        },
+        {
+            "x": 5.09618354900695,
+            "y": 6.6470829236468765,
+            "heading": 1.992073077512808e-12,
+            "angularVelocity": -6.878722613480481e-14,
+            "velocityX": 3.7624109241486314,
+            "velocityY": 0.3935681357313447,
+            "timestamp": 1.5976852786621822
+        },
+        {
+            "x": 5.199926346512542,
+            "y": 6.657866567184633,
+            "heading": 1.990167650654994e-12,
+            "angularVelocity": -6.910829798877678e-14,
+            "velocityX": 3.7626668329101425,
+            "velocityY": 0.3911139746618113,
+            "timestamp": 1.6252568926563509
+        },
+        {
+            "x": 5.303729435372611,
+            "y": 6.668053506977846,
+            "heading": 1.9882412477007472e-12,
+            "angularVelocity": -6.986908358991098e-14,
+            "velocityX": 3.764853551267859,
+            "velocityY": 0.3694720154839007,
+            "timestamp": 1.6528285066505195
+        },
+        {
+            "x": 5.40792279202557,
+            "y": 6.672807389271601,
+            "heading": 1.9863066963577084e-12,
+            "angularVelocity": -7.016461895491627e-14,
+            "velocityX": 3.779008246489486,
+            "velocityY": 0.17241944175311613,
+            "timestamp": 1.6804001206446881
+        },
+        {
+            "x": 5.512213692786393,
+            "y": 6.671303096171843,
+            "heading": 1.984343863250107e-12,
+            "angularVelocity": -7.119037640734205e-14,
+            "velocityX": 3.782546091892586,
+            "velocityY": -0.05455948646048947,
+            "timestamp": 1.7079717346388568
+        },
+        {
+            "x": 5.616221821617454,
+            "y": 6.663482363169505,
+            "heading": 1.9823084185583715e-12,
+            "angularVelocity": -7.382393974706967e-14,
+            "velocityX": 3.772290184136858,
+            "velocityY": -0.2836516209983201,
+            "timestamp": 1.7355433486330254
+        },
+        {
+            "x": 5.7195634841918945,
+            "y": 6.649363040924072,
+            "heading": 1.9801191994112705e-12,
+            "angularVelocity": -7.94012151883752e-14,
+            "velocityX": 3.7481179954212767,
+            "velocityY": -0.5120963266444406,
+            "timestamp": 1.763114962627194
+        },
+        {
+            "x": 5.834816669509981,
+            "y": 6.625475968242897,
+            "heading": 1.9780897732255602e-12,
+            "angularVelocity": -6.522540784880574e-14,
+            "velocityX": 3.704217712719512,
+            "velocityY": -0.7677264405820726,
+            "timestamp": 1.794229004580604
+        },
+        {
+            "x": 5.948163245427426,
+            "y": 6.593751483162918,
+            "heading": 1.9761554326166073e-12,
+            "angularVelocity": -6.216937380396162e-14,
+            "velocityX": 3.6429396118708874,
+            "velocityY": -1.0196195379349016,
+            "timestamp": 1.825343046534014
+        },
+        {
+            "x": 6.0590772896693,
+            "y": 6.554356506595256,
+            "heading": 1.974328708451782e-12,
+            "angularVelocity": -5.871059954922337e-14,
+            "velocityX": 3.564758458833813,
+            "velocityY": -1.2661478256826604,
+            "timestamp": 1.8564570884874239
+        },
+        {
+            "x": 6.167114375070259,
+            "y": 6.507645933449736,
+            "heading": 1.9725156971860347e-12,
+            "angularVelocity": -5.826986956344564e-14,
+            "velocityX": 3.472293492526525,
+            "velocityY": -1.5012698515774907,
+            "timestamp": 1.8875711304408338
+        },
+        {
+            "x": 6.273666318458446,
+            "y": 6.457640205385319,
+            "heading": 1.9706978622350046e-12,
+            "angularVelocity": -5.842490200021065e-14,
+            "velocityX": 3.424561281617029,
+            "velocityY": -1.6071755684828801,
+            "timestamp": 1.9186851723942437
+        },
+        {
+            "x": 6.3802136872176485,
+            "y": 6.4076247308783065,
+            "heading": 1.9688925756308956e-12,
+            "angularVelocity": -5.802160020867771e-14,
+            "velocityX": 3.4244142538170945,
+            "velocityY": -1.6074888174954127,
+            "timestamp": 1.9497992143476537
+        },
+        {
+            "x": 6.486760876374776,
+            "y": 6.3576088737324294,
+            "heading": 1.967091079960254e-12,
+            "angularVelocity": -5.789976010142433e-14,
+            "velocityX": 3.4244084814345546,
+            "velocityY": -1.607501115448455,
+            "timestamp": 1.9809132563010636
+        },
+        {
+            "x": 6.593299366426369,
+            "y": 6.307574489358799,
+            "heading": 1.9652854794360616e-12,
+            "angularVelocity": -5.803168954665514e-14,
+            "velocityX": 3.4241288936675214,
+            "velocityY": -1.6080965773861522,
+            "timestamp": 2.0120272982544734
+        },
+        {
+            "x": 6.6998332452145375,
+            "y": 6.257530287401571,
+            "heading": 1.963481306814657e-12,
+            "angularVelocity": -5.79857968421928e-14,
+            "velocityX": 3.423980688450087,
+            "velocityY": -1.608412112843002,
+            "timestamp": 2.043141340207883
+        },
+        {
+            "x": 6.806367665236628,
+            "y": 6.207487237643806,
+            "heading": 1.9616709532128727e-12,
+            "angularVelocity": -5.818445266643988e-14,
+            "velocityX": 3.4239980836191717,
+            "velocityY": -1.6083750813421427,
+            "timestamp": 2.074255382161293
+        },
+        {
+            "x": 6.912905936531216,
+            "y": 6.157452377320687,
+            "heading": 1.9503301873398137e-12,
+            "angularVelocity": -3.6449015032516537e-13,
+            "velocityX": 3.424121862863347,
+            "velocityY": -1.6081118743030238,
+            "timestamp": 2.1053694241147025
+        },
+        {
+            "x": 7.015182351606042,
+            "y": 6.109386281530938,
+            "heading": 7.52788173701755e-9,
+            "angularVelocity": 2.418821392510085e-7,
+            "velocityX": 3.287146531073864,
+            "velocityY": -1.5448361181009935,
+            "timestamp": 2.1364834660681122
+        },
+        {
+            "x": 7.110154929626141,
+            "y": 6.064756817323096,
+            "heading": 9.121631465411093e-9,
+            "angularVelocity": 5.122284110672045e-8,
+            "velocityX": 3.0524024542461334,
+            "velocityY": -1.4343833653841132,
+            "timestamp": 2.167597508021522
+        },
+        {
+            "x": 7.19782257145347,
+            "y": 6.02356173384038,
+            "heading": 9.19440476065517e-9,
+            "angularVelocity": 2.3389208664172086e-9,
+            "velocityX": 2.8176230513092126,
+            "velocityY": -1.3240029548204029,
+            "timestamp": 2.1987115499749317
+        },
+        {
+            "x": 7.278184934221974,
+            "y": 5.985800317976241,
+            "heading": 8.586659012919787e-9,
+            "angularVelocity": -1.9532843759144805e-8,
+            "velocityX": 2.582832628715854,
+            "velocityY": -1.2136454633793592,
+            "timestamp": 2.2298255919283414
+        },
+        {
+            "x": 7.351241854096833,
+            "y": 5.951472228972044,
+            "heading": 7.633275836953741e-9,
+            "angularVelocity": -3.064157105146478e-8,
+            "velocityX": 2.3480369404994192,
+            "velocityY": -1.1032989238626065,
+            "timestamp": 2.260939633881751
+        },
+        {
+            "x": 7.416993235519351,
+            "y": 5.9205772680990165,
+            "heading": 6.513007078345892e-9,
+            "angularVelocity": -3.600524486341296e-8,
+            "velocityX": 2.1132381810427594,
+            "velocityY": -0.9929587714545899,
+            "timestamp": 2.292053675835161
+        },
+        {
+            "x": 7.47543901596506,
+            "y": 5.89311530534189,
+            "heading": 5.336882897741431e-9,
+            "angularVelocity": -3.780042846669445e-8,
+            "velocityX": 1.8784374120603946,
+            "velocityY": -0.8826227977151127,
+            "timestamp": 2.3231677177885706
+        },
+        {
+            "x": 7.52657915135954,
+            "y": 5.869086249058835,
+            "heading": 4.180544199369271e-9,
+            "angularVelocity": -3.716452645721565e-8,
+            "velocityX": 1.643635226533715,
+            "velocityY": -0.7722897693284123,
+            "timestamp": 2.3542817597419803
+        },
+        {
+            "x": 7.570413608969555,
+            "y": 5.848490031193872,
+            "heading": 3.0988314877379083e-9,
+            "angularVelocity": -3.476606009342686e-8,
+            "velocityX": 1.4088319889664873,
+            "velocityY": -0.6619589282491846,
+            "timestamp": 2.38539580169539
+        },
+        {
+            "x": 7.606942363528039,
+            "y": 5.831326599216932,
+            "heading": 2.1333266690310216e-9,
+            "angularVelocity": -3.103115843804935e-8,
+            "velocityX": 1.1740279393200113,
+            "velocityY": -0.5516297754774387,
+            "timestamp": 2.4165098436487997
+        },
+        {
+            "x": 7.636165394942908,
+            "y": 5.817595911358632,
+            "heading": 1.3166428525752622e-9,
+            "angularVelocity": -2.6248077074736502e-8,
+            "velocityX": 0.9392232439174092,
+            "velocityY": -0.4413019651665061,
+            "timestamp": 2.4476238856022094
+        },
+        {
+            "x": 7.658082686855903,
+            "y": 5.80729793361315,
+            "heading": 6.750439688094791e-10,
+            "angularVelocity": -2.0620877517571588e-8,
+            "velocityX": 0.7044180227631803,
+            "velocityY": -0.3309752477968783,
+            "timestamp": 2.478737927555619
+        },
+        {
+            "x": 7.672694225690866,
+            "y": 5.800432637759072,
+            "heading": 2.301347828245949e-10,
+            "angularVelocity": -1.4299304537044113e-8,
+            "velocityX": 0.46961236527350575,
+            "velocityY": -0.22064943745862983,
+            "timestamp": 2.509851969509029
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -5.36340746801057e-34,
+            "angularVelocity": -7.3964922450186615e-9,
+            "velocityX": 0.23480633985361782,
+            "velocityY": -0.1103243919326242,
+            "timestamp": 2.5409660114624386
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -7.670487347269043e-34,
+            "angularVelocity": -7.414948879954709e-33,
+            "velocityX": 1.169026940913066e-31,
+            "velocityY": 1.7301899688761208e-31,
+            "timestamp": 2.5720800534158483
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.traj
index a6fd22bb..56b4df9b 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.traj	
@@ -5,603 +5,657 @@
             "y": 4.36,
             "heading": -1.04,
             "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
+            "velocityX": -4.815368277433195e-31,
+            "velocityY": 1.7671411030208604e-30,
             "timestamp": 0
         },
         {
-            "x": 0.7226799898554153,
-            "y": 4.366381680411806,
-            "heading": -1.026991220057009,
-            "angularVelocity": 0.3105337423596819,
-            "velocityX": 0.3026851649532591,
-            "velocityY": 0.15233766037290783,
-            "timestamp": 0.04189167928786116
-        },
-        {
-            "x": 0.7480755502425919,
-            "y": 4.379165077654378,
-            "heading": -1.001621000893582,
-            "angularVelocity": 0.6056147567896238,
-            "velocityX": 0.6062196793943158,
-            "velocityY": 0.3051536118838673,
-            "timestamp": 0.08378335857572232
-        },
-        {
-            "x": 0.7862289943958503,
-            "y": 4.398373013543058,
-            "heading": -0.964694126725153,
-            "angularVelocity": 0.8814846956762871,
-            "velocityX": 0.9107642568130221,
-            "velocityY": 0.4585143449774818,
-            "timestamp": 0.12567503786358347
-        },
-        {
-            "x": 0.8371909372554502,
-            "y": 4.424030289360705,
-            "heading": -0.9172220550063085,
-            "angularVelocity": 1.1332100437568369,
-            "velocityX": 1.2165170679697972,
-            "velocityY": 0.6124671116987632,
-            "timestamp": 0.16756671715144464
-        },
-        {
-            "x": 0.9010212440299602,
-            "y": 4.456163358075127,
-            "heading": -0.8604944995891576,
-            "angularVelocity": 1.354148517832005,
-            "velocityX": 1.523698926841682,
-            "velocityY": 0.767051339565964,
-            "timestamp": 0.2094583964393058
-        },
-        {
-            "x": 0.9777886193671033,
-            "y": 4.494801080149917,
-            "heading": -0.796206518536166,
-            "angularVelocity": 1.5346241102256366,
-            "velocityX": 1.8325208404664681,
-            "velocityY": 0.9223244981249834,
-            "timestamp": 0.25135007572716694
-        },
-        {
-            "x": 1.0675667159355893,
-            "y": 4.539976112871383,
-            "heading": -0.7267376359019178,
-            "angularVelocity": 1.658297872398206,
-            "velocityX": 2.143100923492954,
-            "velocityY": 1.07837722166836,
-            "timestamp": 0.2932417550150281
-        },
-        {
-            "x": 1.1704147696438687,
-            "y": 4.591719023811467,
-            "heading": -0.6558650979752687,
-            "angularVelocity": 1.6918046526529587,
-            "velocityX": 2.4550950321555027,
-            "velocityY": 1.23515962643868,
-            "timestamp": 0.3351334343028892
-        },
-        {
-            "x": 1.286251945776662,
-            "y": 4.64999734523705,
-            "heading": -0.5908433350340152,
-            "angularVelocity": 1.5521402829056499,
-            "velocityX": 2.765159528144268,
-            "velocityY": 1.3911669910657207,
-            "timestamp": 0.37702511359075036
-        },
-        {
-            "x": 1.4132089603720126,
-            "y": 4.714842752574763,
-            "heading": -0.5508840698501896,
-            "angularVelocity": 0.9538711711517488,
-            "velocityX": 3.0306021805179513,
-            "velocityY": 1.547930482617412,
-            "timestamp": 0.4189167928786115
-        },
-        {
-            "x": 1.550306465812279,
-            "y": 4.786404286835006,
-            "heading": -0.541265046364201,
-            "angularVelocity": 0.22961656466170327,
-            "velocityX": 3.272666738857444,
-            "velocityY": 1.7082517453765305,
-            "timestamp": 0.46080847216647264
-        },
-        {
-            "x": 1.6919561664396436,
-            "y": 4.857462558344818,
-            "heading": -0.5412650016128938,
-            "angularVelocity": 0.000001068262432444083,
-            "velocityX": 3.3813325948098276,
-            "velocityY": 1.6962383155263616,
-            "timestamp": 0.5027001514543338
-        },
-        {
-            "x": 1.8336059249814771,
-            "y": 4.928520714406238,
-            "heading": -0.541264956861776,
-            "angularVelocity": 0.0000010682579123299043,
-            "velocityX": 3.3813339772912605,
-            "velocityY": 1.6962355596475316,
-            "timestamp": 0.544591830742195
-        },
-        {
-            "x": 1.9752556835258195,
-            "y": 4.999578870462656,
-            "heading": -0.5412649121106583,
-            "angularVelocity": 0.0000010682579080478101,
-            "velocityX": 3.3813339773511535,
-            "velocityY": 1.696235559528139,
-            "timestamp": 0.5864835100300562
-        },
-        {
-            "x": 2.1169054420701623,
-            "y": 5.0706370265190746,
-            "heading": -0.5412648673595408,
-            "angularVelocity": 0.0000010682579032833847,
-            "velocityX": 3.381333977351157,
-            "velocityY": 1.6962355595281329,
-            "timestamp": 0.6283751893179174
-        },
-        {
-            "x": 2.258555200614505,
-            "y": 5.141695182575493,
-            "heading": -0.5412648226084235,
-            "angularVelocity": 0.0000010682578992307337,
-            "velocityX": 3.381333977351157,
-            "velocityY": 1.6962355595281327,
-            "timestamp": 0.6702668686057786
-        },
-        {
-            "x": 2.400204959158848,
-            "y": 5.212753338631911,
-            "heading": -0.5412647778573064,
-            "angularVelocity": 0.0000010682578937479676,
-            "velocityX": 3.3813339773511575,
-            "velocityY": 1.696235559528132,
-            "timestamp": 0.7121585478936397
-        },
-        {
-            "x": 2.5418547177031905,
-            "y": 5.2838114946883294,
-            "heading": -0.5412647331061895,
-            "angularVelocity": 0.0000010682578893581349,
-            "velocityX": 3.3813339773511584,
-            "velocityY": 1.696235559528132,
-            "timestamp": 0.7540502271815009
-        },
-        {
-            "x": 2.6835044762475335,
-            "y": 5.354869650744748,
-            "heading": -0.5412646883550728,
-            "angularVelocity": 0.0000010682578845109594,
-            "velocityX": 3.3813339773511584,
-            "velocityY": 1.6962355595281313,
-            "timestamp": 0.7959419064693621
-        },
-        {
-            "x": 2.8251542347918766,
-            "y": 5.425927806801167,
-            "heading": -0.5412646436039563,
-            "angularVelocity": 0.0000010682578796288815,
-            "velocityX": 3.3813339773511593,
-            "velocityY": 1.6962355595281313,
-            "timestamp": 0.8378335857572233
-        },
-        {
-            "x": 2.9668039933362196,
-            "y": 5.496985962857585,
-            "heading": -0.5412645988528401,
-            "angularVelocity": 0.0000010682578747880459,
-            "velocityX": 3.38133397735116,
-            "velocityY": 1.6962355595281309,
-            "timestamp": 0.8797252650450845
-        },
-        {
-            "x": 3.108453751880562,
-            "y": 5.5680441189140035,
-            "heading": -0.541264554101724,
-            "angularVelocity": 0.0000010682578699288912,
-            "velocityX": 3.38133397735116,
-            "velocityY": 1.69623555952813,
-            "timestamp": 0.9216169443329457
-        },
-        {
-            "x": 3.250103510424905,
-            "y": 5.639102274970423,
-            "heading": -0.5412645093506081,
-            "angularVelocity": 0.000001068257865104235,
-            "velocityX": 3.3813339773511606,
-            "velocityY": 1.69623555952813,
-            "timestamp": 0.9635086236208069
-        },
-        {
-            "x": 3.3917532689692482,
-            "y": 5.71016043102684,
-            "heading": -0.5412644645994924,
-            "angularVelocity": 0.0000010682578613220535,
-            "velocityX": 3.381333977351161,
-            "velocityY": 1.6962355595281293,
-            "timestamp": 1.005400302908668
-        },
-        {
-            "x": 3.5334030275135913,
-            "y": 5.7812185870832575,
-            "heading": -0.5412644198483769,
-            "angularVelocity": 0.0000010682578558700584,
-            "velocityX": 3.381333977351162,
-            "velocityY": 1.6962355595281287,
-            "timestamp": 1.0472919821965292
-        },
-        {
-            "x": 3.675052786057934,
-            "y": 5.852276743139676,
-            "heading": -0.5412643750972616,
-            "angularVelocity": 0.0000010682578513840427,
-            "velocityX": 3.381333977351162,
-            "velocityY": 1.6962355595281287,
-            "timestamp": 1.0891836614843904
-        },
-        {
-            "x": 3.816702544602277,
-            "y": 5.923334899196093,
-            "heading": -0.5412643303461464,
-            "angularVelocity": 0.0000010682578474926016,
-            "velocityX": 3.381333977351163,
-            "velocityY": 1.6962355595281278,
-            "timestamp": 1.1310753407722516
-        },
-        {
-            "x": 3.95835230314662,
-            "y": 5.994393055252511,
-            "heading": -0.5412642855950315,
-            "angularVelocity": 0.000001068257842976485,
-            "velocityX": 3.3813339773511637,
-            "velocityY": 1.6962355595281275,
-            "timestamp": 1.1729670200601128
-        },
-        {
-            "x": 4.100002061690963,
-            "y": 6.065451211308929,
-            "heading": -0.5412642408439168,
-            "angularVelocity": 0.0000010682578369198447,
-            "velocityX": 3.3813339773511637,
-            "velocityY": 1.6962355595281269,
-            "timestamp": 1.214858699347974
-        },
-        {
-            "x": 4.2416518202353055,
-            "y": 6.136509367365346,
-            "heading": -0.5412641960928023,
-            "angularVelocity": 0.0000010682578319796932,
-            "velocityX": 3.3813339773511646,
-            "velocityY": 1.696235559528127,
-            "timestamp": 1.2567503786358352
-        },
-        {
-            "x": 4.383301578779649,
-            "y": 6.207567523421765,
-            "heading": -0.5412641513416879,
-            "angularVelocity": 0.0000010682578277344439,
-            "velocityX": 3.381333977351165,
-            "velocityY": 1.6962355595281264,
-            "timestamp": 1.2986420579236964
-        },
-        {
-            "x": 4.524951337323992,
-            "y": 6.278625679478183,
-            "heading": -0.5412641065905739,
-            "angularVelocity": 0.0000010682578220378627,
-            "velocityX": 3.3813339773511655,
-            "velocityY": 1.6962355595281264,
-            "timestamp": 1.3405337372115576
-        },
-        {
-            "x": 4.666601095868337,
-            "y": 6.349683835534597,
-            "heading": -0.54126406183946,
-            "angularVelocity": 0.0000010682578180389232,
-            "velocityX": 3.381333977351213,
-            "velocityY": 1.6962355595280314,
-            "timestamp": 1.3824254164994187
-        },
-        {
-            "x": 4.8082508544580715,
-            "y": 6.420741991500528,
-            "heading": -0.5412640170883463,
-            "angularVelocity": 0.000001068257814314864,
-            "velocityX": 3.3813339784347316,
-            "velocityY": 1.6962355573681103,
-            "timestamp": 1.42431709578728
-        },
-        {
-            "x": 4.949901660762853,
-            "y": 6.491798058877407,
-            "heading": -0.54126397233727,
-            "angularVelocity": 0.0000010682569195340074,
-            "velocityX": 3.3813589885337056,
-            "velocityY": 1.6961857004731962,
-            "timestamp": 1.4662087750751411
-        },
-        {
-            "x": 5.0966757016987385,
-            "y": 6.551558383908352,
-            "heading": -0.5412639273749705,
-            "angularVelocity": 0.000001073299046263079,
-            "velocityX": 3.503656177813225,
-            "velocityY": 1.4265440308634512,
-            "timestamp": 1.5081004543630023
-        },
-        {
-            "x": 5.2483360143420885,
-            "y": 6.597526467706988,
-            "heading": -0.5412638804954443,
-            "angularVelocity": 0.000001119065339105663,
-            "velocityX": 3.6202968040790893,
-            "velocityY": 1.097308214425216,
-            "timestamp": 1.5499921336508635
-        },
-        {
-            "x": 5.403589728009299,
-            "y": 6.629310092724924,
-            "heading": -0.5412638294474256,
-            "angularVelocity": 0.0000012185717939310704,
-            "velocityX": 3.7060751993343666,
-            "velocityY": 0.7587097380253649,
-            "timestamp": 1.5918838129387247
-        },
-        {
-            "x": 5.561113217492597,
-            "y": 6.646638205385644,
-            "heading": -0.54126377105557,
-            "angularVelocity": 0.0000013938771748607603,
-            "velocityX": 3.760257219598817,
-            "velocityY": 0.4136409176067717,
-            "timestamp": 1.633775492226586
+            "x": 0.7256756209676614,
+            "y": 4.374259536946564,
+            "heading": -0.9853384433657985,
+            "angularVelocity": 0.9927336490852509,
+            "velocityX": 0.2846921559393299,
+            "velocityY": 0.25897400328871373,
+            "timestamp": 0.05506165393280401
+        },
+        {
+            "x": 0.7571509327835594,
+            "y": 4.40368458563067,
+            "heading": -0.8810495211913171,
+            "angularVelocity": 1.8940390403219183,
+            "velocityX": 0.5716376019938672,
+            "velocityY": 0.5344018310849077,
+            "timestamp": 0.11012330786560802
+        },
+        {
+            "x": 0.8047782151983651,
+            "y": 4.4492655723711865,
+            "heading": -0.7351323547259855,
+            "angularVelocity": 2.6500687146592434,
+            "velocityX": 0.8649809624852585,
+            "velocityY": 0.8278172463956943,
+            "timestamp": 0.16518496179841202
+        },
+        {
+            "x": 0.8693373364829333,
+            "y": 4.511878147553763,
+            "heading": -0.5600607568180229,
+            "angularVelocity": 3.179555741677643,
+            "velocityX": 1.1724878690220806,
+            "velocityY": 1.1371357507548814,
+            "timestamp": 0.22024661573121604
+        },
+        {
+            "x": 0.9516435423656624,
+            "y": 4.592143379904835,
+            "heading": -0.3723491539523138,
+            "angularVelocity": 3.4091166802965285,
+            "velocityX": 1.4948008278694735,
+            "velocityY": 1.4577337696599522,
+            "timestamp": 0.27530826966402006
+        },
+        {
+            "x": 1.0514451430006502,
+            "y": 4.68995617229961,
+            "heading": -0.20247463803020274,
+            "angularVelocity": 3.085169147472857,
+            "velocityX": 1.8125427317637794,
+            "velocityY": 1.7764230713854863,
+            "timestamp": 0.33036992359682404
+        },
+        {
+            "x": 1.163482365369112,
+            "y": 4.8045671318094305,
+            "heading": -0.08453465328971378,
+            "angularVelocity": 2.1419622607883397,
+            "velocityX": 2.034759480799993,
+            "velocityY": 2.0815023037586085,
+            "timestamp": 0.38543157752962803
+        },
+        {
+            "x": 1.290791904505612,
+            "y": 4.932877435439672,
+            "heading": -0.017113580237308034,
+            "angularVelocity": 1.2244650902572813,
+            "velocityX": 2.3121270438301877,
+            "velocityY": 2.330302387705078,
+            "timestamp": 0.440493231462432
+        },
+        {
+            "x": 1.4344972301022125,
+            "y": 5.07384452901374,
+            "heading": -0.000001124802148976757,
+            "angularVelocity": 0.3107871669853269,
+            "velocityX": 2.6098984562178145,
+            "velocityY": 2.560168166154319,
+            "timestamp": 0.495554885395236
+        },
+        {
+            "x": 1.5830888632571813,
+            "y": 5.219814728179112,
+            "heading": -9.965376040774715e-7,
+            "angularVelocity": 0.0000023294713396498257,
+            "velocityX": 2.6986409332390493,
+            "velocityY": 2.6510318659074916,
+            "timestamp": 0.55061653932804
+        },
+        {
+            "x": 1.7316392407262138,
+            "y": 5.365826918457485,
+            "heading": -8.792165857555993e-7,
+            "angularVelocity": 0.000002130720927018926,
+            "velocityX": 2.697891669756242,
+            "velocityY": 2.6517944858193996,
+            "timestamp": 0.605678193260844
+        },
+        {
+            "x": 1.8801815584157808,
+            "y": 5.511847308171012,
+            "heading": -7.618956854973843e-7,
+            "angularVelocity": 0.000002130718782810582,
+            "velocityX": 2.69774529239705,
+            "velocityY": 2.6519433995297197,
+            "timestamp": 0.6607398471936481
+        },
+        {
+            "x": 2.0287247432411455,
+            "y": 5.657866815749449,
+            "heading": -6.445747704723799e-7,
+            "angularVelocity": 0.000002130719050997138,
+            "velocityX": 2.697761040854699,
+            "velocityY": 2.651927378662928,
+            "timestamp": 0.7158015011264521
+        },
+        {
+            "x": 2.1772589110251723,
+            "y": 5.8038954956427204,
+            "heading": -5.272539834240943e-7,
+            "angularVelocity": 0.000002130716726753334,
+            "velocityX": 2.6975972782370956,
+            "velocityY": 2.652093961273629,
+            "timestamp": 0.7708631550592562
+        },
+        {
+            "x": 2.325792733071576,
+            "y": 5.949924527226785,
+            "heading": -4.0993217710172413e-7,
+            "angularVelocity": 0.000002130735238257545,
+            "velocityX": 2.6975909991452314,
+            "velocityY": 2.6521003484836045,
+            "timestamp": 0.8259248089920602
+        },
+        {
+            "x": 2.481459175645397,
+            "y": 6.088325151306142,
+            "heading": -2.908915465695403e-7,
+            "angularVelocity": 0.0000021619515945987695,
+            "velocityX": 2.8271297982536217,
+            "velocityY": 2.5135573342462116,
+            "timestamp": 0.8809864629248643
+        },
+        {
+            "x": 2.6527099574524438,
+            "y": 6.206899777175588,
+            "heading": -1.5899541452377036e-7,
+            "angularVelocity": 0.0000023954262651640655,
+            "velocityX": 3.1101641446636745,
+            "velocityY": 2.1534882699533155,
+            "timestamp": 0.9360481168576683
+        },
+        {
+            "x": 2.837061882019043,
+            "y": 6.303859233856201,
+            "heading": 0,
+            "angularVelocity": 0.0000028875887873359224,
+            "velocityX": 3.348100018792042,
+            "velocityY": 1.760925249319746,
+            "timestamp": 0.9911097707904724
+        },
+        {
+            "x": 2.932152988543463,
+            "y": 6.346714053639414,
+            "heading": 2.0172187169325966e-12,
+            "angularVelocity": 7.316288159012044e-11,
+            "velocityX": 3.4488770423117883,
+            "velocityY": 1.5543094355402358,
+            "timestamp": 1.018681384784641
+        },
+        {
+            "x": 3.0296712234656686,
+            "y": 6.383714706348048,
+            "heading": 2.030359324162707e-12,
+            "angularVelocity": 4.765991109637323e-13,
+            "velocityX": 3.5369070139469976,
+            "velocityY": 1.3419835602475594,
+            "timestamp": 1.0462529987788096
+        },
+        {
+            "x": 3.129255935132477,
+            "y": 6.414726637699066,
+            "heading": 2.0283850653761223e-12,
+            "angularVelocity": -7.160477709378183e-14,
+            "velocityX": 3.611856443643096,
+            "velocityY": 1.1247775106178561,
+            "timestamp": 1.0738246127729782
+        },
+        {
+            "x": 3.2305380260225394,
+            "y": 6.439642353966136,
+            "heading": 2.0263773134602995e-12,
+            "angularVelocity": -7.281954448876825e-14,
+            "velocityX": 3.6734190066355246,
+            "velocityY": 0.9036727510191612,
+            "timestamp": 1.1013962267671469
+        },
+        {
+            "x": 3.3331381427053266,
+            "y": 6.458405909393185,
+            "heading": 2.0244535837319738e-12,
+            "angularVelocity": -6.977212868461473e-14,
+            "velocityX": 3.7212227294451083,
+            "velocityY": 0.6805388843797584,
+            "timestamp": 1.1289678407613155
+        },
+        {
+            "x": 3.43663787164561,
+            "y": 6.471315638605137,
+            "heading": 2.022533482460756e-12,
+            "angularVelocity": -6.96405259775829e-14,
+            "velocityX": 3.753850933863969,
+            "velocityY": 0.4682253717820687,
+            "timestamp": 1.1565394547554841
+        },
+        {
+            "x": 3.5403567621307404,
+            "y": 6.4823268492843,
+            "heading": 2.0206101184182045e-12,
+            "angularVelocity": -6.975886505788547e-14,
+            "velocityX": 3.7617997447283904,
+            "velocityY": 0.39936764973541705,
+            "timestamp": 1.1841110687496528
+        },
+        {
+            "x": 3.6440832149520914,
+            "y": 6.493266592802732,
+            "heading": 2.018711397348351e-12,
+            "angularVelocity": -6.886508434036338e-14,
+            "velocityX": 3.7620740245091877,
+            "velocityY": 0.3967755939732401,
+            "timestamp": 1.2116826827438214
+        },
+        {
+            "x": 3.747806936839693,
+            "y": 6.504232199027323,
+            "heading": 2.016809036122773e-12,
+            "angularVelocity": -6.89971097222047e-14,
+            "velocityX": 3.7619749757661194,
+            "velocityY": 0.39771361326329024,
+            "timestamp": 1.23925429673799
+        },
+        {
+            "x": 3.8515283310140283,
+            "y": 6.515219800794399,
+            "heading": 2.0149121825042556e-12,
+            "angularVelocity": -6.87973533298754e-14,
+            "velocityX": 3.761890551500436,
+            "velocityY": 0.3985113736873028,
+            "timestamp": 1.2668259107321587
+        },
+        {
+            "x": 3.9552475387122907,
+            "y": 6.52622802304771,
+            "heading": 2.0130014782113644e-12,
+            "angularVelocity": -6.92997060168648e-14,
+            "velocityX": 3.7618112497917684,
+            "velocityY": 0.3992592619503583,
+            "timestamp": 1.2943975247263273
+        },
+        {
+            "x": 4.058964880445898,
+            "y": 6.537253812271398,
+            "heading": 2.011079593605957e-12,
+            "angularVelocity": -6.970520685257517e-14,
+            "velocityX": 3.7617435727707904,
+            "velocityY": 0.3998964016833969,
+            "timestamp": 1.321969138720496
+        },
+        {
+            "x": 4.162680825175896,
+            "y": 6.5482927349999205,
+            "heading": 2.009190619489212e-12,
+            "angularVelocity": -6.851157050326969e-14,
+            "velocityX": 3.761692904585482,
+            "velocityY": 0.4003727432280311,
+            "timestamp": 1.3495407527146646
+        },
+        {
+            "x": 4.2663959076467215,
+            "y": 6.559339756077265,
+            "heading": 2.007295781849634e-12,
+            "angularVelocity": -6.87242357042844e-14,
+            "velocityX": 3.7616616311467697,
+            "velocityY": 0.40066646375103865,
+            "timestamp": 1.3771123667088332
+        },
+        {
+            "x": 4.370110729285335,
+            "y": 6.570389225581033,
+            "heading": 2.0053851186045697e-12,
+            "angularVelocity": -6.929821753536312e-14,
+            "velocityX": 3.761652170976742,
+            "velocityY": 0.40075526616572615,
+            "timestamp": 1.4046839807030018
+        },
+        {
+            "x": 4.473826107172322,
+            "y": 6.581433472559441,
+            "heading": 2.003480405786031e-12,
+            "angularVelocity": -6.908240040814547e-14,
+            "velocityX": 3.7616723456572294,
+            "velocityY": 0.4005658493780133,
+            "timestamp": 1.4322555946971705
+        },
+        {
+            "x": 4.577543170052599,
+            "y": 6.592461884602437,
+            "heading": 2.001567952942652e-12,
+            "angularVelocity": -6.936312480979614e-14,
+            "velocityX": 3.7617334589969245,
+            "velocityY": 0.399991529116124,
+            "timestamp": 1.459827208691339
+        },
+        {
+            "x": 4.681263099195036,
+            "y": 6.603463307611616,
+            "heading": 1.9996669629945417e-12,
+            "angularVelocity": -6.894737463787157e-14,
+            "velocityX": 3.761837415995553,
+            "velocityY": 0.3990126587978969,
+            "timestamp": 1.4873988226855077
+        },
+        {
+            "x": 4.784986899417181,
+            "y": 6.61442817334549,
+            "heading": 1.997767883198675e-12,
+            "angularVelocity": -6.88780949705506e-14,
+            "velocityX": 3.7619778169075735,
+            "velocityY": 0.3976867561604882,
+            "timestamp": 1.5149704366796763
+        },
+        {
+            "x": 4.888715211119033,
+            "y": 6.625350277801733,
+            "heading": 1.9958591963322718e-12,
+            "angularVelocity": -6.922653588980761e-14,
+            "velocityX": 3.762141444605259,
+            "velocityY": 0.39613583941828256,
+            "timestamp": 1.542542050673845
+        },
+        {
+            "x": 4.992447807319009,
+            "y": 6.636231614926857,
+            "heading": 1.9939696519007763e-12,
+            "angularVelocity": -6.853225532454514e-14,
+            "velocityX": 3.7622968398643226,
+            "velocityY": 0.3946572415062904,
+            "timestamp": 1.5701136646680136
+        },
+        {
+            "x": 5.09618354900695,
+            "y": 6.6470829236468765,
+            "heading": 1.992073077512808e-12,
+            "angularVelocity": -6.878722613480481e-14,
+            "velocityX": 3.7624109241486314,
+            "velocityY": 0.3935681357313447,
+            "timestamp": 1.5976852786621822
+        },
+        {
+            "x": 5.199926346512542,
+            "y": 6.657866567184633,
+            "heading": 1.990167650654994e-12,
+            "angularVelocity": -6.910829798877678e-14,
+            "velocityX": 3.7626668329101425,
+            "velocityY": 0.3911139746618113,
+            "timestamp": 1.6252568926563509
+        },
+        {
+            "x": 5.303729435372611,
+            "y": 6.668053506977846,
+            "heading": 1.9882412477007472e-12,
+            "angularVelocity": -6.986908358991098e-14,
+            "velocityX": 3.764853551267859,
+            "velocityY": 0.3694720154839007,
+            "timestamp": 1.6528285066505195
+        },
+        {
+            "x": 5.40792279202557,
+            "y": 6.672807389271601,
+            "heading": 1.9863066963577084e-12,
+            "angularVelocity": -7.016461895491627e-14,
+            "velocityX": 3.779008246489486,
+            "velocityY": 0.17241944175311613,
+            "timestamp": 1.6804001206446881
+        },
+        {
+            "x": 5.512213692786393,
+            "y": 6.671303096171843,
+            "heading": 1.984343863250107e-12,
+            "angularVelocity": -7.119037640734205e-14,
+            "velocityX": 3.782546091892586,
+            "velocityY": -0.05455948646048947,
+            "timestamp": 1.7079717346388568
+        },
+        {
+            "x": 5.616221821617454,
+            "y": 6.663482363169505,
+            "heading": 1.9823084185583715e-12,
+            "angularVelocity": -7.382393974706967e-14,
+            "velocityX": 3.772290184136858,
+            "velocityY": -0.2836516209983201,
+            "timestamp": 1.7355433486330254
         },
         {
             "x": 5.7195634841918945,
             "y": 6.649363040924072,
-            "heading": -0.5412637028328066,
-            "angularVelocity": 0.000001628551649307522,
-            "velocityX": 3.7823804009024578,
-            "velocityY": 0.06504479134637085,
-            "timestamp": 1.675667171514447
-        },
-        {
-            "x": 5.8386728592662305,
-            "y": 6.64312635201416,
-            "heading": -0.5412636336326876,
-            "angularVelocity": 0.000002194803906563273,
-            "velocityX": 3.777764046962115,
-            "velocityY": -0.19780759592812253,
-            "timestamp": 1.7071962383364254
-        },
-        {
-            "x": 5.95706146956484,
-            "y": 6.62863229973747,
-            "heading": -0.5412635727602775,
-            "angularVelocity": 0.0000019306759164775456,
-            "velocityX": 3.7549037200201067,
-            "velocityY": -0.459704448549871,
-            "timestamp": 1.7387253051584037
-        },
-        {
-            "x": 6.0741575637122,
-            "y": 6.605950895283392,
-            "heading": -0.5412635175985968,
-            "angularVelocity": 0.000001749550058024173,
-            "velocityX": 3.713909289117771,
-            "velocityY": -0.7193807727372066,
-            "timestamp": 1.770254371980382
-        },
-        {
-            "x": 6.189395638406918,
-            "y": 6.575191700510336,
-            "heading": -0.5412634663439824,
-            "angularVelocity": 0.0000016256305579593937,
-            "velocityX": 3.654978922953296,
-            "velocityY": -0.9755821492190131,
-            "timestamp": 1.8017834388023604
-        },
-        {
-            "x": 6.302219177732501,
-            "y": 6.536503313694644,
-            "heading": -0.5412634176727114,
-            "angularVelocity": 0.0000015436952600896132,
-            "velocityX": 3.578397672300798,
-            "velocityY": -1.2270704691051513,
-            "timestamp": 1.8333125056243387
-        },
-        {
-            "x": 6.412083374588637,
-            "y": 6.49007271186475,
-            "heading": -0.5412633705487488,
-            "angularVelocity": 0.0000014946196396789507,
-            "velocityX": 3.484536902930231,
-            "velocityY": -1.4726284825381617,
-            "timestamp": 1.864841572446317
-        },
-        {
-            "x": 6.518458035998333,
-            "y": 6.436124829047777,
-            "heading": -0.5412633241017745,
-            "angularVelocity": 0.0000014731477658308475,
-            "velocityX": 3.3738601275551834,
-            "velocityY": -1.7110523163142606,
-            "timestamp": 1.8963706392682953
-        },
-        {
-            "x": 6.622870515934444,
-            "y": 6.378470889852871,
-            "heading": -0.5412632777559726,
-            "angularVelocity": 0.0000014699389024112294,
-            "velocityX": 3.3116260790606757,
-            "velocityY": -1.8285964351699866,
-            "timestamp": 1.9278997060902736
-        },
-        {
-            "x": 6.7272827038996414,
-            "y": 6.320816421895741,
-            "heading": -0.5412632314101712,
-            "angularVelocity": 0.0000014699388892769531,
-            "velocityX": 3.311616818687876,
-            "velocityY": -1.8286132057971307,
-            "timestamp": 1.959428772912252
-        },
-        {
-            "x": 6.831694893880165,
-            "y": 6.263161957588452,
-            "heading": -0.5412631850642508,
-            "angularVelocity": 0.0000014699426583817394,
-            "velocityX": 3.311616882607505,
-            "velocityY": -1.828613090035986,
-            "timestamp": 1.9909578397342302
-        },
-        {
-            "x": 6.935462774507961,
-            "y": 6.205910954727142,
-            "heading": -0.5387292297574777,
-            "angularVelocity": 0.08036886473933494,
-            "velocityX": 3.2911814743423053,
-            "velocityY": -1.815816598206477,
-            "timestamp": 2.0224869065562086
-        },
-        {
-            "x": 7.033610221214887,
-            "y": 6.151825220973909,
-            "heading": -0.514724354544779,
-            "angularVelocity": 0.7613569836442236,
-            "velocityX": 3.11291949302188,
-            "velocityY": -1.715424502051262,
-            "timestamp": 2.054015973378187
-        },
-        {
-            "x": 7.126006777010342,
-            "y": 6.101041958255997,
-            "heading": -0.47044950446923295,
-            "angularVelocity": 1.4042550109565184,
-            "velocityX": 2.930519838000589,
-            "velocityY": -1.610680804625417,
-            "timestamp": 2.0855450402001656
-        },
-        {
-            "x": 7.2116589199714145,
-            "y": 6.053937431211348,
-            "heading": -0.4144026036835486,
-            "angularVelocity": 1.7776263757548019,
-            "velocityX": 2.7166088817245364,
-            "velocityY": -1.4940032101366643,
-            "timestamp": 2.117074107022144
-        },
-        {
-            "x": 7.290084520639544,
-            "y": 6.01082680132011,
-            "heading": -0.35575031779267857,
-            "angularVelocity": 1.8602607626174494,
-            "velocityX": 2.4874063387585092,
-            "velocityY": -1.3673297130756235,
-            "timestamp": 2.1486031738441227
-        },
-        {
-            "x": 7.3612581622075055,
-            "y": 5.971723406014521,
-            "heading": -0.2979353118382045,
-            "angularVelocity": 1.8337049517168786,
-            "velocityX": 2.2573976568931324,
-            "velocityY": -1.2402331958119186,
-            "timestamp": 2.1801322406661012
-        },
-        {
-            "x": 7.425208576949436,
-            "y": 5.936609973900717,
-            "heading": -0.2429131400794928,
-            "angularVelocity": 1.7451252861171487,
-            "velocityX": 2.028300269811719,
-            "velocityY": -1.1136844712868785,
-            "timestamp": 2.2116613074880798
-        },
-        {
-            "x": 7.481970224994158,
-            "y": 5.90546408447042,
-            "heading": -0.1919554556036592,
-            "angularVelocity": 1.616212898515343,
-            "velocityX": 1.8002958465346786,
-            "velocityY": -0.9878468527519235,
-            "timestamp": 2.2431903743100583
-        },
-        {
-            "x": 7.531573575573894,
-            "y": 5.8782638409384305,
-            "heading": -0.14593987362659777,
-            "angularVelocity": 1.4594653954358323,
-            "velocityX": 1.5732578087328088,
-            "velocityY": -0.8627037294052898,
-            "timestamp": 2.274719441132037
-        },
-        {
-            "x": 7.574043708984396,
-            "y": 5.854989674792864,
-            "heading": -0.10549518757833008,
-            "angularVelocity": 1.2827745989638484,
-            "velocityX": 1.347015236774987,
-            "velocityY": -0.7381812559495933,
-            "timestamp": 2.3062485079540154
-        },
-        {
-            "x": 7.60940100035129,
-            "y": 5.835624772967359,
-            "heading": -0.07108683169331761,
-            "angularVelocity": 1.091321734299697,
-            "velocityX": 1.1214188978865451,
-            "velocityY": -0.614192038566993,
-            "timestamp": 2.337777574775994
-        },
-        {
-            "x": 7.637662176953836,
-            "y": 5.820154874880602,
-            "heading": -0.043071374084194446,
-            "angularVelocity": 0.8885596826352652,
-            "velocityX": 0.8963530941818176,
-            "velocityY": -0.4906551206892414,
-            "timestamp": 2.3693066415979724
-        },
-        {
-            "x": 7.65884126375683,
-            "y": 5.808567782468078,
-            "heading": -0.021732750340195603,
-            "angularVelocity": 0.6767921126395045,
-            "velocityX": 0.6717321169883477,
-            "velocityY": -0.3675050859559891,
-            "timestamp": 2.400835708419951
-        },
-        {
-            "x": 7.672950320288622,
-            "y": 5.8008527701995645,
-            "heading": -0.007307045765720019,
-            "angularVelocity": 0.45753668054709684,
-            "velocityX": 0.44749362902029066,
-            "velocityY": -0.24469523034329732,
-            "timestamp": 2.4323647752419295
+            "heading": 1.9801191994112705e-12,
+            "angularVelocity": -7.94012151883752e-14,
+            "velocityX": 3.7481179954212767,
+            "velocityY": -0.5120963266444406,
+            "timestamp": 1.763114962627194
+        },
+        {
+            "x": 5.834816669509981,
+            "y": 6.625475968242897,
+            "heading": 1.9780897732255602e-12,
+            "angularVelocity": -6.522540784880574e-14,
+            "velocityX": 3.704217712719512,
+            "velocityY": -0.7677264405820726,
+            "timestamp": 1.794229004580604
+        },
+        {
+            "x": 5.948163245427426,
+            "y": 6.593751483162918,
+            "heading": 1.9761554326166073e-12,
+            "angularVelocity": -6.216937380396162e-14,
+            "velocityX": 3.6429396118708874,
+            "velocityY": -1.0196195379349016,
+            "timestamp": 1.825343046534014
+        },
+        {
+            "x": 6.0590772896693,
+            "y": 6.554356506595256,
+            "heading": 1.974328708451782e-12,
+            "angularVelocity": -5.871059954922337e-14,
+            "velocityX": 3.564758458833813,
+            "velocityY": -1.2661478256826604,
+            "timestamp": 1.8564570884874239
+        },
+        {
+            "x": 6.167114375070259,
+            "y": 6.507645933449736,
+            "heading": 1.9725156971860347e-12,
+            "angularVelocity": -5.826986956344564e-14,
+            "velocityX": 3.472293492526525,
+            "velocityY": -1.5012698515774907,
+            "timestamp": 1.8875711304408338
+        },
+        {
+            "x": 6.273666318458446,
+            "y": 6.457640205385319,
+            "heading": 1.9706978622350046e-12,
+            "angularVelocity": -5.842490200021065e-14,
+            "velocityX": 3.424561281617029,
+            "velocityY": -1.6071755684828801,
+            "timestamp": 1.9186851723942437
+        },
+        {
+            "x": 6.3802136872176485,
+            "y": 6.4076247308783065,
+            "heading": 1.9688925756308956e-12,
+            "angularVelocity": -5.802160020867771e-14,
+            "velocityX": 3.4244142538170945,
+            "velocityY": -1.6074888174954127,
+            "timestamp": 1.9497992143476537
+        },
+        {
+            "x": 6.486760876374776,
+            "y": 6.3576088737324294,
+            "heading": 1.967091079960254e-12,
+            "angularVelocity": -5.789976010142433e-14,
+            "velocityX": 3.4244084814345546,
+            "velocityY": -1.607501115448455,
+            "timestamp": 1.9809132563010636
+        },
+        {
+            "x": 6.593299366426369,
+            "y": 6.307574489358799,
+            "heading": 1.9652854794360616e-12,
+            "angularVelocity": -5.803168954665514e-14,
+            "velocityX": 3.4241288936675214,
+            "velocityY": -1.6080965773861522,
+            "timestamp": 2.0120272982544734
+        },
+        {
+            "x": 6.6998332452145375,
+            "y": 6.257530287401571,
+            "heading": 1.963481306814657e-12,
+            "angularVelocity": -5.79857968421928e-14,
+            "velocityX": 3.423980688450087,
+            "velocityY": -1.608412112843002,
+            "timestamp": 2.043141340207883
+        },
+        {
+            "x": 6.806367665236628,
+            "y": 6.207487237643806,
+            "heading": 1.9616709532128727e-12,
+            "angularVelocity": -5.818445266643988e-14,
+            "velocityX": 3.4239980836191717,
+            "velocityY": -1.6083750813421427,
+            "timestamp": 2.074255382161293
+        },
+        {
+            "x": 6.912905936531216,
+            "y": 6.157452377320687,
+            "heading": 1.9503301873398137e-12,
+            "angularVelocity": -3.6449015032516537e-13,
+            "velocityX": 3.424121862863347,
+            "velocityY": -1.6081118743030238,
+            "timestamp": 2.1053694241147025
+        },
+        {
+            "x": 7.015182351606042,
+            "y": 6.109386281530938,
+            "heading": 7.52788173701755e-9,
+            "angularVelocity": 2.418821392510085e-7,
+            "velocityX": 3.287146531073864,
+            "velocityY": -1.5448361181009935,
+            "timestamp": 2.1364834660681122
+        },
+        {
+            "x": 7.110154929626141,
+            "y": 6.064756817323096,
+            "heading": 9.121631465411093e-9,
+            "angularVelocity": 5.122284110672045e-8,
+            "velocityX": 3.0524024542461334,
+            "velocityY": -1.4343833653841132,
+            "timestamp": 2.167597508021522
+        },
+        {
+            "x": 7.19782257145347,
+            "y": 6.02356173384038,
+            "heading": 9.19440476065517e-9,
+            "angularVelocity": 2.3389208664172086e-9,
+            "velocityX": 2.8176230513092126,
+            "velocityY": -1.3240029548204029,
+            "timestamp": 2.1987115499749317
+        },
+        {
+            "x": 7.278184934221974,
+            "y": 5.985800317976241,
+            "heading": 8.586659012919787e-9,
+            "angularVelocity": -1.9532843759144805e-8,
+            "velocityX": 2.582832628715854,
+            "velocityY": -1.2136454633793592,
+            "timestamp": 2.2298255919283414
+        },
+        {
+            "x": 7.351241854096833,
+            "y": 5.951472228972044,
+            "heading": 7.633275836953741e-9,
+            "angularVelocity": -3.064157105146478e-8,
+            "velocityX": 2.3480369404994192,
+            "velocityY": -1.1032989238626065,
+            "timestamp": 2.260939633881751
+        },
+        {
+            "x": 7.416993235519351,
+            "y": 5.9205772680990165,
+            "heading": 6.513007078345892e-9,
+            "angularVelocity": -3.600524486341296e-8,
+            "velocityX": 2.1132381810427594,
+            "velocityY": -0.9929587714545899,
+            "timestamp": 2.292053675835161
+        },
+        {
+            "x": 7.47543901596506,
+            "y": 5.89311530534189,
+            "heading": 5.336882897741431e-9,
+            "angularVelocity": -3.780042846669445e-8,
+            "velocityX": 1.8784374120603946,
+            "velocityY": -0.8826227977151127,
+            "timestamp": 2.3231677177885706
+        },
+        {
+            "x": 7.52657915135954,
+            "y": 5.869086249058835,
+            "heading": 4.180544199369271e-9,
+            "angularVelocity": -3.716452645721565e-8,
+            "velocityX": 1.643635226533715,
+            "velocityY": -0.7722897693284123,
+            "timestamp": 2.3542817597419803
+        },
+        {
+            "x": 7.570413608969555,
+            "y": 5.848490031193872,
+            "heading": 3.0988314877379083e-9,
+            "angularVelocity": -3.476606009342686e-8,
+            "velocityX": 1.4088319889664873,
+            "velocityY": -0.6619589282491846,
+            "timestamp": 2.38539580169539
+        },
+        {
+            "x": 7.606942363528039,
+            "y": 5.831326599216932,
+            "heading": 2.1333266690310216e-9,
+            "angularVelocity": -3.103115843804935e-8,
+            "velocityX": 1.1740279393200113,
+            "velocityY": -0.5516297754774387,
+            "timestamp": 2.4165098436487997
+        },
+        {
+            "x": 7.636165394942908,
+            "y": 5.817595911358632,
+            "heading": 1.3166428525752622e-9,
+            "angularVelocity": -2.6248077074736502e-8,
+            "velocityX": 0.9392232439174092,
+            "velocityY": -0.4413019651665061,
+            "timestamp": 2.4476238856022094
+        },
+        {
+            "x": 7.658082686855903,
+            "y": 5.80729793361315,
+            "heading": 6.750439688094791e-10,
+            "angularVelocity": -2.0620877517571588e-8,
+            "velocityX": 0.7044180227631803,
+            "velocityY": -0.3309752477968783,
+            "timestamp": 2.478737927555619
+        },
+        {
+            "x": 7.672694225690866,
+            "y": 5.800432637759072,
+            "heading": 2.301347828245949e-10,
+            "angularVelocity": -1.4299304537044113e-8,
+            "velocityX": 0.46961236527350575,
+            "velocityY": -0.22064943745862983,
+            "timestamp": 2.509851969509029
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 5.476349034092433e-35,
-            "angularVelocity": 0.23175585268595406,
-            "velocityX": 0.22359303404641942,
-            "velocityY": -0.12219740664444498,
-            "timestamp": 2.463893842063908
+            "heading": -5.36340746801057e-34,
+            "angularVelocity": -7.3964922450186615e-9,
+            "velocityX": 0.23480633985361782,
+            "velocityY": -0.1103243919326242,
+            "timestamp": 2.5409660114624386
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 1.465348958332225e-34,
-            "angularVelocity": 2.9106988947740147e-33,
-            "velocityX": 3.969530612508356e-33,
-            "velocityY": 4.7510565840732025e-33,
-            "timestamp": 2.4954229088858866
+            "heading": -7.670487347269043e-34,
+            "angularVelocity": -7.414948879954709e-33,
+            "velocityX": 1.169026940913066e-31,
+            "velocityY": 1.7301899688761208e-31,
+            "timestamp": 2.5720800534158483
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.1.traj
new file mode 100644
index 00000000..9e7360d5
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.1.traj	
@@ -0,0 +1,391 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": -1.04,
+            "angularVelocity": 5.350140627575144e-31,
+            "velocityX": 5.576066956455863e-32,
+            "velocityY": -7.476130633471959e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7244661136787854,
+            "y": 4.377063116032538,
+            "heading": -0.9951964797725402,
+            "angularVelocity": 0.8134823535576418,
+            "velocityX": 0.2626563301835885,
+            "velocityY": 0.30980922299649477,
+            "timestamp": 0.055076204212072884
+        },
+        {
+            "x": 0.7533392249389502,
+            "y": 4.41173941127898,
+            "heading": -0.9083722132270291,
+            "angularVelocity": 1.5764388230385076,
+            "velocityX": 0.524239309393705,
+            "velocityY": 0.6296057570147754,
+            "timestamp": 0.11015240842414577
+        },
+        {
+            "x": 0.7966290780145974,
+            "y": 4.464680613281826,
+            "heading": -0.783928161336893,
+            "angularVelocity": 2.2594885335771653,
+            "velocityX": 0.7859992113646113,
+            "velocityY": 0.9612354874529038,
+            "timestamp": 0.16522861263621866
+        },
+        {
+            "x": 0.8545700129369538,
+            "y": 4.536623492592967,
+            "heading": -0.6296494882874061,
+            "angularVelocity": 2.8011856527989654,
+            "velocityX": 1.0520139459729396,
+            "velocityY": 1.3062425114505738,
+            "timestamp": 0.22030481684829153
+        },
+        {
+            "x": 0.9276258124093708,
+            "y": 4.628245682798297,
+            "heading": -0.4576364570724038,
+            "angularVelocity": 3.1231823920301154,
+            "velocityX": 1.3264494261643394,
+            "velocityY": 1.663553099130404,
+            "timestamp": 0.27538102106036444
+        },
+        {
+            "x": 1.0160400191585532,
+            "y": 4.739959244546144,
+            "heading": -0.28714155882633985,
+            "angularVelocity": 3.095618165506983,
+            "velocityX": 1.605306829220418,
+            "velocityY": 2.0283453325449616,
+            "timestamp": 0.3304572252724373
+        },
+        {
+            "x": 1.114934453543302,
+            "y": 4.869596690219766,
+            "heading": -0.17212037106177128,
+            "angularVelocity": 2.0884007787042775,
+            "velocityX": 1.795592775492439,
+            "velocityY": 2.3537832268624075,
+            "timestamp": 0.3855334294845102
+        },
+        {
+            "x": 1.2246983786188343,
+            "y": 5.017091614146892,
+            "heading": -0.11040110898627345,
+            "angularVelocity": 1.1206157533631977,
+            "velocityX": 1.9929464393166578,
+            "velocityY": 2.678015415862639,
+            "timestamp": 0.44060963369658307
+        },
+        {
+            "x": 1.345743830891035,
+            "y": 5.182217862842374,
+            "heading": -0.10127955705630985,
+            "angularVelocity": 0.16561693131283492,
+            "velocityX": 2.197781310529416,
+            "velocityY": 2.998141412571981,
+            "timestamp": 0.49568583790865595
+        },
+        {
+            "x": 1.4740265309381277,
+            "y": 5.346392338052181,
+            "heading": -0.10127945031307138,
+            "angularVelocity": 0.0000019381008548903397,
+            "velocityX": 2.3291855690186902,
+            "velocityY": 2.9808603835088525,
+            "timestamp": 0.5507620421207289
+        },
+        {
+            "x": 1.6022800251300493,
+            "y": 5.510589629967512,
+            "heading": -0.10127934358262022,
+            "angularVelocity": 0.0000019378686800961446,
+            "velocityX": 2.3286552881910736,
+            "velocityY": 2.9812746587087395,
+            "timestamp": 0.6058382463328018
+        },
+        {
+            "x": 1.7347437736631814,
+            "y": 5.671409606244062,
+            "heading": -0.10127923743109664,
+            "angularVelocity": 0.0000019273572882478923,
+            "velocityX": 2.4050994513545807,
+            "velocityY": 2.9199538816671686,
+            "timestamp": 0.6609144505448747
+        },
+        {
+            "x": 1.8857033856482435,
+            "y": 5.815009412217227,
+            "heading": -0.10127912902413344,
+            "angularVelocity": 0.0000019683085417376046,
+            "velocityX": 2.7409225843485396,
+            "velocityY": 2.607293077428296,
+            "timestamp": 0.7159906547569477
+        },
+        {
+            "x": 2.0529507817587227,
+            "y": 5.93925921930888,
+            "heading": -0.1012790059378887,
+            "angularVelocity": 0.0000022348352886277144,
+            "velocityX": 3.036654368309179,
+            "velocityY": 2.2559616965109663,
+            "timestamp": 0.7710668589690206
+        },
+        {
+            "x": 2.234021022235051,
+            "y": 6.042327659180149,
+            "heading": -0.10127786160272408,
+            "angularVelocity": 0.0000207773062979661,
+            "velocityX": 3.287631075284561,
+            "velocityY": 1.8713787804693067,
+            "timestamp": 0.8261430631810935
+        },
+        {
+            "x": 2.4203468055894013,
+            "y": 6.121035990183916,
+            "heading": -0.0857184634932982,
+            "angularVelocity": 0.28250672558184264,
+            "velocityX": 3.3830541886455214,
+            "velocityY": 1.4290805281478167,
+            "timestamp": 0.8812192673931665
+        },
+        {
+            "x": 2.608481388818861,
+            "y": 6.1750393209771115,
+            "heading": -0.05159654365961516,
+            "angularVelocity": 0.6195401502670522,
+            "velocityX": 3.4158959558113233,
+            "velocityY": 0.9805202004345134,
+            "timestamp": 0.9362954716052394
+        },
+        {
+            "x": 2.797205924987793,
+            "y": 6.204219341278076,
+            "heading": -3.617531000727075e-31,
+            "angularVelocity": 0.9368209809983128,
+            "velocityX": 3.4266075316708653,
+            "velocityY": 0.5298117529778321,
+            "timestamp": 0.9913716758173123
+        },
+        {
+            "x": 2.966124692846499,
+            "y": 6.210295134186947,
+            "heading": 0.05716064392929573,
+            "angularVelocity": 1.15887425245862,
+            "velocityX": 3.4246572006891465,
+            "velocityY": 0.12318055713425537,
+            "timestamp": 1.0406959608361792
+        },
+        {
+            "x": 3.13289635801508,
+            "y": 6.196205704844962,
+            "heading": 0.11248864097572012,
+            "angularVelocity": 1.12171919015676,
+            "velocityX": 3.3811268648859167,
+            "velocityY": -0.2856489320949118,
+            "timestamp": 1.090020245855046
+        },
+        {
+            "x": 3.2974428000278966,
+            "y": 6.1620266805098955,
+            "heading": 0.16165741156418048,
+            "angularVelocity": 0.9968471021861408,
+            "velocityX": 3.336012715640518,
+            "velocityY": -0.6929451551500881,
+            "timestamp": 1.139344530873913
+        },
+        {
+            "x": 3.45979854217056,
+            "y": 6.107879810654357,
+            "heading": 0.20068182384964747,
+            "angularVelocity": 0.7911804959877227,
+            "velocityX": 3.2915984910995078,
+            "velocityY": -1.0977730307662261,
+            "timestamp": 1.1886688158927798
+        },
+        {
+            "x": 3.619952741472391,
+            "y": 6.03393942235358,
+            "heading": 0.22581697371114728,
+            "angularVelocity": 0.509589745738515,
+            "velocityX": 3.2469644362928305,
+            "velocityY": -1.4990666012186962,
+            "timestamp": 1.2379931009116467
+        },
+        {
+            "x": 3.7772983820923596,
+            "y": 5.940432276289927,
+            "heading": 0.23452421295694514,
+            "angularVelocity": 0.17653047058793564,
+            "velocityX": 3.190023749148815,
+            "velocityY": -1.8957628281461119,
+            "timestamp": 1.2873173859305136
+        },
+        {
+            "x": 3.926570083103172,
+            "y": 5.828476756706829,
+            "heading": 0.23452463928791645,
+            "angularVelocity": 0.000008643429319627254,
+            "velocityX": 3.026332788274898,
+            "velocityY": -2.2697849454943535,
+            "timestamp": 1.3366416709493805
+        },
+        {
+            "x": 4.062952838723869,
+            "y": 5.701135334142216,
+            "heading": 0.23452497322201252,
+            "angularVelocity": 0.000006770176109915362,
+            "velocityX": 2.7650224543249706,
+            "velocityY": -2.5817185695831255,
+            "timestamp": 1.3859659559682473
+        },
+        {
+            "x": 4.192164897918701,
+            "y": 5.566523551940918,
+            "heading": 0.23452535083185597,
+            "angularVelocity": 0.000007655657721212718,
+            "velocityX": 2.6196438355955265,
+            "velocityY": -2.7291177591283535,
+            "timestamp": 1.4352902409871142
+        },
+        {
+            "x": 4.379227990202405,
+            "y": 5.2613971819703,
+            "heading": 0.26255662300085747,
+            "angularVelocity": 0.28814889358818807,
+            "velocityX": 1.922924608906585,
+            "velocityY": -3.136562100411476,
+            "timestamp": 1.5325707539189606
+        },
+        {
+            "x": 4.5172116067735955,
+            "y": 4.993723531483578,
+            "heading": 0.47596391395635385,
+            "angularVelocity": 2.193731144335268,
+            "velocityX": 1.4184096322339512,
+            "velocityY": -2.751564958073857,
+            "timestamp": 1.629851266850807
+        },
+        {
+            "x": 4.704361215089976,
+            "y": 4.7238802473185055,
+            "heading": 0.606383868813914,
+            "angularVelocity": 1.3406585854344948,
+            "velocityX": 1.9238139548821627,
+            "velocityY": -2.773867818255899,
+            "timestamp": 1.7271317797826533
+        },
+        {
+            "x": 4.9212614384999,
+            "y": 4.498429708685273,
+            "heading": 0.7825723935837035,
+            "angularVelocity": 1.8111389368724442,
+            "velocityX": 2.229636921855878,
+            "velocityY": -2.3175303237882856,
+            "timestamp": 1.8244122927144997
+        },
+        {
+            "x": 5.185303406160076,
+            "y": 4.293106225066136,
+            "heading": 0.8921386359376694,
+            "angularVelocity": 1.1262917829260124,
+            "velocityX": 2.71423288901818,
+            "velocityY": -2.110633234047443,
+            "timestamp": 1.921692805646346
+        },
+        {
+            "x": 5.506747569472596,
+            "y": 4.137612897565104,
+            "heading": 0.9241000729554552,
+            "angularVelocity": 0.328549223832528,
+            "velocityX": 3.304301690285293,
+            "velocityY": -1.5984015998144245,
+            "timestamp": 2.0189733185781926
+        },
+        {
+            "x": 5.864556115895607,
+            "y": 4.051579577380226,
+            "heading": 0.924099942108789,
+            "angularVelocity": -0.0000013450449852620257,
+            "velocityX": 3.6781112233000655,
+            "velocityY": -0.8843839078557418,
+            "timestamp": 2.116253831510039
+        },
+        {
+            "x": 6.232480317048254,
+            "y": 4.043802644882202,
+            "heading": 0.9240998495204155,
+            "angularVelocity": -9.517669138022849e-7,
+            "velocityX": 3.7820956126167884,
+            "velocityY": -0.07994337471752075,
+            "timestamp": 2.2135343444418853
+        },
+        {
+            "x": 6.59997941709367,
+            "y": 4.063118723469172,
+            "heading": 0.9240997607721795,
+            "angularVelocity": -9.122920231628579e-7,
+            "velocityX": 3.777725764078584,
+            "velocityY": 0.198560616148301,
+            "timestamp": 2.3108148573737317
+        },
+        {
+            "x": 6.956026800754634,
+            "y": 4.090327871531077,
+            "heading": 0.8973557490580134,
+            "angularVelocity": -0.27491643401286797,
+            "velocityX": 3.660007260759475,
+            "velocityY": 0.2796978268501417,
+            "timestamp": 2.408095370305578
+        },
+        {
+            "x": 7.247734347026976,
+            "y": 4.106615231962139,
+            "heading": 0.7104712895715125,
+            "angularVelocity": -1.9210883439464366,
+            "velocityX": 2.9986226170159034,
+            "velocityY": 0.1674267532128742,
+            "timestamp": 2.5053758832374244
+        },
+        {
+            "x": 7.467840517564824,
+            "y": 4.117242335862297,
+            "heading": 0.4016498999018689,
+            "angularVelocity": -3.1745452440819273,
+            "velocityX": 2.262592619058774,
+            "velocityY": 0.1092418571806094,
+            "timestamp": 2.602656396169271
+        },
+        {
+            "x": 7.6104608023102855,
+            "y": 4.123883769495069,
+            "heading": 0.14488296734927883,
+            "angularVelocity": -2.639448794153438,
+            "velocityX": 1.4660724994879464,
+            "velocityY": 0.06827095615156697,
+            "timestamp": 2.699936909101117
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 9.165039287514682e-34,
+            "angularVelocity": -1.4893318608505086,
+            "velocityX": 0.7148317334473038,
+            "velocityY": 0.032033450595745375,
+            "timestamp": 2.7972174220329635
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": 3.6915361070859757e-32,
+            "velocityX": 0,
+            "velocityY": 3.3618150907593784e-32,
+            "timestamp": 2.89449793496481
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.traj
index 49c665b8..9e7360d5 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.traj	
@@ -4,271 +4,388 @@
             "x": 0.71,
             "y": 4.36,
             "heading": -1.04,
-            "angularVelocity": 0,
-            "velocityX": -1.418990828065155e-30,
-            "velocityY": 1.8798559266210403e-30,
+            "angularVelocity": 5.350140627575144e-31,
+            "velocityX": 5.576066956455863e-32,
+            "velocityY": -7.476130633471959e-32,
             "timestamp": 0
         },
         {
-            "x": 0.7612279700458028,
-            "y": 4.384337560333447,
-            "heading": -0.9750502376080931,
-            "angularVelocity": 0.7696859194569483,
-            "velocityX": 0.6070760811828038,
-            "velocityY": 0.28841179417367857,
-            "timestamp": 0.0843847610434429
-        },
-        {
-            "x": 0.8642667385584091,
-            "y": 4.433290258850185,
-            "heading": -0.8542284973914237,
-            "angularVelocity": 1.4317957261788106,
-            "velocityX": 1.2210589594435701,
-            "velocityY": 0.5801130193585202,
-            "timestamp": 0.1687695220868858
-        },
-        {
-            "x": 1.0199495140663002,
-            "y": 4.507092326164681,
-            "heading": -0.6915219404263555,
-            "angularVelocity": 1.9281509475538106,
-            "velocityX": 1.8449157594667183,
-            "velocityY": 0.8745899899690757,
-            "timestamp": 0.25315428313032873
-        },
-        {
-            "x": 1.2292345273519185,
-            "y": 4.606077015104397,
-            "heading": -0.5137189126301165,
-            "angularVelocity": 2.1070513869757743,
-            "velocityX": 2.480128055086372,
-            "velocityY": 1.173016166850306,
-            "timestamp": 0.3375390441737716
-        },
-        {
-            "x": 1.485641598676529,
-            "y": 4.729632973105035,
-            "heading": -0.42871970592241276,
-            "angularVelocity": 1.007281476615537,
-            "velocityX": 3.038547104406527,
-            "velocityY": 1.4641975218379706,
-            "timestamp": 0.4219238052172145
-        },
-        {
-            "x": 1.7745745246623985,
-            "y": 4.865356167675212,
-            "heading": -0.42871965586223265,
-            "angularVelocity": 5.932372081830595e-7,
-            "velocityX": 3.4239941242128182,
-            "velocityY": 1.6083851265555307,
-            "timestamp": 0.5063085662606575
-        },
-        {
-            "x": 2.063508085893233,
-            "y": 5.001078009918069,
-            "heading": -0.4287196058112515,
-            "angularVelocity": 5.931281972756146e-7,
-            "velocityX": 3.4240016521710777,
-            "velocityY": 1.6083691008265977,
-            "timestamp": 0.5906933273041004
-        },
-        {
-            "x": 2.3524294773436862,
-            "y": 5.136825756955775,
-            "heading": -0.4287195557601797,
-            "angularVelocity": 5.931292703362374e-7,
-            "velocityX": 3.423857434421327,
-            "velocityY": 1.6086760851027126,
-            "timestamp": 0.6750780883475433
-        },
-        {
-            "x": 2.647360543237261,
-            "y": 5.258967835966316,
-            "heading": -0.4287195052637089,
-            "angularVelocity": 5.984074633873495e-7,
-            "velocityX": 3.4950749666965533,
-            "velocityY": 1.447442375852424,
-            "timestamp": 0.7594628493909863
-        },
-        {
-            "x": 2.959809428031792,
-            "y": 5.324379711821066,
-            "heading": -0.42871944828896014,
-            "angularVelocity": 6.751781731100131e-7,
-            "velocityX": 3.702669545200082,
-            "velocityY": 0.7751621862272086,
-            "timestamp": 0.8438476104344292
-        },
-        {
-            "x": 3.2789674137401184,
-            "y": 5.330797896945005,
-            "heading": -0.4287193686010616,
-            "angularVelocity": 9.443399202542765e-7,
-            "velocityX": 3.782175617514861,
-            "velocityY": 0.0760585802995927,
-            "timestamp": 0.9282323714778722
-        },
-        {
-            "x": 3.593793413911528,
-            "y": 5.278000284107237,
-            "heading": -0.42871953037751775,
-            "angularVelocity": -0.000001917128805011847,
-            "velocityX": 3.7308395055989028,
-            "velocityY": -0.625677103128148,
-            "timestamp": 1.0126171325213151
-        },
-        {
-            "x": 3.893396265271842,
-            "y": 5.167813441772834,
-            "heading": -0.42871965571467335,
-            "angularVelocity": -0.0000014853055701792625,
-            "velocityX": 3.550437871193987,
-            "velocityY": -1.3057670718262655,
-            "timestamp": 1.097001893564758
-        },
-        {
-            "x": 4.167411681517661,
-            "y": 5.0040496950416875,
-            "heading": -0.4287197602752934,
-            "angularVelocity": -0.0000012390936318239587,
-            "velocityX": 3.247214459785327,
-            "velocityY": -1.9406791546980178,
-            "timestamp": 1.181386654608201
-        },
-        {
-            "x": 4.4320204493985145,
-            "y": 4.825485382292427,
-            "heading": -0.42871985986303984,
-            "angularVelocity": -0.0000011801626881351292,
-            "velocityX": 3.1357411528921184,
-            "velocityY": -2.1160729797797915,
-            "timestamp": 1.265771415651644
-        },
-        {
-            "x": 4.666313927400726,
-            "y": 4.608668432226026,
-            "heading": -0.4287198793836171,
-            "angularVelocity": -2.3132822841838536e-7,
-            "velocityX": 2.77649038884734,
-            "velocityY": -2.5693851281356843,
-            "timestamp": 1.350156176695087
-        },
-        {
-            "x": 4.936706954544898,
-            "y": 4.438990462685779,
-            "heading": -0.4287197198154701,
-            "angularVelocity": 0.0000018909592816311697,
-            "velocityX": 3.204287406881873,
-            "velocityY": -2.010765539204243,
-            "timestamp": 1.4345409377385299
-        },
-        {
-            "x": 5.233776210787073,
-            "y": 4.322144997944839,
-            "heading": -0.42871957094251306,
-            "angularVelocity": 0.0000017642161351814778,
-            "velocityX": 3.5204135506067917,
-            "velocityY": -1.384674949557486,
-            "timestamp": 1.5189256987819728
-        },
-        {
-            "x": 5.532138329985476,
-            "y": 4.208641466951728,
-            "heading": -0.42871942351676046,
-            "angularVelocity": 0.0000017470660671471722,
-            "velocityX": 3.5357345983929718,
-            "velocityY": -1.3450714274656952,
-            "timestamp": 1.6033104598254158
-        },
-        {
-            "x": 5.845046534221127,
-            "y": 4.145463344127181,
-            "heading": -0.42871936362457447,
-            "angularVelocity": 7.097512069029325e-7,
-            "velocityX": 3.708112701469523,
-            "velocityY": -0.7486911385809435,
-            "timestamp": 1.6876952208688587
-        },
-        {
-            "x": 6.164234411105391,
-            "y": 4.140760073731674,
-            "heading": -0.4287193263126739,
-            "angularVelocity": 4.4216396519651743e-7,
-            "velocityX": 3.7825298423246796,
-            "velocityY": -0.055736016049057806,
-            "timestamp": 1.7720799819123016
-        },
-        {
-            "x": 6.483450051427475,
-            "y": 4.138663317552119,
-            "heading": -0.42871928910258295,
-            "angularVelocity": 4.4095747260310005e-7,
-            "velocityX": 3.782858852414673,
-            "velocityY": -0.024847569082867782,
-            "timestamp": 1.8564647429557446
-        },
-        {
-            "x": 6.802665691714121,
-            "y": 4.136566553568004,
-            "heading": -0.4287192518958351,
-            "angularVelocity": 4.4091785477775863e-7,
-            "velocityX": 3.7828588519947024,
-            "velocityY": -0.024847661569458286,
-            "timestamp": 1.9408495039991875
-        },
-        {
-            "x": 7.0950029322992965,
-            "y": 4.132696093446151,
-            "heading": -0.35666839777179793,
-            "angularVelocity": 0.853837271482519,
-            "velocityX": 3.4643368893901423,
-            "velocityY": -0.04586681379319509,
-            "timestamp": 2.0252342650426303
-        },
-        {
-            "x": 7.329695147990892,
-            "y": 4.13025195968354,
-            "heading": -0.23302792235716066,
-            "angularVelocity": 1.465199093839195,
-            "velocityX": 2.7812156222231823,
-            "velocityY": -0.02896416049909081,
-            "timestamp": 2.109619026086073
-        },
-        {
-            "x": 7.505137224924894,
-            "y": 4.1285705338055685,
-            "heading": -0.12281843303988041,
-            "angularVelocity": 1.3060354494646,
-            "velocityX": 2.0790729838492856,
-            "velocityY": -0.019925705271038107,
-            "timestamp": 2.194003787129516
-        },
-        {
-            "x": 7.621791455543556,
-            "y": 4.127510522704197,
-            "heading": -0.04250049300917318,
-            "angularVelocity": 0.9518062152170125,
-            "velocityX": 1.3824087332380297,
-            "velocityY": -0.012561641322782502,
-            "timestamp": 2.278388548172959
+            "x": 0.7244661136787854,
+            "y": 4.377063116032538,
+            "heading": -0.9951964797725402,
+            "angularVelocity": 0.8134823535576418,
+            "velocityX": 0.2626563301835885,
+            "velocityY": 0.30980922299649477,
+            "timestamp": 0.055076204212072884
+        },
+        {
+            "x": 0.7533392249389502,
+            "y": 4.41173941127898,
+            "heading": -0.9083722132270291,
+            "angularVelocity": 1.5764388230385076,
+            "velocityX": 0.524239309393705,
+            "velocityY": 0.6296057570147754,
+            "timestamp": 0.11015240842414577
+        },
+        {
+            "x": 0.7966290780145974,
+            "y": 4.464680613281826,
+            "heading": -0.783928161336893,
+            "angularVelocity": 2.2594885335771653,
+            "velocityX": 0.7859992113646113,
+            "velocityY": 0.9612354874529038,
+            "timestamp": 0.16522861263621866
+        },
+        {
+            "x": 0.8545700129369538,
+            "y": 4.536623492592967,
+            "heading": -0.6296494882874061,
+            "angularVelocity": 2.8011856527989654,
+            "velocityX": 1.0520139459729396,
+            "velocityY": 1.3062425114505738,
+            "timestamp": 0.22030481684829153
+        },
+        {
+            "x": 0.9276258124093708,
+            "y": 4.628245682798297,
+            "heading": -0.4576364570724038,
+            "angularVelocity": 3.1231823920301154,
+            "velocityX": 1.3264494261643394,
+            "velocityY": 1.663553099130404,
+            "timestamp": 0.27538102106036444
+        },
+        {
+            "x": 1.0160400191585532,
+            "y": 4.739959244546144,
+            "heading": -0.28714155882633985,
+            "angularVelocity": 3.095618165506983,
+            "velocityX": 1.605306829220418,
+            "velocityY": 2.0283453325449616,
+            "timestamp": 0.3304572252724373
+        },
+        {
+            "x": 1.114934453543302,
+            "y": 4.869596690219766,
+            "heading": -0.17212037106177128,
+            "angularVelocity": 2.0884007787042775,
+            "velocityX": 1.795592775492439,
+            "velocityY": 2.3537832268624075,
+            "timestamp": 0.3855334294845102
+        },
+        {
+            "x": 1.2246983786188343,
+            "y": 5.017091614146892,
+            "heading": -0.11040110898627345,
+            "angularVelocity": 1.1206157533631977,
+            "velocityX": 1.9929464393166578,
+            "velocityY": 2.678015415862639,
+            "timestamp": 0.44060963369658307
+        },
+        {
+            "x": 1.345743830891035,
+            "y": 5.182217862842374,
+            "heading": -0.10127955705630985,
+            "angularVelocity": 0.16561693131283492,
+            "velocityX": 2.197781310529416,
+            "velocityY": 2.998141412571981,
+            "timestamp": 0.49568583790865595
+        },
+        {
+            "x": 1.4740265309381277,
+            "y": 5.346392338052181,
+            "heading": -0.10127945031307138,
+            "angularVelocity": 0.0000019381008548903397,
+            "velocityX": 2.3291855690186902,
+            "velocityY": 2.9808603835088525,
+            "timestamp": 0.5507620421207289
+        },
+        {
+            "x": 1.6022800251300493,
+            "y": 5.510589629967512,
+            "heading": -0.10127934358262022,
+            "angularVelocity": 0.0000019378686800961446,
+            "velocityX": 2.3286552881910736,
+            "velocityY": 2.9812746587087395,
+            "timestamp": 0.6058382463328018
+        },
+        {
+            "x": 1.7347437736631814,
+            "y": 5.671409606244062,
+            "heading": -0.10127923743109664,
+            "angularVelocity": 0.0000019273572882478923,
+            "velocityX": 2.4050994513545807,
+            "velocityY": 2.9199538816671686,
+            "timestamp": 0.6609144505448747
+        },
+        {
+            "x": 1.8857033856482435,
+            "y": 5.815009412217227,
+            "heading": -0.10127912902413344,
+            "angularVelocity": 0.0000019683085417376046,
+            "velocityX": 2.7409225843485396,
+            "velocityY": 2.607293077428296,
+            "timestamp": 0.7159906547569477
+        },
+        {
+            "x": 2.0529507817587227,
+            "y": 5.93925921930888,
+            "heading": -0.1012790059378887,
+            "angularVelocity": 0.0000022348352886277144,
+            "velocityX": 3.036654368309179,
+            "velocityY": 2.2559616965109663,
+            "timestamp": 0.7710668589690206
+        },
+        {
+            "x": 2.234021022235051,
+            "y": 6.042327659180149,
+            "heading": -0.10127786160272408,
+            "angularVelocity": 0.0000207773062979661,
+            "velocityX": 3.287631075284561,
+            "velocityY": 1.8713787804693067,
+            "timestamp": 0.8261430631810935
+        },
+        {
+            "x": 2.4203468055894013,
+            "y": 6.121035990183916,
+            "heading": -0.0857184634932982,
+            "angularVelocity": 0.28250672558184264,
+            "velocityX": 3.3830541886455214,
+            "velocityY": 1.4290805281478167,
+            "timestamp": 0.8812192673931665
+        },
+        {
+            "x": 2.608481388818861,
+            "y": 6.1750393209771115,
+            "heading": -0.05159654365961516,
+            "angularVelocity": 0.6195401502670522,
+            "velocityX": 3.4158959558113233,
+            "velocityY": 0.9805202004345134,
+            "timestamp": 0.9362954716052394
+        },
+        {
+            "x": 2.797205924987793,
+            "y": 6.204219341278076,
+            "heading": -3.617531000727075e-31,
+            "angularVelocity": 0.9368209809983128,
+            "velocityX": 3.4266075316708653,
+            "velocityY": 0.5298117529778321,
+            "timestamp": 0.9913716758173123
+        },
+        {
+            "x": 2.966124692846499,
+            "y": 6.210295134186947,
+            "heading": 0.05716064392929573,
+            "angularVelocity": 1.15887425245862,
+            "velocityX": 3.4246572006891465,
+            "velocityY": 0.12318055713425537,
+            "timestamp": 1.0406959608361792
+        },
+        {
+            "x": 3.13289635801508,
+            "y": 6.196205704844962,
+            "heading": 0.11248864097572012,
+            "angularVelocity": 1.12171919015676,
+            "velocityX": 3.3811268648859167,
+            "velocityY": -0.2856489320949118,
+            "timestamp": 1.090020245855046
+        },
+        {
+            "x": 3.2974428000278966,
+            "y": 6.1620266805098955,
+            "heading": 0.16165741156418048,
+            "angularVelocity": 0.9968471021861408,
+            "velocityX": 3.336012715640518,
+            "velocityY": -0.6929451551500881,
+            "timestamp": 1.139344530873913
+        },
+        {
+            "x": 3.45979854217056,
+            "y": 6.107879810654357,
+            "heading": 0.20068182384964747,
+            "angularVelocity": 0.7911804959877227,
+            "velocityX": 3.2915984910995078,
+            "velocityY": -1.0977730307662261,
+            "timestamp": 1.1886688158927798
+        },
+        {
+            "x": 3.619952741472391,
+            "y": 6.03393942235358,
+            "heading": 0.22581697371114728,
+            "angularVelocity": 0.509589745738515,
+            "velocityX": 3.2469644362928305,
+            "velocityY": -1.4990666012186962,
+            "timestamp": 1.2379931009116467
+        },
+        {
+            "x": 3.7772983820923596,
+            "y": 5.940432276289927,
+            "heading": 0.23452421295694514,
+            "angularVelocity": 0.17653047058793564,
+            "velocityX": 3.190023749148815,
+            "velocityY": -1.8957628281461119,
+            "timestamp": 1.2873173859305136
+        },
+        {
+            "x": 3.926570083103172,
+            "y": 5.828476756706829,
+            "heading": 0.23452463928791645,
+            "angularVelocity": 0.000008643429319627254,
+            "velocityX": 3.026332788274898,
+            "velocityY": -2.2697849454943535,
+            "timestamp": 1.3366416709493805
+        },
+        {
+            "x": 4.062952838723869,
+            "y": 5.701135334142216,
+            "heading": 0.23452497322201252,
+            "angularVelocity": 0.000006770176109915362,
+            "velocityX": 2.7650224543249706,
+            "velocityY": -2.5817185695831255,
+            "timestamp": 1.3859659559682473
+        },
+        {
+            "x": 4.192164897918701,
+            "y": 5.566523551940918,
+            "heading": 0.23452535083185597,
+            "angularVelocity": 0.000007655657721212718,
+            "velocityX": 2.6196438355955265,
+            "velocityY": -2.7291177591283535,
+            "timestamp": 1.4352902409871142
+        },
+        {
+            "x": 4.379227990202405,
+            "y": 5.2613971819703,
+            "heading": 0.26255662300085747,
+            "angularVelocity": 0.28814889358818807,
+            "velocityX": 1.922924608906585,
+            "velocityY": -3.136562100411476,
+            "timestamp": 1.5325707539189606
+        },
+        {
+            "x": 4.5172116067735955,
+            "y": 4.993723531483578,
+            "heading": 0.47596391395635385,
+            "angularVelocity": 2.193731144335268,
+            "velocityX": 1.4184096322339512,
+            "velocityY": -2.751564958073857,
+            "timestamp": 1.629851266850807
+        },
+        {
+            "x": 4.704361215089976,
+            "y": 4.7238802473185055,
+            "heading": 0.606383868813914,
+            "angularVelocity": 1.3406585854344948,
+            "velocityX": 1.9238139548821627,
+            "velocityY": -2.773867818255899,
+            "timestamp": 1.7271317797826533
+        },
+        {
+            "x": 4.9212614384999,
+            "y": 4.498429708685273,
+            "heading": 0.7825723935837035,
+            "angularVelocity": 1.8111389368724442,
+            "velocityX": 2.229636921855878,
+            "velocityY": -2.3175303237882856,
+            "timestamp": 1.8244122927144997
+        },
+        {
+            "x": 5.185303406160076,
+            "y": 4.293106225066136,
+            "heading": 0.8921386359376694,
+            "angularVelocity": 1.1262917829260124,
+            "velocityX": 2.71423288901818,
+            "velocityY": -2.110633234047443,
+            "timestamp": 1.921692805646346
+        },
+        {
+            "x": 5.506747569472596,
+            "y": 4.137612897565104,
+            "heading": 0.9241000729554552,
+            "angularVelocity": 0.328549223832528,
+            "velocityX": 3.304301690285293,
+            "velocityY": -1.5984015998144245,
+            "timestamp": 2.0189733185781926
+        },
+        {
+            "x": 5.864556115895607,
+            "y": 4.051579577380226,
+            "heading": 0.924099942108789,
+            "angularVelocity": -0.0000013450449852620257,
+            "velocityX": 3.6781112233000655,
+            "velocityY": -0.8843839078557418,
+            "timestamp": 2.116253831510039
+        },
+        {
+            "x": 6.232480317048254,
+            "y": 4.043802644882202,
+            "heading": 0.9240998495204155,
+            "angularVelocity": -9.517669138022849e-7,
+            "velocityX": 3.7820956126167884,
+            "velocityY": -0.07994337471752075,
+            "timestamp": 2.2135343444418853
+        },
+        {
+            "x": 6.59997941709367,
+            "y": 4.063118723469172,
+            "heading": 0.9240997607721795,
+            "angularVelocity": -9.122920231628579e-7,
+            "velocityX": 3.777725764078584,
+            "velocityY": 0.198560616148301,
+            "timestamp": 2.3108148573737317
+        },
+        {
+            "x": 6.956026800754634,
+            "y": 4.090327871531077,
+            "heading": 0.8973557490580134,
+            "angularVelocity": -0.27491643401286797,
+            "velocityX": 3.660007260759475,
+            "velocityY": 0.2796978268501417,
+            "timestamp": 2.408095370305578
+        },
+        {
+            "x": 7.247734347026976,
+            "y": 4.106615231962139,
+            "heading": 0.7104712895715125,
+            "angularVelocity": -1.9210883439464366,
+            "velocityX": 2.9986226170159034,
+            "velocityY": 0.1674267532128742,
+            "timestamp": 2.5053758832374244
+        },
+        {
+            "x": 7.467840517564824,
+            "y": 4.117242335862297,
+            "heading": 0.4016498999018689,
+            "angularVelocity": -3.1745452440819273,
+            "velocityX": 2.262592619058774,
+            "velocityY": 0.1092418571806094,
+            "timestamp": 2.602656396169271
+        },
+        {
+            "x": 7.6104608023102855,
+            "y": 4.123883769495069,
+            "heading": 0.14488296734927883,
+            "angularVelocity": -2.639448794153438,
+            "velocityX": 1.4660724994879464,
+            "velocityY": 0.06827095615156697,
+            "timestamp": 2.699936909101117
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": -1.5179160305718205e-31,
-            "angularVelocity": 0.5036512811515541,
-            "velocityX": 0.6897992449901723,
-            "velocityY": -0.0060499395609808024,
-            "timestamp": 2.362773309216402
+            "heading": 9.165039287514682e-34,
+            "angularVelocity": -1.4893318608505086,
+            "velocityX": 0.7148317334473038,
+            "velocityY": 0.032033450595745375,
+            "timestamp": 2.7972174220329635
         },
         {
             "x": 7.68,
             "y": 4.127,
             "heading": 0,
-            "angularVelocity": 2.3304076579629275e-30,
+            "angularVelocity": 3.6915361070859757e-32,
             "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 2.447158070259845
+            "velocityY": 3.3618150907593784e-32,
+            "timestamp": 2.89449793496481
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.1.traj
new file mode 100644
index 00000000..00bde21f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.1.traj	
@@ -0,0 +1,679 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": -1.04,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.687708804701003e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7140616140612069,
+            "y": 4.355992841184312,
+            "heading": -1.0347167301054676,
+            "angularVelocity": 0.1988304134727778,
+            "velocityX": 0.15285465616518656,
+            "velocityY": -0.15080528916361516,
+            "timestamp": 0.026571739213607736
+        },
+        {
+            "x": 0.7221925258884458,
+            "y": 4.3479754290630535,
+            "heading": -1.0242624006876555,
+            "angularVelocity": 0.39343790535390527,
+            "velocityX": 0.30599848063671325,
+            "velocityY": -0.3017270362623706,
+            "timestamp": 0.05314347842721547
+        },
+        {
+            "x": 0.7344011981721603,
+            "y": 4.335944497876898,
+            "heading": -1.0087613580778494,
+            "angularVelocity": 0.5833657513042195,
+            "velocityX": 0.45946078973491633,
+            "velocityY": -0.4527716868451825,
+            "timestamp": 0.0797152176408232
+        },
+        {
+            "x": 0.7506970627866689,
+            "y": 4.31989642008046,
+            "heading": -0.9883558514269565,
+            "angularVelocity": 0.7679401971717026,
+            "velocityX": 0.6132780576953482,
+            "velocityY": -0.6039528563572185,
+            "timestamp": 0.10628695685443094
+        },
+        {
+            "x": 0.7710907256657638,
+            "y": 4.29982697901967,
+            "heading": -0.963213241169254,
+            "angularVelocity": 0.9462162057057438,
+            "velocityX": 0.7674944690353961,
+            "velocityY": -0.7552927152962536,
+            "timestamp": 0.13285869606803868
+        },
+        {
+            "x": 0.7955941760833964,
+            "y": 4.27573110299105,
+            "heading": -0.9335350310204893,
+            "angularVelocity": 1.1169088297226009,
+            "velocityX": 0.9221620843352277,
+            "velocityY": -0.9068234425649082,
+            "timestamp": 0.1594304352816464
+        },
+        {
+            "x": 0.8242209914920048,
+            "y": 4.247602572989807,
+            "heading": -0.8995682913456332,
+            "angularVelocity": 1.2783032153748224,
+            "velocityX": 1.0773406730541775,
+            "velocityY": -1.0585882156647894,
+            "timestamp": 0.18600217449525414
+        },
+        {
+            "x": 0.856986533327333,
+            "y": 4.215433742997202,
+            "heading": -0.8616204330063739,
+            "angularVelocity": 1.4281285103018635,
+            "velocityX": 1.2330973735640383,
+            "velocityY": -1.210640738794065,
+            "timestamp": 0.21257391370886186
+        },
+        {
+            "x": 0.893908134971617,
+            "y": 4.179215360896215,
+            "heading": -0.8200791656995261,
+            "angularVelocity": 1.5633627506615773,
+            "velocityX": 1.3895063980372095,
+            "velocityY": -1.3630414558050108,
+            "timestamp": 0.2391456529224696
+        },
+        {
+            "x": 0.9350052770321623,
+            "y": 4.138936674970777,
+            "heading": -0.7754414952973147,
+            "angularVelocity": 1.6798926876172218,
+            "velocityX": 1.546648555074586,
+            "velocityY": -1.5158468025612346,
+            "timestamp": 0.2657173921360773
+        },
+        {
+            "x": 0.9802996637151041,
+            "y": 4.094586228803994,
+            "heading": -0.7283605055619151,
+            "angularVelocity": 1.7718444907546276,
+            "velocityX": 1.7046075275248067,
+            "velocityY": -1.669083299751435,
+            "timestamp": 0.29228913134968504
+        },
+        {
+            "x": 1.0298146765069804,
+            "y": 4.046154350498487,
+            "heading": -0.6797316150493184,
+            "angularVelocity": 1.8300981400454652,
+            "velocityX": 1.8634464381059026,
+            "velocityY": -1.8226837888242364,
+            "timestamp": 0.31886087056329276
+        },
+        {
+            "x": 1.0835715343165668,
+            "y": 3.9936405744754793,
+            "heading": -0.6308802721211105,
+            "angularVelocity": 1.8384699072761677,
+            "velocityX": 2.0230839004342207,
+            "velocityY": -1.9763017994740086,
+            "timestamp": 0.3454326097769005
+        },
+        {
+            "x": 1.1415677463510772,
+            "y": 3.937082152581159,
+            "heading": -0.5840793547040092,
+            "angularVelocity": 1.7613042579137594,
+            "velocityX": 2.1826276243449603,
+            "velocityY": -2.12851787531304,
+            "timestamp": 0.3720043489905082
+        },
+        {
+            "x": 1.203636670620202,
+            "y": 3.876777756204273,
+            "heading": -0.5447644235662668,
+            "angularVelocity": 1.4795768851144226,
+            "velocityX": 2.33589994882,
+            "velocityY": -2.2694937614773476,
+            "timestamp": 0.39857608820411594
+        },
+        {
+            "x": 1.2693974629594884,
+            "y": 3.8133907541919347,
+            "heading": -0.5190150912676422,
+            "angularVelocity": 0.969049563960718,
+            "velocityX": 2.4748395959572558,
+            "velocityY": -2.3855044452595306,
+            "timestamp": 0.42514782741772367
+        },
+        {
+            "x": 1.3388335264794184,
+            "y": 3.7468848835898942,
+            "heading": -0.5067181615747146,
+            "angularVelocity": 0.4627822663045784,
+            "velocityX": 2.613154636275026,
+            "velocityY": -2.5028798479243823,
+            "timestamp": 0.4517195666313314
+        },
+        {
+            "x": 1.410439403600934,
+            "y": 3.676338986398213,
+            "heading": -0.5067181155488927,
+            "angularVelocity": 0.0000017321343363764108,
+            "velocityX": 2.6948133332892708,
+            "velocityY": -2.6549220818618244,
+            "timestamp": 0.4782913058449391
+        },
+        {
+            "x": 1.482040293717765,
+            "y": 3.605788027611728,
+            "heading": -0.5067180695571892,
+            "angularVelocity": 0.0000017308503249172533,
+            "velocityX": 2.694625652511417,
+            "velocityY": -2.655112569761896,
+            "timestamp": 0.5048630450585468
+        },
+        {
+            "x": 1.5536411829944505,
+            "y": 3.5352370679725946,
+            "heading": -0.5067180235654862,
+            "angularVelocity": 0.0000017308503100877725,
+            "velocityX": 2.6946256208934063,
+            "velocityY": -2.6551126018504427,
+            "timestamp": 0.5314347842721546
+        },
+        {
+            "x": 1.6252420722709946,
+            "y": 3.4646861083333174,
+            "heading": -0.5067179775737835,
+            "angularVelocity": 0.0000017308502987237163,
+            "velocityX": 2.6946256208880808,
+            "velocityY": -2.6551126018558486,
+            "timestamp": 0.5580065234857624
+        },
+        {
+            "x": 1.6968429615475384,
+            "y": 3.39413514869404,
+            "heading": -0.506717931582081,
+            "angularVelocity": 0.0000017308502871423956,
+            "velocityX": 2.6946256208880808,
+            "velocityY": -2.65511260185585,
+            "timestamp": 0.5845782626993702
+        },
+        {
+            "x": 1.7684438508240825,
+            "y": 3.323584189054763,
+            "heading": -0.5067178855903789,
+            "angularVelocity": 0.0000017308502762001758,
+            "velocityX": 2.694625620888081,
+            "velocityY": -2.65511260185585,
+            "timestamp": 0.611150001912978
+        },
+        {
+            "x": 1.8400447401006266,
+            "y": 3.2530332294154856,
+            "heading": -0.506717839598677,
+            "angularVelocity": 0.000001730850265567339,
+            "velocityX": 2.6946256208880826,
+            "velocityY": -2.6551126018558495,
+            "timestamp": 0.6377217411265858
+        },
+        {
+            "x": 1.9116456293771706,
+            "y": 3.1824822697762087,
+            "heading": -0.5067177936069754,
+            "angularVelocity": 0.0000017308502535085818,
+            "velocityX": 2.694625620888084,
+            "velocityY": -2.65511260185585,
+            "timestamp": 0.6642934803401935
+        },
+        {
+            "x": 1.9832465186537145,
+            "y": 3.1119313101369315,
+            "heading": -0.5067177476152741,
+            "angularVelocity": 0.0000017308502432657092,
+            "velocityX": 2.6946256208880843,
+            "velocityY": -2.65511260185585,
+            "timestamp": 0.6908652195538013
+        },
+        {
+            "x": 2.0548474079302586,
+            "y": 3.041380350497654,
+            "heading": -0.5067177016235731,
+            "angularVelocity": 0.000001730850231529751,
+            "velocityX": 2.694625620888085,
+            "velocityY": -2.6551126018558504,
+            "timestamp": 0.7174369587674091
+        },
+        {
+            "x": 2.1264482972068026,
+            "y": 2.970829390858377,
+            "heading": -0.5067176556318724,
+            "angularVelocity": 0.0000017308502204807436,
+            "velocityX": 2.6946256208880865,
+            "velocityY": -2.6551126018558495,
+            "timestamp": 0.7440086979810169
+        },
+        {
+            "x": 2.1980491864833867,
+            "y": 2.9002784312191405,
+            "heading": -0.5067176096401721,
+            "angularVelocity": 0.0000017308502087562552,
+            "velocityX": 2.694625620889591,
+            "velocityY": -2.6551126018543236,
+            "timestamp": 0.7705804371946247
+        },
+        {
+            "x": 2.2696500759972245,
+            "y": 2.829727471820689,
+            "heading": -0.506717563648472,
+            "angularVelocity": 0.0000017308501985869009,
+            "velocityX": 2.6946256298184115,
+            "velocityY": -2.6551125927926265,
+            "timestamp": 0.7971521764082324
+        },
+        {
+            "x": 2.3412523738653572,
+            "y": 2.7591779417640674,
+            "heading": -0.5067175176565489,
+            "angularVelocity": 0.0000017308585893832803,
+            "velocityX": 2.694678631779742,
+            "velocityY": -2.6550588009870313,
+            "timestamp": 0.8237239156218402
+        },
+        {
+            "x": 2.4158866145337003,
+            "y": 2.6918440324740667,
+            "heading": -0.5067174710515008,
+            "angularVelocity": 0.000001753932919872258,
+            "velocityX": 2.808782672009746,
+            "velocityY": -2.5340422299311864,
+            "timestamp": 0.850295654835448
+        },
+        {
+            "x": 2.4943342208862305,
+            "y": 2.6289942264556885,
+            "heading": -0.5067174224716422,
+            "angularVelocity": 0.000001828252870098291,
+            "velocityX": 2.952294756541798,
+            "velocityY": -2.365287628074865,
+            "timestamp": 0.8768673940490558
+        },
+        {
+            "x": 2.693828949223235,
+            "y": 2.509453130751911,
+            "heading": -0.5067173754009504,
+            "angularVelocity": 7.656472375459629e-7,
+            "velocityX": 3.244961599231293,
+            "velocityY": -1.9444436869204162,
+            "timestamp": 0.9383456957905196
+        },
+        {
+            "x": 2.900420237054028,
+            "y": 2.4026436735913763,
+            "heading": -0.5067173289743672,
+            "angularVelocity": 7.551702291921459e-7,
+            "velocityX": 3.3603935368868174,
+            "velocityY": -1.7373521085488381,
+            "timestamp": 0.9998239975319835
+        },
+        {
+            "x": 3.1070116472391316,
+            "y": 2.295834453089378,
+            "heading": -0.5067172825477892,
+            "angularVelocity": 7.55170142432061e-7,
+            "velocityX": 3.3603955270899863,
+            "velocityY": -1.7373482590844154,
+            "timestamp": 1.0613022992734473
+        },
+        {
+            "x": 3.3136030574258837,
+            "y": 2.1890252325905677,
+            "heading": -0.5067172361212112,
+            "angularVelocity": 7.551701448333524e-7,
+            "velocityX": 3.3603955271167996,
+            "velocityY": -1.737348259032553,
+            "timestamp": 1.1227806010149113
+        },
+        {
+            "x": 3.5201944676126358,
+            "y": 2.0822160120917577,
+            "heading": -0.5067171896946332,
+            "angularVelocity": 7.551701452492318e-7,
+            "velocityX": 3.3603955271167996,
+            "velocityY": -1.737348259032552,
+            "timestamp": 1.1842589027563752
+        },
+        {
+            "x": 3.726785877799388,
+            "y": 1.9754067915929476,
+            "heading": -0.5067171432680552,
+            "angularVelocity": 7.5517014486464e-7,
+            "velocityX": 3.3603955271168005,
+            "velocityY": -1.7373482590325509,
+            "timestamp": 1.2457372044978392
+        },
+        {
+            "x": 3.93337728798614,
+            "y": 1.8685975710941374,
+            "heading": -0.506717096841477,
+            "angularVelocity": 7.551701472516521e-7,
+            "velocityX": 3.360395527116801,
+            "velocityY": -1.7373482590325502,
+            "timestamp": 1.3072155062393032
+        },
+        {
+            "x": 4.139968698173285,
+            "y": 1.761788350596088,
+            "heading": -0.5067170504148988,
+            "angularVelocity": 7.551701467640916e-7,
+            "velocityX": 3.360395527123197,
+            "velocityY": -1.7373482590201774,
+            "timestamp": 1.3686938079807671
+        },
+        {
+            "x": 4.346560137549085,
+            "y": 1.6549791865550314,
+            "heading": -0.5067170039883208,
+            "angularVelocity": 7.55170145558408e-7,
+            "velocityX": 3.36039600190298,
+            "velocityY": -1.737347340696294,
+            "timestamp": 1.430172109722231
+        },
+        {
+            "x": 4.5552180589852185,
+            "y": 1.552265545137979,
+            "heading": -0.5067169574392034,
+            "angularVelocity": 7.571633584219393e-7,
+            "velocityX": 3.3940091955305984,
+            "velocityY": -1.6707299731374539,
+            "timestamp": 1.491650411463695
+        },
+        {
+            "x": 4.775846094749007,
+            "y": 1.478702717632789,
+            "heading": -0.5067169090858323,
+            "angularVelocity": 7.865111701229533e-7,
+            "velocityX": 3.5887138960279197,
+            "velocityY": -1.1965657056459655,
+            "timestamp": 1.553128713205159
+        },
+        {
+            "x": 5.004393851392972,
+            "y": 1.4356429788646896,
+            "heading": -0.5067168550876733,
+            "angularVelocity": 8.783287361409535e-7,
+            "velocityX": 3.717535295706156,
+            "velocityY": -0.7004054690576753,
+            "timestamp": 1.614607014946623
+        },
+        {
+            "x": 5.236664772033691,
+            "y": 1.4238771200180054,
+            "heading": -0.5067167933216484,
+            "angularVelocity": 0.000001004680076164895,
+            "velocityX": 3.7780959145145716,
+            "velocityY": -0.19138230096471237,
+            "timestamp": 1.676085316688087
+        },
+        {
+            "x": 5.366036550697627,
+            "y": 1.4270871399969536,
+            "heading": -0.5067167282581706,
+            "angularVelocity": 0.000001901925327561791,
+            "velocityX": 3.781775437099809,
+            "velocityY": 0.09383479793163368,
+            "timestamp": 1.7102945871261934
+        },
+        {
+            "x": 5.494798697340742,
+            "y": 1.4400359689986706,
+            "heading": -0.5067166730622293,
+            "angularVelocity": 0.0000016134790500794753,
+            "velocityX": 3.76395476998197,
+            "velocityY": 0.37851812786083194,
+            "timestamp": 1.7445038575643
+        },
+        {
+            "x": 5.6222191444596294,
+            "y": 1.4626499710733982,
+            "heading": -0.5067166242756445,
+            "angularVelocity": 0.0000014261217576278581,
+            "velocityX": 3.724734421022641,
+            "velocityY": 0.6610489433161765,
+            "timestamp": 1.7787131280024064
+        },
+        {
+            "x": 5.747573461036382,
+            "y": 1.4948005438980272,
+            "heading": -0.5067165796372217,
+            "angularVelocity": 0.0000013048633380814483,
+            "velocityX": 3.6643376187618593,
+            "velocityY": 0.9398204759378852,
+            "timestamp": 1.812922398440513
+        },
+        {
+            "x": 5.870148986660447,
+            "y": 1.5363048197489622,
+            "heading": -0.5067165375293654,
+            "angularVelocity": 0.0000012308902156872765,
+            "velocityX": 3.583108439738183,
+            "velocityY": 1.2132464480944425,
+            "timestamp": 1.8471316688786195
+        },
+        {
+            "x": 5.9892489774115445,
+            "y": 1.5869265226922225,
+            "heading": -0.5067164966802475,
+            "angularVelocity": 0.0000011940949728764683,
+            "velocityX": 3.481512152285735,
+            "velocityY": 1.4797656394002383,
+            "timestamp": 1.881340939316726
+        },
+        {
+            "x": 6.104355729464833,
+            "y": 1.6460687267313168,
+            "heading": -0.5067164560018657,
+            "angularVelocity": 0.0000011891040447074693,
+            "velocityX": 3.3647824282469623,
+            "velocityY": 1.7288355840881788,
+            "timestamp": 1.9155502097548325
+        },
+        {
+            "x": 6.219456425834801,
+            "y": 1.705222715286976,
+            "heading": -0.5067164153233215,
+            "angularVelocity": 0.0000011891087921450935,
+            "velocityX": 3.3646054094668667,
+            "velocityY": 1.7291800672184492,
+            "timestamp": 1.949759480192939
+        },
+        {
+            "x": 6.334557121741728,
+            "y": 1.764376704743614,
+            "heading": -0.5067163746447773,
+            "angularVelocity": 0.0000011891087898511233,
+            "velocityX": 3.3646053959312843,
+            "velocityY": 1.7291800935557256,
+            "timestamp": 1.9839687506310455
+        },
+        {
+            "x": 6.449657817648618,
+            "y": 1.8235306942003213,
+            "heading": -0.5067163339662332,
+            "angularVelocity": 0.000001189108787218646,
+            "velocityX": 3.3646053959302495,
+            "velocityY": 1.7291800935577397,
+            "timestamp": 2.018178021069152
+        },
+        {
+            "x": 6.564758513555509,
+            "y": 1.8826846836570283,
+            "heading": -0.5067162932876892,
+            "angularVelocity": 0.0000011891087819705439,
+            "velocityX": 3.3646053959302504,
+            "velocityY": 1.7291800935577386,
+            "timestamp": 2.0523872915072587
+        },
+        {
+            "x": 6.679859209462396,
+            "y": 1.9418386731137416,
+            "heading": -0.5067162526091454,
+            "angularVelocity": 0.0000011891087784797324,
+            "velocityX": 3.3646053959301545,
+            "velocityY": 1.7291800935579253,
+            "timestamp": 2.0865965619453655
+        },
+        {
+            "x": 6.7949599053264,
+            "y": 2.0009926626538963,
+            "heading": -0.5067162119306018,
+            "angularVelocity": 0.0000011891087739781964,
+            "velocityX": 3.3646053946765995,
+            "velocityY": 1.7291800959970696,
+            "timestamp": 2.120805832383472
+        },
+        {
+            "x": 6.910060040297267,
+            "y": 2.06014774355027,
+            "heading": -0.5067161712446878,
+            "angularVelocity": 0.0000011893242211155864,
+            "velocityX": 3.3645889987368625,
+            "velocityY": 1.7292119983499907,
+            "timestamp": 2.155015102821579
+        },
+        {
+            "x": 7.017868923227757,
+            "y": 2.1174564154331827,
+            "heading": -0.4884934235175169,
+            "angularVelocity": 0.5326844885552473,
+            "velocityX": 3.151452268634164,
+            "velocityY": 1.6752380611740627,
+            "timestamp": 2.1892243732596857
+        },
+        {
+            "x": 7.119036711180674,
+            "y": 2.1700871649460596,
+            "heading": -0.45052101262699384,
+            "angularVelocity": 1.1100035284068692,
+            "velocityX": 2.9573208272872957,
+            "velocityY": 1.5384937719762113,
+            "timestamp": 2.2234336436977924
+        },
+        {
+            "x": 7.212945348428852,
+            "y": 2.2182419084801723,
+            "heading": -0.3958429177265348,
+            "angularVelocity": 1.5983414495607555,
+            "velocityX": 2.745122478366898,
+            "velocityY": 1.4076518708938115,
+            "timestamp": 2.257642914135899
+        },
+        {
+            "x": 7.29827375889861,
+            "y": 2.2619654936466005,
+            "heading": -0.3361817714679226,
+            "angularVelocity": 1.7440052212324946,
+            "velocityX": 2.494306641941948,
+            "velocityY": 1.2781209481077944,
+            "timestamp": 2.291852184574006
+        },
+        {
+            "x": 7.374929402835313,
+            "y": 2.3012175879094494,
+            "heading": -0.2767819040095215,
+            "angularVelocity": 1.7363675605380382,
+            "velocityX": 2.2407856979994336,
+            "velocityY": 1.1474110309913224,
+            "timestamp": 2.3260614550121126
+        },
+        {
+            "x": 7.442942536538503,
+            "y": 2.336017519822898,
+            "heading": -0.22036844778019443,
+            "angularVelocity": 1.6490692583284896,
+            "velocityX": 1.9881492014348112,
+            "velocityY": 1.0172661231232827,
+            "timestamp": 2.3602707254502193
+        },
+        {
+            "x": 7.502356073864926,
+            "y": 2.3663915967666074,
+            "heading": -0.16856655782281169,
+            "angularVelocity": 1.5142646801283255,
+            "velocityX": 1.7367671559648723,
+            "velocityY": 0.8878902284298598,
+            "timestamp": 2.394479995888326
+        },
+        {
+            "x": 7.553208169092694,
+            "y": 2.392365594084153,
+            "heading": -0.12246425110299042,
+            "angularVelocity": 1.3476553615264124,
+            "velocityX": 1.4865004303372251,
+            "velocityY": 0.7592677945160818,
+            "timestamp": 2.428689266326433
+        },
+        {
+            "x": 7.595530700913324,
+            "y": 2.4139635888667987,
+            "heading": -0.08285639859822795,
+            "angularVelocity": 1.1578104998299616,
+            "velocityX": 1.2371655775940182,
+            "velocityY": 0.6313491783381474,
+            "timestamp": 2.4628985367645395
+        },
+        {
+            "x": 7.629350339345417,
+            "y": 2.431207580669044,
+            "heading": -0.050358801794747596,
+            "angularVelocity": 0.9499646261757312,
+            "velocityX": 0.988610338629752,
+            "velocityY": 0.5040736496688569,
+            "timestamp": 2.4971078072026462
+        },
+        {
+            "x": 7.654689602970606,
+            "y": 2.444117248020803,
+            "heading": -0.025464280349729525,
+            "angularVelocity": 0.7277127260009457,
+            "velocityX": 0.7407133592934628,
+            "velocityY": 0.3773733606834889,
+            "timestamp": 2.531317077640753
+        },
+        {
+            "x": 7.671567523131761,
+            "y": 2.4527098140869614,
+            "heading": -0.008571412442380029,
+            "angularVelocity": 0.49380965133159116,
+            "velocityX": 0.4933727011714593,
+            "velocityY": 0.2511765365386793,
+            "timestamp": 2.5655263480788597
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 5.0379367595035626e-36,
+            "angularVelocity": 0.2505581771434718,
+            "velocityX": 0.24649683434481964,
+            "velocityY": 0.12541003821758284,
+            "timestamp": 2.5997356185169664
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 2.854280403338002e-36,
+            "angularVelocity": -6.383230494440131e-35,
+            "velocityX": -1.2082929677962258e-35,
+            "velocityY": 1.8766665143888344e-34,
+            "timestamp": 2.633944888955073
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.1.traj
new file mode 100644
index 00000000..818f5ad4
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.1.traj	
@@ -0,0 +1,337 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": -1.04,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 1.4815405069278332e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7317173879428727,
+            "y": 4.344479531295635,
+            "heading": -1.013582317544266,
+            "angularVelocity": 0.4585462516411977,
+            "velocityX": 0.37696065327942924,
+            "velocityY": -0.26939731598432753,
+            "timestamp": 0.05761181638969176
+        },
+        {
+            "x": 0.7752730705207751,
+            "y": 4.313424298338961,
+            "heading": -0.9621480921974644,
+            "angularVelocity": 0.892772152832256,
+            "velocityX": 0.7560199505477795,
+            "velocityY": -0.5390427676609577,
+            "timestamp": 0.11522363277938352
+        },
+        {
+            "x": 0.8408250898493527,
+            "y": 4.266812487548848,
+            "heading": -0.8877439224939486,
+            "angularVelocity": 1.2914741170498636,
+            "velocityX": 1.1378224717856051,
+            "velocityY": -0.8090668496689294,
+            "timestamp": 0.17283544916907528
+        },
+        {
+            "x": 0.9285847241631874,
+            "y": 4.204600540366139,
+            "heading": -0.7936992773737622,
+            "angularVelocity": 1.6323846567179892,
+            "velocityX": 1.5232922656043415,
+            "velocityY": -1.0798470015578314,
+            "timestamp": 0.23044726555876704
+        },
+        {
+            "x": 1.0388213735909644,
+            "y": 4.1267152282410935,
+            "heading": -0.6856850144743787,
+            "angularVelocity": 1.8748629997840256,
+            "velocityX": 1.9134381857035365,
+            "velocityY": -1.3518982216811457,
+            "timestamp": 0.2880590819484588
+        },
+        {
+            "x": 1.1718232312823271,
+            "y": 4.033123229014291,
+            "heading": -0.5741627817643435,
+            "angularVelocity": 1.935752762171023,
+            "velocityX": 2.3085864328894954,
+            "velocityY": -1.6245278328622093,
+            "timestamp": 0.34567089833815057
+        },
+        {
+            "x": 1.3270687303513053,
+            "y": 3.9246703032584405,
+            "heading": -0.488682735559766,
+            "angularVelocity": 1.4837242003685942,
+            "velocityX": 2.6946815566251776,
+            "velocityY": -1.882477112373353,
+            "timestamp": 0.40328271472784233
+        },
+        {
+            "x": 1.5010681185276094,
+            "y": 3.804139759424889,
+            "heading": -0.46956462349945394,
+            "angularVelocity": 0.3318435914430377,
+            "velocityX": 3.020203129846763,
+            "velocityY": -2.0921149754812625,
+            "timestamp": 0.4608945311175341
+        },
+        {
+            "x": 1.6785494425409546,
+            "y": 3.677652137140934,
+            "heading": -0.4695645649556455,
+            "angularVelocity": 0.0000010161770978802793,
+            "velocityX": 3.080641006227699,
+            "velocityY": -2.195515264236428,
+            "timestamp": 0.5185063475072258
+        },
+        {
+            "x": 1.85603075289391,
+            "y": 3.551164495689284,
+            "heading": -0.4695645064119953,
+            "angularVelocity": 0.000001016174352223477,
+            "velocityX": 3.080640769116793,
+            "velocityY": -2.1955155969406546,
+            "timestamp": 0.5761181638969175
+        },
+        {
+            "x": 2.033512086622271,
+            "y": 3.4246768870368545,
+            "heading": -0.4695644478683731,
+            "angularVelocity": 0.0000010161738659585955,
+            "velocityX": 3.0806411748565776,
+            "velocityY": -2.195515027626543,
+            "timestamp": 0.6337299802866092
+        },
+        {
+            "x": 2.2109934521039283,
+            "y": 3.298189322939119,
+            "heading": -0.46956438932476746,
+            "angularVelocity": 0.0000010161735782211743,
+            "velocityX": 3.080641726016013,
+            "velocityY": -2.1955142542661994,
+            "timestamp": 0.6913417966763009
+        },
+        {
+            "x": 2.3884748618363862,
+            "y": 3.17170182093203,
+            "heading": -0.4695643307811488,
+            "angularVelocity": 0.000001016173805422874,
+            "velocityX": 3.0806424941015065,
+            "velocityY": -2.1955131765246763,
+            "timestamp": 0.7489536130659926
+        },
+        {
+            "x": 2.5659563234551954,
+            "y": 3.045214391729511,
+            "heading": -0.4695642722374779,
+            "angularVelocity": 0.0000010161747112988211,
+            "velocityX": 3.0806433947214558,
+            "velocityY": -2.1955119128156926,
+            "timestamp": 0.8065654294556843
+        },
+        {
+            "x": 2.743437831089148,
+            "y": 2.918727027093402,
+            "heading": -0.46956421369376844,
+            "angularVelocity": 0.000001016175381373278,
+            "velocityX": 3.08064419343162,
+            "velocityY": -2.1955107921009867,
+            "timestamp": 0.864177245845376
+        },
+        {
+            "x": 2.920919379832659,
+            "y": 2.7922397201404365,
+            "heading": -0.4695641551500532,
+            "angularVelocity": 0.0000010161754817816058,
+            "velocityX": 3.080644906992851,
+            "velocityY": -2.195509790862922,
+            "timestamp": 0.9217890622350677
+        },
+        {
+            "x": 3.0984009676669872,
+            "y": 2.6657524680380327,
+            "heading": -0.4695640966063568,
+            "angularVelocity": 0.000001016175153333812,
+            "velocityX": 3.080645585513673,
+            "velocityY": -2.195508838791537,
+            "timestamp": 0.9794008786247594
+        },
+        {
+            "x": 3.275882647398041,
+            "y": 2.5392653448813247,
+            "heading": -0.4695640380626867,
+            "angularVelocity": 0.0000010161746978103879,
+            "velocityX": 3.080647180615714,
+            "velocityY": -2.1955066006101345,
+            "timestamp": 1.0370126950144511
+        },
+        {
+            "x": 3.4563703536987305,
+            "y": 2.417105913162231,
+            "heading": -0.4695639788949644,
+            "angularVelocity": 0.0000010270067149252654,
+            "velocityX": 3.1328244379564545,
+            "velocityY": -2.1203884788634437,
+            "timestamp": 1.0946245114041429
+        },
+        {
+            "x": 3.7578524899325245,
+            "y": 2.288322056940197,
+            "heading": -0.4695639274563231,
+            "angularVelocity": 5.9355601779034e-7,
+            "velocityX": 3.4788348155609183,
+            "velocityY": -1.4860507766867785,
+            "timestamp": 1.1812863255510104
+        },
+        {
+            "x": 4.061243078218464,
+            "y": 2.1641008385808727,
+            "heading": -0.46956387627292245,
+            "angularVelocity": 5.906107685245515e-7,
+            "velocityX": 3.5008566491785738,
+            "velocityY": -1.43340200735705,
+            "timestamp": 1.267948139697878
+        },
+        {
+            "x": 4.364633703659485,
+            "y": 2.039879710966971,
+            "heading": -0.46956382508952316,
+            "angularVelocity": 5.906107531304183e-7,
+            "velocityX": 3.5008570779150574,
+            "velocityY": -1.433400960235855,
+            "timestamp": 1.3546099538447456
+        },
+        {
+            "x": 4.668024366282006,
+            "y": 1.9156586741630988,
+            "heading": -0.46956377390610504,
+            "angularVelocity": 5.906109701114576e-7,
+            "velocityX": 3.5008575069563967,
+            "velocityY": -1.4333999123691519,
+            "timestamp": 1.4412717679916132
+        },
+        {
+            "x": 4.971415087165123,
+            "y": 1.7914377796518506,
+            "heading": -0.4695637227226054,
+            "angularVelocity": 5.906119107964098e-7,
+            "velocityX": 3.5008581792316793,
+            "velocityY": -1.4333982704392505,
+            "timestamp": 1.5279335821384807
+        },
+        {
+            "x": 5.2748058711699946,
+            "y": 1.6672170393060612,
+            "heading": -0.4695636715390663,
+            "angularVelocity": 5.906123657897232e-7,
+            "velocityX": 3.5008589076003997,
+            "velocityY": -1.4333964915074275,
+            "timestamp": 1.6145953962853483
+        },
+        {
+            "x": 5.5781967938554144,
+            "y": 1.5429966376674569,
+            "heading": -0.4695636203556594,
+            "angularVelocity": 5.906108405690599e-7,
+            "velocityX": 3.5008605078501667,
+            "velocityY": -1.4333925831287677,
+            "timestamp": 1.701257210432216
+        },
+        {
+            "x": 5.886988948403778,
+            "y": 1.4328868446307816,
+            "heading": -0.4695635793301605,
+            "angularVelocity": 4.733976467078746e-7,
+            "velocityX": 3.5631859035981908,
+            "velocityY": -1.2705687518849984,
+            "timestamp": 1.7879190245790835
+        },
+        {
+            "x": 6.195781169913944,
+            "y": 1.3227772393821355,
+            "heading": -0.4695635383046595,
+            "angularVelocity": 4.7339767079441426e-7,
+            "velocityX": 3.5631866762776085,
+            "velocityY": -1.2705665849787193,
+            "timestamp": 1.874580838725951
+        },
+        {
+            "x": 6.5045734104803365,
+            "y": 1.212667687574965,
+            "heading": -0.46956349727913566,
+            "angularVelocity": 4.733979354519185e-7,
+            "velocityX": 3.5631868961694693,
+            "velocityY": -1.270565968311784,
+            "timestamp": 1.9612426528728186
+        },
+        {
+            "x": 6.813365656566466,
+            "y": 1.1025581512474965,
+            "heading": -0.46956345625352064,
+            "angularVelocity": 4.733989867793667e-7,
+            "velocityX": 3.563186959862305,
+            "velocityY": -1.270565789689833,
+            "timestamp": 2.0479044670196864
+        },
+        {
+            "x": 7.101065618447083,
+            "y": 0.9964425714529646,
+            "heading": -0.4012732100130825,
+            "angularVelocity": 0.7880085007764233,
+            "velocityX": 3.3198008224596522,
+            "velocityY": -1.2244790954259896,
+            "timestamp": 2.134566281166554
+        },
+        {
+            "x": 7.333266238045498,
+            "y": 0.911992130350644,
+            "heading": -0.2613611242014092,
+            "angularVelocity": 1.6144606155435655,
+            "velocityX": 2.6793879390165927,
+            "velocityY": -0.9744827284506232,
+            "timestamp": 2.2212280953134216
+        },
+        {
+            "x": 7.506890250099573,
+            "y": 0.8492417427348896,
+            "heading": -0.1371754084070899,
+            "angularVelocity": 1.43299233943868,
+            "velocityX": 2.0034661605379225,
+            "velocityY": -0.7240834759056616,
+            "timestamp": 2.307889909460289
+        },
+        {
+            "x": 7.622366585478705,
+            "y": 0.8076799304753898,
+            "heading": -0.04732610702177306,
+            "angularVelocity": 1.0367807582823931,
+            "velocityX": 1.332493861522807,
+            "velocityY": -0.4795862245517285,
+            "timestamp": 2.3945517236071567
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 2.9489608951695904e-31,
+            "angularVelocity": 0.5461010421679925,
+            "velocityX": 0.6650381726791765,
+            "velocityY": -0.2386279433331854,
+            "timestamp": 2.4812135377540243
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -7.162012659830636e-32,
+            "angularVelocity": -4.0878892657023166e-30,
+            "velocityX": 2.3668785600931567e-32,
+            "velocityY": 6.81692291678628e-32,
+            "timestamp": 2.567875351900892
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.1.traj
new file mode 100644
index 00000000..fccc0721
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.1.traj	
@@ -0,0 +1,238 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": -1.04,
+            "angularVelocity": 3.226894361928835e-32,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.724288196557008,
+            "y": 4.368098642385211,
+            "heading": -1.0157828821188275,
+            "angularVelocity": 0.526633856876912,
+            "velocityX": 0.3107160851076989,
+            "velocityY": 0.17611589024403787,
+            "timestamp": 0.0459847341088113
+        },
+        {
+            "x": 0.7529777435005003,
+            "y": 4.384403118337835,
+            "heading": -0.9690307892055188,
+            "angularVelocity": 1.0166872510929454,
+            "velocityX": 0.6238928526933836,
+            "velocityY": 0.3545627971674321,
+            "timestamp": 0.0919694682176226
+        },
+        {
+            "x": 0.7962133909539325,
+            "y": 4.4090308090891845,
+            "heading": -0.9019535610081195,
+            "angularVelocity": 1.4586847025950622,
+            "velocityX": 0.9402174067404033,
+            "velocityY": 0.5355623171174385,
+            "timestamp": 0.1379542023264339
+        },
+        {
+            "x": 0.8541780196778807,
+            "y": 4.442096684944602,
+            "heading": -0.8174192357524789,
+            "angularVelocity": 1.8383127986701173,
+            "velocityX": 1.2605189493276492,
+            "velocityY": 0.7190620212605457,
+            "timestamp": 0.1839389364352452
+        },
+        {
+            "x": 0.92708218377146,
+            "y": 4.483710427261066,
+            "heading": -0.7190735575075958,
+            "angularVelocity": 2.1386592779291416,
+            "velocityX": 1.585399274486893,
+            "velocityY": 0.9049468942886179,
+            "timestamp": 0.2299236705440565
+        },
+        {
+            "x": 1.015132605778429,
+            "y": 4.5339957868438265,
+            "heading": -0.6117681897735129,
+            "angularVelocity": 2.333499797567627,
+            "velocityX": 1.914775059884505,
+            "velocityY": 1.0935228953105474,
+            "timestamp": 0.2759084046528678
+        },
+        {
+            "x": 1.1184514719230099,
+            "y": 4.593093468568762,
+            "heading": -0.5033518248752775,
+            "angularVelocity": 2.357659927786404,
+            "velocityX": 2.246807949353511,
+            "velocityY": 1.2851587134349265,
+            "timestamp": 0.32189313876167913
+        },
+        {
+            "x": 1.2364935087746176,
+            "y": 4.660915178231087,
+            "heading": -0.4100171287779816,
+            "angularVelocity": 2.029688719661721,
+            "velocityX": 2.566983133408849,
+            "velocityY": 1.4748744551145279,
+            "timestamp": 0.36787787287049045
+        },
+        {
+            "x": 1.3657550031232673,
+            "y": 4.73811911608404,
+            "heading": -0.35248337281854053,
+            "angularVelocity": 1.2511490405338719,
+            "velocityX": 2.8109653530405243,
+            "velocityY": 1.6789036481164823,
+            "timestamp": 0.4138626069793018
+        },
+        {
+            "x": 1.5059170232916794,
+            "y": 4.82520017003363,
+            "heading": -0.3304805961437169,
+            "angularVelocity": 0.47848002388715516,
+            "velocityX": 3.0480119736421156,
+            "velocityY": 1.8936948454141527,
+            "timestamp": 0.4598473410881131
+        },
+        {
+            "x": 1.6456219690029539,
+            "y": 4.9288521484832435,
+            "heading": -0.3304805109628974,
+            "angularVelocity": 0.0000018523716874218418,
+            "velocityX": 3.038072273739655,
+            "velocityY": 2.2540519252400664,
+            "timestamp": 0.5058320751969244
+        },
+        {
+            "x": 1.7741171019961581,
+            "y": 5.046113419715312,
+            "heading": -0.3304804288379279,
+            "angularVelocity": 0.000001785918111046389,
+            "velocityX": 2.794299792821407,
+            "velocityY": 2.5500043330598703,
+            "timestamp": 0.5518168093057357
+        },
+        {
+            "x": 1.8900823069364519,
+            "y": 5.175779429346925,
+            "heading": -0.330480329845049,
+            "angularVelocity": 0.000002152733527656335,
+            "velocityX": 2.5218196253105503,
+            "velocityY": 2.8197620828859327,
+            "timestamp": 0.597801543414547
+        },
+        {
+            "x": 1.992326225217661,
+            "y": 5.31651812358221,
+            "heading": -0.3304801981354216,
+            "angularVelocity": 0.0000028642033047051737,
+            "velocityX": 2.2234317597502637,
+            "velocityY": 3.0605525281991963,
+            "timestamp": 0.6437862775233584
+        },
+        {
+            "x": 2.079798460006714,
+            "y": 5.466883659362793,
+            "heading": -0.3304799560936257,
+            "angularVelocity": 0.000005263524964464214,
+            "velocityX": 1.9022015998192814,
+            "velocityY": 3.2699011681743237,
+            "timestamp": 0.6897710116321697
+        },
+        {
+            "x": 2.16894705974312,
+            "y": 5.683675424803782,
+            "heading": -0.33027266768763375,
+            "angularVelocity": 0.0033444025204108644,
+            "velocityX": 1.4383284005893175,
+            "velocityY": 3.49773024107632,
+            "timestamp": 0.751751710685232
+        },
+        {
+            "x": 2.226835014262279,
+            "y": 5.903744633222095,
+            "heading": -0.30773378760994813,
+            "angularVelocity": 0.3636435280988788,
+            "velocityX": 0.9339674350815139,
+            "velocityY": 3.550608685938014,
+            "timestamp": 0.8137324097382944
+        },
+        {
+            "x": 2.2584550007033997,
+            "y": 6.112330245271792,
+            "heading": -0.23499422209122486,
+            "angularVelocity": 1.1735841419993758,
+            "velocityX": 0.5101585965339779,
+            "velocityY": 3.3653317119112898,
+            "timestamp": 0.8757131087913568
+        },
+        {
+            "x": 2.277604108948135,
+            "y": 6.297377159682904,
+            "heading": -0.09486114062468509,
+            "angularVelocity": 2.260914826833697,
+            "velocityX": 0.30895276331652,
+            "velocityY": 2.985557072415336,
+            "timestamp": 0.9376938078444191
+        },
+        {
+            "x": 2.288511286424087,
+            "y": 6.451498238253786,
+            "heading": 0.04992678964099742,
+            "angularVelocity": 2.336016412815077,
+            "velocityX": 0.1759769999789598,
+            "velocityY": 2.48659793977057,
+            "timestamp": 0.9996745068974815
+        },
+        {
+            "x": 2.294708988562371,
+            "y": 6.5742371130661414,
+            "heading": 0.17908010356662757,
+            "angularVelocity": 2.083766654762434,
+            "velocityX": 0.09999406642681147,
+            "velocityY": 1.9802757420867263,
+            "timestamp": 1.061655205950544
+        },
+        {
+            "x": 2.2980089897573315,
+            "y": 6.66587189325695,
+            "heading": 0.2827001929036045,
+            "angularVelocity": 1.6718122079950113,
+            "velocityX": 0.05324240038246661,
+            "velocityY": 1.478440572481471,
+            "timestamp": 1.1236359050036064
+        },
+        {
+            "x": 2.2995212581337525,
+            "y": 6.72670613551003,
+            "heading": 0.35522073089449935,
+            "angularVelocity": 1.1700503398453694,
+            "velocityX": 0.024399020978011547,
+            "velocityY": 0.9815030030719727,
+            "timestamp": 1.185616604056669
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0.609532801060419,
+            "velocityX": 0.007724047543277551,
+            "velocityY": 0.48876287219728576,
+            "timestamp": 1.2475973031097314
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 2.340826570383063e-30,
+            "velocityX": -1.14239845433479e-30,
+            "velocityY": 0,
+            "timestamp": 1.3095780021627939
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.traj
index 9649eb8c..fccc0721 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.traj	
@@ -4,190 +4,235 @@
             "x": 0.71,
             "y": 4.36,
             "heading": -1.04,
-            "angularVelocity": 0,
+            "angularVelocity": 3.226894361928835e-32,
             "velocityX": 0,
             "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.7269596910839872,
-            "y": 4.384958038291164,
-            "heading": -1.001571192673364,
-            "angularVelocity": 0.6206074004453302,
-            "velocityX": 0.27389113865872694,
-            "velocityY": 0.40306073338263126,
-            "timestamp": 0.06192128437247234
-        },
-        {
-            "x": 0.760846036232504,
-            "y": 4.4351182711466235,
-            "heading": -0.9264796681168371,
-            "angularVelocity": 1.2126932656117428,
-            "velocityX": 0.5472487447883301,
-            "velocityY": 0.8100644772439347,
-            "timestamp": 0.12384256874494468
-        },
-        {
-            "x": 0.8116293344011996,
-            "y": 4.510831947021006,
-            "heading": -0.817810113065978,
-            "angularVelocity": 1.7549628718484593,
-            "velocityX": 0.8201266928382989,
-            "velocityY": 1.2227407206049843,
-            "timestamp": 0.185763853117417
-        },
-        {
-            "x": 0.8793448344180163,
-            "y": 4.612606899775036,
-            "heading": -0.6817312187907495,
-            "angularVelocity": 2.197610977457753,
-            "velocityX": 1.0935738930977383,
-            "velocityY": 1.6436182450904597,
-            "timestamp": 0.24768513748988935
-        },
-        {
-            "x": 0.9640984700059907,
-            "y": 4.74109575406918,
-            "heading": -0.5303096586797065,
-            "angularVelocity": 2.4453879089491037,
-            "velocityX": 1.3687318738119136,
-            "velocityY": 2.075035354907217,
-            "timestamp": 0.3096064218623617
-        },
-        {
-            "x": 1.0654137163163762,
-            "y": 4.896663616218439,
-            "heading": -0.3925803264267699,
-            "angularVelocity": 2.224264784697608,
-            "velocityX": 1.6361941994120859,
-            "velocityY": 2.5123487622362273,
-            "timestamp": 0.37152770623483405
-        },
-        {
-            "x": 1.179096696888173,
-            "y": 5.075186082347611,
-            "heading": -0.3289347906925715,
-            "angularVelocity": 1.0278458591290565,
-            "velocityX": 1.8359273668802587,
-            "velocityY": 2.88305496144612,
-            "timestamp": 0.4334489906073064
-        },
-        {
-            "x": 1.3095067112863388,
-            "y": 5.269772108097061,
-            "heading": -0.32893471990297235,
-            "angularVelocity": 0.0000011432191669124052,
-            "velocityX": 2.1060611988232667,
-            "velocityY": 3.142473992932148,
-            "timestamp": 0.49537027497977876
-        },
-        {
-            "x": 1.4399171936865056,
-            "y": 5.464357820195416,
-            "heading": -0.32893464911575754,
-            "angularVelocity": 0.0000011431806608935866,
-            "velocityX": 2.106068756838354,
-            "velocityY": 3.1424689276125553,
-            "timestamp": 0.557291559352251
-        },
-        {
-            "x": 1.570327676092705,
-            "y": 5.6589435322897295,
-            "heading": -0.3289345783285442,
-            "angularVelocity": 0.0000011431806365404781,
-            "velocityX": 2.106068756935774,
-            "velocityY": 3.1424689275472675,
-            "timestamp": 0.6192128437247234
-        },
-        {
-            "x": 1.7007381584918697,
-            "y": 5.853529244388756,
-            "heading": -0.32893450754133247,
-            "angularVelocity": 0.000001143180612539941,
-            "velocityX": 2.1060687568221708,
-            "velocityY": 3.1424689276234057,
-            "timestamp": 0.6811341280971958
-        },
-        {
-            "x": 1.8311480951447636,
-            "y": 6.048115322242248,
-            "heading": -0.3289344367512267,
-            "angularVelocity": 0.0000011432273482693713,
-            "velocityX": 2.1060599432732174,
-            "velocityY": 3.1424748343882203,
-            "timestamp": 0.7430554124696681
-        },
-        {
-            "x": 1.9446381116074218,
-            "y": 6.226114434087632,
-            "heading": -0.2658064774598036,
-            "angularVelocity": 1.0194872398268149,
-            "velocityX": 1.8328110860877296,
-            "velocityY": 2.8746030326934724,
-            "timestamp": 0.8049766968421405
-        },
-        {
-            "x": 2.0457809471284243,
-            "y": 6.380047389358405,
-            "heading": -0.1375002927378408,
-            "angularVelocity": 2.0720853261080365,
-            "velocityX": 1.6334098451931771,
-            "velocityY": 2.485945775039604,
-            "timestamp": 0.8668979812146128
-        },
-        {
-            "x": 2.1306775060879315,
-            "y": 6.50749359169558,
-            "heading": 0.01763716177237307,
-            "angularVelocity": 2.5053978786522335,
-            "velocityX": 1.3710400199200075,
-            "velocityY": 2.0581970097802293,
-            "timestamp": 0.9288192655870852
-        },
-        {
-            "x": 2.198423978678441,
-            "y": 6.608249711273454,
-            "heading": 0.15869859646801654,
-            "angularVelocity": 2.278076692452355,
-            "velocityX": 1.0940740857860296,
-            "velocityY": 1.6271645622174213,
-            "timestamp": 0.9907405499595575
-        },
-        {
-            "x": 2.24920191473134,
-            "y": 6.683037424747583,
-            "heading": 0.27235604329703667,
-            "angularVelocity": 1.8355150087866652,
-            "velocityX": 0.8200400971571714,
-            "velocityY": 1.2077868576540292,
-            "timestamp": 1.0526618343320298
-        },
-        {
-            "x": 2.2830642302273354,
-            "y": 6.732456156660043,
-            "heading": 0.35192243545091534,
-            "angularVelocity": 1.2849603001653922,
-            "velocityX": 0.5468606770541943,
-            "velocityY": 0.7980895812043076,
-            "timestamp": 1.114583118704502
+            "x": 0.724288196557008,
+            "y": 4.368098642385211,
+            "heading": -1.0157828821188275,
+            "angularVelocity": 0.526633856876912,
+            "velocityX": 0.3107160851076989,
+            "velocityY": 0.17611589024403787,
+            "timestamp": 0.0459847341088113
+        },
+        {
+            "x": 0.7529777435005003,
+            "y": 4.384403118337835,
+            "heading": -0.9690307892055188,
+            "angularVelocity": 1.0166872510929454,
+            "velocityX": 0.6238928526933836,
+            "velocityY": 0.3545627971674321,
+            "timestamp": 0.0919694682176226
+        },
+        {
+            "x": 0.7962133909539325,
+            "y": 4.4090308090891845,
+            "heading": -0.9019535610081195,
+            "angularVelocity": 1.4586847025950622,
+            "velocityX": 0.9402174067404033,
+            "velocityY": 0.5355623171174385,
+            "timestamp": 0.1379542023264339
+        },
+        {
+            "x": 0.8541780196778807,
+            "y": 4.442096684944602,
+            "heading": -0.8174192357524789,
+            "angularVelocity": 1.8383127986701173,
+            "velocityX": 1.2605189493276492,
+            "velocityY": 0.7190620212605457,
+            "timestamp": 0.1839389364352452
+        },
+        {
+            "x": 0.92708218377146,
+            "y": 4.483710427261066,
+            "heading": -0.7190735575075958,
+            "angularVelocity": 2.1386592779291416,
+            "velocityX": 1.585399274486893,
+            "velocityY": 0.9049468942886179,
+            "timestamp": 0.2299236705440565
+        },
+        {
+            "x": 1.015132605778429,
+            "y": 4.5339957868438265,
+            "heading": -0.6117681897735129,
+            "angularVelocity": 2.333499797567627,
+            "velocityX": 1.914775059884505,
+            "velocityY": 1.0935228953105474,
+            "timestamp": 0.2759084046528678
+        },
+        {
+            "x": 1.1184514719230099,
+            "y": 4.593093468568762,
+            "heading": -0.5033518248752775,
+            "angularVelocity": 2.357659927786404,
+            "velocityX": 2.246807949353511,
+            "velocityY": 1.2851587134349265,
+            "timestamp": 0.32189313876167913
+        },
+        {
+            "x": 1.2364935087746176,
+            "y": 4.660915178231087,
+            "heading": -0.4100171287779816,
+            "angularVelocity": 2.029688719661721,
+            "velocityX": 2.566983133408849,
+            "velocityY": 1.4748744551145279,
+            "timestamp": 0.36787787287049045
+        },
+        {
+            "x": 1.3657550031232673,
+            "y": 4.73811911608404,
+            "heading": -0.35248337281854053,
+            "angularVelocity": 1.2511490405338719,
+            "velocityX": 2.8109653530405243,
+            "velocityY": 1.6789036481164823,
+            "timestamp": 0.4138626069793018
+        },
+        {
+            "x": 1.5059170232916794,
+            "y": 4.82520017003363,
+            "heading": -0.3304805961437169,
+            "angularVelocity": 0.47848002388715516,
+            "velocityX": 3.0480119736421156,
+            "velocityY": 1.8936948454141527,
+            "timestamp": 0.4598473410881131
+        },
+        {
+            "x": 1.6456219690029539,
+            "y": 4.9288521484832435,
+            "heading": -0.3304805109628974,
+            "angularVelocity": 0.0000018523716874218418,
+            "velocityX": 3.038072273739655,
+            "velocityY": 2.2540519252400664,
+            "timestamp": 0.5058320751969244
+        },
+        {
+            "x": 1.7741171019961581,
+            "y": 5.046113419715312,
+            "heading": -0.3304804288379279,
+            "angularVelocity": 0.000001785918111046389,
+            "velocityX": 2.794299792821407,
+            "velocityY": 2.5500043330598703,
+            "timestamp": 0.5518168093057357
+        },
+        {
+            "x": 1.8900823069364519,
+            "y": 5.175779429346925,
+            "heading": -0.330480329845049,
+            "angularVelocity": 0.000002152733527656335,
+            "velocityX": 2.5218196253105503,
+            "velocityY": 2.8197620828859327,
+            "timestamp": 0.597801543414547
+        },
+        {
+            "x": 1.992326225217661,
+            "y": 5.31651812358221,
+            "heading": -0.3304801981354216,
+            "angularVelocity": 0.0000028642033047051737,
+            "velocityX": 2.2234317597502637,
+            "velocityY": 3.0605525281991963,
+            "timestamp": 0.6437862775233584
+        },
+        {
+            "x": 2.079798460006714,
+            "y": 5.466883659362793,
+            "heading": -0.3304799560936257,
+            "angularVelocity": 0.000005263524964464214,
+            "velocityX": 1.9022015998192814,
+            "velocityY": 3.2699011681743237,
+            "timestamp": 0.6897710116321697
+        },
+        {
+            "x": 2.16894705974312,
+            "y": 5.683675424803782,
+            "heading": -0.33027266768763375,
+            "angularVelocity": 0.0033444025204108644,
+            "velocityX": 1.4383284005893175,
+            "velocityY": 3.49773024107632,
+            "timestamp": 0.751751710685232
+        },
+        {
+            "x": 2.226835014262279,
+            "y": 5.903744633222095,
+            "heading": -0.30773378760994813,
+            "angularVelocity": 0.3636435280988788,
+            "velocityX": 0.9339674350815139,
+            "velocityY": 3.550608685938014,
+            "timestamp": 0.8137324097382944
+        },
+        {
+            "x": 2.2584550007033997,
+            "y": 6.112330245271792,
+            "heading": -0.23499422209122486,
+            "angularVelocity": 1.1735841419993758,
+            "velocityX": 0.5101585965339779,
+            "velocityY": 3.3653317119112898,
+            "timestamp": 0.8757131087913568
+        },
+        {
+            "x": 2.277604108948135,
+            "y": 6.297377159682904,
+            "heading": -0.09486114062468509,
+            "angularVelocity": 2.260914826833697,
+            "velocityX": 0.30895276331652,
+            "velocityY": 2.985557072415336,
+            "timestamp": 0.9376938078444191
+        },
+        {
+            "x": 2.288511286424087,
+            "y": 6.451498238253786,
+            "heading": 0.04992678964099742,
+            "angularVelocity": 2.336016412815077,
+            "velocityX": 0.1759769999789598,
+            "velocityY": 2.48659793977057,
+            "timestamp": 0.9996745068974815
+        },
+        {
+            "x": 2.294708988562371,
+            "y": 6.5742371130661414,
+            "heading": 0.17908010356662757,
+            "angularVelocity": 2.083766654762434,
+            "velocityX": 0.09999406642681147,
+            "velocityY": 1.9802757420867263,
+            "timestamp": 1.061655205950544
+        },
+        {
+            "x": 2.2980089897573315,
+            "y": 6.66587189325695,
+            "heading": 0.2827001929036045,
+            "angularVelocity": 1.6718122079950113,
+            "velocityX": 0.05324240038246661,
+            "velocityY": 1.478440572481471,
+            "timestamp": 1.1236359050036064
+        },
+        {
+            "x": 2.2995212581337525,
+            "y": 6.72670613551003,
+            "heading": 0.35522073089449935,
+            "angularVelocity": 1.1700503398453694,
+            "velocityX": 0.024399020978011547,
+            "velocityY": 0.9815030030719727,
+            "timestamp": 1.185616604056669
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.393,
-            "angularVelocity": 0.6633836000880191,
-            "velocityX": 0.2735048205846554,
-            "velocityY": 0.3963716771816175,
-            "timestamp": 1.1765044030769745
+            "angularVelocity": 0.609532801060419,
+            "velocityX": 0.007724047543277551,
+            "velocityY": 0.48876287219728576,
+            "timestamp": 1.2475973031097314
         },
         {
             "x": 2.3,
             "y": 6.757,
             "heading": 0.393,
-            "angularVelocity": 0,
-            "velocityX": 2.8187230537991962e-33,
+            "angularVelocity": 2.340826570383063e-30,
+            "velocityX": -1.14239845433479e-30,
             "velocityY": 0,
-            "timestamp": 1.2384256874494468
+            "timestamp": 1.3095780021627939
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.1.traj
new file mode 100644
index 00000000..ab8f454f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.1.traj	
@@ -0,0 +1,193 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": -1.04,
+            "angularVelocity": -1.335008934228872e-34,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7208748853953925,
+            "y": 4.367346013203549,
+            "heading": -1.0266172190247755,
+            "angularVelocity": 0.3311622793955978,
+            "velocityX": 0.2691033980434437,
+            "velocityY": 0.1817800411932904,
+            "timestamp": 0.040411549889224324
+        },
+        {
+            "x": 0.7426520542538281,
+            "y": 4.382068365397769,
+            "heading": -1.0004187382318452,
+            "angularVelocity": 0.6482919082476458,
+            "velocityX": 0.5388847722527592,
+            "velocityY": 0.3643105061443391,
+            "timestamp": 0.08082309977844865
+        },
+        {
+            "x": 0.7753644718100721,
+            "y": 4.404202344534013,
+            "heading": -0.9621111981448724,
+            "angularVelocity": 0.9479354341019116,
+            "velocityX": 0.8094818844096516,
+            "velocityY": 0.5477141855958662,
+            "timestamp": 0.12123464966767297
+        },
+        {
+            "x": 0.8190534716968692,
+            "y": 4.4337878475210974,
+            "heading": -0.9126009199564967,
+            "angularVelocity": 1.2251516787674004,
+            "velocityX": 1.0811018138764033,
+            "velocityY": 0.7321051300478436,
+            "timestamp": 0.1616461995568973
+        },
+        {
+            "x": 0.8737711662497897,
+            "y": 4.470868927169861,
+            "heading": -0.8530717234087362,
+            "angularVelocity": 1.4730738294111758,
+            "velocityX": 1.3540112839748029,
+            "velocityY": 0.9175861789614466,
+            "timestamp": 0.20205774944612162
+        },
+        {
+            "x": 0.9395812969768862,
+            "y": 4.515493926508116,
+            "heading": -0.7850998870787868,
+            "angularVelocity": 1.681990334849146,
+            "velocityX": 1.6284980632392125,
+            "velocityY": 1.104263495475429,
+            "timestamp": 0.24246929933534594
+        },
+        {
+            "x": 1.0165562063250766,
+            "y": 4.567717120220609,
+            "heading": -0.7108767199363526,
+            "angularVelocity": 1.8366820214986515,
+            "velocityX": 1.9047749853493152,
+            "velocityY": 1.292283860818163,
+            "timestamp": 0.28288084922457024
+        },
+        {
+            "x": 1.104761821592339,
+            "y": 4.627598200600785,
+            "heading": -0.633776622573869,
+            "angularVelocity": 1.9078728129415852,
+            "velocityX": 2.1826833048732497,
+            "velocityY": 1.481781335888433,
+            "timestamp": 0.3232923991137946
+        },
+        {
+            "x": 1.2041714779159751,
+            "y": 4.695156297767735,
+            "heading": -0.5601638063396884,
+            "angularVelocity": 1.8215786436295356,
+            "velocityX": 2.4599317916817567,
+            "velocityY": 1.6717521934233843,
+            "timestamp": 0.36370394900301894
+        },
+        {
+            "x": 1.3134031887504467,
+            "y": 4.770142726541239,
+            "heading": -0.5079341517285784,
+            "angularVelocity": 1.2924437383441445,
+            "velocityX": 2.7029824674850675,
+            "velocityY": 1.8555692364943988,
+            "timestamp": 0.4041154988922433
+        },
+        {
+            "x": 1.4312329739367307,
+            "y": 4.852476770750293,
+            "heading": -0.48455184709669846,
+            "angularVelocity": 0.5786045003464412,
+            "velocityX": 2.9157452636505585,
+            "velocityY": 2.0373889255608297,
+            "timestamp": 0.44452704878146765
+        },
+        {
+            "x": 1.55675883861541,
+            "y": 4.939734199489085,
+            "heading": -0.4845517983636149,
+            "angularVelocity": 0.0000012059196871647072,
+            "velocityX": 3.10618783547697,
+            "velocityY": 2.1592200491686473,
+            "timestamp": 0.484938598670692
+        },
+        {
+            "x": 1.6733111473031865,
+            "y": 5.037206282378932,
+            "heading": -0.48207265962656115,
+            "angularVelocity": 0.061347281751120815,
+            "velocityX": 2.884133595649469,
+            "velocityY": 2.4119857604332493,
+            "timestamp": 0.5253501485599164
+        },
+        {
+            "x": 1.7808787822723389,
+            "y": 5.128108024597168,
+            "heading": -0.4506827811882082,
+            "angularVelocity": 0.7767551238296608,
+            "velocityX": 2.661804243193232,
+            "velocityY": 2.2494000469522843,
+            "timestamp": 0.5657616984491407
+        },
+        {
+            "x": 1.9395772431433458,
+            "y": 5.270947819082425,
+            "heading": -0.3258768071798875,
+            "angularVelocity": 1.7295324507132326,
+            "velocityX": 2.1992067297704168,
+            "velocityY": 1.9794409825205124,
+            "timestamp": 0.6379233823521242
+        },
+        {
+            "x": 2.0647143213126102,
+            "y": 5.386258683660538,
+            "heading": -0.20467464704644728,
+            "angularVelocity": 1.6795916278282566,
+            "velocityX": 1.7341208159374817,
+            "velocityY": 1.5979514105178143,
+            "timestamp": 0.7100850662551077
+        },
+        {
+            "x": 2.157719068276139,
+            "y": 5.473035188211263,
+            "heading": -0.10556396576995164,
+            "angularVelocity": 1.373453000483514,
+            "velocityX": 1.2888383686911677,
+            "velocityY": 1.2025288194132213,
+            "timestamp": 0.7822467501580912
+        },
+        {
+            "x": 2.219336350583297,
+            "y": 5.530988274582093,
+            "heading": -0.03600663550492556,
+            "angularVelocity": 0.9639094669484316,
+            "velocityX": 0.853878110577325,
+            "velocityY": 0.8031005269880169,
+            "timestamp": 0.8544084340610747
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 7.006093267694686e-35,
+            "angularVelocity": 0.49897166414982813,
+            "velocityX": 0.4249297931839902,
+            "velocityY": 0.40203781077103196,
+            "timestamp": 0.9265701179640582
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 2.908643221674668e-34,
+            "velocityX": -1.689604381582848e-35,
+            "velocityY": 1.5152673195076234e-35,
+            "timestamp": 0.9987318018670417
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.traj
index f9120604..ab8f454f 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.traj	
@@ -4,145 +4,190 @@
             "x": 0.71,
             "y": 4.36,
             "heading": -1.04,
-            "angularVelocity": 2.3377521162047767e-32,
+            "angularVelocity": -1.335008934228872e-34,
             "velocityX": 0,
             "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 0.737441410859622,
-            "y": 4.3811882786622816,
-            "heading": -1.0071421705955907,
-            "angularVelocity": 0.501398563530153,
-            "velocityX": 0.41874598035403643,
-            "velocityY": 0.32332545020514236,
-            "timestamp": 0.06553235648117978
-        },
-        {
-            "x": 0.7924436512054522,
-            "y": 4.423719236654154,
-            "heading": -0.9443396668116766,
-            "angularVelocity": 0.9583434375956188,
-            "velocityX": 0.8393142456524665,
-            "velocityY": 0.6490069986127681,
-            "timestamp": 0.13106471296235955
-        },
-        {
-            "x": 0.8751754235694627,
-            "y": 4.487775832809834,
-            "heading": -0.8558630826468019,
-            "angularVelocity": 1.350120595621899,
-            "velocityX": 1.2624568504227318,
-            "velocityY": 0.9774804324956131,
-            "timestamp": 0.19659706944353933
-        },
-        {
-            "x": 0.9858651777004729,
-            "y": 4.5735665588886505,
-            "heading": -0.748264717894899,
-            "angularVelocity": 1.641912034443748,
-            "velocityX": 1.6890855155315267,
-            "velocityY": 1.3091353750334698,
-            "timestamp": 0.2621294259247191
-        },
-        {
-            "x": 1.1247487447497322,
-            "y": 4.6813201098081265,
-            "heading": -0.6327126866941822,
-            "angularVelocity": 1.7632821007116781,
-            "velocityX": 2.1193128785036808,
-            "velocityY": 1.644280119095407,
-            "timestamp": 0.3276617824058989
-        },
-        {
-            "x": 1.291489149115039,
-            "y": 4.810947116825512,
-            "heading": -0.5360472598717524,
-            "angularVelocity": 1.4750793655678647,
-            "velocityX": 2.5443981159626317,
-            "velocityY": 1.9780611285451524,
-            "timestamp": 0.39319413888707866
-        },
-        {
-            "x": 1.4799949106081978,
-            "y": 4.9607414161097925,
-            "heading": -0.5171264547525753,
-            "angularVelocity": 0.288724626049585,
-            "velocityX": 2.876529574322522,
-            "velocityY": 2.285806696533185,
-            "timestamp": 0.45872649536825844
-        },
-        {
-            "x": 1.6682619137790426,
-            "y": 5.11061369240764,
-            "heading": -0.49798057816746544,
-            "angularVelocity": 0.29215913501614493,
-            "velocityX": 2.8728862088900566,
-            "velocityY": 2.2869965974882214,
-            "timestamp": 0.5242588518494382
-        },
-        {
-            "x": 1.8349963336194852,
-            "y": 5.239757443274396,
-            "heading": -0.40436368242247644,
-            "angularVelocity": 1.4285598866244509,
-            "velocityX": 2.54430679428301,
-            "velocityY": 1.970686814899508,
-            "timestamp": 0.589791208330618
-        },
-        {
-            "x": 1.9739513715778099,
-            "y": 5.347175023297594,
-            "heading": -0.28864983484418905,
-            "angularVelocity": 1.7657513599639834,
-            "velocityX": 2.120403498662996,
-            "velocityY": 1.6391533250303247,
-            "timestamp": 0.6553235648117979
-        },
-        {
-            "x": 2.0846889840994356,
-            "y": 5.432668431655269,
-            "heading": -0.18116230899252247,
-            "angularVelocity": 1.6402206730126385,
-            "velocityX": 1.6898158172204303,
-            "velocityY": 1.304598414406612,
-            "timestamp": 0.7208559212929777
-        },
-        {
-            "x": 2.167475833751633,
-            "y": 5.496492915332372,
-            "heading": -0.09361421148490373,
-            "angularVelocity": 1.3359522258711838,
-            "velocityX": 1.2632973098712375,
-            "velocityY": 0.973938480228949,
-            "timestamp": 0.7863882777741575
-        },
-        {
-            "x": 2.2225290058379836,
-            "y": 5.538875443360028,
-            "heading": -0.032000086006940494,
-            "angularVelocity": 0.9402092155141033,
-            "velocityX": 0.8400914455466154,
-            "velocityY": 0.6467420111747162,
-            "timestamp": 0.8519206342553374
+            "x": 0.7208748853953925,
+            "y": 4.367346013203549,
+            "heading": -1.0266172190247755,
+            "angularVelocity": 0.3311622793955978,
+            "velocityX": 0.2691033980434437,
+            "velocityY": 0.1817800411932904,
+            "timestamp": 0.040411549889224324
+        },
+        {
+            "x": 0.7426520542538281,
+            "y": 4.382068365397769,
+            "heading": -1.0004187382318452,
+            "angularVelocity": 0.6482919082476458,
+            "velocityX": 0.5388847722527592,
+            "velocityY": 0.3643105061443391,
+            "timestamp": 0.08082309977844865
+        },
+        {
+            "x": 0.7753644718100721,
+            "y": 4.404202344534013,
+            "heading": -0.9621111981448724,
+            "angularVelocity": 0.9479354341019116,
+            "velocityX": 0.8094818844096516,
+            "velocityY": 0.5477141855958662,
+            "timestamp": 0.12123464966767297
+        },
+        {
+            "x": 0.8190534716968692,
+            "y": 4.4337878475210974,
+            "heading": -0.9126009199564967,
+            "angularVelocity": 1.2251516787674004,
+            "velocityX": 1.0811018138764033,
+            "velocityY": 0.7321051300478436,
+            "timestamp": 0.1616461995568973
+        },
+        {
+            "x": 0.8737711662497897,
+            "y": 4.470868927169861,
+            "heading": -0.8530717234087362,
+            "angularVelocity": 1.4730738294111758,
+            "velocityX": 1.3540112839748029,
+            "velocityY": 0.9175861789614466,
+            "timestamp": 0.20205774944612162
+        },
+        {
+            "x": 0.9395812969768862,
+            "y": 4.515493926508116,
+            "heading": -0.7850998870787868,
+            "angularVelocity": 1.681990334849146,
+            "velocityX": 1.6284980632392125,
+            "velocityY": 1.104263495475429,
+            "timestamp": 0.24246929933534594
+        },
+        {
+            "x": 1.0165562063250766,
+            "y": 4.567717120220609,
+            "heading": -0.7108767199363526,
+            "angularVelocity": 1.8366820214986515,
+            "velocityX": 1.9047749853493152,
+            "velocityY": 1.292283860818163,
+            "timestamp": 0.28288084922457024
+        },
+        {
+            "x": 1.104761821592339,
+            "y": 4.627598200600785,
+            "heading": -0.633776622573869,
+            "angularVelocity": 1.9078728129415852,
+            "velocityX": 2.1826833048732497,
+            "velocityY": 1.481781335888433,
+            "timestamp": 0.3232923991137946
+        },
+        {
+            "x": 1.2041714779159751,
+            "y": 4.695156297767735,
+            "heading": -0.5601638063396884,
+            "angularVelocity": 1.8215786436295356,
+            "velocityX": 2.4599317916817567,
+            "velocityY": 1.6717521934233843,
+            "timestamp": 0.36370394900301894
+        },
+        {
+            "x": 1.3134031887504467,
+            "y": 4.770142726541239,
+            "heading": -0.5079341517285784,
+            "angularVelocity": 1.2924437383441445,
+            "velocityX": 2.7029824674850675,
+            "velocityY": 1.8555692364943988,
+            "timestamp": 0.4041154988922433
+        },
+        {
+            "x": 1.4312329739367307,
+            "y": 4.852476770750293,
+            "heading": -0.48455184709669846,
+            "angularVelocity": 0.5786045003464412,
+            "velocityX": 2.9157452636505585,
+            "velocityY": 2.0373889255608297,
+            "timestamp": 0.44452704878146765
+        },
+        {
+            "x": 1.55675883861541,
+            "y": 4.939734199489085,
+            "heading": -0.4845517983636149,
+            "angularVelocity": 0.0000012059196871647072,
+            "velocityX": 3.10618783547697,
+            "velocityY": 2.1592200491686473,
+            "timestamp": 0.484938598670692
+        },
+        {
+            "x": 1.6733111473031865,
+            "y": 5.037206282378932,
+            "heading": -0.48207265962656115,
+            "angularVelocity": 0.061347281751120815,
+            "velocityX": 2.884133595649469,
+            "velocityY": 2.4119857604332493,
+            "timestamp": 0.5253501485599164
+        },
+        {
+            "x": 1.7808787822723389,
+            "y": 5.128108024597168,
+            "heading": -0.4506827811882082,
+            "angularVelocity": 0.7767551238296608,
+            "velocityX": 2.661804243193232,
+            "velocityY": 2.2494000469522843,
+            "timestamp": 0.5657616984491407
+        },
+        {
+            "x": 1.9395772431433458,
+            "y": 5.270947819082425,
+            "heading": -0.3258768071798875,
+            "angularVelocity": 1.7295324507132326,
+            "velocityX": 2.1992067297704168,
+            "velocityY": 1.9794409825205124,
+            "timestamp": 0.6379233823521242
+        },
+        {
+            "x": 2.0647143213126102,
+            "y": 5.386258683660538,
+            "heading": -0.20467464704644728,
+            "angularVelocity": 1.6795916278282566,
+            "velocityX": 1.7341208159374817,
+            "velocityY": 1.5979514105178143,
+            "timestamp": 0.7100850662551077
+        },
+        {
+            "x": 2.157719068276139,
+            "y": 5.473035188211263,
+            "heading": -0.10556396576995164,
+            "angularVelocity": 1.373453000483514,
+            "velocityX": 1.2888383686911677,
+            "velocityY": 1.2025288194132213,
+            "timestamp": 0.7822467501580912
+        },
+        {
+            "x": 2.219336350583297,
+            "y": 5.530988274582093,
+            "heading": -0.03600663550492556,
+            "angularVelocity": 0.9639094669484316,
+            "velocityX": 0.853878110577325,
+            "velocityY": 0.8031005269880169,
+            "timestamp": 0.8544084340610747
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 2.4308347188047162e-30,
-            "angularVelocity": 0.48830971027463277,
-            "velocityX": 0.41919741082263007,
-            "velocityY": 0.3223530752482305,
-            "timestamp": 0.9174529907365172
+            "heading": 7.006093267694686e-35,
+            "angularVelocity": 0.49897166414982813,
+            "velocityX": 0.4249297931839902,
+            "velocityY": 0.40203781077103196,
+            "timestamp": 0.9265701179640582
         },
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 1.7485008040489027e-29,
-            "angularVelocity": 1.1123032826758857e-30,
-            "velocityX": 0,
-            "velocityY": 1.931278949788811e-31,
-            "timestamp": 0.982985347217697
+            "heading": 0,
+            "angularVelocity": 2.908643221674668e-34,
+            "velocityX": -1.689604381582848e-35,
+            "velocityY": 1.5152673195076234e-35,
+            "timestamp": 0.9987318018670417
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.1.traj
new file mode 100644
index 00000000..ff6d2947
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.1.traj	
@@ -0,0 +1,121 @@
+{
+    "samples": [
+        {
+            "x": 0.71,
+            "y": 4.36,
+            "heading": -1.04,
+            "angularVelocity": -2.204776300650663e-32,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 0.7541176231747116,
+            "y": 4.360036687345491,
+            "heading": -1.0162545684059647,
+            "angularVelocity": 0.32461244675544165,
+            "velocityX": 0.6031109414489596,
+            "velocityY": 0.0005015351663374791,
+            "timestamp": 0.07315009584923313
+        },
+        {
+            "x": 0.8424088865359771,
+            "y": 4.360100324606402,
+            "heading": -0.9701479863262087,
+            "angularVelocity": 0.630301048063541,
+            "velocityX": 1.2069876646948012,
+            "velocityY": 0.0008699545799604022,
+            "timestamp": 0.14630019169846625
+        },
+        {
+            "x": 0.974955221752511,
+            "y": 4.3601789275184695,
+            "heading": -0.9038423136459668,
+            "angularVelocity": 0.9064331619859869,
+            "velocityX": 1.8119776013652948,
+            "velocityY": 0.0010745428444254071,
+            "timestamp": 0.21945028754769938
+        },
+        {
+            "x": 1.151884261990258,
+            "y": 4.360259672825357,
+            "heading": -0.8212724833638784,
+            "angularVelocity": 1.128772687492223,
+            "velocityX": 2.4187123500481573,
+            "velocityY": 0.0011038305000959348,
+            "timestamp": 0.2926003833969325
+        },
+        {
+            "x": 1.3733948258568207,
+            "y": 4.360324542882412,
+            "heading": -0.7323749312717828,
+            "angularVelocity": 1.215275948172809,
+            "velocityX": 3.028165052894887,
+            "velocityY": 0.0008868075468144582,
+            "timestamp": 0.36575047924616566
+        },
+        {
+            "x": 1.6366160230983884,
+            "y": 4.359789775146019,
+            "heading": -0.6992109641912426,
+            "angularVelocity": 0.4533687440268879,
+            "velocityX": 3.5983711871554758,
+            "velocityY": -0.007310554144235059,
+            "timestamp": 0.4389005750953988
+        },
+        {
+            "x": 1.8581232786849116,
+            "y": 4.3598960912826,
+            "heading": -0.6098314600860953,
+            "angularVelocity": 1.2218644838062247,
+            "velocityX": 3.0281198269795637,
+            "velocityY": 0.001453397091854858,
+            "timestamp": 0.512050670944632
+        },
+        {
+            "x": 2.035050245808528,
+            "y": 4.359956526837273,
+            "heading": -0.5268891037054299,
+            "angularVelocity": 1.1338653137464598,
+            "velocityX": 2.4186840094956525,
+            "velocityY": 0.000826185584123381,
+            "timestamp": 0.5852007667938651
+        },
+        {
+            "x": 2.1675937895751547,
+            "y": 4.359986275638159,
+            "heading": -0.46022423203591045,
+            "angularVelocity": 0.9113435996982586,
+            "velocityX": 1.8119394407877087,
+            "velocityY": 0.00040668163915013244,
+            "timestamp": 0.6583508626430983
+        },
+        {
+            "x": 2.255882919836075,
+            "y": 4.359997704013322,
+            "heading": -0.41385948514831483,
+            "angularVelocity": 0.6338302957681125,
+            "velocityX": 1.2069585040994306,
+            "velocityY": 0.0001562318549097459,
+            "timestamp": 0.7315009584923314
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 0.32617161838698483,
+            "velocityX": 0.6031035182080025,
+            "velocityY": 0.00003138733655167869,
+            "timestamp": 0.8046510543415646
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -2.2755735462869925e-32,
+            "velocityX": -9.780003655586715e-35,
+            "velocityY": 0,
+            "timestamp": 0.8778011501907977
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.1.traj
new file mode 100644
index 00000000..bfcd0c92
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.1.traj	
@@ -0,0 +1,256 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 4.0882515102287785e-31,
+            "velocityX": 3.918479950533569e-32,
+            "velocityY": 1.7984537919855076e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 2.31173856270728,
+            "y": 6.7441134449795,
+            "heading": 0.37806624882590184,
+            "angularVelocity": -0.25868952140989293,
+            "velocityX": 0.2544583948907238,
+            "velocityY": -0.279343577911277,
+            "timestamp": 0.04613155998378907
+        },
+        {
+            "x": 2.335747257207826,
+            "y": 6.718800139650514,
+            "heading": 0.35485995839909595,
+            "angularVelocity": -0.5030458635034395,
+            "velocityX": 0.5204396840033778,
+            "velocityY": -0.5487199075401175,
+            "timestamp": 0.09226311996757813
+        },
+        {
+            "x": 2.3726756647692695,
+            "y": 6.681679707204912,
+            "heading": 0.3212123448310367,
+            "angularVelocity": -0.7293838226993238,
+            "velocityX": 0.8005020331941938,
+            "velocityY": -0.8046645823086491,
+            "timestamp": 0.1383946799513672
+        },
+        {
+            "x": 2.4233218216100387,
+            "y": 6.6336167386650216,
+            "heading": 0.2781690598764637,
+            "angularVelocity": -0.9330550488580632,
+            "velocityX": 1.097863520300784,
+            "velocityY": -1.0418674017696439,
+            "timestamp": 0.18452623993515627
+        },
+        {
+            "x": 2.488661485197386,
+            "y": 6.57586429791715,
+            "heading": 0.22709762602154188,
+            "angularVelocity": -1.1070823070554872,
+            "velocityX": 1.416376632620007,
+            "velocityY": -1.2519073876575257,
+            "timestamp": 0.23065779991894533
+        },
+        {
+            "x": 2.5698490572295953,
+            "y": 6.510313323679299,
+            "heading": 0.16995873595290442,
+            "angularVelocity": -1.2386073674663585,
+            "velocityX": 1.7599138650576658,
+            "velocityY": -1.4209572418727212,
+            "timestamp": 0.2767893599027344
+        },
+        {
+            "x": 2.6680837365477243,
+            "y": 6.439909324941273,
+            "heading": 0.10980534967726638,
+            "angularVelocity": -1.3039530052045993,
+            "velocityX": 2.1294462912732417,
+            "velocityY": -1.5261569034900655,
+            "timestamp": 0.3229209198865235
+        },
+        {
+            "x": 2.784053925984014,
+            "y": 6.36914274527536,
+            "heading": 0.05120415857101511,
+            "angularVelocity": -1.2703058627725585,
+            "velocityX": 2.5139013178189233,
+            "velocityY": -1.5340166187915734,
+            "timestamp": 0.36905247987031253
+        },
+        {
+            "x": 2.916773557662964,
+            "y": 6.303858757019043,
+            "heading": 0,
+            "angularVelocity": -1.1099593984900726,
+            "velocityX": 2.8769812190523862,
+            "velocityY": -1.4151697510176988,
+            "timestamp": 0.41518403985410157
+        },
+        {
+            "x": 3.221120615978503,
+            "y": 6.218848325437687,
+            "heading": -0.053482212757092314,
+            "angularVelocity": -0.6045874919246804,
+            "velocityX": 3.4404789027216576,
+            "velocityY": -0.9609969552052925,
+            "timestamp": 0.5036447067905502
+        },
+        {
+            "x": 3.554544204530565,
+            "y": 6.190324835643033,
+            "heading": -0.053482256222138806,
+            "angularVelocity": -4.913488445928212e-7,
+            "velocityX": 3.769173352396249,
+            "velocityY": -0.32244262656470185,
+            "timestamp": 0.5921053737269988
+        },
+        {
+            "x": 3.8871649424892416,
+            "y": 6.227044557920147,
+            "heading": -0.05348225420791585,
+            "angularVelocity": 2.276970118304524e-8,
+            "velocityX": 3.760097560621335,
+            "velocityY": 0.41509660223897277,
+            "timestamp": 0.6805660406634474
+        },
+        {
+            "x": 4.206338511891951,
+            "y": 6.327608602444822,
+            "heading": -0.05348225236183454,
+            "angularVelocity": 2.0868950806255528e-8,
+            "velocityX": 3.608084592353424,
+            "velocityY": 1.1368221381026216,
+            "timestamp": 0.769026707599896
+        },
+        {
+            "x": 4.5243112033895745,
+            "y": 6.4319075091933104,
+            "heading": -0.05348225052024108,
+            "angularVelocity": 2.0818218151781936e-8,
+            "velocityX": 3.5945093170737707,
+            "velocityY": 1.1790427357213897,
+            "timestamp": 0.8574873745363446
+        },
+        {
+            "x": 4.842283928689835,
+            "y": 6.536206312888755,
+            "heading": -0.053482248678638385,
+            "angularVelocity": 2.0818322537631325e-8,
+            "velocityX": 3.59450969919429,
+            "velocityY": 1.1790415707624449,
+            "timestamp": 0.9459480414727932
+        },
+        {
+            "x": 5.160256832233298,
+            "y": 6.640504573177801,
+            "heading": -0.05348224683713029,
+            "angularVelocity": 2.0817253121924564e-8,
+            "velocityX": 3.5945117141373055,
+            "velocityY": 1.1790354278469999,
+            "timestamp": 1.034408708409242
+        },
+        {
+            "x": 5.478229837776281,
+            "y": 6.744802522501547,
+            "heading": -0.05348224499575697,
+            "angularVelocity": 2.081572963279097e-8,
+            "velocityX": 3.594512867186711,
+            "velocityY": 1.1790319125524373,
+            "timestamp": 1.1228693753456906
+        },
+        {
+            "x": 5.79620288183899,
+            "y": 6.849100354390164,
+            "heading": -0.05348224315440185,
+            "angularVelocity": 2.0815523766741823e-8,
+            "velocityX": 3.5945133026313907,
+            "velocityY": 1.1790305850118254,
+            "timestamp": 1.2113300422821394
+        },
+        {
+            "x": 6.114175956134068,
+            "y": 6.9533980941092235,
+            "heading": -0.053482241313040024,
+            "angularVelocity": 2.081559959710574e-8,
+            "velocityX": 3.5945136443919665,
+            "velocityY": 1.179029543084821,
+            "timestamp": 1.299790709218588
+        },
+        {
+            "x": 6.432149055252676,
+            "y": 7.057695758148605,
+            "heading": -0.05348223947166161,
+            "angularVelocity": 2.0815787162382163e-8,
+            "velocityX": 3.594513925008545,
+            "velocityY": 1.1790286875669842,
+            "timestamp": 1.3882513761550368
+        },
+        {
+            "x": 6.750122174049358,
+            "y": 7.16199336219523,
+            "heading": -0.05348223763026628,
+            "angularVelocity": 2.081597839391937e-8,
+            "velocityX": 3.594514147458541,
+            "velocityY": 1.1790280093813261,
+            "timestamp": 1.4767120430914855
+        },
+        {
+            "x": 7.060081426834384,
+            "y": 7.263662245896702,
+            "heading": -0.036655668946107355,
+            "angularVelocity": 0.1902152591303347,
+            "velocityX": 3.503921726112515,
+            "velocityY": 1.1493117474969148,
+            "timestamp": 1.5651727100279342
+        },
+        {
+            "x": 7.308048847978653,
+            "y": 7.344997351864717,
+            "heading": -0.023193676123292754,
+            "angularVelocity": 0.15218054858760893,
+            "velocityX": 2.8031375947279678,
+            "velocityY": 0.9194493867703608,
+            "timestamp": 1.653633376964383
+        },
+        {
+            "x": 7.4940244209186755,
+            "y": 7.405998678341617,
+            "heading": -0.013096928746230797,
+            "angularVelocity": 0.11413826875525969,
+            "velocityX": 2.102353276102148,
+            "velocityY": 0.6895870061744361,
+            "timestamp": 1.7420940439008317
+        },
+        {
+            "x": 7.618008139456229,
+            "y": 7.4466662269505,
+            "heading": -0.006365663946800556,
+            "angularVelocity": 0.07609330827525898,
+            "velocityX": 1.4015688874087386,
+            "velocityY": 0.4597246439267693,
+            "timestamp": 1.8305547108372804
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -0.003,
+            "angularVelocity": 0.038047010760369614,
+            "velocityX": 0.7007844581174931,
+            "velocityY": 0.2298623077769412,
+            "timestamp": 1.919015377773729
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -0.003,
+            "angularVelocity": -2.311552385941746e-29,
+            "velocityX": 0,
+            "velocityY": -6.724711823689692e-32,
+            "timestamp": 2.007476044710178
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.traj
index b6c78aaa..bfcd0c92 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.traj	
@@ -4,199 +4,253 @@
             "x": 2.3,
             "y": 6.757,
             "heading": 0.39,
-            "angularVelocity": 6.154725095706714e-32,
-            "velocityX": 1.2115062621827067e-33,
-            "velocityY": 6.540799795634554e-33,
+            "angularVelocity": 4.0882515102287785e-31,
+            "velocityX": 3.918479950533569e-32,
+            "velocityY": 1.7984537919855076e-32,
             "timestamp": 0
         },
         {
-            "x": 2.3666999539229265,
-            "y": 6.765820977804648,
-            "heading": 0.36628259128993745,
-            "angularVelocity": -0.26339586361920003,
-            "velocityX": 0.7407424724032508,
-            "velocityY": 0.09796218023747214,
-            "timestamp": 0.09004472729439511
-        },
-        {
-            "x": 2.5001846245776895,
-            "y": 6.783480891306791,
-            "heading": 0.3222458902940237,
-            "angularVelocity": -0.4890536327789505,
-            "velocityX": 1.4824262859761226,
-            "velocityY": 0.1961237935054848,
-            "timestamp": 0.18008945458879022
-        },
-        {
-            "x": 2.70056880915549,
-            "y": 6.810010034997764,
-            "heading": 0.26375227569196347,
-            "angularVelocity": -0.6496062163731077,
-            "velocityX": 2.225384990313294,
-            "velocityY": 0.2946218450330504,
-            "timestamp": 0.27013418188318533
-        },
-        {
-            "x": 2.967965472322839,
-            "y": 6.8454603445891635,
-            "heading": 0.20325713210858826,
-            "angularVelocity": -0.6718343805472418,
-            "velocityX": 2.9695982341432847,
-            "velocityY": 0.3936966733820705,
-            "timestamp": 0.36017890917758044
-        },
-        {
-            "x": 3.3011740475809854,
-            "y": 6.889827568786931,
-            "heading": 0.1886164627156386,
-            "angularVelocity": -0.16259330038374148,
-            "velocityX": 3.700478476310374,
-            "velocityY": 0.4927242885939573,
-            "timestamp": 0.45022363647197555
-        },
-        {
-            "x": 3.6388963576772766,
-            "y": 6.934269203744266,
-            "heading": 0.18861644342798295,
-            "angularVelocity": -2.1420083336262225e-7,
-            "velocityX": 3.7506061736644574,
-            "velocityY": 0.4935506641275844,
-            "timestamp": 0.5402683637663707
-        },
-        {
-            "x": 3.9766186774172247,
-            "y": 6.978710765417191,
-            "heading": 0.1886164241403376,
-            "angularVelocity": -2.1420071941558326e-7,
-            "velocityX": 3.750606280762983,
-            "velocityY": 0.4935498502608198,
-            "timestamp": 0.6303130910607657
-        },
-        {
-            "x": 4.314341007224031,
-            "y": 7.023152250589549,
-            "heading": 0.18861640485269274,
-            "angularVelocity": -2.1420071403103001e-7,
-            "velocityX": 3.750606392561398,
-            "velocityY": 0.4935490006767261,
-            "timestamp": 0.7203578183551609
-        },
-        {
-            "x": 4.652063347871557,
-            "y": 7.067593653380398,
-            "heading": 0.1886163855650432,
-            "angularVelocity": -2.1420076555553473e-7,
-            "velocityX": 3.750606512954019,
-            "velocityY": 0.49354808578132814,
-            "timestamp": 0.810402545649556
-        },
-        {
-            "x": 4.989785700043158,
-            "y": 7.112034968596579,
-            "heading": 0.18861636627738598,
-            "angularVelocity": -2.1420085095836123e-7,
-            "velocityX": 3.7506066409356835,
-            "velocityY": 0.4935471132127838,
-            "timestamp": 0.9004472729439512
-        },
-        {
-            "x": 5.327508064058316,
-            "y": 7.15647619381006,
-            "heading": 0.18861634698972354,
-            "angularVelocity": -2.1420090940402864e-7,
-            "velocityX": 3.7506067724653955,
-            "velocityY": 0.4935461136795145,
-            "timestamp": 0.9904920002383464
-        },
-        {
-            "x": 5.6652304398823485,
-            "y": 7.200917329284216,
-            "heading": 0.18861632770206457,
-            "angularVelocity": -2.1420087020950116e-7,
-            "velocityX": 3.750606903609938,
-            "velocityY": 0.4935451170711973,
-            "timestamp": 1.0805367275327415
-        },
-        {
-            "x": 6.002952827157885,
-            "y": 7.245358377734637,
-            "heading": 0.18861630841441618,
-            "angularVelocity": -2.1420075320811155e-7,
-            "velocityX": 3.750607030785673,
-            "velocityY": 0.49354415062109597,
-            "timestamp": 1.1705814548271367
-        },
-        {
-            "x": 6.340675225087303,
-            "y": 7.289799345222555,
-            "heading": 0.1886162891267749,
-            "angularVelocity": -2.1420067405327174e-7,
-            "velocityX": 3.750607149103328,
-            "velocityY": 0.4935432514845822,
-            "timestamp": 1.2606261821215319
-        },
-        {
-            "x": 6.678397632133336,
-            "y": 7.334240243430023,
-            "heading": 0.18861626983912938,
-            "angularVelocity": -2.1420072105835628e-7,
-            "velocityX": 3.7506072503487453,
-            "velocityY": 0.4935424820841753,
-            "timestamp": 1.350670909415927
-        },
-        {
-            "x": 7.011808183476814,
-            "y": 7.378579327724673,
-            "heading": 0.17443906284640512,
-            "angularVelocity": -0.15744627607536488,
-            "velocityX": 3.7027215403008924,
-            "velocityY": 0.49241177831197624,
-            "timestamp": 1.4407156367103222
-        },
-        {
-            "x": 7.279274281127278,
-            "y": 7.414016049929063,
-            "heading": 0.11701095642225132,
-            "angularVelocity": -0.637773117313093,
-            "velocityX": 2.9703693451811146,
-            "velocityY": 0.3935457774060549,
-            "timestamp": 1.5307603640047174
-        },
-        {
-            "x": 7.479727163444043,
-            "y": 7.440534681549159,
-            "heading": 0.061439635089441606,
-            "angularVelocity": -0.6171524197205129,
-            "velocityX": 2.226147919371209,
-            "velocityY": 0.294505102263186,
-            "timestamp": 1.6208050912991125
-        },
-        {
-            "x": 7.613267962945412,
-            "y": 7.458185352165826,
-            "heading": 0.019571051398075176,
-            "angularVelocity": -0.46497540666073534,
-            "velocityX": 1.4830496300440337,
-            "velocityY": 0.19602114579078314,
-            "timestamp": 1.7108498185935077
+            "x": 2.31173856270728,
+            "y": 6.7441134449795,
+            "heading": 0.37806624882590184,
+            "angularVelocity": -0.25868952140989293,
+            "velocityX": 0.2544583948907238,
+            "velocityY": -0.279343577911277,
+            "timestamp": 0.04613155998378907
+        },
+        {
+            "x": 2.335747257207826,
+            "y": 6.718800139650514,
+            "heading": 0.35485995839909595,
+            "angularVelocity": -0.5030458635034395,
+            "velocityX": 0.5204396840033778,
+            "velocityY": -0.5487199075401175,
+            "timestamp": 0.09226311996757813
+        },
+        {
+            "x": 2.3726756647692695,
+            "y": 6.681679707204912,
+            "heading": 0.3212123448310367,
+            "angularVelocity": -0.7293838226993238,
+            "velocityX": 0.8005020331941938,
+            "velocityY": -0.8046645823086491,
+            "timestamp": 0.1383946799513672
+        },
+        {
+            "x": 2.4233218216100387,
+            "y": 6.6336167386650216,
+            "heading": 0.2781690598764637,
+            "angularVelocity": -0.9330550488580632,
+            "velocityX": 1.097863520300784,
+            "velocityY": -1.0418674017696439,
+            "timestamp": 0.18452623993515627
+        },
+        {
+            "x": 2.488661485197386,
+            "y": 6.57586429791715,
+            "heading": 0.22709762602154188,
+            "angularVelocity": -1.1070823070554872,
+            "velocityX": 1.416376632620007,
+            "velocityY": -1.2519073876575257,
+            "timestamp": 0.23065779991894533
+        },
+        {
+            "x": 2.5698490572295953,
+            "y": 6.510313323679299,
+            "heading": 0.16995873595290442,
+            "angularVelocity": -1.2386073674663585,
+            "velocityX": 1.7599138650576658,
+            "velocityY": -1.4209572418727212,
+            "timestamp": 0.2767893599027344
+        },
+        {
+            "x": 2.6680837365477243,
+            "y": 6.439909324941273,
+            "heading": 0.10980534967726638,
+            "angularVelocity": -1.3039530052045993,
+            "velocityX": 2.1294462912732417,
+            "velocityY": -1.5261569034900655,
+            "timestamp": 0.3229209198865235
+        },
+        {
+            "x": 2.784053925984014,
+            "y": 6.36914274527536,
+            "heading": 0.05120415857101511,
+            "angularVelocity": -1.2703058627725585,
+            "velocityX": 2.5139013178189233,
+            "velocityY": -1.5340166187915734,
+            "timestamp": 0.36905247987031253
+        },
+        {
+            "x": 2.916773557662964,
+            "y": 6.303858757019043,
+            "heading": 0,
+            "angularVelocity": -1.1099593984900726,
+            "velocityX": 2.8769812190523862,
+            "velocityY": -1.4151697510176988,
+            "timestamp": 0.41518403985410157
+        },
+        {
+            "x": 3.221120615978503,
+            "y": 6.218848325437687,
+            "heading": -0.053482212757092314,
+            "angularVelocity": -0.6045874919246804,
+            "velocityX": 3.4404789027216576,
+            "velocityY": -0.9609969552052925,
+            "timestamp": 0.5036447067905502
+        },
+        {
+            "x": 3.554544204530565,
+            "y": 6.190324835643033,
+            "heading": -0.053482256222138806,
+            "angularVelocity": -4.913488445928212e-7,
+            "velocityX": 3.769173352396249,
+            "velocityY": -0.32244262656470185,
+            "timestamp": 0.5921053737269988
+        },
+        {
+            "x": 3.8871649424892416,
+            "y": 6.227044557920147,
+            "heading": -0.05348225420791585,
+            "angularVelocity": 2.276970118304524e-8,
+            "velocityX": 3.760097560621335,
+            "velocityY": 0.41509660223897277,
+            "timestamp": 0.6805660406634474
+        },
+        {
+            "x": 4.206338511891951,
+            "y": 6.327608602444822,
+            "heading": -0.05348225236183454,
+            "angularVelocity": 2.0868950806255528e-8,
+            "velocityX": 3.608084592353424,
+            "velocityY": 1.1368221381026216,
+            "timestamp": 0.769026707599896
+        },
+        {
+            "x": 4.5243112033895745,
+            "y": 6.4319075091933104,
+            "heading": -0.05348225052024108,
+            "angularVelocity": 2.0818218151781936e-8,
+            "velocityX": 3.5945093170737707,
+            "velocityY": 1.1790427357213897,
+            "timestamp": 0.8574873745363446
+        },
+        {
+            "x": 4.842283928689835,
+            "y": 6.536206312888755,
+            "heading": -0.053482248678638385,
+            "angularVelocity": 2.0818322537631325e-8,
+            "velocityX": 3.59450969919429,
+            "velocityY": 1.1790415707624449,
+            "timestamp": 0.9459480414727932
+        },
+        {
+            "x": 5.160256832233298,
+            "y": 6.640504573177801,
+            "heading": -0.05348224683713029,
+            "angularVelocity": 2.0817253121924564e-8,
+            "velocityX": 3.5945117141373055,
+            "velocityY": 1.1790354278469999,
+            "timestamp": 1.034408708409242
+        },
+        {
+            "x": 5.478229837776281,
+            "y": 6.744802522501547,
+            "heading": -0.05348224499575697,
+            "angularVelocity": 2.081572963279097e-8,
+            "velocityX": 3.594512867186711,
+            "velocityY": 1.1790319125524373,
+            "timestamp": 1.1228693753456906
+        },
+        {
+            "x": 5.79620288183899,
+            "y": 6.849100354390164,
+            "heading": -0.05348224315440185,
+            "angularVelocity": 2.0815523766741823e-8,
+            "velocityX": 3.5945133026313907,
+            "velocityY": 1.1790305850118254,
+            "timestamp": 1.2113300422821394
+        },
+        {
+            "x": 6.114175956134068,
+            "y": 6.9533980941092235,
+            "heading": -0.053482241313040024,
+            "angularVelocity": 2.081559959710574e-8,
+            "velocityX": 3.5945136443919665,
+            "velocityY": 1.179029543084821,
+            "timestamp": 1.299790709218588
+        },
+        {
+            "x": 6.432149055252676,
+            "y": 7.057695758148605,
+            "heading": -0.05348223947166161,
+            "angularVelocity": 2.0815787162382163e-8,
+            "velocityX": 3.594513925008545,
+            "velocityY": 1.1790286875669842,
+            "timestamp": 1.3882513761550368
+        },
+        {
+            "x": 6.750122174049358,
+            "y": 7.16199336219523,
+            "heading": -0.05348223763026628,
+            "angularVelocity": 2.081597839391937e-8,
+            "velocityX": 3.594514147458541,
+            "velocityY": 1.1790280093813261,
+            "timestamp": 1.4767120430914855
+        },
+        {
+            "x": 7.060081426834384,
+            "y": 7.263662245896702,
+            "heading": -0.036655668946107355,
+            "angularVelocity": 0.1902152591303347,
+            "velocityX": 3.503921726112515,
+            "velocityY": 1.1493117474969148,
+            "timestamp": 1.5651727100279342
+        },
+        {
+            "x": 7.308048847978653,
+            "y": 7.344997351864717,
+            "heading": -0.023193676123292754,
+            "angularVelocity": 0.15218054858760893,
+            "velocityX": 2.8031375947279678,
+            "velocityY": 0.9194493867703608,
+            "timestamp": 1.653633376964383
+        },
+        {
+            "x": 7.4940244209186755,
+            "y": 7.405998678341617,
+            "heading": -0.013096928746230797,
+            "angularVelocity": 0.11413826875525969,
+            "velocityX": 2.102353276102148,
+            "velocityY": 0.6895870061744361,
+            "timestamp": 1.7420940439008317
+        },
+        {
+            "x": 7.618008139456229,
+            "y": 7.4466662269505,
+            "heading": -0.006365663946800556,
+            "angularVelocity": 0.07609330827525898,
+            "velocityX": 1.4015688874087386,
+            "velocityY": 0.4597246439267693,
+            "timestamp": 1.8305547108372804
         },
         {
             "x": 7.68,
             "y": 7.467,
             "heading": -0.003,
-            "angularVelocity": -0.2506648870652985,
-            "velocityX": 0.7410987745724689,
-            "velocityY": 0.09789188216823574,
-            "timestamp": 1.8008945458879029
+            "angularVelocity": 0.038047010760369614,
+            "velocityX": 0.7007844581174931,
+            "velocityY": 0.2298623077769412,
+            "timestamp": 1.919015377773729
         },
         {
             "x": 7.68,
             "y": 7.467,
             "heading": -0.003,
-            "angularVelocity": 5.335131961970383e-32,
+            "angularVelocity": -2.311552385941746e-29,
             "velocityX": 0,
-            "velocityY": -3.2330177098423496e-35,
-            "timestamp": 1.890939273182298
+            "velocityY": -6.724711823689692e-32,
+            "timestamp": 2.007476044710178
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.1.traj
new file mode 100644
index 00000000..3bdf526e
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.1.traj	
@@ -0,0 +1,310 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3119316870686255,
+            "y": 6.7398994499669,
+            "heading": 0.3779608263213178,
+            "angularVelocity": -0.23922354190742906,
+            "velocityX": 0.23708773688854562,
+            "velocityY": -0.3397952597632521,
+            "timestamp": 0.05032604058401226
+        },
+        {
+            "x": 2.3368398722232935,
+            "y": 6.706448700500007,
+            "heading": 0.3544233266085098,
+            "angularVelocity": -0.4677002092687843,
+            "velocityX": 0.49493631657919795,
+            "velocityY": -0.6646807314605355,
+            "timestamp": 0.10065208116802452
+        },
+        {
+            "x": 2.3760366485894098,
+            "y": 6.657739626207859,
+            "heading": 0.32015686455898285,
+            "angularVelocity": -0.6808892901544442,
+            "velocityX": 0.7788567491352839,
+            "velocityY": -0.9678701866250936,
+            "timestamp": 0.1509781217520368
+        },
+        {
+            "x": 2.431161593733442,
+            "y": 6.595431091068774,
+            "heading": 0.27624213154004224,
+            "angularVelocity": -0.8726045703046355,
+            "velocityX": 1.0953562907856245,
+            "velocityY": -1.2380973034243006,
+            "timestamp": 0.20130416233604903
+        },
+        {
+            "x": 2.504176998220011,
+            "y": 6.522134358575033,
+            "heading": 0.22423189981090158,
+            "angularVelocity": -1.0334656000271036,
+            "velocityX": 1.450847387143028,
+            "velocityY": -1.456437495244302,
+            "timestamp": 0.2516302029200613
+        },
+        {
+            "x": 2.597103761438428,
+            "y": 6.441982348073212,
+            "heading": 0.16656086207364196,
+            "angularVelocity": -1.1459482420634122,
+            "velocityX": 1.8464946206783215,
+            "velocityY": -1.5926548079620442,
+            "timestamp": 0.3019562435040735
+        },
+        {
+            "x": 2.711127221877276,
+            "y": 6.361040590595544,
+            "heading": 0.10700626596614256,
+            "angularVelocity": -1.1833753543173746,
+            "velocityX": 2.265695038108593,
+            "velocityY": -1.6083474189181441,
+            "timestamp": 0.35228228408808576
+        },
+        {
+            "x": 2.8452820247148356,
+            "y": 6.2864334030632,
+            "heading": 0.05015861793730577,
+            "angularVelocity": -1.1295871355889644,
+            "velocityX": 2.6657134414062895,
+            "velocityY": -1.482476798622636,
+            "timestamp": 0.402608324672098
+        },
+        {
+            "x": 2.996485471725464,
+            "y": 6.224146842956543,
+            "heading": 0,
+            "angularVelocity": -0.9966732402395141,
+            "velocityX": 3.004477309479889,
+            "velocityY": -1.237660650109664,
+            "timestamp": 0.45293436525611025
+        },
+        {
+            "x": 3.2087154283406467,
+            "y": 6.171188923340848,
+            "heading": -0.04850064735870574,
+            "angularVelocity": -0.763447552582083,
+            "velocityX": 3.340706769624593,
+            "velocityY": -0.8336093706421461,
+            "timestamp": 0.5164628253247648
+        },
+        {
+            "x": 3.43894020563635,
+            "y": 6.146133056520132,
+            "heading": -0.0773967080078905,
+            "angularVelocity": -0.45485221297638595,
+            "velocityX": 3.623962819921881,
+            "velocityY": -0.3944038119865702,
+            "timestamp": 0.5799912853934195
+        },
+        {
+            "x": 3.6791864985656395,
+            "y": 6.1522055528567385,
+            "heading": -0.07740096735197836,
+            "angularVelocity": -0.00006704623623761945,
+            "velocityX": 3.781711262474177,
+            "velocityY": 0.09558702241564195,
+            "timestamp": 0.6435197454620741
+        },
+        {
+            "x": 3.9162306085794563,
+            "y": 6.191776893986129,
+            "heading": -0.0774009621424873,
+            "angularVelocity": 8.200247652314305e-8,
+            "velocityX": 3.731305776303155,
+            "velocityY": 0.6228915526463914,
+            "timestamp": 0.7070482055307288
+        },
+        {
+            "x": 4.1454234559206995,
+            "y": 6.264071031070342,
+            "heading": -0.0774009579813014,
+            "angularVelocity": 6.550112963869378e-8,
+            "velocityX": 3.607719234710775,
+            "velocityY": 1.1379803163195765,
+            "timestamp": 0.7705766655993834
+        },
+        {
+            "x": 4.362271301821872,
+            "y": 6.367670338469585,
+            "heading": -0.07740095431740261,
+            "angularVelocity": 5.7673344791685836e-8,
+            "velocityX": 3.4133968565714166,
+            "velocityY": 1.630754268044481,
+            "timestamp": 0.8341051256680381
+        },
+        {
+            "x": 4.562525576285781,
+            "y": 6.500538810004899,
+            "heading": -0.07740095080920495,
+            "angularVelocity": 5.522245710837058e-8,
+            "velocityX": 3.152197837748554,
+            "velocityY": 2.091479494256989,
+            "timestamp": 0.8976335857366927
+        },
+        {
+            "x": 4.76192015617459,
+            "y": 6.634693969014619,
+            "heading": -0.07740094730433215,
+            "angularVelocity": 5.517012038625121e-8,
+            "velocityX": 3.138665405604548,
+            "velocityY": 2.111733211614557,
+            "timestamp": 0.9611620458053474
+        },
+        {
+            "x": 4.961315208646695,
+            "y": 6.76884842562192,
+            "heading": -0.07740094379946605,
+            "angularVelocity": 5.5170015187013835e-8,
+            "velocityX": 3.138672844527154,
+            "velocityY": 2.1117221551145704,
+            "timestamp": 1.024690505874002
+        },
+        {
+            "x": 5.173514155427892,
+            "y": 6.881664257521493,
+            "heading": -0.07740094020412008,
+            "angularVelocity": 5.659425663991619e-8,
+            "velocityX": 3.3402186445551516,
+            "velocityY": 1.7758313640477164,
+            "timestamp": 1.0882189659426564
+        },
+        {
+            "x": 5.399391012764095,
+            "y": 6.963734040630747,
+            "heading": -0.07740093622781413,
+            "angularVelocity": 6.259093865144696e-8,
+            "velocityX": 3.5555223138118204,
+            "velocityY": 1.2918585311301378,
+            "timestamp": 1.151747426011311
+        },
+        {
+            "x": 5.634517116552883,
+            "y": 7.013448397757994,
+            "heading": -0.07740093140263109,
+            "angularVelocity": 7.595309327466021e-8,
+            "velocityX": 3.7011144852982407,
+            "velocityY": 0.7825525295830946,
+            "timestamp": 1.2152758860799655
+        },
+        {
+            "x": 5.874282332827973,
+            "y": 7.02983251834796,
+            "heading": -0.07740092481326658,
+            "angularVelocity": 1.0372303211074395e-7,
+            "velocityX": 3.7741386461434376,
+            "velocityY": 0.2579020579478282,
+            "timestamp": 1.27880434614862
+        },
+        {
+            "x": 6.113985538482666,
+            "y": 7.0125651359558105,
+            "heading": -0.07740091266766538,
+            "angularVelocity": 1.9118362343988029e-7,
+            "velocityX": 3.773162538422118,
+            "velocityY": -0.27180546126086014,
+            "timestamp": 1.3423328062172746
+        },
+        {
+            "x": 6.409905183229014,
+            "y": 6.936971954690801,
+            "heading": -0.07740090470499697,
+            "angularVelocity": 9.862508855382814e-8,
+            "velocityX": 3.66524129970236,
+            "velocityY": -0.9362921822438863,
+            "timestamp": 1.4230695491897984
+        },
+        {
+            "x": 6.687740416068466,
+            "y": 6.8101241855802925,
+            "heading": -0.0774008999193729,
+            "angularVelocity": 5.9274425678096215e-8,
+            "velocityX": 3.441248960637469,
+            "velocityY": -1.5711281436467635,
+            "timestamp": 1.5038062921623223
+        },
+        {
+            "x": 6.938692776760068,
+            "y": 6.63603889201252,
+            "heading": -0.0774008963221294,
+            "angularVelocity": 4.455522177089406e-8,
+            "velocityX": 3.108279470438973,
+            "velocityY": -2.156209021548162,
+            "timestamp": 1.5845430351348462
+        },
+        {
+            "x": 7.154815342738161,
+            "y": 6.420229219532517,
+            "heading": -0.07740089316210408,
+            "angularVelocity": 3.913986631676039e-8,
+            "velocityX": 2.6768799188697083,
+            "velocityY": -2.6730044405364133,
+            "timestamp": 1.66527977810737
+        },
+        {
+            "x": 7.329876929572681,
+            "y": 6.2124862031192585,
+            "heading": -0.05260075245643602,
+            "angularVelocity": 0.3071729152375715,
+            "velocityX": 2.1683013258795385,
+            "velocityY": -2.5730913678789142,
+            "timestamp": 1.746016521079894
+        },
+        {
+            "x": 7.469926171014247,
+            "y": 6.046291741893866,
+            "heading": -0.03276050638448697,
+            "angularVelocity": 0.2457399858041329,
+            "velocityX": 1.7346407135748194,
+            "velocityY": -2.0584736900019895,
+            "timestamp": 1.8267532640524178
+        },
+        {
+            "x": 7.574963090588109,
+            "y": 5.921645878284834,
+            "heading": -0.01788026453634592,
+            "angularVelocity": 0.184305698995044,
+            "velocityX": 1.300980392652287,
+            "velocityY": -1.5438554866085237,
+            "timestamp": 1.9074900070249416
+        },
+        {
+            "x": 7.644987698235257,
+            "y": 5.8385486280487005,
+            "heading": -0.007960086700925195,
+            "angularVelocity": 0.12287067164441078,
+            "velocityX": 0.8673201948582164,
+            "velocityY": -1.0292370880556136,
+            "timestamp": 1.9882267499974655
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -0.003,
+            "angularVelocity": 0.06143530836524976,
+            "velocityX": 0.43366007192855915,
+            "velocityY": -0.5146185803264369,
+            "timestamp": 2.0689634929699894
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -0.003,
+            "angularVelocity": -2.7405624218065555e-29,
+            "velocityX": 1.4947392660092065e-31,
+            "velocityY": 0,
+            "timestamp": 2.1497002359425132
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.traj
index cd2bebdf..3bdf526e 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.traj	
@@ -4,253 +4,307 @@
             "x": 2.3,
             "y": 6.757,
             "heading": 0.39,
-            "angularVelocity": 2.274971502954598e-31,
-            "velocityX": 3.487335890979115e-33,
-            "velocityY": 2.862188318424697e-32,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 2.3417817158351957,
-            "y": 6.761449131523058,
-            "heading": 0.37953631669999827,
-            "angularVelocity": -0.14722299547472617,
-            "velocityX": 0.5878646347531417,
-            "velocityY": 0.06259884318982863,
-            "timestamp": 0.07107370194626751
-        },
-        {
-            "x": 2.42534939307215,
-            "y": 6.770350816518497,
-            "heading": 0.3588464620969251,
-            "angularVelocity": -0.2911042204995314,
-            "velocityX": 1.1757890042104941,
-            "velocityY": 0.12524583287024788,
-            "timestamp": 0.14214740389253502
-        },
-        {
-            "x": 2.5507092244237364,
-            "y": 6.783710205716013,
-            "heading": 0.3282884162089386,
-            "angularVelocity": -0.42994870185747563,
-            "velocityX": 1.7638005045292426,
-            "velocityY": 0.18796529281121122,
-            "timestamp": 0.21322110583880255
-        },
-        {
-            "x": 2.717871083967313,
-            "y": 6.80153596037414,
-            "heading": 0.2884653946252666,
-            "angularVelocity": -0.5603059991696991,
-            "velocityX": 2.351950932145827,
-            "velocityY": 0.25080661580833774,
-            "timestamp": 0.28429480778507005
-        },
-        {
-            "x": 2.926852904271386,
-            "y": 6.82384561239607,
-            "heading": 0.2406022074409234,
-            "angularVelocity": -0.673430338841082,
-            "velocityX": 2.9403536692385535,
-            "velocityY": 0.31389461096009597,
-            "timestamp": 0.35536850973133755
-        },
-        {
-            "x": 3.1776924244019935,
-            "y": 6.85069218229951,
-            "heading": 0.1884744580400949,
-            "angularVelocity": -0.7334323100298853,
-            "velocityX": 3.529287391280776,
-            "velocityY": 0.3777285995842137,
-            "timestamp": 0.42644221167760504
-        },
-        {
-            "x": 3.4450596021465354,
-            "y": 6.879057072545224,
-            "heading": 0.1884744481252109,
-            "angularVelocity": -1.3950144320454384e-7,
-            "velocityX": 3.7618299092775445,
-            "velocityY": 0.39909121755204374,
-            "timestamp": 0.49751591362387254
-        },
-        {
-            "x": 3.712426777772062,
-            "y": 6.907421982754425,
-            "heading": 0.18847443820909762,
-            "angularVelocity": -1.3951873937625683e-7,
-            "velocityX": 3.7618298794631864,
-            "velocityY": 0.39909149843660235,
-            "timestamp": 0.5685896155701401
-        },
-        {
-            "x": 3.979793955045072,
-            "y": 6.93578687743453,
-            "heading": 0.188474428292985,
-            "angularVelocity": -1.3951872987555026e-7,
-            "velocityX": 3.7618299026431106,
-            "velocityY": 0.3990912799437423,
-            "timestamp": 0.6396633175164076
-        },
-        {
-            "x": 4.247161134956238,
-            "y": 6.964151747247427,
-            "heading": 0.18847441837687146,
-            "angularVelocity": -1.395187425580786e-7,
-            "velocityX": 3.761829939761708,
-            "velocityY": 0.399090930064572,
-            "timestamp": 0.7107370194626751
-        },
-        {
-            "x": 4.514528318048531,
-            "y": 6.992516587075052,
-            "heading": 0.18847440846075478,
-            "angularVelocity": -1.3951878716159625e-7,
-            "velocityX": 3.7618299845198404,
-            "velocityY": 0.39909050817474234,
-            "timestamp": 0.7818107214089426
-        },
-        {
-            "x": 4.781895504744899,
-            "y": 7.020881392930694,
-            "heading": 0.18847439854463183,
-            "angularVelocity": -1.395188750932112e-7,
-            "velocityX": 3.761830035228834,
-            "velocityY": 0.3990900301922755,
-            "timestamp": 0.8528844233552101
-        },
-        {
-            "x": 5.049262689335958,
-            "y": 7.049246218630208,
-            "heading": 0.18847438862849922,
-            "angularVelocity": -1.3951901124406535e-7,
-            "velocityX": 3.7618300056073353,
-            "velocityY": 0.3990903093936624,
-            "timestamp": 0.9239581253014776
-        },
-        {
-            "x": 5.3166271708653765,
-            "y": 7.077636511969196,
-            "heading": 0.18847437871238637,
-            "angularVelocity": -1.3951873300413244e-7,
-            "velocityX": 3.761791973795463,
-            "velocityY": 0.3994486365804798,
-            "timestamp": 0.9950318272477451
-        },
-        {
-            "x": 5.584732592713635,
-            "y": 7.097866624574175,
-            "heading": 0.18847436874075538,
-            "angularVelocity": -1.4029986815741496e-7,
-            "velocityX": 3.772216931248832,
-            "velocityY": 0.28463569578063386,
-            "timestamp": 1.0661055291940127
-        },
-        {
-            "x": 5.85271010689652,
-            "y": 7.076007397472714,
-            "heading": 0.188474358062239,
-            "angularVelocity": -1.5024567569917294e-7,
-            "velocityX": 3.7704172829702145,
-            "velocityY": -0.3075571766057785,
-            "timestamp": 1.1371792311402802
+            "x": 2.3119316870686255,
+            "y": 6.7398994499669,
+            "heading": 0.3779608263213178,
+            "angularVelocity": -0.23922354190742906,
+            "velocityX": 0.23708773688854562,
+            "velocityY": -0.3397952597632521,
+            "timestamp": 0.05032604058401226
+        },
+        {
+            "x": 2.3368398722232935,
+            "y": 6.706448700500007,
+            "heading": 0.3544233266085098,
+            "angularVelocity": -0.4677002092687843,
+            "velocityX": 0.49493631657919795,
+            "velocityY": -0.6646807314605355,
+            "timestamp": 0.10065208116802452
+        },
+        {
+            "x": 2.3760366485894098,
+            "y": 6.657739626207859,
+            "heading": 0.32015686455898285,
+            "angularVelocity": -0.6808892901544442,
+            "velocityX": 0.7788567491352839,
+            "velocityY": -0.9678701866250936,
+            "timestamp": 0.1509781217520368
+        },
+        {
+            "x": 2.431161593733442,
+            "y": 6.595431091068774,
+            "heading": 0.27624213154004224,
+            "angularVelocity": -0.8726045703046355,
+            "velocityX": 1.0953562907856245,
+            "velocityY": -1.2380973034243006,
+            "timestamp": 0.20130416233604903
+        },
+        {
+            "x": 2.504176998220011,
+            "y": 6.522134358575033,
+            "heading": 0.22423189981090158,
+            "angularVelocity": -1.0334656000271036,
+            "velocityX": 1.450847387143028,
+            "velocityY": -1.456437495244302,
+            "timestamp": 0.2516302029200613
+        },
+        {
+            "x": 2.597103761438428,
+            "y": 6.441982348073212,
+            "heading": 0.16656086207364196,
+            "angularVelocity": -1.1459482420634122,
+            "velocityX": 1.8464946206783215,
+            "velocityY": -1.5926548079620442,
+            "timestamp": 0.3019562435040735
+        },
+        {
+            "x": 2.711127221877276,
+            "y": 6.361040590595544,
+            "heading": 0.10700626596614256,
+            "angularVelocity": -1.1833753543173746,
+            "velocityX": 2.265695038108593,
+            "velocityY": -1.6083474189181441,
+            "timestamp": 0.35228228408808576
+        },
+        {
+            "x": 2.8452820247148356,
+            "y": 6.2864334030632,
+            "heading": 0.05015861793730577,
+            "angularVelocity": -1.1295871355889644,
+            "velocityX": 2.6657134414062895,
+            "velocityY": -1.482476798622636,
+            "timestamp": 0.402608324672098
+        },
+        {
+            "x": 2.996485471725464,
+            "y": 6.224146842956543,
+            "heading": 0,
+            "angularVelocity": -0.9966732402395141,
+            "velocityX": 3.004477309479889,
+            "velocityY": -1.237660650109664,
+            "timestamp": 0.45293436525611025
+        },
+        {
+            "x": 3.2087154283406467,
+            "y": 6.171188923340848,
+            "heading": -0.04850064735870574,
+            "angularVelocity": -0.763447552582083,
+            "velocityX": 3.340706769624593,
+            "velocityY": -0.8336093706421461,
+            "timestamp": 0.5164628253247648
+        },
+        {
+            "x": 3.43894020563635,
+            "y": 6.146133056520132,
+            "heading": -0.0773967080078905,
+            "angularVelocity": -0.45485221297638595,
+            "velocityX": 3.623962819921881,
+            "velocityY": -0.3944038119865702,
+            "timestamp": 0.5799912853934195
+        },
+        {
+            "x": 3.6791864985656395,
+            "y": 6.1522055528567385,
+            "heading": -0.07740096735197836,
+            "angularVelocity": -0.00006704623623761945,
+            "velocityX": 3.781711262474177,
+            "velocityY": 0.09558702241564195,
+            "timestamp": 0.6435197454620741
+        },
+        {
+            "x": 3.9162306085794563,
+            "y": 6.191776893986129,
+            "heading": -0.0774009621424873,
+            "angularVelocity": 8.200247652314305e-8,
+            "velocityX": 3.731305776303155,
+            "velocityY": 0.6228915526463914,
+            "timestamp": 0.7070482055307288
+        },
+        {
+            "x": 4.1454234559206995,
+            "y": 6.264071031070342,
+            "heading": -0.0774009579813014,
+            "angularVelocity": 6.550112963869378e-8,
+            "velocityX": 3.607719234710775,
+            "velocityY": 1.1379803163195765,
+            "timestamp": 0.7705766655993834
+        },
+        {
+            "x": 4.362271301821872,
+            "y": 6.367670338469585,
+            "heading": -0.07740095431740261,
+            "angularVelocity": 5.7673344791685836e-8,
+            "velocityX": 3.4133968565714166,
+            "velocityY": 1.630754268044481,
+            "timestamp": 0.8341051256680381
+        },
+        {
+            "x": 4.562525576285781,
+            "y": 6.500538810004899,
+            "heading": -0.07740095080920495,
+            "angularVelocity": 5.522245710837058e-8,
+            "velocityX": 3.152197837748554,
+            "velocityY": 2.091479494256989,
+            "timestamp": 0.8976335857366927
+        },
+        {
+            "x": 4.76192015617459,
+            "y": 6.634693969014619,
+            "heading": -0.07740094730433215,
+            "angularVelocity": 5.517012038625121e-8,
+            "velocityX": 3.138665405604548,
+            "velocityY": 2.111733211614557,
+            "timestamp": 0.9611620458053474
+        },
+        {
+            "x": 4.961315208646695,
+            "y": 6.76884842562192,
+            "heading": -0.07740094379946605,
+            "angularVelocity": 5.5170015187013835e-8,
+            "velocityX": 3.138672844527154,
+            "velocityY": 2.1117221551145704,
+            "timestamp": 1.024690505874002
+        },
+        {
+            "x": 5.173514155427892,
+            "y": 6.881664257521493,
+            "heading": -0.07740094020412008,
+            "angularVelocity": 5.659425663991619e-8,
+            "velocityX": 3.3402186445551516,
+            "velocityY": 1.7758313640477164,
+            "timestamp": 1.0882189659426564
+        },
+        {
+            "x": 5.399391012764095,
+            "y": 6.963734040630747,
+            "heading": -0.07740093622781413,
+            "angularVelocity": 6.259093865144696e-8,
+            "velocityX": 3.5555223138118204,
+            "velocityY": 1.2918585311301378,
+            "timestamp": 1.151747426011311
+        },
+        {
+            "x": 5.634517116552883,
+            "y": 7.013448397757994,
+            "heading": -0.07740093140263109,
+            "angularVelocity": 7.595309327466021e-8,
+            "velocityX": 3.7011144852982407,
+            "velocityY": 0.7825525295830946,
+            "timestamp": 1.2152758860799655
+        },
+        {
+            "x": 5.874282332827973,
+            "y": 7.02983251834796,
+            "heading": -0.07740092481326658,
+            "angularVelocity": 1.0372303211074395e-7,
+            "velocityX": 3.7741386461434376,
+            "velocityY": 0.2579020579478282,
+            "timestamp": 1.27880434614862
         },
         {
             "x": 6.113985538482666,
             "y": 7.0125651359558105,
-            "heading": 0.18847434515977443,
-            "angularVelocity": -1.8153640819270364e-7,
-            "velocityX": 3.6761196396335167,
-            "velocityY": -0.8926263833128883,
-            "timestamp": 1.2082529330865477
-        },
-        {
-            "x": 6.38971197835489,
-            "y": 6.891444245730791,
-            "heading": 0.18847433517810044,
-            "angularVelocity": -1.2538350348338093e-7,
-            "velocityX": 3.4635019217336325,
-            "velocityY": -1.521444357133088,
-            "timestamp": 1.2878620818911364
-        },
-        {
-            "x": 6.640022696468843,
-            "y": 6.723993100546061,
-            "heading": 0.18847432674418188,
-            "angularVelocity": -1.0594157434839283e-7,
-            "velocityX": 3.1442456284571,
-            "velocityY": -2.103415847287377,
-            "timestamp": 1.367471230695725
-        },
-        {
-            "x": 6.8648306229514935,
-            "y": 6.523601637914116,
-            "heading": 0.18847431867026868,
-            "angularVelocity": -1.014194138550125e-7,
-            "velocityX": 2.8238956182605124,
-            "velocityY": -2.5171913736164258,
-            "timestamp": 1.4470803795003138
-        },
-        {
-            "x": 7.089642217529903,
-            "y": 6.3232142903896476,
-            "heading": 0.1884743105959038,
-            "angularVelocity": -1.0142508708190059e-7,
-            "velocityX": 2.823941694568014,
-            "velocityY": -2.517139682229462,
-            "timestamp": 1.5266895283049025
-        },
-        {
-            "x": 7.2864279243188,
-            "y": 6.147809259471006,
-            "heading": 0.12465186300240168,
-            "angularVelocity": -0.8016973997575559,
-            "velocityX": 2.471898139143281,
-            "velocityY": -2.203327551576542,
-            "timestamp": 1.6062986771094911
-        },
-        {
-            "x": 7.443856673448633,
-            "y": 6.007485445692029,
-            "heading": 0.07359246308164451,
-            "angularVelocity": -0.6413760313670541,
-            "velocityX": 1.9775208188222257,
-            "velocityY": -1.7626593913656032,
-            "timestamp": 1.6859078259140798
-        },
-        {
-            "x": 7.561928317357885,
-            "y": 5.902242696623479,
-            "heading": 0.0352966131369852,
-            "angularVelocity": -0.4810483533581169,
-            "velocityX": 1.483141644926293,
-            "velocityY": -1.3219931458747511,
-            "timestamp": 1.7655169747186685
-        },
-        {
-            "x": 7.640642772199918,
-            "y": 5.832080900256126,
-            "heading": 0.009765510491971057,
-            "angularVelocity": -0.320705635324694,
-            "velocityX": 0.9887614178017027,
-            "velocityY": -0.8813283073733266,
-            "timestamp": 1.8451261235232572
+            "heading": -0.07740091266766538,
+            "angularVelocity": 1.9118362343988029e-7,
+            "velocityX": 3.773162538422118,
+            "velocityY": -0.27180546126086014,
+            "timestamp": 1.3423328062172746
+        },
+        {
+            "x": 6.409905183229014,
+            "y": 6.936971954690801,
+            "heading": -0.07740090470499697,
+            "angularVelocity": 9.862508855382814e-8,
+            "velocityX": 3.66524129970236,
+            "velocityY": -0.9362921822438863,
+            "timestamp": 1.4230695491897984
+        },
+        {
+            "x": 6.687740416068466,
+            "y": 6.8101241855802925,
+            "heading": -0.0774008999193729,
+            "angularVelocity": 5.9274425678096215e-8,
+            "velocityX": 3.441248960637469,
+            "velocityY": -1.5711281436467635,
+            "timestamp": 1.5038062921623223
+        },
+        {
+            "x": 6.938692776760068,
+            "y": 6.63603889201252,
+            "heading": -0.0774008963221294,
+            "angularVelocity": 4.455522177089406e-8,
+            "velocityX": 3.108279470438973,
+            "velocityY": -2.156209021548162,
+            "timestamp": 1.5845430351348462
+        },
+        {
+            "x": 7.154815342738161,
+            "y": 6.420229219532517,
+            "heading": -0.07740089316210408,
+            "angularVelocity": 3.913986631676039e-8,
+            "velocityX": 2.6768799188697083,
+            "velocityY": -2.6730044405364133,
+            "timestamp": 1.66527977810737
+        },
+        {
+            "x": 7.329876929572681,
+            "y": 6.2124862031192585,
+            "heading": -0.05260075245643602,
+            "angularVelocity": 0.3071729152375715,
+            "velocityX": 2.1683013258795385,
+            "velocityY": -2.5730913678789142,
+            "timestamp": 1.746016521079894
+        },
+        {
+            "x": 7.469926171014247,
+            "y": 6.046291741893866,
+            "heading": -0.03276050638448697,
+            "angularVelocity": 0.2457399858041329,
+            "velocityX": 1.7346407135748194,
+            "velocityY": -2.0584736900019895,
+            "timestamp": 1.8267532640524178
+        },
+        {
+            "x": 7.574963090588109,
+            "y": 5.921645878284834,
+            "heading": -0.01788026453634592,
+            "angularVelocity": 0.184305698995044,
+            "velocityX": 1.300980392652287,
+            "velocityY": -1.5438554866085237,
+            "timestamp": 1.9074900070249416
+        },
+        {
+            "x": 7.644987698235257,
+            "y": 5.8385486280487005,
+            "heading": -0.007960086700925195,
+            "angularVelocity": 0.12287067164441078,
+            "velocityX": 0.8673201948582164,
+            "velocityY": -1.0292370880556136,
+            "timestamp": 1.9882267499974655
         },
         {
             "x": 7.68,
             "y": 5.797,
             "heading": -0.003,
-            "angularVelocity": -0.1603523047748632,
-            "velocityX": 0.4943807136624207,
-            "velocityY": -0.44066417971947147,
-            "timestamp": 1.924735272327846
+            "angularVelocity": 0.06143530836524976,
+            "velocityX": 0.43366007192855915,
+            "velocityY": -0.5146185803264369,
+            "timestamp": 2.0689634929699894
         },
         {
             "x": 7.68,
             "y": 5.797,
             "heading": -0.003,
-            "angularVelocity": 9.952815155734959e-29,
-            "velocityX": 0,
-            "velocityY": 1.270429811278963e-30,
-            "timestamp": 2.004344421132435
+            "angularVelocity": -2.7405624218065555e-29,
+            "velocityX": 1.4947392660092065e-31,
+            "velocityY": 0,
+            "timestamp": 2.1497002359425132
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.1.traj
new file mode 100644
index 00000000..9479c37b
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.1.traj	
@@ -0,0 +1,373 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.308859553563438,
+            "y": 6.744970476661412,
+            "heading": 0.38132184535212243,
+            "angularVelocity": -0.20370231005101772,
+            "velocityX": 0.20796028650336576,
+            "velocityY": -0.2823689819220092,
+            "timestamp": 0.04260214155501803
+        },
+        {
+            "x": 2.3271744503017344,
+            "y": 6.7213636093363665,
+            "heading": 0.36416244677454657,
+            "angularVelocity": -0.4027825351318442,
+            "velocityX": 0.4299055415945176,
+            "velocityY": -0.554123958640874,
+            "timestamp": 0.08520428311003606
+        },
+        {
+            "x": 2.355668020695042,
+            "y": 6.686794512902429,
+            "heading": 0.33880572426100314,
+            "angularVelocity": -0.5951983066578187,
+            "velocityX": 0.6688295318795241,
+            "velocityY": -0.8114403448308736,
+            "timestamp": 0.1278064246650541
+        },
+        {
+            "x": 2.3952194813587413,
+            "y": 6.64212767454663,
+            "heading": 0.305649210011626,
+            "angularVelocity": -0.7782828054913067,
+            "velocityX": 0.9283913723590802,
+            "velocityY": -1.0484646246742158,
+            "timestamp": 0.1704085662200721
+        },
+        {
+            "x": 2.4468808744427974,
+            "y": 6.58861694166169,
+            "heading": 0.2652376454484053,
+            "angularVelocity": -0.9485805898051318,
+            "velocityX": 1.2126477965277631,
+            "velocityY": -1.2560573466907532,
+            "timestamp": 0.21301070777509012
+        },
+        {
+            "x": 2.5118430486187977,
+            "y": 6.5281213977332095,
+            "heading": 0.2183393877421899,
+            "angularVelocity": -1.100842727487048,
+            "velocityX": 1.5248570096436473,
+            "velocityY": -1.4200118050486843,
+            "timestamp": 0.25561284933010814
+        },
+        {
+            "x": 2.5912567436103777,
+            "y": 6.463385022311091,
+            "heading": 0.16616355843360497,
+            "angularVelocity": -1.2247231571962895,
+            "velocityX": 1.864077534436207,
+            "velocityY": -1.5195568358579936,
+            "timestamp": 0.29821499088512615
+        },
+        {
+            "x": 2.6857686356352475,
+            "y": 6.398206753066201,
+            "heading": 0.11066645077656016,
+            "angularVelocity": -1.3026835185121792,
+            "velocityX": 2.21847748904391,
+            "velocityY": -1.5299294088471524,
+            "timestamp": 0.34081713244014417
+        },
+        {
+            "x": 2.794870523929059,
+            "y": 6.337041243081873,
+            "heading": 0.05441835033855986,
+            "angularVelocity": -1.3203115708481314,
+            "velocityX": 2.5609484479298716,
+            "velocityY": -1.43573791719686,
+            "timestamp": 0.3834192739951622
+        },
+        {
+            "x": 2.916773796081543,
+            "y": 6.28393030166626,
+            "heading": 0,
+            "angularVelocity": -1.2773618497155108,
+            "velocityX": 2.8614353105947288,
+            "velocityY": -1.246673042176136,
+            "timestamp": 0.4260214155501802
+        },
+        {
+            "x": 3.0391710673922954,
+            "y": 6.243974094587608,
+            "heading": -0.047264422869834,
+            "angularVelocity": -1.1928681768219465,
+            "velocityX": 3.0890847917160027,
+            "velocityY": -1.0084220857175146,
+            "timestamp": 0.4656439185125414
+        },
+        {
+            "x": 3.1702962457898094,
+            "y": 6.213670351410701,
+            "heading": -0.08947327835139807,
+            "angularVelocity": -1.0652748394431244,
+            "velocityX": 3.309361312233972,
+            "velocityY": -0.7648114306582993,
+            "timestamp": 0.5052664214749026
+        },
+        {
+            "x": 3.3090884442062665,
+            "y": 6.193565770320959,
+            "heading": -0.12122975141158922,
+            "angularVelocity": -0.8014756940104903,
+            "velocityX": 3.502862970274757,
+            "velocityY": -0.5074031064832265,
+            "timestamp": 0.5448889244372639
+        },
+        {
+            "x": 3.4545421039318374,
+            "y": 6.168639069925164,
+            "heading": -0.11367597167549683,
+            "angularVelocity": 0.19064367900402435,
+            "velocityX": 3.6709861530893746,
+            "velocityY": -0.6291046383282068,
+            "timestamp": 0.5845114273996251
+        },
+        {
+            "x": 3.5909551218669695,
+            "y": 6.134279746914322,
+            "heading": -0.10324626427429143,
+            "angularVelocity": 0.26322686911306303,
+            "velocityX": 3.442816776737095,
+            "velocityY": -0.8671668986555798,
+            "timestamp": 0.6241339303619864
+        },
+        {
+            "x": 3.7180733265010666,
+            "y": 6.090709420708735,
+            "heading": -0.09157048631886859,
+            "angularVelocity": 0.2946754263988333,
+            "velocityX": 3.208232573163052,
+            "velocityY": -1.0996358873889647,
+            "timestamp": 0.6637564333243476
+        },
+        {
+            "x": 3.835817587911086,
+            "y": 6.038004134131813,
+            "heading": -0.07918178962672723,
+            "angularVelocity": 0.31266820028784686,
+            "velocityX": 2.9716512740719234,
+            "velocityY": -1.3301856933921488,
+            "timestamp": 0.7033789362867089
+        },
+        {
+            "x": 3.9441493716247042,
+            "y": 5.9762022678896844,
+            "heading": -0.06634465187684353,
+            "angularVelocity": 0.3239860379864983,
+            "velocityX": 2.734097434897688,
+            "velocityY": -1.5597668400917784,
+            "timestamp": 0.7430014392490701
+        },
+        {
+            "x": 4.0430458841375625,
+            "y": 5.905326959185002,
+            "heading": -0.05321706206750646,
+            "angularVelocity": 0.3313165203573185,
+            "velocityX": 2.4959683290781256,
+            "velocityY": -1.7887640458253868,
+            "timestamp": 0.7826239422114314
+        },
+        {
+            "x": 4.1324920654296875,
+            "y": 5.8253936767578125,
+            "heading": -0.03990404951279715,
+            "angularVelocity": 0.33599625362778834,
+            "velocityX": 2.2574591357111773,
+            "velocityY": -2.017370848659449,
+            "timestamp": 0.8222464451737926
+        },
+        {
+            "x": 4.241428434426057,
+            "y": 5.691951873954815,
+            "heading": -0.020558340356236417,
+            "angularVelocity": 0.3400028267859621,
+            "velocityX": 1.9145678816329739,
+            "velocityY": -2.345253399462351,
+            "timestamp": 0.8791451166920127
+        },
+        {
+            "x": 4.330837596388679,
+            "y": 5.539871857431794,
+            "heading": -0.0011062192039580838,
+            "angularVelocity": 0.34187302854776463,
+            "velocityX": 1.571375211001045,
+            "velocityY": -2.6728219212345348,
+            "timestamp": 0.9360437882102327
+        },
+        {
+            "x": 4.400691025411647,
+            "y": 5.369183558238886,
+            "heading": 0.018249380247094018,
+            "angularVelocity": 0.34017664972817613,
+            "velocityX": 1.2276811946408939,
+            "velocityY": -2.999864401724217,
+            "timestamp": 0.9929424597284527
+        },
+        {
+            "x": 4.450931892836431,
+            "y": 5.179947405025881,
+            "heading": 0.037101343157351356,
+            "angularVelocity": 0.3313251857597514,
+            "velocityX": 0.8829884087662048,
+            "velocityY": -3.3258448424829754,
+            "timestamp": 1.0498411312466729
+        },
+        {
+            "x": 4.481391591894744,
+            "y": 4.9723492336445485,
+            "heading": 0.054218740493726776,
+            "angularVelocity": 0.3008400175194617,
+            "velocityX": 0.5353323416094045,
+            "velocityY": -3.6485591990465953,
+            "timestamp": 1.106739802764893
+        },
+        {
+            "x": 4.496396423991072,
+            "y": 4.780842919446315,
+            "heading": 0.0400753404119281,
+            "angularVelocity": -0.248571710101697,
+            "velocityX": 0.26371146629537756,
+            "velocityY": -3.3657431551263746,
+            "timestamp": 1.163638474283113
+        },
+        {
+            "x": 4.530633514554346,
+            "y": 4.608273372061934,
+            "heading": 0.027581622688211512,
+            "angularVelocity": -0.21957837310341866,
+            "velocityX": 0.6017203855508131,
+            "velocityY": -3.032927532044774,
+            "timestamp": 1.220537145801333
+        },
+        {
+            "x": 4.584102924369241,
+            "y": 4.454640545632531,
+            "heading": 0.016737783854959394,
+            "angularVelocity": -0.1905815820283204,
+            "velocityX": 0.9397303731032456,
+            "velocityY": -2.7001127149375717,
+            "timestamp": 1.277435817319553
+        },
+        {
+            "x": 4.656804671911905,
+            "y": 4.319944426632471,
+            "heading": 0.007543896915478472,
+            "angularVelocity": -0.1615835079126032,
+            "velocityX": 1.277740685375807,
+            "velocityY": -2.3672981355448273,
+            "timestamp": 1.334334488837773
+        },
+        {
+            "x": 4.748738765716553,
+            "y": 4.2041850090026855,
+            "heading": 0,
+            "angularVelocity": -0.13258476365415262,
+            "velocityX": 1.6157511476381003,
+            "velocityY": -2.0344836626408243,
+            "timestamp": 1.391233160355993
+        },
+        {
+            "x": 5.007932614843106,
+            "y": 4.0487951090615,
+            "heading": -0.008478617739268947,
+            "angularVelocity": -0.07486216912690727,
+            "velocityX": 2.288558626731898,
+            "velocityY": -1.372019039864535,
+            "timestamp": 1.5044895332096289
+        },
+        {
+            "x": 5.344428643764395,
+            "y": 3.9673190703928256,
+            "heading": -0.018205683784992727,
+            "angularVelocity": -0.08588537492980046,
+            "velocityX": 2.971100172492302,
+            "velocityY": -0.7193947379364598,
+            "timestamp": 1.6177459060632646
+        },
+        {
+            "x": 5.761576858213634,
+            "y": 3.95566669131606,
+            "heading": -0.05529230717881331,
+            "angularVelocity": -0.3274572764373147,
+            "velocityX": 3.6832206783483272,
+            "velocityY": -0.10288497488635052,
+            "timestamp": 1.7310022789169004
+        },
+        {
+            "x": 6.1883195012502945,
+            "y": 3.9937898497766295,
+            "heading": -0.05529230006422074,
+            "angularVelocity": 6.281847442205705e-8,
+            "velocityX": 3.767934927495438,
+            "velocityY": 0.33660938894658915,
+            "timestamp": 1.8442586517705362
+        },
+        {
+            "x": 6.615062139687873,
+            "y": 4.031913059718256,
+            "heading": -0.05529229294956677,
+            "angularVelocity": 6.281901664491759e-8,
+            "velocityX": 3.7679348868877285,
+            "velocityY": 0.3366098434998829,
+            "timestamp": 1.957515024624172
+        },
+        {
+            "x": 7.0409980156903185,
+            "y": 4.069937864184792,
+            "heading": -0.05280872948360114,
+            "angularVelocity": 0.021928686248633502,
+            "velocityX": 3.7608115576232954,
+            "velocityY": 0.33574096987616325,
+            "timestamp": 2.0707713974778077
+        },
+        {
+            "x": 7.360520062907597,
+            "y": 4.098469259621894,
+            "heading": -0.032478143170072554,
+            "angularVelocity": 0.17950942451425975,
+            "velocityX": 2.821227973018394,
+            "velocityY": 0.25191867546363345,
+            "timestamp": 2.1840277703314435
+        },
+        {
+            "x": 7.573513556720189,
+            "y": 4.1174900304811795,
+            "heading": -0.013829576144119837,
+            "angularVelocity": 0.16465799280057086,
+            "velocityX": 1.88063142449255,
+            "velocityY": 0.1679443759325155,
+            "timestamp": 2.2972841431850792
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -0.003,
+            "angularVelocity": 0.09562001564464007,
+            "velocityX": 0.9402247361163992,
+            "velocityY": 0.08396851567117221,
+            "timestamp": 2.410540516038715
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": -0.003,
+            "angularVelocity": 0,
+            "velocityX": 2.757511206578506e-35,
+            "velocityY": 0,
+            "timestamp": 2.5237968888923508
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.traj
index a91e184b..9479c37b 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.traj	
@@ -10,184 +10,364 @@
             "timestamp": 0
         },
         {
-            "x": 2.3668131239789423,
-            "y": 6.6799532352680275,
-            "heading": 0.31688592464549686,
-            "angularVelocity": -0.6546901107445974,
-            "velocityX": 0.5982690928509629,
-            "velocityY": -0.6899048465062367,
-            "timestamp": 0.11167737858654542
-        },
-        {
-            "x": 2.5010623988117353,
-            "y": 6.525329092787392,
-            "heading": 0.1875032700506318,
-            "angularVelocity": -1.1585395022018103,
-            "velocityX": 1.202116995688263,
-            "velocityY": -1.3845609955896643,
-            "timestamp": 0.22335475717309083
-        },
-        {
-            "x": 2.7034647042549174,
-            "y": 6.29238592031536,
-            "heading": 0.03684167640916454,
-            "angularVelocity": -1.3490788873122188,
-            "velocityX": 1.8123841014618285,
-            "velocityY": -2.0858581694904785,
-            "timestamp": 0.33503213575963625
-        },
-        {
-            "x": 2.970402662431944,
-            "y": 5.984552676414473,
-            "heading": 2.692880535790558e-7,
-            "angularVelocity": -0.32989140314179594,
-            "velocityX": 2.39025988571325,
-            "velocityY": -2.7564511971627637,
-            "timestamp": 0.44670951434618167
-        },
-        {
-            "x": 3.247779632307318,
-            "y": 5.665896160181276,
-            "heading": 2.2674846776964554e-7,
-            "angularVelocity": -3.809149744305609e-7,
-            "velocityX": 2.4837346057547505,
-            "velocityY": -2.853366727141142,
-            "timestamp": 0.5583868929327271
-        },
-        {
-            "x": 3.5251566242922663,
-            "y": 5.347239663193555,
-            "heading": 1.842089099097335e-7,
-            "angularVelocity": -3.8091472416520575e-7,
-            "velocityX": 2.4837348037318896,
-            "velocityY": -2.853366554810183,
-            "timestamp": 0.6700642715192726
-        },
-        {
-            "x": 3.8025335437012195,
-            "y": 5.0285831030297485,
-            "heading": 1.4166936288049968e-7,
-            "angularVelocity": -3.8091462718327505e-7,
-            "velocityX": 2.4837341538598574,
-            "velocityY": -2.8533671205119875,
-            "timestamp": 0.7817416501058181
-        },
-        {
-            "x": 4.079910865419093,
-            "y": 4.7099268930580935,
-            "heading": 9.912982095474378e-8,
-            "angularVelocity": -3.809145814844431e-7,
-            "velocityX": 2.4837377562807075,
-            "velocityY": -2.853363984763686,
-            "timestamp": 0.8934190286923636
-        },
-        {
-            "x": 4.382964098552189,
-            "y": 4.415582373772677,
-            "heading": 5.542611481257948e-8,
-            "angularVelocity": -3.9133893269333644e-7,
-            "velocityX": 2.713649236476739,
-            "velocityY": -2.635668234792112,
-            "timestamp": 1.005096407278909
+            "x": 2.308859553563438,
+            "y": 6.744970476661412,
+            "heading": 0.38132184535212243,
+            "angularVelocity": -0.20370231005101772,
+            "velocityX": 0.20796028650336576,
+            "velocityY": -0.2823689819220092,
+            "timestamp": 0.04260214155501803
+        },
+        {
+            "x": 2.3271744503017344,
+            "y": 6.7213636093363665,
+            "heading": 0.36416244677454657,
+            "angularVelocity": -0.4027825351318442,
+            "velocityX": 0.4299055415945176,
+            "velocityY": -0.554123958640874,
+            "timestamp": 0.08520428311003606
+        },
+        {
+            "x": 2.355668020695042,
+            "y": 6.686794512902429,
+            "heading": 0.33880572426100314,
+            "angularVelocity": -0.5951983066578187,
+            "velocityX": 0.6688295318795241,
+            "velocityY": -0.8114403448308736,
+            "timestamp": 0.1278064246650541
+        },
+        {
+            "x": 2.3952194813587413,
+            "y": 6.64212767454663,
+            "heading": 0.305649210011626,
+            "angularVelocity": -0.7782828054913067,
+            "velocityX": 0.9283913723590802,
+            "velocityY": -1.0484646246742158,
+            "timestamp": 0.1704085662200721
+        },
+        {
+            "x": 2.4468808744427974,
+            "y": 6.58861694166169,
+            "heading": 0.2652376454484053,
+            "angularVelocity": -0.9485805898051318,
+            "velocityX": 1.2126477965277631,
+            "velocityY": -1.2560573466907532,
+            "timestamp": 0.21301070777509012
+        },
+        {
+            "x": 2.5118430486187977,
+            "y": 6.5281213977332095,
+            "heading": 0.2183393877421899,
+            "angularVelocity": -1.100842727487048,
+            "velocityX": 1.5248570096436473,
+            "velocityY": -1.4200118050486843,
+            "timestamp": 0.25561284933010814
+        },
+        {
+            "x": 2.5912567436103777,
+            "y": 6.463385022311091,
+            "heading": 0.16616355843360497,
+            "angularVelocity": -1.2247231571962895,
+            "velocityX": 1.864077534436207,
+            "velocityY": -1.5195568358579936,
+            "timestamp": 0.29821499088512615
+        },
+        {
+            "x": 2.6857686356352475,
+            "y": 6.398206753066201,
+            "heading": 0.11066645077656016,
+            "angularVelocity": -1.3026835185121792,
+            "velocityX": 2.21847748904391,
+            "velocityY": -1.5299294088471524,
+            "timestamp": 0.34081713244014417
+        },
+        {
+            "x": 2.794870523929059,
+            "y": 6.337041243081873,
+            "heading": 0.05441835033855986,
+            "angularVelocity": -1.3203115708481314,
+            "velocityX": 2.5609484479298716,
+            "velocityY": -1.43573791719686,
+            "timestamp": 0.3834192739951622
+        },
+        {
+            "x": 2.916773796081543,
+            "y": 6.28393030166626,
+            "heading": 0,
+            "angularVelocity": -1.2773618497155108,
+            "velocityX": 2.8614353105947288,
+            "velocityY": -1.246673042176136,
+            "timestamp": 0.4260214155501802
+        },
+        {
+            "x": 3.0391710673922954,
+            "y": 6.243974094587608,
+            "heading": -0.047264422869834,
+            "angularVelocity": -1.1928681768219465,
+            "velocityX": 3.0890847917160027,
+            "velocityY": -1.0084220857175146,
+            "timestamp": 0.4656439185125414
+        },
+        {
+            "x": 3.1702962457898094,
+            "y": 6.213670351410701,
+            "heading": -0.08947327835139807,
+            "angularVelocity": -1.0652748394431244,
+            "velocityX": 3.309361312233972,
+            "velocityY": -0.7648114306582993,
+            "timestamp": 0.5052664214749026
+        },
+        {
+            "x": 3.3090884442062665,
+            "y": 6.193565770320959,
+            "heading": -0.12122975141158922,
+            "angularVelocity": -0.8014756940104903,
+            "velocityX": 3.502862970274757,
+            "velocityY": -0.5074031064832265,
+            "timestamp": 0.5448889244372639
+        },
+        {
+            "x": 3.4545421039318374,
+            "y": 6.168639069925164,
+            "heading": -0.11367597167549683,
+            "angularVelocity": 0.19064367900402435,
+            "velocityX": 3.6709861530893746,
+            "velocityY": -0.6291046383282068,
+            "timestamp": 0.5845114273996251
+        },
+        {
+            "x": 3.5909551218669695,
+            "y": 6.134279746914322,
+            "heading": -0.10324626427429143,
+            "angularVelocity": 0.26322686911306303,
+            "velocityX": 3.442816776737095,
+            "velocityY": -0.8671668986555798,
+            "timestamp": 0.6241339303619864
+        },
+        {
+            "x": 3.7180733265010666,
+            "y": 6.090709420708735,
+            "heading": -0.09157048631886859,
+            "angularVelocity": 0.2946754263988333,
+            "velocityX": 3.208232573163052,
+            "velocityY": -1.0996358873889647,
+            "timestamp": 0.6637564333243476
+        },
+        {
+            "x": 3.835817587911086,
+            "y": 6.038004134131813,
+            "heading": -0.07918178962672723,
+            "angularVelocity": 0.31266820028784686,
+            "velocityX": 2.9716512740719234,
+            "velocityY": -1.3301856933921488,
+            "timestamp": 0.7033789362867089
+        },
+        {
+            "x": 3.9441493716247042,
+            "y": 5.9762022678896844,
+            "heading": -0.06634465187684353,
+            "angularVelocity": 0.3239860379864983,
+            "velocityX": 2.734097434897688,
+            "velocityY": -1.5597668400917784,
+            "timestamp": 0.7430014392490701
+        },
+        {
+            "x": 4.0430458841375625,
+            "y": 5.905326959185002,
+            "heading": -0.05321706206750646,
+            "angularVelocity": 0.3313165203573185,
+            "velocityX": 2.4959683290781256,
+            "velocityY": -1.7887640458253868,
+            "timestamp": 0.7826239422114314
+        },
+        {
+            "x": 4.1324920654296875,
+            "y": 5.8253936767578125,
+            "heading": -0.03990404951279715,
+            "angularVelocity": 0.33599625362778834,
+            "velocityX": 2.2574591357111773,
+            "velocityY": -2.017370848659449,
+            "timestamp": 0.8222464451737926
+        },
+        {
+            "x": 4.241428434426057,
+            "y": 5.691951873954815,
+            "heading": -0.020558340356236417,
+            "angularVelocity": 0.3400028267859621,
+            "velocityX": 1.9145678816329739,
+            "velocityY": -2.345253399462351,
+            "timestamp": 0.8791451166920127
+        },
+        {
+            "x": 4.330837596388679,
+            "y": 5.539871857431794,
+            "heading": -0.0011062192039580838,
+            "angularVelocity": 0.34187302854776463,
+            "velocityX": 1.571375211001045,
+            "velocityY": -2.6728219212345348,
+            "timestamp": 0.9360437882102327
+        },
+        {
+            "x": 4.400691025411647,
+            "y": 5.369183558238886,
+            "heading": 0.018249380247094018,
+            "angularVelocity": 0.34017664972817613,
+            "velocityX": 1.2276811946408939,
+            "velocityY": -2.999864401724217,
+            "timestamp": 0.9929424597284527
+        },
+        {
+            "x": 4.450931892836431,
+            "y": 5.179947405025881,
+            "heading": 0.037101343157351356,
+            "angularVelocity": 0.3313251857597514,
+            "velocityX": 0.8829884087662048,
+            "velocityY": -3.3258448424829754,
+            "timestamp": 1.0498411312466729
+        },
+        {
+            "x": 4.481391591894744,
+            "y": 4.9723492336445485,
+            "heading": 0.054218740493726776,
+            "angularVelocity": 0.3008400175194617,
+            "velocityX": 0.5353323416094045,
+            "velocityY": -3.6485591990465953,
+            "timestamp": 1.106739802764893
+        },
+        {
+            "x": 4.496396423991072,
+            "y": 4.780842919446315,
+            "heading": 0.0400753404119281,
+            "angularVelocity": -0.248571710101697,
+            "velocityX": 0.26371146629537756,
+            "velocityY": -3.3657431551263746,
+            "timestamp": 1.163638474283113
+        },
+        {
+            "x": 4.530633514554346,
+            "y": 4.608273372061934,
+            "heading": 0.027581622688211512,
+            "angularVelocity": -0.21957837310341866,
+            "velocityX": 0.6017203855508131,
+            "velocityY": -3.032927532044774,
+            "timestamp": 1.220537145801333
+        },
+        {
+            "x": 4.584102924369241,
+            "y": 4.454640545632531,
+            "heading": 0.016737783854959394,
+            "angularVelocity": -0.1905815820283204,
+            "velocityX": 0.9397303731032456,
+            "velocityY": -2.7001127149375717,
+            "timestamp": 1.277435817319553
+        },
+        {
+            "x": 4.656804671911905,
+            "y": 4.319944426632471,
+            "heading": 0.007543896915478472,
+            "angularVelocity": -0.1615835079126032,
+            "velocityX": 1.277740685375807,
+            "velocityY": -2.3672981355448273,
+            "timestamp": 1.334334488837773
         },
         {
             "x": 4.748738765716553,
             "y": 4.2041850090026855,
             "heading": 0,
-            "angularVelocity": -4.963056575446211e-7,
-            "velocityX": 3.27527984443961,
-            "velocityY": -1.8929291450565913,
-            "timestamp": 1.1167737858654545
-        },
-        {
-            "x": 5.133803316447558,
-            "y": 4.089523361787239,
-            "heading": -1.387645596172588e-10,
-            "angularVelocity": -1.3065520492479572e-9,
-            "velocityX": 3.625615065458012,
-            "velocityY": -1.0796085871456598,
-            "timestamp": 1.2229804690844188
-        },
-        {
-            "x": 5.534974194059096,
-            "y": 4.0675249151482635,
-            "heading": -2.526284831735191e-10,
-            "angularVelocity": -1.0720975375429818e-9,
-            "velocityX": 3.7772658504404184,
-            "velocityY": -0.20712864739049783,
-            "timestamp": 1.3291871523033831
-        },
-        {
-            "x": 5.9365934232146325,
-            "y": 4.0786604378791305,
-            "heading": -3.6352969398434843e-10,
-            "angularVelocity": -1.0442018095219078e-9,
-            "velocityX": 3.781487350730336,
-            "velocityY": 0.10484766488641149,
-            "timestamp": 1.4353938355223474
-        },
-        {
-            "x": 6.3382126442540345,
-            "y": 4.089796253283455,
-            "heading": -4.744184411388132e-10,
-            "angularVelocity": -1.044084456790862e-9,
-            "velocityX": 3.781487274312031,
-            "velocityY": 0.1048504205837075,
-            "timestamp": 1.5416005187413118
-        },
-        {
-            "x": 6.739831868633552,
-            "y": 4.100931948201603,
-            "heading": -5.853040774969199e-10,
-            "angularVelocity": -1.0440551668485128e-9,
-            "velocityX": 3.7814873057612264,
-            "velocityY": 0.10484928613381475,
-            "timestamp": 1.647807201960276
-        },
-        {
-            "x": 7.11589910333758,
-            "y": 4.111359168329562,
-            "heading": -0.0011999964635320353,
-            "angularVelocity": -0.011298685184913941,
-            "velocityX": 3.5408999067290075,
-            "velocityY": 0.09817856854143046,
-            "timestamp": 1.7540138851792404
-        },
-        {
-            "x": 7.397949547057075,
-            "y": 4.119179583905487,
-            "heading": -0.002099997192055183,
-            "angularVelocity": -0.008474049854922212,
-            "velocityX": 2.6556750966226486,
-            "velocityY": 0.0736339309252122,
-            "timestamp": 1.8602205683982047
-        },
-        {
-            "x": 7.585983181328134,
-            "y": 4.124393194553852,
-            "heading": -0.0026999988369627403,
-            "angularVelocity": -0.005649377484767236,
-            "velocityX": 1.7704501126675238,
-            "velocityY": 0.049089289773120645,
-            "timestamp": 1.966427251617169
+            "angularVelocity": -0.13258476365415262,
+            "velocityX": 1.6157511476381003,
+            "velocityY": -2.0344836626408243,
+            "timestamp": 1.391233160355993
+        },
+        {
+            "x": 5.007932614843106,
+            "y": 4.0487951090615,
+            "heading": -0.008478617739268947,
+            "angularVelocity": -0.07486216912690727,
+            "velocityX": 2.288558626731898,
+            "velocityY": -1.372019039864535,
+            "timestamp": 1.5044895332096289
+        },
+        {
+            "x": 5.344428643764395,
+            "y": 3.9673190703928256,
+            "heading": -0.018205683784992727,
+            "angularVelocity": -0.08588537492980046,
+            "velocityX": 2.971100172492302,
+            "velocityY": -0.7193947379364598,
+            "timestamp": 1.6177459060632646
+        },
+        {
+            "x": 5.761576858213634,
+            "y": 3.95566669131606,
+            "heading": -0.05529230717881331,
+            "angularVelocity": -0.3274572764373147,
+            "velocityX": 3.6832206783483272,
+            "velocityY": -0.10288497488635052,
+            "timestamp": 1.7310022789169004
+        },
+        {
+            "x": 6.1883195012502945,
+            "y": 3.9937898497766295,
+            "heading": -0.05529230006422074,
+            "angularVelocity": 6.281847442205705e-8,
+            "velocityX": 3.767934927495438,
+            "velocityY": 0.33660938894658915,
+            "timestamp": 1.8442586517705362
+        },
+        {
+            "x": 6.615062139687873,
+            "y": 4.031913059718256,
+            "heading": -0.05529229294956677,
+            "angularVelocity": 6.281901664491759e-8,
+            "velocityX": 3.7679348868877285,
+            "velocityY": 0.3366098434998829,
+            "timestamp": 1.957515024624172
+        },
+        {
+            "x": 7.0409980156903185,
+            "y": 4.069937864184792,
+            "heading": -0.05280872948360114,
+            "angularVelocity": 0.021928686248633502,
+            "velocityX": 3.7608115576232954,
+            "velocityY": 0.33574096987616325,
+            "timestamp": 2.0707713974778077
+        },
+        {
+            "x": 7.360520062907597,
+            "y": 4.098469259621894,
+            "heading": -0.032478143170072554,
+            "angularVelocity": 0.17950942451425975,
+            "velocityX": 2.821227973018394,
+            "velocityY": 0.25191867546363345,
+            "timestamp": 2.1840277703314435
+        },
+        {
+            "x": 7.573513556720189,
+            "y": 4.1174900304811795,
+            "heading": -0.013829576144119837,
+            "angularVelocity": 0.16465799280057086,
+            "velocityX": 1.88063142449255,
+            "velocityY": 0.1679443759325155,
+            "timestamp": 2.2972841431850792
         },
         {
             "x": 7.68,
             "y": 4.127,
             "heading": -0.003,
-            "angularVelocity": -0.0028246919491873987,
-            "velocityX": 0.8852250707992916,
-            "velocityY": 0.024544646034960003,
-            "timestamp": 2.072633934836133
+            "angularVelocity": 0.09562001564464007,
+            "velocityX": 0.9402247361163992,
+            "velocityY": 0.08396851567117221,
+            "timestamp": 2.410540516038715
         },
         {
             "x": 7.68,
             "y": 4.127,
             "heading": -0.003,
             "angularVelocity": 0,
-            "velocityX": 1.6661554323284825e-27,
+            "velocityX": 2.757511206578506e-35,
             "velocityY": 0,
-            "timestamp": 2.1788406180550974
+            "timestamp": 2.5237968888923508
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.1.traj
new file mode 100644
index 00000000..b2b4ba4b
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.1.traj	
@@ -0,0 +1,472 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.285270702118571,
+            "y": 6.762383358598232,
+            "heading": 0.3634342922015001,
+            "angularVelocity": -0.5809173586087107,
+            "velocityX": -0.3220883435269421,
+            "velocityY": 0.11771892098839686,
+            "timestamp": 0.04573061452687351
+        },
+        {
+            "x": 2.2599794598619596,
+            "y": 6.767831656358603,
+            "heading": 0.2777726454574198,
+            "angularVelocity": -1.8731794363656886,
+            "velocityX": -0.5530483794778248,
+            "velocityY": 0.11913895793336499,
+            "timestamp": 0.09146122905374703
+        },
+        {
+            "x": 2.2450548028766546,
+            "y": 6.7710281276739375,
+            "heading": 0.1344358774143225,
+            "angularVelocity": -3.134372225827269,
+            "velocityX": -0.3263602980128451,
+            "velocityY": 0.06989784301807915,
+            "timestamp": 0.13719184358062053
+        },
+        {
+            "x": 2.2457261085510254,
+            "y": 6.7668328285217285,
+            "heading": 0,
+            "angularVelocity": -2.939734766416754,
+            "velocityX": 0.014679568191384731,
+            "velocityY": -0.09173940030374239,
+            "timestamp": 0.18292245810749405
+        },
+        {
+            "x": 2.262817902753147,
+            "y": 6.755151488221845,
+            "heading": -0.1222613161134929,
+            "angularVelocity": -2.5936618258096975,
+            "velocityX": 0.3625867573309899,
+            "velocityY": -0.24780893395554135,
+            "timestamp": 0.230060953533885
+        },
+        {
+            "x": 2.2962801062686125,
+            "y": 6.73609161362754,
+            "heading": -0.22784823217966704,
+            "angularVelocity": -2.2399297031255614,
+            "velocityX": 0.7098699950598514,
+            "velocityY": -0.40433778002244786,
+            "timestamp": 0.277199448960276
+        },
+        {
+            "x": 2.3460880772414248,
+            "y": 6.709619977067709,
+            "heading": -0.31657680000453947,
+            "angularVelocity": -1.882295288008833,
+            "velocityX": 1.056630478386546,
+            "velocityY": -0.561571520694102,
+            "timestamp": 0.32433794438666697
+        },
+        {
+            "x": 2.4122219538028076,
+            "y": 6.675695367304005,
+            "heading": -0.3884103898720453,
+            "angularVelocity": -1.5238838070197687,
+            "velocityX": 1.4029696103613034,
+            "velocityY": -0.7196795200363996,
+            "timestamp": 0.37147643981305795
+        },
+        {
+            "x": 2.494664207392629,
+            "y": 6.634268490634927,
+            "heading": -0.4434154648490763,
+            "angularVelocity": -1.1668822791122553,
+            "velocityX": 1.7489368899896385,
+            "velocityY": -0.8788332401012526,
+            "timestamp": 0.4186149352394489
+        },
+        {
+            "x": 2.593395417678489,
+            "y": 6.585276348843886,
+            "heading": -0.4817298536594349,
+            "angularVelocity": -0.8128046613242179,
+            "velocityX": 2.094492185055806,
+            "velocityY": -1.0393234096224127,
+            "timestamp": 0.4657534306658399
+        },
+        {
+            "x": 2.7083870607833584,
+            "y": 6.52862575501488,
+            "heading": -0.5035659675601283,
+            "angularVelocity": -0.46323315377732527,
+            "velocityX": 2.4394423721997165,
+            "velocityY": -1.2017904542040352,
+            "timestamp": 0.5128919260922309
+        },
+        {
+            "x": 2.839583375891639,
+            "y": 6.464143202111409,
+            "heading": -0.5093019910748294,
+            "angularVelocity": -0.12168448446963974,
+            "velocityX": 2.78320964471958,
+            "velocityY": -1.367938291626405,
+            "timestamp": 0.5600304215186218
+        },
+        {
+            "x": 2.9868119835174247,
+            "y": 6.391337595074747,
+            "heading": -0.5000000631264199,
+            "angularVelocity": 0.197331880546237,
+            "velocityX": 3.1233200443517304,
+            "velocityY": -1.5445042608591435,
+            "timestamp": 0.6071689169450127
+        },
+        {
+            "x": 3.140901786552832,
+            "y": 6.301587367083572,
+            "heading": -0.5000000197789056,
+            "angularVelocity": 9.19577807856047e-7,
+            "velocityX": 3.2688740198760886,
+            "velocityY": -1.903968872561393,
+            "timestamp": 0.6543074123714037
+        },
+        {
+            "x": 3.284848213195801,
+            "y": 6.196334362030029,
+            "heading": -0.5,
+            "angularVelocity": 4.1959136469938736e-7,
+            "velocityX": 3.0536915813881054,
+            "velocityY": -2.2328460868682716,
+            "timestamp": 0.7014459077977946
+        },
+        {
+            "x": 3.3993231490932048,
+            "y": 6.0978407623199296,
+            "heading": -0.5170802154644099,
+            "angularVelocity": -0.41193186552987154,
+            "velocityX": 2.7608477187370504,
+            "velocityY": -2.375418059313161,
+            "timestamp": 0.7429095985042958
+        },
+        {
+            "x": 3.501098994358481,
+            "y": 5.994630951270013,
+            "heading": -0.5506635755138305,
+            "angularVelocity": -0.8099462319284754,
+            "velocityX": 2.4545775721162104,
+            "velocityY": -2.489161222534419,
+            "timestamp": 0.7843732892107971
+        },
+        {
+            "x": 3.5900181651549583,
+            "y": 5.890084132174874,
+            "heading": -0.6049860857837608,
+            "angularVelocity": -1.3101224069596595,
+            "velocityX": 2.144506899445626,
+            "velocityY": -2.521406495990601,
+            "timestamp": 0.8258369799172983
+        },
+        {
+            "x": 3.6727603343537245,
+            "y": 5.794679716077001,
+            "heading": -0.692814707704561,
+            "angularVelocity": -2.1182056016767716,
+            "velocityX": 1.9955331469267976,
+            "velocityY": -2.3009147153167167,
+            "timestamp": 0.8673006706237996
+        },
+        {
+            "x": 3.757696306555582,
+            "y": 5.694133573084931,
+            "heading": -0.7686614093789431,
+            "angularVelocity": -1.8292318021376788,
+            "velocityX": 2.04844216119069,
+            "velocityY": -2.424920244168773,
+            "timestamp": 0.9087643613303008
+        },
+        {
+            "x": 3.844342447773648,
+            "y": 5.582483853612312,
+            "heading": -0.8164085841781574,
+            "angularVelocity": -1.1515418426489148,
+            "velocityX": 2.0896871393165974,
+            "velocityY": -2.6927105998111585,
+            "timestamp": 0.950228052036802
+        },
+        {
+            "x": 3.9316331213435283,
+            "y": 5.459771926422603,
+            "heading": -0.8357045871440653,
+            "angularVelocity": -0.4653710906373953,
+            "velocityX": 2.105231639596513,
+            "velocityY": -2.9595032448602216,
+            "timestamp": 0.9916917427433033
+        },
+        {
+            "x": 4.018219294626101,
+            "y": 5.3289825292048665,
+            "heading": -0.8357080754934182,
+            "angularVelocity": -0.00008413021836472818,
+            "velocityX": 2.088240863443359,
+            "velocityY": -3.1543115190486835,
+            "timestamp": 1.0331554334498045
+        },
+        {
+            "x": 4.10449796775504,
+            "y": 5.197989757794864,
+            "heading": -0.8357107471897811,
+            "angularVelocity": -0.00006443460091393523,
+            "velocityX": 2.0808247326516955,
+            "velocityY": -3.159216393376985,
+            "timestamp": 1.0746191241563057
+        },
+        {
+            "x": 4.190712810420311,
+            "y": 5.066954954654073,
+            "heading": -0.8357133876254744,
+            "angularVelocity": -0.00006368067213797694,
+            "velocityX": 2.0792853023028366,
+            "velocityY": -3.160230093078832,
+            "timestamp": 1.1160828148628068
+        },
+        {
+            "x": 4.274456024169922,
+            "y": 4.934326648712158,
+            "heading": -0.8357152799414105,
+            "angularVelocity": -0.000045637904003859675,
+            "velocityX": 2.0196758253473424,
+            "velocityY": -3.198661375339825,
+            "timestamp": 1.157546505569308
+        },
+        {
+            "x": 4.34828200363932,
+            "y": 4.789661343281582,
+            "heading": -0.8357152360129204,
+            "angularVelocity": 0.0000010231801667591938,
+            "velocityX": 1.7195509731200054,
+            "velocityY": -3.36953696405698,
+            "timestamp": 1.2004797949125405
+        },
+        {
+            "x": 4.435451827820083,
+            "y": 4.652622129190998,
+            "heading": -0.8357151829793803,
+            "angularVelocity": 0.0000012352545290785843,
+            "velocityX": 2.0303551280191665,
+            "velocityY": -3.1919104309715474,
+            "timestamp": 1.2434130842557731
+        },
+        {
+            "x": 4.535184717372419,
+            "y": 4.524436546290902,
+            "heading": -0.8357137971440172,
+            "angularVelocity": 0.000032278807056995315,
+            "velocityX": 2.322973410097133,
+            "velocityY": -2.985692101886486,
+            "timestamp": 1.2863463735990057
+        },
+        {
+            "x": 4.644325012525673,
+            "y": 4.4080959710874374,
+            "heading": -0.8258667739389303,
+            "angularVelocity": 0.22935636555504585,
+            "velocityX": 2.542090224690803,
+            "velocityY": -2.7097987827900663,
+            "timestamp": 1.3292796629422383
+        },
+        {
+            "x": 4.760978449232698,
+            "y": 4.3048079626404245,
+            "heading": -0.804942882637706,
+            "angularVelocity": 0.4873582159975465,
+            "velocityX": 2.717085937078615,
+            "velocityY": -2.405779059257989,
+            "timestamp": 1.372212952285471
+        },
+        {
+            "x": 4.883873293679825,
+            "y": 4.215276634825419,
+            "heading": -0.7738338273486849,
+            "angularVelocity": 0.7245905395302493,
+            "velocityX": 2.8624604899158337,
+            "velocityY": -2.085359151013129,
+            "timestamp": 1.4151462416287035
+        },
+        {
+            "x": 5.012023716462111,
+            "y": 4.13998910043764,
+            "heading": -0.7341562289641704,
+            "angularVelocity": 0.9241686111518187,
+            "velocityX": 2.9848731542039757,
+            "velocityY": -1.7535934362235386,
+            "timestamp": 1.458079530971936
+        },
+        {
+            "x": 5.144586044931236,
+            "y": 4.079308119917677,
+            "heading": -0.6878803412234658,
+            "angularVelocity": 1.0778556325081365,
+            "velocityX": 3.0876350379153963,
+            "velocityY": -1.4133783236324275,
+            "timestamp": 1.5010128203151687
+        },
+        {
+            "x": 5.2807921990430025,
+            "y": 4.033507276074697,
+            "heading": -0.6371968549741135,
+            "angularVelocity": 1.1805171936433834,
+            "velocityX": 3.1725068401551706,
+            "velocityY": -1.0667909341122943,
+            "timestamp": 1.5439461096584013
+        },
+        {
+            "x": 5.419841766333411,
+            "y": 4.00280308410274,
+            "heading": -0.5848216118259447,
+            "angularVelocity": 1.2199215096111542,
+            "velocityX": 3.2387354758394924,
+            "velocityY": -0.7151604836631573,
+            "timestamp": 1.5868793990016339
+        },
+        {
+            "x": 5.561074826834354,
+            "y": 3.9873115142730846,
+            "heading": -0.5330346380339317,
+            "angularVelocity": 1.2062195695745372,
+            "velocityX": 3.289593289064067,
+            "velocityY": -0.36082885953151356,
+            "timestamp": 1.6298126883448665
+        },
+        {
+            "x": 5.703982353210449,
+            "y": 3.987083911895752,
+            "heading": -0.4835776002687649,
+            "angularVelocity": 1.1519508176925284,
+            "velocityX": 3.328594863384743,
+            "velocityY": -0.005301303040469438,
+            "timestamp": 1.672745977688099
+        },
+        {
+            "x": 5.958820715702131,
+            "y": 3.94375176713671,
+            "heading": -0.42940135113015304,
+            "angularVelocity": 0.7375353070821884,
+            "velocityX": 3.4692746900164138,
+            "velocityY": -0.5899077030899651,
+            "timestamp": 1.7462017819301305
+        },
+        {
+            "x": 6.2169360873076736,
+            "y": 3.85673688488041,
+            "heading": -0.41197233440562925,
+            "angularVelocity": 0.2372721516614769,
+            "velocityX": 3.513886673339918,
+            "velocityY": -1.184588245329031,
+            "timestamp": 1.819657586172162
+        },
+        {
+            "x": 6.462551803453154,
+            "y": 3.726776847306083,
+            "heading": -0.411972258677301,
+            "angularVelocity": 0.0000010309372967585872,
+            "velocityX": 3.343721012654018,
+            "velocityY": -1.76922761809427,
+            "timestamp": 1.8931133904141935
+        },
+        {
+            "x": 6.683975982637168,
+            "y": 3.558883929748973,
+            "heading": -0.4119722206140506,
+            "angularVelocity": 5.181789344885153e-7,
+            "velocityX": 3.0143864255360504,
+            "velocityY": -2.285631738561,
+            "timestamp": 1.966569194656225
+        },
+        {
+            "x": 6.875523396649561,
+            "y": 3.357572509103632,
+            "heading": -0.4119721861737391,
+            "angularVelocity": 4.688575916806749e-7,
+            "velocityX": 2.6076552559586523,
+            "velocityY": -2.7405788109272424,
+            "timestamp": 2.0400249988982564
+        },
+        {
+            "x": 7.060174216266159,
+            "y": 3.149917085871916,
+            "heading": -0.4119721521055279,
+            "angularVelocity": 4.637919568273184e-7,
+            "velocityX": 2.513767584766965,
+            "velocityY": -2.826943702739948,
+            "timestamp": 2.1134808031402876
+        },
+        {
+            "x": 7.236976316543012,
+            "y": 2.954068221435557,
+            "heading": -0.3758421094017394,
+            "angularVelocity": 0.4918609642437705,
+            "velocityX": 2.40691803869303,
+            "velocityY": -2.666213602277847,
+            "timestamp": 2.186936607382319
+        },
+        {
+            "x": 7.385172224991548,
+            "y": 2.787906831681346,
+            "heading": -0.27501394829018777,
+            "angularVelocity": 1.3726370863672974,
+            "velocityX": 2.0174839820722426,
+            "velocityY": -2.262059363024933,
+            "timestamp": 2.26039241162435
+        },
+        {
+            "x": 7.503381996539121,
+            "y": 2.655285343615516,
+            "heading": -0.17500745818288596,
+            "angularVelocity": 1.3614511629031412,
+            "velocityX": 1.6092638664478731,
+            "velocityY": -1.8054596152654476,
+            "timestamp": 2.3338482158663814
+        },
+        {
+            "x": 7.59180643545552,
+            "y": 2.55603678143989,
+            "heading": -0.09217156397731928,
+            "angularVelocity": 1.1276970562139184,
+            "velocityX": 1.2037774254713123,
+            "velocityY": -1.3511330139223812,
+            "timestamp": 2.4073040201084126
+        },
+        {
+            "x": 7.650633785300877,
+            "y": 2.489982671853645,
+            "heading": -0.033538816068743524,
+            "angularVelocity": 0.7982044239197745,
+            "velocityX": 0.8008536623127058,
+            "velocityY": -0.8992360817206846,
+            "timestamp": 2.480759824350444
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -0.003,
+            "angularVelocity": 0.41574408426759185,
+            "velocityX": 0.3997807253237885,
+            "velocityY": -0.44901382802875656,
+            "timestamp": 2.554215628592475
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": -0.003,
+            "angularVelocity": 5.454413556338505e-31,
+            "velocityX": 0,
+            "velocityY": -3.390988673475489e-31,
+            "timestamp": 2.6276714328345063
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.traj
index 785ed7b8..b2b4ba4b 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.traj	
@@ -4,397 +4,469 @@
             "x": 2.3,
             "y": 6.757,
             "heading": 0.39,
-            "angularVelocity": 1.592522039385819e-32,
+            "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": -2.1348488243236776e-32,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 2.311435758812473,
-            "y": 6.743296066265471,
-            "heading": 0.3325883455828011,
-            "angularVelocity": -1.0899722636212008,
-            "velocityX": 0.2171102722189281,
-            "velocityY": -0.26017204737900074,
-            "timestamp": 0.052672582902671385
-        },
-        {
-            "x": 2.3359939039892414,
-            "y": 6.71538911279917,
-            "heading": 0.2289766220305027,
-            "angularVelocity": -1.9670902363712421,
-            "velocityX": 0.46624152117519013,
-            "velocityY": -0.529819346772266,
-            "timestamp": 0.10534516580534277
-        },
-        {
-            "x": 2.3766647023789824,
-            "y": 6.674615688796359,
-            "heading": 0.08902544501665462,
-            "angularVelocity": -2.6570023587499336,
-            "velocityX": 0.7721436115045406,
-            "velocityY": -0.7740919802272123,
-            "timestamp": 0.15801774870801416
-        },
-        {
-            "x": 2.4341603545379895,
-            "y": 6.621640281380937,
-            "heading": -0.08739056422595769,
-            "angularVelocity": -3.349294823240299,
-            "velocityX": 1.0915669783129407,
-            "velocityY": -1.0057491866937662,
-            "timestamp": 0.21069033161068554
-        },
-        {
-            "x": 2.508963105088746,
-            "y": 6.555426903517877,
-            "heading": -0.2922468734649172,
-            "angularVelocity": -3.8892398654057443,
-            "velocityX": 1.4201458601143102,
-            "velocityY": -1.2570748236404126,
-            "timestamp": 0.2633629145133569
-        },
-        {
-            "x": 2.602244928931498,
-            "y": 6.475307627430128,
-            "heading": -0.4990571722352239,
-            "angularVelocity": -3.9263367652285313,
-            "velocityX": 1.7709749304513536,
-            "velocityY": -1.5210812090949455,
-            "timestamp": 0.3160354974160283
-        },
-        {
-            "x": 2.7117676530035184,
-            "y": 6.382297804514516,
-            "heading": -0.6662019146269795,
-            "angularVelocity": -3.173277883497873,
-            "velocityX": 2.0793118172009177,
-            "velocityY": -1.7658109359755114,
-            "timestamp": 0.3687080803186997
-        },
-        {
-            "x": 2.833611348488951,
-            "y": 6.27498915923685,
-            "heading": -0.7793427065081729,
-            "angularVelocity": -2.1480015910792667,
-            "velocityX": 2.3132280357425485,
-            "velocityY": -2.0372770683364947,
-            "timestamp": 0.42138066322137113
-        },
-        {
-            "x": 2.968521622075034,
-            "y": 6.1546184694434425,
-            "heading": -0.8356933529016568,
-            "angularVelocity": -1.0698288044390758,
-            "velocityX": 2.5612997531442825,
-            "velocityY": -2.2852627146807634,
-            "timestamp": 0.47405324612404254
-        },
-        {
-            "x": 3.1177887530073343,
-            "y": 6.022632523021301,
-            "heading": -0.8357129106693775,
-            "angularVelocity": -0.00037130830959681015,
-            "velocityX": 2.8338676918144654,
-            "velocityY": -2.5057807904736045,
-            "timestamp": 0.5267258290267139
-        },
-        {
-            "x": 3.269422043665646,
-            "y": 5.893362401601714,
-            "heading": -0.8357131785361996,
-            "angularVelocity": -0.000005085507627644201,
-            "velocityX": 2.8787897289658386,
-            "velocityY": -2.45422028493365,
-            "timestamp": 0.5793984119293853
-        },
-        {
-            "x": 3.4210553878779164,
-            "y": 5.764092343000596,
-            "heading": -0.8357134464026715,
-            "angularVelocity": -0.000005085500977911915,
-            "velocityX": 2.8787907456989505,
-            "velocityY": -2.4542190923119205,
-            "timestamp": 0.6320709948320568
-        },
-        {
-            "x": 3.572688732091381,
-            "y": 5.634822284400852,
-            "heading": -0.8357137142690544,
-            "angularVelocity": -0.000005085499289329294,
-            "velocityX": 2.878790745721617,
-            "velocityY": -2.4542190922857987,
-            "timestamp": 0.6847435777347282
-        },
-        {
-            "x": 3.7243220763038787,
-            "y": 5.50555222579995,
-            "heading": -0.8357139821353484,
-            "angularVelocity": -0.000005085497600843703,
-            "velocityX": 2.8787907457032484,
-            "velocityY": -2.4542190923078118,
-            "timestamp": 0.7374161606373996
-        },
-        {
-            "x": 3.875955376681643,
-            "y": 5.376282115781124,
-            "heading": -0.8357142500017658,
-            "angularVelocity": -0.000005085499943938472,
-            "velocityX": 2.878789913491687,
-            "velocityY": -2.4542200684878277,
-            "timestamp": 0.790088743540071
-        },
-        {
-            "x": 4.025639474822054,
-            "y": 5.244759916762783,
-            "heading": -0.8357145286285158,
-            "angularVelocity": -0.000005289787108985714,
-            "velocityX": 2.8417839014463553,
-            "velocityY": -2.4969764490450044,
-            "timestamp": 0.8427613264427424
-        },
-        {
-            "x": 4.159073025842643,
-            "y": 5.096777164207917,
-            "heading": -0.835714908739186,
-            "angularVelocity": -0.000007216480551859566,
-            "velocityX": 2.5332638664625207,
-            "velocityY": -2.809483499761346,
-            "timestamp": 0.8954339093454138
+            "x": 2.285270702118571,
+            "y": 6.762383358598232,
+            "heading": 0.3634342922015001,
+            "angularVelocity": -0.5809173586087107,
+            "velocityX": -0.3220883435269421,
+            "velocityY": 0.11771892098839686,
+            "timestamp": 0.04573061452687351
+        },
+        {
+            "x": 2.2599794598619596,
+            "y": 6.767831656358603,
+            "heading": 0.2777726454574198,
+            "angularVelocity": -1.8731794363656886,
+            "velocityX": -0.5530483794778248,
+            "velocityY": 0.11913895793336499,
+            "timestamp": 0.09146122905374703
+        },
+        {
+            "x": 2.2450548028766546,
+            "y": 6.7710281276739375,
+            "heading": 0.1344358774143225,
+            "angularVelocity": -3.134372225827269,
+            "velocityX": -0.3263602980128451,
+            "velocityY": 0.06989784301807915,
+            "timestamp": 0.13719184358062053
+        },
+        {
+            "x": 2.2457261085510254,
+            "y": 6.7668328285217285,
+            "heading": 0,
+            "angularVelocity": -2.939734766416754,
+            "velocityX": 0.014679568191384731,
+            "velocityY": -0.09173940030374239,
+            "timestamp": 0.18292245810749405
+        },
+        {
+            "x": 2.262817902753147,
+            "y": 6.755151488221845,
+            "heading": -0.1222613161134929,
+            "angularVelocity": -2.5936618258096975,
+            "velocityX": 0.3625867573309899,
+            "velocityY": -0.24780893395554135,
+            "timestamp": 0.230060953533885
+        },
+        {
+            "x": 2.2962801062686125,
+            "y": 6.73609161362754,
+            "heading": -0.22784823217966704,
+            "angularVelocity": -2.2399297031255614,
+            "velocityX": 0.7098699950598514,
+            "velocityY": -0.40433778002244786,
+            "timestamp": 0.277199448960276
+        },
+        {
+            "x": 2.3460880772414248,
+            "y": 6.709619977067709,
+            "heading": -0.31657680000453947,
+            "angularVelocity": -1.882295288008833,
+            "velocityX": 1.056630478386546,
+            "velocityY": -0.561571520694102,
+            "timestamp": 0.32433794438666697
+        },
+        {
+            "x": 2.4122219538028076,
+            "y": 6.675695367304005,
+            "heading": -0.3884103898720453,
+            "angularVelocity": -1.5238838070197687,
+            "velocityX": 1.4029696103613034,
+            "velocityY": -0.7196795200363996,
+            "timestamp": 0.37147643981305795
+        },
+        {
+            "x": 2.494664207392629,
+            "y": 6.634268490634927,
+            "heading": -0.4434154648490763,
+            "angularVelocity": -1.1668822791122553,
+            "velocityX": 1.7489368899896385,
+            "velocityY": -0.8788332401012526,
+            "timestamp": 0.4186149352394489
+        },
+        {
+            "x": 2.593395417678489,
+            "y": 6.585276348843886,
+            "heading": -0.4817298536594349,
+            "angularVelocity": -0.8128046613242179,
+            "velocityX": 2.094492185055806,
+            "velocityY": -1.0393234096224127,
+            "timestamp": 0.4657534306658399
+        },
+        {
+            "x": 2.7083870607833584,
+            "y": 6.52862575501488,
+            "heading": -0.5035659675601283,
+            "angularVelocity": -0.46323315377732527,
+            "velocityX": 2.4394423721997165,
+            "velocityY": -1.2017904542040352,
+            "timestamp": 0.5128919260922309
+        },
+        {
+            "x": 2.839583375891639,
+            "y": 6.464143202111409,
+            "heading": -0.5093019910748294,
+            "angularVelocity": -0.12168448446963974,
+            "velocityX": 2.78320964471958,
+            "velocityY": -1.367938291626405,
+            "timestamp": 0.5600304215186218
+        },
+        {
+            "x": 2.9868119835174247,
+            "y": 6.391337595074747,
+            "heading": -0.5000000631264199,
+            "angularVelocity": 0.197331880546237,
+            "velocityX": 3.1233200443517304,
+            "velocityY": -1.5445042608591435,
+            "timestamp": 0.6071689169450127
+        },
+        {
+            "x": 3.140901786552832,
+            "y": 6.301587367083572,
+            "heading": -0.5000000197789056,
+            "angularVelocity": 9.19577807856047e-7,
+            "velocityX": 3.2688740198760886,
+            "velocityY": -1.903968872561393,
+            "timestamp": 0.6543074123714037
+        },
+        {
+            "x": 3.284848213195801,
+            "y": 6.196334362030029,
+            "heading": -0.5,
+            "angularVelocity": 4.1959136469938736e-7,
+            "velocityX": 3.0536915813881054,
+            "velocityY": -2.2328460868682716,
+            "timestamp": 0.7014459077977946
+        },
+        {
+            "x": 3.3993231490932048,
+            "y": 6.0978407623199296,
+            "heading": -0.5170802154644099,
+            "angularVelocity": -0.41193186552987154,
+            "velocityX": 2.7608477187370504,
+            "velocityY": -2.375418059313161,
+            "timestamp": 0.7429095985042958
+        },
+        {
+            "x": 3.501098994358481,
+            "y": 5.994630951270013,
+            "heading": -0.5506635755138305,
+            "angularVelocity": -0.8099462319284754,
+            "velocityX": 2.4545775721162104,
+            "velocityY": -2.489161222534419,
+            "timestamp": 0.7843732892107971
+        },
+        {
+            "x": 3.5900181651549583,
+            "y": 5.890084132174874,
+            "heading": -0.6049860857837608,
+            "angularVelocity": -1.3101224069596595,
+            "velocityX": 2.144506899445626,
+            "velocityY": -2.521406495990601,
+            "timestamp": 0.8258369799172983
+        },
+        {
+            "x": 3.6727603343537245,
+            "y": 5.794679716077001,
+            "heading": -0.692814707704561,
+            "angularVelocity": -2.1182056016767716,
+            "velocityX": 1.9955331469267976,
+            "velocityY": -2.3009147153167167,
+            "timestamp": 0.8673006706237996
+        },
+        {
+            "x": 3.757696306555582,
+            "y": 5.694133573084931,
+            "heading": -0.7686614093789431,
+            "angularVelocity": -1.8292318021376788,
+            "velocityX": 2.04844216119069,
+            "velocityY": -2.424920244168773,
+            "timestamp": 0.9087643613303008
+        },
+        {
+            "x": 3.844342447773648,
+            "y": 5.582483853612312,
+            "heading": -0.8164085841781574,
+            "angularVelocity": -1.1515418426489148,
+            "velocityX": 2.0896871393165974,
+            "velocityY": -2.6927105998111585,
+            "timestamp": 0.950228052036802
+        },
+        {
+            "x": 3.9316331213435283,
+            "y": 5.459771926422603,
+            "heading": -0.8357045871440653,
+            "angularVelocity": -0.4653710906373953,
+            "velocityX": 2.105231639596513,
+            "velocityY": -2.9595032448602216,
+            "timestamp": 0.9916917427433033
+        },
+        {
+            "x": 4.018219294626101,
+            "y": 5.3289825292048665,
+            "heading": -0.8357080754934182,
+            "angularVelocity": -0.00008413021836472818,
+            "velocityX": 2.088240863443359,
+            "velocityY": -3.1543115190486835,
+            "timestamp": 1.0331554334498045
+        },
+        {
+            "x": 4.10449796775504,
+            "y": 5.197989757794864,
+            "heading": -0.8357107471897811,
+            "angularVelocity": -0.00006443460091393523,
+            "velocityX": 2.0808247326516955,
+            "velocityY": -3.159216393376985,
+            "timestamp": 1.0746191241563057
+        },
+        {
+            "x": 4.190712810420311,
+            "y": 5.066954954654073,
+            "heading": -0.8357133876254744,
+            "angularVelocity": -0.00006368067213797694,
+            "velocityX": 2.0792853023028366,
+            "velocityY": -3.160230093078832,
+            "timestamp": 1.1160828148628068
         },
         {
             "x": 4.274456024169922,
             "y": 4.934326648712158,
             "heading": -0.8357152799414105,
-            "angularVelocity": -0.000007047351849068254,
-            "velocityX": 2.19057034929318,
-            "velocityY": -3.0841570043363045,
-            "timestamp": 0.9481064922480852
-        },
-        {
-            "x": 4.3556906912761315,
-            "y": 4.793345794539918,
-            "heading": -0.8357152378475459,
-            "angularVelocity": 9.786625189030426e-7,
-            "velocityX": 1.8886677315079998,
-            "velocityY": -3.2777384277013035,
-            "timestamp": 0.9911181166598327
-        },
-        {
-            "x": 4.442446525481621,
-            "y": 4.655693706781043,
-            "heading": -0.8357151935315337,
-            "angularVelocity": 0.0000010303263992905167,
-            "velocityX": 2.0170322649286843,
-            "velocityY": -3.200346177143663,
-            "timestamp": 1.0341297410715804
-        },
-        {
-            "x": 4.541847641111602,
-            "y": 4.5268757636785075,
-            "heading": -0.8357148699573956,
-            "angularVelocity": 0.000007522946236810339,
-            "velocityX": 2.311029099445763,
-            "velocityY": -2.9949564766345187,
-            "timestamp": 1.077141365483328
-        },
-        {
-            "x": 4.65067034095477,
-            "y": 4.409944405402937,
-            "heading": -0.8255512517653536,
-            "angularVelocity": 0.23629933375095027,
-            "velocityX": 2.530076492843306,
-            "velocityY": -2.7185989805033612,
-            "timestamp": 1.1201529898950755
-        },
-        {
-            "x": 4.7668457664948924,
-            "y": 4.306168868140065,
-            "heading": -0.803496324956771,
-            "angularVelocity": 0.5127666557638517,
-            "velocityX": 2.701023900608423,
-            "velocityY": -2.4127323411326107,
-            "timestamp": 1.163164614306823
-        },
-        {
-            "x": 4.88912359332006,
-            "y": 4.216234980411927,
-            "heading": -0.7705722789003837,
-            "angularVelocity": 0.7654685566210554,
-            "velocityX": 2.8429018549638743,
-            "velocityY": -2.0909205117948377,
-            "timestamp": 1.2061762387185706
-        },
-        {
-            "x": 5.016560626655114,
-            "y": 4.140615450479534,
-            "heading": -0.7285529163528696,
-            "angularVelocity": 0.9769303792218019,
-            "velocityX": 2.9628509752411754,
-            "velocityY": -1.7581184381341082,
-            "timestamp": 1.2491878631303182
-        },
-        {
-            "x": 5.148349456504816,
-            "y": 4.079665503809577,
-            "heading": -0.6795461936491729,
-            "angularVelocity": 1.1393832103284065,
-            "velocityX": 3.0640281935900804,
-            "velocityY": -1.4170575397591803,
-            "timestamp": 1.2921994875420657
-        },
-        {
-            "x": 5.28369745675312,
-            "y": 4.033673765712218,
-            "heading": -0.6261187752553977,
-            "angularVelocity": 1.2421623020399764,
-            "velocityX": 3.1467772282354725,
-            "velocityY": -1.0692862389265216,
-            "timestamp": 1.3352111119538133
-        },
-        {
-            "x": 5.421794981888129,
-            "y": 4.0028549700404525,
-            "heading": -0.5712230670658915,
-            "angularVelocity": 1.2762993479156466,
-            "velocityX": 3.2107023862435504,
-            "velocityY": -0.7165224771968601,
-            "timestamp": 1.3782227363655608
-        },
-        {
-            "x": 5.5620425748748135,
-            "y": 3.9873099729843906,
-            "heading": -0.5170015662419462,
-            "angularVelocity": 1.2606243443606422,
-            "velocityX": 3.260690450658235,
-            "velocityY": -0.3614138565716755,
-            "timestamp": 1.4212343607773084
+            "angularVelocity": -0.000045637904003859675,
+            "velocityX": 2.0196758253473424,
+            "velocityY": -3.198661375339825,
+            "timestamp": 1.157546505569308
+        },
+        {
+            "x": 4.34828200363932,
+            "y": 4.789661343281582,
+            "heading": -0.8357152360129204,
+            "angularVelocity": 0.0000010231801667591938,
+            "velocityX": 1.7195509731200054,
+            "velocityY": -3.36953696405698,
+            "timestamp": 1.2004797949125405
+        },
+        {
+            "x": 4.435451827820083,
+            "y": 4.652622129190998,
+            "heading": -0.8357151829793803,
+            "angularVelocity": 0.0000012352545290785843,
+            "velocityX": 2.0303551280191665,
+            "velocityY": -3.1919104309715474,
+            "timestamp": 1.2434130842557731
+        },
+        {
+            "x": 4.535184717372419,
+            "y": 4.524436546290902,
+            "heading": -0.8357137971440172,
+            "angularVelocity": 0.000032278807056995315,
+            "velocityX": 2.322973410097133,
+            "velocityY": -2.985692101886486,
+            "timestamp": 1.2863463735990057
+        },
+        {
+            "x": 4.644325012525673,
+            "y": 4.4080959710874374,
+            "heading": -0.8258667739389303,
+            "angularVelocity": 0.22935636555504585,
+            "velocityX": 2.542090224690803,
+            "velocityY": -2.7097987827900663,
+            "timestamp": 1.3292796629422383
+        },
+        {
+            "x": 4.760978449232698,
+            "y": 4.3048079626404245,
+            "heading": -0.804942882637706,
+            "angularVelocity": 0.4873582159975465,
+            "velocityX": 2.717085937078615,
+            "velocityY": -2.405779059257989,
+            "timestamp": 1.372212952285471
+        },
+        {
+            "x": 4.883873293679825,
+            "y": 4.215276634825419,
+            "heading": -0.7738338273486849,
+            "angularVelocity": 0.7245905395302493,
+            "velocityX": 2.8624604899158337,
+            "velocityY": -2.085359151013129,
+            "timestamp": 1.4151462416287035
+        },
+        {
+            "x": 5.012023716462111,
+            "y": 4.13998910043764,
+            "heading": -0.7341562289641704,
+            "angularVelocity": 0.9241686111518187,
+            "velocityX": 2.9848731542039757,
+            "velocityY": -1.7535934362235386,
+            "timestamp": 1.458079530971936
+        },
+        {
+            "x": 5.144586044931236,
+            "y": 4.079308119917677,
+            "heading": -0.6878803412234658,
+            "angularVelocity": 1.0778556325081365,
+            "velocityX": 3.0876350379153963,
+            "velocityY": -1.4133783236324275,
+            "timestamp": 1.5010128203151687
+        },
+        {
+            "x": 5.2807921990430025,
+            "y": 4.033507276074697,
+            "heading": -0.6371968549741135,
+            "angularVelocity": 1.1805171936433834,
+            "velocityX": 3.1725068401551706,
+            "velocityY": -1.0667909341122943,
+            "timestamp": 1.5439461096584013
+        },
+        {
+            "x": 5.419841766333411,
+            "y": 4.00280308410274,
+            "heading": -0.5848216118259447,
+            "angularVelocity": 1.2199215096111542,
+            "velocityX": 3.2387354758394924,
+            "velocityY": -0.7151604836631573,
+            "timestamp": 1.5868793990016339
+        },
+        {
+            "x": 5.561074826834354,
+            "y": 3.9873115142730846,
+            "heading": -0.5330346380339317,
+            "angularVelocity": 1.2062195695745372,
+            "velocityX": 3.289593289064067,
+            "velocityY": -0.36082885953151356,
+            "timestamp": 1.6298126883448665
         },
         {
             "x": 5.703982353210449,
             "y": 3.987083911895752,
-            "heading": -0.46507695218435896,
-            "angularVelocity": 1.207222809362328,
-            "velocityX": 3.3000329626441363,
-            "velocityY": -0.005255813788261737,
-            "timestamp": 1.464245985189056
-        },
-        {
-            "x": 5.957415351644791,
-            "y": 3.94389686592464,
-            "heading": -0.40627705964587574,
-            "angularVelocity": 0.8004851872173009,
-            "velocityX": 3.450165509502976,
-            "velocityY": -0.5879362884366123,
-            "timestamp": 1.5377013013865826
-        },
-        {
-            "x": 6.214659603999478,
-            "y": 3.857155103461619,
-            "heading": -0.3855287596647175,
-            "angularVelocity": 0.2824615161326723,
-            "velocityX": 3.502050847660077,
-            "velocityY": -1.180877939858924,
-            "timestamp": 1.6111566175841092
-        },
-        {
-            "x": 6.460339332530576,
-            "y": 3.727320070546642,
-            "heading": -0.38552868222223635,
-            "angularVelocity": 0.0000010542801416145007,
-            "velocityX": 3.3446146752734305,
-            "velocityY": -1.7675375947717866,
-            "timestamp": 1.6846119337816359
-        },
-        {
-            "x": 6.681847040612612,
-            "y": 3.5595404285567858,
-            "heading": -0.3855286464284339,
-            "angularVelocity": 4.872867523834452e-7,
-            "velocityX": 3.0155435923301614,
-            "velocityY": -2.2841048228379357,
-            "timestamp": 1.7580672499791625
-        },
-        {
-            "x": 6.873376982747171,
-            "y": 3.358214932798401,
-            "heading": -0.3855286139444744,
-            "angularVelocity": 4.422274806376815e-7,
-            "velocityX": 2.607434724254966,
-            "velocityY": -2.740788634235891,
-            "timestamp": 1.8315225661766892
-        },
-        {
-            "x": 7.058369006072979,
-            "y": 3.1508658904491242,
-            "heading": -0.3855285817822777,
-            "angularVelocity": 4.3784709334515964e-7,
-            "velocityX": 2.518429337753462,
-            "velocityY": -2.8227915021385286,
-            "timestamp": 1.9049778823742158
-        },
-        {
-            "x": 7.235640342207717,
-            "y": 2.9543486280986375,
-            "heading": -0.3517392740895451,
-            "angularVelocity": 0.4599981245995971,
-            "velocityX": 2.413322075396734,
-            "velocityY": -2.6753306979448217,
-            "timestamp": 1.9784331985717425
-        },
-        {
-            "x": 7.3842205894533075,
-            "y": 2.7881464841241645,
-            "heading": -0.2579536451103976,
-            "angularVelocity": 1.2767711560447332,
-            "velocityX": 2.0227296666458727,
-            "velocityY": -2.2626292088586646,
-            "timestamp": 2.051888514769269
-        },
-        {
-            "x": 7.502781206912582,
-            "y": 2.6554541566341396,
-            "heading": -0.1644602811973769,
-            "angularVelocity": 1.2727923416953275,
-            "velocityX": 1.6140508760517314,
-            "velocityY": -1.806435998903144,
-            "timestamp": 2.125343830966796
-        },
-        {
-            "x": 7.591494122264987,
-            "y": 2.5561305844890163,
-            "heading": -0.0867965293639426,
-            "angularVelocity": 1.0572924582421084,
-            "velocityX": 1.2077126604948574,
-            "velocityY": -1.3521631555983635,
-            "timestamp": 2.1987991471643227
-        },
-        {
-            "x": 7.6505265885867635,
-            "y": 2.4900164938031613,
-            "heading": -0.031723308715129774,
-            "angularVelocity": 0.7497513250193758,
-            "velocityX": 0.8036513812429008,
-            "velocityY": -0.900058622143418,
-            "timestamp": 2.2722544633618496
+            "heading": -0.4835776002687649,
+            "angularVelocity": 1.1519508176925284,
+            "velocityX": 3.328594863384743,
+            "velocityY": -0.005301303040469438,
+            "timestamp": 1.672745977688099
+        },
+        {
+            "x": 5.958820715702131,
+            "y": 3.94375176713671,
+            "heading": -0.42940135113015304,
+            "angularVelocity": 0.7375353070821884,
+            "velocityX": 3.4692746900164138,
+            "velocityY": -0.5899077030899651,
+            "timestamp": 1.7462017819301305
+        },
+        {
+            "x": 6.2169360873076736,
+            "y": 3.85673688488041,
+            "heading": -0.41197233440562925,
+            "angularVelocity": 0.2372721516614769,
+            "velocityX": 3.513886673339918,
+            "velocityY": -1.184588245329031,
+            "timestamp": 1.819657586172162
+        },
+        {
+            "x": 6.462551803453154,
+            "y": 3.726776847306083,
+            "heading": -0.411972258677301,
+            "angularVelocity": 0.0000010309372967585872,
+            "velocityX": 3.343721012654018,
+            "velocityY": -1.76922761809427,
+            "timestamp": 1.8931133904141935
+        },
+        {
+            "x": 6.683975982637168,
+            "y": 3.558883929748973,
+            "heading": -0.4119722206140506,
+            "angularVelocity": 5.181789344885153e-7,
+            "velocityX": 3.0143864255360504,
+            "velocityY": -2.285631738561,
+            "timestamp": 1.966569194656225
+        },
+        {
+            "x": 6.875523396649561,
+            "y": 3.357572509103632,
+            "heading": -0.4119721861737391,
+            "angularVelocity": 4.688575916806749e-7,
+            "velocityX": 2.6076552559586523,
+            "velocityY": -2.7405788109272424,
+            "timestamp": 2.0400249988982564
+        },
+        {
+            "x": 7.060174216266159,
+            "y": 3.149917085871916,
+            "heading": -0.4119721521055279,
+            "angularVelocity": 4.637919568273184e-7,
+            "velocityX": 2.513767584766965,
+            "velocityY": -2.826943702739948,
+            "timestamp": 2.1134808031402876
+        },
+        {
+            "x": 7.236976316543012,
+            "y": 2.954068221435557,
+            "heading": -0.3758421094017394,
+            "angularVelocity": 0.4918609642437705,
+            "velocityX": 2.40691803869303,
+            "velocityY": -2.666213602277847,
+            "timestamp": 2.186936607382319
+        },
+        {
+            "x": 7.385172224991548,
+            "y": 2.787906831681346,
+            "heading": -0.27501394829018777,
+            "angularVelocity": 1.3726370863672974,
+            "velocityX": 2.0174839820722426,
+            "velocityY": -2.262059363024933,
+            "timestamp": 2.26039241162435
+        },
+        {
+            "x": 7.503381996539121,
+            "y": 2.655285343615516,
+            "heading": -0.17500745818288596,
+            "angularVelocity": 1.3614511629031412,
+            "velocityX": 1.6092638664478731,
+            "velocityY": -1.8054596152654476,
+            "timestamp": 2.3338482158663814
+        },
+        {
+            "x": 7.59180643545552,
+            "y": 2.55603678143989,
+            "heading": -0.09217156397731928,
+            "angularVelocity": 1.1276970562139184,
+            "velocityX": 1.2037774254713123,
+            "velocityY": -1.3511330139223812,
+            "timestamp": 2.4073040201084126
+        },
+        {
+            "x": 7.650633785300877,
+            "y": 2.489982671853645,
+            "heading": -0.033538816068743524,
+            "angularVelocity": 0.7982044239197745,
+            "velocityX": 0.8008536623127058,
+            "velocityY": -0.8992360817206846,
+            "timestamp": 2.480759824350444
         },
         {
             "x": 7.68,
             "y": 2.457,
             "heading": -0.003,
-            "angularVelocity": 0.39103104039319253,
-            "velocityX": 0.401242727401519,
-            "velocityY": -0.4494772538229548,
-            "timestamp": 2.3457097795593764
+            "angularVelocity": 0.41574408426759185,
+            "velocityX": 0.3997807253237885,
+            "velocityY": -0.44901382802875656,
+            "timestamp": 2.554215628592475
         },
         {
             "x": 7.68,
             "y": 2.457,
             "heading": -0.003,
-            "angularVelocity": -1.5819357203294863e-32,
-            "velocityX": 5.378736393413154e-34,
-            "velocityY": 0,
-            "timestamp": 2.4191650957569033
+            "angularVelocity": 5.454413556338505e-31,
+            "velocityX": 0,
+            "velocityY": -3.390988673475489e-31,
+            "timestamp": 2.6276714328345063
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.1.traj
new file mode 100644
index 00000000..95620590
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.1.traj	
@@ -0,0 +1,616 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 4.884595084783414e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 2.298507420370097,
+            "y": 6.738728491162364,
+            "heading": 0.38429180377773414,
+            "angularVelocity": -0.12150700609071861,
+            "velocityX": -0.031771662206367984,
+            "velocityY": -0.3889348314553929,
+            "timestamp": 0.04697832994094238
+        },
+        {
+            "x": 2.295523042319471,
+            "y": 6.702182630298916,
+            "heading": 0.3729896107436911,
+            "angularVelocity": -0.2405831166891495,
+            "velocityX": -0.06352669527371438,
+            "velocityY": -0.7779301841804652,
+            "timestamp": 0.09395665988188476
+        },
+        {
+            "x": 2.2910478858134478,
+            "y": 6.647358841243747,
+            "heading": 0.35624034482133005,
+            "angularVelocity": -0.3565317443897418,
+            "velocityX": -0.09526001694076108,
+            "velocityY": -1.1670016606398963,
+            "timestamp": 0.14093498982282715
+        },
+        {
+            "x": 2.285083332518943,
+            "y": 6.574252474793301,
+            "heading": 0.33424119989038353,
+            "angularVelocity": -0.46828282228427764,
+            "velocityX": -0.12696392787914923,
+            "velocityY": -1.556172101953168,
+            "timestamp": 0.18791331976376952
+        },
+        {
+            "x": 2.2776313636635503,
+            "y": 6.48285724279221,
+            "heading": 0.3072714314404645,
+            "angularVelocity": -0.5740895532860236,
+            "velocityX": -0.15862566559434335,
+            "velocityY": -1.9454763955207768,
+            "timestamp": 0.23489164970471188
+        },
+        {
+            "x": 2.268695072443392,
+            "y": 6.373164221156064,
+            "heading": 0.2757564747192681,
+            "angularVelocity": -0.6708402951065854,
+            "velocityX": -0.1902215602681591,
+            "velocityY": -2.334970650809517,
+            "timestamp": 0.28186997964565425
+        },
+        {
+            "x": 2.2582799597491072,
+            "y": 6.245160021300657,
+            "heading": 0.24041955506127732,
+            "angularVelocity": -0.752196165815466,
+            "velocityX": -0.22170036072754723,
+            "velocityY": -2.7247499009931837,
+            "timestamp": 0.3288483095865966
+        },
+        {
+            "x": 2.2463982479830333,
+            "y": 6.098824071496616,
+            "heading": 0.20274918029785605,
+            "angularVelocity": -0.8018670482917891,
+            "velocityX": -0.25291899011757896,
+            "velocityY": -3.114967049446051,
+            "timestamp": 0.375826639527539
+        },
+        {
+            "x": 2.2330953714575887,
+            "y": 5.934154199517091,
+            "heading": 0.1675471140087416,
+            "angularVelocity": -0.7493256216934009,
+            "velocityX": -0.28317048609790474,
+            "velocityY": -3.505230436810659,
+            "timestamp": 0.42280496946848134
+        },
+        {
+            "x": 2.218530566609672,
+            "y": 5.757035827571947,
+            "heading": 0.16754710135001605,
+            "angularVelocity": -2.6945882371217095e-7,
+            "velocityX": -0.3100324099691601,
+            "velocityY": -3.770214313020578,
+            "timestamp": 0.4697832994094237
+        },
+        {
+            "x": 2.203965673260931,
+            "y": 5.579917462904045,
+            "heading": 0.16754708869211127,
+            "angularVelocity": -2.694413527531555e-7,
+            "velocityX": -0.3100342938339967,
+            "velocityY": -3.7702141581142206,
+            "timestamp": 0.5167616293503661
+        },
+        {
+            "x": 2.1894007799094983,
+            "y": 5.402799098236364,
+            "heading": 0.16754707603420654,
+            "angularVelocity": -2.694413521606316e-7,
+            "velocityX": -0.31003429389130516,
+            "velocityY": -3.7702141581095083,
+            "timestamp": 0.5637399592913085
+        },
+        {
+            "x": 2.1748358865580655,
+            "y": 5.225680733568683,
+            "heading": 0.1675470633763018,
+            "angularVelocity": -2.6944135250249435e-7,
+            "velocityX": -0.3100342938913069,
+            "velocityY": -3.770214158109508,
+            "timestamp": 0.6107182892322509
+        },
+        {
+            "x": 2.160270993206632,
+            "y": 5.048562368901002,
+            "heading": 0.16754705071839698,
+            "angularVelocity": -2.694413536359467e-7,
+            "velocityX": -0.31003429389130693,
+            "velocityY": -3.7702141581095083,
+            "timestamp": 0.6576966191731933
+        },
+        {
+            "x": 2.1457060998551993,
+            "y": 4.871444004233322,
+            "heading": 0.1675470380604922,
+            "angularVelocity": -2.6944135247689087e-7,
+            "velocityX": -0.3100342938913068,
+            "velocityY": -3.7702141581095083,
+            "timestamp": 0.7046749491141358
+        },
+        {
+            "x": 2.131141206503766,
+            "y": 4.6943256395656405,
+            "heading": 0.16754702540258745,
+            "angularVelocity": -2.694413528693067e-7,
+            "velocityX": -0.31003429389130693,
+            "velocityY": -3.7702141581095083,
+            "timestamp": 0.7516532790550782
+        },
+        {
+            "x": 2.1165763131523327,
+            "y": 4.51720727489796,
+            "heading": 0.16754701274468267,
+            "angularVelocity": -2.694413529636153e-7,
+            "velocityX": -0.31003429389130693,
+            "velocityY": -3.7702141581095083,
+            "timestamp": 0.7986316089960206
+        },
+        {
+            "x": 2.1020114198009,
+            "y": 4.340088910230279,
+            "heading": 0.16754700008677786,
+            "angularVelocity": -2.694413530501801e-7,
+            "velocityX": -0.3100342938913069,
+            "velocityY": -3.770214158109508,
+            "timestamp": 0.845609938936963
+        },
+        {
+            "x": 2.0874465264494666,
+            "y": 4.162970545562599,
+            "heading": 0.16754698742887306,
+            "angularVelocity": -2.694413534833812e-7,
+            "velocityX": -0.31003429389130704,
+            "velocityY": -3.7702141581095083,
+            "timestamp": 0.8925882688779054
+        },
+        {
+            "x": 2.0728816330980333,
+            "y": 3.9858521808949186,
+            "heading": 0.16754697477096828,
+            "angularVelocity": -2.6944135303696276e-7,
+            "velocityX": -0.31003429389130693,
+            "velocityY": -3.770214158109508,
+            "timestamp": 0.9395665988188479
+        },
+        {
+            "x": 2.0583167397466005,
+            "y": 3.8087338162272375,
+            "heading": 0.16754696211306347,
+            "angularVelocity": -2.694413537071192e-7,
+            "velocityX": -0.31003429389130704,
+            "velocityY": -3.7702141581095083,
+            "timestamp": 0.9865449287597903
+        },
+        {
+            "x": 2.043751846395167,
+            "y": 3.631615451559557,
+            "heading": 0.1675469494551587,
+            "angularVelocity": -2.694413527332354e-7,
+            "velocityX": -0.31003429389130716,
+            "velocityY": -3.770214158109508,
+            "timestamp": 1.0335232587007326
+        },
+        {
+            "x": 2.0291869530437365,
+            "y": 3.4544970868918763,
+            "heading": 0.16754693679725388,
+            "angularVelocity": -2.694413532802898e-7,
+            "velocityX": -0.31003429389125564,
+            "velocityY": -3.7702141581095123,
+            "timestamp": 1.080501588641675
+        },
+        {
+            "x": 2.0146220597715567,
+            "y": 3.2773787222176782,
+            "heading": 0.1675469241393491,
+            "angularVelocity": -2.6944135307750017e-7,
+            "velocityX": -0.31003429220428774,
+            "velocityY": -3.7702141582482356,
+            "timestamp": 1.1274799185826172
+        },
+        {
+            "x": 2.0000597716588953,
+            "y": 3.1002601433334096,
+            "heading": 0.16754691148142445,
+            "angularVelocity": -2.6944177624221033e-7,
+            "velocityX": -0.30997883771037654,
+            "velocityY": -3.770218718011654,
+            "timestamp": 1.1744582485235595
+        },
+        {
+            "x": 2.001608158601577,
+            "y": 2.922550679334096,
+            "heading": 0.16754689857982888,
+            "angularVelocity": -2.7462865480192305e-7,
+            "velocityX": 0.032959599556405594,
+            "velocityY": -3.7827965409310345,
+            "timestamp": 1.2214365784645018
+        },
+        {
+            "x": 2.021524410839774,
+            "y": 2.7459539818451146,
+            "heading": 0.1675468848578902,
+            "angularVelocity": -2.9209081516381327e-7,
+            "velocityX": 0.4239455140962882,
+            "velocityY": -3.7591097365739743,
+            "timestamp": 1.2684149084054441
+        },
+        {
+            "x": 2.0595952756969864,
+            "y": 2.572363489269638,
+            "heading": 0.16754686955661854,
+            "angularVelocity": -3.257091448717022e-7,
+            "velocityX": 0.8103920446953395,
+            "velocityY": -3.6951184257444134,
+            "timestamp": 1.3153932383463864
+        },
+        {
+            "x": 2.1154126290237643,
+            "y": 2.4036404273247642,
+            "heading": 0.16754685154625998,
+            "angularVelocity": -3.8337587986161977e-7,
+            "velocityX": 1.1881510772508834,
+            "velocityY": -3.5915083008906312,
+            "timestamp": 1.3623715682873287
+        },
+        {
+            "x": 2.1883780268761943,
+            "y": 2.2415938375833533,
+            "heading": 0.1675468289010436,
+            "angularVelocity": -4.82035363826859e-7,
+            "velocityX": 1.5531713865554018,
+            "velocityY": -3.449390175111029,
+            "timestamp": 1.409349898228271
+        },
+        {
+            "x": 2.2777091497247044,
+            "y": 2.0879611790897656,
+            "heading": 0.16754679784052873,
+            "angularVelocity": -6.611668595012802e-7,
+            "velocityX": 1.9015389214731706,
+            "velocityY": -3.270287783467881,
+            "timestamp": 1.4563282281692134
+        },
+        {
+            "x": 2.382448196411133,
+            "y": 1.9443897008895872,
+            "heading": 0.16754674836915184,
+            "angularVelocity": -0.0000010530680204285256,
+            "velocityX": 2.2295183080815923,
+            "velocityY": -3.0561213729961265,
+            "timestamp": 1.5033065581101557
+        },
+        {
+            "x": 2.5069799221636964,
+            "y": 1.8074962230648766,
+            "heading": 0.1675467031876573,
+            "angularVelocity": -9.235757728412815e-7,
+            "velocityX": 2.545610454081449,
+            "velocityY": -2.798302730812182,
+            "timestamp": 1.55222673973217
+        },
+        {
+            "x": 2.645527049823341,
+            "y": 1.6848068830394283,
+            "heading": 0.16754667440242366,
+            "angularVelocity": -5.884122397185895e-7,
+            "velocityX": 2.8321057499364835,
+            "velocityY": -2.5079493975189484,
+            "timestamp": 1.6011469213541845
+        },
+        {
+            "x": 2.7964787325047507,
+            "y": 1.577748144471377,
+            "heading": 0.1675466534117406,
+            "angularVelocity": -4.2908023560369677e-7,
+            "velocityX": 3.0856729815058617,
+            "velocityY": -2.1884370625450487,
+            "timestamp": 1.6500671029761989
+        },
+        {
+            "x": 2.958079900807193,
+            "y": 1.4875647290195357,
+            "heading": 0.1675466366326211,
+            "angularVelocity": -3.429897224036844e-7,
+            "velocityX": 3.303364029820377,
+            "velocityY": -1.8434807979383812,
+            "timestamp": 1.6989872845982132
+        },
+        {
+            "x": 3.1284516660647324,
+            "y": 1.4153051353233332,
+            "heading": 0.16754662225394246,
+            "angularVelocity": -2.9392120351406106e-7,
+            "velocityX": 3.4826478481607013,
+            "velocityY": -1.4770916889581902,
+            "timestamp": 1.7479074662202276
+        },
+        {
+            "x": 3.3056131601804433,
+            "y": 1.3618094062641966,
+            "heading": 0.16754660920712522,
+            "angularVelocity": -2.666960100615425e-7,
+            "velocityX": 3.6214398279336546,
+            "velocityY": -1.0935308759169382,
+            "timestamp": 1.796827647842242
+        },
+        {
+            "x": 3.487504529935422,
+            "y": 1.3276990268102373,
+            "heading": 0.16754659675699374,
+            "angularVelocity": -2.544988811687778e-7,
+            "velocityX": 3.7181253978240782,
+            "velocityY": -0.6972660019440657,
+            "timestamp": 1.8457478294642564
+        },
+        {
+            "x": 3.671043389503124,
+            "y": 1.304003553111977,
+            "heading": 0.16754658441216208,
+            "angularVelocity": -2.5234639867690264e-7,
+            "velocityX": 3.751802497092686,
+            "velocityY": -0.48437010887133,
+            "timestamp": 1.8946680110862708
+        },
+        {
+            "x": 3.8545823032662914,
+            "y": 1.2803084992011935,
+            "heading": 0.16754657206733162,
+            "angularVelocity": -2.523463739841797e-7,
+            "velocityX": 3.751803604927206,
+            "velocityY": -0.4843615278018566,
+            "timestamp": 1.9435881927082852
+        },
+        {
+            "x": 4.038121217030901,
+            "y": 1.2566134453015836,
+            "heading": 0.16754655972250113,
+            "angularVelocity": -2.5234637491728997e-7,
+            "velocityX": 3.7518036049566934,
+            "velocityY": -0.48436152757345374,
+            "timestamp": 1.9925083743302996
+        },
+        {
+            "x": 4.221660130795511,
+            "y": 1.232918391401974,
+            "heading": 0.16754654737767066,
+            "angularVelocity": -2.5234637402611237e-7,
+            "velocityX": 3.7518036049566943,
+            "velocityY": -0.4843615275734478,
+            "timestamp": 2.041428555952314
+        },
+        {
+            "x": 4.4051990445601215,
+            "y": 1.2092233375023644,
+            "heading": 0.16754653503284023,
+            "angularVelocity": -2.5234637391390173e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.4843615275734475,
+            "timestamp": 2.090348737574329
+        },
+        {
+            "x": 4.588737958324731,
+            "y": 1.1855282836027548,
+            "heading": 0.16754652268800974,
+            "angularVelocity": -2.523463747404588e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.4843615275734478,
+            "timestamp": 2.1392689191963434
+        },
+        {
+            "x": 4.77227687208934,
+            "y": 1.1618332297031453,
+            "heading": 0.16754651034317924,
+            "angularVelocity": -2.523463747849071e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.48436152757344797,
+            "timestamp": 2.188189100818358
+        },
+        {
+            "x": 4.95581578585395,
+            "y": 1.1381381758035356,
+            "heading": 0.16754649799834875,
+            "angularVelocity": -2.52346374599026e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.4843615275734478,
+            "timestamp": 2.2371092824403727
+        },
+        {
+            "x": 5.1393546996185595,
+            "y": 1.114443121903926,
+            "heading": 0.1675464856535183,
+            "angularVelocity": -2.523463743979587e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.48436152757344764,
+            "timestamp": 2.2860294640623873
+        },
+        {
+            "x": 5.32289361338317,
+            "y": 1.0907480680043165,
+            "heading": 0.16754647330868783,
+            "angularVelocity": -2.523463747332428e-7,
+            "velocityX": 3.7518036049566943,
+            "velocityY": -0.48436152757344797,
+            "timestamp": 2.334949645684402
+        },
+        {
+            "x": 5.506432527147779,
+            "y": 1.0670530141047068,
+            "heading": 0.1675464609638573,
+            "angularVelocity": -2.5234637523589347e-7,
+            "velocityX": 3.7518036049566943,
+            "velocityY": -0.48436152757344764,
+            "timestamp": 2.3838698273064165
+        },
+        {
+            "x": 5.689971440912389,
+            "y": 1.043357960205097,
+            "heading": 0.16754644861902682,
+            "angularVelocity": -2.523463745889681e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.4843615275734477,
+            "timestamp": 2.432790008928431
+        },
+        {
+            "x": 5.8735103546769984,
+            "y": 1.0196629063054876,
+            "heading": 0.16754643627419633,
+            "angularVelocity": -2.5234637515727394e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.4843615275734478,
+            "timestamp": 2.4817101905504457
+        },
+        {
+            "x": 6.057049268441609,
+            "y": 0.995967852405878,
+            "heading": 0.16754642392936583,
+            "angularVelocity": -2.523463750187176e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.4843615275734476,
+            "timestamp": 2.5306303721724603
+        },
+        {
+            "x": 6.240588182206218,
+            "y": 0.9722727985062686,
+            "heading": 0.16754641158453532,
+            "angularVelocity": -2.523463753175626e-7,
+            "velocityX": 3.751803604956694,
+            "velocityY": -0.48436152757344764,
+            "timestamp": 2.579550553794475
+        },
+        {
+            "x": 6.424127095970827,
+            "y": 0.948577744606659,
+            "heading": 0.1675463992397048,
+            "angularVelocity": -2.5234637555305e-7,
+            "velocityX": 3.7518036049566934,
+            "velocityY": -0.4843615275734476,
+            "timestamp": 2.6284707354164896
+        },
+        {
+            "x": 6.607666009735459,
+            "y": 0.9248826907072107,
+            "heading": 0.1675463868948743,
+            "angularVelocity": -2.5234637524939476e-7,
+            "velocityX": 3.7518036049571197,
+            "velocityY": -0.48436152757015266,
+            "timestamp": 2.677390917038504
+        },
+        {
+            "x": 6.79120492428193,
+            "y": 0.9011876428641385,
+            "heading": 0.16754637454996243,
+            "angularVelocity": -2.523480385625403e-7,
+            "velocityX": 3.7518036209390857,
+            "velocityY": -0.4843614037689742,
+            "timestamp": 2.726311098660519
+        },
+        {
+            "x": 6.9688913787459,
+            "y": 0.8784350451744124,
+            "heading": 0.1510406339197301,
+            "angularVelocity": -0.33740145851798203,
+            "velocityX": 3.6321707845828914,
+            "velocityY": -0.4650963454209109,
+            "timestamp": 2.7752312802825334
+        },
+        {
+            "x": 7.1269915101358645,
+            "y": 0.8581344466937117,
+            "heading": 0.12402265057850995,
+            "angularVelocity": -0.5522870611964742,
+            "velocityX": 3.2317977192222487,
+            "velocityY": -0.4149738984526803,
+            "timestamp": 2.824151461904548
+        },
+        {
+            "x": 7.265299167873697,
+            "y": 0.8403581253912503,
+            "heading": 0.0960275261585182,
+            "angularVelocity": -0.5722612527545016,
+            "velocityX": 2.8272106347943935,
+            "velocityY": -0.3633739841730647,
+            "timestamp": 2.8730716435265626
+        },
+        {
+            "x": 7.383819928892336,
+            "y": 0.8251162922066029,
+            "heading": 0.06979646830524168,
+            "angularVelocity": -0.5362011542792869,
+            "velocityX": 2.4227375510254454,
+            "velocityY": -0.3115653433671744,
+            "timestamp": 2.9219918251485772
+        },
+        {
+            "x": 7.482566860925546,
+            "y": 0.8124124441531555,
+            "heading": 0.04664460944238867,
+            "angularVelocity": -0.47325782724474763,
+            "velocityX": 2.01853159083065,
+            "velocityY": -0.2596852184974389,
+            "timestamp": 2.970912006770592
+        },
+        {
+            "x": 7.5615507925764724,
+            "y": 0.8022481812485672,
+            "heading": 0.027346506217350703,
+            "angularVelocity": -0.3944814304686405,
+            "velocityX": 1.6145469831081642,
+            "velocityY": -0.20777238692863387,
+            "timestamp": 3.0198321883926065
+        },
+        {
+            "x": 7.620780133649161,
+            "y": 0.7946243691967044,
+            "heading": 0.012413272398069258,
+            "angularVelocity": -0.3052571213791523,
+            "velocityX": 1.2107342840696753,
+            "velocityY": -0.15584185910773468,
+            "timestamp": 3.068752370014621
+        },
+        {
+            "x": 7.660261458521305,
+            "y": 0.7895415301241143,
+            "heading": 0.002207571288741933,
+            "angularVelocity": -0.2086194443876447,
+            "velocityX": 0.8070559749184711,
+            "velocityY": -0.10390065825721975,
+            "timestamp": 3.1176725516366357
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -0.003,
+            "angularVelocity": -0.10645036702804238,
+            "velocityX": 0.40348463199107626,
+            "velocityY": -0.05195258970523994,
+            "timestamp": 3.1665927332586503
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -0.003,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -1.0902826040652685e-32,
+            "timestamp": 3.215512914880665
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.1.traj
new file mode 100644
index 00000000..5360be44
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3161416084367663,
+            "y": 6.743433917099698,
+            "heading": 0.3883998707887416,
+            "angularVelocity": -0.031816354442463515,
+            "velocityX": 0.32095354030299916,
+            "velocityY": -0.26974278009236136,
+            "timestamp": 0.05029266360959302
+        },
+        {
+            "x": 2.3488347297971495,
+            "y": 6.716804087923855,
+            "heading": 0.385297889956982,
+            "angularVelocity": -0.06167859503006947,
+            "velocityX": 0.6500574639309191,
+            "velocityY": -0.5294972917434406,
+            "timestamp": 0.10058532721918605
+        },
+        {
+            "x": 2.398581891811533,
+            "y": 6.677772540045158,
+            "heading": 0.38082205724745694,
+            "angularVelocity": -0.08899573791258333,
+            "velocityX": 0.9891534558709463,
+            "velocityY": -0.7760883015003333,
+            "timestamp": 0.15087799082877906
+        },
+        {
+            "x": 2.4660071913042128,
+            "y": 6.627244851321165,
+            "heading": 0.3751449326788243,
+            "angularVelocity": -0.11288176368431184,
+            "velocityX": 1.3406587492776654,
+            "velocityY": -1.004673149074478,
+            "timestamp": 0.2011706544383721
+        },
+        {
+            "x": 2.551890499441383,
+            "y": 6.566518101686868,
+            "heading": 0.3685091874361843,
+            "angularVelocity": -0.13194260884949985,
+            "velocityX": 1.7076707013145411,
+            "velocityY": -1.207467357579225,
+            "timestamp": 0.2514633180479651
+        },
+        {
+            "x": 2.6571889825280843,
+            "y": 6.497556440570772,
+            "heading": 0.3612736159712354,
+            "angularVelocity": -0.14386932299145247,
+            "velocityX": 2.093714580402863,
+            "velocityY": -1.3712071735039586,
+            "timestamp": 0.3017559816575581
+        },
+        {
+            "x": 2.7829575448660773,
+            "y": 6.423518691483174,
+            "heading": 0.3539965965907385,
+            "angularVelocity": -0.144693457419283,
+            "velocityX": 2.50073377131696,
+            "velocityY": -1.4721381564184353,
+            "timestamp": 0.35204864526715113
+        },
+        {
+            "x": 2.929814044412336,
+            "y": 6.349664492887633,
+            "heading": 0.3475640372215199,
+            "angularVelocity": -0.1279025390095239,
+            "velocityX": 2.9200382124570217,
+            "velocityY": -1.468488508957291,
+            "timestamp": 0.40234130887674413
+        },
+        {
+            "x": 3.096125602722168,
+            "y": 6.283931255340576,
+            "heading": 0.3432166982946709,
+            "angularVelocity": -0.08644081690714242,
+            "velocityX": 3.3068751259797926,
+            "velocityY": -1.3070144396670602,
+            "timestamp": 0.45263397248633713
+        },
+        {
+            "x": 3.3513045644934745,
+            "y": 6.2237675636491945,
+            "heading": 0.3432161755192605,
+            "angularVelocity": -0.0000075431393477149555,
+            "velocityX": 3.681983552011348,
+            "velocityY": -0.8681033957434081,
+            "timestamp": 0.5219387257984235
+        },
+        {
+            "x": 3.6126701345327414,
+            "y": 6.203172404760708,
+            "heading": 0.34321617069844046,
+            "angularVelocity": -6.955973168022537e-8,
+            "velocityX": 3.771250275753975,
+            "velocityY": -0.2971680570846853,
+            "timestamp": 0.5912434791105099
+        },
+        {
+            "x": 3.8741231287698,
+            "y": 6.222626349014288,
+            "heading": 0.3432161668255195,
+            "angularVelocity": -5.588247279499863e-8,
+            "velocityX": 3.7725117216666155,
+            "velocityY": 0.2807014428862732,
+            "timestamp": 0.6605482324225962
+        },
+        {
+            "x": 4.129562675072727,
+            "y": 6.28167517120369,
+            "heading": 0.34321616334811994,
+            "angularVelocity": -5.017548406400545e-8,
+            "velocityX": 3.68574353266443,
+            "velocityY": 0.8520169161197405,
+            "timestamp": 0.7298529857346825
+        },
+        {
+            "x": 4.378319272574482,
+            "y": 6.3644778320450675,
+            "heading": 0.34321615995103527,
+            "angularVelocity": -4.901661807423297e-8,
+            "velocityX": 3.5893150990896703,
+            "velocityY": 1.1947616416511535,
+            "timestamp": 0.7991577390467688
+        },
+        {
+            "x": 4.606228025075672,
+            "y": 6.4403413045373235,
+            "heading": 0.31944011002240325,
+            "angularVelocity": -0.34306521259177225,
+            "velocityX": 3.2885010278428317,
+            "velocityY": 1.094635921300162,
+            "timestamp": 0.8684624923588551
+        },
+        {
+            "x": 4.796152006740457,
+            "y": 6.503560868780938,
+            "heading": 0.2996267220530126,
+            "angularVelocity": -0.2858878651536105,
+            "velocityX": 2.74041782977768,
+            "velocityY": 0.9121966563957079,
+            "timestamp": 0.9377672456709414
+        },
+        {
+            "x": 4.9480911999499915,
+            "y": 6.554136520865842,
+            "heading": 0.2837760268788632,
+            "angularVelocity": -0.22871007278204386,
+            "velocityX": 2.1923343774897495,
+            "velocityY": 0.7297573350727579,
+            "timestamp": 1.0070719989830277
+        },
+        {
+            "x": 5.062045598488017,
+            "y": 6.592068260177227,
+            "heading": 0.27188801539640783,
+            "angularVelocity": -0.1715324117657857,
+            "velocityX": 1.644250835507307,
+            "velocityY": 0.5473180048787404,
+            "timestamp": 1.076376752295114
+        },
+        {
+            "x": 5.138015199132455,
+            "y": 6.617356086604739,
+            "heading": 0.26396267467345363,
+            "angularVelocity": -0.11435493734845723,
+            "velocityX": 1.0961672470333998,
+            "velocityY": 0.364878673092444,
+            "timestamp": 1.1456815056072003
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": -0.05717753089184424,
+            "velocityX": 0.548083631385222,
+            "velocityY": 0.1824393391651535,
+            "timestamp": 1.2149862589192866
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.26,
+            "angularVelocity": -1.0031982410108139e-28,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 1.2842910122313729
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.traj
index 6d3241e6..5360be44 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.traj	
@@ -4,127 +4,199 @@
             "x": 2.3,
             "y": 6.757,
             "heading": 0.39,
-            "angularVelocity": -4.966859894029903e-31,
-            "velocityX": 7.6981862107013014e-34,
-            "velocityY": 1.676179535721191e-31,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 2.3730209621579745,
-            "y": 6.753775517490119,
-            "heading": 0.38350122654498725,
-            "angularVelocity": -0.06940058989840046,
-            "velocityX": 0.7797929692101098,
-            "velocityY": -0.03443434208804683,
-            "timestamp": 0.09364147285393071
-        },
-        {
-            "x": 2.5190628825903247,
-            "y": 6.747326552846718,
-            "heading": 0.37050348328006333,
-            "angularVelocity": -0.13880327667642306,
-            "velocityX": 1.5595858969471437,
-            "velocityY": -0.06886868015693057,
-            "timestamp": 0.18728294570786141
-        },
-        {
-            "x": 2.73812575373285,
-            "y": 6.7376531065331395,
-            "heading": 0.35100644039340695,
-            "angularVelocity": -0.2082094855243831,
-            "velocityX": 2.3393787439058484,
-            "velocityY": -0.10330301327774684,
-            "timestamp": 0.2809244185617921
-        },
-        {
-            "x": 3.0302095555912687,
-            "y": 6.7247551792664035,
-            "heading": 0.3250096622235805,
-            "angularVelocity": -0.27762034681324244,
-            "velocityX": 3.1191713773449834,
-            "velocityY": -0.13773733874150373,
-            "timestamp": 0.37456589141572283
-        },
-        {
-            "x": 3.3841048026937948,
-            "y": 6.709127593205063,
-            "heading": 0.3250096592315294,
-            "angularVelocity": -3.195216141250517e-8,
-            "velocityX": 3.779257590860006,
-            "velocityY": -0.16688744404647712,
-            "timestamp": 0.46820736426965354
-        },
-        {
-            "x": 3.7380000497922845,
-            "y": 6.693500007049851,
-            "heading": 0.3250096562396269,
-            "angularVelocity": -3.195060442591983e-8,
-            "velocityX": 3.7792575908168353,
-            "velocityY": -0.16688744504894126,
-            "timestamp": 0.5618488371235842
-        },
-        {
-            "x": 4.091895296890773,
-            "y": 6.677872420894639,
-            "heading": 0.3250096532477245,
-            "angularVelocity": -3.195060369258429e-8,
-            "velocityX": 3.7792575908168353,
-            "velocityY": -0.16688744504893968,
-            "timestamp": 0.6554903099775149
-        },
-        {
-            "x": 4.445790543993225,
-            "y": 6.6622448348316095,
-            "heading": 0.3250096502556742,
-            "angularVelocity": -3.195215253798021e-8,
-            "velocityX": 3.7792575908592094,
-            "velocityY": -0.1668874440645195,
-            "timestamp": 0.7491317828314457
-        },
-        {
-            "x": 4.737874310687331,
-            "y": 6.649346902155386,
-            "heading": 0.29900607193864137,
-            "angularVelocity": -0.27769296578215763,
-            "velocityX": 3.119171001824275,
-            "velocityY": -0.1377373965095818,
-            "timestamp": 0.8427732556853764
-        },
-        {
-            "x": 4.956937151198913,
-            "y": 6.639673451411702,
-            "heading": 0.27950309637569815,
-            "angularVelocity": -0.20827284074623165,
-            "velocityX": 2.339378416797119,
-            "velocityY": -0.10330306058697603,
-            "timestamp": 0.9364147285393072
-        },
-        {
-            "x": 5.10297904938153,
-            "y": 6.633224483848421,
-            "heading": 0.2665010284217395,
-            "angularVelocity": -0.1388494601558462,
-            "velocityX": 1.5595856593415998,
-            "velocityY": -0.06886871133840435,
-            "timestamp": 1.030056201393238
+            "x": 2.3161416084367663,
+            "y": 6.743433917099698,
+            "heading": 0.3883998707887416,
+            "angularVelocity": -0.031816354442463515,
+            "velocityX": 0.32095354030299916,
+            "velocityY": -0.26974278009236136,
+            "timestamp": 0.05029266360959302
+        },
+        {
+            "x": 2.3488347297971495,
+            "y": 6.716804087923855,
+            "heading": 0.385297889956982,
+            "angularVelocity": -0.06167859503006947,
+            "velocityX": 0.6500574639309191,
+            "velocityY": -0.5294972917434406,
+            "timestamp": 0.10058532721918605
+        },
+        {
+            "x": 2.398581891811533,
+            "y": 6.677772540045158,
+            "heading": 0.38082205724745694,
+            "angularVelocity": -0.08899573791258333,
+            "velocityX": 0.9891534558709463,
+            "velocityY": -0.7760883015003333,
+            "timestamp": 0.15087799082877906
+        },
+        {
+            "x": 2.4660071913042128,
+            "y": 6.627244851321165,
+            "heading": 0.3751449326788243,
+            "angularVelocity": -0.11288176368431184,
+            "velocityX": 1.3406587492776654,
+            "velocityY": -1.004673149074478,
+            "timestamp": 0.2011706544383721
+        },
+        {
+            "x": 2.551890499441383,
+            "y": 6.566518101686868,
+            "heading": 0.3685091874361843,
+            "angularVelocity": -0.13194260884949985,
+            "velocityX": 1.7076707013145411,
+            "velocityY": -1.207467357579225,
+            "timestamp": 0.2514633180479651
+        },
+        {
+            "x": 2.6571889825280843,
+            "y": 6.497556440570772,
+            "heading": 0.3612736159712354,
+            "angularVelocity": -0.14386932299145247,
+            "velocityX": 2.093714580402863,
+            "velocityY": -1.3712071735039586,
+            "timestamp": 0.3017559816575581
+        },
+        {
+            "x": 2.7829575448660773,
+            "y": 6.423518691483174,
+            "heading": 0.3539965965907385,
+            "angularVelocity": -0.144693457419283,
+            "velocityX": 2.50073377131696,
+            "velocityY": -1.4721381564184353,
+            "timestamp": 0.35204864526715113
+        },
+        {
+            "x": 2.929814044412336,
+            "y": 6.349664492887633,
+            "heading": 0.3475640372215199,
+            "angularVelocity": -0.1279025390095239,
+            "velocityX": 2.9200382124570217,
+            "velocityY": -1.468488508957291,
+            "timestamp": 0.40234130887674413
+        },
+        {
+            "x": 3.096125602722168,
+            "y": 6.283931255340576,
+            "heading": 0.3432166982946709,
+            "angularVelocity": -0.08644081690714242,
+            "velocityX": 3.3068751259797926,
+            "velocityY": -1.3070144396670602,
+            "timestamp": 0.45263397248633713
+        },
+        {
+            "x": 3.3513045644934745,
+            "y": 6.2237675636491945,
+            "heading": 0.3432161755192605,
+            "angularVelocity": -0.0000075431393477149555,
+            "velocityX": 3.681983552011348,
+            "velocityY": -0.8681033957434081,
+            "timestamp": 0.5219387257984235
+        },
+        {
+            "x": 3.6126701345327414,
+            "y": 6.203172404760708,
+            "heading": 0.34321617069844046,
+            "angularVelocity": -6.955973168022537e-8,
+            "velocityX": 3.771250275753975,
+            "velocityY": -0.2971680570846853,
+            "timestamp": 0.5912434791105099
+        },
+        {
+            "x": 3.8741231287698,
+            "y": 6.222626349014288,
+            "heading": 0.3432161668255195,
+            "angularVelocity": -5.588247279499863e-8,
+            "velocityX": 3.7725117216666155,
+            "velocityY": 0.2807014428862732,
+            "timestamp": 0.6605482324225962
+        },
+        {
+            "x": 4.129562675072727,
+            "y": 6.28167517120369,
+            "heading": 0.34321616334811994,
+            "angularVelocity": -5.017548406400545e-8,
+            "velocityX": 3.68574353266443,
+            "velocityY": 0.8520169161197405,
+            "timestamp": 0.7298529857346825
+        },
+        {
+            "x": 4.378319272574482,
+            "y": 6.3644778320450675,
+            "heading": 0.34321615995103527,
+            "angularVelocity": -4.901661807423297e-8,
+            "velocityX": 3.5893150990896703,
+            "velocityY": 1.1947616416511535,
+            "timestamp": 0.7991577390467688
+        },
+        {
+            "x": 4.606228025075672,
+            "y": 6.4403413045373235,
+            "heading": 0.31944011002240325,
+            "angularVelocity": -0.34306521259177225,
+            "velocityX": 3.2885010278428317,
+            "velocityY": 1.094635921300162,
+            "timestamp": 0.8684624923588551
+        },
+        {
+            "x": 4.796152006740457,
+            "y": 6.503560868780938,
+            "heading": 0.2996267220530126,
+            "angularVelocity": -0.2858878651536105,
+            "velocityX": 2.74041782977768,
+            "velocityY": 0.9121966563957079,
+            "timestamp": 0.9377672456709414
+        },
+        {
+            "x": 4.9480911999499915,
+            "y": 6.554136520865842,
+            "heading": 0.2837760268788632,
+            "angularVelocity": -0.22871007278204386,
+            "velocityX": 2.1923343774897495,
+            "velocityY": 0.7297573350727579,
+            "timestamp": 1.0070719989830277
+        },
+        {
+            "x": 5.062045598488017,
+            "y": 6.592068260177227,
+            "heading": 0.27188801539640783,
+            "angularVelocity": -0.1715324117657857,
+            "velocityX": 1.644250835507307,
+            "velocityY": 0.5473180048787404,
+            "timestamp": 1.076376752295114
+        },
+        {
+            "x": 5.138015199132455,
+            "y": 6.617356086604739,
+            "heading": 0.26396267467345363,
+            "angularVelocity": -0.11435493734845723,
+            "velocityX": 1.0961672470333998,
+            "velocityY": 0.364878673092444,
+            "timestamp": 1.1456815056072003
         },
         {
             "x": 5.176,
             "y": 6.63,
             "heading": 0.26,
-            "angularVelocity": -0.06942467075336838,
-            "velocityX": 0.7797928459794067,
-            "velocityY": -0.03443435638235996,
-            "timestamp": 1.1236976742471687
+            "angularVelocity": -0.05717753089184424,
+            "velocityX": 0.548083631385222,
+            "velocityY": 0.1824393391651535,
+            "timestamp": 1.2149862589192866
         },
         {
             "x": 5.176,
             "y": 6.63,
             "heading": 0.26,
-            "angularVelocity": 2.7813300894409408e-31,
-            "velocityX": -8.985802616555554e-33,
-            "velocityY": -4.9789977648629575e-31,
-            "timestamp": 1.2173391471010995
+            "angularVelocity": -1.0031982410108139e-28,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 1.2842910122313729
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.1.traj
new file mode 100644
index 00000000..08c314c4
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": -4.207802187241835e-33,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3149122846118377,
+            "y": 6.742667851221533,
+            "heading": 0.387869941582996,
+            "angularVelocity": -0.04275911471750884,
+            "velocityX": 0.2993514559631543,
+            "velocityY": -0.28770572153040486,
+            "timestamp": 0.049815306773293704
+        },
+        {
+            "x": 2.3455895487746456,
+            "y": 6.714948533605446,
+            "heading": 0.38350547033909727,
+            "angularVelocity": -0.08761305563705182,
+            "velocityX": 0.6158200390577908,
+            "velocityY": -0.5564417728518571,
+            "timestamp": 0.09963061354658741
+        },
+        {
+            "x": 2.393100480232861,
+            "y": 6.675215395028912,
+            "heading": 0.37677783190043923,
+            "angularVelocity": -0.13505163120415611,
+            "velocityX": 0.9537416215146959,
+            "velocityY": -0.7976090312433709,
+            "timestamp": 0.1494459203198811
+        },
+        {
+            "x": 2.4587594870185163,
+            "y": 6.62556801103228,
+            "heading": 0.3675326753009839,
+            "angularVelocity": -0.18558867140063587,
+            "velocityX": 1.318048829538744,
+            "velocityY": -0.996629092792259,
+            "timestamp": 0.19926122709317481
+        },
+        {
+            "x": 2.5440298105146963,
+            "y": 6.569365137451258,
+            "heading": 0.35560686650228135,
+            "angularVelocity": -0.23940048895004992,
+            "velocityX": 1.7117293663218704,
+            "velocityY": -1.1282249818675432,
+            "timestamp": 0.24907653386646852
+        },
+        {
+            "x": 2.649943674204089,
+            "y": 6.511985811495891,
+            "heading": 0.34090990459713355,
+            "angularVelocity": -0.2950290353938729,
+            "velocityX": 2.126130913363638,
+            "velocityY": -1.1518412647039136,
+            "timestamp": 0.2988918406397622
+        },
+        {
+            "x": 2.7755461673317816,
+            "y": 6.460929479776747,
+            "heading": 0.3236377937131317,
+            "angularVelocity": -0.3467229653455557,
+            "velocityX": 2.5213634375333727,
+            "velocityY": -1.0249125223997653,
+            "timestamp": 0.3487071474130559
+        },
+        {
+            "x": 2.916773796081543,
+            "y": 6.423426628112793,
+            "heading": 0.3044078287975214,
+            "angularVelocity": -0.38602522319421667,
+            "velocityX": 2.835024772455506,
+            "velocityY": -0.7528379145513634,
+            "timestamp": 0.39852245418634963
+        },
+        {
+            "x": 3.0358362362248608,
+            "y": 6.404912706911414,
+            "heading": 0.2883220118823199,
+            "angularVelocity": -0.4054270239814924,
+            "velocityX": 3.0008504404627585,
+            "velocityY": -0.46662497866622815,
+            "timestamp": 0.43819868680957713
+        },
+        {
+            "x": 3.161507467990405,
+            "y": 6.397735862008237,
+            "heading": 0.27120244348460076,
+            "angularVelocity": -0.43148170241679884,
+            "velocityX": 3.1674184633137656,
+            "velocityY": -0.1808852410792362,
+            "timestamp": 0.47787491943280463
+        },
+        {
+            "x": 3.294397924367021,
+            "y": 6.401341217255427,
+            "heading": 0.24704597239250262,
+            "angularVelocity": -0.6088398392423459,
+            "velocityX": 3.349371842799791,
+            "velocityY": 0.09086939481941644,
+            "timestamp": 0.5175511520560321
+        },
+        {
+            "x": 3.420816904565574,
+            "y": 6.393526992528847,
+            "heading": 0.22271106774313612,
+            "angularVelocity": -0.6133370796675788,
+            "velocityX": 3.186264719209789,
+            "velocityY": -0.19694976589104882,
+            "timestamp": 0.5572273846792596
+        },
+        {
+            "x": 3.540724707702168,
+            "y": 6.374315752768638,
+            "heading": 0.19854345438454005,
+            "angularVelocity": -0.6091206689927945,
+            "velocityX": 3.0221569743089773,
+            "velocityY": -0.48420019971772477,
+            "timestamp": 0.5969036173024871
+        },
+        {
+            "x": 3.654109001159668,
+            "y": 6.343714714050293,
+            "heading": 0.17465129572461457,
+            "angularVelocity": -0.602178107150174,
+            "velocityX": 2.857738398053468,
+            "velocityY": -0.7712687595352792,
+            "timestamp": 0.6365798499257146
+        },
+        {
+            "x": 3.8638905650676802,
+            "y": 6.228810110896502,
+            "heading": 0.12580249974502505,
+            "angularVelocity": -0.584756466458008,
+            "velocityX": 2.5112415481035373,
+            "velocityY": -1.375493671286808,
+            "timestamp": 0.7201168414454817
+        },
+        {
+            "x": 4.035438177373403,
+            "y": 6.070055732251625,
+            "heading": 0.08069123606907251,
+            "angularVelocity": -0.5400154213754852,
+            "velocityX": 2.0535526739088925,
+            "velocityY": -1.90040813963597,
+            "timestamp": 0.8036538329652487
+        },
+        {
+            "x": 4.153723645387517,
+            "y": 5.887986786487416,
+            "heading": 0.04380352514936875,
+            "angularVelocity": -0.4415733706546099,
+            "velocityX": 1.4159651414562335,
+            "velocityY": -2.179500870834227,
+            "timestamp": 0.8871908244850157
+        },
+        {
+            "x": 4.218422318430694,
+            "y": 5.7283456029173445,
+            "heading": 0.01885386609449773,
+            "angularVelocity": -0.29866599934929844,
+            "velocityX": 0.7744912985986951,
+            "velocityY": -1.9110238550103982,
+            "timestamp": 0.9707278160047828
+        },
+        {
+            "x": 4.250477583142556,
+            "y": 5.616790769692197,
+            "heading": 0.0038966042591740443,
+            "angularVelocity": -0.17904956311230244,
+            "velocityX": 0.38372539073628414,
+            "velocityY": -1.33539443060689,
+            "timestamp": 1.0542648075245498
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -0.003,
+            "angularVelocity": -0.08255748900826321,
+            "velocityX": 0.14990265545387924,
+            "velocityY": -0.6798278063289014,
+            "timestamp": 1.1378017990443168
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": -0.003,
+            "angularVelocity": 2.6015085807950757e-30,
+            "velocityX": -2.4770702901017708e-31,
+            "velocityY": 1.2993067663186955e-31,
+            "timestamp": 1.2213387905640838
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.traj
index 15f8fbd9..08c314c4 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.traj	
@@ -4,127 +4,199 @@
             "x": 2.3,
             "y": 6.757,
             "heading": 0.39,
-            "angularVelocity": -5.274350592003984e-33,
-            "velocityX": 6.842220334639481e-35,
-            "velocityY": 4.754828012658472e-35,
+            "angularVelocity": -4.207802187241835e-33,
+            "velocityX": 0,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 2.3477747650996332,
-            "y": 6.727866811231813,
-            "heading": 0.37690695075838176,
-            "angularVelocity": -0.1596548904259805,
-            "velocityX": 0.5825590927179829,
-            "velocityY": -0.3552462054262789,
-            "timestamp": 0.08200844463131846
-        },
-        {
-            "x": 2.4433243855136606,
-            "y": 6.669600518122756,
-            "heading": 0.35072364334029893,
-            "angularVelocity": -0.3192757469769597,
-            "velocityX": 1.16511928550267,
-            "velocityY": -0.7104913813571495,
-            "timestamp": 0.1640168892626369
-        },
-        {
-            "x": 2.5866489974259967,
-            "y": 6.582201260145896,
-            "heading": 0.3114536293241488,
-            "angularVelocity": -0.47885329605621113,
-            "velocityX": 1.7476811388958082,
-            "velocityY": -1.0657348565721152,
-            "timestamp": 0.24602533389395537
-        },
-        {
-            "x": 2.7777487646808345,
-            "y": 6.465669250143918,
-            "heading": 0.2590995246277934,
-            "angularVelocity": -0.638398946983098,
-            "velocityX": 2.330244990183096,
-            "velocityY": -1.4209757364118492,
-            "timestamp": 0.3280337785252738
-        },
-        {
-            "x": 3.0166238424465086,
-            "y": 6.320004790854863,
-            "heading": 0.19366077275903337,
-            "angularVelocity": -0.797951383701398,
-            "velocityX": 2.912810733572304,
-            "velocityY": -1.7762129247044165,
-            "timestamp": 0.4100422231565923
-        },
-        {
-            "x": 3.281496122930573,
-            "y": 6.158489372926003,
-            "heading": 0.19366076448026312,
-            "angularVelocity": -1.0095021672660313e-7,
-            "velocityX": 3.2298171447445223,
-            "velocityY": -1.9694973932864837,
-            "timestamp": 0.49205066778791073
-        },
-        {
-            "x": 3.546368403715213,
-            "y": 5.996973955490063,
-            "heading": 0.193660756201493,
-            "angularVelocity": -1.0095021513248387e-7,
-            "velocityX": 3.2298171484097047,
-            "velocityY": -1.9694973872758792,
-            "timestamp": 0.5740591124192291
-        },
-        {
-            "x": 3.785245611761769,
-            "y": 5.851315694507869,
-            "heading": 0.1281200370686441,
-            "angularVelocity": -0.7991947588751046,
-            "velocityX": 2.912836709932308,
-            "velocityY": -1.7761373433799759,
-            "timestamp": 0.6560675570505476
-        },
-        {
-            "x": 3.9763473726141734,
-            "y": 5.734789313275287,
-            "heading": 0.07567704387560718,
-            "angularVelocity": -0.6394828414171544,
-            "velocityX": 2.3302692998450527,
-            "velocityY": -1.4209070999510438,
-            "timestamp": 0.7380760016818662
-        },
-        {
-            "x": 4.119673683324307,
-            "y": 5.647394635056781,
-            "heading": 0.036339503004366135,
-            "angularVelocity": -0.47967670949118707,
-            "velocityX": 1.7477018538088223,
-            "velocityY": -1.0656790116115789,
-            "timestamp": 0.8200844463131847
-        },
-        {
-            "x": 4.2152245571841105,
-            "y": 5.589131547509505,
-            "heading": 0.010112949222819228,
-            "angularVelocity": -0.3198030873460947,
-            "velocityX": 1.1651345698527396,
-            "velocityY": -0.710452293165722,
-            "timestamp": 0.9020928909445032
+            "x": 2.3149122846118377,
+            "y": 6.742667851221533,
+            "heading": 0.387869941582996,
+            "angularVelocity": -0.04275911471750884,
+            "velocityX": 0.2993514559631543,
+            "velocityY": -0.28770572153040486,
+            "timestamp": 0.049815306773293704
+        },
+        {
+            "x": 2.3455895487746456,
+            "y": 6.714948533605446,
+            "heading": 0.38350547033909727,
+            "angularVelocity": -0.08761305563705182,
+            "velocityX": 0.6158200390577908,
+            "velocityY": -0.5564417728518571,
+            "timestamp": 0.09963061354658741
+        },
+        {
+            "x": 2.393100480232861,
+            "y": 6.675215395028912,
+            "heading": 0.37677783190043923,
+            "angularVelocity": -0.13505163120415611,
+            "velocityX": 0.9537416215146959,
+            "velocityY": -0.7976090312433709,
+            "timestamp": 0.1494459203198811
+        },
+        {
+            "x": 2.4587594870185163,
+            "y": 6.62556801103228,
+            "heading": 0.3675326753009839,
+            "angularVelocity": -0.18558867140063587,
+            "velocityX": 1.318048829538744,
+            "velocityY": -0.996629092792259,
+            "timestamp": 0.19926122709317481
+        },
+        {
+            "x": 2.5440298105146963,
+            "y": 6.569365137451258,
+            "heading": 0.35560686650228135,
+            "angularVelocity": -0.23940048895004992,
+            "velocityX": 1.7117293663218704,
+            "velocityY": -1.1282249818675432,
+            "timestamp": 0.24907653386646852
+        },
+        {
+            "x": 2.649943674204089,
+            "y": 6.511985811495891,
+            "heading": 0.34090990459713355,
+            "angularVelocity": -0.2950290353938729,
+            "velocityX": 2.126130913363638,
+            "velocityY": -1.1518412647039136,
+            "timestamp": 0.2988918406397622
+        },
+        {
+            "x": 2.7755461673317816,
+            "y": 6.460929479776747,
+            "heading": 0.3236377937131317,
+            "angularVelocity": -0.3467229653455557,
+            "velocityX": 2.5213634375333727,
+            "velocityY": -1.0249125223997653,
+            "timestamp": 0.3487071474130559
+        },
+        {
+            "x": 2.916773796081543,
+            "y": 6.423426628112793,
+            "heading": 0.3044078287975214,
+            "angularVelocity": -0.38602522319421667,
+            "velocityX": 2.835024772455506,
+            "velocityY": -0.7528379145513634,
+            "timestamp": 0.39852245418634963
+        },
+        {
+            "x": 3.0358362362248608,
+            "y": 6.404912706911414,
+            "heading": 0.2883220118823199,
+            "angularVelocity": -0.4054270239814924,
+            "velocityX": 3.0008504404627585,
+            "velocityY": -0.46662497866622815,
+            "timestamp": 0.43819868680957713
+        },
+        {
+            "x": 3.161507467990405,
+            "y": 6.397735862008237,
+            "heading": 0.27120244348460076,
+            "angularVelocity": -0.43148170241679884,
+            "velocityX": 3.1674184633137656,
+            "velocityY": -0.1808852410792362,
+            "timestamp": 0.47787491943280463
+        },
+        {
+            "x": 3.294397924367021,
+            "y": 6.401341217255427,
+            "heading": 0.24704597239250262,
+            "angularVelocity": -0.6088398392423459,
+            "velocityX": 3.349371842799791,
+            "velocityY": 0.09086939481941644,
+            "timestamp": 0.5175511520560321
+        },
+        {
+            "x": 3.420816904565574,
+            "y": 6.393526992528847,
+            "heading": 0.22271106774313612,
+            "angularVelocity": -0.6133370796675788,
+            "velocityX": 3.186264719209789,
+            "velocityY": -0.19694976589104882,
+            "timestamp": 0.5572273846792596
+        },
+        {
+            "x": 3.540724707702168,
+            "y": 6.374315752768638,
+            "heading": 0.19854345438454005,
+            "angularVelocity": -0.6091206689927945,
+            "velocityX": 3.0221569743089773,
+            "velocityY": -0.48420019971772477,
+            "timestamp": 0.5969036173024871
+        },
+        {
+            "x": 3.654109001159668,
+            "y": 6.343714714050293,
+            "heading": 0.17465129572461457,
+            "angularVelocity": -0.602178107150174,
+            "velocityX": 2.857738398053468,
+            "velocityY": -0.7712687595352792,
+            "timestamp": 0.6365798499257146
+        },
+        {
+            "x": 3.8638905650676802,
+            "y": 6.228810110896502,
+            "heading": 0.12580249974502505,
+            "angularVelocity": -0.584756466458008,
+            "velocityX": 2.5112415481035373,
+            "velocityY": -1.375493671286808,
+            "timestamp": 0.7201168414454817
+        },
+        {
+            "x": 4.035438177373403,
+            "y": 6.070055732251625,
+            "heading": 0.08069123606907251,
+            "angularVelocity": -0.5400154213754852,
+            "velocityX": 2.0535526739088925,
+            "velocityY": -1.90040813963597,
+            "timestamp": 0.8036538329652487
+        },
+        {
+            "x": 4.153723645387517,
+            "y": 5.887986786487416,
+            "heading": 0.04380352514936875,
+            "angularVelocity": -0.4415733706546099,
+            "velocityX": 1.4159651414562335,
+            "velocityY": -2.179500870834227,
+            "timestamp": 0.8871908244850157
+        },
+        {
+            "x": 4.218422318430694,
+            "y": 5.7283456029173445,
+            "heading": 0.01885386609449773,
+            "angularVelocity": -0.29866599934929844,
+            "velocityX": 0.7744912985986951,
+            "velocityY": -1.9110238550103982,
+            "timestamp": 0.9707278160047828
+        },
+        {
+            "x": 4.250477583142556,
+            "y": 5.616790769692197,
+            "heading": 0.0038966042591740443,
+            "angularVelocity": -0.17904956311230244,
+            "velocityX": 0.38372539073628414,
+            "velocityY": -1.33539443060689,
+            "timestamp": 1.0542648075245498
         },
         {
             "x": 4.263,
             "y": 5.56,
             "heading": -0.003,
-            "angularVelocity": -0.15989754813385992,
-            "velocityX": 0.5825673566993228,
-            "velocityY": -0.3552261921375451,
-            "timestamp": 0.9841013355758217
+            "angularVelocity": -0.08255748900826321,
+            "velocityX": 0.14990265545387924,
+            "velocityY": -0.6798278063289014,
+            "timestamp": 1.1378017990443168
         },
         {
             "x": 4.263,
             "y": 5.56,
             "heading": -0.003,
-            "angularVelocity": 2.1439897413450708e-33,
-            "velocityX": 3.9230953244452345e-34,
-            "velocityY": -1.8972025919917558e-33,
-            "timestamp": 1.0661097802071402
+            "angularVelocity": 2.6015085807950757e-30,
+            "velocityX": -2.4770702901017708e-31,
+            "velocityY": 1.2993067663186955e-31,
+            "timestamp": 1.2213387905640838
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.1.traj
new file mode 100644
index 00000000..8360b86f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.1.traj	
@@ -0,0 +1,544 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.542786541644192e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2937082670823616,
+            "y": 6.738516922494428,
+            "heading": 0.3838170687349535,
+            "angularVelocity": -0.12752172771189255,
+            "velocityX": -0.1297657401618881,
+            "velocityY": -0.3812097977411177,
+            "timestamp": 0.04848531599949176
+        },
+        {
+            "x": 2.281123433600762,
+            "y": 6.70154498788335,
+            "heading": 0.37169259737448246,
+            "angularVelocity": -0.2500648105569625,
+            "velocityX": -0.25955968775719646,
+            "velocityY": -0.7625387985814647,
+            "timestamp": 0.09697063199898352
+        },
+        {
+            "x": 2.262243832879689,
+            "y": 6.646077048114969,
+            "heading": 0.3539392998096368,
+            "angularVelocity": -0.36615823159004923,
+            "velocityX": -0.3893880102034898,
+            "velocityY": -1.1440152265685524,
+            "timestamp": 0.14545594799847528
+        },
+        {
+            "x": 2.2370673940379584,
+            "y": 6.572104077537847,
+            "heading": 0.3309787534387302,
+            "angularVelocity": -0.4735567026240549,
+            "velocityX": -0.5192590441623445,
+            "velocityY": -1.5256778068215682,
+            "timestamp": 0.19394126399796704
+        },
+        {
+            "x": 2.2055915440305824,
+            "y": 6.479614437749851,
+            "heading": 0.303410444299779,
+            "angularVelocity": -0.5685908933466691,
+            "velocityX": -0.6491831466623577,
+            "velocityY": -1.9075804268039651,
+            "timestamp": 0.2424265799974588
+        },
+        {
+            "x": 2.167813255965874,
+            "y": 6.3685929704461826,
+            "heading": 0.2721534840825349,
+            "angularVelocity": -0.6446685882563702,
+            "velocityX": -0.7791696782014628,
+            "velocityY": -2.2897956838082623,
+            "timestamp": 0.29091189599695055
+        },
+        {
+            "x": 2.123730000007902,
+            "y": 6.239021112222755,
+            "heading": 0.23878941688886735,
+            "angularVelocity": -0.6881272505944181,
+            "velocityX": -0.909208387103519,
+            "velocityY": -2.6723938073299895,
+            "timestamp": 0.3393972119964423
+        },
+        {
+            "x": 2.0733472424162183,
+            "y": 6.090890823809695,
+            "heading": 0.2066566550746183,
+            "angularVelocity": -0.6627318220196932,
+            "velocityX": -1.0391343554894152,
+            "velocityY": -3.0551577391940747,
+            "timestamp": 0.38788252799593403
+        },
+        {
+            "x": 2.0168156590767947,
+            "y": 5.924489369921669,
+            "heading": 0.187194468928521,
+            "angularVelocity": -0.4014037187247154,
+            "velocityX": -1.1659526636929207,
+            "velocityY": -3.431996893448088,
+            "timestamp": 0.4363678439954258
+        },
+        {
+            "x": 1.9576664650342146,
+            "y": 5.750871436780747,
+            "heading": 0.18719445417707173,
+            "angularVelocity": -3.042457077435804e-7,
+            "velocityX": -1.2199403638687387,
+            "velocityY": -3.580835342863344,
+            "timestamp": 0.4848531599949175
+        },
+        {
+            "x": 1.8985172819179166,
+            "y": 5.577253499917319,
+            "heading": 0.18719443942575115,
+            "angularVelocity": -3.042430533078605e-7,
+            "velocityX": -1.2199401385163373,
+            "velocityY": -3.5808354196392784,
+            "timestamp": 0.5333384759944093
+        },
+        {
+            "x": 1.8393681174712941,
+            "y": 5.403635556693396,
+            "heading": 0.18719442467442912,
+            "angularVelocity": -3.0424308271607693e-7,
+            "velocityX": -1.219939753457998,
+            "velocityY": -3.5808355508232466,
+            "timestamp": 0.5818237919939011
+        },
+        {
+            "x": 1.780218973506269,
+            "y": 5.230017606491683,
+            "heading": 0.18719440992310651,
+            "angularVelocity": -3.0424309544377536e-7,
+            "velocityX": -1.2199393310291196,
+            "velocityY": -3.580835694738771,
+            "timestamp": 0.6303091079933929
+        },
+        {
+            "x": 1.7210698517072343,
+            "y": 5.056399648738334,
+            "heading": 0.1871943951717844,
+            "angularVelocity": -3.042430844451726e-7,
+            "velocityX": -1.2199388738599835,
+            "velocityY": -3.580835850489736,
+            "timestamp": 0.6787944239928847
+        },
+        {
+            "x": 1.661920753431811,
+            "y": 4.882781682970833,
+            "heading": 0.18719438042046438,
+            "angularVelocity": -3.042430423387178e-7,
+            "velocityX": -1.2199383886901667,
+            "velocityY": -3.580836015780049,
+            "timestamp": 0.7272797399923765
+        },
+        {
+            "x": 1.6027716795006517,
+            "y": 4.709163708909599,
+            "heading": 0.18719436566914785,
+            "angularVelocity": -3.042429697472901e-7,
+            "velocityX": -1.2199378865945683,
+            "velocityY": -3.5808361868366427,
+            "timestamp": 0.7757650559918683
+        },
+        {
+            "x": 1.5436226301529286,
+            "y": 4.5355457264731545,
+            "heading": 0.18719435091783626,
+            "angularVelocity": -3.042428676797686e-7,
+            "velocityX": -1.219937379566087,
+            "velocityY": -3.5808363595737087,
+            "timestamp": 0.8242503719913601
+        },
+        {
+            "x": 1.4844769425124074,
+            "y": 4.361926598741778,
+            "heading": 0.18719433616649686,
+            "angularVelocity": -3.0424344145116126e-7,
+            "velocityX": -1.2198680450801807,
+            "velocityY": -3.58085998103429,
+            "timestamp": 0.8727356879908519
+        },
+        {
+            "x": 1.4424355744034427,
+            "y": 4.183392744566331,
+            "heading": 0.18719432108286724,
+            "angularVelocity": -3.110968611299024e-7,
+            "velocityX": -0.8670948562865489,
+            "velocityY": -3.682225236552195,
+            "timestamp": 0.9212210039903437
+        },
+        {
+            "x": 1.4196862795998648,
+            "y": 4.0013919676043574,
+            "heading": 0.18719430492048,
+            "angularVelocity": -3.3334602243497606e-7,
+            "velocityX": -0.4691996810625877,
+            "velocityY": -3.753729829541186,
+            "timestamp": 0.9697063199898355
+        },
+        {
+            "x": 1.4164891295727136,
+            "y": 3.81800278977475,
+            "heading": 0.18719428668091828,
+            "angularVelocity": -3.761873328628986e-7,
+            "velocityX": -0.06594058347012394,
+            "velocityY": -3.7823653213178425,
+            "timestamp": 1.0181916359893273
+        },
+        {
+            "x": 1.432880701620732,
+            "y": 3.635319656493888,
+            "heading": 0.1871942648241357,
+            "angularVelocity": -4.507917942103899e-7,
+            "velocityX": 0.3380729136345315,
+            "velocityY": -3.767803292913241,
+            "timestamp": 1.066676951988819
+        },
+        {
+            "x": 1.4686738067578264,
+            "y": 3.455429018696199,
+            "heading": 0.18719405074105777,
+            "angularVelocity": -0.0000044154209051021175,
+            "velocityX": 0.7382256751280081,
+            "velocityY": -3.710208628927779,
+            "timestamp": 1.1151622679883109
+        },
+        {
+            "x": 1.5224561218642167,
+            "y": 3.2829543962052377,
+            "heading": 0.17869632281657946,
+            "angularVelocity": -0.17526394846200616,
+            "velocityX": 1.10924955315069,
+            "velocityY": -3.557254788081333,
+            "timestamp": 1.1636475839878027
+        },
+        {
+            "x": 1.5921416539328836,
+            "y": 3.121386413493124,
+            "heading": 0.1576086367549348,
+            "angularVelocity": -0.43492933122045885,
+            "velocityX": 1.4372502402541338,
+            "velocityY": -3.33230751169348,
+            "timestamp": 1.2121328999872945
+        },
+        {
+            "x": 1.6761645416915945,
+            "y": 2.9727322431612,
+            "heading": 0.12423651758368265,
+            "angularVelocity": -0.6882933210462135,
+            "velocityX": 1.7329553500274961,
+            "velocityY": -3.0659626995783453,
+            "timestamp": 1.2606182159867863
+        },
+        {
+            "x": 1.7731411061925635,
+            "y": 2.838565031466778,
+            "heading": 0.08175388007293943,
+            "angularVelocity": -0.8761959499529206,
+            "velocityX": 2.0001223566816617,
+            "velocityY": -2.767172058765521,
+            "timestamp": 1.309103531986278
+        },
+        {
+            "x": 1.8819216264400211,
+            "y": 2.719965378093697,
+            "heading": 0.033593056188374194,
+            "angularVelocity": -0.9933074146707891,
+            "velocityX": 2.2435765964482535,
+            "velocityY": -2.4460942643771193,
+            "timestamp": 1.3575888479857698
+        },
+        {
+            "x": 2.0016674106146493,
+            "y": 2.617587012881804,
+            "heading": -0.0178597683247242,
+            "angularVelocity": -1.0612042729434041,
+            "velocityX": 2.4697329842357973,
+            "velocityY": -2.1115334220519952,
+            "timestamp": 1.4060741639852616
+        },
+        {
+            "x": 2.131748914718628,
+            "y": 2.531857967376709,
+            "heading": -0.07087516069292028,
+            "angularVelocity": -1.0934319242038333,
+            "velocityX": 2.682905152268552,
+            "velocityY": -1.7681445142231829,
+            "timestamp": 1.4545594799847534
+        },
+        {
+            "x": 2.218821194191698,
+            "y": 2.484029060579093,
+            "heading": -0.10483282223316899,
+            "angularVelocity": -1.096901896110445,
+            "velocityX": 2.812612651242981,
+            "velocityY": -1.5449714785171498,
+            "timestamp": 1.4855172727096597
+        },
+        {
+            "x": 2.3097767814437278,
+            "y": 2.4431840829179357,
+            "heading": -0.13859592156415457,
+            "angularVelocity": -1.0906171389530375,
+            "velocityX": 2.938051432149048,
+            "velocityY": -1.3193762883647426,
+            "timestamp": 1.516475065434566
+        },
+        {
+            "x": 2.4044569077078632,
+            "y": 2.4094074987036276,
+            "heading": -0.1718069818405491,
+            "angularVelocity": -1.0727851488087659,
+            "velocityX": 3.0583616572737307,
+            "velocityY": -1.0910527282945428,
+            "timestamp": 1.5474328581594723
+        },
+        {
+            "x": 2.5026677503931767,
+            "y": 2.3827944816766236,
+            "heading": -0.20403633414457553,
+            "angularVelocity": -1.0410739741365005,
+            "velocityX": 3.1724110164298485,
+            "velocityY": -0.859654861819398,
+            "timestamp": 1.5783906508843786
+        },
+        {
+            "x": 2.6041682782784212,
+            "y": 2.363451815109364,
+            "heading": -0.23475758891885987,
+            "angularVelocity": -0.9923593405260972,
+            "velocityX": 3.2786745743372427,
+            "velocityY": -0.6248076773199155,
+            "timestamp": 1.6093484436092849
+        },
+        {
+            "x": 2.7086523080371263,
+            "y": 2.3514976196224144,
+            "heading": -0.26331031649970843,
+            "angularVelocity": -0.9223114784903548,
+            "velocityX": 3.3750477847899405,
+            "velocityY": -0.38614495527174003,
+            "timestamp": 1.6403062363341911
+        },
+        {
+            "x": 2.8157215031828033,
+            "y": 2.3470577985799963,
+            "heading": -0.288841887845313,
+            "angularVelocity": -0.8247219551529203,
+            "velocityX": 3.458553912306755,
+            "velocityY": -0.14341529714262294,
+            "timestamp": 1.6712640290590974
+        },
+        {
+            "x": 2.9248447015312338,
+            "y": 2.3502540561946077,
+            "heading": -0.3102182612745416,
+            "angularVelocity": -0.6905005668986338,
+            "velocityX": 3.524902415281217,
+            "velocityY": 0.10324565588123534,
+            "timestamp": 1.7022218217840037
+        },
+        {
+            "x": 3.0353304312162184,
+            "y": 2.361176073397281,
+            "heading": -0.326082506948615,
+            "angularVelocity": -0.5124475706430824,
+            "velocityX": 3.568914963243171,
+            "velocityY": 0.3528034863374462,
+            "timestamp": 1.73317961450891
+        },
+        {
+            "x": 3.1466093544989446,
+            "y": 2.379927120590619,
+            "heading": -0.3369287204650216,
+            "angularVelocity": -0.35035487229182166,
+            "velocityX": 3.594536738181179,
+            "velocityY": 0.605697161937562,
+            "timestamp": 1.7641374072338163
+        },
+        {
+            "x": 3.2578769593015027,
+            "y": 2.406586601459548,
+            "heading": -0.34366338052401463,
+            "angularVelocity": -0.21754328930658381,
+            "velocityX": 3.5941711281316278,
+            "velocityY": 0.8611557389062998,
+            "timestamp": 1.7950951999587226
+        },
+        {
+            "x": 3.3679824864036325,
+            "y": 2.4411213005139327,
+            "heading": -0.34790283328354016,
+            "angularVelocity": -0.13694299193573645,
+            "velocityX": 3.556633642474382,
+            "velocityY": 1.1155413876329803,
+            "timestamp": 1.8260529926836289
+        },
+        {
+            "x": 3.475304114496225,
+            "y": 2.4831460278092354,
+            "heading": -0.3524511595124565,
+            "angularVelocity": -0.14692023650373393,
+            "velocityX": 3.466708012625811,
+            "velocityY": 1.3574846136083376,
+            "timestamp": 1.8570107854085351
+        },
+        {
+            "x": 3.5778542240444473,
+            "y": 2.531447362567012,
+            "heading": -0.3616437420636369,
+            "angularVelocity": -0.29693921116020117,
+            "velocityX": 3.312578207999158,
+            "velocityY": 1.560231867531513,
+            "timestamp": 1.8879685781334414
+        },
+        {
+            "x": 3.673558919058804,
+            "y": 2.583451056106446,
+            "heading": -0.37745622232401294,
+            "angularVelocity": -0.5107754419475534,
+            "velocityX": 3.091457322713648,
+            "velocityY": 1.6798256258655577,
+            "timestamp": 1.9189263708583477
+        },
+        {
+            "x": 3.7613706261508923,
+            "y": 2.636186152260406,
+            "heading": -0.3968957996596081,
+            "angularVelocity": -0.6279380932955558,
+            "velocityX": 2.836497675157173,
+            "velocityY": 1.7034514257095354,
+            "timestamp": 1.949884163583254
+        },
+        {
+            "x": 3.841306735175595,
+            "y": 2.6876448087218088,
+            "heading": -0.4179459699473055,
+            "angularVelocity": -0.6799635386030257,
+            "velocityX": 2.582099755467769,
+            "velocityY": 1.6622198138851267,
+            "timestamp": 1.9808419563081603
+        },
+        {
+            "x": 3.913645075614716,
+            "y": 2.736629100397619,
+            "heading": -0.43934657697326235,
+            "angularVelocity": -0.6912833617387507,
+            "velocityX": 2.336676296079001,
+            "velocityY": 1.582292772328868,
+            "timestamp": 2.0117997490330666
+        },
+        {
+            "x": 3.9786691618750996,
+            "y": 2.7823974078733573,
+            "heading": -0.46027689817629036,
+            "angularVelocity": -0.6760921681319296,
+            "velocityX": 2.100410931692797,
+            "velocityY": 1.4784099074015198,
+            "timestamp": 2.0427575417579726
+        },
+        {
+            "x": 4.036618763930541,
+            "y": 2.824460782764613,
+            "heading": -0.48017689659950147,
+            "angularVelocity": -0.6428106357887643,
+            "velocityX": 1.8718906276836884,
+            "velocityY": 1.3587330099747585,
+            "timestamp": 2.0737153344828787
+        },
+        {
+            "x": 4.08768908254176,
+            "y": 2.862477844590781,
+            "heading": -0.49864492956893264,
+            "angularVelocity": -0.5965552238942878,
+            "velocityX": 1.6496757073449355,
+            "velocityY": 1.2280288250525657,
+            "timestamp": 2.1046731272077848
+        },
+        {
+            "x": 4.132038835388808,
+            "y": 2.8961988047734253,
+            "heading": -0.5153800748468587,
+            "angularVelocity": -0.5405794084679139,
+            "velocityX": 1.4325876925792385,
+            "velocityY": 1.0892559583398733,
+            "timestamp": 2.135630919932691
+        },
+        {
+            "x": 4.169798307734006,
+            "y": 2.9254338939938918,
+            "heading": -0.5301487101337384,
+            "angularVelocity": -0.47705711511170307,
+            "velocityX": 1.2197081581622198,
+            "velocityY": 0.944353154640467,
+            "timestamp": 2.166588712657597
+        },
+        {
+            "x": 4.201075770562755,
+            "y": 2.9500345091711524,
+            "heading": -0.5427642633485259,
+            "angularVelocity": -0.4075081620800149,
+            "velocityX": 1.0103259979397883,
+            "velocityY": 0.7946501675963605,
+            "timestamp": 2.197546505382503
+        },
+        {
+            "x": 4.225962326087245,
+            "y": 2.9698813733488647,
+            "heading": -0.5530743601373866,
+            "angularVelocity": -0.3330372058810695,
+            "velocityX": 0.8038866254348108,
+            "velocityY": 0.6410942909939014,
+            "timestamp": 2.228504298107409
+        },
+        {
+            "x": 4.24453552816763,
+            "y": 2.984876771487546,
+            "heading": -0.5609523103628415,
+            "angularVelocity": -0.2544738992160421,
+            "velocityX": 0.5999524011761236,
+            "velocityY": 0.48438201883216997,
+            "timestamp": 2.259462090832315
+        },
+        {
+            "x": 4.256862100646349,
+            "y": 2.9949392682905573,
+            "heading": -0.5662912533122896,
+            "angularVelocity": -0.17245877305093363,
+            "velocityX": 0.398173493448939,
+            "velocityY": 0.32503922008988784,
+            "timestamp": 2.290419883557221
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.569,
+            "angularVelocity": -0.08749805620543083,
+            "velocityX": 0.19826669841112912,
+            "velocityY": 0.16347198117160036,
+            "timestamp": 2.321377676282127
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.569,
+            "angularVelocity": 1.3974941712649426e-29,
+            "velocityX": 2.480011116993708e-30,
+            "velocityY": -4.1979337006948246e-30,
+            "timestamp": 2.3523354690070333
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.1.traj
new file mode 100644
index 00000000..1ef15c59
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.1.traj	
@@ -0,0 +1,562 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 9.203343680058695e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 2.298008161759761,
+            "y": 6.742798044155305,
+            "heading": 0.37730350882287184,
+            "angularVelocity": -0.30162955292479776,
+            "velocityX": -0.04731994607959398,
+            "velocityY": -0.3373947598852993,
+            "timestamp": 0.0420929947149233
+        },
+        {
+            "x": 2.2940179124306126,
+            "y": 6.714368498753073,
+            "heading": 0.3522801711626307,
+            "angularVelocity": -0.5944774856175671,
+            "velocityX": -0.09479604281359731,
+            "velocityY": -0.6753984978919232,
+            "timestamp": 0.0841859894298466
+        },
+        {
+            "x": 2.28802134748838,
+            "y": 6.671681118579196,
+            "heading": 0.31538217591497186,
+            "angularVelocity": -0.8765828019021266,
+            "velocityX": -0.14245992671333493,
+            "velocityY": -1.014120769096589,
+            "timestamp": 0.1262789841447699
+        },
+        {
+            "x": 2.2800080756588827,
+            "y": 6.61469814393844,
+            "heading": 0.2672110996394858,
+            "angularVelocity": -1.1443965106718308,
+            "velocityX": -0.19037067530517543,
+            "velocityY": -1.3537400944426796,
+            "timestamp": 0.1683719788596932
+        },
+        {
+            "x": 2.2699650128963897,
+            "y": 6.543370292056319,
+            "heading": 0.20862519634351256,
+            "angularVelocity": -1.391820745773707,
+            "velocityX": -0.23859226055333851,
+            "velocityY": -1.694530226827328,
+            "timestamp": 0.21046497357461652
+        },
+        {
+            "x": 2.2578780077181495,
+            "y": 6.4576319980717,
+            "heading": 0.14091301714440063,
+            "angularVelocity": -1.608632972248606,
+            "velocityX": -0.2871500414760241,
+            "velocityY": -2.036877978516017,
+            "timestamp": 0.2525579682895398
+        },
+        {
+            "x": 2.2437361567314555,
+            "y": 6.357398182016401,
+            "heading": 0.06608849726258184,
+            "angularVelocity": -1.7776002964049293,
+            "velocityX": -0.3359668534508049,
+            "velocityY": -2.3812469683883926,
+            "timestamp": 0.2946509630044631
+        },
+        {
+            "x": 2.227539619778089,
+            "y": 6.242572554004172,
+            "heading": -0.012517898946656567,
+            "angularVelocity": -1.8674460380308828,
+            "velocityX": -0.3847798680768185,
+            "velocityY": -2.727903509595609,
+            "timestamp": 0.33674395771938637
+        },
+        {
+            "x": 2.209319968778144,
+            "y": 6.1131309245806085,
+            "heading": -0.08849823738903302,
+            "angularVelocity": -1.8050589880087344,
+            "velocityX": -0.4328428310539146,
+            "velocityY": -3.0751347177888695,
+            "timestamp": 0.37883695243430965
+        },
+        {
+            "x": 2.189363625025874,
+            "y": 5.970570114356374,
+            "heading": -0.13907359717832485,
+            "angularVelocity": -1.201514887021361,
+            "velocityX": -0.4741013056311528,
+            "velocityY": -3.38680607520881,
+            "timestamp": 0.42092994714923293
+        },
+        {
+            "x": 2.1674827409245396,
+            "y": 5.816618965425972,
+            "heading": -0.15173248317406665,
+            "angularVelocity": -0.3007361695568274,
+            "velocityX": -0.5198224609468644,
+            "velocityY": -3.657405465518488,
+            "timestamp": 0.4630229418641562
+        },
+        {
+            "x": 2.1445157307124236,
+            "y": 5.659048701686038,
+            "heading": -0.1517325271793822,
+            "angularVelocity": -0.0000010454308573871989,
+            "velocityX": -0.5456254744443967,
+            "velocityY": -3.7433844944292183,
+            "timestamp": 0.5051159365790795
+        },
+        {
+            "x": 2.1215487153797565,
+            "y": 5.501478438692377,
+            "heading": -0.1517325711844689,
+            "angularVelocity": -0.0000010454254199880688,
+            "velocityX": -0.5456255960929445,
+            "velocityY": -3.7433844767000393,
+            "timestamp": 0.5472089312940028
+        },
+        {
+            "x": 2.0985817251133763,
+            "y": 5.343908172045118,
+            "heading": -0.15173261518953962,
+            "angularVelocity": -0.000001045425040533604,
+            "velocityX": -0.5456250005951166,
+            "velocityY": -3.7433845634983034,
+            "timestamp": 0.5893019260089262
+        },
+        {
+            "x": 2.075614762728362,
+            "y": 5.186337901333946,
+            "heading": -0.15173265919459192,
+            "angularVelocity": -0.0000010454246033549915,
+            "velocityX": -0.5456243382196686,
+            "velocityY": -3.743384660044355,
+            "timestamp": 0.6313949207238495
+        },
+        {
+            "x": 2.0526478312972185,
+            "y": 5.028767626111028,
+            "heading": -0.15173270319962293,
+            "angularVelocity": -0.0000010454240970480152,
+            "velocityX": -0.5456236028509798,
+            "velocityY": -3.7433847672295513,
+            "timestamp": 0.6734879154387728
+        },
+        {
+            "x": 2.0296809339445536,
+            "y": 4.871197345920939,
+            "heading": -0.15173274720462945,
+            "angularVelocity": -0.0000010454235162635837,
+            "velocityX": -0.5456227932512114,
+            "velocityY": -3.743384885234266,
+            "timestamp": 0.7155809101536962
+        },
+        {
+            "x": 2.0067140734575584,
+            "y": 4.7136270603574335,
+            "heading": -0.15173279120960867,
+            "angularVelocity": -0.0000010454228670132247,
+            "velocityX": -0.5456219174363699,
+            "velocityY": -3.7433850128900636,
+            "timestamp": 0.7576739048686195
+        },
+        {
+            "x": 1.983747251812091,
+            "y": 4.55605676913253,
+            "heading": -0.15173283521455908,
+            "angularVelocity": -0.000001045422182833939,
+            "velocityX": -0.545620994681206,
+            "velocityY": -3.743385147387475,
+            "timestamp": 0.7997668995835429
+        },
+        {
+            "x": 1.9607804702256346,
+            "y": 4.398486472068785,
+            "heading": -0.15173287921948736,
+            "angularVelocity": -0.0000010454216570158517,
+            "velocityX": -0.5456200430024101,
+            "velocityY": -3.7433852861004024,
+            "timestamp": 0.8418598942984662
+        },
+        {
+            "x": 1.9378163633559966,
+            "y": 4.240915785172817,
+            "heading": -0.15173292322446258,
+            "angularVelocity": -0.0000010454227717672262,
+            "velocityX": -0.5455564999630786,
+            "velocityY": -3.7433945473141725,
+            "timestamp": 0.8839528890133895
+        },
+        {
+            "x": 1.9275487572437193,
+            "y": 4.082011890541997,
+            "heading": -0.15173296776197548,
+            "angularVelocity": -0.000001058074228463605,
+            "velocityX": -0.2439267194414494,
+            "velocityY": -3.775067459728259,
+            "timestamp": 0.9260458837283129
+        },
+        {
+            "x": 1.9320519221515333,
+            "y": 3.9228403083205765,
+            "heading": -0.15173301420750987,
+            "angularVelocity": -0.0000011034029462076059,
+            "velocityX": 0.10698133830372927,
+            "velocityY": -3.781426892988182,
+            "timestamp": 0.9681388784432362
+        },
+        {
+            "x": 1.9512874233679367,
+            "y": 3.764771127717043,
+            "heading": -0.15173306433513473,
+            "angularVelocity": -0.0000011908780826817618,
+            "velocityX": 0.45697630559851177,
+            "velocityY": -3.7552372235348,
+            "timestamp": 1.0102318731581594
+        },
+        {
+            "x": 1.9850897622274482,
+            "y": 3.609164986787025,
+            "heading": -0.15173312063283023,
+            "angularVelocity": -0.0000013374599716310608,
+            "velocityX": 0.8030395339756486,
+            "velocityY": -3.6967229816711127,
+            "timestamp": 1.0523248678730828
+        },
+        {
+            "x": 2.0331680016062417,
+            "y": 3.4573613296965586,
+            "heading": -0.1517331870158403,
+            "angularVelocity": -0.0000015770560044658274,
+            "velocityX": 1.142190991741159,
+            "velocityY": -3.606387668982997,
+            "timestamp": 1.094417862588006
+        },
+        {
+            "x": 2.0951083019579966,
+            "y": 3.3106668726225337,
+            "heading": -0.15173327041919607,
+            "angularVelocity": -0.0000019814070334134687,
+            "velocityX": 1.4715108956074094,
+            "velocityY": -3.4850088017619067,
+            "timestamp": 1.1365108573029294
+        },
+        {
+            "x": 2.170377492904663,
+            "y": 3.170344352722168,
+            "heading": -0.1517333779044128,
+            "angularVelocity": -0.000002553517929494838,
+            "velocityX": 1.7881643122906952,
+            "velocityY": -3.333631186155945,
+            "timestamp": 1.1786038520178528
+        },
+        {
+            "x": 2.2421653916580504,
+            "y": 3.058317386598826,
+            "heading": -0.1517334835913584,
+            "angularVelocity": -0.0000030048345176781924,
+            "velocityX": 2.041035009880172,
+            "velocityY": -3.1850905776457417,
+            "timestamp": 1.213776153524627
+        },
+        {
+            "x": 2.322415894828655,
+            "y": 2.9521882028356305,
+            "heading": -0.15173356325163515,
+            "angularVelocity": -0.000002264858236060329,
+            "velocityX": 2.2816392369190814,
+            "velocityY": -3.0174079948323826,
+            "timestamp": 1.2489484550314012
+        },
+        {
+            "x": 2.4106466853387967,
+            "y": 2.8525946456824203,
+            "heading": -0.1517336276365959,
+            "angularVelocity": -0.0000018305586497777263,
+            "velocityX": 2.5085304836576707,
+            "velocityY": -2.8315905666289245,
+            "timestamp": 1.2841207565381754
+        },
+        {
+            "x": 2.5063274836460994,
+            "y": 2.7601352747554455,
+            "heading": -0.15173368243148194,
+            "angularVelocity": -0.0000015578987915956282,
+            "velocityX": 2.720345107040385,
+            "velocityY": -2.628755212654101,
+            "timestamp": 1.3192930580449496
+        },
+        {
+            "x": 2.6088832332920924,
+            "y": 2.67536576592715,
+            "heading": -0.15173373101035148,
+            "angularVelocity": -0.000001381168347773627,
+            "velocityX": 2.915810033819959,
+            "velocityY": -2.410121180496788,
+            "timestamp": 1.3544653595517238
+        },
+        {
+            "x": 2.7176975542058304,
+            "y": 2.598795565150263,
+            "heading": -0.1517337755722331,
+            "angularVelocity": -0.0000012669595030854418,
+            "velocityX": 3.0937503732242226,
+            "velocityY": -2.1770028544233733,
+            "timestamp": 1.389637661058498
+        },
+        {
+            "x": 2.8321164386506434,
+            "y": 2.5308848045360426,
+            "heading": -0.15173381767917685,
+            "angularVelocity": -0.0000011971620259359605,
+            "velocityX": 3.2530963156555406,
+            "velocityY": -1.930802299108592,
+            "timestamp": 1.4248099625652721
+        },
+        {
+            "x": 2.951452140465328,
+            "y": 2.4720414272198683,
+            "heading": -0.15173385854226368,
+            "angularVelocity": -0.0000011617973536146762,
+            "velocityX": 3.3928886283344455,
+            "velocityY": -1.6730033235055968,
+            "timestamp": 1.4599822640720463
+        },
+        {
+            "x": 3.0749866313948626,
+            "y": 2.4226169148084327,
+            "heading": -0.151733899192453,
+            "angularVelocity": -0.000001155744366153098,
+            "velocityX": 3.512266347021429,
+            "velocityY": -1.4052112114959712,
+            "timestamp": 1.4951545655788205
+        },
+        {
+            "x": 3.1992028583958065,
+            "y": 2.3749316777596947,
+            "heading": -0.1517339398689439,
+            "angularVelocity": -0.0000011564921588237654,
+            "velocityX": 3.5316491011263524,
+            "velocityY": -1.3557610678264387,
+            "timestamp": 1.5303268670855947
+        },
+        {
+            "x": 3.3234191450837622,
+            "y": 2.3272465961911335,
+            "heading": -0.15173398054544393,
+            "angularVelocity": -0.0000011564924185481092,
+            "velocityX": 3.5316507981154315,
+            "velocityY": -1.355756647297499,
+            "timestamp": 1.565499168592369
+        },
+        {
+            "x": 3.4476354415156054,
+            "y": 2.2795615400047207,
+            "heading": -0.15173402122195195,
+            "angularVelocity": -0.0000011564926460273957,
+            "velocityX": 3.5316510751484063,
+            "velocityY": -1.3557559256458895,
+            "timestamp": 1.6006714700991431
+        },
+        {
+            "x": 3.57185174767356,
+            "y": 2.231876509154167,
+            "heading": -0.15173406189847174,
+            "angularVelocity": -0.0000011564929795008858,
+            "velocityX": 3.531651351675977,
+            "velocityY": -1.355755205310334,
+            "timestamp": 1.6358437716059173
+        },
+        {
+            "x": 3.6960680635749434,
+            "y": 2.1841915036846027,
+            "heading": -0.15173410257500647,
+            "angularVelocity": -0.000001156493404477995,
+            "velocityX": 3.5316516286959314,
+            "velocityY": -1.3557544836916666,
+            "timestamp": 1.6710160731126915
+        },
+        {
+            "x": 3.820284389246338,
+            "y": 2.136506523665285,
+            "heading": -0.15173414325155568,
+            "angularVelocity": -0.0000011564938164652249,
+            "velocityX": 3.5316519064716276,
+            "velocityY": -1.3557537601039011,
+            "timestamp": 1.7061883746194657
+        },
+        {
+            "x": 3.9445007247793735,
+            "y": 2.0888215693349235,
+            "heading": -0.15173418392811813,
+            "angularVelocity": -0.0000011564941935950735,
+            "velocityX": 3.5316521868525257,
+            "velocityY": -1.3557530297293134,
+            "timestamp": 1.74136067612624
+        },
+        {
+            "x": 4.068717070394662,
+            "y": 2.0411366412682117,
+            "heading": -0.1517342246046924,
+            "angularVelocity": -0.0000011564945294151802,
+            "velocityX": 3.531652473505735,
+            "velocityY": -1.3557522830153055,
+            "timestamp": 1.776532977633014
+        },
+        {
+            "x": 4.192933426524707,
+            "y": 1.9934517405918115,
+            "heading": -0.15173426528127656,
+            "angularVelocity": -0.0000011564948108738022,
+            "velocityX": 3.5316527724556495,
+            "velocityY": -1.3557515042686648,
+            "timestamp": 1.8117052791397883
+        },
+        {
+            "x": 4.3171498402859925,
+            "y": 1.9457669900421575,
+            "heading": -0.1517343059583456,
+            "angularVelocity": -0.0000011565085966842365,
+            "velocityX": 3.531654410996122,
+            "velocityY": -1.3557472359455842,
+            "timestamp": 1.8468775806465625
+        },
+        {
+            "x": 4.438199866372425,
+            "y": 1.9006463371040783,
+            "heading": -0.16306820061092758,
+            "angularVelocity": -0.3222392100329015,
+            "velocityX": 3.441629375976964,
+            "velocityY": -1.282846188765579,
+            "timestamp": 1.8820498821533367
+        },
+        {
+            "x": 4.551333178258229,
+            "y": 1.8581105785728176,
+            "heading": -0.19901052601489952,
+            "angularVelocity": -1.0218929061849813,
+            "velocityX": 3.2165456066050515,
+            "velocityY": -1.2093538582645278,
+            "timestamp": 1.9172221836601109
+        },
+        {
+            "x": 4.6559660625940325,
+            "y": 1.8184646011809935,
+            "heading": -0.2548690023306322,
+            "angularVelocity": -1.5881382202121255,
+            "velocityX": 2.9748660125539748,
+            "velocityY": -1.1271931518097666,
+            "timestamp": 1.952394485166885
+        },
+        {
+            "x": 4.750968691238499,
+            "y": 1.7823676048635404,
+            "heading": -0.3144227593810797,
+            "angularVelocity": -1.693200458860429,
+            "velocityX": 2.7010637511499986,
+            "velocityY": -1.0262904265875403,
+            "timestamp": 1.9875667866736593
+        },
+        {
+            "x": 4.836310937621718,
+            "y": 1.749878315803868,
+            "heading": -0.37309459063768413,
+            "angularVelocity": -1.6681260180060764,
+            "velocityX": 2.426404947278819,
+            "velocityY": -0.9237180300360781,
+            "timestamp": 2.0227390881804337
+        },
+        {
+            "x": 4.912035358987092,
+            "y": 1.7210053742684595,
+            "heading": -0.4285254423369764,
+            "angularVelocity": -1.5759802266171352,
+            "velocityX": 2.1529561081121917,
+            "velocityY": -0.8208999780650579,
+            "timestamp": 2.057911389687208
+        },
+        {
+            "x": 4.978188578205751,
+            "y": 1.6957473341798728,
+            "heading": -0.4792367761737124,
+            "angularVelocity": -1.4417974276425698,
+            "velocityX": 1.8808328253957978,
+            "velocityY": -0.71812304019178,
+            "timestamp": 2.0930836911939825
+        },
+        {
+            "x": 5.034811621143831,
+            "y": 1.6741013653395114,
+            "heading": -0.5242175587960242,
+            "angularVelocity": -1.278869471013956,
+            "velocityX": 1.6098759680874073,
+            "velocityY": -0.6154265690060781,
+            "timestamp": 2.128255992700757
+        },
+        {
+            "x": 5.081938937252143,
+            "y": 1.656065298518901,
+            "heading": -0.5627406592667319,
+            "angularVelocity": -1.0952681178195878,
+            "velocityX": 1.3398985590759445,
+            "velocityY": -0.5127917721601607,
+            "timestamp": 2.1634282942075314
+        },
+        {
+            "x": 5.11959917834615,
+            "y": 1.6416378108353973,
+            "heading": -0.5942629145071194,
+            "angularVelocity": -0.8962238434785516,
+            "velocityX": 1.0707357631047747,
+            "velocityY": -0.41019458680362747,
+            "timestamp": 2.1986005957143058
+        },
+        {
+            "x": 5.147816310703936,
+            "y": 1.63081813815951,
+            "heading": -0.6183637008514038,
+            "angularVelocity": -0.6852206228143011,
+            "velocityX": 0.8022543634897267,
+            "velocityY": -0.3076191267666608,
+            "timestamp": 2.23377289722108
+        },
+        {
+            "x": 5.1666106911988186,
+            "y": 1.6236057376407542,
+            "heading": -0.6347043261398958,
+            "angularVelocity": -0.4645878884367218,
+            "velocityX": 0.5343517395716112,
+            "velocityY": -0.20505910076332604,
+            "timestamp": 2.2689451987278546
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.643,
+            "angularVelocity": -0.23585814702817923,
+            "velocityX": 0.26695178873562564,
+            "velocityY": -0.10251639745723456,
+            "timestamp": 2.304117500234629
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.643,
+            "angularVelocity": -2.979902278450704e-33,
+            "velocityX": -7.596111021745797e-35,
+            "velocityY": -9.748244176889607e-35,
+            "timestamp": 2.3392898017414034
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.1.traj
new file mode 100644
index 00000000..a5fd0bb0
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.1.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": 1.792103156985809e-33,
+            "velocityX": -7.27073415334495e-34,
+            "velocityY": -7.853383039035825e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 2.298001341213542,
+            "y": 6.709118496985434,
+            "heading": 0.374359095500822,
+            "angularVelocity": -0.2058832051525312,
+            "velocityX": -0.02630859852023658,
+            "velocityY": -0.6302702831973344,
+            "timestamp": 0.07596979310473732
+        },
+        {
+            "x": 2.294004039374054,
+            "y": 6.613355753676287,
+            "heading": 0.3430620070158405,
+            "angularVelocity": -0.4119675361210102,
+            "velocityX": -0.0526169899393711,
+            "velocityY": -1.2605371081782701,
+            "timestamp": 0.15193958620947465
+        },
+        {
+            "x": 2.2880080040813597,
+            "y": 6.469712250400375,
+            "heading": 0.296082691458658,
+            "angularVelocity": -0.6183946755312791,
+            "velocityX": -0.07892657130746197,
+            "velocityY": -1.8907976105433808,
+            "timestamp": 0.22790937931421196
+        },
+        {
+            "x": 2.280012894319733,
+            "y": 6.278188663497132,
+            "heading": 0.23338596481601495,
+            "angularVelocity": -0.8252849465603376,
+            "velocityX": -0.10524064150870725,
+            "velocityY": -2.5210492101669324,
+            "timestamp": 0.3038791724189493
+        },
+        {
+            "x": 2.270017931664709,
+            "y": 6.038785816229407,
+            "heading": 0.1549313176962556,
+            "angularVelocity": -1.0327084478378958,
+            "velocityX": -0.13156495820969516,
+            "velocityY": -3.1512899730774246,
+            "timestamp": 0.37984896552368663
+        },
+        {
+            "x": 2.2620113109689415,
+            "y": 5.847271344563018,
+            "heading": 0.09176539516362914,
+            "angularVelocity": -0.8314610314331311,
+            "velocityX": -0.10539216139142096,
+            "velocityY": -2.5209292251507525,
+            "timestamp": 0.45581875862842397
+        },
+        {
+            "x": 2.2560057963370115,
+            "y": 5.703635646814395,
+            "heading": 0.0443838554377995,
+            "angularVelocity": -0.6236892031613949,
+            "velocityX": -0.079051349049357,
+            "velocityY": -1.8906948653999391,
+            "timestamp": 0.5317885517331613
+        },
+        {
+            "x": 2.252001935551058,
+            "y": 5.607878550007709,
+            "heading": 0.012794605457377246,
+            "angularVelocity": -0.4158132948561684,
+            "velocityX": -0.05270332618167983,
+            "velocityY": -1.2604627825518706,
+            "timestamp": 0.6077583448378986
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -0.003,
+            "angularVelocity": -0.20790639031492017,
+            "velocityX": -0.026351730987313093,
+            "velocityY": -0.6302314123944018,
+            "timestamp": 0.6837281379426359
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -0.003,
+            "angularVelocity": -6.008173298568887e-34,
+            "velocityX": -7.305982513747354e-33,
+            "velocityY": -3.8813573012664696e-35,
+            "timestamp": 0.7596979310473732
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.1.traj
new file mode 100644
index 00000000..5aab1ee6
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.39,
+            "angularVelocity": -2.4663146867319515e-36,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3001378208847427,
+            "y": 6.713171893956039,
+            "heading": 0.36291265694481545,
+            "angularVelocity": -0.37089801282350154,
+            "velocityX": 0.0018871357066083216,
+            "velocityY": -0.6001237332286558,
+            "timestamp": 0.0730317826428328
+        },
+        {
+            "x": 2.3004082222445,
+            "y": 6.625427179140099,
+            "heading": 0.3106389081779897,
+            "angularVelocity": -0.7157671204942956,
+            "velocityX": 0.0037025162192713427,
+            "velocityY": -1.201459304985902,
+            "timestamp": 0.1460635652856656
+        },
+        {
+            "x": 2.300808308388558,
+            "y": 6.493639269598709,
+            "heading": 0.23621859479744584,
+            "angularVelocity": -1.019012691289514,
+            "velocityX": 0.005478246998500719,
+            "velocityY": -1.804528176258669,
+            "timestamp": 0.2190953479284984
+        },
+        {
+            "x": 2.30134752968001,
+            "y": 6.317621999647275,
+            "heading": 0.14515321533996162,
+            "angularVelocity": -1.2469280655909227,
+            "velocityX": 0.007383378468093947,
+            "velocityY": -2.4101461525629078,
+            "timestamp": 0.2921271305713312
+        },
+        {
+            "x": 2.3020704977057354,
+            "y": 6.097150895386178,
+            "heading": 0.04952404086318575,
+            "angularVelocity": -1.309418598536164,
+            "velocityX": 0.00989936161439861,
+            "velocityY": -3.0188377755932185,
+            "timestamp": 0.36515891321416405
+        },
+        {
+            "x": 2.3031517302727234,
+            "y": 5.834309739739576,
+            "heading": 0.004115026920641748,
+            "angularVelocity": -0.6217705812361183,
+            "velocityX": 0.014804959263771778,
+            "velocityY": -3.598996849522972,
+            "timestamp": 0.4381906958569969
+        },
+        {
+            "x": 2.303521267718626,
+            "y": 5.558035105167069,
+            "heading": 0.00411499765356137,
+            "angularVelocity": -4.007444337013033e-7,
+            "velocityX": 0.005059953797233799,
+            "velocityY": -3.7829370251531125,
+            "timestamp": 0.5112224784998297
+        },
+        {
+            "x": 2.3038907744757537,
+            "y": 5.281760470553516,
+            "heading": 0.004114968386478979,
+            "angularVelocity": -4.0074446127092794e-7,
+            "velocityX": 0.005059533586019425,
+            "velocityY": -3.782937025715139,
+            "timestamp": 0.5842542611426625
+        },
+        {
+            "x": 2.3020058670727765,
+            "y": 5.019317688425813,
+            "heading": -0.04121766182368216,
+            "angularVelocity": -0.6207246840990261,
+            "velocityX": -0.02580941248819387,
+            "velocityY": -3.5935420529332935,
+            "timestamp": 0.6572860437854954
+        },
+        {
+            "x": 2.301170882315387,
+            "y": 4.798980955675817,
+            "heading": -0.139726223563735,
+            "angularVelocity": -1.348845094221732,
+            "velocityX": -0.011433169603334988,
+            "velocityY": -3.016997871016921,
+            "timestamp": 0.7303178264283282
+        },
+        {
+            "x": 2.3006513716532107,
+            "y": 4.623098256624685,
+            "heading": -0.23378951684706242,
+            "angularVelocity": -1.2879775062228829,
+            "velocityX": -0.007113487352719777,
+            "velocityY": -2.4083035178163246,
+            "timestamp": 0.803349609071161
+        },
+        {
+            "x": 2.300310703422249,
+            "y": 4.491429967185545,
+            "heading": -0.31073488166813007,
+            "angularVelocity": -1.0535873839664371,
+            "velocityX": -0.004664657203118652,
+            "velocityY": -1.8028902578357346,
+            "timestamp": 0.8763813917139939
+        },
+        {
+            "x": 2.3001003821722423,
+            "y": 4.403776446553788,
+            "heading": -0.36487144716496317,
+            "angularVelocity": -0.7412740527174575,
+            "velocityX": -0.0028798591845282696,
+            "velocityY": -1.2002106132398922,
+            "timestamp": 0.9494131743568267
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.393,
+            "angularVelocity": -0.3851549533249317,
+            "velocityX": -0.0013744998219966374,
+            "velocityY": -0.5994163769475577,
+            "timestamp": 1.0224449569996594
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.393,
+            "angularVelocity": -7.841220088894993e-36,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 1.0954767396424923
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.1.traj
new file mode 100644
index 00000000..f6df5a01
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.1.traj	
@@ -0,0 +1,292 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -5.349509897792121e-32,
+            "angularVelocity": -4.120122643775229e-32,
+            "velocityX": 8.47273432885147e-31,
+            "velocityY": -1.4633096492322666e-31,
+            "timestamp": 0
+        },
+        {
+            "x": 2.237003637990514,
+            "y": 5.5993039448743,
+            "heading": -2.4015234029354595e-10,
+            "angularVelocity": -3.4082853646292762e-9,
+            "velocityX": -0.1844472530475389,
+            "velocityY": 0.5578103057326728,
+            "timestamp": 0.070461130729763
+        },
+        {
+            "x": 2.214179391204466,
+            "y": 5.678821295080181,
+            "heading": -7.831932260526302e-10,
+            "angularVelocity": -7.70693396452574e-9,
+            "velocityX": -0.32392677423960453,
+            "velocityY": 1.1285278760503061,
+            "timestamp": 0.140922261459526
+        },
+        {
+            "x": 2.1888413071411663,
+            "y": 5.799659166026344,
+            "heading": -1.7630537350139963e-9,
+            "angularVelocity": -1.3906355788165327e-8,
+            "velocityX": -0.3596037106073644,
+            "velocityY": 1.714957873876257,
+            "timestamp": 0.21138339218928898
+        },
+        {
+            "x": 2.184731423610446,
+            "y": 5.956036692159833,
+            "heading": -3.5093546140825654e-9,
+            "angularVelocity": -2.4783811808465378e-8,
+            "velocityX": -0.0583283788886903,
+            "velocityY": 2.219344545207332,
+            "timestamp": 0.281844522919052
+        },
+        {
+            "x": 2.2210562385399535,
+            "y": 6.10354071192916,
+            "heading": -5.82104685853391e-9,
+            "angularVelocity": -3.2807944511631915e-8,
+            "velocityX": 0.5155298325992919,
+            "velocityY": 2.0934097741478808,
+            "timestamp": 0.352305653648815
+        },
+        {
+            "x": 2.290250351402012,
+            "y": 6.2258789967414945,
+            "heading": -8.381712631190757e-9,
+            "angularVelocity": -3.6341419943068046e-8,
+            "velocityX": 0.9820182013148953,
+            "velocityY": 1.7362520802853605,
+            "timestamp": 0.422766784378578
+        },
+        {
+            "x": 2.38835072517395,
+            "y": 6.318583965301514,
+            "heading": -1.1074238763723672e-8,
+            "angularVelocity": -3.821280465498069e-8,
+            "velocityX": 1.392262269357751,
+            "velocityY": 1.315689481497163,
+            "timestamp": 0.493227915108341
+        },
+        {
+            "x": 2.4799014983496455,
+            "y": 6.371301429692481,
+            "heading": -1.319458225812021e-8,
+            "angularVelocity": -3.903749361340101e-8,
+            "velocityX": 1.6855317920452093,
+            "velocityY": 0.9705757706637901,
+            "timestamp": 0.5475435759628242
+        },
+        {
+            "x": 2.5874215406057264,
+            "y": 6.405308001046697,
+            "heading": -1.535577963780212e-8,
+            "angularVelocity": -3.978965207197302e-8,
+            "velocityX": 1.979540349233062,
+            "velocityY": 0.626091459097589,
+            "timestamp": 0.6018592368173074
+        },
+        {
+            "x": 2.7109670001208555,
+            "y": 6.420651747906481,
+            "heading": -1.7552309308039307e-8,
+            "angularVelocity": -4.044015305772357e-8,
+            "velocityX": 2.2745826447141577,
+            "velocityY": 0.2824921324780203,
+            "timestamp": 0.6561748976717905
+        },
+        {
+            "x": 2.850621987061303,
+            "y": 6.417405226954699,
+            "heading": -1.9775875083794707e-8,
+            "angularVelocity": -4.093791389017331e-8,
+            "velocityX": 2.571173483741952,
+            "velocityY": -0.059771360579454216,
+            "timestamp": 0.7104905585262736
+        },
+        {
+            "x": 3.006526369448995,
+            "y": 6.395690573007363,
+            "heading": -2.2012612720253862e-8,
+            "angularVelocity": -4.1180421619075874e-8,
+            "velocityX": 2.8703394184132693,
+            "velocityY": -0.39978624220811715,
+            "timestamp": 0.7648062193807568
+        },
+        {
+            "x": 3.178958586879986,
+            "y": 6.35575655651524,
+            "heading": -2.4234645855460274e-8,
+            "angularVelocity": -4.090970062204104e-8,
+            "velocityX": 3.1746316756057436,
+            "velocityY": -0.7352210368724164,
+            "timestamp": 0.8191218802352399
+        },
+        {
+            "x": 3.368741313034932,
+            "y": 6.298384837056506,
+            "heading": -2.6356859092562007e-8,
+            "angularVelocity": -3.9071928501624834e-8,
+            "velocityX": 3.494070092598876,
+            "velocityY": -1.0562647781255272,
+            "timestamp": 0.873437541089723
+        },
+        {
+            "x": 3.5700972080230713,
+            "y": 6.2574591636657715,
+            "heading": -2.635685976829305e-8,
+            "angularVelocity": -1.2440816646696897e-14,
+            "velocityX": 3.7071425040054167,
+            "velocityY": -0.7534783293344505,
+            "timestamp": 0.9277532019442062
+        },
+        {
+            "x": 3.903312814324664,
+            "y": 6.255439725872096,
+            "heading": -2.6356859842955344e-8,
+            "angularVelocity": -8.476143761744691e-16,
+            "velocityX": 3.7828709995267413,
+            "velocityY": -0.02292591506032139,
+            "timestamp": 1.0158385758158621
+        },
+        {
+            "x": 4.231215212285405,
+            "y": 6.314741822504602,
+            "heading": -2.6356859970624137e-8,
+            "angularVelocity": -1.4493758784250442e-15,
+            "velocityX": 3.722552150839135,
+            "velocityY": 0.6732343181536394,
+            "timestamp": 1.103923949687518
+        },
+        {
+            "x": 4.548594281876572,
+            "y": 6.416266440930298,
+            "heading": -2.6357038616847798e-8,
+            "angularVelocity": -2.0281055655946166e-12,
+            "velocityX": 3.603084776124604,
+            "velocityY": 1.1525706703347043,
+            "timestamp": 1.1920093235591738
+        },
+        {
+            "x": 4.864492921080643,
+            "y": 6.522307835013861,
+            "heading": -2.6356860621748925e-8,
+            "angularVelocity": 2.02071358534527e-12,
+            "velocityX": 3.5862780084505173,
+            "velocityY": 1.2038479203550279,
+            "timestamp": 1.2800946974308296
+        },
+        {
+            "x": 5.180407145227995,
+            "y": 6.628302763705304,
+            "heading": -2.6356859722122177e-8,
+            "angularVelocity": 1.021313250736518e-14,
+            "velocityX": 3.586454938732789,
+            "velocityY": 1.2033204153921748,
+            "timestamp": 1.3681800713024854
+        },
+        {
+            "x": 5.4963196348481045,
+            "y": 6.734302860663818,
+            "heading": -2.6356858783882045e-8,
+            "angularVelocity": 1.0651495820397439e-14,
+            "velocityX": 3.58643524725062,
+            "velocityY": 1.20337908892381,
+            "timestamp": 1.4562654451741412
+        },
+        {
+            "x": 5.81223242108415,
+            "y": 6.8403020736225555,
+            "heading": -2.6356857847826716e-8,
+            "angularVelocity": 1.0626692486301617e-14,
+            "velocityX": 3.586438614621235,
+            "velocityY": 1.2033690532024592,
+            "timestamp": 1.544350819045797
+        },
+        {
+            "x": 6.128145213013902,
+            "y": 6.946301269622288,
+            "heading": -2.6356856889534724e-8,
+            "angularVelocity": 1.0879135888139458e-14,
+            "velocityX": 3.5864386792612257,
+            "velocityY": 1.2033688606688582,
+            "timestamp": 1.632436192917453
+        },
+        {
+            "x": 6.444057998192988,
+            "y": 7.052300485742325,
+            "heading": -2.6356856001580027e-8,
+            "angularVelocity": 1.0080624290280854e-14,
+            "velocityX": 3.5864386026236104,
+            "velocityY": 1.2033690890866424,
+            "timestamp": 1.7205215667891087
+        },
+        {
+            "x": 6.759970783516966,
+            "y": 7.158299701416182,
+            "heading": -2.635685511579164e-8,
+            "angularVelocity": 1.0056027630784321e-14,
+            "velocityX": 3.586438604268464,
+            "velocityY": 1.2033690840214906,
+            "timestamp": 1.8086069406607646
+        },
+        {
+            "x": 7.0666471679768685,
+            "y": 7.261199790661366,
+            "heading": -1.748457582258916e-8,
+            "angularVelocity": 1.0072373848515286e-7,
+            "velocityX": 3.4815812317231813,
+            "velocityY": 1.1681858715050373,
+            "timestamp": 1.8966923145324204
+        },
+        {
+            "x": 7.311988294075674,
+            "y": 7.343519871122703,
+            "heading": -1.0463867035430883e-8,
+            "angularVelocity": 7.970353325086049e-8,
+            "velocityX": 2.7852651957526704,
+            "velocityY": 0.9345488001177432,
+            "timestamp": 1.9847776884040762
+        },
+        {
+            "x": 7.495994144762843,
+            "y": 7.405259934452133,
+            "heading": -5.222908649794349e-9,
+            "angularVelocity": 5.94986783168245e-8,
+            "velocityX": 2.088948966213813,
+            "velocityY": 0.7009116339580341,
+            "timestamp": 2.072863062275732
+        },
+        {
+            "x": 7.618664714352163,
+            "y": 7.446419977873495,
+            "heading": -1.738724662079323e-9,
+            "angularVelocity": 3.955466249815561e-8,
+            "velocityX": 1.3926326721215396,
+            "velocityY": 0.4672744362816174,
+            "timestamp": 2.160948436147388
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": -8.289937690891028e-31,
+            "angularVelocity": 1.9739103024170296e-8,
+            "velocityX": 0.6963163457465601,
+            "velocityY": 0.23363722286151184,
+            "timestamp": 2.2490338100190437
+        },
+        {
+            "x": 7.68,
+            "y": 7.467,
+            "heading": 1.1211425849455439e-31,
+            "angularVelocity": 1.0684035256675139e-29,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 2.3371191838906995
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.traj
index 70287ec9..f6df5a01 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.traj	
@@ -3,191 +3,290 @@
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -3.731399929679607e-32,
-            "velocityY": 5.877471754111438e-38,
+            "heading": -5.349509897792121e-32,
+            "angularVelocity": -4.120122643775229e-32,
+            "velocityX": 8.47273432885147e-31,
+            "velocityY": -1.4633096492322666e-31,
             "timestamp": 0
         },
         {
-            "x": 2.3271275380851444,
-            "y": 5.587086964112038,
-            "heading": 3.953847862678034e-18,
-            "angularVelocity": 3.9932078666702264e-17,
-            "velocityX": 0.7789533333445218,
-            "velocityY": 0.2735661154121556,
-            "timestamp": 0.09901432445765257
-        },
-        {
-            "x": 2.4813826113143547,
-            "y": 5.641260891303218,
-            "heading": 1.1398162294527873e-19,
-            "angularVelocity": -3.8780916638560175e-17,
-            "velocityX": 1.557906636985473,
-            "velocityY": 0.5471322203925046,
-            "timestamp": 0.19802864891530514
-        },
-        {
-            "x": 2.7127652138054734,
-            "y": 5.722521779507741,
-            "heading": 2.1553394674505063e-18,
-            "angularVelocity": 2.0616792512846884e-17,
-            "velocityX": 2.336859881219295,
-            "velocityY": 0.8206983045092451,
-            "timestamp": 0.2970429733729577
-        },
-        {
-            "x": 3.0212753279120443,
-            "y": 5.830869622528225,
-            "heading": -1.0672476097054788e-17,
-            "angularVelocity": -1.295551486240031e-16,
-            "velocityX": 3.1158129472318716,
-            "velocityY": 1.0942643260352094,
-            "timestamp": 0.3960572978306103
-        },
-        {
-            "x": 3.3746798137461993,
-            "y": 5.9549842366139965,
-            "heading": 1.8895880094915182e-18,
-            "angularVelocity": 1.2687117924961395e-16,
-            "velocityX": 3.5692258445423475,
-            "velocityY": 1.2535015995473786,
-            "timestamp": 0.49507162228826285
-        },
-        {
-            "x": 3.7280842995803773,
-            "y": 6.079098850699775,
-            "heading": 1.0049319654091473e-17,
-            "angularVelocity": 8.240960778697987e-17,
-            "velocityX": 3.569225844542579,
-            "velocityY": 1.25350159954746,
-            "timestamp": 0.5940859467459154
-        },
-        {
-            "x": 4.081488785414555,
-            "y": 6.203213464785553,
-            "heading": 8.136834826577652e-18,
-            "angularVelocity": -1.931523381075779e-17,
-            "velocityX": 3.569225844542579,
-            "velocityY": 1.25350159954746,
-            "timestamp": 0.693100271203568
-        },
-        {
-            "x": 4.434893271248733,
-            "y": 6.327328078871332,
-            "heading": -2.4381204969164037e-18,
-            "angularVelocity": -1.0680227715899811e-16,
-            "velocityX": 3.5692258445425793,
-            "velocityY": 1.2535015995474599,
-            "timestamp": 0.7921145956612206
-        },
-        {
-            "x": 4.788297757082911,
-            "y": 6.4514426929571105,
-            "heading": 3.192038150785043e-17,
-            "angularVelocity": 3.47005367081625e-16,
-            "velocityX": 3.569225844542579,
-            "velocityY": 1.25350159954746,
-            "timestamp": 0.8911289201188731
-        },
-        {
-            "x": 5.141702242917089,
-            "y": 6.575557307042889,
-            "heading": 6.016026907559514e-17,
-            "angularVelocity": 2.8521012209839933e-16,
-            "velocityX": 3.569225844542579,
-            "velocityY": 1.2535015995474599,
-            "timestamp": 0.9901432445765257
-        },
-        {
-            "x": 5.495106728751266,
-            "y": 6.699671921128667,
-            "heading": 2.4186102644429564e-17,
-            "angularVelocity": -3.63322848771759e-16,
-            "velocityX": 3.569225844542579,
-            "velocityY": 1.2535015995474599,
-            "timestamp": 1.0891575690341782
-        },
-        {
-            "x": 5.848511214585444,
-            "y": 6.823786535214446,
-            "heading": -1.2421929874703125e-18,
-            "angularVelocity": -2.568143121812034e-16,
-            "velocityX": 3.5692258445425793,
-            "velocityY": 1.2535015995474599,
-            "timestamp": 1.1881718934918308
-        },
-        {
-            "x": 6.2019157004196215,
-            "y": 6.9479011493002245,
-            "heading": 2.3184212364315377e-18,
-            "angularVelocity": 3.596059704901295e-17,
-            "velocityX": 3.569225844542579,
-            "velocityY": 1.2535015995474599,
-            "timestamp": 1.2871862179494835
-        },
-        {
-            "x": 6.5553201862538,
-            "y": 7.072015763386003,
-            "heading": 3.1459219033425486e-17,
-            "angularVelocity": 2.9430890890068574e-16,
-            "velocityX": 3.569225844542579,
-            "velocityY": 1.25350159954746,
-            "timestamp": 1.3862005424071362
-        },
-        {
-            "x": 6.9087246720879545,
-            "y": 7.196130377471773,
-            "heading": 2.9741502939674576e-17,
-            "angularVelocity": -1.7348157482865667e-17,
-            "velocityX": 3.5692258445423475,
-            "velocityY": 1.2535015995473786,
-            "timestamp": 1.4852148668647889
-        },
-        {
-            "x": 7.217234786194527,
-            "y": 7.304478220492258,
-            "heading": 1.2383304928032073e-17,
-            "angularVelocity": -1.753099667759744e-16,
-            "velocityX": 3.115812947231872,
-            "velocityY": 1.0942643260352094,
-            "timestamp": 1.5842291913224416
-        },
-        {
-            "x": 7.448617388685645,
-            "y": 7.385739108696781,
-            "heading": -3.803725574168796e-18,
-            "angularVelocity": -1.6348170333424006e-16,
-            "velocityX": 2.336859881219295,
-            "velocityY": 0.8206983045092451,
-            "timestamp": 1.6832435157800942
-        },
-        {
-            "x": 7.602872461914855,
-            "y": 7.439913035887961,
-            "heading": -2.4779119751079125e-18,
-            "angularVelocity": 1.3390118812558602e-17,
-            "velocityX": 1.557906636985473,
-            "velocityY": 0.5471322203925049,
-            "timestamp": 1.782257840237747
+            "x": 2.237003637990514,
+            "y": 5.5993039448743,
+            "heading": -2.4015234029354595e-10,
+            "angularVelocity": -3.4082853646292762e-9,
+            "velocityX": -0.1844472530475389,
+            "velocityY": 0.5578103057326728,
+            "timestamp": 0.070461130729763
+        },
+        {
+            "x": 2.214179391204466,
+            "y": 5.678821295080181,
+            "heading": -7.831932260526302e-10,
+            "angularVelocity": -7.70693396452574e-9,
+            "velocityX": -0.32392677423960453,
+            "velocityY": 1.1285278760503061,
+            "timestamp": 0.140922261459526
+        },
+        {
+            "x": 2.1888413071411663,
+            "y": 5.799659166026344,
+            "heading": -1.7630537350139963e-9,
+            "angularVelocity": -1.3906355788165327e-8,
+            "velocityX": -0.3596037106073644,
+            "velocityY": 1.714957873876257,
+            "timestamp": 0.21138339218928898
+        },
+        {
+            "x": 2.184731423610446,
+            "y": 5.956036692159833,
+            "heading": -3.5093546140825654e-9,
+            "angularVelocity": -2.4783811808465378e-8,
+            "velocityX": -0.0583283788886903,
+            "velocityY": 2.219344545207332,
+            "timestamp": 0.281844522919052
+        },
+        {
+            "x": 2.2210562385399535,
+            "y": 6.10354071192916,
+            "heading": -5.82104685853391e-9,
+            "angularVelocity": -3.2807944511631915e-8,
+            "velocityX": 0.5155298325992919,
+            "velocityY": 2.0934097741478808,
+            "timestamp": 0.352305653648815
+        },
+        {
+            "x": 2.290250351402012,
+            "y": 6.2258789967414945,
+            "heading": -8.381712631190757e-9,
+            "angularVelocity": -3.6341419943068046e-8,
+            "velocityX": 0.9820182013148953,
+            "velocityY": 1.7362520802853605,
+            "timestamp": 0.422766784378578
+        },
+        {
+            "x": 2.38835072517395,
+            "y": 6.318583965301514,
+            "heading": -1.1074238763723672e-8,
+            "angularVelocity": -3.821280465498069e-8,
+            "velocityX": 1.392262269357751,
+            "velocityY": 1.315689481497163,
+            "timestamp": 0.493227915108341
+        },
+        {
+            "x": 2.4799014983496455,
+            "y": 6.371301429692481,
+            "heading": -1.319458225812021e-8,
+            "angularVelocity": -3.903749361340101e-8,
+            "velocityX": 1.6855317920452093,
+            "velocityY": 0.9705757706637901,
+            "timestamp": 0.5475435759628242
+        },
+        {
+            "x": 2.5874215406057264,
+            "y": 6.405308001046697,
+            "heading": -1.535577963780212e-8,
+            "angularVelocity": -3.978965207197302e-8,
+            "velocityX": 1.979540349233062,
+            "velocityY": 0.626091459097589,
+            "timestamp": 0.6018592368173074
+        },
+        {
+            "x": 2.7109670001208555,
+            "y": 6.420651747906481,
+            "heading": -1.7552309308039307e-8,
+            "angularVelocity": -4.044015305772357e-8,
+            "velocityX": 2.2745826447141577,
+            "velocityY": 0.2824921324780203,
+            "timestamp": 0.6561748976717905
+        },
+        {
+            "x": 2.850621987061303,
+            "y": 6.417405226954699,
+            "heading": -1.9775875083794707e-8,
+            "angularVelocity": -4.093791389017331e-8,
+            "velocityX": 2.571173483741952,
+            "velocityY": -0.059771360579454216,
+            "timestamp": 0.7104905585262736
+        },
+        {
+            "x": 3.006526369448995,
+            "y": 6.395690573007363,
+            "heading": -2.2012612720253862e-8,
+            "angularVelocity": -4.1180421619075874e-8,
+            "velocityX": 2.8703394184132693,
+            "velocityY": -0.39978624220811715,
+            "timestamp": 0.7648062193807568
+        },
+        {
+            "x": 3.178958586879986,
+            "y": 6.35575655651524,
+            "heading": -2.4234645855460274e-8,
+            "angularVelocity": -4.090970062204104e-8,
+            "velocityX": 3.1746316756057436,
+            "velocityY": -0.7352210368724164,
+            "timestamp": 0.8191218802352399
+        },
+        {
+            "x": 3.368741313034932,
+            "y": 6.298384837056506,
+            "heading": -2.6356859092562007e-8,
+            "angularVelocity": -3.9071928501624834e-8,
+            "velocityX": 3.494070092598876,
+            "velocityY": -1.0562647781255272,
+            "timestamp": 0.873437541089723
+        },
+        {
+            "x": 3.5700972080230713,
+            "y": 6.2574591636657715,
+            "heading": -2.635685976829305e-8,
+            "angularVelocity": -1.2440816646696897e-14,
+            "velocityX": 3.7071425040054167,
+            "velocityY": -0.7534783293344505,
+            "timestamp": 0.9277532019442062
+        },
+        {
+            "x": 3.903312814324664,
+            "y": 6.255439725872096,
+            "heading": -2.6356859842955344e-8,
+            "angularVelocity": -8.476143761744691e-16,
+            "velocityX": 3.7828709995267413,
+            "velocityY": -0.02292591506032139,
+            "timestamp": 1.0158385758158621
+        },
+        {
+            "x": 4.231215212285405,
+            "y": 6.314741822504602,
+            "heading": -2.6356859970624137e-8,
+            "angularVelocity": -1.4493758784250442e-15,
+            "velocityX": 3.722552150839135,
+            "velocityY": 0.6732343181536394,
+            "timestamp": 1.103923949687518
+        },
+        {
+            "x": 4.548594281876572,
+            "y": 6.416266440930298,
+            "heading": -2.6357038616847798e-8,
+            "angularVelocity": -2.0281055655946166e-12,
+            "velocityX": 3.603084776124604,
+            "velocityY": 1.1525706703347043,
+            "timestamp": 1.1920093235591738
+        },
+        {
+            "x": 4.864492921080643,
+            "y": 6.522307835013861,
+            "heading": -2.6356860621748925e-8,
+            "angularVelocity": 2.02071358534527e-12,
+            "velocityX": 3.5862780084505173,
+            "velocityY": 1.2038479203550279,
+            "timestamp": 1.2800946974308296
+        },
+        {
+            "x": 5.180407145227995,
+            "y": 6.628302763705304,
+            "heading": -2.6356859722122177e-8,
+            "angularVelocity": 1.021313250736518e-14,
+            "velocityX": 3.586454938732789,
+            "velocityY": 1.2033204153921748,
+            "timestamp": 1.3681800713024854
+        },
+        {
+            "x": 5.4963196348481045,
+            "y": 6.734302860663818,
+            "heading": -2.6356858783882045e-8,
+            "angularVelocity": 1.0651495820397439e-14,
+            "velocityX": 3.58643524725062,
+            "velocityY": 1.20337908892381,
+            "timestamp": 1.4562654451741412
+        },
+        {
+            "x": 5.81223242108415,
+            "y": 6.8403020736225555,
+            "heading": -2.6356857847826716e-8,
+            "angularVelocity": 1.0626692486301617e-14,
+            "velocityX": 3.586438614621235,
+            "velocityY": 1.2033690532024592,
+            "timestamp": 1.544350819045797
+        },
+        {
+            "x": 6.128145213013902,
+            "y": 6.946301269622288,
+            "heading": -2.6356856889534724e-8,
+            "angularVelocity": 1.0879135888139458e-14,
+            "velocityX": 3.5864386792612257,
+            "velocityY": 1.2033688606688582,
+            "timestamp": 1.632436192917453
+        },
+        {
+            "x": 6.444057998192988,
+            "y": 7.052300485742325,
+            "heading": -2.6356856001580027e-8,
+            "angularVelocity": 1.0080624290280854e-14,
+            "velocityX": 3.5864386026236104,
+            "velocityY": 1.2033690890866424,
+            "timestamp": 1.7205215667891087
+        },
+        {
+            "x": 6.759970783516966,
+            "y": 7.158299701416182,
+            "heading": -2.635685511579164e-8,
+            "angularVelocity": 1.0056027630784321e-14,
+            "velocityX": 3.586438604268464,
+            "velocityY": 1.2033690840214906,
+            "timestamp": 1.8086069406607646
+        },
+        {
+            "x": 7.0666471679768685,
+            "y": 7.261199790661366,
+            "heading": -1.748457582258916e-8,
+            "angularVelocity": 1.0072373848515286e-7,
+            "velocityX": 3.4815812317231813,
+            "velocityY": 1.1681858715050373,
+            "timestamp": 1.8966923145324204
+        },
+        {
+            "x": 7.311988294075674,
+            "y": 7.343519871122703,
+            "heading": -1.0463867035430883e-8,
+            "angularVelocity": 7.970353325086049e-8,
+            "velocityX": 2.7852651957526704,
+            "velocityY": 0.9345488001177432,
+            "timestamp": 1.9847776884040762
+        },
+        {
+            "x": 7.495994144762843,
+            "y": 7.405259934452133,
+            "heading": -5.222908649794349e-9,
+            "angularVelocity": 5.94986783168245e-8,
+            "velocityX": 2.088948966213813,
+            "velocityY": 0.7009116339580341,
+            "timestamp": 2.072863062275732
+        },
+        {
+            "x": 7.618664714352163,
+            "y": 7.446419977873495,
+            "heading": -1.738724662079323e-9,
+            "angularVelocity": 3.955466249815561e-8,
+            "velocityX": 1.3926326721215396,
+            "velocityY": 0.4672744362816174,
+            "timestamp": 2.160948436147388
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 2.5025792615041696e-17,
-            "velocityX": 0.7789533333445218,
-            "velocityY": 0.27356611541215564,
-            "timestamp": 1.8812721646953996
+            "heading": -8.289937690891028e-31,
+            "angularVelocity": 1.9739103024170296e-8,
+            "velocityX": 0.6963163457465601,
+            "velocityY": 0.23363722286151184,
+            "timestamp": 2.2490338100190437
         },
         {
             "x": 7.68,
             "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 1.3533974507170236e-34,
+            "heading": 1.1211425849455439e-31,
+            "angularVelocity": 1.0684035256675139e-29,
+            "velocityX": 0,
             "velocityY": 0,
-            "timestamp": 1.9802864891530523
+            "timestamp": 2.3371191838906995
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.1.traj
new file mode 100644
index 00000000..7a28104f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.1.traj	
@@ -0,0 +1,616 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 5.330261523342824e-32,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.216523220338838,
+            "y": 5.608939409819235,
+            "heading": -0.000395848810607471,
+            "angularVelocity": -0.004694167537861253,
+            "velocityX": -0.3969839194829395,
+            "velocityY": 0.5803473011402309,
+            "timestamp": 0.08432779772229798
+        },
+        {
+            "x": 2.1530740053514283,
+            "y": 5.7090393916763,
+            "heading": -0.0013136261454272055,
+            "angularVelocity": -0.010883449581379904,
+            "velocityX": -0.7524116210926717,
+            "velocityY": 1.1870342231242381,
+            "timestamp": 0.16865559544459596
+        },
+        {
+            "x": 2.071477218025725,
+            "y": 5.86558734321221,
+            "heading": -0.0031284327789465032,
+            "angularVelocity": -0.02152085886904205,
+            "velocityX": -0.9676143517279073,
+            "velocityY": 1.8564216754650953,
+            "timestamp": 0.25298339316689394
+        },
+        {
+            "x": 2.0458269370451427,
+            "y": 6.041770495606775,
+            "heading": -0.006595224749493174,
+            "angularVelocity": -0.04111090368993278,
+            "velocityX": -0.30417349525777404,
+            "velocityY": 2.089265427928733,
+            "timestamp": 0.3373111908891919
+        },
+        {
+            "x": 2.0715234655116643,
+            "y": 6.188301290095102,
+            "heading": -0.01049068188832321,
+            "angularVelocity": -0.046194223542478534,
+            "velocityX": 0.30472192041755103,
+            "velocityY": 1.7376333598900595,
+            "timestamp": 0.4216389886114899
+        },
+        {
+            "x": 2.1438515186309814,
+            "y": 6.298208713531494,
+            "heading": -0.014554322717054646,
+            "angularVelocity": -0.04818862745724922,
+            "velocityX": 0.8577011978600798,
+            "velocityY": 1.303335630776598,
+            "timestamp": 0.5059667863337879
+        },
+        {
+            "x": 2.2162950329491684,
+            "y": 6.355974505839093,
+            "heading": -0.017431077998725047,
+            "angularVelocity": -0.04874949538291163,
+            "velocityX": 1.2276278031981451,
+            "velocityY": 0.978899124069331,
+            "timestamp": 0.5649777619156877
+        },
+        {
+            "x": 2.310686009240105,
+            "y": 6.394730061346725,
+            "heading": -0.020327199149560698,
+            "angularVelocity": -0.0490776694043422,
+            "velocityX": 1.5995494966853068,
+            "velocityY": 0.6567516487478545,
+            "timestamp": 0.6239887374975874
+        },
+        {
+            "x": 2.4271886355684367,
+            "y": 6.414666608347948,
+            "heading": -0.023223370555002502,
+            "angularVelocity": -0.04907852101890474,
+            "velocityX": 1.9742535211376202,
+            "velocityY": 0.33784472811424154,
+            "timestamp": 0.6829997130794871
+        },
+        {
+            "x": 2.5660476863967387,
+            "y": 6.416075601325133,
+            "heading": -0.02609042518710973,
+            "angularVelocity": -0.048585108174133695,
+            "velocityX": 2.35310549366504,
+            "velocityY": 0.02387679517735602,
+            "timestamp": 0.7420106886613869
+        },
+        {
+            "x": 2.72766661902605,
+            "y": 6.399454975329908,
+            "heading": -0.02887926513240676,
+            "angularVelocity": -0.04725968208112396,
+            "velocityX": 2.738794453669127,
+            "velocityY": -0.281653130308932,
+            "timestamp": 0.8010216642432866
+        },
+        {
+            "x": 2.912832200300888,
+            "y": 6.365845166245685,
+            "heading": -0.031490096116184224,
+            "angularVelocity": -0.04424314219571676,
+            "velocityX": 3.1378159647242496,
+            "velocityY": -0.5695518291775498,
+            "timestamp": 0.8600326398251863
+        },
+        {
+            "x": 3.123679163604623,
+            "y": 6.318688161285352,
+            "heading": -0.033616023768519236,
+            "angularVelocity": -0.03602597027708684,
+            "velocityX": 3.5730126679757386,
+            "velocityY": -0.7991226122822729,
+            "timestamp": 0.919043615407086
+        },
+        {
+            "x": 3.345972776412964,
+            "y": 6.298208713531494,
+            "heading": -0.03361602546428586,
+            "angularVelocity": -2.8736461454443448e-8,
+            "velocityX": 3.766987591991692,
+            "velocityY": -0.34704472434005124,
+            "timestamp": 0.9780545909889857
+        },
+        {
+            "x": 3.4285825306560853,
+            "y": 6.294597584481868,
+            "heading": -0.033616025709707474,
+            "angularVelocity": -1.1227840838251836e-8,
+            "velocityX": 3.779329612797739,
+            "velocityY": -0.16520624081174923,
+            "timestamp": 0.9999128999238256
+        },
+        {
+            "x": 3.5112703400714786,
+            "y": 6.2949695138762145,
+            "heading": -0.033616025930811776,
+            "angularVelocity": -1.0115343355589466e-8,
+            "velocityX": 3.7829005739596693,
+            "velocityY": 0.01701546974451014,
+            "timestamp": 1.0217712088586655
+        },
+        {
+            "x": 3.5938442713679413,
+            "y": 6.299323633866166,
+            "heading": -0.03361602613301592,
+            "angularVelocity": -9.2506762233922e-9,
+            "velocityX": 3.7776907418875507,
+            "velocityY": 0.19919747693799758,
+            "timestamp": 1.0436295177935053
+        },
+        {
+            "x": 3.6761126565475006,
+            "y": 6.3076498312062705,
+            "heading": -0.03361602632044191,
+            "angularVelocity": -8.574588182780684e-9,
+            "velocityX": 3.763712253532677,
+            "velocityY": 0.3809168113107395,
+            "timestamp": 1.0654878267283452
+        },
+        {
+            "x": 3.7578845383740482,
+            "y": 6.31992876945557,
+            "heading": -0.033616026496316675,
+            "angularVelocity": -8.046128410971048e-9,
+            "velocityX": 3.7409976256768736,
+            "velocityY": 0.5617515191089957,
+            "timestamp": 1.087346135663185
+        },
+        {
+            "x": 3.8389701148011586,
+            "y": 6.336131931270735,
+            "heading": -0.03361602666323835,
+            "angularVelocity": -7.63653221094489e-9,
+            "velocityX": 3.709599707316274,
+            "velocityY": 0.741281581455633,
+            "timestamp": 1.109204444598025
+        },
+        {
+            "x": 3.919181182263655,
+            "y": 6.356221678659879,
+            "heading": -0.03361602682336018,
+            "angularVelocity": -7.3254443467155425e-9,
+            "velocityX": 3.6695916276783906,
+            "velocityY": 0.9190897360373237,
+            "timestamp": 1.1310627535328648
+        },
+        {
+            "x": 3.998331580016101,
+            "y": 6.3801513240916305,
+            "heading": -0.03361602697852209,
+            "angularVelocity": -7.098532288064829e-9,
+            "velocityX": 3.621066844118403,
+            "velocityY": 1.0947619737229577,
+            "timestamp": 1.1529210624677046
+        },
+        {
+            "x": 4.076237649565588,
+            "y": 6.407865179672378,
+            "heading": -0.03361602713034875,
+            "angularVelocity": -6.945947165054901e-9,
+            "velocityX": 3.5641398326708003,
+            "velocityY": 1.2678865352009827,
+            "timestamp": 1.1747793714025445
+        },
+        {
+            "x": 4.152718837418924,
+            "y": 6.439298305619894,
+            "heading": -0.033616027280326394,
+            "angularVelocity": -6.861356032634536e-9,
+            "velocityX": 3.4989526445677317,
+            "velocityY": 1.4380401540310954,
+            "timestamp": 1.1966376803373844
+        },
+        {
+            "x": 4.227916828679672,
+            "y": 6.473688356846056,
+            "heading": -0.03361602742978782,
+            "angularVelocity": -6.8377396934186395e-9,
+            "velocityX": 3.4402474356508805,
+            "velocityY": 1.5733171000866744,
+            "timestamp": 1.2184959892722242
+        },
+        {
+            "x": 4.303113417879144,
+            "y": 6.50808147368302,
+            "heading": -0.033616027579243535,
+            "angularVelocity": -6.837478280080917e-9,
+            "velocityX": 3.4401832924786557,
+            "velocityY": 1.5734573493077775,
+            "timestamp": 1.240354298207064
+        },
+        {
+            "x": 4.378310012144908,
+            "y": 6.542474579443129,
+            "heading": -0.03361602772869459,
+            "angularVelocity": -6.837264944013175e-9,
+            "velocityX": 3.4401835242573666,
+            "velocityY": 1.573456842550575,
+            "timestamp": 1.262212607141904
+        },
+        {
+            "x": 4.453506612352025,
+            "y": 6.576867672213151,
+            "heading": -0.03361602787814215,
+            "angularVelocity": -6.837105393969464e-9,
+            "velocityX": 3.4401837960695145,
+            "velocityY": 1.5734562482645857,
+            "timestamp": 1.2840709160767438
+        },
+        {
+            "x": 4.528703218974218,
+            "y": 6.6112607509573404,
+            "heading": -0.03361602802758787,
+            "angularVelocity": -6.837020955916861e-9,
+            "velocityX": 3.440184089554037,
+            "velocityY": 1.5734556065940832,
+            "timestamp": 1.3059292250115837
+        },
+        {
+            "x": 4.603899833102385,
+            "y": 6.645653813290562,
+            "heading": -0.03361602817703386,
+            "angularVelocity": -6.837033689050042e-9,
+            "velocityX": 3.440184432946286,
+            "velocityY": 1.5734548558055208,
+            "timestamp": 1.3277875339464236
+        },
+        {
+            "x": 4.679098505825837,
+            "y": 6.680042374380203,
+            "heading": -0.033616028326483124,
+            "angularVelocity": -6.83718315967473e-9,
+            "velocityX": 3.4402786120196116,
+            "velocityY": 1.573248927543637,
+            "timestamp": 1.3496458428812634
+        },
+        {
+            "x": 4.755659310004632,
+            "y": 6.7112810805754854,
+            "heading": -0.033616028476497646,
+            "angularVelocity": -6.8630434261390676e-9,
+            "velocityX": 3.502595027228562,
+            "velocityY": 1.4291456072107467,
+            "timestamp": 1.3715041518161033
+        },
+        {
+            "x": 4.833635561443148,
+            "y": 6.738796851922075,
+            "heading": -0.0336160286284395,
+            "angularVelocity": -6.951217281059994e-9,
+            "velocityX": 3.5673505974759867,
+            "velocityY": 1.258824341288896,
+            "timestamp": 1.3933624607509432
+        },
+        {
+            "x": 4.9128465230031875,
+            "y": 6.762525251770201,
+            "heading": -0.033616028783803605,
+            "angularVelocity": -7.107782622056134e-9,
+            "velocityX": 3.623837589457144,
+            "velocityY": 1.0855551506230685,
+            "timestamp": 1.415220769685783
+        },
+        {
+            "x": 4.993108392358412,
+            "y": 6.78241106619245,
+            "heading": -0.033616028944227425,
+            "angularVelocity": -7.33926038728002e-9,
+            "velocityX": 3.6719157732872607,
+            "velocityY": 0.9097599673208229,
+            "timestamp": 1.437079078620623
+        },
+        {
+            "x": 5.0742348898890315,
+            "y": 6.798408083033217,
+            "heading": -0.03361602911156847,
+            "angularVelocity": -7.655717557583249e-9,
+            "velocityX": 3.711471814789518,
+            "velocityY": 0.7318506151803719,
+            "timestamp": 1.4589373875554628
+        },
+        {
+            "x": 5.156037716665883,
+            "y": 6.810479143599293,
+            "heading": -0.033616029288003786,
+            "angularVelocity": -8.071773672975751e-9,
+            "velocityX": 3.742413332189024,
+            "velocityY": 0.5522412828028046,
+            "timestamp": 1.4807956964903026
+        },
+        {
+            "x": 5.238326998632702,
+            "y": 6.818596213450421,
+            "heading": -0.033616029476163836,
+            "angularVelocity": -8.608170503938931e-9,
+            "velocityX": 3.764668264691683,
+            "velocityY": 0.37134939739963974,
+            "timestamp": 1.5026540054251425
+        },
+        {
+            "x": 5.3209117299494695,
+            "y": 6.822740441774992,
+            "heading": -0.033616029679319044,
+            "angularVelocity": -9.29418657606336e-9,
+            "velocityX": 3.778184834103796,
+            "velocityY": 0.18959510257284434,
+            "timestamp": 1.5245123143599824
+        },
+        {
+            "x": 5.403600215911865,
+            "y": 6.822902202606201,
+            "heading": -0.03361602991707219,
+            "angularVelocity": -1.087701477803747e-8,
+            "velocityX": 3.7829315254392575,
+            "velocityY": 0.00740042753033698,
+            "timestamp": 1.5463706232948222
+        },
+        {
+            "x": 5.530709925548806,
+            "y": 6.813694137770519,
+            "heading": -0.03361603012559057,
+            "angularVelocity": -6.189541308885362e-9,
+            "velocityX": 3.773052419307907,
+            "velocityY": -0.2733269661668366,
+            "timestamp": 1.5800594501413512
+        },
+        {
+            "x": 5.656785942150605,
+            "y": 6.795079480224979,
+            "heading": -0.03361603030947626,
+            "angularVelocity": -5.458358559418031e-9,
+            "velocityX": 3.7423688624167126,
+            "velocityY": -0.5525469209818521,
+            "timestamp": 1.6137482769878801
+        },
+        {
+            "x": 5.781133118086899,
+            "y": 6.767160888262499,
+            "heading": -0.033616030476517275,
+            "angularVelocity": -4.958350514129881e-9,
+            "velocityX": 3.6910509381274874,
+            "velocityY": -0.8287196253423904,
+            "timestamp": 1.6474371038344091
+        },
+        {
+            "x": 5.903065850616414,
+            "y": 6.730092343263882,
+            "heading": -0.03361603063230265,
+            "angularVelocity": -4.624244623037642e-9,
+            "velocityX": 3.619381971506033,
+            "velocityY": -1.100321633860491,
+            "timestamp": 1.681125930680938
+        },
+        {
+            "x": 6.0219118917079,
+            "y": 6.684078355941691,
+            "heading": -0.033616030781137125,
+            "angularVelocity": -4.4179180116936e-9,
+            "velocityX": 3.5277583761789866,
+            "velocityY": -1.3658530625542256,
+            "timestamp": 1.714814757527467
+        },
+        {
+            "x": 6.137016291187876,
+            "y": 6.629373275360583,
+            "heading": -0.033616030926600995,
+            "angularVelocity": -4.3178668766413575e-9,
+            "velocityX": 3.4166936119307345,
+            "velocityY": -1.623834538089393,
+            "timestamp": 1.748503584373996
+        },
+        {
+            "x": 6.249923393155274,
+            "y": 6.570266127170688,
+            "heading": -0.0336160310713635,
+            "angularVelocity": -4.297048049789566e-9,
+            "velocityX": 3.3514702806883885,
+            "velocityY": -1.7545030125020786,
+            "timestamp": 1.782192411220525
+        },
+        {
+            "x": 6.362830075113665,
+            "y": 6.51115817668204,
+            "heading": -0.03361603121596069,
+            "angularVelocity": -4.292140831453005e-9,
+            "velocityX": 3.351457813379591,
+            "velocityY": -1.7545268274825585,
+            "timestamp": 1.815881238067054
+        },
+        {
+            "x": 6.474865356079977,
+            "y": 6.45041474520773,
+            "heading": -0.033616029845538055,
+            "angularVelocity": 4.06788471811577e-8,
+            "velocityX": 3.32559164130869,
+            "velocityY": -1.8030735160660076,
+            "timestamp": 1.849570064913583
+        },
+        {
+            "x": 6.586900214909621,
+            "y": 6.389670535150393,
+            "heading": -0.03361602847479329,
+            "angularVelocity": 4.068840886286375e-8,
+            "velocityX": 3.3255791108435817,
+            "velocityY": -1.8030966270823277,
+            "timestamp": 1.8832588917601119
+        },
+        {
+            "x": 6.6989350261872,
+            "y": 6.328926237389509,
+            "heading": -0.03361602710404103,
+            "angularVelocity": 4.068863135772345e-8,
+            "velocityX": 3.325577699335118,
+            "velocityY": -1.8030992304246154,
+            "timestamp": 1.9169477186066408
+        },
+        {
+            "x": 6.810969814733213,
+            "y": 6.268181897703298,
+            "heading": -0.03361602573329801,
+            "angularVelocity": 4.068835717596455e-8,
+            "velocityX": 3.3255770245842737,
+            "velocityY": -1.8031004749122261,
+            "timestamp": 1.9506365454531698
+        },
+        {
+            "x": 6.92300458832202,
+            "y": 6.207437530430852,
+            "heading": -0.03361602436246198,
+            "angularVelocity": 4.069111827209e-8,
+            "velocityX": 3.3255765806030024,
+            "velocityY": -1.803101293766319,
+            "timestamp": 1.9843253722996987
+        },
+        {
+            "x": 7.031146753085382,
+            "y": 6.14880363204561,
+            "heading": -0.02881387895998304,
+            "angularVelocity": 0.1425441563861497,
+            "velocityX": 3.210030591329563,
+            "velocityY": -1.7404553341186793,
+            "timestamp": 2.0180141991462275
+        },
+        {
+            "x": 7.130970312732727,
+            "y": 6.094680017318762,
+            "heading": -0.024381039069153343,
+            "angularVelocity": 0.13158190135330103,
+            "velocityX": 2.9631058422454424,
+            "velocityY": -1.6065746359589999,
+            "timestamp": 2.0517030259927562
+        },
+        {
+            "x": 7.222475251377904,
+            "y": 6.04506669568433,
+            "heading": -0.02031756633641395,
+            "angularVelocity": 0.12061781644251243,
+            "velocityX": 2.716180621605835,
+            "velocityY": -1.4726936577651888,
+            "timestamp": 2.085391852839285
+        },
+        {
+            "x": 7.305661563921799,
+            "y": 5.999963670644433,
+            "heading": -0.016623481896744244,
+            "angularVelocity": 0.10965310417300114,
+            "velocityX": 2.469255249607073,
+            "velocityY": -1.33881257561641,
+            "timestamp": 2.1190806796858137
+        },
+        {
+            "x": 7.380529247941795,
+            "y": 5.959370944187338,
+            "heading": -0.013298795761870387,
+            "angularVelocity": 0.09868809472109723,
+            "velocityX": 2.222329805696702,
+            "velocityY": -1.2049314344491047,
+            "timestamp": 2.1527695065323424
+        },
+        {
+            "x": 7.447078302075198,
+            "y": 5.923288517676822,
+            "heading": -0.010343513390295376,
+            "angularVelocity": 0.08772292324209098,
+            "velocityX": 1.9754043213368524,
+            "velocityY": -1.0710502528001746,
+            "timestamp": 2.186458333378871
+        },
+        {
+            "x": 7.505308725482669,
+            "y": 5.891716392152232,
+            "heading": -0.007757637975370072,
+            "angularVelocity": 0.07675765697347174,
+            "velocityX": 1.7284788120626102,
+            "velocityY": -0.9371690402998547,
+            "timestamp": 2.2201471602254
+        },
+        {
+            "x": 7.555220517619577,
+            "y": 5.86465456845934,
+            "heading": -0.005541171447259703,
+            "angularVelocity": 0.06579233341092419,
+            "velocityX": 1.481553286621783,
+            "velocityY": -0.8032878026941531,
+            "timestamp": 2.2538359870719287
+        },
+        {
+            "x": 7.596813678122498,
+            "y": 5.842103047317274,
+            "heading": -0.0036941149792240496,
+            "angularVelocity": 0.054826975021999756,
+            "velocityX": 1.234627750393344,
+            "velocityY": -0.6694065437422472,
+            "timestamp": 2.2875248139184574
+        },
+        {
+            "x": 7.630088206746783,
+            "y": 5.824061829356817,
+            "heading": -0.0022164692716530373,
+            "angularVelocity": 0.04386159584309864,
+            "velocityX": 0.9877022069028755,
+            "velocityY": -0.535525266066523,
+            "timestamp": 2.321213640764986
+        },
+        {
+            "x": 7.655044103329761,
+            "y": 5.81053091514454,
+            "heading": -0.0011082347348293014,
+            "angularVelocity": 0.0328962044856008,
+            "velocityX": 0.7407766585837434,
+            "velocityY": -0.4016439715731056,
+            "timestamp": 2.354902467611515
+        },
+        {
+            "x": 7.671681367767583,
+            "y": 5.8015103051986525,
+            "heading": -0.0003694116016791507,
+            "angularVelocity": 0.021930806214050736,
+            "velocityX": 0.49385110718203246,
+            "velocityY": -0.2677626616973202,
+            "timestamp": 2.3885912944580436
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": -1.3509198173227418e-31,
+            "angularVelocity": 0.010965404149034802,
+            "velocityX": 0.24692555399191934,
+            "velocityY": -0.13388133754848214,
+            "timestamp": 2.4222801213045724
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 2.046162663204931e-31,
+            "angularVelocity": 9.569528449856341e-30,
+            "velocityX": -2.6100111398714645e-32,
+            "velocityY": -7.871960721903e-33,
+            "timestamp": 2.455968948151101
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.traj
index 168ae5bd..7a28104f 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.traj	
@@ -5,522 +5,612 @@
             "y": 5.56,
             "heading": 0,
             "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -1.058013890027075e-32,
+            "velocityX": 5.330261523342824e-32,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 2.26040474690827,
-            "y": 5.564834298284281,
-            "heading": -0.00020682462174428096,
-            "angularVelocity": -0.005575654083684234,
-            "velocityX": 0.28049498700644215,
-            "velocityY": 0.1303247879443207,
-            "timestamp": 0.037094234800108185
-        },
-        {
-            "x": 2.2812142405469196,
-            "y": 5.574502894544252,
-            "heading": -0.0006204719876615776,
-            "angularVelocity": -0.011151257551113,
-            "velocityX": 0.5609899692172368,
-            "velocityY": 0.26064956756953994,
-            "timestamp": 0.07418846960021637
-        },
-        {
-            "x": 2.312428480686622,
-            "y": 5.589005788443611,
-            "heading": -0.0012409398807264826,
-            "angularVelocity": -0.016726801251156914,
-            "velocityX": 0.8414849452457597,
-            "velocityY": 0.39097433812857635,
-            "timestamp": 0.11128270440032456
-        },
-        {
-            "x": 2.3540474670292637,
-            "y": 5.608342979609529,
-            "heading": -0.002068225636714691,
-            "angularVelocity": -0.022302273128063925,
-            "velocityX": 1.1219799132376411,
-            "velocityY": 0.5212990986367875,
-            "timestamp": 0.14837693920043274
-        },
-        {
-            "x": 2.406071199181798,
-            "y": 5.632514467619691,
-            "heading": -0.0031023259847413323,
-            "angularVelocity": -0.02787765682725579,
-            "velocityX": 1.4024748706336019,
-            "velocityY": 0.6516238477600773,
-            "timestamp": 0.18547117400054092
-        },
-        {
-            "x": 2.4684996766150022,
-            "y": 5.66152025198224,
-            "heading": -0.004343236799506005,
-            "angularVelocity": -0.03345292931508493,
-            "velocityX": 1.682969813762594,
-            "velocityY": 0.7819485836236739,
-            "timestamp": 0.2225654088006491
-        },
-        {
-            "x": 2.5413328985945514,
-            "y": 5.69536033210275,
-            "heading": -0.005790952692459236,
-            "angularVelocity": -0.03902805653641911,
-            "velocityX": 1.9634647370954057,
-            "velocityY": 0.9122733034625292,
-            "timestamp": 0.2596596436007573
-        },
-        {
-            "x": 2.6245708640566887,
-            "y": 5.7340347072253275,
-            "heading": -0.00744546628275827,
-            "angularVelocity": -0.04460298478228464,
-            "velocityX": 2.2439596317510366,
-            "velocityY": 1.0425980029237354,
-            "timestamp": 0.2967538784008655
-        },
-        {
-            "x": 2.7182135713591755,
-            "y": 5.777543376315584,
-            "heading": -0.009306766753500902,
-            "angularVelocity": -0.05017762142212937,
-            "velocityX": 2.5244544821346007,
-            "velocityY": 1.1729226744995158,
-            "timestamp": 0.3338481132009737
-        },
-        {
-            "x": 2.8222610176995655,
-            "y": 5.8258863377887575,
-            "heading": -0.011374836524793099,
-            "angularVelocity": -0.055751784136736884,
-            "velocityX": 2.8049492569688192,
-            "velocityY": 1.303247303352706,
-            "timestamp": 0.3709423480010819
-        },
-        {
-            "x": 2.9367131973687606,
-            "y": 5.879063588696104,
-            "heading": -0.013649641477510719,
-            "angularVelocity": -0.061325027055469855,
-            "velocityX": 3.0854438778950355,
-            "velocityY": 1.4335718527125696,
-            "timestamp": 0.4080365828011901
-        },
-        {
-            "x": 3.06157009301301,
-            "y": 5.937075120660461,
-            "heading": -0.01613108463550858,
-            "angularVelocity": -0.06689565565567898,
-            "velocityX": 3.3659380309924987,
-            "velocityY": 1.5638961762378367,
-            "timestamp": 0.4451308176012983
-        },
-        {
-            "x": 3.1888299168589427,
-            "y": 5.996203084762226,
-            "heading": -0.01613108529402469,
-            "angularVelocity": -1.7752519083825314e-8,
-            "velocityX": 3.430717051630934,
-            "velocityY": 1.5939933636693306,
-            "timestamp": 0.4822250524014065
-        },
-        {
-            "x": 3.316089744181463,
-            "y": 6.055331041381533,
-            "heading": -0.016131085952509703,
-            "angularVelocity": -1.7751680763659775e-8,
-            "velocityX": 3.4307171453540595,
-            "velocityY": 1.5939931619544851,
-            "timestamp": 0.5193192872015147
-        },
-        {
-            "x": 3.443349575355442,
-            "y": 6.114458989711434,
-            "heading": -0.01613108661098856,
-            "angularVelocity": -1.7751514817327164e-8,
-            "velocityX": 3.430717249183109,
-            "velocityY": 1.5939929384856546,
-            "timestamp": 0.5564135220016229
-        },
-        {
-            "x": 3.5706094108109987,
-            "y": 6.173586928826195,
-            "heading": -0.016131087269461084,
-            "angularVelocity": -1.7751344035751635e-8,
-            "velocityX": 3.4307173646074394,
-            "velocityY": 1.5939926900605101,
-            "timestamp": 0.5935077568017311
-        },
-        {
-            "x": 3.6978692510432434,
-            "y": 6.232714857660196,
-            "heading": -0.01613108792792704,
-            "angularVelocity": -1.7751166882414398e-8,
-            "velocityX": 3.4307174933791567,
-            "velocityY": 1.5939924129080143,
-            "timestamp": 0.6306019916018393
-        },
-        {
-            "x": 3.825129096624906,
-            "y": 6.291842774980762,
-            "heading": -0.016131088586386323,
-            "angularVelocity": -1.775098711225461e-8,
-            "velocityX": 3.430717637590709,
-            "velocityY": 1.5939921025246122,
-            "timestamp": 0.6676962264019475
-        },
-        {
-            "x": 3.952388948222243,
-            "y": 6.3509706793539165,
-            "heading": -0.016131089244838845,
-            "angularVelocity": -1.775080474504649e-8,
-            "velocityX": 3.4307177997634843,
-            "velocityY": 1.5939917534835466,
-            "timestamp": 0.7047904612020557
-        },
-        {
-            "x": 4.07964880661482,
-            "y": 6.410098569101814,
-            "heading": -0.016131089903284688,
-            "angularVelocity": -1.775062484471552e-8,
-            "velocityX": 3.430717982952055,
-            "velocityY": 1.5939913592104822,
-            "timestamp": 0.7418846960021639
-        },
-        {
-            "x": 4.206908672718448,
-            "y": 6.469226442253365,
-            "heading": -0.016131090561724272,
-            "angularVelocity": -1.7750456035932977e-8,
-            "velocityX": 3.4307181908294035,
-            "velocityY": 1.5939909118000826,
-            "timestamp": 0.7789789308022721
-        },
-        {
-            "x": 4.33416854760619,
-            "y": 6.52835429649903,
-            "heading": -0.016131091220158388,
-            "angularVelocity": -1.775030869099235e-8,
-            "velocityX": 3.43071842763476,
-            "velocityY": 1.593990402128264,
-            "timestamp": 0.8160731656023803
-        },
-        {
-            "x": 4.461428432515134,
-            "y": 6.587482129176235,
-            "heading": -0.016131091878588796,
-            "angularVelocity": -1.7750208582663698e-8,
-            "velocityX": 3.430718697790006,
-            "velocityY": 1.5939898206778003,
-            "timestamp": 0.8531674004024885
-        },
-        {
-            "x": 4.588688397859823,
-            "y": 6.646609788732575,
-            "heading": -0.01613109253701839,
-            "angularVelocity": -1.7750186861935105e-8,
-            "velocityX": 3.4307208662063466,
-            "velocityY": 1.5939851536219738,
-            "timestamp": 0.8902616352025967
-        },
-        {
-            "x": 4.7169608374986005,
-            "y": 6.703507592945478,
-            "heading": -0.016131093196594153,
-            "angularVelocity": -1.7781085471466767e-8,
-            "velocityX": 3.4580155199319775,
-            "velocityY": 1.533871894635887,
-            "timestamp": 0.9273558700027049
-        },
-        {
-            "x": 4.84945193271468,
-            "y": 6.749738410352506,
-            "heading": -0.016131093868950192,
-            "angularVelocity": -1.8125621008136676e-8,
-            "velocityX": 3.571743585763167,
-            "velocityY": 1.2463073481945173,
-            "timestamp": 0.9644501048028131
-        },
-        {
-            "x": 4.985276817345946,
-            "y": 6.784991484609654,
-            "heading": -0.0161310945734632,
-            "angularVelocity": -1.8992520332334574e-8,
-            "velocityX": 3.661617104738627,
-            "velocityY": 0.9503653181449264,
-            "timestamp": 1.0015443396029213
-        },
-        {
-            "x": 5.123527608694991,
-            "y": 6.809030968493646,
-            "heading": -0.01613109533353302,
-            "angularVelocity": -2.0490241268499195e-8,
-            "velocityX": 3.7270155886499414,
-            "velocityY": 0.6480652320645928,
-            "timestamp": 1.0386385744030295
-        },
-        {
-            "x": 5.263280146337045,
-            "y": 6.821696104409015,
-            "heading": -0.016131096180260384,
-            "angularVelocity": -2.2826387114294202e-8,
-            "velocityX": 3.767500216547063,
-            "velocityY": 0.34143138370740883,
-            "timestamp": 1.0757328092031377
+            "x": 2.216523220338838,
+            "y": 5.608939409819235,
+            "heading": -0.000395848810607471,
+            "angularVelocity": -0.004694167537861253,
+            "velocityX": -0.3969839194829395,
+            "velocityY": 0.5803473011402309,
+            "timestamp": 0.08432779772229798
+        },
+        {
+            "x": 2.1530740053514283,
+            "y": 5.7090393916763,
+            "heading": -0.0013136261454272055,
+            "angularVelocity": -0.010883449581379904,
+            "velocityX": -0.7524116210926717,
+            "velocityY": 1.1870342231242381,
+            "timestamp": 0.16865559544459596
+        },
+        {
+            "x": 2.071477218025725,
+            "y": 5.86558734321221,
+            "heading": -0.0031284327789465032,
+            "angularVelocity": -0.02152085886904205,
+            "velocityX": -0.9676143517279073,
+            "velocityY": 1.8564216754650953,
+            "timestamp": 0.25298339316689394
+        },
+        {
+            "x": 2.0458269370451427,
+            "y": 6.041770495606775,
+            "heading": -0.006595224749493174,
+            "angularVelocity": -0.04111090368993278,
+            "velocityX": -0.30417349525777404,
+            "velocityY": 2.089265427928733,
+            "timestamp": 0.3373111908891919
+        },
+        {
+            "x": 2.0715234655116643,
+            "y": 6.188301290095102,
+            "heading": -0.01049068188832321,
+            "angularVelocity": -0.046194223542478534,
+            "velocityX": 0.30472192041755103,
+            "velocityY": 1.7376333598900595,
+            "timestamp": 0.4216389886114899
+        },
+        {
+            "x": 2.1438515186309814,
+            "y": 6.298208713531494,
+            "heading": -0.014554322717054646,
+            "angularVelocity": -0.04818862745724922,
+            "velocityX": 0.8577011978600798,
+            "velocityY": 1.303335630776598,
+            "timestamp": 0.5059667863337879
+        },
+        {
+            "x": 2.2162950329491684,
+            "y": 6.355974505839093,
+            "heading": -0.017431077998725047,
+            "angularVelocity": -0.04874949538291163,
+            "velocityX": 1.2276278031981451,
+            "velocityY": 0.978899124069331,
+            "timestamp": 0.5649777619156877
+        },
+        {
+            "x": 2.310686009240105,
+            "y": 6.394730061346725,
+            "heading": -0.020327199149560698,
+            "angularVelocity": -0.0490776694043422,
+            "velocityX": 1.5995494966853068,
+            "velocityY": 0.6567516487478545,
+            "timestamp": 0.6239887374975874
+        },
+        {
+            "x": 2.4271886355684367,
+            "y": 6.414666608347948,
+            "heading": -0.023223370555002502,
+            "angularVelocity": -0.04907852101890474,
+            "velocityX": 1.9742535211376202,
+            "velocityY": 0.33784472811424154,
+            "timestamp": 0.6829997130794871
+        },
+        {
+            "x": 2.5660476863967387,
+            "y": 6.416075601325133,
+            "heading": -0.02609042518710973,
+            "angularVelocity": -0.048585108174133695,
+            "velocityX": 2.35310549366504,
+            "velocityY": 0.02387679517735602,
+            "timestamp": 0.7420106886613869
+        },
+        {
+            "x": 2.72766661902605,
+            "y": 6.399454975329908,
+            "heading": -0.02887926513240676,
+            "angularVelocity": -0.04725968208112396,
+            "velocityX": 2.738794453669127,
+            "velocityY": -0.281653130308932,
+            "timestamp": 0.8010216642432866
+        },
+        {
+            "x": 2.912832200300888,
+            "y": 6.365845166245685,
+            "heading": -0.031490096116184224,
+            "angularVelocity": -0.04424314219571676,
+            "velocityX": 3.1378159647242496,
+            "velocityY": -0.5695518291775498,
+            "timestamp": 0.8600326398251863
+        },
+        {
+            "x": 3.123679163604623,
+            "y": 6.318688161285352,
+            "heading": -0.033616023768519236,
+            "angularVelocity": -0.03602597027708684,
+            "velocityX": 3.5730126679757386,
+            "velocityY": -0.7991226122822729,
+            "timestamp": 0.919043615407086
+        },
+        {
+            "x": 3.345972776412964,
+            "y": 6.298208713531494,
+            "heading": -0.03361602546428586,
+            "angularVelocity": -2.8736461454443448e-8,
+            "velocityX": 3.766987591991692,
+            "velocityY": -0.34704472434005124,
+            "timestamp": 0.9780545909889857
+        },
+        {
+            "x": 3.4285825306560853,
+            "y": 6.294597584481868,
+            "heading": -0.033616025709707474,
+            "angularVelocity": -1.1227840838251836e-8,
+            "velocityX": 3.779329612797739,
+            "velocityY": -0.16520624081174923,
+            "timestamp": 0.9999128999238256
+        },
+        {
+            "x": 3.5112703400714786,
+            "y": 6.2949695138762145,
+            "heading": -0.033616025930811776,
+            "angularVelocity": -1.0115343355589466e-8,
+            "velocityX": 3.7829005739596693,
+            "velocityY": 0.01701546974451014,
+            "timestamp": 1.0217712088586655
+        },
+        {
+            "x": 3.5938442713679413,
+            "y": 6.299323633866166,
+            "heading": -0.03361602613301592,
+            "angularVelocity": -9.2506762233922e-9,
+            "velocityX": 3.7776907418875507,
+            "velocityY": 0.19919747693799758,
+            "timestamp": 1.0436295177935053
+        },
+        {
+            "x": 3.6761126565475006,
+            "y": 6.3076498312062705,
+            "heading": -0.03361602632044191,
+            "angularVelocity": -8.574588182780684e-9,
+            "velocityX": 3.763712253532677,
+            "velocityY": 0.3809168113107395,
+            "timestamp": 1.0654878267283452
+        },
+        {
+            "x": 3.7578845383740482,
+            "y": 6.31992876945557,
+            "heading": -0.033616026496316675,
+            "angularVelocity": -8.046128410971048e-9,
+            "velocityX": 3.7409976256768736,
+            "velocityY": 0.5617515191089957,
+            "timestamp": 1.087346135663185
+        },
+        {
+            "x": 3.8389701148011586,
+            "y": 6.336131931270735,
+            "heading": -0.03361602666323835,
+            "angularVelocity": -7.63653221094489e-9,
+            "velocityX": 3.709599707316274,
+            "velocityY": 0.741281581455633,
+            "timestamp": 1.109204444598025
+        },
+        {
+            "x": 3.919181182263655,
+            "y": 6.356221678659879,
+            "heading": -0.03361602682336018,
+            "angularVelocity": -7.3254443467155425e-9,
+            "velocityX": 3.6695916276783906,
+            "velocityY": 0.9190897360373237,
+            "timestamp": 1.1310627535328648
+        },
+        {
+            "x": 3.998331580016101,
+            "y": 6.3801513240916305,
+            "heading": -0.03361602697852209,
+            "angularVelocity": -7.098532288064829e-9,
+            "velocityX": 3.621066844118403,
+            "velocityY": 1.0947619737229577,
+            "timestamp": 1.1529210624677046
+        },
+        {
+            "x": 4.076237649565588,
+            "y": 6.407865179672378,
+            "heading": -0.03361602713034875,
+            "angularVelocity": -6.945947165054901e-9,
+            "velocityX": 3.5641398326708003,
+            "velocityY": 1.2678865352009827,
+            "timestamp": 1.1747793714025445
+        },
+        {
+            "x": 4.152718837418924,
+            "y": 6.439298305619894,
+            "heading": -0.033616027280326394,
+            "angularVelocity": -6.861356032634536e-9,
+            "velocityX": 3.4989526445677317,
+            "velocityY": 1.4380401540310954,
+            "timestamp": 1.1966376803373844
+        },
+        {
+            "x": 4.227916828679672,
+            "y": 6.473688356846056,
+            "heading": -0.03361602742978782,
+            "angularVelocity": -6.8377396934186395e-9,
+            "velocityX": 3.4402474356508805,
+            "velocityY": 1.5733171000866744,
+            "timestamp": 1.2184959892722242
+        },
+        {
+            "x": 4.303113417879144,
+            "y": 6.50808147368302,
+            "heading": -0.033616027579243535,
+            "angularVelocity": -6.837478280080917e-9,
+            "velocityX": 3.4401832924786557,
+            "velocityY": 1.5734573493077775,
+            "timestamp": 1.240354298207064
+        },
+        {
+            "x": 4.378310012144908,
+            "y": 6.542474579443129,
+            "heading": -0.03361602772869459,
+            "angularVelocity": -6.837264944013175e-9,
+            "velocityX": 3.4401835242573666,
+            "velocityY": 1.573456842550575,
+            "timestamp": 1.262212607141904
+        },
+        {
+            "x": 4.453506612352025,
+            "y": 6.576867672213151,
+            "heading": -0.03361602787814215,
+            "angularVelocity": -6.837105393969464e-9,
+            "velocityX": 3.4401837960695145,
+            "velocityY": 1.5734562482645857,
+            "timestamp": 1.2840709160767438
+        },
+        {
+            "x": 4.528703218974218,
+            "y": 6.6112607509573404,
+            "heading": -0.03361602802758787,
+            "angularVelocity": -6.837020955916861e-9,
+            "velocityX": 3.440184089554037,
+            "velocityY": 1.5734556065940832,
+            "timestamp": 1.3059292250115837
+        },
+        {
+            "x": 4.603899833102385,
+            "y": 6.645653813290562,
+            "heading": -0.03361602817703386,
+            "angularVelocity": -6.837033689050042e-9,
+            "velocityX": 3.440184432946286,
+            "velocityY": 1.5734548558055208,
+            "timestamp": 1.3277875339464236
+        },
+        {
+            "x": 4.679098505825837,
+            "y": 6.680042374380203,
+            "heading": -0.033616028326483124,
+            "angularVelocity": -6.83718315967473e-9,
+            "velocityX": 3.4402786120196116,
+            "velocityY": 1.573248927543637,
+            "timestamp": 1.3496458428812634
+        },
+        {
+            "x": 4.755659310004632,
+            "y": 6.7112810805754854,
+            "heading": -0.033616028476497646,
+            "angularVelocity": -6.8630434261390676e-9,
+            "velocityX": 3.502595027228562,
+            "velocityY": 1.4291456072107467,
+            "timestamp": 1.3715041518161033
+        },
+        {
+            "x": 4.833635561443148,
+            "y": 6.738796851922075,
+            "heading": -0.0336160286284395,
+            "angularVelocity": -6.951217281059994e-9,
+            "velocityX": 3.5673505974759867,
+            "velocityY": 1.258824341288896,
+            "timestamp": 1.3933624607509432
+        },
+        {
+            "x": 4.9128465230031875,
+            "y": 6.762525251770201,
+            "heading": -0.033616028783803605,
+            "angularVelocity": -7.107782622056134e-9,
+            "velocityX": 3.623837589457144,
+            "velocityY": 1.0855551506230685,
+            "timestamp": 1.415220769685783
+        },
+        {
+            "x": 4.993108392358412,
+            "y": 6.78241106619245,
+            "heading": -0.033616028944227425,
+            "angularVelocity": -7.33926038728002e-9,
+            "velocityX": 3.6719157732872607,
+            "velocityY": 0.9097599673208229,
+            "timestamp": 1.437079078620623
+        },
+        {
+            "x": 5.0742348898890315,
+            "y": 6.798408083033217,
+            "heading": -0.03361602911156847,
+            "angularVelocity": -7.655717557583249e-9,
+            "velocityX": 3.711471814789518,
+            "velocityY": 0.7318506151803719,
+            "timestamp": 1.4589373875554628
+        },
+        {
+            "x": 5.156037716665883,
+            "y": 6.810479143599293,
+            "heading": -0.033616029288003786,
+            "angularVelocity": -8.071773672975751e-9,
+            "velocityX": 3.742413332189024,
+            "velocityY": 0.5522412828028046,
+            "timestamp": 1.4807956964903026
+        },
+        {
+            "x": 5.238326998632702,
+            "y": 6.818596213450421,
+            "heading": -0.033616029476163836,
+            "angularVelocity": -8.608170503938931e-9,
+            "velocityX": 3.764668264691683,
+            "velocityY": 0.37134939739963974,
+            "timestamp": 1.5026540054251425
+        },
+        {
+            "x": 5.3209117299494695,
+            "y": 6.822740441774992,
+            "heading": -0.033616029679319044,
+            "angularVelocity": -9.29418657606336e-9,
+            "velocityX": 3.778184834103796,
+            "velocityY": 0.18959510257284434,
+            "timestamp": 1.5245123143599824
         },
         {
             "x": 5.403600215911865,
             "y": 6.822902202606201,
-            "heading": -0.0161310971470982,
-            "angularVelocity": -2.6064368823146806e-8,
-            "velocityX": 3.7827999507462917,
-            "velocityY": 0.032514437989762196,
-            "timestamp": 1.112827044003246
-        },
-        {
-            "x": 5.530811605245593,
-            "y": 6.814532094483614,
-            "heading": -0.016131098042384447,
-            "angularVelocity": -2.6566067685833464e-8,
-            "velocityX": 3.7747775034488793,
-            "velocityY": -0.2483684519763222,
-            "timestamp": 1.1465274115659696
-        },
-        {
-            "x": 5.657050747526861,
-            "y": 6.796742322973506,
-            "heading": -0.016131098828254187,
-            "angularVelocity": -2.3319322572510494e-8,
-            "velocityX": 3.745927757206515,
-            "velocityY": -0.5278806374143462,
-            "timestamp": 1.1802277791286933
-        },
-        {
-            "x": 5.781621117814577,
-            "y": 6.76963106353518,
-            "heading": -0.016131099539274135,
-            "angularVelocity": -2.109828463295381e-8,
-            "velocityX": 3.6964098405118033,
-            "velocityY": -0.8044796362492181,
-            "timestamp": 1.213928146691417
-        },
-        {
-            "x": 5.903835409646495,
-            "y": 6.733347943713289,
-            "heading": -0.016131100200026813,
-            "angularVelocity": -1.96066906061588e-8,
-            "velocityX": 3.626497295747634,
-            "velocityY": -1.0766386970219277,
-            "timestamp": 1.2476285142541406
-        },
-        {
-            "x": 6.023019351333038,
-            "y": 6.68809326224078,
-            "heading": -0.016131100829259,
-            "angularVelocity": -1.86713746487578e-8,
-            "velocityX": 3.5365769072018636,
-            "velocityY": -1.3428542400399708,
-            "timestamp": 1.2813288818168642
-        },
-        {
-            "x": 6.138515596479519,
-            "y": 6.634117196875684,
-            "heading": -0.016131101442406693,
-            "angularVelocity": -1.81940951537302e-8,
-            "velocityX": 3.4271509036665004,
-            "velocityY": -1.601646191681265,
-            "timestamp": 1.3150292493795879
-        },
-        {
-            "x": 6.2512259314426855,
-            "y": 6.5745422130600915,
-            "heading": -0.016131102051778846,
-            "angularVelocity": -1.80820624580199e-8,
-            "velocityX": 3.3444838473464196,
-            "velocityY": -1.7677843929954529,
-            "timestamp": 1.3487296169423115
-        },
-        {
-            "x": 6.363935646775815,
-            "y": 6.514966056976746,
-            "heading": -0.01613110266098242,
-            "angularVelocity": -1.807706019437417e-8,
-            "velocityX": 3.344465460898959,
-            "velocityY": -1.7678191780093255,
-            "timestamp": 1.3824299845050352
-        },
-        {
-            "x": 6.4758517936606514,
-            "y": 6.453912192428435,
-            "heading": -0.016131102003666662,
-            "angularVelocity": 1.950470585863807e-8,
-            "velocityX": 3.320917692560386,
-            "velocityY": -1.811667615632877,
-            "timestamp": 1.4161303520677588
-        },
-        {
-            "x": 6.587767622018337,
-            "y": 6.392857744000494,
-            "heading": -0.016131101346154864,
-            "angularVelocity": 1.951052313957626e-8,
-            "velocityX": 3.32090824081921,
-            "velocityY": -1.8116849412489389,
-            "timestamp": 1.4498307196304825
-        },
-        {
-            "x": 6.699683405881235,
-            "y": 6.331803214011495,
-            "heading": -0.016131100688641712,
-            "angularVelocity": 1.9510563155904532e-8,
-            "velocityX": 3.3209069205135386,
-            "velocityY": -1.811687361432026,
-            "timestamp": 1.4835310871932061
-        },
-        {
-            "x": 6.811599167638959,
-            "y": 6.270748643502715,
-            "heading": -0.016131100031139815,
-            "angularVelocity": 1.9510229240352237e-8,
-            "velocityX": 3.320906264580792,
-            "velocityY": -1.8116885637862563,
-            "timestamp": 1.5172314547559298
-        },
-        {
-            "x": 6.923514914598752,
-            "y": 6.209694045868981,
-            "heading": -0.016131099373601954,
-            "angularVelocity": 1.95112965056638e-8,
-            "velocityX": 3.3209058254778503,
-            "velocityY": -1.8116893686722635,
-            "timestamp": 1.5509318223186535
-        },
-        {
-            "x": 7.031584175346113,
-            "y": 6.1507377884047765,
-            "heading": -0.013826704963615632,
-            "angularVelocity": 0.0683789102803133,
-            "velocityX": 3.2067680136194574,
-            "velocityY": -1.7494247608568367,
-            "timestamp": 1.584632189881377
-        },
-        {
-            "x": 7.131340439149152,
-            "y": 6.096316611291437,
-            "heading": -0.011699535798216875,
-            "angularVelocity": 0.06312005830317652,
-            "velocityX": 2.960094236876553,
-            "velocityY": -1.614854111369865,
-            "timestamp": 1.6183325574441008
-        },
-        {
-            "x": 7.222783690004588,
-            "y": 6.046430524071638,
-            "heading": -0.00974961791861361,
-            "angularVelocity": 0.057860433598327515,
-            "velocityX": 2.713419985263968,
-            "velocityY": -1.4802831787205646,
-            "timestamp": 1.6520329250068244
-        },
-        {
-            "x": 7.305913922772626,
-            "y": 6.001079530283105,
-            "heading": -0.00797695998479322,
-            "angularVelocity": 0.05260055192336173,
-            "velocityX": 2.466745581136908,
-            "velocityY": -1.3457121410953312,
-            "timestamp": 1.685733292569548
-        },
-        {
-            "x": 7.380731135011445,
-            "y": 5.960263631930958,
-            "heading": -0.00638156583911186,
-            "angularVelocity": 0.04734055623313652,
-            "velocityX": 2.2200711045530213,
-            "velocityY": -1.2111410439717167,
-            "timestamp": 1.7194336601322717
-        },
-        {
-            "x": 7.447235325348076,
-            "y": 5.923982830388488,
-            "heading": -0.004963437343393232,
-            "angularVelocity": 0.042080505296519764,
-            "velocityX": 1.9733965872286794,
-            "velocityY": -1.0765699060980205,
-            "timestamp": 1.7531340276949954
-        },
-        {
-            "x": 7.505426492937412,
-            "y": 5.892237126701046,
-            "heading": -0.0037225753686417596,
-            "angularVelocity": 0.0368204285143562,
-            "velocityX": 1.72672204482727,
-            "velocityY": -0.9419987372053683,
-            "timestamp": 1.786834395257719
-        },
-        {
-            "x": 7.555304637231595,
-            "y": 5.8650265217185185,
-            "heading": -0.002658980227466916,
-            "angularVelocity": 0.03156034245604838,
-            "velocityX": 1.4800474861690802,
-            "velocityY": -0.8074275430937743,
-            "timestamp": 1.8205347628204427
-        },
-        {
-            "x": 7.596869757865521,
-            "y": 5.842351016163066,
-            "heading": -0.0017726518914738514,
-            "angularVelocity": 0.02630025724030852,
-            "velocityX": 1.233372916677093,
-            "velocityY": -0.6728563275533557,
-            "timestamp": 1.8542351303831663
-        },
-        {
-            "x": 7.630121854593904,
-            "y": 5.824210610667954,
-            "heading": -0.001063590116536395,
-            "angularVelocity": 0.021040179268589657,
-            "velocityX": 0.9866983399066315,
-            "velocityY": -0.5382850932218669,
-            "timestamp": 1.88793549794589
-        },
-        {
-            "x": 7.655060927254085,
-            "y": 5.810605305801891,
-            "heading": -0.000531794525727047,
-            "angularVelocity": 0.01578011248154109,
-            "velocityX": 0.7400237583095566,
-            "velocityY": -0.4037138420149424,
-            "timestamp": 1.9216358655086136
-        },
-        {
-            "x": 7.671686975742638,
-            "y": 5.8015351020850225,
-            "heading": -0.00017726465841382563,
-            "angularVelocity": 0.010520059362948236,
-            "velocityX": 0.49334917364355607,
-            "velocityY": -0.26914257537363395,
-            "timestamp": 1.9553362330713373
+            "heading": -0.03361602991707219,
+            "angularVelocity": -1.087701477803747e-8,
+            "velocityX": 3.7829315254392575,
+            "velocityY": 0.00740042753033698,
+            "timestamp": 1.5463706232948222
+        },
+        {
+            "x": 5.530709925548806,
+            "y": 6.813694137770519,
+            "heading": -0.03361603012559057,
+            "angularVelocity": -6.189541308885362e-9,
+            "velocityX": 3.773052419307907,
+            "velocityY": -0.2733269661668366,
+            "timestamp": 1.5800594501413512
+        },
+        {
+            "x": 5.656785942150605,
+            "y": 6.795079480224979,
+            "heading": -0.03361603030947626,
+            "angularVelocity": -5.458358559418031e-9,
+            "velocityX": 3.7423688624167126,
+            "velocityY": -0.5525469209818521,
+            "timestamp": 1.6137482769878801
+        },
+        {
+            "x": 5.781133118086899,
+            "y": 6.767160888262499,
+            "heading": -0.033616030476517275,
+            "angularVelocity": -4.958350514129881e-9,
+            "velocityX": 3.6910509381274874,
+            "velocityY": -0.8287196253423904,
+            "timestamp": 1.6474371038344091
+        },
+        {
+            "x": 5.903065850616414,
+            "y": 6.730092343263882,
+            "heading": -0.03361603063230265,
+            "angularVelocity": -4.624244623037642e-9,
+            "velocityX": 3.619381971506033,
+            "velocityY": -1.100321633860491,
+            "timestamp": 1.681125930680938
+        },
+        {
+            "x": 6.0219118917079,
+            "y": 6.684078355941691,
+            "heading": -0.033616030781137125,
+            "angularVelocity": -4.4179180116936e-9,
+            "velocityX": 3.5277583761789866,
+            "velocityY": -1.3658530625542256,
+            "timestamp": 1.714814757527467
+        },
+        {
+            "x": 6.137016291187876,
+            "y": 6.629373275360583,
+            "heading": -0.033616030926600995,
+            "angularVelocity": -4.3178668766413575e-9,
+            "velocityX": 3.4166936119307345,
+            "velocityY": -1.623834538089393,
+            "timestamp": 1.748503584373996
+        },
+        {
+            "x": 6.249923393155274,
+            "y": 6.570266127170688,
+            "heading": -0.0336160310713635,
+            "angularVelocity": -4.297048049789566e-9,
+            "velocityX": 3.3514702806883885,
+            "velocityY": -1.7545030125020786,
+            "timestamp": 1.782192411220525
+        },
+        {
+            "x": 6.362830075113665,
+            "y": 6.51115817668204,
+            "heading": -0.03361603121596069,
+            "angularVelocity": -4.292140831453005e-9,
+            "velocityX": 3.351457813379591,
+            "velocityY": -1.7545268274825585,
+            "timestamp": 1.815881238067054
+        },
+        {
+            "x": 6.474865356079977,
+            "y": 6.45041474520773,
+            "heading": -0.033616029845538055,
+            "angularVelocity": 4.06788471811577e-8,
+            "velocityX": 3.32559164130869,
+            "velocityY": -1.8030735160660076,
+            "timestamp": 1.849570064913583
+        },
+        {
+            "x": 6.586900214909621,
+            "y": 6.389670535150393,
+            "heading": -0.03361602847479329,
+            "angularVelocity": 4.068840886286375e-8,
+            "velocityX": 3.3255791108435817,
+            "velocityY": -1.8030966270823277,
+            "timestamp": 1.8832588917601119
+        },
+        {
+            "x": 6.6989350261872,
+            "y": 6.328926237389509,
+            "heading": -0.03361602710404103,
+            "angularVelocity": 4.068863135772345e-8,
+            "velocityX": 3.325577699335118,
+            "velocityY": -1.8030992304246154,
+            "timestamp": 1.9169477186066408
+        },
+        {
+            "x": 6.810969814733213,
+            "y": 6.268181897703298,
+            "heading": -0.03361602573329801,
+            "angularVelocity": 4.068835717596455e-8,
+            "velocityX": 3.3255770245842737,
+            "velocityY": -1.8031004749122261,
+            "timestamp": 1.9506365454531698
+        },
+        {
+            "x": 6.92300458832202,
+            "y": 6.207437530430852,
+            "heading": -0.03361602436246198,
+            "angularVelocity": 4.069111827209e-8,
+            "velocityX": 3.3255765806030024,
+            "velocityY": -1.803101293766319,
+            "timestamp": 1.9843253722996987
+        },
+        {
+            "x": 7.031146753085382,
+            "y": 6.14880363204561,
+            "heading": -0.02881387895998304,
+            "angularVelocity": 0.1425441563861497,
+            "velocityX": 3.210030591329563,
+            "velocityY": -1.7404553341186793,
+            "timestamp": 2.0180141991462275
+        },
+        {
+            "x": 7.130970312732727,
+            "y": 6.094680017318762,
+            "heading": -0.024381039069153343,
+            "angularVelocity": 0.13158190135330103,
+            "velocityX": 2.9631058422454424,
+            "velocityY": -1.6065746359589999,
+            "timestamp": 2.0517030259927562
+        },
+        {
+            "x": 7.222475251377904,
+            "y": 6.04506669568433,
+            "heading": -0.02031756633641395,
+            "angularVelocity": 0.12061781644251243,
+            "velocityX": 2.716180621605835,
+            "velocityY": -1.4726936577651888,
+            "timestamp": 2.085391852839285
+        },
+        {
+            "x": 7.305661563921799,
+            "y": 5.999963670644433,
+            "heading": -0.016623481896744244,
+            "angularVelocity": 0.10965310417300114,
+            "velocityX": 2.469255249607073,
+            "velocityY": -1.33881257561641,
+            "timestamp": 2.1190806796858137
+        },
+        {
+            "x": 7.380529247941795,
+            "y": 5.959370944187338,
+            "heading": -0.013298795761870387,
+            "angularVelocity": 0.09868809472109723,
+            "velocityX": 2.222329805696702,
+            "velocityY": -1.2049314344491047,
+            "timestamp": 2.1527695065323424
+        },
+        {
+            "x": 7.447078302075198,
+            "y": 5.923288517676822,
+            "heading": -0.010343513390295376,
+            "angularVelocity": 0.08772292324209098,
+            "velocityX": 1.9754043213368524,
+            "velocityY": -1.0710502528001746,
+            "timestamp": 2.186458333378871
+        },
+        {
+            "x": 7.505308725482669,
+            "y": 5.891716392152232,
+            "heading": -0.007757637975370072,
+            "angularVelocity": 0.07675765697347174,
+            "velocityX": 1.7284788120626102,
+            "velocityY": -0.9371690402998547,
+            "timestamp": 2.2201471602254
+        },
+        {
+            "x": 7.555220517619577,
+            "y": 5.86465456845934,
+            "heading": -0.005541171447259703,
+            "angularVelocity": 0.06579233341092419,
+            "velocityX": 1.481553286621783,
+            "velocityY": -0.8032878026941531,
+            "timestamp": 2.2538359870719287
+        },
+        {
+            "x": 7.596813678122498,
+            "y": 5.842103047317274,
+            "heading": -0.0036941149792240496,
+            "angularVelocity": 0.054826975021999756,
+            "velocityX": 1.234627750393344,
+            "velocityY": -0.6694065437422472,
+            "timestamp": 2.2875248139184574
+        },
+        {
+            "x": 7.630088206746783,
+            "y": 5.824061829356817,
+            "heading": -0.0022164692716530373,
+            "angularVelocity": 0.04386159584309864,
+            "velocityX": 0.9877022069028755,
+            "velocityY": -0.535525266066523,
+            "timestamp": 2.321213640764986
+        },
+        {
+            "x": 7.655044103329761,
+            "y": 5.81053091514454,
+            "heading": -0.0011082347348293014,
+            "angularVelocity": 0.0328962044856008,
+            "velocityX": 0.7407766585837434,
+            "velocityY": -0.4016439715731056,
+            "timestamp": 2.354902467611515
+        },
+        {
+            "x": 7.671681367767583,
+            "y": 5.8015103051986525,
+            "heading": -0.0003694116016791507,
+            "angularVelocity": 0.021930806214050736,
+            "velocityX": 0.49385110718203246,
+            "velocityY": -0.2677626616973202,
+            "timestamp": 2.3885912944580436
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 3.6111480760753294e-31,
-            "angularVelocity": 0.005260021514121506,
-            "velocityX": 0.24667458721003926,
-            "velocityY": -0.1345712944104092,
-            "timestamp": 1.989036600634061
+            "heading": -1.3509198173227418e-31,
+            "angularVelocity": 0.010965404149034802,
+            "velocityX": 0.24692555399191934,
+            "velocityY": -0.13388133754848214,
+            "timestamp": 2.4222801213045724
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": -8.452347473573201e-32,
-            "angularVelocity": -1.5098225699671103e-29,
-            "velocityX": 2.1451735090517096e-32,
-            "velocityY": 9.12030277003484e-33,
-            "timestamp": 2.0227369681967846
+            "heading": 2.046162663204931e-31,
+            "angularVelocity": 9.569528449856341e-30,
+            "velocityX": -2.6100111398714645e-32,
+            "velocityY": -7.871960721903e-33,
+            "timestamp": 2.455968948151101
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.1.traj
new file mode 100644
index 00000000..11fcc4a7
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.1.traj	
@@ -0,0 +1,625 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 3.8917516758910124e-38,
+            "velocityY": -7.197289285396863e-34,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2621897956017167,
+            "y": 5.610801651791703,
+            "heading": 3.480716271732917e-18,
+            "angularVelocity": 4.397302355093269e-17,
+            "velocityX": 0.15399765670735568,
+            "velocityY": 0.6417938075748972,
+            "timestamp": 0.07915572134244127
+        },
+        {
+            "x": 2.2905203388747193,
+            "y": 5.711291077569501,
+            "heading": 3.7329161294500195e-18,
+            "angularVelocity": 3.1861229325774303e-18,
+            "velocityX": 0.35790897729855475,
+            "velocityY": 1.2695156341645175,
+            "timestamp": 0.15831144268488254
+        },
+        {
+            "x": 2.3436883815778624,
+            "y": 5.857742406339618,
+            "heading": 7.49671396174733e-18,
+            "angularVelocity": 4.754928420848352e-17,
+            "velocityX": 0.6716891944314332,
+            "velocityY": 1.8501673193848232,
+            "timestamp": 0.23746716402732382
+        },
+        {
+            "x": 2.444399562681219,
+            "y": 6.0258512044397365,
+            "heading": 1.0345374349306828e-17,
+            "angularVelocity": 3.5988057740869555e-17,
+            "velocityX": 1.2723171413935193,
+            "velocityY": 2.1237731808778637,
+            "timestamp": 0.3166228853697651
+        },
+        {
+            "x": 2.5795093648860483,
+            "y": 6.154639121411333,
+            "heading": 6.772254039775596e-18,
+            "angularVelocity": -4.5140391310627574e-17,
+            "velocityX": 1.706886121602264,
+            "velocityY": 1.6270196871106504,
+            "timestamp": 0.3957786067122063
+        },
+        {
+            "x": 2.7311723872223266,
+            "y": 6.233875155156547,
+            "heading": -7.527039882559706e-19,
+            "angularVelocity": -9.506524797608463e-17,
+            "velocityX": 1.916008341079448,
+            "velocityY": 1.00101461273312,
+            "timestamp": 0.4749343280546476
+        },
+        {
+            "x": 2.892774820327759,
+            "y": 6.261821746826172,
+            "heading": -9.856118161245683e-18,
+            "angularVelocity": -1.1500639891239944e-16,
+            "velocityX": 2.041576153495111,
+            "velocityY": 0.3530583916824199,
+            "timestamp": 0.5540900493970888
+        },
+        {
+            "x": 2.9421812890727397,
+            "y": 6.2655363212140305,
+            "heading": -1.2690994415516207e-17,
+            "angularVelocity": -1.1860957835731596e-16,
+            "velocityX": 2.067137973857932,
+            "velocityY": 0.15541563622967255,
+            "timestamp": 0.5779909545953872
+        },
+        {
+            "x": 2.992183666499164,
+            "y": 6.264525133875901,
+            "heading": -1.5700573063161643e-17,
+            "angularVelocity": -1.2591902515397632e-16,
+            "velocityX": 2.092070447188938,
+            "velocityY": -0.042307491274448855,
+            "timestamp": 0.6018918597936855
+        },
+        {
+            "x": 3.0427650175812166,
+            "y": 6.258786080293358,
+            "heading": -1.8888665404790212e-17,
+            "angularVelocity": -1.333879346368914e-16,
+            "velocityX": 2.1162943688699283,
+            "velocityY": -0.24011867060800707,
+            "timestamp": 0.6257927649919839
+        },
+        {
+            "x": 3.093906133667358,
+            "y": 6.248316847924188,
+            "heading": -2.226054198779156e-17,
+            "angularVelocity": -1.4107736063473173e-16,
+            "velocityX": 2.139714611720349,
+            "velocityY": -0.4380266053653904,
+            "timestamp": 0.6496936701902822
+        },
+        {
+            "x": 3.1455850428469736,
+            "y": 6.233114888531311,
+            "heading": -2.51599280773308e-17,
+            "angularVelocity": -1.2130863242066928e-16,
+            "velocityX": 2.162215562584425,
+            "velocityY": -0.6360411568829696,
+            "timestamp": 0.6735945753885806
+        },
+        {
+            "x": 3.197776369275469,
+            "y": 6.213177387662576,
+            "heading": -2.8136873311901383e-17,
+            "angularVelocity": -1.2455366258796381e-16,
+            "velocityX": 2.1836548028403113,
+            "velocityY": -0.8341734634450271,
+            "timestamp": 0.6974954805868789
+        },
+        {
+            "x": 3.2504504785577715,
+            "y": 6.188501232669928,
+            "heading": -3.110871206110287e-17,
+            "angularVelocity": -1.2434001017092495e-16,
+            "velocityX": 2.203854157207935,
+            "velocityY": -1.0324360013948046,
+            "timestamp": 0.7213963857851773
+        },
+        {
+            "x": 3.3035723121371006,
+            "y": 6.159082982949556,
+            "heading": -3.205106262321403e-17,
+            "angularVelocity": -3.942740156809933e-17,
+            "velocityX": 2.222586681909921,
+            "velocityY": -1.2308424922109886,
+            "timestamp": 0.7452972909834756
+        },
+        {
+            "x": 3.3570997558109252,
+            "y": 6.124918851159325,
+            "heading": -3.3149188675890806e-17,
+            "angularVelocity": -4.594495761931231e-17,
+            "velocityX": 2.239557172823534,
+            "velocityY": -1.429407443223672,
+            "timestamp": 0.769198196181774
+        },
+        {
+            "x": 3.4109812853154073,
+            "y": 6.086004715644374,
+            "heading": -3.4399834546372206e-17,
+            "angularVelocity": -5.232629825878709e-17,
+            "velocityX": 2.2543719184459183,
+            "velocityY": -1.6281448418833084,
+            "timestamp": 0.7930991013800723
+        },
+        {
+            "x": 3.4651524420580837,
+            "y": 6.042336211273467,
+            "heading": -3.565040387084894e-17,
+            "angularVelocity": -5.2323095531564346e-17,
+            "velocityX": 2.2664897539752324,
+            "velocityY": -1.827064875100061,
+            "timestamp": 0.8170000065783707
+        },
+        {
+            "x": 3.5195303175635244,
+            "y": 5.993909012954667,
+            "heading": -3.501524266469143e-17,
+            "angularVelocity": 2.6574776159275344e-17,
+            "velocityX": 2.2751387470175053,
+            "velocityY": -2.0261658676529293,
+            "timestamp": 0.840900911776669
+        },
+        {
+            "x": 3.5740044311548194,
+            "y": 5.940719606067823,
+            "heading": -3.37448298204232e-17,
+            "angularVelocity": 5.315333634409861e-17,
+            "velocityX": 2.2791652926673844,
+            "velocityY": -2.225413909872768,
+            "timestamp": 0.8648018169749674
+        },
+        {
+            "x": 3.628420546352513,
+            "y": 5.8827673726499405,
+            "heading": -3.046153632575651e-17,
+            "angularVelocity": 1.3737109446355186e-16,
+            "velocityX": 2.2767386735448056,
+            "velocityY": -2.4246878073056584,
+            "timestamp": 0.8887027221732657
+        },
+        {
+            "x": 3.6825492209054214,
+            "y": 5.820060642331504,
+            "heading": -2.5159851350702206e-17,
+            "angularVelocity": 2.2181942251709706e-16,
+            "velocityX": 2.264712323814494,
+            "velocityY": -2.6236131978340897,
+            "timestamp": 0.9126036273715641
+        },
+        {
+            "x": 3.736016878561411,
+            "y": 5.75263682695494,
+            "heading": -1.874886441281884e-17,
+            "angularVelocity": 2.6823197281004355e-16,
+            "velocityX": 2.237055760540655,
+            "velocityY": -2.8209732985913787,
+            "timestamp": 0.9365045325698624
+        },
+        {
+            "x": 3.788130152189531,
+            "y": 5.680646456241098,
+            "heading": -1.0550078419468136e-17,
+            "angularVelocity": 3.4303244721005534e-16,
+            "velocityX": 2.1803891189790523,
+            "velocityY": -3.012035323204781,
+            "timestamp": 0.9604054377681608
+        },
+        {
+            "x": 3.837400250784585,
+            "y": 5.604834562813646,
+            "heading": 5.433473713273816e-19,
+            "angularVelocity": 4.641424958072882e-16,
+            "velocityX": 2.061432325942265,
+            "velocityY": -3.171925615304708,
+            "timestamp": 0.9843063429664591
+        },
+        {
+            "x": 3.882609579100313,
+            "y": 5.526533156121729,
+            "heading": 5.968487826268029e-18,
+            "angularVelocity": 2.269847275628469e-16,
+            "velocityX": 1.8915320545661611,
+            "velocityY": -3.2760854052293937,
+            "timestamp": 1.0082072481647575
+        },
+        {
+            "x": 3.923632674014372,
+            "y": 5.445959589727867,
+            "heading": 1.2150851940113108e-17,
+            "angularVelocity": 2.586665259140546e-16,
+            "velocityX": 1.716382478977385,
+            "velocityY": -3.3711512482631147,
+            "timestamp": 1.0321081533630558
+        },
+        {
+            "x": 3.960355768910532,
+            "y": 5.363337507789521,
+            "heading": 1.8690802571261126e-17,
+            "angularVelocity": 2.736277382336804e-16,
+            "velocityX": 1.5364729742024472,
+            "velocityY": -3.456859949564964,
+            "timestamp": 1.0560090585613542
+        },
+        {
+            "x": 4.001130960542707,
+            "y": 5.282638203170642,
+            "heading": 2.7305426078750436e-17,
+            "angularVelocity": 3.6043084711714437e-16,
+            "velocityX": 1.7060103495610794,
+            "velocityY": -3.376412062611935,
+            "timestamp": 1.0799099637596525
+        },
+        {
+            "x": 4.046099298269293,
+            "y": 5.204198147271007,
+            "heading": 3.276849439101543e-17,
+            "angularVelocity": 2.285716070976872e-16,
+            "velocityX": 1.8814491482016318,
+            "velocityY": -3.281886407600119,
+            "timestamp": 1.1038108689579509
+        },
+        {
+            "x": 4.095136049561521,
+            "y": 5.128235061004533,
+            "heading": 2.8631224607755774e-17,
+            "angularVelocity": -1.7310096621588863e-16,
+            "velocityX": 2.051669210240635,
+            "velocityY": -3.178251435928111,
+            "timestamp": 1.1277117741562492
+        },
+        {
+            "x": 4.148105144500732,
+            "y": 5.054959774017334,
+            "heading": 1.6930966409187752e-17,
+            "angularVelocity": -4.895320115071494e-16,
+            "velocityX": 2.2161961858659356,
+            "velocityY": -3.065795474240693,
+            "timestamp": 1.1516126793545476
+        },
+        {
+            "x": 4.265239674165176,
+            "y": 4.923669357008322,
+            "heading": 4.8393962188151695e-17,
+            "angularVelocity": 6.764650813120357e-16,
+            "velocityX": 2.5184321191325005,
+            "velocityY": -2.8227884986351293,
+            "timestamp": 1.1981235734837035
+        },
+        {
+            "x": 4.395200374134661,
+            "y": 4.805061224730511,
+            "heading": 5.597930320273315e-17,
+            "angularVelocity": 1.6308740471674414e-16,
+            "velocityX": 2.7941991312528214,
+            "velocityY": -2.550115075157559,
+            "timestamp": 1.2446344676128593
+        },
+        {
+            "x": 4.536621337779305,
+            "y": 4.700381789438767,
+            "heading": 7.583650477943765e-17,
+            "angularVelocity": 4.269365693457232e-16,
+            "velocityX": 3.040598687523153,
+            "velocityY": -2.250643365424577,
+            "timestamp": 1.2911453617420152
+        },
+        {
+            "x": 4.6880158468821485,
+            "y": 4.610730375722861,
+            "heading": 8.702958111273957e-17,
+            "angularVelocity": 2.4065493779199536e-16,
+            "velocityX": 3.2550332978427186,
+            "velocityY": -1.9275358041271147,
+            "timestamp": 1.337656255871171
+        },
+        {
+            "x": 4.842298757317137,
+            "y": 4.526146352730071,
+            "heading": 1.0187276641692621e-16,
+            "angularVelocity": 3.1913351876162675e-16,
+            "velocityX": 3.317134906212745,
+            "velocityY": -1.81858518475069,
+            "timestamp": 1.384167150000327
+        },
+        {
+            "x": 4.996581769126158,
+            "y": 4.441562514645757,
+            "heading": 8.403781588381205e-17,
+            "angularVelocity": -3.8345748597281753e-16,
+            "velocityX": 3.3171370857888243,
+            "velocityY": -1.818581209155721,
+            "timestamp": 1.4306780441294829
+        },
+        {
+            "x": 5.15086732735743,
+            "y": 4.356983321464328,
+            "heading": 9.522247870814417e-17,
+            "angularVelocity": 2.4047404444902996e-16,
+            "velocityX": 3.317191834731107,
+            "velocityY": -1.818481342167903,
+            "timestamp": 1.4771889382586387
+        },
+        {
+            "x": 5.312538078249567,
+            "y": 4.287555361855474,
+            "heading": 1.0159270601289723e-16,
+            "angularVelocity": 1.369620477960438e-16,
+            "velocityX": 3.4759759819537814,
+            "velocityY": -1.4927246811478623,
+            "timestamp": 1.5236998323877946
+        },
+        {
+            "x": 5.480467319488525,
+            "y": 4.235044002532959,
+            "heading": 8.575039074051001e-17,
+            "angularVelocity": -3.4061515197699265e-16,
+            "velocityX": 3.6105356472537267,
+            "velocityY": -1.1290120369797083,
+            "timestamp": 1.5702107265169505
+        },
+        {
+            "x": 5.592904232264291,
+            "y": 4.2080430634395825,
+            "heading": 8.77612564470466e-17,
+            "angularVelocity": 6.578528400068062e-17,
+            "velocityX": 3.6783631124997167,
+            "velocityY": -0.8833332035895192,
+            "timestamp": 1.6007778318670682
+        },
+        {
+            "x": 5.706904061688869,
+            "y": 4.1886742955111504,
+            "heading": 8.48190648175679e-17,
+            "angularVelocity": -9.625352468867059e-17,
+            "velocityX": 3.7294937848649896,
+            "velocityY": -0.6336474359145746,
+            "timestamp": 1.631344937217186
+        },
+        {
+            "x": 5.821949322476148,
+            "y": 4.1770253308728975,
+            "heading": 6.614143850361712e-17,
+            "angularVelocity": -6.110368020256626e-16,
+            "velocityX": 3.7636949743701025,
+            "velocityY": -0.3810947914375838,
+            "timestamp": 1.6619120425673037
+        },
+        {
+            "x": 5.937517692705647,
+            "y": 4.173144318562267,
+            "heading": 5.289613863539961e-17,
+            "angularVelocity": -4.3331874941909564e-16,
+            "velocityX": 3.7808084509727644,
+            "velocityY": -0.12696695569208769,
+            "timestamp": 1.6924791479174215
+        },
+        {
+            "x": 6.05311068319219,
+            "y": 4.170083108620971,
+            "heading": 6.789574595295852e-17,
+            "angularVelocity": 4.90710754117939e-16,
+            "velocityX": 3.7816139003851448,
+            "velocityY": -0.1001471976568426,
+            "timestamp": 1.7230462532675392
+        },
+        {
+            "x": 6.168703676027133,
+            "y": 4.167021987357848,
+            "heading": 5.651709601242885e-17,
+            "angularVelocity": -3.7225147131887107e-16,
+            "velocityX": 3.7816139772128325,
+            "velocityY": -0.1001442965586977,
+            "timestamp": 1.753613358617657
+        },
+        {
+            "x": 6.284296668862326,
+            "y": 4.163960866104177,
+            "heading": 5.090280597716889e-17,
+            "angularVelocity": -1.8367097476040197e-16,
+            "velocityX": 3.7816139772210215,
+            "velocityY": -0.10014429624946593,
+            "timestamp": 1.7841804639677747
+        },
+        {
+            "x": 6.399889661697518,
+            "y": 4.160899744850507,
+            "heading": 6.173261084277577e-17,
+            "angularVelocity": 3.542960558928146e-16,
+            "velocityX": 3.7816139772210224,
+            "velocityY": -0.10014429624943298,
+            "timestamp": 1.8147475693178925
+        },
+        {
+            "x": 6.515482654532711,
+            "y": 4.157838623596838,
+            "heading": 8.001124700396023e-17,
+            "angularVelocity": 5.979838768447224e-16,
+            "velocityX": 3.7816139772210224,
+            "velocityY": -0.10014429624943295,
+            "timestamp": 1.8453146746680102
+        },
+        {
+            "x": 6.631075647367904,
+            "y": 4.154777502343168,
+            "heading": 7.855690079559015e-17,
+            "angularVelocity": -4.757880053117145e-17,
+            "velocityX": 3.7816139772210224,
+            "velocityY": -0.10014429624943294,
+            "timestamp": 1.875881780018128
+        },
+        {
+            "x": 6.746668640203097,
+            "y": 4.151716381089498,
+            "heading": 7.690538849750639e-17,
+            "angularVelocity": -5.4029070766342576e-17,
+            "velocityX": 3.7816139772210224,
+            "velocityY": -0.10014429624943297,
+            "timestamp": 1.9064488853682457
+        },
+        {
+            "x": 6.862261633037354,
+            "y": 4.148655259835854,
+            "heading": 7.874213255028351e-17,
+            "angularVelocity": 6.00889103413278e-17,
+            "velocityX": 3.781613977190394,
+            "velocityY": -0.10014429624862185,
+            "timestamp": 1.9370159907183635
+        },
+        {
+            "x": 6.971293364430436,
+            "y": 4.1457678932048365,
+            "heading": 7.034301862221845e-17,
+            "angularVelocity": -2.747762270062427e-16,
+            "velocityX": 3.56696292122612,
+            "velocityY": -0.09445992997849091,
+            "timestamp": 1.9675830960684813
+        },
+        {
+            "x": 7.072537143209538,
+            "y": 4.14308676630068,
+            "heading": 6.196577755471392e-17,
+            "angularVelocity": -2.740606584102676e-16,
+            "velocityX": 3.3121807779784156,
+            "velocityY": -0.08771281655386487,
+            "timestamp": 1.998150201418599
+        },
+        {
+            "x": 7.165992951009495,
+            "y": 4.140611879609727,
+            "heading": 5.371713190017333e-17,
+            "angularVelocity": -2.6985367144933565e-16,
+            "velocityX": 3.057398033916134,
+            "velocityY": -0.08096568721853159,
+            "timestamp": 2.0287173067687165
+        },
+        {
+            "x": 7.251660781708473,
+            "y": 4.138343233294095,
+            "heading": 4.5597032779539255e-17,
+            "angularVelocity": -2.6564828431304464e-16,
+            "velocityX": 2.802615089578594,
+            "velocityY": -0.07421855257953036,
+            "timestamp": 2.059284412118834
+        },
+        {
+            "x": 7.329540632245536,
+            "y": 4.136280827434844,
+            "heading": 3.7578034898451724e-17,
+            "angularVelocity": -2.623407674507654e-16,
+            "velocityX": 2.5478320451027607,
+            "velocityY": -0.06747141528867756,
+            "timestamp": 2.0898515174689516
+        },
+        {
+            "x": 7.399632500784112,
+            "y": 4.134424662080611,
+            "heading": 2.960093117982924e-17,
+            "angularVelocity": -2.609702038052913e-16,
+            "velocityX": 2.293048940543746,
+            "velocityY": -0.06072427640670841,
+            "timestamp": 2.120418622819069
+        },
+        {
+            "x": 7.461936386099821,
+            "y": 4.132774737263818,
+            "heading": 2.4038424741612248e-17,
+            "angularVelocity": -1.8197687815335696e-16,
+            "velocityX": 2.0382657959291963,
+            "velocityY": -0.05397713646399285,
+            "timestamp": 2.1509857281691867
+        },
+        {
+            "x": 7.516452287318104,
+            "y": 4.131331053007627,
+            "heading": 1.8665464618967105e-17,
+            "angularVelocity": -1.757758880397929e-16,
+            "velocityX": 1.7834826227035099,
+            "velocityY": -0.04722999576360025,
+            "timestamp": 2.181552833519304
+        },
+        {
+            "x": 7.5631802037830385,
+            "y": 4.130093609329407,
+            "heading": 1.3420986699374698e-17,
+            "angularVelocity": -1.7157260548142223e-16,
+            "velocityX": 1.5286994280194506,
+            "velocityY": -0.04048285449494931,
+            "timestamp": 2.2121199388694217
+        },
+        {
+            "x": 7.6021201349844665,
+            "y": 4.129062406242667,
+            "heading": 8.352476313992174e-18,
+            "angularVelocity": -1.6581584391601633e-16,
+            "velocityX": 1.2739162166455336,
+            "velocityY": -0.033735712784319354,
+            "timestamp": 2.2426870442195392
+        },
+        {
+            "x": 7.633272080514257,
+            "y": 4.128237443758217,
+            "heading": 5.582680805550875e-18,
+            "angularVelocity": -9.061360111293989e-17,
+            "velocityX": 1.019132991919723,
+            "velocityY": -0.026988570720105975,
+            "timestamp": 2.2732541495696568
+        },
+        {
+            "x": 7.656636040038488,
+            "y": 4.127618721884898,
+            "heading": 2.786250494304085e-18,
+            "angularVelocity": -9.148495626615499e-17,
+            "velocityX": 0.7643497562696309,
+            "velocityY": -0.020241428366596953,
+            "timestamp": 2.3038212549197743
+        },
+        {
+            "x": 7.672212013278888,
+            "y": 4.1272062406300805,
+            "heading": 1.8952405540316967e-19,
+            "angularVelocity": -8.495166341555795e-17,
+            "velocityX": 0.5095665115159678,
+            "velocityY": -0.013494285772008131,
+            "timestamp": 2.334388360269892
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 6.998640646947103e-35,
+            "angularVelocity": -6.20026191774287e-18,
+            "velocityX": 0.25478325905928073,
+            "velocityY": -0.006747142973428655,
+            "timestamp": 2.3649554656200094
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 3.616659043590546e-35,
+            "angularVelocity": -1.1064131915191804e-33,
+            "velocityX": -5.781348077610764e-36,
+            "velocityY": -1.0341970128847813e-32,
+            "timestamp": 2.395522570970127
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.traj
index 72a99c84..11fcc4a7 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.traj	
@@ -4,568 +4,622 @@
             "x": 2.25,
             "y": 5.56,
             "heading": 0,
-            "angularVelocity": 8.750120685231394e-32,
-            "velocityX": 3.618098191401073e-32,
-            "velocityY": 7.301888602720515e-32,
+            "angularVelocity": 0,
+            "velocityX": 3.8917516758910124e-38,
+            "velocityY": -7.197289285396863e-34,
             "timestamp": 0
         },
         {
-            "x": 2.256418341264449,
-            "y": 5.558391022160101,
-            "heading": -2.5296452464584536e-15,
-            "angularVelocity": -8.979773235851594e-14,
-            "velocityX": 0.22783925602590605,
-            "velocityY": -0.05711574048502746,
-            "timestamp": 0.0281704802605187
-        },
-        {
-            "x": 2.2692550247581535,
-            "y": 5.555173071000616,
-            "heading": -7.593688933611638e-15,
-            "angularVelocity": -1.7976419432911875e-13,
-            "velocityX": 0.4556785463006615,
-            "velocityY": -0.11423132050732354,
-            "timestamp": 0.0563409605210374
-        },
-        {
-            "x": 2.2885100515708303,
-            "y": 5.55034615163608,
-            "heading": -1.5195474783343342e-14,
-            "angularVelocity": -2.6984935224723246e-13,
-            "velocityX": 0.6835178752583485,
-            "velocityY": -0.1713467189730698,
-            "timestamp": 0.0845114407815561
-        },
-        {
-            "x": 2.314183422940896,
-            "y": 5.54391026989229,
-            "heading": -2.533887633402318e-14,
-            "angularVelocity": -3.6007201361891127e-13,
-            "velocityX": 0.9113572481775378,
-            "velocityY": -0.2284619106337897,
-            "timestamp": 0.1126819210420748
-        },
-        {
-            "x": 2.346275140284958,
-            "y": 5.535865432453464,
-            "heading": -3.8024880419905343e-14,
-            "angularVelocity": -4.5032970578145327e-13,
-            "velocityX": 1.139196671383666,
-            "velocityY": -0.2855768650171331,
-            "timestamp": 0.14085240130259352
-        },
-        {
-            "x": 2.3847852052344276,
-            "y": 5.526211647048542,
-            "heading": -5.3249348657163737e-14,
-            "angularVelocity": -5.404404929621474e-13,
-            "velocityX": 1.3670361525018668,
-            "velocityY": -0.3426915450373883,
-            "timestamp": 0.16902288156311224
-        },
-        {
-            "x": 2.4297136196807942,
-            "y": 5.514948922688138,
-            "heading": -7.10022499667577e-14,
-            "angularVelocity": -6.301951953680268e-13,
-            "velocityX": 1.594875700764475,
-            "velocityY": -0.3998059051974526,
-            "timestamp": 0.19719336182363095
-        },
-        {
-            "x": 2.4810603858307716,
-            "y": 5.502077269965895,
-            "heading": -9.12659901579941e-14,
-            "angularVelocity": -7.193253365088161e-13,
-            "velocityX": 1.8227153273613368,
-            "velocityY": -0.45691988930278143,
-            "timestamp": 0.22536384208414967
-        },
-        {
-            "x": 2.5388255062707983,
-            "y": 5.4875967014378215,
-            "heading": -1.1400857239830863e-13,
-            "angularVelocity": -8.073196492084524e-13,
-            "velocityX": 2.050555045771992,
-            "velocityY": -0.5140334276930385,
-            "timestamp": 0.2535343223446684
-        },
-        {
-            "x": 2.603008984034667,
-            "y": 5.4715072320838045,
-            "heading": -1.3917722607752625e-13,
-            "angularVelocity": -8.934407027436765e-13,
-            "velocityX": 2.2783948718766736,
-            "velocityY": -0.571146434324935,
-            "timestamp": 0.2817048026051871
-        },
-        {
-            "x": 2.673610822650838,
-            "y": 5.453808879815544,
-            "heading": -1.6668826583849139e-13,
-            "angularVelocity": -9.76591080715578e-13,
-            "velocityX": 2.5062348232351805,
-            "velocityY": -0.6282588051246059,
-            "timestamp": 0.3098752828657058
-        },
-        {
-            "x": 2.7506310260932763,
-            "y": 5.434501665856061,
-            "heading": -1.9641690622397207e-13,
-            "angularVelocity": -1.0553118055097966e-12,
-            "velocityX": 2.7340749156620854,
-            "velocityY": -0.6853704225462842,
-            "timestamp": 0.33804576312622453
-        },
-        {
-            "x": 2.8340695983911575,
-            "y": 5.413585614341132,
-            "heading": -2.282023162882806e-13,
-            "angularVelocity": -1.128323328886794e-12,
-            "velocityX": 2.9619151511173074,
-            "velocityY": -0.7424811831924288,
-            "timestamp": 0.36621624338674325
-        },
-        {
-            "x": 2.9239265420414853,
-            "y": 5.391060748788907,
-            "heading": -2.61980248572899e-13,
-            "angularVelocity": -1.1990541861941579e-12,
-            "velocityX": 3.189755475211519,
-            "velocityY": -0.7995911089877218,
-            "timestamp": 0.39438672364726196
-        },
-        {
-            "x": 3.020201851829833,
-            "y": 5.366927076274376,
-            "heading": -2.9867647823192555e-13,
-            "angularVelocity": -1.302648350982896e-12,
-            "velocityX": 3.417595614203248,
-            "velocityY": -0.8567007836339452,
-            "timestamp": 0.4225572039077807
-        },
-        {
-            "x": 3.12289548969471,
-            "y": 5.341184519140017,
-            "heading": -3.474157859958988e-13,
-            "angularVelocity": -1.7301553723681501e-12,
-            "velocityX": 3.6454344020823566,
-            "velocityY": -0.9138132149787032,
-            "timestamp": 0.4507276841682994
-        },
-        {
-            "x": 3.2262642847742486,
-            "y": 5.315271868799975,
-            "heading": -3.474349355086831e-13,
-            "angularVelocity": -6.797723212352284e-16,
-            "velocityX": 3.6694012357471815,
-            "velocityY": -0.9198512095073661,
-            "timestamp": 0.4788981644288181
-        },
-        {
-            "x": 3.3296318094304937,
-            "y": 5.289354150477899,
-            "heading": -3.47428956348503e-13,
-            "angularVelocity": 2.1224913898406702e-16,
-            "velocityX": 3.669356138067555,
-            "velocityY": -0.920031113505745,
-            "timestamp": 0.5070686446893368
-        },
-        {
-            "x": 3.4329975109325144,
-            "y": 5.263429161946644,
-            "heading": -3.4742781851400116e-13,
-            "angularVelocity": 4.0391022494337805e-17,
-            "velocityX": 3.669291419461124,
-            "velocityY": -0.9202891924987843,
-            "timestamp": 0.5352391249498555
-        },
-        {
-            "x": 3.536360974747324,
-            "y": 5.237495253145949,
-            "heading": -3.474322284236003e-13,
-            "angularVelocity": -1.565436427905273e-16,
-            "velocityX": 3.669211985699591,
-            "velocityY": -0.9206058455823695,
-            "timestamp": 0.5634096052103742
-        },
-        {
-            "x": 3.6397250084501422,
-            "y": 5.211563615847215,
-            "heading": -3.474363074560882e-13,
-            "angularVelocity": -1.4479811668640362e-16,
-            "velocityX": 3.6692322156709736,
-            "velocityY": -0.9205252114596718,
-            "timestamp": 0.5915800854708929
-        },
-        {
-            "x": 3.7430830091153613,
-            "y": 5.185607942589789,
-            "heading": -3.474405753435316e-13,
-            "angularVelocity": -1.515021186684875e-16,
-            "velocityX": 3.669018053983158,
-            "velocityY": -0.9213784435831122,
-            "timestamp": 0.6197505657314116
-        },
-        {
-            "x": 3.8464299042958676,
-            "y": 5.1596080862072276,
-            "heading": -3.474364187384818e-13,
-            "angularVelocity": 1.4755179936907513e-16,
-            "velocityX": 3.668623829794913,
-            "velocityY": -0.92294686289042,
-            "timestamp": 0.6479210459919303
-        },
-        {
-            "x": 3.9490305912287407,
-            "y": 5.130804053406161,
-            "heading": -3.4744608105895375e-13,
-            "angularVelocity": -3.429945241470833e-16,
-            "velocityX": 3.642134815737201,
-            "velocityY": -1.0224899446047346,
-            "timestamp": 0.6760915262524491
-        },
-        {
-            "x": 4.049738686089906,
-            "y": 5.09595505292046,
-            "heading": -3.4745592092862155e-13,
-            "angularVelocity": -3.4929719255187864e-16,
-            "velocityX": 3.57495129404341,
-            "velocityY": -1.237075128411728,
-            "timestamp": 0.7042620065129678
+            "x": 2.2621897956017167,
+            "y": 5.610801651791703,
+            "heading": 3.480716271732917e-18,
+            "angularVelocity": 4.397302355093269e-17,
+            "velocityX": 0.15399765670735568,
+            "velocityY": 0.6417938075748972,
+            "timestamp": 0.07915572134244127
+        },
+        {
+            "x": 2.2905203388747193,
+            "y": 5.711291077569501,
+            "heading": 3.7329161294500195e-18,
+            "angularVelocity": 3.1861229325774303e-18,
+            "velocityX": 0.35790897729855475,
+            "velocityY": 1.2695156341645175,
+            "timestamp": 0.15831144268488254
+        },
+        {
+            "x": 2.3436883815778624,
+            "y": 5.857742406339618,
+            "heading": 7.49671396174733e-18,
+            "angularVelocity": 4.754928420848352e-17,
+            "velocityX": 0.6716891944314332,
+            "velocityY": 1.8501673193848232,
+            "timestamp": 0.23746716402732382
+        },
+        {
+            "x": 2.444399562681219,
+            "y": 6.0258512044397365,
+            "heading": 1.0345374349306828e-17,
+            "angularVelocity": 3.5988057740869555e-17,
+            "velocityX": 1.2723171413935193,
+            "velocityY": 2.1237731808778637,
+            "timestamp": 0.3166228853697651
+        },
+        {
+            "x": 2.5795093648860483,
+            "y": 6.154639121411333,
+            "heading": 6.772254039775596e-18,
+            "angularVelocity": -4.5140391310627574e-17,
+            "velocityX": 1.706886121602264,
+            "velocityY": 1.6270196871106504,
+            "timestamp": 0.3957786067122063
+        },
+        {
+            "x": 2.7311723872223266,
+            "y": 6.233875155156547,
+            "heading": -7.527039882559706e-19,
+            "angularVelocity": -9.506524797608463e-17,
+            "velocityX": 1.916008341079448,
+            "velocityY": 1.00101461273312,
+            "timestamp": 0.4749343280546476
+        },
+        {
+            "x": 2.892774820327759,
+            "y": 6.261821746826172,
+            "heading": -9.856118161245683e-18,
+            "angularVelocity": -1.1500639891239944e-16,
+            "velocityX": 2.041576153495111,
+            "velocityY": 0.3530583916824199,
+            "timestamp": 0.5540900493970888
+        },
+        {
+            "x": 2.9421812890727397,
+            "y": 6.2655363212140305,
+            "heading": -1.2690994415516207e-17,
+            "angularVelocity": -1.1860957835731596e-16,
+            "velocityX": 2.067137973857932,
+            "velocityY": 0.15541563622967255,
+            "timestamp": 0.5779909545953872
+        },
+        {
+            "x": 2.992183666499164,
+            "y": 6.264525133875901,
+            "heading": -1.5700573063161643e-17,
+            "angularVelocity": -1.2591902515397632e-16,
+            "velocityX": 2.092070447188938,
+            "velocityY": -0.042307491274448855,
+            "timestamp": 0.6018918597936855
+        },
+        {
+            "x": 3.0427650175812166,
+            "y": 6.258786080293358,
+            "heading": -1.8888665404790212e-17,
+            "angularVelocity": -1.333879346368914e-16,
+            "velocityX": 2.1162943688699283,
+            "velocityY": -0.24011867060800707,
+            "timestamp": 0.6257927649919839
+        },
+        {
+            "x": 3.093906133667358,
+            "y": 6.248316847924188,
+            "heading": -2.226054198779156e-17,
+            "angularVelocity": -1.4107736063473173e-16,
+            "velocityX": 2.139714611720349,
+            "velocityY": -0.4380266053653904,
+            "timestamp": 0.6496936701902822
+        },
+        {
+            "x": 3.1455850428469736,
+            "y": 6.233114888531311,
+            "heading": -2.51599280773308e-17,
+            "angularVelocity": -1.2130863242066928e-16,
+            "velocityX": 2.162215562584425,
+            "velocityY": -0.6360411568829696,
+            "timestamp": 0.6735945753885806
+        },
+        {
+            "x": 3.197776369275469,
+            "y": 6.213177387662576,
+            "heading": -2.8136873311901383e-17,
+            "angularVelocity": -1.2455366258796381e-16,
+            "velocityX": 2.1836548028403113,
+            "velocityY": -0.8341734634450271,
+            "timestamp": 0.6974954805868789
+        },
+        {
+            "x": 3.2504504785577715,
+            "y": 6.188501232669928,
+            "heading": -3.110871206110287e-17,
+            "angularVelocity": -1.2434001017092495e-16,
+            "velocityX": 2.203854157207935,
+            "velocityY": -1.0324360013948046,
+            "timestamp": 0.7213963857851773
+        },
+        {
+            "x": 3.3035723121371006,
+            "y": 6.159082982949556,
+            "heading": -3.205106262321403e-17,
+            "angularVelocity": -3.942740156809933e-17,
+            "velocityX": 2.222586681909921,
+            "velocityY": -1.2308424922109886,
+            "timestamp": 0.7452972909834756
+        },
+        {
+            "x": 3.3570997558109252,
+            "y": 6.124918851159325,
+            "heading": -3.3149188675890806e-17,
+            "angularVelocity": -4.594495761931231e-17,
+            "velocityX": 2.239557172823534,
+            "velocityY": -1.429407443223672,
+            "timestamp": 0.769198196181774
+        },
+        {
+            "x": 3.4109812853154073,
+            "y": 6.086004715644374,
+            "heading": -3.4399834546372206e-17,
+            "angularVelocity": -5.232629825878709e-17,
+            "velocityX": 2.2543719184459183,
+            "velocityY": -1.6281448418833084,
+            "timestamp": 0.7930991013800723
+        },
+        {
+            "x": 3.4651524420580837,
+            "y": 6.042336211273467,
+            "heading": -3.565040387084894e-17,
+            "angularVelocity": -5.2323095531564346e-17,
+            "velocityX": 2.2664897539752324,
+            "velocityY": -1.827064875100061,
+            "timestamp": 0.8170000065783707
+        },
+        {
+            "x": 3.5195303175635244,
+            "y": 5.993909012954667,
+            "heading": -3.501524266469143e-17,
+            "angularVelocity": 2.6574776159275344e-17,
+            "velocityX": 2.2751387470175053,
+            "velocityY": -2.0261658676529293,
+            "timestamp": 0.840900911776669
+        },
+        {
+            "x": 3.5740044311548194,
+            "y": 5.940719606067823,
+            "heading": -3.37448298204232e-17,
+            "angularVelocity": 5.315333634409861e-17,
+            "velocityX": 2.2791652926673844,
+            "velocityY": -2.225413909872768,
+            "timestamp": 0.8648018169749674
+        },
+        {
+            "x": 3.628420546352513,
+            "y": 5.8827673726499405,
+            "heading": -3.046153632575651e-17,
+            "angularVelocity": 1.3737109446355186e-16,
+            "velocityX": 2.2767386735448056,
+            "velocityY": -2.4246878073056584,
+            "timestamp": 0.8887027221732657
+        },
+        {
+            "x": 3.6825492209054214,
+            "y": 5.820060642331504,
+            "heading": -2.5159851350702206e-17,
+            "angularVelocity": 2.2181942251709706e-16,
+            "velocityX": 2.264712323814494,
+            "velocityY": -2.6236131978340897,
+            "timestamp": 0.9126036273715641
+        },
+        {
+            "x": 3.736016878561411,
+            "y": 5.75263682695494,
+            "heading": -1.874886441281884e-17,
+            "angularVelocity": 2.6823197281004355e-16,
+            "velocityX": 2.237055760540655,
+            "velocityY": -2.8209732985913787,
+            "timestamp": 0.9365045325698624
+        },
+        {
+            "x": 3.788130152189531,
+            "y": 5.680646456241098,
+            "heading": -1.0550078419468136e-17,
+            "angularVelocity": 3.4303244721005534e-16,
+            "velocityX": 2.1803891189790523,
+            "velocityY": -3.012035323204781,
+            "timestamp": 0.9604054377681608
+        },
+        {
+            "x": 3.837400250784585,
+            "y": 5.604834562813646,
+            "heading": 5.433473713273816e-19,
+            "angularVelocity": 4.641424958072882e-16,
+            "velocityX": 2.061432325942265,
+            "velocityY": -3.171925615304708,
+            "timestamp": 0.9843063429664591
+        },
+        {
+            "x": 3.882609579100313,
+            "y": 5.526533156121729,
+            "heading": 5.968487826268029e-18,
+            "angularVelocity": 2.269847275628469e-16,
+            "velocityX": 1.8915320545661611,
+            "velocityY": -3.2760854052293937,
+            "timestamp": 1.0082072481647575
+        },
+        {
+            "x": 3.923632674014372,
+            "y": 5.445959589727867,
+            "heading": 1.2150851940113108e-17,
+            "angularVelocity": 2.586665259140546e-16,
+            "velocityX": 1.716382478977385,
+            "velocityY": -3.3711512482631147,
+            "timestamp": 1.0321081533630558
+        },
+        {
+            "x": 3.960355768910532,
+            "y": 5.363337507789521,
+            "heading": 1.8690802571261126e-17,
+            "angularVelocity": 2.736277382336804e-16,
+            "velocityX": 1.5364729742024472,
+            "velocityY": -3.456859949564964,
+            "timestamp": 1.0560090585613542
+        },
+        {
+            "x": 4.001130960542707,
+            "y": 5.282638203170642,
+            "heading": 2.7305426078750436e-17,
+            "angularVelocity": 3.6043084711714437e-16,
+            "velocityX": 1.7060103495610794,
+            "velocityY": -3.376412062611935,
+            "timestamp": 1.0799099637596525
+        },
+        {
+            "x": 4.046099298269293,
+            "y": 5.204198147271007,
+            "heading": 3.276849439101543e-17,
+            "angularVelocity": 2.285716070976872e-16,
+            "velocityX": 1.8814491482016318,
+            "velocityY": -3.281886407600119,
+            "timestamp": 1.1038108689579509
+        },
+        {
+            "x": 4.095136049561521,
+            "y": 5.128235061004533,
+            "heading": 2.8631224607755774e-17,
+            "angularVelocity": -1.7310096621588863e-16,
+            "velocityX": 2.051669210240635,
+            "velocityY": -3.178251435928111,
+            "timestamp": 1.1277117741562492
         },
         {
             "x": 4.148105144500732,
             "y": 5.054959774017334,
-            "heading": -3.474586084587748e-13,
-            "angularVelocity": -9.540235481877701e-17,
-            "velocityX": 3.491827526585996,
-            "velocityY": -1.4552566560457803,
-            "timestamp": 0.7324324867734865
-        },
-        {
-            "x": 4.301558125072517,
-            "y": 4.97182210350292,
-            "heading": -3.4745624581283064e-13,
-            "angularVelocity": 5.1211249699831376e-17,
-            "velocityX": 3.3261517465815813,
-            "velocityY": -1.8020406443565171,
-            "timestamp": 0.7785677783246081
-        },
-        {
-            "x": 4.445898119107738,
-            "y": 4.873711676741488,
-            "heading": -3.4747329562610653e-13,
-            "angularVelocity": -3.6956119081305154e-16,
-            "velocityX": 3.1286242956827928,
-            "velocityY": -2.1265808335182323,
-            "timestamp": 0.8247030698757297
-        },
-        {
-            "x": 4.587571576014754,
-            "y": 4.771788561847154,
-            "heading": -3.4746251546031876e-13,
-            "angularVelocity": 2.3366419539300746e-16,
-            "velocityX": 3.0708260887450245,
-            "velocityY": -2.2092222996219535,
-            "timestamp": 0.8708383614268513
-        },
-        {
-            "x": 4.729219222551993,
-            "y": 4.66982957987028,
-            "heading": -3.474587781166732e-13,
-            "angularVelocity": 8.100834549122579e-17,
-            "velocityX": 3.070266639108493,
-            "velocityY": -2.209999732353969,
-            "timestamp": 0.916973652977973
-        },
-        {
-            "x": 4.870877206231954,
-            "y": 4.567884960544782,
-            "heading": -3.47487867941798e-13,
-            "angularVelocity": -6.305330291624225e-16,
-            "velocityX": 3.070490700636254,
-            "velocityY": -2.209688416384298,
-            "timestamp": 0.9631089445290946
-        },
-        {
-            "x": 5.012587244979431,
-            "y": 4.466012713767847,
-            "heading": -3.4750411523417436e-13,
-            "angularVelocity": -3.5216624493767476e-16,
-            "velocityX": 3.0716190140743462,
-            "velocityY": -2.208119713797033,
-            "timestamp": 1.009244236080216
-        },
-        {
-            "x": 5.160453670184351,
-            "y": 4.373302358522453,
-            "heading": -3.47491376477911e-13,
-            "angularVelocity": 2.7611738924244047e-16,
-            "velocityX": 3.2050610331965634,
-            "velocityY": -2.009532228521384,
-            "timestamp": 1.0553795276313376
-        },
-        {
-            "x": 5.316942928228011,
-            "y": 4.29603139716118,
-            "heading": -3.4750002331748596e-13,
-            "angularVelocity": -1.8742353831509497e-16,
-            "velocityX": 3.3919642161767567,
-            "velocityY": -1.6748774910066955,
-            "timestamp": 1.101514819182459
+            "heading": 1.6930966409187752e-17,
+            "angularVelocity": -4.895320115071494e-16,
+            "velocityX": 2.2161961858659356,
+            "velocityY": -3.065795474240693,
+            "timestamp": 1.1516126793545476
+        },
+        {
+            "x": 4.265239674165176,
+            "y": 4.923669357008322,
+            "heading": 4.8393962188151695e-17,
+            "angularVelocity": 6.764650813120357e-16,
+            "velocityX": 2.5184321191325005,
+            "velocityY": -2.8227884986351293,
+            "timestamp": 1.1981235734837035
+        },
+        {
+            "x": 4.395200374134661,
+            "y": 4.805061224730511,
+            "heading": 5.597930320273315e-17,
+            "angularVelocity": 1.6308740471674414e-16,
+            "velocityX": 2.7941991312528214,
+            "velocityY": -2.550115075157559,
+            "timestamp": 1.2446344676128593
+        },
+        {
+            "x": 4.536621337779305,
+            "y": 4.700381789438767,
+            "heading": 7.583650477943765e-17,
+            "angularVelocity": 4.269365693457232e-16,
+            "velocityX": 3.040598687523153,
+            "velocityY": -2.250643365424577,
+            "timestamp": 1.2911453617420152
+        },
+        {
+            "x": 4.6880158468821485,
+            "y": 4.610730375722861,
+            "heading": 8.702958111273957e-17,
+            "angularVelocity": 2.4065493779199536e-16,
+            "velocityX": 3.2550332978427186,
+            "velocityY": -1.9275358041271147,
+            "timestamp": 1.337656255871171
+        },
+        {
+            "x": 4.842298757317137,
+            "y": 4.526146352730071,
+            "heading": 1.0187276641692621e-16,
+            "angularVelocity": 3.1913351876162675e-16,
+            "velocityX": 3.317134906212745,
+            "velocityY": -1.81858518475069,
+            "timestamp": 1.384167150000327
+        },
+        {
+            "x": 4.996581769126158,
+            "y": 4.441562514645757,
+            "heading": 8.403781588381205e-17,
+            "angularVelocity": -3.8345748597281753e-16,
+            "velocityX": 3.3171370857888243,
+            "velocityY": -1.818581209155721,
+            "timestamp": 1.4306780441294829
+        },
+        {
+            "x": 5.15086732735743,
+            "y": 4.356983321464328,
+            "heading": 9.522247870814417e-17,
+            "angularVelocity": 2.4047404444902996e-16,
+            "velocityX": 3.317191834731107,
+            "velocityY": -1.818481342167903,
+            "timestamp": 1.4771889382586387
+        },
+        {
+            "x": 5.312538078249567,
+            "y": 4.287555361855474,
+            "heading": 1.0159270601289723e-16,
+            "angularVelocity": 1.369620477960438e-16,
+            "velocityX": 3.4759759819537814,
+            "velocityY": -1.4927246811478623,
+            "timestamp": 1.5236998323877946
         },
         {
             "x": 5.480467319488525,
             "y": 4.235044002532959,
-            "heading": -3.475163170309309e-13,
-            "angularVelocity": -3.5317243928660403e-16,
-            "velocityX": 3.5444534056932553,
-            "velocityY": -1.321924985762245,
-            "timestamp": 1.1476501107335806
-        },
-        {
-            "x": 5.5914176787721175,
-            "y": 4.201991970759951,
-            "heading": -3.4752963658489967e-13,
-            "angularVelocity": -4.352387405096259e-16,
-            "velocityX": 3.625488497149835,
-            "velocityY": -1.0800304007696078,
-            "timestamp": 1.1782529824212593
-        },
-        {
-            "x": 5.704342870003183,
-            "y": 4.176490587918632,
-            "heading": -3.4752519346531394e-13,
-            "angularVelocity": 1.4518636145588672e-16,
-            "velocityX": 3.6900194329281493,
-            "velocityY": -0.8333003223399679,
-            "timestamp": 1.208855854108938
-        },
-        {
-            "x": 5.818728492967466,
-            "y": 4.15864825583045,
-            "heading": -3.47528320752536e-13,
-            "angularVelocity": -1.0218933861720526e-16,
-            "velocityX": 3.737741481635173,
-            "velocityY": -0.5830280331479286,
-            "timestamp": 1.2394587257966168
-        },
-        {
-            "x": 5.934052260797228,
-            "y": 4.148506842880417,
-            "heading": -3.475166383708343e-13,
-            "angularVelocity": 3.817413550372978e-16,
-            "velocityX": 3.768396933681978,
-            "velocityY": -0.3313876244719343,
-            "timestamp": 1.2700615974842955
-        },
-        {
-            "x": 6.049784939896788,
-            "y": 4.145614314337883,
-            "heading": -3.4752553093783573e-13,
-            "angularVelocity": -2.9057949488904467e-16,
-            "velocityX": 3.7817587931181613,
-            "velocityY": -0.0945182064161531,
-            "timestamp": 1.3006644691719742
-        },
-        {
-            "x": 6.1655453259831,
-            "y": 4.144216905783092,
-            "heading": -3.4753489515012823e-13,
-            "angularVelocity": -3.0599129336581387e-16,
-            "velocityX": 3.7826641652362696,
-            "velocityY": -0.04566266099543126,
-            "timestamp": 1.331267340859653
-        },
-        {
-            "x": 6.281306292241875,
-            "y": 4.142868414150292,
-            "heading": -3.4754223223995867e-13,
-            "angularVelocity": -2.397516775868154e-16,
-            "velocityX": 3.7826831233414593,
-            "velocityY": -0.04406421878574171,
-            "timestamp": 1.3618702125473316
-        },
-        {
-            "x": 6.39706739301833,
-            "y": 4.141531519398385,
-            "heading": -3.475633687164305e-13,
-            "angularVelocity": -6.906697084916283e-16,
-            "velocityX": 3.7826875189315166,
-            "velocityY": -0.04368527131792852,
-            "timestamp": 1.3924730842350104
-        },
-        {
-            "x": 6.512828617962261,
-            "y": 4.14020542040497,
-            "heading": -3.4758398313992887e-13,
-            "angularVelocity": -6.73610754719036e-16,
-            "velocityX": 3.7826915763114313,
-            "velocityY": -0.043332501854336236,
-            "timestamp": 1.423075955922689
-        },
-        {
-            "x": 6.628589931637748,
-            "y": 4.138887090943481,
-            "heading": -3.4757335612235213e-13,
-            "angularVelocity": 3.472555676974266e-16,
-            "velocityX": 3.7826944757635745,
-            "velocityY": -0.04307861939704374,
-            "timestamp": 1.4536788276103678
-        },
-        {
-            "x": 6.744351296535766,
-            "y": 4.137573267392239,
-            "heading": -3.475819448429605e-13,
-            "angularVelocity": -2.806508055564177e-16,
-            "velocityX": 3.782696149545524,
-            "velocityY": -0.042931381231176444,
-            "timestamp": 1.4842816992980465
-        },
-        {
-            "x": 6.860112695906494,
-            "y": 4.136261713498386,
-            "heading": -3.475979530506806e-13,
-            "angularVelocity": -5.230943272138926e-16,
-            "velocityX": 3.7826972759991557,
-            "velocityY": -0.04285721636551622,
-            "timestamp": 1.5148845709857253
-        },
-        {
-            "x": 6.9694302247553,
-            "y": 4.135029589899985,
-            "heading": -2.392571520797574e-13,
-            "angularVelocity": 3.5402168172320343e-12,
-            "velocityX": 3.5721330326269505,
-            "velocityY": -0.04026169865772549,
-            "timestamp": 1.545487442673404
-        },
-        {
-            "x": 7.070940187048315,
-            "y": 4.133884255746116,
-            "heading": -1.6833543298116243e-13,
-            "angularVelocity": 2.317485846705618e-12,
-            "velocityX": 3.3170077412667367,
-            "velocityY": -0.03742570844047695,
-            "timestamp": 1.5760903143610827
-        },
-        {
-            "x": 7.164641690009617,
-            "y": 4.132826276383854,
-            "heading": -1.1981581485168293e-13,
-            "angularVelocity": 1.5854597325485145e-12,
-            "velocityX": 3.061853276960511,
-            "velocityY": -0.034571244567840914,
-            "timestamp": 1.6066931860487614
-        },
-        {
-            "x": 7.250534738513507,
-            "y": 4.131855992058627,
-            "heading": -8.523066967233492e-14,
-            "angularVelocity": 1.1301275060622065e-12,
-            "velocityX": 2.8066989719298543,
-            "velocityY": -0.03170566260882202,
-            "timestamp": 1.6372960577364402
-        },
-        {
-            "x": 7.328619329487729,
-            "y": 4.1309736027819035,
-            "heading": -6.003549662940244e-14,
-            "angularVelocity": 8.232944437274219e-13,
-            "velocityX": 2.5515445665080714,
-            "velocityY": -0.028833544941282405,
-            "timestamp": 1.6678989294241189
-        },
-        {
-            "x": 7.398895462248961,
-            "y": 4.130179235828021,
-            "heading": -4.153185117983844e-14,
-            "angularVelocity": 6.046375734752488e-13,
-            "velocityX": 2.2963901387575416,
-            "velocityY": -0.02595726837221309,
-            "timestamp": 1.6985018011117976
-        },
-        {
-            "x": 7.461363136410412,
-            "y": 4.129472977946104,
-            "heading": -2.797073854746349e-14,
-            "angularVelocity": 4.431320366500053e-13,
-            "velocityX": 2.041235698368024,
-            "velocityY": -0.0230781571305896,
-            "timestamp": 1.7291046727994763
-        },
-        {
-            "x": 7.516022351719397,
-            "y": 4.1288548915008905,
-            "heading": -1.8141238812912564e-14,
-            "angularVelocity": 3.211953369611228e-13,
-            "velocityX": 1.7860812497215437,
-            "velocityY": -0.02019700801679021,
-            "timestamp": 1.759707544487155
-        },
-        {
-            "x": 7.5628731079961895,
-            "y": 4.128325023227497,
-            "heading": -1.1163529343344889e-14,
-            "angularVelocity": 2.2800831618830555e-13,
-            "velocityX": 1.5309267952022354,
-            "velocityY": -0.017314331754962387,
-            "timestamp": 1.7903104161748338
-        },
-        {
-            "x": 7.601915405105201,
-            "y": 4.127883409320133,
-            "heading": -6.372564950074351e-15,
-            "angularVelocity": 1.5655276164796086e-13,
-            "velocityX": 1.2757723362523365,
-            "velocityY": -0.014430472786738478,
-            "timestamp": 1.8209132878625125
-        },
-        {
-            "x": 7.63314924293974,
-            "y": 4.127530078564582,
-            "heading": -3.2435183420078772e-15,
-            "angularVelocity": 1.0224681844586284e-13,
-            "velocityX": 1.020617873816125,
-            "velocityY": -0.011545673194894595,
-            "timestamp": 1.8515161595501912
-        },
-        {
-            "x": 7.656574621413203,
-            "y": 4.127265054360105,
-            "heading": -1.3581702257498648e-15,
-            "angularVelocity": 6.160689169092944e-14,
-            "velocityX": 0.7654634085499833,
-            "velocityY": -0.008660108992989924,
-            "timestamp": 1.88211903123787
-        },
-        {
-            "x": 7.672191540453606,
-            "y": 4.127088356077157,
-            "heading": -3.7222995775515084e-16,
-            "angularVelocity": 3.2217238765593925e-14,
-            "velocityX": 0.5103089409316736,
-            "velocityY": -0.0057739118290542285,
-            "timestamp": 1.9127219029255487
+            "heading": 8.575039074051001e-17,
+            "angularVelocity": -3.4061515197699265e-16,
+            "velocityX": 3.6105356472537267,
+            "velocityY": -1.1290120369797083,
+            "timestamp": 1.5702107265169505
+        },
+        {
+            "x": 5.592904232264291,
+            "y": 4.2080430634395825,
+            "heading": 8.77612564470466e-17,
+            "angularVelocity": 6.578528400068062e-17,
+            "velocityX": 3.6783631124997167,
+            "velocityY": -0.8833332035895192,
+            "timestamp": 1.6007778318670682
+        },
+        {
+            "x": 5.706904061688869,
+            "y": 4.1886742955111504,
+            "heading": 8.48190648175679e-17,
+            "angularVelocity": -9.625352468867059e-17,
+            "velocityX": 3.7294937848649896,
+            "velocityY": -0.6336474359145746,
+            "timestamp": 1.631344937217186
+        },
+        {
+            "x": 5.821949322476148,
+            "y": 4.1770253308728975,
+            "heading": 6.614143850361712e-17,
+            "angularVelocity": -6.110368020256626e-16,
+            "velocityX": 3.7636949743701025,
+            "velocityY": -0.3810947914375838,
+            "timestamp": 1.6619120425673037
+        },
+        {
+            "x": 5.937517692705647,
+            "y": 4.173144318562267,
+            "heading": 5.289613863539961e-17,
+            "angularVelocity": -4.3331874941909564e-16,
+            "velocityX": 3.7808084509727644,
+            "velocityY": -0.12696695569208769,
+            "timestamp": 1.6924791479174215
+        },
+        {
+            "x": 6.05311068319219,
+            "y": 4.170083108620971,
+            "heading": 6.789574595295852e-17,
+            "angularVelocity": 4.90710754117939e-16,
+            "velocityX": 3.7816139003851448,
+            "velocityY": -0.1001471976568426,
+            "timestamp": 1.7230462532675392
+        },
+        {
+            "x": 6.168703676027133,
+            "y": 4.167021987357848,
+            "heading": 5.651709601242885e-17,
+            "angularVelocity": -3.7225147131887107e-16,
+            "velocityX": 3.7816139772128325,
+            "velocityY": -0.1001442965586977,
+            "timestamp": 1.753613358617657
+        },
+        {
+            "x": 6.284296668862326,
+            "y": 4.163960866104177,
+            "heading": 5.090280597716889e-17,
+            "angularVelocity": -1.8367097476040197e-16,
+            "velocityX": 3.7816139772210215,
+            "velocityY": -0.10014429624946593,
+            "timestamp": 1.7841804639677747
+        },
+        {
+            "x": 6.399889661697518,
+            "y": 4.160899744850507,
+            "heading": 6.173261084277577e-17,
+            "angularVelocity": 3.542960558928146e-16,
+            "velocityX": 3.7816139772210224,
+            "velocityY": -0.10014429624943298,
+            "timestamp": 1.8147475693178925
+        },
+        {
+            "x": 6.515482654532711,
+            "y": 4.157838623596838,
+            "heading": 8.001124700396023e-17,
+            "angularVelocity": 5.979838768447224e-16,
+            "velocityX": 3.7816139772210224,
+            "velocityY": -0.10014429624943295,
+            "timestamp": 1.8453146746680102
+        },
+        {
+            "x": 6.631075647367904,
+            "y": 4.154777502343168,
+            "heading": 7.855690079559015e-17,
+            "angularVelocity": -4.757880053117145e-17,
+            "velocityX": 3.7816139772210224,
+            "velocityY": -0.10014429624943294,
+            "timestamp": 1.875881780018128
+        },
+        {
+            "x": 6.746668640203097,
+            "y": 4.151716381089498,
+            "heading": 7.690538849750639e-17,
+            "angularVelocity": -5.4029070766342576e-17,
+            "velocityX": 3.7816139772210224,
+            "velocityY": -0.10014429624943297,
+            "timestamp": 1.9064488853682457
+        },
+        {
+            "x": 6.862261633037354,
+            "y": 4.148655259835854,
+            "heading": 7.874213255028351e-17,
+            "angularVelocity": 6.00889103413278e-17,
+            "velocityX": 3.781613977190394,
+            "velocityY": -0.10014429624862185,
+            "timestamp": 1.9370159907183635
+        },
+        {
+            "x": 6.971293364430436,
+            "y": 4.1457678932048365,
+            "heading": 7.034301862221845e-17,
+            "angularVelocity": -2.747762270062427e-16,
+            "velocityX": 3.56696292122612,
+            "velocityY": -0.09445992997849091,
+            "timestamp": 1.9675830960684813
+        },
+        {
+            "x": 7.072537143209538,
+            "y": 4.14308676630068,
+            "heading": 6.196577755471392e-17,
+            "angularVelocity": -2.740606584102676e-16,
+            "velocityX": 3.3121807779784156,
+            "velocityY": -0.08771281655386487,
+            "timestamp": 1.998150201418599
+        },
+        {
+            "x": 7.165992951009495,
+            "y": 4.140611879609727,
+            "heading": 5.371713190017333e-17,
+            "angularVelocity": -2.6985367144933565e-16,
+            "velocityX": 3.057398033916134,
+            "velocityY": -0.08096568721853159,
+            "timestamp": 2.0287173067687165
+        },
+        {
+            "x": 7.251660781708473,
+            "y": 4.138343233294095,
+            "heading": 4.5597032779539255e-17,
+            "angularVelocity": -2.6564828431304464e-16,
+            "velocityX": 2.802615089578594,
+            "velocityY": -0.07421855257953036,
+            "timestamp": 2.059284412118834
+        },
+        {
+            "x": 7.329540632245536,
+            "y": 4.136280827434844,
+            "heading": 3.7578034898451724e-17,
+            "angularVelocity": -2.623407674507654e-16,
+            "velocityX": 2.5478320451027607,
+            "velocityY": -0.06747141528867756,
+            "timestamp": 2.0898515174689516
+        },
+        {
+            "x": 7.399632500784112,
+            "y": 4.134424662080611,
+            "heading": 2.960093117982924e-17,
+            "angularVelocity": -2.609702038052913e-16,
+            "velocityX": 2.293048940543746,
+            "velocityY": -0.06072427640670841,
+            "timestamp": 2.120418622819069
+        },
+        {
+            "x": 7.461936386099821,
+            "y": 4.132774737263818,
+            "heading": 2.4038424741612248e-17,
+            "angularVelocity": -1.8197687815335696e-16,
+            "velocityX": 2.0382657959291963,
+            "velocityY": -0.05397713646399285,
+            "timestamp": 2.1509857281691867
+        },
+        {
+            "x": 7.516452287318104,
+            "y": 4.131331053007627,
+            "heading": 1.8665464618967105e-17,
+            "angularVelocity": -1.757758880397929e-16,
+            "velocityX": 1.7834826227035099,
+            "velocityY": -0.04722999576360025,
+            "timestamp": 2.181552833519304
+        },
+        {
+            "x": 7.5631802037830385,
+            "y": 4.130093609329407,
+            "heading": 1.3420986699374698e-17,
+            "angularVelocity": -1.7157260548142223e-16,
+            "velocityX": 1.5286994280194506,
+            "velocityY": -0.04048285449494931,
+            "timestamp": 2.2121199388694217
+        },
+        {
+            "x": 7.6021201349844665,
+            "y": 4.129062406242667,
+            "heading": 8.352476313992174e-18,
+            "angularVelocity": -1.6581584391601633e-16,
+            "velocityX": 1.2739162166455336,
+            "velocityY": -0.033735712784319354,
+            "timestamp": 2.2426870442195392
+        },
+        {
+            "x": 7.633272080514257,
+            "y": 4.128237443758217,
+            "heading": 5.582680805550875e-18,
+            "angularVelocity": -9.061360111293989e-17,
+            "velocityX": 1.019132991919723,
+            "velocityY": -0.026988570720105975,
+            "timestamp": 2.2732541495696568
+        },
+        {
+            "x": 7.656636040038488,
+            "y": 4.127618721884898,
+            "heading": 2.786250494304085e-18,
+            "angularVelocity": -9.148495626615499e-17,
+            "velocityX": 0.7643497562696309,
+            "velocityY": -0.020241428366596953,
+            "timestamp": 2.3038212549197743
+        },
+        {
+            "x": 7.672212013278888,
+            "y": 4.1272062406300805,
+            "heading": 1.8952405540316967e-19,
+            "angularVelocity": -8.495166341555795e-17,
+            "velocityX": 0.5095665115159678,
+            "velocityY": -0.013494285772008131,
+            "timestamp": 2.334388360269892
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 2.9643987227812266e-34,
-            "angularVelocity": 1.2163231774130929e-14,
-            "velocityX": 0.25515447132169783,
-            "velocityY": -0.0028871825495910644,
-            "timestamp": 1.9433247746132274
+            "heading": 6.998640646947103e-35,
+            "angularVelocity": -6.20026191774287e-18,
+            "velocityX": 0.25478325905928073,
+            "velocityY": -0.006747142973428655,
+            "timestamp": 2.3649554656200094
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": -1.7511816257564745e-34,
-            "angularVelocity": -1.536305146096975e-32,
-            "velocityX": 2.290424541792184e-34,
-            "velocityY": 7.21466542895957e-33,
-            "timestamp": 1.9739276463009061
+            "heading": 3.616659043590546e-35,
+            "angularVelocity": -1.1064131915191804e-33,
+            "velocityX": -5.781348077610764e-36,
+            "velocityY": -1.0341970128847813e-32,
+            "timestamp": 2.395522570970127
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.1.traj
new file mode 100644
index 00000000..84ac552d
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.1.traj	
@@ -0,0 +1,589 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -8.663522895809945e-37,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2925575950089905,
+            "y": 5.560958672147739,
+            "heading": -0.030620645935394094,
+            "angularVelocity": -0.4244497974010598,
+            "velocityX": 0.5899144850684857,
+            "velocityY": 0.0132886876305762,
+            "timestamp": 0.07214197326253131
+        },
+        {
+            "x": 2.3778169001331375,
+            "y": 5.562706698566846,
+            "heading": -0.08906407022187784,
+            "angularVelocity": -0.8101167966920272,
+            "velocityX": 1.181826629746885,
+            "velocityY": 0.02423036604148944,
+            "timestamp": 0.14428394652506263
+        },
+        {
+            "x": 2.5059697216292194,
+            "y": 5.56499180242334,
+            "heading": -0.17113009935557016,
+            "angularVelocity": -1.1375628558848325,
+            "velocityX": 1.7763975075885672,
+            "velocityY": 0.031675094998839956,
+            "timestamp": 0.21642591978759396
+        },
+        {
+            "x": 2.6772662600517445,
+            "y": 5.56737716078426,
+            "heading": -0.269909808145774,
+            "angularVelocity": -1.3692404618700307,
+            "velocityX": 2.3744365544197357,
+            "velocityY": 0.033064778422963216,
+            "timestamp": 0.28856789305012526
+        },
+        {
+            "x": 2.891943869686031,
+            "y": 5.568884231737924,
+            "heading": -0.3711645487758272,
+            "angularVelocity": -1.4035482542399815,
+            "velocityX": 2.975765700961003,
+            "velocityY": 0.020890348371527884,
+            "timestamp": 0.36070986631265656
+        },
+        {
+            "x": 3.1471065368375797,
+            "y": 5.565499357743455,
+            "heading": -0.4176508519135821,
+            "angularVelocity": -0.644372492676169,
+            "velocityX": 3.5369515915927057,
+            "velocityY": -0.04691962031799383,
+            "timestamp": 0.43285183957518786
+        },
+        {
+            "x": 3.4167471065440913,
+            "y": 5.523390468620082,
+            "heading": -0.41765089729124494,
+            "angularVelocity": -6.290050140591191e-7,
+            "velocityX": 3.7376378481534562,
+            "velocityY": -0.5836947233219467,
+            "timestamp": 0.5049938128377192
+        },
+        {
+            "x": 3.6595394652992312,
+            "y": 5.451715121543425,
+            "heading": -0.35516305517194063,
+            "angularVelocity": 0.8661787208384958,
+            "velocityX": 3.3654798694179893,
+            "velocityY": -0.9935318350085096,
+            "timestamp": 0.5771357861002505
+        },
+        {
+            "x": 3.865437953511339,
+            "y": 5.371729850961066,
+            "heading": -0.21394633729907842,
+            "angularVelocity": 1.957483438371744,
+            "velocityX": 2.8540734180200986,
+            "velocityY": -1.1087203047702217,
+            "timestamp": 0.6492777593627819
+        },
+        {
+            "x": 4.032804489135742,
+            "y": 5.285560607910156,
+            "heading": 0,
+            "angularVelocity": 2.9656291285588754,
+            "velocityX": 2.319960600680286,
+            "velocityY": -1.1944397852458482,
+            "timestamp": 0.7214197326253132
+        },
+        {
+            "x": 4.094074407124677,
+            "y": 5.249842876180867,
+            "heading": 0.09747269322901686,
+            "angularVelocity": 3.3393263465697696,
+            "velocityX": 2.099051997177287,
+            "velocityY": -1.2236571972325518,
+            "timestamp": 0.7506090609846522
+        },
+        {
+            "x": 4.1489483150763835,
+            "y": 5.213294373235887,
+            "heading": 0.20635910635937496,
+            "angularVelocity": 3.7303500714335676,
+            "velocityX": 1.8799304758291948,
+            "velocityY": -1.2521186679955063,
+            "timestamp": 0.7797983893439911
+        },
+        {
+            "x": 4.197569720233833,
+            "y": 5.1758877650462445,
+            "heading": 0.3277826636682362,
+            "angularVelocity": 4.159861296363549,
+            "velocityX": 1.6657253828827165,
+            "velocityY": -1.2815165778788733,
+            "timestamp": 0.8089877177033301
+        },
+        {
+            "x": 4.240207786511328,
+            "y": 5.137521274665071,
+            "heading": 0.4634475312713068,
+            "angularVelocity": 4.64775571170911,
+            "velocityX": 1.460741602293634,
+            "velocityY": -1.3144012739470166,
+            "timestamp": 0.8381770460626691
+        },
+        {
+            "x": 4.277338609770458,
+            "y": 5.097982888442082,
+            "heading": 0.6156568342246089,
+            "angularVelocity": 5.214553109256573,
+            "velocityX": 1.27206843549211,
+            "velocityY": -1.3545493659959265,
+            "timestamp": 0.867366374422008
+        },
+        {
+            "x": 4.3089076073694805,
+            "y": 5.060359750747959,
+            "heading": 0.7826775871522255,
+            "angularVelocity": 5.721979994588639,
+            "velocityX": 1.0815253167318162,
+            "velocityY": -1.2889346829415846,
+            "timestamp": 0.896555702781347
+        },
+        {
+            "x": 4.3340758913958135,
+            "y": 5.025518984665433,
+            "heading": 0.9544942987723,
+            "angularVelocity": 5.886285203444992,
+            "velocityX": 0.8622426565111364,
+            "velocityY": -1.1936131470246671,
+            "timestamp": 0.925745031140686
+        },
+        {
+            "x": 4.353180255759456,
+            "y": 4.9936006932957335,
+            "heading": 1.134787723551591,
+            "angularVelocity": 6.17668973262302,
+            "velocityX": 0.6544982511572759,
+            "velocityY": -1.0934918055244334,
+            "timestamp": 0.9549343595000249
+        },
+        {
+            "x": 4.3789611483924675,
+            "y": 4.9633684481765155,
+            "heading": 1.3088307728290547,
+            "angularVelocity": 5.962557518791945,
+            "velocityX": 0.8832300735266126,
+            "velocityY": -1.0357293853095562,
+            "timestamp": 0.9841236878593639
+        },
+        {
+            "x": 4.410526959907193,
+            "y": 4.931127063214471,
+            "heading": 1.468917613249843,
+            "angularVelocity": 5.4844304209407735,
+            "velocityX": 1.0814161643642388,
+            "velocityY": -1.104560699894603,
+            "timestamp": 1.013313016218703
+        },
+        {
+            "x": 4.4435007518243745,
+            "y": 4.892801223101424,
+            "heading": 1.6159090393580717,
+            "angularVelocity": 5.035793365940867,
+            "velocityX": 1.1296523000205303,
+            "velocityY": -1.3130086325122463,
+            "timestamp": 1.042502344578042
+        },
+        {
+            "x": 4.478070868346437,
+            "y": 4.848243847113431,
+            "heading": 1.7513264495243417,
+            "angularVelocity": 4.639278043646516,
+            "velocityX": 1.1843409377729661,
+            "velocityY": -1.5264954177589778,
+            "timestamp": 1.0716916729373809
+        },
+        {
+            "x": 4.514676965101799,
+            "y": 4.797506620196597,
+            "heading": 1.8758559761070788,
+            "angularVelocity": 4.266268995630874,
+            "velocityX": 1.2540917798696012,
+            "velocityY": -1.738211523479656,
+            "timestamp": 1.1008810012967198
+        },
+        {
+            "x": 4.554041400948399,
+            "y": 4.740897535447995,
+            "heading": 1.989313365398794,
+            "angularVelocity": 3.886947582177413,
+            "velocityX": 1.3485899833664619,
+            "velocityY": -1.939376064146053,
+            "timestamp": 1.1300703296560588
+        },
+        {
+            "x": 4.597166214346246,
+            "y": 4.6791223056158096,
+            "heading": 2.090829457044177,
+            "angularVelocity": 3.477849520744562,
+            "velocityX": 1.4774171185768328,
+            "velocityY": -2.1163635240829413,
+            "timestamp": 1.1592596580153978
+        },
+        {
+            "x": 4.645233022395554,
+            "y": 4.613520579676299,
+            "heading": 2.179525396238879,
+            "angularVelocity": 3.0386426882728963,
+            "velocityX": 1.646725387359914,
+            "velocityY": -2.2474558212478137,
+            "timestamp": 1.1884489863747367
+        },
+        {
+            "x": 4.699569055464054,
+            "y": 4.547167372776357,
+            "heading": 2.256949250765549,
+            "angularVelocity": 2.6524712584521715,
+            "velocityX": 1.8615033686143418,
+            "velocityY": -2.2732008795506418,
+            "timestamp": 1.2176383147340757
+        },
+        {
+            "x": 4.760368188580363,
+            "y": 4.482555153301175,
+            "heading": 2.3258054605639615,
+            "angularVelocity": 2.358951495928562,
+            "velocityX": 2.082923333069965,
+            "velocityY": -2.213556224376411,
+            "timestamp": 1.2468276430934147
+        },
+        {
+            "x": 4.827470532546925,
+            "y": 4.4190609396421525,
+            "heading": 2.3838171568684117,
+            "angularVelocity": 1.9874282679714124,
+            "velocityX": 2.2988656381705637,
+            "velocityY": -2.175254355885453,
+            "timestamp": 1.2760169714527536
+        },
+        {
+            "x": 4.899988963730756,
+            "y": 4.356794264470432,
+            "heading": 2.432111827676766,
+            "angularVelocity": 1.6545317594778726,
+            "velocityX": 2.484415889639006,
+            "velocityY": -2.1331999971078073,
+            "timestamp": 1.3052062998120926
+        },
+        {
+            "x": 4.972562290971905,
+            "y": 4.300811163566705,
+            "heading": 2.4919687455960093,
+            "angularVelocity": 2.050643892259759,
+            "velocityX": 2.486296578932036,
+            "velocityY": -1.9179304235623322,
+            "timestamp": 1.3343956281714315
+        },
+        {
+            "x": 5.045616975616562,
+            "y": 4.25136299402395,
+            "heading": 2.5615851998228103,
+            "angularVelocity": 2.384996782720671,
+            "velocityX": 2.5027874483889563,
+            "velocityY": -1.6940495832592366,
+            "timestamp": 1.3635849565307705
+        },
+        {
+            "x": 5.124307057757338,
+            "y": 4.205650619988245,
+            "heading": 2.6233534867290937,
+            "angularVelocity": 2.116125665718561,
+            "velocityX": 2.6958510717359463,
+            "velocityY": -1.5660646066588564,
+            "timestamp": 1.3927742848901095
+        },
+        {
+            "x": 5.208588203701103,
+            "y": 4.163420547805767,
+            "heading": 2.6756025601758724,
+            "angularVelocity": 1.7900060187599918,
+            "velocityX": 2.8873958628376504,
+            "velocityY": -1.4467640934590689,
+            "timestamp": 1.4219636132494484
+        },
+        {
+            "x": 5.297563997608419,
+            "y": 4.1261005954207315,
+            "heading": 2.720276777571372,
+            "angularVelocity": 1.5304982987457612,
+            "velocityX": 3.048230257714994,
+            "velocityY": -1.2785478283571272,
+            "timestamp": 1.4511529416087874
+        },
+        {
+            "x": 5.390789031982422,
+            "y": 4.09412145614624,
+            "heading": 2.757748451080741,
+            "angularVelocity": 1.2837456569082273,
+            "velocityX": 3.1938053944353855,
+            "velocityY": -1.0955763997309995,
+            "timestamp": 1.4803422699681263
+        },
+        {
+            "x": 5.532416911390752,
+            "y": 4.059853570653742,
+            "heading": 2.796595493071039,
+            "angularVelocity": 0.9277997104504593,
+            "velocityX": 3.3825562713263437,
+            "velocityY": -0.818433852585974,
+            "timestamp": 1.522212343382395
+        },
+        {
+            "x": 5.681515537517707,
+            "y": 4.021534249773671,
+            "heading": 2.782742222711128,
+            "angularVelocity": -0.3308632928068908,
+            "velocityX": 3.5609831550031092,
+            "velocityY": -0.9151959324488058,
+            "timestamp": 1.5640824167966638
+        },
+        {
+            "x": 5.826340094588043,
+            "y": 3.969241008458542,
+            "heading": 2.7680381624736805,
+            "angularVelocity": -0.351183053632681,
+            "velocityX": 3.4589038246343846,
+            "velocityY": -1.2489407600921083,
+            "timestamp": 1.6059524902109326
+        },
+        {
+            "x": 5.965226819905552,
+            "y": 3.9129528098786466,
+            "heading": 2.7396888789742415,
+            "angularVelocity": -0.6770774729470177,
+            "velocityX": 3.317088172818383,
+            "velocityY": -1.3443539499673571,
+            "timestamp": 1.6478225636252013
+        },
+        {
+            "x": 6.097371617524639,
+            "y": 3.854212614126153,
+            "heading": 2.694967938521201,
+            "angularVelocity": -1.0680884174853156,
+            "velocityX": 3.1560679703526437,
+            "velocityY": -1.402915996141418,
+            "timestamp": 1.68969263703947
+        },
+        {
+            "x": 6.217931948857121,
+            "y": 3.789923024350862,
+            "heading": 2.6256097109731904,
+            "angularVelocity": -1.6565107699184056,
+            "velocityX": 2.8793914483893763,
+            "velocityY": -1.535454431598441,
+            "timestamp": 1.7315627104537388
+        },
+        {
+            "x": 6.32897922975643,
+            "y": 3.7148340577600942,
+            "heading": 2.547026684843302,
+            "angularVelocity": -1.8768303879569581,
+            "velocityX": 2.652187394098668,
+            "velocityY": -1.7933803422751633,
+            "timestamp": 1.7734327838680075
+        },
+        {
+            "x": 6.440862810984381,
+            "y": 3.629355422988249,
+            "heading": 2.48919642870263,
+            "angularVelocity": -1.381183538144076,
+            "velocityX": 2.672161095132505,
+            "velocityY": -2.0415210149288825,
+            "timestamp": 1.8153028572822762
+        },
+        {
+            "x": 6.556707402178449,
+            "y": 3.5350910926988375,
+            "heading": 2.4587564932761583,
+            "angularVelocity": -0.7270093635923316,
+            "velocityX": 2.766763507861174,
+            "velocityY": -2.2513533558144507,
+            "timestamp": 1.857172930696545
+        },
+        {
+            "x": 6.674645090324846,
+            "y": 3.429370276775565,
+            "heading": 2.4587373064570563,
+            "angularVelocity": -0.00045824660759140524,
+            "velocityX": 2.816753794040547,
+            "velocityY": -2.5249732637737297,
+            "timestamp": 1.8990430041108137
+        },
+        {
+            "x": 6.791280321048844,
+            "y": 3.3222067883695963,
+            "heading": 2.45873486083509,
+            "angularVelocity": -0.00005840978453659185,
+            "velocityX": 2.785646673460325,
+            "velocityY": -2.559429197691564,
+            "timestamp": 1.9409130775250825
+        },
+        {
+            "x": 6.907916035753451,
+            "y": 3.2150438274589583,
+            "heading": 2.4587324135695865,
+            "angularVelocity": -0.000058449037785704465,
+            "velocityX": 2.78565823256613,
+            "velocityY": -2.5594165993059286,
+            "timestamp": 1.9827831509393512
+        },
+        {
+            "x": 7.020962902689577,
+            "y": 3.1172887235192603,
+            "heading": 2.428615335233058,
+            "angularVelocity": -0.7192984363448716,
+            "velocityX": 2.6999443210338705,
+            "velocityY": -2.334724923276202,
+            "timestamp": 2.02465322435362
+        },
+        {
+            "x": 7.121900964071713,
+            "y": 3.0192154080188542,
+            "heading": 2.3705886859446603,
+            "angularVelocity": -1.385874075602246,
+            "velocityX": 2.4107447909976107,
+            "velocityY": -2.3423248994587165,
+            "timestamp": 2.0665232977678887
+        },
+        {
+            "x": 7.210190441218643,
+            "y": 2.92089200334631,
+            "heading": 2.287196223783446,
+            "angularVelocity": -1.9916961056197946,
+            "velocityX": 2.10865350708562,
+            "velocityY": -2.348297880916461,
+            "timestamp": 2.1083933711821574
+        },
+        {
+            "x": 7.2890247338542125,
+            "y": 2.830773686809754,
+            "heading": 2.1745743754916034,
+            "angularVelocity": -2.689793427815253,
+            "velocityX": 1.8828314881507633,
+            "velocityY": -2.1523324223703058,
+            "timestamp": 2.150263444596426
+        },
+        {
+            "x": 7.359953440317656,
+            "y": 2.7509272688501483,
+            "heading": 2.035955336818745,
+            "angularVelocity": -3.3106949037643396,
+            "velocityX": 1.6940191568728353,
+            "velocityY": -1.9070044890916185,
+            "timestamp": 2.192133518010695
+        },
+        {
+            "x": 7.4240572342258595,
+            "y": 2.6822786221440054,
+            "heading": 1.872800890040008,
+            "angularVelocity": -3.896684038847076,
+            "velocityX": 1.5310169933057254,
+            "velocityY": -1.6395635619484732,
+            "timestamp": 2.2340035914249636
+        },
+        {
+            "x": 7.482217323317858,
+            "y": 2.6251237224270416,
+            "heading": 1.684191854325982,
+            "angularVelocity": -4.504626343687046,
+            "velocityX": 1.3890610727273844,
+            "velocityY": -1.3650537258787387,
+            "timestamp": 2.2758736648392324
+        },
+        {
+            "x": 7.535468730024687,
+            "y": 2.5792763511542147,
+            "heading": 1.466995470236355,
+            "angularVelocity": -5.187389616938403,
+            "velocityX": 1.2718250140130243,
+            "velocityY": -1.0949914230912903,
+            "timestamp": 2.317743738253501
+        },
+        {
+            "x": 7.580274182888533,
+            "y": 2.5411410245640176,
+            "heading": 1.2146613001768354,
+            "angularVelocity": -6.02659965658257,
+            "velocityX": 1.0701068617801175,
+            "velocityY": -0.91080152195771,
+            "timestamp": 2.35961381166777
+        },
+        {
+            "x": 7.616169603666072,
+            "y": 2.509680561991982,
+            "heading": 0.922946754094809,
+            "angularVelocity": -6.967137200734248,
+            "velocityX": 0.8573049400316086,
+            "velocityY": -0.7513830286553573,
+            "timestamp": 2.4014838850820386
+        },
+        {
+            "x": 7.640414148022587,
+            "y": 2.4865073161659272,
+            "heading": 0.6422191973798425,
+            "angularVelocity": -6.704730463149791,
+            "velocityX": 0.5790423178062244,
+            "velocityY": -0.5534560590991822,
+            "timestamp": 2.4433539584963073
+        },
+        {
+            "x": 7.656220323606262,
+            "y": 2.4718614980447855,
+            "heading": 0.3932969564478367,
+            "angularVelocity": -5.9451111649394885,
+            "velocityX": 0.37750532288984423,
+            "velocityY": -0.34979203347062976,
+            "timestamp": 2.485224031910576
+        },
+        {
+            "x": 7.668143711832241,
+            "y": 2.4642737767821443,
+            "heading": 0.1972336516813064,
+            "angularVelocity": -4.682659684559189,
+            "velocityX": 0.28477113254632147,
+            "velocityY": -0.18122063430765137,
+            "timestamp": 2.527094105324845
+        },
+        {
+            "x": 7.676158456210791,
+            "y": 2.459440783193878,
+            "heading": 0.06602780599823356,
+            "angularVelocity": -3.1336425992116688,
+            "velocityX": 0.19141940113768102,
+            "velocityY": -0.11542835238065828,
+            "timestamp": 2.5689641787391135
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 1.4385689598890085e-34,
+            "angularVelocity": -1.576968956919288,
+            "velocityX": 0.09174915341561693,
+            "velocityY": -0.05829421815741107,
+            "timestamp": 2.6108342521533823
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 5.954582727394106e-35,
+            "angularVelocity": -1.9052580558070114e-33,
+            "velocityX": -2.275662344047759e-34,
+            "velocityY": 3.071010312134925e-34,
+            "timestamp": 2.652704325567651
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.traj
index 5f6c2964..84ac552d 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.traj	
@@ -6,584 +6,584 @@
             "heading": 0,
             "angularVelocity": 0,
             "velocityX": 0,
-            "velocityY": 0,
+            "velocityY": -8.663522895809945e-37,
             "timestamp": 0
         },
         {
-            "x": 2.253608767824088,
-            "y": 5.559959646221142,
-            "heading": 0.04333845773860565,
-            "angularVelocity": 1.2657316926377489,
-            "velocityX": 0.10539673178657698,
-            "velocityY": -0.0011785619397610312,
-            "timestamp": 0.03423984560921402
-        },
-        {
-            "x": 2.260842860117559,
-            "y": 5.560038303784881,
-            "heading": 0.12991520296053788,
-            "angularVelocity": 2.5285378389274697,
-            "velocityX": 0.211277012637124,
-            "velocityY": 0.002297252290136546,
-            "timestamp": 0.06847969121842803
-        },
-        {
-            "x": 2.271629728646886,
-            "y": 5.560441749821537,
-            "heading": 0.2597959154554935,
-            "angularVelocity": 3.7932622120236568,
-            "velocityX": 0.3150384687021929,
-            "velocityY": 0.011782939714764799,
-            "timestamp": 0.10271953682764205
-        },
-        {
-            "x": 2.285721166472015,
-            "y": 5.561272024294721,
-            "heading": 0.433416913958427,
-            "angularVelocity": 5.070729596286839,
-            "velocityX": 0.41155085761651194,
-            "velocityY": 0.024248779701277118,
-            "timestamp": 0.13695938243685607
-        },
-        {
-            "x": 2.302746549134424,
-            "y": 5.561830584813036,
-            "heading": 0.6513804515506975,
-            "angularVelocity": 6.36578622695705,
-            "velocityX": 0.497238885266109,
-            "velocityY": 0.01631317280720512,
-            "timestamp": 0.17119922804607007
-        },
-        {
-            "x": 2.325552284167229,
-            "y": 5.559809655953846,
-            "heading": 0.9008347442106217,
-            "angularVelocity": 7.285497005652254,
-            "velocityX": 0.6660583488924382,
-            "velocityY": -0.05902272113767896,
-            "timestamp": 0.20543907365528408
-        },
-        {
-            "x": 2.357593597831257,
-            "y": 5.5547755319598515,
-            "heading": 1.1463891087836293,
-            "angularVelocity": 7.171596723173552,
-            "velocityX": 0.9357902494573596,
-            "velocityY": -0.14702531230571617,
-            "timestamp": 0.23967891926449808
-        },
-        {
-            "x": 2.398826095012215,
-            "y": 5.54977146589545,
-            "heading": 1.3795454342192353,
-            "angularVelocity": 6.809502825937482,
-            "velocityX": 1.2042255578939407,
-            "velocityY": -0.14614744825411982,
-            "timestamp": 0.2739187648737121
-        },
-        {
-            "x": 2.4467424620473803,
-            "y": 5.551292722520974,
-            "heading": 1.6025344347532522,
-            "angularVelocity": 6.512558586829906,
-            "velocityX": 1.399432917485785,
-            "velocityY": 0.04442942421196648,
-            "timestamp": 0.3081586104829261
-        },
-        {
-            "x": 2.499282041125752,
-            "y": 5.560003512593039,
-            "heading": 1.8091707816342142,
-            "angularVelocity": 6.034967249541443,
-            "velocityX": 1.5344572425359657,
-            "velocityY": 0.2544050627880495,
-            "timestamp": 0.3423984560921401
-        },
-        {
-            "x": 2.558927262082263,
-            "y": 5.571143049666431,
-            "heading": 1.9899644077603311,
-            "angularVelocity": 5.2802114877955235,
-            "velocityX": 1.7419827658469296,
-            "velocityY": 0.3253384142127747,
-            "timestamp": 0.3766383017013541
-        },
-        {
-            "x": 2.624771333399717,
-            "y": 5.5772713811756995,
-            "heading": 2.148730759711285,
-            "angularVelocity": 4.63688866366352,
-            "velocityX": 1.9230247726273466,
-            "velocityY": 0.17898245159199333,
-            "timestamp": 0.4108781473105681
-        },
-        {
-            "x": 2.6962684360037885,
-            "y": 5.576686266483743,
-            "heading": 2.2908744479705683,
-            "angularVelocity": 4.151411483614645,
-            "velocityX": 2.0881257298902987,
-            "velocityY": -0.0170887070763856,
-            "timestamp": 0.4451179929197821
-        },
-        {
-            "x": 2.773687793639176,
-            "y": 5.569334398339137,
-            "heading": 2.4184902740666434,
-            "angularVelocity": 3.727114530613817,
-            "velocityX": 2.2610895656186547,
-            "velocityY": -0.2147167434256298,
-            "timestamp": 0.4793578385289961
-        },
-        {
-            "x": 2.8582271385194487,
-            "y": 5.556743437974915,
-            "heading": 2.5300657438997165,
-            "angularVelocity": 3.258644069441941,
-            "velocityX": 2.4690340559690878,
-            "velocityY": -0.36772830426646747,
-            "timestamp": 0.5135976841382102
-        },
-        {
-            "x": 2.9508927177061977,
-            "y": 5.541492370817128,
-            "heading": 2.6233336392816216,
-            "angularVelocity": 2.7239578252305745,
-            "velocityX": 2.706366735538439,
-            "velocityY": -0.445418689437171,
-            "timestamp": 0.5478375297474242
-        },
-        {
-            "x": 3.0518533392296137,
-            "y": 5.525810656425357,
-            "heading": 2.697205719709221,
-            "angularVelocity": 2.1574887127329996,
-            "velocityX": 2.948629578406965,
-            "velocityY": -0.45799606022611716,
-            "timestamp": 0.5820773753566382
-        },
-        {
-            "x": 3.1608544462331407,
-            "y": 5.511189976920693,
-            "heading": 2.751249379299885,
-            "angularVelocity": 1.5783850256649679,
-            "velocityX": 3.183457900119567,
-            "velocityY": -0.42700775206563796,
-            "timestamp": 0.6163172209658522
-        },
-        {
-            "x": 3.2775207255393237,
-            "y": 5.498694402974223,
-            "heading": 2.785399702937351,
-            "angularVelocity": 0.9973854446433661,
-            "velocityX": 3.40732492306531,
-            "velocityY": -0.36494247342945346,
-            "timestamp": 0.6505570665750662
-        },
-        {
-            "x": 3.401135401206032,
-            "y": 5.488837187261409,
-            "heading": 2.800896234569334,
-            "angularVelocity": 0.4525876608454414,
-            "velocityX": 3.610257974803583,
-            "velocityY": -0.28788727102673717,
-            "timestamp": 0.6847969121842802
-        },
-        {
-            "x": 3.5179017921083777,
-            "y": 5.474779629986763,
-            "heading": 2.835487088185811,
-            "angularVelocity": 1.0102514483058458,
-            "velocityX": 3.410248756230483,
-            "velocityY": -0.4105613510962388,
-            "timestamp": 0.7190367577934942
-        },
-        {
-            "x": 3.6287098119916026,
-            "y": 5.4546983370085105,
-            "heading": 2.8876881760682602,
-            "angularVelocity": 1.5245713569573334,
-            "velocityX": 3.2362301263825226,
-            "velocityY": -0.5864890048700768,
-            "timestamp": 0.7532766034027082
-        },
-        {
-            "x": 3.7356372636451365,
-            "y": 5.425785607175325,
-            "heading": 2.9455736884009496,
-            "angularVelocity": 1.6905891747687094,
-            "velocityX": 3.1228952628442794,
-            "velocityY": -0.8444176461299453,
-            "timestamp": 0.7875164490119222
-        },
-        {
-            "x": 3.8388362586877394,
-            "y": 5.387864157402447,
-            "heading": 3.0009390226743027,
-            "angularVelocity": 1.6169855117119507,
-            "velocityX": 3.0140029315678847,
-            "velocityY": -1.1075239709221774,
-            "timestamp": 0.8217562946211362
-        },
-        {
-            "x": 3.9379980862804507,
-            "y": 5.341070528950818,
-            "heading": 3.053673275383994,
-            "angularVelocity": 1.5401428298350917,
-            "velocityX": 2.896094472050628,
-            "velocityY": -1.3666425072616961,
-            "timestamp": 0.8559961402303502
+            "x": 2.2925575950089905,
+            "y": 5.560958672147739,
+            "heading": -0.030620645935394094,
+            "angularVelocity": -0.4244497974010598,
+            "velocityX": 0.5899144850684857,
+            "velocityY": 0.0132886876305762,
+            "timestamp": 0.07214197326253131
+        },
+        {
+            "x": 2.3778169001331375,
+            "y": 5.562706698566846,
+            "heading": -0.08906407022187784,
+            "angularVelocity": -0.8101167966920272,
+            "velocityX": 1.181826629746885,
+            "velocityY": 0.02423036604148944,
+            "timestamp": 0.14428394652506263
+        },
+        {
+            "x": 2.5059697216292194,
+            "y": 5.56499180242334,
+            "heading": -0.17113009935557016,
+            "angularVelocity": -1.1375628558848325,
+            "velocityX": 1.7763975075885672,
+            "velocityY": 0.031675094998839956,
+            "timestamp": 0.21642591978759396
+        },
+        {
+            "x": 2.6772662600517445,
+            "y": 5.56737716078426,
+            "heading": -0.269909808145774,
+            "angularVelocity": -1.3692404618700307,
+            "velocityX": 2.3744365544197357,
+            "velocityY": 0.033064778422963216,
+            "timestamp": 0.28856789305012526
+        },
+        {
+            "x": 2.891943869686031,
+            "y": 5.568884231737924,
+            "heading": -0.3711645487758272,
+            "angularVelocity": -1.4035482542399815,
+            "velocityX": 2.975765700961003,
+            "velocityY": 0.020890348371527884,
+            "timestamp": 0.36070986631265656
+        },
+        {
+            "x": 3.1471065368375797,
+            "y": 5.565499357743455,
+            "heading": -0.4176508519135821,
+            "angularVelocity": -0.644372492676169,
+            "velocityX": 3.5369515915927057,
+            "velocityY": -0.04691962031799383,
+            "timestamp": 0.43285183957518786
+        },
+        {
+            "x": 3.4167471065440913,
+            "y": 5.523390468620082,
+            "heading": -0.41765089729124494,
+            "angularVelocity": -6.290050140591191e-7,
+            "velocityX": 3.7376378481534562,
+            "velocityY": -0.5836947233219467,
+            "timestamp": 0.5049938128377192
+        },
+        {
+            "x": 3.6595394652992312,
+            "y": 5.451715121543425,
+            "heading": -0.35516305517194063,
+            "angularVelocity": 0.8661787208384958,
+            "velocityX": 3.3654798694179893,
+            "velocityY": -0.9935318350085096,
+            "timestamp": 0.5771357861002505
+        },
+        {
+            "x": 3.865437953511339,
+            "y": 5.371729850961066,
+            "heading": -0.21394633729907842,
+            "angularVelocity": 1.957483438371744,
+            "velocityX": 2.8540734180200986,
+            "velocityY": -1.1087203047702217,
+            "timestamp": 0.6492777593627819
         },
         {
             "x": 4.032804489135742,
             "y": 5.285560607910156,
-            "heading": 3.1036105469083837,
-            "angularVelocity": 1.4584549268806075,
-            "velocityX": 2.76889107320562,
-            "velocityY": -1.62120827512502,
-            "timestamp": 0.8902359858395642
-        },
-        {
-            "x": 4.173725003261308,
-            "y": 5.173637687034127,
-            "heading": 3.175484729095798,
-            "angularVelocity": 1.2994086167535415,
-            "velocityX": 2.547692714675012,
-            "velocityY": -2.023447131813128,
-            "timestamp": 0.9455489807364162
-        },
-        {
-            "x": 4.301872286760783,
-            "y": 5.039921697201636,
-            "heading": 3.2346580028937466,
-            "angularVelocity": 1.0697897285853943,
-            "velocityX": 2.3167663175433564,
-            "velocityY": -2.4174425934058767,
-            "timestamp": 1.0008619756332682
-        },
-        {
-            "x": 4.416442024233276,
-            "y": 4.885133369968262,
-            "heading": 3.2770092965554323,
-            "angularVelocity": 0.765666255111711,
-            "velocityX": 2.0712987551323803,
-            "velocityY": -2.7984079965661373,
-            "timestamp": 1.0561749705301202
-        },
-        {
-            "x": 4.517908742039174,
-            "y": 4.711097539386313,
-            "heading": 3.282986865137276,
-            "angularVelocity": 0.10806806959180061,
-            "velocityX": 1.834410123608644,
-            "velocityY": -3.146382344808711,
-            "timestamp": 1.1114879654269723
-        },
-        {
-            "x": 4.627451956204509,
-            "y": 4.5560902871099245,
-            "heading": 3.2338916761611958,
-            "angularVelocity": -0.8875886953442504,
-            "velocityX": 1.9804245705663106,
-            "velocityY": -2.802365928032756,
-            "timestamp": 1.1668009603238243
-        },
-        {
-            "x": 4.751713919325069,
-            "y": 4.421499213962681,
-            "heading": 3.169974564154514,
-            "angularVelocity": -1.1555532678328913,
-            "velocityX": 2.246523865726009,
-            "velocityY": -2.433263167149585,
-            "timestamp": 1.2221139552206763
-        },
-        {
-            "x": 4.891049798654671,
-            "y": 4.307366204928411,
-            "heading": 3.098048256171184,
-            "angularVelocity": -1.3003509955925967,
-            "velocityX": 2.5190442063286667,
-            "velocityY": -2.063403170396147,
-            "timestamp": 1.2774269501175284
-        },
-        {
-            "x": 5.0446094980944505,
-            "y": 4.214363163694223,
-            "heading": 3.021068837736617,
-            "angularVelocity": -1.3917058473893738,
-            "velocityX": 2.7761957154216415,
-            "velocityY": -1.6813958710357464,
-            "timestamp": 1.3327399450143804
-        },
-        {
-            "x": 5.211468562298102,
-            "y": 4.143120621239898,
-            "heading": 2.9418200950482523,
-            "angularVelocity": -1.432732811451417,
-            "velocityX": 3.016634057056783,
-            "velocityY": -1.2879892435254758,
-            "timestamp": 1.3880529399112325
+            "heading": 0,
+            "angularVelocity": 2.9656291285588754,
+            "velocityX": 2.319960600680286,
+            "velocityY": -1.1944397852458482,
+            "timestamp": 0.7214197326253132
+        },
+        {
+            "x": 4.094074407124677,
+            "y": 5.249842876180867,
+            "heading": 0.09747269322901686,
+            "angularVelocity": 3.3393263465697696,
+            "velocityX": 2.099051997177287,
+            "velocityY": -1.2236571972325518,
+            "timestamp": 0.7506090609846522
+        },
+        {
+            "x": 4.1489483150763835,
+            "y": 5.213294373235887,
+            "heading": 0.20635910635937496,
+            "angularVelocity": 3.7303500714335676,
+            "velocityX": 1.8799304758291948,
+            "velocityY": -1.2521186679955063,
+            "timestamp": 0.7797983893439911
+        },
+        {
+            "x": 4.197569720233833,
+            "y": 5.1758877650462445,
+            "heading": 0.3277826636682362,
+            "angularVelocity": 4.159861296363549,
+            "velocityX": 1.6657253828827165,
+            "velocityY": -1.2815165778788733,
+            "timestamp": 0.8089877177033301
+        },
+        {
+            "x": 4.240207786511328,
+            "y": 5.137521274665071,
+            "heading": 0.4634475312713068,
+            "angularVelocity": 4.64775571170911,
+            "velocityX": 1.460741602293634,
+            "velocityY": -1.3144012739470166,
+            "timestamp": 0.8381770460626691
+        },
+        {
+            "x": 4.277338609770458,
+            "y": 5.097982888442082,
+            "heading": 0.6156568342246089,
+            "angularVelocity": 5.214553109256573,
+            "velocityX": 1.27206843549211,
+            "velocityY": -1.3545493659959265,
+            "timestamp": 0.867366374422008
+        },
+        {
+            "x": 4.3089076073694805,
+            "y": 5.060359750747959,
+            "heading": 0.7826775871522255,
+            "angularVelocity": 5.721979994588639,
+            "velocityX": 1.0815253167318162,
+            "velocityY": -1.2889346829415846,
+            "timestamp": 0.896555702781347
+        },
+        {
+            "x": 4.3340758913958135,
+            "y": 5.025518984665433,
+            "heading": 0.9544942987723,
+            "angularVelocity": 5.886285203444992,
+            "velocityX": 0.8622426565111364,
+            "velocityY": -1.1936131470246671,
+            "timestamp": 0.925745031140686
+        },
+        {
+            "x": 4.353180255759456,
+            "y": 4.9936006932957335,
+            "heading": 1.134787723551591,
+            "angularVelocity": 6.17668973262302,
+            "velocityX": 0.6544982511572759,
+            "velocityY": -1.0934918055244334,
+            "timestamp": 0.9549343595000249
+        },
+        {
+            "x": 4.3789611483924675,
+            "y": 4.9633684481765155,
+            "heading": 1.3088307728290547,
+            "angularVelocity": 5.962557518791945,
+            "velocityX": 0.8832300735266126,
+            "velocityY": -1.0357293853095562,
+            "timestamp": 0.9841236878593639
+        },
+        {
+            "x": 4.410526959907193,
+            "y": 4.931127063214471,
+            "heading": 1.468917613249843,
+            "angularVelocity": 5.4844304209407735,
+            "velocityX": 1.0814161643642388,
+            "velocityY": -1.104560699894603,
+            "timestamp": 1.013313016218703
+        },
+        {
+            "x": 4.4435007518243745,
+            "y": 4.892801223101424,
+            "heading": 1.6159090393580717,
+            "angularVelocity": 5.035793365940867,
+            "velocityX": 1.1296523000205303,
+            "velocityY": -1.3130086325122463,
+            "timestamp": 1.042502344578042
+        },
+        {
+            "x": 4.478070868346437,
+            "y": 4.848243847113431,
+            "heading": 1.7513264495243417,
+            "angularVelocity": 4.639278043646516,
+            "velocityX": 1.1843409377729661,
+            "velocityY": -1.5264954177589778,
+            "timestamp": 1.0716916729373809
+        },
+        {
+            "x": 4.514676965101799,
+            "y": 4.797506620196597,
+            "heading": 1.8758559761070788,
+            "angularVelocity": 4.266268995630874,
+            "velocityX": 1.2540917798696012,
+            "velocityY": -1.738211523479656,
+            "timestamp": 1.1008810012967198
+        },
+        {
+            "x": 4.554041400948399,
+            "y": 4.740897535447995,
+            "heading": 1.989313365398794,
+            "angularVelocity": 3.886947582177413,
+            "velocityX": 1.3485899833664619,
+            "velocityY": -1.939376064146053,
+            "timestamp": 1.1300703296560588
+        },
+        {
+            "x": 4.597166214346246,
+            "y": 4.6791223056158096,
+            "heading": 2.090829457044177,
+            "angularVelocity": 3.477849520744562,
+            "velocityX": 1.4774171185768328,
+            "velocityY": -2.1163635240829413,
+            "timestamp": 1.1592596580153978
+        },
+        {
+            "x": 4.645233022395554,
+            "y": 4.613520579676299,
+            "heading": 2.179525396238879,
+            "angularVelocity": 3.0386426882728963,
+            "velocityX": 1.646725387359914,
+            "velocityY": -2.2474558212478137,
+            "timestamp": 1.1884489863747367
+        },
+        {
+            "x": 4.699569055464054,
+            "y": 4.547167372776357,
+            "heading": 2.256949250765549,
+            "angularVelocity": 2.6524712584521715,
+            "velocityX": 1.8615033686143418,
+            "velocityY": -2.2732008795506418,
+            "timestamp": 1.2176383147340757
+        },
+        {
+            "x": 4.760368188580363,
+            "y": 4.482555153301175,
+            "heading": 2.3258054605639615,
+            "angularVelocity": 2.358951495928562,
+            "velocityX": 2.082923333069965,
+            "velocityY": -2.213556224376411,
+            "timestamp": 1.2468276430934147
+        },
+        {
+            "x": 4.827470532546925,
+            "y": 4.4190609396421525,
+            "heading": 2.3838171568684117,
+            "angularVelocity": 1.9874282679714124,
+            "velocityX": 2.2988656381705637,
+            "velocityY": -2.175254355885453,
+            "timestamp": 1.2760169714527536
+        },
+        {
+            "x": 4.899988963730756,
+            "y": 4.356794264470432,
+            "heading": 2.432111827676766,
+            "angularVelocity": 1.6545317594778726,
+            "velocityX": 2.484415889639006,
+            "velocityY": -2.1331999971078073,
+            "timestamp": 1.3052062998120926
+        },
+        {
+            "x": 4.972562290971905,
+            "y": 4.300811163566705,
+            "heading": 2.4919687455960093,
+            "angularVelocity": 2.050643892259759,
+            "velocityX": 2.486296578932036,
+            "velocityY": -1.9179304235623322,
+            "timestamp": 1.3343956281714315
+        },
+        {
+            "x": 5.045616975616562,
+            "y": 4.25136299402395,
+            "heading": 2.5615851998228103,
+            "angularVelocity": 2.384996782720671,
+            "velocityX": 2.5027874483889563,
+            "velocityY": -1.6940495832592366,
+            "timestamp": 1.3635849565307705
+        },
+        {
+            "x": 5.124307057757338,
+            "y": 4.205650619988245,
+            "heading": 2.6233534867290937,
+            "angularVelocity": 2.116125665718561,
+            "velocityX": 2.6958510717359463,
+            "velocityY": -1.5660646066588564,
+            "timestamp": 1.3927742848901095
+        },
+        {
+            "x": 5.208588203701103,
+            "y": 4.163420547805767,
+            "heading": 2.6756025601758724,
+            "angularVelocity": 1.7900060187599918,
+            "velocityX": 2.8873958628376504,
+            "velocityY": -1.4467640934590689,
+            "timestamp": 1.4219636132494484
+        },
+        {
+            "x": 5.297563997608419,
+            "y": 4.1261005954207315,
+            "heading": 2.720276777571372,
+            "angularVelocity": 1.5304982987457612,
+            "velocityX": 3.048230257714994,
+            "velocityY": -1.2785478283571272,
+            "timestamp": 1.4511529416087874
         },
         {
             "x": 5.390789031982422,
             "y": 4.09412145614624,
-            "heading": 2.865640793449113,
-            "angularVelocity": -1.3772405876991196,
-            "velocityX": 3.2419229878750393,
-            "velocityY": -0.8858526858838836,
-            "timestamp": 1.4433659348080845
-        },
-        {
-            "x": 5.535592658236132,
-            "y": 4.049337172847784,
-            "heading": 2.841642220937184,
-            "angularVelocity": -0.5722130304709164,
-            "velocityX": 3.452643767066762,
-            "velocityY": -1.0678197818199107,
-            "timestamp": 1.4853058614332775
-        },
-        {
-            "x": 5.681734895850587,
-            "y": 4.003264514213434,
-            "heading": 2.8233979117656802,
-            "angularVelocity": -0.4350105171749157,
-            "velocityX": 3.484561118108144,
-            "velocityY": -1.098539323783022,
-            "timestamp": 1.5272457880584704
-        },
-        {
-            "x": 5.827742551539107,
-            "y": 3.9572785916007636,
-            "heading": 2.804954082691072,
-            "angularVelocity": -0.4397677954812897,
-            "velocityX": 3.481352196758831,
-            "velocityY": -1.096471222366128,
-            "timestamp": 1.5691857146836634
-        },
-        {
-            "x": 5.9679573209760495,
-            "y": 3.908501280249766,
-            "heading": 2.7717324952229117,
-            "angularVelocity": -0.7921231661908814,
-            "velocityX": 3.3432287731451744,
-            "velocityY": -1.1630280564605782,
-            "timestamp": 1.6111256413088564
-        },
-        {
-            "x": 6.096940014064459,
-            "y": 3.8543767396001813,
-            "heading": 2.711734513417447,
-            "angularVelocity": -1.4305695463335533,
-            "velocityX": 3.07541532537948,
-            "velocityY": -1.2905254015650245,
-            "timestamp": 1.6530655679340494
-        },
-        {
-            "x": 6.215088187160598,
-            "y": 3.7918967611554555,
-            "heading": 2.632459037184743,
-            "angularVelocity": -1.890214948184595,
-            "velocityX": 2.8170810633981405,
-            "velocityY": -1.4897493503766595,
-            "timestamp": 1.6950054945592423
-        },
-        {
-            "x": 6.32424952389229,
-            "y": 3.717983083464812,
-            "heading": 2.5463998013121922,
-            "angularVelocity": -2.0519643880553406,
-            "velocityX": 2.6028022821127137,
-            "velocityY": -1.7623702194615716,
-            "timestamp": 1.7369454211844353
-        },
-        {
-            "x": 6.43630340548619,
-            "y": 3.6322698094657313,
-            "heading": 2.4886072976482807,
-            "angularVelocity": -1.3779829464268867,
-            "velocityX": 2.6717710451737124,
-            "velocityY": -2.043715401914737,
-            "timestamp": 1.7788853478096283
-        },
-        {
-            "x": 6.552059359988985,
-            "y": 3.5378523447078845,
-            "heading": 2.4573864362362134,
-            "angularVelocity": -0.7444185987992016,
-            "velocityX": 2.7600418936656252,
-            "velocityY": -2.251254886581771,
-            "timestamp": 1.8208252744348212
-        },
-        {
-            "x": 6.67032629558411,
-            "y": 3.432112823682954,
-            "heading": 2.4573457680499984,
-            "angularVelocity": -0.0009696770950155036,
-            "velocityX": 2.8199127922194083,
-            "velocityY": -2.521213782033988,
-            "timestamp": 1.8627652010600142
-        },
-        {
-            "x": 6.787202921818474,
-            "y": 3.3248215481814336,
-            "heading": 2.4573432508654287,
-            "angularVelocity": -0.00006001881195361165,
-            "velocityX": 2.7867627733081557,
-            "velocityY": -2.558213238195593,
-            "timestamp": 1.9047051276852072
-        },
-        {
-            "x": 6.904079764497574,
-            "y": 3.217530508972575,
-            "heading": 2.4573407325391345,
-            "angularVelocity": -0.000060046034804859665,
-            "velocityX": 2.7867679341358973,
-            "velocityY": -2.5582076041213115,
-            "timestamp": 1.9466450543104001
-        },
-        {
-            "x": 7.017539426879104,
-            "y": 3.1193532157248125,
-            "heading": 2.428286840001792,
-            "angularVelocity": -0.6927501995172902,
-            "velocityX": 2.7052899590285975,
-            "velocityY": -2.3409028376503684,
-            "timestamp": 1.988584980935593
-        },
-        {
-            "x": 7.118887938132189,
-            "y": 3.020968055873212,
-            "heading": 2.3710302150679574,
-            "angularVelocity": -1.365205653446252,
-            "velocityX": 2.4165161794108876,
-            "velocityY": -2.345859131582302,
-            "timestamp": 2.030524907560786
-        },
-        {
-            "x": 7.207551237715759,
-            "y": 2.9223074295194613,
-            "heading": 2.2882909026899103,
-            "angularVelocity": -1.9728053679604207,
-            "velocityX": 2.1140547139229047,
-            "velocityY": -2.3524272523282552,
-            "timestamp": 2.0724648341859786
-        },
-        {
-            "x": 7.28673413242529,
-            "y": 2.8318719270188626,
-            "heading": 2.1761461762136682,
-            "angularVelocity": -2.6739371167349164,
-            "velocityX": 1.8880074688058086,
-            "velocityY": -2.1563104606452623,
-            "timestamp": 2.1144047608111713
-        },
-        {
-            "x": 7.3579924776067,
-            "y": 2.7517426977667294,
-            "heading": 2.0378729016459984,
-            "angularVelocity": -3.2969364921256217,
-            "velocityX": 1.6990574594521515,
-            "velocityY": -1.9105715174046114,
-            "timestamp": 2.156344687436364
-        },
-        {
-            "x": 7.4224137082831785,
-            "y": 2.6828558926645387,
-            "heading": 1.874977709266495,
-            "angularVelocity": -3.884012335912244,
-            "velocityX": 1.5360358460374843,
-            "velocityY": -1.6425113405136766,
-            "timestamp": 2.198284614061557
-        },
-        {
-            "x": 7.480883603746462,
-            "y": 2.6255140291502124,
-            "heading": 1.6865689006450588,
-            "angularVelocity": -4.4923495051672475,
-            "velocityX": 1.3941344243592637,
-            "velocityY": -1.3672380504328716,
-            "timestamp": 2.2402245406867496
-        },
-        {
-            "x": 7.534437188323511,
-            "y": 2.579534136686106,
-            "heading": 1.4695142507662173,
-            "angularVelocity": -5.175370281846384,
-            "velocityX": 1.27691173748694,
-            "velocityY": -1.0963274417482318,
-            "timestamp": 2.2821644673119423
-        },
-        {
-            "x": 7.579534895925645,
-            "y": 2.5413039154996566,
-            "heading": 1.2172257697546927,
-            "angularVelocity": -6.015472636997345,
-            "velocityX": 1.0752929542572058,
-            "velocityY": -0.9115471643072651,
-            "timestamp": 2.324104393937135
-        },
-        {
-            "x": 7.615712715030757,
-            "y": 2.5097826728946813,
-            "heading": 0.9253708991340255,
-            "angularVelocity": -6.958878903840344,
-            "velocityX": 0.8626104530039771,
-            "velocityY": -0.7515807761580874,
-            "timestamp": 2.366044320562328
-        },
-        {
-            "x": 7.6401514150011085,
-            "y": 2.4865499215178333,
-            "heading": 0.6440959436378817,
-            "angularVelocity": -6.706615345559152,
-            "velocityX": 0.5827072657697844,
-            "velocityY": -0.5539530763721487,
-            "timestamp": 2.4079842471875206
-        },
-        {
-            "x": 7.656070937434432,
-            "y": 2.471863188851966,
-            "heading": 0.3945229348796906,
-            "angularVelocity": -5.950725927314207,
-            "velocityX": 0.37957916749813,
-            "velocityY": -0.3501849871393242,
-            "timestamp": 2.4499241738127133
-        },
-        {
-            "x": 7.668072859318959,
-            "y": 2.4642781587251137,
-            "heading": 0.19785094472919199,
-            "angularVelocity": -4.689373729909185,
-            "velocityX": 0.28616935818189254,
-            "velocityY": -0.18085463512222474,
-            "timestamp": 2.491864100437906
-        },
-        {
-            "x": 7.676136280306529,
-            "y": 2.459442710773055,
-            "heading": 0.06623696286278732,
-            "angularVelocity": -3.1381547956105695,
-            "velocityX": 0.19226120874341576,
-            "velocityY": -0.11529462116784107,
-            "timestamp": 2.533804027063099
+            "heading": 2.757748451080741,
+            "angularVelocity": 1.2837456569082273,
+            "velocityX": 3.1938053944353855,
+            "velocityY": -1.0955763997309995,
+            "timestamp": 1.4803422699681263
+        },
+        {
+            "x": 5.532416911390752,
+            "y": 4.059853570653742,
+            "heading": 2.796595493071039,
+            "angularVelocity": 0.9277997104504593,
+            "velocityX": 3.3825562713263437,
+            "velocityY": -0.818433852585974,
+            "timestamp": 1.522212343382395
+        },
+        {
+            "x": 5.681515537517707,
+            "y": 4.021534249773671,
+            "heading": 2.782742222711128,
+            "angularVelocity": -0.3308632928068908,
+            "velocityX": 3.5609831550031092,
+            "velocityY": -0.9151959324488058,
+            "timestamp": 1.5640824167966638
+        },
+        {
+            "x": 5.826340094588043,
+            "y": 3.969241008458542,
+            "heading": 2.7680381624736805,
+            "angularVelocity": -0.351183053632681,
+            "velocityX": 3.4589038246343846,
+            "velocityY": -1.2489407600921083,
+            "timestamp": 1.6059524902109326
+        },
+        {
+            "x": 5.965226819905552,
+            "y": 3.9129528098786466,
+            "heading": 2.7396888789742415,
+            "angularVelocity": -0.6770774729470177,
+            "velocityX": 3.317088172818383,
+            "velocityY": -1.3443539499673571,
+            "timestamp": 1.6478225636252013
+        },
+        {
+            "x": 6.097371617524639,
+            "y": 3.854212614126153,
+            "heading": 2.694967938521201,
+            "angularVelocity": -1.0680884174853156,
+            "velocityX": 3.1560679703526437,
+            "velocityY": -1.402915996141418,
+            "timestamp": 1.68969263703947
+        },
+        {
+            "x": 6.217931948857121,
+            "y": 3.789923024350862,
+            "heading": 2.6256097109731904,
+            "angularVelocity": -1.6565107699184056,
+            "velocityX": 2.8793914483893763,
+            "velocityY": -1.535454431598441,
+            "timestamp": 1.7315627104537388
+        },
+        {
+            "x": 6.32897922975643,
+            "y": 3.7148340577600942,
+            "heading": 2.547026684843302,
+            "angularVelocity": -1.8768303879569581,
+            "velocityX": 2.652187394098668,
+            "velocityY": -1.7933803422751633,
+            "timestamp": 1.7734327838680075
+        },
+        {
+            "x": 6.440862810984381,
+            "y": 3.629355422988249,
+            "heading": 2.48919642870263,
+            "angularVelocity": -1.381183538144076,
+            "velocityX": 2.672161095132505,
+            "velocityY": -2.0415210149288825,
+            "timestamp": 1.8153028572822762
+        },
+        {
+            "x": 6.556707402178449,
+            "y": 3.5350910926988375,
+            "heading": 2.4587564932761583,
+            "angularVelocity": -0.7270093635923316,
+            "velocityX": 2.766763507861174,
+            "velocityY": -2.2513533558144507,
+            "timestamp": 1.857172930696545
+        },
+        {
+            "x": 6.674645090324846,
+            "y": 3.429370276775565,
+            "heading": 2.4587373064570563,
+            "angularVelocity": -0.00045824660759140524,
+            "velocityX": 2.816753794040547,
+            "velocityY": -2.5249732637737297,
+            "timestamp": 1.8990430041108137
+        },
+        {
+            "x": 6.791280321048844,
+            "y": 3.3222067883695963,
+            "heading": 2.45873486083509,
+            "angularVelocity": -0.00005840978453659185,
+            "velocityX": 2.785646673460325,
+            "velocityY": -2.559429197691564,
+            "timestamp": 1.9409130775250825
+        },
+        {
+            "x": 6.907916035753451,
+            "y": 3.2150438274589583,
+            "heading": 2.4587324135695865,
+            "angularVelocity": -0.000058449037785704465,
+            "velocityX": 2.78565823256613,
+            "velocityY": -2.5594165993059286,
+            "timestamp": 1.9827831509393512
+        },
+        {
+            "x": 7.020962902689577,
+            "y": 3.1172887235192603,
+            "heading": 2.428615335233058,
+            "angularVelocity": -0.7192984363448716,
+            "velocityX": 2.6999443210338705,
+            "velocityY": -2.334724923276202,
+            "timestamp": 2.02465322435362
+        },
+        {
+            "x": 7.121900964071713,
+            "y": 3.0192154080188542,
+            "heading": 2.3705886859446603,
+            "angularVelocity": -1.385874075602246,
+            "velocityX": 2.4107447909976107,
+            "velocityY": -2.3423248994587165,
+            "timestamp": 2.0665232977678887
+        },
+        {
+            "x": 7.210190441218643,
+            "y": 2.92089200334631,
+            "heading": 2.287196223783446,
+            "angularVelocity": -1.9916961056197946,
+            "velocityX": 2.10865350708562,
+            "velocityY": -2.348297880916461,
+            "timestamp": 2.1083933711821574
+        },
+        {
+            "x": 7.2890247338542125,
+            "y": 2.830773686809754,
+            "heading": 2.1745743754916034,
+            "angularVelocity": -2.689793427815253,
+            "velocityX": 1.8828314881507633,
+            "velocityY": -2.1523324223703058,
+            "timestamp": 2.150263444596426
+        },
+        {
+            "x": 7.359953440317656,
+            "y": 2.7509272688501483,
+            "heading": 2.035955336818745,
+            "angularVelocity": -3.3106949037643396,
+            "velocityX": 1.6940191568728353,
+            "velocityY": -1.9070044890916185,
+            "timestamp": 2.192133518010695
+        },
+        {
+            "x": 7.4240572342258595,
+            "y": 2.6822786221440054,
+            "heading": 1.872800890040008,
+            "angularVelocity": -3.896684038847076,
+            "velocityX": 1.5310169933057254,
+            "velocityY": -1.6395635619484732,
+            "timestamp": 2.2340035914249636
+        },
+        {
+            "x": 7.482217323317858,
+            "y": 2.6251237224270416,
+            "heading": 1.684191854325982,
+            "angularVelocity": -4.504626343687046,
+            "velocityX": 1.3890610727273844,
+            "velocityY": -1.3650537258787387,
+            "timestamp": 2.2758736648392324
+        },
+        {
+            "x": 7.535468730024687,
+            "y": 2.5792763511542147,
+            "heading": 1.466995470236355,
+            "angularVelocity": -5.187389616938403,
+            "velocityX": 1.2718250140130243,
+            "velocityY": -1.0949914230912903,
+            "timestamp": 2.317743738253501
+        },
+        {
+            "x": 7.580274182888533,
+            "y": 2.5411410245640176,
+            "heading": 1.2146613001768354,
+            "angularVelocity": -6.02659965658257,
+            "velocityX": 1.0701068617801175,
+            "velocityY": -0.91080152195771,
+            "timestamp": 2.35961381166777
+        },
+        {
+            "x": 7.616169603666072,
+            "y": 2.509680561991982,
+            "heading": 0.922946754094809,
+            "angularVelocity": -6.967137200734248,
+            "velocityX": 0.8573049400316086,
+            "velocityY": -0.7513830286553573,
+            "timestamp": 2.4014838850820386
+        },
+        {
+            "x": 7.640414148022587,
+            "y": 2.4865073161659272,
+            "heading": 0.6422191973798425,
+            "angularVelocity": -6.704730463149791,
+            "velocityX": 0.5790423178062244,
+            "velocityY": -0.5534560590991822,
+            "timestamp": 2.4433539584963073
+        },
+        {
+            "x": 7.656220323606262,
+            "y": 2.4718614980447855,
+            "heading": 0.3932969564478367,
+            "angularVelocity": -5.9451111649394885,
+            "velocityX": 0.37750532288984423,
+            "velocityY": -0.34979203347062976,
+            "timestamp": 2.485224031910576
+        },
+        {
+            "x": 7.668143711832241,
+            "y": 2.4642737767821443,
+            "heading": 0.1972336516813064,
+            "angularVelocity": -4.682659684559189,
+            "velocityX": 0.28477113254632147,
+            "velocityY": -0.18122063430765137,
+            "timestamp": 2.527094105324845
+        },
+        {
+            "x": 7.676158456210791,
+            "y": 2.459440783193878,
+            "heading": 0.06602780599823356,
+            "angularVelocity": -3.1336425992116688,
+            "velocityX": 0.19141940113768102,
+            "velocityY": -0.11542835238065828,
+            "timestamp": 2.5689641787391135
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": 2.1057025676027217e-33,
-            "angularVelocity": -1.57932948845455,
-            "velocityX": 0.09212509425682942,
-            "velocityY": -0.058243086471875524,
-            "timestamp": 2.5757439536882916
+            "heading": 1.4385689598890085e-34,
+            "angularVelocity": -1.576968956919288,
+            "velocityX": 0.09174915341561693,
+            "velocityY": -0.05829421815741107,
+            "timestamp": 2.6108342521533823
         },
         {
             "x": 7.68,
             "y": 2.457,
-            "heading": -7.524501141381425e-34,
-            "angularVelocity": -5.907340858361994e-32,
-            "velocityX": -7.634877430876997e-33,
-            "velocityY": -7.90956322394541e-33,
-            "timestamp": 2.6176838803134843
+            "heading": 5.954582727394106e-35,
+            "angularVelocity": -1.9052580558070114e-33,
+            "velocityX": -2.275662344047759e-34,
+            "velocityY": 3.071010312134925e-34,
+            "timestamp": 2.652704325567651
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.1.traj
new file mode 100644
index 00000000..cfa6b046
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.1.traj	
@@ -0,0 +1,625 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 6.892560738534845e-27,
+            "timestamp": 0
+        },
+        {
+            "x": 2.24821488195175,
+            "y": 5.550858605907564,
+            "heading": -1.0860081979744884e-8,
+            "angularVelocity": -3.249360952601713e-7,
+            "velocityX": -0.053411135206172675,
+            "velocityY": -0.27351257600550494,
+            "timestamp": 0.0334222075852833
+        },
+        {
+            "x": 2.2446446457975586,
+            "y": 5.532575817984525,
+            "heading": -3.253781151640391e-8,
+            "angularVelocity": -6.486025430910346e-7,
+            "velocityX": -0.10682227213842765,
+            "velocityY": -0.5470251441769277,
+            "timestamp": 0.0668444151705666
+        },
+        {
+            "x": 2.2392892913368425,
+            "y": 5.505151636566201,
+            "heading": -6.492851000422694e-8,
+            "angularVelocity": -9.691369834959166e-7,
+            "velocityX": -0.1602334150722015,
+            "velocityY": -0.8205377023156053,
+            "timestamp": 0.10026662275584991
+        },
+        {
+            "x": 2.2321488181807467,
+            "y": 5.468586062083933,
+            "heading": -1.0785000725838172e-7,
+            "angularVelocity": -0.000001284220850944888,
+            "velocityX": -0.21364456964054193,
+            "velocityY": -1.0940502475484135,
+            "timestamp": 0.1336888303411332
+        },
+        {
+            "x": 2.2232232256851456,
+            "y": 5.42287909509794,
+            "heading": -1.6102300741445552e-7,
+            "angularVelocity": -0.0000015909481231905412,
+            "velocityX": -0.26705574348127187,
+            "velocityY": -1.3675627760193463,
+            "timestamp": 0.16711103792641652
+        },
+        {
+            "x": 2.212512512848178,
+            "y": 5.368030736346982,
+            "heading": -2.2404505707142586e-7,
+            "angularVelocity": -0.0000018856338989468898,
+            "velocityX": -0.32046694729784664,
+            "velocityY": -1.6410752823864576,
+            "timestamp": 0.20053324551169982
+        },
+        {
+            "x": 2.200016678146418,
+            "y": 5.304040986826963,
+            "heading": -2.963548114247805e-7,
+            "angularVelocity": -0.000002163524111560841,
+            "velocityX": -0.3738781966956197,
+            "velocityY": -1.9145877589558649,
+            "timestamp": 0.23395545309698312
+        },
+        {
+            "x": 2.1857357192573095,
+            "y": 5.230909847922857,
+            "heading": -3.7718073304542066e-7,
+            "angularVelocity": -0.0000024183297502514874,
+            "velocityX": -0.4272895155847271,
+            "velocityY": -2.1881001940865983,
+            "timestamp": 0.2673776606822664
+        },
+        {
+            "x": 2.169669632551276,
+            "y": 5.148637321647842,
+            "heading": -4.654625982128869e-7,
+            "angularVelocity": -0.0000026414132559778984,
+            "velocityX": -0.48070094307146977,
+            "velocityY": -2.4616125689825754,
+            "timestamp": 0.30079986826754973
+        },
+        {
+            "x": 2.15181841205741,
+            "y": 5.057223411125392,
+            "heading": -5.597189955047307e-7,
+            "angularVelocity": -0.0000028201725975039026,
+            "velocityX": -0.5341125492089794,
+            "velocityY": -2.735124850423542,
+            "timestamp": 0.33422207585283303
+        },
+        {
+            "x": 2.1321820469941515,
+            "y": 4.956668121725741,
+            "heading": -6.57784320236276e-7,
+            "angularVelocity": -0.0000029341365576412115,
+            "velocityX": -0.5875244779329194,
+            "velocityY": -3.008636971185932,
+            "timestamp": 0.36764428343811634
+        },
+        {
+            "x": 2.1107605140888954,
+            "y": 4.846971464532025,
+            "heading": -7.56114109936986e-7,
+            "angularVelocity": -0.0000029420493990798148,
+            "velocityX": -0.6409371029779476,
+            "velocityY": -3.282148760338516,
+            "timestamp": 0.40106649102339964
+        },
+        {
+            "x": 2.087553735835974,
+            "y": 4.728133474365802,
+            "heading": -8.465523402036467e-7,
+            "angularVelocity": -0.0000027059321418483588,
+            "velocityX": -0.6943520470083785,
+            "velocityY": -3.5556595076225634,
+            "timestamp": 0.43448869860868294
+        },
+        {
+            "x": 2.0633251233427066,
+            "y": 4.604042454619442,
+            "heading": -8.465523885072994e-7,
+            "angularVelocity": -1.4452562228577176e-12,
+            "velocityX": -0.724925558303899,
+            "velocityY": -3.712831339152857,
+            "timestamp": 0.46791090619396625
+        },
+        {
+            "x": 2.039088654512122,
+            "y": 4.479952969280304,
+            "heading": -8.465523654496814e-7,
+            "angularVelocity": 6.898891300327124e-13,
+            "velocityX": -0.725160621670614,
+            "velocityY": -3.7127854293461913,
+            "timestamp": 0.5013331137792496
+        },
+        {
+            "x": 2.0148473415026222,
+            "y": 4.355864430051467,
+            "heading": -8.465523174606992e-7,
+            "angularVelocity": 1.4358411666699279e-12,
+            "velocityX": -0.7253055606758912,
+            "velocityY": -3.712757121497041,
+            "timestamp": 0.5347553213645329
+        },
+        {
+            "x": 1.9907968545860597,
+            "y": 4.231738764257757,
+            "heading": -8.465522534990754e-7,
+            "angularVelocity": 1.9137462023851052e-12,
+            "velocityX": -0.7195960008785613,
+            "velocityY": -3.7138679567028667,
+            "timestamp": 0.5681775289498161
+        },
+        {
+            "x": 1.9669031646192998,
+            "y": 4.107582820499775,
+            "heading": -8.465521766194915e-7,
+            "angularVelocity": 2.3002544763334585e-12,
+            "velocityX": -0.7149046006511871,
+            "velocityY": -3.7147738802470687,
+            "timestamp": 0.6015997365350993
+        },
+        {
+            "x": 1.9504985155072192,
+            "y": 3.982217377752346,
+            "heading": -8.465520941748615e-7,
+            "angularVelocity": 2.4667619198655976e-12,
+            "velocityX": -0.4908308067350749,
+            "velocityY": -3.7509623631996885,
+            "timestamp": 0.6350219441203826
+        },
+        {
+            "x": 1.94330794196602,
+            "y": 3.855987816020002,
+            "heading": -8.465520093116816e-7,
+            "angularVelocity": 2.5391254773053977e-12,
+            "velocityX": -0.2151435844765948,
+            "velocityY": -3.7768169984060798,
+            "timestamp": 0.6684441517056658
+        },
+        {
+            "x": 1.9454293697222904,
+            "y": 3.7295714172218672,
+            "heading": -8.465519250884413e-7,
+            "angularVelocity": 2.519978339764665e-12,
+            "velocityX": 0.0634735976376205,
+            "velocityY": -3.7824072056149025,
+            "timestamp": 0.7018663592909491
+        },
+        {
+            "x": 1.9568513812515056,
+            "y": 3.603654207925799,
+            "heading": -8.465518451486746e-7,
+            "angularVelocity": 2.391815832767542e-12,
+            "velocityX": 0.34174916483985474,
+            "velocityY": -3.767471342961152,
+            "timestamp": 0.7352885668762323
+        },
+        {
+            "x": 1.9775120291721782,
+            "y": 3.4789195174340257,
+            "heading": -8.465517739710846e-7,
+            "angularVelocity": 2.129649524174011e-12,
+            "velocityX": 0.6181712523959566,
+            "velocityY": -3.7320901132416737,
+            "timestamp": 0.7687107744615156
+        },
+        {
+            "x": 2.0072992100178335,
+            "y": 3.3560442616931043,
+            "heading": -8.465517105809759e-7,
+            "angularVelocity": 1.8966463157464128e-12,
+            "velocityX": 0.8912391789143821,
+            "velocityY": -3.6764554055067418,
+            "timestamp": 0.8021329820467988
+        },
+        {
+            "x": 2.046051283468891,
+            "y": 3.235695267642527,
+            "heading": -8.465516527951994e-7,
+            "angularVelocity": 1.7289634182989629e-12,
+            "velocityX": 1.1594707905613406,
+            "velocityY": -3.600869085126494,
+            "timestamp": 0.8355551896320821
+        },
+        {
+            "x": 2.0935579534966013,
+            "y": 3.1185256537724477,
+            "heading": -8.46551596321545e-7,
+            "angularVelocity": 1.6897044173558387e-12,
+            "velocityX": 1.421410297528723,
+            "velocityY": -3.5057413120014775,
+            "timestamp": 0.8689773972173653
+        },
+        {
+            "x": 2.1495614051818848,
+            "y": 3.005171298980713,
+            "heading": -8.465515057383081e-7,
+            "angularVelocity": 2.7102707524537993e-12,
+            "velocityX": 1.6756359238847198,
+            "velocityY": -3.3915878986274275,
+            "timestamp": 0.9023996048026486
+        },
+        {
+            "x": 2.288743031901554,
+            "y": 2.8026748842621148,
+            "heading": -8.46551464739476e-7,
+            "angularVelocity": 6.312009526868921e-13,
+            "velocityX": 2.142782210187666,
+            "velocityY": -3.117550249366458,
+            "timestamp": 0.9673533019435205
+        },
+        {
+            "x": 2.455414708004788,
+            "y": 2.6221288200825,
+            "heading": -8.46551435217149e-7,
+            "angularVelocity": 4.5451345461171864e-13,
+            "velocityX": 2.566007532127087,
+            "velocityY": -2.779611817758129,
+            "timestamp": 1.0323069990843925
+        },
+        {
+            "x": 2.646160177401748,
+            "y": 2.4672337011827246,
+            "heading": -8.465514075797998e-7,
+            "angularVelocity": 4.254931139136813e-13,
+            "velocityX": 2.9366376017527633,
+            "velocityY": -2.384700574689416,
+            "timestamp": 1.0972606962252645
+        },
+        {
+            "x": 2.857069726290126,
+            "y": 2.341164292711998,
+            "heading": -8.465513787578765e-7,
+            "angularVelocity": 4.4373032078954734e-13,
+            "velocityX": 3.2470753501649696,
+            "velocityY": -1.9409119729950552,
+            "timestamp": 1.1622143933661364
+        },
+        {
+            "x": 3.0838173053505575,
+            "y": 2.2464971410417167,
+            "heading": -8.465513471236816e-7,
+            "angularVelocity": 4.870268741320482e-13,
+            "velocityX": 3.490911049579749,
+            "velocityY": -1.457455939200772,
+            "timestamp": 1.2271680905070084
+        },
+        {
+            "x": 3.3145112489857773,
+            "y": 2.1619002222864983,
+            "heading": -8.465513138187862e-7,
+            "angularVelocity": 5.127482864627404e-13,
+            "velocityX": 3.5516676307835056,
+            "velocityY": -1.3024188380298123,
+            "timestamp": 1.2921217876478803
+        },
+        {
+            "x": 3.5452013267301767,
+            "y": 2.0772927620920076,
+            "heading": -8.465512839726007e-7,
+            "angularVelocity": 4.594994306997602e-13,
+            "velocityX": 3.5516081131487938,
+            "velocityY": -1.302581129610464,
+            "timestamp": 1.3570754847887523
+        },
+        {
+            "x": 3.77589133724735,
+            "y": 1.9926851185956267,
+            "heading": -8.465512625373994e-7,
+            "angularVelocity": 3.3000743037279784e-13,
+            "velocityX": 3.5516070781466733,
+            "velocityY": -1.302583951649345,
+            "timestamp": 1.4220291819296242
+        },
+        {
+            "x": 4.006581361028944,
+            "y": 1.9080775112659911,
+            "heading": -8.465512535495126e-7,
+            "angularVelocity": 1.3837377280671698e-13,
+            "velocityX": 3.5516072823601217,
+            "velocityY": -1.302583394841325,
+            "timestamp": 1.4869828790704962
+        },
+        {
+            "x": 4.2372714040114055,
+            "y": 1.823469956289169,
+            "heading": -8.465512575529088e-7,
+            "angularVelocity": -6.163459506326445e-14,
+            "velocityX": 3.551607577968653,
+            "velocityY": -1.3025825888392473,
+            "timestamp": 1.5519365762113682
+        },
+        {
+            "x": 4.467961461705261,
+            "y": 1.7388624414245573,
+            "heading": -8.465512722492451e-7,
+            "angularVelocity": -2.2625863777166434e-13,
+            "velocityX": 3.5516078044591044,
+            "velocityY": -1.3025819712885556,
+            "timestamp": 1.6168902733522401
+        },
+        {
+            "x": 4.698651553926265,
+            "y": 1.6542550207002664,
+            "heading": -8.465512926518018e-7,
+            "angularVelocity": -3.141092229421582e-13,
+            "velocityX": 3.551608336024854,
+            "velocityY": -1.3025805219428788,
+            "timestamp": 1.681843970493112
+        },
+        {
+            "x": 4.929586887359619,
+            "y": 1.5703192949295044,
+            "heading": -8.465513064459796e-7,
+            "angularVelocity": -2.1236938120360531e-13,
+            "velocityX": 3.555383967330626,
+            "velocityY": -1.2922393869021438,
+            "timestamp": 1.746797667633984
+        },
+        {
+            "x": 5.059495672066219,
+            "y": 1.5327262216225606,
+            "heading": -8.465512750899748e-7,
+            "angularVelocity": 8.770995258558909e-13,
+            "velocityX": 3.633847296905686,
+            "velocityY": -1.051564666141291,
+            "timestamp": 1.7825473219513848
+        },
+        {
+            "x": 5.189548300230332,
+            "y": 1.4956338313822377,
+            "heading": -8.465512186056332e-7,
+            "angularVelocity": 1.579996862146135e-12,
+            "velocityX": 3.637870929029155,
+            "velocityY": -1.0375594099736098,
+            "timestamp": 1.8182969762687855
+        },
+        {
+            "x": 5.319623170079142,
+            "y": 1.4586195133360007,
+            "heading": -8.46551136487956e-7,
+            "angularVelocity": 2.2970201754243682e-12,
+            "velocityX": 3.638493080071459,
+            "velocityY": -1.0353755512600933,
+            "timestamp": 1.8540466305861862
+        },
+        {
+            "x": 5.4496989055100356,
+            "y": 1.42160823723968,
+            "heading": -8.465510387564672e-7,
+            "angularVelocity": 2.7337743725450157e-12,
+            "velocityX": 3.6385172923918394,
+            "velocityY": -1.0352904609291835,
+            "timestamp": 1.889796284903587
+        },
+        {
+            "x": 5.57977446957584,
+            "y": 1.3845963588884114,
+            "heading": -8.465509335637302e-7,
+            "angularVelocity": 2.9424826347181554e-12,
+            "velocityX": 3.6385124989164392,
+            "velocityY": -1.03530730738433,
+            "timestamp": 1.9255459392209877
+        },
+        {
+            "x": 5.709850057975644,
+            "y": 1.3475845660573411,
+            "heading": -8.465508277220688e-7,
+            "angularVelocity": 2.960634540295608e-12,
+            "velocityX": 3.6385131795943324,
+            "velocityY": -1.0353049151878095,
+            "timestamp": 1.9612955935383884
+        },
+        {
+            "x": 5.839925662966751,
+            "y": 1.3105728315354166,
+            "heading": -8.465507264753305e-7,
+            "angularVelocity": 2.832103981671134e-12,
+            "velocityX": 3.6385136436912036,
+            "velocityY": -1.0353032841469862,
+            "timestamp": 1.9970452478557892
+        },
+        {
+            "x": 5.970001275498046,
+            "y": 1.273561123513075,
+            "heading": -8.465506337302748e-7,
+            "angularVelocity": 2.594292378130792e-12,
+            "velocityX": 3.638513854607602,
+            "velocityY": -1.0353025428927405,
+            "timestamp": 2.03279490217319
+        },
+        {
+            "x": 6.100076894564937,
+            "y": 1.2365494384597497,
+            "heading": -8.46550552278632e-7,
+            "angularVelocity": 2.2783896618382965e-12,
+            "velocityX": 3.6385140374232323,
+            "velocityY": -1.0353019003965735,
+            "timestamp": 2.0685445564905907
+        },
+        {
+            "x": 6.230152520105601,
+            "y": 1.1995377761581834,
+            "heading": -8.465504825990077e-7,
+            "angularVelocity": 1.9490992420524453e-12,
+            "velocityX": 3.6385142185095396,
+            "velocityY": -1.03530126397757,
+            "timestamp": 2.1042942108079914
+        },
+        {
+            "x": 6.3602281516940815,
+            "y": 1.16252613511139,
+            "heading": -8.465504225140753e-7,
+            "angularVelocity": 1.680713660601537e-12,
+            "velocityX": 3.6385143876808734,
+            "velocityY": -1.0353006694327234,
+            "timestamp": 2.140043865125392
+        },
+        {
+            "x": 6.490303788775648,
+            "y": 1.1255145133698101,
+            "heading": -8.465503700121188e-7,
+            "angularVelocity": 1.4686003981542915e-12,
+            "velocityX": 3.638514541335137,
+            "velocityY": -1.0353001294215285,
+            "timestamp": 2.175793519442793
+        },
+        {
+            "x": 6.62037943067546,
+            "y": 1.0885029085617548,
+            "heading": -8.465503232940394e-7,
+            "angularVelocity": 1.306812060469801e-12,
+            "velocityX": 3.638514676112538,
+            "velocityY": -1.0352996557519205,
+            "timestamp": 2.2115431737601936
+        },
+        {
+            "x": 6.7504550766965234,
+            "y": 1.0514913182376744,
+            "heading": -8.465502808582129e-7,
+            "angularVelocity": 1.187027603302574e-12,
+            "velocityX": 3.638514791393393,
+            "velocityY": -1.0352992506018621,
+            "timestamp": 2.2472928280775943
+        },
+        {
+            "x": 6.880530727542945,
+            "y": 1.0144797448710672,
+            "heading": -8.465502433332782e-7,
+            "angularVelocity": 1.0496586699357012e-12,
+            "velocityX": 3.6385149263697616,
+            "velocityY": -1.0352987762623522,
+            "timestamp": 2.283042482394995
+        },
+        {
+            "x": 7.003525953233288,
+            "y": 0.9794828656529073,
+            "heading": -7.145353891597533e-7,
+            "angularVelocity": 0.000003692758900357643,
+            "velocityX": 3.440459160761053,
+            "velocityY": -0.9789431502593731,
+            "timestamp": 2.318792136712396
+        },
+        {
+            "x": 7.116271603762692,
+            "y": 0.9474023877264399,
+            "heading": -5.944969754078434e-7,
+            "angularVelocity": 0.0000033577503345576707,
+            "velocityX": 3.153754985388111,
+            "velocityY": -0.8973647029323183,
+            "timestamp": 2.3545417910297965
+        },
+        {
+            "x": 7.218767661219051,
+            "y": 0.9182383156679585,
+            "heading": -4.859087151281335e-7,
+            "angularVelocity": 0.0000030374632243726353,
+            "velocityX": 2.867050308972391,
+            "velocityY": -0.8157861275958178,
+            "timestamp": 2.3902914453471973
+        },
+        {
+            "x": 7.311014119553375,
+            "y": 0.8919906507276494,
+            "heading": -3.8850367492824467e-7,
+            "angularVelocity": 0.000002724642854703664,
+            "velocityX": 2.580345463352478,
+            "velocityY": -0.7342075172887301,
+            "timestamp": 2.426041099664598
+        },
+        {
+            "x": 7.393010975686544,
+            "y": 0.8686593933386082,
+            "heading": -3.0210953963079075e-7,
+            "angularVelocity": 0.0000024166425366030095,
+            "velocityX": 2.293640531602499,
+            "velocityY": -0.6526288948669626,
+            "timestamp": 2.4617907539819988
+        },
+        {
+            "x": 7.464758227730078,
+            "y": 0.8482445436165693,
+            "heading": -2.2661033918382436e-7,
+            "angularVelocity": 0.000002111886167406066,
+            "velocityX": 2.006935547027439,
+            "velocityY": -0.5710502692078399,
+            "timestamp": 2.4975404082993995
+        },
+        {
+            "x": 7.526255874392654,
+            "y": 0.8307461015250325,
+            "heading": -1.6193015142138781e-7,
+            "angularVelocity": 0.0000018092535156938384,
+            "velocityX": 1.7202305263310733,
+            "velocityY": -0.48947164456971237,
+            "timestamp": 2.5332900626168002
+        },
+        {
+            "x": 7.577503914725456,
+            "y": 0.8161640669449958,
+            "heading": -1.0802379214331377e-7,
+            "angularVelocity": 0.0000015078847699136293,
+            "velocityX": 1.4335254790941394,
+            "velocityY": -0.4078930232603403,
+            "timestamp": 2.569039716934201
+        },
+        {
+            "x": 7.618502347994633,
+            "y": 0.8044984397090525,
+            "heading": -6.487055743300418e-8,
+            "angularVelocity": 0.0000012070951602673883,
+            "velocityX": 1.1468204113297347,
+            "velocityY": -0.3263144066337203,
+            "timestamp": 2.6047893712516017
+        },
+        {
+            "x": 7.649251173610278,
+            "y": 0.7957492196197445,
+            "heading": -3.246962719134405e-8,
+            "angularVelocity": 9.063284906782187e-7,
+            "velocityX": 0.8601153270642448,
+            "velocityY": -0.24473579553046104,
+            "timestamp": 2.6405390255690024
+        },
+        {
+            "x": 7.669750391083677,
+            "y": 0.7899164064601075,
+            "heading": -1.0836483282929717e-8,
+            "angularVelocity": 6.051287577519143e-7,
+            "velocityX": 0.573410229128317,
+            "velocityY": -0.16315719049619,
+            "timestamp": 2.676288679886403
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 2.0494644799121527e-30,
+            "angularVelocity": 3.031213445055282e-7,
+            "velocityX": 0.28670511958863415,
+            "velocityY": -0.08157859189950141,
+            "timestamp": 2.712038334203804
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -1.7051999701665118e-29,
+            "angularVelocity": -2.5846596071248955e-28,
+            "velocityX": 4.524644517512267e-29,
+            "velocityY": 1.710870028377411e-29,
+            "timestamp": 2.7477879885212046
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.1.traj
new file mode 100644
index 00000000..bd47bcab
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.1.traj	
@@ -0,0 +1,202 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -9.427656700789375e-18,
+            "angularVelocity": -1.3516738833302546e-18,
+            "velocityX": 8.730641062152612e-18,
+            "velocityY": -7.295689550902649e-17,
+            "timestamp": 0
+        },
+        {
+            "x": 2.220337966696873,
+            "y": 5.62696613740055,
+            "heading": -0.005158227148275901,
+            "angularVelocity": -0.05503188081512997,
+            "velocityX": -0.31645707615215996,
+            "velocityY": 0.7144455616516493,
+            "timestamp": 0.09373161650796653
+        },
+        {
+            "x": 2.1631184668461043,
+            "y": 5.761793353879717,
+            "heading": -0.015168353452777405,
+            "angularVelocity": -0.10679562219702789,
+            "velocityX": -0.6104610374014564,
+            "velocityY": 1.4384390401258744,
+            "timestamp": 0.18746323301593307
+        },
+        {
+            "x": 2.0853065565036237,
+            "y": 5.966914141221312,
+            "heading": -0.029010799448120003,
+            "angularVelocity": -0.14768171627730414,
+            "velocityX": -0.8301564961900216,
+            "velocityY": 2.1883841865051155,
+            "timestamp": 0.2811948495238996
+        },
+        {
+            "x": 2.0799249337959744,
+            "y": 6.161512978496455,
+            "heading": -0.03183861533006777,
+            "angularVelocity": -0.0301692853201504,
+            "velocityX": -0.05741523413491959,
+            "velocityY": 2.076128040089899,
+            "timestamp": 0.37492646603186613
+        },
+        {
+            "x": 2.1174615185646393,
+            "y": 6.2967780571782885,
+            "heading": -0.027602730416951004,
+            "angularVelocity": 0.045191634060389424,
+            "velocityX": 0.4004687656856407,
+            "velocityY": 1.443110486314265,
+            "timestamp": 0.46865808253983265
+        },
+        {
+            "x": 2.193636869673838,
+            "y": 6.369832500172102,
+            "heading": -0.016913451231696384,
+            "angularVelocity": 0.11404134040882687,
+            "velocityX": 0.8126964406159006,
+            "velocityY": 0.7794002249774918,
+            "timestamp": 0.5623896990477992
+        },
+        {
+            "x": 2.306850910186767,
+            "y": 6.379708766937256,
+            "heading": 7.831941019246608e-17,
+            "angularVelocity": 0.1804455301403977,
+            "velocityX": 1.20785328079034,
+            "velocityY": 0.10536750707073723,
+            "timestamp": 0.6561213155557658
+        },
+        {
+            "x": 2.454400050444712,
+            "y": 6.392580268241816,
+            "heading": 0.024770297736306776,
+            "angularVelocity": 0.3129674474840377,
+            "velocityX": 1.864252028642711,
+            "velocityY": 0.1626286834118752,
+            "timestamp": 0.7352678783546651
+        },
+        {
+            "x": 2.653900880802349,
+            "y": 6.4099837935603645,
+            "heading": 0.06003005706528144,
+            "angularVelocity": 0.445499565389401,
+            "velocityX": 2.520650591795653,
+            "velocityY": 0.2198898436406934,
+            "timestamp": 0.8144144411535644
+        },
+        {
+            "x": 2.905353386807521,
+            "y": 6.431919341632162,
+            "heading": 0.10577908294347244,
+            "angularVelocity": 0.5780292189622053,
+            "velocityX": 3.177048972348669,
+            "velocityY": 0.2771509879403426,
+            "timestamp": 0.8935610039524637
+        },
+        {
+            "x": 3.203627341926949,
+            "y": 6.457939375335675,
+            "heading": 0.10577908974967822,
+            "angularVelocity": 8.599496273628574e-8,
+            "velocityX": 3.768628030977189,
+            "velocityY": 0.3287575958241692,
+            "timestamp": 0.972707566751363
+        },
+        {
+            "x": 3.501901297052475,
+            "y": 6.48395940903972,
+            "heading": 0.10577909654992135,
+            "angularVelocity": 8.591962579346019e-8,
+            "velocityX": 3.7686280310542424,
+            "velocityY": 0.328757595830891,
+            "timestamp": 1.0518541295502624
+        },
+        {
+            "x": 3.8001752521780374,
+            "y": 6.509979442743767,
+            "heading": 0.10577910335027264,
+            "angularVelocity": 8.592099253060419e-8,
+            "velocityX": 3.768628031054697,
+            "velocityY": 0.32875759583093067,
+            "timestamp": 1.1310006923491618
+        },
+        {
+            "x": 4.0984492073035454,
+            "y": 6.53599947644781,
+            "heading": 0.1057791101506597,
+            "angularVelocity": 8.592144417290625e-8,
+            "velocityX": 3.7686280310540092,
+            "velocityY": 0.32875759583087066,
+            "timestamp": 1.2101472551480612
+        },
+        {
+            "x": 4.396723162417087,
+            "y": 6.562019510150809,
+            "heading": 0.10577911696012733,
+            "angularVelocity": 8.603617553663074e-8,
+            "velocityX": 3.7686280309028084,
+            "velocityY": 0.3287575958176806,
+            "timestamp": 1.2892938179469606
+        },
+        {
+            "x": 4.656482038612004,
+            "y": 6.5846796673565295,
+            "heading": 0.15819030641666595,
+            "angularVelocity": 0.6622042398696235,
+            "velocityX": 3.2819981943490006,
+            "velocityY": 0.28630627034678896,
+            "timestamp": 1.36844038074586
+        },
+        {
+            "x": 4.864289197548449,
+            "y": 6.6028077981790565,
+            "heading": 0.2001158851669991,
+            "angularVelocity": 0.5297207770962873,
+            "velocityX": 2.6255992880506356,
+            "velocityY": 0.22904508018355874,
+            "timestamp": 1.4475869435447595
+        },
+        {
+            "x": 5.020144593431014,
+            "y": 6.616403898623411,
+            "heading": 0.2315582663704034,
+            "angularVelocity": 0.39726780407754475,
+            "velocityX": 1.9691998031370395,
+            "velocityY": 0.17178383954462473,
+            "timestamp": 1.526733506343659
+        },
+        {
+            "x": 5.124048198578653,
+            "y": 6.625467966274828,
+            "heading": 0.2525193347871735,
+            "angularVelocity": 0.2648386446045577,
+            "velocityX": 1.3127999684793037,
+            "velocityY": 0.1145225683956373,
+            "timestamp": 1.6058800691425583
+        },
+        {
+            "x": 5.176000000000001,
+            "y": 6.63,
+            "heading": 0.26299999999999985,
+            "angularVelocity": 0.13242097751555232,
+            "velocityX": 0.6563999696784293,
+            "velocityY": 0.05726128292756526,
+            "timestamp": 1.6850266319414577
+        },
+        {
+            "x": 5.176000000000001,
+            "y": 6.63,
+            "heading": 0.26299999999999996,
+            "angularVelocity": 1.6428599245730974e-17,
+            "velocityX": 1.9833104251084488e-16,
+            "velocityY": 1.8748049703629392e-17,
+            "timestamp": 1.7641731947403572
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.traj
index 2aaf78f3..bd47bcab 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.traj	
@@ -3,146 +3,200 @@
         {
             "x": 2.25,
             "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 4.4337460853781175e-35,
-            "velocityX": 0,
-            "velocityY": 0,
+            "heading": -9.427656700789375e-18,
+            "angularVelocity": -1.3516738833302546e-18,
+            "velocityX": 8.730641062152612e-18,
+            "velocityY": -7.295689550902649e-17,
             "timestamp": 0
         },
         {
-            "x": 2.3070751783284065,
-            "y": 5.580871519986164,
-            "heading": 0.00875997642576413,
-            "angularVelocity": 0.10256751513529143,
-            "velocityX": 0.6682733985254588,
-            "velocityY": 0.24437736336610263,
-            "timestamp": 0.08540692844327274
-        },
-        {
-            "x": 2.421225525991661,
-            "y": 5.622614560146346,
-            "heading": 0.026280802198455293,
-            "angularVelocity": 0.20514525100066663,
-            "velocityX": 1.3365466917484683,
-            "velocityY": 0.48875472893171557,
-            "timestamp": 0.1708138568865455
-        },
-        {
-            "x": 2.592451026856299,
-            "y": 5.685229121649564,
-            "heading": 0.052563939294491345,
-            "angularVelocity": 0.30774010463909035,
-            "velocityX": 2.0048197960703633,
-            "velocityY": 0.7331321081849562,
-            "timestamp": 0.25622078532981823
-        },
-        {
-            "x": 2.8207516562305845,
-            "y": 5.768715208168554,
-            "heading": 0.0876113178799828,
-            "angularVelocity": 0.41035755792072426,
-            "velocityX": 2.673092611285307,
-            "velocityY": 0.9775095304409794,
-            "timestamp": 0.341627713773091
-        },
-        {
-            "x": 3.106127374237044,
-            "y": 5.873072825511933,
-            "heading": 0.13142500230326418,
-            "angularVelocity": 0.5129991819385286,
-            "velocityX": 3.341364959588794,
-            "velocityY": 1.221887020708095,
-            "timestamp": 0.4270346422163637
-        },
-        {
-            "x": 3.40956412390744,
-            "y": 5.984036121763709,
-            "heading": 0.13142500754936084,
-            "angularVelocity": 6.142471997127927e-8,
-            "velocityX": 3.5528352933560723,
-            "velocityY": 1.299230615997127,
-            "timestamp": 0.5124415706596365
-        },
-        {
-            "x": 3.7130008735619784,
-            "y": 6.094999418058892,
-            "heading": 0.1314250127954367,
-            "angularVelocity": 6.142447634082945e-8,
-            "velocityX": 3.5528352931703995,
-            "velocityY": 1.2992306165053664,
-            "timestamp": 0.5978484991029092
-        },
-        {
-            "x": 4.016437623216516,
-            "y": 6.205962714354075,
-            "heading": 0.13142501804151255,
-            "angularVelocity": 6.142447606684555e-8,
-            "velocityX": 3.552835293170399,
-            "velocityY": 1.2992306165053664,
-            "timestamp": 0.683255427546182
-        },
-        {
-            "x": 4.3198743728836435,
-            "y": 6.316926010614789,
-            "heading": 0.13142502328760913,
-            "angularVelocity": 6.142471942956278e-8,
-            "velocityX": 3.5528352933178016,
-            "velocityY": 1.2992306161017806,
-            "timestamp": 0.7686623559894548
-        },
-        {
-            "x": 4.605249584503867,
-            "y": 6.421283973215489,
-            "heading": 0.17528171773503587,
-            "angularVelocity": 0.5135027713419801,
-            "velocityX": 3.3413590304886087,
-            "velocityY": 1.2218910632059061,
-            "timestamp": 0.8540692844327276
-        },
-        {
-            "x": 4.83354975240058,
-            "y": 6.5047703704067485,
-            "heading": 0.21036850612417926,
-            "angularVelocity": 0.4108189935954435,
-            "velocityX": 2.6730872080050085,
-            "velocityY": 0.9775131679944534,
-            "timestamp": 0.9394762128760004
-        },
-        {
-            "x": 5.004774875721697,
-            "y": 6.567385181417454,
-            "heading": 0.23668415165777779,
-            "angularVelocity": 0.3081207346202291,
-            "velocityX": 2.0048153755446823,
-            "velocityY": 0.7331350295812797,
-            "timestamp": 1.0248831413192732
-        },
-        {
-            "x": 5.118924957871047,
-            "y": 6.6091283935214395,
-            "heading": 0.25422806054578806,
-            "angularVelocity": 0.20541552316405948,
-            "velocityX": 1.3365435829385781,
-            "velocityY": 0.48875674216187737,
-            "timestamp": 1.110290069762546
-        },
-        {
-            "x": 5.176,
+            "x": 2.220337966696873,
+            "y": 5.62696613740055,
+            "heading": -0.005158227148275901,
+            "angularVelocity": -0.05503188081512997,
+            "velocityX": -0.31645707615215996,
+            "velocityY": 0.7144455616516493,
+            "timestamp": 0.09373161650796653
+        },
+        {
+            "x": 2.1631184668461043,
+            "y": 5.761793353879717,
+            "heading": -0.015168353452777405,
+            "angularVelocity": -0.10679562219702789,
+            "velocityX": -0.6104610374014564,
+            "velocityY": 1.4384390401258744,
+            "timestamp": 0.18746323301593307
+        },
+        {
+            "x": 2.0853065565036237,
+            "y": 5.966914141221312,
+            "heading": -0.029010799448120003,
+            "angularVelocity": -0.14768171627730414,
+            "velocityX": -0.8301564961900216,
+            "velocityY": 2.1883841865051155,
+            "timestamp": 0.2811948495238996
+        },
+        {
+            "x": 2.0799249337959744,
+            "y": 6.161512978496455,
+            "heading": -0.03183861533006777,
+            "angularVelocity": -0.0301692853201504,
+            "velocityX": -0.05741523413491959,
+            "velocityY": 2.076128040089899,
+            "timestamp": 0.37492646603186613
+        },
+        {
+            "x": 2.1174615185646393,
+            "y": 6.2967780571782885,
+            "heading": -0.027602730416951004,
+            "angularVelocity": 0.045191634060389424,
+            "velocityX": 0.4004687656856407,
+            "velocityY": 1.443110486314265,
+            "timestamp": 0.46865808253983265
+        },
+        {
+            "x": 2.193636869673838,
+            "y": 6.369832500172102,
+            "heading": -0.016913451231696384,
+            "angularVelocity": 0.11404134040882687,
+            "velocityX": 0.8126964406159006,
+            "velocityY": 0.7794002249774918,
+            "timestamp": 0.5623896990477992
+        },
+        {
+            "x": 2.306850910186767,
+            "y": 6.379708766937256,
+            "heading": 7.831941019246608e-17,
+            "angularVelocity": 0.1804455301403977,
+            "velocityX": 1.20785328079034,
+            "velocityY": 0.10536750707073723,
+            "timestamp": 0.6561213155557658
+        },
+        {
+            "x": 2.454400050444712,
+            "y": 6.392580268241816,
+            "heading": 0.024770297736306776,
+            "angularVelocity": 0.3129674474840377,
+            "velocityX": 1.864252028642711,
+            "velocityY": 0.1626286834118752,
+            "timestamp": 0.7352678783546651
+        },
+        {
+            "x": 2.653900880802349,
+            "y": 6.4099837935603645,
+            "heading": 0.06003005706528144,
+            "angularVelocity": 0.445499565389401,
+            "velocityX": 2.520650591795653,
+            "velocityY": 0.2198898436406934,
+            "timestamp": 0.8144144411535644
+        },
+        {
+            "x": 2.905353386807521,
+            "y": 6.431919341632162,
+            "heading": 0.10577908294347244,
+            "angularVelocity": 0.5780292189622053,
+            "velocityX": 3.177048972348669,
+            "velocityY": 0.2771509879403426,
+            "timestamp": 0.8935610039524637
+        },
+        {
+            "x": 3.203627341926949,
+            "y": 6.457939375335675,
+            "heading": 0.10577908974967822,
+            "angularVelocity": 8.599496273628574e-8,
+            "velocityX": 3.768628030977189,
+            "velocityY": 0.3287575958241692,
+            "timestamp": 0.972707566751363
+        },
+        {
+            "x": 3.501901297052475,
+            "y": 6.48395940903972,
+            "heading": 0.10577909654992135,
+            "angularVelocity": 8.591962579346019e-8,
+            "velocityX": 3.7686280310542424,
+            "velocityY": 0.328757595830891,
+            "timestamp": 1.0518541295502624
+        },
+        {
+            "x": 3.8001752521780374,
+            "y": 6.509979442743767,
+            "heading": 0.10577910335027264,
+            "angularVelocity": 8.592099253060419e-8,
+            "velocityX": 3.768628031054697,
+            "velocityY": 0.32875759583093067,
+            "timestamp": 1.1310006923491618
+        },
+        {
+            "x": 4.0984492073035454,
+            "y": 6.53599947644781,
+            "heading": 0.1057791101506597,
+            "angularVelocity": 8.592144417290625e-8,
+            "velocityX": 3.7686280310540092,
+            "velocityY": 0.32875759583087066,
+            "timestamp": 1.2101472551480612
+        },
+        {
+            "x": 4.396723162417087,
+            "y": 6.562019510150809,
+            "heading": 0.10577911696012733,
+            "angularVelocity": 8.603617553663074e-8,
+            "velocityX": 3.7686280309028084,
+            "velocityY": 0.3287575958176806,
+            "timestamp": 1.2892938179469606
+        },
+        {
+            "x": 4.656482038612004,
+            "y": 6.5846796673565295,
+            "heading": 0.15819030641666595,
+            "angularVelocity": 0.6622042398696235,
+            "velocityX": 3.2819981943490006,
+            "velocityY": 0.28630627034678896,
+            "timestamp": 1.36844038074586
+        },
+        {
+            "x": 4.864289197548449,
+            "y": 6.6028077981790565,
+            "heading": 0.2001158851669991,
+            "angularVelocity": 0.5297207770962873,
+            "velocityX": 2.6255992880506356,
+            "velocityY": 0.22904508018355874,
+            "timestamp": 1.4475869435447595
+        },
+        {
+            "x": 5.020144593431014,
+            "y": 6.616403898623411,
+            "heading": 0.2315582663704034,
+            "angularVelocity": 0.39726780407754475,
+            "velocityX": 1.9691998031370395,
+            "velocityY": 0.17178383954462473,
+            "timestamp": 1.526733506343659
+        },
+        {
+            "x": 5.124048198578653,
+            "y": 6.625467966274828,
+            "heading": 0.2525193347871735,
+            "angularVelocity": 0.2648386446045577,
+            "velocityX": 1.3127999684793037,
+            "velocityY": 0.1145225683956373,
+            "timestamp": 1.6058800691425583
+        },
+        {
+            "x": 5.176000000000001,
             "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": 0.10270758607175808,
-            "velocityX": 0.6682718038134577,
-            "velocityY": 0.2443783760754632,
-            "timestamp": 1.1956969982058188
+            "heading": 0.26299999999999985,
+            "angularVelocity": 0.13242097751555232,
+            "velocityX": 0.6563999696784293,
+            "velocityY": 0.05726128292756526,
+            "timestamp": 1.6850266319414577
         },
         {
-            "x": 5.176,
+            "x": 5.176000000000001,
             "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": 1.86588452749739e-32,
-            "velocityX": -1.0770205854918045e-34,
-            "velocityY": 0,
-            "timestamp": 1.2811039266490916
+            "heading": 0.26299999999999996,
+            "angularVelocity": 1.6428599245730974e-17,
+            "velocityX": 1.9833104251084488e-16,
+            "velocityY": 1.8748049703629392e-17,
+            "timestamp": 1.7641731947403572
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.1.traj
new file mode 100644
index 00000000..96b0e72c
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.1.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3319036557469652,
+            "y": 5.56,
+            "heading": -1.815106931159875e-19,
+            "angularVelocity": -1.831407645203353e-18,
+            "velocityX": 0.8263919813283321,
+            "velocityY": 8.319303153097966e-34,
+            "timestamp": 0.099109935233537
+        },
+        {
+            "x": 2.4957109641637625,
+            "y": 5.56,
+            "heading": -5.438547016393454e-19,
+            "angularVelocity": -3.655980680963076e-18,
+            "velocityX": 1.6527839316089838,
+            "velocityY": 1.3987786573815764e-33,
+            "timestamp": 0.198219870467074
+        },
+        {
+            "x": 2.741421919096125,
+            "y": 5.56,
+            "heading": -1.0860329369861945e-18,
+            "angularVelocity": -5.4704731072674304e-18,
+            "velocityX": 2.4791758197942864,
+            "velocityY": 2.272018181833636e-33,
+            "timestamp": 0.297329805700611
+        },
+        {
+            "x": 3.0690365020812695,
+            "y": 5.56,
+            "heading": -1.80338172905721e-18,
+            "angularVelocity": -7.237910009033217e-18,
+            "velocityX": 3.3055675216936846,
+            "velocityY": 3.0574601440031788e-33,
+            "timestamp": 0.396439740934148
+        },
+        {
+            "x": 3.4439634979187304,
+            "y": 5.56,
+            "heading": 1.7928981709293245e-18,
+            "angularVelocity": 3.6285765816640505e-17,
+            "velocityX": 3.7829405796099502,
+            "velocityY": 2.997011014948752e-33,
+            "timestamp": 0.495549676167685
+        },
+        {
+            "x": 3.771578080903875,
+            "y": 5.56,
+            "heading": 1.0808328664707184e-18,
+            "angularVelocity": -7.184600643588333e-18,
+            "velocityX": 3.305567521693685,
+            "velocityY": -4.3766441461200414e-33,
+            "timestamp": 0.594659611401222
+        },
+        {
+            "x": 4.017289035836238,
+            "y": 5.56,
+            "heading": 5.416591039281022e-19,
+            "angularVelocity": -5.4401585595028644e-18,
+            "velocityX": 2.479175819794286,
+            "velocityY": -5.119323199602909e-33,
+            "timestamp": 0.6937695466347591
+        },
+        {
+            "x": 4.181096344253034,
+            "y": 5.56,
+            "heading": 1.8086099379948042e-19,
+            "angularVelocity": -3.64038286599245e-18,
+            "velocityX": 1.6527839316089838,
+            "velocityY": -5.9637817525889115e-33,
+            "timestamp": 0.792879481868296
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": -1.8248523050897816e-18,
+            "velocityX": 0.8263919813283321,
+            "velocityY": -3.8230331267939286e-33,
+            "timestamp": 0.891989417101833
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 2.0684896902092895e-40,
+            "timestamp": 0.9910993523353699
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.1.traj
new file mode 100644
index 00000000..7d72e43b
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.1.traj	
@@ -0,0 +1,526 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -4.99624931676796e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2447003077480563,
+            "y": 5.548424158968356,
+            "heading": -0.001651505925702968,
+            "angularVelocity": -0.042250731032121805,
+            "velocityX": -0.13558284496887443,
+            "velocityY": -0.2961465280181771,
+            "timestamp": 0.03908822132445813
+        },
+        {
+            "x": 2.234100924566356,
+            "y": 5.525272476708423,
+            "heading": -0.0049545293965937804,
+            "angularVelocity": -0.08450175932731885,
+            "velocityX": -0.27116565611205723,
+            "velocityY": -0.5922930610671583,
+            "timestamp": 0.07817644264891627
+        },
+        {
+            "x": 2.2182018518980375,
+            "y": 5.490544953203296,
+            "heading": -0.009909100000268336,
+            "angularVelocity": -0.12675354456654908,
+            "velocityX": -0.4067484303351874,
+            "velocityY": -0.8884395945486457,
+            "timestamp": 0.1172646639733744
+        },
+        {
+            "x": 2.1970030913105703,
+            "y": 5.444241588640874,
+            "heading": -0.016515264878529855,
+            "angularVelocity": -0.16900653584180803,
+            "velocityX": -0.5423311644575062,
+            "velocityY": -1.1845861232230646,
+            "timestamp": 0.15635288529783253
+        },
+        {
+            "x": 2.170504644501127,
+            "y": 5.386362383452732,
+            "heading": -0.024773088013563174,
+            "angularVelocity": -0.21126116398317848,
+            "velocityX": -0.6779138551608025,
+            "velocityY": -1.480732640856411,
+            "timestamp": 0.19544110662229067
+        },
+        {
+            "x": 2.1387065133083243,
+            "y": 5.31690733837683,
+            "heading": -0.03468264893636917,
+            "angularVelocity": -0.2535178267784212,
+            "velocityX": -0.8134964988264785,
+            "velocityY": -1.7768791396104282,
+            "timestamp": 0.2345293279467488
+        },
+        {
+            "x": 2.1016086997403236,
+            "y": 5.235876454566401,
+            "heading": -0.04624404029676127,
+            "angularVelocity": -0.2957768598481422,
+            "velocityX": -0.9490790911170489,
+            "velocityY": -2.0730256088611454,
+            "timestamp": 0.27361754927120696
+        },
+        {
+            "x": 2.0592112060450187,
+            "y": 5.143269733801358,
+            "heading": -0.05945736298631538,
+            "angularVelocity": -0.33803847404262444,
+            "velocityX": -1.0846616258994186,
+            "velocityY": -2.369172032575987,
+            "timestamp": 0.31270577059566507
+        },
+        {
+            "x": 2.011514034901109,
+            "y": 5.039087178972333,
+            "heading": -0.07432271508161363,
+            "angularVelocity": -0.3803025973454465,
+            "velocityX": -1.2202440921521842,
+            "velocityY": -2.6653183823394446,
+            "timestamp": 0.3517939919201232
+        },
+        {
+            "x": 1.9585171900495353,
+            "y": 4.923328795522167,
+            "heading": -0.09084016031123929,
+            "angularVelocity": -0.4225683510264515,
+            "velocityX": -1.3558264626999135,
+            "velocityY": -2.9614645928578263,
+            "timestamp": 0.3908822132445813
+        },
+        {
+            "x": 1.9002206796290688,
+            "y": 4.795994598697166,
+            "heading": -0.10900958241827326,
+            "angularVelocity": -0.46483113048874997,
+            "velocityX": -1.491408625032185,
+            "velocityY": -3.257610413327559,
+            "timestamp": 0.4299704345690394
+        },
+        {
+            "x": 1.8386671955063139,
+            "y": 4.661546724325594,
+            "heading": -0.10900958703871873,
+            "angularVelocity": -1.182055697467884e-7,
+            "velocityX": -1.5747322860207698,
+            "velocityY": -3.439600724104782,
+            "timestamp": 0.4690586558934975
+        },
+        {
+            "x": 1.7777943297173011,
+            "y": 4.526789325478927,
+            "heading": -0.10900959166091434,
+            "angularVelocity": -1.1825034389504455e-7,
+            "velocityX": -1.5573199221351706,
+            "velocityY": -3.447519336526256,
+            "timestamp": 0.5081468772179556
+        },
+        {
+            "x": 1.728739704549767,
+            "y": 4.38729485969642,
+            "heading": -0.10900959636164997,
+            "angularVelocity": -1.2025964513922604e-7,
+            "velocityX": -1.2549720479816096,
+            "velocityY": -3.5687084511883413,
+            "timestamp": 0.5472350985424137
+        },
+        {
+            "x": 1.6918738865194354,
+            "y": 4.244095790476986,
+            "heading": -0.10900960128999353,
+            "angularVelocity": -1.2608257379035844e-7,
+            "velocityX": -0.9431439134643449,
+            "velocityY": -3.6634838927766116,
+            "timestamp": 0.5863233198668718
+        },
+        {
+            "x": 1.6674707596733624,
+            "y": 4.098254956364247,
+            "heading": -0.10900960662650709,
+            "angularVelocity": -1.3652485001388814e-7,
+            "velocityX": -0.6243089610931135,
+            "velocityY": -3.7310685718382324,
+            "timestamp": 0.6254115411913299
+        },
+        {
+            "x": 1.6557115284805652,
+            "y": 3.9508548842430677,
+            "heading": -0.1090096126146074,
+            "angularVelocity": -1.531944945007552e-7,
+            "velocityX": -0.3008382268193907,
+            "velocityY": -3.7709587985000237,
+            "timestamp": 0.664499762515788
+        },
+        {
+            "x": 1.6566835066189385,
+            "y": 3.8029896922639588,
+            "heading": -0.10900961961577929,
+            "angularVelocity": -1.79112061302596e-7,
+            "velocityX": 0.024866266754091815,
+            "velocityY": -3.7828580316236344,
+            "timestamp": 0.7035879838402461
+        },
+        {
+            "x": 1.6703794917330834,
+            "y": 3.6557569809887474,
+            "heading": -0.10900969829200789,
+            "angularVelocity": -0.000002012786100988185,
+            "velocityX": 0.3503865013576149,
+            "velocityY": -3.766677180142832,
+            "timestamp": 0.7426762051647042
+        },
+        {
+            "x": 1.6964297342683634,
+            "y": 3.5114772969924033,
+            "heading": -0.1124967438020359,
+            "angularVelocity": -0.0892096235606636,
+            "velocityX": 0.6664473760284892,
+            "velocityY": -3.6911294274233937,
+            "timestamp": 0.7817644264891623
+        },
+        {
+            "x": 1.7337062034817727,
+            "y": 3.3729422790966246,
+            "heading": -0.12243166775651844,
+            "angularVelocity": -0.2541666931326176,
+            "velocityX": 0.9536496660724848,
+            "velocityY": -3.54416274779685,
+            "timestamp": 0.8208526478136204
+        },
+        {
+            "x": 1.7809992442628195,
+            "y": 3.2421845220219927,
+            "heading": -0.1367067464310455,
+            "angularVelocity": -0.3652015413039525,
+            "velocityX": 1.2099051626962352,
+            "velocityY": -3.3451958836719715,
+            "timestamp": 0.8599408691380785
+        },
+        {
+            "x": 1.8374227958909133,
+            "y": 3.120270451905998,
+            "heading": -0.154016140064332,
+            "angularVelocity": -0.4428288892862246,
+            "velocityX": 1.443492431128254,
+            "velocityY": -3.118946475052685,
+            "timestamp": 0.8990290904625367
+        },
+        {
+            "x": 1.9023445230612375,
+            "y": 3.007828092475748,
+            "heading": -0.17349166731783877,
+            "angularVelocity": -0.4982454200677405,
+            "velocityX": 1.660902567846671,
+            "velocityY": -2.8766302384778886,
+            "timestamp": 0.9381173117869948
+        },
+        {
+            "x": 1.9753011558084586,
+            "y": 2.90526251520286,
+            "heading": -0.19452421295735298,
+            "angularVelocity": -0.5380788617862533,
+            "velocityX": 1.866460797529241,
+            "velocityY": -2.623950995915806,
+            "timestamp": 0.9772055331114529
+        },
+        {
+            "x": 2.0559421744907826,
+            "y": 2.8128534355639654,
+            "heading": -0.21666661660279088,
+            "angularVelocity": -0.5664725304751266,
+            "velocityX": 2.0630516291077963,
+            "velocityY": -2.3641157491367464,
+            "timestamp": 1.016293754435911
+        },
+        {
+            "x": 2.1439944855858766,
+            "y": 2.7308042438703497,
+            "heading": -0.2395781801271676,
+            "angularVelocity": -0.586150066389446,
+            "velocityX": 2.252655866948419,
+            "velocityY": -2.099077136627731,
+            "timestamp": 1.055381975760369
+        },
+        {
+            "x": 2.2392399311065674,
+            "y": 2.659268856048584,
+            "heading": -0.2629913490343549,
+            "angularVelocity": -0.5989827143282399,
+            "velocityX": 2.4366789353272003,
+            "velocityY": -1.830100869209989,
+            "timestamp": 1.0944701970848272
+        },
+        {
+            "x": 2.3166964235934535,
+            "y": 2.610513248595422,
+            "heading": -0.2811797949710667,
+            "angularVelocity": -0.6046182408202432,
+            "velocityX": 2.5747998696792043,
+            "velocityY": -1.620728330008343,
+            "timestamp": 1.124552726371468
+        },
+        {
+            "x": 2.398213254515867,
+            "y": 2.5681176642029904,
+            "heading": -0.29946946527559826,
+            "angularVelocity": -0.6079831296849904,
+            "velocityX": 2.7097731758410535,
+            "velocityY": -1.409309170398157,
+            "timestamp": 1.154635255658109
+        },
+        {
+            "x": 2.483678189488588,
+            "y": 2.5321524360945276,
+            "heading": -0.31778023201379,
+            "angularVelocity": -0.6086844149213316,
+            "velocityX": 2.8410155993982453,
+            "velocityY": -1.1955520018204469,
+            "timestamp": 1.1847177849447499
+        },
+        {
+            "x": 2.5729562458095865,
+            "y": 2.5026984837196036,
+            "heading": -0.33601688083426684,
+            "angularVelocity": -0.6062205955726221,
+            "velocityX": 2.9677709434035733,
+            "velocityY": -0.9791049181495252,
+            "timestamp": 1.2148003142313908
+        },
+        {
+            "x": 2.665882293188928,
+            "y": 2.479849558811985,
+            "heading": -0.35406456410202075,
+            "angularVelocity": -0.5999390242685017,
+            "velocityX": 3.08903704518594,
+            "velocityY": -0.7595413500609144,
+            "timestamp": 1.2448828435180317
+        },
+        {
+            "x": 2.7622502226261916,
+            "y": 2.4637149411085275,
+            "heading": -0.37178230702948656,
+            "angularVelocity": -0.5889711851900935,
+            "velocityX": 3.2034516951353296,
+            "velocityY": -0.5363451174508257,
+            "timestamp": 1.2749653728046726
+        },
+        {
+            "x": 2.8617965704136163,
+            "y": 2.4544224370162078,
+            "heading": -0.3889934776908366,
+            "angularVelocity": -0.57213176782302,
+            "velocityX": 3.309108314626244,
+            "velocityY": -0.30890035886875983,
+            "timestamp": 1.3050479020913135
+        },
+        {
+            "x": 2.96417486527032,
+            "y": 2.452120989561553,
+            "heading": -0.40547141981393114,
+            "angularVelocity": -0.5477578685651567,
+            "velocityX": 3.4032475754010525,
+            "velocityY": -0.07650445322366382,
+            "timestamp": 1.3351304313779544
+        },
+        {
+            "x": 3.068913958464018,
+            "y": 2.456980552046256,
+            "heading": -0.4209173641629785,
+            "angularVelocity": -0.5134523165215639,
+            "velocityX": 3.4817249638713506,
+            "velocityY": 0.16154102065001244,
+            "timestamp": 1.3652129606645953
+        },
+        {
+            "x": 3.175348297900835,
+            "y": 2.469181868617686,
+            "heading": -0.43492680962689906,
+            "angularVelocity": -0.4657003847794821,
+            "velocityX": 3.5380781457121575,
+            "velocityY": 0.4055947708108251,
+            "timestamp": 1.3952954899512362
+        },
+        {
+            "x": 3.2825012821379658,
+            "y": 2.488873812540701,
+            "heading": -0.44694408076253883,
+            "angularVelocity": -0.3994767534718848,
+            "velocityX": 3.5619672540204803,
+            "velocityY": 0.6545973490252659,
+            "timestamp": 1.425378019237877
+        },
+        {
+            "x": 3.3889723525651156,
+            "y": 2.516050236824646,
+            "heading": -0.4565561347315361,
+            "angularVelocity": -0.3195227993435668,
+            "velocityX": 3.539299153094488,
+            "velocityY": 0.9033955896790181,
+            "timestamp": 1.455460548524518
+        },
+        {
+            "x": 3.4932758480756925,
+            "y": 2.5504542246131905,
+            "heading": -0.4662703499641935,
+            "angularVelocity": -0.32291883239253294,
+            "velocityX": 3.4672448754798637,
+            "velocityY": 1.1436534295611822,
+            "timestamp": 1.485543077811159
+        },
+        {
+            "x": 3.5930108067726363,
+            "y": 2.5908608883384305,
+            "heading": -0.4766346317588353,
+            "angularVelocity": -0.3445282707409342,
+            "velocityX": 3.315378096922126,
+            "velocityY": 1.3431936969204072,
+            "timestamp": 1.5156256070977998
+        },
+        {
+            "x": 3.686096614652251,
+            "y": 2.634834387797071,
+            "heading": -0.48720467819938723,
+            "angularVelocity": -0.3513682755805826,
+            "velocityX": 3.094347785475507,
+            "velocityY": 1.4617620426674243,
+            "timestamp": 1.5457081363844407
+        },
+        {
+            "x": 3.771715868998783,
+            "y": 2.679896340499022,
+            "heading": -0.4975881116213702,
+            "angularVelocity": -0.34516490694787005,
+            "velocityX": 2.8461454663839936,
+            "velocityY": 1.497944280967274,
+            "timestamp": 1.5757906656710816
+        },
+        {
+            "x": 3.8498203373891897,
+            "y": 2.724287723719235,
+            "heading": -0.507530883254748,
+            "angularVelocity": -0.33051647814035945,
+            "velocityX": 2.596339810598824,
+            "velocityY": 1.475653286903962,
+            "timestamp": 1.6058731949577225
+        },
+        {
+            "x": 3.920599661686896,
+            "y": 2.7668759161025953,
+            "heading": -0.5168796397769617,
+            "angularVelocity": -0.3107702957131982,
+            "velocityX": 2.35283820796086,
+            "velocityY": 1.415711823216824,
+            "timestamp": 1.6359557242443634
+        },
+        {
+            "x": 3.984278053809844,
+            "y": 2.806919241781621,
+            "heading": -0.525538519539268,
+            "angularVelocity": -0.287837491315874,
+            "velocityX": 2.11678983227083,
+            "velocityY": 1.3311156551191636,
+            "timestamp": 1.6660382535310043
+        },
+        {
+            "x": 4.041059171114166,
+            "y": 2.843909698702473,
+            "heading": -0.5334440710807248,
+            "angularVelocity": -0.26279544070665484,
+            "velocityX": 1.8875114111345854,
+            "velocityY": 1.229632540814369,
+            "timestamp": 1.6961207828176452
+        },
+        {
+            "x": 4.091116341717457,
+            "y": 2.8774832028649056,
+            "heading": -0.5405519914461838,
+            "angularVelocity": -0.23628067632639732,
+            "velocityX": 1.6639947434713853,
+            "velocityY": 1.11604658778956,
+            "timestamp": 1.7262033121042861
+        },
+        {
+            "x": 4.1345948250930045,
+            "y": 2.9073683770064043,
+            "heading": -0.5468299348831553,
+            "angularVelocity": -0.20869067813911088,
+            "velocityX": 1.445306774615446,
+            "velocityY": 0.9934395428236782,
+            "timestamp": 1.756285841390927
+        },
+        {
+            "x": 4.171616484177909,
+            "y": 2.933356232054327,
+            "heading": -0.552253409487953,
+            "angularVelocity": -0.18028652288901045,
+            "velocityX": 1.2306697595852323,
+            "velocityY": 0.863885306993297,
+            "timestamp": 1.786368370677568
+        },
+        {
+            "x": 4.202284260430688,
+            "y": 2.9552814196152344,
+            "heading": -0.5568033106587852,
+            "angularVelocity": -0.15124729464987938,
+            "velocityX": 1.0194547127524736,
+            "velocityY": 0.7288345787681281,
+            "timestamp": 1.8164508999642088
+        },
+        {
+            "x": 4.226685878711894,
+            "y": 2.973010149818714,
+            "heading": -0.5604643670760648,
+            "angularVelocity": -0.12170041895060278,
+            "velocityX": 0.8111558057069341,
+            "velocityY": 0.5893364229633967,
+            "timestamp": 1.8465334292508497
+        },
+        {
+            "x": 4.244896778926994,
+            "y": 2.98643211128414,
+            "heading": -0.5632241207030344,
+            "angularVelocity": -0.09173941461743668,
+            "velocityX": 0.6053646633758383,
+            "velocityY": 0.4461713088529227,
+            "timestamp": 1.8766159585374906
+        },
+        {
+            "x": 4.256982406189367,
+            "y": 2.9954548894256625,
+            "heading": -0.565072233243941,
+            "angularVelocity": -0.06143474583855434,
+            "velocityX": 0.4017490400230914,
+            "velocityY": 0.2999341596429636,
+            "timestamp": 1.9066984878241315
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -0.030840716457660636,
+            "velocityX": 0.20003616561938206,
+            "velocityY": 0.15108804618886046,
+            "timestamp": 1.9367810171107724
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": -4.9732030554004765e-30,
+            "velocityX": -4.223945033236224e-32,
+            "velocityY": 9.969983495378875e-32,
+            "timestamp": 1.9668635463974133
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.1.traj
new file mode 100644
index 00000000..848b8926
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.1.traj	
@@ -0,0 +1,553 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 1.4317938010297452e-32,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2473468901462774,
+            "y": 5.551784954334613,
+            "heading": -0.004046039178700474,
+            "angularVelocity": -0.12521248615625988,
+            "velocityX": -0.08210560159156222,
+            "velocityY": -0.2542304328304919,
+            "timestamp": 0.032313384255078216
+        },
+        {
+            "x": 2.2420392078855844,
+            "y": 5.535352460743813,
+            "heading": -0.012062241078351911,
+            "angularVelocity": -0.24807682898124367,
+            "velocityX": -0.16425646471427996,
+            "velocityY": -0.5085352082308542,
+            "timestamp": 0.06462676851015643
+        },
+        {
+            "x": 2.234075272195187,
+            "y": 5.510699784095841,
+            "heading": -0.023960373728225737,
+            "angularVelocity": -0.36821066329520014,
+            "velocityX": -0.24645935032774877,
+            "velocityY": -0.7629246275588465,
+            "timestamp": 0.09694015276523466
+        },
+        {
+            "x": 2.2234531308615284,
+            "y": 5.477823761292619,
+            "heading": -0.03963572188039383,
+            "angularVelocity": -0.4851038822931769,
+            "velocityX": -0.32872265095504705,
+            "velocityY": -1.017411935057708,
+            "timestamp": 0.12925353702031286
+        },
+        {
+            "x": 2.2101704924088987,
+            "y": 5.436720678146679,
+            "heading": -0.05896132607089341,
+            "angularVelocity": -0.5980680958065965,
+            "velocityX": -0.4110568657178767,
+            "velocityY": -1.27201418525146,
+            "timestamp": 0.16156692127539107
+        },
+        {
+            "x": 2.1942246366351648,
+            "y": 5.38738610923438,
+            "heading": -0.08177969135834087,
+            "angularVelocity": -0.7061583245915709,
+            "velocityX": -0.4934752623822669,
+            "velocityY": -1.5267533887152571,
+            "timestamp": 0.19388030553046928
+        },
+        {
+            "x": 2.1756122932404036,
+            "y": 5.329814710705053,
+            "heading": -0.10789037911156549,
+            "angularVelocity": -0.8080455933402793,
+            "velocityX": -0.5759948647853634,
+            "velocityY": -1.7816579679449487,
+            "timestamp": 0.2261936897855475
+        },
+        {
+            "x": 2.1543294701171383,
+            "y": 5.263999960188385,
+            "heading": -0.1370304714941401,
+            "angularVelocity": -0.9017963625397062,
+            "velocityX": -0.6586380106540685,
+            "velocityY": -2.0367643945039555,
+            "timestamp": 0.25850707404062573
+        },
+        {
+            "x": 2.1303711997350336,
+            "y": 5.1899338649176165,
+            "heading": -0.1688416775308267,
+            "angularVelocity": -0.9844591264590365,
+            "velocityX": -0.7414348863300811,
+            "velocityY": -2.2921181726463207,
+            "timestamp": 0.29082045829570397
+        },
+        {
+            "x": 2.1037311567595207,
+            "y": 5.107606785885568,
+            "heading": -0.20280965108227242,
+            "angularVelocity": -1.051204457054369,
+            "velocityX": -0.8244275116843016,
+            "velocityY": -2.5477702484570597,
+            "timestamp": 0.3231338425507822
+        },
+        {
+            "x": 2.074401135228672,
+            "y": 5.017008143765364,
+            "heading": -0.23813648207052604,
+            "angularVelocity": -1.0932569213236978,
+            "velocityX": -0.9076740863575303,
+            "velocityY": -2.8037497219427316,
+            "timestamp": 0.35544722680586044
+        },
+        {
+            "x": 2.0423710011934597,
+            "y": 4.918132506074763,
+            "heading": -0.27341197795784133,
+            "angularVelocity": -1.0916682576129122,
+            "velocityX": -0.9912342756292476,
+            "velocityY": -3.0598973140692034,
+            "timestamp": 0.3877606110609387
+        },
+        {
+            "x": 2.00763899967256,
+            "y": 4.811032729430715,
+            "heading": -0.3053550995537523,
+            "angularVelocity": -0.9885415078704507,
+            "velocityX": -1.0748487761829064,
+            "velocityY": -3.3144091562374,
+            "timestamp": 0.4200739953160169
+        },
+        {
+            "x": 1.9707337568018743,
+            "y": 4.697468766706309,
+            "heading": -0.3150299299334663,
+            "angularVelocity": -0.2994062863655037,
+            "velocityX": -1.1421039213769328,
+            "velocityY": -3.514455862250325,
+            "timestamp": 0.45238737957109515
+        },
+        {
+            "x": 1.9331993710024595,
+            "y": 4.581134395027877,
+            "heading": -0.3150299515537849,
+            "angularVelocity": -6.690824588937528e-7,
+            "velocityX": -1.1615739627617825,
+            "velocityY": -3.6001915107405935,
+            "timestamp": 0.4847007638261734
+        },
+        {
+            "x": 1.8956658385726595,
+            "y": 4.464799748019651,
+            "heading": -0.3150299731737692,
+            "angularVelocity": -6.690721139443934e-7,
+            "velocityX": -1.1615475535931208,
+            "velocityY": -3.600200031352131,
+            "timestamp": 0.5170141480812516
+        },
+        {
+            "x": 1.8630740613066095,
+            "y": 4.346985096208152,
+            "heading": -0.31502999489570194,
+            "angularVelocity": -6.722271053919432e-7,
+            "velocityX": -1.0086154086732113,
+            "velocityY": -3.64600163453891,
+            "timestamp": 0.5493275323363298
+        },
+        {
+            "x": 1.8389498768606893,
+            "y": 4.227149626761845,
+            "heading": -0.3150300170939308,
+            "angularVelocity": -6.869670074324573e-7,
+            "velocityX": -0.7465694170405304,
+            "velocityY": -3.708539733886721,
+            "timestamp": 0.581640916591408
+        },
+        {
+            "x": 1.8234162959667812,
+            "y": 4.10590102408459,
+            "heading": -0.31503004024876186,
+            "angularVelocity": -7.165709048863888e-7,
+            "velocityX": -0.4807166210536207,
+            "velocityY": -3.7522718672898008,
+            "timestamp": 0.6139543008464863
+        },
+        {
+            "x": 1.816552246405149,
+            "y": 3.98385431066073,
+            "heading": -0.315030064931292,
+            "angularVelocity": -7.63848502164674e-7,
+            "velocityX": -0.21242125267501,
+            "velocityY": -3.776970943694307,
+            "timestamp": 0.6462676851015645
+        },
+        {
+            "x": 1.818392595454208,
+            "y": 3.861628584870307,
+            "heading": -0.315030091871416,
+            "angularVelocity": -8.337140982566696e-7,
+            "velocityX": 0.05695315088421205,
+            "velocityY": -3.782510826646546,
+            "timestamp": 0.6785810693566428
+        },
+        {
+            "x": 1.828928030254088,
+            "y": 3.739843861498511,
+            "heading": -0.3150301220653929,
+            "angularVelocity": -9.344108498281474e-7,
+            "velocityX": 0.32603935003261814,
+            "velocityY": -3.7688631562216113,
+            "timestamp": 0.710894453611721
+        },
+        {
+            "x": 1.8481051197294465,
+            "y": 3.619117921789427,
+            "heading": -0.3150301569615331,
+            "angularVelocity": -0.0000010799283629257,
+            "velocityX": 0.593472021499706,
+            "velocityY": -3.7360970536569806,
+            "timestamp": 0.7432078378667992
+        },
+        {
+            "x": 1.875826590719106,
+            "y": 3.500063178055521,
+            "heading": -0.31503019880828736,
+            "angularVelocity": -0.0000012950285239246719,
+            "velocityX": 0.8578943873792161,
+            "velocityY": -3.684378670896923,
+            "timestamp": 0.7755212221218775
+        },
+        {
+            "x": 1.9119518234377388,
+            "y": 3.3832835666467993,
+            "heading": -0.31503025136937535,
+            "angularVelocity": -0.0000016266042432729645,
+            "velocityX": 1.1179650027822445,
+            "velocityY": -3.61397031294762,
+            "timestamp": 0.8078346063769557
+        },
+        {
+            "x": 1.9562975654606443,
+            "y": 3.269371484637439,
+            "heading": -0.31503032157308203,
+            "angularVelocity": -0.0000021725891085409373,
+            "velocityX": 1.3723645184560365,
+            "velocityY": -3.5252290849559675,
+            "timestamp": 0.8401479906320339
+        },
+        {
+            "x": 2.008638858795166,
+            "y": 3.158904790878296,
+            "heading": -0.3150304332740777,
+            "angularVelocity": -0.0000034568027529519608,
+            "velocityX": 1.6198022751608165,
+            "velocityY": -3.4186049002831713,
+            "timestamp": 0.8724613748871122
+        },
+        {
+            "x": 2.0775165641117233,
+            "y": 3.0394044472847606,
+            "heading": -0.3150305189714284,
+            "angularVelocity": -0.000002350392551393865,
+            "velocityX": 1.8890857675343722,
+            "velocityY": -3.277495922091173,
+            "timestamp": 0.9089222420961518
+        },
+        {
+            "x": 2.155767726009908,
+            "y": 2.9258208622381168,
+            "heading": -0.3150305770370975,
+            "angularVelocity": -0.00000159254767063084,
+            "velocityX": 2.1461684235196876,
+            "velocityY": -3.1152189660064753,
+            "timestamp": 0.9453831093051914
+        },
+        {
+            "x": 2.242886953120307,
+            "y": 2.818887620265158,
+            "heading": -0.3150306203630764,
+            "angularVelocity": -0.0000011882871183876306,
+            "velocityX": 2.389389879591231,
+            "velocityY": -2.932822232666113,
+            "timestamp": 0.981843976514231
+        },
+        {
+            "x": 2.338311580267833,
+            "y": 2.719295349641276,
+            "heading": -0.3150306549359537,
+            "angularVelocity": -9.482187337248934e-7,
+            "velocityX": 2.617179306252704,
+            "velocityY": -2.731483868798106,
+            "timestamp": 1.0183048437232707
+        },
+        {
+            "x": 2.4414253013619946,
+            "y": 2.6276872624602983,
+            "heading": -0.31503068395926054,
+            "angularVelocity": -7.960125215618009e-7,
+            "velocityX": 2.8280655120730875,
+            "velocityY": -2.512504342142078,
+            "timestamp": 1.0547657109323103
+        },
+        {
+            "x": 2.5515621486647997,
+            "y": 2.5446549983777076,
+            "heading": -0.3150307093352344,
+            "angularVelocity": -6.959783403374032e-7,
+            "velocityX": 3.020686443670201,
+            "velocityY": -2.2772981127010774,
+            "timestamp": 1.09122657814135
+        },
+        {
+            "x": 2.6680107915990137,
+            "y": 2.4707347968336117,
+            "heading": -0.31503073229503964,
+            "angularVelocity": -6.297108928988803e-7,
+            "velocityX": 3.1937979496369038,
+            "velocityY": -2.0273846236374995,
+            "timestamp": 1.1276874453503896
+        },
+        {
+            "x": 2.7900191236562386,
+            "y": 2.4064040113359084,
+            "heading": -0.3150307537024253,
+            "angularVelocity": -5.871332010760787e-7,
+            "velocityX": 3.3462816821585726,
+            "velocityY": -1.764378919702536,
+            "timestamp": 1.1641483125594292
+        },
+        {
+            "x": 2.9167990841955884,
+            "y": 2.352077913506683,
+            "heading": -0.3150307742165802,
+            "angularVelocity": -5.626348607586924e-7,
+            "velocityX": 3.477151539278742,
+            "velocityY": -1.4899836999970275,
+            "timestamp": 1.2006091797684688
+        },
+        {
+            "x": 3.0475311021850517,
+            "y": 2.308104934680324,
+            "heading": -0.31503079438993686,
+            "angularVelocity": -5.532878997498012e-7,
+            "velocityX": 3.585543296047864,
+            "velocityY": -1.2060321707174542,
+            "timestamp": 1.2370700469775084
+        },
+        {
+            "x": 3.178817728061225,
+            "y": 2.2658165761507623,
+            "heading": -0.3150308145386853,
+            "angularVelocity": -5.526129783451472e-7,
+            "velocityX": 3.600754340906743,
+            "velocityY": -1.159828653748452,
+            "timestamp": 1.273530914186548
+        },
+        {
+            "x": 3.3101043979960063,
+            "y": 2.2235283544039026,
+            "heading": -0.3150308346874411,
+            "angularVelocity": -5.526131795163358e-7,
+            "velocityX": 3.600755549287413,
+            "velocityY": -1.1598249022550946,
+            "timestamp": 1.3099917813955877
+        },
+        {
+            "x": 3.4413910762638236,
+            "y": 2.1812401585275256,
+            "heading": -0.31503085483620696,
+            "angularVelocity": -5.52613457009699e-7,
+            "velocityX": 3.60075577783481,
+            "velocityY": -1.159824192714005,
+            "timestamp": 1.3464526486046273
+        },
+        {
+            "x": 3.572677762945226,
+            "y": 2.138951988771722,
+            "heading": -0.31503087498498206,
+            "angularVelocity": -5.52613710159953e-7,
+            "velocityX": 3.6007560085914068,
+            "velocityY": -1.1598234763137998,
+            "timestamp": 1.382913515813667
+        },
+        {
+            "x": 3.7039644581047573,
+            "y": 2.0966638453368867,
+            "heading": -0.31503089513376514,
+            "angularVelocity": -5.526139295643368e-7,
+            "velocityX": 3.6007562411181984,
+            "velocityY": -1.1598227544173942,
+            "timestamp": 1.4193743830227066
+        },
+        {
+            "x": 3.8352511618245644,
+            "y": 2.05437572847807,
+            "heading": -0.3150309152825546,
+            "angularVelocity": -5.526141044747151e-7,
+            "velocityX": 3.600756475898009,
+            "velocityY": -1.1598220255258234,
+            "timestamp": 1.4558352502317462
+        },
+        {
+            "x": 3.966537874250263,
+            "y": 2.01208763864737,
+            "heading": -0.3150309354313486,
+            "angularVelocity": -5.526142273042023e-7,
+            "velocityX": 3.6007567146716046,
+            "velocityY": -1.159821284234734,
+            "timestamp": 1.4922961174407858
+        },
+        {
+            "x": 4.097824595645338,
+            "y": 1.9697995766628096,
+            "heading": -0.3150309555801447,
+            "angularVelocity": -5.526142864672618e-7,
+            "velocityX": 3.600756960671674,
+            "velocityY": -1.159820520507965,
+            "timestamp": 1.5287569846498255
+        },
+        {
+            "x": 4.229111326453571,
+            "y": 1.9275115439021684,
+            "heading": -0.3150309757289402,
+            "angularVelocity": -5.5261426880692e-7,
+            "velocityX": 3.600757218843208,
+            "velocityY": -1.1598197189933317,
+            "timestamp": 1.565217851858865
+        },
+        {
+            "x": 4.360398019008411,
+            "y": 1.885223392382287,
+            "heading": -0.31503099587881916,
+            "angularVelocity": -5.52643987126892e-7,
+            "velocityX": 3.6007561696802117,
+            "velocityY": -1.1598229761632481,
+            "timestamp": 1.6016787190679047
+        },
+        {
+            "x": 4.485161526388635,
+            "y": 1.844329974236374,
+            "heading": -0.3370638509456068,
+            "angularVelocity": -0.6042877406197901,
+            "velocityX": 3.4218469534726492,
+            "velocityY": -1.1215700907896822,
+            "timestamp": 1.6381395862769443
+        },
+        {
+            "x": 4.60044458089581,
+            "y": 1.8067761222092775,
+            "heading": -0.3770388194107599,
+            "angularVelocity": -1.0963800788381923,
+            "velocityX": 3.1618297460185825,
+            "velocityY": -1.0299769287381584,
+            "timestamp": 1.674600453485984
+        },
+        {
+            "x": 4.705219804759057,
+            "y": 1.772711102552083,
+            "heading": -0.4189139196427851,
+            "angularVelocity": -1.148494356756437,
+            "velocityX": 2.8736349923479074,
+            "velocityY": -0.9342898911836356,
+            "timestamp": 1.7110613206950236
+        },
+        {
+            "x": 4.799468970200479,
+            "y": 1.7421012617230478,
+            "heading": -0.45948472427125614,
+            "angularVelocity": -1.1127218778387968,
+            "velocityX": 2.5849403115144614,
+            "velocityY": -0.8395258580532651,
+            "timestamp": 1.7475221879040632
+        },
+        {
+            "x": 4.883206648654635,
+            "y": 1.7149242324789875,
+            "heading": -0.49729979894596704,
+            "angularVelocity": -1.037141394852447,
+            "velocityX": 2.296645276538986,
+            "velocityY": -0.7453752838144925,
+            "timestamp": 1.7839830551131028
+        },
+        {
+            "x": 4.956447680828415,
+            "y": 1.6911655730288653,
+            "heading": -0.5315269969459382,
+            "angularVelocity": -0.9387379023033031,
+            "velocityX": 2.0087572726635705,
+            "velocityY": -0.6516207997442213,
+            "timestamp": 1.8204439223221425
+        },
+        {
+            "x": 5.019204214404415,
+            "y": 1.6708154773305386,
+            "heading": -0.5616288277349688,
+            "angularVelocity": -0.825592836737831,
+            "velocityX": 1.7212024392124468,
+            "velocityY": -0.5581352627093101,
+            "timestamp": 1.856904789531182
+        },
+        {
+            "x": 5.071485921099441,
+            "y": 1.6538669506708767,
+            "heading": -0.587231204588931,
+            "angularVelocity": -0.7021878198118158,
+            "velocityX": 1.4339128687006073,
+            "velocityY": -0.46484156733002446,
+            "timestamp": 1.8933656567402217
+        },
+        {
+            "x": 5.11330055140079,
+            "y": 1.6403147844077772,
+            "heading": -0.6080594902506735,
+            "angularVelocity": -0.5712504187662296,
+            "velocityX": 1.1468358682094448,
+            "velocityY": -0.37169072763413913,
+            "timestamp": 1.9298265239492614
+        },
+        {
+            "x": 5.1446544158470235,
+            "y": 1.6301549523614358,
+            "heading": -0.6239033665029361,
+            "angularVelocity": -0.43454469037794713,
+            "velocityX": 0.859931944747064,
+            "velocityY": -0.2786503126240163,
+            "timestamp": 1.966287391158301
+        },
+        {
+            "x": 5.165552752490355,
+            "y": 1.6233842390024875,
+            "heading": -0.6345958555798376,
+            "angularVelocity": -0.29325931869912014,
+            "velocityX": 0.5731716835893133,
+            "velocityY": -0.18569808886140712,
+            "timestamp": 2.0027482583673404
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -0.14821766002380998,
+            "velocityX": 0.2865331603263514,
+            "velocityY": -0.09281839027812194,
+            "timestamp": 2.03920912557638
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 2.046404654845657e-31,
+            "velocityX": 2.793064044552828e-33,
+            "velocityY": 5.221456528298987e-32,
+            "timestamp": 2.0756699927854196
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj
new file mode 100644
index 00000000..35bba061
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": -5.451838641793523e-34,
+            "angularVelocity": -2.0131697388730357e-33,
+            "velocityX": -3.0924959567962797e-34,
+            "velocityY": 4.789643826321005e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2520020156777094,
+            "y": 5.6078780285533805,
+            "heading": 0.01582108665485387,
+            "angularVelocity": 0.208254594271014,
+            "velocityX": 0.026352738707599337,
+            "velocityY": 0.630223424496655,
+            "timestamp": 0.07596992858781765
+        },
+        {
+            "x": 2.2560055471462697,
+            "y": 5.703634194455967,
+            "heading": 0.04745824180691435,
+            "angularVelocity": 0.4164431340157103,
+            "velocityX": 0.05269889735295001,
+            "velocityY": 1.2604482810839739,
+            "timestamp": 0.1519398571756353
+        },
+        {
+            "x": 2.262009891384212,
+            "y": 5.847268762594484,
+            "heading": 0.09489870943903907,
+            "angularVelocity": 0.6244637649920359,
+            "velocityX": 0.07903580205425154,
+            "velocityY": 1.8906766244025437,
+            "timestamp": 0.22790978576345294
+        },
+        {
+            "x": 2.2700142224347877,
+            "y": 6.038782263911744,
+            "heading": 0.1581161098401003,
+            "angularVelocity": 0.8321371571119077,
+            "velocityX": 0.1053618346017344,
+            "velocityY": 2.5209119565760743,
+            "timestamp": 0.3038797143512706
+        },
+        {
+            "x": 2.280009491675263,
+            "y": 6.278187226323642,
+            "heading": 0.23645965423367718,
+            "angularVelocity": 1.0312441494928544,
+            "velocityX": 0.13156875919557368,
+            "velocityY": 3.151312194997739,
+            "timestamp": 0.37984964293908824
+        },
+        {
+            "x": 2.288005657404111,
+            "y": 6.469712008115255,
+            "heading": 0.2990934539478391,
+            "angularVelocity": 0.8244551611202343,
+            "velocityX": 0.10525435363026732,
+            "velocityY": 2.52106044262263,
+            "timestamp": 0.4558195715269059
+        },
+        {
+            "x": 2.294002815862431,
+            "y": 6.613355952122642,
+            "heading": 0.3460499471183657,
+            "angularVelocity": 0.6180931592721867,
+            "velocityX": 0.07894121489645621,
+            "velocityY": 1.8908000399308182,
+            "timestamp": 0.5317895001147235
+        },
+        {
+            "x": 2.2980009414583966,
+            "y": 6.709118669367686,
+            "heading": 0.3773492268804029,
+            "angularVelocity": 0.4119956454330042,
+            "velocityX": 0.05262773929482027,
+            "velocityY": 1.260534517079955,
+            "timestamp": 0.6077594287025412
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0.20601273965271094,
+            "velocityX": 0.026313813620245378,
+            "velocityY": 0.6302668901019914,
+            "timestamp": 0.6837293572903589
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 9.82651204832558e-34,
+            "velocityX": 0,
+            "velocityY": 3.8258931367738186e-36,
+            "timestamp": 0.7596992858781766
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj
new file mode 100644
index 00000000..f46e6706
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 5.7052362935559275e-34,
+            "velocityX": 0,
+            "velocityY": -1.2494033787640354e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2520019447146944,
+            "y": 5.512001877264397,
+            "heading": -0.01569721301150974,
+            "angularVelocity": -0.20637467835572687,
+            "velocityX": 0.02632000319280478,
+            "velocityY": -0.6310417737196751,
+            "timestamp": 0.07606172005488261
+        },
+        {
+            "x": 2.2560053559202022,
+            "y": 5.416005527882177,
+            "heading": -0.04708679827781808,
+            "angularVelocity": -0.41268571422864314,
+            "velocityX": 0.05263371907208186,
+            "velocityY": -1.262084913580087,
+            "timestamp": 0.15212344010976522
+        },
+        {
+            "x": 2.262009560249172,
+            "y": 5.272010699235591,
+            "heading": -0.0941564711571504,
+            "angularVelocity": -0.6188352412405218,
+            "velocityX": 0.07893858204412474,
+            "velocityY": -1.8931313746610776,
+            "timestamp": 0.22818516016464785
+        },
+        {
+            "x": 2.2700137636187,
+            "y": 5.080016884298785,
+            "heading": -0.15688079244309955,
+            "angularVelocity": -0.8246503134650409,
+            "velocityX": 0.10523300503528431,
+            "velocityY": -2.5241845017213995,
+            "timestamp": 0.30424688021953045
+        },
+        {
+            "x": 2.2800092014745617,
+            "y": 4.840012168782865,
+            "heading": -0.23463455266722952,
+            "angularVelocity": -1.0222456206358,
+            "velocityX": 0.13141219852311892,
+            "velocityY": -3.155394268532757,
+            "timestamp": 0.38030860027441304
+        },
+        {
+            "x": 2.2880054914438257,
+            "y": 4.648007616430203,
+            "heading": -0.2967978427641432,
+            "angularVelocity": -0.8172743142287549,
+            "velocityX": 0.10512896583845424,
+            "velocityY": -2.524325668866294,
+            "timestamp": 0.45637032032929564
+        },
+        {
+            "x": 2.2940027351611283,
+            "y": 4.5040038585170965,
+            "heading": -0.3434020516948695,
+            "angularVelocity": -0.6127156853289512,
+            "velocityX": 0.07884706936650254,
+            "velocityY": -1.8932487696728393,
+            "timestamp": 0.5324320403841782
+        },
+        {
+            "x": 2.2980009144821483,
+            "y": 4.408001268437703,
+            "heading": -0.37446662411182524,
+            "angularVelocity": -0.40841269950956927,
+            "velocityX": 0.05256493434720428,
+            "velocityY": -1.2621669613850792,
+            "timestamp": 0.6084937604390609
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -0.20422067601109492,
+            "velocityX": 0.026282412709165117,
+            "velocityY": -0.6310831309503169,
+            "timestamp": 0.6845554804939435
+        },
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 8.777318885868316e-35,
+            "velocityX": -1.1404320857258046e-34,
+            "velocityY": 1.134511753128188e-36,
+            "timestamp": 0.7606172005488262
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.1.traj
new file mode 100644
index 00000000..8dba26f9
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.1.traj	
@@ -0,0 +1,634 @@
+{
+    "samples": [
+        {
+            "x": 2.2999999999310377,
+            "y": 4.359999999993798,
+            "heading": -0.3900000000006091,
+            "angularVelocity": -1.7156392127364986e-12,
+            "velocityX": -4.6004235672195495e-11,
+            "velocityY": -2.541589729484622e-10,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2830691122580777,
+            "y": 4.4226688824706715,
+            "heading": -0.3961360368292833,
+            "angularVelocity": -0.04017087715885992,
+            "velocityX": -0.11084167601962927,
+            "velocityY": 0.4102752396150543,
+            "timestamp": 0.1527483904267538
+        },
+        {
+            "x": 2.2499539829007165,
+            "y": 4.551962677072163,
+            "heading": -0.39368124618911415,
+            "angularVelocity": 0.016070811835100392,
+            "velocityX": -0.21679527487321065,
+            "velocityY": 0.8464494731937718,
+            "timestamp": 0.3054967808535076
+        },
+        {
+            "x": 2.208537052758008,
+            "y": 4.723131432963358,
+            "heading": -0.3887340677228252,
+            "angularVelocity": 0.032387761683442845,
+            "velocityX": -0.27114478888671906,
+            "velocityY": 1.120592861369484,
+            "timestamp": 0.45824517128026143
+        },
+        {
+            "x": 2.1635048676051025,
+            "y": 4.925567881437362,
+            "heading": -0.37513416937632715,
+            "angularVelocity": 0.08903464258184278,
+            "velocityX": -0.294812828865451,
+            "velocityY": 1.3252934968125494,
+            "timestamp": 0.6109935617070152
+        },
+        {
+            "x": 2.119479495251874,
+            "y": 5.143090740041083,
+            "heading": -0.3554033967273168,
+            "angularVelocity": 0.12917172215836753,
+            "velocityX": -0.28822151359768483,
+            "velocityY": 1.4240599066467436,
+            "timestamp": 0.763741952133769
+        },
+        {
+            "x": 2.08196432941898,
+            "y": 5.365761039079461,
+            "heading": -0.32917795560380303,
+            "angularVelocity": 0.1716904580681245,
+            "velocityX": -0.24560105455579473,
+            "velocityY": 1.4577587261323537,
+            "timestamp": 0.9164903425605229
+        },
+        {
+            "x": 2.052346293594587,
+            "y": 5.582587972754811,
+            "heading": -0.29825458565802576,
+            "angularVelocity": 0.20244645364142877,
+            "velocityX": -0.19390080434302895,
+            "velocityY": 1.419503885313628,
+            "timestamp": 1.0692387329872766
+        },
+        {
+            "x": 2.030803808580604,
+            "y": 5.789012383645482,
+            "heading": -0.2654877856320327,
+            "angularVelocity": 0.2145148628152058,
+            "velocityX": -0.14103248420843936,
+            "velocityY": 1.3514015455604398,
+            "timestamp": 1.2219871234140305
+        },
+        {
+            "x": 2.0185215947450232,
+            "y": 5.980748966625485,
+            "heading": -0.23040407839794677,
+            "angularVelocity": 0.2296829912414591,
+            "velocityX": -0.08040813886408783,
+            "velocityY": 1.2552445394842306,
+            "timestamp": 1.3747355138407844
+        },
+        {
+            "x": 2.017537204929899,
+            "y": 6.152494473346335,
+            "heading": -0.19497588875865757,
+            "angularVelocity": 0.23193821893113326,
+            "velocityX": -0.006444518152378274,
+            "velocityY": 1.1243686841111533,
+            "timestamp": 1.5274839042675383
+        },
+        {
+            "x": 2.0415938446669073,
+            "y": 6.276321571691329,
+            "heading": -0.16155868009406138,
+            "angularVelocity": 0.21877290194991583,
+            "velocityX": 0.15749193627933206,
+            "velocityY": 0.8106605773582652,
+            "timestamp": 1.6802322946942922
+        },
+        {
+            "x": 2.1241281032079797,
+            "y": 6.2825961111606565,
+            "heading": -0.060349903928264906,
+            "angularVelocity": 0.6625848944275851,
+            "velocityX": 0.5403281719922621,
+            "velocityY": 0.04107761453342325,
+            "timestamp": 1.8329806851210462
+        },
+        {
+            "x": 2.1855581068088026,
+            "y": 6.287266231959223,
+            "heading": 0.012078838000808553,
+            "angularVelocity": 0.7458652653001706,
+            "velocityX": 0.6326011577496066,
+            "velocityY": 0.048092524107779686,
+            "timestamp": 1.9300876899626762
+        },
+        {
+            "x": 2.2551653620091354,
+            "y": 6.2925580157259535,
+            "heading": 0.08826667660975476,
+            "angularVelocity": 0.784576135772582,
+            "velocityX": 0.7168098264960764,
+            "velocityY": 0.05449435852189788,
+            "timestamp": 2.0271946948043063
+        },
+        {
+            "x": 2.3319122711008053,
+            "y": 6.29839258064769,
+            "heading": 0.16657489622034788,
+            "angularVelocity": 0.8064116460755234,
+            "velocityX": 0.7903333988427459,
+            "velocityY": 0.06008387443491743,
+            "timestamp": 2.1243016996459363
+        },
+        {
+            "x": 2.4149946395663093,
+            "y": 6.304708789079692,
+            "heading": 0.2457212902089042,
+            "angularVelocity": 0.8150430973252747,
+            "velocityX": 0.8555754417689245,
+            "velocityY": 0.06504379984951533,
+            "timestamp": 2.2214087044875663
+        },
+        {
+            "x": 2.5038373372195277,
+            "y": 6.311462917641332,
+            "heading": 0.32495163133637983,
+            "angularVelocity": 0.8159075780845946,
+            "velocityX": 0.9148948410382448,
+            "velocityY": 0.06955346543875748,
+            "timestamp": 2.3185157093291964
+        },
+        {
+            "x": 2.598073257877524,
+            "y": 6.3186270576568155,
+            "heading": 0.40431668105413066,
+            "angularVelocity": 0.8172947961305416,
+            "velocityX": 0.9704338108518987,
+            "velocityY": 0.0737757297255109,
+            "timestamp": 2.4156227141708264
+        },
+        {
+            "x": 2.697382502344975,
+            "y": 6.326176889286233,
+            "heading": 0.48416024389571777,
+            "angularVelocity": 0.8222224852016302,
+            "velocityX": 1.0226784849096606,
+            "velocityY": 0.07774755027256483,
+            "timestamp": 2.5127297190124565
+        },
+        {
+            "x": 2.8014852531337993,
+            "y": 6.334091139837385,
+            "heading": 0.5646614354219063,
+            "angularVelocity": 0.8289946917215433,
+            "velocityX": 1.0720416213516444,
+            "velocityY": 0.08150030637886811,
+            "timestamp": 2.6098367238540865
+        },
+        {
+            "x": 2.910148663901152,
+            "y": 6.342352107524804,
+            "heading": 0.6457203378655492,
+            "angularVelocity": 0.8347379532340086,
+            "velocityX": 1.119006925166821,
+            "velocityY": 0.0850707709678732,
+            "timestamp": 2.7069437286957165
+        },
+        {
+            "x": 3.0231702594110863,
+            "y": 6.350944399479571,
+            "heading": 0.7270565797219191,
+            "angularVelocity": 0.8375939718950361,
+            "velocityX": 1.1638871555335075,
+            "velocityY": 0.08848272107248593,
+            "timestamp": 2.8040507335373466
+        },
+        {
+            "x": 3.140351376164449,
+            "y": 6.359852912614292,
+            "heading": 0.8083350217640537,
+            "angularVelocity": 0.83699875417797,
+            "velocityX": 1.2067215654083316,
+            "velocityY": 0.09173914083795442,
+            "timestamp": 2.9011577383789766
+        },
+        {
+            "x": 3.261472243862432,
+            "y": 6.3690609392298585,
+            "heading": 0.8891526159463483,
+            "angularVelocity": 0.8322529806195065,
+            "velocityX": 1.2472927986960072,
+            "velocityY": 0.09482350610318373,
+            "timestamp": 2.9982647432206067
+        },
+        {
+            "x": 3.386280226554517,
+            "y": 6.378549273066906,
+            "heading": 0.9690024828336246,
+            "angularVelocity": 0.8222874040565998,
+            "velocityX": 1.2852624076036416,
+            "velocityY": 0.09771008689812195,
+            "timestamp": 3.0953717480622367
+        },
+        {
+            "x": 3.5144942146424594,
+            "y": 6.388296543201246,
+            "heading": 1.0472679478096496,
+            "angularVelocity": 0.8059713626886263,
+            "velocityX": 1.3203371728362745,
+            "velocityY": 0.10037659168026988,
+            "timestamp": 3.1924787529038667
+        },
+        {
+            "x": 3.64579126909874,
+            "y": 6.398278198691388,
+            "heading": 1.1233630936332748,
+            "angularVelocity": 0.783621593451087,
+            "velocityX": 1.3520863383515835,
+            "velocityY": 0.10279027288912505,
+            "timestamp": 3.2895857577454968
+        },
+        {
+            "x": 3.779855631903331,
+            "y": 6.408470234520327,
+            "heading": 1.1966377830577097,
+            "angularVelocity": 0.7545767639735752,
+            "velocityX": 1.3805838535787394,
+            "velocityY": 0.10495675240615632,
+            "timestamp": 3.386692762587127
+        },
+        {
+            "x": 3.916399996401096,
+            "y": 6.418850808641588,
+            "heading": 1.2663868507253695,
+            "angularVelocity": 0.7182701992663435,
+            "velocityX": 1.406122708347489,
+            "velocityY": 0.10689830431744617,
+            "timestamp": 3.483799767428757
+        },
+        {
+            "x": 4.055149110841729,
+            "y": 6.429398995480507,
+            "heading": 1.3319162676094702,
+            "angularVelocity": 0.6748165803464835,
+            "velocityX": 1.4288270430729209,
+            "velocityY": 0.10862436634220823,
+            "timestamp": 3.580906772270387
+        },
+        {
+            "x": 4.195643021249515,
+            "y": 6.440079827739076,
+            "heading": 1.392465239071673,
+            "angularVelocity": 0.6235283604234658,
+            "velocityX": 1.4467948093350342,
+            "velocityY": 0.10999033795341527,
+            "timestamp": 3.678013777112017
+        },
+        {
+            "x": 4.337478451024624,
+            "y": 6.45086264692987,
+            "heading": 1.4461855651355748,
+            "angularVelocity": 0.5532075282023133,
+            "velocityX": 1.4606096661273347,
+            "velocityY": 0.11104059096144768,
+            "timestamp": 3.775120781953647
+        },
+        {
+            "x": 4.47965775917095,
+            "y": 6.461671608979337,
+            "heading": 1.49351259060778,
+            "angularVelocity": 0.48736984118650195,
+            "velocityX": 1.4641508974652757,
+            "velocityY": 0.11130980792840979,
+            "timestamp": 3.872227786795277
+        },
+        {
+            "x": 4.621471854948602,
+            "y": 6.4724528062914635,
+            "heading": 1.5350394838354429,
+            "angularVelocity": 0.42764055328666206,
+            "velocityX": 1.4603899702927075,
+            "velocityY": 0.11102388888102613,
+            "timestamp": 3.969334791636907
+        },
+        {
+            "x": 4.762134624452166,
+            "y": 6.483146475819268,
+            "heading": 1.5711187532761213,
+            "angularVelocity": 0.37154136862160403,
+            "velocityX": 1.4485337063274482,
+            "velocityY": 0.11012253482900584,
+            "timestamp": 4.0664417964785375
+        },
+        {
+            "x": 4.901310758087736,
+            "y": 6.493727126167039,
+            "heading": 1.6012523506615222,
+            "angularVelocity": 0.3103133235343918,
+            "velocityX": 1.433224451514287,
+            "velocityY": 0.10895867241503827,
+            "timestamp": 4.163548801320168
+        },
+        {
+            "x": 5.0385652541984065,
+            "y": 6.504161687000508,
+            "heading": 1.6187441808494436,
+            "angularVelocity": 0.18012943788636607,
+            "velocityX": 1.413435585164251,
+            "velocityY": 0.1074542543793728,
+            "timestamp": 4.2606558061617985
+        },
+        {
+            "x": 5.1726571421935885,
+            "y": 6.5143558154807435,
+            "heading": 1.6310181673546418,
+            "angularVelocity": 0.12639650992409435,
+            "velocityX": 1.3808673050498854,
+            "velocityY": 0.104978301436634,
+            "timestamp": 4.357762811003429
+        },
+        {
+            "x": 5.303636473288953,
+            "y": 6.524313316659959,
+            "heading": 1.6376756877148975,
+            "angularVelocity": 0.06855860046187665,
+            "velocityX": 1.3488144481980282,
+            "velocityY": 0.1025415325257505,
+            "timestamp": 4.4548698158450595
+        },
+        {
+            "x": 5.4313983902221485,
+            "y": 6.53402621894025,
+            "heading": 1.6386463375950402,
+            "angularVelocity": 0.009995672907119661,
+            "velocityX": 1.3156817797402116,
+            "velocityY": 0.1000226726538996,
+            "timestamp": 4.55197682068669
+        },
+        {
+            "x": 5.555758593734715,
+            "y": 6.5434805112143435,
+            "heading": 1.6339151298589827,
+            "angularVelocity": -0.048721590949420676,
+            "velocityX": 1.2806512124922533,
+            "velocityY": 0.09735952745677612,
+            "timestamp": 4.6490838255283204
+        },
+        {
+            "x": 5.676522731590665,
+            "y": 6.552661418256687,
+            "heading": 1.6211253547169766,
+            "angularVelocity": -0.13170805962068485,
+            "velocityX": 1.2436192217305724,
+            "velocityY": 0.0945442281350229,
+            "timestamp": 4.746190830369951
+        },
+        {
+            "x": 5.8040475153273645,
+            "y": 6.568758338060969,
+            "heading": 1.594817916740329,
+            "angularVelocity": -0.24917360707174604,
+            "velocityX": 1.2078641151592902,
+            "velocityY": 0.15246363592342288,
+            "timestamp": 4.851769580629666
+        },
+        {
+            "x": 5.9283875752902055,
+            "y": 6.58985033542802,
+            "heading": 1.559498986776326,
+            "angularVelocity": -0.33452688097090233,
+            "velocityX": 1.1776996757735678,
+            "velocityY": 0.19977502680380196,
+            "timestamp": 4.957348330889381
+        },
+        {
+            "x": 6.049721376535089,
+            "y": 6.61532142807792,
+            "heading": 1.5174190958344496,
+            "angularVelocity": -0.3985640187289682,
+            "velocityX": 1.1492255865917638,
+            "velocityY": 0.2412520781772084,
+            "timestamp": 5.0629270811490965
+        },
+        {
+            "x": 6.167915961542227,
+            "y": 6.644893212673861,
+            "heading": 1.4694436889435494,
+            "angularVelocity": -0.45440400479138554,
+            "velocityX": 1.119492176613303,
+            "velocityY": 0.280092203729064,
+            "timestamp": 5.168505831408812
+        },
+        {
+            "x": 6.2827599678090085,
+            "y": 6.678289224223258,
+            "heading": 1.4161083022197893,
+            "angularVelocity": -0.5051716053929952,
+            "velocityX": 1.087756824956101,
+            "velocityY": 0.31631376267017064,
+            "timestamp": 5.274084581668527
+        },
+        {
+            "x": 6.394018817655917,
+            "y": 6.715173356117064,
+            "heading": 1.3577889107275223,
+            "angularVelocity": -0.5523781194839231,
+            "velocityX": 1.0537996466964168,
+            "velocityY": 0.34935185331081736,
+            "timestamp": 5.379663331928242
+        },
+        {
+            "x": 6.501464569355021,
+            "y": 6.755149800954155,
+            "heading": 1.2948750452839908,
+            "angularVelocity": -0.5958951523626541,
+            "velocityX": 1.0176834949234126,
+            "velocityY": 0.3786410134453019,
+            "timestamp": 5.485242082187957
+        },
+        {
+            "x": 6.6048776092971675,
+            "y": 6.797770420027359,
+            "heading": 1.22768881668393,
+            "angularVelocity": -0.6363612797096425,
+            "velocityX": 0.9794872502530397,
+            "velocityY": 0.4036855817031342,
+            "timestamp": 5.5908208324476725
+        },
+        {
+            "x": 6.70404339771049,
+            "y": 6.8425429323292,
+            "heading": 1.1564439654233045,
+            "angularVelocity": -0.6748029419532755,
+            "velocityX": 0.939258970068964,
+            "velocityY": 0.4240674597490353,
+            "timestamp": 5.696399582707388
+        },
+        {
+            "x": 6.79877778418725,
+            "y": 6.888932915699562,
+            "heading": 1.0813771004184407,
+            "angularVelocity": -0.7110035384939538,
+            "velocityX": 0.8972864903430491,
+            "velocityY": 0.4393875033009405,
+            "timestamp": 5.801978332967103
+        },
+        {
+            "x": 6.88894352921239,
+            "y": 6.936373258053616,
+            "heading": 1.0028508284852347,
+            "angularVelocity": -0.7437696672526416,
+            "velocityX": 0.8540141324253476,
+            "velocityY": 0.44933608637180433,
+            "timestamp": 5.907557083226818
+        },
+        {
+            "x": 6.974457896759412,
+            "y": 6.984282785155121,
+            "heading": 0.9213875618911144,
+            "angularVelocity": -0.7715877144447874,
+            "velocityX": 0.809958131723675,
+            "velocityY": 0.45378001785130806,
+            "timestamp": 6.013135833486533
+        },
+        {
+            "x": 7.055294443748765,
+            "y": 7.032089281864471,
+            "heading": 0.8376815844807217,
+            "angularVelocity": -0.792829780737162,
+            "velocityX": 0.7656516726874414,
+            "velocityY": 0.45280415475213037,
+            "timestamp": 6.118714583746248
+        },
+        {
+            "x": 7.131475603500295,
+            "y": 7.0792526481673885,
+            "heading": 0.752591160441856,
+            "angularVelocity": -0.805942709434023,
+            "velocityX": 0.7215576931722669,
+            "velocityY": 0.44671267879627613,
+            "timestamp": 6.224293334005964
+        },
+        {
+            "x": 7.203056823429108,
+            "y": 7.125284419454981,
+            "heading": 0.6670903070961578,
+            "angularVelocity": -0.8098301328249344,
+            "velocityX": 0.6779888895797812,
+            "velocityY": 0.4359946600433992,
+            "timestamp": 6.329872084265679
+        },
+        {
+            "x": 7.270104931236086,
+            "y": 7.169760877903185,
+            "heading": 0.5821807537401323,
+            "angularVelocity": -0.8042295741051197,
+            "velocityX": 0.6350530514787667,
+            "velocityY": 0.4212633543513555,
+            "timestamp": 6.435450834525394
+        },
+        {
+            "x": 7.332675465601759,
+            "y": 7.212330496694183,
+            "heading": 0.49884759114131233,
+            "angularVelocity": -0.789298626772507,
+            "velocityX": 0.592643253784208,
+            "velocityY": 0.4032025262817465,
+            "timestamp": 6.541029584785109
+        },
+        {
+            "x": 7.390806680523043,
+            "y": 7.252712685445242,
+            "heading": 0.4183197118253645,
+            "angularVelocity": -0.7627280973825078,
+            "velocityX": 0.5505957828143127,
+            "velocityY": 0.38248405756315146,
+            "timestamp": 6.646608335044824
+        },
+        {
+            "x": 7.444527736090263,
+            "y": 7.290684851015479,
+            "heading": 0.34198396458440167,
+            "angularVelocity": -0.7230218869110173,
+            "velocityX": 0.5088245046789369,
+            "velocityY": 0.3596572750296197,
+            "timestamp": 6.75218708530454
+        },
+        {
+            "x": 7.4938504955203875,
+            "y": 7.326060981562411,
+            "heading": 0.27119241907250147,
+            "angularVelocity": -0.6705094095505424,
+            "velocityX": 0.46716559024619986,
+            "velocityY": 0.3350686622032283,
+            "timestamp": 6.857765835564255
+        },
+        {
+            "x": 7.538762197286186,
+            "y": 7.358679050922846,
+            "heading": 0.2071469006235064,
+            "angularVelocity": -0.6066137198463124,
+            "velocityX": 0.4253858037038331,
+            "velocityY": 0.3089454019040213,
+            "timestamp": 6.96334458582397
+        },
+        {
+            "x": 7.57923356269503,
+            "y": 7.3883979284340455,
+            "heading": 0.15090824268085423,
+            "angularVelocity": -0.5326702370042972,
+            "velocityX": 0.38332870100058564,
+            "velocityY": 0.28148540800086536,
+            "timestamp": 7.068923336083685
+        },
+        {
+            "x": 7.612613012416259,
+            "y": 7.4137927319874946,
+            "heading": 0.0952672119425909,
+            "angularVelocity": -0.5270097494122625,
+            "velocityX": 0.316156891737545,
+            "velocityY": 0.2405294965409898,
+            "timestamp": 7.1745020863434
+        },
+        {
+            "x": 7.641161660602441,
+            "y": 7.4358681154587,
+            "heading": 0.04946855188730579,
+            "angularVelocity": -0.43378672242570576,
+            "velocityX": 0.27040145789287734,
+            "velocityY": 0.20908926766306235,
+            "timestamp": 7.280080836603116
+        },
+        {
+            "x": 7.664737893553612,
+            "y": 7.454419635462092,
+            "heading": 0.014221249681079469,
+            "angularVelocity": -0.3338484505242331,
+            "velocityX": 0.22330471529975876,
+            "velocityY": 0.17571263220528385,
+            "timestamp": 7.385659586862831
+        },
+        {
+            "x": 7.679999999654181,
+            "y": 7.467000000250119,
+            "heading": 5.571589197087409e-11,
+            "angularVelocity": -0.13469802934148392,
+            "velocityX": 0.1445566071683118,
+            "velocityY": 0.11915622105321995,
+            "timestamp": 7.491238337122546
+        },
+        {
+            "x": 7.6799999998383255,
+            "y": 7.467000000125839,
+            "heading": 2.732434354976469e-11,
+            "angularVelocity": -1.9015024944340445e-11,
+            "velocityX": 1.5544110263140067e-10,
+            "velocityY": -2.911726567326558e-11,
+            "timestamp": 7.596817087382261
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.traj
index 5529ce29..8dba26f9 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.traj	
@@ -1,553 +1,634 @@
 {
     "samples": [
         {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 1.095053106710567e-32,
+            "x": 2.2999999999310377,
+            "y": 4.359999999993798,
+            "heading": -0.3900000000006091,
+            "angularVelocity": -1.7156392127364986e-12,
+            "velocityX": -4.6004235672195495e-11,
+            "velocityY": -2.541589729484622e-10,
             "timestamp": 0
         },
         {
-            "x": 2.3031602212725026,
-            "y": 4.367935645195281,
-            "heading": -0.3933962364434778,
-            "angularVelocity": -0.10578752662991632,
-            "velocityX": 0.0984360180997807,
-            "velocityY": 0.24718310735800264,
-            "timestamp": 0.032104318454850914
-        },
-        {
-            "x": 2.309652031024921,
-            "y": 4.383743043384139,
-            "heading": -0.39998627296935113,
-            "angularVelocity": -0.20526947286363117,
-            "velocityX": 0.20220986038211264,
-            "velocityY": 0.4923760711845366,
-            "timestamp": 0.06420863690970183
-        },
-        {
-            "x": 2.3196739941431104,
-            "y": 4.407342390021744,
-            "heading": -0.40953275493580865,
-            "angularVelocity": -0.29735818811675285,
-            "velocityX": 0.31216869257896596,
-            "velocityY": 0.7350832465356143,
-            "timestamp": 0.09631295536455274
-        },
-        {
-            "x": 2.333458307096076,
-            "y": 4.438632228164309,
-            "heading": -0.4217541925110665,
-            "angularVelocity": -0.3806789292987277,
-            "velocityX": 0.4293600866297902,
-            "velocityY": 0.9746301945817274,
-            "timestamp": 0.12841727381940365
-        },
-        {
-            "x": 2.3512791398330983,
-            "y": 4.477480960055527,
-            "heading": -0.4363123371700568,
-            "angularVelocity": -0.4534637506622021,
-            "velocityX": 0.5550914517025046,
-            "velocityY": 1.21007807550416,
-            "timestamp": 0.16052159227425455
-        },
-        {
-            "x": 2.3734634122157945,
-            "y": 4.523713969255995,
-            "heading": -0.45279460426513807,
-            "angularVelocity": -0.5133971966500652,
-            "velocityX": 0.691005866201282,
-            "velocityY": 1.4400869236793286,
-            "timestamp": 0.19262591072910545
-        },
-        {
-            "x": 2.4004044405613056,
-            "y": 4.577093572392249,
-            "heading": -0.4706891378053212,
-            "angularVelocity": -0.5573871180398601,
-            "velocityX": 0.8391714773013875,
-            "velocityY": 1.6626923013900174,
-            "timestamp": 0.22473022918395635
-        },
-        {
-            "x": 2.432578225951104,
-            "y": 4.63728707308353,
-            "heading": -0.48934877664951465,
-            "angularVelocity": -0.581218968109697,
-            "velocityX": 1.002163787873114,
-            "velocityY": 1.8749347000134435,
-            "timestamp": 0.25683454763880725
-        },
-        {
-            "x": 2.4705256917588616,
-            "y": 4.703846202675345,
-            "heading": -0.5079484621261662,
-            "angularVelocity": -0.5793515131868818,
-            "velocityX": 1.1820050271780043,
-            "velocityY": 2.073214221489191,
-            "timestamp": 0.28893886609365815
-        },
-        {
-            "x": 2.5143889510907504,
-            "y": 4.77663720854218,
-            "heading": -0.5259955708448517,
-            "angularVelocity": -0.5621395995079442,
-            "velocityX": 1.366272870535308,
-            "velocityY": 2.267327554989265,
-            "timestamp": 0.32104318454850905
-        },
-        {
-            "x": 2.564366566619072,
-            "y": 4.855456871813396,
-            "heading": -0.5427715655324792,
-            "angularVelocity": -0.5225463580925951,
-            "velocityX": 1.5567256348583118,
-            "velocityY": 2.4551109341275055,
-            "timestamp": 0.35314750300335995
-        },
-        {
-            "x": 2.620751841763659,
-            "y": 4.939963572730219,
-            "heading": -0.5571329301868887,
-            "angularVelocity": -0.44733435704626356,
-            "velocityX": 1.7563143482981505,
-            "velocityY": 2.6322533847172953,
-            "timestamp": 0.38525182145821085
-        },
-        {
-            "x": 2.6840019979529464,
-            "y": 5.029487269137319,
-            "heading": -0.5670183536644556,
-            "angularVelocity": -0.30791569338152147,
-            "velocityX": 1.9701448039844656,
-            "velocityY": 2.788525055686805,
-            "timestamp": 0.41735613991306175
-        },
-        {
-            "x": 2.7547918742113016,
-            "y": 5.122370588176559,
-            "heading": -0.568384562504894,
-            "angularVelocity": -0.0425552980468726,
-            "velocityX": 2.2049954543625816,
-            "velocityY": 2.893172118569162,
-            "timestamp": 0.44946045836791265
-        },
-        {
-            "x": 2.8341176789452183,
-            "y": 5.214333601098647,
-            "heading": -0.5683844391321649,
-            "angularVelocity": 0.0000038428702119811485,
-            "velocityX": 2.4708764599838617,
-            "velocityY": 2.864506002562167,
-            "timestamp": 0.48156477682276355
-        },
-        {
-            "x": 2.9197482153809866,
-            "y": 5.3004567509455125,
-            "heading": -0.5683843814173083,
-            "angularVelocity": 0.0000017977287583592433,
-            "velocityX": 2.6672591276526463,
-            "velocityY": 2.6826032755681406,
-            "timestamp": 0.5136690952776145
-        },
-        {
-            "x": 3.0112544304014244,
-            "y": 5.380309105627853,
-            "heading": -0.5683843265105415,
-            "angularVelocity": 0.0000017102610943456471,
-            "velocityX": 2.850277452521701,
-            "velocityY": 2.487277678691065,
-            "timestamp": 0.5457734137324655
-        },
-        {
-            "x": 3.107551803887462,
-            "y": 5.454313172250514,
-            "heading": -0.5683842730108181,
-            "angularVelocity": 0.0000016664338651344868,
-            "velocityX": 2.999514648518804,
-            "velocityY": 2.305112526426488,
-            "timestamp": 0.5778777321873164
-        },
-        {
-            "x": 3.2038516202489813,
-            "y": 5.528314059986814,
-            "heading": -0.5683842195116714,
-            "angularVelocity": 0.000001666415897857932,
-            "velocityX": 2.9995907403219917,
-            "velocityY": 2.305013509019602,
-            "timestamp": 0.6099820506421674
-        },
-        {
-            "x": 3.3001514533185667,
-            "y": 5.602314925980335,
-            "heading": -0.5683841660125195,
-            "angularVelocity": 0.000001666416059709123,
-            "velocityX": 2.9995912607524815,
-            "velocityY": 2.305012831765639,
-            "timestamp": 0.6420863690970183
-        },
-        {
-            "x": 3.3964512995777554,
-            "y": 5.676315774809768,
-            "heading": -0.568384112513365,
-            "angularVelocity": 0.0000016664161452516387,
-            "velocityX": 2.999591671588281,
-            "velocityY": 2.30501229713074,
-            "timestamp": 0.6741906875518693
-        },
-        {
-            "x": 3.492751156794414,
-            "y": 5.750316609379858,
-            "heading": -0.5683840590142086,
-            "angularVelocity": 0.000001666416198927138,
-            "velocityX": 2.999592012896559,
-            "velocityY": 2.3050118529742454,
-            "timestamp": 0.7062950060067202
-        },
-        {
-            "x": 3.5890510233460025,
-            "y": 5.824317431802072,
-            "heading": -0.5683840055150509,
-            "angularVelocity": 0.0000016664162437925357,
-            "velocityX": 2.999592303665221,
-            "velocityY": 2.305011474586604,
-            "timestamp": 0.7383993244615712
-        },
-        {
-            "x": 3.6853508979893648,
-            "y": 5.898318243694168,
-            "heading": -0.5683839520158918,
-            "angularVelocity": 0.0000016664162889267752,
-            "velocityX": 2.9995925557115117,
-            "velocityY": 2.3050111465896648,
-            "timestamp": 0.7705036429164221
-        },
-        {
-            "x": 3.7816507797340138,
-            "y": 5.972319046345101,
-            "heading": -0.5683838985167311,
-            "angularVelocity": 0.0000016664163350532929,
-            "velocityX": 2.9995927769056334,
-            "velocityY": 2.305010858741686,
-            "timestamp": 0.8026079613712731
-        },
-        {
-            "x": 3.877950667767949,
-            "y": 6.046319840811553,
-            "heading": -0.5683838450175687,
-            "angularVelocity": 0.0000016664163859759036,
-            "velocityX": 2.999592972807203,
-            "velocityY": 2.305010603807763,
-            "timestamp": 0.8347122798261241
-        },
-        {
-            "x": 3.9742505615842325,
-            "y": 6.120320627753221,
-            "heading": -0.5683837915184047,
-            "angularVelocity": 0.0000016664164376020863,
-            "velocityX": 2.999593152918432,
-            "velocityY": 2.3050103694223405,
-            "timestamp": 0.866816598280975
-        },
-        {
-            "x": 4.070552264919673,
-            "y": 6.194319059842733,
-            "heading": -0.5683837380195041,
-            "angularVelocity": 0.0000016664082331173329,
-            "velocityX": 2.999649516649054,
-            "velocityY": 2.304937019409932,
-            "timestamp": 0.898920916735826
-        },
-        {
-            "x": 4.171102213813095,
-            "y": 6.262432889296395,
-            "heading": -0.5683836849080734,
-            "angularVelocity": 0.0000016543391427080076,
-            "velocityX": 3.131975812999348,
-            "velocityY": 2.1216407241116846,
-            "timestamp": 0.9310252351906769
-        },
-        {
-            "x": 4.276216983795166,
-            "y": 6.32326602935791,
-            "heading": -0.5683836309724583,
-            "angularVelocity": 0.0000016800112198234746,
-            "velocityX": 3.274162948822482,
-            "velocityY": 1.8948584797732355,
-            "timestamp": 0.9631295536455279
-        },
-        {
-            "x": 4.413834682670106,
-            "y": 6.38740625436239,
-            "heading": -0.5683835821873748,
-            "angularVelocity": 0.0000012155044166425132,
-            "velocityX": 3.428812837046335,
-            "velocityY": 1.5980853383274327,
-            "timestamp": 1.0032652231659112
-        },
-        {
-            "x": 4.5565820466376845,
-            "y": 6.439134291384663,
-            "heading": -0.5683835352510233,
-            "angularVelocity": 0.0000011694423477450916,
-            "velocityX": 3.556620972650851,
-            "velocityY": 1.2888295533727718,
-            "timestamp": 1.0434008926862943
-        },
-        {
-            "x": 4.7007810920363475,
-            "y": 6.4866657027970485,
-            "heading": -0.5683834885999294,
-            "angularVelocity": 0.000001162335011018839,
-            "velocityX": 3.5927903314389917,
-            "velocityY": 1.184268556632563,
-            "timestamp": 1.0835365622066775
-        },
-        {
-            "x": 4.84498021653845,
-            "y": 6.534196874227661,
-            "heading": -0.5683834419488406,
-            "angularVelocity": 0.0000011623348856187121,
-            "velocityX": 3.5927923023401758,
-            "velocityY": 1.1842625773683353,
-            "timestamp": 1.1236722317270607
-        },
-        {
-            "x": 4.989179343628637,
-            "y": 6.581728037806586,
-            "heading": -0.5683833952977476,
-            "angularVelocity": 0.0000011623349868626642,
-            "velocityX": 3.5927923668235673,
-            "velocityY": 1.1842623817396558,
-            "timestamp": 1.1638079012474438
-        },
-        {
-            "x": 5.133378473125504,
-            "y": 6.629259194084157,
-            "heading": -0.5683833486466496,
-            "angularVelocity": 0.0000011623351115999292,
-            "velocityX": 3.592792426787221,
-            "velocityY": 1.1842621998228242,
-            "timestamp": 1.203943570767827
-        },
-        {
-            "x": 5.277577604867278,
-            "y": 6.676790343551162,
-            "heading": -0.5683833019955459,
-            "angularVelocity": 0.0000011623352597981392,
-            "velocityX": 3.5927924827201663,
-            "velocityY": 1.184262030134241,
-            "timestamp": 1.2440792402882102
-        },
-        {
-            "x": 5.421776738706852,
-            "y": 6.7243214866538885,
-            "heading": -0.5683832553444353,
-            "angularVelocity": 0.0000011623354265532326,
-            "velocityX": 3.5927925349879053,
-            "velocityY": 1.1842618715650601,
-            "timestamp": 1.2842149098085933
-        },
-        {
-            "x": 5.565975874510482,
-            "y": 6.771852623798087,
-            "heading": -0.5683832086933173,
-            "angularVelocity": 0.0000011623356113966992,
-            "velocityX": 3.5927925839233144,
-            "velocityY": 1.18426172310541,
-            "timestamp": 1.3243505793289765
-        },
-        {
-            "x": 5.710175012156514,
-            "y": 6.819383755352828,
-            "heading": -0.5683831620421912,
-            "angularVelocity": 0.0000011623358144790056,
-            "velocityX": 3.5927926298276605,
-            "velocityY": 1.1842615838413222,
-            "timestamp": 1.3644862488493597
-        },
-        {
-            "x": 5.854374151534237,
-            "y": 6.866914881653982,
-            "heading": -0.5683831153910563,
-            "angularVelocity": 0.000001162336033125057,
-            "velocityX": 3.592792672973599,
-            "velocityY": 1.1842614529456208,
-            "timestamp": 1.4046219183697428
-        },
-        {
-            "x": 5.998573292523273,
-            "y": 6.9144460030667485,
-            "heading": -0.568383068739913,
-            "angularVelocity": 0.000001162336242705986,
-            "velocityX": 3.59279271312027,
-            "velocityY": 1.184261331149038,
-            "timestamp": 1.444757587890126
-        },
-        {
-            "x": 6.142772435013044,
-            "y": 6.9619771199265985,
-            "heading": -0.5683830220887615,
-            "angularVelocity": 0.0000011623364467362213,
-            "velocityX": 3.5927927505118338,
-            "velocityY": 1.1842612177108516,
-            "timestamp": 1.4848932574105091
-        },
-        {
-            "x": 6.286971578902684,
-            "y": 7.00950823253954,
-            "heading": -0.5683829754376022,
-            "angularVelocity": 0.0000011623366413601916,
-            "velocityX": 3.59279278539024,
-            "velocityY": 1.1842611118970372,
-            "timestamp": 1.5250289269308923
-        },
-        {
-            "x": 6.431170724100105,
-            "y": 7.057039341184945,
-            "heading": -0.5683829287864354,
-            "angularVelocity": 0.0000011623368281432719,
-            "velocityX": 3.592792817974252,
-            "velocityY": 1.1842610130439195,
-            "timestamp": 1.5651645964512755
-        },
-        {
-            "x": 6.575369870521147,
-            "y": 7.10457044611814,
-            "heading": -0.5683828821352613,
-            "angularVelocity": 0.0000011623370098744323,
-            "velocityX": 3.5927928484613587,
-            "velocityY": 1.1842609205523604,
-            "timestamp": 1.6053002659716586
-        },
-        {
-            "x": 6.719569018075523,
-            "y": 7.152101547613029,
-            "heading": -0.5683828354840802,
-            "angularVelocity": 0.0000011623371832362076,
-            "velocityX": 3.5927928766989634,
-            "velocityY": 1.1842608348853054,
-            "timestamp": 1.6454359354920418
-        },
-        {
-            "x": 6.8637678827817385,
-            "y": 7.1996335071853474,
-            "heading": -0.5683827888209949,
-            "angularVelocity": 0.0000011626337817909648,
-            "velocityX": 3.592785829397527,
-            "velocityY": 1.1842822143076228,
-            "timestamp": 1.685571605012425
-        },
-        {
-            "x": 6.99756534811195,
-            "y": 7.244965845733408,
-            "heading": -0.5424482126349314,
-            "angularVelocity": 0.6461727559544621,
-            "velocityX": 3.3336298342366253,
-            "velocityY": 1.1294775716906411,
-            "timestamp": 1.7257072745328081
-        },
-        {
-            "x": 7.121516587135396,
-            "y": 7.285176196921745,
-            "heading": -0.489772109644447,
-            "angularVelocity": 1.3124510845329824,
-            "velocityX": 3.088306249893159,
-            "velocityY": 1.0018607305881775,
-            "timestamp": 1.7658429440531913
-        },
-        {
-            "x": 7.233956124618328,
-            "y": 7.321947450112657,
-            "heading": -0.4161805837381191,
-            "angularVelocity": 1.8335691614401448,
-            "velocityX": 2.8014865287304875,
-            "velocityY": 0.9161739079059802,
-            "timestamp": 1.8059786135735745
-        },
-        {
-            "x": 7.333707629732149,
-            "y": 7.35452494917019,
-            "heading": -0.3377597360881954,
-            "angularVelocity": 1.953894094381483,
-            "velocityX": 2.4853579448365832,
-            "velocityY": 0.8116844554190893,
-            "timestamp": 1.8461142830939576
-        },
-        {
-            "x": 7.420722566948463,
-            "y": 7.382895867936689,
-            "heading": -0.26184604843223563,
-            "angularVelocity": 1.8914269666638126,
-            "velocityX": 2.1680200743162716,
-            "velocityY": 0.7068754328887092,
-            "timestamp": 1.8862499526143408
-        },
-        {
-            "x": 7.495090208476241,
-            "y": 7.40709623575076,
-            "heading": -0.19218016370533045,
-            "angularVelocity": 1.7357598754276362,
-            "velocityX": 1.8529064649092093,
-            "velocityY": 0.6029640990984653,
-            "timestamp": 1.926385622134724
-        },
-        {
-            "x": 7.556901497509452,
-            "y": 7.427169942598205,
-            "heading": -0.13107853540878284,
-            "angularVelocity": 1.522377202790068,
-            "velocityX": 1.5400587500308252,
-            "velocityY": 0.500146305949861,
-            "timestamp": 1.9665212916551071
-        },
-        {
-            "x": 7.606235479245265,
-            "y": 7.443160506733181,
-            "heading": -0.08018624320935208,
-            "angularVelocity": 1.2680065589434022,
-            "velocityX": 1.2291804852229526,
-            "velocityY": 0.3984127915657456,
-            "timestamp": 2.0066569611754903
-        },
-        {
-            "x": 7.643159935464096,
-            "y": 7.455107253348189,
-            "heading": -0.040758426269481916,
-            "angularVelocity": 0.982363503861482,
-            "velocityX": 0.9199910369024534,
-            "velocityY": 0.29765908374694494,
-            "timestamp": 2.0467926306958737
-        },
-        {
-            "x": 7.667732329164437,
-            "y": 7.4630440972663505,
-            "heading": -0.013776014916518935,
-            "angularVelocity": 0.6722800859036306,
-            "velocityX": 0.6122333075286244,
-            "velocityY": 0.1977503804721968,
-            "timestamp": 2.086928300216257
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 2.662292212835229e-35,
-            "angularVelocity": 0.3432362056280802,
-            "velocityX": 0.30565506897381545,
-            "velocityY": 0.09856326756028964,
-            "timestamp": 2.1270639697366405
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -1.5591265591207004e-41,
-            "angularVelocity": 3.563315968840566e-34,
-            "velocityX": -7.620676928265867e-35,
-            "velocityY": -1.1745137620932586e-33,
-            "timestamp": 2.167199639257024
+            "x": 2.2830691122580777,
+            "y": 4.4226688824706715,
+            "heading": -0.3961360368292833,
+            "angularVelocity": -0.04017087715885992,
+            "velocityX": -0.11084167601962927,
+            "velocityY": 0.4102752396150543,
+            "timestamp": 0.1527483904267538
+        },
+        {
+            "x": 2.2499539829007165,
+            "y": 4.551962677072163,
+            "heading": -0.39368124618911415,
+            "angularVelocity": 0.016070811835100392,
+            "velocityX": -0.21679527487321065,
+            "velocityY": 0.8464494731937718,
+            "timestamp": 0.3054967808535076
+        },
+        {
+            "x": 2.208537052758008,
+            "y": 4.723131432963358,
+            "heading": -0.3887340677228252,
+            "angularVelocity": 0.032387761683442845,
+            "velocityX": -0.27114478888671906,
+            "velocityY": 1.120592861369484,
+            "timestamp": 0.45824517128026143
+        },
+        {
+            "x": 2.1635048676051025,
+            "y": 4.925567881437362,
+            "heading": -0.37513416937632715,
+            "angularVelocity": 0.08903464258184278,
+            "velocityX": -0.294812828865451,
+            "velocityY": 1.3252934968125494,
+            "timestamp": 0.6109935617070152
+        },
+        {
+            "x": 2.119479495251874,
+            "y": 5.143090740041083,
+            "heading": -0.3554033967273168,
+            "angularVelocity": 0.12917172215836753,
+            "velocityX": -0.28822151359768483,
+            "velocityY": 1.4240599066467436,
+            "timestamp": 0.763741952133769
+        },
+        {
+            "x": 2.08196432941898,
+            "y": 5.365761039079461,
+            "heading": -0.32917795560380303,
+            "angularVelocity": 0.1716904580681245,
+            "velocityX": -0.24560105455579473,
+            "velocityY": 1.4577587261323537,
+            "timestamp": 0.9164903425605229
+        },
+        {
+            "x": 2.052346293594587,
+            "y": 5.582587972754811,
+            "heading": -0.29825458565802576,
+            "angularVelocity": 0.20244645364142877,
+            "velocityX": -0.19390080434302895,
+            "velocityY": 1.419503885313628,
+            "timestamp": 1.0692387329872766
+        },
+        {
+            "x": 2.030803808580604,
+            "y": 5.789012383645482,
+            "heading": -0.2654877856320327,
+            "angularVelocity": 0.2145148628152058,
+            "velocityX": -0.14103248420843936,
+            "velocityY": 1.3514015455604398,
+            "timestamp": 1.2219871234140305
+        },
+        {
+            "x": 2.0185215947450232,
+            "y": 5.980748966625485,
+            "heading": -0.23040407839794677,
+            "angularVelocity": 0.2296829912414591,
+            "velocityX": -0.08040813886408783,
+            "velocityY": 1.2552445394842306,
+            "timestamp": 1.3747355138407844
+        },
+        {
+            "x": 2.017537204929899,
+            "y": 6.152494473346335,
+            "heading": -0.19497588875865757,
+            "angularVelocity": 0.23193821893113326,
+            "velocityX": -0.006444518152378274,
+            "velocityY": 1.1243686841111533,
+            "timestamp": 1.5274839042675383
+        },
+        {
+            "x": 2.0415938446669073,
+            "y": 6.276321571691329,
+            "heading": -0.16155868009406138,
+            "angularVelocity": 0.21877290194991583,
+            "velocityX": 0.15749193627933206,
+            "velocityY": 0.8106605773582652,
+            "timestamp": 1.6802322946942922
+        },
+        {
+            "x": 2.1241281032079797,
+            "y": 6.2825961111606565,
+            "heading": -0.060349903928264906,
+            "angularVelocity": 0.6625848944275851,
+            "velocityX": 0.5403281719922621,
+            "velocityY": 0.04107761453342325,
+            "timestamp": 1.8329806851210462
+        },
+        {
+            "x": 2.1855581068088026,
+            "y": 6.287266231959223,
+            "heading": 0.012078838000808553,
+            "angularVelocity": 0.7458652653001706,
+            "velocityX": 0.6326011577496066,
+            "velocityY": 0.048092524107779686,
+            "timestamp": 1.9300876899626762
+        },
+        {
+            "x": 2.2551653620091354,
+            "y": 6.2925580157259535,
+            "heading": 0.08826667660975476,
+            "angularVelocity": 0.784576135772582,
+            "velocityX": 0.7168098264960764,
+            "velocityY": 0.05449435852189788,
+            "timestamp": 2.0271946948043063
+        },
+        {
+            "x": 2.3319122711008053,
+            "y": 6.29839258064769,
+            "heading": 0.16657489622034788,
+            "angularVelocity": 0.8064116460755234,
+            "velocityX": 0.7903333988427459,
+            "velocityY": 0.06008387443491743,
+            "timestamp": 2.1243016996459363
+        },
+        {
+            "x": 2.4149946395663093,
+            "y": 6.304708789079692,
+            "heading": 0.2457212902089042,
+            "angularVelocity": 0.8150430973252747,
+            "velocityX": 0.8555754417689245,
+            "velocityY": 0.06504379984951533,
+            "timestamp": 2.2214087044875663
+        },
+        {
+            "x": 2.5038373372195277,
+            "y": 6.311462917641332,
+            "heading": 0.32495163133637983,
+            "angularVelocity": 0.8159075780845946,
+            "velocityX": 0.9148948410382448,
+            "velocityY": 0.06955346543875748,
+            "timestamp": 2.3185157093291964
+        },
+        {
+            "x": 2.598073257877524,
+            "y": 6.3186270576568155,
+            "heading": 0.40431668105413066,
+            "angularVelocity": 0.8172947961305416,
+            "velocityX": 0.9704338108518987,
+            "velocityY": 0.0737757297255109,
+            "timestamp": 2.4156227141708264
+        },
+        {
+            "x": 2.697382502344975,
+            "y": 6.326176889286233,
+            "heading": 0.48416024389571777,
+            "angularVelocity": 0.8222224852016302,
+            "velocityX": 1.0226784849096606,
+            "velocityY": 0.07774755027256483,
+            "timestamp": 2.5127297190124565
+        },
+        {
+            "x": 2.8014852531337993,
+            "y": 6.334091139837385,
+            "heading": 0.5646614354219063,
+            "angularVelocity": 0.8289946917215433,
+            "velocityX": 1.0720416213516444,
+            "velocityY": 0.08150030637886811,
+            "timestamp": 2.6098367238540865
+        },
+        {
+            "x": 2.910148663901152,
+            "y": 6.342352107524804,
+            "heading": 0.6457203378655492,
+            "angularVelocity": 0.8347379532340086,
+            "velocityX": 1.119006925166821,
+            "velocityY": 0.0850707709678732,
+            "timestamp": 2.7069437286957165
+        },
+        {
+            "x": 3.0231702594110863,
+            "y": 6.350944399479571,
+            "heading": 0.7270565797219191,
+            "angularVelocity": 0.8375939718950361,
+            "velocityX": 1.1638871555335075,
+            "velocityY": 0.08848272107248593,
+            "timestamp": 2.8040507335373466
+        },
+        {
+            "x": 3.140351376164449,
+            "y": 6.359852912614292,
+            "heading": 0.8083350217640537,
+            "angularVelocity": 0.83699875417797,
+            "velocityX": 1.2067215654083316,
+            "velocityY": 0.09173914083795442,
+            "timestamp": 2.9011577383789766
+        },
+        {
+            "x": 3.261472243862432,
+            "y": 6.3690609392298585,
+            "heading": 0.8891526159463483,
+            "angularVelocity": 0.8322529806195065,
+            "velocityX": 1.2472927986960072,
+            "velocityY": 0.09482350610318373,
+            "timestamp": 2.9982647432206067
+        },
+        {
+            "x": 3.386280226554517,
+            "y": 6.378549273066906,
+            "heading": 0.9690024828336246,
+            "angularVelocity": 0.8222874040565998,
+            "velocityX": 1.2852624076036416,
+            "velocityY": 0.09771008689812195,
+            "timestamp": 3.0953717480622367
+        },
+        {
+            "x": 3.5144942146424594,
+            "y": 6.388296543201246,
+            "heading": 1.0472679478096496,
+            "angularVelocity": 0.8059713626886263,
+            "velocityX": 1.3203371728362745,
+            "velocityY": 0.10037659168026988,
+            "timestamp": 3.1924787529038667
+        },
+        {
+            "x": 3.64579126909874,
+            "y": 6.398278198691388,
+            "heading": 1.1233630936332748,
+            "angularVelocity": 0.783621593451087,
+            "velocityX": 1.3520863383515835,
+            "velocityY": 0.10279027288912505,
+            "timestamp": 3.2895857577454968
+        },
+        {
+            "x": 3.779855631903331,
+            "y": 6.408470234520327,
+            "heading": 1.1966377830577097,
+            "angularVelocity": 0.7545767639735752,
+            "velocityX": 1.3805838535787394,
+            "velocityY": 0.10495675240615632,
+            "timestamp": 3.386692762587127
+        },
+        {
+            "x": 3.916399996401096,
+            "y": 6.418850808641588,
+            "heading": 1.2663868507253695,
+            "angularVelocity": 0.7182701992663435,
+            "velocityX": 1.406122708347489,
+            "velocityY": 0.10689830431744617,
+            "timestamp": 3.483799767428757
+        },
+        {
+            "x": 4.055149110841729,
+            "y": 6.429398995480507,
+            "heading": 1.3319162676094702,
+            "angularVelocity": 0.6748165803464835,
+            "velocityX": 1.4288270430729209,
+            "velocityY": 0.10862436634220823,
+            "timestamp": 3.580906772270387
+        },
+        {
+            "x": 4.195643021249515,
+            "y": 6.440079827739076,
+            "heading": 1.392465239071673,
+            "angularVelocity": 0.6235283604234658,
+            "velocityX": 1.4467948093350342,
+            "velocityY": 0.10999033795341527,
+            "timestamp": 3.678013777112017
+        },
+        {
+            "x": 4.337478451024624,
+            "y": 6.45086264692987,
+            "heading": 1.4461855651355748,
+            "angularVelocity": 0.5532075282023133,
+            "velocityX": 1.4606096661273347,
+            "velocityY": 0.11104059096144768,
+            "timestamp": 3.775120781953647
+        },
+        {
+            "x": 4.47965775917095,
+            "y": 6.461671608979337,
+            "heading": 1.49351259060778,
+            "angularVelocity": 0.48736984118650195,
+            "velocityX": 1.4641508974652757,
+            "velocityY": 0.11130980792840979,
+            "timestamp": 3.872227786795277
+        },
+        {
+            "x": 4.621471854948602,
+            "y": 6.4724528062914635,
+            "heading": 1.5350394838354429,
+            "angularVelocity": 0.42764055328666206,
+            "velocityX": 1.4603899702927075,
+            "velocityY": 0.11102388888102613,
+            "timestamp": 3.969334791636907
+        },
+        {
+            "x": 4.762134624452166,
+            "y": 6.483146475819268,
+            "heading": 1.5711187532761213,
+            "angularVelocity": 0.37154136862160403,
+            "velocityX": 1.4485337063274482,
+            "velocityY": 0.11012253482900584,
+            "timestamp": 4.0664417964785375
+        },
+        {
+            "x": 4.901310758087736,
+            "y": 6.493727126167039,
+            "heading": 1.6012523506615222,
+            "angularVelocity": 0.3103133235343918,
+            "velocityX": 1.433224451514287,
+            "velocityY": 0.10895867241503827,
+            "timestamp": 4.163548801320168
+        },
+        {
+            "x": 5.0385652541984065,
+            "y": 6.504161687000508,
+            "heading": 1.6187441808494436,
+            "angularVelocity": 0.18012943788636607,
+            "velocityX": 1.413435585164251,
+            "velocityY": 0.1074542543793728,
+            "timestamp": 4.2606558061617985
+        },
+        {
+            "x": 5.1726571421935885,
+            "y": 6.5143558154807435,
+            "heading": 1.6310181673546418,
+            "angularVelocity": 0.12639650992409435,
+            "velocityX": 1.3808673050498854,
+            "velocityY": 0.104978301436634,
+            "timestamp": 4.357762811003429
+        },
+        {
+            "x": 5.303636473288953,
+            "y": 6.524313316659959,
+            "heading": 1.6376756877148975,
+            "angularVelocity": 0.06855860046187665,
+            "velocityX": 1.3488144481980282,
+            "velocityY": 0.1025415325257505,
+            "timestamp": 4.4548698158450595
+        },
+        {
+            "x": 5.4313983902221485,
+            "y": 6.53402621894025,
+            "heading": 1.6386463375950402,
+            "angularVelocity": 0.009995672907119661,
+            "velocityX": 1.3156817797402116,
+            "velocityY": 0.1000226726538996,
+            "timestamp": 4.55197682068669
+        },
+        {
+            "x": 5.555758593734715,
+            "y": 6.5434805112143435,
+            "heading": 1.6339151298589827,
+            "angularVelocity": -0.048721590949420676,
+            "velocityX": 1.2806512124922533,
+            "velocityY": 0.09735952745677612,
+            "timestamp": 4.6490838255283204
+        },
+        {
+            "x": 5.676522731590665,
+            "y": 6.552661418256687,
+            "heading": 1.6211253547169766,
+            "angularVelocity": -0.13170805962068485,
+            "velocityX": 1.2436192217305724,
+            "velocityY": 0.0945442281350229,
+            "timestamp": 4.746190830369951
+        },
+        {
+            "x": 5.8040475153273645,
+            "y": 6.568758338060969,
+            "heading": 1.594817916740329,
+            "angularVelocity": -0.24917360707174604,
+            "velocityX": 1.2078641151592902,
+            "velocityY": 0.15246363592342288,
+            "timestamp": 4.851769580629666
+        },
+        {
+            "x": 5.9283875752902055,
+            "y": 6.58985033542802,
+            "heading": 1.559498986776326,
+            "angularVelocity": -0.33452688097090233,
+            "velocityX": 1.1776996757735678,
+            "velocityY": 0.19977502680380196,
+            "timestamp": 4.957348330889381
+        },
+        {
+            "x": 6.049721376535089,
+            "y": 6.61532142807792,
+            "heading": 1.5174190958344496,
+            "angularVelocity": -0.3985640187289682,
+            "velocityX": 1.1492255865917638,
+            "velocityY": 0.2412520781772084,
+            "timestamp": 5.0629270811490965
+        },
+        {
+            "x": 6.167915961542227,
+            "y": 6.644893212673861,
+            "heading": 1.4694436889435494,
+            "angularVelocity": -0.45440400479138554,
+            "velocityX": 1.119492176613303,
+            "velocityY": 0.280092203729064,
+            "timestamp": 5.168505831408812
+        },
+        {
+            "x": 6.2827599678090085,
+            "y": 6.678289224223258,
+            "heading": 1.4161083022197893,
+            "angularVelocity": -0.5051716053929952,
+            "velocityX": 1.087756824956101,
+            "velocityY": 0.31631376267017064,
+            "timestamp": 5.274084581668527
+        },
+        {
+            "x": 6.394018817655917,
+            "y": 6.715173356117064,
+            "heading": 1.3577889107275223,
+            "angularVelocity": -0.5523781194839231,
+            "velocityX": 1.0537996466964168,
+            "velocityY": 0.34935185331081736,
+            "timestamp": 5.379663331928242
+        },
+        {
+            "x": 6.501464569355021,
+            "y": 6.755149800954155,
+            "heading": 1.2948750452839908,
+            "angularVelocity": -0.5958951523626541,
+            "velocityX": 1.0176834949234126,
+            "velocityY": 0.3786410134453019,
+            "timestamp": 5.485242082187957
+        },
+        {
+            "x": 6.6048776092971675,
+            "y": 6.797770420027359,
+            "heading": 1.22768881668393,
+            "angularVelocity": -0.6363612797096425,
+            "velocityX": 0.9794872502530397,
+            "velocityY": 0.4036855817031342,
+            "timestamp": 5.5908208324476725
+        },
+        {
+            "x": 6.70404339771049,
+            "y": 6.8425429323292,
+            "heading": 1.1564439654233045,
+            "angularVelocity": -0.6748029419532755,
+            "velocityX": 0.939258970068964,
+            "velocityY": 0.4240674597490353,
+            "timestamp": 5.696399582707388
+        },
+        {
+            "x": 6.79877778418725,
+            "y": 6.888932915699562,
+            "heading": 1.0813771004184407,
+            "angularVelocity": -0.7110035384939538,
+            "velocityX": 0.8972864903430491,
+            "velocityY": 0.4393875033009405,
+            "timestamp": 5.801978332967103
+        },
+        {
+            "x": 6.88894352921239,
+            "y": 6.936373258053616,
+            "heading": 1.0028508284852347,
+            "angularVelocity": -0.7437696672526416,
+            "velocityX": 0.8540141324253476,
+            "velocityY": 0.44933608637180433,
+            "timestamp": 5.907557083226818
+        },
+        {
+            "x": 6.974457896759412,
+            "y": 6.984282785155121,
+            "heading": 0.9213875618911144,
+            "angularVelocity": -0.7715877144447874,
+            "velocityX": 0.809958131723675,
+            "velocityY": 0.45378001785130806,
+            "timestamp": 6.013135833486533
+        },
+        {
+            "x": 7.055294443748765,
+            "y": 7.032089281864471,
+            "heading": 0.8376815844807217,
+            "angularVelocity": -0.792829780737162,
+            "velocityX": 0.7656516726874414,
+            "velocityY": 0.45280415475213037,
+            "timestamp": 6.118714583746248
+        },
+        {
+            "x": 7.131475603500295,
+            "y": 7.0792526481673885,
+            "heading": 0.752591160441856,
+            "angularVelocity": -0.805942709434023,
+            "velocityX": 0.7215576931722669,
+            "velocityY": 0.44671267879627613,
+            "timestamp": 6.224293334005964
+        },
+        {
+            "x": 7.203056823429108,
+            "y": 7.125284419454981,
+            "heading": 0.6670903070961578,
+            "angularVelocity": -0.8098301328249344,
+            "velocityX": 0.6779888895797812,
+            "velocityY": 0.4359946600433992,
+            "timestamp": 6.329872084265679
+        },
+        {
+            "x": 7.270104931236086,
+            "y": 7.169760877903185,
+            "heading": 0.5821807537401323,
+            "angularVelocity": -0.8042295741051197,
+            "velocityX": 0.6350530514787667,
+            "velocityY": 0.4212633543513555,
+            "timestamp": 6.435450834525394
+        },
+        {
+            "x": 7.332675465601759,
+            "y": 7.212330496694183,
+            "heading": 0.49884759114131233,
+            "angularVelocity": -0.789298626772507,
+            "velocityX": 0.592643253784208,
+            "velocityY": 0.4032025262817465,
+            "timestamp": 6.541029584785109
+        },
+        {
+            "x": 7.390806680523043,
+            "y": 7.252712685445242,
+            "heading": 0.4183197118253645,
+            "angularVelocity": -0.7627280973825078,
+            "velocityX": 0.5505957828143127,
+            "velocityY": 0.38248405756315146,
+            "timestamp": 6.646608335044824
+        },
+        {
+            "x": 7.444527736090263,
+            "y": 7.290684851015479,
+            "heading": 0.34198396458440167,
+            "angularVelocity": -0.7230218869110173,
+            "velocityX": 0.5088245046789369,
+            "velocityY": 0.3596572750296197,
+            "timestamp": 6.75218708530454
+        },
+        {
+            "x": 7.4938504955203875,
+            "y": 7.326060981562411,
+            "heading": 0.27119241907250147,
+            "angularVelocity": -0.6705094095505424,
+            "velocityX": 0.46716559024619986,
+            "velocityY": 0.3350686622032283,
+            "timestamp": 6.857765835564255
+        },
+        {
+            "x": 7.538762197286186,
+            "y": 7.358679050922846,
+            "heading": 0.2071469006235064,
+            "angularVelocity": -0.6066137198463124,
+            "velocityX": 0.4253858037038331,
+            "velocityY": 0.3089454019040213,
+            "timestamp": 6.96334458582397
+        },
+        {
+            "x": 7.57923356269503,
+            "y": 7.3883979284340455,
+            "heading": 0.15090824268085423,
+            "angularVelocity": -0.5326702370042972,
+            "velocityX": 0.38332870100058564,
+            "velocityY": 0.28148540800086536,
+            "timestamp": 7.068923336083685
+        },
+        {
+            "x": 7.612613012416259,
+            "y": 7.4137927319874946,
+            "heading": 0.0952672119425909,
+            "angularVelocity": -0.5270097494122625,
+            "velocityX": 0.316156891737545,
+            "velocityY": 0.2405294965409898,
+            "timestamp": 7.1745020863434
+        },
+        {
+            "x": 7.641161660602441,
+            "y": 7.4358681154587,
+            "heading": 0.04946855188730579,
+            "angularVelocity": -0.43378672242570576,
+            "velocityX": 0.27040145789287734,
+            "velocityY": 0.20908926766306235,
+            "timestamp": 7.280080836603116
+        },
+        {
+            "x": 7.664737893553612,
+            "y": 7.454419635462092,
+            "heading": 0.014221249681079469,
+            "angularVelocity": -0.3338484505242331,
+            "velocityX": 0.22330471529975876,
+            "velocityY": 0.17571263220528385,
+            "timestamp": 7.385659586862831
+        },
+        {
+            "x": 7.679999999654181,
+            "y": 7.467000000250119,
+            "heading": 5.571589197087409e-11,
+            "angularVelocity": -0.13469802934148392,
+            "velocityX": 0.1445566071683118,
+            "velocityY": 0.11915622105321995,
+            "timestamp": 7.491238337122546
+        },
+        {
+            "x": 7.6799999998383255,
+            "y": 7.467000000125839,
+            "heading": 2.732434354976469e-11,
+            "angularVelocity": -1.9015024944340445e-11,
+            "velocityX": 1.5544110263140067e-10,
+            "velocityY": -2.911726567326558e-11,
+            "timestamp": 7.596817087382261
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.1.traj
new file mode 100644
index 00000000..de7450e6
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.1.traj	
@@ -0,0 +1,634 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 0,
+            "velocityX": -2.538537990246625e-31,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2837844841867163,
+            "y": 4.387687502531162,
+            "heading": -0.3845108824411683,
+            "angularVelocity": 0.0884354987329395,
+            "velocityX": -0.26124913754506907,
+            "velocityY": 0.4460749963360829,
+            "timestamp": 0.06206916495779045
+        },
+        {
+            "x": 2.252431932443338,
+            "y": 4.44366896754607,
+            "heading": -0.37369458929412197,
+            "angularVelocity": 0.17426194076711193,
+            "velocityX": -0.5051228216917889,
+            "velocityY": 0.9019207049574874,
+            "timestamp": 0.1241383299155809
+        },
+        {
+            "x": 2.2074803876744995,
+            "y": 4.528718281766582,
+            "heading": -0.3577885652255258,
+            "angularVelocity": 0.2562628976859123,
+            "velocityX": -0.724217005329779,
+            "velocityY": 1.3702345484796434,
+            "timestamp": 0.18620749487337135
+        },
+        {
+            "x": 2.1512700092503536,
+            "y": 4.643824218738039,
+            "heading": -0.3371682293163389,
+            "angularVelocity": 0.3322154555127859,
+            "velocityX": -0.905608742482958,
+            "velocityY": 1.8544785812676818,
+            "timestamp": 0.2482766598311618
+        },
+        {
+            "x": 2.087674884006875,
+            "y": 4.7901724592691535,
+            "heading": -0.3124919375355934,
+            "angularVelocity": 0.3975612012535503,
+            "velocityX": -1.0245848367214008,
+            "velocityY": 2.357825187926933,
+            "timestamp": 0.31034582478895223
+        },
+        {
+            "x": 2.0237484850052736,
+            "y": 4.968633105417482,
+            "heading": -0.2850705208032402,
+            "angularVelocity": 0.44178807225385713,
+            "velocityX": -1.029922007895696,
+            "velocityY": 2.8751900604698215,
+            "timestamp": 0.3724149897467427
+        },
+        {
+            "x": 1.9724195760606846,
+            "y": 5.1766438961084935,
+            "heading": -0.25765844821795814,
+            "angularVelocity": 0.4416375281458893,
+            "velocityX": -0.8269630980122488,
+            "velocityY": 3.351274192787545,
+            "timestamp": 0.43448415470453317
+        },
+        {
+            "x": 1.9484287592243066,
+            "y": 5.401486281993408,
+            "heading": -0.23420864800659155,
+            "angularVelocity": 0.3778011227958159,
+            "velocityX": -0.38651747373332257,
+            "velocityY": 3.6224490217904175,
+            "timestamp": 0.49655331966232363
+        },
+        {
+            "x": 1.9564542032821701,
+            "y": 5.628890453578366,
+            "heading": -0.21272720707613346,
+            "angularVelocity": 0.34608876961188295,
+            "velocityX": 0.12929840547219124,
+            "velocityY": 3.663722103228105,
+            "timestamp": 0.5586224846201141
+        },
+        {
+            "x": 1.99619273658118,
+            "y": 5.8512380599288125,
+            "heading": -0.18843692284079885,
+            "angularVelocity": 0.39134221077502945,
+            "velocityX": 0.6402298681838031,
+            "velocityY": 3.5822554806643043,
+            "timestamp": 0.6206916495779046
+        },
+        {
+            "x": 2.0663437221615903,
+            "y": 6.063242956223716,
+            "heading": -0.16364621155748288,
+            "angularVelocity": 0.3994046206362532,
+            "velocityX": 1.130206691653697,
+            "velocityY": 3.4156234651942974,
+            "timestamp": 0.682760814535695
+        },
+        {
+            "x": 2.1656765937805176,
+            "y": 6.261821746826172,
+            "heading": -0.13977158803318993,
+            "angularVelocity": 0.38464547638239754,
+            "velocityX": 1.6003577893593972,
+            "velocityY": 3.1993146796407252,
+            "timestamp": 0.7448299794934855
+        },
+        {
+            "x": 2.225814155156259,
+            "y": 6.362141570414172,
+            "heading": -0.12770962458788607,
+            "angularVelocity": 0.3689752694416071,
+            "velocityX": 1.8396070434406133,
+            "velocityY": 3.068781803712599,
+            "timestamp": 0.7775204182423698
+        },
+        {
+            "x": 2.2936460979223643,
+            "y": 6.4579721340232545,
+            "heading": -0.11638786741998439,
+            "angularVelocity": 0.3463323712222401,
+            "velocityX": 2.074978047465827,
+            "velocityY": 2.9314554125487073,
+            "timestamp": 0.8102108569912542
+        },
+        {
+            "x": 2.3690076803030626,
+            "y": 6.549046337769426,
+            "heading": -0.10608509188235166,
+            "angularVelocity": 0.31516173940473013,
+            "velocityX": 2.3053096031972573,
+            "velocityY": 2.7859584401928488,
+            "timestamp": 0.8429012957401385
+        },
+        {
+            "x": 2.4516786592945876,
+            "y": 6.635038598364521,
+            "heading": -0.09714822011858378,
+            "angularVelocity": 0.27337876474752265,
+            "velocityX": 2.528903928955548,
+            "velocityY": 2.6305018802439832,
+            "timestamp": 0.8755917344890228
+        },
+        {
+            "x": 2.5413546021547084,
+            "y": 6.715546857152131,
+            "heading": -0.09001694608551258,
+            "angularVelocity": 0.21814555896345397,
+            "velocityX": 2.7431856619836545,
+            "velocityY": 2.462746352407108,
+            "timestamp": 0.9082821732379072
+        },
+        {
+            "x": 2.6375981860796323,
+            "y": 6.79006985383775,
+            "heading": -0.08525828181365991,
+            "angularVelocity": 0.14556746421386957,
+            "velocityX": 2.9440896974181805,
+            "velocityY": 2.2796572801553694,
+            "timestamp": 0.9409726119867915
+        },
+        {
+            "x": 2.73984178672467,
+            "y": 6.858044565031264,
+            "heading": -0.08306946201564708,
+            "angularVelocity": 0.06695596274838787,
+            "velocityX": 3.1276301131997113,
+            "velocityY": 2.079345331378129,
+            "timestamp": 0.9736630507356758
+        },
+        {
+            "x": 2.847369129600613,
+            "y": 6.918863246924361,
+            "heading": -0.08274471887343776,
+            "angularVelocity": 0.009933887583435854,
+            "velocityX": 3.2892597037781757,
+            "velocityY": 1.8604425091962247,
+            "timestamp": 1.0063534894845603
+        },
+        {
+            "x": 2.9591159211859868,
+            "y": 6.971834136920771,
+            "heading": -0.08274467756065068,
+            "angularVelocity": 0.0000012637574442965605,
+            "velocityX": 3.418332572524967,
+            "velocityY": 1.6203786802362972,
+            "timestamp": 1.0390439282334447
+        },
+        {
+            "x": 3.0743869446602656,
+            "y": 7.0166209198254075,
+            "heading": -0.0827446619031275,
+            "angularVelocity": 4.789633846658565e-7,
+            "velocityX": 3.526138769803835,
+            "velocityY": 1.3700269748141811,
+            "timestamp": 1.0717343669823292
+        },
+        {
+            "x": 3.192583723464696,
+            "y": 7.052991069898469,
+            "heading": -0.08274464937093684,
+            "angularVelocity": 3.833595125790563e-7,
+            "velocityX": 3.61563757869459,
+            "velocityY": 1.1125623107197633,
+            "timestamp": 1.1044248057312136
+        },
+        {
+            "x": 3.313092599420772,
+            "y": 7.080755764680444,
+            "heading": -0.08274463888121661,
+            "angularVelocity": 3.208803751652956e-7,
+            "velocityX": 3.6863645936910636,
+            "velocityY": 0.8493215706001247,
+            "timestamp": 1.137115244480098
+        },
+        {
+            "x": 3.4352879115666495,
+            "y": 7.0997708632875725,
+            "heading": -0.08274462977736693,
+            "angularVelocity": 2.7848661636087506e-7,
+            "velocityX": 3.7379526498424447,
+            "velocityY": 0.5816715631449295,
+            "timestamp": 1.1698056832289825
+        },
+        {
+            "x": 3.5585352449602814,
+            "y": 7.109937657437644,
+            "heading": -0.08274462163132143,
+            "angularVelocity": 2.491874023135916e-7,
+            "velocityX": 3.770133963033834,
+            "velocityY": 0.31100207091115845,
+            "timestamp": 1.202496121977867
+        },
+        {
+            "x": 3.6821947259931562,
+            "y": 7.1112033907952945,
+            "heading": -0.08274461414532981,
+            "angularVelocity": 2.2899636479565772e-7,
+            "velocityX": 3.782741552745206,
+            "velocityY": 0.03871876321990418,
+            "timestamp": 1.2351865607267514
+        },
+        {
+            "x": 3.8056243495254822,
+            "y": 7.103561554173401,
+            "heading": -0.08274460709859606,
+            "angularVelocity": 2.1555947310703947e-7,
+            "velocityX": 3.775710215468651,
+            "velocityY": -0.23376366040158306,
+            "timestamp": 1.2678769994756358
+        },
+        {
+            "x": 3.9281833350664277,
+            "y": 7.087052015079957,
+            "heading": -0.08274460031581693,
+            "angularVelocity": 2.0748510569540933e-7,
+            "velocityX": 3.7490774131963343,
+            "velocityY": -0.5050265375893709,
+            "timestamp": 1.3005674382245203
+        },
+        {
+            "x": 4.0492357154991785,
+            "y": 7.0617618768818975,
+            "heading": -0.08274459364684544,
+            "angularVelocity": 2.0400373145008536e-7,
+            "velocityX": 3.7029903869628296,
+            "velocityY": -0.7736249241717706,
+            "timestamp": 1.3332578769734047
+        },
+        {
+            "x": 4.169706806774688,
+            "y": 7.033833646086136,
+            "heading": -0.0827445869919534,
+            "angularVelocity": 2.0357304165195936e-7,
+            "velocityX": 3.6852087609158533,
+            "velocityY": -0.8543241346506245,
+            "timestamp": 1.3659483157222891
+        },
+        {
+            "x": 4.290177832890272,
+            "y": 7.00590513421804,
+            "heading": -0.08274458033705716,
+            "angularVelocity": 2.0357317018224517e-7,
+            "velocityX": 3.6852067676748597,
+            "velocityY": -0.8543327326486478,
+            "timestamp": 1.3986387544711736
+        },
+        {
+            "x": 4.410648856777724,
+            "y": 6.977976612738755,
+            "heading": -0.08274457368215653,
+            "angularVelocity": 2.0357330400976238e-7,
+            "velocityX": 3.685206699516314,
+            "velocityY": -0.8543330266547546,
+            "timestamp": 1.431329193220058
+        },
+        {
+            "x": 4.531119878449975,
+            "y": 6.950048081704109,
+            "heading": -0.08274456702725173,
+            "angularVelocity": 2.0357343141081232e-7,
+            "velocityX": 3.6852066317533563,
+            "velocityY": -0.8543333189530982,
+            "timestamp": 1.4640196319689425
+        },
+        {
+            "x": 4.651590897922017,
+            "y": 6.922119541178764,
+            "heading": -0.082744560372343,
+            "angularVelocity": 2.0357355233562972e-7,
+            "velocityX": 3.6852065644490066,
+            "velocityY": -0.8543336092734171,
+            "timestamp": 1.496710070717827
+        },
+        {
+            "x": 4.772061915216332,
+            "y": 6.894190991259702,
+            "heading": -0.08274455371743049,
+            "angularVelocity": 2.0357366776279015e-7,
+            "velocityX": 3.68520649783238,
+            "velocityY": -0.8543338966271132,
+            "timestamp": 1.5294005094667114
+        },
+        {
+            "x": 4.892532930362486,
+            "y": 6.866262432074455,
+            "heading": -0.08274454706251447,
+            "angularVelocity": 2.0357377502591166e-7,
+            "velocityX": 3.6852064321201583,
+            "velocityY": -0.8543341800795031,
+            "timestamp": 1.5620909482155958
+        },
+        {
+            "x": 5.0130039433965745,
+            "y": 6.838333863778735,
+            "heading": -0.08274454040759514,
+            "angularVelocity": 2.0357387593869802e-7,
+            "velocityX": 3.685206367512119,
+            "velocityY": -0.854334458768776,
+            "timestamp": 1.5947813869644802
+        },
+        {
+            "x": 5.133474954360567,
+            "y": 6.810405286553609,
+            "heading": -0.08274453375267282,
+            "angularVelocity": 2.0357396812707888e-7,
+            "velocityX": 3.6852063041879153,
+            "velocityY": -0.8543347319191732,
+            "timestamp": 1.6274718257133647
+        },
+        {
+            "x": 5.253945963272805,
+            "y": 6.782476700477742,
+            "heading": -0.08274452709774775,
+            "angularVelocity": 2.035740515232947e-7,
+            "velocityX": 3.6852062414248348,
+            "velocityY": -0.8543350026628553,
+            "timestamp": 1.6601622644622491
+        },
+        {
+            "x": 5.374416652936201,
+            "y": 6.754546737348104,
+            "heading": -0.08274452044281651,
+            "angularVelocity": 2.0357424100563973e-7,
+            "velocityX": 3.685196475607398,
+            "velocityY": -0.8543771267177642,
+            "timestamp": 1.6928527032111336
+        },
+        {
+            "x": 5.493278503417969,
+            "y": 6.720412731170654,
+            "heading": -0.08274451374626977,
+            "angularVelocity": 2.0484725764732438e-7,
+            "velocityX": 3.6359821106976624,
+            "velocityY": -1.0441587046150123,
+            "timestamp": 1.725543141960018
+        },
+        {
+            "x": 5.6078450645507765,
+            "y": 6.6784969077855525,
+            "heading": -0.0827445073182203,
+            "angularVelocity": 1.9932954157588389e-7,
+            "velocityX": 3.5526329088991675,
+            "velocityY": -1.2997818219306485,
+            "timestamp": 1.7577914952435287
+        },
+        {
+            "x": 5.720102544926552,
+            "y": 6.630740323854381,
+            "heading": -0.08274450093221485,
+            "angularVelocity": 1.980257845223315e-7,
+            "velocityX": 3.481029849458213,
+            "velocityY": -1.4808999241394636,
+            "timestamp": 1.7900398485270395
+        },
+        {
+            "x": 5.832359462510262,
+            "y": 6.582982417035901,
+            "heading": -0.08274449454621227,
+            "angularVelocity": 1.9802569455219195e-7,
+            "velocityX": 3.481012397650423,
+            "velocityY": -1.480940945995501,
+            "timestamp": 1.8222882018105502
+        },
+        {
+            "x": 5.944616376948871,
+            "y": 6.535224502824747,
+            "heading": -0.08274448816020867,
+            "angularVelocity": 1.9802572730548282e-7,
+            "velocityX": 3.4810123001228868,
+            "velocityY": -1.4809411752374333,
+            "timestamp": 1.854536555094061
+        },
+        {
+            "x": 6.0568732883758765,
+            "y": 6.487466581534697,
+            "heading": -0.0827444817742042,
+            "angularVelocity": 1.9802575385283685e-7,
+            "velocityX": 3.4810122067350586,
+            "velocityY": -1.4809413947492354,
+            "timestamp": 1.8867849083775716
+        },
+        {
+            "x": 6.169130196876316,
+            "y": 6.439708653365641,
+            "heading": -0.08274447538819907,
+            "angularVelocity": 1.980257744166014e-7,
+            "velocityX": 3.4810121159842042,
+            "velocityY": -1.4809416080626625,
+            "timestamp": 1.9190332616610823
+        },
+        {
+            "x": 6.281387102537832,
+            "y": 6.391950718523587,
+            "heading": -0.08274446900219351,
+            "angularVelocity": 1.9802578764982062e-7,
+            "velocityX": 3.481012027951073,
+            "velocityY": -1.4809418149879348,
+            "timestamp": 1.951281614944593
+        },
+        {
+            "x": 6.393644005449787,
+            "y": 6.344192777218584,
+            "heading": -0.08274446261618765,
+            "angularVelocity": 1.9802579747327125e-7,
+            "velocityX": 3.481011942689016,
+            "velocityY": -1.4809420153996509,
+            "timestamp": 1.9835299682281038
+        },
+        {
+            "x": 6.505900905702341,
+            "y": 6.296434829662559,
+            "heading": -0.08274445623018166,
+            "angularVelocity": 1.9802580083930334e-7,
+            "velocityX": 3.4810118602227385,
+            "velocityY": -1.4809422092397408,
+            "timestamp": 2.0157783215116143
+        },
+        {
+            "x": 6.618157803385852,
+            "y": 6.2486768760679015,
+            "heading": -0.08274444984417571,
+            "angularVelocity": 1.9802579953185327e-7,
+            "velocityX": 3.4810117805584304,
+            "velocityY": -1.4809423964936643,
+            "timestamp": 2.048026674795125
+        },
+        {
+            "x": 6.730414698590335,
+            "y": 6.200918916646197,
+            "heading": -0.08274444345816992,
+            "angularVelocity": 1.9802579502845229e-7,
+            "velocityX": 3.4810117036854167,
+            "velocityY": -1.4809425771865148,
+            "timestamp": 2.0802750280786353
+        },
+        {
+            "x": 6.842671592155369,
+            "y": 6.153160953370972,
+            "heading": -0.08274443707212797,
+            "angularVelocity": 1.980269160510039e-7,
+            "velocityX": 3.4810116528472412,
+            "velocityY": -1.4809426966816255,
+            "timestamp": 2.112523381362146
+        },
+        {
+            "x": 6.954235049253699,
+            "y": 6.105720053055757,
+            "heading": -0.08066018407058251,
+            "angularVelocity": 0.06463130018523208,
+            "velocityX": 3.459508648938503,
+            "velocityY": -1.4711107850419303,
+            "timestamp": 2.1447717346456563
+        },
+        {
+            "x": 7.05792262192444,
+            "y": 6.061617369901677,
+            "heading": -0.07347223812764758,
+            "angularVelocity": 0.2228934258982459,
+            "velocityX": 3.215282707900597,
+            "velocityY": -1.3675948897716332,
+            "timestamp": 2.177020087929167
+        },
+        {
+            "x": 7.153639251658588,
+            "y": 6.020903567478063,
+            "heading": -0.06480991744029473,
+            "angularVelocity": 0.2686128067130366,
+            "velocityX": 2.9681090656833784,
+            "velocityY": -1.2625079508922343,
+            "timestamp": 2.2092684412126773
+        },
+        {
+            "x": 7.241376863236811,
+            "y": 5.983582900712933,
+            "heading": -0.05573995757660787,
+            "angularVelocity": 0.28125342661540725,
+            "velocityX": 2.7206850162822076,
+            "velocityY": -1.1572890695232254,
+            "timestamp": 2.241516794496188
+        },
+        {
+            "x": 7.321134939051253,
+            "y": 5.949655957408732,
+            "heading": -0.04677486060395727,
+            "angularVelocity": 0.2780016980662186,
+            "velocityX": 2.4732449162054873,
+            "velocityY": -1.0520519607910965,
+            "timestamp": 2.2737651477796983
+        },
+        {
+            "x": 7.392914197093691,
+            "y": 5.919122647933878,
+            "heading": -0.03821600737741098,
+            "angularVelocity": 0.26540434952773495,
+            "velocityX": 2.2258270805765674,
+            "velocityY": -0.946817631474723,
+            "timestamp": 2.306013501063209
+        },
+        {
+            "x": 7.456715523277294,
+            "y": 5.891982736437705,
+            "heading": -0.030261865024148932,
+            "angularVelocity": 0.24665266729552143,
+            "velocityX": 1.9784367165261438,
+            "velocityY": -0.8415906157307577,
+            "timestamp": 2.3382618543467193
+        },
+        {
+            "x": 7.512539743308544,
+            "y": 5.868235970303718,
+            "heading": -0.023053012750234723,
+            "angularVelocity": 0.2235417173255794,
+            "velocityX": 1.7310719570848203,
+            "velocityY": -0.736371433456386,
+            "timestamp": 2.37051020763023
+        },
+        {
+            "x": 7.560387579465668,
+            "y": 5.847882114762634,
+            "heading": -0.016694240391467878,
+            "angularVelocity": 0.19718130420078028,
+            "velocityX": 1.4837295950113008,
+            "velocityY": -0.6311595312214304,
+            "timestamp": 2.4027585609137403
+        },
+        {
+            "x": 7.600259653459209,
+            "y": 5.830920960662069,
+            "heading": -0.011266668966831511,
+            "angularVelocity": 0.1683053821980557,
+            "velocityX": 1.2364065117684333,
+            "velocityY": -0.5259541146628961,
+            "timestamp": 2.435006914197251
+        },
+        {
+            "x": 7.632156499892169,
+            "y": 5.817352324088122,
+            "heading": -0.006834955017927562,
+            "angularVelocity": 0.1374245038171387,
+            "velocityX": 0.9891000062092736,
+            "velocityY": -0.4207544011521409,
+            "timestamp": 2.4672552674807613
+        },
+        {
+            "x": 7.656078580521334,
+            "y": 5.807176043662066,
+            "heading": -0.0034518424131983775,
+            "angularVelocity": 0.10490807313441347,
+            "velocityX": 0.7418078194211917,
+            "velocityY": -0.31555969188850075,
+            "timestamp": 2.499503620764272
+        },
+        {
+            "x": 7.672026296758129,
+            "y": 5.800391977434502,
+            "heading": -0.0011611797809329624,
+            "angularVelocity": 0.07103192563441285,
+            "velocityX": 0.49452808013452626,
+            "velocityY": -0.21036938438134845,
+            "timestamp": 2.5317519740477823
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 1.9934013230001416e-31,
+            "angularVelocity": 0.036007413176266045,
+            "velocityX": 0.2472592374491084,
+            "velocityY": -0.10518296561320875,
+            "timestamp": 2.564000327331293
+        },
+        {
+            "x": 7.68,
+            "y": 5.797,
+            "heading": 1.0109342582388432e-32,
+            "angularVelocity": -5.855934233748412e-30,
+            "velocityX": 8.513095472636782e-32,
+            "velocityY": -2.1397535642954256e-34,
+            "timestamp": 2.5962486806148033
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.traj
index 0646af70..de7450e6 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.traj	
@@ -5,549 +5,630 @@
             "y": 4.36,
             "heading": -0.39,
             "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -3.138879459525789e-33,
+            "velocityX": -2.538537990246625e-31,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 2.304641315068496,
-            "y": 4.371901353899003,
-            "heading": -0.39554860460981267,
-            "angularVelocity": -0.1412451028521288,
-            "velocityX": 0.11814916908296796,
-            "velocityY": 0.3029604871416256,
-            "timestamp": 0.039283518492098594
-        },
-        {
-            "x": 2.314310378834228,
-            "y": 4.395560050627161,
-            "heading": -0.4062436571914542,
-            "angularVelocity": -0.27225291909092403,
-            "velocityX": 0.24613538035494495,
-            "velocityY": 0.6022550330596644,
-            "timestamp": 0.07856703698419719
-        },
-        {
-            "x": 2.3294686593461313,
-            "y": 4.43078385259534,
-            "heading": -0.42159423390969814,
-            "angularVelocity": -0.3907637937605655,
-            "velocityX": 0.385868707635076,
-            "velocityY": 0.8966559850097278,
-            "timestamp": 0.11785055547629578
-        },
-        {
-            "x": 2.3506733572393026,
-            "y": 4.477309554936099,
-            "heading": -0.44099148005781696,
-            "angularVelocity": -0.49377568233887975,
-            "velocityX": 0.5397861165984255,
-            "velocityY": 1.1843568022074944,
-            "timestamp": 0.15713407396839438
-        },
-        {
-            "x": 2.3786040121360466,
-            "y": 4.534766426387325,
-            "heading": -0.4636665715847995,
-            "angularVelocity": -0.5772164102747018,
-            "velocityX": 0.7110018646207296,
-            "velocityY": 1.462620296162777,
-            "timestamp": 0.19641759246049298
-        },
-        {
-            "x": 2.4140943920114943,
-            "y": 4.602616046670193,
-            "heading": -0.48862953085625704,
-            "angularVelocity": -0.6354562989682879,
-            "velocityX": 0.903441983756771,
-            "velocityY": 1.727177780588029,
-            "timestamp": 0.23570111095259158
-        },
-        {
-            "x": 2.458163120855333,
-            "y": 4.68005234098581,
-            "heading": -0.5145799632934547,
-            "angularVelocity": -0.6605933845360225,
-            "velocityX": 1.1218121628463973,
-            "velocityY": 1.9712158505149888,
-            "timestamp": 0.27498462944469015
-        },
-        {
-            "x": 2.5115097740388146,
-            "y": 4.766403162212065,
-            "heading": -0.5400224328341426,
-            "angularVelocity": -0.6476626971635644,
-            "velocityX": 1.3579907103842537,
-            "velocityY": 2.1981437646332993,
-            "timestamp": 0.31426814793678876
-        },
-        {
-            "x": 2.5744195202560527,
-            "y": 4.861351309968927,
-            "heading": -0.563860333505168,
-            "angularVelocity": -0.6068168429413633,
-            "velocityX": 1.6014285031491418,
-            "velocityY": 2.416997035944537,
-            "timestamp": 0.35355166642888736
-        },
-        {
-            "x": 2.6473242968990576,
-            "y": 4.964346697364103,
-            "heading": -0.5842892856317753,
-            "angularVelocity": -0.5200387569839264,
-            "velocityX": 1.8558616804566808,
-            "velocityY": 2.6218473127835553,
-            "timestamp": 0.39283518492098596
-        },
-        {
-            "x": 2.7309171688764238,
-            "y": 5.07424676618957,
-            "heading": -0.5978668700067779,
-            "angularVelocity": -0.3456305569399124,
-            "velocityX": 2.127937496081736,
-            "velocityY": 2.7976126641403014,
-            "timestamp": 0.43211870341308456
-        },
-        {
-            "x": 2.826173576921219,
-            "y": 5.187875652325513,
-            "heading": -0.5974605263008922,
-            "angularVelocity": 0.010343872481393205,
-            "velocityX": 2.4248441000503926,
-            "velocityY": 2.8925333192550147,
-            "timestamp": 0.47140222190518316
-        },
-        {
-            "x": 2.9311441629204515,
-            "y": 5.293067244130248,
-            "heading": -0.5974604636085413,
-            "angularVelocity": 0.000001595894492248037,
-            "velocityX": 2.672127905761066,
-            "velocityY": 2.677753822532693,
-            "timestamp": 0.5106857403972818
-        },
-        {
-            "x": 3.044820699228354,
-            "y": 5.388784226639372,
-            "heading": -0.597460407679312,
-            "angularVelocity": 0.0000014237326879146737,
-            "velocityX": 2.8937463005192323,
-            "velocityY": 2.436568468997834,
-            "timestamp": 0.5499692588893803
-        },
-        {
-            "x": 3.1663502143390825,
-            "y": 5.474310087323753,
-            "heading": -0.5974603544159754,
-            "angularVelocity": 0.0000013558698005366095,
-            "velocityX": 3.0936514796952492,
-            "velocityY": 2.177143595261183,
-            "timestamp": 0.5892527773814789
-        },
-        {
-            "x": 3.289918343405979,
-            "y": 5.556863181758907,
-            "heading": -0.5974603016382944,
-            "angularVelocity": 0.0000013435069733969365,
-            "velocityX": 3.145546371864123,
-            "velocityY": 2.1014689519673895,
-            "timestamp": 0.6285362958735774
-        },
-        {
-            "x": 3.413486607435696,
-            "y": 5.639416074177005,
-            "heading": -0.5974602488606314,
-            "angularVelocity": 0.0000013435065140042136,
-            "velocityX": 3.145549807473418,
-            "velocityY": 2.101463809427605,
-            "timestamp": 0.667819814365676
-        },
-        {
-            "x": 3.5370548851900594,
-            "y": 5.721968946051526,
-            "heading": -0.5974601960829673,
-            "angularVelocity": 0.0000013435065431603766,
-            "velocityX": 3.145550156847558,
-            "velocityY": 2.1014632864709872,
-            "timestamp": 0.7071033328577745
-        },
-        {
-            "x": 3.6606231741521,
-            "y": 5.804521801149959,
-            "heading": -0.5974601433053026,
-            "angularVelocity": 0.0000013435065598734946,
-            "velocityX": 3.1455504421498475,
-            "velocityY": 2.1014628594194082,
-            "timestamp": 0.746386851349873
-        },
-        {
-            "x": 3.7841914725218415,
-            "y": 5.887074642166574,
-            "heading": -0.597460090527637,
-            "angularVelocity": 0.0000013435065802330538,
-            "velocityX": 3.1455506816319834,
-            "velocityY": 2.101462500953099,
-            "timestamp": 0.7856703698419716
-        },
-        {
-            "x": 3.9077597789317933,
-            "y": 5.969627471148282,
-            "heading": -0.5974600377499704,
-            "angularVelocity": 0.0000013435066065618314,
-            "velocityX": 3.14555088630331,
-            "velocityY": 2.1014621945928877,
-            "timestamp": 0.8249538883340701
-        },
-        {
-            "x": 4.031328092300017,
-            "y": 6.052180289714568,
-            "heading": -0.5974599849723026,
-            "angularVelocity": 0.0000013435066364860128,
-            "velocityX": 3.1455510634328734,
-            "velocityY": 2.101461929458217,
-            "timestamp": 0.8642374068261687
-        },
-        {
-            "x": 4.154896411746533,
-            "y": 6.134733099182619,
-            "heading": -0.5974599321946334,
-            "angularVelocity": 0.0000013435066718879358,
-            "velocityX": 3.14555121816168,
-            "velocityY": 2.1014616978538454,
-            "timestamp": 0.9035209253182672
-        },
-        {
-            "x": 4.27846473654201,
-            "y": 6.217285900644126,
-            "heading": -0.5974598794169629,
-            "angularVelocity": 0.000001343506708988157,
-            "velocityX": 3.14555135432466,
-            "velocityY": 2.1014614940395124,
-            "timestamp": 0.9428044438103658
-        },
-        {
-            "x": 4.4020330660739395,
-            "y": 6.299838695015915,
-            "heading": -0.5974598266392908,
-            "angularVelocity": 0.0000013435067467217265,
-            "velocityX": 3.1455514748956417,
-            "velocityY": 2.1014613135638847,
-            "timestamp": 0.9820879623024643
-        },
-        {
-            "x": 4.525601399950297,
-            "y": 6.382391482884783,
-            "heading": -0.5974597738616171,
-            "angularVelocity": 0.0000013435067857429384,
-            "velocityX": 3.1455515854872873,
-            "velocityY": 2.101461148025713,
-            "timestamp": 1.0213714807945629
-        },
-        {
-            "x": 4.6491721527053596,
-            "y": 6.464940649963802,
-            "heading": -0.5974597210842042,
-            "angularVelocity": 0.0000013435001464771913,
-            "velocityX": 3.1456131603868194,
-            "velocityY": 2.101368977314248,
-            "timestamp": 1.0606549992866614
-        },
-        {
-            "x": 4.778837800088318,
-            "y": 6.537541722502267,
-            "heading": -0.5974596685786843,
-            "angularVelocity": 0.0000013365788503576918,
-            "velocityX": 3.300764604603488,
-            "velocityY": 1.8481305984081453,
-            "timestamp": 1.09993851777876
-        },
-        {
-            "x": 4.914297553492409,
-            "y": 6.59865432938236,
-            "heading": -0.5974596146272461,
-            "angularVelocity": 0.0000013733860988547744,
-            "velocityX": 3.4482591835906553,
-            "velocityY": 1.5556805812161723,
-            "timestamp": 1.1392220362708585
-        },
-        {
-            "x": 5.054536029054771,
-            "y": 6.647820017049671,
-            "heading": -0.5974595570647036,
-            "angularVelocity": 0.0000014653102566024163,
-            "velocityX": 3.5699061831898264,
-            "velocityY": 1.2515601848929776,
-            "timestamp": 1.178505554762957
-        },
-        {
-            "x": 5.198501873691458,
-            "y": 6.684670110841039,
-            "heading": -0.5974594930097576,
-            "angularVelocity": 0.0000016305806703071774,
-            "velocityX": 3.6647899720507366,
-            "velocityY": 0.9380548180449542,
-            "timestamp": 1.2177890732550556
-        },
-        {
-            "x": 5.345115761176417,
-            "y": 6.7089283098414425,
-            "heading": -0.5974594180971905,
-            "angularVelocity": 0.0000019069719319939646,
-            "velocityX": 3.7321984667555648,
-            "velocityY": 0.6175159438731035,
-            "timestamp": 1.2570725917471541
+            "x": 2.2837844841867163,
+            "y": 4.387687502531162,
+            "heading": -0.3845108824411683,
+            "angularVelocity": 0.0884354987329395,
+            "velocityX": -0.26124913754506907,
+            "velocityY": 0.4460749963360829,
+            "timestamp": 0.06206916495779045
+        },
+        {
+            "x": 2.252431932443338,
+            "y": 4.44366896754607,
+            "heading": -0.37369458929412197,
+            "angularVelocity": 0.17426194076711193,
+            "velocityX": -0.5051228216917889,
+            "velocityY": 0.9019207049574874,
+            "timestamp": 0.1241383299155809
+        },
+        {
+            "x": 2.2074803876744995,
+            "y": 4.528718281766582,
+            "heading": -0.3577885652255258,
+            "angularVelocity": 0.2562628976859123,
+            "velocityX": -0.724217005329779,
+            "velocityY": 1.3702345484796434,
+            "timestamp": 0.18620749487337135
+        },
+        {
+            "x": 2.1512700092503536,
+            "y": 4.643824218738039,
+            "heading": -0.3371682293163389,
+            "angularVelocity": 0.3322154555127859,
+            "velocityX": -0.905608742482958,
+            "velocityY": 1.8544785812676818,
+            "timestamp": 0.2482766598311618
+        },
+        {
+            "x": 2.087674884006875,
+            "y": 4.7901724592691535,
+            "heading": -0.3124919375355934,
+            "angularVelocity": 0.3975612012535503,
+            "velocityX": -1.0245848367214008,
+            "velocityY": 2.357825187926933,
+            "timestamp": 0.31034582478895223
+        },
+        {
+            "x": 2.0237484850052736,
+            "y": 4.968633105417482,
+            "heading": -0.2850705208032402,
+            "angularVelocity": 0.44178807225385713,
+            "velocityX": -1.029922007895696,
+            "velocityY": 2.8751900604698215,
+            "timestamp": 0.3724149897467427
+        },
+        {
+            "x": 1.9724195760606846,
+            "y": 5.1766438961084935,
+            "heading": -0.25765844821795814,
+            "angularVelocity": 0.4416375281458893,
+            "velocityX": -0.8269630980122488,
+            "velocityY": 3.351274192787545,
+            "timestamp": 0.43448415470453317
+        },
+        {
+            "x": 1.9484287592243066,
+            "y": 5.401486281993408,
+            "heading": -0.23420864800659155,
+            "angularVelocity": 0.3778011227958159,
+            "velocityX": -0.38651747373332257,
+            "velocityY": 3.6224490217904175,
+            "timestamp": 0.49655331966232363
+        },
+        {
+            "x": 1.9564542032821701,
+            "y": 5.628890453578366,
+            "heading": -0.21272720707613346,
+            "angularVelocity": 0.34608876961188295,
+            "velocityX": 0.12929840547219124,
+            "velocityY": 3.663722103228105,
+            "timestamp": 0.5586224846201141
+        },
+        {
+            "x": 1.99619273658118,
+            "y": 5.8512380599288125,
+            "heading": -0.18843692284079885,
+            "angularVelocity": 0.39134221077502945,
+            "velocityX": 0.6402298681838031,
+            "velocityY": 3.5822554806643043,
+            "timestamp": 0.6206916495779046
+        },
+        {
+            "x": 2.0663437221615903,
+            "y": 6.063242956223716,
+            "heading": -0.16364621155748288,
+            "angularVelocity": 0.3994046206362532,
+            "velocityX": 1.130206691653697,
+            "velocityY": 3.4156234651942974,
+            "timestamp": 0.682760814535695
+        },
+        {
+            "x": 2.1656765937805176,
+            "y": 6.261821746826172,
+            "heading": -0.13977158803318993,
+            "angularVelocity": 0.38464547638239754,
+            "velocityX": 1.6003577893593972,
+            "velocityY": 3.1993146796407252,
+            "timestamp": 0.7448299794934855
+        },
+        {
+            "x": 2.225814155156259,
+            "y": 6.362141570414172,
+            "heading": -0.12770962458788607,
+            "angularVelocity": 0.3689752694416071,
+            "velocityX": 1.8396070434406133,
+            "velocityY": 3.068781803712599,
+            "timestamp": 0.7775204182423698
+        },
+        {
+            "x": 2.2936460979223643,
+            "y": 6.4579721340232545,
+            "heading": -0.11638786741998439,
+            "angularVelocity": 0.3463323712222401,
+            "velocityX": 2.074978047465827,
+            "velocityY": 2.9314554125487073,
+            "timestamp": 0.8102108569912542
+        },
+        {
+            "x": 2.3690076803030626,
+            "y": 6.549046337769426,
+            "heading": -0.10608509188235166,
+            "angularVelocity": 0.31516173940473013,
+            "velocityX": 2.3053096031972573,
+            "velocityY": 2.7859584401928488,
+            "timestamp": 0.8429012957401385
+        },
+        {
+            "x": 2.4516786592945876,
+            "y": 6.635038598364521,
+            "heading": -0.09714822011858378,
+            "angularVelocity": 0.27337876474752265,
+            "velocityX": 2.528903928955548,
+            "velocityY": 2.6305018802439832,
+            "timestamp": 0.8755917344890228
+        },
+        {
+            "x": 2.5413546021547084,
+            "y": 6.715546857152131,
+            "heading": -0.09001694608551258,
+            "angularVelocity": 0.21814555896345397,
+            "velocityX": 2.7431856619836545,
+            "velocityY": 2.462746352407108,
+            "timestamp": 0.9082821732379072
+        },
+        {
+            "x": 2.6375981860796323,
+            "y": 6.79006985383775,
+            "heading": -0.08525828181365991,
+            "angularVelocity": 0.14556746421386957,
+            "velocityX": 2.9440896974181805,
+            "velocityY": 2.2796572801553694,
+            "timestamp": 0.9409726119867915
+        },
+        {
+            "x": 2.73984178672467,
+            "y": 6.858044565031264,
+            "heading": -0.08306946201564708,
+            "angularVelocity": 0.06695596274838787,
+            "velocityX": 3.1276301131997113,
+            "velocityY": 2.079345331378129,
+            "timestamp": 0.9736630507356758
+        },
+        {
+            "x": 2.847369129600613,
+            "y": 6.918863246924361,
+            "heading": -0.08274471887343776,
+            "angularVelocity": 0.009933887583435854,
+            "velocityX": 3.2892597037781757,
+            "velocityY": 1.8604425091962247,
+            "timestamp": 1.0063534894845603
+        },
+        {
+            "x": 2.9591159211859868,
+            "y": 6.971834136920771,
+            "heading": -0.08274467756065068,
+            "angularVelocity": 0.0000012637574442965605,
+            "velocityX": 3.418332572524967,
+            "velocityY": 1.6203786802362972,
+            "timestamp": 1.0390439282334447
+        },
+        {
+            "x": 3.0743869446602656,
+            "y": 7.0166209198254075,
+            "heading": -0.0827446619031275,
+            "angularVelocity": 4.789633846658565e-7,
+            "velocityX": 3.526138769803835,
+            "velocityY": 1.3700269748141811,
+            "timestamp": 1.0717343669823292
+        },
+        {
+            "x": 3.192583723464696,
+            "y": 7.052991069898469,
+            "heading": -0.08274464937093684,
+            "angularVelocity": 3.833595125790563e-7,
+            "velocityX": 3.61563757869459,
+            "velocityY": 1.1125623107197633,
+            "timestamp": 1.1044248057312136
+        },
+        {
+            "x": 3.313092599420772,
+            "y": 7.080755764680444,
+            "heading": -0.08274463888121661,
+            "angularVelocity": 3.208803751652956e-7,
+            "velocityX": 3.6863645936910636,
+            "velocityY": 0.8493215706001247,
+            "timestamp": 1.137115244480098
+        },
+        {
+            "x": 3.4352879115666495,
+            "y": 7.0997708632875725,
+            "heading": -0.08274462977736693,
+            "angularVelocity": 2.7848661636087506e-7,
+            "velocityX": 3.7379526498424447,
+            "velocityY": 0.5816715631449295,
+            "timestamp": 1.1698056832289825
+        },
+        {
+            "x": 3.5585352449602814,
+            "y": 7.109937657437644,
+            "heading": -0.08274462163132143,
+            "angularVelocity": 2.491874023135916e-7,
+            "velocityX": 3.770133963033834,
+            "velocityY": 0.31100207091115845,
+            "timestamp": 1.202496121977867
+        },
+        {
+            "x": 3.6821947259931562,
+            "y": 7.1112033907952945,
+            "heading": -0.08274461414532981,
+            "angularVelocity": 2.2899636479565772e-7,
+            "velocityX": 3.782741552745206,
+            "velocityY": 0.03871876321990418,
+            "timestamp": 1.2351865607267514
+        },
+        {
+            "x": 3.8056243495254822,
+            "y": 7.103561554173401,
+            "heading": -0.08274460709859606,
+            "angularVelocity": 2.1555947310703947e-7,
+            "velocityX": 3.775710215468651,
+            "velocityY": -0.23376366040158306,
+            "timestamp": 1.2678769994756358
+        },
+        {
+            "x": 3.9281833350664277,
+            "y": 7.087052015079957,
+            "heading": -0.08274460031581693,
+            "angularVelocity": 2.0748510569540933e-7,
+            "velocityX": 3.7490774131963343,
+            "velocityY": -0.5050265375893709,
+            "timestamp": 1.3005674382245203
+        },
+        {
+            "x": 4.0492357154991785,
+            "y": 7.0617618768818975,
+            "heading": -0.08274459364684544,
+            "angularVelocity": 2.0400373145008536e-7,
+            "velocityX": 3.7029903869628296,
+            "velocityY": -0.7736249241717706,
+            "timestamp": 1.3332578769734047
+        },
+        {
+            "x": 4.169706806774688,
+            "y": 7.033833646086136,
+            "heading": -0.0827445869919534,
+            "angularVelocity": 2.0357304165195936e-7,
+            "velocityX": 3.6852087609158533,
+            "velocityY": -0.8543241346506245,
+            "timestamp": 1.3659483157222891
+        },
+        {
+            "x": 4.290177832890272,
+            "y": 7.00590513421804,
+            "heading": -0.08274458033705716,
+            "angularVelocity": 2.0357317018224517e-7,
+            "velocityX": 3.6852067676748597,
+            "velocityY": -0.8543327326486478,
+            "timestamp": 1.3986387544711736
+        },
+        {
+            "x": 4.410648856777724,
+            "y": 6.977976612738755,
+            "heading": -0.08274457368215653,
+            "angularVelocity": 2.0357330400976238e-7,
+            "velocityX": 3.685206699516314,
+            "velocityY": -0.8543330266547546,
+            "timestamp": 1.431329193220058
+        },
+        {
+            "x": 4.531119878449975,
+            "y": 6.950048081704109,
+            "heading": -0.08274456702725173,
+            "angularVelocity": 2.0357343141081232e-7,
+            "velocityX": 3.6852066317533563,
+            "velocityY": -0.8543333189530982,
+            "timestamp": 1.4640196319689425
+        },
+        {
+            "x": 4.651590897922017,
+            "y": 6.922119541178764,
+            "heading": -0.082744560372343,
+            "angularVelocity": 2.0357355233562972e-7,
+            "velocityX": 3.6852065644490066,
+            "velocityY": -0.8543336092734171,
+            "timestamp": 1.496710070717827
+        },
+        {
+            "x": 4.772061915216332,
+            "y": 6.894190991259702,
+            "heading": -0.08274455371743049,
+            "angularVelocity": 2.0357366776279015e-7,
+            "velocityX": 3.68520649783238,
+            "velocityY": -0.8543338966271132,
+            "timestamp": 1.5294005094667114
+        },
+        {
+            "x": 4.892532930362486,
+            "y": 6.866262432074455,
+            "heading": -0.08274454706251447,
+            "angularVelocity": 2.0357377502591166e-7,
+            "velocityX": 3.6852064321201583,
+            "velocityY": -0.8543341800795031,
+            "timestamp": 1.5620909482155958
+        },
+        {
+            "x": 5.0130039433965745,
+            "y": 6.838333863778735,
+            "heading": -0.08274454040759514,
+            "angularVelocity": 2.0357387593869802e-7,
+            "velocityX": 3.685206367512119,
+            "velocityY": -0.854334458768776,
+            "timestamp": 1.5947813869644802
+        },
+        {
+            "x": 5.133474954360567,
+            "y": 6.810405286553609,
+            "heading": -0.08274453375267282,
+            "angularVelocity": 2.0357396812707888e-7,
+            "velocityX": 3.6852063041879153,
+            "velocityY": -0.8543347319191732,
+            "timestamp": 1.6274718257133647
+        },
+        {
+            "x": 5.253945963272805,
+            "y": 6.782476700477742,
+            "heading": -0.08274452709774775,
+            "angularVelocity": 2.035740515232947e-7,
+            "velocityX": 3.6852062414248348,
+            "velocityY": -0.8543350026628553,
+            "timestamp": 1.6601622644622491
+        },
+        {
+            "x": 5.374416652936201,
+            "y": 6.754546737348104,
+            "heading": -0.08274452044281651,
+            "angularVelocity": 2.0357424100563973e-7,
+            "velocityX": 3.685196475607398,
+            "velocityY": -0.8543771267177642,
+            "timestamp": 1.6928527032111336
         },
         {
             "x": 5.493278503417969,
             "y": 6.720412731170654,
-            "heading": -0.5974593280342978,
-            "angularVelocity": 0.0000022926381384477543,
-            "velocityX": 3.7716260642834416,
-            "velocityY": 0.2923470648746024,
-            "timestamp": 1.2963561102392527
-        },
-        {
-            "x": 5.617528419554277,
-            "y": 6.721027319451783,
-            "heading": -0.5974592417476945,
-            "angularVelocity": 0.0000026270679336343826,
-            "velocityX": 3.7828927995748978,
-            "velocityY": 0.018711655155499447,
-            "timestamp": 1.3292013222818118
-        },
-        {
-            "x": 5.7414972044827435,
-            "y": 6.712651079745108,
-            "heading": -0.5974591685782128,
-            "angularVelocity": 0.0000022277061710620633,
-            "velocityX": 3.7743335244066216,
-            "velocityY": -0.25502163589934396,
-            "timestamp": 1.3620465343243708
-        },
-        {
-            "x": 5.864535126960113,
-            "y": 6.695327921921503,
-            "heading": -0.597459104000633,
-            "angularVelocity": 0.000001966118527479946,
-            "velocityX": 3.745992637160019,
-            "velocityY": -0.5274180541456184,
-            "timestamp": 1.39489174636693
-        },
-        {
-            "x": 5.985997338800097,
-            "y": 6.669148654285341,
-            "heading": -0.5974590451258569,
-            "angularVelocity": 0.0000017924918823644274,
-            "velocityX": 3.6980188065957362,
-            "velocityY": -0.7970497374814797,
-            "timestamp": 1.427736958409489
-        },
-        {
-            "x": 6.105247258928482,
-            "y": 6.634250515985412,
-            "heading": -0.5974589899613981,
-            "angularVelocity": 0.0000016795281661896423,
-            "velocityX": 3.6306637318675383,
-            "velocityY": -1.062503059946642,
-            "timestamp": 1.460582170452048
-        },
-        {
-            "x": 6.221659925637703,
-            "y": 6.5908164867561565,
-            "heading": -0.5974589370314296,
-            "angularVelocity": 0.0000016114972403778573,
-            "velocityX": 3.544281174328253,
-            "velocityY": -1.3223854110871922,
-            "timestamp": 1.4934273824946072
-        },
-        {
-            "x": 6.334625365293315,
-            "y": 6.539074496952364,
-            "heading": -0.5974588851619594,
-            "angularVelocity": 0.000001579209477235118,
-            "velocityX": 3.439327458421827,
-            "velocityY": -1.5753282316046957,
-            "timestamp": 1.5262725945371662
-        },
-        {
-            "x": 6.443562887659525,
-            "y": 6.4793163144818955,
-            "heading": -0.5974588333468978,
-            "angularVelocity": 0.000001577552964572491,
-            "velocityX": 3.3166941417319027,
-            "velocityY": -1.819387933752184,
-            "timestamp": 1.5591178065797253
-        },
-        {
-            "x": 6.552407332287041,
-            "y": 6.419388766569548,
-            "heading": -0.5974587815290724,
-            "angularVelocity": 0.0000015776371105880298,
-            "velocityX": 3.313860312014723,
-            "velocityY": -1.82454440649205,
-            "timestamp": 1.5919630186222844
-        },
-        {
-            "x": 6.661251765496281,
-            "y": 6.359461197918566,
-            "heading": -0.5974587297112393,
-            "angularVelocity": 0.0000015776373457032493,
-            "velocityX": 3.3138599643757076,
-            "velocityY": -1.8245450378971961,
-            "timestamp": 1.6248082306648435
-        },
-        {
-            "x": 6.7700961954772865,
-            "y": 6.299533623404254,
-            "heading": -0.5974586778933993,
-            "angularVelocity": 0.0000015776375522202912,
-            "velocityX": 3.3138598660894165,
-            "velocityY": -1.8245452164111537,
-            "timestamp": 1.6576534427074026
-        },
-        {
-            "x": 6.878940549868732,
-            "y": 6.239605911600315,
-            "heading": -0.5974586260742238,
-            "angularVelocity": 0.0000015776782163840074,
-            "velocityX": 3.313857564701912,
-            "velocityY": -1.8245493963088628,
-            "timestamp": 1.6904986547499616
-        },
-        {
-            "x": 6.983465419726436,
-            "y": 6.181138244955353,
-            "heading": -0.5831453971579332,
-            "angularVelocity": 0.4357782466962171,
-            "velocityX": 3.182347238991671,
-            "velocityY": -1.7800970981449478,
-            "timestamp": 1.7233438667925207
-        },
-        {
-            "x": 7.08190934617205,
-            "y": 6.126284607678917,
-            "heading": -0.5460039664027865,
-            "angularVelocity": 1.1308019782912204,
-            "velocityX": 2.997207821891462,
-            "velocityY": -1.67006494600528,
-            "timestamp": 1.7561890788350798
-        },
-        {
-            "x": 7.174234078294548,
-            "y": 6.075101338854014,
-            "heading": -0.4864629486170815,
-            "angularVelocity": 1.8127761729340726,
-            "velocityX": 2.8109038237566843,
-            "velocityY": -1.5583175032799648,
-            "timestamp": 1.7890342908776389
-        },
-        {
-            "x": 7.258956033298187,
-            "y": 6.028257642494675,
-            "heading": -0.41936488473987615,
-            "angularVelocity": 2.0428567728619895,
-            "velocityX": 2.5794309043853687,
-            "velocityY": -1.4261955836547433,
-            "timestamp": 1.821879502920198
-        },
-        {
-            "x": 7.335844716602702,
-            "y": 5.985834491701992,
-            "heading": -0.35186229403264746,
-            "angularVelocity": 2.0551729311350044,
-            "velocityX": 2.3409403843979617,
-            "velocityY": -1.2916083701234404,
-            "timestamp": 1.854724714962757
-        },
-        {
-            "x": 7.404915151007572,
-            "y": 5.947794038432469,
-            "heading": -0.2870870788105502,
-            "angularVelocity": 1.9721356993583767,
-            "velocityX": 2.1029072461275145,
-            "velocityY": -1.1581734719873862,
-            "timestamp": 1.8875699270053161
-        },
-        {
-            "x": 7.466209510665893,
-            "y": 5.914092129035952,
-            "heading": -0.22688750133470567,
-            "angularVelocity": 1.8328265744751362,
-            "velocityX": 1.866158135283738,
-            "velocityY": -1.0260828687243908,
-            "timestamp": 1.9204151390478752
-        },
-        {
-            "x": 7.519766848297347,
-            "y": 5.8846886900937605,
-            "heading": -0.17246123732016092,
-            "angularVelocity": 1.657053208974533,
-            "velocityX": 1.6305980172109753,
-            "velocityY": -0.8952123342686217,
-            "timestamp": 1.9532603510904343
-        },
-        {
-            "x": 7.565618503926309,
-            "y": 5.859549729252428,
-            "heading": -0.12462132977080041,
-            "angularVelocity": 1.4565260680124767,
-            "velocityX": 1.395992072438578,
-            "velocityY": -0.7653767254946843,
-            "timestamp": 1.9861055631329934
-        },
-        {
-            "x": 7.603788954978725,
-            "y": 5.838647308998552,
-            "heading": -0.08394075693399232,
-            "angularVelocity": 1.2385541242393951,
-            "velocityX": 1.1621313634075436,
-            "velocityY": -0.6363916977236806,
-            "timestamp": 2.0189507751755524
-        },
-        {
-            "x": 7.634297586862898,
-            "y": 5.821958675739452,
-            "heading": -0.050840342274680116,
-            "angularVelocity": 1.0077698574891136,
-            "velocityX": 0.928860859374263,
-            "velocityY": -0.5080994221459559,
-            "timestamp": 2.0517959872181115
-        },
-        {
-            "x": 7.657160251333572,
-            "y": 5.809465047264352,
-            "heading": -0.025644988874275507,
-            "angularVelocity": 0.7670936442051631,
-            "velocityX": 0.6960729752956952,
-            "velocityY": -0.3803789867120345,
-            "timestamp": 2.0846411992606706
-        },
-        {
-            "x": 7.672390418071531,
-            "y": 5.801150340997017,
-            "heading": -0.00862072816733467,
-            "angularVelocity": 0.5183178810016766,
-            "velocityX": 0.46369518693407513,
-            "velocityY": -0.2531481987862983,
-            "timestamp": 2.1174864113032297
+            "heading": -0.08274451374626977,
+            "angularVelocity": 2.0484725764732438e-7,
+            "velocityX": 3.6359821106976624,
+            "velocityY": -1.0441587046150123,
+            "timestamp": 1.725543141960018
+        },
+        {
+            "x": 5.6078450645507765,
+            "y": 6.6784969077855525,
+            "heading": -0.0827445073182203,
+            "angularVelocity": 1.9932954157588389e-7,
+            "velocityX": 3.5526329088991675,
+            "velocityY": -1.2997818219306485,
+            "timestamp": 1.7577914952435287
+        },
+        {
+            "x": 5.720102544926552,
+            "y": 6.630740323854381,
+            "heading": -0.08274450093221485,
+            "angularVelocity": 1.980257845223315e-7,
+            "velocityX": 3.481029849458213,
+            "velocityY": -1.4808999241394636,
+            "timestamp": 1.7900398485270395
+        },
+        {
+            "x": 5.832359462510262,
+            "y": 6.582982417035901,
+            "heading": -0.08274449454621227,
+            "angularVelocity": 1.9802569455219195e-7,
+            "velocityX": 3.481012397650423,
+            "velocityY": -1.480940945995501,
+            "timestamp": 1.8222882018105502
+        },
+        {
+            "x": 5.944616376948871,
+            "y": 6.535224502824747,
+            "heading": -0.08274448816020867,
+            "angularVelocity": 1.9802572730548282e-7,
+            "velocityX": 3.4810123001228868,
+            "velocityY": -1.4809411752374333,
+            "timestamp": 1.854536555094061
+        },
+        {
+            "x": 6.0568732883758765,
+            "y": 6.487466581534697,
+            "heading": -0.0827444817742042,
+            "angularVelocity": 1.9802575385283685e-7,
+            "velocityX": 3.4810122067350586,
+            "velocityY": -1.4809413947492354,
+            "timestamp": 1.8867849083775716
+        },
+        {
+            "x": 6.169130196876316,
+            "y": 6.439708653365641,
+            "heading": -0.08274447538819907,
+            "angularVelocity": 1.980257744166014e-7,
+            "velocityX": 3.4810121159842042,
+            "velocityY": -1.4809416080626625,
+            "timestamp": 1.9190332616610823
+        },
+        {
+            "x": 6.281387102537832,
+            "y": 6.391950718523587,
+            "heading": -0.08274446900219351,
+            "angularVelocity": 1.9802578764982062e-7,
+            "velocityX": 3.481012027951073,
+            "velocityY": -1.4809418149879348,
+            "timestamp": 1.951281614944593
+        },
+        {
+            "x": 6.393644005449787,
+            "y": 6.344192777218584,
+            "heading": -0.08274446261618765,
+            "angularVelocity": 1.9802579747327125e-7,
+            "velocityX": 3.481011942689016,
+            "velocityY": -1.4809420153996509,
+            "timestamp": 1.9835299682281038
+        },
+        {
+            "x": 6.505900905702341,
+            "y": 6.296434829662559,
+            "heading": -0.08274445623018166,
+            "angularVelocity": 1.9802580083930334e-7,
+            "velocityX": 3.4810118602227385,
+            "velocityY": -1.4809422092397408,
+            "timestamp": 2.0157783215116143
+        },
+        {
+            "x": 6.618157803385852,
+            "y": 6.2486768760679015,
+            "heading": -0.08274444984417571,
+            "angularVelocity": 1.9802579953185327e-7,
+            "velocityX": 3.4810117805584304,
+            "velocityY": -1.4809423964936643,
+            "timestamp": 2.048026674795125
+        },
+        {
+            "x": 6.730414698590335,
+            "y": 6.200918916646197,
+            "heading": -0.08274444345816992,
+            "angularVelocity": 1.9802579502845229e-7,
+            "velocityX": 3.4810117036854167,
+            "velocityY": -1.4809425771865148,
+            "timestamp": 2.0802750280786353
+        },
+        {
+            "x": 6.842671592155369,
+            "y": 6.153160953370972,
+            "heading": -0.08274443707212797,
+            "angularVelocity": 1.980269160510039e-7,
+            "velocityX": 3.4810116528472412,
+            "velocityY": -1.4809426966816255,
+            "timestamp": 2.112523381362146
+        },
+        {
+            "x": 6.954235049253699,
+            "y": 6.105720053055757,
+            "heading": -0.08066018407058251,
+            "angularVelocity": 0.06463130018523208,
+            "velocityX": 3.459508648938503,
+            "velocityY": -1.4711107850419303,
+            "timestamp": 2.1447717346456563
+        },
+        {
+            "x": 7.05792262192444,
+            "y": 6.061617369901677,
+            "heading": -0.07347223812764758,
+            "angularVelocity": 0.2228934258982459,
+            "velocityX": 3.215282707900597,
+            "velocityY": -1.3675948897716332,
+            "timestamp": 2.177020087929167
+        },
+        {
+            "x": 7.153639251658588,
+            "y": 6.020903567478063,
+            "heading": -0.06480991744029473,
+            "angularVelocity": 0.2686128067130366,
+            "velocityX": 2.9681090656833784,
+            "velocityY": -1.2625079508922343,
+            "timestamp": 2.2092684412126773
+        },
+        {
+            "x": 7.241376863236811,
+            "y": 5.983582900712933,
+            "heading": -0.05573995757660787,
+            "angularVelocity": 0.28125342661540725,
+            "velocityX": 2.7206850162822076,
+            "velocityY": -1.1572890695232254,
+            "timestamp": 2.241516794496188
+        },
+        {
+            "x": 7.321134939051253,
+            "y": 5.949655957408732,
+            "heading": -0.04677486060395727,
+            "angularVelocity": 0.2780016980662186,
+            "velocityX": 2.4732449162054873,
+            "velocityY": -1.0520519607910965,
+            "timestamp": 2.2737651477796983
+        },
+        {
+            "x": 7.392914197093691,
+            "y": 5.919122647933878,
+            "heading": -0.03821600737741098,
+            "angularVelocity": 0.26540434952773495,
+            "velocityX": 2.2258270805765674,
+            "velocityY": -0.946817631474723,
+            "timestamp": 2.306013501063209
+        },
+        {
+            "x": 7.456715523277294,
+            "y": 5.891982736437705,
+            "heading": -0.030261865024148932,
+            "angularVelocity": 0.24665266729552143,
+            "velocityX": 1.9784367165261438,
+            "velocityY": -0.8415906157307577,
+            "timestamp": 2.3382618543467193
+        },
+        {
+            "x": 7.512539743308544,
+            "y": 5.868235970303718,
+            "heading": -0.023053012750234723,
+            "angularVelocity": 0.2235417173255794,
+            "velocityX": 1.7310719570848203,
+            "velocityY": -0.736371433456386,
+            "timestamp": 2.37051020763023
+        },
+        {
+            "x": 7.560387579465668,
+            "y": 5.847882114762634,
+            "heading": -0.016694240391467878,
+            "angularVelocity": 0.19718130420078028,
+            "velocityX": 1.4837295950113008,
+            "velocityY": -0.6311595312214304,
+            "timestamp": 2.4027585609137403
+        },
+        {
+            "x": 7.600259653459209,
+            "y": 5.830920960662069,
+            "heading": -0.011266668966831511,
+            "angularVelocity": 0.1683053821980557,
+            "velocityX": 1.2364065117684333,
+            "velocityY": -0.5259541146628961,
+            "timestamp": 2.435006914197251
+        },
+        {
+            "x": 7.632156499892169,
+            "y": 5.817352324088122,
+            "heading": -0.006834955017927562,
+            "angularVelocity": 0.1374245038171387,
+            "velocityX": 0.9891000062092736,
+            "velocityY": -0.4207544011521409,
+            "timestamp": 2.4672552674807613
+        },
+        {
+            "x": 7.656078580521334,
+            "y": 5.807176043662066,
+            "heading": -0.0034518424131983775,
+            "angularVelocity": 0.10490807313441347,
+            "velocityX": 0.7418078194211917,
+            "velocityY": -0.31555969188850075,
+            "timestamp": 2.499503620764272
+        },
+        {
+            "x": 7.672026296758129,
+            "y": 5.800391977434502,
+            "heading": -0.0011611797809329624,
+            "angularVelocity": 0.07103192563441285,
+            "velocityX": 0.49452808013452626,
+            "velocityY": -0.21036938438134845,
+            "timestamp": 2.5317519740477823
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 8.329701949714689e-33,
-            "angularVelocity": 0.26246529193262896,
-            "velocityX": 0.2316800975013136,
-            "velocityY": -0.12636060901771562,
-            "timestamp": 2.1503316233457888
+            "heading": 1.9934013230001416e-31,
+            "angularVelocity": 0.036007413176266045,
+            "velocityX": 0.2472592374491084,
+            "velocityY": -0.10518296561320875,
+            "timestamp": 2.564000327331293
         },
         {
             "x": 7.68,
             "y": 5.797,
-            "heading": 5.789840595031671e-40,
-            "angularVelocity": 1.2947513524749855e-30,
-            "velocityX": 9.16355000594536e-32,
-            "velocityY": -1.9486684829089695e-31,
-            "timestamp": 2.183176835388348
+            "heading": 1.0109342582388432e-32,
+            "angularVelocity": -5.855934233748412e-30,
+            "velocityX": 8.513095472636782e-32,
+            "velocityY": -2.1397535642954256e-34,
+            "timestamp": 2.5962486806148033
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.1.traj
new file mode 100644
index 00000000..9c22ef7a
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.1.traj	
@@ -0,0 +1,400 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 1.5561411635921053e-32,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2879322251049214,
+            "y": 4.397976336389526,
+            "heading": -0.38546801638598305,
+            "angularVelocity": 0.06554072441421986,
+            "velocityX": -0.17452197008732642,
+            "velocityY": 0.5492068837058847,
+            "timestamp": 0.06914759722835051
+        },
+        {
+            "x": 2.264556974735031,
+            "y": 4.474163552813777,
+            "heading": -0.3765424472118302,
+            "angularVelocity": 0.12907996129426796,
+            "velocityX": -0.3380486279621587,
+            "velocityY": 1.1018056950361832,
+            "timestamp": 0.13829519445670102
+        },
+        {
+            "x": 2.231004605798313,
+            "y": 4.588879932488501,
+            "heading": -0.3634307413762601,
+            "angularVelocity": 0.18961910985573333,
+            "velocityX": -0.48522826940697783,
+            "velocityY": 1.6590074604604372,
+            "timestamp": 0.20744279168505153
+        },
+        {
+            "x": 2.1891253320707884,
+            "y": 4.7425700174397605,
+            "heading": -0.3464768905998804,
+            "angularVelocity": 0.24518351259275634,
+            "velocityX": -0.6056504550588213,
+            "velocityY": 2.2226381119759067,
+            "timestamp": 0.27659038891340204
+        },
+        {
+            "x": 2.142455487111455,
+            "y": 4.9358291258163405,
+            "heading": -0.3263533789303141,
+            "angularVelocity": 0.29102257309767376,
+            "velocityX": -0.6749308266671239,
+            "velocityY": 2.7948781465017833,
+            "timestamp": 0.34573798614175255
+        },
+        {
+            "x": 2.099906819940071,
+            "y": 5.168740583772433,
+            "heading": -0.30483662973794956,
+            "angularVelocity": 0.3111713212639373,
+            "velocityX": -0.6153311015404023,
+            "velocityY": 3.3683232287448175,
+            "timestamp": 0.41488558337010306
+        },
+        {
+            "x": 2.090216091849313,
+            "y": 5.4241817814008675,
+            "heading": -0.28862249413017305,
+            "angularVelocity": 0.23448588609832868,
+            "velocityX": -0.14014555066512774,
+            "velocityY": 3.69414423446863,
+            "timestamp": 0.48403318059845357
+        },
+        {
+            "x": 2.1169158362249996,
+            "y": 5.665111810235099,
+            "heading": -0.24564536248803095,
+            "angularVelocity": 0.6215274769324497,
+            "velocityX": 0.3861268568382121,
+            "velocityY": 3.484286345315426,
+            "timestamp": 0.5531807778268041
+        },
+        {
+            "x": 2.1721169779622786,
+            "y": 5.878339660700231,
+            "heading": -0.1915552026069021,
+            "angularVelocity": 0.7822420741739773,
+            "velocityX": 0.7983088921367857,
+            "velocityY": 3.08366247001803,
+            "timestamp": 0.6223283750551546
+        },
+        {
+            "x": 2.2518565055578685,
+            "y": 6.060184802876031,
+            "heading": -0.1318933186992544,
+            "angularVelocity": 0.8628193357100991,
+            "velocityX": 1.1531785744091572,
+            "velocityY": 2.629811439074749,
+            "timestamp": 0.6914759722835051
+        },
+        {
+            "x": 2.3540132373645406,
+            "y": 6.209070989598032,
+            "heading": -0.06941508964740774,
+            "angularVelocity": 0.9035488079854361,
+            "velocityX": 1.4773721127192665,
+            "velocityY": 2.153165007746782,
+            "timestamp": 0.7606235695118556
+        },
+        {
+            "x": 2.477290153503418,
+            "y": 6.3241448402404785,
+            "heading": -0.005748673882586236,
+            "angularVelocity": 0.9207321485592678,
+            "velocityX": 1.782808384965753,
+            "velocityY": 1.6641771407079455,
+            "timestamp": 0.8297711667402061
+        },
+        {
+            "x": 2.6074231512941006,
+            "y": 6.400809151237731,
+            "heading": 0.05275689385662717,
+            "angularVelocity": 0.9223034785287629,
+            "velocityX": 2.051464863433381,
+            "velocityY": 1.20856464509814,
+            "timestamp": 0.8932053498501396
+        },
+        {
+            "x": 2.7533092580755354,
+            "y": 6.447850328813001,
+            "heading": 0.11062964933050726,
+            "angularVelocity": 0.9123275911695883,
+            "velocityX": 2.2998027188043615,
+            "velocityY": 0.7415745780760178,
+            "timestamp": 0.956639532960073
+        },
+        {
+            "x": 2.9132889651123484,
+            "y": 6.4644464131499,
+            "heading": 0.16695291665745207,
+            "angularVelocity": 0.8879008850767368,
+            "velocityX": 2.5219794627047807,
+            "velocityY": 0.26162683151755056,
+            "timestamp": 1.0200737160700064
+        },
+        {
+            "x": 3.0851729747755345,
+            "y": 6.449682823375658,
+            "heading": 0.22055798508605634,
+            "angularVelocity": 0.8450501890441832,
+            "velocityX": 2.7096433064356162,
+            "velocityY": -0.23273870727739393,
+            "timestamp": 1.0835078991799398
+        },
+        {
+            "x": 3.266000929743579,
+            "y": 6.40260327654133,
+            "heading": 0.269929363572885,
+            "angularVelocity": 0.7783087298743283,
+            "velocityX": 2.8506389789001907,
+            "velocityY": -0.7421794453112333,
+            "timestamp": 1.1469420822898733
+        },
+        {
+            "x": 3.4516899170753974,
+            "y": 6.322371527535034,
+            "heading": 0.31309009761375267,
+            "angularVelocity": 0.6804018263443473,
+            "velocityX": 2.9272700967841274,
+            "velocityY": -1.2648030615800407,
+            "timestamp": 1.2103762653998067
+        },
+        {
+            "x": 3.636598494450249,
+            "y": 6.208685958737944,
+            "heading": 0.3475353077130109,
+            "angularVelocity": 0.5430070730006203,
+            "velocityX": 2.9149674246500377,
+            "velocityY": -1.7921814899081216,
+            "timestamp": 1.2738104485097401
+        },
+        {
+            "x": 3.8132641934440024,
+            "y": 6.062639398740876,
+            "heading": 0.3704029865204341,
+            "angularVelocity": 0.36049457383033545,
+            "velocityX": 2.785023631307647,
+            "velocityY": -2.3023321628339066,
+            "timestamp": 1.3372446316196736
+        },
+        {
+            "x": 3.9730355739593506,
+            "y": 5.887885093688965,
+            "heading": 0.37894290950178183,
+            "angularVelocity": 0.1346265146219495,
+            "velocityX": 2.5186953261297353,
+            "velocityY": -2.754891707981806,
+            "timestamp": 1.400678814729607
+        },
+        {
+            "x": 4.101195684299111,
+            "y": 5.704704428790184,
+            "heading": 0.3789451050812407,
+            "angularVelocity": 0.000037151619885842266,
+            "velocityX": 2.1686126132704535,
+            "velocityY": -3.099622022424022,
+            "timestamp": 1.459776557478272
+        },
+        {
+            "x": 4.204458426061581,
+            "y": 5.506418480888323,
+            "heading": 0.3789451145933903,
+            "angularVelocity": 1.6095622464276883e-7,
+            "velocityX": 1.7473212505255857,
+            "velocityY": -3.3552203295641,
+            "timestamp": 1.518874300226937
+        },
+        {
+            "x": 4.281071316930325,
+            "y": 5.296392335982766,
+            "heading": 0.3789451203460425,
+            "angularVelocity": 9.73413180945881e-8,
+            "velocityX": 1.2963759241186865,
+            "velocityY": -3.5538776125279528,
+            "timestamp": 1.577972042975602
+        },
+        {
+            "x": 4.344371609432443,
+            "y": 5.081977806335691,
+            "heading": 0.37894512554853116,
+            "angularVelocity": 8.803193422611526e-8,
+            "velocityX": 1.0711118489099893,
+            "velocityY": -3.628133997587826,
+            "timestamp": 1.6370697857242669
+        },
+        {
+            "x": 4.4350051048376455,
+            "y": 4.877610290730759,
+            "heading": 0.3789451268821095,
+            "angularVelocity": 2.2565639514634894e-8,
+            "velocityX": 1.5336202567072863,
+            "velocityY": -3.4581272667918515,
+            "timestamp": 1.6961675284729318
+        },
+        {
+            "x": 4.551081441257298,
+            "y": 4.6873967879425225,
+            "heading": 0.3767623699353526,
+            "angularVelocity": -0.03693469235752831,
+            "velocityX": 1.964141624038618,
+            "velocityY": -3.218625516664622,
+            "timestamp": 1.7552652712215968
+        },
+        {
+            "x": 4.687723026941447,
+            "y": 4.517482449028109,
+            "heading": 0.36165339974597627,
+            "angularVelocity": -0.25566069848849177,
+            "velocityX": 2.312128675793254,
+            "velocityY": -2.875140927749109,
+            "timestamp": 1.8143630139702618
+        },
+        {
+            "x": 4.8416817092622,
+            "y": 4.370738170800582,
+            "heading": 0.3345492154544829,
+            "angularVelocity": -0.45863315638965824,
+            "velocityX": 2.6051533469768047,
+            "velocityY": -2.483077549196378,
+            "timestamp": 1.8734607567189268
+        },
+        {
+            "x": 5.0108564738059584,
+            "y": 4.248670932935222,
+            "heading": 0.2976430739087661,
+            "angularVelocity": -0.6244932518551631,
+            "velocityX": 2.8626265010319796,
+            "velocityY": -2.065514386638646,
+            "timestamp": 1.9325584994675917
+        },
+        {
+            "x": 5.194237326297875,
+            "y": 4.152011519418658,
+            "heading": 0.2577973678603408,
+            "angularVelocity": -0.6742339757109184,
+            "velocityX": 3.103009420708511,
+            "velocityY": -1.635585540509506,
+            "timestamp": 1.9916562422162567
+        },
+        {
+            "x": 5.390789031982422,
+            "y": 4.081310272216797,
+            "heading": 0.22104423660220585,
+            "angularVelocity": -0.6219041464028032,
+            "velocityX": 3.3258750088027327,
+            "velocityY": -1.1963442919045442,
+            "timestamp": 2.050753984964922
+        },
+        {
+            "x": 5.67793189235796,
+            "y": 4.03407988691648,
+            "heading": 0.19119773345256094,
+            "angularVelocity": -0.37371337139653454,
+            "velocityX": 3.5953667967399574,
+            "velocityY": -0.5913800499300379,
+            "timestamp": 2.130618676555184
+        },
+        {
+            "x": 5.980039011502812,
+            "y": 4.037197033313024,
+            "heading": 0.19119714274274896,
+            "angularVelocity": -0.000007396382559589573,
+            "velocityX": 3.782736940809626,
+            "velocityY": 0.039030344129324,
+            "timestamp": 2.210483368145446
+        },
+        {
+            "x": 6.281741681649569,
+            "y": 4.053135447217484,
+            "heading": 0.19119713453456852,
+            "angularVelocity": -1.0277608664279704e-7,
+            "velocityX": 3.777672763010384,
+            "velocityY": 0.1995677136803443,
+            "timestamp": 2.290348059735708
+        },
+        {
+            "x": 6.583444349801777,
+            "y": 4.069073898877065,
+            "heading": 0.19119712632638652,
+            "angularVelocity": -1.0277610566434499e-7,
+            "velocityX": 3.777672738036278,
+            "velocityY": 0.1995681864189316,
+            "timestamp": 2.3702127513259703
+        },
+        {
+            "x": 6.885147017932413,
+            "y": 4.08501235094329,
+            "heading": 0.1911971181180728,
+            "angularVelocity": -1.0277775515776054e-7,
+            "velocityX": 3.7776727377661725,
+            "velocityY": 0.1995681915106077,
+            "timestamp": 2.4500774429162324
+        },
+        {
+            "x": 7.150097958068984,
+            "y": 4.099008563262217,
+            "heading": 0.12746367267042205,
+            "angularVelocity": -0.7980177995885095,
+            "velocityX": 3.317497818634029,
+            "velocityY": 0.17524906238576554,
+            "timestamp": 2.5299421345064945
+        },
+        {
+            "x": 7.362058748903403,
+            "y": 4.110205268667911,
+            "heading": 0.07647732188798662,
+            "angularVelocity": -0.6384091613855594,
+            "velocityX": 2.6539987397918514,
+            "velocityY": 0.14019593869007996,
+            "timestamp": 2.6098068260967566
+        },
+        {
+            "x": 7.521029367100823,
+            "y": 4.118602666583934,
+            "heading": 0.03823840686026882,
+            "angularVelocity": -0.47879625234031903,
+            "velocityX": 1.9904993687699384,
+            "velocityY": 0.10514531201226385,
+            "timestamp": 2.6896715176870187
+        },
+        {
+            "x": 7.627009789128585,
+            "y": 4.124200888756926,
+            "heading": 0.012746172901683662,
+            "angularVelocity": -0.31919279284744,
+            "velocityX": 1.3269997030913787,
+            "velocityY": 0.07009633495752653,
+            "timestamp": 2.769536209277281
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 1.6188506035771725e-33,
+            "angularVelocity": -0.15959709663791943,
+            "velocityX": 0.6634998497618533,
+            "velocityY": 0.03504816943929136,
+            "timestamp": 2.849400900867543
+        },
+        {
+            "x": 7.68,
+            "y": 4.127,
+            "heading": 0,
+            "angularVelocity": -1.4796214296723179e-31,
+            "velocityX": -2.3067017155365004e-33,
+            "velocityY": 0,
+            "timestamp": 2.929265592457805
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.traj
index ea265506..9c22ef7a 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.traj	
@@ -4,307 +4,397 @@
             "x": 2.3,
             "y": 4.36,
             "heading": -0.39,
-            "angularVelocity": 0,
+            "angularVelocity": 1.5561411635921053e-32,
             "velocityX": 0,
-            "velocityY": 1.9823938961383557e-29,
+            "velocityY": 0,
             "timestamp": 0
         },
         {
-            "x": 2.312764146242865,
-            "y": 4.398513079462153,
-            "heading": -0.39804543849617763,
-            "angularVelocity": -0.11524733034669515,
-            "velocityX": 0.18284072142228638,
-            "velocityY": 0.5516827447028667,
-            "timestamp": 0.06981019405093988
-        },
-        {
-            "x": 2.339895425931373,
-            "y": 4.474992910905513,
-            "heading": -0.4126392714825622,
-            "angularVelocity": -0.20905017072795415,
-            "velocityX": 0.3886435220157258,
-            "velocityY": 1.0955395910772,
-            "timestamp": 0.13962038810187977
-        },
-        {
-            "x": 2.3838571031189573,
-            "y": 4.588440455462814,
-            "heading": -0.43142102712692415,
-            "angularVelocity": -0.2690403013169746,
-            "velocityX": 0.6297314852832604,
-            "velocityY": 1.6250856497386854,
-            "timestamp": 0.20943058215281965
-        },
-        {
-            "x": 2.448821264845437,
-            "y": 4.736675156040894,
-            "heading": -0.45019318323311186,
-            "angularVelocity": -0.26890279219946406,
-            "velocityX": 0.9305827409539079,
-            "velocityY": 2.1233961972720627,
-            "timestamp": 0.27924077620375953
-        },
-        {
-            "x": 2.542623225560298,
-            "y": 4.913429983875953,
-            "heading": -0.4600490129319591,
-            "angularVelocity": -0.14118037962391586,
-            "velocityX": 1.343671393412285,
-            "velocityY": 2.5319343433823196,
-            "timestamp": 0.34905097025469944
-        },
-        {
-            "x": 2.675927389267578,
-            "y": 5.095797559489528,
-            "heading": -0.44245218860567326,
-            "angularVelocity": 0.25206668688824874,
-            "velocityX": 1.9095228930350086,
-            "velocityY": 2.6123344605070336,
-            "timestamp": 0.41886116430563936
-        },
-        {
-            "x": 2.83772713712192,
-            "y": 5.250967901017311,
-            "heading": -0.38879601501351296,
-            "angularVelocity": 0.7686008371275667,
-            "velocityX": 2.3177094699953265,
-            "velocityY": 2.2227461710524636,
-            "timestamp": 0.48867135835657927
-        },
-        {
-            "x": 3.013235940099558,
-            "y": 5.368218424760011,
-            "heading": -0.31718431053283597,
-            "angularVelocity": 1.0258058360734288,
-            "velocityX": 2.514085591121856,
-            "velocityY": 1.6795616361728827,
-            "timestamp": 0.5584815524075192
-        },
-        {
-            "x": 3.1945050869972476,
-            "y": 5.445323486359522,
-            "heading": -0.23653218942173773,
-            "angularVelocity": 1.1553057859494003,
-            "velocityX": 2.5965999573717062,
-            "velocityY": 1.1044957351381008,
-            "timestamp": 0.6282917464584591
-        },
-        {
-            "x": 3.377225953863421,
-            "y": 5.481834951968023,
-            "heading": -0.15180568644047004,
-            "angularVelocity": 1.2136694952369833,
-            "velocityX": 2.6173952006491676,
-            "velocityY": 0.523010515923758,
-            "timestamp": 0.698101940509399
-        },
-        {
-            "x": 3.558790922164917,
-            "y": 5.477728366851807,
-            "heading": -0.06601521216801902,
-            "angularVelocity": 1.2289104110775417,
-            "velocityX": 2.6008374674955865,
-            "velocityY": -0.05882500648124433,
-            "timestamp": 0.7679121345603389
-        },
-        {
-            "x": 3.7207755893743903,
-            "y": 5.440802283745667,
-            "heading": 0.01087618717506099,
-            "angularVelocity": 1.2168377614801031,
-            "velocityX": 2.5634734381227786,
-            "velocityY": -0.5843703286511746,
-            "timestamp": 0.8311016588990232
-        },
-        {
-            "x": 3.8800031263189014,
-            "y": 5.370704697701765,
-            "heading": 0.08523277253593173,
-            "angularVelocity": 1.1767232961398173,
-            "velocityX": 2.519840726939224,
-            "velocityY": -1.1093228945227975,
-            "timestamp": 0.8942911832377075
-        },
-        {
-            "x": 4.035880089753818,
-            "y": 5.267514783368049,
-            "heading": 0.15438353932229767,
-            "angularVelocity": 1.0943390935099226,
-            "velocityX": 2.4668165343191313,
-            "velocityY": -1.633022489291642,
-            "timestamp": 0.9574807075763918
-        },
-        {
-            "x": 4.187424190703861,
-            "y": 5.131425904894565,
-            "heading": 0.2138534475851711,
-            "angularVelocity": 0.9411355581283213,
-            "velocityX": 2.3982472179488545,
-            "velocityY": -2.153662017503815,
-            "timestamp": 1.0206702319150762
-        },
-        {
-            "x": 4.332733928007212,
-            "y": 4.96304651892829,
-            "heading": 0.25476003945729764,
-            "angularVelocity": 0.6473635034690916,
-            "velocityX": 2.2995858699928062,
-            "velocityY": -2.664672471059999,
-            "timestamp": 1.0838597562537606
-        },
-        {
-            "x": 4.46716744223408,
-            "y": 4.765388489141315,
-            "heading": 0.2547605945579538,
-            "angularVelocity": 0.000008784690993212257,
-            "velocityX": 2.12746520302083,
-            "velocityY": -3.1280189533989655,
-            "timestamp": 1.147049280592445
-        },
-        {
-            "x": 4.612743650855858,
-            "y": 4.575786978724626,
-            "heading": 0.2547605407972418,
-            "angularVelocity": -8.507852266556338e-7,
-            "velocityX": 2.303802887345736,
-            "velocityY": -3.0005212478052368,
-            "timestamp": 1.2102388049311295
-        },
-        {
-            "x": 4.782527461386278,
-            "y": 4.409030186652972,
-            "heading": 0.25208197397278675,
-            "angularVelocity": -0.04238941267495421,
-            "velocityX": 2.6868980627285057,
-            "velocityY": -2.638994260793031,
-            "timestamp": 1.2734283292698139
-        },
-        {
-            "x": 4.970403968588328,
-            "y": 4.270066434839271,
-            "heading": 0.23891743895373094,
-            "angularVelocity": -0.2083341369570075,
-            "velocityX": 2.9732223682415455,
-            "velocityY": -2.1991580609051504,
-            "timestamp": 1.3366178536084983
-        },
-        {
-            "x": 5.173968377386639,
-            "y": 4.1604195633547185,
-            "heading": 0.21971651393307054,
-            "angularVelocity": -0.3038624711945638,
-            "velocityX": 3.2214898106793903,
-            "velocityY": -1.7352064702582655,
-            "timestamp": 1.3998073779471827
+            "x": 2.2879322251049214,
+            "y": 4.397976336389526,
+            "heading": -0.38546801638598305,
+            "angularVelocity": 0.06554072441421986,
+            "velocityX": -0.17452197008732642,
+            "velocityY": 0.5492068837058847,
+            "timestamp": 0.06914759722835051
+        },
+        {
+            "x": 2.264556974735031,
+            "y": 4.474163552813777,
+            "heading": -0.3765424472118302,
+            "angularVelocity": 0.12907996129426796,
+            "velocityX": -0.3380486279621587,
+            "velocityY": 1.1018056950361832,
+            "timestamp": 0.13829519445670102
+        },
+        {
+            "x": 2.231004605798313,
+            "y": 4.588879932488501,
+            "heading": -0.3634307413762601,
+            "angularVelocity": 0.18961910985573333,
+            "velocityX": -0.48522826940697783,
+            "velocityY": 1.6590074604604372,
+            "timestamp": 0.20744279168505153
+        },
+        {
+            "x": 2.1891253320707884,
+            "y": 4.7425700174397605,
+            "heading": -0.3464768905998804,
+            "angularVelocity": 0.24518351259275634,
+            "velocityX": -0.6056504550588213,
+            "velocityY": 2.2226381119759067,
+            "timestamp": 0.27659038891340204
+        },
+        {
+            "x": 2.142455487111455,
+            "y": 4.9358291258163405,
+            "heading": -0.3263533789303141,
+            "angularVelocity": 0.29102257309767376,
+            "velocityX": -0.6749308266671239,
+            "velocityY": 2.7948781465017833,
+            "timestamp": 0.34573798614175255
+        },
+        {
+            "x": 2.099906819940071,
+            "y": 5.168740583772433,
+            "heading": -0.30483662973794956,
+            "angularVelocity": 0.3111713212639373,
+            "velocityX": -0.6153311015404023,
+            "velocityY": 3.3683232287448175,
+            "timestamp": 0.41488558337010306
+        },
+        {
+            "x": 2.090216091849313,
+            "y": 5.4241817814008675,
+            "heading": -0.28862249413017305,
+            "angularVelocity": 0.23448588609832868,
+            "velocityX": -0.14014555066512774,
+            "velocityY": 3.69414423446863,
+            "timestamp": 0.48403318059845357
+        },
+        {
+            "x": 2.1169158362249996,
+            "y": 5.665111810235099,
+            "heading": -0.24564536248803095,
+            "angularVelocity": 0.6215274769324497,
+            "velocityX": 0.3861268568382121,
+            "velocityY": 3.484286345315426,
+            "timestamp": 0.5531807778268041
+        },
+        {
+            "x": 2.1721169779622786,
+            "y": 5.878339660700231,
+            "heading": -0.1915552026069021,
+            "angularVelocity": 0.7822420741739773,
+            "velocityX": 0.7983088921367857,
+            "velocityY": 3.08366247001803,
+            "timestamp": 0.6223283750551546
+        },
+        {
+            "x": 2.2518565055578685,
+            "y": 6.060184802876031,
+            "heading": -0.1318933186992544,
+            "angularVelocity": 0.8628193357100991,
+            "velocityX": 1.1531785744091572,
+            "velocityY": 2.629811439074749,
+            "timestamp": 0.6914759722835051
+        },
+        {
+            "x": 2.3540132373645406,
+            "y": 6.209070989598032,
+            "heading": -0.06941508964740774,
+            "angularVelocity": 0.9035488079854361,
+            "velocityX": 1.4773721127192665,
+            "velocityY": 2.153165007746782,
+            "timestamp": 0.7606235695118556
+        },
+        {
+            "x": 2.477290153503418,
+            "y": 6.3241448402404785,
+            "heading": -0.005748673882586236,
+            "angularVelocity": 0.9207321485592678,
+            "velocityX": 1.782808384965753,
+            "velocityY": 1.6641771407079455,
+            "timestamp": 0.8297711667402061
+        },
+        {
+            "x": 2.6074231512941006,
+            "y": 6.400809151237731,
+            "heading": 0.05275689385662717,
+            "angularVelocity": 0.9223034785287629,
+            "velocityX": 2.051464863433381,
+            "velocityY": 1.20856464509814,
+            "timestamp": 0.8932053498501396
+        },
+        {
+            "x": 2.7533092580755354,
+            "y": 6.447850328813001,
+            "heading": 0.11062964933050726,
+            "angularVelocity": 0.9123275911695883,
+            "velocityX": 2.2998027188043615,
+            "velocityY": 0.7415745780760178,
+            "timestamp": 0.956639532960073
+        },
+        {
+            "x": 2.9132889651123484,
+            "y": 6.4644464131499,
+            "heading": 0.16695291665745207,
+            "angularVelocity": 0.8879008850767368,
+            "velocityX": 2.5219794627047807,
+            "velocityY": 0.26162683151755056,
+            "timestamp": 1.0200737160700064
+        },
+        {
+            "x": 3.0851729747755345,
+            "y": 6.449682823375658,
+            "heading": 0.22055798508605634,
+            "angularVelocity": 0.8450501890441832,
+            "velocityX": 2.7096433064356162,
+            "velocityY": -0.23273870727739393,
+            "timestamp": 1.0835078991799398
+        },
+        {
+            "x": 3.266000929743579,
+            "y": 6.40260327654133,
+            "heading": 0.269929363572885,
+            "angularVelocity": 0.7783087298743283,
+            "velocityX": 2.8506389789001907,
+            "velocityY": -0.7421794453112333,
+            "timestamp": 1.1469420822898733
+        },
+        {
+            "x": 3.4516899170753974,
+            "y": 6.322371527535034,
+            "heading": 0.31309009761375267,
+            "angularVelocity": 0.6804018263443473,
+            "velocityX": 2.9272700967841274,
+            "velocityY": -1.2648030615800407,
+            "timestamp": 1.2103762653998067
+        },
+        {
+            "x": 3.636598494450249,
+            "y": 6.208685958737944,
+            "heading": 0.3475353077130109,
+            "angularVelocity": 0.5430070730006203,
+            "velocityX": 2.9149674246500377,
+            "velocityY": -1.7921814899081216,
+            "timestamp": 1.2738104485097401
+        },
+        {
+            "x": 3.8132641934440024,
+            "y": 6.062639398740876,
+            "heading": 0.3704029865204341,
+            "angularVelocity": 0.36049457383033545,
+            "velocityX": 2.785023631307647,
+            "velocityY": -2.3023321628339066,
+            "timestamp": 1.3372446316196736
+        },
+        {
+            "x": 3.9730355739593506,
+            "y": 5.887885093688965,
+            "heading": 0.37894290950178183,
+            "angularVelocity": 0.1346265146219495,
+            "velocityX": 2.5186953261297353,
+            "velocityY": -2.754891707981806,
+            "timestamp": 1.400678814729607
+        },
+        {
+            "x": 4.101195684299111,
+            "y": 5.704704428790184,
+            "heading": 0.3789451050812407,
+            "angularVelocity": 0.000037151619885842266,
+            "velocityX": 2.1686126132704535,
+            "velocityY": -3.099622022424022,
+            "timestamp": 1.459776557478272
+        },
+        {
+            "x": 4.204458426061581,
+            "y": 5.506418480888323,
+            "heading": 0.3789451145933903,
+            "angularVelocity": 1.6095622464276883e-7,
+            "velocityX": 1.7473212505255857,
+            "velocityY": -3.3552203295641,
+            "timestamp": 1.518874300226937
+        },
+        {
+            "x": 4.281071316930325,
+            "y": 5.296392335982766,
+            "heading": 0.3789451203460425,
+            "angularVelocity": 9.73413180945881e-8,
+            "velocityX": 1.2963759241186865,
+            "velocityY": -3.5538776125279528,
+            "timestamp": 1.577972042975602
+        },
+        {
+            "x": 4.344371609432443,
+            "y": 5.081977806335691,
+            "heading": 0.37894512554853116,
+            "angularVelocity": 8.803193422611526e-8,
+            "velocityX": 1.0711118489099893,
+            "velocityY": -3.628133997587826,
+            "timestamp": 1.6370697857242669
+        },
+        {
+            "x": 4.4350051048376455,
+            "y": 4.877610290730759,
+            "heading": 0.3789451268821095,
+            "angularVelocity": 2.2565639514634894e-8,
+            "velocityX": 1.5336202567072863,
+            "velocityY": -3.4581272667918515,
+            "timestamp": 1.6961675284729318
+        },
+        {
+            "x": 4.551081441257298,
+            "y": 4.6873967879425225,
+            "heading": 0.3767623699353526,
+            "angularVelocity": -0.03693469235752831,
+            "velocityX": 1.964141624038618,
+            "velocityY": -3.218625516664622,
+            "timestamp": 1.7552652712215968
+        },
+        {
+            "x": 4.687723026941447,
+            "y": 4.517482449028109,
+            "heading": 0.36165339974597627,
+            "angularVelocity": -0.25566069848849177,
+            "velocityX": 2.312128675793254,
+            "velocityY": -2.875140927749109,
+            "timestamp": 1.8143630139702618
+        },
+        {
+            "x": 4.8416817092622,
+            "y": 4.370738170800582,
+            "heading": 0.3345492154544829,
+            "angularVelocity": -0.45863315638965824,
+            "velocityX": 2.6051533469768047,
+            "velocityY": -2.483077549196378,
+            "timestamp": 1.8734607567189268
+        },
+        {
+            "x": 5.0108564738059584,
+            "y": 4.248670932935222,
+            "heading": 0.2976430739087661,
+            "angularVelocity": -0.6244932518551631,
+            "velocityX": 2.8626265010319796,
+            "velocityY": -2.065514386638646,
+            "timestamp": 1.9325584994675917
+        },
+        {
+            "x": 5.194237326297875,
+            "y": 4.152011519418658,
+            "heading": 0.2577973678603408,
+            "angularVelocity": -0.6742339757109184,
+            "velocityX": 3.103009420708511,
+            "velocityY": -1.635585540509506,
+            "timestamp": 1.9916562422162567
         },
         {
             "x": 5.390789031982422,
             "y": 4.081310272216797,
-            "heading": 0.19899217158132915,
-            "angularVelocity": -0.3279711719205329,
-            "velocityX": 3.4312753081359326,
-            "velocityY": -1.25193680386463,
-            "timestamp": 1.462996902285867
-        },
-        {
-            "x": 5.681530534379024,
-            "y": 4.031634954440038,
-            "heading": 0.1807325834147537,
-            "angularVelocity": -0.22892527550150313,
-            "velocityX": 3.645102941474752,
-            "velocityY": -0.6227925681552634,
-            "timestamp": 1.542759122191511
-        },
-        {
-            "x": 5.983259555492767,
-            "y": 4.033604453850848,
-            "heading": 0.1807316659794543,
-            "angularVelocity": -0.000011502133581259988,
-            "velocityX": 3.7828563632073102,
-            "velocityY": 0.024692133859281144,
-            "timestamp": 1.6225213420971547
-        },
-        {
-            "x": 6.284539191755923,
-            "y": 4.050188554583329,
-            "heading": 0.18073165818528908,
-            "angularVelocity": -9.771750653524246e-8,
-            "velocityX": 3.777222306745724,
-            "velocityY": 0.20791924738433482,
-            "timestamp": 1.7022835620027985
-        },
-        {
-            "x": 6.585818825161056,
-            "y": 4.0667727072366,
-            "heading": 0.18073165039112304,
-            "angularVelocity": -9.77175162921636e-8,
-            "velocityX": 3.777222270913928,
-            "velocityY": 0.20791989832899174,
-            "timestamp": 1.7820457819084423
-        },
-        {
-            "x": 6.88709845856161,
-            "y": 4.083356859971397,
-            "heading": 0.180731642596831,
-            "angularVelocity": -9.771909620772691e-8,
-            "velocityX": 3.777222270856537,
-            "velocityY": 0.20791989935110283,
-            "timestamp": 1.8618080018140861
-        },
-        {
-            "x": 7.15139892307148,
-            "y": 4.097904823415219,
-            "heading": 0.12048670978727856,
-            "angularVelocity": -0.7553066211144209,
-            "velocityX": 3.313604671767403,
-            "velocityY": 0.1823916568650699,
-            "timestamp": 1.94157022171973
-        },
-        {
-            "x": 7.3628393316310445,
-            "y": 4.109542993215659,
-            "heading": 0.07229126938267637,
-            "angularVelocity": -0.6042389550038586,
-            "velocityX": 2.650884200686756,
-            "velocityY": 0.1459108060709806,
-            "timestamp": 2.0213324416253737
-        },
-        {
-            "x": 7.521419659615098,
-            "y": 4.118271521022388,
-            "heading": 0.03614542857914898,
-            "angularVelocity": -0.4531699449471921,
-            "velocityX": 1.98816342087339,
-            "velocityY": 0.10943185654883955,
-            "timestamp": 2.1010946615310178
-        },
-        {
-            "x": 7.627139886542953,
-            "y": 4.12409050689369,
-            "heading": 0.012048510432709781,
-            "angularVelocity": -0.302109421915929,
-            "velocityX": 1.3254423842882344,
-            "velocityY": 0.07295416148382058,
-            "timestamp": 2.180856881436662
+            "heading": 0.22104423660220585,
+            "angularVelocity": -0.6219041464028032,
+            "velocityX": 3.3258750088027327,
+            "velocityY": -1.1963442919045442,
+            "timestamp": 2.050753984964922
+        },
+        {
+            "x": 5.67793189235796,
+            "y": 4.03407988691648,
+            "heading": 0.19119773345256094,
+            "angularVelocity": -0.37371337139653454,
+            "velocityX": 3.5953667967399574,
+            "velocityY": -0.5913800499300379,
+            "timestamp": 2.130618676555184
+        },
+        {
+            "x": 5.980039011502812,
+            "y": 4.037197033313024,
+            "heading": 0.19119714274274896,
+            "angularVelocity": -0.000007396382559589573,
+            "velocityX": 3.782736940809626,
+            "velocityY": 0.039030344129324,
+            "timestamp": 2.210483368145446
+        },
+        {
+            "x": 6.281741681649569,
+            "y": 4.053135447217484,
+            "heading": 0.19119713453456852,
+            "angularVelocity": -1.0277608664279704e-7,
+            "velocityX": 3.777672763010384,
+            "velocityY": 0.1995677136803443,
+            "timestamp": 2.290348059735708
+        },
+        {
+            "x": 6.583444349801777,
+            "y": 4.069073898877065,
+            "heading": 0.19119712632638652,
+            "angularVelocity": -1.0277610566434499e-7,
+            "velocityX": 3.777672738036278,
+            "velocityY": 0.1995681864189316,
+            "timestamp": 2.3702127513259703
+        },
+        {
+            "x": 6.885147017932413,
+            "y": 4.08501235094329,
+            "heading": 0.1911971181180728,
+            "angularVelocity": -1.0277775515776054e-7,
+            "velocityX": 3.7776727377661725,
+            "velocityY": 0.1995681915106077,
+            "timestamp": 2.4500774429162324
+        },
+        {
+            "x": 7.150097958068984,
+            "y": 4.099008563262217,
+            "heading": 0.12746367267042205,
+            "angularVelocity": -0.7980177995885095,
+            "velocityX": 3.317497818634029,
+            "velocityY": 0.17524906238576554,
+            "timestamp": 2.5299421345064945
+        },
+        {
+            "x": 7.362058748903403,
+            "y": 4.110205268667911,
+            "heading": 0.07647732188798662,
+            "angularVelocity": -0.6384091613855594,
+            "velocityX": 2.6539987397918514,
+            "velocityY": 0.14019593869007996,
+            "timestamp": 2.6098068260967566
+        },
+        {
+            "x": 7.521029367100823,
+            "y": 4.118602666583934,
+            "heading": 0.03823840686026882,
+            "angularVelocity": -0.47879625234031903,
+            "velocityX": 1.9904993687699384,
+            "velocityY": 0.10514531201226385,
+            "timestamp": 2.6896715176870187
+        },
+        {
+            "x": 7.627009789128585,
+            "y": 4.124200888756926,
+            "heading": 0.012746172901683662,
+            "angularVelocity": -0.31919279284744,
+            "velocityX": 1.3269997030913787,
+            "velocityY": 0.07009633495752653,
+            "timestamp": 2.769536209277281
         },
         {
             "x": 7.68,
             "y": 4.127,
-            "heading": 2.791295127527068e-32,
-            "angularVelocity": -0.15105535486544666,
-            "velocityX": 0.6627211920578459,
-            "velocityY": 0.03647708288149429,
-            "timestamp": 2.260619101342306
+            "heading": 1.6188506035771725e-33,
+            "angularVelocity": -0.15959709663791943,
+            "velocityX": 0.6634998497618533,
+            "velocityY": 0.03504816943929136,
+            "timestamp": 2.849400900867543
         },
         {
             "x": 7.68,
             "y": 4.127,
             "heading": 0,
-            "angularVelocity": -1.938584239537671e-30,
-            "velocityX": -1.689352589413745e-32,
+            "angularVelocity": -1.4796214296723179e-31,
+            "velocityX": -2.3067017155365004e-33,
             "velocityY": 0,
-            "timestamp": 2.34038132124795
+            "timestamp": 2.929265592457805
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.1.traj
new file mode 100644
index 00000000..ce59bb95
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.1.traj	
@@ -0,0 +1,625 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.298402468716654,
+            "y": 4.3526843009931255,
+            "heading": -0.37533408415570846,
+            "angularVelocity": 0.4620245693037669,
+            "velocityX": -0.050327487964169394,
+            "velocityY": -0.2304685720750862,
+            "timestamp": 0.03174271850172795
+        },
+        {
+            "x": 2.2952744131725273,
+            "y": 4.338010072410149,
+            "heading": -0.34621912169362346,
+            "angularVelocity": 0.9172170449263883,
+            "velocityX": -0.09854403440450425,
+            "velocityY": -0.462286447903903,
+            "timestamp": 0.0634854370034559
+        },
+        {
+            "x": 2.2907034035575107,
+            "y": 4.3159283160610205,
+            "heading": -0.3028943022400045,
+            "angularVelocity": 1.364874260887912,
+            "velocityX": -0.14400183194038138,
+            "velocityY": -0.6956479278208441,
+            "timestamp": 0.09522815550518385
+        },
+        {
+            "x": 2.2847999846220546,
+            "y": 4.286378308280002,
+            "heading": -0.24568561151458213,
+            "angularVelocity": 1.8022618548662834,
+            "velocityX": -0.1859771063759033,
+            "velocityY": -0.9309224028625668,
+            "timestamp": 0.1269708740069118
+        },
+        {
+            "x": 2.2777013130983144,
+            "y": 4.249280679670162,
+            "heading": -0.1750916915555501,
+            "angularVelocity": 2.223940585151498,
+            "velocityX": -0.22363149278956135,
+            "velocityY": -1.1686972748670326,
+            "timestamp": 0.15871359250863973
+        },
+        {
+            "x": 2.2695785209744463,
+            "y": 4.204529290238701,
+            "heading": -0.09189311777806415,
+            "angularVelocity": 2.6210286233977427,
+            "velocityX": -0.25589465890975394,
+            "velocityY": -1.409815905623355,
+            "timestamp": 0.19045631101036767
+        },
+        {
+            "x": 2.2606569711878857,
+            "y": 4.151982062664549,
+            "heading": 0.002706319559794192,
+            "angularVelocity": 2.9801933105606198,
+            "velocityX": -0.2810581515277304,
+            "velocityY": -1.6554104391308149,
+            "timestamp": 0.2221990295120956
+        },
+        {
+            "x": 2.251280347316294,
+            "y": 4.091451352956859,
+            "heading": 0.10685098530745366,
+            "angularVelocity": 3.2808993893194844,
+            "velocityX": -0.2953944814487593,
+            "velocityY": -1.90691637530654,
+            "timestamp": 0.2539417480138236
+        },
+        {
+            "x": 2.242133674900212,
+            "y": 4.022709367456616,
+            "heading": 0.21739538615105536,
+            "angularVelocity": 3.4825120865934673,
+            "velocityX": -0.28815025454054716,
+            "velocityY": -2.165598560706175,
+            "timestamp": 0.2856844665155515
+        },
+        {
+            "x": 2.2351422077578347,
+            "y": 3.945850682832356,
+            "heading": 0.32707167243976176,
+            "angularVelocity": 3.455163623831899,
+            "velocityX": -0.22025420229828702,
+            "velocityY": -2.4213012700873704,
+            "timestamp": 0.31742718501727946
+        },
+        {
+            "x": 2.2344515310594897,
+            "y": 3.864378163543378,
+            "heading": 0.4256089749961829,
+            "angularVelocity": 3.1042490122910227,
+            "velocityX": -0.021758586880562782,
+            "velocityY": -2.566652231898267,
+            "timestamp": 0.3491699035190074
+        },
+        {
+            "x": 2.2416429003575074,
+            "y": 3.7806290345563274,
+            "heading": 0.5177286971400864,
+            "angularVelocity": 2.9020741288711216,
+            "velocityX": 0.22655177746121863,
+            "velocityY": -2.638372922674897,
+            "timestamp": 0.38091262202073534
+        },
+        {
+            "x": 2.256915898193716,
+            "y": 3.695399323053411,
+            "heading": 0.6036240465401915,
+            "angularVelocity": 2.705985922265222,
+            "velocityX": 0.4811496480800017,
+            "velocityY": -2.6850161399464656,
+            "timestamp": 0.4126553405224633
+        },
+        {
+            "x": 2.280071045497703,
+            "y": 3.6084777900686467,
+            "heading": 0.6811513337134051,
+            "angularVelocity": 2.4423644486842004,
+            "velocityX": 0.729463272111561,
+            "velocityY": -2.7383140791811433,
+            "timestamp": 0.4443980590241912
+        },
+        {
+            "x": 2.3096855944046597,
+            "y": 3.5244760350530098,
+            "heading": 0.76088689172136,
+            "angularVelocity": 2.511932240574098,
+            "velocityX": 0.932955660534971,
+            "velocityY": -2.6463314731869354,
+            "timestamp": 0.47614077752591916
+        },
+        {
+            "x": 2.3348942368332115,
+            "y": 3.446420745412462,
+            "heading": 0.8559069862879931,
+            "angularVelocity": 2.993445396350149,
+            "velocityX": 0.7941551202420774,
+            "velocityY": -2.458998262429792,
+            "timestamp": 0.5078834960276472
+        },
+        {
+            "x": 2.355075569752848,
+            "y": 3.373868279566975,
+            "heading": 0.9657696902827837,
+            "angularVelocity": 3.4610363944982643,
+            "velocityX": 0.6357783413709821,
+            "velocityY": -2.285641220097014,
+            "timestamp": 0.5396262145293751
+        },
+        {
+            "x": 2.370025604730125,
+            "y": 3.306495756130267,
+            "heading": 1.091029644988018,
+            "angularVelocity": 3.9461004166489144,
+            "velocityX": 0.4709752561508733,
+            "velocityY": -2.1224560030370347,
+            "timestamp": 0.571368933031103
+        },
+        {
+            "x": 2.3800631598495494,
+            "y": 3.2441375159647747,
+            "heading": 1.2334110423596254,
+            "angularVelocity": 4.485482154399005,
+            "velocityX": 0.31621598883778346,
+            "velocityY": -1.9644895934825695,
+            "timestamp": 0.603111651532831
+        },
+        {
+            "x": 2.3890320547539976,
+            "y": 3.1882487557001293,
+            "heading": 1.3963738258608078,
+            "angularVelocity": 5.133863487221998,
+            "velocityX": 0.28254967840764567,
+            "velocityY": -1.760679705539431,
+            "timestamp": 0.6348543700345589
+        },
+        {
+            "x": 2.399614951757013,
+            "y": 3.1248820965808815,
+            "heading": 1.5465533770663946,
+            "angularVelocity": 4.7311496397956585,
+            "velocityX": 0.33339605120587323,
+            "velocityY": -1.9962581061165,
+            "timestamp": 0.6665970885362869
+        },
+        {
+            "x": 2.4156041612276775,
+            "y": 3.056078363317597,
+            "heading": 1.6846738705269053,
+            "angularVelocity": 4.351249671731503,
+            "velocityX": 0.5037126694046519,
+            "velocityY": -2.167543818263053,
+            "timestamp": 0.6983398070380148
+        },
+        {
+            "x": 2.4386535219180887,
+            "y": 2.9838461830329104,
+            "heading": 1.8122491249375776,
+            "angularVelocity": 4.019039969866676,
+            "velocityX": 0.7261306459671242,
+            "velocityY": -2.275551171861821,
+            "timestamp": 0.7300825255397427
+        },
+        {
+            "x": 2.4695800740474128,
+            "y": 2.910121142766613,
+            "heading": 1.9308538821800434,
+            "angularVelocity": 3.736439814882526,
+            "velocityX": 0.9742880757878827,
+            "velocityY": -2.3225811696715355,
+            "timestamp": 0.7618252440414707
+        },
+        {
+            "x": 2.508646228883482,
+            "y": 2.8366969767650785,
+            "heading": 2.0420489888957944,
+            "angularVelocity": 3.503011461028351,
+            "velocityX": 1.2307123233299608,
+            "velocityY": -2.313102641084062,
+            "timestamp": 0.7935679625431986
+        },
+        {
+            "x": 2.555734299118471,
+            "y": 2.7651139971749186,
+            "heading": 2.147169748045839,
+            "angularVelocity": 3.3116495408016253,
+            "velocityX": 1.4834290337302587,
+            "velocityY": -2.255099215470886,
+            "timestamp": 0.8253106810449266
+        },
+        {
+            "x": 2.6104869842529297,
+            "y": 2.6966493129730225,
+            "heading": 2.247054267099527,
+            "angularVelocity": 3.1466907614812087,
+            "velocityX": 1.7248896036260801,
+            "velocityY": -2.1568626580665695,
+            "timestamp": 0.8570533995466545
+        },
+        {
+            "x": 2.751018773357776,
+            "y": 2.5737681266542305,
+            "heading": 2.423101617636397,
+            "angularVelocity": 2.7414576820608,
+            "velocityX": 2.188399607493837,
+            "velocityY": -1.9135395743649004,
+            "timestamp": 0.9212700940770386
+        },
+        {
+            "x": 2.920945282929504,
+            "y": 2.4615612619997704,
+            "heading": 2.5486951706813996,
+            "angularVelocity": 1.9557772937936253,
+            "velocityX": 2.646142265876451,
+            "velocityY": -1.7473161064272718,
+            "timestamp": 0.9854867886074228
+        },
+        {
+            "x": 3.118708267205298,
+            "y": 2.3583606370011725,
+            "heading": 2.6122511958810777,
+            "angularVelocity": 0.9897118757741349,
+            "velocityX": 3.079619493373657,
+            "velocityY": -1.6070684695514719,
+            "timestamp": 1.049703483137807
+        },
+        {
+            "x": 3.3434611246912245,
+            "y": 2.26616504003377,
+            "heading": 2.6122519169792926,
+            "angularVelocity": 0.000011229139401682067,
+            "velocityX": 3.499913209945475,
+            "velocityY": -1.4356951512629466,
+            "timestamp": 1.113920177668191
+        },
+        {
+            "x": 3.5722970045795748,
+            "y": 2.184629282343257,
+            "heading": 2.6122522165022306,
+            "angularVelocity": 0.000004664253435080619,
+            "velocityX": 3.5634951559220656,
+            "velocityY": -1.269697207039109,
+            "timestamp": 1.1781368721985752
+        },
+        {
+            "x": 3.80113295504209,
+            "y": 2.103093722723815,
+            "heading": 2.6122525160244625,
+            "angularVelocity": 0.000004664242440601037,
+            "velocityX": 3.56349625492233,
+            "velocityY": -1.2696941226220015,
+            "timestamp": 1.2423535667289594
+        },
+        {
+            "x": 4.029968923086795,
+            "y": 2.0215582124500746,
+            "heading": 2.612252815546594,
+            "angularVelocity": 0.00000466424087155042,
+            "velocityX": 3.563496528717025,
+            "velocityY": -1.2696933541971844,
+            "timestamp": 1.3065702612593435
+        },
+        {
+            "x": 4.258804909490432,
+            "y": 1.940022753702107,
+            "heading": 2.6122531150686714,
+            "angularVelocity": 0.000004664240033500949,
+            "velocityX": 3.5634968146073587,
+            "velocityY": -1.2696925518237052,
+            "timestamp": 1.3707869557897276
+        },
+        {
+            "x": 4.487640916105967,
+            "y": 1.8584873516805105,
+            "heading": 2.6122534145907514,
+            "angularVelocity": 0.000004664240074560689,
+            "velocityX": 3.563497129352575,
+            "velocityY": -1.2696916684650783,
+            "timestamp": 1.4350036503201118
+        },
+        {
+            "x": 4.716476945604857,
+            "y": 1.7769520138830965,
+            "heading": 2.6122537141129194,
+            "angularVelocity": 0.000004664241438058459,
+            "velocityX": 3.5634974856984654,
+            "velocityY": -1.2696906683484759,
+            "timestamp": 1.499220344850496
+        },
+        {
+            "x": 4.945312999982612,
+            "y": 1.6954167459104819,
+            "heading": 2.612254013635151,
+            "angularVelocity": 0.000004664242435524347,
+            "velocityX": 3.563497873118986,
+            "velocityY": -1.269689581017522,
+            "timestamp": 1.56343703938088
+        },
+        {
+            "x": 5.174149135873993,
+            "y": 1.613881706713074,
+            "heading": 2.6122543131567744,
+            "angularVelocity": 0.0000046642329655920085,
+            "velocityX": 3.563499142471556,
+            "velocityY": -1.269686018467187,
+            "timestamp": 1.6276537339112642
+        },
+        {
+            "x": 5.406651475687904,
+            "y": 1.5434785463415874,
+            "heading": 2.61225458420666,
+            "angularVelocity": 0.000004220863245612629,
+            "velocityX": 3.6205902766281897,
+            "velocityY": -1.096337344772168,
+            "timestamp": 1.6918704284416484
+        },
+        {
+            "x": 5.646764755249023,
+            "y": 1.5066063404083252,
+            "heading": 2.6122548161665473,
+            "angularVelocity": 0.0000036121430551488755,
+            "velocityX": 3.7391099202016678,
+            "velocityY": -0.5741841152508433,
+            "timestamp": 1.7560871229720325
+        },
+        {
+            "x": 5.787436421990979,
+            "y": 1.496717214982701,
+            "heading": 2.612254832075579,
+            "angularVelocity": 4.2677202387530557e-7,
+            "velocityX": 3.773625708136921,
+            "velocityY": -0.26528339929021244,
+            "timestamp": 1.7933647115888978
+        },
+        {
+            "x": 5.926771623718837,
+            "y": 1.4982765413849686,
+            "heading": 2.6080354125638485,
+            "angularVelocity": -0.11318917527356848,
+            "velocityX": 3.7377740057150746,
+            "velocityY": 0.04183013065287323,
+            "timestamp": 1.830642300205763
+        },
+        {
+            "x": 6.064421169286406,
+            "y": 1.511276136415188,
+            "heading": 2.60121232475526,
+            "angularVelocity": -0.18303458087688948,
+            "velocityX": 3.6925549820916683,
+            "velocityY": 0.34872413996054913,
+            "timestamp": 1.8679198888226283
+        },
+        {
+            "x": 6.200023778520788,
+            "y": 1.5356760041928015,
+            "heading": 2.5932728220304684,
+            "angularVelocity": -0.2129832700927332,
+            "velocityX": 3.6376443398227996,
+            "velocityY": 0.6545452289951588,
+            "timestamp": 1.9051974774394935
+        },
+        {
+            "x": 6.333175231231244,
+            "y": 1.5714002898334831,
+            "heading": 2.5855391112934316,
+            "angularVelocity": -0.20746274166290535,
+            "velocityX": 3.5718901798871894,
+            "velocityY": 0.9583314523868992,
+            "timestamp": 1.9424750660563588
+        },
+        {
+            "x": 6.463368734714764,
+            "y": 1.6183219287773098,
+            "heading": 2.5790588590244545,
+            "angularVelocity": -0.17383775371256402,
+            "velocityX": 3.4925409157130343,
+            "velocityY": 1.258709071181673,
+            "timestamp": 1.979752654673224
+        },
+        {
+            "x": 6.589861303526444,
+            "y": 1.6762129661852994,
+            "heading": 2.574283804517795,
+            "angularVelocity": -0.12809451157737914,
+            "velocityX": 3.393260495247066,
+            "velocityY": 1.5529716260079764,
+            "timestamp": 2.017030243290089
+        },
+        {
+            "x": 6.711340738368746,
+            "y": 1.7445488061219798,
+            "heading": 2.570091844488755,
+            "angularVelocity": -0.11245255352014773,
+            "velocityX": 3.258779318878531,
+            "velocityY": 1.8331614911851861,
+            "timestamp": 2.054307831906954
+        },
+        {
+            "x": 6.825305386557433,
+            "y": 1.8215960437325822,
+            "heading": 2.560973344137785,
+            "angularVelocity": -0.24461078866147912,
+            "velocityX": 3.057189384216965,
+            "velocityY": 2.0668514372666396,
+            "timestamp": 2.091585420523819
+        },
+        {
+            "x": 6.929260415482823,
+            "y": 1.9025407238929235,
+            "heading": 2.5356026772435865,
+            "angularVelocity": -0.680587662333947,
+            "velocityX": 2.788673645010342,
+            "velocityY": 2.171403332771371,
+            "timestamp": 2.128863009140684
+        },
+        {
+            "x": 7.023455735639986,
+            "y": 1.9838668818491878,
+            "heading": 2.488274688562586,
+            "angularVelocity": -1.2696097155701718,
+            "velocityX": 2.5268619471418052,
+            "velocityY": 2.1816367681967113,
+            "timestamp": 2.166140597757549
+        },
+        {
+            "x": 7.110101814938688,
+            "y": 2.060020595789651,
+            "heading": 2.4125158730705962,
+            "angularVelocity": -2.0322885224854694,
+            "velocityX": 2.3243477519224953,
+            "velocityY": 2.0428819772427436,
+            "timestamp": 2.2034181863744142
+        },
+        {
+            "x": 7.191835709933807,
+            "y": 2.1285351921147058,
+            "heading": 2.3080325133818067,
+            "angularVelocity": -2.8028465242925935,
+            "velocityX": 2.1925746280203615,
+            "velocityY": 1.8379567688575535,
+            "timestamp": 2.2406957749912793
+        },
+        {
+            "x": 7.274315415796274,
+            "y": 2.1867028423762775,
+            "heading": 2.1850264648931392,
+            "angularVelocity": -3.2997319046816447,
+            "velocityX": 2.2125815784434106,
+            "velocityY": 1.5603919786607532,
+            "timestamp": 2.2779733636081443
+        },
+        {
+            "x": 7.351412179448119,
+            "y": 2.2360068924730196,
+            "heading": 2.043511695780604,
+            "angularVelocity": -3.796242578000636,
+            "velocityX": 2.0681800114335913,
+            "velocityY": 1.322619083639869,
+            "timestamp": 2.3152509522250093
+        },
+        {
+            "x": 7.420172175937219,
+            "y": 2.2789013315216375,
+            "heading": 1.8838780266002726,
+            "angularVelocity": -4.282296014933475,
+            "velocityX": 1.8445398170951257,
+            "velocityY": 1.1506763350355667,
+            "timestamp": 2.3525285408418743
+        },
+        {
+            "x": 7.479537718839642,
+            "y": 2.3170603162912466,
+            "heading": 1.7061327210648876,
+            "angularVelocity": -4.768154597182533,
+            "velocityX": 1.592526370537947,
+            "velocityY": 1.0236441300375574,
+            "timestamp": 2.3898061294587394
+        },
+        {
+            "x": 7.529281054923766,
+            "y": 2.3515994880332096,
+            "heading": 1.5086439015102746,
+            "angularVelocity": -5.2977895535137876,
+            "velocityX": 1.3344032682848619,
+            "velocityY": 0.9265398601007353,
+            "timestamp": 2.4270837180756044
+        },
+        {
+            "x": 7.569719945861703,
+            "y": 2.383438536522646,
+            "heading": 1.288404786035775,
+            "angularVelocity": -5.908083748068905,
+            "velocityX": 1.0848043674059311,
+            "velocityY": 0.8541069760888933,
+            "timestamp": 2.4643613066924694
+        },
+        {
+            "x": 7.603202598509127,
+            "y": 2.4091886515491456,
+            "heading": 1.0408456353949866,
+            "angularVelocity": -6.640964714353526,
+            "velocityX": 0.8981979223912477,
+            "velocityY": 0.6907666504707783,
+            "timestamp": 2.5016388953093345
+        },
+        {
+            "x": 7.630132008026638,
+            "y": 2.427100499568469,
+            "heading": 0.7720308301798764,
+            "angularVelocity": -7.211164004677401,
+            "velocityX": 0.7224021326670985,
+            "velocityY": 0.48049910640464033,
+            "timestamp": 2.5389164839261995
+        },
+        {
+            "x": 7.649623319500903,
+            "y": 2.438227448776781,
+            "heading": 0.5254269726030665,
+            "angularVelocity": -6.615338242807932,
+            "velocityX": 0.5228694289911082,
+            "velocityY": 0.29848897477445496,
+            "timestamp": 2.5761940725430645
+        },
+        {
+            "x": 7.66290885295536,
+            "y": 2.4458359453096237,
+            "heading": 0.3183730749926382,
+            "angularVelocity": -5.554380132752273,
+            "velocityX": 0.3563946582222695,
+            "velocityY": 0.2041037742822349,
+            "timestamp": 2.6134716611599296
+        },
+        {
+            "x": 7.671732744074589,
+            "y": 2.451418069701132,
+            "heading": 0.15988099310288126,
+            "angularVelocity": -4.251672057404801,
+            "velocityX": 0.23670766931625417,
+            "velocityY": 0.14974478228408286,
+            "timestamp": 2.6507492497767946
+        },
+        {
+            "x": 7.677275184086734,
+            "y": 2.4551558590330185,
+            "heading": 0.05341064749493927,
+            "angularVelocity": -2.8561489505727446,
+            "velocityX": 0.14868021827027583,
+            "velocityY": 0.10026907508163654,
+            "timestamp": 2.6880268383936596
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 1.0247603884557974e-38,
+            "angularVelocity": -1.4327817189005392,
+            "velocityX": 0.07309528363731636,
+            "velocityY": 0.04947050051802475,
+            "timestamp": 2.7253044270105247
+        },
+        {
+            "x": 7.68,
+            "y": 2.457,
+            "heading": 0,
+            "angularVelocity": -2.6319896813971663e-37,
+            "velocityX": 5.227147746061896e-36,
+            "velocityY": -6.467829854732196e-35,
+            "timestamp": 2.7625820156273897
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.1.traj
new file mode 100644
index 00000000..8826d851
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.1.traj	
@@ -0,0 +1,571 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": -4.609910300520975e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 2.298808348352727,
+            "y": 4.35514296819253,
+            "heading": -0.3837399750205753,
+            "angularVelocity": 0.24884159467028183,
+            "velocityX": -0.047369219319970686,
+            "velocityY": -0.19307136062676852,
+            "timestamp": 0.02515666638336474
+        },
+        {
+            "x": 2.2965596540188837,
+            "y": 4.3453750993410925,
+            "heading": -0.37149402212882315,
+            "angularVelocity": 0.48678758564965857,
+            "velocityX": -0.08938761199815681,
+            "velocityY": -0.38828152755153583,
+            "timestamp": 0.05031333276672948
+        },
+        {
+            "x": 2.2934096466067952,
+            "y": 4.330641358670927,
+            "heading": -0.3535645425221108,
+            "angularVelocity": 0.7127128584321681,
+            "velocityX": -0.1252156133919033,
+            "velocityY": -0.5856793760205414,
+            "timestamp": 0.07546999915009422
+        },
+        {
+            "x": 2.289538880665139,
+            "y": 4.310886769515238,
+            "heading": -0.33028900557963653,
+            "angularVelocity": 0.9252234214095049,
+            "velocityX": -0.15386640990779057,
+            "velocityY": -0.7852625961900879,
+            "timestamp": 0.10062666553345896
+        },
+        {
+            "x": 2.2851573432175654,
+            "y": 4.286058471237428,
+            "heading": -0.3020481691352866,
+            "angularVelocity": 1.1225985197714665,
+            "velocityX": -0.17417003432819553,
+            "velocityY": -0.9869470739663532,
+            "timestamp": 0.1257833319168237
+        },
+        {
+            "x": 2.280510060472114,
+            "y": 4.25610900707676,
+            "heading": -0.26927550139535,
+            "angularVelocity": 1.3027428690475438,
+            "velocityX": -0.18473364771909595,
+            "velocityY": -1.1905180004483138,
+            "timestamp": 0.15093999830018845
+        },
+        {
+            "x": 2.2758836751536,
+            "y": 4.2210014881511375,
+            "heading": -0.23246752663964654,
+            "angularVelocity": 1.463149933889621,
+            "velocityX": -0.1839029562984313,
+            "velocityY": -1.3955552930033008,
+            "timestamp": 0.1760966646835532
+        },
+        {
+            "x": 2.271613693612169,
+            "y": 4.180717494993525,
+            "heading": -0.19219487185489256,
+            "angularVelocity": 1.6008740653882818,
+            "velocityX": -0.16973558723403512,
+            "velocityY": -1.6013247758554696,
+            "timestamp": 0.20125333106691795
+        },
+        {
+            "x": 2.268091592763015,
+            "y": 4.135268758759812,
+            "heading": -0.1491141249151469,
+            "angularVelocity": 1.7124982413501877,
+            "velocityX": -0.1400066604803655,
+            "velocityY": -1.8066279347634933,
+            "timestamp": 0.2264099974502827
+        },
+        {
+            "x": 2.2657701315686967,
+            "y": 4.084713672798125,
+            "heading": -0.10398137252966654,
+            "angularVelocity": 1.7940672940403972,
+            "velocityX": -0.09228015981695388,
+            "velocityY": -2.0096099058307106,
+            "timestamp": 0.25156666383364745
+        },
+        {
+            "x": 2.265163909208295,
+            "y": 4.029179243747037,
+            "heading": -0.057668550112221675,
+            "angularVelocity": 1.8409761337881332,
+            "velocityX": -0.02409788129965802,
+            "velocityY": -2.2075432493635576,
+            "timestamp": 0.2767233302170122
+        },
+        {
+            "x": 2.266840363517046,
+            "y": 3.9688874958283327,
+            "heading": -0.011179237419756825,
+            "angularVelocity": 1.8479917801512262,
+            "velocityX": 0.06664055893588815,
+            "velocityY": -2.3966509314037507,
+            "timestamp": 0.3018799966003769
+        },
+        {
+            "x": 2.2713096946462064,
+            "y": 3.9041248350083095,
+            "heading": 0.03446639703260334,
+            "angularVelocity": 1.8144548151476838,
+            "velocityX": 0.1776599117328043,
+            "velocityY": -2.574373719995273,
+            "timestamp": 0.3270366629837416
+        },
+        {
+            "x": 2.2787160490905882,
+            "y": 3.8349972418338227,
+            "heading": 0.07865637806724814,
+            "angularVelocity": 1.7565912892125506,
+            "velocityX": 0.29440921668697884,
+            "velocityY": -2.74788368701343,
+            "timestamp": 0.35219332936710634
+        },
+        {
+            "x": 2.289237241442295,
+            "y": 3.7616571782461574,
+            "heading": 0.12059430680114623,
+            "angularVelocity": 1.667070195025133,
+            "velocityX": 0.41822681079334173,
+            "velocityY": -2.915333155436007,
+            "timestamp": 0.37734999575047107
+        },
+        {
+            "x": 2.303092280593543,
+            "y": 3.684333317171128,
+            "heading": 0.15922184993862323,
+            "angularVelocity": 1.5354794052927534,
+            "velocityX": 0.550750204343837,
+            "velocityY": -3.073692670431113,
+            "timestamp": 0.4025066621338358
+        },
+        {
+            "x": 2.3205444941015814,
+            "y": 3.6033821039148743,
+            "heading": 0.19314125064776663,
+            "angularVelocity": 1.3483265307192482,
+            "velocityX": 0.6937411039317692,
+            "velocityY": -3.217883165544707,
+            "timestamp": 0.4276633285172005
+        },
+        {
+            "x": 2.341904587688066,
+            "y": 3.5193842561187316,
+            "heading": 0.22066827104785883,
+            "angularVelocity": 1.0942236932591152,
+            "velocityX": 0.8490828339882706,
+            "velocityY": -3.3389896147642064,
+            "timestamp": 0.45281999490056524
+        },
+        {
+            "x": 2.3675851289681646,
+            "y": 3.4337423994061407,
+            "heading": 0.23918679984335672,
+            "angularVelocity": 0.7361280907928144,
+            "velocityX": 1.0208244959308244,
+            "velocityY": -3.40434044032252,
+            "timestamp": 0.47797666128392996
+        },
+        {
+            "x": 2.3978885412195137,
+            "y": 3.3469640370766824,
+            "heading": 0.24945072521405584,
+            "angularVelocity": 0.40800021808479553,
+            "velocityX": 1.2045877537807594,
+            "velocityY": -3.4495175555868487,
+            "timestamp": 0.5031333276672947
+        },
+        {
+            "x": 2.4331457577160354,
+            "y": 3.2598611919071945,
+            "heading": 0.25313177220508776,
+            "angularVelocity": 0.1463249118518414,
+            "velocityX": 1.4015059054023244,
+            "velocityY": -3.462416038839161,
+            "timestamp": 0.5282899940506595
+        },
+        {
+            "x": 2.473498104501041,
+            "y": 3.173715186571369,
+            "heading": 0.2532432496800442,
+            "angularVelocity": 0.004431329384332257,
+            "velocityX": 1.604041893710102,
+            "velocityY": -3.424380799229854,
+            "timestamp": 0.5534466604340242
+        },
+        {
+            "x": 2.5185817721335604,
+            "y": 3.089905547060991,
+            "heading": 0.25324321969105484,
+            "angularVelocity": -0.0000011920891614956396,
+            "velocityX": 1.7921161311870697,
+            "velocityY": -3.3315081669882356,
+            "timestamp": 0.578603326817389
+        },
+        {
+            "x": 2.568241478882672,
+            "y": 3.0087236123099395,
+            "heading": 0.2532431890633549,
+            "angularVelocity": -0.0000012174784783024135,
+            "velocityX": 1.97401778090718,
+            "velocityY": -3.2270545514223747,
+            "timestamp": 0.6037599932007538
+        },
+        {
+            "x": 2.6223245423144714,
+            "y": 2.9304189809174512,
+            "heading": 0.25324315880142845,
+            "angularVelocity": -0.0000012029386552177117,
+            "velocityX": 2.1498501672527754,
+            "velocityY": -3.1126791681853656,
+            "timestamp": 0.6289166595841186
+        },
+        {
+            "x": 2.6806646810204167,
+            "y": 2.855232404738762,
+            "heading": 0.25324312847978453,
+            "angularVelocity": -0.0000012053124772832063,
+            "velocityX": 2.319072718813166,
+            "velocityY": -2.9887336832676343,
+            "timestamp": 0.6540733259674834
+        },
+        {
+            "x": 2.7430825233459473,
+            "y": 2.7833950519561768,
+            "heading": 0.2532430937019675,
+            "angularVelocity": -0.0000013824493495341482,
+            "velocityX": 2.4811650865953045,
+            "velocityY": -2.8555990562441647,
+            "timestamp": 0.6792299923508481
+        },
+        {
+            "x": 2.8747857816965947,
+            "y": 2.6605842580771375,
+            "heading": 0.2532430684120768,
+            "angularVelocity": -5.312693002073383e-7,
+            "velocityX": 2.7667141273306664,
+            "velocityY": -2.579908520631989,
+            "timestamp": 0.7268327631068262
+        },
+        {
+            "x": 3.0186320127358077,
+            "y": 2.5522491183421203,
+            "heading": 0.2532430469824583,
+            "angularVelocity": -4.5017586399077444e-7,
+            "velocityX": 3.021803747025554,
+            "velocityY": -2.2758158404343,
+            "timestamp": 0.7744355338628043
+        },
+        {
+            "x": 3.1730376030001257,
+            "y": 2.4595822138278454,
+            "heading": 0.2532430277420137,
+            "angularVelocity": -4.041874945252969e-7,
+            "velocityX": 3.2436261127705017,
+            "velocityY": -1.946670394236201,
+            "timestamp": 0.8220383046187824
+        },
+        {
+            "x": 3.336302611834685,
+            "y": 2.38360340882859,
+            "heading": 0.25324300957525164,
+            "angularVelocity": -3.816324501860361e-7,
+            "velocityX": 3.4297375182526793,
+            "velocityY": -1.5961004746706646,
+            "timestamp": 0.8696410753747604
+        },
+        {
+            "x": 3.505316504811254,
+            "y": 2.3214540241877932,
+            "heading": 0.25324299166355524,
+            "angularVelocity": -3.7627424124720095e-7,
+            "velocityX": 3.550505365390803,
+            "velocityY": -1.3055833442844713,
+            "timestamp": 0.9172438461307385
+        },
+        {
+            "x": 3.6743308817857088,
+            "y": 2.2593059557844626,
+            "heading": 0.25324297375187266,
+            "angularVelocity": -3.76273948990246e-7,
+            "velocityX": 3.5505155328218008,
+            "velocityY": -1.3055556938463668,
+            "timestamp": 0.9648466168867166
+        },
+        {
+            "x": 3.843345266545321,
+            "y": 2.1971579085532174,
+            "heading": 0.2532429558401917,
+            "angularVelocity": -3.762739157561073e-7,
+            "velocityX": 3.5505156963660025,
+            "velocityY": -1.3055552490805507,
+            "timestamp": 1.0124493876426945
+        },
+        {
+            "x": 4.012359658311276,
+            "y": 2.135009880376043,
+            "heading": 0.25324293792850894,
+            "angularVelocity": -3.7627395316214525e-7,
+            "velocityX": 3.5505158435494915,
+            "velocityY": -1.3055548488082471,
+            "timestamp": 1.0600521583986726
+        },
+        {
+            "x": 4.181374056391421,
+            "y": 2.07286186937061,
+            "heading": 0.25324292001682175,
+            "angularVelocity": -3.7627404706616014e-7,
+            "velocityX": 3.5505159761928384,
+            "velocityY": -1.30555448807838,
+            "timestamp": 1.1076549291546507
+        },
+        {
+            "x": 4.3503884601627245,
+            "y": 2.010713873842556,
+            "heading": 0.25324290210512795,
+            "angularVelocity": -3.7627418619029544e-7,
+            "velocityX": 3.5505160957480255,
+            "velocityY": -1.30555416294226,
+            "timestamp": 1.1552576999106288
+        },
+        {
+            "x": 4.51940286907005,
+            "y": 1.9485658922821711,
+            "heading": 0.2532428841934256,
+            "angularVelocity": -3.7627436389492574e-7,
+            "velocityX": 3.5505162036413855,
+            "velocityY": -1.3055538695209365,
+            "timestamp": 1.2028604706666068
+        },
+        {
+            "x": 4.688417282561973,
+            "y": 1.8864179231898264,
+            "heading": 0.25324286628171494,
+            "angularVelocity": -3.7627454024029507e-7,
+            "velocityX": 3.550516299950831,
+            "velocityY": -1.3055536076025602,
+            "timestamp": 1.250463241422585
+        },
+        {
+            "x": 4.857431700124039,
+            "y": 1.824269965166445,
+            "heading": 0.2532428483699966,
+            "angularVelocity": -3.7627470129766985e-7,
+            "velocityX": 3.5505163854530717,
+            "velocityY": -1.3055533750748494,
+            "timestamp": 1.298066012178563
+        },
+        {
+            "x": 5.02644612130461,
+            "y": 1.7621220169837726,
+            "heading": 0.2532428304582713,
+            "angularVelocity": -3.76274846815446e-7,
+            "velocityX": 3.5505164614676405,
+            "velocityY": -1.3055531683492934,
+            "timestamp": 1.345668782934541
+        },
+        {
+            "x": 5.1954605457099055,
+            "y": 1.6999740775709098,
+            "heading": 0.25324281254653974,
+            "angularVelocity": -3.7627497879654404e-7,
+            "velocityX": 3.5505165292100247,
+            "velocityY": -1.3055529841203253,
+            "timestamp": 1.3932715536905191
+        },
+        {
+            "x": 5.364474972997734,
+            "y": 1.6378261459972472,
+            "heading": 0.25324279463480254,
+            "angularVelocity": -3.7627509767034604e-7,
+            "velocityX": 3.550516589763906,
+            "velocityY": -1.3055528194408317,
+            "timestamp": 1.4408743244464972
+        },
+        {
+            "x": 5.533489402870974,
+            "y": 1.5756782214547507,
+            "heading": 0.2532427767230601,
+            "angularVelocity": -3.762752068122207e-7,
+            "velocityX": 3.5505166440761182,
+            "velocityY": -1.3055526717358554,
+            "timestamp": 1.4884770952024753
+        },
+        {
+            "x": 5.702503835071411,
+            "y": 1.513530303241192,
+            "heading": 0.25324275881131303,
+            "angularVelocity": -3.7627530496428226e-7,
+            "velocityX": 3.550516692963962,
+            "velocityY": -1.3055525387827163,
+            "timestamp": 1.5360798659584534
+        },
+        {
+            "x": 5.871518269374186,
+            "y": 1.4513823907450571,
+            "heading": 0.2532427408995616,
+            "angularVelocity": -3.762753964251323e-7,
+            "velocityX": 3.5505167371281625,
+            "velocityY": -1.3055524186757583,
+            "timestamp": 1.5836826367144314
+        },
+        {
+            "x": 6.040532705582966,
+            "y": 1.3892344834324082,
+            "heading": 0.25324272298780626,
+            "angularVelocity": -3.7627547903960503e-7,
+            "velocityX": 3.550516777167954,
+            "velocityY": -1.3055523097853368,
+            "timestamp": 1.6312854074704095
+        },
+        {
+            "x": 6.20954714352589,
+            "y": 1.3270865808358587,
+            "heading": 0.2532427050760471,
+            "angularVelocity": -3.7627555795965075e-7,
+            "velocityX": 3.5505168135974197,
+            "velocityY": -1.3055522107133783,
+            "timestamp": 1.6788881782263876
+        },
+        {
+            "x": 6.378561583051887,
+            "y": 1.2649386825445668,
+            "heading": 0.25324268716428455,
+            "angularVelocity": -3.7627562961930894e-7,
+            "velocityX": 3.5505168468533315,
+            "velocityY": -1.3055521202720615,
+            "timestamp": 1.7264909489823657
+        },
+        {
+            "x": 6.547576024027691,
+            "y": 1.2027907881961044,
+            "heading": 0.25324266925251887,
+            "angularVelocity": -3.7627569544162935e-7,
+            "velocityX": 3.5505168773096716,
+            "velocityY": -1.3055520374443248,
+            "timestamp": 1.7740937197383437
+        },
+        {
+            "x": 6.716590466335514,
+            "y": 1.1406428974701475,
+            "heading": 0.25324265134075025,
+            "angularVelocity": -3.762757576316506e-7,
+            "velocityX": 3.550516905291665,
+            "velocityY": -1.3055519613457025,
+            "timestamp": 1.8216964904943218
+        },
+        {
+            "x": 6.88560491686231,
+            "y": 1.0784950290966298,
+            "heading": 0.253242633428611,
+            "angularVelocity": -3.7628354454135297e-7,
+            "velocityX": 3.550517077949107,
+            "velocityY": -1.3055514917839766,
+            "timestamp": 1.8692992612502999
+        },
+        {
+            "x": 7.0444139114910005,
+            "y": 1.0203058482073266,
+            "heading": 0.2259027129119336,
+            "angularVelocity": -0.574334646544581,
+            "velocityX": 3.336129223292024,
+            "velocityY": -1.2223906290579827,
+            "timestamp": 1.916902032006278
+        },
+        {
+            "x": 7.185810534595712,
+            "y": 0.9684234171781619,
+            "heading": 0.18518533646408028,
+            "angularVelocity": -0.8553572786042075,
+            "velocityX": 2.9703443908662663,
+            "velocityY": -1.089903596055891,
+            "timestamp": 1.964504802762256
+        },
+        {
+            "x": 7.3094655243228,
+            "y": 0.9230369546143523,
+            "heading": 0.14380134057962407,
+            "angularVelocity": -0.8693610734677483,
+            "velocityX": 2.59764269523238,
+            "velocityY": -0.953441613650398,
+            "timestamp": 2.012107573518234
+        },
+        {
+            "x": 7.415398593682274,
+            "y": 0.8841490900710189,
+            "heading": 0.10540313701630608,
+            "angularVelocity": -0.8066379950897271,
+            "velocityX": 2.2253551143589885,
+            "velocityY": -0.8169243917057022,
+            "timestamp": 2.059710344274212
+        },
+        {
+            "x": 7.503637039429732,
+            "y": 0.8517540010892736,
+            "heading": 0.07171800478429124,
+            "angularVelocity": -0.70762965468317,
+            "velocityX": 1.8536409613588987,
+            "velocityY": -0.680529483206126,
+            "timestamp": 2.1073131150301903
+        },
+        {
+            "x": 7.574202084965293,
+            "y": 0.8258459262172375,
+            "heading": 0.04375776286271558,
+            "angularVelocity": -0.5873658502969467,
+            "velocityX": 1.482372652157004,
+            "velocityY": -0.5442556065663985,
+            "timestamp": 2.1549158857861683
+        },
+        {
+            "x": 7.627109788299601,
+            "y": 0.8064200554899893,
+            "heading": 0.022189516524924582,
+            "angularVelocity": -0.4530880449870122,
+            "velocityX": 1.1114416764840027,
+            "velocityY": -0.40808277372822227,
+            "timestamp": 2.2025186565421464
+        },
+        {
+            "x": 7.66237251850946,
+            "y": 0.7934724766877013,
+            "heading": 0.0074863600733853135,
+            "angularVelocity": -0.3088718622474896,
+            "velocityX": 0.7407705402406998,
+            "velocityY": -0.27199212559831976,
+            "timestamp": 2.2501214272981245
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": -1.0670900456397286e-32,
+            "angularVelocity": -0.1572673177316084,
+            "velocityX": 0.37030368633165434,
+            "velocityY": -0.13596848639085585,
+            "timestamp": 2.2977241980541026
+        },
+        {
+            "x": 7.68,
+            "y": 0.787,
+            "heading": 0,
+            "angularVelocity": 2.230662363627099e-31,
+            "velocityX": 2.3924515989499496e-33,
+            "velocityY": 1.2585115119762557e-33,
+            "timestamp": 2.3453269688100806
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.1.traj
new file mode 100644
index 00000000..297f1b3f
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.1.traj	
@@ -0,0 +1,256 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -4.289844309028223e-18,
+            "velocityX": -1.3032340882001426e-17,
+            "velocityY": -3.394190195762378e-17,
+            "timestamp": 0
+        },
+        {
+            "x": 2.271980215040446,
+            "y": 4.410826628196332,
+            "heading": -0.3872050871501322,
+            "angularVelocity": 0.033498492691473286,
+            "velocityX": -0.3358317815939762,
+            "velocityY": 0.6091837294305799,
+            "timestamp": 0.0834339883696639
+        },
+        {
+            "x": 2.216375801038755,
+            "y": 4.512717243328352,
+            "heading": -0.38158865604258857,
+            "angularVelocity": 0.06731586511476574,
+            "velocityX": -0.6664479918611741,
+            "velocityY": 1.2212123275291613,
+            "timestamp": 0.1668679767393278
+        },
+        {
+            "x": 2.133913144213912,
+            "y": 4.66605742038127,
+            "heading": -0.3731070083031397,
+            "angularVelocity": 0.10165698541488709,
+            "velocityX": -0.9883580832768961,
+            "velocityY": 1.837862243543364,
+            "timestamp": 0.2503019651089917
+        },
+        {
+            "x": 2.026047479965626,
+            "y": 4.871580643769176,
+            "heading": -0.3616752437087536,
+            "angularVelocity": 0.13701567930528194,
+            "velocityX": -1.2928264171046289,
+            "velocityY": 2.4633033539909763,
+            "timestamp": 0.3337359534786556
+        },
+        {
+            "x": 1.8971303225339013,
+            "y": 5.131188087013384,
+            "heading": -0.34705934044989933,
+            "angularVelocity": 0.1751792470161148,
+            "velocityX": -1.5451395762230817,
+            "velocityY": 3.111531023711595,
+            "timestamp": 0.41716994184831946
+        },
+        {
+            "x": 1.812941926504828,
+            "y": 5.427772716701387,
+            "heading": -0.3288482106163032,
+            "angularVelocity": 0.21826991841509946,
+            "velocityX": -1.0090419704726812,
+            "velocityY": 3.5547219482539485,
+            "timestamp": 0.5006039302179833
+        },
+        {
+            "x": 1.7681664127359014,
+            "y": 5.682327706651028,
+            "heading": -0.2856043053433686,
+            "angularVelocity": 0.5183008282076023,
+            "velocityX": -0.5366579573130098,
+            "velocityY": 3.0509747277288763,
+            "timestamp": 0.5840379185876472
+        },
+        {
+            "x": 1.7580426073358177,
+            "y": 5.890413977811611,
+            "heading": -0.23166998700826372,
+            "angularVelocity": 0.6464310215597402,
+            "velocityX": -0.12133910409739124,
+            "velocityY": 2.4940228224314014,
+            "timestamp": 0.6674719069573111
+        },
+        {
+            "x": 1.7809381631803756,
+            "y": 6.0507944700387695,
+            "heading": -0.17161076742769205,
+            "angularVelocity": 0.7198411672948628,
+            "velocityX": 0.27441521485536685,
+            "velocityY": 1.9222441041228078,
+            "timestamp": 0.7509058953269749
+        },
+        {
+            "x": 1.8360347454575048,
+            "y": 6.162894796860879,
+            "heading": -0.10766394629623156,
+            "angularVelocity": 0.7664361057242964,
+            "velocityX": 0.6603613629651003,
+            "velocityY": 1.34358106345549,
+            "timestamp": 0.8343398836966388
+        },
+        {
+            "x": 1.9228414263613316,
+            "y": 6.226384193614056,
+            "heading": -0.041156065591054634,
+            "angularVelocity": 0.797131744563275,
+            "velocityX": 1.0404234844874343,
+            "velocityY": 0.760953635248516,
+            "timestamp": 0.9177738720663027
+        },
+        {
+            "x": 2.041031122207642,
+            "y": 6.2410478591918945,
+            "heading": 0.027035456766794728,
+            "angularVelocity": 0.8173110705883178,
+            "velocityX": 1.4165653369372246,
+            "velocityY": 0.17575170340583585,
+            "timestamp": 1.0012078604359667
+        },
+        {
+            "x": 2.201330972597037,
+            "y": 6.260936085330867,
+            "heading": 0.09277658873792281,
+            "angularVelocity": 0.8449644804697937,
+            "velocityX": 2.060318642882427,
+            "velocityY": 0.2556214680702958,
+            "timestamp": 1.0790112870770416
+        },
+        {
+            "x": 2.4117170311040814,
+            "y": 6.287038452225851,
+            "heading": 0.16066894258910833,
+            "angularVelocity": 0.8726139294965615,
+            "velocityX": 2.7040718846096095,
+            "velocityY": 0.33549122476729665,
+            "timestamp": 1.1568147137181166
+        },
+        {
+            "x": 2.672189282535082,
+            "y": 6.319354957991783,
+            "heading": 0.23071153953234388,
+            "angularVelocity": 0.9002507982373139,
+            "velocityX": 3.3478249310538675,
+            "velocityY": 0.4153609572357597,
+            "timestamp": 1.2346181403591916
+        },
+        {
+            "x": 2.9642755449736304,
+            "y": 6.355593779334123,
+            "heading": 0.23071154098757585,
+            "angularVelocity": 1.8703321803502023e-8,
+            "velocityX": 3.7541567903686675,
+            "velocityY": 0.4657741041344694,
+            "timestamp": 1.3124215670002666
+        },
+        {
+            "x": 3.256361807413129,
+            "y": 6.39183260067658,
+            "heading": 0.23071154244151956,
+            "angularVelocity": 1.868721301612308e-8,
+            "velocityX": 3.7541567903805326,
+            "velocityY": 0.46577410413594156,
+            "timestamp": 1.3902249936413416
+        },
+        {
+            "x": 3.5484480698526375,
+            "y": 6.4280714220190385,
+            "heading": 0.23071154389546672,
+            "angularVelocity": 1.868722261906901e-8,
+            "velocityX": 3.7541567903806516,
+            "velocityY": 0.4657741041359563,
+            "timestamp": 1.4680284202824165
+        },
+        {
+            "x": 3.8405343322921546,
+            "y": 6.464310243361498,
+            "heading": 0.23071154534942268,
+            "angularVelocity": 1.868730306107698e-8,
+            "velocityX": 3.754156790380741,
+            "velocityY": 0.46577410413596737,
+            "timestamp": 1.5458318469234915
+        },
+        {
+            "x": 4.132620594731676,
+            "y": 6.500549064703958,
+            "heading": 0.23071154680338032,
+            "angularVelocity": 1.86873022983191e-8,
+            "velocityX": 3.754156790380778,
+            "velocityY": 0.465774104135972,
+            "timestamp": 1.6236352735645665
+        },
+        {
+            "x": 4.424706857172369,
+            "y": 6.536787886046564,
+            "heading": 0.23071154826355325,
+            "angularVelocity": 1.876697436713731e-8,
+            "velocityX": 3.7541567903965425,
+            "velocityY": 0.46577410413792786,
+            "timestamp": 1.7014387002056415
+        },
+        {
+            "x": 4.6751378783325555,
+            "y": 6.567858587416207,
+            "heading": 0.24147534520321134,
+            "angularVelocity": 0.13834605240872366,
+            "velocityX": 3.2187659589270945,
+            "velocityY": 0.39934875250390567,
+            "timestamp": 1.7792421268467165
+        },
+        {
+            "x": 4.875482718759792,
+            "y": 6.592715151427428,
+            "heading": 0.2500855408103817,
+            "angularVelocity": 0.11066602050088868,
+            "velocityX": 2.575013069173925,
+            "velocityY": 0.31947903947593354,
+            "timestamp": 1.8570455534877914
+        },
+        {
+            "x": 5.025741356581141,
+            "y": 6.611357575366475,
+            "heading": 0.25654288837544836,
+            "angularVelocity": 0.08299567055584532,
+            "velocityX": 1.931259898289217,
+            "velocityY": 0.23960929156828065,
+            "timestamp": 1.9348489801288664
+        },
+        {
+            "x": 5.125913784825429,
+            "y": 6.623785858368443,
+            "heading": 0.2608476597362627,
+            "angularVelocity": 0.05532881451478021,
+            "velocityX": 1.2875066378045663,
+            "velocityY": 0.15973953254406137,
+            "timestamp": 2.0126524067699414
+        },
+        {
+            "x": 5.175999999999999,
+            "y": 6.63,
+            "heading": 0.26300000000000007,
+            "angularVelocity": 0.02766382352150404,
+            "velocityX": 0.6437533324290226,
+            "velocityY": 0.07986976795027836,
+            "timestamp": 2.090455833411016
+        },
+        {
+            "x": 5.176,
+            "y": 6.63,
+            "heading": 0.263,
+            "angularVelocity": -5.112167395133813e-18,
+            "velocityX": -1.2520036782163602e-16,
+            "velocityY": -1.3106574021367763e-17,
+            "timestamp": 2.168259260052091
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.traj
index 714e9458..297f1b3f 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.traj	
@@ -4,172 +4,253 @@
             "x": 2.3,
             "y": 4.36,
             "heading": -0.39,
-            "angularVelocity": 5.020065556819226e-31,
-            "velocityX": -2.911156118414847e-31,
-            "velocityY": 8.635491730351355e-32,
+            "angularVelocity": -4.289844309028223e-18,
+            "velocityX": -1.3032340882001426e-17,
+            "velocityY": -3.394190195762378e-17,
             "timestamp": 0
         },
         {
-            "x": 2.3221690164859408,
-            "y": 4.411229757983623,
-            "heading": -0.4048176043167742,
-            "angularVelocity": -0.1808500047435309,
-            "velocityX": 0.27057455786553103,
-            "velocityY": 0.6252631516043967,
-            "timestamp": 0.08193311544453755
-        },
-        {
-            "x": 2.3687303275118676,
-            "y": 4.5127321003230465,
-            "heading": -0.4313760168590284,
-            "angularVelocity": -0.32414747563532126,
-            "velocityX": 0.5682843960383053,
-            "velocityY": 1.2388439251785637,
-            "timestamp": 0.1638662308890751
-        },
-        {
-            "x": 2.4431953998066236,
-            "y": 4.662720767025029,
-            "heading": -0.4646753197408115,
-            "angularVelocity": -0.40642056268856935,
-            "velocityX": 0.9088519567543129,
-            "velocityY": 1.830623257619443,
-            "timestamp": 0.24579934633361264
-        },
-        {
-            "x": 2.551740018878988,
-            "y": 4.857106976368847,
-            "heading": -0.4953245435557511,
-            "angularVelocity": -0.3740761430663416,
-            "velocityX": 1.3247954564335083,
-            "velocityY": 2.3724986934666186,
-            "timestamp": 0.3277324617781502
-        },
-        {
-            "x": 2.701293241756868,
-            "y": 5.089437731072216,
-            "heading": -0.5138028115710582,
-            "angularVelocity": -0.2255286878210994,
-            "velocityX": 1.8253086321258618,
-            "velocityY": 2.835614799252222,
-            "timestamp": 0.4096655772226877
-        },
-        {
-            "x": 2.9065804913658706,
-            "y": 5.321655101693131,
-            "heading": -0.5138024716246538,
-            "angularVelocity": 0.000004149072112901325,
-            "velocityX": 2.5055467315650244,
-            "velocityY": 2.8342309377715424,
-            "timestamp": 0.4915986926672252
-        },
-        {
-            "x": 3.1502852438919966,
-            "y": 5.513164387705616,
-            "heading": -0.5138023715427595,
-            "angularVelocity": 0.0000012215072474193347,
-            "velocityX": 2.974435320857471,
-            "velocityY": 2.3373856220824885,
-            "timestamp": 0.5735318081117627
-        },
-        {
-            "x": 3.4075351073224858,
-            "y": 5.686052564856294,
-            "heading": -0.5138022777253332,
-            "angularVelocity": 0.0000011450489327031063,
-            "velocityX": 3.1397544452539083,
-            "velocityY": 2.110113550701131,
-            "timestamp": 0.6554649235563003
-        },
-        {
-            "x": 3.664785106053728,
-            "y": 5.858940540685368,
-            "heading": -0.5138021839079745,
-            "angularVelocity": 0.0000011450481052658978,
-            "velocityX": 3.13975609661006,
-            "velocityY": 2.110111093555414,
-            "timestamp": 0.7373980390008378
-        },
-        {
-            "x": 3.922035141067721,
-            "y": 6.031828462527248,
-            "heading": -0.5138020900906538,
-            "angularVelocity": 0.000001145047641454271,
-            "velocityX": 3.1397565394438236,
-            "velocityY": 2.1101104346375243,
-            "timestamp": 0.8193311544453753
-        },
-        {
-            "x": 4.179285313417939,
-            "y": 6.204716180018366,
-            "heading": -0.5138019962733545,
-            "angularVelocity": 0.00000114504738127865,
-            "velocityX": 3.139758215643051,
-            "velocityY": 2.110107940520708,
-            "timestamp": 0.9012642698899128
-        },
-        {
-            "x": 4.443584109358565,
-            "y": 6.354084975447565,
-            "heading": -0.4978326665645833,
-            "angularVelocity": 0.19490690207652947,
-            "velocityX": 3.225787210783358,
-            "velocityY": 1.8230576808752068,
-            "timestamp": 0.9831973853344503
-        },
-        {
-            "x": 4.674709010100242,
-            "y": 6.4693171293955105,
-            "heading": -0.3712044647358083,
-            "angularVelocity": 1.5455069801964878,
-            "velocityX": 2.8208972585465766,
-            "velocityY": 1.406417335929929,
-            "timestamp": 1.065130500778988
-        },
-        {
-            "x": 4.870816853917273,
-            "y": 6.548974021375514,
-            "heading": -0.15522232897542676,
-            "angularVelocity": 2.636078642787472,
-            "velocityX": 2.3935113751384223,
-            "velocityY": 0.9722185168697003,
-            "timestamp": 1.1470636162235255
-        },
-        {
-            "x": 5.022935942406859,
-            "y": 6.594453289671109,
-            "heading": 0.041233782182786366,
-            "angularVelocity": 2.3977620049271122,
-            "velocityX": 1.856625220025449,
-            "velocityY": 0.5550779809707214,
-            "timestamp": 1.228996731668063
-        },
-        {
-            "x": 5.124959992690689,
-            "y": 6.619307485763033,
-            "heading": 0.18624088374392397,
-            "angularVelocity": 1.769822870452196,
-            "velocityX": 1.2452114109208137,
-            "velocityY": 0.3033473822772884,
-            "timestamp": 1.3109298471126005
+            "x": 2.271980215040446,
+            "y": 4.410826628196332,
+            "heading": -0.3872050871501322,
+            "angularVelocity": 0.033498492691473286,
+            "velocityX": -0.3358317815939762,
+            "velocityY": 0.6091837294305799,
+            "timestamp": 0.0834339883696639
         },
         {
-            "x": 5.176,
+            "x": 2.216375801038755,
+            "y": 4.512717243328352,
+            "heading": -0.38158865604258857,
+            "angularVelocity": 0.06731586511476574,
+            "velocityX": -0.6664479918611741,
+            "velocityY": 1.2212123275291613,
+            "timestamp": 0.1668679767393278
+        },
+        {
+            "x": 2.133913144213912,
+            "y": 4.66605742038127,
+            "heading": -0.3731070083031397,
+            "angularVelocity": 0.10165698541488709,
+            "velocityX": -0.9883580832768961,
+            "velocityY": 1.837862243543364,
+            "timestamp": 0.2503019651089917
+        },
+        {
+            "x": 2.026047479965626,
+            "y": 4.871580643769176,
+            "heading": -0.3616752437087536,
+            "angularVelocity": 0.13701567930528194,
+            "velocityX": -1.2928264171046289,
+            "velocityY": 2.4633033539909763,
+            "timestamp": 0.3337359534786556
+        },
+        {
+            "x": 1.8971303225339013,
+            "y": 5.131188087013384,
+            "heading": -0.34705934044989933,
+            "angularVelocity": 0.1751792470161148,
+            "velocityX": -1.5451395762230817,
+            "velocityY": 3.111531023711595,
+            "timestamp": 0.41716994184831946
+        },
+        {
+            "x": 1.812941926504828,
+            "y": 5.427772716701387,
+            "heading": -0.3288482106163032,
+            "angularVelocity": 0.21826991841509946,
+            "velocityX": -1.0090419704726812,
+            "velocityY": 3.5547219482539485,
+            "timestamp": 0.5006039302179833
+        },
+        {
+            "x": 1.7681664127359014,
+            "y": 5.682327706651028,
+            "heading": -0.2856043053433686,
+            "angularVelocity": 0.5183008282076023,
+            "velocityX": -0.5366579573130098,
+            "velocityY": 3.0509747277288763,
+            "timestamp": 0.5840379185876472
+        },
+        {
+            "x": 1.7580426073358177,
+            "y": 5.890413977811611,
+            "heading": -0.23166998700826372,
+            "angularVelocity": 0.6464310215597402,
+            "velocityX": -0.12133910409739124,
+            "velocityY": 2.4940228224314014,
+            "timestamp": 0.6674719069573111
+        },
+        {
+            "x": 1.7809381631803756,
+            "y": 6.0507944700387695,
+            "heading": -0.17161076742769205,
+            "angularVelocity": 0.7198411672948628,
+            "velocityX": 0.27441521485536685,
+            "velocityY": 1.9222441041228078,
+            "timestamp": 0.7509058953269749
+        },
+        {
+            "x": 1.8360347454575048,
+            "y": 6.162894796860879,
+            "heading": -0.10766394629623156,
+            "angularVelocity": 0.7664361057242964,
+            "velocityX": 0.6603613629651003,
+            "velocityY": 1.34358106345549,
+            "timestamp": 0.8343398836966388
+        },
+        {
+            "x": 1.9228414263613316,
+            "y": 6.226384193614056,
+            "heading": -0.041156065591054634,
+            "angularVelocity": 0.797131744563275,
+            "velocityX": 1.0404234844874343,
+            "velocityY": 0.760953635248516,
+            "timestamp": 0.9177738720663027
+        },
+        {
+            "x": 2.041031122207642,
+            "y": 6.2410478591918945,
+            "heading": 0.027035456766794728,
+            "angularVelocity": 0.8173110705883178,
+            "velocityX": 1.4165653369372246,
+            "velocityY": 0.17575170340583585,
+            "timestamp": 1.0012078604359667
+        },
+        {
+            "x": 2.201330972597037,
+            "y": 6.260936085330867,
+            "heading": 0.09277658873792281,
+            "angularVelocity": 0.8449644804697937,
+            "velocityX": 2.060318642882427,
+            "velocityY": 0.2556214680702958,
+            "timestamp": 1.0790112870770416
+        },
+        {
+            "x": 2.4117170311040814,
+            "y": 6.287038452225851,
+            "heading": 0.16066894258910833,
+            "angularVelocity": 0.8726139294965615,
+            "velocityX": 2.7040718846096095,
+            "velocityY": 0.33549122476729665,
+            "timestamp": 1.1568147137181166
+        },
+        {
+            "x": 2.672189282535082,
+            "y": 6.319354957991783,
+            "heading": 0.23071153953234388,
+            "angularVelocity": 0.9002507982373139,
+            "velocityX": 3.3478249310538675,
+            "velocityY": 0.4153609572357597,
+            "timestamp": 1.2346181403591916
+        },
+        {
+            "x": 2.9642755449736304,
+            "y": 6.355593779334123,
+            "heading": 0.23071154098757585,
+            "angularVelocity": 1.8703321803502023e-8,
+            "velocityX": 3.7541567903686675,
+            "velocityY": 0.4657741041344694,
+            "timestamp": 1.3124215670002666
+        },
+        {
+            "x": 3.256361807413129,
+            "y": 6.39183260067658,
+            "heading": 0.23071154244151956,
+            "angularVelocity": 1.868721301612308e-8,
+            "velocityX": 3.7541567903805326,
+            "velocityY": 0.46577410413594156,
+            "timestamp": 1.3902249936413416
+        },
+        {
+            "x": 3.5484480698526375,
+            "y": 6.4280714220190385,
+            "heading": 0.23071154389546672,
+            "angularVelocity": 1.868722261906901e-8,
+            "velocityX": 3.7541567903806516,
+            "velocityY": 0.4657741041359563,
+            "timestamp": 1.4680284202824165
+        },
+        {
+            "x": 3.8405343322921546,
+            "y": 6.464310243361498,
+            "heading": 0.23071154534942268,
+            "angularVelocity": 1.868730306107698e-8,
+            "velocityX": 3.754156790380741,
+            "velocityY": 0.46577410413596737,
+            "timestamp": 1.5458318469234915
+        },
+        {
+            "x": 4.132620594731676,
+            "y": 6.500549064703958,
+            "heading": 0.23071154680338032,
+            "angularVelocity": 1.86873022983191e-8,
+            "velocityX": 3.754156790380778,
+            "velocityY": 0.465774104135972,
+            "timestamp": 1.6236352735645665
+        },
+        {
+            "x": 4.424706857172369,
+            "y": 6.536787886046564,
+            "heading": 0.23071154826355325,
+            "angularVelocity": 1.876697436713731e-8,
+            "velocityX": 3.7541567903965425,
+            "velocityY": 0.46577410413792786,
+            "timestamp": 1.7014387002056415
+        },
+        {
+            "x": 4.6751378783325555,
+            "y": 6.567858587416207,
+            "heading": 0.24147534520321134,
+            "angularVelocity": 0.13834605240872366,
+            "velocityX": 3.2187659589270945,
+            "velocityY": 0.39934875250390567,
+            "timestamp": 1.7792421268467165
+        },
+        {
+            "x": 4.875482718759792,
+            "y": 6.592715151427428,
+            "heading": 0.2500855408103817,
+            "angularVelocity": 0.11066602050088868,
+            "velocityX": 2.575013069173925,
+            "velocityY": 0.31947903947593354,
+            "timestamp": 1.8570455534877914
+        },
+        {
+            "x": 5.025741356581141,
+            "y": 6.611357575366475,
+            "heading": 0.25654288837544836,
+            "angularVelocity": 0.08299567055584532,
+            "velocityX": 1.931259898289217,
+            "velocityY": 0.23960929156828065,
+            "timestamp": 1.9348489801288664
+        },
+        {
+            "x": 5.125913784825429,
+            "y": 6.623785858368443,
+            "heading": 0.2608476597362627,
+            "angularVelocity": 0.05532881451478021,
+            "velocityX": 1.2875066378045663,
+            "velocityY": 0.15973953254406137,
+            "timestamp": 2.0126524067699414
+        },
+        {
+            "x": 5.175999999999999,
             "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": 0.9368509404239479,
-            "velocityX": 0.6229472299739557,
-            "velocityY": 0.13050296182384175,
-            "timestamp": 1.392862962557138
+            "heading": 0.26300000000000007,
+            "angularVelocity": 0.02766382352150404,
+            "velocityX": 0.6437533324290226,
+            "velocityY": 0.07986976795027836,
+            "timestamp": 2.090455833411016
         },
         {
             "x": 5.176,
             "y": 6.63,
             "heading": 0.263,
-            "angularVelocity": 1.5668772442975024e-30,
-            "velocityX": 0,
-            "velocityY": -2.8584411322391187e-31,
-            "timestamp": 1.4747960780016756
+            "angularVelocity": -5.112167395133813e-18,
+            "velocityX": -1.2520036782163602e-16,
+            "velocityY": -1.3106574021367763e-17,
+            "timestamp": 2.168259260052091
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.1.traj
new file mode 100644
index 00000000..253677d3
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.1.traj	
@@ -0,0 +1,265 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": -7.3580792676211765e-19,
+            "velocityX": -1.2600069549143939e-17,
+            "velocityY": 7.063684033512061e-18,
+            "timestamp": 0
+        },
+        {
+            "x": 2.277488982590109,
+            "y": 4.417838766017227,
+            "heading": -0.3894665385190853,
+            "angularVelocity": 0.006183201184696677,
+            "velocityX": -0.26091883762393897,
+            "velocityY": 0.6703927825220056,
+            "timestamp": 0.08627593781609397
+        },
+        {
+            "x": 2.232568196785243,
+            "y": 4.5335556038331974,
+            "heading": -0.3884042841517919,
+            "angularVelocity": 0.012312290010209009,
+            "velocityX": -0.520664126544967,
+            "velocityY": 1.3412411472435377,
+            "timestamp": 0.17255187563218793
+        },
+        {
+            "x": 2.165406410226816,
+            "y": 4.707215578005854,
+            "heading": -0.38682100523451757,
+            "angularVelocity": 0.018351338245094856,
+            "velocityX": -0.7784532774547995,
+            "velocityY": 2.012843656858665,
+            "timestamp": 0.2588278134482819
+        },
+        {
+            "x": 2.0763412317308223,
+            "y": 4.9389471550535315,
+            "heading": -0.3847322299034305,
+            "angularVelocity": 0.024210404244363496,
+            "velocityX": -1.0323293000400813,
+            "velocityY": 2.685935185562845,
+            "timestamp": 0.34510375126437587
+        },
+        {
+            "x": 1.9663857850872422,
+            "y": 5.229122444622766,
+            "heading": -0.3821844673054777,
+            "angularVelocity": 0.029530395872206988,
+            "velocityX": -1.2744624912447062,
+            "velocityY": 3.3633397319630034,
+            "timestamp": 0.43137968908046986
+        },
+        {
+            "x": 1.9100206214147308,
+            "y": 5.550595198428695,
+            "heading": -0.3821844591902738,
+            "angularVelocity": 9.406103402067565e-8,
+            "velocityX": -0.653312674417546,
+            "velocityY": 3.7260997903167445,
+            "timestamp": 0.5176556268965639
+        },
+        {
+            "x": 1.8984249514650446,
+            "y": 5.829240348205722,
+            "heading": -0.3690339918971659,
+            "angularVelocity": 0.15242334799236965,
+            "velocityX": -0.13440213161644662,
+            "velocityY": 3.229697141872688,
+            "timestamp": 0.6039315647126579
+        },
+        {
+            "x": 1.916554046418155,
+            "y": 6.053409726810643,
+            "heading": -0.3524665993885735,
+            "angularVelocity": 0.1920279620015428,
+            "velocityX": 0.2101292134517386,
+            "velocityY": 2.5982838816862235,
+            "timestamp": 0.6902075025287518
+        },
+        {
+            "x": 1.9615881089279723,
+            "y": 6.221650822615583,
+            "heading": -0.33415152161256073,
+            "angularVelocity": 0.21228488776393065,
+            "velocityX": 0.5219770848021326,
+            "velocityY": 1.9500349699305692,
+            "timestamp": 0.7764834403448458
+        },
+        {
+            "x": 2.0323565347582764,
+            "y": 6.333414099671539,
+            "heading": -0.3147709502575115,
+            "angularVelocity": 0.22463472256153053,
+            "velocityX": 0.8202568134484147,
+            "velocityY": 1.2954165423758215,
+            "timestamp": 0.8627593781609398
+        },
+        {
+            "x": 2.1282194334415006,
+            "y": 6.388411890330483,
+            "heading": -0.2946952221482423,
+            "angularVelocity": 0.232692087938443,
+            "velocityX": 1.111119752619376,
+            "velocityY": 0.6374638404531465,
+            "timestamp": 0.9490353159770338
+        },
+        {
+            "x": 2.2487735748291016,
+            "y": 6.386467456817627,
+            "heading": -0.27415678824354056,
+            "angularVelocity": 0.23805517997945658,
+            "velocityX": 1.3973089651552055,
+            "velocityY": -0.022537379043057543,
+            "timestamp": 1.0353112537931277
+        },
+        {
+            "x": 2.4244121861833303,
+            "y": 6.383634558684574,
+            "heading": -0.2535069736087744,
+            "angularVelocity": 0.24641991696382376,
+            "velocityX": 2.0959438518486566,
+            "velocityY": -0.03380575250000802,
+            "timestamp": 1.1191105460305186
+        },
+        {
+            "x": 2.658595728295013,
+            "y": 6.37985738170666,
+            "heading": -0.2319955871365083,
+            "angularVelocity": 0.2567012906424869,
+            "velocityX": 2.79457661108014,
+            "velocityY": -0.04507409164284618,
+            "timestamp": 1.2029098382679095
+        },
+        {
+            "x": 2.9513225155845477,
+            "y": 6.37513595307082,
+            "heading": -0.20870362422898678,
+            "angularVelocity": 0.27794939892259674,
+            "velocityX": 3.493189255826692,
+            "velocityY": -0.05634210635652628,
+            "timestamp": 1.2867091305053004
+        },
+        {
+            "x": 3.1855040701620707,
+            "y": 6.371358808150105,
+            "heading": -0.1849223646442006,
+            "angularVelocity": 0.2837883107343998,
+            "velocityX": 2.794552893288424,
+            "velocityY": -0.04507370909548172,
+            "timestamp": 1.3705084227426914
+        },
+        {
+            "x": 3.3611402034786066,
+            "y": 6.368525949985646,
+            "heading": -0.1612618078271142,
+            "angularVelocity": 0.28234793141284903,
+            "velocityX": 2.0959142807433877,
+            "velocityY": -0.03380527554378375,
+            "timestamp": 1.4543077149800823
+        },
+        {
+            "x": 3.478230941686313,
+            "y": 6.366637378155631,
+            "heading": -0.13785841940389754,
+            "angularVelocity": 0.2792790702446555,
+            "velocityX": 1.3972759802792332,
+            "velocityY": -0.022536847025679124,
+            "timestamp": 1.5381070072174732
+        },
+        {
+            "x": 3.536776304244995,
+            "y": 6.365693092346191,
+            "heading": -0.11477234815509793,
+            "angularVelocity": 0.2754924371366116,
+            "velocityX": 0.6986379120342898,
+            "velocityY": -0.01126842225306853,
+            "timestamp": 1.621906299454864
+        },
+        {
+            "x": 3.536776304244995,
+            "y": 6.365693092346191,
+            "heading": -0.09203745526508908,
+            "angularVelocity": 0.27130172920323553,
+            "velocityX": -5.971595919811331e-16,
+            "velocityY": 2.294938009446505e-16,
+            "timestamp": 1.705705591692255
+        },
+        {
+            "x": 3.5821674028193073,
+            "y": 6.315334924760779,
+            "heading": -0.06814456679249249,
+            "angularVelocity": 0.26497230333398186,
+            "velocityX": 0.5033876064793726,
+            "velocityY": -0.558472437189569,
+            "timestamp": 1.7958768593000762
+        },
+        {
+            "x": 3.6729493357217975,
+            "y": 6.214618882752082,
+            "heading": -0.0452520324499122,
+            "angularVelocity": 0.2538783689072417,
+            "velocityX": 1.006772282467237,
+            "velocityY": -1.1169416232090381,
+            "timestamp": 1.8860481269078975
+        },
+        {
+            "x": 3.809120897123708,
+            "y": 6.063546304100605,
+            "heading": -0.024382898724538877,
+            "angularVelocity": 0.23143884165108508,
+            "velocityX": 1.5101435858055878,
+            "velocityY": -1.6753959732332042,
+            "timestamp": 1.9762193945157187
+        },
+        {
+            "x": 3.9906611673735113,
+            "y": 5.862140397658975,
+            "heading": -0.011123644415836652,
+            "angularVelocity": 0.14704522472038728,
+            "velocityX": 2.0132828900595015,
+            "velocityY": -2.2335929369164194,
+            "timestamp": 2.06639066212354
+        },
+        {
+            "x": 4.126829478795296,
+            "y": 5.711071424627299,
+            "heading": -0.0049044530235942295,
+            "angularVelocity": 0.06897087683516459,
+            "velocityX": 1.5101075435028497,
+            "velocityY": -1.6753559868839316,
+            "timestamp": 2.1565619297313616
+        },
+        {
+            "x": 4.217609649564756,
+            "y": 5.610357337578787,
+            "heading": -0.0015015365594071913,
+            "angularVelocity": 0.03773836782456074,
+            "velocityX": 1.0067527403993772,
+            "velocityY": -1.1169199426866636,
+            "timestamp": 2.246733197339183
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 4.052308831945937e-18,
+            "angularVelocity": 0.016652051138278397,
+            "velocityX": 0.5033793096117788,
+            "velocityY": -0.5584632324101773,
+            "timestamp": 2.3369044649470045
+        },
+        {
+            "x": 4.263,
+            "y": 5.56,
+            "heading": 1.897041849287005e-18,
+            "angularVelocity": -2.8636978124151836e-18,
+            "velocityX": 2.1901737652514486e-16,
+            "velocityY": -2.4329458809860463e-16,
+            "timestamp": 2.427075732554826
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.traj
index 72f3f88a..253677d3 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.traj	
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.traj	
@@ -4,127 +4,262 @@
             "x": 2.3,
             "y": 4.36,
             "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
+            "angularVelocity": -7.3580792676211765e-19,
+            "velocityX": -1.2600069549143939e-17,
+            "velocityY": 7.063684033512061e-18,
             "timestamp": 0
         },
         {
-            "x": 2.3223907681513127,
-            "y": 4.418261357040706,
-            "heading": -0.429219543622513,
-            "angularVelocity": -0.4499475285553182,
-            "velocityX": 0.2568788379871729,
-            "velocityY": 0.6684053711347925,
-            "timestamp": 0.08716470506781053
-        },
-        {
-            "x": 2.374909672838796,
-            "y": 4.531319644218887,
-            "heading": -0.5050759499486299,
-            "angularVelocity": -0.8702651637162607,
-            "velocityX": 0.6025248940683728,
-            "velocityY": 1.297064988520603,
-            "timestamp": 0.17432941013562106
-        },
-        {
-            "x": 2.469260718438912,
-            "y": 4.691180971030917,
-            "heading": -0.6116327562753044,
-            "angularVelocity": -1.2224765315706085,
-            "velocityX": 1.082445532589298,
-            "velocityY": 1.834014429208093,
-            "timestamp": 0.2614941152034316
-        },
-        {
-            "x": 2.620331804373909,
-            "y": 4.8788277526908095,
-            "heading": -0.7357228691689549,
-            "angularVelocity": -1.4236279787456398,
-            "velocityX": 1.7331680961631113,
-            "velocityY": 2.1527839911109723,
-            "timestamp": 0.3486588202712421
-        },
-        {
-            "x": 2.832381156144981,
-            "y": 5.083303561006889,
-            "heading": -0.84759450212177,
-            "angularVelocity": -1.2834510581523242,
-            "velocityX": 2.432743294503276,
-            "velocityY": 2.3458555634072913,
-            "timestamp": 0.43582352533905266
-        },
-        {
-            "x": 3.098611796220168,
-            "y": 5.267387874253087,
-            "heading": -0.8666590247194526,
-            "angularVelocity": -0.21871837440225492,
-            "velocityX": 3.0543399403241303,
-            "velocityY": 2.111913452847551,
-            "timestamp": 0.5229882304068632
-        },
-        {
-            "x": 3.3974595011437105,
-            "y": 5.397776588765608,
-            "heading": -0.8564937886773937,
-            "angularVelocity": 0.11662101115527058,
-            "velocityX": 3.42854030987716,
-            "velocityY": 1.4958888969002289,
-            "timestamp": 0.6101529354746736
-        },
-        {
-            "x": 3.678181484629676,
-            "y": 5.472509187071852,
-            "heading": -0.7581230365630254,
-            "angularVelocity": 1.1285617502845884,
-            "velocityX": 3.220592363245829,
-            "velocityY": 0.8573722385466142,
-            "timestamp": 0.6973176405424841
-        },
-        {
-            "x": 3.915033154173224,
-            "y": 5.514676302538828,
-            "heading": -0.5395209628894968,
-            "angularVelocity": 2.5079196161274817,
-            "velocityX": 2.7172887163363586,
-            "velocityY": 0.4837636453214852,
-            "timestamp": 0.7844823456102946
-        },
-        {
-            "x": 4.091189385673594,
-            "y": 5.53976294658506,
-            "heading": -0.29904398170320484,
-            "angularVelocity": 2.7588802256510867,
-            "velocityX": 2.0209582693284847,
-            "velocityY": 0.2878073645372324,
-            "timestamp": 0.8716470506781051
-        },
-        {
-            "x": 4.2064124532392215,
-            "y": 5.553849604220975,
-            "heading": -0.10702579424844161,
-            "angularVelocity": 2.2029350905894844,
-            "velocityX": 1.3219005040628353,
-            "velocityY": 0.161609651807531,
-            "timestamp": 0.9588117557459156
+            "x": 2.277488982590109,
+            "y": 4.417838766017227,
+            "heading": -0.3894665385190853,
+            "angularVelocity": 0.006183201184696677,
+            "velocityX": -0.26091883762393897,
+            "velocityY": 0.6703927825220056,
+            "timestamp": 0.08627593781609397
+        },
+        {
+            "x": 2.232568196785243,
+            "y": 4.5335556038331974,
+            "heading": -0.3884042841517919,
+            "angularVelocity": 0.012312290010209009,
+            "velocityX": -0.520664126544967,
+            "velocityY": 1.3412411472435377,
+            "timestamp": 0.17255187563218793
+        },
+        {
+            "x": 2.165406410226816,
+            "y": 4.707215578005854,
+            "heading": -0.38682100523451757,
+            "angularVelocity": 0.018351338245094856,
+            "velocityX": -0.7784532774547995,
+            "velocityY": 2.012843656858665,
+            "timestamp": 0.2588278134482819
+        },
+        {
+            "x": 2.0763412317308223,
+            "y": 4.9389471550535315,
+            "heading": -0.3847322299034305,
+            "angularVelocity": 0.024210404244363496,
+            "velocityX": -1.0323293000400813,
+            "velocityY": 2.685935185562845,
+            "timestamp": 0.34510375126437587
+        },
+        {
+            "x": 1.9663857850872422,
+            "y": 5.229122444622766,
+            "heading": -0.3821844673054777,
+            "angularVelocity": 0.029530395872206988,
+            "velocityX": -1.2744624912447062,
+            "velocityY": 3.3633397319630034,
+            "timestamp": 0.43137968908046986
+        },
+        {
+            "x": 1.9100206214147308,
+            "y": 5.550595198428695,
+            "heading": -0.3821844591902738,
+            "angularVelocity": 9.406103402067565e-8,
+            "velocityX": -0.653312674417546,
+            "velocityY": 3.7260997903167445,
+            "timestamp": 0.5176556268965639
+        },
+        {
+            "x": 1.8984249514650446,
+            "y": 5.829240348205722,
+            "heading": -0.3690339918971659,
+            "angularVelocity": 0.15242334799236965,
+            "velocityX": -0.13440213161644662,
+            "velocityY": 3.229697141872688,
+            "timestamp": 0.6039315647126579
+        },
+        {
+            "x": 1.916554046418155,
+            "y": 6.053409726810643,
+            "heading": -0.3524665993885735,
+            "angularVelocity": 0.1920279620015428,
+            "velocityX": 0.2101292134517386,
+            "velocityY": 2.5982838816862235,
+            "timestamp": 0.6902075025287518
+        },
+        {
+            "x": 1.9615881089279723,
+            "y": 6.221650822615583,
+            "heading": -0.33415152161256073,
+            "angularVelocity": 0.21228488776393065,
+            "velocityX": 0.5219770848021326,
+            "velocityY": 1.9500349699305692,
+            "timestamp": 0.7764834403448458
+        },
+        {
+            "x": 2.0323565347582764,
+            "y": 6.333414099671539,
+            "heading": -0.3147709502575115,
+            "angularVelocity": 0.22463472256153053,
+            "velocityX": 0.8202568134484147,
+            "velocityY": 1.2954165423758215,
+            "timestamp": 0.8627593781609398
+        },
+        {
+            "x": 2.1282194334415006,
+            "y": 6.388411890330483,
+            "heading": -0.2946952221482423,
+            "angularVelocity": 0.232692087938443,
+            "velocityX": 1.111119752619376,
+            "velocityY": 0.6374638404531465,
+            "timestamp": 0.9490353159770338
+        },
+        {
+            "x": 2.2487735748291016,
+            "y": 6.386467456817627,
+            "heading": -0.27415678824354056,
+            "angularVelocity": 0.23805517997945658,
+            "velocityX": 1.3973089651552055,
+            "velocityY": -0.022537379043057543,
+            "timestamp": 1.0353112537931277
+        },
+        {
+            "x": 2.4244121861833303,
+            "y": 6.383634558684574,
+            "heading": -0.2535069736087744,
+            "angularVelocity": 0.24641991696382376,
+            "velocityX": 2.0959438518486566,
+            "velocityY": -0.03380575250000802,
+            "timestamp": 1.1191105460305186
+        },
+        {
+            "x": 2.658595728295013,
+            "y": 6.37985738170666,
+            "heading": -0.2319955871365083,
+            "angularVelocity": 0.2567012906424869,
+            "velocityX": 2.79457661108014,
+            "velocityY": -0.04507409164284618,
+            "timestamp": 1.2029098382679095
+        },
+        {
+            "x": 2.9513225155845477,
+            "y": 6.37513595307082,
+            "heading": -0.20870362422898678,
+            "angularVelocity": 0.27794939892259674,
+            "velocityX": 3.493189255826692,
+            "velocityY": -0.05634210635652628,
+            "timestamp": 1.2867091305053004
+        },
+        {
+            "x": 3.1855040701620707,
+            "y": 6.371358808150105,
+            "heading": -0.1849223646442006,
+            "angularVelocity": 0.2837883107343998,
+            "velocityX": 2.794552893288424,
+            "velocityY": -0.04507370909548172,
+            "timestamp": 1.3705084227426914
+        },
+        {
+            "x": 3.3611402034786066,
+            "y": 6.368525949985646,
+            "heading": -0.1612618078271142,
+            "angularVelocity": 0.28234793141284903,
+            "velocityX": 2.0959142807433877,
+            "velocityY": -0.03380527554378375,
+            "timestamp": 1.4543077149800823
+        },
+        {
+            "x": 3.478230941686313,
+            "y": 6.366637378155631,
+            "heading": -0.13785841940389754,
+            "angularVelocity": 0.2792790702446555,
+            "velocityX": 1.3972759802792332,
+            "velocityY": -0.022536847025679124,
+            "timestamp": 1.5381070072174732
+        },
+        {
+            "x": 3.536776304244995,
+            "y": 6.365693092346191,
+            "heading": -0.11477234815509793,
+            "angularVelocity": 0.2754924371366116,
+            "velocityX": 0.6986379120342898,
+            "velocityY": -0.01126842225306853,
+            "timestamp": 1.621906299454864
+        },
+        {
+            "x": 3.536776304244995,
+            "y": 6.365693092346191,
+            "heading": -0.09203745526508908,
+            "angularVelocity": 0.27130172920323553,
+            "velocityX": -5.971595919811331e-16,
+            "velocityY": 2.294938009446505e-16,
+            "timestamp": 1.705705591692255
+        },
+        {
+            "x": 3.5821674028193073,
+            "y": 6.315334924760779,
+            "heading": -0.06814456679249249,
+            "angularVelocity": 0.26497230333398186,
+            "velocityX": 0.5033876064793726,
+            "velocityY": -0.558472437189569,
+            "timestamp": 1.7958768593000762
+        },
+        {
+            "x": 3.6729493357217975,
+            "y": 6.214618882752082,
+            "heading": -0.0452520324499122,
+            "angularVelocity": 0.2538783689072417,
+            "velocityX": 1.006772282467237,
+            "velocityY": -1.1169416232090381,
+            "timestamp": 1.8860481269078975
+        },
+        {
+            "x": 3.809120897123708,
+            "y": 6.063546304100605,
+            "heading": -0.024382898724538877,
+            "angularVelocity": 0.23143884165108508,
+            "velocityX": 1.5101435858055878,
+            "velocityY": -1.6753959732332042,
+            "timestamp": 1.9762193945157187
+        },
+        {
+            "x": 3.9906611673735113,
+            "y": 5.862140397658975,
+            "heading": -0.011123644415836652,
+            "angularVelocity": 0.14704522472038728,
+            "velocityX": 2.0132828900595015,
+            "velocityY": -2.2335929369164194,
+            "timestamp": 2.06639066212354
+        },
+        {
+            "x": 4.126829478795296,
+            "y": 5.711071424627299,
+            "heading": -0.0049044530235942295,
+            "angularVelocity": 0.06897087683516459,
+            "velocityX": 1.5101075435028497,
+            "velocityY": -1.6753559868839316,
+            "timestamp": 2.1565619297313616
+        },
+        {
+            "x": 4.217609649564756,
+            "y": 5.610357337578787,
+            "heading": -0.0015015365594071913,
+            "angularVelocity": 0.03773836782456074,
+            "velocityX": 1.0067527403993772,
+            "velocityY": -1.1169199426866636,
+            "timestamp": 2.246733197339183
         },
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 2.9287791201697906e-29,
-            "angularVelocity": 1.2278570112200775,
-            "velocityX": 0.6492025266047246,
-            "velocityY": 0.0705606216901654,
-            "timestamp": 1.045976460813726
+            "heading": 4.052308831945937e-18,
+            "angularVelocity": 0.016652051138278397,
+            "velocityX": 0.5033793096117788,
+            "velocityY": -0.5584632324101773,
+            "timestamp": 2.3369044649470045
         },
         {
             "x": 4.263,
             "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 3.0874616220611523e-31,
-            "velocityY": -3.4071713040637054e-30,
-            "timestamp": 1.1331411658815367
+            "heading": 1.897041849287005e-18,
+            "angularVelocity": -2.8636978124151836e-18,
+            "velocityX": 2.1901737652514486e-16,
+            "velocityY": -2.4329458809860463e-16,
+            "timestamp": 2.427075732554826
         }
     ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.1.traj
new file mode 100644
index 00000000..4ff6b6c8
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.1.traj	
@@ -0,0 +1,229 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.284359298751307,
+            "y": 4.3328670624584875,
+            "heading": -0.3913603360985968,
+            "angularVelocity": -0.02219557362709146,
+            "velocityX": -0.2551974739939649,
+            "velocityY": -0.44270758788444614,
+            "timestamp": 0.061288620940906494
+        },
+        {
+            "x": 2.2541133789559487,
+            "y": 4.2780305351291235,
+            "heading": -0.3941996942145126,
+            "angularVelocity": -0.04632765548197678,
+            "velocityX": -0.4934997611468709,
+            "velocityY": -0.89472607618691,
+            "timestamp": 0.12257724188181299
+        },
+        {
+            "x": 2.2108693669713233,
+            "y": 4.194701771021148,
+            "heading": -0.39869373638946143,
+            "angularVelocity": -0.0733258818024626,
+            "velocityX": -0.7055797849705345,
+            "velocityY": -1.3596123200148318,
+            "timestamp": 0.1838658628227195
+        },
+        {
+            "x": 2.157413012714945,
+            "y": 4.081769184110331,
+            "heading": -0.40512443208979565,
+            "angularVelocity": -0.1049247903054391,
+            "velocityX": -0.8722068376757797,
+            "velocityY": -1.8426354709417345,
+            "timestamp": 0.24515448376362597
+        },
+        {
+            "x": 2.099439422754663,
+            "y": 3.9378510886177804,
+            "heading": -0.41398926531008917,
+            "angularVelocity": -0.1446407682894496,
+            "velocityX": -0.9459111507204496,
+            "velocityY": -2.348202542056109,
+            "timestamp": 0.30644310470453245
+        },
+        {
+            "x": 2.05084134036617,
+            "y": 3.764062564560248,
+            "heading": -0.42610260644735526,
+            "angularVelocity": -0.1976442111325286,
+            "velocityX": -0.7929380958881483,
+            "velocityY": -2.835575697242347,
+            "timestamp": 0.36773172564543893
+        },
+        {
+            "x": 2.0313297442556637,
+            "y": 3.578686605657539,
+            "heading": -0.44115755081157715,
+            "angularVelocity": -0.24564012263773397,
+            "velocityX": -0.3183559331400068,
+            "velocityY": -3.024639093795978,
+            "timestamp": 0.4290203465863454
+        },
+        {
+            "x": 2.0424292488902682,
+            "y": 3.3999266361315956,
+            "heading": -0.45764648760363913,
+            "angularVelocity": -0.2690374907923018,
+            "velocityX": 0.18110220892890863,
+            "velocityY": -2.9166910069375023,
+            "timestamp": 0.4903089675272519
+        },
+        {
+            "x": 2.0816987250314285,
+            "y": 3.234855796713624,
+            "heading": -0.4747853916237668,
+            "angularVelocity": -0.27964251368378285,
+            "velocityX": 0.6407302944379066,
+            "velocityY": -2.6933358408754287,
+            "timestamp": 0.5515975884681584
+        },
+        {
+            "x": 2.1473776050382467,
+            "y": 3.086622748034336,
+            "heading": -0.49218081575104855,
+            "angularVelocity": -0.2838279579508591,
+            "velocityX": 1.071632531431,
+            "velocityY": -2.418606364502998,
+            "timestamp": 0.6128862094090649
+        },
+        {
+            "x": 2.2382969856262207,
+            "y": 2.9569339752197266,
+            "heading": -0.5096053711064815,
+            "angularVelocity": -0.2843032701328595,
+            "velocityX": 1.4834626590087072,
+            "velocityY": -2.116033463041253,
+            "timestamp": 0.6741748303499714
+        },
+        {
+            "x": 2.3750291535583634,
+            "y": 2.833789120602945,
+            "heading": -0.5294729435704262,
+            "angularVelocity": -0.282105779182567,
+            "velocityX": 1.9415021560293924,
+            "velocityY": -1.7485717103605045,
+            "timestamp": 0.7446007979480589
+        },
+        {
+            "x": 2.5431654959507464,
+            "y": 2.737552434720476,
+            "heading": -0.5488979650801319,
+            "angularVelocity": -0.2758218619098283,
+            "velocityX": 2.3874196993915153,
+            "velocityY": -1.366494336743516,
+            "timestamp": 0.8150267655461465
+        },
+        {
+            "x": 2.7413669689925393,
+            "y": 2.669711436345154,
+            "heading": -0.5674506234763109,
+            "angularVelocity": -0.2634349094364851,
+            "velocityX": 2.8143237473555907,
+            "velocityY": -0.9632952260234809,
+            "timestamp": 0.8854527331442341
+        },
+        {
+            "x": 2.9672720655352354,
+            "y": 2.6325722400915463,
+            "heading": -0.5844267226661307,
+            "angularVelocity": -0.24104885979984425,
+            "velocityX": 3.207696028145596,
+            "velocityY": -0.5273508837756647,
+            "timestamp": 0.9558787007423217
+        },
+        {
+            "x": 3.21586763898004,
+            "y": 2.629998478445332,
+            "heading": -0.5985057428976699,
+            "angularVelocity": -0.19991234358165255,
+            "velocityX": 3.529885096694866,
+            "velocityY": -0.03654563414595668,
+            "timestamp": 1.0263046683404093
+        },
+        {
+            "x": 3.4730987079905056,
+            "y": 2.6678365706373897,
+            "heading": -0.6068355688677516,
+            "angularVelocity": -0.11827776392962061,
+            "velocityX": 3.652503157336117,
+            "velocityY": 0.5372747224148254,
+            "timestamp": 1.096730635938497
+        },
+        {
+            "x": 3.704150591962856,
+            "y": 2.737591563426048,
+            "heading": -0.6062385008661115,
+            "angularVelocity": 0.00847795240879764,
+            "velocityX": 3.28077684769537,
+            "velocityY": 0.9904726220694814,
+            "timestamp": 1.1671566035365846
+        },
+        {
+            "x": 3.894448488053554,
+            "y": 2.8141322684658854,
+            "heading": -0.5991077413907381,
+            "angularVelocity": 0.10125184954600595,
+            "velocityX": 2.702098424500253,
+            "velocityY": 1.086825039829707,
+            "timestamp": 1.2375825711346722
+        },
+        {
+            "x": 4.043960756386648,
+            "y": 2.883897596305393,
+            "heading": -0.5885679211534333,
+            "angularVelocity": 0.1496581530473843,
+            "velocityX": 2.122970736963697,
+            "velocityY": 0.9906193726389452,
+            "timestamp": 1.3080085387327598
+        },
+        {
+            "x": 4.154363809487208,
+            "y": 2.940218619761556,
+            "heading": -0.5783208756256007,
+            "angularVelocity": 0.14550095479427785,
+            "velocityX": 1.5676469470837475,
+            "velocityY": 0.7997195548321091,
+            "timestamp": 1.3784345063308474
+        },
+        {
+            "x": 4.227038806897989,
+            "y": 2.9796144762337837,
+            "heading": -0.5703846528919979,
+            "angularVelocity": 0.11268887037369305,
+            "velocityX": 1.0319346668480025,
+            "velocityY": 0.5593938971070241,
+            "timestamp": 1.448860473928935
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 0.06225903656760019,
+            "velocityX": 0.5106240542868632,
+            "velocityY": 0.2894603292148455,
+            "timestamp": 1.5192864415270226
+        },
+        {
+            "x": 4.263,
+            "y": 3,
+            "heading": -0.566,
+            "angularVelocity": 9.24329495349137e-32,
+            "velocityX": 0,
+            "velocityY": 1.2740379664097394e-33,
+            "timestamp": 1.5897124091251102
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.1.traj
new file mode 100644
index 00000000..2b0725c8
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.1.traj	
@@ -0,0 +1,256 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 0,
+            "velocityX": 0,
+            "velocityY": 9.57151893871228e-33,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2927892107770984,
+            "y": 4.332309394440523,
+            "heading": -0.3722824674321399,
+            "angularVelocity": 0.3002660031020358,
+            "velocityX": -0.12220408518393268,
+            "velocityY": -0.4692835993371687,
+            "timestamp": 0.059006122520770025
+        },
+        {
+            "x": 2.2795380946752672,
+            "y": 4.276530177721837,
+            "heading": -0.3395678297253276,
+            "angularVelocity": 0.5544278510301563,
+            "velocityX": -0.224571883996739,
+            "velocityY": -0.9453123563415277,
+            "timestamp": 0.11801224504154005
+        },
+        {
+            "x": 2.2618545093830753,
+            "y": 4.192237257969867,
+            "heading": -0.2954402922678697,
+            "angularVelocity": 0.7478467584770477,
+            "velocityX": -0.29969068524994186,
+            "velocityY": -1.4285453127732544,
+            "timestamp": 0.17701836756231007
+        },
+        {
+            "x": 2.2420434066604593,
+            "y": 4.079048226765037,
+            "heading": -0.24482967906698166,
+            "angularVelocity": 0.8577179966887506,
+            "velocityX": -0.33574656114103973,
+            "velocityY": -1.9182590953165608,
+            "timestamp": 0.2360244900830801
+        },
+        {
+            "x": 2.2234541825747725,
+            "y": 3.936854095556344,
+            "heading": -0.19463836205748358,
+            "angularVelocity": 0.8506120189787272,
+            "velocityX": -0.3150389026010499,
+            "velocityY": -2.40981994976269,
+            "timestamp": 0.2950306126038501
+        },
+        {
+            "x": 2.211142623620853,
+            "y": 3.7664761504723194,
+            "heading": -0.15521354512815658,
+            "angularVelocity": 0.6681479013546628,
+            "velocityX": -0.2086488389333828,
+            "velocityY": -2.8874621446961175,
+            "timestamp": 0.35403673512462014
+        },
+        {
+            "x": 2.2125400151692816,
+            "y": 3.5715383450180926,
+            "heading": -0.14131017152189063,
+            "angularVelocity": 0.23562594883900537,
+            "velocityX": 0.023682144983125607,
+            "velocityY": -3.303687772156347,
+            "timestamp": 0.41304285764539017
+        },
+        {
+            "x": 2.235363094184648,
+            "y": 3.3576277458759463,
+            "heading": -0.144791073330025,
+            "angularVelocity": -0.058992214018278255,
+            "velocityX": 0.38679170974728244,
+            "velocityY": -3.6252271798888405,
+            "timestamp": 0.4720489801661602
+        },
+        {
+            "x": 2.287018648077521,
+            "y": 3.1404706845748738,
+            "heading": -0.14479216409186968,
+            "angularVelocity": -0.000018485570624385508,
+            "velocityX": 0.8754270181825738,
+            "velocityY": -3.6802462528296687,
+            "timestamp": 0.5310551026869302
+        },
+        {
+            "x": 2.3664206535422005,
+            "y": 2.9318538879811458,
+            "heading": -0.14479229465212481,
+            "angularVelocity": -0.000002212656069584398,
+            "velocityX": 1.3456570618876043,
+            "velocityY": -3.53551102295012,
+            "timestamp": 0.5900612252077002
+        },
+        {
+            "x": 2.4722259044647217,
+            "y": 2.73530650138855,
+            "heading": -0.14479252387880184,
+            "angularVelocity": -0.000003884794784189704,
+            "velocityX": 1.7931232625102267,
+            "velocityY": -3.330965977698533,
+            "timestamp": 0.6490673477284702
+        },
+        {
+            "x": 2.6222628757424284,
+            "y": 2.533912575740545,
+            "heading": -0.14479267276909355,
+            "angularVelocity": -0.0000022427572708026126,
+            "velocityX": 2.260029880862475,
+            "velocityY": -3.033627551346843,
+            "timestamp": 0.7154545103709612
+        },
+        {
+            "x": 2.800083925097921,
+            "y": 2.356570260194387,
+            "heading": -0.14479275333723068,
+            "angularVelocity": -0.0000012136101911737535,
+            "velocityX": 2.678545704884188,
+            "velocityY": -2.671334464182248,
+            "timestamp": 0.7818416730134522
+        },
+        {
+            "x": 3.001881619185789,
+            "y": 2.207076716019015,
+            "heading": -0.14479280967120484,
+            "angularVelocity": -8.485672817028763e-7,
+            "velocityX": 3.0397095772053357,
+            "velocityY": -2.2518441551784094,
+            "timestamp": 0.8482288356559432
+        },
+        {
+            "x": 3.2233351494929763,
+            "y": 2.0886327789398735,
+            "heading": -0.14479285532879432,
+            "angularVelocity": -6.877472642883405e-7,
+            "velocityX": 3.3357884490373766,
+            "velocityY": -1.7841391673415428,
+            "timestamp": 0.9146159982984342
+        },
+        {
+            "x": 3.4597028157497376,
+            "y": 2.003774307484127,
+            "heading": -0.14479289641721121,
+            "angularVelocity": -6.189211176196774e-7,
+            "velocityX": 3.5604423633775646,
+            "velocityY": -1.2782361540698368,
+            "timestamp": 0.9810031609409252
+        },
+        {
+            "x": 3.704835569204283,
+            "y": 1.949179734375487,
+            "heading": -0.14479293648657837,
+            "angularVelocity": -6.035710154638316e-7,
+            "velocityX": 3.692472214464675,
+            "velocityY": -0.8223664174750668,
+            "timestamp": 1.047390323583416
+        },
+        {
+            "x": 3.9499686718209266,
+            "y": 1.8945867290481686,
+            "heading": -0.1447929765559331,
+            "angularVelocity": -6.035708281824289e-7,
+            "velocityX": 3.692477473946853,
+            "velocityY": -0.8223428017448724,
+            "timestamp": 1.113777486225907
+        },
+        {
+            "x": 4.195101784677066,
+            "y": 1.8399937696982192,
+            "heading": -0.14479301662532418,
+            "angularVelocity": -6.035713756667977e-7,
+            "velocityX": 3.692477628185933,
+            "velocityY": -0.8223421091807183,
+            "timestamp": 1.1801646488683981
+        },
+        {
+            "x": 4.435904668374261,
+            "y": 1.7860806487644325,
+            "heading": -0.15898872227240893,
+            "angularVelocity": -0.21383208864538455,
+            "velocityX": 3.627250723064772,
+            "velocityY": -0.8121015989811171,
+            "timestamp": 1.2465518115108891
+        },
+        {
+            "x": 4.648245693564204,
+            "y": 1.7385415440023928,
+            "heading": -0.26462575063007965,
+            "angularVelocity": -1.5912267395211415,
+            "velocityX": 3.1985253886123286,
+            "velocityY": -0.7160888170209653,
+            "timestamp": 1.3129389741533801
+        },
+        {
+            "x": 4.824757883271093,
+            "y": 1.6989793937314068,
+            "heading": -0.37654836939407044,
+            "angularVelocity": -1.6859075506317058,
+            "velocityX": 2.6588301515075172,
+            "velocityY": -0.5959307296206723,
+            "timestamp": 1.3793261367958711
+        },
+        {
+            "x": 4.96555652453039,
+            "y": 1.6673723665346096,
+            "heading": -0.4762340168485147,
+            "angularVelocity": -1.501580177349536,
+            "velocityX": 2.1208715006774397,
+            "velocityY": -0.47610149219673153,
+            "timestamp": 1.4457132994383621
+        },
+        {
+            "x": 5.070906714490224,
+            "y": 1.643682366519874,
+            "heading": -0.5558849180153355,
+            "angularVelocity": -1.1997937251175768,
+            "velocityX": 1.5869060487969315,
+            "velocityY": -0.3568460990313944,
+            "timestamp": 1.5121004620808531
+        },
+        {
+            "x": 5.141006607602348,
+            "y": 1.6278935430132826,
+            "heading": -0.6113350442973207,
+            "angularVelocity": -0.8352537459779052,
+            "velocityX": 1.055925427776252,
+            "velocityY": -0.23782946699526453,
+            "timestamp": 1.5784876247233441
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": -0.4317846186174008,
+            "velocityX": 0.5271108299370921,
+            "velocityY": -0.11890164753374041,
+            "timestamp": 1.6448747873658351
+        },
+        {
+            "x": 5.176,
+            "y": 1.62,
+            "heading": -0.64,
+            "angularVelocity": 1.0662498702136072e-32,
+            "velocityX": 0,
+            "velocityY": 8.479039452208929e-33,
+            "timestamp": 1.7112619500083261
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.1.traj
new file mode 100644
index 00000000..087304c1
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.1.traj	
@@ -0,0 +1,148 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 1.4326217036715317e-32,
+            "velocityX": -1.74824017803789e-33,
+            "velocityY": 0,
+            "timestamp": 0
+        },
+        {
+            "x": 2.3001377838932537,
+            "y": 4.403828106793783,
+            "heading": -0.3629126711329447,
+            "angularVelocity": 0.37089781855009624,
+            "velocityX": 0.0018866291944089578,
+            "velocityY": 0.600123743495633,
+            "timestamp": 0.07303178264284323
+        },
+        {
+            "x": 2.300408122825978,
+            "y": 4.49157282256222,
+            "heading": -0.3106389398567459,
+            "angularVelocity": 0.7157668810008344,
+            "velocityX": 0.0037016614265912673,
+            "velocityY": 1.2014593180279707,
+            "timestamp": 0.14606356528568645
+        },
+        {
+            "x": 2.3008081314200264,
+            "y": 4.623360732427042,
+            "heading": -0.2362186265841639,
+            "angularVelocity": 1.019012689811082,
+            "velocityX": 0.005477185131915077,
+            "velocityY": 1.8045281806870437,
+            "timestamp": 0.2190953479285297
+        },
+        {
+            "x": 2.301347271135313,
+            "y": 4.79937800153835,
+            "heading": -0.14515320669009546,
+            "angularVelocity": 1.2469286192754925,
+            "velocityX": 0.007382261472700921,
+            "velocityY": 2.410146141058999,
+            "timestamp": 0.2921271305713729
+        },
+        {
+            "x": 2.302070167703843,
+            "y": 5.019849104537176,
+            "heading": -0.04952394454697364,
+            "angularVelocity": 1.3094197989222025,
+            "velocityX": 0.009898383174695338,
+            "velocityY": 3.018837758308937,
+            "timestamp": 0.3651589132142161
+        },
+        {
+            "x": 2.3031513688730865,
+            "y": 5.282690253403799,
+            "heading": -0.004114907781753178,
+            "angularVelocity": 0.6217708937393756,
+            "velocityX": 0.014804529344862738,
+            "velocityY": 3.598996756686451,
+            "timestamp": 0.43819069585705933
+        },
+        {
+            "x": 2.303520931398292,
+            "y": 5.5589648879428,
+            "heading": -0.0041148785146688655,
+            "angularVelocity": 4.00744487575221e-7,
+            "velocityX": 0.005060297199824665,
+            "velocityY": 3.7829370246937892,
+            "timestamp": 0.5112224784999025
+        },
+        {
+            "x": 2.3038904865521834,
+            "y": 5.835239522491655,
+            "heading": -0.004114849247579104,
+            "angularVelocity": 4.0074456218901027e-7,
+            "velocityX": 0.005060196266858216,
+            "velocityY": 3.7829370248287195,
+            "timestamp": 0.5842542611427458
+        },
+        {
+            "x": 2.302005656251481,
+            "y": 6.097682306820458,
+            "heading": 0.0412177605138735,
+            "angularVelocity": 0.6207244041015477,
+            "velocityX": -0.025808356752290516,
+            "velocityY": 3.5935420830717044,
+            "timestamp": 0.6572860437855891
+        },
+        {
+            "x": 2.3011707390613565,
+            "y": 6.318019040898657,
+            "heading": 0.13972629361592653,
+            "angularVelocity": 1.3488447020908976,
+            "velocityX": -0.01143224442716972,
+            "velocityY": 3.016997889203128,
+            "timestamp": 0.7303178264284323
+        },
+        {
+            "x": 2.3006512841511224,
+            "y": 6.493901741171369,
+            "heading": 0.23378956116370217,
+            "angularVelocity": 1.2879771538343168,
+            "velocityX": -0.007112723959845375,
+            "velocityY": 2.408303534542668,
+            "timestamp": 0.8033496090712756
+        },
+        {
+            "x": 2.300310658917655,
+            "y": 6.625570031648912,
+            "heading": 0.310734904704345,
+            "angularVelocity": 1.053587092580481,
+            "velocityX": -0.00466406845267762,
+            "velocityY": 1.802890272053981,
+            "timestamp": 0.8763813917141189
+        },
+        {
+            "x": 2.3001003670918414,
+            "y": 6.7132235530415985,
+            "heading": 0.3648714550351714,
+            "angularVelocity": 0.7412738450542471,
+            "velocityX": -0.0028794562888088465,
+            "velocityY": 1.2002106236588805,
+            "timestamp": 0.9494131743569622
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 0.38515484556072405,
+            "velocityX": -0.0013742933310585549,
+            "velocityY": 0.5994163824877027,
+            "timestamp": 1.0224449569998053
+        },
+        {
+            "x": 2.3,
+            "y": 6.757,
+            "heading": 0.393,
+            "angularVelocity": 2.8585488767662063e-32,
+            "velocityX": -1.0732467562704116e-31,
+            "velocityY": 0,
+            "timestamp": 1.0954767396426486
+        }
+    ]
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj
new file mode 100644
index 00000000..4345e451
--- /dev/null
+++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj	
@@ -0,0 +1,103 @@
+{
+    "samples": [
+        {
+            "x": 2.3,
+            "y": 4.36,
+            "heading": -0.39,
+            "angularVelocity": 0,
+            "velocityX": 2.0150450292109907e-34,
+            "velocityY": 3.039389955204995e-35,
+            "timestamp": 0
+        },
+        {
+            "x": 2.2980012520823867,
+            "y": 4.4080014432651184,
+            "heading": -0.3744767713085023,
+            "angularVelocity": 0.2040875990087458,
+            "velocityX": -0.02627801674742958,
+            "velocityY": 0.6310864511258738,
+            "timestamp": 0.07606159691668693
+        },
+        {
+            "x": 2.294003780157748,
+            "y": 4.504004078538053,
+            "heading": -0.34341554427294535,
+            "angularVelocity": 0.40836937817095076,
+            "velocityX": -0.05255571913664827,
+            "velocityY": 1.262169598911913,
+            "timestamp": 0.15212319383337386
+        },
+        {
+            "x": 2.2880075055108158,
+            "y": 4.648007445856594,
+            "heading": -0.296791131111893,
+            "angularVelocity": 0.6129823071177671,
+            "velocityX": -0.07883445641432706,
+            "velocityY": 1.8932466994648223,
+            "timestamp": 0.2281847907500608
+        },
+        {
+            "x": 2.2800121106149036,
+            "y": 4.840010896492559,
+            "heading": -0.2345694561462187,
+            "angularVelocity": 0.8180432371651087,
+            "velocityX": -0.10511736829125365,
+            "velocityY": 2.5243152710332057,
+            "timestamp": 0.3042463876667477
+        },
+        {
+            "x": 2.270016860554186,
+            "y": 5.080013636963748,
+            "heading": -0.15671112019223,
+            "angularVelocity": 1.023622157700289,
+            "velocityX": -0.13140994228224737,
+            "velocityY": 3.1553734105013973,
+            "timestamp": 0.3803079845834346
+        },
+        {
+            "x": 2.2620106394348882,
+            "y": 5.2720083260038075,
+            "heading": -0.09403333701953483,
+            "angularVelocity": 0.82403980081234,
+            "velocityX": -0.10525970323851294,
+            "velocityY": 2.524200080237073,
+            "timestamp": 0.4563695815001215
+        },
+        {
+            "x": 2.256005452365101,
+            "y": 5.416004188429854,
+            "heading": -0.0470178538159679,
+            "angularVelocity": 0.6181237984664553,
+            "velocityX": -0.07895163016843816,
+            "velocityY": 1.8931480308488688,
+            "timestamp": 0.5324311784168084
+        },
+        {
+            "x": 2.2520018201736485,
+            "y": 5.5120013950535185,
+            "heading": -0.01567260522346991,
+            "angularVelocity": 0.4121034774859067,
+            "velocityX": -0.05263670963729669,
+            "velocityY": 1.262098227162051,
+            "timestamp": 0.6084927753334953
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 1.3656663814986182e-35,
+            "angularVelocity": 0.20605148798856665,
+            "velocityX": -0.026318408432062,
+            "velocityY": 0.6310491350721452,
+            "timestamp": 0.6845543722501822
+        },
+        {
+            "x": 2.25,
+            "y": 5.56,
+            "heading": 0,
+            "angularVelocity": 6.6551444751621536e-34,
+            "velocityX": 3.9479513180483775e-34,
+            "velocityY": 0,
+            "timestamp": 0.7606159691668691
+        }
+    ]
+}
\ No newline at end of file

From b82eeee265e5efffeb85fde989e3f3898de958ec Mon Sep 17 00:00:00 2001
From: Stoopipoopy <anotherguywithabeanie@gmail.com>
Date: Sun, 25 Feb 2024 12:17:09 -0800
Subject: [PATCH 59/62] added getting alliance spicific pose for notes and
 shooting location

---
 ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 018 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 src/main/java/frc/robot/Robot.java            |  16 +++++----
 .../robot/utils/dynamicauto/DynamicAuto.java  |  32 +++++++-----------
 .../frc/robot/utils/dynamicauto/Note.java     |  17 ++++++++++
 .../utils/dynamicauto/ShootingLocation.java   |  21 ++++++++++++
 8 files changed, 60 insertions(+), 26 deletions(-)

diff --git a/ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat
index 8356e56c9c25dd412ef81784cc6311b6de7e4a67..9acaac4b6e9b274a80753012e6029c0e09688710 100644
GIT binary patch
delta 65
zcmZn=Xb@nz%`l~EqG0Mo1>T8eUMzPQCL?%U#~Ic>?Ga~SU|^akXvTGdVb#+<D9?N|
IBh!CY0L?oTq5uE@

delta 65
zcmZn=Xb@nz%P^^GqG0Mo1>T8eUM%+*CL(xTM;U})XoxZ}FfdING~+tLAofBH$}`{0
I$n>8T0HFgCqyPW_

diff --git a/ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat
index 54190244d90ba175cefe5fdc9f75e5b4f1d72b4a..256fc7184cda9417e5aa3c286db55bf1c2488f36 100644
GIT binary patch
delta 49
xcmZn=Xb@n&&rmgyc`x$?hE5>Gb(lf&g9;x50|VnkMKi8L3^E^-V7x_4Yyfao4F>=K

delta 49
ycmZn=Xb@n&$j~v7c`x%NhISyub)KR0(;jXH1_s87ie_Br7%D#Pg7FqHu>k;=@ec(6

diff --git a/ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat
index b89a239738ecaaed513123ae5d49b7cdf3c7794f..0bf9df99fab8de4c7742b43b43df5efc53689d1a 100644
GIT binary patch
delta 51
zcmZn=Xb@n&$dEsgc`x%NhCCp}b(&$}vnk>X3=E7D70tL#F)VpD3Cc6y%*gbg6#%fn
B599y<

delta 51
zcmZn=Xb@n&%aA;gc`x%Fh7=&hb)13srH&{A0|VnkMKi8r3<58;pgi-<j7<Mo0fev(
A<^TWy

diff --git a/ctre_sim/Talon FX vers. C - 018 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 018 - 0 - ext.dat
index c8dd93ae9df6ff4768403184ef996463e08fd6c2..deb82d5b243580dacfcf8338c4879ec23b71f004 100644
GIT binary patch
delta 63
zcmZn=Xb@nz&7jgSQ80C)0`J5!FP1wD$_O6kafZ3id&C(S1SSfaa~@+@@VpDcvDnPW
H^q&;~gP{}7

delta 63
zcmZn=Xb@nz%b?USQ80C)0`J5!FP3`@iU=O(VFu1uDxwSw0uu$zIgc=~y;6g4EH*PT
H{bvOLT4oW@

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 4f474f83..eeae36db 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -18,6 +18,7 @@
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -178,6 +179,7 @@ public void robotInit() {
       noteDropdown.addOption(note.getName(), note);
     }
 
+
     // Default Commands here
     swerve.setDefaultCommand(
         swerve.runVelocityFieldRelative(
@@ -393,7 +395,7 @@ public void robotPeriodic() {
         "DynamicAuto/Closest Note Within Distance",
         swerve
                 .getPose()
-                .minus(DynamicAuto.getClosestNote(swerve::getPose).getPose())
+                .minus(DynamicAuto.getClosestNote(swerve::getPose).getPoseAllianceSpicific())
                 .getTranslation()
                 .getNorm()
             > 1.5);
@@ -403,16 +405,16 @@ public void robotPeriodic() {
         "DynamicAuto/Closest Note Exists",
         DynamicAuto.getClosestNote(swerve::getPose).getExistence());
     Logger.recordOutput(
-        "DynamicAuto/Closest Note", DynamicAuto.getClosestNote(swerve::getPose).getPose());
+        "DynamicAuto/Closest Note", DynamicAuto.getClosestNote(swerve::getPose).getPoseAllianceSpicific());
 
     Logger.recordOutput(
         "DynamicAuto/Absolute Closest Note",
-        DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getPose());
+        DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getPoseAllianceSpicific());
 
     Logger.recordOutput(
         "DynamicAuto/Closest Shooting Location",
         DynamicAuto.closestShootingLocation(() -> swerve.getPose(), DynamicAuto.shootingLocations)
-            .getPose());
+            .getPoseAllianceSpicific());
     // Logger.recordOutput("Canivore Util", CANBus.getStatus("canivore").BusUtilization);
   }
 
@@ -623,7 +625,7 @@ public CommandSelector selectAuto() {
         || (DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getExistence()
             && swerve
                     .getPose()
-                    .minus(DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getPose())
+                    .minus(DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getPoseAllianceSpicific())
                     .getTranslation()
                     .getNorm()
                 < 1.5)) {
@@ -635,7 +637,7 @@ public CommandSelector selectAuto() {
         || (!DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getExistence()
             && swerve
                     .getPose()
-                    .minus(DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getPose())
+                    .minus(DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getPoseAllianceSpicific())
                     .getTranslation()
                     .getNorm()
                 < 1.5)) {
@@ -647,7 +649,7 @@ public CommandSelector selectAuto() {
         || (DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getExistence()
             && swerve
                     .getPose()
-                    .minus(DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getPose())
+                    .minus(DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getPoseAllianceSpicific())
                     .getTranslation()
                     .getNorm()
                 > 1.5)) {
diff --git a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
index ba4e3246..12555fce 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
@@ -62,13 +62,13 @@ public static void updateNote(Note note, BooleanSupplier blacklist, IntSupplier
 
   public static Command DynamicToNote(Supplier<Pose2d> curPose) {
     return AutoBuilder.pathfindToPose(
-        getClosestNote(curPose).getPose(),
+        getClosestNote(curPose).getPoseAllianceSpicific(),
         new PathConstraints(3.0, 4.0, Units.degreesToRadians(540), Units.degreesToRadians(720)));
   }
 
   public static Command DynamicToShoot(Supplier<Pose2d> curPose) {
     return AutoBuilder.pathfindToPose(
-        closestShootingLocation(curPose, shootingLocations).getPose(),
+        closestShootingLocation(curPose, shootingLocations).getPoseAllianceSpicific(),
         new PathConstraints(3.0, 4.0, Units.degreesToRadians(540), Units.degreesToRadians(720)));
   }
 
@@ -78,25 +78,19 @@ public static ShootingLocation closestShootingLocation(
     ShootingLocation closestLocation = new ShootingLocation("shooting shooting");
     double shortestDistance = 9999999;
     for (ShootingLocation location : shootingLocations) {
-      if (shortestDistance > curPose.get().minus(location.getPose()).getTranslation().getNorm()) {
+      if (shortestDistance > curPose.get().minus(location.getPoseAllianceSpicific()).getTranslation().getNorm()) {
         closestLocation = location;
-        shortestDistance = curPose.get().minus(location.getPose()).getTranslation().getNorm();
+        shortestDistance = curPose.get().minus(location.getPoseAllianceSpicific()).getTranslation().getNorm();
       }
     }
     return closestLocation;
   }
 
-  public static void flipNote(Note note) {
-    Pose2d pose = note.getPose();
-    note.setPose(
-        new Pose2d(
-            16.5410515 - pose.getX(),
-            pose.getY(),
-            Rotation2d.fromRadians(Math.PI - pose.getRotation().getRadians())));
-  }
+  
 
   public static ChoreoTrajectory makeStartToNote(Supplier<Pose2d> startingPose) {
     ShootingLocation startingLocation = closestShootingLocation(startingPose, startingLocations);
+    startingLocation.setPose(startingLocation.getPoseAllianceSpicific());
     Note closestNote = getClosestNote(startingPose);
     if ("Uninitialized" == closestNote.getName()) {
       closestNote = getAbsoluteClosestNote(startingPose);
@@ -145,8 +139,8 @@ public static Note getAbsoluteClosestNote(Supplier<Pose2d> curPose) {
     Note closestNote = new Note();
     double shortestDistance = 99999999;
     for (Note note : notes) {
-      if (shortestDistance > curPose.get().minus(note.getPose()).getTranslation().getNorm()) {
-        shortestDistance = curPose.get().minus(note.getPose()).getTranslation().getNorm();
+      if (shortestDistance > curPose.get().minus(note.getPoseAllianceSpicific()).getTranslation().getNorm()) {
+        shortestDistance = curPose.get().minus(note.getPoseAllianceSpicific()).getTranslation().getNorm();
         closestNote = note;
       }
     }
@@ -162,15 +156,15 @@ public static Note getClosestNote(Supplier<Pose2d> curPose) {
 
         continue;
       } else if (note.getPriority() > closestNote.getPriority()) {
-        shortestDistance = curPose.get().minus(note.getPose()).getTranslation().getNorm();
+        shortestDistance = curPose.get().minus(note.getPoseAllianceSpicific()).getTranslation().getNorm();
         closestNote = note;
-        if (shortestDistance > curPose.get().minus(note.getPose()).getTranslation().getNorm()) {
-          shortestDistance = curPose.get().minus(note.getPose()).getTranslation().getNorm();
+        if (shortestDistance > curPose.get().minus(note.getPoseAllianceSpicific()).getTranslation().getNorm()) {
+          shortestDistance = curPose.get().minus(note.getPoseAllianceSpicific()).getTranslation().getNorm();
           closestNote = note;
         }
       } else if (note.getPriority() == closestNote.getPriority()) {
-        if (shortestDistance > curPose.get().minus(note.getPose()).getTranslation().getNorm()) {
-          shortestDistance = curPose.get().minus(note.getPose()).getTranslation().getNorm();
+        if (shortestDistance > curPose.get().minus(note.getPoseAllianceSpicific()).getTranslation().getNorm()) {
+          shortestDistance = curPose.get().minus(note.getPoseAllianceSpicific()).getTranslation().getNorm();
           closestNote = note;
         }
       }
diff --git a/src/main/java/frc/robot/utils/dynamicauto/Note.java b/src/main/java/frc/robot/utils/dynamicauto/Note.java
index fb683b13..6bfcc0b3 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/Note.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/Note.java
@@ -5,6 +5,9 @@
 package frc.robot.utils.dynamicauto;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 
 /** Add your docs here. */
 public class Note {
@@ -65,6 +68,20 @@ public void setPose(Pose2d pose) {
     this.pose = pose;
   }
 
+  public Pose2d getPoseAllianceSpicific() {
+    if(DriverStation.getAlliance().isPresent()){
+      if(DriverStation.getAlliance().get() == Alliance.Red){
+        return new Pose2d(
+        16.5410515 - this.pose.getX(),
+        this.pose.getY(),
+        Rotation2d.fromRadians(Math.PI - this.pose.getRotation().getRadians()));
+      }
+    }
+    return this.pose;
+  
+    
+  }
+
   public boolean getBlacklist() {
     return blacklisted;
   }
diff --git a/src/main/java/frc/robot/utils/dynamicauto/ShootingLocation.java b/src/main/java/frc/robot/utils/dynamicauto/ShootingLocation.java
index dc54041e..a28846f0 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/ShootingLocation.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/ShootingLocation.java
@@ -5,6 +5,9 @@
 package frc.robot.utils.dynamicauto;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 
 /** Add your docs here. */
 public class ShootingLocation {
@@ -25,10 +28,28 @@ public ShootingLocation(String name) {
     this.name = name;
   }
 
+  public void setPose(Pose2d pose){
+    this.pose = pose;
+  }
+
   public Pose2d getPose() {
     return pose;
   }
 
+  public Pose2d getPoseAllianceSpicific() {
+    if(DriverStation.getAlliance().isPresent()){
+      if(DriverStation.getAlliance().get() == Alliance.Red){
+        return new Pose2d(
+        16.5410515 - this.pose.getX(),
+        this.pose.getY(),
+        Rotation2d.fromRadians(Math.PI - this.pose.getRotation().getRadians()));
+      }
+    }
+    return this.pose;
+  
+    
+  }
+
   public String getName() {
     return name;
   }

From 1a8c77de4dbb0b3a894aa513a1e4aa8cc8bab714 Mon Sep 17 00:00:00 2001
From: Awesomeplayer165 <dev@jacob2.dev>
Date: Sun, 25 Feb 2024 16:32:42 -0800
Subject: [PATCH 60/62] add more trajectory logging

---
 ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 018 - 0 - ext.dat | Bin 2048 -> 2048 bytes
 src/main/java/frc/robot/Robot.java            |  31 ++++++++++---
 .../robot/utils/dynamicauto/DynamicAuto.java  |  41 +++++++++++++-----
 .../frc/robot/utils/dynamicauto/Note.java     |  12 +++--
 .../utils/dynamicauto/ShootingLocation.java   |  14 +++---
 8 files changed, 65 insertions(+), 33 deletions(-)

diff --git a/ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat
index 9acaac4b6e9b274a80753012e6029c0e09688710..56d956c14b7edfdbc500f3750efb8165c04949bf 100644
GIT binary patch
delta 40
rcmZn=Xb{-Yz{quwA^zS2IR*v>rip@PTn8AE?%jj(%r`SK{bvOL{MZZ4

delta 40
rcmZn=Xb{-Yz{qu+VeQi%aRvqkrip@PTqhV-J?(??%r`SK{bvOL{#Xmm

diff --git a/ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat
index 256fc7184cda9417e5aa3c286db55bf1c2488f36..f59bb310f0c8ede190539d2679870e53f786ccfc 100644
GIT binary patch
delta 38
ocmZn=Xb{-Yz{qu);oyrBQ3eJE#)*n%TxS^ey(ok67BR5_0O*zq+yDRo

delta 38
ncmZn=Xb{-Yz{qu&LGptN9|Hpe<3vR>u0sqmACzFcMNDh}zy1i`

diff --git a/ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat
index 0bf9df99fab8de4c7742b43b43df5efc53689d1a..06d3cf562a6acbd67d795fa5cca4840613e967e0 100644
GIT binary patch
delta 40
rcmZn=Xb{-Yz{qu&A@cqcIR*v>#)*n%Tt^tf?mvU_%r`SK{bvOL{+J8x

delta 40
rcmZn=Xb{-Yz{qu)Vd1kW;tUK7j1v{jxK1%Fc{T~kGvCa}^q&;~04EFW

diff --git a/ctre_sim/Talon FX vers. C - 018 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 018 - 0 - ext.dat
index deb82d5b243580dacfcf8338c4879ec23b71f004..abeb3b49bc95e09fca083e2c313cc286753a55c7 100644
GIT binary patch
delta 38
qcmZn=Xb{-Yz{q)?!S%r%IR*xSiGt>w2N;4L+=Fl|HZwB)X9WQ5s0+;i

delta 38
qcmZn=Xb{-Yz{q)=Vea!DaRvr~iGt>w#~2no?}BhFHZwB)X9WQ72n)>s

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index eeae36db..ee700088 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -18,7 +18,6 @@
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -94,6 +93,7 @@ private static enum CommandSelector {
   private int dynamicAutoCounter = 0;
   private boolean atShootingLocation = false;
   private ChoreoTrajectory curTrajectory = Choreo.getTrajectory("Amp Side To C1");
+  private Pose2d[] forwardLookingTrajectory = curTrajectory.getPoses();
 
   private final SwerveSubsystem swerve =
       new SwerveSubsystem(
@@ -179,7 +179,6 @@ public void robotInit() {
       noteDropdown.addOption(note.getName(), note);
     }
 
-
     // Default Commands here
     swerve.setDefaultCommand(
         swerve.runVelocityFieldRelative(
@@ -405,7 +404,8 @@ public void robotPeriodic() {
         "DynamicAuto/Closest Note Exists",
         DynamicAuto.getClosestNote(swerve::getPose).getExistence());
     Logger.recordOutput(
-        "DynamicAuto/Closest Note", DynamicAuto.getClosestNote(swerve::getPose).getPoseAllianceSpicific());
+        "DynamicAuto/Closest Note",
+        DynamicAuto.getClosestNote(swerve::getPose).getPoseAllianceSpicific());
 
     Logger.recordOutput(
         "DynamicAuto/Absolute Closest Note",
@@ -415,6 +415,8 @@ public void robotPeriodic() {
         "DynamicAuto/Closest Shooting Location",
         DynamicAuto.closestShootingLocation(() -> swerve.getPose(), DynamicAuto.shootingLocations)
             .getPoseAllianceSpicific());
+    Logger.recordOutput("DynamicAuto/Curent Trajectory Followed", curTrajectory.getPoses());
+    Logger.recordOutput("DynamicAuto/Forward Trajectory Followed", forwardLookingTrajectory);
     // Logger.recordOutput("Canivore Util", CANBus.getStatus("canivore").BusUtilization);
   }
 
@@ -539,7 +541,10 @@ public Command startToNote() {
         .runChoreoTraj(
             () -> {
               curTrajectory = DynamicAuto.makeStartToNote(swerve::getPose);
-              return DynamicAuto.makeStartToNote(swerve::getPose);
+              forwardLookingTrajectory =
+                  DynamicAuto.addTwoTrajectories(
+                      curTrajectory, DynamicAuto.makeNoteToShooting(curTrajectory::getFinalPose));
+              return curTrajectory;
             })
         .onlyIf(
             () -> {
@@ -558,6 +563,9 @@ public Command noteToNote() {
             () -> {
               System.out.println(DynamicAuto.whitelistCount);
               curTrajectory = DynamicAuto.makeNoteToNote(swerve::getPose);
+              forwardLookingTrajectory =
+                  DynamicAuto.addTwoTrajectories(
+                      curTrajectory, DynamicAuto.makeNoteToShooting(curTrajectory::getFinalPose));
               return DynamicAuto.makeNoteToNote(swerve::getPose);
             })
         .onlyIf(
@@ -576,6 +584,9 @@ public Command shootToNote() {
         .runChoreoTraj(
             () -> {
               curTrajectory = DynamicAuto.makeShootingToNote(swerve::getPose);
+              forwardLookingTrajectory =
+                  DynamicAuto.addTwoTrajectories(
+                      curTrajectory, DynamicAuto.makeNoteToShooting(curTrajectory::getFinalPose));
               return DynamicAuto.makeShootingToNote(swerve::getPose);
             })
         .onlyIf(
@@ -625,7 +636,9 @@ public CommandSelector selectAuto() {
         || (DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getExistence()
             && swerve
                     .getPose()
-                    .minus(DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getPoseAllianceSpicific())
+                    .minus(
+                        DynamicAuto.getAbsoluteClosestNote(swerve::getPose)
+                            .getPoseAllianceSpicific())
                     .getTranslation()
                     .getNorm()
                 < 1.5)) {
@@ -637,7 +650,9 @@ public CommandSelector selectAuto() {
         || (!DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getExistence()
             && swerve
                     .getPose()
-                    .minus(DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getPoseAllianceSpicific())
+                    .minus(
+                        DynamicAuto.getAbsoluteClosestNote(swerve::getPose)
+                            .getPoseAllianceSpicific())
                     .getTranslation()
                     .getNorm()
                 < 1.5)) {
@@ -649,7 +664,9 @@ public CommandSelector selectAuto() {
         || (DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getExistence()
             && swerve
                     .getPose()
-                    .minus(DynamicAuto.getAbsoluteClosestNote(swerve::getPose).getPoseAllianceSpicific())
+                    .minus(
+                        DynamicAuto.getAbsoluteClosestNote(swerve::getPose)
+                            .getPoseAllianceSpicific())
                     .getTranslation()
                     .getNorm()
                 > 1.5)) {
diff --git a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
index 12555fce..2408512f 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/DynamicAuto.java
@@ -78,16 +78,16 @@ public static ShootingLocation closestShootingLocation(
     ShootingLocation closestLocation = new ShootingLocation("shooting shooting");
     double shortestDistance = 9999999;
     for (ShootingLocation location : shootingLocations) {
-      if (shortestDistance > curPose.get().minus(location.getPoseAllianceSpicific()).getTranslation().getNorm()) {
+      if (shortestDistance
+          > curPose.get().minus(location.getPoseAllianceSpicific()).getTranslation().getNorm()) {
         closestLocation = location;
-        shortestDistance = curPose.get().minus(location.getPoseAllianceSpicific()).getTranslation().getNorm();
+        shortestDistance =
+            curPose.get().minus(location.getPoseAllianceSpicific()).getTranslation().getNorm();
       }
     }
     return closestLocation;
   }
 
-  
-
   public static ChoreoTrajectory makeStartToNote(Supplier<Pose2d> startingPose) {
     ShootingLocation startingLocation = closestShootingLocation(startingPose, startingLocations);
     startingLocation.setPose(startingLocation.getPoseAllianceSpicific());
@@ -139,8 +139,10 @@ public static Note getAbsoluteClosestNote(Supplier<Pose2d> curPose) {
     Note closestNote = new Note();
     double shortestDistance = 99999999;
     for (Note note : notes) {
-      if (shortestDistance > curPose.get().minus(note.getPoseAllianceSpicific()).getTranslation().getNorm()) {
-        shortestDistance = curPose.get().minus(note.getPoseAllianceSpicific()).getTranslation().getNorm();
+      if (shortestDistance
+          > curPose.get().minus(note.getPoseAllianceSpicific()).getTranslation().getNorm()) {
+        shortestDistance =
+            curPose.get().minus(note.getPoseAllianceSpicific()).getTranslation().getNorm();
         closestNote = note;
       }
     }
@@ -156,15 +158,20 @@ public static Note getClosestNote(Supplier<Pose2d> curPose) {
 
         continue;
       } else if (note.getPriority() > closestNote.getPriority()) {
-        shortestDistance = curPose.get().minus(note.getPoseAllianceSpicific()).getTranslation().getNorm();
+        shortestDistance =
+            curPose.get().minus(note.getPoseAllianceSpicific()).getTranslation().getNorm();
         closestNote = note;
-        if (shortestDistance > curPose.get().minus(note.getPoseAllianceSpicific()).getTranslation().getNorm()) {
-          shortestDistance = curPose.get().minus(note.getPoseAllianceSpicific()).getTranslation().getNorm();
+        if (shortestDistance
+            > curPose.get().minus(note.getPoseAllianceSpicific()).getTranslation().getNorm()) {
+          shortestDistance =
+              curPose.get().minus(note.getPoseAllianceSpicific()).getTranslation().getNorm();
           closestNote = note;
         }
       } else if (note.getPriority() == closestNote.getPriority()) {
-        if (shortestDistance > curPose.get().minus(note.getPoseAllianceSpicific()).getTranslation().getNorm()) {
-          shortestDistance = curPose.get().minus(note.getPoseAllianceSpicific()).getTranslation().getNorm();
+        if (shortestDistance
+            > curPose.get().minus(note.getPoseAllianceSpicific()).getTranslation().getNorm()) {
+          shortestDistance =
+              curPose.get().minus(note.getPoseAllianceSpicific()).getTranslation().getNorm();
           closestNote = note;
         }
       }
@@ -172,4 +179,16 @@ public static Note getClosestNote(Supplier<Pose2d> curPose) {
 
     return closestNote;
   }
+
+  public static Pose2d[] addTwoTrajectories(
+      ChoreoTrajectory trajectory1, ChoreoTrajectory trajectory2) {
+    Pose2d[] poses = new Pose2d[trajectory1.getPoses().length + trajectory2.getPoses().length];
+    for (int i = 0; i < trajectory1.getPoses().length; i++) {
+      poses[i] = trajectory1.getPoses()[i];
+    }
+    for (int i = 0; i < trajectory2.getPoses().length; i++) {
+      poses[i + trajectory1.getPoses().length] = trajectory2.getPoses()[i];
+    }
+    return poses;
+  }
 }
diff --git a/src/main/java/frc/robot/utils/dynamicauto/Note.java b/src/main/java/frc/robot/utils/dynamicauto/Note.java
index 6bfcc0b3..71e10f80 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/Note.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/Note.java
@@ -69,17 +69,15 @@ public void setPose(Pose2d pose) {
   }
 
   public Pose2d getPoseAllianceSpicific() {
-    if(DriverStation.getAlliance().isPresent()){
-      if(DriverStation.getAlliance().get() == Alliance.Red){
+    if (DriverStation.getAlliance().isPresent()) {
+      if (DriverStation.getAlliance().get() == Alliance.Red) {
         return new Pose2d(
-        16.5410515 - this.pose.getX(),
-        this.pose.getY(),
-        Rotation2d.fromRadians(Math.PI - this.pose.getRotation().getRadians()));
+            16.5410515 - this.pose.getX(),
+            this.pose.getY(),
+            Rotation2d.fromRadians(Math.PI - this.pose.getRotation().getRadians()));
       }
     }
     return this.pose;
-  
-    
   }
 
   public boolean getBlacklist() {
diff --git a/src/main/java/frc/robot/utils/dynamicauto/ShootingLocation.java b/src/main/java/frc/robot/utils/dynamicauto/ShootingLocation.java
index a28846f0..9e081011 100644
--- a/src/main/java/frc/robot/utils/dynamicauto/ShootingLocation.java
+++ b/src/main/java/frc/robot/utils/dynamicauto/ShootingLocation.java
@@ -28,7 +28,7 @@ public ShootingLocation(String name) {
     this.name = name;
   }
 
-  public void setPose(Pose2d pose){
+  public void setPose(Pose2d pose) {
     this.pose = pose;
   }
 
@@ -37,17 +37,15 @@ public Pose2d getPose() {
   }
 
   public Pose2d getPoseAllianceSpicific() {
-    if(DriverStation.getAlliance().isPresent()){
-      if(DriverStation.getAlliance().get() == Alliance.Red){
+    if (DriverStation.getAlliance().isPresent()) {
+      if (DriverStation.getAlliance().get() == Alliance.Red) {
         return new Pose2d(
-        16.5410515 - this.pose.getX(),
-        this.pose.getY(),
-        Rotation2d.fromRadians(Math.PI - this.pose.getRotation().getRadians()));
+            16.5410515 - this.pose.getX(),
+            this.pose.getY(),
+            Rotation2d.fromRadians(Math.PI - this.pose.getRotation().getRadians()));
       }
     }
     return this.pose;
-  
-    
   }
 
   public String getName() {

From e9c92d93ccac9441ccda37aea6e7db4bb4c8285f Mon Sep 17 00:00:00 2001
From: Awesomeplayer165 <dev@jacob2.dev>
Date: Wed, 28 Feb 2024 22:10:18 -0800
Subject: [PATCH 61/62] remove generated trajectory and data.chor files

---
 .../frc/robot/utils/dynamicauto/data.chor     | 45944 ----------------
 .../deploy/choreo/Amp Side To C1.1.traj       |   328 -
 .../deploy/choreo/Amp Side To C1.traj         |   328 -
 .../deploy/choreo/Amp Side To C2.1.traj       |   364 -
 .../deploy/choreo/Amp Side To C2.traj         |   364 -
 .../deploy/choreo/Amp Side To C3.1.traj       |   670 -
 .../deploy/choreo/Amp Side To C3.traj         |   670 -
 .../deploy/choreo/Amp Side To C4.1.traj       |   454 -
 .../deploy/choreo/Amp Side To C4.traj         |   454 -
 .../deploy/choreo/Amp Side To C5.1.traj       |   859 -
 .../deploy/choreo/Amp Side To C5.traj         |   859 -
 .../deploy/choreo/Amp Side To W1.1.traj       |   121 -
 .../deploy/choreo/Amp Side To W1.traj         |   121 -
 .../deploy/choreo/Amp Side To W2.1.traj       |   148 -
 .../deploy/choreo/Amp Side To W2.traj         |   148 -
 .../deploy/choreo/Amp Side To W3.1.traj       |   238 -
 .../deploy/choreo/Amp Side To W3.traj         |   238 -
 .../dynamicauto/deploy/choreo/C1 To C2.1.traj |    94 -
 .../dynamicauto/deploy/choreo/C1 To C2.traj   |    94 -
 .../dynamicauto/deploy/choreo/C1 To C3.1.traj |   139 -
 .../dynamicauto/deploy/choreo/C1 To C3.traj   |   139 -
 .../dynamicauto/deploy/choreo/C1 To C4.1.traj |   175 -
 .../dynamicauto/deploy/choreo/C1 To C4.traj   |   175 -
 .../dynamicauto/deploy/choreo/C1 To C5.1.traj |   220 -
 .../dynamicauto/deploy/choreo/C1 To C5.traj   |   220 -
 .../dynamicauto/deploy/choreo/C1 To S1.1.traj |   130 -
 .../dynamicauto/deploy/choreo/C1 To S1.traj   |   130 -
 .../dynamicauto/deploy/choreo/C1 To S2.1.traj |   148 -
 .../dynamicauto/deploy/choreo/C1 To S2.traj   |   148 -
 .../dynamicauto/deploy/choreo/C1 To S3.1.traj |   265 -
 .../dynamicauto/deploy/choreo/C1 To S3.traj   |   265 -
 .../dynamicauto/deploy/choreo/C1 To S4.1.traj |   301 -
 .../dynamicauto/deploy/choreo/C1 To S4.traj   |   301 -
 .../dynamicauto/deploy/choreo/C1 To W1.1.traj |   202 -
 .../dynamicauto/deploy/choreo/C1 To W1.traj   |   202 -
 .../dynamicauto/deploy/choreo/C1 To W2.1.traj |   193 -
 .../dynamicauto/deploy/choreo/C1 To W2.traj   |   193 -
 .../dynamicauto/deploy/choreo/C1 To W3.1.traj |   346 -
 .../dynamicauto/deploy/choreo/C1 To W3.traj   |   346 -
 .../dynamicauto/deploy/choreo/C2 To C1.1.traj |    94 -
 .../dynamicauto/deploy/choreo/C2 To C1.traj   |    94 -
 .../dynamicauto/deploy/choreo/C2 To C3.1.traj |    94 -
 .../dynamicauto/deploy/choreo/C2 To C3.traj   |    94 -
 .../dynamicauto/deploy/choreo/C2 To C4.1.traj |   139 -
 .../dynamicauto/deploy/choreo/C2 To C4.traj   |   139 -
 .../dynamicauto/deploy/choreo/C2 To C5.1.traj |   175 -
 .../dynamicauto/deploy/choreo/C2 To C5.traj   |   175 -
 .../dynamicauto/deploy/choreo/C2 To S1.1.traj |   130 -
 .../dynamicauto/deploy/choreo/C2 To S1.traj   |   130 -
 .../dynamicauto/deploy/choreo/C2 To S2.1.traj |   193 -
 .../dynamicauto/deploy/choreo/C2 To S2.traj   |   193 -
 .../dynamicauto/deploy/choreo/C2 To S3.1.traj |   193 -
 .../dynamicauto/deploy/choreo/C2 To S3.traj   |   193 -
 .../dynamicauto/deploy/choreo/C2 To S4.1.traj |   256 -
 .../dynamicauto/deploy/choreo/C2 To S4.traj   |   256 -
 .../dynamicauto/deploy/choreo/C2 To W1.1.traj |   202 -
 .../dynamicauto/deploy/choreo/C2 To W1.traj   |   202 -
 .../dynamicauto/deploy/choreo/C2 To W2.1.traj |   184 -
 .../dynamicauto/deploy/choreo/C2 To W2.traj   |   184 -
 .../dynamicauto/deploy/choreo/C2 To W3.1.traj |   346 -
 .../dynamicauto/deploy/choreo/C2 To W3.traj   |   346 -
 .../dynamicauto/deploy/choreo/C3 To C1.1.traj |   139 -
 .../dynamicauto/deploy/choreo/C3 To C1.traj   |   139 -
 .../dynamicauto/deploy/choreo/C3 To C2.1.traj |    94 -
 .../dynamicauto/deploy/choreo/C3 To C2.traj   |    94 -
 .../dynamicauto/deploy/choreo/C3 To C4.1.traj |    94 -
 .../dynamicauto/deploy/choreo/C3 To C4.traj   |    94 -
 .../dynamicauto/deploy/choreo/C3 To C5.1.traj |   139 -
 .../dynamicauto/deploy/choreo/C3 To C5.traj   |   139 -
 .../dynamicauto/deploy/choreo/C3 To S1.1.traj |   157 -
 .../dynamicauto/deploy/choreo/C3 To S1.traj   |   157 -
 .../dynamicauto/deploy/choreo/C3 To S2.1.traj |   148 -
 .../dynamicauto/deploy/choreo/C3 To S2.traj   |   148 -
 .../dynamicauto/deploy/choreo/C3 To S3.1.traj |   166 -
 .../dynamicauto/deploy/choreo/C3 To S3.traj   |   166 -
 .../dynamicauto/deploy/choreo/C3 To S4.1.traj |   238 -
 .../dynamicauto/deploy/choreo/C3 To S4.traj   |   238 -
 .../dynamicauto/deploy/choreo/C3 To W1.1.traj |   265 -
 .../dynamicauto/deploy/choreo/C3 To W1.traj   |   265 -
 .../dynamicauto/deploy/choreo/C3 To W2.1.traj |   193 -
 .../dynamicauto/deploy/choreo/C3 To W2.traj   |   193 -
 .../dynamicauto/deploy/choreo/C3 To W3.1.traj |   202 -
 .../dynamicauto/deploy/choreo/C3 To W3.traj   |   202 -
 .../dynamicauto/deploy/choreo/C4 To C1.1.traj |   175 -
 .../dynamicauto/deploy/choreo/C4 To C1.traj   |   175 -
 .../dynamicauto/deploy/choreo/C4 To C2.1.traj |   139 -
 .../dynamicauto/deploy/choreo/C4 To C2.traj   |   139 -
 .../dynamicauto/deploy/choreo/C4 To C3.1.traj |    94 -
 .../dynamicauto/deploy/choreo/C4 To C3.traj   |    94 -
 .../dynamicauto/deploy/choreo/C4 To C5.1.traj |    94 -
 .../dynamicauto/deploy/choreo/C4 To C5.traj   |    94 -
 .../dynamicauto/deploy/choreo/C4 To S1.1.traj |   256 -
 .../dynamicauto/deploy/choreo/C4 To S1.traj   |   256 -
 .../dynamicauto/deploy/choreo/C4 To S2.1.traj |   211 -
 .../dynamicauto/deploy/choreo/C4 To S2.traj   |   211 -
 .../dynamicauto/deploy/choreo/C4 To S3.1.traj |   229 -
 .../dynamicauto/deploy/choreo/C4 To S3.traj   |   229 -
 .../dynamicauto/deploy/choreo/C4 To S4.1.traj |   193 -
 .../dynamicauto/deploy/choreo/C4 To S4.traj   |   193 -
 .../dynamicauto/deploy/choreo/C4 To W1.1.traj |   238 -
 .../dynamicauto/deploy/choreo/C4 To W1.traj   |   238 -
 .../dynamicauto/deploy/choreo/C4 To W2.1.traj |   319 -
 .../dynamicauto/deploy/choreo/C4 To W2.traj   |   319 -
 .../dynamicauto/deploy/choreo/C4 To W3.1.traj |   274 -
 .../dynamicauto/deploy/choreo/C4 To W3.traj   |   274 -
 .../dynamicauto/deploy/choreo/C5 To C1.1.traj |   220 -
 .../dynamicauto/deploy/choreo/C5 To C1.traj   |   220 -
 .../dynamicauto/deploy/choreo/C5 To C2.1.traj |   175 -
 .../dynamicauto/deploy/choreo/C5 To C2.traj   |   175 -
 .../dynamicauto/deploy/choreo/C5 To C3.1.traj |   139 -
 .../dynamicauto/deploy/choreo/C5 To C3.traj   |   139 -
 .../dynamicauto/deploy/choreo/C5 To C4.1.traj |    94 -
 .../dynamicauto/deploy/choreo/C5 To C4.traj   |    94 -
 .../dynamicauto/deploy/choreo/C5 To S1.1.traj |   292 -
 .../dynamicauto/deploy/choreo/C5 To S1.traj   |   292 -
 .../dynamicauto/deploy/choreo/C5 To S2.1.traj |   247 -
 .../dynamicauto/deploy/choreo/C5 To S2.traj   |   247 -
 .../dynamicauto/deploy/choreo/C5 To S3.1.traj |   247 -
 .../dynamicauto/deploy/choreo/C5 To S3.traj   |   247 -
 .../dynamicauto/deploy/choreo/C5 To S4.1.traj |   148 -
 .../dynamicauto/deploy/choreo/C5 To S4.traj   |   148 -
 .../dynamicauto/deploy/choreo/C5 To W1.1.traj |   310 -
 .../dynamicauto/deploy/choreo/C5 To W1.traj   |   310 -
 .../dynamicauto/deploy/choreo/C5 To W2.1.traj |   346 -
 .../dynamicauto/deploy/choreo/C5 To W2.traj   |   346 -
 .../dynamicauto/deploy/choreo/C5 To W3.1.traj |   292 -
 .../dynamicauto/deploy/choreo/C5 To W3.traj   |   292 -
 .../deploy/choreo/Center To C1.1.traj         |   265 -
 .../deploy/choreo/Center To C1.traj           |   265 -
 .../deploy/choreo/Center To C2.1.traj         |   265 -
 .../deploy/choreo/Center To C2.traj           |   265 -
 .../deploy/choreo/Center To C3.1.traj         |   274 -
 .../deploy/choreo/Center To C3.traj           |   274 -
 .../deploy/choreo/Center To C4.1.traj         |  1453 -
 .../deploy/choreo/Center To C4.traj           |  1453 -
 .../deploy/choreo/Center To C5.1.traj         |   625 -
 .../deploy/choreo/Center To C5.traj           |   625 -
 .../deploy/choreo/Center To W1.1.traj         |   112 -
 .../deploy/choreo/Center To W1.traj           |   112 -
 .../deploy/choreo/Center To W2.1.traj         |    76 -
 .../deploy/choreo/Center To W2.traj           |    76 -
 .../deploy/choreo/Center To W3.1.traj         |   112 -
 .../deploy/choreo/Center To W3.traj           |   112 -
 .../dynamicauto/deploy/choreo/S1 To C1.1.traj |   130 -
 .../dynamicauto/deploy/choreo/S1 To C1.traj   |   130 -
 .../dynamicauto/deploy/choreo/S1 To C2.1.traj |   130 -
 .../dynamicauto/deploy/choreo/S1 To C2.traj   |   130 -
 .../dynamicauto/deploy/choreo/S1 To C3.1.traj |   157 -
 .../dynamicauto/deploy/choreo/S1 To C3.traj   |   157 -
 .../dynamicauto/deploy/choreo/S1 To C4.1.traj |   247 -
 .../dynamicauto/deploy/choreo/S1 To C4.traj   |   247 -
 .../dynamicauto/deploy/choreo/S1 To C5.1.traj |   292 -
 .../dynamicauto/deploy/choreo/S1 To C5.traj   |   292 -
 .../dynamicauto/deploy/choreo/S1 To W1.1.traj |   130 -
 .../dynamicauto/deploy/choreo/S1 To W1.traj   |   130 -
 .../dynamicauto/deploy/choreo/S1 To W2.1.traj |   148 -
 .../dynamicauto/deploy/choreo/S1 To W2.traj   |   148 -
 .../dynamicauto/deploy/choreo/S1 To W3.1.traj |   283 -
 .../dynamicauto/deploy/choreo/S1 To W3.traj   |   283 -
 .../dynamicauto/deploy/choreo/S2 To C1.1.traj |   148 -
 .../dynamicauto/deploy/choreo/S2 To C1.traj   |   148 -
 .../dynamicauto/deploy/choreo/S2 To C2.1.traj |   202 -
 .../dynamicauto/deploy/choreo/S2 To C2.traj   |   202 -
 .../dynamicauto/deploy/choreo/S2 To C3.1.traj |   148 -
 .../dynamicauto/deploy/choreo/S2 To C3.traj   |   148 -
 .../dynamicauto/deploy/choreo/S2 To C4.1.traj |   166 -
 .../dynamicauto/deploy/choreo/S2 To C4.traj   |   166 -
 .../dynamicauto/deploy/choreo/S2 To C5.1.traj |   202 -
 .../dynamicauto/deploy/choreo/S2 To C5.traj   |   202 -
 .../dynamicauto/deploy/choreo/S2 To W1.1.traj |   130 -
 .../dynamicauto/deploy/choreo/S2 To W1.traj   |   130 -
 .../dynamicauto/deploy/choreo/S2 To W2.1.traj |   103 -
 .../dynamicauto/deploy/choreo/S2 To W2.traj   |   103 -
 .../dynamicauto/deploy/choreo/S2 To W3.1.traj |   130 -
 .../dynamicauto/deploy/choreo/S2 To W3.traj   |   130 -
 .../dynamicauto/deploy/choreo/S3 To C1.1.traj |   220 -
 .../dynamicauto/deploy/choreo/S3 To C1.traj   |   220 -
 .../dynamicauto/deploy/choreo/S3 To C2.1.traj |   193 -
 .../dynamicauto/deploy/choreo/S3 To C2.traj   |   193 -
 .../dynamicauto/deploy/choreo/S3 To C3.1.traj |   166 -
 .../dynamicauto/deploy/choreo/S3 To C3.traj   |   166 -
 .../dynamicauto/deploy/choreo/S3 To C4.1.traj |   238 -
 .../dynamicauto/deploy/choreo/S3 To C4.traj   |   238 -
 .../dynamicauto/deploy/choreo/S3 To C5.1.traj |   184 -
 .../dynamicauto/deploy/choreo/S3 To C5.traj   |   184 -
 .../dynamicauto/deploy/choreo/S3 To W1.1.traj |   202 -
 .../dynamicauto/deploy/choreo/S3 To W1.traj   |   202 -
 .../dynamicauto/deploy/choreo/S3 To W2.1.traj |   157 -
 .../dynamicauto/deploy/choreo/S3 To W2.traj   |   157 -
 .../dynamicauto/deploy/choreo/S3 To W3.1.traj |   121 -
 .../dynamicauto/deploy/choreo/S3 To W3.traj   |   121 -
 .../dynamicauto/deploy/choreo/S4 To C1.1.traj |   292 -
 .../dynamicauto/deploy/choreo/S4 To C1.traj   |   292 -
 .../dynamicauto/deploy/choreo/S4 To C2.1.traj |   202 -
 .../dynamicauto/deploy/choreo/S4 To C2.traj   |   202 -
 .../dynamicauto/deploy/choreo/S4 To C3.1.traj |   238 -
 .../dynamicauto/deploy/choreo/S4 To C3.traj   |   238 -
 .../dynamicauto/deploy/choreo/S4 To C4.1.traj |   148 -
 .../dynamicauto/deploy/choreo/S4 To C4.traj   |   148 -
 .../dynamicauto/deploy/choreo/S4 To C5.1.traj |   148 -
 .../dynamicauto/deploy/choreo/S4 To C5.traj   |   148 -
 .../dynamicauto/deploy/choreo/S4 To W1.1.traj |   247 -
 .../dynamicauto/deploy/choreo/S4 To W1.traj   |   247 -
 .../dynamicauto/deploy/choreo/S4 To W2.1.traj |   202 -
 .../dynamicauto/deploy/choreo/S4 To W2.traj   |   202 -
 .../dynamicauto/deploy/choreo/S4 To W3.1.traj |   256 -
 .../dynamicauto/deploy/choreo/S4 To W3.traj   |   256 -
 .../deploy/choreo/Stage Side To C1.1.traj     |   427 -
 .../deploy/choreo/Stage Side To C1.traj       |   427 -
 .../deploy/choreo/Stage Side To C2.1.traj     |   661 -
 .../deploy/choreo/Stage Side To C2.traj       |   661 -
 .../deploy/choreo/Stage Side To C3.1.traj     |   391 -
 .../deploy/choreo/Stage Side To C3.traj       |   391 -
 .../deploy/choreo/Stage Side To C4.1.traj     |   679 -
 .../deploy/choreo/Stage Side To C4.traj       |   679 -
 .../deploy/choreo/Stage Side To C5.1.traj     |   337 -
 .../deploy/choreo/Stage Side To C5.traj       |   337 -
 .../deploy/choreo/Stage Side To W1.1.traj     |   238 -
 .../deploy/choreo/Stage Side To W1.traj       |   238 -
 .../deploy/choreo/Stage Side To W2.1.traj     |   193 -
 .../deploy/choreo/Stage Side To W2.traj       |   193 -
 .../deploy/choreo/Stage Side To W3.1.traj     |   121 -
 .../deploy/choreo/Stage Side To W3.traj       |   121 -
 .../dynamicauto/deploy/choreo/W1 To C1.1.traj |   256 -
 .../dynamicauto/deploy/choreo/W1 To C1.traj   |   256 -
 .../dynamicauto/deploy/choreo/W1 To C2.1.traj |   310 -
 .../dynamicauto/deploy/choreo/W1 To C2.traj   |   310 -
 .../dynamicauto/deploy/choreo/W1 To C3.1.traj |   373 -
 .../dynamicauto/deploy/choreo/W1 To C3.traj   |   373 -
 .../dynamicauto/deploy/choreo/W1 To C4.1.traj |   472 -
 .../dynamicauto/deploy/choreo/W1 To C4.traj   |   472 -
 .../dynamicauto/deploy/choreo/W1 To C5.1.traj |   616 -
 .../dynamicauto/deploy/choreo/W1 To C5.traj   |   616 -
 .../dynamicauto/deploy/choreo/W1 To S1.1.traj |   202 -
 .../dynamicauto/deploy/choreo/W1 To S1.traj   |   202 -
 .../dynamicauto/deploy/choreo/W1 To S2.1.traj |   202 -
 .../dynamicauto/deploy/choreo/W1 To S2.traj   |   202 -
 .../dynamicauto/deploy/choreo/W1 To S3.1.traj |   544 -
 .../dynamicauto/deploy/choreo/W1 To S3.traj   |   544 -
 .../dynamicauto/deploy/choreo/W1 To S4.1.traj |   562 -
 .../dynamicauto/deploy/choreo/W1 To S4.traj   |   562 -
 .../dynamicauto/deploy/choreo/W1 To W2.1.traj |   103 -
 .../dynamicauto/deploy/choreo/W1 To W2.traj   |   103 -
 .../dynamicauto/deploy/choreo/W1 To W3.1.traj |   148 -
 .../dynamicauto/deploy/choreo/W1 To W3.traj   |   148 -
 .../dynamicauto/deploy/choreo/W2 To C1.1.traj |   292 -
 .../dynamicauto/deploy/choreo/W2 To C1.traj   |   292 -
 .../dynamicauto/deploy/choreo/W2 To C2.1.traj |   616 -
 .../dynamicauto/deploy/choreo/W2 To C2.traj   |   616 -
 .../dynamicauto/deploy/choreo/W2 To C3.1.traj |   625 -
 .../dynamicauto/deploy/choreo/W2 To C3.traj   |   625 -
 .../dynamicauto/deploy/choreo/W2 To C4.1.traj |   589 -
 .../dynamicauto/deploy/choreo/W2 To C4.traj   |   589 -
 .../dynamicauto/deploy/choreo/W2 To C5.1.traj |   625 -
 .../dynamicauto/deploy/choreo/W2 To C5.traj   |   625 -
 .../dynamicauto/deploy/choreo/W2 To S1.1.traj |   202 -
 .../dynamicauto/deploy/choreo/W2 To S1.traj   |   202 -
 .../dynamicauto/deploy/choreo/W2 To S2.1.traj |   103 -
 .../dynamicauto/deploy/choreo/W2 To S2.traj   |   103 -
 .../dynamicauto/deploy/choreo/W2 To S3.1.traj |   526 -
 .../dynamicauto/deploy/choreo/W2 To S3.traj   |   526 -
 .../dynamicauto/deploy/choreo/W2 To S4.1.traj |   553 -
 .../dynamicauto/deploy/choreo/W2 To S4.traj   |   553 -
 .../dynamicauto/deploy/choreo/W2 To W1.1.traj |   103 -
 .../dynamicauto/deploy/choreo/W2 To W1.traj   |   103 -
 .../dynamicauto/deploy/choreo/W2 To W3.1.traj |   103 -
 .../dynamicauto/deploy/choreo/W2 To W3.traj   |   103 -
 .../dynamicauto/deploy/choreo/W3 To C1.1.traj |   634 -
 .../dynamicauto/deploy/choreo/W3 To C1.traj   |   634 -
 .../dynamicauto/deploy/choreo/W3 To C2.1.traj |   634 -
 .../dynamicauto/deploy/choreo/W3 To C2.traj   |   634 -
 .../dynamicauto/deploy/choreo/W3 To C3.1.traj |   400 -
 .../dynamicauto/deploy/choreo/W3 To C3.traj   |   400 -
 .../dynamicauto/deploy/choreo/W3 To C4.1.traj |   625 -
 .../dynamicauto/deploy/choreo/W3 To C4.traj   |   625 -
 .../dynamicauto/deploy/choreo/W3 To C5.1.traj |   571 -
 .../dynamicauto/deploy/choreo/W3 To C5.traj   |   571 -
 .../dynamicauto/deploy/choreo/W3 To S1.1.traj |   256 -
 .../dynamicauto/deploy/choreo/W3 To S1.traj   |   256 -
 .../dynamicauto/deploy/choreo/W3 To S2.1.traj |   265 -
 .../dynamicauto/deploy/choreo/W3 To S2.traj   |   265 -
 .../dynamicauto/deploy/choreo/W3 To S3.1.traj |   229 -
 .../dynamicauto/deploy/choreo/W3 To S3.traj   |   229 -
 .../dynamicauto/deploy/choreo/W3 To S4.1.traj |   256 -
 .../dynamicauto/deploy/choreo/W3 To S4.traj   |   256 -
 .../dynamicauto/deploy/choreo/W3 To W1.1.traj |   148 -
 .../dynamicauto/deploy/choreo/W3 To W1.traj   |   148 -
 .../dynamicauto/deploy/choreo/W3 To W2.1.traj |   103 -
 .../dynamicauto/deploy/choreo/W3 To W2.traj   |   103 -
 289 files changed, 122750 deletions(-)
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/data.chor
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C5.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C5.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C5.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj
 delete mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.traj

diff --git a/src/main/java/frc/robot/utils/dynamicauto/data.chor b/src/main/java/frc/robot/utils/dynamicauto/data.chor
deleted file mode 100644
index aae8a25b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/data.chor
+++ /dev/null
@@ -1,45944 +0,0 @@
-{
-    "version": "v0.2.2",
-    "robotConfiguration": {
-        "mass": 74.08797700309194,
-        "rotationalInertia": 6,
-        "motorMaxTorque": 1.162295081967213,
-        "motorMaxVelocity": 4800,
-        "gearing": 6.75,
-        "wheelbase": 0.5778496879611685,
-        "trackWidth": 0.5778496879611685,
-        "bumperLength": 0.8762995267982555,
-        "bumperWidth": 0.8762995267982555,
-        "wheelRadius": 0.050799972568014815
-    },
-    "paths": {
-        "Amp Side To W1": {
-            "waypoints": [
-                {
-                    "x": 0.71,
-                    "y": 6.72,
-                    "heading": 1.04,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 12
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 0.71,
-                    "y": 6.72,
-                    "heading": 1.04,
-                    "angularVelocity": -3.0520704892108665e-32,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 0.7541195079597249,
-                    "y": 6.720987197392724,
-                    "heading": 1.0162961519009226,
-                    "angularVelocity": -0.3240021115694771,
-                    "velocityX": 0.6030587810307456,
-                    "velocityY": 0.013493760103527496,
-                    "timestamp": 0.07315954820244353
-                },
-                {
-                    "x": 0.8424136303815098,
-                    "y": 6.722972492793459,
-                    "heading": 0.9702543819568796,
-                    "angularVelocity": -0.6293337107086672,
-                    "velocityX": 1.2068707993857855,
-                    "velocityY": 0.027136518055622637,
-                    "timestamp": 0.14631909640488705
-                },
-                {
-                    "x": 0.9749626751701804,
-                    "y": 6.7259693430054925,
-                    "heading": 0.9040114694947134,
-                    "angularVelocity": -0.9054581949963141,
-                    "velocityX": 1.8117805268819878,
-                    "velocityY": 0.0409632137659495,
-                    "timestamp": 0.21947864460733058
-                },
-                {
-                    "x": 1.1518931657803464,
-                    "y": 6.729992727527654,
-                    "heading": 0.8214709979365993,
-                    "angularVelocity": -1.1282255506783914,
-                    "velocityX": 2.4184196725842533,
-                    "velocityY": 0.0549946605880228,
-                    "timestamp": 0.2926381928097741
-                },
-                {
-                    "x": 1.3734034446931793,
-                    "y": 6.735064446892539,
-                    "heading": 0.7325177816386138,
-                    "angularVelocity": -1.215879792639604,
-                    "velocityX": 3.0277699132296103,
-                    "velocityY": 0.06932409356680827,
-                    "timestamp": 0.3657977410122176
-                },
-                {
-                    "x": 1.6366130282089124,
-                    "y": 6.741790657706068,
-                    "heading": 0.6993734786052862,
-                    "angularVelocity": -0.4530413848603092,
-                    "velocityX": 3.597747525550208,
-                    "velocityY": 0.09193893317775545,
-                    "timestamp": 0.43895728921466115
-                },
-                {
-                    "x": 1.858120509684905,
-                    "y": 6.746838490569188,
-                    "heading": 0.6098796286126051,
-                    "angularVelocity": -1.2232695825988174,
-                    "velocityX": 3.0277316757485147,
-                    "velocityY": 0.0689975948068392,
-                    "timestamp": 0.5121168374171047
-                },
-                {
-                    "x": 2.035048195511774,
-                    "y": 6.750890505964207,
-                    "heading": 0.5269052635268205,
-                    "angularVelocity": -1.1341563353588457,
-                    "velocityX": 2.418381334686234,
-                    "velocityY": 0.0553860089978706,
-                    "timestamp": 0.5852763856195482
-                },
-                {
-                    "x": 2.1675926440263233,
-                    "y": 6.753939666061267,
-                    "heading": 0.4602292179018529,
-                    "angularVelocity": -0.9113785864349585,
-                    "velocityX": 1.811717701533345,
-                    "velocityY": 0.041678224811154865,
-                    "timestamp": 0.6584359338219917
-                },
-                {
-                    "x": 2.2558825150080364,
-                    "y": 6.755978237986949,
-                    "heading": 0.41386063697230147,
-                    "angularVelocity": -0.6338008102679221,
-                    "velocityX": 1.206812687489562,
-                    "velocityY": 0.027864741865846625,
-                    "timestamp": 0.7315954820244353
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "angularVelocity": -0.3261452203924221,
-                    "velocityX": 0.6030311295783809,
-                    "velocityY": 0.013966215458624026,
-                    "timestamp": 0.8047550302268788
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "angularVelocity": 9.371559495672325e-33,
-                    "velocityX": 2.5226214414524116e-33,
-                    "velocityY": -5.278780681595662e-33,
-                    "timestamp": 0.8779145784293223
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "Amp Side To W2": {
-            "waypoints": [
-                {
-                    "x": 0.71,
-                    "y": 6.72,
-                    "heading": 1.04,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": -0.003,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 0.71,
-                    "y": 6.72,
-                    "heading": 1.04,
-                    "angularVelocity": 3.547724392998225e-32,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 0.737435339105297,
-                    "y": 6.6995140898970185,
-                    "heading": 1.00766177496316,
-                    "angularVelocity": -0.49661186995303425,
-                    "velocityX": 0.4213191985755434,
-                    "velocityY": -0.3145981609176562,
-                    "timestamp": 0.06511770457661178
-                },
-                {
-                    "x": 0.7924238483684921,
-                    "y": 6.658398285939555,
-                    "heading": 0.945783535120445,
-                    "angularVelocity": -0.95025216636614,
-                    "velocityX": 0.8444479058456402,
-                    "velocityY": -0.6314074524707902,
-                    "timestamp": 0.13023540915322357
-                },
-                {
-                    "x": 0.8751312092590968,
-                    "y": 6.596482141852294,
-                    "heading": 0.8584519121155169,
-                    "angularVelocity": -1.3411348506945964,
-                    "velocityX": 1.2701209514118896,
-                    "velocityY": -0.9508342545216032,
-                    "timestamp": 0.19535311372983535
-                },
-                {
-                    "x": 0.9857818913354864,
-                    "y": 6.513571279138628,
-                    "heading": 0.7519056782658311,
-                    "angularVelocity": -1.6362099146835547,
-                    "velocityX": 1.6992411325894867,
-                    "velocityY": -1.273246089565702,
-                    "timestamp": 0.26047081830644714
-                },
-                {
-                    "x": 1.1246151480908613,
-                    "y": 6.409448227792029,
-                    "heading": 0.6367147770436085,
-                    "angularVelocity": -1.7689644002530829,
-                    "velocityX": 2.132035483407367,
-                    "velocityY": -1.5989975694566836,
-                    "timestamp": 0.32558852288305895
-                },
-                {
-                    "x": 1.2913898404503776,
-                    "y": 6.284148794801154,
-                    "heading": 0.5378900995711877,
-                    "angularVelocity": -1.5176314662036392,
-                    "velocityX": 2.5611267080722016,
-                    "velocityY": -1.9241991683453519,
-                    "timestamp": 0.39070622745967076
-                },
-                {
-                    "x": 1.479902092849603,
-                    "y": 6.139172935751937,
-                    "heading": 0.5154398140678585,
-                    "angularVelocity": -0.3447646941687853,
-                    "velocityX": 2.8949462150871557,
-                    "velocityY": -2.2263662392867607,
-                    "timestamp": 0.4558239320362826
-                },
-                {
-                    "x": 1.6682099030328927,
-                    "y": 5.994133301373275,
-                    "heading": 0.4928532472983225,
-                    "angularVelocity": -0.34685753922672635,
-                    "velocityX": 2.8918066355017875,
-                    "velocityY": -2.2273456246913312,
-                    "timestamp": 0.5209416366128944
-                },
-                {
-                    "x": 1.8350205910276742,
-                    "y": 5.8693381796986746,
-                    "heading": 0.39723540571375665,
-                    "angularVelocity": -1.4683847074503305,
-                    "velocityX": 2.561679486083952,
-                    "velocityY": -1.9164545569596636,
-                    "timestamp": 0.5860593411895062
-                },
-                {
-                    "x": 1.9739536597530634,
-                    "y": 5.765582600106914,
-                    "heading": 0.2820330722411117,
-                    "angularVelocity": -1.7691399631126123,
-                    "velocityX": 2.1335682765342634,
-                    "velocityY": -1.5933543767607987,
-                    "timestamp": 0.651177045766118
-                },
-                {
-                    "x": 2.08468090414567,
-                    "y": 5.683000750382208,
-                    "heading": 0.1756895135649168,
-                    "angularVelocity": -1.6330974712273538,
-                    "velocityX": 1.7004168852778658,
-                    "velocityY": -1.2681935007022147,
-                    "timestamp": 0.7162947503427298
-                },
-                {
-                    "x": 2.167467858188685,
-                    "y": 5.6213477608369935,
-                    "heading": 0.08927498136102419,
-                    "angularVelocity": -1.3270512645639152,
-                    "velocityX": 1.2713432480657338,
-                    "velocityY": -0.946793041094972,
-                    "timestamp": 0.7814124549193416
-                },
-                {
-                    "x": 2.2225251033432207,
-                    "y": 5.5804064125777195,
-                    "heading": 0.028526912579098198,
-                    "angularVelocity": -0.9328963478811614,
-                    "velocityX": 0.8455034696402836,
-                    "velocityY": -0.628728370041152,
-                    "timestamp": 0.8465301594959534
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": -0.003,
-                    "angularVelocity": -0.4841527013902356,
-                    "velocityX": 0.42192667624600166,
-                    "velocityY": -0.31337733279145685,
-                    "timestamp": 0.9116478640725653
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": -0.003,
-                    "angularVelocity": -3.2592320489843035e-28,
-                    "velocityX": -3.6408651267401044e-33,
-                    "velocityY": 2.9208085569078165e-32,
-                    "timestamp": 0.9767655686491771
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "Amp Side To W3": {
-            "waypoints": [
-                {
-                    "x": 0.71,
-                    "y": 6.72,
-                    "heading": 1.04,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 14
-                },
-                {
-                    "x": 1.8406627178192139,
-                    "y": 5.925227165222168,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 11
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.393,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 0.71,
-                    "y": 6.72,
-                    "heading": 1.04,
-                    "angularVelocity": -9.490919672428962e-31,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 0.7219893017338576,
-                    "y": 6.7135407128428675,
-                    "heading": 1.0202523530723182,
-                    "angularVelocity": -0.4720627806088485,
-                    "velocityX": 0.2866013928023673,
-                    "velocityY": -0.15440771588183827,
-                    "timestamp": 0.041832670862584576
-                },
-                {
-                    "x": 0.7460365692584342,
-                    "y": 6.700508326600243,
-                    "heading": 0.982041462546113,
-                    "angularVelocity": -0.9134222065746536,
-                    "velocityX": 0.574844183475,
-                    "velocityY": -0.3115360787128853,
-                    "timestamp": 0.08366534172516915
-                },
-                {
-                    "x": 0.7822226491271561,
-                    "y": 6.680770273598179,
-                    "heading": 0.9269846511448561,
-                    "angularVelocity": -1.3161199193358153,
-                    "velocityX": 0.8650195916869635,
-                    "velocityY": -0.4718334401092994,
-                    "timestamp": 0.12549801258775373
-                },
-                {
-                    "x": 0.830641015185364,
-                    "y": 6.654177161634895,
-                    "heading": 0.8571062408800131,
-                    "angularVelocity": -1.6704266981755755,
-                    "velocityX": 1.1574294698338659,
-                    "velocityY": -0.6357019863882251,
-                    "timestamp": 0.1673306834503383
-                },
-                {
-                    "x": 0.8913887859512984,
-                    "y": 6.620557389613832,
-                    "heading": 0.7749072042273928,
-                    "angularVelocity": -1.9649483276514406,
-                    "velocityX": 1.4521609429503868,
-                    "velocityY": -0.8036726158720227,
-                    "timestamp": 0.20916335431292288
-                },
-                {
-                    "x": 0.9645435989149714,
-                    "y": 6.579696019447196,
-                    "heading": 0.6835223857085977,
-                    "angularVelocity": -2.1845322479884017,
-                    "velocityX": 1.7487483217121933,
-                    "velocityY": -0.976781288979697,
-                    "timestamp": 0.25099602517550745
-                },
-                {
-                    "x": 1.05011662601444,
-                    "y": 6.531295841858201,
-                    "heading": 0.5872583086768085,
-                    "angularVelocity": -2.301169756719744,
-                    "velocityX": 2.0456027629834046,
-                    "velocityY": -1.1569946788233771,
-                    "timestamp": 0.29282869603809203
-                },
-                {
-                    "x": 1.147914322294414,
-                    "y": 6.47495195963137,
-                    "heading": 0.4931548271581024,
-                    "angularVelocity": -2.249521237308087,
-                    "velocityX": 2.337830558350811,
-                    "velocityY": -1.3468870398426824,
-                    "timestamp": 0.3346613669006766
-                },
-                {
-                    "x": 1.256785786947157,
-                    "y": 6.410210097873117,
-                    "heading": 0.4149981322548756,
-                    "angularVelocity": -1.8683171141513,
-                    "velocityX": 2.602546344945923,
-                    "velocityY": -1.5476387336328392,
-                    "timestamp": 0.3764940377632612
-                },
-                {
-                    "x": 1.3739249067400805,
-                    "y": 6.336279355727049,
-                    "heading": 0.36626571694971094,
-                    "angularVelocity": -1.1649367420322734,
-                    "velocityX": 2.800182665307358,
-                    "velocityY": -1.7672967233894428,
-                    "timestamp": 0.41832670862584576
-                },
-                {
-                    "x": 1.4978217219933585,
-                    "y": 6.2516824812952025,
-                    "heading": 0.34588578820783056,
-                    "angularVelocity": -0.48717732627787996,
-                    "velocityX": 2.961723760365251,
-                    "velocityY": -2.022268066740437,
-                    "timestamp": 0.46015937948843033
-                },
-                {
-                    "x": 1.6219909683827523,
-                    "y": 6.153574664712983,
-                    "heading": 0.34588517926846474,
-                    "angularVelocity": -0.000014556550044979093,
-                    "velocityX": 2.9682361615699526,
-                    "velocityY": -2.345243910065211,
-                    "timestamp": 0.5019920503510149
-                },
-                {
-                    "x": 1.7365961394307947,
-                    "y": 6.044446925458561,
-                    "heading": 0.3458850779757818,
-                    "angularVelocity": -0.0000024213773805789224,
-                    "velocityX": 2.7396092261115483,
-                    "velocityY": -2.608672528056193,
-                    "timestamp": 0.5438247212135995
-                },
-                {
-                    "x": 1.840662717819214,
-                    "y": 5.925227165222168,
-                    "heading": 0.34588493628260575,
-                    "angularVelocity": -0.000003387141510558926,
-                    "velocityX": 2.4876866870453034,
-                    "velocityY": -2.849919877887629,
-                    "timestamp": 0.5856573920761841
-                },
-                {
-                    "x": 1.973177224121443,
-                    "y": 5.721056374144466,
-                    "heading": 0.3458848333105023,
-                    "angularVelocity": -0.0000016003693825600053,
-                    "velocityX": 2.0595107939713087,
-                    "velocityY": -3.173176731904646,
-                    "timestamp": 0.6500001023402711
-                },
-                {
-                    "x": 2.0754764812101274,
-                    "y": 5.5001929040123185,
-                    "heading": 0.34588474927124485,
-                    "angularVelocity": -0.0000013061193280868856,
-                    "velocityX": 1.5899121542881502,
-                    "velocityY": -3.432610613162356,
-                    "timestamp": 0.7143428126043581
-                },
-                {
-                    "x": 2.145503113949715,
-                    "y": 5.26707902131208,
-                    "heading": 0.3458846699393237,
-                    "angularVelocity": -0.0000012329589594933622,
-                    "velocityX": 1.0883382507855357,
-                    "velocityY": -3.6230037830772255,
-                    "timestamp": 0.7786855228684451
-                },
-                {
-                    "x": 2.1857192532601815,
-                    "y": 5.0438534457178905,
-                    "heading": 0.2952188838034157,
-                    "angularVelocity": -0.787436306738375,
-                    "velocityX": 0.6250302348985738,
-                    "velocityY": -3.4693219275035725,
-                    "timestamp": 0.8430282331325321
-                },
-                {
-                    "x": 2.219169026901941,
-                    "y": 4.846871449995286,
-                    "heading": 0.15700289500909007,
-                    "angularVelocity": -2.1481219586034865,
-                    "velocityX": 0.5198688943072862,
-                    "velocityY": -3.0614500836865797,
-                    "timestamp": 0.9073709433966191
-                },
-                {
-                    "x": 2.2467778858930516,
-                    "y": 4.683227688496762,
-                    "heading": -0.005808843023566441,
-                    "angularVelocity": -2.5303835875804483,
-                    "velocityX": 0.42909070627880197,
-                    "velocityY": -2.54331471004047,
-                    "timestamp": 0.9717136536607061
-                },
-                {
-                    "x": 2.268435157539554,
-                    "y": 4.553295114204883,
-                    "heading": -0.15269665079354766,
-                    "angularVelocity": -2.282897427962143,
-                    "velocityX": 0.33659246801420317,
-                    "velocityY": -2.019382984623824,
-                    "timestamp": 1.036056363924793
-                },
-                {
-                    "x": 2.2843848876654955,
-                    "y": 4.456384809980395,
-                    "heading": -0.26981466762495615,
-                    "angularVelocity": -1.820222001073865,
-                    "velocityX": 0.24788713531789203,
-                    "velocityY": -1.5061582551734227,
-                    "timestamp": 1.10039907418888
-                },
-                {
-                    "x": 2.2948521877927464,
-                    "y": 4.392049751527424,
-                    "heading": -0.3510408876957304,
-                    "angularVelocity": -1.2623997300920147,
-                    "velocityX": 0.16268043550387537,
-                    "velocityY": -0.9998810772644539,
-                    "timestamp": 1.164741784452967
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.393,
-                    "angularVelocity": -0.6521191310103414,
-                    "velocityX": 0.0800061450027687,
-                    "velocityY": -0.498110063997614,
-                    "timestamp": 1.229084494717054
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.393,
-                    "angularVelocity": -1.3043243176582522e-30,
-                    "velocityX": 1.4896025231223568e-31,
-                    "velocityY": 2.1651038615450103e-31,
-                    "timestamp": 1.293427204981141
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "Amp Side To C1": {
-            "waypoints": [
-                {
-                    "x": 0.71,
-                    "y": 6.72,
-                    "heading": 1.04,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 14
-                },
-                {
-                    "x": 1.9168630838394165,
-                    "y": 6.151812553405762,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 21
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": -0.003,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 0.71,
-                    "y": 6.72,
-                    "heading": 1.04,
-                    "angularVelocity": 1.821187546700747e-33,
-                    "velocityX": -3.3437580769683535e-34,
-                    "velocityY": -3.622513119436198e-34,
-                    "timestamp": 0
-                },
-                {
-                    "x": 0.7214806358425874,
-                    "y": 6.713430005414815,
-                    "heading": 1.0269113199415918,
-                    "angularVelocity": -0.32293003145597954,
-                    "velocityX": 0.28325561303637664,
-                    "velocityY": -0.1620979769229106,
-                    "timestamp": 0.04053100914583816
-                },
-                {
-                    "x": 0.7444896856754503,
-                    "y": 6.700284020801432,
-                    "heading": 1.001394622951622,
-                    "angularVelocity": -0.6295598734824499,
-                    "velocityX": 0.5676900308618518,
-                    "velocityY": -0.32434387621788874,
-                    "timestamp": 0.08106201829167632
-                },
-                {
-                    "x": 0.7790845165676208,
-                    "y": 6.680557699463914,
-                    "heading": 0.9642667952582056,
-                    "angularVelocity": -0.916035116713321,
-                    "velocityX": 0.8535398358252526,
-                    "velocityY": -0.4866970192264045,
-                    "timestamp": 0.12159302743751449
-                },
-                {
-                    "x": 0.8253351719483972,
-                    "y": 6.654251504147771,
-                    "heading": 0.9165522532050936,
-                    "angularVelocity": -1.177235481145466,
-                    "velocityX": 1.141117785011419,
-                    "velocityY": -0.6490387451614623,
-                    "timestamp": 0.16212403658335264
-                },
-                {
-                    "x": 0.883327254582786,
-                    "y": 6.621374965178265,
-                    "heading": 0.8595502901552765,
-                    "angularVelocity": -1.4063790724951688,
-                    "velocityX": 1.4308077656226736,
-                    "velocityY": -0.8111453344576391,
-                    "timestamp": 0.2026550457291908
-                },
-                {
-                    "x": 0.953164245540845,
-                    "y": 6.581952530815573,
-                    "heading": 0.7949436194315174,
-                    "angularVelocity": -1.5940059743218395,
-                    "velocityX": 1.7230508795567456,
-                    "velocityY": -0.9726487248526893,
-                    "timestamp": 0.24318605487502895
-                },
-                {
-                    "x": 1.0349687566216221,
-                    "y": 6.5360339228116,
-                    "heading": 0.7250282822193501,
-                    "angularVelocity": -1.724983874953593,
-                    "velocityX": 2.0183191291001217,
-                    "velocityY": -1.132925356947043,
-                    "timestamp": 0.2837170640208671
-                },
-                {
-                    "x": 1.1288774102342096,
-                    "y": 6.483723562804198,
-                    "heading": 0.6532984652847575,
-                    "angularVelocity": -1.7697515666707262,
-                    "velocityX": 2.316958190571729,
-                    "velocityY": -1.2906256495904058,
-                    "timestamp": 0.3242480731667053
-                },
-                {
-                    "x": 1.2349754296776467,
-                    "y": 6.425294263584176,
-                    "heading": 0.5860844259008366,
-                    "angularVelocity": -1.6583361924711089,
-                    "velocityX": 2.6176999211067367,
-                    "velocityY": -1.4415949775586157,
-                    "timestamp": 0.36477908231254347
-                },
-                {
-                    "x": 1.3525875597286727,
-                    "y": 6.361480505266573,
-                    "heading": 0.5370018573395918,
-                    "angularVelocity": -1.2109880705076077,
-                    "velocityX": 2.901781439189822,
-                    "velocityY": -1.5744428688658776,
-                    "timestamp": 0.40531009145838165
-                },
-                {
-                    "x": 1.481480430033789,
-                    "y": 6.296375538238497,
-                    "heading": 0.5147559483468728,
-                    "angularVelocity": -0.5488614633964374,
-                    "velocityX": 3.1801051348446676,
-                    "velocityY": -1.6063001736230227,
-                    "timestamp": 0.44584110060421983
-                },
-                {
-                    "x": 1.6221023727568105,
-                    "y": 6.235265511699909,
-                    "heading": 0.514755821640399,
-                    "angularVelocity": -0.0000031261613391085408,
-                    "velocityX": 3.469490291175273,
-                    "velocityY": -1.5077351348125194,
-                    "timestamp": 0.486372109750058
-                },
-                {
-                    "x": 1.7676170388967807,
-                    "y": 6.186949296866798,
-                    "heading": 0.5147557675556996,
-                    "angularVelocity": -0.0000013344029787740425,
-                    "velocityX": 3.5902058499550584,
-                    "velocityY": -1.1920802331681561,
-                    "timestamp": 0.5269031188958961
-                },
-                {
-                    "x": 1.9168630838394165,
-                    "y": 6.151812553405762,
-                    "heading": 0.5147557009631692,
-                    "angularVelocity": -0.000001643002030243893,
-                    "velocityX": 3.6822681716515033,
-                    "velocityY": -0.866910155989649,
-                    "timestamp": 0.5674341280417343
-                },
-                {
-                    "x": 2.2488381415683802,
-                    "y": 6.139406390407425,
-                    "heading": 0.5147556500188685,
-                    "angularVelocity": -5.801183495483463e-7,
-                    "velocityX": 3.780301616970564,
-                    "velocityY": -0.14127277622555945,
-                    "timestamp": 0.6552512102128125
-                },
-                {
-                    "x": 2.57698446655607,
-                    "y": 6.191188083838006,
-                    "heading": 0.5147556048038259,
-                    "angularVelocity": -5.148775328112685e-7,
-                    "velocityX": 3.7367026650739894,
-                    "velocityY": 0.58965399612918,
-                    "timestamp": 0.7430682923838907
-                },
-                {
-                    "x": 2.8993349699211755,
-                    "y": 6.271509362001365,
-                    "heading": 0.5147555599835898,
-                    "angularVelocity": -5.103817495962876e-7,
-                    "velocityX": 3.6707038698590346,
-                    "velocityY": 0.9146429849136081,
-                    "timestamp": 0.8308853745549689
-                },
-                {
-                    "x": 3.221685430514468,
-                    "y": 6.351830811819113,
-                    "heading": 0.514755515163351,
-                    "angularVelocity": -5.103817801462039e-7,
-                    "velocityX": 3.6707033828033078,
-                    "velocityY": 0.9146449395946943,
-                    "timestamp": 0.9187024567260471
-                },
-                {
-                    "x": 3.5440358911075664,
-                    "y": 6.432152261637641,
-                    "heading": 0.514755470343112,
-                    "angularVelocity": -5.103817833267703e-7,
-                    "velocityX": 3.670703382801096,
-                    "velocityY": 0.9146449396035715,
-                    "timestamp": 1.0065195388971253
-                },
-                {
-                    "x": 3.8663863517006654,
-                    "y": 6.512473711456168,
-                    "heading": 0.5147554255228727,
-                    "angularVelocity": -5.103817863197995e-7,
-                    "velocityX": 3.6707033828010953,
-                    "velocityY": 0.9146449396035711,
-                    "timestamp": 1.0943366210682035
-                },
-                {
-                    "x": 4.188736812293763,
-                    "y": 6.592795161274696,
-                    "heading": 0.5147553807026332,
-                    "angularVelocity": -5.103817901912557e-7,
-                    "velocityX": 3.6707033828010953,
-                    "velocityY": 0.9146449396035711,
-                    "timestamp": 1.1821537032392817
-                },
-                {
-                    "x": 4.511087272886861,
-                    "y": 6.673116611093223,
-                    "heading": 0.5147553358823933,
-                    "angularVelocity": -5.103817929337743e-7,
-                    "velocityX": 3.6707033828010958,
-                    "velocityY": 0.9146449396035705,
-                    "timestamp": 1.26997078541036
-                },
-                {
-                    "x": 4.833437733479959,
-                    "y": 6.753438060911751,
-                    "heading": 0.5147552910621531,
-                    "angularVelocity": -5.103817959943963e-7,
-                    "velocityX": 3.6707033828010953,
-                    "velocityY": 0.9146449396035703,
-                    "timestamp": 1.357787867581438
-                },
-                {
-                    "x": 5.155788194073057,
-                    "y": 6.833759510730278,
-                    "heading": 0.5147552462419127,
-                    "angularVelocity": -5.10381799453682e-7,
-                    "velocityX": 3.670703382801095,
-                    "velocityY": 0.9146449396035702,
-                    "timestamp": 1.4456049497525163
-                },
-                {
-                    "x": 5.4781386546661555,
-                    "y": 6.914080960548806,
-                    "heading": 0.514755201421672,
-                    "angularVelocity": -5.10381803359495e-7,
-                    "velocityX": 3.670703382801095,
-                    "velocityY": 0.9146449396035701,
-                    "timestamp": 1.5334220319235945
-                },
-                {
-                    "x": 5.8004891152592535,
-                    "y": 6.994402410367334,
-                    "heading": 0.5147551566014309,
-                    "angularVelocity": -5.1038180633665e-7,
-                    "velocityX": 3.670703382801095,
-                    "velocityY": 0.9146449396035699,
-                    "timestamp": 1.6212391140946727
-                },
-                {
-                    "x": 6.1228395758523515,
-                    "y": 7.074723860185862,
-                    "heading": 0.5147551117811896,
-                    "angularVelocity": -5.103818092114042e-7,
-                    "velocityX": 3.670703382801095,
-                    "velocityY": 0.9146449396035696,
-                    "timestamp": 1.709056196265751
-                },
-                {
-                    "x": 6.4451900364454335,
-                    "y": 7.1550453100044535,
-                    "heading": 0.514755066960948,
-                    "angularVelocity": -5.10381812997418e-7,
-                    "velocityX": 3.6707033828009097,
-                    "velocityY": 0.9146449396043118,
-                    "timestamp": 1.796873278436829
-                },
-                {
-                    "x": 6.767540493462307,
-                    "y": 7.235366774175193,
-                    "heading": 0.5147550221406476,
-                    "angularVelocity": -5.103824823798507e-7,
-                    "velocityX": 3.670703342077523,
-                    "velocityY": 0.9146451030365995,
-                    "timestamp": 1.8846903606079073
-                },
-                {
-                    "x": 7.069460815974501,
-                    "y": 7.313239150235045,
-                    "heading": 0.4557447568037101,
-                    "angularVelocity": -0.6719679574638842,
-                    "velocityX": 3.438059145759549,
-                    "velocityY": 0.8867565869263073,
-                    "timestamp": 1.9725074427789855
-                },
-                {
-                    "x": 7.3146543806816275,
-                    "y": 7.37536322701953,
-                    "heading": 0.2979903486408247,
-                    "angularVelocity": -1.7963977424752164,
-                    "velocityX": 2.7920941876599903,
-                    "velocityY": 0.7074258817146762,
-                    "timestamp": 2.0603245249500635
-                },
-                {
-                    "x": 7.497733370662191,
-                    "y": 7.421431644323229,
-                    "heading": 0.15573272655395068,
-                    "angularVelocity": -1.6199310950657548,
-                    "velocityX": 2.084776508787937,
-                    "velocityY": 0.5245951717451859,
-                    "timestamp": 2.1481416071211417
-                },
-                {
-                    "x": 7.619355739034295,
-                    "y": 7.45187793306489,
-                    "heading": 0.05195662802885072,
-                    "angularVelocity": -1.1817302050975884,
-                    "velocityX": 1.3849511435049664,
-                    "velocityY": 0.34670121107359286,
-                    "timestamp": 2.23595868929222
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": -0.003,
-                    "angularVelocity": -0.6258079484101811,
-                    "velocityX": 0.6905747659386144,
-                    "velocityY": 0.1721996058312404,
-                    "timestamp": 2.323775771463298
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": -0.003,
-                    "angularVelocity": 0,
-                    "velocityX": 2.3316560699920504e-34,
-                    "velocityY": 0,
-                    "timestamp": 2.4115928536343763
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "Amp Side To C2": {
-            "waypoints": [
-                {
-                    "x": 0.71,
-                    "y": 6.72,
-                    "heading": 1.04,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 14
-                },
-                {
-                    "x": 2.159510374069214,
-                    "y": 6.264003276824951,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 15
-                },
-                {
-                    "x": 5.9063591957092285,
-                    "y": 6.791097164154053,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 10
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": -0.003,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 0.71,
-                    "y": 6.72,
-                    "heading": 1.04,
-                    "angularVelocity": -7.919640222280865e-31,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 0.7245966822673344,
-                    "y": 6.714849739719678,
-                    "heading": 1.024358911223375,
-                    "angularVelocity": -0.35643014194665995,
-                    "velocityX": 0.33263013891161514,
-                    "velocityY": -0.1173644641361684,
-                    "timestamp": 0.04388262084457728
-                },
-                {
-                    "x": 0.7538441442080397,
-                    "y": 6.704534883949825,
-                    "heading": 0.9938971402916456,
-                    "angularVelocity": -0.6941648047781096,
-                    "velocityX": 0.6664930530083778,
-                    "velocityY": -0.23505559994630235,
-                    "timestamp": 0.08776524168915456
-                },
-                {
-                    "x": 0.7978084886925392,
-                    "y": 6.689039857166789,
-                    "heading": 0.9496619935965729,
-                    "angularVelocity": -1.0080333818652591,
-                    "velocityX": 1.0018623235886674,
-                    "velocityY": -0.3531016717960754,
-                    "timestamp": 0.13164786253373184
-                },
-                {
-                    "x": 0.856569122575896,
-                    "y": 6.668349449234606,
-                    "heading": 0.8929800253175195,
-                    "angularVelocity": -1.291672356576085,
-                    "velocityX": 1.3390411227143098,
-                    "velocityY": -0.4714943532072734,
-                    "timestamp": 0.17553048337830912
-                },
-                {
-                    "x": 0.9302192448226602,
-                    "y": 6.642448160442897,
-                    "heading": 0.8255526698192454,
-                    "angularVelocity": -1.5365389350167389,
-                    "velocityX": 1.6783437458676926,
-                    "velocityY": -0.5902402430203169,
-                    "timestamp": 0.2194131042228864
-                },
-                {
-                    "x": 1.018865391503866,
-                    "y": 6.6113159895257,
-                    "heading": 0.7496899973128085,
-                    "angularVelocity": -1.728763484184805,
-                    "velocityX": 2.020074119893375,
-                    "velocityY": -0.7094419229748447,
-                    "timestamp": 0.2632957250674637
-                },
-                {
-                    "x": 1.122620892464334,
-                    "y": 6.574924283447733,
-                    "heading": 0.6689630682339129,
-                    "angularVelocity": -1.839610477341524,
-                    "velocityX": 2.364387061747502,
-                    "velocityY": -0.8292965501492129,
-                    "timestamp": 0.30717834591204096
-                },
-                {
-                    "x": 1.2415335684286195,
-                    "y": 6.533261700289126,
-                    "heading": 0.5901648629283005,
-                    "angularVelocity": -1.7956585953398425,
-                    "velocityX": 2.7097897453635067,
-                    "velocityY": -0.9494096377266702,
-                    "timestamp": 0.35106096675661824
-                },
-                {
-                    "x": 1.3748018786634473,
-                    "y": 6.486525528750173,
-                    "heading": 0.5293493772235701,
-                    "angularVelocity": -1.3858672188271242,
-                    "velocityX": 3.03692686694496,
-                    "velocityY": -1.0650268976507753,
-                    "timestamp": 0.3949435876011955
-                },
-                {
-                    "x": 1.5197726766571482,
-                    "y": 6.4335443318697845,
-                    "heading": 0.5007639504532418,
-                    "angularVelocity": -0.6514065527572436,
-                    "velocityX": 3.3036039143420037,
-                    "velocityY": -1.2073389387557742,
-                    "timestamp": 0.4388262084457728
-                },
-                {
-                    "x": 1.6765216724762246,
-                    "y": 6.378886133078717,
-                    "heading": 0.5007639037196894,
-                    "angularVelocity": -0.0000010649672122148667,
-                    "velocityX": 3.572006247627606,
-                    "velocityY": -1.2455545666831078,
-                    "timestamp": 0.4827088292903501
-                },
-                {
-                    "x": 1.8335562881896872,
-                    "y": 6.325054024772622,
-                    "heading": 0.5007638569717894,
-                    "angularVelocity": -0.0000010652941670826544,
-                    "velocityX": 3.5785149722331555,
-                    "velocityY": -1.2267295633425939,
-                    "timestamp": 0.5265914501349274
-                },
-                {
-                    "x": 1.9950553751457016,
-                    "y": 6.286637737718841,
-                    "heading": 0.5007638092073705,
-                    "angularVelocity": -0.0000010884586644439279,
-                    "velocityX": 3.6802516314615477,
-                    "velocityY": -0.8754328322739401,
-                    "timestamp": 0.5704740709795046
-                },
-                {
-                    "x": 2.159510374069214,
-                    "y": 6.264003276824951,
-                    "heading": 0.500763757205963,
-                    "angularVelocity": -0.0000011850114334401085,
-                    "velocityX": 3.7476111444210547,
-                    "velocityY": -0.5157955577412445,
-                    "timestamp": 0.6143566918240819
-                },
-                {
-                    "x": 2.4127218287177645,
-                    "y": 6.26676408374459,
-                    "heading": 0.5007637073683532,
-                    "angularVelocity": -7.445219928061725e-7,
-                    "velocityX": 3.7827154561393628,
-                    "velocityY": 0.04124358046976413,
-                    "timestamp": 0.681295761808209
-                },
-                {
-                    "x": 2.6627712176567337,
-                    "y": 6.306751164644663,
-                    "heading": 0.5007636590304977,
-                    "angularVelocity": -7.221172264287654e-7,
-                    "velocityX": 3.7354774871878274,
-                    "velocityY": 0.5973653489565757,
-                    "timestamp": 0.7482348317923362
-                },
-                {
-                    "x": 2.9117460938747426,
-                    "y": 6.352959114176208,
-                    "heading": 0.5007636106421453,
-                    "angularVelocity": -7.22871595873179e-7,
-                    "velocityX": 3.719425386056917,
-                    "velocityY": 0.6902986483459334,
-                    "timestamp": 0.8151739017764633
-                },
-                {
-                    "x": 3.160720959556854,
-                    "y": 6.399167120476624,
-                    "heading": 0.5007635622537979,
-                    "angularVelocity": -7.228715220114282e-7,
-                    "velocityX": 3.719425228661581,
-                    "velocityY": 0.6902994964137207,
-                    "timestamp": 0.8821129717605904
-                },
-                {
-                    "x": 3.409695827610573,
-                    "y": 6.445375113998521,
-                    "heading": 0.5007635138654579,
-                    "angularVelocity": -7.228714109969877e-7,
-                    "velocityX": 3.719425264090935,
-                    "velocityY": 0.6902993055159711,
-                    "timestamp": 0.9490520417447176
-                },
-                {
-                    "x": 3.658670698533997,
-                    "y": 6.491583092058085,
-                    "heading": 0.5007634654771249,
-                    "angularVelocity": -7.228713077638952e-7,
-                    "velocityX": 3.719425306961334,
-                    "velocityY": 0.6902990745248123,
-                    "timestamp": 1.0159911117288447
-                },
-                {
-                    "x": 3.9076455729725814,
-                    "y": 6.537791051177508,
-                    "heading": 0.5007634170887977,
-                    "angularVelocity": -7.228712197353323e-7,
-                    "velocityX": 3.7194253594741626,
-                    "velocityY": 0.6902987915788225,
-                    "timestamp": 1.082930181712972
-                },
-                {
-                    "x": 4.156620451761658,
-                    "y": 6.583998986855888,
-                    "heading": 0.5007633687004744,
-                    "angularVelocity": -7.228711615866084e-7,
-                    "velocityX": 3.7194254244659963,
-                    "velocityY": 0.6902984413940524,
-                    "timestamp": 1.1498692516970992
-                },
-                {
-                    "x": 4.405595335954841,
-                    "y": 6.630206893416186,
-                    "heading": 0.5007633203121516,
-                    "angularVelocity": -7.22871154439704e-7,
-                    "velocityX": 3.719425505197721,
-                    "velocityY": 0.6902980064002329,
-                    "timestamp": 1.2168083216812264
-                },
-                {
-                    "x": 4.65457022677848,
-                    "y": 6.676414764250624,
-                    "heading": 0.5007632719238245,
-                    "angularVelocity": -7.228712197620323e-7,
-                    "velocityX": 3.719425604249829,
-                    "velocityY": 0.690297472693087,
-                    "timestamp": 1.2837473916653537
-                },
-                {
-                    "x": 4.903545125380086,
-                    "y": 6.722622593176218,
-                    "heading": 0.5007632235354964,
-                    "angularVelocity": -7.228712343344299e-7,
-                    "velocityX": 3.7194257204445353,
-                    "velocityY": 0.69029684661859,
-                    "timestamp": 1.350686461649481
-                },
-                {
-                    "x": 5.152520035964963,
-                    "y": 6.768830357534066,
-                    "heading": 0.500763175147177,
-                    "angularVelocity": -7.228711020086952e-7,
-                    "velocityX": 3.7194258994621494,
-                    "velocityY": 0.6902958820437133,
-                    "timestamp": 1.4176255316336082
-                },
-                {
-                    "x": 5.401781723000098,
-                    "y": 6.813465264887065,
-                    "heading": 0.5007631267776413,
-                    "angularVelocity": -7.225904944160688e-7,
-                    "velocityX": 3.7237100410005364,
-                    "velocityY": 0.6667990362518919,
-                    "timestamp": 1.4845646016177354
-                },
-                {
-                    "x": 5.654897775782049,
-                    "y": 6.8209436894170015,
-                    "heading": 0.5007630775196736,
-                    "angularVelocity": -7.358627453849351e-7,
-                    "velocityX": 3.781290251597022,
-                    "velocityY": 0.11171987498067794,
-                    "timestamp": 1.5515036716018626
-                },
-                {
-                    "x": 5.9063591957092285,
-                    "y": 6.791097164154053,
-                    "heading": 0.500763020915307,
-                    "angularVelocity": -8.456102929151622e-7,
-                    "velocityX": 3.756571759763139,
-                    "velocityY": -0.44587600739018746,
-                    "timestamp": 1.6184427415859899
-                },
-                {
-                    "x": 6.200798567763329,
-                    "y": 6.700713487178965,
-                    "heading": 0.5007629755779084,
-                    "angularVelocity": -5.568470838728409e-7,
-                    "velocityX": 3.616389792279645,
-                    "velocityY": -1.1101185433221765,
-                    "timestamp": 1.6998607824959264
-                },
-                {
-                    "x": 6.474342410112796,
-                    "y": 6.559159535827226,
-                    "heading": 0.5007629354438458,
-                    "angularVelocity": -4.929382012989922e-7,
-                    "velocityX": 3.3597448340971368,
-                    "velocityY": -1.7386066990769444,
-                    "timestamp": 1.781278823405863
-                },
-                {
-                    "x": 6.734766818786864,
-                    "y": 6.394712333057686,
-                    "heading": 0.5007628957809265,
-                    "angularVelocity": -4.871514819255018e-7,
-                    "velocityX": 3.1986081433983946,
-                    "velocityY": -2.0197882549329598,
-                    "timestamp": 1.8626968643157995
-                },
-                {
-                    "x": 6.9951910734023155,
-                    "y": 6.230264886316009,
-                    "heading": 0.5007628561176116,
-                    "angularVelocity": -4.871563403692846e-7,
-                    "velocityX": 3.198606251205787,
-                    "velocityY": -2.019791251469529,
-                    "timestamp": 1.944114905225736
-                },
-                {
-                    "x": 7.223286683326485,
-                    "y": 6.0852317833380125,
-                    "heading": 0.4070504938542789,
-                    "angularVelocity": -1.1510024217727797,
-                    "velocityX": 2.801536457705794,
-                    "velocityY": -1.781338648745319,
-                    "timestamp": 2.0255329461356726
-                },
-                {
-                    "x": 7.40663062232063,
-                    "y": 5.969243166944327,
-                    "heading": 0.26222076447726717,
-                    "angularVelocity": -1.7788407551738947,
-                    "velocityX": 2.2518834516904875,
-                    "velocityY": -1.4246058379369684,
-                    "timestamp": 2.106950987045609
-                },
-                {
-                    "x": 7.543566910533025,
-                    "y": 5.882836756425157,
-                    "heading": 0.1356651998410208,
-                    "angularVelocity": -1.5543921620054602,
-                    "velocityX": 1.6818912206923584,
-                    "velocityY": -1.061268602799615,
-                    "timestamp": 2.1883690279555457
-                },
-                {
-                    "x": 7.634586620516168,
-                    "y": 5.825537139087579,
-                    "heading": 0.04464428184824825,
-                    "angularVelocity": -1.1179453223820228,
-                    "velocityX": 1.1179304852572838,
-                    "velocityY": -0.7037705242866615,
-                    "timestamp": 2.269787068865482
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": -0.003,
-                    "angularVelocity": -0.5851808925364741,
-                    "velocityX": 0.5577803024524691,
-                    "velocityY": -0.35050142165846687,
-                    "timestamp": 2.3512051097754187
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": -0.003,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 2.4326231506853553
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "Amp Side To C3": {
-            "waypoints": [
-                {
-                    "x": 0.71,
-                    "y": 6.72,
-                    "heading": 1.04,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 16
-                },
-                {
-                    "x": 2.817133903503418,
-                    "y": 6.303859233856201,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 29
-                },
-                {
-                    "x": 4.900439739227295,
-                    "y": 4.161101341247559,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 28
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": -0.003,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 0.71,
-                    "y": 6.72,
-                    "heading": 1.04,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 0.7276374646295759,
-                    "y": 6.716389445106627,
-                    "heading": 1.0769704869840742,
-                    "angularVelocity": 0.7463572831464532,
-                    "velocityX": 0.35606374858489526,
-                    "velocityY": -0.07288959818107699,
-                    "timestamp": 0.0495345698619811
-                },
-                {
-                    "x": 0.7632753901023804,
-                    "y": 6.70941368019147,
-                    "heading": 1.1486786527951847,
-                    "angularVelocity": 1.4476388108510687,
-                    "velocityX": 0.7194556361777777,
-                    "velocityY": -0.1408261933931805,
-                    "timestamp": 0.0990691397239622
-                },
-                {
-                    "x": 0.8173493088554353,
-                    "y": 6.699320044331327,
-                    "heading": 1.251948933720564,
-                    "angularVelocity": 2.084812308111838,
-                    "velocityX": 1.0916400183503512,
-                    "velocityY": -0.20376952677844481,
-                    "timestamp": 0.1486037095859433
-                },
-                {
-                    "x": 0.8904201618241163,
-                    "y": 6.686277798850656,
-                    "heading": 1.3814946265878076,
-                    "angularVelocity": 2.615258257579252,
-                    "velocityX": 1.4751486320027993,
-                    "velocityY": -0.263295825859426,
-                    "timestamp": 0.1981382794479244
-                },
-                {
-                    "x": 0.9831303708035419,
-                    "y": 6.670301294322549,
-                    "heading": 1.5290667264747733,
-                    "angularVelocity": 2.9791739445430228,
-                    "velocityX": 1.871626406322422,
-                    "velocityY": -0.32253241669944677,
-                    "timestamp": 0.2476728493099055
-                },
-                {
-                    "x": 1.0960137924135722,
-                    "y": 6.651369432356597,
-                    "heading": 1.6827078385240544,
-                    "angularVelocity": 3.1016946846903584,
-                    "velocityX": 2.278881636090858,
-                    "velocityY": -0.3821949401851956,
-                    "timestamp": 0.2972074191718866
-                },
-                {
-                    "x": 1.2286250246243187,
-                    "y": 6.629796552300239,
-                    "heading": 1.8192744268134131,
-                    "angularVelocity": 2.7569955421025307,
-                    "velocityX": 2.677145124715176,
-                    "velocityY": -0.4355116056504451,
-                    "timestamp": 0.34674198903386766
-                },
-                {
-                    "x": 1.37806258880499,
-                    "y": 6.607677950088394,
-                    "heading": 1.9114236020956594,
-                    "angularVelocity": 1.8603003021733433,
-                    "velocityX": 3.0168337909701455,
-                    "velocityY": -0.446528601597435,
-                    "timestamp": 0.3962765588958488
-                },
-                {
-                    "x": 1.5443963470492121,
-                    "y": 6.584716640433465,
-                    "heading": 1.9605466016146158,
-                    "angularVelocity": 0.991691250288047,
-                    "velocityX": 3.3579328276762688,
-                    "velocityY": -0.4635411131687398,
-                    "timestamp": 0.4458111287578299
-                },
-                {
-                    "x": 1.7276120279383835,
-                    "y": 6.5605525122280195,
-                    "heading": 1.9670840049159166,
-                    "angularVelocity": 0.13197658361397824,
-                    "velocityX": 3.698743754106468,
-                    "velocityY": -0.4878235194494175,
-                    "timestamp": 0.49534569861981104
-                },
-                {
-                    "x": 1.9125583307039895,
-                    "y": 6.530411390267246,
-                    "heading": 1.9670841172507754,
-                    "angularVelocity": 0.0000022678072915558937,
-                    "velocityX": 3.7336814124171944,
-                    "velocityY": -0.6084865992338514,
-                    "timestamp": 0.5448802684817922
-                },
-                {
-                    "x": 2.0974975773537823,
-                    "y": 6.500227003772903,
-                    "heading": 1.967084229554738,
-                    "angularVelocity": 0.0000022671835628273576,
-                    "velocityX": 3.733538964104325,
-                    "velocityY": -0.6093600202458971,
-                    "timestamp": 0.5944148383437733
-                },
-                {
-                    "x": 2.2824570922079714,
-                    "y": 6.470167063452025,
-                    "heading": 1.9670843418904687,
-                    "angularVelocity": 0.0000022678248970866596,
-                    "velocityX": 3.7339481370144885,
-                    "velocityY": -0.6068477107210625,
-                    "timestamp": 0.6439494082057544
-                },
-                {
-                    "x": 2.4663667602317028,
-                    "y": 6.434238576599238,
-                    "heading": 1.9670844533817493,
-                    "angularVelocity": 0.000002250777202166417,
-                    "velocityX": 3.712753911780929,
-                    "velocityY": -0.7253214664890316,
-                    "timestamp": 0.6934839780677355
-                },
-                {
-                    "x": 2.6453124624138216,
-                    "y": 6.3786323067408315,
-                    "heading": 1.96708456679077,
-                    "angularVelocity": 0.000002289492388953058,
-                    "velocityX": 3.6125417598312857,
-                    "velocityY": -1.1225750019579073,
-                    "timestamp": 0.7430185479297167
-                },
-                {
-                    "x": 2.817133903503418,
-                    "y": 6.303859233856201,
-                    "heading": 1.96708468933932,
-                    "angularVelocity": 0.0000024740004860391374,
-                    "velocityX": 3.468717737283832,
-                    "velocityY": -1.5095129137971557,
-                    "timestamp": 0.7925531177916978
-                },
-                {
-                    "x": 2.91295588800905,
-                    "y": 6.254800700059255,
-                    "heading": 1.967084804070116,
-                    "angularVelocity": 0.000004031753460568297,
-                    "velocityX": 3.367279151013272,
-                    "velocityY": -1.7239653184522483,
-                    "timestamp": 0.8210099163546827
-                },
-                {
-                    "x": 3.0055158966135553,
-                    "y": 6.199834910977839,
-                    "heading": 1.9670849118890987,
-                    "angularVelocity": 0.000003788865518014884,
-                    "velocityX": 3.2526500969440626,
-                    "velocityY": -1.9315521020251178,
-                    "timestamp": 0.8494667149176676
-                },
-                {
-                    "x": 3.0944547185216633,
-                    "y": 6.139184440666953,
-                    "heading": 1.96708501618758,
-                    "angularVelocity": 0.0000036651516103387324,
-                    "velocityX": 3.125398020841055,
-                    "velocityY": -2.13131741353383,
-                    "timestamp": 0.8779235134806525
-                },
-                {
-                    "x": 3.179446617500541,
-                    "y": 6.0731165142841,
-                    "heading": 1.9670851198382022,
-                    "angularVelocity": 0.0000036423852113204396,
-                    "velocityX": 2.986699251881383,
-                    "velocityY": -2.3216921691473744,
-                    "timestamp": 0.9063803120436373
-                },
-                {
-                    "x": 3.2604149841370136,
-                    "y": 6.002174866532259,
-                    "heading": 1.9670852253780247,
-                    "angularVelocity": 0.0000037087735735672327,
-                    "velocityX": 2.845308352497901,
-                    "velocityY": -2.4929595504099007,
-                    "timestamp": 0.9348371106066222
-                },
-                {
-                    "x": 3.3400950350688365,
-                    "y": 5.929789210280793,
-                    "heading": 1.9670853319364994,
-                    "angularVelocity": 0.000003744570018753785,
-                    "velocityX": 2.8000356665421786,
-                    "velocityY": -2.5437034349136662,
-                    "timestamp": 0.9632939091696071
-                },
-                {
-                    "x": 3.4197035202865202,
-                    "y": 5.857324854756433,
-                    "heading": 1.9670854385695549,
-                    "angularVelocity": 0.0000037471908580905978,
-                    "velocityX": 2.7975207766800336,
-                    "velocityY": -2.5464690050744796,
-                    "timestamp": 0.991750707732592
-                },
-                {
-                    "x": 3.4993494510128746,
-                    "y": 5.7849016577830055,
-                    "heading": 1.9670855451672207,
-                    "angularVelocity": 0.0000037459472354931967,
-                    "velocityX": 2.7988366488248966,
-                    "velocityY": -2.5450226529586133,
-                    "timestamp": 1.0202075062955769
-                },
-                {
-                    "x": 3.579011291529686,
-                    "y": 5.712495961052324,
-                    "heading": 1.96708565174968,
-                    "angularVelocity": 0.0000037454128599628807,
-                    "velocityX": 2.7993957345647376,
-                    "velocityY": -2.544407677155567,
-                    "timestamp": 1.0486643048585618
-                },
-                {
-                    "x": 3.658639843469943,
-                    "y": 5.640053656617691,
-                    "heading": 1.9670857583635384,
-                    "angularVelocity": 0.000003746516260337957,
-                    "velocityX": 2.7982259411140946,
-                    "velocityY": -2.545694108020349,
-                    "timestamp": 1.0771211034215467
-                },
-                {
-                    "x": 3.7382184197836335,
-                    "y": 5.567556457074997,
-                    "heading": 1.9670858650249143,
-                    "angularVelocity": 0.0000037481860642492875,
-                    "velocityX": 2.796469748251449,
-                    "velocityY": -2.547623176320097,
-                    "timestamp": 1.1055779019845315
-                },
-                {
-                    "x": 3.81773211309123,
-                    "y": 5.494988100908759,
-                    "heading": 1.967085971748553,
-                    "angularVelocity": 0.0000037503740493629023,
-                    "velocityX": 2.794189695354055,
-                    "velocityY": -2.5501236903290603,
-                    "timestamp": 1.1340347005475164
-                },
-                {
-                    "x": 3.8971703522513814,
-                    "y": 5.422337155346324,
-                    "heading": 1.967086078550155,
-                    "angularVelocity": 0.000003753113746455045,
-                    "velocityX": 2.7915381621131705,
-                    "velocityY": -2.5530259632560406,
-                    "timestamp": 1.1624914991105013
-                },
-                {
-                    "x": 3.9762575655393007,
-                    "y": 5.349304242205494,
-                    "heading": 1.9670861858107123,
-                    "angularVelocity": 0.0000037692418878753097,
-                    "velocityX": 2.779202766357265,
-                    "velocityY": -2.566448681118608,
-                    "timestamp": 1.1909482976734862
-                },
-                {
-                    "x": 4.0513085153658634,
-                    "y": 5.27212945123473,
-                    "heading": 1.9670862984844089,
-                    "angularVelocity": 0.000003959464953334102,
-                    "velocityX": 2.637364482882678,
-                    "velocityY": -2.7119983577895854,
-                    "timestamp": 1.219405096236471
-                },
-                {
-                    "x": 4.121436785634375,
-                    "y": 5.190455596279305,
-                    "heading": 1.9670864209307273,
-                    "angularVelocity": 0.000004302884530781832,
-                    "velocityX": 2.4643766625150745,
-                    "velocityY": -2.8700999086264516,
-                    "timestamp": 1.247861894799456
-                },
-                {
-                    "x": 4.18632086285232,
-                    "y": 5.104556536444466,
-                    "heading": 1.9670865586915784,
-                    "angularVelocity": 0.0000048410523378145445,
-                    "velocityX": 2.280090540555213,
-                    "velocityY": -3.0185777800939864,
-                    "timestamp": 1.2763186933624409
-                },
-                {
-                    "x": 4.245696839289754,
-                    "y": 5.014761859934199,
-                    "heading": 1.9670867202387443,
-                    "angularVelocity": 0.000005676926915437751,
-                    "velocityX": 2.086530440380401,
-                    "velocityY": -3.1554735966350926,
-                    "timestamp": 1.3047754919254257
-                },
-                {
-                    "x": 4.299328195356381,
-                    "y": 4.921422294262094,
-                    "heading": 1.967086761089818,
-                    "angularVelocity": 0.0000014355470678059053,
-                    "velocityX": 1.8846588082611082,
-                    "velocityY": -3.2800445020371574,
-                    "timestamp": 1.3332322904884106
-                },
-                {
-                    "x": 4.347003427577455,
-                    "y": 4.824904615507251,
-                    "heading": 1.9670865846023797,
-                    "angularVelocity": -0.000006201942843803764,
-                    "velocityX": 1.6753547351988247,
-                    "velocityY": -3.391726533861692,
-                    "timestamp": 1.3616890890513955
-                },
-                {
-                    "x": 4.391601867669633,
-                    "y": 4.730655732437251,
-                    "heading": 1.956126696369797,
-                    "angularVelocity": -0.3851412943835458,
-                    "velocityX": 1.5672332217368503,
-                    "velocityY": -3.3119988132677265,
-                    "timestamp": 1.3901458876143804
-                },
-                {
-                    "x": 4.4407405541886815,
-                    "y": 4.641134625282056,
-                    "heading": 1.9393492777589223,
-                    "angularVelocity": -0.5895750561568437,
-                    "velocityX": 1.7267819642570823,
-                    "velocityY": -3.145860099373803,
-                    "timestamp": 1.4186026861773653
-                },
-                {
-                    "x": 4.494270192698628,
-                    "y": 4.5565595160525465,
-                    "heading": 1.9177097235934124,
-                    "angularVelocity": -0.7604353004627978,
-                    "velocityX": 1.8810843528824521,
-                    "velocityY": -2.97205284854274,
-                    "timestamp": 1.4470594847403502
-                },
-                {
-                    "x": 4.552047682392307,
-                    "y": 4.477111099902014,
-                    "heading": 1.8920595280024661,
-                    "angularVelocity": -0.901373200296037,
-                    "velocityX": 2.030358037844879,
-                    "velocityY": -2.791895791605024,
-                    "timestamp": 1.475516283303335
-                },
-                {
-                    "x": 4.613941492209361,
-                    "y": 4.402936898219151,
-                    "heading": 1.8631628723317806,
-                    "angularVelocity": -1.0154570130820861,
-                    "velocityX": 2.175009591471355,
-                    "velocityY": -2.6065546874046253,
-                    "timestamp": 1.50397308186632
-                },
-                {
-                    "x": 4.679832286793745,
-                    "y": 4.334157239741354,
-                    "heading": 1.831710656249818,
-                    "angularVelocity": -1.105261929317901,
-                    "velocityX": 2.315467582853696,
-                    "velocityY": -2.416985112558446,
-                    "timestamp": 1.5324298804293048
-                },
-                {
-                    "x": 4.749611538460051,
-                    "y": 4.270871230504256,
-                    "heading": 1.798335587748108,
-                    "angularVelocity": -1.1728328619898605,
-                    "velocityX": 2.4521118042094026,
-                    "velocityY": -2.223932853761988,
-                    "timestamp": 1.5608866789922897
-                },
-                {
-                    "x": 4.82317903379308,
-                    "y": 4.213162042716923,
-                    "heading": 1.7636285592897518,
-                    "angularVelocity": -1.2196392500559503,
-                    "velocityX": 2.585234427203579,
-                    "velocityY": -2.0279578414139063,
-                    "timestamp": 1.5893434775552746
-                },
-                {
-                    "x": 4.900439739227295,
-                    "y": 4.161101341247559,
-                    "heading": 1.7281561592052472,
-                    "angularVelocity": -1.2465351647339973,
-                    "velocityX": 2.715017476867699,
-                    "velocityY": -1.8294644548346262,
-                    "timestamp": 1.6178002761182595
-                },
-                {
-                    "x": 5.010449341197396,
-                    "y": 4.1015961278120825,
-                    "heading": 1.6804127870661807,
-                    "angularVelocity": -1.251568549956003,
-                    "velocityX": 2.8838465288429167,
-                    "velocityY": -1.5598993191587458,
-                    "timestamp": 1.6559471056633712
-                },
-                {
-                    "x": 5.126648667318374,
-                    "y": 4.05252161275018,
-                    "heading": 1.6338488947780365,
-                    "angularVelocity": -1.2206490773575045,
-                    "velocityX": 3.0461070423551457,
-                    "velocityY": -1.2864637939008783,
-                    "timestamp": 1.694093935208483
-                },
-                {
-                    "x": 5.248756740712563,
-                    "y": 4.014017499979329,
-                    "heading": 1.589989878848282,
-                    "angularVelocity": -1.1497421005375177,
-                    "velocityX": 3.2010018879727395,
-                    "velocityY": -1.0093660005294856,
-                    "timestamp": 1.7322407647535947
-                },
-                {
-                    "x": 5.376454980725802,
-                    "y": 3.9862068849459353,
-                    "heading": 1.5506775545962859,
-                    "angularVelocity": -1.0305528590747366,
-                    "velocityX": 3.347545301559156,
-                    "velocityY": -0.7290413217820071,
-                    "timestamp": 1.7703875942987064
-                },
-                {
-                    "x": 5.509414833155046,
-                    "y": 3.969152206173251,
-                    "heading": 1.5183946275971996,
-                    "angularVelocity": -0.8462807364186931,
-                    "velocityX": 3.485475831533742,
-                    "velocityY": -0.4470798484702507,
-                    "timestamp": 1.8085344238438181
-                },
-                {
-                    "x": 5.647435915880888,
-                    "y": 3.96261903739833,
-                    "heading": 1.4980750155673637,
-                    "angularVelocity": -0.5326684359468331,
-                    "velocityX": 3.618153444771329,
-                    "velocityY": -0.17126374204166636,
-                    "timestamp": 1.8466812533889299
-                },
-                {
-                    "x": 5.791726765964232,
-                    "y": 3.964767379969892,
-                    "heading": 1.498073903211314,
-                    "angularVelocity": -0.00002915985583373997,
-                    "velocityX": 3.7825122507942877,
-                    "velocityY": 0.056317722783366074,
-                    "timestamp": 1.8848280829340416
-                },
-                {
-                    "x": 5.935369435750422,
-                    "y": 3.9785994401105067,
-                    "heading": 1.4980736722429098,
-                    "angularVelocity": -0.00000605472084526459,
-                    "velocityX": 3.765520529467262,
-                    "velocityY": 0.3626005176676574,
-                    "timestamp": 1.9229749124791533
-                },
-                {
-                    "x": 6.078764518036592,
-                    "y": 3.9947980029743344,
-                    "heading": 1.498073449658831,
-                    "angularVelocity": -0.000005834929965972279,
-                    "velocityX": 3.7590301473577066,
-                    "velocityY": 0.42463719939010114,
-                    "timestamp": 1.961121742024265
-                },
-                {
-                    "x": 6.222150100858587,
-                    "y": 4.0110804389753145,
-                    "heading": 1.4980732273393433,
-                    "angularVelocity": -0.000005827993841710607,
-                    "velocityX": 3.7587811236710817,
-                    "velocityY": 0.4268358915939984,
-                    "timestamp": 1.9992685715693768
-                },
-                {
-                    "x": 6.3655338107868324,
-                    "y": 4.0273793596330805,
-                    "heading": 1.4980730050704238,
-                    "angularVelocity": -0.000005826668223171757,
-                    "velocityX": 3.758732026701189,
-                    "velocityY": 0.42726802860026514,
-                    "timestamp": 2.0374154011144885
-                },
-                {
-                    "x": 6.508922861230399,
-                    "y": 4.043631229943055,
-                    "heading": 1.4980727826574463,
-                    "angularVelocity": -0.000005830444635518525,
-                    "velocityX": 3.7588720256305224,
-                    "velocityY": 0.42603462734214087,
-                    "timestamp": 2.0755622306596
-                },
-                {
-                    "x": 6.6523224677448685,
-                    "y": 4.059789689420356,
-                    "heading": 1.4980725589565675,
-                    "angularVelocity": -0.000005864206314008162,
-                    "velocityX": 3.759148747731203,
-                    "velocityY": 0.4235859092458616,
-                    "timestamp": 2.1137090602047115
-                },
-                {
-                    "x": 6.792869039761006,
-                    "y": 4.068475137054533,
-                    "heading": 1.4874839096219183,
-                    "angularVelocity": -0.2775761304689174,
-                    "velocityX": 3.684357879593819,
-                    "velocityY": 0.2276846526483585,
-                    "timestamp": 2.151855889749823
-                },
-                {
-                    "x": 6.923724640852256,
-                    "y": 4.075654653912489,
-                    "heading": 1.4487910280540974,
-                    "angularVelocity": -1.014314479844837,
-                    "velocityX": 3.430313938317647,
-                    "velocityY": 0.18820743280285396,
-                    "timestamp": 2.1900027192949345
-                },
-                {
-                    "x": 7.04453293582936,
-                    "y": 4.081916427546979,
-                    "heading": 1.3838287724616933,
-                    "angularVelocity": -1.7029529417506197,
-                    "velocityX": 3.1669288488112004,
-                    "velocityY": 0.1641492545881335,
-                    "timestamp": 2.228149548840046
-                },
-                {
-                    "x": 7.155015675902827,
-                    "y": 4.087769066352678,
-                    "heading": 1.2944912667640571,
-                    "angularVelocity": -2.3419378953075096,
-                    "velocityX": 2.896249606872667,
-                    "velocityY": 0.1534239902944478,
-                    "timestamp": 2.2662963783851575
-                },
-                {
-                    "x": 7.255027442080941,
-                    "y": 4.093665982164644,
-                    "heading": 1.1821266374518384,
-                    "angularVelocity": -2.945582389205927,
-                    "velocityX": 2.621758279016409,
-                    "velocityY": 0.15458468980091475,
-                    "timestamp": 2.304443207930269
-                },
-                {
-                    "x": 7.344594486390613,
-                    "y": 4.100041344791564,
-                    "heading": 1.0471923484078691,
-                    "angularVelocity": -3.5372346969048754,
-                    "velocityX": 2.3479551348760803,
-                    "velocityY": 0.16712693303897977,
-                    "timestamp": 2.3425900374753805
-                },
-                {
-                    "x": 7.423972884493799,
-                    "y": 4.107363761305008,
-                    "heading": 0.8891062073498277,
-                    "angularVelocity": -4.144148883225662,
-                    "velocityX": 2.080864885752389,
-                    "velocityY": 0.19195347557968287,
-                    "timestamp": 2.380736867020492
-                },
-                {
-                    "x": 7.492968640563269,
-                    "y": 4.113132211800853,
-                    "heading": 0.7092469399580156,
-                    "angularVelocity": -4.714920467480658,
-                    "velocityX": 1.8086891333366921,
-                    "velocityY": 0.1512170359820358,
-                    "timestamp": 2.4188836965656035
-                },
-                {
-                    "x": 7.549873361136887,
-                    "y": 4.117954510008317,
-                    "heading": 0.5306887118642236,
-                    "angularVelocity": -4.680814374954733,
-                    "velocityX": 1.4917287033341673,
-                    "velocityY": 0.12641412838299443,
-                    "timestamp": 2.457030526110715
-                },
-                {
-                    "x": 7.595282353021441,
-                    "y": 4.121668824298383,
-                    "heading": 0.3658074925223697,
-                    "angularVelocity": -4.322278451654422,
-                    "velocityX": 1.1903739426332636,
-                    "velocityY": 0.09736888581300335,
-                    "timestamp": 2.4951773556558265
-                },
-                {
-                    "x": 7.630209510034256,
-                    "y": 4.124185175295285,
-                    "heading": 0.22438733020455492,
-                    "angularVelocity": -3.7072586111122043,
-                    "velocityX": 0.9155978997292245,
-                    "velocityY": 0.06596487904592362,
-                    "timestamp": 2.533324185200938
-                },
-                {
-                    "x": 7.65555835714788,
-                    "y": 4.125725160446138,
-                    "heading": 0.1131980096846438,
-                    "angularVelocity": -2.91477225881568,
-                    "velocityX": 0.6645073107231708,
-                    "velocityY": 0.04036993818976583,
-                    "timestamp": 2.5714710147460496
-                },
-                {
-                    "x": 7.671987700512514,
-                    "y": 4.126592780034806,
-                    "heading": 0.03644789338444829,
-                    "angularVelocity": -2.011965796774904,
-                    "velocityX": 0.43068699445162756,
-                    "velocityY": 0.02274421227203338,
-                    "timestamp": 2.609617844291161
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": -0.003,
-                    "angularVelocity": -1.0341067358634992,
-                    "velocityX": 0.21003841165919293,
-                    "velocityY": 0.010675067103991773,
-                    "timestamp": 2.6477646738362726
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": -0.003,
-                    "angularVelocity": 1.475285929099793e-30,
-                    "velocityX": 9.681452358723228e-32,
-                    "velocityY": 1.6696229962948817e-32,
-                    "timestamp": 2.685911503381384
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "Amp Side To C4": {
-            "waypoints": [
-                {
-                    "x": 0.71,
-                    "y": 6.72,
-                    "heading": 1.04,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 16
-                },
-                {
-                    "x": 2.837061882019043,
-                    "y": 6.323787212371826,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 17
-                },
-                {
-                    "x": 5.489266395568848,
-                    "y": 4.304305553436279,
-                    "heading": -0.5942133396987443,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 16
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": -0.003,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 10
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 0.71,
-                    "y": 6.72,
-                    "heading": 1.04,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 0.7232220177061155,
-                    "y": 6.714742544338994,
-                    "heading": 0.9631209877994957,
-                    "angularVelocity": -1.4677311041544436,
-                    "velocityX": 0.25242736725510606,
-                    "velocityY": -0.10037240309808587,
-                    "timestamp": 0.052379493752565884
-                },
-                {
-                    "x": 0.7500587763634208,
-                    "y": 6.703146440373859,
-                    "heading": 0.8150187561291465,
-                    "angularVelocity": -2.8274849766583348,
-                    "velocityX": 0.5123523870635092,
-                    "velocityY": -0.22138633145086953,
-                    "timestamp": 0.10475898750513177
-                },
-                {
-                    "x": 0.7962886642360024,
-                    "y": 6.691501707403842,
-                    "heading": 0.6243144861489608,
-                    "angularVelocity": -3.6408192656662295,
-                    "velocityX": 0.882595163881611,
-                    "velocityY": -0.22231472921494633,
-                    "timestamp": 0.15713848125769764
-                },
-                {
-                    "x": 0.8619102089829233,
-                    "y": 6.679485060515379,
-                    "heading": 0.3880805528376709,
-                    "angularVelocity": -4.510046134222473,
-                    "velocityX": 1.2528098315899883,
-                    "velocityY": -0.22941510174244611,
-                    "timestamp": 0.20951797501026354
-                },
-                {
-                    "x": 0.948820798934311,
-                    "y": 6.665315928828611,
-                    "heading": 0.12168049392786562,
-                    "angularVelocity": -5.0859609328842605,
-                    "velocityX": 1.6592483761286883,
-                    "velocityY": -0.2705091376732434,
-                    "timestamp": 0.26189746876282943
-                },
-                {
-                    "x": 1.058148493976547,
-                    "y": 6.648822752325803,
-                    "heading": -0.13051602501618126,
-                    "angularVelocity": -4.814794891592335,
-                    "velocityX": 2.0872232091185596,
-                    "velocityY": -0.3148785015128142,
-                    "timestamp": 0.31427696251539533
-                },
-                {
-                    "x": 1.1845022977802875,
-                    "y": 6.626138581630819,
-                    "heading": -0.32549006587483453,
-                    "angularVelocity": -3.722335343286937,
-                    "velocityX": 2.41227615525686,
-                    "velocityY": -0.43307350014000373,
-                    "timestamp": 0.36665645626796123
-                },
-                {
-                    "x": 1.3283887493945847,
-                    "y": 6.598463049204702,
-                    "heading": -0.4648019456307372,
-                    "angularVelocity": -2.6596644941624366,
-                    "velocityX": 2.7469996616232804,
-                    "velocityY": -0.5283657867495356,
-                    "timestamp": 0.4190359500205271
-                },
-                {
-                    "x": 1.4904287339592295,
-                    "y": 6.567163978467113,
-                    "heading": -0.552248498766723,
-                    "angularVelocity": -1.6694806854963555,
-                    "velocityX": 3.0935767598308836,
-                    "velocityY": -0.5975443536251414,
-                    "timestamp": 0.471415443773093
-                },
-                {
-                    "x": 1.6709503203675815,
-                    "y": 6.533637134258406,
-                    "heading": -0.590326905081816,
-                    "angularVelocity": -0.7269716369343046,
-                    "velocityX": 3.4464171658686396,
-                    "velocityY": -0.6400757587899589,
-                    "timestamp": 0.5237949375256589
-                },
-                {
-                    "x": 1.8675588987397893,
-                    "y": 6.50898247632772,
-                    "heading": -0.5903272348122566,
-                    "angularVelocity": -0.000006295029161022994,
-                    "velocityX": 3.7535410193340524,
-                    "velocityY": -0.4706929403928793,
-                    "timestamp": 0.5761744312782248
-                },
-                {
-                    "x": 2.0641677208301488,
-                    "y": 6.4843297618820115,
-                    "heading": -0.5903275645018402,
-                    "angularVelocity": -0.000006294249141189924,
-                    "velocityX": 3.7535456722646963,
-                    "velocityY": -0.47065583646465164,
-                    "timestamp": 0.6285539250307907
-                },
-                {
-                    "x": 2.2607765429184905,
-                    "y": 6.459677047420138,
-                    "heading": -0.5903278941913652,
-                    "angularVelocity": -0.000006294248023551401,
-                    "velocityX": 3.753545672226176,
-                    "velocityY": -0.4706558367732492,
-                    "timestamp": 0.6809334187833566
-                },
-                {
-                    "x": 2.4573850313302983,
-                    "y": 6.435021672008896,
-                    "heading": -0.5903282238892777,
-                    "angularVelocity": -0.000006294408153628446,
-                    "velocityX": 3.753539301860412,
-                    "velocityY": -0.4707066381303828,
-                    "timestamp": 0.7333129125359225
-                },
-                {
-                    "x": 2.6504394143479257,
-                    "y": 6.390381144575869,
-                    "heading": -0.5903286607455448,
-                    "angularVelocity": -0.000008340215525789833,
-                    "velocityX": 3.685686309409395,
-                    "velocityY": -0.8522519832647445,
-                    "timestamp": 0.7856924062884884
-                },
-                {
-                    "x": 2.837061882019043,
-                    "y": 6.323787212371826,
-                    "heading": -0.5903296599203023,
-                    "angularVelocity": -0.000019075685653139892,
-                    "velocityX": 3.56289177884571,
-                    "velocityY": -1.2713741090859834,
-                    "timestamp": 0.8380719000410543
-                },
-                {
-                    "x": 3.013317183081598,
-                    "y": 6.237060061778313,
-                    "heading": -0.5903324266212199,
-                    "angularVelocity": -0.000053280252957349455,
-                    "velocityX": 3.394268952515208,
-                    "velocityY": -1.6701640905268176,
-                    "timestamp": 0.8899992232815851
-                },
-                {
-                    "x": 3.177545337709341,
-                    "y": 6.131367851066497,
-                    "heading": -0.5941789632152575,
-                    "angularVelocity": -0.0740753875608063,
-                    "velocityX": 3.1626539628670427,
-                    "velocityY": -2.0353872319249358,
-                    "timestamp": 0.9419265465221159
-                },
-                {
-                    "x": 3.3316611475687807,
-                    "y": 6.009565055315307,
-                    "heading": -0.5941812553691556,
-                    "angularVelocity": -0.00004414157624491074,
-                    "velocityX": 2.967913619301448,
-                    "velocityY": -2.3456397932739814,
-                    "timestamp": 0.9938538697626468
-                },
-                {
-                    "x": 3.48577581317155,
-                    "y": 5.887760811491488,
-                    "heading": -0.5941835469667753,
-                    "angularVelocity": -0.00004413086361592441,
-                    "velocityX": 2.9678915835676754,
-                    "velocityY": -2.3456676797995843,
-                    "timestamp": 1.0457811930031775
-                },
-                {
-                    "x": 3.639890478746931,
-                    "y": 5.7659565676417115,
-                    "heading": -0.5941858385862709,
-                    "angularVelocity": -0.00004413128489121798,
-                    "velocityX": 2.967891583040235,
-                    "velocityY": -2.3456676802994654,
-                    "timestamp": 1.0977085162437084
-                },
-                {
-                    "x": 3.7940051443206335,
-                    "y": 5.644152323798514,
-                    "heading": -0.594188130227655,
-                    "angularVelocity": -0.0000441317064133718,
-                    "velocityX": 2.9678915830079062,
-                    "velocityY": -2.3456676801727796,
-                    "timestamp": 1.1496358394842392
-                },
-                {
-                    "x": 3.948119809892657,
-                    "y": 5.522348079961896,
-                    "heading": -0.5941904218909294,
-                    "angularVelocity": -0.0000441321279705331,
-                    "velocityX": 2.9678915829755823,
-                    "velocityY": -2.345667680046072,
-                    "timestamp": 1.20156316272477
-                },
-                {
-                    "x": 4.1022344754630025,
-                    "y": 5.400543836131857,
-                    "heading": -0.5941927135760963,
-                    "angularVelocity": -0.00004413254956830847,
-                    "velocityX": 2.9678915829432535,
-                    "velocityY": -2.3456676799193525,
-                    "timestamp": 1.2534904859653009
-                },
-                {
-                    "x": 4.256349141031668,
-                    "y": 5.2787395923083995,
-                    "heading": -0.5941950052831575,
-                    "angularVelocity": -0.000044132971203721087,
-                    "velocityX": 2.9678915829109185,
-                    "velocityY": -2.3456676797926255,
-                    "timestamp": 1.3054178092058317
-                },
-                {
-                    "x": 4.410463806598655,
-                    "y": 5.1569353484915235,
-                    "heading": -0.594197297012115,
-                    "angularVelocity": -0.00004413339287361589,
-                    "velocityX": 2.9678915828785764,
-                    "velocityY": -2.345667679665889,
-                    "timestamp": 1.3573451324463626
-                },
-                {
-                    "x": 4.564578472163962,
-                    "y": 5.035131104681229,
-                    "heading": -0.5941995887629704,
-                    "angularVelocity": -0.000044133814578891406,
-                    "velocityX": 2.9678915828462293,
-                    "velocityY": -2.3456676795391433,
-                    "timestamp": 1.4092724556868934
-                },
-                {
-                    "x": 4.718693137727589,
-                    "y": 4.913326860877516,
-                    "heading": -0.5942018805357254,
-                    "angularVelocity": -0.00004413423631330234,
-                    "velocityX": 2.967891582813878,
-                    "velocityY": -2.3456676794123914,
-                    "timestamp": 1.4611997789274243
-                },
-                {
-                    "x": 4.872807803289535,
-                    "y": 4.791522617080385,
-                    "heading": -0.5942041723303811,
-                    "angularVelocity": -0.00004413465807134274,
-                    "velocityX": 2.9678915827815215,
-                    "velocityY": -2.3456676792856337,
-                    "timestamp": 1.513127102167955
-                },
-                {
-                    "x": 5.026922468849802,
-                    "y": 4.669718373289836,
-                    "heading": -0.5942064641469391,
-                    "angularVelocity": -0.000044135079855748624,
-                    "velocityX": 2.9678915827491625,
-                    "velocityY": -2.3456676791588693,
-                    "timestamp": 1.565054425408486
-                },
-                {
-                    "x": 5.181037134408388,
-                    "y": 4.5479141295058705,
-                    "heading": -0.5942087559854001,
-                    "angularVelocity": -0.000044135501659061016,
-                    "velocityX": 2.967891582716799,
-                    "velocityY": -2.345667679032102,
-                    "timestamp": 1.6169817486490168
-                },
-                {
-                    "x": 5.335151799963721,
-                    "y": 4.4261098857264995,
-                    "heading": -0.594211047845765,
-                    "angularVelocity": -0.00004413592347400816,
-                    "velocityX": 2.967891582654166,
-                    "velocityY": -2.345667678943628,
-                    "timestamp": 1.6689090718895476
-                },
-                {
-                    "x": 5.489266395568848,
-                    "y": 4.304305553436279,
-                    "heading": -0.5942133396987443,
-                    "angularVelocity": -0.000044135781248471857,
-                    "velocityX": 2.9678902355751515,
-                    "velocityY": -2.345669383457615,
-                    "timestamp": 1.7208363951300785
-                },
-                {
-                    "x": 5.673870655984677,
-                    "y": 4.147383169820797,
-                    "heading": -0.5942132891163008,
-                    "angularVelocity": 7.897648233188991e-7,
-                    "velocityX": 2.882303439061699,
-                    "velocityY": -2.450094732060046,
-                    "timestamp": 1.7848838711570736
-                },
-                {
-                    "x": 5.858474845870276,
-                    "y": 3.9904607032332415,
-                    "heading": -0.5942132385338614,
-                    "angularVelocity": 7.897647575237561e-7,
-                    "velocityX": 2.882302337843733,
-                    "velocityY": -2.450096027537681,
-                    "timestamp": 1.8489313471840687
-                },
-                {
-                    "x": 6.043079035755059,
-                    "y": 3.833538236644727,
-                    "heading": -0.5942131879514223,
-                    "angularVelocity": 7.897647543646741e-7,
-                    "velocityX": 2.882302337831004,
-                    "velocityY": -2.4500960275526547,
-                    "timestamp": 1.9129788232110638
-                },
-                {
-                    "x": 6.227683225639842,
-                    "y": 3.676615770056212,
-                    "heading": -0.5942131373689834,
-                    "angularVelocity": 7.897647493463809e-7,
-                    "velocityX": 2.8823023378310055,
-                    "velocityY": -2.450096027552654,
-                    "timestamp": 1.977026299238059
-                },
-                {
-                    "x": 6.412287415524625,
-                    "y": 3.5196933034676974,
-                    "heading": -0.5942130867865447,
-                    "angularVelocity": 7.89764745190247e-7,
-                    "velocityX": 2.882302337831007,
-                    "velocityY": -2.4500960275526538,
-                    "timestamp": 2.041073775265054
-                },
-                {
-                    "x": 6.596891605409407,
-                    "y": 3.362770836879183,
-                    "heading": -0.5942130362041063,
-                    "angularVelocity": 7.897647430611541e-7,
-                    "velocityX": 2.882302337831007,
-                    "velocityY": -2.450096027552653,
-                    "timestamp": 2.105121251292049
-                },
-                {
-                    "x": 6.781495795294782,
-                    "y": 3.2058483702913625,
-                    "heading": -0.5942129856216681,
-                    "angularVelocity": 7.897647381910717e-7,
-                    "velocityX": 2.8823023378402177,
-                    "velocityY": -2.4500960275418184,
-                    "timestamp": 2.169168727319044
-                },
-                {
-                    "x": 6.966100036209046,
-                    "y": 3.048925963734244,
-                    "heading": -0.5942129350390435,
-                    "angularVelocity": 7.897676514556789e-7,
-                    "velocityX": 2.882303134575621,
-                    "velocityY": -2.4500950902573897,
-                    "timestamp": 2.233216203346039
-                },
-                {
-                    "x": 7.143621774279278,
-                    "y": 2.9040156366918444,
-                    "heading": -0.5543160517170741,
-                    "angularVelocity": 0.6229267068253146,
-                    "velocityX": 2.771721058850299,
-                    "velocityY": -2.262545474568311,
-                    "timestamp": 2.2972636793730343
-                },
-                {
-                    "x": 7.2980980894932665,
-                    "y": 2.7758641788664877,
-                    "heading": -0.43954991420980805,
-                    "angularVelocity": 1.7918916501704738,
-                    "velocityX": 2.411903244225882,
-                    "velocityY": -2.000882248214473,
-                    "timestamp": 2.3613111554000294
-                },
-                {
-                    "x": 7.426195766165712,
-                    "y": 2.6690947773000513,
-                    "heading": -0.31008673137290993,
-                    "angularVelocity": 2.0213627588124035,
-                    "velocityX": 2.000042540606181,
-                    "velocityY": -1.667035271170323,
-                    "timestamp": 2.4253586314270246
-                },
-                {
-                    "x": 7.528097771332223,
-                    "y": 2.5840084569104262,
-                    "heading": -0.19375204823220424,
-                    "angularVelocity": 1.816382008428752,
-                    "velocityX": 1.591038577750685,
-                    "velocityY": -1.3284882663254731,
-                    "timestamp": 2.4894061074540197
-                },
-                {
-                    "x": 7.6042028509496316,
-                    "y": 2.5203982409108687,
-                    "heading": -0.10083265861354884,
-                    "angularVelocity": 1.4507892485800875,
-                    "velocityX": 1.1882604021012733,
-                    "velocityY": -0.9931728765197068,
-                    "timestamp": 2.553453583481015
-                },
-                {
-                    "x": 7.654775762663328,
-                    "y": 2.4780994279137802,
-                    "heading": -0.036281315483046445,
-                    "angularVelocity": 1.0078670875850742,
-                    "velocityX": 0.7896159981757911,
-                    "velocityY": -0.660429038284975,
-                    "timestamp": 2.61750105950801
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": -0.003,
-                    "angularVelocity": 0.5196350824038538,
-                    "velocityX": 0.39383655533967327,
-                    "velocityY": -0.32943418261927393,
-                    "timestamp": 2.681548535535005
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": -0.003,
-                    "angularVelocity": 1.9104375826405664e-34,
-                    "velocityX": -1.672523979523505e-35,
-                    "velocityY": 0,
-                    "timestamp": 2.745596011562
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 10,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "Amp Side To C5": {
-            "waypoints": [
-                {
-                    "x": 0.71,
-                    "y": 6.72,
-                    "heading": 1.04,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 16
-                },
-                {
-                    "x": 2.797205924987793,
-                    "y": 6.264003276824951,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 13
-                },
-                {
-                    "x": 4.092995643615723,
-                    "y": 5.023272514343262,
-                    "heading": -0.5720542830492642,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 29
-                },
-                {
-                    "x": 5.7832255363464355,
-                    "y": 3.9654417037963867,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 9
-                },
-                {
-                    "x": 7.140008449554443,
-                    "y": 3.2525558471679688,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 27
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": -0.003,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 25
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 0.71,
-                    "y": 6.72,
-                    "heading": 1.04,
-                    "angularVelocity": 0,
-                    "velocityX": 1.8950109818726043e-31,
-                    "velocityY": 4.417214679587952e-30,
-                    "timestamp": 0
-                },
-                {
-                    "x": 0.7239179438589063,
-                    "y": 6.714864648059082,
-                    "heading": 0.9709045474564667,
-                    "angularVelocity": -1.3401351995297424,
-                    "velocityX": 0.269944341976283,
-                    "velocityY": -0.09960229862677873,
-                    "timestamp": 0.051558568544241005
-                },
-                {
-                    "x": 0.7525251092531191,
-                    "y": 6.703796484721446,
-                    "heading": 0.8397844996535714,
-                    "angularVelocity": -2.5431281648227726,
-                    "velocityX": 0.5548479370541143,
-                    "velocityY": -0.21467165691630782,
-                    "timestamp": 0.10311713708848201
-                },
-                {
-                    "x": 0.8002738106410594,
-                    "y": 6.689991944618376,
-                    "heading": 0.6713194026020708,
-                    "angularVelocity": -3.2674510136339485,
-                    "velocityX": 0.9261060331222868,
-                    "velocityY": -0.26774482870254956,
-                    "timestamp": 0.154675705632723
-                },
-                {
-                    "x": 0.8672232338097706,
-                    "y": 6.675504894458598,
-                    "heading": 0.46260124525274376,
-                    "angularVelocity": -4.048175952947979,
-                    "velocityX": 1.2985120622830493,
-                    "velocityY": -0.28098239669505554,
-                    "timestamp": 0.20623427417696402
-                },
-                {
-                    "x": 0.954736838142347,
-                    "y": 6.658878661036629,
-                    "heading": 0.2244001490575843,
-                    "angularVelocity": -4.620009882366156,
-                    "velocityX": 1.6973629564117712,
-                    "velocityY": -0.3224727507251158,
-                    "timestamp": 0.257792842721205
-                },
-                {
-                    "x": 1.0641109683124677,
-                    "y": 6.639652990231364,
-                    "heading": -0.004541406089891365,
-                    "angularVelocity": -4.440417211175481,
-                    "velocityX": 2.12135699765032,
-                    "velocityY": -0.37288992592389836,
-                    "timestamp": 0.309351411265446
-                },
-                {
-                    "x": 1.1925189459621883,
-                    "y": 6.613890706063681,
-                    "heading": -0.19800120779802596,
-                    "angularVelocity": -3.752233763861444,
-                    "velocityX": 2.4905264299488024,
-                    "velocityY": -0.49967027586341123,
-                    "timestamp": 0.36090997980968703
-                },
-                {
-                    "x": 1.3374709433458876,
-                    "y": 6.583331183982878,
-                    "heading": -0.33396197230189334,
-                    "angularVelocity": -2.6370158897471825,
-                    "velocityX": 2.811404611812854,
-                    "velocityY": -0.5927147115125508,
-                    "timestamp": 0.41246854835392804
-                },
-                {
-                    "x": 1.49983426972339,
-                    "y": 6.549745693102723,
-                    "heading": -0.41661329783619183,
-                    "angularVelocity": -1.6030570255902088,
-                    "velocityX": 3.149104619501603,
-                    "velocityY": -0.6514046419107491,
-                    "timestamp": 0.46402711689816906
-                },
-                {
-                    "x": 1.6800135166896315,
-                    "y": 6.515230594671984,
-                    "heading": -0.44907632730235497,
-                    "angularVelocity": -0.6296340333474829,
-                    "velocityX": 3.4946518503843107,
-                    "velocityY": -0.6694347691375233,
-                    "timestamp": 0.51558568544241
-                },
-                {
-                    "x": 1.8735253740457867,
-                    "y": 6.490840556482913,
-                    "heading": -0.4490767643443874,
-                    "angularVelocity": -0.00000847661300213044,
-                    "velocityX": 3.7532434049278374,
-                    "velocityY": -0.47305499119601163,
-                    "timestamp": 0.567144253986651
-                },
-                {
-                    "x": 2.067041889759694,
-                    "y": 6.46648750315131,
-                    "heading": -0.4490772002167485,
-                    "angularVelocity": -0.000008453926735380078,
-                    "velocityX": 3.753333755723225,
-                    "velocityY": -0.47233765442907966,
-                    "timestamp": 0.618702822530892
-                },
-                {
-                    "x": 2.2605372007770526,
-                    "y": 6.441966542289934,
-                    "heading": -0.4490776373561405,
-                    "angularVelocity": -0.0000084785013311663,
-                    "velocityX": 3.7529224817654048,
-                    "velocityY": -0.4755942911914249,
-                    "timestamp": 0.670261391075133
-                },
-                {
-                    "x": 2.45117838314973,
-                    "y": 6.400765025555903,
-                    "heading": -0.4490787415251664,
-                    "angularVelocity": -0.000021415820050526827,
-                    "velocityX": 3.6975654630332984,
-                    "velocityY": -0.7991206485676197,
-                    "timestamp": 0.7218199596193741
-                },
-                {
-                    "x": 2.6301938207404594,
-                    "y": 6.341459154774922,
-                    "heading": -0.46856582254307677,
-                    "angularVelocity": -0.377960086337202,
-                    "velocityX": 3.472079280806918,
-                    "velocityY": -1.150262166999653,
-                    "timestamp": 0.7733785281636151
-                },
-                {
-                    "x": 2.797205924987793,
-                    "y": 6.264003276824951,
-                    "heading": -0.5033519686488042,
-                    "angularVelocity": -0.6746918521648854,
-                    "velocityX": 3.239269610517634,
-                    "velocityY": -1.502289146830184,
-                    "timestamp": 0.8249370967078561
-                },
-                {
-                    "x": 2.911639196984497,
-                    "y": 6.1983709493948735,
-                    "heading": -0.5311684488221697,
-                    "angularVelocity": -0.7502107513814861,
-                    "velocityX": 3.0862665022221747,
-                    "velocityY": -1.770104534065962,
-                    "timestamp": 0.8620153178901536
-                },
-                {
-                    "x": 3.0203603627243916,
-                    "y": 6.122823189814021,
-                    "heading": -0.5565632404510816,
-                    "angularVelocity": -0.6848977868761134,
-                    "velocityX": 2.9322109387469486,
-                    "velocityY": -2.037523839376905,
-                    "timestamp": 0.8990935390724512
-                },
-                {
-                    "x": 3.1248710366711436,
-                    "y": 6.036966714195615,
-                    "heading": -0.5720315951438852,
-                    "angularVelocity": -0.41718168240112047,
-                    "velocityX": 2.8186539325316726,
-                    "velocityY": -2.3155500150894683,
-                    "timestamp": 0.9361717602547487
-                },
-                {
-                    "x": 3.2269084939172057,
-                    "y": 5.940732357266897,
-                    "heading": -0.5720474132070273,
-                    "angularVelocity": -0.00042661332192557474,
-                    "velocityX": 2.7519512531259087,
-                    "velocityY": -2.5954415789079244,
-                    "timestamp": 0.9732499814370462
-                },
-                {
-                    "x": 3.3218654576085616,
-                    "y": 5.837498151619579,
-                    "heading": -0.5720480482162372,
-                    "angularVelocity": -0.000017126204806487387,
-                    "velocityX": 2.560990270400005,
-                    "velocityY": -2.7842275695894796,
-                    "timestamp": 1.0103282026193439
-                },
-                {
-                    "x": 3.4166998907386708,
-                    "y": 5.734151371991349,
-                    "heading": -0.5720486788754773,
-                    "angularVelocity": -0.00001700888607859636,
-                    "velocityX": 2.557685619889607,
-                    "velocityY": -2.7872636910247786,
-                    "timestamp": 1.0474064238016414
-                },
-                {
-                    "x": 3.5114576080251196,
-                    "y": 5.630734246765037,
-                    "heading": -0.5720493072137337,
-                    "angularVelocity": -0.000016946289124090365,
-                    "velocityX": 2.555616592803881,
-                    "velocityY": -2.7891609125222083,
-                    "timestamp": 1.084484644983939
-                },
-                {
-                    "x": 3.6061908144763444,
-                    "y": 5.527294668181728,
-                    "heading": -0.572049934817026,
-                    "angularVelocity": -0.00001692646713700802,
-                    "velocityX": 2.5549555353779705,
-                    "velocityY": -2.7897664797435726,
-                    "timestamp": 1.1215628661662365
-                },
-                {
-                    "x": 3.7009275327159292,
-                    "y": 5.423858305931887,
-                    "heading": -0.5720505625260676,
-                    "angularVelocity": -0.00001692931919251171,
-                    "velocityX": 2.555050248355605,
-                    "velocityY": -2.789679735188215,
-                    "timestamp": 1.158641087348534
-                },
-                {
-                    "x": 3.7956729593070784,
-                    "y": 5.320429920428362,
-                    "heading": -0.5720511904967662,
-                    "angularVelocity": -0.000016936376088998586,
-                    "velocityX": 2.5552851126613465,
-                    "velocityY": -2.7894646022814182,
-                    "timestamp": 1.1957193085308315
-                },
-                {
-                    "x": 3.890467288385621,
-                    "y": 5.217046353888283,
-                    "heading": -0.5720518199780259,
-                    "angularVelocity": -0.000016977115933005807,
-                    "velocityX": 2.556604013243175,
-                    "velocityY": -2.7882558343819337,
-                    "timestamp": 1.232797529713129
-                },
-                {
-                    "x": 3.9877865474692196,
-                    "y": 5.11603605874111,
-                    "heading": -0.5720525488313646,
-                    "angularVelocity": -0.00001965718190938232,
-                    "velocityX": 2.624701400996587,
-                    "velocityY": -2.7242486809308257,
-                    "timestamp": 1.2698757508954266
-                },
-                {
-                    "x": 4.092995643615723,
-                    "y": 5.023272514343262,
-                    "heading": -0.5720542830492642,
-                    "angularVelocity": -0.000046771874277348444,
-                    "velocityX": 2.8374903863181586,
-                    "velocityY": -2.5018337298792988,
-                    "timestamp": 1.3069539720777241
-                },
-                {
-                    "x": 4.14638765114505,
-                    "y": 4.979871763007162,
-                    "heading": -0.5720542261628284,
-                    "angularVelocity": 0.000003127581707131923,
-                    "velocityX": 2.935460161172297,
-                    "velocityY": -2.386146961079825,
-                    "timestamp": 1.3251426050680648
-                },
-                {
-                    "x": 4.201473544713913,
-                    "y": 4.938642209546033,
-                    "heading": -0.5720541700948967,
-                    "angularVelocity": 0.000003082580845535541,
-                    "velocityX": 3.0285889873152927,
-                    "velocityY": -2.266775819987239,
-                    "timestamp": 1.3433312380584055
-                },
-                {
-                    "x": 4.258151834683336,
-                    "y": 4.899630631332575,
-                    "heading": -0.5720541145494444,
-                    "angularVelocity": 0.0000030538552492495714,
-                    "velocityX": 3.11613797471883,
-                    "velocityY": -2.1448328873407987,
-                    "timestamp": 1.3615198710487462
-                },
-                {
-                    "x": 4.316201884310526,
-                    "y": 4.862691173522813,
-                    "heading": -0.5720540592428014,
-                    "angularVelocity": 0.000003040725654248182,
-                    "velocityX": 3.1915564879460585,
-                    "velocityY": -2.030908965471825,
-                    "timestamp": 1.3797085040390868
-                },
-                {
-                    "x": 4.374575644994192,
-                    "y": 4.826265415180517,
-                    "heading": -0.5720540039764063,
-                    "angularVelocity": 0.0000030385128512512867,
-                    "velocityX": 3.209353925315247,
-                    "velocityY": -2.002666080603007,
-                    "timestamp": 1.3978971370294275
-                },
-                {
-                    "x": 4.432961770826435,
-                    "y": 4.789859479973715,
-                    "heading": -0.572053948711591,
-                    "angularVelocity": 0.000003038425994779569,
-                    "velocityX": 3.210033753678121,
-                    "velocityY": -2.0015762166465794,
-                    "timestamp": 1.4160857700197682
-                },
-                {
-                    "x": 4.491358693973173,
-                    "y": 4.753470866702408,
-                    "heading": -0.5720538934471577,
-                    "angularVelocity": 0.000003038404992596871,
-                    "velocityX": 3.21062738347468,
-                    "velocityY": -2.000623867148186,
-                    "timestamp": 1.434274403010109
-                },
-                {
-                    "x": 4.549763643159385,
-                    "y": 4.7170951368878224,
-                    "heading": -0.5720538381830028,
-                    "angularVelocity": 0.0000030383896855688938,
-                    "velocityX": 3.2110686502526127,
-                    "velocityY": -1.9999155425186408,
-                    "timestamp": 1.4524630360004496
-                },
-                {
-                    "x": 4.608173712818011,
-                    "y": 4.680727629815019,
-                    "heading": -0.5720537829190162,
-                    "angularVelocity": 0.0000030383804361409647,
-                    "velocityX": 3.211350170717194,
-                    "velocityY": -1.9994634611677902,
-                    "timestamp": 1.4706516689907903
-                },
-                {
-                    "x": 4.666585738503753,
-                    "y": 4.644363264529719,
-                    "heading": -0.5720537276550893,
-                    "angularVelocity": 0.0000030383771462609746,
-                    "velocityX": 3.211457711899863,
-                    "velocityY": -1.999290727599226,
-                    "timestamp": 1.488840301981131
-                },
-                {
-                    "x": 4.724997877507934,
-                    "y": 4.607999081268276,
-                    "heading": -0.5720536723911652,
-                    "angularVelocity": 0.0000030383769981998487,
-                    "velocityX": 3.2114639420784044,
-                    "velocityY": -1.9992807200388194,
-                    "timestamp": 1.5070289349714716
-                },
-                {
-                    "x": 4.783409732751982,
-                    "y": 4.571634442201932,
-                    "heading": -0.5720536171272324,
-                    "angularVelocity": 0.0000030383774810111486,
-                    "velocityX": 3.2114483411183334,
-                    "velocityY": -1.9993057799158553,
-                    "timestamp": 1.5252175679618123
-                },
-                {
-                    "x": 4.841821302805823,
-                    "y": 4.535269345043953,
-                    "heading": -0.5720535618632904,
-                    "angularVelocity": 0.0000030383779810703436,
-                    "velocityX": 3.211432661533899,
-                    "velocityY": -1.9993309655158582,
-                    "timestamp": 1.543406200952153
-                },
-                {
-                    "x": 4.900232750957554,
-                    "y": 4.4989040520829935,
-                    "heading": -0.5720535065993444,
-                    "angularVelocity": 0.0000030383782054284467,
-                    "velocityX": 3.2114259594301156,
-                    "velocityY": -1.9993417306445722,
-                    "timestamp": 1.5615948339424937
-                },
-                {
-                    "x": 4.958643009851236,
-                    "y": 4.46253684895675,
-                    "heading": -0.5720534513353597,
-                    "angularVelocity": 0.000003038380333213315,
-                    "velocityX": 3.2113605747448064,
-                    "velocityY": -1.999446750371714,
-                    "timestamp": 1.5797834669328343
-                },
-                {
-                    "x": 5.017052204377269,
-                    "y": 4.426167936377253,
-                    "heading": -0.5720533960713398,
-                    "angularVelocity": 0.0000030383822663214653,
-                    "velocityX": 3.21130205645789,
-                    "velocityY": -1.999540735073477,
-                    "timestamp": 1.597972099923175
-                },
-                {
-                    "x": 5.075461342766336,
-                    "y": 4.389798933639636,
-                    "heading": -0.572053340807318,
-                    "angularVelocity": 0.0000030383823681408014,
-                    "velocityX": 3.211298970081695,
-                    "velocityY": -1.999545691911999,
-                    "timestamp": 1.6161607329135157
-                },
-                {
-                    "x": 5.133870231273224,
-                    "y": 4.35342952959225,
-                    "heading": -0.5720532855432875,
-                    "angularVelocity": 0.0000030383828455618877,
-                    "velocityX": 3.211285231711017,
-                    "velocityY": -1.9995677556796618,
-                    "timestamp": 1.6343493659038564
-                },
-                {
-                    "x": 5.192278843825858,
-                    "y": 4.317059682370623,
-                    "heading": -0.5720532302792477,
-                    "angularVelocity": 0.0000030383833608448334,
-                    "velocityX": 3.2112700599133515,
-                    "velocityY": -1.9995921211310104,
-                    "timestamp": 1.652537998894197
-                },
-                {
-                    "x": 5.250688378518623,
-                    "y": 4.28069131610433,
-                    "heading": -0.5720531750152431,
-                    "angularVelocity": 0.000003038381418986995,
-                    "velocityX": 3.211320758617258,
-                    "velocityY": -1.9995106991068379,
-                    "timestamp": 1.6707266318845377
-                },
-                {
-                    "x": 5.309100997862405,
-                    "y": 4.2443279043993805,
-                    "heading": -0.5720531197513381,
-                    "angularVelocity": 0.0000030383759466885435,
-                    "velocityX": 3.2114903508569643,
-                    "velocityY": -1.9992383003329772,
-                    "timestamp": 1.6889152648748784
-                },
-                {
-                    "x": 5.367519658270811,
-                    "y": 4.20797419858894,
-                    "heading": -0.5720530644876303,
-                    "angularVelocity": 0.0000030383651093388606,
-                    "velocityX": 3.2118224849224113,
-                    "velocityY": -1.998704676145307,
-                    "timestamp": 1.707103897865219
-                },
-                {
-                    "x": 5.425947098435591,
-                    "y": 4.171634605233085,
-                    "heading": -0.5720530092241862,
-                    "angularVelocity": 0.000003038350601996349,
-                    "velocityX": 3.212305190598148,
-                    "velocityY": -1.9979287819592548,
-                    "timestamp": 1.7252925308555598
-                },
-                {
-                    "x": 5.484385608043213,
-                    "y": 4.135312815587752,
-                    "heading": -0.5720529539612775,
-                    "angularVelocity": 0.0000030383211709796797,
-                    "velocityX": 3.2129137818460802,
-                    "velocityY": -1.9969499447620875,
-                    "timestamp": 1.7434811638459005
-                },
-                {
-                    "x": 5.542805855367704,
-                    "y": 4.098961660035809,
-                    "heading": -0.5720528986974772,
-                    "angularVelocity": 0.0000030383701899944643,
-                    "velocityX": 3.21190973260657,
-                    "velocityY": -1.9985644644794618,
-                    "timestamp": 1.7616697968362411
-                },
-                {
-                    "x": 5.601240344083477,
-                    "y": 4.062633401947461,
-                    "heading": -0.5720528434300965,
-                    "angularVelocity": 0.000003038567036318681,
-                    "velocityX": 3.2126927156544984,
-                    "velocityY": -1.9973055758301874,
-                    "timestamp": 1.7798584298265818
-                },
-                {
-                    "x": 5.6606145093793865,
-                    "y": 4.027862145195579,
-                    "heading": -0.5720527881030576,
-                    "angularVelocity": 0.0000030418470110587657,
-                    "velocityX": 3.2643555635790893,
-                    "velocityY": -1.9117025875630587,
-                    "timestamp": 1.7980470628169225
-                },
-                {
-                    "x": 5.721296662408543,
-                    "y": 3.9954274711883437,
-                    "heading": -0.5720527324898503,
-                    "angularVelocity": 0.0000030575803747562297,
-                    "velocityX": 3.3362679351103024,
-                    "velocityY": -1.7832386867362542,
-                    "timestamp": 1.8162356958072632
-                },
-                {
-                    "x": 5.7832255363464355,
-                    "y": 3.9654417037963863,
-                    "heading": -0.5720526754836338,
-                    "angularVelocity": 0.0000031341671777608764,
-                    "velocityX": 3.404811893823314,
-                    "velocityY": -1.6485992876907705,
-                    "timestamp": 1.8344243287976039
-                },
-                {
-                    "x": 5.946280542451374,
-                    "y": 3.905964298639421,
-                    "heading": -0.5720526220643124,
-                    "angularVelocity": 0.0000011643081031480314,
-                    "velocityX": 3.5538876198319724,
-                    "velocityY": -1.296347894456755,
-                    "timestamp": 1.880305072239023
-                },
-                {
-                    "x": 6.113507668800577,
-                    "y": 3.8594929620139826,
-                    "heading": -0.5720525704992387,
-                    "angularVelocity": 0.0000011238935971124524,
-                    "velocityX": 3.6448216355273746,
-                    "velocityY": -1.0128723542630858,
-                    "timestamp": 1.9261858156804423
-                },
-                {
-                    "x": 6.280741389533688,
-                    "y": 3.813045361829368,
-                    "heading": -0.5720525189366377,
-                    "angularVelocity": 0.000001123839702331554,
-                    "velocityX": 3.64496536431679,
-                    "velocityY": -1.0123550034458102,
-                    "timestamp": 1.9720665591218616
-                },
-                {
-                    "x": 6.445214711488264,
-                    "y": 3.7576105066241974,
-                    "heading": -0.5720524661781627,
-                    "angularVelocity": 0.000001149904532146113,
-                    "velocityX": 3.584800716329628,
-                    "velocityY": -1.2082379457532038,
-                    "timestamp": 2.0179473025632806
-                },
-                {
-                    "x": 6.603250853740864,
-                    "y": 3.685853457646542,
-                    "heading": -0.5720524095450499,
-                    "angularVelocity": 0.0000012343547357711312,
-                    "velocityX": 3.444498288357164,
-                    "velocityY": -1.563990545825532,
-                    "timestamp": 2.0638280460046996
-                },
-                {
-                    "x": 6.753231641982998,
-                    "y": 3.5985021065376834,
-                    "heading": -0.5720523460454168,
-                    "angularVelocity": 0.0000013840149106858422,
-                    "velocityX": 3.268926721589291,
-                    "velocityY": -1.9038782843733606,
-                    "timestamp": 2.1097087894461186
-                },
-                {
-                    "x": 6.893623224143754,
-                    "y": 3.4964496816334383,
-                    "heading": -0.5720522715269948,
-                    "angularVelocity": 0.000001624176426799149,
-                    "velocityX": 3.059923872852999,
-                    "velocityY": -2.2242975429267773,
-                    "timestamp": 2.1555895328875376
-                },
-                {
-                    "x": 7.022989830955657,
-                    "y": 3.3807398195880776,
-                    "heading": -0.5720521794208858,
-                    "angularVelocity": 0.000002007511259312441,
-                    "velocityX": 2.819627519265176,
-                    "velocityY": -2.521970076471132,
-                    "timestamp": 2.2014702763289566
-                },
-                {
-                    "x": 7.140008449554443,
-                    "y": 3.2525558471679688,
-                    "heading": -0.5720520666435536,
-                    "angularVelocity": 0.0000024580537218711263,
-                    "velocityX": 2.550495258387596,
-                    "velocityY": -2.7938512501200163,
-                    "timestamp": 2.2473510197703757
-                },
-                {
-                    "x": 7.220333202972963,
-                    "y": 3.149893277120657,
-                    "heading": -0.57205195105401,
-                    "angularVelocity": 0.0000033545158672128766,
-                    "velocityX": 2.3310989161613462,
-                    "velocityY": -2.9793630927244275,
-                    "timestamp": 2.28180891122307
-                },
-                {
-                    "x": 7.292634698776439,
-                    "y": 3.0414305376048008,
-                    "heading": -0.5720518635503085,
-                    "angularVelocity": 0.000002539438655350069,
-                    "velocityX": 2.098256531535021,
-                    "velocityY": -3.1476893954673923,
-                    "timestamp": 2.3162668026757647
-                },
-                {
-                    "x": 7.356495877093559,
-                    "y": 2.9277932908128785,
-                    "heading": -0.5720517925210026,
-                    "angularVelocity": 0.000002061336399152925,
-                    "velocityX": 1.853310682252877,
-                    "velocityY": -3.297858400531866,
-                    "timestamp": 2.350724694128459
-                },
-                {
-                    "x": 7.411548371625277,
-                    "y": 2.8096370468188216,
-                    "heading": -0.5720517318127031,
-                    "angularVelocity": 0.0000017618112132015592,
-                    "velocityX": 1.5976745009881768,
-                    "velocityY": -3.4290038946879324,
-                    "timestamp": 2.3851825855811537
-                },
-                {
-                    "x": 7.457474639290446,
-                    "y": 2.6876433834723,
-                    "heading": -0.5720516777421675,
-                    "angularVelocity": 0.0000015691771403552433,
-                    "velocityX": 1.332822924707698,
-                    "velocityY": -3.540369366882859,
-                    "timestamp": 2.419640477033848
-                },
-                {
-                    "x": 7.49400980759948,
-                    "y": 2.5625160164724585,
-                    "heading": -0.5720516278694773,
-                    "angularVelocity": 0.0000014473517720699968,
-                    "velocityX": 1.0602845028769212,
-                    "velocityY": -3.631312356172488,
-                    "timestamp": 2.4540983684865427
-                },
-                {
-                    "x": 7.520944004354476,
-                    "y": 2.4349769014207707,
-                    "heading": -0.572051580412575,
-                    "angularVelocity": 0.0000013772433620936625,
-                    "velocityX": 0.7816553950223419,
-                    "velocityY": -3.7013035236575447,
-                    "timestamp": 2.4885562599392372
-                },
-                {
-                    "x": 7.53814696549347,
-                    "y": 2.3057649240296647,
-                    "heading": -0.5720515339265989,
-                    "angularVelocity": 0.0000013490661810968306,
-                    "velocityX": 0.4992459031544392,
-                    "velocityY": -3.7498515418008185,
-                    "timestamp": 2.5230141513919317
-                },
-                {
-                    "x": 7.550702118784975,
-                    "y": 2.1760188446169613,
-                    "heading": -0.5720514875103294,
-                    "angularVelocity": 0.0000013470432314212435,
-                    "velocityX": 0.3643622044850161,
-                    "velocityY": -3.7653516783177263,
-                    "timestamp": 2.5574720428446263
-                },
-                {
-                    "x": 7.56324456343349,
-                    "y": 2.046271536053824,
-                    "heading": -0.5720514410938347,
-                    "angularVelocity": 0.000001347049770461967,
-                    "velocityX": 0.36399338786688684,
-                    "velocityY": -3.765387349404451,
-                    "timestamp": 2.5919299342973208
-                },
-                {
-                    "x": 7.57578727298306,
-                    "y": 1.916524253098162,
-                    "heading": -0.5720513946773481,
-                    "angularVelocity": 0.0000013470495296572014,
-                    "velocityX": 0.3640010755350196,
-                    "velocityY": -3.7653866062523553,
-                    "timestamp": 2.6263878257500153
-                },
-                {
-                    "x": 7.588311679081848,
-                    "y": 1.7867752020286953,
-                    "heading": -0.572051348260254,
-                    "angularVelocity": 0.0000013470671654480904,
-                    "velocityX": 0.3634698923895803,
-                    "velocityY": -3.765437918557532,
-                    "timestamp": 2.66084571720271
-                },
-                {
-                    "x": 7.600787981664821,
-                    "y": 1.657021516812979,
-                    "heading": -0.5720513014102728,
-                    "angularVelocity": 0.0000013596299477389166,
-                    "velocityX": 0.3620738837176714,
-                    "velocityY": -3.7655724057824624,
-                    "timestamp": 2.6953036086554043
-                },
-                {
-                    "x": 7.6103162713524775,
-                    "y": 1.534453918276911,
-                    "heading": -0.5536995765422551,
-                    "angularVelocity": 0.5325840930573226,
-                    "velocityX": 0.27651981261615693,
-                    "velocityY": -3.557025498914872,
-                    "timestamp": 2.729761500108099
-                },
-                {
-                    "x": 7.620120451226168,
-                    "y": 1.4200989858077522,
-                    "heading": -0.5149948955625814,
-                    "angularVelocity": 1.1232457747100673,
-                    "velocityX": 0.2845264019455608,
-                    "velocityY": -3.3186863051722235,
-                    "timestamp": 2.7642193915607933
-                },
-                {
-                    "x": 7.630164619508576,
-                    "y": 1.3139569074919104,
-                    "heading": -0.4555658179030021,
-                    "angularVelocity": 1.7246870064917483,
-                    "velocityX": 0.29149108836702503,
-                    "velocityY": -3.080341652987332,
-                    "timestamp": 2.798677283013488
-                },
-                {
-                    "x": 7.6393124908526255,
-                    "y": 1.2174783858270968,
-                    "heading": -0.38913465703239974,
-                    "angularVelocity": 1.9278939618746356,
-                    "velocityX": 0.26547971911102464,
-                    "velocityY": -2.799896267514583,
-                    "timestamp": 2.8331351744661823
-                },
-                {
-                    "x": 7.647521237661079,
-                    "y": 1.1308545482085492,
-                    "heading": -0.3222243620613333,
-                    "angularVelocity": 1.9417988782892024,
-                    "velocityX": 0.23822545322294675,
-                    "velocityY": -2.5139041875930466,
-                    "timestamp": 2.867593065918877
-                },
-                {
-                    "x": 7.654790830767336,
-                    "y": 1.0540553472510912,
-                    "heading": -0.25810310965019956,
-                    "angularVelocity": 1.8608582739068054,
-                    "velocityX": 0.21097034089302885,
-                    "velocityY": -2.2287841106847384,
-                    "timestamp": 2.9020509573715714
-                },
-                {
-                    "x": 7.661128119511832,
-                    "y": 0.9870214726082436,
-                    "heading": -0.19877373026472553,
-                    "angularVelocity": 1.7217936700197296,
-                    "velocityX": 0.18391400278139633,
-                    "velocityY": -1.9453852750938303,
-                    "timestamp": 2.936508848824266
-                },
-                {
-                    "x": 7.6665422514065025,
-                    "y": 0.9296939844226774,
-                    "heading": -0.14563708266620207,
-                    "angularVelocity": 1.5420748443496368,
-                    "velocityX": 0.15712313395848782,
-                    "velocityY": -1.6636969288812407,
-                    "timestamp": 2.9709667402769604
-                },
-                {
-                    "x": 7.6710416569080175,
-                    "y": 0.8820195033603357,
-                    "heading": -0.09974947152576126,
-                    "angularVelocity": 1.331701076470322,
-                    "velocityX": 0.1305769248148914,
-                    "velocityY": -1.3835577004993256,
-                    "timestamp": 3.005424631729655
-                },
-                {
-                    "x": 7.674632846509551,
-                    "y": 0.8439509935506372,
-                    "heading": -0.06193916709248371,
-                    "angularVelocity": 1.0972901370124863,
-                    "velocityX": 0.10421965622732904,
-                    "velocityY": -1.1047834967488865,
-                    "timestamp": 3.0398825231823494
-                },
-                {
-                    "x": 7.677320522590369,
-                    "y": 0.8154475641909131,
-                    "heading": -0.032869067302209315,
-                    "angularVelocity": 0.8436412840348516,
-                    "velocityX": 0.07799885505174348,
-                    "velocityY": -0.8271959820541263,
-                    "timestamp": 3.074340414635044
-                },
-                {
-                    "x": 7.6791083601324726,
-                    "y": 0.7964739655020694,
-                    "heading": -0.013076341373692615,
-                    "angularVelocity": 0.5744032816313593,
-                    "velocityX": 0.05188470526556513,
-                    "velocityY": -0.5506314486738934,
-                    "timestamp": 3.1087983060877384
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": -0.003,
-                    "angularVelocity": 0.2924247813459689,
-                    "velocityX": 0.02587621673680596,
-                    "velocityY": -0.2749432743171821,
-                    "timestamp": 3.143256197540433
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": -0.003,
-                    "angularVelocity": -5.23227119774542e-33,
-                    "velocityX": 0,
-                    "velocityY": -1.249481443135334e-33,
-                    "timestamp": 3.1777140889931275
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 25,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "Center To W1": {
-            "waypoints": [
-                {
-                    "x": 1.35,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 11
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 1.35,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": -1.3982338019676807e-32,
-                    "velocityX": 1.9225496754980614e-32,
-                    "velocityY": -8.567103847838605e-38,
-                    "timestamp": 0
-                },
-                {
-                    "x": 1.3816658535274804,
-                    "y": 5.599900984360149,
-                    "heading": 0.013082876743969601,
-                    "angularVelocity": 0.16716651866961724,
-                    "velocityX": 0.4046105912700521,
-                    "velocityY": 0.509835013927756,
-                    "timestamp": 0.07826254233257365
-                },
-                {
-                    "x": 1.4449977124682947,
-                    "y": 5.679702872674784,
-                    "heading": 0.039246215534929935,
-                    "angularVelocity": 0.3343021835373132,
-                    "velocityX": 0.8092231232623148,
-                    "velocityY": 1.0196690004717623,
-                    "timestamp": 0.1565250846651473
-                },
-                {
-                    "x": 1.5399958043003432,
-                    "y": 5.799405527723277,
-                    "heading": 0.07848745324353076,
-                    "angularVelocity": 0.501405098007769,
-                    "velocityX": 1.2138385618545031,
-                    "velocityY": 1.5295012336785618,
-                    "timestamp": 0.23478762699772096
-                },
-                {
-                    "x": 1.6666604122853452,
-                    "y": 5.959008727776544,
-                    "heading": 0.1308059995871385,
-                    "angularVelocity": 0.6685004701391144,
-                    "velocityX": 1.6184576198246376,
-                    "velocityY": 2.039330633740969,
-                    "timestamp": 0.3130501693302946
-                },
-                {
-                    "x": 1.8249918316073899,
-                    "y": 6.158512135429991,
-                    "heading": 0.19620604644996878,
-                    "angularVelocity": 0.8356494040905814,
-                    "velocityX": 2.023080449510844,
-                    "velocityY": 2.5491557226145924,
-                    "timestamp": 0.3913127116628683
-                },
-                {
-                    "x": 1.9833278311922586,
-                    "y": 6.358008426099132,
-                    "heading": 0.26178707056088246,
-                    "angularVelocity": 0.8379618417228061,
-                    "velocityX": 2.023138973840461,
-                    "velocityY": 2.549064785314409,
-                    "timestamp": 0.469575253995442
-                },
-                {
-                    "x": 2.109996681019138,
-                    "y": 6.517605179473628,
-                    "heading": 0.31426571552794874,
-                    "angularVelocity": 0.6705461310477281,
-                    "velocityX": 1.618511819979031,
-                    "velocityY": 2.039248261272889,
-                    "timestamp": 0.5478377963280157
-                },
-                {
-                    "x": 2.204998332006287,
-                    "y": 6.637302612224517,
-                    "heading": 0.35363163522076196,
-                    "angularVelocity": 0.5029982226430836,
-                    "velocityX": 1.2138840389754155,
-                    "velocityY": 1.5294345057465644,
-                    "timestamp": 0.6261003386605893
-                },
-                {
-                    "x": 2.268332773183591,
-                    "y": 6.717100865272008,
-                    "heading": 0.3798775217934225,
-                    "angularVelocity": 0.3353569382033373,
-                    "velocityX": 0.8092561178011136,
-                    "velocityY": 1.0196225508288703,
-                    "timestamp": 0.704362880993163
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "angularVelocity": 0.1676725265429579,
-                    "velocityX": 0.40462813847574725,
-                    "velocityY": 0.509811380244222,
-                    "timestamp": 0.7826254233257367
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "angularVelocity": 9.092584352802723e-32,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0.8608879656583104
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "Center To W2": {
-            "waypoints": [
-                {
-                    "x": 1.35,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 7
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 1.35,
-                    "y": 5.56,
-                    "heading": 7.039472547405692e-42,
-                    "angularVelocity": 1.0705191064262213e-40,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 1.4250000045295101,
-                    "y": 5.56,
-                    "heading": 8.194631828276944e-23,
-                    "angularVelocity": 8.6403876700238e-22,
-                    "velocityX": 0.7907971475606789,
-                    "velocityY": 8.424071056371489e-35,
-                    "timestamp": 0.0948410155004449
-                },
-                {
-                    "x": 1.5750000090590195,
-                    "y": 5.56,
-                    "heading": 2.6954329855279996e-22,
-                    "angularVelocity": 1.978015178210688e-21,
-                    "velocityX": 1.5815942473623736,
-                    "velocityY": 1.2488550168110521e-34,
-                    "timestamp": 0.1896820310008898
-                },
-                {
-                    "x": 1.7999999999999998,
-                    "y": 5.56,
-                    "heading": 5.87848829010012e-22,
-                    "angularVelocity": 3.3562006983676933e-21,
-                    "velocityX": 2.3723912038871497,
-                    "velocityY": 2.135553914770072e-34,
-                    "timestamp": 0.2845230465013347
-                },
-                {
-                    "x": 2.0249999909409806,
-                    "y": 5.56,
-                    "heading": 4.475372520266276e-22,
-                    "angularVelocity": -1.4794393741169351e-21,
-                    "velocityX": 2.37239120388715,
-                    "velocityY": 2.583104828416982e-34,
-                    "timestamp": 0.3793640620017796
-                },
-                {
-                    "x": 2.17499999547049,
-                    "y": 5.56,
-                    "heading": 1.6184867431561565e-22,
-                    "angularVelocity": -3.012289189189882e-21,
-                    "velocityX": 1.5815942473623736,
-                    "velocityY": 1.9272829246954695e-34,
-                    "timestamp": 0.47420507750222446
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 1.246024226890136e-41,
-                    "angularVelocity": -1.7065260802739357e-21,
-                    "velocityX": 0.7907971475606789,
-                    "velocityY": -3.1542839688982834e-34,
-                    "timestamp": 0.5690460930026694
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 7.131086050293936e-41,
-                    "angularVelocity": 6.207915462550707e-40,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0.6638871085031143
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "Center To W3": {
-            "waypoints": [
-                {
-                    "x": 1.35,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 11
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 1.35,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 5.56203910119815e-35,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 1.3816658774685813,
-                    "y": 5.519999060819193,
-                    "heading": -0.012983997100427386,
-                    "angularVelocity": -0.16577951920330813,
-                    "velocityX": 0.4043095436088268,
-                    "velocityY": -0.5107315115509736,
-                    "timestamp": 0.07832087559926142
-                },
-                {
-                    "x": 1.4449977779940397,
-                    "y": 5.439997258819368,
-                    "heading": -0.038949622534069785,
-                    "angularVelocity": -0.33152879401526647,
-                    "velocityX": 0.8086209460872757,
-                    "velocityY": -1.0214620481155445,
-                    "timestamp": 0.15664175119852283
-                },
-                {
-                    "x": 1.5399959199593471,
-                    "y": 5.3199947242673264,
-                    "heading": -0.07789432529768456,
-                    "angularVelocity": -0.497245497648426,
-                    "velocityX": 1.2129351368768808,
-                    "velocityY": -1.5321909214351659,
-                    "timestamp": 0.23496262679778424
-                },
-                {
-                    "x": 1.6666605759505058,
-                    "y": 5.159991667949077,
-                    "heading": -0.1298174067167751,
-                    "angularVelocity": -0.662953280613991,
-                    "velocityX": 1.6172528080412967,
-                    "velocityY": -2.042917103441561,
-                    "timestamp": 0.31328350239704567
-                },
-                {
-                    "x": 1.8249920311985446,
-                    "y": 4.959988411604181,
-                    "heading": -0.19472272142711608,
-                    "angularVelocity": -0.828710279522869,
-                    "velocityX": 2.0215741210320677,
-                    "velocityY": -2.5536391774810774,
-                    "timestamp": 0.3916043779963071
-                },
-                {
-                    "x": 1.9833279596837488,
-                    "y": 4.759991951944026,
-                    "heading": -0.259798921868918,
-                    "angularVelocity": -0.8308921464919682,
-                    "velocityX": 2.021631235270526,
-                    "velocityY": -2.553552397491847,
-                    "timestamp": 0.4699252535955685
-                },
-                {
-                    "x": 2.1099967561163506,
-                    "y": 4.599995051908685,
-                    "heading": -0.3118731002379449,
-                    "angularVelocity": -0.6648824846579983,
-                    "velocityX": 1.6173056731479674,
-                    "velocityY": -2.042838500095239,
-                    "timestamp": 0.54824612919483
-                },
-                {
-                    "x": 2.2049983692603425,
-                    "y": 4.4799975041635465,
-                    "heading": -0.3509353765811358,
-                    "angularVelocity": -0.4987466757018699,
-                    "velocityX": 1.2129794568446812,
-                    "velocityY": -1.532127249944463,
-                    "timestamp": 0.6265670047940913
-                },
-                {
-                    "x": 2.268332785802464,
-                    "y": 4.399999173265512,
-                    "heading": -0.3769787554180828,
-                    "angularVelocity": -0.33252154853581545,
-                    "velocityX": 0.808653070557838,
-                    "velocityY": -1.021417729129522,
-                    "timestamp": 0.7048878803933527
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": -0.16625509460009477,
-                    "velocityX": 0.40432661094808764,
-                    "velocityY": -0.5107089643656818,
-                    "timestamp": 0.7832087559926141
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": -5.703180031220666e-32,
-                    "velocityX": 1.0257416304055705e-32,
-                    "velocityY": 0,
-                    "timestamp": 0.8615296315918755
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "Center To C1": {
-            "waypoints": [
-                {
-                    "x": 1.35,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 10
-                },
-                {
-                    "x": 2.896845817565918,
-                    "y": 6.343715190887451,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 18
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 1.35,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": -1.4340376968599448e-32,
-                    "velocityX": -4.9971810322519555e-29,
-                    "velocityY": 8.414879145245036e-29,
-                    "timestamp": 0
-                },
-                {
-                    "x": 1.3816327251489127,
-                    "y": 5.576194684745321,
-                    "heading": 4.506697153639657e-18,
-                    "angularVelocity": 6.903205799499945e-17,
-                    "velocityX": 0.4845393494564494,
-                    "velocityY": 0.24806468536018447,
-                    "timestamp": 0.06528412023584423
-                },
-                {
-                    "x": 1.4448981743168023,
-                    "y": 5.608584053657482,
-                    "heading": 9.944941296273304e-18,
-                    "angularVelocity": 8.330117912909241e-17,
-                    "velocityX": 0.9690786816049286,
-                    "velocityY": 0.4961293618593833,
-                    "timestamp": 0.13056824047168847
-                },
-                {
-                    "x": 1.5397963458087682,
-                    "y": 5.657168105868761,
-                    "heading": 6.849951425963079e-18,
-                    "angularVelocity": -4.74080026246192e-17,
-                    "velocityX": 1.4536179877914959,
-                    "velocityY": 0.7441940250671262,
-                    "timestamp": 0.1958523607075327
-                },
-                {
-                    "x": 1.666327236799984,
-                    "y": 5.721946839932961,
-                    "heading": -2.8668788112285783e-18,
-                    "angularVelocity": -1.4883910439092565e-16,
-                    "velocityX": 1.9381572507083278,
-                    "velocityY": 0.9922586661225022,
-                    "timestamp": 0.26113648094337694
-                },
-                {
-                    "x": 1.8244908416408254,
-                    "y": 5.802920252957701,
-                    "heading": -1.6578007712929266e-17,
-                    "angularVelocity": -2.1002241242045617e-16,
-                    "velocityX": 2.4226964270861338,
-                    "velocityY": 1.2403232628733727,
-                    "timestamp": 0.32642060117922117
-                },
-                {
-                    "x": 2.0142871433826555,
-                    "y": 5.900088336265959,
-                    "heading": -2.5925213294324244e-17,
-                    "angularVelocity": -1.4317730798527664e-16,
-                    "velocityX": 2.9072353438504677,
-                    "velocityY": 1.4883877267125722,
-                    "timestamp": 0.3917047214150654
-                },
-                {
-                    "x": 2.234118517125294,
-                    "y": 6.012633170077576,
-                    "heading": -1.2540245641654488e-17,
-                    "angularVelocity": 2.0502639235211304e-16,
-                    "velocityX": 3.3673023845382315,
-                    "velocityY": 1.723923572915424,
-                    "timestamp": 0.45698884165090964
-                },
-                {
-                    "x": 2.4539498908690707,
-                    "y": 6.125178003888657,
-                    "heading": 1.1802241639861868e-17,
-                    "angularVelocity": 3.728699597195476e-16,
-                    "velocityX": 3.3673023845556527,
-                    "velocityY": 1.72392357290723,
-                    "timestamp": 0.5222729618867539
-                },
-                {
-                    "x": 2.6737813045614214,
-                    "y": 6.237722759669026,
-                    "heading": 3.649409202314283e-17,
-                    "angularVelocity": 3.78221385080107e-16,
-                    "velocityX": 3.367302996474358,
-                    "velocityY": 1.7239223776592436,
-                    "timestamp": 0.5875570821225982
-                },
-                {
-                    "x": 2.896845817565918,
-                    "y": 6.343715190887451,
-                    "heading": 3.1275322529875387e-17,
-                    "angularVelocity": -7.99393399896351e-17,
-                    "velocityX": 3.416826514605052,
-                    "velocityY": 1.6235560935112305,
-                    "timestamp": 0.6528412023584425
-                },
-                {
-                    "x": 3.2165948847884085,
-                    "y": 6.4256885407325814,
-                    "heading": 6.106570262583634e-20,
-                    "angularVelocity": -3.577262143903813e-16,
-                    "velocityX": 3.664435260812148,
-                    "velocityY": 0.9394430333407913,
-                    "timestamp": 0.7400985952347529
-                },
-                {
-                    "x": 3.5380520485012785,
-                    "y": 6.500684460821759,
-                    "heading": -4.2865718547534536e-17,
-                    "angularVelocity": -4.919558421997663e-16,
-                    "velocityX": 3.6840106392881062,
-                    "velocityY": 0.8594792672236512,
-                    "timestamp": 0.8273559881110633
-                },
-                {
-                    "x": 3.8595092199655934,
-                    "y": 6.5756803476856875,
-                    "heading": -3.198465678230611e-17,
-                    "angularVelocity": 1.2470074347342177e-16,
-                    "velocityX": 3.684010728122356,
-                    "velocityY": 0.8594788864507628,
-                    "timestamp": 0.9146133809873737
-                },
-                {
-                    "x": 4.180966391429945,
-                    "y": 6.6506762345494606,
-                    "heading": 1.830329484329921e-18,
-                    "angularVelocity": 3.8753147618851267e-16,
-                    "velocityX": 3.6840107281227708,
-                    "velocityY": 0.8594788864489847,
-                    "timestamp": 1.001870773863684
-                },
-                {
-                    "x": 4.502423562894296,
-                    "y": 6.725672121413234,
-                    "heading": 1.519427405027002e-17,
-                    "angularVelocity": 1.531554419106218e-16,
-                    "velocityX": 3.684010728122771,
-                    "velocityY": 0.8594788864489847,
-                    "timestamp": 1.0891281667399946
-                },
-                {
-                    "x": 4.823880734358647,
-                    "y": 6.800668008277006,
-                    "heading": 2.800360653678013e-17,
-                    "angularVelocity": 1.4679939507274274e-16,
-                    "velocityX": 3.6840107281227708,
-                    "velocityY": 0.8594788864489846,
-                    "timestamp": 1.176385559616305
-                },
-                {
-                    "x": 5.1453379058229975,
-                    "y": 6.875663895140779,
-                    "heading": -9.132380596349043e-18,
-                    "angularVelocity": -4.2559129844470954e-16,
-                    "velocityX": 3.6840107281227708,
-                    "velocityY": 0.8594788864489846,
-                    "timestamp": 1.2636429524926156
-                },
-                {
-                    "x": 5.466795077287348,
-                    "y": 6.950659782004552,
-                    "heading": 1.9053427809876355e-18,
-                    "angularVelocity": 1.2649613980635305e-16,
-                    "velocityX": 3.6840107281227708,
-                    "velocityY": 0.8594788864489846,
-                    "timestamp": 1.3509003453689261
-                },
-                {
-                    "x": 5.788252248751699,
-                    "y": 7.025655668868324,
-                    "heading": -3.1119644574102345e-18,
-                    "angularVelocity": -5.750008191867264e-17,
-                    "velocityX": 3.684010728122771,
-                    "velocityY": 0.8594788864489846,
-                    "timestamp": 1.4381577382452366
-                },
-                {
-                    "x": 6.10970942021605,
-                    "y": 7.100651555732097,
-                    "heading": 3.013239534330982e-17,
-                    "angularVelocity": 3.80991898964227e-16,
-                    "velocityX": 3.6840107281227708,
-                    "velocityY": 0.8594788864489846,
-                    "timestamp": 1.5254151311215471
-                },
-                {
-                    "x": 6.431166591680401,
-                    "y": 7.17564744259587,
-                    "heading": 3.192919981892433e-17,
-                    "angularVelocity": 2.0592002767735807e-17,
-                    "velocityX": 3.6840107281227708,
-                    "velocityY": 0.8594788864489847,
-                    "timestamp": 1.6126725239978577
-                },
-                {
-                    "x": 6.752623763144745,
-                    "y": 7.250643329459641,
-                    "heading": 5.4408724078031107e-17,
-                    "angularVelocity": 2.576231481446227e-16,
-                    "velocityX": 3.6840107281226917,
-                    "velocityY": 0.8594788864489661,
-                    "timestamp": 1.6999299168741682
-                },
-                {
-                    "x": 7.061749152944237,
-                    "y": 7.322762214393876,
-                    "heading": 4.954044927264927e-17,
-                    "angularVelocity": -5.579212125566035e-17,
-                    "velocityX": 3.5426842312110383,
-                    "velocityY": 0.8265074460391663,
-                    "timestamp": 1.7871873097504787
-                },
-                {
-                    "x": 7.309049484595465,
-                    "y": 7.380457326963315,
-                    "heading": 3.4625344335160436e-17,
-                    "angularVelocity": -1.7093228018551423e-16,
-                    "velocityX": 2.834147612017054,
-                    "velocityY": 0.6612060098016332,
-                    "timestamp": 1.8744447026267892
-                },
-                {
-                    "x": 7.49452473986686,
-                    "y": 7.4237286629145345,
-                    "heading": 2.1983691983745354e-17,
-                    "angularVelocity": -1.448777241544276e-16,
-                    "velocityX": 2.125610783882924,
-                    "velocityY": 0.49590452481841046,
-                    "timestamp": 1.9617020955030997
-                },
-                {
-                    "x": 7.618174912681235,
-                    "y": 7.452576220829731,
-                    "heading": 1.0249539478821592e-17,
-                    "angularVelocity": -1.3447746039961902e-16,
-                    "velocityX": 1.4170738861021466,
-                    "velocityY": 0.3306030235866404,
-                    "timestamp": 2.04895948837941
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "angularVelocity": -1.174632794233726e-16,
-                    "velocityX": 0.7085369534980623,
-                    "velocityY": 0.16530151423060047,
-                    "timestamp": 2.1362168812557205
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": -1.0281234266250986e-33,
-                    "velocityY": 0,
-                    "timestamp": 2.223474274132031
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "Center To C2": {
-            "waypoints": [
-                {
-                    "x": 1.35,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 10
-                },
-                {
-                    "x": 2.976557731628418,
-                    "y": 6.303859233856201,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 18
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 1.35,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": -1.3272791352480801e-30,
-                    "velocityX": -6.138871086325045e-33,
-                    "velocityY": -8.954734510196666e-33,
-                    "timestamp": 0
-                },
-                {
-                    "x": 1.3835743361511121,
-                    "y": 5.575814713247826,
-                    "heading": -0.005199936040550204,
-                    "angularVelocity": -0.07791207752010428,
-                    "velocityX": 0.5030535492153302,
-                    "velocityY": 0.23695621540617479,
-                    "timestamp": 0.06674107796969504
-                },
-                {
-                    "x": 1.4507230090250494,
-                    "y": 5.607444140582854,
-                    "heading": -0.015599510401707435,
-                    "angularVelocity": -0.15581969422009245,
-                    "velocityX": 1.0061071069967922,
-                    "velocityY": 0.4739124433888902,
-                    "timestamp": 0.13348215593939008
-                },
-                {
-                    "x": 1.5514460199136038,
-                    "y": 5.654888284136176,
-                    "heading": -0.03119820909467289,
-                    "angularVelocity": -0.23371960968398373,
-                    "velocityX": 1.5091606841335288,
-                    "velocityY": 0.7108687033024069,
-                    "timestamp": 0.20022323390908514
-                },
-                {
-                    "x": 1.6857433700165965,
-                    "y": 5.718147147394209,
-                    "heading": -0.051995311102797384,
-                    "angularVelocity": -0.3116087219563307,
-                    "velocityX": 2.0122142792475226,
-                    "velocityY": 0.9478250154538677,
-                    "timestamp": 0.26696431187878017
-                },
-                {
-                    "x": 1.853615058416753,
-                    "y": 5.79722073482494,
-                    "heading": -0.07798989994858517,
-                    "angularVelocity": -0.3894841023932979,
-                    "velocityX": 2.515267860617741,
-                    "velocityY": 1.184781394550373,
-                    "timestamp": 0.3337053898484752
-                },
-                {
-                    "x": 2.0550610733526136,
-                    "y": 5.892109048523229,
-                    "heading": -0.10918084380142022,
-                    "angularVelocity": -0.4673425242996203,
-                    "velocityX": 3.0183212657627503,
-                    "velocityY": 1.4217378050347718,
-                    "timestamp": 0.4004464678181702
-                },
-                {
-                    "x": 2.283467758752316,
-                    "y": 5.999697539336407,
-                    "heading": -0.1091808488901602,
-                    "angularVelocity": -7.624599630551263e-8,
-                    "velocityX": 3.422280435797193,
-                    "velocityY": 1.6120280655645223,
-                    "timestamp": 0.46718754578786525
-                },
-                {
-                    "x": 2.511874447545338,
-                    "y": 6.107286022946165,
-                    "heading": -0.10918085397868219,
-                    "angularVelocity": -7.624272995870229e-8,
-                    "velocityX": 3.4222804866402323,
-                    "velocityY": 1.6120279576336907,
-                    "timestamp": 0.5339286237575603
-                },
-                {
-                    "x": 2.7402812851669234,
-                    "y": 6.214874190597382,
-                    "heading": -0.10918085906719958,
-                    "angularVelocity": -7.624266119750657e-8,
-                    "velocityX": 3.4222827165797036,
-                    "velocityY": 1.612023223539615,
-                    "timestamp": 0.6006697017272554
-                },
-                {
-                    "x": 2.976557731628418,
-                    "y": 6.303859233856201,
-                    "heading": -0.10918086426900071,
-                    "angularVelocity": -7.794002274324629e-8,
-                    "velocityX": 3.5401952388119984,
-                    "velocityY": 1.3332874740084952,
-                    "timestamp": 0.6674107796969505
-                },
-                {
-                    "x": 3.298504794987031,
-                    "y": 6.35898613680169,
-                    "heading": -0.1091808690742104,
-                    "angularVelocity": -5.565219068812679e-8,
-                    "velocityX": 3.7286737752062145,
-                    "velocityY": 0.6384597367556102,
-                    "timestamp": 0.753754365316963
-                },
-                {
-                    "x": 3.623164348220099,
-                    "y": 6.394833894219333,
-                    "heading": -0.10918087380844538,
-                    "angularVelocity": -5.483018746401896e-8,
-                    "velocityX": 3.760088846227146,
-                    "velocityY": 0.4151756863029092,
-                    "timestamp": 0.8400979509369756
-                },
-                {
-                    "x": 3.94782391548866,
-                    "y": 6.430681524522641,
-                    "heading": -0.10918087854267373,
-                    "angularVelocity": -5.483011064647812e-8,
-                    "velocityX": 3.7600890087811236,
-                    "velocityY": 0.41517421411094135,
-                    "timestamp": 0.9264415365569881
-                },
-                {
-                    "x": 4.272483486014202,
-                    "y": 6.466529125328635,
-                    "heading": -0.10918088327689503,
-                    "angularVelocity": -5.483002882633892e-8,
-                    "velocityX": 3.7600890465022867,
-                    "velocityY": 0.4151738724837589,
-                    "timestamp": 1.0127851221770008
-                },
-                {
-                    "x": 4.597143061517746,
-                    "y": 6.502376681050562,
-                    "heading": -0.10918088801110931,
-                    "angularVelocity": -5.482994771911856e-8,
-                    "velocityX": 3.760089104155704,
-                    "velocityY": 0.4151733503364993,
-                    "timestamp": 1.0991287077970133
-                },
-                {
-                    "x": 4.921802670469747,
-                    "y": 6.538223933839528,
-                    "heading": -0.10918089274532473,
-                    "angularVelocity": -5.482996090104271e-8,
-                    "velocityX": 3.76008949154355,
-                    "velocityY": 0.41516984187713296,
-                    "timestamp": 1.1854722934170259
-                },
-                {
-                    "x": 5.24839709757225,
-                    "y": 6.543220677288062,
-                    "heading": -0.10918090874246278,
-                    "angularVelocity": -1.8527303358226695e-7,
-                    "velocityX": 3.782497851546312,
-                    "velocityY": 0.05787046498769136,
-                    "timestamp": 1.2718158790370384
-                },
-                {
-                    "x": 5.573089202193745,
-                    "y": 6.507668986743235,
-                    "heading": -0.10918094712419395,
-                    "angularVelocity": -4.4452324855688095e-7,
-                    "velocityX": 3.760465844566904,
-                    "velocityY": -0.4117467474802974,
-                    "timestamp": 1.358159464657051
-                },
-                {
-                    "x": 5.897772644305168,
-                    "y": 6.472038270745821,
-                    "heading": -0.10918098550925279,
-                    "angularVelocity": -4.4456178841484606e-7,
-                    "velocityX": 3.76036551852636,
-                    "velocityY": -0.4126619915255808,
-                    "timestamp": 1.4445030502770635
-                },
-                {
-                    "x": 6.218673539970231,
-                    "y": 6.411116178321686,
-                    "heading": -0.10918101517811797,
-                    "angularVelocity": -3.436140042414391e-7,
-                    "velocityX": 3.716557441537206,
-                    "velocityY": -0.7055775132184731,
-                    "timestamp": 1.530846635897076
-                },
-                {
-                    "x": 6.522221927429097,
-                    "y": 6.290504141989104,
-                    "heading": -0.10918101099769996,
-                    "angularVelocity": 4.8416080721557094e-8,
-                    "velocityX": 3.5155870037033834,
-                    "velocityY": -1.3968847305389978,
-                    "timestamp": 1.6171902215170886
-                },
-                {
-                    "x": 6.822696463094792,
-                    "y": 6.162426273800445,
-                    "heading": -0.10918100683974102,
-                    "angularVelocity": 4.8155967911031915e-8,
-                    "velocityX": 3.479986770390183,
-                    "velocityY": -1.4833512792984263,
-                    "timestamp": 1.7035338071371011
-                },
-                {
-                    "x": 7.1084642887127405,
-                    "y": 6.040617535278895,
-                    "heading": -0.0727866812513063,
-                    "angularVelocity": 0.42150584003543307,
-                    "velocityX": 3.309659004382518,
-                    "velocityY": -1.4107444999749623,
-                    "timestamp": 1.7898773927571137
-                },
-                {
-                    "x": 7.337078567452098,
-                    "y": 5.943170529564944,
-                    "heading": -0.043671737033755105,
-                    "angularVelocity": 0.33719869297161775,
-                    "velocityX": 2.6477274148129606,
-                    "velocityY": -1.1285957725082563,
-                    "timestamp": 1.8762209783771262
-                },
-                {
-                    "x": 7.508539282574383,
-                    "y": 5.870085268029055,
-                    "heading": -0.021835852662427283,
-                    "angularVelocity": 0.2528952696894573,
-                    "velocityX": 1.985795631384405,
-                    "velocityY": -0.8464469133529862,
-                    "timestamp": 1.9625645639971387
-                },
-                {
-                    "x": 7.622846427592322,
-                    "y": 5.821361756830362,
-                    "heading": -0.007278657657073996,
-                    "angularVelocity": 0.16859613717477176,
-                    "velocityX": 1.323863772822564,
-                    "velocityY": -0.564297982864862,
-                    "timestamp": 2.0489081496171515
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "angularVelocity": 0.08429876527373403,
-                    "velocityX": 0.6619318852381739,
-                    "velocityY": -0.282149005689615,
-                    "timestamp": 2.1352517352371643
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": -1.8142028796058936e-31,
-                    "velocityY": -1.068352585526189e-31,
-                    "timestamp": 2.221595320857177
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 5.472021923065186,
-                    "y": 5.359512748718262,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "Center To C3": {
-            "waypoints": [
-                {
-                    "x": 1.35,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 10
-                },
-                {
-                    "x": 2.936701774597168,
-                    "y": 6.303859233856201,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 19
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 1.35,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": -7.013886661360986e-29,
-                    "velocityX": -3.078938571640024e-30,
-                    "velocityY": -8.674299238088502e-31,
-                    "timestamp": 0
-                },
-                {
-                    "x": 1.3762618085283742,
-                    "y": 5.591108793803365,
-                    "heading": -0.012389964804724785,
-                    "angularVelocity": -0.17699325527244014,
-                    "velocityX": 0.3751554628363854,
-                    "velocityY": 0.44439566776177375,
-                    "timestamp": 0.07000246865606696
-                },
-                {
-                    "x": 1.4301971031295795,
-                    "y": 5.652094846604856,
-                    "heading": -0.03662832018577037,
-                    "angularVelocity": -0.3462500086931304,
-                    "velocityX": 0.7704770365480613,
-                    "velocityY": 0.871198601597385,
-                    "timestamp": 0.14000493731213393
-                },
-                {
-                    "x": 1.513800165351683,
-                    "y": 5.741006259883096,
-                    "heading": -0.07186447572819399,
-                    "angularVelocity": -0.5033558989907949,
-                    "velocityX": 1.1942873419625006,
-                    "velocityY": 1.2701182541879128,
-                    "timestamp": 0.2100074059682009
-                },
-                {
-                    "x": 1.6300142433743692,
-                    "y": 5.854390700910866,
-                    "heading": -0.1166088429554257,
-                    "angularVelocity": -0.6391827043487479,
-                    "velocityX": 1.660142567166188,
-                    "velocityY": 1.6197206070673247,
-                    "timestamp": 0.28000987462426785
-                },
-                {
-                    "x": 1.7831654317988928,
-                    "y": 5.985149557302319,
-                    "heading": -0.16783055719619017,
-                    "angularVelocity": -0.7317129698935645,
-                    "velocityX": 2.187796964375065,
-                    "velocityY": 1.8679177877841964,
-                    "timestamp": 0.3500123432803348
-                },
-                {
-                    "x": 1.9771685219359858,
-                    "y": 6.116694524674393,
-                    "heading": -0.21853613173426145,
-                    "angularVelocity": -0.7243398055979559,
-                    "velocityX": 2.771374979505968,
-                    "velocityY": 1.8791475486148668,
-                    "timestamp": 0.4200148119364018
-                },
-                {
-                    "x": 2.202537147154592,
-                    "y": 6.2222527712605,
-                    "heading": -0.25772100339188303,
-                    "angularVelocity": -0.5597641398939388,
-                    "velocityX": 3.2194382504688988,
-                    "velocityY": 1.507921772083263,
-                    "timestamp": 0.49001728059246874
-                },
-                {
-                    "x": 2.4433286317618386,
-                    "y": 6.289980043332844,
-                    "heading": -0.29446027058227175,
-                    "angularVelocity": -0.5248281652898821,
-                    "velocityX": 3.4397570432873823,
-                    "velocityY": 0.9674983378806481,
-                    "timestamp": 0.5600197492485357
-                },
-                {
-                    "x": 2.6903031212328568,
-                    "y": 6.317347154244271,
-                    "heading": -0.33666392758398794,
-                    "angularVelocity": -0.6028881239905549,
-                    "velocityX": 3.528082569267318,
-                    "velocityY": 0.39094494004013314,
-                    "timestamp": 0.6300222179046027
-                },
-                {
-                    "x": 2.936701774597168,
-                    "y": 6.303859233856201,
-                    "heading": -0.37968716420906834,
-                    "angularVelocity": -0.6145959914234042,
-                    "velocityX": 3.5198566292706706,
-                    "velocityY": -0.1926777818987785,
-                    "timestamp": 0.7000246865606696
-                },
-                {
-                    "x": 3.257320421397908,
-                    "y": 6.21346874174157,
-                    "heading": -0.4320815750431537,
-                    "angularVelocity": -0.5604086145235933,
-                    "velocityX": 3.429324784540344,
-                    "velocityY": -0.9668132467920023,
-                    "timestamp": 0.7935179153869726
-                },
-                {
-                    "x": 3.5608980813516515,
-                    "y": 6.0524118876009405,
-                    "heading": -0.46560329407698275,
-                    "angularVelocity": -0.35854702479102396,
-                    "velocityX": 3.247055040935253,
-                    "velocityY": -1.7226579524796546,
-                    "timestamp": 0.8870111442132755
-                },
-                {
-                    "x": 3.8328887233978395,
-                    "y": 5.82633673900318,
-                    "heading": -0.46560402675062595,
-                    "angularVelocity": -0.00000783664977303189,
-                    "velocityX": 2.909201505399863,
-                    "velocityY": -2.418091143453467,
-                    "timestamp": 0.9805043730395785
-                },
-                {
-                    "x": 4.052764522028411,
-                    "y": 5.549310224236533,
-                    "heading": -0.46560404923284493,
-                    "angularVelocity": -2.404689503787649e-7,
-                    "velocityX": 2.3517831332905046,
-                    "velocityY": -2.963065007427709,
-                    "timestamp": 1.0739976018658814
-                },
-                {
-                    "x": 4.211188193978367,
-                    "y": 5.233096683639544,
-                    "heading": -0.46560406449531083,
-                    "angularVelocity": -1.6324674914477828e-7,
-                    "velocityX": 1.6944935364707439,
-                    "velocityY": -3.382207937052538,
-                    "timestamp": 1.1674908306921843
-                },
-                {
-                    "x": 4.383758324037826,
-                    "y": 4.9243757535417085,
-                    "heading": -0.4656040804041062,
-                    "angularVelocity": -1.701598650622723e-7,
-                    "velocityX": 1.8458035113972522,
-                    "velocityY": -3.302067261698653,
-                    "timestamp": 1.2609840595184871
-                },
-                {
-                    "x": 4.615944430524366,
-                    "y": 4.657582435311582,
-                    "heading": -0.46560407064369363,
-                    "angularVelocity": 1.0439700291593356e-7,
-                    "velocityX": 2.4834537153263514,
-                    "velocityY": -2.8536111286282813,
-                    "timestamp": 1.35447728834479
-                },
-                {
-                    "x": 4.89788653877274,
-                    "y": 4.444046330431443,
-                    "heading": -0.46560381887866387,
-                    "angularVelocity": 0.0000026928691303704606,
-                    "velocityX": 3.015642007317779,
-                    "velocityY": -2.2839740113892115,
-                    "timestamp": 1.4479705171710928
-                },
-                {
-                    "x": 5.217611687922929,
-                    "y": 4.2928350259750445,
-                    "heading": -0.4656035898817651,
-                    "angularVelocity": 0.000002449342071651111,
-                    "velocityX": 3.4197679678406536,
-                    "velocityY": -1.6173503295872917,
-                    "timestamp": 1.5414637459973957
-                },
-                {
-                    "x": 5.553860912374937,
-                    "y": 4.183174208089645,
-                    "heading": -0.46560339968651515,
-                    "angularVelocity": 0.0000020343211207896953,
-                    "velocityX": 3.5965088453273077,
-                    "velocityY": -1.1729279142677982,
-                    "timestamp": 1.6349569748236985
-                },
-                {
-                    "x": 5.905448307826856,
-                    "y": 4.144763712168745,
-                    "heading": -0.4656033544319255,
-                    "angularVelocity": 4.840413598760029e-7,
-                    "velocityX": 3.7605653357540834,
-                    "velocityY": -0.4108371954108264,
-                    "timestamp": 1.7284502036500013
-                },
-                {
-                    "x": 6.2591141169961295,
-                    "y": 4.141672024564684,
-                    "heading": -0.4656033129772286,
-                    "angularVelocity": 4.433978530009588e-7,
-                    "velocityX": 3.7827959693886872,
-                    "velocityY": -0.0330685723754928,
-                    "timestamp": 1.8219434324763042
-                },
-                {
-                    "x": 6.612779928543081,
-                    "y": 4.138580608962963,
-                    "heading": -0.465603271522502,
-                    "angularVelocity": 4.433981701164172e-7,
-                    "velocityX": 3.7827959948202516,
-                    "velocityY": -0.03306566304887272,
-                    "timestamp": 1.915436661302607
-                },
-                {
-                    "x": 6.963404638780936,
-                    "y": 4.132792400258934,
-                    "heading": -0.45779034574220756,
-                    "angularVelocity": 0.08356675535090403,
-                    "velocityX": 3.7502684915210915,
-                    "velocityY": -0.061910458935820756,
-                    "timestamp": 2.00892989012891
-                },
-                {
-                    "x": 7.251193869015599,
-                    "y": 4.13050050188738,
-                    "heading": -0.30806732380169355,
-                    "angularVelocity": 1.6014317167147898,
-                    "velocityX": 3.0781826004676223,
-                    "velocityY": -0.024514057331484977,
-                    "timestamp": 2.1024231189552127
-                },
-                {
-                    "x": 7.466125497714746,
-                    "y": 4.12875254378885,
-                    "heading": -0.16499798934349721,
-                    "angularVelocity": 1.5302641298654869,
-                    "velocityX": 2.29890048078732,
-                    "velocityY": -0.018696092973500365,
-                    "timestamp": 2.1959163477815156
-                },
-                {
-                    "x": 7.608856917870355,
-                    "y": 4.12758509655783,
-                    "heading": -0.05770853615881093,
-                    "angularVelocity": 1.147563888118738,
-                    "velocityX": 1.5266498114080966,
-                    "velocityY": -0.012486970935497041,
-                    "timestamp": 2.2894095766078184
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": -1.6446149791629635e-35,
-                    "angularVelocity": 0.6172482957672322,
-                    "velocityX": 0.7609436856846457,
-                    "velocityY": -0.0062581704063020965,
-                    "timestamp": 2.3829028054341213
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "angularVelocity": -1.0953722106158818e-30,
-                    "velocityX": 0,
-                    "velocityY": -1.282624680344039e-29,
-                    "timestamp": 2.476396034260424
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "Center To C4": {
-            "waypoints": [
-                {
-                    "x": 1.35,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                },
-                {
-                    "x": 2.956629753112793,
-                    "y": 6.283931255340576,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 40
-                },
-                {
-                    "x": 4.018431663513184,
-                    "y": 5.088562488555908,
-                    "heading": -0.4829717875114574,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                },
-                {
-                    "x": 6.893649101257324,
-                    "y": 3.5118300914764404,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 40
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 1.35,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 3.439885530040909e-30,
-                    "timestamp": 0
-                },
-                {
-                    "x": 1.3518715996750759,
-                    "y": 5.562488632849977,
-                    "heading": -0.00039025483625088287,
-                    "angularVelocity": -0.020188245249371933,
-                    "velocityX": 0.09681959001984766,
-                    "velocityY": 0.12873928941829918,
-                    "timestamp": 0.019330795293514086
-                },
-                {
-                    "x": 1.3556369831983786,
-                    "y": 5.567448998179342,
-                    "heading": -0.001175615506548702,
-                    "angularVelocity": -0.04062743712160119,
-                    "velocityX": 0.19478678792724713,
-                    "velocityY": 0.256604306964468,
-                    "timestamp": 0.03866159058702817
-                },
-                {
-                    "x": 1.3613203761105706,
-                    "y": 5.574862277415471,
-                    "heading": -0.0023613813379046128,
-                    "angularVelocity": -0.061340768103509684,
-                    "velocityX": 0.2940071955600798,
-                    "velocityY": 0.38349582226532697,
-                    "timestamp": 0.05799238588054226
-                },
-                {
-                    "x": 1.3689483300707626,
-                    "y": 5.58470739953387,
-                    "heading": -0.003953362765501964,
-                    "angularVelocity": -0.08235467829569511,
-                    "velocityX": 0.39460114518679135,
-                    "velocityY": 0.5092973138928011,
-                    "timestamp": 0.07732318117405634
-                },
-                {
-                    "x": 1.3785500603498269,
-                    "y": 5.596960627127346,
-                    "heading": -0.0059579558445008625,
-                    "angularVelocity": -0.10369946236363117,
-                    "velocityX": 0.4967064279184545,
-                    "velocityY": 0.6338708473927627,
-                    "timestamp": 0.09665397646757043
-                },
-                {
-                    "x": 1.3901578478597558,
-                    "y": 5.611595038412612,
-                    "heading": -0.008382231168191269,
-                    "angularVelocity": -0.12541001479146147,
-                    "velocityX": 0.6004816322183893,
-                    "velocityY": 0.7570516920313731,
-                    "timestamp": 0.11598477176108452
-                },
-                {
-                    "x": 1.4038075203372553,
-                    "y": 5.62857987161075,
-                    "heading": -0.01123404053815137,
-                    "angularVelocity": -0.14752674820972528,
-                    "velocityX": 0.7061102386242472,
-                    "velocityY": 0.8786412012669059,
-                    "timestamp": 0.1353155670545986
-                },
-                {
-                    "x": 1.4195390306280726,
-                    "y": 5.647879684835849,
-                    "heading": -0.014522145551621221,
-                    "angularVelocity": -0.17009672719326588,
-                    "velocityX": 0.8138056428591633,
-                    "velocityY": 0.9983972688167116,
-                    "timestamp": 0.1546463623481127
-                },
-                {
-                    "x": 1.4373971536587393,
-                    "y": 5.669453265066441,
-                    "heading": -0.018256373225668265,
-                    "angularVelocity": -0.19317506689959266,
-                    "velocityX": 0.923817295642181,
-                    "velocityY": 1.116021348476537,
-                    "timestamp": 0.17397715764162677
-                },
-                {
-                    "x": 1.457432326992564,
-                    "y": 5.693252190583062,
-                    "heading": -0.022447804782031403,
-                    "angularVelocity": -0.21682664850159125,
-                    "velocityX": 1.0364381304346626,
-                    "velocityY": 1.2311405276019616,
-                    "timestamp": 0.19330795293514086
-                },
-                {
-                    "x": 1.47970166115617,
-                    "y": 5.719218907072145,
-                    "heading": -0.027109004507978946,
-                    "angularVelocity": -0.24112819235695526,
-                    "velocityX": 1.1520133458284385,
-                    "velocityY": 1.3432823686149544,
-                    "timestamp": 0.21263874822865494
-                },
-                {
-                    "x": 1.5042701411060604,
-                    "y": 5.747284109919373,
-                    "heading": -0.03225429550223853,
-                    "angularVelocity": -0.2661706834165162,
-                    "velocityX": 1.2709502933970824,
-                    "velocityY": 1.4518390175412745,
-                    "timestamp": 0.23196954352216903
-                },
-                {
-                    "x": 1.5312120197646597,
-                    "y": 5.777363120811461,
-                    "heading": -0.037900086436595266,
-                    "angularVelocity": -0.2920620103121596,
-                    "velocityX": 1.3937284136281272,
-                    "velocityY": 1.5560151786502046,
-                    "timestamp": 0.25130033881568314
-                },
-                {
-                    "x": 1.5606123466917503,
-                    "y": 5.809350786193417,
-                    "heading": -0.044065244214455006,
-                    "angularVelocity": -0.3189293396494718,
-                    "velocityX": 1.5209062266028461,
-                    "velocityY": 1.6547516486654796,
-                    "timestamp": 0.27063113410919726
-                },
-                {
-                    "x": 1.5925684302904153,
-                    "y": 5.843114184319679,
-                    "heading": -0.050771482089097104,
-                    "angularVelocity": -0.34691991575183656,
-                    "velocityX": 1.653117893674399,
-                    "velocityY": 1.7466119532904336,
-                    "timestamp": 0.28996192940271137
-                },
-                {
-                    "x": 1.6271906907680806,
-                    "y": 5.8784820940308995,
-                    "heading": -0.058043668844819325,
-                    "angularVelocity": -0.37619697717051565,
-                    "velocityX": 1.791041700663091,
-                    "velocityY": 1.8296148282676763,
-                    "timestamp": 0.3092927246962255
-                },
-                {
-                    "x": 1.6646015916598045,
-                    "y": 5.915229821006168,
-                    "heading": -0.06590980894370155,
-                    "angularVelocity": -0.4069227354304089,
-                    "velocityX": 1.935300660096276,
-                    "velocityY": 1.9009940572697726,
-                    "timestamp": 0.3286235199897396
-                },
-                {
-                    "x": 1.7049297157110095,
-                    "y": 5.953057987948439,
-                    "heading": -0.07440007857488001,
-                    "angularVelocity": -0.43920953598980744,
-                    "velocityX": 2.086211324411271,
-                    "velocityY": 1.9568862205562443,
-                    "timestamp": 0.3479543152832537
-                },
-                {
-                    "x": 1.7482931222517035,
-                    "y": 5.991565656180575,
-                    "heading": -0.08354352935308812,
-                    "angularVelocity": -0.47299920357005454,
-                    "velocityX": 2.2432293075517364,
-                    "velocityY": 1.9920374535783947,
-                    "timestamp": 0.3672851105767678
-                },
-                {
-                    "x": 1.7947629094651933,
-                    "y": 6.030225122456552,
-                    "heading": -0.09335994332155541,
-                    "angularVelocity": -0.5078122146252688,
-                    "velocityX": 2.4039252657691588,
-                    "velocityY": 1.9998901074156783,
-                    "timestamp": 0.38661590587028194
-                },
-                {
-                    "x": 1.844301928458138,
-                    "y": 6.068381679285448,
-                    "heading": -0.10384499839936924,
-                    "angularVelocity": -0.5424016404194022,
-                    "velocityX": 2.562699477220483,
-                    "velocityY": 1.9738741344852324,
-                    "timestamp": 0.40594670116379605
-                },
-                {
-                    "x": 1.8967008920878712,
-                    "y": 6.105314232951604,
-                    "heading": -0.1149551002367539,
-                    "angularVelocity": -0.5747358899978738,
-                    "velocityX": 2.710647070341413,
-                    "velocityY": 1.9105553137044158,
-                    "timestamp": 0.42527749645731017
-                },
-                {
-                    "x": 1.951571944971935,
-                    "y": 6.140356350737874,
-                    "heading": -0.126607814812745,
-                    "angularVelocity": -0.6028057510857173,
-                    "velocityX": 2.83853054418689,
-                    "velocityY": 1.8127613093097652,
-                    "timestamp": 0.4446082917508243
-                },
-                {
-                    "x": 2.008427579479744,
-                    "y": 6.172999646985284,
-                    "heading": -0.13870025114422718,
-                    "angularVelocity": -0.6255529660251172,
-                    "velocityX": 2.9411947953783932,
-                    "velocityY": 1.6886680424557017,
-                    "timestamp": 0.4639390870443384
-                },
-                {
-                    "x": 2.066782288063465,
-                    "y": 6.202913115188225,
-                    "heading": -0.15112974078850008,
-                    "angularVelocity": -0.6429890470384952,
-                    "velocityX": 3.018743290054933,
-                    "velocityY": 1.5474515015415655,
-                    "timestamp": 0.4832698823378525
-                },
-                {
-                    "x": 2.126212031523665,
-                    "y": 6.229902950259444,
-                    "heading": -0.1638060412217236,
-                    "angularVelocity": -0.6557567984529158,
-                    "velocityX": 3.074355842986947,
-                    "velocityY": 1.3962092434072775,
-                    "timestamp": 0.5026006776313666
-                },
-                {
-                    "x": 2.1863688602207922,
-                    "y": 6.253863406934038,
-                    "heading": -0.17665473442964857,
-                    "angularVelocity": -0.6646748368514358,
-                    "velocityX": 3.111968637798953,
-                    "velocityY": 1.2394966844759612,
-                    "timestamp": 0.5219314729248807
-                },
-                {
-                    "x": 2.246973575487724,
-                    "y": 6.2747405977079325,
-                    "heading": -0.1896159238006329,
-                    "angularVelocity": -0.6704943678821678,
-                    "velocityX": 3.1351382261683427,
-                    "velocityY": 1.0799964748940398,
-                    "timestamp": 0.5412622682183948
-                },
-                {
-                    "x": 2.30780256528041,
-                    "y": 6.292510136082229,
-                    "heading": -0.20264154258755296,
-                    "angularVelocity": -0.6738273614273119,
-                    "velocityX": 3.146740155750102,
-                    "velocityY": 0.9192347290676759,
-                    "timestamp": 0.560593063511909
-                },
-                {
-                    "x": 2.3686753728703707,
-                    "y": 6.307164203375093,
-                    "heading": -0.2156927432279827,
-                    "angularVelocity": -0.6751507344764429,
-                    "velocityX": 3.1490068911124522,
-                    "velocityY": 0.7580685155659881,
-                    "timestamp": 0.5799238588054231
-                },
-                {
-                    "x": 2.4294446969864714,
-                    "y": 6.318704202261767,
-                    "heading": -0.228737776875863,
-                    "angularVelocity": -0.6748317102223548,
-                    "velocityX": 3.143653594867338,
-                    "velocityY": 0.5969748637576952,
-                    "timestamp": 0.5992546540989372
-                },
-                {
-                    "x": 2.4899887888473393,
-                    "y": 6.32713657767754,
-                    "heading": -0.24175037423147705,
-                    "angularVelocity": -0.6731537506881662,
-                    "velocityX": 3.132002121049924,
-                    "velocityY": 0.43621461443967424,
-                    "timestamp": 0.6185854493924513
-                },
-                {
-                    "x": 2.550205774126716,
-                    "y": 6.332470420209207,
-                    "heading": -0.25470853343764965,
-                    "angularVelocity": -0.6703376146412596,
-                    "velocityX": 3.1150805936877464,
-                    "velocityY": 0.2759246296222352,
-                    "timestamp": 0.6379162446859654
-                },
-                {
-                    "x": 2.610009417191064,
-                    "y": 6.334716080800322,
-                    "heading": -0.26759361438505785,
-                    "angularVelocity": -0.6665572084213006,
-                    "velocityX": 3.09369801688466,
-                    "velocityY": 0.11617010873154458,
-                    "timestamp": 0.6572470399794795
-                },
-                {
-                    "x": 2.669325940254228,
-                    "y": 6.333884367249437,
-                    "heading": -0.28038965768785973,
-                    "angularVelocity": -0.6619512083445117,
-                    "velocityX": 3.0684988466597254,
-                    "velocityY": -0.04302531469893782,
-                    "timestamp": 0.6765778352729936
-                },
-                {
-                    "x": 2.7280916104140402,
-                    "y": 6.329986080406231,
-                    "heading": -0.2930828674707726,
-                    "angularVelocity": -0.656631534822156,
-                    "velocityX": 3.040002714194029,
-                    "velocityY": -0.2016620001410218,
-                    "timestamp": 0.6959086305665078
-                },
-                {
-                    "x": 2.7862508879843872,
-                    "y": 6.323031751356315,
-                    "heading": -0.3056612140555153,
-                    "angularVelocity": -0.650689554866023,
-                    "velocityX": 3.0086334621659443,
-                    "velocityY": -0.35975390274029156,
-                    "timestamp": 0.7152394258600219
-                },
-                {
-                    "x": 2.843754988622848,
-                    "y": 6.3130314987287885,
-                    "heading": -0.31811412514136855,
-                    "angularVelocity": -0.6442006599713835,
-                    "velocityX": 2.974740550780866,
-                    "velocityY": -0.5173223592555559,
-                    "timestamp": 0.734570221153536
-                },
-                {
-                    "x": 2.9005607537314586,
-                    "y": 6.299994958196607,
-                    "heading": -0.3304322429824912,
-                    "angularVelocity": -0.6372276801904713,
-                    "velocityX": 2.9386150050261657,
-                    "velocityY": -0.6743923534566658,
-                    "timestamp": 0.7539010164470501
-                },
-                {
-                    "x": 2.956629753112793,
-                    "y": 6.283931255340576,
-                    "heading": -0.34260723133624194,
-                    "angularVelocity": -0.6298234588328613,
-                    "velocityX": 2.9005014294547,
-                    "velocityY": -0.8309902728844937,
-                    "timestamp": 0.7732318117405642
-                },
-                {
-                    "x": 2.9906169983281425,
-                    "y": 6.272983358953205,
-                    "heading": -0.34999362222052477,
-                    "angularVelocity": -0.625060967472129,
-                    "velocityX": 2.8761137487624757,
-                    "velocityY": -0.9264474104986867,
-                    "timestamp": 0.7850488844241373
-                },
-                {
-                    "x": 3.024312445989771,
-                    "y": 6.26090838895792,
-                    "heading": -0.35731821138357023,
-                    "angularVelocity": -0.6198311002374715,
-                    "velocityX": 2.8514208691014065,
-                    "velocityY": -1.0218241284130187,
-                    "timestamp": 0.7968659571077105
-                },
-                {
-                    "x": 3.0577122238990846,
-                    "y": 6.2477073818247915,
-                    "heading": -0.36457506252100974,
-                    "angularVelocity": -0.6140988831800235,
-                    "velocityX": 2.8264003111145226,
-                    "velocityY": -1.1171131367820717,
-                    "timestamp": 0.8086830297912836
-                },
-                {
-                    "x": 3.090812163934883,
-                    "y": 6.233381471982772,
-                    "heading": -0.3717577779780659,
-                    "angularVelocity": -0.6078252752934934,
-                    "velocityX": 2.8010270328463904,
-                    "velocityY": -1.2123061459996218,
-                    "timestamp": 0.8205001024748567
-                },
-                {
-                    "x": 3.1236077670194575,
-                    "y": 6.217931905831193,
-                    "heading": -0.3788594429566703,
-                    "angularVelocity": -0.6009665141923228,
-                    "velocityX": 2.7752730276563167,
-                    "velocityY": -1.307393680759415,
-                    "timestamp": 0.8323171751584298
-                },
-                {
-                    "x": 3.1560941623709735,
-                    "y": 6.201360058471141,
-                    "heading": -0.3858725603738981,
-                    "angularVelocity": -0.5934733249949906,
-                    "velocityX": 2.749106840704776,
-                    "velocityY": -1.4023648498911914,
-                    "timestamp": 0.844134247842003
-                },
-                {
-                    "x": 3.188266059838492,
-                    "y": 6.1836674538325775,
-                    "heading": -0.3927889743327873,
-                    "angularVelocity": -0.5852899566661331,
-                    "velocityX": 2.722492983583009,
-                    "velocityY": -1.4972070590002187,
-                    "timestamp": 0.8559513205255761
-                },
-                {
-                    "x": 3.2201176938016274,
-                    "y": 6.164855789080512,
-                    "heading": -0.399599779609834,
-                    "angularVelocity": -0.576352998701121,
-                    "velocityX": 2.695391220484986,
-                    "velocityY": -1.5919056483603757,
-                    "timestamp": 0.8677683932091492
-                },
-                {
-                    "x": 3.251642756703662,
-                    "y": 6.144926964467571,
-                    "heading": -0.40629521382129,
-                    "angularVelocity": -0.5665899153487628,
-                    "velocityX": 2.6677556909553144,
-                    "velocityY": -1.686443432022221,
-                    "timestamp": 0.8795854658927224
-                },
-                {
-                    "x": 3.282834319738247,
-                    "y": 6.123883120194607,
-                    "heading": -0.41286452792903033,
-                    "angularVelocity": -0.5559172126335613,
-                    "velocityX": 2.6395338227837533,
-                    "velocityY": -1.7808001047684463,
-                    "timestamp": 0.8914025385762955
-                },
-                {
-                    "x": 3.3136847374734915,
-                    "y": 6.101726682396313,
-                    "heading": -0.4192958293777808,
-                    "angularVelocity": -0.5442381223304499,
-                    "velocityX": 2.6106649727330486,
-                    "velocityY": -1.874951469926556,
-                    "timestamp": 0.9032196112598686
-                },
-                {
-                    "x": 3.3441855321977045,
-                    "y": 6.078460421163092,
-                    "heading": -0.42557589025707343,
-                    "angularVelocity": -0.5314396422408687,
-                    "velocityX": 2.5810787105179256,
-                    "velocityY": -1.9688684208198879,
-                    "timestamp": 0.9150366839434417
-                },
-                {
-                    "x": 3.37432725239814,
-                    "y": 6.0540875246663015,
-                    "heading": -0.4316899102075926,
-                    "angularVelocity": -0.5173887065125765,
-                    "velocityX": 2.5506926298537165,
-                    "velocityY": -2.0625155780475546,
-                    "timestamp": 0.9268537566270149
-                },
-                {
-                    "x": 3.4040992978747684,
-                    "y": 6.028611695167346,
-                    "heading": -0.43762121996231873,
-                    "angularVelocity": -0.5019271619587302,
-                    "velocityX": 2.519409525001445,
-                    "velocityY": -2.1558494375996973,
-                    "timestamp": 0.938670829310588
-                },
-                {
-                    "x": 3.4334897013019283,
-                    "y": 6.00203727528326,
-                    "heading": -0.44335090582849124,
-                    "angularVelocity": -0.4848650778070557,
-                    "velocityX": 2.48711370524239,
-                    "velocityY": -2.2488158104525207,
-                    "timestamp": 0.9504879019941611
-                },
-                {
-                    "x": 3.462484852215012,
-                    "y": 5.974369416891247,
-                    "heading": -0.4488573271132443,
-                    "angularVelocity": -0.46597168623725893,
-                    "velocityX": 2.4536661226929253,
-                    "velocityY": -2.341346214318684,
-                    "timestamp": 0.9623049746777342
-                },
-                {
-                    "x": 3.4910691438628887,
-                    "y": 5.945614311410449,
-                    "heading": -0.45411548589649775,
-                    "angularVelocity": -0.4449628875146618,
-                    "velocityX": 2.4188978449470153,
-                    "velocityY": -2.4333526797013256,
-                    "timestamp": 0.9741220473613074
-                },
-                {
-                    "x": 3.5192245152965964,
-                    "y": 5.915779510545288,
-                    "heading": -0.4590961890390477,
-                    "angularVelocity": -0.42148366824158007,
-                    "velocityX": 2.3826011896200363,
-                    "velocityY": -2.524720094735007,
-                    "timestamp": 0.9859391200448805
-                },
-                {
-                    "x": 3.5469298492407,
-                    "y": 5.884874383909108,
-                    "heading": -0.46376491150225324,
-                    "angularVelocity": -0.39508282535110545,
-                    "velocityX": 2.3445175201991417,
-                    "velocityY": -2.6152946219194915,
-                    "timestamp": 0.9977561927284536
-                },
-                {
-                    "x": 3.5741601690568516,
-                    "y": 5.852910789899007,
-                    "heading": -0.46808022074004524,
-                    "angularVelocity": -0.36517582258680764,
-                    "velocityX": 2.304320244556386,
-                    "velocityY": -2.7048656520944543,
-                    "timestamp": 1.0095732654120266
-                },
-                {
-                    "x": 3.6008855536785225,
-                    "y": 5.819904089620565,
-                    "heading": -0.4719915430906053,
-                    "angularVelocity": -0.3309891083260508,
-                    "velocityX": 2.2615909487314654,
-                    "velocityY": -2.7931367744165776,
-                    "timestamp": 1.0213903380955998
-                },
-                {
-                    "x": 3.62706965768041,
-                    "y": 5.785874731899473,
-                    "heading": -0.4754359323425916,
-                    "angularVelocity": -0.2914756762708573,
-                    "velocityX": 2.2157859821143067,
-                    "velocityY": -2.8796774490856496,
-                    "timestamp": 1.0332074107791729
-                },
-                {
-                    "x": 3.652667693323084,
-                    "y": 5.750850822222342,
-                    "heading": -0.4783333470461038,
-                    "angularVelocity": -0.24518887046705806,
-                    "velocityX": 2.1661909280000957,
-                    "velocityY": -2.9638397439848143,
-                    "timestamp": 1.045024483462746
-                },
-                {
-                    "x": 3.677623743737937,
-                    "y": 5.7148724362586085,
-                    "heading": -0.48057991095642455,
-                    "angularVelocity": -0.1901117112907446,
-                    "velocityX": 2.1118640024567648,
-                    "velocityY": -3.044610702424323,
-                    "timestamp": 1.0568415561463191
-                },
-                {
-                    "x": 3.701867477642102,
-                    "y": 5.67799904418439,
-                    "heading": -0.48203966919964636,
-                    "angularVelocity": -0.12352959843022769,
-                    "velocityX": 2.0515854097999866,
-                    "velocityY": -3.120349096733213,
-                    "timestamp": 1.0686586288298923
-                },
-                {
-                    "x": 3.725329217347855,
-                    "y": 5.640287080856699,
-                    "heading": -0.4827696693750387,
-                    "angularVelocity": -0.06177504318881954,
-                    "velocityX": 1.9854104594250916,
-                    "velocityY": -3.191311785711063,
-                    "timestamp": 1.0804757015134654
-                },
-                {
-                    "x": 3.74792659306077,
-                    "y": 5.60180905514361,
-                    "heading": -0.48297166201638797,
-                    "angularVelocity": -0.017093289239954308,
-                    "velocityX": 1.9122651030425624,
-                    "velocityY": -3.2561385330714976,
-                    "timestamp": 1.0922927741970385
-                },
-                {
-                    "x": 3.769555777117098,
-                    "y": 5.56268673154527,
-                    "heading": -0.48297173035274543,
-                    "angularVelocity": -0.000005782849890954889,
-                    "velocityX": 1.8303335043707702,
-                    "velocityY": -3.310661163379628,
-                    "timestamp": 1.1041098468806116
-                },
-                {
-                    "x": 3.7901587344932506,
-                    "y": 5.52301434523992,
-                    "heading": -0.48297173483145855,
-                    "angularVelocity": -3.790036003454015e-7,
-                    "velocityX": 1.743490788949144,
-                    "velocityY": -3.3572092994315077,
-                    "timestamp": 1.1159269195641848
-                },
-                {
-                    "x": 3.809721481337777,
-                    "y": 5.482818833345491,
-                    "heading": -0.4829717389588181,
-                    "angularVelocity": -3.4927089311686727e-7,
-                    "velocityX": 1.6554647135005363,
-                    "velocityY": -3.401477926957743,
-                    "timestamp": 1.127743992247758
-                },
-                {
-                    "x": 3.8282308263038667,
-                    "y": 5.442127501029994,
-                    "heading": -0.482971742791846,
-                    "angularVelocity": -3.2436357183409427e-7,
-                    "velocityX": 1.5663223424036268,
-                    "velocityY": -3.443435900336205,
-                    "timestamp": 1.139561064931331
-                },
-                {
-                    "x": 3.8456768429335884,
-                    "y": 5.400969063215312,
-                    "heading": -0.48297174636762497,
-                    "angularVelocity": -3.0259430956931755e-7,
-                    "velocityX": 1.4763399614162735,
-                    "velocityY": -3.482963921504598,
-                    "timestamp": 1.1513781376149042
-                },
-                {
-                    "x": 3.863665898181474,
-                    "y": 5.360045056052632,
-                    "heading": -0.48297175000247344,
-                    "angularVelocity": -3.075929693793739e-7,
-                    "velocityX": 1.5222936957043627,
-                    "velocityY": -3.463125619898205,
-                    "timestamp": 1.1631952102984773
-                },
-                {
-                    "x": 3.8827072670006824,
-                    "y": 5.319599949808908,
-                    "heading": -0.4829717539017329,
-                    "angularVelocity": -3.299683104273868e-7,
-                    "velocityX": 1.6113439706329509,
-                    "velocityY": -3.4225994310711667,
-                    "timestamp": 1.1750122829820504
-                },
-                {
-                    "x": 3.902794276163034,
-                    "y": 5.279663847505986,
-                    "heading": -0.48297175811002735,
-                    "angularVelocity": -3.5611987502134074e-7,
-                    "velocityX": 1.6998295347945822,
-                    "velocityY": -3.379525824397886,
-                    "timestamp": 1.1868293556656235
-                },
-                {
-                    "x": 3.92391456197569,
-                    "y": 5.240264436686968,
-                    "heading": -0.48297176267932185,
-                    "angularVelocity": -3.8666890356003695e-7,
-                    "velocityX": 1.78726884214022,
-                    "velocityY": -3.3341092057245874,
-                    "timestamp": 1.1986464283491967
-                },
-                {
-                    "x": 3.9460538007835475,
-                    "y": 5.201428448582004,
-                    "heading": -0.4829717676728866,
-                    "angularVelocity": -4.225720568590589e-7,
-                    "velocityX": 1.8734960341433124,
-                    "velocityY": -3.286430501434571,
-                    "timestamp": 1.2104635010327698
-                },
-                {
-                    "x": 3.969196975663963,
-                    "y": 5.163182231498957,
-                    "heading": -0.4829717731698369,
-                    "angularVelocity": -4.651702214495778e-7,
-                    "velocityX": 1.9584524441985565,
-                    "velocityY": -3.2365221156854496,
-                    "timestamp": 1.222280573716343
-                },
-                {
-                    "x": 3.993328387583419,
-                    "y": 5.125551733327025,
-                    "heading": -0.48297177927069973,
-                    "angularVelocity": -5.162753076431712e-7,
-                    "velocityX": 2.0420803498146496,
-                    "velocityY": -3.184417933236803,
-                    "timestamp": 1.234097646399916
-                },
-                {
-                    "x": 4.018431663513184,
-                    "y": 5.088562488555908,
-                    "heading": -0.4829717875114574,
-                    "angularVelocity": -6.973603296302728e-7,
-                    "velocityX": 2.124322715274553,
-                    "velocityY": -3.1301529373289303,
-                    "timestamp": 1.2459147190834892
-                },
-                {
-                    "x": 4.068270320597518,
-                    "y": 5.022262107474161,
-                    "heading": -0.48297170333499495,
-                    "angularVelocity": 0.000003839169547390073,
-                    "velocityX": 2.2730707479901486,
-                    "velocityY": -3.023866725833157,
-                    "timestamp": 1.2678404145896236
-                },
-                {
-                    "x": 4.121253956507243,
-                    "y": 4.9584470041490425,
-                    "heading": -0.48297162876403243,
-                    "angularVelocity": 0.0000034010762602073378,
-                    "velocityX": 2.4165087896481814,
-                    "velocityY": -2.910516718033651,
-                    "timestamp": 1.289766110095758
-                },
-                {
-                    "x": 4.177258824192737,
-                    "y": 4.89726621863139,
-                    "heading": -0.4829715614847619,
-                    "angularVelocity": 0.0000030685124925835,
-                    "velocityX": 2.5543029031769633,
-                    "velocityY": -2.7903692040481487,
-                    "timestamp": 1.3116918056018925
-                },
-                {
-                    "x": 4.236154120064876,
-                    "y": 4.83886263701252,
-                    "heading": -0.482971499836949,
-                    "angularVelocity": 0.000002811669663014854,
-                    "velocityX": 2.686131249777681,
-                    "velocityY": -2.6637048572771733,
-                    "timestamp": 1.333617501108027
-                },
-                {
-                    "x": 4.2978022891900425,
-                    "y": 4.7833726571672255,
-                    "heading": -0.48297144258671054,
-                    "angularVelocity": 0.0000026111025032599054,
-                    "velocityX": 2.811685910164931,
-                    "velocityY": -2.530819596111362,
-                    "timestamp": 1.3555431966141613
-                },
-                {
-                    "x": 4.362059346053497,
-                    "y": 4.730925869094327,
-                    "heading": -0.48297138878758666,
-                    "angularVelocity": 0.0000024537020442790352,
-                    "velocityX": 2.9306735946176077,
-                    "velocityY": -2.392023918156931,
-                    "timestamp": 1.3774688921202958
-                },
-                {
-                    "x": 4.428775209886728,
-                    "y": 4.681644750011071,
-                    "heading": -0.4829713376913353,
-                    "angularVelocity": 0.000002330427847533908,
-                    "velocityX": 3.042816307221113,
-                    "velocityY": -2.247642227334136,
-                    "timestamp": 1.3993945876264302
-                },
-                {
-                    "x": 4.497794001617253,
-                    "y": 4.635644296035039,
-                    "heading": -0.4829712886902182,
-                    "angularVelocity": 0.000002234871730357886,
-                    "velocityX": 3.147849595521449,
-                    "velocityY": -2.098015726031637,
-                    "timestamp": 1.4213202831325646
-                },
-                {
-                    "x": 4.568948026555497,
-                    "y": 4.593021059923411,
-                    "heading": -0.4829712412701429,
-                    "angularVelocity": 0.0000021627626491587156,
-                    "velocityX": 3.2452345659144926,
-                    "velocityY": -1.9439855898636562,
-                    "timestamp": 1.443245978638699
-                },
-                {
-                    "x": 4.64205867202195,
-                    "y": 4.55384875924605,
-                    "heading": -0.4829711949807044,
-                    "angularVelocity": 0.000002111195903735364,
-                    "velocityX": 3.3344732643018493,
-                    "velocityY": -1.7865932994667033,
-                    "timestamp": 1.4651716741448335
-                },
-                {
-                    "x": 4.71691342103475,
-                    "y": 4.5181219873551415,
-                    "heading": -0.4829711494130406,
-                    "angularVelocity": 0.000002078276777149451,
-                    "velocityX": 3.414019363346236,
-                    "velocityY": -1.6294475986389252,
-                    "timestamp": 1.487097369650968
-                },
-                {
-                    "x": 4.7930985438005,
-                    "y": 4.485327738304056,
-                    "heading": -0.4829711041848689,
-                    "angularVelocity": 0.000002062793021590678,
-                    "velocityX": 3.4746958309446994,
-                    "velocityY": -1.4956993743693363,
-                    "timestamp": 1.5090230651571024
-                },
-                {
-                    "x": 4.8695687201769084,
-                    "y": 4.453203793512952,
-                    "heading": -0.4829710590084327,
-                    "angularVelocity": 0.0000020604334399265313,
-                    "velocityX": 3.48769672346274,
-                    "velocityY": -1.4651277439372516,
-                    "timestamp": 1.5309487606632368
-                },
-                {
-                    "x": 4.94605170532014,
-                    "y": 4.421110356684282,
-                    "heading": -0.48297101383350444,
-                    "angularVelocity": 0.0000020603646626736416,
-                    "velocityX": 3.4882809132254033,
-                    "velocityY": -1.463736318862417,
-                    "timestamp": 1.5528744561693713
-                },
-                {
-                    "x": 5.022532332392591,
-                    "y": 4.389011300831823,
-                    "heading": -0.48297096865831407,
-                    "angularVelocity": 0.000002060376618948169,
-                    "velocityX": 3.4881733649487248,
-                    "velocityY": -1.463992594598631,
-                    "timestamp": 1.5748001516755057
-                },
-                {
-                    "x": 5.099012556138694,
-                    "y": 4.356911284014184,
-                    "heading": -0.48297092348307896,
-                    "angularVelocity": 0.000002060378658787562,
-                    "velocityX": 3.488154969803465,
-                    "velocityY": -1.4640364228636573,
-                    "timestamp": 1.5967258471816401
-                },
-                {
-                    "x": 5.175492855161546,
-                    "y": 4.324811446548553,
-                    "heading": -0.48297087830785207,
-                    "angularVelocity": 0.0000020603782854914656,
-                    "velocityX": 3.48815840306965,
-                    "velocityY": -1.46402824287194,
-                    "timestamp": 1.6186515426877746
-                },
-                {
-                    "x": 5.2519731561146505,
-                    "y": 4.292711613681912,
-                    "heading": -0.48297083313262523,
-                    "angularVelocity": 0.0000020603782806948455,
-                    "velocityX": 3.488158491105669,
-                    "velocityY": -1.464028033118535,
-                    "timestamp": 1.640577238193909
-                },
-                {
-                    "x": 5.328453454863713,
-                    "y": 4.260611775564003,
-                    "heading": -0.48297078795739806,
-                    "angularVelocity": 0.0000020603782977173096,
-                    "velocityX": 3.488158390582454,
-                    "velocityY": -1.4640282726214169,
-                    "timestamp": 1.6625029337000434
-                },
-                {
-                    "x": 5.404933754197733,
-                    "y": 4.228511938839819,
-                    "heading": -0.4829707427821709,
-                    "angularVelocity": 0.000002060378298482567,
-                    "velocityX": 3.4881584172616003,
-                    "velocityY": -1.4640282090556718,
-                    "timestamp": 1.6844286292061779
-                },
-                {
-                    "x": 5.481414053769572,
-                    "y": 4.196412102682258,
-                    "heading": -0.48297069760694356,
-                    "angularVelocity": 0.000002060378303797646,
-                    "velocityX": 3.488158428108085,
-                    "velocityY": -1.4640281832128117,
-                    "timestamp": 1.7063543247123123
-                },
-                {
-                    "x": 5.5578943539285115,
-                    "y": 4.164312267923512,
-                    "heading": -0.4829706524317162,
-                    "angularVelocity": 0.000002060378305885037,
-                    "velocityX": 3.488158454884962,
-                    "velocityY": -1.4640281194148257,
-                    "timestamp": 1.7282800202184467
-                },
-                {
-                    "x": 5.634374653438418,
-                    "y": 4.132212431618396,
-                    "heading": -0.4829706072564887,
-                    "angularVelocity": 0.0000020603783152723795,
-                    "velocityX": 3.4881584252834856,
-                    "velocityY": -1.4640281899425134,
-                    "timestamp": 1.7502057157245812
-                },
-                {
-                    "x": 5.71085495402441,
-                    "y": 4.100112597877132,
-                    "heading": -0.4829705620812611,
-                    "angularVelocity": 0.0000020603783147781934,
-                    "velocityX": 3.4881584743622165,
-                    "velocityY": -1.4640280730089004,
-                    "timestamp": 1.7721314112307156
-                },
-                {
-                    "x": 5.7873352532620705,
-                    "y": 4.068012760923364,
-                    "heading": -0.4829705169060333,
-                    "angularVelocity": 0.0000020603783267765002,
-                    "velocityX": 3.488158412866673,
-                    "velocityY": -1.4640282195266534,
-                    "timestamp": 1.79405710673685
-                },
-                {
-                    "x": 5.863815552376704,
-                    "y": 4.035912923676468,
-                    "heading": -0.4829704717308054,
-                    "angularVelocity": 0.000002060378332596705,
-                    "velocityX": 3.4881584072556215,
-                    "velocityY": -1.4640282328957663,
-                    "timestamp": 1.8159828022429845
-                },
-                {
-                    "x": 5.940295849970619,
-                    "y": 4.00381308280633,
-                    "heading": -0.48297042655557715,
-                    "angularVelocity": 0.0000020603783450065593,
-                    "velocityX": 3.4881583378978336,
-                    "velocityY": -1.4640283981467839,
-                    "timestamp": 1.837908497749119
-                },
-                {
-                    "x": 6.016776146567219,
-                    "y": 3.971713239559965,
-                    "heading": -0.4829703813803487,
-                    "angularVelocity": 0.0000020603783564721777,
-                    "velocityX": 3.4881582924116907,
-                    "velocityY": -1.4640285065230472,
-                    "timestamp": 1.8598341932552533
-                },
-                {
-                    "x": 6.093256433235532,
-                    "y": 3.939613372658632,
-                    "heading": -0.48297033620511903,
-                    "angularVelocity": 0.000002060378411227836,
-                    "velocityX": 3.4881578395965716,
-                    "velocityY": -1.464029585392675,
-                    "timestamp": 1.8817598887613878
-                },
-                {
-                    "x": 6.169736625426417,
-                    "y": 3.9075132806585757,
-                    "heading": -0.4829702910298788,
-                    "angularVelocity": 0.0000020603788925938424,
-                    "velocityX": 3.488153530614592,
-                    "velocityY": -1.4640398518273146,
-                    "timestamp": 1.9036855842675222
-                },
-                {
-                    "x": 6.2462171159811755,
-                    "y": 3.8754138995336747,
-                    "heading": -0.4829702458546715,
-                    "angularVelocity": 0.0000020603773887814844,
-                    "velocityX": 3.4881671385698874,
-                    "velocityY": -1.4640074298145878,
-                    "timestamp": 1.9256112797736566
-                },
-                {
-                    "x": 6.322701012459848,
-                    "y": 3.843322634623867,
-                    "heading": -0.4829702006798376,
-                    "angularVelocity": 0.0000020603603619090433,
-                    "velocityX": 3.4883224779483077,
-                    "velocityY": -1.4636372607092853,
-                    "timestamp": 1.947536975279791
-                },
-                {
-                    "x": 6.399174860669393,
-                    "y": 3.811207431927173,
-                    "heading": -0.4829701555038708,
-                    "angularVelocity": 0.000002060412030184656,
-                    "velocityX": 3.4878641905846774,
-                    "velocityY": -1.4647290293616972,
-                    "timestamp": 1.9694626707859255
-                },
-                {
-                    "x": 6.47530722887592,
-                    "y": 3.7782908978955247,
-                    "heading": -0.4829701102655904,
-                    "angularVelocity": 0.0000020632540676234596,
-                    "velocityX": 3.4722897700184046,
-                    "velocityY": -1.501276619595699,
-                    "timestamp": 1.99138836629206
-                },
-                {
-                    "x": 6.550061088980348,
-                    "y": 3.742353506827061,
-                    "heading": -0.48297006466550657,
-                    "angularVelocity": 0.0000020797554102610617,
-                    "velocityX": 3.409417962749362,
-                    "velocityY": -1.6390536418100325,
-                    "timestamp": 2.0133140617981944
-                },
-                {
-                    "x": 6.623057371447113,
-                    "y": 3.702968505083503,
-                    "heading": -0.482970018320009,
-                    "angularVelocity": 0.0000021137526750992832,
-                    "velocityX": 3.3292573294334487,
-                    "velocityY": -1.796294294637834,
-                    "timestamp": 2.035239757304329
-                },
-                {
-                    "x": 6.694087047655727,
-                    "y": 3.6601383683558693,
-                    "heading": -0.48296997081845616,
-                    "angularVelocity": 0.0000021664787259977457,
-                    "velocityX": 3.2395631960090396,
-                    "velocityY": -1.9534220346951927,
-                    "timestamp": 2.0571654528104633
-                },
-                {
-                    "x": 6.762971951373863,
-                    "y": 3.6139376604858007,
-                    "heading": -0.48296992170702124,
-                    "angularVelocity": 0.000002239903173580672,
-                    "velocityX": 3.141743152404488,
-                    "velocityY": -2.1071490232631853,
-                    "timestamp": 2.0790911483165977
-                },
-                {
-                    "x": 6.829544518243924,
-                    "y": 3.564463136615443,
-                    "heading": -0.4829698704667692,
-                    "angularVelocity": 0.0000023369955126814126,
-                    "velocityX": 3.0362807351500685,
-                    "velocityY": -2.256463146473092,
-                    "timestamp": 2.101016843822732
-                },
-                {
-                    "x": 6.893649101257324,
-                    "y": 3.5118300914764404,
-                    "heading": -0.48296981752333895,
-                    "angularVelocity": 0.0000024146750677216304,
-                    "velocityX": 2.923719477698919,
-                    "velocityY": -2.400518839837981,
-                    "timestamp": 2.1229425393288666
-                },
-                {
-                    "x": 6.935492251520178,
-                    "y": 3.475140361342702,
-                    "heading": -0.48296976475625003,
-                    "angularVelocity": 0.000003586928740757824,
-                    "velocityX": 2.8443562341956334,
-                    "velocityY": -2.4940441143007757,
-                    "timestamp": 2.1376534780501846
-                },
-                {
-                    "x": 6.976123919914837,
-                    "y": 3.4371133556518583,
-                    "heading": -0.4829697143572031,
-                    "angularVelocity": 0.0000034259572361559534,
-                    "velocityX": 2.7620037826529793,
-                    "velocityY": -2.5849475965623756,
-                    "timestamp": 2.1523644167715026
-                },
-                {
-                    "x": 7.015501922342544,
-                    "y": 3.3977895885157845,
-                    "heading": -0.4829696659490819,
-                    "angularVelocity": 0.0000032906208164279545,
-                    "velocityX": 2.676783798347444,
-                    "velocityY": -2.673097066136968,
-                    "timestamp": 2.1670753554928206
-                },
-                {
-                    "x": 7.053586816421239,
-                    "y": 3.357212172853621,
-                    "heading": -0.4829696191989941,
-                    "angularVelocity": 0.000003177913298539426,
-                    "velocityX": 2.5888826539333314,
-                    "velocityY": -2.758315864871489,
-                    "timestamp": 2.1817862942141386
-                },
-                {
-                    "x": 7.090341266441407,
-                    "y": 3.315425855846777,
-                    "heading": -0.48296957381059136,
-                    "angularVelocity": 0.0000030853505387280562,
-                    "velocityX": 2.4984435539051053,
-                    "velocityY": -2.8404929011286835,
-                    "timestamp": 2.1964972329354566
-                },
-                {
-                    "x": 7.125730460448936,
-                    "y": 3.2724771552259067,
-                    "heading": -0.48296952951403316,
-                    "angularVelocity": 0.000003011130632666104,
-                    "velocityX": 2.405638054643597,
-                    "velocityY": -2.9195078189422343,
-                    "timestamp": 2.2112081716567746
-                },
-                {
-                    "x": 7.1597225937186835,
-                    "y": 3.228414468562161,
-                    "heading": -0.48296947336189594,
-                    "angularVelocity": 0.000003817032908672554,
-                    "velocityX": 2.3106705774318574,
-                    "velocityY": -2.9952328330951756,
-                    "timestamp": 2.2259191103780926
-                },
-                {
-                    "x": 7.192119389825632,
-                    "y": 3.1836076694729414,
-                    "heading": -0.4820443638475483,
-                    "angularVelocity": 0.06288582475110666,
-                    "velocityX": 2.2022249375566023,
-                    "velocityY": -3.0458150861769275,
-                    "timestamp": 2.2406300490994107
-                },
-                {
-                    "x": 7.223229630823693,
-                    "y": 3.1396490395754038,
-                    "heading": -0.47743685492285376,
-                    "angularVelocity": 0.3132029173651504,
-                    "velocityX": 2.11476926030427,
-                    "velocityY": -2.988159405071387,
-                    "timestamp": 2.2553409878207287
-                },
-                {
-                    "x": 7.253205323915578,
-                    "y": 3.0967066455951215,
-                    "heading": -0.4690721734359265,
-                    "angularVelocity": 0.5686028366637977,
-                    "velocityX": 2.0376465200313345,
-                    "velocityY": -2.9190791147849615,
-                    "timestamp": 2.2700519265420467
-                },
-                {
-                    "x": 7.28210093319025,
-                    "y": 3.0548329265465135,
-                    "heading": -0.45697001578434165,
-                    "angularVelocity": 0.8226638612835941,
-                    "velocityX": 1.9642260648397363,
-                    "velocityY": -2.8464341971547444,
-                    "timestamp": 2.2847628652633647
-                },
-                {
-                    "x": 7.309941019631162,
-                    "y": 3.0140523923628573,
-                    "heading": -0.44116512849373885,
-                    "angularVelocity": 1.0743629342769347,
-                    "velocityX": 1.892475182468747,
-                    "velocityY": -2.7721231769227326,
-                    "timestamp": 2.2994738039846827
-                },
-                {
-                    "x": 7.336737361434823,
-                    "y": 2.9743781069216433,
-                    "heading": -0.42169586611687776,
-                    "angularVelocity": 1.3234547941286585,
-                    "velocityX": 1.8215249421732278,
-                    "velocityY": -2.696924118358335,
-                    "timestamp": 2.3141847427060007
-                },
-                {
-                    "x": 7.362502163456035,
-                    "y": 2.936038971640856,
-                    "heading": -0.39992762164634665,
-                    "angularVelocity": 1.479731843283817,
-                    "velocityX": 1.7514043467447793,
-                    "velocityY": -2.606165113394762,
-                    "timestamp": 2.3288956814273187
-                },
-                {
-                    "x": 7.3872337303102205,
-                    "y": 2.8991268417513614,
-                    "heading": -0.37676830696947144,
-                    "angularVelocity": 1.5742921043722566,
-                    "velocityX": 1.6811685047907687,
-                    "velocityY": -2.5091620996289765,
-                    "timestamp": 2.3436066201486367
-                },
-                {
-                    "x": 7.410930819346905,
-                    "y": 2.863676250815302,
-                    "heading": -0.3528129850042874,
-                    "angularVelocity": 1.6284019952084616,
-                    "velocityX": 1.6108481916483202,
-                    "velocityY": -2.4098116107768903,
-                    "timestamp": 2.3583175588699548
-                },
-                {
-                    "x": 7.433596368468817,
-                    "y": 2.8297026440026514,
-                    "heading": -0.328491545307115,
-                    "angularVelocity": 1.6532894438563033,
-                    "velocityX": 1.5407275872250867,
-                    "velocityY": -2.3094112113606737,
-                    "timestamp": 2.3730284975912728
-                },
-                {
-                    "x": 7.455235422231159,
-                    "y": 2.7972130374670496,
-                    "heading": -0.30412705199668255,
-                    "angularVelocity": 1.6562160832826247,
-                    "velocityX": 1.4709498946511312,
-                    "velocityY": -2.208533877482683,
-                    "timestamp": 2.387739436312591
-                },
-                {
-                    "x": 7.475853646381716,
-                    "y": 2.766210258525358,
-                    "heading": -0.2799689621545816,
-                    "angularVelocity": 1.6421854716240782,
-                    "velocityX": 1.4015573405031783,
-                    "velocityY": -2.1074643521399103,
-                    "timestamp": 2.402450375033909
-                },
-                {
-                    "x": 7.495456618827617,
-                    "y": 2.736694950498885,
-                    "heading": -0.2562142168141467,
-                    "angularVelocity": 1.6147674727249637,
-                    "velocityX": 1.332543953669861,
-                    "velocityY": -2.006351095984268,
-                    "timestamp": 2.417161313755227
-                },
-                {
-                    "x": 7.514049552575163,
-                    "y": 2.7086666100983954,
-                    "heading": -0.23302134486774898,
-                    "angularVelocity": 1.576573214378762,
-                    "velocityX": 1.2638849294234806,
-                    "velocityY": -1.9052720517334563,
-                    "timestamp": 2.431872252476545
-                },
-                {
-                    "x": 7.5316372223467996,
-                    "y": 2.682124150428342,
-                    "heading": -0.2105202201245662,
-                    "angularVelocity": 1.5295505724985052,
-                    "velocityX": 1.1955504747055303,
-                    "velocityY": -1.804266890976241,
-                    "timestamp": 2.446583191197863
-                },
-                {
-                    "x": 7.5482239779420395,
-                    "y": 2.6570662133282936,
-                    "heading": -0.18881898554249216,
-                    "angularVelocity": 1.4751767370647726,
-                    "velocityX": 1.127511704688336,
-                    "velocityY": -1.7033540533846026,
-                    "timestamp": 2.461294129919181
-                },
-                {
-                    "x": 7.563813788966016,
-                    "y": 2.6334913430066904,
-                    "heading": -0.16800906628713844,
-                    "angularVelocity": 1.414588127214289,
-                    "velocityX": 1.0597427750401904,
-                    "velocityY": -1.602540175593351,
-                    "timestamp": 2.476005068640499
-                },
-                {
-                    "x": 7.5784102960217234,
-                    "y": 2.611398080774854,
-                    "heading": -0.1481688563613113,
-                    "angularVelocity": 1.3486705574455213,
-                    "velocityX": 0.9922213213052737,
-                    "velocityY": -1.5018254545389353,
-                    "timestamp": 2.490716007361817
-                },
-                {
-                    "x": 7.5920168580101715,
-                    "y": 2.5907850143215074,
-                    "heading": -0.1293664624719475,
-                    "angularVelocity": 1.2781233234366292,
-                    "velocityX": 0.9249281943327172,
-                    "velocityY": -1.401206737641836,
-                    "timestamp": 2.505426946083135
-                },
-                {
-                    "x": 7.604636591974509,
-                    "y": 2.571650800836196,
-                    "heading": -0.1116617644111604,
-                    "angularVelocity": 1.2035056631111207,
-                    "velocityX": 0.8578469534407521,
-                    "velocityY": -1.3006793004707733,
-                    "timestamp": 2.520137884804453
-                },
-                {
-                    "x": 7.616272404986451,
-                    "y": 2.5539941753706046,
-                    "heading": -0.09510797189421191,
-                    "angularVelocity": 1.1252709857977845,
-                    "velocityX": 0.790963325479677,
-                    "velocityY": -1.20023785021998,
-                    "timestamp": 2.534848823525771
-                },
-                {
-                    "x": 7.626927018849464,
-                    "y": 2.537813951229366,
-                    "heading": -0.07975280576069126,
-                    "angularVelocity": 1.0437924067530058,
-                    "velocityX": 0.7242647165386202,
-                    "velocityY": -1.0998770675179592,
-                    "timestamp": 2.549559762247089
-                },
-                {
-                    "x": 7.636602988821212,
-                    "y": 2.523109016437205,
-                    "heading": -0.0656393963876239,
-                    "angularVelocity": 0.9593819701400241,
-                    "velocityX": 0.6577398053956987,
-                    "velocityY": -0.9995918731448548,
-                    "timestamp": 2.564270700968407
-                },
-                {
-                    "x": 7.64530271759392,
-                    "y": 2.509878328655525,
-                    "heading": -0.05280696792868869,
-                    "angularVelocity": 0.8723052078477697,
-                    "velocityX": 0.5913782211668082,
-                    "velocityY": -0.899377533434203,
-                    "timestamp": 2.578981639689725
-                },
-                {
-                    "x": 7.653028465638572,
-                    "y": 2.498120909886367,
-                    "heading": -0.04129135982777762,
-                    "angularVelocity": 0.7827922010322448,
-                    "velocityX": 0.5251702961317726,
-                    "velocityY": -0.799229674726393,
-                    "timestamp": 2.593692578411043
-                },
-                {
-                    "x": 7.6597823588265515,
-                    "y": 2.48783584165749,
-                    "heading": -0.03112542462311605,
-                    "angularVelocity": 0.6910459894669937,
-                    "velocityX": 0.4591068806636599,
-                    "velocityY": -0.6991442506638482,
-                    "timestamp": 2.608403517132361
-                },
-                {
-                    "x": 7.665566394043072,
-                    "y": 2.4790222609826817,
-                    "heading": -0.02233933188139214,
-                    "angularVelocity": 0.5972489525084974,
-                    "velocityX": 0.39317920671771145,
-                    "velocityY": -0.5991174894934748,
-                    "timestamp": 2.623114455853679
-                },
-                {
-                    "x": 7.670382443324971,
-                    "y": 2.471679357147111,
-                    "heading": -0.014960801225629315,
-                    "angularVelocity": 0.5015676290643761,
-                    "velocityX": 0.3273787875222405,
-                    "velocityY": -0.4991458379831521,
-                    "timestamp": 2.637825394574997
-                },
-                {
-                    "x": 7.674232256901065,
-                    "y": 2.4658063692226495,
-                    "heading": -0.00901528221740954,
-                    "angularVelocity": 0.40415633025538694,
-                    "velocityX": 0.2616973429788216,
-                    "velocityY": -0.399225911800697,
-                    "timestamp": 2.652536333296315
-                },
-                {
-                    "x": 7.677117465389097,
-                    "y": 2.461402584136771,
-                    "heading": -0.004526094899743595,
-                    "angularVelocity": 0.30515981357196037,
-                    "velocityX": 0.19612674233020058,
-                    "velocityY": -0.2993544578835629,
-                    "timestamp": 2.667247272017633
-                },
-                {
-                    "x": 7.679039581308467,
-                    "y": 2.45846733507745,
-                    "heading": -0.001514541807715108,
-                    "angularVelocity": 0.20471522239871504,
-                    "velocityX": 0.13065895764927307,
-                    "velocityY": -0.1995283315991124,
-                    "timestamp": 2.681958210738951
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 7.235538726171091e-30,
-                    "angularVelocity": 0.10295344412796348,
-                    "velocityX": 0.06528602353168424,
-                    "velocityY": -0.09974448981448966,
-                    "timestamp": 2.696669149460269
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 1.9723672087239994e-30,
-                    "angularVelocity": -3.5777263674767652e-28,
-                    "velocityX": 6.106461964279306e-29,
-                    "velocityY": 3.383375524617378e-29,
-                    "timestamp": 2.711380088181587
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": false,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "Center To C5": {
-            "waypoints": [
-                {
-                    "x": 1.35,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 28
-                },
-                {
-                    "x": 2.697566270828247,
-                    "y": 2.995814085006714,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 13
-                },
-                {
-                    "x": 5.311619758605957,
-                    "y": 1.6392407417297363,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 27
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 1.35,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -4.1566893564125267e-32,
-                    "timestamp": 0
-                },
-                {
-                    "x": 1.3546830312136449,
-                    "y": 5.550921973415328,
-                    "heading": -1.4050458772244498e-18,
-                    "angularVelocity": -4.014308801297789e-17,
-                    "velocityX": 0.13379730014070312,
-                    "velocityY": -0.25936522568857817,
-                    "timestamp": 0.03500093954603885
-                },
-                {
-                    "x": 1.3640490935091651,
-                    "y": 5.532765920501419,
-                    "heading": -3.603876179363921e-18,
-                    "angularVelocity": -6.28220327965604e-17,
-                    "velocityX": 0.26759459651667833,
-                    "velocityY": -0.5187304440792354,
-                    "timestamp": 0.0700018790920777
-                },
-                {
-                    "x": 1.3780981867284385,
-                    "y": 5.505531841564791,
-                    "heading": -4.091508575199503e-18,
-                    "angularVelocity": -1.3931950355515604e-17,
-                    "velocityX": 0.4013918883749871,
-                    "velocityY": -0.7780956537124009,
-                    "timestamp": 0.10500281863811654
-                },
-                {
-                    "x": 1.3968303106782045,
-                    "y": 5.469219736980081,
-                    "heading": -4.985304542720085e-18,
-                    "angularVelocity": -2.5536326104216816e-17,
-                    "velocityX": 0.5351891747117141,
-                    "velocityY": -1.0374608526419855,
-                    "timestamp": 0.1400037581841554
-                },
-                {
-                    "x": 1.4202454651168877,
-                    "y": 5.423829607215581,
-                    "heading": -4.3424315250592746e-18,
-                    "angularVelocity": 1.8367331153854942e-17,
-                    "velocityX": 0.6689864541464801,
-                    "velocityY": -1.2968260381921248,
-                    "timestamp": 0.17500469773019423
-                },
-                {
-                    "x": 1.4483436497338926,
-                    "y": 5.369361452873384,
-                    "heading": -2.0560193041515625e-18,
-                    "angularVelocity": 6.532433512064817e-17,
-                    "velocityX": 0.8027837247073224,
-                    "velocityY": -1.5561912065401715,
-                    "timestamp": 0.21000563727623306
-                },
-                {
-                    "x": 1.4811248641150936,
-                    "y": 5.305815274756268,
-                    "heading": 2.1208651292681738e-19,
-                    "angularVelocity": 6.480130833433249e-17,
-                    "velocityX": 0.9365809834363137,
-                    "velocityY": -1.815556351952184,
-                    "timestamp": 0.2450065768222719
-                },
-                {
-                    "x": 1.518589107680717,
-                    "y": 5.233191073988126,
-                    "heading": 3.0112738874394488e-18,
-                    "angularVelocity": 7.99746695569e-17,
-                    "velocityX": 1.0703782256008032,
-                    "velocityY": -2.074921465253921,
-                    "timestamp": 0.2800075163683107
-                },
-                {
-                    "x": 1.5607363795611104,
-                    "y": 5.151488852254779,
-                    "heading": 5.4707987602321704e-18,
-                    "angularVelocity": 7.027028770373186e-17,
-                    "velocityX": 1.2041754429187383,
-                    "velocityY": -2.334286530390624,
-                    "timestamp": 0.31500845591434956
-                },
-                {
-                    "x": 1.6075666783068712,
-                    "y": 5.060708612365892,
-                    "heading": 7.441304754324509e-18,
-                    "angularVelocity": 5.629869559433992e-17,
-                    "velocityX": 1.3379726188262824,
-                    "velocityY": -2.5936515152532893,
-                    "timestamp": 0.3500093954603884
-                },
-                {
-                    "x": 1.6590800010192646,
-                    "y": 4.9608503599406655,
-                    "heading": 8.015191331788108e-18,
-                    "angularVelocity": 1.639633984973558e-17,
-                    "velocityX": 1.4717697119150586,
-                    "velocityY": -2.853016339571725,
-                    "timestamp": 0.38501033500642723
-                },
-                {
-                    "x": 1.7152763390026586,
-                    "y": 4.851914111835609,
-                    "heading": 8.871725964928404e-18,
-                    "angularVelocity": 2.4471795525975083e-17,
-                    "velocityX": 1.6055665565638684,
-                    "velocityY": -3.112380682288518,
-                    "timestamp": 0.42001127455246606
-                },
-                {
-                    "x": 1.775979033805623,
-                    "y": 4.734242319347404,
-                    "heading": 1.526334137039101e-17,
-                    "angularVelocity": 1.8261268092684144e-16,
-                    "velocityX": 1.7343161523752855,
-                    "velocityY": -3.361960964887902,
-                    "timestamp": 0.4550122140985049
-                },
-                {
-                    "x": 1.8366817286107127,
-                    "y": 4.616570526856247,
-                    "heading": 2.6738432547349554e-17,
-                    "angularVelocity": 3.2785094702569765e-16,
-                    "velocityX": 1.734316152436003,
-                    "velocityY": -3.3619609649722197,
-                    "timestamp": 0.49001315364454373
-                },
-                {
-                    "x": 1.8973844234157844,
-                    "y": 4.498898734365081,
-                    "heading": 1.9852147320466475e-17,
-                    "angularVelocity": -1.967457250062152e-16,
-                    "velocityX": 1.7343161524354893,
-                    "velocityY": -3.3619609649724835,
-                    "timestamp": 0.5250140931905826
-                },
-                {
-                    "x": 1.9580871182208561,
-                    "y": 4.381226941873916,
-                    "heading": 2.777024505719557e-17,
-                    "angularVelocity": 2.2622529107579697e-16,
-                    "velocityX": 1.7343161524354895,
-                    "velocityY": -3.361960964972483,
-                    "timestamp": 0.5600150327366215
-                },
-                {
-                    "x": 2.018789813025928,
-                    "y": 4.26355514938275,
-                    "heading": 1.7414652311267274e-17,
-                    "angularVelocity": -2.9586613617347484e-16,
-                    "velocityX": 1.7343161524354893,
-                    "velocityY": -3.361960964972483,
-                    "timestamp": 0.5950159722826603
-                },
-                {
-                    "x": 2.0794925078309996,
-                    "y": 4.145883356891583,
-                    "heading": 6.771469247853252e-18,
-                    "angularVelocity": -3.040827818183264e-16,
-                    "velocityX": 1.7343161524354895,
-                    "velocityY": -3.361960964972483,
-                    "timestamp": 0.6300169118286992
-                },
-                {
-                    "x": 2.140195202636071,
-                    "y": 4.028211564400419,
-                    "heading": 1.7156471914154247e-17,
-                    "angularVelocity": 2.9670636674740876e-16,
-                    "velocityX": 1.7343161524354882,
-                    "velocityY": -3.361960964972482,
-                    "timestamp": 0.6650178513747381
-                },
-                {
-                    "x": 2.2008978974411426,
-                    "y": 3.910539771909253,
-                    "heading": 9.427453062324153e-18,
-                    "angularVelocity": -2.208231822366884e-16,
-                    "velocityX": 1.734316152435471,
-                    "velocityY": -3.361960964972468,
-                    "timestamp": 0.700018790920777
-                },
-                {
-                    "x": 2.261600592246207,
-                    "y": 3.7928679794180944,
-                    "heading": -2.0403441270686145e-18,
-                    "angularVelocity": -3.2764255297515643e-16,
-                    "velocityX": 1.7343161524352513,
-                    "velocityY": -3.3619609649722837,
-                    "timestamp": 0.7350197304668159
-                },
-                {
-                    "x": 2.322303287051213,
-                    "y": 3.6751961869269927,
-                    "heading": 3.357966916114392e-18,
-                    "angularVelocity": 1.5423331811082687e-16,
-                    "velocityX": 1.7343161524334347,
-                    "velocityY": -3.3619609649707387,
-                    "timestamp": 0.7700206700128548
-                },
-                {
-                    "x": 2.383005981879539,
-                    "y": 3.55752439444761,
-                    "heading": 1.6083387472754582e-17,
-                    "angularVelocity": 3.6357368464068287e-16,
-                    "velocityX": 1.7343161531001825,
-                    "velocityY": -3.361960964635663,
-                    "timestamp": 0.8050216095588937
-                },
-                {
-                    "x": 2.4437089810285184,
-                    "y": 3.439852758955193,
-                    "heading": 2.2946423291224054e-17,
-                    "angularVelocity": 1.9608147402560587e-16,
-                    "velocityX": 1.7343248477441577,
-                    "velocityY": -3.3619564794134043,
-                    "timestamp": 0.8400225491049326
-                },
-                {
-                    "x": 2.507799181067132,
-                    "y": 3.323991142238629,
-                    "heading": 9.669545465348722e-18,
-                    "angularVelocity": -3.793291836756151e-16,
-                    "velocityX": 1.8310994181462568,
-                    "velocityY": -3.3102430454713967,
-                    "timestamp": 0.8750234886509715
-                },
-                {
-                    "x": 2.5806292795037855,
-                    "y": 3.213414270581727,
-                    "heading": 3.38196516760506e-17,
-                    "angularVelocity": 6.899845125281262e-16,
-                    "velocityX": 2.0808040978720626,
-                    "velocityY": -3.159254382625257,
-                    "timestamp": 0.9100244281970103
-                },
-                {
-                    "x": 2.661766753531331,
-                    "y": 3.1087807297901673,
-                    "heading": 5.146705636243318e-17,
-                    "angularVelocity": 5.041980276892245e-16,
-                    "velocityX": 2.3181513147947848,
-                    "velocityY": -2.9894494876003863,
-                    "timestamp": 0.9450253677430492
-                },
-                {
-                    "x": 2.7507288455963135,
-                    "y": 3.0107133388519287,
-                    "heading": 6.422105835490463e-17,
-                    "angularVelocity": 3.643902751722035e-16,
-                    "velocityX": 2.5417058290140595,
-                    "velocityY": -2.8018502420229328,
-                    "timestamp": 0.9800263072890881
-                },
-                {
-                    "x": 2.9215337593923705,
-                    "y": 2.8658352980219064,
-                    "heading": 5.1318206888106e-17,
-                    "angularVelocity": -2.1793112103404205e-16,
-                    "velocityX": 2.884920936848368,
-                    "velocityY": -2.4470121145413257,
-                    "timestamp": 1.0392324060123015
-                },
-                {
-                    "x": 3.109750180151373,
-                    "y": 2.744432888397421,
-                    "heading": 6.648964720130561e-17,
-                    "angularVelocity": 2.5624793121609224e-16,
-                    "velocityX": 3.179003934013426,
-                    "velocityY": -2.0505051378665997,
-                    "timestamp": 1.098438504735515
-                },
-                {
-                    "x": 3.309758578030313,
-                    "y": 2.6436330516151556,
-                    "heading": 6.809268296964168e-17,
-                    "angularVelocity": 2.7075517621497836e-17,
-                    "velocityX": 3.3781722186064402,
-                    "velocityY": -1.702524553319066,
-                    "timestamp": 1.1576446034587284
-                },
-                {
-                    "x": 3.5097675494851996,
-                    "y": 2.542834352942729,
-                    "heading": 6.982732716570642e-17,
-                    "angularVelocity": 2.929840393934814e-17,
-                    "velocityX": 3.378181906393955,
-                    "velocityY": -1.702505330465446,
-                    "timestamp": 1.216850702181942
-                },
-                {
-                    "x": 3.709776520948374,
-                    "y": 2.4420356542872104,
-                    "heading": 8.871000937094426e-17,
-                    "angularVelocity": 3.189313704575333e-16,
-                    "velocityX": 3.3781819065336705,
-                    "velocityY": -1.702505330180368,
-                    "timestamp": 1.2760568009051554
-                },
-                {
-                    "x": 3.9097854924115545,
-                    "y": 2.341236955631689,
-                    "heading": 7.04137580064196e-17,
-                    "angularVelocity": -3.090264644873572e-16,
-                    "velocityX": 3.3781819065337895,
-                    "velocityY": -1.7025053301803954,
-                    "timestamp": 1.3352628996283689
-                },
-                {
-                    "x": 4.109794463874735,
-                    "y": 2.240438256976168,
-                    "heading": 6.635408667627181e-17,
-                    "angularVelocity": -6.856846534504647e-17,
-                    "velocityX": 3.3781819065337926,
-                    "velocityY": -1.7025053301803963,
-                    "timestamp": 1.3944689983515823
-                },
-                {
-                    "x": 4.3098034353379155,
-                    "y": 2.1396395583206465,
-                    "heading": 8.756379552984881e-17,
-                    "angularVelocity": 3.5823520399025277e-16,
-                    "velocityX": 3.3781819065337926,
-                    "velocityY": -1.7025053301803963,
-                    "timestamp": 1.4536750970747958
-                },
-                {
-                    "x": 4.509812406801097,
-                    "y": 2.0388408596651253,
-                    "heading": 7.076319345079811e-17,
-                    "angularVelocity": -2.837647208878057e-16,
-                    "velocityX": 3.3781819065337926,
-                    "velocityY": -1.7025053301803963,
-                    "timestamp": 1.5128811957980093
-                },
-                {
-                    "x": 4.709821378264278,
-                    "y": 1.9380421610096037,
-                    "heading": 6.652684249010322e-17,
-                    "angularVelocity": -7.15526111686399e-17,
-                    "velocityX": 3.378181906533802,
-                    "velocityY": -1.7025053301803985,
-                    "timestamp": 1.5720872945212228
-                },
-                {
-                    "x": 4.9098303497278835,
-                    "y": 1.837243462354927,
-                    "heading": 7.855425123263822e-17,
-                    "angularVelocity": 2.0314476045367986e-16,
-                    "velocityX": 3.378181906540968,
-                    "velocityY": -1.7025053301661324,
-                    "timestamp": 1.6312933932444362
-                },
-                {
-                    "x": 5.109839349236117,
-                    "y": 1.7364448193478421,
-                    "heading": 7.31685607290898e-17,
-                    "angularVelocity": -9.096513061477316e-17,
-                    "velocityX": 3.378182380218786,
-                    "velocityY": -1.7025043902702668,
-                    "timestamp": 1.6904994919676497
-                },
-                {
-                    "x": 5.311619758605957,
-                    "y": 1.6392407417297363,
-                    "heading": 6.073866217998324e-17,
-                    "angularVelocity": -2.0994287441931112e-16,
-                    "velocityX": 3.4081017618384997,
-                    "velocityY": -1.6417916348765775,
-                    "timestamp": 1.7497055906908632
-                },
-                {
-                    "x": 5.430192834880249,
-                    "y": 1.5926107881863976,
-                    "heading": 7.355026472308393e-17,
-                    "angularVelocity": 3.8038299911394555e-16,
-                    "velocityX": 3.5204949744296554,
-                    "velocityY": -1.3844670499027307,
-                    "timestamp": 1.78338638760593
-                },
-                {
-                    "x": 5.550146685206584,
-                    "y": 1.5496573158737061,
-                    "heading": 8.480161924468705e-17,
-                    "angularVelocity": 3.3405844137167443e-16,
-                    "velocityX": 3.5614908586879754,
-                    "velocityY": -1.2753104512640399,
-                    "timestamp": 1.8170671845209967
-                },
-                {
-                    "x": 5.670100671705041,
-                    "y": 1.506704223843596,
-                    "heading": 8.467696633181045e-17,
-                    "angularVelocity": -3.701008417078351e-18,
-                    "velocityX": 3.5614949017075612,
-                    "velocityY": -1.2752991604806918,
-                    "timestamp": 1.8507479814360634
-                },
-                {
-                    "x": 5.790054658213967,
-                    "y": 1.4637511318444054,
-                    "heading": 8.225246246818014e-17,
-                    "angularVelocity": -7.198475350046388e-17,
-                    "velocityX": 3.561494902018399,
-                    "velocityY": -1.275299159562779,
-                    "timestamp": 1.8844287783511302
-                },
-                {
-                    "x": 5.910008644722922,
-                    "y": 1.4207980398452125,
-                    "heading": 9.443730259968232e-17,
-                    "angularVelocity": 3.617741041647218e-16,
-                    "velocityX": 3.5614949020192235,
-                    "velocityY": -1.2752991595628402,
-                    "timestamp": 1.918109575266197
-                },
-                {
-                    "x": 6.02996263123188,
-                    "y": 1.3778449478460182,
-                    "heading": 8.582959689044548e-17,
-                    "angularVelocity": -2.555671628240573e-16,
-                    "velocityX": 3.5614949020193487,
-                    "velocityY": -1.2752991595628858,
-                    "timestamp": 1.9517903721812637
-                },
-                {
-                    "x": 6.149916617740839,
-                    "y": 1.3348918558468235,
-                    "heading": 7.701354643967334e-17,
-                    "angularVelocity": -2.6175302422337863e-16,
-                    "velocityX": 3.56149490201936,
-                    "velocityY": -1.2752991595628895,
-                    "timestamp": 1.9854711690963305
-                },
-                {
-                    "x": 6.269870604249798,
-                    "y": 1.291938763847629,
-                    "heading": 8.367010625720842e-17,
-                    "angularVelocity": 1.9763664839406868e-16,
-                    "velocityX": 3.5614949020193607,
-                    "velocityY": -1.27529915956289,
-                    "timestamp": 2.019151966011397
-                },
-                {
-                    "x": 6.3898245907587565,
-                    "y": 1.2489856718484342,
-                    "heading": 7.968637324735258e-17,
-                    "angularVelocity": -1.1827906031741492e-16,
-                    "velocityX": 3.5614949020193607,
-                    "velocityY": -1.2752991595628902,
-                    "timestamp": 2.0528327629264638
-                },
-                {
-                    "x": 6.509778577267715,
-                    "y": 1.2060325798492397,
-                    "heading": 7.914041276645377e-17,
-                    "angularVelocity": -1.6209844508000374e-17,
-                    "velocityX": 3.561494902019361,
-                    "velocityY": -1.27529915956289,
-                    "timestamp": 2.0865135598415305
-                },
-                {
-                    "x": 6.629732563776674,
-                    "y": 1.1630794878500452,
-                    "heading": 7.575912442206911e-17,
-                    "angularVelocity": -1.0039217162560839e-16,
-                    "velocityX": 3.561494902019361,
-                    "velocityY": -1.27529915956289,
-                    "timestamp": 2.1201943567565973
-                },
-                {
-                    "x": 6.749686550285633,
-                    "y": 1.1201263958508503,
-                    "heading": 6.889954024340196e-17,
-                    "angularVelocity": -2.0366454499176497e-16,
-                    "velocityX": 3.5614949020193576,
-                    "velocityY": -1.2752991595628989,
-                    "timestamp": 2.153875153671664
-                },
-                {
-                    "x": 6.869640536794461,
-                    "y": 1.077173303851719,
-                    "heading": 8.054846416919904e-17,
-                    "angularVelocity": 3.458624792985843e-16,
-                    "velocityX": 3.5614949020154905,
-                    "velocityY": -1.275299159561009,
-                    "timestamp": 2.187555950586731
-                },
-                {
-                    "x": 6.985406128017608,
-                    "y": 1.0357199913369377,
-                    "heading": 6.97905590140722e-17,
-                    "angularVelocity": -3.1940767538205783e-16,
-                    "velocityX": 3.437139314579595,
-                    "velocityY": -1.2307699434580792,
-                    "timestamp": 2.2212367475017976
-                },
-                {
-                    "x": 7.092266700241795,
-                    "y": 0.9974553856875888,
-                    "heading": 5.947830002893604e-17,
-                    "angularVelocity": -3.0617621445548207e-16,
-                    "velocityX": 3.172744768885924,
-                    "velocityY": -1.1360956139440452,
-                    "timestamp": 2.2549175444168643
-                },
-                {
-                    "x": 7.190222235939473,
-                    "y": 0.9623794931799317,
-                    "heading": 5.044588900647289e-17,
-                    "angularVelocity": -2.6817687744625345e-16,
-                    "velocityX": 2.9083497027904013,
-                    "velocityY": -1.0414210980846788,
-                    "timestamp": 2.288598341331931
-                },
-                {
-                    "x": 7.279272729268112,
-                    "y": 0.930492315906057,
-                    "heading": 4.27627289839913e-17,
-                    "angularVelocity": -2.281169282784766e-16,
-                    "velocityX": 2.6439544632272356,
-                    "velocityY": -0.9467465201100714,
-                    "timestamp": 2.322279138246998
-                },
-                {
-                    "x": 7.35941817730645,
-                    "y": 0.9017938549120106,
-                    "heading": 3.626946964953948e-17,
-                    "angularVelocity": -1.9278817213060433e-16,
-                    "velocityX": 2.379559136930222,
-                    "velocityY": -0.8520719110778341,
-                    "timestamp": 2.3559599351620646
-                },
-                {
-                    "x": 7.430658578301727,
-                    "y": 0.8762841108254199,
-                    "heading": 2.9442071838197796e-17,
-                    "angularVelocity": -2.0270891340137855e-16,
-                    "velocityX": 2.1151637585928986,
-                    "velocityY": -0.7573972834110186,
-                    "timestamp": 2.3896407320771313
-                },
-                {
-                    "x": 7.492993931085436,
-                    "y": 0.8539630840647031,
-                    "heading": 2.3413209096667504e-17,
-                    "angularVelocity": -1.790000012026981e-16,
-                    "velocityX": 1.850768345562039,
-                    "velocityY": -0.662722643321152,
-                    "timestamp": 2.423321528992198
-                },
-                {
-                    "x": 7.546424234822929,
-                    "y": 0.8348307749287305,
-                    "heading": 1.7731266545962487e-17,
-                    "angularVelocity": -1.6869976443484408e-16,
-                    "velocityX": 1.5863729077500839,
-                    "velocityY": -0.5680479943576784,
-                    "timestamp": 2.457002325907265
-                },
-                {
-                    "x": 7.590949488888224,
-                    "y": 0.8188871836416546,
-                    "heading": 1.2363765500283435e-17,
-                    "angularVelocity": -1.593638362048541e-16,
-                    "velocityX": 1.321977451352309,
-                    "velocityY": -0.4733733387389999,
-                    "timestamp": 2.4906831228223316
-                },
-                {
-                    "x": 7.626569692794441,
-                    "y": 0.8061323103778162,
-                    "heading": 7.88828312817125e-18,
-                    "angularVelocity": -1.3287934794211324e-16,
-                    "velocityX": 1.057581980498898,
-                    "velocityY": -0.37869867794405154,
-                    "timestamp": 2.5243639197373984
-                },
-                {
-                    "x": 7.653284846152079,
-                    "y": 0.7965661552766882,
-                    "heading": 4.236479358871306e-18,
-                    "angularVelocity": -1.0842391112822455e-16,
-                    "velocityX": 0.7931864980809786,
-                    "velocityY": -0.28402401300808766,
-                    "timestamp": 2.558044716652465
-                },
-                {
-                    "x": 7.671094948642455,
-                    "y": 0.7901887184523844,
-                    "heading": 1.249730071610374e-18,
-                    "angularVelocity": -8.867810508445172e-17,
-                    "velocityX": 0.5287910062011898,
-                    "velocityY": -0.18934934468402032,
-                    "timestamp": 2.591725513567532
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": -2.906959217388984e-34,
-                    "angularVelocity": -3.7105121777105e-17,
-                    "velocityX": 0.26439550643651005,
-                    "velocityY": -0.09467467353653367,
-                    "timestamp": 2.6254063104825986
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": -4.078160951605287e-34,
-                    "angularVelocity": -3.477050502256718e-33,
-                    "velocityX": 7.685636324015334e-33,
-                    "velocityY": -2.3747808746440768e-32,
-                    "timestamp": 2.6590871073976654
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "Stage Side To W1": {
-            "waypoints": [
-                {
-                    "x": 0.71,
-                    "y": 4.36,
-                    "heading": -1.04,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
-                {
-                    "x": 2.079798460006714,
-                    "y": 5.466883659362793,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 10
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 0.71,
-                    "y": 4.36,
-                    "heading": -1.04,
-                    "angularVelocity": 3.226894361928835e-32,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 0.724288196557008,
-                    "y": 4.368098642385211,
-                    "heading": -1.0157828821188275,
-                    "angularVelocity": 0.526633856876912,
-                    "velocityX": 0.3107160851076989,
-                    "velocityY": 0.17611589024403787,
-                    "timestamp": 0.0459847341088113
-                },
-                {
-                    "x": 0.7529777435005003,
-                    "y": 4.384403118337835,
-                    "heading": -0.9690307892055188,
-                    "angularVelocity": 1.0166872510929454,
-                    "velocityX": 0.6238928526933836,
-                    "velocityY": 0.3545627971674321,
-                    "timestamp": 0.0919694682176226
-                },
-                {
-                    "x": 0.7962133909539325,
-                    "y": 4.4090308090891845,
-                    "heading": -0.9019535610081195,
-                    "angularVelocity": 1.4586847025950622,
-                    "velocityX": 0.9402174067404033,
-                    "velocityY": 0.5355623171174385,
-                    "timestamp": 0.1379542023264339
-                },
-                {
-                    "x": 0.8541780196778807,
-                    "y": 4.442096684944602,
-                    "heading": -0.8174192357524789,
-                    "angularVelocity": 1.8383127986701173,
-                    "velocityX": 1.2605189493276492,
-                    "velocityY": 0.7190620212605457,
-                    "timestamp": 0.1839389364352452
-                },
-                {
-                    "x": 0.92708218377146,
-                    "y": 4.483710427261066,
-                    "heading": -0.7190735575075958,
-                    "angularVelocity": 2.1386592779291416,
-                    "velocityX": 1.585399274486893,
-                    "velocityY": 0.9049468942886179,
-                    "timestamp": 0.2299236705440565
-                },
-                {
-                    "x": 1.015132605778429,
-                    "y": 4.5339957868438265,
-                    "heading": -0.6117681897735129,
-                    "angularVelocity": 2.333499797567627,
-                    "velocityX": 1.914775059884505,
-                    "velocityY": 1.0935228953105474,
-                    "timestamp": 0.2759084046528678
-                },
-                {
-                    "x": 1.1184514719230099,
-                    "y": 4.593093468568762,
-                    "heading": -0.5033518248752775,
-                    "angularVelocity": 2.357659927786404,
-                    "velocityX": 2.246807949353511,
-                    "velocityY": 1.2851587134349265,
-                    "timestamp": 0.32189313876167913
-                },
-                {
-                    "x": 1.2364935087746176,
-                    "y": 4.660915178231087,
-                    "heading": -0.4100171287779816,
-                    "angularVelocity": 2.029688719661721,
-                    "velocityX": 2.566983133408849,
-                    "velocityY": 1.4748744551145279,
-                    "timestamp": 0.36787787287049045
-                },
-                {
-                    "x": 1.3657550031232673,
-                    "y": 4.73811911608404,
-                    "heading": -0.35248337281854053,
-                    "angularVelocity": 1.2511490405338719,
-                    "velocityX": 2.8109653530405243,
-                    "velocityY": 1.6789036481164823,
-                    "timestamp": 0.4138626069793018
-                },
-                {
-                    "x": 1.5059170232916794,
-                    "y": 4.82520017003363,
-                    "heading": -0.3304805961437169,
-                    "angularVelocity": 0.47848002388715516,
-                    "velocityX": 3.0480119736421156,
-                    "velocityY": 1.8936948454141527,
-                    "timestamp": 0.4598473410881131
-                },
-                {
-                    "x": 1.6456219690029539,
-                    "y": 4.9288521484832435,
-                    "heading": -0.3304805109628974,
-                    "angularVelocity": 0.0000018523716874218418,
-                    "velocityX": 3.038072273739655,
-                    "velocityY": 2.2540519252400664,
-                    "timestamp": 0.5058320751969244
-                },
-                {
-                    "x": 1.7741171019961581,
-                    "y": 5.046113419715312,
-                    "heading": -0.3304804288379279,
-                    "angularVelocity": 0.000001785918111046389,
-                    "velocityX": 2.794299792821407,
-                    "velocityY": 2.5500043330598703,
-                    "timestamp": 0.5518168093057357
-                },
-                {
-                    "x": 1.8900823069364519,
-                    "y": 5.175779429346925,
-                    "heading": -0.330480329845049,
-                    "angularVelocity": 0.000002152733527656335,
-                    "velocityX": 2.5218196253105503,
-                    "velocityY": 2.8197620828859327,
-                    "timestamp": 0.597801543414547
-                },
-                {
-                    "x": 1.992326225217661,
-                    "y": 5.31651812358221,
-                    "heading": -0.3304801981354216,
-                    "angularVelocity": 0.0000028642033047051737,
-                    "velocityX": 2.2234317597502637,
-                    "velocityY": 3.0605525281991963,
-                    "timestamp": 0.6437862775233584
-                },
-                {
-                    "x": 2.079798460006714,
-                    "y": 5.466883659362793,
-                    "heading": -0.3304799560936257,
-                    "angularVelocity": 0.000005263524964464214,
-                    "velocityX": 1.9022015998192814,
-                    "velocityY": 3.2699011681743237,
-                    "timestamp": 0.6897710116321697
-                },
-                {
-                    "x": 2.16894705974312,
-                    "y": 5.683675424803782,
-                    "heading": -0.33027266768763375,
-                    "angularVelocity": 0.0033444025204108644,
-                    "velocityX": 1.4383284005893175,
-                    "velocityY": 3.49773024107632,
-                    "timestamp": 0.751751710685232
-                },
-                {
-                    "x": 2.226835014262279,
-                    "y": 5.903744633222095,
-                    "heading": -0.30773378760994813,
-                    "angularVelocity": 0.3636435280988788,
-                    "velocityX": 0.9339674350815139,
-                    "velocityY": 3.550608685938014,
-                    "timestamp": 0.8137324097382944
-                },
-                {
-                    "x": 2.2584550007033997,
-                    "y": 6.112330245271792,
-                    "heading": -0.23499422209122486,
-                    "angularVelocity": 1.1735841419993758,
-                    "velocityX": 0.5101585965339779,
-                    "velocityY": 3.3653317119112898,
-                    "timestamp": 0.8757131087913568
-                },
-                {
-                    "x": 2.277604108948135,
-                    "y": 6.297377159682904,
-                    "heading": -0.09486114062468509,
-                    "angularVelocity": 2.260914826833697,
-                    "velocityX": 0.30895276331652,
-                    "velocityY": 2.985557072415336,
-                    "timestamp": 0.9376938078444191
-                },
-                {
-                    "x": 2.288511286424087,
-                    "y": 6.451498238253786,
-                    "heading": 0.04992678964099742,
-                    "angularVelocity": 2.336016412815077,
-                    "velocityX": 0.1759769999789598,
-                    "velocityY": 2.48659793977057,
-                    "timestamp": 0.9996745068974815
-                },
-                {
-                    "x": 2.294708988562371,
-                    "y": 6.5742371130661414,
-                    "heading": 0.17908010356662757,
-                    "angularVelocity": 2.083766654762434,
-                    "velocityX": 0.09999406642681147,
-                    "velocityY": 1.9802757420867263,
-                    "timestamp": 1.061655205950544
-                },
-                {
-                    "x": 2.2980089897573315,
-                    "y": 6.66587189325695,
-                    "heading": 0.2827001929036045,
-                    "angularVelocity": 1.6718122079950113,
-                    "velocityX": 0.05324240038246661,
-                    "velocityY": 1.478440572481471,
-                    "timestamp": 1.1236359050036064
-                },
-                {
-                    "x": 2.2995212581337525,
-                    "y": 6.72670613551003,
-                    "heading": 0.35522073089449935,
-                    "angularVelocity": 1.1700503398453694,
-                    "velocityX": 0.024399020978011547,
-                    "velocityY": 0.9815030030719727,
-                    "timestamp": 1.185616604056669
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "angularVelocity": 0.609532801060419,
-                    "velocityX": 0.007724047543277551,
-                    "velocityY": 0.48876287219728576,
-                    "timestamp": 1.2475973031097314
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "angularVelocity": 2.340826570383063e-30,
-                    "velocityX": -1.14239845433479e-30,
-                    "velocityY": 0,
-                    "timestamp": 1.3095780021627939
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "Stage Side To W2": {
-            "waypoints": [
-                {
-                    "x": 0.71,
-                    "y": 4.36,
-                    "heading": -1.04,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 14
-                },
-                {
-                    "x": 1.7808787822723389,
-                    "y": 5.128108024597168,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 6
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 0.71,
-                    "y": 4.36,
-                    "heading": -1.04,
-                    "angularVelocity": -1.335008934228872e-34,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 0.7208748853953925,
-                    "y": 4.367346013203549,
-                    "heading": -1.0266172190247755,
-                    "angularVelocity": 0.3311622793955978,
-                    "velocityX": 0.2691033980434437,
-                    "velocityY": 0.1817800411932904,
-                    "timestamp": 0.040411549889224324
-                },
-                {
-                    "x": 0.7426520542538281,
-                    "y": 4.382068365397769,
-                    "heading": -1.0004187382318452,
-                    "angularVelocity": 0.6482919082476458,
-                    "velocityX": 0.5388847722527592,
-                    "velocityY": 0.3643105061443391,
-                    "timestamp": 0.08082309977844865
-                },
-                {
-                    "x": 0.7753644718100721,
-                    "y": 4.404202344534013,
-                    "heading": -0.9621111981448724,
-                    "angularVelocity": 0.9479354341019116,
-                    "velocityX": 0.8094818844096516,
-                    "velocityY": 0.5477141855958662,
-                    "timestamp": 0.12123464966767297
-                },
-                {
-                    "x": 0.8190534716968692,
-                    "y": 4.4337878475210974,
-                    "heading": -0.9126009199564967,
-                    "angularVelocity": 1.2251516787674004,
-                    "velocityX": 1.0811018138764033,
-                    "velocityY": 0.7321051300478436,
-                    "timestamp": 0.1616461995568973
-                },
-                {
-                    "x": 0.8737711662497897,
-                    "y": 4.470868927169861,
-                    "heading": -0.8530717234087362,
-                    "angularVelocity": 1.4730738294111758,
-                    "velocityX": 1.3540112839748029,
-                    "velocityY": 0.9175861789614466,
-                    "timestamp": 0.20205774944612162
-                },
-                {
-                    "x": 0.9395812969768862,
-                    "y": 4.515493926508116,
-                    "heading": -0.7850998870787868,
-                    "angularVelocity": 1.681990334849146,
-                    "velocityX": 1.6284980632392125,
-                    "velocityY": 1.104263495475429,
-                    "timestamp": 0.24246929933534594
-                },
-                {
-                    "x": 1.0165562063250766,
-                    "y": 4.567717120220609,
-                    "heading": -0.7108767199363526,
-                    "angularVelocity": 1.8366820214986515,
-                    "velocityX": 1.9047749853493152,
-                    "velocityY": 1.292283860818163,
-                    "timestamp": 0.28288084922457024
-                },
-                {
-                    "x": 1.104761821592339,
-                    "y": 4.627598200600785,
-                    "heading": -0.633776622573869,
-                    "angularVelocity": 1.9078728129415852,
-                    "velocityX": 2.1826833048732497,
-                    "velocityY": 1.481781335888433,
-                    "timestamp": 0.3232923991137946
-                },
-                {
-                    "x": 1.2041714779159751,
-                    "y": 4.695156297767735,
-                    "heading": -0.5601638063396884,
-                    "angularVelocity": 1.8215786436295356,
-                    "velocityX": 2.4599317916817567,
-                    "velocityY": 1.6717521934233843,
-                    "timestamp": 0.36370394900301894
-                },
-                {
-                    "x": 1.3134031887504467,
-                    "y": 4.770142726541239,
-                    "heading": -0.5079341517285784,
-                    "angularVelocity": 1.2924437383441445,
-                    "velocityX": 2.7029824674850675,
-                    "velocityY": 1.8555692364943988,
-                    "timestamp": 0.4041154988922433
-                },
-                {
-                    "x": 1.4312329739367307,
-                    "y": 4.852476770750293,
-                    "heading": -0.48455184709669846,
-                    "angularVelocity": 0.5786045003464412,
-                    "velocityX": 2.9157452636505585,
-                    "velocityY": 2.0373889255608297,
-                    "timestamp": 0.44452704878146765
-                },
-                {
-                    "x": 1.55675883861541,
-                    "y": 4.939734199489085,
-                    "heading": -0.4845517983636149,
-                    "angularVelocity": 0.0000012059196871647072,
-                    "velocityX": 3.10618783547697,
-                    "velocityY": 2.1592200491686473,
-                    "timestamp": 0.484938598670692
-                },
-                {
-                    "x": 1.6733111473031865,
-                    "y": 5.037206282378932,
-                    "heading": -0.48207265962656115,
-                    "angularVelocity": 0.061347281751120815,
-                    "velocityX": 2.884133595649469,
-                    "velocityY": 2.4119857604332493,
-                    "timestamp": 0.5253501485599164
-                },
-                {
-                    "x": 1.7808787822723389,
-                    "y": 5.128108024597168,
-                    "heading": -0.4506827811882082,
-                    "angularVelocity": 0.7767551238296608,
-                    "velocityX": 2.661804243193232,
-                    "velocityY": 2.2494000469522843,
-                    "timestamp": 0.5657616984491407
-                },
-                {
-                    "x": 1.9395772431433458,
-                    "y": 5.270947819082425,
-                    "heading": -0.3258768071798875,
-                    "angularVelocity": 1.7295324507132326,
-                    "velocityX": 2.1992067297704168,
-                    "velocityY": 1.9794409825205124,
-                    "timestamp": 0.6379233823521242
-                },
-                {
-                    "x": 2.0647143213126102,
-                    "y": 5.386258683660538,
-                    "heading": -0.20467464704644728,
-                    "angularVelocity": 1.6795916278282566,
-                    "velocityX": 1.7341208159374817,
-                    "velocityY": 1.5979514105178143,
-                    "timestamp": 0.7100850662551077
-                },
-                {
-                    "x": 2.157719068276139,
-                    "y": 5.473035188211263,
-                    "heading": -0.10556396576995164,
-                    "angularVelocity": 1.373453000483514,
-                    "velocityX": 1.2888383686911677,
-                    "velocityY": 1.2025288194132213,
-                    "timestamp": 0.7822467501580912
-                },
-                {
-                    "x": 2.219336350583297,
-                    "y": 5.530988274582093,
-                    "heading": -0.03600663550492556,
-                    "angularVelocity": 0.9639094669484316,
-                    "velocityX": 0.853878110577325,
-                    "velocityY": 0.8031005269880169,
-                    "timestamp": 0.8544084340610747
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 7.006093267694686e-35,
-                    "angularVelocity": 0.49897166414982813,
-                    "velocityX": 0.4249297931839902,
-                    "velocityY": 0.40203781077103196,
-                    "timestamp": 0.9265701179640582
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 2.908643221674668e-34,
-                    "velocityX": -1.689604381582848e-35,
-                    "velocityY": 1.5152673195076234e-35,
-                    "timestamp": 0.9987318018670417
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "Stage Side To W3": {
-            "waypoints": [
-                {
-                    "x": 0.71,
-                    "y": 4.36,
-                    "heading": -1.04,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 12
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 0.71,
-                    "y": 4.36,
-                    "heading": -1.04,
-                    "angularVelocity": -2.204776300650663e-32,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 0.7541176231747116,
-                    "y": 4.360036687345491,
-                    "heading": -1.0162545684059647,
-                    "angularVelocity": 0.32461244675544165,
-                    "velocityX": 0.6031109414489596,
-                    "velocityY": 0.0005015351663374791,
-                    "timestamp": 0.07315009584923313
-                },
-                {
-                    "x": 0.8424088865359771,
-                    "y": 4.360100324606402,
-                    "heading": -0.9701479863262087,
-                    "angularVelocity": 0.630301048063541,
-                    "velocityX": 1.2069876646948012,
-                    "velocityY": 0.0008699545799604022,
-                    "timestamp": 0.14630019169846625
-                },
-                {
-                    "x": 0.974955221752511,
-                    "y": 4.3601789275184695,
-                    "heading": -0.9038423136459668,
-                    "angularVelocity": 0.9064331619859869,
-                    "velocityX": 1.8119776013652948,
-                    "velocityY": 0.0010745428444254071,
-                    "timestamp": 0.21945028754769938
-                },
-                {
-                    "x": 1.151884261990258,
-                    "y": 4.360259672825357,
-                    "heading": -0.8212724833638784,
-                    "angularVelocity": 1.128772687492223,
-                    "velocityX": 2.4187123500481573,
-                    "velocityY": 0.0011038305000959348,
-                    "timestamp": 0.2926003833969325
-                },
-                {
-                    "x": 1.3733948258568207,
-                    "y": 4.360324542882412,
-                    "heading": -0.7323749312717828,
-                    "angularVelocity": 1.215275948172809,
-                    "velocityX": 3.028165052894887,
-                    "velocityY": 0.0008868075468144582,
-                    "timestamp": 0.36575047924616566
-                },
-                {
-                    "x": 1.6366160230983884,
-                    "y": 4.359789775146019,
-                    "heading": -0.6992109641912426,
-                    "angularVelocity": 0.4533687440268879,
-                    "velocityX": 3.5983711871554758,
-                    "velocityY": -0.007310554144235059,
-                    "timestamp": 0.4389005750953988
-                },
-                {
-                    "x": 1.8581232786849116,
-                    "y": 4.3598960912826,
-                    "heading": -0.6098314600860953,
-                    "angularVelocity": 1.2218644838062247,
-                    "velocityX": 3.0281198269795637,
-                    "velocityY": 0.001453397091854858,
-                    "timestamp": 0.512050670944632
-                },
-                {
-                    "x": 2.035050245808528,
-                    "y": 4.359956526837273,
-                    "heading": -0.5268891037054299,
-                    "angularVelocity": 1.1338653137464598,
-                    "velocityX": 2.4186840094956525,
-                    "velocityY": 0.000826185584123381,
-                    "timestamp": 0.5852007667938651
-                },
-                {
-                    "x": 2.1675937895751547,
-                    "y": 4.359986275638159,
-                    "heading": -0.46022423203591045,
-                    "angularVelocity": 0.9113435996982586,
-                    "velocityX": 1.8119394407877087,
-                    "velocityY": 0.00040668163915013244,
-                    "timestamp": 0.6583508626430983
-                },
-                {
-                    "x": 2.255882919836075,
-                    "y": 4.359997704013322,
-                    "heading": -0.41385948514831483,
-                    "angularVelocity": 0.6338302957681125,
-                    "velocityX": 1.2069585040994306,
-                    "velocityY": 0.0001562318549097459,
-                    "timestamp": 0.7315009584923314
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": 0.32617161838698483,
-                    "velocityX": 0.6031035182080025,
-                    "velocityY": 0.00003138733655167869,
-                    "timestamp": 0.8046510543415646
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": -2.2755735462869925e-32,
-                    "velocityX": -9.780003655586715e-35,
-                    "velocityY": 0,
-                    "timestamp": 0.8778011501907977
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "Stage Side To C1": {
-            "waypoints": [
-                {
-                    "x": 0.71,
-                    "y": 4.36,
-                    "heading": -1.04,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 14
-                },
-                {
-                    "x": 1.7609508037567139,
-                    "y": 5.167963981628418,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 9
-                },
-                {
-                    "x": 2.896845817565918,
-                    "y": 6.323787212371826,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 7
-                },
-                {
-                    "x": 3.793605089187622,
-                    "y": 6.363643169403076,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 16
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 0.71,
-                    "y": 4.36,
-                    "heading": -1.04,
-                    "angularVelocity": -3.8536087109009435e-31,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 0.7208977987123659,
-                    "y": 4.366230614894293,
-                    "heading": -1.0125695703411368,
-                    "angularVelocity": 0.6607032256639885,
-                    "velocityX": 0.262489900865184,
-                    "velocityY": 0.1500737469179648,
-                    "timestamp": 0.04151702094612918
-                },
-                {
-                    "x": 0.7428348149738224,
-                    "y": 4.378911218024912,
-                    "heading": -0.9596996326030717,
-                    "angularVelocity": 1.2734521055122956,
-                    "velocityX": 0.5283860874778604,
-                    "velocityY": 0.30543143129192235,
-                    "timestamp": 0.08303404189225835
-                },
-                {
-                    "x": 0.7759938113776359,
-                    "y": 4.398288777276395,
-                    "heading": -0.8840273814949616,
-                    "angularVelocity": 1.8226801775305617,
-                    "velocityX": 0.7986843865038376,
-                    "velocityY": 0.46673770925264374,
-                    "timestamp": 0.12455106283838753
-                },
-                {
-                    "x": 0.8206097262268935,
-                    "y": 4.424610495629411,
-                    "heading": -0.7889470653064761,
-                    "angularVelocity": 2.2901526656373563,
-                    "velocityX": 1.0746415285232955,
-                    "velocityY": 0.6339982434446707,
-                    "timestamp": 0.1660680837845167
-                },
-                {
-                    "x": 0.876940069443398,
-                    "y": 4.458114052595694,
-                    "heading": -0.6785120124719297,
-                    "angularVelocity": 2.6599946315634684,
-                    "velocityX": 1.3568011849799015,
-                    "velocityY": 0.8069836467704494,
-                    "timestamp": 0.2075851047306459
-                },
-                {
-                    "x": 0.9451906760990277,
-                    "y": 4.499074634849957,
-                    "heading": -0.5574056685886438,
-                    "angularVelocity": 2.9170287540888538,
-                    "velocityX": 1.6439186892586013,
-                    "velocityY": 0.9865973357595108,
-                    "timestamp": 0.2491021256767751
-                },
-                {
-                    "x": 1.0254198074155758,
-                    "y": 4.547882919999799,
-                    "heading": -0.43194974381010975,
-                    "angularVelocity": 3.0217949631302035,
-                    "velocityX": 1.9324395028417536,
-                    "velocityY": 1.175621083531942,
-                    "timestamp": 0.2906191466229043
-                },
-                {
-                    "x": 1.117246624722019,
-                    "y": 4.6049782109254505,
-                    "heading": -0.31304235816438886,
-                    "angularVelocity": 2.8640635319318393,
-                    "velocityX": 2.211787243252437,
-                    "velocityY": 1.3752261030390898,
-                    "timestamp": 0.33213616756903347
-                },
-                {
-                    "x": 1.217280699738284,
-                    "y": 4.671027805780365,
-                    "heading": -0.22202476317889255,
-                    "angularVelocity": 2.1922959044630015,
-                    "velocityX": 2.409471410436724,
-                    "velocityY": 1.5909040039398776,
-                    "timestamp": 0.37365318851516266
-                },
-                {
-                    "x": 1.3245506705159111,
-                    "y": 4.7468577293550815,
-                    "heading": -0.15914757808634442,
-                    "angularVelocity": 1.5144917351940563,
-                    "velocityX": 2.5837588616171234,
-                    "velocityY": 1.8264779564293179,
-                    "timestamp": 0.41517020946129185
-                },
-                {
-                    "x": 1.4375421795348913,
-                    "y": 4.833718356767073,
-                    "heading": -0.12357000071830693,
-                    "angularVelocity": 0.8569395529247944,
-                    "velocityX": 2.7215707303608654,
-                    "velocityY": 2.0921690774675916,
-                    "timestamp": 0.45668723040742104
-                },
-                {
-                    "x": 1.5526379689004826,
-                    "y": 4.933380534066268,
-                    "heading": -0.11179394732799099,
-                    "angularVelocity": 0.2836439879911631,
-                    "velocityX": 2.7722554928653214,
-                    "velocityY": 2.4005136936152596,
-                    "timestamp": 0.49820425135355023
-                },
-                {
-                    "x": 1.6621482250840773,
-                    "y": 5.045879001435295,
-                    "heading": -0.11165007196252263,
-                    "angularVelocity": 0.0034654549371103873,
-                    "velocityX": 2.6377195108883376,
-                    "velocityY": 2.709695079403094,
-                    "timestamp": 0.5397212722996794
-                },
-                {
-                    "x": 1.760950803756714,
-                    "y": 5.167963981628418,
-                    "heading": -0.11164985662454338,
-                    "angularVelocity": 0.000005186739663230103,
-                    "velocityX": 2.3798089655981185,
-                    "velocityY": 2.940600684033376,
-                    "timestamp": 0.5812382932458086
-                },
-                {
-                    "x": 1.8636898338676644,
-                    "y": 5.328509480252694,
-                    "heading": -0.11164971710954129,
-                    "angularVelocity": 0.000002768959602908003,
-                    "velocityX": 2.039065475146862,
-                    "velocityY": 3.1863526751214604,
-                    "timestamp": 0.6316236444586746
-                },
-                {
-                    "x": 1.9479932631191832,
-                    "y": 5.499457142063362,
-                    "heading": -0.11164961128911181,
-                    "angularVelocity": 0.0000021002221265930453,
-                    "velocityX": 1.6731733970781186,
-                    "velocityY": 3.3928048072606183,
-                    "timestamp": 0.6820089956715406
-                },
-                {
-                    "x": 2.043351099956472,
-                    "y": 5.66449366180702,
-                    "heading": -0.11164949905977092,
-                    "angularVelocity": 0.000002227420038927282,
-                    "velocityX": 1.8925706488092502,
-                    "velocityY": 3.275486143730564,
-                    "timestamp": 0.7323943468844066
-                },
-                {
-                    "x": 2.1564178056024077,
-                    "y": 5.817933945525861,
-                    "heading": -0.11163496800446338,
-                    "angularVelocity": 0.0002883986749930945,
-                    "velocityX": 2.244039247910364,
-                    "velocityY": 3.0453352020399613,
-                    "timestamp": 0.7827796980972727
-                },
-                {
-                    "x": 2.2829725450073877,
-                    "y": 5.955279135270152,
-                    "heading": -0.10213363222908782,
-                    "angularVelocity": 0.1885733760372676,
-                    "velocityX": 2.5117367718340784,
-                    "velocityY": 2.7258952540120527,
-                    "timestamp": 0.8331650493101387
-                },
-                {
-                    "x": 2.421245283059438,
-                    "y": 6.075180412328107,
-                    "heading": -0.08356505866258204,
-                    "angularVelocity": 0.36853119252760197,
-                    "velocityX": 2.7443043409080725,
-                    "velocityY": 2.379685249204375,
-                    "timestamp": 0.8835504005230047
-                },
-                {
-                    "x": 2.570177190376964,
-                    "y": 6.176890921850726,
-                    "heading": -0.058269582676031995,
-                    "angularVelocity": 0.5020402830847994,
-                    "velocityX": 2.9558572825813,
-                    "velocityY": 2.018652387526583,
-                    "timestamp": 0.9339357517358707
-                },
-                {
-                    "x": 2.728959152818659,
-                    "y": 6.259893890741026,
-                    "heading": -0.029260248722302817,
-                    "angularVelocity": 0.5757493647196714,
-                    "velocityX": 3.151351704820358,
-                    "velocityY": 1.6473631103484576,
-                    "timestamp": 0.9843211029487368
-                },
-                {
-                    "x": 2.896845817565918,
-                    "y": 6.323787212371826,
-                    "heading": -5.388333556894659e-33,
-                    "angularVelocity": 0.580729279745141,
-                    "velocityX": 3.332053081047339,
-                    "velocityY": 1.2680932075002167,
-                    "timestamp": 1.034706454161603
-                },
-                {
-                    "x": 3.0174812814006593,
-                    "y": 6.35877580192698,
-                    "heading": 0.017969633030989543,
-                    "angularVelocity": 0.512929568417621,
-                    "velocityX": 3.443447970619488,
-                    "velocityY": 0.9987227956847937,
-                    "timestamp": 1.069739788442565
-                },
-                {
-                    "x": 3.141783379846136,
-                    "y": 6.384241994405335,
-                    "heading": 0.0331535119160805,
-                    "angularVelocity": 0.43341232568386434,
-                    "velocityX": 3.5481092792260864,
-                    "velocityY": 0.7269131814328351,
-                    "timestamp": 1.1047731227235271
-                },
-                {
-                    "x": 3.269386252376355,
-                    "y": 6.400068652575719,
-                    "heading": 0.0449202302050253,
-                    "angularVelocity": 0.3358720639272233,
-                    "velocityX": 3.642327376147768,
-                    "velocityY": 0.45175997361343984,
-                    "timestamp": 1.1398064570044892
-                },
-                {
-                    "x": 3.3996512457384864,
-                    "y": 6.406094302159117,
-                    "heading": 0.052174145586997546,
-                    "angularVelocity": 0.20705752202617325,
-                    "velocityX": 3.7183155995559303,
-                    "velocityY": 0.17199760476778314,
-                    "timestamp": 1.1748397912854514
-                },
-                {
-                    "x": 3.531556253779136,
-                    "y": 6.402132340099008,
-                    "heading": 0.05397985174916118,
-                    "angularVelocity": 0.05154251518314789,
-                    "velocityX": 3.7651285767486513,
-                    "velocityY": -0.11309120703085081,
-                    "timestamp": 1.2098731255664135
-                },
-                {
-                    "x": 3.663324219452337,
-                    "y": 6.387950136374773,
-                    "heading": 0.05397986327261661,
-                    "angularVelocity": 3.2892831051068895e-7,
-                    "velocityX": 3.7612168061547906,
-                    "velocityY": -0.4048202666228151,
-                    "timestamp": 1.2449064598473756
-                },
-                {
-                    "x": 3.793605089187622,
-                    "y": 6.363643169403076,
-                    "heading": 0.053979860367415415,
-                    "angularVelocity": -8.292676841260746e-8,
-                    "velocityX": 3.718768778627541,
-                    "velocityY": -0.6938239671072872,
-                    "timestamp": 1.2799397941283377
-                },
-                {
-                    "x": 4.111061973016203,
-                    "y": 6.363890421291427,
-                    "heading": 0.05397985774152245,
-                    "angularVelocity": -3.129115871783651e-8,
-                    "velocityX": 3.7829393158349456,
-                    "velocityY": 0.002946349368989899,
-                    "timestamp": 1.3638578423242849
-                },
-                {
-                    "x": 4.423042820635495,
-                    "y": 6.422600616815994,
-                    "heading": 0.053979855524806185,
-                    "angularVelocity": -2.641525047123925e-8,
-                    "velocityX": 3.7176847451316233,
-                    "velocityY": 0.6996134536813446,
-                    "timestamp": 1.447775890520232
-                },
-                {
-                    "x": 4.7253380303410815,
-                    "y": 6.519536384153899,
-                    "heading": 0.05397985339265137,
-                    "angularVelocity": -2.5407583503873633e-8,
-                    "velocityX": 3.602266928321921,
-                    "velocityY": 1.1551241886794503,
-                    "timestamp": 1.5316939387161792
-                },
-                {
-                    "x": 5.027633110558163,
-                    "y": 6.616472555302148,
-                    "heading": 0.053979851260496835,
-                    "angularVelocity": -2.5407580227573106e-8,
-                    "velocityX": 3.6022653852867035,
-                    "velocityY": 1.1551290006400656,
-                    "timestamp": 1.6156119869121264
-                },
-                {
-                    "x": 5.329928190774575,
-                    "y": 6.71340872645249,
-                    "heading": 0.05397984912834238,
-                    "angularVelocity": -2.540757910781892e-8,
-                    "velocityX": 3.602265385278706,
-                    "velocityY": 1.1551290006650068,
-                    "timestamp": 1.6995300351080735
-                },
-                {
-                    "x": 5.632223270990985,
-                    "y": 6.810344897602833,
-                    "heading": 0.053979846996187915,
-                    "angularVelocity": -2.5407579299100972e-8,
-                    "velocityX": 3.602265385278706,
-                    "velocityY": 1.155129000665007,
-                    "timestamp": 1.7834480833040207
-                },
-                {
-                    "x": 5.934518351207396,
-                    "y": 6.907281068753174,
-                    "heading": 0.05397984486403343,
-                    "angularVelocity": -2.5407579624099417e-8,
-                    "velocityX": 3.602265385278706,
-                    "velocityY": 1.1551290006650068,
-                    "timestamp": 1.8673661314999679
-                },
-                {
-                    "x": 6.236813431423807,
-                    "y": 7.004217239903516,
-                    "heading": 0.053979842731878905,
-                    "angularVelocity": -2.5407579955507513e-8,
-                    "velocityX": 3.602265385278706,
-                    "velocityY": 1.1551290006650072,
-                    "timestamp": 1.951284179695915
-                },
-                {
-                    "x": 6.539108511640218,
-                    "y": 7.1011534110538586,
-                    "heading": 0.053979840599724424,
-                    "angularVelocity": -2.5407579566600852e-8,
-                    "velocityX": 3.6022653852787063,
-                    "velocityY": 1.155129000665007,
-                    "timestamp": 2.0352022278918622
-                },
-                {
-                    "x": 6.841403591857615,
-                    "y": 7.198089582201063,
-                    "heading": 0.05397983846756001,
-                    "angularVelocity": -2.5407697930922655e-8,
-                    "velocityX": 3.6022653852904623,
-                    "velocityY": 1.1551290006276251,
-                    "timestamp": 2.119120276087809
-                },
-                {
-                    "x": 7.120935706136836,
-                    "y": 7.287726381323771,
-                    "heading": 0.035986533919804255,
-                    "angularVelocity": -0.21441519356800662,
-                    "velocityX": 3.331013057245073,
-                    "velocityY": 1.0681468533849583,
-                    "timestamp": 2.203038324283756
-                },
-                {
-                    "x": 7.344561416731701,
-                    "y": 7.359435826630496,
-                    "heading": 0.021591909406666148,
-                    "angularVelocity": -0.17153192695269756,
-                    "velocityX": 2.664810674250942,
-                    "velocityY": 0.8545175543082761,
-                    "timestamp": 2.286956372479703
-                },
-                {
-                    "x": 7.512280706030795,
-                    "y": 7.413217912582047,
-                    "heading": 0.010795955954566746,
-                    "angularVelocity": -0.12864876727003446,
-                    "velocityX": 1.9986080813923648,
-                    "velocityY": 0.6408881892244497,
-                    "timestamp": 2.37087442067565
-                },
-                {
-                    "x": 7.624093568104708,
-                    "y": 7.449072637343766,
-                    "heading": 0.0035986551256689905,
-                    "angularVelocity": -0.08576582729978673,
-                    "velocityX": 1.3324054178766367,
-                    "velocityY": 0.42725880227814755,
-                    "timestamp": 2.454792468871597
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": -6.709954361314771e-31,
-                    "angularVelocity": -0.04288296979061891,
-                    "velocityX": 0.6662027191665668,
-                    "velocityY": 0.21362940442053102,
-                    "timestamp": 2.538710517067544
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 6.378058636012386e-33,
-                    "angularVelocity": 8.071845383822447e-30,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 2.622628565263491
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "Stage Side To C2": {
-            "waypoints": [
-                {
-                    "x": 0.71,
-                    "y": 4.36,
-                    "heading": -1.04,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 18
-                },
-                {
-                    "x": 2.837061882019043,
-                    "y": 6.303859233856201,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 28
-                },
-                {
-                    "x": 5.7195634841918945,
-                    "y": 6.649363040924072,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 26
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 0.71,
-                    "y": 4.36,
-                    "heading": -1.04,
-                    "angularVelocity": 0,
-                    "velocityX": -4.815368277433195e-31,
-                    "velocityY": 1.7671411030208604e-30,
-                    "timestamp": 0
-                },
-                {
-                    "x": 0.7256756209676614,
-                    "y": 4.374259536946564,
-                    "heading": -0.9853384433657985,
-                    "angularVelocity": 0.9927336490852509,
-                    "velocityX": 0.2846921559393299,
-                    "velocityY": 0.25897400328871373,
-                    "timestamp": 0.05506165393280401
-                },
-                {
-                    "x": 0.7571509327835594,
-                    "y": 4.40368458563067,
-                    "heading": -0.8810495211913171,
-                    "angularVelocity": 1.8940390403219183,
-                    "velocityX": 0.5716376019938672,
-                    "velocityY": 0.5344018310849077,
-                    "timestamp": 0.11012330786560802
-                },
-                {
-                    "x": 0.8047782151983651,
-                    "y": 4.4492655723711865,
-                    "heading": -0.7351323547259855,
-                    "angularVelocity": 2.6500687146592434,
-                    "velocityX": 0.8649809624852585,
-                    "velocityY": 0.8278172463956943,
-                    "timestamp": 0.16518496179841202
-                },
-                {
-                    "x": 0.8693373364829333,
-                    "y": 4.511878147553763,
-                    "heading": -0.5600607568180229,
-                    "angularVelocity": 3.179555741677643,
-                    "velocityX": 1.1724878690220806,
-                    "velocityY": 1.1371357507548814,
-                    "timestamp": 0.22024661573121604
-                },
-                {
-                    "x": 0.9516435423656624,
-                    "y": 4.592143379904835,
-                    "heading": -0.3723491539523138,
-                    "angularVelocity": 3.4091166802965285,
-                    "velocityX": 1.4948008278694735,
-                    "velocityY": 1.4577337696599522,
-                    "timestamp": 0.27530826966402006
-                },
-                {
-                    "x": 1.0514451430006502,
-                    "y": 4.68995617229961,
-                    "heading": -0.20247463803020274,
-                    "angularVelocity": 3.085169147472857,
-                    "velocityX": 1.8125427317637794,
-                    "velocityY": 1.7764230713854863,
-                    "timestamp": 0.33036992359682404
-                },
-                {
-                    "x": 1.163482365369112,
-                    "y": 4.8045671318094305,
-                    "heading": -0.08453465328971378,
-                    "angularVelocity": 2.1419622607883397,
-                    "velocityX": 2.034759480799993,
-                    "velocityY": 2.0815023037586085,
-                    "timestamp": 0.38543157752962803
-                },
-                {
-                    "x": 1.290791904505612,
-                    "y": 4.932877435439672,
-                    "heading": -0.017113580237308034,
-                    "angularVelocity": 1.2244650902572813,
-                    "velocityX": 2.3121270438301877,
-                    "velocityY": 2.330302387705078,
-                    "timestamp": 0.440493231462432
-                },
-                {
-                    "x": 1.4344972301022125,
-                    "y": 5.07384452901374,
-                    "heading": -0.000001124802148976757,
-                    "angularVelocity": 0.3107871669853269,
-                    "velocityX": 2.6098984562178145,
-                    "velocityY": 2.560168166154319,
-                    "timestamp": 0.495554885395236
-                },
-                {
-                    "x": 1.5830888632571813,
-                    "y": 5.219814728179112,
-                    "heading": -9.965376040774715e-7,
-                    "angularVelocity": 0.0000023294713396498257,
-                    "velocityX": 2.6986409332390493,
-                    "velocityY": 2.6510318659074916,
-                    "timestamp": 0.55061653932804
-                },
-                {
-                    "x": 1.7316392407262138,
-                    "y": 5.365826918457485,
-                    "heading": -8.792165857555993e-7,
-                    "angularVelocity": 0.000002130720927018926,
-                    "velocityX": 2.697891669756242,
-                    "velocityY": 2.6517944858193996,
-                    "timestamp": 0.605678193260844
-                },
-                {
-                    "x": 1.8801815584157808,
-                    "y": 5.511847308171012,
-                    "heading": -7.618956854973843e-7,
-                    "angularVelocity": 0.000002130718782810582,
-                    "velocityX": 2.69774529239705,
-                    "velocityY": 2.6519433995297197,
-                    "timestamp": 0.6607398471936481
-                },
-                {
-                    "x": 2.0287247432411455,
-                    "y": 5.657866815749449,
-                    "heading": -6.445747704723799e-7,
-                    "angularVelocity": 0.000002130719050997138,
-                    "velocityX": 2.697761040854699,
-                    "velocityY": 2.651927378662928,
-                    "timestamp": 0.7158015011264521
-                },
-                {
-                    "x": 2.1772589110251723,
-                    "y": 5.8038954956427204,
-                    "heading": -5.272539834240943e-7,
-                    "angularVelocity": 0.000002130716726753334,
-                    "velocityX": 2.6975972782370956,
-                    "velocityY": 2.652093961273629,
-                    "timestamp": 0.7708631550592562
-                },
-                {
-                    "x": 2.325792733071576,
-                    "y": 5.949924527226785,
-                    "heading": -4.0993217710172413e-7,
-                    "angularVelocity": 0.000002130735238257545,
-                    "velocityX": 2.6975909991452314,
-                    "velocityY": 2.6521003484836045,
-                    "timestamp": 0.8259248089920602
-                },
-                {
-                    "x": 2.481459175645397,
-                    "y": 6.088325151306142,
-                    "heading": -2.908915465695403e-7,
-                    "angularVelocity": 0.0000021619515945987695,
-                    "velocityX": 2.8271297982536217,
-                    "velocityY": 2.5135573342462116,
-                    "timestamp": 0.8809864629248643
-                },
-                {
-                    "x": 2.6527099574524438,
-                    "y": 6.206899777175588,
-                    "heading": -1.5899541452377036e-7,
-                    "angularVelocity": 0.0000023954262651640655,
-                    "velocityX": 3.1101641446636745,
-                    "velocityY": 2.1534882699533155,
-                    "timestamp": 0.9360481168576683
-                },
-                {
-                    "x": 2.837061882019043,
-                    "y": 6.303859233856201,
-                    "heading": 0,
-                    "angularVelocity": 0.0000028875887873359224,
-                    "velocityX": 3.348100018792042,
-                    "velocityY": 1.760925249319746,
-                    "timestamp": 0.9911097707904724
-                },
-                {
-                    "x": 2.932152988543463,
-                    "y": 6.346714053639414,
-                    "heading": 2.0172187169325966e-12,
-                    "angularVelocity": 7.316288159012044e-11,
-                    "velocityX": 3.4488770423117883,
-                    "velocityY": 1.5543094355402358,
-                    "timestamp": 1.018681384784641
-                },
-                {
-                    "x": 3.0296712234656686,
-                    "y": 6.383714706348048,
-                    "heading": 2.030359324162707e-12,
-                    "angularVelocity": 4.765991109637323e-13,
-                    "velocityX": 3.5369070139469976,
-                    "velocityY": 1.3419835602475594,
-                    "timestamp": 1.0462529987788096
-                },
-                {
-                    "x": 3.129255935132477,
-                    "y": 6.414726637699066,
-                    "heading": 2.0283850653761223e-12,
-                    "angularVelocity": -7.160477709378183e-14,
-                    "velocityX": 3.611856443643096,
-                    "velocityY": 1.1247775106178561,
-                    "timestamp": 1.0738246127729782
-                },
-                {
-                    "x": 3.2305380260225394,
-                    "y": 6.439642353966136,
-                    "heading": 2.0263773134602995e-12,
-                    "angularVelocity": -7.281954448876825e-14,
-                    "velocityX": 3.6734190066355246,
-                    "velocityY": 0.9036727510191612,
-                    "timestamp": 1.1013962267671469
-                },
-                {
-                    "x": 3.3331381427053266,
-                    "y": 6.458405909393185,
-                    "heading": 2.0244535837319738e-12,
-                    "angularVelocity": -6.977212868461473e-14,
-                    "velocityX": 3.7212227294451083,
-                    "velocityY": 0.6805388843797584,
-                    "timestamp": 1.1289678407613155
-                },
-                {
-                    "x": 3.43663787164561,
-                    "y": 6.471315638605137,
-                    "heading": 2.022533482460756e-12,
-                    "angularVelocity": -6.96405259775829e-14,
-                    "velocityX": 3.753850933863969,
-                    "velocityY": 0.4682253717820687,
-                    "timestamp": 1.1565394547554841
-                },
-                {
-                    "x": 3.5403567621307404,
-                    "y": 6.4823268492843,
-                    "heading": 2.0206101184182045e-12,
-                    "angularVelocity": -6.975886505788547e-14,
-                    "velocityX": 3.7617997447283904,
-                    "velocityY": 0.39936764973541705,
-                    "timestamp": 1.1841110687496528
-                },
-                {
-                    "x": 3.6440832149520914,
-                    "y": 6.493266592802732,
-                    "heading": 2.018711397348351e-12,
-                    "angularVelocity": -6.886508434036338e-14,
-                    "velocityX": 3.7620740245091877,
-                    "velocityY": 0.3967755939732401,
-                    "timestamp": 1.2116826827438214
-                },
-                {
-                    "x": 3.747806936839693,
-                    "y": 6.504232199027323,
-                    "heading": 2.016809036122773e-12,
-                    "angularVelocity": -6.89971097222047e-14,
-                    "velocityX": 3.7619749757661194,
-                    "velocityY": 0.39771361326329024,
-                    "timestamp": 1.23925429673799
-                },
-                {
-                    "x": 3.8515283310140283,
-                    "y": 6.515219800794399,
-                    "heading": 2.0149121825042556e-12,
-                    "angularVelocity": -6.87973533298754e-14,
-                    "velocityX": 3.761890551500436,
-                    "velocityY": 0.3985113736873028,
-                    "timestamp": 1.2668259107321587
-                },
-                {
-                    "x": 3.9552475387122907,
-                    "y": 6.52622802304771,
-                    "heading": 2.0130014782113644e-12,
-                    "angularVelocity": -6.92997060168648e-14,
-                    "velocityX": 3.7618112497917684,
-                    "velocityY": 0.3992592619503583,
-                    "timestamp": 1.2943975247263273
-                },
-                {
-                    "x": 4.058964880445898,
-                    "y": 6.537253812271398,
-                    "heading": 2.011079593605957e-12,
-                    "angularVelocity": -6.970520685257517e-14,
-                    "velocityX": 3.7617435727707904,
-                    "velocityY": 0.3998964016833969,
-                    "timestamp": 1.321969138720496
-                },
-                {
-                    "x": 4.162680825175896,
-                    "y": 6.5482927349999205,
-                    "heading": 2.009190619489212e-12,
-                    "angularVelocity": -6.851157050326969e-14,
-                    "velocityX": 3.761692904585482,
-                    "velocityY": 0.4003727432280311,
-                    "timestamp": 1.3495407527146646
-                },
-                {
-                    "x": 4.2663959076467215,
-                    "y": 6.559339756077265,
-                    "heading": 2.007295781849634e-12,
-                    "angularVelocity": -6.87242357042844e-14,
-                    "velocityX": 3.7616616311467697,
-                    "velocityY": 0.40066646375103865,
-                    "timestamp": 1.3771123667088332
-                },
-                {
-                    "x": 4.370110729285335,
-                    "y": 6.570389225581033,
-                    "heading": 2.0053851186045697e-12,
-                    "angularVelocity": -6.929821753536312e-14,
-                    "velocityX": 3.761652170976742,
-                    "velocityY": 0.40075526616572615,
-                    "timestamp": 1.4046839807030018
-                },
-                {
-                    "x": 4.473826107172322,
-                    "y": 6.581433472559441,
-                    "heading": 2.003480405786031e-12,
-                    "angularVelocity": -6.908240040814547e-14,
-                    "velocityX": 3.7616723456572294,
-                    "velocityY": 0.4005658493780133,
-                    "timestamp": 1.4322555946971705
-                },
-                {
-                    "x": 4.577543170052599,
-                    "y": 6.592461884602437,
-                    "heading": 2.001567952942652e-12,
-                    "angularVelocity": -6.936312480979614e-14,
-                    "velocityX": 3.7617334589969245,
-                    "velocityY": 0.399991529116124,
-                    "timestamp": 1.459827208691339
-                },
-                {
-                    "x": 4.681263099195036,
-                    "y": 6.603463307611616,
-                    "heading": 1.9996669629945417e-12,
-                    "angularVelocity": -6.894737463787157e-14,
-                    "velocityX": 3.761837415995553,
-                    "velocityY": 0.3990126587978969,
-                    "timestamp": 1.4873988226855077
-                },
-                {
-                    "x": 4.784986899417181,
-                    "y": 6.61442817334549,
-                    "heading": 1.997767883198675e-12,
-                    "angularVelocity": -6.88780949705506e-14,
-                    "velocityX": 3.7619778169075735,
-                    "velocityY": 0.3976867561604882,
-                    "timestamp": 1.5149704366796763
-                },
-                {
-                    "x": 4.888715211119033,
-                    "y": 6.625350277801733,
-                    "heading": 1.9958591963322718e-12,
-                    "angularVelocity": -6.922653588980761e-14,
-                    "velocityX": 3.762141444605259,
-                    "velocityY": 0.39613583941828256,
-                    "timestamp": 1.542542050673845
-                },
-                {
-                    "x": 4.992447807319009,
-                    "y": 6.636231614926857,
-                    "heading": 1.9939696519007763e-12,
-                    "angularVelocity": -6.853225532454514e-14,
-                    "velocityX": 3.7622968398643226,
-                    "velocityY": 0.3946572415062904,
-                    "timestamp": 1.5701136646680136
-                },
-                {
-                    "x": 5.09618354900695,
-                    "y": 6.6470829236468765,
-                    "heading": 1.992073077512808e-12,
-                    "angularVelocity": -6.878722613480481e-14,
-                    "velocityX": 3.7624109241486314,
-                    "velocityY": 0.3935681357313447,
-                    "timestamp": 1.5976852786621822
-                },
-                {
-                    "x": 5.199926346512542,
-                    "y": 6.657866567184633,
-                    "heading": 1.990167650654994e-12,
-                    "angularVelocity": -6.910829798877678e-14,
-                    "velocityX": 3.7626668329101425,
-                    "velocityY": 0.3911139746618113,
-                    "timestamp": 1.6252568926563509
-                },
-                {
-                    "x": 5.303729435372611,
-                    "y": 6.668053506977846,
-                    "heading": 1.9882412477007472e-12,
-                    "angularVelocity": -6.986908358991098e-14,
-                    "velocityX": 3.764853551267859,
-                    "velocityY": 0.3694720154839007,
-                    "timestamp": 1.6528285066505195
-                },
-                {
-                    "x": 5.40792279202557,
-                    "y": 6.672807389271601,
-                    "heading": 1.9863066963577084e-12,
-                    "angularVelocity": -7.016461895491627e-14,
-                    "velocityX": 3.779008246489486,
-                    "velocityY": 0.17241944175311613,
-                    "timestamp": 1.6804001206446881
-                },
-                {
-                    "x": 5.512213692786393,
-                    "y": 6.671303096171843,
-                    "heading": 1.984343863250107e-12,
-                    "angularVelocity": -7.119037640734205e-14,
-                    "velocityX": 3.782546091892586,
-                    "velocityY": -0.05455948646048947,
-                    "timestamp": 1.7079717346388568
-                },
-                {
-                    "x": 5.616221821617454,
-                    "y": 6.663482363169505,
-                    "heading": 1.9823084185583715e-12,
-                    "angularVelocity": -7.382393974706967e-14,
-                    "velocityX": 3.772290184136858,
-                    "velocityY": -0.2836516209983201,
-                    "timestamp": 1.7355433486330254
-                },
-                {
-                    "x": 5.7195634841918945,
-                    "y": 6.649363040924072,
-                    "heading": 1.9801191994112705e-12,
-                    "angularVelocity": -7.94012151883752e-14,
-                    "velocityX": 3.7481179954212767,
-                    "velocityY": -0.5120963266444406,
-                    "timestamp": 1.763114962627194
-                },
-                {
-                    "x": 5.834816669509981,
-                    "y": 6.625475968242897,
-                    "heading": 1.9780897732255602e-12,
-                    "angularVelocity": -6.522540784880574e-14,
-                    "velocityX": 3.704217712719512,
-                    "velocityY": -0.7677264405820726,
-                    "timestamp": 1.794229004580604
-                },
-                {
-                    "x": 5.948163245427426,
-                    "y": 6.593751483162918,
-                    "heading": 1.9761554326166073e-12,
-                    "angularVelocity": -6.216937380396162e-14,
-                    "velocityX": 3.6429396118708874,
-                    "velocityY": -1.0196195379349016,
-                    "timestamp": 1.825343046534014
-                },
-                {
-                    "x": 6.0590772896693,
-                    "y": 6.554356506595256,
-                    "heading": 1.974328708451782e-12,
-                    "angularVelocity": -5.871059954922337e-14,
-                    "velocityX": 3.564758458833813,
-                    "velocityY": -1.2661478256826604,
-                    "timestamp": 1.8564570884874239
-                },
-                {
-                    "x": 6.167114375070259,
-                    "y": 6.507645933449736,
-                    "heading": 1.9725156971860347e-12,
-                    "angularVelocity": -5.826986956344564e-14,
-                    "velocityX": 3.472293492526525,
-                    "velocityY": -1.5012698515774907,
-                    "timestamp": 1.8875711304408338
-                },
-                {
-                    "x": 6.273666318458446,
-                    "y": 6.457640205385319,
-                    "heading": 1.9706978622350046e-12,
-                    "angularVelocity": -5.842490200021065e-14,
-                    "velocityX": 3.424561281617029,
-                    "velocityY": -1.6071755684828801,
-                    "timestamp": 1.9186851723942437
-                },
-                {
-                    "x": 6.3802136872176485,
-                    "y": 6.4076247308783065,
-                    "heading": 1.9688925756308956e-12,
-                    "angularVelocity": -5.802160020867771e-14,
-                    "velocityX": 3.4244142538170945,
-                    "velocityY": -1.6074888174954127,
-                    "timestamp": 1.9497992143476537
-                },
-                {
-                    "x": 6.486760876374776,
-                    "y": 6.3576088737324294,
-                    "heading": 1.967091079960254e-12,
-                    "angularVelocity": -5.789976010142433e-14,
-                    "velocityX": 3.4244084814345546,
-                    "velocityY": -1.607501115448455,
-                    "timestamp": 1.9809132563010636
-                },
-                {
-                    "x": 6.593299366426369,
-                    "y": 6.307574489358799,
-                    "heading": 1.9652854794360616e-12,
-                    "angularVelocity": -5.803168954665514e-14,
-                    "velocityX": 3.4241288936675214,
-                    "velocityY": -1.6080965773861522,
-                    "timestamp": 2.0120272982544734
-                },
-                {
-                    "x": 6.6998332452145375,
-                    "y": 6.257530287401571,
-                    "heading": 1.963481306814657e-12,
-                    "angularVelocity": -5.79857968421928e-14,
-                    "velocityX": 3.423980688450087,
-                    "velocityY": -1.608412112843002,
-                    "timestamp": 2.043141340207883
-                },
-                {
-                    "x": 6.806367665236628,
-                    "y": 6.207487237643806,
-                    "heading": 1.9616709532128727e-12,
-                    "angularVelocity": -5.818445266643988e-14,
-                    "velocityX": 3.4239980836191717,
-                    "velocityY": -1.6083750813421427,
-                    "timestamp": 2.074255382161293
-                },
-                {
-                    "x": 6.912905936531216,
-                    "y": 6.157452377320687,
-                    "heading": 1.9503301873398137e-12,
-                    "angularVelocity": -3.6449015032516537e-13,
-                    "velocityX": 3.424121862863347,
-                    "velocityY": -1.6081118743030238,
-                    "timestamp": 2.1053694241147025
-                },
-                {
-                    "x": 7.015182351606042,
-                    "y": 6.109386281530938,
-                    "heading": 7.52788173701755e-9,
-                    "angularVelocity": 2.418821392510085e-7,
-                    "velocityX": 3.287146531073864,
-                    "velocityY": -1.5448361181009935,
-                    "timestamp": 2.1364834660681122
-                },
-                {
-                    "x": 7.110154929626141,
-                    "y": 6.064756817323096,
-                    "heading": 9.121631465411093e-9,
-                    "angularVelocity": 5.122284110672045e-8,
-                    "velocityX": 3.0524024542461334,
-                    "velocityY": -1.4343833653841132,
-                    "timestamp": 2.167597508021522
-                },
-                {
-                    "x": 7.19782257145347,
-                    "y": 6.02356173384038,
-                    "heading": 9.19440476065517e-9,
-                    "angularVelocity": 2.3389208664172086e-9,
-                    "velocityX": 2.8176230513092126,
-                    "velocityY": -1.3240029548204029,
-                    "timestamp": 2.1987115499749317
-                },
-                {
-                    "x": 7.278184934221974,
-                    "y": 5.985800317976241,
-                    "heading": 8.586659012919787e-9,
-                    "angularVelocity": -1.9532843759144805e-8,
-                    "velocityX": 2.582832628715854,
-                    "velocityY": -1.2136454633793592,
-                    "timestamp": 2.2298255919283414
-                },
-                {
-                    "x": 7.351241854096833,
-                    "y": 5.951472228972044,
-                    "heading": 7.633275836953741e-9,
-                    "angularVelocity": -3.064157105146478e-8,
-                    "velocityX": 2.3480369404994192,
-                    "velocityY": -1.1032989238626065,
-                    "timestamp": 2.260939633881751
-                },
-                {
-                    "x": 7.416993235519351,
-                    "y": 5.9205772680990165,
-                    "heading": 6.513007078345892e-9,
-                    "angularVelocity": -3.600524486341296e-8,
-                    "velocityX": 2.1132381810427594,
-                    "velocityY": -0.9929587714545899,
-                    "timestamp": 2.292053675835161
-                },
-                {
-                    "x": 7.47543901596506,
-                    "y": 5.89311530534189,
-                    "heading": 5.336882897741431e-9,
-                    "angularVelocity": -3.780042846669445e-8,
-                    "velocityX": 1.8784374120603946,
-                    "velocityY": -0.8826227977151127,
-                    "timestamp": 2.3231677177885706
-                },
-                {
-                    "x": 7.52657915135954,
-                    "y": 5.869086249058835,
-                    "heading": 4.180544199369271e-9,
-                    "angularVelocity": -3.716452645721565e-8,
-                    "velocityX": 1.643635226533715,
-                    "velocityY": -0.7722897693284123,
-                    "timestamp": 2.3542817597419803
-                },
-                {
-                    "x": 7.570413608969555,
-                    "y": 5.848490031193872,
-                    "heading": 3.0988314877379083e-9,
-                    "angularVelocity": -3.476606009342686e-8,
-                    "velocityX": 1.4088319889664873,
-                    "velocityY": -0.6619589282491846,
-                    "timestamp": 2.38539580169539
-                },
-                {
-                    "x": 7.606942363528039,
-                    "y": 5.831326599216932,
-                    "heading": 2.1333266690310216e-9,
-                    "angularVelocity": -3.103115843804935e-8,
-                    "velocityX": 1.1740279393200113,
-                    "velocityY": -0.5516297754774387,
-                    "timestamp": 2.4165098436487997
-                },
-                {
-                    "x": 7.636165394942908,
-                    "y": 5.817595911358632,
-                    "heading": 1.3166428525752622e-9,
-                    "angularVelocity": -2.6248077074736502e-8,
-                    "velocityX": 0.9392232439174092,
-                    "velocityY": -0.4413019651665061,
-                    "timestamp": 2.4476238856022094
-                },
-                {
-                    "x": 7.658082686855903,
-                    "y": 5.80729793361315,
-                    "heading": 6.750439688094791e-10,
-                    "angularVelocity": -2.0620877517571588e-8,
-                    "velocityX": 0.7044180227631803,
-                    "velocityY": -0.3309752477968783,
-                    "timestamp": 2.478737927555619
-                },
-                {
-                    "x": 7.672694225690866,
-                    "y": 5.800432637759072,
-                    "heading": 2.301347828245949e-10,
-                    "angularVelocity": -1.4299304537044113e-8,
-                    "velocityX": 0.46961236527350575,
-                    "velocityY": -0.22064943745862983,
-                    "timestamp": 2.509851969509029
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": -5.36340746801057e-34,
-                    "angularVelocity": -7.3964922450186615e-9,
-                    "velocityX": 0.23480633985361782,
-                    "velocityY": -0.1103243919326242,
-                    "timestamp": 2.5409660114624386
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": -7.670487347269043e-34,
-                    "angularVelocity": -7.414948879954709e-33,
-                    "velocityX": 1.169026940913066e-31,
-                    "velocityY": 1.7301899688761208e-31,
-                    "timestamp": 2.5720800534158483
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "Stage Side To C3": {
-            "waypoints": [
-                {
-                    "x": 0.71,
-                    "y": 4.36,
-                    "heading": -1.04,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 18
-                },
-                {
-                    "x": 2.797205924987793,
-                    "y": 6.204219341278076,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 9
-                },
-                {
-                    "x": 4.192164897918701,
-                    "y": 5.566523551940918,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 15
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 0.71,
-                    "y": 4.36,
-                    "heading": -1.04,
-                    "angularVelocity": 5.350140627575144e-31,
-                    "velocityX": 5.576066956455863e-32,
-                    "velocityY": -7.476130633471959e-32,
-                    "timestamp": 0
-                },
-                {
-                    "x": 0.7244661136787854,
-                    "y": 4.377063116032538,
-                    "heading": -0.9951964797725402,
-                    "angularVelocity": 0.8134823535576418,
-                    "velocityX": 0.2626563301835885,
-                    "velocityY": 0.30980922299649477,
-                    "timestamp": 0.055076204212072884
-                },
-                {
-                    "x": 0.7533392249389502,
-                    "y": 4.41173941127898,
-                    "heading": -0.9083722132270291,
-                    "angularVelocity": 1.5764388230385076,
-                    "velocityX": 0.524239309393705,
-                    "velocityY": 0.6296057570147754,
-                    "timestamp": 0.11015240842414577
-                },
-                {
-                    "x": 0.7966290780145974,
-                    "y": 4.464680613281826,
-                    "heading": -0.783928161336893,
-                    "angularVelocity": 2.2594885335771653,
-                    "velocityX": 0.7859992113646113,
-                    "velocityY": 0.9612354874529038,
-                    "timestamp": 0.16522861263621866
-                },
-                {
-                    "x": 0.8545700129369538,
-                    "y": 4.536623492592967,
-                    "heading": -0.6296494882874061,
-                    "angularVelocity": 2.8011856527989654,
-                    "velocityX": 1.0520139459729396,
-                    "velocityY": 1.3062425114505738,
-                    "timestamp": 0.22030481684829153
-                },
-                {
-                    "x": 0.9276258124093708,
-                    "y": 4.628245682798297,
-                    "heading": -0.4576364570724038,
-                    "angularVelocity": 3.1231823920301154,
-                    "velocityX": 1.3264494261643394,
-                    "velocityY": 1.663553099130404,
-                    "timestamp": 0.27538102106036444
-                },
-                {
-                    "x": 1.0160400191585532,
-                    "y": 4.739959244546144,
-                    "heading": -0.28714155882633985,
-                    "angularVelocity": 3.095618165506983,
-                    "velocityX": 1.605306829220418,
-                    "velocityY": 2.0283453325449616,
-                    "timestamp": 0.3304572252724373
-                },
-                {
-                    "x": 1.114934453543302,
-                    "y": 4.869596690219766,
-                    "heading": -0.17212037106177128,
-                    "angularVelocity": 2.0884007787042775,
-                    "velocityX": 1.795592775492439,
-                    "velocityY": 2.3537832268624075,
-                    "timestamp": 0.3855334294845102
-                },
-                {
-                    "x": 1.2246983786188343,
-                    "y": 5.017091614146892,
-                    "heading": -0.11040110898627345,
-                    "angularVelocity": 1.1206157533631977,
-                    "velocityX": 1.9929464393166578,
-                    "velocityY": 2.678015415862639,
-                    "timestamp": 0.44060963369658307
-                },
-                {
-                    "x": 1.345743830891035,
-                    "y": 5.182217862842374,
-                    "heading": -0.10127955705630985,
-                    "angularVelocity": 0.16561693131283492,
-                    "velocityX": 2.197781310529416,
-                    "velocityY": 2.998141412571981,
-                    "timestamp": 0.49568583790865595
-                },
-                {
-                    "x": 1.4740265309381277,
-                    "y": 5.346392338052181,
-                    "heading": -0.10127945031307138,
-                    "angularVelocity": 0.0000019381008548903397,
-                    "velocityX": 2.3291855690186902,
-                    "velocityY": 2.9808603835088525,
-                    "timestamp": 0.5507620421207289
-                },
-                {
-                    "x": 1.6022800251300493,
-                    "y": 5.510589629967512,
-                    "heading": -0.10127934358262022,
-                    "angularVelocity": 0.0000019378686800961446,
-                    "velocityX": 2.3286552881910736,
-                    "velocityY": 2.9812746587087395,
-                    "timestamp": 0.6058382463328018
-                },
-                {
-                    "x": 1.7347437736631814,
-                    "y": 5.671409606244062,
-                    "heading": -0.10127923743109664,
-                    "angularVelocity": 0.0000019273572882478923,
-                    "velocityX": 2.4050994513545807,
-                    "velocityY": 2.9199538816671686,
-                    "timestamp": 0.6609144505448747
-                },
-                {
-                    "x": 1.8857033856482435,
-                    "y": 5.815009412217227,
-                    "heading": -0.10127912902413344,
-                    "angularVelocity": 0.0000019683085417376046,
-                    "velocityX": 2.7409225843485396,
-                    "velocityY": 2.607293077428296,
-                    "timestamp": 0.7159906547569477
-                },
-                {
-                    "x": 2.0529507817587227,
-                    "y": 5.93925921930888,
-                    "heading": -0.1012790059378887,
-                    "angularVelocity": 0.0000022348352886277144,
-                    "velocityX": 3.036654368309179,
-                    "velocityY": 2.2559616965109663,
-                    "timestamp": 0.7710668589690206
-                },
-                {
-                    "x": 2.234021022235051,
-                    "y": 6.042327659180149,
-                    "heading": -0.10127786160272408,
-                    "angularVelocity": 0.0000207773062979661,
-                    "velocityX": 3.287631075284561,
-                    "velocityY": 1.8713787804693067,
-                    "timestamp": 0.8261430631810935
-                },
-                {
-                    "x": 2.4203468055894013,
-                    "y": 6.121035990183916,
-                    "heading": -0.0857184634932982,
-                    "angularVelocity": 0.28250672558184264,
-                    "velocityX": 3.3830541886455214,
-                    "velocityY": 1.4290805281478167,
-                    "timestamp": 0.8812192673931665
-                },
-                {
-                    "x": 2.608481388818861,
-                    "y": 6.1750393209771115,
-                    "heading": -0.05159654365961516,
-                    "angularVelocity": 0.6195401502670522,
-                    "velocityX": 3.4158959558113233,
-                    "velocityY": 0.9805202004345134,
-                    "timestamp": 0.9362954716052394
-                },
-                {
-                    "x": 2.797205924987793,
-                    "y": 6.204219341278076,
-                    "heading": -3.617531000727075e-31,
-                    "angularVelocity": 0.9368209809983128,
-                    "velocityX": 3.4266075316708653,
-                    "velocityY": 0.5298117529778321,
-                    "timestamp": 0.9913716758173123
-                },
-                {
-                    "x": 2.966124692846499,
-                    "y": 6.210295134186947,
-                    "heading": 0.05716064392929573,
-                    "angularVelocity": 1.15887425245862,
-                    "velocityX": 3.4246572006891465,
-                    "velocityY": 0.12318055713425537,
-                    "timestamp": 1.0406959608361792
-                },
-                {
-                    "x": 3.13289635801508,
-                    "y": 6.196205704844962,
-                    "heading": 0.11248864097572012,
-                    "angularVelocity": 1.12171919015676,
-                    "velocityX": 3.3811268648859167,
-                    "velocityY": -0.2856489320949118,
-                    "timestamp": 1.090020245855046
-                },
-                {
-                    "x": 3.2974428000278966,
-                    "y": 6.1620266805098955,
-                    "heading": 0.16165741156418048,
-                    "angularVelocity": 0.9968471021861408,
-                    "velocityX": 3.336012715640518,
-                    "velocityY": -0.6929451551500881,
-                    "timestamp": 1.139344530873913
-                },
-                {
-                    "x": 3.45979854217056,
-                    "y": 6.107879810654357,
-                    "heading": 0.20068182384964747,
-                    "angularVelocity": 0.7911804959877227,
-                    "velocityX": 3.2915984910995078,
-                    "velocityY": -1.0977730307662261,
-                    "timestamp": 1.1886688158927798
-                },
-                {
-                    "x": 3.619952741472391,
-                    "y": 6.03393942235358,
-                    "heading": 0.22581697371114728,
-                    "angularVelocity": 0.509589745738515,
-                    "velocityX": 3.2469644362928305,
-                    "velocityY": -1.4990666012186962,
-                    "timestamp": 1.2379931009116467
-                },
-                {
-                    "x": 3.7772983820923596,
-                    "y": 5.940432276289927,
-                    "heading": 0.23452421295694514,
-                    "angularVelocity": 0.17653047058793564,
-                    "velocityX": 3.190023749148815,
-                    "velocityY": -1.8957628281461119,
-                    "timestamp": 1.2873173859305136
-                },
-                {
-                    "x": 3.926570083103172,
-                    "y": 5.828476756706829,
-                    "heading": 0.23452463928791645,
-                    "angularVelocity": 0.000008643429319627254,
-                    "velocityX": 3.026332788274898,
-                    "velocityY": -2.2697849454943535,
-                    "timestamp": 1.3366416709493805
-                },
-                {
-                    "x": 4.062952838723869,
-                    "y": 5.701135334142216,
-                    "heading": 0.23452497322201252,
-                    "angularVelocity": 0.000006770176109915362,
-                    "velocityX": 2.7650224543249706,
-                    "velocityY": -2.5817185695831255,
-                    "timestamp": 1.3859659559682473
-                },
-                {
-                    "x": 4.192164897918701,
-                    "y": 5.566523551940918,
-                    "heading": 0.23452535083185597,
-                    "angularVelocity": 0.000007655657721212718,
-                    "velocityX": 2.6196438355955265,
-                    "velocityY": -2.7291177591283535,
-                    "timestamp": 1.4352902409871142
-                },
-                {
-                    "x": 4.379227990202405,
-                    "y": 5.2613971819703,
-                    "heading": 0.26255662300085747,
-                    "angularVelocity": 0.28814889358818807,
-                    "velocityX": 1.922924608906585,
-                    "velocityY": -3.136562100411476,
-                    "timestamp": 1.5325707539189606
-                },
-                {
-                    "x": 4.5172116067735955,
-                    "y": 4.993723531483578,
-                    "heading": 0.47596391395635385,
-                    "angularVelocity": 2.193731144335268,
-                    "velocityX": 1.4184096322339512,
-                    "velocityY": -2.751564958073857,
-                    "timestamp": 1.629851266850807
-                },
-                {
-                    "x": 4.704361215089976,
-                    "y": 4.7238802473185055,
-                    "heading": 0.606383868813914,
-                    "angularVelocity": 1.3406585854344948,
-                    "velocityX": 1.9238139548821627,
-                    "velocityY": -2.773867818255899,
-                    "timestamp": 1.7271317797826533
-                },
-                {
-                    "x": 4.9212614384999,
-                    "y": 4.498429708685273,
-                    "heading": 0.7825723935837035,
-                    "angularVelocity": 1.8111389368724442,
-                    "velocityX": 2.229636921855878,
-                    "velocityY": -2.3175303237882856,
-                    "timestamp": 1.8244122927144997
-                },
-                {
-                    "x": 5.185303406160076,
-                    "y": 4.293106225066136,
-                    "heading": 0.8921386359376694,
-                    "angularVelocity": 1.1262917829260124,
-                    "velocityX": 2.71423288901818,
-                    "velocityY": -2.110633234047443,
-                    "timestamp": 1.921692805646346
-                },
-                {
-                    "x": 5.506747569472596,
-                    "y": 4.137612897565104,
-                    "heading": 0.9241000729554552,
-                    "angularVelocity": 0.328549223832528,
-                    "velocityX": 3.304301690285293,
-                    "velocityY": -1.5984015998144245,
-                    "timestamp": 2.0189733185781926
-                },
-                {
-                    "x": 5.864556115895607,
-                    "y": 4.051579577380226,
-                    "heading": 0.924099942108789,
-                    "angularVelocity": -0.0000013450449852620257,
-                    "velocityX": 3.6781112233000655,
-                    "velocityY": -0.8843839078557418,
-                    "timestamp": 2.116253831510039
-                },
-                {
-                    "x": 6.232480317048254,
-                    "y": 4.043802644882202,
-                    "heading": 0.9240998495204155,
-                    "angularVelocity": -9.517669138022849e-7,
-                    "velocityX": 3.7820956126167884,
-                    "velocityY": -0.07994337471752075,
-                    "timestamp": 2.2135343444418853
-                },
-                {
-                    "x": 6.59997941709367,
-                    "y": 4.063118723469172,
-                    "heading": 0.9240997607721795,
-                    "angularVelocity": -9.122920231628579e-7,
-                    "velocityX": 3.777725764078584,
-                    "velocityY": 0.198560616148301,
-                    "timestamp": 2.3108148573737317
-                },
-                {
-                    "x": 6.956026800754634,
-                    "y": 4.090327871531077,
-                    "heading": 0.8973557490580134,
-                    "angularVelocity": -0.27491643401286797,
-                    "velocityX": 3.660007260759475,
-                    "velocityY": 0.2796978268501417,
-                    "timestamp": 2.408095370305578
-                },
-                {
-                    "x": 7.247734347026976,
-                    "y": 4.106615231962139,
-                    "heading": 0.7104712895715125,
-                    "angularVelocity": -1.9210883439464366,
-                    "velocityX": 2.9986226170159034,
-                    "velocityY": 0.1674267532128742,
-                    "timestamp": 2.5053758832374244
-                },
-                {
-                    "x": 7.467840517564824,
-                    "y": 4.117242335862297,
-                    "heading": 0.4016498999018689,
-                    "angularVelocity": -3.1745452440819273,
-                    "velocityX": 2.262592619058774,
-                    "velocityY": 0.1092418571806094,
-                    "timestamp": 2.602656396169271
-                },
-                {
-                    "x": 7.6104608023102855,
-                    "y": 4.123883769495069,
-                    "heading": 0.14488296734927883,
-                    "angularVelocity": -2.639448794153438,
-                    "velocityX": 1.4660724994879464,
-                    "velocityY": 0.06827095615156697,
-                    "timestamp": 2.699936909101117
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 9.165039287514682e-34,
-                    "angularVelocity": -1.4893318608505086,
-                    "velocityX": 0.7148317334473038,
-                    "velocityY": 0.032033450595745375,
-                    "timestamp": 2.7972174220329635
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "angularVelocity": 3.6915361070859757e-32,
-                    "velocityX": 0,
-                    "velocityY": 3.3618150907593784e-32,
-                    "timestamp": 2.89449793496481
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "Stage Side To C4": {
-            "waypoints": [
-                {
-                    "x": 0.71,
-                    "y": 4.36,
-                    "heading": -1.04,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 33
-                },
-                {
-                    "x": 2.4943342208862305,
-                    "y": 2.6289942264556885,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 13
-                },
-                {
-                    "x": 5.236664772033691,
-                    "y": 1.4238771200180054,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 28
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 0.71,
-                    "y": 4.36,
-                    "heading": -1.04,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 2.687708804701003e-35,
-                    "timestamp": 0
-                },
-                {
-                    "x": 0.7140616140612069,
-                    "y": 4.355992841184312,
-                    "heading": -1.0347167301054676,
-                    "angularVelocity": 0.1988304134727778,
-                    "velocityX": 0.15285465616518656,
-                    "velocityY": -0.15080528916361516,
-                    "timestamp": 0.026571739213607736
-                },
-                {
-                    "x": 0.7221925258884458,
-                    "y": 4.3479754290630535,
-                    "heading": -1.0242624006876555,
-                    "angularVelocity": 0.39343790535390527,
-                    "velocityX": 0.30599848063671325,
-                    "velocityY": -0.3017270362623706,
-                    "timestamp": 0.05314347842721547
-                },
-                {
-                    "x": 0.7344011981721603,
-                    "y": 4.335944497876898,
-                    "heading": -1.0087613580778494,
-                    "angularVelocity": 0.5833657513042195,
-                    "velocityX": 0.45946078973491633,
-                    "velocityY": -0.4527716868451825,
-                    "timestamp": 0.0797152176408232
-                },
-                {
-                    "x": 0.7506970627866689,
-                    "y": 4.31989642008046,
-                    "heading": -0.9883558514269565,
-                    "angularVelocity": 0.7679401971717026,
-                    "velocityX": 0.6132780576953482,
-                    "velocityY": -0.6039528563572185,
-                    "timestamp": 0.10628695685443094
-                },
-                {
-                    "x": 0.7710907256657638,
-                    "y": 4.29982697901967,
-                    "heading": -0.963213241169254,
-                    "angularVelocity": 0.9462162057057438,
-                    "velocityX": 0.7674944690353961,
-                    "velocityY": -0.7552927152962536,
-                    "timestamp": 0.13285869606803868
-                },
-                {
-                    "x": 0.7955941760833964,
-                    "y": 4.27573110299105,
-                    "heading": -0.9335350310204893,
-                    "angularVelocity": 1.1169088297226009,
-                    "velocityX": 0.9221620843352277,
-                    "velocityY": -0.9068234425649082,
-                    "timestamp": 0.1594304352816464
-                },
-                {
-                    "x": 0.8242209914920048,
-                    "y": 4.247602572989807,
-                    "heading": -0.8995682913456332,
-                    "angularVelocity": 1.2783032153748224,
-                    "velocityX": 1.0773406730541775,
-                    "velocityY": -1.0585882156647894,
-                    "timestamp": 0.18600217449525414
-                },
-                {
-                    "x": 0.856986533327333,
-                    "y": 4.215433742997202,
-                    "heading": -0.8616204330063739,
-                    "angularVelocity": 1.4281285103018635,
-                    "velocityX": 1.2330973735640383,
-                    "velocityY": -1.210640738794065,
-                    "timestamp": 0.21257391370886186
-                },
-                {
-                    "x": 0.893908134971617,
-                    "y": 4.179215360896215,
-                    "heading": -0.8200791656995261,
-                    "angularVelocity": 1.5633627506615773,
-                    "velocityX": 1.3895063980372095,
-                    "velocityY": -1.3630414558050108,
-                    "timestamp": 0.2391456529224696
-                },
-                {
-                    "x": 0.9350052770321623,
-                    "y": 4.138936674970777,
-                    "heading": -0.7754414952973147,
-                    "angularVelocity": 1.6798926876172218,
-                    "velocityX": 1.546648555074586,
-                    "velocityY": -1.5158468025612346,
-                    "timestamp": 0.2657173921360773
-                },
-                {
-                    "x": 0.9802996637151041,
-                    "y": 4.094586228803994,
-                    "heading": -0.7283605055619151,
-                    "angularVelocity": 1.7718444907546276,
-                    "velocityX": 1.7046075275248067,
-                    "velocityY": -1.669083299751435,
-                    "timestamp": 0.29228913134968504
-                },
-                {
-                    "x": 1.0298146765069804,
-                    "y": 4.046154350498487,
-                    "heading": -0.6797316150493184,
-                    "angularVelocity": 1.8300981400454652,
-                    "velocityX": 1.8634464381059026,
-                    "velocityY": -1.8226837888242364,
-                    "timestamp": 0.31886087056329276
-                },
-                {
-                    "x": 1.0835715343165668,
-                    "y": 3.9936405744754793,
-                    "heading": -0.6308802721211105,
-                    "angularVelocity": 1.8384699072761677,
-                    "velocityX": 2.0230839004342207,
-                    "velocityY": -1.9763017994740086,
-                    "timestamp": 0.3454326097769005
-                },
-                {
-                    "x": 1.1415677463510772,
-                    "y": 3.937082152581159,
-                    "heading": -0.5840793547040092,
-                    "angularVelocity": 1.7613042579137594,
-                    "velocityX": 2.1826276243449603,
-                    "velocityY": -2.12851787531304,
-                    "timestamp": 0.3720043489905082
-                },
-                {
-                    "x": 1.203636670620202,
-                    "y": 3.876777756204273,
-                    "heading": -0.5447644235662668,
-                    "angularVelocity": 1.4795768851144226,
-                    "velocityX": 2.33589994882,
-                    "velocityY": -2.2694937614773476,
-                    "timestamp": 0.39857608820411594
-                },
-                {
-                    "x": 1.2693974629594884,
-                    "y": 3.8133907541919347,
-                    "heading": -0.5190150912676422,
-                    "angularVelocity": 0.969049563960718,
-                    "velocityX": 2.4748395959572558,
-                    "velocityY": -2.3855044452595306,
-                    "timestamp": 0.42514782741772367
-                },
-                {
-                    "x": 1.3388335264794184,
-                    "y": 3.7468848835898942,
-                    "heading": -0.5067181615747146,
-                    "angularVelocity": 0.4627822663045784,
-                    "velocityX": 2.613154636275026,
-                    "velocityY": -2.5028798479243823,
-                    "timestamp": 0.4517195666313314
-                },
-                {
-                    "x": 1.410439403600934,
-                    "y": 3.676338986398213,
-                    "heading": -0.5067181155488927,
-                    "angularVelocity": 0.0000017321343363764108,
-                    "velocityX": 2.6948133332892708,
-                    "velocityY": -2.6549220818618244,
-                    "timestamp": 0.4782913058449391
-                },
-                {
-                    "x": 1.482040293717765,
-                    "y": 3.605788027611728,
-                    "heading": -0.5067180695571892,
-                    "angularVelocity": 0.0000017308503249172533,
-                    "velocityX": 2.694625652511417,
-                    "velocityY": -2.655112569761896,
-                    "timestamp": 0.5048630450585468
-                },
-                {
-                    "x": 1.5536411829944505,
-                    "y": 3.5352370679725946,
-                    "heading": -0.5067180235654862,
-                    "angularVelocity": 0.0000017308503100877725,
-                    "velocityX": 2.6946256208934063,
-                    "velocityY": -2.6551126018504427,
-                    "timestamp": 0.5314347842721546
-                },
-                {
-                    "x": 1.6252420722709946,
-                    "y": 3.4646861083333174,
-                    "heading": -0.5067179775737835,
-                    "angularVelocity": 0.0000017308502987237163,
-                    "velocityX": 2.6946256208880808,
-                    "velocityY": -2.6551126018558486,
-                    "timestamp": 0.5580065234857624
-                },
-                {
-                    "x": 1.6968429615475384,
-                    "y": 3.39413514869404,
-                    "heading": -0.506717931582081,
-                    "angularVelocity": 0.0000017308502871423956,
-                    "velocityX": 2.6946256208880808,
-                    "velocityY": -2.65511260185585,
-                    "timestamp": 0.5845782626993702
-                },
-                {
-                    "x": 1.7684438508240825,
-                    "y": 3.323584189054763,
-                    "heading": -0.5067178855903789,
-                    "angularVelocity": 0.0000017308502762001758,
-                    "velocityX": 2.694625620888081,
-                    "velocityY": -2.65511260185585,
-                    "timestamp": 0.611150001912978
-                },
-                {
-                    "x": 1.8400447401006266,
-                    "y": 3.2530332294154856,
-                    "heading": -0.506717839598677,
-                    "angularVelocity": 0.000001730850265567339,
-                    "velocityX": 2.6946256208880826,
-                    "velocityY": -2.6551126018558495,
-                    "timestamp": 0.6377217411265858
-                },
-                {
-                    "x": 1.9116456293771706,
-                    "y": 3.1824822697762087,
-                    "heading": -0.5067177936069754,
-                    "angularVelocity": 0.0000017308502535085818,
-                    "velocityX": 2.694625620888084,
-                    "velocityY": -2.65511260185585,
-                    "timestamp": 0.6642934803401935
-                },
-                {
-                    "x": 1.9832465186537145,
-                    "y": 3.1119313101369315,
-                    "heading": -0.5067177476152741,
-                    "angularVelocity": 0.0000017308502432657092,
-                    "velocityX": 2.6946256208880843,
-                    "velocityY": -2.65511260185585,
-                    "timestamp": 0.6908652195538013
-                },
-                {
-                    "x": 2.0548474079302586,
-                    "y": 3.041380350497654,
-                    "heading": -0.5067177016235731,
-                    "angularVelocity": 0.000001730850231529751,
-                    "velocityX": 2.694625620888085,
-                    "velocityY": -2.6551126018558504,
-                    "timestamp": 0.7174369587674091
-                },
-                {
-                    "x": 2.1264482972068026,
-                    "y": 2.970829390858377,
-                    "heading": -0.5067176556318724,
-                    "angularVelocity": 0.0000017308502204807436,
-                    "velocityX": 2.6946256208880865,
-                    "velocityY": -2.6551126018558495,
-                    "timestamp": 0.7440086979810169
-                },
-                {
-                    "x": 2.1980491864833867,
-                    "y": 2.9002784312191405,
-                    "heading": -0.5067176096401721,
-                    "angularVelocity": 0.0000017308502087562552,
-                    "velocityX": 2.694625620889591,
-                    "velocityY": -2.6551126018543236,
-                    "timestamp": 0.7705804371946247
-                },
-                {
-                    "x": 2.2696500759972245,
-                    "y": 2.829727471820689,
-                    "heading": -0.506717563648472,
-                    "angularVelocity": 0.0000017308501985869009,
-                    "velocityX": 2.6946256298184115,
-                    "velocityY": -2.6551125927926265,
-                    "timestamp": 0.7971521764082324
-                },
-                {
-                    "x": 2.3412523738653572,
-                    "y": 2.7591779417640674,
-                    "heading": -0.5067175176565489,
-                    "angularVelocity": 0.0000017308585893832803,
-                    "velocityX": 2.694678631779742,
-                    "velocityY": -2.6550588009870313,
-                    "timestamp": 0.8237239156218402
-                },
-                {
-                    "x": 2.4158866145337003,
-                    "y": 2.6918440324740667,
-                    "heading": -0.5067174710515008,
-                    "angularVelocity": 0.000001753932919872258,
-                    "velocityX": 2.808782672009746,
-                    "velocityY": -2.5340422299311864,
-                    "timestamp": 0.850295654835448
-                },
-                {
-                    "x": 2.4943342208862305,
-                    "y": 2.6289942264556885,
-                    "heading": -0.5067174224716422,
-                    "angularVelocity": 0.000001828252870098291,
-                    "velocityX": 2.952294756541798,
-                    "velocityY": -2.365287628074865,
-                    "timestamp": 0.8768673940490558
-                },
-                {
-                    "x": 2.693828949223235,
-                    "y": 2.509453130751911,
-                    "heading": -0.5067173754009504,
-                    "angularVelocity": 7.656472375459629e-7,
-                    "velocityX": 3.244961599231293,
-                    "velocityY": -1.9444436869204162,
-                    "timestamp": 0.9383456957905196
-                },
-                {
-                    "x": 2.900420237054028,
-                    "y": 2.4026436735913763,
-                    "heading": -0.5067173289743672,
-                    "angularVelocity": 7.551702291921459e-7,
-                    "velocityX": 3.3603935368868174,
-                    "velocityY": -1.7373521085488381,
-                    "timestamp": 0.9998239975319835
-                },
-                {
-                    "x": 3.1070116472391316,
-                    "y": 2.295834453089378,
-                    "heading": -0.5067172825477892,
-                    "angularVelocity": 7.55170142432061e-7,
-                    "velocityX": 3.3603955270899863,
-                    "velocityY": -1.7373482590844154,
-                    "timestamp": 1.0613022992734473
-                },
-                {
-                    "x": 3.3136030574258837,
-                    "y": 2.1890252325905677,
-                    "heading": -0.5067172361212112,
-                    "angularVelocity": 7.551701448333524e-7,
-                    "velocityX": 3.3603955271167996,
-                    "velocityY": -1.737348259032553,
-                    "timestamp": 1.1227806010149113
-                },
-                {
-                    "x": 3.5201944676126358,
-                    "y": 2.0822160120917577,
-                    "heading": -0.5067171896946332,
-                    "angularVelocity": 7.551701452492318e-7,
-                    "velocityX": 3.3603955271167996,
-                    "velocityY": -1.737348259032552,
-                    "timestamp": 1.1842589027563752
-                },
-                {
-                    "x": 3.726785877799388,
-                    "y": 1.9754067915929476,
-                    "heading": -0.5067171432680552,
-                    "angularVelocity": 7.5517014486464e-7,
-                    "velocityX": 3.3603955271168005,
-                    "velocityY": -1.7373482590325509,
-                    "timestamp": 1.2457372044978392
-                },
-                {
-                    "x": 3.93337728798614,
-                    "y": 1.8685975710941374,
-                    "heading": -0.506717096841477,
-                    "angularVelocity": 7.551701472516521e-7,
-                    "velocityX": 3.360395527116801,
-                    "velocityY": -1.7373482590325502,
-                    "timestamp": 1.3072155062393032
-                },
-                {
-                    "x": 4.139968698173285,
-                    "y": 1.761788350596088,
-                    "heading": -0.5067170504148988,
-                    "angularVelocity": 7.551701467640916e-7,
-                    "velocityX": 3.360395527123197,
-                    "velocityY": -1.7373482590201774,
-                    "timestamp": 1.3686938079807671
-                },
-                {
-                    "x": 4.346560137549085,
-                    "y": 1.6549791865550314,
-                    "heading": -0.5067170039883208,
-                    "angularVelocity": 7.55170145558408e-7,
-                    "velocityX": 3.36039600190298,
-                    "velocityY": -1.737347340696294,
-                    "timestamp": 1.430172109722231
-                },
-                {
-                    "x": 4.5552180589852185,
-                    "y": 1.552265545137979,
-                    "heading": -0.5067169574392034,
-                    "angularVelocity": 7.571633584219393e-7,
-                    "velocityX": 3.3940091955305984,
-                    "velocityY": -1.6707299731374539,
-                    "timestamp": 1.491650411463695
-                },
-                {
-                    "x": 4.775846094749007,
-                    "y": 1.478702717632789,
-                    "heading": -0.5067169090858323,
-                    "angularVelocity": 7.865111701229533e-7,
-                    "velocityX": 3.5887138960279197,
-                    "velocityY": -1.1965657056459655,
-                    "timestamp": 1.553128713205159
-                },
-                {
-                    "x": 5.004393851392972,
-                    "y": 1.4356429788646896,
-                    "heading": -0.5067168550876733,
-                    "angularVelocity": 8.783287361409535e-7,
-                    "velocityX": 3.717535295706156,
-                    "velocityY": -0.7004054690576753,
-                    "timestamp": 1.614607014946623
-                },
-                {
-                    "x": 5.236664772033691,
-                    "y": 1.4238771200180054,
-                    "heading": -0.5067167933216484,
-                    "angularVelocity": 0.000001004680076164895,
-                    "velocityX": 3.7780959145145716,
-                    "velocityY": -0.19138230096471237,
-                    "timestamp": 1.676085316688087
-                },
-                {
-                    "x": 5.366036550697627,
-                    "y": 1.4270871399969536,
-                    "heading": -0.5067167282581706,
-                    "angularVelocity": 0.000001901925327561791,
-                    "velocityX": 3.781775437099809,
-                    "velocityY": 0.09383479793163368,
-                    "timestamp": 1.7102945871261934
-                },
-                {
-                    "x": 5.494798697340742,
-                    "y": 1.4400359689986706,
-                    "heading": -0.5067166730622293,
-                    "angularVelocity": 0.0000016134790500794753,
-                    "velocityX": 3.76395476998197,
-                    "velocityY": 0.37851812786083194,
-                    "timestamp": 1.7445038575643
-                },
-                {
-                    "x": 5.6222191444596294,
-                    "y": 1.4626499710733982,
-                    "heading": -0.5067166242756445,
-                    "angularVelocity": 0.0000014261217576278581,
-                    "velocityX": 3.724734421022641,
-                    "velocityY": 0.6610489433161765,
-                    "timestamp": 1.7787131280024064
-                },
-                {
-                    "x": 5.747573461036382,
-                    "y": 1.4948005438980272,
-                    "heading": -0.5067165796372217,
-                    "angularVelocity": 0.0000013048633380814483,
-                    "velocityX": 3.6643376187618593,
-                    "velocityY": 0.9398204759378852,
-                    "timestamp": 1.812922398440513
-                },
-                {
-                    "x": 5.870148986660447,
-                    "y": 1.5363048197489622,
-                    "heading": -0.5067165375293654,
-                    "angularVelocity": 0.0000012308902156872765,
-                    "velocityX": 3.583108439738183,
-                    "velocityY": 1.2132464480944425,
-                    "timestamp": 1.8471316688786195
-                },
-                {
-                    "x": 5.9892489774115445,
-                    "y": 1.5869265226922225,
-                    "heading": -0.5067164966802475,
-                    "angularVelocity": 0.0000011940949728764683,
-                    "velocityX": 3.481512152285735,
-                    "velocityY": 1.4797656394002383,
-                    "timestamp": 1.881340939316726
-                },
-                {
-                    "x": 6.104355729464833,
-                    "y": 1.6460687267313168,
-                    "heading": -0.5067164560018657,
-                    "angularVelocity": 0.0000011891040447074693,
-                    "velocityX": 3.3647824282469623,
-                    "velocityY": 1.7288355840881788,
-                    "timestamp": 1.9155502097548325
-                },
-                {
-                    "x": 6.219456425834801,
-                    "y": 1.705222715286976,
-                    "heading": -0.5067164153233215,
-                    "angularVelocity": 0.0000011891087921450935,
-                    "velocityX": 3.3646054094668667,
-                    "velocityY": 1.7291800672184492,
-                    "timestamp": 1.949759480192939
-                },
-                {
-                    "x": 6.334557121741728,
-                    "y": 1.764376704743614,
-                    "heading": -0.5067163746447773,
-                    "angularVelocity": 0.0000011891087898511233,
-                    "velocityX": 3.3646053959312843,
-                    "velocityY": 1.7291800935557256,
-                    "timestamp": 1.9839687506310455
-                },
-                {
-                    "x": 6.449657817648618,
-                    "y": 1.8235306942003213,
-                    "heading": -0.5067163339662332,
-                    "angularVelocity": 0.000001189108787218646,
-                    "velocityX": 3.3646053959302495,
-                    "velocityY": 1.7291800935577397,
-                    "timestamp": 2.018178021069152
-                },
-                {
-                    "x": 6.564758513555509,
-                    "y": 1.8826846836570283,
-                    "heading": -0.5067162932876892,
-                    "angularVelocity": 0.0000011891087819705439,
-                    "velocityX": 3.3646053959302504,
-                    "velocityY": 1.7291800935577386,
-                    "timestamp": 2.0523872915072587
-                },
-                {
-                    "x": 6.679859209462396,
-                    "y": 1.9418386731137416,
-                    "heading": -0.5067162526091454,
-                    "angularVelocity": 0.0000011891087784797324,
-                    "velocityX": 3.3646053959301545,
-                    "velocityY": 1.7291800935579253,
-                    "timestamp": 2.0865965619453655
-                },
-                {
-                    "x": 6.7949599053264,
-                    "y": 2.0009926626538963,
-                    "heading": -0.5067162119306018,
-                    "angularVelocity": 0.0000011891087739781964,
-                    "velocityX": 3.3646053946765995,
-                    "velocityY": 1.7291800959970696,
-                    "timestamp": 2.120805832383472
-                },
-                {
-                    "x": 6.910060040297267,
-                    "y": 2.06014774355027,
-                    "heading": -0.5067161712446878,
-                    "angularVelocity": 0.0000011893242211155864,
-                    "velocityX": 3.3645889987368625,
-                    "velocityY": 1.7292119983499907,
-                    "timestamp": 2.155015102821579
-                },
-                {
-                    "x": 7.017868923227757,
-                    "y": 2.1174564154331827,
-                    "heading": -0.4884934235175169,
-                    "angularVelocity": 0.5326844885552473,
-                    "velocityX": 3.151452268634164,
-                    "velocityY": 1.6752380611740627,
-                    "timestamp": 2.1892243732596857
-                },
-                {
-                    "x": 7.119036711180674,
-                    "y": 2.1700871649460596,
-                    "heading": -0.45052101262699384,
-                    "angularVelocity": 1.1100035284068692,
-                    "velocityX": 2.9573208272872957,
-                    "velocityY": 1.5384937719762113,
-                    "timestamp": 2.2234336436977924
-                },
-                {
-                    "x": 7.212945348428852,
-                    "y": 2.2182419084801723,
-                    "heading": -0.3958429177265348,
-                    "angularVelocity": 1.5983414495607555,
-                    "velocityX": 2.745122478366898,
-                    "velocityY": 1.4076518708938115,
-                    "timestamp": 2.257642914135899
-                },
-                {
-                    "x": 7.29827375889861,
-                    "y": 2.2619654936466005,
-                    "heading": -0.3361817714679226,
-                    "angularVelocity": 1.7440052212324946,
-                    "velocityX": 2.494306641941948,
-                    "velocityY": 1.2781209481077944,
-                    "timestamp": 2.291852184574006
-                },
-                {
-                    "x": 7.374929402835313,
-                    "y": 2.3012175879094494,
-                    "heading": -0.2767819040095215,
-                    "angularVelocity": 1.7363675605380382,
-                    "velocityX": 2.2407856979994336,
-                    "velocityY": 1.1474110309913224,
-                    "timestamp": 2.3260614550121126
-                },
-                {
-                    "x": 7.442942536538503,
-                    "y": 2.336017519822898,
-                    "heading": -0.22036844778019443,
-                    "angularVelocity": 1.6490692583284896,
-                    "velocityX": 1.9881492014348112,
-                    "velocityY": 1.0172661231232827,
-                    "timestamp": 2.3602707254502193
-                },
-                {
-                    "x": 7.502356073864926,
-                    "y": 2.3663915967666074,
-                    "heading": -0.16856655782281169,
-                    "angularVelocity": 1.5142646801283255,
-                    "velocityX": 1.7367671559648723,
-                    "velocityY": 0.8878902284298598,
-                    "timestamp": 2.394479995888326
-                },
-                {
-                    "x": 7.553208169092694,
-                    "y": 2.392365594084153,
-                    "heading": -0.12246425110299042,
-                    "angularVelocity": 1.3476553615264124,
-                    "velocityX": 1.4865004303372251,
-                    "velocityY": 0.7592677945160818,
-                    "timestamp": 2.428689266326433
-                },
-                {
-                    "x": 7.595530700913324,
-                    "y": 2.4139635888667987,
-                    "heading": -0.08285639859822795,
-                    "angularVelocity": 1.1578104998299616,
-                    "velocityX": 1.2371655775940182,
-                    "velocityY": 0.6313491783381474,
-                    "timestamp": 2.4628985367645395
-                },
-                {
-                    "x": 7.629350339345417,
-                    "y": 2.431207580669044,
-                    "heading": -0.050358801794747596,
-                    "angularVelocity": 0.9499646261757312,
-                    "velocityX": 0.988610338629752,
-                    "velocityY": 0.5040736496688569,
-                    "timestamp": 2.4971078072026462
-                },
-                {
-                    "x": 7.654689602970606,
-                    "y": 2.444117248020803,
-                    "heading": -0.025464280349729525,
-                    "angularVelocity": 0.7277127260009457,
-                    "velocityX": 0.7407133592934628,
-                    "velocityY": 0.3773733606834889,
-                    "timestamp": 2.531317077640753
-                },
-                {
-                    "x": 7.671567523131761,
-                    "y": 2.4527098140869614,
-                    "heading": -0.008571412442380029,
-                    "angularVelocity": 0.49380965133159116,
-                    "velocityX": 0.4933727011714593,
-                    "velocityY": 0.2511765365386793,
-                    "timestamp": 2.5655263480788597
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 5.0379367595035626e-36,
-                    "angularVelocity": 0.2505581771434718,
-                    "velocityX": 0.24649683434481964,
-                    "velocityY": 0.12541003821758284,
-                    "timestamp": 2.5997356185169664
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 2.854280403338002e-36,
-                    "angularVelocity": -6.383230494440131e-35,
-                    "velocityX": -1.2082929677962258e-35,
-                    "velocityY": 1.8766665143888344e-34,
-                    "timestamp": 2.633944888955073
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "Stage Side To C5": {
-            "waypoints": [
-                {
-                    "x": 0.71,
-                    "y": 4.36,
-                    "heading": -1.04,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 19
-                },
-                {
-                    "x": 3.4563703536987305,
-                    "y": 2.4171059131622314,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 17
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 0.71,
-                    "y": 4.36,
-                    "heading": -1.04,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 1.4815405069278332e-31,
-                    "timestamp": 0
-                },
-                {
-                    "x": 0.7317173879428727,
-                    "y": 4.344479531295635,
-                    "heading": -1.013582317544266,
-                    "angularVelocity": 0.4585462516411977,
-                    "velocityX": 0.37696065327942924,
-                    "velocityY": -0.26939731598432753,
-                    "timestamp": 0.05761181638969176
-                },
-                {
-                    "x": 0.7752730705207751,
-                    "y": 4.313424298338961,
-                    "heading": -0.9621480921974644,
-                    "angularVelocity": 0.892772152832256,
-                    "velocityX": 0.7560199505477795,
-                    "velocityY": -0.5390427676609577,
-                    "timestamp": 0.11522363277938352
-                },
-                {
-                    "x": 0.8408250898493527,
-                    "y": 4.266812487548848,
-                    "heading": -0.8877439224939486,
-                    "angularVelocity": 1.2914741170498636,
-                    "velocityX": 1.1378224717856051,
-                    "velocityY": -0.8090668496689294,
-                    "timestamp": 0.17283544916907528
-                },
-                {
-                    "x": 0.9285847241631874,
-                    "y": 4.204600540366139,
-                    "heading": -0.7936992773737622,
-                    "angularVelocity": 1.6323846567179892,
-                    "velocityX": 1.5232922656043415,
-                    "velocityY": -1.0798470015578314,
-                    "timestamp": 0.23044726555876704
-                },
-                {
-                    "x": 1.0388213735909644,
-                    "y": 4.1267152282410935,
-                    "heading": -0.6856850144743787,
-                    "angularVelocity": 1.8748629997840256,
-                    "velocityX": 1.9134381857035365,
-                    "velocityY": -1.3518982216811457,
-                    "timestamp": 0.2880590819484588
-                },
-                {
-                    "x": 1.1718232312823271,
-                    "y": 4.033123229014291,
-                    "heading": -0.5741627817643435,
-                    "angularVelocity": 1.935752762171023,
-                    "velocityX": 2.3085864328894954,
-                    "velocityY": -1.6245278328622093,
-                    "timestamp": 0.34567089833815057
-                },
-                {
-                    "x": 1.3270687303513053,
-                    "y": 3.9246703032584405,
-                    "heading": -0.488682735559766,
-                    "angularVelocity": 1.4837242003685942,
-                    "velocityX": 2.6946815566251776,
-                    "velocityY": -1.882477112373353,
-                    "timestamp": 0.40328271472784233
-                },
-                {
-                    "x": 1.5010681185276094,
-                    "y": 3.804139759424889,
-                    "heading": -0.46956462349945394,
-                    "angularVelocity": 0.3318435914430377,
-                    "velocityX": 3.020203129846763,
-                    "velocityY": -2.0921149754812625,
-                    "timestamp": 0.4608945311175341
-                },
-                {
-                    "x": 1.6785494425409546,
-                    "y": 3.677652137140934,
-                    "heading": -0.4695645649556455,
-                    "angularVelocity": 0.0000010161770978802793,
-                    "velocityX": 3.080641006227699,
-                    "velocityY": -2.195515264236428,
-                    "timestamp": 0.5185063475072258
-                },
-                {
-                    "x": 1.85603075289391,
-                    "y": 3.551164495689284,
-                    "heading": -0.4695645064119953,
-                    "angularVelocity": 0.000001016174352223477,
-                    "velocityX": 3.080640769116793,
-                    "velocityY": -2.1955155969406546,
-                    "timestamp": 0.5761181638969175
-                },
-                {
-                    "x": 2.033512086622271,
-                    "y": 3.4246768870368545,
-                    "heading": -0.4695644478683731,
-                    "angularVelocity": 0.0000010161738659585955,
-                    "velocityX": 3.0806411748565776,
-                    "velocityY": -2.195515027626543,
-                    "timestamp": 0.6337299802866092
-                },
-                {
-                    "x": 2.2109934521039283,
-                    "y": 3.298189322939119,
-                    "heading": -0.46956438932476746,
-                    "angularVelocity": 0.0000010161735782211743,
-                    "velocityX": 3.080641726016013,
-                    "velocityY": -2.1955142542661994,
-                    "timestamp": 0.6913417966763009
-                },
-                {
-                    "x": 2.3884748618363862,
-                    "y": 3.17170182093203,
-                    "heading": -0.4695643307811488,
-                    "angularVelocity": 0.000001016173805422874,
-                    "velocityX": 3.0806424941015065,
-                    "velocityY": -2.1955131765246763,
-                    "timestamp": 0.7489536130659926
-                },
-                {
-                    "x": 2.5659563234551954,
-                    "y": 3.045214391729511,
-                    "heading": -0.4695642722374779,
-                    "angularVelocity": 0.0000010161747112988211,
-                    "velocityX": 3.0806433947214558,
-                    "velocityY": -2.1955119128156926,
-                    "timestamp": 0.8065654294556843
-                },
-                {
-                    "x": 2.743437831089148,
-                    "y": 2.918727027093402,
-                    "heading": -0.46956421369376844,
-                    "angularVelocity": 0.000001016175381373278,
-                    "velocityX": 3.08064419343162,
-                    "velocityY": -2.1955107921009867,
-                    "timestamp": 0.864177245845376
-                },
-                {
-                    "x": 2.920919379832659,
-                    "y": 2.7922397201404365,
-                    "heading": -0.4695641551500532,
-                    "angularVelocity": 0.0000010161754817816058,
-                    "velocityX": 3.080644906992851,
-                    "velocityY": -2.195509790862922,
-                    "timestamp": 0.9217890622350677
-                },
-                {
-                    "x": 3.0984009676669872,
-                    "y": 2.6657524680380327,
-                    "heading": -0.4695640966063568,
-                    "angularVelocity": 0.000001016175153333812,
-                    "velocityX": 3.080645585513673,
-                    "velocityY": -2.195508838791537,
-                    "timestamp": 0.9794008786247594
-                },
-                {
-                    "x": 3.275882647398041,
-                    "y": 2.5392653448813247,
-                    "heading": -0.4695640380626867,
-                    "angularVelocity": 0.0000010161746978103879,
-                    "velocityX": 3.080647180615714,
-                    "velocityY": -2.1955066006101345,
-                    "timestamp": 1.0370126950144511
-                },
-                {
-                    "x": 3.4563703536987305,
-                    "y": 2.417105913162231,
-                    "heading": -0.4695639788949644,
-                    "angularVelocity": 0.0000010270067149252654,
-                    "velocityX": 3.1328244379564545,
-                    "velocityY": -2.1203884788634437,
-                    "timestamp": 1.0946245114041429
-                },
-                {
-                    "x": 3.7578524899325245,
-                    "y": 2.288322056940197,
-                    "heading": -0.4695639274563231,
-                    "angularVelocity": 5.9355601779034e-7,
-                    "velocityX": 3.4788348155609183,
-                    "velocityY": -1.4860507766867785,
-                    "timestamp": 1.1812863255510104
-                },
-                {
-                    "x": 4.061243078218464,
-                    "y": 2.1641008385808727,
-                    "heading": -0.46956387627292245,
-                    "angularVelocity": 5.906107685245515e-7,
-                    "velocityX": 3.5008566491785738,
-                    "velocityY": -1.43340200735705,
-                    "timestamp": 1.267948139697878
-                },
-                {
-                    "x": 4.364633703659485,
-                    "y": 2.039879710966971,
-                    "heading": -0.46956382508952316,
-                    "angularVelocity": 5.906107531304183e-7,
-                    "velocityX": 3.5008570779150574,
-                    "velocityY": -1.433400960235855,
-                    "timestamp": 1.3546099538447456
-                },
-                {
-                    "x": 4.668024366282006,
-                    "y": 1.9156586741630988,
-                    "heading": -0.46956377390610504,
-                    "angularVelocity": 5.906109701114576e-7,
-                    "velocityX": 3.5008575069563967,
-                    "velocityY": -1.4333999123691519,
-                    "timestamp": 1.4412717679916132
-                },
-                {
-                    "x": 4.971415087165123,
-                    "y": 1.7914377796518506,
-                    "heading": -0.4695637227226054,
-                    "angularVelocity": 5.906119107964098e-7,
-                    "velocityX": 3.5008581792316793,
-                    "velocityY": -1.4333982704392505,
-                    "timestamp": 1.5279335821384807
-                },
-                {
-                    "x": 5.2748058711699946,
-                    "y": 1.6672170393060612,
-                    "heading": -0.4695636715390663,
-                    "angularVelocity": 5.906123657897232e-7,
-                    "velocityX": 3.5008589076003997,
-                    "velocityY": -1.4333964915074275,
-                    "timestamp": 1.6145953962853483
-                },
-                {
-                    "x": 5.5781967938554144,
-                    "y": 1.5429966376674569,
-                    "heading": -0.4695636203556594,
-                    "angularVelocity": 5.906108405690599e-7,
-                    "velocityX": 3.5008605078501667,
-                    "velocityY": -1.4333925831287677,
-                    "timestamp": 1.701257210432216
-                },
-                {
-                    "x": 5.886988948403778,
-                    "y": 1.4328868446307816,
-                    "heading": -0.4695635793301605,
-                    "angularVelocity": 4.733976467078746e-7,
-                    "velocityX": 3.5631859035981908,
-                    "velocityY": -1.2705687518849984,
-                    "timestamp": 1.7879190245790835
-                },
-                {
-                    "x": 6.195781169913944,
-                    "y": 1.3227772393821355,
-                    "heading": -0.4695635383046595,
-                    "angularVelocity": 4.7339767079441426e-7,
-                    "velocityX": 3.5631866762776085,
-                    "velocityY": -1.2705665849787193,
-                    "timestamp": 1.874580838725951
-                },
-                {
-                    "x": 6.5045734104803365,
-                    "y": 1.212667687574965,
-                    "heading": -0.46956349727913566,
-                    "angularVelocity": 4.733979354519185e-7,
-                    "velocityX": 3.5631868961694693,
-                    "velocityY": -1.270565968311784,
-                    "timestamp": 1.9612426528728186
-                },
-                {
-                    "x": 6.813365656566466,
-                    "y": 1.1025581512474965,
-                    "heading": -0.46956345625352064,
-                    "angularVelocity": 4.733989867793667e-7,
-                    "velocityX": 3.563186959862305,
-                    "velocityY": -1.270565789689833,
-                    "timestamp": 2.0479044670196864
-                },
-                {
-                    "x": 7.101065618447083,
-                    "y": 0.9964425714529646,
-                    "heading": -0.4012732100130825,
-                    "angularVelocity": 0.7880085007764233,
-                    "velocityX": 3.3198008224596522,
-                    "velocityY": -1.2244790954259896,
-                    "timestamp": 2.134566281166554
-                },
-                {
-                    "x": 7.333266238045498,
-                    "y": 0.911992130350644,
-                    "heading": -0.2613611242014092,
-                    "angularVelocity": 1.6144606155435655,
-                    "velocityX": 2.6793879390165927,
-                    "velocityY": -0.9744827284506232,
-                    "timestamp": 2.2212280953134216
-                },
-                {
-                    "x": 7.506890250099573,
-                    "y": 0.8492417427348896,
-                    "heading": -0.1371754084070899,
-                    "angularVelocity": 1.43299233943868,
-                    "velocityX": 2.0034661605379225,
-                    "velocityY": -0.7240834759056616,
-                    "timestamp": 2.307889909460289
-                },
-                {
-                    "x": 7.622366585478705,
-                    "y": 0.8076799304753898,
-                    "heading": -0.04732610702177306,
-                    "angularVelocity": 1.0367807582823931,
-                    "velocityX": 1.332493861522807,
-                    "velocityY": -0.4795862245517285,
-                    "timestamp": 2.3945517236071567
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 2.9489608951695904e-31,
-                    "angularVelocity": 0.5461010421679925,
-                    "velocityX": 0.6650381726791765,
-                    "velocityY": -0.2386279433331854,
-                    "timestamp": 2.4812135377540243
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": -7.162012659830636e-32,
-                    "angularVelocity": -4.0878892657023166e-30,
-                    "velocityX": 2.3668785600931567e-32,
-                    "velocityY": 6.81692291678628e-32,
-                    "timestamp": 2.567875351900892
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "W1 To W2": {
-            "waypoints": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 10
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": -0.003,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "angularVelocity": 1.792103156985809e-33,
-                    "velocityX": -7.27073415334495e-34,
-                    "velocityY": -7.853383039035825e-35,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.298001341213542,
-                    "y": 6.709118496985434,
-                    "heading": 0.374359095500822,
-                    "angularVelocity": -0.2058832051525312,
-                    "velocityX": -0.02630859852023658,
-                    "velocityY": -0.6302702831973344,
-                    "timestamp": 0.07596979310473732
-                },
-                {
-                    "x": 2.294004039374054,
-                    "y": 6.613355753676287,
-                    "heading": 0.3430620070158405,
-                    "angularVelocity": -0.4119675361210102,
-                    "velocityX": -0.0526169899393711,
-                    "velocityY": -1.2605371081782701,
-                    "timestamp": 0.15193958620947465
-                },
-                {
-                    "x": 2.2880080040813597,
-                    "y": 6.469712250400375,
-                    "heading": 0.296082691458658,
-                    "angularVelocity": -0.6183946755312791,
-                    "velocityX": -0.07892657130746197,
-                    "velocityY": -1.8907976105433808,
-                    "timestamp": 0.22790937931421196
-                },
-                {
-                    "x": 2.280012894319733,
-                    "y": 6.278188663497132,
-                    "heading": 0.23338596481601495,
-                    "angularVelocity": -0.8252849465603376,
-                    "velocityX": -0.10524064150870725,
-                    "velocityY": -2.5210492101669324,
-                    "timestamp": 0.3038791724189493
-                },
-                {
-                    "x": 2.270017931664709,
-                    "y": 6.038785816229407,
-                    "heading": 0.1549313176962556,
-                    "angularVelocity": -1.0327084478378958,
-                    "velocityX": -0.13156495820969516,
-                    "velocityY": -3.1512899730774246,
-                    "timestamp": 0.37984896552368663
-                },
-                {
-                    "x": 2.2620113109689415,
-                    "y": 5.847271344563018,
-                    "heading": 0.09176539516362914,
-                    "angularVelocity": -0.8314610314331311,
-                    "velocityX": -0.10539216139142096,
-                    "velocityY": -2.5209292251507525,
-                    "timestamp": 0.45581875862842397
-                },
-                {
-                    "x": 2.2560057963370115,
-                    "y": 5.703635646814395,
-                    "heading": 0.0443838554377995,
-                    "angularVelocity": -0.6236892031613949,
-                    "velocityX": -0.079051349049357,
-                    "velocityY": -1.8906948653999391,
-                    "timestamp": 0.5317885517331613
-                },
-                {
-                    "x": 2.252001935551058,
-                    "y": 5.607878550007709,
-                    "heading": 0.012794605457377246,
-                    "angularVelocity": -0.4158132948561684,
-                    "velocityX": -0.05270332618167983,
-                    "velocityY": -1.2604627825518706,
-                    "timestamp": 0.6077583448378986
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": -0.003,
-                    "angularVelocity": -0.20790639031492017,
-                    "velocityX": -0.026351730987313093,
-                    "velocityY": -0.6302314123944018,
-                    "timestamp": 0.6837281379426359
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": -0.003,
-                    "angularVelocity": -6.008173298568887e-34,
-                    "velocityX": -7.305982513747354e-33,
-                    "velocityY": -3.8813573012664696e-35,
-                    "timestamp": 0.7596979310473732
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "W1 To W3": {
-            "waypoints": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.393,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "angularVelocity": -2.4663146867319515e-36,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.3001378208847427,
-                    "y": 6.713171893956039,
-                    "heading": 0.36291265694481545,
-                    "angularVelocity": -0.37089801282350154,
-                    "velocityX": 0.0018871357066083216,
-                    "velocityY": -0.6001237332286558,
-                    "timestamp": 0.0730317826428328
-                },
-                {
-                    "x": 2.3004082222445,
-                    "y": 6.625427179140099,
-                    "heading": 0.3106389081779897,
-                    "angularVelocity": -0.7157671204942956,
-                    "velocityX": 0.0037025162192713427,
-                    "velocityY": -1.201459304985902,
-                    "timestamp": 0.1460635652856656
-                },
-                {
-                    "x": 2.300808308388558,
-                    "y": 6.493639269598709,
-                    "heading": 0.23621859479744584,
-                    "angularVelocity": -1.019012691289514,
-                    "velocityX": 0.005478246998500719,
-                    "velocityY": -1.804528176258669,
-                    "timestamp": 0.2190953479284984
-                },
-                {
-                    "x": 2.30134752968001,
-                    "y": 6.317621999647275,
-                    "heading": 0.14515321533996162,
-                    "angularVelocity": -1.2469280655909227,
-                    "velocityX": 0.007383378468093947,
-                    "velocityY": -2.4101461525629078,
-                    "timestamp": 0.2921271305713312
-                },
-                {
-                    "x": 2.3020704977057354,
-                    "y": 6.097150895386178,
-                    "heading": 0.04952404086318575,
-                    "angularVelocity": -1.309418598536164,
-                    "velocityX": 0.00989936161439861,
-                    "velocityY": -3.0188377755932185,
-                    "timestamp": 0.36515891321416405
-                },
-                {
-                    "x": 2.3031517302727234,
-                    "y": 5.834309739739576,
-                    "heading": 0.004115026920641748,
-                    "angularVelocity": -0.6217705812361183,
-                    "velocityX": 0.014804959263771778,
-                    "velocityY": -3.598996849522972,
-                    "timestamp": 0.4381906958569969
-                },
-                {
-                    "x": 2.303521267718626,
-                    "y": 5.558035105167069,
-                    "heading": 0.00411499765356137,
-                    "angularVelocity": -4.007444337013033e-7,
-                    "velocityX": 0.005059953797233799,
-                    "velocityY": -3.7829370251531125,
-                    "timestamp": 0.5112224784998297
-                },
-                {
-                    "x": 2.3038907744757537,
-                    "y": 5.281760470553516,
-                    "heading": 0.004114968386478979,
-                    "angularVelocity": -4.0074446127092794e-7,
-                    "velocityX": 0.005059533586019425,
-                    "velocityY": -3.782937025715139,
-                    "timestamp": 0.5842542611426625
-                },
-                {
-                    "x": 2.3020058670727765,
-                    "y": 5.019317688425813,
-                    "heading": -0.04121766182368216,
-                    "angularVelocity": -0.6207246840990261,
-                    "velocityX": -0.02580941248819387,
-                    "velocityY": -3.5935420529332935,
-                    "timestamp": 0.6572860437854954
-                },
-                {
-                    "x": 2.301170882315387,
-                    "y": 4.798980955675817,
-                    "heading": -0.139726223563735,
-                    "angularVelocity": -1.348845094221732,
-                    "velocityX": -0.011433169603334988,
-                    "velocityY": -3.016997871016921,
-                    "timestamp": 0.7303178264283282
-                },
-                {
-                    "x": 2.3006513716532107,
-                    "y": 4.623098256624685,
-                    "heading": -0.23378951684706242,
-                    "angularVelocity": -1.2879775062228829,
-                    "velocityX": -0.007113487352719777,
-                    "velocityY": -2.4083035178163246,
-                    "timestamp": 0.803349609071161
-                },
-                {
-                    "x": 2.300310703422249,
-                    "y": 4.491429967185545,
-                    "heading": -0.31073488166813007,
-                    "angularVelocity": -1.0535873839664371,
-                    "velocityX": -0.004664657203118652,
-                    "velocityY": -1.8028902578357346,
-                    "timestamp": 0.8763813917139939
-                },
-                {
-                    "x": 2.3001003821722423,
-                    "y": 4.403776446553788,
-                    "heading": -0.36487144716496317,
-                    "angularVelocity": -0.7412740527174575,
-                    "velocityX": -0.0028798591845282696,
-                    "velocityY": -1.2002106132398922,
-                    "timestamp": 0.9494131743568267
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.393,
-                    "angularVelocity": -0.3851549533249317,
-                    "velocityX": -0.0013744998219966374,
-                    "velocityY": -0.5994163769475577,
-                    "timestamp": 1.0224449569996594
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.393,
-                    "angularVelocity": -7.841220088894993e-36,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 1.0954767396424923
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "W1 To S1": {
-            "waypoints": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 9
-                },
-                {
-                    "x": 3.096125602722168,
-                    "y": 6.283931255340576,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 12
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.26,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.3161416084367663,
-                    "y": 6.743433917099698,
-                    "heading": 0.3883998707887416,
-                    "angularVelocity": -0.031816354442463515,
-                    "velocityX": 0.32095354030299916,
-                    "velocityY": -0.26974278009236136,
-                    "timestamp": 0.05029266360959302
-                },
-                {
-                    "x": 2.3488347297971495,
-                    "y": 6.716804087923855,
-                    "heading": 0.385297889956982,
-                    "angularVelocity": -0.06167859503006947,
-                    "velocityX": 0.6500574639309191,
-                    "velocityY": -0.5294972917434406,
-                    "timestamp": 0.10058532721918605
-                },
-                {
-                    "x": 2.398581891811533,
-                    "y": 6.677772540045158,
-                    "heading": 0.38082205724745694,
-                    "angularVelocity": -0.08899573791258333,
-                    "velocityX": 0.9891534558709463,
-                    "velocityY": -0.7760883015003333,
-                    "timestamp": 0.15087799082877906
-                },
-                {
-                    "x": 2.4660071913042128,
-                    "y": 6.627244851321165,
-                    "heading": 0.3751449326788243,
-                    "angularVelocity": -0.11288176368431184,
-                    "velocityX": 1.3406587492776654,
-                    "velocityY": -1.004673149074478,
-                    "timestamp": 0.2011706544383721
-                },
-                {
-                    "x": 2.551890499441383,
-                    "y": 6.566518101686868,
-                    "heading": 0.3685091874361843,
-                    "angularVelocity": -0.13194260884949985,
-                    "velocityX": 1.7076707013145411,
-                    "velocityY": -1.207467357579225,
-                    "timestamp": 0.2514633180479651
-                },
-                {
-                    "x": 2.6571889825280843,
-                    "y": 6.497556440570772,
-                    "heading": 0.3612736159712354,
-                    "angularVelocity": -0.14386932299145247,
-                    "velocityX": 2.093714580402863,
-                    "velocityY": -1.3712071735039586,
-                    "timestamp": 0.3017559816575581
-                },
-                {
-                    "x": 2.7829575448660773,
-                    "y": 6.423518691483174,
-                    "heading": 0.3539965965907385,
-                    "angularVelocity": -0.144693457419283,
-                    "velocityX": 2.50073377131696,
-                    "velocityY": -1.4721381564184353,
-                    "timestamp": 0.35204864526715113
-                },
-                {
-                    "x": 2.929814044412336,
-                    "y": 6.349664492887633,
-                    "heading": 0.3475640372215199,
-                    "angularVelocity": -0.1279025390095239,
-                    "velocityX": 2.9200382124570217,
-                    "velocityY": -1.468488508957291,
-                    "timestamp": 0.40234130887674413
-                },
-                {
-                    "x": 3.096125602722168,
-                    "y": 6.283931255340576,
-                    "heading": 0.3432166982946709,
-                    "angularVelocity": -0.08644081690714242,
-                    "velocityX": 3.3068751259797926,
-                    "velocityY": -1.3070144396670602,
-                    "timestamp": 0.45263397248633713
-                },
-                {
-                    "x": 3.3513045644934745,
-                    "y": 6.2237675636491945,
-                    "heading": 0.3432161755192605,
-                    "angularVelocity": -0.0000075431393477149555,
-                    "velocityX": 3.681983552011348,
-                    "velocityY": -0.8681033957434081,
-                    "timestamp": 0.5219387257984235
-                },
-                {
-                    "x": 3.6126701345327414,
-                    "y": 6.203172404760708,
-                    "heading": 0.34321617069844046,
-                    "angularVelocity": -6.955973168022537e-8,
-                    "velocityX": 3.771250275753975,
-                    "velocityY": -0.2971680570846853,
-                    "timestamp": 0.5912434791105099
-                },
-                {
-                    "x": 3.8741231287698,
-                    "y": 6.222626349014288,
-                    "heading": 0.3432161668255195,
-                    "angularVelocity": -5.588247279499863e-8,
-                    "velocityX": 3.7725117216666155,
-                    "velocityY": 0.2807014428862732,
-                    "timestamp": 0.6605482324225962
-                },
-                {
-                    "x": 4.129562675072727,
-                    "y": 6.28167517120369,
-                    "heading": 0.34321616334811994,
-                    "angularVelocity": -5.017548406400545e-8,
-                    "velocityX": 3.68574353266443,
-                    "velocityY": 0.8520169161197405,
-                    "timestamp": 0.7298529857346825
-                },
-                {
-                    "x": 4.378319272574482,
-                    "y": 6.3644778320450675,
-                    "heading": 0.34321615995103527,
-                    "angularVelocity": -4.901661807423297e-8,
-                    "velocityX": 3.5893150990896703,
-                    "velocityY": 1.1947616416511535,
-                    "timestamp": 0.7991577390467688
-                },
-                {
-                    "x": 4.606228025075672,
-                    "y": 6.4403413045373235,
-                    "heading": 0.31944011002240325,
-                    "angularVelocity": -0.34306521259177225,
-                    "velocityX": 3.2885010278428317,
-                    "velocityY": 1.094635921300162,
-                    "timestamp": 0.8684624923588551
-                },
-                {
-                    "x": 4.796152006740457,
-                    "y": 6.503560868780938,
-                    "heading": 0.2996267220530126,
-                    "angularVelocity": -0.2858878651536105,
-                    "velocityX": 2.74041782977768,
-                    "velocityY": 0.9121966563957079,
-                    "timestamp": 0.9377672456709414
-                },
-                {
-                    "x": 4.9480911999499915,
-                    "y": 6.554136520865842,
-                    "heading": 0.2837760268788632,
-                    "angularVelocity": -0.22871007278204386,
-                    "velocityX": 2.1923343774897495,
-                    "velocityY": 0.7297573350727579,
-                    "timestamp": 1.0070719989830277
-                },
-                {
-                    "x": 5.062045598488017,
-                    "y": 6.592068260177227,
-                    "heading": 0.27188801539640783,
-                    "angularVelocity": -0.1715324117657857,
-                    "velocityX": 1.644250835507307,
-                    "velocityY": 0.5473180048787404,
-                    "timestamp": 1.076376752295114
-                },
-                {
-                    "x": 5.138015199132455,
-                    "y": 6.617356086604739,
-                    "heading": 0.26396267467345363,
-                    "angularVelocity": -0.11435493734845723,
-                    "velocityX": 1.0961672470333998,
-                    "velocityY": 0.364878673092444,
-                    "timestamp": 1.1456815056072003
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.26,
-                    "angularVelocity": -0.05717753089184424,
-                    "velocityX": 0.548083631385222,
-                    "velocityY": 0.1824393391651535,
-                    "timestamp": 1.2149862589192866
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.26,
-                    "angularVelocity": -1.0031982410108139e-28,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 1.2842910122313729
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "W1 To S2": {
-            "waypoints": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 8
-                },
-                {
-                    "x": 2.916773796081543,
-                    "y": 6.423426628112793,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 6
-                },
-                {
-                    "x": 3.654109001159668,
-                    "y": 6.343714714050293,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 7
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": -0.003,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "angularVelocity": -4.207802187241835e-33,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.3149122846118377,
-                    "y": 6.742667851221533,
-                    "heading": 0.387869941582996,
-                    "angularVelocity": -0.04275911471750884,
-                    "velocityX": 0.2993514559631543,
-                    "velocityY": -0.28770572153040486,
-                    "timestamp": 0.049815306773293704
-                },
-                {
-                    "x": 2.3455895487746456,
-                    "y": 6.714948533605446,
-                    "heading": 0.38350547033909727,
-                    "angularVelocity": -0.08761305563705182,
-                    "velocityX": 0.6158200390577908,
-                    "velocityY": -0.5564417728518571,
-                    "timestamp": 0.09963061354658741
-                },
-                {
-                    "x": 2.393100480232861,
-                    "y": 6.675215395028912,
-                    "heading": 0.37677783190043923,
-                    "angularVelocity": -0.13505163120415611,
-                    "velocityX": 0.9537416215146959,
-                    "velocityY": -0.7976090312433709,
-                    "timestamp": 0.1494459203198811
-                },
-                {
-                    "x": 2.4587594870185163,
-                    "y": 6.62556801103228,
-                    "heading": 0.3675326753009839,
-                    "angularVelocity": -0.18558867140063587,
-                    "velocityX": 1.318048829538744,
-                    "velocityY": -0.996629092792259,
-                    "timestamp": 0.19926122709317481
-                },
-                {
-                    "x": 2.5440298105146963,
-                    "y": 6.569365137451258,
-                    "heading": 0.35560686650228135,
-                    "angularVelocity": -0.23940048895004992,
-                    "velocityX": 1.7117293663218704,
-                    "velocityY": -1.1282249818675432,
-                    "timestamp": 0.24907653386646852
-                },
-                {
-                    "x": 2.649943674204089,
-                    "y": 6.511985811495891,
-                    "heading": 0.34090990459713355,
-                    "angularVelocity": -0.2950290353938729,
-                    "velocityX": 2.126130913363638,
-                    "velocityY": -1.1518412647039136,
-                    "timestamp": 0.2988918406397622
-                },
-                {
-                    "x": 2.7755461673317816,
-                    "y": 6.460929479776747,
-                    "heading": 0.3236377937131317,
-                    "angularVelocity": -0.3467229653455557,
-                    "velocityX": 2.5213634375333727,
-                    "velocityY": -1.0249125223997653,
-                    "timestamp": 0.3487071474130559
-                },
-                {
-                    "x": 2.916773796081543,
-                    "y": 6.423426628112793,
-                    "heading": 0.3044078287975214,
-                    "angularVelocity": -0.38602522319421667,
-                    "velocityX": 2.835024772455506,
-                    "velocityY": -0.7528379145513634,
-                    "timestamp": 0.39852245418634963
-                },
-                {
-                    "x": 3.0358362362248608,
-                    "y": 6.404912706911414,
-                    "heading": 0.2883220118823199,
-                    "angularVelocity": -0.4054270239814924,
-                    "velocityX": 3.0008504404627585,
-                    "velocityY": -0.46662497866622815,
-                    "timestamp": 0.43819868680957713
-                },
-                {
-                    "x": 3.161507467990405,
-                    "y": 6.397735862008237,
-                    "heading": 0.27120244348460076,
-                    "angularVelocity": -0.43148170241679884,
-                    "velocityX": 3.1674184633137656,
-                    "velocityY": -0.1808852410792362,
-                    "timestamp": 0.47787491943280463
-                },
-                {
-                    "x": 3.294397924367021,
-                    "y": 6.401341217255427,
-                    "heading": 0.24704597239250262,
-                    "angularVelocity": -0.6088398392423459,
-                    "velocityX": 3.349371842799791,
-                    "velocityY": 0.09086939481941644,
-                    "timestamp": 0.5175511520560321
-                },
-                {
-                    "x": 3.420816904565574,
-                    "y": 6.393526992528847,
-                    "heading": 0.22271106774313612,
-                    "angularVelocity": -0.6133370796675788,
-                    "velocityX": 3.186264719209789,
-                    "velocityY": -0.19694976589104882,
-                    "timestamp": 0.5572273846792596
-                },
-                {
-                    "x": 3.540724707702168,
-                    "y": 6.374315752768638,
-                    "heading": 0.19854345438454005,
-                    "angularVelocity": -0.6091206689927945,
-                    "velocityX": 3.0221569743089773,
-                    "velocityY": -0.48420019971772477,
-                    "timestamp": 0.5969036173024871
-                },
-                {
-                    "x": 3.654109001159668,
-                    "y": 6.343714714050293,
-                    "heading": 0.17465129572461457,
-                    "angularVelocity": -0.602178107150174,
-                    "velocityX": 2.857738398053468,
-                    "velocityY": -0.7712687595352792,
-                    "timestamp": 0.6365798499257146
-                },
-                {
-                    "x": 3.8638905650676802,
-                    "y": 6.228810110896502,
-                    "heading": 0.12580249974502505,
-                    "angularVelocity": -0.584756466458008,
-                    "velocityX": 2.5112415481035373,
-                    "velocityY": -1.375493671286808,
-                    "timestamp": 0.7201168414454817
-                },
-                {
-                    "x": 4.035438177373403,
-                    "y": 6.070055732251625,
-                    "heading": 0.08069123606907251,
-                    "angularVelocity": -0.5400154213754852,
-                    "velocityX": 2.0535526739088925,
-                    "velocityY": -1.90040813963597,
-                    "timestamp": 0.8036538329652487
-                },
-                {
-                    "x": 4.153723645387517,
-                    "y": 5.887986786487416,
-                    "heading": 0.04380352514936875,
-                    "angularVelocity": -0.4415733706546099,
-                    "velocityX": 1.4159651414562335,
-                    "velocityY": -2.179500870834227,
-                    "timestamp": 0.8871908244850157
-                },
-                {
-                    "x": 4.218422318430694,
-                    "y": 5.7283456029173445,
-                    "heading": 0.01885386609449773,
-                    "angularVelocity": -0.29866599934929844,
-                    "velocityX": 0.7744912985986951,
-                    "velocityY": -1.9110238550103982,
-                    "timestamp": 0.9707278160047828
-                },
-                {
-                    "x": 4.250477583142556,
-                    "y": 5.616790769692197,
-                    "heading": 0.0038966042591740443,
-                    "angularVelocity": -0.17904956311230244,
-                    "velocityX": 0.38372539073628414,
-                    "velocityY": -1.33539443060689,
-                    "timestamp": 1.0542648075245498
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": -0.003,
-                    "angularVelocity": -0.08255748900826321,
-                    "velocityX": 0.14990265545387924,
-                    "velocityY": -0.6798278063289014,
-                    "timestamp": 1.1378017990443168
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": -0.003,
-                    "angularVelocity": 2.6015085807950757e-30,
-                    "velocityX": -2.4770702901017708e-31,
-                    "velocityY": 1.2993067663186955e-31,
-                    "timestamp": 1.2213387905640838
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "W1 To S3": {
-            "waypoints": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 30
-                },
-                {
-                    "x": 2.131748914718628,
-                    "y": 2.531857967376709,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 29
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.569,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 2.542786541644192e-31,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.2937082670823616,
-                    "y": 6.738516922494428,
-                    "heading": 0.3838170687349535,
-                    "angularVelocity": -0.12752172771189255,
-                    "velocityX": -0.1297657401618881,
-                    "velocityY": -0.3812097977411177,
-                    "timestamp": 0.04848531599949176
-                },
-                {
-                    "x": 2.281123433600762,
-                    "y": 6.70154498788335,
-                    "heading": 0.37169259737448246,
-                    "angularVelocity": -0.2500648105569625,
-                    "velocityX": -0.25955968775719646,
-                    "velocityY": -0.7625387985814647,
-                    "timestamp": 0.09697063199898352
-                },
-                {
-                    "x": 2.262243832879689,
-                    "y": 6.646077048114969,
-                    "heading": 0.3539392998096368,
-                    "angularVelocity": -0.36615823159004923,
-                    "velocityX": -0.3893880102034898,
-                    "velocityY": -1.1440152265685524,
-                    "timestamp": 0.14545594799847528
-                },
-                {
-                    "x": 2.2370673940379584,
-                    "y": 6.572104077537847,
-                    "heading": 0.3309787534387302,
-                    "angularVelocity": -0.4735567026240549,
-                    "velocityX": -0.5192590441623445,
-                    "velocityY": -1.5256778068215682,
-                    "timestamp": 0.19394126399796704
-                },
-                {
-                    "x": 2.2055915440305824,
-                    "y": 6.479614437749851,
-                    "heading": 0.303410444299779,
-                    "angularVelocity": -0.5685908933466691,
-                    "velocityX": -0.6491831466623577,
-                    "velocityY": -1.9075804268039651,
-                    "timestamp": 0.2424265799974588
-                },
-                {
-                    "x": 2.167813255965874,
-                    "y": 6.3685929704461826,
-                    "heading": 0.2721534840825349,
-                    "angularVelocity": -0.6446685882563702,
-                    "velocityX": -0.7791696782014628,
-                    "velocityY": -2.2897956838082623,
-                    "timestamp": 0.29091189599695055
-                },
-                {
-                    "x": 2.123730000007902,
-                    "y": 6.239021112222755,
-                    "heading": 0.23878941688886735,
-                    "angularVelocity": -0.6881272505944181,
-                    "velocityX": -0.909208387103519,
-                    "velocityY": -2.6723938073299895,
-                    "timestamp": 0.3393972119964423
-                },
-                {
-                    "x": 2.0733472424162183,
-                    "y": 6.090890823809695,
-                    "heading": 0.2066566550746183,
-                    "angularVelocity": -0.6627318220196932,
-                    "velocityX": -1.0391343554894152,
-                    "velocityY": -3.0551577391940747,
-                    "timestamp": 0.38788252799593403
-                },
-                {
-                    "x": 2.0168156590767947,
-                    "y": 5.924489369921669,
-                    "heading": 0.187194468928521,
-                    "angularVelocity": -0.4014037187247154,
-                    "velocityX": -1.1659526636929207,
-                    "velocityY": -3.431996893448088,
-                    "timestamp": 0.4363678439954258
-                },
-                {
-                    "x": 1.9576664650342146,
-                    "y": 5.750871436780747,
-                    "heading": 0.18719445417707173,
-                    "angularVelocity": -3.042457077435804e-7,
-                    "velocityX": -1.2199403638687387,
-                    "velocityY": -3.580835342863344,
-                    "timestamp": 0.4848531599949175
-                },
-                {
-                    "x": 1.8985172819179166,
-                    "y": 5.577253499917319,
-                    "heading": 0.18719443942575115,
-                    "angularVelocity": -3.042430533078605e-7,
-                    "velocityX": -1.2199401385163373,
-                    "velocityY": -3.5808354196392784,
-                    "timestamp": 0.5333384759944093
-                },
-                {
-                    "x": 1.8393681174712941,
-                    "y": 5.403635556693396,
-                    "heading": 0.18719442467442912,
-                    "angularVelocity": -3.0424308271607693e-7,
-                    "velocityX": -1.219939753457998,
-                    "velocityY": -3.5808355508232466,
-                    "timestamp": 0.5818237919939011
-                },
-                {
-                    "x": 1.780218973506269,
-                    "y": 5.230017606491683,
-                    "heading": 0.18719440992310651,
-                    "angularVelocity": -3.0424309544377536e-7,
-                    "velocityX": -1.2199393310291196,
-                    "velocityY": -3.580835694738771,
-                    "timestamp": 0.6303091079933929
-                },
-                {
-                    "x": 1.7210698517072343,
-                    "y": 5.056399648738334,
-                    "heading": 0.1871943951717844,
-                    "angularVelocity": -3.042430844451726e-7,
-                    "velocityX": -1.2199388738599835,
-                    "velocityY": -3.580835850489736,
-                    "timestamp": 0.6787944239928847
-                },
-                {
-                    "x": 1.661920753431811,
-                    "y": 4.882781682970833,
-                    "heading": 0.18719438042046438,
-                    "angularVelocity": -3.042430423387178e-7,
-                    "velocityX": -1.2199383886901667,
-                    "velocityY": -3.580836015780049,
-                    "timestamp": 0.7272797399923765
-                },
-                {
-                    "x": 1.6027716795006517,
-                    "y": 4.709163708909599,
-                    "heading": 0.18719436566914785,
-                    "angularVelocity": -3.042429697472901e-7,
-                    "velocityX": -1.2199378865945683,
-                    "velocityY": -3.5808361868366427,
-                    "timestamp": 0.7757650559918683
-                },
-                {
-                    "x": 1.5436226301529286,
-                    "y": 4.5355457264731545,
-                    "heading": 0.18719435091783626,
-                    "angularVelocity": -3.042428676797686e-7,
-                    "velocityX": -1.219937379566087,
-                    "velocityY": -3.5808363595737087,
-                    "timestamp": 0.8242503719913601
-                },
-                {
-                    "x": 1.4844769425124074,
-                    "y": 4.361926598741778,
-                    "heading": 0.18719433616649686,
-                    "angularVelocity": -3.0424344145116126e-7,
-                    "velocityX": -1.2198680450801807,
-                    "velocityY": -3.58085998103429,
-                    "timestamp": 0.8727356879908519
-                },
-                {
-                    "x": 1.4424355744034427,
-                    "y": 4.183392744566331,
-                    "heading": 0.18719432108286724,
-                    "angularVelocity": -3.110968611299024e-7,
-                    "velocityX": -0.8670948562865489,
-                    "velocityY": -3.682225236552195,
-                    "timestamp": 0.9212210039903437
-                },
-                {
-                    "x": 1.4196862795998648,
-                    "y": 4.0013919676043574,
-                    "heading": 0.18719430492048,
-                    "angularVelocity": -3.3334602243497606e-7,
-                    "velocityX": -0.4691996810625877,
-                    "velocityY": -3.753729829541186,
-                    "timestamp": 0.9697063199898355
-                },
-                {
-                    "x": 1.4164891295727136,
-                    "y": 3.81800278977475,
-                    "heading": 0.18719428668091828,
-                    "angularVelocity": -3.761873328628986e-7,
-                    "velocityX": -0.06594058347012394,
-                    "velocityY": -3.7823653213178425,
-                    "timestamp": 1.0181916359893273
-                },
-                {
-                    "x": 1.432880701620732,
-                    "y": 3.635319656493888,
-                    "heading": 0.1871942648241357,
-                    "angularVelocity": -4.507917942103899e-7,
-                    "velocityX": 0.3380729136345315,
-                    "velocityY": -3.767803292913241,
-                    "timestamp": 1.066676951988819
-                },
-                {
-                    "x": 1.4686738067578264,
-                    "y": 3.455429018696199,
-                    "heading": 0.18719405074105777,
-                    "angularVelocity": -0.0000044154209051021175,
-                    "velocityX": 0.7382256751280081,
-                    "velocityY": -3.710208628927779,
-                    "timestamp": 1.1151622679883109
-                },
-                {
-                    "x": 1.5224561218642167,
-                    "y": 3.2829543962052377,
-                    "heading": 0.17869632281657946,
-                    "angularVelocity": -0.17526394846200616,
-                    "velocityX": 1.10924955315069,
-                    "velocityY": -3.557254788081333,
-                    "timestamp": 1.1636475839878027
-                },
-                {
-                    "x": 1.5921416539328836,
-                    "y": 3.121386413493124,
-                    "heading": 0.1576086367549348,
-                    "angularVelocity": -0.43492933122045885,
-                    "velocityX": 1.4372502402541338,
-                    "velocityY": -3.33230751169348,
-                    "timestamp": 1.2121328999872945
-                },
-                {
-                    "x": 1.6761645416915945,
-                    "y": 2.9727322431612,
-                    "heading": 0.12423651758368265,
-                    "angularVelocity": -0.6882933210462135,
-                    "velocityX": 1.7329553500274961,
-                    "velocityY": -3.0659626995783453,
-                    "timestamp": 1.2606182159867863
-                },
-                {
-                    "x": 1.7731411061925635,
-                    "y": 2.838565031466778,
-                    "heading": 0.08175388007293943,
-                    "angularVelocity": -0.8761959499529206,
-                    "velocityX": 2.0001223566816617,
-                    "velocityY": -2.767172058765521,
-                    "timestamp": 1.309103531986278
-                },
-                {
-                    "x": 1.8819216264400211,
-                    "y": 2.719965378093697,
-                    "heading": 0.033593056188374194,
-                    "angularVelocity": -0.9933074146707891,
-                    "velocityX": 2.2435765964482535,
-                    "velocityY": -2.4460942643771193,
-                    "timestamp": 1.3575888479857698
-                },
-                {
-                    "x": 2.0016674106146493,
-                    "y": 2.617587012881804,
-                    "heading": -0.0178597683247242,
-                    "angularVelocity": -1.0612042729434041,
-                    "velocityX": 2.4697329842357973,
-                    "velocityY": -2.1115334220519952,
-                    "timestamp": 1.4060741639852616
-                },
-                {
-                    "x": 2.131748914718628,
-                    "y": 2.531857967376709,
-                    "heading": -0.07087516069292028,
-                    "angularVelocity": -1.0934319242038333,
-                    "velocityX": 2.682905152268552,
-                    "velocityY": -1.7681445142231829,
-                    "timestamp": 1.4545594799847534
-                },
-                {
-                    "x": 2.218821194191698,
-                    "y": 2.484029060579093,
-                    "heading": -0.10483282223316899,
-                    "angularVelocity": -1.096901896110445,
-                    "velocityX": 2.812612651242981,
-                    "velocityY": -1.5449714785171498,
-                    "timestamp": 1.4855172727096597
-                },
-                {
-                    "x": 2.3097767814437278,
-                    "y": 2.4431840829179357,
-                    "heading": -0.13859592156415457,
-                    "angularVelocity": -1.0906171389530375,
-                    "velocityX": 2.938051432149048,
-                    "velocityY": -1.3193762883647426,
-                    "timestamp": 1.516475065434566
-                },
-                {
-                    "x": 2.4044569077078632,
-                    "y": 2.4094074987036276,
-                    "heading": -0.1718069818405491,
-                    "angularVelocity": -1.0727851488087659,
-                    "velocityX": 3.0583616572737307,
-                    "velocityY": -1.0910527282945428,
-                    "timestamp": 1.5474328581594723
-                },
-                {
-                    "x": 2.5026677503931767,
-                    "y": 2.3827944816766236,
-                    "heading": -0.20403633414457553,
-                    "angularVelocity": -1.0410739741365005,
-                    "velocityX": 3.1724110164298485,
-                    "velocityY": -0.859654861819398,
-                    "timestamp": 1.5783906508843786
-                },
-                {
-                    "x": 2.6041682782784212,
-                    "y": 2.363451815109364,
-                    "heading": -0.23475758891885987,
-                    "angularVelocity": -0.9923593405260972,
-                    "velocityX": 3.2786745743372427,
-                    "velocityY": -0.6248076773199155,
-                    "timestamp": 1.6093484436092849
-                },
-                {
-                    "x": 2.7086523080371263,
-                    "y": 2.3514976196224144,
-                    "heading": -0.26331031649970843,
-                    "angularVelocity": -0.9223114784903548,
-                    "velocityX": 3.3750477847899405,
-                    "velocityY": -0.38614495527174003,
-                    "timestamp": 1.6403062363341911
-                },
-                {
-                    "x": 2.8157215031828033,
-                    "y": 2.3470577985799963,
-                    "heading": -0.288841887845313,
-                    "angularVelocity": -0.8247219551529203,
-                    "velocityX": 3.458553912306755,
-                    "velocityY": -0.14341529714262294,
-                    "timestamp": 1.6712640290590974
-                },
-                {
-                    "x": 2.9248447015312338,
-                    "y": 2.3502540561946077,
-                    "heading": -0.3102182612745416,
-                    "angularVelocity": -0.6905005668986338,
-                    "velocityX": 3.524902415281217,
-                    "velocityY": 0.10324565588123534,
-                    "timestamp": 1.7022218217840037
-                },
-                {
-                    "x": 3.0353304312162184,
-                    "y": 2.361176073397281,
-                    "heading": -0.326082506948615,
-                    "angularVelocity": -0.5124475706430824,
-                    "velocityX": 3.568914963243171,
-                    "velocityY": 0.3528034863374462,
-                    "timestamp": 1.73317961450891
-                },
-                {
-                    "x": 3.1466093544989446,
-                    "y": 2.379927120590619,
-                    "heading": -0.3369287204650216,
-                    "angularVelocity": -0.35035487229182166,
-                    "velocityX": 3.594536738181179,
-                    "velocityY": 0.605697161937562,
-                    "timestamp": 1.7641374072338163
-                },
-                {
-                    "x": 3.2578769593015027,
-                    "y": 2.406586601459548,
-                    "heading": -0.34366338052401463,
-                    "angularVelocity": -0.21754328930658381,
-                    "velocityX": 3.5941711281316278,
-                    "velocityY": 0.8611557389062998,
-                    "timestamp": 1.7950951999587226
-                },
-                {
-                    "x": 3.3679824864036325,
-                    "y": 2.4411213005139327,
-                    "heading": -0.34790283328354016,
-                    "angularVelocity": -0.13694299193573645,
-                    "velocityX": 3.556633642474382,
-                    "velocityY": 1.1155413876329803,
-                    "timestamp": 1.8260529926836289
-                },
-                {
-                    "x": 3.475304114496225,
-                    "y": 2.4831460278092354,
-                    "heading": -0.3524511595124565,
-                    "angularVelocity": -0.14692023650373393,
-                    "velocityX": 3.466708012625811,
-                    "velocityY": 1.3574846136083376,
-                    "timestamp": 1.8570107854085351
-                },
-                {
-                    "x": 3.5778542240444473,
-                    "y": 2.531447362567012,
-                    "heading": -0.3616437420636369,
-                    "angularVelocity": -0.29693921116020117,
-                    "velocityX": 3.312578207999158,
-                    "velocityY": 1.560231867531513,
-                    "timestamp": 1.8879685781334414
-                },
-                {
-                    "x": 3.673558919058804,
-                    "y": 2.583451056106446,
-                    "heading": -0.37745622232401294,
-                    "angularVelocity": -0.5107754419475534,
-                    "velocityX": 3.091457322713648,
-                    "velocityY": 1.6798256258655577,
-                    "timestamp": 1.9189263708583477
-                },
-                {
-                    "x": 3.7613706261508923,
-                    "y": 2.636186152260406,
-                    "heading": -0.3968957996596081,
-                    "angularVelocity": -0.6279380932955558,
-                    "velocityX": 2.836497675157173,
-                    "velocityY": 1.7034514257095354,
-                    "timestamp": 1.949884163583254
-                },
-                {
-                    "x": 3.841306735175595,
-                    "y": 2.6876448087218088,
-                    "heading": -0.4179459699473055,
-                    "angularVelocity": -0.6799635386030257,
-                    "velocityX": 2.582099755467769,
-                    "velocityY": 1.6622198138851267,
-                    "timestamp": 1.9808419563081603
-                },
-                {
-                    "x": 3.913645075614716,
-                    "y": 2.736629100397619,
-                    "heading": -0.43934657697326235,
-                    "angularVelocity": -0.6912833617387507,
-                    "velocityX": 2.336676296079001,
-                    "velocityY": 1.582292772328868,
-                    "timestamp": 2.0117997490330666
-                },
-                {
-                    "x": 3.9786691618750996,
-                    "y": 2.7823974078733573,
-                    "heading": -0.46027689817629036,
-                    "angularVelocity": -0.6760921681319296,
-                    "velocityX": 2.100410931692797,
-                    "velocityY": 1.4784099074015198,
-                    "timestamp": 2.0427575417579726
-                },
-                {
-                    "x": 4.036618763930541,
-                    "y": 2.824460782764613,
-                    "heading": -0.48017689659950147,
-                    "angularVelocity": -0.6428106357887643,
-                    "velocityX": 1.8718906276836884,
-                    "velocityY": 1.3587330099747585,
-                    "timestamp": 2.0737153344828787
-                },
-                {
-                    "x": 4.08768908254176,
-                    "y": 2.862477844590781,
-                    "heading": -0.49864492956893264,
-                    "angularVelocity": -0.5965552238942878,
-                    "velocityX": 1.6496757073449355,
-                    "velocityY": 1.2280288250525657,
-                    "timestamp": 2.1046731272077848
-                },
-                {
-                    "x": 4.132038835388808,
-                    "y": 2.8961988047734253,
-                    "heading": -0.5153800748468587,
-                    "angularVelocity": -0.5405794084679139,
-                    "velocityX": 1.4325876925792385,
-                    "velocityY": 1.0892559583398733,
-                    "timestamp": 2.135630919932691
-                },
-                {
-                    "x": 4.169798307734006,
-                    "y": 2.9254338939938918,
-                    "heading": -0.5301487101337384,
-                    "angularVelocity": -0.47705711511170307,
-                    "velocityX": 1.2197081581622198,
-                    "velocityY": 0.944353154640467,
-                    "timestamp": 2.166588712657597
-                },
-                {
-                    "x": 4.201075770562755,
-                    "y": 2.9500345091711524,
-                    "heading": -0.5427642633485259,
-                    "angularVelocity": -0.4075081620800149,
-                    "velocityX": 1.0103259979397883,
-                    "velocityY": 0.7946501675963605,
-                    "timestamp": 2.197546505382503
-                },
-                {
-                    "x": 4.225962326087245,
-                    "y": 2.9698813733488647,
-                    "heading": -0.5530743601373866,
-                    "angularVelocity": -0.3330372058810695,
-                    "velocityX": 0.8038866254348108,
-                    "velocityY": 0.6410942909939014,
-                    "timestamp": 2.228504298107409
-                },
-                {
-                    "x": 4.24453552816763,
-                    "y": 2.984876771487546,
-                    "heading": -0.5609523103628415,
-                    "angularVelocity": -0.2544738992160421,
-                    "velocityX": 0.5999524011761236,
-                    "velocityY": 0.48438201883216997,
-                    "timestamp": 2.259462090832315
-                },
-                {
-                    "x": 4.256862100646349,
-                    "y": 2.9949392682905573,
-                    "heading": -0.5662912533122896,
-                    "angularVelocity": -0.17245877305093363,
-                    "velocityX": 0.398173493448939,
-                    "velocityY": 0.32503922008988784,
-                    "timestamp": 2.290419883557221
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.569,
-                    "angularVelocity": -0.08749805620543083,
-                    "velocityX": 0.19826669841112912,
-                    "velocityY": 0.16347198117160036,
-                    "timestamp": 2.321377676282127
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.569,
-                    "angularVelocity": 1.3974941712649426e-29,
-                    "velocityX": 2.480011116993708e-30,
-                    "velocityY": -4.1979337006948246e-30,
-                    "timestamp": 2.3523354690070333
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "W1 To S4": {
-            "waypoints": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 28
-                },
-                {
-                    "x": 2.170377492904663,
-                    "y": 3.170344352722168,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 33
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.643,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 9.203343680058695e-35,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.298008161759761,
-                    "y": 6.742798044155305,
-                    "heading": 0.37730350882287184,
-                    "angularVelocity": -0.30162955292479776,
-                    "velocityX": -0.04731994607959398,
-                    "velocityY": -0.3373947598852993,
-                    "timestamp": 0.0420929947149233
-                },
-                {
-                    "x": 2.2940179124306126,
-                    "y": 6.714368498753073,
-                    "heading": 0.3522801711626307,
-                    "angularVelocity": -0.5944774856175671,
-                    "velocityX": -0.09479604281359731,
-                    "velocityY": -0.6753984978919232,
-                    "timestamp": 0.0841859894298466
-                },
-                {
-                    "x": 2.28802134748838,
-                    "y": 6.671681118579196,
-                    "heading": 0.31538217591497186,
-                    "angularVelocity": -0.8765828019021266,
-                    "velocityX": -0.14245992671333493,
-                    "velocityY": -1.014120769096589,
-                    "timestamp": 0.1262789841447699
-                },
-                {
-                    "x": 2.2800080756588827,
-                    "y": 6.61469814393844,
-                    "heading": 0.2672110996394858,
-                    "angularVelocity": -1.1443965106718308,
-                    "velocityX": -0.19037067530517543,
-                    "velocityY": -1.3537400944426796,
-                    "timestamp": 0.1683719788596932
-                },
-                {
-                    "x": 2.2699650128963897,
-                    "y": 6.543370292056319,
-                    "heading": 0.20862519634351256,
-                    "angularVelocity": -1.391820745773707,
-                    "velocityX": -0.23859226055333851,
-                    "velocityY": -1.694530226827328,
-                    "timestamp": 0.21046497357461652
-                },
-                {
-                    "x": 2.2578780077181495,
-                    "y": 6.4576319980717,
-                    "heading": 0.14091301714440063,
-                    "angularVelocity": -1.608632972248606,
-                    "velocityX": -0.2871500414760241,
-                    "velocityY": -2.036877978516017,
-                    "timestamp": 0.2525579682895398
-                },
-                {
-                    "x": 2.2437361567314555,
-                    "y": 6.357398182016401,
-                    "heading": 0.06608849726258184,
-                    "angularVelocity": -1.7776002964049293,
-                    "velocityX": -0.3359668534508049,
-                    "velocityY": -2.3812469683883926,
-                    "timestamp": 0.2946509630044631
-                },
-                {
-                    "x": 2.227539619778089,
-                    "y": 6.242572554004172,
-                    "heading": -0.012517898946656567,
-                    "angularVelocity": -1.8674460380308828,
-                    "velocityX": -0.3847798680768185,
-                    "velocityY": -2.727903509595609,
-                    "timestamp": 0.33674395771938637
-                },
-                {
-                    "x": 2.209319968778144,
-                    "y": 6.1131309245806085,
-                    "heading": -0.08849823738903302,
-                    "angularVelocity": -1.8050589880087344,
-                    "velocityX": -0.4328428310539146,
-                    "velocityY": -3.0751347177888695,
-                    "timestamp": 0.37883695243430965
-                },
-                {
-                    "x": 2.189363625025874,
-                    "y": 5.970570114356374,
-                    "heading": -0.13907359717832485,
-                    "angularVelocity": -1.201514887021361,
-                    "velocityX": -0.4741013056311528,
-                    "velocityY": -3.38680607520881,
-                    "timestamp": 0.42092994714923293
-                },
-                {
-                    "x": 2.1674827409245396,
-                    "y": 5.816618965425972,
-                    "heading": -0.15173248317406665,
-                    "angularVelocity": -0.3007361695568274,
-                    "velocityX": -0.5198224609468644,
-                    "velocityY": -3.657405465518488,
-                    "timestamp": 0.4630229418641562
-                },
-                {
-                    "x": 2.1445157307124236,
-                    "y": 5.659048701686038,
-                    "heading": -0.1517325271793822,
-                    "angularVelocity": -0.0000010454308573871989,
-                    "velocityX": -0.5456254744443967,
-                    "velocityY": -3.7433844944292183,
-                    "timestamp": 0.5051159365790795
-                },
-                {
-                    "x": 2.1215487153797565,
-                    "y": 5.501478438692377,
-                    "heading": -0.1517325711844689,
-                    "angularVelocity": -0.0000010454254199880688,
-                    "velocityX": -0.5456255960929445,
-                    "velocityY": -3.7433844767000393,
-                    "timestamp": 0.5472089312940028
-                },
-                {
-                    "x": 2.0985817251133763,
-                    "y": 5.343908172045118,
-                    "heading": -0.15173261518953962,
-                    "angularVelocity": -0.000001045425040533604,
-                    "velocityX": -0.5456250005951166,
-                    "velocityY": -3.7433845634983034,
-                    "timestamp": 0.5893019260089262
-                },
-                {
-                    "x": 2.075614762728362,
-                    "y": 5.186337901333946,
-                    "heading": -0.15173265919459192,
-                    "angularVelocity": -0.0000010454246033549915,
-                    "velocityX": -0.5456243382196686,
-                    "velocityY": -3.743384660044355,
-                    "timestamp": 0.6313949207238495
-                },
-                {
-                    "x": 2.0526478312972185,
-                    "y": 5.028767626111028,
-                    "heading": -0.15173270319962293,
-                    "angularVelocity": -0.0000010454240970480152,
-                    "velocityX": -0.5456236028509798,
-                    "velocityY": -3.7433847672295513,
-                    "timestamp": 0.6734879154387728
-                },
-                {
-                    "x": 2.0296809339445536,
-                    "y": 4.871197345920939,
-                    "heading": -0.15173274720462945,
-                    "angularVelocity": -0.0000010454235162635837,
-                    "velocityX": -0.5456227932512114,
-                    "velocityY": -3.743384885234266,
-                    "timestamp": 0.7155809101536962
-                },
-                {
-                    "x": 2.0067140734575584,
-                    "y": 4.7136270603574335,
-                    "heading": -0.15173279120960867,
-                    "angularVelocity": -0.0000010454228670132247,
-                    "velocityX": -0.5456219174363699,
-                    "velocityY": -3.7433850128900636,
-                    "timestamp": 0.7576739048686195
-                },
-                {
-                    "x": 1.983747251812091,
-                    "y": 4.55605676913253,
-                    "heading": -0.15173283521455908,
-                    "angularVelocity": -0.000001045422182833939,
-                    "velocityX": -0.545620994681206,
-                    "velocityY": -3.743385147387475,
-                    "timestamp": 0.7997668995835429
-                },
-                {
-                    "x": 1.9607804702256346,
-                    "y": 4.398486472068785,
-                    "heading": -0.15173287921948736,
-                    "angularVelocity": -0.0000010454216570158517,
-                    "velocityX": -0.5456200430024101,
-                    "velocityY": -3.7433852861004024,
-                    "timestamp": 0.8418598942984662
-                },
-                {
-                    "x": 1.9378163633559966,
-                    "y": 4.240915785172817,
-                    "heading": -0.15173292322446258,
-                    "angularVelocity": -0.0000010454227717672262,
-                    "velocityX": -0.5455564999630786,
-                    "velocityY": -3.7433945473141725,
-                    "timestamp": 0.8839528890133895
-                },
-                {
-                    "x": 1.9275487572437193,
-                    "y": 4.082011890541997,
-                    "heading": -0.15173296776197548,
-                    "angularVelocity": -0.000001058074228463605,
-                    "velocityX": -0.2439267194414494,
-                    "velocityY": -3.775067459728259,
-                    "timestamp": 0.9260458837283129
-                },
-                {
-                    "x": 1.9320519221515333,
-                    "y": 3.9228403083205765,
-                    "heading": -0.15173301420750987,
-                    "angularVelocity": -0.0000011034029462076059,
-                    "velocityX": 0.10698133830372927,
-                    "velocityY": -3.781426892988182,
-                    "timestamp": 0.9681388784432362
-                },
-                {
-                    "x": 1.9512874233679367,
-                    "y": 3.764771127717043,
-                    "heading": -0.15173306433513473,
-                    "angularVelocity": -0.0000011908780826817618,
-                    "velocityX": 0.45697630559851177,
-                    "velocityY": -3.7552372235348,
-                    "timestamp": 1.0102318731581594
-                },
-                {
-                    "x": 1.9850897622274482,
-                    "y": 3.609164986787025,
-                    "heading": -0.15173312063283023,
-                    "angularVelocity": -0.0000013374599716310608,
-                    "velocityX": 0.8030395339756486,
-                    "velocityY": -3.6967229816711127,
-                    "timestamp": 1.0523248678730828
-                },
-                {
-                    "x": 2.0331680016062417,
-                    "y": 3.4573613296965586,
-                    "heading": -0.1517331870158403,
-                    "angularVelocity": -0.0000015770560044658274,
-                    "velocityX": 1.142190991741159,
-                    "velocityY": -3.606387668982997,
-                    "timestamp": 1.094417862588006
-                },
-                {
-                    "x": 2.0951083019579966,
-                    "y": 3.3106668726225337,
-                    "heading": -0.15173327041919607,
-                    "angularVelocity": -0.0000019814070334134687,
-                    "velocityX": 1.4715108956074094,
-                    "velocityY": -3.4850088017619067,
-                    "timestamp": 1.1365108573029294
-                },
-                {
-                    "x": 2.170377492904663,
-                    "y": 3.170344352722168,
-                    "heading": -0.1517333779044128,
-                    "angularVelocity": -0.000002553517929494838,
-                    "velocityX": 1.7881643122906952,
-                    "velocityY": -3.333631186155945,
-                    "timestamp": 1.1786038520178528
-                },
-                {
-                    "x": 2.2421653916580504,
-                    "y": 3.058317386598826,
-                    "heading": -0.1517334835913584,
-                    "angularVelocity": -0.0000030048345176781924,
-                    "velocityX": 2.041035009880172,
-                    "velocityY": -3.1850905776457417,
-                    "timestamp": 1.213776153524627
-                },
-                {
-                    "x": 2.322415894828655,
-                    "y": 2.9521882028356305,
-                    "heading": -0.15173356325163515,
-                    "angularVelocity": -0.000002264858236060329,
-                    "velocityX": 2.2816392369190814,
-                    "velocityY": -3.0174079948323826,
-                    "timestamp": 1.2489484550314012
-                },
-                {
-                    "x": 2.4106466853387967,
-                    "y": 2.8525946456824203,
-                    "heading": -0.1517336276365959,
-                    "angularVelocity": -0.0000018305586497777263,
-                    "velocityX": 2.5085304836576707,
-                    "velocityY": -2.8315905666289245,
-                    "timestamp": 1.2841207565381754
-                },
-                {
-                    "x": 2.5063274836460994,
-                    "y": 2.7601352747554455,
-                    "heading": -0.15173368243148194,
-                    "angularVelocity": -0.0000015578987915956282,
-                    "velocityX": 2.720345107040385,
-                    "velocityY": -2.628755212654101,
-                    "timestamp": 1.3192930580449496
-                },
-                {
-                    "x": 2.6088832332920924,
-                    "y": 2.67536576592715,
-                    "heading": -0.15173373101035148,
-                    "angularVelocity": -0.000001381168347773627,
-                    "velocityX": 2.915810033819959,
-                    "velocityY": -2.410121180496788,
-                    "timestamp": 1.3544653595517238
-                },
-                {
-                    "x": 2.7176975542058304,
-                    "y": 2.598795565150263,
-                    "heading": -0.1517337755722331,
-                    "angularVelocity": -0.0000012669595030854418,
-                    "velocityX": 3.0937503732242226,
-                    "velocityY": -2.1770028544233733,
-                    "timestamp": 1.389637661058498
-                },
-                {
-                    "x": 2.8321164386506434,
-                    "y": 2.5308848045360426,
-                    "heading": -0.15173381767917685,
-                    "angularVelocity": -0.0000011971620259359605,
-                    "velocityX": 3.2530963156555406,
-                    "velocityY": -1.930802299108592,
-                    "timestamp": 1.4248099625652721
-                },
-                {
-                    "x": 2.951452140465328,
-                    "y": 2.4720414272198683,
-                    "heading": -0.15173385854226368,
-                    "angularVelocity": -0.0000011617973536146762,
-                    "velocityX": 3.3928886283344455,
-                    "velocityY": -1.6730033235055968,
-                    "timestamp": 1.4599822640720463
-                },
-                {
-                    "x": 3.0749866313948626,
-                    "y": 2.4226169148084327,
-                    "heading": -0.151733899192453,
-                    "angularVelocity": -0.000001155744366153098,
-                    "velocityX": 3.512266347021429,
-                    "velocityY": -1.4052112114959712,
-                    "timestamp": 1.4951545655788205
-                },
-                {
-                    "x": 3.1992028583958065,
-                    "y": 2.3749316777596947,
-                    "heading": -0.1517339398689439,
-                    "angularVelocity": -0.0000011564921588237654,
-                    "velocityX": 3.5316491011263524,
-                    "velocityY": -1.3557610678264387,
-                    "timestamp": 1.5303268670855947
-                },
-                {
-                    "x": 3.3234191450837622,
-                    "y": 2.3272465961911335,
-                    "heading": -0.15173398054544393,
-                    "angularVelocity": -0.0000011564924185481092,
-                    "velocityX": 3.5316507981154315,
-                    "velocityY": -1.355756647297499,
-                    "timestamp": 1.565499168592369
-                },
-                {
-                    "x": 3.4476354415156054,
-                    "y": 2.2795615400047207,
-                    "heading": -0.15173402122195195,
-                    "angularVelocity": -0.0000011564926460273957,
-                    "velocityX": 3.5316510751484063,
-                    "velocityY": -1.3557559256458895,
-                    "timestamp": 1.6006714700991431
-                },
-                {
-                    "x": 3.57185174767356,
-                    "y": 2.231876509154167,
-                    "heading": -0.15173406189847174,
-                    "angularVelocity": -0.0000011564929795008858,
-                    "velocityX": 3.531651351675977,
-                    "velocityY": -1.355755205310334,
-                    "timestamp": 1.6358437716059173
-                },
-                {
-                    "x": 3.6960680635749434,
-                    "y": 2.1841915036846027,
-                    "heading": -0.15173410257500647,
-                    "angularVelocity": -0.000001156493404477995,
-                    "velocityX": 3.5316516286959314,
-                    "velocityY": -1.3557544836916666,
-                    "timestamp": 1.6710160731126915
-                },
-                {
-                    "x": 3.820284389246338,
-                    "y": 2.136506523665285,
-                    "heading": -0.15173414325155568,
-                    "angularVelocity": -0.0000011564938164652249,
-                    "velocityX": 3.5316519064716276,
-                    "velocityY": -1.3557537601039011,
-                    "timestamp": 1.7061883746194657
-                },
-                {
-                    "x": 3.9445007247793735,
-                    "y": 2.0888215693349235,
-                    "heading": -0.15173418392811813,
-                    "angularVelocity": -0.0000011564941935950735,
-                    "velocityX": 3.5316521868525257,
-                    "velocityY": -1.3557530297293134,
-                    "timestamp": 1.74136067612624
-                },
-                {
-                    "x": 4.068717070394662,
-                    "y": 2.0411366412682117,
-                    "heading": -0.1517342246046924,
-                    "angularVelocity": -0.0000011564945294151802,
-                    "velocityX": 3.531652473505735,
-                    "velocityY": -1.3557522830153055,
-                    "timestamp": 1.776532977633014
-                },
-                {
-                    "x": 4.192933426524707,
-                    "y": 1.9934517405918115,
-                    "heading": -0.15173426528127656,
-                    "angularVelocity": -0.0000011564948108738022,
-                    "velocityX": 3.5316527724556495,
-                    "velocityY": -1.3557515042686648,
-                    "timestamp": 1.8117052791397883
-                },
-                {
-                    "x": 4.3171498402859925,
-                    "y": 1.9457669900421575,
-                    "heading": -0.1517343059583456,
-                    "angularVelocity": -0.0000011565085966842365,
-                    "velocityX": 3.531654410996122,
-                    "velocityY": -1.3557472359455842,
-                    "timestamp": 1.8468775806465625
-                },
-                {
-                    "x": 4.438199866372425,
-                    "y": 1.9006463371040783,
-                    "heading": -0.16306820061092758,
-                    "angularVelocity": -0.3222392100329015,
-                    "velocityX": 3.441629375976964,
-                    "velocityY": -1.282846188765579,
-                    "timestamp": 1.8820498821533367
-                },
-                {
-                    "x": 4.551333178258229,
-                    "y": 1.8581105785728176,
-                    "heading": -0.19901052601489952,
-                    "angularVelocity": -1.0218929061849813,
-                    "velocityX": 3.2165456066050515,
-                    "velocityY": -1.2093538582645278,
-                    "timestamp": 1.9172221836601109
-                },
-                {
-                    "x": 4.6559660625940325,
-                    "y": 1.8184646011809935,
-                    "heading": -0.2548690023306322,
-                    "angularVelocity": -1.5881382202121255,
-                    "velocityX": 2.9748660125539748,
-                    "velocityY": -1.1271931518097666,
-                    "timestamp": 1.952394485166885
-                },
-                {
-                    "x": 4.750968691238499,
-                    "y": 1.7823676048635404,
-                    "heading": -0.3144227593810797,
-                    "angularVelocity": -1.693200458860429,
-                    "velocityX": 2.7010637511499986,
-                    "velocityY": -1.0262904265875403,
-                    "timestamp": 1.9875667866736593
-                },
-                {
-                    "x": 4.836310937621718,
-                    "y": 1.749878315803868,
-                    "heading": -0.37309459063768413,
-                    "angularVelocity": -1.6681260180060764,
-                    "velocityX": 2.426404947278819,
-                    "velocityY": -0.9237180300360781,
-                    "timestamp": 2.0227390881804337
-                },
-                {
-                    "x": 4.912035358987092,
-                    "y": 1.7210053742684595,
-                    "heading": -0.4285254423369764,
-                    "angularVelocity": -1.5759802266171352,
-                    "velocityX": 2.1529561081121917,
-                    "velocityY": -0.8208999780650579,
-                    "timestamp": 2.057911389687208
-                },
-                {
-                    "x": 4.978188578205751,
-                    "y": 1.6957473341798728,
-                    "heading": -0.4792367761737124,
-                    "angularVelocity": -1.4417974276425698,
-                    "velocityX": 1.8808328253957978,
-                    "velocityY": -0.71812304019178,
-                    "timestamp": 2.0930836911939825
-                },
-                {
-                    "x": 5.034811621143831,
-                    "y": 1.6741013653395114,
-                    "heading": -0.5242175587960242,
-                    "angularVelocity": -1.278869471013956,
-                    "velocityX": 1.6098759680874073,
-                    "velocityY": -0.6154265690060781,
-                    "timestamp": 2.128255992700757
-                },
-                {
-                    "x": 5.081938937252143,
-                    "y": 1.656065298518901,
-                    "heading": -0.5627406592667319,
-                    "angularVelocity": -1.0952681178195878,
-                    "velocityX": 1.3398985590759445,
-                    "velocityY": -0.5127917721601607,
-                    "timestamp": 2.1634282942075314
-                },
-                {
-                    "x": 5.11959917834615,
-                    "y": 1.6416378108353973,
-                    "heading": -0.5942629145071194,
-                    "angularVelocity": -0.8962238434785516,
-                    "velocityX": 1.0707357631047747,
-                    "velocityY": -0.41019458680362747,
-                    "timestamp": 2.1986005957143058
-                },
-                {
-                    "x": 5.147816310703936,
-                    "y": 1.63081813815951,
-                    "heading": -0.6183637008514038,
-                    "angularVelocity": -0.6852206228143011,
-                    "velocityX": 0.8022543634897267,
-                    "velocityY": -0.3076191267666608,
-                    "timestamp": 2.23377289722108
-                },
-                {
-                    "x": 5.1666106911988186,
-                    "y": 1.6236057376407542,
-                    "heading": -0.6347043261398958,
-                    "angularVelocity": -0.4645878884367218,
-                    "velocityX": 0.5343517395716112,
-                    "velocityY": -0.20505910076332604,
-                    "timestamp": 2.2689451987278546
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.643,
-                    "angularVelocity": -0.23585814702817923,
-                    "velocityX": 0.26695178873562564,
-                    "velocityY": -0.10251639745723456,
-                    "timestamp": 2.304117500234629
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.643,
-                    "angularVelocity": -2.979902278450704e-33,
-                    "velocityX": -7.596111021745797e-35,
-                    "velocityY": -9.748244176889607e-35,
-                    "timestamp": 2.3392898017414034
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "W1 To C1": {
-            "waypoints": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 9
-                },
-                {
-                    "x": 2.916773557662964,
-                    "y": 6.303858757019043,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 18
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": -0.003,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "angularVelocity": 4.0882515102287785e-31,
-                    "velocityX": 3.918479950533569e-32,
-                    "velocityY": 1.7984537919855076e-32,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.31173856270728,
-                    "y": 6.7441134449795,
-                    "heading": 0.37806624882590184,
-                    "angularVelocity": -0.25868952140989293,
-                    "velocityX": 0.2544583948907238,
-                    "velocityY": -0.279343577911277,
-                    "timestamp": 0.04613155998378907
-                },
-                {
-                    "x": 2.335747257207826,
-                    "y": 6.718800139650514,
-                    "heading": 0.35485995839909595,
-                    "angularVelocity": -0.5030458635034395,
-                    "velocityX": 0.5204396840033778,
-                    "velocityY": -0.5487199075401175,
-                    "timestamp": 0.09226311996757813
-                },
-                {
-                    "x": 2.3726756647692695,
-                    "y": 6.681679707204912,
-                    "heading": 0.3212123448310367,
-                    "angularVelocity": -0.7293838226993238,
-                    "velocityX": 0.8005020331941938,
-                    "velocityY": -0.8046645823086491,
-                    "timestamp": 0.1383946799513672
-                },
-                {
-                    "x": 2.4233218216100387,
-                    "y": 6.6336167386650216,
-                    "heading": 0.2781690598764637,
-                    "angularVelocity": -0.9330550488580632,
-                    "velocityX": 1.097863520300784,
-                    "velocityY": -1.0418674017696439,
-                    "timestamp": 0.18452623993515627
-                },
-                {
-                    "x": 2.488661485197386,
-                    "y": 6.57586429791715,
-                    "heading": 0.22709762602154188,
-                    "angularVelocity": -1.1070823070554872,
-                    "velocityX": 1.416376632620007,
-                    "velocityY": -1.2519073876575257,
-                    "timestamp": 0.23065779991894533
-                },
-                {
-                    "x": 2.5698490572295953,
-                    "y": 6.510313323679299,
-                    "heading": 0.16995873595290442,
-                    "angularVelocity": -1.2386073674663585,
-                    "velocityX": 1.7599138650576658,
-                    "velocityY": -1.4209572418727212,
-                    "timestamp": 0.2767893599027344
-                },
-                {
-                    "x": 2.6680837365477243,
-                    "y": 6.439909324941273,
-                    "heading": 0.10980534967726638,
-                    "angularVelocity": -1.3039530052045993,
-                    "velocityX": 2.1294462912732417,
-                    "velocityY": -1.5261569034900655,
-                    "timestamp": 0.3229209198865235
-                },
-                {
-                    "x": 2.784053925984014,
-                    "y": 6.36914274527536,
-                    "heading": 0.05120415857101511,
-                    "angularVelocity": -1.2703058627725585,
-                    "velocityX": 2.5139013178189233,
-                    "velocityY": -1.5340166187915734,
-                    "timestamp": 0.36905247987031253
-                },
-                {
-                    "x": 2.916773557662964,
-                    "y": 6.303858757019043,
-                    "heading": 0,
-                    "angularVelocity": -1.1099593984900726,
-                    "velocityX": 2.8769812190523862,
-                    "velocityY": -1.4151697510176988,
-                    "timestamp": 0.41518403985410157
-                },
-                {
-                    "x": 3.221120615978503,
-                    "y": 6.218848325437687,
-                    "heading": -0.053482212757092314,
-                    "angularVelocity": -0.6045874919246804,
-                    "velocityX": 3.4404789027216576,
-                    "velocityY": -0.9609969552052925,
-                    "timestamp": 0.5036447067905502
-                },
-                {
-                    "x": 3.554544204530565,
-                    "y": 6.190324835643033,
-                    "heading": -0.053482256222138806,
-                    "angularVelocity": -4.913488445928212e-7,
-                    "velocityX": 3.769173352396249,
-                    "velocityY": -0.32244262656470185,
-                    "timestamp": 0.5921053737269988
-                },
-                {
-                    "x": 3.8871649424892416,
-                    "y": 6.227044557920147,
-                    "heading": -0.05348225420791585,
-                    "angularVelocity": 2.276970118304524e-8,
-                    "velocityX": 3.760097560621335,
-                    "velocityY": 0.41509660223897277,
-                    "timestamp": 0.6805660406634474
-                },
-                {
-                    "x": 4.206338511891951,
-                    "y": 6.327608602444822,
-                    "heading": -0.05348225236183454,
-                    "angularVelocity": 2.0868950806255528e-8,
-                    "velocityX": 3.608084592353424,
-                    "velocityY": 1.1368221381026216,
-                    "timestamp": 0.769026707599896
-                },
-                {
-                    "x": 4.5243112033895745,
-                    "y": 6.4319075091933104,
-                    "heading": -0.05348225052024108,
-                    "angularVelocity": 2.0818218151781936e-8,
-                    "velocityX": 3.5945093170737707,
-                    "velocityY": 1.1790427357213897,
-                    "timestamp": 0.8574873745363446
-                },
-                {
-                    "x": 4.842283928689835,
-                    "y": 6.536206312888755,
-                    "heading": -0.053482248678638385,
-                    "angularVelocity": 2.0818322537631325e-8,
-                    "velocityX": 3.59450969919429,
-                    "velocityY": 1.1790415707624449,
-                    "timestamp": 0.9459480414727932
-                },
-                {
-                    "x": 5.160256832233298,
-                    "y": 6.640504573177801,
-                    "heading": -0.05348224683713029,
-                    "angularVelocity": 2.0817253121924564e-8,
-                    "velocityX": 3.5945117141373055,
-                    "velocityY": 1.1790354278469999,
-                    "timestamp": 1.034408708409242
-                },
-                {
-                    "x": 5.478229837776281,
-                    "y": 6.744802522501547,
-                    "heading": -0.05348224499575697,
-                    "angularVelocity": 2.081572963279097e-8,
-                    "velocityX": 3.594512867186711,
-                    "velocityY": 1.1790319125524373,
-                    "timestamp": 1.1228693753456906
-                },
-                {
-                    "x": 5.79620288183899,
-                    "y": 6.849100354390164,
-                    "heading": -0.05348224315440185,
-                    "angularVelocity": 2.0815523766741823e-8,
-                    "velocityX": 3.5945133026313907,
-                    "velocityY": 1.1790305850118254,
-                    "timestamp": 1.2113300422821394
-                },
-                {
-                    "x": 6.114175956134068,
-                    "y": 6.9533980941092235,
-                    "heading": -0.053482241313040024,
-                    "angularVelocity": 2.081559959710574e-8,
-                    "velocityX": 3.5945136443919665,
-                    "velocityY": 1.179029543084821,
-                    "timestamp": 1.299790709218588
-                },
-                {
-                    "x": 6.432149055252676,
-                    "y": 7.057695758148605,
-                    "heading": -0.05348223947166161,
-                    "angularVelocity": 2.0815787162382163e-8,
-                    "velocityX": 3.594513925008545,
-                    "velocityY": 1.1790286875669842,
-                    "timestamp": 1.3882513761550368
-                },
-                {
-                    "x": 6.750122174049358,
-                    "y": 7.16199336219523,
-                    "heading": -0.05348223763026628,
-                    "angularVelocity": 2.081597839391937e-8,
-                    "velocityX": 3.594514147458541,
-                    "velocityY": 1.1790280093813261,
-                    "timestamp": 1.4767120430914855
-                },
-                {
-                    "x": 7.060081426834384,
-                    "y": 7.263662245896702,
-                    "heading": -0.036655668946107355,
-                    "angularVelocity": 0.1902152591303347,
-                    "velocityX": 3.503921726112515,
-                    "velocityY": 1.1493117474969148,
-                    "timestamp": 1.5651727100279342
-                },
-                {
-                    "x": 7.308048847978653,
-                    "y": 7.344997351864717,
-                    "heading": -0.023193676123292754,
-                    "angularVelocity": 0.15218054858760893,
-                    "velocityX": 2.8031375947279678,
-                    "velocityY": 0.9194493867703608,
-                    "timestamp": 1.653633376964383
-                },
-                {
-                    "x": 7.4940244209186755,
-                    "y": 7.405998678341617,
-                    "heading": -0.013096928746230797,
-                    "angularVelocity": 0.11413826875525969,
-                    "velocityX": 2.102353276102148,
-                    "velocityY": 0.6895870061744361,
-                    "timestamp": 1.7420940439008317
-                },
-                {
-                    "x": 7.618008139456229,
-                    "y": 7.4466662269505,
-                    "heading": -0.006365663946800556,
-                    "angularVelocity": 0.07609330827525898,
-                    "velocityX": 1.4015688874087386,
-                    "velocityY": 0.4597246439267693,
-                    "timestamp": 1.8305547108372804
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": -0.003,
-                    "angularVelocity": 0.038047010760369614,
-                    "velocityX": 0.7007844581174931,
-                    "velocityY": 0.2298623077769412,
-                    "timestamp": 1.919015377773729
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": -0.003,
-                    "angularVelocity": -2.311552385941746e-29,
-                    "velocityX": 0,
-                    "velocityY": -6.724711823689692e-32,
-                    "timestamp": 2.007476044710178
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.49965229137145656
-                }
-            ]
-        },
-        "W1 To C2": {
-            "waypoints": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 9
-                },
-                {
-                    "x": 2.996485471725464,
-                    "y": 6.224146842956543,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 14
-                },
-                {
-                    "x": 6.113985538482666,
-                    "y": 7.0125651359558105,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 10
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": -0.003,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.3119316870686255,
-                    "y": 6.7398994499669,
-                    "heading": 0.3779608263213178,
-                    "angularVelocity": -0.23922354190742906,
-                    "velocityX": 0.23708773688854562,
-                    "velocityY": -0.3397952597632521,
-                    "timestamp": 0.05032604058401226
-                },
-                {
-                    "x": 2.3368398722232935,
-                    "y": 6.706448700500007,
-                    "heading": 0.3544233266085098,
-                    "angularVelocity": -0.4677002092687843,
-                    "velocityX": 0.49493631657919795,
-                    "velocityY": -0.6646807314605355,
-                    "timestamp": 0.10065208116802452
-                },
-                {
-                    "x": 2.3760366485894098,
-                    "y": 6.657739626207859,
-                    "heading": 0.32015686455898285,
-                    "angularVelocity": -0.6808892901544442,
-                    "velocityX": 0.7788567491352839,
-                    "velocityY": -0.9678701866250936,
-                    "timestamp": 0.1509781217520368
-                },
-                {
-                    "x": 2.431161593733442,
-                    "y": 6.595431091068774,
-                    "heading": 0.27624213154004224,
-                    "angularVelocity": -0.8726045703046355,
-                    "velocityX": 1.0953562907856245,
-                    "velocityY": -1.2380973034243006,
-                    "timestamp": 0.20130416233604903
-                },
-                {
-                    "x": 2.504176998220011,
-                    "y": 6.522134358575033,
-                    "heading": 0.22423189981090158,
-                    "angularVelocity": -1.0334656000271036,
-                    "velocityX": 1.450847387143028,
-                    "velocityY": -1.456437495244302,
-                    "timestamp": 0.2516302029200613
-                },
-                {
-                    "x": 2.597103761438428,
-                    "y": 6.441982348073212,
-                    "heading": 0.16656086207364196,
-                    "angularVelocity": -1.1459482420634122,
-                    "velocityX": 1.8464946206783215,
-                    "velocityY": -1.5926548079620442,
-                    "timestamp": 0.3019562435040735
-                },
-                {
-                    "x": 2.711127221877276,
-                    "y": 6.361040590595544,
-                    "heading": 0.10700626596614256,
-                    "angularVelocity": -1.1833753543173746,
-                    "velocityX": 2.265695038108593,
-                    "velocityY": -1.6083474189181441,
-                    "timestamp": 0.35228228408808576
-                },
-                {
-                    "x": 2.8452820247148356,
-                    "y": 6.2864334030632,
-                    "heading": 0.05015861793730577,
-                    "angularVelocity": -1.1295871355889644,
-                    "velocityX": 2.6657134414062895,
-                    "velocityY": -1.482476798622636,
-                    "timestamp": 0.402608324672098
-                },
-                {
-                    "x": 2.996485471725464,
-                    "y": 6.224146842956543,
-                    "heading": 0,
-                    "angularVelocity": -0.9966732402395141,
-                    "velocityX": 3.004477309479889,
-                    "velocityY": -1.237660650109664,
-                    "timestamp": 0.45293436525611025
-                },
-                {
-                    "x": 3.2087154283406467,
-                    "y": 6.171188923340848,
-                    "heading": -0.04850064735870574,
-                    "angularVelocity": -0.763447552582083,
-                    "velocityX": 3.340706769624593,
-                    "velocityY": -0.8336093706421461,
-                    "timestamp": 0.5164628253247648
-                },
-                {
-                    "x": 3.43894020563635,
-                    "y": 6.146133056520132,
-                    "heading": -0.0773967080078905,
-                    "angularVelocity": -0.45485221297638595,
-                    "velocityX": 3.623962819921881,
-                    "velocityY": -0.3944038119865702,
-                    "timestamp": 0.5799912853934195
-                },
-                {
-                    "x": 3.6791864985656395,
-                    "y": 6.1522055528567385,
-                    "heading": -0.07740096735197836,
-                    "angularVelocity": -0.00006704623623761945,
-                    "velocityX": 3.781711262474177,
-                    "velocityY": 0.09558702241564195,
-                    "timestamp": 0.6435197454620741
-                },
-                {
-                    "x": 3.9162306085794563,
-                    "y": 6.191776893986129,
-                    "heading": -0.0774009621424873,
-                    "angularVelocity": 8.200247652314305e-8,
-                    "velocityX": 3.731305776303155,
-                    "velocityY": 0.6228915526463914,
-                    "timestamp": 0.7070482055307288
-                },
-                {
-                    "x": 4.1454234559206995,
-                    "y": 6.264071031070342,
-                    "heading": -0.0774009579813014,
-                    "angularVelocity": 6.550112963869378e-8,
-                    "velocityX": 3.607719234710775,
-                    "velocityY": 1.1379803163195765,
-                    "timestamp": 0.7705766655993834
-                },
-                {
-                    "x": 4.362271301821872,
-                    "y": 6.367670338469585,
-                    "heading": -0.07740095431740261,
-                    "angularVelocity": 5.7673344791685836e-8,
-                    "velocityX": 3.4133968565714166,
-                    "velocityY": 1.630754268044481,
-                    "timestamp": 0.8341051256680381
-                },
-                {
-                    "x": 4.562525576285781,
-                    "y": 6.500538810004899,
-                    "heading": -0.07740095080920495,
-                    "angularVelocity": 5.522245710837058e-8,
-                    "velocityX": 3.152197837748554,
-                    "velocityY": 2.091479494256989,
-                    "timestamp": 0.8976335857366927
-                },
-                {
-                    "x": 4.76192015617459,
-                    "y": 6.634693969014619,
-                    "heading": -0.07740094730433215,
-                    "angularVelocity": 5.517012038625121e-8,
-                    "velocityX": 3.138665405604548,
-                    "velocityY": 2.111733211614557,
-                    "timestamp": 0.9611620458053474
-                },
-                {
-                    "x": 4.961315208646695,
-                    "y": 6.76884842562192,
-                    "heading": -0.07740094379946605,
-                    "angularVelocity": 5.5170015187013835e-8,
-                    "velocityX": 3.138672844527154,
-                    "velocityY": 2.1117221551145704,
-                    "timestamp": 1.024690505874002
-                },
-                {
-                    "x": 5.173514155427892,
-                    "y": 6.881664257521493,
-                    "heading": -0.07740094020412008,
-                    "angularVelocity": 5.659425663991619e-8,
-                    "velocityX": 3.3402186445551516,
-                    "velocityY": 1.7758313640477164,
-                    "timestamp": 1.0882189659426564
-                },
-                {
-                    "x": 5.399391012764095,
-                    "y": 6.963734040630747,
-                    "heading": -0.07740093622781413,
-                    "angularVelocity": 6.259093865144696e-8,
-                    "velocityX": 3.5555223138118204,
-                    "velocityY": 1.2918585311301378,
-                    "timestamp": 1.151747426011311
-                },
-                {
-                    "x": 5.634517116552883,
-                    "y": 7.013448397757994,
-                    "heading": -0.07740093140263109,
-                    "angularVelocity": 7.595309327466021e-8,
-                    "velocityX": 3.7011144852982407,
-                    "velocityY": 0.7825525295830946,
-                    "timestamp": 1.2152758860799655
-                },
-                {
-                    "x": 5.874282332827973,
-                    "y": 7.02983251834796,
-                    "heading": -0.07740092481326658,
-                    "angularVelocity": 1.0372303211074395e-7,
-                    "velocityX": 3.7741386461434376,
-                    "velocityY": 0.2579020579478282,
-                    "timestamp": 1.27880434614862
-                },
-                {
-                    "x": 6.113985538482666,
-                    "y": 7.0125651359558105,
-                    "heading": -0.07740091266766538,
-                    "angularVelocity": 1.9118362343988029e-7,
-                    "velocityX": 3.773162538422118,
-                    "velocityY": -0.27180546126086014,
-                    "timestamp": 1.3423328062172746
-                },
-                {
-                    "x": 6.409905183229014,
-                    "y": 6.936971954690801,
-                    "heading": -0.07740090470499697,
-                    "angularVelocity": 9.862508855382814e-8,
-                    "velocityX": 3.66524129970236,
-                    "velocityY": -0.9362921822438863,
-                    "timestamp": 1.4230695491897984
-                },
-                {
-                    "x": 6.687740416068466,
-                    "y": 6.8101241855802925,
-                    "heading": -0.0774008999193729,
-                    "angularVelocity": 5.9274425678096215e-8,
-                    "velocityX": 3.441248960637469,
-                    "velocityY": -1.5711281436467635,
-                    "timestamp": 1.5038062921623223
-                },
-                {
-                    "x": 6.938692776760068,
-                    "y": 6.63603889201252,
-                    "heading": -0.0774008963221294,
-                    "angularVelocity": 4.455522177089406e-8,
-                    "velocityX": 3.108279470438973,
-                    "velocityY": -2.156209021548162,
-                    "timestamp": 1.5845430351348462
-                },
-                {
-                    "x": 7.154815342738161,
-                    "y": 6.420229219532517,
-                    "heading": -0.07740089316210408,
-                    "angularVelocity": 3.913986631676039e-8,
-                    "velocityX": 2.6768799188697083,
-                    "velocityY": -2.6730044405364133,
-                    "timestamp": 1.66527977810737
-                },
-                {
-                    "x": 7.329876929572681,
-                    "y": 6.2124862031192585,
-                    "heading": -0.05260075245643602,
-                    "angularVelocity": 0.3071729152375715,
-                    "velocityX": 2.1683013258795385,
-                    "velocityY": -2.5730913678789142,
-                    "timestamp": 1.746016521079894
-                },
-                {
-                    "x": 7.469926171014247,
-                    "y": 6.046291741893866,
-                    "heading": -0.03276050638448697,
-                    "angularVelocity": 0.2457399858041329,
-                    "velocityX": 1.7346407135748194,
-                    "velocityY": -2.0584736900019895,
-                    "timestamp": 1.8267532640524178
-                },
-                {
-                    "x": 7.574963090588109,
-                    "y": 5.921645878284834,
-                    "heading": -0.01788026453634592,
-                    "angularVelocity": 0.184305698995044,
-                    "velocityX": 1.300980392652287,
-                    "velocityY": -1.5438554866085237,
-                    "timestamp": 1.9074900070249416
-                },
-                {
-                    "x": 7.644987698235257,
-                    "y": 5.8385486280487005,
-                    "heading": -0.007960086700925195,
-                    "angularVelocity": 0.12287067164441078,
-                    "velocityX": 0.8673201948582164,
-                    "velocityY": -1.0292370880556136,
-                    "timestamp": 1.9882267499974655
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": -0.003,
-                    "angularVelocity": 0.06143530836524976,
-                    "velocityX": 0.43366007192855915,
-                    "velocityY": -0.5146185803264369,
-                    "timestamp": 2.0689634929699894
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": -0.003,
-                    "angularVelocity": -2.7405624218065555e-29,
-                    "velocityX": 1.4947392660092065e-31,
-                    "velocityY": 0,
-                    "timestamp": 2.1497002359425132
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "W1 To C3": {
-            "waypoints": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 10
-                },
-                {
-                    "x": 2.916773796081543,
-                    "y": 6.28393030166626,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 10
-                },
-                {
-                    "x": 4.1324920654296875,
-                    "y": 5.8253936767578125,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 10
-                },
-                {
-                    "x": 4.748738765716553,
-                    "y": 4.2041850090026855,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 10
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": -0.003,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 10
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.308859553563438,
-                    "y": 6.744970476661412,
-                    "heading": 0.38132184535212243,
-                    "angularVelocity": -0.20370231005101772,
-                    "velocityX": 0.20796028650336576,
-                    "velocityY": -0.2823689819220092,
-                    "timestamp": 0.04260214155501803
-                },
-                {
-                    "x": 2.3271744503017344,
-                    "y": 6.7213636093363665,
-                    "heading": 0.36416244677454657,
-                    "angularVelocity": -0.4027825351318442,
-                    "velocityX": 0.4299055415945176,
-                    "velocityY": -0.554123958640874,
-                    "timestamp": 0.08520428311003606
-                },
-                {
-                    "x": 2.355668020695042,
-                    "y": 6.686794512902429,
-                    "heading": 0.33880572426100314,
-                    "angularVelocity": -0.5951983066578187,
-                    "velocityX": 0.6688295318795241,
-                    "velocityY": -0.8114403448308736,
-                    "timestamp": 0.1278064246650541
-                },
-                {
-                    "x": 2.3952194813587413,
-                    "y": 6.64212767454663,
-                    "heading": 0.305649210011626,
-                    "angularVelocity": -0.7782828054913067,
-                    "velocityX": 0.9283913723590802,
-                    "velocityY": -1.0484646246742158,
-                    "timestamp": 0.1704085662200721
-                },
-                {
-                    "x": 2.4468808744427974,
-                    "y": 6.58861694166169,
-                    "heading": 0.2652376454484053,
-                    "angularVelocity": -0.9485805898051318,
-                    "velocityX": 1.2126477965277631,
-                    "velocityY": -1.2560573466907532,
-                    "timestamp": 0.21301070777509012
-                },
-                {
-                    "x": 2.5118430486187977,
-                    "y": 6.5281213977332095,
-                    "heading": 0.2183393877421899,
-                    "angularVelocity": -1.100842727487048,
-                    "velocityX": 1.5248570096436473,
-                    "velocityY": -1.4200118050486843,
-                    "timestamp": 0.25561284933010814
-                },
-                {
-                    "x": 2.5912567436103777,
-                    "y": 6.463385022311091,
-                    "heading": 0.16616355843360497,
-                    "angularVelocity": -1.2247231571962895,
-                    "velocityX": 1.864077534436207,
-                    "velocityY": -1.5195568358579936,
-                    "timestamp": 0.29821499088512615
-                },
-                {
-                    "x": 2.6857686356352475,
-                    "y": 6.398206753066201,
-                    "heading": 0.11066645077656016,
-                    "angularVelocity": -1.3026835185121792,
-                    "velocityX": 2.21847748904391,
-                    "velocityY": -1.5299294088471524,
-                    "timestamp": 0.34081713244014417
-                },
-                {
-                    "x": 2.794870523929059,
-                    "y": 6.337041243081873,
-                    "heading": 0.05441835033855986,
-                    "angularVelocity": -1.3203115708481314,
-                    "velocityX": 2.5609484479298716,
-                    "velocityY": -1.43573791719686,
-                    "timestamp": 0.3834192739951622
-                },
-                {
-                    "x": 2.916773796081543,
-                    "y": 6.28393030166626,
-                    "heading": 0,
-                    "angularVelocity": -1.2773618497155108,
-                    "velocityX": 2.8614353105947288,
-                    "velocityY": -1.246673042176136,
-                    "timestamp": 0.4260214155501802
-                },
-                {
-                    "x": 3.0391710673922954,
-                    "y": 6.243974094587608,
-                    "heading": -0.047264422869834,
-                    "angularVelocity": -1.1928681768219465,
-                    "velocityX": 3.0890847917160027,
-                    "velocityY": -1.0084220857175146,
-                    "timestamp": 0.4656439185125414
-                },
-                {
-                    "x": 3.1702962457898094,
-                    "y": 6.213670351410701,
-                    "heading": -0.08947327835139807,
-                    "angularVelocity": -1.0652748394431244,
-                    "velocityX": 3.309361312233972,
-                    "velocityY": -0.7648114306582993,
-                    "timestamp": 0.5052664214749026
-                },
-                {
-                    "x": 3.3090884442062665,
-                    "y": 6.193565770320959,
-                    "heading": -0.12122975141158922,
-                    "angularVelocity": -0.8014756940104903,
-                    "velocityX": 3.502862970274757,
-                    "velocityY": -0.5074031064832265,
-                    "timestamp": 0.5448889244372639
-                },
-                {
-                    "x": 3.4545421039318374,
-                    "y": 6.168639069925164,
-                    "heading": -0.11367597167549683,
-                    "angularVelocity": 0.19064367900402435,
-                    "velocityX": 3.6709861530893746,
-                    "velocityY": -0.6291046383282068,
-                    "timestamp": 0.5845114273996251
-                },
-                {
-                    "x": 3.5909551218669695,
-                    "y": 6.134279746914322,
-                    "heading": -0.10324626427429143,
-                    "angularVelocity": 0.26322686911306303,
-                    "velocityX": 3.442816776737095,
-                    "velocityY": -0.8671668986555798,
-                    "timestamp": 0.6241339303619864
-                },
-                {
-                    "x": 3.7180733265010666,
-                    "y": 6.090709420708735,
-                    "heading": -0.09157048631886859,
-                    "angularVelocity": 0.2946754263988333,
-                    "velocityX": 3.208232573163052,
-                    "velocityY": -1.0996358873889647,
-                    "timestamp": 0.6637564333243476
-                },
-                {
-                    "x": 3.835817587911086,
-                    "y": 6.038004134131813,
-                    "heading": -0.07918178962672723,
-                    "angularVelocity": 0.31266820028784686,
-                    "velocityX": 2.9716512740719234,
-                    "velocityY": -1.3301856933921488,
-                    "timestamp": 0.7033789362867089
-                },
-                {
-                    "x": 3.9441493716247042,
-                    "y": 5.9762022678896844,
-                    "heading": -0.06634465187684353,
-                    "angularVelocity": 0.3239860379864983,
-                    "velocityX": 2.734097434897688,
-                    "velocityY": -1.5597668400917784,
-                    "timestamp": 0.7430014392490701
-                },
-                {
-                    "x": 4.0430458841375625,
-                    "y": 5.905326959185002,
-                    "heading": -0.05321706206750646,
-                    "angularVelocity": 0.3313165203573185,
-                    "velocityX": 2.4959683290781256,
-                    "velocityY": -1.7887640458253868,
-                    "timestamp": 0.7826239422114314
-                },
-                {
-                    "x": 4.1324920654296875,
-                    "y": 5.8253936767578125,
-                    "heading": -0.03990404951279715,
-                    "angularVelocity": 0.33599625362778834,
-                    "velocityX": 2.2574591357111773,
-                    "velocityY": -2.017370848659449,
-                    "timestamp": 0.8222464451737926
-                },
-                {
-                    "x": 4.241428434426057,
-                    "y": 5.691951873954815,
-                    "heading": -0.020558340356236417,
-                    "angularVelocity": 0.3400028267859621,
-                    "velocityX": 1.9145678816329739,
-                    "velocityY": -2.345253399462351,
-                    "timestamp": 0.8791451166920127
-                },
-                {
-                    "x": 4.330837596388679,
-                    "y": 5.539871857431794,
-                    "heading": -0.0011062192039580838,
-                    "angularVelocity": 0.34187302854776463,
-                    "velocityX": 1.571375211001045,
-                    "velocityY": -2.6728219212345348,
-                    "timestamp": 0.9360437882102327
-                },
-                {
-                    "x": 4.400691025411647,
-                    "y": 5.369183558238886,
-                    "heading": 0.018249380247094018,
-                    "angularVelocity": 0.34017664972817613,
-                    "velocityX": 1.2276811946408939,
-                    "velocityY": -2.999864401724217,
-                    "timestamp": 0.9929424597284527
-                },
-                {
-                    "x": 4.450931892836431,
-                    "y": 5.179947405025881,
-                    "heading": 0.037101343157351356,
-                    "angularVelocity": 0.3313251857597514,
-                    "velocityX": 0.8829884087662048,
-                    "velocityY": -3.3258448424829754,
-                    "timestamp": 1.0498411312466729
-                },
-                {
-                    "x": 4.481391591894744,
-                    "y": 4.9723492336445485,
-                    "heading": 0.054218740493726776,
-                    "angularVelocity": 0.3008400175194617,
-                    "velocityX": 0.5353323416094045,
-                    "velocityY": -3.6485591990465953,
-                    "timestamp": 1.106739802764893
-                },
-                {
-                    "x": 4.496396423991072,
-                    "y": 4.780842919446315,
-                    "heading": 0.0400753404119281,
-                    "angularVelocity": -0.248571710101697,
-                    "velocityX": 0.26371146629537756,
-                    "velocityY": -3.3657431551263746,
-                    "timestamp": 1.163638474283113
-                },
-                {
-                    "x": 4.530633514554346,
-                    "y": 4.608273372061934,
-                    "heading": 0.027581622688211512,
-                    "angularVelocity": -0.21957837310341866,
-                    "velocityX": 0.6017203855508131,
-                    "velocityY": -3.032927532044774,
-                    "timestamp": 1.220537145801333
-                },
-                {
-                    "x": 4.584102924369241,
-                    "y": 4.454640545632531,
-                    "heading": 0.016737783854959394,
-                    "angularVelocity": -0.1905815820283204,
-                    "velocityX": 0.9397303731032456,
-                    "velocityY": -2.7001127149375717,
-                    "timestamp": 1.277435817319553
-                },
-                {
-                    "x": 4.656804671911905,
-                    "y": 4.319944426632471,
-                    "heading": 0.007543896915478472,
-                    "angularVelocity": -0.1615835079126032,
-                    "velocityX": 1.277740685375807,
-                    "velocityY": -2.3672981355448273,
-                    "timestamp": 1.334334488837773
-                },
-                {
-                    "x": 4.748738765716553,
-                    "y": 4.2041850090026855,
-                    "heading": 0,
-                    "angularVelocity": -0.13258476365415262,
-                    "velocityX": 1.6157511476381003,
-                    "velocityY": -2.0344836626408243,
-                    "timestamp": 1.391233160355993
-                },
-                {
-                    "x": 5.007932614843106,
-                    "y": 4.0487951090615,
-                    "heading": -0.008478617739268947,
-                    "angularVelocity": -0.07486216912690727,
-                    "velocityX": 2.288558626731898,
-                    "velocityY": -1.372019039864535,
-                    "timestamp": 1.5044895332096289
-                },
-                {
-                    "x": 5.344428643764395,
-                    "y": 3.9673190703928256,
-                    "heading": -0.018205683784992727,
-                    "angularVelocity": -0.08588537492980046,
-                    "velocityX": 2.971100172492302,
-                    "velocityY": -0.7193947379364598,
-                    "timestamp": 1.6177459060632646
-                },
-                {
-                    "x": 5.761576858213634,
-                    "y": 3.95566669131606,
-                    "heading": -0.05529230717881331,
-                    "angularVelocity": -0.3274572764373147,
-                    "velocityX": 3.6832206783483272,
-                    "velocityY": -0.10288497488635052,
-                    "timestamp": 1.7310022789169004
-                },
-                {
-                    "x": 6.1883195012502945,
-                    "y": 3.9937898497766295,
-                    "heading": -0.05529230006422074,
-                    "angularVelocity": 6.281847442205705e-8,
-                    "velocityX": 3.767934927495438,
-                    "velocityY": 0.33660938894658915,
-                    "timestamp": 1.8442586517705362
-                },
-                {
-                    "x": 6.615062139687873,
-                    "y": 4.031913059718256,
-                    "heading": -0.05529229294956677,
-                    "angularVelocity": 6.281901664491759e-8,
-                    "velocityX": 3.7679348868877285,
-                    "velocityY": 0.3366098434998829,
-                    "timestamp": 1.957515024624172
-                },
-                {
-                    "x": 7.0409980156903185,
-                    "y": 4.069937864184792,
-                    "heading": -0.05280872948360114,
-                    "angularVelocity": 0.021928686248633502,
-                    "velocityX": 3.7608115576232954,
-                    "velocityY": 0.33574096987616325,
-                    "timestamp": 2.0707713974778077
-                },
-                {
-                    "x": 7.360520062907597,
-                    "y": 4.098469259621894,
-                    "heading": -0.032478143170072554,
-                    "angularVelocity": 0.17950942451425975,
-                    "velocityX": 2.821227973018394,
-                    "velocityY": 0.25191867546363345,
-                    "timestamp": 2.1840277703314435
-                },
-                {
-                    "x": 7.573513556720189,
-                    "y": 4.1174900304811795,
-                    "heading": -0.013829576144119837,
-                    "angularVelocity": 0.16465799280057086,
-                    "velocityX": 1.88063142449255,
-                    "velocityY": 0.1679443759325155,
-                    "timestamp": 2.2972841431850792
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": -0.003,
-                    "angularVelocity": 0.09562001564464007,
-                    "velocityX": 0.9402247361163992,
-                    "velocityY": 0.08396851567117221,
-                    "timestamp": 2.410540516038715
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": -0.003,
-                    "angularVelocity": 0,
-                    "velocityX": 2.757511206578506e-35,
-                    "velocityY": 0,
-                    "timestamp": 2.5237968888923508
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": false,
-            "defaultControlIntervalCount": 10,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "W1 To C4": {
-            "waypoints": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 4
-                },
-                {
-                    "x": 2.2457261085510254,
-                    "y": 6.7668328285217285,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 11
-                },
-                {
-                    "x": 3.284848213195801,
-                    "y": 6.196334362030029,
-                    "heading": -0.5,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 11
-                },
-                {
-                    "x": 4.274456024169922,
-                    "y": 4.934326648712158,
-                    "heading": -0.8357152799414105,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 12
-                },
-                {
-                    "x": 5.703982353210449,
-                    "y": 3.987083911895752,
-                    "heading": -0.31586356261422194,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 13
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": -0.003,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.285270702118571,
-                    "y": 6.762383358598232,
-                    "heading": 0.3634342922015001,
-                    "angularVelocity": -0.5809173586087107,
-                    "velocityX": -0.3220883435269421,
-                    "velocityY": 0.11771892098839686,
-                    "timestamp": 0.04573061452687351
-                },
-                {
-                    "x": 2.2599794598619596,
-                    "y": 6.767831656358603,
-                    "heading": 0.2777726454574198,
-                    "angularVelocity": -1.8731794363656886,
-                    "velocityX": -0.5530483794778248,
-                    "velocityY": 0.11913895793336499,
-                    "timestamp": 0.09146122905374703
-                },
-                {
-                    "x": 2.2450548028766546,
-                    "y": 6.7710281276739375,
-                    "heading": 0.1344358774143225,
-                    "angularVelocity": -3.134372225827269,
-                    "velocityX": -0.3263602980128451,
-                    "velocityY": 0.06989784301807915,
-                    "timestamp": 0.13719184358062053
-                },
-                {
-                    "x": 2.2457261085510254,
-                    "y": 6.7668328285217285,
-                    "heading": 0,
-                    "angularVelocity": -2.939734766416754,
-                    "velocityX": 0.014679568191384731,
-                    "velocityY": -0.09173940030374239,
-                    "timestamp": 0.18292245810749405
-                },
-                {
-                    "x": 2.262817902753147,
-                    "y": 6.755151488221845,
-                    "heading": -0.1222613161134929,
-                    "angularVelocity": -2.5936618258096975,
-                    "velocityX": 0.3625867573309899,
-                    "velocityY": -0.24780893395554135,
-                    "timestamp": 0.230060953533885
-                },
-                {
-                    "x": 2.2962801062686125,
-                    "y": 6.73609161362754,
-                    "heading": -0.22784823217966704,
-                    "angularVelocity": -2.2399297031255614,
-                    "velocityX": 0.7098699950598514,
-                    "velocityY": -0.40433778002244786,
-                    "timestamp": 0.277199448960276
-                },
-                {
-                    "x": 2.3460880772414248,
-                    "y": 6.709619977067709,
-                    "heading": -0.31657680000453947,
-                    "angularVelocity": -1.882295288008833,
-                    "velocityX": 1.056630478386546,
-                    "velocityY": -0.561571520694102,
-                    "timestamp": 0.32433794438666697
-                },
-                {
-                    "x": 2.4122219538028076,
-                    "y": 6.675695367304005,
-                    "heading": -0.3884103898720453,
-                    "angularVelocity": -1.5238838070197687,
-                    "velocityX": 1.4029696103613034,
-                    "velocityY": -0.7196795200363996,
-                    "timestamp": 0.37147643981305795
-                },
-                {
-                    "x": 2.494664207392629,
-                    "y": 6.634268490634927,
-                    "heading": -0.4434154648490763,
-                    "angularVelocity": -1.1668822791122553,
-                    "velocityX": 1.7489368899896385,
-                    "velocityY": -0.8788332401012526,
-                    "timestamp": 0.4186149352394489
-                },
-                {
-                    "x": 2.593395417678489,
-                    "y": 6.585276348843886,
-                    "heading": -0.4817298536594349,
-                    "angularVelocity": -0.8128046613242179,
-                    "velocityX": 2.094492185055806,
-                    "velocityY": -1.0393234096224127,
-                    "timestamp": 0.4657534306658399
-                },
-                {
-                    "x": 2.7083870607833584,
-                    "y": 6.52862575501488,
-                    "heading": -0.5035659675601283,
-                    "angularVelocity": -0.46323315377732527,
-                    "velocityX": 2.4394423721997165,
-                    "velocityY": -1.2017904542040352,
-                    "timestamp": 0.5128919260922309
-                },
-                {
-                    "x": 2.839583375891639,
-                    "y": 6.464143202111409,
-                    "heading": -0.5093019910748294,
-                    "angularVelocity": -0.12168448446963974,
-                    "velocityX": 2.78320964471958,
-                    "velocityY": -1.367938291626405,
-                    "timestamp": 0.5600304215186218
-                },
-                {
-                    "x": 2.9868119835174247,
-                    "y": 6.391337595074747,
-                    "heading": -0.5000000631264199,
-                    "angularVelocity": 0.197331880546237,
-                    "velocityX": 3.1233200443517304,
-                    "velocityY": -1.5445042608591435,
-                    "timestamp": 0.6071689169450127
-                },
-                {
-                    "x": 3.140901786552832,
-                    "y": 6.301587367083572,
-                    "heading": -0.5000000197789056,
-                    "angularVelocity": 9.19577807856047e-7,
-                    "velocityX": 3.2688740198760886,
-                    "velocityY": -1.903968872561393,
-                    "timestamp": 0.6543074123714037
-                },
-                {
-                    "x": 3.284848213195801,
-                    "y": 6.196334362030029,
-                    "heading": -0.5,
-                    "angularVelocity": 4.1959136469938736e-7,
-                    "velocityX": 3.0536915813881054,
-                    "velocityY": -2.2328460868682716,
-                    "timestamp": 0.7014459077977946
-                },
-                {
-                    "x": 3.3993231490932048,
-                    "y": 6.0978407623199296,
-                    "heading": -0.5170802154644099,
-                    "angularVelocity": -0.41193186552987154,
-                    "velocityX": 2.7608477187370504,
-                    "velocityY": -2.375418059313161,
-                    "timestamp": 0.7429095985042958
-                },
-                {
-                    "x": 3.501098994358481,
-                    "y": 5.994630951270013,
-                    "heading": -0.5506635755138305,
-                    "angularVelocity": -0.8099462319284754,
-                    "velocityX": 2.4545775721162104,
-                    "velocityY": -2.489161222534419,
-                    "timestamp": 0.7843732892107971
-                },
-                {
-                    "x": 3.5900181651549583,
-                    "y": 5.890084132174874,
-                    "heading": -0.6049860857837608,
-                    "angularVelocity": -1.3101224069596595,
-                    "velocityX": 2.144506899445626,
-                    "velocityY": -2.521406495990601,
-                    "timestamp": 0.8258369799172983
-                },
-                {
-                    "x": 3.6727603343537245,
-                    "y": 5.794679716077001,
-                    "heading": -0.692814707704561,
-                    "angularVelocity": -2.1182056016767716,
-                    "velocityX": 1.9955331469267976,
-                    "velocityY": -2.3009147153167167,
-                    "timestamp": 0.8673006706237996
-                },
-                {
-                    "x": 3.757696306555582,
-                    "y": 5.694133573084931,
-                    "heading": -0.7686614093789431,
-                    "angularVelocity": -1.8292318021376788,
-                    "velocityX": 2.04844216119069,
-                    "velocityY": -2.424920244168773,
-                    "timestamp": 0.9087643613303008
-                },
-                {
-                    "x": 3.844342447773648,
-                    "y": 5.582483853612312,
-                    "heading": -0.8164085841781574,
-                    "angularVelocity": -1.1515418426489148,
-                    "velocityX": 2.0896871393165974,
-                    "velocityY": -2.6927105998111585,
-                    "timestamp": 0.950228052036802
-                },
-                {
-                    "x": 3.9316331213435283,
-                    "y": 5.459771926422603,
-                    "heading": -0.8357045871440653,
-                    "angularVelocity": -0.4653710906373953,
-                    "velocityX": 2.105231639596513,
-                    "velocityY": -2.9595032448602216,
-                    "timestamp": 0.9916917427433033
-                },
-                {
-                    "x": 4.018219294626101,
-                    "y": 5.3289825292048665,
-                    "heading": -0.8357080754934182,
-                    "angularVelocity": -0.00008413021836472818,
-                    "velocityX": 2.088240863443359,
-                    "velocityY": -3.1543115190486835,
-                    "timestamp": 1.0331554334498045
-                },
-                {
-                    "x": 4.10449796775504,
-                    "y": 5.197989757794864,
-                    "heading": -0.8357107471897811,
-                    "angularVelocity": -0.00006443460091393523,
-                    "velocityX": 2.0808247326516955,
-                    "velocityY": -3.159216393376985,
-                    "timestamp": 1.0746191241563057
-                },
-                {
-                    "x": 4.190712810420311,
-                    "y": 5.066954954654073,
-                    "heading": -0.8357133876254744,
-                    "angularVelocity": -0.00006368067213797694,
-                    "velocityX": 2.0792853023028366,
-                    "velocityY": -3.160230093078832,
-                    "timestamp": 1.1160828148628068
-                },
-                {
-                    "x": 4.274456024169922,
-                    "y": 4.934326648712158,
-                    "heading": -0.8357152799414105,
-                    "angularVelocity": -0.000045637904003859675,
-                    "velocityX": 2.0196758253473424,
-                    "velocityY": -3.198661375339825,
-                    "timestamp": 1.157546505569308
-                },
-                {
-                    "x": 4.34828200363932,
-                    "y": 4.789661343281582,
-                    "heading": -0.8357152360129204,
-                    "angularVelocity": 0.0000010231801667591938,
-                    "velocityX": 1.7195509731200054,
-                    "velocityY": -3.36953696405698,
-                    "timestamp": 1.2004797949125405
-                },
-                {
-                    "x": 4.435451827820083,
-                    "y": 4.652622129190998,
-                    "heading": -0.8357151829793803,
-                    "angularVelocity": 0.0000012352545290785843,
-                    "velocityX": 2.0303551280191665,
-                    "velocityY": -3.1919104309715474,
-                    "timestamp": 1.2434130842557731
-                },
-                {
-                    "x": 4.535184717372419,
-                    "y": 4.524436546290902,
-                    "heading": -0.8357137971440172,
-                    "angularVelocity": 0.000032278807056995315,
-                    "velocityX": 2.322973410097133,
-                    "velocityY": -2.985692101886486,
-                    "timestamp": 1.2863463735990057
-                },
-                {
-                    "x": 4.644325012525673,
-                    "y": 4.4080959710874374,
-                    "heading": -0.8258667739389303,
-                    "angularVelocity": 0.22935636555504585,
-                    "velocityX": 2.542090224690803,
-                    "velocityY": -2.7097987827900663,
-                    "timestamp": 1.3292796629422383
-                },
-                {
-                    "x": 4.760978449232698,
-                    "y": 4.3048079626404245,
-                    "heading": -0.804942882637706,
-                    "angularVelocity": 0.4873582159975465,
-                    "velocityX": 2.717085937078615,
-                    "velocityY": -2.405779059257989,
-                    "timestamp": 1.372212952285471
-                },
-                {
-                    "x": 4.883873293679825,
-                    "y": 4.215276634825419,
-                    "heading": -0.7738338273486849,
-                    "angularVelocity": 0.7245905395302493,
-                    "velocityX": 2.8624604899158337,
-                    "velocityY": -2.085359151013129,
-                    "timestamp": 1.4151462416287035
-                },
-                {
-                    "x": 5.012023716462111,
-                    "y": 4.13998910043764,
-                    "heading": -0.7341562289641704,
-                    "angularVelocity": 0.9241686111518187,
-                    "velocityX": 2.9848731542039757,
-                    "velocityY": -1.7535934362235386,
-                    "timestamp": 1.458079530971936
-                },
-                {
-                    "x": 5.144586044931236,
-                    "y": 4.079308119917677,
-                    "heading": -0.6878803412234658,
-                    "angularVelocity": 1.0778556325081365,
-                    "velocityX": 3.0876350379153963,
-                    "velocityY": -1.4133783236324275,
-                    "timestamp": 1.5010128203151687
-                },
-                {
-                    "x": 5.2807921990430025,
-                    "y": 4.033507276074697,
-                    "heading": -0.6371968549741135,
-                    "angularVelocity": 1.1805171936433834,
-                    "velocityX": 3.1725068401551706,
-                    "velocityY": -1.0667909341122943,
-                    "timestamp": 1.5439461096584013
-                },
-                {
-                    "x": 5.419841766333411,
-                    "y": 4.00280308410274,
-                    "heading": -0.5848216118259447,
-                    "angularVelocity": 1.2199215096111542,
-                    "velocityX": 3.2387354758394924,
-                    "velocityY": -0.7151604836631573,
-                    "timestamp": 1.5868793990016339
-                },
-                {
-                    "x": 5.561074826834354,
-                    "y": 3.9873115142730846,
-                    "heading": -0.5330346380339317,
-                    "angularVelocity": 1.2062195695745372,
-                    "velocityX": 3.289593289064067,
-                    "velocityY": -0.36082885953151356,
-                    "timestamp": 1.6298126883448665
-                },
-                {
-                    "x": 5.703982353210449,
-                    "y": 3.987083911895752,
-                    "heading": -0.4835776002687649,
-                    "angularVelocity": 1.1519508176925284,
-                    "velocityX": 3.328594863384743,
-                    "velocityY": -0.005301303040469438,
-                    "timestamp": 1.672745977688099
-                },
-                {
-                    "x": 5.958820715702131,
-                    "y": 3.94375176713671,
-                    "heading": -0.42940135113015304,
-                    "angularVelocity": 0.7375353070821884,
-                    "velocityX": 3.4692746900164138,
-                    "velocityY": -0.5899077030899651,
-                    "timestamp": 1.7462017819301305
-                },
-                {
-                    "x": 6.2169360873076736,
-                    "y": 3.85673688488041,
-                    "heading": -0.41197233440562925,
-                    "angularVelocity": 0.2372721516614769,
-                    "velocityX": 3.513886673339918,
-                    "velocityY": -1.184588245329031,
-                    "timestamp": 1.819657586172162
-                },
-                {
-                    "x": 6.462551803453154,
-                    "y": 3.726776847306083,
-                    "heading": -0.411972258677301,
-                    "angularVelocity": 0.0000010309372967585872,
-                    "velocityX": 3.343721012654018,
-                    "velocityY": -1.76922761809427,
-                    "timestamp": 1.8931133904141935
-                },
-                {
-                    "x": 6.683975982637168,
-                    "y": 3.558883929748973,
-                    "heading": -0.4119722206140506,
-                    "angularVelocity": 5.181789344885153e-7,
-                    "velocityX": 3.0143864255360504,
-                    "velocityY": -2.285631738561,
-                    "timestamp": 1.966569194656225
-                },
-                {
-                    "x": 6.875523396649561,
-                    "y": 3.357572509103632,
-                    "heading": -0.4119721861737391,
-                    "angularVelocity": 4.688575916806749e-7,
-                    "velocityX": 2.6076552559586523,
-                    "velocityY": -2.7405788109272424,
-                    "timestamp": 2.0400249988982564
-                },
-                {
-                    "x": 7.060174216266159,
-                    "y": 3.149917085871916,
-                    "heading": -0.4119721521055279,
-                    "angularVelocity": 4.637919568273184e-7,
-                    "velocityX": 2.513767584766965,
-                    "velocityY": -2.826943702739948,
-                    "timestamp": 2.1134808031402876
-                },
-                {
-                    "x": 7.236976316543012,
-                    "y": 2.954068221435557,
-                    "heading": -0.3758421094017394,
-                    "angularVelocity": 0.4918609642437705,
-                    "velocityX": 2.40691803869303,
-                    "velocityY": -2.666213602277847,
-                    "timestamp": 2.186936607382319
-                },
-                {
-                    "x": 7.385172224991548,
-                    "y": 2.787906831681346,
-                    "heading": -0.27501394829018777,
-                    "angularVelocity": 1.3726370863672974,
-                    "velocityX": 2.0174839820722426,
-                    "velocityY": -2.262059363024933,
-                    "timestamp": 2.26039241162435
-                },
-                {
-                    "x": 7.503381996539121,
-                    "y": 2.655285343615516,
-                    "heading": -0.17500745818288596,
-                    "angularVelocity": 1.3614511629031412,
-                    "velocityX": 1.6092638664478731,
-                    "velocityY": -1.8054596152654476,
-                    "timestamp": 2.3338482158663814
-                },
-                {
-                    "x": 7.59180643545552,
-                    "y": 2.55603678143989,
-                    "heading": -0.09217156397731928,
-                    "angularVelocity": 1.1276970562139184,
-                    "velocityX": 1.2037774254713123,
-                    "velocityY": -1.3511330139223812,
-                    "timestamp": 2.4073040201084126
-                },
-                {
-                    "x": 7.650633785300877,
-                    "y": 2.489982671853645,
-                    "heading": -0.033538816068743524,
-                    "angularVelocity": 0.7982044239197745,
-                    "velocityX": 0.8008536623127058,
-                    "velocityY": -0.8992360817206846,
-                    "timestamp": 2.480759824350444
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": -0.003,
-                    "angularVelocity": 0.41574408426759185,
-                    "velocityX": 0.3997807253237885,
-                    "velocityY": -0.44901382802875656,
-                    "timestamp": 2.554215628592475
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": -0.003,
-                    "angularVelocity": 5.454413556338505e-31,
-                    "velocityX": 0,
-                    "velocityY": -3.390988673475489e-31,
-                    "timestamp": 2.6276714328345063
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        4
-                    ],
-                    "type": "WptVelocityDirection",
-                    "direction": 3.14
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "W1 To C5": {
-            "waypoints": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 32
-                },
-                {
-                    "x": 2.382448196411133,
-                    "y": 1.9443897008895874,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 35
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": -0.003,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 4.884595084783414e-34,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.298507420370097,
-                    "y": 6.738728491162364,
-                    "heading": 0.38429180377773414,
-                    "angularVelocity": -0.12150700609071861,
-                    "velocityX": -0.031771662206367984,
-                    "velocityY": -0.3889348314553929,
-                    "timestamp": 0.04697832994094238
-                },
-                {
-                    "x": 2.295523042319471,
-                    "y": 6.702182630298916,
-                    "heading": 0.3729896107436911,
-                    "angularVelocity": -0.2405831166891495,
-                    "velocityX": -0.06352669527371438,
-                    "velocityY": -0.7779301841804652,
-                    "timestamp": 0.09395665988188476
-                },
-                {
-                    "x": 2.2910478858134478,
-                    "y": 6.647358841243747,
-                    "heading": 0.35624034482133005,
-                    "angularVelocity": -0.3565317443897418,
-                    "velocityX": -0.09526001694076108,
-                    "velocityY": -1.1670016606398963,
-                    "timestamp": 0.14093498982282715
-                },
-                {
-                    "x": 2.285083332518943,
-                    "y": 6.574252474793301,
-                    "heading": 0.33424119989038353,
-                    "angularVelocity": -0.46828282228427764,
-                    "velocityX": -0.12696392787914923,
-                    "velocityY": -1.556172101953168,
-                    "timestamp": 0.18791331976376952
-                },
-                {
-                    "x": 2.2776313636635503,
-                    "y": 6.48285724279221,
-                    "heading": 0.3072714314404645,
-                    "angularVelocity": -0.5740895532860236,
-                    "velocityX": -0.15862566559434335,
-                    "velocityY": -1.9454763955207768,
-                    "timestamp": 0.23489164970471188
-                },
-                {
-                    "x": 2.268695072443392,
-                    "y": 6.373164221156064,
-                    "heading": 0.2757564747192681,
-                    "angularVelocity": -0.6708402951065854,
-                    "velocityX": -0.1902215602681591,
-                    "velocityY": -2.334970650809517,
-                    "timestamp": 0.28186997964565425
-                },
-                {
-                    "x": 2.2582799597491072,
-                    "y": 6.245160021300657,
-                    "heading": 0.24041955506127732,
-                    "angularVelocity": -0.752196165815466,
-                    "velocityX": -0.22170036072754723,
-                    "velocityY": -2.7247499009931837,
-                    "timestamp": 0.3288483095865966
-                },
-                {
-                    "x": 2.2463982479830333,
-                    "y": 6.098824071496616,
-                    "heading": 0.20274918029785605,
-                    "angularVelocity": -0.8018670482917891,
-                    "velocityX": -0.25291899011757896,
-                    "velocityY": -3.114967049446051,
-                    "timestamp": 0.375826639527539
-                },
-                {
-                    "x": 2.2330953714575887,
-                    "y": 5.934154199517091,
-                    "heading": 0.1675471140087416,
-                    "angularVelocity": -0.7493256216934009,
-                    "velocityX": -0.28317048609790474,
-                    "velocityY": -3.505230436810659,
-                    "timestamp": 0.42280496946848134
-                },
-                {
-                    "x": 2.218530566609672,
-                    "y": 5.757035827571947,
-                    "heading": 0.16754710135001605,
-                    "angularVelocity": -2.6945882371217095e-7,
-                    "velocityX": -0.3100324099691601,
-                    "velocityY": -3.770214313020578,
-                    "timestamp": 0.4697832994094237
-                },
-                {
-                    "x": 2.203965673260931,
-                    "y": 5.579917462904045,
-                    "heading": 0.16754708869211127,
-                    "angularVelocity": -2.694413527531555e-7,
-                    "velocityX": -0.3100342938339967,
-                    "velocityY": -3.7702141581142206,
-                    "timestamp": 0.5167616293503661
-                },
-                {
-                    "x": 2.1894007799094983,
-                    "y": 5.402799098236364,
-                    "heading": 0.16754707603420654,
-                    "angularVelocity": -2.694413521606316e-7,
-                    "velocityX": -0.31003429389130516,
-                    "velocityY": -3.7702141581095083,
-                    "timestamp": 0.5637399592913085
-                },
-                {
-                    "x": 2.1748358865580655,
-                    "y": 5.225680733568683,
-                    "heading": 0.1675470633763018,
-                    "angularVelocity": -2.6944135250249435e-7,
-                    "velocityX": -0.3100342938913069,
-                    "velocityY": -3.770214158109508,
-                    "timestamp": 0.6107182892322509
-                },
-                {
-                    "x": 2.160270993206632,
-                    "y": 5.048562368901002,
-                    "heading": 0.16754705071839698,
-                    "angularVelocity": -2.694413536359467e-7,
-                    "velocityX": -0.31003429389130693,
-                    "velocityY": -3.7702141581095083,
-                    "timestamp": 0.6576966191731933
-                },
-                {
-                    "x": 2.1457060998551993,
-                    "y": 4.871444004233322,
-                    "heading": 0.1675470380604922,
-                    "angularVelocity": -2.6944135247689087e-7,
-                    "velocityX": -0.3100342938913068,
-                    "velocityY": -3.7702141581095083,
-                    "timestamp": 0.7046749491141358
-                },
-                {
-                    "x": 2.131141206503766,
-                    "y": 4.6943256395656405,
-                    "heading": 0.16754702540258745,
-                    "angularVelocity": -2.694413528693067e-7,
-                    "velocityX": -0.31003429389130693,
-                    "velocityY": -3.7702141581095083,
-                    "timestamp": 0.7516532790550782
-                },
-                {
-                    "x": 2.1165763131523327,
-                    "y": 4.51720727489796,
-                    "heading": 0.16754701274468267,
-                    "angularVelocity": -2.694413529636153e-7,
-                    "velocityX": -0.31003429389130693,
-                    "velocityY": -3.7702141581095083,
-                    "timestamp": 0.7986316089960206
-                },
-                {
-                    "x": 2.1020114198009,
-                    "y": 4.340088910230279,
-                    "heading": 0.16754700008677786,
-                    "angularVelocity": -2.694413530501801e-7,
-                    "velocityX": -0.3100342938913069,
-                    "velocityY": -3.770214158109508,
-                    "timestamp": 0.845609938936963
-                },
-                {
-                    "x": 2.0874465264494666,
-                    "y": 4.162970545562599,
-                    "heading": 0.16754698742887306,
-                    "angularVelocity": -2.694413534833812e-7,
-                    "velocityX": -0.31003429389130704,
-                    "velocityY": -3.7702141581095083,
-                    "timestamp": 0.8925882688779054
-                },
-                {
-                    "x": 2.0728816330980333,
-                    "y": 3.9858521808949186,
-                    "heading": 0.16754697477096828,
-                    "angularVelocity": -2.6944135303696276e-7,
-                    "velocityX": -0.31003429389130693,
-                    "velocityY": -3.770214158109508,
-                    "timestamp": 0.9395665988188479
-                },
-                {
-                    "x": 2.0583167397466005,
-                    "y": 3.8087338162272375,
-                    "heading": 0.16754696211306347,
-                    "angularVelocity": -2.694413537071192e-7,
-                    "velocityX": -0.31003429389130704,
-                    "velocityY": -3.7702141581095083,
-                    "timestamp": 0.9865449287597903
-                },
-                {
-                    "x": 2.043751846395167,
-                    "y": 3.631615451559557,
-                    "heading": 0.1675469494551587,
-                    "angularVelocity": -2.694413527332354e-7,
-                    "velocityX": -0.31003429389130716,
-                    "velocityY": -3.770214158109508,
-                    "timestamp": 1.0335232587007326
-                },
-                {
-                    "x": 2.0291869530437365,
-                    "y": 3.4544970868918763,
-                    "heading": 0.16754693679725388,
-                    "angularVelocity": -2.694413532802898e-7,
-                    "velocityX": -0.31003429389125564,
-                    "velocityY": -3.7702141581095123,
-                    "timestamp": 1.080501588641675
-                },
-                {
-                    "x": 2.0146220597715567,
-                    "y": 3.2773787222176782,
-                    "heading": 0.1675469241393491,
-                    "angularVelocity": -2.6944135307750017e-7,
-                    "velocityX": -0.31003429220428774,
-                    "velocityY": -3.7702141582482356,
-                    "timestamp": 1.1274799185826172
-                },
-                {
-                    "x": 2.0000597716588953,
-                    "y": 3.1002601433334096,
-                    "heading": 0.16754691148142445,
-                    "angularVelocity": -2.6944177624221033e-7,
-                    "velocityX": -0.30997883771037654,
-                    "velocityY": -3.770218718011654,
-                    "timestamp": 1.1744582485235595
-                },
-                {
-                    "x": 2.001608158601577,
-                    "y": 2.922550679334096,
-                    "heading": 0.16754689857982888,
-                    "angularVelocity": -2.7462865480192305e-7,
-                    "velocityX": 0.032959599556405594,
-                    "velocityY": -3.7827965409310345,
-                    "timestamp": 1.2214365784645018
-                },
-                {
-                    "x": 2.021524410839774,
-                    "y": 2.7459539818451146,
-                    "heading": 0.1675468848578902,
-                    "angularVelocity": -2.9209081516381327e-7,
-                    "velocityX": 0.4239455140962882,
-                    "velocityY": -3.7591097365739743,
-                    "timestamp": 1.2684149084054441
-                },
-                {
-                    "x": 2.0595952756969864,
-                    "y": 2.572363489269638,
-                    "heading": 0.16754686955661854,
-                    "angularVelocity": -3.257091448717022e-7,
-                    "velocityX": 0.8103920446953395,
-                    "velocityY": -3.6951184257444134,
-                    "timestamp": 1.3153932383463864
-                },
-                {
-                    "x": 2.1154126290237643,
-                    "y": 2.4036404273247642,
-                    "heading": 0.16754685154625998,
-                    "angularVelocity": -3.8337587986161977e-7,
-                    "velocityX": 1.1881510772508834,
-                    "velocityY": -3.5915083008906312,
-                    "timestamp": 1.3623715682873287
-                },
-                {
-                    "x": 2.1883780268761943,
-                    "y": 2.2415938375833533,
-                    "heading": 0.1675468289010436,
-                    "angularVelocity": -4.82035363826859e-7,
-                    "velocityX": 1.5531713865554018,
-                    "velocityY": -3.449390175111029,
-                    "timestamp": 1.409349898228271
-                },
-                {
-                    "x": 2.2777091497247044,
-                    "y": 2.0879611790897656,
-                    "heading": 0.16754679784052873,
-                    "angularVelocity": -6.611668595012802e-7,
-                    "velocityX": 1.9015389214731706,
-                    "velocityY": -3.270287783467881,
-                    "timestamp": 1.4563282281692134
-                },
-                {
-                    "x": 2.382448196411133,
-                    "y": 1.9443897008895872,
-                    "heading": 0.16754674836915184,
-                    "angularVelocity": -0.0000010530680204285256,
-                    "velocityX": 2.2295183080815923,
-                    "velocityY": -3.0561213729961265,
-                    "timestamp": 1.5033065581101557
-                },
-                {
-                    "x": 2.5069799221636964,
-                    "y": 1.8074962230648766,
-                    "heading": 0.1675467031876573,
-                    "angularVelocity": -9.235757728412815e-7,
-                    "velocityX": 2.545610454081449,
-                    "velocityY": -2.798302730812182,
-                    "timestamp": 1.55222673973217
-                },
-                {
-                    "x": 2.645527049823341,
-                    "y": 1.6848068830394283,
-                    "heading": 0.16754667440242366,
-                    "angularVelocity": -5.884122397185895e-7,
-                    "velocityX": 2.8321057499364835,
-                    "velocityY": -2.5079493975189484,
-                    "timestamp": 1.6011469213541845
-                },
-                {
-                    "x": 2.7964787325047507,
-                    "y": 1.577748144471377,
-                    "heading": 0.1675466534117406,
-                    "angularVelocity": -4.2908023560369677e-7,
-                    "velocityX": 3.0856729815058617,
-                    "velocityY": -2.1884370625450487,
-                    "timestamp": 1.6500671029761989
-                },
-                {
-                    "x": 2.958079900807193,
-                    "y": 1.4875647290195357,
-                    "heading": 0.1675466366326211,
-                    "angularVelocity": -3.429897224036844e-7,
-                    "velocityX": 3.303364029820377,
-                    "velocityY": -1.8434807979383812,
-                    "timestamp": 1.6989872845982132
-                },
-                {
-                    "x": 3.1284516660647324,
-                    "y": 1.4153051353233332,
-                    "heading": 0.16754662225394246,
-                    "angularVelocity": -2.9392120351406106e-7,
-                    "velocityX": 3.4826478481607013,
-                    "velocityY": -1.4770916889581902,
-                    "timestamp": 1.7479074662202276
-                },
-                {
-                    "x": 3.3056131601804433,
-                    "y": 1.3618094062641966,
-                    "heading": 0.16754660920712522,
-                    "angularVelocity": -2.666960100615425e-7,
-                    "velocityX": 3.6214398279336546,
-                    "velocityY": -1.0935308759169382,
-                    "timestamp": 1.796827647842242
-                },
-                {
-                    "x": 3.487504529935422,
-                    "y": 1.3276990268102373,
-                    "heading": 0.16754659675699374,
-                    "angularVelocity": -2.544988811687778e-7,
-                    "velocityX": 3.7181253978240782,
-                    "velocityY": -0.6972660019440657,
-                    "timestamp": 1.8457478294642564
-                },
-                {
-                    "x": 3.671043389503124,
-                    "y": 1.304003553111977,
-                    "heading": 0.16754658441216208,
-                    "angularVelocity": -2.5234639867690264e-7,
-                    "velocityX": 3.751802497092686,
-                    "velocityY": -0.48437010887133,
-                    "timestamp": 1.8946680110862708
-                },
-                {
-                    "x": 3.8545823032662914,
-                    "y": 1.2803084992011935,
-                    "heading": 0.16754657206733162,
-                    "angularVelocity": -2.523463739841797e-7,
-                    "velocityX": 3.751803604927206,
-                    "velocityY": -0.4843615278018566,
-                    "timestamp": 1.9435881927082852
-                },
-                {
-                    "x": 4.038121217030901,
-                    "y": 1.2566134453015836,
-                    "heading": 0.16754655972250113,
-                    "angularVelocity": -2.5234637491728997e-7,
-                    "velocityX": 3.7518036049566934,
-                    "velocityY": -0.48436152757345374,
-                    "timestamp": 1.9925083743302996
-                },
-                {
-                    "x": 4.221660130795511,
-                    "y": 1.232918391401974,
-                    "heading": 0.16754654737767066,
-                    "angularVelocity": -2.5234637402611237e-7,
-                    "velocityX": 3.7518036049566943,
-                    "velocityY": -0.4843615275734478,
-                    "timestamp": 2.041428555952314
-                },
-                {
-                    "x": 4.4051990445601215,
-                    "y": 1.2092233375023644,
-                    "heading": 0.16754653503284023,
-                    "angularVelocity": -2.5234637391390173e-7,
-                    "velocityX": 3.751803604956694,
-                    "velocityY": -0.4843615275734475,
-                    "timestamp": 2.090348737574329
-                },
-                {
-                    "x": 4.588737958324731,
-                    "y": 1.1855282836027548,
-                    "heading": 0.16754652268800974,
-                    "angularVelocity": -2.523463747404588e-7,
-                    "velocityX": 3.751803604956694,
-                    "velocityY": -0.4843615275734478,
-                    "timestamp": 2.1392689191963434
-                },
-                {
-                    "x": 4.77227687208934,
-                    "y": 1.1618332297031453,
-                    "heading": 0.16754651034317924,
-                    "angularVelocity": -2.523463747849071e-7,
-                    "velocityX": 3.751803604956694,
-                    "velocityY": -0.48436152757344797,
-                    "timestamp": 2.188189100818358
-                },
-                {
-                    "x": 4.95581578585395,
-                    "y": 1.1381381758035356,
-                    "heading": 0.16754649799834875,
-                    "angularVelocity": -2.52346374599026e-7,
-                    "velocityX": 3.751803604956694,
-                    "velocityY": -0.4843615275734478,
-                    "timestamp": 2.2371092824403727
-                },
-                {
-                    "x": 5.1393546996185595,
-                    "y": 1.114443121903926,
-                    "heading": 0.1675464856535183,
-                    "angularVelocity": -2.523463743979587e-7,
-                    "velocityX": 3.751803604956694,
-                    "velocityY": -0.48436152757344764,
-                    "timestamp": 2.2860294640623873
-                },
-                {
-                    "x": 5.32289361338317,
-                    "y": 1.0907480680043165,
-                    "heading": 0.16754647330868783,
-                    "angularVelocity": -2.523463747332428e-7,
-                    "velocityX": 3.7518036049566943,
-                    "velocityY": -0.48436152757344797,
-                    "timestamp": 2.334949645684402
-                },
-                {
-                    "x": 5.506432527147779,
-                    "y": 1.0670530141047068,
-                    "heading": 0.1675464609638573,
-                    "angularVelocity": -2.5234637523589347e-7,
-                    "velocityX": 3.7518036049566943,
-                    "velocityY": -0.48436152757344764,
-                    "timestamp": 2.3838698273064165
-                },
-                {
-                    "x": 5.689971440912389,
-                    "y": 1.043357960205097,
-                    "heading": 0.16754644861902682,
-                    "angularVelocity": -2.523463745889681e-7,
-                    "velocityX": 3.751803604956694,
-                    "velocityY": -0.4843615275734477,
-                    "timestamp": 2.432790008928431
-                },
-                {
-                    "x": 5.8735103546769984,
-                    "y": 1.0196629063054876,
-                    "heading": 0.16754643627419633,
-                    "angularVelocity": -2.5234637515727394e-7,
-                    "velocityX": 3.751803604956694,
-                    "velocityY": -0.4843615275734478,
-                    "timestamp": 2.4817101905504457
-                },
-                {
-                    "x": 6.057049268441609,
-                    "y": 0.995967852405878,
-                    "heading": 0.16754642392936583,
-                    "angularVelocity": -2.523463750187176e-7,
-                    "velocityX": 3.751803604956694,
-                    "velocityY": -0.4843615275734476,
-                    "timestamp": 2.5306303721724603
-                },
-                {
-                    "x": 6.240588182206218,
-                    "y": 0.9722727985062686,
-                    "heading": 0.16754641158453532,
-                    "angularVelocity": -2.523463753175626e-7,
-                    "velocityX": 3.751803604956694,
-                    "velocityY": -0.48436152757344764,
-                    "timestamp": 2.579550553794475
-                },
-                {
-                    "x": 6.424127095970827,
-                    "y": 0.948577744606659,
-                    "heading": 0.1675463992397048,
-                    "angularVelocity": -2.5234637555305e-7,
-                    "velocityX": 3.7518036049566934,
-                    "velocityY": -0.4843615275734476,
-                    "timestamp": 2.6284707354164896
-                },
-                {
-                    "x": 6.607666009735459,
-                    "y": 0.9248826907072107,
-                    "heading": 0.1675463868948743,
-                    "angularVelocity": -2.5234637524939476e-7,
-                    "velocityX": 3.7518036049571197,
-                    "velocityY": -0.48436152757015266,
-                    "timestamp": 2.677390917038504
-                },
-                {
-                    "x": 6.79120492428193,
-                    "y": 0.9011876428641385,
-                    "heading": 0.16754637454996243,
-                    "angularVelocity": -2.523480385625403e-7,
-                    "velocityX": 3.7518036209390857,
-                    "velocityY": -0.4843614037689742,
-                    "timestamp": 2.726311098660519
-                },
-                {
-                    "x": 6.9688913787459,
-                    "y": 0.8784350451744124,
-                    "heading": 0.1510406339197301,
-                    "angularVelocity": -0.33740145851798203,
-                    "velocityX": 3.6321707845828914,
-                    "velocityY": -0.4650963454209109,
-                    "timestamp": 2.7752312802825334
-                },
-                {
-                    "x": 7.1269915101358645,
-                    "y": 0.8581344466937117,
-                    "heading": 0.12402265057850995,
-                    "angularVelocity": -0.5522870611964742,
-                    "velocityX": 3.2317977192222487,
-                    "velocityY": -0.4149738984526803,
-                    "timestamp": 2.824151461904548
-                },
-                {
-                    "x": 7.265299167873697,
-                    "y": 0.8403581253912503,
-                    "heading": 0.0960275261585182,
-                    "angularVelocity": -0.5722612527545016,
-                    "velocityX": 2.8272106347943935,
-                    "velocityY": -0.3633739841730647,
-                    "timestamp": 2.8730716435265626
-                },
-                {
-                    "x": 7.383819928892336,
-                    "y": 0.8251162922066029,
-                    "heading": 0.06979646830524168,
-                    "angularVelocity": -0.5362011542792869,
-                    "velocityX": 2.4227375510254454,
-                    "velocityY": -0.3115653433671744,
-                    "timestamp": 2.9219918251485772
-                },
-                {
-                    "x": 7.482566860925546,
-                    "y": 0.8124124441531555,
-                    "heading": 0.04664460944238867,
-                    "angularVelocity": -0.47325782724474763,
-                    "velocityX": 2.01853159083065,
-                    "velocityY": -0.2596852184974389,
-                    "timestamp": 2.970912006770592
-                },
-                {
-                    "x": 7.5615507925764724,
-                    "y": 0.8022481812485672,
-                    "heading": 0.027346506217350703,
-                    "angularVelocity": -0.3944814304686405,
-                    "velocityX": 1.6145469831081642,
-                    "velocityY": -0.20777238692863387,
-                    "timestamp": 3.0198321883926065
-                },
-                {
-                    "x": 7.620780133649161,
-                    "y": 0.7946243691967044,
-                    "heading": 0.012413272398069258,
-                    "angularVelocity": -0.3052571213791523,
-                    "velocityX": 1.2107342840696753,
-                    "velocityY": -0.15584185910773468,
-                    "timestamp": 3.068752370014621
-                },
-                {
-                    "x": 7.660261458521305,
-                    "y": 0.7895415301241143,
-                    "heading": 0.002207571288741933,
-                    "angularVelocity": -0.2086194443876447,
-                    "velocityX": 0.8070559749184711,
-                    "velocityY": -0.10390065825721975,
-                    "timestamp": 3.1176725516366357
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": -0.003,
-                    "angularVelocity": -0.10645036702804238,
-                    "velocityX": 0.40348463199107626,
-                    "velocityY": -0.05195258970523994,
-                    "timestamp": 3.1665927332586503
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": -0.003,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -1.0902826040652685e-32,
-                    "timestamp": 3.215512914880665
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "W2 To W1": {
-            "waypoints": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 10
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": -5.451838641793523e-34,
-                    "angularVelocity": -2.0131697388730357e-33,
-                    "velocityX": -3.0924959567962797e-34,
-                    "velocityY": 4.789643826321005e-35,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.2520020156777094,
-                    "y": 5.6078780285533805,
-                    "heading": 0.01582108665485387,
-                    "angularVelocity": 0.208254594271014,
-                    "velocityX": 0.026352738707599337,
-                    "velocityY": 0.630223424496655,
-                    "timestamp": 0.07596992858781765
-                },
-                {
-                    "x": 2.2560055471462697,
-                    "y": 5.703634194455967,
-                    "heading": 0.04745824180691435,
-                    "angularVelocity": 0.4164431340157103,
-                    "velocityX": 0.05269889735295001,
-                    "velocityY": 1.2604482810839739,
-                    "timestamp": 0.1519398571756353
-                },
-                {
-                    "x": 2.262009891384212,
-                    "y": 5.847268762594484,
-                    "heading": 0.09489870943903907,
-                    "angularVelocity": 0.6244637649920359,
-                    "velocityX": 0.07903580205425154,
-                    "velocityY": 1.8906766244025437,
-                    "timestamp": 0.22790978576345294
-                },
-                {
-                    "x": 2.2700142224347877,
-                    "y": 6.038782263911744,
-                    "heading": 0.1581161098401003,
-                    "angularVelocity": 0.8321371571119077,
-                    "velocityX": 0.1053618346017344,
-                    "velocityY": 2.5209119565760743,
-                    "timestamp": 0.3038797143512706
-                },
-                {
-                    "x": 2.280009491675263,
-                    "y": 6.278187226323642,
-                    "heading": 0.23645965423367718,
-                    "angularVelocity": 1.0312441494928544,
-                    "velocityX": 0.13156875919557368,
-                    "velocityY": 3.151312194997739,
-                    "timestamp": 0.37984964293908824
-                },
-                {
-                    "x": 2.288005657404111,
-                    "y": 6.469712008115255,
-                    "heading": 0.2990934539478391,
-                    "angularVelocity": 0.8244551611202343,
-                    "velocityX": 0.10525435363026732,
-                    "velocityY": 2.52106044262263,
-                    "timestamp": 0.4558195715269059
-                },
-                {
-                    "x": 2.294002815862431,
-                    "y": 6.613355952122642,
-                    "heading": 0.3460499471183657,
-                    "angularVelocity": 0.6180931592721867,
-                    "velocityX": 0.07894121489645621,
-                    "velocityY": 1.8908000399308182,
-                    "timestamp": 0.5317895001147235
-                },
-                {
-                    "x": 2.2980009414583966,
-                    "y": 6.709118669367686,
-                    "heading": 0.3773492268804029,
-                    "angularVelocity": 0.4119956454330042,
-                    "velocityX": 0.05262773929482027,
-                    "velocityY": 1.260534517079955,
-                    "timestamp": 0.6077594287025412
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "angularVelocity": 0.20601273965271094,
-                    "velocityX": 0.026313813620245378,
-                    "velocityY": 0.6302668901019914,
-                    "timestamp": 0.6837293572903589
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "angularVelocity": 9.82651204832558e-34,
-                    "velocityX": 0,
-                    "velocityY": 3.8258931367738186e-36,
-                    "timestamp": 0.7596992858781766
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "W2 To W3": {
-            "waypoints": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 10
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 5.7052362935559275e-34,
-                    "velocityX": 0,
-                    "velocityY": -1.2494033787640354e-35,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.2520019447146944,
-                    "y": 5.512001877264397,
-                    "heading": -0.01569721301150974,
-                    "angularVelocity": -0.20637467835572687,
-                    "velocityX": 0.02632000319280478,
-                    "velocityY": -0.6310417737196751,
-                    "timestamp": 0.07606172005488261
-                },
-                {
-                    "x": 2.2560053559202022,
-                    "y": 5.416005527882177,
-                    "heading": -0.04708679827781808,
-                    "angularVelocity": -0.41268571422864314,
-                    "velocityX": 0.05263371907208186,
-                    "velocityY": -1.262084913580087,
-                    "timestamp": 0.15212344010976522
-                },
-                {
-                    "x": 2.262009560249172,
-                    "y": 5.272010699235591,
-                    "heading": -0.0941564711571504,
-                    "angularVelocity": -0.6188352412405218,
-                    "velocityX": 0.07893858204412474,
-                    "velocityY": -1.8931313746610776,
-                    "timestamp": 0.22818516016464785
-                },
-                {
-                    "x": 2.2700137636187,
-                    "y": 5.080016884298785,
-                    "heading": -0.15688079244309955,
-                    "angularVelocity": -0.8246503134650409,
-                    "velocityX": 0.10523300503528431,
-                    "velocityY": -2.5241845017213995,
-                    "timestamp": 0.30424688021953045
-                },
-                {
-                    "x": 2.2800092014745617,
-                    "y": 4.840012168782865,
-                    "heading": -0.23463455266722952,
-                    "angularVelocity": -1.0222456206358,
-                    "velocityX": 0.13141219852311892,
-                    "velocityY": -3.155394268532757,
-                    "timestamp": 0.38030860027441304
-                },
-                {
-                    "x": 2.2880054914438257,
-                    "y": 4.648007616430203,
-                    "heading": -0.2967978427641432,
-                    "angularVelocity": -0.8172743142287549,
-                    "velocityX": 0.10512896583845424,
-                    "velocityY": -2.524325668866294,
-                    "timestamp": 0.45637032032929564
-                },
-                {
-                    "x": 2.2940027351611283,
-                    "y": 4.5040038585170965,
-                    "heading": -0.3434020516948695,
-                    "angularVelocity": -0.6127156853289512,
-                    "velocityX": 0.07884706936650254,
-                    "velocityY": -1.8932487696728393,
-                    "timestamp": 0.5324320403841782
-                },
-                {
-                    "x": 2.2980009144821483,
-                    "y": 4.408001268437703,
-                    "heading": -0.37446662411182524,
-                    "angularVelocity": -0.40841269950956927,
-                    "velocityX": 0.05256493434720428,
-                    "velocityY": -1.2621669613850792,
-                    "timestamp": 0.6084937604390609
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": -0.20422067601109492,
-                    "velocityX": 0.026282412709165117,
-                    "velocityY": -0.6310831309503169,
-                    "timestamp": 0.6845554804939435
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": 8.777318885868316e-35,
-                    "velocityX": -1.1404320857258046e-34,
-                    "velocityY": 1.134511753128188e-36,
-                    "timestamp": 0.7606172005488262
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "W2 To S1": {
-            "waypoints": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 7
-                },
-                {
-                    "x": 2.3068509101867676,
-                    "y": 6.379708766937256,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 14
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.263,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": -9.427656700789375e-18,
-                    "angularVelocity": -1.3516738833302546e-18,
-                    "velocityX": 8.730641062152612e-18,
-                    "velocityY": -7.295689550902649e-17,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.220337966696873,
-                    "y": 5.62696613740055,
-                    "heading": -0.005158227148275901,
-                    "angularVelocity": -0.05503188081512997,
-                    "velocityX": -0.31645707615215996,
-                    "velocityY": 0.7144455616516493,
-                    "timestamp": 0.09373161650796653
-                },
-                {
-                    "x": 2.1631184668461043,
-                    "y": 5.761793353879717,
-                    "heading": -0.015168353452777405,
-                    "angularVelocity": -0.10679562219702789,
-                    "velocityX": -0.6104610374014564,
-                    "velocityY": 1.4384390401258744,
-                    "timestamp": 0.18746323301593307
-                },
-                {
-                    "x": 2.0853065565036237,
-                    "y": 5.966914141221312,
-                    "heading": -0.029010799448120003,
-                    "angularVelocity": -0.14768171627730414,
-                    "velocityX": -0.8301564961900216,
-                    "velocityY": 2.1883841865051155,
-                    "timestamp": 0.2811948495238996
-                },
-                {
-                    "x": 2.0799249337959744,
-                    "y": 6.161512978496455,
-                    "heading": -0.03183861533006777,
-                    "angularVelocity": -0.0301692853201504,
-                    "velocityX": -0.05741523413491959,
-                    "velocityY": 2.076128040089899,
-                    "timestamp": 0.37492646603186613
-                },
-                {
-                    "x": 2.1174615185646393,
-                    "y": 6.2967780571782885,
-                    "heading": -0.027602730416951004,
-                    "angularVelocity": 0.045191634060389424,
-                    "velocityX": 0.4004687656856407,
-                    "velocityY": 1.443110486314265,
-                    "timestamp": 0.46865808253983265
-                },
-                {
-                    "x": 2.193636869673838,
-                    "y": 6.369832500172102,
-                    "heading": -0.016913451231696384,
-                    "angularVelocity": 0.11404134040882687,
-                    "velocityX": 0.8126964406159006,
-                    "velocityY": 0.7794002249774918,
-                    "timestamp": 0.5623896990477992
-                },
-                {
-                    "x": 2.306850910186767,
-                    "y": 6.379708766937256,
-                    "heading": 7.831941019246608e-17,
-                    "angularVelocity": 0.1804455301403977,
-                    "velocityX": 1.20785328079034,
-                    "velocityY": 0.10536750707073723,
-                    "timestamp": 0.6561213155557658
-                },
-                {
-                    "x": 2.454400050444712,
-                    "y": 6.392580268241816,
-                    "heading": 0.024770297736306776,
-                    "angularVelocity": 0.3129674474840377,
-                    "velocityX": 1.864252028642711,
-                    "velocityY": 0.1626286834118752,
-                    "timestamp": 0.7352678783546651
-                },
-                {
-                    "x": 2.653900880802349,
-                    "y": 6.4099837935603645,
-                    "heading": 0.06003005706528144,
-                    "angularVelocity": 0.445499565389401,
-                    "velocityX": 2.520650591795653,
-                    "velocityY": 0.2198898436406934,
-                    "timestamp": 0.8144144411535644
-                },
-                {
-                    "x": 2.905353386807521,
-                    "y": 6.431919341632162,
-                    "heading": 0.10577908294347244,
-                    "angularVelocity": 0.5780292189622053,
-                    "velocityX": 3.177048972348669,
-                    "velocityY": 0.2771509879403426,
-                    "timestamp": 0.8935610039524637
-                },
-                {
-                    "x": 3.203627341926949,
-                    "y": 6.457939375335675,
-                    "heading": 0.10577908974967822,
-                    "angularVelocity": 8.599496273628574e-8,
-                    "velocityX": 3.768628030977189,
-                    "velocityY": 0.3287575958241692,
-                    "timestamp": 0.972707566751363
-                },
-                {
-                    "x": 3.501901297052475,
-                    "y": 6.48395940903972,
-                    "heading": 0.10577909654992135,
-                    "angularVelocity": 8.591962579346019e-8,
-                    "velocityX": 3.7686280310542424,
-                    "velocityY": 0.328757595830891,
-                    "timestamp": 1.0518541295502624
-                },
-                {
-                    "x": 3.8001752521780374,
-                    "y": 6.509979442743767,
-                    "heading": 0.10577910335027264,
-                    "angularVelocity": 8.592099253060419e-8,
-                    "velocityX": 3.768628031054697,
-                    "velocityY": 0.32875759583093067,
-                    "timestamp": 1.1310006923491618
-                },
-                {
-                    "x": 4.0984492073035454,
-                    "y": 6.53599947644781,
-                    "heading": 0.1057791101506597,
-                    "angularVelocity": 8.592144417290625e-8,
-                    "velocityX": 3.7686280310540092,
-                    "velocityY": 0.32875759583087066,
-                    "timestamp": 1.2101472551480612
-                },
-                {
-                    "x": 4.396723162417087,
-                    "y": 6.562019510150809,
-                    "heading": 0.10577911696012733,
-                    "angularVelocity": 8.603617553663074e-8,
-                    "velocityX": 3.7686280309028084,
-                    "velocityY": 0.3287575958176806,
-                    "timestamp": 1.2892938179469606
-                },
-                {
-                    "x": 4.656482038612004,
-                    "y": 6.5846796673565295,
-                    "heading": 0.15819030641666595,
-                    "angularVelocity": 0.6622042398696235,
-                    "velocityX": 3.2819981943490006,
-                    "velocityY": 0.28630627034678896,
-                    "timestamp": 1.36844038074586
-                },
-                {
-                    "x": 4.864289197548449,
-                    "y": 6.6028077981790565,
-                    "heading": 0.2001158851669991,
-                    "angularVelocity": 0.5297207770962873,
-                    "velocityX": 2.6255992880506356,
-                    "velocityY": 0.22904508018355874,
-                    "timestamp": 1.4475869435447595
-                },
-                {
-                    "x": 5.020144593431014,
-                    "y": 6.616403898623411,
-                    "heading": 0.2315582663704034,
-                    "angularVelocity": 0.39726780407754475,
-                    "velocityX": 1.9691998031370395,
-                    "velocityY": 0.17178383954462473,
-                    "timestamp": 1.526733506343659
-                },
-                {
-                    "x": 5.124048198578653,
-                    "y": 6.625467966274828,
-                    "heading": 0.2525193347871735,
-                    "angularVelocity": 0.2648386446045577,
-                    "velocityX": 1.3127999684793037,
-                    "velocityY": 0.1145225683956373,
-                    "timestamp": 1.6058800691425583
-                },
-                {
-                    "x": 5.176000000000001,
-                    "y": 6.63,
-                    "heading": 0.26299999999999985,
-                    "angularVelocity": 0.13242097751555232,
-                    "velocityX": 0.6563999696784293,
-                    "velocityY": 0.05726128292756526,
-                    "timestamp": 1.6850266319414577
-                },
-                {
-                    "x": 5.176000000000001,
-                    "y": 6.63,
-                    "heading": 0.26299999999999996,
-                    "angularVelocity": 1.6428599245730974e-17,
-                    "velocityX": 1.9833104251084488e-16,
-                    "velocityY": 1.8748049703629392e-17,
-                    "timestamp": 1.7641731947403572
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        1,
-                        2
-                    ],
-                    "type": "StraightLine"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "W2 To S2": {
-            "waypoints": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 10
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.3319036557469652,
-                    "y": 5.56,
-                    "heading": -1.815106931159875e-19,
-                    "angularVelocity": -1.831407645203353e-18,
-                    "velocityX": 0.8263919813283321,
-                    "velocityY": 8.319303153097966e-34,
-                    "timestamp": 0.099109935233537
-                },
-                {
-                    "x": 2.4957109641637625,
-                    "y": 5.56,
-                    "heading": -5.438547016393454e-19,
-                    "angularVelocity": -3.655980680963076e-18,
-                    "velocityX": 1.6527839316089838,
-                    "velocityY": 1.3987786573815764e-33,
-                    "timestamp": 0.198219870467074
-                },
-                {
-                    "x": 2.741421919096125,
-                    "y": 5.56,
-                    "heading": -1.0860329369861945e-18,
-                    "angularVelocity": -5.4704731072674304e-18,
-                    "velocityX": 2.4791758197942864,
-                    "velocityY": 2.272018181833636e-33,
-                    "timestamp": 0.297329805700611
-                },
-                {
-                    "x": 3.0690365020812695,
-                    "y": 5.56,
-                    "heading": -1.80338172905721e-18,
-                    "angularVelocity": -7.237910009033217e-18,
-                    "velocityX": 3.3055675216936846,
-                    "velocityY": 3.0574601440031788e-33,
-                    "timestamp": 0.396439740934148
-                },
-                {
-                    "x": 3.4439634979187304,
-                    "y": 5.56,
-                    "heading": 1.7928981709293245e-18,
-                    "angularVelocity": 3.6285765816640505e-17,
-                    "velocityX": 3.7829405796099502,
-                    "velocityY": 2.997011014948752e-33,
-                    "timestamp": 0.495549676167685
-                },
-                {
-                    "x": 3.771578080903875,
-                    "y": 5.56,
-                    "heading": 1.0808328664707184e-18,
-                    "angularVelocity": -7.184600643588333e-18,
-                    "velocityX": 3.305567521693685,
-                    "velocityY": -4.3766441461200414e-33,
-                    "timestamp": 0.594659611401222
-                },
-                {
-                    "x": 4.017289035836238,
-                    "y": 5.56,
-                    "heading": 5.416591039281022e-19,
-                    "angularVelocity": -5.4401585595028644e-18,
-                    "velocityX": 2.479175819794286,
-                    "velocityY": -5.119323199602909e-33,
-                    "timestamp": 0.6937695466347591
-                },
-                {
-                    "x": 4.181096344253034,
-                    "y": 5.56,
-                    "heading": 1.8086099379948042e-19,
-                    "angularVelocity": -3.64038286599245e-18,
-                    "velocityX": 1.6527839316089838,
-                    "velocityY": -5.9637817525889115e-33,
-                    "timestamp": 0.792879481868296
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": -1.8248523050897816e-18,
-                    "velocityX": 0.8263919813283321,
-                    "velocityY": -3.8230331267939286e-33,
-                    "timestamp": 0.891989417101833
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 2.0684896902092895e-40,
-                    "timestamp": 0.9910993523353699
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "W2 To S3": {
-            "waypoints": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 28
-                },
-                {
-                    "x": 2.2392399311065674,
-                    "y": 2.659268856048584,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 29
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -4.99624931676796e-32,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.2447003077480563,
-                    "y": 5.548424158968356,
-                    "heading": -0.001651505925702968,
-                    "angularVelocity": -0.042250731032121805,
-                    "velocityX": -0.13558284496887443,
-                    "velocityY": -0.2961465280181771,
-                    "timestamp": 0.03908822132445813
-                },
-                {
-                    "x": 2.234100924566356,
-                    "y": 5.525272476708423,
-                    "heading": -0.0049545293965937804,
-                    "angularVelocity": -0.08450175932731885,
-                    "velocityX": -0.27116565611205723,
-                    "velocityY": -0.5922930610671583,
-                    "timestamp": 0.07817644264891627
-                },
-                {
-                    "x": 2.2182018518980375,
-                    "y": 5.490544953203296,
-                    "heading": -0.009909100000268336,
-                    "angularVelocity": -0.12675354456654908,
-                    "velocityX": -0.4067484303351874,
-                    "velocityY": -0.8884395945486457,
-                    "timestamp": 0.1172646639733744
-                },
-                {
-                    "x": 2.1970030913105703,
-                    "y": 5.444241588640874,
-                    "heading": -0.016515264878529855,
-                    "angularVelocity": -0.16900653584180803,
-                    "velocityX": -0.5423311644575062,
-                    "velocityY": -1.1845861232230646,
-                    "timestamp": 0.15635288529783253
-                },
-                {
-                    "x": 2.170504644501127,
-                    "y": 5.386362383452732,
-                    "heading": -0.024773088013563174,
-                    "angularVelocity": -0.21126116398317848,
-                    "velocityX": -0.6779138551608025,
-                    "velocityY": -1.480732640856411,
-                    "timestamp": 0.19544110662229067
-                },
-                {
-                    "x": 2.1387065133083243,
-                    "y": 5.31690733837683,
-                    "heading": -0.03468264893636917,
-                    "angularVelocity": -0.2535178267784212,
-                    "velocityX": -0.8134964988264785,
-                    "velocityY": -1.7768791396104282,
-                    "timestamp": 0.2345293279467488
-                },
-                {
-                    "x": 2.1016086997403236,
-                    "y": 5.235876454566401,
-                    "heading": -0.04624404029676127,
-                    "angularVelocity": -0.2957768598481422,
-                    "velocityX": -0.9490790911170489,
-                    "velocityY": -2.0730256088611454,
-                    "timestamp": 0.27361754927120696
-                },
-                {
-                    "x": 2.0592112060450187,
-                    "y": 5.143269733801358,
-                    "heading": -0.05945736298631538,
-                    "angularVelocity": -0.33803847404262444,
-                    "velocityX": -1.0846616258994186,
-                    "velocityY": -2.369172032575987,
-                    "timestamp": 0.31270577059566507
-                },
-                {
-                    "x": 2.011514034901109,
-                    "y": 5.039087178972333,
-                    "heading": -0.07432271508161363,
-                    "angularVelocity": -0.3803025973454465,
-                    "velocityX": -1.2202440921521842,
-                    "velocityY": -2.6653183823394446,
-                    "timestamp": 0.3517939919201232
-                },
-                {
-                    "x": 1.9585171900495353,
-                    "y": 4.923328795522167,
-                    "heading": -0.09084016031123929,
-                    "angularVelocity": -0.4225683510264515,
-                    "velocityX": -1.3558264626999135,
-                    "velocityY": -2.9614645928578263,
-                    "timestamp": 0.3908822132445813
-                },
-                {
-                    "x": 1.9002206796290688,
-                    "y": 4.795994598697166,
-                    "heading": -0.10900958241827326,
-                    "angularVelocity": -0.46483113048874997,
-                    "velocityX": -1.491408625032185,
-                    "velocityY": -3.257610413327559,
-                    "timestamp": 0.4299704345690394
-                },
-                {
-                    "x": 1.8386671955063139,
-                    "y": 4.661546724325594,
-                    "heading": -0.10900958703871873,
-                    "angularVelocity": -1.182055697467884e-7,
-                    "velocityX": -1.5747322860207698,
-                    "velocityY": -3.439600724104782,
-                    "timestamp": 0.4690586558934975
-                },
-                {
-                    "x": 1.7777943297173011,
-                    "y": 4.526789325478927,
-                    "heading": -0.10900959166091434,
-                    "angularVelocity": -1.1825034389504455e-7,
-                    "velocityX": -1.5573199221351706,
-                    "velocityY": -3.447519336526256,
-                    "timestamp": 0.5081468772179556
-                },
-                {
-                    "x": 1.728739704549767,
-                    "y": 4.38729485969642,
-                    "heading": -0.10900959636164997,
-                    "angularVelocity": -1.2025964513922604e-7,
-                    "velocityX": -1.2549720479816096,
-                    "velocityY": -3.5687084511883413,
-                    "timestamp": 0.5472350985424137
-                },
-                {
-                    "x": 1.6918738865194354,
-                    "y": 4.244095790476986,
-                    "heading": -0.10900960128999353,
-                    "angularVelocity": -1.2608257379035844e-7,
-                    "velocityX": -0.9431439134643449,
-                    "velocityY": -3.6634838927766116,
-                    "timestamp": 0.5863233198668718
-                },
-                {
-                    "x": 1.6674707596733624,
-                    "y": 4.098254956364247,
-                    "heading": -0.10900960662650709,
-                    "angularVelocity": -1.3652485001388814e-7,
-                    "velocityX": -0.6243089610931135,
-                    "velocityY": -3.7310685718382324,
-                    "timestamp": 0.6254115411913299
-                },
-                {
-                    "x": 1.6557115284805652,
-                    "y": 3.9508548842430677,
-                    "heading": -0.1090096126146074,
-                    "angularVelocity": -1.531944945007552e-7,
-                    "velocityX": -0.3008382268193907,
-                    "velocityY": -3.7709587985000237,
-                    "timestamp": 0.664499762515788
-                },
-                {
-                    "x": 1.6566835066189385,
-                    "y": 3.8029896922639588,
-                    "heading": -0.10900961961577929,
-                    "angularVelocity": -1.79112061302596e-7,
-                    "velocityX": 0.024866266754091815,
-                    "velocityY": -3.7828580316236344,
-                    "timestamp": 0.7035879838402461
-                },
-                {
-                    "x": 1.6703794917330834,
-                    "y": 3.6557569809887474,
-                    "heading": -0.10900969829200789,
-                    "angularVelocity": -0.000002012786100988185,
-                    "velocityX": 0.3503865013576149,
-                    "velocityY": -3.766677180142832,
-                    "timestamp": 0.7426762051647042
-                },
-                {
-                    "x": 1.6964297342683634,
-                    "y": 3.5114772969924033,
-                    "heading": -0.1124967438020359,
-                    "angularVelocity": -0.0892096235606636,
-                    "velocityX": 0.6664473760284892,
-                    "velocityY": -3.6911294274233937,
-                    "timestamp": 0.7817644264891623
-                },
-                {
-                    "x": 1.7337062034817727,
-                    "y": 3.3729422790966246,
-                    "heading": -0.12243166775651844,
-                    "angularVelocity": -0.2541666931326176,
-                    "velocityX": 0.9536496660724848,
-                    "velocityY": -3.54416274779685,
-                    "timestamp": 0.8208526478136204
-                },
-                {
-                    "x": 1.7809992442628195,
-                    "y": 3.2421845220219927,
-                    "heading": -0.1367067464310455,
-                    "angularVelocity": -0.3652015413039525,
-                    "velocityX": 1.2099051626962352,
-                    "velocityY": -3.3451958836719715,
-                    "timestamp": 0.8599408691380785
-                },
-                {
-                    "x": 1.8374227958909133,
-                    "y": 3.120270451905998,
-                    "heading": -0.154016140064332,
-                    "angularVelocity": -0.4428288892862246,
-                    "velocityX": 1.443492431128254,
-                    "velocityY": -3.118946475052685,
-                    "timestamp": 0.8990290904625367
-                },
-                {
-                    "x": 1.9023445230612375,
-                    "y": 3.007828092475748,
-                    "heading": -0.17349166731783877,
-                    "angularVelocity": -0.4982454200677405,
-                    "velocityX": 1.660902567846671,
-                    "velocityY": -2.8766302384778886,
-                    "timestamp": 0.9381173117869948
-                },
-                {
-                    "x": 1.9753011558084586,
-                    "y": 2.90526251520286,
-                    "heading": -0.19452421295735298,
-                    "angularVelocity": -0.5380788617862533,
-                    "velocityX": 1.866460797529241,
-                    "velocityY": -2.623950995915806,
-                    "timestamp": 0.9772055331114529
-                },
-                {
-                    "x": 2.0559421744907826,
-                    "y": 2.8128534355639654,
-                    "heading": -0.21666661660279088,
-                    "angularVelocity": -0.5664725304751266,
-                    "velocityX": 2.0630516291077963,
-                    "velocityY": -2.3641157491367464,
-                    "timestamp": 1.016293754435911
-                },
-                {
-                    "x": 2.1439944855858766,
-                    "y": 2.7308042438703497,
-                    "heading": -0.2395781801271676,
-                    "angularVelocity": -0.586150066389446,
-                    "velocityX": 2.252655866948419,
-                    "velocityY": -2.099077136627731,
-                    "timestamp": 1.055381975760369
-                },
-                {
-                    "x": 2.2392399311065674,
-                    "y": 2.659268856048584,
-                    "heading": -0.2629913490343549,
-                    "angularVelocity": -0.5989827143282399,
-                    "velocityX": 2.4366789353272003,
-                    "velocityY": -1.830100869209989,
-                    "timestamp": 1.0944701970848272
-                },
-                {
-                    "x": 2.3166964235934535,
-                    "y": 2.610513248595422,
-                    "heading": -0.2811797949710667,
-                    "angularVelocity": -0.6046182408202432,
-                    "velocityX": 2.5747998696792043,
-                    "velocityY": -1.620728330008343,
-                    "timestamp": 1.124552726371468
-                },
-                {
-                    "x": 2.398213254515867,
-                    "y": 2.5681176642029904,
-                    "heading": -0.29946946527559826,
-                    "angularVelocity": -0.6079831296849904,
-                    "velocityX": 2.7097731758410535,
-                    "velocityY": -1.409309170398157,
-                    "timestamp": 1.154635255658109
-                },
-                {
-                    "x": 2.483678189488588,
-                    "y": 2.5321524360945276,
-                    "heading": -0.31778023201379,
-                    "angularVelocity": -0.6086844149213316,
-                    "velocityX": 2.8410155993982453,
-                    "velocityY": -1.1955520018204469,
-                    "timestamp": 1.1847177849447499
-                },
-                {
-                    "x": 2.5729562458095865,
-                    "y": 2.5026984837196036,
-                    "heading": -0.33601688083426684,
-                    "angularVelocity": -0.6062205955726221,
-                    "velocityX": 2.9677709434035733,
-                    "velocityY": -0.9791049181495252,
-                    "timestamp": 1.2148003142313908
-                },
-                {
-                    "x": 2.665882293188928,
-                    "y": 2.479849558811985,
-                    "heading": -0.35406456410202075,
-                    "angularVelocity": -0.5999390242685017,
-                    "velocityX": 3.08903704518594,
-                    "velocityY": -0.7595413500609144,
-                    "timestamp": 1.2448828435180317
-                },
-                {
-                    "x": 2.7622502226261916,
-                    "y": 2.4637149411085275,
-                    "heading": -0.37178230702948656,
-                    "angularVelocity": -0.5889711851900935,
-                    "velocityX": 3.2034516951353296,
-                    "velocityY": -0.5363451174508257,
-                    "timestamp": 1.2749653728046726
-                },
-                {
-                    "x": 2.8617965704136163,
-                    "y": 2.4544224370162078,
-                    "heading": -0.3889934776908366,
-                    "angularVelocity": -0.57213176782302,
-                    "velocityX": 3.309108314626244,
-                    "velocityY": -0.30890035886875983,
-                    "timestamp": 1.3050479020913135
-                },
-                {
-                    "x": 2.96417486527032,
-                    "y": 2.452120989561553,
-                    "heading": -0.40547141981393114,
-                    "angularVelocity": -0.5477578685651567,
-                    "velocityX": 3.4032475754010525,
-                    "velocityY": -0.07650445322366382,
-                    "timestamp": 1.3351304313779544
-                },
-                {
-                    "x": 3.068913958464018,
-                    "y": 2.456980552046256,
-                    "heading": -0.4209173641629785,
-                    "angularVelocity": -0.5134523165215639,
-                    "velocityX": 3.4817249638713506,
-                    "velocityY": 0.16154102065001244,
-                    "timestamp": 1.3652129606645953
-                },
-                {
-                    "x": 3.175348297900835,
-                    "y": 2.469181868617686,
-                    "heading": -0.43492680962689906,
-                    "angularVelocity": -0.4657003847794821,
-                    "velocityX": 3.5380781457121575,
-                    "velocityY": 0.4055947708108251,
-                    "timestamp": 1.3952954899512362
-                },
-                {
-                    "x": 3.2825012821379658,
-                    "y": 2.488873812540701,
-                    "heading": -0.44694408076253883,
-                    "angularVelocity": -0.3994767534718848,
-                    "velocityX": 3.5619672540204803,
-                    "velocityY": 0.6545973490252659,
-                    "timestamp": 1.425378019237877
-                },
-                {
-                    "x": 3.3889723525651156,
-                    "y": 2.516050236824646,
-                    "heading": -0.4565561347315361,
-                    "angularVelocity": -0.3195227993435668,
-                    "velocityX": 3.539299153094488,
-                    "velocityY": 0.9033955896790181,
-                    "timestamp": 1.455460548524518
-                },
-                {
-                    "x": 3.4932758480756925,
-                    "y": 2.5504542246131905,
-                    "heading": -0.4662703499641935,
-                    "angularVelocity": -0.32291883239253294,
-                    "velocityX": 3.4672448754798637,
-                    "velocityY": 1.1436534295611822,
-                    "timestamp": 1.485543077811159
-                },
-                {
-                    "x": 3.5930108067726363,
-                    "y": 2.5908608883384305,
-                    "heading": -0.4766346317588353,
-                    "angularVelocity": -0.3445282707409342,
-                    "velocityX": 3.315378096922126,
-                    "velocityY": 1.3431936969204072,
-                    "timestamp": 1.5156256070977998
-                },
-                {
-                    "x": 3.686096614652251,
-                    "y": 2.634834387797071,
-                    "heading": -0.48720467819938723,
-                    "angularVelocity": -0.3513682755805826,
-                    "velocityX": 3.094347785475507,
-                    "velocityY": 1.4617620426674243,
-                    "timestamp": 1.5457081363844407
-                },
-                {
-                    "x": 3.771715868998783,
-                    "y": 2.679896340499022,
-                    "heading": -0.4975881116213702,
-                    "angularVelocity": -0.34516490694787005,
-                    "velocityX": 2.8461454663839936,
-                    "velocityY": 1.497944280967274,
-                    "timestamp": 1.5757906656710816
-                },
-                {
-                    "x": 3.8498203373891897,
-                    "y": 2.724287723719235,
-                    "heading": -0.507530883254748,
-                    "angularVelocity": -0.33051647814035945,
-                    "velocityX": 2.596339810598824,
-                    "velocityY": 1.475653286903962,
-                    "timestamp": 1.6058731949577225
-                },
-                {
-                    "x": 3.920599661686896,
-                    "y": 2.7668759161025953,
-                    "heading": -0.5168796397769617,
-                    "angularVelocity": -0.3107702957131982,
-                    "velocityX": 2.35283820796086,
-                    "velocityY": 1.415711823216824,
-                    "timestamp": 1.6359557242443634
-                },
-                {
-                    "x": 3.984278053809844,
-                    "y": 2.806919241781621,
-                    "heading": -0.525538519539268,
-                    "angularVelocity": -0.287837491315874,
-                    "velocityX": 2.11678983227083,
-                    "velocityY": 1.3311156551191636,
-                    "timestamp": 1.6660382535310043
-                },
-                {
-                    "x": 4.041059171114166,
-                    "y": 2.843909698702473,
-                    "heading": -0.5334440710807248,
-                    "angularVelocity": -0.26279544070665484,
-                    "velocityX": 1.8875114111345854,
-                    "velocityY": 1.229632540814369,
-                    "timestamp": 1.6961207828176452
-                },
-                {
-                    "x": 4.091116341717457,
-                    "y": 2.8774832028649056,
-                    "heading": -0.5405519914461838,
-                    "angularVelocity": -0.23628067632639732,
-                    "velocityX": 1.6639947434713853,
-                    "velocityY": 1.11604658778956,
-                    "timestamp": 1.7262033121042861
-                },
-                {
-                    "x": 4.1345948250930045,
-                    "y": 2.9073683770064043,
-                    "heading": -0.5468299348831553,
-                    "angularVelocity": -0.20869067813911088,
-                    "velocityX": 1.445306774615446,
-                    "velocityY": 0.9934395428236782,
-                    "timestamp": 1.756285841390927
-                },
-                {
-                    "x": 4.171616484177909,
-                    "y": 2.933356232054327,
-                    "heading": -0.552253409487953,
-                    "angularVelocity": -0.18028652288901045,
-                    "velocityX": 1.2306697595852323,
-                    "velocityY": 0.863885306993297,
-                    "timestamp": 1.786368370677568
-                },
-                {
-                    "x": 4.202284260430688,
-                    "y": 2.9552814196152344,
-                    "heading": -0.5568033106587852,
-                    "angularVelocity": -0.15124729464987938,
-                    "velocityX": 1.0194547127524736,
-                    "velocityY": 0.7288345787681281,
-                    "timestamp": 1.8164508999642088
-                },
-                {
-                    "x": 4.226685878711894,
-                    "y": 2.973010149818714,
-                    "heading": -0.5604643670760648,
-                    "angularVelocity": -0.12170041895060278,
-                    "velocityX": 0.8111558057069341,
-                    "velocityY": 0.5893364229633967,
-                    "timestamp": 1.8465334292508497
-                },
-                {
-                    "x": 4.244896778926994,
-                    "y": 2.98643211128414,
-                    "heading": -0.5632241207030344,
-                    "angularVelocity": -0.09173941461743668,
-                    "velocityX": 0.6053646633758383,
-                    "velocityY": 0.4461713088529227,
-                    "timestamp": 1.8766159585374906
-                },
-                {
-                    "x": 4.256982406189367,
-                    "y": 2.9954548894256625,
-                    "heading": -0.565072233243941,
-                    "angularVelocity": -0.06143474583855434,
-                    "velocityX": 0.4017490400230914,
-                    "velocityY": 0.2999341596429636,
-                    "timestamp": 1.9066984878241315
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "angularVelocity": -0.030840716457660636,
-                    "velocityX": 0.20003616561938206,
-                    "velocityY": 0.15108804618886046,
-                    "timestamp": 1.9367810171107724
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "angularVelocity": -4.9732030554004765e-30,
-                    "velocityX": -4.223945033236224e-32,
-                    "velocityY": 9.969983495378875e-32,
-                    "timestamp": 1.9668635463974133
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "W2 To S4": {
-            "waypoints": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 27
-                },
-                {
-                    "x": 2.008638858795166,
-                    "y": 3.158904790878296,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 33
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 1.4317938010297452e-32,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.2473468901462774,
-                    "y": 5.551784954334613,
-                    "heading": -0.004046039178700474,
-                    "angularVelocity": -0.12521248615625988,
-                    "velocityX": -0.08210560159156222,
-                    "velocityY": -0.2542304328304919,
-                    "timestamp": 0.032313384255078216
-                },
-                {
-                    "x": 2.2420392078855844,
-                    "y": 5.535352460743813,
-                    "heading": -0.012062241078351911,
-                    "angularVelocity": -0.24807682898124367,
-                    "velocityX": -0.16425646471427996,
-                    "velocityY": -0.5085352082308542,
-                    "timestamp": 0.06462676851015643
-                },
-                {
-                    "x": 2.234075272195187,
-                    "y": 5.510699784095841,
-                    "heading": -0.023960373728225737,
-                    "angularVelocity": -0.36821066329520014,
-                    "velocityX": -0.24645935032774877,
-                    "velocityY": -0.7629246275588465,
-                    "timestamp": 0.09694015276523466
-                },
-                {
-                    "x": 2.2234531308615284,
-                    "y": 5.477823761292619,
-                    "heading": -0.03963572188039383,
-                    "angularVelocity": -0.4851038822931769,
-                    "velocityX": -0.32872265095504705,
-                    "velocityY": -1.017411935057708,
-                    "timestamp": 0.12925353702031286
-                },
-                {
-                    "x": 2.2101704924088987,
-                    "y": 5.436720678146679,
-                    "heading": -0.05896132607089341,
-                    "angularVelocity": -0.5980680958065965,
-                    "velocityX": -0.4110568657178767,
-                    "velocityY": -1.27201418525146,
-                    "timestamp": 0.16156692127539107
-                },
-                {
-                    "x": 2.1942246366351648,
-                    "y": 5.38738610923438,
-                    "heading": -0.08177969135834087,
-                    "angularVelocity": -0.7061583245915709,
-                    "velocityX": -0.4934752623822669,
-                    "velocityY": -1.5267533887152571,
-                    "timestamp": 0.19388030553046928
-                },
-                {
-                    "x": 2.1756122932404036,
-                    "y": 5.329814710705053,
-                    "heading": -0.10789037911156549,
-                    "angularVelocity": -0.8080455933402793,
-                    "velocityX": -0.5759948647853634,
-                    "velocityY": -1.7816579679449487,
-                    "timestamp": 0.2261936897855475
-                },
-                {
-                    "x": 2.1543294701171383,
-                    "y": 5.263999960188385,
-                    "heading": -0.1370304714941401,
-                    "angularVelocity": -0.9017963625397062,
-                    "velocityX": -0.6586380106540685,
-                    "velocityY": -2.0367643945039555,
-                    "timestamp": 0.25850707404062573
-                },
-                {
-                    "x": 2.1303711997350336,
-                    "y": 5.1899338649176165,
-                    "heading": -0.1688416775308267,
-                    "angularVelocity": -0.9844591264590365,
-                    "velocityX": -0.7414348863300811,
-                    "velocityY": -2.2921181726463207,
-                    "timestamp": 0.29082045829570397
-                },
-                {
-                    "x": 2.1037311567595207,
-                    "y": 5.107606785885568,
-                    "heading": -0.20280965108227242,
-                    "angularVelocity": -1.051204457054369,
-                    "velocityX": -0.8244275116843016,
-                    "velocityY": -2.5477702484570597,
-                    "timestamp": 0.3231338425507822
-                },
-                {
-                    "x": 2.074401135228672,
-                    "y": 5.017008143765364,
-                    "heading": -0.23813648207052604,
-                    "angularVelocity": -1.0932569213236978,
-                    "velocityX": -0.9076740863575303,
-                    "velocityY": -2.8037497219427316,
-                    "timestamp": 0.35544722680586044
-                },
-                {
-                    "x": 2.0423710011934597,
-                    "y": 4.918132506074763,
-                    "heading": -0.27341197795784133,
-                    "angularVelocity": -1.0916682576129122,
-                    "velocityX": -0.9912342756292476,
-                    "velocityY": -3.0598973140692034,
-                    "timestamp": 0.3877606110609387
-                },
-                {
-                    "x": 2.00763899967256,
-                    "y": 4.811032729430715,
-                    "heading": -0.3053550995537523,
-                    "angularVelocity": -0.9885415078704507,
-                    "velocityX": -1.0748487761829064,
-                    "velocityY": -3.3144091562374,
-                    "timestamp": 0.4200739953160169
-                },
-                {
-                    "x": 1.9707337568018743,
-                    "y": 4.697468766706309,
-                    "heading": -0.3150299299334663,
-                    "angularVelocity": -0.2994062863655037,
-                    "velocityX": -1.1421039213769328,
-                    "velocityY": -3.514455862250325,
-                    "timestamp": 0.45238737957109515
-                },
-                {
-                    "x": 1.9331993710024595,
-                    "y": 4.581134395027877,
-                    "heading": -0.3150299515537849,
-                    "angularVelocity": -6.690824588937528e-7,
-                    "velocityX": -1.1615739627617825,
-                    "velocityY": -3.6001915107405935,
-                    "timestamp": 0.4847007638261734
-                },
-                {
-                    "x": 1.8956658385726595,
-                    "y": 4.464799748019651,
-                    "heading": -0.3150299731737692,
-                    "angularVelocity": -6.690721139443934e-7,
-                    "velocityX": -1.1615475535931208,
-                    "velocityY": -3.600200031352131,
-                    "timestamp": 0.5170141480812516
-                },
-                {
-                    "x": 1.8630740613066095,
-                    "y": 4.346985096208152,
-                    "heading": -0.31502999489570194,
-                    "angularVelocity": -6.722271053919432e-7,
-                    "velocityX": -1.0086154086732113,
-                    "velocityY": -3.64600163453891,
-                    "timestamp": 0.5493275323363298
-                },
-                {
-                    "x": 1.8389498768606893,
-                    "y": 4.227149626761845,
-                    "heading": -0.3150300170939308,
-                    "angularVelocity": -6.869670074324573e-7,
-                    "velocityX": -0.7465694170405304,
-                    "velocityY": -3.708539733886721,
-                    "timestamp": 0.581640916591408
-                },
-                {
-                    "x": 1.8234162959667812,
-                    "y": 4.10590102408459,
-                    "heading": -0.31503004024876186,
-                    "angularVelocity": -7.165709048863888e-7,
-                    "velocityX": -0.4807166210536207,
-                    "velocityY": -3.7522718672898008,
-                    "timestamp": 0.6139543008464863
-                },
-                {
-                    "x": 1.816552246405149,
-                    "y": 3.98385431066073,
-                    "heading": -0.315030064931292,
-                    "angularVelocity": -7.63848502164674e-7,
-                    "velocityX": -0.21242125267501,
-                    "velocityY": -3.776970943694307,
-                    "timestamp": 0.6462676851015645
-                },
-                {
-                    "x": 1.818392595454208,
-                    "y": 3.861628584870307,
-                    "heading": -0.315030091871416,
-                    "angularVelocity": -8.337140982566696e-7,
-                    "velocityX": 0.05695315088421205,
-                    "velocityY": -3.782510826646546,
-                    "timestamp": 0.6785810693566428
-                },
-                {
-                    "x": 1.828928030254088,
-                    "y": 3.739843861498511,
-                    "heading": -0.3150301220653929,
-                    "angularVelocity": -9.344108498281474e-7,
-                    "velocityX": 0.32603935003261814,
-                    "velocityY": -3.7688631562216113,
-                    "timestamp": 0.710894453611721
-                },
-                {
-                    "x": 1.8481051197294465,
-                    "y": 3.619117921789427,
-                    "heading": -0.3150301569615331,
-                    "angularVelocity": -0.0000010799283629257,
-                    "velocityX": 0.593472021499706,
-                    "velocityY": -3.7360970536569806,
-                    "timestamp": 0.7432078378667992
-                },
-                {
-                    "x": 1.875826590719106,
-                    "y": 3.500063178055521,
-                    "heading": -0.31503019880828736,
-                    "angularVelocity": -0.0000012950285239246719,
-                    "velocityX": 0.8578943873792161,
-                    "velocityY": -3.684378670896923,
-                    "timestamp": 0.7755212221218775
-                },
-                {
-                    "x": 1.9119518234377388,
-                    "y": 3.3832835666467993,
-                    "heading": -0.31503025136937535,
-                    "angularVelocity": -0.0000016266042432729645,
-                    "velocityX": 1.1179650027822445,
-                    "velocityY": -3.61397031294762,
-                    "timestamp": 0.8078346063769557
-                },
-                {
-                    "x": 1.9562975654606443,
-                    "y": 3.269371484637439,
-                    "heading": -0.31503032157308203,
-                    "angularVelocity": -0.0000021725891085409373,
-                    "velocityX": 1.3723645184560365,
-                    "velocityY": -3.5252290849559675,
-                    "timestamp": 0.8401479906320339
-                },
-                {
-                    "x": 2.008638858795166,
-                    "y": 3.158904790878296,
-                    "heading": -0.3150304332740777,
-                    "angularVelocity": -0.0000034568027529519608,
-                    "velocityX": 1.6198022751608165,
-                    "velocityY": -3.4186049002831713,
-                    "timestamp": 0.8724613748871122
-                },
-                {
-                    "x": 2.0775165641117233,
-                    "y": 3.0394044472847606,
-                    "heading": -0.3150305189714284,
-                    "angularVelocity": -0.000002350392551393865,
-                    "velocityX": 1.8890857675343722,
-                    "velocityY": -3.277495922091173,
-                    "timestamp": 0.9089222420961518
-                },
-                {
-                    "x": 2.155767726009908,
-                    "y": 2.9258208622381168,
-                    "heading": -0.3150305770370975,
-                    "angularVelocity": -0.00000159254767063084,
-                    "velocityX": 2.1461684235196876,
-                    "velocityY": -3.1152189660064753,
-                    "timestamp": 0.9453831093051914
-                },
-                {
-                    "x": 2.242886953120307,
-                    "y": 2.818887620265158,
-                    "heading": -0.3150306203630764,
-                    "angularVelocity": -0.0000011882871183876306,
-                    "velocityX": 2.389389879591231,
-                    "velocityY": -2.932822232666113,
-                    "timestamp": 0.981843976514231
-                },
-                {
-                    "x": 2.338311580267833,
-                    "y": 2.719295349641276,
-                    "heading": -0.3150306549359537,
-                    "angularVelocity": -9.482187337248934e-7,
-                    "velocityX": 2.617179306252704,
-                    "velocityY": -2.731483868798106,
-                    "timestamp": 1.0183048437232707
-                },
-                {
-                    "x": 2.4414253013619946,
-                    "y": 2.6276872624602983,
-                    "heading": -0.31503068395926054,
-                    "angularVelocity": -7.960125215618009e-7,
-                    "velocityX": 2.8280655120730875,
-                    "velocityY": -2.512504342142078,
-                    "timestamp": 1.0547657109323103
-                },
-                {
-                    "x": 2.5515621486647997,
-                    "y": 2.5446549983777076,
-                    "heading": -0.3150307093352344,
-                    "angularVelocity": -6.959783403374032e-7,
-                    "velocityX": 3.020686443670201,
-                    "velocityY": -2.2772981127010774,
-                    "timestamp": 1.09122657814135
-                },
-                {
-                    "x": 2.6680107915990137,
-                    "y": 2.4707347968336117,
-                    "heading": -0.31503073229503964,
-                    "angularVelocity": -6.297108928988803e-7,
-                    "velocityX": 3.1937979496369038,
-                    "velocityY": -2.0273846236374995,
-                    "timestamp": 1.1276874453503896
-                },
-                {
-                    "x": 2.7900191236562386,
-                    "y": 2.4064040113359084,
-                    "heading": -0.3150307537024253,
-                    "angularVelocity": -5.871332010760787e-7,
-                    "velocityX": 3.3462816821585726,
-                    "velocityY": -1.764378919702536,
-                    "timestamp": 1.1641483125594292
-                },
-                {
-                    "x": 2.9167990841955884,
-                    "y": 2.352077913506683,
-                    "heading": -0.3150307742165802,
-                    "angularVelocity": -5.626348607586924e-7,
-                    "velocityX": 3.477151539278742,
-                    "velocityY": -1.4899836999970275,
-                    "timestamp": 1.2006091797684688
-                },
-                {
-                    "x": 3.0475311021850517,
-                    "y": 2.308104934680324,
-                    "heading": -0.31503079438993686,
-                    "angularVelocity": -5.532878997498012e-7,
-                    "velocityX": 3.585543296047864,
-                    "velocityY": -1.2060321707174542,
-                    "timestamp": 1.2370700469775084
-                },
-                {
-                    "x": 3.178817728061225,
-                    "y": 2.2658165761507623,
-                    "heading": -0.3150308145386853,
-                    "angularVelocity": -5.526129783451472e-7,
-                    "velocityX": 3.600754340906743,
-                    "velocityY": -1.159828653748452,
-                    "timestamp": 1.273530914186548
-                },
-                {
-                    "x": 3.3101043979960063,
-                    "y": 2.2235283544039026,
-                    "heading": -0.3150308346874411,
-                    "angularVelocity": -5.526131795163358e-7,
-                    "velocityX": 3.600755549287413,
-                    "velocityY": -1.1598249022550946,
-                    "timestamp": 1.3099917813955877
-                },
-                {
-                    "x": 3.4413910762638236,
-                    "y": 2.1812401585275256,
-                    "heading": -0.31503085483620696,
-                    "angularVelocity": -5.52613457009699e-7,
-                    "velocityX": 3.60075577783481,
-                    "velocityY": -1.159824192714005,
-                    "timestamp": 1.3464526486046273
-                },
-                {
-                    "x": 3.572677762945226,
-                    "y": 2.138951988771722,
-                    "heading": -0.31503087498498206,
-                    "angularVelocity": -5.52613710159953e-7,
-                    "velocityX": 3.6007560085914068,
-                    "velocityY": -1.1598234763137998,
-                    "timestamp": 1.382913515813667
-                },
-                {
-                    "x": 3.7039644581047573,
-                    "y": 2.0966638453368867,
-                    "heading": -0.31503089513376514,
-                    "angularVelocity": -5.526139295643368e-7,
-                    "velocityX": 3.6007562411181984,
-                    "velocityY": -1.1598227544173942,
-                    "timestamp": 1.4193743830227066
-                },
-                {
-                    "x": 3.8352511618245644,
-                    "y": 2.05437572847807,
-                    "heading": -0.3150309152825546,
-                    "angularVelocity": -5.526141044747151e-7,
-                    "velocityX": 3.600756475898009,
-                    "velocityY": -1.1598220255258234,
-                    "timestamp": 1.4558352502317462
-                },
-                {
-                    "x": 3.966537874250263,
-                    "y": 2.01208763864737,
-                    "heading": -0.3150309354313486,
-                    "angularVelocity": -5.526142273042023e-7,
-                    "velocityX": 3.6007567146716046,
-                    "velocityY": -1.159821284234734,
-                    "timestamp": 1.4922961174407858
-                },
-                {
-                    "x": 4.097824595645338,
-                    "y": 1.9697995766628096,
-                    "heading": -0.3150309555801447,
-                    "angularVelocity": -5.526142864672618e-7,
-                    "velocityX": 3.600756960671674,
-                    "velocityY": -1.159820520507965,
-                    "timestamp": 1.5287569846498255
-                },
-                {
-                    "x": 4.229111326453571,
-                    "y": 1.9275115439021684,
-                    "heading": -0.3150309757289402,
-                    "angularVelocity": -5.5261426880692e-7,
-                    "velocityX": 3.600757218843208,
-                    "velocityY": -1.1598197189933317,
-                    "timestamp": 1.565217851858865
-                },
-                {
-                    "x": 4.360398019008411,
-                    "y": 1.885223392382287,
-                    "heading": -0.31503099587881916,
-                    "angularVelocity": -5.52643987126892e-7,
-                    "velocityX": 3.6007561696802117,
-                    "velocityY": -1.1598229761632481,
-                    "timestamp": 1.6016787190679047
-                },
-                {
-                    "x": 4.485161526388635,
-                    "y": 1.844329974236374,
-                    "heading": -0.3370638509456068,
-                    "angularVelocity": -0.6042877406197901,
-                    "velocityX": 3.4218469534726492,
-                    "velocityY": -1.1215700907896822,
-                    "timestamp": 1.6381395862769443
-                },
-                {
-                    "x": 4.60044458089581,
-                    "y": 1.8067761222092775,
-                    "heading": -0.3770388194107599,
-                    "angularVelocity": -1.0963800788381923,
-                    "velocityX": 3.1618297460185825,
-                    "velocityY": -1.0299769287381584,
-                    "timestamp": 1.674600453485984
-                },
-                {
-                    "x": 4.705219804759057,
-                    "y": 1.772711102552083,
-                    "heading": -0.4189139196427851,
-                    "angularVelocity": -1.148494356756437,
-                    "velocityX": 2.8736349923479074,
-                    "velocityY": -0.9342898911836356,
-                    "timestamp": 1.7110613206950236
-                },
-                {
-                    "x": 4.799468970200479,
-                    "y": 1.7421012617230478,
-                    "heading": -0.45948472427125614,
-                    "angularVelocity": -1.1127218778387968,
-                    "velocityX": 2.5849403115144614,
-                    "velocityY": -0.8395258580532651,
-                    "timestamp": 1.7475221879040632
-                },
-                {
-                    "x": 4.883206648654635,
-                    "y": 1.7149242324789875,
-                    "heading": -0.49729979894596704,
-                    "angularVelocity": -1.037141394852447,
-                    "velocityX": 2.296645276538986,
-                    "velocityY": -0.7453752838144925,
-                    "timestamp": 1.7839830551131028
-                },
-                {
-                    "x": 4.956447680828415,
-                    "y": 1.6911655730288653,
-                    "heading": -0.5315269969459382,
-                    "angularVelocity": -0.9387379023033031,
-                    "velocityX": 2.0087572726635705,
-                    "velocityY": -0.6516207997442213,
-                    "timestamp": 1.8204439223221425
-                },
-                {
-                    "x": 5.019204214404415,
-                    "y": 1.6708154773305386,
-                    "heading": -0.5616288277349688,
-                    "angularVelocity": -0.825592836737831,
-                    "velocityX": 1.7212024392124468,
-                    "velocityY": -0.5581352627093101,
-                    "timestamp": 1.856904789531182
-                },
-                {
-                    "x": 5.071485921099441,
-                    "y": 1.6538669506708767,
-                    "heading": -0.587231204588931,
-                    "angularVelocity": -0.7021878198118158,
-                    "velocityX": 1.4339128687006073,
-                    "velocityY": -0.46484156733002446,
-                    "timestamp": 1.8933656567402217
-                },
-                {
-                    "x": 5.11330055140079,
-                    "y": 1.6403147844077772,
-                    "heading": -0.6080594902506735,
-                    "angularVelocity": -0.5712504187662296,
-                    "velocityX": 1.1468358682094448,
-                    "velocityY": -0.37169072763413913,
-                    "timestamp": 1.9298265239492614
-                },
-                {
-                    "x": 5.1446544158470235,
-                    "y": 1.6301549523614358,
-                    "heading": -0.6239033665029361,
-                    "angularVelocity": -0.43454469037794713,
-                    "velocityX": 0.859931944747064,
-                    "velocityY": -0.2786503126240163,
-                    "timestamp": 1.966287391158301
-                },
-                {
-                    "x": 5.165552752490355,
-                    "y": 1.6233842390024875,
-                    "heading": -0.6345958555798376,
-                    "angularVelocity": -0.29325931869912014,
-                    "velocityX": 0.5731716835893133,
-                    "velocityY": -0.18569808886140712,
-                    "timestamp": 2.0027482583673404
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "angularVelocity": -0.14821766002380998,
-                    "velocityX": 0.2865331603263514,
-                    "velocityY": -0.09281839027812194,
-                    "timestamp": 2.03920912557638
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "angularVelocity": 2.046404654845657e-31,
-                    "velocityX": 2.793064044552828e-33,
-                    "velocityY": 5.221456528298987e-32,
-                    "timestamp": 2.0756699927854196
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "W2 To C1": {
-            "waypoints": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 7
-                },
-                {
-                    "x": 2.38835072517395,
-                    "y": 6.318583965301514,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 8
-                },
-                {
-                    "x": 3.5700972080230713,
-                    "y": 6.2574591636657715,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 16
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": -5.349509897792121e-32,
-                    "angularVelocity": -4.120122643775229e-32,
-                    "velocityX": 8.47273432885147e-31,
-                    "velocityY": -1.4633096492322666e-31,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.237003637990514,
-                    "y": 5.5993039448743,
-                    "heading": -2.4015234029354595e-10,
-                    "angularVelocity": -3.4082853646292762e-9,
-                    "velocityX": -0.1844472530475389,
-                    "velocityY": 0.5578103057326728,
-                    "timestamp": 0.070461130729763
-                },
-                {
-                    "x": 2.214179391204466,
-                    "y": 5.678821295080181,
-                    "heading": -7.831932260526302e-10,
-                    "angularVelocity": -7.70693396452574e-9,
-                    "velocityX": -0.32392677423960453,
-                    "velocityY": 1.1285278760503061,
-                    "timestamp": 0.140922261459526
-                },
-                {
-                    "x": 2.1888413071411663,
-                    "y": 5.799659166026344,
-                    "heading": -1.7630537350139963e-9,
-                    "angularVelocity": -1.3906355788165327e-8,
-                    "velocityX": -0.3596037106073644,
-                    "velocityY": 1.714957873876257,
-                    "timestamp": 0.21138339218928898
-                },
-                {
-                    "x": 2.184731423610446,
-                    "y": 5.956036692159833,
-                    "heading": -3.5093546140825654e-9,
-                    "angularVelocity": -2.4783811808465378e-8,
-                    "velocityX": -0.0583283788886903,
-                    "velocityY": 2.219344545207332,
-                    "timestamp": 0.281844522919052
-                },
-                {
-                    "x": 2.2210562385399535,
-                    "y": 6.10354071192916,
-                    "heading": -5.82104685853391e-9,
-                    "angularVelocity": -3.2807944511631915e-8,
-                    "velocityX": 0.5155298325992919,
-                    "velocityY": 2.0934097741478808,
-                    "timestamp": 0.352305653648815
-                },
-                {
-                    "x": 2.290250351402012,
-                    "y": 6.2258789967414945,
-                    "heading": -8.381712631190757e-9,
-                    "angularVelocity": -3.6341419943068046e-8,
-                    "velocityX": 0.9820182013148953,
-                    "velocityY": 1.7362520802853605,
-                    "timestamp": 0.422766784378578
-                },
-                {
-                    "x": 2.38835072517395,
-                    "y": 6.318583965301514,
-                    "heading": -1.1074238763723672e-8,
-                    "angularVelocity": -3.821280465498069e-8,
-                    "velocityX": 1.392262269357751,
-                    "velocityY": 1.315689481497163,
-                    "timestamp": 0.493227915108341
-                },
-                {
-                    "x": 2.4799014983496455,
-                    "y": 6.371301429692481,
-                    "heading": -1.319458225812021e-8,
-                    "angularVelocity": -3.903749361340101e-8,
-                    "velocityX": 1.6855317920452093,
-                    "velocityY": 0.9705757706637901,
-                    "timestamp": 0.5475435759628242
-                },
-                {
-                    "x": 2.5874215406057264,
-                    "y": 6.405308001046697,
-                    "heading": -1.535577963780212e-8,
-                    "angularVelocity": -3.978965207197302e-8,
-                    "velocityX": 1.979540349233062,
-                    "velocityY": 0.626091459097589,
-                    "timestamp": 0.6018592368173074
-                },
-                {
-                    "x": 2.7109670001208555,
-                    "y": 6.420651747906481,
-                    "heading": -1.7552309308039307e-8,
-                    "angularVelocity": -4.044015305772357e-8,
-                    "velocityX": 2.2745826447141577,
-                    "velocityY": 0.2824921324780203,
-                    "timestamp": 0.6561748976717905
-                },
-                {
-                    "x": 2.850621987061303,
-                    "y": 6.417405226954699,
-                    "heading": -1.9775875083794707e-8,
-                    "angularVelocity": -4.093791389017331e-8,
-                    "velocityX": 2.571173483741952,
-                    "velocityY": -0.059771360579454216,
-                    "timestamp": 0.7104905585262736
-                },
-                {
-                    "x": 3.006526369448995,
-                    "y": 6.395690573007363,
-                    "heading": -2.2012612720253862e-8,
-                    "angularVelocity": -4.1180421619075874e-8,
-                    "velocityX": 2.8703394184132693,
-                    "velocityY": -0.39978624220811715,
-                    "timestamp": 0.7648062193807568
-                },
-                {
-                    "x": 3.178958586879986,
-                    "y": 6.35575655651524,
-                    "heading": -2.4234645855460274e-8,
-                    "angularVelocity": -4.090970062204104e-8,
-                    "velocityX": 3.1746316756057436,
-                    "velocityY": -0.7352210368724164,
-                    "timestamp": 0.8191218802352399
-                },
-                {
-                    "x": 3.368741313034932,
-                    "y": 6.298384837056506,
-                    "heading": -2.6356859092562007e-8,
-                    "angularVelocity": -3.9071928501624834e-8,
-                    "velocityX": 3.494070092598876,
-                    "velocityY": -1.0562647781255272,
-                    "timestamp": 0.873437541089723
-                },
-                {
-                    "x": 3.5700972080230713,
-                    "y": 6.2574591636657715,
-                    "heading": -2.635685976829305e-8,
-                    "angularVelocity": -1.2440816646696897e-14,
-                    "velocityX": 3.7071425040054167,
-                    "velocityY": -0.7534783293344505,
-                    "timestamp": 0.9277532019442062
-                },
-                {
-                    "x": 3.903312814324664,
-                    "y": 6.255439725872096,
-                    "heading": -2.6356859842955344e-8,
-                    "angularVelocity": -8.476143761744691e-16,
-                    "velocityX": 3.7828709995267413,
-                    "velocityY": -0.02292591506032139,
-                    "timestamp": 1.0158385758158621
-                },
-                {
-                    "x": 4.231215212285405,
-                    "y": 6.314741822504602,
-                    "heading": -2.6356859970624137e-8,
-                    "angularVelocity": -1.4493758784250442e-15,
-                    "velocityX": 3.722552150839135,
-                    "velocityY": 0.6732343181536394,
-                    "timestamp": 1.103923949687518
-                },
-                {
-                    "x": 4.548594281876572,
-                    "y": 6.416266440930298,
-                    "heading": -2.6357038616847798e-8,
-                    "angularVelocity": -2.0281055655946166e-12,
-                    "velocityX": 3.603084776124604,
-                    "velocityY": 1.1525706703347043,
-                    "timestamp": 1.1920093235591738
-                },
-                {
-                    "x": 4.864492921080643,
-                    "y": 6.522307835013861,
-                    "heading": -2.6356860621748925e-8,
-                    "angularVelocity": 2.02071358534527e-12,
-                    "velocityX": 3.5862780084505173,
-                    "velocityY": 1.2038479203550279,
-                    "timestamp": 1.2800946974308296
-                },
-                {
-                    "x": 5.180407145227995,
-                    "y": 6.628302763705304,
-                    "heading": -2.6356859722122177e-8,
-                    "angularVelocity": 1.021313250736518e-14,
-                    "velocityX": 3.586454938732789,
-                    "velocityY": 1.2033204153921748,
-                    "timestamp": 1.3681800713024854
-                },
-                {
-                    "x": 5.4963196348481045,
-                    "y": 6.734302860663818,
-                    "heading": -2.6356858783882045e-8,
-                    "angularVelocity": 1.0651495820397439e-14,
-                    "velocityX": 3.58643524725062,
-                    "velocityY": 1.20337908892381,
-                    "timestamp": 1.4562654451741412
-                },
-                {
-                    "x": 5.81223242108415,
-                    "y": 6.8403020736225555,
-                    "heading": -2.6356857847826716e-8,
-                    "angularVelocity": 1.0626692486301617e-14,
-                    "velocityX": 3.586438614621235,
-                    "velocityY": 1.2033690532024592,
-                    "timestamp": 1.544350819045797
-                },
-                {
-                    "x": 6.128145213013902,
-                    "y": 6.946301269622288,
-                    "heading": -2.6356856889534724e-8,
-                    "angularVelocity": 1.0879135888139458e-14,
-                    "velocityX": 3.5864386792612257,
-                    "velocityY": 1.2033688606688582,
-                    "timestamp": 1.632436192917453
-                },
-                {
-                    "x": 6.444057998192988,
-                    "y": 7.052300485742325,
-                    "heading": -2.6356856001580027e-8,
-                    "angularVelocity": 1.0080624290280854e-14,
-                    "velocityX": 3.5864386026236104,
-                    "velocityY": 1.2033690890866424,
-                    "timestamp": 1.7205215667891087
-                },
-                {
-                    "x": 6.759970783516966,
-                    "y": 7.158299701416182,
-                    "heading": -2.635685511579164e-8,
-                    "angularVelocity": 1.0056027630784321e-14,
-                    "velocityX": 3.586438604268464,
-                    "velocityY": 1.2033690840214906,
-                    "timestamp": 1.8086069406607646
-                },
-                {
-                    "x": 7.0666471679768685,
-                    "y": 7.261199790661366,
-                    "heading": -1.748457582258916e-8,
-                    "angularVelocity": 1.0072373848515286e-7,
-                    "velocityX": 3.4815812317231813,
-                    "velocityY": 1.1681858715050373,
-                    "timestamp": 1.8966923145324204
-                },
-                {
-                    "x": 7.311988294075674,
-                    "y": 7.343519871122703,
-                    "heading": -1.0463867035430883e-8,
-                    "angularVelocity": 7.970353325086049e-8,
-                    "velocityX": 2.7852651957526704,
-                    "velocityY": 0.9345488001177432,
-                    "timestamp": 1.9847776884040762
-                },
-                {
-                    "x": 7.495994144762843,
-                    "y": 7.405259934452133,
-                    "heading": -5.222908649794349e-9,
-                    "angularVelocity": 5.94986783168245e-8,
-                    "velocityX": 2.088948966213813,
-                    "velocityY": 0.7009116339580341,
-                    "timestamp": 2.072863062275732
-                },
-                {
-                    "x": 7.618664714352163,
-                    "y": 7.446419977873495,
-                    "heading": -1.738724662079323e-9,
-                    "angularVelocity": 3.955466249815561e-8,
-                    "velocityX": 1.3926326721215396,
-                    "velocityY": 0.4672744362816174,
-                    "timestamp": 2.160948436147388
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": -8.289937690891028e-31,
-                    "angularVelocity": 1.9739103024170296e-8,
-                    "velocityX": 0.6963163457465601,
-                    "velocityY": 0.23363722286151184,
-                    "timestamp": 2.2490338100190437
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 1.1211425849455439e-31,
-                    "angularVelocity": 1.0684035256675139e-29,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 2.3371191838906995
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "W2 To C2": {
-            "waypoints": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 6
-                },
-                {
-                    "x": 2.1438515186309814,
-                    "y": 6.298208713531494,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 8
-                },
-                {
-                    "x": 3.345972776412964,
-                    "y": 6.298208713531494,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 26
-                },
-                {
-                    "x": 5.403600215911865,
-                    "y": 6.822902202606201,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 27
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 5.330261523342824e-32,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.216523220338838,
-                    "y": 5.608939409819235,
-                    "heading": -0.000395848810607471,
-                    "angularVelocity": -0.004694167537861253,
-                    "velocityX": -0.3969839194829395,
-                    "velocityY": 0.5803473011402309,
-                    "timestamp": 0.08432779772229798
-                },
-                {
-                    "x": 2.1530740053514283,
-                    "y": 5.7090393916763,
-                    "heading": -0.0013136261454272055,
-                    "angularVelocity": -0.010883449581379904,
-                    "velocityX": -0.7524116210926717,
-                    "velocityY": 1.1870342231242381,
-                    "timestamp": 0.16865559544459596
-                },
-                {
-                    "x": 2.071477218025725,
-                    "y": 5.86558734321221,
-                    "heading": -0.0031284327789465032,
-                    "angularVelocity": -0.02152085886904205,
-                    "velocityX": -0.9676143517279073,
-                    "velocityY": 1.8564216754650953,
-                    "timestamp": 0.25298339316689394
-                },
-                {
-                    "x": 2.0458269370451427,
-                    "y": 6.041770495606775,
-                    "heading": -0.006595224749493174,
-                    "angularVelocity": -0.04111090368993278,
-                    "velocityX": -0.30417349525777404,
-                    "velocityY": 2.089265427928733,
-                    "timestamp": 0.3373111908891919
-                },
-                {
-                    "x": 2.0715234655116643,
-                    "y": 6.188301290095102,
-                    "heading": -0.01049068188832321,
-                    "angularVelocity": -0.046194223542478534,
-                    "velocityX": 0.30472192041755103,
-                    "velocityY": 1.7376333598900595,
-                    "timestamp": 0.4216389886114899
-                },
-                {
-                    "x": 2.1438515186309814,
-                    "y": 6.298208713531494,
-                    "heading": -0.014554322717054646,
-                    "angularVelocity": -0.04818862745724922,
-                    "velocityX": 0.8577011978600798,
-                    "velocityY": 1.303335630776598,
-                    "timestamp": 0.5059667863337879
-                },
-                {
-                    "x": 2.2162950329491684,
-                    "y": 6.355974505839093,
-                    "heading": -0.017431077998725047,
-                    "angularVelocity": -0.04874949538291163,
-                    "velocityX": 1.2276278031981451,
-                    "velocityY": 0.978899124069331,
-                    "timestamp": 0.5649777619156877
-                },
-                {
-                    "x": 2.310686009240105,
-                    "y": 6.394730061346725,
-                    "heading": -0.020327199149560698,
-                    "angularVelocity": -0.0490776694043422,
-                    "velocityX": 1.5995494966853068,
-                    "velocityY": 0.6567516487478545,
-                    "timestamp": 0.6239887374975874
-                },
-                {
-                    "x": 2.4271886355684367,
-                    "y": 6.414666608347948,
-                    "heading": -0.023223370555002502,
-                    "angularVelocity": -0.04907852101890474,
-                    "velocityX": 1.9742535211376202,
-                    "velocityY": 0.33784472811424154,
-                    "timestamp": 0.6829997130794871
-                },
-                {
-                    "x": 2.5660476863967387,
-                    "y": 6.416075601325133,
-                    "heading": -0.02609042518710973,
-                    "angularVelocity": -0.048585108174133695,
-                    "velocityX": 2.35310549366504,
-                    "velocityY": 0.02387679517735602,
-                    "timestamp": 0.7420106886613869
-                },
-                {
-                    "x": 2.72766661902605,
-                    "y": 6.399454975329908,
-                    "heading": -0.02887926513240676,
-                    "angularVelocity": -0.04725968208112396,
-                    "velocityX": 2.738794453669127,
-                    "velocityY": -0.281653130308932,
-                    "timestamp": 0.8010216642432866
-                },
-                {
-                    "x": 2.912832200300888,
-                    "y": 6.365845166245685,
-                    "heading": -0.031490096116184224,
-                    "angularVelocity": -0.04424314219571676,
-                    "velocityX": 3.1378159647242496,
-                    "velocityY": -0.5695518291775498,
-                    "timestamp": 0.8600326398251863
-                },
-                {
-                    "x": 3.123679163604623,
-                    "y": 6.318688161285352,
-                    "heading": -0.033616023768519236,
-                    "angularVelocity": -0.03602597027708684,
-                    "velocityX": 3.5730126679757386,
-                    "velocityY": -0.7991226122822729,
-                    "timestamp": 0.919043615407086
-                },
-                {
-                    "x": 3.345972776412964,
-                    "y": 6.298208713531494,
-                    "heading": -0.03361602546428586,
-                    "angularVelocity": -2.8736461454443448e-8,
-                    "velocityX": 3.766987591991692,
-                    "velocityY": -0.34704472434005124,
-                    "timestamp": 0.9780545909889857
-                },
-                {
-                    "x": 3.4285825306560853,
-                    "y": 6.294597584481868,
-                    "heading": -0.033616025709707474,
-                    "angularVelocity": -1.1227840838251836e-8,
-                    "velocityX": 3.779329612797739,
-                    "velocityY": -0.16520624081174923,
-                    "timestamp": 0.9999128999238256
-                },
-                {
-                    "x": 3.5112703400714786,
-                    "y": 6.2949695138762145,
-                    "heading": -0.033616025930811776,
-                    "angularVelocity": -1.0115343355589466e-8,
-                    "velocityX": 3.7829005739596693,
-                    "velocityY": 0.01701546974451014,
-                    "timestamp": 1.0217712088586655
-                },
-                {
-                    "x": 3.5938442713679413,
-                    "y": 6.299323633866166,
-                    "heading": -0.03361602613301592,
-                    "angularVelocity": -9.2506762233922e-9,
-                    "velocityX": 3.7776907418875507,
-                    "velocityY": 0.19919747693799758,
-                    "timestamp": 1.0436295177935053
-                },
-                {
-                    "x": 3.6761126565475006,
-                    "y": 6.3076498312062705,
-                    "heading": -0.03361602632044191,
-                    "angularVelocity": -8.574588182780684e-9,
-                    "velocityX": 3.763712253532677,
-                    "velocityY": 0.3809168113107395,
-                    "timestamp": 1.0654878267283452
-                },
-                {
-                    "x": 3.7578845383740482,
-                    "y": 6.31992876945557,
-                    "heading": -0.033616026496316675,
-                    "angularVelocity": -8.046128410971048e-9,
-                    "velocityX": 3.7409976256768736,
-                    "velocityY": 0.5617515191089957,
-                    "timestamp": 1.087346135663185
-                },
-                {
-                    "x": 3.8389701148011586,
-                    "y": 6.336131931270735,
-                    "heading": -0.03361602666323835,
-                    "angularVelocity": -7.63653221094489e-9,
-                    "velocityX": 3.709599707316274,
-                    "velocityY": 0.741281581455633,
-                    "timestamp": 1.109204444598025
-                },
-                {
-                    "x": 3.919181182263655,
-                    "y": 6.356221678659879,
-                    "heading": -0.03361602682336018,
-                    "angularVelocity": -7.3254443467155425e-9,
-                    "velocityX": 3.6695916276783906,
-                    "velocityY": 0.9190897360373237,
-                    "timestamp": 1.1310627535328648
-                },
-                {
-                    "x": 3.998331580016101,
-                    "y": 6.3801513240916305,
-                    "heading": -0.03361602697852209,
-                    "angularVelocity": -7.098532288064829e-9,
-                    "velocityX": 3.621066844118403,
-                    "velocityY": 1.0947619737229577,
-                    "timestamp": 1.1529210624677046
-                },
-                {
-                    "x": 4.076237649565588,
-                    "y": 6.407865179672378,
-                    "heading": -0.03361602713034875,
-                    "angularVelocity": -6.945947165054901e-9,
-                    "velocityX": 3.5641398326708003,
-                    "velocityY": 1.2678865352009827,
-                    "timestamp": 1.1747793714025445
-                },
-                {
-                    "x": 4.152718837418924,
-                    "y": 6.439298305619894,
-                    "heading": -0.033616027280326394,
-                    "angularVelocity": -6.861356032634536e-9,
-                    "velocityX": 3.4989526445677317,
-                    "velocityY": 1.4380401540310954,
-                    "timestamp": 1.1966376803373844
-                },
-                {
-                    "x": 4.227916828679672,
-                    "y": 6.473688356846056,
-                    "heading": -0.03361602742978782,
-                    "angularVelocity": -6.8377396934186395e-9,
-                    "velocityX": 3.4402474356508805,
-                    "velocityY": 1.5733171000866744,
-                    "timestamp": 1.2184959892722242
-                },
-                {
-                    "x": 4.303113417879144,
-                    "y": 6.50808147368302,
-                    "heading": -0.033616027579243535,
-                    "angularVelocity": -6.837478280080917e-9,
-                    "velocityX": 3.4401832924786557,
-                    "velocityY": 1.5734573493077775,
-                    "timestamp": 1.240354298207064
-                },
-                {
-                    "x": 4.378310012144908,
-                    "y": 6.542474579443129,
-                    "heading": -0.03361602772869459,
-                    "angularVelocity": -6.837264944013175e-9,
-                    "velocityX": 3.4401835242573666,
-                    "velocityY": 1.573456842550575,
-                    "timestamp": 1.262212607141904
-                },
-                {
-                    "x": 4.453506612352025,
-                    "y": 6.576867672213151,
-                    "heading": -0.03361602787814215,
-                    "angularVelocity": -6.837105393969464e-9,
-                    "velocityX": 3.4401837960695145,
-                    "velocityY": 1.5734562482645857,
-                    "timestamp": 1.2840709160767438
-                },
-                {
-                    "x": 4.528703218974218,
-                    "y": 6.6112607509573404,
-                    "heading": -0.03361602802758787,
-                    "angularVelocity": -6.837020955916861e-9,
-                    "velocityX": 3.440184089554037,
-                    "velocityY": 1.5734556065940832,
-                    "timestamp": 1.3059292250115837
-                },
-                {
-                    "x": 4.603899833102385,
-                    "y": 6.645653813290562,
-                    "heading": -0.03361602817703386,
-                    "angularVelocity": -6.837033689050042e-9,
-                    "velocityX": 3.440184432946286,
-                    "velocityY": 1.5734548558055208,
-                    "timestamp": 1.3277875339464236
-                },
-                {
-                    "x": 4.679098505825837,
-                    "y": 6.680042374380203,
-                    "heading": -0.033616028326483124,
-                    "angularVelocity": -6.83718315967473e-9,
-                    "velocityX": 3.4402786120196116,
-                    "velocityY": 1.573248927543637,
-                    "timestamp": 1.3496458428812634
-                },
-                {
-                    "x": 4.755659310004632,
-                    "y": 6.7112810805754854,
-                    "heading": -0.033616028476497646,
-                    "angularVelocity": -6.8630434261390676e-9,
-                    "velocityX": 3.502595027228562,
-                    "velocityY": 1.4291456072107467,
-                    "timestamp": 1.3715041518161033
-                },
-                {
-                    "x": 4.833635561443148,
-                    "y": 6.738796851922075,
-                    "heading": -0.0336160286284395,
-                    "angularVelocity": -6.951217281059994e-9,
-                    "velocityX": 3.5673505974759867,
-                    "velocityY": 1.258824341288896,
-                    "timestamp": 1.3933624607509432
-                },
-                {
-                    "x": 4.9128465230031875,
-                    "y": 6.762525251770201,
-                    "heading": -0.033616028783803605,
-                    "angularVelocity": -7.107782622056134e-9,
-                    "velocityX": 3.623837589457144,
-                    "velocityY": 1.0855551506230685,
-                    "timestamp": 1.415220769685783
-                },
-                {
-                    "x": 4.993108392358412,
-                    "y": 6.78241106619245,
-                    "heading": -0.033616028944227425,
-                    "angularVelocity": -7.33926038728002e-9,
-                    "velocityX": 3.6719157732872607,
-                    "velocityY": 0.9097599673208229,
-                    "timestamp": 1.437079078620623
-                },
-                {
-                    "x": 5.0742348898890315,
-                    "y": 6.798408083033217,
-                    "heading": -0.03361602911156847,
-                    "angularVelocity": -7.655717557583249e-9,
-                    "velocityX": 3.711471814789518,
-                    "velocityY": 0.7318506151803719,
-                    "timestamp": 1.4589373875554628
-                },
-                {
-                    "x": 5.156037716665883,
-                    "y": 6.810479143599293,
-                    "heading": -0.033616029288003786,
-                    "angularVelocity": -8.071773672975751e-9,
-                    "velocityX": 3.742413332189024,
-                    "velocityY": 0.5522412828028046,
-                    "timestamp": 1.4807956964903026
-                },
-                {
-                    "x": 5.238326998632702,
-                    "y": 6.818596213450421,
-                    "heading": -0.033616029476163836,
-                    "angularVelocity": -8.608170503938931e-9,
-                    "velocityX": 3.764668264691683,
-                    "velocityY": 0.37134939739963974,
-                    "timestamp": 1.5026540054251425
-                },
-                {
-                    "x": 5.3209117299494695,
-                    "y": 6.822740441774992,
-                    "heading": -0.033616029679319044,
-                    "angularVelocity": -9.29418657606336e-9,
-                    "velocityX": 3.778184834103796,
-                    "velocityY": 0.18959510257284434,
-                    "timestamp": 1.5245123143599824
-                },
-                {
-                    "x": 5.403600215911865,
-                    "y": 6.822902202606201,
-                    "heading": -0.03361602991707219,
-                    "angularVelocity": -1.087701477803747e-8,
-                    "velocityX": 3.7829315254392575,
-                    "velocityY": 0.00740042753033698,
-                    "timestamp": 1.5463706232948222
-                },
-                {
-                    "x": 5.530709925548806,
-                    "y": 6.813694137770519,
-                    "heading": -0.03361603012559057,
-                    "angularVelocity": -6.189541308885362e-9,
-                    "velocityX": 3.773052419307907,
-                    "velocityY": -0.2733269661668366,
-                    "timestamp": 1.5800594501413512
-                },
-                {
-                    "x": 5.656785942150605,
-                    "y": 6.795079480224979,
-                    "heading": -0.03361603030947626,
-                    "angularVelocity": -5.458358559418031e-9,
-                    "velocityX": 3.7423688624167126,
-                    "velocityY": -0.5525469209818521,
-                    "timestamp": 1.6137482769878801
-                },
-                {
-                    "x": 5.781133118086899,
-                    "y": 6.767160888262499,
-                    "heading": -0.033616030476517275,
-                    "angularVelocity": -4.958350514129881e-9,
-                    "velocityX": 3.6910509381274874,
-                    "velocityY": -0.8287196253423904,
-                    "timestamp": 1.6474371038344091
-                },
-                {
-                    "x": 5.903065850616414,
-                    "y": 6.730092343263882,
-                    "heading": -0.03361603063230265,
-                    "angularVelocity": -4.624244623037642e-9,
-                    "velocityX": 3.619381971506033,
-                    "velocityY": -1.100321633860491,
-                    "timestamp": 1.681125930680938
-                },
-                {
-                    "x": 6.0219118917079,
-                    "y": 6.684078355941691,
-                    "heading": -0.033616030781137125,
-                    "angularVelocity": -4.4179180116936e-9,
-                    "velocityX": 3.5277583761789866,
-                    "velocityY": -1.3658530625542256,
-                    "timestamp": 1.714814757527467
-                },
-                {
-                    "x": 6.137016291187876,
-                    "y": 6.629373275360583,
-                    "heading": -0.033616030926600995,
-                    "angularVelocity": -4.3178668766413575e-9,
-                    "velocityX": 3.4166936119307345,
-                    "velocityY": -1.623834538089393,
-                    "timestamp": 1.748503584373996
-                },
-                {
-                    "x": 6.249923393155274,
-                    "y": 6.570266127170688,
-                    "heading": -0.0336160310713635,
-                    "angularVelocity": -4.297048049789566e-9,
-                    "velocityX": 3.3514702806883885,
-                    "velocityY": -1.7545030125020786,
-                    "timestamp": 1.782192411220525
-                },
-                {
-                    "x": 6.362830075113665,
-                    "y": 6.51115817668204,
-                    "heading": -0.03361603121596069,
-                    "angularVelocity": -4.292140831453005e-9,
-                    "velocityX": 3.351457813379591,
-                    "velocityY": -1.7545268274825585,
-                    "timestamp": 1.815881238067054
-                },
-                {
-                    "x": 6.474865356079977,
-                    "y": 6.45041474520773,
-                    "heading": -0.033616029845538055,
-                    "angularVelocity": 4.06788471811577e-8,
-                    "velocityX": 3.32559164130869,
-                    "velocityY": -1.8030735160660076,
-                    "timestamp": 1.849570064913583
-                },
-                {
-                    "x": 6.586900214909621,
-                    "y": 6.389670535150393,
-                    "heading": -0.03361602847479329,
-                    "angularVelocity": 4.068840886286375e-8,
-                    "velocityX": 3.3255791108435817,
-                    "velocityY": -1.8030966270823277,
-                    "timestamp": 1.8832588917601119
-                },
-                {
-                    "x": 6.6989350261872,
-                    "y": 6.328926237389509,
-                    "heading": -0.03361602710404103,
-                    "angularVelocity": 4.068863135772345e-8,
-                    "velocityX": 3.325577699335118,
-                    "velocityY": -1.8030992304246154,
-                    "timestamp": 1.9169477186066408
-                },
-                {
-                    "x": 6.810969814733213,
-                    "y": 6.268181897703298,
-                    "heading": -0.03361602573329801,
-                    "angularVelocity": 4.068835717596455e-8,
-                    "velocityX": 3.3255770245842737,
-                    "velocityY": -1.8031004749122261,
-                    "timestamp": 1.9506365454531698
-                },
-                {
-                    "x": 6.92300458832202,
-                    "y": 6.207437530430852,
-                    "heading": -0.03361602436246198,
-                    "angularVelocity": 4.069111827209e-8,
-                    "velocityX": 3.3255765806030024,
-                    "velocityY": -1.803101293766319,
-                    "timestamp": 1.9843253722996987
-                },
-                {
-                    "x": 7.031146753085382,
-                    "y": 6.14880363204561,
-                    "heading": -0.02881387895998304,
-                    "angularVelocity": 0.1425441563861497,
-                    "velocityX": 3.210030591329563,
-                    "velocityY": -1.7404553341186793,
-                    "timestamp": 2.0180141991462275
-                },
-                {
-                    "x": 7.130970312732727,
-                    "y": 6.094680017318762,
-                    "heading": -0.024381039069153343,
-                    "angularVelocity": 0.13158190135330103,
-                    "velocityX": 2.9631058422454424,
-                    "velocityY": -1.6065746359589999,
-                    "timestamp": 2.0517030259927562
-                },
-                {
-                    "x": 7.222475251377904,
-                    "y": 6.04506669568433,
-                    "heading": -0.02031756633641395,
-                    "angularVelocity": 0.12061781644251243,
-                    "velocityX": 2.716180621605835,
-                    "velocityY": -1.4726936577651888,
-                    "timestamp": 2.085391852839285
-                },
-                {
-                    "x": 7.305661563921799,
-                    "y": 5.999963670644433,
-                    "heading": -0.016623481896744244,
-                    "angularVelocity": 0.10965310417300114,
-                    "velocityX": 2.469255249607073,
-                    "velocityY": -1.33881257561641,
-                    "timestamp": 2.1190806796858137
-                },
-                {
-                    "x": 7.380529247941795,
-                    "y": 5.959370944187338,
-                    "heading": -0.013298795761870387,
-                    "angularVelocity": 0.09868809472109723,
-                    "velocityX": 2.222329805696702,
-                    "velocityY": -1.2049314344491047,
-                    "timestamp": 2.1527695065323424
-                },
-                {
-                    "x": 7.447078302075198,
-                    "y": 5.923288517676822,
-                    "heading": -0.010343513390295376,
-                    "angularVelocity": 0.08772292324209098,
-                    "velocityX": 1.9754043213368524,
-                    "velocityY": -1.0710502528001746,
-                    "timestamp": 2.186458333378871
-                },
-                {
-                    "x": 7.505308725482669,
-                    "y": 5.891716392152232,
-                    "heading": -0.007757637975370072,
-                    "angularVelocity": 0.07675765697347174,
-                    "velocityX": 1.7284788120626102,
-                    "velocityY": -0.9371690402998547,
-                    "timestamp": 2.2201471602254
-                },
-                {
-                    "x": 7.555220517619577,
-                    "y": 5.86465456845934,
-                    "heading": -0.005541171447259703,
-                    "angularVelocity": 0.06579233341092419,
-                    "velocityX": 1.481553286621783,
-                    "velocityY": -0.8032878026941531,
-                    "timestamp": 2.2538359870719287
-                },
-                {
-                    "x": 7.596813678122498,
-                    "y": 5.842103047317274,
-                    "heading": -0.0036941149792240496,
-                    "angularVelocity": 0.054826975021999756,
-                    "velocityX": 1.234627750393344,
-                    "velocityY": -0.6694065437422472,
-                    "timestamp": 2.2875248139184574
-                },
-                {
-                    "x": 7.630088206746783,
-                    "y": 5.824061829356817,
-                    "heading": -0.0022164692716530373,
-                    "angularVelocity": 0.04386159584309864,
-                    "velocityX": 0.9877022069028755,
-                    "velocityY": -0.535525266066523,
-                    "timestamp": 2.321213640764986
-                },
-                {
-                    "x": 7.655044103329761,
-                    "y": 5.81053091514454,
-                    "heading": -0.0011082347348293014,
-                    "angularVelocity": 0.0328962044856008,
-                    "velocityX": 0.7407766585837434,
-                    "velocityY": -0.4016439715731056,
-                    "timestamp": 2.354902467611515
-                },
-                {
-                    "x": 7.671681367767583,
-                    "y": 5.8015103051986525,
-                    "heading": -0.0003694116016791507,
-                    "angularVelocity": 0.021930806214050736,
-                    "velocityX": 0.49385110718203246,
-                    "velocityY": -0.2677626616973202,
-                    "timestamp": 2.3885912944580436
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": -1.3509198173227418e-31,
-                    "angularVelocity": 0.010965404149034802,
-                    "velocityX": 0.24692555399191934,
-                    "velocityY": -0.13388133754848214,
-                    "timestamp": 2.4222801213045724
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 2.046162663204931e-31,
-                    "angularVelocity": 9.569528449856341e-30,
-                    "velocityX": -2.6100111398714645e-32,
-                    "velocityY": -7.871960721903e-33,
-                    "timestamp": 2.455968948151101
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "W2 To C3": {
-            "waypoints": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 7
-                },
-                {
-                    "x": 2.892774820327759,
-                    "y": 6.261821746826172,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 25
-                },
-                {
-                    "x": 4.148105144500732,
-                    "y": 5.054959774017334,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 9
-                },
-                {
-                    "x": 5.480467319488525,
-                    "y": 4.235044002532959,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 27
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 3.8917516758910124e-38,
-                    "velocityY": -7.197289285396863e-34,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.2621897956017167,
-                    "y": 5.610801651791703,
-                    "heading": 3.480716271732917e-18,
-                    "angularVelocity": 4.397302355093269e-17,
-                    "velocityX": 0.15399765670735568,
-                    "velocityY": 0.6417938075748972,
-                    "timestamp": 0.07915572134244127
-                },
-                {
-                    "x": 2.2905203388747193,
-                    "y": 5.711291077569501,
-                    "heading": 3.7329161294500195e-18,
-                    "angularVelocity": 3.1861229325774303e-18,
-                    "velocityX": 0.35790897729855475,
-                    "velocityY": 1.2695156341645175,
-                    "timestamp": 0.15831144268488254
-                },
-                {
-                    "x": 2.3436883815778624,
-                    "y": 5.857742406339618,
-                    "heading": 7.49671396174733e-18,
-                    "angularVelocity": 4.754928420848352e-17,
-                    "velocityX": 0.6716891944314332,
-                    "velocityY": 1.8501673193848232,
-                    "timestamp": 0.23746716402732382
-                },
-                {
-                    "x": 2.444399562681219,
-                    "y": 6.0258512044397365,
-                    "heading": 1.0345374349306828e-17,
-                    "angularVelocity": 3.5988057740869555e-17,
-                    "velocityX": 1.2723171413935193,
-                    "velocityY": 2.1237731808778637,
-                    "timestamp": 0.3166228853697651
-                },
-                {
-                    "x": 2.5795093648860483,
-                    "y": 6.154639121411333,
-                    "heading": 6.772254039775596e-18,
-                    "angularVelocity": -4.5140391310627574e-17,
-                    "velocityX": 1.706886121602264,
-                    "velocityY": 1.6270196871106504,
-                    "timestamp": 0.3957786067122063
-                },
-                {
-                    "x": 2.7311723872223266,
-                    "y": 6.233875155156547,
-                    "heading": -7.527039882559706e-19,
-                    "angularVelocity": -9.506524797608463e-17,
-                    "velocityX": 1.916008341079448,
-                    "velocityY": 1.00101461273312,
-                    "timestamp": 0.4749343280546476
-                },
-                {
-                    "x": 2.892774820327759,
-                    "y": 6.261821746826172,
-                    "heading": -9.856118161245683e-18,
-                    "angularVelocity": -1.1500639891239944e-16,
-                    "velocityX": 2.041576153495111,
-                    "velocityY": 0.3530583916824199,
-                    "timestamp": 0.5540900493970888
-                },
-                {
-                    "x": 2.9421812890727397,
-                    "y": 6.2655363212140305,
-                    "heading": -1.2690994415516207e-17,
-                    "angularVelocity": -1.1860957835731596e-16,
-                    "velocityX": 2.067137973857932,
-                    "velocityY": 0.15541563622967255,
-                    "timestamp": 0.5779909545953872
-                },
-                {
-                    "x": 2.992183666499164,
-                    "y": 6.264525133875901,
-                    "heading": -1.5700573063161643e-17,
-                    "angularVelocity": -1.2591902515397632e-16,
-                    "velocityX": 2.092070447188938,
-                    "velocityY": -0.042307491274448855,
-                    "timestamp": 0.6018918597936855
-                },
-                {
-                    "x": 3.0427650175812166,
-                    "y": 6.258786080293358,
-                    "heading": -1.8888665404790212e-17,
-                    "angularVelocity": -1.333879346368914e-16,
-                    "velocityX": 2.1162943688699283,
-                    "velocityY": -0.24011867060800707,
-                    "timestamp": 0.6257927649919839
-                },
-                {
-                    "x": 3.093906133667358,
-                    "y": 6.248316847924188,
-                    "heading": -2.226054198779156e-17,
-                    "angularVelocity": -1.4107736063473173e-16,
-                    "velocityX": 2.139714611720349,
-                    "velocityY": -0.4380266053653904,
-                    "timestamp": 0.6496936701902822
-                },
-                {
-                    "x": 3.1455850428469736,
-                    "y": 6.233114888531311,
-                    "heading": -2.51599280773308e-17,
-                    "angularVelocity": -1.2130863242066928e-16,
-                    "velocityX": 2.162215562584425,
-                    "velocityY": -0.6360411568829696,
-                    "timestamp": 0.6735945753885806
-                },
-                {
-                    "x": 3.197776369275469,
-                    "y": 6.213177387662576,
-                    "heading": -2.8136873311901383e-17,
-                    "angularVelocity": -1.2455366258796381e-16,
-                    "velocityX": 2.1836548028403113,
-                    "velocityY": -0.8341734634450271,
-                    "timestamp": 0.6974954805868789
-                },
-                {
-                    "x": 3.2504504785577715,
-                    "y": 6.188501232669928,
-                    "heading": -3.110871206110287e-17,
-                    "angularVelocity": -1.2434001017092495e-16,
-                    "velocityX": 2.203854157207935,
-                    "velocityY": -1.0324360013948046,
-                    "timestamp": 0.7213963857851773
-                },
-                {
-                    "x": 3.3035723121371006,
-                    "y": 6.159082982949556,
-                    "heading": -3.205106262321403e-17,
-                    "angularVelocity": -3.942740156809933e-17,
-                    "velocityX": 2.222586681909921,
-                    "velocityY": -1.2308424922109886,
-                    "timestamp": 0.7452972909834756
-                },
-                {
-                    "x": 3.3570997558109252,
-                    "y": 6.124918851159325,
-                    "heading": -3.3149188675890806e-17,
-                    "angularVelocity": -4.594495761931231e-17,
-                    "velocityX": 2.239557172823534,
-                    "velocityY": -1.429407443223672,
-                    "timestamp": 0.769198196181774
-                },
-                {
-                    "x": 3.4109812853154073,
-                    "y": 6.086004715644374,
-                    "heading": -3.4399834546372206e-17,
-                    "angularVelocity": -5.232629825878709e-17,
-                    "velocityX": 2.2543719184459183,
-                    "velocityY": -1.6281448418833084,
-                    "timestamp": 0.7930991013800723
-                },
-                {
-                    "x": 3.4651524420580837,
-                    "y": 6.042336211273467,
-                    "heading": -3.565040387084894e-17,
-                    "angularVelocity": -5.2323095531564346e-17,
-                    "velocityX": 2.2664897539752324,
-                    "velocityY": -1.827064875100061,
-                    "timestamp": 0.8170000065783707
-                },
-                {
-                    "x": 3.5195303175635244,
-                    "y": 5.993909012954667,
-                    "heading": -3.501524266469143e-17,
-                    "angularVelocity": 2.6574776159275344e-17,
-                    "velocityX": 2.2751387470175053,
-                    "velocityY": -2.0261658676529293,
-                    "timestamp": 0.840900911776669
-                },
-                {
-                    "x": 3.5740044311548194,
-                    "y": 5.940719606067823,
-                    "heading": -3.37448298204232e-17,
-                    "angularVelocity": 5.315333634409861e-17,
-                    "velocityX": 2.2791652926673844,
-                    "velocityY": -2.225413909872768,
-                    "timestamp": 0.8648018169749674
-                },
-                {
-                    "x": 3.628420546352513,
-                    "y": 5.8827673726499405,
-                    "heading": -3.046153632575651e-17,
-                    "angularVelocity": 1.3737109446355186e-16,
-                    "velocityX": 2.2767386735448056,
-                    "velocityY": -2.4246878073056584,
-                    "timestamp": 0.8887027221732657
-                },
-                {
-                    "x": 3.6825492209054214,
-                    "y": 5.820060642331504,
-                    "heading": -2.5159851350702206e-17,
-                    "angularVelocity": 2.2181942251709706e-16,
-                    "velocityX": 2.264712323814494,
-                    "velocityY": -2.6236131978340897,
-                    "timestamp": 0.9126036273715641
-                },
-                {
-                    "x": 3.736016878561411,
-                    "y": 5.75263682695494,
-                    "heading": -1.874886441281884e-17,
-                    "angularVelocity": 2.6823197281004355e-16,
-                    "velocityX": 2.237055760540655,
-                    "velocityY": -2.8209732985913787,
-                    "timestamp": 0.9365045325698624
-                },
-                {
-                    "x": 3.788130152189531,
-                    "y": 5.680646456241098,
-                    "heading": -1.0550078419468136e-17,
-                    "angularVelocity": 3.4303244721005534e-16,
-                    "velocityX": 2.1803891189790523,
-                    "velocityY": -3.012035323204781,
-                    "timestamp": 0.9604054377681608
-                },
-                {
-                    "x": 3.837400250784585,
-                    "y": 5.604834562813646,
-                    "heading": 5.433473713273816e-19,
-                    "angularVelocity": 4.641424958072882e-16,
-                    "velocityX": 2.061432325942265,
-                    "velocityY": -3.171925615304708,
-                    "timestamp": 0.9843063429664591
-                },
-                {
-                    "x": 3.882609579100313,
-                    "y": 5.526533156121729,
-                    "heading": 5.968487826268029e-18,
-                    "angularVelocity": 2.269847275628469e-16,
-                    "velocityX": 1.8915320545661611,
-                    "velocityY": -3.2760854052293937,
-                    "timestamp": 1.0082072481647575
-                },
-                {
-                    "x": 3.923632674014372,
-                    "y": 5.445959589727867,
-                    "heading": 1.2150851940113108e-17,
-                    "angularVelocity": 2.586665259140546e-16,
-                    "velocityX": 1.716382478977385,
-                    "velocityY": -3.3711512482631147,
-                    "timestamp": 1.0321081533630558
-                },
-                {
-                    "x": 3.960355768910532,
-                    "y": 5.363337507789521,
-                    "heading": 1.8690802571261126e-17,
-                    "angularVelocity": 2.736277382336804e-16,
-                    "velocityX": 1.5364729742024472,
-                    "velocityY": -3.456859949564964,
-                    "timestamp": 1.0560090585613542
-                },
-                {
-                    "x": 4.001130960542707,
-                    "y": 5.282638203170642,
-                    "heading": 2.7305426078750436e-17,
-                    "angularVelocity": 3.6043084711714437e-16,
-                    "velocityX": 1.7060103495610794,
-                    "velocityY": -3.376412062611935,
-                    "timestamp": 1.0799099637596525
-                },
-                {
-                    "x": 4.046099298269293,
-                    "y": 5.204198147271007,
-                    "heading": 3.276849439101543e-17,
-                    "angularVelocity": 2.285716070976872e-16,
-                    "velocityX": 1.8814491482016318,
-                    "velocityY": -3.281886407600119,
-                    "timestamp": 1.1038108689579509
-                },
-                {
-                    "x": 4.095136049561521,
-                    "y": 5.128235061004533,
-                    "heading": 2.8631224607755774e-17,
-                    "angularVelocity": -1.7310096621588863e-16,
-                    "velocityX": 2.051669210240635,
-                    "velocityY": -3.178251435928111,
-                    "timestamp": 1.1277117741562492
-                },
-                {
-                    "x": 4.148105144500732,
-                    "y": 5.054959774017334,
-                    "heading": 1.6930966409187752e-17,
-                    "angularVelocity": -4.895320115071494e-16,
-                    "velocityX": 2.2161961858659356,
-                    "velocityY": -3.065795474240693,
-                    "timestamp": 1.1516126793545476
-                },
-                {
-                    "x": 4.265239674165176,
-                    "y": 4.923669357008322,
-                    "heading": 4.8393962188151695e-17,
-                    "angularVelocity": 6.764650813120357e-16,
-                    "velocityX": 2.5184321191325005,
-                    "velocityY": -2.8227884986351293,
-                    "timestamp": 1.1981235734837035
-                },
-                {
-                    "x": 4.395200374134661,
-                    "y": 4.805061224730511,
-                    "heading": 5.597930320273315e-17,
-                    "angularVelocity": 1.6308740471674414e-16,
-                    "velocityX": 2.7941991312528214,
-                    "velocityY": -2.550115075157559,
-                    "timestamp": 1.2446344676128593
-                },
-                {
-                    "x": 4.536621337779305,
-                    "y": 4.700381789438767,
-                    "heading": 7.583650477943765e-17,
-                    "angularVelocity": 4.269365693457232e-16,
-                    "velocityX": 3.040598687523153,
-                    "velocityY": -2.250643365424577,
-                    "timestamp": 1.2911453617420152
-                },
-                {
-                    "x": 4.6880158468821485,
-                    "y": 4.610730375722861,
-                    "heading": 8.702958111273957e-17,
-                    "angularVelocity": 2.4065493779199536e-16,
-                    "velocityX": 3.2550332978427186,
-                    "velocityY": -1.9275358041271147,
-                    "timestamp": 1.337656255871171
-                },
-                {
-                    "x": 4.842298757317137,
-                    "y": 4.526146352730071,
-                    "heading": 1.0187276641692621e-16,
-                    "angularVelocity": 3.1913351876162675e-16,
-                    "velocityX": 3.317134906212745,
-                    "velocityY": -1.81858518475069,
-                    "timestamp": 1.384167150000327
-                },
-                {
-                    "x": 4.996581769126158,
-                    "y": 4.441562514645757,
-                    "heading": 8.403781588381205e-17,
-                    "angularVelocity": -3.8345748597281753e-16,
-                    "velocityX": 3.3171370857888243,
-                    "velocityY": -1.818581209155721,
-                    "timestamp": 1.4306780441294829
-                },
-                {
-                    "x": 5.15086732735743,
-                    "y": 4.356983321464328,
-                    "heading": 9.522247870814417e-17,
-                    "angularVelocity": 2.4047404444902996e-16,
-                    "velocityX": 3.317191834731107,
-                    "velocityY": -1.818481342167903,
-                    "timestamp": 1.4771889382586387
-                },
-                {
-                    "x": 5.312538078249567,
-                    "y": 4.287555361855474,
-                    "heading": 1.0159270601289723e-16,
-                    "angularVelocity": 1.369620477960438e-16,
-                    "velocityX": 3.4759759819537814,
-                    "velocityY": -1.4927246811478623,
-                    "timestamp": 1.5236998323877946
-                },
-                {
-                    "x": 5.480467319488525,
-                    "y": 4.235044002532959,
-                    "heading": 8.575039074051001e-17,
-                    "angularVelocity": -3.4061515197699265e-16,
-                    "velocityX": 3.6105356472537267,
-                    "velocityY": -1.1290120369797083,
-                    "timestamp": 1.5702107265169505
-                },
-                {
-                    "x": 5.592904232264291,
-                    "y": 4.2080430634395825,
-                    "heading": 8.77612564470466e-17,
-                    "angularVelocity": 6.578528400068062e-17,
-                    "velocityX": 3.6783631124997167,
-                    "velocityY": -0.8833332035895192,
-                    "timestamp": 1.6007778318670682
-                },
-                {
-                    "x": 5.706904061688869,
-                    "y": 4.1886742955111504,
-                    "heading": 8.48190648175679e-17,
-                    "angularVelocity": -9.625352468867059e-17,
-                    "velocityX": 3.7294937848649896,
-                    "velocityY": -0.6336474359145746,
-                    "timestamp": 1.631344937217186
-                },
-                {
-                    "x": 5.821949322476148,
-                    "y": 4.1770253308728975,
-                    "heading": 6.614143850361712e-17,
-                    "angularVelocity": -6.110368020256626e-16,
-                    "velocityX": 3.7636949743701025,
-                    "velocityY": -0.3810947914375838,
-                    "timestamp": 1.6619120425673037
-                },
-                {
-                    "x": 5.937517692705647,
-                    "y": 4.173144318562267,
-                    "heading": 5.289613863539961e-17,
-                    "angularVelocity": -4.3331874941909564e-16,
-                    "velocityX": 3.7808084509727644,
-                    "velocityY": -0.12696695569208769,
-                    "timestamp": 1.6924791479174215
-                },
-                {
-                    "x": 6.05311068319219,
-                    "y": 4.170083108620971,
-                    "heading": 6.789574595295852e-17,
-                    "angularVelocity": 4.90710754117939e-16,
-                    "velocityX": 3.7816139003851448,
-                    "velocityY": -0.1001471976568426,
-                    "timestamp": 1.7230462532675392
-                },
-                {
-                    "x": 6.168703676027133,
-                    "y": 4.167021987357848,
-                    "heading": 5.651709601242885e-17,
-                    "angularVelocity": -3.7225147131887107e-16,
-                    "velocityX": 3.7816139772128325,
-                    "velocityY": -0.1001442965586977,
-                    "timestamp": 1.753613358617657
-                },
-                {
-                    "x": 6.284296668862326,
-                    "y": 4.163960866104177,
-                    "heading": 5.090280597716889e-17,
-                    "angularVelocity": -1.8367097476040197e-16,
-                    "velocityX": 3.7816139772210215,
-                    "velocityY": -0.10014429624946593,
-                    "timestamp": 1.7841804639677747
-                },
-                {
-                    "x": 6.399889661697518,
-                    "y": 4.160899744850507,
-                    "heading": 6.173261084277577e-17,
-                    "angularVelocity": 3.542960558928146e-16,
-                    "velocityX": 3.7816139772210224,
-                    "velocityY": -0.10014429624943298,
-                    "timestamp": 1.8147475693178925
-                },
-                {
-                    "x": 6.515482654532711,
-                    "y": 4.157838623596838,
-                    "heading": 8.001124700396023e-17,
-                    "angularVelocity": 5.979838768447224e-16,
-                    "velocityX": 3.7816139772210224,
-                    "velocityY": -0.10014429624943295,
-                    "timestamp": 1.8453146746680102
-                },
-                {
-                    "x": 6.631075647367904,
-                    "y": 4.154777502343168,
-                    "heading": 7.855690079559015e-17,
-                    "angularVelocity": -4.757880053117145e-17,
-                    "velocityX": 3.7816139772210224,
-                    "velocityY": -0.10014429624943294,
-                    "timestamp": 1.875881780018128
-                },
-                {
-                    "x": 6.746668640203097,
-                    "y": 4.151716381089498,
-                    "heading": 7.690538849750639e-17,
-                    "angularVelocity": -5.4029070766342576e-17,
-                    "velocityX": 3.7816139772210224,
-                    "velocityY": -0.10014429624943297,
-                    "timestamp": 1.9064488853682457
-                },
-                {
-                    "x": 6.862261633037354,
-                    "y": 4.148655259835854,
-                    "heading": 7.874213255028351e-17,
-                    "angularVelocity": 6.00889103413278e-17,
-                    "velocityX": 3.781613977190394,
-                    "velocityY": -0.10014429624862185,
-                    "timestamp": 1.9370159907183635
-                },
-                {
-                    "x": 6.971293364430436,
-                    "y": 4.1457678932048365,
-                    "heading": 7.034301862221845e-17,
-                    "angularVelocity": -2.747762270062427e-16,
-                    "velocityX": 3.56696292122612,
-                    "velocityY": -0.09445992997849091,
-                    "timestamp": 1.9675830960684813
-                },
-                {
-                    "x": 7.072537143209538,
-                    "y": 4.14308676630068,
-                    "heading": 6.196577755471392e-17,
-                    "angularVelocity": -2.740606584102676e-16,
-                    "velocityX": 3.3121807779784156,
-                    "velocityY": -0.08771281655386487,
-                    "timestamp": 1.998150201418599
-                },
-                {
-                    "x": 7.165992951009495,
-                    "y": 4.140611879609727,
-                    "heading": 5.371713190017333e-17,
-                    "angularVelocity": -2.6985367144933565e-16,
-                    "velocityX": 3.057398033916134,
-                    "velocityY": -0.08096568721853159,
-                    "timestamp": 2.0287173067687165
-                },
-                {
-                    "x": 7.251660781708473,
-                    "y": 4.138343233294095,
-                    "heading": 4.5597032779539255e-17,
-                    "angularVelocity": -2.6564828431304464e-16,
-                    "velocityX": 2.802615089578594,
-                    "velocityY": -0.07421855257953036,
-                    "timestamp": 2.059284412118834
-                },
-                {
-                    "x": 7.329540632245536,
-                    "y": 4.136280827434844,
-                    "heading": 3.7578034898451724e-17,
-                    "angularVelocity": -2.623407674507654e-16,
-                    "velocityX": 2.5478320451027607,
-                    "velocityY": -0.06747141528867756,
-                    "timestamp": 2.0898515174689516
-                },
-                {
-                    "x": 7.399632500784112,
-                    "y": 4.134424662080611,
-                    "heading": 2.960093117982924e-17,
-                    "angularVelocity": -2.609702038052913e-16,
-                    "velocityX": 2.293048940543746,
-                    "velocityY": -0.06072427640670841,
-                    "timestamp": 2.120418622819069
-                },
-                {
-                    "x": 7.461936386099821,
-                    "y": 4.132774737263818,
-                    "heading": 2.4038424741612248e-17,
-                    "angularVelocity": -1.8197687815335696e-16,
-                    "velocityX": 2.0382657959291963,
-                    "velocityY": -0.05397713646399285,
-                    "timestamp": 2.1509857281691867
-                },
-                {
-                    "x": 7.516452287318104,
-                    "y": 4.131331053007627,
-                    "heading": 1.8665464618967105e-17,
-                    "angularVelocity": -1.757758880397929e-16,
-                    "velocityX": 1.7834826227035099,
-                    "velocityY": -0.04722999576360025,
-                    "timestamp": 2.181552833519304
-                },
-                {
-                    "x": 7.5631802037830385,
-                    "y": 4.130093609329407,
-                    "heading": 1.3420986699374698e-17,
-                    "angularVelocity": -1.7157260548142223e-16,
-                    "velocityX": 1.5286994280194506,
-                    "velocityY": -0.04048285449494931,
-                    "timestamp": 2.2121199388694217
-                },
-                {
-                    "x": 7.6021201349844665,
-                    "y": 4.129062406242667,
-                    "heading": 8.352476313992174e-18,
-                    "angularVelocity": -1.6581584391601633e-16,
-                    "velocityX": 1.2739162166455336,
-                    "velocityY": -0.033735712784319354,
-                    "timestamp": 2.2426870442195392
-                },
-                {
-                    "x": 7.633272080514257,
-                    "y": 4.128237443758217,
-                    "heading": 5.582680805550875e-18,
-                    "angularVelocity": -9.061360111293989e-17,
-                    "velocityX": 1.019132991919723,
-                    "velocityY": -0.026988570720105975,
-                    "timestamp": 2.2732541495696568
-                },
-                {
-                    "x": 7.656636040038488,
-                    "y": 4.127618721884898,
-                    "heading": 2.786250494304085e-18,
-                    "angularVelocity": -9.148495626615499e-17,
-                    "velocityX": 0.7643497562696309,
-                    "velocityY": -0.020241428366596953,
-                    "timestamp": 2.3038212549197743
-                },
-                {
-                    "x": 7.672212013278888,
-                    "y": 4.1272062406300805,
-                    "heading": 1.8952405540316967e-19,
-                    "angularVelocity": -8.495166341555795e-17,
-                    "velocityX": 0.5095665115159678,
-                    "velocityY": -0.013494285772008131,
-                    "timestamp": 2.334388360269892
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 6.998640646947103e-35,
-                    "angularVelocity": -6.20026191774287e-18,
-                    "velocityX": 0.25478325905928073,
-                    "velocityY": -0.006747142973428655,
-                    "timestamp": 2.3649554656200094
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.616659043590546e-35,
-                    "angularVelocity": -1.1064131915191804e-33,
-                    "velocityX": -5.781348077610764e-36,
-                    "velocityY": -1.0341970128847813e-32,
-                    "timestamp": 2.395522570970127
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "W2 To C4": {
-            "waypoints": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 10
-                },
-                {
-                    "x": 4.032804489135742,
-                    "y": 5.285560607910156,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 11
-                },
-                {
-                    "x": 5.406457424163818,
-                    "y": 4.101301193237305,
-                    "heading": -0.2,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 13
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -8.663522895809945e-37,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.2925575950089905,
-                    "y": 5.560958672147739,
-                    "heading": -0.030620645935394094,
-                    "angularVelocity": -0.4244497974010598,
-                    "velocityX": 0.5899144850684857,
-                    "velocityY": 0.0132886876305762,
-                    "timestamp": 0.07214197326253131
-                },
-                {
-                    "x": 2.3778169001331375,
-                    "y": 5.562706698566846,
-                    "heading": -0.08906407022187784,
-                    "angularVelocity": -0.8101167966920272,
-                    "velocityX": 1.181826629746885,
-                    "velocityY": 0.02423036604148944,
-                    "timestamp": 0.14428394652506263
-                },
-                {
-                    "x": 2.5059697216292194,
-                    "y": 5.56499180242334,
-                    "heading": -0.17113009935557016,
-                    "angularVelocity": -1.1375628558848325,
-                    "velocityX": 1.7763975075885672,
-                    "velocityY": 0.031675094998839956,
-                    "timestamp": 0.21642591978759396
-                },
-                {
-                    "x": 2.6772662600517445,
-                    "y": 5.56737716078426,
-                    "heading": -0.269909808145774,
-                    "angularVelocity": -1.3692404618700307,
-                    "velocityX": 2.3744365544197357,
-                    "velocityY": 0.033064778422963216,
-                    "timestamp": 0.28856789305012526
-                },
-                {
-                    "x": 2.891943869686031,
-                    "y": 5.568884231737924,
-                    "heading": -0.3711645487758272,
-                    "angularVelocity": -1.4035482542399815,
-                    "velocityX": 2.975765700961003,
-                    "velocityY": 0.020890348371527884,
-                    "timestamp": 0.36070986631265656
-                },
-                {
-                    "x": 3.1471065368375797,
-                    "y": 5.565499357743455,
-                    "heading": -0.4176508519135821,
-                    "angularVelocity": -0.644372492676169,
-                    "velocityX": 3.5369515915927057,
-                    "velocityY": -0.04691962031799383,
-                    "timestamp": 0.43285183957518786
-                },
-                {
-                    "x": 3.4167471065440913,
-                    "y": 5.523390468620082,
-                    "heading": -0.41765089729124494,
-                    "angularVelocity": -6.290050140591191e-7,
-                    "velocityX": 3.7376378481534562,
-                    "velocityY": -0.5836947233219467,
-                    "timestamp": 0.5049938128377192
-                },
-                {
-                    "x": 3.6595394652992312,
-                    "y": 5.451715121543425,
-                    "heading": -0.35516305517194063,
-                    "angularVelocity": 0.8661787208384958,
-                    "velocityX": 3.3654798694179893,
-                    "velocityY": -0.9935318350085096,
-                    "timestamp": 0.5771357861002505
-                },
-                {
-                    "x": 3.865437953511339,
-                    "y": 5.371729850961066,
-                    "heading": -0.21394633729907842,
-                    "angularVelocity": 1.957483438371744,
-                    "velocityX": 2.8540734180200986,
-                    "velocityY": -1.1087203047702217,
-                    "timestamp": 0.6492777593627819
-                },
-                {
-                    "x": 4.032804489135742,
-                    "y": 5.285560607910156,
-                    "heading": 0,
-                    "angularVelocity": 2.9656291285588754,
-                    "velocityX": 2.319960600680286,
-                    "velocityY": -1.1944397852458482,
-                    "timestamp": 0.7214197326253132
-                },
-                {
-                    "x": 4.094074407124677,
-                    "y": 5.249842876180867,
-                    "heading": 0.09747269322901686,
-                    "angularVelocity": 3.3393263465697696,
-                    "velocityX": 2.099051997177287,
-                    "velocityY": -1.2236571972325518,
-                    "timestamp": 0.7506090609846522
-                },
-                {
-                    "x": 4.1489483150763835,
-                    "y": 5.213294373235887,
-                    "heading": 0.20635910635937496,
-                    "angularVelocity": 3.7303500714335676,
-                    "velocityX": 1.8799304758291948,
-                    "velocityY": -1.2521186679955063,
-                    "timestamp": 0.7797983893439911
-                },
-                {
-                    "x": 4.197569720233833,
-                    "y": 5.1758877650462445,
-                    "heading": 0.3277826636682362,
-                    "angularVelocity": 4.159861296363549,
-                    "velocityX": 1.6657253828827165,
-                    "velocityY": -1.2815165778788733,
-                    "timestamp": 0.8089877177033301
-                },
-                {
-                    "x": 4.240207786511328,
-                    "y": 5.137521274665071,
-                    "heading": 0.4634475312713068,
-                    "angularVelocity": 4.64775571170911,
-                    "velocityX": 1.460741602293634,
-                    "velocityY": -1.3144012739470166,
-                    "timestamp": 0.8381770460626691
-                },
-                {
-                    "x": 4.277338609770458,
-                    "y": 5.097982888442082,
-                    "heading": 0.6156568342246089,
-                    "angularVelocity": 5.214553109256573,
-                    "velocityX": 1.27206843549211,
-                    "velocityY": -1.3545493659959265,
-                    "timestamp": 0.867366374422008
-                },
-                {
-                    "x": 4.3089076073694805,
-                    "y": 5.060359750747959,
-                    "heading": 0.7826775871522255,
-                    "angularVelocity": 5.721979994588639,
-                    "velocityX": 1.0815253167318162,
-                    "velocityY": -1.2889346829415846,
-                    "timestamp": 0.896555702781347
-                },
-                {
-                    "x": 4.3340758913958135,
-                    "y": 5.025518984665433,
-                    "heading": 0.9544942987723,
-                    "angularVelocity": 5.886285203444992,
-                    "velocityX": 0.8622426565111364,
-                    "velocityY": -1.1936131470246671,
-                    "timestamp": 0.925745031140686
-                },
-                {
-                    "x": 4.353180255759456,
-                    "y": 4.9936006932957335,
-                    "heading": 1.134787723551591,
-                    "angularVelocity": 6.17668973262302,
-                    "velocityX": 0.6544982511572759,
-                    "velocityY": -1.0934918055244334,
-                    "timestamp": 0.9549343595000249
-                },
-                {
-                    "x": 4.3789611483924675,
-                    "y": 4.9633684481765155,
-                    "heading": 1.3088307728290547,
-                    "angularVelocity": 5.962557518791945,
-                    "velocityX": 0.8832300735266126,
-                    "velocityY": -1.0357293853095562,
-                    "timestamp": 0.9841236878593639
-                },
-                {
-                    "x": 4.410526959907193,
-                    "y": 4.931127063214471,
-                    "heading": 1.468917613249843,
-                    "angularVelocity": 5.4844304209407735,
-                    "velocityX": 1.0814161643642388,
-                    "velocityY": -1.104560699894603,
-                    "timestamp": 1.013313016218703
-                },
-                {
-                    "x": 4.4435007518243745,
-                    "y": 4.892801223101424,
-                    "heading": 1.6159090393580717,
-                    "angularVelocity": 5.035793365940867,
-                    "velocityX": 1.1296523000205303,
-                    "velocityY": -1.3130086325122463,
-                    "timestamp": 1.042502344578042
-                },
-                {
-                    "x": 4.478070868346437,
-                    "y": 4.848243847113431,
-                    "heading": 1.7513264495243417,
-                    "angularVelocity": 4.639278043646516,
-                    "velocityX": 1.1843409377729661,
-                    "velocityY": -1.5264954177589778,
-                    "timestamp": 1.0716916729373809
-                },
-                {
-                    "x": 4.514676965101799,
-                    "y": 4.797506620196597,
-                    "heading": 1.8758559761070788,
-                    "angularVelocity": 4.266268995630874,
-                    "velocityX": 1.2540917798696012,
-                    "velocityY": -1.738211523479656,
-                    "timestamp": 1.1008810012967198
-                },
-                {
-                    "x": 4.554041400948399,
-                    "y": 4.740897535447995,
-                    "heading": 1.989313365398794,
-                    "angularVelocity": 3.886947582177413,
-                    "velocityX": 1.3485899833664619,
-                    "velocityY": -1.939376064146053,
-                    "timestamp": 1.1300703296560588
-                },
-                {
-                    "x": 4.597166214346246,
-                    "y": 4.6791223056158096,
-                    "heading": 2.090829457044177,
-                    "angularVelocity": 3.477849520744562,
-                    "velocityX": 1.4774171185768328,
-                    "velocityY": -2.1163635240829413,
-                    "timestamp": 1.1592596580153978
-                },
-                {
-                    "x": 4.645233022395554,
-                    "y": 4.613520579676299,
-                    "heading": 2.179525396238879,
-                    "angularVelocity": 3.0386426882728963,
-                    "velocityX": 1.646725387359914,
-                    "velocityY": -2.2474558212478137,
-                    "timestamp": 1.1884489863747367
-                },
-                {
-                    "x": 4.699569055464054,
-                    "y": 4.547167372776357,
-                    "heading": 2.256949250765549,
-                    "angularVelocity": 2.6524712584521715,
-                    "velocityX": 1.8615033686143418,
-                    "velocityY": -2.2732008795506418,
-                    "timestamp": 1.2176383147340757
-                },
-                {
-                    "x": 4.760368188580363,
-                    "y": 4.482555153301175,
-                    "heading": 2.3258054605639615,
-                    "angularVelocity": 2.358951495928562,
-                    "velocityX": 2.082923333069965,
-                    "velocityY": -2.213556224376411,
-                    "timestamp": 1.2468276430934147
-                },
-                {
-                    "x": 4.827470532546925,
-                    "y": 4.4190609396421525,
-                    "heading": 2.3838171568684117,
-                    "angularVelocity": 1.9874282679714124,
-                    "velocityX": 2.2988656381705637,
-                    "velocityY": -2.175254355885453,
-                    "timestamp": 1.2760169714527536
-                },
-                {
-                    "x": 4.899988963730756,
-                    "y": 4.356794264470432,
-                    "heading": 2.432111827676766,
-                    "angularVelocity": 1.6545317594778726,
-                    "velocityX": 2.484415889639006,
-                    "velocityY": -2.1331999971078073,
-                    "timestamp": 1.3052062998120926
-                },
-                {
-                    "x": 4.972562290971905,
-                    "y": 4.300811163566705,
-                    "heading": 2.4919687455960093,
-                    "angularVelocity": 2.050643892259759,
-                    "velocityX": 2.486296578932036,
-                    "velocityY": -1.9179304235623322,
-                    "timestamp": 1.3343956281714315
-                },
-                {
-                    "x": 5.045616975616562,
-                    "y": 4.25136299402395,
-                    "heading": 2.5615851998228103,
-                    "angularVelocity": 2.384996782720671,
-                    "velocityX": 2.5027874483889563,
-                    "velocityY": -1.6940495832592366,
-                    "timestamp": 1.3635849565307705
-                },
-                {
-                    "x": 5.124307057757338,
-                    "y": 4.205650619988245,
-                    "heading": 2.6233534867290937,
-                    "angularVelocity": 2.116125665718561,
-                    "velocityX": 2.6958510717359463,
-                    "velocityY": -1.5660646066588564,
-                    "timestamp": 1.3927742848901095
-                },
-                {
-                    "x": 5.208588203701103,
-                    "y": 4.163420547805767,
-                    "heading": 2.6756025601758724,
-                    "angularVelocity": 1.7900060187599918,
-                    "velocityX": 2.8873958628376504,
-                    "velocityY": -1.4467640934590689,
-                    "timestamp": 1.4219636132494484
-                },
-                {
-                    "x": 5.297563997608419,
-                    "y": 4.1261005954207315,
-                    "heading": 2.720276777571372,
-                    "angularVelocity": 1.5304982987457612,
-                    "velocityX": 3.048230257714994,
-                    "velocityY": -1.2785478283571272,
-                    "timestamp": 1.4511529416087874
-                },
-                {
-                    "x": 5.390789031982422,
-                    "y": 4.09412145614624,
-                    "heading": 2.757748451080741,
-                    "angularVelocity": 1.2837456569082273,
-                    "velocityX": 3.1938053944353855,
-                    "velocityY": -1.0955763997309995,
-                    "timestamp": 1.4803422699681263
-                },
-                {
-                    "x": 5.532416911390752,
-                    "y": 4.059853570653742,
-                    "heading": 2.796595493071039,
-                    "angularVelocity": 0.9277997104504593,
-                    "velocityX": 3.3825562713263437,
-                    "velocityY": -0.818433852585974,
-                    "timestamp": 1.522212343382395
-                },
-                {
-                    "x": 5.681515537517707,
-                    "y": 4.021534249773671,
-                    "heading": 2.782742222711128,
-                    "angularVelocity": -0.3308632928068908,
-                    "velocityX": 3.5609831550031092,
-                    "velocityY": -0.9151959324488058,
-                    "timestamp": 1.5640824167966638
-                },
-                {
-                    "x": 5.826340094588043,
-                    "y": 3.969241008458542,
-                    "heading": 2.7680381624736805,
-                    "angularVelocity": -0.351183053632681,
-                    "velocityX": 3.4589038246343846,
-                    "velocityY": -1.2489407600921083,
-                    "timestamp": 1.6059524902109326
-                },
-                {
-                    "x": 5.965226819905552,
-                    "y": 3.9129528098786466,
-                    "heading": 2.7396888789742415,
-                    "angularVelocity": -0.6770774729470177,
-                    "velocityX": 3.317088172818383,
-                    "velocityY": -1.3443539499673571,
-                    "timestamp": 1.6478225636252013
-                },
-                {
-                    "x": 6.097371617524639,
-                    "y": 3.854212614126153,
-                    "heading": 2.694967938521201,
-                    "angularVelocity": -1.0680884174853156,
-                    "velocityX": 3.1560679703526437,
-                    "velocityY": -1.402915996141418,
-                    "timestamp": 1.68969263703947
-                },
-                {
-                    "x": 6.217931948857121,
-                    "y": 3.789923024350862,
-                    "heading": 2.6256097109731904,
-                    "angularVelocity": -1.6565107699184056,
-                    "velocityX": 2.8793914483893763,
-                    "velocityY": -1.535454431598441,
-                    "timestamp": 1.7315627104537388
-                },
-                {
-                    "x": 6.32897922975643,
-                    "y": 3.7148340577600942,
-                    "heading": 2.547026684843302,
-                    "angularVelocity": -1.8768303879569581,
-                    "velocityX": 2.652187394098668,
-                    "velocityY": -1.7933803422751633,
-                    "timestamp": 1.7734327838680075
-                },
-                {
-                    "x": 6.440862810984381,
-                    "y": 3.629355422988249,
-                    "heading": 2.48919642870263,
-                    "angularVelocity": -1.381183538144076,
-                    "velocityX": 2.672161095132505,
-                    "velocityY": -2.0415210149288825,
-                    "timestamp": 1.8153028572822762
-                },
-                {
-                    "x": 6.556707402178449,
-                    "y": 3.5350910926988375,
-                    "heading": 2.4587564932761583,
-                    "angularVelocity": -0.7270093635923316,
-                    "velocityX": 2.766763507861174,
-                    "velocityY": -2.2513533558144507,
-                    "timestamp": 1.857172930696545
-                },
-                {
-                    "x": 6.674645090324846,
-                    "y": 3.429370276775565,
-                    "heading": 2.4587373064570563,
-                    "angularVelocity": -0.00045824660759140524,
-                    "velocityX": 2.816753794040547,
-                    "velocityY": -2.5249732637737297,
-                    "timestamp": 1.8990430041108137
-                },
-                {
-                    "x": 6.791280321048844,
-                    "y": 3.3222067883695963,
-                    "heading": 2.45873486083509,
-                    "angularVelocity": -0.00005840978453659185,
-                    "velocityX": 2.785646673460325,
-                    "velocityY": -2.559429197691564,
-                    "timestamp": 1.9409130775250825
-                },
-                {
-                    "x": 6.907916035753451,
-                    "y": 3.2150438274589583,
-                    "heading": 2.4587324135695865,
-                    "angularVelocity": -0.000058449037785704465,
-                    "velocityX": 2.78565823256613,
-                    "velocityY": -2.5594165993059286,
-                    "timestamp": 1.9827831509393512
-                },
-                {
-                    "x": 7.020962902689577,
-                    "y": 3.1172887235192603,
-                    "heading": 2.428615335233058,
-                    "angularVelocity": -0.7192984363448716,
-                    "velocityX": 2.6999443210338705,
-                    "velocityY": -2.334724923276202,
-                    "timestamp": 2.02465322435362
-                },
-                {
-                    "x": 7.121900964071713,
-                    "y": 3.0192154080188542,
-                    "heading": 2.3705886859446603,
-                    "angularVelocity": -1.385874075602246,
-                    "velocityX": 2.4107447909976107,
-                    "velocityY": -2.3423248994587165,
-                    "timestamp": 2.0665232977678887
-                },
-                {
-                    "x": 7.210190441218643,
-                    "y": 2.92089200334631,
-                    "heading": 2.287196223783446,
-                    "angularVelocity": -1.9916961056197946,
-                    "velocityX": 2.10865350708562,
-                    "velocityY": -2.348297880916461,
-                    "timestamp": 2.1083933711821574
-                },
-                {
-                    "x": 7.2890247338542125,
-                    "y": 2.830773686809754,
-                    "heading": 2.1745743754916034,
-                    "angularVelocity": -2.689793427815253,
-                    "velocityX": 1.8828314881507633,
-                    "velocityY": -2.1523324223703058,
-                    "timestamp": 2.150263444596426
-                },
-                {
-                    "x": 7.359953440317656,
-                    "y": 2.7509272688501483,
-                    "heading": 2.035955336818745,
-                    "angularVelocity": -3.3106949037643396,
-                    "velocityX": 1.6940191568728353,
-                    "velocityY": -1.9070044890916185,
-                    "timestamp": 2.192133518010695
-                },
-                {
-                    "x": 7.4240572342258595,
-                    "y": 2.6822786221440054,
-                    "heading": 1.872800890040008,
-                    "angularVelocity": -3.896684038847076,
-                    "velocityX": 1.5310169933057254,
-                    "velocityY": -1.6395635619484732,
-                    "timestamp": 2.2340035914249636
-                },
-                {
-                    "x": 7.482217323317858,
-                    "y": 2.6251237224270416,
-                    "heading": 1.684191854325982,
-                    "angularVelocity": -4.504626343687046,
-                    "velocityX": 1.3890610727273844,
-                    "velocityY": -1.3650537258787387,
-                    "timestamp": 2.2758736648392324
-                },
-                {
-                    "x": 7.535468730024687,
-                    "y": 2.5792763511542147,
-                    "heading": 1.466995470236355,
-                    "angularVelocity": -5.187389616938403,
-                    "velocityX": 1.2718250140130243,
-                    "velocityY": -1.0949914230912903,
-                    "timestamp": 2.317743738253501
-                },
-                {
-                    "x": 7.580274182888533,
-                    "y": 2.5411410245640176,
-                    "heading": 1.2146613001768354,
-                    "angularVelocity": -6.02659965658257,
-                    "velocityX": 1.0701068617801175,
-                    "velocityY": -0.91080152195771,
-                    "timestamp": 2.35961381166777
-                },
-                {
-                    "x": 7.616169603666072,
-                    "y": 2.509680561991982,
-                    "heading": 0.922946754094809,
-                    "angularVelocity": -6.967137200734248,
-                    "velocityX": 0.8573049400316086,
-                    "velocityY": -0.7513830286553573,
-                    "timestamp": 2.4014838850820386
-                },
-                {
-                    "x": 7.640414148022587,
-                    "y": 2.4865073161659272,
-                    "heading": 0.6422191973798425,
-                    "angularVelocity": -6.704730463149791,
-                    "velocityX": 0.5790423178062244,
-                    "velocityY": -0.5534560590991822,
-                    "timestamp": 2.4433539584963073
-                },
-                {
-                    "x": 7.656220323606262,
-                    "y": 2.4718614980447855,
-                    "heading": 0.3932969564478367,
-                    "angularVelocity": -5.9451111649394885,
-                    "velocityX": 0.37750532288984423,
-                    "velocityY": -0.34979203347062976,
-                    "timestamp": 2.485224031910576
-                },
-                {
-                    "x": 7.668143711832241,
-                    "y": 2.4642737767821443,
-                    "heading": 0.1972336516813064,
-                    "angularVelocity": -4.682659684559189,
-                    "velocityX": 0.28477113254632147,
-                    "velocityY": -0.18122063430765137,
-                    "timestamp": 2.527094105324845
-                },
-                {
-                    "x": 7.676158456210791,
-                    "y": 2.459440783193878,
-                    "heading": 0.06602780599823356,
-                    "angularVelocity": -3.1336425992116688,
-                    "velocityX": 0.19141940113768102,
-                    "velocityY": -0.11542835238065828,
-                    "timestamp": 2.5689641787391135
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 1.4385689598890085e-34,
-                    "angularVelocity": -1.576968956919288,
-                    "velocityX": 0.09174915341561693,
-                    "velocityY": -0.05829421815741107,
-                    "timestamp": 2.6108342521533823
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 5.954582727394106e-35,
-                    "angularVelocity": -1.9052580558070114e-33,
-                    "velocityX": -2.275662344047759e-34,
-                    "velocityY": 3.071010312134925e-34,
-                    "timestamp": 2.652704325567651
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "W2 To C5": {
-            "waypoints": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 27
-                },
-                {
-                    "x": 2.1495614051818848,
-                    "y": 3.005171298980713,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 13
-                },
-                {
-                    "x": 4.929586887359619,
-                    "y": 1.5703192949295044,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 28
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 6.892560738534845e-27,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.24821488195175,
-                    "y": 5.550858605907564,
-                    "heading": -1.0860081979744884e-8,
-                    "angularVelocity": -3.249360952601713e-7,
-                    "velocityX": -0.053411135206172675,
-                    "velocityY": -0.27351257600550494,
-                    "timestamp": 0.0334222075852833
-                },
-                {
-                    "x": 2.2446446457975586,
-                    "y": 5.532575817984525,
-                    "heading": -3.253781151640391e-8,
-                    "angularVelocity": -6.486025430910346e-7,
-                    "velocityX": -0.10682227213842765,
-                    "velocityY": -0.5470251441769277,
-                    "timestamp": 0.0668444151705666
-                },
-                {
-                    "x": 2.2392892913368425,
-                    "y": 5.505151636566201,
-                    "heading": -6.492851000422694e-8,
-                    "angularVelocity": -9.691369834959166e-7,
-                    "velocityX": -0.1602334150722015,
-                    "velocityY": -0.8205377023156053,
-                    "timestamp": 0.10026662275584991
-                },
-                {
-                    "x": 2.2321488181807467,
-                    "y": 5.468586062083933,
-                    "heading": -1.0785000725838172e-7,
-                    "angularVelocity": -0.000001284220850944888,
-                    "velocityX": -0.21364456964054193,
-                    "velocityY": -1.0940502475484135,
-                    "timestamp": 0.1336888303411332
-                },
-                {
-                    "x": 2.2232232256851456,
-                    "y": 5.42287909509794,
-                    "heading": -1.6102300741445552e-7,
-                    "angularVelocity": -0.0000015909481231905412,
-                    "velocityX": -0.26705574348127187,
-                    "velocityY": -1.3675627760193463,
-                    "timestamp": 0.16711103792641652
-                },
-                {
-                    "x": 2.212512512848178,
-                    "y": 5.368030736346982,
-                    "heading": -2.2404505707142586e-7,
-                    "angularVelocity": -0.0000018856338989468898,
-                    "velocityX": -0.32046694729784664,
-                    "velocityY": -1.6410752823864576,
-                    "timestamp": 0.20053324551169982
-                },
-                {
-                    "x": 2.200016678146418,
-                    "y": 5.304040986826963,
-                    "heading": -2.963548114247805e-7,
-                    "angularVelocity": -0.000002163524111560841,
-                    "velocityX": -0.3738781966956197,
-                    "velocityY": -1.9145877589558649,
-                    "timestamp": 0.23395545309698312
-                },
-                {
-                    "x": 2.1857357192573095,
-                    "y": 5.230909847922857,
-                    "heading": -3.7718073304542066e-7,
-                    "angularVelocity": -0.0000024183297502514874,
-                    "velocityX": -0.4272895155847271,
-                    "velocityY": -2.1881001940865983,
-                    "timestamp": 0.2673776606822664
-                },
-                {
-                    "x": 2.169669632551276,
-                    "y": 5.148637321647842,
-                    "heading": -4.654625982128869e-7,
-                    "angularVelocity": -0.0000026414132559778984,
-                    "velocityX": -0.48070094307146977,
-                    "velocityY": -2.4616125689825754,
-                    "timestamp": 0.30079986826754973
-                },
-                {
-                    "x": 2.15181841205741,
-                    "y": 5.057223411125392,
-                    "heading": -5.597189955047307e-7,
-                    "angularVelocity": -0.0000028201725975039026,
-                    "velocityX": -0.5341125492089794,
-                    "velocityY": -2.735124850423542,
-                    "timestamp": 0.33422207585283303
-                },
-                {
-                    "x": 2.1321820469941515,
-                    "y": 4.956668121725741,
-                    "heading": -6.57784320236276e-7,
-                    "angularVelocity": -0.0000029341365576412115,
-                    "velocityX": -0.5875244779329194,
-                    "velocityY": -3.008636971185932,
-                    "timestamp": 0.36764428343811634
-                },
-                {
-                    "x": 2.1107605140888954,
-                    "y": 4.846971464532025,
-                    "heading": -7.56114109936986e-7,
-                    "angularVelocity": -0.0000029420493990798148,
-                    "velocityX": -0.6409371029779476,
-                    "velocityY": -3.282148760338516,
-                    "timestamp": 0.40106649102339964
-                },
-                {
-                    "x": 2.087553735835974,
-                    "y": 4.728133474365802,
-                    "heading": -8.465523402036467e-7,
-                    "angularVelocity": -0.0000027059321418483588,
-                    "velocityX": -0.6943520470083785,
-                    "velocityY": -3.5556595076225634,
-                    "timestamp": 0.43448869860868294
-                },
-                {
-                    "x": 2.0633251233427066,
-                    "y": 4.604042454619442,
-                    "heading": -8.465523885072994e-7,
-                    "angularVelocity": -1.4452562228577176e-12,
-                    "velocityX": -0.724925558303899,
-                    "velocityY": -3.712831339152857,
-                    "timestamp": 0.46791090619396625
-                },
-                {
-                    "x": 2.039088654512122,
-                    "y": 4.479952969280304,
-                    "heading": -8.465523654496814e-7,
-                    "angularVelocity": 6.898891300327124e-13,
-                    "velocityX": -0.725160621670614,
-                    "velocityY": -3.7127854293461913,
-                    "timestamp": 0.5013331137792496
-                },
-                {
-                    "x": 2.0148473415026222,
-                    "y": 4.355864430051467,
-                    "heading": -8.465523174606992e-7,
-                    "angularVelocity": 1.4358411666699279e-12,
-                    "velocityX": -0.7253055606758912,
-                    "velocityY": -3.712757121497041,
-                    "timestamp": 0.5347553213645329
-                },
-                {
-                    "x": 1.9907968545860597,
-                    "y": 4.231738764257757,
-                    "heading": -8.465522534990754e-7,
-                    "angularVelocity": 1.9137462023851052e-12,
-                    "velocityX": -0.7195960008785613,
-                    "velocityY": -3.7138679567028667,
-                    "timestamp": 0.5681775289498161
-                },
-                {
-                    "x": 1.9669031646192998,
-                    "y": 4.107582820499775,
-                    "heading": -8.465521766194915e-7,
-                    "angularVelocity": 2.3002544763334585e-12,
-                    "velocityX": -0.7149046006511871,
-                    "velocityY": -3.7147738802470687,
-                    "timestamp": 0.6015997365350993
-                },
-                {
-                    "x": 1.9504985155072192,
-                    "y": 3.982217377752346,
-                    "heading": -8.465520941748615e-7,
-                    "angularVelocity": 2.4667619198655976e-12,
-                    "velocityX": -0.4908308067350749,
-                    "velocityY": -3.7509623631996885,
-                    "timestamp": 0.6350219441203826
-                },
-                {
-                    "x": 1.94330794196602,
-                    "y": 3.855987816020002,
-                    "heading": -8.465520093116816e-7,
-                    "angularVelocity": 2.5391254773053977e-12,
-                    "velocityX": -0.2151435844765948,
-                    "velocityY": -3.7768169984060798,
-                    "timestamp": 0.6684441517056658
-                },
-                {
-                    "x": 1.9454293697222904,
-                    "y": 3.7295714172218672,
-                    "heading": -8.465519250884413e-7,
-                    "angularVelocity": 2.519978339764665e-12,
-                    "velocityX": 0.0634735976376205,
-                    "velocityY": -3.7824072056149025,
-                    "timestamp": 0.7018663592909491
-                },
-                {
-                    "x": 1.9568513812515056,
-                    "y": 3.603654207925799,
-                    "heading": -8.465518451486746e-7,
-                    "angularVelocity": 2.391815832767542e-12,
-                    "velocityX": 0.34174916483985474,
-                    "velocityY": -3.767471342961152,
-                    "timestamp": 0.7352885668762323
-                },
-                {
-                    "x": 1.9775120291721782,
-                    "y": 3.4789195174340257,
-                    "heading": -8.465517739710846e-7,
-                    "angularVelocity": 2.129649524174011e-12,
-                    "velocityX": 0.6181712523959566,
-                    "velocityY": -3.7320901132416737,
-                    "timestamp": 0.7687107744615156
-                },
-                {
-                    "x": 2.0072992100178335,
-                    "y": 3.3560442616931043,
-                    "heading": -8.465517105809759e-7,
-                    "angularVelocity": 1.8966463157464128e-12,
-                    "velocityX": 0.8912391789143821,
-                    "velocityY": -3.6764554055067418,
-                    "timestamp": 0.8021329820467988
-                },
-                {
-                    "x": 2.046051283468891,
-                    "y": 3.235695267642527,
-                    "heading": -8.465516527951994e-7,
-                    "angularVelocity": 1.7289634182989629e-12,
-                    "velocityX": 1.1594707905613406,
-                    "velocityY": -3.600869085126494,
-                    "timestamp": 0.8355551896320821
-                },
-                {
-                    "x": 2.0935579534966013,
-                    "y": 3.1185256537724477,
-                    "heading": -8.46551596321545e-7,
-                    "angularVelocity": 1.6897044173558387e-12,
-                    "velocityX": 1.421410297528723,
-                    "velocityY": -3.5057413120014775,
-                    "timestamp": 0.8689773972173653
-                },
-                {
-                    "x": 2.1495614051818848,
-                    "y": 3.005171298980713,
-                    "heading": -8.465515057383081e-7,
-                    "angularVelocity": 2.7102707524537993e-12,
-                    "velocityX": 1.6756359238847198,
-                    "velocityY": -3.3915878986274275,
-                    "timestamp": 0.9023996048026486
-                },
-                {
-                    "x": 2.288743031901554,
-                    "y": 2.8026748842621148,
-                    "heading": -8.46551464739476e-7,
-                    "angularVelocity": 6.312009526868921e-13,
-                    "velocityX": 2.142782210187666,
-                    "velocityY": -3.117550249366458,
-                    "timestamp": 0.9673533019435205
-                },
-                {
-                    "x": 2.455414708004788,
-                    "y": 2.6221288200825,
-                    "heading": -8.46551435217149e-7,
-                    "angularVelocity": 4.5451345461171864e-13,
-                    "velocityX": 2.566007532127087,
-                    "velocityY": -2.779611817758129,
-                    "timestamp": 1.0323069990843925
-                },
-                {
-                    "x": 2.646160177401748,
-                    "y": 2.4672337011827246,
-                    "heading": -8.465514075797998e-7,
-                    "angularVelocity": 4.254931139136813e-13,
-                    "velocityX": 2.9366376017527633,
-                    "velocityY": -2.384700574689416,
-                    "timestamp": 1.0972606962252645
-                },
-                {
-                    "x": 2.857069726290126,
-                    "y": 2.341164292711998,
-                    "heading": -8.465513787578765e-7,
-                    "angularVelocity": 4.4373032078954734e-13,
-                    "velocityX": 3.2470753501649696,
-                    "velocityY": -1.9409119729950552,
-                    "timestamp": 1.1622143933661364
-                },
-                {
-                    "x": 3.0838173053505575,
-                    "y": 2.2464971410417167,
-                    "heading": -8.465513471236816e-7,
-                    "angularVelocity": 4.870268741320482e-13,
-                    "velocityX": 3.490911049579749,
-                    "velocityY": -1.457455939200772,
-                    "timestamp": 1.2271680905070084
-                },
-                {
-                    "x": 3.3145112489857773,
-                    "y": 2.1619002222864983,
-                    "heading": -8.465513138187862e-7,
-                    "angularVelocity": 5.127482864627404e-13,
-                    "velocityX": 3.5516676307835056,
-                    "velocityY": -1.3024188380298123,
-                    "timestamp": 1.2921217876478803
-                },
-                {
-                    "x": 3.5452013267301767,
-                    "y": 2.0772927620920076,
-                    "heading": -8.465512839726007e-7,
-                    "angularVelocity": 4.594994306997602e-13,
-                    "velocityX": 3.5516081131487938,
-                    "velocityY": -1.302581129610464,
-                    "timestamp": 1.3570754847887523
-                },
-                {
-                    "x": 3.77589133724735,
-                    "y": 1.9926851185956267,
-                    "heading": -8.465512625373994e-7,
-                    "angularVelocity": 3.3000743037279784e-13,
-                    "velocityX": 3.5516070781466733,
-                    "velocityY": -1.302583951649345,
-                    "timestamp": 1.4220291819296242
-                },
-                {
-                    "x": 4.006581361028944,
-                    "y": 1.9080775112659911,
-                    "heading": -8.465512535495126e-7,
-                    "angularVelocity": 1.3837377280671698e-13,
-                    "velocityX": 3.5516072823601217,
-                    "velocityY": -1.302583394841325,
-                    "timestamp": 1.4869828790704962
-                },
-                {
-                    "x": 4.2372714040114055,
-                    "y": 1.823469956289169,
-                    "heading": -8.465512575529088e-7,
-                    "angularVelocity": -6.163459506326445e-14,
-                    "velocityX": 3.551607577968653,
-                    "velocityY": -1.3025825888392473,
-                    "timestamp": 1.5519365762113682
-                },
-                {
-                    "x": 4.467961461705261,
-                    "y": 1.7388624414245573,
-                    "heading": -8.465512722492451e-7,
-                    "angularVelocity": -2.2625863777166434e-13,
-                    "velocityX": 3.5516078044591044,
-                    "velocityY": -1.3025819712885556,
-                    "timestamp": 1.6168902733522401
-                },
-                {
-                    "x": 4.698651553926265,
-                    "y": 1.6542550207002664,
-                    "heading": -8.465512926518018e-7,
-                    "angularVelocity": -3.141092229421582e-13,
-                    "velocityX": 3.551608336024854,
-                    "velocityY": -1.3025805219428788,
-                    "timestamp": 1.681843970493112
-                },
-                {
-                    "x": 4.929586887359619,
-                    "y": 1.5703192949295044,
-                    "heading": -8.465513064459796e-7,
-                    "angularVelocity": -2.1236938120360531e-13,
-                    "velocityX": 3.555383967330626,
-                    "velocityY": -1.2922393869021438,
-                    "timestamp": 1.746797667633984
-                },
-                {
-                    "x": 5.059495672066219,
-                    "y": 1.5327262216225606,
-                    "heading": -8.465512750899748e-7,
-                    "angularVelocity": 8.770995258558909e-13,
-                    "velocityX": 3.633847296905686,
-                    "velocityY": -1.051564666141291,
-                    "timestamp": 1.7825473219513848
-                },
-                {
-                    "x": 5.189548300230332,
-                    "y": 1.4956338313822377,
-                    "heading": -8.465512186056332e-7,
-                    "angularVelocity": 1.579996862146135e-12,
-                    "velocityX": 3.637870929029155,
-                    "velocityY": -1.0375594099736098,
-                    "timestamp": 1.8182969762687855
-                },
-                {
-                    "x": 5.319623170079142,
-                    "y": 1.4586195133360007,
-                    "heading": -8.46551136487956e-7,
-                    "angularVelocity": 2.2970201754243682e-12,
-                    "velocityX": 3.638493080071459,
-                    "velocityY": -1.0353755512600933,
-                    "timestamp": 1.8540466305861862
-                },
-                {
-                    "x": 5.4496989055100356,
-                    "y": 1.42160823723968,
-                    "heading": -8.465510387564672e-7,
-                    "angularVelocity": 2.7337743725450157e-12,
-                    "velocityX": 3.6385172923918394,
-                    "velocityY": -1.0352904609291835,
-                    "timestamp": 1.889796284903587
-                },
-                {
-                    "x": 5.57977446957584,
-                    "y": 1.3845963588884114,
-                    "heading": -8.465509335637302e-7,
-                    "angularVelocity": 2.9424826347181554e-12,
-                    "velocityX": 3.6385124989164392,
-                    "velocityY": -1.03530730738433,
-                    "timestamp": 1.9255459392209877
-                },
-                {
-                    "x": 5.709850057975644,
-                    "y": 1.3475845660573411,
-                    "heading": -8.465508277220688e-7,
-                    "angularVelocity": 2.960634540295608e-12,
-                    "velocityX": 3.6385131795943324,
-                    "velocityY": -1.0353049151878095,
-                    "timestamp": 1.9612955935383884
-                },
-                {
-                    "x": 5.839925662966751,
-                    "y": 1.3105728315354166,
-                    "heading": -8.465507264753305e-7,
-                    "angularVelocity": 2.832103981671134e-12,
-                    "velocityX": 3.6385136436912036,
-                    "velocityY": -1.0353032841469862,
-                    "timestamp": 1.9970452478557892
-                },
-                {
-                    "x": 5.970001275498046,
-                    "y": 1.273561123513075,
-                    "heading": -8.465506337302748e-7,
-                    "angularVelocity": 2.594292378130792e-12,
-                    "velocityX": 3.638513854607602,
-                    "velocityY": -1.0353025428927405,
-                    "timestamp": 2.03279490217319
-                },
-                {
-                    "x": 6.100076894564937,
-                    "y": 1.2365494384597497,
-                    "heading": -8.46550552278632e-7,
-                    "angularVelocity": 2.2783896618382965e-12,
-                    "velocityX": 3.6385140374232323,
-                    "velocityY": -1.0353019003965735,
-                    "timestamp": 2.0685445564905907
-                },
-                {
-                    "x": 6.230152520105601,
-                    "y": 1.1995377761581834,
-                    "heading": -8.465504825990077e-7,
-                    "angularVelocity": 1.9490992420524453e-12,
-                    "velocityX": 3.6385142185095396,
-                    "velocityY": -1.03530126397757,
-                    "timestamp": 2.1042942108079914
-                },
-                {
-                    "x": 6.3602281516940815,
-                    "y": 1.16252613511139,
-                    "heading": -8.465504225140753e-7,
-                    "angularVelocity": 1.680713660601537e-12,
-                    "velocityX": 3.6385143876808734,
-                    "velocityY": -1.0353006694327234,
-                    "timestamp": 2.140043865125392
-                },
-                {
-                    "x": 6.490303788775648,
-                    "y": 1.1255145133698101,
-                    "heading": -8.465503700121188e-7,
-                    "angularVelocity": 1.4686003981542915e-12,
-                    "velocityX": 3.638514541335137,
-                    "velocityY": -1.0353001294215285,
-                    "timestamp": 2.175793519442793
-                },
-                {
-                    "x": 6.62037943067546,
-                    "y": 1.0885029085617548,
-                    "heading": -8.465503232940394e-7,
-                    "angularVelocity": 1.306812060469801e-12,
-                    "velocityX": 3.638514676112538,
-                    "velocityY": -1.0352996557519205,
-                    "timestamp": 2.2115431737601936
-                },
-                {
-                    "x": 6.7504550766965234,
-                    "y": 1.0514913182376744,
-                    "heading": -8.465502808582129e-7,
-                    "angularVelocity": 1.187027603302574e-12,
-                    "velocityX": 3.638514791393393,
-                    "velocityY": -1.0352992506018621,
-                    "timestamp": 2.2472928280775943
-                },
-                {
-                    "x": 6.880530727542945,
-                    "y": 1.0144797448710672,
-                    "heading": -8.465502433332782e-7,
-                    "angularVelocity": 1.0496586699357012e-12,
-                    "velocityX": 3.6385149263697616,
-                    "velocityY": -1.0352987762623522,
-                    "timestamp": 2.283042482394995
-                },
-                {
-                    "x": 7.003525953233288,
-                    "y": 0.9794828656529073,
-                    "heading": -7.145353891597533e-7,
-                    "angularVelocity": 0.000003692758900357643,
-                    "velocityX": 3.440459160761053,
-                    "velocityY": -0.9789431502593731,
-                    "timestamp": 2.318792136712396
-                },
-                {
-                    "x": 7.116271603762692,
-                    "y": 0.9474023877264399,
-                    "heading": -5.944969754078434e-7,
-                    "angularVelocity": 0.0000033577503345576707,
-                    "velocityX": 3.153754985388111,
-                    "velocityY": -0.8973647029323183,
-                    "timestamp": 2.3545417910297965
-                },
-                {
-                    "x": 7.218767661219051,
-                    "y": 0.9182383156679585,
-                    "heading": -4.859087151281335e-7,
-                    "angularVelocity": 0.0000030374632243726353,
-                    "velocityX": 2.867050308972391,
-                    "velocityY": -0.8157861275958178,
-                    "timestamp": 2.3902914453471973
-                },
-                {
-                    "x": 7.311014119553375,
-                    "y": 0.8919906507276494,
-                    "heading": -3.8850367492824467e-7,
-                    "angularVelocity": 0.000002724642854703664,
-                    "velocityX": 2.580345463352478,
-                    "velocityY": -0.7342075172887301,
-                    "timestamp": 2.426041099664598
-                },
-                {
-                    "x": 7.393010975686544,
-                    "y": 0.8686593933386082,
-                    "heading": -3.0210953963079075e-7,
-                    "angularVelocity": 0.0000024166425366030095,
-                    "velocityX": 2.293640531602499,
-                    "velocityY": -0.6526288948669626,
-                    "timestamp": 2.4617907539819988
-                },
-                {
-                    "x": 7.464758227730078,
-                    "y": 0.8482445436165693,
-                    "heading": -2.2661033918382436e-7,
-                    "angularVelocity": 0.000002111886167406066,
-                    "velocityX": 2.006935547027439,
-                    "velocityY": -0.5710502692078399,
-                    "timestamp": 2.4975404082993995
-                },
-                {
-                    "x": 7.526255874392654,
-                    "y": 0.8307461015250325,
-                    "heading": -1.6193015142138781e-7,
-                    "angularVelocity": 0.0000018092535156938384,
-                    "velocityX": 1.7202305263310733,
-                    "velocityY": -0.48947164456971237,
-                    "timestamp": 2.5332900626168002
-                },
-                {
-                    "x": 7.577503914725456,
-                    "y": 0.8161640669449958,
-                    "heading": -1.0802379214331377e-7,
-                    "angularVelocity": 0.0000015078847699136293,
-                    "velocityX": 1.4335254790941394,
-                    "velocityY": -0.4078930232603403,
-                    "timestamp": 2.569039716934201
-                },
-                {
-                    "x": 7.618502347994633,
-                    "y": 0.8044984397090525,
-                    "heading": -6.487055743300418e-8,
-                    "angularVelocity": 0.0000012070951602673883,
-                    "velocityX": 1.1468204113297347,
-                    "velocityY": -0.3263144066337203,
-                    "timestamp": 2.6047893712516017
-                },
-                {
-                    "x": 7.649251173610278,
-                    "y": 0.7957492196197445,
-                    "heading": -3.246962719134405e-8,
-                    "angularVelocity": 9.063284906782187e-7,
-                    "velocityX": 0.8601153270642448,
-                    "velocityY": -0.24473579553046104,
-                    "timestamp": 2.6405390255690024
-                },
-                {
-                    "x": 7.669750391083677,
-                    "y": 0.7899164064601075,
-                    "heading": -1.0836483282929717e-8,
-                    "angularVelocity": 6.051287577519143e-7,
-                    "velocityX": 0.573410229128317,
-                    "velocityY": -0.16315719049619,
-                    "timestamp": 2.676288679886403
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 2.0494644799121527e-30,
-                    "angularVelocity": 3.031213445055282e-7,
-                    "velocityX": 0.28670511958863415,
-                    "velocityY": -0.08157859189950141,
-                    "timestamp": 2.712038334203804
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": -1.7051999701665118e-29,
-                    "angularVelocity": -2.5846596071248955e-28,
-                    "velocityX": 4.524644517512267e-29,
-                    "velocityY": 1.710870028377411e-29,
-                    "timestamp": 2.7477879885212046
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "W3 To W1": {
-            "waypoints": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": 1.4326217036715317e-32,
-                    "velocityX": -1.74824017803789e-33,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.3001377838932537,
-                    "y": 4.403828106793783,
-                    "heading": -0.3629126711329447,
-                    "angularVelocity": 0.37089781855009624,
-                    "velocityX": 0.0018866291944089578,
-                    "velocityY": 0.600123743495633,
-                    "timestamp": 0.07303178264284323
-                },
-                {
-                    "x": 2.300408122825978,
-                    "y": 4.49157282256222,
-                    "heading": -0.3106389398567459,
-                    "angularVelocity": 0.7157668810008344,
-                    "velocityX": 0.0037016614265912673,
-                    "velocityY": 1.2014593180279707,
-                    "timestamp": 0.14606356528568645
-                },
-                {
-                    "x": 2.3008081314200264,
-                    "y": 4.623360732427042,
-                    "heading": -0.2362186265841639,
-                    "angularVelocity": 1.019012689811082,
-                    "velocityX": 0.005477185131915077,
-                    "velocityY": 1.8045281806870437,
-                    "timestamp": 0.2190953479285297
-                },
-                {
-                    "x": 2.301347271135313,
-                    "y": 4.79937800153835,
-                    "heading": -0.14515320669009546,
-                    "angularVelocity": 1.2469286192754925,
-                    "velocityX": 0.007382261472700921,
-                    "velocityY": 2.410146141058999,
-                    "timestamp": 0.2921271305713729
-                },
-                {
-                    "x": 2.302070167703843,
-                    "y": 5.019849104537176,
-                    "heading": -0.04952394454697364,
-                    "angularVelocity": 1.3094197989222025,
-                    "velocityX": 0.009898383174695338,
-                    "velocityY": 3.018837758308937,
-                    "timestamp": 0.3651589132142161
-                },
-                {
-                    "x": 2.3031513688730865,
-                    "y": 5.282690253403799,
-                    "heading": -0.004114907781753178,
-                    "angularVelocity": 0.6217708937393756,
-                    "velocityX": 0.014804529344862738,
-                    "velocityY": 3.598996756686451,
-                    "timestamp": 0.43819069585705933
-                },
-                {
-                    "x": 2.303520931398292,
-                    "y": 5.5589648879428,
-                    "heading": -0.0041148785146688655,
-                    "angularVelocity": 4.00744487575221e-7,
-                    "velocityX": 0.005060297199824665,
-                    "velocityY": 3.7829370246937892,
-                    "timestamp": 0.5112224784999025
-                },
-                {
-                    "x": 2.3038904865521834,
-                    "y": 5.835239522491655,
-                    "heading": -0.004114849247579104,
-                    "angularVelocity": 4.0074456218901027e-7,
-                    "velocityX": 0.005060196266858216,
-                    "velocityY": 3.7829370248287195,
-                    "timestamp": 0.5842542611427458
-                },
-                {
-                    "x": 2.302005656251481,
-                    "y": 6.097682306820458,
-                    "heading": 0.0412177605138735,
-                    "angularVelocity": 0.6207244041015477,
-                    "velocityX": -0.025808356752290516,
-                    "velocityY": 3.5935420830717044,
-                    "timestamp": 0.6572860437855891
-                },
-                {
-                    "x": 2.3011707390613565,
-                    "y": 6.318019040898657,
-                    "heading": 0.13972629361592653,
-                    "angularVelocity": 1.3488447020908976,
-                    "velocityX": -0.01143224442716972,
-                    "velocityY": 3.016997889203128,
-                    "timestamp": 0.7303178264284323
-                },
-                {
-                    "x": 2.3006512841511224,
-                    "y": 6.493901741171369,
-                    "heading": 0.23378956116370217,
-                    "angularVelocity": 1.2879771538343168,
-                    "velocityX": -0.007112723959845375,
-                    "velocityY": 2.408303534542668,
-                    "timestamp": 0.8033496090712756
-                },
-                {
-                    "x": 2.300310658917655,
-                    "y": 6.625570031648912,
-                    "heading": 0.310734904704345,
-                    "angularVelocity": 1.053587092580481,
-                    "velocityX": -0.00466406845267762,
-                    "velocityY": 1.802890272053981,
-                    "timestamp": 0.8763813917141189
-                },
-                {
-                    "x": 2.3001003670918414,
-                    "y": 6.7132235530415985,
-                    "heading": 0.3648714550351714,
-                    "angularVelocity": 0.7412738450542471,
-                    "velocityX": -0.0028794562888088465,
-                    "velocityY": 1.2002106236588805,
-                    "timestamp": 0.9494131743569622
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "angularVelocity": 0.38515484556072405,
-                    "velocityX": -0.0013742933310585549,
-                    "velocityY": 0.5994163824877027,
-                    "timestamp": 1.0224449569998053
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "angularVelocity": 2.8585488767662063e-32,
-                    "velocityX": -1.0732467562704116e-31,
-                    "velocityY": 0,
-                    "timestamp": 1.0954767396426486
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "W3 To W2": {
-            "waypoints": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 10
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": 0,
-                    "velocityX": 2.0150450292109907e-34,
-                    "velocityY": 3.039389955204995e-35,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.2980012520823867,
-                    "y": 4.4080014432651184,
-                    "heading": -0.3744767713085023,
-                    "angularVelocity": 0.2040875990087458,
-                    "velocityX": -0.02627801674742958,
-                    "velocityY": 0.6310864511258738,
-                    "timestamp": 0.07606159691668693
-                },
-                {
-                    "x": 2.294003780157748,
-                    "y": 4.504004078538053,
-                    "heading": -0.34341554427294535,
-                    "angularVelocity": 0.40836937817095076,
-                    "velocityX": -0.05255571913664827,
-                    "velocityY": 1.262169598911913,
-                    "timestamp": 0.15212319383337386
-                },
-                {
-                    "x": 2.2880075055108158,
-                    "y": 4.648007445856594,
-                    "heading": -0.296791131111893,
-                    "angularVelocity": 0.6129823071177671,
-                    "velocityX": -0.07883445641432706,
-                    "velocityY": 1.8932466994648223,
-                    "timestamp": 0.2281847907500608
-                },
-                {
-                    "x": 2.2800121106149036,
-                    "y": 4.840010896492559,
-                    "heading": -0.2345694561462187,
-                    "angularVelocity": 0.8180432371651087,
-                    "velocityX": -0.10511736829125365,
-                    "velocityY": 2.5243152710332057,
-                    "timestamp": 0.3042463876667477
-                },
-                {
-                    "x": 2.270016860554186,
-                    "y": 5.080013636963748,
-                    "heading": -0.15671112019223,
-                    "angularVelocity": 1.023622157700289,
-                    "velocityX": -0.13140994228224737,
-                    "velocityY": 3.1553734105013973,
-                    "timestamp": 0.3803079845834346
-                },
-                {
-                    "x": 2.2620106394348882,
-                    "y": 5.2720083260038075,
-                    "heading": -0.09403333701953483,
-                    "angularVelocity": 0.82403980081234,
-                    "velocityX": -0.10525970323851294,
-                    "velocityY": 2.524200080237073,
-                    "timestamp": 0.4563695815001215
-                },
-                {
-                    "x": 2.256005452365101,
-                    "y": 5.416004188429854,
-                    "heading": -0.0470178538159679,
-                    "angularVelocity": 0.6181237984664553,
-                    "velocityX": -0.07895163016843816,
-                    "velocityY": 1.8931480308488688,
-                    "timestamp": 0.5324311784168084
-                },
-                {
-                    "x": 2.2520018201736485,
-                    "y": 5.5120013950535185,
-                    "heading": -0.01567260522346991,
-                    "angularVelocity": 0.4121034774859067,
-                    "velocityX": -0.05263670963729669,
-                    "velocityY": 1.262098227162051,
-                    "timestamp": 0.6084927753334953
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 1.3656663814986182e-35,
-                    "angularVelocity": 0.20605148798856665,
-                    "velocityX": -0.026318408432062,
-                    "velocityY": 0.6310491350721452,
-                    "timestamp": 0.6845543722501822
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 6.6551444751621536e-34,
-                    "velocityX": 3.9479513180483775e-34,
-                    "velocityY": 0,
-                    "timestamp": 0.7606159691668691
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "W3 To S1": {
-            "waypoints": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 12
-                },
-                {
-                    "x": 2.0410311222076416,
-                    "y": 6.2410478591918945,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 15
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.263,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": -4.289844309028223e-18,
-                    "velocityX": -1.3032340882001426e-17,
-                    "velocityY": -3.394190195762378e-17,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.271980215040446,
-                    "y": 4.410826628196332,
-                    "heading": -0.3872050871501322,
-                    "angularVelocity": 0.033498492691473286,
-                    "velocityX": -0.3358317815939762,
-                    "velocityY": 0.6091837294305799,
-                    "timestamp": 0.0834339883696639
-                },
-                {
-                    "x": 2.216375801038755,
-                    "y": 4.512717243328352,
-                    "heading": -0.38158865604258857,
-                    "angularVelocity": 0.06731586511476574,
-                    "velocityX": -0.6664479918611741,
-                    "velocityY": 1.2212123275291613,
-                    "timestamp": 0.1668679767393278
-                },
-                {
-                    "x": 2.133913144213912,
-                    "y": 4.66605742038127,
-                    "heading": -0.3731070083031397,
-                    "angularVelocity": 0.10165698541488709,
-                    "velocityX": -0.9883580832768961,
-                    "velocityY": 1.837862243543364,
-                    "timestamp": 0.2503019651089917
-                },
-                {
-                    "x": 2.026047479965626,
-                    "y": 4.871580643769176,
-                    "heading": -0.3616752437087536,
-                    "angularVelocity": 0.13701567930528194,
-                    "velocityX": -1.2928264171046289,
-                    "velocityY": 2.4633033539909763,
-                    "timestamp": 0.3337359534786556
-                },
-                {
-                    "x": 1.8971303225339013,
-                    "y": 5.131188087013384,
-                    "heading": -0.34705934044989933,
-                    "angularVelocity": 0.1751792470161148,
-                    "velocityX": -1.5451395762230817,
-                    "velocityY": 3.111531023711595,
-                    "timestamp": 0.41716994184831946
-                },
-                {
-                    "x": 1.812941926504828,
-                    "y": 5.427772716701387,
-                    "heading": -0.3288482106163032,
-                    "angularVelocity": 0.21826991841509946,
-                    "velocityX": -1.0090419704726812,
-                    "velocityY": 3.5547219482539485,
-                    "timestamp": 0.5006039302179833
-                },
-                {
-                    "x": 1.7681664127359014,
-                    "y": 5.682327706651028,
-                    "heading": -0.2856043053433686,
-                    "angularVelocity": 0.5183008282076023,
-                    "velocityX": -0.5366579573130098,
-                    "velocityY": 3.0509747277288763,
-                    "timestamp": 0.5840379185876472
-                },
-                {
-                    "x": 1.7580426073358177,
-                    "y": 5.890413977811611,
-                    "heading": -0.23166998700826372,
-                    "angularVelocity": 0.6464310215597402,
-                    "velocityX": -0.12133910409739124,
-                    "velocityY": 2.4940228224314014,
-                    "timestamp": 0.6674719069573111
-                },
-                {
-                    "x": 1.7809381631803756,
-                    "y": 6.0507944700387695,
-                    "heading": -0.17161076742769205,
-                    "angularVelocity": 0.7198411672948628,
-                    "velocityX": 0.27441521485536685,
-                    "velocityY": 1.9222441041228078,
-                    "timestamp": 0.7509058953269749
-                },
-                {
-                    "x": 1.8360347454575048,
-                    "y": 6.162894796860879,
-                    "heading": -0.10766394629623156,
-                    "angularVelocity": 0.7664361057242964,
-                    "velocityX": 0.6603613629651003,
-                    "velocityY": 1.34358106345549,
-                    "timestamp": 0.8343398836966388
-                },
-                {
-                    "x": 1.9228414263613316,
-                    "y": 6.226384193614056,
-                    "heading": -0.041156065591054634,
-                    "angularVelocity": 0.797131744563275,
-                    "velocityX": 1.0404234844874343,
-                    "velocityY": 0.760953635248516,
-                    "timestamp": 0.9177738720663027
-                },
-                {
-                    "x": 2.041031122207642,
-                    "y": 6.2410478591918945,
-                    "heading": 0.027035456766794728,
-                    "angularVelocity": 0.8173110705883178,
-                    "velocityX": 1.4165653369372246,
-                    "velocityY": 0.17575170340583585,
-                    "timestamp": 1.0012078604359667
-                },
-                {
-                    "x": 2.201330972597037,
-                    "y": 6.260936085330867,
-                    "heading": 0.09277658873792281,
-                    "angularVelocity": 0.8449644804697937,
-                    "velocityX": 2.060318642882427,
-                    "velocityY": 0.2556214680702958,
-                    "timestamp": 1.0790112870770416
-                },
-                {
-                    "x": 2.4117170311040814,
-                    "y": 6.287038452225851,
-                    "heading": 0.16066894258910833,
-                    "angularVelocity": 0.8726139294965615,
-                    "velocityX": 2.7040718846096095,
-                    "velocityY": 0.33549122476729665,
-                    "timestamp": 1.1568147137181166
-                },
-                {
-                    "x": 2.672189282535082,
-                    "y": 6.319354957991783,
-                    "heading": 0.23071153953234388,
-                    "angularVelocity": 0.9002507982373139,
-                    "velocityX": 3.3478249310538675,
-                    "velocityY": 0.4153609572357597,
-                    "timestamp": 1.2346181403591916
-                },
-                {
-                    "x": 2.9642755449736304,
-                    "y": 6.355593779334123,
-                    "heading": 0.23071154098757585,
-                    "angularVelocity": 1.8703321803502023e-8,
-                    "velocityX": 3.7541567903686675,
-                    "velocityY": 0.4657741041344694,
-                    "timestamp": 1.3124215670002666
-                },
-                {
-                    "x": 3.256361807413129,
-                    "y": 6.39183260067658,
-                    "heading": 0.23071154244151956,
-                    "angularVelocity": 1.868721301612308e-8,
-                    "velocityX": 3.7541567903805326,
-                    "velocityY": 0.46577410413594156,
-                    "timestamp": 1.3902249936413416
-                },
-                {
-                    "x": 3.5484480698526375,
-                    "y": 6.4280714220190385,
-                    "heading": 0.23071154389546672,
-                    "angularVelocity": 1.868722261906901e-8,
-                    "velocityX": 3.7541567903806516,
-                    "velocityY": 0.4657741041359563,
-                    "timestamp": 1.4680284202824165
-                },
-                {
-                    "x": 3.8405343322921546,
-                    "y": 6.464310243361498,
-                    "heading": 0.23071154534942268,
-                    "angularVelocity": 1.868730306107698e-8,
-                    "velocityX": 3.754156790380741,
-                    "velocityY": 0.46577410413596737,
-                    "timestamp": 1.5458318469234915
-                },
-                {
-                    "x": 4.132620594731676,
-                    "y": 6.500549064703958,
-                    "heading": 0.23071154680338032,
-                    "angularVelocity": 1.86873022983191e-8,
-                    "velocityX": 3.754156790380778,
-                    "velocityY": 0.465774104135972,
-                    "timestamp": 1.6236352735645665
-                },
-                {
-                    "x": 4.424706857172369,
-                    "y": 6.536787886046564,
-                    "heading": 0.23071154826355325,
-                    "angularVelocity": 1.876697436713731e-8,
-                    "velocityX": 3.7541567903965425,
-                    "velocityY": 0.46577410413792786,
-                    "timestamp": 1.7014387002056415
-                },
-                {
-                    "x": 4.6751378783325555,
-                    "y": 6.567858587416207,
-                    "heading": 0.24147534520321134,
-                    "angularVelocity": 0.13834605240872366,
-                    "velocityX": 3.2187659589270945,
-                    "velocityY": 0.39934875250390567,
-                    "timestamp": 1.7792421268467165
-                },
-                {
-                    "x": 4.875482718759792,
-                    "y": 6.592715151427428,
-                    "heading": 0.2500855408103817,
-                    "angularVelocity": 0.11066602050088868,
-                    "velocityX": 2.575013069173925,
-                    "velocityY": 0.31947903947593354,
-                    "timestamp": 1.8570455534877914
-                },
-                {
-                    "x": 5.025741356581141,
-                    "y": 6.611357575366475,
-                    "heading": 0.25654288837544836,
-                    "angularVelocity": 0.08299567055584532,
-                    "velocityX": 1.931259898289217,
-                    "velocityY": 0.23960929156828065,
-                    "timestamp": 1.9348489801288664
-                },
-                {
-                    "x": 5.125913784825429,
-                    "y": 6.623785858368443,
-                    "heading": 0.2608476597362627,
-                    "angularVelocity": 0.05532881451478021,
-                    "velocityX": 1.2875066378045663,
-                    "velocityY": 0.15973953254406137,
-                    "timestamp": 2.0126524067699414
-                },
-                {
-                    "x": 5.175999999999999,
-                    "y": 6.63,
-                    "heading": 0.26300000000000007,
-                    "angularVelocity": 0.02766382352150404,
-                    "velocityX": 0.6437533324290226,
-                    "velocityY": 0.07986976795027836,
-                    "timestamp": 2.090455833411016
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.263,
-                    "angularVelocity": -5.112167395133813e-18,
-                    "velocityX": -1.2520036782163602e-16,
-                    "velocityY": -1.3106574021367763e-17,
-                    "timestamp": 2.168259260052091
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        1,
-                        2
-                    ],
-                    "type": "StraightLine"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "W3 To S2": {
-            "waypoints": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 12
-                },
-                {
-                    "x": 2.2487735748291016,
-                    "y": 6.386467456817627,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 8
-                },
-                {
-                    "x": 3.536776304244995,
-                    "y": 6.365693092346191,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 8
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": -7.3580792676211765e-19,
-                    "velocityX": -1.2600069549143939e-17,
-                    "velocityY": 7.063684033512061e-18,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.277488982590109,
-                    "y": 4.417838766017227,
-                    "heading": -0.3894665385190853,
-                    "angularVelocity": 0.006183201184696677,
-                    "velocityX": -0.26091883762393897,
-                    "velocityY": 0.6703927825220056,
-                    "timestamp": 0.08627593781609397
-                },
-                {
-                    "x": 2.232568196785243,
-                    "y": 4.5335556038331974,
-                    "heading": -0.3884042841517919,
-                    "angularVelocity": 0.012312290010209009,
-                    "velocityX": -0.520664126544967,
-                    "velocityY": 1.3412411472435377,
-                    "timestamp": 0.17255187563218793
-                },
-                {
-                    "x": 2.165406410226816,
-                    "y": 4.707215578005854,
-                    "heading": -0.38682100523451757,
-                    "angularVelocity": 0.018351338245094856,
-                    "velocityX": -0.7784532774547995,
-                    "velocityY": 2.012843656858665,
-                    "timestamp": 0.2588278134482819
-                },
-                {
-                    "x": 2.0763412317308223,
-                    "y": 4.9389471550535315,
-                    "heading": -0.3847322299034305,
-                    "angularVelocity": 0.024210404244363496,
-                    "velocityX": -1.0323293000400813,
-                    "velocityY": 2.685935185562845,
-                    "timestamp": 0.34510375126437587
-                },
-                {
-                    "x": 1.9663857850872422,
-                    "y": 5.229122444622766,
-                    "heading": -0.3821844673054777,
-                    "angularVelocity": 0.029530395872206988,
-                    "velocityX": -1.2744624912447062,
-                    "velocityY": 3.3633397319630034,
-                    "timestamp": 0.43137968908046986
-                },
-                {
-                    "x": 1.9100206214147308,
-                    "y": 5.550595198428695,
-                    "heading": -0.3821844591902738,
-                    "angularVelocity": 9.406103402067565e-8,
-                    "velocityX": -0.653312674417546,
-                    "velocityY": 3.7260997903167445,
-                    "timestamp": 0.5176556268965639
-                },
-                {
-                    "x": 1.8984249514650446,
-                    "y": 5.829240348205722,
-                    "heading": -0.3690339918971659,
-                    "angularVelocity": 0.15242334799236965,
-                    "velocityX": -0.13440213161644662,
-                    "velocityY": 3.229697141872688,
-                    "timestamp": 0.6039315647126579
-                },
-                {
-                    "x": 1.916554046418155,
-                    "y": 6.053409726810643,
-                    "heading": -0.3524665993885735,
-                    "angularVelocity": 0.1920279620015428,
-                    "velocityX": 0.2101292134517386,
-                    "velocityY": 2.5982838816862235,
-                    "timestamp": 0.6902075025287518
-                },
-                {
-                    "x": 1.9615881089279723,
-                    "y": 6.221650822615583,
-                    "heading": -0.33415152161256073,
-                    "angularVelocity": 0.21228488776393065,
-                    "velocityX": 0.5219770848021326,
-                    "velocityY": 1.9500349699305692,
-                    "timestamp": 0.7764834403448458
-                },
-                {
-                    "x": 2.0323565347582764,
-                    "y": 6.333414099671539,
-                    "heading": -0.3147709502575115,
-                    "angularVelocity": 0.22463472256153053,
-                    "velocityX": 0.8202568134484147,
-                    "velocityY": 1.2954165423758215,
-                    "timestamp": 0.8627593781609398
-                },
-                {
-                    "x": 2.1282194334415006,
-                    "y": 6.388411890330483,
-                    "heading": -0.2946952221482423,
-                    "angularVelocity": 0.232692087938443,
-                    "velocityX": 1.111119752619376,
-                    "velocityY": 0.6374638404531465,
-                    "timestamp": 0.9490353159770338
-                },
-                {
-                    "x": 2.2487735748291016,
-                    "y": 6.386467456817627,
-                    "heading": -0.27415678824354056,
-                    "angularVelocity": 0.23805517997945658,
-                    "velocityX": 1.3973089651552055,
-                    "velocityY": -0.022537379043057543,
-                    "timestamp": 1.0353112537931277
-                },
-                {
-                    "x": 2.4244121861833303,
-                    "y": 6.383634558684574,
-                    "heading": -0.2535069736087744,
-                    "angularVelocity": 0.24641991696382376,
-                    "velocityX": 2.0959438518486566,
-                    "velocityY": -0.03380575250000802,
-                    "timestamp": 1.1191105460305186
-                },
-                {
-                    "x": 2.658595728295013,
-                    "y": 6.37985738170666,
-                    "heading": -0.2319955871365083,
-                    "angularVelocity": 0.2567012906424869,
-                    "velocityX": 2.79457661108014,
-                    "velocityY": -0.04507409164284618,
-                    "timestamp": 1.2029098382679095
-                },
-                {
-                    "x": 2.9513225155845477,
-                    "y": 6.37513595307082,
-                    "heading": -0.20870362422898678,
-                    "angularVelocity": 0.27794939892259674,
-                    "velocityX": 3.493189255826692,
-                    "velocityY": -0.05634210635652628,
-                    "timestamp": 1.2867091305053004
-                },
-                {
-                    "x": 3.1855040701620707,
-                    "y": 6.371358808150105,
-                    "heading": -0.1849223646442006,
-                    "angularVelocity": 0.2837883107343998,
-                    "velocityX": 2.794552893288424,
-                    "velocityY": -0.04507370909548172,
-                    "timestamp": 1.3705084227426914
-                },
-                {
-                    "x": 3.3611402034786066,
-                    "y": 6.368525949985646,
-                    "heading": -0.1612618078271142,
-                    "angularVelocity": 0.28234793141284903,
-                    "velocityX": 2.0959142807433877,
-                    "velocityY": -0.03380527554378375,
-                    "timestamp": 1.4543077149800823
-                },
-                {
-                    "x": 3.478230941686313,
-                    "y": 6.366637378155631,
-                    "heading": -0.13785841940389754,
-                    "angularVelocity": 0.2792790702446555,
-                    "velocityX": 1.3972759802792332,
-                    "velocityY": -0.022536847025679124,
-                    "timestamp": 1.5381070072174732
-                },
-                {
-                    "x": 3.536776304244995,
-                    "y": 6.365693092346191,
-                    "heading": -0.11477234815509793,
-                    "angularVelocity": 0.2754924371366116,
-                    "velocityX": 0.6986379120342898,
-                    "velocityY": -0.01126842225306853,
-                    "timestamp": 1.621906299454864
-                },
-                {
-                    "x": 3.536776304244995,
-                    "y": 6.365693092346191,
-                    "heading": -0.09203745526508908,
-                    "angularVelocity": 0.27130172920323553,
-                    "velocityX": -5.971595919811331e-16,
-                    "velocityY": 2.294938009446505e-16,
-                    "timestamp": 1.705705591692255
-                },
-                {
-                    "x": 3.5821674028193073,
-                    "y": 6.315334924760779,
-                    "heading": -0.06814456679249249,
-                    "angularVelocity": 0.26497230333398186,
-                    "velocityX": 0.5033876064793726,
-                    "velocityY": -0.558472437189569,
-                    "timestamp": 1.7958768593000762
-                },
-                {
-                    "x": 3.6729493357217975,
-                    "y": 6.214618882752082,
-                    "heading": -0.0452520324499122,
-                    "angularVelocity": 0.2538783689072417,
-                    "velocityX": 1.006772282467237,
-                    "velocityY": -1.1169416232090381,
-                    "timestamp": 1.8860481269078975
-                },
-                {
-                    "x": 3.809120897123708,
-                    "y": 6.063546304100605,
-                    "heading": -0.024382898724538877,
-                    "angularVelocity": 0.23143884165108508,
-                    "velocityX": 1.5101435858055878,
-                    "velocityY": -1.6753959732332042,
-                    "timestamp": 1.9762193945157187
-                },
-                {
-                    "x": 3.9906611673735113,
-                    "y": 5.862140397658975,
-                    "heading": -0.011123644415836652,
-                    "angularVelocity": 0.14704522472038728,
-                    "velocityX": 2.0132828900595015,
-                    "velocityY": -2.2335929369164194,
-                    "timestamp": 2.06639066212354
-                },
-                {
-                    "x": 4.126829478795296,
-                    "y": 5.711071424627299,
-                    "heading": -0.0049044530235942295,
-                    "angularVelocity": 0.06897087683516459,
-                    "velocityX": 1.5101075435028497,
-                    "velocityY": -1.6753559868839316,
-                    "timestamp": 2.1565619297313616
-                },
-                {
-                    "x": 4.217609649564756,
-                    "y": 5.610357337578787,
-                    "heading": -0.0015015365594071913,
-                    "angularVelocity": 0.03773836782456074,
-                    "velocityX": 1.0067527403993772,
-                    "velocityY": -1.1169199426866636,
-                    "timestamp": 2.246733197339183
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 4.052308831945937e-18,
-                    "angularVelocity": 0.016652051138278397,
-                    "velocityX": 0.5033793096117788,
-                    "velocityY": -0.5584632324101773,
-                    "timestamp": 2.3369044649470045
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 1.897041849287005e-18,
-                    "angularVelocity": -2.8636978124151836e-18,
-                    "velocityX": 2.1901737652514486e-16,
-                    "velocityY": -2.4329458809860463e-16,
-                    "timestamp": 2.427075732554826
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        1,
-                        2
-                    ],
-                    "type": "StraightLine"
-                },
-                {
-                    "scope": [
-                        2,
-                        3
-                    ],
-                    "type": "StraightLine"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "W3 To S3": {
-            "waypoints": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 11
-                },
-                {
-                    "x": 2.2382969856262207,
-                    "y": 2.9569339752197266,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 13
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.284359298751307,
-                    "y": 4.3328670624584875,
-                    "heading": -0.3913603360985968,
-                    "angularVelocity": -0.02219557362709146,
-                    "velocityX": -0.2551974739939649,
-                    "velocityY": -0.44270758788444614,
-                    "timestamp": 0.061288620940906494
-                },
-                {
-                    "x": 2.2541133789559487,
-                    "y": 4.2780305351291235,
-                    "heading": -0.3941996942145126,
-                    "angularVelocity": -0.04632765548197678,
-                    "velocityX": -0.4934997611468709,
-                    "velocityY": -0.89472607618691,
-                    "timestamp": 0.12257724188181299
-                },
-                {
-                    "x": 2.2108693669713233,
-                    "y": 4.194701771021148,
-                    "heading": -0.39869373638946143,
-                    "angularVelocity": -0.0733258818024626,
-                    "velocityX": -0.7055797849705345,
-                    "velocityY": -1.3596123200148318,
-                    "timestamp": 0.1838658628227195
-                },
-                {
-                    "x": 2.157413012714945,
-                    "y": 4.081769184110331,
-                    "heading": -0.40512443208979565,
-                    "angularVelocity": -0.1049247903054391,
-                    "velocityX": -0.8722068376757797,
-                    "velocityY": -1.8426354709417345,
-                    "timestamp": 0.24515448376362597
-                },
-                {
-                    "x": 2.099439422754663,
-                    "y": 3.9378510886177804,
-                    "heading": -0.41398926531008917,
-                    "angularVelocity": -0.1446407682894496,
-                    "velocityX": -0.9459111507204496,
-                    "velocityY": -2.348202542056109,
-                    "timestamp": 0.30644310470453245
-                },
-                {
-                    "x": 2.05084134036617,
-                    "y": 3.764062564560248,
-                    "heading": -0.42610260644735526,
-                    "angularVelocity": -0.1976442111325286,
-                    "velocityX": -0.7929380958881483,
-                    "velocityY": -2.835575697242347,
-                    "timestamp": 0.36773172564543893
-                },
-                {
-                    "x": 2.0313297442556637,
-                    "y": 3.578686605657539,
-                    "heading": -0.44115755081157715,
-                    "angularVelocity": -0.24564012263773397,
-                    "velocityX": -0.3183559331400068,
-                    "velocityY": -3.024639093795978,
-                    "timestamp": 0.4290203465863454
-                },
-                {
-                    "x": 2.0424292488902682,
-                    "y": 3.3999266361315956,
-                    "heading": -0.45764648760363913,
-                    "angularVelocity": -0.2690374907923018,
-                    "velocityX": 0.18110220892890863,
-                    "velocityY": -2.9166910069375023,
-                    "timestamp": 0.4903089675272519
-                },
-                {
-                    "x": 2.0816987250314285,
-                    "y": 3.234855796713624,
-                    "heading": -0.4747853916237668,
-                    "angularVelocity": -0.27964251368378285,
-                    "velocityX": 0.6407302944379066,
-                    "velocityY": -2.6933358408754287,
-                    "timestamp": 0.5515975884681584
-                },
-                {
-                    "x": 2.1473776050382467,
-                    "y": 3.086622748034336,
-                    "heading": -0.49218081575104855,
-                    "angularVelocity": -0.2838279579508591,
-                    "velocityX": 1.071632531431,
-                    "velocityY": -2.418606364502998,
-                    "timestamp": 0.6128862094090649
-                },
-                {
-                    "x": 2.2382969856262207,
-                    "y": 2.9569339752197266,
-                    "heading": -0.5096053711064815,
-                    "angularVelocity": -0.2843032701328595,
-                    "velocityX": 1.4834626590087072,
-                    "velocityY": -2.116033463041253,
-                    "timestamp": 0.6741748303499714
-                },
-                {
-                    "x": 2.3750291535583634,
-                    "y": 2.833789120602945,
-                    "heading": -0.5294729435704262,
-                    "angularVelocity": -0.282105779182567,
-                    "velocityX": 1.9415021560293924,
-                    "velocityY": -1.7485717103605045,
-                    "timestamp": 0.7446007979480589
-                },
-                {
-                    "x": 2.5431654959507464,
-                    "y": 2.737552434720476,
-                    "heading": -0.5488979650801319,
-                    "angularVelocity": -0.2758218619098283,
-                    "velocityX": 2.3874196993915153,
-                    "velocityY": -1.366494336743516,
-                    "timestamp": 0.8150267655461465
-                },
-                {
-                    "x": 2.7413669689925393,
-                    "y": 2.669711436345154,
-                    "heading": -0.5674506234763109,
-                    "angularVelocity": -0.2634349094364851,
-                    "velocityX": 2.8143237473555907,
-                    "velocityY": -0.9632952260234809,
-                    "timestamp": 0.8854527331442341
-                },
-                {
-                    "x": 2.9672720655352354,
-                    "y": 2.6325722400915463,
-                    "heading": -0.5844267226661307,
-                    "angularVelocity": -0.24104885979984425,
-                    "velocityX": 3.207696028145596,
-                    "velocityY": -0.5273508837756647,
-                    "timestamp": 0.9558787007423217
-                },
-                {
-                    "x": 3.21586763898004,
-                    "y": 2.629998478445332,
-                    "heading": -0.5985057428976699,
-                    "angularVelocity": -0.19991234358165255,
-                    "velocityX": 3.529885096694866,
-                    "velocityY": -0.03654563414595668,
-                    "timestamp": 1.0263046683404093
-                },
-                {
-                    "x": 3.4730987079905056,
-                    "y": 2.6678365706373897,
-                    "heading": -0.6068355688677516,
-                    "angularVelocity": -0.11827776392962061,
-                    "velocityX": 3.652503157336117,
-                    "velocityY": 0.5372747224148254,
-                    "timestamp": 1.096730635938497
-                },
-                {
-                    "x": 3.704150591962856,
-                    "y": 2.737591563426048,
-                    "heading": -0.6062385008661115,
-                    "angularVelocity": 0.00847795240879764,
-                    "velocityX": 3.28077684769537,
-                    "velocityY": 0.9904726220694814,
-                    "timestamp": 1.1671566035365846
-                },
-                {
-                    "x": 3.894448488053554,
-                    "y": 2.8141322684658854,
-                    "heading": -0.5991077413907381,
-                    "angularVelocity": 0.10125184954600595,
-                    "velocityX": 2.702098424500253,
-                    "velocityY": 1.086825039829707,
-                    "timestamp": 1.2375825711346722
-                },
-                {
-                    "x": 4.043960756386648,
-                    "y": 2.883897596305393,
-                    "heading": -0.5885679211534333,
-                    "angularVelocity": 0.1496581530473843,
-                    "velocityX": 2.122970736963697,
-                    "velocityY": 0.9906193726389452,
-                    "timestamp": 1.3080085387327598
-                },
-                {
-                    "x": 4.154363809487208,
-                    "y": 2.940218619761556,
-                    "heading": -0.5783208756256007,
-                    "angularVelocity": 0.14550095479427785,
-                    "velocityX": 1.5676469470837475,
-                    "velocityY": 0.7997195548321091,
-                    "timestamp": 1.3784345063308474
-                },
-                {
-                    "x": 4.227038806897989,
-                    "y": 2.9796144762337837,
-                    "heading": -0.5703846528919979,
-                    "angularVelocity": 0.11268887037369305,
-                    "velocityX": 1.0319346668480025,
-                    "velocityY": 0.5593938971070241,
-                    "timestamp": 1.448860473928935
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "angularVelocity": 0.06225903656760019,
-                    "velocityX": 0.5106240542868632,
-                    "velocityY": 0.2894603292148455,
-                    "timestamp": 1.5192864415270226
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "angularVelocity": 9.24329495349137e-32,
-                    "velocityX": 0,
-                    "velocityY": 1.2740379664097394e-33,
-                    "timestamp": 1.5897124091251102
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "W3 To S4": {
-            "waypoints": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 11
-                },
-                {
-                    "x": 2.4722259044647217,
-                    "y": 2.73530650138855,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 16
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 9.57151893871228e-33,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.2927892107770984,
-                    "y": 4.332309394440523,
-                    "heading": -0.3722824674321399,
-                    "angularVelocity": 0.3002660031020358,
-                    "velocityX": -0.12220408518393268,
-                    "velocityY": -0.4692835993371687,
-                    "timestamp": 0.059006122520770025
-                },
-                {
-                    "x": 2.2795380946752672,
-                    "y": 4.276530177721837,
-                    "heading": -0.3395678297253276,
-                    "angularVelocity": 0.5544278510301563,
-                    "velocityX": -0.224571883996739,
-                    "velocityY": -0.9453123563415277,
-                    "timestamp": 0.11801224504154005
-                },
-                {
-                    "x": 2.2618545093830753,
-                    "y": 4.192237257969867,
-                    "heading": -0.2954402922678697,
-                    "angularVelocity": 0.7478467584770477,
-                    "velocityX": -0.29969068524994186,
-                    "velocityY": -1.4285453127732544,
-                    "timestamp": 0.17701836756231007
-                },
-                {
-                    "x": 2.2420434066604593,
-                    "y": 4.079048226765037,
-                    "heading": -0.24482967906698166,
-                    "angularVelocity": 0.8577179966887506,
-                    "velocityX": -0.33574656114103973,
-                    "velocityY": -1.9182590953165608,
-                    "timestamp": 0.2360244900830801
-                },
-                {
-                    "x": 2.2234541825747725,
-                    "y": 3.936854095556344,
-                    "heading": -0.19463836205748358,
-                    "angularVelocity": 0.8506120189787272,
-                    "velocityX": -0.3150389026010499,
-                    "velocityY": -2.40981994976269,
-                    "timestamp": 0.2950306126038501
-                },
-                {
-                    "x": 2.211142623620853,
-                    "y": 3.7664761504723194,
-                    "heading": -0.15521354512815658,
-                    "angularVelocity": 0.6681479013546628,
-                    "velocityX": -0.2086488389333828,
-                    "velocityY": -2.8874621446961175,
-                    "timestamp": 0.35403673512462014
-                },
-                {
-                    "x": 2.2125400151692816,
-                    "y": 3.5715383450180926,
-                    "heading": -0.14131017152189063,
-                    "angularVelocity": 0.23562594883900537,
-                    "velocityX": 0.023682144983125607,
-                    "velocityY": -3.303687772156347,
-                    "timestamp": 0.41304285764539017
-                },
-                {
-                    "x": 2.235363094184648,
-                    "y": 3.3576277458759463,
-                    "heading": -0.144791073330025,
-                    "angularVelocity": -0.058992214018278255,
-                    "velocityX": 0.38679170974728244,
-                    "velocityY": -3.6252271798888405,
-                    "timestamp": 0.4720489801661602
-                },
-                {
-                    "x": 2.287018648077521,
-                    "y": 3.1404706845748738,
-                    "heading": -0.14479216409186968,
-                    "angularVelocity": -0.000018485570624385508,
-                    "velocityX": 0.8754270181825738,
-                    "velocityY": -3.6802462528296687,
-                    "timestamp": 0.5310551026869302
-                },
-                {
-                    "x": 2.3664206535422005,
-                    "y": 2.9318538879811458,
-                    "heading": -0.14479229465212481,
-                    "angularVelocity": -0.000002212656069584398,
-                    "velocityX": 1.3456570618876043,
-                    "velocityY": -3.53551102295012,
-                    "timestamp": 0.5900612252077002
-                },
-                {
-                    "x": 2.4722259044647217,
-                    "y": 2.73530650138855,
-                    "heading": -0.14479252387880184,
-                    "angularVelocity": -0.000003884794784189704,
-                    "velocityX": 1.7931232625102267,
-                    "velocityY": -3.330965977698533,
-                    "timestamp": 0.6490673477284702
-                },
-                {
-                    "x": 2.6222628757424284,
-                    "y": 2.533912575740545,
-                    "heading": -0.14479267276909355,
-                    "angularVelocity": -0.0000022427572708026126,
-                    "velocityX": 2.260029880862475,
-                    "velocityY": -3.033627551346843,
-                    "timestamp": 0.7154545103709612
-                },
-                {
-                    "x": 2.800083925097921,
-                    "y": 2.356570260194387,
-                    "heading": -0.14479275333723068,
-                    "angularVelocity": -0.0000012136101911737535,
-                    "velocityX": 2.678545704884188,
-                    "velocityY": -2.671334464182248,
-                    "timestamp": 0.7818416730134522
-                },
-                {
-                    "x": 3.001881619185789,
-                    "y": 2.207076716019015,
-                    "heading": -0.14479280967120484,
-                    "angularVelocity": -8.485672817028763e-7,
-                    "velocityX": 3.0397095772053357,
-                    "velocityY": -2.2518441551784094,
-                    "timestamp": 0.8482288356559432
-                },
-                {
-                    "x": 3.2233351494929763,
-                    "y": 2.0886327789398735,
-                    "heading": -0.14479285532879432,
-                    "angularVelocity": -6.877472642883405e-7,
-                    "velocityX": 3.3357884490373766,
-                    "velocityY": -1.7841391673415428,
-                    "timestamp": 0.9146159982984342
-                },
-                {
-                    "x": 3.4597028157497376,
-                    "y": 2.003774307484127,
-                    "heading": -0.14479289641721121,
-                    "angularVelocity": -6.189211176196774e-7,
-                    "velocityX": 3.5604423633775646,
-                    "velocityY": -1.2782361540698368,
-                    "timestamp": 0.9810031609409252
-                },
-                {
-                    "x": 3.704835569204283,
-                    "y": 1.949179734375487,
-                    "heading": -0.14479293648657837,
-                    "angularVelocity": -6.035710154638316e-7,
-                    "velocityX": 3.692472214464675,
-                    "velocityY": -0.8223664174750668,
-                    "timestamp": 1.047390323583416
-                },
-                {
-                    "x": 3.9499686718209266,
-                    "y": 1.8945867290481686,
-                    "heading": -0.1447929765559331,
-                    "angularVelocity": -6.035708281824289e-7,
-                    "velocityX": 3.692477473946853,
-                    "velocityY": -0.8223428017448724,
-                    "timestamp": 1.113777486225907
-                },
-                {
-                    "x": 4.195101784677066,
-                    "y": 1.8399937696982192,
-                    "heading": -0.14479301662532418,
-                    "angularVelocity": -6.035713756667977e-7,
-                    "velocityX": 3.692477628185933,
-                    "velocityY": -0.8223421091807183,
-                    "timestamp": 1.1801646488683981
-                },
-                {
-                    "x": 4.435904668374261,
-                    "y": 1.7860806487644325,
-                    "heading": -0.15898872227240893,
-                    "angularVelocity": -0.21383208864538455,
-                    "velocityX": 3.627250723064772,
-                    "velocityY": -0.8121015989811171,
-                    "timestamp": 1.2465518115108891
-                },
-                {
-                    "x": 4.648245693564204,
-                    "y": 1.7385415440023928,
-                    "heading": -0.26462575063007965,
-                    "angularVelocity": -1.5912267395211415,
-                    "velocityX": 3.1985253886123286,
-                    "velocityY": -0.7160888170209653,
-                    "timestamp": 1.3129389741533801
-                },
-                {
-                    "x": 4.824757883271093,
-                    "y": 1.6989793937314068,
-                    "heading": -0.37654836939407044,
-                    "angularVelocity": -1.6859075506317058,
-                    "velocityX": 2.6588301515075172,
-                    "velocityY": -0.5959307296206723,
-                    "timestamp": 1.3793261367958711
-                },
-                {
-                    "x": 4.96555652453039,
-                    "y": 1.6673723665346096,
-                    "heading": -0.4762340168485147,
-                    "angularVelocity": -1.501580177349536,
-                    "velocityX": 2.1208715006774397,
-                    "velocityY": -0.47610149219673153,
-                    "timestamp": 1.4457132994383621
-                },
-                {
-                    "x": 5.070906714490224,
-                    "y": 1.643682366519874,
-                    "heading": -0.5558849180153355,
-                    "angularVelocity": -1.1997937251175768,
-                    "velocityX": 1.5869060487969315,
-                    "velocityY": -0.3568460990313944,
-                    "timestamp": 1.5121004620808531
-                },
-                {
-                    "x": 5.141006607602348,
-                    "y": 1.6278935430132826,
-                    "heading": -0.6113350442973207,
-                    "angularVelocity": -0.8352537459779052,
-                    "velocityX": 1.055925427776252,
-                    "velocityY": -0.23782946699526453,
-                    "timestamp": 1.5784876247233441
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "angularVelocity": -0.4317846186174008,
-                    "velocityX": 0.5271108299370921,
-                    "velocityY": -0.11890164753374041,
-                    "timestamp": 1.6448747873658351
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "angularVelocity": 1.0662498702136072e-32,
-                    "velocityX": 0,
-                    "velocityY": 8.479039452208929e-33,
-                    "timestamp": 1.7112619500083261
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "W3 To C1": {
-            "waypoints": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 12
-                },
-                {
-                    "x": 2.1241281032562256,
-                    "y": 6.282596111297607,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 30
-                },
-                {
-                    "x": 5.676522731781006,
-                    "y": 6.552661418914795,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 27
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.2999999999310377,
-                    "y": 4.359999999993798,
-                    "heading": -0.3900000000006091,
-                    "angularVelocity": -1.7156392127364986e-12,
-                    "velocityX": -4.6004235672195495e-11,
-                    "velocityY": -2.541589729484622e-10,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.2830691122580777,
-                    "y": 4.4226688824706715,
-                    "heading": -0.3961360368292833,
-                    "angularVelocity": -0.04017087715885992,
-                    "velocityX": -0.11084167601962927,
-                    "velocityY": 0.4102752396150543,
-                    "timestamp": 0.1527483904267538
-                },
-                {
-                    "x": 2.2499539829007165,
-                    "y": 4.551962677072163,
-                    "heading": -0.39368124618911415,
-                    "angularVelocity": 0.016070811835100392,
-                    "velocityX": -0.21679527487321065,
-                    "velocityY": 0.8464494731937718,
-                    "timestamp": 0.3054967808535076
-                },
-                {
-                    "x": 2.208537052758008,
-                    "y": 4.723131432963358,
-                    "heading": -0.3887340677228252,
-                    "angularVelocity": 0.032387761683442845,
-                    "velocityX": -0.27114478888671906,
-                    "velocityY": 1.120592861369484,
-                    "timestamp": 0.45824517128026143
-                },
-                {
-                    "x": 2.1635048676051025,
-                    "y": 4.925567881437362,
-                    "heading": -0.37513416937632715,
-                    "angularVelocity": 0.08903464258184278,
-                    "velocityX": -0.294812828865451,
-                    "velocityY": 1.3252934968125494,
-                    "timestamp": 0.6109935617070152
-                },
-                {
-                    "x": 2.119479495251874,
-                    "y": 5.143090740041083,
-                    "heading": -0.3554033967273168,
-                    "angularVelocity": 0.12917172215836753,
-                    "velocityX": -0.28822151359768483,
-                    "velocityY": 1.4240599066467436,
-                    "timestamp": 0.763741952133769
-                },
-                {
-                    "x": 2.08196432941898,
-                    "y": 5.365761039079461,
-                    "heading": -0.32917795560380303,
-                    "angularVelocity": 0.1716904580681245,
-                    "velocityX": -0.24560105455579473,
-                    "velocityY": 1.4577587261323537,
-                    "timestamp": 0.9164903425605229
-                },
-                {
-                    "x": 2.052346293594587,
-                    "y": 5.582587972754811,
-                    "heading": -0.29825458565802576,
-                    "angularVelocity": 0.20244645364142877,
-                    "velocityX": -0.19390080434302895,
-                    "velocityY": 1.419503885313628,
-                    "timestamp": 1.0692387329872766
-                },
-                {
-                    "x": 2.030803808580604,
-                    "y": 5.789012383645482,
-                    "heading": -0.2654877856320327,
-                    "angularVelocity": 0.2145148628152058,
-                    "velocityX": -0.14103248420843936,
-                    "velocityY": 1.3514015455604398,
-                    "timestamp": 1.2219871234140305
-                },
-                {
-                    "x": 2.0185215947450232,
-                    "y": 5.980748966625485,
-                    "heading": -0.23040407839794677,
-                    "angularVelocity": 0.2296829912414591,
-                    "velocityX": -0.08040813886408783,
-                    "velocityY": 1.2552445394842306,
-                    "timestamp": 1.3747355138407844
-                },
-                {
-                    "x": 2.017537204929899,
-                    "y": 6.152494473346335,
-                    "heading": -0.19497588875865757,
-                    "angularVelocity": 0.23193821893113326,
-                    "velocityX": -0.006444518152378274,
-                    "velocityY": 1.1243686841111533,
-                    "timestamp": 1.5274839042675383
-                },
-                {
-                    "x": 2.0415938446669073,
-                    "y": 6.276321571691329,
-                    "heading": -0.16155868009406138,
-                    "angularVelocity": 0.21877290194991583,
-                    "velocityX": 0.15749193627933206,
-                    "velocityY": 0.8106605773582652,
-                    "timestamp": 1.6802322946942922
-                },
-                {
-                    "x": 2.1241281032079797,
-                    "y": 6.2825961111606565,
-                    "heading": -0.060349903928264906,
-                    "angularVelocity": 0.6625848944275851,
-                    "velocityX": 0.5403281719922621,
-                    "velocityY": 0.04107761453342325,
-                    "timestamp": 1.8329806851210462
-                },
-                {
-                    "x": 2.1855581068088026,
-                    "y": 6.287266231959223,
-                    "heading": 0.012078838000808553,
-                    "angularVelocity": 0.7458652653001706,
-                    "velocityX": 0.6326011577496066,
-                    "velocityY": 0.048092524107779686,
-                    "timestamp": 1.9300876899626762
-                },
-                {
-                    "x": 2.2551653620091354,
-                    "y": 6.2925580157259535,
-                    "heading": 0.08826667660975476,
-                    "angularVelocity": 0.784576135772582,
-                    "velocityX": 0.7168098264960764,
-                    "velocityY": 0.05449435852189788,
-                    "timestamp": 2.0271946948043063
-                },
-                {
-                    "x": 2.3319122711008053,
-                    "y": 6.29839258064769,
-                    "heading": 0.16657489622034788,
-                    "angularVelocity": 0.8064116460755234,
-                    "velocityX": 0.7903333988427459,
-                    "velocityY": 0.06008387443491743,
-                    "timestamp": 2.1243016996459363
-                },
-                {
-                    "x": 2.4149946395663093,
-                    "y": 6.304708789079692,
-                    "heading": 0.2457212902089042,
-                    "angularVelocity": 0.8150430973252747,
-                    "velocityX": 0.8555754417689245,
-                    "velocityY": 0.06504379984951533,
-                    "timestamp": 2.2214087044875663
-                },
-                {
-                    "x": 2.5038373372195277,
-                    "y": 6.311462917641332,
-                    "heading": 0.32495163133637983,
-                    "angularVelocity": 0.8159075780845946,
-                    "velocityX": 0.9148948410382448,
-                    "velocityY": 0.06955346543875748,
-                    "timestamp": 2.3185157093291964
-                },
-                {
-                    "x": 2.598073257877524,
-                    "y": 6.3186270576568155,
-                    "heading": 0.40431668105413066,
-                    "angularVelocity": 0.8172947961305416,
-                    "velocityX": 0.9704338108518987,
-                    "velocityY": 0.0737757297255109,
-                    "timestamp": 2.4156227141708264
-                },
-                {
-                    "x": 2.697382502344975,
-                    "y": 6.326176889286233,
-                    "heading": 0.48416024389571777,
-                    "angularVelocity": 0.8222224852016302,
-                    "velocityX": 1.0226784849096606,
-                    "velocityY": 0.07774755027256483,
-                    "timestamp": 2.5127297190124565
-                },
-                {
-                    "x": 2.8014852531337993,
-                    "y": 6.334091139837385,
-                    "heading": 0.5646614354219063,
-                    "angularVelocity": 0.8289946917215433,
-                    "velocityX": 1.0720416213516444,
-                    "velocityY": 0.08150030637886811,
-                    "timestamp": 2.6098367238540865
-                },
-                {
-                    "x": 2.910148663901152,
-                    "y": 6.342352107524804,
-                    "heading": 0.6457203378655492,
-                    "angularVelocity": 0.8347379532340086,
-                    "velocityX": 1.119006925166821,
-                    "velocityY": 0.0850707709678732,
-                    "timestamp": 2.7069437286957165
-                },
-                {
-                    "x": 3.0231702594110863,
-                    "y": 6.350944399479571,
-                    "heading": 0.7270565797219191,
-                    "angularVelocity": 0.8375939718950361,
-                    "velocityX": 1.1638871555335075,
-                    "velocityY": 0.08848272107248593,
-                    "timestamp": 2.8040507335373466
-                },
-                {
-                    "x": 3.140351376164449,
-                    "y": 6.359852912614292,
-                    "heading": 0.8083350217640537,
-                    "angularVelocity": 0.83699875417797,
-                    "velocityX": 1.2067215654083316,
-                    "velocityY": 0.09173914083795442,
-                    "timestamp": 2.9011577383789766
-                },
-                {
-                    "x": 3.261472243862432,
-                    "y": 6.3690609392298585,
-                    "heading": 0.8891526159463483,
-                    "angularVelocity": 0.8322529806195065,
-                    "velocityX": 1.2472927986960072,
-                    "velocityY": 0.09482350610318373,
-                    "timestamp": 2.9982647432206067
-                },
-                {
-                    "x": 3.386280226554517,
-                    "y": 6.378549273066906,
-                    "heading": 0.9690024828336246,
-                    "angularVelocity": 0.8222874040565998,
-                    "velocityX": 1.2852624076036416,
-                    "velocityY": 0.09771008689812195,
-                    "timestamp": 3.0953717480622367
-                },
-                {
-                    "x": 3.5144942146424594,
-                    "y": 6.388296543201246,
-                    "heading": 1.0472679478096496,
-                    "angularVelocity": 0.8059713626886263,
-                    "velocityX": 1.3203371728362745,
-                    "velocityY": 0.10037659168026988,
-                    "timestamp": 3.1924787529038667
-                },
-                {
-                    "x": 3.64579126909874,
-                    "y": 6.398278198691388,
-                    "heading": 1.1233630936332748,
-                    "angularVelocity": 0.783621593451087,
-                    "velocityX": 1.3520863383515835,
-                    "velocityY": 0.10279027288912505,
-                    "timestamp": 3.2895857577454968
-                },
-                {
-                    "x": 3.779855631903331,
-                    "y": 6.408470234520327,
-                    "heading": 1.1966377830577097,
-                    "angularVelocity": 0.7545767639735752,
-                    "velocityX": 1.3805838535787394,
-                    "velocityY": 0.10495675240615632,
-                    "timestamp": 3.386692762587127
-                },
-                {
-                    "x": 3.916399996401096,
-                    "y": 6.418850808641588,
-                    "heading": 1.2663868507253695,
-                    "angularVelocity": 0.7182701992663435,
-                    "velocityX": 1.406122708347489,
-                    "velocityY": 0.10689830431744617,
-                    "timestamp": 3.483799767428757
-                },
-                {
-                    "x": 4.055149110841729,
-                    "y": 6.429398995480507,
-                    "heading": 1.3319162676094702,
-                    "angularVelocity": 0.6748165803464835,
-                    "velocityX": 1.4288270430729209,
-                    "velocityY": 0.10862436634220823,
-                    "timestamp": 3.580906772270387
-                },
-                {
-                    "x": 4.195643021249515,
-                    "y": 6.440079827739076,
-                    "heading": 1.392465239071673,
-                    "angularVelocity": 0.6235283604234658,
-                    "velocityX": 1.4467948093350342,
-                    "velocityY": 0.10999033795341527,
-                    "timestamp": 3.678013777112017
-                },
-                {
-                    "x": 4.337478451024624,
-                    "y": 6.45086264692987,
-                    "heading": 1.4461855651355748,
-                    "angularVelocity": 0.5532075282023133,
-                    "velocityX": 1.4606096661273347,
-                    "velocityY": 0.11104059096144768,
-                    "timestamp": 3.775120781953647
-                },
-                {
-                    "x": 4.47965775917095,
-                    "y": 6.461671608979337,
-                    "heading": 1.49351259060778,
-                    "angularVelocity": 0.48736984118650195,
-                    "velocityX": 1.4641508974652757,
-                    "velocityY": 0.11130980792840979,
-                    "timestamp": 3.872227786795277
-                },
-                {
-                    "x": 4.621471854948602,
-                    "y": 6.4724528062914635,
-                    "heading": 1.5350394838354429,
-                    "angularVelocity": 0.42764055328666206,
-                    "velocityX": 1.4603899702927075,
-                    "velocityY": 0.11102388888102613,
-                    "timestamp": 3.969334791636907
-                },
-                {
-                    "x": 4.762134624452166,
-                    "y": 6.483146475819268,
-                    "heading": 1.5711187532761213,
-                    "angularVelocity": 0.37154136862160403,
-                    "velocityX": 1.4485337063274482,
-                    "velocityY": 0.11012253482900584,
-                    "timestamp": 4.0664417964785375
-                },
-                {
-                    "x": 4.901310758087736,
-                    "y": 6.493727126167039,
-                    "heading": 1.6012523506615222,
-                    "angularVelocity": 0.3103133235343918,
-                    "velocityX": 1.433224451514287,
-                    "velocityY": 0.10895867241503827,
-                    "timestamp": 4.163548801320168
-                },
-                {
-                    "x": 5.0385652541984065,
-                    "y": 6.504161687000508,
-                    "heading": 1.6187441808494436,
-                    "angularVelocity": 0.18012943788636607,
-                    "velocityX": 1.413435585164251,
-                    "velocityY": 0.1074542543793728,
-                    "timestamp": 4.2606558061617985
-                },
-                {
-                    "x": 5.1726571421935885,
-                    "y": 6.5143558154807435,
-                    "heading": 1.6310181673546418,
-                    "angularVelocity": 0.12639650992409435,
-                    "velocityX": 1.3808673050498854,
-                    "velocityY": 0.104978301436634,
-                    "timestamp": 4.357762811003429
-                },
-                {
-                    "x": 5.303636473288953,
-                    "y": 6.524313316659959,
-                    "heading": 1.6376756877148975,
-                    "angularVelocity": 0.06855860046187665,
-                    "velocityX": 1.3488144481980282,
-                    "velocityY": 0.1025415325257505,
-                    "timestamp": 4.4548698158450595
-                },
-                {
-                    "x": 5.4313983902221485,
-                    "y": 6.53402621894025,
-                    "heading": 1.6386463375950402,
-                    "angularVelocity": 0.009995672907119661,
-                    "velocityX": 1.3156817797402116,
-                    "velocityY": 0.1000226726538996,
-                    "timestamp": 4.55197682068669
-                },
-                {
-                    "x": 5.555758593734715,
-                    "y": 6.5434805112143435,
-                    "heading": 1.6339151298589827,
-                    "angularVelocity": -0.048721590949420676,
-                    "velocityX": 1.2806512124922533,
-                    "velocityY": 0.09735952745677612,
-                    "timestamp": 4.6490838255283204
-                },
-                {
-                    "x": 5.676522731590665,
-                    "y": 6.552661418256687,
-                    "heading": 1.6211253547169766,
-                    "angularVelocity": -0.13170805962068485,
-                    "velocityX": 1.2436192217305724,
-                    "velocityY": 0.0945442281350229,
-                    "timestamp": 4.746190830369951
-                },
-                {
-                    "x": 5.8040475153273645,
-                    "y": 6.568758338060969,
-                    "heading": 1.594817916740329,
-                    "angularVelocity": -0.24917360707174604,
-                    "velocityX": 1.2078641151592902,
-                    "velocityY": 0.15246363592342288,
-                    "timestamp": 4.851769580629666
-                },
-                {
-                    "x": 5.9283875752902055,
-                    "y": 6.58985033542802,
-                    "heading": 1.559498986776326,
-                    "angularVelocity": -0.33452688097090233,
-                    "velocityX": 1.1776996757735678,
-                    "velocityY": 0.19977502680380196,
-                    "timestamp": 4.957348330889381
-                },
-                {
-                    "x": 6.049721376535089,
-                    "y": 6.61532142807792,
-                    "heading": 1.5174190958344496,
-                    "angularVelocity": -0.3985640187289682,
-                    "velocityX": 1.1492255865917638,
-                    "velocityY": 0.2412520781772084,
-                    "timestamp": 5.0629270811490965
-                },
-                {
-                    "x": 6.167915961542227,
-                    "y": 6.644893212673861,
-                    "heading": 1.4694436889435494,
-                    "angularVelocity": -0.45440400479138554,
-                    "velocityX": 1.119492176613303,
-                    "velocityY": 0.280092203729064,
-                    "timestamp": 5.168505831408812
-                },
-                {
-                    "x": 6.2827599678090085,
-                    "y": 6.678289224223258,
-                    "heading": 1.4161083022197893,
-                    "angularVelocity": -0.5051716053929952,
-                    "velocityX": 1.087756824956101,
-                    "velocityY": 0.31631376267017064,
-                    "timestamp": 5.274084581668527
-                },
-                {
-                    "x": 6.394018817655917,
-                    "y": 6.715173356117064,
-                    "heading": 1.3577889107275223,
-                    "angularVelocity": -0.5523781194839231,
-                    "velocityX": 1.0537996466964168,
-                    "velocityY": 0.34935185331081736,
-                    "timestamp": 5.379663331928242
-                },
-                {
-                    "x": 6.501464569355021,
-                    "y": 6.755149800954155,
-                    "heading": 1.2948750452839908,
-                    "angularVelocity": -0.5958951523626541,
-                    "velocityX": 1.0176834949234126,
-                    "velocityY": 0.3786410134453019,
-                    "timestamp": 5.485242082187957
-                },
-                {
-                    "x": 6.6048776092971675,
-                    "y": 6.797770420027359,
-                    "heading": 1.22768881668393,
-                    "angularVelocity": -0.6363612797096425,
-                    "velocityX": 0.9794872502530397,
-                    "velocityY": 0.4036855817031342,
-                    "timestamp": 5.5908208324476725
-                },
-                {
-                    "x": 6.70404339771049,
-                    "y": 6.8425429323292,
-                    "heading": 1.1564439654233045,
-                    "angularVelocity": -0.6748029419532755,
-                    "velocityX": 0.939258970068964,
-                    "velocityY": 0.4240674597490353,
-                    "timestamp": 5.696399582707388
-                },
-                {
-                    "x": 6.79877778418725,
-                    "y": 6.888932915699562,
-                    "heading": 1.0813771004184407,
-                    "angularVelocity": -0.7110035384939538,
-                    "velocityX": 0.8972864903430491,
-                    "velocityY": 0.4393875033009405,
-                    "timestamp": 5.801978332967103
-                },
-                {
-                    "x": 6.88894352921239,
-                    "y": 6.936373258053616,
-                    "heading": 1.0028508284852347,
-                    "angularVelocity": -0.7437696672526416,
-                    "velocityX": 0.8540141324253476,
-                    "velocityY": 0.44933608637180433,
-                    "timestamp": 5.907557083226818
-                },
-                {
-                    "x": 6.974457896759412,
-                    "y": 6.984282785155121,
-                    "heading": 0.9213875618911144,
-                    "angularVelocity": -0.7715877144447874,
-                    "velocityX": 0.809958131723675,
-                    "velocityY": 0.45378001785130806,
-                    "timestamp": 6.013135833486533
-                },
-                {
-                    "x": 7.055294443748765,
-                    "y": 7.032089281864471,
-                    "heading": 0.8376815844807217,
-                    "angularVelocity": -0.792829780737162,
-                    "velocityX": 0.7656516726874414,
-                    "velocityY": 0.45280415475213037,
-                    "timestamp": 6.118714583746248
-                },
-                {
-                    "x": 7.131475603500295,
-                    "y": 7.0792526481673885,
-                    "heading": 0.752591160441856,
-                    "angularVelocity": -0.805942709434023,
-                    "velocityX": 0.7215576931722669,
-                    "velocityY": 0.44671267879627613,
-                    "timestamp": 6.224293334005964
-                },
-                {
-                    "x": 7.203056823429108,
-                    "y": 7.125284419454981,
-                    "heading": 0.6670903070961578,
-                    "angularVelocity": -0.8098301328249344,
-                    "velocityX": 0.6779888895797812,
-                    "velocityY": 0.4359946600433992,
-                    "timestamp": 6.329872084265679
-                },
-                {
-                    "x": 7.270104931236086,
-                    "y": 7.169760877903185,
-                    "heading": 0.5821807537401323,
-                    "angularVelocity": -0.8042295741051197,
-                    "velocityX": 0.6350530514787667,
-                    "velocityY": 0.4212633543513555,
-                    "timestamp": 6.435450834525394
-                },
-                {
-                    "x": 7.332675465601759,
-                    "y": 7.212330496694183,
-                    "heading": 0.49884759114131233,
-                    "angularVelocity": -0.789298626772507,
-                    "velocityX": 0.592643253784208,
-                    "velocityY": 0.4032025262817465,
-                    "timestamp": 6.541029584785109
-                },
-                {
-                    "x": 7.390806680523043,
-                    "y": 7.252712685445242,
-                    "heading": 0.4183197118253645,
-                    "angularVelocity": -0.7627280973825078,
-                    "velocityX": 0.5505957828143127,
-                    "velocityY": 0.38248405756315146,
-                    "timestamp": 6.646608335044824
-                },
-                {
-                    "x": 7.444527736090263,
-                    "y": 7.290684851015479,
-                    "heading": 0.34198396458440167,
-                    "angularVelocity": -0.7230218869110173,
-                    "velocityX": 0.5088245046789369,
-                    "velocityY": 0.3596572750296197,
-                    "timestamp": 6.75218708530454
-                },
-                {
-                    "x": 7.4938504955203875,
-                    "y": 7.326060981562411,
-                    "heading": 0.27119241907250147,
-                    "angularVelocity": -0.6705094095505424,
-                    "velocityX": 0.46716559024619986,
-                    "velocityY": 0.3350686622032283,
-                    "timestamp": 6.857765835564255
-                },
-                {
-                    "x": 7.538762197286186,
-                    "y": 7.358679050922846,
-                    "heading": 0.2071469006235064,
-                    "angularVelocity": -0.6066137198463124,
-                    "velocityX": 0.4253858037038331,
-                    "velocityY": 0.3089454019040213,
-                    "timestamp": 6.96334458582397
-                },
-                {
-                    "x": 7.57923356269503,
-                    "y": 7.3883979284340455,
-                    "heading": 0.15090824268085423,
-                    "angularVelocity": -0.5326702370042972,
-                    "velocityX": 0.38332870100058564,
-                    "velocityY": 0.28148540800086536,
-                    "timestamp": 7.068923336083685
-                },
-                {
-                    "x": 7.612613012416259,
-                    "y": 7.4137927319874946,
-                    "heading": 0.0952672119425909,
-                    "angularVelocity": -0.5270097494122625,
-                    "velocityX": 0.316156891737545,
-                    "velocityY": 0.2405294965409898,
-                    "timestamp": 7.1745020863434
-                },
-                {
-                    "x": 7.641161660602441,
-                    "y": 7.4358681154587,
-                    "heading": 0.04946855188730579,
-                    "angularVelocity": -0.43378672242570576,
-                    "velocityX": 0.27040145789287734,
-                    "velocityY": 0.20908926766306235,
-                    "timestamp": 7.280080836603116
-                },
-                {
-                    "x": 7.664737893553612,
-                    "y": 7.454419635462092,
-                    "heading": 0.014221249681079469,
-                    "angularVelocity": -0.3338484505242331,
-                    "velocityX": 0.22330471529975876,
-                    "velocityY": 0.17571263220528385,
-                    "timestamp": 7.385659586862831
-                },
-                {
-                    "x": 7.679999999654181,
-                    "y": 7.467000000250119,
-                    "heading": 5.571589197087409e-11,
-                    "angularVelocity": -0.13469802934148392,
-                    "velocityX": 0.1445566071683118,
-                    "velocityY": 0.11915622105321995,
-                    "timestamp": 7.491238337122546
-                },
-                {
-                    "x": 7.6799999998383255,
-                    "y": 7.467000000125839,
-                    "heading": 2.732434354976469e-11,
-                    "angularVelocity": -1.9015024944340445e-11,
-                    "velocityX": 1.5544110263140067e-10,
-                    "velocityY": -2.911726567326558e-11,
-                    "timestamp": 7.596817087382261
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        1,
-                        2
-                    ],
-                    "type": "StraightLine"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "W3 To C2": {
-            "waypoints": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 12
-                },
-                {
-                    "x": 2.1656765937805176,
-                    "y": 6.261821746826172,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 30
-                },
-                {
-                    "x": 5.493278503417969,
-                    "y": 6.720412731170654,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 27
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": 0,
-                    "velocityX": -2.538537990246625e-31,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.2837844841867163,
-                    "y": 4.387687502531162,
-                    "heading": -0.3845108824411683,
-                    "angularVelocity": 0.0884354987329395,
-                    "velocityX": -0.26124913754506907,
-                    "velocityY": 0.4460749963360829,
-                    "timestamp": 0.06206916495779045
-                },
-                {
-                    "x": 2.252431932443338,
-                    "y": 4.44366896754607,
-                    "heading": -0.37369458929412197,
-                    "angularVelocity": 0.17426194076711193,
-                    "velocityX": -0.5051228216917889,
-                    "velocityY": 0.9019207049574874,
-                    "timestamp": 0.1241383299155809
-                },
-                {
-                    "x": 2.2074803876744995,
-                    "y": 4.528718281766582,
-                    "heading": -0.3577885652255258,
-                    "angularVelocity": 0.2562628976859123,
-                    "velocityX": -0.724217005329779,
-                    "velocityY": 1.3702345484796434,
-                    "timestamp": 0.18620749487337135
-                },
-                {
-                    "x": 2.1512700092503536,
-                    "y": 4.643824218738039,
-                    "heading": -0.3371682293163389,
-                    "angularVelocity": 0.3322154555127859,
-                    "velocityX": -0.905608742482958,
-                    "velocityY": 1.8544785812676818,
-                    "timestamp": 0.2482766598311618
-                },
-                {
-                    "x": 2.087674884006875,
-                    "y": 4.7901724592691535,
-                    "heading": -0.3124919375355934,
-                    "angularVelocity": 0.3975612012535503,
-                    "velocityX": -1.0245848367214008,
-                    "velocityY": 2.357825187926933,
-                    "timestamp": 0.31034582478895223
-                },
-                {
-                    "x": 2.0237484850052736,
-                    "y": 4.968633105417482,
-                    "heading": -0.2850705208032402,
-                    "angularVelocity": 0.44178807225385713,
-                    "velocityX": -1.029922007895696,
-                    "velocityY": 2.8751900604698215,
-                    "timestamp": 0.3724149897467427
-                },
-                {
-                    "x": 1.9724195760606846,
-                    "y": 5.1766438961084935,
-                    "heading": -0.25765844821795814,
-                    "angularVelocity": 0.4416375281458893,
-                    "velocityX": -0.8269630980122488,
-                    "velocityY": 3.351274192787545,
-                    "timestamp": 0.43448415470453317
-                },
-                {
-                    "x": 1.9484287592243066,
-                    "y": 5.401486281993408,
-                    "heading": -0.23420864800659155,
-                    "angularVelocity": 0.3778011227958159,
-                    "velocityX": -0.38651747373332257,
-                    "velocityY": 3.6224490217904175,
-                    "timestamp": 0.49655331966232363
-                },
-                {
-                    "x": 1.9564542032821701,
-                    "y": 5.628890453578366,
-                    "heading": -0.21272720707613346,
-                    "angularVelocity": 0.34608876961188295,
-                    "velocityX": 0.12929840547219124,
-                    "velocityY": 3.663722103228105,
-                    "timestamp": 0.5586224846201141
-                },
-                {
-                    "x": 1.99619273658118,
-                    "y": 5.8512380599288125,
-                    "heading": -0.18843692284079885,
-                    "angularVelocity": 0.39134221077502945,
-                    "velocityX": 0.6402298681838031,
-                    "velocityY": 3.5822554806643043,
-                    "timestamp": 0.6206916495779046
-                },
-                {
-                    "x": 2.0663437221615903,
-                    "y": 6.063242956223716,
-                    "heading": -0.16364621155748288,
-                    "angularVelocity": 0.3994046206362532,
-                    "velocityX": 1.130206691653697,
-                    "velocityY": 3.4156234651942974,
-                    "timestamp": 0.682760814535695
-                },
-                {
-                    "x": 2.1656765937805176,
-                    "y": 6.261821746826172,
-                    "heading": -0.13977158803318993,
-                    "angularVelocity": 0.38464547638239754,
-                    "velocityX": 1.6003577893593972,
-                    "velocityY": 3.1993146796407252,
-                    "timestamp": 0.7448299794934855
-                },
-                {
-                    "x": 2.225814155156259,
-                    "y": 6.362141570414172,
-                    "heading": -0.12770962458788607,
-                    "angularVelocity": 0.3689752694416071,
-                    "velocityX": 1.8396070434406133,
-                    "velocityY": 3.068781803712599,
-                    "timestamp": 0.7775204182423698
-                },
-                {
-                    "x": 2.2936460979223643,
-                    "y": 6.4579721340232545,
-                    "heading": -0.11638786741998439,
-                    "angularVelocity": 0.3463323712222401,
-                    "velocityX": 2.074978047465827,
-                    "velocityY": 2.9314554125487073,
-                    "timestamp": 0.8102108569912542
-                },
-                {
-                    "x": 2.3690076803030626,
-                    "y": 6.549046337769426,
-                    "heading": -0.10608509188235166,
-                    "angularVelocity": 0.31516173940473013,
-                    "velocityX": 2.3053096031972573,
-                    "velocityY": 2.7859584401928488,
-                    "timestamp": 0.8429012957401385
-                },
-                {
-                    "x": 2.4516786592945876,
-                    "y": 6.635038598364521,
-                    "heading": -0.09714822011858378,
-                    "angularVelocity": 0.27337876474752265,
-                    "velocityX": 2.528903928955548,
-                    "velocityY": 2.6305018802439832,
-                    "timestamp": 0.8755917344890228
-                },
-                {
-                    "x": 2.5413546021547084,
-                    "y": 6.715546857152131,
-                    "heading": -0.09001694608551258,
-                    "angularVelocity": 0.21814555896345397,
-                    "velocityX": 2.7431856619836545,
-                    "velocityY": 2.462746352407108,
-                    "timestamp": 0.9082821732379072
-                },
-                {
-                    "x": 2.6375981860796323,
-                    "y": 6.79006985383775,
-                    "heading": -0.08525828181365991,
-                    "angularVelocity": 0.14556746421386957,
-                    "velocityX": 2.9440896974181805,
-                    "velocityY": 2.2796572801553694,
-                    "timestamp": 0.9409726119867915
-                },
-                {
-                    "x": 2.73984178672467,
-                    "y": 6.858044565031264,
-                    "heading": -0.08306946201564708,
-                    "angularVelocity": 0.06695596274838787,
-                    "velocityX": 3.1276301131997113,
-                    "velocityY": 2.079345331378129,
-                    "timestamp": 0.9736630507356758
-                },
-                {
-                    "x": 2.847369129600613,
-                    "y": 6.918863246924361,
-                    "heading": -0.08274471887343776,
-                    "angularVelocity": 0.009933887583435854,
-                    "velocityX": 3.2892597037781757,
-                    "velocityY": 1.8604425091962247,
-                    "timestamp": 1.0063534894845603
-                },
-                {
-                    "x": 2.9591159211859868,
-                    "y": 6.971834136920771,
-                    "heading": -0.08274467756065068,
-                    "angularVelocity": 0.0000012637574442965605,
-                    "velocityX": 3.418332572524967,
-                    "velocityY": 1.6203786802362972,
-                    "timestamp": 1.0390439282334447
-                },
-                {
-                    "x": 3.0743869446602656,
-                    "y": 7.0166209198254075,
-                    "heading": -0.0827446619031275,
-                    "angularVelocity": 4.789633846658565e-7,
-                    "velocityX": 3.526138769803835,
-                    "velocityY": 1.3700269748141811,
-                    "timestamp": 1.0717343669823292
-                },
-                {
-                    "x": 3.192583723464696,
-                    "y": 7.052991069898469,
-                    "heading": -0.08274464937093684,
-                    "angularVelocity": 3.833595125790563e-7,
-                    "velocityX": 3.61563757869459,
-                    "velocityY": 1.1125623107197633,
-                    "timestamp": 1.1044248057312136
-                },
-                {
-                    "x": 3.313092599420772,
-                    "y": 7.080755764680444,
-                    "heading": -0.08274463888121661,
-                    "angularVelocity": 3.208803751652956e-7,
-                    "velocityX": 3.6863645936910636,
-                    "velocityY": 0.8493215706001247,
-                    "timestamp": 1.137115244480098
-                },
-                {
-                    "x": 3.4352879115666495,
-                    "y": 7.0997708632875725,
-                    "heading": -0.08274462977736693,
-                    "angularVelocity": 2.7848661636087506e-7,
-                    "velocityX": 3.7379526498424447,
-                    "velocityY": 0.5816715631449295,
-                    "timestamp": 1.1698056832289825
-                },
-                {
-                    "x": 3.5585352449602814,
-                    "y": 7.109937657437644,
-                    "heading": -0.08274462163132143,
-                    "angularVelocity": 2.491874023135916e-7,
-                    "velocityX": 3.770133963033834,
-                    "velocityY": 0.31100207091115845,
-                    "timestamp": 1.202496121977867
-                },
-                {
-                    "x": 3.6821947259931562,
-                    "y": 7.1112033907952945,
-                    "heading": -0.08274461414532981,
-                    "angularVelocity": 2.2899636479565772e-7,
-                    "velocityX": 3.782741552745206,
-                    "velocityY": 0.03871876321990418,
-                    "timestamp": 1.2351865607267514
-                },
-                {
-                    "x": 3.8056243495254822,
-                    "y": 7.103561554173401,
-                    "heading": -0.08274460709859606,
-                    "angularVelocity": 2.1555947310703947e-7,
-                    "velocityX": 3.775710215468651,
-                    "velocityY": -0.23376366040158306,
-                    "timestamp": 1.2678769994756358
-                },
-                {
-                    "x": 3.9281833350664277,
-                    "y": 7.087052015079957,
-                    "heading": -0.08274460031581693,
-                    "angularVelocity": 2.0748510569540933e-7,
-                    "velocityX": 3.7490774131963343,
-                    "velocityY": -0.5050265375893709,
-                    "timestamp": 1.3005674382245203
-                },
-                {
-                    "x": 4.0492357154991785,
-                    "y": 7.0617618768818975,
-                    "heading": -0.08274459364684544,
-                    "angularVelocity": 2.0400373145008536e-7,
-                    "velocityX": 3.7029903869628296,
-                    "velocityY": -0.7736249241717706,
-                    "timestamp": 1.3332578769734047
-                },
-                {
-                    "x": 4.169706806774688,
-                    "y": 7.033833646086136,
-                    "heading": -0.0827445869919534,
-                    "angularVelocity": 2.0357304165195936e-7,
-                    "velocityX": 3.6852087609158533,
-                    "velocityY": -0.8543241346506245,
-                    "timestamp": 1.3659483157222891
-                },
-                {
-                    "x": 4.290177832890272,
-                    "y": 7.00590513421804,
-                    "heading": -0.08274458033705716,
-                    "angularVelocity": 2.0357317018224517e-7,
-                    "velocityX": 3.6852067676748597,
-                    "velocityY": -0.8543327326486478,
-                    "timestamp": 1.3986387544711736
-                },
-                {
-                    "x": 4.410648856777724,
-                    "y": 6.977976612738755,
-                    "heading": -0.08274457368215653,
-                    "angularVelocity": 2.0357330400976238e-7,
-                    "velocityX": 3.685206699516314,
-                    "velocityY": -0.8543330266547546,
-                    "timestamp": 1.431329193220058
-                },
-                {
-                    "x": 4.531119878449975,
-                    "y": 6.950048081704109,
-                    "heading": -0.08274456702725173,
-                    "angularVelocity": 2.0357343141081232e-7,
-                    "velocityX": 3.6852066317533563,
-                    "velocityY": -0.8543333189530982,
-                    "timestamp": 1.4640196319689425
-                },
-                {
-                    "x": 4.651590897922017,
-                    "y": 6.922119541178764,
-                    "heading": -0.082744560372343,
-                    "angularVelocity": 2.0357355233562972e-7,
-                    "velocityX": 3.6852065644490066,
-                    "velocityY": -0.8543336092734171,
-                    "timestamp": 1.496710070717827
-                },
-                {
-                    "x": 4.772061915216332,
-                    "y": 6.894190991259702,
-                    "heading": -0.08274455371743049,
-                    "angularVelocity": 2.0357366776279015e-7,
-                    "velocityX": 3.68520649783238,
-                    "velocityY": -0.8543338966271132,
-                    "timestamp": 1.5294005094667114
-                },
-                {
-                    "x": 4.892532930362486,
-                    "y": 6.866262432074455,
-                    "heading": -0.08274454706251447,
-                    "angularVelocity": 2.0357377502591166e-7,
-                    "velocityX": 3.6852064321201583,
-                    "velocityY": -0.8543341800795031,
-                    "timestamp": 1.5620909482155958
-                },
-                {
-                    "x": 5.0130039433965745,
-                    "y": 6.838333863778735,
-                    "heading": -0.08274454040759514,
-                    "angularVelocity": 2.0357387593869802e-7,
-                    "velocityX": 3.685206367512119,
-                    "velocityY": -0.854334458768776,
-                    "timestamp": 1.5947813869644802
-                },
-                {
-                    "x": 5.133474954360567,
-                    "y": 6.810405286553609,
-                    "heading": -0.08274453375267282,
-                    "angularVelocity": 2.0357396812707888e-7,
-                    "velocityX": 3.6852063041879153,
-                    "velocityY": -0.8543347319191732,
-                    "timestamp": 1.6274718257133647
-                },
-                {
-                    "x": 5.253945963272805,
-                    "y": 6.782476700477742,
-                    "heading": -0.08274452709774775,
-                    "angularVelocity": 2.035740515232947e-7,
-                    "velocityX": 3.6852062414248348,
-                    "velocityY": -0.8543350026628553,
-                    "timestamp": 1.6601622644622491
-                },
-                {
-                    "x": 5.374416652936201,
-                    "y": 6.754546737348104,
-                    "heading": -0.08274452044281651,
-                    "angularVelocity": 2.0357424100563973e-7,
-                    "velocityX": 3.685196475607398,
-                    "velocityY": -0.8543771267177642,
-                    "timestamp": 1.6928527032111336
-                },
-                {
-                    "x": 5.493278503417969,
-                    "y": 6.720412731170654,
-                    "heading": -0.08274451374626977,
-                    "angularVelocity": 2.0484725764732438e-7,
-                    "velocityX": 3.6359821106976624,
-                    "velocityY": -1.0441587046150123,
-                    "timestamp": 1.725543141960018
-                },
-                {
-                    "x": 5.6078450645507765,
-                    "y": 6.6784969077855525,
-                    "heading": -0.0827445073182203,
-                    "angularVelocity": 1.9932954157588389e-7,
-                    "velocityX": 3.5526329088991675,
-                    "velocityY": -1.2997818219306485,
-                    "timestamp": 1.7577914952435287
-                },
-                {
-                    "x": 5.720102544926552,
-                    "y": 6.630740323854381,
-                    "heading": -0.08274450093221485,
-                    "angularVelocity": 1.980257845223315e-7,
-                    "velocityX": 3.481029849458213,
-                    "velocityY": -1.4808999241394636,
-                    "timestamp": 1.7900398485270395
-                },
-                {
-                    "x": 5.832359462510262,
-                    "y": 6.582982417035901,
-                    "heading": -0.08274449454621227,
-                    "angularVelocity": 1.9802569455219195e-7,
-                    "velocityX": 3.481012397650423,
-                    "velocityY": -1.480940945995501,
-                    "timestamp": 1.8222882018105502
-                },
-                {
-                    "x": 5.944616376948871,
-                    "y": 6.535224502824747,
-                    "heading": -0.08274448816020867,
-                    "angularVelocity": 1.9802572730548282e-7,
-                    "velocityX": 3.4810123001228868,
-                    "velocityY": -1.4809411752374333,
-                    "timestamp": 1.854536555094061
-                },
-                {
-                    "x": 6.0568732883758765,
-                    "y": 6.487466581534697,
-                    "heading": -0.0827444817742042,
-                    "angularVelocity": 1.9802575385283685e-7,
-                    "velocityX": 3.4810122067350586,
-                    "velocityY": -1.4809413947492354,
-                    "timestamp": 1.8867849083775716
-                },
-                {
-                    "x": 6.169130196876316,
-                    "y": 6.439708653365641,
-                    "heading": -0.08274447538819907,
-                    "angularVelocity": 1.980257744166014e-7,
-                    "velocityX": 3.4810121159842042,
-                    "velocityY": -1.4809416080626625,
-                    "timestamp": 1.9190332616610823
-                },
-                {
-                    "x": 6.281387102537832,
-                    "y": 6.391950718523587,
-                    "heading": -0.08274446900219351,
-                    "angularVelocity": 1.9802578764982062e-7,
-                    "velocityX": 3.481012027951073,
-                    "velocityY": -1.4809418149879348,
-                    "timestamp": 1.951281614944593
-                },
-                {
-                    "x": 6.393644005449787,
-                    "y": 6.344192777218584,
-                    "heading": -0.08274446261618765,
-                    "angularVelocity": 1.9802579747327125e-7,
-                    "velocityX": 3.481011942689016,
-                    "velocityY": -1.4809420153996509,
-                    "timestamp": 1.9835299682281038
-                },
-                {
-                    "x": 6.505900905702341,
-                    "y": 6.296434829662559,
-                    "heading": -0.08274445623018166,
-                    "angularVelocity": 1.9802580083930334e-7,
-                    "velocityX": 3.4810118602227385,
-                    "velocityY": -1.4809422092397408,
-                    "timestamp": 2.0157783215116143
-                },
-                {
-                    "x": 6.618157803385852,
-                    "y": 6.2486768760679015,
-                    "heading": -0.08274444984417571,
-                    "angularVelocity": 1.9802579953185327e-7,
-                    "velocityX": 3.4810117805584304,
-                    "velocityY": -1.4809423964936643,
-                    "timestamp": 2.048026674795125
-                },
-                {
-                    "x": 6.730414698590335,
-                    "y": 6.200918916646197,
-                    "heading": -0.08274444345816992,
-                    "angularVelocity": 1.9802579502845229e-7,
-                    "velocityX": 3.4810117036854167,
-                    "velocityY": -1.4809425771865148,
-                    "timestamp": 2.0802750280786353
-                },
-                {
-                    "x": 6.842671592155369,
-                    "y": 6.153160953370972,
-                    "heading": -0.08274443707212797,
-                    "angularVelocity": 1.980269160510039e-7,
-                    "velocityX": 3.4810116528472412,
-                    "velocityY": -1.4809426966816255,
-                    "timestamp": 2.112523381362146
-                },
-                {
-                    "x": 6.954235049253699,
-                    "y": 6.105720053055757,
-                    "heading": -0.08066018407058251,
-                    "angularVelocity": 0.06463130018523208,
-                    "velocityX": 3.459508648938503,
-                    "velocityY": -1.4711107850419303,
-                    "timestamp": 2.1447717346456563
-                },
-                {
-                    "x": 7.05792262192444,
-                    "y": 6.061617369901677,
-                    "heading": -0.07347223812764758,
-                    "angularVelocity": 0.2228934258982459,
-                    "velocityX": 3.215282707900597,
-                    "velocityY": -1.3675948897716332,
-                    "timestamp": 2.177020087929167
-                },
-                {
-                    "x": 7.153639251658588,
-                    "y": 6.020903567478063,
-                    "heading": -0.06480991744029473,
-                    "angularVelocity": 0.2686128067130366,
-                    "velocityX": 2.9681090656833784,
-                    "velocityY": -1.2625079508922343,
-                    "timestamp": 2.2092684412126773
-                },
-                {
-                    "x": 7.241376863236811,
-                    "y": 5.983582900712933,
-                    "heading": -0.05573995757660787,
-                    "angularVelocity": 0.28125342661540725,
-                    "velocityX": 2.7206850162822076,
-                    "velocityY": -1.1572890695232254,
-                    "timestamp": 2.241516794496188
-                },
-                {
-                    "x": 7.321134939051253,
-                    "y": 5.949655957408732,
-                    "heading": -0.04677486060395727,
-                    "angularVelocity": 0.2780016980662186,
-                    "velocityX": 2.4732449162054873,
-                    "velocityY": -1.0520519607910965,
-                    "timestamp": 2.2737651477796983
-                },
-                {
-                    "x": 7.392914197093691,
-                    "y": 5.919122647933878,
-                    "heading": -0.03821600737741098,
-                    "angularVelocity": 0.26540434952773495,
-                    "velocityX": 2.2258270805765674,
-                    "velocityY": -0.946817631474723,
-                    "timestamp": 2.306013501063209
-                },
-                {
-                    "x": 7.456715523277294,
-                    "y": 5.891982736437705,
-                    "heading": -0.030261865024148932,
-                    "angularVelocity": 0.24665266729552143,
-                    "velocityX": 1.9784367165261438,
-                    "velocityY": -0.8415906157307577,
-                    "timestamp": 2.3382618543467193
-                },
-                {
-                    "x": 7.512539743308544,
-                    "y": 5.868235970303718,
-                    "heading": -0.023053012750234723,
-                    "angularVelocity": 0.2235417173255794,
-                    "velocityX": 1.7310719570848203,
-                    "velocityY": -0.736371433456386,
-                    "timestamp": 2.37051020763023
-                },
-                {
-                    "x": 7.560387579465668,
-                    "y": 5.847882114762634,
-                    "heading": -0.016694240391467878,
-                    "angularVelocity": 0.19718130420078028,
-                    "velocityX": 1.4837295950113008,
-                    "velocityY": -0.6311595312214304,
-                    "timestamp": 2.4027585609137403
-                },
-                {
-                    "x": 7.600259653459209,
-                    "y": 5.830920960662069,
-                    "heading": -0.011266668966831511,
-                    "angularVelocity": 0.1683053821980557,
-                    "velocityX": 1.2364065117684333,
-                    "velocityY": -0.5259541146628961,
-                    "timestamp": 2.435006914197251
-                },
-                {
-                    "x": 7.632156499892169,
-                    "y": 5.817352324088122,
-                    "heading": -0.006834955017927562,
-                    "angularVelocity": 0.1374245038171387,
-                    "velocityX": 0.9891000062092736,
-                    "velocityY": -0.4207544011521409,
-                    "timestamp": 2.4672552674807613
-                },
-                {
-                    "x": 7.656078580521334,
-                    "y": 5.807176043662066,
-                    "heading": -0.0034518424131983775,
-                    "angularVelocity": 0.10490807313441347,
-                    "velocityX": 0.7418078194211917,
-                    "velocityY": -0.31555969188850075,
-                    "timestamp": 2.499503620764272
-                },
-                {
-                    "x": 7.672026296758129,
-                    "y": 5.800391977434502,
-                    "heading": -0.0011611797809329624,
-                    "angularVelocity": 0.07103192563441285,
-                    "velocityX": 0.49452808013452626,
-                    "velocityY": -0.21036938438134845,
-                    "timestamp": 2.5317519740477823
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 1.9934013230001416e-31,
-                    "angularVelocity": 0.036007413176266045,
-                    "velocityX": 0.2472592374491084,
-                    "velocityY": -0.10518296561320875,
-                    "timestamp": 2.564000327331293
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 1.0109342582388432e-32,
-                    "angularVelocity": -5.855934233748412e-30,
-                    "velocityX": 8.513095472636782e-32,
-                    "velocityY": -2.1397535642954256e-34,
-                    "timestamp": 2.5962486806148033
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        1,
-                        2
-                    ],
-                    "type": "StraightLine"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "W3 To C3": {
-            "waypoints": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 12
-                },
-                {
-                    "x": 2.477290153503418,
-                    "y": 6.3241448402404785,
-                    "heading": 0.002,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 9
-                },
-                {
-                    "x": 3.9730355739593506,
-                    "y": 5.887885093688965,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 11
-                },
-                {
-                    "x": 5.390789031982422,
-                    "y": 4.081310272216797,
-                    "heading": 0.008,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 11
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": 1.5561411635921053e-32,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.2879322251049214,
-                    "y": 4.397976336389526,
-                    "heading": -0.38546801638598305,
-                    "angularVelocity": 0.06554072441421986,
-                    "velocityX": -0.17452197008732642,
-                    "velocityY": 0.5492068837058847,
-                    "timestamp": 0.06914759722835051
-                },
-                {
-                    "x": 2.264556974735031,
-                    "y": 4.474163552813777,
-                    "heading": -0.3765424472118302,
-                    "angularVelocity": 0.12907996129426796,
-                    "velocityX": -0.3380486279621587,
-                    "velocityY": 1.1018056950361832,
-                    "timestamp": 0.13829519445670102
-                },
-                {
-                    "x": 2.231004605798313,
-                    "y": 4.588879932488501,
-                    "heading": -0.3634307413762601,
-                    "angularVelocity": 0.18961910985573333,
-                    "velocityX": -0.48522826940697783,
-                    "velocityY": 1.6590074604604372,
-                    "timestamp": 0.20744279168505153
-                },
-                {
-                    "x": 2.1891253320707884,
-                    "y": 4.7425700174397605,
-                    "heading": -0.3464768905998804,
-                    "angularVelocity": 0.24518351259275634,
-                    "velocityX": -0.6056504550588213,
-                    "velocityY": 2.2226381119759067,
-                    "timestamp": 0.27659038891340204
-                },
-                {
-                    "x": 2.142455487111455,
-                    "y": 4.9358291258163405,
-                    "heading": -0.3263533789303141,
-                    "angularVelocity": 0.29102257309767376,
-                    "velocityX": -0.6749308266671239,
-                    "velocityY": 2.7948781465017833,
-                    "timestamp": 0.34573798614175255
-                },
-                {
-                    "x": 2.099906819940071,
-                    "y": 5.168740583772433,
-                    "heading": -0.30483662973794956,
-                    "angularVelocity": 0.3111713212639373,
-                    "velocityX": -0.6153311015404023,
-                    "velocityY": 3.3683232287448175,
-                    "timestamp": 0.41488558337010306
-                },
-                {
-                    "x": 2.090216091849313,
-                    "y": 5.4241817814008675,
-                    "heading": -0.28862249413017305,
-                    "angularVelocity": 0.23448588609832868,
-                    "velocityX": -0.14014555066512774,
-                    "velocityY": 3.69414423446863,
-                    "timestamp": 0.48403318059845357
-                },
-                {
-                    "x": 2.1169158362249996,
-                    "y": 5.665111810235099,
-                    "heading": -0.24564536248803095,
-                    "angularVelocity": 0.6215274769324497,
-                    "velocityX": 0.3861268568382121,
-                    "velocityY": 3.484286345315426,
-                    "timestamp": 0.5531807778268041
-                },
-                {
-                    "x": 2.1721169779622786,
-                    "y": 5.878339660700231,
-                    "heading": -0.1915552026069021,
-                    "angularVelocity": 0.7822420741739773,
-                    "velocityX": 0.7983088921367857,
-                    "velocityY": 3.08366247001803,
-                    "timestamp": 0.6223283750551546
-                },
-                {
-                    "x": 2.2518565055578685,
-                    "y": 6.060184802876031,
-                    "heading": -0.1318933186992544,
-                    "angularVelocity": 0.8628193357100991,
-                    "velocityX": 1.1531785744091572,
-                    "velocityY": 2.629811439074749,
-                    "timestamp": 0.6914759722835051
-                },
-                {
-                    "x": 2.3540132373645406,
-                    "y": 6.209070989598032,
-                    "heading": -0.06941508964740774,
-                    "angularVelocity": 0.9035488079854361,
-                    "velocityX": 1.4773721127192665,
-                    "velocityY": 2.153165007746782,
-                    "timestamp": 0.7606235695118556
-                },
-                {
-                    "x": 2.477290153503418,
-                    "y": 6.3241448402404785,
-                    "heading": -0.005748673882586236,
-                    "angularVelocity": 0.9207321485592678,
-                    "velocityX": 1.782808384965753,
-                    "velocityY": 1.6641771407079455,
-                    "timestamp": 0.8297711667402061
-                },
-                {
-                    "x": 2.6074231512941006,
-                    "y": 6.400809151237731,
-                    "heading": 0.05275689385662717,
-                    "angularVelocity": 0.9223034785287629,
-                    "velocityX": 2.051464863433381,
-                    "velocityY": 1.20856464509814,
-                    "timestamp": 0.8932053498501396
-                },
-                {
-                    "x": 2.7533092580755354,
-                    "y": 6.447850328813001,
-                    "heading": 0.11062964933050726,
-                    "angularVelocity": 0.9123275911695883,
-                    "velocityX": 2.2998027188043615,
-                    "velocityY": 0.7415745780760178,
-                    "timestamp": 0.956639532960073
-                },
-                {
-                    "x": 2.9132889651123484,
-                    "y": 6.4644464131499,
-                    "heading": 0.16695291665745207,
-                    "angularVelocity": 0.8879008850767368,
-                    "velocityX": 2.5219794627047807,
-                    "velocityY": 0.26162683151755056,
-                    "timestamp": 1.0200737160700064
-                },
-                {
-                    "x": 3.0851729747755345,
-                    "y": 6.449682823375658,
-                    "heading": 0.22055798508605634,
-                    "angularVelocity": 0.8450501890441832,
-                    "velocityX": 2.7096433064356162,
-                    "velocityY": -0.23273870727739393,
-                    "timestamp": 1.0835078991799398
-                },
-                {
-                    "x": 3.266000929743579,
-                    "y": 6.40260327654133,
-                    "heading": 0.269929363572885,
-                    "angularVelocity": 0.7783087298743283,
-                    "velocityX": 2.8506389789001907,
-                    "velocityY": -0.7421794453112333,
-                    "timestamp": 1.1469420822898733
-                },
-                {
-                    "x": 3.4516899170753974,
-                    "y": 6.322371527535034,
-                    "heading": 0.31309009761375267,
-                    "angularVelocity": 0.6804018263443473,
-                    "velocityX": 2.9272700967841274,
-                    "velocityY": -1.2648030615800407,
-                    "timestamp": 1.2103762653998067
-                },
-                {
-                    "x": 3.636598494450249,
-                    "y": 6.208685958737944,
-                    "heading": 0.3475353077130109,
-                    "angularVelocity": 0.5430070730006203,
-                    "velocityX": 2.9149674246500377,
-                    "velocityY": -1.7921814899081216,
-                    "timestamp": 1.2738104485097401
-                },
-                {
-                    "x": 3.8132641934440024,
-                    "y": 6.062639398740876,
-                    "heading": 0.3704029865204341,
-                    "angularVelocity": 0.36049457383033545,
-                    "velocityX": 2.785023631307647,
-                    "velocityY": -2.3023321628339066,
-                    "timestamp": 1.3372446316196736
-                },
-                {
-                    "x": 3.9730355739593506,
-                    "y": 5.887885093688965,
-                    "heading": 0.37894290950178183,
-                    "angularVelocity": 0.1346265146219495,
-                    "velocityX": 2.5186953261297353,
-                    "velocityY": -2.754891707981806,
-                    "timestamp": 1.400678814729607
-                },
-                {
-                    "x": 4.101195684299111,
-                    "y": 5.704704428790184,
-                    "heading": 0.3789451050812407,
-                    "angularVelocity": 0.000037151619885842266,
-                    "velocityX": 2.1686126132704535,
-                    "velocityY": -3.099622022424022,
-                    "timestamp": 1.459776557478272
-                },
-                {
-                    "x": 4.204458426061581,
-                    "y": 5.506418480888323,
-                    "heading": 0.3789451145933903,
-                    "angularVelocity": 1.6095622464276883e-7,
-                    "velocityX": 1.7473212505255857,
-                    "velocityY": -3.3552203295641,
-                    "timestamp": 1.518874300226937
-                },
-                {
-                    "x": 4.281071316930325,
-                    "y": 5.296392335982766,
-                    "heading": 0.3789451203460425,
-                    "angularVelocity": 9.73413180945881e-8,
-                    "velocityX": 1.2963759241186865,
-                    "velocityY": -3.5538776125279528,
-                    "timestamp": 1.577972042975602
-                },
-                {
-                    "x": 4.344371609432443,
-                    "y": 5.081977806335691,
-                    "heading": 0.37894512554853116,
-                    "angularVelocity": 8.803193422611526e-8,
-                    "velocityX": 1.0711118489099893,
-                    "velocityY": -3.628133997587826,
-                    "timestamp": 1.6370697857242669
-                },
-                {
-                    "x": 4.4350051048376455,
-                    "y": 4.877610290730759,
-                    "heading": 0.3789451268821095,
-                    "angularVelocity": 2.2565639514634894e-8,
-                    "velocityX": 1.5336202567072863,
-                    "velocityY": -3.4581272667918515,
-                    "timestamp": 1.6961675284729318
-                },
-                {
-                    "x": 4.551081441257298,
-                    "y": 4.6873967879425225,
-                    "heading": 0.3767623699353526,
-                    "angularVelocity": -0.03693469235752831,
-                    "velocityX": 1.964141624038618,
-                    "velocityY": -3.218625516664622,
-                    "timestamp": 1.7552652712215968
-                },
-                {
-                    "x": 4.687723026941447,
-                    "y": 4.517482449028109,
-                    "heading": 0.36165339974597627,
-                    "angularVelocity": -0.25566069848849177,
-                    "velocityX": 2.312128675793254,
-                    "velocityY": -2.875140927749109,
-                    "timestamp": 1.8143630139702618
-                },
-                {
-                    "x": 4.8416817092622,
-                    "y": 4.370738170800582,
-                    "heading": 0.3345492154544829,
-                    "angularVelocity": -0.45863315638965824,
-                    "velocityX": 2.6051533469768047,
-                    "velocityY": -2.483077549196378,
-                    "timestamp": 1.8734607567189268
-                },
-                {
-                    "x": 5.0108564738059584,
-                    "y": 4.248670932935222,
-                    "heading": 0.2976430739087661,
-                    "angularVelocity": -0.6244932518551631,
-                    "velocityX": 2.8626265010319796,
-                    "velocityY": -2.065514386638646,
-                    "timestamp": 1.9325584994675917
-                },
-                {
-                    "x": 5.194237326297875,
-                    "y": 4.152011519418658,
-                    "heading": 0.2577973678603408,
-                    "angularVelocity": -0.6742339757109184,
-                    "velocityX": 3.103009420708511,
-                    "velocityY": -1.635585540509506,
-                    "timestamp": 1.9916562422162567
-                },
-                {
-                    "x": 5.390789031982422,
-                    "y": 4.081310272216797,
-                    "heading": 0.22104423660220585,
-                    "angularVelocity": -0.6219041464028032,
-                    "velocityX": 3.3258750088027327,
-                    "velocityY": -1.1963442919045442,
-                    "timestamp": 2.050753984964922
-                },
-                {
-                    "x": 5.67793189235796,
-                    "y": 4.03407988691648,
-                    "heading": 0.19119773345256094,
-                    "angularVelocity": -0.37371337139653454,
-                    "velocityX": 3.5953667967399574,
-                    "velocityY": -0.5913800499300379,
-                    "timestamp": 2.130618676555184
-                },
-                {
-                    "x": 5.980039011502812,
-                    "y": 4.037197033313024,
-                    "heading": 0.19119714274274896,
-                    "angularVelocity": -0.000007396382559589573,
-                    "velocityX": 3.782736940809626,
-                    "velocityY": 0.039030344129324,
-                    "timestamp": 2.210483368145446
-                },
-                {
-                    "x": 6.281741681649569,
-                    "y": 4.053135447217484,
-                    "heading": 0.19119713453456852,
-                    "angularVelocity": -1.0277608664279704e-7,
-                    "velocityX": 3.777672763010384,
-                    "velocityY": 0.1995677136803443,
-                    "timestamp": 2.290348059735708
-                },
-                {
-                    "x": 6.583444349801777,
-                    "y": 4.069073898877065,
-                    "heading": 0.19119712632638652,
-                    "angularVelocity": -1.0277610566434499e-7,
-                    "velocityX": 3.777672738036278,
-                    "velocityY": 0.1995681864189316,
-                    "timestamp": 2.3702127513259703
-                },
-                {
-                    "x": 6.885147017932413,
-                    "y": 4.08501235094329,
-                    "heading": 0.1911971181180728,
-                    "angularVelocity": -1.0277775515776054e-7,
-                    "velocityX": 3.7776727377661725,
-                    "velocityY": 0.1995681915106077,
-                    "timestamp": 2.4500774429162324
-                },
-                {
-                    "x": 7.150097958068984,
-                    "y": 4.099008563262217,
-                    "heading": 0.12746367267042205,
-                    "angularVelocity": -0.7980177995885095,
-                    "velocityX": 3.317497818634029,
-                    "velocityY": 0.17524906238576554,
-                    "timestamp": 2.5299421345064945
-                },
-                {
-                    "x": 7.362058748903403,
-                    "y": 4.110205268667911,
-                    "heading": 0.07647732188798662,
-                    "angularVelocity": -0.6384091613855594,
-                    "velocityX": 2.6539987397918514,
-                    "velocityY": 0.14019593869007996,
-                    "timestamp": 2.6098068260967566
-                },
-                {
-                    "x": 7.521029367100823,
-                    "y": 4.118602666583934,
-                    "heading": 0.03823840686026882,
-                    "angularVelocity": -0.47879625234031903,
-                    "velocityX": 1.9904993687699384,
-                    "velocityY": 0.10514531201226385,
-                    "timestamp": 2.6896715176870187
-                },
-                {
-                    "x": 7.627009789128585,
-                    "y": 4.124200888756926,
-                    "heading": 0.012746172901683662,
-                    "angularVelocity": -0.31919279284744,
-                    "velocityX": 1.3269997030913787,
-                    "velocityY": 0.07009633495752653,
-                    "timestamp": 2.769536209277281
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 1.6188506035771725e-33,
-                    "angularVelocity": -0.15959709663791943,
-                    "velocityX": 0.6634998497618533,
-                    "velocityY": 0.03504816943929136,
-                    "timestamp": 2.849400900867543
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "angularVelocity": -1.4796214296723179e-31,
-                    "velocityX": -2.3067017155365004e-33,
-                    "velocityY": 0,
-                    "timestamp": 2.929265592457805
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "W3 To C4": {
-            "waypoints": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 27
-                },
-                {
-                    "x": 2.6104869842529297,
-                    "y": 2.6966493129730225,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 14
-                },
-                {
-                    "x": 5.646764755249023,
-                    "y": 1.5066063404083252,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 27
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.298402468716654,
-                    "y": 4.3526843009931255,
-                    "heading": -0.37533408415570846,
-                    "angularVelocity": 0.4620245693037669,
-                    "velocityX": -0.050327487964169394,
-                    "velocityY": -0.2304685720750862,
-                    "timestamp": 0.03174271850172795
-                },
-                {
-                    "x": 2.2952744131725273,
-                    "y": 4.338010072410149,
-                    "heading": -0.34621912169362346,
-                    "angularVelocity": 0.9172170449263883,
-                    "velocityX": -0.09854403440450425,
-                    "velocityY": -0.462286447903903,
-                    "timestamp": 0.0634854370034559
-                },
-                {
-                    "x": 2.2907034035575107,
-                    "y": 4.3159283160610205,
-                    "heading": -0.3028943022400045,
-                    "angularVelocity": 1.364874260887912,
-                    "velocityX": -0.14400183194038138,
-                    "velocityY": -0.6956479278208441,
-                    "timestamp": 0.09522815550518385
-                },
-                {
-                    "x": 2.2847999846220546,
-                    "y": 4.286378308280002,
-                    "heading": -0.24568561151458213,
-                    "angularVelocity": 1.8022618548662834,
-                    "velocityX": -0.1859771063759033,
-                    "velocityY": -0.9309224028625668,
-                    "timestamp": 0.1269708740069118
-                },
-                {
-                    "x": 2.2777013130983144,
-                    "y": 4.249280679670162,
-                    "heading": -0.1750916915555501,
-                    "angularVelocity": 2.223940585151498,
-                    "velocityX": -0.22363149278956135,
-                    "velocityY": -1.1686972748670326,
-                    "timestamp": 0.15871359250863973
-                },
-                {
-                    "x": 2.2695785209744463,
-                    "y": 4.204529290238701,
-                    "heading": -0.09189311777806415,
-                    "angularVelocity": 2.6210286233977427,
-                    "velocityX": -0.25589465890975394,
-                    "velocityY": -1.409815905623355,
-                    "timestamp": 0.19045631101036767
-                },
-                {
-                    "x": 2.2606569711878857,
-                    "y": 4.151982062664549,
-                    "heading": 0.002706319559794192,
-                    "angularVelocity": 2.9801933105606198,
-                    "velocityX": -0.2810581515277304,
-                    "velocityY": -1.6554104391308149,
-                    "timestamp": 0.2221990295120956
-                },
-                {
-                    "x": 2.251280347316294,
-                    "y": 4.091451352956859,
-                    "heading": 0.10685098530745366,
-                    "angularVelocity": 3.2808993893194844,
-                    "velocityX": -0.2953944814487593,
-                    "velocityY": -1.90691637530654,
-                    "timestamp": 0.2539417480138236
-                },
-                {
-                    "x": 2.242133674900212,
-                    "y": 4.022709367456616,
-                    "heading": 0.21739538615105536,
-                    "angularVelocity": 3.4825120865934673,
-                    "velocityX": -0.28815025454054716,
-                    "velocityY": -2.165598560706175,
-                    "timestamp": 0.2856844665155515
-                },
-                {
-                    "x": 2.2351422077578347,
-                    "y": 3.945850682832356,
-                    "heading": 0.32707167243976176,
-                    "angularVelocity": 3.455163623831899,
-                    "velocityX": -0.22025420229828702,
-                    "velocityY": -2.4213012700873704,
-                    "timestamp": 0.31742718501727946
-                },
-                {
-                    "x": 2.2344515310594897,
-                    "y": 3.864378163543378,
-                    "heading": 0.4256089749961829,
-                    "angularVelocity": 3.1042490122910227,
-                    "velocityX": -0.021758586880562782,
-                    "velocityY": -2.566652231898267,
-                    "timestamp": 0.3491699035190074
-                },
-                {
-                    "x": 2.2416429003575074,
-                    "y": 3.7806290345563274,
-                    "heading": 0.5177286971400864,
-                    "angularVelocity": 2.9020741288711216,
-                    "velocityX": 0.22655177746121863,
-                    "velocityY": -2.638372922674897,
-                    "timestamp": 0.38091262202073534
-                },
-                {
-                    "x": 2.256915898193716,
-                    "y": 3.695399323053411,
-                    "heading": 0.6036240465401915,
-                    "angularVelocity": 2.705985922265222,
-                    "velocityX": 0.4811496480800017,
-                    "velocityY": -2.6850161399464656,
-                    "timestamp": 0.4126553405224633
-                },
-                {
-                    "x": 2.280071045497703,
-                    "y": 3.6084777900686467,
-                    "heading": 0.6811513337134051,
-                    "angularVelocity": 2.4423644486842004,
-                    "velocityX": 0.729463272111561,
-                    "velocityY": -2.7383140791811433,
-                    "timestamp": 0.4443980590241912
-                },
-                {
-                    "x": 2.3096855944046597,
-                    "y": 3.5244760350530098,
-                    "heading": 0.76088689172136,
-                    "angularVelocity": 2.511932240574098,
-                    "velocityX": 0.932955660534971,
-                    "velocityY": -2.6463314731869354,
-                    "timestamp": 0.47614077752591916
-                },
-                {
-                    "x": 2.3348942368332115,
-                    "y": 3.446420745412462,
-                    "heading": 0.8559069862879931,
-                    "angularVelocity": 2.993445396350149,
-                    "velocityX": 0.7941551202420774,
-                    "velocityY": -2.458998262429792,
-                    "timestamp": 0.5078834960276472
-                },
-                {
-                    "x": 2.355075569752848,
-                    "y": 3.373868279566975,
-                    "heading": 0.9657696902827837,
-                    "angularVelocity": 3.4610363944982643,
-                    "velocityX": 0.6357783413709821,
-                    "velocityY": -2.285641220097014,
-                    "timestamp": 0.5396262145293751
-                },
-                {
-                    "x": 2.370025604730125,
-                    "y": 3.306495756130267,
-                    "heading": 1.091029644988018,
-                    "angularVelocity": 3.9461004166489144,
-                    "velocityX": 0.4709752561508733,
-                    "velocityY": -2.1224560030370347,
-                    "timestamp": 0.571368933031103
-                },
-                {
-                    "x": 2.3800631598495494,
-                    "y": 3.2441375159647747,
-                    "heading": 1.2334110423596254,
-                    "angularVelocity": 4.485482154399005,
-                    "velocityX": 0.31621598883778346,
-                    "velocityY": -1.9644895934825695,
-                    "timestamp": 0.603111651532831
-                },
-                {
-                    "x": 2.3890320547539976,
-                    "y": 3.1882487557001293,
-                    "heading": 1.3963738258608078,
-                    "angularVelocity": 5.133863487221998,
-                    "velocityX": 0.28254967840764567,
-                    "velocityY": -1.760679705539431,
-                    "timestamp": 0.6348543700345589
-                },
-                {
-                    "x": 2.399614951757013,
-                    "y": 3.1248820965808815,
-                    "heading": 1.5465533770663946,
-                    "angularVelocity": 4.7311496397956585,
-                    "velocityX": 0.33339605120587323,
-                    "velocityY": -1.9962581061165,
-                    "timestamp": 0.6665970885362869
-                },
-                {
-                    "x": 2.4156041612276775,
-                    "y": 3.056078363317597,
-                    "heading": 1.6846738705269053,
-                    "angularVelocity": 4.351249671731503,
-                    "velocityX": 0.5037126694046519,
-                    "velocityY": -2.167543818263053,
-                    "timestamp": 0.6983398070380148
-                },
-                {
-                    "x": 2.4386535219180887,
-                    "y": 2.9838461830329104,
-                    "heading": 1.8122491249375776,
-                    "angularVelocity": 4.019039969866676,
-                    "velocityX": 0.7261306459671242,
-                    "velocityY": -2.275551171861821,
-                    "timestamp": 0.7300825255397427
-                },
-                {
-                    "x": 2.4695800740474128,
-                    "y": 2.910121142766613,
-                    "heading": 1.9308538821800434,
-                    "angularVelocity": 3.736439814882526,
-                    "velocityX": 0.9742880757878827,
-                    "velocityY": -2.3225811696715355,
-                    "timestamp": 0.7618252440414707
-                },
-                {
-                    "x": 2.508646228883482,
-                    "y": 2.8366969767650785,
-                    "heading": 2.0420489888957944,
-                    "angularVelocity": 3.503011461028351,
-                    "velocityX": 1.2307123233299608,
-                    "velocityY": -2.313102641084062,
-                    "timestamp": 0.7935679625431986
-                },
-                {
-                    "x": 2.555734299118471,
-                    "y": 2.7651139971749186,
-                    "heading": 2.147169748045839,
-                    "angularVelocity": 3.3116495408016253,
-                    "velocityX": 1.4834290337302587,
-                    "velocityY": -2.255099215470886,
-                    "timestamp": 0.8253106810449266
-                },
-                {
-                    "x": 2.6104869842529297,
-                    "y": 2.6966493129730225,
-                    "heading": 2.247054267099527,
-                    "angularVelocity": 3.1466907614812087,
-                    "velocityX": 1.7248896036260801,
-                    "velocityY": -2.1568626580665695,
-                    "timestamp": 0.8570533995466545
-                },
-                {
-                    "x": 2.751018773357776,
-                    "y": 2.5737681266542305,
-                    "heading": 2.423101617636397,
-                    "angularVelocity": 2.7414576820608,
-                    "velocityX": 2.188399607493837,
-                    "velocityY": -1.9135395743649004,
-                    "timestamp": 0.9212700940770386
-                },
-                {
-                    "x": 2.920945282929504,
-                    "y": 2.4615612619997704,
-                    "heading": 2.5486951706813996,
-                    "angularVelocity": 1.9557772937936253,
-                    "velocityX": 2.646142265876451,
-                    "velocityY": -1.7473161064272718,
-                    "timestamp": 0.9854867886074228
-                },
-                {
-                    "x": 3.118708267205298,
-                    "y": 2.3583606370011725,
-                    "heading": 2.6122511958810777,
-                    "angularVelocity": 0.9897118757741349,
-                    "velocityX": 3.079619493373657,
-                    "velocityY": -1.6070684695514719,
-                    "timestamp": 1.049703483137807
-                },
-                {
-                    "x": 3.3434611246912245,
-                    "y": 2.26616504003377,
-                    "heading": 2.6122519169792926,
-                    "angularVelocity": 0.000011229139401682067,
-                    "velocityX": 3.499913209945475,
-                    "velocityY": -1.4356951512629466,
-                    "timestamp": 1.113920177668191
-                },
-                {
-                    "x": 3.5722970045795748,
-                    "y": 2.184629282343257,
-                    "heading": 2.6122522165022306,
-                    "angularVelocity": 0.000004664253435080619,
-                    "velocityX": 3.5634951559220656,
-                    "velocityY": -1.269697207039109,
-                    "timestamp": 1.1781368721985752
-                },
-                {
-                    "x": 3.80113295504209,
-                    "y": 2.103093722723815,
-                    "heading": 2.6122525160244625,
-                    "angularVelocity": 0.000004664242440601037,
-                    "velocityX": 3.56349625492233,
-                    "velocityY": -1.2696941226220015,
-                    "timestamp": 1.2423535667289594
-                },
-                {
-                    "x": 4.029968923086795,
-                    "y": 2.0215582124500746,
-                    "heading": 2.612252815546594,
-                    "angularVelocity": 0.00000466424087155042,
-                    "velocityX": 3.563496528717025,
-                    "velocityY": -1.2696933541971844,
-                    "timestamp": 1.3065702612593435
-                },
-                {
-                    "x": 4.258804909490432,
-                    "y": 1.940022753702107,
-                    "heading": 2.6122531150686714,
-                    "angularVelocity": 0.000004664240033500949,
-                    "velocityX": 3.5634968146073587,
-                    "velocityY": -1.2696925518237052,
-                    "timestamp": 1.3707869557897276
-                },
-                {
-                    "x": 4.487640916105967,
-                    "y": 1.8584873516805105,
-                    "heading": 2.6122534145907514,
-                    "angularVelocity": 0.000004664240074560689,
-                    "velocityX": 3.563497129352575,
-                    "velocityY": -1.2696916684650783,
-                    "timestamp": 1.4350036503201118
-                },
-                {
-                    "x": 4.716476945604857,
-                    "y": 1.7769520138830965,
-                    "heading": 2.6122537141129194,
-                    "angularVelocity": 0.000004664241438058459,
-                    "velocityX": 3.5634974856984654,
-                    "velocityY": -1.2696906683484759,
-                    "timestamp": 1.499220344850496
-                },
-                {
-                    "x": 4.945312999982612,
-                    "y": 1.6954167459104819,
-                    "heading": 2.612254013635151,
-                    "angularVelocity": 0.000004664242435524347,
-                    "velocityX": 3.563497873118986,
-                    "velocityY": -1.269689581017522,
-                    "timestamp": 1.56343703938088
-                },
-                {
-                    "x": 5.174149135873993,
-                    "y": 1.613881706713074,
-                    "heading": 2.6122543131567744,
-                    "angularVelocity": 0.0000046642329655920085,
-                    "velocityX": 3.563499142471556,
-                    "velocityY": -1.269686018467187,
-                    "timestamp": 1.6276537339112642
-                },
-                {
-                    "x": 5.406651475687904,
-                    "y": 1.5434785463415874,
-                    "heading": 2.61225458420666,
-                    "angularVelocity": 0.000004220863245612629,
-                    "velocityX": 3.6205902766281897,
-                    "velocityY": -1.096337344772168,
-                    "timestamp": 1.6918704284416484
-                },
-                {
-                    "x": 5.646764755249023,
-                    "y": 1.5066063404083252,
-                    "heading": 2.6122548161665473,
-                    "angularVelocity": 0.0000036121430551488755,
-                    "velocityX": 3.7391099202016678,
-                    "velocityY": -0.5741841152508433,
-                    "timestamp": 1.7560871229720325
-                },
-                {
-                    "x": 5.787436421990979,
-                    "y": 1.496717214982701,
-                    "heading": 2.612254832075579,
-                    "angularVelocity": 4.2677202387530557e-7,
-                    "velocityX": 3.773625708136921,
-                    "velocityY": -0.26528339929021244,
-                    "timestamp": 1.7933647115888978
-                },
-                {
-                    "x": 5.926771623718837,
-                    "y": 1.4982765413849686,
-                    "heading": 2.6080354125638485,
-                    "angularVelocity": -0.11318917527356848,
-                    "velocityX": 3.7377740057150746,
-                    "velocityY": 0.04183013065287323,
-                    "timestamp": 1.830642300205763
-                },
-                {
-                    "x": 6.064421169286406,
-                    "y": 1.511276136415188,
-                    "heading": 2.60121232475526,
-                    "angularVelocity": -0.18303458087688948,
-                    "velocityX": 3.6925549820916683,
-                    "velocityY": 0.34872413996054913,
-                    "timestamp": 1.8679198888226283
-                },
-                {
-                    "x": 6.200023778520788,
-                    "y": 1.5356760041928015,
-                    "heading": 2.5932728220304684,
-                    "angularVelocity": -0.2129832700927332,
-                    "velocityX": 3.6376443398227996,
-                    "velocityY": 0.6545452289951588,
-                    "timestamp": 1.9051974774394935
-                },
-                {
-                    "x": 6.333175231231244,
-                    "y": 1.5714002898334831,
-                    "heading": 2.5855391112934316,
-                    "angularVelocity": -0.20746274166290535,
-                    "velocityX": 3.5718901798871894,
-                    "velocityY": 0.9583314523868992,
-                    "timestamp": 1.9424750660563588
-                },
-                {
-                    "x": 6.463368734714764,
-                    "y": 1.6183219287773098,
-                    "heading": 2.5790588590244545,
-                    "angularVelocity": -0.17383775371256402,
-                    "velocityX": 3.4925409157130343,
-                    "velocityY": 1.258709071181673,
-                    "timestamp": 1.979752654673224
-                },
-                {
-                    "x": 6.589861303526444,
-                    "y": 1.6762129661852994,
-                    "heading": 2.574283804517795,
-                    "angularVelocity": -0.12809451157737914,
-                    "velocityX": 3.393260495247066,
-                    "velocityY": 1.5529716260079764,
-                    "timestamp": 2.017030243290089
-                },
-                {
-                    "x": 6.711340738368746,
-                    "y": 1.7445488061219798,
-                    "heading": 2.570091844488755,
-                    "angularVelocity": -0.11245255352014773,
-                    "velocityX": 3.258779318878531,
-                    "velocityY": 1.8331614911851861,
-                    "timestamp": 2.054307831906954
-                },
-                {
-                    "x": 6.825305386557433,
-                    "y": 1.8215960437325822,
-                    "heading": 2.560973344137785,
-                    "angularVelocity": -0.24461078866147912,
-                    "velocityX": 3.057189384216965,
-                    "velocityY": 2.0668514372666396,
-                    "timestamp": 2.091585420523819
-                },
-                {
-                    "x": 6.929260415482823,
-                    "y": 1.9025407238929235,
-                    "heading": 2.5356026772435865,
-                    "angularVelocity": -0.680587662333947,
-                    "velocityX": 2.788673645010342,
-                    "velocityY": 2.171403332771371,
-                    "timestamp": 2.128863009140684
-                },
-                {
-                    "x": 7.023455735639986,
-                    "y": 1.9838668818491878,
-                    "heading": 2.488274688562586,
-                    "angularVelocity": -1.2696097155701718,
-                    "velocityX": 2.5268619471418052,
-                    "velocityY": 2.1816367681967113,
-                    "timestamp": 2.166140597757549
-                },
-                {
-                    "x": 7.110101814938688,
-                    "y": 2.060020595789651,
-                    "heading": 2.4125158730705962,
-                    "angularVelocity": -2.0322885224854694,
-                    "velocityX": 2.3243477519224953,
-                    "velocityY": 2.0428819772427436,
-                    "timestamp": 2.2034181863744142
-                },
-                {
-                    "x": 7.191835709933807,
-                    "y": 2.1285351921147058,
-                    "heading": 2.3080325133818067,
-                    "angularVelocity": -2.8028465242925935,
-                    "velocityX": 2.1925746280203615,
-                    "velocityY": 1.8379567688575535,
-                    "timestamp": 2.2406957749912793
-                },
-                {
-                    "x": 7.274315415796274,
-                    "y": 2.1867028423762775,
-                    "heading": 2.1850264648931392,
-                    "angularVelocity": -3.2997319046816447,
-                    "velocityX": 2.2125815784434106,
-                    "velocityY": 1.5603919786607532,
-                    "timestamp": 2.2779733636081443
-                },
-                {
-                    "x": 7.351412179448119,
-                    "y": 2.2360068924730196,
-                    "heading": 2.043511695780604,
-                    "angularVelocity": -3.796242578000636,
-                    "velocityX": 2.0681800114335913,
-                    "velocityY": 1.322619083639869,
-                    "timestamp": 2.3152509522250093
-                },
-                {
-                    "x": 7.420172175937219,
-                    "y": 2.2789013315216375,
-                    "heading": 1.8838780266002726,
-                    "angularVelocity": -4.282296014933475,
-                    "velocityX": 1.8445398170951257,
-                    "velocityY": 1.1506763350355667,
-                    "timestamp": 2.3525285408418743
-                },
-                {
-                    "x": 7.479537718839642,
-                    "y": 2.3170603162912466,
-                    "heading": 1.7061327210648876,
-                    "angularVelocity": -4.768154597182533,
-                    "velocityX": 1.592526370537947,
-                    "velocityY": 1.0236441300375574,
-                    "timestamp": 2.3898061294587394
-                },
-                {
-                    "x": 7.529281054923766,
-                    "y": 2.3515994880332096,
-                    "heading": 1.5086439015102746,
-                    "angularVelocity": -5.2977895535137876,
-                    "velocityX": 1.3344032682848619,
-                    "velocityY": 0.9265398601007353,
-                    "timestamp": 2.4270837180756044
-                },
-                {
-                    "x": 7.569719945861703,
-                    "y": 2.383438536522646,
-                    "heading": 1.288404786035775,
-                    "angularVelocity": -5.908083748068905,
-                    "velocityX": 1.0848043674059311,
-                    "velocityY": 0.8541069760888933,
-                    "timestamp": 2.4643613066924694
-                },
-                {
-                    "x": 7.603202598509127,
-                    "y": 2.4091886515491456,
-                    "heading": 1.0408456353949866,
-                    "angularVelocity": -6.640964714353526,
-                    "velocityX": 0.8981979223912477,
-                    "velocityY": 0.6907666504707783,
-                    "timestamp": 2.5016388953093345
-                },
-                {
-                    "x": 7.630132008026638,
-                    "y": 2.427100499568469,
-                    "heading": 0.7720308301798764,
-                    "angularVelocity": -7.211164004677401,
-                    "velocityX": 0.7224021326670985,
-                    "velocityY": 0.48049910640464033,
-                    "timestamp": 2.5389164839261995
-                },
-                {
-                    "x": 7.649623319500903,
-                    "y": 2.438227448776781,
-                    "heading": 0.5254269726030665,
-                    "angularVelocity": -6.615338242807932,
-                    "velocityX": 0.5228694289911082,
-                    "velocityY": 0.29848897477445496,
-                    "timestamp": 2.5761940725430645
-                },
-                {
-                    "x": 7.66290885295536,
-                    "y": 2.4458359453096237,
-                    "heading": 0.3183730749926382,
-                    "angularVelocity": -5.554380132752273,
-                    "velocityX": 0.3563946582222695,
-                    "velocityY": 0.2041037742822349,
-                    "timestamp": 2.6134716611599296
-                },
-                {
-                    "x": 7.671732744074589,
-                    "y": 2.451418069701132,
-                    "heading": 0.15988099310288126,
-                    "angularVelocity": -4.251672057404801,
-                    "velocityX": 0.23670766931625417,
-                    "velocityY": 0.14974478228408286,
-                    "timestamp": 2.6507492497767946
-                },
-                {
-                    "x": 7.677275184086734,
-                    "y": 2.4551558590330185,
-                    "heading": 0.05341064749493927,
-                    "angularVelocity": -2.8561489505727446,
-                    "velocityX": 0.14868021827027583,
-                    "velocityY": 0.10026907508163654,
-                    "timestamp": 2.6880268383936596
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 1.0247603884557974e-38,
-                    "angularVelocity": -1.4327817189005392,
-                    "velocityX": 0.07309528363731636,
-                    "velocityY": 0.04947050051802475,
-                    "timestamp": 2.7253044270105247
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "angularVelocity": -2.6319896813971663e-37,
-                    "velocityX": 5.227147746061896e-36,
-                    "velocityY": -6.467829854732196e-35,
-                    "timestamp": 2.7625820156273897
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "W3 To C5": {
-            "waypoints": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 27
-                },
-                {
-                    "x": 2.7430825233459473,
-                    "y": 2.7833950519561768,
-                    "heading": 3.14,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 35
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -4.609910300520975e-35,
-                    "timestamp": 0
-                },
-                {
-                    "x": 2.298808348352727,
-                    "y": 4.35514296819253,
-                    "heading": -0.3837399750205753,
-                    "angularVelocity": 0.24884159467028183,
-                    "velocityX": -0.047369219319970686,
-                    "velocityY": -0.19307136062676852,
-                    "timestamp": 0.02515666638336474
-                },
-                {
-                    "x": 2.2965596540188837,
-                    "y": 4.3453750993410925,
-                    "heading": -0.37149402212882315,
-                    "angularVelocity": 0.48678758564965857,
-                    "velocityX": -0.08938761199815681,
-                    "velocityY": -0.38828152755153583,
-                    "timestamp": 0.05031333276672948
-                },
-                {
-                    "x": 2.2934096466067952,
-                    "y": 4.330641358670927,
-                    "heading": -0.3535645425221108,
-                    "angularVelocity": 0.7127128584321681,
-                    "velocityX": -0.1252156133919033,
-                    "velocityY": -0.5856793760205414,
-                    "timestamp": 0.07546999915009422
-                },
-                {
-                    "x": 2.289538880665139,
-                    "y": 4.310886769515238,
-                    "heading": -0.33028900557963653,
-                    "angularVelocity": 0.9252234214095049,
-                    "velocityX": -0.15386640990779057,
-                    "velocityY": -0.7852625961900879,
-                    "timestamp": 0.10062666553345896
-                },
-                {
-                    "x": 2.2851573432175654,
-                    "y": 4.286058471237428,
-                    "heading": -0.3020481691352866,
-                    "angularVelocity": 1.1225985197714665,
-                    "velocityX": -0.17417003432819553,
-                    "velocityY": -0.9869470739663532,
-                    "timestamp": 0.1257833319168237
-                },
-                {
-                    "x": 2.280510060472114,
-                    "y": 4.25610900707676,
-                    "heading": -0.26927550139535,
-                    "angularVelocity": 1.3027428690475438,
-                    "velocityX": -0.18473364771909595,
-                    "velocityY": -1.1905180004483138,
-                    "timestamp": 0.15093999830018845
-                },
-                {
-                    "x": 2.2758836751536,
-                    "y": 4.2210014881511375,
-                    "heading": -0.23246752663964654,
-                    "angularVelocity": 1.463149933889621,
-                    "velocityX": -0.1839029562984313,
-                    "velocityY": -1.3955552930033008,
-                    "timestamp": 0.1760966646835532
-                },
-                {
-                    "x": 2.271613693612169,
-                    "y": 4.180717494993525,
-                    "heading": -0.19219487185489256,
-                    "angularVelocity": 1.6008740653882818,
-                    "velocityX": -0.16973558723403512,
-                    "velocityY": -1.6013247758554696,
-                    "timestamp": 0.20125333106691795
-                },
-                {
-                    "x": 2.268091592763015,
-                    "y": 4.135268758759812,
-                    "heading": -0.1491141249151469,
-                    "angularVelocity": 1.7124982413501877,
-                    "velocityX": -0.1400066604803655,
-                    "velocityY": -1.8066279347634933,
-                    "timestamp": 0.2264099974502827
-                },
-                {
-                    "x": 2.2657701315686967,
-                    "y": 4.084713672798125,
-                    "heading": -0.10398137252966654,
-                    "angularVelocity": 1.7940672940403972,
-                    "velocityX": -0.09228015981695388,
-                    "velocityY": -2.0096099058307106,
-                    "timestamp": 0.25156666383364745
-                },
-                {
-                    "x": 2.265163909208295,
-                    "y": 4.029179243747037,
-                    "heading": -0.057668550112221675,
-                    "angularVelocity": 1.8409761337881332,
-                    "velocityX": -0.02409788129965802,
-                    "velocityY": -2.2075432493635576,
-                    "timestamp": 0.2767233302170122
-                },
-                {
-                    "x": 2.266840363517046,
-                    "y": 3.9688874958283327,
-                    "heading": -0.011179237419756825,
-                    "angularVelocity": 1.8479917801512262,
-                    "velocityX": 0.06664055893588815,
-                    "velocityY": -2.3966509314037507,
-                    "timestamp": 0.3018799966003769
-                },
-                {
-                    "x": 2.2713096946462064,
-                    "y": 3.9041248350083095,
-                    "heading": 0.03446639703260334,
-                    "angularVelocity": 1.8144548151476838,
-                    "velocityX": 0.1776599117328043,
-                    "velocityY": -2.574373719995273,
-                    "timestamp": 0.3270366629837416
-                },
-                {
-                    "x": 2.2787160490905882,
-                    "y": 3.8349972418338227,
-                    "heading": 0.07865637806724814,
-                    "angularVelocity": 1.7565912892125506,
-                    "velocityX": 0.29440921668697884,
-                    "velocityY": -2.74788368701343,
-                    "timestamp": 0.35219332936710634
-                },
-                {
-                    "x": 2.289237241442295,
-                    "y": 3.7616571782461574,
-                    "heading": 0.12059430680114623,
-                    "angularVelocity": 1.667070195025133,
-                    "velocityX": 0.41822681079334173,
-                    "velocityY": -2.915333155436007,
-                    "timestamp": 0.37734999575047107
-                },
-                {
-                    "x": 2.303092280593543,
-                    "y": 3.684333317171128,
-                    "heading": 0.15922184993862323,
-                    "angularVelocity": 1.5354794052927534,
-                    "velocityX": 0.550750204343837,
-                    "velocityY": -3.073692670431113,
-                    "timestamp": 0.4025066621338358
-                },
-                {
-                    "x": 2.3205444941015814,
-                    "y": 3.6033821039148743,
-                    "heading": 0.19314125064776663,
-                    "angularVelocity": 1.3483265307192482,
-                    "velocityX": 0.6937411039317692,
-                    "velocityY": -3.217883165544707,
-                    "timestamp": 0.4276633285172005
-                },
-                {
-                    "x": 2.341904587688066,
-                    "y": 3.5193842561187316,
-                    "heading": 0.22066827104785883,
-                    "angularVelocity": 1.0942236932591152,
-                    "velocityX": 0.8490828339882706,
-                    "velocityY": -3.3389896147642064,
-                    "timestamp": 0.45281999490056524
-                },
-                {
-                    "x": 2.3675851289681646,
-                    "y": 3.4337423994061407,
-                    "heading": 0.23918679984335672,
-                    "angularVelocity": 0.7361280907928144,
-                    "velocityX": 1.0208244959308244,
-                    "velocityY": -3.40434044032252,
-                    "timestamp": 0.47797666128392996
-                },
-                {
-                    "x": 2.3978885412195137,
-                    "y": 3.3469640370766824,
-                    "heading": 0.24945072521405584,
-                    "angularVelocity": 0.40800021808479553,
-                    "velocityX": 1.2045877537807594,
-                    "velocityY": -3.4495175555868487,
-                    "timestamp": 0.5031333276672947
-                },
-                {
-                    "x": 2.4331457577160354,
-                    "y": 3.2598611919071945,
-                    "heading": 0.25313177220508776,
-                    "angularVelocity": 0.1463249118518414,
-                    "velocityX": 1.4015059054023244,
-                    "velocityY": -3.462416038839161,
-                    "timestamp": 0.5282899940506595
-                },
-                {
-                    "x": 2.473498104501041,
-                    "y": 3.173715186571369,
-                    "heading": 0.2532432496800442,
-                    "angularVelocity": 0.004431329384332257,
-                    "velocityX": 1.604041893710102,
-                    "velocityY": -3.424380799229854,
-                    "timestamp": 0.5534466604340242
-                },
-                {
-                    "x": 2.5185817721335604,
-                    "y": 3.089905547060991,
-                    "heading": 0.25324321969105484,
-                    "angularVelocity": -0.0000011920891614956396,
-                    "velocityX": 1.7921161311870697,
-                    "velocityY": -3.3315081669882356,
-                    "timestamp": 0.578603326817389
-                },
-                {
-                    "x": 2.568241478882672,
-                    "y": 3.0087236123099395,
-                    "heading": 0.2532431890633549,
-                    "angularVelocity": -0.0000012174784783024135,
-                    "velocityX": 1.97401778090718,
-                    "velocityY": -3.2270545514223747,
-                    "timestamp": 0.6037599932007538
-                },
-                {
-                    "x": 2.6223245423144714,
-                    "y": 2.9304189809174512,
-                    "heading": 0.25324315880142845,
-                    "angularVelocity": -0.0000012029386552177117,
-                    "velocityX": 2.1498501672527754,
-                    "velocityY": -3.1126791681853656,
-                    "timestamp": 0.6289166595841186
-                },
-                {
-                    "x": 2.6806646810204167,
-                    "y": 2.855232404738762,
-                    "heading": 0.25324312847978453,
-                    "angularVelocity": -0.0000012053124772832063,
-                    "velocityX": 2.319072718813166,
-                    "velocityY": -2.9887336832676343,
-                    "timestamp": 0.6540733259674834
-                },
-                {
-                    "x": 2.7430825233459473,
-                    "y": 2.7833950519561768,
-                    "heading": 0.2532430937019675,
-                    "angularVelocity": -0.0000013824493495341482,
-                    "velocityX": 2.4811650865953045,
-                    "velocityY": -2.8555990562441647,
-                    "timestamp": 0.6792299923508481
-                },
-                {
-                    "x": 2.8747857816965947,
-                    "y": 2.6605842580771375,
-                    "heading": 0.2532430684120768,
-                    "angularVelocity": -5.312693002073383e-7,
-                    "velocityX": 2.7667141273306664,
-                    "velocityY": -2.579908520631989,
-                    "timestamp": 0.7268327631068262
-                },
-                {
-                    "x": 3.0186320127358077,
-                    "y": 2.5522491183421203,
-                    "heading": 0.2532430469824583,
-                    "angularVelocity": -4.5017586399077444e-7,
-                    "velocityX": 3.021803747025554,
-                    "velocityY": -2.2758158404343,
-                    "timestamp": 0.7744355338628043
-                },
-                {
-                    "x": 3.1730376030001257,
-                    "y": 2.4595822138278454,
-                    "heading": 0.2532430277420137,
-                    "angularVelocity": -4.041874945252969e-7,
-                    "velocityX": 3.2436261127705017,
-                    "velocityY": -1.946670394236201,
-                    "timestamp": 0.8220383046187824
-                },
-                {
-                    "x": 3.336302611834685,
-                    "y": 2.38360340882859,
-                    "heading": 0.25324300957525164,
-                    "angularVelocity": -3.816324501860361e-7,
-                    "velocityX": 3.4297375182526793,
-                    "velocityY": -1.5961004746706646,
-                    "timestamp": 0.8696410753747604
-                },
-                {
-                    "x": 3.505316504811254,
-                    "y": 2.3214540241877932,
-                    "heading": 0.25324299166355524,
-                    "angularVelocity": -3.7627424124720095e-7,
-                    "velocityX": 3.550505365390803,
-                    "velocityY": -1.3055833442844713,
-                    "timestamp": 0.9172438461307385
-                },
-                {
-                    "x": 3.6743308817857088,
-                    "y": 2.2593059557844626,
-                    "heading": 0.25324297375187266,
-                    "angularVelocity": -3.76273948990246e-7,
-                    "velocityX": 3.5505155328218008,
-                    "velocityY": -1.3055556938463668,
-                    "timestamp": 0.9648466168867166
-                },
-                {
-                    "x": 3.843345266545321,
-                    "y": 2.1971579085532174,
-                    "heading": 0.2532429558401917,
-                    "angularVelocity": -3.762739157561073e-7,
-                    "velocityX": 3.5505156963660025,
-                    "velocityY": -1.3055552490805507,
-                    "timestamp": 1.0124493876426945
-                },
-                {
-                    "x": 4.012359658311276,
-                    "y": 2.135009880376043,
-                    "heading": 0.25324293792850894,
-                    "angularVelocity": -3.7627395316214525e-7,
-                    "velocityX": 3.5505158435494915,
-                    "velocityY": -1.3055548488082471,
-                    "timestamp": 1.0600521583986726
-                },
-                {
-                    "x": 4.181374056391421,
-                    "y": 2.07286186937061,
-                    "heading": 0.25324292001682175,
-                    "angularVelocity": -3.7627404706616014e-7,
-                    "velocityX": 3.5505159761928384,
-                    "velocityY": -1.30555448807838,
-                    "timestamp": 1.1076549291546507
-                },
-                {
-                    "x": 4.3503884601627245,
-                    "y": 2.010713873842556,
-                    "heading": 0.25324290210512795,
-                    "angularVelocity": -3.7627418619029544e-7,
-                    "velocityX": 3.5505160957480255,
-                    "velocityY": -1.30555416294226,
-                    "timestamp": 1.1552576999106288
-                },
-                {
-                    "x": 4.51940286907005,
-                    "y": 1.9485658922821711,
-                    "heading": 0.2532428841934256,
-                    "angularVelocity": -3.7627436389492574e-7,
-                    "velocityX": 3.5505162036413855,
-                    "velocityY": -1.3055538695209365,
-                    "timestamp": 1.2028604706666068
-                },
-                {
-                    "x": 4.688417282561973,
-                    "y": 1.8864179231898264,
-                    "heading": 0.25324286628171494,
-                    "angularVelocity": -3.7627454024029507e-7,
-                    "velocityX": 3.550516299950831,
-                    "velocityY": -1.3055536076025602,
-                    "timestamp": 1.250463241422585
-                },
-                {
-                    "x": 4.857431700124039,
-                    "y": 1.824269965166445,
-                    "heading": 0.2532428483699966,
-                    "angularVelocity": -3.7627470129766985e-7,
-                    "velocityX": 3.5505163854530717,
-                    "velocityY": -1.3055533750748494,
-                    "timestamp": 1.298066012178563
-                },
-                {
-                    "x": 5.02644612130461,
-                    "y": 1.7621220169837726,
-                    "heading": 0.2532428304582713,
-                    "angularVelocity": -3.76274846815446e-7,
-                    "velocityX": 3.5505164614676405,
-                    "velocityY": -1.3055531683492934,
-                    "timestamp": 1.345668782934541
-                },
-                {
-                    "x": 5.1954605457099055,
-                    "y": 1.6999740775709098,
-                    "heading": 0.25324281254653974,
-                    "angularVelocity": -3.7627497879654404e-7,
-                    "velocityX": 3.5505165292100247,
-                    "velocityY": -1.3055529841203253,
-                    "timestamp": 1.3932715536905191
-                },
-                {
-                    "x": 5.364474972997734,
-                    "y": 1.6378261459972472,
-                    "heading": 0.25324279463480254,
-                    "angularVelocity": -3.7627509767034604e-7,
-                    "velocityX": 3.550516589763906,
-                    "velocityY": -1.3055528194408317,
-                    "timestamp": 1.4408743244464972
-                },
-                {
-                    "x": 5.533489402870974,
-                    "y": 1.5756782214547507,
-                    "heading": 0.2532427767230601,
-                    "angularVelocity": -3.762752068122207e-7,
-                    "velocityX": 3.5505166440761182,
-                    "velocityY": -1.3055526717358554,
-                    "timestamp": 1.4884770952024753
-                },
-                {
-                    "x": 5.702503835071411,
-                    "y": 1.513530303241192,
-                    "heading": 0.25324275881131303,
-                    "angularVelocity": -3.7627530496428226e-7,
-                    "velocityX": 3.550516692963962,
-                    "velocityY": -1.3055525387827163,
-                    "timestamp": 1.5360798659584534
-                },
-                {
-                    "x": 5.871518269374186,
-                    "y": 1.4513823907450571,
-                    "heading": 0.2532427408995616,
-                    "angularVelocity": -3.762753964251323e-7,
-                    "velocityX": 3.5505167371281625,
-                    "velocityY": -1.3055524186757583,
-                    "timestamp": 1.5836826367144314
-                },
-                {
-                    "x": 6.040532705582966,
-                    "y": 1.3892344834324082,
-                    "heading": 0.25324272298780626,
-                    "angularVelocity": -3.7627547903960503e-7,
-                    "velocityX": 3.550516777167954,
-                    "velocityY": -1.3055523097853368,
-                    "timestamp": 1.6312854074704095
-                },
-                {
-                    "x": 6.20954714352589,
-                    "y": 1.3270865808358587,
-                    "heading": 0.2532427050760471,
-                    "angularVelocity": -3.7627555795965075e-7,
-                    "velocityX": 3.5505168135974197,
-                    "velocityY": -1.3055522107133783,
-                    "timestamp": 1.6788881782263876
-                },
-                {
-                    "x": 6.378561583051887,
-                    "y": 1.2649386825445668,
-                    "heading": 0.25324268716428455,
-                    "angularVelocity": -3.7627562961930894e-7,
-                    "velocityX": 3.5505168468533315,
-                    "velocityY": -1.3055521202720615,
-                    "timestamp": 1.7264909489823657
-                },
-                {
-                    "x": 6.547576024027691,
-                    "y": 1.2027907881961044,
-                    "heading": 0.25324266925251887,
-                    "angularVelocity": -3.7627569544162935e-7,
-                    "velocityX": 3.5505168773096716,
-                    "velocityY": -1.3055520374443248,
-                    "timestamp": 1.7740937197383437
-                },
-                {
-                    "x": 6.716590466335514,
-                    "y": 1.1406428974701475,
-                    "heading": 0.25324265134075025,
-                    "angularVelocity": -3.762757576316506e-7,
-                    "velocityX": 3.550516905291665,
-                    "velocityY": -1.3055519613457025,
-                    "timestamp": 1.8216964904943218
-                },
-                {
-                    "x": 6.88560491686231,
-                    "y": 1.0784950290966298,
-                    "heading": 0.253242633428611,
-                    "angularVelocity": -3.7628354454135297e-7,
-                    "velocityX": 3.550517077949107,
-                    "velocityY": -1.3055514917839766,
-                    "timestamp": 1.8692992612502999
-                },
-                {
-                    "x": 7.0444139114910005,
-                    "y": 1.0203058482073266,
-                    "heading": 0.2259027129119336,
-                    "angularVelocity": -0.574334646544581,
-                    "velocityX": 3.336129223292024,
-                    "velocityY": -1.2223906290579827,
-                    "timestamp": 1.916902032006278
-                },
-                {
-                    "x": 7.185810534595712,
-                    "y": 0.9684234171781619,
-                    "heading": 0.18518533646408028,
-                    "angularVelocity": -0.8553572786042075,
-                    "velocityX": 2.9703443908662663,
-                    "velocityY": -1.089903596055891,
-                    "timestamp": 1.964504802762256
-                },
-                {
-                    "x": 7.3094655243228,
-                    "y": 0.9230369546143523,
-                    "heading": 0.14380134057962407,
-                    "angularVelocity": -0.8693610734677483,
-                    "velocityX": 2.59764269523238,
-                    "velocityY": -0.953441613650398,
-                    "timestamp": 2.012107573518234
-                },
-                {
-                    "x": 7.415398593682274,
-                    "y": 0.8841490900710189,
-                    "heading": 0.10540313701630608,
-                    "angularVelocity": -0.8066379950897271,
-                    "velocityX": 2.2253551143589885,
-                    "velocityY": -0.8169243917057022,
-                    "timestamp": 2.059710344274212
-                },
-                {
-                    "x": 7.503637039429732,
-                    "y": 0.8517540010892736,
-                    "heading": 0.07171800478429124,
-                    "angularVelocity": -0.70762965468317,
-                    "velocityX": 1.8536409613588987,
-                    "velocityY": -0.680529483206126,
-                    "timestamp": 2.1073131150301903
-                },
-                {
-                    "x": 7.574202084965293,
-                    "y": 0.8258459262172375,
-                    "heading": 0.04375776286271558,
-                    "angularVelocity": -0.5873658502969467,
-                    "velocityX": 1.482372652157004,
-                    "velocityY": -0.5442556065663985,
-                    "timestamp": 2.1549158857861683
-                },
-                {
-                    "x": 7.627109788299601,
-                    "y": 0.8064200554899893,
-                    "heading": 0.022189516524924582,
-                    "angularVelocity": -0.4530880449870122,
-                    "velocityX": 1.1114416764840027,
-                    "velocityY": -0.40808277372822227,
-                    "timestamp": 2.2025186565421464
-                },
-                {
-                    "x": 7.66237251850946,
-                    "y": 0.7934724766877013,
-                    "heading": 0.0074863600733853135,
-                    "angularVelocity": -0.3088718622474896,
-                    "velocityX": 0.7407705402406998,
-                    "velocityY": -0.27199212559831976,
-                    "timestamp": 2.2501214272981245
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": -1.0670900456397286e-32,
-                    "angularVelocity": -0.1572673177316084,
-                    "velocityX": 0.37030368633165434,
-                    "velocityY": -0.13596848639085585,
-                    "timestamp": 2.2977241980541026
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "angularVelocity": 2.230662363627099e-31,
-                    "velocityX": 2.3924515989499496e-33,
-                    "velocityY": 1.2585115119762557e-33,
-                    "timestamp": 2.3453269688100806
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "S1 To W1": {
-            "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.26,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 13
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.26,
-                    "angularVelocity": -5.95142511939464e-31,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.1029790507443495,
-                    "y": 6.633224483607701,
-                    "heading": 0.26650125878935643,
-                    "angularVelocity": 0.06942713064122713,
-                    "velocityX": -0.7797928290155524,
-                    "velocityY": 0.034434353705267144,
-                    "timestamp": 0.09364147314336706
-                },
-                {
-                    "x": 4.9569371541735245,
-                    "y": 6.639673450192742,
-                    "heading": 0.2795035687162854,
-                    "angularVelocity": 0.13885204376302582,
-                    "velocityX": -1.559585637308702,
-                    "velocityY": 0.06886870067889261,
-                    "timestamp": 0.18728294628673411
-                },
-                {
-                    "x": 4.737874314376445,
-                    "y": 6.649346898931416,
-                    "heading": 0.2990065710588269,
-                    "angularVelocity": 0.20827312608258508,
-                    "velocityX": -2.339378401936154,
-                    "velocityY": 0.1033030388561254,
-                    "timestamp": 0.28092441943010116
-                },
-                {
-                    "x": 4.445790546320906,
-                    "y": 6.66224482863892,
-                    "heading": 0.3250097249050781,
-                    "angularVelocity": 0.2776884319880837,
-                    "velocityX": -3.119171006722006,
-                    "velocityY": 0.13773736438080994,
-                    "timestamp": 0.37456589257346823
-                },
-                {
-                    "x": 4.09189529815297,
-                    "y": 6.677872415392626,
-                    "heading": 0.32500972789735083,
-                    "angularVelocity": 3.195452790688951e-8,
-                    "velocityX": -3.7792575905562438,
-                    "velocityY": 0.16688745092443205,
-                    "timestamp": 0.4682073657168353
-                },
-                {
-                    "x": 3.7380000499890724,
-                    "y": 6.693500002240241,
-                    "heading": 0.32500973088947493,
-                    "angularVelocity": 3.1952970747065184e-8,
-                    "velocityX": -3.7792575905130557,
-                    "velocityY": 0.16688745192729154,
-                    "timestamp": 0.5618488388602023
-                },
-                {
-                    "x": 3.384104801825175,
-                    "y": 6.7091275890878554,
-                    "heading": 0.3250097338815991,
-                    "angularVelocity": 3.195297127919844e-8,
-                    "velocityX": -3.7792575905130557,
-                    "velocityY": 0.1668874519272919,
-                    "timestamp": 0.6554903120035693
-                },
-                {
-                    "x": 3.030209553657211,
-                    "y": 6.724755175840924,
-                    "heading": 0.32500973687387263,
-                    "angularVelocity": 3.195453683514771e-8,
-                    "velocityX": -3.7792575905565435,
-                    "velocityY": 0.16688745091763021,
-                    "timestamp": 0.7491317851469363
-                },
-                {
-                    "x": 2.7381257502813003,
-                    "y": 6.7376531044804615,
-                    "heading": 0.35100602630019134,
-                    "angularVelocity": 0.2776151266490911,
-                    "velocityX": -3.119171383909283,
-                    "velocityY": 0.13773735297596457,
-                    "timestamp": 0.8427732582903034
-                },
-                {
-                    "x": 2.5190628797280783,
-                    "y": 6.747326551971013,
-                    "heading": 0.37050304728737704,
-                    "angularVelocity": 0.20820925101574836,
-                    "velocityX": -2.3393787303818834,
-                    "velocityY": 0.10330302552738789,
-                    "timestamp": 0.9364147314336704
-                },
-                {
-                    "x": 2.3730209608288053,
-                    "y": 6.7537755172838185,
-                    "heading": 0.3835010053218344,
-                    "angularVelocity": 0.13880556977745576,
-                    "velocityX": -1.559585875754842,
-                    "velocityY": 0.06886868709264785,
-                    "timestamp": 1.0300562045770374
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "angularVelocity": 0.06940295213233785,
-                    "velocityX": -0.7797929526056125,
-                    "velocityY": 0.03443434418469861,
-                    "timestamp": 1.1236976777204044
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "angularVelocity": -1.6940949553652625e-30,
-                    "velocityX": -1.6586267397188983e-32,
-                    "velocityY": 0,
-                    "timestamp": 1.2173391508637714
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "S1 To W2": {
-            "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.26,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": -0.003,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.26,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 3.363577949177338e-35,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.118924970919473,
-                    "y": 6.609128397315466,
-                    "heading": 0.25122660793754975,
-                    "angularVelocity": -0.10272459100864365,
-                    "velocityX": -0.6682716305586017,
-                    "velocityY": -0.24437832416494054,
-                    "timestamp": 0.08540693106008139
-                },
-                {
-                    "x": 5.0047749070529814,
-                    "y": 6.567385203608531,
-                    "heading": 0.23368024533248913,
-                    "angularVelocity": -0.20544424658833893,
-                    "velocityX": -1.3365433279201937,
-                    "velocityY": -0.48875651178205104,
-                    "timestamp": 0.17081386212016278
-                },
-                {
-                    "x": 4.833549798383878,
-                    "y": 6.504770434941582,
-                    "heading": 0.20736177398264571,
-                    "angularVelocity": -0.30815381167752187,
-                    "velocityX": -2.004815142563213,
-                    "velocityY": -0.7331345113302418,
-                    "timestamp": 0.25622079318024416
-                },
-                {
-                    "x": 4.605249631158782,
-                    "y": 6.421284110658292,
-                    "heading": 0.17227269285622732,
-                    "angularVelocity": -0.4108458258702004,
-                    "velocityX": -2.6730871182397737,
-                    "velocityY": -0.977512284390117,
-                    "timestamp": 0.34162772424032556
-                },
-                {
-                    "x": 4.319874394275373,
-                    "y": 6.316926253811125,
-                    "heading": 0.12841549400557886,
-                    "angularVelocity": -0.5135086614902031,
-                    "velocityX": -3.3413592239095435,
-                    "velocityY": -1.2218897875367427,
-                    "timestamp": 0.42703465530040696
-                },
-                {
-                    "x": 4.016437653324315,
-                    "y": 6.205962904892466,
-                    "heading": 0.1284154887581796,
-                    "angularVelocity": -6.143996998796484e-8,
-                    "velocityX": -3.552835082407984,
-                    "velocityY": -1.2992311928477889,
-                    "timestamp": 0.5124415863604883
-                },
-                {
-                    "x": 3.713000912388581,
-                    "y": 6.094999555931864,
-                    "heading": 0.12841548351080107,
-                    "angularVelocity": -6.143972697353832e-8,
-                    "velocityX": -3.5528350822285746,
-                    "velocityY": -1.299231193338901,
-                    "timestamp": 0.5978485174205697
-                },
-                {
-                    "x": 3.4095641714528466,
-                    "y": 5.98403620697126,
-                    "heading": 0.12841547826342262,
-                    "angularVelocity": -6.143972595662007e-8,
-                    "velocityX": -3.5528350822285746,
-                    "velocityY": -1.299231193338901,
-                    "timestamp": 0.683255448480651
-                },
-                {
-                    "x": 3.106127430499178,
-                    "y": 5.873072858059743,
-                    "heading": 0.12841547301602332,
-                    "angularVelocity": -6.143997022352915e-8,
-                    "velocityX": -3.55283508243856,
-                    "velocityY": -1.2992311927641775,
-                    "timestamp": 0.7686623795407324
-                },
-                {
-                    "x": 2.820751661400481,
-                    "y": 5.768715245814204,
-                    "heading": 0.08460804498823243,
-                    "angularVelocity": -0.5129259122655233,
-                    "velocityX": -3.3413654554329186,
-                    "velocityY": -1.2218869235814813,
-                    "timestamp": 0.8540693106008137
-                },
-                {
-                    "x": 2.5924510107246115,
-                    "y": 5.685229149936981,
-                    "heading": 0.04956396734437731,
-                    "angularVelocity": -0.41031889577208425,
-                    "velocityX": -2.6730927787964465,
-                    "velocityY": -0.977509610063062,
-                    "timestamp": 0.939476241660895
-                },
-                {
-                    "x": 2.4212255084791887,
-                    "y": 5.622614575599846,
-                    "heading": 0.02328185696555881,
-                    "angularVelocity": -0.30772807373595673,
-                    "velocityX": -2.00481975081121,
-                    "velocityY": -0.7331322359901695,
-                    "timestamp": 1.0248831727209764
-                },
-                {
-                    "x": 2.307075169405903,
-                    "y": 5.580871525201533,
-                    "heading": 0.005760683198102726,
-                    "angularVelocity": -0.20514931926462068,
-                    "velocityX": -1.3365465502206675,
-                    "velocityY": -0.48875483383130996,
-                    "timestamp": 1.1102901037810577
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": -0.003,
-                    "angularVelocity": -0.10257578734376759,
-                    "velocityX": -0.6682732735795421,
-                    "velocityY": -0.24437741694350015,
-                    "timestamp": 1.195697034841139
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": -0.003,
-                    "angularVelocity": 2.4865352537032555e-34,
-                    "velocityX": 0,
-                    "velocityY": -6.568641934425214e-34,
-                    "timestamp": 1.2811039659012204
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "S1 To W3": {
-            "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.26,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 12
-                },
-                {
-                    "x": 2.9135491847991943,
-                    "y": 6.344918727874756,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 8
-                },
-                {
-                    "x": 1.9371600151062012,
-                    "y": 5.555497646331787,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 10
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.393,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.26,
-                    "angularVelocity": 1.3458298549409377e-33,
-                    "velocityX": 0,
-                    "velocityY": 3.5102521636207017e-36,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.139424093284861,
-                    "y": 6.625864418761083,
-                    "heading": 0.2540205855596587,
-                    "angularVelocity": -0.08994806739995133,
-                    "velocityX": -0.5502097496757357,
-                    "velocityY": -0.06221136596695031,
-                    "timestamp": 0.06647629696982885
-                },
-                {
-                    "x": 5.066272282481334,
-                    "y": 6.617593263412388,
-                    "heading": 0.24206149463416465,
-                    "angularVelocity": -0.17990007672842862,
-                    "velocityX": -1.1004194598373498,
-                    "velocityY": -0.12442262469054285,
-                    "timestamp": 0.1329525939396577
-                },
-                {
-                    "x": 4.956544571814103,
-                    "y": 6.605186545652003,
-                    "heading": 0.22412231737841096,
-                    "angularVelocity": -0.26985825133875346,
-                    "velocityX": -1.6506291064472642,
-                    "velocityY": -0.1866337074403447,
-                    "timestamp": 0.19942889090948654
-                },
-                {
-                    "x": 4.810240966932036,
-                    "y": 6.588644282109891,
-                    "heading": 0.2002025750538153,
-                    "angularVelocity": -0.35982362759243297,
-                    "velocityX": -2.2008386680814915,
-                    "velocityY": -0.24884454002634754,
-                    "timestamp": 0.2659051878793154
-                },
-                {
-                    "x": 4.627361475577568,
-                    "y": 6.567966494763936,
-                    "heading": 0.17030184062671833,
-                    "angularVelocity": -0.4497954276945935,
-                    "velocityX": -2.7510481132465947,
-                    "velocityY": -0.31105504200002715,
-                    "timestamp": 0.3323814848491442
-                },
-                {
-                    "x": 4.407906114797318,
-                    "y": 6.543153211904946,
-                    "heading": 0.13441995513073204,
-                    "angularVelocity": -0.5397696191211097,
-                    "velocityX": -3.3012573019801734,
-                    "velocityY": -0.37326511839627413,
-                    "timestamp": 0.398857781818973
-                },
-                {
-                    "x": 4.158022417123658,
-                    "y": 6.514899899511502,
-                    "heading": 0.134419949269586,
-                    "angularVelocity": -8.816896134746106e-8,
-                    "velocityX": -3.758989430278769,
-                    "velocityY": -0.4250133307856765,
-                    "timestamp": 0.46533407878880184
-                },
-                {
-                    "x": 3.908138721969202,
-                    "y": 6.486646564834816,
-                    "heading": 0.13441994340875443,
-                    "angularVelocity": -8.816423004519242e-8,
-                    "velocityX": -3.758989392382515,
-                    "velocityY": -0.42501366599148993,
-                    "timestamp": 0.5318103757586307
-                },
-                {
-                    "x": 3.6582550295868868,
-                    "y": 6.45839320564021,
-                    "heading": 0.1344199375479281,
-                    "angularVelocity": -8.816415176708587e-8,
-                    "velocityX": -3.7589893506813095,
-                    "velocityY": -0.42501403481347844,
-                    "timestamp": 0.5982866727284595
-                },
-                {
-                    "x": 3.4083713401101514,
-                    "y": 6.430139820747532,
-                    "heading": 0.13441993168710784,
-                    "angularVelocity": -8.816406061478911e-8,
-                    "velocityX": -3.758989306972801,
-                    "velocityY": -0.42501442138844536,
-                    "timestamp": 0.6647629696982883
-                },
-                {
-                    "x": 3.1584877665624913,
-                    "y": 6.401885410552911,
-                    "heading": 0.1344199258262894,
-                    "angularVelocity": -8.816403354486871e-8,
-                    "velocityX": -3.7589875630568486,
-                    "velocityY": -0.4250298449602976,
-                    "timestamp": 0.7312392666681171
-                },
-                {
-                    "x": 2.9135491847991943,
-                    "y": 6.344918727874756,
-                    "heading": 0.13441991981204132,
-                    "angularVelocity": -9.047206787345137e-8,
-                    "velocityX": -3.684600269994962,
-                    "velocityY": -0.8569472921154051,
-                    "timestamp": 0.7977155636379459
-                },
-                {
-                    "x": 2.741853960900858,
-                    "y": 6.285250221506208,
-                    "heading": 0.13441991359516897,
-                    "angularVelocity": -1.293850671116252e-7,
-                    "velocityX": -3.573307735472737,
-                    "velocityY": -1.2418163448570063,
-                    "timestamp": 0.8457649443822611
-                },
-                {
-                    "x": 2.5774323687799447,
-                    "y": 6.207758184927692,
-                    "heading": 0.1344190973329971,
-                    "angularVelocity": -0.000016987985264431854,
-                    "velocityX": -3.4219294728447913,
-                    "velocityY": -1.6127582786315857,
-                    "timestamp": 0.8938143251265763
-                },
-                {
-                    "x": 2.4266376481277105,
-                    "y": 6.11739996688274,
-                    "heading": 0.11881021575379135,
-                    "angularVelocity": -0.32485083756365457,
-                    "velocityX": -3.1383280765814066,
-                    "velocityY": -1.8805282533353447,
-                    "timestamp": 0.9418637058708915
-                },
-                {
-                    "x": 2.292578699649266,
-                    "y": 6.017904789932588,
-                    "heading": 0.09375834131328029,
-                    "angularVelocity": -0.5213776754755571,
-                    "velocityX": -2.7900244790211017,
-                    "velocityY": -2.0706859361954097,
-                    "timestamp": 0.9899130866152067
-                },
-                {
-                    "x": 2.176263979579871,
-                    "y": 5.911118232592638,
-                    "heading": 0.0629895244075883,
-                    "angularVelocity": -0.6403582403990148,
-                    "velocityX": -2.420732968200766,
-                    "velocityY": -2.2224335815729503,
-                    "timestamp": 1.037962467359522
-                },
-                {
-                    "x": 2.0781210346390075,
-                    "y": 5.798061969403822,
-                    "heading": 0.02853978404947447,
-                    "angularVelocity": -0.7169653349214,
-                    "velocityX": -2.0425433880846535,
-                    "velocityY": -2.3529182153339443,
-                    "timestamp": 1.0860118481038372
-                },
-                {
-                    "x": 1.9983746548109367,
-                    "y": 5.679376225369417,
-                    "heading": -0.008333608258354223,
-                    "angularVelocity": -0.7674061920598474,
-                    "velocityX": -1.659675496182245,
-                    "velocityY": -2.470078535787315,
-                    "timestamp": 1.1340612288481524
-                },
-                {
-                    "x": 1.9371600151062012,
-                    "y": 5.555497646331787,
-                    "heading": -0.04678243253848999,
-                    "angularVelocity": -0.8001939605576434,
-                    "velocityX": -1.273994352403345,
-                    "velocityY": -2.5781514167024167,
-                    "timestamp": 1.1821106095924676
-                },
-                {
-                    "x": 1.8879136197731217,
-                    "y": 5.371859224657917,
-                    "heading": -0.10269858839018778,
-                    "angularVelocity": -0.8284337962966012,
-                    "velocityX": -0.7296170063605633,
-                    "velocityY": -2.720721274486196,
-                    "timestamp": 1.2496068369333404
-                },
-                {
-                    "x": 1.8760341540552343,
-                    "y": 5.181395172179021,
-                    "heading": -0.15967327513417093,
-                    "angularVelocity": -0.8441166119736836,
-                    "velocityX": -0.17600192167619955,
-                    "velocityY": -2.821847382921196,
-                    "timestamp": 1.3171030642742134
-                },
-                {
-                    "x": 1.9020919030528052,
-                    "y": 4.989002664772072,
-                    "heading": -0.21623702759974286,
-                    "angularVelocity": -0.8380283564577732,
-                    "velocityX": 0.3860623033932351,
-                    "velocityY": -2.8504186824445474,
-                    "timestamp": 1.3845992916150864
-                },
-                {
-                    "x": 1.9652989688718845,
-                    "y": 4.8044863939922715,
-                    "heading": -0.26949747833980514,
-                    "angularVelocity": -0.7890878177691865,
-                    "velocityX": 0.9364533146403533,
-                    "velocityY": -2.7337271733418085,
-                    "timestamp": 1.4520955189559595
-                },
-                {
-                    "x": 2.0553904334698565,
-                    "y": 4.6466969161112965,
-                    "heading": -0.31411027699435934,
-                    "angularVelocity": -0.6609672927236123,
-                    "velocityX": 1.3347629659801235,
-                    "velocityY": -2.337752554436838,
-                    "timestamp": 1.5195917462968325
-                },
-                {
-                    "x": 2.145781200762326,
-                    "y": 4.526760098072049,
-                    "heading": -0.34739291091529667,
-                    "angularVelocity": -0.49310361826375293,
-                    "velocityX": 1.3391973278146792,
-                    "velocityY": -1.7769410641803345,
-                    "timestamp": 1.5870879736377055
-                },
-                {
-                    "x": 2.220455144634577,
-                    "y": 4.441296194130274,
-                    "heading": -0.37087004938287466,
-                    "angularVelocity": -0.3478288993696278,
-                    "velocityX": 1.1063424848196184,
-                    "velocityY": -1.2662026798943848,
-                    "timestamp": 1.6545842009785785
-                },
-                {
-                    "x": 2.2728997785605394,
-                    "y": 4.3865419949716715,
-                    "heading": -0.3858025738529217,
-                    "angularVelocity": -0.22123494983851594,
-                    "velocityX": 0.7770009671963372,
-                    "velocityY": -0.8112186608901896,
-                    "timestamp": 1.7220804283194515
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.393,
-                    "angularVelocity": -0.1066344954471229,
-                    "velocityX": 0.4015072028040527,
-                    "velocityY": -0.3932367188113757,
-                    "timestamp": 1.7895766556603245
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.393,
-                    "angularVelocity": 8.903141052781706e-32,
-                    "velocityX": 0,
-                    "velocityY": -2.026142404451774e-33,
-                    "timestamp": 1.8570728830011975
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "S1 To C1": {
-            "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.26,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 13
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": -0.003,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.26,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.238209882810899,
-                    "y": 6.650800869780916,
-                    "heading": 0.2470077179050236,
-                    "angularVelocity": -0.14636941117769697,
-                    "velocityX": 0.7008486923098889,
-                    "velocityY": 0.23433997503541223,
-                    "timestamp": 0.08876364255722656
-                },
-                {
-                    "x": 5.362652939631404,
-                    "y": 6.692404642961414,
-                    "heading": 0.22258646311132924,
-                    "angularVelocity": -0.2751267759001645,
-                    "velocityX": 1.401959780326283,
-                    "velocityY": 0.4687028605622118,
-                    "timestamp": 0.17752728511445312
-                },
-                {
-                    "x": 5.549362223961953,
-                    "y": 6.754812642679732,
-                    "heading": 0.18937073322571307,
-                    "angularVelocity": -0.37420422291202055,
-                    "velocityX": 2.1034432449090223,
-                    "velocityY": 0.7030806523975865,
-                    "timestamp": 0.26629092767167967
-                },
-                {
-                    "x": 5.798381050208957,
-                    "y": 6.8380191875714225,
-                    "heading": 0.15275465831989177,
-                    "angularVelocity": -0.4125120809759026,
-                    "velocityX": 2.8054146841303256,
-                    "velocityY": 0.9373944386978721,
-                    "timestamp": 0.35505457022890624
-                },
-                {
-                    "x": 6.109622945204874,
-                    "y": 6.941929047341865,
-                    "heading": 0.1302624797347834,
-                    "angularVelocity": -0.25339404667863696,
-                    "velocityX": 3.506411927553799,
-                    "velocityY": 1.1706353725121932,
-                    "timestamp": 0.4438182127861328
-                },
-                {
-                    "x": 6.4280450770381,
-                    "y": 7.048515369175043,
-                    "heading": 0.1302624686930749,
-                    "angularVelocity": -1.2439475582812342e-7,
-                    "velocityX": 3.587303570016191,
-                    "velocityY": 1.2007880564873126,
-                    "timestamp": 0.5325818553433593
-                },
-                {
-                    "x": 6.746467208870004,
-                    "y": 7.1551016910121685,
-                    "heading": 0.13026245765136438,
-                    "angularVelocity": -1.2439477842907252e-7,
-                    "velocityX": 3.5873035700012967,
-                    "velocityY": 1.2007880565318134,
-                    "timestamp": 0.6213454979005859
-                },
-                {
-                    "x": 7.057671454315644,
-                    "y": 7.258999412253756,
-                    "heading": 0.10797351007767278,
-                    "angularVelocity": -0.2511044717394208,
-                    "velocityX": 3.5059877724700192,
-                    "velocityY": 1.1704986213770205,
-                    "timestamp": 0.7101091404578125
-                },
-                {
-                    "x": 7.3066745305511045,
-                    "y": 7.342200909102172,
-                    "heading": 0.07025614763700679,
-                    "angularVelocity": -0.4249190474112408,
-                    "velocityX": 2.8052372464872497,
-                    "velocityY": 0.9373375680789704,
-                    "timestamp": 0.7988727830150391
-                },
-                {
-                    "x": 7.49336787770315,
-                    "y": 7.4046033027166684,
-                    "heading": 0.03585076588111199,
-                    "angularVelocity": -0.3876066908033862,
-                    "velocityX": 2.103263698666567,
-                    "velocityY": 0.7030174947390144,
-                    "timestamp": 0.8876364255722656
-                },
-                {
-                    "x": 7.617797738081694,
-                    "y": 7.446202129881464,
-                    "heading": 0.010501179707511226,
-                    "angularVelocity": -0.2855852401136722,
-                    "velocityX": 1.4018111108759532,
-                    "velocityY": 0.4686471393732002,
-                    "timestamp": 0.9764000681294922
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": -0.003,
-                    "angularVelocity": -0.15210258748116404,
-                    "velocityX": 0.7007628362953491,
-                    "velocityY": 0.23430618121741137,
-                    "timestamp": 1.0651637106867187
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": -0.003,
-                    "angularVelocity": -3.7910464085672074e-31,
-                    "velocityX": 1.2889893239032156e-32,
-                    "velocityY": 1.1630630989344696e-31,
-                    "timestamp": 1.1539273532439451
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "S1 To C2": {
-            "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.26,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 13
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": -0.003,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.26,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.242063425541118,
-                    "y": 6.625917379024557,
-                    "heading": 0.21680591849920633,
-                    "angularVelocity": -0.48090771817524164,
-                    "velocityX": 0.735526954803639,
-                    "velocityY": -0.04545446665971497,
-                    "timestamp": 0.08981781715770647
-                },
-                {
-                    "x": 5.374390185654174,
-                    "y": 6.616899293663565,
-                    "heading": 0.13592925245739512,
-                    "angularVelocity": -0.9004523668149724,
-                    "velocityX": 1.473279626476675,
-                    "velocityY": -0.10040419202302918,
-                    "timestamp": 0.17963563431541293
-                },
-                {
-                    "x": 5.573225936717345,
-                    "y": 6.601485332040172,
-                    "heading": 0.027174297785337913,
-                    "angularVelocity": -1.2108394315695736,
-                    "velocityX": 2.2137673499017074,
-                    "velocityY": -0.1716136298027423,
-                    "timestamp": 0.2694534514731194
-                },
-                {
-                    "x": 5.8386201791233034,
-                    "y": 6.576536765329541,
-                    "heading": -0.0884788299359431,
-                    "angularVelocity": -1.287641265186969,
-                    "velocityX": 2.9548061932964313,
-                    "velocityY": -0.27776857087079165,
-                    "timestamp": 0.35927126863082587
-                },
-                {
-                    "x": 6.16462893780028,
-                    "y": 6.53182752327052,
-                    "heading": -0.12460373299787371,
-                    "angularVelocity": -0.4022019706680331,
-                    "velocityX": 3.6296669078981743,
-                    "velocityY": -0.49777698316268804,
-                    "timestamp": 0.44908908578853235
-                },
-                {
-                    "x": 6.485747891959683,
-                    "y": 6.420787077377064,
-                    "heading": -0.12460373462910332,
-                    "angularVelocity": -1.816153684215185e-8,
-                    "velocityX": 3.5752255434527758,
-                    "velocityY": -1.2362852873443355,
-                    "timestamp": 0.5389069029462388
-                },
-                {
-                    "x": 6.7868856142210525,
-                    "y": 6.263423057279399,
-                    "heading": -0.12460372221510443,
-                    "angularVelocity": 1.382130993675135e-7,
-                    "velocityX": 3.352761532075727,
-                    "velocityY": -1.7520356770790513,
-                    "timestamp": 0.6287247201039452
-                },
-                {
-                    "x": 7.0844936481565455,
-                    "y": 6.108042080528118,
-                    "heading": -0.11377963349551397,
-                    "angularVelocity": 0.12051159850149781,
-                    "velocityX": 3.313463223148008,
-                    "velocityY": -1.7299571696165401,
-                    "timestamp": 0.7185425372616516
-                },
-                {
-                    "x": 7.322771112714566,
-                    "y": 5.983596379959823,
-                    "heading": -0.07830444960996434,
-                    "angularVelocity": 0.39496822577262786,
-                    "velocityX": 2.652897521876331,
-                    "velocityY": -1.3855346801602506,
-                    "timestamp": 0.808360354419358
-                },
-                {
-                    "x": 7.501421172621153,
-                    "y": 5.8902827497271355,
-                    "heading": -0.04357887494407857,
-                    "angularVelocity": 0.3866223402525239,
-                    "velocityX": 1.9890269610193678,
-                    "velocityY": -1.038921153793384,
-                    "timestamp": 0.8981781715770645
-                },
-                {
-                    "x": 7.62048387761172,
-                    "y": 5.828089668977501,
-                    "heading": -0.01724918093308134,
-                    "angularVelocity": 0.29314555668577735,
-                    "velocityX": 1.3256022998366965,
-                    "velocityY": -0.6924358965486016,
-                    "timestamp": 0.9879959887347709
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": -0.003,
-                    "angularVelocity": 0.15864537108558247,
-                    "velocityX": 0.6626315832612425,
-                    "velocityY": -0.346141444552285,
-                    "timestamp": 1.0778138058924773
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": -0.003,
-                    "angularVelocity": 0,
-                    "velocityX": 4.386752331989148e-33,
-                    "velocityY": -2.33652586669764e-33,
-                    "timestamp": 1.1676316230501838
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "S1 To C3": {
-            "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.26,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 16
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": -0.003,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.26,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.246662494261999,
-                    "y": 6.628692732737936,
-                    "heading": 0.18398762968545185,
-                    "angularVelocity": -0.8088463398355284,
-                    "velocityX": 0.7519183997414132,
-                    "velocityY": -0.013910608703967415,
-                    "timestamp": 0.09397628025368156
-                },
-                {
-                    "x": 5.3885394117424426,
-                    "y": 6.6228569584156665,
-                    "heading": 0.04133864379940616,
-                    "angularVelocity": -1.5179254328962157,
-                    "velocityX": 1.509709866122152,
-                    "velocityY": -0.0620983753189278,
-                    "timestamp": 0.18795256050736311
-                },
-                {
-                    "x": 5.602094427902648,
-                    "y": 6.606624752166298,
-                    "heading": -0.14949344735311892,
-                    "angularVelocity": -2.0306410366252936,
-                    "velocityX": 2.272435295201409,
-                    "velocityY": -0.172726630651381,
-                    "timestamp": 0.2819288407610447
-                },
-                {
-                    "x": 5.885007714232348,
-                    "y": 6.565697735942791,
-                    "heading": -0.3456589598447775,
-                    "angularVelocity": -2.087393882393782,
-                    "velocityX": 3.010475468554385,
-                    "velocityY": -0.4355036836213049,
-                    "timestamp": 0.37590512101472623
-                },
-                {
-                    "x": 6.196762938930662,
-                    "y": 6.464967292979978,
-                    "heading": -0.4305176558630451,
-                    "angularVelocity": -0.9029799412064233,
-                    "velocityX": 3.3173820442430326,
-                    "velocityY": -1.0718709305252299,
-                    "timestamp": 0.4698814012684078
-                },
-                {
-                    "x": 6.507653773495931,
-                    "y": 6.294193106326001,
-                    "heading": -0.4331106719684992,
-                    "angularVelocity": -0.027592240280786797,
-                    "velocityX": 3.3081840835372915,
-                    "velocityY": -1.8172052159649787,
-                    "timestamp": 0.5638576815220894
-                },
-                {
-                    "x": 6.777249226823148,
-                    "y": 6.0624534109685,
-                    "heading": -0.433110646067504,
-                    "angularVelocity": 2.7561204856502443e-7,
-                    "velocityX": 2.8687606340606973,
-                    "velocityY": -2.465938157287546,
-                    "timestamp": 0.6578339617757709
-                },
-                {
-                    "x": 6.99331744885121,
-                    "y": 5.780142381909384,
-                    "heading": -0.4331105937998759,
-                    "angularVelocity": 5.561789419211182e-7,
-                    "velocityX": 2.2991782761011916,
-                    "velocityY": -3.004066859180208,
-                    "timestamp": 0.7518102420294525
-                },
-                {
-                    "x": 7.1465883664001515,
-                    "y": 5.459372959434879,
-                    "heading": -0.43311054984028735,
-                    "angularVelocity": 4.677732342413875e-7,
-                    "velocityX": 1.6309532270823943,
-                    "velocityY": -3.4133019694822235,
-                    "timestamp": 0.845786522283134
-                },
-                {
-                    "x": 7.2797482024400875,
-                    "y": 5.1297467242247805,
-                    "heading": -0.4331105067287741,
-                    "angularVelocity": 4.5874887908824285e-7,
-                    "velocityX": 1.4169515507581365,
-                    "velocityY": -3.507547163180951,
-                    "timestamp": 0.9397628025368155
-                },
-                {
-                    "x": 7.407925031698652,
-                    "y": 4.799268903667217,
-                    "heading": -0.4305589051420175,
-                    "angularVelocity": 0.027151549091629798,
-                    "velocityX": 1.3639274603395894,
-                    "velocityY": -3.5166088683810885,
-                    "timestamp": 1.033739082790497
-                },
-                {
-                    "x": 7.517390810231398,
-                    "y": 4.529437032753176,
-                    "heading": -0.29559925755962413,
-                    "angularVelocity": 1.4361033147734772,
-                    "velocityX": 1.164823487770018,
-                    "velocityY": -2.871276349581525,
-                    "timestamp": 1.1277153630441787
-                },
-                {
-                    "x": 7.59899827381295,
-                    "y": 4.327770783855267,
-                    "heading": -0.16069313013903275,
-                    "angularVelocity": 1.4355338076419173,
-                    "velocityX": 0.8683836321384404,
-                    "velocityY": -2.1459271249460685,
-                    "timestamp": 1.2216916432978604
-                },
-                {
-                    "x": 7.653087401223331,
-                    "y": 4.193799869992724,
-                    "heading": -0.05829734753961687,
-                    "angularVelocity": 1.0895917812772173,
-                    "velocityX": 0.5755614849233384,
-                    "velocityY": -1.4255822160751557,
-                    "timestamp": 1.315667923551542
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": -0.003,
-                    "angularVelocity": 0.5884181347713066,
-                    "velocityX": 0.28637650590148805,
-                    "velocityY": -0.7108162805806236,
-                    "timestamp": 1.4096442038052237
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": -0.003,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 2.8051389340682807e-33,
-                    "timestamp": 1.5036204840589054
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "S1 To C4": {
-            "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.26,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 11
-                },
-                {
-                    "x": 6.950631618499756,
-                    "y": 6.338046073913574,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 15
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": -0.003,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.26,
-                    "angularVelocity": 0,
-                    "velocityX": -1.7123315632321391e-31,
-                    "velocityY": 4.571975133912053e-33,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.2081944432473115,
-                    "y": 6.63354239454729,
-                    "heading": 0.2524699257138384,
-                    "angularVelocity": -0.12069194203891866,
-                    "velocityX": 0.5160121574790498,
-                    "velocityY": 0.0567774581143745,
-                    "timestamp": 0.062390861883169
-                },
-                {
-                    "x": 5.2726441787772975,
-                    "y": 6.640106281427696,
-                    "heading": 0.23785051964847434,
-                    "angularVelocity": -0.2343196683632906,
-                    "velocityX": 1.0329996025807835,
-                    "velocityY": 0.1052059016703473,
-                    "timestamp": 0.124781723766338
-                },
-                {
-                    "x": 5.369418980987113,
-                    "y": 6.648966513737627,
-                    "heading": 0.21675536507543205,
-                    "angularVelocity": -0.33811288923278293,
-                    "velocityX": 1.551105390898958,
-                    "velocityY": 0.14201169918963874,
-                    "timestamp": 0.187172585649507
-                },
-                {
-                    "x": 5.498590594297912,
-                    "y": 6.659046571836944,
-                    "heading": 0.19009632617593986,
-                    "angularVelocity": -0.42729076173705777,
-                    "velocityX": 2.070361097955043,
-                    "velocityY": 0.16156305258605722,
-                    "timestamp": 0.249563447532676
-                },
-                {
-                    "x": 5.6601948909824875,
-                    "y": 6.668590441412665,
-                    "heading": 0.15936442465931713,
-                    "angularVelocity": -0.49257055583183496,
-                    "velocityX": 2.5901917653772815,
-                    "velocityY": 0.15296902924013772,
-                    "timestamp": 0.311954309415845
-                },
-                {
-                    "x": 5.854024302844021,
-                    "y": 6.674275452040902,
-                    "heading": 0.12740102584180835,
-                    "angularVelocity": -0.512308980077189,
-                    "velocityX": 3.106695532183737,
-                    "velocityY": 0.09111928344386595,
-                    "timestamp": 0.374345171299014
-                },
-                {
-                    "x": 6.078018113514707,
-                    "y": 6.668089747759068,
-                    "heading": 0.10126004011963459,
-                    "angularVelocity": -0.4189874115076864,
-                    "velocityX": 3.590170161299073,
-                    "velocityY": -0.09914439543117176,
-                    "timestamp": 0.436736033182183
-                },
-                {
-                    "x": 6.311203816285634,
-                    "y": 6.63161682590639,
-                    "heading": 0.10125992071184643,
-                    "angularVelocity": -0.000001913866624365403,
-                    "velocityX": 3.737497699704501,
-                    "velocityY": -0.584587562213517,
-                    "timestamp": 0.499126895065352
-                },
-                {
-                    "x": 6.53718081427167,
-                    "y": 6.563497401833266,
-                    "heading": 0.10125989664992657,
-                    "angularVelocity": -3.856641685769623e-7,
-                    "velocityX": 3.621956664249834,
-                    "velocityY": -1.0918173273625358,
-                    "timestamp": 0.561517756948521
-                },
-                {
-                    "x": 6.7516755548198235,
-                    "y": 6.465019697743108,
-                    "heading": 0.10125985817262735,
-                    "angularVelocity": -6.167136938553595e-7,
-                    "velocityX": 3.4379191771674527,
-                    "velocityY": -1.5783994821960954,
-                    "timestamp": 0.62390861883169
-                },
-                {
-                    "x": 6.950631618499756,
-                    "y": 6.338046073913574,
-                    "heading": 0.10125971935519859,
-                    "angularVelocity": -0.000002224964167076665,
-                    "velocityX": 3.1888654471946203,
-                    "velocityY": -2.035131748416977,
-                    "timestamp": 0.686299480714859
-                },
-                {
-                    "x": 7.196058677643184,
-                    "y": 6.101503366378845,
-                    "heading": 0.10125966337035743,
-                    "angularVelocity": -6.213285755242982e-7,
-                    "velocityX": 2.723788105663008,
-                    "velocityY": -2.625188174087521,
-                    "timestamp": 0.7764045291337008
-                },
-                {
-                    "x": 7.389899318639429,
-                    "y": 5.821123760827136,
-                    "heading": 0.10125964077104323,
-                    "angularVelocity": -2.508107435019506e-7,
-                    "velocityX": 2.1512739230237647,
-                    "velocityY": -3.1116969633976366,
-                    "timestamp": 0.8665095775525427
-                },
-                {
-                    "x": 7.524507781478929,
-                    "y": 5.507966472295818,
-                    "heading": 0.101259626680438,
-                    "angularVelocity": -1.5637975290365853e-7,
-                    "velocityX": 1.4939058931947202,
-                    "velocityY": -3.475468844716046,
-                    "timestamp": 0.9566146259713845
-                },
-                {
-                    "x": 7.594574766806078,
-                    "y": 5.174383579102459,
-                    "heading": 0.10125961555168732,
-                    "angularVelocity": -1.235086255399969e-7,
-                    "velocityX": 0.7776144240159958,
-                    "velocityY": -3.702155418004344,
-                    "timestamp": 1.0467196743902263
-                },
-                {
-                    "x": 7.605242469312774,
-                    "y": 4.833688510035603,
-                    "heading": 0.10125960514477245,
-                    "angularVelocity": -1.1549757817967306e-7,
-                    "velocityX": 0.11839184034432584,
-                    "velocityY": -3.781087464524514,
-                    "timestamp": 1.136824722809068
-                },
-                {
-                    "x": 7.615908675824772,
-                    "y": 4.492993394135096,
-                    "heading": 0.10125959473787158,
-                    "angularVelocity": -1.1549742264017462e-7,
-                    "velocityX": 0.11837523756082047,
-                    "velocityY": -3.781087984291716,
-                    "timestamp": 1.2269297712279097
-                },
-                {
-                    "x": 7.6265748461946865,
-                    "y": 4.152298277103123,
-                    "heading": 0.10125958433097336,
-                    "angularVelocity": -1.1549739343149193e-7,
-                    "velocityX": 0.11837483645025074,
-                    "velocityY": -3.781087996848918,
-                    "timestamp": 1.3170348196467514
-                },
-                {
-                    "x": 7.637240986091898,
-                    "y": 3.8116031591171406,
-                    "heading": 0.10125957392407539,
-                    "angularVelocity": -1.1549739036497336e-7,
-                    "velocityX": 0.11837449825941278,
-                    "velocityY": -3.78108800743665,
-                    "timestamp": 1.407139868065593
-                },
-                {
-                    "x": 7.647907101492937,
-                    "y": 3.470908040364265,
-                    "heading": 0.10125956351717343,
-                    "angularVelocity": -1.1549743491773867e-7,
-                    "velocityX": 0.11837422639702805,
-                    "velocityY": -3.781088015947766,
-                    "timestamp": 1.4972449164844348
-                },
-                {
-                    "x": 7.658649980665503,
-                    "y": 3.1330359890940187,
-                    "heading": 0.09207038783752043,
-                    "angularVelocity": -0.10198291706072676,
-                    "velocityX": 0.11922616280763662,
-                    "velocityY": -3.7497571689844853,
-                    "timestamp": 1.5873499649032765
-                },
-                {
-                    "x": 7.667206120655169,
-                    "y": 2.862563871388905,
-                    "heading": 0.061604548821908564,
-                    "angularVelocity": -0.33811467337536444,
-                    "velocityX": 0.09495738740298482,
-                    "velocityY": -3.0017421049246735,
-                    "timestamp": 1.6774550133221182
-                },
-                {
-                    "x": 7.673608699461006,
-                    "y": 2.659753859006467,
-                    "heading": 0.031813701428875164,
-                    "angularVelocity": -0.33062351017837394,
-                    "velocityX": 0.07105682665061783,
-                    "velocityY": -2.250817417462574,
-                    "timestamp": 1.76756006174096
-                },
-                {
-                    "x": 7.677870998153504,
-                    "y": 2.524576540197495,
-                    "heading": 0.009226184845951672,
-                    "angularVelocity": -0.25067981183391225,
-                    "velocityX": 0.04730366130750142,
-                    "velocityY": -1.5002191462193974,
-                    "timestamp": 1.8576651101598016
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": -0.003,
-                    "angularVelocity": -0.1356881224803119,
-                    "velocityX": 0.023627997363707604,
-                    "velocityY": -0.7499750722442785,
-                    "timestamp": 1.9477701585786433
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": -0.003,
-                    "angularVelocity": 0,
-                    "velocityX": -1.899719241029588e-32,
-                    "velocityY": -4.254972095731821e-34,
-                    "timestamp": 2.037875206997485
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "S1 To C5": {
-            "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.26,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 12
-                },
-                {
-                    "x": 7.0632171630859375,
-                    "y": 6.06283712387085,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 19
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.26,
-                    "angularVelocity": -8.010592837331029e-32,
-                    "velocityX": 9.396271853657066e-34,
-                    "velocityY": -2.4298772375808502e-33,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.207063492876026,
-                    "y": 6.6289176408913075,
-                    "heading": 0.24374543730705361,
-                    "angularVelocity": -0.264834788594711,
-                    "velocityX": 0.5061159579768882,
-                    "velocityY": -0.01763482359684202,
-                    "timestamp": 0.06137623678217253
-                },
-                {
-                    "x": 5.269220913885746,
-                    "y": 6.626580685647671,
-                    "heading": 0.2121773997323976,
-                    "angularVelocity": -0.514336479877237,
-                    "velocityX": 1.0127277961065038,
-                    "velocityY": -0.03807589657100854,
-                    "timestamp": 0.12275247356434506
-                },
-                {
-                    "x": 5.36250981468421,
-                    "y": 6.6227475288112485,
-                    "heading": 0.16662318019476377,
-                    "angularVelocity": -0.7422126530713462,
-                    "velocityX": 1.5199514615005028,
-                    "velocityY": -0.06245343535846346,
-                    "timestamp": 0.1841287103465176
-                },
-                {
-                    "x": 5.486975524936631,
-                    "y": 6.617054798693029,
-                    "heading": 0.10910232843404971,
-                    "angularVelocity": -0.9371844019185364,
-                    "velocityX": 2.0279136808950478,
-                    "velocityY": -0.0927513711605005,
-                    "timestamp": 0.24550494712869012
-                },
-                {
-                    "x": 5.642662315095453,
-                    "y": 6.608892934358269,
-                    "heading": 0.04300979987381644,
-                    "angularVelocity": -1.076842309423355,
-                    "velocityX": 2.536597196588689,
-                    "velocityY": -0.1329808532205177,
-                    "timestamp": 0.30688118391086266
-                },
-                {
-                    "x": 5.829535978248522,
-                    "y": 6.597025931717244,
-                    "heading": -0.024847542112422006,
-                    "angularVelocity": -1.1055963275666998,
-                    "velocityX": 3.0447233807490215,
-                    "velocityY": -0.19334848897856358,
-                    "timestamp": 0.3682574206930352
-                },
-                {
-                    "x": 6.046434546533161,
-                    "y": 6.5777851666622515,
-                    "heading": -0.07267029178585922,
-                    "angularVelocity": -0.7791737027338695,
-                    "velocityX": 3.533917679808606,
-                    "velocityY": -0.31348883645767445,
-                    "timestamp": 0.4296336574752077
-                },
-                {
-                    "x": 6.274079716361941,
-                    "y": 6.532107529947078,
-                    "heading": -0.07267036448034916,
-                    "angularVelocity": -0.0000011844077407082938,
-                    "velocityX": 3.709011528952235,
-                    "velocityY": -0.7442234830605138,
-                    "timestamp": 0.49100989425738023
-                },
-                {
-                    "x": 6.493476634626264,
-                    "y": 6.456122214314626,
-                    "heading": -0.07267035803800323,
-                    "angularVelocity": 1.049648246960038e-7,
-                    "velocityX": 3.5746231728571716,
-                    "velocityY": -1.238025001469686,
-                    "timestamp": 0.5523861310395528
-                },
-                {
-                    "x": 6.700610073343003,
-                    "y": 6.351219816335689,
-                    "heading": -0.07267035070446132,
-                    "angularVelocity": 1.1948503655702063e-7,
-                    "velocityX": 3.3748149051866063,
-                    "velocityY": -1.7091695985083282,
-                    "timestamp": 0.6137623678217253
-                },
-                {
-                    "x": 6.891689244621814,
-                    "y": 6.21932014936447,
-                    "heading": -0.07267034149477326,
-                    "angularVelocity": 1.5005299351249212e-7,
-                    "velocityX": 3.1132435173071933,
-                    "velocityY": -2.1490347712151214,
-                    "timestamp": 0.6751386046038979
-                },
-                {
-                    "x": 7.0632171630859375,
-                    "y": 6.06283712387085,
-                    "heading": -0.07267032569899272,
-                    "angularVelocity": 2.573598736854518e-7,
-                    "velocityX": 2.7946959190881038,
-                    "velocityY": -2.549570219643589,
-                    "timestamp": 0.7365148413860704
-                },
-                {
-                    "x": 7.263317671759933,
-                    "y": 5.7907628781558955,
-                    "heading": -0.07267031513885969,
-                    "angularVelocity": 1.182832965996254e-7,
-                    "velocityX": 2.24131152203967,
-                    "velocityY": -3.0474842158680824,
-                    "timestamp": 0.8257931530827405
-                },
-                {
-                    "x": 7.406264443711533,
-                    "y": 5.484771313458312,
-                    "heading": -0.07267030869611313,
-                    "angularVelocity": 7.216474459034548e-8,
-                    "velocityX": 1.6011365944875051,
-                    "velocityY": -3.4273896860551396,
-                    "timestamp": 0.9150714647794106
-                },
-                {
-                    "x": 7.486522259331217,
-                    "y": 5.1567114044359705,
-                    "heading": -0.07267030368970051,
-                    "angularVelocity": 5.607647052205946e-8,
-                    "velocityX": 0.8989620669840581,
-                    "velocityY": -3.6745756364317224,
-                    "timestamp": 1.0043497764760807
-                },
-                {
-                    "x": 7.50149841248159,
-                    "y": 4.81930906784098,
-                    "heading": -0.07267029905812865,
-                    "angularVelocity": 5.187790600157932e-8,
-                    "velocityX": 0.1677468230047605,
-                    "velocityY": -3.7792195011633143,
-                    "timestamp": 1.093628088172751
-                },
-                {
-                    "x": 7.516449925771596,
-                    "y": 4.481905638472984,
-                    "heading": -0.0726702944266145,
-                    "angularVelocity": 5.1877259457701335e-8,
-                    "velocityX": 0.16747083368699095,
-                    "velocityY": -3.7792317412357557,
-                    "timestamp": 1.1829063998694211
-                },
-                {
-                    "x": 7.531401402222247,
-                    "y": 4.144502207472513,
-                    "heading": -0.07267028979509418,
-                    "angularVelocity": 5.187732873980297e-8,
-                    "velocityX": 0.16747042105201052,
-                    "velocityY": -3.779231759520992,
-                    "timestamp": 1.2721847115660914
-                },
-                {
-                    "x": 7.546352847973806,
-                    "y": 3.807098775111667,
-                    "heading": -0.07267028516356967,
-                    "angularVelocity": 5.1877375603275234e-8,
-                    "velocityX": 0.16747007719365256,
-                    "velocityY": -3.7792317747584647,
-                    "timestamp": 1.3614630232627616
-                },
-                {
-                    "x": 7.561304268557873,
-                    "y": 3.4696953416355707,
-                    "heading": -0.07267028053204275,
-                    "angularVelocity": 5.1877402813012475e-8,
-                    "velocityX": 0.16746979529438435,
-                    "velocityY": -3.7792317872503007,
-                    "timestamp": 1.4507413349594318
-                },
-                {
-                    "x": 7.576255668582546,
-                    "y": 3.132291907248425,
-                    "heading": -0.07267027590051456,
-                    "angularVelocity": 5.1877416880821484e-8,
-                    "velocityX": 0.16746956501007867,
-                    "velocityY": -3.779231797454896,
-                    "timestamp": 1.540019646656102
-                },
-                {
-                    "x": 7.591207051730301,
-                    "y": 2.794888472113414,
-                    "heading": -0.07267027126898577,
-                    "angularVelocity": 5.1877423431260937e-8,
-                    "velocityX": 0.16746937597291248,
-                    "velocityY": -3.779231805831691,
-                    "timestamp": 1.6292979583527722
-                },
-                {
-                    "x": 7.60615842090353,
-                    "y": 2.457485036359151,
-                    "heading": -0.07267026663745696,
-                    "angularVelocity": 5.1877423851232326e-8,
-                    "velocityX": 0.16746921944524512,
-                    "velocityY": -3.779231812767884,
-                    "timestamp": 1.7185762700494425
-                },
-                {
-                    "x": 7.621109778388665,
-                    "y": 2.1200816000869556,
-                    "heading": -0.07267026200592838,
-                    "angularVelocity": 5.187742117887202e-8,
-                    "velocityX": 0.1674690885277524,
-                    "velocityY": -3.779231818569208,
-                    "timestamp": 1.8078545817461127
-                },
-                {
-                    "x": 7.636061125621916,
-                    "y": 1.7826781633604734,
-                    "heading": -0.07267025737439563,
-                    "angularVelocity": 5.1877468094637746e-8,
-                    "velocityX": 0.16746897369713376,
-                    "velocityY": -3.7792318236576405,
-                    "timestamp": 1.897132893442783
-                },
-                {
-                    "x": 7.6506850878742485,
-                    "y": 1.4507667606163608,
-                    "heading": -0.05536549896524321,
-                    "angularVelocity": 0.19382936438075055,
-                    "velocityX": 0.16380195788222543,
-                    "velocityY": -3.7177159428351065,
-                    "timestamp": 1.9864112051394531
-                },
-                {
-                    "x": 7.662404359721407,
-                    "y": 1.1852476637582392,
-                    "heading": -0.0353858682020255,
-                    "angularVelocity": 0.2237904187872895,
-                    "velocityX": 0.13126672788097452,
-                    "velocityY": -2.974060461181691,
-                    "timestamp": 2.0756895168361233
-                },
-                {
-                    "x": 7.6711999602966845,
-                    "y": 0.9861189087552604,
-                    "heading": -0.018423007440779238,
-                    "angularVelocity": 0.18999979321832794,
-                    "velocityX": 0.0985188945458782,
-                    "velocityY": -2.2304269784976873,
-                    "timestamp": 2.1649678285327933
-                },
-                {
-                    "x": 7.677066105120016,
-                    "y": 0.8533716446291538,
-                    "heading": -0.006322956528169105,
-                    "angularVelocity": 0.13553180702747777,
-                    "velocityX": 0.06570626966225444,
-                    "velocityY": -1.4868926349898441,
-                    "timestamp": 2.2542461402294633
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": -2.444948224573009e-31,
-                    "angularVelocity": 0.0708229849781762,
-                    "velocityX": 0.03286234724008497,
-                    "velocityY": -0.7434240563896022,
-                    "timestamp": 2.3435244519261333
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "angularVelocity": 1.883272219480887e-30,
-                    "velocityX": 0,
-                    "velocityY": -3.281240907044904e-32,
-                    "timestamp": 2.4328027636228033
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "S2 To W1": {
-            "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 13
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 8.330960047205791e-35,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.215224456008129,
-                    "y": 5.5891314798928855,
-                    "heading": 0.013109691961439362,
-                    "angularVelocity": 0.15985781811927008,
-                    "velocityX": -0.5825685488618567,
-                    "velocityY": 0.35522534228568337,
-                    "timestamp": 0.08200845048227925
-                },
-                {
-                    "x": 4.11967337131145,
-                    "y": 5.647394543004508,
-                    "heading": 0.03932481805987993,
-                    "angularVelocity": 0.31966371690079975,
-                    "velocityX": -1.1651370576417237,
-                    "velocityY": 0.7104519445128596,
-                    "timestamp": 0.1640169009645585
-                },
-                {
-                    "x": 3.976346757700282,
-                    "y": 5.734789362523958,
-                    "heading": 0.07863903763442207,
-                    "angularVelocity": 0.47939229851730114,
-                    "velocityX": -1.7477054226519961,
-                    "velocityY": 1.0656806585844962,
-                    "timestamp": 0.24602535144683776
-                },
-                {
-                    "x": 3.7852446629452063,
-                    "y": 5.851316183985278,
-                    "heading": 0.13104540832787342,
-                    "angularVelocity": 0.6390362259652201,
-                    "velocityX": -2.3302732051542674,
-                    "velocityY": 1.4209123666652002,
-                    "timestamp": 0.328033801929117
-                },
-                {
-                    "x": 3.5463672091086966,
-                    "y": 5.996975313640393,
-                    "heading": 0.19653856458379848,
-                    "angularVelocity": 0.7986147265406159,
-                    "velocityX": -2.912839499243156,
-                    "velocityY": 1.77614780914058,
-                    "timestamp": 0.41004225241139625
-                },
-                {
-                    "x": 3.281494446824011,
-                    "y": 6.15848998396632,
-                    "heading": 0.19653857286419524,
-                    "angularVelocity": 1.009700426817188e-7,
-                    "velocityX": -3.229822789322431,
-                    "velocityY": 1.9694881366015824,
-                    "timestamp": 0.4920507028936755
-                },
-                {
-                    "x": 3.016621684855716,
-                    "y": 6.320004654811106,
-                    "heading": 0.1965385811445921,
-                    "angularVelocity": 1.0097004416983622e-7,
-                    "velocityX": -3.229822785464406,
-                    "velocityY": 1.969488142928474,
-                    "timestamp": 0.5740591533759547
-                },
-                {
-                    "x": 2.77774755563319,
-                    "y": 6.465669559960414,
-                    "heading": 0.2620174097526663,
-                    "angularVelocity": 0.7984400171324189,
-                    "velocityX": -2.9127989593480157,
-                    "velocityY": 1.7762182347389206,
-                    "timestamp": 0.6560676038582339
-                },
-                {
-                    "x": 2.5866484397575076,
-                    "y": 6.582201656937504,
-                    "heading": 0.3144069758542471,
-                    "angularVelocity": 0.6388313130352502,
-                    "velocityX": -2.330236881100143,
-                    "velocityY": 1.4209766955939638,
-                    "timestamp": 0.7380760543405132
-                },
-                {
-                    "x": 2.443324199843316,
-                    "y": 6.6696008076986475,
-                    "heading": 0.35370275100271265,
-                    "angularVelocity": 0.4791673896698825,
-                    "velocityX": -1.7476764781107736,
-                    "velocityY": 1.0657334731623573,
-                    "timestamp": 0.8200845048227924
-                },
-                {
-                    "x": 2.347774735979553,
-                    "y": 6.727866934294423,
-                    "heading": 0.37990105353306003,
-                    "angularVelocity": 0.3194585725773273,
-                    "velocityX": -1.1651172934234333,
-                    "velocityY": 0.7104893002260324,
-                    "timestamp": 0.9020929553050716
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "angularVelocity": 0.15972678900658524,
-                    "velocityX": -0.5825586960684825,
-                    "velocityY": 0.3552446794720561,
-                    "timestamp": 0.9841014057873508
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "angularVelocity": -4.59054414969708e-34,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 1.06610985626963
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "S2 To W2": {
-            "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 10
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.181096344253034,
-                    "y": 5.56,
-                    "heading": 2.5710705899848466e-19,
-                    "angularVelocity": 2.5941602968880693e-18,
-                    "velocityX": -0.8263919813283321,
-                    "velocityY": -1.4479549544654444e-34,
-                    "timestamp": 0.099109935233537
-                },
-                {
-                    "x": 4.017289035836238,
-                    "y": 5.56,
-                    "heading": 7.7182898862159e-19,
-                    "angularVelocity": 5.193444314378615e-18,
-                    "velocityX": -1.6527839316089838,
-                    "velocityY": -7.36495654370163e-34,
-                    "timestamp": 0.198219870467074
-                },
-                {
-                    "x": 3.771578080903875,
-                    "y": 5.56,
-                    "heading": 1.545129672215716e-18,
-                    "angularVelocity": 7.802453727507411e-18,
-                    "velocityX": -2.479175819794286,
-                    "velocityY": -8.54847043233614e-34,
-                    "timestamp": 0.297329805700611
-                },
-                {
-                    "x": 3.4439634979187304,
-                    "y": 5.56,
-                    "heading": 2.5806642382479214e-18,
-                    "angularVelocity": 1.0448342676399075e-17,
-                    "velocityX": -3.3055675216936846,
-                    "velocityY": -5.0910568375180236e-34,
-                    "timestamp": 0.396439740934148
-                },
-                {
-                    "x": 3.0690365020812695,
-                    "y": 5.56,
-                    "heading": -2.5850068638158606e-18,
-                    "angularVelocity": -5.212061828001078e-17,
-                    "velocityX": -3.7829405796099502,
-                    "velocityY": -4.536187828596351e-34,
-                    "timestamp": 0.495549676167685
-                },
-                {
-                    "x": 2.741421919096125,
-                    "y": 5.56,
-                    "heading": -1.5478395588580842e-18,
-                    "angularVelocity": 1.0464816695997814e-17,
-                    "velocityX": -3.305567521693684,
-                    "velocityY": -2.872506388563931e-33,
-                    "timestamp": 0.594659611401222
-                },
-                {
-                    "x": 2.4957109641637625,
-                    "y": 5.56,
-                    "heading": -7.728706253244835e-19,
-                    "angularVelocity": 7.819286045545264e-18,
-                    "velocityX": -2.479175819794286,
-                    "velocityY": -1.6595590840772702e-33,
-                    "timestamp": 0.6937695466347591
-                },
-                {
-                    "x": 2.3319036557469652,
-                    "y": 5.56,
-                    "heading": -2.5738443375571516e-19,
-                    "angularVelocity": 5.201155568994596e-18,
-                    "velocityX": -1.6527839316089838,
-                    "velocityY": -1.4347514653062086e-33,
-                    "timestamp": 0.792879481868296
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 2.5969589542999405e-18,
-                    "velocityX": -0.8263919813283321,
-                    "velocityY": 2.9921012122849815e-33,
-                    "timestamp": 0.891989417101833
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 1.808288986442481e-43,
-                    "timestamp": 0.9910993523353699
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "S2 To W3": {
-            "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 13
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 9.014129666558106e-32,
-                    "angularVelocity": 9.039417129994225e-31,
-                    "velocityX": 0,
-                    "velocityY": -2.779554313855136e-31,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.206523478699805,
-                    "y": 5.55440930401173,
-                    "heading": -0.10760119525493232,
-                    "angularVelocity": -1.2344158099898592,
-                    "velocityX": -0.6479064718660169,
-                    "velocityY": -0.06413723844253004,
-                    "timestamp": 0.08716770668695249
-                },
-                {
-                    "x": 4.091196989898444,
-                    "y": 5.541197271810027,
-                    "heading": -0.295913717265277,
-                    "angularVelocity": -2.1603473254910437,
-                    "velocityX": -1.3230414471673093,
-                    "velocityY": -0.15157026270235532,
-                    "timestamp": 0.17433541337390498
-                },
-                {
-                    "x": 3.9151950960136452,
-                    "y": 5.516370757100726,
-                    "heading": -0.5340653762567812,
-                    "angularVelocity": -2.7321088054637612,
-                    "velocityX": -2.019118095155108,
-                    "velocityY": -0.2848132141236808,
-                    "timestamp": 0.26150312006085746
-                },
-                {
-                    "x": 3.678758800597895,
-                    "y": 5.473177706772957,
-                    "heading": -0.7546654655667131,
-                    "angularVelocity": -2.530754767957573,
-                    "velocityX": -2.712429917020411,
-                    "velocityY": -0.4955166536948123,
-                    "timestamp": 0.34867082674780997
-                },
-                {
-                    "x": 3.3968844521889907,
-                    "y": 5.398848640381046,
-                    "heading": -0.8527884173152881,
-                    "angularVelocity": -1.1256800881657552,
-                    "velocityX": -3.233701552126478,
-                    "velocityY": -0.8527133409492049,
-                    "timestamp": 0.4358385334347625
-                },
-                {
-                    "x": 3.098615494079836,
-                    "y": 5.268356714792353,
-                    "heading": -0.864385170870599,
-                    "angularVelocity": -0.1330395624260059,
-                    "velocityX": -3.421782784538955,
-                    "velocityY": -1.4970214377364781,
-                    "timestamp": 0.5230062401217149
-                },
-                {
-                    "x": 2.832096469446971,
-                    "y": 5.0836195976119996,
-                    "heading": -0.8474300397750907,
-                    "angularVelocity": 0.19451161146636953,
-                    "velocityX": -3.0575431517318843,
-                    "velocityY": -2.1193297862453186,
-                    "timestamp": 0.6101739468086674
-                },
-                {
-                    "x": 2.620164345186853,
-                    "y": 4.879059766080367,
-                    "heading": -0.7372424721392209,
-                    "angularVelocity": 1.2640870320425897,
-                    "velocityX": -2.431314672774813,
-                    "velocityY": -2.346738709855847,
-                    "timestamp": 0.6973416534956199
-                },
-                {
-                    "x": 2.469226841034766,
-                    "y": 4.6911608685478585,
-                    "heading": -0.6128307853924054,
-                    "angularVelocity": 1.427268095897297,
-                    "velocityX": -1.7315759458276552,
-                    "velocityY": -2.155602168212534,
-                    "timestamp": 0.7845093601825724
-                },
-                {
-                    "x": 2.3748885049182054,
-                    "y": 4.5313089505917965,
-                    "heading": -0.5057978702393489,
-                    "angularVelocity": 1.2278964219794164,
-                    "velocityX": -1.0822624536327627,
-                    "velocityY": -1.8338433352404204,
-                    "timestamp": 0.871677066869525
-                },
-                {
-                    "x": 2.3223904484665,
-                    "y": 4.418253055823363,
-                    "heading": -0.42949016513376354,
-                    "angularVelocity": 0.87541255822675,
-                    "velocityX": -0.6022649722820163,
-                    "velocityY": -1.2969928780443263,
-                    "timestamp": 0.9588447735564775
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": 0.4530366420626772,
-                    "velocityX": -0.256866324898396,
-                    "velocityY": -0.6682871218875669,
-                    "timestamp": 1.0460124802434299
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": -3.4100744325418914e-31,
-                    "velocityX": -9.400788860397695e-33,
-                    "velocityY": 0,
-                    "timestamp": 1.1331801869303824
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "S2 To C1": {
-            "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 5.380258045140596e-28,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.304265282414897,
-                    "y": 5.638698322322049,
-                    "heading": -0.04261698283734242,
-                    "angularVelocity": -0.41098530784979936,
-                    "velocityX": 0.3979499173257931,
-                    "velocityY": 0.7589428456312429,
-                    "timestamp": 0.10369466261532896
-                },
-                {
-                    "x": 4.38866009880019,
-                    "y": 5.795292929312062,
-                    "heading": -0.1228057028496872,
-                    "angularVelocity": -0.7733157907057984,
-                    "velocityX": 0.8138781134605525,
-                    "velocityY": 1.510151082422871,
-                    "timestamp": 0.20738932523065792
-                },
-                {
-                    "x": 4.519821720835651,
-                    "y": 6.028001091828693,
-                    "heading": -0.23019023538729733,
-                    "angularVelocity": -1.0355839908170448,
-                    "velocityX": 1.264883058851596,
-                    "velocityY": 2.2441672179396264,
-                    "timestamp": 0.3110839878459869
-                },
-                {
-                    "x": 4.707676475524912,
-                    "y": 6.330157785456564,
-                    "heading": -0.33387571773962255,
-                    "angularVelocity": -0.9999114683169592,
-                    "velocityX": 1.8116145031122473,
-                    "velocityY": 2.9139078715050806,
-                    "timestamp": 0.41477865046131585
-                },
-                {
-                    "x": 4.978690211282069,
-                    "y": 6.613755936139097,
-                    "heading": -0.3338757185080882,
-                    "angularVelocity": -7.410850442524386e-9,
-                    "velocityX": 2.6135745941189206,
-                    "velocityY": 2.7349348898948014,
-                    "timestamp": 0.5184733130766448
-                },
-                {
-                    "x": 5.307019127730767,
-                    "y": 6.828410259978494,
-                    "heading": -0.3338757060420003,
-                    "angularVelocity": 1.2021918642580205e-7,
-                    "velocityX": 3.166304881734213,
-                    "velocityY": 2.070061451819255,
-                    "timestamp": 0.6221679756919738
-                },
-                {
-                    "x": 5.675511606586513,
-                    "y": 6.962908031631864,
-                    "heading": -0.3338756959696621,
-                    "angularVelocity": 9.713458668099172e-8,
-                    "velocityX": 3.5536301441350284,
-                    "velocityY": 1.2970558779125847,
-                    "timestamp": 0.7258626383073028
-                },
-                {
-                    "x": 6.055844888125957,
-                    "y": 7.058944236891981,
-                    "heading": -0.3338756861984713,
-                    "angularVelocity": 9.423041207795053e-8,
-                    "velocityX": 3.6678192680981745,
-                    "velocityY": 0.9261441508940896,
-                    "timestamp": 0.8295573009226318
-                },
-                {
-                    "x": 6.436176952400414,
-                    "y": 7.154985262770466,
-                    "heading": -0.3338756764272739,
-                    "angularVelocity": 9.423047569354886e-8,
-                    "velocityX": 3.667807529162388,
-                    "velocityY": 0.9261906394812687,
-                    "timestamp": 0.9332519635379608
-                },
-                {
-                    "x": 6.8165090646020285,
-                    "y": 7.2510260988513195,
-                    "heading": -0.33387566665551793,
-                    "angularVelocity": 9.423586238606553e-8,
-                    "velocityX": 3.6678079913574164,
-                    "velocityY": 0.9261888091302356,
-                    "timestamp": 1.0369466261532898
-                },
-                {
-                    "x": 7.162467192455402,
-                    "y": 7.33732437680055,
-                    "heading": -0.24283297058924916,
-                    "angularVelocity": 0.8779882567727307,
-                    "velocityX": 3.336315670718354,
-                    "velocityY": 0.8322345217454595,
-                    "timestamp": 1.1406412887686188
-                },
-                {
-                    "x": 7.421550289934561,
-                    "y": 7.4021983509049765,
-                    "heading": -0.1315877257451376,
-                    "angularVelocity": 1.0728155339758783,
-                    "velocityX": 2.4985191228237835,
-                    "velocityY": 0.6256250077700213,
-                    "timestamp": 1.2443359513839478
-                },
-                {
-                    "x": 7.593930847137365,
-                    "y": 7.445412549605325,
-                    "heading": -0.046073342510862675,
-                    "angularVelocity": 0.8246748779298502,
-                    "velocityX": 1.662386017323544,
-                    "velocityY": 0.41674467721309977,
-                    "timestamp": 1.3480306139992768
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "angularVelocity": 0.44431739637147044,
-                    "velocityX": 0.8300249086292989,
-                    "velocityY": 0.20818285001567757,
-                    "timestamp": 1.4517252766146058
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 6.677691966309128e-30,
-                    "velocityY": 0,
-                    "timestamp": 1.5554199392299348
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "S2 To C2": {
-            "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 9
-                },
-                {
-                    "x": 5.261847496032715,
-                    "y": 6.900974273681641,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 12
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.272659932573731,
-                    "y": 5.605321030989923,
-                    "heading": -5.269773313916661e-8,
-                    "angularVelocity": -7.069117507058631e-7,
-                    "velocityX": 0.1295790656325301,
-                    "velocityY": 0.6079397351586845,
-                    "timestamp": 0.07454855863477708
-                },
-                {
-                    "x": 4.294668340488188,
-                    "y": 5.695305527660525,
-                    "heading": -1.390931476532318e-7,
-                    "angularVelocity": -0.0000011589489187991593,
-                    "velocityX": 0.2952224471421504,
-                    "velocityY": 1.2070588395619357,
-                    "timestamp": 0.14909711726955416
-                },
-                {
-                    "x": 4.333068049168991,
-                    "y": 5.8286332482919585,
-                    "heading": -2.3393980900082764e-7,
-                    "angularVelocity": -0.0000012723197345504802,
-                    "velocityX": 0.5150965944736963,
-                    "velocityY": 1.7884681217524945,
-                    "timestamp": 0.22364567590433124
-                },
-                {
-                    "x": 4.394361213548298,
-                    "y": 6.0022499148295365,
-                    "heading": -3.0377830712505715e-7,
-                    "angularVelocity": -9.368513338342214e-7,
-                    "velocityX": 0.8221911395208825,
-                    "velocityY": 2.3289070872699873,
-                    "timestamp": 0.2981942345391083
-                },
-                {
-                    "x": 4.489214393007107,
-                    "y": 6.207820209268236,
-                    "heading": -3.104589480689307e-7,
-                    "angularVelocity": -8.962358439949817e-8,
-                    "velocityX": 1.2723677183517785,
-                    "velocityY": 2.75753546703125,
-                    "timestamp": 0.37274279317388537
-                },
-                {
-                    "x": 4.629403012529928,
-                    "y": 6.42298249437822,
-                    "heading": -2.4199647648725644e-7,
-                    "angularVelocity": 9.18393943226279e-7,
-                    "velocityX": 1.880500737963715,
-                    "velocityY": 2.8862031546566858,
-                    "timestamp": 0.4472913518086624
-                },
-                {
-                    "x": 4.811531801184116,
-                    "y": 6.618438666310206,
-                    "heading": -1.472447710538824e-7,
-                    "angularVelocity": 0.0000012710752897138218,
-                    "velocityX": 2.443089336474201,
-                    "velocityY": 2.6218638627509074,
-                    "timestamp": 0.5218399104434395
-                },
-                {
-                    "x": 5.024847227590037,
-                    "y": 6.779620899070013,
-                    "heading": -6.254572131926062e-8,
-                    "angularVelocity": 0.000001136235548213432,
-                    "velocityX": 2.861429252247402,
-                    "velocityY": 2.1621106520395927,
-                    "timestamp": 0.5963884690782165
-                },
-                {
-                    "x": 5.261847496032715,
-                    "y": 6.900974273681641,
-                    "heading": 1.6289151195668303e-9,
-                    "angularVelocity": 8.609140934997053e-7,
-                    "velocityX": 3.1791395133448552,
-                    "velocityY": 1.6278433384167708,
-                    "timestamp": 0.6709370277129936
-                },
-                {
-                    "x": 5.5631639462020495,
-                    "y": 6.984947323321856,
-                    "heading": 4.965855378058324e-8,
-                    "angularVelocity": 5.539886921154719e-7,
-                    "velocityX": 3.4757171947403775,
-                    "velocityY": 0.9686380297960585,
-                    "timestamp": 0.7576289054784
-                },
-                {
-                    "x": 5.883696640724643,
-                    "y": 7.009274337947639,
-                    "heading": 7.188189606013307e-8,
-                    "angularVelocity": 2.563216327096045e-7,
-                    "velocityX": 3.697378610459799,
-                    "velocityY": 0.2806146929817695,
-                    "timestamp": 0.8443207832438064
-                },
-                {
-                    "x": 6.209423466668327,
-                    "y": 6.971151903870608,
-                    "heading": 7.193737396357795e-8,
-                    "angularVelocity": 6.438452889791163e-10,
-                    "velocityX": 3.7572934667332714,
-                    "velocityY": -0.4397463179458525,
-                    "timestamp": 0.9310126610092129
-                },
-                {
-                    "x": 6.521952745942683,
-                    "y": 6.871769894938554,
-                    "heading": 7.193784305655878e-8,
-                    "angularVelocity": 5.630908705768223e-12,
-                    "velocityX": 3.6050583669434406,
-                    "velocityY": -1.1463820079014435,
-                    "timestamp": 1.0177045387746193
-                },
-                {
-                    "x": 6.809873374118548,
-                    "y": 6.714756988695251,
-                    "heading": 7.193831945924844e-8,
-                    "angularVelocity": 5.91084758847972e-12,
-                    "velocityX": 3.321194967875227,
-                    "velocityY": -1.8111605177424126,
-                    "timestamp": 1.1043964165400257
-                },
-                {
-                    "x": 7.062672913559571,
-                    "y": 6.505846154651799,
-                    "heading": 7.193893641048392e-8,
-                    "angularVelocity": 7.48396872358976e-12,
-                    "velocityX": 2.9160694861627596,
-                    "velocityY": -2.4098086166543093,
-                    "timestamp": 1.1910882943054322
-                },
-                {
-                    "x": 7.268448732261103,
-                    "y": 6.269564240975314,
-                    "heading": 6.958123303548702e-8,
-                    "angularVelocity": -2.7156604096657526e-8,
-                    "velocityX": 2.3736458816624144,
-                    "velocityY": -2.7255369226670942,
-                    "timestamp": 1.2777801720708386
-                },
-                {
-                    "x": 7.433069281193563,
-                    "y": 6.080538590855167,
-                    "heading": 5.4913746267249155e-8,
-                    "angularVelocity": -1.6915879682512025e-7,
-                    "velocityX": 1.8989154825033356,
-                    "velocityY": -2.180430912690726,
-                    "timestamp": 1.364472049836245
-                },
-                {
-                    "x": 7.556534655605292,
-                    "y": 5.9387693118426395,
-                    "heading": 3.4027865091828515e-8,
-                    "angularVelocity": -2.408962130800618e-7,
-                    "velocityX": 1.424186181835385,
-                    "velocityY": -1.635323662267605,
-                    "timestamp": 1.4511639276016515
-                },
-                {
-                    "x": 7.638844889158882,
-                    "y": 5.8442564415645775,
-                    "heading": 1.3493455787871544e-8,
-                    "angularVelocity": -2.368497714278514e-7,
-                    "velocityX": 0.9494572693998051,
-                    "velocityY": -1.0902159778725726,
-                    "timestamp": 1.537855805367058
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": -2.563219583366834e-29,
-                    "angularVelocity": -1.5563981959470815e-7,
-                    "velocityX": 0.4747285662416774,
-                    "velocityY": -0.5451080630467725,
-                    "timestamp": 1.6245476831324643
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "angularVelocity": 4.328374921762355e-28,
-                    "velocityX": 0,
-                    "velocityY": 6.636862452110489e-29,
-                    "timestamp": 1.7112395608978708
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "S2 To C3": {
-            "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": -9.522541691773037e-34,
-                    "velocityX": 0,
-                    "velocityY": -4.2308696063129985e-35,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.271262994735721,
-                    "y": 5.464085078742271,
-                    "heading": 0.013066573413208545,
-                    "angularVelocity": 0.12156077000957308,
-                    "velocityX": 0.0768721814736735,
-                    "velocityY": -0.8923144052220247,
-                    "timestamp": 0.10749005137249076
-                },
-                {
-                    "x": 4.301424317332477,
-                    "y": 5.27437671657762,
-                    "heading": 0.033784324267673776,
-                    "angularVelocity": 0.192741101059399,
-                    "velocityX": 0.2805964106597774,
-                    "velocityY": -1.7648922829820315,
-                    "timestamp": 0.21498010274498153
-                },
-                {
-                    "x": 4.379013311524821,
-                    "y": 5.000817118183451,
-                    "heading": 0.051063906614390905,
-                    "angularVelocity": 0.16075517804747627,
-                    "velocityX": 0.7218248870629933,
-                    "velocityY": -2.544975975927198,
-                    "timestamp": 0.32247015411747226
-                },
-                {
-                    "x": 4.5426840546799285,
-                    "y": 4.684552935799795,
-                    "heading": 0.043724640930798246,
-                    "angularVelocity": -0.06827855778168279,
-                    "velocityX": 1.5226594560638123,
-                    "velocityY": -2.9422646872470954,
-                    "timestamp": 0.42996020548996305
-                },
-                {
-                    "x": 4.7987371937718075,
-                    "y": 4.3933637164950845,
-                    "heading": -0.0025480008668042047,
-                    "angularVelocity": -0.4304830187237655,
-                    "velocityX": 2.382110119238521,
-                    "velocityY": -2.7089876280330105,
-                    "timestamp": 0.5374502568624538
-                },
-                {
-                    "x": 5.129552682758233,
-                    "y": 4.161939062753506,
-                    "heading": -0.00976861092561694,
-                    "angularVelocity": -0.06717468236935505,
-                    "velocityX": 3.0776382070935524,
-                    "velocityY": -2.152986725623672,
-                    "timestamp": 0.6449403082349446
-                },
-                {
-                    "x": 5.508331065778466,
-                    "y": 4.0140416634822165,
-                    "heading": -0.009768639154040644,
-                    "angularVelocity": -2.6261429169311795e-7,
-                    "velocityX": 3.5238459576843355,
-                    "velocityY": -1.3759170954228361,
-                    "timestamp": 0.7524303596074354
-                },
-                {
-                    "x": 5.911272135069668,
-                    "y": 3.959404581326429,
-                    "heading": -0.009768652411608816,
-                    "angularVelocity": -1.233376298753571e-7,
-                    "velocityX": 3.7486359355701584,
-                    "velocityY": -0.5082989677477323,
-                    "timestamp": 0.8599204109799262
-                },
-                {
-                    "x": 6.316087697151271,
-                    "y": 3.997759236961094,
-                    "heading": -0.009768652034230711,
-                    "angularVelocity": 3.51081891151765e-9,
-                    "velocityX": 3.7660746916826353,
-                    "velocityY": 0.3568205163634492,
-                    "timestamp": 0.967410462352417
-                },
-                {
-                    "x": 6.720902827559142,
-                    "y": 4.036118448432054,
-                    "heading": -0.009768651656829377,
-                    "angularVelocity": 3.5110350109919825e-9,
-                    "velocityX": 3.766070675741356,
-                    "velocityY": 0.35686290015837574,
-                    "timestamp": 1.0749005137249077
-                },
-                {
-                    "x": 7.104541677751467,
-                    "y": 4.072471064803872,
-                    "heading": -0.005861206189315246,
-                    "angularVelocity": 0.03635169411142489,
-                    "velocityX": 3.569063790497984,
-                    "velocityY": 0.3381951716242375,
-                    "timestamp": 1.1823905650973985
-                },
-                {
-                    "x": 7.392270834070994,
-                    "y": 4.099735530737146,
-                    "heading": -0.0029306066828158664,
-                    "angularVelocity": 0.027263913907193366,
-                    "velocityX": 2.6767980166131413,
-                    "velocityY": 0.2536464127158502,
-                    "timestamp": 1.2898806164698893
-                },
-                {
-                    "x": 7.584090276967534,
-                    "y": 4.117911843086064,
-                    "heading": -0.0009768696053991616,
-                    "angularVelocity": 0.018175980497453854,
-                    "velocityX": 1.7845320608492279,
-                    "velocityY": 0.16909762454137037,
-                    "timestamp": 1.39737066784238
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": -6.809535330810639e-32,
-                    "angularVelocity": 0.009088000172350516,
-                    "velocityX": 0.8922660451626774,
-                    "velocityY": 0.08454881915017429,
-                    "timestamp": 1.5048607192148709
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "angularVelocity": -9.489213882041181e-31,
-                    "velocityX": 2.198140684032685e-33,
-                    "velocityY": -6.756921472052438e-32,
-                    "timestamp": 1.6123507705873616
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "S2 To C4": {
-            "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 17
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 7.441191049306378e-32,
-                    "velocityX": -6.15829104129807e-34,
-                    "velocityY": 2.4252394047543234e-33,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.262142393092874,
-                    "y": 5.471176724349191,
-                    "heading": -0.06580713819401311,
-                    "angularVelocity": -0.6311237133024392,
-                    "velocityX": -0.008224883662067025,
-                    "velocityY": -0.8518601035521746,
-                    "timestamp": 0.10426979181255681
-                },
-                {
-                    "x": 4.27936690849282,
-                    "y": 5.294863623901166,
-                    "heading": -0.18680392528158013,
-                    "angularVelocity": -1.1604203382805245,
-                    "velocityX": 0.16519180771848577,
-                    "velocityY": -1.6909317395107013,
-                    "timestamp": 0.20853958362511363
-                },
-                {
-                    "x": 4.348116425232891,
-                    "y": 5.0445566747334585,
-                    "heading": -0.3412830267774945,
-                    "angularVelocity": -1.4815326549574157,
-                    "velocityX": 0.6593426105967491,
-                    "velocityY": -2.400570144205118,
-                    "timestamp": 0.31280937543767046
-                },
-                {
-                    "x": 4.502267029635194,
-                    "y": 4.763921050939977,
-                    "heading": -0.4878370803946758,
-                    "angularVelocity": -1.4055274405902505,
-                    "velocityX": 1.4783822018117698,
-                    "velocityY": -2.6914374615609895,
-                    "timestamp": 0.41707916725022726
-                },
-                {
-                    "x": 4.740029978369904,
-                    "y": 4.496898841850153,
-                    "heading": -0.5207249369577782,
-                    "angularVelocity": -0.31541116551017917,
-                    "velocityX": 2.280266840487519,
-                    "velocityY": -2.5608779345205184,
-                    "timestamp": 0.5213489590627841
-                },
-                {
-                    "x": 5.0582868126043,
-                    "y": 4.2697391199027495,
-                    "heading": -0.509978725166827,
-                    "angularVelocity": 0.10306160206274768,
-                    "velocityX": 3.0522438829312937,
-                    "velocityY": -2.1785765368723764,
-                    "timestamp": 0.6256187508753409
-                },
-                {
-                    "x": 5.423286105863135,
-                    "y": 4.1201949004052105,
-                    "heading": -0.5099785687835147,
-                    "angularVelocity": 0.0000014997949991595825,
-                    "velocityX": 3.5005276879710774,
-                    "velocityY": -1.4342046425715584,
-                    "timestamp": 0.7298885426878977
-                },
-                {
-                    "x": 5.805099761015269,
-                    "y": 4.021168099470067,
-                    "heading": -0.5099784851647622,
-                    "angularVelocity": 8.019460959727046e-7,
-                    "velocityX": 3.6617859162748747,
-                    "velocityY": -0.9497170677501874,
-                    "timestamp": 0.8341583345004545
-                },
-                {
-                    "x": 6.154222032422552,
-                    "y": 3.8375880828272497,
-                    "heading": -0.5099784796190555,
-                    "angularVelocity": 5.318612994948929e-8,
-                    "velocityX": 3.348259024386382,
-                    "velocityY": -1.7606251384181746,
-                    "timestamp": 0.9384281263130113
-                },
-                {
-                    "x": 6.493379431179238,
-                    "y": 3.636192348012897,
-                    "heading": -0.5099784741544249,
-                    "angularVelocity": 5.240856867670856e-8,
-                    "velocityX": 3.2526908595576858,
-                    "velocityY": -1.9314868795019449,
-                    "timestamp": 1.0426979181255682
-                },
-                {
-                    "x": 6.795285003697281,
-                    "y": 3.3823400148372036,
-                    "heading": -0.5099784394461939,
-                    "angularVelocity": 3.32869476219209e-7,
-                    "velocityX": 2.895427019368865,
-                    "velocityY": -2.4345721686299853,
-                    "timestamp": 1.146967709938125
-                },
-                {
-                    "x": 7.066681771760518,
-                    "y": 3.0961027469536373,
-                    "heading": -0.509978390292543,
-                    "angularVelocity": 4.714083530965781e-7,
-                    "velocityX": 2.6028321659174227,
-                    "velocityY": -2.7451600593787298,
-                    "timestamp": 1.2512375017506818
-                },
-                {
-                    "x": 7.312943361341716,
-                    "y": 2.8410544331524363,
-                    "heading": -0.40533862017886096,
-                    "angularVelocity": 1.0035482788897323,
-                    "velocityX": 2.361773101301441,
-                    "velocityY": -2.446042227260748,
-                    "timestamp": 1.3555072935632386
-                },
-                {
-                    "x": 7.49731350893092,
-                    "y": 2.6484381177603322,
-                    "heading": -0.22178416093118458,
-                    "angularVelocity": 1.7603800300824186,
-                    "velocityX": 1.768202893515326,
-                    "velocityY": -1.8472878102448496,
-                    "timestamp": 1.4597770853757954
-                },
-                {
-                    "x": 7.619307085639959,
-                    "y": 2.520663921373434,
-                    "heading": -0.0776901780416505,
-                    "angularVelocity": 1.3819341190262298,
-                    "velocityX": 1.1699800545142,
-                    "velocityY": -1.225419118670484,
-                    "timestamp": 1.5640468771883522
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 1.429761379305419e-34,
-                    "angularVelocity": 0.7450880709660586,
-                    "velocityX": 0.5820757220763062,
-                    "velocityY": -0.6105691808408042,
-                    "timestamp": 1.668316669000909
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "angularVelocity": -5.61453765178046e-32,
-                    "velocityX": 0,
-                    "velocityY": 2.318867528557263e-33,
-                    "timestamp": 1.7725864608134658
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "S2 To C5": {
-            "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 21
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": -1.2613927548149136e-32,
-                    "velocityX": 4.983015637334932e-33,
-                    "velocityY": -3.6905945238681026e-33,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.258006222511338,
-                    "y": 5.473607841813564,
-                    "heading": -0.06530003091818118,
-                    "angularVelocity": -0.6342708718414432,
-                    "velocityX": -0.04850545332028736,
-                    "velocityY": -0.8391424739420397,
-                    "timestamp": 0.10295290831912149
-                },
-                {
-                    "x": 4.271377859079598,
-                    "y": 5.302453682559796,
-                    "heading": -0.1916286288786279,
-                    "angularVelocity": -1.2270522515873779,
-                    "velocityX": 0.12988109599401854,
-                    "velocityY": -1.662450940416803,
-                    "timestamp": 0.20590581663824298
-                },
-                {
-                    "x": 4.340766271943036,
-                    "y": 5.0642424556348775,
-                    "heading": -0.3653686640748466,
-                    "angularVelocity": -1.6875680156375907,
-                    "velocityX": 0.673982056420939,
-                    "velocityY": -2.3137882242873524,
-                    "timestamp": 0.30885872495736444
-                },
-                {
-                    "x": 4.494848034080926,
-                    "y": 4.801255485663876,
-                    "heading": -0.5565822720790902,
-                    "angularVelocity": -1.8572919514963226,
-                    "velocityX": 1.4966236957608356,
-                    "velocityY": -2.5544394448365244,
-                    "timestamp": 0.41181163327648596
-                },
-                {
-                    "x": 4.73233691115851,
-                    "y": 4.546498868370588,
-                    "heading": -0.6490495118957932,
-                    "angularVelocity": -0.8981508276588281,
-                    "velocityX": 2.3067719111095206,
-                    "velocityY": -2.4744965582091383,
-                    "timestamp": 0.5147645415956075
-                },
-                {
-                    "x": 5.049349589860016,
-                    "y": 4.3202563768587865,
-                    "heading": -0.6490493910522949,
-                    "angularVelocity": 0.0000011737744981731646,
-                    "velocityX": 3.079200810130272,
-                    "velocityY": -2.1975337579636096,
-                    "timestamp": 0.617717449914729
-                },
-                {
-                    "x": 5.4092082222704905,
-                    "y": 4.171311637049865,
-                    "heading": -0.6490493274377749,
-                    "angularVelocity": 6.178992034513071e-7,
-                    "velocityX": 3.4953712166637025,
-                    "velocityY": -1.446726879703489,
-                    "timestamp": 0.7206703582338505
-                },
-                {
-                    "x": 5.789554256648214,
-                    "y": 4.087528164867666,
-                    "heading": -0.6490492895940018,
-                    "angularVelocity": 3.675833320529158e-7,
-                    "velocityX": 3.6943690138288297,
-                    "velocityY": -0.8138038405141147,
-                    "timestamp": 0.823623266552972
-                },
-                {
-                    "x": 6.141218018459777,
-                    "y": 3.9201502365347105,
-                    "heading": -0.6490493503243893,
-                    "angularVelocity": -5.898851085689342e-7,
-                    "velocityX": 3.4157729738097102,
-                    "velocityY": -1.6257717345306835,
-                    "timestamp": 0.9265761748720935
-                },
-                {
-                    "x": 6.456138339829479,
-                    "y": 3.69100407104571,
-                    "heading": -0.6490494074378677,
-                    "angularVelocity": -5.547534235510974e-7,
-                    "velocityX": 3.0588773693847333,
-                    "velocityY": -2.2257376622981755,
-                    "timestamp": 1.029529083191215
-                },
-                {
-                    "x": 6.721775326533507,
-                    "y": 3.406188756476768,
-                    "heading": -0.6490494689481024,
-                    "angularVelocity": -5.974599048336077e-7,
-                    "velocityX": 2.580179531020508,
-                    "velocityY": -2.7664620574496457,
-                    "timestamp": 1.1324819915103363
-                },
-                {
-                    "x": 6.916359430113237,
-                    "y": 3.0688168532177102,
-                    "heading": -0.6490494077682868,
-                    "angularVelocity": 5.942504852606566e-7,
-                    "velocityX": 1.8900301774533879,
-                    "velocityY": -3.276953597204961,
-                    "timestamp": 1.2354348998294578
-                },
-                {
-                    "x": 7.040594979350531,
-                    "y": 2.699698573462511,
-                    "heading": -0.6490493528706274,
-                    "angularVelocity": 5.332307776860117e-7,
-                    "velocityX": 1.206722095234099,
-                    "velocityY": -3.585311826364818,
-                    "timestamp": 1.3383878081485792
-                },
-                {
-                    "x": 7.164812386541997,
-                    "y": 2.330574188065911,
-                    "heading": -0.649049297974002,
-                    "angularVelocity": 5.332207349098274e-7,
-                    "velocityX": 1.2065458783003051,
-                    "velocityY": -3.585371131551044,
-                    "timestamp": 1.4413407164677006
-                },
-                {
-                    "x": 7.289029734328084,
-                    "y": 1.9614497826782673,
-                    "heading": -0.6490492430773671,
-                    "angularVelocity": 5.332208261000816e-7,
-                    "velocityX": 1.206545301285243,
-                    "velocityY": -3.5853713257276327,
-                    "timestamp": 1.544293624786822
-                },
-                {
-                    "x": 7.413246520819429,
-                    "y": 1.5923251884092744,
-                    "heading": -0.6490491881699392,
-                    "angularVelocity": 5.333256623024379e-7,
-                    "velocityX": 1.2065398493290926,
-                    "velocityY": -3.585373160365936,
-                    "timestamp": 1.6472465331059434
-                },
-                {
-                    "x": 7.520074458284658,
-                    "y": 1.2704783284902474,
-                    "heading": -0.5256216946651037,
-                    "angularVelocity": 1.1988733054752465,
-                    "velocityX": 1.037638850707321,
-                    "velocityY": -3.1261560763432104,
-                    "timestamp": 1.7501994414250648
-                },
-                {
-                    "x": 7.601104601009551,
-                    "y": 1.0271731610238788,
-                    "heading": -0.2913266412644364,
-                    "angularVelocity": 2.275749730881093,
-                    "velocityX": 0.7870602593733868,
-                    "velocityY": -2.3632665792422265,
-                    "timestamp": 1.8531523497441862
-                },
-                {
-                    "x": 7.654009357663848,
-                    "y": 0.8666565360101991,
-                    "heading": -0.10310334462657153,
-                    "angularVelocity": 1.828246522715339,
-                    "velocityX": 0.5138733574219042,
-                    "velocityY": -1.5591266690216163,
-                    "timestamp": 1.9561052580633076
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": -1.429149737287701e-32,
-                    "angularVelocity": 1.0014612147428026,
-                    "velocityX": 0.2524517545010862,
-                    "velocityY": -0.7737181718391956,
-                    "timestamp": 2.059058166382429
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "angularVelocity": -8.032741852653566e-32,
-                    "velocityX": 0,
-                    "velocityY": 2.733638748586157e-32,
-                    "timestamp": 2.1620110747015504
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "S3 To W1": {
-            "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 21
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "angularVelocity": -1.6794679447375943e-30,
-                    "velocityX": 6.742046899995187e-33,
-                    "velocityY": 9.708842875589211e-32,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.290023963257714,
-                    "y": 3.0497874441329906,
-                    "heading": -0.5200869306801483,
-                    "angularVelocity": 0.5503538158132694,
-                    "velocityX": 0.3239326300245928,
-                    "velocityY": 0.5967954280576505,
-                    "timestamp": 0.08342464065958187
-                },
-                {
-                    "x": 4.342730369898429,
-                    "y": 3.150178003649595,
-                    "heading": -0.42997936088427535,
-                    "angularVelocity": 1.0801073769506666,
-                    "velocityX": 0.6317846408926776,
-                    "velocityY": 1.2033681982072046,
-                    "timestamp": 0.16684928131916374
-                },
-                {
-                    "x": 4.418537982231504,
-                    "y": 3.3026029150734924,
-                    "heading": -0.2990998156933407,
-                    "angularVelocity": 1.5688355880967437,
-                    "velocityX": 0.9086957010988046,
-                    "velocityY": 1.8270970089745318,
-                    "timestamp": 0.2502739219787456
-                },
-                {
-                    "x": 4.5104462205151705,
-                    "y": 3.5100667276192694,
-                    "heading": -0.13617594937576347,
-                    "angularVelocity": 1.9529465758491624,
-                    "velocityX": 1.1016917490685052,
-                    "velocityY": 2.4868409489750483,
-                    "timestamp": 0.3336985626383275
-                },
-                {
-                    "x": 4.573472263152817,
-                    "y": 3.7674681923433693,
-                    "heading": 0.004287761033867021,
-                    "angularVelocity": 1.6837196935950753,
-                    "velocityX": 0.7554847361563879,
-                    "velocityY": 3.0854369007645883,
-                    "timestamp": 0.41712320329790936
-                },
-                {
-                    "x": 4.6071871310155705,
-                    "y": 4.066840423815047,
-                    "heading": 0.04705620405427594,
-                    "angularVelocity": 0.5126596013152507,
-                    "velocityX": 0.40413560785150865,
-                    "velocityY": 3.5885348633778387,
-                    "timestamp": 0.5005478439574912
-                },
-                {
-                    "x": 4.587417601167403,
-                    "y": 4.370555626705338,
-                    "heading": 0.08458578048595149,
-                    "angularVelocity": 0.4498620088136386,
-                    "velocityX": -0.23697470785444333,
-                    "velocityY": 3.6405934804036506,
-                    "timestamp": 0.5839724846170731
-                },
-                {
-                    "x": 4.510022832963655,
-                    "y": 4.67116829443229,
-                    "heading": 0.1002321163817716,
-                    "angularVelocity": 0.1875505338964035,
-                    "velocityX": -0.9277207260569706,
-                    "velocityY": 3.603403806719601,
-                    "timestamp": 0.667397125276655
-                },
-                {
-                    "x": 4.376634060333898,
-                    "y": 4.957183607465761,
-                    "heading": 0.10023219176740035,
-                    "angularVelocity": 9.036374403612271e-7,
-                    "velocityX": -1.5989133615097586,
-                    "velocityY": 3.4284272700744434,
-                    "timestamp": 0.7508217659362368
-                },
-                {
-                    "x": 4.19313075849175,
-                    "y": 5.213940045953902,
-                    "heading": 0.10023221692184411,
-                    "angularVelocity": 3.015229499928316e-7,
-                    "velocityX": -2.1996295146291525,
-                    "velocityY": 3.0777050576201734,
-                    "timestamp": 0.8342464065958187
-                },
-                {
-                    "x": 3.9657176811920825,
-                    "y": 5.432756463781841,
-                    "heading": 0.10023223856547658,
-                    "angularVelocity": 2.5943932502769966e-7,
-                    "velocityX": -2.725970115084308,
-                    "velocityY": 2.6229231087830502,
-                    "timestamp": 0.9176710472554006
-                },
-                {
-                    "x": 3.7186426713834653,
-                    "y": 5.629101242954768,
-                    "heading": 0.10023225962798245,
-                    "angularVelocity": 2.524734383899783e-7,
-                    "velocityX": -2.961655068037016,
-                    "velocityY": 2.3535585843770224,
-                    "timestamp": 1.0010956879149824
-                },
-                {
-                    "x": 3.4715770348675625,
-                    "y": 5.825457816649942,
-                    "heading": 0.10023228069061084,
-                    "angularVelocity": 2.5247490699416256e-7,
-                    "velocityX": -2.961542711631996,
-                    "velocityY": 2.353699963736333,
-                    "timestamp": 1.0845203285745642
-                },
-                {
-                    "x": 3.224511384705754,
-                    "y": 6.021814373175139,
-                    "heading": 0.10023230175325984,
-                    "angularVelocity": 2.5247515406639525e-7,
-                    "velocityX": -2.96154287520364,
-                    "velocityY": 2.353699757922098,
-                    "timestamp": 1.167944969234146
-                },
-                {
-                    "x": 2.9774457295747583,
-                    "y": 6.218170923447359,
-                    "heading": 0.10023232281654228,
-                    "angularVelocity": 2.524827469327523e-7,
-                    "velocityX": -2.9615429347686173,
-                    "velocityY": 2.3536996829685104,
-                    "timestamp": 1.2513696098937277
-                },
-                {
-                    "x": 2.751412346836958,
-                    "y": 6.39797212959724,
-                    "heading": 0.1658210049038553,
-                    "angularVelocity": 0.7862027521934517,
-                    "velocityX": -2.7094319010631414,
-                    "velocityY": 2.155252989144637,
-                    "timestamp": 1.3347942505533095
-                },
-                {
-                    "x": 2.5706626829115913,
-                    "y": 6.54172747182469,
-                    "heading": 0.24638520673123038,
-                    "angularVelocity": 0.9657123026291593,
-                    "velocityX": -2.166622025534686,
-                    "velocityY": 1.723176043563091,
-                    "timestamp": 1.4182188912128912
-                },
-                {
-                    "x": 2.435257350873027,
-                    "y": 6.649419264379792,
-                    "heading": 0.31518856404188306,
-                    "angularVelocity": 0.8247366337651717,
-                    "velocityX": -1.6230855891977036,
-                    "velocityY": 1.2908871012647558,
-                    "timestamp": 1.501643531872473
-                },
-                {
-                    "x": 2.3450659538089185,
-                    "y": 6.721154353305962,
-                    "heading": 0.3643262231924431,
-                    "angularVelocity": 0.5890065424563048,
-                    "velocityX": -1.0811122031935256,
-                    "velocityY": 0.8598789081859941,
-                    "timestamp": 1.5850681725320548
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "angularVelocity": 0.3077481257883978,
-                    "velocityX": -0.5401995555822935,
-                    "velocityY": 0.42967696846676384,
-                    "timestamp": 1.6684928131916366
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "angularVelocity": -2.8369790280137563e-29,
-                    "velocityX": 0,
-                    "velocityY": -1.9155196850548007e-31,
-                    "timestamp": 1.7519174538512183
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "S3 To W2": {
-            "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 16
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 2.1175745999315144e-33,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.331197427717942,
-                    "y": 3.064203342806604,
-                    "heading": -0.5918486780870994,
-                    "angularVelocity": -0.24352382070717674,
-                    "velocityX": 0.642497001367488,
-                    "velocityY": 0.604868198278966,
-                    "timestamp": 0.10614435176007272
-                },
-                {
-                    "x": 4.457228812945604,
-                    "y": 3.20188389088458,
-                    "heading": -0.6500618235234895,
-                    "angularVelocity": -0.5484337552691863,
-                    "velocityX": 1.1873583769444531,
-                    "velocityY": 1.2971066834454588,
-                    "timestamp": 0.21228870352014545
-                },
-                {
-                    "x": 4.610677951553499,
-                    "y": 3.4285671398637003,
-                    "heading": -0.7523465196948876,
-                    "angularVelocity": -0.9636376733695737,
-                    "velocityX": 1.4456646638603075,
-                    "velocityY": 2.1356129197671536,
-                    "timestamp": 0.31843305528021815
-                },
-                {
-                    "x": 4.708912918877337,
-                    "y": 3.730071149255109,
-                    "heading": -0.9027593729452231,
-                    "angularVelocity": -1.4170594172577993,
-                    "velocityX": 0.9254846413861598,
-                    "velocityY": 2.840509215911215,
-                    "timestamp": 0.4245774070402909
-                },
-                {
-                    "x": 4.717930123488916,
-                    "y": 4.059451821332611,
-                    "heading": -1.0196863730682848,
-                    "angularVelocity": -1.1015847587195386,
-                    "velocityX": 0.08495227925044571,
-                    "velocityY": 3.10313894819415,
-                    "timestamp": 0.5307217588003637
-                },
-                {
-                    "x": 4.639893034223715,
-                    "y": 4.389369622514019,
-                    "heading": -1.0106655427651627,
-                    "angularVelocity": 0.08498643737080536,
-                    "velocityX": -0.7351977563685651,
-                    "velocityY": 3.1081993126412395,
-                    "timestamp": 0.6368661105604364
-                },
-                {
-                    "x": 4.471904603111427,
-                    "y": 4.710090421849215,
-                    "heading": -0.9107146957571205,
-                    "angularVelocity": 0.9416501712118399,
-                    "velocityX": -1.5826412647185075,
-                    "velocityY": 3.021553139823678,
-                    "timestamp": 0.7430104623205092
-                },
-                {
-                    "x": 4.218561716713538,
-                    "y": 4.992188206454831,
-                    "heading": -0.8355436069817835,
-                    "angularVelocity": 0.7081967860640641,
-                    "velocityX": -2.3867768957743736,
-                    "velocityY": 2.657680601255776,
-                    "timestamp": 0.8491548140805819
-                },
-                {
-                    "x": 3.8896224481808446,
-                    "y": 5.222473200709689,
-                    "heading": -0.83554222765338,
-                    "angularVelocity": 0.000012994835623464205,
-                    "velocityX": -3.098980426921093,
-                    "velocityY": 2.169545439171289,
-                    "timestamp": 0.9552991658406547
-                },
-                {
-                    "x": 3.516178298161353,
-                    "y": 5.370026719708121,
-                    "heading": -0.8355421446101063,
-                    "angularVelocity": 7.823616840140413e-7,
-                    "velocityX": -3.518266811442025,
-                    "velocityY": 1.390121250464274,
-                    "timestamp": 1.0614435176007273
-                },
-                {
-                    "x": 3.1189612059977283,
-                    "y": 5.42877328608909,
-                    "heading": -0.8355420737846555,
-                    "angularVelocity": 6.672559542292647e-7,
-                    "velocityX": -3.742234848835754,
-                    "velocityY": 0.55345918465599,
-                    "timestamp": 1.1675878693608
-                },
-                {
-                    "x": 2.774416222135212,
-                    "y": 5.484170620762628,
-                    "heading": -0.7035735462776811,
-                    "angularVelocity": 1.243292980914089,
-                    "velocityX": -3.2460039384980304,
-                    "velocityY": 0.5219056290320314,
-                    "timestamp": 1.2737322211208726
-                },
-                {
-                    "x": 2.5095166350816602,
-                    "y": 5.523131311595933,
-                    "heading": -0.3884677902217159,
-                    "angularVelocity": 2.9686530732056857,
-                    "velocityX": -2.4956541036901063,
-                    "velocityY": 0.3670538298766209,
-                    "timestamp": 1.3798765728809452
-                },
-                {
-                    "x": 2.335800273937377,
-                    "y": 5.547984865708467,
-                    "heading": -0.13742924633332027,
-                    "angularVelocity": 2.365067379711733,
-                    "velocityX": -1.6366048523895966,
-                    "velocityY": 0.23414862590816604,
-                    "timestamp": 1.4860209246410179
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 1.2947391364164484,
-                    "velocityX": -0.8083357476365635,
-                    "velocityY": 0.11319617193282665,
-                    "timestamp": 1.5921652764010905
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": -1.986468774957569e-34,
-                    "velocityY": 0,
-                    "timestamp": 1.6983096281611632
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "S3 To W3": {
-            "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 12
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "angularVelocity": 0,
-                    "velocityX": -3.809622996347125e-33,
-                    "velocityY": 4.7029981856297106e-31,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.1621681366579715,
-                    "y": 2.9362991956112916,
-                    "heading": -0.5618109581410129,
-                    "angularVelocity": 0.03502487973446616,
-                    "velocityX": -0.8430624486077395,
-                    "velocityY": -0.5326069988815002,
-                    "timestamp": 0.11960189130537771
-                },
-                {
-                    "x": 3.949371568032143,
-                    "y": 2.832059073063443,
-                    "heading": -0.5332490097615643,
-                    "angularVelocity": 0.2388085010004113,
-                    "velocityX": -1.7792073879709687,
-                    "velocityY": -0.8715591485229383,
-                    "timestamp": 0.23920378261075542
-                },
-                {
-                    "x": 3.6196334559490815,
-                    "y": 2.7428786025637635,
-                    "heading": -0.4400400521459035,
-                    "angularVelocity": 0.7793267865446122,
-                    "velocityX": -2.756964028613438,
-                    "velocityY": -0.7456443165432319,
-                    "timestamp": 0.35880567391613316
-                },
-                {
-                    "x": 3.2124362068162218,
-                    "y": 2.7443781288173086,
-                    "heading": -0.34020495702705544,
-                    "angularVelocity": 0.8347283979314292,
-                    "velocityX": -3.4046054346513173,
-                    "velocityY": 0.012537646664080927,
-                    "timestamp": 0.47840756522151084
-                },
-                {
-                    "x": 2.829910158877178,
-                    "y": 2.8625567119312745,
-                    "heading": -0.23363822858373923,
-                    "angularVelocity": 0.891012067453128,
-                    "velocityX": -3.1983277502054186,
-                    "velocityY": 0.9880996180254248,
-                    "timestamp": 0.5980094565268885
-                },
-                {
-                    "x": 2.5228974587582895,
-                    "y": 3.073051960500077,
-                    "heading": -0.12255766699698727,
-                    "angularVelocity": 0.9287525504349555,
-                    "velocityX": -2.566955227614215,
-                    "velocityY": 1.7599658857512914,
-                    "timestamp": 0.7176113478322662
-                },
-                {
-                    "x": 2.307806503132395,
-                    "y": 3.359545759981809,
-                    "heading": -0.008191563913849259,
-                    "angularVelocity": 0.9562231987710198,
-                    "velocityX": -1.7983909224036065,
-                    "velocityY": 2.395395226236259,
-                    "timestamp": 0.8372132391376439
-                },
-                {
-                    "x": 2.197079056424326,
-                    "y": 3.692587049160117,
-                    "heading": -0.017161158826022753,
-                    "angularVelocity": -0.07499542703130056,
-                    "velocityX": -0.9258001315827844,
-                    "velocityY": 2.7845821294578066,
-                    "timestamp": 0.9568151304430216
-                },
-                {
-                    "x": 2.2005818293568207,
-                    "y": 4.024472502975606,
-                    "heading": -0.15274865723996348,
-                    "angularVelocity": -1.133656808718376,
-                    "velocityX": 0.029286935969546842,
-                    "velocityY": 2.774918107006309,
-                    "timestamp": 1.0764170217483993
-                },
-                {
-                    "x": 2.2590478504225837,
-                    "y": 4.250579625353832,
-                    "heading": -0.3027008584759083,
-                    "angularVelocity": -1.2537611203242134,
-                    "velocityX": 0.48883859968803833,
-                    "velocityY": 1.8904978835235247,
-                    "timestamp": 1.196018913053777
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": -0.729914389908579,
-                    "velocityX": 0.3424038627687911,
-                    "velocityY": 0.914871608232253,
-                    "timestamp": 1.3156208043591548
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": 3.1982065543283105e-28,
-                    "velocityX": 4.1170744495691044e-30,
-                    "velocityY": 1.8090480101607624e-30,
-                    "timestamp": 1.4352226956645326
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "S3 To C1": {
-            "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 23
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "angularVelocity": 3.2688003829398086e-32,
-                    "velocityX": -2.078713781970587e-32,
-                    "velocityY": -4.638967376158192e-34,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.29154268151456,
-                    "y": 3.0623077457400183,
-                    "heading": -0.5127665183876435,
-                    "angularVelocity": 0.580669777278592,
-                    "velocityX": 0.31134301225465,
-                    "velocityY": 0.6796516730776717,
-                    "timestamp": 0.09167599846825866
-                },
-                {
-                    "x": 4.3492977760941525,
-                    "y": 3.186661868715021,
-                    "heading": -0.4070750475472326,
-                    "angularVelocity": 1.1528804987818582,
-                    "velocityX": 0.6299914431757085,
-                    "velocityY": 1.3564523436094174,
-                    "timestamp": 0.18335199693651733
-                },
-                {
-                    "x": 4.438053987492854,
-                    "y": 3.3723523918382017,
-                    "heading": -0.25179906449139605,
-                    "angularVelocity": 1.6937473891773143,
-                    "velocityX": 0.968151019696088,
-                    "velocityY": 2.025508597950785,
-                    "timestamp": 0.27502799540477596
-                },
-                {
-                    "x": 4.5709698511757795,
-                    "y": 3.6117104026755453,
-                    "heading": -0.06452556421817643,
-                    "angularVelocity": 2.042775681773022,
-                    "velocityX": 1.449843643960376,
-                    "velocityY": 2.6109125053076734,
-                    "timestamp": 0.36670399387303465
-                },
-                {
-                    "x": 4.761013106791701,
-                    "y": 3.810602792551275,
-                    "heading": 0.11153690871840848,
-                    "angularVelocity": 1.9204860146414893,
-                    "velocityX": 2.0729881189319217,
-                    "velocityY": 2.1695143025313537,
-                    "timestamp": 0.45837999234129334
-                },
-                {
-                    "x": 5.007328053154023,
-                    "y": 3.968591564774131,
-                    "heading": 0.25752334504560864,
-                    "angularVelocity": 1.5924171949733021,
-                    "velocityX": 2.6867986220799525,
-                    "velocityY": 1.7233384404049508,
-                    "timestamp": 0.550055990809552
-                },
-                {
-                    "x": 5.305296422655045,
-                    "y": 4.081628168088871,
-                    "heading": 0.35020186087766364,
-                    "angularVelocity": 1.0109354398157564,
-                    "velocityX": 3.2502331524012615,
-                    "velocityY": 1.2330010602925316,
-                    "timestamp": 0.6417319892778107
-                },
-                {
-                    "x": 5.6446965706540295,
-                    "y": 4.152909996485894,
-                    "heading": 0.3502021264888923,
-                    "angularVelocity": 0.0000028972820926514052,
-                    "velocityX": 3.702170182705961,
-                    "velocityY": 0.7775407913523397,
-                    "timestamp": 0.7334079877460694
-                },
-                {
-                    "x": 5.981889969698284,
-                    "y": 4.233991234618027,
-                    "heading": 0.3502024148432213,
-                    "angularVelocity": 0.0000031453633856654694,
-                    "velocityX": 3.6780990082262504,
-                    "velocityY": 0.884432561268565,
-                    "timestamp": 0.8250839862143281
-                },
-                {
-                    "x": 6.295899946950055,
-                    "y": 4.381202864314355,
-                    "heading": 0.350202827556894,
-                    "angularVelocity": 0.000004501872678764248,
-                    "velocityX": 3.425214696303447,
-                    "velocityY": 1.6057815803042044,
-                    "timestamp": 0.9167599846825868
-                },
-                {
-                    "x": 6.57390491063703,
-                    "y": 4.588535293884794,
-                    "heading": 0.350203324900352,
-                    "angularVelocity": 0.0000054250127210279085,
-                    "velocityX": 3.0324727118541346,
-                    "velocityY": 2.2615780906081357,
-                    "timestamp": 1.0084359831508454
-                },
-                {
-                    "x": 6.804553755441848,
-                    "y": 4.847523149382792,
-                    "heading": 0.35020327996165895,
-                    "angularVelocity": -4.901903857536901e-7,
-                    "velocityX": 2.5159130923965525,
-                    "velocityY": 2.825034467311176,
-                    "timestamp": 1.100111981619104
-                },
-                {
-                    "x": 6.978428828519116,
-                    "y": 5.147591577808381,
-                    "heading": 0.35020324373344786,
-                    "angularVelocity": -3.9517661894346874e-7,
-                    "velocityX": 1.8966258997164847,
-                    "velocityY": 3.273140554116584,
-                    "timestamp": 1.1917879800873628
-                },
-                {
-                    "x": 7.088431556948234,
-                    "y": 5.476488224562697,
-                    "heading": 0.3502032113343976,
-                    "angularVelocity": -3.5340820734555857e-7,
-                    "velocityX": 1.1999076123202526,
-                    "velocityY": 3.587598196360961,
-                    "timestamp": 1.2834639785556214
-                },
-                {
-                    "x": 7.187536215850895,
-                    "y": 5.808831226900728,
-                    "heading": 0.3502031791295722,
-                    "angularVelocity": -3.5128960565806065e-7,
-                    "velocityX": 1.0810316828670656,
-                    "velocityY": 3.6251909757285055,
-                    "timestamp": 1.3751399770238801
-                },
-                {
-                    "x": 7.286640767886556,
-                    "y": 6.141174261106461,
-                    "heading": 0.3502031469247365,
-                    "angularVelocity": -3.512897194087527e-7,
-                    "velocityX": 1.0810305171639119,
-                    "velocityY": 3.6251913233407715,
-                    "timestamp": 1.4668159754921388
-                },
-                {
-                    "x": 7.3857452620780215,
-                    "y": 6.473517312561292,
-                    "heading": 0.350203114719879,
-                    "angularVelocity": -3.512899557892109e-7,
-                    "velocityX": 1.0810298862005645,
-                    "velocityY": 3.625191511493593,
-                    "timestamp": 1.5584919739603975
-                },
-                {
-                    "x": 7.481976228665169,
-                    "y": 6.80241098377653,
-                    "heading": 0.33998449018149474,
-                    "angularVelocity": -0.11146455679915658,
-                    "velocityX": 1.0496855032395944,
-                    "velocityY": 3.5875657392388547,
-                    "timestamp": 1.6501679724286562
-                },
-                {
-                    "x": 7.561455250393006,
-                    "y": 7.068914029375232,
-                    "heading": 0.2316909043200923,
-                    "angularVelocity": -1.1812643185871456,
-                    "velocityX": 0.8669556160367877,
-                    "velocityY": 2.907010014087551,
-                    "timestamp": 1.741843970896915
-                },
-                {
-                    "x": 7.620860593043084,
-                    "y": 7.268277067392774,
-                    "heading": 0.12481105079738766,
-                    "angularVelocity": -1.1658433538600752,
-                    "velocityX": 0.6479923168837385,
-                    "velocityY": 2.1746481232660675,
-                    "timestamp": 1.8335199693651736
-                },
-                {
-                    "x": 7.66032628893014,
-                    "y": 7.4008489817464715,
-                    "heading": 0.04379045785830094,
-                    "angularVelocity": -0.8837710446877722,
-                    "velocityX": 0.430491039600946,
-                    "velocityY": 1.4460918513977026,
-                    "timestamp": 1.9251959678334323
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 6.52409657127002e-31,
-                    "angularVelocity": -0.4776654586801488,
-                    "velocityX": 0.21460045593800534,
-                    "velocityY": 0.7215740145599008,
-                    "timestamp": 2.0168719663016907
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "angularVelocity": -6.745238901014272e-30,
-                    "velocityX": 0,
-                    "velocityY": 3.373152313643601e-31,
-                    "timestamp": 2.1085479647699494
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "S3 To C2": {
-            "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 20
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "angularVelocity": -2.8196964588027846e-31,
-                    "velocityX": -1.564828148658428e-31,
-                    "velocityY": 3.07478616058615e-32,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.294807569207289,
-                    "y": 3.0477321348748183,
-                    "heading": -0.5960373953696199,
-                    "angularVelocity": -0.36026847684168845,
-                    "velocityX": 0.381499939303545,
-                    "velocityY": 0.5724991570056517,
-                    "timestamp": 0.08337503084628527
-                },
-                {
-                    "x": 4.359939756522268,
-                    "y": 3.142193816557181,
-                    "heading": -0.6551934033575421,
-                    "angularVelocity": -0.7095170746861289,
-                    "velocityX": 0.7811953609355742,
-                    "velocityY": 1.1329732741750633,
-                    "timestamp": 0.16675006169257053
-                },
-                {
-                    "x": 4.461050205756915,
-                    "y": 3.2814565261988764,
-                    "heading": -0.7417418377173162,
-                    "angularVelocity": -1.038061797174498,
-                    "velocityX": 1.2127185826300881,
-                    "velocityY": 1.6703167390540123,
-                    "timestamp": 0.2501250925388558
-                },
-                {
-                    "x": 4.603825208189836,
-                    "y": 3.4604941356958814,
-                    "heading": -0.8514332359801334,
-                    "angularVelocity": -1.315638472926628,
-                    "velocityX": 1.7124431737380506,
-                    "velocityY": 2.147376830685539,
-                    "timestamp": 0.33350012338514107
-                },
-                {
-                    "x": 4.8030511460470855,
-                    "y": 3.6526461815261504,
-                    "heading": -0.9644207302174959,
-                    "angularVelocity": -1.3551718433024906,
-                    "velocityX": 2.3895156119888465,
-                    "velocityY": 2.3046713611960277,
-                    "timestamp": 0.41687515423142635
-                },
-                {
-                    "x": 5.051097476442208,
-                    "y": 3.8467657037250893,
-                    "heading": -0.9658198239131095,
-                    "angularVelocity": -0.016780727772000164,
-                    "velocityX": 2.975067329839247,
-                    "velocityY": 2.328269269930804,
-                    "timestamp": 0.5002501850777116
-                },
-                {
-                    "x": 5.320526470824812,
-                    "y": 4.010737925204951,
-                    "heading": -0.965820109061322,
-                    "angularVelocity": -0.000003420067250854742,
-                    "velocityX": 3.2315309709370754,
-                    "velocityY": 1.9666825884858703,
-                    "timestamp": 0.5836252159239969
-                },
-                {
-                    "x": 5.614243645591117,
-                    "y": 4.1256688963704224,
-                    "heading": -0.9658201941489747,
-                    "angularVelocity": -0.000001020541184209393,
-                    "velocityX": 3.5228433715103167,
-                    "velocityY": 1.37848190278171,
-                    "timestamp": 0.6670002467702821
-                },
-                {
-                    "x": 5.906557788271019,
-                    "y": 4.244122886725936,
-                    "heading": -0.965820105112684,
-                    "angularVelocity": 0.0000010679011417361832,
-                    "velocityX": 3.506015406684877,
-                    "velocityY": 1.4207369898777267,
-                    "timestamp": 0.7503752776165674
-                },
-                {
-                    "x": 6.172253111887642,
-                    "y": 4.41406398245588,
-                    "heading": -0.9657938677546691,
-                    "angularVelocity": 0.0003146908342779248,
-                    "velocityX": 3.1867493291423483,
-                    "velocityY": 2.0382732576526004,
-                    "timestamp": 0.8337503084628527
-                },
-                {
-                    "x": 6.426785390452526,
-                    "y": 4.5994304003649855,
-                    "heading": -0.9640144900160125,
-                    "angularVelocity": 0.021341854037057975,
-                    "velocityX": 3.0528597828545716,
-                    "velocityY": 2.2232845496735987,
-                    "timestamp": 0.917125339309138
-                },
-                {
-                    "x": 6.681598941757368,
-                    "y": 4.785246665794811,
-                    "heading": -0.9639002296367303,
-                    "angularVelocity": 0.0013704388246982366,
-                    "velocityX": 3.0562333676929114,
-                    "velocityY": 2.2286800201898096,
-                    "timestamp": 1.0005003701554231
-                },
-                {
-                    "x": 6.9014822245071255,
-                    "y": 5.01136660335564,
-                    "heading": -0.9639000238890051,
-                    "angularVelocity": 0.000002467737919950576,
-                    "velocityX": 2.6372797769052228,
-                    "velocityY": 2.7120822057351384,
-                    "timestamp": 1.0838754010017084
-                },
-                {
-                    "x": 7.122012116423131,
-                    "y": 5.23107273201418,
-                    "heading": -0.9515772318649645,
-                    "angularVelocity": 0.1477995498047812,
-                    "velocityX": 2.645035206314798,
-                    "velocityY": 2.635154990990089,
-                    "timestamp": 1.1672504318479937
-                },
-                {
-                    "x": 7.311148990719613,
-                    "y": 5.417840497645029,
-                    "heading": -0.7917020160601519,
-                    "angularVelocity": 1.9175431083146124,
-                    "velocityX": 2.2685073981583956,
-                    "velocityY": 2.2400923122317544,
-                    "timestamp": 1.250625462694279
-                },
-                {
-                    "x": 7.462007390054419,
-                    "y": 5.572027096128993,
-                    "heading": -0.5225549044290918,
-                    "angularVelocity": 3.228150069620655,
-                    "velocityX": 1.8093954245478587,
-                    "velocityY": 1.8493138403538476,
-                    "timestamp": 1.3340004935405643
-                },
-                {
-                    "x": 7.571928087470151,
-                    "y": 5.685474744918936,
-                    "heading": -0.27088873612536774,
-                    "angularVelocity": 3.018483660506339,
-                    "velocityX": 1.318388686636741,
-                    "velocityY": 1.3606909363440005,
-                    "timestamp": 1.4173755243868496
-                },
-                {
-                    "x": 7.6442805622690475,
-                    "y": 5.7599789636443175,
-                    "heading": -0.0924144798498188,
-                    "angularVelocity": 2.1406199729580164,
-                    "velocityX": 0.8677954786282445,
-                    "velocityY": 0.8936034921863096,
-                    "timestamp": 1.5007505552331348
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": -2.5332548087748344e-31,
-                    "angularVelocity": 1.1084191383412998,
-                    "velocityX": 0.42841888474747514,
-                    "velocityY": 0.4440302567795869,
-                    "timestamp": 1.5841255860794201
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "angularVelocity": 5.737670210968143e-33,
-                    "velocityX": 0,
-                    "velocityY": -3.2710687085311585e-31,
-                    "timestamp": 1.6675006169257054
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "S3 To C3": {
-            "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 17
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "angularVelocity": -1.0027901855857707e-30,
-                    "velocityX": -5.17158696002297e-32,
-                    "velocityY": 1.9371271312239428e-32,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.300239460670815,
-                    "y": 3.0530479185441526,
-                    "heading": -0.6028935811234369,
-                    "angularVelocity": -0.4158951640704737,
-                    "velocityX": 0.4197942063083121,
-                    "velocityY": 0.5980003056006388,
-                    "timestamp": 0.08870884855296232
-                },
-                {
-                    "x": 4.375862012794379,
-                    "y": 3.1584586937478076,
-                    "heading": -0.6740120078870278,
-                    "angularVelocity": -0.8017061197804932,
-                    "velocityX": 0.8524803709791626,
-                    "velocityY": 1.1882780232540273,
-                    "timestamp": 0.17741769710592464
-                },
-                {
-                    "x": 4.491898262382256,
-                    "y": 3.3149103934712074,
-                    "heading": -0.7745169367898204,
-                    "angularVelocity": -1.1329752391362358,
-                    "velocityX": 1.3080572172977813,
-                    "velocityY": 1.763653821185524,
-                    "timestamp": 0.26612654565888694
-                },
-                {
-                    "x": 4.65291407862912,
-                    "y": 3.5188640922932937,
-                    "heading": -0.893163244153065,
-                    "angularVelocity": -1.3374799616794542,
-                    "velocityX": 1.8151043427277864,
-                    "velocityY": 2.299135905256609,
-                    "timestamp": 0.3548353942118493
-                },
-                {
-                    "x": 4.870914712338585,
-                    "y": 3.7498942741357597,
-                    "heading": -0.9455687285938831,
-                    "angularVelocity": -0.5907582534963245,
-                    "velocityX": 2.457484650804715,
-                    "velocityY": 2.6043645657798487,
-                    "timestamp": 0.44354424276481164
-                },
-                {
-                    "x": 5.142859309500119,
-                    "y": 3.9465163862068953,
-                    "heading": -0.9455688623878027,
-                    "angularVelocity": -0.0000015082364580335284,
-                    "velocityX": 3.065585920655626,
-                    "velocityY": 2.216488155110533,
-                    "timestamp": 0.532253091317774
-                },
-                {
-                    "x": 5.447866241414174,
-                    "y": 4.086462405550872,
-                    "heading": -0.9455689961292948,
-                    "angularVelocity": -0.0000015076454528851402,
-                    "velocityX": 3.438292085732067,
-                    "velocityY": 1.5775880492961702,
-                    "timestamp": 0.6209619398707363
-                },
-                {
-                    "x": 5.774274975125892,
-                    "y": 4.16438201701327,
-                    "heading": -0.9455689173593601,
-                    "angularVelocity": 8.879602884106285e-7,
-                    "velocityX": 3.6795510147653454,
-                    "velocityY": 0.8783747363813094,
-                    "timestamp": 0.7096707884236987
-                },
-                {
-                    "x": 6.109606649873876,
-                    "y": 4.1772967927030695,
-                    "heading": -0.9455688167433821,
-                    "angularVelocity": 0.0000011342270768412302,
-                    "velocityX": 3.780137835379285,
-                    "velocityY": 0.14558610443566755,
-                    "timestamp": 0.798379636976661
-                },
-                {
-                    "x": 6.445060320326861,
-                    "y": 4.168079453807152,
-                    "heading": -0.9455687201101868,
-                    "angularVelocity": 0.0000010893298350239914,
-                    "velocityX": 3.781513072539862,
-                    "velocityY": -0.10390551840399499,
-                    "timestamp": 0.8870884855296234
-                },
-                {
-                    "x": 6.780513985890507,
-                    "y": 4.158861936977066,
-                    "heading": -0.9455686234766241,
-                    "angularVelocity": 0.0000010893339756447378,
-                    "velocityX": 3.781513017423149,
-                    "velocityY": -0.10390752422611307,
-                    "timestamp": 0.9757973340825857
-                },
-                {
-                    "x": 7.079210544299498,
-                    "y": 4.144050975293544,
-                    "heading": -0.856180476499644,
-                    "angularVelocity": 1.0076576174203435,
-                    "velocityX": 3.3671563015572095,
-                    "velocityY": -0.16696149172400718,
-                    "timestamp": 1.064506182635548
-                },
-                {
-                    "x": 7.324091725395792,
-                    "y": 4.136558502839443,
-                    "heading": -0.6317167360096363,
-                    "angularVelocity": 2.5303421716267107,
-                    "velocityX": 2.760504561730298,
-                    "velocityY": -0.08446138774564337,
-                    "timestamp": 1.1532150311885103
-                },
-                {
-                    "x": 7.50528856962614,
-                    "y": 4.13147246408345,
-                    "heading": -0.35065072299807204,
-                    "angularVelocity": 3.1684101146207286,
-                    "velocityX": 2.042601692909661,
-                    "velocityY": -0.05733406350050504,
-                    "timestamp": 1.2419238797414727
-                },
-                {
-                    "x": 7.622685008969739,
-                    "y": 4.128418008534293,
-                    "heading": -0.12509027443555182,
-                    "angularVelocity": 2.5427051781407424,
-                    "velocityX": 1.323390408720159,
-                    "velocityY": -0.03443236609404116,
-                    "timestamp": 1.330632728294435
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.405276849475189e-32,
-                    "angularVelocity": 1.4101217237744719,
-                    "velocityX": 0.6461022994345517,
-                    "velocityY": -0.01598497283444213,
-                    "timestamp": 1.4193415768473974
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "angularVelocity": -2.3485646679922316e-31,
-                    "velocityX": 0,
-                    "velocityY": -2.6996723431981715e-31,
-                    "timestamp": 1.5080504254003597
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "S3 To C4": {
-            "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 13
-                },
-                {
-                    "x": 5.136752128601074,
-                    "y": 1.409298062324524,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 12
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "angularVelocity": 0,
-                    "velocityX": 5.832812358477626e-33,
-                    "velocityY": -2.114400767819492e-31,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.2637473414422145,
-                    "y": 2.974020167404826,
-                    "heading": -0.5570803398431683,
-                    "angularVelocity": 0.15939480257853877,
-                    "velocityX": 0.013355031418934325,
-                    "velocityY": -0.46426099366130524,
-                    "timestamp": 0.05595954204614445
-                },
-                {
-                    "x": 4.266239342094047,
-                    "y": 2.922097369670203,
-                    "heading": -0.5396590874905302,
-                    "angularVelocity": 0.31131870840312753,
-                    "velocityX": 0.04453218451605929,
-                    "velocityY": -0.9278631639230981,
-                    "timestamp": 0.1119190840922889
-                },
-                {
-                    "x": 4.271816064940073,
-                    "y": 2.844342168020101,
-                    "heading": -0.5143157133438189,
-                    "angularVelocity": 0.4528874472527088,
-                    "velocityX": 0.0996563345966598,
-                    "velocityY": -1.3894895992176792,
-                    "timestamp": 0.16787862613843335
-                },
-                {
-                    "x": 4.282354343554946,
-                    "y": 2.7410289237168692,
-                    "heading": -0.48190029722305777,
-                    "angularVelocity": 0.5792652143941603,
-                    "velocityX": 0.18831960072480589,
-                    "velocityY": -1.8462131841257783,
-                    "timestamp": 0.2238381681845778
-                },
-                {
-                    "x": 4.3006106845058465,
-                    "y": 2.6128237251760056,
-                    "heading": -0.4437500478548854,
-                    "angularVelocity": 0.6817469902936669,
-                    "velocityX": 0.32624178617911886,
-                    "velocityY": -2.2910337335345985,
-                    "timestamp": 0.27979771023072225
-                },
-                {
-                    "x": 4.330801306241698,
-                    "y": 2.4614150778969766,
-                    "heading": -0.40214700400499553,
-                    "angularVelocity": 0.7434486117771366,
-                    "velocityX": 0.5395080201148661,
-                    "velocityY": -2.705680599640681,
-                    "timestamp": 0.3357572522768667
-                },
-                {
-                    "x": 4.379172513113433,
-                    "y": 2.2912661424924936,
-                    "heading": -0.3611794534128463,
-                    "angularVelocity": 0.7320923133783194,
-                    "velocityX": 0.864396045840541,
-                    "velocityY": -3.0405705476320635,
-                    "timestamp": 0.39171679432301115
-                },
-                {
-                    "x": 4.451949891834961,
-                    "y": 2.111933259909663,
-                    "heading": -0.32494756197390784,
-                    "angularVelocity": 0.6474658318159084,
-                    "velocityX": 1.3005356380778725,
-                    "velocityY": -3.2046881733762564,
-                    "timestamp": 0.4476763363691556
-                },
-                {
-                    "x": 4.550285732793113,
-                    "y": 1.9373903674143325,
-                    "heading": -0.2971520965733793,
-                    "angularVelocity": 0.49670644869849423,
-                    "velocityX": 1.7572667209653445,
-                    "velocityY": -3.1190907951212283,
-                    "timestamp": 0.5036358784153001
-                },
-                {
-                    "x": 4.671000177640236,
-                    "y": 1.7762856565305696,
-                    "heading": -0.2715476133805437,
-                    "angularVelocity": 0.457553479828869,
-                    "velocityX": 2.157173565637473,
-                    "velocityY": -2.878949773229245,
-                    "timestamp": 0.5595954204614446
-                },
-                {
-                    "x": 4.810884164899462,
-                    "y": 1.6328902215117949,
-                    "heading": -0.24301377044973924,
-                    "angularVelocity": 0.5099012945331528,
-                    "velocityX": 2.4997343106181424,
-                    "velocityY": -2.5624840693036233,
-                    "timestamp": 0.6155549625075891
-                },
-                {
-                    "x": 4.966899091534391,
-                    "y": 1.5100232904396305,
-                    "heading": -0.21315700918191657,
-                    "angularVelocity": 0.5335419157506132,
-                    "velocityX": 2.7879950573269263,
-                    "velocityY": -2.1956386092446096,
-                    "timestamp": 0.6715145045537336
-                },
-                {
-                    "x": 5.136752128601074,
-                    "y": 1.409298062324524,
-                    "heading": -0.1837509654270181,
-                    "angularVelocity": 0.5254875697633724,
-                    "velocityX": 3.035282828558936,
-                    "velocityY": -1.7999651968567865,
-                    "timestamp": 0.7274740465998781
-                },
-                {
-                    "x": 5.4382274814533025,
-                    "y": 1.3074419969375322,
-                    "heading": -0.14111678525881038,
-                    "angularVelocity": 0.4782734646509064,
-                    "velocityX": 3.3819733590888914,
-                    "velocityY": -1.142629061849971,
-                    "timestamp": 0.8166158939305274
-                },
-                {
-                    "x": 5.764139683697637,
-                    "y": 1.267066681764717,
-                    "heading": -0.11133632809533998,
-                    "angularVelocity": 0.3340794257159682,
-                    "velocityX": 3.6561077877984602,
-                    "velocityY": -0.4529333459183563,
-                    "timestamp": 0.9057577412611766
-                },
-                {
-                    "x": 6.100462721846291,
-                    "y": 1.2915989985388854,
-                    "heading": -0.11133120767369757,
-                    "angularVelocity": 0.000057441278584387276,
-                    "velocityX": 3.7728973340784897,
-                    "velocityY": 0.2752053890377148,
-                    "timestamp": 0.9948995885918258
-                },
-                {
-                    "x": 6.42549869748969,
-                    "y": 1.3814198753662668,
-                    "heading": -0.1113311879575033,
-                    "angularVelocity": 2.2117776172639892e-7,
-                    "velocityX": 3.6462782113742715,
-                    "velocityY": 1.0076174043625032,
-                    "timestamp": 1.084041435922475
-                },
-                {
-                    "x": 6.7266979846048365,
-                    "y": 1.533061465523067,
-                    "heading": -0.11133117504978503,
-                    "angularVelocity": 1.4479976182990285e-7,
-                    "velocityX": 3.3788764327254777,
-                    "velocityY": 1.7011268522889103,
-                    "timestamp": 1.1731832832531242
-                },
-                {
-                    "x": 6.992432961420095,
-                    "y": 1.740669527522903,
-                    "heading": -0.11133116451645807,
-                    "angularVelocity": 1.1816366034373416e-7,
-                    "velocityX": 2.981035111708839,
-                    "velocityY": 2.3289629754897407,
-                    "timestamp": 1.2623251305837733
-                },
-                {
-                    "x": 7.221595592407175,
-                    "y": 1.979432543612985,
-                    "heading": -0.09587073631794915,
-                    "angularVelocity": 0.17343625537800947,
-                    "velocityX": 2.5707637641507293,
-                    "velocityY": 2.678461611912118,
-                    "timestamp": 1.3514669779144224
-                },
-                {
-                    "x": 7.404993837760709,
-                    "y": 2.170501480308595,
-                    "heading": -0.06411284637687509,
-                    "angularVelocity": 0.3562624164974272,
-                    "velocityX": 2.0573754173309005,
-                    "velocityY": 2.1434258142181197,
-                    "timestamp": 1.4406088252450715
-                },
-                {
-                    "x": 7.54251389523076,
-                    "y": 2.3137697288578556,
-                    "heading": -0.0342508837604198,
-                    "angularVelocity": 0.33499376006509896,
-                    "velocityX": 1.5427104282454527,
-                    "velocityY": 1.6071940714649837,
-                    "timestamp": 1.5297506725757206
-                },
-                {
-                    "x": 7.634176122098128,
-                    "y": 2.4092619199619465,
-                    "heading": -0.011960250388889534,
-                    "angularVelocity": 0.25005801471490263,
-                    "velocityX": 1.028273808679041,
-                    "velocityY": 1.0712386377846093,
-                    "timestamp": 1.6188925199063697
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 9.313254165846089e-33,
-                    "angularVelocity": 0.1341709954082624,
-                    "velocityX": 0.5140557355952123,
-                    "velocityY": 0.5355294002487934,
-                    "timestamp": 1.7080343672370188
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "angularVelocity": -1.1876173054192403e-31,
-                    "velocityX": 4.5955148793706025e-32,
-                    "velocityY": 0,
-                    "timestamp": 1.797176214567668
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "S3 To C5": {
-            "waypoints": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 19
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "angularVelocity": -2.2118032332823513e-27,
-                    "velocityX": -2.2398078489200873e-28,
-                    "velocityY": 7.32316842173409e-29,
-                    "timestamp": 0
-                },
-                {
-                    "x": 4.257275610783411,
-                    "y": 2.9390850995701614,
-                    "heading": -0.6153074756658753,
-                    "angularVelocity": -0.5685220144659363,
-                    "velocityX": -0.06600299944485946,
-                    "velocityY": -0.7023572274929988,
-                    "timestamp": 0.0867292284401611
-                },
-                {
-                    "x": 4.259999029271463,
-                    "y": 2.8165594202827866,
-                    "heading": -0.6936339270516045,
-                    "angularVelocity": -0.9031148183195571,
-                    "velocityX": 0.03140139186100546,
-                    "velocityY": -1.4127380295088343,
-                    "timestamp": 0.1734584568803222
-                },
-                {
-                    "x": 4.293815085802935,
-                    "y": 2.6396142169455077,
-                    "heading": -0.7636289676296801,
-                    "angularVelocity": -0.8070524993355632,
-                    "velocityX": 0.3899038091270867,
-                    "velocityY": -2.0402026689233352,
-                    "timestamp": 0.2601876853204833
-                },
-                {
-                    "x": 4.379794167011224,
-                    "y": 2.432035667571809,
-                    "heading": -0.7730870281638328,
-                    "angularVelocity": -0.10905274616477362,
-                    "velocityX": 0.9913506986587586,
-                    "velocityY": -2.3934093858210397,
-                    "timestamp": 0.3469169137606444
-                },
-                {
-                    "x": 4.522000800858502,
-                    "y": 2.2059673134468256,
-                    "heading": -0.7069342437539652,
-                    "angularVelocity": 0.7627507542686203,
-                    "velocityX": 1.6396621577856372,
-                    "velocityY": -2.6065993920487904,
-                    "timestamp": 0.43364614220080544
-                },
-                {
-                    "x": 4.726803922559788,
-                    "y": 1.9764479224139355,
-                    "heading": -0.6474090008701758,
-                    "angularVelocity": 0.6863342837743838,
-                    "velocityX": 2.3614083208706247,
-                    "velocityY": -2.6463903249323897,
-                    "timestamp": 0.5203753706409665
-                },
-                {
-                    "x": 4.985426543182091,
-                    "y": 1.774560944536545,
-                    "heading": -0.6474088034070749,
-                    "angularVelocity": 0.000002276776866191791,
-                    "velocityX": 2.9819545875556583,
-                    "velocityY": -2.3277847792302735,
-                    "timestamp": 0.6071045990811276
-                },
-                {
-                    "x": 5.274431548584119,
-                    "y": 1.6192543598542732,
-                    "heading": -0.6474086941993331,
-                    "angularVelocity": 0.000001259180368575944,
-                    "velocityX": 3.3322676864516145,
-                    "velocityY": -1.7907064028526984,
-                    "timestamp": 0.6938338275212886
-                },
-                {
-                    "x": 5.576835908979959,
-                    "y": 1.4919922741488194,
-                    "heading": -0.6474086295363626,
-                    "angularVelocity": 7.455729944345581e-7,
-                    "velocityX": 3.4867641028824017,
-                    "velocityY": -1.4673494506325018,
-                    "timestamp": 0.7805630559614497
-                },
-                {
-                    "x": 5.888110144336631,
-                    "y": 1.388298557512795,
-                    "heading": -0.6474085736154427,
-                    "angularVelocity": 6.447759407143774e-7,
-                    "velocityX": 3.589034988031027,
-                    "velocityY": -1.1956028953671916,
-                    "timestamp": 0.8672922844016108
-                },
-                {
-                    "x": 6.199390629137113,
-                    "y": 1.2846236026664921,
-                    "heading": -0.6474085176958322,
-                    "angularVelocity": 6.447608435198244e-7,
-                    "velocityX": 3.5891070449826046,
-                    "velocityY": -1.1953865693365275,
-                    "timestamp": 0.9540215128417718
-                },
-                {
-                    "x": 6.510671944712167,
-                    "y": 1.1809511422971397,
-                    "heading": -0.6474084617763125,
-                    "angularVelocity": 6.447597973785494e-7,
-                    "velocityX": 3.589116623928252,
-                    "velocityY": -1.1953578076724478,
-                    "timestamp": 1.040750741281933
-                },
-                {
-                    "x": 6.8219538378473334,
-                    "y": 1.0772804160640193,
-                    "heading": -0.6474084058668956,
-                    "angularVelocity": 6.446433092317229e-7,
-                    "velocityX": 3.5891232832762197,
-                    "velocityY": -1.1953378128419065,
-                    "timestamp": 1.127479969722094
-                },
-                {
-                    "x": 7.104843996542823,
-                    "y": 0.9795715918943764,
-                    "heading": -0.5659567926753784,
-                    "angularVelocity": 0.9391483662017565,
-                    "velocityX": 3.2617626581409067,
-                    "velocityY": -1.126596257420401,
-                    "timestamp": 1.2142091981622551
-                },
-                {
-                    "x": 7.336171547926519,
-                    "y": 0.9014041069183023,
-                    "heading": -0.36771202193482067,
-                    "angularVelocity": 2.2857896271651064,
-                    "velocityX": 2.6672386638755765,
-                    "velocityY": -0.9012819136285574,
-                    "timestamp": 1.3009384266024162
-                },
-                {
-                    "x": 7.508580909570239,
-                    "y": 0.8437450323938049,
-                    "heading": -0.19262457792893875,
-                    "angularVelocity": 2.018782446873579,
-                    "velocityX": 1.9879037868147627,
-                    "velocityY": -0.6648171044699364,
-                    "timestamp": 1.3876676550425773
-                },
-                {
-                    "x": 7.622996353926532,
-                    "y": 0.8057914476404057,
-                    "heading": -0.06643070405801102,
-                    "angularVelocity": 1.4550328204291798,
-                    "velocityX": 1.3192258989739947,
-                    "velocityY": -0.4376100818144245,
-                    "timestamp": 1.4743968834827383
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": -1.2156690964354266e-28,
-                    "angularVelocity": 0.7659552062526378,
-                    "velocityX": 0.6572599237729437,
-                    "velocityY": -0.21666799046150395,
-                    "timestamp": 1.5611261119228994
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "angularVelocity": 1.25023472254267e-27,
-                    "velocityX": 0,
-                    "velocityY": -4.9030509543456705e-29,
-                    "timestamp": 1.6478553403630605
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "S4 To W1": {
-            "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 26
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "angularVelocity": 0,
-                    "velocityX": -4.2485019622555225e-35,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.123544064624381,
-                    "y": 1.640479835564366,
-                    "heading": -0.5261105113915723,
-                    "angularVelocity": 1.3044458865146935,
-                    "velocityX": -0.6008098724480858,
-                    "velocityY": 0.23456806756139406,
-                    "timestamp": 0.08730871076049408
-                },
-                {
-                    "x": 5.016418569113309,
-                    "y": 1.6815339921073649,
-                    "heading": -0.31680443246750034,
-                    "angularVelocity": 2.397310384049101,
-                    "velocityX": -1.2269737415426907,
-                    "velocityY": 0.4702183342922002,
-                    "timestamp": 0.17461742152098816
-                },
-                {
-                    "x": 4.8522086939147036,
-                    "y": 1.7459427773647251,
-                    "heading": -0.051891218250848345,
-                    "angularVelocity": 3.0342128741697256,
-                    "velocityX": -1.8807960141464901,
-                    "velocityY": 0.7377131639710823,
-                    "timestamp": 0.26192613228148226
-                },
-                {
-                    "x": 4.632595451769563,
-                    "y": 1.839668047804236,
-                    "heading": 0.17554509517769362,
-                    "angularVelocity": 2.6049670353333583,
-                    "velocityX": -2.515364620920639,
-                    "velocityY": 1.073492777789593,
-                    "timestamp": 0.34923484304197633
-                },
-                {
-                    "x": 4.3704901470738005,
-                    "y": 1.9627849470607086,
-                    "heading": 0.27538033292006053,
-                    "angularVelocity": 1.1434739658020574,
-                    "velocityX": -3.002052171114658,
-                    "velocityY": 1.4101330575617794,
-                    "timestamp": 0.4365435538024704
-                },
-                {
-                    "x": 4.075718453010027,
-                    "y": 2.1117712847634245,
-                    "heading": 0.2753804447713452,
-                    "angularVelocity": 0.0000012811010915301473,
-                    "velocityX": -3.3762002839830365,
-                    "velocityY": 1.7064315393617047,
-                    "timestamp": 0.5238522645629645
-                },
-                {
-                    "x": 3.780946849777969,
-                    "y": 2.260757802178556,
-                    "heading": 0.2753805566219007,
-                    "angularVelocity": 0.0000012810927397041138,
-                    "velocityX": -3.3761992436319233,
-                    "velocityY": 1.7064335977177825,
-                    "timestamp": 0.6111609753234586
-                },
-                {
-                    "x": 3.4861752863833098,
-                    "y": 2.409744398412427,
-                    "heading": 0.27538066847245973,
-                    "angularVelocity": 0.0000012810927810381003,
-                    "velocityX": -3.3761987873498507,
-                    "velocityY": 1.7064345004769594,
-                    "timestamp": 0.6984696860839527
-                },
-                {
-                    "x": 3.191403823869153,
-                    "y": 2.5587311942392557,
-                    "heading": 0.2753807803230405,
-                    "angularVelocity": 0.000001281093028963251,
-                    "velocityX": -3.3761976319038296,
-                    "velocityY": 1.7064367865370367,
-                    "timestamp": 0.7857783968444467
-                },
-                {
-                    "x": 2.907963654667952,
-                    "y": 2.72828634801538,
-                    "heading": 0.27538089480545036,
-                    "angularVelocity": 0.0000013112369766496227,
-                    "velocityX": -3.246413407463273,
-                    "velocityY": 1.9420187550501031,
-                    "timestamp": 0.8730871076049408
-                },
-                {
-                    "x": 2.662249448997808,
-                    "y": 2.9489938499602837,
-                    "heading": 0.27538103946019826,
-                    "angularVelocity": 0.000001656819194794751,
-                    "velocityX": -2.81431490088302,
-                    "velocityY": 2.527897846875211,
-                    "timestamp": 0.9603958183654349
-                },
-                {
-                    "x": 2.463361015566044,
-                    "y": 3.212680537293485,
-                    "heading": 0.27538116270778873,
-                    "angularVelocity": 0.0000014116299433562534,
-                    "velocityX": -2.277990726232991,
-                    "velocityY": 3.0201647125055904,
-                    "timestamp": 1.047704529125929
-                },
-                {
-                    "x": 2.3186638703555946,
-                    "y": 3.509581240155329,
-                    "heading": 0.2753811333053821,
-                    "angularVelocity": -3.3676372418890626e-7,
-                    "velocityX": -1.657304797540575,
-                    "velocityY": 3.4005851223287915,
-                    "timestamp": 1.1350132398864232
-                },
-                {
-                    "x": 2.233516432992889,
-                    "y": 3.8287006658490608,
-                    "heading": 0.2753811088610493,
-                    "angularVelocity": -2.799758765995743e-7,
-                    "velocityX": -0.9752456154836949,
-                    "velocityY": 3.655069728026828,
-                    "timestamp": 1.2223219506469174
-                },
-                {
-                    "x": 2.1898375161467403,
-                    "y": 4.15608338798651,
-                    "heading": 0.27538108549661844,
-                    "angularVelocity": -2.6760709935894875e-7,
-                    "velocityX": -0.5002813174732252,
-                    "velocityY": 3.749714310127982,
-                    "timestamp": 1.3096306614074116
-                },
-                {
-                    "x": 2.1796859113101297,
-                    "y": 4.486211000706444,
-                    "heading": 0.2753810837562719,
-                    "angularVelocity": -1.9933252293602923e-8,
-                    "velocityX": -0.11627253166592438,
-                    "velocityY": 3.78115321878412,
-                    "timestamp": 1.3969393721679058
-                },
-                {
-                    "x": 2.197179274626327,
-                    "y": 4.816031069152981,
-                    "heading": 0.2753810893961792,
-                    "angularVelocity": 6.459730400705482e-8,
-                    "velocityX": 0.20036217650934227,
-                    "velocityY": 3.7776307263464504,
-                    "timestamp": 1.4842480829284
-                },
-                {
-                    "x": 2.214672942788018,
-                    "y": 5.1458511214306375,
-                    "heading": 0.27538109503608554,
-                    "angularVelocity": 6.459729260879602e-8,
-                    "velocityX": 0.20036566809100828,
-                    "velocityY": 3.777630541154376,
-                    "timestamp": 1.5715567936888941
-                },
-                {
-                    "x": 2.2321666974024836,
-                    "y": 5.475671169122818,
-                    "heading": 0.2753811006759881,
-                    "angularVelocity": 6.459724966098765e-8,
-                    "velocityX": 0.20036665828744704,
-                    "velocityY": 3.7776304886341237,
-                    "timestamp": 1.6588655044493883
-                },
-                {
-                    "x": 2.2496605217353887,
-                    "y": 5.805491213117091,
-                    "heading": 0.2753811063158993,
-                    "angularVelocity": 6.459734855412882e-8,
-                    "velocityX": 0.20036745681532722,
-                    "velocityY": 3.7776304462797263,
-                    "timestamp": 1.7461742152098825
-                },
-                {
-                    "x": 2.2664262161021513,
-                    "y": 6.122693971906954,
-                    "heading": 0.30917552472065174,
-                    "angularVelocity": 0.3870681185232194,
-                    "velocityX": 0.19202773950877436,
-                    "velocityY": 3.633116971112041,
-                    "timestamp": 1.8334829259703767
-                },
-                {
-                    "x": 2.279851444819284,
-                    "y": 6.376429629998848,
-                    "heading": 0.34011357351362637,
-                    "angularVelocity": 0.35435237244361656,
-                    "velocityX": 0.15376734578020115,
-                    "velocityY": 2.9061894956614607,
-                    "timestamp": 1.9207916367308708
-                },
-                {
-                    "x": 2.289924296428913,
-                    "y": 6.566719611746641,
-                    "heading": 0.3645867540299274,
-                    "angularVelocity": 0.2803062867740199,
-                    "velocityX": 0.11537052284807026,
-                    "velocityY": 2.179507406423607,
-                    "timestamp": 2.008100347491365
-                },
-                {
-                    "x": 2.296641079194263,
-                    "y": 6.6935744437684805,
-                    "heading": 0.3814115840440414,
-                    "angularVelocity": 0.192705056203017,
-                    "velocityX": 0.07693141619941274,
-                    "velocityY": 1.4529458849739352,
-                    "timestamp": 2.095409058251859
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "angularVelocity": 0.09836837448577633,
-                    "velocityX": 0.03847177190545204,
-                    "velocityY": 0.7264516412973858,
-                    "timestamp": 2.182717769012353
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.39,
-                    "angularVelocity": 2.6936232860428826e-33,
-                    "velocityX": 0,
-                    "velocityY": 5.62663175256514e-35,
-                    "timestamp": 2.2700264797728473
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "S4 To W2": {
-            "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 21
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "angularVelocity": -2.256840111784074e-31,
-                    "velocityX": -2.1308644711626517e-33,
-                    "velocityY": -1.0217798620245829e-33,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.1132615869041915,
-                    "y": 1.6556053336408048,
-                    "heading": -0.6461329738139044,
-                    "angularVelocity": -0.06592692141286781,
-                    "velocityX": -0.6744118848768044,
-                    "velocityY": 0.3827425493802767,
-                    "timestamp": 0.09302684976743651
-                },
-                {
-                    "x": 4.987784771636742,
-                    "y": 1.7268160147391884,
-                    "heading": -0.6583988506620269,
-                    "angularVelocity": -0.131853081973501,
-                    "velocityX": -1.348823652323362,
-                    "velocityY": 0.7654852472851404,
-                    "timestamp": 0.18605369953487302
-                },
-                {
-                    "x": 4.799569566809376,
-                    "y": 1.8336320552005005,
-                    "heading": -0.6767974581881981,
-                    "angularVelocity": -0.19777738977689827,
-                    "velocityX": -2.02323528419909,
-                    "velocityY": 1.148228073167565,
-                    "timestamp": 0.2790805493023095
-                },
-                {
-                    "x": 4.54861599428641,
-                    "y": 1.9760534609244509,
-                    "heading": -0.7013284716868837,
-                    "angularVelocity": -0.26369820713065356,
-                    "velocityX": -2.697646681042515,
-                    "velocityY": 1.5309709624694186,
-                    "timestamp": 0.37210739906974605
-                },
-                {
-                    "x": 4.242554383646119,
-                    "y": 2.1497500028424636,
-                    "heading": -0.7013284745367188,
-                    "angularVelocity": -3.063454187141868e-8,
-                    "velocityX": -3.29003520387323,
-                    "velocityY": 1.8671656876724017,
-                    "timestamp": 0.4651342488371826
-                },
-                {
-                    "x": 3.9364928269084327,
-                    "y": 2.3234466397396827,
-                    "heading": -0.7013284773864732,
-                    "angularVelocity": -3.0633677229161635e-8,
-                    "velocityX": -3.290034624442601,
-                    "velocityY": 1.8671667086594281,
-                    "timestamp": 0.558161098604619
-                },
-                {
-                    "x": 3.630431318940711,
-                    "y": 2.497143362571822,
-                    "heading": -0.7013284802362207,
-                    "angularVelocity": -3.063360291364155e-8,
-                    "velocityX": -3.2900341001857365,
-                    "velocityY": 1.867167632424124,
-                    "timestamp": 0.6511879483720555
-                },
-                {
-                    "x": 3.3243698648507793,
-                    "y": 2.6708401803390394,
-                    "heading": -0.7013284830859674,
-                    "angularVelocity": -3.063359264364299e-8,
-                    "velocityX": -3.2900335210218676,
-                    "velocityY": 1.8671686529367946,
-                    "timestamp": 0.744214798139492
-                },
-                {
-                    "x": 3.0183085209148324,
-                    "y": 2.844537192202297,
-                    "heading": -0.7013284859357423,
-                    "angularVelocity": -3.063389607722874e-8,
-                    "velocityX": -3.290032336912287,
-                    "velocityY": 1.8671707393886097,
-                    "timestamp": 0.8372416479069285
-                },
-                {
-                    "x": 2.714360997870643,
-                    "y": 3.021907148464231,
-                    "heading": -0.7013284887932081,
-                    "angularVelocity": -3.071657002046057e-8,
-                    "velocityX": -3.2673096402172703,
-                    "velocityY": 1.9066533662630898,
-                    "timestamp": 0.9302684976743649
-                },
-                {
-                    "x": 2.4529780195268613,
-                    "y": 3.2575406255854364,
-                    "heading": -0.7013284919399162,
-                    "angularVelocity": -3.382580562568125e-8,
-                    "velocityX": -2.8097584621776335,
-                    "velocityY": 2.532962018065539,
-                    "timestamp": 1.0232953474418014
-                },
-                {
-                    "x": 2.2451503238556145,
-                    "y": 3.5415333045009083,
-                    "heading": -0.7013284961513817,
-                    "angularVelocity": -4.527150531202637e-8,
-                    "velocityX": -2.2340614155032408,
-                    "velocityY": 3.0528033532839474,
-                    "timestamp": 1.1163221972092379
-                },
-                {
-                    "x": 2.099615704405981,
-                    "y": 3.861945316168419,
-                    "heading": -0.7013284332934493,
-                    "angularVelocity": 6.756966674200994e-7,
-                    "velocityX": -1.564436717071087,
-                    "velocityY": 3.444296055047848,
-                    "timestamp": 1.2093490469766743
-                },
-                {
-                    "x": 2.0224929199626733,
-                    "y": 4.205305507968674,
-                    "heading": -0.7013282219960775,
-                    "angularVelocity": 0.0000022713589924444385,
-                    "velocityX": -0.8290379028862299,
-                    "velocityY": 3.6909794608614943,
-                    "timestamp": 1.3023758967441108
-                },
-                {
-                    "x": 2.0170244087221176,
-                    "y": 4.5571780060792255,
-                    "heading": -0.7013280555552984,
-                    "angularVelocity": 0.0000017891692504432307,
-                    "velocityX": -0.05878422470745493,
-                    "velocityY": 3.782483218449498,
-                    "timestamp": 1.3954027465115473
-                },
-                {
-                    "x": 2.079587370397571,
-                    "y": 4.887709205686539,
-                    "heading": -0.661535006789308,
-                    "angularVelocity": 0.4277587477752017,
-                    "velocityX": 0.6725258549747536,
-                    "velocityY": 3.5530731227987227,
-                    "timestamp": 1.4884295962789837
-                },
-                {
-                    "x": 2.143441139356313,
-                    "y": 5.160361503306635,
-                    "heading": -0.462273406431507,
-                    "angularVelocity": 2.1419794484704924,
-                    "velocityX": 0.6864014971846709,
-                    "velocityY": 2.9308989641347156,
-                    "timestamp": 1.5814564460464202
-                },
-                {
-                    "x": 2.1958862911911914,
-                    "y": 5.3618445595482385,
-                    "heading": -0.25085794958853325,
-                    "angularVelocity": 2.272628358065485,
-                    "velocityX": 0.5637636012182422,
-                    "velocityY": 2.165859176628076,
-                    "timestamp": 1.6744832958138567
-                },
-                {
-                    "x": 2.231834886718389,
-                    "y": 5.494391410801975,
-                    "heading": -0.08836645960984135,
-                    "angularVelocity": 1.7467160328971074,
-                    "velocityX": 0.38643247209883785,
-                    "velocityY": 1.4248236029178551,
-                    "timestamp": 1.7675101455812932
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": -3.8128513304390615e-33,
-                    "angularVelocity": 0.9499027413134385,
-                    "velocityX": 0.19526742362042082,
-                    "velocityY": 0.7052650859622128,
-                    "timestamp": 1.8605369953487296
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 5.82518331797089e-32,
-                    "velocityX": 0,
-                    "velocityY": -4.1086986258215814e-33,
-                    "timestamp": 1.953563845116166
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "S4 To W3": {
-            "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 17
-                },
-                {
-                    "x": 2.1829307079315186,
-                    "y": 3.053825616836548,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 10
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "angularVelocity": -2.137980318394988e-34,
-                    "velocityX": 1.3258659962604387e-36,
-                    "velocityY": 7.61509960712835e-36,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.139985955005962,
-                    "y": 1.6266018860262446,
-                    "heading": -0.6327443563347177,
-                    "angularVelocity": 0.10940864810395588,
-                    "velocityX": -0.5430597418126536,
-                    "velocityY": 0.09955056482775779,
-                    "timestamp": 0.06631691178916982
-                },
-                {
-                    "x": 5.067957879081044,
-                    "y": 1.6398056681484225,
-                    "heading": -0.6182319813670383,
-                    "angularVelocity": 0.2188336968074761,
-                    "velocityX": -1.0861192715653594,
-                    "velocityY": 0.19910128149746392,
-                    "timestamp": 0.13263382357833964
-                },
-                {
-                    "x": 4.959915797050431,
-                    "y": 1.6596113536414707,
-                    "heading": -0.5964609094580124,
-                    "angularVelocity": 0.328288385596709,
-                    "velocityX": -1.6291784269764191,
-                    "velocityY": 0.2986521078667379,
-                    "timestamp": 0.19895073536750946
-                },
-                {
-                    "x": 4.815859744494909,
-                    "y": 1.686018943789264,
-                    "heading": -0.5674282989321845,
-                    "angularVelocity": 0.43778592432238833,
-                    "velocityX": -2.172237045860861,
-                    "velocityY": 0.39820295359570684,
-                    "timestamp": 0.2652676471566793
-                },
-                {
-                    "x": 4.635789767966758,
-                    "y": 1.719028429378711,
-                    "heading": -0.5311304969514992,
-                    "angularVelocity": 0.5473385445944894,
-                    "velocityX": -2.7152949627783816,
-                    "velocityY": 0.49775366039945246,
-                    "timestamp": 0.3315845589458491
-                },
-                {
-                    "x": 4.41970592970577,
-                    "y": 1.758639783689655,
-                    "heading": -0.48756329690665473,
-                    "angularVelocity": 0.6569545967904882,
-                    "velocityX": -3.258351941175237,
-                    "velocityY": 0.5973039642870227,
-                    "timestamp": 0.3979014707350189
-                },
-                {
-                    "x": 4.172944378919847,
-                    "y": 1.8038720825822125,
-                    "heading": -0.4875632898742738,
-                    "angularVelocity": 1.0604204507812919e-7,
-                    "velocityX": -3.7209445393116334,
-                    "velocityY": 0.6820628052819626,
-                    "timestamp": 0.46421838252418873
-                },
-                {
-                    "x": 3.9261828364502485,
-                    "y": 1.849104426845956,
-                    "heading": -0.4875632828424199,
-                    "angularVelocity": 1.0603409772331237e-7,
-                    "velocityX": -3.7209444139088643,
-                    "velocityY": 0.6820634894390606,
-                    "timestamp": 0.5305352943133586
-                },
-                {
-                    "x": 3.6794213056437455,
-                    "y": 1.8943368347367484,
-                    "heading": -0.487563275810563,
-                    "angularVelocity": 1.0603414195839125e-7,
-                    "velocityX": -3.720944238039768,
-                    "velocityY": 0.6820644488783205,
-                    "timestamp": 0.5968522061025283
-                },
-                {
-                    "x": 3.4326599282156307,
-                    "y": 1.9395700793621615,
-                    "heading": -0.4875632687787025,
-                    "angularVelocity": 1.0603419804191013e-7,
-                    "velocityX": -3.720941925230195,
-                    "velocityY": 0.6820770660916139,
-                    "timestamp": 0.6631691178916981
-                },
-                {
-                    "x": 3.192328571865682,
-                    "y": 2.011528836252569,
-                    "heading": -0.4875632615410612,
-                    "angularVelocity": 1.0913718718412795e-7,
-                    "velocityX": -3.6239829308396314,
-                    "velocityY": 1.0850740022269671,
-                    "timestamp": 0.7294860296808678
-                },
-                {
-                    "x": 2.965054771843869,
-                    "y": 2.1177542675713954,
-                    "heading": -0.48756325344928275,
-                    "angularVelocity": 1.2201681646141545e-7,
-                    "velocityX": -3.4270866041583203,
-                    "velocityY": 1.6017849512735258,
-                    "timestamp": 0.7958029414700376
-                },
-                {
-                    "x": 2.7556945515371734,
-                    "y": 2.2559769801698577,
-                    "heading": -0.48756324342419455,
-                    "angularVelocity": 1.5116940667572282e-7,
-                    "velocityX": -3.156965767228115,
-                    "velocityY": 2.084275471660842,
-                    "timestamp": 0.8621198532592074
-                },
-                {
-                    "x": 2.5687211592217127,
-                    "y": 2.4232437322251608,
-                    "heading": -0.4875632292213124,
-                    "angularVelocity": 2.1416682100483036e-7,
-                    "velocityX": -2.8193923280063653,
-                    "velocityY": 2.5222337340898235,
-                    "timestamp": 0.9284367650483771
-                },
-                {
-                    "x": 2.4081295337898796,
-                    "y": 2.6159806661875145,
-                    "heading": -0.48756314249031313,
-                    "angularVelocity": 0.0000013078262679985986,
-                    "velocityX": -2.421578766248568,
-                    "velocityY": 2.906301405817713,
-                    "timestamp": 0.9947536768375469
-                },
-                {
-                    "x": 2.2783797783684188,
-                    "y": 2.8285106690600896,
-                    "heading": -0.4828540771785898,
-                    "angularVelocity": 0.07100851328382284,
-                    "velocityX": -1.9565108193510647,
-                    "velocityY": 3.2047632668456583,
-                    "timestamp": 1.0610705886267167
-                },
-                {
-                    "x": 2.1829307079315186,
-                    "y": 3.053825616836548,
-                    "heading": -0.47038335932986286,
-                    "angularVelocity": 0.18804732476646344,
-                    "velocityX": -1.4392870213912456,
-                    "velocityY": 3.3975488559051734,
-                    "timestamp": 1.1273875004158864
-                },
-                {
-                    "x": 2.124568521000596,
-                    "y": 3.2749812318761844,
-                    "heading": -0.4550563591461243,
-                    "angularVelocity": 0.24322153086635045,
-                    "velocityX": -0.9261395106595871,
-                    "velocityY": 3.5094804335357668,
-                    "timestamp": 1.1904041261609186
-                },
-                {
-                    "x": 2.099309698866093,
-                    "y": 3.4965263857952165,
-                    "heading": -0.4375068558264715,
-                    "angularVelocity": 0.2784900510328627,
-                    "velocityX": -0.40082790590377965,
-                    "velocityY": 3.5156619590425686,
-                    "timestamp": 1.2534207519059508
-                },
-                {
-                    "x": 2.105558929434475,
-                    "y": 3.707892405315209,
-                    "heading": -0.4199092504656253,
-                    "angularVelocity": 0.2792533740547583,
-                    "velocityX": 0.09916796550908585,
-                    "velocityY": 3.354131025281934,
-                    "timestamp": 1.316437377650983
-                },
-                {
-                    "x": 2.135680280502732,
-                    "y": 3.8963278332771982,
-                    "heading": -0.40525850037923283,
-                    "angularVelocity": 0.23249023433387317,
-                    "velocityX": 0.4779905415775436,
-                    "velocityY": 2.990249410122441,
-                    "timestamp": 1.3794540033960152
-                },
-                {
-                    "x": 2.177481885950485,
-                    "y": 4.0538159629335215,
-                    "heading": -0.39590861500997965,
-                    "angularVelocity": 0.14837172347315433,
-                    "velocityX": 0.6633424902324003,
-                    "velocityY": 2.4991520538330105,
-                    "timestamp": 1.4424706291410474
-                },
-                {
-                    "x": 2.2205433619064863,
-                    "y": 4.17824561445827,
-                    "heading": -0.39150641146352744,
-                    "angularVelocity": 0.06985781124276201,
-                    "velocityX": 0.6833351587282585,
-                    "velocityY": 1.9745527478446014,
-                    "timestamp": 1.5054872548860796
-                },
-                {
-                    "x": 2.2579292824582606,
-                    "y": 4.270063380418578,
-                    "heading": -0.38992694464863337,
-                    "angularVelocity": 0.0250642873403699,
-                    "velocityX": 0.5932707457717501,
-                    "velocityY": 1.4570403425249505,
-                    "timestamp": 1.5685038806311118
-                },
-                {
-                    "x": 2.285342089257776,
-                    "y": 4.3303097045050025,
-                    "heading": -0.38974725509942826,
-                    "angularVelocity": 0.002851462563739305,
-                    "velocityX": 0.43500911823537725,
-                    "velocityY": 0.9560385592555154,
-                    "timestamp": 1.631520506376144
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": -0.004010765374750144,
-                    "velocityX": 0.23260386555018367,
-                    "velocityY": 0.47115019479344145,
-                    "timestamp": 1.6945371321211762
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": -1.6105528525551922e-32,
-                    "velocityX": 0,
-                    "velocityY": -4.5276793712997926e-36,
-                    "timestamp": 1.7575537578662084
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "S4 To C1": {
-            "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 14
-                },
-                {
-                    "x": 5.034329891204834,
-                    "y": 3.981222629547119,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 17
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "angularVelocity": 1.1411819845129358e-30,
-                    "velocityX": 2.0257584812603577e-32,
-                    "velocityY": 3.042151659469263e-32,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.128157808100627,
-                    "y": 1.6381505638691025,
-                    "heading": -0.6351424860741187,
-                    "angularVelocity": 0.06199634399134786,
-                    "velocityX": -0.6106088487961494,
-                    "velocityY": 0.23165524966799825,
-                    "timestamp": 0.07835161903352221
-                },
-                {
-                    "x": 5.034138141125347,
-                    "y": 1.6783506940439978,
-                    "heading": -0.625731099901857,
-                    "angularVelocity": 0.12011731586854772,
-                    "velocityX": -1.199970953185457,
-                    "velocityY": 0.5130733821555872,
-                    "timestamp": 0.15670323806704442
-                },
-                {
-                    "x": 4.897649658060979,
-                    "y": 1.7471051706265737,
-                    "heading": -0.6123788413002932,
-                    "angularVelocity": 0.1704145844880519,
-                    "velocityX": -1.74199952404266,
-                    "velocityY": 0.8775118808095047,
-                    "timestamp": 0.23505485710056662
-                },
-                {
-                    "x": 4.7291295198915435,
-                    "y": 1.855780594555545,
-                    "heading": -0.5966040827715752,
-                    "angularVelocity": 0.20133289807283902,
-                    "velocityX": -2.150818837544829,
-                    "velocityY": 1.3870220586313022,
-                    "timestamp": 0.31340647613408884
-                },
-                {
-                    "x": 4.559409610033689,
-                    "y": 2.0156280635815147,
-                    "heading": -0.5822183456142529,
-                    "angularVelocity": 0.18360484869071408,
-                    "velocityX": -2.1661314973624393,
-                    "velocityY": 2.0401297509574126,
-                    "timestamp": 0.39175809516761106
-                },
-                {
-                    "x": 4.423281260574217,
-                    "y": 2.2140742145732073,
-                    "heading": -0.5727758831618049,
-                    "angularVelocity": 0.1205139417528553,
-                    "velocityX": -1.7374031467203144,
-                    "velocityY": 2.532763884646578,
-                    "timestamp": 0.4701097142011333
-                },
-                {
-                    "x": 4.3324586171803325,
-                    "y": 2.4362881098121476,
-                    "heading": -0.5691499320560302,
-                    "angularVelocity": 0.04627793465535804,
-                    "velocityX": -1.1591674111421555,
-                    "velocityY": 2.8361110846205655,
-                    "timestamp": 0.5484613332346555
-                },
-                {
-                    "x": 4.290356765831772,
-                    "y": 2.674111890908862,
-                    "heading": -0.5714790836196856,
-                    "angularVelocity": -0.02972691046318988,
-                    "velocityX": -0.5373450079001806,
-                    "velocityY": 3.0353397164002804,
-                    "timestamp": 0.6268129522681777
-                },
-                {
-                    "x": 4.298268630722052,
-                    "y": 2.9211188280591673,
-                    "heading": -0.5947192771376703,
-                    "angularVelocity": -0.29661408155511804,
-                    "velocityX": 0.1009789585444941,
-                    "velocityY": 3.1525441362561364,
-                    "timestamp": 0.7051645713016998
-                },
-                {
-                    "x": 4.356924477851345,
-                    "y": 3.1722484967201265,
-                    "heading": -0.6368890523591307,
-                    "angularVelocity": -0.5382119188043637,
-                    "velocityX": 0.7486232939768359,
-                    "velocityY": 3.2051624683532776,
-                    "timestamp": 0.783516190335222
-                },
-                {
-                    "x": 4.466386404708156,
-                    "y": 3.418800428561669,
-                    "heading": -0.694172117002664,
-                    "angularVelocity": -0.7311025011368958,
-                    "velocityX": 1.3970601782967371,
-                    "velocityY": 3.1467369134523744,
-                    "timestamp": 0.8618678093687442
-                },
-                {
-                    "x": 4.622742851027307,
-                    "y": 3.644917946871822,
-                    "heading": -0.7581397869413499,
-                    "angularVelocity": -0.8164179723116849,
-                    "velocityX": 1.9955739045067467,
-                    "velocityY": 2.8859329404975034,
-                    "timestamp": 0.9402194284022664
-                },
-                {
-                    "x": 4.8130531116785615,
-                    "y": 3.833426042269654,
-                    "heading": -0.8482959054101304,
-                    "angularVelocity": -1.1506605681014435,
-                    "velocityX": 2.4289256941816704,
-                    "velocityY": 2.4059246984696085,
-                    "timestamp": 1.0185710474357885
-                },
-                {
-                    "x": 5.034329891204834,
-                    "y": 3.981222629547119,
-                    "heading": -0.9454274313666489,
-                    "angularVelocity": -1.2396875412997066,
-                    "velocityX": 2.824150697276595,
-                    "velocityY": 1.8863246107809373,
-                    "timestamp": 1.0969226664693108
-                },
-                {
-                    "x": 5.3307679727813255,
-                    "y": 4.095215339701575,
-                    "heading": -1.0325645415374454,
-                    "angularVelocity": -0.9491279076997933,
-                    "velocityX": 3.2289073573561264,
-                    "velocityY": 1.241648503948074,
-                    "timestamp": 1.1887302189223545
-                },
-                {
-                    "x": 5.6706736122505745,
-                    "y": 4.158722341660029,
-                    "heading": -1.0366531166779425,
-                    "angularVelocity": -0.04453419170049298,
-                    "velocityX": 3.7023712144281196,
-                    "velocityY": 0.691740496958957,
-                    "timestamp": 1.2805377713753983
-                },
-                {
-                    "x": 5.985432145120126,
-                    "y": 4.2874622186845945,
-                    "heading": -1.0150469515971576,
-                    "angularVelocity": 0.23534191363870294,
-                    "velocityX": 3.4284601262030105,
-                    "velocityY": 1.402279807975625,
-                    "timestamp": 1.372345323828442
-                },
-                {
-                    "x": 6.260473058973373,
-                    "y": 4.473603429076733,
-                    "heading": -0.9641067919300658,
-                    "angularVelocity": 0.5548580515001295,
-                    "velocityX": 2.9958419160985765,
-                    "velocityY": 2.027515225257119,
-                    "timestamp": 1.4641528762814857
-                },
-                {
-                    "x": 6.487005993334624,
-                    "y": 4.6803674717401,
-                    "heading": -0.8309910438969466,
-                    "angularVelocity": 1.4499433268434323,
-                    "velocityX": 2.4674760224886074,
-                    "velocityY": 2.2521463337029823,
-                    "timestamp": 1.5559604287345294
-                },
-                {
-                    "x": 6.71888754498383,
-                    "y": 4.9198952355114685,
-                    "heading": -0.7843851972685187,
-                    "angularVelocity": 0.507647196588375,
-                    "velocityX": 2.5257350343568326,
-                    "velocityY": 2.609020253468568,
-                    "timestamp": 1.6477679811875732
-                },
-                {
-                    "x": 6.906293835583718,
-                    "y": 5.2122954621107285,
-                    "heading": -0.7843850302000089,
-                    "angularVelocity": 0.0000018197686939661552,
-                    "velocityX": 2.0412949217411986,
-                    "velocityY": 3.1849256274293083,
-                    "timestamp": 1.739575533640617
-                },
-                {
-                    "x": 7.030998336898649,
-                    "y": 5.536437116039629,
-                    "heading": -0.7843849409482478,
-                    "angularVelocity": 9.721614257680115e-7,
-                    "velocityX": 1.3583250830993825,
-                    "velocityY": 3.530664365490929,
-                    "timestamp": 1.8313830860936606
-                },
-                {
-                    "x": 7.139916048661206,
-                    "y": 5.86621879705104,
-                    "heading": -0.7843848550588985,
-                    "angularVelocity": 9.355368597772783e-7,
-                    "velocityX": 1.1863698448802928,
-                    "velocityY": 3.592097514854049,
-                    "timestamp": 1.9231906385467044
-                },
-                {
-                    "x": 7.248833621341376,
-                    "y": 6.196000523997438,
-                    "heading": -0.7843847691695015,
-                    "angularVelocity": 9.355373786921671e-7,
-                    "velocityX": 1.1863683299462482,
-                    "velocityY": 3.592098015194004,
-                    "timestamp": 2.0149981909997483
-                },
-                {
-                    "x": 7.357751176995467,
-                    "y": 6.5257822565670125,
-                    "heading": -0.7843846832799689,
-                    "angularVelocity": 9.355388566444766e-7,
-                    "velocityX": 1.1863681444922285,
-                    "velocityY": 3.5920980764436137,
-                    "timestamp": 2.1068057434527923
-                },
-                {
-                    "x": 7.4669549324183055,
-                    "y": 6.834400664448086,
-                    "heading": -0.7318192896940902,
-                    "angularVelocity": 0.5725606682823186,
-                    "velocityX": 1.189485532562175,
-                    "velocityY": 3.361579735380924,
-                    "timestamp": 2.198613295905836
-                },
-                {
-                    "x": 7.555413306559794,
-                    "y": 7.089894078912264,
-                    "heading": -0.5139853289463429,
-                    "angularVelocity": 2.3727237566774426,
-                    "velocityX": 0.9635195773978621,
-                    "velocityY": 2.782923709842436,
-                    "timestamp": 2.29042084835888
-                },
-                {
-                    "x": 7.619158448834915,
-                    "y": 7.279787689667393,
-                    "heading": -0.2782484542424907,
-                    "angularVelocity": 2.5677285626846773,
-                    "velocityX": 0.6943344046528581,
-                    "velocityY": 2.0683876835977384,
-                    "timestamp": 2.382228400811924
-                },
-                {
-                    "x": 7.660128944224933,
-                    "y": 7.404974004983973,
-                    "heading": -0.09793823848426357,
-                    "angularVelocity": 1.964001990472943,
-                    "velocityX": 0.44626497815605787,
-                    "velocityY": 1.3635731698719276,
-                    "timestamp": 2.474035953264968
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 2.533782828853193e-32,
-                    "angularVelocity": 1.0667775783953661,
-                    "velocityX": 0.21644249567844834,
-                    "velocityY": 0.6756088508922024,
-                    "timestamp": 2.565843505718012
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "angularVelocity": 2.4912680070874024e-31,
-                    "velocityX": 0,
-                    "velocityY": -3.645397861822831e-32,
-                    "timestamp": 2.657651058171056
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "S4 To C2": {
-            "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 21
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "angularVelocity": -2.491480567253294e-33,
-                    "velocityX": -2.0103104784295923e-34,
-                    "velocityY": 1.605618083193578e-34,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.240158813928264,
-                    "y": 1.6204190327819517,
-                    "heading": -0.5237338108165747,
-                    "angularVelocity": 1.2526389382742513,
-                    "velocityX": 0.6912398963489377,
-                    "velocityY": 0.004514612397400031,
-                    "timestamp": 0.0928170006782674
-                },
-                {
-                    "x": 5.369626464101249,
-                    "y": 1.6247166229091008,
-                    "heading": -0.2974184918320366,
-                    "angularVelocity": 2.4382959730515035,
-                    "velocityX": 1.394870004707003,
-                    "velocityY": 0.046301756097956205,
-                    "timestamp": 0.1856340013565348
-                },
-                {
-                    "x": 5.5674551093283755,
-                    "y": 1.639197763374398,
-                    "heading": 0.0009013327241277373,
-                    "angularVelocity": 3.214064475001015,
-                    "velocityX": 2.1313837312289596,
-                    "velocityY": 0.15601819019657054,
-                    "timestamp": 0.2784510020348022
-                },
-                {
-                    "x": 5.831585704851482,
-                    "y": 1.6774487923340957,
-                    "heading": 0.2480008567342784,
-                    "angularVelocity": 2.6622226769282764,
-                    "velocityX": 2.8457135394696165,
-                    "velocityY": 0.41211231434085943,
-                    "timestamp": 0.3712680027130696
-                },
-                {
-                    "x": 6.141513989428359,
-                    "y": 1.75469627200855,
-                    "heading": 0.3453177753240257,
-                    "angularVelocity": 1.0484816130514494,
-                    "velocityX": 3.339132726893265,
-                    "velocityY": 0.8322557194259916,
-                    "timestamp": 0.46408500339133696
-                },
-                {
-                    "x": 6.452443961209774,
-                    "y": 1.9008832260175597,
-                    "heading": 0.36957097138497697,
-                    "angularVelocity": 0.2613012258930921,
-                    "velocityX": 3.3499247929718696,
-                    "velocityY": 1.5750019171136467,
-                    "timestamp": 0.5569020040696043
-                },
-                {
-                    "x": 6.733109637262523,
-                    "y": 2.111863899724239,
-                    "heading": 0.36957109822659895,
-                    "angularVelocity": 0.0000013665774702958542,
-                    "velocityX": 3.023860650546378,
-                    "velocityY": 2.273082217319261,
-                    "timestamp": 0.6497190047478717
-                },
-                {
-                    "x": 6.964965499466835,
-                    "y": 2.3755472934997988,
-                    "heading": 0.36957101579118784,
-                    "angularVelocity": -8.881499138395027e-7,
-                    "velocityX": 2.4979891669630336,
-                    "velocityY": 2.840895437782658,
-                    "timestamp": 0.7425360054261391
-                },
-                {
-                    "x": 7.138307566970259,
-                    "y": 2.680897277378083,
-                    "heading": 0.36957096673523665,
-                    "angularVelocity": -5.285233395413412e-7,
-                    "velocityX": 1.867568077364202,
-                    "velocityY": 3.289806626446832,
-                    "timestamp": 0.8353530061044064
-                },
-                {
-                    "x": 7.245881008413397,
-                    "y": 3.0151337865477195,
-                    "heading": 0.3695709277774119,
-                    "angularVelocity": -4.197272531605919e-7,
-                    "velocityX": 1.1589842448800958,
-                    "velocityY": 3.6010268240427665,
-                    "timestamp": 0.9281700067826738
-                },
-                {
-                    "x": 7.300266060372569,
-                    "y": 3.36201756600148,
-                    "heading": 0.3695708905369613,
-                    "angularVelocity": -4.012244559973519e-7,
-                    "velocityX": 0.5859384763755527,
-                    "velocityY": 3.7372871017042186,
-                    "timestamp": 1.0209870074609413
-                },
-                {
-                    "x": 7.354650396010165,
-                    "y": 3.708901457760651,
-                    "heading": 0.3695708532965109,
-                    "angularVelocity": -4.0122445392101915e-7,
-                    "velocityX": 0.5859307588068667,
-                    "velocityY": 3.7372883116701696,
-                    "timestamp": 1.1138040081392087
-                },
-                {
-                    "x": 7.409034640641628,
-                    "y": 4.055785363787717,
-                    "heading": 0.36957081605605097,
-                    "angularVelocity": -4.012245569815441e-7,
-                    "velocityX": 0.5859297783169686,
-                    "velocityY": 3.737288465390881,
-                    "timestamp": 1.206621008817476
-                },
-                {
-                    "x": 7.463418795891099,
-                    "y": 4.402669283828022,
-                    "heading": 0.36957077881557854,
-                    "angularVelocity": -4.0122469066112833e-7,
-                    "velocityX": 0.5859288153253637,
-                    "velocityY": 3.7372886163679513,
-                    "timestamp": 1.2994380094957434
-                },
-                {
-                    "x": 7.517802854177964,
-                    "y": 4.7495532190700205,
-                    "heading": 0.3695707415750949,
-                    "angularVelocity": -4.0122481249990114e-7,
-                    "velocityX": 0.5859277706610763,
-                    "velocityY": 3.737288780149305,
-                    "timestamp": 1.3922550101740108
-                },
-                {
-                    "x": 7.56989445358868,
-                    "y": 5.095450165697103,
-                    "heading": 0.36620989603022114,
-                    "angularVelocity": -0.036209374579162214,
-                    "velocityX": 0.5612290747390308,
-                    "velocityY": 3.7266550750338174,
-                    "timestamp": 1.4850720108522781
-                },
-                {
-                    "x": 7.614129631330889,
-                    "y": 5.376850109081837,
-                    "heading": 0.24917441522367106,
-                    "angularVelocity": -1.2609272003114111,
-                    "velocityX": 0.4765848650458117,
-                    "velocityY": 3.0317715647821197,
-                    "timestamp": 1.5778890115305455
-                },
-                {
-                    "x": 7.6471559939174325,
-                    "y": 5.587299345919279,
-                    "heading": 0.1342077337923999,
-                    "angularVelocity": -1.2386381868746383,
-                    "velocityX": 0.35582234229936965,
-                    "velocityY": 2.267356575838128,
-                    "timestamp": 1.6707060122088129
-                },
-                {
-                    "x": 7.669079055841979,
-                    "y": 5.72720580380319,
-                    "heading": 0.04709984861251457,
-                    "angularVelocity": -0.9384906271840042,
-                    "velocityX": 0.23619662092442378,
-                    "velocityY": 1.5073365532341503,
-                    "timestamp": 1.7635230128870802
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": -9.59663427624948e-34,
-                    "angularVelocity": -0.5074485090913172,
-                    "velocityX": 0.11766103276571077,
-                    "velocityY": 0.751954875580805,
-                    "timestamp": 1.8563400135653476
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "angularVelocity": 1.9250732473517817e-32,
-                    "velocityX": 0,
-                    "velocityY": 2.1529996497623123e-34,
-                    "timestamp": 1.949157014243615
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "S4 To C3": {
-            "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 12
-                },
-                {
-                    "x": 6.529238224029541,
-                    "y": 1.531233787536621,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 13
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "angularVelocity": 0,
-                    "velocityX": 2.653642496841617e-31,
-                    "velocityY": -1.2761459725684476e-31,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.194122324075795,
-                    "y": 1.6116215778070426,
-                    "heading": -0.6335243646936302,
-                    "angularVelocity": 0.13206571622689564,
-                    "velocityX": 0.3695911822570569,
-                    "velocityY": -0.1708716250073132,
-                    "timestamp": 0.049033431926404414
-                },
-                {
-                    "x": 5.2306889078335095,
-                    "y": 1.5955898422898989,
-                    "heading": -0.6207226842720189,
-                    "angularVelocity": 0.2610806529090014,
-                    "velocityX": 0.745747999295838,
-                    "velocityY": -0.3269551994890449,
-                    "timestamp": 0.09806686385280883
-                },
-                {
-                    "x": 5.286073830286703,
-                    "y": 1.5728623577205654,
-                    "heading": -0.6018022859553105,
-                    "angularVelocity": 0.38586730672362957,
-                    "velocityX": 1.1295338767299938,
-                    "velocityY": -0.4635099701654054,
-                    "timestamp": 0.14710029577921324
-                },
-                {
-                    "x": 5.360700050095615,
-                    "y": 1.5447484727758143,
-                    "heading": -0.577064216904969,
-                    "angularVelocity": 0.5045143298894789,
-                    "velocityX": 1.5219456782249914,
-                    "velocityY": -0.5733615584348081,
-                    "timestamp": 0.19613372770561766
-                },
-                {
-                    "x": 5.45500431506412,
-                    "y": 1.513111103841015,
-                    "heading": -0.5469692052395991,
-                    "angularVelocity": 0.6137651492712723,
-                    "velocityX": 1.9232646230039372,
-                    "velocityY": -0.6452203668359155,
-                    "timestamp": 0.24516715963202207
-                },
-                {
-                    "x": 5.5692993756365,
-                    "y": 1.480706991826158,
-                    "heading": -0.5122625268421303,
-                    "angularVelocity": 0.7078166270271339,
-                    "velocityX": 2.330961878090668,
-                    "velocityY": -0.6608575158164777,
-                    "timestamp": 0.2942005915584265
-                },
-                {
-                    "x": 5.703331364670941,
-                    "y": 1.4516882815399292,
-                    "heading": -0.47420565589215874,
-                    "angularVelocity": 0.7761412867722233,
-                    "velocityX": 2.733481703578104,
-                    "velocityY": -0.5918147913809916,
-                    "timestamp": 0.3432340234848309
-                },
-                {
-                    "x": 5.855159622936256,
-                    "y": 1.4318923943368613,
-                    "heading": -0.4349045247507558,
-                    "angularVelocity": 0.8015170384282412,
-                    "velocityX": 3.096423242273411,
-                    "velocityY": -0.40372224470801127,
-                    "timestamp": 0.3922674554112353
-                },
-                {
-                    "x": 6.019815732483991,
-                    "y": 1.4274954608427641,
-                    "heading": -0.3972296573283927,
-                    "angularVelocity": 0.768350611865634,
-                    "velocityX": 3.358037630221609,
-                    "velocityY": -0.08967215471871418,
-                    "timestamp": 0.4413008873376397
-                },
-                {
-                    "x": 6.190507691154086,
-                    "y": 1.4421792499476667,
-                    "heading": -0.36375877276285506,
-                    "angularVelocity": 0.6826135404074718,
-                    "velocityX": 3.4811342376834093,
-                    "velocityY": 0.2994648452702519,
-                    "timestamp": 0.49033431926404414
-                },
-                {
-                    "x": 6.361514712702491,
-                    "y": 1.4768435219754552,
-                    "heading": -0.33601787986089265,
-                    "angularVelocity": 0.5657546659914753,
-                    "velocityX": 3.487559708345244,
-                    "velocityY": 0.7069517809772359,
-                    "timestamp": 0.5393677511904486
-                },
-                {
-                    "x": 6.529238224029541,
-                    "y": 1.531233787536621,
-                    "heading": -0.3133159497925571,
-                    "angularVelocity": 0.4629888052692109,
-                    "velocityX": 3.4205949846286416,
-                    "velocityY": 1.1092485968081267,
-                    "timestamp": 0.588401183116853
-                },
-                {
-                    "x": 6.794330666624461,
-                    "y": 1.6764087741891647,
-                    "heading": -0.29089151826229404,
-                    "angularVelocity": 0.27241251742532613,
-                    "velocityX": 3.2203491776466797,
-                    "velocityY": 1.7635891250039288,
-                    "timestamp": 0.6707191020664587
-                },
-                {
-                    "x": 7.034738463760469,
-                    "y": 1.8721289187979775,
-                    "heading": -0.28740556521383775,
-                    "angularVelocity": 0.042347438965477144,
-                    "velocityX": 2.9204795286821774,
-                    "velocityY": 2.377612883150898,
-                    "timestamp": 0.7530370210160644
-                },
-                {
-                    "x": 7.236654527584481,
-                    "y": 2.109198968129657,
-                    "heading": -0.2874055261243408,
-                    "angularVelocity": 4.748601214403234e-7,
-                    "velocityX": 2.452881054338489,
-                    "velocityY": 2.879932490479633,
-                    "timestamp": 0.8353549399656701
-                },
-                {
-                    "x": 7.392410351066591,
-                    "y": 2.3788514084218195,
-                    "heading": -0.2874055078665042,
-                    "angularVelocity": 2.2179662469221753e-7,
-                    "velocityX": 1.8921253776776576,
-                    "velocityY": 3.2757441360636306,
-                    "timestamp": 0.9176728589152758
-                },
-                {
-                    "x": 7.49687844031107,
-                    "y": 2.6722091028580075,
-                    "heading": -0.28740549372573876,
-                    "angularVelocity": 1.7178234776742224e-7,
-                    "velocityX": 1.2690807855393922,
-                    "velocityY": 3.5637161164844247,
-                    "timestamp": 0.9999907778648816
-                },
-                {
-                    "x": 7.546620338444338,
-                    "y": 2.979614475892834,
-                    "heading": -0.28740548096651763,
-                    "angularVelocity": 1.5499931641741705e-7,
-                    "velocityX": 0.6042657390764874,
-                    "velocityY": 3.7343676438541644,
-                    "timestamp": 1.0823086968144873
-                },
-                {
-                    "x": 7.582493232504898,
-                    "y": 3.288945132948743,
-                    "heading": -0.2874054683444637,
-                    "angularVelocity": 1.5333300623146833e-7,
-                    "velocityX": 0.43578475401492967,
-                    "velocityY": 3.7577560390622633,
-                    "timestamp": 1.164626615764093
-                },
-                {
-                    "x": 7.6150468277369106,
-                    "y": 3.5683420563080905,
-                    "heading": -0.1936159509616472,
-                    "angularVelocity": 1.1393572454157397,
-                    "velocityX": 0.39546183440153926,
-                    "velocityY": 3.3941203437178378,
-                    "timestamp": 1.2469445347136987
-                },
-                {
-                    "x": 7.641044736890772,
-                    "y": 3.791820131624945,
-                    "heading": -0.11680359430817472,
-                    "angularVelocity": 0.9331183007718732,
-                    "velocityX": 0.31582320697043825,
-                    "velocityY": 2.714816873027222,
-                    "timestamp": 1.3292624536633044
-                },
-                {
-                    "x": 7.660527962829294,
-                    "y": 3.959415202649684,
-                    "heading": -0.05861701943494889,
-                    "angularVelocity": 0.7068518691380767,
-                    "velocityX": 0.23668268327338735,
-                    "velocityY": 2.035948833052459,
-                    "timestamp": 1.4115803726129101
-                },
-                {
-                    "x": 7.673510674167818,
-                    "y": 4.0711397209924485,
-                    "heading": -0.019593791766857216,
-                    "angularVelocity": 0.4740550801809447,
-                    "velocityX": 0.1577142802460941,
-                    "velocityY": 1.3572320555281903,
-                    "timestamp": 1.4938982915625159
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": -2.071552209824535e-33,
-                    "angularVelocity": 0.23802583953553896,
-                    "velocityX": 0.07883248161483059,
-                    "velocityY": 0.6785919726876813,
-                    "timestamp": 1.5762162105121216
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "angularVelocity": -4.785738309812717e-31,
-                    "velocityX": -1.2484093100405624e-30,
-                    "velocityY": 4.284452606087597e-31,
-                    "timestamp": 1.6585341294617273
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "S4 To C4": {
-            "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.222401190948388,
-                    "y": 1.635609178560311,
-                    "heading": -0.6120137040899031,
-                    "angularVelocity": 0.36292240482976423,
-                    "velocityX": 0.6017242103081577,
-                    "velocityY": 0.20241766322785323,
-                    "timestamp": 0.07711371780208913
-                },
-                {
-                    "x": 5.315327100770923,
-                    "y": 1.6668589219810437,
-                    "heading": -0.5590312951883875,
-                    "angularVelocity": 0.6870685321838068,
-                    "velocityX": 1.205050313629373,
-                    "velocityY": 0.4052423396435706,
-                    "timestamp": 0.15422743560417826
-                },
-                {
-                    "x": 5.454938302762681,
-                    "y": 1.713800132384211,
-                    "heading": -0.4856728473163301,
-                    "angularVelocity": 0.9513021802467544,
-                    "velocityX": 1.8104587091763231,
-                    "velocityY": 0.6087271077195515,
-                    "timestamp": 0.23134115340626737
-                },
-                {
-                    "x": 5.641435450684747,
-                    "y": 1.7765099385015581,
-                    "heading": -0.4002091399727602,
-                    "angularVelocity": 1.1082815065785667,
-                    "velocityX": 2.418469154874715,
-                    "velocityY": 0.8132120704942623,
-                    "timestamp": 0.3084548712083565
-                },
-                {
-                    "x": 5.8748863814192855,
-                    "y": 1.8550365409977465,
-                    "heading": -0.32154781798640775,
-                    "angularVelocity": 1.0200691164734093,
-                    "velocityX": 3.0273592998548686,
-                    "velocityY": 1.0183220928048784,
-                    "timestamp": 0.38556858901044566
-                },
-                {
-                    "x": 6.151206632813102,
-                    "y": 1.945953456728306,
-                    "heading": -0.3195100296755088,
-                    "angularVelocity": 0.026425756259478697,
-                    "velocityX": 3.583282705976986,
-                    "velocityY": 1.178997956808355,
-                    "timestamp": 0.4626823068125348
-                },
-                {
-                    "x": 6.4277763897122595,
-                    "y": 2.0387313847088295,
-                    "heading": -0.31950999788997536,
-                    "angularVelocity": 4.121903903341971e-7,
-                    "velocityX": 3.5865182587742526,
-                    "velocityY": 1.2031313056210848,
-                    "timestamp": 0.539796024614624
-                },
-                {
-                    "x": 6.7043461469693755,
-                    "y": 2.1315093116222874,
-                    "heading": -0.31950996610444216,
-                    "angularVelocity": 4.121903870025697e-7,
-                    "velocityX": 3.5865182634161936,
-                    "velocityY": 1.2031312917835268,
-                    "timestamp": 0.616909742416713
-                },
-                {
-                    "x": 6.980831721332989,
-                    "y": 2.2223162260189957,
-                    "heading": -0.31777519026878787,
-                    "angularVelocity": 0.022496332495664188,
-                    "velocityX": 3.5854265913259273,
-                    "velocityY": 1.1775714747635835,
-                    "timestamp": 0.6940234602188021
-                },
-                {
-                    "x": 7.214355031558839,
-                    "y": 2.3007720927806434,
-                    "heading": -0.23870969056825803,
-                    "angularVelocity": 1.0253104370281303,
-                    "velocityX": 3.028297907062184,
-                    "velocityY": 1.0174048016074528,
-                    "timestamp": 0.7711371780208912
-                },
-                {
-                    "x": 7.400917361819277,
-                    "y": 2.3634042679352882,
-                    "heading": -0.15373574527352316,
-                    "angularVelocity": 1.1019303402387481,
-                    "velocityX": 2.419314430400663,
-                    "velocityY": 0.8122053629341165,
-                    "timestamp": 0.8482508958229803
-                },
-                {
-                    "x": 7.540592119323217,
-                    "y": 2.4102654434053137,
-                    "heading": -0.08076255152981597,
-                    "angularVelocity": 0.9463062581289249,
-                    "velocityX": 1.8112828882458099,
-                    "velocityY": 0.6076892258040765,
-                    "timestamp": 0.9253646136250694
-                },
-                {
-                    "x": 7.633568850123268,
-                    "y": 2.4414404537843537,
-                    "heading": -0.02793953661151031,
-                    "angularVelocity": 0.6850015331107969,
-                    "velocityX": 1.2057093530190521,
-                    "velocityY": 0.4042732119212608,
-                    "timestamp": 1.0024783314271586
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": -3.3818227179407965e-32,
-                    "angularVelocity": 0.3623160367292381,
-                    "velocityX": 0.6021127135368662,
-                    "velocityY": 0.20177403786417583,
-                    "timestamp": 1.0795920492292477
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": -1.7311819232040874e-32,
-                    "angularVelocity": 2.140534191184349e-31,
-                    "velocityX": 0,
-                    "velocityY": -2.0522160776390996e-31,
-                    "timestamp": 1.1567057670313368
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "S4 To C5": {
-            "waypoints": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 5.22245562506892,
-                    "y": 1.6045082039129626,
-                    "heading": -0.6133597798196372,
-                    "angularVelocity": 0.3456549186542876,
-                    "velocityX": 0.6027583554308628,
-                    "velocityY": -0.20100492713723658,
-                    "timestamp": 0.07707172310487943
-                },
-                {
-                    "x": 5.31546298986369,
-                    "y": 1.573504853521539,
-                    "heading": -0.5625082909022232,
-                    "angularVelocity": 0.6597943690478437,
-                    "velocityX": 1.2067637915426601,
-                    "velocityY": -0.4022662157070755,
-                    "timestamp": 0.15414344620975887
-                },
-                {
-                    "x": 5.455153171011431,
-                    "y": 1.526958597742617,
-                    "heading": -0.4912836919139272,
-                    "angularVelocity": 0.924133989989731,
-                    "velocityX": 1.8124699373549893,
-                    "velocityY": -0.6039342822993798,
-                    "timestamp": 0.2312151693146383
-                },
-                {
-                    "x": 5.641701159953251,
-                    "y": 1.4648201004985182,
-                    "heading": -0.406509898431932,
-                    "angularVelocity": 1.0999338027856826,
-                    "velocityX": 2.420446584384324,
-                    "velocityY": -0.8062424809101578,
-                    "timestamp": 0.30828689241951773
-                },
-                {
-                    "x": 5.875244890448534,
-                    "y": 1.3870507382512116,
-                    "heading": -0.323292875073049,
-                    "angularVelocity": 1.0797348236997513,
-                    "velocityX": 3.03021291190643,
-                    "velocityY": -1.0090518171168696,
-                    "timestamp": 0.38535861552439715
-                },
-                {
-                    "x": 6.151315012657669,
-                    "y": 1.295160148354896,
-                    "heading": -0.32123146568205607,
-                    "angularVelocity": 0.026746636872095383,
-                    "velocityX": 3.5819897504232276,
-                    "velocityY": -1.192273718485189,
-                    "timestamp": 0.46243033862927657
-                },
-                {
-                    "x": 6.427941847872596,
-                    "y": 1.2030541885235448,
-                    "heading": -0.3212314367193528,
-                    "angularVelocity": 3.757889680371681e-7,
-                    "velocityX": 3.5892130611702004,
-                    "velocityY": -1.1950681277232245,
-                    "timestamp": 0.539502061734156
-                },
-                {
-                    "x": 6.704568684105482,
-                    "y": 1.1109482317494896,
-                    "heading": -0.3212314077566493,
-                    "angularVelocity": 3.7578897090221527e-7,
-                    "velocityX": 3.589213074378161,
-                    "velocityY": -1.1950680880550335,
-                    "timestamp": 0.6165737848390355
-                },
-                {
-                    "x": 6.9807077062893965,
-                    "y": 1.0195126541212949,
-                    "heading": -0.3189025007582472,
-                    "angularVelocity": 0.030217398866675224,
-                    "velocityX": 3.582883722583232,
-                    "velocityY": -1.1863699674103443,
-                    "timestamp": 0.6936455079439149
-                },
-                {
-                    "x": 7.214256981896637,
-                    "y": 0.9418352430707998,
-                    "heading": -0.2350676331360113,
-                    "angularVelocity": 1.0877513080660361,
-                    "velocityX": 3.0302848593306506,
-                    "velocityY": -1.0078587570280646,
-                    "timestamp": 0.7707172310487943
-                },
-                {
-                    "x": 7.4008131962231865,
-                    "y": 0.8797969152412516,
-                    "heading": -0.14969687190170503,
-                    "angularVelocity": 1.1076794159400525,
-                    "velocityX": 2.4205533081527695,
-                    "velocityY": -0.8049427900441033,
-                    "timestamp": 0.8477889541536737
-                },
-                {
-                    "x": 7.54051565231881,
-                    "y": 0.8333524229780926,
-                    "heading": -0.0780215306756512,
-                    "angularVelocity": 0.9299823377312819,
-                    "velocityX": 1.812629203910703,
-                    "velocityY": -0.6026139080860716,
-                    "timestamp": 0.9248606772585531
-                },
-                {
-                    "x": 7.633536087871107,
-                    "y": 0.802437413208921,
-                    "heading": -0.026843150662094223,
-                    "angularVelocity": 0.6640357572376224,
-                    "velocityX": 1.2069333836706095,
-                    "velocityY": -0.401120002560503,
-                    "timestamp": 1.0019324003634327
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 3.8658238709310755e-33,
-                    "angularVelocity": 0.3482879268907221,
-                    "velocityX": 0.6028658794310917,
-                    "velocityY": -0.20029931325025216,
-                    "timestamp": 1.079004123468312
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 4.381697144346615e-34,
-                    "angularVelocity": -4.4473487408728983e-32,
-                    "velocityX": 0,
-                    "velocityY": -1.5918634852021244e-32,
-                    "timestamp": 1.1560758465731915
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "C1 To W1": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 21
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 3.136513796099116e-33,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.613270105003341,
-                    "y": 7.458186274184093,
-                    "heading": 0.022686687743237347,
-                    "angularVelocity": 0.25194782208363986,
-                    "velocityX": -0.7410712353675238,
-                    "velocityY": -0.0978811472565877,
-                    "timestamp": 0.09004518298914281
-                },
-                {
-                    "x": 7.479732366880497,
-                    "y": 7.440537508755927,
-                    "heading": 0.06475240251532689,
-                    "angularVelocity": 0.4671622998107697,
-                    "velocityX": -1.483008126475186,
-                    "velocityY": -0.19599899564082535,
-                    "timestamp": 0.18009036597828562
-                },
-                {
-                    "x": 7.2792817573683966,
-                    "y": 7.414022951557291,
-                    "heading": 0.12053408331695856,
-                    "angularVelocity": 0.6194854510802376,
-                    "velocityX": -2.226111412714547,
-                    "velocityY": -0.2944583632178676,
-                    "timestamp": 0.2701355489674284
-                },
-                {
-                    "x": 7.01181645042657,
-                    "y": 7.378594749595234,
-                    "heading": 0.17807816149187164,
-                    "angularVelocity": 0.6390578181383835,
-                    "velocityX": -2.9703455316879728,
-                    "velocityY": -0.39344916391950197,
-                    "timestamp": 0.36018073195657124
-                },
-                {
-                    "x": 6.678421151155383,
-                    "y": 7.334256434894407,
-                    "heading": 0.1920911587056688,
-                    "angularVelocity": 0.15562184170903165,
-                    "velocityX": -3.7025334193766524,
-                    "velocityY": -0.49240073959507324,
-                    "timestamp": 0.45022591494571407
-                },
-                {
-                    "x": 6.340697514229505,
-                    "y": 7.289811670135368,
-                    "heading": 0.1920911781237931,
-                    "angularVelocity": 2.1564867368321945e-7,
-                    "velocityX": -3.7506019279965175,
-                    "velocityY": -0.49358292452353475,
-                    "timestamp": 0.5402710979348568
-                },
-                {
-                    "x": 6.002973877037569,
-                    "y": 7.245366907398008,
-                    "heading": 0.19209119754191162,
-                    "angularVelocity": 2.1564861007875057e-7,
-                    "velocityX": -3.750601930951228,
-                    "velocityY": -0.49358290207171196,
-                    "timestamp": 0.6303162809239996
-                },
-                {
-                    "x": 5.665250239845632,
-                    "y": 7.200922144660658,
-                    "heading": 0.19209121696003015,
-                    "angularVelocity": 2.1564860966767442e-7,
-                    "velocityX": -3.7506019309512406,
-                    "velocityY": -0.493582902071613,
-                    "timestamp": 0.7203614639131424
-                },
-                {
-                    "x": 5.327526602653695,
-                    "y": 7.156477381923307,
-                    "heading": 0.19209123637814873,
-                    "angularVelocity": 2.1564861026621434e-7,
-                    "velocityX": -3.7506019309512415,
-                    "velocityY": -0.493582902071613,
-                    "timestamp": 0.8104066469022851
-                },
-                {
-                    "x": 4.989802965461758,
-                    "y": 7.112032619185957,
-                    "heading": 0.19209125579626718,
-                    "angularVelocity": 2.156486089426075e-7,
-                    "velocityX": -3.7506019309512415,
-                    "velocityY": -0.49358290207161315,
-                    "timestamp": 0.9004518298914279
-                },
-                {
-                    "x": 4.652079328269822,
-                    "y": 7.067587856448606,
-                    "heading": 0.19209127521438563,
-                    "angularVelocity": 2.156486089612396e-7,
-                    "velocityX": -3.750601930951241,
-                    "velocityY": -0.4935829020716131,
-                    "timestamp": 0.9904970128805707
-                },
-                {
-                    "x": 4.314355691077885,
-                    "y": 7.023143093711255,
-                    "heading": 0.19209129463250418,
-                    "angularVelocity": 2.1564860999442945e-7,
-                    "velocityX": -3.7506019309512415,
-                    "velocityY": -0.4935829020716131,
-                    "timestamp": 1.0805421958697135
-                },
-                {
-                    "x": 3.976632053885947,
-                    "y": 6.978698330973905,
-                    "heading": 0.1920913140506227,
-                    "angularVelocity": 2.1564860984546372e-7,
-                    "velocityX": -3.750601930951241,
-                    "velocityY": -0.49358290207161315,
-                    "timestamp": 1.1705873788588563
-                },
-                {
-                    "x": 3.638908416694012,
-                    "y": 6.934253568236545,
-                    "heading": 0.19209133346874124,
-                    "angularVelocity": 2.1564860987115903e-7,
-                    "velocityX": -3.750601930951227,
-                    "velocityY": -0.4935829020717237,
-                    "timestamp": 1.2606325618479992
-                },
-                {
-                    "x": 3.3011847797995815,
-                    "y": 6.8898088032385445,
-                    "heading": 0.19209135288686577,
-                    "angularVelocity": 2.1564867640744468e-7,
-                    "velocityX": -3.7506019276472724,
-                    "velocityY": -0.4935829271773418,
-                    "timestamp": 1.350677744837142
-                },
-                {
-                    "x": 2.967975868183031,
-                    "y": 6.845440652800829,
-                    "heading": 0.2065131865620181,
-                    "angularVelocity": 0.16016218965195775,
-                    "velocityX": -3.7004634846122446,
-                    "velocityY": -0.492732081438091,
-                    "timestamp": 1.440722927826285
-                },
-                {
-                    "x": 2.7005740636525677,
-                    "y": 6.810001078107782,
-                    "heading": 0.266815731322117,
-                    "angularVelocity": 0.6696920674520688,
-                    "velocityX": -2.9696403033875236,
-                    "velocityY": -0.39357546419024264,
-                    "timestamp": 1.5307681108154279
-                },
-                {
-                    "x": 2.5001864407535797,
-                    "y": 6.78347721885534,
-                    "heading": 0.32524318999567287,
-                    "angularVelocity": 0.6488682318586739,
-                    "velocityX": -2.2254119126300136,
-                    "velocityY": -0.29456166750908425,
-                    "timestamp": 1.6208132938045707
-                },
-                {
-                    "x": 2.366700230698382,
-                    "y": 6.765819830273621,
-                    "heading": 0.3692681816828941,
-                    "angularVelocity": 0.48892111966199864,
-                    "velocityX": -1.4824358796771253,
-                    "velocityY": -0.19609476038099796,
-                    "timestamp": 1.7108584767937136
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "angularVelocity": 0.26355455704906927,
-                    "velocityX": -0.740741797442122,
-                    "velocityY": -0.09794894053004785,
-                    "timestamp": 1.8009036597828565
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "angularVelocity": 0,
-                    "velocityX": 6.927431243507319e-35,
-                    "velocityY": 0,
-                    "timestamp": 1.8909488427719994
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "C1 To W2": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 20
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": -2.4374604803646449e-32,
-                    "velocityY": 2.350988701644575e-38,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.602872461914855,
-                    "y": 7.439913035887961,
-                    "heading": 5.3325436298288445e-18,
-                    "angularVelocity": 5.3856284215509377e-17,
-                    "velocityX": -0.7789533333445217,
-                    "velocityY": -0.2735661154121556,
-                    "timestamp": 0.09901432445765257
-                },
-                {
-                    "x": 7.448617388685645,
-                    "y": 7.385739108696781,
-                    "heading": -4.4022234029758545e-18,
-                    "angularVelocity": -9.831675377471393e-17,
-                    "velocityX": -1.557906636985473,
-                    "velocityY": -0.5471322203925048,
-                    "timestamp": 0.19802864891530514
-                },
-                {
-                    "x": 7.217234786194527,
-                    "y": 7.304478220492258,
-                    "heading": -7.053417683545123e-18,
-                    "angularVelocity": -2.6775864730895626e-17,
-                    "velocityX": -2.3368598812192953,
-                    "velocityY": -0.8206983045092451,
-                    "timestamp": 0.2970429733729577
-                },
-                {
-                    "x": 6.9087246720879545,
-                    "y": 7.196130377471774,
-                    "heading": 2.6673179344101e-18,
-                    "angularVelocity": 9.817504407722252e-17,
-                    "velocityX": -3.1158129472318725,
-                    "velocityY": -1.0942643260352094,
-                    "timestamp": 0.3960572978306103
-                },
-                {
-                    "x": 6.5553201862538,
-                    "y": 7.072015763386003,
-                    "heading": 8.41895708879283e-18,
-                    "angularVelocity": 5.808896021749495e-17,
-                    "velocityX": -3.5692258445423475,
-                    "velocityY": -1.2535015995473784,
-                    "timestamp": 0.49507162228826285
-                },
-                {
-                    "x": 6.2019157004196215,
-                    "y": 6.9479011493002245,
-                    "heading": -3.69857396739816e-19,
-                    "angularVelocity": -8.876306064099344e-17,
-                    "velocityX": -3.569225844542579,
-                    "velocityY": -1.2535015995474599,
-                    "timestamp": 0.5940859467459154
-                },
-                {
-                    "x": 5.848511214585444,
-                    "y": 6.823786535214446,
-                    "heading": -8.032367976884792e-18,
-                    "angularVelocity": -7.738789939855791e-17,
-                    "velocityX": -3.569225844542579,
-                    "velocityY": -1.2535015995474599,
-                    "timestamp": 0.693100271203568
-                },
-                {
-                    "x": 5.495106728751266,
-                    "y": 6.699671921128667,
-                    "heading": -3.490709714785344e-17,
-                    "angularVelocity": -2.714226382747745e-16,
-                    "velocityX": -3.569225844542579,
-                    "velocityY": -1.2535015995474599,
-                    "timestamp": 0.7921145956612206
-                },
-                {
-                    "x": 5.141702242917089,
-                    "y": 6.575557307042889,
-                    "heading": -1.2741337636064699e-17,
-                    "angularVelocity": 2.238641694845762e-16,
-                    "velocityX": -3.569225844542579,
-                    "velocityY": -1.2535015995474599,
-                    "timestamp": 0.8911289201188731
-                },
-                {
-                    "x": 4.788297757082911,
-                    "y": 6.4514426929571105,
-                    "heading": -2.2645871045802615e-17,
-                    "angularVelocity": -1.0003131833516608e-16,
-                    "velocityX": -3.569225844542579,
-                    "velocityY": -1.2535015995474597,
-                    "timestamp": 0.9901432445765257
-                },
-                {
-                    "x": 4.4348932712487334,
-                    "y": 6.327328078871332,
-                    "heading": -7.505924377434873e-18,
-                    "angularVelocity": 1.529066299375997e-16,
-                    "velocityX": -3.569225844542579,
-                    "velocityY": -1.25350159954746,
-                    "timestamp": 1.0891575690341782
-                },
-                {
-                    "x": 4.081488785414556,
-                    "y": 6.203213464785553,
-                    "heading": -3.3249179069580955e-17,
-                    "angularVelocity": -2.5999525658031657e-16,
-                    "velocityX": -3.569225844542579,
-                    "velocityY": -1.25350159954746,
-                    "timestamp": 1.1881718934918308
-                },
-                {
-                    "x": 3.728084299580378,
-                    "y": 6.079098850699775,
-                    "heading": -3.31250701341244e-17,
-                    "angularVelocity": 1.2534442479544336e-18,
-                    "velocityX": -3.569225844542579,
-                    "velocityY": -1.2535015995474597,
-                    "timestamp": 1.2871862179494835
-                },
-                {
-                    "x": 3.3746798137461993,
-                    "y": 5.9549842366139965,
-                    "heading": -3.474935359347948e-17,
-                    "angularVelocity": -1.6404530025230332e-17,
-                    "velocityX": -3.569225844542579,
-                    "velocityY": -1.2535015995474599,
-                    "timestamp": 1.3862005424071362
-                },
-                {
-                    "x": 3.021275327912045,
-                    "y": 5.830869622528225,
-                    "heading": -2.4652412430964293e-17,
-                    "angularVelocity": 1.0197454982216833e-16,
-                    "velocityX": -3.5692258445423475,
-                    "velocityY": -1.2535015995473784,
-                    "timestamp": 1.4852148668647889
-                },
-                {
-                    "x": 2.7127652138054734,
-                    "y": 5.722521779507741,
-                    "heading": 1.414690860074665e-18,
-                    "angularVelocity": 2.632659789801892e-16,
-                    "velocityX": -3.115812947231872,
-                    "velocityY": -1.0942643260352094,
-                    "timestamp": 1.5842291913224416
-                },
-                {
-                    "x": 2.4813826113143547,
-                    "y": 5.641260891303218,
-                    "heading": 8.307803002216867e-18,
-                    "angularVelocity": 6.961732115429119e-17,
-                    "velocityX": -2.336859881219295,
-                    "velocityY": -0.8206983045092451,
-                    "timestamp": 1.6832435157800942
-                },
-                {
-                    "x": 2.3271275380851444,
-                    "y": 5.587086964112038,
-                    "heading": -3.972937436005726e-18,
-                    "angularVelocity": -1.2402993582700068e-16,
-                    "velocityX": -1.557906636985473,
-                    "velocityY": -0.5471322203925048,
-                    "timestamp": 1.782257840237747
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 4.012487544797503e-17,
-                    "velocityX": -0.7789533333445219,
-                    "velocityY": -0.2735661154121556,
-                    "timestamp": 1.8812721646953996
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": -5.265297803216144e-35,
-                    "velocityY": 0,
-                    "timestamp": 1.9802864891530523
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "C1 To W3": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 18
-                },
-                {
-                    "x": 2.9550976753234863,
-                    "y": 6.407241344451904,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 8
-                },
-                {
-                    "x": 1.9163856506347656,
-                    "y": 5.763239860534668,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 11
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "angularVelocity": -3.2387949852399556e-31,
-                    "velocityX": -1.054254683790836e-34,
-                    "velocityY": 3.285843466136746e-33,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.625901434953002,
-                    "y": 7.454866143797358,
-                    "heading": -0.004441360542181856,
-                    "angularVelocity": -0.05445952619049241,
-                    "velocityX": -0.6633512843785839,
-                    "velocityY": -0.14878415147416332,
-                    "timestamp": 0.08155341871038292
-                },
-                {
-                    "x": 7.517704306524476,
-                    "y": 7.430598431564336,
-                    "heading": -0.01332401825146925,
-                    "angularVelocity": -0.10891827528203153,
-                    "velocityX": -1.3267025483353667,
-                    "velocityY": -0.29756830083607516,
-                    "timestamp": 0.16310683742076584
-                },
-                {
-                    "x": 7.355408617374589,
-                    "y": 7.3941968638363065,
-                    "heading": -0.02664786051722782,
-                    "angularVelocity": -0.1633756435530466,
-                    "velocityX": -1.9900537796734206,
-                    "velocityY": -0.4463524436333081,
-                    "timestamp": 0.24466025613114878
-                },
-                {
-                    "x": 7.139014373014635,
-                    "y": 7.345661441877259,
-                    "heading": -0.044412717144295275,
-                    "angularVelocity": -0.21783092490771921,
-                    "velocityX": -2.653404943432558,
-                    "velocityY": -0.5951365709316123,
-                    "timestamp": 0.3262136748415317
-                },
-                {
-                    "x": 6.868521591139737,
-                    "y": 7.284992169702681,
-                    "heading": -0.06661833727396635,
-                    "angularVelocity": -0.2722831302576181,
-                    "velocityX": -3.3167558902158003,
-                    "velocityY": -0.7439206489924082,
-                    "timestamp": 0.4077670935519146
-                },
-                {
-                    "x": 6.5674889621165065,
-                    "y": 7.217472965131769,
-                    "heading": -0.06661834019422361,
-                    "angularVelocity": -3.5807907402848094e-8,
-                    "velocityX": -3.691232492561401,
-                    "velocityY": -0.827913846391786,
-                    "timestamp": 0.4893205122622975
-                },
-                {
-                    "x": 6.266456335099212,
-                    "y": 7.149953751617259,
-                    "heading": -0.06661834311445712,
-                    "angularVelocity": -3.5807616132797033e-8,
-                    "velocityX": -3.6912324679648125,
-                    "velocityY": -0.8279139560572942,
-                    "timestamp": 0.5708739309726805
-                },
-                {
-                    "x": 5.9654237103471415,
-                    "y": 7.082434528003299,
-                    "heading": -0.06661834603468975,
-                    "angularVelocity": -3.5807605171686195e-8,
-                    "velocityX": -3.6912324401888563,
-                    "velocityY": -0.8279140798957559,
-                    "timestamp": 0.6524273496830634
-                },
-                {
-                    "x": 5.664391088171292,
-                    "y": 7.014915292903322,
-                    "heading": -0.06661834895492107,
-                    "angularVelocity": -3.580758928944026e-8,
-                    "velocityX": -3.691232408599494,
-                    "velocityY": -0.8279142207361639,
-                    "timestamp": 0.7339807683934463
-                },
-                {
-                    "x": 5.363358468935584,
-                    "y": 6.947396044694795,
-                    "heading": -0.06661835187515067,
-                    "angularVelocity": -3.580756830329485e-8,
-                    "velocityX": -3.6912323725477565,
-                    "velocityY": -0.8279143814719224,
-                    "timestamp": 0.8155341871038292
-                },
-                {
-                    "x": 5.062325853062933,
-                    "y": 6.879876781492171,
-                    "heading": -0.06661835479537813,
-                    "angularVelocity": -3.58075419139389e-8,
-                    "velocityX": -3.691232331310291,
-                    "velocityY": -0.8279145653280606,
-                    "timestamp": 0.8970876058142121
-                },
-                {
-                    "x": 4.761293241038404,
-                    "y": 6.812357501132806,
-                    "heading": -0.06661835771560275,
-                    "angularVelocity": -3.580750706252144e-8,
-                    "velocityX": -3.691232284125005,
-                    "velocityY": -0.8279147757023306,
-                    "timestamp": 0.978641024524595
-                },
-                {
-                    "x": 4.46026063340559,
-                    "y": 6.744838201193102,
-                    "heading": -0.06661836063582369,
-                    "angularVelocity": -3.580746208542298e-8,
-                    "velocityX": -3.6912322302742044,
-                    "velocityY": -0.8279150157945213,
-                    "timestamp": 1.060194443234978
-                },
-                {
-                    "x": 4.159228030751222,
-                    "y": 6.677318879057164,
-                    "heading": -0.06661836355604021,
-                    "angularVelocity": -3.5807407853670686e-8,
-                    "velocityX": -3.691232169229015,
-                    "velocityY": -0.8279152879625719,
-                    "timestamp": 1.141747861945361
-                },
-                {
-                    "x": 3.8581954336724973,
-                    "y": 6.609799532062401,
-                    "heading": -0.06661836647625141,
-                    "angularVelocity": -3.580734253915704e-8,
-                    "velocityX": -3.6912321008610123,
-                    "velocityY": -0.8279155927790207,
-                    "timestamp": 1.223301280655744
-                },
-                {
-                    "x": 3.557162842693998,
-                    "y": 6.542280157870002,
-                    "heading": -0.0666183693964589,
-                    "angularVelocity": -3.580729730790545e-8,
-                    "velocityX": -3.691232026060653,
-                    "velocityY": -0.8279159262737572,
-                    "timestamp": 1.304854699366127
-                },
-                {
-                    "x": 3.256130258261278,
-                    "y": 6.474760754493496,
-                    "heading": -0.06661837231666672,
-                    "angularVelocity": -3.580730095676624e-8,
-                    "velocityX": -3.6912319457969542,
-                    "velocityY": -0.8279162841264173,
-                    "timestamp": 1.38640811807651
-                },
-                {
-                    "x": 2.9550976753234863,
-                    "y": 6.407241344451904,
-                    "heading": -0.06661837523687889,
-                    "angularVelocity": -3.580735442493409e-8,
-                    "velocityX": -3.6912319274662786,
-                    "velocityY": -0.8279163658530455,
-                    "timestamp": 1.467961536786893
-                },
-                {
-                    "x": 2.779035844872165,
-                    "y": 6.36860208155972,
-                    "heading": -0.0666183783001983,
-                    "angularVelocity": -6.428976142053906e-8,
-                    "velocityX": -3.695002547647375,
-                    "velocityY": -0.8109206547436999,
-                    "timestamp": 1.5156101725269335
-                },
-                {
-                    "x": 2.608054120329012,
-                    "y": 6.311726454658268,
-                    "heading": -0.06677425221357433,
-                    "angularVelocity": -0.0032713195629861798,
-                    "velocityX": -3.5883865694652988,
-                    "velocityY": -1.1936464920308683,
-                    "timestamp": 1.563258808266974
-                },
-                {
-                    "x": 2.4510058546542055,
-                    "y": 6.242296424291011,
-                    "heading": -0.07609605863752185,
-                    "angularVelocity": -0.19563637614744786,
-                    "velocityX": -3.29596562914219,
-                    "velocityY": -1.457125252146435,
-                    "timestamp": 1.6109074440070146
-                },
-                {
-                    "x": 2.3100257060565728,
-                    "y": 6.162946485717711,
-                    "heading": -0.09018973905193617,
-                    "angularVelocity": -0.2957835034721525,
-                    "velocityX": -2.9587447029271896,
-                    "velocityY": -1.6653139663050547,
-                    "timestamp": 1.658556079747055
-                },
-                {
-                    "x": 2.1858070710130613,
-                    "y": 6.074835566620609,
-                    "heading": -0.10721183288872499,
-                    "angularVelocity": -0.3572419980708675,
-                    "velocityX": -2.6069714927652616,
-                    "velocityY": -1.8491803118526937,
-                    "timestamp": 1.7062047154870956
-                },
-                {
-                    "x": 2.078679040228791,
-                    "y": 5.978601109574319,
-                    "heading": -0.12617474237211068,
-                    "angularVelocity": -0.3979738179094536,
-                    "velocityX": -2.248291669223545,
-                    "velocityY": -2.0196686757492435,
-                    "timestamp": 1.7538533512271361
-                },
-                {
-                    "x": 1.9888310933691378,
-                    "y": 5.874644187394139,
-                    "heading": -0.14646656775118436,
-                    "angularVelocity": -0.42586372231046465,
-                    "velocityX": -1.8856352435727148,
-                    "velocityY": -2.181739740617511,
-                    "timestamp": 1.8015019869671767
-                },
-                {
-                    "x": 1.9163856506347656,
-                    "y": 5.763239860534668,
-                    "heading": -0.16767175460275374,
-                    "angularVelocity": -0.44503240276194317,
-                    "velocityX": -1.5204095900989456,
-                    "velocityY": -2.3380381227986136,
-                    "timestamp": 1.8491506227072172
-                },
-                {
-                    "x": 1.8485889050766984,
-                    "y": 5.589343182132367,
-                    "heading": -0.1992083750464638,
-                    "angularVelocity": -0.46340502818407264,
-                    "velocityX": -0.9962181217957636,
-                    "velocityY": -2.555270476750044,
-                    "timestamp": 1.9172047406506962
-                },
-                {
-                    "x": 1.817164507457459,
-                    "y": 5.402475290970386,
-                    "heading": -0.23166704244574374,
-                    "angularVelocity": -0.47695375945076895,
-                    "velocityX": -0.4617560049104581,
-                    "velocityY": -2.7458719150120894,
-                    "timestamp": 1.9852588585941753
-                },
-                {
-                    "x": 1.8230483770902466,
-                    "y": 5.205640044676734,
-                    "heading": -0.2645113992669886,
-                    "angularVelocity": -0.4826211523089798,
-                    "velocityX": 0.08645868627131759,
-                    "velocityY": -2.8923341047058035,
-                    "timestamp": 2.0533129765376543
-                },
-                {
-                    "x": 1.8673165212991787,
-                    "y": 5.004575832748109,
-                    "heading": -0.29674148680613366,
-                    "angularVelocity": -0.4735949640250024,
-                    "velocityX": 0.6504844313118886,
-                    "velocityY": -2.9544753205907597,
-                    "timestamp": 2.1213670944811334
-                },
-                {
-                    "x": 1.9491517735306563,
-                    "y": 4.812420400099087,
-                    "heading": -0.3261812067352161,
-                    "angularVelocity": -0.43259277790693457,
-                    "velocityX": 1.202502577425556,
-                    "velocityY": -2.823568043429816,
-                    "timestamp": 2.1894212124246124
-                },
-                {
-                    "x": 2.050948548896826,
-                    "y": 4.653276186897604,
-                    "heading": -0.34937275798086526,
-                    "angularVelocity": -0.34078101291423535,
-                    "velocityX": 1.4958209501847175,
-                    "velocityY": -2.338494980328238,
-                    "timestamp": 2.2574753303680914
-                },
-                {
-                    "x": 2.14520275900139,
-                    "y": 4.5319666650276815,
-                    "heading": -0.36647677931046935,
-                    "angularVelocity": -0.2513297041615066,
-                    "velocityX": 1.3849890785871777,
-                    "velocityY": -1.7825449147790617,
-                    "timestamp": 2.3255294483115705
-                },
-                {
-                    "x": 2.2207736095496604,
-                    "y": 4.444424274499807,
-                    "heading": -0.37856224680514944,
-                    "angularVelocity": -0.17758613086130492,
-                    "velocityX": 1.1104522816836297,
-                    "velocityY": -1.2863643402237204,
-                    "timestamp": 2.3935835662550495
-                },
-                {
-                    "x": 2.273137984258771,
-                    "y": 4.387723780671385,
-                    "heading": -0.38627299411990695,
-                    "angularVelocity": -0.113303170297753,
-                    "velocityX": 0.7694519639887757,
-                    "velocityY": -0.8331677133117752,
-                    "timestamp": 2.4616376841985286
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": -0.054765324902410414,
-                    "velocityX": 0.3947155080835744,
-                    "velocityY": -0.4073784439379148,
-                    "timestamp": 2.5296918021420076
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": 3.1303206075717874e-30,
-                    "velocityX": 0,
-                    "velocityY": -1.5697460792586675e-31,
-                    "timestamp": 2.5977459200854867
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C1 To S1": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 13
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.263,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": -7.649625564275916e-33,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.617797764168564,
-                    "y": 7.446201717117849,
-                    "heading": 0.013442479376395878,
-                    "angularVelocity": 0.15144225686963014,
-                    "velocityX": -0.7007670767707749,
-                    "velocityY": -0.23431234749607086,
-                    "timestamp": 0.08876306820531243
-                },
-                {
-                    "x": 7.493368305965553,
-                    "y": 7.404602586388856,
-                    "heading": 0.03870227011836747,
-                    "angularVelocity": 0.28457545746724416,
-                    "velocityX": -1.401815650572125,
-                    "velocityY": -0.4686535917478179,
-                    "timestamp": 0.17752613641062487
-                },
-                {
-                    "x": 7.30667604944407,
-                    "y": 7.3422008161122045,
-                    "heading": 0.0730276629215222,
-                    "angularVelocity": 0.38670804756608035,
-                    "velocityX": -2.1032650210965835,
-                    "velocityY": -0.7030150212056434,
-                    "timestamp": 0.2662892046159373
-                },
-                {
-                    "x": 7.057674574154849,
-                    "y": 7.259002273186937,
-                    "heading": 0.11075765125141449,
-                    "angularVelocity": 0.42506403948785515,
-                    "velocityX": -2.8052373619302045,
-                    "velocityY": -0.9373103544914424,
-                    "timestamp": 0.35505227282124974
-                },
-                {
-                    "x": 6.746464870184761,
-                    "y": 7.155111739606062,
-                    "heading": 0.13339737013954753,
-                    "angularVelocity": 0.25505786749611997,
-                    "velocityX": -3.5060719538216505,
-                    "velocityY": -1.170425219424191,
-                    "timestamp": 0.44381534102656217
-                },
-                {
-                    "x": 6.428047828205506,
-                    "y": 7.048517057389171,
-                    "heading": 0.13339738106725169,
-                    "angularVelocity": 1.2311157711164836e-7,
-                    "velocityX": -3.5872694400628884,
-                    "velocityY": -1.2008900139638257,
-                    "timestamp": 0.5325784092318746
-                },
-                {
-                    "x": 6.109630786224927,
-                    "y": 6.941922375176235,
-                    "heading": 0.1333973919949535,
-                    "angularVelocity": 1.23111550991103e-7,
-                    "velocityX": -3.587269440077816,
-                    "velocityY": -1.200890013919235,
-                    "timestamp": 0.621341477437187
-                },
-                {
-                    "x": 5.798381397539494,
-                    "y": 6.838018370585249,
-                    "heading": 0.15626352052401435,
-                    "angularVelocity": 0.2576085864583032,
-                    "velocityX": -3.5065190397107555,
-                    "velocityY": -1.1705769831075976,
-                    "timestamp": 0.7101045456424995
-                },
-                {
-                    "x": 5.5493631968567065,
-                    "y": 6.754814060290535,
-                    "heading": 0.1928079538664876,
-                    "angularVelocity": 0.4117076401528937,
-                    "velocityX": -2.8054257893230465,
-                    "velocityY": -0.9373753293689796,
-                    "timestamp": 0.7988676138478119
-                },
-                {
-                    "x": 5.362653942099421,
-                    "y": 6.692406067264648,
-                    "heading": 0.22585011955614995,
-                    "angularVelocity": 0.37225127926004886,
-                    "velocityX": -2.1034565223155943,
-                    "velocityY": -0.703085126367409,
-                    "timestamp": 0.8876306820531243
-                },
-                {
-                    "x": 5.238210383284219,
-                    "y": 6.650801525067523,
-                    "heading": 0.25010704549903756,
-                    "angularVelocity": 0.2732772360607878,
-                    "velocityX": -1.4019745073178984,
-                    "velocityY": -0.4687145570597535,
-                    "timestamp": 0.9763937502584368
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.263,
-                    "angularVelocity": 0.1452513388947291,
-                    "velocityX": -0.7008588655398115,
-                    "velocityY": -0.2343488737838604,
-                    "timestamp": 1.0651568184637492
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.263,
-                    "angularVelocity": -2.2743517080702824e-31,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 1.1539198866690616
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "C1 To S2": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": -6.443666081756986e-29,
-                    "angularVelocity": 1.4558689567742243e-28,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.593939912495806,
-                    "y": 7.445359803187238,
-                    "heading": -0.04618112566514864,
-                    "angularVelocity": -0.44533178373248467,
-                    "velocityX": -0.8298908206418381,
-                    "velocityY": -0.2086797865609081,
-                    "timestamp": 0.10370049332228601
-                },
-                {
-                    "x": 7.421570715628637,
-                    "y": 7.402025139488172,
-                    "heading": -0.13159426902714605,
-                    "angularVelocity": -0.823652237569843,
-                    "velocityX": -1.662182997832739,
-                    "velocityY": -0.41788290788915955,
-                    "timestamp": 0.20740098664457202
-                },
-                {
-                    "x": 7.162505124622784,
-                    "y": 7.336932458231965,
-                    "heading": -0.24235041158328322,
-                    "angularVelocity": -1.0680387239038096,
-                    "velocityX": -2.4982098224038025,
-                    "velocityY": -0.627698858229146,
-                    "timestamp": 0.311101479966858
-                },
-                {
-                    "x": 6.816624735331939,
-                    "y": 7.250238713519174,
-                    "heading": -0.3315039987658666,
-                    "angularVelocity": -0.8597219196007858,
-                    "velocityX": -3.3353784365894983,
-                    "velocityY": -0.8360012757447058,
-                    "timestamp": 0.41480197328914403
-                },
-                {
-                    "x": 6.436297385949381,
-                    "y": 7.154089090014683,
-                    "heading": -0.3315040257643058,
-                    "angularVelocity": -2.603501519346576e-7,
-                    "velocityX": -3.6675558350581356,
-                    "velocityY": -0.927185786914993,
-                    "timestamp": 0.5185024666114301
-                },
-                {
-                    "x": 6.055970043958053,
-                    "y": 7.05793943727301,
-                    "heading": -0.33150405276269507,
-                    "angularVelocity": -2.60349670609762e-7,
-                    "velocityX": -3.6675557637833287,
-                    "velocityY": -0.9271860688537472,
-                    "timestamp": 0.6222029599337161
-                },
-                {
-                    "x": 5.675642817406181,
-                    "y": 6.961789327902759,
-                    "heading": -0.33150407976108204,
-                    "angularVelocity": -2.6034964813703677e-7,
-                    "velocityX": -3.6675546505826837,
-                    "velocityY": -0.9271904721942118,
-                    "timestamp": 0.7259034532560021
-                },
-                {
-                    "x": 5.306931756724127,
-                    "y": 6.827827408155551,
-                    "heading": -0.331504107569482,
-                    "angularVelocity": -2.6816073038858223e-7,
-                    "velocityX": -3.5555381548297533,
-                    "velocityY": -1.2918156457642633,
-                    "timestamp": 0.8296039465782882
-                },
-                {
-                    "x": 4.97827117667767,
-                    "y": 6.613640897314209,
-                    "heading": -0.3315041419146958,
-                    "angularVelocity": -3.311962425493206e-7,
-                    "velocityX": -3.1693251354651193,
-                    "velocityY": -2.065433866121419,
-                    "timestamp": 0.9333044399005742
-                },
-                {
-                    "x": 4.706831845168329,
-                    "y": 6.330419557819514,
-                    "heading": -0.3315041418010325,
-                    "angularVelocity": 1.0960729474265148e-9,
-                    "velocityX": -2.6175317282796997,
-                    "velocityY": -2.7311474653691747,
-                    "timestamp": 1.0370049332228601
-                },
-                {
-                    "x": 4.518922560808715,
-                    "y": 6.028421366315536,
-                    "heading": -0.2325817741817243,
-                    "angularVelocity": 0.9539237900428917,
-                    "velocityX": -1.8120384806234937,
-                    "velocityY": -2.912215572257732,
-                    "timestamp": 1.1407054265451462
-                },
-                {
-                    "x": 4.388098262013575,
-                    "y": 5.795527878516759,
-                    "heading": -0.12496957799500227,
-                    "angularVelocity": 1.0377211596504115,
-                    "velocityX": -1.2615590784949995,
-                    "velocityY": -2.245828157007679,
-                    "timestamp": 1.2444059198674322
-                },
-                {
-                    "x": 4.304052100915554,
-                    "y": 5.638778154553602,
-                    "heading": -0.0435889655527213,
-                    "angularVelocity": 0.7847659141732163,
-                    "velocityX": -0.8104702148022851,
-                    "velocityY": -1.5115619891604988,
-                    "timestamp": 1.3481064131897182
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 7.875266725539043e-28,
-                    "angularVelocity": 0.42033518025077415,
-                    "velocityX": -0.39587179964482383,
-                    "velocityY": -0.7596700076322139,
-                    "timestamp": 1.4518069065120043
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": -1.3117957600781845e-26,
-                    "velocityX": -2.8963805377195657e-28,
-                    "velocityY": 0,
-                    "timestamp": 1.5555073998342903
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C1 To S3": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 16
-                },
-                {
-                    "x": 5.366531848907471,
-                    "y": 4.105798244476318,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 12
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "angularVelocity": 3.237981257278863e-31,
-                    "velocityX": 3.6994687797817426e-32,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.663124715083084,
-                    "y": 7.408544235319849,
-                    "heading": 0.027485198347684953,
-                    "angularVelocity": 0.3204918925418427,
-                    "velocityX": -0.19677471240300645,
-                    "velocityY": -0.6816250119548423,
-                    "timestamp": 0.08575941852911824
-                },
-                {
-                    "x": 7.629347887601235,
-                    "y": 7.291532761279695,
-                    "heading": 0.07929730291499706,
-                    "angularVelocity": 0.6041564350126787,
-                    "velocityX": -0.3938556028149893,
-                    "velocityY": -1.3644154315298311,
-                    "timestamp": 0.17151883705823648
-                },
-                {
-                    "x": 7.578636110956943,
-                    "y": 7.115823334781324,
-                    "heading": 0.15015093083574435,
-                    "angularVelocity": 0.8261906288076145,
-                    "velocityX": -0.5913260317532774,
-                    "velocityY": -2.048864480566788,
-                    "timestamp": 0.25727825558735473
-                },
-                {
-                    "x": 7.510961813450331,
-                    "y": 6.881225838812792,
-                    "heading": 0.22908991381513497,
-                    "angularVelocity": 0.9204701283345157,
-                    "velocityX": -0.7891179612374983,
-                    "velocityY": -2.7355303941208287,
-                    "timestamp": 0.34303767411647296
-                },
-                {
-                    "x": 7.426596388399054,
-                    "y": 6.588221528517866,
-                    "heading": 0.2780284674480044,
-                    "angularVelocity": 0.5706493172671544,
-                    "velocityX": -0.9837453016618997,
-                    "velocityY": -3.4165846191627605,
-                    "timestamp": 0.4287970926455912
-                },
-                {
-                    "x": 7.336364964950643,
-                    "y": 6.276599275391096,
-                    "heading": 0.2780284905344079,
-                    "angularVelocity": 2.691996271350766e-7,
-                    "velocityX": -1.0521459333096546,
-                    "velocityY": -3.6336796409243712,
-                    "timestamp": 0.5145565111747095
-                },
-                {
-                    "x": 7.246133490581996,
-                    "y": 5.964977037008454,
-                    "heading": 0.27802851362075814,
-                    "angularVelocity": 2.6919900573505555e-7,
-                    "velocityX": -1.0521465270664079,
-                    "velocityY": -3.633679469000075,
-                    "timestamp": 0.6003159297038277
-                },
-                {
-                    "x": 7.155901958204915,
-                    "y": 5.653354815422387,
-                    "heading": 0.2780285367070952,
-                    "angularVelocity": 2.6919885218329885e-7,
-                    "velocityX": -1.0521472034753196,
-                    "velocityY": -3.633679273143183,
-                    "timestamp": 0.686075348232946
-                },
-                {
-                    "x": 7.06567006903972,
-                    "y": 5.341732697146042,
-                    "heading": 0.27802855979342844,
-                    "angularVelocity": 2.6919880800587287e-7,
-                    "velocityX": -1.052151363812688,
-                    "velocityY": -3.6336780684973866,
-                    "timestamp": 0.7718347667620643
-                },
-                {
-                    "x": 6.941702537617662,
-                    "y": 5.041929112902265,
-                    "heading": 0.2780285835706332,
-                    "angularVelocity": 2.772547333115093e-7,
-                    "velocityX": -1.445526725207054,
-                    "velocityY": -3.495867735413615,
-                    "timestamp": 0.8575941852911826
-                },
-                {
-                    "x": 6.763533066668612,
-                    "y": 4.770809627708675,
-                    "heading": 0.2780286115416881,
-                    "angularVelocity": 3.2615723612466423e-7,
-                    "velocityX": -2.0775498948673006,
-                    "velocityY": -3.1613960290733045,
-                    "timestamp": 0.9433536038203009
-                },
-                {
-                    "x": 6.537527544905599,
-                    "y": 4.538061625407243,
-                    "heading": 0.2780286466069666,
-                    "angularVelocity": 4.0887962009830865e-7,
-                    "velocityX": -2.6353434484432303,
-                    "velocityY": -2.713964323608448,
-                    "timestamp": 1.0291130223494191
-                },
-                {
-                    "x": 6.274440682462666,
-                    "y": 4.354088811409382,
-                    "heading": 0.26884633764727317,
-                    "angularVelocity": -0.10707055991260324,
-                    "velocityX": -3.0677314160380695,
-                    "velocityY": -2.145219932145364,
-                    "timestamp": 1.1148724408785373
-                },
-                {
-                    "x": 5.982323590323029,
-                    "y": 4.22411616098336,
-                    "heading": 0.2547621599051439,
-                    "angularVelocity": -0.16422893232822805,
-                    "velocityX": -3.40623918806603,
-                    "velocityY": -1.5155495764222278,
-                    "timestamp": 1.2006318594076555
-                },
-                {
-                    "x": 5.6751302181547745,
-                    "y": 4.153390532297334,
-                    "heading": 0.22346642432676106,
-                    "angularVelocity": -0.3649247641266729,
-                    "velocityX": -3.582036555716069,
-                    "velocityY": -0.8246980902979518,
-                    "timestamp": 1.2863912779367737
-                },
-                {
-                    "x": 5.366531848907471,
-                    "y": 4.105798244476318,
-                    "heading": 0.18319904977282217,
-                    "angularVelocity": -0.4695388010388101,
-                    "velocityX": -3.5984195618411468,
-                    "velocityY": -0.5549511486584576,
-                    "timestamp": 1.3721506964658918
-                },
-                {
-                    "x": 5.15360011207683,
-                    "y": 4.041515222282991,
-                    "heading": 0.15622154454817708,
-                    "angularVelocity": -0.44121073940112593,
-                    "velocityX": -3.482448368249538,
-                    "velocityY": -1.051333676582719,
-                    "timestamp": 1.4332949588681712
-                },
-                {
-                    "x": 4.961585993321137,
-                    "y": 3.955708543808847,
-                    "heading": 0.09940090208560654,
-                    "angularVelocity": -0.9292882149552651,
-                    "velocityX": -3.1403456548775974,
-                    "velocityY": -1.403348001969601,
-                    "timestamp": 1.4944392212704505
-                },
-                {
-                    "x": 4.792433392014809,
-                    "y": 3.849704139398821,
-                    "heading": 0.01980990286006859,
-                    "angularVelocity": -1.301692032882722,
-                    "velocityX": -2.766450925410501,
-                    "velocityY": -1.7336770490843985,
-                    "timestamp": 1.5555834836727298
-                },
-                {
-                    "x": 4.64716063510186,
-                    "y": 3.7242164869664776,
-                    "heading": -0.07646566712737501,
-                    "angularVelocity": -1.5745642551712946,
-                    "velocityX": -2.3759016987918233,
-                    "velocityY": -2.0523209783239644,
-                    "timestamp": 1.6167277460750091
-                },
-                {
-                    "x": 4.526373969972362,
-                    "y": 3.5797426645365276,
-                    "heading": -0.18496768957846801,
-                    "angularVelocity": -1.7745250034621045,
-                    "velocityX": -1.975437438999914,
-                    "velocityY": -2.362835313629761,
-                    "timestamp": 1.6778720084772885
-                },
-                {
-                    "x": 4.435318488505818,
-                    "y": 3.4263170797911884,
-                    "heading": -0.3033986089581413,
-                    "angularVelocity": -1.936909772506439,
-                    "velocityX": -1.489190937777188,
-                    "velocityY": -2.5092392763841707,
-                    "timestamp": 1.7390162708795678
-                },
-                {
-                    "x": 4.37042278768838,
-                    "y": 3.2883178140362905,
-                    "heading": -0.39683380701899557,
-                    "angularVelocity": -1.5281106417823267,
-                    "velocityX": -1.0613538910728308,
-                    "velocityY": -2.2569454652502787,
-                    "timestamp": 1.800160533281847
-                },
-                {
-                    "x": 4.3241368483863285,
-                    "y": 3.1745112953889034,
-                    "heading": -0.4672868225779202,
-                    "angularVelocity": -1.1522424638210744,
-                    "velocityX": -0.7569956277749857,
-                    "velocityY": -1.8612787884925825,
-                    "timestamp": 1.8613047956841264
-                },
-                {
-                    "x": 4.292271472981756,
-                    "y": 3.08778416242541,
-                    "heading": -0.5177338397629753,
-                    "angularVelocity": -0.8250490757931602,
-                    "velocityX": -0.5211507041318677,
-                    "velocityY": -1.418401818193496,
-                    "timestamp": 1.9224490580864058
-                },
-                {
-                    "x": 4.272405555044274,
-                    "y": 3.0293931910282037,
-                    "heading": -0.5502025950282511,
-                    "angularVelocity": -0.5310188395381721,
-                    "velocityX": -0.3249024055075056,
-                    "velocityY": -0.9549705745576195,
-                    "timestamp": 1.983593320488685
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "angularVelocity": -0.25836283489389134,
-                    "velocityX": -0.15382563587721435,
-                    "velocityY": -0.48071871134564137,
-                    "timestamp": 2.0447375828909644
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "angularVelocity": 0,
-                    "velocityX": 3.3381626772503296e-32,
-                    "velocityY": -2.986960154995716e-30,
-                    "timestamp": 2.105881845293244
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C1 To S4": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 20
-                },
-                {
-                    "x": 6.667655944824219,
-                    "y": 1.738860011100769,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 12
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "angularVelocity": 9.776523821034258e-32,
-                    "velocityX": -4.560355515853987e-33,
-                    "velocityY": 2.3850698853192112e-33,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.67836355889096,
-                    "y": 7.400720406813562,
-                    "heading": -0.028945523101477338,
-                    "angularVelocity": -0.3234212568317029,
-                    "velocityX": -0.018284687354286195,
-                    "velocityY": -0.7405714954779761,
-                    "timestamp": 0.0894978993806123
-                },
-                {
-                    "x": 7.675074058245281,
-                    "y": 7.268033905727148,
-                    "heading": -0.0827237859595526,
-                    "angularVelocity": -0.6008885485609575,
-                    "velocityX": -0.03675505982201489,
-                    "velocityY": -1.4825655351097347,
-                    "timestamp": 0.1789957987612246
-                },
-                {
-                    "x": 7.670107822881543,
-                    "y": 7.068767684711926,
-                    "heading": -0.1543450175390465,
-                    "angularVelocity": -0.8002560068466026,
-                    "velocityX": -0.05548996566520156,
-                    "velocityY": -2.226490480718353,
-                    "timestamp": 0.2684936981418369
-                },
-                {
-                    "x": 7.663417838093003,
-                    "y": 6.802742204879413,
-                    "heading": -0.22912917227010834,
-                    "angularVelocity": -0.8355967597967521,
-                    "velocityX": -0.07475018782384167,
-                    "velocityY": -2.9724214945109684,
-                    "timestamp": 0.3579915975224492
-                },
-                {
-                    "x": 7.654753648982881,
-                    "y": 6.471615180012679,
-                    "heading": -0.2502157544796387,
-                    "angularVelocity": -0.23560980040272358,
-                    "velocityX": -0.0968088543986332,
-                    "velocityY": -3.6998301318619107,
-                    "timestamp": 0.4474894969030615
-                },
-                {
-                    "x": 7.646619499365586,
-                    "y": 6.133147678353669,
-                    "heading": -0.250215779137968,
-                    "angularVelocity": -2.7551852588992195e-7,
-                    "velocityX": -0.09088648642693918,
-                    "velocityY": -3.78184855735655,
-                    "timestamp": 0.5369873962836738
-                },
-                {
-                    "x": 7.638485287213415,
-                    "y": 5.7946801781975115,
-                    "heading": -0.25021580379628167,
-                    "angularVelocity": -2.755183512776458e-7,
-                    "velocityX": -0.09088718515702116,
-                    "velocityY": -3.7818485405645093,
-                    "timestamp": 0.626485295664286
-                },
-                {
-                    "x": 7.630351002745582,
-                    "y": 5.456212679779282,
-                    "heading": -0.2502158284545949,
-                    "angularVelocity": -2.7551834628402704e-7,
-                    "velocityX": -0.09088799317222593,
-                    "velocityY": -3.781848521145859,
-                    "timestamp": 0.7159831950448983
-                },
-                {
-                    "x": 7.622216641973054,
-                    "y": 5.117745183194865,
-                    "heading": -0.25021585311291117,
-                    "angularVelocity": -2.7551838019723297e-7,
-                    "velocityX": -0.09088884575864668,
-                    "velocityY": -3.781848500655846,
-                    "timestamp": 0.8054810944255105
-                },
-                {
-                    "x": 7.614082202627753,
-                    "y": 4.779277688498788,
-                    "heading": -0.25021587777122717,
-                    "angularVelocity": -2.755183772950637e-7,
-                    "velocityX": -0.09088972368733664,
-                    "velocityY": -3.7818484795565834,
-                    "timestamp": 0.8949789938061228
-                },
-                {
-                    "x": 7.605947683187911,
-                    "y": 4.440810195727643,
-                    "heading": -0.2502159024295383,
-                    "angularVelocity": -2.755183223713384e-7,
-                    "velocityX": -0.09089061861940764,
-                    "velocityY": -3.7818484580484664,
-                    "timestamp": 0.984476893186735
-                },
-                {
-                    "x": 7.597813082423135,
-                    "y": 4.1023427049110195,
-                    "heading": -0.25021592708784146,
-                    "angularVelocity": -2.755182337798815e-7,
-                    "velocityX": -0.09089152729921718,
-                    "velocityY": -3.781848436209727,
-                    "timestamp": 1.0739747925673473
-                },
-                {
-                    "x": 7.589678399478048,
-                    "y": 3.7638752160694957,
-                    "heading": -0.2502159517461411,
-                    "angularVelocity": -2.755181940361221e-7,
-                    "velocityX": -0.09089244553653594,
-                    "velocityY": -3.781848414141052,
-                    "timestamp": 1.1634726919479597
-                },
-                {
-                    "x": 7.581543633206817,
-                    "y": 3.4254077292306317,
-                    "heading": -0.2502159764044425,
-                    "angularVelocity": -2.7551821391136536e-7,
-                    "velocityX": -0.09089337657676429,
-                    "velocityY": -3.7818483917644334,
-                    "timestamp": 1.252970591328572
-                },
-                {
-                    "x": 7.573407384740196,
-                    "y": 3.0869402780182864,
-                    "heading": -0.2502160010627749,
-                    "angularVelocity": -2.7551856112041726e-7,
-                    "velocityX": -0.09090993780780313,
-                    "velocityY": -3.7818479936934355,
-                    "timestamp": 1.3424684907091844
-                },
-                {
-                    "x": 7.507089727245593,
-                    "y": 2.754933679576005,
-                    "heading": -0.25021602759782946,
-                    "angularVelocity": -2.964880153822211e-7,
-                    "velocityX": -0.7409968049928212,
-                    "velocityY": -3.709658000243552,
-                    "timestamp": 1.4319663900897968
-                },
-                {
-                    "x": 7.3768883522731565,
-                    "y": 2.4424052627551904,
-                    "heading": -0.2502160612558337,
-                    "angularVelocity": -3.7607591242222915e-7,
-                    "velocityX": -1.454798111168213,
-                    "velocityY": -3.492019577931195,
-                    "timestamp": 1.5214642894704091
-                },
-                {
-                    "x": 7.1878697455519145,
-                    "y": 2.1615167326419513,
-                    "heading": -0.2502161148657548,
-                    "angularVelocity": -5.990075908840489e-7,
-                    "velocityX": -2.111989309574672,
-                    "velocityY": -3.1384929932119427,
-                    "timestamp": 1.6109621888510215
-                },
-                {
-                    "x": 6.94738938516625,
-                    "y": 1.9231986374023808,
-                    "heading": -0.25021645217340494,
-                    "angularVelocity": -0.000003768889018580663,
-                    "velocityX": -2.686994466350113,
-                    "velocityY": -2.6628345121941095,
-                    "timestamp": 1.7004600882316339
-                },
-                {
-                    "x": 6.667655944824219,
-                    "y": 1.738860011100769,
-                    "heading": -0.25893161706240975,
-                    "angularVelocity": -0.09737842954215001,
-                    "velocityX": -3.125586659329227,
-                    "velocityY": -2.059697798242895,
-                    "timestamp": 1.7899579876122462
-                },
-                {
-                    "x": 6.480451878153193,
-                    "y": 1.646736643588724,
-                    "heading": -0.268441983388845,
-                    "angularVelocity": -0.16928620019223647,
-                    "velocityX": -3.3322654479868237,
-                    "velocityY": -1.6398122111953808,
-                    "timestamp": 1.846137205502623
-                },
-                {
-                    "x": 6.284980922842322,
-                    "y": 1.5795583069683912,
-                    "heading": -0.28291661109989463,
-                    "angularVelocity": -0.25765092955355146,
-                    "velocityX": -3.47941752575298,
-                    "velocityY": -1.1957862558966221,
-                    "timestamp": 1.9023164233929999
-                },
-                {
-                    "x": 6.086191677834544,
-                    "y": 1.5383458560749974,
-                    "heading": -0.3070406119029933,
-                    "angularVelocity": -0.429411474723066,
-                    "velocityX": -3.53848366838567,
-                    "velocityY": -0.7335889042423714,
-                    "timestamp": 1.9584956412833767
-                },
-                {
-                    "x": 5.891572555778559,
-                    "y": 1.5225106097486452,
-                    "heading": -0.35143250880916643,
-                    "angularVelocity": -0.7901836047765775,
-                    "velocityX": -3.4642547433776456,
-                    "velocityY": -0.2818701811985366,
-                    "timestamp": 2.0146748591737533
-                },
-                {
-                    "x": 5.713348199791213,
-                    "y": 1.5270062964148978,
-                    "heading": -0.407385332387855,
-                    "angularVelocity": -0.9959701412692558,
-                    "velocityX": -3.172425011952234,
-                    "velocityY": 0.08002401662169539,
-                    "timestamp": 2.07085407706413
-                },
-                {
-                    "x": 5.558872974287929,
-                    "y": 1.5428236796222032,
-                    "heading": -0.4653888399418059,
-                    "angularVelocity": -1.0324726781908529,
-                    "velocityX": -2.7496862951120815,
-                    "velocityY": 0.2815522145247918,
-                    "timestamp": 2.1270332949545065
-                },
-                {
-                    "x": 5.4303297125098124,
-                    "y": 1.5629685460150702,
-                    "heading": -0.519265179295127,
-                    "angularVelocity": -0.9590083553396718,
-                    "velocityX": -2.2880927610801525,
-                    "velocityY": 0.35858217948453525,
-                    "timestamp": 2.183212512844883
-                },
-                {
-                    "x": 5.3280145107455015,
-                    "y": 1.583083454211334,
-                    "heading": -0.5654730488836437,
-                    "angularVelocity": -0.8225082392332849,
-                    "velocityX": -1.8212286608183241,
-                    "velocityY": 0.3580489182942135,
-                    "timestamp": 2.2393917307352598
-                },
-                {
-                    "x": 5.251723840078425,
-                    "y": 1.6004232571954151,
-                    "heading": -0.6018676415285119,
-                    "angularVelocity": -0.6478301765590008,
-                    "velocityX": -1.3579874112157035,
-                    "velocityY": 0.3086515554188953,
-                    "timestamp": 2.2955709486256364
-                },
-                {
-                    "x": 5.20115218978415,
-                    "y": 1.613158164546165,
-                    "heading": -0.6270407807035692,
-                    "angularVelocity": -0.44808632302748147,
-                    "velocityX": -0.9001843064628601,
-                    "velocityY": 0.22668359989631479,
-                    "timestamp": 2.351750166516013
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "angularVelocity": -0.2306763921441399,
-                    "velocityX": -0.44771342016952703,
-                    "velocityY": 0.12178587938311458,
-                    "timestamp": 2.4079293844063896
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "angularVelocity": 3.4924263364045203e-31,
-                    "velocityX": 0,
-                    "velocityY": 4.738614736798697e-32,
-                    "timestamp": 2.464108602296766
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C1 To C2": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 9
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": -5.382102664363315e-41,
-                    "angularVelocity": -2.8652892976895567e-45,
-                    "velocityX": 1.592735858156421e-40,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.383499994424153,
-                    "heading": 1.659495816830368e-21,
-                    "angularVelocity": 1.658316243120447e-20,
-                    "velocityX": 2.780383744489643e-33,
-                    "velocityY": -0.8344065481228213,
-                    "timestamp": 0.10007112931184184
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.216499986370153,
-                    "heading": 4.9512387638524226e-21,
-                    "angularVelocity": 3.289403167370527e-20,
-                    "velocityX": 3.618624574631574e-33,
-                    "velocityY": -1.668813065290736,
-                    "timestamp": 0.20014225862368368
-                },
-                {
-                    "x": 7.68,
-                    "y": 6.9659999820333836,
-                    "heading": 9.801760970151702e-21,
-                    "angularVelocity": 4.8470744390330394e-20,
-                    "velocityX": 3.439292531132709e-33,
-                    "velocityY": -2.503219520548835,
-                    "timestamp": 0.30021338793552554
-                },
-                {
-                    "x": 7.68,
-                    "y": 6.632,
-                    "heading": 1.506109654033251e-20,
-                    "angularVelocity": 5.255597191401202e-20,
-                    "velocityX": 4.639675238422859e-33,
-                    "velocityY": -3.3376257900775017,
-                    "timestamp": 0.40028451724736736
-                },
-                {
-                    "x": 7.68,
-                    "y": 6.298000017966616,
-                    "heading": 5.3586032763760104e-21,
-                    "angularVelocity": -9.69559666912617e-20,
-                    "velocityX": 4.935439546802769e-33,
-                    "velocityY": -3.3376257900775017,
-                    "timestamp": 0.5003556465592092
-                },
-                {
-                    "x": 7.68,
-                    "y": 6.047500013629846,
-                    "heading": 1.8760590350448333e-21,
-                    "angularVelocity": -3.480068842301423e-20,
-                    "velocityX": 4.701730454770122e-33,
-                    "velocityY": -2.5032195205488352,
-                    "timestamp": 0.600426775871051
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.880500005575846,
-                    "heading": 3.168722428707802e-22,
-                    "angularVelocity": -1.5580785203142817e-20,
-                    "velocityX": 3.49539235617723e-33,
-                    "velocityY": -1.668813065290736,
-                    "timestamp": 0.7004979051828928
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 5.881496870004905e-41,
-                    "angularVelocity": -3.1664700918333573e-21,
-                    "velocityX": 3.364378386644386e-33,
-                    "velocityY": -0.8344065481228213,
-                    "timestamp": 0.8005690344947346
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 9.145026397391945e-42,
-                    "angularVelocity": -4.963805231001231e-40,
-                    "velocityX": -4.870998572760777e-42,
-                    "velocityY": 3.489748854003666e-39,
-                    "timestamp": 0.9006401638065764
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "C1 To C3": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 14
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "angularVelocity": 1.0127320738607452e-39,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.390554627994461,
-                    "heading": -1.4011773737891353e-18,
-                    "angularVelocity": -1.4633626501290335e-17,
-                    "velocityX": 9.653222160408549e-34,
-                    "velocityY": -0.7983807350946673,
-                    "timestamp": 0.09575052183150974
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.237663887017311,
-                    "heading": -4.2066729097937526e-18,
-                    "angularVelocity": -2.93000547907741e-17,
-                    "velocityX": 5.229262005564601e-34,
-                    "velocityY": -1.5967614385035824,
-                    "timestamp": 0.19150104366301948
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.0083277831364,
-                    "heading": -8.423669065336014e-18,
-                    "angularVelocity": -4.4041495279144365e-17,
-                    "velocityX": -1.080014219031917e-33,
-                    "velocityY": -2.3951420785410225,
-                    "timestamp": 0.28725156549452924
-                },
-                {
-                    "x": 7.68,
-                    "y": 6.702546334555254,
-                    "heading": -1.4072329853209785e-17,
-                    "angularVelocity": -5.899352482411367e-17,
-                    "velocityX": -1.5994734307756055e-33,
-                    "velocityY": -3.1935225284643685,
-                    "timestamp": 0.38300208732603896
-                },
-                {
-                    "x": 7.68,
-                    "y": 6.340327800733179,
-                    "heading": -3.9014262815434015e-17,
-                    "angularVelocity": -2.604887418380216e-16,
-                    "velocityX": 4.89416509473128e-33,
-                    "velocityY": -3.7829405719528495,
-                    "timestamp": 0.4787526091575487
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.978109266911059,
-                    "heading": -3.149786356993182e-17,
-                    "angularVelocity": 7.849982539759592e-17,
-                    "velocityX": -6.0692119772378415e-33,
-                    "velocityY": -3.782940571953311,
-                    "timestamp": 0.5745031309890585
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.61589073308894,
-                    "heading": -3.435394827038493e-17,
-                    "angularVelocity": -2.98283982773363e-17,
-                    "velocityX": 7.480170031140856e-33,
-                    "velocityY": -3.782940571953311,
-                    "timestamp": 0.6702536528205683
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.253672199266821,
-                    "heading": -8.183196831726821e-18,
-                    "angularVelocity": 2.7332228522691796e-16,
-                    "velocityX": -1.1202872015905622e-32,
-                    "velocityY": -3.782940571953311,
-                    "timestamp": 0.766004174652078
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.891453665444746,
-                    "heading": 1.4014689490668664e-17,
-                    "angularVelocity": 2.318304474774222e-16,
-                    "velocityX": 5.155427490211361e-33,
-                    "velocityY": -3.7829405719528495,
-                    "timestamp": 0.8617546964835878
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.5856722168636,
-                    "heading": 8.395813027039913e-18,
-                    "angularVelocity": -5.868246309956073e-17,
-                    "velocityX": -4.168254505442575e-33,
-                    "velocityY": -3.1935225284643685,
-                    "timestamp": 0.9575052183150976
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.356336112982689,
-                    "heading": 4.1945848336017725e-18,
-                    "angularVelocity": -4.387681773072387e-17,
-                    "velocityX": 2.1988846402360483e-33,
-                    "velocityY": -2.3951420785410225,
-                    "timestamp": 1.0532557401466074
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.203445372005539,
-                    "heading": 1.3975677775871127e-18,
-                    "angularVelocity": -2.9211507184809557e-17,
-                    "velocityX": 3.5559383909284404e-33,
-                    "velocityY": -1.5967614385035824,
-                    "timestamp": 1.1490062619781172
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "angularVelocity": -1.4595928576161535e-17,
-                    "velocityX": -6.471812078129625e-34,
-                    "velocityY": -0.7983807350946673,
-                    "timestamp": 1.244756783809627
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 8.546727720642512e-43,
-                    "velocityY": 6.233759417908357e-39,
-                    "timestamp": 1.3405073056411367
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "C1 To C4": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 18
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 3.5626996813915863e-32,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.385155846048237,
-                    "heading": 1.781123128781041e-18,
-                    "angularVelocity": 1.7977717932873702e-17,
-                    "velocityX": 2.837123063321612e-19,
-                    "velocityY": -0.8260917454276266,
-                    "timestamp": 0.099073927811004
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.22146754125807,
-                    "heading": 5.344190150368438e-18,
-                    "angularVelocity": 3.5963720225105045e-17,
-                    "velocityX": 5.673750850451309e-19,
-                    "velocityY": -1.65218345943065,
-                    "timestamp": 0.198147855622008
-                },
-                {
-                    "x": 7.68,
-                    "y": 6.975935091856216,
-                    "heading": 1.0690981078424305e-17,
-                    "angularVelocity": 5.396768903659916e-17,
-                    "velocityX": 8.509388117154945e-19,
-                    "velocityY": -2.47827511058448,
-                    "timestamp": 0.29722178343301203
-                },
-                {
-                    "x": 7.68,
-                    "y": 6.648558516522808,
-                    "heading": 1.7824905328944302e-17,
-                    "angularVelocity": 7.200607069613354e-17,
-                    "velocityX": 1.134205375948437e-18,
-                    "velocityY": -3.304366573190878,
-                    "timestamp": 0.396295711244016
-                },
-                {
-                    "x": 7.68,
-                    "y": 6.273767735073314,
-                    "heading": 3.2118763957552995e-17,
-                    "angularVelocity": 1.4427467391699638e-16,
-                    "velocityX": 1.2981920193437937e-18,
-                    "velocityY": -3.782940575087071,
-                    "timestamp": 0.49536963905502
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.898976953623795,
-                    "heading": 3.194072142377472e-17,
-                    "angularVelocity": -1.7970674806511035e-18,
-                    "velocityX": 1.2970624694923187e-18,
-                    "velocityY": -3.7829405750873115,
-                    "timestamp": 0.5944435668660241
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.524186172174278,
-                    "heading": 3.523836351949418e-17,
-                    "angularVelocity": 3.3284660945411024e-17,
-                    "velocityX": 1.2988038949897902e-18,
-                    "velocityY": -3.782940575087312,
-                    "timestamp": 0.6935174946770281
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.149395390724759,
-                    "heading": 4.1591047407718544e-17,
-                    "angularVelocity": 6.412064231794501e-17,
-                    "velocityX": 1.299093645390414e-18,
-                    "velocityY": -3.782940575087312,
-                    "timestamp": 0.792591422488032
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.774604609275242,
-                    "heading": 3.0405238206332753e-17,
-                    "angularVelocity": -1.1290366142265185e-16,
-                    "velocityX": 1.2929821768433727e-18,
-                    "velocityY": -3.782940575087312,
-                    "timestamp": 0.891665350299036
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.3998138278257235,
-                    "heading": 3.8390344063969795e-17,
-                    "angularVelocity": 8.059744913789504e-17,
-                    "velocityX": 1.304793755296235e-18,
-                    "velocityY": -3.782940575087312,
-                    "timestamp": 0.99073927811004
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.025023046376205,
-                    "heading": 1.0979594277826173e-17,
-                    "angularVelocity": -2.766696586253564e-16,
-                    "velocityX": 1.307186742413441e-18,
-                    "velocityY": -3.782940575087312,
-                    "timestamp": 1.0898132059210441
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.650232264926687,
-                    "heading": -2.14668342522941e-17,
-                    "angularVelocity": -3.2749714528425564e-16,
-                    "velocityX": 1.3056923664932481e-18,
-                    "velocityY": -3.7829405750873115,
-                    "timestamp": 1.1888871337320481
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.2754414834771928,
-                    "heading": -1.7689075007663427e-17,
-                    "angularVelocity": 3.813071035032691e-17,
-                    "velocityX": 1.3012288375156972e-18,
-                    "velocityY": -3.782940575087071,
-                    "timestamp": 1.2879610615430521
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.948064908143785,
-                    "heading": -1.0623620375651477e-17,
-                    "angularVelocity": 7.131497447110055e-17,
-                    "velocityX": 1.136256754825581e-18,
-                    "velocityY": -3.304366573190878,
-                    "timestamp": 1.3870349893540561
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.70253245874193,
-                    "heading": -5.31498515459649e-18,
-                    "angularVelocity": 5.35825654501949e-17,
-                    "velocityX": 8.521926280380639e-19,
-                    "velocityY": -2.4782751105844802,
-                    "timestamp": 1.48610891716506
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.538844153951763,
-                    "heading": -1.7723662552803497e-18,
-                    "angularVelocity": 3.575732766130284e-17,
-                    "velocityX": 5.681284366395801e-19,
-                    "velocityY": -1.6521834594306501,
-                    "timestamp": 1.585182844976064
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "angularVelocity": 1.7889330669293064e-17,
-                    "velocityX": 2.8406422370407584e-19,
-                    "velocityY": -0.8260917454276266,
-                    "timestamp": 1.684256772787068
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 1.0461754707969188e-34,
-                    "velocityY": 0,
-                    "timestamp": 1.783330700598072
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "C1 To C5": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 23
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -8.452820466081124e-33,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.3890447295128565,
-                    "heading": -6.111572142469688e-20,
-                    "angularVelocity": -6.320692328199756e-19,
-                    "velocityX": -1.9797111149811777e-21,
-                    "velocityY": -0.8062267267095136,
-                    "timestamp": 0.09669149868710691
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.2331341916331855,
-                    "heading": -1.8226742072400974e-19,
-                    "angularVelocity": -1.2529715733337834e-18,
-                    "velocityX": -3.753564400558873e-21,
-                    "velocityY": -1.6124534214139752,
-                    "timestamp": 0.19338299737421383
-                },
-                {
-                    "x": 7.68,
-                    "y": 6.999268392550218,
-                    "heading": -3.611632829024713e-19,
-                    "angularVelocity": -1.850171571559818e-18,
-                    "velocityX": -5.1157697830729356e-21,
-                    "velocityY": -2.418680052108357,
-                    "timestamp": 0.29007449606132074
-                },
-                {
-                    "x": 7.68,
-                    "y": 6.687447350831621,
-                    "heading": -5.912144198792796e-19,
-                    "angularVelocity": -2.3792281637439373e-18,
-                    "velocityX": -5.242727749103024e-21,
-                    "velocityY": -3.224906490772764,
-                    "timestamp": 0.38676599474842766
-                },
-                {
-                    "x": 7.68,
-                    "y": 6.321669157855709,
-                    "heading": -4.100561450064452e-17,
-                    "angularVelocity": -4.179726307846966e-16,
-                    "velocityX": -1.166132844762965e-20,
-                    "velocityY": -3.782940568121389,
-                    "timestamp": 0.48345749343553457
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.955890964879757,
-                    "heading": -6.040572418063157e-17,
-                    "angularVelocity": -2.0063924898701218e-16,
-                    "velocityX": -1.9392305823460385e-22,
-                    "velocityY": -3.782940568121787,
-                    "timestamp": 0.5801489921226415
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.590112771903805,
-                    "heading": -8.327536807485113e-17,
-                    "angularVelocity": -2.365217646302656e-16,
-                    "velocityX": 3.675624932403311e-22,
-                    "velocityY": -3.782940568121787,
-                    "timestamp": 0.6768404908097484
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.224334578927854,
-                    "heading": -7.575439540555723e-17,
-                    "angularVelocity": 7.778318436801094e-17,
-                    "velocityX": -2.1057162419585226e-21,
-                    "velocityY": -3.782940568121787,
-                    "timestamp": 0.7735319894968553
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.858556385951902,
-                    "heading": -4.8431591576069396e-17,
-                    "angularVelocity": 2.82577105541659e-16,
-                    "velocityX": 1.016892835365518e-21,
-                    "velocityY": -3.782940568121787,
-                    "timestamp": 0.8702234881839622
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.49277819297595,
-                    "heading": -7.892411277004063e-17,
-                    "angularVelocity": -3.1535886409915893e-16,
-                    "velocityX": 1.448731313682566e-20,
-                    "velocityY": -3.782940568121787,
-                    "timestamp": 0.9669149868710691
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.126999999999999,
-                    "heading": -8.668140475632877e-17,
-                    "angularVelocity": -8.022723912254832e-17,
-                    "velocityX": 2.655349226445793e-21,
-                    "velocityY": -3.782940568121787,
-                    "timestamp": 1.063606485558176
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.7612218070240475,
-                    "heading": -4.236036305527668e-17,
-                    "angularVelocity": 4.583757859030679e-16,
-                    "velocityX": 7.500561292003624e-21,
-                    "velocityY": -3.782940568121787,
-                    "timestamp": 1.160297984245283
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.395443614048096,
-                    "heading": -2.060715967072721e-17,
-                    "angularVelocity": 2.249753461257506e-16,
-                    "velocityX": 1.3311646441840309e-21,
-                    "velocityY": -3.782940568121787,
-                    "timestamp": 1.25698948293239
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.0296654210721448,
-                    "heading": -1.7378554757194933e-17,
-                    "angularVelocity": 3.3390783650741015e-17,
-                    "velocityX": 1.5677048108756238e-21,
-                    "velocityY": -3.782940568121787,
-                    "timestamp": 1.353680981619497
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.663887228096194,
-                    "heading": -2.7058170522872628e-18,
-                    "angularVelocity": 1.5174796030816139e-16,
-                    "velocityX": 2.708497475030379e-21,
-                    "velocityY": -3.782940568121787,
-                    "timestamp": 1.450372480306604
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.2981090351202424,
-                    "heading": -9.881529062229235e-18,
-                    "angularVelocity": -7.421243963841227e-17,
-                    "velocityX": 2.3345387345088647e-21,
-                    "velocityY": -3.782940568121787,
-                    "timestamp": 1.547063978993711
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.9323308421442913,
-                    "heading": -6.5997498705085955e-18,
-                    "angularVelocity": 3.39407211211041e-17,
-                    "velocityX": -6.072343928076645e-20,
-                    "velocityY": -3.782940568121787,
-                    "timestamp": 1.643755477680818
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.5665526491683786,
-                    "heading": 6.487893960505722e-19,
-                    "angularVelocity": 7.496563157826373e-17,
-                    "velocityX": 5.228734565126193e-20,
-                    "velocityY": -3.782940568121389,
-                    "timestamp": 1.7404469763679251
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.254731607449781,
-                    "heading": 3.9009642634108723e-19,
-                    "angularVelocity": -2.6754468948553036e-18,
-                    "velocityX": 1.8077268140623735e-21,
-                    "velocityY": -3.224906490772764,
-                    "timestamp": 1.8371384750550321
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.020865808366814,
-                    "heading": 1.9475770164363563e-19,
-                    "angularVelocity": -2.020226466917104e-18,
-                    "velocityX": 1.3557598034014801e-21,
-                    "velocityY": -2.418680052108357,
-                    "timestamp": 1.9338299737421392
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.8649552704871435,
-                    "heading": 6.488190339479676e-20,
-                    "angularVelocity": -1.3431976959152128e-18,
-                    "velocityX": 9.038296351119003e-22,
-                    "velocityY": -1.6124534214139752,
-                    "timestamp": 2.030521472429246
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "angularVelocity": -6.710197305720325e-19,
-                    "velocityX": 4.519117474728098e-22,
-                    "velocityY": -0.8062267267095136,
-                    "timestamp": 2.127212971116353
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 1.0656726462878492e-32,
-                    "velocityY": 0,
-                    "timestamp": 2.2239044698034602
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "C2 To W1": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 21
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 2.477402495362019e-32,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.61783610738437,
-                    "y": 5.830159445563511,
-                    "heading": -0.009839426193567341,
-                    "angularVelocity": -0.10699963479066096,
-                    "velocityX": -0.6760062707098423,
-                    "velocityY": 0.3605950687933442,
-                    "timestamp": 0.09195756801242966
-                },
-                {
-                    "x": 7.493508328314676,
-                    "y": 5.8964783245931205,
-                    "heading": -0.029519357600464063,
-                    "angularVelocity": -0.21401100346886773,
-                    "velocityX": -1.3520124744152573,
-                    "velocityY": 0.7211900060324206,
-                    "timestamp": 0.18391513602485932
-                },
-                {
-                    "x": 7.307016677169599,
-                    "y": 5.995956620127639,
-                    "heading": -0.05904164496194892,
-                    "angularVelocity": -0.32104249818236175,
-                    "velocityX": -2.028018521758536,
-                    "velocityY": 1.0817847588256284,
-                    "timestamp": 0.275872704037289
-                },
-                {
-                    "x": 7.058361185168299,
-                    "y": 6.1285943089313815,
-                    "heading": -0.09840880514573262,
-                    "angularVelocity": -0.42810136277704247,
-                    "velocityX": -2.7040242296065236,
-                    "velocityY": 1.4423792589405482,
-                    "timestamp": 0.36783027204971863
-                },
-                {
-                    "x": 6.751428461707863,
-                    "y": 6.292319225940392,
-                    "heading": -0.09840881013370367,
-                    "angularVelocity": -5.424209394751822e-8,
-                    "velocityX": -3.3377646896767494,
-                    "velocityY": 1.7804398327158815,
-                    "timestamp": 0.4597878400621483
-                },
-                {
-                    "x": 6.443018732830252,
-                    "y": 6.45324462570272,
-                    "heading": -0.09840881513118924,
-                    "angularVelocity": -5.434556026601013e-8,
-                    "velocityX": -3.3538265043712756,
-                    "velocityY": 1.7499962563230937,
-                    "timestamp": 0.551745408074578
-                },
-                {
-                    "x": 6.108495593860041,
-                    "y": 6.548679499028843,
-                    "heading": -0.09840879814777771,
-                    "angularVelocity": 1.8468748020115896e-7,
-                    "velocityX": -3.637798891386455,
-                    "velocityY": 1.0378142374668238,
-                    "timestamp": 0.6437029760870077
-                },
-                {
-                    "x": 5.7623127419169835,
-                    "y": 6.582899007638951,
-                    "heading": -0.09840874396641834,
-                    "angularVelocity": 5.891995683553032e-7,
-                    "velocityX": -3.764593381767812,
-                    "velocityY": 0.37212281000605224,
-                    "timestamp": 0.7356605440994373
-                },
-                {
-                    "x": 5.416121749902788,
-                    "y": 6.617036066627776,
-                    "heading": -0.09840868977891702,
-                    "angularVelocity": 5.892663593006056e-7,
-                    "velocityX": -3.7646819016288298,
-                    "velocityY": 0.37122620494064046,
-                    "timestamp": 0.8276181121118669
-                },
-                {
-                    "x": 5.068594601971295,
-                    "y": 6.632477066079001,
-                    "heading": -0.0984086463109951,
-                    "angularVelocity": 4.72695427552955e-7,
-                    "velocityX": -3.7792120370616824,
-                    "velocityY": 0.1679143955735957,
-                    "timestamp": 0.9195756801242965
-                },
-                {
-                    "x": 4.721067428734796,
-                    "y": 6.647917495985935,
-                    "heading": -0.09840860284326601,
-                    "angularVelocity": 4.7269333042542933e-7,
-                    "velocityX": -3.7792123122430223,
-                    "velocityY": 0.16790820201819795,
-                    "timestamp": 1.011533248136726
-                },
-                {
-                    "x": 4.3735402523552525,
-                    "y": 6.663357855150138,
-                    "heading": -0.09840855937552709,
-                    "angularVelocity": 4.726934374928119e-7,
-                    "velocityX": -3.779212346422309,
-                    "velocityY": 0.16790743272066663,
-                    "timestamp": 1.1034908161491557
-                },
-                {
-                    "x": 4.026013074283842,
-                    "y": 6.678798176234082,
-                    "heading": -0.09840851590777203,
-                    "angularVelocity": 4.7269361285314424e-7,
-                    "velocityX": -3.779212364820652,
-                    "velocityY": 0.1679070186138197,
-                    "timestamp": 1.1954483841615853
-                },
-                {
-                    "x": 3.6784858950907164,
-                    "y": 6.69423847207055,
-                    "heading": -0.09840847244000124,
-                    "angularVelocity": 4.7269378404370134e-7,
-                    "velocityX": -3.7792123770188395,
-                    "velocityY": 0.16790674405809006,
-                    "timestamp": 1.2874059521740149
-                },
-                {
-                    "x": 3.330958715199463,
-                    "y": 6.709678752193432,
-                    "heading": -0.09840842897219951,
-                    "angularVelocity": 4.7269412044437995e-7,
-                    "velocityX": -3.7792123846106827,
-                    "velocityY": 0.16790657317944163,
-                    "timestamp": 1.3793635201864445
-                },
-                {
-                    "x": 2.991492162018093,
-                    "y": 6.7253542779205,
-                    "heading": -0.07153342912076269,
-                    "angularVelocity": 0.2922543563549246,
-                    "velocityX": -3.6915564484642,
-                    "velocityY": 0.17046477050097905,
-                    "timestamp": 1.471321088198874
-                },
-                {
-                    "x": 2.713831676976805,
-                    "y": 6.738399816428541,
-                    "heading": 0.08699262812295999,
-                    "angularVelocity": 1.7239044123295555,
-                    "velocityX": -3.0194413689122035,
-                    "velocityY": 0.14186476208546492,
-                    "timestamp": 1.5632786562113037
-                },
-                {
-                    "x": 2.5064819943185284,
-                    "y": 6.7478572981535265,
-                    "heading": 0.2316797715135602,
-                    "angularVelocity": 1.5734120259796696,
-                    "velocityX": -2.2548408699787488,
-                    "velocityY": 0.10284614882059591,
-                    "timestamp": 1.6552362242237333
-                },
-                {
-                    "x": 2.368710598807467,
-                    "y": 6.7539967145328434,
-                    "heading": 0.3372042735506754,
-                    "angularVelocity": 1.147534719739997,
-                    "velocityX": -1.4982061671361233,
-                    "velocityY": 0.06676357924654308,
-                    "timestamp": 1.747193792236163
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "angularVelocity": 0.6067551334304849,
-                    "velocityX": -0.7471989559160562,
-                    "velocityY": 0.03265947036300673,
-                    "timestamp": 1.8391513602485925
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "angularVelocity": 0,
-                    "velocityX": -4.676461204476467e-32,
-                    "velocityY": 0,
-                    "timestamp": 1.931108928261022
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C2 To W2": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 19
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": -7.048442713490885e-38,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.598761685357721,
-                    "y": 5.839328892781844,
-                    "heading": -0.010877614975927053,
-                    "angularVelocity": -0.10375029848266179,
-                    "velocityX": -0.774848108801196,
-                    "velocityY": 0.4037314494285639,
-                    "timestamp": 0.10484418006512992
-                },
-                {
-                    "x": 7.436285061134455,
-                    "y": 5.923986667743732,
-                    "heading": -0.03263383242489801,
-                    "angularVelocity": -0.20751001567713226,
-                    "velocityX": -1.5496961693280003,
-                    "velocityY": 0.8074627977375359,
-                    "timestamp": 0.20968836013025985
-                },
-                {
-                    "x": 7.192570139480125,
-                    "y": 6.050973310343331,
-                    "heading": -0.06527034306657675,
-                    "angularVelocity": -0.31128585889464805,
-                    "velocityX": -2.3245441139692504,
-                    "velocityY": 1.2111940073422762,
-                    "timestamp": 0.3145325401953898
-                },
-                {
-                    "x": 6.867616948374112,
-                    "y": 6.220288799737083,
-                    "heading": -0.10878948434468085,
-                    "angularVelocity": -0.41508399656585776,
-                    "velocityX": -3.0993917917441864,
-                    "velocityY": 1.6149250181418864,
-                    "timestamp": 0.4193767202605197
-                },
-                {
-                    "x": 6.5158804421759,
-                    "y": 6.403559919066418,
-                    "heading": -0.10878948853448296,
-                    "angularVelocity": -3.996218113987742e-8,
-                    "velocityX": -3.3548500830442913,
-                    "velocityY": 1.7480333120587623,
-                    "timestamp": 0.5242209003256496
-                },
-                {
-                    "x": 6.14377891604402,
-                    "y": 6.540845489176542,
-                    "heading": -0.10878948531130445,
-                    "angularVelocity": 3.074256021150283e-8,
-                    "velocityX": -3.549090907103546,
-                    "velocityY": 1.309424805695852,
-                    "timestamp": 0.6290650803907796
-                },
-                {
-                    "x": 5.750100268474419,
-                    "y": 6.5890532439035745,
-                    "heading": -0.10878941324219087,
-                    "angularVelocity": 6.873926003335433e-7,
-                    "velocityX": -3.754892711498581,
-                    "velocityY": 0.4598038221776824,
-                    "timestamp": 0.7339092604559095
-                },
-                {
-                    "x": 5.35529697720587,
-                    "y": 6.626964014921626,
-                    "heading": -0.10878934177449004,
-                    "angularVelocity": 6.816563474487017e-7,
-                    "velocityX": -3.7656195224503146,
-                    "velocityY": 0.3615915637329853,
-                    "timestamp": 0.8387534405210395
-                },
-                {
-                    "x": 4.9606240931120595,
-                    "y": 6.587718813133039,
-                    "heading": -0.10878931155021784,
-                    "angularVelocity": 2.882780157548741e-7,
-                    "velocityX": -3.7643757035310585,
-                    "velocityY": -0.3743193161910223,
-                    "timestamp": 0.9435976205861695
-                },
-                {
-                    "x": 4.585497747567555,
-                    "y": 6.458927912145357,
-                    "heading": -0.10878930710495487,
-                    "angularVelocity": 4.239875765382173e-8,
-                    "velocityX": -3.5779415253328617,
-                    "velocityY": -1.2284029586351546,
-                    "timestamp": 1.0484418006512994
-                },
-                {
-                    "x": 4.215349929173217,
-                    "y": 6.316458743577884,
-                    "heading": -0.10878930270384918,
-                    "angularVelocity": 4.197758707259389e-8,
-                    "velocityX": -3.530456513317188,
-                    "velocityY": -1.3588657804273945,
-                    "timestamp": 1.1532859807164293
-                },
-                {
-                    "x": 3.8452021463353314,
-                    "y": 6.173989482631574,
-                    "heading": -0.10878929830273087,
-                    "angularVelocity": 4.197770719831182e-8,
-                    "velocityX": -3.530456174181034,
-                    "velocityY": -1.3588666615333942,
-                    "timestamp": 1.2581301607815591
-                },
-                {
-                    "x": 3.475054376591526,
-                    "y": 6.0315201876656985,
-                    "heading": -0.10878929390160265,
-                    "angularVelocity": 4.197780205439048e-8,
-                    "velocityX": -3.530456049290165,
-                    "velocityY": -1.3588669860107905,
-                    "timestamp": 1.362974340846689
-                },
-                {
-                    "x": 3.104906616234201,
-                    "y": 5.889050868312964,
-                    "heading": -0.10878928950045777,
-                    "angularVelocity": 4.19779608468647e-8,
-                    "velocityX": -3.530455959762256,
-                    "velocityY": -1.358867218611777,
-                    "timestamp": 1.4678185209118189
-                },
-                {
-                    "x": 2.7629439773689555,
-                    "y": 5.757430528072475,
-                    "heading": -0.06527490613894259,
-                    "angularVelocity": 0.4150386157294016,
-                    "velocityX": -3.2616272896866185,
-                    "velocityY": -1.2553900479618894,
-                    "timestamp": 1.5726627009769487
-                },
-                {
-                    "x": 2.506471990883214,
-                    "y": 5.658715266844828,
-                    "heading": -0.03263773447849684,
-                    "angularVelocity": 0.311292163667756,
-                    "velocityX": -2.446220537243172,
-                    "velocityY": -0.9415425936501655,
-                    "timestamp": 1.6775068810420786
-                },
-                {
-                    "x": 2.335490664707938,
-                    "y": 5.592905089907167,
-                    "heading": -0.010879217132893882,
-                    "angularVelocity": 0.20753195200808147,
-                    "velocityX": -1.6308137091544919,
-                    "velocityY": -0.6276950890052175,
-                    "timestamp": 1.7823510611072084
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 0.10376557979790346,
-                    "velocityX": -0.8154068700316104,
-                    "velocityY": -0.3138475582214142,
-                    "timestamp": 1.8871952411723383
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 2.158258486540926e-34,
-                    "velocityY": 0,
-                    "timestamp": 1.9920394212374681
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C2 To W3": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 22
-                },
-                {
-                    "x": 3.4425296783447266,
-                    "y": 2.5693652629852295,
-                    "heading": 0.5090709736575311,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 15
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "angularVelocity": -2.981242670977675e-31,
-                    "velocityX": -5.763030958279131e-32,
-                    "velocityY": 3.5928963704249306e-32,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.6464505983184665,
-                    "y": 5.761517456765686,
-                    "heading": 0.04441523610418216,
-                    "angularVelocity": 0.571175712894524,
-                    "velocityX": -0.43144211544178024,
-                    "velocityY": -0.4563021319898165,
-                    "timestamp": 0.07776107264628128
-                },
-                {
-                    "x": 7.57910774134765,
-                    "y": 5.690410071738714,
-                    "heading": 0.1294441379757556,
-                    "angularVelocity": 1.0934635927458562,
-                    "velocityX": -0.8660227370723712,
-                    "velocityY": -0.914434209908928,
-                    "timestamp": 0.15552214529256256
-                },
-                {
-                    "x": 7.4776816969238515,
-                    "y": 5.583411802725407,
-                    "heading": 0.24873158333085876,
-                    "angularVelocity": 1.5340252043296483,
-                    "velocityX": -1.3043292867777738,
-                    "velocityY": -1.3759875651409672,
-                    "timestamp": 0.23328321793884382
-                },
-                {
-                    "x": 7.341761998810456,
-                    "y": 5.440105324158156,
-                    "heading": 0.38885380810835574,
-                    "angularVelocity": 1.8019584865410045,
-                    "velocityX": -1.747914393255167,
-                    "velocityY": -1.842907687489359,
-                    "timestamp": 0.3110442905851251
-                },
-                {
-                    "x": 7.171108690296084,
-                    "y": 5.260470754663091,
-                    "heading": 0.5147184350397563,
-                    "angularVelocity": 1.6186071339824826,
-                    "velocityX": -2.1945853202236263,
-                    "velocityY": -2.3100834824152265,
-                    "timestamp": 0.3888053632314064
-                },
-                {
-                    "x": 6.970033766909185,
-                    "y": 5.052418524467747,
-                    "heading": 0.5282282166004705,
-                    "angularVelocity": 0.17373450623768458,
-                    "velocityX": -2.585804394720051,
-                    "velocityY": -2.67553189681076,
-                    "timestamp": 0.4665664358776877
-                },
-                {
-                    "x": 6.768438732548067,
-                    "y": 4.838192511419719,
-                    "heading": 0.5282282675213732,
-                    "angularVelocity": 6.548379670470849e-7,
-                    "velocityX": -2.592492972391604,
-                    "velocityY": -2.7549261572367425,
-                    "timestamp": 0.5443275085239689
-                },
-                {
-                    "x": 6.5423880710685225,
-                    "y": 4.649952125007829,
-                    "heading": 0.5282283002658353,
-                    "angularVelocity": 4.210906690145718e-7,
-                    "velocityX": -2.9069900116707847,
-                    "velocityY": -2.42075346964613,
-                    "timestamp": 0.6220885811702502
-                },
-                {
-                    "x": 6.292219183977312,
-                    "y": 4.49519788912002,
-                    "heading": 0.5282283052888641,
-                    "angularVelocity": 6.459567356551782e-8,
-                    "velocityX": -3.217148099656167,
-                    "velocityY": -1.9901247580747212,
-                    "timestamp": 0.6998496538165315
-                },
-                {
-                    "x": 6.0420493950115075,
-                    "y": 4.3404451111724525,
-                    "heading": 0.5282283103117955,
-                    "angularVelocity": 6.459441933764687e-8,
-                    "velocityX": -3.217159697677684,
-                    "velocityY": -1.990106009101862,
-                    "timestamp": 0.7776107264628128
-                },
-                {
-                    "x": 5.780250458218128,
-                    "y": 4.206300927357815,
-                    "heading": 0.5282283154384368,
-                    "angularVelocity": 6.592812032299238e-8,
-                    "velocityX": -3.366709432935023,
-                    "velocityY": -1.7250814482051704,
-                    "timestamp": 0.8553717991090941
-                },
-                {
-                    "x": 5.499389832161033,
-                    "y": 4.11883296395985,
-                    "heading": 0.5282284192975907,
-                    "angularVelocity": 0.0000013356188425542589,
-                    "velocityX": -3.611840944307328,
-                    "velocityY": -1.1248296920470104,
-                    "timestamp": 0.9331328717553754
-                },
-                {
-                    "x": 5.2375954046767585,
-                    "y": 3.9846908767107623,
-                    "heading": 0.5282448470390027,
-                    "angularVelocity": 0.0002112591919891304,
-                    "velocityX": -3.3666514436488617,
-                    "velocityY": -1.7250544865716144,
-                    "timestamp": 1.0108939444016567
-                },
-                {
-                    "x": 4.9990709441074195,
-                    "y": 3.8334805845295827,
-                    "heading": 0.56802081206443,
-                    "angularVelocity": 0.5115151279660829,
-                    "velocityX": -3.067401881842984,
-                    "velocityY": -1.944549978997939,
-                    "timestamp": 1.0886550170479379
-                },
-                {
-                    "x": 4.779271759525853,
-                    "y": 3.6735037985529155,
-                    "heading": 0.6417219019727908,
-                    "angularVelocity": 0.9477890080506808,
-                    "velocityX": -2.826596613210087,
-                    "velocityY": -2.057286255609729,
-                    "timestamp": 1.166416089694219
-                },
-                {
-                    "x": 4.5625385904493,
-                    "y": 3.4912294193947493,
-                    "heading": 0.6783171685956464,
-                    "angularVelocity": 0.47061164895862556,
-                    "velocityX": -2.7871679453603333,
-                    "velocityY": -2.3440311836655594,
-                    "timestamp": 1.2441771623405002
-                },
-                {
-                    "x": 4.372028277853454,
-                    "y": 3.267088484433369,
-                    "heading": 0.6783172621445687,
-                    "angularVelocity": 0.0000012030302442626518,
-                    "velocityX": -2.4499445045265382,
-                    "velocityY": -2.8824311102413667,
-                    "timestamp": 1.3219382349867814
-                },
-                {
-                    "x": 4.1999208824099785,
-                    "y": 3.028525386131778,
-                    "heading": 0.678317151805174,
-                    "angularVelocity": -0.0000014189541221835924,
-                    "velocityX": -2.213284739864076,
-                    "velocityY": -3.067898759405809,
-                    "timestamp": 1.3996993076330626
-                },
-                {
-                    "x": 4.015537599692405,
-                    "y": 2.8383590216563426,
-                    "heading": 0.6532286408155417,
-                    "angularVelocity": -0.32263586568249475,
-                    "velocityX": -2.371151482905778,
-                    "velocityY": -2.4455213644037763,
-                    "timestamp": 1.4774603802793438
-                },
-                {
-                    "x": 3.8266113642227695,
-                    "y": 2.6983370404519986,
-                    "heading": 0.6187227841336836,
-                    "angularVelocity": -0.4437420357974964,
-                    "velocityX": -2.429573423311953,
-                    "velocityY": -1.8006693637223163,
-                    "timestamp": 1.555221452925625
-                },
-                {
-                    "x": 3.635277950307217,
-                    "y": 2.6086556112169066,
-                    "heading": 0.5790475746735078,
-                    "angularVelocity": -0.5102194209775099,
-                    "velocityX": -2.460529509229992,
-                    "velocityY": -1.1532946522360021,
-                    "timestamp": 1.6329825255719062
-                },
-                {
-                    "x": 3.4425296783447266,
-                    "y": 2.5693652629852295,
-                    "heading": 0.5361762744054714,
-                    "angularVelocity": -0.5513208448531661,
-                    "velocityX": -2.4787244491760223,
-                    "velocityY": -0.5052701421751292,
-                    "timestamp": 1.7107435982181873
-                },
-                {
-                    "x": 3.273952117174966,
-                    "y": 2.5733990398146904,
-                    "heading": 0.49726118807033537,
-                    "angularVelocity": -0.5743772146540196,
-                    "velocityX": -2.4881638242809143,
-                    "velocityY": 0.05953756545439798,
-                    "timestamp": 1.7784953915385004
-                },
-                {
-                    "x": 3.1059881626789445,
-                    "y": 2.6157002802478813,
-                    "heading": 0.456882179671172,
-                    "angularVelocity": -0.5959843484623638,
-                    "velocityX": -2.4791071389347814,
-                    "velocityY": 0.6243560260198714,
-                    "timestamp": 1.8462471848588136
-                },
-                {
-                    "x": 2.9403803042277463,
-                    "y": 2.6962017103764078,
-                    "heading": 0.41517848324514556,
-                    "angularVelocity": -0.6155364217277942,
-                    "velocityX": -2.444331734043506,
-                    "velocityY": 1.1881815400506988,
-                    "timestamp": 1.9139989781791267
-                },
-                {
-                    "x": 2.7796953039379972,
-                    "y": 2.8146585273254012,
-                    "heading": 0.3723609476398507,
-                    "angularVelocity": -0.6319764172568042,
-                    "velocityX": -2.3716715442508174,
-                    "velocityY": 1.7483938231562413,
-                    "timestamp": 1.9817507714994398
-                },
-                {
-                    "x": 2.628009703912687,
-                    "y": 2.970316258543535,
-                    "heading": 0.3287788631859365,
-                    "angularVelocity": -0.6432609724124856,
-                    "velocityX": -2.23884258396202,
-                    "velocityY": 2.2974702748047298,
-                    "timestamp": 2.0495025648197527
-                },
-                {
-                    "x": 2.4923790399755665,
-                    "y": 3.160718496478581,
-                    "heading": 0.28508218813026037,
-                    "angularVelocity": -0.6449523018392979,
-                    "velocityX": -2.001875630005736,
-                    "velocityY": 2.8102907480968544,
-                    "timestamp": 2.1172543581400656
-                },
-                {
-                    "x": 2.384956097652455,
-                    "y": 3.376988776463332,
-                    "heading": 0.24257394609876834,
-                    "angularVelocity": -0.627411319291935,
-                    "velocityX": -1.5855365158416335,
-                    "velocityY": 3.192096760631575,
-                    "timestamp": 2.1850061514603785
-                },
-                {
-                    "x": 2.3144199991578422,
-                    "y": 3.6018152629752547,
-                    "heading": 0.17996830737750358,
-                    "angularVelocity": -0.9240440090682317,
-                    "velocityX": -1.0410956675513523,
-                    "velocityY": 3.3183842890918167,
-                    "timestamp": 2.2527579447806914
-                },
-                {
-                    "x": 2.277461106698391,
-                    "y": 3.8129334421080285,
-                    "heading": 0.07321465766316056,
-                    "angularVelocity": -1.57565791963083,
-                    "velocityX": -0.5455042685692306,
-                    "velocityY": 3.1160530044520236,
-                    "timestamp": 2.3205097381010042
-                },
-                {
-                    "x": 2.2642620699526455,
-                    "y": 3.9942911908489713,
-                    "heading": -0.04721496632936169,
-                    "angularVelocity": -1.7775119755599884,
-                    "velocityX": -0.1948145738865332,
-                    "velocityY": 2.676796286166625,
-                    "timestamp": 2.388261531421317
-                },
-                {
-                    "x": 2.2685016834700633,
-                    "y": 4.1415779064179725,
-                    "heading": -0.1677701441610935,
-                    "angularVelocity": -1.7793651197065368,
-                    "velocityX": 0.06257566493294611,
-                    "velocityY": 2.173916118687331,
-                    "timestamp": 2.45601332474163
-                },
-                {
-                    "x": 2.2803498467055876,
-                    "y": 4.251629868834139,
-                    "heading": -0.2721018795383186,
-                    "angularVelocity": -1.5399110527447033,
-                    "velocityX": 0.17487600925202312,
-                    "velocityY": 1.624340213341216,
-                    "timestamp": 2.523765118061943
-                },
-                {
-                    "x": 2.2924250535428135,
-                    "y": 4.3241863896986095,
-                    "heading": -0.3487735727111741,
-                    "angularVelocity": -1.1316555535345214,
-                    "velocityX": 0.17822711762236845,
-                    "velocityY": 1.0709166105971757,
-                    "timestamp": 2.591516911382256
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": -0.6084920452794201,
-                    "velocityX": 0.1118043683563332,
-                    "velocityY": 0.5286001823165565,
-                    "timestamp": 2.6592687047025687
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": -7.720140427747849e-31,
-                    "velocityX": 0,
-                    "velocityY": -1.167304580662352e-31,
-                    "timestamp": 2.7270204980228816
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.615683555603027,
-                    "y": 2.860041856765747,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C2 To S1": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 13
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.263,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "angularVelocity": 7.776001513534033e-33,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.620476769374415,
-                    "y": 5.82809535402629,
-                    "heading": -0.014618874829729222,
-                    "angularVelocity": -0.16274311325472623,
-                    "velocityX": -0.6626362135126213,
-                    "velocityY": 0.34616581514913597,
-                    "timestamp": 0.0898279167539809
-                },
-                {
-                    "x": 7.5013982904113785,
-                    "y": 5.890300493780534,
-                    "heading": -0.04163561529349077,
-                    "angularVelocity": -0.30076107116849365,
-                    "velocityX": -1.3256288608937272,
-                    "velocityY": 0.6924922897255835,
-                    "timestamp": 0.1796558335079618
-                },
-                {
-                    "x": 7.3227216232188646,
-                    "y": 5.983633458070523,
-                    "heading": -0.07726722819497155,
-                    "angularVelocity": -0.39666524827763777,
-                    "velocityX": -1.9890995321851956,
-                    "velocityY": 1.0390195794655717,
-                    "timestamp": 0.2694837502619427
-                },
-                {
-                    "x": 7.084404558965627,
-                    "y": 6.108106280619183,
-                    "heading": -0.11365614248079976,
-                    "angularVelocity": -0.40509582767559393,
-                    "velocityX": -2.6530400889284445,
-                    "velocityY": 1.3856808333823862,
-                    "timestamp": 0.3593116670159236
-                },
-                {
-                    "x": 6.786764320803032,
-                    "y": 6.263508027124416,
-                    "heading": -0.12453213515982267,
-                    "angularVelocity": -0.12107586452004557,
-                    "velocityX": -3.313449191722499,
-                    "velocityY": 1.7299938829800985,
-                    "timestamp": 0.4491395837699045
-                },
-                {
-                    "x": 6.485598483527544,
-                    "y": 6.42090073842616,
-                    "heading": -0.1245321478638912,
-                    "angularVelocity": -1.4142672994383305e-7,
-                    "velocityX": -3.3526975594938513,
-                    "velocityY": 1.7521580928210598,
-                    "timestamp": 0.5389675005238854
-                },
-                {
-                    "x": 6.164419225817841,
-                    "y": 6.531883674964022,
-                    "heading": -0.12453214704403566,
-                    "angularVelocity": 9.126957138704023e-9,
-                    "velocityX": -3.575494894191328,
-                    "velocityY": 1.2355060714790946,
-                    "timestamp": 0.6287954172778664
-                },
-                {
-                    "x": 5.838530026083828,
-                    "y": 6.576482035403971,
-                    "heading": -0.08897109734241883,
-                    "angularVelocity": 0.39587971074750417,
-                    "velocityX": -3.62792783702814,
-                    "velocityY": 0.4964866385813549,
-                    "timestamp": 0.7186233340318473
-                },
-                {
-                    "x": 5.57314678987786,
-                    "y": 6.601508855183474,
-                    "heading": 0.02708371106201491,
-                    "angularVelocity": 1.2919681608812392,
-                    "velocityX": -2.9543514510393742,
-                    "velocityY": 0.27860848479928674,
-                    "timestamp": 0.8084512507858282
-                },
-                {
-                    "x": 5.3743330310505755,
-                    "y": 6.616929267325104,
-                    "heading": 0.1370261836602847,
-                    "angularVelocity": 1.2239232141983023,
-                    "velocityX": -2.213273623742076,
-                    "velocityY": 0.17166614454461948,
-                    "timestamp": 0.8982791675398092
-                },
-                {
-                    "x": 5.242037120273562,
-                    "y": 6.625930657483227,
-                    "heading": 0.21902488355780253,
-                    "angularVelocity": 0.9128420524556349,
-                    "velocityX": -1.4727705546076917,
-                    "velocityY": 0.10020704568687104,
-                    "timestamp": 0.9881070842937901
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.263,
-                    "angularVelocity": 0.48954843918551255,
-                    "velocityX": -0.7351514168409599,
-                    "velocityY": 0.0453015350218741,
-                    "timestamp": 1.077935001047771
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.263,
-                    "angularVelocity": -4.780109867389741e-33,
-                    "velocityX": 0,
-                    "velocityY": -5.233682494366388e-34,
-                    "timestamp": 1.167762917801752
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C2 To S2": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 11
-                },
-                {
-                    "x": 5.40805721282959,
-                    "y": 6.638837814331055,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 9
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "angularVelocity": 1.3211551091100433e-31,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.6375302942972665,
-                    "y": 5.822387711771769,
-                    "heading": 0.010500881646553203,
-                    "angularVelocity": 0.13619602846032355,
-                    "velocityX": -0.5508304389364825,
-                    "velocityY": 0.32927763890619355,
-                    "timestamp": 0.07710123243140166
-                },
-                {
-                    "x": 7.5525908811480225,
-                    "y": 5.873163180716977,
-                    "heading": 0.031500279151306286,
-                    "angularVelocity": 0.27236137273728567,
-                    "velocityX": -1.1016609004897018,
-                    "velocityY": 0.6585558666702785,
-                    "timestamp": 0.1542024648628033
-                },
-                {
-                    "x": 7.4251817593524025,
-                    "y": 5.949326484276257,
-                    "heading": 0.06299451206522548,
-                    "angularVelocity": 0.40847898173275193,
-                    "velocityX": -1.6524913776051289,
-                    "velocityY": 0.987835098836381,
-                    "timestamp": 0.23130369729420497
-                },
-                {
-                    "x": 7.255302939480135,
-                    "y": 6.050877733979633,
-                    "heading": 0.10497913316622995,
-                    "angularVelocity": 0.5445389104299847,
-                    "velocityX": -2.203321717631576,
-                    "velocityY": 1.3171157775425744,
-                    "timestamp": 0.3084049297256066
-                },
-                {
-                    "x": 7.042954462706059,
-                    "y": 6.177817072431174,
-                    "heading": 0.1574496496087124,
-                    "angularVelocity": 0.6805405670936119,
-                    "velocityX": -2.754151523621958,
-                    "velocityY": 1.6463983058180132,
-                    "timestamp": 0.3855061621570083
-                },
-                {
-                    "x": 6.792607460054783,
-                    "y": 6.3274746833219355,
-                    "heading": 0.15744965721454035,
-                    "angularVelocity": 9.864729365588338e-8,
-                    "velocityX": -3.246990933303352,
-                    "velocityY": 1.941053419916652,
-                    "timestamp": 0.46260739458840994
-                },
-                {
-                    "x": 6.542259846259959,
-                    "y": 6.4771312718889495,
-                    "heading": 0.15744966481969572,
-                    "angularVelocity": 9.863857053631023e-8,
-                    "velocityX": -3.246998859811527,
-                    "velocityY": 1.9410401604172332,
-                    "timestamp": 0.5397086270198116
-                },
-                {
-                    "x": 6.273256747030451,
-                    "y": 6.589862608843688,
-                    "heading": 0.1574496727775284,
-                    "angularVelocity": 1.0321278187268036e-7,
-                    "velocityX": -3.4889597837342676,
-                    "velocityY": 1.4621210764047974,
-                    "timestamp": 0.6168098594512132
-                },
-                {
-                    "x": 5.98904964850956,
-                    "y": 6.655416753006686,
-                    "heading": 0.15744969119167346,
-                    "angularVelocity": 2.3883074855376557e-7,
-                    "velocityX": -3.686155065987494,
-                    "velocityY": 0.8502347121535809,
-                    "timestamp": 0.6939110918826149
-                },
-                {
-                    "x": 5.6978465137603145,
-                    "y": 6.6719003531074685,
-                    "heading": 0.15744999446473232,
-                    "angularVelocity": 0.00000393343983387992,
-                    "velocityX": -3.7768933850484796,
-                    "velocityY": 0.21379165521703997,
-                    "timestamp": 0.7710123243140166
-                },
-                {
-                    "x": 5.40805721282959,
-                    "y": 6.638837814331055,
-                    "heading": 0.15745035498223803,
-                    "angularVelocity": 0.00000467589809324531,
-                    "velocityX": -3.7585560151526085,
-                    "velocityY": -0.428819848059242,
-                    "timestamp": 0.8481135567454182
-                },
-                {
-                    "x": 5.104307335648147,
-                    "y": 6.562247743535093,
-                    "heading": 0.18966206475475966,
-                    "angularVelocity": 0.37702586381684944,
-                    "velocityX": -3.555277277652879,
-                    "velocityY": -0.896457772827513,
-                    "timestamp": 0.9335498964342235
-                },
-                {
-                    "x": 4.836190414730584,
-                    "y": 6.438293970082571,
-                    "heading": 0.27176288986041636,
-                    "angularVelocity": 0.9609590650149814,
-                    "velocityX": -3.1382070193334055,
-                    "velocityY": -1.4508319750590115,
-                    "timestamp": 1.0189862361230289
-                },
-                {
-                    "x": 4.616497381460216,
-                    "y": 6.278221072521664,
-                    "heading": 0.3274582685749672,
-                    "angularVelocity": 0.6518933151562509,
-                    "velocityX": -2.5714237532949347,
-                    "velocityY": -1.8735926438791781,
-                    "timestamp": 1.1044225758118342
-                },
-                {
-                    "x": 4.449006665280017,
-                    "y": 6.09487376587715,
-                    "heading": 0.3096216512620487,
-                    "angularVelocity": -0.2087708506460702,
-                    "velocityX": -1.960415401575849,
-                    "velocityY": -2.1460107878256482,
-                    "timestamp": 1.1898589155006396
-                },
-                {
-                    "x": 4.339814814870372,
-                    "y": 5.903698766708507,
-                    "heading": 0.23521329843339078,
-                    "angularVelocity": -0.8709215902704173,
-                    "velocityX": -1.2780492564097095,
-                    "velocityY": -2.237630964352897,
-                    "timestamp": 1.275295255189445
-                },
-                {
-                    "x": 4.286146359443328,
-                    "y": 5.736442488850756,
-                    "heading": 0.1356278137676922,
-                    "angularVelocity": -1.1656103834554516,
-                    "velocityX": -0.628168945703036,
-                    "velocityY": -1.957671389796991,
-                    "timestamp": 1.3607315948782504
-                },
-                {
-                    "x": 4.266951931468271,
-                    "y": 5.619203136466468,
-                    "heading": 0.049466079264437586,
-                    "angularVelocity": -1.0084904715849423,
-                    "velocityX": -0.2246635102225946,
-                    "velocityY": -1.372242219309977,
-                    "timestamp": 1.4461679345670557
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 4.182976778726496e-34,
-                    "angularVelocity": -0.578981724224301,
-                    "velocityX": -0.046255861178807596,
-                    "velocityY": -0.6929502912005617,
-                    "timestamp": 1.531604274255861
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -6.30298269203181e-33,
-                    "timestamp": 1.6170406139446665
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C2 To S3": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 20
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "angularVelocity": 7.446350164213638e-30,
-                    "velocityX": -3.476389258222187e-32,
-                    "velocityY": 2.4606016306403766e-37,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.6436015698643995,
-                    "y": 5.74806653588022,
-                    "heading": -0.0038807165751560974,
-                    "angularVelocity": -0.045372809336175934,
-                    "velocityX": -0.4255654848002544,
-                    "velocityY": -0.572123393880186,
-                    "timestamp": 0.08552956345292874
-                },
-                {
-                    "x": 7.570804707716712,
-                    "y": 5.6501996114987145,
-                    "heading": -0.011642164214821673,
-                    "angularVelocity": -0.09074578807989701,
-                    "velocityX": -0.8511309915401587,
-                    "velocityY": -1.1442467426525054,
-                    "timestamp": 0.17105912690585748
-                },
-                {
-                    "x": 7.461609412342726,
-                    "y": 5.503399231866818,
-                    "heading": -0.02328436718357624,
-                    "angularVelocity": -0.136119050521775,
-                    "velocityX": -1.2766965124764296,
-                    "velocityY": -1.7163700328330225,
-                    "timestamp": 0.25658869035878623
-                },
-                {
-                    "x": 7.3160156842382476,
-                    "y": 5.307665404737857,
-                    "heading": -0.038807352623976644,
-                    "angularVelocity": -0.18149263031072865,
-                    "velocityX": -1.7022620276158245,
-                    "velocityY": -2.288493232362672,
-                    "timestamp": 0.34211825381171496
-                },
-                {
-                    "x": 7.134023531165023,
-                    "y": 5.062998148299065,
-                    "heading": -0.05821111114579378,
-                    "angularVelocity": -0.22686610031040333,
-                    "velocityX": -2.127827452005934,
-                    "velocityY": -2.8606162192496707,
-                    "timestamp": 0.4276478172646437
-                },
-                {
-                    "x": 6.9409164317403365,
-                    "y": 4.803390071813602,
-                    "heading": -0.058211113467083825,
-                    "angularVelocity": -2.714020682501112e-8,
-                    "velocityX": -2.2577818900123794,
-                    "velocityY": -3.0353022511138943,
-                    "timestamp": 0.5131773807175725
-                },
-                {
-                    "x": 6.70968017178257,
-                    "y": 4.577079452070077,
-                    "heading": -0.05821111591888997,
-                    "angularVelocity": -2.866618337367041e-8,
-                    "velocityX": -2.7035828387575864,
-                    "velocityY": -2.645992924634468,
-                    "timestamp": 0.5987069441705012
-                },
-                {
-                    "x": 6.4400791107327935,
-                    "y": 4.398188508585333,
-                    "heading": -0.05821111892081074,
-                    "angularVelocity": -3.509804859460385e-8,
-                    "velocityX": -3.152138864804934,
-                    "velocityY": -2.091568532127445,
-                    "timestamp": 0.6842365076234299
-                },
-                {
-                    "x": 6.141694589328426,
-                    "y": 4.273075039260189,
-                    "heading": -0.058211144902897714,
-                    "angularVelocity": -3.0377902004237926e-7,
-                    "velocityX": -3.488671160686832,
-                    "velocityY": -1.462809632999017,
-                    "timestamp": 0.7697660710763586
-                },
-                {
-                    "x": 5.825131041705898,
-                    "y": 4.206185466847777,
-                    "heading": -0.05821124348692034,
-                    "angularVelocity": -0.000001152630957627144,
-                    "velocityX": -3.7012178578082944,
-                    "velocityY": -0.7820637649954203,
-                    "timestamp": 0.8552956345292874
-                },
-                {
-                    "x": 5.503318785454965,
-                    "y": 4.1726658015863975,
-                    "heading": -0.058211338856237596,
-                    "angularVelocity": -0.0000011150450605169934,
-                    "velocityX": -3.76258504380235,
-                    "velocityY": -0.39190735820634004,
-                    "timestamp": 0.9408251979822161
-                },
-                {
-                    "x": 5.1935160355708865,
-                    "y": 4.079344267897855,
-                    "heading": -0.0582116136415855,
-                    "angularVelocity": -0.0000032127528402629677,
-                    "velocityX": -3.622171532005757,
-                    "velocityY": -1.091102654112135,
-                    "timestamp": 1.026354761435145
-                },
-                {
-                    "x": 4.918641828284195,
-                    "y": 3.9369980738928456,
-                    "heading": -0.09326111746178506,
-                    "angularVelocity": -0.4097940221510551,
-                    "velocityX": -3.2137917719873403,
-                    "velocityY": -1.664292301495839,
-                    "timestamp": 1.1118843248880736
-                },
-                {
-                    "x": 4.689025118769517,
-                    "y": 3.755353610578967,
-                    "heading": -0.16867561276178636,
-                    "angularVelocity": -0.8817360016283226,
-                    "velocityX": -2.6846472756878708,
-                    "velocityY": -2.1237623107224977,
-                    "timestamp": 1.1974138883410024
-                },
-                {
-                    "x": 4.510985991586203,
-                    "y": 3.542062267326439,
-                    "heading": -0.2715725395002121,
-                    "angularVelocity": -1.2030568447254515,
-                    "velocityX": -2.0816092120158958,
-                    "velocityY": -2.493773318156982,
-                    "timestamp": 1.282943451793931
-                },
-                {
-                    "x": 4.39344009542189,
-                    "y": 3.3364721307805043,
-                    "heading": -0.3792171957634167,
-                    "angularVelocity": -1.2585666513128753,
-                    "velocityX": -1.3743305989045989,
-                    "velocityY": -2.4037318588569803,
-                    "timestamp": 1.3684730152468598
-                },
-                {
-                    "x": 4.321460732883776,
-                    "y": 3.1711461674458645,
-                    "heading": -0.4697306425856333,
-                    "angularVelocity": -1.0582708851545903,
-                    "velocityX": -0.8415728975131295,
-                    "velocityY": -1.932968632835679,
-                    "timestamp": 1.4540025786997886
-                },
-                {
-                    "x": 4.280782976343733,
-                    "y": 3.057632956228743,
-                    "heading": -0.5333100471714295,
-                    "angularVelocity": -0.7433617338733072,
-                    "velocityX": -0.475598785938263,
-                    "velocityY": -1.3271809960727023,
-                    "timestamp": 1.5395321421527173
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "angularVelocity": -0.38220647351440606,
-                    "velocityX": -0.20791613596297567,
-                    "velocityY": -0.6738366700592523,
-                    "timestamp": 1.625061705605646
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "angularVelocity": -1.9453350650223036e-32,
-                    "velocityX": 0,
-                    "velocityY": 3.675541041669649e-33,
-                    "timestamp": 1.7105912690585747
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C2 To S4": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 12
-                },
-                {
-                    "x": 6.7368645668029785,
-                    "y": 3.4413950443267822,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 15
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "angularVelocity": -1.007826409753455e-39,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.661226431831969,
-                    "y": 5.757681823957774,
-                    "heading": -0.0001846118691686639,
-                    "angularVelocity": -0.0025538708536768893,
-                    "velocityX": -0.25970848342391806,
-                    "velocityY": -0.5439170529292338,
-                    "timestamp": 0.07228708096295171
-                },
-                {
-                    "x": 7.623679292701499,
-                    "y": 5.679045474684997,
-                    "heading": -0.0005538375109882563,
-                    "angularVelocity": -0.005107768039614528,
-                    "velocityX": -0.5194170055049473,
-                    "velocityY": -1.0878340669625215,
-                    "timestamp": 0.14457416192590342
-                },
-                {
-                    "x": 7.567358579979895,
-                    "y": 5.561090955652203,
-                    "heading": -0.0011076778331591435,
-                    "angularVelocity": -0.0076616777824344475,
-                    "velocityX": -0.779125563950612,
-                    "velocityY": -1.6317510329853882,
-                    "timestamp": 0.2168612428888551
-                },
-                {
-                    "x": 7.492264291454711,
-                    "y": 5.403818271607319,
-                    "heading": -0.001846132228095339,
-                    "angularVelocity": -0.010215579120073546,
-                    "velocityX": -1.0388341530026832,
-                    "velocityY": -2.175667933326667,
-                    "timestamp": 0.28914832385180683
-                },
-                {
-                    "x": 7.39839642615194,
-                    "y": 5.207227430397753,
-                    "heading": -0.0027691974892554232,
-                    "angularVelocity": -0.012769436099282666,
-                    "velocityX": -1.298542755528879,
-                    "velocityY": -2.719584725109078,
-                    "timestamp": 0.36143540481475855
-                },
-                {
-                    "x": 7.285754988600711,
-                    "y": 4.97131845200793,
-                    "heading": -0.0038768638943260385,
-                    "angularVelocity": -0.015323158582628504,
-                    "velocityX": -1.5582512954003445,
-                    "velocityY": -3.263501240432299,
-                    "timestamp": 0.4337224857777103
-                },
-                {
-                    "x": 7.167927209247298,
-                    "y": 4.7245478329210355,
-                    "heading": -0.0038768640537450113,
-                    "angularVelocity": -2.205359112715029e-9,
-                    "velocityX": -1.629997750411335,
-                    "velocityY": -3.4137582511232942,
-                    "timestamp": 0.506009566740662
-                },
-                {
-                    "x": 7.050099366875978,
-                    "y": 4.477777243923851,
-                    "heading": -0.003876864213154645,
-                    "angularVelocity": -2.2052299170164517e-9,
-                    "velocityX": -1.6299986221840805,
-                    "velocityY": -3.413757834870371,
-                    "timestamp": 0.5782966477036137
-                },
-                {
-                    "x": 6.932271561638543,
-                    "y": 4.2310066371960025,
-                    "heading": -0.0038768643725604667,
-                    "angularVelocity": -2.2051771914474314e-9,
-                    "velocityX": -1.629998108483918,
-                    "velocityY": -3.413758080151585,
-                    "timestamp": 0.6505837286665653
-                },
-                {
-                    "x": 6.8252544845676155,
-                    "y": 3.9793591274718993,
-                    "heading": -0.0038768645333797187,
-                    "angularVelocity": -2.2247301918048757e-9,
-                    "velocityX": -1.4804454080221456,
-                    "velocityY": -3.4812238421008592,
-                    "timestamp": 0.722870809629517
-                },
-                {
-                    "x": 6.7595626994044276,
-                    "y": 3.7139091088268694,
-                    "heading": -0.003876864707631206,
-                    "angularVelocity": -2.410548124556829e-9,
-                    "velocityX": -0.9087624550347577,
-                    "velocityY": -3.6721640313720423,
-                    "timestamp": 0.7951578905924687
-                },
-                {
-                    "x": 6.7368645668029785,
-                    "y": 3.4413950443267822,
-                    "heading": -0.0038768649214938546,
-                    "angularVelocity": -2.9585182521427424e-9,
-                    "velocityX": -0.31399985030634436,
-                    "velocityY": -3.7698861382956523,
-                    "timestamp": 0.8674449715554203
-                },
-                {
-                    "x": 6.763602448674763,
-                    "y": 3.1456190677825386,
-                    "heading": -0.0038769163309766456,
-                    "angularVelocity": -6.548510424231184e-7,
-                    "velocityX": 0.340585605327871,
-                    "velocityY": -3.7675774205236117,
-                    "timestamp": 0.9459505849856912
-                },
-                {
-                    "x": 6.763837716877819,
-                    "y": 2.855181760345028,
-                    "heading": -0.02573615579182971,
-                    "angularVelocity": -0.27844173818562123,
-                    "velocityX": 0.0029968328731749647,
-                    "velocityY": -3.699573759722024,
-                    "timestamp": 1.0244561984159621
-                },
-                {
-                    "x": 6.746691140594586,
-                    "y": 2.576419895706607,
-                    "heading": -0.08408576500639096,
-                    "angularVelocity": -0.7432539746522423,
-                    "velocityX": -0.2184121049950426,
-                    "velocityY": -3.5508526391685153,
-                    "timestamp": 1.102961811846233
-                },
-                {
-                    "x": 6.69086397201793,
-                    "y": 2.322743038831463,
-                    "heading": -0.2046015958190934,
-                    "angularVelocity": -1.5351237388870964,
-                    "velocityX": -0.7111232705192682,
-                    "velocityY": -3.2313212494092722,
-                    "timestamp": 1.1814674252765038
-                },
-                {
-                    "x": 6.586917702656428,
-                    "y": 2.0871040226876985,
-                    "heading": -0.32491541634118276,
-                    "angularVelocity": -1.5325505433946174,
-                    "velocityX": -1.324061615719081,
-                    "velocityY": -3.001556269005664,
-                    "timestamp": 1.2599730387067747
-                },
-                {
-                    "x": 6.439847045017927,
-                    "y": 1.879401635438828,
-                    "heading": -0.448122685606705,
-                    "angularVelocity": -1.5694071274910255,
-                    "velocityX": -1.8733775995410846,
-                    "velocityY": -2.6457010928696563,
-                    "timestamp": 1.3384786521370455
-                },
-                {
-                    "x": 6.25456019704424,
-                    "y": 1.7060438180074393,
-                    "heading": -0.5681346722700569,
-                    "angularVelocity": -1.528705801018258,
-                    "velocityX": -2.3601732395640638,
-                    "velocityY": -2.2082219328859343,
-                    "timestamp": 1.4169842655673164
-                },
-                {
-                    "x": 6.049779045368227,
-                    "y": 1.57972317182915,
-                    "heading": -0.6633309119306381,
-                    "angularVelocity": -1.2126042393788183,
-                    "velocityX": -2.6084905617341763,
-                    "velocityY": -1.6090651439911137,
-                    "timestamp": 1.4954898789975872
-                },
-                {
-                    "x": 5.840944041976222,
-                    "y": 1.503518626439596,
-                    "heading": -0.7208467997653518,
-                    "angularVelocity": -0.7326340795464229,
-                    "velocityX": -2.660128292322611,
-                    "velocityY": -0.9706891273098467,
-                    "timestamp": 1.573995492427858
-                },
-                {
-                    "x": 5.636524773547887,
-                    "y": 1.4770268115818306,
-                    "heading": -0.7348631463149735,
-                    "angularVelocity": -0.17853941822989652,
-                    "velocityX": -2.603880913686534,
-                    "velocityY": -0.33745121782018417,
-                    "timestamp": 1.652501105858129
-                },
-                {
-                    "x": 5.451777868175388,
-                    "y": 1.4973120784666072,
-                    "heading": -0.7197568228935092,
-                    "angularVelocity": 0.19242348109134802,
-                    "velocityX": -2.353295481686686,
-                    "velocityY": 0.25839256581051945,
-                    "timestamp": 1.7310067192883998
-                },
-                {
-                    "x": 5.309771976878378,
-                    "y": 1.5457401851903851,
-                    "heading": -0.687868735789328,
-                    "angularVelocity": 0.4061886241103559,
-                    "velocityX": -1.808862896449324,
-                    "velocityY": 0.6168744451222204,
-                    "timestamp": 1.8095123327186706
-                },
-                {
-                    "x": 5.2191255322417085,
-                    "y": 1.592492981200916,
-                    "heading": -0.6576952711638115,
-                    "angularVelocity": 0.38434786134518467,
-                    "velocityX": -1.154649211386418,
-                    "velocityY": 0.5955344333696231,
-                    "timestamp": 1.8880179461489415
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "angularVelocity": 0.22540134890517805,
-                    "velocityX": -0.5493305555788384,
-                    "velocityY": 0.35038282738234927,
-                    "timestamp": 1.9665235595792123
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "angularVelocity": 0,
-                    "velocityX": 6.617232561290069e-35,
-                    "velocityY": 1.0472745338276337e-32,
-                    "timestamp": 2.045029173009483
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C2 To C1": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 9
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": -3.817834980565894e-40,
-                    "angularVelocity": -1.9855221195343998e-39,
-                    "velocityX": -9.621145750064654e-40,
-                    "velocityY": 1.4575297691012433e-39,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.880500005575846,
-                    "heading": 3.479114156010015e-22,
-                    "angularVelocity": 3.4766412561552746e-21,
-                    "velocityX": 2.7068848151306367e-33,
-                    "velocityY": 0.8344065481228213,
-                    "timestamp": 0.10007112931184182
-                },
-                {
-                    "x": 7.68,
-                    "y": 6.047500013629847,
-                    "heading": 1.081691491825702e-21,
-                    "angularVelocity": 7.332585052389997e-21,
-                    "velocityX": 5.1589068200097806e-33,
-                    "velocityY": 1.6688130652907358,
-                    "timestamp": 0.20014225862368365
-                },
-                {
-                    "x": 7.68,
-                    "y": 6.298000017966616,
-                    "heading": 2.2511586271387426e-21,
-                    "angularVelocity": 1.1686358860616172e-20,
-                    "velocityX": 5.8498916949463956e-33,
-                    "velocityY": 2.503219520548835,
-                    "timestamp": 0.3002133879355255
-                },
-                {
-                    "x": 7.68,
-                    "y": 6.632,
-                    "heading": 4.481673966100434e-21,
-                    "angularVelocity": 2.228929929167172e-20,
-                    "velocityX": 6.576791251337367e-33,
-                    "velocityY": 3.3376257900775013,
-                    "timestamp": 0.4002845172473673
-                },
-                {
-                    "x": 7.68,
-                    "y": 6.9659999820333836,
-                    "heading": 1.4391637904192512e-21,
-                    "angularVelocity": -3.0403475801105514e-20,
-                    "velocityX": 1.0509973010204301e-32,
-                    "velocityY": 3.3376257900775013,
-                    "timestamp": 0.5003556465592092
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.216499986370152,
-                    "heading": 4.7457500532831525e-22,
-                    "angularVelocity": -9.63903176962576e-21,
-                    "velocityX": 7.983324746463204e-33,
-                    "velocityY": 2.503219520548835,
-                    "timestamp": 0.600426775871051
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.383499994424153,
-                    "heading": 1.0909640932435146e-22,
-                    "angularVelocity": -3.652188228166023e-21,
-                    "velocityX": 7.341551448907972e-33,
-                    "velocityY": 1.6688130652907358,
-                    "timestamp": 0.7004979051828928
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": -9.42965601107167e-40,
-                    "angularVelocity": -1.09018866193586e-21,
-                    "velocityX": 7.44470103227488e-33,
-                    "velocityY": 0.8344065481228213,
-                    "timestamp": 0.8005690344947346
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 8.249839679439821e-43,
-                    "angularVelocity": 9.431229878048832e-39,
-                    "velocityX": 1.012759498983901e-41,
-                    "velocityY": 5.142787784847508e-39,
-                    "timestamp": 0.9006401638065764
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "C2 To C3": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 9
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": -5.382102664363315e-41,
-                    "angularVelocity": -2.8652892976895567e-45,
-                    "velocityX": 1.592735858156421e-40,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.713499994424153,
-                    "heading": 1.659495816830368e-21,
-                    "angularVelocity": 1.658316243120447e-20,
-                    "velocityX": 2.780383744489643e-33,
-                    "velocityY": -0.8344065481228213,
-                    "timestamp": 0.10007112931184184
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.546499986370153,
-                    "heading": 4.9512387638524226e-21,
-                    "angularVelocity": 3.289403167370527e-20,
-                    "velocityX": 3.618624574631574e-33,
-                    "velocityY": -1.668813065290736,
-                    "timestamp": 0.20014225862368368
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.295999982033384,
-                    "heading": 9.801760970151702e-21,
-                    "angularVelocity": 4.8470744390330394e-20,
-                    "velocityX": 3.439292531132709e-33,
-                    "velocityY": -2.503219520548835,
-                    "timestamp": 0.30021338793552554
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.962,
-                    "heading": 1.506109654033251e-20,
-                    "angularVelocity": 5.255597191401202e-20,
-                    "velocityX": 4.639675238422859e-33,
-                    "velocityY": -3.3376257900775017,
-                    "timestamp": 0.40028451724736736
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.628000017966616,
-                    "heading": 5.3586032763760104e-21,
-                    "angularVelocity": -9.69559666912617e-20,
-                    "velocityX": 4.935439546802769e-33,
-                    "velocityY": -3.3376257900775017,
-                    "timestamp": 0.5003556465592092
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.377500013629846,
-                    "heading": 1.8760590350448333e-21,
-                    "angularVelocity": -3.480068842301423e-20,
-                    "velocityX": 4.701730454770122e-33,
-                    "velocityY": -2.5032195205488352,
-                    "timestamp": 0.600426775871051
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.210500005575846,
-                    "heading": 3.168722428707802e-22,
-                    "angularVelocity": -1.5580785203142817e-20,
-                    "velocityX": 3.49539235617723e-33,
-                    "velocityY": -1.668813065290736,
-                    "timestamp": 0.7004979051828928
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 5.881496870004905e-41,
-                    "angularVelocity": -3.1664700918333573e-21,
-                    "velocityX": 3.364378386644386e-33,
-                    "velocityY": -0.8344065481228213,
-                    "timestamp": 0.8005690344947346
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 9.145026397391945e-42,
-                    "angularVelocity": -4.963805231001231e-40,
-                    "velocityX": -4.870998572760777e-42,
-                    "velocityY": 3.489748854003666e-39,
-                    "timestamp": 0.9006401638065764
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "C2 To C4": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 14
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "angularVelocity": 7.067561628822357e-40,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.720554627994461,
-                    "heading": -3.8243496398495774e-19,
-                    "angularVelocity": -3.994077070459528e-18,
-                    "velocityX": -4.592295404090674e-34,
-                    "velocityY": -0.7983807350946677,
-                    "timestamp": 0.09575052183150977
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.567663887017311,
-                    "heading": -1.1460790562861162e-18,
-                    "angularVelocity": -7.975351749671857e-18,
-                    "velocityX": 3.216033275420413e-35,
-                    "velocityY": -1.5967614385035829,
-                    "timestamp": 0.19150104366301954
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.3383277831364,
-                    "heading": -2.289011643788579e-18,
-                    "angularVelocity": -1.1936567706665239e-17,
-                    "velocityX": -6.99395718741277e-36,
-                    "velocityY": -2.3951420785410233,
-                    "timestamp": 0.2872515654945293
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.032546334555254,
-                    "heading": -3.801510097022925e-18,
-                    "angularVelocity": -1.5796242396621782e-17,
-                    "velocityX": -1.9389720200265424e-34,
-                    "velocityY": -3.1935225284643693,
-                    "timestamp": 0.3830020873260391
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.670327800733179,
-                    "heading": -7.244345840319028e-19,
-                    "angularVelocity": 3.2136383741131935e-17,
-                    "velocityX": -8.432373407035536e-35,
-                    "velocityY": -3.7829405719528495,
-                    "timestamp": 0.47875260915754886
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.308109266911059,
-                    "heading": -3.757861819981246e-18,
-                    "angularVelocity": -3.168052954622205e-17,
-                    "velocityX": -1.6004910806259417e-34,
-                    "velocityY": -3.782940571953311,
-                    "timestamp": 0.5745031309890586
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.9458907330889397,
-                    "heading": -5.3976563030855884e-18,
-                    "angularVelocity": -1.712569761227889e-17,
-                    "velocityX": -2.8603575042124878e-33,
-                    "velocityY": -3.782940571953311,
-                    "timestamp": 0.6702536528205684
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.5836721992668203,
-                    "heading": -4.35645304230582e-18,
-                    "angularVelocity": 1.0874126227865062e-17,
-                    "velocityX": -1.9073271103237224e-34,
-                    "velocityY": -3.782940571953311,
-                    "timestamp": 0.7660041746520782
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.221453665444745,
-                    "heading": 3.661990694649205e-18,
-                    "angularVelocity": 8.37430813282138e-17,
-                    "velocityX": -3.48109229998139e-33,
-                    "velocityY": -3.7829405719528495,
-                    "timestamp": 0.8617546964835879
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.915672216863599,
-                    "heading": 2.222559506115206e-18,
-                    "angularVelocity": -1.503314198751682e-17,
-                    "velocityX": 7.233727398507661e-34,
-                    "velocityY": -3.1935225284643693,
-                    "timestamp": 0.9575052183150977
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.686336112982689,
-                    "heading": 1.1174854461234148e-18,
-                    "angularVelocity": -1.1541180553049724e-17,
-                    "velocityX": 7.218959232022663e-34,
-                    "velocityY": -2.3951420785410233,
-                    "timestamp": 1.0532557401466074
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.5334453720055388,
-                    "heading": 3.7383227049768543e-19,
-                    "angularVelocity": -7.766570474567622e-18,
-                    "velocityX": 1.7872274250399634e-33,
-                    "velocityY": -1.5967614385035829,
-                    "timestamp": 1.1490062619781172
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "angularVelocity": -3.904232200157263e-18,
-                    "velocityX": 6.089132400706592e-33,
-                    "velocityY": -0.7983807350946677,
-                    "timestamp": 1.244756783809627
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 3.5763678522980893e-43,
-                    "velocityY": -5.882608067207678e-38,
-                    "timestamp": 1.3405073056411367
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "C2 To C5": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 18
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -1.2882449649302714e-32,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.715155846048237,
-                    "heading": -2.6675793503802873e-18,
-                    "angularVelocity": -2.692513973443352e-17,
-                    "velocityX": 2.411941793716939e-19,
-                    "velocityY": -0.8260917454276263,
-                    "timestamp": 0.09907392781100396
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.55146754125807,
-                    "heading": -8.002991694377614e-18,
-                    "angularVelocity": -5.385283961043983e-17,
-                    "velocityX": 4.822926900034719e-19,
-                    "velocityY": -1.6521834594306495,
-                    "timestamp": 0.19814785562200793
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.305935091856216,
-                    "heading": -1.6007101079545214e-17,
-                    "angularVelocity": -8.07892607262788e-17,
-                    "velocityX": 7.23199861883135e-19,
-                    "velocityY": -2.478275110584479,
-                    "timestamp": 0.29722178343301187
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.978558516522808,
-                    "heading": -2.6680861930648114e-17,
-                    "angularVelocity": -1.0773531530269989e-16,
-                    "velocityX": 9.635330187135107e-19,
-                    "velocityY": -3.3043665731908765,
-                    "timestamp": 0.39629571124401586
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.603767735073314,
-                    "heading": -3.033567109313287e-17,
-                    "angularVelocity": -3.688971703490728e-17,
-                    "velocityX": 1.1102275506466142e-18,
-                    "velocityY": -3.782940575087071,
-                    "timestamp": 0.49536963905501985
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.228976953623796,
-                    "heading": -3.5529792774719166e-17,
-                    "angularVelocity": -5.2426726146536295e-17,
-                    "velocityX": 1.1087814850277944e-18,
-                    "velocityY": -3.7829405750873115,
-                    "timestamp": 0.5944435668660238
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.8541861721742774,
-                    "heading": -4.8775545061581196e-17,
-                    "angularVelocity": -1.3369564101798844e-16,
-                    "velocityX": 1.1066129122730268e-18,
-                    "velocityY": -3.782940575087312,
-                    "timestamp": 0.6935174946770278
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.4793953907247595,
-                    "heading": -3.3383639047364345e-17,
-                    "angularVelocity": 1.5535778538404392e-16,
-                    "velocityX": 1.113993391664377e-18,
-                    "velocityY": -3.782940575087312,
-                    "timestamp": 0.7925914224880318
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.104604609275241,
-                    "heading": -4.339521377299963e-18,
-                    "angularVelocity": 2.9315601300747485e-16,
-                    "velocityX": 1.114171335036007e-18,
-                    "velocityY": -3.782940575087312,
-                    "timestamp": 0.8916653502990358
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.7298138278257227,
-                    "heading": 1.3174711336089981e-17,
-                    "angularVelocity": 1.7677943229992181e-16,
-                    "velocityX": 1.1158489583207238e-18,
-                    "velocityY": -3.782940575087312,
-                    "timestamp": 0.9907392781100398
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.3550230463762043,
-                    "heading": 2.965421117619106e-17,
-                    "angularVelocity": 1.6633538413393434e-16,
-                    "velocityX": 1.1181357718618657e-18,
-                    "velocityY": -3.782940575087312,
-                    "timestamp": 1.0898132059210437
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.9802322649266861,
-                    "heading": 2.902288280090785e-17,
-                    "angularVelocity": -6.372295813531433e-18,
-                    "velocityX": 1.121394742098521e-18,
-                    "velocityY": -3.7829405750873115,
-                    "timestamp": 1.1888871337320477
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.605441483477192,
-                    "heading": 2.667314494747493e-17,
-                    "angularVelocity": -2.3717015216307205e-17,
-                    "velocityX": 1.1050727300415562e-18,
-                    "velocityY": -3.782940575087071,
-                    "timestamp": 1.2879610615430517
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.2780649081437845,
-                    "heading": 1.6004201006274906e-17,
-                    "angularVelocity": -1.0768669595616384e-16,
-                    "velocityX": 9.663666503471993e-19,
-                    "velocityY": -3.304366573190877,
-                    "timestamp": 1.3870349893540557
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.0325324587419298,
-                    "heading": 8.002125258512226e-18,
-                    "angularVelocity": -8.076873426351199e-17,
-                    "velocityX": 7.247750412790524e-19,
-                    "velocityY": -2.478275110584479,
-                    "timestamp": 1.4861089171650597
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.868844153951763,
-                    "heading": 2.6673738748743342e-18,
-                    "angularVelocity": -5.384616822543564e-17,
-                    "velocityX": 4.831833763651305e-19,
-                    "velocityY": -1.6521834594306495,
-                    "timestamp": 1.5851828449760637
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "angularVelocity": -2.6923065773077948e-17,
-                    "velocityX": 2.415916928361511e-19,
-                    "velocityY": -0.8260917454276263,
-                    "timestamp": 1.6842567727870676
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": -1.1487176577652646e-33,
-                    "velocityY": 0,
-                    "timestamp": 1.7833307005980716
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "C3 To W1": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 16
-                },
-                {
-                    "x": 3.82761549949646,
-                    "y": 5.887885093688965,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 12
-                },
-                {
-                    "x": 2.6227097511291504,
-                    "y": 6.303369998931885,
-                    "heading": 0.7,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 7
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "angularVelocity": -6.709404579113994e-34,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.616979036560275,
-                    "y": 4.122277530863763,
-                    "heading": 0.08798648745533431,
-                    "angularVelocity": 0.9786973137324638,
-                    "velocityX": -0.7009990898727472,
-                    "velocityY": -0.05252929161611988,
-                    "timestamp": 0.08990163375413819
-                },
-                {
-                    "x": 7.489636713382173,
-                    "y": 4.1124897070611155,
-                    "heading": 0.24774232666841967,
-                    "angularVelocity": 1.7770070747544424,
-                    "velocityX": -1.4164628367750984,
-                    "velocityY": -0.10887259100780497,
-                    "timestamp": 0.17980326750827638
-                },
-                {
-                    "x": 7.296297558859626,
-                    "y": 4.097684850959325,
-                    "heading": 0.45307882023210666,
-                    "angularVelocity": 2.2840129260079944,
-                    "velocityX": -2.150563304013888,
-                    "velocityY": -0.16467838773963095,
-                    "timestamp": 0.2697049012624146
-                },
-                {
-                    "x": 7.035802443968742,
-                    "y": 4.077851050872189,
-                    "heading": 0.6494071349161524,
-                    "angularVelocity": 2.183812534719467,
-                    "velocityX": -2.8975570744718895,
-                    "velocityY": -0.22061668135394896,
-                    "timestamp": 0.35960653501655276
-                },
-                {
-                    "x": 6.7206589840239435,
-                    "y": 4.046579170766799,
-                    "heading": 0.7075219081795038,
-                    "angularVelocity": 0.6464262198202391,
-                    "velocityX": -3.505425283000413,
-                    "velocityY": -0.34784551514284123,
-                    "timestamp": 0.44950816877069094
-                },
-                {
-                    "x": 6.381493052293109,
-                    "y": 4.021491444364005,
-                    "heading": 0.7075219804762528,
-                    "angularVelocity": 8.041761424949666e-7,
-                    "velocityX": -3.7726336838147025,
-                    "velocityY": -0.27905751380892024,
-                    "timestamp": 0.5394098025248292
-                },
-                {
-                    "x": 6.042319780035644,
-                    "y": 3.9965031541240665,
-                    "heading": 0.7075220527697934,
-                    "angularVelocity": 8.041404542109454e-7,
-                    "velocityX": -3.7727153344624633,
-                    "velocityY": -0.27795145868295346,
-                    "timestamp": 0.6293114362789674
-                },
-                {
-                    "x": 5.7039977727154,
-                    "y": 4.0311606356057235,
-                    "heading": 0.7075221294336903,
-                    "angularVelocity": 8.52753100011537e-7,
-                    "velocityX": -3.7632464861037187,
-                    "velocityY": 0.3855044678769589,
-                    "timestamp": 0.7192130700331055
-                },
-                {
-                    "x": 5.379151739194528,
-                    "y": 4.1318481159369504,
-                    "heading": 0.707522232494757,
-                    "angularVelocity": 0.0000011463759035114744,
-                    "velocityX": -3.6133496128586025,
-                    "velocityY": 1.1199738661767524,
-                    "timestamp": 0.8091147037872437
-                },
-                {
-                    "x": 5.0805369696371825,
-                    "y": 4.294612195205207,
-                    "heading": 0.7075222695179495,
-                    "angularVelocity": 4.1181890719021464e-7,
-                    "velocityX": -3.3215722238596133,
-                    "velocityY": 1.8104685362379804,
-                    "timestamp": 0.8990163375413819
-                },
-                {
-                    "x": 4.819878772182475,
-                    "y": 4.513061803267287,
-                    "heading": 0.7075222309589103,
-                    "angularVelocity": -4.2890254069180694e-7,
-                    "velocityX": -2.899371085597298,
-                    "velocityY": 2.429873617864304,
-                    "timestamp": 0.9889179712955201
-                },
-                {
-                    "x": 4.607411800528633,
-                    "y": 4.7786191711792085,
-                    "heading": 0.7075221980431953,
-                    "angularVelocity": -3.6613033230177587e-7,
-                    "velocityX": -2.363327147478702,
-                    "velocityY": 2.953865873429686,
-                    "timestamp": 1.0788196050496583
-                },
-                {
-                    "x": 4.4192729102850965,
-                    "y": 5.061932933767741,
-                    "heading": 0.7075221661049191,
-                    "angularVelocity": -3.552580165687493e-7,
-                    "velocityX": -2.0927193687943375,
-                    "velocityY": 3.151375016869355,
-                    "timestamp": 1.1687212388037966
-                },
-                {
-                    "x": 4.23999192605832,
-                    "y": 5.350933382498888,
-                    "heading": 0.7075221408571981,
-                    "angularVelocity": -2.808371773455553e-7,
-                    "velocityX": -1.9941905028900322,
-                    "velocityY": 3.2146295530233306,
-                    "timestamp": 1.258622872557935
-                },
-                {
-                    "x": 4.059747399496467,
-                    "y": 5.639333870205677,
-                    "heading": 0.7075221155890865,
-                    "angularVelocity": -2.8106398720718004e-7,
-                    "velocityX": -2.0049082428773954,
-                    "velocityY": 3.2079560255325883,
-                    "timestamp": 1.3485245063120732
-                },
-                {
-                    "x": 3.82761549949646,
-                    "y": 5.887885093688965,
-                    "heading": 0.7075220884594695,
-                    "angularVelocity": -3.017700109769218e-7,
-                    "velocityX": -2.5820654231362323,
-                    "velocityY": 2.764701964849852,
-                    "timestamp": 1.4384261400662115
-                },
-                {
-                    "x": 3.6506709942338365,
-                    "y": 6.0327931031008335,
-                    "heading": 0.7075220641283702,
-                    "angularVelocity": -4.024467762457879e-7,
-                    "velocityX": -2.92673770558828,
-                    "velocityY": 2.3968403786158894,
-                    "timestamp": 1.4988840707255708
-                },
-                {
-                    "x": 3.4560308994306905,
-                    "y": 6.152888579058325,
-                    "heading": 0.7075220417785688,
-                    "angularVelocity": -3.696752622643165e-7,
-                    "velocityX": -3.2194303159267204,
-                    "velocityY": 1.9864304756722417,
-                    "timestamp": 1.5593420013849302
-                },
-                {
-                    "x": 3.2531901663020784,
-                    "y": 6.258545225655943,
-                    "heading": 0.7075220198614292,
-                    "angularVelocity": -3.625188534499236e-7,
-                    "velocityX": -3.3550723770436006,
-                    "velocityY": 1.7476060699673126,
-                    "timestamp": 1.6197999320442895
-                },
-                {
-                    "x": 3.0503686595470367,
-                    "y": 6.36423877502023,
-                    "heading": 0.7075219979437177,
-                    "angularVelocity": -3.625283082596746e-7,
-                    "velocityX": -3.354754364616084,
-                    "velocityY": 1.7482164574835888,
-                    "timestamp": 1.6802578627036489
-                },
-                {
-                    "x": 2.862919086961248,
-                    "y": 6.462612745515211,
-                    "heading": 0.6632456903097049,
-                    "angularVelocity": -0.7323490425675934,
-                    "velocityX": -3.1004960067524197,
-                    "velocityY": 1.6271474961510723,
-                    "timestamp": 1.7407157933630082
-                },
-                {
-                    "x": 2.701858233172079,
-                    "y": 6.5469153850893225,
-                    "heading": 0.5979529513983839,
-                    "angularVelocity": -1.0799697938593102,
-                    "velocityX": -2.6640153249158556,
-                    "velocityY": 1.394401671618308,
-                    "timestamp": 1.8011737240223675
-                },
-                {
-                    "x": 2.567771439572364,
-                    "y": 6.617045170730925,
-                    "heading": 0.5348900965776917,
-                    "angularVelocity": -1.0430865584205178,
-                    "velocityX": -2.2178528463909877,
-                    "velocityY": 1.1599766131037368,
-                    "timestamp": 1.8616316546817269
-                },
-                {
-                    "x": 2.4605956436791856,
-                    "y": 6.67307612750581,
-                    "heading": 0.4804771381393031,
-                    "angularVelocity": -0.9000135771262476,
-                    "velocityX": -1.7727334482731019,
-                    "velocityY": 0.9267759607999744,
-                    "timestamp": 1.9220895853410862
-                },
-                {
-                    "x": 2.380269344431837,
-                    "y": 6.715058292556038,
-                    "heading": 0.43766723065506036,
-                    "angularVelocity": -0.7080941576620494,
-                    "velocityX": -1.3286313039718116,
-                    "velocityY": 0.694402944201169,
-                    "timestamp": 1.9825475160004455
-                },
-                {
-                    "x": 2.3267482247734668,
-                    "y": 6.743025081240933,
-                    "heading": 0.4081449342684466,
-                    "angularVelocity": -0.48831139380108807,
-                    "velocityX": -0.8852621827225426,
-                    "velocityY": 0.462582631920201,
-                    "timestamp": 2.0430054466598047
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "angularVelocity": -0.2505036825320822,
-                    "velocityX": -0.4424270642703138,
-                    "velocityY": 0.23115112619031306,
-                    "timestamp": 2.103463377319164
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -2.9211624967517807e-31,
-                    "timestamp": 2.1639213079785233
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C3 To W2": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 20
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "angularVelocity": -8.248091288877123e-33,
-                    "velocityX": -1.7499589770527321e-34,
-                    "velocityY": 1.243057151683689e-35,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.599512207946833,
-                    "y": 4.126338330595212,
-                    "heading": -0.05672955044148203,
-                    "angularVelocity": -0.5720722032058021,
-                    "velocityX": -0.8116550928518662,
-                    "velocityY": -0.0066724074357217035,
-                    "timestamp": 0.09916501819801522
-                },
-                {
-                    "x": 7.438184902121127,
-                    "y": 4.125091465076226,
-                    "heading": -0.16291927611759752,
-                    "angularVelocity": -1.070838563898341,
-                    "velocityX": -1.6268570183042166,
-                    "velocityY": -0.01257364281924666,
-                    "timestamp": 0.19833003639603045
-                },
-                {
-                    "x": 7.19545269041534,
-                    "y": 4.123364615129754,
-                    "heading": -0.30423484322627714,
-                    "angularVelocity": -1.4250546178138856,
-                    "velocityX": -2.4477604715515007,
-                    "velocityY": -0.017413902380612675,
-                    "timestamp": 0.2974950545940457
-                },
-                {
-                    "x": 6.870785634071806,
-                    "y": 4.1216134281783505,
-                    "heading": -0.4343749203987102,
-                    "angularVelocity": -1.3123587282822462,
-                    "velocityX": -3.274007933878773,
-                    "velocityY": -0.01765932163604032,
-                    "timestamp": 0.3966600727920609
-                },
-                {
-                    "x": 6.495672194218458,
-                    "y": 4.117557524267507,
-                    "heading": -0.4343749552288927,
-                    "angularVelocity": -3.5123456983963544e-7,
-                    "velocityX": -3.782719417287976,
-                    "velocityY": -0.040900551268437134,
-                    "timestamp": 0.4958250909900761
-                },
-                {
-                    "x": 6.120558752684864,
-                    "y": 4.113501775750749,
-                    "heading": -0.43437499005887026,
-                    "angularVelocity": -3.512325030859258e-7,
-                    "velocityX": -3.782719434231919,
-                    "velocityY": -0.04089898424321494,
-                    "timestamp": 0.5949901091880914
-                },
-                {
-                    "x": 5.747019040696225,
-                    "y": 4.148065138700183,
-                    "heading": -0.43437502656757854,
-                    "angularVelocity": -3.681611615448704e-7,
-                    "velocityX": -3.766849628794958,
-                    "velocityY": 0.3485439076955189,
-                    "timestamp": 0.6941551273861066
-                },
-                {
-                    "x": 5.389911454629062,
-                    "y": 4.262959433025878,
-                    "heading": -0.434375162279621,
-                    "angularVelocity": -0.0000013685475476133038,
-                    "velocityX": -3.601144764115114,
-                    "velocityY": 1.1586171859140066,
-                    "timestamp": 0.7933201455841219
-                },
-                {
-                    "x": 5.045913588511523,
-                    "y": 4.412598985545015,
-                    "heading": -0.4343752985502662,
-                    "angularVelocity": -0.0000013741806101348962,
-                    "velocityX": -3.468943709873945,
-                    "velocityY": 1.5089953618556682,
-                    "timestamp": 0.8924851637821372
-                },
-                {
-                    "x": 4.713243052034498,
-                    "y": 4.585968073714987,
-                    "heading": -0.43437543856751193,
-                    "angularVelocity": -0.0000014119620841874337,
-                    "velocityX": -3.354716638207433,
-                    "velocityY": 1.7482887748155678,
-                    "timestamp": 0.9916501819801524
-                },
-                {
-                    "x": 4.426185676495707,
-                    "y": 4.82747296096862,
-                    "heading": -0.4343754384530987,
-                    "angularVelocity": 1.153766536466057e-9,
-                    "velocityX": -2.894744343873237,
-                    "velocityY": 2.4353838847827403,
-                    "timestamp": 1.0908152001781677
-                },
-                {
-                    "x": 4.09351517988242,
-                    "y": 5.000842166204109,
-                    "heading": -0.4343753660253311,
-                    "angularVelocity": 7.303761838407394e-7,
-                    "velocityX": -3.354716236213479,
-                    "velocityY": 1.748289955327798,
-                    "timestamp": 1.189980218376183
-                },
-                {
-                    "x": 3.752063829141965,
-                    "y": 5.156204697333757,
-                    "heading": -0.43437529472941433,
-                    "angularVelocity": 7.189623727917814e-7,
-                    "velocityX": -3.4432641363372296,
-                    "velocityY": 1.5667070298864525,
-                    "timestamp": 1.2891452365741982
-                },
-                {
-                    "x": 3.4085469650969755,
-                    "y": 5.306945220863998,
-                    "heading": -0.43437522535251666,
-                    "angularVelocity": 6.996105981774619e-7,
-                    "velocityX": -3.46409318817495,
-                    "velocityY": 1.5200977750968405,
-                    "timestamp": 1.3883102547722135
-                },
-                {
-                    "x": 3.0417944650952866,
-                    "y": 5.3858068126004595,
-                    "heading": -0.43437520041032157,
-                    "angularVelocity": 2.5152211488411685e-7,
-                    "velocityX": -3.6984060172242224,
-                    "velocityY": 0.7952561615930772,
-                    "timestamp": 1.4874752729702287
-                },
-                {
-                    "x": 2.7246409461958954,
-                    "y": 5.456187357907523,
-                    "heading": -0.2777696543737322,
-                    "angularVelocity": 1.5792418423589194,
-                    "velocityX": -3.1982399102281316,
-                    "velocityY": 0.7097315826285274,
-                    "timestamp": 1.586640291168244
-                },
-                {
-                    "x": 2.4871805723397817,
-                    "y": 5.508279050924135,
-                    "heading": -0.14311381126711797,
-                    "angularVelocity": 1.3578966207390795,
-                    "velocityX": -2.394598197742947,
-                    "velocityY": 0.5253031155865293,
-                    "timestamp": 1.6858053093662593
-                },
-                {
-                    "x": 2.3290265259060328,
-                    "y": 5.5428007138640085,
-                    "heading": -0.04864902301750531,
-                    "angularVelocity": 0.9526019353012469,
-                    "velocityX": -1.594857231992267,
-                    "velocityY": 0.3481233964071807,
-                    "timestamp": 1.7849703275642745
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": -2.5681051863102055e-32,
-                    "angularVelocity": 0.490586538494469,
-                    "velocityX": -0.7969193909512572,
-                    "velocityY": 0.1734410626703791,
-                    "timestamp": 1.8841353457622898
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 1.14717044306562e-31,
-                    "velocityX": 0,
-                    "velocityY": 5.846493116157662e-32,
-                    "timestamp": 1.983300363960305
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C3 To W3": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 19
-                },
-                {
-                    "x": 2.7473552227020264,
-                    "y": 6.261821746826172,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 12
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "angularVelocity": 5.58164160972529e-32,
-                    "velocityX": 1.1273756142039836e-32,
-                    "velocityY": 2.4507602066878393e-32,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.588395006557631,
-                    "y": 4.128528104169273,
-                    "heading": -0.06094869084906696,
-                    "angularVelocity": -0.5766457987157873,
-                    "velocityX": -0.8666902254019087,
-                    "velocityY": 0.01445765014696692,
-                    "timestamp": 0.10569519622756653
-                },
-                {
-                    "x": 7.4046836501246664,
-                    "y": 4.131675342578391,
-                    "heading": -0.17161815397097052,
-                    "angularVelocity": -1.0470623743734504,
-                    "velocityX": -1.7381239922903071,
-                    "velocityY": 0.029776551077511915,
-                    "timestamp": 0.21139039245513305
-                },
-                {
-                    "x": 7.128148507793846,
-                    "y": 4.136585421276787,
-                    "heading": -0.30937735862178317,
-                    "angularVelocity": -1.3033629679272352,
-                    "velocityX": -2.6163454177749763,
-                    "velocityY": 0.0464550790730766,
-                    "timestamp": 0.3170855886826996
-                },
-                {
-                    "x": 6.759745872373483,
-                    "y": 4.143898882898015,
-                    "heading": -0.3925495634756188,
-                    "angularVelocity": -0.7869061965196801,
-                    "velocityX": -3.485519196418107,
-                    "velocityY": 0.06919388848555069,
-                    "timestamp": 0.4227807849102661
-                },
-                {
-                    "x": 6.3599483672427946,
-                    "y": 4.149634642956697,
-                    "heading": -0.39254959749689916,
-                    "angularVelocity": -3.2188104690533836e-7,
-                    "velocityX": -3.782551330619671,
-                    "velocityY": 0.05426698907234567,
-                    "timestamp": 0.5284759811378327
-                },
-                {
-                    "x": 5.960150865134662,
-                    "y": 4.155370613693523,
-                    "heading": -0.39254963151811834,
-                    "angularVelocity": -3.2188046698591794e-7,
-                    "velocityX": -3.7825513020227493,
-                    "velocityY": 0.05426898233365955,
-                    "timestamp": 0.6341711773653992
-                },
-                {
-                    "x": 5.56430885680334,
-                    "y": 4.211762521698523,
-                    "heading": -0.3925496691964927,
-                    "angularVelocity": -3.5648142721372326e-7,
-                    "velocityX": -3.745127711188092,
-                    "velocityY": 0.5335333110464675,
-                    "timestamp": 0.7398663735929657
-                },
-                {
-                    "x": 5.196451370697568,
-                    "y": 4.356905683947299,
-                    "heading": -0.407192768864793,
-                    "angularVelocity": -0.13854082485237101,
-                    "velocityX": -3.4803614472105,
-                    "velocityY": 1.3732238306864724,
-                    "timestamp": 0.8455615698205322
-                },
-                {
-                    "x": 4.853119399386178,
-                    "y": 4.529141900436451,
-                    "heading": -0.46036689242459117,
-                    "angularVelocity": -0.5030893120753747,
-                    "velocityX": -3.2483214333807724,
-                    "velocityY": 1.6295557663597022,
-                    "timestamp": 0.9512567660480987
-                },
-                {
-                    "x": 4.514017884763102,
-                    "y": 4.740993233583884,
-                    "heading": -0.4603674462778229,
-                    "angularVelocity": -0.000005240098429403888,
-                    "velocityX": -3.208296372268186,
-                    "velocityY": 2.0043610372916896,
-                    "timestamp": 1.0569519622756653
-                },
-                {
-                    "x": 4.21750261375463,
-                    "y": 5.009227519029098,
-                    "heading": -0.46036738913342323,
-                    "angularVelocity": 5.406527608893175e-7,
-                    "velocityX": -2.8053807702864813,
-                    "velocityY": 2.537809616888297,
-                    "timestamp": 1.1626471585032319
-                },
-                {
-                    "x": 3.866969646487487,
-                    "y": 5.201574981603133,
-                    "heading": -0.46036721071098435,
-                    "angularVelocity": 0.0000016880846552508654,
-                    "velocityX": -3.3164512653198517,
-                    "velocityY": 1.8198316426784653,
-                    "timestamp": 1.2683423547307984
-                },
-                {
-                    "x": 3.5045677865422378,
-                    "y": 5.303672838764994,
-                    "heading": -0.3887465759877037,
-                    "angularVelocity": 0.6776148517580518,
-                    "velocityX": -3.4287448519891264,
-                    "velocityY": 0.9659649710289621,
-                    "timestamp": 1.3740375509583649
-                },
-                {
-                    "x": 3.16557460185915,
-                    "y": 5.315860378892371,
-                    "heading": -0.29320626763004903,
-                    "angularVelocity": 0.9039229006392311,
-                    "velocityX": -3.207271444515043,
-                    "velocityY": 0.1153083636945626,
-                    "timestamp": 1.4797327471859314
-                },
-                {
-                    "x": 2.8681536233597122,
-                    "y": 5.2454821587095966,
-                    "heading": -0.23849853505250618,
-                    "angularVelocity": 0.5175990445181124,
-                    "velocityX": -2.8139498209462337,
-                    "velocityY": -0.6658601591622668,
-                    "timestamp": 1.585427943413498
-                },
-                {
-                    "x": 2.618332286986499,
-                    "y": 5.0970676603891505,
-                    "heading": -0.24744806192055374,
-                    "angularVelocity": -0.08467297651615932,
-                    "velocityX": -2.3636016137889215,
-                    "velocityY": -1.4041744905880433,
-                    "timestamp": 1.6911231396410644
-                },
-                {
-                    "x": 2.4325478975720265,
-                    "y": 4.8813111498736355,
-                    "heading": -0.27927376577641677,
-                    "angularVelocity": -0.3011083283987745,
-                    "velocityX": -1.7577373054350616,
-                    "velocityY": -2.0413085761341616,
-                    "timestamp": 1.796818335868631
-                },
-                {
-                    "x": 2.3348070320385017,
-                    "y": 4.635496280912222,
-                    "heading": -0.32770640318337707,
-                    "angularVelocity": -0.4582293153861289,
-                    "velocityX": -0.9247427416008974,
-                    "velocityY": -2.3256957528340565,
-                    "timestamp": 1.9025135320961974
-                },
-                {
-                    "x": 2.305548232287739,
-                    "y": 4.452786448669473,
-                    "heading": -0.3683698320727586,
-                    "angularVelocity": -0.3847235289845278,
-                    "velocityX": -0.2768224176221525,
-                    "velocityY": -1.72864840374928,
-                    "timestamp": 2.008208728323764
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": -0.2046466509288718,
-                    "velocityX": -0.05249275734153,
-                    "velocityY": -0.8778681716972112,
-                    "timestamp": 2.1139039245513307
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": 5.518495272475637e-32,
-                    "velocityX": 0,
-                    "velocityY": 3.7393231194786592e-34,
-                    "timestamp": 2.219599120778897
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C3 To S1": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 16
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.263,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": -7.709535672280055e-33,
-                    "angularVelocity": 3.0776034223095e-32,
-                    "velocityX": -2.0787724059290825e-32,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.653625318689537,
-                    "y": 4.193887618013532,
-                    "heading": -0.05699658004084171,
-                    "angularVelocity": -0.6065329552716214,
-                    "velocityX": -0.28066795214939116,
-                    "velocityY": 0.7117891037630026,
-                    "timestamp": 0.0939711182145365
-                },
-                {
-                    "x": 7.600609429037226,
-                    "y": 4.328102182971276,
-                    "heading": -0.1617251039431606,
-                    "angularVelocity": -1.1144756590341216,
-                    "velocityX": -0.5641721697008566,
-                    "velocityY": 1.4282533560080801,
-                    "timestamp": 0.187942236429073
-                },
-                {
-                    "x": 7.520673672603614,
-                    "y": 4.5302373715153195,
-                    "heading": -0.299017155202882,
-                    "angularVelocity": -1.4610026343017766,
-                    "velocityX": -0.8506417498525306,
-                    "velocityY": 2.151035258328725,
-                    "timestamp": 0.2819133546436095
-                },
-                {
-                    "x": 7.413680530437971,
-                    "y": 4.80084724043498,
-                    "heading": -0.43363333911195256,
-                    "angularVelocity": -1.4325272111984575,
-                    "velocityX": -1.1385747472045327,
-                    "velocityY": 2.8797131933862543,
-                    "timestamp": 0.375884472858146
-                },
-                {
-                    "x": 7.279380160390009,
-                    "y": 5.1284214711267,
-                    "heading": -0.4371879316114584,
-                    "angularVelocity": -0.037826436111898676,
-                    "velocityX": -1.4291664566697349,
-                    "velocityY": 3.4859032957750506,
-                    "timestamp": 0.46985559107268254
-                },
-                {
-                    "x": 7.147763767829931,
-                    "y": 5.458645997615663,
-                    "heading": -0.43718797458387054,
-                    "angularVelocity": -4.5729382513446123e-7,
-                    "velocityX": -1.4006047289934083,
-                    "velocityY": 3.514106597466034,
-                    "timestamp": 0.563826709287219
-                },
-                {
-                    "x": 6.993957305230812,
-                    "y": 5.779137314807096,
-                    "heading": -0.43718801851214967,
-                    "angularVelocity": -4.6746574877776706e-7,
-                    "velocityX": -1.6367418577267254,
-                    "velocityY": 3.410529993478958,
-                    "timestamp": 0.6577978275017555
-                },
-                {
-                    "x": 6.777425503833482,
-                    "y": 6.0610683140345785,
-                    "heading": -0.43718807107725144,
-                    "angularVelocity": -5.593750798607861e-7,
-                    "velocityX": -2.3042377861566687,
-                    "velocityY": 3.000187766030755,
-                    "timestamp": 0.751768945716292
-                },
-                {
-                    "x": 6.507457353311083,
-                    "y": 6.292343710685516,
-                    "heading": -0.4371880918313372,
-                    "angularVelocity": -2.2085600557820128e-7,
-                    "velocityX": -2.87288430372895,
-                    "velocityY": 2.4611327506280603,
-                    "timestamp": 0.8457400639308286
-                },
-                {
-                    "x": 6.1957042493791805,
-                    "y": 6.462993619393473,
-                    "heading": -0.4369141825473297,
-                    "angularVelocity": 0.0029148241418374306,
-                    "velocityX": -3.3175417070186306,
-                    "velocityY": 1.8159825268692036,
-                    "timestamp": 0.9397111821453651
-                },
-                {
-                    "x": 5.885878112264826,
-                    "y": 6.564806359719212,
-                    "heading": -0.3410861416211598,
-                    "angularVelocity": 1.0197605684269313,
-                    "velocityX": -3.297035759508787,
-                    "velocityY": 1.083447151211916,
-                    "timestamp": 1.0336823003599016
-                },
-                {
-                    "x": 5.602663899704114,
-                    "y": 6.605182844765102,
-                    "heading": -0.13545254769192583,
-                    "angularVelocity": 2.1882637754695176,
-                    "velocityX": -3.0138431673669435,
-                    "velocityY": 0.4296690920896744,
-                    "timestamp": 1.127653418574438
-                },
-                {
-                    "x": 5.388992479168701,
-                    "y": 6.621862905384601,
-                    "heading": 0.0527880784098822,
-                    "angularVelocity": 2.0031753338515554,
-                    "velocityX": -2.2737988500636965,
-                    "velocityY": 0.17750199142483714,
-                    "timestamp": 1.2216245367889744
-                },
-                {
-                    "x": 5.246852509457665,
-                    "y": 6.628278045699216,
-                    "heading": 0.19017469868317557,
-                    "angularVelocity": 1.4620089968455965,
-                    "velocityX": -1.5125920858633375,
-                    "velocityY": 0.06826714884852915,
-                    "timestamp": 1.3155956550035108
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.263,
-                    "angularVelocity": 0.7749753615846516,
-                    "velocityX": -0.7539817638001085,
-                    "velocityY": 0.01832429296896703,
-                    "timestamp": 1.4095667732180472
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.263,
-                    "angularVelocity": 6.971645129510918e-32,
-                    "velocityX": 3.634932162806737e-32,
-                    "velocityY": 0,
-                    "timestamp": 1.5035378914325837
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C3 To S2": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 1.358270377512338e-32,
-                    "angularVelocity": -4.2989327725546786e-32,
-                    "velocityX": 9.28706448380456e-34,
-                    "velocityY": 4.1113328098198935e-33,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.585313636391529,
-                    "y": 4.127746947940637,
-                    "heading": -0.08924586799649764,
-                    "angularVelocity": -0.8241960155421162,
-                    "velocityX": -0.8744396280098535,
-                    "velocityY": 0.006898151480975746,
-                    "timestamp": 0.10828233370892486
-                },
-                {
-                    "x": 7.394793136195202,
-                    "y": 4.129317895600586,
-                    "heading": -0.2483761167773939,
-                    "angularVelocity": -1.4695864351119023,
-                    "velocityX": -1.7594790735529262,
-                    "velocityY": 0.014507885138232623,
-                    "timestamp": 0.21656466741784972
-                },
-                {
-                    "x": 7.106938899563738,
-                    "y": 4.131715387581193,
-                    "heading": -0.440143741107867,
-                    "angularVelocity": -1.7709964106053178,
-                    "velocityX": -2.6583674988502586,
-                    "velocityY": 0.022141118486160887,
-                    "timestamp": 0.32484700112677456
-                },
-                {
-                    "x": 6.726884682874397,
-                    "y": 4.136782702937484,
-                    "heading": -0.5152103460374668,
-                    "angularVelocity": -0.6932488648738155,
-                    "velocityX": -3.509845084341914,
-                    "velocityY": 0.04679724921622947,
-                    "timestamp": 0.43312933483569943
-                },
-                {
-                    "x": 6.317264615210477,
-                    "y": 4.138917325587368,
-                    "heading": -0.515210397277194,
-                    "angularVelocity": -4.7320486478177106e-7,
-                    "velocityX": -3.782889171607859,
-                    "velocityY": 0.019713489511795114,
-                    "timestamp": 0.5414116685446243
-                },
-                {
-                    "x": 5.907644548289307,
-                    "y": 4.14105209076814,
-                    "heading": -0.5152104485168384,
-                    "angularVelocity": -4.732041015646636e-7,
-                    "velocityX": -3.7828891647484775,
-                    "velocityY": 0.019714805801191258,
-                    "timestamp": 0.6496940022535491
-                },
-                {
-                    "x": 5.500258277815471,
-                    "y": 4.183825709430579,
-                    "heading": -0.5152105024899405,
-                    "angularVelocity": -4.984479029228616e-7,
-                    "velocityX": -3.762259793632967,
-                    "velocityY": 0.3950193646307994,
-                    "timestamp": 0.7579763359624739
-                },
-                {
-                    "x": 5.114610765355458,
-                    "y": 4.3219164843017905,
-                    "heading": -0.5152109227554116,
-                    "angularVelocity": -0.000003881200715936794,
-                    "velocityX": -3.5614998241234512,
-                    "velocityY": 1.2752844359857862,
-                    "timestamp": 0.8662586696713988
-                },
-                {
-                    "x": 4.790273412388094,
-                    "y": 4.533322369220829,
-                    "heading": -0.5901314422767667,
-                    "angularVelocity": -0.6918997490647923,
-                    "velocityX": -2.9952933397170605,
-                    "velocityY": 1.952358041038544,
-                    "timestamp": 0.9745410033803236
-                },
-                {
-                    "x": 4.547771251602889,
-                    "y": 4.790487049727204,
-                    "heading": -0.7662492650374113,
-                    "angularVelocity": -1.6264686650923976,
-                    "velocityX": -2.239535780943542,
-                    "velocityY": 2.3749458632620883,
-                    "timestamp": 1.0828233370892484
-                },
-                {
-                    "x": 4.394005608234175,
-                    "y": 5.057835455573767,
-                    "heading": -0.8024780004938983,
-                    "angularVelocity": -0.3345766037318139,
-                    "velocityX": -1.4200436775037824,
-                    "velocityY": 2.468993756315091,
-                    "timestamp": 1.1911056707981733
-                },
-                {
-                    "x": 4.299277084870805,
-                    "y": 5.304155354811625,
-                    "heading": -0.5153879845736841,
-                    "angularVelocity": 2.651309831315093,
-                    "velocityX": -0.8748289782709269,
-                    "velocityY": 2.2747930414945974,
-                    "timestamp": 1.2993880045070982
-                },
-                {
-                    "x": 4.268346904630433,
-                    "y": 5.476762087961931,
-                    "heading": -0.20205360473699635,
-                    "angularVelocity": 2.893679597624539,
-                    "velocityX": -0.2856438273995415,
-                    "velocityY": 1.5940433424190128,
-                    "timestamp": 1.4076703382160232
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 2.864290108945775e-30,
-                    "angularVelocity": 1.8659886411401074,
-                    "velocityX": -0.04937928882107947,
-                    "velocityY": 0.7687118404911842,
-                    "timestamp": 1.515952671924948
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": -1.339793214188515e-31,
-                    "velocityX": -1.755749143439243e-31,
-                    "velocityY": 0,
-                    "timestamp": 1.624235005633873
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C3 To S3": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 17
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "angularVelocity": 4.8594393424661863e-32,
-                    "velocityX": 8.070887902586468e-34,
-                    "velocityY": -1.5484627527783442e-32,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.613090966537703,
-                    "y": 4.130415229492922,
-                    "heading": -0.02261774188189381,
-                    "angularVelocity": -0.25176724339812206,
-                    "velocityX": -0.7447915447554241,
-                    "velocityY": 0.038016302406854975,
-                    "timestamp": 0.08983591977315897
-                },
-                {
-                    "x": 7.479193313628693,
-                    "y": 4.137259099498979,
-                    "heading": -0.06454119971556918,
-                    "angularVelocity": -0.46666698504702897,
-                    "velocityX": -1.4904689933291897,
-                    "velocityY": 0.0761818883097586,
-                    "timestamp": 0.17967183954631794
-                },
-                {
-                    "x": 7.278200146895047,
-                    "y": 4.147551727639681,
-                    "heading": -0.12012980810986089,
-                    "angularVelocity": -0.6187793093216658,
-                    "velocityX": -2.237336326507015,
-                    "velocityY": 0.11457141160316474,
-                    "timestamp": 0.2695077593194769
-                },
-                {
-                    "x": 7.010006404514947,
-                    "y": 4.161329433657577,
-                    "heading": -0.1775477987284744,
-                    "angularVelocity": -0.6391429036434741,
-                    "velocityX": -2.985373145364283,
-                    "velocityY": 0.15336522465649807,
-                    "timestamp": 0.35934367909263587
-                },
-                {
-                    "x": 6.675632733992502,
-                    "y": 4.178685588756415,
-                    "heading": -0.19285230682653456,
-                    "angularVelocity": -0.17036067687364617,
-                    "velocityX": -3.7220487235639528,
-                    "velocityY": 0.19319839038333309,
-                    "timestamp": 0.44917959886579484
-                },
-                {
-                    "x": 6.3362226416697665,
-                    "y": 4.195852203162663,
-                    "heading": -0.19285232652882817,
-                    "angularVelocity": -2.193200640663355e-7,
-                    "velocityX": -3.7781111740035236,
-                    "velocityY": 0.1910885362012055,
-                    "timestamp": 0.5390155186389538
-                },
-                {
-                    "x": 5.996812544590127,
-                    "y": 4.213018723498308,
-                    "heading": -0.19285234623213549,
-                    "angularVelocity": -2.1933156264800868e-7,
-                    "velocityX": -3.7781112269558137,
-                    "velocityY": 0.1910874890356692,
-                    "timestamp": 0.6288514384121128
-                },
-                {
-                    "x": 5.65703115405788,
-                    "y": 4.206499194085455,
-                    "heading": -0.1928524148916534,
-                    "angularVelocity": -7.642946895727943e-7,
-                    "velocityX": -3.782244244720454,
-                    "velocityY": -0.07257152185432766,
-                    "timestamp": 0.7186873581852717
-                },
-                {
-                    "x": 5.325195518902672,
-                    "y": 4.133157682156235,
-                    "heading": -0.192852601151229,
-                    "angularVelocity": -0.0000020733763948999505,
-                    "velocityX": -3.6937968241559735,
-                    "velocityY": -0.8163940672507154,
-                    "timestamp": 0.8085232779584307
-                },
-                {
-                    "x": 5.022921580906399,
-                    "y": 3.999719932132448,
-                    "heading": -0.21619143682617745,
-                    "angularVelocity": -0.2597940304978627,
-                    "velocityX": -3.364733602738329,
-                    "velocityY": -1.485349628067107,
-                    "timestamp": 0.8983591977315897
-                },
-                {
-                    "x": 4.76405537852928,
-                    "y": 3.8156128777716756,
-                    "heading": -0.2707600447886353,
-                    "angularVelocity": -0.6074252715762222,
-                    "velocityX": -2.881544520603651,
-                    "velocityY": -2.0493701720293274,
-                    "timestamp": 0.9881951175047486
-                },
-                {
-                    "x": 4.5584097754694755,
-                    "y": 3.590752248351908,
-                    "heading": -0.3464948618413025,
-                    "angularVelocity": -0.8430349156856024,
-                    "velocityX": -2.2891244791432657,
-                    "velocityY": -2.5030147182477354,
-                    "timestamp": 1.0780310372779076
-                },
-                {
-                    "x": 4.420003796304514,
-                    "y": 3.3682412112408735,
-                    "heading": -0.42679529224450924,
-                    "angularVelocity": -0.8938566066393836,
-                    "velocityX": -1.5406529984228516,
-                    "velocityY": -2.476860454851527,
-                    "timestamp": 1.1678669570510665
-                },
-                {
-                    "x": 4.3340794956684014,
-                    "y": 3.187695881574611,
-                    "heading": -0.4942860320434666,
-                    "angularVelocity": -0.7512667535241839,
-                    "velocityX": -0.9564581834687845,
-                    "velocityY": -2.009723172222817,
-                    "timestamp": 1.2577028768242253
-                },
-                {
-                    "x": 4.284833149556787,
-                    "y": 3.063288277698013,
-                    "heading": -0.5416494132844137,
-                    "angularVelocity": -0.5272209752980105,
-                    "velocityX": -0.5481810197605272,
-                    "velocityY": -1.3848314147664058,
-                    "timestamp": 1.3475387965973842
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "angularVelocity": -0.27105624093568903,
-                    "velocityX": -0.24303362855860308,
-                    "velocityY": -0.7044874461979944,
-                    "timestamp": 1.437374716370543
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "angularVelocity": 6.221864662981848e-31,
-                    "velocityX": 0,
-                    "velocityY": 2.6923638006152617e-32,
-                    "timestamp": 1.527210636143702
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C3 To S4": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 12
-                },
-                {
-                    "x": 6.792231559753418,
-                    "y": 1.766543984413147,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 13
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "angularVelocity": 3.185769476987751e-31,
-                    "velocityX": 7.333541525795099e-32,
-                    "velocityY": -1.3517842061565195e-33,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.679078910576486,
-                    "y": 4.080614551713116,
-                    "heading": -0.023748331346269917,
-                    "angularVelocity": -0.3167839884646853,
-                    "velocityX": -0.012286605616975374,
-                    "velocityY": -0.6187452541731726,
-                    "timestamp": 0.07496695606800095
-                },
-                {
-                    "x": 7.6772274783441,
-                    "y": 3.987757014244699,
-                    "heading": -0.06889026621284731,
-                    "angularVelocity": -0.6021577670253255,
-                    "velocityX": -0.024696644088144176,
-                    "velocityY": -1.2386462294692595,
-                    "timestamp": 0.1499339121360019
-                },
-                {
-                    "x": 7.674435217742111,
-                    "y": 3.848312298051719,
-                    "heading": -0.1318457680879735,
-                    "angularVelocity": -0.8397766853177616,
-                    "velocityX": -0.037246551660047825,
-                    "velocityY": -1.8600824083946148,
-                    "timestamp": 0.22490086820400285
-                },
-                {
-                    "x": 7.670687411814683,
-                    "y": 3.6621317365631327,
-                    "heading": -0.20653378464827088,
-                    "angularVelocity": -0.996279167218362,
-                    "velocityX": -0.049992771802326974,
-                    "velocityY": -2.483501681990474,
-                    "timestamp": 0.2998678242720038
-                },
-                {
-                    "x": 7.665952301925262,
-                    "y": 3.4291021726627307,
-                    "heading": -0.27999342706200125,
-                    "angularVelocity": -0.9798936260280143,
-                    "velocityX": -0.06316262707958344,
-                    "velocityY": -3.1084303821676644,
-                    "timestamp": 0.3748347803400048
-                },
-                {
-                    "x": 7.659724245559776,
-                    "y": 3.1517739981457797,
-                    "heading": -0.2973184560258276,
-                    "angularVelocity": -0.23110220652567398,
-                    "velocityX": -0.0830773542389579,
-                    "velocityY": -3.699338869586643,
-                    "timestamp": 0.44980173640800575
-                },
-                {
-                    "x": 7.61848609595722,
-                    "y": 2.8711927485616604,
-                    "heading": -0.2973184850947423,
-                    "angularVelocity": -3.8775636846908307e-7,
-                    "velocityX": -0.5500843540339733,
-                    "velocityY": -3.742732322352917,
-                    "timestamp": 0.5247686924760067
-                },
-                {
-                    "x": 7.5316071002697536,
-                    "y": 2.6012326390018865,
-                    "heading": -0.2973185190564447,
-                    "angularVelocity": -4.530222940715824e-7,
-                    "velocityX": -1.1588972027711093,
-                    "velocityY": -3.6010547008857787,
-                    "timestamp": 0.5997356485440076
-                },
-                {
-                    "x": 7.401459139481823,
-                    "y": 2.3492645367371554,
-                    "heading": -0.29731856454052086,
-                    "angularVelocity": -6.067216612944958e-7,
-                    "velocityX": -1.7360710320141561,
-                    "velocityY": -3.3610555300681284,
-                    "timestamp": 0.6747026046120086
-                },
-                {
-                    "x": 7.231595664022054,
-                    "y": 2.1221680676798096,
-                    "heading": -0.2973186386883636,
-                    "angularVelocity": -9.89073675332688e-7,
-                    "velocityX": -2.265844638345654,
-                    "velocityY": -3.0292875817359826,
-                    "timestamp": 0.7496695606800096
-                },
-                {
-                    "x": 7.026654548839687,
-                    "y": 1.9261437915792998,
-                    "heading": -0.2973188151543491,
-                    "angularVelocity": -0.000002353916908579506,
-                    "velocityX": -2.7337526549226907,
-                    "velocityY": -2.614809062311352,
-                    "timestamp": 0.8246365167480105
-                },
-                {
-                    "x": 6.792231559753418,
-                    "y": 1.766543984413147,
-                    "heading": -0.2973195709317279,
-                    "angularVelocity": -0.00001008147347055336,
-                    "velocityX": -3.127017573897922,
-                    "velocityY": -2.1289354075064555,
-                    "timestamp": 0.8996034728160115
-                },
-                {
-                    "x": 6.610935711613446,
-                    "y": 1.6724691195207,
-                    "heading": -0.2973321063250889,
-                    "angularVelocity": -0.00023216320074468077,
-                    "velocityX": -3.3577106919370485,
-                    "velocityY": -1.7423243992220112,
-                    "timestamp": 0.9535973587619662
-                },
-                {
-                    "x": 6.419882816475647,
-                    "y": 1.600658398177247,
-                    "heading": -0.29770902924967546,
-                    "angularVelocity": -0.006980844552986395,
-                    "velocityX": -3.538417207626563,
-                    "velocityY": -1.329978757508455,
-                    "timestamp": 1.007591244707921
-                },
-                {
-                    "x": 6.223144239006287,
-                    "y": 1.5524565272909876,
-                    "heading": -0.30199019992690734,
-                    "angularVelocity": -0.07928991592723345,
-                    "velocityX": -3.6437195438439516,
-                    "velocityY": -0.892728316211746,
-                    "timestamp": 1.0615851306538757
-                },
-                {
-                    "x": 6.027009837481886,
-                    "y": 1.528287215105613,
-                    "heading": -0.31920772077283077,
-                    "angularVelocity": -0.31887908314582397,
-                    "velocityX": -3.63252983348375,
-                    "velocityY": -0.44763053745692566,
-                    "timestamp": 1.1155790165998305
-                },
-                {
-                    "x": 5.840489459577601,
-                    "y": 1.5253276945335545,
-                    "heading": -0.3622266817701244,
-                    "angularVelocity": -0.7967376350788925,
-                    "velocityX": -3.454472198777908,
-                    "velocityY": -0.05481214252704887,
-                    "timestamp": 1.1695729025457853
-                },
-                {
-                    "x": 5.673855769295343,
-                    "y": 1.5359985529910274,
-                    "heading": -0.41631972448131044,
-                    "angularVelocity": -1.0018364443214895,
-                    "velocityX": -3.086158504113808,
-                    "velocityY": 0.19763086635687102,
-                    "timestamp": 1.22356678849174
-                },
-                {
-                    "x": 5.530708201179271,
-                    "y": 1.5529237053226916,
-                    "heading": -0.47196047387840134,
-                    "angularVelocity": -1.0305009247305976,
-                    "velocityX": -2.651181066303626,
-                    "velocityY": 0.3134642383140704,
-                    "timestamp": 1.2775606744376948
-                },
-                {
-                    "x": 5.411784437145291,
-                    "y": 1.57153818466476,
-                    "heading": -0.5236032237572267,
-                    "angularVelocity": -0.956455512954414,
-                    "velocityX": -2.2025413053807186,
-                    "velocityY": 0.34475161429762524,
-                    "timestamp": 1.3315545603836496
-                },
-                {
-                    "x": 5.3170603467926,
-                    "y": 1.5890782395606275,
-                    "heading": -0.5679992830659408,
-                    "angularVelocity": -0.8222423433861833,
-                    "velocityX": -1.754348454332506,
-                    "velocityY": 0.32485261226475975,
-                    "timestamp": 1.3855484463296044
-                },
-                {
-                    "x": 5.246334967071984,
-                    "y": 1.6037618746917708,
-                    "heading": -0.6030801100679523,
-                    "angularVelocity": -0.6497185077054303,
-                    "velocityX": -1.309877562644932,
-                    "velocityY": 0.2719499601462664,
-                    "timestamp": 1.4395423322755592
-                },
-                {
-                    "x": 5.199384048260516,
-                    "y": 1.6143645569232876,
-                    "heading": -0.6274260701015184,
-                    "angularVelocity": -0.45090216432919045,
-                    "velocityX": -0.8695599138477248,
-                    "velocityY": 0.19636820069091412,
-                    "timestamp": 1.493536218221514
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "angularVelocity": -0.23287692075121041,
-                    "velocityX": -0.4330869662524824,
-                    "velocityY": 0.10437187429616973,
-                    "timestamp": 1.5475301041674687
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "angularVelocity": 0,
-                    "velocityX": 3.9288115092123874e-33,
-                    "velocityY": 2.0888895254257856e-32,
-                    "timestamp": 1.6015239901134235
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C3 To C1": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 14
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "angularVelocity": -2.1749467140472343e-39,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.203445372005539,
-                    "heading": -7.837837605587582e-19,
-                    "angularVelocity": -8.185686568731239e-18,
-                    "velocityX": 1.377912473786286e-33,
-                    "velocityY": 0.7983807350946673,
-                    "timestamp": 0.09575052183150974
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.356336112982689,
-                    "heading": -2.3565356003209568e-18,
-                    "angularVelocity": -1.6425517161035916e-17,
-                    "velocityX": 3.260516050321855e-34,
-                    "velocityY": 1.5967614385035824,
-                    "timestamp": 0.19150104366301948
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.5856722168636,
-                    "heading": -4.728579560129296e-18,
-                    "angularVelocity": -2.4773170043438095e-17,
-                    "velocityX": -1.693775140889528e-33,
-                    "velocityY": 2.3951420785410225,
-                    "timestamp": 0.28725156549452924
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.891453665444746,
-                    "heading": -7.932188825954499e-18,
-                    "angularVelocity": -3.345787785122934e-17,
-                    "velocityX": -9.607950389092107e-34,
-                    "velocityY": 3.1935225284643685,
-                    "timestamp": 0.38300208732603896
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.253672199266821,
-                    "heading": -4.35728250993897e-17,
-                    "angularVelocity": -3.722239377050204e-16,
-                    "velocityX": -3.660377688643743e-33,
-                    "velocityY": 3.7829405719528495,
-                    "timestamp": 0.4787526091575487
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.61589073308894,
-                    "heading": -3.405116375197899e-17,
-                    "angularVelocity": 9.944239639932065e-17,
-                    "velocityX": -5.791062266320423e-33,
-                    "velocityY": 3.782940571953311,
-                    "timestamp": 0.5745031309890585
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.978109266911059,
-                    "heading": 5.876472199762529e-18,
-                    "angularVelocity": 4.169965362904391e-16,
-                    "velocityX": -1.6171107443968654e-32,
-                    "velocityY": 3.782940571953311,
-                    "timestamp": 0.6702536528205683
-                },
-                {
-                    "x": 7.68,
-                    "y": 6.340327800733179,
-                    "heading": 2.547705878984039e-17,
-                    "angularVelocity": 2.047047495424479e-16,
-                    "velocityX": -4.3724642588945734e-33,
-                    "velocityY": 3.782940571953311,
-                    "timestamp": 0.766004174652078
-                },
-                {
-                    "x": 7.68,
-                    "y": 6.702546334555254,
-                    "heading": 7.687233753276096e-18,
-                    "angularVelocity": -1.8579350479017399e-16,
-                    "velocityX": -2.6103711792186515e-33,
-                    "velocityY": 3.7829405719528495,
-                    "timestamp": 0.8617546964835878
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.0083277831364,
-                    "heading": 4.613320516549708e-18,
-                    "angularVelocity": -3.210335754994756e-17,
-                    "velocityX": 1.2273832122580678e-32,
-                    "velocityY": 3.1935225284643685,
-                    "timestamp": 0.9575052183150976
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.237663887017311,
-                    "heading": 2.3068261479777784e-18,
-                    "angularVelocity": -2.4088582761110343e-17,
-                    "velocityX": 8.179933156696061e-33,
-                    "velocityY": 2.3951420785410225,
-                    "timestamp": 1.0532557401466074
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.390554627994461,
-                    "heading": 7.689558058679801e-19,
-                    "angularVelocity": -1.6061221530256843e-17,
-                    "velocityX": 9.843160057308665e-33,
-                    "velocityY": 1.5967614385035824,
-                    "timestamp": 1.1490062619781172
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "angularVelocity": -8.030826271263815e-18,
-                    "velocityX": 3.1854559448557055e-33,
-                    "velocityY": 0.7983807350946673,
-                    "timestamp": 1.244756783809627
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 2.257351446612169e-43,
-                    "velocityY": -1.1946741683702123e-38,
-                    "timestamp": 1.3405073056411367
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "C3 To C2": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 9
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": -3.817834980565894e-40,
-                    "angularVelocity": -1.9855221195343998e-39,
-                    "velocityX": -9.621145750064654e-40,
-                    "velocityY": 1.4575297691012433e-39,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.210500005575846,
-                    "heading": 3.479114156010015e-22,
-                    "angularVelocity": 3.4766412561552746e-21,
-                    "velocityX": 2.7068848151306367e-33,
-                    "velocityY": 0.8344065481228213,
-                    "timestamp": 0.10007112931184182
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.377500013629847,
-                    "heading": 1.081691491825702e-21,
-                    "angularVelocity": 7.332585052389997e-21,
-                    "velocityX": 5.1589068200097806e-33,
-                    "velocityY": 1.6688130652907358,
-                    "timestamp": 0.20014225862368365
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.628000017966616,
-                    "heading": 2.2511586271387426e-21,
-                    "angularVelocity": 1.1686358860616172e-20,
-                    "velocityX": 5.8498916949463956e-33,
-                    "velocityY": 2.503219520548835,
-                    "timestamp": 0.3002133879355255
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.962,
-                    "heading": 4.481673966100434e-21,
-                    "angularVelocity": 2.228929929167172e-20,
-                    "velocityX": 6.576791251337367e-33,
-                    "velocityY": 3.3376257900775013,
-                    "timestamp": 0.4002845172473673
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.295999982033384,
-                    "heading": 1.4391637904192512e-21,
-                    "angularVelocity": -3.0403475801105514e-20,
-                    "velocityX": 1.0509973010204301e-32,
-                    "velocityY": 3.3376257900775013,
-                    "timestamp": 0.5003556465592092
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.5464999863701525,
-                    "heading": 4.7457500532831525e-22,
-                    "angularVelocity": -9.63903176962576e-21,
-                    "velocityX": 7.983324746463204e-33,
-                    "velocityY": 2.503219520548835,
-                    "timestamp": 0.600426775871051
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.713499994424153,
-                    "heading": 1.0909640932435146e-22,
-                    "angularVelocity": -3.652188228166023e-21,
-                    "velocityX": 7.341551448907972e-33,
-                    "velocityY": 1.6688130652907358,
-                    "timestamp": 0.7004979051828928
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": -9.42965601107167e-40,
-                    "angularVelocity": -1.09018866193586e-21,
-                    "velocityX": 7.44470103227488e-33,
-                    "velocityY": 0.8344065481228213,
-                    "timestamp": 0.8005690344947346
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 8.249839679439821e-43,
-                    "angularVelocity": 9.431229878048832e-39,
-                    "velocityX": 1.012759498983901e-41,
-                    "velocityY": 5.142787784847508e-39,
-                    "timestamp": 0.9006401638065764
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "C3 To C4": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 9
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": -1.6773104267984464e-40,
-                    "angularVelocity": -1.9855220924111835e-39,
-                    "velocityX": -5.3948128499400994e-40,
-                    "velocityY": -2.7550648847397363e-40,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.043499994424153,
-                    "heading": -2.181197470484939e-22,
-                    "angularVelocity": -2.1796471324426728e-21,
-                    "velocityX": 2.2751291494664054e-33,
-                    "velocityY": -0.8344065481228213,
-                    "timestamp": 0.10007112931184182
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.8764999863701526,
-                    "heading": -1.0976856482379068e-21,
-                    "angularVelocity": -8.789407177499062e-21,
-                    "velocityX": 3.967849397509383e-33,
-                    "velocityY": -1.668813065290736,
-                    "timestamp": 0.20014225862368365
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.6259999820333837,
-                    "heading": -3.357689035831156e-21,
-                    "angularVelocity": -2.2583970074080117e-20,
-                    "velocityX": 6.1318806903820435e-33,
-                    "velocityY": -2.503219520548835,
-                    "timestamp": 0.3002133879355255
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.2919999999999994,
-                    "heading": -1.0603568936345678e-20,
-                    "angularVelocity": -7.2407296109284e-20,
-                    "velocityX": 8.195284274358776e-33,
-                    "velocityY": -3.3376257900775017,
-                    "timestamp": 0.4002845172473673
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.9580000179666155,
-                    "heading": -4.785144304302103e-21,
-                    "angularVelocity": 5.814288954429956e-20,
-                    "velocityX": 7.586727405106972e-33,
-                    "velocityY": -3.3376257900775013,
-                    "timestamp": 0.5003556465592092
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.7075000136298466,
-                    "heading": -2.6182090767908737e-21,
-                    "angularVelocity": 2.165395003165498e-20,
-                    "velocityX": 5.326134827450793e-33,
-                    "velocityY": -2.503219520548835,
-                    "timestamp": 0.600426775871051
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.540500005575846,
-                    "heading": -9.668053802454109e-22,
-                    "angularVelocity": 1.6502299039719674e-20,
-                    "velocityX": 3.4348416075821124e-33,
-                    "velocityY": -1.6688130652907358,
-                    "timestamp": 0.7004979051828928
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 1.8181769494337196e-40,
-                    "angularVelocity": 9.661181877631647e-21,
-                    "velocityX": 1.7280403247932335e-33,
-                    "velocityY": -0.8344065481228213,
-                    "timestamp": 0.8005690344947346
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": -1.6883012335149235e-41,
-                    "angularVelocity": -1.9855220810412424e-39,
-                    "velocityX": -1.2101303998889038e-42,
-                    "velocityY": -7.163168700323315e-39,
-                    "timestamp": 0.9006401638065764
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "C3 To C5": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 14
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "angularVelocity": -1.1842498081533988e-39,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.050554627994461,
-                    "heading": 1.1070575354189048e-18,
-                    "angularVelocity": 1.156189558439314e-17,
-                    "velocityX": 1.1339790734570631e-33,
-                    "velocityY": -0.7983807350946678,
-                    "timestamp": 0.09575052183150978
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.897663887017311,
-                    "heading": 3.320268139408775e-18,
-                    "angularVelocity": 2.3114345091427606e-17,
-                    "velocityX": 5.50963145740529e-34,
-                    "velocityY": -1.596761438503583,
-                    "timestamp": 0.19150104366301957
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.6683277831364007,
-                    "heading": 6.638014774282681e-18,
-                    "angularVelocity": 3.4649906568297676e-17,
-                    "velocityX": 4.767145470149766e-34,
-                    "velocityY": -2.3951420785410233,
-                    "timestamp": 0.28725156549452935
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.362546334555254,
-                    "heading": 1.1056344671920077e-17,
-                    "angularVelocity": 4.614418609717799e-17,
-                    "velocityX": -5.447572473811757e-34,
-                    "velocityY": -3.19352252846437,
-                    "timestamp": 0.38300208732603913
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.0003278007331793,
-                    "heading": 2.1759975564527066e-17,
-                    "angularVelocity": 1.11786658577609e-16,
-                    "velocityX": -1.9220305893434716e-33,
-                    "velocityY": -3.7829405719528495,
-                    "timestamp": 0.4787526091575489
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.63810926691106,
-                    "heading": -1.7246492437320545e-17,
-                    "angularVelocity": -4.0737603572006183e-16,
-                    "velocityX": 5.877602560652089e-33,
-                    "velocityY": -3.782940571953311,
-                    "timestamp": 0.5745031309890587
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.27589073308894,
-                    "heading": -9.722837303851897e-18,
-                    "angularVelocity": 7.857560449339153e-17,
-                    "velocityX": 3.350655179860002e-33,
-                    "velocityY": -3.782940571953311,
-                    "timestamp": 0.6702536528205685
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.9136721992668206,
-                    "heading": -4.541786403704224e-17,
-                    "angularVelocity": -3.727919811862982e-16,
-                    "velocityX": 1.4553646654154268e-32,
-                    "velocityY": -3.782940571953311,
-                    "timestamp": 0.7660041746520783
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.5514536654447455,
-                    "heading": -1.11893051502424e-17,
-                    "angularVelocity": 3.5747647356983727e-16,
-                    "velocityX": -5.35720215261278e-33,
-                    "velocityY": -3.7829405719528495,
-                    "timestamp": 0.861754696483588
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.2456722168635992,
-                    "heading": -6.7032993083141955e-18,
-                    "angularVelocity": 4.6850980602319425e-17,
-                    "velocityX": -4.3527390380082413e-33,
-                    "velocityY": -3.19352252846437,
-                    "timestamp": 0.9575052183150978
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.016336112982689,
-                    "heading": -3.348658686079055e-18,
-                    "angularVelocity": 3.5035220254491646e-17,
-                    "velocityX": -6.634490282826358e-33,
-                    "velocityY": -2.395142078541024,
-                    "timestamp": 1.0532557401466076
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.8634453720055388,
-                    "heading": -1.115588951241814e-18,
-                    "angularVelocity": 2.3321750022200153e-17,
-                    "velocityX": -4.820835446954302e-33,
-                    "velocityY": -1.596761438503583,
-                    "timestamp": 1.1490062619781174
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "angularVelocity": 1.1650996045214628e-17,
-                    "velocityX": -3.709395222464697e-33,
-                    "velocityY": -0.7983807350946678,
-                    "timestamp": 1.2447567838096272
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 6.978360472267901e-43,
-                    "velocityY": -3.6401965239950315e-38,
-                    "timestamp": 1.340507305641137
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "C4 To W1": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 25
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "angularVelocity": 2.322866633708497e-28,
-                    "velocityX": -2.1618763976557105e-31,
-                    "velocityY": -1.247476705840714e-31,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.635666877682845,
-                    "y": 2.515362381859407,
-                    "heading": -0.0008300252680408195,
-                    "angularVelocity": -0.00885317162168204,
-                    "velocityX": -0.4728636048938783,
-                    "velocityY": 0.6225017511467659,
-                    "timestamp": 0.09375456655646779
-                },
-                {
-                    "x": 7.547000631163484,
-                    "y": 2.632087139923421,
-                    "heading": -0.0024900693866490363,
-                    "angularVelocity": -0.01770627479364842,
-                    "velocityX": -0.9457272298940119,
-                    "velocityY": 1.2450034419786018,
-                    "timestamp": 0.18750913311293557
-                },
-                {
-                    "x": 7.4140012603329,
-                    "y": 2.8071742656560237,
-                    "heading": -0.004980126198969091,
-                    "angularVelocity": -0.026559312295687615,
-                    "velocityX": -1.418590856056911,
-                    "velocityY": 1.867505041764004,
-                    "timestamp": 0.28126369966940334
-                },
-                {
-                    "x": 7.236668772838196,
-                    "y": 3.0406237394955027,
-                    "heading": -0.008300187115435396,
-                    "angularVelocity": -0.035412258180156565,
-                    "velocityX": -1.891454400654691,
-                    "velocityY": 2.4900064329013105,
-                    "timestamp": 0.37501826622587114
-                },
-                {
-                    "x": 7.022136423676778,
-                    "y": 3.3230511197393278,
-                    "heading": -0.008300187546286333,
-                    "angularVelocity": -4.59551947810532e-9,
-                    "velocityX": -2.2882335980104607,
-                    "velocityY": 3.012411988206685,
-                    "timestamp": 0.46877283278233894
-                },
-                {
-                    "x": 6.786560921928144,
-                    "y": 3.5881804025568163,
-                    "heading": -0.008300187986713265,
-                    "angularVelocity": -4.6976584720676315e-9,
-                    "velocityX": -2.51268295936015,
-                    "velocityY": 2.827907936172931,
-                    "timestamp": 0.5625273993388067
-                },
-                {
-                    "x": 6.5015204114211205,
-                    "y": 3.799228403383322,
-                    "heading": -0.008300188501414158,
-                    "angularVelocity": -5.489875460815635e-9,
-                    "velocityX": -3.0402840200359327,
-                    "velocityY": 2.251069026055312,
-                    "timestamp": 0.6562819658952744
-                },
-                {
-                    "x": 6.179186778264032,
-                    "y": 3.9471823835437703,
-                    "heading": -0.008300169798343478,
-                    "angularVelocity": 1.9948970346527163e-7,
-                    "velocityX": -3.4380579527606097,
-                    "velocityY": 1.5780989192812942,
-                    "timestamp": 0.7500365324517422
-                },
-                {
-                    "x": 5.833324835964247,
-                    "y": 4.025724182042927,
-                    "heading": -0.008299866211032016,
-                    "angularVelocity": 0.0000032381069276099122,
-                    "velocityX": -3.689014359545628,
-                    "velocityY": 0.8377383778085268,
-                    "timestamp": 0.8437910990082099
-                },
-                {
-                    "x": 5.481092764022396,
-                    "y": 4.0672194252777,
-                    "heading": -0.008299498284594501,
-                    "angularVelocity": 0.000003924357511610969,
-                    "velocityX": -3.7569590994770814,
-                    "velocityY": 0.44259436909433375,
-                    "timestamp": 0.9375456655646777
-                },
-                {
-                    "x": 5.14904862351455,
-                    "y": 4.179095043376677,
-                    "heading": 0.0033651913425959347,
-                    "angularVelocity": 0.12441729566489874,
-                    "velocityX": -3.5416316527670983,
-                    "velocityY": 1.1932818017093059,
-                    "timestamp": 1.0313002321211455
-                },
-                {
-                    "x": 4.858987953002521,
-                    "y": 4.350333701259038,
-                    "heading": 0.047887100206889445,
-                    "angularVelocity": 0.474877229979814,
-                    "velocityX": -3.093829785211866,
-                    "velocityY": 1.8264567174893411,
-                    "timestamp": 1.1250547986776134
-                },
-                {
-                    "x": 4.6181973815778665,
-                    "y": 4.575631660267378,
-                    "heading": 0.10901822024185409,
-                    "angularVelocity": 0.652033519862158,
-                    "velocityX": -2.5683076597621346,
-                    "velocityY": 2.403061176467013,
-                    "timestamp": 1.2188093652340812
-                },
-                {
-                    "x": 4.427445933507109,
-                    "y": 4.854263962903063,
-                    "heading": 0.15298068479753676,
-                    "angularVelocity": 0.46891011467909954,
-                    "velocityX": -2.0345830083473238,
-                    "velocityY": 2.971933132108998,
-                    "timestamp": 1.312563931790549
-                },
-                {
-                    "x": 4.19709169118396,
-                    "y": 5.123942070734021,
-                    "heading": 0.15298069409509907,
-                    "angularVelocity": 9.916916750047786e-8,
-                    "velocityX": -2.4569922381797515,
-                    "velocityY": 2.8764263729866686,
-                    "timestamp": 1.406318498347017
-                },
-                {
-                    "x": 3.9282946631899107,
-                    "y": 5.355323824233057,
-                    "heading": 0.15298070467968702,
-                    "angularVelocity": 1.1289677246612154e-7,
-                    "velocityX": -2.8670286458223346,
-                    "velocityY": 2.4679518235485323,
-                    "timestamp": 1.5000730649034848
-                },
-                {
-                    "x": 3.6594937496643456,
-                    "y": 5.586701063819283,
-                    "heading": 0.1529807152642756,
-                    "angularVelocity": 1.1289677885329184e-7,
-                    "velocityX": -2.867070089473112,
-                    "velocityY": 2.4679036774904017,
-                    "timestamp": 1.5938276314599527
-                },
-                {
-                    "x": 3.3906928188108107,
-                    "y": 5.818078283274842,
-                    "heading": 0.1529807258488866,
-                    "angularVelocity": 1.1289701863908675e-7,
-                    "velocityX": -2.8670702742957928,
-                    "velocityY": 2.467903462773746,
-                    "timestamp": 1.6875821980164205
-                },
-                {
-                    "x": 3.1218918754104785,
-                    "y": 6.049455488154229,
-                    "heading": 0.15298073643351034,
-                    "angularVelocity": 1.1289715379702627e-7,
-                    "velocityX": -2.867070408121775,
-                    "velocityY": 2.467903307302158,
-                    "timestamp": 1.7813367645728884
-                },
-                {
-                    "x": 2.853090944613356,
-                    "y": 6.280832707674931,
-                    "heading": 0.15298074701856604,
-                    "angularVelocity": 1.1290176153813175e-7,
-                    "velocityX": -2.8670702736940825,
-                    "velocityY": 2.4679034634685753,
-                    "timestamp": 1.8750913311293562
-                },
-                {
-                    "x": 2.631853660621228,
-                    "y": 6.471299719512446,
-                    "heading": 0.24894777935018825,
-                    "angularVelocity": 1.0235984854542721,
-                    "velocityX": -2.3597494193403232,
-                    "velocityY": 2.0315491696374846,
-                    "timestamp": 1.968845897685824
-                },
-                {
-                    "x": 2.465926649664366,
-                    "y": 6.614149879341798,
-                    "heading": 0.32096506390227886,
-                    "angularVelocity": 0.7681469521670183,
-                    "velocityX": -1.7698019099360354,
-                    "velocityY": 1.52366082075921,
-                    "timestamp": 2.062600464242292
-                },
-                {
-                    "x": 2.355308907973467,
-                    "y": 6.709383273555383,
-                    "heading": 0.3689895410149908,
-                    "angularVelocity": 0.5122361382129277,
-                    "velocityX": -1.1798651068828128,
-                    "velocityY": 1.0157733933549364,
-                    "timestamp": 2.1563550307987596
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "angularVelocity": 0.25609908793666974,
-                    "velocityX": -0.5899329494543092,
-                    "velocityY": 0.5078870095990298,
-                    "timestamp": 2.250109597355227
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "angularVelocity": 0,
-                    "velocityX": -3.330818616870831e-32,
-                    "velocityY": 5.736512740310497e-33,
-                    "timestamp": 2.343864163911695
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C4 To W2": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 14
-                },
-                {
-                    "x": 4.992805004119873,
-                    "y": 4.258058071136475,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 13
-                },
-                {
-                    "x": 2.851226329803467,
-                    "y": 6.282596111297607,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 7
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "angularVelocity": -1.0202939485165084e-30,
-                    "velocityX": 2.0196671556133315e-31,
-                    "velocityY": 6.267725686062039e-32,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.650233104605526,
-                    "y": 2.498844879842391,
-                    "heading": 0.006167890922909293,
-                    "angularVelocity": 0.07857204140894247,
-                    "velocityX": -0.37919700052787514,
-                    "velocityY": 0.5330570324317279,
-                    "timestamp": 0.07849981764896931
-                },
-                {
-                    "x": 7.590699315345095,
-                    "y": 2.5825346366761623,
-                    "heading": 0.018503878854508167,
-                    "angularVelocity": 0.15714670812054757,
-                    "velocityX": -0.758393981584137,
-                    "velocityY": 1.066114028544761,
-                    "timestamp": 0.15699963529793862
-                },
-                {
-                    "x": 7.501398636107905,
-                    "y": 2.7080692662264956,
-                    "heading": 0.037008343975294504,
-                    "angularVelocity": 0.23572621790707007,
-                    "velocityX": -1.1375909130963788,
-                    "velocityY": 1.599170970201378,
-                    "timestamp": 0.23549945294690794
-                },
-                {
-                    "x": 7.382331074013197,
-                    "y": 2.875448760857071,
-                    "heading": 0.061681864177524894,
-                    "angularVelocity": 0.31431308939549074,
-                    "velocityX": -1.5167877539174253,
-                    "velocityY": 2.132227814579811,
-                    "timestamp": 0.31399927059587723
-                },
-                {
-                    "x": 7.2334966444595095,
-                    "y": 3.084673101412061,
-                    "heading": 0.09252521384117814,
-                    "angularVelocity": 0.39290982561993243,
-                    "velocityX": -1.8959843985782956,
-                    "velocityY": 2.665284414934403,
-                    "timestamp": 0.39249908824484653
-                },
-                {
-                    "x": 7.061360166565958,
-                    "y": 3.326653173894572,
-                    "heading": 0.09252521821441809,
-                    "angularVelocity": 5.5710192458533506e-8,
-                    "velocityX": -2.192826468251186,
-                    "velocityY": 3.0825558546464755,
-                    "timestamp": 0.4709989058938158
-                },
-                {
-                    "x": 6.8697148114409385,
-                    "y": 3.553495365981472,
-                    "heading": 0.09252522268181232,
-                    "angularVelocity": 5.690961282999965e-8,
-                    "velocityX": -2.4413477746153216,
-                    "velocityY": 2.8897161659824335,
-                    "timestamp": 0.5494987235427852
-                },
-                {
-                    "x": 6.64183630533757,
-                    "y": 3.743907306900521,
-                    "heading": 0.09252522772743905,
-                    "angularVelocity": 6.427564912453075e-8,
-                    "velocityX": -2.902917649087824,
-                    "velocityY": 2.425635455237886,
-                    "timestamp": 0.6279985411917545
-                },
-                {
-                    "x": 6.384546476442235,
-                    "y": 3.8921883279204876,
-                    "heading": 0.09252523431940601,
-                    "angularVelocity": 8.397429651316677e-8,
-                    "velocityX": -3.277585051800601,
-                    "velocityY": 1.8889345919635134,
-                    "timestamp": 0.7064983588407238
-                },
-                {
-                    "x": 6.105547891541977,
-                    "y": 3.9938992218783795,
-                    "heading": 0.09252527901816404,
-                    "angularVelocity": 5.694122529822745e-7,
-                    "velocityX": -3.5541303566827107,
-                    "velocityY": 1.2956831876058104,
-                    "timestamp": 0.7849981764896931
-                },
-                {
-                    "x": 5.813193382307703,
-                    "y": 4.045994945482373,
-                    "heading": 0.09252629740574593,
-                    "angularVelocity": 0.000012973120350997966,
-                    "velocityX": -3.7242699153978625,
-                    "velocityY": 0.6636413327346956,
-                    "timestamp": 0.8634979941386624
-                },
-                {
-                    "x": 5.523572440187452,
-                    "y": 4.068080526231475,
-                    "heading": 0.11166809511552328,
-                    "angularVelocity": 0.24384512325081395,
-                    "velocityX": -3.6894473235002847,
-                    "velocityY": 0.28134563124547074,
-                    "timestamp": 0.9419978117876316
-                },
-                {
-                    "x": 5.248612904615211,
-                    "y": 4.139233352038044,
-                    "heading": 0.1457449942262438,
-                    "angularVelocity": 0.43410163400767243,
-                    "velocityX": -3.502677379478612,
-                    "velocityY": 0.9064075298205869,
-                    "timestamp": 1.020497629436601
-                },
-                {
-                    "x": 4.992805004119873,
-                    "y": 4.258058071136475,
-                    "heading": 0.1827080793073441,
-                    "angularVelocity": 0.47086841967443066,
-                    "velocityX": -3.258706939158039,
-                    "velocityY": 1.513694205377446,
-                    "timestamp": 1.0989974470855703
-                },
-                {
-                    "x": 4.782367562736006,
-                    "y": 4.400850786547769,
-                    "heading": 0.2087590252622848,
-                    "angularVelocity": 0.37195010692359587,
-                    "velocityX": -3.004582979783548,
-                    "velocityY": 2.0387653429944033,
-                    "timestamp": 1.1690362653840851
-                },
-                {
-                    "x": 4.593366685154892,
-                    "y": 4.57831882888555,
-                    "heading": 0.22294244363545143,
-                    "angularVelocity": 0.20250796226614393,
-                    "velocityX": -2.6985160825467664,
-                    "velocityY": 2.5338526070127387,
-                    "timestamp": 1.2390750836825999
-                },
-                {
-                    "x": 4.4303113780727985,
-                    "y": 4.787155425256865,
-                    "heading": 0.2229432862167668,
-                    "angularVelocity": 0.000012030204616055241,
-                    "velocityX": -2.3280705049466333,
-                    "velocityY": 2.9817264403466797,
-                    "timestamp": 1.3091139019811147
-                },
-                {
-                    "x": 4.301341913399859,
-                    "y": 5.018600438478609,
-                    "heading": 0.22294334445620553,
-                    "angularVelocity": 8.315308587772638e-7,
-                    "velocityX": -1.8413997809506994,
-                    "velocityY": 3.3045248170136206,
-                    "timestamp": 1.3791527202796294
-                },
-                {
-                    "x": 4.209527987601824,
-                    "y": 5.267136397095996,
-                    "heading": 0.22294334228224533,
-                    "angularVelocity": -3.103936119065208e-8,
-                    "velocityX": -1.3109005552544912,
-                    "velocityY": 3.5485458586481258,
-                    "timestamp": 1.4491915385781442
-                },
-                {
-                    "x": 4.095163246631834,
-                    "y": 5.506135613505712,
-                    "heading": 0.22294334005402738,
-                    "angularVelocity": -3.181404259759038e-8,
-                    "velocityX": -1.6328765068917115,
-                    "velocityY": 3.412382193414732,
-                    "timestamp": 1.519230356876659
-                },
-                {
-                    "x": 3.9453760475707735,
-                    "y": 5.724684722787353,
-                    "heading": 0.22294333755566106,
-                    "angularVelocity": -3.567116580835512e-8,
-                    "velocityX": -2.138631157692488,
-                    "velocityY": 3.120399723908581,
-                    "timestamp": 1.5892691751751737
-                },
-                {
-                    "x": 3.7637358458155963,
-                    "y": 5.917575181509908,
-                    "heading": 0.22294327041118944,
-                    "angularVelocity": -9.586751071941434e-7,
-                    "velocityX": -2.5934218504529762,
-                    "velocityY": 2.7540507308438293,
-                    "timestamp": 1.6593079934736885
-                },
-                {
-                    "x": 3.5705120272666107,
-                    "y": 6.071441420591724,
-                    "heading": 0.20870193370475335,
-                    "angularVelocity": -0.20333490844666557,
-                    "velocityX": -2.7588103746352264,
-                    "velocityY": 2.1968708613263175,
-                    "timestamp": 1.7293468117722033
-                },
-                {
-                    "x": 3.3793495617108644,
-                    "y": 6.184488312917552,
-                    "heading": 0.18879422691132347,
-                    "angularVelocity": -0.2842381878658059,
-                    "velocityX": -2.7293787959290245,
-                    "velocityY": 1.614060532031302,
-                    "timestamp": 1.799385630070718
-                },
-                {
-                    "x": 3.1945436254003248,
-                    "y": 6.257137552443532,
-                    "heading": 0.1660945073730067,
-                    "angularVelocity": -0.3241019778712817,
-                    "velocityX": -2.638621564444474,
-                    "velocityY": 1.037271063258847,
-                    "timestamp": 1.8694244483692328
-                },
-                {
-                    "x": 3.018114205815821,
-                    "y": 6.289753496686376,
-                    "heading": 0.14197845255388317,
-                    "angularVelocity": -0.34432412489220054,
-                    "velocityX": -2.519023362625089,
-                    "velocityY": 0.46568381699052885,
-                    "timestamp": 1.9394632666677476
-                },
-                {
-                    "x": 2.851226329803467,
-                    "y": 6.282596111297607,
-                    "heading": 0.11724722927389757,
-                    "angularVelocity": -0.3531073750355408,
-                    "velocityX": -2.382791144492779,
-                    "velocityY": -0.10219169258892027,
-                    "timestamp": 2.0095020849662624
-                },
-                {
-                    "x": 2.661249908723212,
-                    "y": 6.213695182206588,
-                    "heading": 0.08663876117142615,
-                    "angularVelocity": -0.3547239131111928,
-                    "velocityX": -2.2016514926122546,
-                    "velocityY": -0.7984982163199495,
-                    "timestamp": 2.0957902289879784
-                },
-                {
-                    "x": 2.4936356536291973,
-                    "y": 6.0868793491721815,
-                    "heading": 0.056963129639284946,
-                    "angularVelocity": -0.3439131976774355,
-                    "velocityX": -1.9424946149239757,
-                    "velocityY": -1.4696785343117862,
-                    "timestamp": 2.1820783730096944
-                },
-                {
-                    "x": 2.3641635269275443,
-                    "y": 5.911089762977232,
-                    "heading": 0.030889400871559385,
-                    "angularVelocity": -0.3021704669089439,
-                    "velocityX": -1.500462527842606,
-                    "velocityY": -2.037239161740341,
-                    "timestamp": 2.2683665170314105
-                },
-                {
-                    "x": 2.2965373597407854,
-                    "y": 5.740054606624193,
-                    "heading": 0.01408623201300084,
-                    "angularVelocity": -0.19473322840668433,
-                    "velocityX": -0.7837249016474405,
-                    "velocityY": -1.9821397051956142,
-                    "timestamp": 2.3546546610531265
-                },
-                {
-                    "x": 2.263390196849459,
-                    "y": 5.620608972310546,
-                    "heading": 0.004408633441468959,
-                    "angularVelocity": -0.11215444116055903,
-                    "velocityX": -0.38414504410927525,
-                    "velocityY": -1.384264729156902,
-                    "timestamp": 2.4409428050748425
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": -1.7581112791577623e-31,
-                    "angularVelocity": -0.05109199521503166,
-                    "velocityX": -0.1551800308289656,
-                    "velocityY": -0.7024020854510996,
-                    "timestamp": 2.5272309490965585
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 5.961493131289069e-30,
-                    "velocityX": -9.776555553264502e-32,
-                    "velocityY": 0,
-                    "timestamp": 2.6135190931182746
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C4 To W3": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
-                {
-                    "x": 3.8162567615509033,
-                    "y": 2.2371633052825928,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 14
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "angularVelocity": -4.62162066936404e-33,
-                    "velocityX": 3.0688151507515204e-31,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.623584376302727,
-                    "y": 2.4297857949616017,
-                    "heading": 0.033539307380936634,
-                    "angularVelocity": 0.38483343876921716,
-                    "velocityX": -0.6473186288895079,
-                    "velocityY": -0.3122585684118786,
-                    "timestamp": 0.08715278871867994
-                },
-                {
-                    "x": 7.510615150776605,
-                    "y": 2.375312721693156,
-                    "heading": 0.0969954791736376,
-                    "angularVelocity": 0.7281025968948721,
-                    "velocityX": -1.2962204329545195,
-                    "velocityY": -0.6250296068468768,
-                    "timestamp": 0.1743055774373599
-                },
-                {
-                    "x": 7.340880293995689,
-                    "y": 2.2935273668964844,
-                    "heading": 0.1838752961952568,
-                    "angularVelocity": 0.9968678948651678,
-                    "velocityX": -1.9475550843106406,
-                    "velocityY": -0.9384135149210906,
-                    "timestamp": 0.26145836615603985
-                },
-                {
-                    "x": 7.114069036249808,
-                    "y": 2.1844157133503646,
-                    "heading": 0.27996244701307155,
-                    "angularVelocity": 1.1025137833279668,
-                    "velocityX": -2.6024555390648842,
-                    "velocityY": -1.251958258023393,
-                    "timestamp": 0.3486111548747198
-                },
-                {
-                    "x": 6.831104073589339,
-                    "y": 2.0490142604857513,
-                    "heading": 0.32912237638568254,
-                    "angularVelocity": 0.5640660510737534,
-                    "velocityX": -3.246768885088134,
-                    "velocityY": -1.5536101007814562,
-                    "timestamp": 0.4357639435933997
-                },
-                {
-                    "x": 6.534528280813305,
-                    "y": 1.9049975621731787,
-                    "heading": 0.3291224030694369,
-                    "angularVelocity": 3.0617212321522066e-7,
-                    "velocityX": -3.4029409401155197,
-                    "velocityY": -1.6524623070575855,
-                    "timestamp": 0.5229167323120797
-                },
-                {
-                    "x": 6.237952262222522,
-                    "y": 1.7609813288852085,
-                    "heading": 0.3291224297529421,
-                    "angularVelocity": 3.0616926449614015e-7,
-                    "velocityX": -3.402943531137019,
-                    "velocityY": -1.6524569713178037,
-                    "timestamp": 0.6100695210307596
-                },
-                {
-                    "x": 5.936787262454918,
-                    "y": 1.6268259970502223,
-                    "heading": 0.32912245667190637,
-                    "angularVelocity": 3.0887094574020004e-7,
-                    "velocityX": -3.4555979699024117,
-                    "velocityY": -1.5393119808021931,
-                    "timestamp": 0.6972223097494396
-                },
-                {
-                    "x": 5.615527521313305,
-                    "y": 1.5527301986214108,
-                    "heading": 0.32912248658360005,
-                    "angularVelocity": 3.4320982850713803e-7,
-                    "velocityX": -3.6861670850098154,
-                    "velocityY": -0.850182759704744,
-                    "timestamp": 0.7843750984681195
-                },
-                {
-                    "x": 5.286027461953728,
-                    "y": 1.5414290964422628,
-                    "heading": 0.32912252585289026,
-                    "angularVelocity": 4.50579847209181e-7,
-                    "velocityX": -3.780717337951944,
-                    "velocityY": -0.12967000075725457,
-                    "timestamp": 0.8715278871867994
-                },
-                {
-                    "x": 4.960446284223111,
-                    "y": 1.593339788205863,
-                    "heading": 0.3291233220408656,
-                    "angularVelocity": 0.000009135542155699871,
-                    "velocityX": -3.7357516898462033,
-                    "velocityY": 0.5956285797252272,
-                    "timestamp": 0.9586806759054793
-                },
-                {
-                    "x": 4.650797848549272,
-                    "y": 1.7065466836899894,
-                    "heading": 0.3291235959637348,
-                    "angularVelocity": 0.0000031430189815062194,
-                    "velocityX": -3.5529377800330906,
-                    "velocityY": 1.2989474823295228,
-                    "timestamp": 1.0458334646241594
-                },
-                {
-                    "x": 4.368508202692925,
-                    "y": 1.8768714467040708,
-                    "heading": 0.32912381701495697,
-                    "angularVelocity": 0.000002536364302217629,
-                    "velocityX": -3.2390202310972316,
-                    "velocityY": 1.9543237286860882,
-                    "timestamp": 1.1329862533428394
-                },
-                {
-                    "x": 4.092384496633659,
-                    "y": 2.0570204638146983,
-                    "heading": 0.3291240376025158,
-                    "angularVelocity": 0.000002531044182467546,
-                    "velocityX": -3.1682716080441704,
-                    "velocityY": 2.067048223690569,
-                    "timestamp": 1.2201390420615195
-                },
-                {
-                    "x": 3.8162567615509033,
-                    "y": 2.2371633052825928,
-                    "heading": 0.32912425818941665,
-                    "angularVelocity": 0.0000025310366317804206,
-                    "velocityX": -3.1683178374712258,
-                    "velocityY": 2.066977363735053,
-                    "timestamp": 1.3072918307801995
-                },
-                {
-                    "x": 3.558387727399027,
-                    "y": 2.3518646644990793,
-                    "heading": 0.32912440943088817,
-                    "angularVelocity": 0.0000020272145178599712,
-                    "velocityX": -3.4564319203160125,
-                    "velocityY": 1.5374371746628512,
-                    "timestamp": 1.3818973893726278
-                },
-                {
-                    "x": 3.300516056227942,
-                    "y": 2.466560095044955,
-                    "heading": 0.3291245606718786,
-                    "angularVelocity": 0.000002027208070545675,
-                    "velocityX": -3.4564672664652627,
-                    "velocityY": 1.5373577077876843,
-                    "timestamp": 1.4565029479650562
-                },
-                {
-                    "x": 3.052458337002912,
-                    "y": 2.601171414843865,
-                    "heading": 0.329124716605593,
-                    "angularVelocity": 0.0000020901085294853097,
-                    "velocityX": -3.3249227524744356,
-                    "velocityY": 1.804306841723335,
-                    "timestamp": 1.5311085065574845
-                },
-                {
-                    "x": 2.829814908557799,
-                    "y": 2.7746151638451657,
-                    "heading": 0.3291249161701291,
-                    "angularVelocity": 0.000002674928516490017,
-                    "velocityX": -2.984273995740964,
-                    "velocityY": 2.3248100044237634,
-                    "timestamp": 1.6057140651499129
-                },
-                {
-                    "x": 2.638606432152533,
-                    "y": 2.982201542308141,
-                    "heading": 0.3291253317978473,
-                    "angularVelocity": 0.000005571002027267265,
-                    "velocityX": -2.562925337103115,
-                    "velocityY": 2.7824519027734036,
-                    "timestamp": 1.6803196237423412
-                },
-                {
-                    "x": 2.4840036736729245,
-                    "y": 3.218316897029485,
-                    "heading": 0.3291273340604081,
-                    "angularVelocity": 0.000026837980957261232,
-                    "velocityX": -2.0722686271167303,
-                    "velocityY": 3.1648493647939433,
-                    "timestamp": 1.7549251823347696
-                },
-                {
-                    "x": 2.3733884856918945,
-                    "y": 3.4659660107751304,
-                    "heading": 0.2954274954004952,
-                    "angularVelocity": -0.4517068070492687,
-                    "velocityX": -1.4826668423638905,
-                    "velocityY": 3.31944587532086,
-                    "timestamp": 1.829530740927198
-                },
-                {
-                    "x": 2.305023098579148,
-                    "y": 3.7064756804968377,
-                    "heading": 0.19598318894217118,
-                    "angularVelocity": -1.3329342790875696,
-                    "velocityX": -0.9163578210871337,
-                    "velocityY": 3.2237500028063106,
-                    "timestamp": 1.9041362995196263
-                },
-                {
-                    "x": 2.272475779606133,
-                    "y": 3.9200958593762563,
-                    "heading": 0.053473340322528436,
-                    "angularVelocity": -1.9101773555262378,
-                    "velocityX": -0.43625863256144176,
-                    "velocityY": 2.8633279196585013,
-                    "timestamp": 1.9787418581120546
-                },
-                {
-                    "x": 2.2677016800416907,
-                    "y": 4.09662450199971,
-                    "heading": -0.09830221598718161,
-                    "angularVelocity": -2.034373298360552,
-                    "velocityX": -0.06399120460344819,
-                    "velocityY": 2.3661593848232187,
-                    "timestamp": 2.053347416704483
-                },
-                {
-                    "x": 2.2778239174156503,
-                    "y": 4.229436050413757,
-                    "heading": -0.23380831521279008,
-                    "angularVelocity": -1.8163003103546322,
-                    "velocityX": 0.13567671853055638,
-                    "velocityY": 1.7801830174558415,
-                    "timestamp": 2.1279529752969113
-                },
-                {
-                    "x": 2.29107059750262,
-                    "y": 4.316936633046431,
-                    "heading": -0.33501872696397506,
-                    "angularVelocity": -1.3566068488823915,
-                    "velocityX": 0.1775562081015563,
-                    "velocityY": 1.1728426712906341,
-                    "timestamp": 2.2025585338893396
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": -0.7369594715641588,
-                    "velocityX": 0.11968816621507666,
-                    "velocityY": 0.5772139203303202,
-                    "timestamp": 2.277164092481768
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": -8.092865962806098e-32,
-                    "velocityX": 0,
-                    "velocityY": 4.648171279746289e-33,
-                    "timestamp": 2.3517696510741963
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C4 To S3": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 11
-                },
-                {
-                    "x": 5.546474933624268,
-                    "y": 1.3928169012069702,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 13
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 7.203126847267154e-32,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.6459509070752,
-                    "y": 2.420050479995149,
-                    "heading": -0.026297254997510774,
-                    "angularVelocity": -0.33695614983619343,
-                    "velocityX": -0.4362832264603014,
-                    "velocityY": -0.47344743777665743,
-                    "timestamp": 0.078043552581802
-                },
-                {
-                    "x": 7.57752123433784,
-                    "y": 2.3463325784745406,
-                    "heading": -0.07655763531528098,
-                    "angularVelocity": -0.6440042598660941,
-                    "velocityX": -0.8768139131756897,
-                    "velocityY": -0.9445738831958564,
-                    "timestamp": 0.156087105163604
-                },
-                {
-                    "x": 7.474195561438856,
-                    "y": 2.236153787211583,
-                    "heading": -0.14701004173730176,
-                    "angularVelocity": -0.9027319245646475,
-                    "velocityX": -1.3239488654839289,
-                    "velocityY": -1.4117603263569152,
-                    "timestamp": 0.23413065774540598
-                },
-                {
-                    "x": 7.33507656702923,
-                    "y": 2.0901340736402396,
-                    "heading": -0.23055657416165956,
-                    "angularVelocity": -1.0705116522834195,
-                    "velocityX": -1.7825815177216175,
-                    "velocityY": -1.8710029046700958,
-                    "timestamp": 0.312174210327208
-                },
-                {
-                    "x": 7.158294386463989,
-                    "y": 1.910067298253472,
-                    "heading": -0.30975533817585227,
-                    "angularVelocity": -1.014802137964151,
-                    "velocityX": -2.2651734155741057,
-                    "velocityY": -2.3072601057983104,
-                    "timestamp": 0.39021776290901
-                },
-                {
-                    "x": 6.940360292129115,
-                    "y": 1.7108993896694566,
-                    "heading": -0.30975560484167247,
-                    "angularVelocity": -0.00000341688469228089,
-                    "velocityX": -2.7924676302559823,
-                    "velocityY": -2.552009768843728,
-                    "timestamp": 0.468261315490812
-                },
-                {
-                    "x": 6.6915168927330235,
-                    "y": 1.5520279357314815,
-                    "heading": -0.30975564017638474,
-                    "angularVelocity": -4.5275632793576304e-7,
-                    "velocityX": -3.1885196299240257,
-                    "velocityY": -2.035676858398594,
-                    "timestamp": 0.546304868072614
-                },
-                {
-                    "x": 6.419127595403782,
-                    "y": 1.43815413781978,
-                    "heading": -0.30975568823518523,
-                    "angularVelocity": -6.157946287917162e-7,
-                    "velocityX": -3.490221655962278,
-                    "velocityY": -1.4591057703627237,
-                    "timestamp": 0.624348420654416
-                },
-                {
-                    "x": 6.131252530334066,
-                    "y": 1.37264761019134,
-                    "heading": -0.30975577587401576,
-                    "angularVelocity": -0.0000011229477336220622,
-                    "velocityX": -3.6886463461279972,
-                    "velocityY": -0.8393586076157934,
-                    "timestamp": 0.7023919732362179
-                },
-                {
-                    "x": 5.83657654735769,
-                    "y": 1.3574693553002635,
-                    "heading": -0.31020540722452916,
-                    "angularVelocity": -0.005761287587978277,
-                    "velocityX": -3.7757889438387497,
-                    "velocityY": -0.1944844178532558,
-                    "timestamp": 0.7804355258180199
-                },
-                {
-                    "x": 5.546474933624268,
-                    "y": 1.3928169012069702,
-                    "heading": -0.3189695864044355,
-                    "angularVelocity": -0.11229856778804584,
-                    "velocityX": -3.717175911864151,
-                    "velocityY": 0.4529207697160109,
-                    "timestamp": 0.8584790783998218
-                },
-                {
-                    "x": 5.303276771996363,
-                    "y": 1.4607318697473424,
-                    "heading": -0.3307229091538368,
-                    "angularVelocity": -0.17360472172913416,
-                    "velocityX": -3.5922053767043294,
-                    "velocityY": 1.0031511484968414,
-                    "timestamp": 0.9261807090485242
-                },
-                {
-                    "x": 5.073563214559545,
-                    "y": 1.564404927734292,
-                    "heading": -0.34378229938421223,
-                    "angularVelocity": -0.1928962434919207,
-                    "velocityX": -3.3930284283517023,
-                    "velocityY": 1.5313229089691542,
-                    "timestamp": 0.9938823396972266
-                },
-                {
-                    "x": 4.862974125111252,
-                    "y": 1.7011642845126724,
-                    "heading": -0.35827284529055603,
-                    "angularVelocity": -0.2140354045770704,
-                    "velocityX": -3.1105467834446325,
-                    "velocityY": 2.020030469989471,
-                    "timestamp": 1.0615839703459289
-                },
-                {
-                    "x": 4.677671719496688,
-                    "y": 1.8665243414779629,
-                    "heading": -0.3791415933598452,
-                    "angularVelocity": -0.30824587043792007,
-                    "velocityX": -2.737044940262413,
-                    "velocityY": 2.442482631227878,
-                    "timestamp": 1.1292856009946313
-                },
-                {
-                    "x": 4.523629451068858,
-                    "y": 2.053477766497112,
-                    "heading": -0.41478083919993697,
-                    "angularVelocity": -0.5264163580502632,
-                    "velocityX": -2.27531105162187,
-                    "velocityY": 2.761431640977936,
-                    "timestamp": 1.1969872316433336
-                },
-                {
-                    "x": 4.406611773563404,
-                    "y": 2.2498215105595722,
-                    "heading": -0.45494733767836987,
-                    "angularVelocity": -0.5932870167770028,
-                    "velocityX": -1.7284321867022447,
-                    "velocityY": 2.900133160474012,
-                    "timestamp": 1.264688862292036
-                },
-                {
-                    "x": 4.327688857963163,
-                    "y": 2.4438365235550945,
-                    "heading": -0.48799769437613305,
-                    "angularVelocity": -0.4881766713911416,
-                    "velocityX": -1.1657461547677304,
-                    "velocityY": 2.865736188870776,
-                    "timestamp": 1.3323904929407384
-                },
-                {
-                    "x": 4.28360260298108,
-                    "y": 2.622213717107768,
-                    "heading": -0.5154307830643909,
-                    "angularVelocity": -0.40520573028252516,
-                    "velocityX": -0.6511845366152605,
-                    "velocityY": 2.634754759132393,
-                    "timestamp": 1.4000921235894408
-                },
-                {
-                    "x": 4.264391797811532,
-                    "y": 2.7716303897908317,
-                    "heading": -0.5365689504315967,
-                    "angularVelocity": -0.3122253801676767,
-                    "velocityX": -0.283756904899847,
-                    "velocityY": 2.2069877970647664,
-                    "timestamp": 1.4677937542381432
-                },
-                {
-                    "x": 4.259328393836262,
-                    "y": 2.8855840206662466,
-                    "heading": -0.5517027167443236,
-                    "angularVelocity": -0.22353621571242488,
-                    "velocityX": -0.07478998551086755,
-                    "velocityY": 1.6831740946789628,
-                    "timestamp": 1.5354953848868456
-                },
-                {
-                    "x": 4.260599830778067,
-                    "y": 2.9618797181043535,
-                    "heading": -0.5613571988137664,
-                    "angularVelocity": -0.1426033904496253,
-                    "velocityX": 0.018780004700190214,
-                    "velocityY": 1.1269403219251917,
-                    "timestamp": 1.603197015535548
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "angularVelocity": -0.0685773908510417,
-                    "velocityX": 0.035452162657366115,
-                    "velocityY": 0.5630629798778553,
-                    "timestamp": 1.6708986461842503
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "angularVelocity": 0,
-                    "velocityX": 1.1764020723168042e-36,
-                    "velocityY": 0,
-                    "timestamp": 1.7386002768329527
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C4 To S4": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 9
-                },
-                {
-                    "x": 6.4046630859375,
-                    "y": 1.6973352432250977,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 11
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "angularVelocity": 1.0550386775092505e-35,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.6504501923310215,
-                    "y": 2.437078665707293,
-                    "heading": -0.0029944704440250615,
-                    "angularVelocity": -0.0457972069493345,
-                    "velocityX": -0.45193254781641656,
-                    "velocityY": -0.30467539632270557,
-                    "timestamp": 0.0653854381848624
-                },
-                {
-                    "x": 7.5913505770823395,
-                    "y": 2.3972359988448533,
-                    "heading": -0.008983367804687599,
-                    "angularVelocity": -0.09159374819405963,
-                    "velocityX": -0.9038650942674934,
-                    "velocityY": -0.609350766294381,
-                    "timestamp": 0.1307708763697248
-                },
-                {
-                    "x": 7.50270115478166,
-                    "y": 2.337472001372052,
-                    "heading": -0.017966623739921285,
-                    "angularVelocity": -0.13738924422033513,
-                    "velocityX": -1.3557976326478658,
-                    "velocityY": -0.91402610629957,
-                    "timestamp": 0.19615631455458718
-                },
-                {
-                    "x": 7.384501926877228,
-                    "y": 2.2577866758321745,
-                    "heading": -0.02994413949472175,
-                    "angularVelocity": -0.18318322989496186,
-                    "velocityX": -1.8077301488788986,
-                    "velocityY": -1.2187014074079496,
-                    "timestamp": 0.2615417527394496
-                },
-                {
-                    "x": 7.2367528971895485,
-                    "y": 2.1581800263627264,
-                    "heading": -0.044915766222073404,
-                    "angularVelocity": -0.2289749391144676,
-                    "velocityX": -2.25966260667937,
-                    "velocityY": -1.5233766452376303,
-                    "timestamp": 0.326927190924312
-                },
-                {
-                    "x": 7.059454079201559,
-                    "y": 2.0386520636124454,
-                    "heading": -0.06288119646669635,
-                    "angularVelocity": -0.2747619461359239,
-                    "velocityX": -2.711594858272837,
-                    "velocityY": -1.82805172020631,
-                    "timestamp": 0.3923126291091744
-                },
-                {
-                    "x": 6.854358935700705,
-                    "y": 1.900385597577332,
-                    "heading": -0.06288119947800026,
-                    "angularVelocity": -4.6054656889756363e-8,
-                    "velocityX": -3.136709781174733,
-                    "velocityY": -2.114636987584247,
-                    "timestamp": 0.45769806729403684
-                },
-                {
-                    "x": 6.636845838072133,
-                    "y": 1.782615967982618,
-                    "heading": -0.06288120256062167,
-                    "angularVelocity": -4.714538109405926e-8,
-                    "velocityX": -3.3266290425951324,
-                    "velocityY": -1.801159904469057,
-                    "timestamp": 0.5230835054788993
-                },
-                {
-                    "x": 6.4046630859375,
-                    "y": 1.6973352432250977,
-                    "heading": -0.06288120593828476,
-                    "angularVelocity": -5.1657726572043936e-8,
-                    "velocityX": -3.5509856411482486,
-                    "velocityY": -1.3042770244409516,
-                    "timestamp": 0.5884689436637617
-                },
-                {
-                    "x": 6.207746736822867,
-                    "y": 1.6503996785686041,
-                    "heading": -0.06288121580879107,
-                    "angularVelocity": -1.8445407171610623e-7,
-                    "velocityX": -3.679854027466254,
-                    "velocityY": -0.8771035386810692,
-                    "timestamp": 0.6419809437716079
-                },
-                {
-                    "x": 6.009908768236521,
-                    "y": 1.6271742750630454,
-                    "heading": -0.0724170210773865,
-                    "angularVelocity": -0.17819938050114592,
-                    "velocityX": -3.697076696584537,
-                    "velocityY": -0.43402234001253925,
-                    "timestamp": 0.695492943879454
-                },
-                {
-                    "x": 5.8260492909953046,
-                    "y": 1.6186155743000838,
-                    "heading": -0.12620190803666176,
-                    "angularVelocity": -1.0050995449783064,
-                    "velocityX": -3.43585507681777,
-                    "velocityY": -0.1599398405163814,
-                    "timestamp": 0.7490049439873002
-                },
-                {
-                    "x": 5.6625912601386625,
-                    "y": 1.614879890442449,
-                    "heading": -0.22199295256207294,
-                    "angularVelocity": -1.7900852954918034,
-                    "velocityX": -3.0546051451490293,
-                    "velocityY": -0.06981020799271546,
-                    "timestamp": 0.8025169440951464
-                },
-                {
-                    "x": 5.5227239807129855,
-                    "y": 1.6141995251565013,
-                    "heading": -0.32512734843535046,
-                    "angularVelocity": -1.9273134187738115,
-                    "velocityX": -2.61375540334492,
-                    "velocityY": -0.012714256327113658,
-                    "timestamp": 0.8560289442029926
-                },
-                {
-                    "x": 5.406619958232834,
-                    "y": 1.6150039384726416,
-                    "heading": -0.42188556792063076,
-                    "angularVelocity": -1.8081592781110263,
-                    "velocityX": -2.169681982474204,
-                    "velocityY": 0.015032391136922594,
-                    "timestamp": 0.9095409443108388
-                },
-                {
-                    "x": 5.314084878105213,
-                    "y": 1.6164421457440925,
-                    "heading": -0.5051954003859285,
-                    "angularVelocity": -1.5568439284160218,
-                    "velocityX": -1.7292397955809797,
-                    "velocityY": 0.026876350511144828,
-                    "timestamp": 0.9630529444186849
-                },
-                {
-                    "x": 5.244912091070787,
-                    "y": 1.617980616061371,
-                    "heading": -0.5709381839957368,
-                    "angularVelocity": -1.2285615091439794,
-                    "velocityX": -1.2926593454742539,
-                    "velocityY": 0.028750005871169333,
-                    "timestamp": 1.016564944526531
-                },
-                {
-                    "x": 5.198930392622704,
-                    "y": 1.6192554664106382,
-                    "heading": -0.6164892293058035,
-                    "angularVelocity": -0.851230475748707,
-                    "velocityX": -0.8592782619863396,
-                    "velocityY": 0.023823634823929046,
-                    "timestamp": 1.0700769446343772
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "angularVelocity": -0.4393551100092281,
-                    "velocityX": -0.42850935447171357,
-                    "velocityY": 0.013913394899485362,
-                    "timestamp": 1.1235889447422234
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 6.2403191565519e-36,
-                    "timestamp": 1.1771009448500696
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C4 To C1": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 18
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -2.2542235108498663e-32,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.538844153951763,
-                    "heading": 2.1223723168001845e-18,
-                    "angularVelocity": 2.1422107346760384e-17,
-                    "velocityX": -4.524824041974102e-19,
-                    "velocityY": 0.8260917454276265,
-                    "timestamp": 0.09907392781100399
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.70253245874193,
-                    "heading": 6.368502121517421e-18,
-                    "angularVelocity": 4.285819588010552e-17,
-                    "velocityX": -9.048618887508432e-19,
-                    "velocityY": 1.6521834594306501,
-                    "timestamp": 0.19814785562200798
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.9480649081437846,
-                    "heading": 1.2741259045974704e-17,
-                    "angularVelocity": 6.432324896396774e-17,
-                    "velocityX": -1.3570355356096521e-18,
-                    "velocityY": 2.47827511058448,
-                    "timestamp": 0.297221783433012
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.2754414834771923,
-                    "heading": 2.1247069179422305e-17,
-                    "angularVelocity": 8.585316360490521e-17,
-                    "velocityX": -1.8085916683463265e-18,
-                    "velocityY": 3.3043665731908773,
-                    "timestamp": 0.39629571124401597
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.6502322649266867,
-                    "heading": 5.059469084858907e-18,
-                    "angularVelocity": -1.633891019789118e-16,
-                    "velocityX": -2.0715339797290713e-18,
-                    "velocityY": 3.782940575087071,
-                    "timestamp": 0.49536963905501996
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.025023046376204,
-                    "heading": -4.370025378852898e-19,
-                    "angularVelocity": -5.5478487063848375e-17,
-                    "velocityX": -2.0691441879135133e-18,
-                    "velocityY": 3.7829405750873115,
-                    "timestamp": 0.594443566866024
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.3998138278257235,
-                    "heading": 3.678666355048194e-17,
-                    "angularVelocity": 3.7571606285132846e-16,
-                    "velocityX": -2.072328507072934e-18,
-                    "velocityY": 3.782940575087312,
-                    "timestamp": 0.693517494677028
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.774604609275241,
-                    "heading": 5.708604515927294e-17,
-                    "angularVelocity": 2.04891257037182e-16,
-                    "velocityX": -2.07516702935077e-18,
-                    "velocityY": 3.782940575087312,
-                    "timestamp": 0.7925914224880319
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.14939539072476,
-                    "heading": 3.2711500565517004e-17,
-                    "angularVelocity": -2.460238039643844e-16,
-                    "velocityX": -2.078706211560206e-18,
-                    "velocityY": 3.782940575087312,
-                    "timestamp": 0.8916653502990359
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.524186172174278,
-                    "heading": 3.4723638610714384e-17,
-                    "angularVelocity": 2.0309460720999983e-17,
-                    "velocityX": -2.0667219645048642e-18,
-                    "velocityY": 3.782940575087312,
-                    "timestamp": 0.9907392781100399
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.898976953623796,
-                    "heading": 1.3963468905806427e-18,
-                    "angularVelocity": -3.363881139719203e-16,
-                    "velocityX": -2.078319856885538e-18,
-                    "velocityY": 3.782940575087312,
-                    "timestamp": 1.089813205921044
-                },
-                {
-                    "x": 7.68,
-                    "y": 6.273767735073314,
-                    "heading": -1.338725801529433e-17,
-                    "angularVelocity": -1.492179146704526e-16,
-                    "velocityX": -2.0709974384556896e-18,
-                    "velocityY": 3.7829405750873115,
-                    "timestamp": 1.188887133732048
-                },
-                {
-                    "x": 7.68,
-                    "y": 6.648558516522808,
-                    "heading": -2.1247111946894708e-17,
-                    "angularVelocity": -7.933322222365232e-17,
-                    "velocityX": -2.070808166139798e-18,
-                    "velocityY": 3.782940575087071,
-                    "timestamp": 1.287961061543052
-                },
-                {
-                    "x": 7.68,
-                    "y": 6.975935091856216,
-                    "heading": -1.273985947141283e-17,
-                    "angularVelocity": 8.586772184549593e-17,
-                    "velocityX": -1.8108827209778567e-18,
-                    "velocityY": 3.304366573190878,
-                    "timestamp": 1.387034989354056
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.22146754125807,
-                    "heading": -6.367750753434283e-18,
-                    "angularVelocity": 6.431670631033216e-17,
-                    "velocityX": -1.3581621409711223e-18,
-                    "velocityY": 2.47827511058448,
-                    "timestamp": 1.4861089171650599
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.385155846048237,
-                    "heading": -2.122107432521882e-18,
-                    "angularVelocity": 4.285328556951628e-17,
-                    "velocityX": -9.05441456368543e-19,
-                    "velocityY": 1.65218345943065,
-                    "timestamp": 1.5851828449760639
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "angularVelocity": 2.141943374454498e-17,
-                    "velocityX": -4.527207369006345e-19,
-                    "velocityY": 0.8260917454276265,
-                    "timestamp": 1.6842567727870679
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": -3.1368402443636883e-34,
-                    "velocityY": 0,
-                    "timestamp": 1.7833307005980719
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "C4 To C2": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 14
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "angularVelocity": -1.7120676318191985e-39,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.5334453720055388,
-                    "heading": -1.6066276958150233e-18,
-                    "angularVelocity": -1.677931007769515e-17,
-                    "velocityX": 1.4398812722194594e-33,
-                    "velocityY": 0.7983807350946674,
-                    "timestamp": 0.09575052183150976
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.686336112982689,
-                    "heading": -4.816833322743681e-18,
-                    "angularVelocity": -3.3526769001814837e-17,
-                    "velocityX": 1.5865173753469157e-33,
-                    "velocityY": 1.5967614385035827,
-                    "timestamp": 0.1915010436630195
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.915672216863599,
-                    "heading": -9.624489104750263e-18,
-                    "angularVelocity": -5.0210230614541765e-17,
-                    "velocityX": 1.0182538660036936e-34,
-                    "velocityY": 2.395142078541023,
-                    "timestamp": 0.28725156549452924
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.221453665444745,
-                    "heading": -1.6009153132358185e-17,
-                    "angularVelocity": -6.66802008672839e-17,
-                    "velocityX": 3.094783239015371e-34,
-                    "velocityY": 3.193522528464369,
-                    "timestamp": 0.383002087326039
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.5836721992668203,
-                    "heading": -3.473454086768081e-17,
-                    "angularVelocity": -1.9556434134398854e-16,
-                    "velocityX": 3.082893791022991e-33,
-                    "velocityY": 3.7829405719528495,
-                    "timestamp": 0.4787526091575488
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.9458907330889397,
-                    "heading": -3.402618832360637e-17,
-                    "angularVelocity": 7.397897479044197e-18,
-                    "velocityX": 3.015867889332214e-33,
-                    "velocityY": 3.782940571953311,
-                    "timestamp": 0.5745031309890586
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.308109266911059,
-                    "heading": -1.7375698441954866e-17,
-                    "angularVelocity": 1.7389450796885638e-16,
-                    "velocityX": 5.330140345912211e-33,
-                    "velocityY": 3.782940571953311,
-                    "timestamp": 0.6702536528205684
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.670327800733179,
-                    "heading": 7.579747606314133e-18,
-                    "angularVelocity": 2.6062986990486198e-16,
-                    "velocityX": 7.600730979130334e-33,
-                    "velocityY": 3.782940571953311,
-                    "timestamp": 0.7660041746520782
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.032546334555254,
-                    "heading": 1.6243735534665814e-17,
-                    "angularVelocity": 9.048502047432954e-17,
-                    "velocityX": -2.799411465702586e-33,
-                    "velocityY": 3.7829405719528495,
-                    "timestamp": 0.8617546964835879
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.3383277831364,
-                    "heading": 9.733952786983855e-18,
-                    "angularVelocity": -6.798691664538921e-17,
-                    "velocityX": -8.030123288193386e-33,
-                    "velocityY": 3.193522528464369,
-                    "timestamp": 0.9575052183150977
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.567663887017311,
-                    "heading": 4.864296415465134e-18,
-                    "angularVelocity": -5.08577528116881e-17,
-                    "velocityX": -5.179461626817983e-33,
-                    "velocityY": 2.395142078541023,
-                    "timestamp": 1.0532557401466074
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.720554627994461,
-                    "heading": 1.6208272352196832e-18,
-                    "angularVelocity": -3.3874167136237696e-17,
-                    "velocityX": -4.721587370568867e-33,
-                    "velocityY": 1.5967614385035827,
-                    "timestamp": 1.1490062619781172
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "angularVelocity": -1.69276073284528e-17,
-                    "velocityX": -2.4599412486527172e-33,
-                    "velocityY": 0.7983807350946674,
-                    "timestamp": 1.244756783809627
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": -3.907521125363084e-43,
-                    "velocityY": 8.448865646535191e-39,
-                    "timestamp": 1.3405073056411367
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "C4 To C3": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 9
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": -2.24426408200091e-39,
-                    "angularVelocity": 3.072610310721988e-47,
-                    "velocityX": -5.239815815905201e-40,
-                    "velocityY": -4.5917748078995606e-40,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.540500005575846,
-                    "heading": -7.160737227180709e-23,
-                    "angularVelocity": -7.15564807370433e-22,
-                    "velocityX": 1.97902852084548e-33,
-                    "velocityY": 0.8344065481228216,
-                    "timestamp": 0.10007112931184187
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.7075000136298466,
-                    "heading": 1.9234971876596208e-22,
-                    "angularVelocity": 2.6376946581601568e-21,
-                    "velocityX": 2.759296119129438e-33,
-                    "velocityY": 1.6688130652907365,
-                    "timestamp": 0.20014225862368373
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.9580000179666155,
-                    "heading": 1.2715262688933367e-21,
-                    "angularVelocity": 1.0784094425251139e-20,
-                    "velocityX": 3.360924516770788e-33,
-                    "velocityY": 2.5032195205488357,
-                    "timestamp": 0.3002133879355256
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.2919999999999994,
-                    "heading": 4.5990005279924634e-21,
-                    "angularVelocity": 3.325109025879841e-20,
-                    "velocityX": 4.212493187097162e-33,
-                    "velocityY": 3.3376257900775026,
-                    "timestamp": 0.40028451724736747
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.6259999820333837,
-                    "heading": 2.8774590551268706e-21,
-                    "angularVelocity": -1.7203177491039364e-20,
-                    "velocityX": -1.8223734723751632e-33,
-                    "velocityY": 3.3376257900775026,
-                    "timestamp": 0.5003556465592094
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.8764999863701526,
-                    "heading": 1.4966782173181084e-21,
-                    "angularVelocity": -1.3797993485663648e-20,
-                    "velocityX": -1.777649344678044e-33,
-                    "velocityY": 2.5032195205488357,
-                    "timestamp": 0.6004267758710513
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.043499994424153,
-                    "heading": 5.147844205579642e-22,
-                    "angularVelocity": -9.81195850164629e-21,
-                    "velocityX": -2.007686184897088e-33,
-                    "velocityY": 1.6688130652907365,
-                    "timestamp": 0.7004979051828932
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": -4.8498876719579466e-42,
-                    "angularVelocity": -5.144185056490002e-21,
-                    "velocityX": -2.233495211598662e-33,
-                    "velocityY": 0.8344065481228216,
-                    "timestamp": 0.8005690344947352
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 9.449822208481667e-41,
-                    "angularVelocity": 9.927610425813228e-40,
-                    "velocityX": 1.9465664275137074e-41,
-                    "velocityY": 5.142787784847508e-39,
-                    "timestamp": 0.9006401638065771
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "C4 To C5": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 9
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": -4.712717929333765e-40,
-                    "angularVelocity": 9.927649781151023e-40,
-                    "velocityX": 1.7772849076267516e-40,
-                    "velocityY": -4.543888050052464e-40,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.3734999944241535,
-                    "heading": 2.8227518572323466e-21,
-                    "angularVelocity": 2.820745460648545e-20,
-                    "velocityX": 2.2465911667965736e-32,
-                    "velocityY": -0.8344065481228212,
-                    "timestamp": 0.10007112931184181
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.206499986370153,
-                    "heading": 8.436925006375396e-21,
-                    "angularVelocity": 5.6101826293588e-20,
-                    "velocityX": 4.4654374072917263e-32,
-                    "velocityY": -1.6688130652907358,
-                    "timestamp": 0.20014225862368362
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.955999982033384,
-                    "heading": 1.682221146414935e-20,
-                    "angularVelocity": 8.379326253074398e-20,
-                    "velocityX": 6.797738711780288e-32,
-                    "velocityY": -2.503219520548835,
-                    "timestamp": 0.30021338793552543
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.6219999999999999,
-                    "heading": 2.78675063552584e-20,
-                    "angularVelocity": 1.1037443968745167e-19,
-                    "velocityX": 8.936946075377416e-32,
-                    "velocityY": -3.3376257900775013,
-                    "timestamp": 0.40028451724736724
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.288000017966616,
-                    "heading": 1.0287640204896551e-20,
-                    "angularVelocity": -1.75673704816788e-19,
-                    "velocityX": 9.060995032099377e-32,
-                    "velocityY": -3.3376257900775013,
-                    "timestamp": 0.500355646559209
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.037500013629847,
-                    "heading": 3.2969803204781124e-21,
-                    "angularVelocity": -6.985690973056607e-20,
-                    "velocityX": 6.916062716279199e-32,
-                    "velocityY": -2.503219520548835,
-                    "timestamp": 0.6004267758710509
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.8705000055758465,
-                    "heading": 6.18120991020044e-22,
-                    "angularVelocity": -2.676955216055553e-20,
-                    "velocityX": 4.34564572466329e-32,
-                    "velocityY": -1.6688130652907358,
-                    "timestamp": 0.7004979051828927
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": -2.2737142324276756e-40,
-                    "angularVelocity": -6.1768164103594875e-21,
-                    "velocityX": 2.131975080632403e-32,
-                    "velocityY": -0.8344065481228212,
-                    "timestamp": 0.8005690344947345
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 2.0993519451368808e-41,
-                    "angularVelocity": 2.4819025971975228e-39,
-                    "velocityX": -8.732390511178075e-43,
-                    "velocityY": -2.571393892423754e-39,
-                    "timestamp": 0.9006401638065763
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "C5 To W1": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 16
-                },
-                {
-                    "x": 4.8543877601623535,
-                    "y": 4.064273357391357,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 17
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "angularVelocity": -3.516607019090483e-32,
-                    "velocityX": -4.092268936373955e-33,
-                    "velocityY": -4.396806616029776e-33,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.6580594556627535,
-                    "y": 0.8642766421574168,
-                    "heading": -0.06977499787943942,
-                    "angularVelocity": -0.7009716360726537,
-                    "velocityX": -0.22041848409624198,
-                    "velocityY": 0.7763344454252913,
-                    "timestamp": 0.09954040119279155
-                },
-                {
-                    "x": 7.613877684624128,
-                    "y": 1.0194223152597515,
-                    "heading": -0.19807907998906352,
-                    "angularVelocity": -1.2889648883484262,
-                    "velocityX": -0.4438576749661031,
-                    "velocityY": 1.558620130552277,
-                    "timestamp": 0.1990808023855831
-                },
-                {
-                    "x": 7.54706531781781,
-                    "y": 1.253315507091588,
-                    "heading": -0.3641253773176819,
-                    "angularVelocity": -1.6681296773861405,
-                    "velocityX": -0.6712085344815377,
-                    "velocityY": 2.349731255139591,
-                    "timestamp": 0.2986212035783746
-                },
-                {
-                    "x": 7.457648537893544,
-                    "y": 1.5658706931334108,
-                    "heading": -0.49008580394214096,
-                    "angularVelocity": -1.265420122031624,
-                    "velocityX": -0.8982963585919527,
-                    "velocityY": 3.139983185686188,
-                    "timestamp": 0.3981616047711662
-                },
-                {
-                    "x": 7.353528135913809,
-                    "y": 1.9277448806018234,
-                    "heading": -0.490085841920944,
-                    "angularVelocity": -3.8154159059291386e-7,
-                    "velocityX": -1.0460114760646044,
-                    "velocityY": 3.6354503611807685,
-                    "timestamp": 0.49770200596395775
-                },
-                {
-                    "x": 7.249407681423134,
-                    "y": 2.2896190529615703,
-                    "heading": -0.4900858798995876,
-                    "angularVelocity": -3.8153998933914064e-7,
-                    "velocityX": -1.0460120035985523,
-                    "velocityY": 3.635450209396512,
-                    "timestamp": 0.5972424071567493
-                },
-                {
-                    "x": 7.145287117295142,
-                    "y": 2.651493193775847,
-                    "heading": -0.4900859178782245,
-                    "angularVelocity": -3.8153992177326955e-7,
-                    "velocityX": -1.0460131050338848,
-                    "velocityY": 3.6354498924852887,
-                    "timestamp": 0.6967828083495408
-                },
-                {
-                    "x": 7.025437006860337,
-                    "y": 3.00846649318931,
-                    "heading": -0.49008595631381496,
-                    "angularVelocity": -3.8613055604326664e-7,
-                    "velocityX": -1.204034834083884,
-                    "velocityY": 3.586215196401226,
-                    "timestamp": 0.7963232095423323
-                },
-                {
-                    "x": 6.830624533504276,
-                    "y": 3.3307118703307705,
-                    "heading": -0.49008600153372534,
-                    "angularVelocity": -4.542870012894294e-7,
-                    "velocityX": -1.9571196320451347,
-                    "velocityY": 3.237332513029861,
-                    "timestamp": 0.8958636107351238
-                },
-                {
-                    "x": 6.570226686668516,
-                    "y": 3.602717269957059,
-                    "heading": -0.4900860357215963,
-                    "angularVelocity": -3.4345723556551647e-7,
-                    "velocityX": -2.6160015804177474,
-                    "velocityY": 2.7326130532612987,
-                    "timestamp": 0.9954040119279153
-                },
-                {
-                    "x": 6.270670526052627,
-                    "y": 3.8032215967371097,
-                    "heading": -0.4416564414377051,
-                    "angularVelocity": 0.4865320382835506,
-                    "velocityX": -3.009392739292897,
-                    "velocityY": 2.0143009710369753,
-                    "timestamp": 1.094944413120707
-                },
-                {
-                    "x": 5.9498699270968665,
-                    "y": 3.9255658761230467,
-                    "heading": -0.3340733830984328,
-                    "angularVelocity": 1.0807979177309486,
-                    "velocityX": -3.222818022748657,
-                    "velocityY": 1.229091684581206,
-                    "timestamp": 1.1944848143134985
-                },
-                {
-                    "x": 5.637666976812727,
-                    "y": 3.9694128449739723,
-                    "heading": -0.13726885518093873,
-                    "angularVelocity": 1.977132154976145,
-                    "velocityX": -3.136444564649281,
-                    "velocityY": 0.4404941945733328,
-                    "timestamp": 1.2940252155062901
-                },
-                {
-                    "x": 5.333615486534618,
-                    "y": 3.942111372133816,
-                    "heading": 0.08955200005531357,
-                    "angularVelocity": 2.278681344642582,
-                    "velocityX": -3.0545535946677287,
-                    "velocityY": -0.2742752943830166,
-                    "timestamp": 1.3935656166990817
-                },
-                {
-                    "x": 5.078082684497777,
-                    "y": 3.9650753252247246,
-                    "heading": 0.19884455131252843,
-                    "angularVelocity": 1.0979717777662474,
-                    "velocityX": -2.5671265031564414,
-                    "velocityY": 0.2306998245509574,
-                    "timestamp": 1.4931060178918734
-                },
-                {
-                    "x": 4.8543877601623535,
-                    "y": 4.064273357391357,
-                    "heading": 0.3064368289582943,
-                    "angularVelocity": 1.0808905364705061,
-                    "velocityX": -2.2472777048805277,
-                    "velocityY": 0.9965605018459247,
-                    "timestamp": 1.592646419084665
-                },
-                {
-                    "x": 4.700013932982481,
-                    "y": 4.187105651970831,
-                    "heading": 0.3888797273509616,
-                    "angularVelocity": 1.0676410431648378,
-                    "velocityX": -1.99915137751061,
-                    "velocityY": 1.5906864226746236,
-                    "timestamp": 1.6698660978526145
-                },
-                {
-                    "x": 4.564169886311024,
-                    "y": 4.35607150380987,
-                    "heading": 0.4692148602751942,
-                    "angularVelocity": 1.040345339504005,
-                    "velocityX": -1.7591894817340108,
-                    "velocityY": 2.1881190719116157,
-                    "timestamp": 1.747085776620564
-                },
-                {
-                    "x": 4.445019228108457,
-                    "y": 4.571847534724939,
-                    "heading": 0.5443267073647724,
-                    "angularVelocity": 0.9727034389160626,
-                    "velocityX": -1.543008985580261,
-                    "velocityY": 2.7943140188848803,
-                    "timestamp": 1.8243054553885136
-                },
-                {
-                    "x": 4.302573716554072,
-                    "y": 4.826880596020871,
-                    "heading": 0.5443268349443496,
-                    "angularVelocity": 0.0000016521640508021742,
-                    "velocityX": -1.8446788930894609,
-                    "velocityY": 3.302695185541032,
-                    "timestamp": 1.9015251341564632
-                },
-                {
-                    "x": 4.139811862406642,
-                    "y": 5.06945260129317,
-                    "heading": 0.544326895208066,
-                    "angularVelocity": 7.80419154390702e-7,
-                    "velocityX": -2.1077768872432143,
-                    "velocityY": 3.141323677365259,
-                    "timestamp": 1.9787448129244127
-                },
-                {
-                    "x": 3.9382711614032555,
-                    "y": 5.28090933180296,
-                    "heading": 0.5443268879285122,
-                    "angularVelocity": -9.42707078520697e-8,
-                    "velocityX": -2.60996554529876,
-                    "velocityY": 2.738378790013248,
-                    "timestamp": 2.0559644916923623
-                },
-                {
-                    "x": 3.721249786731846,
-                    "y": 5.476445328956613,
-                    "heading": 0.5443268807879367,
-                    "angularVelocity": -9.247093001843253e-8,
-                    "velocityX": -2.810441303745562,
-                    "velocityY": 2.53220422919983,
-                    "timestamp": 2.1331841704603116
-                },
-                {
-                    "x": 3.5042282470745802,
-                    "y": 5.671981142995692,
-                    "heading": 0.5443268736473624,
-                    "angularVelocity": -9.247091428352055e-8,
-                    "velocityX": -2.810443440323428,
-                    "velocityY": 2.53220185785385,
-                    "timestamp": 2.210403849228261
-                },
-                {
-                    "x": 3.287206691377032,
-                    "y": 5.867516939231946,
-                    "heading": 0.5443268665067905,
-                    "angularVelocity": -9.247088327815795e-8,
-                    "velocityX": -2.810443648046155,
-                    "velocityY": 2.5322016273060566,
-                    "timestamp": 2.2876235279962103
-                },
-                {
-                    "x": 3.0701851252220784,
-                    "y": 6.063052723861702,
-                    "heading": 0.5443268593662193,
-                    "angularVelocity": -9.247087442211068e-8,
-                    "velocityX": -2.8104437834702534,
-                    "velocityY": 2.532201477001163,
-                    "timestamp": 2.3648432067641596
-                },
-                {
-                    "x": 2.853163562032648,
-                    "y": 6.258588511782302,
-                    "heading": 0.5443268522252032,
-                    "angularVelocity": -9.247663534512907e-8,
-                    "velocityX": -2.8104437450665345,
-                    "velocityY": 2.5322015196178005,
-                    "timestamp": 2.442062885532109
-                },
-                {
-                    "x": 2.6687757934039245,
-                    "y": 6.424725614600548,
-                    "heading": 0.4938774787934042,
-                    "angularVelocity": -0.6533227570578579,
-                    "velocityX": -2.3878339248576954,
-                    "velocityY": 2.1514865830703513,
-                    "timestamp": 2.5192825643000583
-                },
-                {
-                    "x": 2.521265507868239,
-                    "y": 6.557635351096379,
-                    "heading": 0.4535239115954702,
-                    "angularVelocity": -0.522581391709739,
-                    "velocityX": -1.9102680545844264,
-                    "velocityY": 1.7211899688838848,
-                    "timestamp": 2.5965022430680076
-                },
-                {
-                    "x": 2.4106327602038906,
-                    "y": 6.6573176734827975,
-                    "heading": 0.42326153130587874,
-                    "angularVelocity": -0.391899846935805,
-                    "velocityX": -1.432701474928527,
-                    "velocityY": 1.2908927358526117,
-                    "timestamp": 2.673721921835957
-                },
-                {
-                    "x": 2.336877585524826,
-                    "y": 6.723772559032594,
-                    "heading": 0.40308732310971707,
-                    "angularVelocity": -0.2612573442164476,
-                    "velocityX": -0.9551344405446706,
-                    "velocityY": 0.8605952085024652,
-                    "timestamp": 2.7509416006039062
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "angularVelocity": -0.13063150832354717,
-                    "velocityX": -0.4775671967717696,
-                    "velocityY": 0.43029758084408004,
-                    "timestamp": 2.8281612793718556
-                },
-                {
-                    "x": 2.3,
-                    "y": 6.757,
-                    "heading": 0.393,
-                    "angularVelocity": -5.871044564465464e-32,
-                    "velocityX": 0,
-                    "velocityY": -1.090928833064942e-33,
-                    "timestamp": 2.905380958139805
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C5 To W2": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 17
-                },
-                {
-                    "x": 4.937438011169434,
-                    "y": 4.2165327072143555,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 13
-                },
-                {
-                    "x": 2.851226329803467,
-                    "y": 6.365692615509033,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 7
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "angularVelocity": -1.6344658510237988e-32,
-                    "velocityX": 1.0699335804910329e-32,
-                    "velocityY": 2.3378742346984854e-33,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.662237951731404,
-                    "y": 0.8490986120749341,
-                    "heading": -0.06160217222995063,
-                    "angularVelocity": -0.6883164695444892,
-                    "velocityX": -0.19846557213082008,
-                    "velocityY": 0.6938634772078657,
-                    "timestamp": 0.0894968738300239
-                },
-                {
-                    "x": 7.626433684211601,
-                    "y": 0.9738312717265619,
-                    "heading": -0.1754105069837687,
-                    "angularVelocity": -1.271645923297472,
-                    "velocityX": -0.4000616556484858,
-                    "velocityY": 1.3937096829608293,
-                    "timestamp": 0.1789937476600478
-                },
-                {
-                    "x": 7.572249558904937,
-                    "y": 1.161886141289839,
-                    "heading": -0.32706746606296133,
-                    "angularVelocity": -1.6945503523086813,
-                    "velocityX": -0.6054303685464192,
-                    "velocityY": 2.101245121929494,
-                    "timestamp": 0.2684906214900717
-                },
-                {
-                    "x": 7.499449827352778,
-                    "y": 1.4140559658703253,
-                    "heading": -0.4849879770703222,
-                    "angularVelocity": -1.7645366173045323,
-                    "velocityX": -0.8134332344437395,
-                    "velocityY": 2.8176383575075215,
-                    "timestamp": 0.3579874953200956
-                },
-                {
-                    "x": 7.40801256781841,
-                    "y": 1.7212752924886132,
-                    "heading": -0.5291211000298751,
-                    "angularVelocity": -0.4931247435901767,
-                    "velocityX": -1.0216810445024997,
-                    "velocityY": 3.432738077553094,
-                    "timestamp": 0.4474843691501195
-                },
-                {
-                    "x": 7.313226703333856,
-                    "y": 2.0462974852478544,
-                    "heading": -0.5291211457832385,
-                    "angularVelocity": -5.112286210233085e-7,
-                    "velocityX": -1.0590969318612773,
-                    "velocityY": 3.631659731227445,
-                    "timestamp": 0.5369812429801434
-                },
-                {
-                    "x": 7.218440797992873,
-                    "y": 2.371319666092203,
-                    "heading": -0.5291211915365511,
-                    "angularVelocity": -5.112280543858435e-7,
-                    "velocityX": -1.059097388373643,
-                    "velocityY": 3.6316595980955055,
-                    "timestamp": 0.6264781168101673
-                },
-                {
-                    "x": 7.1236547448927325,
-                    "y": 2.6963418038456624,
-                    "heading": -0.5291212372898596,
-                    "angularVelocity": -5.112280079310825e-7,
-                    "velocityX": -1.0590990393716089,
-                    "velocityY": 3.631659116616238,
-                    "timestamp": 0.7159749906401912
-                },
-                {
-                    "x": 7.010927268830994,
-                    "y": 3.0155851307576884,
-                    "heading": -0.5291212838180788,
-                    "angularVelocity": -5.198865285819975e-7,
-                    "velocityX": -1.2595688680236534,
-                    "velocityY": 3.567089142335248,
-                    "timestamp": 0.8054718644702151
-                },
-                {
-                    "x": 6.837725577807192,
-                    "y": 3.30648882172331,
-                    "heading": -0.5291213383056133,
-                    "angularVelocity": -6.088205344213367e-7,
-                    "velocityX": -1.9352820228419736,
-                    "velocityY": 3.2504341047500516,
-                    "timestamp": 0.894968738300239
-                },
-                {
-                    "x": 6.610788937088648,
-                    "y": 3.5577327922782125,
-                    "heading": -0.5291214021937537,
-                    "angularVelocity": -7.138589055640395e-7,
-                    "velocityX": -2.535693494161062,
-                    "velocityY": 2.8072932584447,
-                    "timestamp": 0.984465612130263
-                },
-                {
-                    "x": 6.351860794366729,
-                    "y": 3.7516776606884883,
-                    "heading": -0.4841599241371699,
-                    "angularVelocity": 0.5023804310972586,
-                    "velocityX": -2.8931529297178065,
-                    "velocityY": 2.167057463690003,
-                    "timestamp": 1.0739624859602868
-                },
-                {
-                    "x": 6.072750894879938,
-                    "y": 3.8834732684056306,
-                    "heading": -0.3889789775410024,
-                    "angularVelocity": 1.0635114113252733,
-                    "velocityX": -3.1186552953445856,
-                    "velocityY": 1.4726280603661466,
-                    "timestamp": 1.1634593597903107
-                },
-                {
-                    "x": 5.7916594444015015,
-                    "y": 3.950870142776961,
-                    "heading": -0.22937923493241555,
-                    "angularVelocity": 1.783299636942698,
-                    "velocityX": -3.1407963032574324,
-                    "velocityY": 0.7530640064515952,
-                    "timestamp": 1.2529562336203346
-                },
-                {
-                    "x": 5.498028130376245,
-                    "y": 3.9763121038871923,
-                    "heading": -0.08643118620464799,
-                    "angularVelocity": 1.5972406924431666,
-                    "velocityX": -3.280911404603181,
-                    "velocityY": 0.28427765151386064,
-                    "timestamp": 1.3424531074503585
-                },
-                {
-                    "x": 5.202994132142315,
-                    "y": 4.066458551574186,
-                    "heading": -0.003961984667318494,
-                    "angularVelocity": 0.9214757790753437,
-                    "velocityX": -3.2965844013084786,
-                    "velocityY": 1.0072580619765965,
-                    "timestamp": 1.4319499812803824
-                },
-                {
-                    "x": 4.937438011169434,
-                    "y": 4.2165327072143555,
-                    "heading": 0.07947932900863551,
-                    "angularVelocity": 0.9323377466170348,
-                    "velocityX": -2.967211139433056,
-                    "velocityY": 1.6768647799385226,
-                    "timestamp": 1.5214468551104063
-                },
-                {
-                    "x": 4.74667334234301,
-                    "y": 4.373655515147294,
-                    "heading": 0.13953709439935258,
-                    "angularVelocity": 0.8363208396611413,
-                    "velocityX": -2.6564502853656404,
-                    "velocityY": 2.187978154123957,
-                    "timestamp": 1.5932587274715584
-                },
-                {
-                    "x": 4.579753248112816,
-                    "y": 4.566217048926455,
-                    "heading": 0.1819755811120341,
-                    "angularVelocity": 0.5909675561619205,
-                    "velocityX": -2.324408050394913,
-                    "velocityY": 2.681472122196475,
-                    "timestamp": 1.6650705998327104
-                },
-                {
-                    "x": 4.438339398678278,
-                    "y": 4.792399271574889,
-                    "heading": 0.19511904158847473,
-                    "angularVelocity": 0.18302628861061485,
-                    "velocityX": -1.9692266025783034,
-                    "velocityY": 3.1496494271996025,
-                    "timestamp": 1.7368824721938625
-                },
-                {
-                    "x": 4.33242543936342,
-                    "y": 5.0425620352859015,
-                    "heading": 0.1951190405932856,
-                    "angularVelocity": -1.3858280983712203e-8,
-                    "velocityX": -1.4748809052380785,
-                    "velocityY": 3.483585032470794,
-                    "timestamp": 1.8086943445550145
-                },
-                {
-                    "x": 4.2346577127829645,
-                    "y": 5.296019422041832,
-                    "heading": 0.19511903926490842,
-                    "angularVelocity": -1.849801636869094e-8,
-                    "velocityX": -1.3614423822396335,
-                    "velocityY": 3.5294635611400946,
-                    "timestamp": 1.8805062169161666
-                },
-                {
-                    "x": 4.124562899973198,
-                    "y": 5.544370599514641,
-                    "heading": 0.19511903792287785,
-                    "angularVelocity": -1.8688143477674107e-8,
-                    "velocityX": -1.533100435761989,
-                    "velocityY": 3.4583581977060263,
-                    "timestamp": 1.9523180892773186
-                },
-                {
-                    "x": 3.9766612552398675,
-                    "y": 5.772239592130343,
-                    "heading": 0.1951190364645304,
-                    "angularVelocity": -2.030788798410176e-8,
-                    "velocityX": -2.059570929852819,
-                    "velocityY": 3.1731381611903378,
-                    "timestamp": 2.024129961638471
-                },
-                {
-                    "x": 3.7946576924754596,
-                    "y": 5.973917226463319,
-                    "heading": 0.19511901792814293,
-                    "angularVelocity": -2.581242745470272e-7,
-                    "velocityX": -2.534449482797023,
-                    "velocityY": 2.8084163203364336,
-                    "timestamp": 2.095941833999623
-                },
-                {
-                    "x": 3.5938514397947183,
-                    "y": 6.137095652578835,
-                    "heading": 0.18216101901891746,
-                    "angularVelocity": -0.1804436854683003,
-                    "velocityX": -2.7962820920593496,
-                    "velocityY": 2.272304296633136,
-                    "timestamp": 2.167753706360775
-                },
-                {
-                    "x": 3.3950104775935763,
-                    "y": 6.257338696148904,
-                    "heading": 0.16464788328752675,
-                    "angularVelocity": -0.2438752138826665,
-                    "velocityX": -2.768914883616214,
-                    "velocityY": 1.6744173298441403,
-                    "timestamp": 2.239565578721927
-                },
-                {
-                    "x": 3.203569657298003,
-                    "y": 6.335228188048846,
-                    "heading": 0.14501288414051958,
-                    "angularVelocity": -0.273422743362824,
-                    "velocityX": -2.6658658798476953,
-                    "velocityY": 1.0846325174230818,
-                    "timestamp": 2.311377451083079
-                },
-                {
-                    "x": 3.0218761949257575,
-                    "y": 6.3712387750954615,
-                    "heading": 0.12433283555566121,
-                    "angularVelocity": -0.2879753431418064,
-                    "velocityX": -2.530131249864694,
-                    "velocityY": 0.5014572919852904,
-                    "timestamp": 2.383189323444231
-                },
-                {
-                    "x": 2.851226329803467,
-                    "y": 6.365692615509033,
-                    "heading": 0.10320964185538832,
-                    "angularVelocity": -0.2941462603013797,
-                    "velocityX": -2.3763461320722596,
-                    "velocityY": -0.07723179195964242,
-                    "timestamp": 2.4550011958053832
-                },
-                {
-                    "x": 2.6537141886903095,
-                    "y": 6.292049225449601,
-                    "heading": 0.07629879670595036,
-                    "angularVelocity": -0.2952020622183997,
-                    "velocityX": -2.1666354603877345,
-                    "velocityY": -0.8078408720936208,
-                    "timestamp": 2.5461619585290816
-                },
-                {
-                    "x": 2.4816525262537397,
-                    "y": 6.153957047067382,
-                    "heading": 0.05014624127109134,
-                    "angularVelocity": -0.2868839032657673,
-                    "velocityX": -1.8874530806425645,
-                    "velocityY": -1.5148203487586651,
-                    "timestamp": 2.63732272125278
-                },
-                {
-                    "x": 2.3499481448537543,
-                    "y": 5.9595438725680525,
-                    "heading": 0.026875208437583037,
-                    "angularVelocity": -0.25527466135886906,
-                    "velocityX": -1.4447485679685645,
-                    "velocityY": -2.1326409377308697,
-                    "timestamp": 2.728483483976478
-                },
-                {
-                    "x": 2.2874888909600317,
-                    "y": 5.764180997032623,
-                    "heading": 0.012070368781366221,
-                    "angularVelocity": -0.1624036396129025,
-                    "velocityX": -0.6851550165615855,
-                    "velocityY": -2.1430588083994038,
-                    "timestamp": 2.8196442467001765
-                },
-                {
-                    "x": 2.260118679493569,
-                    "y": 5.628541423814637,
-                    "heading": 0.0037496226116310134,
-                    "angularVelocity": -0.09127552162935264,
-                    "velocityX": -0.3002411415689918,
-                    "velocityY": -1.487916173201615,
-                    "timestamp": 2.910805009423875
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": -4.5794503744404666e-32,
-                    "angularVelocity": -0.04113197936919252,
-                    "velocityX": -0.11099818815950499,
-                    "velocityY": -0.7518741810265671,
-                    "timestamp": 3.001965772147573
-                },
-                {
-                    "x": 2.25,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": -2.1498446437166114e-31,
-                    "velocityX": 0,
-                    "velocityY": -6.253874378421497e-34,
-                    "timestamp": 3.0931265348712715
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C5 To W3": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 19
-                },
-                {
-                    "x": 2.8888590335845947,
-                    "y": 2.6108903884887695,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 12
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "angularVelocity": -1.5069359770438582e-30,
-                    "velocityX": 5.69693995403372e-32,
-                    "velocityY": 3.042278119963715e-31,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.627290492953592,
-                    "y": 0.8035142159340071,
-                    "heading": 0.015175150751654183,
-                    "angularVelocity": 0.18617297608634506,
-                    "velocityX": -0.6466549133823012,
-                    "velocityY": 0.20260100070713255,
-                    "timestamp": 0.08151102845675925
-                },
-                {
-                    "x": 7.521871327482097,
-                    "y": 0.8365426259859824,
-                    "heading": 0.045515680376982795,
-                    "angularVelocity": 0.3722260680519286,
-                    "velocityX": -1.2933116839203953,
-                    "velocityY": 0.4052017337690261,
-                    "timestamp": 0.1630220569135185
-                },
-                {
-                    "x": 7.363742244741542,
-                    "y": 0.8860852747673063,
-                    "heading": 0.09100430929463568,
-                    "angularVelocity": 0.5580671693007231,
-                    "velocityX": -1.9399716300284648,
-                    "velocityY": 0.6078030141357551,
-                    "timestamp": 0.24453308537027776
-                },
-                {
-                    "x": 7.152902908699907,
-                    "y": 0.95214234591484,
-                    "heading": 0.15161661460632753,
-                    "angularVelocity": 0.7436086436309897,
-                    "velocityX": -2.5866356986709165,
-                    "velocityY": 0.8104065474106494,
-                    "timestamp": 0.326044113827037
-                },
-                {
-                    "x": 6.889352971860368,
-                    "y": 1.0347142415767305,
-                    "heading": 0.22732297101557483,
-                    "angularVelocity": 0.9287866665726976,
-                    "velocityX": -3.233304030501219,
-                    "velocityY": 1.0130150143510155,
-                    "timestamp": 0.4075551422837963
-                },
-                {
-                    "x": 6.595109187456524,
-                    "y": 1.1269159867790934,
-                    "heading": 0.22732298101704992,
-                    "angularVelocity": 1.22700882310899e-7,
-                    "velocityX": -3.6098647014365244,
-                    "velocityY": 1.1311566906713055,
-                    "timestamp": 0.4890661707405556
-                },
-                {
-                    "x": 6.300865420236581,
-                    "y": 1.219117786820698,
-                    "heading": 0.2273229910184091,
-                    "angularVelocity": 1.2269945998464997e-7,
-                    "velocityX": -3.6098644906196586,
-                    "velocityY": 1.131157363454394,
-                    "timestamp": 0.5705771991973149
-                },
-                {
-                    "x": 6.006621672014901,
-                    "y": 1.3113196474914972,
-                    "heading": 0.2273230010197575,
-                    "angularVelocity": 1.2269932759504928e-7,
-                    "velocityX": -3.6098642575436695,
-                    "velocityY": 1.1311581072702415,
-                    "timestamp": 0.6520882276540741
-                },
-                {
-                    "x": 5.7123779465456614,
-                    "y": 1.4035215807721424,
-                    "heading": 0.22732301102110228,
-                    "angularVelocity": 1.2269928353992546e-7,
-                    "velocityX": -3.6098639784103828,
-                    "velocityY": 1.131158998068056,
-                    "timestamp": 0.7335992561108334
-                },
-                {
-                    "x": 5.418134249112936,
-                    "y": 1.4957236035255936,
-                    "heading": 0.2273230210224565,
-                    "angularVelocity": 1.2269939906393907e-7,
-                    "velocityX": -3.60986363445063,
-                    "velocityY": 1.1311600957453605,
-                    "timestamp": 0.8151102845675927
-                },
-                {
-                    "x": 5.123890658648026,
-                    "y": 1.5879259676440571,
-                    "heading": 0.22732303102385681,
-                    "angularVelocity": 1.2269996504792678e-7,
-                    "velocityX": -3.609862322139673,
-                    "velocityY": 1.1311642837064038,
-                    "timestamp": 0.896621313024352
-                },
-                {
-                    "x": 4.838467298400658,
-                    "y": 1.7046053225590203,
-                    "heading": 0.22732304878104165,
-                    "angularVelocity": 2.1785008908970575e-7,
-                    "velocityX": -3.5016532811726435,
-                    "velocityY": 1.4314548242617389,
-                    "timestamp": 0.9781323414811113
-                },
-                {
-                    "x": 4.5530441892872355,
-                    "y": 1.8212852918009321,
-                    "heading": 0.2273230665381978,
-                    "angularVelocity": 2.1784973753126392e-7,
-                    "velocityX": -3.5016502001914627,
-                    "velocityY": 1.4314623609958343,
-                    "timestamp": 1.0596433699378704
-                },
-                {
-                    "x": 4.267621124217833,
-                    "y": 1.9379653687834795,
-                    "heading": 0.2273230842953322,
-                    "angularVelocity": 2.1784947052459537e-7,
-                    "velocityX": -3.5016496598471556,
-                    "velocityY": 1.4314636827879597,
-                    "timestamp": 1.1411543983946297
-                },
-                {
-                    "x": 3.9821980933555543,
-                    "y": 2.054645529443536,
-                    "heading": 0.22732310205245027,
-                    "angularVelocity": 2.1784927045604472e-7,
-                    "velocityX": -3.5016492401846184,
-                    "velocityY": 1.4314647093669521,
-                    "timestamp": 1.222665426851389
-                },
-                {
-                    "x": 3.6967750938784922,
-                    "y": 2.171325766878081,
-                    "heading": 0.22732311980956,
-                    "angularVelocity": 2.1784916783717796e-7,
-                    "velocityX": -3.501648855142053,
-                    "velocityY": 1.4314656512577588,
-                    "timestamp": 1.3041764553081483
-                },
-                {
-                    "x": 3.411352191599502,
-                    "y": 2.288006242078097,
-                    "heading": 0.2273231375666713,
-                    "angularVelocity": 2.178491871140625e-7,
-                    "velocityX": -3.501647662689034,
-                    "velocityY": 1.4314685682307748,
-                    "timestamp": 1.3856874837649076
-                },
-                {
-                    "x": 3.135544385824121,
-                    "y": 2.425885248705818,
-                    "heading": 0.22732315564878777,
-                    "angularVelocity": 2.218364413735132e-7,
-                    "velocityX": -3.3836869807340535,
-                    "velocityY": 1.6915380561155315,
-                    "timestamp": 1.4671985122216669
-                },
-                {
-                    "x": 2.8888590335845947,
-                    "y": 2.6108903884887695,
-                    "heading": 0.22732317584620773,
-                    "angularVelocity": 2.4778757367040615e-7,
-                    "velocityX": -3.026404609412922,
-                    "velocityY": 2.2696945834893776,
-                    "timestamp": 1.5487095406784261
-                },
-                {
-                    "x": 2.714424659127437,
-                    "y": 2.7861164379371415,
-                    "heading": 0.22732319539367835,
-                    "angularVelocity": 2.990800632496739e-7,
-                    "velocityX": -2.668879509458687,
-                    "velocityY": 2.6809922892290525,
-                    "timestamp": 1.6140681954393303
-                },
-                {
-                    "x": 2.566977650329029,
-                    "y": 2.984587845024715,
-                    "heading": 0.2273232142228265,
-                    "angularVelocity": 2.880895914276183e-7,
-                    "velocityX": -2.2559676195571057,
-                    "velocityY": 3.0366507360596735,
-                    "timestamp": 1.6794268502002345
-                },
-                {
-                    "x": 2.4495779823740462,
-                    "y": 3.20218572489824,
-                    "heading": 0.22732323403642601,
-                    "angularVelocity": 3.0315188694924184e-7,
-                    "velocityX": -1.7962375202558922,
-                    "velocityY": 3.3292894517113587,
-                    "timestamp": 1.7447855049611387
-                },
-                {
-                    "x": 2.3646621591221844,
-                    "y": 3.4343941523127826,
-                    "heading": 0.22732276450933128,
-                    "angularVelocity": -0.000007183855179950579,
-                    "velocityX": -1.2992284428512828,
-                    "velocityY": 3.5528336417572115,
-                    "timestamp": 1.810144159722043
-                },
-                {
-                    "x": 2.3096843159184894,
-                    "y": 3.656196634862642,
-                    "heading": 0.16843360905648927,
-                    "angularVelocity": -0.9010154151467895,
-                    "velocityX": -0.8411715847703609,
-                    "velocityY": 3.3936206820850954,
-                    "timestamp": 1.875502814482947
-                },
-                {
-                    "x": 2.277656929328602,
-                    "y": 3.853541817964372,
-                    "heading": 0.0672675417750508,
-                    "angularVelocity": -1.547860304829062,
-                    "velocityX": -0.49002518039951914,
-                    "velocityY": 3.0194192922676324,
-                    "timestamp": 1.9408614692438513
-                },
-                {
-                    "x": 2.265357919525011,
-                    "y": 4.021569174077037,
-                    "heading": -0.049806740829473055,
-                    "angularVelocity": -1.791259061753831,
-                    "velocityX": -0.188177217670429,
-                    "velocityY": 2.570850895376378,
-                    "timestamp": 2.0062201240047557
-                },
-                {
-                    "x": 2.269649055536998,
-                    "y": 4.158080976089645,
-                    "heading": -0.1685192187929243,
-                    "angularVelocity": -1.8163237661135343,
-                    "velocityX": 0.06565520706762865,
-                    "velocityY": 2.088656850603768,
-                    "timestamp": 2.07157877876566
-                },
-                {
-                    "x": 2.2810300353091644,
-                    "y": 4.259951613943982,
-                    "heading": -0.2719091294332062,
-                    "angularVelocity": -1.581885536330179,
-                    "velocityX": 0.1741311814602323,
-                    "velocityY": 1.5586403702310236,
-                    "timestamp": 2.1369374335265645
-                },
-                {
-                    "x": 2.2926691286250658,
-                    "y": 4.326989538481201,
-                    "heading": -0.34849979264864867,
-                    "angularVelocity": -1.1718518916219969,
-                    "velocityX": 0.1780803683686499,
-                    "velocityY": 1.0256931508529106,
-                    "timestamp": 2.202296088287469
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": -0.6349611616573703,
-                    "velocityX": 0.11216374329844404,
-                    "velocityY": 0.5050664160631704,
-                    "timestamp": 2.2676547430483733
-                },
-                {
-                    "x": 2.3,
-                    "y": 4.36,
-                    "heading": -0.39,
-                    "angularVelocity": -1.761626207104486e-30,
-                    "velocityX": 0,
-                    "velocityY": -1.839087679439853e-31,
-                    "timestamp": 2.3330133978092777
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C5 To S3": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 14
-                },
-                {
-                    "x": 4.536026477813721,
-                    "y": 1.628126621246338,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 12
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "angularVelocity": -3.982415645335998e-31,
-                    "velocityX": 3.346280928312519e-33,
-                    "velocityY": 3.541078180282768e-38,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.629395539536042,
-                    "y": 0.7881897280541793,
-                    "heading": -0.02207380596023684,
-                    "angularVelocity": -0.2822732032732244,
-                    "velocityX": -0.6471146471374848,
-                    "velocityY": 0.015213885156899113,
-                    "timestamp": 0.0782001468948451
-                },
-                {
-                    "x": 7.528184091504648,
-                    "y": 0.7905692108612611,
-                    "heading": -0.06614402373093474,
-                    "angularVelocity": -0.5635567133951742,
-                    "velocityX": -1.2942616101155584,
-                    "velocityY": 0.030428111729043,
-                    "timestamp": 0.1564002937896902
-                },
-                {
-                    "x": 7.376360973312952,
-                    "y": 0.7941378827255743,
-                    "heading": -0.13206235338942335,
-                    "angularVelocity": -0.8429438086302324,
-                    "velocityX": -1.9414684526853105,
-                    "velocityY": 0.04563510435477082,
-                    "timestamp": 0.23460044068453528
-                },
-                {
-                    "x": 7.173916466766183,
-                    "y": 0.7988917309385671,
-                    "heading": -0.21951772550448787,
-                    "angularVelocity": -1.1183530413716716,
-                    "velocityX": -2.588799568612108,
-                    "velocityY": 0.060790783668257076,
-                    "timestamp": 0.3128005875793804
-                },
-                {
-                    "x": 6.920824289019536,
-                    "y": 0.8048108023879232,
-                    "heading": -0.3276760191057486,
-                    "angularVelocity": -1.383095785572821,
-                    "velocityX": -3.236466781667369,
-                    "velocityY": 0.07569130857196414,
-                    "timestamp": 0.3910007344742255
-                },
-                {
-                    "x": 6.625087122175782,
-                    "y": 0.8120803100358527,
-                    "heading": -0.3276760358371292,
-                    "angularVelocity": -2.1395587179409026e-7,
-                    "velocityX": -3.781798098684327,
-                    "velocityY": 0.09296028123113304,
-                    "timestamp": 0.4692008813690706
-                },
-                {
-                    "x": 6.329343587367458,
-                    "y": 0.8190859886904246,
-                    "heading": -0.3276760513572638,
-                    "angularVelocity": -1.9846682128968708e-7,
-                    "velocityX": -3.78187953030339,
-                    "velocityY": 0.08958651527359804,
-                    "timestamp": 0.5474010282639157
-                },
-                {
-                    "x": 6.033600055760057,
-                    "y": 0.826091802470254,
-                    "heading": -0.3276760668773978,
-                    "angularVelocity": -1.9846681457625392e-7,
-                    "velocityX": -3.7818794893709464,
-                    "velocityY": 0.0895882432148227,
-                    "timestamp": 0.6256011751587608
-                },
-                {
-                    "x": 5.7383102527596765,
-                    "y": 0.8439033174946344,
-                    "heading": -0.3276760825327013,
-                    "angularVelocity": -2.0019532088764418e-7,
-                    "velocityX": -3.7760773441696487,
-                    "velocityY": 0.22776830647924015,
-                    "timestamp": 0.7038013220536059
-                },
-                {
-                    "x": 5.450465299571764,
-                    "y": 0.9121572529445121,
-                    "heading": -0.32767609968005607,
-                    "angularVelocity": -2.1927522433491614e-7,
-                    "velocityX": -3.680874839978332,
-                    "velocityY": 0.8728108342551084,
-                    "timestamp": 0.782001468948451
-                },
-                {
-                    "x": 5.178616904201158,
-                    "y": 1.028826716826592,
-                    "heading": -0.32767612118071976,
-                    "angularVelocity": -2.749440322201668e-7,
-                    "velocityX": -3.4763156613508643,
-                    "velocityY": 1.4919340757663395,
-                    "timestamp": 0.8602016158432961
-                },
-                {
-                    "x": 4.930841529412244,
-                    "y": 1.190445624246428,
-                    "heading": -0.3276761534331376,
-                    "angularVelocity": -4.1243423626599064e-7,
-                    "velocityX": -3.1684771017365216,
-                    "velocityY": 2.0667340642903906,
-                    "timestamp": 0.9384017627381412
-                },
-                {
-                    "x": 4.7145004488669215,
-                    "y": 1.3922123936122235,
-                    "heading": -0.32767621878386327,
-                    "angularVelocity": -8.356854591223944e-7,
-                    "velocityX": -2.7665047846524513,
-                    "velocityY": 2.5801328690255976,
-                    "timestamp": 1.0166019096329864
-                },
-                {
-                    "x": 4.536026477813721,
-                    "y": 1.628126621246338,
-                    "heading": -0.327696539002069,
-                    "angularVelocity": -0.00025984882823803823,
-                    "velocityX": -2.282271557538034,
-                    "velocityY": 3.016800313073996,
-                    "timestamp": 1.0948020565278314
-                },
-                {
-                    "x": 4.4347589269746495,
-                    "y": 1.8009845745755027,
-                    "heading": -0.3311064514524199,
-                    "angularVelocity": -0.06394544615832723,
-                    "velocityX": -1.899051313343304,
-                    "velocityY": 3.241572651564751,
-                    "timestamp": 1.1481273933038434
-                },
-                {
-                    "x": 4.355560152796487,
-                    "y": 1.9823487857199633,
-                    "heading": -0.3404702376957046,
-                    "angularVelocity": -0.17559732033810904,
-                    "velocityX": -1.4851996999249673,
-                    "velocityY": 3.4010889027473166,
-                    "timestamp": 1.2014527300798554
-                },
-                {
-                    "x": 4.29961502910342,
-                    "y": 2.1668323093896307,
-                    "heading": -0.3620606217709223,
-                    "angularVelocity": -0.40488040734269476,
-                    "velocityX": -1.049128370778876,
-                    "velocityY": 3.4595847832064672,
-                    "timestamp": 1.2547780668558675
-                },
-                {
-                    "x": 4.26624261518445,
-                    "y": 2.3445509425061104,
-                    "heading": -0.3930653835662206,
-                    "angularVelocity": -0.5814264600999738,
-                    "velocityX": -0.6258265945712352,
-                    "velocityY": 3.332724064417915,
-                    "timestamp": 1.3081034036318795
-                },
-                {
-                    "x": 4.250245555353669,
-                    "y": 2.506179624728893,
-                    "heading": -0.42827286207411436,
-                    "angularVelocity": -0.6602392152823773,
-                    "velocityX": -0.2999898509389163,
-                    "velocityY": 3.0309922448647635,
-                    "timestamp": 1.3614287404078915
-                },
-                {
-                    "x": 4.245253885327146,
-                    "y": 2.646807044638539,
-                    "heading": -0.46326114250173545,
-                    "angularVelocity": -0.6561286349527023,
-                    "velocityX": -0.09360784813172378,
-                    "velocityY": 2.6371595270071015,
-                    "timestamp": 1.4147540771839036
-                },
-                {
-                    "x": 4.246491895421447,
-                    "y": 2.764573131222025,
-                    "heading": -0.49520995766037457,
-                    "angularVelocity": -0.5991301150594935,
-                    "velocityX": 0.023216170194527277,
-                    "velocityY": 2.2084452476729117,
-                    "timestamp": 1.4680794139599156
-                },
-                {
-                    "x": 4.250760184591696,
-                    "y": 2.858861309855261,
-                    "heading": -0.522398988898447,
-                    "angularVelocity": -0.5098707834110707,
-                    "velocityX": 0.08004242313776105,
-                    "velocityY": 1.7681684605074017,
-                    "timestamp": 1.5214047507359276
-                },
-                {
-                    "x": 4.255888225984497,
-                    "y": 2.929511890736146,
-                    "heading": -0.5437259487715407,
-                    "angularVelocity": -0.3999404628668233,
-                    "velocityX": 0.09616519468543994,
-                    "velocityY": 1.324897040550944,
-                    "timestamp": 1.5747300875119397
-                },
-                {
-                    "x": 4.260343621579396,
-                    "y": 2.9765343465211713,
-                    "heading": -0.5584397105575287,
-                    "angularVelocity": -0.2759244043319095,
-                    "velocityX": 0.08355119468920533,
-                    "velocityY": 0.8818032595380478,
-                    "timestamp": 1.6280554242879517
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "angularVelocity": -0.14177668439456792,
-                    "velocityX": 0.04981456435476623,
-                    "velocityY": 0.4400469813703916,
-                    "timestamp": 1.6813807610639637
-                },
-                {
-                    "x": 4.263,
-                    "y": 3,
-                    "heading": -0.566,
-                    "angularVelocity": 0,
-                    "velocityX": 3.549235715029611e-32,
-                    "velocityY": -9.598836837089386e-32,
-                    "timestamp": 1.7347060978399758
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C5 To S4": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "angularVelocity": 2.8467808772943154e-31,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.633526252649942,
-                    "y": 0.8024218861992577,
-                    "heading": -0.02675508445787236,
-                    "angularVelocity": -0.3471453590528024,
-                    "velocityX": -0.602993637929559,
-                    "velocityY": 0.20009790028293237,
-                    "timestamp": 0.07707170428800919
-                },
-                {
-                    "x": 7.540488865885011,
-                    "y": 0.8333059401759839,
-                    "heading": -0.07783349665964928,
-                    "angularVelocity": -0.6627388439588938,
-                    "velocityX": -1.2071536191448495,
-                    "velocityY": 0.400718451240102,
-                    "timestamp": 0.15414340857601838
-                },
-                {
-                    "x": 7.400762361365792,
-                    "y": 0.8797032018771439,
-                    "heading": -0.14939031986058435,
-                    "angularVelocity": -0.9284448016555393,
-                    "velocityX": -1.8129416730824437,
-                    "velocityY": 0.6020012419574645,
-                    "timestamp": 0.23121511286402757
-                },
-                {
-                    "x": 7.214174023420098,
-                    "y": 0.9416744881041137,
-                    "heading": -0.23458534610519022,
-                    "angularVelocity": -1.105399537114693,
-                    "velocityX": -2.4209707008480317,
-                    "velocityY": 0.8040731264406625,
-                    "timestamp": 0.30828681715203676
-                },
-                {
-                    "x": 6.98058449689491,
-                    "y": 1.0192605514016468,
-                    "heading": -0.3182722929391017,
-                    "angularVelocity": -1.085832311702643,
-                    "velocityX": -3.030807852026836,
-                    "velocityY": 1.0066737723562156,
-                    "timestamp": 0.38535852144004595
-                },
-                {
-                    "x": 6.7044949607857305,
-                    "y": 1.1109419048147462,
-                    "heading": -0.3204977394277859,
-                    "angularVelocity": -0.02887501333002772,
-                    "velocityX": -3.5822425189594846,
-                    "velocityY": 1.189559180766205,
-                    "timestamp": 0.46243022572805514
-                },
-                {
-                    "x": 6.427835578289735,
-                    "y": 1.202949830409229,
-                    "heading": -0.32049776831244,
-                    "angularVelocity": -3.7477637567079044e-7,
-                    "velocityX": -3.5896362361749183,
-                    "velocityY": 1.1937964320946928,
-                    "timestamp": 0.5395019300160644
-                },
-                {
-                    "x": 6.1511761970625,
-                    "y": 1.294957759818759,
-                    "heading": -0.3204977971970938,
-                    "angularVelocity": -3.7477637370825724e-7,
-                    "velocityX": -3.589636219712834,
-                    "velocityY": 1.1937964815946673,
-                    "timestamp": 0.6165736343040735
-                },
-                {
-                    "x": 5.87524941446386,
-                    "y": 1.3871813457379307,
-                    "heading": -0.32266464886855784,
-                    "angularVelocity": -0.028114749653993878,
-                    "velocityX": -3.5801308034856563,
-                    "velocityY": 1.1965946098005253,
-                    "timestamp": 0.6936453385920827
-                },
-                {
-                    "x": 5.641703675217654,
-                    "y": 1.4649149367510799,
-                    "heading": -0.4060390277887848,
-                    "angularVelocity": -1.081776764773039,
-                    "velocityX": -3.030239715129037,
-                    "velocityY": 1.0085879342004245,
-                    "timestamp": 0.7707170428800918
-                },
-                {
-                    "x": 5.455154020778328,
-                    "y": 1.5270189169325792,
-                    "heading": -0.49098864699750666,
-                    "angularVelocity": -1.1022153979010723,
-                    "velocityX": -2.420468785044752,
-                    "velocityY": 0.8057948212669979,
-                    "timestamp": 0.8477887471681009
-                },
-                {
-                    "x": 5.315462796839212,
-                    "y": 1.573537321073397,
-                    "heading": -0.5623285861822422,
-                    "angularVelocity": -0.9256307466375128,
-                    "velocityX": -1.8124839100106522,
-                    "velocityY": 0.6035730566821683,
-                    "timestamp": 0.9248604514561101
-                },
-                {
-                    "x": 5.222455052636063,
-                    "y": 1.6045200564296747,
-                    "heading": -0.613275527154411,
-                    "angularVelocity": -0.6610330139033284,
-                    "velocityX": -1.2067690089684353,
-                    "velocityY": 0.40199883527290925,
-                    "timestamp": 1.0019321557441192
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "angularVelocity": -0.34674817551357506,
-                    "velocityX": -0.602751075316368,
-                    "velocityY": 0.2008511906325378,
-                    "timestamp": 1.0790038600321283
-                },
-                {
-                    "x": 5.176,
-                    "y": 1.62,
-                    "heading": -0.64,
-                    "angularVelocity": 8.760742141155177e-29,
-                    "velocityX": 3.313743560366677e-30,
-                    "velocityY": 1.2206105198294578e-30,
-                    "timestamp": 1.1560755643201375
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "C5 To C1": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 23
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": -1.1520076473353784e-31,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.8649552704871435,
-                    "heading": -1.2072196888077206e-18,
-                    "angularVelocity": -1.2485272283928241e-17,
-                    "velocityX": -1.2865550388289577e-22,
-                    "velocityY": 0.8062267267095138,
-                    "timestamp": 0.09669149868710694
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.0208658083668138,
-                    "heading": -3.6224298175784965e-18,
-                    "angularVelocity": -2.497851580797786e-17,
-                    "velocityX": -3.7534290948711127e-22,
-                    "velocityY": 1.6124534214139756,
-                    "timestamp": 0.19338299737421388
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.254731607449781,
-                    "heading": -7.246204108405477e-18,
-                    "angularVelocity": -3.7477692864160655e-17,
-                    "velocityX": -8.581224837351263e-22,
-                    "velocityY": 2.4186800521083573,
-                    "timestamp": 0.29007449606132085
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.5665526491683786,
-                    "heading": -1.208276035533523e-17,
-                    "angularVelocity": -5.0020491067317066e-17,
-                    "velocityX": -2.0493771641846554e-21,
-                    "velocityY": 3.2249064907727645,
-                    "timestamp": 0.38676599474842777
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.9323308421442915,
-                    "heading": -4.9593132234185194e-18,
-                    "angularVelocity": 7.367190734076976e-17,
-                    "velocityX": 8.922575847291113e-21,
-                    "velocityY": 3.782940568121389,
-                    "timestamp": 0.4834574934355347
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.298109035120243,
-                    "heading": 4.533704301231546e-17,
-                    "angularVelocity": 5.201735097557347e-16,
-                    "velocityX": 4.097272417616201e-22,
-                    "velocityY": 3.782940568121787,
-                    "timestamp": 0.5801489921226416
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.6638872280961943,
-                    "heading": 2.5191507323029372e-17,
-                    "angularVelocity": -2.083485721374208e-16,
-                    "velocityX": 1.8682331175835897e-21,
-                    "velocityY": 3.782940568121787,
-                    "timestamp": 0.6768404908097485
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.0296654210721456,
-                    "heading": 5.2668856754382746e-17,
-                    "angularVelocity": 2.8417544256148633e-16,
-                    "velocityX": 2.5170076181953607e-21,
-                    "velocityY": 3.782940568121787,
-                    "timestamp": 0.7735319894968554
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.395443614048097,
-                    "heading": 4.929611386988728e-17,
-                    "angularVelocity": -3.48814831737134e-17,
-                    "velocityX": 3.0934195236040766e-21,
-                    "velocityY": 3.782940568121787,
-                    "timestamp": 0.8702234881839623
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.7612218070240484,
-                    "heading": 4.8005098046144365e-17,
-                    "angularVelocity": -1.3351906227498234e-17,
-                    "velocityX": -1.4483318353601114e-20,
-                    "velocityY": 3.782940568121787,
-                    "timestamp": 0.9669149868710692
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 3.8136822799879804e-17,
-                    "angularVelocity": -1.0205938867695289e-16,
-                    "velocityX": -1.0465805642283078e-22,
-                    "velocityY": 3.782940568121787,
-                    "timestamp": 1.0636064855581762
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.492778192975952,
-                    "heading": 4.310486593468924e-17,
-                    "angularVelocity": 5.1380350929111975e-17,
-                    "velocityX": 2.042774398860212e-21,
-                    "velocityY": 3.782940568121787,
-                    "timestamp": 1.1602979842452832
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.8585563859519025,
-                    "heading": 1.5339712420766394e-17,
-                    "angularVelocity": -2.8715196155735124e-16,
-                    "velocityX": 3.895366641644577e-22,
-                    "velocityY": 3.782940568121787,
-                    "timestamp": 1.2569894829323902
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.224334578927854,
-                    "heading": -2.710858238227145e-17,
-                    "angularVelocity": -4.390075175101735e-16,
-                    "velocityX": -3.863371094269421e-21,
-                    "velocityY": 3.782940568121787,
-                    "timestamp": 1.3536809816194972
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.590112771903805,
-                    "heading": -3.378098304971619e-17,
-                    "angularVelocity": -6.900710774001486e-17,
-                    "velocityX": -4.042329982282679e-22,
-                    "velocityY": 3.782940568121787,
-                    "timestamp": 1.4503724803066043
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.955890964879757,
-                    "heading": -3.941496183315801e-17,
-                    "angularVelocity": -5.826757119208287e-17,
-                    "velocityX": 2.6937052538433574e-21,
-                    "velocityY": 3.782940568121787,
-                    "timestamp": 1.5470639789937113
-                },
-                {
-                    "x": 7.68,
-                    "y": 6.321669157855708,
-                    "heading": -1.9923808577496458e-17,
-                    "angularVelocity": 2.0158083720198419e-16,
-                    "velocityX": -4.499746264941967e-21,
-                    "velocityY": 3.782940568121787,
-                    "timestamp": 1.6437554776808183
-                },
-                {
-                    "x": 7.68,
-                    "y": 6.68744735083162,
-                    "heading": 1.1959451428437676e-17,
-                    "angularVelocity": 3.297421225035248e-16,
-                    "velocityX": 5.431014750114918e-21,
-                    "velocityY": 3.782940568121389,
-                    "timestamp": 1.7404469763679253
-                },
-                {
-                    "x": 7.68,
-                    "y": 6.999268392550218,
-                    "heading": 7.18685516064291e-18,
-                    "angularVelocity": -4.935900605111643e-17,
-                    "velocityX": -2.4046856363551417e-22,
-                    "velocityY": 3.2249064907727645,
-                    "timestamp": 1.8371384750550324
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.2331341916331855,
-                    "heading": 3.596188330325507e-18,
-                    "angularVelocity": -3.713528985888002e-17,
-                    "velocityX": -1.8035546521541034e-22,
-                    "velocityY": 2.4186800521083573,
-                    "timestamp": 1.9338299737421394
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.3890447295128565,
-                    "heading": 1.199348678010309e-18,
-                    "angularVelocity": -2.4788525206448663e-17,
-                    "velocityX": -1.2023814854884031e-22,
-                    "velocityY": 1.6124534214139756,
-                    "timestamp": 2.030521472429246
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "angularVelocity": -1.2403868947555692e-17,
-                    "velocityX": -6.011942579686287e-23,
-                    "velocityY": 0.8062267267095138,
-                    "timestamp": 2.127212971116353
-                },
-                {
-                    "x": 7.68,
-                    "y": 7.467,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": -2.719452927302828e-33,
-                    "velocityY": 0,
-                    "timestamp": 2.2239044698034602
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "C5 To C2": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 18
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 0,
-                    "velocityY": 2.5195168592304314e-32,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.8688441539517631,
-                    "heading": 1.4487899486085006e-18,
-                    "angularVelocity": 1.4623322004407643e-17,
-                    "velocityX": 2.341988908112997e-19,
-                    "velocityY": 0.8260917454276263,
-                    "timestamp": 0.09907392781100396
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.03253245874193,
-                    "heading": 4.3483011920968684e-18,
-                    "angularVelocity": 2.926613800008521e-17,
-                    "velocityX": 4.684469740721679e-19,
-                    "velocityY": 1.6521834594306495,
-                    "timestamp": 0.19814785562200793
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.2780649081437847,
-                    "heading": 8.701494230539972e-18,
-                    "angularVelocity": 4.393883572228609e-17,
-                    "velocityX": 7.027934476495029e-19,
-                    "velocityY": 2.4782751105844794,
-                    "timestamp": 0.29722178343301187
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.6054414834771922,
-                    "heading": 1.4516368843485863e-17,
-                    "angularVelocity": 5.869227900187472e-17,
-                    "velocityX": 9.37435089599571e-19,
-                    "velocityY": 3.304366573190877,
-                    "timestamp": 0.39629571124401586
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.9802322649266864,
-                    "heading": 6.647154079192576e-18,
-                    "angularVelocity": -7.942770553424421e-17,
-                    "velocityX": 1.0709424285811041e-18,
-                    "velocityY": 3.782940575087071,
-                    "timestamp": 0.49536963905501985
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.3550230463762047,
-                    "heading": -4.11606275908414e-17,
-                    "angularVelocity": -4.825465460619937e-16,
-                    "velocityX": 1.073083844244948e-18,
-                    "velocityY": 3.7829405750873115,
-                    "timestamp": 0.5944435668660238
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.729813827825723,
-                    "heading": -7.635066611173202e-17,
-                    "angularVelocity": -3.5518969822232415e-16,
-                    "velocityX": 1.0738946259920485e-18,
-                    "velocityY": 3.782940575087312,
-                    "timestamp": 0.6935174946770278
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.1046046092752415,
-                    "heading": -5.864574302033697e-17,
-                    "angularVelocity": 1.787041604453991e-16,
-                    "velocityX": 1.0691415340399175e-18,
-                    "velocityY": 3.782940575087312,
-                    "timestamp": 0.7925914224880318
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.4793953907247595,
-                    "heading": -2.6391173603704442e-17,
-                    "angularVelocity": 3.255606205313886e-16,
-                    "velocityX": 1.0732289061667476e-18,
-                    "velocityY": 3.782940575087312,
-                    "timestamp": 0.8916653502990358
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.854186172174278,
-                    "heading": -6.511485011951871e-17,
-                    "angularVelocity": -3.9085637736788184e-16,
-                    "velocityX": 1.0716697150075795e-18,
-                    "velocityY": 3.782940575087312,
-                    "timestamp": 0.9907392781100398
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.228976953623796,
-                    "heading": -6.241329479526944e-17,
-                    "angularVelocity": 2.7268075304360027e-17,
-                    "velocityX": 1.0686111075844087e-18,
-                    "velocityY": 3.782940575087312,
-                    "timestamp": 1.0898132059210437
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.603767735073314,
-                    "heading": -2.2737833863559818e-17,
-                    "angularVelocity": 4.004631875225205e-16,
-                    "velocityX": 1.0710197916722359e-18,
-                    "velocityY": 3.7829405750873115,
-                    "timestamp": 1.1888871337320477
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.978558516522808,
-                    "heading": -1.448944579275074e-17,
-                    "angularVelocity": 8.325488100707829e-17,
-                    "velocityX": 1.0714504382145178e-18,
-                    "velocityY": 3.782940575087071,
-                    "timestamp": 1.2879610615430517
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.305935091856216,
-                    "heading": -8.687037170545135e-18,
-                    "angularVelocity": 5.856645386228716e-17,
-                    "velocityX": 9.36753995288457e-19,
-                    "velocityY": 3.304366573190877,
-                    "timestamp": 1.3870349893540557
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.55146754125807,
-                    "heading": -4.341905072762387e-18,
-                    "angularVelocity": 4.385747283685971e-17,
-                    "velocityX": 7.025655481927048e-19,
-                    "velocityY": 2.4782751105844794,
-                    "timestamp": 1.4861089171650597
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.715155846048237,
-                    "heading": -1.4469357825353783e-18,
-                    "angularVelocity": 2.9220293918225814e-17,
-                    "velocityX": 4.683770471216647e-19,
-                    "velocityY": 1.6521834594306495,
-                    "timestamp": 1.5851828449760637
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "angularVelocity": 1.460460702963924e-17,
-                    "velocityX": 2.341885280588165e-19,
-                    "velocityY": 0.8260917454276263,
-                    "timestamp": 1.6842567727870676
-                },
-                {
-                    "x": 7.68,
-                    "y": 5.797,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 3.1812192470207854e-34,
-                    "velocityY": 0,
-                    "timestamp": 1.7833307005980716
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "C5 To C3": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 14
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "angularVelocity": 2.417290808823166e-40,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.8634453720055388,
-                    "heading": 1.009810999063182e-18,
-                    "angularVelocity": 1.0546271494620871e-17,
-                    "velocityX": -3.226662245674165e-34,
-                    "velocityY": 0.7983807350946677,
-                    "timestamp": 0.09575052183150978
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.0163361129826893,
-                    "heading": 3.0323418642587253e-18,
-                    "angularVelocity": 2.112292263585315e-17,
-                    "velocityX": -6.037770263383471e-34,
-                    "velocityY": 1.596761438503583,
-                    "timestamp": 0.19150104366301957
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.2456722168635992,
-                    "heading": 6.073900773319643e-18,
-                    "angularVelocity": 3.1765455168164525e-17,
-                    "velocityX": -4.821612934727609e-34,
-                    "velocityY": 2.3951420785410233,
-                    "timestamp": 0.28725156549452935
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.5514536654447455,
-                    "heading": 1.0152788689831222e-17,
-                    "angularVelocity": 4.2599119433487623e-17,
-                    "velocityX": -1.2140491970354773e-33,
-                    "velocityY": 3.19352252846437,
-                    "timestamp": 0.38300208732603913
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.9136721992668209,
-                    "heading": 4.411917253468209e-18,
-                    "angularVelocity": -5.995655508243724e-17,
-                    "velocityX": 4.90795348832912e-33,
-                    "velocityY": 3.7829405719528495,
-                    "timestamp": 0.4787526091575489
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.27589073308894,
-                    "heading": 1.2796107303848674e-17,
-                    "angularVelocity": 8.756286535058141e-17,
-                    "velocityX": -1.5435756690671252e-33,
-                    "velocityY": 3.782940571953311,
-                    "timestamp": 0.5745031309890587
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.63810926691106,
-                    "heading": 6.994895399550348e-18,
-                    "angularVelocity": -6.058673930264937e-17,
-                    "velocityX": 4.776897057793067e-34,
-                    "velocityY": 3.782940571953311,
-                    "timestamp": 0.6702536528205685
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.0003278007331793,
-                    "heading": -9.534020306729642e-18,
-                    "angularVelocity": -1.7262481070720064e-16,
-                    "velocityX": 2.9275045647973183e-33,
-                    "velocityY": 3.782940571953311,
-                    "timestamp": 0.7660041746520783
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.3625463345552546,
-                    "heading": -1.0013819273727212e-17,
-                    "angularVelocity": -5.010927959660929e-18,
-                    "velocityX": 2.237784317663532e-33,
-                    "velocityY": 3.7829405719528495,
-                    "timestamp": 0.861754696483588
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.6683277831364007,
-                    "heading": -6.0064636534747415e-18,
-                    "angularVelocity": 4.185204993640188e-17,
-                    "velocityX": -2.2313398142102424e-33,
-                    "velocityY": 3.19352252846437,
-                    "timestamp": 0.9575052183150978
-                },
-                {
-                    "x": 7.68,
-                    "y": 3.897663887017311,
-                    "heading": -3.002975312877452e-18,
-                    "angularVelocity": 3.136785349810471e-17,
-                    "velocityX": -2.8833027864886262e-33,
-                    "velocityY": 2.3951420785410233,
-                    "timestamp": 1.0532557401466076
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.050554627994461,
-                    "heading": -1.0009557447862449e-18,
-                    "angularVelocity": 2.0908706606963636e-17,
-                    "velocityX": -3.742421273800505e-33,
-                    "velocityY": 1.596761438503583,
-                    "timestamp": 1.1490062619781174
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "angularVelocity": 1.0453788927770355e-17,
-                    "velocityX": -1.5032681111068507e-33,
-                    "velocityY": 0.7983807350946677,
-                    "timestamp": 1.2447567838096272
-                },
-                {
-                    "x": 7.68,
-                    "y": 4.127,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": 9.665729080515376e-43,
-                    "velocityY": 2.80463986608038e-38,
-                    "timestamp": 1.340507305641137
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "C5 To C4": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 9
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 9.069206296242399e-40,
-                    "angularVelocity": -9.927610039652183e-40,
-                    "velocityX": -8.253947176298157e-40,
-                    "velocityY": 1.8367099231598242e-40,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.68,
-                    "y": 0.8705000055758465,
-                    "heading": 6.490773345690525e-22,
-                    "angularVelocity": 6.486159683422905e-21,
-                    "velocityX": -7.132918469580153e-33,
-                    "velocityY": 0.8344065481228213,
-                    "timestamp": 0.10007112931184182
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.037500013629847,
-                    "heading": 2.023593010760001e-21,
-                    "angularVelocity": 1.3735386658326633e-20,
-                    "velocityX": -1.5648094960655976e-32,
-                    "velocityY": 1.668813065290736,
-                    "timestamp": 0.20014225862368365
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.2880000179666162,
-                    "heading": 3.787875676222919e-21,
-                    "angularVelocity": 1.7630286060953193e-20,
-                    "velocityX": -2.3089760494009302e-32,
-                    "velocityY": 2.503219520548835,
-                    "timestamp": 0.3002133879355255
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.6219999999999999,
-                    "heading": 5.860880748165694e-21,
-                    "angularVelocity": 2.0715315514205814e-20,
-                    "velocityX": -3.0704903308303665e-32,
-                    "velocityY": 3.3376257900775013,
-                    "timestamp": 0.4002845172473673
-                },
-                {
-                    "x": 7.68,
-                    "y": 1.9559999820333838,
-                    "heading": 3.934658943161884e-21,
-                    "angularVelocity": -1.9248526176341983e-20,
-                    "velocityX": -4.040562356218352e-32,
-                    "velocityY": 3.3376257900775017,
-                    "timestamp": 0.5003556465592092
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.206499986370153,
-                    "heading": 1.6176834885162177e-21,
-                    "angularVelocity": -2.3153285409554772e-20,
-                    "velocityX": -3.08903871620124e-32,
-                    "velocityY": 2.503219520548835,
-                    "timestamp": 0.600426775871051
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.3734999944241535,
-                    "heading": 4.992981261257204e-22,
-                    "angularVelocity": -1.1175904096118944e-20,
-                    "velocityX": -2.3331216062245415e-32,
-                    "velocityY": 1.668813065290736,
-                    "timestamp": 0.7004979051828928
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 1.7562757360140444e-40,
-                    "angularVelocity": -4.9894322348928345e-21,
-                    "velocityX": -1.3647931662662733e-32,
-                    "velocityY": 0.8344065481228213,
-                    "timestamp": 0.8005690344947346
-                },
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": -7.274275970183695e-41,
-                    "angularVelocity": -2.4819026120794767e-39,
-                    "velocityX": -8.637198685981915e-42,
-                    "velocityY": 5.142787784847508e-39,
-                    "timestamp": 0.9006401638065764
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": []
-        },
-        "C5 To S1": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 19
-                },
-                {
-                    "x": 7.198879241943359,
-                    "y": 5.9179229736328125,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 12
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.26,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "angularVelocity": 4.273608378959021e-31,
-                    "velocityX": -7.420825794722196e-33,
-                    "velocityY": 7.583788429881001e-34,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.678491547493202,
-                    "y": 0.8436471873828707,
-                    "heading": -0.006289831251442517,
-                    "angularVelocity": -0.0762785379812196,
-                    "velocityX": -0.018293424295956956,
-                    "velocityY": 0.686976241742982,
-                    "timestamp": 0.08245872846948342
-                },
-                {
-                    "x": 7.67547464227306,
-                    "y": 0.9569415605229419,
-                    "heading": -0.018869443032064528,
-                    "angularVelocity": -0.1525564608394078,
-                    "velocityX": -0.03658685109677269,
-                    "velocityY": 1.3739524637710059,
-                    "timestamp": 0.16491745693896684
-                },
-                {
-                    "x": 7.670949285584863,
-                    "y": 1.1268831170081794,
-                    "heading": -0.03773871517098893,
-                    "angularVelocity": -0.22883292635185057,
-                    "velocityX": -0.05488026279561822,
-                    "velocityY": 2.060928656547622,
-                    "timestamp": 0.24737618540845024
-                },
-                {
-                    "x": 7.664915480002808,
-                    "y": 1.3534718520373763,
-                    "heading": -0.06289740953278049,
-                    "angularVelocity": -0.3051065039294483,
-                    "velocityX": -0.07317364327643945,
-                    "velocityY": 2.7479047910986583,
-                    "timestamp": 0.3298349138779337
-                },
-                {
-                    "x": 7.657373229393316,
-                    "y": 1.6367077493690674,
-                    "heading": -0.09434508673453766,
-                    "angularVelocity": -0.38137475298804396,
-                    "velocityX": -0.09146697686811679,
-                    "velocityY": 3.4348807286849197,
-                    "timestamp": 0.4122936423474171
-                },
-                {
-                    "x": 7.649069464629009,
-                    "y": 1.9485336685806538,
-                    "heading": -0.09434509076634809,
-                    "angularVelocity": -4.889488962341613e-8,
-                    "velocityX": -0.10070207142933957,
-                    "velocityY": 3.781599898511507,
-                    "timestamp": 0.49475237081690054
-                },
-                {
-                    "x": 7.6407656424773585,
-                    "y": 2.260359586264075,
-                    "heading": -0.09434509479812381,
-                    "angularVelocity": -4.889446872332144e-8,
-                    "velocityX": -0.1007027673816724,
-                    "velocityY": 3.781599879979021,
-                    "timestamp": 0.5772110992863839
-                },
-                {
-                    "x": 7.63246175736497,
-                    "y": 2.5721855022708704,
-                    "heading": -0.09434509882989094,
-                    "angularVelocity": -4.889436444733497e-8,
-                    "velocityX": -0.10070353092409444,
-                    "velocityY": 3.7815998596461093,
-                    "timestamp": 0.6596698277558674
-                },
-                {
-                    "x": 7.6241578045918486,
-                    "y": 2.8840114164758646,
-                    "heading": -0.09434510286165439,
-                    "angularVelocity": -4.8894319896339695e-8,
-                    "velocityX": -0.10070435146468282,
-                    "velocityY": 3.781599837795173,
-                    "timestamp": 0.7421285562253508
-                },
-                {
-                    "x": 7.615853780314312,
-                    "y": 3.195837328776685,
-                    "heading": -0.09434510689341911,
-                    "angularVelocity": -4.889433549543518e-8,
-                    "velocityX": -0.10070521861867329,
-                    "velocityY": 3.78159981470272,
-                    "timestamp": 0.8245872846948342
-                },
-                {
-                    "x": 7.607549681647345,
-                    "y": 3.5076632390964844,
-                    "heading": -0.09434511092518724,
-                    "angularVelocity": -4.889437661104965e-8,
-                    "velocityX": -0.10070612076003535,
-                    "velocityY": 3.7815997906783263,
-                    "timestamp": 0.9070460131643177
-                },
-                {
-                    "x": 7.599245507241111,
-                    "y": 3.819489147399297,
-                    "heading": -0.0943451149569584,
-                    "angularVelocity": -4.889441348359375e-8,
-                    "velocityX": -0.10070703927126574,
-                    "velocityY": 3.7815997662177643,
-                    "timestamp": 0.9895047416338011
-                },
-                {
-                    "x": 7.590941256893013,
-                    "y": 4.131315053679708,
-                    "heading": -0.09434511898873191,
-                    "angularVelocity": -4.88944418703562e-8,
-                    "velocityX": -0.10070796023943433,
-                    "velocityY": 3.781599741691539,
-                    "timestamp": 1.0719634701032845
-                },
-                {
-                    "x": 7.582636930618974,
-                    "y": 4.443140957938123,
-                    "heading": -0.09434512302050634,
-                    "angularVelocity": -4.889445311998471e-8,
-                    "velocityX": -0.10070888101448454,
-                    "velocityY": 3.781599717170225,
-                    "timestamp": 1.1544221985727678
-                },
-                {
-                    "x": 7.574332505518219,
-                    "y": 4.754966859564639,
-                    "heading": -0.09434512705227821,
-                    "angularVelocity": -4.889442208864286e-8,
-                    "velocityX": -0.10071007951363715,
-                    "velocityY": 3.78159968525245,
-                    "timestamp": 1.2368809270422512
-                },
-                {
-                    "x": 7.562081052721255,
-                    "y": 5.066662637971005,
-                    "heading": -0.09434513109398723,
-                    "angularVelocity": -4.901493268640619e-8,
-                    "velocityX": -0.1485767853126217,
-                    "velocityY": 3.7800216446669976,
-                    "timestamp": 1.3193396555117345
-                },
-                {
-                    "x": 7.493618412578439,
-                    "y": 5.370993415792126,
-                    "heading": -0.09434513545729191,
-                    "angularVelocity": -5.29150129274595e-8,
-                    "velocityX": -0.8302655329951328,
-                    "velocityY": 3.6907042282825056,
-                    "timestamp": 1.4017983839812178
-                },
-                {
-                    "x": 7.37120325739307,
-                    "y": 5.657906089273735,
-                    "heading": -0.09434514092208332,
-                    "angularVelocity": -6.627304961707395e-8,
-                    "velocityX": -1.4845627316539582,
-                    "velocityY": 3.479470018601967,
-                    "timestamp": 1.4842571124507011
-                },
-                {
-                    "x": 7.198879241943359,
-                    "y": 5.9179229736328125,
-                    "heading": -0.09434514894422652,
-                    "angularVelocity": -9.728676819194244e-8,
-                    "velocityX": -2.089821400938593,
-                    "velocityY": 3.153297281988846,
-                    "timestamp": 1.5667158409201845
-                },
-                {
-                    "x": 7.016660952404083,
-                    "y": 6.116497130944734,
-                    "heading": -0.09434515627038953,
-                    "angularVelocity": -1.028329950777357e-7,
-                    "velocityX": -2.5576898068579945,
-                    "velocityY": 2.7872673997010713,
-                    "timestamp": 1.6379591516026175
-                },
-                {
-                    "x": 6.805603401067166,
-                    "y": 6.284097595272953,
-                    "heading": -0.09434516139165254,
-                    "angularVelocity": -7.188412437441302e-8,
-                    "velocityX": -2.9624893806199624,
-                    "velocityY": 2.352508084236811,
-                    "timestamp": 1.7092024622850506
-                },
-                {
-                    "x": 6.57091097390028,
-                    "y": 6.416591642615984,
-                    "heading": -0.09434516555011045,
-                    "angularVelocity": -5.83698017255904e-8,
-                    "velocityX": -3.2942380824078485,
-                    "velocityY": 1.8597401787463752,
-                    "timestamp": 1.7804457729674836
-                },
-                {
-                    "x": 6.318370883291981,
-                    "y": 6.510712371224652,
-                    "heading": -0.09434516933867851,
-                    "angularVelocity": -5.317787750894471e-8,
-                    "velocityX": -3.544755124224867,
-                    "velocityY": 1.3211167154795889,
-                    "timestamp": 1.8516890836499167
-                },
-                {
-                    "x": 6.054210380502568,
-                    "y": 6.5641393107028225,
-                    "heading": -0.09434515209433943,
-                    "angularVelocity": 2.420485363909629e-7,
-                    "velocityX": -3.7078639420184567,
-                    "velocityY": 0.7499221887135695,
-                    "timestamp": 1.9229323943323497
-                },
-                {
-                    "x": 5.803536575930751,
-                    "y": 6.585943895129495,
-                    "heading": -0.03682720962895,
-                    "angularVelocity": 0.8073451656643992,
-                    "velocityX": -3.5185591765828246,
-                    "velocityY": 0.30605798941413415,
-                    "timestamp": 1.9941757050147828
-                },
-                {
-                    "x": 5.594045383796189,
-                    "y": 6.601646508747958,
-                    "heading": 0.04817170409455108,
-                    "angularVelocity": 1.1930792225867084,
-                    "velocityX": -2.9405033276509123,
-                    "velocityY": 0.2204082526211729,
-                    "timestamp": 2.065419015697216
-                },
-                {
-                    "x": 5.426658878791764,
-                    "y": 6.613398993876379,
-                    "heading": 0.12734083591800166,
-                    "angularVelocity": 1.111250039689289,
-                    "velocityX": -2.3495048644012306,
-                    "velocityY": 0.16496264724146908,
-                    "timestamp": 2.136662326379649
-                },
-                {
-                    "x": 5.301258489415666,
-                    "y": 6.621853943751229,
-                    "heading": 0.1915936210653456,
-                    "angularVelocity": 0.9018781487254319,
-                    "velocityX": -1.7601707188351954,
-                    "velocityY": 0.11867710517466466,
-                    "timestamp": 2.207905637062082
-                },
-                {
-                    "x": 5.217732575552286,
-                    "y": 6.627325619944874,
-                    "heading": 0.2366430637253094,
-                    "angularVelocity": 0.6323322460514409,
-                    "velocityX": -1.1724035992052257,
-                    "velocityY": 0.07680266597989953,
-                    "timestamp": 2.279148947744515
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.26,
-                    "angularVelocity": 0.32784742947732926,
-                    "velocityX": -0.5857753542407405,
-                    "velocityY": 0.037538682993647744,
-                    "timestamp": 2.350392258426948
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.26,
-                    "angularVelocity": -8.68876182413906e-32,
-                    "velocityX": 0,
-                    "velocityY": -2.1409750340846636e-32,
-                    "timestamp": 2.421635569109381
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C5 To S2": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 15
-                },
-                {
-                    "x": 6.126883506774902,
-                    "y": 4.1052751541137695,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 11
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 0.787,
-                    "heading": 0,
-                    "angularVelocity": -1.3156383032943886e-37,
-                    "velocityX": 0,
-                    "velocityY": 0,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.664225467591612,
-                    "y": 0.8361290574305452,
-                    "heading": -0.011405957532827086,
-                    "angularVelocity": -0.14485338302751044,
-                    "velocityX": -0.20033341159264173,
-                    "velocityY": 0.6239292188565086,
-                    "timestamp": 0.07874139557141634
-                },
-                {
-                    "x": 7.632676408078482,
-                    "y": 0.9343872155736361,
-                    "heading": -0.03421427612768491,
-                    "angularVelocity": -0.2896610915940815,
-                    "velocityX": -0.4006667558300489,
-                    "velocityY": 1.2478589873857826,
-                    "timestamp": 0.15748279114283267
-                },
-                {
-                    "x": 7.585352811707443,
-                    "y": 1.0817745497576996,
-                    "heading": -0.06841862032572539,
-                    "angularVelocity": -0.4343883410984004,
-                    "velocityX": -0.6010002239307173,
-                    "velocityY": 1.8717897125710332,
-                    "timestamp": 0.236224186714249
-                },
-                {
-                    "x": 7.522254637419436,
-                    "y": 1.278291160108196,
-                    "heading": -0.11400992700252059,
-                    "angularVelocity": -0.5790004907322871,
-                    "velocityX": -0.8013342134732577,
-                    "velocityY": 2.4957217093296378,
-                    "timestamp": 0.31496558228566535
-                },
-                {
-                    "x": 7.443381794868827,
-                    "y": 1.5239371518673812,
-                    "heading": -0.1709767215367488,
-                    "angularVelocity": -0.723466914966739,
-                    "velocityX": -1.0016693503872847,
-                    "velocityY": 3.1196550426438785,
-                    "timestamp": 0.3937069778570817
-                },
-                {
-                    "x": 7.3523133020023,
-                    "y": 1.807548605142893,
-                    "heading": -0.17097672945138376,
-                    "angularVelocity": -1.00514283666892e-7,
-                    "velocityX": -1.1565516740674315,
-                    "velocityY": 3.6018088226323544,
-                    "timestamp": 0.47244837342849805
-                },
-                {
-                    "x": 7.26124476293705,
-                    "y": 2.0911600435840003,
-                    "heading": -0.17097673736581726,
-                    "angularVelocity": -1.0051172494783456e-7,
-                    "velocityX": -1.1565522607819834,
-                    "velocityY": 3.6018086342383846,
-                    "timestamp": 0.5511897689999143
-                },
-                {
-                    "x": 7.170176175790629,
-                    "y": 2.3747714665860937,
-                    "heading": -0.17097674528023646,
-                    "angularVelocity": -1.0051154369078753e-7,
-                    "velocityX": -1.1565528714032516,
-                    "velocityY": 3.601808438165991,
-                    "timestamp": 0.6299311645713307
-                },
-                {
-                    "x": 7.079107531002594,
-                    "y": 2.6583828710792723,
-                    "heading": -0.17097675319464586,
-                    "angularVelocity": -1.0051141913755183e-7,
-                    "velocityX": -1.1565536034402273,
-                    "velocityY": 3.601808203106465,
-                    "timestamp": 0.708672560142747
-                },
-                {
-                    "x": 6.988038816793172,
-                    "y": 2.9419942532809915,
-                    "heading": -0.17097676110905413,
-                    "angularVelocity": -1.0051140450859988e-7,
-                    "velocityX": -1.1565544850779974,
-                    "velocityY": 3.601807920009381,
-                    "timestamp": 0.7874139557141634
-                },
-                {
-                    "x": 6.896968817946089,
-                    "y": 3.2256052229773635,
-                    "heading": -0.17097676902348397,
-                    "angularVelocity": -1.0051167869642574e-7,
-                    "velocityX": -1.156570799719756,
-                    "velocityY": 3.6018026812738513,
-                    "timestamp": 0.8661553512855797
-                },
-                {
-                    "x": 6.765153291475094,
-                    "y": 3.4927261567236307,
-                    "heading": -0.1709767773203105,
-                    "angularVelocity": -1.053680400636008e-7,
-                    "velocityX": -1.6740308641271406,
-                    "velocityY": 3.392382517579277,
-                    "timestamp": 0.9448967468569961
-                },
-                {
-                    "x": 6.589137393143544,
-                    "y": 3.7330328933669676,
-                    "heading": -0.17097678717611917,
-                    "angularVelocity": -1.2516680215717606e-7,
-                    "velocityX": -2.235366760446956,
-                    "velocityY": 3.0518475688608455,
-                    "timestamp": 1.0236381424284124
-                },
-                {
-                    "x": 6.374222917834714,
-                    "y": 3.939286833341542,
-                    "heading": -0.17097680095561982,
-                    "angularVelocity": -1.7499690668870768e-7,
-                    "velocityX": -2.7293709204570815,
-                    "velocityY": 2.6193838511219654,
-                    "timestamp": 1.1023795379998287
-                },
-                {
-                    "x": 6.126883506774902,
-                    "y": 4.1052751541137695,
-                    "heading": -0.17097682837855596,
-                    "angularVelocity": -3.482658130630958e-7,
-                    "velocityX": -3.1411611296053437,
-                    "velocityY": 2.108018527836233,
-                    "timestamp": 1.181120933571245
-                },
-                {
-                    "x": 5.792856415059812,
-                    "y": 4.239084767822678,
-                    "heading": -0.17097684795948573,
-                    "angularVelocity": -2.0585572281638246e-7,
-                    "velocityX": -3.5116508401962974,
-                    "velocityY": 1.4067500932170736,
-                    "timestamp": 1.2762406100184678
-                },
-                {
-                    "x": 5.438271606876556,
-                    "y": 4.300308177252639,
-                    "heading": -0.1709790019096565,
-                    "angularVelocity": -0.000022644633069121558,
-                    "velocityX": -3.7277755920458318,
-                    "velocityY": 0.6436461068486719,
-                    "timestamp": 1.3713602864656906
-                },
-                {
-                    "x": 5.110881399181515,
-                    "y": 4.377812934831655,
-                    "heading": -0.2483895057711766,
-                    "angularVelocity": -0.8138221948691259,
-                    "velocityX": -3.4418768011337173,
-                    "velocityY": 0.8148130909803968,
-                    "timestamp": 1.4664799629129135
-                },
-                {
-                    "x": 4.831528638348494,
-                    "y": 4.508515906317057,
-                    "heading": -0.41196599842011083,
-                    "angularVelocity": -1.7196914325049715,
-                    "velocityX": -2.936855667165985,
-                    "velocityY": 1.374089740075198,
-                    "timestamp": 1.5615996393601363
-                },
-                {
-                    "x": 4.597313308728367,
-                    "y": 4.699607232649008,
-                    "heading": -0.5851564423497488,
-                    "angularVelocity": -1.8207635938052467,
-                    "velocityX": -2.4623226063019743,
-                    "velocityY": 2.0089568580269215,
-                    "timestamp": 1.6567193158073592
-                },
-                {
-                    "x": 4.422373656654268,
-                    "y": 4.922240435570765,
-                    "heading": -0.6321779806524386,
-                    "angularVelocity": -0.49434081421397164,
-                    "velocityX": -1.8391531448402672,
-                    "velocityY": 2.340558875274187,
-                    "timestamp": 1.751838992254582
-                },
-                {
-                    "x": 4.3095074978210945,
-                    "y": 5.147658123857161,
-                    "heading": -0.526303235570326,
-                    "angularVelocity": 1.11306881012003,
-                    "velocityX": -1.1865700457443962,
-                    "velocityY": 2.3698323701875594,
-                    "timestamp": 1.846958668701805
-                },
-                {
-                    "x": 4.263497791071102,
-                    "y": 5.351217934034869,
-                    "heading": -0.3174810959543699,
-                    "angularVelocity": 2.1953621733755666,
-                    "velocityX": -0.48370335632419137,
-                    "velocityY": 2.1400389254967007,
-                    "timestamp": 1.9420783451490278
-                },
-                {
-                    "x": 4.258299402660224,
-                    "y": 5.491389588363689,
-                    "heading": -0.11860771311510487,
-                    "angularVelocity": 2.090770177814994,
-                    "velocityX": -0.05465103125915711,
-                    "velocityY": 1.4736346838458172,
-                    "timestamp": 2.0371980215962506
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 6.542074475732666e-36,
-                    "angularVelocity": 1.2469314188732994,
-                    "velocityX": 0.04941771792488758,
-                    "velocityY": 0.7213061923563153,
-                    "timestamp": 2.1323176980434737
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": 0,
-                    "velocityX": -6.642487421560091e-37,
-                    "velocityY": 5.186129384888016e-33,
-                    "timestamp": 2.227437374490697
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C4 To S1": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 16
-                },
-                {
-                    "x": 4.353217601776123,
-                    "y": 4.923890113830566,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 11
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.26,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "angularVelocity": -3.908498355076501e-28,
-                    "velocityX": -5.285170566576542e-31,
-                    "velocityY": -1.5421992925545815e-31,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.644672083885401,
-                    "y": 2.5062543236054426,
-                    "heading": 0.0003371503707738104,
-                    "angularVelocity": 0.003954317681676867,
-                    "velocityX": -0.4143486570570975,
-                    "velocityY": 0.5776865743787781,
-                    "timestamp": 0.08526132645270777
-                },
-                {
-                    "x": 7.574016248997627,
-                    "y": 2.604762966378743,
-                    "heading": 0.0010114541718947692,
-                    "angularVelocity": 0.00790867124800936,
-                    "velocityX": -0.8286973452956875,
-                    "velocityY": 1.1553730967106797,
-                    "timestamp": 0.17052265290541554
-                },
-                {
-                    "x": 7.4680324930779305,
-                    "y": 2.7525259224839496,
-                    "heading": 0.0020229127878395882,
-                    "angularVelocity": 0.01186304105238048,
-                    "velocityX": -1.243046060026343,
-                    "velocityY": 1.73305955059478,
-                    "timestamp": 0.2557839793581233
-                },
-                {
-                    "x": 7.326720815099078,
-                    "y": 2.949543182754876,
-                    "heading": 0.0033715247016779517,
-                    "angularVelocity": 0.015817393065211405,
-                    "velocityX": -1.657394786805044,
-                    "velocityY": 2.310745896971908,
-                    "timestamp": 0.3410453058108311
-                },
-                {
-                    "x": 7.1500812197312955,
-                    "y": 3.1958147252550657,
-                    "heading": 0.005057281636821727,
-                    "angularVelocity": 0.01977164800486184,
-                    "velocityX": -2.0717434588083226,
-                    "velocityY": 2.8884319860640817,
-                    "timestamp": 0.4263066322635389
-                },
-                {
-                    "x": 6.957409028465344,
-                    "y": 3.454481547786369,
-                    "heading": 0.005057281852786698,
-                    "angularVelocity": 2.532976909732762e-9,
-                    "velocityX": -2.2597841164587846,
-                    "velocityY": 3.0338118499096547,
-                    "timestamp": 0.5115679587162466
-                },
-                {
-                    "x": 6.719745009541024,
-                    "y": 3.6725340903222867,
-                    "heading": 0.005057282088140234,
-                    "angularVelocity": 2.760378532567681e-9,
-                    "velocityX": -2.787477380570894,
-                    "velocityY": 2.5574612970252826,
-                    "timestamp": 0.5968292851689544
-                },
-                {
-                    "x": 6.445480997294438,
-                    "y": 3.842270194716782,
-                    "heading": 0.005057282389511224,
-                    "angularVelocity": 3.534673960970524e-9,
-                    "velocityX": -3.2167457821427474,
-                    "velocityY": 1.9907748501725318,
-                    "timestamp": 0.6820906116216622
-                },
-                {
-                    "x": 6.1443030532206215,
-                    "y": 3.9576952313720115,
-                    "heading": 0.0050573451663417065,
-                    "angularVelocity": 7.362872839766494e-7,
-                    "velocityX": -3.5324097877003195,
-                    "velocityY": 1.3537795089212774,
-                    "timestamp": 0.76735193807437
-                },
-                {
-                    "x": 5.8268478532983945,
-                    "y": 4.0147328665402,
-                    "heading": 0.005057406885810731,
-                    "angularVelocity": 7.238858646916699e-7,
-                    "velocityX": -3.723319975538169,
-                    "velocityY": 0.6689742881224171,
-                    "timestamp": 0.8526132645270778
-                },
-                {
-                    "x": 5.505959202112108,
-                    "y": 4.047314610372366,
-                    "heading": 0.005057466820580134,
-                    "angularVelocity": 7.029537528496964e-7,
-                    "velocityX": -3.7635897133768776,
-                    "velocityY": 0.3821397717793548,
-                    "timestamp": 0.9378745909797855
-                },
-                {
-                    "x": 5.196832806272751,
-                    "y": 4.139357882135735,
-                    "heading": 0.005057539347948128,
-                    "angularVelocity": 8.506478964233103e-7,
-                    "velocityX": -3.625634372587421,
-                    "velocityY": 1.0795430424660248,
-                    "timestamp": 1.0231359174324932
-                },
-                {
-                    "x": 4.911616541791353,
-                    "y": 4.287091740854739,
-                    "heading": 0.008468262161445878,
-                    "angularVelocity": 0.040003163864724633,
-                    "velocityX": -3.3452008823671444,
-                    "velocityY": 1.7327182776242007,
-                    "timestamp": 1.108397243885201
-                },
-                {
-                    "x": 4.672649683479067,
-                    "y": 4.471709481894194,
-                    "heading": 0.05890642847923189,
-                    "angularVelocity": 0.5915714476464851,
-                    "velocityX": -2.8027579238375626,
-                    "velocityY": 2.165316313039838,
-                    "timestamp": 1.1936585703379088
-                },
-                {
-                    "x": 4.485112052095124,
-                    "y": 4.685980588264911,
-                    "heading": 0.1533943172973333,
-                    "angularVelocity": 1.1082150929337078,
-                    "velocityX": -2.1995626761432403,
-                    "velocityY": 2.513110167122247,
-                    "timestamp": 1.2789198967906166
-                },
-                {
-                    "x": 4.353217601776123,
-                    "y": 4.923890113830566,
-                    "heading": 0.2635187752392858,
-                    "angularVelocity": 1.291610892363634,
-                    "velocityX": -1.5469434479432043,
-                    "velocityY": 2.7903568413003317,
-                    "timestamp": 1.3641812232433244
-                },
-                {
-                    "x": 4.2800604900800625,
-                    "y": 5.16873168779887,
-                    "heading": 0.3704891219101591,
-                    "angularVelocity": 1.3052114311350342,
-                    "velocityX": -0.8926352155228909,
-                    "velocityY": 2.987463639297507,
-                    "timestamp": 1.4461375592962549
-                },
-                {
-                    "x": 4.26199164107549,
-                    "y": 5.42343977988949,
-                    "heading": 0.46980573799025893,
-                    "angularVelocity": 1.2118235253560785,
-                    "velocityX": -0.22046921415441664,
-                    "velocityY": 3.107851135829132,
-                    "timestamp": 1.5280938953491854
-                },
-                {
-                    "x": 4.2996600311730235,
-                    "y": 5.679753559001703,
-                    "heading": 0.550426603173402,
-                    "angularVelocity": 0.9837050930723119,
-                    "velocityX": 0.45961535021819666,
-                    "velocityY": 3.127443117352554,
-                    "timestamp": 1.610050231402116
-                },
-                {
-                    "x": 4.391931306983731,
-                    "y": 5.927283560440951,
-                    "heading": 0.5996302346047028,
-                    "angularVelocity": 0.6003639718722656,
-                    "velocityX": 1.1258589665496648,
-                    "velocityY": 3.0202668074301977,
-                    "timestamp": 1.6920065674550464
-                },
-                {
-                    "x": 4.533405002431738,
-                    "y": 6.1528417215915425,
-                    "heading": 0.5943564820218653,
-                    "angularVelocity": -0.06434832053139185,
-                    "velocityX": 1.7262081525536632,
-                    "velocityY": 2.752174780055045,
-                    "timestamp": 1.773962903507977
-                },
-                {
-                    "x": 4.71098037645994,
-                    "y": 6.338671203945781,
-                    "heading": 0.5303639205613101,
-                    "angularVelocity": -0.7808128638054942,
-                    "velocityX": 2.166707085507742,
-                    "velocityY": 2.2674205717817246,
-                    "timestamp": 1.8559192395609074
-                },
-                {
-                    "x": 4.8864330916758485,
-                    "y": 6.469038507428809,
-                    "heading": 0.43960837441640055,
-                    "angularVelocity": -1.107364610427659,
-                    "velocityX": 2.1408072110807552,
-                    "velocityY": 1.5906921875921145,
-                    "timestamp": 1.937875575613838
-                },
-                {
-                    "x": 5.028345407892232,
-                    "y": 6.554592883552385,
-                    "heading": 0.3555321434700494,
-                    "angularVelocity": -1.0258661501483803,
-                    "velocityX": 1.7315600361235026,
-                    "velocityY": 1.0439019146530064,
-                    "timestamp": 2.0198319116667687
-                },
-                {
-                    "x": 5.126173645194142,
-                    "y": 6.6061448414321,
-                    "heading": 0.29324170563915075,
-                    "angularVelocity": -0.760044199524953,
-                    "velocityX": 1.1936628943333356,
-                    "velocityY": 0.6290173568329146,
-                    "timestamp": 2.1017882477196994
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.26,
-                    "angularVelocity": -0.405602632332653,
-                    "velocityX": 0.6079622053097948,
-                    "velocityY": 0.29107155976928517,
-                    "timestamp": 2.18374458377263
-                },
-                {
-                    "x": 5.176,
-                    "y": 6.63,
-                    "heading": 0.26,
-                    "angularVelocity": 0,
-                    "velocityX": 1.6766347007166246e-32,
-                    "velocityY": 1.3162553729765107e-31,
-                    "timestamp": 2.265700919825561
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        },
-        "C4 To S2": {
-            "waypoints": [
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 12
-                },
-                {
-                    "x": 5.581140041351318,
-                    "y": 4.144257068634033,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": false,
-                    "controlIntervalCount": 10
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "isInitialGuess": false,
-                    "translationConstrained": true,
-                    "headingConstrained": true,
-                    "controlIntervalCount": 40
-                }
-            ],
-            "trajectory": [
-                {
-                    "x": 7.68,
-                    "y": 2.457,
-                    "heading": 0,
-                    "angularVelocity": -1.8537798939082814e-32,
-                    "velocityX": 2.7477856809061307e-34,
-                    "velocityY": 6.925951302123011e-34,
-                    "timestamp": 0
-                },
-                {
-                    "x": 7.6474171775045905,
-                    "y": 2.4907449431726314,
-                    "heading": -0.04287864331544942,
-                    "angularVelocity": -0.5625230567182208,
-                    "velocityX": -0.42745263118016213,
-                    "velocityY": 0.4426984418000905,
-                    "timestamp": 0.07622557476240162
-                },
-                {
-                    "x": 7.582037622461459,
-                    "y": 2.5583409566544844,
-                    "heading": -0.12553871024043178,
-                    "angularVelocity": -1.084413822823079,
-                    "velocityX": -0.8577115390329612,
-                    "velocityY": 0.8867891608892778,
-                    "timestamp": 0.15245114952480324
-                },
-                {
-                    "x": 7.483606625879984,
-                    "y": 2.659997722482693,
-                    "heading": -0.2429013405408753,
-                    "angularVelocity": -1.5396752424139517,
-                    "velocityX": -1.2913119630555505,
-                    "velocityY": 1.333630689503849,
-                    "timestamp": 0.22867672428720487
-                },
-                {
-                    "x": 7.3517522665520065,
-                    "y": 2.7960803575645476,
-                    "heading": -0.3842395530561246,
-                    "angularVelocity": -1.8542098627108627,
-                    "velocityX": -1.7297916052318956,
-                    "velocityY": 1.785262170944986,
-                    "timestamp": 0.3049022990496065
-                },
-                {
-                    "x": 7.186045392769314,
-                    "y": 2.9668045775727996,
-                    "heading": -0.5218107815453775,
-                    "angularVelocity": -1.8047909631126855,
-                    "velocityX": -2.173901270002986,
-                    "velocityY": 2.2397236169147496,
-                    "timestamp": 0.3811278738120081
-                },
-                {
-                    "x": 6.990773522032128,
-                    "y": 3.1645972581511623,
-                    "heading": -0.5514162056749212,
-                    "angularVelocity": -0.38839227151549954,
-                    "velocityX": -2.5617631791673237,
-                    "velocityY": 2.59483357383517,
-                    "timestamp": 0.4573534485744097
-                },
-                {
-                    "x": 6.791088913724043,
-                    "y": 3.3726253897030523,
-                    "heading": -0.5514162553808045,
-                    "angularVelocity": -6.520893213469087e-7,
-                    "velocityX": -2.619653691435051,
-                    "velocityY": 2.729112009982516,
-                    "timestamp": 0.5335790233368113
-                },
-                {
-                    "x": 6.578307530314685,
-                    "y": 3.567237130643777,
-                    "heading": -0.5514163004289083,
-                    "angularVelocity": -5.90984115130558e-7,
-                    "velocityX": -2.7914697143656473,
-                    "velocityY": 2.55310296507882,
-                    "timestamp": 0.609804598099213
-                },
-                {
-                    "x": 6.335948512349157,
-                    "y": 3.7234797461898013,
-                    "heading": -0.551416294920752,
-                    "angularVelocity": 7.226126339733625e-8,
-                    "velocityX": -3.179497415676709,
-                    "velocityY": 2.0497400778287207,
-                    "timestamp": 0.6860301728616146
-                },
-                {
-                    "x": 6.088743524617576,
-                    "y": 3.871936280628758,
-                    "heading": -0.5514162894475348,
-                    "angularVelocity": 7.180289728327137e-8,
-                    "velocityX": -3.2430714822699573,
-                    "velocityY": 1.9475948184280927,
-                    "timestamp": 0.7622557476240163
-                },
-                {
-                    "x": 5.841537785943911,
-                    "y": 4.020391564616265,
-                    "heading": -0.5514162839746023,
-                    "angularVelocity": 7.179916458555982e-8,
-                    "velocityX": -3.2430813338464923,
-                    "velocityY": 1.947578413809903,
-                    "timestamp": 0.8384813223864179
-                },
-                {
-                    "x": 5.581140041351318,
-                    "y": 4.144257068634033,
-                    "heading": -0.5514162991479159,
-                    "angularVelocity": -1.9905804152585698e-7,
-                    "velocityX": -3.416146685731968,
-                    "velocityY": 1.6249861598795787,
-                    "timestamp": 0.9147068971488196
-                },
-                {
-                    "x": 5.261335999197585,
-                    "y": 4.231658536911035,
-                    "heading": -0.5514164840698423,
-                    "angularVelocity": -0.000002110046686592666,
-                    "velocityX": -3.6491154543506057,
-                    "velocityY": 0.9972921119903242,
-                    "timestamp": 1.0023456814389826
-                },
-                {
-                    "x": 4.96430123021485,
-                    "y": 4.378912897735688,
-                    "heading": -0.551417377431188,
-                    "angularVelocity": -0.000010193675698949444,
-                    "velocityX": -3.3893072728996803,
-                    "velocityY": 1.6802419387415284,
-                    "timestamp": 1.0899844657291458
-                },
-                {
-                    "x": 4.714895393240272,
-                    "y": 4.564810737884653,
-                    "heading": -0.6189140340648516,
-                    "angularVelocity": -0.7701687920520328,
-                    "velocityX": -2.8458386203626582,
-                    "velocityY": 2.121182324180531,
-                    "timestamp": 1.177623250019309
-                },
-                {
-                    "x": 4.521287798442847,
-                    "y": 4.7820097530438215,
-                    "heading": -0.6965700023585185,
-                    "angularVelocity": -0.8860913455457793,
-                    "velocityX": -2.209154273026079,
-                    "velocityY": 2.4783435429688825,
-                    "timestamp": 1.265262034309472
-                },
-                {
-                    "x": 4.386338131009024,
-                    "y": 5.001818806087968,
-                    "heading": -0.6848521841568085,
-                    "angularVelocity": 0.1337058506301677,
-                    "velocityX": -1.539839564490294,
-                    "velocityY": 2.5081253103235808,
-                    "timestamp": 1.3529008185996352
-                },
-                {
-                    "x": 4.301033328473477,
-                    "y": 5.211510806429668,
-                    "heading": -0.519384386353178,
-                    "angularVelocity": 1.888065873389841,
-                    "velocityX": -0.9733681637243018,
-                    "velocityY": 2.3926849515327757,
-                    "timestamp": 1.4405396028897983
-                },
-                {
-                    "x": 4.267176527868507,
-                    "y": 5.385808771912563,
-                    "heading": -0.30251041793580363,
-                    "angularVelocity": 2.474634605830756,
-                    "velocityX": -0.38632211616348305,
-                    "velocityY": 1.988822265103685,
-                    "timestamp": 1.5281783871799615
-                },
-                {
-                    "x": 4.261077190237836,
-                    "y": 5.502861748658668,
-                    "heading": -0.11181365176971161,
-                    "angularVelocity": 2.175940340919318,
-                    "velocityX": -0.06959632861263781,
-                    "velocityY": 1.335629854911658,
-                    "timestamp": 1.6158171714701246
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 9.015876776981611e-33,
-                    "angularVelocity": 1.275846677647969,
-                    "velocityX": 0.021940169272516404,
-                    "velocityY": 0.6519744860010044,
-                    "timestamp": 1.7034559557602877
-                },
-                {
-                    "x": 4.263,
-                    "y": 5.56,
-                    "heading": 0,
-                    "angularVelocity": -9.923717904588528e-32,
-                    "velocityX": -3.152679648787722e-32,
-                    "velocityY": -1.9738855170003778e-32,
-                    "timestamp": 1.7910947400504509
-                }
-            ],
-            "constraints": [
-                {
-                    "scope": [
-                        "first"
-                    ],
-                    "type": "StopPoint"
-                },
-                {
-                    "scope": [
-                        "last"
-                    ],
-                    "type": "StopPoint"
-                }
-            ],
-            "usesControlIntervalGuessing": true,
-            "defaultControlIntervalCount": 40,
-            "usesDefaultFieldObstacles": true,
-            "circleObstacles": [
-                {
-                    "x": 3.42,
-                    "y": 4.04,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 2.81,
-                    "radius": 0.5
-                },
-                {
-                    "x": 5.61,
-                    "y": 5.44,
-                    "radius": 0.5
-                }
-            ]
-        }
-    },
-    "splitTrajectoriesAtStopPoints": false,
-    "usesObstacles": true
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.1.traj
deleted file mode 100644
index 0c5693aa..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.1.traj	
+++ /dev/null
@@ -1,328 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": 1.821187546700747e-33,
-            "velocityX": -3.3437580769683535e-34,
-            "velocityY": -3.622513119436198e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7214806358425874,
-            "y": 6.713430005414815,
-            "heading": 1.0269113199415918,
-            "angularVelocity": -0.32293003145597954,
-            "velocityX": 0.28325561303637664,
-            "velocityY": -0.1620979769229106,
-            "timestamp": 0.04053100914583816
-        },
-        {
-            "x": 0.7444896856754503,
-            "y": 6.700284020801432,
-            "heading": 1.001394622951622,
-            "angularVelocity": -0.6295598734824499,
-            "velocityX": 0.5676900308618518,
-            "velocityY": -0.32434387621788874,
-            "timestamp": 0.08106201829167632
-        },
-        {
-            "x": 0.7790845165676208,
-            "y": 6.680557699463914,
-            "heading": 0.9642667952582056,
-            "angularVelocity": -0.916035116713321,
-            "velocityX": 0.8535398358252526,
-            "velocityY": -0.4866970192264045,
-            "timestamp": 0.12159302743751449
-        },
-        {
-            "x": 0.8253351719483972,
-            "y": 6.654251504147771,
-            "heading": 0.9165522532050936,
-            "angularVelocity": -1.177235481145466,
-            "velocityX": 1.141117785011419,
-            "velocityY": -0.6490387451614623,
-            "timestamp": 0.16212403658335264
-        },
-        {
-            "x": 0.883327254582786,
-            "y": 6.621374965178265,
-            "heading": 0.8595502901552765,
-            "angularVelocity": -1.4063790724951688,
-            "velocityX": 1.4308077656226736,
-            "velocityY": -0.8111453344576391,
-            "timestamp": 0.2026550457291908
-        },
-        {
-            "x": 0.953164245540845,
-            "y": 6.581952530815573,
-            "heading": 0.7949436194315174,
-            "angularVelocity": -1.5940059743218395,
-            "velocityX": 1.7230508795567456,
-            "velocityY": -0.9726487248526893,
-            "timestamp": 0.24318605487502895
-        },
-        {
-            "x": 1.0349687566216221,
-            "y": 6.5360339228116,
-            "heading": 0.7250282822193501,
-            "angularVelocity": -1.724983874953593,
-            "velocityX": 2.0183191291001217,
-            "velocityY": -1.132925356947043,
-            "timestamp": 0.2837170640208671
-        },
-        {
-            "x": 1.1288774102342096,
-            "y": 6.483723562804198,
-            "heading": 0.6532984652847575,
-            "angularVelocity": -1.7697515666707262,
-            "velocityX": 2.316958190571729,
-            "velocityY": -1.2906256495904058,
-            "timestamp": 0.3242480731667053
-        },
-        {
-            "x": 1.2349754296776467,
-            "y": 6.425294263584176,
-            "heading": 0.5860844259008366,
-            "angularVelocity": -1.6583361924711089,
-            "velocityX": 2.6176999211067367,
-            "velocityY": -1.4415949775586157,
-            "timestamp": 0.36477908231254347
-        },
-        {
-            "x": 1.3525875597286727,
-            "y": 6.361480505266573,
-            "heading": 0.5370018573395918,
-            "angularVelocity": -1.2109880705076077,
-            "velocityX": 2.901781439189822,
-            "velocityY": -1.5744428688658776,
-            "timestamp": 0.40531009145838165
-        },
-        {
-            "x": 1.481480430033789,
-            "y": 6.296375538238497,
-            "heading": 0.5147559483468728,
-            "angularVelocity": -0.5488614633964374,
-            "velocityX": 3.1801051348446676,
-            "velocityY": -1.6063001736230227,
-            "timestamp": 0.44584110060421983
-        },
-        {
-            "x": 1.6221023727568105,
-            "y": 6.235265511699909,
-            "heading": 0.514755821640399,
-            "angularVelocity": -0.0000031261613391085408,
-            "velocityX": 3.469490291175273,
-            "velocityY": -1.5077351348125194,
-            "timestamp": 0.486372109750058
-        },
-        {
-            "x": 1.7676170388967807,
-            "y": 6.186949296866798,
-            "heading": 0.5147557675556996,
-            "angularVelocity": -0.0000013344029787740425,
-            "velocityX": 3.5902058499550584,
-            "velocityY": -1.1920802331681561,
-            "timestamp": 0.5269031188958961
-        },
-        {
-            "x": 1.9168630838394165,
-            "y": 6.151812553405762,
-            "heading": 0.5147557009631692,
-            "angularVelocity": -0.000001643002030243893,
-            "velocityX": 3.6822681716515033,
-            "velocityY": -0.866910155989649,
-            "timestamp": 0.5674341280417343
-        },
-        {
-            "x": 2.2488381415683802,
-            "y": 6.139406390407425,
-            "heading": 0.5147556500188685,
-            "angularVelocity": -5.801183495483463e-7,
-            "velocityX": 3.780301616970564,
-            "velocityY": -0.14127277622555945,
-            "timestamp": 0.6552512102128125
-        },
-        {
-            "x": 2.57698446655607,
-            "y": 6.191188083838006,
-            "heading": 0.5147556048038259,
-            "angularVelocity": -5.148775328112685e-7,
-            "velocityX": 3.7367026650739894,
-            "velocityY": 0.58965399612918,
-            "timestamp": 0.7430682923838907
-        },
-        {
-            "x": 2.8993349699211755,
-            "y": 6.271509362001365,
-            "heading": 0.5147555599835898,
-            "angularVelocity": -5.103817495962876e-7,
-            "velocityX": 3.6707038698590346,
-            "velocityY": 0.9146429849136081,
-            "timestamp": 0.8308853745549689
-        },
-        {
-            "x": 3.221685430514468,
-            "y": 6.351830811819113,
-            "heading": 0.514755515163351,
-            "angularVelocity": -5.103817801462039e-7,
-            "velocityX": 3.6707033828033078,
-            "velocityY": 0.9146449395946943,
-            "timestamp": 0.9187024567260471
-        },
-        {
-            "x": 3.5440358911075664,
-            "y": 6.432152261637641,
-            "heading": 0.514755470343112,
-            "angularVelocity": -5.103817833267703e-7,
-            "velocityX": 3.670703382801096,
-            "velocityY": 0.9146449396035715,
-            "timestamp": 1.0065195388971253
-        },
-        {
-            "x": 3.8663863517006654,
-            "y": 6.512473711456168,
-            "heading": 0.5147554255228727,
-            "angularVelocity": -5.103817863197995e-7,
-            "velocityX": 3.6707033828010953,
-            "velocityY": 0.9146449396035711,
-            "timestamp": 1.0943366210682035
-        },
-        {
-            "x": 4.188736812293763,
-            "y": 6.592795161274696,
-            "heading": 0.5147553807026332,
-            "angularVelocity": -5.103817901912557e-7,
-            "velocityX": 3.6707033828010953,
-            "velocityY": 0.9146449396035711,
-            "timestamp": 1.1821537032392817
-        },
-        {
-            "x": 4.511087272886861,
-            "y": 6.673116611093223,
-            "heading": 0.5147553358823933,
-            "angularVelocity": -5.103817929337743e-7,
-            "velocityX": 3.6707033828010958,
-            "velocityY": 0.9146449396035705,
-            "timestamp": 1.26997078541036
-        },
-        {
-            "x": 4.833437733479959,
-            "y": 6.753438060911751,
-            "heading": 0.5147552910621531,
-            "angularVelocity": -5.103817959943963e-7,
-            "velocityX": 3.6707033828010953,
-            "velocityY": 0.9146449396035703,
-            "timestamp": 1.357787867581438
-        },
-        {
-            "x": 5.155788194073057,
-            "y": 6.833759510730278,
-            "heading": 0.5147552462419127,
-            "angularVelocity": -5.10381799453682e-7,
-            "velocityX": 3.670703382801095,
-            "velocityY": 0.9146449396035702,
-            "timestamp": 1.4456049497525163
-        },
-        {
-            "x": 5.4781386546661555,
-            "y": 6.914080960548806,
-            "heading": 0.514755201421672,
-            "angularVelocity": -5.10381803359495e-7,
-            "velocityX": 3.670703382801095,
-            "velocityY": 0.9146449396035701,
-            "timestamp": 1.5334220319235945
-        },
-        {
-            "x": 5.8004891152592535,
-            "y": 6.994402410367334,
-            "heading": 0.5147551566014309,
-            "angularVelocity": -5.1038180633665e-7,
-            "velocityX": 3.670703382801095,
-            "velocityY": 0.9146449396035699,
-            "timestamp": 1.6212391140946727
-        },
-        {
-            "x": 6.1228395758523515,
-            "y": 7.074723860185862,
-            "heading": 0.5147551117811896,
-            "angularVelocity": -5.103818092114042e-7,
-            "velocityX": 3.670703382801095,
-            "velocityY": 0.9146449396035696,
-            "timestamp": 1.709056196265751
-        },
-        {
-            "x": 6.4451900364454335,
-            "y": 7.1550453100044535,
-            "heading": 0.514755066960948,
-            "angularVelocity": -5.10381812997418e-7,
-            "velocityX": 3.6707033828009097,
-            "velocityY": 0.9146449396043118,
-            "timestamp": 1.796873278436829
-        },
-        {
-            "x": 6.767540493462307,
-            "y": 7.235366774175193,
-            "heading": 0.5147550221406476,
-            "angularVelocity": -5.103824823798507e-7,
-            "velocityX": 3.670703342077523,
-            "velocityY": 0.9146451030365995,
-            "timestamp": 1.8846903606079073
-        },
-        {
-            "x": 7.069460815974501,
-            "y": 7.313239150235045,
-            "heading": 0.4557447568037101,
-            "angularVelocity": -0.6719679574638842,
-            "velocityX": 3.438059145759549,
-            "velocityY": 0.8867565869263073,
-            "timestamp": 1.9725074427789855
-        },
-        {
-            "x": 7.3146543806816275,
-            "y": 7.37536322701953,
-            "heading": 0.2979903486408247,
-            "angularVelocity": -1.7963977424752164,
-            "velocityX": 2.7920941876599903,
-            "velocityY": 0.7074258817146762,
-            "timestamp": 2.0603245249500635
-        },
-        {
-            "x": 7.497733370662191,
-            "y": 7.421431644323229,
-            "heading": 0.15573272655395068,
-            "angularVelocity": -1.6199310950657548,
-            "velocityX": 2.084776508787937,
-            "velocityY": 0.5245951717451859,
-            "timestamp": 2.1481416071211417
-        },
-        {
-            "x": 7.619355739034295,
-            "y": 7.45187793306489,
-            "heading": 0.05195662802885072,
-            "angularVelocity": -1.1817302050975884,
-            "velocityX": 1.3849511435049664,
-            "velocityY": 0.34670121107359286,
-            "timestamp": 2.23595868929222
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -0.003,
-            "angularVelocity": -0.6258079484101811,
-            "velocityX": 0.6905747659386144,
-            "velocityY": 0.1721996058312404,
-            "timestamp": 2.323775771463298
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -0.003,
-            "angularVelocity": 0,
-            "velocityX": 2.3316560699920504e-34,
-            "velocityY": 0,
-            "timestamp": 2.4115928536343763
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.traj
deleted file mode 100644
index 0c5693aa..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.traj	
+++ /dev/null
@@ -1,328 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": 1.821187546700747e-33,
-            "velocityX": -3.3437580769683535e-34,
-            "velocityY": -3.622513119436198e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7214806358425874,
-            "y": 6.713430005414815,
-            "heading": 1.0269113199415918,
-            "angularVelocity": -0.32293003145597954,
-            "velocityX": 0.28325561303637664,
-            "velocityY": -0.1620979769229106,
-            "timestamp": 0.04053100914583816
-        },
-        {
-            "x": 0.7444896856754503,
-            "y": 6.700284020801432,
-            "heading": 1.001394622951622,
-            "angularVelocity": -0.6295598734824499,
-            "velocityX": 0.5676900308618518,
-            "velocityY": -0.32434387621788874,
-            "timestamp": 0.08106201829167632
-        },
-        {
-            "x": 0.7790845165676208,
-            "y": 6.680557699463914,
-            "heading": 0.9642667952582056,
-            "angularVelocity": -0.916035116713321,
-            "velocityX": 0.8535398358252526,
-            "velocityY": -0.4866970192264045,
-            "timestamp": 0.12159302743751449
-        },
-        {
-            "x": 0.8253351719483972,
-            "y": 6.654251504147771,
-            "heading": 0.9165522532050936,
-            "angularVelocity": -1.177235481145466,
-            "velocityX": 1.141117785011419,
-            "velocityY": -0.6490387451614623,
-            "timestamp": 0.16212403658335264
-        },
-        {
-            "x": 0.883327254582786,
-            "y": 6.621374965178265,
-            "heading": 0.8595502901552765,
-            "angularVelocity": -1.4063790724951688,
-            "velocityX": 1.4308077656226736,
-            "velocityY": -0.8111453344576391,
-            "timestamp": 0.2026550457291908
-        },
-        {
-            "x": 0.953164245540845,
-            "y": 6.581952530815573,
-            "heading": 0.7949436194315174,
-            "angularVelocity": -1.5940059743218395,
-            "velocityX": 1.7230508795567456,
-            "velocityY": -0.9726487248526893,
-            "timestamp": 0.24318605487502895
-        },
-        {
-            "x": 1.0349687566216221,
-            "y": 6.5360339228116,
-            "heading": 0.7250282822193501,
-            "angularVelocity": -1.724983874953593,
-            "velocityX": 2.0183191291001217,
-            "velocityY": -1.132925356947043,
-            "timestamp": 0.2837170640208671
-        },
-        {
-            "x": 1.1288774102342096,
-            "y": 6.483723562804198,
-            "heading": 0.6532984652847575,
-            "angularVelocity": -1.7697515666707262,
-            "velocityX": 2.316958190571729,
-            "velocityY": -1.2906256495904058,
-            "timestamp": 0.3242480731667053
-        },
-        {
-            "x": 1.2349754296776467,
-            "y": 6.425294263584176,
-            "heading": 0.5860844259008366,
-            "angularVelocity": -1.6583361924711089,
-            "velocityX": 2.6176999211067367,
-            "velocityY": -1.4415949775586157,
-            "timestamp": 0.36477908231254347
-        },
-        {
-            "x": 1.3525875597286727,
-            "y": 6.361480505266573,
-            "heading": 0.5370018573395918,
-            "angularVelocity": -1.2109880705076077,
-            "velocityX": 2.901781439189822,
-            "velocityY": -1.5744428688658776,
-            "timestamp": 0.40531009145838165
-        },
-        {
-            "x": 1.481480430033789,
-            "y": 6.296375538238497,
-            "heading": 0.5147559483468728,
-            "angularVelocity": -0.5488614633964374,
-            "velocityX": 3.1801051348446676,
-            "velocityY": -1.6063001736230227,
-            "timestamp": 0.44584110060421983
-        },
-        {
-            "x": 1.6221023727568105,
-            "y": 6.235265511699909,
-            "heading": 0.514755821640399,
-            "angularVelocity": -0.0000031261613391085408,
-            "velocityX": 3.469490291175273,
-            "velocityY": -1.5077351348125194,
-            "timestamp": 0.486372109750058
-        },
-        {
-            "x": 1.7676170388967807,
-            "y": 6.186949296866798,
-            "heading": 0.5147557675556996,
-            "angularVelocity": -0.0000013344029787740425,
-            "velocityX": 3.5902058499550584,
-            "velocityY": -1.1920802331681561,
-            "timestamp": 0.5269031188958961
-        },
-        {
-            "x": 1.9168630838394165,
-            "y": 6.151812553405762,
-            "heading": 0.5147557009631692,
-            "angularVelocity": -0.000001643002030243893,
-            "velocityX": 3.6822681716515033,
-            "velocityY": -0.866910155989649,
-            "timestamp": 0.5674341280417343
-        },
-        {
-            "x": 2.2488381415683802,
-            "y": 6.139406390407425,
-            "heading": 0.5147556500188685,
-            "angularVelocity": -5.801183495483463e-7,
-            "velocityX": 3.780301616970564,
-            "velocityY": -0.14127277622555945,
-            "timestamp": 0.6552512102128125
-        },
-        {
-            "x": 2.57698446655607,
-            "y": 6.191188083838006,
-            "heading": 0.5147556048038259,
-            "angularVelocity": -5.148775328112685e-7,
-            "velocityX": 3.7367026650739894,
-            "velocityY": 0.58965399612918,
-            "timestamp": 0.7430682923838907
-        },
-        {
-            "x": 2.8993349699211755,
-            "y": 6.271509362001365,
-            "heading": 0.5147555599835898,
-            "angularVelocity": -5.103817495962876e-7,
-            "velocityX": 3.6707038698590346,
-            "velocityY": 0.9146429849136081,
-            "timestamp": 0.8308853745549689
-        },
-        {
-            "x": 3.221685430514468,
-            "y": 6.351830811819113,
-            "heading": 0.514755515163351,
-            "angularVelocity": -5.103817801462039e-7,
-            "velocityX": 3.6707033828033078,
-            "velocityY": 0.9146449395946943,
-            "timestamp": 0.9187024567260471
-        },
-        {
-            "x": 3.5440358911075664,
-            "y": 6.432152261637641,
-            "heading": 0.514755470343112,
-            "angularVelocity": -5.103817833267703e-7,
-            "velocityX": 3.670703382801096,
-            "velocityY": 0.9146449396035715,
-            "timestamp": 1.0065195388971253
-        },
-        {
-            "x": 3.8663863517006654,
-            "y": 6.512473711456168,
-            "heading": 0.5147554255228727,
-            "angularVelocity": -5.103817863197995e-7,
-            "velocityX": 3.6707033828010953,
-            "velocityY": 0.9146449396035711,
-            "timestamp": 1.0943366210682035
-        },
-        {
-            "x": 4.188736812293763,
-            "y": 6.592795161274696,
-            "heading": 0.5147553807026332,
-            "angularVelocity": -5.103817901912557e-7,
-            "velocityX": 3.6707033828010953,
-            "velocityY": 0.9146449396035711,
-            "timestamp": 1.1821537032392817
-        },
-        {
-            "x": 4.511087272886861,
-            "y": 6.673116611093223,
-            "heading": 0.5147553358823933,
-            "angularVelocity": -5.103817929337743e-7,
-            "velocityX": 3.6707033828010958,
-            "velocityY": 0.9146449396035705,
-            "timestamp": 1.26997078541036
-        },
-        {
-            "x": 4.833437733479959,
-            "y": 6.753438060911751,
-            "heading": 0.5147552910621531,
-            "angularVelocity": -5.103817959943963e-7,
-            "velocityX": 3.6707033828010953,
-            "velocityY": 0.9146449396035703,
-            "timestamp": 1.357787867581438
-        },
-        {
-            "x": 5.155788194073057,
-            "y": 6.833759510730278,
-            "heading": 0.5147552462419127,
-            "angularVelocity": -5.10381799453682e-7,
-            "velocityX": 3.670703382801095,
-            "velocityY": 0.9146449396035702,
-            "timestamp": 1.4456049497525163
-        },
-        {
-            "x": 5.4781386546661555,
-            "y": 6.914080960548806,
-            "heading": 0.514755201421672,
-            "angularVelocity": -5.10381803359495e-7,
-            "velocityX": 3.670703382801095,
-            "velocityY": 0.9146449396035701,
-            "timestamp": 1.5334220319235945
-        },
-        {
-            "x": 5.8004891152592535,
-            "y": 6.994402410367334,
-            "heading": 0.5147551566014309,
-            "angularVelocity": -5.1038180633665e-7,
-            "velocityX": 3.670703382801095,
-            "velocityY": 0.9146449396035699,
-            "timestamp": 1.6212391140946727
-        },
-        {
-            "x": 6.1228395758523515,
-            "y": 7.074723860185862,
-            "heading": 0.5147551117811896,
-            "angularVelocity": -5.103818092114042e-7,
-            "velocityX": 3.670703382801095,
-            "velocityY": 0.9146449396035696,
-            "timestamp": 1.709056196265751
-        },
-        {
-            "x": 6.4451900364454335,
-            "y": 7.1550453100044535,
-            "heading": 0.514755066960948,
-            "angularVelocity": -5.10381812997418e-7,
-            "velocityX": 3.6707033828009097,
-            "velocityY": 0.9146449396043118,
-            "timestamp": 1.796873278436829
-        },
-        {
-            "x": 6.767540493462307,
-            "y": 7.235366774175193,
-            "heading": 0.5147550221406476,
-            "angularVelocity": -5.103824823798507e-7,
-            "velocityX": 3.670703342077523,
-            "velocityY": 0.9146451030365995,
-            "timestamp": 1.8846903606079073
-        },
-        {
-            "x": 7.069460815974501,
-            "y": 7.313239150235045,
-            "heading": 0.4557447568037101,
-            "angularVelocity": -0.6719679574638842,
-            "velocityX": 3.438059145759549,
-            "velocityY": 0.8867565869263073,
-            "timestamp": 1.9725074427789855
-        },
-        {
-            "x": 7.3146543806816275,
-            "y": 7.37536322701953,
-            "heading": 0.2979903486408247,
-            "angularVelocity": -1.7963977424752164,
-            "velocityX": 2.7920941876599903,
-            "velocityY": 0.7074258817146762,
-            "timestamp": 2.0603245249500635
-        },
-        {
-            "x": 7.497733370662191,
-            "y": 7.421431644323229,
-            "heading": 0.15573272655395068,
-            "angularVelocity": -1.6199310950657548,
-            "velocityX": 2.084776508787937,
-            "velocityY": 0.5245951717451859,
-            "timestamp": 2.1481416071211417
-        },
-        {
-            "x": 7.619355739034295,
-            "y": 7.45187793306489,
-            "heading": 0.05195662802885072,
-            "angularVelocity": -1.1817302050975884,
-            "velocityX": 1.3849511435049664,
-            "velocityY": 0.34670121107359286,
-            "timestamp": 2.23595868929222
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -0.003,
-            "angularVelocity": -0.6258079484101811,
-            "velocityX": 0.6905747659386144,
-            "velocityY": 0.1721996058312404,
-            "timestamp": 2.323775771463298
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -0.003,
-            "angularVelocity": 0,
-            "velocityX": 2.3316560699920504e-34,
-            "velocityY": 0,
-            "timestamp": 2.4115928536343763
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.1.traj
deleted file mode 100644
index 38a8bead..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.1.traj	
+++ /dev/null
@@ -1,364 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": -7.919640222280865e-31,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7245966822673344,
-            "y": 6.714849739719678,
-            "heading": 1.024358911223375,
-            "angularVelocity": -0.35643014194665995,
-            "velocityX": 0.33263013891161514,
-            "velocityY": -0.1173644641361684,
-            "timestamp": 0.04388262084457728
-        },
-        {
-            "x": 0.7538441442080397,
-            "y": 6.704534883949825,
-            "heading": 0.9938971402916456,
-            "angularVelocity": -0.6941648047781096,
-            "velocityX": 0.6664930530083778,
-            "velocityY": -0.23505559994630235,
-            "timestamp": 0.08776524168915456
-        },
-        {
-            "x": 0.7978084886925392,
-            "y": 6.689039857166789,
-            "heading": 0.9496619935965729,
-            "angularVelocity": -1.0080333818652591,
-            "velocityX": 1.0018623235886674,
-            "velocityY": -0.3531016717960754,
-            "timestamp": 0.13164786253373184
-        },
-        {
-            "x": 0.856569122575896,
-            "y": 6.668349449234606,
-            "heading": 0.8929800253175195,
-            "angularVelocity": -1.291672356576085,
-            "velocityX": 1.3390411227143098,
-            "velocityY": -0.4714943532072734,
-            "timestamp": 0.17553048337830912
-        },
-        {
-            "x": 0.9302192448226602,
-            "y": 6.642448160442897,
-            "heading": 0.8255526698192454,
-            "angularVelocity": -1.5365389350167389,
-            "velocityX": 1.6783437458676926,
-            "velocityY": -0.5902402430203169,
-            "timestamp": 0.2194131042228864
-        },
-        {
-            "x": 1.018865391503866,
-            "y": 6.6113159895257,
-            "heading": 0.7496899973128085,
-            "angularVelocity": -1.728763484184805,
-            "velocityX": 2.020074119893375,
-            "velocityY": -0.7094419229748447,
-            "timestamp": 0.2632957250674637
-        },
-        {
-            "x": 1.122620892464334,
-            "y": 6.574924283447733,
-            "heading": 0.6689630682339129,
-            "angularVelocity": -1.839610477341524,
-            "velocityX": 2.364387061747502,
-            "velocityY": -0.8292965501492129,
-            "timestamp": 0.30717834591204096
-        },
-        {
-            "x": 1.2415335684286195,
-            "y": 6.533261700289126,
-            "heading": 0.5901648629283005,
-            "angularVelocity": -1.7956585953398425,
-            "velocityX": 2.7097897453635067,
-            "velocityY": -0.9494096377266702,
-            "timestamp": 0.35106096675661824
-        },
-        {
-            "x": 1.3748018786634473,
-            "y": 6.486525528750173,
-            "heading": 0.5293493772235701,
-            "angularVelocity": -1.3858672188271242,
-            "velocityX": 3.03692686694496,
-            "velocityY": -1.0650268976507753,
-            "timestamp": 0.3949435876011955
-        },
-        {
-            "x": 1.5197726766571482,
-            "y": 6.4335443318697845,
-            "heading": 0.5007639504532418,
-            "angularVelocity": -0.6514065527572436,
-            "velocityX": 3.3036039143420037,
-            "velocityY": -1.2073389387557742,
-            "timestamp": 0.4388262084457728
-        },
-        {
-            "x": 1.6765216724762246,
-            "y": 6.378886133078717,
-            "heading": 0.5007639037196894,
-            "angularVelocity": -0.0000010649672122148667,
-            "velocityX": 3.572006247627606,
-            "velocityY": -1.2455545666831078,
-            "timestamp": 0.4827088292903501
-        },
-        {
-            "x": 1.8335562881896872,
-            "y": 6.325054024772622,
-            "heading": 0.5007638569717894,
-            "angularVelocity": -0.0000010652941670826544,
-            "velocityX": 3.5785149722331555,
-            "velocityY": -1.2267295633425939,
-            "timestamp": 0.5265914501349274
-        },
-        {
-            "x": 1.9950553751457016,
-            "y": 6.286637737718841,
-            "heading": 0.5007638092073705,
-            "angularVelocity": -0.0000010884586644439279,
-            "velocityX": 3.6802516314615477,
-            "velocityY": -0.8754328322739401,
-            "timestamp": 0.5704740709795046
-        },
-        {
-            "x": 2.159510374069214,
-            "y": 6.264003276824951,
-            "heading": 0.500763757205963,
-            "angularVelocity": -0.0000011850114334401085,
-            "velocityX": 3.7476111444210547,
-            "velocityY": -0.5157955577412445,
-            "timestamp": 0.6143566918240819
-        },
-        {
-            "x": 2.4127218287177645,
-            "y": 6.26676408374459,
-            "heading": 0.5007637073683532,
-            "angularVelocity": -7.445219928061725e-7,
-            "velocityX": 3.7827154561393628,
-            "velocityY": 0.04124358046976413,
-            "timestamp": 0.681295761808209
-        },
-        {
-            "x": 2.6627712176567337,
-            "y": 6.306751164644663,
-            "heading": 0.5007636590304977,
-            "angularVelocity": -7.221172264287654e-7,
-            "velocityX": 3.7354774871878274,
-            "velocityY": 0.5973653489565757,
-            "timestamp": 0.7482348317923362
-        },
-        {
-            "x": 2.9117460938747426,
-            "y": 6.352959114176208,
-            "heading": 0.5007636106421453,
-            "angularVelocity": -7.22871595873179e-7,
-            "velocityX": 3.719425386056917,
-            "velocityY": 0.6902986483459334,
-            "timestamp": 0.8151739017764633
-        },
-        {
-            "x": 3.160720959556854,
-            "y": 6.399167120476624,
-            "heading": 0.5007635622537979,
-            "angularVelocity": -7.228715220114282e-7,
-            "velocityX": 3.719425228661581,
-            "velocityY": 0.6902994964137207,
-            "timestamp": 0.8821129717605904
-        },
-        {
-            "x": 3.409695827610573,
-            "y": 6.445375113998521,
-            "heading": 0.5007635138654579,
-            "angularVelocity": -7.228714109969877e-7,
-            "velocityX": 3.719425264090935,
-            "velocityY": 0.6902993055159711,
-            "timestamp": 0.9490520417447176
-        },
-        {
-            "x": 3.658670698533997,
-            "y": 6.491583092058085,
-            "heading": 0.5007634654771249,
-            "angularVelocity": -7.228713077638952e-7,
-            "velocityX": 3.719425306961334,
-            "velocityY": 0.6902990745248123,
-            "timestamp": 1.0159911117288447
-        },
-        {
-            "x": 3.9076455729725814,
-            "y": 6.537791051177508,
-            "heading": 0.5007634170887977,
-            "angularVelocity": -7.228712197353323e-7,
-            "velocityX": 3.7194253594741626,
-            "velocityY": 0.6902987915788225,
-            "timestamp": 1.082930181712972
-        },
-        {
-            "x": 4.156620451761658,
-            "y": 6.583998986855888,
-            "heading": 0.5007633687004744,
-            "angularVelocity": -7.228711615866084e-7,
-            "velocityX": 3.7194254244659963,
-            "velocityY": 0.6902984413940524,
-            "timestamp": 1.1498692516970992
-        },
-        {
-            "x": 4.405595335954841,
-            "y": 6.630206893416186,
-            "heading": 0.5007633203121516,
-            "angularVelocity": -7.22871154439704e-7,
-            "velocityX": 3.719425505197721,
-            "velocityY": 0.6902980064002329,
-            "timestamp": 1.2168083216812264
-        },
-        {
-            "x": 4.65457022677848,
-            "y": 6.676414764250624,
-            "heading": 0.5007632719238245,
-            "angularVelocity": -7.228712197620323e-7,
-            "velocityX": 3.719425604249829,
-            "velocityY": 0.690297472693087,
-            "timestamp": 1.2837473916653537
-        },
-        {
-            "x": 4.903545125380086,
-            "y": 6.722622593176218,
-            "heading": 0.5007632235354964,
-            "angularVelocity": -7.228712343344299e-7,
-            "velocityX": 3.7194257204445353,
-            "velocityY": 0.69029684661859,
-            "timestamp": 1.350686461649481
-        },
-        {
-            "x": 5.152520035964963,
-            "y": 6.768830357534066,
-            "heading": 0.500763175147177,
-            "angularVelocity": -7.228711020086952e-7,
-            "velocityX": 3.7194258994621494,
-            "velocityY": 0.6902958820437133,
-            "timestamp": 1.4176255316336082
-        },
-        {
-            "x": 5.401781723000098,
-            "y": 6.813465264887065,
-            "heading": 0.5007631267776413,
-            "angularVelocity": -7.225904944160688e-7,
-            "velocityX": 3.7237100410005364,
-            "velocityY": 0.6667990362518919,
-            "timestamp": 1.4845646016177354
-        },
-        {
-            "x": 5.654897775782049,
-            "y": 6.8209436894170015,
-            "heading": 0.5007630775196736,
-            "angularVelocity": -7.358627453849351e-7,
-            "velocityX": 3.781290251597022,
-            "velocityY": 0.11171987498067794,
-            "timestamp": 1.5515036716018626
-        },
-        {
-            "x": 5.9063591957092285,
-            "y": 6.791097164154053,
-            "heading": 0.500763020915307,
-            "angularVelocity": -8.456102929151622e-7,
-            "velocityX": 3.756571759763139,
-            "velocityY": -0.44587600739018746,
-            "timestamp": 1.6184427415859899
-        },
-        {
-            "x": 6.200798567763329,
-            "y": 6.700713487178965,
-            "heading": 0.5007629755779084,
-            "angularVelocity": -5.568470838728409e-7,
-            "velocityX": 3.616389792279645,
-            "velocityY": -1.1101185433221765,
-            "timestamp": 1.6998607824959264
-        },
-        {
-            "x": 6.474342410112796,
-            "y": 6.559159535827226,
-            "heading": 0.5007629354438458,
-            "angularVelocity": -4.929382012989922e-7,
-            "velocityX": 3.3597448340971368,
-            "velocityY": -1.7386066990769444,
-            "timestamp": 1.781278823405863
-        },
-        {
-            "x": 6.734766818786864,
-            "y": 6.394712333057686,
-            "heading": 0.5007628957809265,
-            "angularVelocity": -4.871514819255018e-7,
-            "velocityX": 3.1986081433983946,
-            "velocityY": -2.0197882549329598,
-            "timestamp": 1.8626968643157995
-        },
-        {
-            "x": 6.9951910734023155,
-            "y": 6.230264886316009,
-            "heading": 0.5007628561176116,
-            "angularVelocity": -4.871563403692846e-7,
-            "velocityX": 3.198606251205787,
-            "velocityY": -2.019791251469529,
-            "timestamp": 1.944114905225736
-        },
-        {
-            "x": 7.223286683326485,
-            "y": 6.0852317833380125,
-            "heading": 0.4070504938542789,
-            "angularVelocity": -1.1510024217727797,
-            "velocityX": 2.801536457705794,
-            "velocityY": -1.781338648745319,
-            "timestamp": 2.0255329461356726
-        },
-        {
-            "x": 7.40663062232063,
-            "y": 5.969243166944327,
-            "heading": 0.26222076447726717,
-            "angularVelocity": -1.7788407551738947,
-            "velocityX": 2.2518834516904875,
-            "velocityY": -1.4246058379369684,
-            "timestamp": 2.106950987045609
-        },
-        {
-            "x": 7.543566910533025,
-            "y": 5.882836756425157,
-            "heading": 0.1356651998410208,
-            "angularVelocity": -1.5543921620054602,
-            "velocityX": 1.6818912206923584,
-            "velocityY": -1.061268602799615,
-            "timestamp": 2.1883690279555457
-        },
-        {
-            "x": 7.634586620516168,
-            "y": 5.825537139087579,
-            "heading": 0.04464428184824825,
-            "angularVelocity": -1.1179453223820228,
-            "velocityX": 1.1179304852572838,
-            "velocityY": -0.7037705242866615,
-            "timestamp": 2.269787068865482
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -0.003,
-            "angularVelocity": -0.5851808925364741,
-            "velocityX": 0.5577803024524691,
-            "velocityY": -0.35050142165846687,
-            "timestamp": 2.3512051097754187
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -0.003,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 2.4326231506853553
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.traj
deleted file mode 100644
index 38a8bead..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.traj	
+++ /dev/null
@@ -1,364 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": -7.919640222280865e-31,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7245966822673344,
-            "y": 6.714849739719678,
-            "heading": 1.024358911223375,
-            "angularVelocity": -0.35643014194665995,
-            "velocityX": 0.33263013891161514,
-            "velocityY": -0.1173644641361684,
-            "timestamp": 0.04388262084457728
-        },
-        {
-            "x": 0.7538441442080397,
-            "y": 6.704534883949825,
-            "heading": 0.9938971402916456,
-            "angularVelocity": -0.6941648047781096,
-            "velocityX": 0.6664930530083778,
-            "velocityY": -0.23505559994630235,
-            "timestamp": 0.08776524168915456
-        },
-        {
-            "x": 0.7978084886925392,
-            "y": 6.689039857166789,
-            "heading": 0.9496619935965729,
-            "angularVelocity": -1.0080333818652591,
-            "velocityX": 1.0018623235886674,
-            "velocityY": -0.3531016717960754,
-            "timestamp": 0.13164786253373184
-        },
-        {
-            "x": 0.856569122575896,
-            "y": 6.668349449234606,
-            "heading": 0.8929800253175195,
-            "angularVelocity": -1.291672356576085,
-            "velocityX": 1.3390411227143098,
-            "velocityY": -0.4714943532072734,
-            "timestamp": 0.17553048337830912
-        },
-        {
-            "x": 0.9302192448226602,
-            "y": 6.642448160442897,
-            "heading": 0.8255526698192454,
-            "angularVelocity": -1.5365389350167389,
-            "velocityX": 1.6783437458676926,
-            "velocityY": -0.5902402430203169,
-            "timestamp": 0.2194131042228864
-        },
-        {
-            "x": 1.018865391503866,
-            "y": 6.6113159895257,
-            "heading": 0.7496899973128085,
-            "angularVelocity": -1.728763484184805,
-            "velocityX": 2.020074119893375,
-            "velocityY": -0.7094419229748447,
-            "timestamp": 0.2632957250674637
-        },
-        {
-            "x": 1.122620892464334,
-            "y": 6.574924283447733,
-            "heading": 0.6689630682339129,
-            "angularVelocity": -1.839610477341524,
-            "velocityX": 2.364387061747502,
-            "velocityY": -0.8292965501492129,
-            "timestamp": 0.30717834591204096
-        },
-        {
-            "x": 1.2415335684286195,
-            "y": 6.533261700289126,
-            "heading": 0.5901648629283005,
-            "angularVelocity": -1.7956585953398425,
-            "velocityX": 2.7097897453635067,
-            "velocityY": -0.9494096377266702,
-            "timestamp": 0.35106096675661824
-        },
-        {
-            "x": 1.3748018786634473,
-            "y": 6.486525528750173,
-            "heading": 0.5293493772235701,
-            "angularVelocity": -1.3858672188271242,
-            "velocityX": 3.03692686694496,
-            "velocityY": -1.0650268976507753,
-            "timestamp": 0.3949435876011955
-        },
-        {
-            "x": 1.5197726766571482,
-            "y": 6.4335443318697845,
-            "heading": 0.5007639504532418,
-            "angularVelocity": -0.6514065527572436,
-            "velocityX": 3.3036039143420037,
-            "velocityY": -1.2073389387557742,
-            "timestamp": 0.4388262084457728
-        },
-        {
-            "x": 1.6765216724762246,
-            "y": 6.378886133078717,
-            "heading": 0.5007639037196894,
-            "angularVelocity": -0.0000010649672122148667,
-            "velocityX": 3.572006247627606,
-            "velocityY": -1.2455545666831078,
-            "timestamp": 0.4827088292903501
-        },
-        {
-            "x": 1.8335562881896872,
-            "y": 6.325054024772622,
-            "heading": 0.5007638569717894,
-            "angularVelocity": -0.0000010652941670826544,
-            "velocityX": 3.5785149722331555,
-            "velocityY": -1.2267295633425939,
-            "timestamp": 0.5265914501349274
-        },
-        {
-            "x": 1.9950553751457016,
-            "y": 6.286637737718841,
-            "heading": 0.5007638092073705,
-            "angularVelocity": -0.0000010884586644439279,
-            "velocityX": 3.6802516314615477,
-            "velocityY": -0.8754328322739401,
-            "timestamp": 0.5704740709795046
-        },
-        {
-            "x": 2.159510374069214,
-            "y": 6.264003276824951,
-            "heading": 0.500763757205963,
-            "angularVelocity": -0.0000011850114334401085,
-            "velocityX": 3.7476111444210547,
-            "velocityY": -0.5157955577412445,
-            "timestamp": 0.6143566918240819
-        },
-        {
-            "x": 2.4127218287177645,
-            "y": 6.26676408374459,
-            "heading": 0.5007637073683532,
-            "angularVelocity": -7.445219928061725e-7,
-            "velocityX": 3.7827154561393628,
-            "velocityY": 0.04124358046976413,
-            "timestamp": 0.681295761808209
-        },
-        {
-            "x": 2.6627712176567337,
-            "y": 6.306751164644663,
-            "heading": 0.5007636590304977,
-            "angularVelocity": -7.221172264287654e-7,
-            "velocityX": 3.7354774871878274,
-            "velocityY": 0.5973653489565757,
-            "timestamp": 0.7482348317923362
-        },
-        {
-            "x": 2.9117460938747426,
-            "y": 6.352959114176208,
-            "heading": 0.5007636106421453,
-            "angularVelocity": -7.22871595873179e-7,
-            "velocityX": 3.719425386056917,
-            "velocityY": 0.6902986483459334,
-            "timestamp": 0.8151739017764633
-        },
-        {
-            "x": 3.160720959556854,
-            "y": 6.399167120476624,
-            "heading": 0.5007635622537979,
-            "angularVelocity": -7.228715220114282e-7,
-            "velocityX": 3.719425228661581,
-            "velocityY": 0.6902994964137207,
-            "timestamp": 0.8821129717605904
-        },
-        {
-            "x": 3.409695827610573,
-            "y": 6.445375113998521,
-            "heading": 0.5007635138654579,
-            "angularVelocity": -7.228714109969877e-7,
-            "velocityX": 3.719425264090935,
-            "velocityY": 0.6902993055159711,
-            "timestamp": 0.9490520417447176
-        },
-        {
-            "x": 3.658670698533997,
-            "y": 6.491583092058085,
-            "heading": 0.5007634654771249,
-            "angularVelocity": -7.228713077638952e-7,
-            "velocityX": 3.719425306961334,
-            "velocityY": 0.6902990745248123,
-            "timestamp": 1.0159911117288447
-        },
-        {
-            "x": 3.9076455729725814,
-            "y": 6.537791051177508,
-            "heading": 0.5007634170887977,
-            "angularVelocity": -7.228712197353323e-7,
-            "velocityX": 3.7194253594741626,
-            "velocityY": 0.6902987915788225,
-            "timestamp": 1.082930181712972
-        },
-        {
-            "x": 4.156620451761658,
-            "y": 6.583998986855888,
-            "heading": 0.5007633687004744,
-            "angularVelocity": -7.228711615866084e-7,
-            "velocityX": 3.7194254244659963,
-            "velocityY": 0.6902984413940524,
-            "timestamp": 1.1498692516970992
-        },
-        {
-            "x": 4.405595335954841,
-            "y": 6.630206893416186,
-            "heading": 0.5007633203121516,
-            "angularVelocity": -7.22871154439704e-7,
-            "velocityX": 3.719425505197721,
-            "velocityY": 0.6902980064002329,
-            "timestamp": 1.2168083216812264
-        },
-        {
-            "x": 4.65457022677848,
-            "y": 6.676414764250624,
-            "heading": 0.5007632719238245,
-            "angularVelocity": -7.228712197620323e-7,
-            "velocityX": 3.719425604249829,
-            "velocityY": 0.690297472693087,
-            "timestamp": 1.2837473916653537
-        },
-        {
-            "x": 4.903545125380086,
-            "y": 6.722622593176218,
-            "heading": 0.5007632235354964,
-            "angularVelocity": -7.228712343344299e-7,
-            "velocityX": 3.7194257204445353,
-            "velocityY": 0.69029684661859,
-            "timestamp": 1.350686461649481
-        },
-        {
-            "x": 5.152520035964963,
-            "y": 6.768830357534066,
-            "heading": 0.500763175147177,
-            "angularVelocity": -7.228711020086952e-7,
-            "velocityX": 3.7194258994621494,
-            "velocityY": 0.6902958820437133,
-            "timestamp": 1.4176255316336082
-        },
-        {
-            "x": 5.401781723000098,
-            "y": 6.813465264887065,
-            "heading": 0.5007631267776413,
-            "angularVelocity": -7.225904944160688e-7,
-            "velocityX": 3.7237100410005364,
-            "velocityY": 0.6667990362518919,
-            "timestamp": 1.4845646016177354
-        },
-        {
-            "x": 5.654897775782049,
-            "y": 6.8209436894170015,
-            "heading": 0.5007630775196736,
-            "angularVelocity": -7.358627453849351e-7,
-            "velocityX": 3.781290251597022,
-            "velocityY": 0.11171987498067794,
-            "timestamp": 1.5515036716018626
-        },
-        {
-            "x": 5.9063591957092285,
-            "y": 6.791097164154053,
-            "heading": 0.500763020915307,
-            "angularVelocity": -8.456102929151622e-7,
-            "velocityX": 3.756571759763139,
-            "velocityY": -0.44587600739018746,
-            "timestamp": 1.6184427415859899
-        },
-        {
-            "x": 6.200798567763329,
-            "y": 6.700713487178965,
-            "heading": 0.5007629755779084,
-            "angularVelocity": -5.568470838728409e-7,
-            "velocityX": 3.616389792279645,
-            "velocityY": -1.1101185433221765,
-            "timestamp": 1.6998607824959264
-        },
-        {
-            "x": 6.474342410112796,
-            "y": 6.559159535827226,
-            "heading": 0.5007629354438458,
-            "angularVelocity": -4.929382012989922e-7,
-            "velocityX": 3.3597448340971368,
-            "velocityY": -1.7386066990769444,
-            "timestamp": 1.781278823405863
-        },
-        {
-            "x": 6.734766818786864,
-            "y": 6.394712333057686,
-            "heading": 0.5007628957809265,
-            "angularVelocity": -4.871514819255018e-7,
-            "velocityX": 3.1986081433983946,
-            "velocityY": -2.0197882549329598,
-            "timestamp": 1.8626968643157995
-        },
-        {
-            "x": 6.9951910734023155,
-            "y": 6.230264886316009,
-            "heading": 0.5007628561176116,
-            "angularVelocity": -4.871563403692846e-7,
-            "velocityX": 3.198606251205787,
-            "velocityY": -2.019791251469529,
-            "timestamp": 1.944114905225736
-        },
-        {
-            "x": 7.223286683326485,
-            "y": 6.0852317833380125,
-            "heading": 0.4070504938542789,
-            "angularVelocity": -1.1510024217727797,
-            "velocityX": 2.801536457705794,
-            "velocityY": -1.781338648745319,
-            "timestamp": 2.0255329461356726
-        },
-        {
-            "x": 7.40663062232063,
-            "y": 5.969243166944327,
-            "heading": 0.26222076447726717,
-            "angularVelocity": -1.7788407551738947,
-            "velocityX": 2.2518834516904875,
-            "velocityY": -1.4246058379369684,
-            "timestamp": 2.106950987045609
-        },
-        {
-            "x": 7.543566910533025,
-            "y": 5.882836756425157,
-            "heading": 0.1356651998410208,
-            "angularVelocity": -1.5543921620054602,
-            "velocityX": 1.6818912206923584,
-            "velocityY": -1.061268602799615,
-            "timestamp": 2.1883690279555457
-        },
-        {
-            "x": 7.634586620516168,
-            "y": 5.825537139087579,
-            "heading": 0.04464428184824825,
-            "angularVelocity": -1.1179453223820228,
-            "velocityX": 1.1179304852572838,
-            "velocityY": -0.7037705242866615,
-            "timestamp": 2.269787068865482
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -0.003,
-            "angularVelocity": -0.5851808925364741,
-            "velocityX": 0.5577803024524691,
-            "velocityY": -0.35050142165846687,
-            "timestamp": 2.3512051097754187
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -0.003,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 2.4326231506853553
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.1.traj
deleted file mode 100644
index 78068220..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.1.traj	
+++ /dev/null
@@ -1,670 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7276374646295759,
-            "y": 6.716389445106627,
-            "heading": 1.0769704869840742,
-            "angularVelocity": 0.7463572831464532,
-            "velocityX": 0.35606374858489526,
-            "velocityY": -0.07288959818107699,
-            "timestamp": 0.0495345698619811
-        },
-        {
-            "x": 0.7632753901023804,
-            "y": 6.70941368019147,
-            "heading": 1.1486786527951847,
-            "angularVelocity": 1.4476388108510687,
-            "velocityX": 0.7194556361777777,
-            "velocityY": -0.1408261933931805,
-            "timestamp": 0.0990691397239622
-        },
-        {
-            "x": 0.8173493088554353,
-            "y": 6.699320044331327,
-            "heading": 1.251948933720564,
-            "angularVelocity": 2.084812308111838,
-            "velocityX": 1.0916400183503512,
-            "velocityY": -0.20376952677844481,
-            "timestamp": 0.1486037095859433
-        },
-        {
-            "x": 0.8904201618241163,
-            "y": 6.686277798850656,
-            "heading": 1.3814946265878076,
-            "angularVelocity": 2.615258257579252,
-            "velocityX": 1.4751486320027993,
-            "velocityY": -0.263295825859426,
-            "timestamp": 0.1981382794479244
-        },
-        {
-            "x": 0.9831303708035419,
-            "y": 6.670301294322549,
-            "heading": 1.5290667264747733,
-            "angularVelocity": 2.9791739445430228,
-            "velocityX": 1.871626406322422,
-            "velocityY": -0.32253241669944677,
-            "timestamp": 0.2476728493099055
-        },
-        {
-            "x": 1.0960137924135722,
-            "y": 6.651369432356597,
-            "heading": 1.6827078385240544,
-            "angularVelocity": 3.1016946846903584,
-            "velocityX": 2.278881636090858,
-            "velocityY": -0.3821949401851956,
-            "timestamp": 0.2972074191718866
-        },
-        {
-            "x": 1.2286250246243187,
-            "y": 6.629796552300239,
-            "heading": 1.8192744268134131,
-            "angularVelocity": 2.7569955421025307,
-            "velocityX": 2.677145124715176,
-            "velocityY": -0.4355116056504451,
-            "timestamp": 0.34674198903386766
-        },
-        {
-            "x": 1.37806258880499,
-            "y": 6.607677950088394,
-            "heading": 1.9114236020956594,
-            "angularVelocity": 1.8603003021733433,
-            "velocityX": 3.0168337909701455,
-            "velocityY": -0.446528601597435,
-            "timestamp": 0.3962765588958488
-        },
-        {
-            "x": 1.5443963470492121,
-            "y": 6.584716640433465,
-            "heading": 1.9605466016146158,
-            "angularVelocity": 0.991691250288047,
-            "velocityX": 3.3579328276762688,
-            "velocityY": -0.4635411131687398,
-            "timestamp": 0.4458111287578299
-        },
-        {
-            "x": 1.7276120279383835,
-            "y": 6.5605525122280195,
-            "heading": 1.9670840049159166,
-            "angularVelocity": 0.13197658361397824,
-            "velocityX": 3.698743754106468,
-            "velocityY": -0.4878235194494175,
-            "timestamp": 0.49534569861981104
-        },
-        {
-            "x": 1.9125583307039895,
-            "y": 6.530411390267246,
-            "heading": 1.9670841172507754,
-            "angularVelocity": 0.0000022678072915558937,
-            "velocityX": 3.7336814124171944,
-            "velocityY": -0.6084865992338514,
-            "timestamp": 0.5448802684817922
-        },
-        {
-            "x": 2.0974975773537823,
-            "y": 6.500227003772903,
-            "heading": 1.967084229554738,
-            "angularVelocity": 0.0000022671835628273576,
-            "velocityX": 3.733538964104325,
-            "velocityY": -0.6093600202458971,
-            "timestamp": 0.5944148383437733
-        },
-        {
-            "x": 2.2824570922079714,
-            "y": 6.470167063452025,
-            "heading": 1.9670843418904687,
-            "angularVelocity": 0.0000022678248970866596,
-            "velocityX": 3.7339481370144885,
-            "velocityY": -0.6068477107210625,
-            "timestamp": 0.6439494082057544
-        },
-        {
-            "x": 2.4663667602317028,
-            "y": 6.434238576599238,
-            "heading": 1.9670844533817493,
-            "angularVelocity": 0.000002250777202166417,
-            "velocityX": 3.712753911780929,
-            "velocityY": -0.7253214664890316,
-            "timestamp": 0.6934839780677355
-        },
-        {
-            "x": 2.6453124624138216,
-            "y": 6.3786323067408315,
-            "heading": 1.96708456679077,
-            "angularVelocity": 0.000002289492388953058,
-            "velocityX": 3.6125417598312857,
-            "velocityY": -1.1225750019579073,
-            "timestamp": 0.7430185479297167
-        },
-        {
-            "x": 2.817133903503418,
-            "y": 6.303859233856201,
-            "heading": 1.96708468933932,
-            "angularVelocity": 0.0000024740004860391374,
-            "velocityX": 3.468717737283832,
-            "velocityY": -1.5095129137971557,
-            "timestamp": 0.7925531177916978
-        },
-        {
-            "x": 2.91295588800905,
-            "y": 6.254800700059255,
-            "heading": 1.967084804070116,
-            "angularVelocity": 0.000004031753460568297,
-            "velocityX": 3.367279151013272,
-            "velocityY": -1.7239653184522483,
-            "timestamp": 0.8210099163546827
-        },
-        {
-            "x": 3.0055158966135553,
-            "y": 6.199834910977839,
-            "heading": 1.9670849118890987,
-            "angularVelocity": 0.000003788865518014884,
-            "velocityX": 3.2526500969440626,
-            "velocityY": -1.9315521020251178,
-            "timestamp": 0.8494667149176676
-        },
-        {
-            "x": 3.0944547185216633,
-            "y": 6.139184440666953,
-            "heading": 1.96708501618758,
-            "angularVelocity": 0.0000036651516103387324,
-            "velocityX": 3.125398020841055,
-            "velocityY": -2.13131741353383,
-            "timestamp": 0.8779235134806525
-        },
-        {
-            "x": 3.179446617500541,
-            "y": 6.0731165142841,
-            "heading": 1.9670851198382022,
-            "angularVelocity": 0.0000036423852113204396,
-            "velocityX": 2.986699251881383,
-            "velocityY": -2.3216921691473744,
-            "timestamp": 0.9063803120436373
-        },
-        {
-            "x": 3.2604149841370136,
-            "y": 6.002174866532259,
-            "heading": 1.9670852253780247,
-            "angularVelocity": 0.0000037087735735672327,
-            "velocityX": 2.845308352497901,
-            "velocityY": -2.4929595504099007,
-            "timestamp": 0.9348371106066222
-        },
-        {
-            "x": 3.3400950350688365,
-            "y": 5.929789210280793,
-            "heading": 1.9670853319364994,
-            "angularVelocity": 0.000003744570018753785,
-            "velocityX": 2.8000356665421786,
-            "velocityY": -2.5437034349136662,
-            "timestamp": 0.9632939091696071
-        },
-        {
-            "x": 3.4197035202865202,
-            "y": 5.857324854756433,
-            "heading": 1.9670854385695549,
-            "angularVelocity": 0.0000037471908580905978,
-            "velocityX": 2.7975207766800336,
-            "velocityY": -2.5464690050744796,
-            "timestamp": 0.991750707732592
-        },
-        {
-            "x": 3.4993494510128746,
-            "y": 5.7849016577830055,
-            "heading": 1.9670855451672207,
-            "angularVelocity": 0.0000037459472354931967,
-            "velocityX": 2.7988366488248966,
-            "velocityY": -2.5450226529586133,
-            "timestamp": 1.0202075062955769
-        },
-        {
-            "x": 3.579011291529686,
-            "y": 5.712495961052324,
-            "heading": 1.96708565174968,
-            "angularVelocity": 0.0000037454128599628807,
-            "velocityX": 2.7993957345647376,
-            "velocityY": -2.544407677155567,
-            "timestamp": 1.0486643048585618
-        },
-        {
-            "x": 3.658639843469943,
-            "y": 5.640053656617691,
-            "heading": 1.9670857583635384,
-            "angularVelocity": 0.000003746516260337957,
-            "velocityX": 2.7982259411140946,
-            "velocityY": -2.545694108020349,
-            "timestamp": 1.0771211034215467
-        },
-        {
-            "x": 3.7382184197836335,
-            "y": 5.567556457074997,
-            "heading": 1.9670858650249143,
-            "angularVelocity": 0.0000037481860642492875,
-            "velocityX": 2.796469748251449,
-            "velocityY": -2.547623176320097,
-            "timestamp": 1.1055779019845315
-        },
-        {
-            "x": 3.81773211309123,
-            "y": 5.494988100908759,
-            "heading": 1.967085971748553,
-            "angularVelocity": 0.0000037503740493629023,
-            "velocityX": 2.794189695354055,
-            "velocityY": -2.5501236903290603,
-            "timestamp": 1.1340347005475164
-        },
-        {
-            "x": 3.8971703522513814,
-            "y": 5.422337155346324,
-            "heading": 1.967086078550155,
-            "angularVelocity": 0.000003753113746455045,
-            "velocityX": 2.7915381621131705,
-            "velocityY": -2.5530259632560406,
-            "timestamp": 1.1624914991105013
-        },
-        {
-            "x": 3.9762575655393007,
-            "y": 5.349304242205494,
-            "heading": 1.9670861858107123,
-            "angularVelocity": 0.0000037692418878753097,
-            "velocityX": 2.779202766357265,
-            "velocityY": -2.566448681118608,
-            "timestamp": 1.1909482976734862
-        },
-        {
-            "x": 4.0513085153658634,
-            "y": 5.27212945123473,
-            "heading": 1.9670862984844089,
-            "angularVelocity": 0.000003959464953334102,
-            "velocityX": 2.637364482882678,
-            "velocityY": -2.7119983577895854,
-            "timestamp": 1.219405096236471
-        },
-        {
-            "x": 4.121436785634375,
-            "y": 5.190455596279305,
-            "heading": 1.9670864209307273,
-            "angularVelocity": 0.000004302884530781832,
-            "velocityX": 2.4643766625150745,
-            "velocityY": -2.8700999086264516,
-            "timestamp": 1.247861894799456
-        },
-        {
-            "x": 4.18632086285232,
-            "y": 5.104556536444466,
-            "heading": 1.9670865586915784,
-            "angularVelocity": 0.0000048410523378145445,
-            "velocityX": 2.280090540555213,
-            "velocityY": -3.0185777800939864,
-            "timestamp": 1.2763186933624409
-        },
-        {
-            "x": 4.245696839289754,
-            "y": 5.014761859934199,
-            "heading": 1.9670867202387443,
-            "angularVelocity": 0.000005676926915437751,
-            "velocityX": 2.086530440380401,
-            "velocityY": -3.1554735966350926,
-            "timestamp": 1.3047754919254257
-        },
-        {
-            "x": 4.299328195356381,
-            "y": 4.921422294262094,
-            "heading": 1.967086761089818,
-            "angularVelocity": 0.0000014355470678059053,
-            "velocityX": 1.8846588082611082,
-            "velocityY": -3.2800445020371574,
-            "timestamp": 1.3332322904884106
-        },
-        {
-            "x": 4.347003427577455,
-            "y": 4.824904615507251,
-            "heading": 1.9670865846023797,
-            "angularVelocity": -0.000006201942843803764,
-            "velocityX": 1.6753547351988247,
-            "velocityY": -3.391726533861692,
-            "timestamp": 1.3616890890513955
-        },
-        {
-            "x": 4.391601867669633,
-            "y": 4.730655732437251,
-            "heading": 1.956126696369797,
-            "angularVelocity": -0.3851412943835458,
-            "velocityX": 1.5672332217368503,
-            "velocityY": -3.3119988132677265,
-            "timestamp": 1.3901458876143804
-        },
-        {
-            "x": 4.4407405541886815,
-            "y": 4.641134625282056,
-            "heading": 1.9393492777589223,
-            "angularVelocity": -0.5895750561568437,
-            "velocityX": 1.7267819642570823,
-            "velocityY": -3.145860099373803,
-            "timestamp": 1.4186026861773653
-        },
-        {
-            "x": 4.494270192698628,
-            "y": 4.5565595160525465,
-            "heading": 1.9177097235934124,
-            "angularVelocity": -0.7604353004627978,
-            "velocityX": 1.8810843528824521,
-            "velocityY": -2.97205284854274,
-            "timestamp": 1.4470594847403502
-        },
-        {
-            "x": 4.552047682392307,
-            "y": 4.477111099902014,
-            "heading": 1.8920595280024661,
-            "angularVelocity": -0.901373200296037,
-            "velocityX": 2.030358037844879,
-            "velocityY": -2.791895791605024,
-            "timestamp": 1.475516283303335
-        },
-        {
-            "x": 4.613941492209361,
-            "y": 4.402936898219151,
-            "heading": 1.8631628723317806,
-            "angularVelocity": -1.0154570130820861,
-            "velocityX": 2.175009591471355,
-            "velocityY": -2.6065546874046253,
-            "timestamp": 1.50397308186632
-        },
-        {
-            "x": 4.679832286793745,
-            "y": 4.334157239741354,
-            "heading": 1.831710656249818,
-            "angularVelocity": -1.105261929317901,
-            "velocityX": 2.315467582853696,
-            "velocityY": -2.416985112558446,
-            "timestamp": 1.5324298804293048
-        },
-        {
-            "x": 4.749611538460051,
-            "y": 4.270871230504256,
-            "heading": 1.798335587748108,
-            "angularVelocity": -1.1728328619898605,
-            "velocityX": 2.4521118042094026,
-            "velocityY": -2.223932853761988,
-            "timestamp": 1.5608866789922897
-        },
-        {
-            "x": 4.82317903379308,
-            "y": 4.213162042716923,
-            "heading": 1.7636285592897518,
-            "angularVelocity": -1.2196392500559503,
-            "velocityX": 2.585234427203579,
-            "velocityY": -2.0279578414139063,
-            "timestamp": 1.5893434775552746
-        },
-        {
-            "x": 4.900439739227295,
-            "y": 4.161101341247559,
-            "heading": 1.7281561592052472,
-            "angularVelocity": -1.2465351647339973,
-            "velocityX": 2.715017476867699,
-            "velocityY": -1.8294644548346262,
-            "timestamp": 1.6178002761182595
-        },
-        {
-            "x": 5.010449341197396,
-            "y": 4.1015961278120825,
-            "heading": 1.6804127870661807,
-            "angularVelocity": -1.251568549956003,
-            "velocityX": 2.8838465288429167,
-            "velocityY": -1.5598993191587458,
-            "timestamp": 1.6559471056633712
-        },
-        {
-            "x": 5.126648667318374,
-            "y": 4.05252161275018,
-            "heading": 1.6338488947780365,
-            "angularVelocity": -1.2206490773575045,
-            "velocityX": 3.0461070423551457,
-            "velocityY": -1.2864637939008783,
-            "timestamp": 1.694093935208483
-        },
-        {
-            "x": 5.248756740712563,
-            "y": 4.014017499979329,
-            "heading": 1.589989878848282,
-            "angularVelocity": -1.1497421005375177,
-            "velocityX": 3.2010018879727395,
-            "velocityY": -1.0093660005294856,
-            "timestamp": 1.7322407647535947
-        },
-        {
-            "x": 5.376454980725802,
-            "y": 3.9862068849459353,
-            "heading": 1.5506775545962859,
-            "angularVelocity": -1.0305528590747366,
-            "velocityX": 3.347545301559156,
-            "velocityY": -0.7290413217820071,
-            "timestamp": 1.7703875942987064
-        },
-        {
-            "x": 5.509414833155046,
-            "y": 3.969152206173251,
-            "heading": 1.5183946275971996,
-            "angularVelocity": -0.8462807364186931,
-            "velocityX": 3.485475831533742,
-            "velocityY": -0.4470798484702507,
-            "timestamp": 1.8085344238438181
-        },
-        {
-            "x": 5.647435915880888,
-            "y": 3.96261903739833,
-            "heading": 1.4980750155673637,
-            "angularVelocity": -0.5326684359468331,
-            "velocityX": 3.618153444771329,
-            "velocityY": -0.17126374204166636,
-            "timestamp": 1.8466812533889299
-        },
-        {
-            "x": 5.791726765964232,
-            "y": 3.964767379969892,
-            "heading": 1.498073903211314,
-            "angularVelocity": -0.00002915985583373997,
-            "velocityX": 3.7825122507942877,
-            "velocityY": 0.056317722783366074,
-            "timestamp": 1.8848280829340416
-        },
-        {
-            "x": 5.935369435750422,
-            "y": 3.9785994401105067,
-            "heading": 1.4980736722429098,
-            "angularVelocity": -0.00000605472084526459,
-            "velocityX": 3.765520529467262,
-            "velocityY": 0.3626005176676574,
-            "timestamp": 1.9229749124791533
-        },
-        {
-            "x": 6.078764518036592,
-            "y": 3.9947980029743344,
-            "heading": 1.498073449658831,
-            "angularVelocity": -0.000005834929965972279,
-            "velocityX": 3.7590301473577066,
-            "velocityY": 0.42463719939010114,
-            "timestamp": 1.961121742024265
-        },
-        {
-            "x": 6.222150100858587,
-            "y": 4.0110804389753145,
-            "heading": 1.4980732273393433,
-            "angularVelocity": -0.000005827993841710607,
-            "velocityX": 3.7587811236710817,
-            "velocityY": 0.4268358915939984,
-            "timestamp": 1.9992685715693768
-        },
-        {
-            "x": 6.3655338107868324,
-            "y": 4.0273793596330805,
-            "heading": 1.4980730050704238,
-            "angularVelocity": -0.000005826668223171757,
-            "velocityX": 3.758732026701189,
-            "velocityY": 0.42726802860026514,
-            "timestamp": 2.0374154011144885
-        },
-        {
-            "x": 6.508922861230399,
-            "y": 4.043631229943055,
-            "heading": 1.4980727826574463,
-            "angularVelocity": -0.000005830444635518525,
-            "velocityX": 3.7588720256305224,
-            "velocityY": 0.42603462734214087,
-            "timestamp": 2.0755622306596
-        },
-        {
-            "x": 6.6523224677448685,
-            "y": 4.059789689420356,
-            "heading": 1.4980725589565675,
-            "angularVelocity": -0.000005864206314008162,
-            "velocityX": 3.759148747731203,
-            "velocityY": 0.4235859092458616,
-            "timestamp": 2.1137090602047115
-        },
-        {
-            "x": 6.792869039761006,
-            "y": 4.068475137054533,
-            "heading": 1.4874839096219183,
-            "angularVelocity": -0.2775761304689174,
-            "velocityX": 3.684357879593819,
-            "velocityY": 0.2276846526483585,
-            "timestamp": 2.151855889749823
-        },
-        {
-            "x": 6.923724640852256,
-            "y": 4.075654653912489,
-            "heading": 1.4487910280540974,
-            "angularVelocity": -1.014314479844837,
-            "velocityX": 3.430313938317647,
-            "velocityY": 0.18820743280285396,
-            "timestamp": 2.1900027192949345
-        },
-        {
-            "x": 7.04453293582936,
-            "y": 4.081916427546979,
-            "heading": 1.3838287724616933,
-            "angularVelocity": -1.7029529417506197,
-            "velocityX": 3.1669288488112004,
-            "velocityY": 0.1641492545881335,
-            "timestamp": 2.228149548840046
-        },
-        {
-            "x": 7.155015675902827,
-            "y": 4.087769066352678,
-            "heading": 1.2944912667640571,
-            "angularVelocity": -2.3419378953075096,
-            "velocityX": 2.896249606872667,
-            "velocityY": 0.1534239902944478,
-            "timestamp": 2.2662963783851575
-        },
-        {
-            "x": 7.255027442080941,
-            "y": 4.093665982164644,
-            "heading": 1.1821266374518384,
-            "angularVelocity": -2.945582389205927,
-            "velocityX": 2.621758279016409,
-            "velocityY": 0.15458468980091475,
-            "timestamp": 2.304443207930269
-        },
-        {
-            "x": 7.344594486390613,
-            "y": 4.100041344791564,
-            "heading": 1.0471923484078691,
-            "angularVelocity": -3.5372346969048754,
-            "velocityX": 2.3479551348760803,
-            "velocityY": 0.16712693303897977,
-            "timestamp": 2.3425900374753805
-        },
-        {
-            "x": 7.423972884493799,
-            "y": 4.107363761305008,
-            "heading": 0.8891062073498277,
-            "angularVelocity": -4.144148883225662,
-            "velocityX": 2.080864885752389,
-            "velocityY": 0.19195347557968287,
-            "timestamp": 2.380736867020492
-        },
-        {
-            "x": 7.492968640563269,
-            "y": 4.113132211800853,
-            "heading": 0.7092469399580156,
-            "angularVelocity": -4.714920467480658,
-            "velocityX": 1.8086891333366921,
-            "velocityY": 0.1512170359820358,
-            "timestamp": 2.4188836965656035
-        },
-        {
-            "x": 7.549873361136887,
-            "y": 4.117954510008317,
-            "heading": 0.5306887118642236,
-            "angularVelocity": -4.680814374954733,
-            "velocityX": 1.4917287033341673,
-            "velocityY": 0.12641412838299443,
-            "timestamp": 2.457030526110715
-        },
-        {
-            "x": 7.595282353021441,
-            "y": 4.121668824298383,
-            "heading": 0.3658074925223697,
-            "angularVelocity": -4.322278451654422,
-            "velocityX": 1.1903739426332636,
-            "velocityY": 0.09736888581300335,
-            "timestamp": 2.4951773556558265
-        },
-        {
-            "x": 7.630209510034256,
-            "y": 4.124185175295285,
-            "heading": 0.22438733020455492,
-            "angularVelocity": -3.7072586111122043,
-            "velocityX": 0.9155978997292245,
-            "velocityY": 0.06596487904592362,
-            "timestamp": 2.533324185200938
-        },
-        {
-            "x": 7.65555835714788,
-            "y": 4.125725160446138,
-            "heading": 0.1131980096846438,
-            "angularVelocity": -2.91477225881568,
-            "velocityX": 0.6645073107231708,
-            "velocityY": 0.04036993818976583,
-            "timestamp": 2.5714710147460496
-        },
-        {
-            "x": 7.671987700512514,
-            "y": 4.126592780034806,
-            "heading": 0.03644789338444829,
-            "angularVelocity": -2.011965796774904,
-            "velocityX": 0.43068699445162756,
-            "velocityY": 0.02274421227203338,
-            "timestamp": 2.609617844291161
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -0.003,
-            "angularVelocity": -1.0341067358634992,
-            "velocityX": 0.21003841165919293,
-            "velocityY": 0.010675067103991773,
-            "timestamp": 2.6477646738362726
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -0.003,
-            "angularVelocity": 1.475285929099793e-30,
-            "velocityX": 9.681452358723228e-32,
-            "velocityY": 1.6696229962948817e-32,
-            "timestamp": 2.685911503381384
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.traj
deleted file mode 100644
index 78068220..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.traj	
+++ /dev/null
@@ -1,670 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7276374646295759,
-            "y": 6.716389445106627,
-            "heading": 1.0769704869840742,
-            "angularVelocity": 0.7463572831464532,
-            "velocityX": 0.35606374858489526,
-            "velocityY": -0.07288959818107699,
-            "timestamp": 0.0495345698619811
-        },
-        {
-            "x": 0.7632753901023804,
-            "y": 6.70941368019147,
-            "heading": 1.1486786527951847,
-            "angularVelocity": 1.4476388108510687,
-            "velocityX": 0.7194556361777777,
-            "velocityY": -0.1408261933931805,
-            "timestamp": 0.0990691397239622
-        },
-        {
-            "x": 0.8173493088554353,
-            "y": 6.699320044331327,
-            "heading": 1.251948933720564,
-            "angularVelocity": 2.084812308111838,
-            "velocityX": 1.0916400183503512,
-            "velocityY": -0.20376952677844481,
-            "timestamp": 0.1486037095859433
-        },
-        {
-            "x": 0.8904201618241163,
-            "y": 6.686277798850656,
-            "heading": 1.3814946265878076,
-            "angularVelocity": 2.615258257579252,
-            "velocityX": 1.4751486320027993,
-            "velocityY": -0.263295825859426,
-            "timestamp": 0.1981382794479244
-        },
-        {
-            "x": 0.9831303708035419,
-            "y": 6.670301294322549,
-            "heading": 1.5290667264747733,
-            "angularVelocity": 2.9791739445430228,
-            "velocityX": 1.871626406322422,
-            "velocityY": -0.32253241669944677,
-            "timestamp": 0.2476728493099055
-        },
-        {
-            "x": 1.0960137924135722,
-            "y": 6.651369432356597,
-            "heading": 1.6827078385240544,
-            "angularVelocity": 3.1016946846903584,
-            "velocityX": 2.278881636090858,
-            "velocityY": -0.3821949401851956,
-            "timestamp": 0.2972074191718866
-        },
-        {
-            "x": 1.2286250246243187,
-            "y": 6.629796552300239,
-            "heading": 1.8192744268134131,
-            "angularVelocity": 2.7569955421025307,
-            "velocityX": 2.677145124715176,
-            "velocityY": -0.4355116056504451,
-            "timestamp": 0.34674198903386766
-        },
-        {
-            "x": 1.37806258880499,
-            "y": 6.607677950088394,
-            "heading": 1.9114236020956594,
-            "angularVelocity": 1.8603003021733433,
-            "velocityX": 3.0168337909701455,
-            "velocityY": -0.446528601597435,
-            "timestamp": 0.3962765588958488
-        },
-        {
-            "x": 1.5443963470492121,
-            "y": 6.584716640433465,
-            "heading": 1.9605466016146158,
-            "angularVelocity": 0.991691250288047,
-            "velocityX": 3.3579328276762688,
-            "velocityY": -0.4635411131687398,
-            "timestamp": 0.4458111287578299
-        },
-        {
-            "x": 1.7276120279383835,
-            "y": 6.5605525122280195,
-            "heading": 1.9670840049159166,
-            "angularVelocity": 0.13197658361397824,
-            "velocityX": 3.698743754106468,
-            "velocityY": -0.4878235194494175,
-            "timestamp": 0.49534569861981104
-        },
-        {
-            "x": 1.9125583307039895,
-            "y": 6.530411390267246,
-            "heading": 1.9670841172507754,
-            "angularVelocity": 0.0000022678072915558937,
-            "velocityX": 3.7336814124171944,
-            "velocityY": -0.6084865992338514,
-            "timestamp": 0.5448802684817922
-        },
-        {
-            "x": 2.0974975773537823,
-            "y": 6.500227003772903,
-            "heading": 1.967084229554738,
-            "angularVelocity": 0.0000022671835628273576,
-            "velocityX": 3.733538964104325,
-            "velocityY": -0.6093600202458971,
-            "timestamp": 0.5944148383437733
-        },
-        {
-            "x": 2.2824570922079714,
-            "y": 6.470167063452025,
-            "heading": 1.9670843418904687,
-            "angularVelocity": 0.0000022678248970866596,
-            "velocityX": 3.7339481370144885,
-            "velocityY": -0.6068477107210625,
-            "timestamp": 0.6439494082057544
-        },
-        {
-            "x": 2.4663667602317028,
-            "y": 6.434238576599238,
-            "heading": 1.9670844533817493,
-            "angularVelocity": 0.000002250777202166417,
-            "velocityX": 3.712753911780929,
-            "velocityY": -0.7253214664890316,
-            "timestamp": 0.6934839780677355
-        },
-        {
-            "x": 2.6453124624138216,
-            "y": 6.3786323067408315,
-            "heading": 1.96708456679077,
-            "angularVelocity": 0.000002289492388953058,
-            "velocityX": 3.6125417598312857,
-            "velocityY": -1.1225750019579073,
-            "timestamp": 0.7430185479297167
-        },
-        {
-            "x": 2.817133903503418,
-            "y": 6.303859233856201,
-            "heading": 1.96708468933932,
-            "angularVelocity": 0.0000024740004860391374,
-            "velocityX": 3.468717737283832,
-            "velocityY": -1.5095129137971557,
-            "timestamp": 0.7925531177916978
-        },
-        {
-            "x": 2.91295588800905,
-            "y": 6.254800700059255,
-            "heading": 1.967084804070116,
-            "angularVelocity": 0.000004031753460568297,
-            "velocityX": 3.367279151013272,
-            "velocityY": -1.7239653184522483,
-            "timestamp": 0.8210099163546827
-        },
-        {
-            "x": 3.0055158966135553,
-            "y": 6.199834910977839,
-            "heading": 1.9670849118890987,
-            "angularVelocity": 0.000003788865518014884,
-            "velocityX": 3.2526500969440626,
-            "velocityY": -1.9315521020251178,
-            "timestamp": 0.8494667149176676
-        },
-        {
-            "x": 3.0944547185216633,
-            "y": 6.139184440666953,
-            "heading": 1.96708501618758,
-            "angularVelocity": 0.0000036651516103387324,
-            "velocityX": 3.125398020841055,
-            "velocityY": -2.13131741353383,
-            "timestamp": 0.8779235134806525
-        },
-        {
-            "x": 3.179446617500541,
-            "y": 6.0731165142841,
-            "heading": 1.9670851198382022,
-            "angularVelocity": 0.0000036423852113204396,
-            "velocityX": 2.986699251881383,
-            "velocityY": -2.3216921691473744,
-            "timestamp": 0.9063803120436373
-        },
-        {
-            "x": 3.2604149841370136,
-            "y": 6.002174866532259,
-            "heading": 1.9670852253780247,
-            "angularVelocity": 0.0000037087735735672327,
-            "velocityX": 2.845308352497901,
-            "velocityY": -2.4929595504099007,
-            "timestamp": 0.9348371106066222
-        },
-        {
-            "x": 3.3400950350688365,
-            "y": 5.929789210280793,
-            "heading": 1.9670853319364994,
-            "angularVelocity": 0.000003744570018753785,
-            "velocityX": 2.8000356665421786,
-            "velocityY": -2.5437034349136662,
-            "timestamp": 0.9632939091696071
-        },
-        {
-            "x": 3.4197035202865202,
-            "y": 5.857324854756433,
-            "heading": 1.9670854385695549,
-            "angularVelocity": 0.0000037471908580905978,
-            "velocityX": 2.7975207766800336,
-            "velocityY": -2.5464690050744796,
-            "timestamp": 0.991750707732592
-        },
-        {
-            "x": 3.4993494510128746,
-            "y": 5.7849016577830055,
-            "heading": 1.9670855451672207,
-            "angularVelocity": 0.0000037459472354931967,
-            "velocityX": 2.7988366488248966,
-            "velocityY": -2.5450226529586133,
-            "timestamp": 1.0202075062955769
-        },
-        {
-            "x": 3.579011291529686,
-            "y": 5.712495961052324,
-            "heading": 1.96708565174968,
-            "angularVelocity": 0.0000037454128599628807,
-            "velocityX": 2.7993957345647376,
-            "velocityY": -2.544407677155567,
-            "timestamp": 1.0486643048585618
-        },
-        {
-            "x": 3.658639843469943,
-            "y": 5.640053656617691,
-            "heading": 1.9670857583635384,
-            "angularVelocity": 0.000003746516260337957,
-            "velocityX": 2.7982259411140946,
-            "velocityY": -2.545694108020349,
-            "timestamp": 1.0771211034215467
-        },
-        {
-            "x": 3.7382184197836335,
-            "y": 5.567556457074997,
-            "heading": 1.9670858650249143,
-            "angularVelocity": 0.0000037481860642492875,
-            "velocityX": 2.796469748251449,
-            "velocityY": -2.547623176320097,
-            "timestamp": 1.1055779019845315
-        },
-        {
-            "x": 3.81773211309123,
-            "y": 5.494988100908759,
-            "heading": 1.967085971748553,
-            "angularVelocity": 0.0000037503740493629023,
-            "velocityX": 2.794189695354055,
-            "velocityY": -2.5501236903290603,
-            "timestamp": 1.1340347005475164
-        },
-        {
-            "x": 3.8971703522513814,
-            "y": 5.422337155346324,
-            "heading": 1.967086078550155,
-            "angularVelocity": 0.000003753113746455045,
-            "velocityX": 2.7915381621131705,
-            "velocityY": -2.5530259632560406,
-            "timestamp": 1.1624914991105013
-        },
-        {
-            "x": 3.9762575655393007,
-            "y": 5.349304242205494,
-            "heading": 1.9670861858107123,
-            "angularVelocity": 0.0000037692418878753097,
-            "velocityX": 2.779202766357265,
-            "velocityY": -2.566448681118608,
-            "timestamp": 1.1909482976734862
-        },
-        {
-            "x": 4.0513085153658634,
-            "y": 5.27212945123473,
-            "heading": 1.9670862984844089,
-            "angularVelocity": 0.000003959464953334102,
-            "velocityX": 2.637364482882678,
-            "velocityY": -2.7119983577895854,
-            "timestamp": 1.219405096236471
-        },
-        {
-            "x": 4.121436785634375,
-            "y": 5.190455596279305,
-            "heading": 1.9670864209307273,
-            "angularVelocity": 0.000004302884530781832,
-            "velocityX": 2.4643766625150745,
-            "velocityY": -2.8700999086264516,
-            "timestamp": 1.247861894799456
-        },
-        {
-            "x": 4.18632086285232,
-            "y": 5.104556536444466,
-            "heading": 1.9670865586915784,
-            "angularVelocity": 0.0000048410523378145445,
-            "velocityX": 2.280090540555213,
-            "velocityY": -3.0185777800939864,
-            "timestamp": 1.2763186933624409
-        },
-        {
-            "x": 4.245696839289754,
-            "y": 5.014761859934199,
-            "heading": 1.9670867202387443,
-            "angularVelocity": 0.000005676926915437751,
-            "velocityX": 2.086530440380401,
-            "velocityY": -3.1554735966350926,
-            "timestamp": 1.3047754919254257
-        },
-        {
-            "x": 4.299328195356381,
-            "y": 4.921422294262094,
-            "heading": 1.967086761089818,
-            "angularVelocity": 0.0000014355470678059053,
-            "velocityX": 1.8846588082611082,
-            "velocityY": -3.2800445020371574,
-            "timestamp": 1.3332322904884106
-        },
-        {
-            "x": 4.347003427577455,
-            "y": 4.824904615507251,
-            "heading": 1.9670865846023797,
-            "angularVelocity": -0.000006201942843803764,
-            "velocityX": 1.6753547351988247,
-            "velocityY": -3.391726533861692,
-            "timestamp": 1.3616890890513955
-        },
-        {
-            "x": 4.391601867669633,
-            "y": 4.730655732437251,
-            "heading": 1.956126696369797,
-            "angularVelocity": -0.3851412943835458,
-            "velocityX": 1.5672332217368503,
-            "velocityY": -3.3119988132677265,
-            "timestamp": 1.3901458876143804
-        },
-        {
-            "x": 4.4407405541886815,
-            "y": 4.641134625282056,
-            "heading": 1.9393492777589223,
-            "angularVelocity": -0.5895750561568437,
-            "velocityX": 1.7267819642570823,
-            "velocityY": -3.145860099373803,
-            "timestamp": 1.4186026861773653
-        },
-        {
-            "x": 4.494270192698628,
-            "y": 4.5565595160525465,
-            "heading": 1.9177097235934124,
-            "angularVelocity": -0.7604353004627978,
-            "velocityX": 1.8810843528824521,
-            "velocityY": -2.97205284854274,
-            "timestamp": 1.4470594847403502
-        },
-        {
-            "x": 4.552047682392307,
-            "y": 4.477111099902014,
-            "heading": 1.8920595280024661,
-            "angularVelocity": -0.901373200296037,
-            "velocityX": 2.030358037844879,
-            "velocityY": -2.791895791605024,
-            "timestamp": 1.475516283303335
-        },
-        {
-            "x": 4.613941492209361,
-            "y": 4.402936898219151,
-            "heading": 1.8631628723317806,
-            "angularVelocity": -1.0154570130820861,
-            "velocityX": 2.175009591471355,
-            "velocityY": -2.6065546874046253,
-            "timestamp": 1.50397308186632
-        },
-        {
-            "x": 4.679832286793745,
-            "y": 4.334157239741354,
-            "heading": 1.831710656249818,
-            "angularVelocity": -1.105261929317901,
-            "velocityX": 2.315467582853696,
-            "velocityY": -2.416985112558446,
-            "timestamp": 1.5324298804293048
-        },
-        {
-            "x": 4.749611538460051,
-            "y": 4.270871230504256,
-            "heading": 1.798335587748108,
-            "angularVelocity": -1.1728328619898605,
-            "velocityX": 2.4521118042094026,
-            "velocityY": -2.223932853761988,
-            "timestamp": 1.5608866789922897
-        },
-        {
-            "x": 4.82317903379308,
-            "y": 4.213162042716923,
-            "heading": 1.7636285592897518,
-            "angularVelocity": -1.2196392500559503,
-            "velocityX": 2.585234427203579,
-            "velocityY": -2.0279578414139063,
-            "timestamp": 1.5893434775552746
-        },
-        {
-            "x": 4.900439739227295,
-            "y": 4.161101341247559,
-            "heading": 1.7281561592052472,
-            "angularVelocity": -1.2465351647339973,
-            "velocityX": 2.715017476867699,
-            "velocityY": -1.8294644548346262,
-            "timestamp": 1.6178002761182595
-        },
-        {
-            "x": 5.010449341197396,
-            "y": 4.1015961278120825,
-            "heading": 1.6804127870661807,
-            "angularVelocity": -1.251568549956003,
-            "velocityX": 2.8838465288429167,
-            "velocityY": -1.5598993191587458,
-            "timestamp": 1.6559471056633712
-        },
-        {
-            "x": 5.126648667318374,
-            "y": 4.05252161275018,
-            "heading": 1.6338488947780365,
-            "angularVelocity": -1.2206490773575045,
-            "velocityX": 3.0461070423551457,
-            "velocityY": -1.2864637939008783,
-            "timestamp": 1.694093935208483
-        },
-        {
-            "x": 5.248756740712563,
-            "y": 4.014017499979329,
-            "heading": 1.589989878848282,
-            "angularVelocity": -1.1497421005375177,
-            "velocityX": 3.2010018879727395,
-            "velocityY": -1.0093660005294856,
-            "timestamp": 1.7322407647535947
-        },
-        {
-            "x": 5.376454980725802,
-            "y": 3.9862068849459353,
-            "heading": 1.5506775545962859,
-            "angularVelocity": -1.0305528590747366,
-            "velocityX": 3.347545301559156,
-            "velocityY": -0.7290413217820071,
-            "timestamp": 1.7703875942987064
-        },
-        {
-            "x": 5.509414833155046,
-            "y": 3.969152206173251,
-            "heading": 1.5183946275971996,
-            "angularVelocity": -0.8462807364186931,
-            "velocityX": 3.485475831533742,
-            "velocityY": -0.4470798484702507,
-            "timestamp": 1.8085344238438181
-        },
-        {
-            "x": 5.647435915880888,
-            "y": 3.96261903739833,
-            "heading": 1.4980750155673637,
-            "angularVelocity": -0.5326684359468331,
-            "velocityX": 3.618153444771329,
-            "velocityY": -0.17126374204166636,
-            "timestamp": 1.8466812533889299
-        },
-        {
-            "x": 5.791726765964232,
-            "y": 3.964767379969892,
-            "heading": 1.498073903211314,
-            "angularVelocity": -0.00002915985583373997,
-            "velocityX": 3.7825122507942877,
-            "velocityY": 0.056317722783366074,
-            "timestamp": 1.8848280829340416
-        },
-        {
-            "x": 5.935369435750422,
-            "y": 3.9785994401105067,
-            "heading": 1.4980736722429098,
-            "angularVelocity": -0.00000605472084526459,
-            "velocityX": 3.765520529467262,
-            "velocityY": 0.3626005176676574,
-            "timestamp": 1.9229749124791533
-        },
-        {
-            "x": 6.078764518036592,
-            "y": 3.9947980029743344,
-            "heading": 1.498073449658831,
-            "angularVelocity": -0.000005834929965972279,
-            "velocityX": 3.7590301473577066,
-            "velocityY": 0.42463719939010114,
-            "timestamp": 1.961121742024265
-        },
-        {
-            "x": 6.222150100858587,
-            "y": 4.0110804389753145,
-            "heading": 1.4980732273393433,
-            "angularVelocity": -0.000005827993841710607,
-            "velocityX": 3.7587811236710817,
-            "velocityY": 0.4268358915939984,
-            "timestamp": 1.9992685715693768
-        },
-        {
-            "x": 6.3655338107868324,
-            "y": 4.0273793596330805,
-            "heading": 1.4980730050704238,
-            "angularVelocity": -0.000005826668223171757,
-            "velocityX": 3.758732026701189,
-            "velocityY": 0.42726802860026514,
-            "timestamp": 2.0374154011144885
-        },
-        {
-            "x": 6.508922861230399,
-            "y": 4.043631229943055,
-            "heading": 1.4980727826574463,
-            "angularVelocity": -0.000005830444635518525,
-            "velocityX": 3.7588720256305224,
-            "velocityY": 0.42603462734214087,
-            "timestamp": 2.0755622306596
-        },
-        {
-            "x": 6.6523224677448685,
-            "y": 4.059789689420356,
-            "heading": 1.4980725589565675,
-            "angularVelocity": -0.000005864206314008162,
-            "velocityX": 3.759148747731203,
-            "velocityY": 0.4235859092458616,
-            "timestamp": 2.1137090602047115
-        },
-        {
-            "x": 6.792869039761006,
-            "y": 4.068475137054533,
-            "heading": 1.4874839096219183,
-            "angularVelocity": -0.2775761304689174,
-            "velocityX": 3.684357879593819,
-            "velocityY": 0.2276846526483585,
-            "timestamp": 2.151855889749823
-        },
-        {
-            "x": 6.923724640852256,
-            "y": 4.075654653912489,
-            "heading": 1.4487910280540974,
-            "angularVelocity": -1.014314479844837,
-            "velocityX": 3.430313938317647,
-            "velocityY": 0.18820743280285396,
-            "timestamp": 2.1900027192949345
-        },
-        {
-            "x": 7.04453293582936,
-            "y": 4.081916427546979,
-            "heading": 1.3838287724616933,
-            "angularVelocity": -1.7029529417506197,
-            "velocityX": 3.1669288488112004,
-            "velocityY": 0.1641492545881335,
-            "timestamp": 2.228149548840046
-        },
-        {
-            "x": 7.155015675902827,
-            "y": 4.087769066352678,
-            "heading": 1.2944912667640571,
-            "angularVelocity": -2.3419378953075096,
-            "velocityX": 2.896249606872667,
-            "velocityY": 0.1534239902944478,
-            "timestamp": 2.2662963783851575
-        },
-        {
-            "x": 7.255027442080941,
-            "y": 4.093665982164644,
-            "heading": 1.1821266374518384,
-            "angularVelocity": -2.945582389205927,
-            "velocityX": 2.621758279016409,
-            "velocityY": 0.15458468980091475,
-            "timestamp": 2.304443207930269
-        },
-        {
-            "x": 7.344594486390613,
-            "y": 4.100041344791564,
-            "heading": 1.0471923484078691,
-            "angularVelocity": -3.5372346969048754,
-            "velocityX": 2.3479551348760803,
-            "velocityY": 0.16712693303897977,
-            "timestamp": 2.3425900374753805
-        },
-        {
-            "x": 7.423972884493799,
-            "y": 4.107363761305008,
-            "heading": 0.8891062073498277,
-            "angularVelocity": -4.144148883225662,
-            "velocityX": 2.080864885752389,
-            "velocityY": 0.19195347557968287,
-            "timestamp": 2.380736867020492
-        },
-        {
-            "x": 7.492968640563269,
-            "y": 4.113132211800853,
-            "heading": 0.7092469399580156,
-            "angularVelocity": -4.714920467480658,
-            "velocityX": 1.8086891333366921,
-            "velocityY": 0.1512170359820358,
-            "timestamp": 2.4188836965656035
-        },
-        {
-            "x": 7.549873361136887,
-            "y": 4.117954510008317,
-            "heading": 0.5306887118642236,
-            "angularVelocity": -4.680814374954733,
-            "velocityX": 1.4917287033341673,
-            "velocityY": 0.12641412838299443,
-            "timestamp": 2.457030526110715
-        },
-        {
-            "x": 7.595282353021441,
-            "y": 4.121668824298383,
-            "heading": 0.3658074925223697,
-            "angularVelocity": -4.322278451654422,
-            "velocityX": 1.1903739426332636,
-            "velocityY": 0.09736888581300335,
-            "timestamp": 2.4951773556558265
-        },
-        {
-            "x": 7.630209510034256,
-            "y": 4.124185175295285,
-            "heading": 0.22438733020455492,
-            "angularVelocity": -3.7072586111122043,
-            "velocityX": 0.9155978997292245,
-            "velocityY": 0.06596487904592362,
-            "timestamp": 2.533324185200938
-        },
-        {
-            "x": 7.65555835714788,
-            "y": 4.125725160446138,
-            "heading": 0.1131980096846438,
-            "angularVelocity": -2.91477225881568,
-            "velocityX": 0.6645073107231708,
-            "velocityY": 0.04036993818976583,
-            "timestamp": 2.5714710147460496
-        },
-        {
-            "x": 7.671987700512514,
-            "y": 4.126592780034806,
-            "heading": 0.03644789338444829,
-            "angularVelocity": -2.011965796774904,
-            "velocityX": 0.43068699445162756,
-            "velocityY": 0.02274421227203338,
-            "timestamp": 2.609617844291161
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -0.003,
-            "angularVelocity": -1.0341067358634992,
-            "velocityX": 0.21003841165919293,
-            "velocityY": 0.010675067103991773,
-            "timestamp": 2.6477646738362726
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -0.003,
-            "angularVelocity": 1.475285929099793e-30,
-            "velocityX": 9.681452358723228e-32,
-            "velocityY": 1.6696229962948817e-32,
-            "timestamp": 2.685911503381384
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.1.traj
deleted file mode 100644
index f0cd1af3..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.1.traj	
+++ /dev/null
@@ -1,454 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7232220177061155,
-            "y": 6.714742544338994,
-            "heading": 0.9631209877994957,
-            "angularVelocity": -1.4677311041544436,
-            "velocityX": 0.25242736725510606,
-            "velocityY": -0.10037240309808587,
-            "timestamp": 0.052379493752565884
-        },
-        {
-            "x": 0.7500587763634208,
-            "y": 6.703146440373859,
-            "heading": 0.8150187561291465,
-            "angularVelocity": -2.8274849766583348,
-            "velocityX": 0.5123523870635092,
-            "velocityY": -0.22138633145086953,
-            "timestamp": 0.10475898750513177
-        },
-        {
-            "x": 0.7962886642360024,
-            "y": 6.691501707403842,
-            "heading": 0.6243144861489608,
-            "angularVelocity": -3.6408192656662295,
-            "velocityX": 0.882595163881611,
-            "velocityY": -0.22231472921494633,
-            "timestamp": 0.15713848125769764
-        },
-        {
-            "x": 0.8619102089829233,
-            "y": 6.679485060515379,
-            "heading": 0.3880805528376709,
-            "angularVelocity": -4.510046134222473,
-            "velocityX": 1.2528098315899883,
-            "velocityY": -0.22941510174244611,
-            "timestamp": 0.20951797501026354
-        },
-        {
-            "x": 0.948820798934311,
-            "y": 6.665315928828611,
-            "heading": 0.12168049392786562,
-            "angularVelocity": -5.0859609328842605,
-            "velocityX": 1.6592483761286883,
-            "velocityY": -0.2705091376732434,
-            "timestamp": 0.26189746876282943
-        },
-        {
-            "x": 1.058148493976547,
-            "y": 6.648822752325803,
-            "heading": -0.13051602501618126,
-            "angularVelocity": -4.814794891592335,
-            "velocityX": 2.0872232091185596,
-            "velocityY": -0.3148785015128142,
-            "timestamp": 0.31427696251539533
-        },
-        {
-            "x": 1.1845022977802875,
-            "y": 6.626138581630819,
-            "heading": -0.32549006587483453,
-            "angularVelocity": -3.722335343286937,
-            "velocityX": 2.41227615525686,
-            "velocityY": -0.43307350014000373,
-            "timestamp": 0.36665645626796123
-        },
-        {
-            "x": 1.3283887493945847,
-            "y": 6.598463049204702,
-            "heading": -0.4648019456307372,
-            "angularVelocity": -2.6596644941624366,
-            "velocityX": 2.7469996616232804,
-            "velocityY": -0.5283657867495356,
-            "timestamp": 0.4190359500205271
-        },
-        {
-            "x": 1.4904287339592295,
-            "y": 6.567163978467113,
-            "heading": -0.552248498766723,
-            "angularVelocity": -1.6694806854963555,
-            "velocityX": 3.0935767598308836,
-            "velocityY": -0.5975443536251414,
-            "timestamp": 0.471415443773093
-        },
-        {
-            "x": 1.6709503203675815,
-            "y": 6.533637134258406,
-            "heading": -0.590326905081816,
-            "angularVelocity": -0.7269716369343046,
-            "velocityX": 3.4464171658686396,
-            "velocityY": -0.6400757587899589,
-            "timestamp": 0.5237949375256589
-        },
-        {
-            "x": 1.8675588987397893,
-            "y": 6.50898247632772,
-            "heading": -0.5903272348122566,
-            "angularVelocity": -0.000006295029161022994,
-            "velocityX": 3.7535410193340524,
-            "velocityY": -0.4706929403928793,
-            "timestamp": 0.5761744312782248
-        },
-        {
-            "x": 2.0641677208301488,
-            "y": 6.4843297618820115,
-            "heading": -0.5903275645018402,
-            "angularVelocity": -0.000006294249141189924,
-            "velocityX": 3.7535456722646963,
-            "velocityY": -0.47065583646465164,
-            "timestamp": 0.6285539250307907
-        },
-        {
-            "x": 2.2607765429184905,
-            "y": 6.459677047420138,
-            "heading": -0.5903278941913652,
-            "angularVelocity": -0.000006294248023551401,
-            "velocityX": 3.753545672226176,
-            "velocityY": -0.4706558367732492,
-            "timestamp": 0.6809334187833566
-        },
-        {
-            "x": 2.4573850313302983,
-            "y": 6.435021672008896,
-            "heading": -0.5903282238892777,
-            "angularVelocity": -0.000006294408153628446,
-            "velocityX": 3.753539301860412,
-            "velocityY": -0.4707066381303828,
-            "timestamp": 0.7333129125359225
-        },
-        {
-            "x": 2.6504394143479257,
-            "y": 6.390381144575869,
-            "heading": -0.5903286607455448,
-            "angularVelocity": -0.000008340215525789833,
-            "velocityX": 3.685686309409395,
-            "velocityY": -0.8522519832647445,
-            "timestamp": 0.7856924062884884
-        },
-        {
-            "x": 2.837061882019043,
-            "y": 6.323787212371826,
-            "heading": -0.5903296599203023,
-            "angularVelocity": -0.000019075685653139892,
-            "velocityX": 3.56289177884571,
-            "velocityY": -1.2713741090859834,
-            "timestamp": 0.8380719000410543
-        },
-        {
-            "x": 3.013317183081598,
-            "y": 6.237060061778313,
-            "heading": -0.5903324266212199,
-            "angularVelocity": -0.000053280252957349455,
-            "velocityX": 3.394268952515208,
-            "velocityY": -1.6701640905268176,
-            "timestamp": 0.8899992232815851
-        },
-        {
-            "x": 3.177545337709341,
-            "y": 6.131367851066497,
-            "heading": -0.5941789632152575,
-            "angularVelocity": -0.0740753875608063,
-            "velocityX": 3.1626539628670427,
-            "velocityY": -2.0353872319249358,
-            "timestamp": 0.9419265465221159
-        },
-        {
-            "x": 3.3316611475687807,
-            "y": 6.009565055315307,
-            "heading": -0.5941812553691556,
-            "angularVelocity": -0.00004414157624491074,
-            "velocityX": 2.967913619301448,
-            "velocityY": -2.3456397932739814,
-            "timestamp": 0.9938538697626468
-        },
-        {
-            "x": 3.48577581317155,
-            "y": 5.887760811491488,
-            "heading": -0.5941835469667753,
-            "angularVelocity": -0.00004413086361592441,
-            "velocityX": 2.9678915835676754,
-            "velocityY": -2.3456676797995843,
-            "timestamp": 1.0457811930031775
-        },
-        {
-            "x": 3.639890478746931,
-            "y": 5.7659565676417115,
-            "heading": -0.5941858385862709,
-            "angularVelocity": -0.00004413128489121798,
-            "velocityX": 2.967891583040235,
-            "velocityY": -2.3456676802994654,
-            "timestamp": 1.0977085162437084
-        },
-        {
-            "x": 3.7940051443206335,
-            "y": 5.644152323798514,
-            "heading": -0.594188130227655,
-            "angularVelocity": -0.0000441317064133718,
-            "velocityX": 2.9678915830079062,
-            "velocityY": -2.3456676801727796,
-            "timestamp": 1.1496358394842392
-        },
-        {
-            "x": 3.948119809892657,
-            "y": 5.522348079961896,
-            "heading": -0.5941904218909294,
-            "angularVelocity": -0.0000441321279705331,
-            "velocityX": 2.9678915829755823,
-            "velocityY": -2.345667680046072,
-            "timestamp": 1.20156316272477
-        },
-        {
-            "x": 4.1022344754630025,
-            "y": 5.400543836131857,
-            "heading": -0.5941927135760963,
-            "angularVelocity": -0.00004413254956830847,
-            "velocityX": 2.9678915829432535,
-            "velocityY": -2.3456676799193525,
-            "timestamp": 1.2534904859653009
-        },
-        {
-            "x": 4.256349141031668,
-            "y": 5.2787395923083995,
-            "heading": -0.5941950052831575,
-            "angularVelocity": -0.000044132971203721087,
-            "velocityX": 2.9678915829109185,
-            "velocityY": -2.3456676797926255,
-            "timestamp": 1.3054178092058317
-        },
-        {
-            "x": 4.410463806598655,
-            "y": 5.1569353484915235,
-            "heading": -0.594197297012115,
-            "angularVelocity": -0.00004413339287361589,
-            "velocityX": 2.9678915828785764,
-            "velocityY": -2.345667679665889,
-            "timestamp": 1.3573451324463626
-        },
-        {
-            "x": 4.564578472163962,
-            "y": 5.035131104681229,
-            "heading": -0.5941995887629704,
-            "angularVelocity": -0.000044133814578891406,
-            "velocityX": 2.9678915828462293,
-            "velocityY": -2.3456676795391433,
-            "timestamp": 1.4092724556868934
-        },
-        {
-            "x": 4.718693137727589,
-            "y": 4.913326860877516,
-            "heading": -0.5942018805357254,
-            "angularVelocity": -0.00004413423631330234,
-            "velocityX": 2.967891582813878,
-            "velocityY": -2.3456676794123914,
-            "timestamp": 1.4611997789274243
-        },
-        {
-            "x": 4.872807803289535,
-            "y": 4.791522617080385,
-            "heading": -0.5942041723303811,
-            "angularVelocity": -0.00004413465807134274,
-            "velocityX": 2.9678915827815215,
-            "velocityY": -2.3456676792856337,
-            "timestamp": 1.513127102167955
-        },
-        {
-            "x": 5.026922468849802,
-            "y": 4.669718373289836,
-            "heading": -0.5942064641469391,
-            "angularVelocity": -0.000044135079855748624,
-            "velocityX": 2.9678915827491625,
-            "velocityY": -2.3456676791588693,
-            "timestamp": 1.565054425408486
-        },
-        {
-            "x": 5.181037134408388,
-            "y": 4.5479141295058705,
-            "heading": -0.5942087559854001,
-            "angularVelocity": -0.000044135501659061016,
-            "velocityX": 2.967891582716799,
-            "velocityY": -2.345667679032102,
-            "timestamp": 1.6169817486490168
-        },
-        {
-            "x": 5.335151799963721,
-            "y": 4.4261098857264995,
-            "heading": -0.594211047845765,
-            "angularVelocity": -0.00004413592347400816,
-            "velocityX": 2.967891582654166,
-            "velocityY": -2.345667678943628,
-            "timestamp": 1.6689090718895476
-        },
-        {
-            "x": 5.489266395568848,
-            "y": 4.304305553436279,
-            "heading": -0.5942133396987443,
-            "angularVelocity": -0.000044135781248471857,
-            "velocityX": 2.9678902355751515,
-            "velocityY": -2.345669383457615,
-            "timestamp": 1.7208363951300785
-        },
-        {
-            "x": 5.673870655984677,
-            "y": 4.147383169820797,
-            "heading": -0.5942132891163008,
-            "angularVelocity": 7.897648233188991e-7,
-            "velocityX": 2.882303439061699,
-            "velocityY": -2.450094732060046,
-            "timestamp": 1.7848838711570736
-        },
-        {
-            "x": 5.858474845870276,
-            "y": 3.9904607032332415,
-            "heading": -0.5942132385338614,
-            "angularVelocity": 7.897647575237561e-7,
-            "velocityX": 2.882302337843733,
-            "velocityY": -2.450096027537681,
-            "timestamp": 1.8489313471840687
-        },
-        {
-            "x": 6.043079035755059,
-            "y": 3.833538236644727,
-            "heading": -0.5942131879514223,
-            "angularVelocity": 7.897647543646741e-7,
-            "velocityX": 2.882302337831004,
-            "velocityY": -2.4500960275526547,
-            "timestamp": 1.9129788232110638
-        },
-        {
-            "x": 6.227683225639842,
-            "y": 3.676615770056212,
-            "heading": -0.5942131373689834,
-            "angularVelocity": 7.897647493463809e-7,
-            "velocityX": 2.8823023378310055,
-            "velocityY": -2.450096027552654,
-            "timestamp": 1.977026299238059
-        },
-        {
-            "x": 6.412287415524625,
-            "y": 3.5196933034676974,
-            "heading": -0.5942130867865447,
-            "angularVelocity": 7.89764745190247e-7,
-            "velocityX": 2.882302337831007,
-            "velocityY": -2.4500960275526538,
-            "timestamp": 2.041073775265054
-        },
-        {
-            "x": 6.596891605409407,
-            "y": 3.362770836879183,
-            "heading": -0.5942130362041063,
-            "angularVelocity": 7.897647430611541e-7,
-            "velocityX": 2.882302337831007,
-            "velocityY": -2.450096027552653,
-            "timestamp": 2.105121251292049
-        },
-        {
-            "x": 6.781495795294782,
-            "y": 3.2058483702913625,
-            "heading": -0.5942129856216681,
-            "angularVelocity": 7.897647381910717e-7,
-            "velocityX": 2.8823023378402177,
-            "velocityY": -2.4500960275418184,
-            "timestamp": 2.169168727319044
-        },
-        {
-            "x": 6.966100036209046,
-            "y": 3.048925963734244,
-            "heading": -0.5942129350390435,
-            "angularVelocity": 7.897676514556789e-7,
-            "velocityX": 2.882303134575621,
-            "velocityY": -2.4500950902573897,
-            "timestamp": 2.233216203346039
-        },
-        {
-            "x": 7.143621774279278,
-            "y": 2.9040156366918444,
-            "heading": -0.5543160517170741,
-            "angularVelocity": 0.6229267068253146,
-            "velocityX": 2.771721058850299,
-            "velocityY": -2.262545474568311,
-            "timestamp": 2.2972636793730343
-        },
-        {
-            "x": 7.2980980894932665,
-            "y": 2.7758641788664877,
-            "heading": -0.43954991420980805,
-            "angularVelocity": 1.7918916501704738,
-            "velocityX": 2.411903244225882,
-            "velocityY": -2.000882248214473,
-            "timestamp": 2.3613111554000294
-        },
-        {
-            "x": 7.426195766165712,
-            "y": 2.6690947773000513,
-            "heading": -0.31008673137290993,
-            "angularVelocity": 2.0213627588124035,
-            "velocityX": 2.000042540606181,
-            "velocityY": -1.667035271170323,
-            "timestamp": 2.4253586314270246
-        },
-        {
-            "x": 7.528097771332223,
-            "y": 2.5840084569104262,
-            "heading": -0.19375204823220424,
-            "angularVelocity": 1.816382008428752,
-            "velocityX": 1.591038577750685,
-            "velocityY": -1.3284882663254731,
-            "timestamp": 2.4894061074540197
-        },
-        {
-            "x": 7.6042028509496316,
-            "y": 2.5203982409108687,
-            "heading": -0.10083265861354884,
-            "angularVelocity": 1.4507892485800875,
-            "velocityX": 1.1882604021012733,
-            "velocityY": -0.9931728765197068,
-            "timestamp": 2.553453583481015
-        },
-        {
-            "x": 7.654775762663328,
-            "y": 2.4780994279137802,
-            "heading": -0.036281315483046445,
-            "angularVelocity": 1.0078670875850742,
-            "velocityX": 0.7896159981757911,
-            "velocityY": -0.660429038284975,
-            "timestamp": 2.61750105950801
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -0.003,
-            "angularVelocity": 0.5196350824038538,
-            "velocityX": 0.39383655533967327,
-            "velocityY": -0.32943418261927393,
-            "timestamp": 2.681548535535005
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -0.003,
-            "angularVelocity": 1.9104375826405664e-34,
-            "velocityX": -1.672523979523505e-35,
-            "velocityY": 0,
-            "timestamp": 2.745596011562
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.traj
deleted file mode 100644
index f0cd1af3..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.traj	
+++ /dev/null
@@ -1,454 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7232220177061155,
-            "y": 6.714742544338994,
-            "heading": 0.9631209877994957,
-            "angularVelocity": -1.4677311041544436,
-            "velocityX": 0.25242736725510606,
-            "velocityY": -0.10037240309808587,
-            "timestamp": 0.052379493752565884
-        },
-        {
-            "x": 0.7500587763634208,
-            "y": 6.703146440373859,
-            "heading": 0.8150187561291465,
-            "angularVelocity": -2.8274849766583348,
-            "velocityX": 0.5123523870635092,
-            "velocityY": -0.22138633145086953,
-            "timestamp": 0.10475898750513177
-        },
-        {
-            "x": 0.7962886642360024,
-            "y": 6.691501707403842,
-            "heading": 0.6243144861489608,
-            "angularVelocity": -3.6408192656662295,
-            "velocityX": 0.882595163881611,
-            "velocityY": -0.22231472921494633,
-            "timestamp": 0.15713848125769764
-        },
-        {
-            "x": 0.8619102089829233,
-            "y": 6.679485060515379,
-            "heading": 0.3880805528376709,
-            "angularVelocity": -4.510046134222473,
-            "velocityX": 1.2528098315899883,
-            "velocityY": -0.22941510174244611,
-            "timestamp": 0.20951797501026354
-        },
-        {
-            "x": 0.948820798934311,
-            "y": 6.665315928828611,
-            "heading": 0.12168049392786562,
-            "angularVelocity": -5.0859609328842605,
-            "velocityX": 1.6592483761286883,
-            "velocityY": -0.2705091376732434,
-            "timestamp": 0.26189746876282943
-        },
-        {
-            "x": 1.058148493976547,
-            "y": 6.648822752325803,
-            "heading": -0.13051602501618126,
-            "angularVelocity": -4.814794891592335,
-            "velocityX": 2.0872232091185596,
-            "velocityY": -0.3148785015128142,
-            "timestamp": 0.31427696251539533
-        },
-        {
-            "x": 1.1845022977802875,
-            "y": 6.626138581630819,
-            "heading": -0.32549006587483453,
-            "angularVelocity": -3.722335343286937,
-            "velocityX": 2.41227615525686,
-            "velocityY": -0.43307350014000373,
-            "timestamp": 0.36665645626796123
-        },
-        {
-            "x": 1.3283887493945847,
-            "y": 6.598463049204702,
-            "heading": -0.4648019456307372,
-            "angularVelocity": -2.6596644941624366,
-            "velocityX": 2.7469996616232804,
-            "velocityY": -0.5283657867495356,
-            "timestamp": 0.4190359500205271
-        },
-        {
-            "x": 1.4904287339592295,
-            "y": 6.567163978467113,
-            "heading": -0.552248498766723,
-            "angularVelocity": -1.6694806854963555,
-            "velocityX": 3.0935767598308836,
-            "velocityY": -0.5975443536251414,
-            "timestamp": 0.471415443773093
-        },
-        {
-            "x": 1.6709503203675815,
-            "y": 6.533637134258406,
-            "heading": -0.590326905081816,
-            "angularVelocity": -0.7269716369343046,
-            "velocityX": 3.4464171658686396,
-            "velocityY": -0.6400757587899589,
-            "timestamp": 0.5237949375256589
-        },
-        {
-            "x": 1.8675588987397893,
-            "y": 6.50898247632772,
-            "heading": -0.5903272348122566,
-            "angularVelocity": -0.000006295029161022994,
-            "velocityX": 3.7535410193340524,
-            "velocityY": -0.4706929403928793,
-            "timestamp": 0.5761744312782248
-        },
-        {
-            "x": 2.0641677208301488,
-            "y": 6.4843297618820115,
-            "heading": -0.5903275645018402,
-            "angularVelocity": -0.000006294249141189924,
-            "velocityX": 3.7535456722646963,
-            "velocityY": -0.47065583646465164,
-            "timestamp": 0.6285539250307907
-        },
-        {
-            "x": 2.2607765429184905,
-            "y": 6.459677047420138,
-            "heading": -0.5903278941913652,
-            "angularVelocity": -0.000006294248023551401,
-            "velocityX": 3.753545672226176,
-            "velocityY": -0.4706558367732492,
-            "timestamp": 0.6809334187833566
-        },
-        {
-            "x": 2.4573850313302983,
-            "y": 6.435021672008896,
-            "heading": -0.5903282238892777,
-            "angularVelocity": -0.000006294408153628446,
-            "velocityX": 3.753539301860412,
-            "velocityY": -0.4707066381303828,
-            "timestamp": 0.7333129125359225
-        },
-        {
-            "x": 2.6504394143479257,
-            "y": 6.390381144575869,
-            "heading": -0.5903286607455448,
-            "angularVelocity": -0.000008340215525789833,
-            "velocityX": 3.685686309409395,
-            "velocityY": -0.8522519832647445,
-            "timestamp": 0.7856924062884884
-        },
-        {
-            "x": 2.837061882019043,
-            "y": 6.323787212371826,
-            "heading": -0.5903296599203023,
-            "angularVelocity": -0.000019075685653139892,
-            "velocityX": 3.56289177884571,
-            "velocityY": -1.2713741090859834,
-            "timestamp": 0.8380719000410543
-        },
-        {
-            "x": 3.013317183081598,
-            "y": 6.237060061778313,
-            "heading": -0.5903324266212199,
-            "angularVelocity": -0.000053280252957349455,
-            "velocityX": 3.394268952515208,
-            "velocityY": -1.6701640905268176,
-            "timestamp": 0.8899992232815851
-        },
-        {
-            "x": 3.177545337709341,
-            "y": 6.131367851066497,
-            "heading": -0.5941789632152575,
-            "angularVelocity": -0.0740753875608063,
-            "velocityX": 3.1626539628670427,
-            "velocityY": -2.0353872319249358,
-            "timestamp": 0.9419265465221159
-        },
-        {
-            "x": 3.3316611475687807,
-            "y": 6.009565055315307,
-            "heading": -0.5941812553691556,
-            "angularVelocity": -0.00004414157624491074,
-            "velocityX": 2.967913619301448,
-            "velocityY": -2.3456397932739814,
-            "timestamp": 0.9938538697626468
-        },
-        {
-            "x": 3.48577581317155,
-            "y": 5.887760811491488,
-            "heading": -0.5941835469667753,
-            "angularVelocity": -0.00004413086361592441,
-            "velocityX": 2.9678915835676754,
-            "velocityY": -2.3456676797995843,
-            "timestamp": 1.0457811930031775
-        },
-        {
-            "x": 3.639890478746931,
-            "y": 5.7659565676417115,
-            "heading": -0.5941858385862709,
-            "angularVelocity": -0.00004413128489121798,
-            "velocityX": 2.967891583040235,
-            "velocityY": -2.3456676802994654,
-            "timestamp": 1.0977085162437084
-        },
-        {
-            "x": 3.7940051443206335,
-            "y": 5.644152323798514,
-            "heading": -0.594188130227655,
-            "angularVelocity": -0.0000441317064133718,
-            "velocityX": 2.9678915830079062,
-            "velocityY": -2.3456676801727796,
-            "timestamp": 1.1496358394842392
-        },
-        {
-            "x": 3.948119809892657,
-            "y": 5.522348079961896,
-            "heading": -0.5941904218909294,
-            "angularVelocity": -0.0000441321279705331,
-            "velocityX": 2.9678915829755823,
-            "velocityY": -2.345667680046072,
-            "timestamp": 1.20156316272477
-        },
-        {
-            "x": 4.1022344754630025,
-            "y": 5.400543836131857,
-            "heading": -0.5941927135760963,
-            "angularVelocity": -0.00004413254956830847,
-            "velocityX": 2.9678915829432535,
-            "velocityY": -2.3456676799193525,
-            "timestamp": 1.2534904859653009
-        },
-        {
-            "x": 4.256349141031668,
-            "y": 5.2787395923083995,
-            "heading": -0.5941950052831575,
-            "angularVelocity": -0.000044132971203721087,
-            "velocityX": 2.9678915829109185,
-            "velocityY": -2.3456676797926255,
-            "timestamp": 1.3054178092058317
-        },
-        {
-            "x": 4.410463806598655,
-            "y": 5.1569353484915235,
-            "heading": -0.594197297012115,
-            "angularVelocity": -0.00004413339287361589,
-            "velocityX": 2.9678915828785764,
-            "velocityY": -2.345667679665889,
-            "timestamp": 1.3573451324463626
-        },
-        {
-            "x": 4.564578472163962,
-            "y": 5.035131104681229,
-            "heading": -0.5941995887629704,
-            "angularVelocity": -0.000044133814578891406,
-            "velocityX": 2.9678915828462293,
-            "velocityY": -2.3456676795391433,
-            "timestamp": 1.4092724556868934
-        },
-        {
-            "x": 4.718693137727589,
-            "y": 4.913326860877516,
-            "heading": -0.5942018805357254,
-            "angularVelocity": -0.00004413423631330234,
-            "velocityX": 2.967891582813878,
-            "velocityY": -2.3456676794123914,
-            "timestamp": 1.4611997789274243
-        },
-        {
-            "x": 4.872807803289535,
-            "y": 4.791522617080385,
-            "heading": -0.5942041723303811,
-            "angularVelocity": -0.00004413465807134274,
-            "velocityX": 2.9678915827815215,
-            "velocityY": -2.3456676792856337,
-            "timestamp": 1.513127102167955
-        },
-        {
-            "x": 5.026922468849802,
-            "y": 4.669718373289836,
-            "heading": -0.5942064641469391,
-            "angularVelocity": -0.000044135079855748624,
-            "velocityX": 2.9678915827491625,
-            "velocityY": -2.3456676791588693,
-            "timestamp": 1.565054425408486
-        },
-        {
-            "x": 5.181037134408388,
-            "y": 4.5479141295058705,
-            "heading": -0.5942087559854001,
-            "angularVelocity": -0.000044135501659061016,
-            "velocityX": 2.967891582716799,
-            "velocityY": -2.345667679032102,
-            "timestamp": 1.6169817486490168
-        },
-        {
-            "x": 5.335151799963721,
-            "y": 4.4261098857264995,
-            "heading": -0.594211047845765,
-            "angularVelocity": -0.00004413592347400816,
-            "velocityX": 2.967891582654166,
-            "velocityY": -2.345667678943628,
-            "timestamp": 1.6689090718895476
-        },
-        {
-            "x": 5.489266395568848,
-            "y": 4.304305553436279,
-            "heading": -0.5942133396987443,
-            "angularVelocity": -0.000044135781248471857,
-            "velocityX": 2.9678902355751515,
-            "velocityY": -2.345669383457615,
-            "timestamp": 1.7208363951300785
-        },
-        {
-            "x": 5.673870655984677,
-            "y": 4.147383169820797,
-            "heading": -0.5942132891163008,
-            "angularVelocity": 7.897648233188991e-7,
-            "velocityX": 2.882303439061699,
-            "velocityY": -2.450094732060046,
-            "timestamp": 1.7848838711570736
-        },
-        {
-            "x": 5.858474845870276,
-            "y": 3.9904607032332415,
-            "heading": -0.5942132385338614,
-            "angularVelocity": 7.897647575237561e-7,
-            "velocityX": 2.882302337843733,
-            "velocityY": -2.450096027537681,
-            "timestamp": 1.8489313471840687
-        },
-        {
-            "x": 6.043079035755059,
-            "y": 3.833538236644727,
-            "heading": -0.5942131879514223,
-            "angularVelocity": 7.897647543646741e-7,
-            "velocityX": 2.882302337831004,
-            "velocityY": -2.4500960275526547,
-            "timestamp": 1.9129788232110638
-        },
-        {
-            "x": 6.227683225639842,
-            "y": 3.676615770056212,
-            "heading": -0.5942131373689834,
-            "angularVelocity": 7.897647493463809e-7,
-            "velocityX": 2.8823023378310055,
-            "velocityY": -2.450096027552654,
-            "timestamp": 1.977026299238059
-        },
-        {
-            "x": 6.412287415524625,
-            "y": 3.5196933034676974,
-            "heading": -0.5942130867865447,
-            "angularVelocity": 7.89764745190247e-7,
-            "velocityX": 2.882302337831007,
-            "velocityY": -2.4500960275526538,
-            "timestamp": 2.041073775265054
-        },
-        {
-            "x": 6.596891605409407,
-            "y": 3.362770836879183,
-            "heading": -0.5942130362041063,
-            "angularVelocity": 7.897647430611541e-7,
-            "velocityX": 2.882302337831007,
-            "velocityY": -2.450096027552653,
-            "timestamp": 2.105121251292049
-        },
-        {
-            "x": 6.781495795294782,
-            "y": 3.2058483702913625,
-            "heading": -0.5942129856216681,
-            "angularVelocity": 7.897647381910717e-7,
-            "velocityX": 2.8823023378402177,
-            "velocityY": -2.4500960275418184,
-            "timestamp": 2.169168727319044
-        },
-        {
-            "x": 6.966100036209046,
-            "y": 3.048925963734244,
-            "heading": -0.5942129350390435,
-            "angularVelocity": 7.897676514556789e-7,
-            "velocityX": 2.882303134575621,
-            "velocityY": -2.4500950902573897,
-            "timestamp": 2.233216203346039
-        },
-        {
-            "x": 7.143621774279278,
-            "y": 2.9040156366918444,
-            "heading": -0.5543160517170741,
-            "angularVelocity": 0.6229267068253146,
-            "velocityX": 2.771721058850299,
-            "velocityY": -2.262545474568311,
-            "timestamp": 2.2972636793730343
-        },
-        {
-            "x": 7.2980980894932665,
-            "y": 2.7758641788664877,
-            "heading": -0.43954991420980805,
-            "angularVelocity": 1.7918916501704738,
-            "velocityX": 2.411903244225882,
-            "velocityY": -2.000882248214473,
-            "timestamp": 2.3613111554000294
-        },
-        {
-            "x": 7.426195766165712,
-            "y": 2.6690947773000513,
-            "heading": -0.31008673137290993,
-            "angularVelocity": 2.0213627588124035,
-            "velocityX": 2.000042540606181,
-            "velocityY": -1.667035271170323,
-            "timestamp": 2.4253586314270246
-        },
-        {
-            "x": 7.528097771332223,
-            "y": 2.5840084569104262,
-            "heading": -0.19375204823220424,
-            "angularVelocity": 1.816382008428752,
-            "velocityX": 1.591038577750685,
-            "velocityY": -1.3284882663254731,
-            "timestamp": 2.4894061074540197
-        },
-        {
-            "x": 7.6042028509496316,
-            "y": 2.5203982409108687,
-            "heading": -0.10083265861354884,
-            "angularVelocity": 1.4507892485800875,
-            "velocityX": 1.1882604021012733,
-            "velocityY": -0.9931728765197068,
-            "timestamp": 2.553453583481015
-        },
-        {
-            "x": 7.654775762663328,
-            "y": 2.4780994279137802,
-            "heading": -0.036281315483046445,
-            "angularVelocity": 1.0078670875850742,
-            "velocityX": 0.7896159981757911,
-            "velocityY": -0.660429038284975,
-            "timestamp": 2.61750105950801
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -0.003,
-            "angularVelocity": 0.5196350824038538,
-            "velocityX": 0.39383655533967327,
-            "velocityY": -0.32943418261927393,
-            "timestamp": 2.681548535535005
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -0.003,
-            "angularVelocity": 1.9104375826405664e-34,
-            "velocityX": -1.672523979523505e-35,
-            "velocityY": 0,
-            "timestamp": 2.745596011562
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.1.traj
deleted file mode 100644
index 00f6917b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.1.traj	
+++ /dev/null
@@ -1,859 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": 0,
-            "velocityX": 1.8950109818726043e-31,
-            "velocityY": 4.417214679587952e-30,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7239179438589063,
-            "y": 6.714864648059082,
-            "heading": 0.9709045474564667,
-            "angularVelocity": -1.3401351995297424,
-            "velocityX": 0.269944341976283,
-            "velocityY": -0.09960229862677873,
-            "timestamp": 0.051558568544241005
-        },
-        {
-            "x": 0.7525251092531191,
-            "y": 6.703796484721446,
-            "heading": 0.8397844996535714,
-            "angularVelocity": -2.5431281648227726,
-            "velocityX": 0.5548479370541143,
-            "velocityY": -0.21467165691630782,
-            "timestamp": 0.10311713708848201
-        },
-        {
-            "x": 0.8002738106410594,
-            "y": 6.689991944618376,
-            "heading": 0.6713194026020708,
-            "angularVelocity": -3.2674510136339485,
-            "velocityX": 0.9261060331222868,
-            "velocityY": -0.26774482870254956,
-            "timestamp": 0.154675705632723
-        },
-        {
-            "x": 0.8672232338097706,
-            "y": 6.675504894458598,
-            "heading": 0.46260124525274376,
-            "angularVelocity": -4.048175952947979,
-            "velocityX": 1.2985120622830493,
-            "velocityY": -0.28098239669505554,
-            "timestamp": 0.20623427417696402
-        },
-        {
-            "x": 0.954736838142347,
-            "y": 6.658878661036629,
-            "heading": 0.2244001490575843,
-            "angularVelocity": -4.620009882366156,
-            "velocityX": 1.6973629564117712,
-            "velocityY": -0.3224727507251158,
-            "timestamp": 0.257792842721205
-        },
-        {
-            "x": 1.0641109683124677,
-            "y": 6.639652990231364,
-            "heading": -0.004541406089891365,
-            "angularVelocity": -4.440417211175481,
-            "velocityX": 2.12135699765032,
-            "velocityY": -0.37288992592389836,
-            "timestamp": 0.309351411265446
-        },
-        {
-            "x": 1.1925189459621883,
-            "y": 6.613890706063681,
-            "heading": -0.19800120779802596,
-            "angularVelocity": -3.752233763861444,
-            "velocityX": 2.4905264299488024,
-            "velocityY": -0.49967027586341123,
-            "timestamp": 0.36090997980968703
-        },
-        {
-            "x": 1.3374709433458876,
-            "y": 6.583331183982878,
-            "heading": -0.33396197230189334,
-            "angularVelocity": -2.6370158897471825,
-            "velocityX": 2.811404611812854,
-            "velocityY": -0.5927147115125508,
-            "timestamp": 0.41246854835392804
-        },
-        {
-            "x": 1.49983426972339,
-            "y": 6.549745693102723,
-            "heading": -0.41661329783619183,
-            "angularVelocity": -1.6030570255902088,
-            "velocityX": 3.149104619501603,
-            "velocityY": -0.6514046419107491,
-            "timestamp": 0.46402711689816906
-        },
-        {
-            "x": 1.6800135166896315,
-            "y": 6.515230594671984,
-            "heading": -0.44907632730235497,
-            "angularVelocity": -0.6296340333474829,
-            "velocityX": 3.4946518503843107,
-            "velocityY": -0.6694347691375233,
-            "timestamp": 0.51558568544241
-        },
-        {
-            "x": 1.8735253740457867,
-            "y": 6.490840556482913,
-            "heading": -0.4490767643443874,
-            "angularVelocity": -0.00000847661300213044,
-            "velocityX": 3.7532434049278374,
-            "velocityY": -0.47305499119601163,
-            "timestamp": 0.567144253986651
-        },
-        {
-            "x": 2.067041889759694,
-            "y": 6.46648750315131,
-            "heading": -0.4490772002167485,
-            "angularVelocity": -0.000008453926735380078,
-            "velocityX": 3.753333755723225,
-            "velocityY": -0.47233765442907966,
-            "timestamp": 0.618702822530892
-        },
-        {
-            "x": 2.2605372007770526,
-            "y": 6.441966542289934,
-            "heading": -0.4490776373561405,
-            "angularVelocity": -0.0000084785013311663,
-            "velocityX": 3.7529224817654048,
-            "velocityY": -0.4755942911914249,
-            "timestamp": 0.670261391075133
-        },
-        {
-            "x": 2.45117838314973,
-            "y": 6.400765025555903,
-            "heading": -0.4490787415251664,
-            "angularVelocity": -0.000021415820050526827,
-            "velocityX": 3.6975654630332984,
-            "velocityY": -0.7991206485676197,
-            "timestamp": 0.7218199596193741
-        },
-        {
-            "x": 2.6301938207404594,
-            "y": 6.341459154774922,
-            "heading": -0.46856582254307677,
-            "angularVelocity": -0.377960086337202,
-            "velocityX": 3.472079280806918,
-            "velocityY": -1.150262166999653,
-            "timestamp": 0.7733785281636151
-        },
-        {
-            "x": 2.797205924987793,
-            "y": 6.264003276824951,
-            "heading": -0.5033519686488042,
-            "angularVelocity": -0.6746918521648854,
-            "velocityX": 3.239269610517634,
-            "velocityY": -1.502289146830184,
-            "timestamp": 0.8249370967078561
-        },
-        {
-            "x": 2.911639196984497,
-            "y": 6.1983709493948735,
-            "heading": -0.5311684488221697,
-            "angularVelocity": -0.7502107513814861,
-            "velocityX": 3.0862665022221747,
-            "velocityY": -1.770104534065962,
-            "timestamp": 0.8620153178901536
-        },
-        {
-            "x": 3.0203603627243916,
-            "y": 6.122823189814021,
-            "heading": -0.5565632404510816,
-            "angularVelocity": -0.6848977868761134,
-            "velocityX": 2.9322109387469486,
-            "velocityY": -2.037523839376905,
-            "timestamp": 0.8990935390724512
-        },
-        {
-            "x": 3.1248710366711436,
-            "y": 6.036966714195615,
-            "heading": -0.5720315951438852,
-            "angularVelocity": -0.41718168240112047,
-            "velocityX": 2.8186539325316726,
-            "velocityY": -2.3155500150894683,
-            "timestamp": 0.9361717602547487
-        },
-        {
-            "x": 3.2269084939172057,
-            "y": 5.940732357266897,
-            "heading": -0.5720474132070273,
-            "angularVelocity": -0.00042661332192557474,
-            "velocityX": 2.7519512531259087,
-            "velocityY": -2.5954415789079244,
-            "timestamp": 0.9732499814370462
-        },
-        {
-            "x": 3.3218654576085616,
-            "y": 5.837498151619579,
-            "heading": -0.5720480482162372,
-            "angularVelocity": -0.000017126204806487387,
-            "velocityX": 2.560990270400005,
-            "velocityY": -2.7842275695894796,
-            "timestamp": 1.0103282026193439
-        },
-        {
-            "x": 3.4166998907386708,
-            "y": 5.734151371991349,
-            "heading": -0.5720486788754773,
-            "angularVelocity": -0.00001700888607859636,
-            "velocityX": 2.557685619889607,
-            "velocityY": -2.7872636910247786,
-            "timestamp": 1.0474064238016414
-        },
-        {
-            "x": 3.5114576080251196,
-            "y": 5.630734246765037,
-            "heading": -0.5720493072137337,
-            "angularVelocity": -0.000016946289124090365,
-            "velocityX": 2.555616592803881,
-            "velocityY": -2.7891609125222083,
-            "timestamp": 1.084484644983939
-        },
-        {
-            "x": 3.6061908144763444,
-            "y": 5.527294668181728,
-            "heading": -0.572049934817026,
-            "angularVelocity": -0.00001692646713700802,
-            "velocityX": 2.5549555353779705,
-            "velocityY": -2.7897664797435726,
-            "timestamp": 1.1215628661662365
-        },
-        {
-            "x": 3.7009275327159292,
-            "y": 5.423858305931887,
-            "heading": -0.5720505625260676,
-            "angularVelocity": -0.00001692931919251171,
-            "velocityX": 2.555050248355605,
-            "velocityY": -2.789679735188215,
-            "timestamp": 1.158641087348534
-        },
-        {
-            "x": 3.7956729593070784,
-            "y": 5.320429920428362,
-            "heading": -0.5720511904967662,
-            "angularVelocity": -0.000016936376088998586,
-            "velocityX": 2.5552851126613465,
-            "velocityY": -2.7894646022814182,
-            "timestamp": 1.1957193085308315
-        },
-        {
-            "x": 3.890467288385621,
-            "y": 5.217046353888283,
-            "heading": -0.5720518199780259,
-            "angularVelocity": -0.000016977115933005807,
-            "velocityX": 2.556604013243175,
-            "velocityY": -2.7882558343819337,
-            "timestamp": 1.232797529713129
-        },
-        {
-            "x": 3.9877865474692196,
-            "y": 5.11603605874111,
-            "heading": -0.5720525488313646,
-            "angularVelocity": -0.00001965718190938232,
-            "velocityX": 2.624701400996587,
-            "velocityY": -2.7242486809308257,
-            "timestamp": 1.2698757508954266
-        },
-        {
-            "x": 4.092995643615723,
-            "y": 5.023272514343262,
-            "heading": -0.5720542830492642,
-            "angularVelocity": -0.000046771874277348444,
-            "velocityX": 2.8374903863181586,
-            "velocityY": -2.5018337298792988,
-            "timestamp": 1.3069539720777241
-        },
-        {
-            "x": 4.14638765114505,
-            "y": 4.979871763007162,
-            "heading": -0.5720542261628284,
-            "angularVelocity": 0.000003127581707131923,
-            "velocityX": 2.935460161172297,
-            "velocityY": -2.386146961079825,
-            "timestamp": 1.3251426050680648
-        },
-        {
-            "x": 4.201473544713913,
-            "y": 4.938642209546033,
-            "heading": -0.5720541700948967,
-            "angularVelocity": 0.000003082580845535541,
-            "velocityX": 3.0285889873152927,
-            "velocityY": -2.266775819987239,
-            "timestamp": 1.3433312380584055
-        },
-        {
-            "x": 4.258151834683336,
-            "y": 4.899630631332575,
-            "heading": -0.5720541145494444,
-            "angularVelocity": 0.0000030538552492495714,
-            "velocityX": 3.11613797471883,
-            "velocityY": -2.1448328873407987,
-            "timestamp": 1.3615198710487462
-        },
-        {
-            "x": 4.316201884310526,
-            "y": 4.862691173522813,
-            "heading": -0.5720540592428014,
-            "angularVelocity": 0.000003040725654248182,
-            "velocityX": 3.1915564879460585,
-            "velocityY": -2.030908965471825,
-            "timestamp": 1.3797085040390868
-        },
-        {
-            "x": 4.374575644994192,
-            "y": 4.826265415180517,
-            "heading": -0.5720540039764063,
-            "angularVelocity": 0.0000030385128512512867,
-            "velocityX": 3.209353925315247,
-            "velocityY": -2.002666080603007,
-            "timestamp": 1.3978971370294275
-        },
-        {
-            "x": 4.432961770826435,
-            "y": 4.789859479973715,
-            "heading": -0.572053948711591,
-            "angularVelocity": 0.000003038425994779569,
-            "velocityX": 3.210033753678121,
-            "velocityY": -2.0015762166465794,
-            "timestamp": 1.4160857700197682
-        },
-        {
-            "x": 4.491358693973173,
-            "y": 4.753470866702408,
-            "heading": -0.5720538934471577,
-            "angularVelocity": 0.000003038404992596871,
-            "velocityX": 3.21062738347468,
-            "velocityY": -2.000623867148186,
-            "timestamp": 1.434274403010109
-        },
-        {
-            "x": 4.549763643159385,
-            "y": 4.7170951368878224,
-            "heading": -0.5720538381830028,
-            "angularVelocity": 0.0000030383896855688938,
-            "velocityX": 3.2110686502526127,
-            "velocityY": -1.9999155425186408,
-            "timestamp": 1.4524630360004496
-        },
-        {
-            "x": 4.608173712818011,
-            "y": 4.680727629815019,
-            "heading": -0.5720537829190162,
-            "angularVelocity": 0.0000030383804361409647,
-            "velocityX": 3.211350170717194,
-            "velocityY": -1.9994634611677902,
-            "timestamp": 1.4706516689907903
-        },
-        {
-            "x": 4.666585738503753,
-            "y": 4.644363264529719,
-            "heading": -0.5720537276550893,
-            "angularVelocity": 0.0000030383771462609746,
-            "velocityX": 3.211457711899863,
-            "velocityY": -1.999290727599226,
-            "timestamp": 1.488840301981131
-        },
-        {
-            "x": 4.724997877507934,
-            "y": 4.607999081268276,
-            "heading": -0.5720536723911652,
-            "angularVelocity": 0.0000030383769981998487,
-            "velocityX": 3.2114639420784044,
-            "velocityY": -1.9992807200388194,
-            "timestamp": 1.5070289349714716
-        },
-        {
-            "x": 4.783409732751982,
-            "y": 4.571634442201932,
-            "heading": -0.5720536171272324,
-            "angularVelocity": 0.0000030383774810111486,
-            "velocityX": 3.2114483411183334,
-            "velocityY": -1.9993057799158553,
-            "timestamp": 1.5252175679618123
-        },
-        {
-            "x": 4.841821302805823,
-            "y": 4.535269345043953,
-            "heading": -0.5720535618632904,
-            "angularVelocity": 0.0000030383779810703436,
-            "velocityX": 3.211432661533899,
-            "velocityY": -1.9993309655158582,
-            "timestamp": 1.543406200952153
-        },
-        {
-            "x": 4.900232750957554,
-            "y": 4.4989040520829935,
-            "heading": -0.5720535065993444,
-            "angularVelocity": 0.0000030383782054284467,
-            "velocityX": 3.2114259594301156,
-            "velocityY": -1.9993417306445722,
-            "timestamp": 1.5615948339424937
-        },
-        {
-            "x": 4.958643009851236,
-            "y": 4.46253684895675,
-            "heading": -0.5720534513353597,
-            "angularVelocity": 0.000003038380333213315,
-            "velocityX": 3.2113605747448064,
-            "velocityY": -1.999446750371714,
-            "timestamp": 1.5797834669328343
-        },
-        {
-            "x": 5.017052204377269,
-            "y": 4.426167936377253,
-            "heading": -0.5720533960713398,
-            "angularVelocity": 0.0000030383822663214653,
-            "velocityX": 3.21130205645789,
-            "velocityY": -1.999540735073477,
-            "timestamp": 1.597972099923175
-        },
-        {
-            "x": 5.075461342766336,
-            "y": 4.389798933639636,
-            "heading": -0.572053340807318,
-            "angularVelocity": 0.0000030383823681408014,
-            "velocityX": 3.211298970081695,
-            "velocityY": -1.999545691911999,
-            "timestamp": 1.6161607329135157
-        },
-        {
-            "x": 5.133870231273224,
-            "y": 4.35342952959225,
-            "heading": -0.5720532855432875,
-            "angularVelocity": 0.0000030383828455618877,
-            "velocityX": 3.211285231711017,
-            "velocityY": -1.9995677556796618,
-            "timestamp": 1.6343493659038564
-        },
-        {
-            "x": 5.192278843825858,
-            "y": 4.317059682370623,
-            "heading": -0.5720532302792477,
-            "angularVelocity": 0.0000030383833608448334,
-            "velocityX": 3.2112700599133515,
-            "velocityY": -1.9995921211310104,
-            "timestamp": 1.652537998894197
-        },
-        {
-            "x": 5.250688378518623,
-            "y": 4.28069131610433,
-            "heading": -0.5720531750152431,
-            "angularVelocity": 0.000003038381418986995,
-            "velocityX": 3.211320758617258,
-            "velocityY": -1.9995106991068379,
-            "timestamp": 1.6707266318845377
-        },
-        {
-            "x": 5.309100997862405,
-            "y": 4.2443279043993805,
-            "heading": -0.5720531197513381,
-            "angularVelocity": 0.0000030383759466885435,
-            "velocityX": 3.2114903508569643,
-            "velocityY": -1.9992383003329772,
-            "timestamp": 1.6889152648748784
-        },
-        {
-            "x": 5.367519658270811,
-            "y": 4.20797419858894,
-            "heading": -0.5720530644876303,
-            "angularVelocity": 0.0000030383651093388606,
-            "velocityX": 3.2118224849224113,
-            "velocityY": -1.998704676145307,
-            "timestamp": 1.707103897865219
-        },
-        {
-            "x": 5.425947098435591,
-            "y": 4.171634605233085,
-            "heading": -0.5720530092241862,
-            "angularVelocity": 0.000003038350601996349,
-            "velocityX": 3.212305190598148,
-            "velocityY": -1.9979287819592548,
-            "timestamp": 1.7252925308555598
-        },
-        {
-            "x": 5.484385608043213,
-            "y": 4.135312815587752,
-            "heading": -0.5720529539612775,
-            "angularVelocity": 0.0000030383211709796797,
-            "velocityX": 3.2129137818460802,
-            "velocityY": -1.9969499447620875,
-            "timestamp": 1.7434811638459005
-        },
-        {
-            "x": 5.542805855367704,
-            "y": 4.098961660035809,
-            "heading": -0.5720528986974772,
-            "angularVelocity": 0.0000030383701899944643,
-            "velocityX": 3.21190973260657,
-            "velocityY": -1.9985644644794618,
-            "timestamp": 1.7616697968362411
-        },
-        {
-            "x": 5.601240344083477,
-            "y": 4.062633401947461,
-            "heading": -0.5720528434300965,
-            "angularVelocity": 0.000003038567036318681,
-            "velocityX": 3.2126927156544984,
-            "velocityY": -1.9973055758301874,
-            "timestamp": 1.7798584298265818
-        },
-        {
-            "x": 5.6606145093793865,
-            "y": 4.027862145195579,
-            "heading": -0.5720527881030576,
-            "angularVelocity": 0.0000030418470110587657,
-            "velocityX": 3.2643555635790893,
-            "velocityY": -1.9117025875630587,
-            "timestamp": 1.7980470628169225
-        },
-        {
-            "x": 5.721296662408543,
-            "y": 3.9954274711883437,
-            "heading": -0.5720527324898503,
-            "angularVelocity": 0.0000030575803747562297,
-            "velocityX": 3.3362679351103024,
-            "velocityY": -1.7832386867362542,
-            "timestamp": 1.8162356958072632
-        },
-        {
-            "x": 5.7832255363464355,
-            "y": 3.9654417037963863,
-            "heading": -0.5720526754836338,
-            "angularVelocity": 0.0000031341671777608764,
-            "velocityX": 3.404811893823314,
-            "velocityY": -1.6485992876907705,
-            "timestamp": 1.8344243287976039
-        },
-        {
-            "x": 5.946280542451374,
-            "y": 3.905964298639421,
-            "heading": -0.5720526220643124,
-            "angularVelocity": 0.0000011643081031480314,
-            "velocityX": 3.5538876198319724,
-            "velocityY": -1.296347894456755,
-            "timestamp": 1.880305072239023
-        },
-        {
-            "x": 6.113507668800577,
-            "y": 3.8594929620139826,
-            "heading": -0.5720525704992387,
-            "angularVelocity": 0.0000011238935971124524,
-            "velocityX": 3.6448216355273746,
-            "velocityY": -1.0128723542630858,
-            "timestamp": 1.9261858156804423
-        },
-        {
-            "x": 6.280741389533688,
-            "y": 3.813045361829368,
-            "heading": -0.5720525189366377,
-            "angularVelocity": 0.000001123839702331554,
-            "velocityX": 3.64496536431679,
-            "velocityY": -1.0123550034458102,
-            "timestamp": 1.9720665591218616
-        },
-        {
-            "x": 6.445214711488264,
-            "y": 3.7576105066241974,
-            "heading": -0.5720524661781627,
-            "angularVelocity": 0.000001149904532146113,
-            "velocityX": 3.584800716329628,
-            "velocityY": -1.2082379457532038,
-            "timestamp": 2.0179473025632806
-        },
-        {
-            "x": 6.603250853740864,
-            "y": 3.685853457646542,
-            "heading": -0.5720524095450499,
-            "angularVelocity": 0.0000012343547357711312,
-            "velocityX": 3.444498288357164,
-            "velocityY": -1.563990545825532,
-            "timestamp": 2.0638280460046996
-        },
-        {
-            "x": 6.753231641982998,
-            "y": 3.5985021065376834,
-            "heading": -0.5720523460454168,
-            "angularVelocity": 0.0000013840149106858422,
-            "velocityX": 3.268926721589291,
-            "velocityY": -1.9038782843733606,
-            "timestamp": 2.1097087894461186
-        },
-        {
-            "x": 6.893623224143754,
-            "y": 3.4964496816334383,
-            "heading": -0.5720522715269948,
-            "angularVelocity": 0.000001624176426799149,
-            "velocityX": 3.059923872852999,
-            "velocityY": -2.2242975429267773,
-            "timestamp": 2.1555895328875376
-        },
-        {
-            "x": 7.022989830955657,
-            "y": 3.3807398195880776,
-            "heading": -0.5720521794208858,
-            "angularVelocity": 0.000002007511259312441,
-            "velocityX": 2.819627519265176,
-            "velocityY": -2.521970076471132,
-            "timestamp": 2.2014702763289566
-        },
-        {
-            "x": 7.140008449554443,
-            "y": 3.2525558471679688,
-            "heading": -0.5720520666435536,
-            "angularVelocity": 0.0000024580537218711263,
-            "velocityX": 2.550495258387596,
-            "velocityY": -2.7938512501200163,
-            "timestamp": 2.2473510197703757
-        },
-        {
-            "x": 7.220333202972963,
-            "y": 3.149893277120657,
-            "heading": -0.57205195105401,
-            "angularVelocity": 0.0000033545158672128766,
-            "velocityX": 2.3310989161613462,
-            "velocityY": -2.9793630927244275,
-            "timestamp": 2.28180891122307
-        },
-        {
-            "x": 7.292634698776439,
-            "y": 3.0414305376048008,
-            "heading": -0.5720518635503085,
-            "angularVelocity": 0.000002539438655350069,
-            "velocityX": 2.098256531535021,
-            "velocityY": -3.1476893954673923,
-            "timestamp": 2.3162668026757647
-        },
-        {
-            "x": 7.356495877093559,
-            "y": 2.9277932908128785,
-            "heading": -0.5720517925210026,
-            "angularVelocity": 0.000002061336399152925,
-            "velocityX": 1.853310682252877,
-            "velocityY": -3.297858400531866,
-            "timestamp": 2.350724694128459
-        },
-        {
-            "x": 7.411548371625277,
-            "y": 2.8096370468188216,
-            "heading": -0.5720517318127031,
-            "angularVelocity": 0.0000017618112132015592,
-            "velocityX": 1.5976745009881768,
-            "velocityY": -3.4290038946879324,
-            "timestamp": 2.3851825855811537
-        },
-        {
-            "x": 7.457474639290446,
-            "y": 2.6876433834723,
-            "heading": -0.5720516777421675,
-            "angularVelocity": 0.0000015691771403552433,
-            "velocityX": 1.332822924707698,
-            "velocityY": -3.540369366882859,
-            "timestamp": 2.419640477033848
-        },
-        {
-            "x": 7.49400980759948,
-            "y": 2.5625160164724585,
-            "heading": -0.5720516278694773,
-            "angularVelocity": 0.0000014473517720699968,
-            "velocityX": 1.0602845028769212,
-            "velocityY": -3.631312356172488,
-            "timestamp": 2.4540983684865427
-        },
-        {
-            "x": 7.520944004354476,
-            "y": 2.4349769014207707,
-            "heading": -0.572051580412575,
-            "angularVelocity": 0.0000013772433620936625,
-            "velocityX": 0.7816553950223419,
-            "velocityY": -3.7013035236575447,
-            "timestamp": 2.4885562599392372
-        },
-        {
-            "x": 7.53814696549347,
-            "y": 2.3057649240296647,
-            "heading": -0.5720515339265989,
-            "angularVelocity": 0.0000013490661810968306,
-            "velocityX": 0.4992459031544392,
-            "velocityY": -3.7498515418008185,
-            "timestamp": 2.5230141513919317
-        },
-        {
-            "x": 7.550702118784975,
-            "y": 2.1760188446169613,
-            "heading": -0.5720514875103294,
-            "angularVelocity": 0.0000013470432314212435,
-            "velocityX": 0.3643622044850161,
-            "velocityY": -3.7653516783177263,
-            "timestamp": 2.5574720428446263
-        },
-        {
-            "x": 7.56324456343349,
-            "y": 2.046271536053824,
-            "heading": -0.5720514410938347,
-            "angularVelocity": 0.000001347049770461967,
-            "velocityX": 0.36399338786688684,
-            "velocityY": -3.765387349404451,
-            "timestamp": 2.5919299342973208
-        },
-        {
-            "x": 7.57578727298306,
-            "y": 1.916524253098162,
-            "heading": -0.5720513946773481,
-            "angularVelocity": 0.0000013470495296572014,
-            "velocityX": 0.3640010755350196,
-            "velocityY": -3.7653866062523553,
-            "timestamp": 2.6263878257500153
-        },
-        {
-            "x": 7.588311679081848,
-            "y": 1.7867752020286953,
-            "heading": -0.572051348260254,
-            "angularVelocity": 0.0000013470671654480904,
-            "velocityX": 0.3634698923895803,
-            "velocityY": -3.765437918557532,
-            "timestamp": 2.66084571720271
-        },
-        {
-            "x": 7.600787981664821,
-            "y": 1.657021516812979,
-            "heading": -0.5720513014102728,
-            "angularVelocity": 0.0000013596299477389166,
-            "velocityX": 0.3620738837176714,
-            "velocityY": -3.7655724057824624,
-            "timestamp": 2.6953036086554043
-        },
-        {
-            "x": 7.6103162713524775,
-            "y": 1.534453918276911,
-            "heading": -0.5536995765422551,
-            "angularVelocity": 0.5325840930573226,
-            "velocityX": 0.27651981261615693,
-            "velocityY": -3.557025498914872,
-            "timestamp": 2.729761500108099
-        },
-        {
-            "x": 7.620120451226168,
-            "y": 1.4200989858077522,
-            "heading": -0.5149948955625814,
-            "angularVelocity": 1.1232457747100673,
-            "velocityX": 0.2845264019455608,
-            "velocityY": -3.3186863051722235,
-            "timestamp": 2.7642193915607933
-        },
-        {
-            "x": 7.630164619508576,
-            "y": 1.3139569074919104,
-            "heading": -0.4555658179030021,
-            "angularVelocity": 1.7246870064917483,
-            "velocityX": 0.29149108836702503,
-            "velocityY": -3.080341652987332,
-            "timestamp": 2.798677283013488
-        },
-        {
-            "x": 7.6393124908526255,
-            "y": 1.2174783858270968,
-            "heading": -0.38913465703239974,
-            "angularVelocity": 1.9278939618746356,
-            "velocityX": 0.26547971911102464,
-            "velocityY": -2.799896267514583,
-            "timestamp": 2.8331351744661823
-        },
-        {
-            "x": 7.647521237661079,
-            "y": 1.1308545482085492,
-            "heading": -0.3222243620613333,
-            "angularVelocity": 1.9417988782892024,
-            "velocityX": 0.23822545322294675,
-            "velocityY": -2.5139041875930466,
-            "timestamp": 2.867593065918877
-        },
-        {
-            "x": 7.654790830767336,
-            "y": 1.0540553472510912,
-            "heading": -0.25810310965019956,
-            "angularVelocity": 1.8608582739068054,
-            "velocityX": 0.21097034089302885,
-            "velocityY": -2.2287841106847384,
-            "timestamp": 2.9020509573715714
-        },
-        {
-            "x": 7.661128119511832,
-            "y": 0.9870214726082436,
-            "heading": -0.19877373026472553,
-            "angularVelocity": 1.7217936700197296,
-            "velocityX": 0.18391400278139633,
-            "velocityY": -1.9453852750938303,
-            "timestamp": 2.936508848824266
-        },
-        {
-            "x": 7.6665422514065025,
-            "y": 0.9296939844226774,
-            "heading": -0.14563708266620207,
-            "angularVelocity": 1.5420748443496368,
-            "velocityX": 0.15712313395848782,
-            "velocityY": -1.6636969288812407,
-            "timestamp": 2.9709667402769604
-        },
-        {
-            "x": 7.6710416569080175,
-            "y": 0.8820195033603357,
-            "heading": -0.09974947152576126,
-            "angularVelocity": 1.331701076470322,
-            "velocityX": 0.1305769248148914,
-            "velocityY": -1.3835577004993256,
-            "timestamp": 3.005424631729655
-        },
-        {
-            "x": 7.674632846509551,
-            "y": 0.8439509935506372,
-            "heading": -0.06193916709248371,
-            "angularVelocity": 1.0972901370124863,
-            "velocityX": 0.10421965622732904,
-            "velocityY": -1.1047834967488865,
-            "timestamp": 3.0398825231823494
-        },
-        {
-            "x": 7.677320522590369,
-            "y": 0.8154475641909131,
-            "heading": -0.032869067302209315,
-            "angularVelocity": 0.8436412840348516,
-            "velocityX": 0.07799885505174348,
-            "velocityY": -0.8271959820541263,
-            "timestamp": 3.074340414635044
-        },
-        {
-            "x": 7.6791083601324726,
-            "y": 0.7964739655020694,
-            "heading": -0.013076341373692615,
-            "angularVelocity": 0.5744032816313593,
-            "velocityX": 0.05188470526556513,
-            "velocityY": -0.5506314486738934,
-            "timestamp": 3.1087983060877384
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -0.003,
-            "angularVelocity": 0.2924247813459689,
-            "velocityX": 0.02587621673680596,
-            "velocityY": -0.2749432743171821,
-            "timestamp": 3.143256197540433
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -0.003,
-            "angularVelocity": -5.23227119774542e-33,
-            "velocityX": 0,
-            "velocityY": -1.249481443135334e-33,
-            "timestamp": 3.1777140889931275
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.traj
deleted file mode 100644
index 00f6917b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.traj	
+++ /dev/null
@@ -1,859 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": 0,
-            "velocityX": 1.8950109818726043e-31,
-            "velocityY": 4.417214679587952e-30,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7239179438589063,
-            "y": 6.714864648059082,
-            "heading": 0.9709045474564667,
-            "angularVelocity": -1.3401351995297424,
-            "velocityX": 0.269944341976283,
-            "velocityY": -0.09960229862677873,
-            "timestamp": 0.051558568544241005
-        },
-        {
-            "x": 0.7525251092531191,
-            "y": 6.703796484721446,
-            "heading": 0.8397844996535714,
-            "angularVelocity": -2.5431281648227726,
-            "velocityX": 0.5548479370541143,
-            "velocityY": -0.21467165691630782,
-            "timestamp": 0.10311713708848201
-        },
-        {
-            "x": 0.8002738106410594,
-            "y": 6.689991944618376,
-            "heading": 0.6713194026020708,
-            "angularVelocity": -3.2674510136339485,
-            "velocityX": 0.9261060331222868,
-            "velocityY": -0.26774482870254956,
-            "timestamp": 0.154675705632723
-        },
-        {
-            "x": 0.8672232338097706,
-            "y": 6.675504894458598,
-            "heading": 0.46260124525274376,
-            "angularVelocity": -4.048175952947979,
-            "velocityX": 1.2985120622830493,
-            "velocityY": -0.28098239669505554,
-            "timestamp": 0.20623427417696402
-        },
-        {
-            "x": 0.954736838142347,
-            "y": 6.658878661036629,
-            "heading": 0.2244001490575843,
-            "angularVelocity": -4.620009882366156,
-            "velocityX": 1.6973629564117712,
-            "velocityY": -0.3224727507251158,
-            "timestamp": 0.257792842721205
-        },
-        {
-            "x": 1.0641109683124677,
-            "y": 6.639652990231364,
-            "heading": -0.004541406089891365,
-            "angularVelocity": -4.440417211175481,
-            "velocityX": 2.12135699765032,
-            "velocityY": -0.37288992592389836,
-            "timestamp": 0.309351411265446
-        },
-        {
-            "x": 1.1925189459621883,
-            "y": 6.613890706063681,
-            "heading": -0.19800120779802596,
-            "angularVelocity": -3.752233763861444,
-            "velocityX": 2.4905264299488024,
-            "velocityY": -0.49967027586341123,
-            "timestamp": 0.36090997980968703
-        },
-        {
-            "x": 1.3374709433458876,
-            "y": 6.583331183982878,
-            "heading": -0.33396197230189334,
-            "angularVelocity": -2.6370158897471825,
-            "velocityX": 2.811404611812854,
-            "velocityY": -0.5927147115125508,
-            "timestamp": 0.41246854835392804
-        },
-        {
-            "x": 1.49983426972339,
-            "y": 6.549745693102723,
-            "heading": -0.41661329783619183,
-            "angularVelocity": -1.6030570255902088,
-            "velocityX": 3.149104619501603,
-            "velocityY": -0.6514046419107491,
-            "timestamp": 0.46402711689816906
-        },
-        {
-            "x": 1.6800135166896315,
-            "y": 6.515230594671984,
-            "heading": -0.44907632730235497,
-            "angularVelocity": -0.6296340333474829,
-            "velocityX": 3.4946518503843107,
-            "velocityY": -0.6694347691375233,
-            "timestamp": 0.51558568544241
-        },
-        {
-            "x": 1.8735253740457867,
-            "y": 6.490840556482913,
-            "heading": -0.4490767643443874,
-            "angularVelocity": -0.00000847661300213044,
-            "velocityX": 3.7532434049278374,
-            "velocityY": -0.47305499119601163,
-            "timestamp": 0.567144253986651
-        },
-        {
-            "x": 2.067041889759694,
-            "y": 6.46648750315131,
-            "heading": -0.4490772002167485,
-            "angularVelocity": -0.000008453926735380078,
-            "velocityX": 3.753333755723225,
-            "velocityY": -0.47233765442907966,
-            "timestamp": 0.618702822530892
-        },
-        {
-            "x": 2.2605372007770526,
-            "y": 6.441966542289934,
-            "heading": -0.4490776373561405,
-            "angularVelocity": -0.0000084785013311663,
-            "velocityX": 3.7529224817654048,
-            "velocityY": -0.4755942911914249,
-            "timestamp": 0.670261391075133
-        },
-        {
-            "x": 2.45117838314973,
-            "y": 6.400765025555903,
-            "heading": -0.4490787415251664,
-            "angularVelocity": -0.000021415820050526827,
-            "velocityX": 3.6975654630332984,
-            "velocityY": -0.7991206485676197,
-            "timestamp": 0.7218199596193741
-        },
-        {
-            "x": 2.6301938207404594,
-            "y": 6.341459154774922,
-            "heading": -0.46856582254307677,
-            "angularVelocity": -0.377960086337202,
-            "velocityX": 3.472079280806918,
-            "velocityY": -1.150262166999653,
-            "timestamp": 0.7733785281636151
-        },
-        {
-            "x": 2.797205924987793,
-            "y": 6.264003276824951,
-            "heading": -0.5033519686488042,
-            "angularVelocity": -0.6746918521648854,
-            "velocityX": 3.239269610517634,
-            "velocityY": -1.502289146830184,
-            "timestamp": 0.8249370967078561
-        },
-        {
-            "x": 2.911639196984497,
-            "y": 6.1983709493948735,
-            "heading": -0.5311684488221697,
-            "angularVelocity": -0.7502107513814861,
-            "velocityX": 3.0862665022221747,
-            "velocityY": -1.770104534065962,
-            "timestamp": 0.8620153178901536
-        },
-        {
-            "x": 3.0203603627243916,
-            "y": 6.122823189814021,
-            "heading": -0.5565632404510816,
-            "angularVelocity": -0.6848977868761134,
-            "velocityX": 2.9322109387469486,
-            "velocityY": -2.037523839376905,
-            "timestamp": 0.8990935390724512
-        },
-        {
-            "x": 3.1248710366711436,
-            "y": 6.036966714195615,
-            "heading": -0.5720315951438852,
-            "angularVelocity": -0.41718168240112047,
-            "velocityX": 2.8186539325316726,
-            "velocityY": -2.3155500150894683,
-            "timestamp": 0.9361717602547487
-        },
-        {
-            "x": 3.2269084939172057,
-            "y": 5.940732357266897,
-            "heading": -0.5720474132070273,
-            "angularVelocity": -0.00042661332192557474,
-            "velocityX": 2.7519512531259087,
-            "velocityY": -2.5954415789079244,
-            "timestamp": 0.9732499814370462
-        },
-        {
-            "x": 3.3218654576085616,
-            "y": 5.837498151619579,
-            "heading": -0.5720480482162372,
-            "angularVelocity": -0.000017126204806487387,
-            "velocityX": 2.560990270400005,
-            "velocityY": -2.7842275695894796,
-            "timestamp": 1.0103282026193439
-        },
-        {
-            "x": 3.4166998907386708,
-            "y": 5.734151371991349,
-            "heading": -0.5720486788754773,
-            "angularVelocity": -0.00001700888607859636,
-            "velocityX": 2.557685619889607,
-            "velocityY": -2.7872636910247786,
-            "timestamp": 1.0474064238016414
-        },
-        {
-            "x": 3.5114576080251196,
-            "y": 5.630734246765037,
-            "heading": -0.5720493072137337,
-            "angularVelocity": -0.000016946289124090365,
-            "velocityX": 2.555616592803881,
-            "velocityY": -2.7891609125222083,
-            "timestamp": 1.084484644983939
-        },
-        {
-            "x": 3.6061908144763444,
-            "y": 5.527294668181728,
-            "heading": -0.572049934817026,
-            "angularVelocity": -0.00001692646713700802,
-            "velocityX": 2.5549555353779705,
-            "velocityY": -2.7897664797435726,
-            "timestamp": 1.1215628661662365
-        },
-        {
-            "x": 3.7009275327159292,
-            "y": 5.423858305931887,
-            "heading": -0.5720505625260676,
-            "angularVelocity": -0.00001692931919251171,
-            "velocityX": 2.555050248355605,
-            "velocityY": -2.789679735188215,
-            "timestamp": 1.158641087348534
-        },
-        {
-            "x": 3.7956729593070784,
-            "y": 5.320429920428362,
-            "heading": -0.5720511904967662,
-            "angularVelocity": -0.000016936376088998586,
-            "velocityX": 2.5552851126613465,
-            "velocityY": -2.7894646022814182,
-            "timestamp": 1.1957193085308315
-        },
-        {
-            "x": 3.890467288385621,
-            "y": 5.217046353888283,
-            "heading": -0.5720518199780259,
-            "angularVelocity": -0.000016977115933005807,
-            "velocityX": 2.556604013243175,
-            "velocityY": -2.7882558343819337,
-            "timestamp": 1.232797529713129
-        },
-        {
-            "x": 3.9877865474692196,
-            "y": 5.11603605874111,
-            "heading": -0.5720525488313646,
-            "angularVelocity": -0.00001965718190938232,
-            "velocityX": 2.624701400996587,
-            "velocityY": -2.7242486809308257,
-            "timestamp": 1.2698757508954266
-        },
-        {
-            "x": 4.092995643615723,
-            "y": 5.023272514343262,
-            "heading": -0.5720542830492642,
-            "angularVelocity": -0.000046771874277348444,
-            "velocityX": 2.8374903863181586,
-            "velocityY": -2.5018337298792988,
-            "timestamp": 1.3069539720777241
-        },
-        {
-            "x": 4.14638765114505,
-            "y": 4.979871763007162,
-            "heading": -0.5720542261628284,
-            "angularVelocity": 0.000003127581707131923,
-            "velocityX": 2.935460161172297,
-            "velocityY": -2.386146961079825,
-            "timestamp": 1.3251426050680648
-        },
-        {
-            "x": 4.201473544713913,
-            "y": 4.938642209546033,
-            "heading": -0.5720541700948967,
-            "angularVelocity": 0.000003082580845535541,
-            "velocityX": 3.0285889873152927,
-            "velocityY": -2.266775819987239,
-            "timestamp": 1.3433312380584055
-        },
-        {
-            "x": 4.258151834683336,
-            "y": 4.899630631332575,
-            "heading": -0.5720541145494444,
-            "angularVelocity": 0.0000030538552492495714,
-            "velocityX": 3.11613797471883,
-            "velocityY": -2.1448328873407987,
-            "timestamp": 1.3615198710487462
-        },
-        {
-            "x": 4.316201884310526,
-            "y": 4.862691173522813,
-            "heading": -0.5720540592428014,
-            "angularVelocity": 0.000003040725654248182,
-            "velocityX": 3.1915564879460585,
-            "velocityY": -2.030908965471825,
-            "timestamp": 1.3797085040390868
-        },
-        {
-            "x": 4.374575644994192,
-            "y": 4.826265415180517,
-            "heading": -0.5720540039764063,
-            "angularVelocity": 0.0000030385128512512867,
-            "velocityX": 3.209353925315247,
-            "velocityY": -2.002666080603007,
-            "timestamp": 1.3978971370294275
-        },
-        {
-            "x": 4.432961770826435,
-            "y": 4.789859479973715,
-            "heading": -0.572053948711591,
-            "angularVelocity": 0.000003038425994779569,
-            "velocityX": 3.210033753678121,
-            "velocityY": -2.0015762166465794,
-            "timestamp": 1.4160857700197682
-        },
-        {
-            "x": 4.491358693973173,
-            "y": 4.753470866702408,
-            "heading": -0.5720538934471577,
-            "angularVelocity": 0.000003038404992596871,
-            "velocityX": 3.21062738347468,
-            "velocityY": -2.000623867148186,
-            "timestamp": 1.434274403010109
-        },
-        {
-            "x": 4.549763643159385,
-            "y": 4.7170951368878224,
-            "heading": -0.5720538381830028,
-            "angularVelocity": 0.0000030383896855688938,
-            "velocityX": 3.2110686502526127,
-            "velocityY": -1.9999155425186408,
-            "timestamp": 1.4524630360004496
-        },
-        {
-            "x": 4.608173712818011,
-            "y": 4.680727629815019,
-            "heading": -0.5720537829190162,
-            "angularVelocity": 0.0000030383804361409647,
-            "velocityX": 3.211350170717194,
-            "velocityY": -1.9994634611677902,
-            "timestamp": 1.4706516689907903
-        },
-        {
-            "x": 4.666585738503753,
-            "y": 4.644363264529719,
-            "heading": -0.5720537276550893,
-            "angularVelocity": 0.0000030383771462609746,
-            "velocityX": 3.211457711899863,
-            "velocityY": -1.999290727599226,
-            "timestamp": 1.488840301981131
-        },
-        {
-            "x": 4.724997877507934,
-            "y": 4.607999081268276,
-            "heading": -0.5720536723911652,
-            "angularVelocity": 0.0000030383769981998487,
-            "velocityX": 3.2114639420784044,
-            "velocityY": -1.9992807200388194,
-            "timestamp": 1.5070289349714716
-        },
-        {
-            "x": 4.783409732751982,
-            "y": 4.571634442201932,
-            "heading": -0.5720536171272324,
-            "angularVelocity": 0.0000030383774810111486,
-            "velocityX": 3.2114483411183334,
-            "velocityY": -1.9993057799158553,
-            "timestamp": 1.5252175679618123
-        },
-        {
-            "x": 4.841821302805823,
-            "y": 4.535269345043953,
-            "heading": -0.5720535618632904,
-            "angularVelocity": 0.0000030383779810703436,
-            "velocityX": 3.211432661533899,
-            "velocityY": -1.9993309655158582,
-            "timestamp": 1.543406200952153
-        },
-        {
-            "x": 4.900232750957554,
-            "y": 4.4989040520829935,
-            "heading": -0.5720535065993444,
-            "angularVelocity": 0.0000030383782054284467,
-            "velocityX": 3.2114259594301156,
-            "velocityY": -1.9993417306445722,
-            "timestamp": 1.5615948339424937
-        },
-        {
-            "x": 4.958643009851236,
-            "y": 4.46253684895675,
-            "heading": -0.5720534513353597,
-            "angularVelocity": 0.000003038380333213315,
-            "velocityX": 3.2113605747448064,
-            "velocityY": -1.999446750371714,
-            "timestamp": 1.5797834669328343
-        },
-        {
-            "x": 5.017052204377269,
-            "y": 4.426167936377253,
-            "heading": -0.5720533960713398,
-            "angularVelocity": 0.0000030383822663214653,
-            "velocityX": 3.21130205645789,
-            "velocityY": -1.999540735073477,
-            "timestamp": 1.597972099923175
-        },
-        {
-            "x": 5.075461342766336,
-            "y": 4.389798933639636,
-            "heading": -0.572053340807318,
-            "angularVelocity": 0.0000030383823681408014,
-            "velocityX": 3.211298970081695,
-            "velocityY": -1.999545691911999,
-            "timestamp": 1.6161607329135157
-        },
-        {
-            "x": 5.133870231273224,
-            "y": 4.35342952959225,
-            "heading": -0.5720532855432875,
-            "angularVelocity": 0.0000030383828455618877,
-            "velocityX": 3.211285231711017,
-            "velocityY": -1.9995677556796618,
-            "timestamp": 1.6343493659038564
-        },
-        {
-            "x": 5.192278843825858,
-            "y": 4.317059682370623,
-            "heading": -0.5720532302792477,
-            "angularVelocity": 0.0000030383833608448334,
-            "velocityX": 3.2112700599133515,
-            "velocityY": -1.9995921211310104,
-            "timestamp": 1.652537998894197
-        },
-        {
-            "x": 5.250688378518623,
-            "y": 4.28069131610433,
-            "heading": -0.5720531750152431,
-            "angularVelocity": 0.000003038381418986995,
-            "velocityX": 3.211320758617258,
-            "velocityY": -1.9995106991068379,
-            "timestamp": 1.6707266318845377
-        },
-        {
-            "x": 5.309100997862405,
-            "y": 4.2443279043993805,
-            "heading": -0.5720531197513381,
-            "angularVelocity": 0.0000030383759466885435,
-            "velocityX": 3.2114903508569643,
-            "velocityY": -1.9992383003329772,
-            "timestamp": 1.6889152648748784
-        },
-        {
-            "x": 5.367519658270811,
-            "y": 4.20797419858894,
-            "heading": -0.5720530644876303,
-            "angularVelocity": 0.0000030383651093388606,
-            "velocityX": 3.2118224849224113,
-            "velocityY": -1.998704676145307,
-            "timestamp": 1.707103897865219
-        },
-        {
-            "x": 5.425947098435591,
-            "y": 4.171634605233085,
-            "heading": -0.5720530092241862,
-            "angularVelocity": 0.000003038350601996349,
-            "velocityX": 3.212305190598148,
-            "velocityY": -1.9979287819592548,
-            "timestamp": 1.7252925308555598
-        },
-        {
-            "x": 5.484385608043213,
-            "y": 4.135312815587752,
-            "heading": -0.5720529539612775,
-            "angularVelocity": 0.0000030383211709796797,
-            "velocityX": 3.2129137818460802,
-            "velocityY": -1.9969499447620875,
-            "timestamp": 1.7434811638459005
-        },
-        {
-            "x": 5.542805855367704,
-            "y": 4.098961660035809,
-            "heading": -0.5720528986974772,
-            "angularVelocity": 0.0000030383701899944643,
-            "velocityX": 3.21190973260657,
-            "velocityY": -1.9985644644794618,
-            "timestamp": 1.7616697968362411
-        },
-        {
-            "x": 5.601240344083477,
-            "y": 4.062633401947461,
-            "heading": -0.5720528434300965,
-            "angularVelocity": 0.000003038567036318681,
-            "velocityX": 3.2126927156544984,
-            "velocityY": -1.9973055758301874,
-            "timestamp": 1.7798584298265818
-        },
-        {
-            "x": 5.6606145093793865,
-            "y": 4.027862145195579,
-            "heading": -0.5720527881030576,
-            "angularVelocity": 0.0000030418470110587657,
-            "velocityX": 3.2643555635790893,
-            "velocityY": -1.9117025875630587,
-            "timestamp": 1.7980470628169225
-        },
-        {
-            "x": 5.721296662408543,
-            "y": 3.9954274711883437,
-            "heading": -0.5720527324898503,
-            "angularVelocity": 0.0000030575803747562297,
-            "velocityX": 3.3362679351103024,
-            "velocityY": -1.7832386867362542,
-            "timestamp": 1.8162356958072632
-        },
-        {
-            "x": 5.7832255363464355,
-            "y": 3.9654417037963863,
-            "heading": -0.5720526754836338,
-            "angularVelocity": 0.0000031341671777608764,
-            "velocityX": 3.404811893823314,
-            "velocityY": -1.6485992876907705,
-            "timestamp": 1.8344243287976039
-        },
-        {
-            "x": 5.946280542451374,
-            "y": 3.905964298639421,
-            "heading": -0.5720526220643124,
-            "angularVelocity": 0.0000011643081031480314,
-            "velocityX": 3.5538876198319724,
-            "velocityY": -1.296347894456755,
-            "timestamp": 1.880305072239023
-        },
-        {
-            "x": 6.113507668800577,
-            "y": 3.8594929620139826,
-            "heading": -0.5720525704992387,
-            "angularVelocity": 0.0000011238935971124524,
-            "velocityX": 3.6448216355273746,
-            "velocityY": -1.0128723542630858,
-            "timestamp": 1.9261858156804423
-        },
-        {
-            "x": 6.280741389533688,
-            "y": 3.813045361829368,
-            "heading": -0.5720525189366377,
-            "angularVelocity": 0.000001123839702331554,
-            "velocityX": 3.64496536431679,
-            "velocityY": -1.0123550034458102,
-            "timestamp": 1.9720665591218616
-        },
-        {
-            "x": 6.445214711488264,
-            "y": 3.7576105066241974,
-            "heading": -0.5720524661781627,
-            "angularVelocity": 0.000001149904532146113,
-            "velocityX": 3.584800716329628,
-            "velocityY": -1.2082379457532038,
-            "timestamp": 2.0179473025632806
-        },
-        {
-            "x": 6.603250853740864,
-            "y": 3.685853457646542,
-            "heading": -0.5720524095450499,
-            "angularVelocity": 0.0000012343547357711312,
-            "velocityX": 3.444498288357164,
-            "velocityY": -1.563990545825532,
-            "timestamp": 2.0638280460046996
-        },
-        {
-            "x": 6.753231641982998,
-            "y": 3.5985021065376834,
-            "heading": -0.5720523460454168,
-            "angularVelocity": 0.0000013840149106858422,
-            "velocityX": 3.268926721589291,
-            "velocityY": -1.9038782843733606,
-            "timestamp": 2.1097087894461186
-        },
-        {
-            "x": 6.893623224143754,
-            "y": 3.4964496816334383,
-            "heading": -0.5720522715269948,
-            "angularVelocity": 0.000001624176426799149,
-            "velocityX": 3.059923872852999,
-            "velocityY": -2.2242975429267773,
-            "timestamp": 2.1555895328875376
-        },
-        {
-            "x": 7.022989830955657,
-            "y": 3.3807398195880776,
-            "heading": -0.5720521794208858,
-            "angularVelocity": 0.000002007511259312441,
-            "velocityX": 2.819627519265176,
-            "velocityY": -2.521970076471132,
-            "timestamp": 2.2014702763289566
-        },
-        {
-            "x": 7.140008449554443,
-            "y": 3.2525558471679688,
-            "heading": -0.5720520666435536,
-            "angularVelocity": 0.0000024580537218711263,
-            "velocityX": 2.550495258387596,
-            "velocityY": -2.7938512501200163,
-            "timestamp": 2.2473510197703757
-        },
-        {
-            "x": 7.220333202972963,
-            "y": 3.149893277120657,
-            "heading": -0.57205195105401,
-            "angularVelocity": 0.0000033545158672128766,
-            "velocityX": 2.3310989161613462,
-            "velocityY": -2.9793630927244275,
-            "timestamp": 2.28180891122307
-        },
-        {
-            "x": 7.292634698776439,
-            "y": 3.0414305376048008,
-            "heading": -0.5720518635503085,
-            "angularVelocity": 0.000002539438655350069,
-            "velocityX": 2.098256531535021,
-            "velocityY": -3.1476893954673923,
-            "timestamp": 2.3162668026757647
-        },
-        {
-            "x": 7.356495877093559,
-            "y": 2.9277932908128785,
-            "heading": -0.5720517925210026,
-            "angularVelocity": 0.000002061336399152925,
-            "velocityX": 1.853310682252877,
-            "velocityY": -3.297858400531866,
-            "timestamp": 2.350724694128459
-        },
-        {
-            "x": 7.411548371625277,
-            "y": 2.8096370468188216,
-            "heading": -0.5720517318127031,
-            "angularVelocity": 0.0000017618112132015592,
-            "velocityX": 1.5976745009881768,
-            "velocityY": -3.4290038946879324,
-            "timestamp": 2.3851825855811537
-        },
-        {
-            "x": 7.457474639290446,
-            "y": 2.6876433834723,
-            "heading": -0.5720516777421675,
-            "angularVelocity": 0.0000015691771403552433,
-            "velocityX": 1.332822924707698,
-            "velocityY": -3.540369366882859,
-            "timestamp": 2.419640477033848
-        },
-        {
-            "x": 7.49400980759948,
-            "y": 2.5625160164724585,
-            "heading": -0.5720516278694773,
-            "angularVelocity": 0.0000014473517720699968,
-            "velocityX": 1.0602845028769212,
-            "velocityY": -3.631312356172488,
-            "timestamp": 2.4540983684865427
-        },
-        {
-            "x": 7.520944004354476,
-            "y": 2.4349769014207707,
-            "heading": -0.572051580412575,
-            "angularVelocity": 0.0000013772433620936625,
-            "velocityX": 0.7816553950223419,
-            "velocityY": -3.7013035236575447,
-            "timestamp": 2.4885562599392372
-        },
-        {
-            "x": 7.53814696549347,
-            "y": 2.3057649240296647,
-            "heading": -0.5720515339265989,
-            "angularVelocity": 0.0000013490661810968306,
-            "velocityX": 0.4992459031544392,
-            "velocityY": -3.7498515418008185,
-            "timestamp": 2.5230141513919317
-        },
-        {
-            "x": 7.550702118784975,
-            "y": 2.1760188446169613,
-            "heading": -0.5720514875103294,
-            "angularVelocity": 0.0000013470432314212435,
-            "velocityX": 0.3643622044850161,
-            "velocityY": -3.7653516783177263,
-            "timestamp": 2.5574720428446263
-        },
-        {
-            "x": 7.56324456343349,
-            "y": 2.046271536053824,
-            "heading": -0.5720514410938347,
-            "angularVelocity": 0.000001347049770461967,
-            "velocityX": 0.36399338786688684,
-            "velocityY": -3.765387349404451,
-            "timestamp": 2.5919299342973208
-        },
-        {
-            "x": 7.57578727298306,
-            "y": 1.916524253098162,
-            "heading": -0.5720513946773481,
-            "angularVelocity": 0.0000013470495296572014,
-            "velocityX": 0.3640010755350196,
-            "velocityY": -3.7653866062523553,
-            "timestamp": 2.6263878257500153
-        },
-        {
-            "x": 7.588311679081848,
-            "y": 1.7867752020286953,
-            "heading": -0.572051348260254,
-            "angularVelocity": 0.0000013470671654480904,
-            "velocityX": 0.3634698923895803,
-            "velocityY": -3.765437918557532,
-            "timestamp": 2.66084571720271
-        },
-        {
-            "x": 7.600787981664821,
-            "y": 1.657021516812979,
-            "heading": -0.5720513014102728,
-            "angularVelocity": 0.0000013596299477389166,
-            "velocityX": 0.3620738837176714,
-            "velocityY": -3.7655724057824624,
-            "timestamp": 2.6953036086554043
-        },
-        {
-            "x": 7.6103162713524775,
-            "y": 1.534453918276911,
-            "heading": -0.5536995765422551,
-            "angularVelocity": 0.5325840930573226,
-            "velocityX": 0.27651981261615693,
-            "velocityY": -3.557025498914872,
-            "timestamp": 2.729761500108099
-        },
-        {
-            "x": 7.620120451226168,
-            "y": 1.4200989858077522,
-            "heading": -0.5149948955625814,
-            "angularVelocity": 1.1232457747100673,
-            "velocityX": 0.2845264019455608,
-            "velocityY": -3.3186863051722235,
-            "timestamp": 2.7642193915607933
-        },
-        {
-            "x": 7.630164619508576,
-            "y": 1.3139569074919104,
-            "heading": -0.4555658179030021,
-            "angularVelocity": 1.7246870064917483,
-            "velocityX": 0.29149108836702503,
-            "velocityY": -3.080341652987332,
-            "timestamp": 2.798677283013488
-        },
-        {
-            "x": 7.6393124908526255,
-            "y": 1.2174783858270968,
-            "heading": -0.38913465703239974,
-            "angularVelocity": 1.9278939618746356,
-            "velocityX": 0.26547971911102464,
-            "velocityY": -2.799896267514583,
-            "timestamp": 2.8331351744661823
-        },
-        {
-            "x": 7.647521237661079,
-            "y": 1.1308545482085492,
-            "heading": -0.3222243620613333,
-            "angularVelocity": 1.9417988782892024,
-            "velocityX": 0.23822545322294675,
-            "velocityY": -2.5139041875930466,
-            "timestamp": 2.867593065918877
-        },
-        {
-            "x": 7.654790830767336,
-            "y": 1.0540553472510912,
-            "heading": -0.25810310965019956,
-            "angularVelocity": 1.8608582739068054,
-            "velocityX": 0.21097034089302885,
-            "velocityY": -2.2287841106847384,
-            "timestamp": 2.9020509573715714
-        },
-        {
-            "x": 7.661128119511832,
-            "y": 0.9870214726082436,
-            "heading": -0.19877373026472553,
-            "angularVelocity": 1.7217936700197296,
-            "velocityX": 0.18391400278139633,
-            "velocityY": -1.9453852750938303,
-            "timestamp": 2.936508848824266
-        },
-        {
-            "x": 7.6665422514065025,
-            "y": 0.9296939844226774,
-            "heading": -0.14563708266620207,
-            "angularVelocity": 1.5420748443496368,
-            "velocityX": 0.15712313395848782,
-            "velocityY": -1.6636969288812407,
-            "timestamp": 2.9709667402769604
-        },
-        {
-            "x": 7.6710416569080175,
-            "y": 0.8820195033603357,
-            "heading": -0.09974947152576126,
-            "angularVelocity": 1.331701076470322,
-            "velocityX": 0.1305769248148914,
-            "velocityY": -1.3835577004993256,
-            "timestamp": 3.005424631729655
-        },
-        {
-            "x": 7.674632846509551,
-            "y": 0.8439509935506372,
-            "heading": -0.06193916709248371,
-            "angularVelocity": 1.0972901370124863,
-            "velocityX": 0.10421965622732904,
-            "velocityY": -1.1047834967488865,
-            "timestamp": 3.0398825231823494
-        },
-        {
-            "x": 7.677320522590369,
-            "y": 0.8154475641909131,
-            "heading": -0.032869067302209315,
-            "angularVelocity": 0.8436412840348516,
-            "velocityX": 0.07799885505174348,
-            "velocityY": -0.8271959820541263,
-            "timestamp": 3.074340414635044
-        },
-        {
-            "x": 7.6791083601324726,
-            "y": 0.7964739655020694,
-            "heading": -0.013076341373692615,
-            "angularVelocity": 0.5744032816313593,
-            "velocityX": 0.05188470526556513,
-            "velocityY": -0.5506314486738934,
-            "timestamp": 3.1087983060877384
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -0.003,
-            "angularVelocity": 0.2924247813459689,
-            "velocityX": 0.02587621673680596,
-            "velocityY": -0.2749432743171821,
-            "timestamp": 3.143256197540433
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -0.003,
-            "angularVelocity": -5.23227119774542e-33,
-            "velocityX": 0,
-            "velocityY": -1.249481443135334e-33,
-            "timestamp": 3.1777140889931275
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj
deleted file mode 100644
index 59066a5b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.1.traj	
+++ /dev/null
@@ -1,121 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": -3.0520704892108665e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7541195079597249,
-            "y": 6.720987197392724,
-            "heading": 1.0162961519009226,
-            "angularVelocity": -0.3240021115694771,
-            "velocityX": 0.6030587810307456,
-            "velocityY": 0.013493760103527496,
-            "timestamp": 0.07315954820244353
-        },
-        {
-            "x": 0.8424136303815098,
-            "y": 6.722972492793459,
-            "heading": 0.9702543819568796,
-            "angularVelocity": -0.6293337107086672,
-            "velocityX": 1.2068707993857855,
-            "velocityY": 0.027136518055622637,
-            "timestamp": 0.14631909640488705
-        },
-        {
-            "x": 0.9749626751701804,
-            "y": 6.7259693430054925,
-            "heading": 0.9040114694947134,
-            "angularVelocity": -0.9054581949963141,
-            "velocityX": 1.8117805268819878,
-            "velocityY": 0.0409632137659495,
-            "timestamp": 0.21947864460733058
-        },
-        {
-            "x": 1.1518931657803464,
-            "y": 6.729992727527654,
-            "heading": 0.8214709979365993,
-            "angularVelocity": -1.1282255506783914,
-            "velocityX": 2.4184196725842533,
-            "velocityY": 0.0549946605880228,
-            "timestamp": 0.2926381928097741
-        },
-        {
-            "x": 1.3734034446931793,
-            "y": 6.735064446892539,
-            "heading": 0.7325177816386138,
-            "angularVelocity": -1.215879792639604,
-            "velocityX": 3.0277699132296103,
-            "velocityY": 0.06932409356680827,
-            "timestamp": 0.3657977410122176
-        },
-        {
-            "x": 1.6366130282089124,
-            "y": 6.741790657706068,
-            "heading": 0.6993734786052862,
-            "angularVelocity": -0.4530413848603092,
-            "velocityX": 3.597747525550208,
-            "velocityY": 0.09193893317775545,
-            "timestamp": 0.43895728921466115
-        },
-        {
-            "x": 1.858120509684905,
-            "y": 6.746838490569188,
-            "heading": 0.6098796286126051,
-            "angularVelocity": -1.2232695825988174,
-            "velocityX": 3.0277316757485147,
-            "velocityY": 0.0689975948068392,
-            "timestamp": 0.5121168374171047
-        },
-        {
-            "x": 2.035048195511774,
-            "y": 6.750890505964207,
-            "heading": 0.5269052635268205,
-            "angularVelocity": -1.1341563353588457,
-            "velocityX": 2.418381334686234,
-            "velocityY": 0.0553860089978706,
-            "timestamp": 0.5852763856195482
-        },
-        {
-            "x": 2.1675926440263233,
-            "y": 6.753939666061267,
-            "heading": 0.4602292179018529,
-            "angularVelocity": -0.9113785864349585,
-            "velocityX": 1.811717701533345,
-            "velocityY": 0.041678224811154865,
-            "timestamp": 0.6584359338219917
-        },
-        {
-            "x": 2.2558825150080364,
-            "y": 6.755978237986949,
-            "heading": 0.41386063697230147,
-            "angularVelocity": -0.6338008102679221,
-            "velocityX": 1.206812687489562,
-            "velocityY": 0.027864741865846625,
-            "timestamp": 0.7315954820244353
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": -0.3261452203924221,
-            "velocityX": 0.6030311295783809,
-            "velocityY": 0.013966215458624026,
-            "timestamp": 0.8047550302268788
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 9.371559495672325e-33,
-            "velocityX": 2.5226214414524116e-33,
-            "velocityY": -5.278780681595662e-33,
-            "timestamp": 0.8779145784293223
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.traj
deleted file mode 100644
index 59066a5b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.traj	
+++ /dev/null
@@ -1,121 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": -3.0520704892108665e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7541195079597249,
-            "y": 6.720987197392724,
-            "heading": 1.0162961519009226,
-            "angularVelocity": -0.3240021115694771,
-            "velocityX": 0.6030587810307456,
-            "velocityY": 0.013493760103527496,
-            "timestamp": 0.07315954820244353
-        },
-        {
-            "x": 0.8424136303815098,
-            "y": 6.722972492793459,
-            "heading": 0.9702543819568796,
-            "angularVelocity": -0.6293337107086672,
-            "velocityX": 1.2068707993857855,
-            "velocityY": 0.027136518055622637,
-            "timestamp": 0.14631909640488705
-        },
-        {
-            "x": 0.9749626751701804,
-            "y": 6.7259693430054925,
-            "heading": 0.9040114694947134,
-            "angularVelocity": -0.9054581949963141,
-            "velocityX": 1.8117805268819878,
-            "velocityY": 0.0409632137659495,
-            "timestamp": 0.21947864460733058
-        },
-        {
-            "x": 1.1518931657803464,
-            "y": 6.729992727527654,
-            "heading": 0.8214709979365993,
-            "angularVelocity": -1.1282255506783914,
-            "velocityX": 2.4184196725842533,
-            "velocityY": 0.0549946605880228,
-            "timestamp": 0.2926381928097741
-        },
-        {
-            "x": 1.3734034446931793,
-            "y": 6.735064446892539,
-            "heading": 0.7325177816386138,
-            "angularVelocity": -1.215879792639604,
-            "velocityX": 3.0277699132296103,
-            "velocityY": 0.06932409356680827,
-            "timestamp": 0.3657977410122176
-        },
-        {
-            "x": 1.6366130282089124,
-            "y": 6.741790657706068,
-            "heading": 0.6993734786052862,
-            "angularVelocity": -0.4530413848603092,
-            "velocityX": 3.597747525550208,
-            "velocityY": 0.09193893317775545,
-            "timestamp": 0.43895728921466115
-        },
-        {
-            "x": 1.858120509684905,
-            "y": 6.746838490569188,
-            "heading": 0.6098796286126051,
-            "angularVelocity": -1.2232695825988174,
-            "velocityX": 3.0277316757485147,
-            "velocityY": 0.0689975948068392,
-            "timestamp": 0.5121168374171047
-        },
-        {
-            "x": 2.035048195511774,
-            "y": 6.750890505964207,
-            "heading": 0.5269052635268205,
-            "angularVelocity": -1.1341563353588457,
-            "velocityX": 2.418381334686234,
-            "velocityY": 0.0553860089978706,
-            "timestamp": 0.5852763856195482
-        },
-        {
-            "x": 2.1675926440263233,
-            "y": 6.753939666061267,
-            "heading": 0.4602292179018529,
-            "angularVelocity": -0.9113785864349585,
-            "velocityX": 1.811717701533345,
-            "velocityY": 0.041678224811154865,
-            "timestamp": 0.6584359338219917
-        },
-        {
-            "x": 2.2558825150080364,
-            "y": 6.755978237986949,
-            "heading": 0.41386063697230147,
-            "angularVelocity": -0.6338008102679221,
-            "velocityX": 1.206812687489562,
-            "velocityY": 0.027864741865846625,
-            "timestamp": 0.7315954820244353
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": -0.3261452203924221,
-            "velocityX": 0.6030311295783809,
-            "velocityY": 0.013966215458624026,
-            "timestamp": 0.8047550302268788
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 9.371559495672325e-33,
-            "velocityX": 2.5226214414524116e-33,
-            "velocityY": -5.278780681595662e-33,
-            "timestamp": 0.8779145784293223
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.1.traj
deleted file mode 100644
index fb2e2531..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": 3.547724392998225e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.737435339105297,
-            "y": 6.6995140898970185,
-            "heading": 1.00766177496316,
-            "angularVelocity": -0.49661186995303425,
-            "velocityX": 0.4213191985755434,
-            "velocityY": -0.3145981609176562,
-            "timestamp": 0.06511770457661178
-        },
-        {
-            "x": 0.7924238483684921,
-            "y": 6.658398285939555,
-            "heading": 0.945783535120445,
-            "angularVelocity": -0.95025216636614,
-            "velocityX": 0.8444479058456402,
-            "velocityY": -0.6314074524707902,
-            "timestamp": 0.13023540915322357
-        },
-        {
-            "x": 0.8751312092590968,
-            "y": 6.596482141852294,
-            "heading": 0.8584519121155169,
-            "angularVelocity": -1.3411348506945964,
-            "velocityX": 1.2701209514118896,
-            "velocityY": -0.9508342545216032,
-            "timestamp": 0.19535311372983535
-        },
-        {
-            "x": 0.9857818913354864,
-            "y": 6.513571279138628,
-            "heading": 0.7519056782658311,
-            "angularVelocity": -1.6362099146835547,
-            "velocityX": 1.6992411325894867,
-            "velocityY": -1.273246089565702,
-            "timestamp": 0.26047081830644714
-        },
-        {
-            "x": 1.1246151480908613,
-            "y": 6.409448227792029,
-            "heading": 0.6367147770436085,
-            "angularVelocity": -1.7689644002530829,
-            "velocityX": 2.132035483407367,
-            "velocityY": -1.5989975694566836,
-            "timestamp": 0.32558852288305895
-        },
-        {
-            "x": 1.2913898404503776,
-            "y": 6.284148794801154,
-            "heading": 0.5378900995711877,
-            "angularVelocity": -1.5176314662036392,
-            "velocityX": 2.5611267080722016,
-            "velocityY": -1.9241991683453519,
-            "timestamp": 0.39070622745967076
-        },
-        {
-            "x": 1.479902092849603,
-            "y": 6.139172935751937,
-            "heading": 0.5154398140678585,
-            "angularVelocity": -0.3447646941687853,
-            "velocityX": 2.8949462150871557,
-            "velocityY": -2.2263662392867607,
-            "timestamp": 0.4558239320362826
-        },
-        {
-            "x": 1.6682099030328927,
-            "y": 5.994133301373275,
-            "heading": 0.4928532472983225,
-            "angularVelocity": -0.34685753922672635,
-            "velocityX": 2.8918066355017875,
-            "velocityY": -2.2273456246913312,
-            "timestamp": 0.5209416366128944
-        },
-        {
-            "x": 1.8350205910276742,
-            "y": 5.8693381796986746,
-            "heading": 0.39723540571375665,
-            "angularVelocity": -1.4683847074503305,
-            "velocityX": 2.561679486083952,
-            "velocityY": -1.9164545569596636,
-            "timestamp": 0.5860593411895062
-        },
-        {
-            "x": 1.9739536597530634,
-            "y": 5.765582600106914,
-            "heading": 0.2820330722411117,
-            "angularVelocity": -1.7691399631126123,
-            "velocityX": 2.1335682765342634,
-            "velocityY": -1.5933543767607987,
-            "timestamp": 0.651177045766118
-        },
-        {
-            "x": 2.08468090414567,
-            "y": 5.683000750382208,
-            "heading": 0.1756895135649168,
-            "angularVelocity": -1.6330974712273538,
-            "velocityX": 1.7004168852778658,
-            "velocityY": -1.2681935007022147,
-            "timestamp": 0.7162947503427298
-        },
-        {
-            "x": 2.167467858188685,
-            "y": 5.6213477608369935,
-            "heading": 0.08927498136102419,
-            "angularVelocity": -1.3270512645639152,
-            "velocityX": 1.2713432480657338,
-            "velocityY": -0.946793041094972,
-            "timestamp": 0.7814124549193416
-        },
-        {
-            "x": 2.2225251033432207,
-            "y": 5.5804064125777195,
-            "heading": 0.028526912579098198,
-            "angularVelocity": -0.9328963478811614,
-            "velocityX": 0.8455034696402836,
-            "velocityY": -0.628728370041152,
-            "timestamp": 0.8465301594959534
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": -0.4841527013902356,
-            "velocityX": 0.42192667624600166,
-            "velocityY": -0.31337733279145685,
-            "timestamp": 0.9116478640725653
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": -3.2592320489843035e-28,
-            "velocityX": -3.6408651267401044e-33,
-            "velocityY": 2.9208085569078165e-32,
-            "timestamp": 0.9767655686491771
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.traj
deleted file mode 100644
index fb2e2531..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": 3.547724392998225e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.737435339105297,
-            "y": 6.6995140898970185,
-            "heading": 1.00766177496316,
-            "angularVelocity": -0.49661186995303425,
-            "velocityX": 0.4213191985755434,
-            "velocityY": -0.3145981609176562,
-            "timestamp": 0.06511770457661178
-        },
-        {
-            "x": 0.7924238483684921,
-            "y": 6.658398285939555,
-            "heading": 0.945783535120445,
-            "angularVelocity": -0.95025216636614,
-            "velocityX": 0.8444479058456402,
-            "velocityY": -0.6314074524707902,
-            "timestamp": 0.13023540915322357
-        },
-        {
-            "x": 0.8751312092590968,
-            "y": 6.596482141852294,
-            "heading": 0.8584519121155169,
-            "angularVelocity": -1.3411348506945964,
-            "velocityX": 1.2701209514118896,
-            "velocityY": -0.9508342545216032,
-            "timestamp": 0.19535311372983535
-        },
-        {
-            "x": 0.9857818913354864,
-            "y": 6.513571279138628,
-            "heading": 0.7519056782658311,
-            "angularVelocity": -1.6362099146835547,
-            "velocityX": 1.6992411325894867,
-            "velocityY": -1.273246089565702,
-            "timestamp": 0.26047081830644714
-        },
-        {
-            "x": 1.1246151480908613,
-            "y": 6.409448227792029,
-            "heading": 0.6367147770436085,
-            "angularVelocity": -1.7689644002530829,
-            "velocityX": 2.132035483407367,
-            "velocityY": -1.5989975694566836,
-            "timestamp": 0.32558852288305895
-        },
-        {
-            "x": 1.2913898404503776,
-            "y": 6.284148794801154,
-            "heading": 0.5378900995711877,
-            "angularVelocity": -1.5176314662036392,
-            "velocityX": 2.5611267080722016,
-            "velocityY": -1.9241991683453519,
-            "timestamp": 0.39070622745967076
-        },
-        {
-            "x": 1.479902092849603,
-            "y": 6.139172935751937,
-            "heading": 0.5154398140678585,
-            "angularVelocity": -0.3447646941687853,
-            "velocityX": 2.8949462150871557,
-            "velocityY": -2.2263662392867607,
-            "timestamp": 0.4558239320362826
-        },
-        {
-            "x": 1.6682099030328927,
-            "y": 5.994133301373275,
-            "heading": 0.4928532472983225,
-            "angularVelocity": -0.34685753922672635,
-            "velocityX": 2.8918066355017875,
-            "velocityY": -2.2273456246913312,
-            "timestamp": 0.5209416366128944
-        },
-        {
-            "x": 1.8350205910276742,
-            "y": 5.8693381796986746,
-            "heading": 0.39723540571375665,
-            "angularVelocity": -1.4683847074503305,
-            "velocityX": 2.561679486083952,
-            "velocityY": -1.9164545569596636,
-            "timestamp": 0.5860593411895062
-        },
-        {
-            "x": 1.9739536597530634,
-            "y": 5.765582600106914,
-            "heading": 0.2820330722411117,
-            "angularVelocity": -1.7691399631126123,
-            "velocityX": 2.1335682765342634,
-            "velocityY": -1.5933543767607987,
-            "timestamp": 0.651177045766118
-        },
-        {
-            "x": 2.08468090414567,
-            "y": 5.683000750382208,
-            "heading": 0.1756895135649168,
-            "angularVelocity": -1.6330974712273538,
-            "velocityX": 1.7004168852778658,
-            "velocityY": -1.2681935007022147,
-            "timestamp": 0.7162947503427298
-        },
-        {
-            "x": 2.167467858188685,
-            "y": 5.6213477608369935,
-            "heading": 0.08927498136102419,
-            "angularVelocity": -1.3270512645639152,
-            "velocityX": 1.2713432480657338,
-            "velocityY": -0.946793041094972,
-            "timestamp": 0.7814124549193416
-        },
-        {
-            "x": 2.2225251033432207,
-            "y": 5.5804064125777195,
-            "heading": 0.028526912579098198,
-            "angularVelocity": -0.9328963478811614,
-            "velocityX": 0.8455034696402836,
-            "velocityY": -0.628728370041152,
-            "timestamp": 0.8465301594959534
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": -0.4841527013902356,
-            "velocityX": 0.42192667624600166,
-            "velocityY": -0.31337733279145685,
-            "timestamp": 0.9116478640725653
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": -3.2592320489843035e-28,
-            "velocityX": -3.6408651267401044e-33,
-            "velocityY": 2.9208085569078165e-32,
-            "timestamp": 0.9767655686491771
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.1.traj
deleted file mode 100644
index 331d33ff..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.1.traj	
+++ /dev/null
@@ -1,238 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": -9.490919672428962e-31,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7219893017338576,
-            "y": 6.7135407128428675,
-            "heading": 1.0202523530723182,
-            "angularVelocity": -0.4720627806088485,
-            "velocityX": 0.2866013928023673,
-            "velocityY": -0.15440771588183827,
-            "timestamp": 0.041832670862584576
-        },
-        {
-            "x": 0.7460365692584342,
-            "y": 6.700508326600243,
-            "heading": 0.982041462546113,
-            "angularVelocity": -0.9134222065746536,
-            "velocityX": 0.574844183475,
-            "velocityY": -0.3115360787128853,
-            "timestamp": 0.08366534172516915
-        },
-        {
-            "x": 0.7822226491271561,
-            "y": 6.680770273598179,
-            "heading": 0.9269846511448561,
-            "angularVelocity": -1.3161199193358153,
-            "velocityX": 0.8650195916869635,
-            "velocityY": -0.4718334401092994,
-            "timestamp": 0.12549801258775373
-        },
-        {
-            "x": 0.830641015185364,
-            "y": 6.654177161634895,
-            "heading": 0.8571062408800131,
-            "angularVelocity": -1.6704266981755755,
-            "velocityX": 1.1574294698338659,
-            "velocityY": -0.6357019863882251,
-            "timestamp": 0.1673306834503383
-        },
-        {
-            "x": 0.8913887859512984,
-            "y": 6.620557389613832,
-            "heading": 0.7749072042273928,
-            "angularVelocity": -1.9649483276514406,
-            "velocityX": 1.4521609429503868,
-            "velocityY": -0.8036726158720227,
-            "timestamp": 0.20916335431292288
-        },
-        {
-            "x": 0.9645435989149714,
-            "y": 6.579696019447196,
-            "heading": 0.6835223857085977,
-            "angularVelocity": -2.1845322479884017,
-            "velocityX": 1.7487483217121933,
-            "velocityY": -0.976781288979697,
-            "timestamp": 0.25099602517550745
-        },
-        {
-            "x": 1.05011662601444,
-            "y": 6.531295841858201,
-            "heading": 0.5872583086768085,
-            "angularVelocity": -2.301169756719744,
-            "velocityX": 2.0456027629834046,
-            "velocityY": -1.1569946788233771,
-            "timestamp": 0.29282869603809203
-        },
-        {
-            "x": 1.147914322294414,
-            "y": 6.47495195963137,
-            "heading": 0.4931548271581024,
-            "angularVelocity": -2.249521237308087,
-            "velocityX": 2.337830558350811,
-            "velocityY": -1.3468870398426824,
-            "timestamp": 0.3346613669006766
-        },
-        {
-            "x": 1.256785786947157,
-            "y": 6.410210097873117,
-            "heading": 0.4149981322548756,
-            "angularVelocity": -1.8683171141513,
-            "velocityX": 2.602546344945923,
-            "velocityY": -1.5476387336328392,
-            "timestamp": 0.3764940377632612
-        },
-        {
-            "x": 1.3739249067400805,
-            "y": 6.336279355727049,
-            "heading": 0.36626571694971094,
-            "angularVelocity": -1.1649367420322734,
-            "velocityX": 2.800182665307358,
-            "velocityY": -1.7672967233894428,
-            "timestamp": 0.41832670862584576
-        },
-        {
-            "x": 1.4978217219933585,
-            "y": 6.2516824812952025,
-            "heading": 0.34588578820783056,
-            "angularVelocity": -0.48717732627787996,
-            "velocityX": 2.961723760365251,
-            "velocityY": -2.022268066740437,
-            "timestamp": 0.46015937948843033
-        },
-        {
-            "x": 1.6219909683827523,
-            "y": 6.153574664712983,
-            "heading": 0.34588517926846474,
-            "angularVelocity": -0.000014556550044979093,
-            "velocityX": 2.9682361615699526,
-            "velocityY": -2.345243910065211,
-            "timestamp": 0.5019920503510149
-        },
-        {
-            "x": 1.7365961394307947,
-            "y": 6.044446925458561,
-            "heading": 0.3458850779757818,
-            "angularVelocity": -0.0000024213773805789224,
-            "velocityX": 2.7396092261115483,
-            "velocityY": -2.608672528056193,
-            "timestamp": 0.5438247212135995
-        },
-        {
-            "x": 1.840662717819214,
-            "y": 5.925227165222168,
-            "heading": 0.34588493628260575,
-            "angularVelocity": -0.000003387141510558926,
-            "velocityX": 2.4876866870453034,
-            "velocityY": -2.849919877887629,
-            "timestamp": 0.5856573920761841
-        },
-        {
-            "x": 1.973177224121443,
-            "y": 5.721056374144466,
-            "heading": 0.3458848333105023,
-            "angularVelocity": -0.0000016003693825600053,
-            "velocityX": 2.0595107939713087,
-            "velocityY": -3.173176731904646,
-            "timestamp": 0.6500001023402711
-        },
-        {
-            "x": 2.0754764812101274,
-            "y": 5.5001929040123185,
-            "heading": 0.34588474927124485,
-            "angularVelocity": -0.0000013061193280868856,
-            "velocityX": 1.5899121542881502,
-            "velocityY": -3.432610613162356,
-            "timestamp": 0.7143428126043581
-        },
-        {
-            "x": 2.145503113949715,
-            "y": 5.26707902131208,
-            "heading": 0.3458846699393237,
-            "angularVelocity": -0.0000012329589594933622,
-            "velocityX": 1.0883382507855357,
-            "velocityY": -3.6230037830772255,
-            "timestamp": 0.7786855228684451
-        },
-        {
-            "x": 2.1857192532601815,
-            "y": 5.0438534457178905,
-            "heading": 0.2952188838034157,
-            "angularVelocity": -0.787436306738375,
-            "velocityX": 0.6250302348985738,
-            "velocityY": -3.4693219275035725,
-            "timestamp": 0.8430282331325321
-        },
-        {
-            "x": 2.219169026901941,
-            "y": 4.846871449995286,
-            "heading": 0.15700289500909007,
-            "angularVelocity": -2.1481219586034865,
-            "velocityX": 0.5198688943072862,
-            "velocityY": -3.0614500836865797,
-            "timestamp": 0.9073709433966191
-        },
-        {
-            "x": 2.2467778858930516,
-            "y": 4.683227688496762,
-            "heading": -0.005808843023566441,
-            "angularVelocity": -2.5303835875804483,
-            "velocityX": 0.42909070627880197,
-            "velocityY": -2.54331471004047,
-            "timestamp": 0.9717136536607061
-        },
-        {
-            "x": 2.268435157539554,
-            "y": 4.553295114204883,
-            "heading": -0.15269665079354766,
-            "angularVelocity": -2.282897427962143,
-            "velocityX": 0.33659246801420317,
-            "velocityY": -2.019382984623824,
-            "timestamp": 1.036056363924793
-        },
-        {
-            "x": 2.2843848876654955,
-            "y": 4.456384809980395,
-            "heading": -0.26981466762495615,
-            "angularVelocity": -1.820222001073865,
-            "velocityX": 0.24788713531789203,
-            "velocityY": -1.5061582551734227,
-            "timestamp": 1.10039907418888
-        },
-        {
-            "x": 2.2948521877927464,
-            "y": 4.392049751527424,
-            "heading": -0.3510408876957304,
-            "angularVelocity": -1.2623997300920147,
-            "velocityX": 0.16268043550387537,
-            "velocityY": -0.9998810772644539,
-            "timestamp": 1.164741784452967
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.393,
-            "angularVelocity": -0.6521191310103414,
-            "velocityX": 0.0800061450027687,
-            "velocityY": -0.498110063997614,
-            "timestamp": 1.229084494717054
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.393,
-            "angularVelocity": -1.3043243176582522e-30,
-            "velocityX": 1.4896025231223568e-31,
-            "velocityY": 2.1651038615450103e-31,
-            "timestamp": 1.293427204981141
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.traj
deleted file mode 100644
index 331d33ff..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.traj	
+++ /dev/null
@@ -1,238 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": -9.490919672428962e-31,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7219893017338576,
-            "y": 6.7135407128428675,
-            "heading": 1.0202523530723182,
-            "angularVelocity": -0.4720627806088485,
-            "velocityX": 0.2866013928023673,
-            "velocityY": -0.15440771588183827,
-            "timestamp": 0.041832670862584576
-        },
-        {
-            "x": 0.7460365692584342,
-            "y": 6.700508326600243,
-            "heading": 0.982041462546113,
-            "angularVelocity": -0.9134222065746536,
-            "velocityX": 0.574844183475,
-            "velocityY": -0.3115360787128853,
-            "timestamp": 0.08366534172516915
-        },
-        {
-            "x": 0.7822226491271561,
-            "y": 6.680770273598179,
-            "heading": 0.9269846511448561,
-            "angularVelocity": -1.3161199193358153,
-            "velocityX": 0.8650195916869635,
-            "velocityY": -0.4718334401092994,
-            "timestamp": 0.12549801258775373
-        },
-        {
-            "x": 0.830641015185364,
-            "y": 6.654177161634895,
-            "heading": 0.8571062408800131,
-            "angularVelocity": -1.6704266981755755,
-            "velocityX": 1.1574294698338659,
-            "velocityY": -0.6357019863882251,
-            "timestamp": 0.1673306834503383
-        },
-        {
-            "x": 0.8913887859512984,
-            "y": 6.620557389613832,
-            "heading": 0.7749072042273928,
-            "angularVelocity": -1.9649483276514406,
-            "velocityX": 1.4521609429503868,
-            "velocityY": -0.8036726158720227,
-            "timestamp": 0.20916335431292288
-        },
-        {
-            "x": 0.9645435989149714,
-            "y": 6.579696019447196,
-            "heading": 0.6835223857085977,
-            "angularVelocity": -2.1845322479884017,
-            "velocityX": 1.7487483217121933,
-            "velocityY": -0.976781288979697,
-            "timestamp": 0.25099602517550745
-        },
-        {
-            "x": 1.05011662601444,
-            "y": 6.531295841858201,
-            "heading": 0.5872583086768085,
-            "angularVelocity": -2.301169756719744,
-            "velocityX": 2.0456027629834046,
-            "velocityY": -1.1569946788233771,
-            "timestamp": 0.29282869603809203
-        },
-        {
-            "x": 1.147914322294414,
-            "y": 6.47495195963137,
-            "heading": 0.4931548271581024,
-            "angularVelocity": -2.249521237308087,
-            "velocityX": 2.337830558350811,
-            "velocityY": -1.3468870398426824,
-            "timestamp": 0.3346613669006766
-        },
-        {
-            "x": 1.256785786947157,
-            "y": 6.410210097873117,
-            "heading": 0.4149981322548756,
-            "angularVelocity": -1.8683171141513,
-            "velocityX": 2.602546344945923,
-            "velocityY": -1.5476387336328392,
-            "timestamp": 0.3764940377632612
-        },
-        {
-            "x": 1.3739249067400805,
-            "y": 6.336279355727049,
-            "heading": 0.36626571694971094,
-            "angularVelocity": -1.1649367420322734,
-            "velocityX": 2.800182665307358,
-            "velocityY": -1.7672967233894428,
-            "timestamp": 0.41832670862584576
-        },
-        {
-            "x": 1.4978217219933585,
-            "y": 6.2516824812952025,
-            "heading": 0.34588578820783056,
-            "angularVelocity": -0.48717732627787996,
-            "velocityX": 2.961723760365251,
-            "velocityY": -2.022268066740437,
-            "timestamp": 0.46015937948843033
-        },
-        {
-            "x": 1.6219909683827523,
-            "y": 6.153574664712983,
-            "heading": 0.34588517926846474,
-            "angularVelocity": -0.000014556550044979093,
-            "velocityX": 2.9682361615699526,
-            "velocityY": -2.345243910065211,
-            "timestamp": 0.5019920503510149
-        },
-        {
-            "x": 1.7365961394307947,
-            "y": 6.044446925458561,
-            "heading": 0.3458850779757818,
-            "angularVelocity": -0.0000024213773805789224,
-            "velocityX": 2.7396092261115483,
-            "velocityY": -2.608672528056193,
-            "timestamp": 0.5438247212135995
-        },
-        {
-            "x": 1.840662717819214,
-            "y": 5.925227165222168,
-            "heading": 0.34588493628260575,
-            "angularVelocity": -0.000003387141510558926,
-            "velocityX": 2.4876866870453034,
-            "velocityY": -2.849919877887629,
-            "timestamp": 0.5856573920761841
-        },
-        {
-            "x": 1.973177224121443,
-            "y": 5.721056374144466,
-            "heading": 0.3458848333105023,
-            "angularVelocity": -0.0000016003693825600053,
-            "velocityX": 2.0595107939713087,
-            "velocityY": -3.173176731904646,
-            "timestamp": 0.6500001023402711
-        },
-        {
-            "x": 2.0754764812101274,
-            "y": 5.5001929040123185,
-            "heading": 0.34588474927124485,
-            "angularVelocity": -0.0000013061193280868856,
-            "velocityX": 1.5899121542881502,
-            "velocityY": -3.432610613162356,
-            "timestamp": 0.7143428126043581
-        },
-        {
-            "x": 2.145503113949715,
-            "y": 5.26707902131208,
-            "heading": 0.3458846699393237,
-            "angularVelocity": -0.0000012329589594933622,
-            "velocityX": 1.0883382507855357,
-            "velocityY": -3.6230037830772255,
-            "timestamp": 0.7786855228684451
-        },
-        {
-            "x": 2.1857192532601815,
-            "y": 5.0438534457178905,
-            "heading": 0.2952188838034157,
-            "angularVelocity": -0.787436306738375,
-            "velocityX": 0.6250302348985738,
-            "velocityY": -3.4693219275035725,
-            "timestamp": 0.8430282331325321
-        },
-        {
-            "x": 2.219169026901941,
-            "y": 4.846871449995286,
-            "heading": 0.15700289500909007,
-            "angularVelocity": -2.1481219586034865,
-            "velocityX": 0.5198688943072862,
-            "velocityY": -3.0614500836865797,
-            "timestamp": 0.9073709433966191
-        },
-        {
-            "x": 2.2467778858930516,
-            "y": 4.683227688496762,
-            "heading": -0.005808843023566441,
-            "angularVelocity": -2.5303835875804483,
-            "velocityX": 0.42909070627880197,
-            "velocityY": -2.54331471004047,
-            "timestamp": 0.9717136536607061
-        },
-        {
-            "x": 2.268435157539554,
-            "y": 4.553295114204883,
-            "heading": -0.15269665079354766,
-            "angularVelocity": -2.282897427962143,
-            "velocityX": 0.33659246801420317,
-            "velocityY": -2.019382984623824,
-            "timestamp": 1.036056363924793
-        },
-        {
-            "x": 2.2843848876654955,
-            "y": 4.456384809980395,
-            "heading": -0.26981466762495615,
-            "angularVelocity": -1.820222001073865,
-            "velocityX": 0.24788713531789203,
-            "velocityY": -1.5061582551734227,
-            "timestamp": 1.10039907418888
-        },
-        {
-            "x": 2.2948521877927464,
-            "y": 4.392049751527424,
-            "heading": -0.3510408876957304,
-            "angularVelocity": -1.2623997300920147,
-            "velocityX": 0.16268043550387537,
-            "velocityY": -0.9998810772644539,
-            "timestamp": 1.164741784452967
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.393,
-            "angularVelocity": -0.6521191310103414,
-            "velocityX": 0.0800061450027687,
-            "velocityY": -0.498110063997614,
-            "timestamp": 1.229084494717054
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.393,
-            "angularVelocity": -1.3043243176582522e-30,
-            "velocityX": 1.4896025231223568e-31,
-            "velocityY": 2.1651038615450103e-31,
-            "timestamp": 1.293427204981141
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C2.1.traj
deleted file mode 100644
index 50686485..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C2.1.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -5.382102664363315e-41,
-            "angularVelocity": -2.8652892976895567e-45,
-            "velocityX": 1.592735858156421e-40,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 7.383499994424153,
-            "heading": 1.659495816830368e-21,
-            "angularVelocity": 1.658316243120447e-20,
-            "velocityX": 2.780383744489643e-33,
-            "velocityY": -0.8344065481228213,
-            "timestamp": 0.10007112931184184
-        },
-        {
-            "x": 7.68,
-            "y": 7.216499986370153,
-            "heading": 4.9512387638524226e-21,
-            "angularVelocity": 3.289403167370527e-20,
-            "velocityX": 3.618624574631574e-33,
-            "velocityY": -1.668813065290736,
-            "timestamp": 0.20014225862368368
-        },
-        {
-            "x": 7.68,
-            "y": 6.9659999820333836,
-            "heading": 9.801760970151702e-21,
-            "angularVelocity": 4.8470744390330394e-20,
-            "velocityX": 3.439292531132709e-33,
-            "velocityY": -2.503219520548835,
-            "timestamp": 0.30021338793552554
-        },
-        {
-            "x": 7.68,
-            "y": 6.632,
-            "heading": 1.506109654033251e-20,
-            "angularVelocity": 5.255597191401202e-20,
-            "velocityX": 4.639675238422859e-33,
-            "velocityY": -3.3376257900775017,
-            "timestamp": 0.40028451724736736
-        },
-        {
-            "x": 7.68,
-            "y": 6.298000017966616,
-            "heading": 5.3586032763760104e-21,
-            "angularVelocity": -9.69559666912617e-20,
-            "velocityX": 4.935439546802769e-33,
-            "velocityY": -3.3376257900775017,
-            "timestamp": 0.5003556465592092
-        },
-        {
-            "x": 7.68,
-            "y": 6.047500013629846,
-            "heading": 1.8760590350448333e-21,
-            "angularVelocity": -3.480068842301423e-20,
-            "velocityX": 4.701730454770122e-33,
-            "velocityY": -2.5032195205488352,
-            "timestamp": 0.600426775871051
-        },
-        {
-            "x": 7.68,
-            "y": 5.880500005575846,
-            "heading": 3.168722428707802e-22,
-            "angularVelocity": -1.5580785203142817e-20,
-            "velocityX": 3.49539235617723e-33,
-            "velocityY": -1.668813065290736,
-            "timestamp": 0.7004979051828928
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 5.881496870004905e-41,
-            "angularVelocity": -3.1664700918333573e-21,
-            "velocityX": 3.364378386644386e-33,
-            "velocityY": -0.8344065481228213,
-            "timestamp": 0.8005690344947346
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 9.145026397391945e-42,
-            "angularVelocity": -4.963805231001231e-40,
-            "velocityX": -4.870998572760777e-42,
-            "velocityY": 3.489748854003666e-39,
-            "timestamp": 0.9006401638065764
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C2.traj
deleted file mode 100644
index 50686485..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C2.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -5.382102664363315e-41,
-            "angularVelocity": -2.8652892976895567e-45,
-            "velocityX": 1.592735858156421e-40,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 7.383499994424153,
-            "heading": 1.659495816830368e-21,
-            "angularVelocity": 1.658316243120447e-20,
-            "velocityX": 2.780383744489643e-33,
-            "velocityY": -0.8344065481228213,
-            "timestamp": 0.10007112931184184
-        },
-        {
-            "x": 7.68,
-            "y": 7.216499986370153,
-            "heading": 4.9512387638524226e-21,
-            "angularVelocity": 3.289403167370527e-20,
-            "velocityX": 3.618624574631574e-33,
-            "velocityY": -1.668813065290736,
-            "timestamp": 0.20014225862368368
-        },
-        {
-            "x": 7.68,
-            "y": 6.9659999820333836,
-            "heading": 9.801760970151702e-21,
-            "angularVelocity": 4.8470744390330394e-20,
-            "velocityX": 3.439292531132709e-33,
-            "velocityY": -2.503219520548835,
-            "timestamp": 0.30021338793552554
-        },
-        {
-            "x": 7.68,
-            "y": 6.632,
-            "heading": 1.506109654033251e-20,
-            "angularVelocity": 5.255597191401202e-20,
-            "velocityX": 4.639675238422859e-33,
-            "velocityY": -3.3376257900775017,
-            "timestamp": 0.40028451724736736
-        },
-        {
-            "x": 7.68,
-            "y": 6.298000017966616,
-            "heading": 5.3586032763760104e-21,
-            "angularVelocity": -9.69559666912617e-20,
-            "velocityX": 4.935439546802769e-33,
-            "velocityY": -3.3376257900775017,
-            "timestamp": 0.5003556465592092
-        },
-        {
-            "x": 7.68,
-            "y": 6.047500013629846,
-            "heading": 1.8760590350448333e-21,
-            "angularVelocity": -3.480068842301423e-20,
-            "velocityX": 4.701730454770122e-33,
-            "velocityY": -2.5032195205488352,
-            "timestamp": 0.600426775871051
-        },
-        {
-            "x": 7.68,
-            "y": 5.880500005575846,
-            "heading": 3.168722428707802e-22,
-            "angularVelocity": -1.5580785203142817e-20,
-            "velocityX": 3.49539235617723e-33,
-            "velocityY": -1.668813065290736,
-            "timestamp": 0.7004979051828928
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 5.881496870004905e-41,
-            "angularVelocity": -3.1664700918333573e-21,
-            "velocityX": 3.364378386644386e-33,
-            "velocityY": -0.8344065481228213,
-            "timestamp": 0.8005690344947346
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 9.145026397391945e-42,
-            "angularVelocity": -4.963805231001231e-40,
-            "velocityX": -4.870998572760777e-42,
-            "velocityY": 3.489748854003666e-39,
-            "timestamp": 0.9006401638065764
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C3.1.traj
deleted file mode 100644
index 688fbdaa..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C3.1.traj	
+++ /dev/null
@@ -1,139 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 1.0127320738607452e-39,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 7.390554627994461,
-            "heading": -1.4011773737891353e-18,
-            "angularVelocity": -1.4633626501290335e-17,
-            "velocityX": 9.653222160408549e-34,
-            "velocityY": -0.7983807350946673,
-            "timestamp": 0.09575052183150974
-        },
-        {
-            "x": 7.68,
-            "y": 7.237663887017311,
-            "heading": -4.2066729097937526e-18,
-            "angularVelocity": -2.93000547907741e-17,
-            "velocityX": 5.229262005564601e-34,
-            "velocityY": -1.5967614385035824,
-            "timestamp": 0.19150104366301948
-        },
-        {
-            "x": 7.68,
-            "y": 7.0083277831364,
-            "heading": -8.423669065336014e-18,
-            "angularVelocity": -4.4041495279144365e-17,
-            "velocityX": -1.080014219031917e-33,
-            "velocityY": -2.3951420785410225,
-            "timestamp": 0.28725156549452924
-        },
-        {
-            "x": 7.68,
-            "y": 6.702546334555254,
-            "heading": -1.4072329853209785e-17,
-            "angularVelocity": -5.899352482411367e-17,
-            "velocityX": -1.5994734307756055e-33,
-            "velocityY": -3.1935225284643685,
-            "timestamp": 0.38300208732603896
-        },
-        {
-            "x": 7.68,
-            "y": 6.340327800733179,
-            "heading": -3.9014262815434015e-17,
-            "angularVelocity": -2.604887418380216e-16,
-            "velocityX": 4.89416509473128e-33,
-            "velocityY": -3.7829405719528495,
-            "timestamp": 0.4787526091575487
-        },
-        {
-            "x": 7.68,
-            "y": 5.978109266911059,
-            "heading": -3.149786356993182e-17,
-            "angularVelocity": 7.849982539759592e-17,
-            "velocityX": -6.0692119772378415e-33,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.5745031309890585
-        },
-        {
-            "x": 7.68,
-            "y": 5.61589073308894,
-            "heading": -3.435394827038493e-17,
-            "angularVelocity": -2.98283982773363e-17,
-            "velocityX": 7.480170031140856e-33,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.6702536528205683
-        },
-        {
-            "x": 7.68,
-            "y": 5.253672199266821,
-            "heading": -8.183196831726821e-18,
-            "angularVelocity": 2.7332228522691796e-16,
-            "velocityX": -1.1202872015905622e-32,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.766004174652078
-        },
-        {
-            "x": 7.68,
-            "y": 4.891453665444746,
-            "heading": 1.4014689490668664e-17,
-            "angularVelocity": 2.318304474774222e-16,
-            "velocityX": 5.155427490211361e-33,
-            "velocityY": -3.7829405719528495,
-            "timestamp": 0.8617546964835878
-        },
-        {
-            "x": 7.68,
-            "y": 4.5856722168636,
-            "heading": 8.395813027039913e-18,
-            "angularVelocity": -5.868246309956073e-17,
-            "velocityX": -4.168254505442575e-33,
-            "velocityY": -3.1935225284643685,
-            "timestamp": 0.9575052183150976
-        },
-        {
-            "x": 7.68,
-            "y": 4.356336112982689,
-            "heading": 4.1945848336017725e-18,
-            "angularVelocity": -4.387681773072387e-17,
-            "velocityX": 2.1988846402360483e-33,
-            "velocityY": -2.3951420785410225,
-            "timestamp": 1.0532557401466074
-        },
-        {
-            "x": 7.68,
-            "y": 4.203445372005539,
-            "heading": 1.3975677775871127e-18,
-            "angularVelocity": -2.9211507184809557e-17,
-            "velocityX": 3.5559383909284404e-33,
-            "velocityY": -1.5967614385035824,
-            "timestamp": 1.1490062619781172
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -1.4595928576161535e-17,
-            "velocityX": -6.471812078129625e-34,
-            "velocityY": -0.7983807350946673,
-            "timestamp": 1.244756783809627
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 8.546727720642512e-43,
-            "velocityY": 6.233759417908357e-39,
-            "timestamp": 1.3405073056411367
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C3.traj
deleted file mode 100644
index 688fbdaa..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C3.traj	
+++ /dev/null
@@ -1,139 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 1.0127320738607452e-39,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 7.390554627994461,
-            "heading": -1.4011773737891353e-18,
-            "angularVelocity": -1.4633626501290335e-17,
-            "velocityX": 9.653222160408549e-34,
-            "velocityY": -0.7983807350946673,
-            "timestamp": 0.09575052183150974
-        },
-        {
-            "x": 7.68,
-            "y": 7.237663887017311,
-            "heading": -4.2066729097937526e-18,
-            "angularVelocity": -2.93000547907741e-17,
-            "velocityX": 5.229262005564601e-34,
-            "velocityY": -1.5967614385035824,
-            "timestamp": 0.19150104366301948
-        },
-        {
-            "x": 7.68,
-            "y": 7.0083277831364,
-            "heading": -8.423669065336014e-18,
-            "angularVelocity": -4.4041495279144365e-17,
-            "velocityX": -1.080014219031917e-33,
-            "velocityY": -2.3951420785410225,
-            "timestamp": 0.28725156549452924
-        },
-        {
-            "x": 7.68,
-            "y": 6.702546334555254,
-            "heading": -1.4072329853209785e-17,
-            "angularVelocity": -5.899352482411367e-17,
-            "velocityX": -1.5994734307756055e-33,
-            "velocityY": -3.1935225284643685,
-            "timestamp": 0.38300208732603896
-        },
-        {
-            "x": 7.68,
-            "y": 6.340327800733179,
-            "heading": -3.9014262815434015e-17,
-            "angularVelocity": -2.604887418380216e-16,
-            "velocityX": 4.89416509473128e-33,
-            "velocityY": -3.7829405719528495,
-            "timestamp": 0.4787526091575487
-        },
-        {
-            "x": 7.68,
-            "y": 5.978109266911059,
-            "heading": -3.149786356993182e-17,
-            "angularVelocity": 7.849982539759592e-17,
-            "velocityX": -6.0692119772378415e-33,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.5745031309890585
-        },
-        {
-            "x": 7.68,
-            "y": 5.61589073308894,
-            "heading": -3.435394827038493e-17,
-            "angularVelocity": -2.98283982773363e-17,
-            "velocityX": 7.480170031140856e-33,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.6702536528205683
-        },
-        {
-            "x": 7.68,
-            "y": 5.253672199266821,
-            "heading": -8.183196831726821e-18,
-            "angularVelocity": 2.7332228522691796e-16,
-            "velocityX": -1.1202872015905622e-32,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.766004174652078
-        },
-        {
-            "x": 7.68,
-            "y": 4.891453665444746,
-            "heading": 1.4014689490668664e-17,
-            "angularVelocity": 2.318304474774222e-16,
-            "velocityX": 5.155427490211361e-33,
-            "velocityY": -3.7829405719528495,
-            "timestamp": 0.8617546964835878
-        },
-        {
-            "x": 7.68,
-            "y": 4.5856722168636,
-            "heading": 8.395813027039913e-18,
-            "angularVelocity": -5.868246309956073e-17,
-            "velocityX": -4.168254505442575e-33,
-            "velocityY": -3.1935225284643685,
-            "timestamp": 0.9575052183150976
-        },
-        {
-            "x": 7.68,
-            "y": 4.356336112982689,
-            "heading": 4.1945848336017725e-18,
-            "angularVelocity": -4.387681773072387e-17,
-            "velocityX": 2.1988846402360483e-33,
-            "velocityY": -2.3951420785410225,
-            "timestamp": 1.0532557401466074
-        },
-        {
-            "x": 7.68,
-            "y": 4.203445372005539,
-            "heading": 1.3975677775871127e-18,
-            "angularVelocity": -2.9211507184809557e-17,
-            "velocityX": 3.5559383909284404e-33,
-            "velocityY": -1.5967614385035824,
-            "timestamp": 1.1490062619781172
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -1.4595928576161535e-17,
-            "velocityX": -6.471812078129625e-34,
-            "velocityY": -0.7983807350946673,
-            "timestamp": 1.244756783809627
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 8.546727720642512e-43,
-            "velocityY": 6.233759417908357e-39,
-            "timestamp": 1.3405073056411367
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.1.traj
deleted file mode 100644
index d59fe62f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.1.traj	
+++ /dev/null
@@ -1,175 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 3.5626996813915863e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 7.385155846048237,
-            "heading": 1.781123128781041e-18,
-            "angularVelocity": 1.7977717932873702e-17,
-            "velocityX": 2.837123063321612e-19,
-            "velocityY": -0.8260917454276266,
-            "timestamp": 0.099073927811004
-        },
-        {
-            "x": 7.68,
-            "y": 7.22146754125807,
-            "heading": 5.344190150368438e-18,
-            "angularVelocity": 3.5963720225105045e-17,
-            "velocityX": 5.673750850451309e-19,
-            "velocityY": -1.65218345943065,
-            "timestamp": 0.198147855622008
-        },
-        {
-            "x": 7.68,
-            "y": 6.975935091856216,
-            "heading": 1.0690981078424305e-17,
-            "angularVelocity": 5.396768903659916e-17,
-            "velocityX": 8.509388117154945e-19,
-            "velocityY": -2.47827511058448,
-            "timestamp": 0.29722178343301203
-        },
-        {
-            "x": 7.68,
-            "y": 6.648558516522808,
-            "heading": 1.7824905328944302e-17,
-            "angularVelocity": 7.200607069613354e-17,
-            "velocityX": 1.134205375948437e-18,
-            "velocityY": -3.304366573190878,
-            "timestamp": 0.396295711244016
-        },
-        {
-            "x": 7.68,
-            "y": 6.273767735073314,
-            "heading": 3.2118763957552995e-17,
-            "angularVelocity": 1.4427467391699638e-16,
-            "velocityX": 1.2981920193437937e-18,
-            "velocityY": -3.782940575087071,
-            "timestamp": 0.49536963905502
-        },
-        {
-            "x": 7.68,
-            "y": 5.898976953623795,
-            "heading": 3.194072142377472e-17,
-            "angularVelocity": -1.7970674806511035e-18,
-            "velocityX": 1.2970624694923187e-18,
-            "velocityY": -3.7829405750873115,
-            "timestamp": 0.5944435668660241
-        },
-        {
-            "x": 7.68,
-            "y": 5.524186172174278,
-            "heading": 3.523836351949418e-17,
-            "angularVelocity": 3.3284660945411024e-17,
-            "velocityX": 1.2988038949897902e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.6935174946770281
-        },
-        {
-            "x": 7.68,
-            "y": 5.149395390724759,
-            "heading": 4.1591047407718544e-17,
-            "angularVelocity": 6.412064231794501e-17,
-            "velocityX": 1.299093645390414e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.792591422488032
-        },
-        {
-            "x": 7.68,
-            "y": 4.774604609275242,
-            "heading": 3.0405238206332753e-17,
-            "angularVelocity": -1.1290366142265185e-16,
-            "velocityX": 1.2929821768433727e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.891665350299036
-        },
-        {
-            "x": 7.68,
-            "y": 4.3998138278257235,
-            "heading": 3.8390344063969795e-17,
-            "angularVelocity": 8.059744913789504e-17,
-            "velocityX": 1.304793755296235e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.99073927811004
-        },
-        {
-            "x": 7.68,
-            "y": 4.025023046376205,
-            "heading": 1.0979594277826173e-17,
-            "angularVelocity": -2.766696586253564e-16,
-            "velocityX": 1.307186742413441e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 1.0898132059210441
-        },
-        {
-            "x": 7.68,
-            "y": 3.650232264926687,
-            "heading": -2.14668342522941e-17,
-            "angularVelocity": -3.2749714528425564e-16,
-            "velocityX": 1.3056923664932481e-18,
-            "velocityY": -3.7829405750873115,
-            "timestamp": 1.1888871337320481
-        },
-        {
-            "x": 7.68,
-            "y": 3.2754414834771928,
-            "heading": -1.7689075007663427e-17,
-            "angularVelocity": 3.813071035032691e-17,
-            "velocityX": 1.3012288375156972e-18,
-            "velocityY": -3.782940575087071,
-            "timestamp": 1.2879610615430521
-        },
-        {
-            "x": 7.68,
-            "y": 2.948064908143785,
-            "heading": -1.0623620375651477e-17,
-            "angularVelocity": 7.131497447110055e-17,
-            "velocityX": 1.136256754825581e-18,
-            "velocityY": -3.304366573190878,
-            "timestamp": 1.3870349893540561
-        },
-        {
-            "x": 7.68,
-            "y": 2.70253245874193,
-            "heading": -5.31498515459649e-18,
-            "angularVelocity": 5.35825654501949e-17,
-            "velocityX": 8.521926280380639e-19,
-            "velocityY": -2.4782751105844802,
-            "timestamp": 1.48610891716506
-        },
-        {
-            "x": 7.68,
-            "y": 2.538844153951763,
-            "heading": -1.7723662552803497e-18,
-            "angularVelocity": 3.575732766130284e-17,
-            "velocityX": 5.681284366395801e-19,
-            "velocityY": -1.6521834594306501,
-            "timestamp": 1.585182844976064
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 1.7889330669293064e-17,
-            "velocityX": 2.8406422370407584e-19,
-            "velocityY": -0.8260917454276266,
-            "timestamp": 1.684256772787068
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 1.0461754707969188e-34,
-            "velocityY": 0,
-            "timestamp": 1.783330700598072
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.traj
deleted file mode 100644
index d59fe62f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.traj	
+++ /dev/null
@@ -1,175 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 3.5626996813915863e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 7.385155846048237,
-            "heading": 1.781123128781041e-18,
-            "angularVelocity": 1.7977717932873702e-17,
-            "velocityX": 2.837123063321612e-19,
-            "velocityY": -0.8260917454276266,
-            "timestamp": 0.099073927811004
-        },
-        {
-            "x": 7.68,
-            "y": 7.22146754125807,
-            "heading": 5.344190150368438e-18,
-            "angularVelocity": 3.5963720225105045e-17,
-            "velocityX": 5.673750850451309e-19,
-            "velocityY": -1.65218345943065,
-            "timestamp": 0.198147855622008
-        },
-        {
-            "x": 7.68,
-            "y": 6.975935091856216,
-            "heading": 1.0690981078424305e-17,
-            "angularVelocity": 5.396768903659916e-17,
-            "velocityX": 8.509388117154945e-19,
-            "velocityY": -2.47827511058448,
-            "timestamp": 0.29722178343301203
-        },
-        {
-            "x": 7.68,
-            "y": 6.648558516522808,
-            "heading": 1.7824905328944302e-17,
-            "angularVelocity": 7.200607069613354e-17,
-            "velocityX": 1.134205375948437e-18,
-            "velocityY": -3.304366573190878,
-            "timestamp": 0.396295711244016
-        },
-        {
-            "x": 7.68,
-            "y": 6.273767735073314,
-            "heading": 3.2118763957552995e-17,
-            "angularVelocity": 1.4427467391699638e-16,
-            "velocityX": 1.2981920193437937e-18,
-            "velocityY": -3.782940575087071,
-            "timestamp": 0.49536963905502
-        },
-        {
-            "x": 7.68,
-            "y": 5.898976953623795,
-            "heading": 3.194072142377472e-17,
-            "angularVelocity": -1.7970674806511035e-18,
-            "velocityX": 1.2970624694923187e-18,
-            "velocityY": -3.7829405750873115,
-            "timestamp": 0.5944435668660241
-        },
-        {
-            "x": 7.68,
-            "y": 5.524186172174278,
-            "heading": 3.523836351949418e-17,
-            "angularVelocity": 3.3284660945411024e-17,
-            "velocityX": 1.2988038949897902e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.6935174946770281
-        },
-        {
-            "x": 7.68,
-            "y": 5.149395390724759,
-            "heading": 4.1591047407718544e-17,
-            "angularVelocity": 6.412064231794501e-17,
-            "velocityX": 1.299093645390414e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.792591422488032
-        },
-        {
-            "x": 7.68,
-            "y": 4.774604609275242,
-            "heading": 3.0405238206332753e-17,
-            "angularVelocity": -1.1290366142265185e-16,
-            "velocityX": 1.2929821768433727e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.891665350299036
-        },
-        {
-            "x": 7.68,
-            "y": 4.3998138278257235,
-            "heading": 3.8390344063969795e-17,
-            "angularVelocity": 8.059744913789504e-17,
-            "velocityX": 1.304793755296235e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.99073927811004
-        },
-        {
-            "x": 7.68,
-            "y": 4.025023046376205,
-            "heading": 1.0979594277826173e-17,
-            "angularVelocity": -2.766696586253564e-16,
-            "velocityX": 1.307186742413441e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 1.0898132059210441
-        },
-        {
-            "x": 7.68,
-            "y": 3.650232264926687,
-            "heading": -2.14668342522941e-17,
-            "angularVelocity": -3.2749714528425564e-16,
-            "velocityX": 1.3056923664932481e-18,
-            "velocityY": -3.7829405750873115,
-            "timestamp": 1.1888871337320481
-        },
-        {
-            "x": 7.68,
-            "y": 3.2754414834771928,
-            "heading": -1.7689075007663427e-17,
-            "angularVelocity": 3.813071035032691e-17,
-            "velocityX": 1.3012288375156972e-18,
-            "velocityY": -3.782940575087071,
-            "timestamp": 1.2879610615430521
-        },
-        {
-            "x": 7.68,
-            "y": 2.948064908143785,
-            "heading": -1.0623620375651477e-17,
-            "angularVelocity": 7.131497447110055e-17,
-            "velocityX": 1.136256754825581e-18,
-            "velocityY": -3.304366573190878,
-            "timestamp": 1.3870349893540561
-        },
-        {
-            "x": 7.68,
-            "y": 2.70253245874193,
-            "heading": -5.31498515459649e-18,
-            "angularVelocity": 5.35825654501949e-17,
-            "velocityX": 8.521926280380639e-19,
-            "velocityY": -2.4782751105844802,
-            "timestamp": 1.48610891716506
-        },
-        {
-            "x": 7.68,
-            "y": 2.538844153951763,
-            "heading": -1.7723662552803497e-18,
-            "angularVelocity": 3.575732766130284e-17,
-            "velocityX": 5.681284366395801e-19,
-            "velocityY": -1.6521834594306501,
-            "timestamp": 1.585182844976064
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 1.7889330669293064e-17,
-            "velocityX": 2.8406422370407584e-19,
-            "velocityY": -0.8260917454276266,
-            "timestamp": 1.684256772787068
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 1.0461754707969188e-34,
-            "velocityY": 0,
-            "timestamp": 1.783330700598072
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.1.traj
deleted file mode 100644
index 6f64999b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.1.traj	
+++ /dev/null
@@ -1,220 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -8.452820466081124e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 7.3890447295128565,
-            "heading": -6.111572142469688e-20,
-            "angularVelocity": -6.320692328199756e-19,
-            "velocityX": -1.9797111149811777e-21,
-            "velocityY": -0.8062267267095136,
-            "timestamp": 0.09669149868710691
-        },
-        {
-            "x": 7.68,
-            "y": 7.2331341916331855,
-            "heading": -1.8226742072400974e-19,
-            "angularVelocity": -1.2529715733337834e-18,
-            "velocityX": -3.753564400558873e-21,
-            "velocityY": -1.6124534214139752,
-            "timestamp": 0.19338299737421383
-        },
-        {
-            "x": 7.68,
-            "y": 6.999268392550218,
-            "heading": -3.611632829024713e-19,
-            "angularVelocity": -1.850171571559818e-18,
-            "velocityX": -5.1157697830729356e-21,
-            "velocityY": -2.418680052108357,
-            "timestamp": 0.29007449606132074
-        },
-        {
-            "x": 7.68,
-            "y": 6.687447350831621,
-            "heading": -5.912144198792796e-19,
-            "angularVelocity": -2.3792281637439373e-18,
-            "velocityX": -5.242727749103024e-21,
-            "velocityY": -3.224906490772764,
-            "timestamp": 0.38676599474842766
-        },
-        {
-            "x": 7.68,
-            "y": 6.321669157855709,
-            "heading": -4.100561450064452e-17,
-            "angularVelocity": -4.179726307846966e-16,
-            "velocityX": -1.166132844762965e-20,
-            "velocityY": -3.782940568121389,
-            "timestamp": 0.48345749343553457
-        },
-        {
-            "x": 7.68,
-            "y": 5.955890964879757,
-            "heading": -6.040572418063157e-17,
-            "angularVelocity": -2.0063924898701218e-16,
-            "velocityX": -1.9392305823460385e-22,
-            "velocityY": -3.782940568121787,
-            "timestamp": 0.5801489921226415
-        },
-        {
-            "x": 7.68,
-            "y": 5.590112771903805,
-            "heading": -8.327536807485113e-17,
-            "angularVelocity": -2.365217646302656e-16,
-            "velocityX": 3.675624932403311e-22,
-            "velocityY": -3.782940568121787,
-            "timestamp": 0.6768404908097484
-        },
-        {
-            "x": 7.68,
-            "y": 5.224334578927854,
-            "heading": -7.575439540555723e-17,
-            "angularVelocity": 7.778318436801094e-17,
-            "velocityX": -2.1057162419585226e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 0.7735319894968553
-        },
-        {
-            "x": 7.68,
-            "y": 4.858556385951902,
-            "heading": -4.8431591576069396e-17,
-            "angularVelocity": 2.82577105541659e-16,
-            "velocityX": 1.016892835365518e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 0.8702234881839622
-        },
-        {
-            "x": 7.68,
-            "y": 4.49277819297595,
-            "heading": -7.892411277004063e-17,
-            "angularVelocity": -3.1535886409915893e-16,
-            "velocityX": 1.448731313682566e-20,
-            "velocityY": -3.782940568121787,
-            "timestamp": 0.9669149868710691
-        },
-        {
-            "x": 7.68,
-            "y": 4.126999999999999,
-            "heading": -8.668140475632877e-17,
-            "angularVelocity": -8.022723912254832e-17,
-            "velocityX": 2.655349226445793e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 1.063606485558176
-        },
-        {
-            "x": 7.68,
-            "y": 3.7612218070240475,
-            "heading": -4.236036305527668e-17,
-            "angularVelocity": 4.583757859030679e-16,
-            "velocityX": 7.500561292003624e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 1.160297984245283
-        },
-        {
-            "x": 7.68,
-            "y": 3.395443614048096,
-            "heading": -2.060715967072721e-17,
-            "angularVelocity": 2.249753461257506e-16,
-            "velocityX": 1.3311646441840309e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 1.25698948293239
-        },
-        {
-            "x": 7.68,
-            "y": 3.0296654210721448,
-            "heading": -1.7378554757194933e-17,
-            "angularVelocity": 3.3390783650741015e-17,
-            "velocityX": 1.5677048108756238e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 1.353680981619497
-        },
-        {
-            "x": 7.68,
-            "y": 2.663887228096194,
-            "heading": -2.7058170522872628e-18,
-            "angularVelocity": 1.5174796030816139e-16,
-            "velocityX": 2.708497475030379e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 1.450372480306604
-        },
-        {
-            "x": 7.68,
-            "y": 2.2981090351202424,
-            "heading": -9.881529062229235e-18,
-            "angularVelocity": -7.421243963841227e-17,
-            "velocityX": 2.3345387345088647e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 1.547063978993711
-        },
-        {
-            "x": 7.68,
-            "y": 1.9323308421442913,
-            "heading": -6.5997498705085955e-18,
-            "angularVelocity": 3.39407211211041e-17,
-            "velocityX": -6.072343928076645e-20,
-            "velocityY": -3.782940568121787,
-            "timestamp": 1.643755477680818
-        },
-        {
-            "x": 7.68,
-            "y": 1.5665526491683786,
-            "heading": 6.487893960505722e-19,
-            "angularVelocity": 7.496563157826373e-17,
-            "velocityX": 5.228734565126193e-20,
-            "velocityY": -3.782940568121389,
-            "timestamp": 1.7404469763679251
-        },
-        {
-            "x": 7.68,
-            "y": 1.254731607449781,
-            "heading": 3.9009642634108723e-19,
-            "angularVelocity": -2.6754468948553036e-18,
-            "velocityX": 1.8077268140623735e-21,
-            "velocityY": -3.224906490772764,
-            "timestamp": 1.8371384750550321
-        },
-        {
-            "x": 7.68,
-            "y": 1.020865808366814,
-            "heading": 1.9475770164363563e-19,
-            "angularVelocity": -2.020226466917104e-18,
-            "velocityX": 1.3557598034014801e-21,
-            "velocityY": -2.418680052108357,
-            "timestamp": 1.9338299737421392
-        },
-        {
-            "x": 7.68,
-            "y": 0.8649552704871435,
-            "heading": 6.488190339479676e-20,
-            "angularVelocity": -1.3431976959152128e-18,
-            "velocityX": 9.038296351119003e-22,
-            "velocityY": -1.6124534214139752,
-            "timestamp": 2.030521472429246
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -6.710197305720325e-19,
-            "velocityX": 4.519117474728098e-22,
-            "velocityY": -0.8062267267095136,
-            "timestamp": 2.127212971116353
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 1.0656726462878492e-32,
-            "velocityY": 0,
-            "timestamp": 2.2239044698034602
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.traj
deleted file mode 100644
index 6f64999b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.traj	
+++ /dev/null
@@ -1,220 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -8.452820466081124e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 7.3890447295128565,
-            "heading": -6.111572142469688e-20,
-            "angularVelocity": -6.320692328199756e-19,
-            "velocityX": -1.9797111149811777e-21,
-            "velocityY": -0.8062267267095136,
-            "timestamp": 0.09669149868710691
-        },
-        {
-            "x": 7.68,
-            "y": 7.2331341916331855,
-            "heading": -1.8226742072400974e-19,
-            "angularVelocity": -1.2529715733337834e-18,
-            "velocityX": -3.753564400558873e-21,
-            "velocityY": -1.6124534214139752,
-            "timestamp": 0.19338299737421383
-        },
-        {
-            "x": 7.68,
-            "y": 6.999268392550218,
-            "heading": -3.611632829024713e-19,
-            "angularVelocity": -1.850171571559818e-18,
-            "velocityX": -5.1157697830729356e-21,
-            "velocityY": -2.418680052108357,
-            "timestamp": 0.29007449606132074
-        },
-        {
-            "x": 7.68,
-            "y": 6.687447350831621,
-            "heading": -5.912144198792796e-19,
-            "angularVelocity": -2.3792281637439373e-18,
-            "velocityX": -5.242727749103024e-21,
-            "velocityY": -3.224906490772764,
-            "timestamp": 0.38676599474842766
-        },
-        {
-            "x": 7.68,
-            "y": 6.321669157855709,
-            "heading": -4.100561450064452e-17,
-            "angularVelocity": -4.179726307846966e-16,
-            "velocityX": -1.166132844762965e-20,
-            "velocityY": -3.782940568121389,
-            "timestamp": 0.48345749343553457
-        },
-        {
-            "x": 7.68,
-            "y": 5.955890964879757,
-            "heading": -6.040572418063157e-17,
-            "angularVelocity": -2.0063924898701218e-16,
-            "velocityX": -1.9392305823460385e-22,
-            "velocityY": -3.782940568121787,
-            "timestamp": 0.5801489921226415
-        },
-        {
-            "x": 7.68,
-            "y": 5.590112771903805,
-            "heading": -8.327536807485113e-17,
-            "angularVelocity": -2.365217646302656e-16,
-            "velocityX": 3.675624932403311e-22,
-            "velocityY": -3.782940568121787,
-            "timestamp": 0.6768404908097484
-        },
-        {
-            "x": 7.68,
-            "y": 5.224334578927854,
-            "heading": -7.575439540555723e-17,
-            "angularVelocity": 7.778318436801094e-17,
-            "velocityX": -2.1057162419585226e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 0.7735319894968553
-        },
-        {
-            "x": 7.68,
-            "y": 4.858556385951902,
-            "heading": -4.8431591576069396e-17,
-            "angularVelocity": 2.82577105541659e-16,
-            "velocityX": 1.016892835365518e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 0.8702234881839622
-        },
-        {
-            "x": 7.68,
-            "y": 4.49277819297595,
-            "heading": -7.892411277004063e-17,
-            "angularVelocity": -3.1535886409915893e-16,
-            "velocityX": 1.448731313682566e-20,
-            "velocityY": -3.782940568121787,
-            "timestamp": 0.9669149868710691
-        },
-        {
-            "x": 7.68,
-            "y": 4.126999999999999,
-            "heading": -8.668140475632877e-17,
-            "angularVelocity": -8.022723912254832e-17,
-            "velocityX": 2.655349226445793e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 1.063606485558176
-        },
-        {
-            "x": 7.68,
-            "y": 3.7612218070240475,
-            "heading": -4.236036305527668e-17,
-            "angularVelocity": 4.583757859030679e-16,
-            "velocityX": 7.500561292003624e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 1.160297984245283
-        },
-        {
-            "x": 7.68,
-            "y": 3.395443614048096,
-            "heading": -2.060715967072721e-17,
-            "angularVelocity": 2.249753461257506e-16,
-            "velocityX": 1.3311646441840309e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 1.25698948293239
-        },
-        {
-            "x": 7.68,
-            "y": 3.0296654210721448,
-            "heading": -1.7378554757194933e-17,
-            "angularVelocity": 3.3390783650741015e-17,
-            "velocityX": 1.5677048108756238e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 1.353680981619497
-        },
-        {
-            "x": 7.68,
-            "y": 2.663887228096194,
-            "heading": -2.7058170522872628e-18,
-            "angularVelocity": 1.5174796030816139e-16,
-            "velocityX": 2.708497475030379e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 1.450372480306604
-        },
-        {
-            "x": 7.68,
-            "y": 2.2981090351202424,
-            "heading": -9.881529062229235e-18,
-            "angularVelocity": -7.421243963841227e-17,
-            "velocityX": 2.3345387345088647e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 1.547063978993711
-        },
-        {
-            "x": 7.68,
-            "y": 1.9323308421442913,
-            "heading": -6.5997498705085955e-18,
-            "angularVelocity": 3.39407211211041e-17,
-            "velocityX": -6.072343928076645e-20,
-            "velocityY": -3.782940568121787,
-            "timestamp": 1.643755477680818
-        },
-        {
-            "x": 7.68,
-            "y": 1.5665526491683786,
-            "heading": 6.487893960505722e-19,
-            "angularVelocity": 7.496563157826373e-17,
-            "velocityX": 5.228734565126193e-20,
-            "velocityY": -3.782940568121389,
-            "timestamp": 1.7404469763679251
-        },
-        {
-            "x": 7.68,
-            "y": 1.254731607449781,
-            "heading": 3.9009642634108723e-19,
-            "angularVelocity": -2.6754468948553036e-18,
-            "velocityX": 1.8077268140623735e-21,
-            "velocityY": -3.224906490772764,
-            "timestamp": 1.8371384750550321
-        },
-        {
-            "x": 7.68,
-            "y": 1.020865808366814,
-            "heading": 1.9475770164363563e-19,
-            "angularVelocity": -2.020226466917104e-18,
-            "velocityX": 1.3557598034014801e-21,
-            "velocityY": -2.418680052108357,
-            "timestamp": 1.9338299737421392
-        },
-        {
-            "x": 7.68,
-            "y": 0.8649552704871435,
-            "heading": 6.488190339479676e-20,
-            "angularVelocity": -1.3431976959152128e-18,
-            "velocityX": 9.038296351119003e-22,
-            "velocityY": -1.6124534214139752,
-            "timestamp": 2.030521472429246
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -6.710197305720325e-19,
-            "velocityX": 4.519117474728098e-22,
-            "velocityY": -0.8062267267095136,
-            "timestamp": 2.127212971116353
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 1.0656726462878492e-32,
-            "velocityY": 0,
-            "timestamp": 2.2239044698034602
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.1.traj
deleted file mode 100644
index ce3947a6..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -7.649625564275916e-33,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.617797764168564,
-            "y": 7.446201717117849,
-            "heading": 0.013442479376395878,
-            "angularVelocity": 0.15144225686963014,
-            "velocityX": -0.7007670767707749,
-            "velocityY": -0.23431234749607086,
-            "timestamp": 0.08876306820531243
-        },
-        {
-            "x": 7.493368305965553,
-            "y": 7.404602586388856,
-            "heading": 0.03870227011836747,
-            "angularVelocity": 0.28457545746724416,
-            "velocityX": -1.401815650572125,
-            "velocityY": -0.4686535917478179,
-            "timestamp": 0.17752613641062487
-        },
-        {
-            "x": 7.30667604944407,
-            "y": 7.3422008161122045,
-            "heading": 0.0730276629215222,
-            "angularVelocity": 0.38670804756608035,
-            "velocityX": -2.1032650210965835,
-            "velocityY": -0.7030150212056434,
-            "timestamp": 0.2662892046159373
-        },
-        {
-            "x": 7.057674574154849,
-            "y": 7.259002273186937,
-            "heading": 0.11075765125141449,
-            "angularVelocity": 0.42506403948785515,
-            "velocityX": -2.8052373619302045,
-            "velocityY": -0.9373103544914424,
-            "timestamp": 0.35505227282124974
-        },
-        {
-            "x": 6.746464870184761,
-            "y": 7.155111739606062,
-            "heading": 0.13339737013954753,
-            "angularVelocity": 0.25505786749611997,
-            "velocityX": -3.5060719538216505,
-            "velocityY": -1.170425219424191,
-            "timestamp": 0.44381534102656217
-        },
-        {
-            "x": 6.428047828205506,
-            "y": 7.048517057389171,
-            "heading": 0.13339738106725169,
-            "angularVelocity": 1.2311157711164836e-7,
-            "velocityX": -3.5872694400628884,
-            "velocityY": -1.2008900139638257,
-            "timestamp": 0.5325784092318746
-        },
-        {
-            "x": 6.109630786224927,
-            "y": 6.941922375176235,
-            "heading": 0.1333973919949535,
-            "angularVelocity": 1.23111550991103e-7,
-            "velocityX": -3.587269440077816,
-            "velocityY": -1.200890013919235,
-            "timestamp": 0.621341477437187
-        },
-        {
-            "x": 5.798381397539494,
-            "y": 6.838018370585249,
-            "heading": 0.15626352052401435,
-            "angularVelocity": 0.2576085864583032,
-            "velocityX": -3.5065190397107555,
-            "velocityY": -1.1705769831075976,
-            "timestamp": 0.7101045456424995
-        },
-        {
-            "x": 5.5493631968567065,
-            "y": 6.754814060290535,
-            "heading": 0.1928079538664876,
-            "angularVelocity": 0.4117076401528937,
-            "velocityX": -2.8054257893230465,
-            "velocityY": -0.9373753293689796,
-            "timestamp": 0.7988676138478119
-        },
-        {
-            "x": 5.362653942099421,
-            "y": 6.692406067264648,
-            "heading": 0.22585011955614995,
-            "angularVelocity": 0.37225127926004886,
-            "velocityX": -2.1034565223155943,
-            "velocityY": -0.703085126367409,
-            "timestamp": 0.8876306820531243
-        },
-        {
-            "x": 5.238210383284219,
-            "y": 6.650801525067523,
-            "heading": 0.25010704549903756,
-            "angularVelocity": 0.2732772360607878,
-            "velocityX": -1.4019745073178984,
-            "velocityY": -0.4687145570597535,
-            "timestamp": 0.9763937502584368
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": 0.1452513388947291,
-            "velocityX": -0.7008588655398115,
-            "velocityY": -0.2343488737838604,
-            "timestamp": 1.0651568184637492
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": -2.2743517080702824e-31,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 1.1539198866690616
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.traj
deleted file mode 100644
index ce3947a6..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -7.649625564275916e-33,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.617797764168564,
-            "y": 7.446201717117849,
-            "heading": 0.013442479376395878,
-            "angularVelocity": 0.15144225686963014,
-            "velocityX": -0.7007670767707749,
-            "velocityY": -0.23431234749607086,
-            "timestamp": 0.08876306820531243
-        },
-        {
-            "x": 7.493368305965553,
-            "y": 7.404602586388856,
-            "heading": 0.03870227011836747,
-            "angularVelocity": 0.28457545746724416,
-            "velocityX": -1.401815650572125,
-            "velocityY": -0.4686535917478179,
-            "timestamp": 0.17752613641062487
-        },
-        {
-            "x": 7.30667604944407,
-            "y": 7.3422008161122045,
-            "heading": 0.0730276629215222,
-            "angularVelocity": 0.38670804756608035,
-            "velocityX": -2.1032650210965835,
-            "velocityY": -0.7030150212056434,
-            "timestamp": 0.2662892046159373
-        },
-        {
-            "x": 7.057674574154849,
-            "y": 7.259002273186937,
-            "heading": 0.11075765125141449,
-            "angularVelocity": 0.42506403948785515,
-            "velocityX": -2.8052373619302045,
-            "velocityY": -0.9373103544914424,
-            "timestamp": 0.35505227282124974
-        },
-        {
-            "x": 6.746464870184761,
-            "y": 7.155111739606062,
-            "heading": 0.13339737013954753,
-            "angularVelocity": 0.25505786749611997,
-            "velocityX": -3.5060719538216505,
-            "velocityY": -1.170425219424191,
-            "timestamp": 0.44381534102656217
-        },
-        {
-            "x": 6.428047828205506,
-            "y": 7.048517057389171,
-            "heading": 0.13339738106725169,
-            "angularVelocity": 1.2311157711164836e-7,
-            "velocityX": -3.5872694400628884,
-            "velocityY": -1.2008900139638257,
-            "timestamp": 0.5325784092318746
-        },
-        {
-            "x": 6.109630786224927,
-            "y": 6.941922375176235,
-            "heading": 0.1333973919949535,
-            "angularVelocity": 1.23111550991103e-7,
-            "velocityX": -3.587269440077816,
-            "velocityY": -1.200890013919235,
-            "timestamp": 0.621341477437187
-        },
-        {
-            "x": 5.798381397539494,
-            "y": 6.838018370585249,
-            "heading": 0.15626352052401435,
-            "angularVelocity": 0.2576085864583032,
-            "velocityX": -3.5065190397107555,
-            "velocityY": -1.1705769831075976,
-            "timestamp": 0.7101045456424995
-        },
-        {
-            "x": 5.5493631968567065,
-            "y": 6.754814060290535,
-            "heading": 0.1928079538664876,
-            "angularVelocity": 0.4117076401528937,
-            "velocityX": -2.8054257893230465,
-            "velocityY": -0.9373753293689796,
-            "timestamp": 0.7988676138478119
-        },
-        {
-            "x": 5.362653942099421,
-            "y": 6.692406067264648,
-            "heading": 0.22585011955614995,
-            "angularVelocity": 0.37225127926004886,
-            "velocityX": -2.1034565223155943,
-            "velocityY": -0.703085126367409,
-            "timestamp": 0.8876306820531243
-        },
-        {
-            "x": 5.238210383284219,
-            "y": 6.650801525067523,
-            "heading": 0.25010704549903756,
-            "angularVelocity": 0.2732772360607878,
-            "velocityX": -1.4019745073178984,
-            "velocityY": -0.4687145570597535,
-            "timestamp": 0.9763937502584368
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": 0.1452513388947291,
-            "velocityX": -0.7008588655398115,
-            "velocityY": -0.2343488737838604,
-            "timestamp": 1.0651568184637492
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": -2.2743517080702824e-31,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 1.1539198866690616
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.1.traj
deleted file mode 100644
index f95228f2..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -6.443666081756986e-29,
-            "angularVelocity": 1.4558689567742243e-28,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.593939912495806,
-            "y": 7.445359803187238,
-            "heading": -0.04618112566514864,
-            "angularVelocity": -0.44533178373248467,
-            "velocityX": -0.8298908206418381,
-            "velocityY": -0.2086797865609081,
-            "timestamp": 0.10370049332228601
-        },
-        {
-            "x": 7.421570715628637,
-            "y": 7.402025139488172,
-            "heading": -0.13159426902714605,
-            "angularVelocity": -0.823652237569843,
-            "velocityX": -1.662182997832739,
-            "velocityY": -0.41788290788915955,
-            "timestamp": 0.20740098664457202
-        },
-        {
-            "x": 7.162505124622784,
-            "y": 7.336932458231965,
-            "heading": -0.24235041158328322,
-            "angularVelocity": -1.0680387239038096,
-            "velocityX": -2.4982098224038025,
-            "velocityY": -0.627698858229146,
-            "timestamp": 0.311101479966858
-        },
-        {
-            "x": 6.816624735331939,
-            "y": 7.250238713519174,
-            "heading": -0.3315039987658666,
-            "angularVelocity": -0.8597219196007858,
-            "velocityX": -3.3353784365894983,
-            "velocityY": -0.8360012757447058,
-            "timestamp": 0.41480197328914403
-        },
-        {
-            "x": 6.436297385949381,
-            "y": 7.154089090014683,
-            "heading": -0.3315040257643058,
-            "angularVelocity": -2.603501519346576e-7,
-            "velocityX": -3.6675558350581356,
-            "velocityY": -0.927185786914993,
-            "timestamp": 0.5185024666114301
-        },
-        {
-            "x": 6.055970043958053,
-            "y": 7.05793943727301,
-            "heading": -0.33150405276269507,
-            "angularVelocity": -2.60349670609762e-7,
-            "velocityX": -3.6675557637833287,
-            "velocityY": -0.9271860688537472,
-            "timestamp": 0.6222029599337161
-        },
-        {
-            "x": 5.675642817406181,
-            "y": 6.961789327902759,
-            "heading": -0.33150407976108204,
-            "angularVelocity": -2.6034964813703677e-7,
-            "velocityX": -3.6675546505826837,
-            "velocityY": -0.9271904721942118,
-            "timestamp": 0.7259034532560021
-        },
-        {
-            "x": 5.306931756724127,
-            "y": 6.827827408155551,
-            "heading": -0.331504107569482,
-            "angularVelocity": -2.6816073038858223e-7,
-            "velocityX": -3.5555381548297533,
-            "velocityY": -1.2918156457642633,
-            "timestamp": 0.8296039465782882
-        },
-        {
-            "x": 4.97827117667767,
-            "y": 6.613640897314209,
-            "heading": -0.3315041419146958,
-            "angularVelocity": -3.311962425493206e-7,
-            "velocityX": -3.1693251354651193,
-            "velocityY": -2.065433866121419,
-            "timestamp": 0.9333044399005742
-        },
-        {
-            "x": 4.706831845168329,
-            "y": 6.330419557819514,
-            "heading": -0.3315041418010325,
-            "angularVelocity": 1.0960729474265148e-9,
-            "velocityX": -2.6175317282796997,
-            "velocityY": -2.7311474653691747,
-            "timestamp": 1.0370049332228601
-        },
-        {
-            "x": 4.518922560808715,
-            "y": 6.028421366315536,
-            "heading": -0.2325817741817243,
-            "angularVelocity": 0.9539237900428917,
-            "velocityX": -1.8120384806234937,
-            "velocityY": -2.912215572257732,
-            "timestamp": 1.1407054265451462
-        },
-        {
-            "x": 4.388098262013575,
-            "y": 5.795527878516759,
-            "heading": -0.12496957799500227,
-            "angularVelocity": 1.0377211596504115,
-            "velocityX": -1.2615590784949995,
-            "velocityY": -2.245828157007679,
-            "timestamp": 1.2444059198674322
-        },
-        {
-            "x": 4.304052100915554,
-            "y": 5.638778154553602,
-            "heading": -0.0435889655527213,
-            "angularVelocity": 0.7847659141732163,
-            "velocityX": -0.8104702148022851,
-            "velocityY": -1.5115619891604988,
-            "timestamp": 1.3481064131897182
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 7.875266725539043e-28,
-            "angularVelocity": 0.42033518025077415,
-            "velocityX": -0.39587179964482383,
-            "velocityY": -0.7596700076322139,
-            "timestamp": 1.4518069065120043
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -1.3117957600781845e-26,
-            "velocityX": -2.8963805377195657e-28,
-            "velocityY": 0,
-            "timestamp": 1.5555073998342903
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.traj
deleted file mode 100644
index f95228f2..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -6.443666081756986e-29,
-            "angularVelocity": 1.4558689567742243e-28,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.593939912495806,
-            "y": 7.445359803187238,
-            "heading": -0.04618112566514864,
-            "angularVelocity": -0.44533178373248467,
-            "velocityX": -0.8298908206418381,
-            "velocityY": -0.2086797865609081,
-            "timestamp": 0.10370049332228601
-        },
-        {
-            "x": 7.421570715628637,
-            "y": 7.402025139488172,
-            "heading": -0.13159426902714605,
-            "angularVelocity": -0.823652237569843,
-            "velocityX": -1.662182997832739,
-            "velocityY": -0.41788290788915955,
-            "timestamp": 0.20740098664457202
-        },
-        {
-            "x": 7.162505124622784,
-            "y": 7.336932458231965,
-            "heading": -0.24235041158328322,
-            "angularVelocity": -1.0680387239038096,
-            "velocityX": -2.4982098224038025,
-            "velocityY": -0.627698858229146,
-            "timestamp": 0.311101479966858
-        },
-        {
-            "x": 6.816624735331939,
-            "y": 7.250238713519174,
-            "heading": -0.3315039987658666,
-            "angularVelocity": -0.8597219196007858,
-            "velocityX": -3.3353784365894983,
-            "velocityY": -0.8360012757447058,
-            "timestamp": 0.41480197328914403
-        },
-        {
-            "x": 6.436297385949381,
-            "y": 7.154089090014683,
-            "heading": -0.3315040257643058,
-            "angularVelocity": -2.603501519346576e-7,
-            "velocityX": -3.6675558350581356,
-            "velocityY": -0.927185786914993,
-            "timestamp": 0.5185024666114301
-        },
-        {
-            "x": 6.055970043958053,
-            "y": 7.05793943727301,
-            "heading": -0.33150405276269507,
-            "angularVelocity": -2.60349670609762e-7,
-            "velocityX": -3.6675557637833287,
-            "velocityY": -0.9271860688537472,
-            "timestamp": 0.6222029599337161
-        },
-        {
-            "x": 5.675642817406181,
-            "y": 6.961789327902759,
-            "heading": -0.33150407976108204,
-            "angularVelocity": -2.6034964813703677e-7,
-            "velocityX": -3.6675546505826837,
-            "velocityY": -0.9271904721942118,
-            "timestamp": 0.7259034532560021
-        },
-        {
-            "x": 5.306931756724127,
-            "y": 6.827827408155551,
-            "heading": -0.331504107569482,
-            "angularVelocity": -2.6816073038858223e-7,
-            "velocityX": -3.5555381548297533,
-            "velocityY": -1.2918156457642633,
-            "timestamp": 0.8296039465782882
-        },
-        {
-            "x": 4.97827117667767,
-            "y": 6.613640897314209,
-            "heading": -0.3315041419146958,
-            "angularVelocity": -3.311962425493206e-7,
-            "velocityX": -3.1693251354651193,
-            "velocityY": -2.065433866121419,
-            "timestamp": 0.9333044399005742
-        },
-        {
-            "x": 4.706831845168329,
-            "y": 6.330419557819514,
-            "heading": -0.3315041418010325,
-            "angularVelocity": 1.0960729474265148e-9,
-            "velocityX": -2.6175317282796997,
-            "velocityY": -2.7311474653691747,
-            "timestamp": 1.0370049332228601
-        },
-        {
-            "x": 4.518922560808715,
-            "y": 6.028421366315536,
-            "heading": -0.2325817741817243,
-            "angularVelocity": 0.9539237900428917,
-            "velocityX": -1.8120384806234937,
-            "velocityY": -2.912215572257732,
-            "timestamp": 1.1407054265451462
-        },
-        {
-            "x": 4.388098262013575,
-            "y": 5.795527878516759,
-            "heading": -0.12496957799500227,
-            "angularVelocity": 1.0377211596504115,
-            "velocityX": -1.2615590784949995,
-            "velocityY": -2.245828157007679,
-            "timestamp": 1.2444059198674322
-        },
-        {
-            "x": 4.304052100915554,
-            "y": 5.638778154553602,
-            "heading": -0.0435889655527213,
-            "angularVelocity": 0.7847659141732163,
-            "velocityX": -0.8104702148022851,
-            "velocityY": -1.5115619891604988,
-            "timestamp": 1.3481064131897182
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 7.875266725539043e-28,
-            "angularVelocity": 0.42033518025077415,
-            "velocityX": -0.39587179964482383,
-            "velocityY": -0.7596700076322139,
-            "timestamp": 1.4518069065120043
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -1.3117957600781845e-26,
-            "velocityX": -2.8963805377195657e-28,
-            "velocityY": 0,
-            "timestamp": 1.5555073998342903
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S3.1.traj
deleted file mode 100644
index f3e004b9..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S3.1.traj	
+++ /dev/null
@@ -1,265 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 3.237981257278863e-31,
-            "velocityX": 3.6994687797817426e-32,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.663124715083084,
-            "y": 7.408544235319849,
-            "heading": 0.027485198347684953,
-            "angularVelocity": 0.3204918925418427,
-            "velocityX": -0.19677471240300645,
-            "velocityY": -0.6816250119548423,
-            "timestamp": 0.08575941852911824
-        },
-        {
-            "x": 7.629347887601235,
-            "y": 7.291532761279695,
-            "heading": 0.07929730291499706,
-            "angularVelocity": 0.6041564350126787,
-            "velocityX": -0.3938556028149893,
-            "velocityY": -1.3644154315298311,
-            "timestamp": 0.17151883705823648
-        },
-        {
-            "x": 7.578636110956943,
-            "y": 7.115823334781324,
-            "heading": 0.15015093083574435,
-            "angularVelocity": 0.8261906288076145,
-            "velocityX": -0.5913260317532774,
-            "velocityY": -2.048864480566788,
-            "timestamp": 0.25727825558735473
-        },
-        {
-            "x": 7.510961813450331,
-            "y": 6.881225838812792,
-            "heading": 0.22908991381513497,
-            "angularVelocity": 0.9204701283345157,
-            "velocityX": -0.7891179612374983,
-            "velocityY": -2.7355303941208287,
-            "timestamp": 0.34303767411647296
-        },
-        {
-            "x": 7.426596388399054,
-            "y": 6.588221528517866,
-            "heading": 0.2780284674480044,
-            "angularVelocity": 0.5706493172671544,
-            "velocityX": -0.9837453016618997,
-            "velocityY": -3.4165846191627605,
-            "timestamp": 0.4287970926455912
-        },
-        {
-            "x": 7.336364964950643,
-            "y": 6.276599275391096,
-            "heading": 0.2780284905344079,
-            "angularVelocity": 2.691996271350766e-7,
-            "velocityX": -1.0521459333096546,
-            "velocityY": -3.6336796409243712,
-            "timestamp": 0.5145565111747095
-        },
-        {
-            "x": 7.246133490581996,
-            "y": 5.964977037008454,
-            "heading": 0.27802851362075814,
-            "angularVelocity": 2.6919900573505555e-7,
-            "velocityX": -1.0521465270664079,
-            "velocityY": -3.633679469000075,
-            "timestamp": 0.6003159297038277
-        },
-        {
-            "x": 7.155901958204915,
-            "y": 5.653354815422387,
-            "heading": 0.2780285367070952,
-            "angularVelocity": 2.6919885218329885e-7,
-            "velocityX": -1.0521472034753196,
-            "velocityY": -3.633679273143183,
-            "timestamp": 0.686075348232946
-        },
-        {
-            "x": 7.06567006903972,
-            "y": 5.341732697146042,
-            "heading": 0.27802855979342844,
-            "angularVelocity": 2.6919880800587287e-7,
-            "velocityX": -1.052151363812688,
-            "velocityY": -3.6336780684973866,
-            "timestamp": 0.7718347667620643
-        },
-        {
-            "x": 6.941702537617662,
-            "y": 5.041929112902265,
-            "heading": 0.2780285835706332,
-            "angularVelocity": 2.772547333115093e-7,
-            "velocityX": -1.445526725207054,
-            "velocityY": -3.495867735413615,
-            "timestamp": 0.8575941852911826
-        },
-        {
-            "x": 6.763533066668612,
-            "y": 4.770809627708675,
-            "heading": 0.2780286115416881,
-            "angularVelocity": 3.2615723612466423e-7,
-            "velocityX": -2.0775498948673006,
-            "velocityY": -3.1613960290733045,
-            "timestamp": 0.9433536038203009
-        },
-        {
-            "x": 6.537527544905599,
-            "y": 4.538061625407243,
-            "heading": 0.2780286466069666,
-            "angularVelocity": 4.0887962009830865e-7,
-            "velocityX": -2.6353434484432303,
-            "velocityY": -2.713964323608448,
-            "timestamp": 1.0291130223494191
-        },
-        {
-            "x": 6.274440682462666,
-            "y": 4.354088811409382,
-            "heading": 0.26884633764727317,
-            "angularVelocity": -0.10707055991260324,
-            "velocityX": -3.0677314160380695,
-            "velocityY": -2.145219932145364,
-            "timestamp": 1.1148724408785373
-        },
-        {
-            "x": 5.982323590323029,
-            "y": 4.22411616098336,
-            "heading": 0.2547621599051439,
-            "angularVelocity": -0.16422893232822805,
-            "velocityX": -3.40623918806603,
-            "velocityY": -1.5155495764222278,
-            "timestamp": 1.2006318594076555
-        },
-        {
-            "x": 5.6751302181547745,
-            "y": 4.153390532297334,
-            "heading": 0.22346642432676106,
-            "angularVelocity": -0.3649247641266729,
-            "velocityX": -3.582036555716069,
-            "velocityY": -0.8246980902979518,
-            "timestamp": 1.2863912779367737
-        },
-        {
-            "x": 5.366531848907471,
-            "y": 4.105798244476318,
-            "heading": 0.18319904977282217,
-            "angularVelocity": -0.4695388010388101,
-            "velocityX": -3.5984195618411468,
-            "velocityY": -0.5549511486584576,
-            "timestamp": 1.3721506964658918
-        },
-        {
-            "x": 5.15360011207683,
-            "y": 4.041515222282991,
-            "heading": 0.15622154454817708,
-            "angularVelocity": -0.44121073940112593,
-            "velocityX": -3.482448368249538,
-            "velocityY": -1.051333676582719,
-            "timestamp": 1.4332949588681712
-        },
-        {
-            "x": 4.961585993321137,
-            "y": 3.955708543808847,
-            "heading": 0.09940090208560654,
-            "angularVelocity": -0.9292882149552651,
-            "velocityX": -3.1403456548775974,
-            "velocityY": -1.403348001969601,
-            "timestamp": 1.4944392212704505
-        },
-        {
-            "x": 4.792433392014809,
-            "y": 3.849704139398821,
-            "heading": 0.01980990286006859,
-            "angularVelocity": -1.301692032882722,
-            "velocityX": -2.766450925410501,
-            "velocityY": -1.7336770490843985,
-            "timestamp": 1.5555834836727298
-        },
-        {
-            "x": 4.64716063510186,
-            "y": 3.7242164869664776,
-            "heading": -0.07646566712737501,
-            "angularVelocity": -1.5745642551712946,
-            "velocityX": -2.3759016987918233,
-            "velocityY": -2.0523209783239644,
-            "timestamp": 1.6167277460750091
-        },
-        {
-            "x": 4.526373969972362,
-            "y": 3.5797426645365276,
-            "heading": -0.18496768957846801,
-            "angularVelocity": -1.7745250034621045,
-            "velocityX": -1.975437438999914,
-            "velocityY": -2.362835313629761,
-            "timestamp": 1.6778720084772885
-        },
-        {
-            "x": 4.435318488505818,
-            "y": 3.4263170797911884,
-            "heading": -0.3033986089581413,
-            "angularVelocity": -1.936909772506439,
-            "velocityX": -1.489190937777188,
-            "velocityY": -2.5092392763841707,
-            "timestamp": 1.7390162708795678
-        },
-        {
-            "x": 4.37042278768838,
-            "y": 3.2883178140362905,
-            "heading": -0.39683380701899557,
-            "angularVelocity": -1.5281106417823267,
-            "velocityX": -1.0613538910728308,
-            "velocityY": -2.2569454652502787,
-            "timestamp": 1.800160533281847
-        },
-        {
-            "x": 4.3241368483863285,
-            "y": 3.1745112953889034,
-            "heading": -0.4672868225779202,
-            "angularVelocity": -1.1522424638210744,
-            "velocityX": -0.7569956277749857,
-            "velocityY": -1.8612787884925825,
-            "timestamp": 1.8613047956841264
-        },
-        {
-            "x": 4.292271472981756,
-            "y": 3.08778416242541,
-            "heading": -0.5177338397629753,
-            "angularVelocity": -0.8250490757931602,
-            "velocityX": -0.5211507041318677,
-            "velocityY": -1.418401818193496,
-            "timestamp": 1.9224490580864058
-        },
-        {
-            "x": 4.272405555044274,
-            "y": 3.0293931910282037,
-            "heading": -0.5502025950282511,
-            "angularVelocity": -0.5310188395381721,
-            "velocityX": -0.3249024055075056,
-            "velocityY": -0.9549705745576195,
-            "timestamp": 1.983593320488685
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -0.25836283489389134,
-            "velocityX": -0.15382563587721435,
-            "velocityY": -0.48071871134564137,
-            "timestamp": 2.0447375828909644
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 0,
-            "velocityX": 3.3381626772503296e-32,
-            "velocityY": -2.986960154995716e-30,
-            "timestamp": 2.105881845293244
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S3.traj
deleted file mode 100644
index f3e004b9..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S3.traj	
+++ /dev/null
@@ -1,265 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 3.237981257278863e-31,
-            "velocityX": 3.6994687797817426e-32,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.663124715083084,
-            "y": 7.408544235319849,
-            "heading": 0.027485198347684953,
-            "angularVelocity": 0.3204918925418427,
-            "velocityX": -0.19677471240300645,
-            "velocityY": -0.6816250119548423,
-            "timestamp": 0.08575941852911824
-        },
-        {
-            "x": 7.629347887601235,
-            "y": 7.291532761279695,
-            "heading": 0.07929730291499706,
-            "angularVelocity": 0.6041564350126787,
-            "velocityX": -0.3938556028149893,
-            "velocityY": -1.3644154315298311,
-            "timestamp": 0.17151883705823648
-        },
-        {
-            "x": 7.578636110956943,
-            "y": 7.115823334781324,
-            "heading": 0.15015093083574435,
-            "angularVelocity": 0.8261906288076145,
-            "velocityX": -0.5913260317532774,
-            "velocityY": -2.048864480566788,
-            "timestamp": 0.25727825558735473
-        },
-        {
-            "x": 7.510961813450331,
-            "y": 6.881225838812792,
-            "heading": 0.22908991381513497,
-            "angularVelocity": 0.9204701283345157,
-            "velocityX": -0.7891179612374983,
-            "velocityY": -2.7355303941208287,
-            "timestamp": 0.34303767411647296
-        },
-        {
-            "x": 7.426596388399054,
-            "y": 6.588221528517866,
-            "heading": 0.2780284674480044,
-            "angularVelocity": 0.5706493172671544,
-            "velocityX": -0.9837453016618997,
-            "velocityY": -3.4165846191627605,
-            "timestamp": 0.4287970926455912
-        },
-        {
-            "x": 7.336364964950643,
-            "y": 6.276599275391096,
-            "heading": 0.2780284905344079,
-            "angularVelocity": 2.691996271350766e-7,
-            "velocityX": -1.0521459333096546,
-            "velocityY": -3.6336796409243712,
-            "timestamp": 0.5145565111747095
-        },
-        {
-            "x": 7.246133490581996,
-            "y": 5.964977037008454,
-            "heading": 0.27802851362075814,
-            "angularVelocity": 2.6919900573505555e-7,
-            "velocityX": -1.0521465270664079,
-            "velocityY": -3.633679469000075,
-            "timestamp": 0.6003159297038277
-        },
-        {
-            "x": 7.155901958204915,
-            "y": 5.653354815422387,
-            "heading": 0.2780285367070952,
-            "angularVelocity": 2.6919885218329885e-7,
-            "velocityX": -1.0521472034753196,
-            "velocityY": -3.633679273143183,
-            "timestamp": 0.686075348232946
-        },
-        {
-            "x": 7.06567006903972,
-            "y": 5.341732697146042,
-            "heading": 0.27802855979342844,
-            "angularVelocity": 2.6919880800587287e-7,
-            "velocityX": -1.052151363812688,
-            "velocityY": -3.6336780684973866,
-            "timestamp": 0.7718347667620643
-        },
-        {
-            "x": 6.941702537617662,
-            "y": 5.041929112902265,
-            "heading": 0.2780285835706332,
-            "angularVelocity": 2.772547333115093e-7,
-            "velocityX": -1.445526725207054,
-            "velocityY": -3.495867735413615,
-            "timestamp": 0.8575941852911826
-        },
-        {
-            "x": 6.763533066668612,
-            "y": 4.770809627708675,
-            "heading": 0.2780286115416881,
-            "angularVelocity": 3.2615723612466423e-7,
-            "velocityX": -2.0775498948673006,
-            "velocityY": -3.1613960290733045,
-            "timestamp": 0.9433536038203009
-        },
-        {
-            "x": 6.537527544905599,
-            "y": 4.538061625407243,
-            "heading": 0.2780286466069666,
-            "angularVelocity": 4.0887962009830865e-7,
-            "velocityX": -2.6353434484432303,
-            "velocityY": -2.713964323608448,
-            "timestamp": 1.0291130223494191
-        },
-        {
-            "x": 6.274440682462666,
-            "y": 4.354088811409382,
-            "heading": 0.26884633764727317,
-            "angularVelocity": -0.10707055991260324,
-            "velocityX": -3.0677314160380695,
-            "velocityY": -2.145219932145364,
-            "timestamp": 1.1148724408785373
-        },
-        {
-            "x": 5.982323590323029,
-            "y": 4.22411616098336,
-            "heading": 0.2547621599051439,
-            "angularVelocity": -0.16422893232822805,
-            "velocityX": -3.40623918806603,
-            "velocityY": -1.5155495764222278,
-            "timestamp": 1.2006318594076555
-        },
-        {
-            "x": 5.6751302181547745,
-            "y": 4.153390532297334,
-            "heading": 0.22346642432676106,
-            "angularVelocity": -0.3649247641266729,
-            "velocityX": -3.582036555716069,
-            "velocityY": -0.8246980902979518,
-            "timestamp": 1.2863912779367737
-        },
-        {
-            "x": 5.366531848907471,
-            "y": 4.105798244476318,
-            "heading": 0.18319904977282217,
-            "angularVelocity": -0.4695388010388101,
-            "velocityX": -3.5984195618411468,
-            "velocityY": -0.5549511486584576,
-            "timestamp": 1.3721506964658918
-        },
-        {
-            "x": 5.15360011207683,
-            "y": 4.041515222282991,
-            "heading": 0.15622154454817708,
-            "angularVelocity": -0.44121073940112593,
-            "velocityX": -3.482448368249538,
-            "velocityY": -1.051333676582719,
-            "timestamp": 1.4332949588681712
-        },
-        {
-            "x": 4.961585993321137,
-            "y": 3.955708543808847,
-            "heading": 0.09940090208560654,
-            "angularVelocity": -0.9292882149552651,
-            "velocityX": -3.1403456548775974,
-            "velocityY": -1.403348001969601,
-            "timestamp": 1.4944392212704505
-        },
-        {
-            "x": 4.792433392014809,
-            "y": 3.849704139398821,
-            "heading": 0.01980990286006859,
-            "angularVelocity": -1.301692032882722,
-            "velocityX": -2.766450925410501,
-            "velocityY": -1.7336770490843985,
-            "timestamp": 1.5555834836727298
-        },
-        {
-            "x": 4.64716063510186,
-            "y": 3.7242164869664776,
-            "heading": -0.07646566712737501,
-            "angularVelocity": -1.5745642551712946,
-            "velocityX": -2.3759016987918233,
-            "velocityY": -2.0523209783239644,
-            "timestamp": 1.6167277460750091
-        },
-        {
-            "x": 4.526373969972362,
-            "y": 3.5797426645365276,
-            "heading": -0.18496768957846801,
-            "angularVelocity": -1.7745250034621045,
-            "velocityX": -1.975437438999914,
-            "velocityY": -2.362835313629761,
-            "timestamp": 1.6778720084772885
-        },
-        {
-            "x": 4.435318488505818,
-            "y": 3.4263170797911884,
-            "heading": -0.3033986089581413,
-            "angularVelocity": -1.936909772506439,
-            "velocityX": -1.489190937777188,
-            "velocityY": -2.5092392763841707,
-            "timestamp": 1.7390162708795678
-        },
-        {
-            "x": 4.37042278768838,
-            "y": 3.2883178140362905,
-            "heading": -0.39683380701899557,
-            "angularVelocity": -1.5281106417823267,
-            "velocityX": -1.0613538910728308,
-            "velocityY": -2.2569454652502787,
-            "timestamp": 1.800160533281847
-        },
-        {
-            "x": 4.3241368483863285,
-            "y": 3.1745112953889034,
-            "heading": -0.4672868225779202,
-            "angularVelocity": -1.1522424638210744,
-            "velocityX": -0.7569956277749857,
-            "velocityY": -1.8612787884925825,
-            "timestamp": 1.8613047956841264
-        },
-        {
-            "x": 4.292271472981756,
-            "y": 3.08778416242541,
-            "heading": -0.5177338397629753,
-            "angularVelocity": -0.8250490757931602,
-            "velocityX": -0.5211507041318677,
-            "velocityY": -1.418401818193496,
-            "timestamp": 1.9224490580864058
-        },
-        {
-            "x": 4.272405555044274,
-            "y": 3.0293931910282037,
-            "heading": -0.5502025950282511,
-            "angularVelocity": -0.5310188395381721,
-            "velocityX": -0.3249024055075056,
-            "velocityY": -0.9549705745576195,
-            "timestamp": 1.983593320488685
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -0.25836283489389134,
-            "velocityX": -0.15382563587721435,
-            "velocityY": -0.48071871134564137,
-            "timestamp": 2.0447375828909644
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 0,
-            "velocityX": 3.3381626772503296e-32,
-            "velocityY": -2.986960154995716e-30,
-            "timestamp": 2.105881845293244
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S4.1.traj
deleted file mode 100644
index 6d9a6add..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S4.1.traj	
+++ /dev/null
@@ -1,301 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 9.776523821034258e-32,
-            "velocityX": -4.560355515853987e-33,
-            "velocityY": 2.3850698853192112e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 7.67836355889096,
-            "y": 7.400720406813562,
-            "heading": -0.028945523101477338,
-            "angularVelocity": -0.3234212568317029,
-            "velocityX": -0.018284687354286195,
-            "velocityY": -0.7405714954779761,
-            "timestamp": 0.0894978993806123
-        },
-        {
-            "x": 7.675074058245281,
-            "y": 7.268033905727148,
-            "heading": -0.0827237859595526,
-            "angularVelocity": -0.6008885485609575,
-            "velocityX": -0.03675505982201489,
-            "velocityY": -1.4825655351097347,
-            "timestamp": 0.1789957987612246
-        },
-        {
-            "x": 7.670107822881543,
-            "y": 7.068767684711926,
-            "heading": -0.1543450175390465,
-            "angularVelocity": -0.8002560068466026,
-            "velocityX": -0.05548996566520156,
-            "velocityY": -2.226490480718353,
-            "timestamp": 0.2684936981418369
-        },
-        {
-            "x": 7.663417838093003,
-            "y": 6.802742204879413,
-            "heading": -0.22912917227010834,
-            "angularVelocity": -0.8355967597967521,
-            "velocityX": -0.07475018782384167,
-            "velocityY": -2.9724214945109684,
-            "timestamp": 0.3579915975224492
-        },
-        {
-            "x": 7.654753648982881,
-            "y": 6.471615180012679,
-            "heading": -0.2502157544796387,
-            "angularVelocity": -0.23560980040272358,
-            "velocityX": -0.0968088543986332,
-            "velocityY": -3.6998301318619107,
-            "timestamp": 0.4474894969030615
-        },
-        {
-            "x": 7.646619499365586,
-            "y": 6.133147678353669,
-            "heading": -0.250215779137968,
-            "angularVelocity": -2.7551852588992195e-7,
-            "velocityX": -0.09088648642693918,
-            "velocityY": -3.78184855735655,
-            "timestamp": 0.5369873962836738
-        },
-        {
-            "x": 7.638485287213415,
-            "y": 5.7946801781975115,
-            "heading": -0.25021580379628167,
-            "angularVelocity": -2.755183512776458e-7,
-            "velocityX": -0.09088718515702116,
-            "velocityY": -3.7818485405645093,
-            "timestamp": 0.626485295664286
-        },
-        {
-            "x": 7.630351002745582,
-            "y": 5.456212679779282,
-            "heading": -0.2502158284545949,
-            "angularVelocity": -2.7551834628402704e-7,
-            "velocityX": -0.09088799317222593,
-            "velocityY": -3.781848521145859,
-            "timestamp": 0.7159831950448983
-        },
-        {
-            "x": 7.622216641973054,
-            "y": 5.117745183194865,
-            "heading": -0.25021585311291117,
-            "angularVelocity": -2.7551838019723297e-7,
-            "velocityX": -0.09088884575864668,
-            "velocityY": -3.781848500655846,
-            "timestamp": 0.8054810944255105
-        },
-        {
-            "x": 7.614082202627753,
-            "y": 4.779277688498788,
-            "heading": -0.25021587777122717,
-            "angularVelocity": -2.755183772950637e-7,
-            "velocityX": -0.09088972368733664,
-            "velocityY": -3.7818484795565834,
-            "timestamp": 0.8949789938061228
-        },
-        {
-            "x": 7.605947683187911,
-            "y": 4.440810195727643,
-            "heading": -0.2502159024295383,
-            "angularVelocity": -2.755183223713384e-7,
-            "velocityX": -0.09089061861940764,
-            "velocityY": -3.7818484580484664,
-            "timestamp": 0.984476893186735
-        },
-        {
-            "x": 7.597813082423135,
-            "y": 4.1023427049110195,
-            "heading": -0.25021592708784146,
-            "angularVelocity": -2.755182337798815e-7,
-            "velocityX": -0.09089152729921718,
-            "velocityY": -3.781848436209727,
-            "timestamp": 1.0739747925673473
-        },
-        {
-            "x": 7.589678399478048,
-            "y": 3.7638752160694957,
-            "heading": -0.2502159517461411,
-            "angularVelocity": -2.755181940361221e-7,
-            "velocityX": -0.09089244553653594,
-            "velocityY": -3.781848414141052,
-            "timestamp": 1.1634726919479597
-        },
-        {
-            "x": 7.581543633206817,
-            "y": 3.4254077292306317,
-            "heading": -0.2502159764044425,
-            "angularVelocity": -2.7551821391136536e-7,
-            "velocityX": -0.09089337657676429,
-            "velocityY": -3.7818483917644334,
-            "timestamp": 1.252970591328572
-        },
-        {
-            "x": 7.573407384740196,
-            "y": 3.0869402780182864,
-            "heading": -0.2502160010627749,
-            "angularVelocity": -2.7551856112041726e-7,
-            "velocityX": -0.09090993780780313,
-            "velocityY": -3.7818479936934355,
-            "timestamp": 1.3424684907091844
-        },
-        {
-            "x": 7.507089727245593,
-            "y": 2.754933679576005,
-            "heading": -0.25021602759782946,
-            "angularVelocity": -2.964880153822211e-7,
-            "velocityX": -0.7409968049928212,
-            "velocityY": -3.709658000243552,
-            "timestamp": 1.4319663900897968
-        },
-        {
-            "x": 7.3768883522731565,
-            "y": 2.4424052627551904,
-            "heading": -0.2502160612558337,
-            "angularVelocity": -3.7607591242222915e-7,
-            "velocityX": -1.454798111168213,
-            "velocityY": -3.492019577931195,
-            "timestamp": 1.5214642894704091
-        },
-        {
-            "x": 7.1878697455519145,
-            "y": 2.1615167326419513,
-            "heading": -0.2502161148657548,
-            "angularVelocity": -5.990075908840489e-7,
-            "velocityX": -2.111989309574672,
-            "velocityY": -3.1384929932119427,
-            "timestamp": 1.6109621888510215
-        },
-        {
-            "x": 6.94738938516625,
-            "y": 1.9231986374023808,
-            "heading": -0.25021645217340494,
-            "angularVelocity": -0.000003768889018580663,
-            "velocityX": -2.686994466350113,
-            "velocityY": -2.6628345121941095,
-            "timestamp": 1.7004600882316339
-        },
-        {
-            "x": 6.667655944824219,
-            "y": 1.738860011100769,
-            "heading": -0.25893161706240975,
-            "angularVelocity": -0.09737842954215001,
-            "velocityX": -3.125586659329227,
-            "velocityY": -2.059697798242895,
-            "timestamp": 1.7899579876122462
-        },
-        {
-            "x": 6.480451878153193,
-            "y": 1.646736643588724,
-            "heading": -0.268441983388845,
-            "angularVelocity": -0.16928620019223647,
-            "velocityX": -3.3322654479868237,
-            "velocityY": -1.6398122111953808,
-            "timestamp": 1.846137205502623
-        },
-        {
-            "x": 6.284980922842322,
-            "y": 1.5795583069683912,
-            "heading": -0.28291661109989463,
-            "angularVelocity": -0.25765092955355146,
-            "velocityX": -3.47941752575298,
-            "velocityY": -1.1957862558966221,
-            "timestamp": 1.9023164233929999
-        },
-        {
-            "x": 6.086191677834544,
-            "y": 1.5383458560749974,
-            "heading": -0.3070406119029933,
-            "angularVelocity": -0.429411474723066,
-            "velocityX": -3.53848366838567,
-            "velocityY": -0.7335889042423714,
-            "timestamp": 1.9584956412833767
-        },
-        {
-            "x": 5.891572555778559,
-            "y": 1.5225106097486452,
-            "heading": -0.35143250880916643,
-            "angularVelocity": -0.7901836047765775,
-            "velocityX": -3.4642547433776456,
-            "velocityY": -0.2818701811985366,
-            "timestamp": 2.0146748591737533
-        },
-        {
-            "x": 5.713348199791213,
-            "y": 1.5270062964148978,
-            "heading": -0.407385332387855,
-            "angularVelocity": -0.9959701412692558,
-            "velocityX": -3.172425011952234,
-            "velocityY": 0.08002401662169539,
-            "timestamp": 2.07085407706413
-        },
-        {
-            "x": 5.558872974287929,
-            "y": 1.5428236796222032,
-            "heading": -0.4653888399418059,
-            "angularVelocity": -1.0324726781908529,
-            "velocityX": -2.7496862951120815,
-            "velocityY": 0.2815522145247918,
-            "timestamp": 2.1270332949545065
-        },
-        {
-            "x": 5.4303297125098124,
-            "y": 1.5629685460150702,
-            "heading": -0.519265179295127,
-            "angularVelocity": -0.9590083553396718,
-            "velocityX": -2.2880927610801525,
-            "velocityY": 0.35858217948453525,
-            "timestamp": 2.183212512844883
-        },
-        {
-            "x": 5.3280145107455015,
-            "y": 1.583083454211334,
-            "heading": -0.5654730488836437,
-            "angularVelocity": -0.8225082392332849,
-            "velocityX": -1.8212286608183241,
-            "velocityY": 0.3580489182942135,
-            "timestamp": 2.2393917307352598
-        },
-        {
-            "x": 5.251723840078425,
-            "y": 1.6004232571954151,
-            "heading": -0.6018676415285119,
-            "angularVelocity": -0.6478301765590008,
-            "velocityX": -1.3579874112157035,
-            "velocityY": 0.3086515554188953,
-            "timestamp": 2.2955709486256364
-        },
-        {
-            "x": 5.20115218978415,
-            "y": 1.613158164546165,
-            "heading": -0.6270407807035692,
-            "angularVelocity": -0.44808632302748147,
-            "velocityX": -0.9001843064628601,
-            "velocityY": 0.22668359989631479,
-            "timestamp": 2.351750166516013
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -0.2306763921441399,
-            "velocityX": -0.44771342016952703,
-            "velocityY": 0.12178587938311458,
-            "timestamp": 2.4079293844063896
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 3.4924263364045203e-31,
-            "velocityX": 0,
-            "velocityY": 4.738614736798697e-32,
-            "timestamp": 2.464108602296766
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S4.traj
deleted file mode 100644
index 6d9a6add..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S4.traj	
+++ /dev/null
@@ -1,301 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 9.776523821034258e-32,
-            "velocityX": -4.560355515853987e-33,
-            "velocityY": 2.3850698853192112e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 7.67836355889096,
-            "y": 7.400720406813562,
-            "heading": -0.028945523101477338,
-            "angularVelocity": -0.3234212568317029,
-            "velocityX": -0.018284687354286195,
-            "velocityY": -0.7405714954779761,
-            "timestamp": 0.0894978993806123
-        },
-        {
-            "x": 7.675074058245281,
-            "y": 7.268033905727148,
-            "heading": -0.0827237859595526,
-            "angularVelocity": -0.6008885485609575,
-            "velocityX": -0.03675505982201489,
-            "velocityY": -1.4825655351097347,
-            "timestamp": 0.1789957987612246
-        },
-        {
-            "x": 7.670107822881543,
-            "y": 7.068767684711926,
-            "heading": -0.1543450175390465,
-            "angularVelocity": -0.8002560068466026,
-            "velocityX": -0.05548996566520156,
-            "velocityY": -2.226490480718353,
-            "timestamp": 0.2684936981418369
-        },
-        {
-            "x": 7.663417838093003,
-            "y": 6.802742204879413,
-            "heading": -0.22912917227010834,
-            "angularVelocity": -0.8355967597967521,
-            "velocityX": -0.07475018782384167,
-            "velocityY": -2.9724214945109684,
-            "timestamp": 0.3579915975224492
-        },
-        {
-            "x": 7.654753648982881,
-            "y": 6.471615180012679,
-            "heading": -0.2502157544796387,
-            "angularVelocity": -0.23560980040272358,
-            "velocityX": -0.0968088543986332,
-            "velocityY": -3.6998301318619107,
-            "timestamp": 0.4474894969030615
-        },
-        {
-            "x": 7.646619499365586,
-            "y": 6.133147678353669,
-            "heading": -0.250215779137968,
-            "angularVelocity": -2.7551852588992195e-7,
-            "velocityX": -0.09088648642693918,
-            "velocityY": -3.78184855735655,
-            "timestamp": 0.5369873962836738
-        },
-        {
-            "x": 7.638485287213415,
-            "y": 5.7946801781975115,
-            "heading": -0.25021580379628167,
-            "angularVelocity": -2.755183512776458e-7,
-            "velocityX": -0.09088718515702116,
-            "velocityY": -3.7818485405645093,
-            "timestamp": 0.626485295664286
-        },
-        {
-            "x": 7.630351002745582,
-            "y": 5.456212679779282,
-            "heading": -0.2502158284545949,
-            "angularVelocity": -2.7551834628402704e-7,
-            "velocityX": -0.09088799317222593,
-            "velocityY": -3.781848521145859,
-            "timestamp": 0.7159831950448983
-        },
-        {
-            "x": 7.622216641973054,
-            "y": 5.117745183194865,
-            "heading": -0.25021585311291117,
-            "angularVelocity": -2.7551838019723297e-7,
-            "velocityX": -0.09088884575864668,
-            "velocityY": -3.781848500655846,
-            "timestamp": 0.8054810944255105
-        },
-        {
-            "x": 7.614082202627753,
-            "y": 4.779277688498788,
-            "heading": -0.25021587777122717,
-            "angularVelocity": -2.755183772950637e-7,
-            "velocityX": -0.09088972368733664,
-            "velocityY": -3.7818484795565834,
-            "timestamp": 0.8949789938061228
-        },
-        {
-            "x": 7.605947683187911,
-            "y": 4.440810195727643,
-            "heading": -0.2502159024295383,
-            "angularVelocity": -2.755183223713384e-7,
-            "velocityX": -0.09089061861940764,
-            "velocityY": -3.7818484580484664,
-            "timestamp": 0.984476893186735
-        },
-        {
-            "x": 7.597813082423135,
-            "y": 4.1023427049110195,
-            "heading": -0.25021592708784146,
-            "angularVelocity": -2.755182337798815e-7,
-            "velocityX": -0.09089152729921718,
-            "velocityY": -3.781848436209727,
-            "timestamp": 1.0739747925673473
-        },
-        {
-            "x": 7.589678399478048,
-            "y": 3.7638752160694957,
-            "heading": -0.2502159517461411,
-            "angularVelocity": -2.755181940361221e-7,
-            "velocityX": -0.09089244553653594,
-            "velocityY": -3.781848414141052,
-            "timestamp": 1.1634726919479597
-        },
-        {
-            "x": 7.581543633206817,
-            "y": 3.4254077292306317,
-            "heading": -0.2502159764044425,
-            "angularVelocity": -2.7551821391136536e-7,
-            "velocityX": -0.09089337657676429,
-            "velocityY": -3.7818483917644334,
-            "timestamp": 1.252970591328572
-        },
-        {
-            "x": 7.573407384740196,
-            "y": 3.0869402780182864,
-            "heading": -0.2502160010627749,
-            "angularVelocity": -2.7551856112041726e-7,
-            "velocityX": -0.09090993780780313,
-            "velocityY": -3.7818479936934355,
-            "timestamp": 1.3424684907091844
-        },
-        {
-            "x": 7.507089727245593,
-            "y": 2.754933679576005,
-            "heading": -0.25021602759782946,
-            "angularVelocity": -2.964880153822211e-7,
-            "velocityX": -0.7409968049928212,
-            "velocityY": -3.709658000243552,
-            "timestamp": 1.4319663900897968
-        },
-        {
-            "x": 7.3768883522731565,
-            "y": 2.4424052627551904,
-            "heading": -0.2502160612558337,
-            "angularVelocity": -3.7607591242222915e-7,
-            "velocityX": -1.454798111168213,
-            "velocityY": -3.492019577931195,
-            "timestamp": 1.5214642894704091
-        },
-        {
-            "x": 7.1878697455519145,
-            "y": 2.1615167326419513,
-            "heading": -0.2502161148657548,
-            "angularVelocity": -5.990075908840489e-7,
-            "velocityX": -2.111989309574672,
-            "velocityY": -3.1384929932119427,
-            "timestamp": 1.6109621888510215
-        },
-        {
-            "x": 6.94738938516625,
-            "y": 1.9231986374023808,
-            "heading": -0.25021645217340494,
-            "angularVelocity": -0.000003768889018580663,
-            "velocityX": -2.686994466350113,
-            "velocityY": -2.6628345121941095,
-            "timestamp": 1.7004600882316339
-        },
-        {
-            "x": 6.667655944824219,
-            "y": 1.738860011100769,
-            "heading": -0.25893161706240975,
-            "angularVelocity": -0.09737842954215001,
-            "velocityX": -3.125586659329227,
-            "velocityY": -2.059697798242895,
-            "timestamp": 1.7899579876122462
-        },
-        {
-            "x": 6.480451878153193,
-            "y": 1.646736643588724,
-            "heading": -0.268441983388845,
-            "angularVelocity": -0.16928620019223647,
-            "velocityX": -3.3322654479868237,
-            "velocityY": -1.6398122111953808,
-            "timestamp": 1.846137205502623
-        },
-        {
-            "x": 6.284980922842322,
-            "y": 1.5795583069683912,
-            "heading": -0.28291661109989463,
-            "angularVelocity": -0.25765092955355146,
-            "velocityX": -3.47941752575298,
-            "velocityY": -1.1957862558966221,
-            "timestamp": 1.9023164233929999
-        },
-        {
-            "x": 6.086191677834544,
-            "y": 1.5383458560749974,
-            "heading": -0.3070406119029933,
-            "angularVelocity": -0.429411474723066,
-            "velocityX": -3.53848366838567,
-            "velocityY": -0.7335889042423714,
-            "timestamp": 1.9584956412833767
-        },
-        {
-            "x": 5.891572555778559,
-            "y": 1.5225106097486452,
-            "heading": -0.35143250880916643,
-            "angularVelocity": -0.7901836047765775,
-            "velocityX": -3.4642547433776456,
-            "velocityY": -0.2818701811985366,
-            "timestamp": 2.0146748591737533
-        },
-        {
-            "x": 5.713348199791213,
-            "y": 1.5270062964148978,
-            "heading": -0.407385332387855,
-            "angularVelocity": -0.9959701412692558,
-            "velocityX": -3.172425011952234,
-            "velocityY": 0.08002401662169539,
-            "timestamp": 2.07085407706413
-        },
-        {
-            "x": 5.558872974287929,
-            "y": 1.5428236796222032,
-            "heading": -0.4653888399418059,
-            "angularVelocity": -1.0324726781908529,
-            "velocityX": -2.7496862951120815,
-            "velocityY": 0.2815522145247918,
-            "timestamp": 2.1270332949545065
-        },
-        {
-            "x": 5.4303297125098124,
-            "y": 1.5629685460150702,
-            "heading": -0.519265179295127,
-            "angularVelocity": -0.9590083553396718,
-            "velocityX": -2.2880927610801525,
-            "velocityY": 0.35858217948453525,
-            "timestamp": 2.183212512844883
-        },
-        {
-            "x": 5.3280145107455015,
-            "y": 1.583083454211334,
-            "heading": -0.5654730488836437,
-            "angularVelocity": -0.8225082392332849,
-            "velocityX": -1.8212286608183241,
-            "velocityY": 0.3580489182942135,
-            "timestamp": 2.2393917307352598
-        },
-        {
-            "x": 5.251723840078425,
-            "y": 1.6004232571954151,
-            "heading": -0.6018676415285119,
-            "angularVelocity": -0.6478301765590008,
-            "velocityX": -1.3579874112157035,
-            "velocityY": 0.3086515554188953,
-            "timestamp": 2.2955709486256364
-        },
-        {
-            "x": 5.20115218978415,
-            "y": 1.613158164546165,
-            "heading": -0.6270407807035692,
-            "angularVelocity": -0.44808632302748147,
-            "velocityX": -0.9001843064628601,
-            "velocityY": 0.22668359989631479,
-            "timestamp": 2.351750166516013
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -0.2306763921441399,
-            "velocityX": -0.44771342016952703,
-            "velocityY": 0.12178587938311458,
-            "timestamp": 2.4079293844063896
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 3.4924263364045203e-31,
-            "velocityX": 0,
-            "velocityY": 4.738614736798697e-32,
-            "timestamp": 2.464108602296766
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.1.traj
deleted file mode 100644
index 1a381a4d..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 3.136513796099116e-33,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.613270105003341,
-            "y": 7.458186274184093,
-            "heading": 0.022686687743237347,
-            "angularVelocity": 0.25194782208363986,
-            "velocityX": -0.7410712353675238,
-            "velocityY": -0.0978811472565877,
-            "timestamp": 0.09004518298914281
-        },
-        {
-            "x": 7.479732366880497,
-            "y": 7.440537508755927,
-            "heading": 0.06475240251532689,
-            "angularVelocity": 0.4671622998107697,
-            "velocityX": -1.483008126475186,
-            "velocityY": -0.19599899564082535,
-            "timestamp": 0.18009036597828562
-        },
-        {
-            "x": 7.2792817573683966,
-            "y": 7.414022951557291,
-            "heading": 0.12053408331695856,
-            "angularVelocity": 0.6194854510802376,
-            "velocityX": -2.226111412714547,
-            "velocityY": -0.2944583632178676,
-            "timestamp": 0.2701355489674284
-        },
-        {
-            "x": 7.01181645042657,
-            "y": 7.378594749595234,
-            "heading": 0.17807816149187164,
-            "angularVelocity": 0.6390578181383835,
-            "velocityX": -2.9703455316879728,
-            "velocityY": -0.39344916391950197,
-            "timestamp": 0.36018073195657124
-        },
-        {
-            "x": 6.678421151155383,
-            "y": 7.334256434894407,
-            "heading": 0.1920911587056688,
-            "angularVelocity": 0.15562184170903165,
-            "velocityX": -3.7025334193766524,
-            "velocityY": -0.49240073959507324,
-            "timestamp": 0.45022591494571407
-        },
-        {
-            "x": 6.340697514229505,
-            "y": 7.289811670135368,
-            "heading": 0.1920911781237931,
-            "angularVelocity": 2.1564867368321945e-7,
-            "velocityX": -3.7506019279965175,
-            "velocityY": -0.49358292452353475,
-            "timestamp": 0.5402710979348568
-        },
-        {
-            "x": 6.002973877037569,
-            "y": 7.245366907398008,
-            "heading": 0.19209119754191162,
-            "angularVelocity": 2.1564861007875057e-7,
-            "velocityX": -3.750601930951228,
-            "velocityY": -0.49358290207171196,
-            "timestamp": 0.6303162809239996
-        },
-        {
-            "x": 5.665250239845632,
-            "y": 7.200922144660658,
-            "heading": 0.19209121696003015,
-            "angularVelocity": 2.1564860966767442e-7,
-            "velocityX": -3.7506019309512406,
-            "velocityY": -0.493582902071613,
-            "timestamp": 0.7203614639131424
-        },
-        {
-            "x": 5.327526602653695,
-            "y": 7.156477381923307,
-            "heading": 0.19209123637814873,
-            "angularVelocity": 2.1564861026621434e-7,
-            "velocityX": -3.7506019309512415,
-            "velocityY": -0.493582902071613,
-            "timestamp": 0.8104066469022851
-        },
-        {
-            "x": 4.989802965461758,
-            "y": 7.112032619185957,
-            "heading": 0.19209125579626718,
-            "angularVelocity": 2.156486089426075e-7,
-            "velocityX": -3.7506019309512415,
-            "velocityY": -0.49358290207161315,
-            "timestamp": 0.9004518298914279
-        },
-        {
-            "x": 4.652079328269822,
-            "y": 7.067587856448606,
-            "heading": 0.19209127521438563,
-            "angularVelocity": 2.156486089612396e-7,
-            "velocityX": -3.750601930951241,
-            "velocityY": -0.4935829020716131,
-            "timestamp": 0.9904970128805707
-        },
-        {
-            "x": 4.314355691077885,
-            "y": 7.023143093711255,
-            "heading": 0.19209129463250418,
-            "angularVelocity": 2.1564860999442945e-7,
-            "velocityX": -3.7506019309512415,
-            "velocityY": -0.4935829020716131,
-            "timestamp": 1.0805421958697135
-        },
-        {
-            "x": 3.976632053885947,
-            "y": 6.978698330973905,
-            "heading": 0.1920913140506227,
-            "angularVelocity": 2.1564860984546372e-7,
-            "velocityX": -3.750601930951241,
-            "velocityY": -0.49358290207161315,
-            "timestamp": 1.1705873788588563
-        },
-        {
-            "x": 3.638908416694012,
-            "y": 6.934253568236545,
-            "heading": 0.19209133346874124,
-            "angularVelocity": 2.1564860987115903e-7,
-            "velocityX": -3.750601930951227,
-            "velocityY": -0.4935829020717237,
-            "timestamp": 1.2606325618479992
-        },
-        {
-            "x": 3.3011847797995815,
-            "y": 6.8898088032385445,
-            "heading": 0.19209135288686577,
-            "angularVelocity": 2.1564867640744468e-7,
-            "velocityX": -3.7506019276472724,
-            "velocityY": -0.4935829271773418,
-            "timestamp": 1.350677744837142
-        },
-        {
-            "x": 2.967975868183031,
-            "y": 6.845440652800829,
-            "heading": 0.2065131865620181,
-            "angularVelocity": 0.16016218965195775,
-            "velocityX": -3.7004634846122446,
-            "velocityY": -0.492732081438091,
-            "timestamp": 1.440722927826285
-        },
-        {
-            "x": 2.7005740636525677,
-            "y": 6.810001078107782,
-            "heading": 0.266815731322117,
-            "angularVelocity": 0.6696920674520688,
-            "velocityX": -2.9696403033875236,
-            "velocityY": -0.39357546419024264,
-            "timestamp": 1.5307681108154279
-        },
-        {
-            "x": 2.5001864407535797,
-            "y": 6.78347721885534,
-            "heading": 0.32524318999567287,
-            "angularVelocity": 0.6488682318586739,
-            "velocityX": -2.2254119126300136,
-            "velocityY": -0.29456166750908425,
-            "timestamp": 1.6208132938045707
-        },
-        {
-            "x": 2.366700230698382,
-            "y": 6.765819830273621,
-            "heading": 0.3692681816828941,
-            "angularVelocity": 0.48892111966199864,
-            "velocityX": -1.4824358796771253,
-            "velocityY": -0.19609476038099796,
-            "timestamp": 1.7108584767937136
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.26355455704906927,
-            "velocityX": -0.740741797442122,
-            "velocityY": -0.09794894053004785,
-            "timestamp": 1.8009036597828565
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0,
-            "velocityX": 6.927431243507319e-35,
-            "velocityY": 0,
-            "timestamp": 1.8909488427719994
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.traj
deleted file mode 100644
index 1a381a4d..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 3.136513796099116e-33,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.613270105003341,
-            "y": 7.458186274184093,
-            "heading": 0.022686687743237347,
-            "angularVelocity": 0.25194782208363986,
-            "velocityX": -0.7410712353675238,
-            "velocityY": -0.0978811472565877,
-            "timestamp": 0.09004518298914281
-        },
-        {
-            "x": 7.479732366880497,
-            "y": 7.440537508755927,
-            "heading": 0.06475240251532689,
-            "angularVelocity": 0.4671622998107697,
-            "velocityX": -1.483008126475186,
-            "velocityY": -0.19599899564082535,
-            "timestamp": 0.18009036597828562
-        },
-        {
-            "x": 7.2792817573683966,
-            "y": 7.414022951557291,
-            "heading": 0.12053408331695856,
-            "angularVelocity": 0.6194854510802376,
-            "velocityX": -2.226111412714547,
-            "velocityY": -0.2944583632178676,
-            "timestamp": 0.2701355489674284
-        },
-        {
-            "x": 7.01181645042657,
-            "y": 7.378594749595234,
-            "heading": 0.17807816149187164,
-            "angularVelocity": 0.6390578181383835,
-            "velocityX": -2.9703455316879728,
-            "velocityY": -0.39344916391950197,
-            "timestamp": 0.36018073195657124
-        },
-        {
-            "x": 6.678421151155383,
-            "y": 7.334256434894407,
-            "heading": 0.1920911587056688,
-            "angularVelocity": 0.15562184170903165,
-            "velocityX": -3.7025334193766524,
-            "velocityY": -0.49240073959507324,
-            "timestamp": 0.45022591494571407
-        },
-        {
-            "x": 6.340697514229505,
-            "y": 7.289811670135368,
-            "heading": 0.1920911781237931,
-            "angularVelocity": 2.1564867368321945e-7,
-            "velocityX": -3.7506019279965175,
-            "velocityY": -0.49358292452353475,
-            "timestamp": 0.5402710979348568
-        },
-        {
-            "x": 6.002973877037569,
-            "y": 7.245366907398008,
-            "heading": 0.19209119754191162,
-            "angularVelocity": 2.1564861007875057e-7,
-            "velocityX": -3.750601930951228,
-            "velocityY": -0.49358290207171196,
-            "timestamp": 0.6303162809239996
-        },
-        {
-            "x": 5.665250239845632,
-            "y": 7.200922144660658,
-            "heading": 0.19209121696003015,
-            "angularVelocity": 2.1564860966767442e-7,
-            "velocityX": -3.7506019309512406,
-            "velocityY": -0.493582902071613,
-            "timestamp": 0.7203614639131424
-        },
-        {
-            "x": 5.327526602653695,
-            "y": 7.156477381923307,
-            "heading": 0.19209123637814873,
-            "angularVelocity": 2.1564861026621434e-7,
-            "velocityX": -3.7506019309512415,
-            "velocityY": -0.493582902071613,
-            "timestamp": 0.8104066469022851
-        },
-        {
-            "x": 4.989802965461758,
-            "y": 7.112032619185957,
-            "heading": 0.19209125579626718,
-            "angularVelocity": 2.156486089426075e-7,
-            "velocityX": -3.7506019309512415,
-            "velocityY": -0.49358290207161315,
-            "timestamp": 0.9004518298914279
-        },
-        {
-            "x": 4.652079328269822,
-            "y": 7.067587856448606,
-            "heading": 0.19209127521438563,
-            "angularVelocity": 2.156486089612396e-7,
-            "velocityX": -3.750601930951241,
-            "velocityY": -0.4935829020716131,
-            "timestamp": 0.9904970128805707
-        },
-        {
-            "x": 4.314355691077885,
-            "y": 7.023143093711255,
-            "heading": 0.19209129463250418,
-            "angularVelocity": 2.1564860999442945e-7,
-            "velocityX": -3.7506019309512415,
-            "velocityY": -0.4935829020716131,
-            "timestamp": 1.0805421958697135
-        },
-        {
-            "x": 3.976632053885947,
-            "y": 6.978698330973905,
-            "heading": 0.1920913140506227,
-            "angularVelocity": 2.1564860984546372e-7,
-            "velocityX": -3.750601930951241,
-            "velocityY": -0.49358290207161315,
-            "timestamp": 1.1705873788588563
-        },
-        {
-            "x": 3.638908416694012,
-            "y": 6.934253568236545,
-            "heading": 0.19209133346874124,
-            "angularVelocity": 2.1564860987115903e-7,
-            "velocityX": -3.750601930951227,
-            "velocityY": -0.4935829020717237,
-            "timestamp": 1.2606325618479992
-        },
-        {
-            "x": 3.3011847797995815,
-            "y": 6.8898088032385445,
-            "heading": 0.19209135288686577,
-            "angularVelocity": 2.1564867640744468e-7,
-            "velocityX": -3.7506019276472724,
-            "velocityY": -0.4935829271773418,
-            "timestamp": 1.350677744837142
-        },
-        {
-            "x": 2.967975868183031,
-            "y": 6.845440652800829,
-            "heading": 0.2065131865620181,
-            "angularVelocity": 0.16016218965195775,
-            "velocityX": -3.7004634846122446,
-            "velocityY": -0.492732081438091,
-            "timestamp": 1.440722927826285
-        },
-        {
-            "x": 2.7005740636525677,
-            "y": 6.810001078107782,
-            "heading": 0.266815731322117,
-            "angularVelocity": 0.6696920674520688,
-            "velocityX": -2.9696403033875236,
-            "velocityY": -0.39357546419024264,
-            "timestamp": 1.5307681108154279
-        },
-        {
-            "x": 2.5001864407535797,
-            "y": 6.78347721885534,
-            "heading": 0.32524318999567287,
-            "angularVelocity": 0.6488682318586739,
-            "velocityX": -2.2254119126300136,
-            "velocityY": -0.29456166750908425,
-            "timestamp": 1.6208132938045707
-        },
-        {
-            "x": 2.366700230698382,
-            "y": 6.765819830273621,
-            "heading": 0.3692681816828941,
-            "angularVelocity": 0.48892111966199864,
-            "velocityX": -1.4824358796771253,
-            "velocityY": -0.19609476038099796,
-            "timestamp": 1.7108584767937136
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.26355455704906927,
-            "velocityX": -0.740741797442122,
-            "velocityY": -0.09794894053004785,
-            "timestamp": 1.8009036597828565
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0,
-            "velocityX": 6.927431243507319e-35,
-            "velocityY": 0,
-            "timestamp": 1.8909488427719994
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.1.traj
deleted file mode 100644
index 28a53b93..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.1.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -2.4374604803646449e-32,
-            "velocityY": 2.350988701644575e-38,
-            "timestamp": 0
-        },
-        {
-            "x": 7.602872461914855,
-            "y": 7.439913035887961,
-            "heading": 5.3325436298288445e-18,
-            "angularVelocity": 5.3856284215509377e-17,
-            "velocityX": -0.7789533333445217,
-            "velocityY": -0.2735661154121556,
-            "timestamp": 0.09901432445765257
-        },
-        {
-            "x": 7.448617388685645,
-            "y": 7.385739108696781,
-            "heading": -4.4022234029758545e-18,
-            "angularVelocity": -9.831675377471393e-17,
-            "velocityX": -1.557906636985473,
-            "velocityY": -0.5471322203925048,
-            "timestamp": 0.19802864891530514
-        },
-        {
-            "x": 7.217234786194527,
-            "y": 7.304478220492258,
-            "heading": -7.053417683545123e-18,
-            "angularVelocity": -2.6775864730895626e-17,
-            "velocityX": -2.3368598812192953,
-            "velocityY": -0.8206983045092451,
-            "timestamp": 0.2970429733729577
-        },
-        {
-            "x": 6.9087246720879545,
-            "y": 7.196130377471774,
-            "heading": 2.6673179344101e-18,
-            "angularVelocity": 9.817504407722252e-17,
-            "velocityX": -3.1158129472318725,
-            "velocityY": -1.0942643260352094,
-            "timestamp": 0.3960572978306103
-        },
-        {
-            "x": 6.5553201862538,
-            "y": 7.072015763386003,
-            "heading": 8.41895708879283e-18,
-            "angularVelocity": 5.808896021749495e-17,
-            "velocityX": -3.5692258445423475,
-            "velocityY": -1.2535015995473784,
-            "timestamp": 0.49507162228826285
-        },
-        {
-            "x": 6.2019157004196215,
-            "y": 6.9479011493002245,
-            "heading": -3.69857396739816e-19,
-            "angularVelocity": -8.876306064099344e-17,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.2535015995474599,
-            "timestamp": 0.5940859467459154
-        },
-        {
-            "x": 5.848511214585444,
-            "y": 6.823786535214446,
-            "heading": -8.032367976884792e-18,
-            "angularVelocity": -7.738789939855791e-17,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.2535015995474599,
-            "timestamp": 0.693100271203568
-        },
-        {
-            "x": 5.495106728751266,
-            "y": 6.699671921128667,
-            "heading": -3.490709714785344e-17,
-            "angularVelocity": -2.714226382747745e-16,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.2535015995474599,
-            "timestamp": 0.7921145956612206
-        },
-        {
-            "x": 5.141702242917089,
-            "y": 6.575557307042889,
-            "heading": -1.2741337636064699e-17,
-            "angularVelocity": 2.238641694845762e-16,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.2535015995474599,
-            "timestamp": 0.8911289201188731
-        },
-        {
-            "x": 4.788297757082911,
-            "y": 6.4514426929571105,
-            "heading": -2.2645871045802615e-17,
-            "angularVelocity": -1.0003131833516608e-16,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.2535015995474597,
-            "timestamp": 0.9901432445765257
-        },
-        {
-            "x": 4.4348932712487334,
-            "y": 6.327328078871332,
-            "heading": -7.505924377434873e-18,
-            "angularVelocity": 1.529066299375997e-16,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.25350159954746,
-            "timestamp": 1.0891575690341782
-        },
-        {
-            "x": 4.081488785414556,
-            "y": 6.203213464785553,
-            "heading": -3.3249179069580955e-17,
-            "angularVelocity": -2.5999525658031657e-16,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.25350159954746,
-            "timestamp": 1.1881718934918308
-        },
-        {
-            "x": 3.728084299580378,
-            "y": 6.079098850699775,
-            "heading": -3.31250701341244e-17,
-            "angularVelocity": 1.2534442479544336e-18,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.2535015995474597,
-            "timestamp": 1.2871862179494835
-        },
-        {
-            "x": 3.3746798137461993,
-            "y": 5.9549842366139965,
-            "heading": -3.474935359347948e-17,
-            "angularVelocity": -1.6404530025230332e-17,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.2535015995474599,
-            "timestamp": 1.3862005424071362
-        },
-        {
-            "x": 3.021275327912045,
-            "y": 5.830869622528225,
-            "heading": -2.4652412430964293e-17,
-            "angularVelocity": 1.0197454982216833e-16,
-            "velocityX": -3.5692258445423475,
-            "velocityY": -1.2535015995473784,
-            "timestamp": 1.4852148668647889
-        },
-        {
-            "x": 2.7127652138054734,
-            "y": 5.722521779507741,
-            "heading": 1.414690860074665e-18,
-            "angularVelocity": 2.632659789801892e-16,
-            "velocityX": -3.115812947231872,
-            "velocityY": -1.0942643260352094,
-            "timestamp": 1.5842291913224416
-        },
-        {
-            "x": 2.4813826113143547,
-            "y": 5.641260891303218,
-            "heading": 8.307803002216867e-18,
-            "angularVelocity": 6.961732115429119e-17,
-            "velocityX": -2.336859881219295,
-            "velocityY": -0.8206983045092451,
-            "timestamp": 1.6832435157800942
-        },
-        {
-            "x": 2.3271275380851444,
-            "y": 5.587086964112038,
-            "heading": -3.972937436005726e-18,
-            "angularVelocity": -1.2402993582700068e-16,
-            "velocityX": -1.557906636985473,
-            "velocityY": -0.5471322203925048,
-            "timestamp": 1.782257840237747
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 4.012487544797503e-17,
-            "velocityX": -0.7789533333445219,
-            "velocityY": -0.2735661154121556,
-            "timestamp": 1.8812721646953996
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -5.265297803216144e-35,
-            "velocityY": 0,
-            "timestamp": 1.9802864891530523
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.traj
deleted file mode 100644
index 28a53b93..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -2.4374604803646449e-32,
-            "velocityY": 2.350988701644575e-38,
-            "timestamp": 0
-        },
-        {
-            "x": 7.602872461914855,
-            "y": 7.439913035887961,
-            "heading": 5.3325436298288445e-18,
-            "angularVelocity": 5.3856284215509377e-17,
-            "velocityX": -0.7789533333445217,
-            "velocityY": -0.2735661154121556,
-            "timestamp": 0.09901432445765257
-        },
-        {
-            "x": 7.448617388685645,
-            "y": 7.385739108696781,
-            "heading": -4.4022234029758545e-18,
-            "angularVelocity": -9.831675377471393e-17,
-            "velocityX": -1.557906636985473,
-            "velocityY": -0.5471322203925048,
-            "timestamp": 0.19802864891530514
-        },
-        {
-            "x": 7.217234786194527,
-            "y": 7.304478220492258,
-            "heading": -7.053417683545123e-18,
-            "angularVelocity": -2.6775864730895626e-17,
-            "velocityX": -2.3368598812192953,
-            "velocityY": -0.8206983045092451,
-            "timestamp": 0.2970429733729577
-        },
-        {
-            "x": 6.9087246720879545,
-            "y": 7.196130377471774,
-            "heading": 2.6673179344101e-18,
-            "angularVelocity": 9.817504407722252e-17,
-            "velocityX": -3.1158129472318725,
-            "velocityY": -1.0942643260352094,
-            "timestamp": 0.3960572978306103
-        },
-        {
-            "x": 6.5553201862538,
-            "y": 7.072015763386003,
-            "heading": 8.41895708879283e-18,
-            "angularVelocity": 5.808896021749495e-17,
-            "velocityX": -3.5692258445423475,
-            "velocityY": -1.2535015995473784,
-            "timestamp": 0.49507162228826285
-        },
-        {
-            "x": 6.2019157004196215,
-            "y": 6.9479011493002245,
-            "heading": -3.69857396739816e-19,
-            "angularVelocity": -8.876306064099344e-17,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.2535015995474599,
-            "timestamp": 0.5940859467459154
-        },
-        {
-            "x": 5.848511214585444,
-            "y": 6.823786535214446,
-            "heading": -8.032367976884792e-18,
-            "angularVelocity": -7.738789939855791e-17,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.2535015995474599,
-            "timestamp": 0.693100271203568
-        },
-        {
-            "x": 5.495106728751266,
-            "y": 6.699671921128667,
-            "heading": -3.490709714785344e-17,
-            "angularVelocity": -2.714226382747745e-16,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.2535015995474599,
-            "timestamp": 0.7921145956612206
-        },
-        {
-            "x": 5.141702242917089,
-            "y": 6.575557307042889,
-            "heading": -1.2741337636064699e-17,
-            "angularVelocity": 2.238641694845762e-16,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.2535015995474599,
-            "timestamp": 0.8911289201188731
-        },
-        {
-            "x": 4.788297757082911,
-            "y": 6.4514426929571105,
-            "heading": -2.2645871045802615e-17,
-            "angularVelocity": -1.0003131833516608e-16,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.2535015995474597,
-            "timestamp": 0.9901432445765257
-        },
-        {
-            "x": 4.4348932712487334,
-            "y": 6.327328078871332,
-            "heading": -7.505924377434873e-18,
-            "angularVelocity": 1.529066299375997e-16,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.25350159954746,
-            "timestamp": 1.0891575690341782
-        },
-        {
-            "x": 4.081488785414556,
-            "y": 6.203213464785553,
-            "heading": -3.3249179069580955e-17,
-            "angularVelocity": -2.5999525658031657e-16,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.25350159954746,
-            "timestamp": 1.1881718934918308
-        },
-        {
-            "x": 3.728084299580378,
-            "y": 6.079098850699775,
-            "heading": -3.31250701341244e-17,
-            "angularVelocity": 1.2534442479544336e-18,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.2535015995474597,
-            "timestamp": 1.2871862179494835
-        },
-        {
-            "x": 3.3746798137461993,
-            "y": 5.9549842366139965,
-            "heading": -3.474935359347948e-17,
-            "angularVelocity": -1.6404530025230332e-17,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.2535015995474599,
-            "timestamp": 1.3862005424071362
-        },
-        {
-            "x": 3.021275327912045,
-            "y": 5.830869622528225,
-            "heading": -2.4652412430964293e-17,
-            "angularVelocity": 1.0197454982216833e-16,
-            "velocityX": -3.5692258445423475,
-            "velocityY": -1.2535015995473784,
-            "timestamp": 1.4852148668647889
-        },
-        {
-            "x": 2.7127652138054734,
-            "y": 5.722521779507741,
-            "heading": 1.414690860074665e-18,
-            "angularVelocity": 2.632659789801892e-16,
-            "velocityX": -3.115812947231872,
-            "velocityY": -1.0942643260352094,
-            "timestamp": 1.5842291913224416
-        },
-        {
-            "x": 2.4813826113143547,
-            "y": 5.641260891303218,
-            "heading": 8.307803002216867e-18,
-            "angularVelocity": 6.961732115429119e-17,
-            "velocityX": -2.336859881219295,
-            "velocityY": -0.8206983045092451,
-            "timestamp": 1.6832435157800942
-        },
-        {
-            "x": 2.3271275380851444,
-            "y": 5.587086964112038,
-            "heading": -3.972937436005726e-18,
-            "angularVelocity": -1.2402993582700068e-16,
-            "velocityX": -1.557906636985473,
-            "velocityY": -0.5471322203925048,
-            "timestamp": 1.782257840237747
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 4.012487544797503e-17,
-            "velocityX": -0.7789533333445219,
-            "velocityY": -0.2735661154121556,
-            "timestamp": 1.8812721646953996
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -5.265297803216144e-35,
-            "velocityY": 0,
-            "timestamp": 1.9802864891530523
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.1.traj
deleted file mode 100644
index d8ed1eba..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.1.traj	
+++ /dev/null
@@ -1,346 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": -3.2387949852399556e-31,
-            "velocityX": -1.054254683790836e-34,
-            "velocityY": 3.285843466136746e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 7.625901434953002,
-            "y": 7.454866143797358,
-            "heading": -0.004441360542181856,
-            "angularVelocity": -0.05445952619049241,
-            "velocityX": -0.6633512843785839,
-            "velocityY": -0.14878415147416332,
-            "timestamp": 0.08155341871038292
-        },
-        {
-            "x": 7.517704306524476,
-            "y": 7.430598431564336,
-            "heading": -0.01332401825146925,
-            "angularVelocity": -0.10891827528203153,
-            "velocityX": -1.3267025483353667,
-            "velocityY": -0.29756830083607516,
-            "timestamp": 0.16310683742076584
-        },
-        {
-            "x": 7.355408617374589,
-            "y": 7.3941968638363065,
-            "heading": -0.02664786051722782,
-            "angularVelocity": -0.1633756435530466,
-            "velocityX": -1.9900537796734206,
-            "velocityY": -0.4463524436333081,
-            "timestamp": 0.24466025613114878
-        },
-        {
-            "x": 7.139014373014635,
-            "y": 7.345661441877259,
-            "heading": -0.044412717144295275,
-            "angularVelocity": -0.21783092490771921,
-            "velocityX": -2.653404943432558,
-            "velocityY": -0.5951365709316123,
-            "timestamp": 0.3262136748415317
-        },
-        {
-            "x": 6.868521591139737,
-            "y": 7.284992169702681,
-            "heading": -0.06661833727396635,
-            "angularVelocity": -0.2722831302576181,
-            "velocityX": -3.3167558902158003,
-            "velocityY": -0.7439206489924082,
-            "timestamp": 0.4077670935519146
-        },
-        {
-            "x": 6.5674889621165065,
-            "y": 7.217472965131769,
-            "heading": -0.06661834019422361,
-            "angularVelocity": -3.5807907402848094e-8,
-            "velocityX": -3.691232492561401,
-            "velocityY": -0.827913846391786,
-            "timestamp": 0.4893205122622975
-        },
-        {
-            "x": 6.266456335099212,
-            "y": 7.149953751617259,
-            "heading": -0.06661834311445712,
-            "angularVelocity": -3.5807616132797033e-8,
-            "velocityX": -3.6912324679648125,
-            "velocityY": -0.8279139560572942,
-            "timestamp": 0.5708739309726805
-        },
-        {
-            "x": 5.9654237103471415,
-            "y": 7.082434528003299,
-            "heading": -0.06661834603468975,
-            "angularVelocity": -3.5807605171686195e-8,
-            "velocityX": -3.6912324401888563,
-            "velocityY": -0.8279140798957559,
-            "timestamp": 0.6524273496830634
-        },
-        {
-            "x": 5.664391088171292,
-            "y": 7.014915292903322,
-            "heading": -0.06661834895492107,
-            "angularVelocity": -3.580758928944026e-8,
-            "velocityX": -3.691232408599494,
-            "velocityY": -0.8279142207361639,
-            "timestamp": 0.7339807683934463
-        },
-        {
-            "x": 5.363358468935584,
-            "y": 6.947396044694795,
-            "heading": -0.06661835187515067,
-            "angularVelocity": -3.580756830329485e-8,
-            "velocityX": -3.6912323725477565,
-            "velocityY": -0.8279143814719224,
-            "timestamp": 0.8155341871038292
-        },
-        {
-            "x": 5.062325853062933,
-            "y": 6.879876781492171,
-            "heading": -0.06661835479537813,
-            "angularVelocity": -3.58075419139389e-8,
-            "velocityX": -3.691232331310291,
-            "velocityY": -0.8279145653280606,
-            "timestamp": 0.8970876058142121
-        },
-        {
-            "x": 4.761293241038404,
-            "y": 6.812357501132806,
-            "heading": -0.06661835771560275,
-            "angularVelocity": -3.580750706252144e-8,
-            "velocityX": -3.691232284125005,
-            "velocityY": -0.8279147757023306,
-            "timestamp": 0.978641024524595
-        },
-        {
-            "x": 4.46026063340559,
-            "y": 6.744838201193102,
-            "heading": -0.06661836063582369,
-            "angularVelocity": -3.580746208542298e-8,
-            "velocityX": -3.6912322302742044,
-            "velocityY": -0.8279150157945213,
-            "timestamp": 1.060194443234978
-        },
-        {
-            "x": 4.159228030751222,
-            "y": 6.677318879057164,
-            "heading": -0.06661836355604021,
-            "angularVelocity": -3.5807407853670686e-8,
-            "velocityX": -3.691232169229015,
-            "velocityY": -0.8279152879625719,
-            "timestamp": 1.141747861945361
-        },
-        {
-            "x": 3.8581954336724973,
-            "y": 6.609799532062401,
-            "heading": -0.06661836647625141,
-            "angularVelocity": -3.580734253915704e-8,
-            "velocityX": -3.6912321008610123,
-            "velocityY": -0.8279155927790207,
-            "timestamp": 1.223301280655744
-        },
-        {
-            "x": 3.557162842693998,
-            "y": 6.542280157870002,
-            "heading": -0.0666183693964589,
-            "angularVelocity": -3.580729730790545e-8,
-            "velocityX": -3.691232026060653,
-            "velocityY": -0.8279159262737572,
-            "timestamp": 1.304854699366127
-        },
-        {
-            "x": 3.256130258261278,
-            "y": 6.474760754493496,
-            "heading": -0.06661837231666672,
-            "angularVelocity": -3.580730095676624e-8,
-            "velocityX": -3.6912319457969542,
-            "velocityY": -0.8279162841264173,
-            "timestamp": 1.38640811807651
-        },
-        {
-            "x": 2.9550976753234863,
-            "y": 6.407241344451904,
-            "heading": -0.06661837523687889,
-            "angularVelocity": -3.580735442493409e-8,
-            "velocityX": -3.6912319274662786,
-            "velocityY": -0.8279163658530455,
-            "timestamp": 1.467961536786893
-        },
-        {
-            "x": 2.779035844872165,
-            "y": 6.36860208155972,
-            "heading": -0.0666183783001983,
-            "angularVelocity": -6.428976142053906e-8,
-            "velocityX": -3.695002547647375,
-            "velocityY": -0.8109206547436999,
-            "timestamp": 1.5156101725269335
-        },
-        {
-            "x": 2.608054120329012,
-            "y": 6.311726454658268,
-            "heading": -0.06677425221357433,
-            "angularVelocity": -0.0032713195629861798,
-            "velocityX": -3.5883865694652988,
-            "velocityY": -1.1936464920308683,
-            "timestamp": 1.563258808266974
-        },
-        {
-            "x": 2.4510058546542055,
-            "y": 6.242296424291011,
-            "heading": -0.07609605863752185,
-            "angularVelocity": -0.19563637614744786,
-            "velocityX": -3.29596562914219,
-            "velocityY": -1.457125252146435,
-            "timestamp": 1.6109074440070146
-        },
-        {
-            "x": 2.3100257060565728,
-            "y": 6.162946485717711,
-            "heading": -0.09018973905193617,
-            "angularVelocity": -0.2957835034721525,
-            "velocityX": -2.9587447029271896,
-            "velocityY": -1.6653139663050547,
-            "timestamp": 1.658556079747055
-        },
-        {
-            "x": 2.1858070710130613,
-            "y": 6.074835566620609,
-            "heading": -0.10721183288872499,
-            "angularVelocity": -0.3572419980708675,
-            "velocityX": -2.6069714927652616,
-            "velocityY": -1.8491803118526937,
-            "timestamp": 1.7062047154870956
-        },
-        {
-            "x": 2.078679040228791,
-            "y": 5.978601109574319,
-            "heading": -0.12617474237211068,
-            "angularVelocity": -0.3979738179094536,
-            "velocityX": -2.248291669223545,
-            "velocityY": -2.0196686757492435,
-            "timestamp": 1.7538533512271361
-        },
-        {
-            "x": 1.9888310933691378,
-            "y": 5.874644187394139,
-            "heading": -0.14646656775118436,
-            "angularVelocity": -0.42586372231046465,
-            "velocityX": -1.8856352435727148,
-            "velocityY": -2.181739740617511,
-            "timestamp": 1.8015019869671767
-        },
-        {
-            "x": 1.9163856506347656,
-            "y": 5.763239860534668,
-            "heading": -0.16767175460275374,
-            "angularVelocity": -0.44503240276194317,
-            "velocityX": -1.5204095900989456,
-            "velocityY": -2.3380381227986136,
-            "timestamp": 1.8491506227072172
-        },
-        {
-            "x": 1.8485889050766984,
-            "y": 5.589343182132367,
-            "heading": -0.1992083750464638,
-            "angularVelocity": -0.46340502818407264,
-            "velocityX": -0.9962181217957636,
-            "velocityY": -2.555270476750044,
-            "timestamp": 1.9172047406506962
-        },
-        {
-            "x": 1.817164507457459,
-            "y": 5.402475290970386,
-            "heading": -0.23166704244574374,
-            "angularVelocity": -0.47695375945076895,
-            "velocityX": -0.4617560049104581,
-            "velocityY": -2.7458719150120894,
-            "timestamp": 1.9852588585941753
-        },
-        {
-            "x": 1.8230483770902466,
-            "y": 5.205640044676734,
-            "heading": -0.2645113992669886,
-            "angularVelocity": -0.4826211523089798,
-            "velocityX": 0.08645868627131759,
-            "velocityY": -2.8923341047058035,
-            "timestamp": 2.0533129765376543
-        },
-        {
-            "x": 1.8673165212991787,
-            "y": 5.004575832748109,
-            "heading": -0.29674148680613366,
-            "angularVelocity": -0.4735949640250024,
-            "velocityX": 0.6504844313118886,
-            "velocityY": -2.9544753205907597,
-            "timestamp": 2.1213670944811334
-        },
-        {
-            "x": 1.9491517735306563,
-            "y": 4.812420400099087,
-            "heading": -0.3261812067352161,
-            "angularVelocity": -0.43259277790693457,
-            "velocityX": 1.202502577425556,
-            "velocityY": -2.823568043429816,
-            "timestamp": 2.1894212124246124
-        },
-        {
-            "x": 2.050948548896826,
-            "y": 4.653276186897604,
-            "heading": -0.34937275798086526,
-            "angularVelocity": -0.34078101291423535,
-            "velocityX": 1.4958209501847175,
-            "velocityY": -2.338494980328238,
-            "timestamp": 2.2574753303680914
-        },
-        {
-            "x": 2.14520275900139,
-            "y": 4.5319666650276815,
-            "heading": -0.36647677931046935,
-            "angularVelocity": -0.2513297041615066,
-            "velocityX": 1.3849890785871777,
-            "velocityY": -1.7825449147790617,
-            "timestamp": 2.3255294483115705
-        },
-        {
-            "x": 2.2207736095496604,
-            "y": 4.444424274499807,
-            "heading": -0.37856224680514944,
-            "angularVelocity": -0.17758613086130492,
-            "velocityX": 1.1104522816836297,
-            "velocityY": -1.2863643402237204,
-            "timestamp": 2.3935835662550495
-        },
-        {
-            "x": 2.273137984258771,
-            "y": 4.387723780671385,
-            "heading": -0.38627299411990695,
-            "angularVelocity": -0.113303170297753,
-            "velocityX": 0.7694519639887757,
-            "velocityY": -0.8331677133117752,
-            "timestamp": 2.4616376841985286
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.054765324902410414,
-            "velocityX": 0.3947155080835744,
-            "velocityY": -0.4073784439379148,
-            "timestamp": 2.5296918021420076
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 3.1303206075717874e-30,
-            "velocityX": 0,
-            "velocityY": -1.5697460792586675e-31,
-            "timestamp": 2.5977459200854867
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.traj
deleted file mode 100644
index d8ed1eba..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.traj	
+++ /dev/null
@@ -1,346 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": -3.2387949852399556e-31,
-            "velocityX": -1.054254683790836e-34,
-            "velocityY": 3.285843466136746e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 7.625901434953002,
-            "y": 7.454866143797358,
-            "heading": -0.004441360542181856,
-            "angularVelocity": -0.05445952619049241,
-            "velocityX": -0.6633512843785839,
-            "velocityY": -0.14878415147416332,
-            "timestamp": 0.08155341871038292
-        },
-        {
-            "x": 7.517704306524476,
-            "y": 7.430598431564336,
-            "heading": -0.01332401825146925,
-            "angularVelocity": -0.10891827528203153,
-            "velocityX": -1.3267025483353667,
-            "velocityY": -0.29756830083607516,
-            "timestamp": 0.16310683742076584
-        },
-        {
-            "x": 7.355408617374589,
-            "y": 7.3941968638363065,
-            "heading": -0.02664786051722782,
-            "angularVelocity": -0.1633756435530466,
-            "velocityX": -1.9900537796734206,
-            "velocityY": -0.4463524436333081,
-            "timestamp": 0.24466025613114878
-        },
-        {
-            "x": 7.139014373014635,
-            "y": 7.345661441877259,
-            "heading": -0.044412717144295275,
-            "angularVelocity": -0.21783092490771921,
-            "velocityX": -2.653404943432558,
-            "velocityY": -0.5951365709316123,
-            "timestamp": 0.3262136748415317
-        },
-        {
-            "x": 6.868521591139737,
-            "y": 7.284992169702681,
-            "heading": -0.06661833727396635,
-            "angularVelocity": -0.2722831302576181,
-            "velocityX": -3.3167558902158003,
-            "velocityY": -0.7439206489924082,
-            "timestamp": 0.4077670935519146
-        },
-        {
-            "x": 6.5674889621165065,
-            "y": 7.217472965131769,
-            "heading": -0.06661834019422361,
-            "angularVelocity": -3.5807907402848094e-8,
-            "velocityX": -3.691232492561401,
-            "velocityY": -0.827913846391786,
-            "timestamp": 0.4893205122622975
-        },
-        {
-            "x": 6.266456335099212,
-            "y": 7.149953751617259,
-            "heading": -0.06661834311445712,
-            "angularVelocity": -3.5807616132797033e-8,
-            "velocityX": -3.6912324679648125,
-            "velocityY": -0.8279139560572942,
-            "timestamp": 0.5708739309726805
-        },
-        {
-            "x": 5.9654237103471415,
-            "y": 7.082434528003299,
-            "heading": -0.06661834603468975,
-            "angularVelocity": -3.5807605171686195e-8,
-            "velocityX": -3.6912324401888563,
-            "velocityY": -0.8279140798957559,
-            "timestamp": 0.6524273496830634
-        },
-        {
-            "x": 5.664391088171292,
-            "y": 7.014915292903322,
-            "heading": -0.06661834895492107,
-            "angularVelocity": -3.580758928944026e-8,
-            "velocityX": -3.691232408599494,
-            "velocityY": -0.8279142207361639,
-            "timestamp": 0.7339807683934463
-        },
-        {
-            "x": 5.363358468935584,
-            "y": 6.947396044694795,
-            "heading": -0.06661835187515067,
-            "angularVelocity": -3.580756830329485e-8,
-            "velocityX": -3.6912323725477565,
-            "velocityY": -0.8279143814719224,
-            "timestamp": 0.8155341871038292
-        },
-        {
-            "x": 5.062325853062933,
-            "y": 6.879876781492171,
-            "heading": -0.06661835479537813,
-            "angularVelocity": -3.58075419139389e-8,
-            "velocityX": -3.691232331310291,
-            "velocityY": -0.8279145653280606,
-            "timestamp": 0.8970876058142121
-        },
-        {
-            "x": 4.761293241038404,
-            "y": 6.812357501132806,
-            "heading": -0.06661835771560275,
-            "angularVelocity": -3.580750706252144e-8,
-            "velocityX": -3.691232284125005,
-            "velocityY": -0.8279147757023306,
-            "timestamp": 0.978641024524595
-        },
-        {
-            "x": 4.46026063340559,
-            "y": 6.744838201193102,
-            "heading": -0.06661836063582369,
-            "angularVelocity": -3.580746208542298e-8,
-            "velocityX": -3.6912322302742044,
-            "velocityY": -0.8279150157945213,
-            "timestamp": 1.060194443234978
-        },
-        {
-            "x": 4.159228030751222,
-            "y": 6.677318879057164,
-            "heading": -0.06661836355604021,
-            "angularVelocity": -3.5807407853670686e-8,
-            "velocityX": -3.691232169229015,
-            "velocityY": -0.8279152879625719,
-            "timestamp": 1.141747861945361
-        },
-        {
-            "x": 3.8581954336724973,
-            "y": 6.609799532062401,
-            "heading": -0.06661836647625141,
-            "angularVelocity": -3.580734253915704e-8,
-            "velocityX": -3.6912321008610123,
-            "velocityY": -0.8279155927790207,
-            "timestamp": 1.223301280655744
-        },
-        {
-            "x": 3.557162842693998,
-            "y": 6.542280157870002,
-            "heading": -0.0666183693964589,
-            "angularVelocity": -3.580729730790545e-8,
-            "velocityX": -3.691232026060653,
-            "velocityY": -0.8279159262737572,
-            "timestamp": 1.304854699366127
-        },
-        {
-            "x": 3.256130258261278,
-            "y": 6.474760754493496,
-            "heading": -0.06661837231666672,
-            "angularVelocity": -3.580730095676624e-8,
-            "velocityX": -3.6912319457969542,
-            "velocityY": -0.8279162841264173,
-            "timestamp": 1.38640811807651
-        },
-        {
-            "x": 2.9550976753234863,
-            "y": 6.407241344451904,
-            "heading": -0.06661837523687889,
-            "angularVelocity": -3.580735442493409e-8,
-            "velocityX": -3.6912319274662786,
-            "velocityY": -0.8279163658530455,
-            "timestamp": 1.467961536786893
-        },
-        {
-            "x": 2.779035844872165,
-            "y": 6.36860208155972,
-            "heading": -0.0666183783001983,
-            "angularVelocity": -6.428976142053906e-8,
-            "velocityX": -3.695002547647375,
-            "velocityY": -0.8109206547436999,
-            "timestamp": 1.5156101725269335
-        },
-        {
-            "x": 2.608054120329012,
-            "y": 6.311726454658268,
-            "heading": -0.06677425221357433,
-            "angularVelocity": -0.0032713195629861798,
-            "velocityX": -3.5883865694652988,
-            "velocityY": -1.1936464920308683,
-            "timestamp": 1.563258808266974
-        },
-        {
-            "x": 2.4510058546542055,
-            "y": 6.242296424291011,
-            "heading": -0.07609605863752185,
-            "angularVelocity": -0.19563637614744786,
-            "velocityX": -3.29596562914219,
-            "velocityY": -1.457125252146435,
-            "timestamp": 1.6109074440070146
-        },
-        {
-            "x": 2.3100257060565728,
-            "y": 6.162946485717711,
-            "heading": -0.09018973905193617,
-            "angularVelocity": -0.2957835034721525,
-            "velocityX": -2.9587447029271896,
-            "velocityY": -1.6653139663050547,
-            "timestamp": 1.658556079747055
-        },
-        {
-            "x": 2.1858070710130613,
-            "y": 6.074835566620609,
-            "heading": -0.10721183288872499,
-            "angularVelocity": -0.3572419980708675,
-            "velocityX": -2.6069714927652616,
-            "velocityY": -1.8491803118526937,
-            "timestamp": 1.7062047154870956
-        },
-        {
-            "x": 2.078679040228791,
-            "y": 5.978601109574319,
-            "heading": -0.12617474237211068,
-            "angularVelocity": -0.3979738179094536,
-            "velocityX": -2.248291669223545,
-            "velocityY": -2.0196686757492435,
-            "timestamp": 1.7538533512271361
-        },
-        {
-            "x": 1.9888310933691378,
-            "y": 5.874644187394139,
-            "heading": -0.14646656775118436,
-            "angularVelocity": -0.42586372231046465,
-            "velocityX": -1.8856352435727148,
-            "velocityY": -2.181739740617511,
-            "timestamp": 1.8015019869671767
-        },
-        {
-            "x": 1.9163856506347656,
-            "y": 5.763239860534668,
-            "heading": -0.16767175460275374,
-            "angularVelocity": -0.44503240276194317,
-            "velocityX": -1.5204095900989456,
-            "velocityY": -2.3380381227986136,
-            "timestamp": 1.8491506227072172
-        },
-        {
-            "x": 1.8485889050766984,
-            "y": 5.589343182132367,
-            "heading": -0.1992083750464638,
-            "angularVelocity": -0.46340502818407264,
-            "velocityX": -0.9962181217957636,
-            "velocityY": -2.555270476750044,
-            "timestamp": 1.9172047406506962
-        },
-        {
-            "x": 1.817164507457459,
-            "y": 5.402475290970386,
-            "heading": -0.23166704244574374,
-            "angularVelocity": -0.47695375945076895,
-            "velocityX": -0.4617560049104581,
-            "velocityY": -2.7458719150120894,
-            "timestamp": 1.9852588585941753
-        },
-        {
-            "x": 1.8230483770902466,
-            "y": 5.205640044676734,
-            "heading": -0.2645113992669886,
-            "angularVelocity": -0.4826211523089798,
-            "velocityX": 0.08645868627131759,
-            "velocityY": -2.8923341047058035,
-            "timestamp": 2.0533129765376543
-        },
-        {
-            "x": 1.8673165212991787,
-            "y": 5.004575832748109,
-            "heading": -0.29674148680613366,
-            "angularVelocity": -0.4735949640250024,
-            "velocityX": 0.6504844313118886,
-            "velocityY": -2.9544753205907597,
-            "timestamp": 2.1213670944811334
-        },
-        {
-            "x": 1.9491517735306563,
-            "y": 4.812420400099087,
-            "heading": -0.3261812067352161,
-            "angularVelocity": -0.43259277790693457,
-            "velocityX": 1.202502577425556,
-            "velocityY": -2.823568043429816,
-            "timestamp": 2.1894212124246124
-        },
-        {
-            "x": 2.050948548896826,
-            "y": 4.653276186897604,
-            "heading": -0.34937275798086526,
-            "angularVelocity": -0.34078101291423535,
-            "velocityX": 1.4958209501847175,
-            "velocityY": -2.338494980328238,
-            "timestamp": 2.2574753303680914
-        },
-        {
-            "x": 2.14520275900139,
-            "y": 4.5319666650276815,
-            "heading": -0.36647677931046935,
-            "angularVelocity": -0.2513297041615066,
-            "velocityX": 1.3849890785871777,
-            "velocityY": -1.7825449147790617,
-            "timestamp": 2.3255294483115705
-        },
-        {
-            "x": 2.2207736095496604,
-            "y": 4.444424274499807,
-            "heading": -0.37856224680514944,
-            "angularVelocity": -0.17758613086130492,
-            "velocityX": 1.1104522816836297,
-            "velocityY": -1.2863643402237204,
-            "timestamp": 2.3935835662550495
-        },
-        {
-            "x": 2.273137984258771,
-            "y": 4.387723780671385,
-            "heading": -0.38627299411990695,
-            "angularVelocity": -0.113303170297753,
-            "velocityX": 0.7694519639887757,
-            "velocityY": -0.8331677133117752,
-            "timestamp": 2.4616376841985286
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.054765324902410414,
-            "velocityX": 0.3947155080835744,
-            "velocityY": -0.4073784439379148,
-            "timestamp": 2.5296918021420076
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 3.1303206075717874e-30,
-            "velocityX": 0,
-            "velocityY": -1.5697460792586675e-31,
-            "timestamp": 2.5977459200854867
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.1.traj
deleted file mode 100644
index 90a593eb..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.1.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -3.817834980565894e-40,
-            "angularVelocity": -1.9855221195343998e-39,
-            "velocityX": -9.621145750064654e-40,
-            "velocityY": 1.4575297691012433e-39,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 5.880500005575846,
-            "heading": 3.479114156010015e-22,
-            "angularVelocity": 3.4766412561552746e-21,
-            "velocityX": 2.7068848151306367e-33,
-            "velocityY": 0.8344065481228213,
-            "timestamp": 0.10007112931184182
-        },
-        {
-            "x": 7.68,
-            "y": 6.047500013629847,
-            "heading": 1.081691491825702e-21,
-            "angularVelocity": 7.332585052389997e-21,
-            "velocityX": 5.1589068200097806e-33,
-            "velocityY": 1.6688130652907358,
-            "timestamp": 0.20014225862368365
-        },
-        {
-            "x": 7.68,
-            "y": 6.298000017966616,
-            "heading": 2.2511586271387426e-21,
-            "angularVelocity": 1.1686358860616172e-20,
-            "velocityX": 5.8498916949463956e-33,
-            "velocityY": 2.503219520548835,
-            "timestamp": 0.3002133879355255
-        },
-        {
-            "x": 7.68,
-            "y": 6.632,
-            "heading": 4.481673966100434e-21,
-            "angularVelocity": 2.228929929167172e-20,
-            "velocityX": 6.576791251337367e-33,
-            "velocityY": 3.3376257900775013,
-            "timestamp": 0.4002845172473673
-        },
-        {
-            "x": 7.68,
-            "y": 6.9659999820333836,
-            "heading": 1.4391637904192512e-21,
-            "angularVelocity": -3.0403475801105514e-20,
-            "velocityX": 1.0509973010204301e-32,
-            "velocityY": 3.3376257900775013,
-            "timestamp": 0.5003556465592092
-        },
-        {
-            "x": 7.68,
-            "y": 7.216499986370152,
-            "heading": 4.7457500532831525e-22,
-            "angularVelocity": -9.63903176962576e-21,
-            "velocityX": 7.983324746463204e-33,
-            "velocityY": 2.503219520548835,
-            "timestamp": 0.600426775871051
-        },
-        {
-            "x": 7.68,
-            "y": 7.383499994424153,
-            "heading": 1.0909640932435146e-22,
-            "angularVelocity": -3.652188228166023e-21,
-            "velocityX": 7.341551448907972e-33,
-            "velocityY": 1.6688130652907358,
-            "timestamp": 0.7004979051828928
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -9.42965601107167e-40,
-            "angularVelocity": -1.09018866193586e-21,
-            "velocityX": 7.44470103227488e-33,
-            "velocityY": 0.8344065481228213,
-            "timestamp": 0.8005690344947346
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 8.249839679439821e-43,
-            "angularVelocity": 9.431229878048832e-39,
-            "velocityX": 1.012759498983901e-41,
-            "velocityY": 5.142787784847508e-39,
-            "timestamp": 0.9006401638065764
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.traj
deleted file mode 100644
index 90a593eb..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -3.817834980565894e-40,
-            "angularVelocity": -1.9855221195343998e-39,
-            "velocityX": -9.621145750064654e-40,
-            "velocityY": 1.4575297691012433e-39,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 5.880500005575846,
-            "heading": 3.479114156010015e-22,
-            "angularVelocity": 3.4766412561552746e-21,
-            "velocityX": 2.7068848151306367e-33,
-            "velocityY": 0.8344065481228213,
-            "timestamp": 0.10007112931184182
-        },
-        {
-            "x": 7.68,
-            "y": 6.047500013629847,
-            "heading": 1.081691491825702e-21,
-            "angularVelocity": 7.332585052389997e-21,
-            "velocityX": 5.1589068200097806e-33,
-            "velocityY": 1.6688130652907358,
-            "timestamp": 0.20014225862368365
-        },
-        {
-            "x": 7.68,
-            "y": 6.298000017966616,
-            "heading": 2.2511586271387426e-21,
-            "angularVelocity": 1.1686358860616172e-20,
-            "velocityX": 5.8498916949463956e-33,
-            "velocityY": 2.503219520548835,
-            "timestamp": 0.3002133879355255
-        },
-        {
-            "x": 7.68,
-            "y": 6.632,
-            "heading": 4.481673966100434e-21,
-            "angularVelocity": 2.228929929167172e-20,
-            "velocityX": 6.576791251337367e-33,
-            "velocityY": 3.3376257900775013,
-            "timestamp": 0.4002845172473673
-        },
-        {
-            "x": 7.68,
-            "y": 6.9659999820333836,
-            "heading": 1.4391637904192512e-21,
-            "angularVelocity": -3.0403475801105514e-20,
-            "velocityX": 1.0509973010204301e-32,
-            "velocityY": 3.3376257900775013,
-            "timestamp": 0.5003556465592092
-        },
-        {
-            "x": 7.68,
-            "y": 7.216499986370152,
-            "heading": 4.7457500532831525e-22,
-            "angularVelocity": -9.63903176962576e-21,
-            "velocityX": 7.983324746463204e-33,
-            "velocityY": 2.503219520548835,
-            "timestamp": 0.600426775871051
-        },
-        {
-            "x": 7.68,
-            "y": 7.383499994424153,
-            "heading": 1.0909640932435146e-22,
-            "angularVelocity": -3.652188228166023e-21,
-            "velocityX": 7.341551448907972e-33,
-            "velocityY": 1.6688130652907358,
-            "timestamp": 0.7004979051828928
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -9.42965601107167e-40,
-            "angularVelocity": -1.09018866193586e-21,
-            "velocityX": 7.44470103227488e-33,
-            "velocityY": 0.8344065481228213,
-            "timestamp": 0.8005690344947346
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 8.249839679439821e-43,
-            "angularVelocity": 9.431229878048832e-39,
-            "velocityX": 1.012759498983901e-41,
-            "velocityY": 5.142787784847508e-39,
-            "timestamp": 0.9006401638065764
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.1.traj
deleted file mode 100644
index 74357de2..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.1.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -5.382102664363315e-41,
-            "angularVelocity": -2.8652892976895567e-45,
-            "velocityX": 1.592735858156421e-40,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 5.713499994424153,
-            "heading": 1.659495816830368e-21,
-            "angularVelocity": 1.658316243120447e-20,
-            "velocityX": 2.780383744489643e-33,
-            "velocityY": -0.8344065481228213,
-            "timestamp": 0.10007112931184184
-        },
-        {
-            "x": 7.68,
-            "y": 5.546499986370153,
-            "heading": 4.9512387638524226e-21,
-            "angularVelocity": 3.289403167370527e-20,
-            "velocityX": 3.618624574631574e-33,
-            "velocityY": -1.668813065290736,
-            "timestamp": 0.20014225862368368
-        },
-        {
-            "x": 7.68,
-            "y": 5.295999982033384,
-            "heading": 9.801760970151702e-21,
-            "angularVelocity": 4.8470744390330394e-20,
-            "velocityX": 3.439292531132709e-33,
-            "velocityY": -2.503219520548835,
-            "timestamp": 0.30021338793552554
-        },
-        {
-            "x": 7.68,
-            "y": 4.962,
-            "heading": 1.506109654033251e-20,
-            "angularVelocity": 5.255597191401202e-20,
-            "velocityX": 4.639675238422859e-33,
-            "velocityY": -3.3376257900775017,
-            "timestamp": 0.40028451724736736
-        },
-        {
-            "x": 7.68,
-            "y": 4.628000017966616,
-            "heading": 5.3586032763760104e-21,
-            "angularVelocity": -9.69559666912617e-20,
-            "velocityX": 4.935439546802769e-33,
-            "velocityY": -3.3376257900775017,
-            "timestamp": 0.5003556465592092
-        },
-        {
-            "x": 7.68,
-            "y": 4.377500013629846,
-            "heading": 1.8760590350448333e-21,
-            "angularVelocity": -3.480068842301423e-20,
-            "velocityX": 4.701730454770122e-33,
-            "velocityY": -2.5032195205488352,
-            "timestamp": 0.600426775871051
-        },
-        {
-            "x": 7.68,
-            "y": 4.210500005575846,
-            "heading": 3.168722428707802e-22,
-            "angularVelocity": -1.5580785203142817e-20,
-            "velocityX": 3.49539235617723e-33,
-            "velocityY": -1.668813065290736,
-            "timestamp": 0.7004979051828928
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 5.881496870004905e-41,
-            "angularVelocity": -3.1664700918333573e-21,
-            "velocityX": 3.364378386644386e-33,
-            "velocityY": -0.8344065481228213,
-            "timestamp": 0.8005690344947346
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 9.145026397391945e-42,
-            "angularVelocity": -4.963805231001231e-40,
-            "velocityX": -4.870998572760777e-42,
-            "velocityY": 3.489748854003666e-39,
-            "timestamp": 0.9006401638065764
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.traj
deleted file mode 100644
index 74357de2..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -5.382102664363315e-41,
-            "angularVelocity": -2.8652892976895567e-45,
-            "velocityX": 1.592735858156421e-40,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 5.713499994424153,
-            "heading": 1.659495816830368e-21,
-            "angularVelocity": 1.658316243120447e-20,
-            "velocityX": 2.780383744489643e-33,
-            "velocityY": -0.8344065481228213,
-            "timestamp": 0.10007112931184184
-        },
-        {
-            "x": 7.68,
-            "y": 5.546499986370153,
-            "heading": 4.9512387638524226e-21,
-            "angularVelocity": 3.289403167370527e-20,
-            "velocityX": 3.618624574631574e-33,
-            "velocityY": -1.668813065290736,
-            "timestamp": 0.20014225862368368
-        },
-        {
-            "x": 7.68,
-            "y": 5.295999982033384,
-            "heading": 9.801760970151702e-21,
-            "angularVelocity": 4.8470744390330394e-20,
-            "velocityX": 3.439292531132709e-33,
-            "velocityY": -2.503219520548835,
-            "timestamp": 0.30021338793552554
-        },
-        {
-            "x": 7.68,
-            "y": 4.962,
-            "heading": 1.506109654033251e-20,
-            "angularVelocity": 5.255597191401202e-20,
-            "velocityX": 4.639675238422859e-33,
-            "velocityY": -3.3376257900775017,
-            "timestamp": 0.40028451724736736
-        },
-        {
-            "x": 7.68,
-            "y": 4.628000017966616,
-            "heading": 5.3586032763760104e-21,
-            "angularVelocity": -9.69559666912617e-20,
-            "velocityX": 4.935439546802769e-33,
-            "velocityY": -3.3376257900775017,
-            "timestamp": 0.5003556465592092
-        },
-        {
-            "x": 7.68,
-            "y": 4.377500013629846,
-            "heading": 1.8760590350448333e-21,
-            "angularVelocity": -3.480068842301423e-20,
-            "velocityX": 4.701730454770122e-33,
-            "velocityY": -2.5032195205488352,
-            "timestamp": 0.600426775871051
-        },
-        {
-            "x": 7.68,
-            "y": 4.210500005575846,
-            "heading": 3.168722428707802e-22,
-            "angularVelocity": -1.5580785203142817e-20,
-            "velocityX": 3.49539235617723e-33,
-            "velocityY": -1.668813065290736,
-            "timestamp": 0.7004979051828928
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 5.881496870004905e-41,
-            "angularVelocity": -3.1664700918333573e-21,
-            "velocityX": 3.364378386644386e-33,
-            "velocityY": -0.8344065481228213,
-            "timestamp": 0.8005690344947346
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 9.145026397391945e-42,
-            "angularVelocity": -4.963805231001231e-40,
-            "velocityX": -4.870998572760777e-42,
-            "velocityY": 3.489748854003666e-39,
-            "timestamp": 0.9006401638065764
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.1.traj
deleted file mode 100644
index bcede3cb..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.1.traj	
+++ /dev/null
@@ -1,139 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 7.067561628822357e-40,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 5.720554627994461,
-            "heading": -3.8243496398495774e-19,
-            "angularVelocity": -3.994077070459528e-18,
-            "velocityX": -4.592295404090674e-34,
-            "velocityY": -0.7983807350946677,
-            "timestamp": 0.09575052183150977
-        },
-        {
-            "x": 7.68,
-            "y": 5.567663887017311,
-            "heading": -1.1460790562861162e-18,
-            "angularVelocity": -7.975351749671857e-18,
-            "velocityX": 3.216033275420413e-35,
-            "velocityY": -1.5967614385035829,
-            "timestamp": 0.19150104366301954
-        },
-        {
-            "x": 7.68,
-            "y": 5.3383277831364,
-            "heading": -2.289011643788579e-18,
-            "angularVelocity": -1.1936567706665239e-17,
-            "velocityX": -6.99395718741277e-36,
-            "velocityY": -2.3951420785410233,
-            "timestamp": 0.2872515654945293
-        },
-        {
-            "x": 7.68,
-            "y": 5.032546334555254,
-            "heading": -3.801510097022925e-18,
-            "angularVelocity": -1.5796242396621782e-17,
-            "velocityX": -1.9389720200265424e-34,
-            "velocityY": -3.1935225284643693,
-            "timestamp": 0.3830020873260391
-        },
-        {
-            "x": 7.68,
-            "y": 4.670327800733179,
-            "heading": -7.244345840319028e-19,
-            "angularVelocity": 3.2136383741131935e-17,
-            "velocityX": -8.432373407035536e-35,
-            "velocityY": -3.7829405719528495,
-            "timestamp": 0.47875260915754886
-        },
-        {
-            "x": 7.68,
-            "y": 4.308109266911059,
-            "heading": -3.757861819981246e-18,
-            "angularVelocity": -3.168052954622205e-17,
-            "velocityX": -1.6004910806259417e-34,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.5745031309890586
-        },
-        {
-            "x": 7.68,
-            "y": 3.9458907330889397,
-            "heading": -5.3976563030855884e-18,
-            "angularVelocity": -1.712569761227889e-17,
-            "velocityX": -2.8603575042124878e-33,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.6702536528205684
-        },
-        {
-            "x": 7.68,
-            "y": 3.5836721992668203,
-            "heading": -4.35645304230582e-18,
-            "angularVelocity": 1.0874126227865062e-17,
-            "velocityX": -1.9073271103237224e-34,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.7660041746520782
-        },
-        {
-            "x": 7.68,
-            "y": 3.221453665444745,
-            "heading": 3.661990694649205e-18,
-            "angularVelocity": 8.37430813282138e-17,
-            "velocityX": -3.48109229998139e-33,
-            "velocityY": -3.7829405719528495,
-            "timestamp": 0.8617546964835879
-        },
-        {
-            "x": 7.68,
-            "y": 2.915672216863599,
-            "heading": 2.222559506115206e-18,
-            "angularVelocity": -1.503314198751682e-17,
-            "velocityX": 7.233727398507661e-34,
-            "velocityY": -3.1935225284643693,
-            "timestamp": 0.9575052183150977
-        },
-        {
-            "x": 7.68,
-            "y": 2.686336112982689,
-            "heading": 1.1174854461234148e-18,
-            "angularVelocity": -1.1541180553049724e-17,
-            "velocityX": 7.218959232022663e-34,
-            "velocityY": -2.3951420785410233,
-            "timestamp": 1.0532557401466074
-        },
-        {
-            "x": 7.68,
-            "y": 2.5334453720055388,
-            "heading": 3.7383227049768543e-19,
-            "angularVelocity": -7.766570474567622e-18,
-            "velocityX": 1.7872274250399634e-33,
-            "velocityY": -1.5967614385035829,
-            "timestamp": 1.1490062619781172
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -3.904232200157263e-18,
-            "velocityX": 6.089132400706592e-33,
-            "velocityY": -0.7983807350946677,
-            "timestamp": 1.244756783809627
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 3.5763678522980893e-43,
-            "velocityY": -5.882608067207678e-38,
-            "timestamp": 1.3405073056411367
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.traj
deleted file mode 100644
index bcede3cb..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.traj	
+++ /dev/null
@@ -1,139 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 7.067561628822357e-40,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 5.720554627994461,
-            "heading": -3.8243496398495774e-19,
-            "angularVelocity": -3.994077070459528e-18,
-            "velocityX": -4.592295404090674e-34,
-            "velocityY": -0.7983807350946677,
-            "timestamp": 0.09575052183150977
-        },
-        {
-            "x": 7.68,
-            "y": 5.567663887017311,
-            "heading": -1.1460790562861162e-18,
-            "angularVelocity": -7.975351749671857e-18,
-            "velocityX": 3.216033275420413e-35,
-            "velocityY": -1.5967614385035829,
-            "timestamp": 0.19150104366301954
-        },
-        {
-            "x": 7.68,
-            "y": 5.3383277831364,
-            "heading": -2.289011643788579e-18,
-            "angularVelocity": -1.1936567706665239e-17,
-            "velocityX": -6.99395718741277e-36,
-            "velocityY": -2.3951420785410233,
-            "timestamp": 0.2872515654945293
-        },
-        {
-            "x": 7.68,
-            "y": 5.032546334555254,
-            "heading": -3.801510097022925e-18,
-            "angularVelocity": -1.5796242396621782e-17,
-            "velocityX": -1.9389720200265424e-34,
-            "velocityY": -3.1935225284643693,
-            "timestamp": 0.3830020873260391
-        },
-        {
-            "x": 7.68,
-            "y": 4.670327800733179,
-            "heading": -7.244345840319028e-19,
-            "angularVelocity": 3.2136383741131935e-17,
-            "velocityX": -8.432373407035536e-35,
-            "velocityY": -3.7829405719528495,
-            "timestamp": 0.47875260915754886
-        },
-        {
-            "x": 7.68,
-            "y": 4.308109266911059,
-            "heading": -3.757861819981246e-18,
-            "angularVelocity": -3.168052954622205e-17,
-            "velocityX": -1.6004910806259417e-34,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.5745031309890586
-        },
-        {
-            "x": 7.68,
-            "y": 3.9458907330889397,
-            "heading": -5.3976563030855884e-18,
-            "angularVelocity": -1.712569761227889e-17,
-            "velocityX": -2.8603575042124878e-33,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.6702536528205684
-        },
-        {
-            "x": 7.68,
-            "y": 3.5836721992668203,
-            "heading": -4.35645304230582e-18,
-            "angularVelocity": 1.0874126227865062e-17,
-            "velocityX": -1.9073271103237224e-34,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.7660041746520782
-        },
-        {
-            "x": 7.68,
-            "y": 3.221453665444745,
-            "heading": 3.661990694649205e-18,
-            "angularVelocity": 8.37430813282138e-17,
-            "velocityX": -3.48109229998139e-33,
-            "velocityY": -3.7829405719528495,
-            "timestamp": 0.8617546964835879
-        },
-        {
-            "x": 7.68,
-            "y": 2.915672216863599,
-            "heading": 2.222559506115206e-18,
-            "angularVelocity": -1.503314198751682e-17,
-            "velocityX": 7.233727398507661e-34,
-            "velocityY": -3.1935225284643693,
-            "timestamp": 0.9575052183150977
-        },
-        {
-            "x": 7.68,
-            "y": 2.686336112982689,
-            "heading": 1.1174854461234148e-18,
-            "angularVelocity": -1.1541180553049724e-17,
-            "velocityX": 7.218959232022663e-34,
-            "velocityY": -2.3951420785410233,
-            "timestamp": 1.0532557401466074
-        },
-        {
-            "x": 7.68,
-            "y": 2.5334453720055388,
-            "heading": 3.7383227049768543e-19,
-            "angularVelocity": -7.766570474567622e-18,
-            "velocityX": 1.7872274250399634e-33,
-            "velocityY": -1.5967614385035829,
-            "timestamp": 1.1490062619781172
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -3.904232200157263e-18,
-            "velocityX": 6.089132400706592e-33,
-            "velocityY": -0.7983807350946677,
-            "timestamp": 1.244756783809627
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 3.5763678522980893e-43,
-            "velocityY": -5.882608067207678e-38,
-            "timestamp": 1.3405073056411367
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.1.traj
deleted file mode 100644
index 3fd40c6e..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.1.traj	
+++ /dev/null
@@ -1,175 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -1.2882449649302714e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 5.715155846048237,
-            "heading": -2.6675793503802873e-18,
-            "angularVelocity": -2.692513973443352e-17,
-            "velocityX": 2.411941793716939e-19,
-            "velocityY": -0.8260917454276263,
-            "timestamp": 0.09907392781100396
-        },
-        {
-            "x": 7.68,
-            "y": 5.55146754125807,
-            "heading": -8.002991694377614e-18,
-            "angularVelocity": -5.385283961043983e-17,
-            "velocityX": 4.822926900034719e-19,
-            "velocityY": -1.6521834594306495,
-            "timestamp": 0.19814785562200793
-        },
-        {
-            "x": 7.68,
-            "y": 5.305935091856216,
-            "heading": -1.6007101079545214e-17,
-            "angularVelocity": -8.07892607262788e-17,
-            "velocityX": 7.23199861883135e-19,
-            "velocityY": -2.478275110584479,
-            "timestamp": 0.29722178343301187
-        },
-        {
-            "x": 7.68,
-            "y": 4.978558516522808,
-            "heading": -2.6680861930648114e-17,
-            "angularVelocity": -1.0773531530269989e-16,
-            "velocityX": 9.635330187135107e-19,
-            "velocityY": -3.3043665731908765,
-            "timestamp": 0.39629571124401586
-        },
-        {
-            "x": 7.68,
-            "y": 4.603767735073314,
-            "heading": -3.033567109313287e-17,
-            "angularVelocity": -3.688971703490728e-17,
-            "velocityX": 1.1102275506466142e-18,
-            "velocityY": -3.782940575087071,
-            "timestamp": 0.49536963905501985
-        },
-        {
-            "x": 7.68,
-            "y": 4.228976953623796,
-            "heading": -3.5529792774719166e-17,
-            "angularVelocity": -5.2426726146536295e-17,
-            "velocityX": 1.1087814850277944e-18,
-            "velocityY": -3.7829405750873115,
-            "timestamp": 0.5944435668660238
-        },
-        {
-            "x": 7.68,
-            "y": 3.8541861721742774,
-            "heading": -4.8775545061581196e-17,
-            "angularVelocity": -1.3369564101798844e-16,
-            "velocityX": 1.1066129122730268e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.6935174946770278
-        },
-        {
-            "x": 7.68,
-            "y": 3.4793953907247595,
-            "heading": -3.3383639047364345e-17,
-            "angularVelocity": 1.5535778538404392e-16,
-            "velocityX": 1.113993391664377e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.7925914224880318
-        },
-        {
-            "x": 7.68,
-            "y": 3.104604609275241,
-            "heading": -4.339521377299963e-18,
-            "angularVelocity": 2.9315601300747485e-16,
-            "velocityX": 1.114171335036007e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.8916653502990358
-        },
-        {
-            "x": 7.68,
-            "y": 2.7298138278257227,
-            "heading": 1.3174711336089981e-17,
-            "angularVelocity": 1.7677943229992181e-16,
-            "velocityX": 1.1158489583207238e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.9907392781100398
-        },
-        {
-            "x": 7.68,
-            "y": 2.3550230463762043,
-            "heading": 2.965421117619106e-17,
-            "angularVelocity": 1.6633538413393434e-16,
-            "velocityX": 1.1181357718618657e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 1.0898132059210437
-        },
-        {
-            "x": 7.68,
-            "y": 1.9802322649266861,
-            "heading": 2.902288280090785e-17,
-            "angularVelocity": -6.372295813531433e-18,
-            "velocityX": 1.121394742098521e-18,
-            "velocityY": -3.7829405750873115,
-            "timestamp": 1.1888871337320477
-        },
-        {
-            "x": 7.68,
-            "y": 1.605441483477192,
-            "heading": 2.667314494747493e-17,
-            "angularVelocity": -2.3717015216307205e-17,
-            "velocityX": 1.1050727300415562e-18,
-            "velocityY": -3.782940575087071,
-            "timestamp": 1.2879610615430517
-        },
-        {
-            "x": 7.68,
-            "y": 1.2780649081437845,
-            "heading": 1.6004201006274906e-17,
-            "angularVelocity": -1.0768669595616384e-16,
-            "velocityX": 9.663666503471993e-19,
-            "velocityY": -3.304366573190877,
-            "timestamp": 1.3870349893540557
-        },
-        {
-            "x": 7.68,
-            "y": 1.0325324587419298,
-            "heading": 8.002125258512226e-18,
-            "angularVelocity": -8.076873426351199e-17,
-            "velocityX": 7.247750412790524e-19,
-            "velocityY": -2.478275110584479,
-            "timestamp": 1.4861089171650597
-        },
-        {
-            "x": 7.68,
-            "y": 0.868844153951763,
-            "heading": 2.6673738748743342e-18,
-            "angularVelocity": -5.384616822543564e-17,
-            "velocityX": 4.831833763651305e-19,
-            "velocityY": -1.6521834594306495,
-            "timestamp": 1.5851828449760637
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -2.6923065773077948e-17,
-            "velocityX": 2.415916928361511e-19,
-            "velocityY": -0.8260917454276263,
-            "timestamp": 1.6842567727870676
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -1.1487176577652646e-33,
-            "velocityY": 0,
-            "timestamp": 1.7833307005980716
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.traj
deleted file mode 100644
index 3fd40c6e..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.traj	
+++ /dev/null
@@ -1,175 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -1.2882449649302714e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 5.715155846048237,
-            "heading": -2.6675793503802873e-18,
-            "angularVelocity": -2.692513973443352e-17,
-            "velocityX": 2.411941793716939e-19,
-            "velocityY": -0.8260917454276263,
-            "timestamp": 0.09907392781100396
-        },
-        {
-            "x": 7.68,
-            "y": 5.55146754125807,
-            "heading": -8.002991694377614e-18,
-            "angularVelocity": -5.385283961043983e-17,
-            "velocityX": 4.822926900034719e-19,
-            "velocityY": -1.6521834594306495,
-            "timestamp": 0.19814785562200793
-        },
-        {
-            "x": 7.68,
-            "y": 5.305935091856216,
-            "heading": -1.6007101079545214e-17,
-            "angularVelocity": -8.07892607262788e-17,
-            "velocityX": 7.23199861883135e-19,
-            "velocityY": -2.478275110584479,
-            "timestamp": 0.29722178343301187
-        },
-        {
-            "x": 7.68,
-            "y": 4.978558516522808,
-            "heading": -2.6680861930648114e-17,
-            "angularVelocity": -1.0773531530269989e-16,
-            "velocityX": 9.635330187135107e-19,
-            "velocityY": -3.3043665731908765,
-            "timestamp": 0.39629571124401586
-        },
-        {
-            "x": 7.68,
-            "y": 4.603767735073314,
-            "heading": -3.033567109313287e-17,
-            "angularVelocity": -3.688971703490728e-17,
-            "velocityX": 1.1102275506466142e-18,
-            "velocityY": -3.782940575087071,
-            "timestamp": 0.49536963905501985
-        },
-        {
-            "x": 7.68,
-            "y": 4.228976953623796,
-            "heading": -3.5529792774719166e-17,
-            "angularVelocity": -5.2426726146536295e-17,
-            "velocityX": 1.1087814850277944e-18,
-            "velocityY": -3.7829405750873115,
-            "timestamp": 0.5944435668660238
-        },
-        {
-            "x": 7.68,
-            "y": 3.8541861721742774,
-            "heading": -4.8775545061581196e-17,
-            "angularVelocity": -1.3369564101798844e-16,
-            "velocityX": 1.1066129122730268e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.6935174946770278
-        },
-        {
-            "x": 7.68,
-            "y": 3.4793953907247595,
-            "heading": -3.3383639047364345e-17,
-            "angularVelocity": 1.5535778538404392e-16,
-            "velocityX": 1.113993391664377e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.7925914224880318
-        },
-        {
-            "x": 7.68,
-            "y": 3.104604609275241,
-            "heading": -4.339521377299963e-18,
-            "angularVelocity": 2.9315601300747485e-16,
-            "velocityX": 1.114171335036007e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.8916653502990358
-        },
-        {
-            "x": 7.68,
-            "y": 2.7298138278257227,
-            "heading": 1.3174711336089981e-17,
-            "angularVelocity": 1.7677943229992181e-16,
-            "velocityX": 1.1158489583207238e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.9907392781100398
-        },
-        {
-            "x": 7.68,
-            "y": 2.3550230463762043,
-            "heading": 2.965421117619106e-17,
-            "angularVelocity": 1.6633538413393434e-16,
-            "velocityX": 1.1181357718618657e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 1.0898132059210437
-        },
-        {
-            "x": 7.68,
-            "y": 1.9802322649266861,
-            "heading": 2.902288280090785e-17,
-            "angularVelocity": -6.372295813531433e-18,
-            "velocityX": 1.121394742098521e-18,
-            "velocityY": -3.7829405750873115,
-            "timestamp": 1.1888871337320477
-        },
-        {
-            "x": 7.68,
-            "y": 1.605441483477192,
-            "heading": 2.667314494747493e-17,
-            "angularVelocity": -2.3717015216307205e-17,
-            "velocityX": 1.1050727300415562e-18,
-            "velocityY": -3.782940575087071,
-            "timestamp": 1.2879610615430517
-        },
-        {
-            "x": 7.68,
-            "y": 1.2780649081437845,
-            "heading": 1.6004201006274906e-17,
-            "angularVelocity": -1.0768669595616384e-16,
-            "velocityX": 9.663666503471993e-19,
-            "velocityY": -3.304366573190877,
-            "timestamp": 1.3870349893540557
-        },
-        {
-            "x": 7.68,
-            "y": 1.0325324587419298,
-            "heading": 8.002125258512226e-18,
-            "angularVelocity": -8.076873426351199e-17,
-            "velocityX": 7.247750412790524e-19,
-            "velocityY": -2.478275110584479,
-            "timestamp": 1.4861089171650597
-        },
-        {
-            "x": 7.68,
-            "y": 0.868844153951763,
-            "heading": 2.6673738748743342e-18,
-            "angularVelocity": -5.384616822543564e-17,
-            "velocityX": 4.831833763651305e-19,
-            "velocityY": -1.6521834594306495,
-            "timestamp": 1.5851828449760637
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -2.6923065773077948e-17,
-            "velocityX": 2.415916928361511e-19,
-            "velocityY": -0.8260917454276263,
-            "timestamp": 1.6842567727870676
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -1.1487176577652646e-33,
-            "velocityY": 0,
-            "timestamp": 1.7833307005980716
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.1.traj
deleted file mode 100644
index 6cbc4c1d..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 7.776001513534033e-33,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.620476769374415,
-            "y": 5.82809535402629,
-            "heading": -0.014618874829729222,
-            "angularVelocity": -0.16274311325472623,
-            "velocityX": -0.6626362135126213,
-            "velocityY": 0.34616581514913597,
-            "timestamp": 0.0898279167539809
-        },
-        {
-            "x": 7.5013982904113785,
-            "y": 5.890300493780534,
-            "heading": -0.04163561529349077,
-            "angularVelocity": -0.30076107116849365,
-            "velocityX": -1.3256288608937272,
-            "velocityY": 0.6924922897255835,
-            "timestamp": 0.1796558335079618
-        },
-        {
-            "x": 7.3227216232188646,
-            "y": 5.983633458070523,
-            "heading": -0.07726722819497155,
-            "angularVelocity": -0.39666524827763777,
-            "velocityX": -1.9890995321851956,
-            "velocityY": 1.0390195794655717,
-            "timestamp": 0.2694837502619427
-        },
-        {
-            "x": 7.084404558965627,
-            "y": 6.108106280619183,
-            "heading": -0.11365614248079976,
-            "angularVelocity": -0.40509582767559393,
-            "velocityX": -2.6530400889284445,
-            "velocityY": 1.3856808333823862,
-            "timestamp": 0.3593116670159236
-        },
-        {
-            "x": 6.786764320803032,
-            "y": 6.263508027124416,
-            "heading": -0.12453213515982267,
-            "angularVelocity": -0.12107586452004557,
-            "velocityX": -3.313449191722499,
-            "velocityY": 1.7299938829800985,
-            "timestamp": 0.4491395837699045
-        },
-        {
-            "x": 6.485598483527544,
-            "y": 6.42090073842616,
-            "heading": -0.1245321478638912,
-            "angularVelocity": -1.4142672994383305e-7,
-            "velocityX": -3.3526975594938513,
-            "velocityY": 1.7521580928210598,
-            "timestamp": 0.5389675005238854
-        },
-        {
-            "x": 6.164419225817841,
-            "y": 6.531883674964022,
-            "heading": -0.12453214704403566,
-            "angularVelocity": 9.126957138704023e-9,
-            "velocityX": -3.575494894191328,
-            "velocityY": 1.2355060714790946,
-            "timestamp": 0.6287954172778664
-        },
-        {
-            "x": 5.838530026083828,
-            "y": 6.576482035403971,
-            "heading": -0.08897109734241883,
-            "angularVelocity": 0.39587971074750417,
-            "velocityX": -3.62792783702814,
-            "velocityY": 0.4964866385813549,
-            "timestamp": 0.7186233340318473
-        },
-        {
-            "x": 5.57314678987786,
-            "y": 6.601508855183474,
-            "heading": 0.02708371106201491,
-            "angularVelocity": 1.2919681608812392,
-            "velocityX": -2.9543514510393742,
-            "velocityY": 0.27860848479928674,
-            "timestamp": 0.8084512507858282
-        },
-        {
-            "x": 5.3743330310505755,
-            "y": 6.616929267325104,
-            "heading": 0.1370261836602847,
-            "angularVelocity": 1.2239232141983023,
-            "velocityX": -2.213273623742076,
-            "velocityY": 0.17166614454461948,
-            "timestamp": 0.8982791675398092
-        },
-        {
-            "x": 5.242037120273562,
-            "y": 6.625930657483227,
-            "heading": 0.21902488355780253,
-            "angularVelocity": 0.9128420524556349,
-            "velocityX": -1.4727705546076917,
-            "velocityY": 0.10020704568687104,
-            "timestamp": 0.9881070842937901
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": 0.48954843918551255,
-            "velocityX": -0.7351514168409599,
-            "velocityY": 0.0453015350218741,
-            "timestamp": 1.077935001047771
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": -4.780109867389741e-33,
-            "velocityX": 0,
-            "velocityY": -5.233682494366388e-34,
-            "timestamp": 1.167762917801752
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.traj
deleted file mode 100644
index 6cbc4c1d..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 7.776001513534033e-33,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.620476769374415,
-            "y": 5.82809535402629,
-            "heading": -0.014618874829729222,
-            "angularVelocity": -0.16274311325472623,
-            "velocityX": -0.6626362135126213,
-            "velocityY": 0.34616581514913597,
-            "timestamp": 0.0898279167539809
-        },
-        {
-            "x": 7.5013982904113785,
-            "y": 5.890300493780534,
-            "heading": -0.04163561529349077,
-            "angularVelocity": -0.30076107116849365,
-            "velocityX": -1.3256288608937272,
-            "velocityY": 0.6924922897255835,
-            "timestamp": 0.1796558335079618
-        },
-        {
-            "x": 7.3227216232188646,
-            "y": 5.983633458070523,
-            "heading": -0.07726722819497155,
-            "angularVelocity": -0.39666524827763777,
-            "velocityX": -1.9890995321851956,
-            "velocityY": 1.0390195794655717,
-            "timestamp": 0.2694837502619427
-        },
-        {
-            "x": 7.084404558965627,
-            "y": 6.108106280619183,
-            "heading": -0.11365614248079976,
-            "angularVelocity": -0.40509582767559393,
-            "velocityX": -2.6530400889284445,
-            "velocityY": 1.3856808333823862,
-            "timestamp": 0.3593116670159236
-        },
-        {
-            "x": 6.786764320803032,
-            "y": 6.263508027124416,
-            "heading": -0.12453213515982267,
-            "angularVelocity": -0.12107586452004557,
-            "velocityX": -3.313449191722499,
-            "velocityY": 1.7299938829800985,
-            "timestamp": 0.4491395837699045
-        },
-        {
-            "x": 6.485598483527544,
-            "y": 6.42090073842616,
-            "heading": -0.1245321478638912,
-            "angularVelocity": -1.4142672994383305e-7,
-            "velocityX": -3.3526975594938513,
-            "velocityY": 1.7521580928210598,
-            "timestamp": 0.5389675005238854
-        },
-        {
-            "x": 6.164419225817841,
-            "y": 6.531883674964022,
-            "heading": -0.12453214704403566,
-            "angularVelocity": 9.126957138704023e-9,
-            "velocityX": -3.575494894191328,
-            "velocityY": 1.2355060714790946,
-            "timestamp": 0.6287954172778664
-        },
-        {
-            "x": 5.838530026083828,
-            "y": 6.576482035403971,
-            "heading": -0.08897109734241883,
-            "angularVelocity": 0.39587971074750417,
-            "velocityX": -3.62792783702814,
-            "velocityY": 0.4964866385813549,
-            "timestamp": 0.7186233340318473
-        },
-        {
-            "x": 5.57314678987786,
-            "y": 6.601508855183474,
-            "heading": 0.02708371106201491,
-            "angularVelocity": 1.2919681608812392,
-            "velocityX": -2.9543514510393742,
-            "velocityY": 0.27860848479928674,
-            "timestamp": 0.8084512507858282
-        },
-        {
-            "x": 5.3743330310505755,
-            "y": 6.616929267325104,
-            "heading": 0.1370261836602847,
-            "angularVelocity": 1.2239232141983023,
-            "velocityX": -2.213273623742076,
-            "velocityY": 0.17166614454461948,
-            "timestamp": 0.8982791675398092
-        },
-        {
-            "x": 5.242037120273562,
-            "y": 6.625930657483227,
-            "heading": 0.21902488355780253,
-            "angularVelocity": 0.9128420524556349,
-            "velocityX": -1.4727705546076917,
-            "velocityY": 0.10020704568687104,
-            "timestamp": 0.9881070842937901
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": 0.48954843918551255,
-            "velocityX": -0.7351514168409599,
-            "velocityY": 0.0453015350218741,
-            "timestamp": 1.077935001047771
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": -4.780109867389741e-33,
-            "velocityX": 0,
-            "velocityY": -5.233682494366388e-34,
-            "timestamp": 1.167762917801752
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.1.traj
deleted file mode 100644
index 4e0731ce..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.1.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 1.3211551091100433e-31,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.6375302942972665,
-            "y": 5.822387711771769,
-            "heading": 0.010500881646553203,
-            "angularVelocity": 0.13619602846032355,
-            "velocityX": -0.5508304389364825,
-            "velocityY": 0.32927763890619355,
-            "timestamp": 0.07710123243140166
-        },
-        {
-            "x": 7.5525908811480225,
-            "y": 5.873163180716977,
-            "heading": 0.031500279151306286,
-            "angularVelocity": 0.27236137273728567,
-            "velocityX": -1.1016609004897018,
-            "velocityY": 0.6585558666702785,
-            "timestamp": 0.1542024648628033
-        },
-        {
-            "x": 7.4251817593524025,
-            "y": 5.949326484276257,
-            "heading": 0.06299451206522548,
-            "angularVelocity": 0.40847898173275193,
-            "velocityX": -1.6524913776051289,
-            "velocityY": 0.987835098836381,
-            "timestamp": 0.23130369729420497
-        },
-        {
-            "x": 7.255302939480135,
-            "y": 6.050877733979633,
-            "heading": 0.10497913316622995,
-            "angularVelocity": 0.5445389104299847,
-            "velocityX": -2.203321717631576,
-            "velocityY": 1.3171157775425744,
-            "timestamp": 0.3084049297256066
-        },
-        {
-            "x": 7.042954462706059,
-            "y": 6.177817072431174,
-            "heading": 0.1574496496087124,
-            "angularVelocity": 0.6805405670936119,
-            "velocityX": -2.754151523621958,
-            "velocityY": 1.6463983058180132,
-            "timestamp": 0.3855061621570083
-        },
-        {
-            "x": 6.792607460054783,
-            "y": 6.3274746833219355,
-            "heading": 0.15744965721454035,
-            "angularVelocity": 9.864729365588338e-8,
-            "velocityX": -3.246990933303352,
-            "velocityY": 1.941053419916652,
-            "timestamp": 0.46260739458840994
-        },
-        {
-            "x": 6.542259846259959,
-            "y": 6.4771312718889495,
-            "heading": 0.15744966481969572,
-            "angularVelocity": 9.863857053631023e-8,
-            "velocityX": -3.246998859811527,
-            "velocityY": 1.9410401604172332,
-            "timestamp": 0.5397086270198116
-        },
-        {
-            "x": 6.273256747030451,
-            "y": 6.589862608843688,
-            "heading": 0.1574496727775284,
-            "angularVelocity": 1.0321278187268036e-7,
-            "velocityX": -3.4889597837342676,
-            "velocityY": 1.4621210764047974,
-            "timestamp": 0.6168098594512132
-        },
-        {
-            "x": 5.98904964850956,
-            "y": 6.655416753006686,
-            "heading": 0.15744969119167346,
-            "angularVelocity": 2.3883074855376557e-7,
-            "velocityX": -3.686155065987494,
-            "velocityY": 0.8502347121535809,
-            "timestamp": 0.6939110918826149
-        },
-        {
-            "x": 5.6978465137603145,
-            "y": 6.6719003531074685,
-            "heading": 0.15744999446473232,
-            "angularVelocity": 0.00000393343983387992,
-            "velocityX": -3.7768933850484796,
-            "velocityY": 0.21379165521703997,
-            "timestamp": 0.7710123243140166
-        },
-        {
-            "x": 5.40805721282959,
-            "y": 6.638837814331055,
-            "heading": 0.15745035498223803,
-            "angularVelocity": 0.00000467589809324531,
-            "velocityX": -3.7585560151526085,
-            "velocityY": -0.428819848059242,
-            "timestamp": 0.8481135567454182
-        },
-        {
-            "x": 5.104307335648147,
-            "y": 6.562247743535093,
-            "heading": 0.18966206475475966,
-            "angularVelocity": 0.37702586381684944,
-            "velocityX": -3.555277277652879,
-            "velocityY": -0.896457772827513,
-            "timestamp": 0.9335498964342235
-        },
-        {
-            "x": 4.836190414730584,
-            "y": 6.438293970082571,
-            "heading": 0.27176288986041636,
-            "angularVelocity": 0.9609590650149814,
-            "velocityX": -3.1382070193334055,
-            "velocityY": -1.4508319750590115,
-            "timestamp": 1.0189862361230289
-        },
-        {
-            "x": 4.616497381460216,
-            "y": 6.278221072521664,
-            "heading": 0.3274582685749672,
-            "angularVelocity": 0.6518933151562509,
-            "velocityX": -2.5714237532949347,
-            "velocityY": -1.8735926438791781,
-            "timestamp": 1.1044225758118342
-        },
-        {
-            "x": 4.449006665280017,
-            "y": 6.09487376587715,
-            "heading": 0.3096216512620487,
-            "angularVelocity": -0.2087708506460702,
-            "velocityX": -1.960415401575849,
-            "velocityY": -2.1460107878256482,
-            "timestamp": 1.1898589155006396
-        },
-        {
-            "x": 4.339814814870372,
-            "y": 5.903698766708507,
-            "heading": 0.23521329843339078,
-            "angularVelocity": -0.8709215902704173,
-            "velocityX": -1.2780492564097095,
-            "velocityY": -2.237630964352897,
-            "timestamp": 1.275295255189445
-        },
-        {
-            "x": 4.286146359443328,
-            "y": 5.736442488850756,
-            "heading": 0.1356278137676922,
-            "angularVelocity": -1.1656103834554516,
-            "velocityX": -0.628168945703036,
-            "velocityY": -1.957671389796991,
-            "timestamp": 1.3607315948782504
-        },
-        {
-            "x": 4.266951931468271,
-            "y": 5.619203136466468,
-            "heading": 0.049466079264437586,
-            "angularVelocity": -1.0084904715849423,
-            "velocityX": -0.2246635102225946,
-            "velocityY": -1.372242219309977,
-            "timestamp": 1.4461679345670557
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 4.182976778726496e-34,
-            "angularVelocity": -0.578981724224301,
-            "velocityX": -0.046255861178807596,
-            "velocityY": -0.6929502912005617,
-            "timestamp": 1.531604274255861
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -6.30298269203181e-33,
-            "timestamp": 1.6170406139446665
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.traj
deleted file mode 100644
index 4e0731ce..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 1.3211551091100433e-31,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.6375302942972665,
-            "y": 5.822387711771769,
-            "heading": 0.010500881646553203,
-            "angularVelocity": 0.13619602846032355,
-            "velocityX": -0.5508304389364825,
-            "velocityY": 0.32927763890619355,
-            "timestamp": 0.07710123243140166
-        },
-        {
-            "x": 7.5525908811480225,
-            "y": 5.873163180716977,
-            "heading": 0.031500279151306286,
-            "angularVelocity": 0.27236137273728567,
-            "velocityX": -1.1016609004897018,
-            "velocityY": 0.6585558666702785,
-            "timestamp": 0.1542024648628033
-        },
-        {
-            "x": 7.4251817593524025,
-            "y": 5.949326484276257,
-            "heading": 0.06299451206522548,
-            "angularVelocity": 0.40847898173275193,
-            "velocityX": -1.6524913776051289,
-            "velocityY": 0.987835098836381,
-            "timestamp": 0.23130369729420497
-        },
-        {
-            "x": 7.255302939480135,
-            "y": 6.050877733979633,
-            "heading": 0.10497913316622995,
-            "angularVelocity": 0.5445389104299847,
-            "velocityX": -2.203321717631576,
-            "velocityY": 1.3171157775425744,
-            "timestamp": 0.3084049297256066
-        },
-        {
-            "x": 7.042954462706059,
-            "y": 6.177817072431174,
-            "heading": 0.1574496496087124,
-            "angularVelocity": 0.6805405670936119,
-            "velocityX": -2.754151523621958,
-            "velocityY": 1.6463983058180132,
-            "timestamp": 0.3855061621570083
-        },
-        {
-            "x": 6.792607460054783,
-            "y": 6.3274746833219355,
-            "heading": 0.15744965721454035,
-            "angularVelocity": 9.864729365588338e-8,
-            "velocityX": -3.246990933303352,
-            "velocityY": 1.941053419916652,
-            "timestamp": 0.46260739458840994
-        },
-        {
-            "x": 6.542259846259959,
-            "y": 6.4771312718889495,
-            "heading": 0.15744966481969572,
-            "angularVelocity": 9.863857053631023e-8,
-            "velocityX": -3.246998859811527,
-            "velocityY": 1.9410401604172332,
-            "timestamp": 0.5397086270198116
-        },
-        {
-            "x": 6.273256747030451,
-            "y": 6.589862608843688,
-            "heading": 0.1574496727775284,
-            "angularVelocity": 1.0321278187268036e-7,
-            "velocityX": -3.4889597837342676,
-            "velocityY": 1.4621210764047974,
-            "timestamp": 0.6168098594512132
-        },
-        {
-            "x": 5.98904964850956,
-            "y": 6.655416753006686,
-            "heading": 0.15744969119167346,
-            "angularVelocity": 2.3883074855376557e-7,
-            "velocityX": -3.686155065987494,
-            "velocityY": 0.8502347121535809,
-            "timestamp": 0.6939110918826149
-        },
-        {
-            "x": 5.6978465137603145,
-            "y": 6.6719003531074685,
-            "heading": 0.15744999446473232,
-            "angularVelocity": 0.00000393343983387992,
-            "velocityX": -3.7768933850484796,
-            "velocityY": 0.21379165521703997,
-            "timestamp": 0.7710123243140166
-        },
-        {
-            "x": 5.40805721282959,
-            "y": 6.638837814331055,
-            "heading": 0.15745035498223803,
-            "angularVelocity": 0.00000467589809324531,
-            "velocityX": -3.7585560151526085,
-            "velocityY": -0.428819848059242,
-            "timestamp": 0.8481135567454182
-        },
-        {
-            "x": 5.104307335648147,
-            "y": 6.562247743535093,
-            "heading": 0.18966206475475966,
-            "angularVelocity": 0.37702586381684944,
-            "velocityX": -3.555277277652879,
-            "velocityY": -0.896457772827513,
-            "timestamp": 0.9335498964342235
-        },
-        {
-            "x": 4.836190414730584,
-            "y": 6.438293970082571,
-            "heading": 0.27176288986041636,
-            "angularVelocity": 0.9609590650149814,
-            "velocityX": -3.1382070193334055,
-            "velocityY": -1.4508319750590115,
-            "timestamp": 1.0189862361230289
-        },
-        {
-            "x": 4.616497381460216,
-            "y": 6.278221072521664,
-            "heading": 0.3274582685749672,
-            "angularVelocity": 0.6518933151562509,
-            "velocityX": -2.5714237532949347,
-            "velocityY": -1.8735926438791781,
-            "timestamp": 1.1044225758118342
-        },
-        {
-            "x": 4.449006665280017,
-            "y": 6.09487376587715,
-            "heading": 0.3096216512620487,
-            "angularVelocity": -0.2087708506460702,
-            "velocityX": -1.960415401575849,
-            "velocityY": -2.1460107878256482,
-            "timestamp": 1.1898589155006396
-        },
-        {
-            "x": 4.339814814870372,
-            "y": 5.903698766708507,
-            "heading": 0.23521329843339078,
-            "angularVelocity": -0.8709215902704173,
-            "velocityX": -1.2780492564097095,
-            "velocityY": -2.237630964352897,
-            "timestamp": 1.275295255189445
-        },
-        {
-            "x": 4.286146359443328,
-            "y": 5.736442488850756,
-            "heading": 0.1356278137676922,
-            "angularVelocity": -1.1656103834554516,
-            "velocityX": -0.628168945703036,
-            "velocityY": -1.957671389796991,
-            "timestamp": 1.3607315948782504
-        },
-        {
-            "x": 4.266951931468271,
-            "y": 5.619203136466468,
-            "heading": 0.049466079264437586,
-            "angularVelocity": -1.0084904715849423,
-            "velocityX": -0.2246635102225946,
-            "velocityY": -1.372242219309977,
-            "timestamp": 1.4461679345670557
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 4.182976778726496e-34,
-            "angularVelocity": -0.578981724224301,
-            "velocityX": -0.046255861178807596,
-            "velocityY": -0.6929502912005617,
-            "timestamp": 1.531604274255861
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -6.30298269203181e-33,
-            "timestamp": 1.6170406139446665
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.1.traj
deleted file mode 100644
index 90749c58..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.1.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 7.446350164213638e-30,
-            "velocityX": -3.476389258222187e-32,
-            "velocityY": 2.4606016306403766e-37,
-            "timestamp": 0
-        },
-        {
-            "x": 7.6436015698643995,
-            "y": 5.74806653588022,
-            "heading": -0.0038807165751560974,
-            "angularVelocity": -0.045372809336175934,
-            "velocityX": -0.4255654848002544,
-            "velocityY": -0.572123393880186,
-            "timestamp": 0.08552956345292874
-        },
-        {
-            "x": 7.570804707716712,
-            "y": 5.6501996114987145,
-            "heading": -0.011642164214821673,
-            "angularVelocity": -0.09074578807989701,
-            "velocityX": -0.8511309915401587,
-            "velocityY": -1.1442467426525054,
-            "timestamp": 0.17105912690585748
-        },
-        {
-            "x": 7.461609412342726,
-            "y": 5.503399231866818,
-            "heading": -0.02328436718357624,
-            "angularVelocity": -0.136119050521775,
-            "velocityX": -1.2766965124764296,
-            "velocityY": -1.7163700328330225,
-            "timestamp": 0.25658869035878623
-        },
-        {
-            "x": 7.3160156842382476,
-            "y": 5.307665404737857,
-            "heading": -0.038807352623976644,
-            "angularVelocity": -0.18149263031072865,
-            "velocityX": -1.7022620276158245,
-            "velocityY": -2.288493232362672,
-            "timestamp": 0.34211825381171496
-        },
-        {
-            "x": 7.134023531165023,
-            "y": 5.062998148299065,
-            "heading": -0.05821111114579378,
-            "angularVelocity": -0.22686610031040333,
-            "velocityX": -2.127827452005934,
-            "velocityY": -2.8606162192496707,
-            "timestamp": 0.4276478172646437
-        },
-        {
-            "x": 6.9409164317403365,
-            "y": 4.803390071813602,
-            "heading": -0.058211113467083825,
-            "angularVelocity": -2.714020682501112e-8,
-            "velocityX": -2.2577818900123794,
-            "velocityY": -3.0353022511138943,
-            "timestamp": 0.5131773807175725
-        },
-        {
-            "x": 6.70968017178257,
-            "y": 4.577079452070077,
-            "heading": -0.05821111591888997,
-            "angularVelocity": -2.866618337367041e-8,
-            "velocityX": -2.7035828387575864,
-            "velocityY": -2.645992924634468,
-            "timestamp": 0.5987069441705012
-        },
-        {
-            "x": 6.4400791107327935,
-            "y": 4.398188508585333,
-            "heading": -0.05821111892081074,
-            "angularVelocity": -3.509804859460385e-8,
-            "velocityX": -3.152138864804934,
-            "velocityY": -2.091568532127445,
-            "timestamp": 0.6842365076234299
-        },
-        {
-            "x": 6.141694589328426,
-            "y": 4.273075039260189,
-            "heading": -0.058211144902897714,
-            "angularVelocity": -3.0377902004237926e-7,
-            "velocityX": -3.488671160686832,
-            "velocityY": -1.462809632999017,
-            "timestamp": 0.7697660710763586
-        },
-        {
-            "x": 5.825131041705898,
-            "y": 4.206185466847777,
-            "heading": -0.05821124348692034,
-            "angularVelocity": -0.000001152630957627144,
-            "velocityX": -3.7012178578082944,
-            "velocityY": -0.7820637649954203,
-            "timestamp": 0.8552956345292874
-        },
-        {
-            "x": 5.503318785454965,
-            "y": 4.1726658015863975,
-            "heading": -0.058211338856237596,
-            "angularVelocity": -0.0000011150450605169934,
-            "velocityX": -3.76258504380235,
-            "velocityY": -0.39190735820634004,
-            "timestamp": 0.9408251979822161
-        },
-        {
-            "x": 5.1935160355708865,
-            "y": 4.079344267897855,
-            "heading": -0.0582116136415855,
-            "angularVelocity": -0.0000032127528402629677,
-            "velocityX": -3.622171532005757,
-            "velocityY": -1.091102654112135,
-            "timestamp": 1.026354761435145
-        },
-        {
-            "x": 4.918641828284195,
-            "y": 3.9369980738928456,
-            "heading": -0.09326111746178506,
-            "angularVelocity": -0.4097940221510551,
-            "velocityX": -3.2137917719873403,
-            "velocityY": -1.664292301495839,
-            "timestamp": 1.1118843248880736
-        },
-        {
-            "x": 4.689025118769517,
-            "y": 3.755353610578967,
-            "heading": -0.16867561276178636,
-            "angularVelocity": -0.8817360016283226,
-            "velocityX": -2.6846472756878708,
-            "velocityY": -2.1237623107224977,
-            "timestamp": 1.1974138883410024
-        },
-        {
-            "x": 4.510985991586203,
-            "y": 3.542062267326439,
-            "heading": -0.2715725395002121,
-            "angularVelocity": -1.2030568447254515,
-            "velocityX": -2.0816092120158958,
-            "velocityY": -2.493773318156982,
-            "timestamp": 1.282943451793931
-        },
-        {
-            "x": 4.39344009542189,
-            "y": 3.3364721307805043,
-            "heading": -0.3792171957634167,
-            "angularVelocity": -1.2585666513128753,
-            "velocityX": -1.3743305989045989,
-            "velocityY": -2.4037318588569803,
-            "timestamp": 1.3684730152468598
-        },
-        {
-            "x": 4.321460732883776,
-            "y": 3.1711461674458645,
-            "heading": -0.4697306425856333,
-            "angularVelocity": -1.0582708851545903,
-            "velocityX": -0.8415728975131295,
-            "velocityY": -1.932968632835679,
-            "timestamp": 1.4540025786997886
-        },
-        {
-            "x": 4.280782976343733,
-            "y": 3.057632956228743,
-            "heading": -0.5333100471714295,
-            "angularVelocity": -0.7433617338733072,
-            "velocityX": -0.475598785938263,
-            "velocityY": -1.3271809960727023,
-            "timestamp": 1.5395321421527173
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -0.38220647351440606,
-            "velocityX": -0.20791613596297567,
-            "velocityY": -0.6738366700592523,
-            "timestamp": 1.625061705605646
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -1.9453350650223036e-32,
-            "velocityX": 0,
-            "velocityY": 3.675541041669649e-33,
-            "timestamp": 1.7105912690585747
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.traj
deleted file mode 100644
index 90749c58..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 7.446350164213638e-30,
-            "velocityX": -3.476389258222187e-32,
-            "velocityY": 2.4606016306403766e-37,
-            "timestamp": 0
-        },
-        {
-            "x": 7.6436015698643995,
-            "y": 5.74806653588022,
-            "heading": -0.0038807165751560974,
-            "angularVelocity": -0.045372809336175934,
-            "velocityX": -0.4255654848002544,
-            "velocityY": -0.572123393880186,
-            "timestamp": 0.08552956345292874
-        },
-        {
-            "x": 7.570804707716712,
-            "y": 5.6501996114987145,
-            "heading": -0.011642164214821673,
-            "angularVelocity": -0.09074578807989701,
-            "velocityX": -0.8511309915401587,
-            "velocityY": -1.1442467426525054,
-            "timestamp": 0.17105912690585748
-        },
-        {
-            "x": 7.461609412342726,
-            "y": 5.503399231866818,
-            "heading": -0.02328436718357624,
-            "angularVelocity": -0.136119050521775,
-            "velocityX": -1.2766965124764296,
-            "velocityY": -1.7163700328330225,
-            "timestamp": 0.25658869035878623
-        },
-        {
-            "x": 7.3160156842382476,
-            "y": 5.307665404737857,
-            "heading": -0.038807352623976644,
-            "angularVelocity": -0.18149263031072865,
-            "velocityX": -1.7022620276158245,
-            "velocityY": -2.288493232362672,
-            "timestamp": 0.34211825381171496
-        },
-        {
-            "x": 7.134023531165023,
-            "y": 5.062998148299065,
-            "heading": -0.05821111114579378,
-            "angularVelocity": -0.22686610031040333,
-            "velocityX": -2.127827452005934,
-            "velocityY": -2.8606162192496707,
-            "timestamp": 0.4276478172646437
-        },
-        {
-            "x": 6.9409164317403365,
-            "y": 4.803390071813602,
-            "heading": -0.058211113467083825,
-            "angularVelocity": -2.714020682501112e-8,
-            "velocityX": -2.2577818900123794,
-            "velocityY": -3.0353022511138943,
-            "timestamp": 0.5131773807175725
-        },
-        {
-            "x": 6.70968017178257,
-            "y": 4.577079452070077,
-            "heading": -0.05821111591888997,
-            "angularVelocity": -2.866618337367041e-8,
-            "velocityX": -2.7035828387575864,
-            "velocityY": -2.645992924634468,
-            "timestamp": 0.5987069441705012
-        },
-        {
-            "x": 6.4400791107327935,
-            "y": 4.398188508585333,
-            "heading": -0.05821111892081074,
-            "angularVelocity": -3.509804859460385e-8,
-            "velocityX": -3.152138864804934,
-            "velocityY": -2.091568532127445,
-            "timestamp": 0.6842365076234299
-        },
-        {
-            "x": 6.141694589328426,
-            "y": 4.273075039260189,
-            "heading": -0.058211144902897714,
-            "angularVelocity": -3.0377902004237926e-7,
-            "velocityX": -3.488671160686832,
-            "velocityY": -1.462809632999017,
-            "timestamp": 0.7697660710763586
-        },
-        {
-            "x": 5.825131041705898,
-            "y": 4.206185466847777,
-            "heading": -0.05821124348692034,
-            "angularVelocity": -0.000001152630957627144,
-            "velocityX": -3.7012178578082944,
-            "velocityY": -0.7820637649954203,
-            "timestamp": 0.8552956345292874
-        },
-        {
-            "x": 5.503318785454965,
-            "y": 4.1726658015863975,
-            "heading": -0.058211338856237596,
-            "angularVelocity": -0.0000011150450605169934,
-            "velocityX": -3.76258504380235,
-            "velocityY": -0.39190735820634004,
-            "timestamp": 0.9408251979822161
-        },
-        {
-            "x": 5.1935160355708865,
-            "y": 4.079344267897855,
-            "heading": -0.0582116136415855,
-            "angularVelocity": -0.0000032127528402629677,
-            "velocityX": -3.622171532005757,
-            "velocityY": -1.091102654112135,
-            "timestamp": 1.026354761435145
-        },
-        {
-            "x": 4.918641828284195,
-            "y": 3.9369980738928456,
-            "heading": -0.09326111746178506,
-            "angularVelocity": -0.4097940221510551,
-            "velocityX": -3.2137917719873403,
-            "velocityY": -1.664292301495839,
-            "timestamp": 1.1118843248880736
-        },
-        {
-            "x": 4.689025118769517,
-            "y": 3.755353610578967,
-            "heading": -0.16867561276178636,
-            "angularVelocity": -0.8817360016283226,
-            "velocityX": -2.6846472756878708,
-            "velocityY": -2.1237623107224977,
-            "timestamp": 1.1974138883410024
-        },
-        {
-            "x": 4.510985991586203,
-            "y": 3.542062267326439,
-            "heading": -0.2715725395002121,
-            "angularVelocity": -1.2030568447254515,
-            "velocityX": -2.0816092120158958,
-            "velocityY": -2.493773318156982,
-            "timestamp": 1.282943451793931
-        },
-        {
-            "x": 4.39344009542189,
-            "y": 3.3364721307805043,
-            "heading": -0.3792171957634167,
-            "angularVelocity": -1.2585666513128753,
-            "velocityX": -1.3743305989045989,
-            "velocityY": -2.4037318588569803,
-            "timestamp": 1.3684730152468598
-        },
-        {
-            "x": 4.321460732883776,
-            "y": 3.1711461674458645,
-            "heading": -0.4697306425856333,
-            "angularVelocity": -1.0582708851545903,
-            "velocityX": -0.8415728975131295,
-            "velocityY": -1.932968632835679,
-            "timestamp": 1.4540025786997886
-        },
-        {
-            "x": 4.280782976343733,
-            "y": 3.057632956228743,
-            "heading": -0.5333100471714295,
-            "angularVelocity": -0.7433617338733072,
-            "velocityX": -0.475598785938263,
-            "velocityY": -1.3271809960727023,
-            "timestamp": 1.5395321421527173
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -0.38220647351440606,
-            "velocityX": -0.20791613596297567,
-            "velocityY": -0.6738366700592523,
-            "timestamp": 1.625061705605646
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -1.9453350650223036e-32,
-            "velocityX": 0,
-            "velocityY": 3.675541041669649e-33,
-            "timestamp": 1.7105912690585747
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S4.1.traj
deleted file mode 100644
index 4efefc66..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S4.1.traj	
+++ /dev/null
@@ -1,256 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": -1.007826409753455e-39,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.661226431831969,
-            "y": 5.757681823957774,
-            "heading": -0.0001846118691686639,
-            "angularVelocity": -0.0025538708536768893,
-            "velocityX": -0.25970848342391806,
-            "velocityY": -0.5439170529292338,
-            "timestamp": 0.07228708096295171
-        },
-        {
-            "x": 7.623679292701499,
-            "y": 5.679045474684997,
-            "heading": -0.0005538375109882563,
-            "angularVelocity": -0.005107768039614528,
-            "velocityX": -0.5194170055049473,
-            "velocityY": -1.0878340669625215,
-            "timestamp": 0.14457416192590342
-        },
-        {
-            "x": 7.567358579979895,
-            "y": 5.561090955652203,
-            "heading": -0.0011076778331591435,
-            "angularVelocity": -0.0076616777824344475,
-            "velocityX": -0.779125563950612,
-            "velocityY": -1.6317510329853882,
-            "timestamp": 0.2168612428888551
-        },
-        {
-            "x": 7.492264291454711,
-            "y": 5.403818271607319,
-            "heading": -0.001846132228095339,
-            "angularVelocity": -0.010215579120073546,
-            "velocityX": -1.0388341530026832,
-            "velocityY": -2.175667933326667,
-            "timestamp": 0.28914832385180683
-        },
-        {
-            "x": 7.39839642615194,
-            "y": 5.207227430397753,
-            "heading": -0.0027691974892554232,
-            "angularVelocity": -0.012769436099282666,
-            "velocityX": -1.298542755528879,
-            "velocityY": -2.719584725109078,
-            "timestamp": 0.36143540481475855
-        },
-        {
-            "x": 7.285754988600711,
-            "y": 4.97131845200793,
-            "heading": -0.0038768638943260385,
-            "angularVelocity": -0.015323158582628504,
-            "velocityX": -1.5582512954003445,
-            "velocityY": -3.263501240432299,
-            "timestamp": 0.4337224857777103
-        },
-        {
-            "x": 7.167927209247298,
-            "y": 4.7245478329210355,
-            "heading": -0.0038768640537450113,
-            "angularVelocity": -2.205359112715029e-9,
-            "velocityX": -1.629997750411335,
-            "velocityY": -3.4137582511232942,
-            "timestamp": 0.506009566740662
-        },
-        {
-            "x": 7.050099366875978,
-            "y": 4.477777243923851,
-            "heading": -0.003876864213154645,
-            "angularVelocity": -2.2052299170164517e-9,
-            "velocityX": -1.6299986221840805,
-            "velocityY": -3.413757834870371,
-            "timestamp": 0.5782966477036137
-        },
-        {
-            "x": 6.932271561638543,
-            "y": 4.2310066371960025,
-            "heading": -0.0038768643725604667,
-            "angularVelocity": -2.2051771914474314e-9,
-            "velocityX": -1.629998108483918,
-            "velocityY": -3.413758080151585,
-            "timestamp": 0.6505837286665653
-        },
-        {
-            "x": 6.8252544845676155,
-            "y": 3.9793591274718993,
-            "heading": -0.0038768645333797187,
-            "angularVelocity": -2.2247301918048757e-9,
-            "velocityX": -1.4804454080221456,
-            "velocityY": -3.4812238421008592,
-            "timestamp": 0.722870809629517
-        },
-        {
-            "x": 6.7595626994044276,
-            "y": 3.7139091088268694,
-            "heading": -0.003876864707631206,
-            "angularVelocity": -2.410548124556829e-9,
-            "velocityX": -0.9087624550347577,
-            "velocityY": -3.6721640313720423,
-            "timestamp": 0.7951578905924687
-        },
-        {
-            "x": 6.7368645668029785,
-            "y": 3.4413950443267822,
-            "heading": -0.0038768649214938546,
-            "angularVelocity": -2.9585182521427424e-9,
-            "velocityX": -0.31399985030634436,
-            "velocityY": -3.7698861382956523,
-            "timestamp": 0.8674449715554203
-        },
-        {
-            "x": 6.763602448674763,
-            "y": 3.1456190677825386,
-            "heading": -0.0038769163309766456,
-            "angularVelocity": -6.548510424231184e-7,
-            "velocityX": 0.340585605327871,
-            "velocityY": -3.7675774205236117,
-            "timestamp": 0.9459505849856912
-        },
-        {
-            "x": 6.763837716877819,
-            "y": 2.855181760345028,
-            "heading": -0.02573615579182971,
-            "angularVelocity": -0.27844173818562123,
-            "velocityX": 0.0029968328731749647,
-            "velocityY": -3.699573759722024,
-            "timestamp": 1.0244561984159621
-        },
-        {
-            "x": 6.746691140594586,
-            "y": 2.576419895706607,
-            "heading": -0.08408576500639096,
-            "angularVelocity": -0.7432539746522423,
-            "velocityX": -0.2184121049950426,
-            "velocityY": -3.5508526391685153,
-            "timestamp": 1.102961811846233
-        },
-        {
-            "x": 6.69086397201793,
-            "y": 2.322743038831463,
-            "heading": -0.2046015958190934,
-            "angularVelocity": -1.5351237388870964,
-            "velocityX": -0.7111232705192682,
-            "velocityY": -3.2313212494092722,
-            "timestamp": 1.1814674252765038
-        },
-        {
-            "x": 6.586917702656428,
-            "y": 2.0871040226876985,
-            "heading": -0.32491541634118276,
-            "angularVelocity": -1.5325505433946174,
-            "velocityX": -1.324061615719081,
-            "velocityY": -3.001556269005664,
-            "timestamp": 1.2599730387067747
-        },
-        {
-            "x": 6.439847045017927,
-            "y": 1.879401635438828,
-            "heading": -0.448122685606705,
-            "angularVelocity": -1.5694071274910255,
-            "velocityX": -1.8733775995410846,
-            "velocityY": -2.6457010928696563,
-            "timestamp": 1.3384786521370455
-        },
-        {
-            "x": 6.25456019704424,
-            "y": 1.7060438180074393,
-            "heading": -0.5681346722700569,
-            "angularVelocity": -1.528705801018258,
-            "velocityX": -2.3601732395640638,
-            "velocityY": -2.2082219328859343,
-            "timestamp": 1.4169842655673164
-        },
-        {
-            "x": 6.049779045368227,
-            "y": 1.57972317182915,
-            "heading": -0.6633309119306381,
-            "angularVelocity": -1.2126042393788183,
-            "velocityX": -2.6084905617341763,
-            "velocityY": -1.6090651439911137,
-            "timestamp": 1.4954898789975872
-        },
-        {
-            "x": 5.840944041976222,
-            "y": 1.503518626439596,
-            "heading": -0.7208467997653518,
-            "angularVelocity": -0.7326340795464229,
-            "velocityX": -2.660128292322611,
-            "velocityY": -0.9706891273098467,
-            "timestamp": 1.573995492427858
-        },
-        {
-            "x": 5.636524773547887,
-            "y": 1.4770268115818306,
-            "heading": -0.7348631463149735,
-            "angularVelocity": -0.17853941822989652,
-            "velocityX": -2.603880913686534,
-            "velocityY": -0.33745121782018417,
-            "timestamp": 1.652501105858129
-        },
-        {
-            "x": 5.451777868175388,
-            "y": 1.4973120784666072,
-            "heading": -0.7197568228935092,
-            "angularVelocity": 0.19242348109134802,
-            "velocityX": -2.353295481686686,
-            "velocityY": 0.25839256581051945,
-            "timestamp": 1.7310067192883998
-        },
-        {
-            "x": 5.309771976878378,
-            "y": 1.5457401851903851,
-            "heading": -0.687868735789328,
-            "angularVelocity": 0.4061886241103559,
-            "velocityX": -1.808862896449324,
-            "velocityY": 0.6168744451222204,
-            "timestamp": 1.8095123327186706
-        },
-        {
-            "x": 5.2191255322417085,
-            "y": 1.592492981200916,
-            "heading": -0.6576952711638115,
-            "angularVelocity": 0.38434786134518467,
-            "velocityX": -1.154649211386418,
-            "velocityY": 0.5955344333696231,
-            "timestamp": 1.8880179461489415
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0.22540134890517805,
-            "velocityX": -0.5493305555788384,
-            "velocityY": 0.35038282738234927,
-            "timestamp": 1.9665235595792123
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0,
-            "velocityX": 6.617232561290069e-35,
-            "velocityY": 1.0472745338276337e-32,
-            "timestamp": 2.045029173009483
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S4.traj
deleted file mode 100644
index 4efefc66..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S4.traj	
+++ /dev/null
@@ -1,256 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": -1.007826409753455e-39,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.661226431831969,
-            "y": 5.757681823957774,
-            "heading": -0.0001846118691686639,
-            "angularVelocity": -0.0025538708536768893,
-            "velocityX": -0.25970848342391806,
-            "velocityY": -0.5439170529292338,
-            "timestamp": 0.07228708096295171
-        },
-        {
-            "x": 7.623679292701499,
-            "y": 5.679045474684997,
-            "heading": -0.0005538375109882563,
-            "angularVelocity": -0.005107768039614528,
-            "velocityX": -0.5194170055049473,
-            "velocityY": -1.0878340669625215,
-            "timestamp": 0.14457416192590342
-        },
-        {
-            "x": 7.567358579979895,
-            "y": 5.561090955652203,
-            "heading": -0.0011076778331591435,
-            "angularVelocity": -0.0076616777824344475,
-            "velocityX": -0.779125563950612,
-            "velocityY": -1.6317510329853882,
-            "timestamp": 0.2168612428888551
-        },
-        {
-            "x": 7.492264291454711,
-            "y": 5.403818271607319,
-            "heading": -0.001846132228095339,
-            "angularVelocity": -0.010215579120073546,
-            "velocityX": -1.0388341530026832,
-            "velocityY": -2.175667933326667,
-            "timestamp": 0.28914832385180683
-        },
-        {
-            "x": 7.39839642615194,
-            "y": 5.207227430397753,
-            "heading": -0.0027691974892554232,
-            "angularVelocity": -0.012769436099282666,
-            "velocityX": -1.298542755528879,
-            "velocityY": -2.719584725109078,
-            "timestamp": 0.36143540481475855
-        },
-        {
-            "x": 7.285754988600711,
-            "y": 4.97131845200793,
-            "heading": -0.0038768638943260385,
-            "angularVelocity": -0.015323158582628504,
-            "velocityX": -1.5582512954003445,
-            "velocityY": -3.263501240432299,
-            "timestamp": 0.4337224857777103
-        },
-        {
-            "x": 7.167927209247298,
-            "y": 4.7245478329210355,
-            "heading": -0.0038768640537450113,
-            "angularVelocity": -2.205359112715029e-9,
-            "velocityX": -1.629997750411335,
-            "velocityY": -3.4137582511232942,
-            "timestamp": 0.506009566740662
-        },
-        {
-            "x": 7.050099366875978,
-            "y": 4.477777243923851,
-            "heading": -0.003876864213154645,
-            "angularVelocity": -2.2052299170164517e-9,
-            "velocityX": -1.6299986221840805,
-            "velocityY": -3.413757834870371,
-            "timestamp": 0.5782966477036137
-        },
-        {
-            "x": 6.932271561638543,
-            "y": 4.2310066371960025,
-            "heading": -0.0038768643725604667,
-            "angularVelocity": -2.2051771914474314e-9,
-            "velocityX": -1.629998108483918,
-            "velocityY": -3.413758080151585,
-            "timestamp": 0.6505837286665653
-        },
-        {
-            "x": 6.8252544845676155,
-            "y": 3.9793591274718993,
-            "heading": -0.0038768645333797187,
-            "angularVelocity": -2.2247301918048757e-9,
-            "velocityX": -1.4804454080221456,
-            "velocityY": -3.4812238421008592,
-            "timestamp": 0.722870809629517
-        },
-        {
-            "x": 6.7595626994044276,
-            "y": 3.7139091088268694,
-            "heading": -0.003876864707631206,
-            "angularVelocity": -2.410548124556829e-9,
-            "velocityX": -0.9087624550347577,
-            "velocityY": -3.6721640313720423,
-            "timestamp": 0.7951578905924687
-        },
-        {
-            "x": 6.7368645668029785,
-            "y": 3.4413950443267822,
-            "heading": -0.0038768649214938546,
-            "angularVelocity": -2.9585182521427424e-9,
-            "velocityX": -0.31399985030634436,
-            "velocityY": -3.7698861382956523,
-            "timestamp": 0.8674449715554203
-        },
-        {
-            "x": 6.763602448674763,
-            "y": 3.1456190677825386,
-            "heading": -0.0038769163309766456,
-            "angularVelocity": -6.548510424231184e-7,
-            "velocityX": 0.340585605327871,
-            "velocityY": -3.7675774205236117,
-            "timestamp": 0.9459505849856912
-        },
-        {
-            "x": 6.763837716877819,
-            "y": 2.855181760345028,
-            "heading": -0.02573615579182971,
-            "angularVelocity": -0.27844173818562123,
-            "velocityX": 0.0029968328731749647,
-            "velocityY": -3.699573759722024,
-            "timestamp": 1.0244561984159621
-        },
-        {
-            "x": 6.746691140594586,
-            "y": 2.576419895706607,
-            "heading": -0.08408576500639096,
-            "angularVelocity": -0.7432539746522423,
-            "velocityX": -0.2184121049950426,
-            "velocityY": -3.5508526391685153,
-            "timestamp": 1.102961811846233
-        },
-        {
-            "x": 6.69086397201793,
-            "y": 2.322743038831463,
-            "heading": -0.2046015958190934,
-            "angularVelocity": -1.5351237388870964,
-            "velocityX": -0.7111232705192682,
-            "velocityY": -3.2313212494092722,
-            "timestamp": 1.1814674252765038
-        },
-        {
-            "x": 6.586917702656428,
-            "y": 2.0871040226876985,
-            "heading": -0.32491541634118276,
-            "angularVelocity": -1.5325505433946174,
-            "velocityX": -1.324061615719081,
-            "velocityY": -3.001556269005664,
-            "timestamp": 1.2599730387067747
-        },
-        {
-            "x": 6.439847045017927,
-            "y": 1.879401635438828,
-            "heading": -0.448122685606705,
-            "angularVelocity": -1.5694071274910255,
-            "velocityX": -1.8733775995410846,
-            "velocityY": -2.6457010928696563,
-            "timestamp": 1.3384786521370455
-        },
-        {
-            "x": 6.25456019704424,
-            "y": 1.7060438180074393,
-            "heading": -0.5681346722700569,
-            "angularVelocity": -1.528705801018258,
-            "velocityX": -2.3601732395640638,
-            "velocityY": -2.2082219328859343,
-            "timestamp": 1.4169842655673164
-        },
-        {
-            "x": 6.049779045368227,
-            "y": 1.57972317182915,
-            "heading": -0.6633309119306381,
-            "angularVelocity": -1.2126042393788183,
-            "velocityX": -2.6084905617341763,
-            "velocityY": -1.6090651439911137,
-            "timestamp": 1.4954898789975872
-        },
-        {
-            "x": 5.840944041976222,
-            "y": 1.503518626439596,
-            "heading": -0.7208467997653518,
-            "angularVelocity": -0.7326340795464229,
-            "velocityX": -2.660128292322611,
-            "velocityY": -0.9706891273098467,
-            "timestamp": 1.573995492427858
-        },
-        {
-            "x": 5.636524773547887,
-            "y": 1.4770268115818306,
-            "heading": -0.7348631463149735,
-            "angularVelocity": -0.17853941822989652,
-            "velocityX": -2.603880913686534,
-            "velocityY": -0.33745121782018417,
-            "timestamp": 1.652501105858129
-        },
-        {
-            "x": 5.451777868175388,
-            "y": 1.4973120784666072,
-            "heading": -0.7197568228935092,
-            "angularVelocity": 0.19242348109134802,
-            "velocityX": -2.353295481686686,
-            "velocityY": 0.25839256581051945,
-            "timestamp": 1.7310067192883998
-        },
-        {
-            "x": 5.309771976878378,
-            "y": 1.5457401851903851,
-            "heading": -0.687868735789328,
-            "angularVelocity": 0.4061886241103559,
-            "velocityX": -1.808862896449324,
-            "velocityY": 0.6168744451222204,
-            "timestamp": 1.8095123327186706
-        },
-        {
-            "x": 5.2191255322417085,
-            "y": 1.592492981200916,
-            "heading": -0.6576952711638115,
-            "angularVelocity": 0.38434786134518467,
-            "velocityX": -1.154649211386418,
-            "velocityY": 0.5955344333696231,
-            "timestamp": 1.8880179461489415
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0.22540134890517805,
-            "velocityX": -0.5493305555788384,
-            "velocityY": 0.35038282738234927,
-            "timestamp": 1.9665235595792123
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0,
-            "velocityX": 6.617232561290069e-35,
-            "velocityY": 1.0472745338276337e-32,
-            "timestamp": 2.045029173009483
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.1.traj
deleted file mode 100644
index 0428af29..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 2.477402495362019e-32,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.61783610738437,
-            "y": 5.830159445563511,
-            "heading": -0.009839426193567341,
-            "angularVelocity": -0.10699963479066096,
-            "velocityX": -0.6760062707098423,
-            "velocityY": 0.3605950687933442,
-            "timestamp": 0.09195756801242966
-        },
-        {
-            "x": 7.493508328314676,
-            "y": 5.8964783245931205,
-            "heading": -0.029519357600464063,
-            "angularVelocity": -0.21401100346886773,
-            "velocityX": -1.3520124744152573,
-            "velocityY": 0.7211900060324206,
-            "timestamp": 0.18391513602485932
-        },
-        {
-            "x": 7.307016677169599,
-            "y": 5.995956620127639,
-            "heading": -0.05904164496194892,
-            "angularVelocity": -0.32104249818236175,
-            "velocityX": -2.028018521758536,
-            "velocityY": 1.0817847588256284,
-            "timestamp": 0.275872704037289
-        },
-        {
-            "x": 7.058361185168299,
-            "y": 6.1285943089313815,
-            "heading": -0.09840880514573262,
-            "angularVelocity": -0.42810136277704247,
-            "velocityX": -2.7040242296065236,
-            "velocityY": 1.4423792589405482,
-            "timestamp": 0.36783027204971863
-        },
-        {
-            "x": 6.751428461707863,
-            "y": 6.292319225940392,
-            "heading": -0.09840881013370367,
-            "angularVelocity": -5.424209394751822e-8,
-            "velocityX": -3.3377646896767494,
-            "velocityY": 1.7804398327158815,
-            "timestamp": 0.4597878400621483
-        },
-        {
-            "x": 6.443018732830252,
-            "y": 6.45324462570272,
-            "heading": -0.09840881513118924,
-            "angularVelocity": -5.434556026601013e-8,
-            "velocityX": -3.3538265043712756,
-            "velocityY": 1.7499962563230937,
-            "timestamp": 0.551745408074578
-        },
-        {
-            "x": 6.108495593860041,
-            "y": 6.548679499028843,
-            "heading": -0.09840879814777771,
-            "angularVelocity": 1.8468748020115896e-7,
-            "velocityX": -3.637798891386455,
-            "velocityY": 1.0378142374668238,
-            "timestamp": 0.6437029760870077
-        },
-        {
-            "x": 5.7623127419169835,
-            "y": 6.582899007638951,
-            "heading": -0.09840874396641834,
-            "angularVelocity": 5.891995683553032e-7,
-            "velocityX": -3.764593381767812,
-            "velocityY": 0.37212281000605224,
-            "timestamp": 0.7356605440994373
-        },
-        {
-            "x": 5.416121749902788,
-            "y": 6.617036066627776,
-            "heading": -0.09840868977891702,
-            "angularVelocity": 5.892663593006056e-7,
-            "velocityX": -3.7646819016288298,
-            "velocityY": 0.37122620494064046,
-            "timestamp": 0.8276181121118669
-        },
-        {
-            "x": 5.068594601971295,
-            "y": 6.632477066079001,
-            "heading": -0.0984086463109951,
-            "angularVelocity": 4.72695427552955e-7,
-            "velocityX": -3.7792120370616824,
-            "velocityY": 0.1679143955735957,
-            "timestamp": 0.9195756801242965
-        },
-        {
-            "x": 4.721067428734796,
-            "y": 6.647917495985935,
-            "heading": -0.09840860284326601,
-            "angularVelocity": 4.7269333042542933e-7,
-            "velocityX": -3.7792123122430223,
-            "velocityY": 0.16790820201819795,
-            "timestamp": 1.011533248136726
-        },
-        {
-            "x": 4.3735402523552525,
-            "y": 6.663357855150138,
-            "heading": -0.09840855937552709,
-            "angularVelocity": 4.726934374928119e-7,
-            "velocityX": -3.779212346422309,
-            "velocityY": 0.16790743272066663,
-            "timestamp": 1.1034908161491557
-        },
-        {
-            "x": 4.026013074283842,
-            "y": 6.678798176234082,
-            "heading": -0.09840851590777203,
-            "angularVelocity": 4.7269361285314424e-7,
-            "velocityX": -3.779212364820652,
-            "velocityY": 0.1679070186138197,
-            "timestamp": 1.1954483841615853
-        },
-        {
-            "x": 3.6784858950907164,
-            "y": 6.69423847207055,
-            "heading": -0.09840847244000124,
-            "angularVelocity": 4.7269378404370134e-7,
-            "velocityX": -3.7792123770188395,
-            "velocityY": 0.16790674405809006,
-            "timestamp": 1.2874059521740149
-        },
-        {
-            "x": 3.330958715199463,
-            "y": 6.709678752193432,
-            "heading": -0.09840842897219951,
-            "angularVelocity": 4.7269412044437995e-7,
-            "velocityX": -3.7792123846106827,
-            "velocityY": 0.16790657317944163,
-            "timestamp": 1.3793635201864445
-        },
-        {
-            "x": 2.991492162018093,
-            "y": 6.7253542779205,
-            "heading": -0.07153342912076269,
-            "angularVelocity": 0.2922543563549246,
-            "velocityX": -3.6915564484642,
-            "velocityY": 0.17046477050097905,
-            "timestamp": 1.471321088198874
-        },
-        {
-            "x": 2.713831676976805,
-            "y": 6.738399816428541,
-            "heading": 0.08699262812295999,
-            "angularVelocity": 1.7239044123295555,
-            "velocityX": -3.0194413689122035,
-            "velocityY": 0.14186476208546492,
-            "timestamp": 1.5632786562113037
-        },
-        {
-            "x": 2.5064819943185284,
-            "y": 6.7478572981535265,
-            "heading": 0.2316797715135602,
-            "angularVelocity": 1.5734120259796696,
-            "velocityX": -2.2548408699787488,
-            "velocityY": 0.10284614882059591,
-            "timestamp": 1.6552362242237333
-        },
-        {
-            "x": 2.368710598807467,
-            "y": 6.7539967145328434,
-            "heading": 0.3372042735506754,
-            "angularVelocity": 1.147534719739997,
-            "velocityX": -1.4982061671361233,
-            "velocityY": 0.06676357924654308,
-            "timestamp": 1.747193792236163
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.6067551334304849,
-            "velocityX": -0.7471989559160562,
-            "velocityY": 0.03265947036300673,
-            "timestamp": 1.8391513602485925
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0,
-            "velocityX": -4.676461204476467e-32,
-            "velocityY": 0,
-            "timestamp": 1.931108928261022
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.traj
deleted file mode 100644
index 0428af29..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 2.477402495362019e-32,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.61783610738437,
-            "y": 5.830159445563511,
-            "heading": -0.009839426193567341,
-            "angularVelocity": -0.10699963479066096,
-            "velocityX": -0.6760062707098423,
-            "velocityY": 0.3605950687933442,
-            "timestamp": 0.09195756801242966
-        },
-        {
-            "x": 7.493508328314676,
-            "y": 5.8964783245931205,
-            "heading": -0.029519357600464063,
-            "angularVelocity": -0.21401100346886773,
-            "velocityX": -1.3520124744152573,
-            "velocityY": 0.7211900060324206,
-            "timestamp": 0.18391513602485932
-        },
-        {
-            "x": 7.307016677169599,
-            "y": 5.995956620127639,
-            "heading": -0.05904164496194892,
-            "angularVelocity": -0.32104249818236175,
-            "velocityX": -2.028018521758536,
-            "velocityY": 1.0817847588256284,
-            "timestamp": 0.275872704037289
-        },
-        {
-            "x": 7.058361185168299,
-            "y": 6.1285943089313815,
-            "heading": -0.09840880514573262,
-            "angularVelocity": -0.42810136277704247,
-            "velocityX": -2.7040242296065236,
-            "velocityY": 1.4423792589405482,
-            "timestamp": 0.36783027204971863
-        },
-        {
-            "x": 6.751428461707863,
-            "y": 6.292319225940392,
-            "heading": -0.09840881013370367,
-            "angularVelocity": -5.424209394751822e-8,
-            "velocityX": -3.3377646896767494,
-            "velocityY": 1.7804398327158815,
-            "timestamp": 0.4597878400621483
-        },
-        {
-            "x": 6.443018732830252,
-            "y": 6.45324462570272,
-            "heading": -0.09840881513118924,
-            "angularVelocity": -5.434556026601013e-8,
-            "velocityX": -3.3538265043712756,
-            "velocityY": 1.7499962563230937,
-            "timestamp": 0.551745408074578
-        },
-        {
-            "x": 6.108495593860041,
-            "y": 6.548679499028843,
-            "heading": -0.09840879814777771,
-            "angularVelocity": 1.8468748020115896e-7,
-            "velocityX": -3.637798891386455,
-            "velocityY": 1.0378142374668238,
-            "timestamp": 0.6437029760870077
-        },
-        {
-            "x": 5.7623127419169835,
-            "y": 6.582899007638951,
-            "heading": -0.09840874396641834,
-            "angularVelocity": 5.891995683553032e-7,
-            "velocityX": -3.764593381767812,
-            "velocityY": 0.37212281000605224,
-            "timestamp": 0.7356605440994373
-        },
-        {
-            "x": 5.416121749902788,
-            "y": 6.617036066627776,
-            "heading": -0.09840868977891702,
-            "angularVelocity": 5.892663593006056e-7,
-            "velocityX": -3.7646819016288298,
-            "velocityY": 0.37122620494064046,
-            "timestamp": 0.8276181121118669
-        },
-        {
-            "x": 5.068594601971295,
-            "y": 6.632477066079001,
-            "heading": -0.0984086463109951,
-            "angularVelocity": 4.72695427552955e-7,
-            "velocityX": -3.7792120370616824,
-            "velocityY": 0.1679143955735957,
-            "timestamp": 0.9195756801242965
-        },
-        {
-            "x": 4.721067428734796,
-            "y": 6.647917495985935,
-            "heading": -0.09840860284326601,
-            "angularVelocity": 4.7269333042542933e-7,
-            "velocityX": -3.7792123122430223,
-            "velocityY": 0.16790820201819795,
-            "timestamp": 1.011533248136726
-        },
-        {
-            "x": 4.3735402523552525,
-            "y": 6.663357855150138,
-            "heading": -0.09840855937552709,
-            "angularVelocity": 4.726934374928119e-7,
-            "velocityX": -3.779212346422309,
-            "velocityY": 0.16790743272066663,
-            "timestamp": 1.1034908161491557
-        },
-        {
-            "x": 4.026013074283842,
-            "y": 6.678798176234082,
-            "heading": -0.09840851590777203,
-            "angularVelocity": 4.7269361285314424e-7,
-            "velocityX": -3.779212364820652,
-            "velocityY": 0.1679070186138197,
-            "timestamp": 1.1954483841615853
-        },
-        {
-            "x": 3.6784858950907164,
-            "y": 6.69423847207055,
-            "heading": -0.09840847244000124,
-            "angularVelocity": 4.7269378404370134e-7,
-            "velocityX": -3.7792123770188395,
-            "velocityY": 0.16790674405809006,
-            "timestamp": 1.2874059521740149
-        },
-        {
-            "x": 3.330958715199463,
-            "y": 6.709678752193432,
-            "heading": -0.09840842897219951,
-            "angularVelocity": 4.7269412044437995e-7,
-            "velocityX": -3.7792123846106827,
-            "velocityY": 0.16790657317944163,
-            "timestamp": 1.3793635201864445
-        },
-        {
-            "x": 2.991492162018093,
-            "y": 6.7253542779205,
-            "heading": -0.07153342912076269,
-            "angularVelocity": 0.2922543563549246,
-            "velocityX": -3.6915564484642,
-            "velocityY": 0.17046477050097905,
-            "timestamp": 1.471321088198874
-        },
-        {
-            "x": 2.713831676976805,
-            "y": 6.738399816428541,
-            "heading": 0.08699262812295999,
-            "angularVelocity": 1.7239044123295555,
-            "velocityX": -3.0194413689122035,
-            "velocityY": 0.14186476208546492,
-            "timestamp": 1.5632786562113037
-        },
-        {
-            "x": 2.5064819943185284,
-            "y": 6.7478572981535265,
-            "heading": 0.2316797715135602,
-            "angularVelocity": 1.5734120259796696,
-            "velocityX": -2.2548408699787488,
-            "velocityY": 0.10284614882059591,
-            "timestamp": 1.6552362242237333
-        },
-        {
-            "x": 2.368710598807467,
-            "y": 6.7539967145328434,
-            "heading": 0.3372042735506754,
-            "angularVelocity": 1.147534719739997,
-            "velocityX": -1.4982061671361233,
-            "velocityY": 0.06676357924654308,
-            "timestamp": 1.747193792236163
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.6067551334304849,
-            "velocityX": -0.7471989559160562,
-            "velocityY": 0.03265947036300673,
-            "timestamp": 1.8391513602485925
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0,
-            "velocityX": -4.676461204476467e-32,
-            "velocityY": 0,
-            "timestamp": 1.931108928261022
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.1.traj
deleted file mode 100644
index 5429265f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.1.traj	
+++ /dev/null
@@ -1,184 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -7.048442713490885e-38,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.598761685357721,
-            "y": 5.839328892781844,
-            "heading": -0.010877614975927053,
-            "angularVelocity": -0.10375029848266179,
-            "velocityX": -0.774848108801196,
-            "velocityY": 0.4037314494285639,
-            "timestamp": 0.10484418006512992
-        },
-        {
-            "x": 7.436285061134455,
-            "y": 5.923986667743732,
-            "heading": -0.03263383242489801,
-            "angularVelocity": -0.20751001567713226,
-            "velocityX": -1.5496961693280003,
-            "velocityY": 0.8074627977375359,
-            "timestamp": 0.20968836013025985
-        },
-        {
-            "x": 7.192570139480125,
-            "y": 6.050973310343331,
-            "heading": -0.06527034306657675,
-            "angularVelocity": -0.31128585889464805,
-            "velocityX": -2.3245441139692504,
-            "velocityY": 1.2111940073422762,
-            "timestamp": 0.3145325401953898
-        },
-        {
-            "x": 6.867616948374112,
-            "y": 6.220288799737083,
-            "heading": -0.10878948434468085,
-            "angularVelocity": -0.41508399656585776,
-            "velocityX": -3.0993917917441864,
-            "velocityY": 1.6149250181418864,
-            "timestamp": 0.4193767202605197
-        },
-        {
-            "x": 6.5158804421759,
-            "y": 6.403559919066418,
-            "heading": -0.10878948853448296,
-            "angularVelocity": -3.996218113987742e-8,
-            "velocityX": -3.3548500830442913,
-            "velocityY": 1.7480333120587623,
-            "timestamp": 0.5242209003256496
-        },
-        {
-            "x": 6.14377891604402,
-            "y": 6.540845489176542,
-            "heading": -0.10878948531130445,
-            "angularVelocity": 3.074256021150283e-8,
-            "velocityX": -3.549090907103546,
-            "velocityY": 1.309424805695852,
-            "timestamp": 0.6290650803907796
-        },
-        {
-            "x": 5.750100268474419,
-            "y": 6.5890532439035745,
-            "heading": -0.10878941324219087,
-            "angularVelocity": 6.873926003335433e-7,
-            "velocityX": -3.754892711498581,
-            "velocityY": 0.4598038221776824,
-            "timestamp": 0.7339092604559095
-        },
-        {
-            "x": 5.35529697720587,
-            "y": 6.626964014921626,
-            "heading": -0.10878934177449004,
-            "angularVelocity": 6.816563474487017e-7,
-            "velocityX": -3.7656195224503146,
-            "velocityY": 0.3615915637329853,
-            "timestamp": 0.8387534405210395
-        },
-        {
-            "x": 4.9606240931120595,
-            "y": 6.587718813133039,
-            "heading": -0.10878931155021784,
-            "angularVelocity": 2.882780157548741e-7,
-            "velocityX": -3.7643757035310585,
-            "velocityY": -0.3743193161910223,
-            "timestamp": 0.9435976205861695
-        },
-        {
-            "x": 4.585497747567555,
-            "y": 6.458927912145357,
-            "heading": -0.10878930710495487,
-            "angularVelocity": 4.239875765382173e-8,
-            "velocityX": -3.5779415253328617,
-            "velocityY": -1.2284029586351546,
-            "timestamp": 1.0484418006512994
-        },
-        {
-            "x": 4.215349929173217,
-            "y": 6.316458743577884,
-            "heading": -0.10878930270384918,
-            "angularVelocity": 4.197758707259389e-8,
-            "velocityX": -3.530456513317188,
-            "velocityY": -1.3588657804273945,
-            "timestamp": 1.1532859807164293
-        },
-        {
-            "x": 3.8452021463353314,
-            "y": 6.173989482631574,
-            "heading": -0.10878929830273087,
-            "angularVelocity": 4.197770719831182e-8,
-            "velocityX": -3.530456174181034,
-            "velocityY": -1.3588666615333942,
-            "timestamp": 1.2581301607815591
-        },
-        {
-            "x": 3.475054376591526,
-            "y": 6.0315201876656985,
-            "heading": -0.10878929390160265,
-            "angularVelocity": 4.197780205439048e-8,
-            "velocityX": -3.530456049290165,
-            "velocityY": -1.3588669860107905,
-            "timestamp": 1.362974340846689
-        },
-        {
-            "x": 3.104906616234201,
-            "y": 5.889050868312964,
-            "heading": -0.10878928950045777,
-            "angularVelocity": 4.19779608468647e-8,
-            "velocityX": -3.530455959762256,
-            "velocityY": -1.358867218611777,
-            "timestamp": 1.4678185209118189
-        },
-        {
-            "x": 2.7629439773689555,
-            "y": 5.757430528072475,
-            "heading": -0.06527490613894259,
-            "angularVelocity": 0.4150386157294016,
-            "velocityX": -3.2616272896866185,
-            "velocityY": -1.2553900479618894,
-            "timestamp": 1.5726627009769487
-        },
-        {
-            "x": 2.506471990883214,
-            "y": 5.658715266844828,
-            "heading": -0.03263773447849684,
-            "angularVelocity": 0.311292163667756,
-            "velocityX": -2.446220537243172,
-            "velocityY": -0.9415425936501655,
-            "timestamp": 1.6775068810420786
-        },
-        {
-            "x": 2.335490664707938,
-            "y": 5.592905089907167,
-            "heading": -0.010879217132893882,
-            "angularVelocity": 0.20753195200808147,
-            "velocityX": -1.6308137091544919,
-            "velocityY": -0.6276950890052175,
-            "timestamp": 1.7823510611072084
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0.10376557979790346,
-            "velocityX": -0.8154068700316104,
-            "velocityY": -0.3138475582214142,
-            "timestamp": 1.8871952411723383
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 2.158258486540926e-34,
-            "velocityY": 0,
-            "timestamp": 1.9920394212374681
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.traj
deleted file mode 100644
index 5429265f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.traj	
+++ /dev/null
@@ -1,184 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -7.048442713490885e-38,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.598761685357721,
-            "y": 5.839328892781844,
-            "heading": -0.010877614975927053,
-            "angularVelocity": -0.10375029848266179,
-            "velocityX": -0.774848108801196,
-            "velocityY": 0.4037314494285639,
-            "timestamp": 0.10484418006512992
-        },
-        {
-            "x": 7.436285061134455,
-            "y": 5.923986667743732,
-            "heading": -0.03263383242489801,
-            "angularVelocity": -0.20751001567713226,
-            "velocityX": -1.5496961693280003,
-            "velocityY": 0.8074627977375359,
-            "timestamp": 0.20968836013025985
-        },
-        {
-            "x": 7.192570139480125,
-            "y": 6.050973310343331,
-            "heading": -0.06527034306657675,
-            "angularVelocity": -0.31128585889464805,
-            "velocityX": -2.3245441139692504,
-            "velocityY": 1.2111940073422762,
-            "timestamp": 0.3145325401953898
-        },
-        {
-            "x": 6.867616948374112,
-            "y": 6.220288799737083,
-            "heading": -0.10878948434468085,
-            "angularVelocity": -0.41508399656585776,
-            "velocityX": -3.0993917917441864,
-            "velocityY": 1.6149250181418864,
-            "timestamp": 0.4193767202605197
-        },
-        {
-            "x": 6.5158804421759,
-            "y": 6.403559919066418,
-            "heading": -0.10878948853448296,
-            "angularVelocity": -3.996218113987742e-8,
-            "velocityX": -3.3548500830442913,
-            "velocityY": 1.7480333120587623,
-            "timestamp": 0.5242209003256496
-        },
-        {
-            "x": 6.14377891604402,
-            "y": 6.540845489176542,
-            "heading": -0.10878948531130445,
-            "angularVelocity": 3.074256021150283e-8,
-            "velocityX": -3.549090907103546,
-            "velocityY": 1.309424805695852,
-            "timestamp": 0.6290650803907796
-        },
-        {
-            "x": 5.750100268474419,
-            "y": 6.5890532439035745,
-            "heading": -0.10878941324219087,
-            "angularVelocity": 6.873926003335433e-7,
-            "velocityX": -3.754892711498581,
-            "velocityY": 0.4598038221776824,
-            "timestamp": 0.7339092604559095
-        },
-        {
-            "x": 5.35529697720587,
-            "y": 6.626964014921626,
-            "heading": -0.10878934177449004,
-            "angularVelocity": 6.816563474487017e-7,
-            "velocityX": -3.7656195224503146,
-            "velocityY": 0.3615915637329853,
-            "timestamp": 0.8387534405210395
-        },
-        {
-            "x": 4.9606240931120595,
-            "y": 6.587718813133039,
-            "heading": -0.10878931155021784,
-            "angularVelocity": 2.882780157548741e-7,
-            "velocityX": -3.7643757035310585,
-            "velocityY": -0.3743193161910223,
-            "timestamp": 0.9435976205861695
-        },
-        {
-            "x": 4.585497747567555,
-            "y": 6.458927912145357,
-            "heading": -0.10878930710495487,
-            "angularVelocity": 4.239875765382173e-8,
-            "velocityX": -3.5779415253328617,
-            "velocityY": -1.2284029586351546,
-            "timestamp": 1.0484418006512994
-        },
-        {
-            "x": 4.215349929173217,
-            "y": 6.316458743577884,
-            "heading": -0.10878930270384918,
-            "angularVelocity": 4.197758707259389e-8,
-            "velocityX": -3.530456513317188,
-            "velocityY": -1.3588657804273945,
-            "timestamp": 1.1532859807164293
-        },
-        {
-            "x": 3.8452021463353314,
-            "y": 6.173989482631574,
-            "heading": -0.10878929830273087,
-            "angularVelocity": 4.197770719831182e-8,
-            "velocityX": -3.530456174181034,
-            "velocityY": -1.3588666615333942,
-            "timestamp": 1.2581301607815591
-        },
-        {
-            "x": 3.475054376591526,
-            "y": 6.0315201876656985,
-            "heading": -0.10878929390160265,
-            "angularVelocity": 4.197780205439048e-8,
-            "velocityX": -3.530456049290165,
-            "velocityY": -1.3588669860107905,
-            "timestamp": 1.362974340846689
-        },
-        {
-            "x": 3.104906616234201,
-            "y": 5.889050868312964,
-            "heading": -0.10878928950045777,
-            "angularVelocity": 4.19779608468647e-8,
-            "velocityX": -3.530455959762256,
-            "velocityY": -1.358867218611777,
-            "timestamp": 1.4678185209118189
-        },
-        {
-            "x": 2.7629439773689555,
-            "y": 5.757430528072475,
-            "heading": -0.06527490613894259,
-            "angularVelocity": 0.4150386157294016,
-            "velocityX": -3.2616272896866185,
-            "velocityY": -1.2553900479618894,
-            "timestamp": 1.5726627009769487
-        },
-        {
-            "x": 2.506471990883214,
-            "y": 5.658715266844828,
-            "heading": -0.03263773447849684,
-            "angularVelocity": 0.311292163667756,
-            "velocityX": -2.446220537243172,
-            "velocityY": -0.9415425936501655,
-            "timestamp": 1.6775068810420786
-        },
-        {
-            "x": 2.335490664707938,
-            "y": 5.592905089907167,
-            "heading": -0.010879217132893882,
-            "angularVelocity": 0.20753195200808147,
-            "velocityX": -1.6308137091544919,
-            "velocityY": -0.6276950890052175,
-            "timestamp": 1.7823510611072084
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0.10376557979790346,
-            "velocityX": -0.8154068700316104,
-            "velocityY": -0.3138475582214142,
-            "timestamp": 1.8871952411723383
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 2.158258486540926e-34,
-            "velocityY": 0,
-            "timestamp": 1.9920394212374681
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.1.traj
deleted file mode 100644
index cf63c7e3..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.1.traj	
+++ /dev/null
@@ -1,346 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": -2.981242670977675e-31,
-            "velocityX": -5.763030958279131e-32,
-            "velocityY": 3.5928963704249306e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.6464505983184665,
-            "y": 5.761517456765686,
-            "heading": 0.04441523610418216,
-            "angularVelocity": 0.571175712894524,
-            "velocityX": -0.43144211544178024,
-            "velocityY": -0.4563021319898165,
-            "timestamp": 0.07776107264628128
-        },
-        {
-            "x": 7.57910774134765,
-            "y": 5.690410071738714,
-            "heading": 0.1294441379757556,
-            "angularVelocity": 1.0934635927458562,
-            "velocityX": -0.8660227370723712,
-            "velocityY": -0.914434209908928,
-            "timestamp": 0.15552214529256256
-        },
-        {
-            "x": 7.4776816969238515,
-            "y": 5.583411802725407,
-            "heading": 0.24873158333085876,
-            "angularVelocity": 1.5340252043296483,
-            "velocityX": -1.3043292867777738,
-            "velocityY": -1.3759875651409672,
-            "timestamp": 0.23328321793884382
-        },
-        {
-            "x": 7.341761998810456,
-            "y": 5.440105324158156,
-            "heading": 0.38885380810835574,
-            "angularVelocity": 1.8019584865410045,
-            "velocityX": -1.747914393255167,
-            "velocityY": -1.842907687489359,
-            "timestamp": 0.3110442905851251
-        },
-        {
-            "x": 7.171108690296084,
-            "y": 5.260470754663091,
-            "heading": 0.5147184350397563,
-            "angularVelocity": 1.6186071339824826,
-            "velocityX": -2.1945853202236263,
-            "velocityY": -2.3100834824152265,
-            "timestamp": 0.3888053632314064
-        },
-        {
-            "x": 6.970033766909185,
-            "y": 5.052418524467747,
-            "heading": 0.5282282166004705,
-            "angularVelocity": 0.17373450623768458,
-            "velocityX": -2.585804394720051,
-            "velocityY": -2.67553189681076,
-            "timestamp": 0.4665664358776877
-        },
-        {
-            "x": 6.768438732548067,
-            "y": 4.838192511419719,
-            "heading": 0.5282282675213732,
-            "angularVelocity": 6.548379670470849e-7,
-            "velocityX": -2.592492972391604,
-            "velocityY": -2.7549261572367425,
-            "timestamp": 0.5443275085239689
-        },
-        {
-            "x": 6.5423880710685225,
-            "y": 4.649952125007829,
-            "heading": 0.5282283002658353,
-            "angularVelocity": 4.210906690145718e-7,
-            "velocityX": -2.9069900116707847,
-            "velocityY": -2.42075346964613,
-            "timestamp": 0.6220885811702502
-        },
-        {
-            "x": 6.292219183977312,
-            "y": 4.49519788912002,
-            "heading": 0.5282283052888641,
-            "angularVelocity": 6.459567356551782e-8,
-            "velocityX": -3.217148099656167,
-            "velocityY": -1.9901247580747212,
-            "timestamp": 0.6998496538165315
-        },
-        {
-            "x": 6.0420493950115075,
-            "y": 4.3404451111724525,
-            "heading": 0.5282283103117955,
-            "angularVelocity": 6.459441933764687e-8,
-            "velocityX": -3.217159697677684,
-            "velocityY": -1.990106009101862,
-            "timestamp": 0.7776107264628128
-        },
-        {
-            "x": 5.780250458218128,
-            "y": 4.206300927357815,
-            "heading": 0.5282283154384368,
-            "angularVelocity": 6.592812032299238e-8,
-            "velocityX": -3.366709432935023,
-            "velocityY": -1.7250814482051704,
-            "timestamp": 0.8553717991090941
-        },
-        {
-            "x": 5.499389832161033,
-            "y": 4.11883296395985,
-            "heading": 0.5282284192975907,
-            "angularVelocity": 0.0000013356188425542589,
-            "velocityX": -3.611840944307328,
-            "velocityY": -1.1248296920470104,
-            "timestamp": 0.9331328717553754
-        },
-        {
-            "x": 5.2375954046767585,
-            "y": 3.9846908767107623,
-            "heading": 0.5282448470390027,
-            "angularVelocity": 0.0002112591919891304,
-            "velocityX": -3.3666514436488617,
-            "velocityY": -1.7250544865716144,
-            "timestamp": 1.0108939444016567
-        },
-        {
-            "x": 4.9990709441074195,
-            "y": 3.8334805845295827,
-            "heading": 0.56802081206443,
-            "angularVelocity": 0.5115151279660829,
-            "velocityX": -3.067401881842984,
-            "velocityY": -1.944549978997939,
-            "timestamp": 1.0886550170479379
-        },
-        {
-            "x": 4.779271759525853,
-            "y": 3.6735037985529155,
-            "heading": 0.6417219019727908,
-            "angularVelocity": 0.9477890080506808,
-            "velocityX": -2.826596613210087,
-            "velocityY": -2.057286255609729,
-            "timestamp": 1.166416089694219
-        },
-        {
-            "x": 4.5625385904493,
-            "y": 3.4912294193947493,
-            "heading": 0.6783171685956464,
-            "angularVelocity": 0.47061164895862556,
-            "velocityX": -2.7871679453603333,
-            "velocityY": -2.3440311836655594,
-            "timestamp": 1.2441771623405002
-        },
-        {
-            "x": 4.372028277853454,
-            "y": 3.267088484433369,
-            "heading": 0.6783172621445687,
-            "angularVelocity": 0.0000012030302442626518,
-            "velocityX": -2.4499445045265382,
-            "velocityY": -2.8824311102413667,
-            "timestamp": 1.3219382349867814
-        },
-        {
-            "x": 4.1999208824099785,
-            "y": 3.028525386131778,
-            "heading": 0.678317151805174,
-            "angularVelocity": -0.0000014189541221835924,
-            "velocityX": -2.213284739864076,
-            "velocityY": -3.067898759405809,
-            "timestamp": 1.3996993076330626
-        },
-        {
-            "x": 4.015537599692405,
-            "y": 2.8383590216563426,
-            "heading": 0.6532286408155417,
-            "angularVelocity": -0.32263586568249475,
-            "velocityX": -2.371151482905778,
-            "velocityY": -2.4455213644037763,
-            "timestamp": 1.4774603802793438
-        },
-        {
-            "x": 3.8266113642227695,
-            "y": 2.6983370404519986,
-            "heading": 0.6187227841336836,
-            "angularVelocity": -0.4437420357974964,
-            "velocityX": -2.429573423311953,
-            "velocityY": -1.8006693637223163,
-            "timestamp": 1.555221452925625
-        },
-        {
-            "x": 3.635277950307217,
-            "y": 2.6086556112169066,
-            "heading": 0.5790475746735078,
-            "angularVelocity": -0.5102194209775099,
-            "velocityX": -2.460529509229992,
-            "velocityY": -1.1532946522360021,
-            "timestamp": 1.6329825255719062
-        },
-        {
-            "x": 3.4425296783447266,
-            "y": 2.5693652629852295,
-            "heading": 0.5361762744054714,
-            "angularVelocity": -0.5513208448531661,
-            "velocityX": -2.4787244491760223,
-            "velocityY": -0.5052701421751292,
-            "timestamp": 1.7107435982181873
-        },
-        {
-            "x": 3.273952117174966,
-            "y": 2.5733990398146904,
-            "heading": 0.49726118807033537,
-            "angularVelocity": -0.5743772146540196,
-            "velocityX": -2.4881638242809143,
-            "velocityY": 0.05953756545439798,
-            "timestamp": 1.7784953915385004
-        },
-        {
-            "x": 3.1059881626789445,
-            "y": 2.6157002802478813,
-            "heading": 0.456882179671172,
-            "angularVelocity": -0.5959843484623638,
-            "velocityX": -2.4791071389347814,
-            "velocityY": 0.6243560260198714,
-            "timestamp": 1.8462471848588136
-        },
-        {
-            "x": 2.9403803042277463,
-            "y": 2.6962017103764078,
-            "heading": 0.41517848324514556,
-            "angularVelocity": -0.6155364217277942,
-            "velocityX": -2.444331734043506,
-            "velocityY": 1.1881815400506988,
-            "timestamp": 1.9139989781791267
-        },
-        {
-            "x": 2.7796953039379972,
-            "y": 2.8146585273254012,
-            "heading": 0.3723609476398507,
-            "angularVelocity": -0.6319764172568042,
-            "velocityX": -2.3716715442508174,
-            "velocityY": 1.7483938231562413,
-            "timestamp": 1.9817507714994398
-        },
-        {
-            "x": 2.628009703912687,
-            "y": 2.970316258543535,
-            "heading": 0.3287788631859365,
-            "angularVelocity": -0.6432609724124856,
-            "velocityX": -2.23884258396202,
-            "velocityY": 2.2974702748047298,
-            "timestamp": 2.0495025648197527
-        },
-        {
-            "x": 2.4923790399755665,
-            "y": 3.160718496478581,
-            "heading": 0.28508218813026037,
-            "angularVelocity": -0.6449523018392979,
-            "velocityX": -2.001875630005736,
-            "velocityY": 2.8102907480968544,
-            "timestamp": 2.1172543581400656
-        },
-        {
-            "x": 2.384956097652455,
-            "y": 3.376988776463332,
-            "heading": 0.24257394609876834,
-            "angularVelocity": -0.627411319291935,
-            "velocityX": -1.5855365158416335,
-            "velocityY": 3.192096760631575,
-            "timestamp": 2.1850061514603785
-        },
-        {
-            "x": 2.3144199991578422,
-            "y": 3.6018152629752547,
-            "heading": 0.17996830737750358,
-            "angularVelocity": -0.9240440090682317,
-            "velocityX": -1.0410956675513523,
-            "velocityY": 3.3183842890918167,
-            "timestamp": 2.2527579447806914
-        },
-        {
-            "x": 2.277461106698391,
-            "y": 3.8129334421080285,
-            "heading": 0.07321465766316056,
-            "angularVelocity": -1.57565791963083,
-            "velocityX": -0.5455042685692306,
-            "velocityY": 3.1160530044520236,
-            "timestamp": 2.3205097381010042
-        },
-        {
-            "x": 2.2642620699526455,
-            "y": 3.9942911908489713,
-            "heading": -0.04721496632936169,
-            "angularVelocity": -1.7775119755599884,
-            "velocityX": -0.1948145738865332,
-            "velocityY": 2.676796286166625,
-            "timestamp": 2.388261531421317
-        },
-        {
-            "x": 2.2685016834700633,
-            "y": 4.1415779064179725,
-            "heading": -0.1677701441610935,
-            "angularVelocity": -1.7793651197065368,
-            "velocityX": 0.06257566493294611,
-            "velocityY": 2.173916118687331,
-            "timestamp": 2.45601332474163
-        },
-        {
-            "x": 2.2803498467055876,
-            "y": 4.251629868834139,
-            "heading": -0.2721018795383186,
-            "angularVelocity": -1.5399110527447033,
-            "velocityX": 0.17487600925202312,
-            "velocityY": 1.624340213341216,
-            "timestamp": 2.523765118061943
-        },
-        {
-            "x": 2.2924250535428135,
-            "y": 4.3241863896986095,
-            "heading": -0.3487735727111741,
-            "angularVelocity": -1.1316555535345214,
-            "velocityX": 0.17822711762236845,
-            "velocityY": 1.0709166105971757,
-            "timestamp": 2.591516911382256
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.6084920452794201,
-            "velocityX": 0.1118043683563332,
-            "velocityY": 0.5286001823165565,
-            "timestamp": 2.6592687047025687
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -7.720140427747849e-31,
-            "velocityX": 0,
-            "velocityY": -1.167304580662352e-31,
-            "timestamp": 2.7270204980228816
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.traj
deleted file mode 100644
index cf63c7e3..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.traj	
+++ /dev/null
@@ -1,346 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": -2.981242670977675e-31,
-            "velocityX": -5.763030958279131e-32,
-            "velocityY": 3.5928963704249306e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.6464505983184665,
-            "y": 5.761517456765686,
-            "heading": 0.04441523610418216,
-            "angularVelocity": 0.571175712894524,
-            "velocityX": -0.43144211544178024,
-            "velocityY": -0.4563021319898165,
-            "timestamp": 0.07776107264628128
-        },
-        {
-            "x": 7.57910774134765,
-            "y": 5.690410071738714,
-            "heading": 0.1294441379757556,
-            "angularVelocity": 1.0934635927458562,
-            "velocityX": -0.8660227370723712,
-            "velocityY": -0.914434209908928,
-            "timestamp": 0.15552214529256256
-        },
-        {
-            "x": 7.4776816969238515,
-            "y": 5.583411802725407,
-            "heading": 0.24873158333085876,
-            "angularVelocity": 1.5340252043296483,
-            "velocityX": -1.3043292867777738,
-            "velocityY": -1.3759875651409672,
-            "timestamp": 0.23328321793884382
-        },
-        {
-            "x": 7.341761998810456,
-            "y": 5.440105324158156,
-            "heading": 0.38885380810835574,
-            "angularVelocity": 1.8019584865410045,
-            "velocityX": -1.747914393255167,
-            "velocityY": -1.842907687489359,
-            "timestamp": 0.3110442905851251
-        },
-        {
-            "x": 7.171108690296084,
-            "y": 5.260470754663091,
-            "heading": 0.5147184350397563,
-            "angularVelocity": 1.6186071339824826,
-            "velocityX": -2.1945853202236263,
-            "velocityY": -2.3100834824152265,
-            "timestamp": 0.3888053632314064
-        },
-        {
-            "x": 6.970033766909185,
-            "y": 5.052418524467747,
-            "heading": 0.5282282166004705,
-            "angularVelocity": 0.17373450623768458,
-            "velocityX": -2.585804394720051,
-            "velocityY": -2.67553189681076,
-            "timestamp": 0.4665664358776877
-        },
-        {
-            "x": 6.768438732548067,
-            "y": 4.838192511419719,
-            "heading": 0.5282282675213732,
-            "angularVelocity": 6.548379670470849e-7,
-            "velocityX": -2.592492972391604,
-            "velocityY": -2.7549261572367425,
-            "timestamp": 0.5443275085239689
-        },
-        {
-            "x": 6.5423880710685225,
-            "y": 4.649952125007829,
-            "heading": 0.5282283002658353,
-            "angularVelocity": 4.210906690145718e-7,
-            "velocityX": -2.9069900116707847,
-            "velocityY": -2.42075346964613,
-            "timestamp": 0.6220885811702502
-        },
-        {
-            "x": 6.292219183977312,
-            "y": 4.49519788912002,
-            "heading": 0.5282283052888641,
-            "angularVelocity": 6.459567356551782e-8,
-            "velocityX": -3.217148099656167,
-            "velocityY": -1.9901247580747212,
-            "timestamp": 0.6998496538165315
-        },
-        {
-            "x": 6.0420493950115075,
-            "y": 4.3404451111724525,
-            "heading": 0.5282283103117955,
-            "angularVelocity": 6.459441933764687e-8,
-            "velocityX": -3.217159697677684,
-            "velocityY": -1.990106009101862,
-            "timestamp": 0.7776107264628128
-        },
-        {
-            "x": 5.780250458218128,
-            "y": 4.206300927357815,
-            "heading": 0.5282283154384368,
-            "angularVelocity": 6.592812032299238e-8,
-            "velocityX": -3.366709432935023,
-            "velocityY": -1.7250814482051704,
-            "timestamp": 0.8553717991090941
-        },
-        {
-            "x": 5.499389832161033,
-            "y": 4.11883296395985,
-            "heading": 0.5282284192975907,
-            "angularVelocity": 0.0000013356188425542589,
-            "velocityX": -3.611840944307328,
-            "velocityY": -1.1248296920470104,
-            "timestamp": 0.9331328717553754
-        },
-        {
-            "x": 5.2375954046767585,
-            "y": 3.9846908767107623,
-            "heading": 0.5282448470390027,
-            "angularVelocity": 0.0002112591919891304,
-            "velocityX": -3.3666514436488617,
-            "velocityY": -1.7250544865716144,
-            "timestamp": 1.0108939444016567
-        },
-        {
-            "x": 4.9990709441074195,
-            "y": 3.8334805845295827,
-            "heading": 0.56802081206443,
-            "angularVelocity": 0.5115151279660829,
-            "velocityX": -3.067401881842984,
-            "velocityY": -1.944549978997939,
-            "timestamp": 1.0886550170479379
-        },
-        {
-            "x": 4.779271759525853,
-            "y": 3.6735037985529155,
-            "heading": 0.6417219019727908,
-            "angularVelocity": 0.9477890080506808,
-            "velocityX": -2.826596613210087,
-            "velocityY": -2.057286255609729,
-            "timestamp": 1.166416089694219
-        },
-        {
-            "x": 4.5625385904493,
-            "y": 3.4912294193947493,
-            "heading": 0.6783171685956464,
-            "angularVelocity": 0.47061164895862556,
-            "velocityX": -2.7871679453603333,
-            "velocityY": -2.3440311836655594,
-            "timestamp": 1.2441771623405002
-        },
-        {
-            "x": 4.372028277853454,
-            "y": 3.267088484433369,
-            "heading": 0.6783172621445687,
-            "angularVelocity": 0.0000012030302442626518,
-            "velocityX": -2.4499445045265382,
-            "velocityY": -2.8824311102413667,
-            "timestamp": 1.3219382349867814
-        },
-        {
-            "x": 4.1999208824099785,
-            "y": 3.028525386131778,
-            "heading": 0.678317151805174,
-            "angularVelocity": -0.0000014189541221835924,
-            "velocityX": -2.213284739864076,
-            "velocityY": -3.067898759405809,
-            "timestamp": 1.3996993076330626
-        },
-        {
-            "x": 4.015537599692405,
-            "y": 2.8383590216563426,
-            "heading": 0.6532286408155417,
-            "angularVelocity": -0.32263586568249475,
-            "velocityX": -2.371151482905778,
-            "velocityY": -2.4455213644037763,
-            "timestamp": 1.4774603802793438
-        },
-        {
-            "x": 3.8266113642227695,
-            "y": 2.6983370404519986,
-            "heading": 0.6187227841336836,
-            "angularVelocity": -0.4437420357974964,
-            "velocityX": -2.429573423311953,
-            "velocityY": -1.8006693637223163,
-            "timestamp": 1.555221452925625
-        },
-        {
-            "x": 3.635277950307217,
-            "y": 2.6086556112169066,
-            "heading": 0.5790475746735078,
-            "angularVelocity": -0.5102194209775099,
-            "velocityX": -2.460529509229992,
-            "velocityY": -1.1532946522360021,
-            "timestamp": 1.6329825255719062
-        },
-        {
-            "x": 3.4425296783447266,
-            "y": 2.5693652629852295,
-            "heading": 0.5361762744054714,
-            "angularVelocity": -0.5513208448531661,
-            "velocityX": -2.4787244491760223,
-            "velocityY": -0.5052701421751292,
-            "timestamp": 1.7107435982181873
-        },
-        {
-            "x": 3.273952117174966,
-            "y": 2.5733990398146904,
-            "heading": 0.49726118807033537,
-            "angularVelocity": -0.5743772146540196,
-            "velocityX": -2.4881638242809143,
-            "velocityY": 0.05953756545439798,
-            "timestamp": 1.7784953915385004
-        },
-        {
-            "x": 3.1059881626789445,
-            "y": 2.6157002802478813,
-            "heading": 0.456882179671172,
-            "angularVelocity": -0.5959843484623638,
-            "velocityX": -2.4791071389347814,
-            "velocityY": 0.6243560260198714,
-            "timestamp": 1.8462471848588136
-        },
-        {
-            "x": 2.9403803042277463,
-            "y": 2.6962017103764078,
-            "heading": 0.41517848324514556,
-            "angularVelocity": -0.6155364217277942,
-            "velocityX": -2.444331734043506,
-            "velocityY": 1.1881815400506988,
-            "timestamp": 1.9139989781791267
-        },
-        {
-            "x": 2.7796953039379972,
-            "y": 2.8146585273254012,
-            "heading": 0.3723609476398507,
-            "angularVelocity": -0.6319764172568042,
-            "velocityX": -2.3716715442508174,
-            "velocityY": 1.7483938231562413,
-            "timestamp": 1.9817507714994398
-        },
-        {
-            "x": 2.628009703912687,
-            "y": 2.970316258543535,
-            "heading": 0.3287788631859365,
-            "angularVelocity": -0.6432609724124856,
-            "velocityX": -2.23884258396202,
-            "velocityY": 2.2974702748047298,
-            "timestamp": 2.0495025648197527
-        },
-        {
-            "x": 2.4923790399755665,
-            "y": 3.160718496478581,
-            "heading": 0.28508218813026037,
-            "angularVelocity": -0.6449523018392979,
-            "velocityX": -2.001875630005736,
-            "velocityY": 2.8102907480968544,
-            "timestamp": 2.1172543581400656
-        },
-        {
-            "x": 2.384956097652455,
-            "y": 3.376988776463332,
-            "heading": 0.24257394609876834,
-            "angularVelocity": -0.627411319291935,
-            "velocityX": -1.5855365158416335,
-            "velocityY": 3.192096760631575,
-            "timestamp": 2.1850061514603785
-        },
-        {
-            "x": 2.3144199991578422,
-            "y": 3.6018152629752547,
-            "heading": 0.17996830737750358,
-            "angularVelocity": -0.9240440090682317,
-            "velocityX": -1.0410956675513523,
-            "velocityY": 3.3183842890918167,
-            "timestamp": 2.2527579447806914
-        },
-        {
-            "x": 2.277461106698391,
-            "y": 3.8129334421080285,
-            "heading": 0.07321465766316056,
-            "angularVelocity": -1.57565791963083,
-            "velocityX": -0.5455042685692306,
-            "velocityY": 3.1160530044520236,
-            "timestamp": 2.3205097381010042
-        },
-        {
-            "x": 2.2642620699526455,
-            "y": 3.9942911908489713,
-            "heading": -0.04721496632936169,
-            "angularVelocity": -1.7775119755599884,
-            "velocityX": -0.1948145738865332,
-            "velocityY": 2.676796286166625,
-            "timestamp": 2.388261531421317
-        },
-        {
-            "x": 2.2685016834700633,
-            "y": 4.1415779064179725,
-            "heading": -0.1677701441610935,
-            "angularVelocity": -1.7793651197065368,
-            "velocityX": 0.06257566493294611,
-            "velocityY": 2.173916118687331,
-            "timestamp": 2.45601332474163
-        },
-        {
-            "x": 2.2803498467055876,
-            "y": 4.251629868834139,
-            "heading": -0.2721018795383186,
-            "angularVelocity": -1.5399110527447033,
-            "velocityX": 0.17487600925202312,
-            "velocityY": 1.624340213341216,
-            "timestamp": 2.523765118061943
-        },
-        {
-            "x": 2.2924250535428135,
-            "y": 4.3241863896986095,
-            "heading": -0.3487735727111741,
-            "angularVelocity": -1.1316555535345214,
-            "velocityX": 0.17822711762236845,
-            "velocityY": 1.0709166105971757,
-            "timestamp": 2.591516911382256
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.6084920452794201,
-            "velocityX": 0.1118043683563332,
-            "velocityY": 0.5286001823165565,
-            "timestamp": 2.6592687047025687
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -7.720140427747849e-31,
-            "velocityX": 0,
-            "velocityY": -1.167304580662352e-31,
-            "timestamp": 2.7270204980228816
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.1.traj
deleted file mode 100644
index 57334e24..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.1.traj	
+++ /dev/null
@@ -1,139 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -2.1749467140472343e-39,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 4.203445372005539,
-            "heading": -7.837837605587582e-19,
-            "angularVelocity": -8.185686568731239e-18,
-            "velocityX": 1.377912473786286e-33,
-            "velocityY": 0.7983807350946673,
-            "timestamp": 0.09575052183150974
-        },
-        {
-            "x": 7.68,
-            "y": 4.356336112982689,
-            "heading": -2.3565356003209568e-18,
-            "angularVelocity": -1.6425517161035916e-17,
-            "velocityX": 3.260516050321855e-34,
-            "velocityY": 1.5967614385035824,
-            "timestamp": 0.19150104366301948
-        },
-        {
-            "x": 7.68,
-            "y": 4.5856722168636,
-            "heading": -4.728579560129296e-18,
-            "angularVelocity": -2.4773170043438095e-17,
-            "velocityX": -1.693775140889528e-33,
-            "velocityY": 2.3951420785410225,
-            "timestamp": 0.28725156549452924
-        },
-        {
-            "x": 7.68,
-            "y": 4.891453665444746,
-            "heading": -7.932188825954499e-18,
-            "angularVelocity": -3.345787785122934e-17,
-            "velocityX": -9.607950389092107e-34,
-            "velocityY": 3.1935225284643685,
-            "timestamp": 0.38300208732603896
-        },
-        {
-            "x": 7.68,
-            "y": 5.253672199266821,
-            "heading": -4.35728250993897e-17,
-            "angularVelocity": -3.722239377050204e-16,
-            "velocityX": -3.660377688643743e-33,
-            "velocityY": 3.7829405719528495,
-            "timestamp": 0.4787526091575487
-        },
-        {
-            "x": 7.68,
-            "y": 5.61589073308894,
-            "heading": -3.405116375197899e-17,
-            "angularVelocity": 9.944239639932065e-17,
-            "velocityX": -5.791062266320423e-33,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.5745031309890585
-        },
-        {
-            "x": 7.68,
-            "y": 5.978109266911059,
-            "heading": 5.876472199762529e-18,
-            "angularVelocity": 4.169965362904391e-16,
-            "velocityX": -1.6171107443968654e-32,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.6702536528205683
-        },
-        {
-            "x": 7.68,
-            "y": 6.340327800733179,
-            "heading": 2.547705878984039e-17,
-            "angularVelocity": 2.047047495424479e-16,
-            "velocityX": -4.3724642588945734e-33,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.766004174652078
-        },
-        {
-            "x": 7.68,
-            "y": 6.702546334555254,
-            "heading": 7.687233753276096e-18,
-            "angularVelocity": -1.8579350479017399e-16,
-            "velocityX": -2.6103711792186515e-33,
-            "velocityY": 3.7829405719528495,
-            "timestamp": 0.8617546964835878
-        },
-        {
-            "x": 7.68,
-            "y": 7.0083277831364,
-            "heading": 4.613320516549708e-18,
-            "angularVelocity": -3.210335754994756e-17,
-            "velocityX": 1.2273832122580678e-32,
-            "velocityY": 3.1935225284643685,
-            "timestamp": 0.9575052183150976
-        },
-        {
-            "x": 7.68,
-            "y": 7.237663887017311,
-            "heading": 2.3068261479777784e-18,
-            "angularVelocity": -2.4088582761110343e-17,
-            "velocityX": 8.179933156696061e-33,
-            "velocityY": 2.3951420785410225,
-            "timestamp": 1.0532557401466074
-        },
-        {
-            "x": 7.68,
-            "y": 7.390554627994461,
-            "heading": 7.689558058679801e-19,
-            "angularVelocity": -1.6061221530256843e-17,
-            "velocityX": 9.843160057308665e-33,
-            "velocityY": 1.5967614385035824,
-            "timestamp": 1.1490062619781172
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": -8.030826271263815e-18,
-            "velocityX": 3.1854559448557055e-33,
-            "velocityY": 0.7983807350946673,
-            "timestamp": 1.244756783809627
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 2.257351446612169e-43,
-            "velocityY": -1.1946741683702123e-38,
-            "timestamp": 1.3405073056411367
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.traj
deleted file mode 100644
index 57334e24..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.traj	
+++ /dev/null
@@ -1,139 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -2.1749467140472343e-39,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 4.203445372005539,
-            "heading": -7.837837605587582e-19,
-            "angularVelocity": -8.185686568731239e-18,
-            "velocityX": 1.377912473786286e-33,
-            "velocityY": 0.7983807350946673,
-            "timestamp": 0.09575052183150974
-        },
-        {
-            "x": 7.68,
-            "y": 4.356336112982689,
-            "heading": -2.3565356003209568e-18,
-            "angularVelocity": -1.6425517161035916e-17,
-            "velocityX": 3.260516050321855e-34,
-            "velocityY": 1.5967614385035824,
-            "timestamp": 0.19150104366301948
-        },
-        {
-            "x": 7.68,
-            "y": 4.5856722168636,
-            "heading": -4.728579560129296e-18,
-            "angularVelocity": -2.4773170043438095e-17,
-            "velocityX": -1.693775140889528e-33,
-            "velocityY": 2.3951420785410225,
-            "timestamp": 0.28725156549452924
-        },
-        {
-            "x": 7.68,
-            "y": 4.891453665444746,
-            "heading": -7.932188825954499e-18,
-            "angularVelocity": -3.345787785122934e-17,
-            "velocityX": -9.607950389092107e-34,
-            "velocityY": 3.1935225284643685,
-            "timestamp": 0.38300208732603896
-        },
-        {
-            "x": 7.68,
-            "y": 5.253672199266821,
-            "heading": -4.35728250993897e-17,
-            "angularVelocity": -3.722239377050204e-16,
-            "velocityX": -3.660377688643743e-33,
-            "velocityY": 3.7829405719528495,
-            "timestamp": 0.4787526091575487
-        },
-        {
-            "x": 7.68,
-            "y": 5.61589073308894,
-            "heading": -3.405116375197899e-17,
-            "angularVelocity": 9.944239639932065e-17,
-            "velocityX": -5.791062266320423e-33,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.5745031309890585
-        },
-        {
-            "x": 7.68,
-            "y": 5.978109266911059,
-            "heading": 5.876472199762529e-18,
-            "angularVelocity": 4.169965362904391e-16,
-            "velocityX": -1.6171107443968654e-32,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.6702536528205683
-        },
-        {
-            "x": 7.68,
-            "y": 6.340327800733179,
-            "heading": 2.547705878984039e-17,
-            "angularVelocity": 2.047047495424479e-16,
-            "velocityX": -4.3724642588945734e-33,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.766004174652078
-        },
-        {
-            "x": 7.68,
-            "y": 6.702546334555254,
-            "heading": 7.687233753276096e-18,
-            "angularVelocity": -1.8579350479017399e-16,
-            "velocityX": -2.6103711792186515e-33,
-            "velocityY": 3.7829405719528495,
-            "timestamp": 0.8617546964835878
-        },
-        {
-            "x": 7.68,
-            "y": 7.0083277831364,
-            "heading": 4.613320516549708e-18,
-            "angularVelocity": -3.210335754994756e-17,
-            "velocityX": 1.2273832122580678e-32,
-            "velocityY": 3.1935225284643685,
-            "timestamp": 0.9575052183150976
-        },
-        {
-            "x": 7.68,
-            "y": 7.237663887017311,
-            "heading": 2.3068261479777784e-18,
-            "angularVelocity": -2.4088582761110343e-17,
-            "velocityX": 8.179933156696061e-33,
-            "velocityY": 2.3951420785410225,
-            "timestamp": 1.0532557401466074
-        },
-        {
-            "x": 7.68,
-            "y": 7.390554627994461,
-            "heading": 7.689558058679801e-19,
-            "angularVelocity": -1.6061221530256843e-17,
-            "velocityX": 9.843160057308665e-33,
-            "velocityY": 1.5967614385035824,
-            "timestamp": 1.1490062619781172
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": -8.030826271263815e-18,
-            "velocityX": 3.1854559448557055e-33,
-            "velocityY": 0.7983807350946673,
-            "timestamp": 1.244756783809627
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 2.257351446612169e-43,
-            "velocityY": -1.1946741683702123e-38,
-            "timestamp": 1.3405073056411367
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C2.1.traj
deleted file mode 100644
index 5ffe4bce..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C2.1.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -3.817834980565894e-40,
-            "angularVelocity": -1.9855221195343998e-39,
-            "velocityX": -9.621145750064654e-40,
-            "velocityY": 1.4575297691012433e-39,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 4.210500005575846,
-            "heading": 3.479114156010015e-22,
-            "angularVelocity": 3.4766412561552746e-21,
-            "velocityX": 2.7068848151306367e-33,
-            "velocityY": 0.8344065481228213,
-            "timestamp": 0.10007112931184182
-        },
-        {
-            "x": 7.68,
-            "y": 4.377500013629847,
-            "heading": 1.081691491825702e-21,
-            "angularVelocity": 7.332585052389997e-21,
-            "velocityX": 5.1589068200097806e-33,
-            "velocityY": 1.6688130652907358,
-            "timestamp": 0.20014225862368365
-        },
-        {
-            "x": 7.68,
-            "y": 4.628000017966616,
-            "heading": 2.2511586271387426e-21,
-            "angularVelocity": 1.1686358860616172e-20,
-            "velocityX": 5.8498916949463956e-33,
-            "velocityY": 2.503219520548835,
-            "timestamp": 0.3002133879355255
-        },
-        {
-            "x": 7.68,
-            "y": 4.962,
-            "heading": 4.481673966100434e-21,
-            "angularVelocity": 2.228929929167172e-20,
-            "velocityX": 6.576791251337367e-33,
-            "velocityY": 3.3376257900775013,
-            "timestamp": 0.4002845172473673
-        },
-        {
-            "x": 7.68,
-            "y": 5.295999982033384,
-            "heading": 1.4391637904192512e-21,
-            "angularVelocity": -3.0403475801105514e-20,
-            "velocityX": 1.0509973010204301e-32,
-            "velocityY": 3.3376257900775013,
-            "timestamp": 0.5003556465592092
-        },
-        {
-            "x": 7.68,
-            "y": 5.5464999863701525,
-            "heading": 4.7457500532831525e-22,
-            "angularVelocity": -9.63903176962576e-21,
-            "velocityX": 7.983324746463204e-33,
-            "velocityY": 2.503219520548835,
-            "timestamp": 0.600426775871051
-        },
-        {
-            "x": 7.68,
-            "y": 5.713499994424153,
-            "heading": 1.0909640932435146e-22,
-            "angularVelocity": -3.652188228166023e-21,
-            "velocityX": 7.341551448907972e-33,
-            "velocityY": 1.6688130652907358,
-            "timestamp": 0.7004979051828928
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -9.42965601107167e-40,
-            "angularVelocity": -1.09018866193586e-21,
-            "velocityX": 7.44470103227488e-33,
-            "velocityY": 0.8344065481228213,
-            "timestamp": 0.8005690344947346
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 8.249839679439821e-43,
-            "angularVelocity": 9.431229878048832e-39,
-            "velocityX": 1.012759498983901e-41,
-            "velocityY": 5.142787784847508e-39,
-            "timestamp": 0.9006401638065764
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C2.traj
deleted file mode 100644
index 5ffe4bce..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C2.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -3.817834980565894e-40,
-            "angularVelocity": -1.9855221195343998e-39,
-            "velocityX": -9.621145750064654e-40,
-            "velocityY": 1.4575297691012433e-39,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 4.210500005575846,
-            "heading": 3.479114156010015e-22,
-            "angularVelocity": 3.4766412561552746e-21,
-            "velocityX": 2.7068848151306367e-33,
-            "velocityY": 0.8344065481228213,
-            "timestamp": 0.10007112931184182
-        },
-        {
-            "x": 7.68,
-            "y": 4.377500013629847,
-            "heading": 1.081691491825702e-21,
-            "angularVelocity": 7.332585052389997e-21,
-            "velocityX": 5.1589068200097806e-33,
-            "velocityY": 1.6688130652907358,
-            "timestamp": 0.20014225862368365
-        },
-        {
-            "x": 7.68,
-            "y": 4.628000017966616,
-            "heading": 2.2511586271387426e-21,
-            "angularVelocity": 1.1686358860616172e-20,
-            "velocityX": 5.8498916949463956e-33,
-            "velocityY": 2.503219520548835,
-            "timestamp": 0.3002133879355255
-        },
-        {
-            "x": 7.68,
-            "y": 4.962,
-            "heading": 4.481673966100434e-21,
-            "angularVelocity": 2.228929929167172e-20,
-            "velocityX": 6.576791251337367e-33,
-            "velocityY": 3.3376257900775013,
-            "timestamp": 0.4002845172473673
-        },
-        {
-            "x": 7.68,
-            "y": 5.295999982033384,
-            "heading": 1.4391637904192512e-21,
-            "angularVelocity": -3.0403475801105514e-20,
-            "velocityX": 1.0509973010204301e-32,
-            "velocityY": 3.3376257900775013,
-            "timestamp": 0.5003556465592092
-        },
-        {
-            "x": 7.68,
-            "y": 5.5464999863701525,
-            "heading": 4.7457500532831525e-22,
-            "angularVelocity": -9.63903176962576e-21,
-            "velocityX": 7.983324746463204e-33,
-            "velocityY": 2.503219520548835,
-            "timestamp": 0.600426775871051
-        },
-        {
-            "x": 7.68,
-            "y": 5.713499994424153,
-            "heading": 1.0909640932435146e-22,
-            "angularVelocity": -3.652188228166023e-21,
-            "velocityX": 7.341551448907972e-33,
-            "velocityY": 1.6688130652907358,
-            "timestamp": 0.7004979051828928
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -9.42965601107167e-40,
-            "angularVelocity": -1.09018866193586e-21,
-            "velocityX": 7.44470103227488e-33,
-            "velocityY": 0.8344065481228213,
-            "timestamp": 0.8005690344947346
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 8.249839679439821e-43,
-            "angularVelocity": 9.431229878048832e-39,
-            "velocityX": 1.012759498983901e-41,
-            "velocityY": 5.142787784847508e-39,
-            "timestamp": 0.9006401638065764
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.1.traj
deleted file mode 100644
index 1862a6fc..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.1.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -1.6773104267984464e-40,
-            "angularVelocity": -1.9855220924111835e-39,
-            "velocityX": -5.3948128499400994e-40,
-            "velocityY": -2.7550648847397363e-40,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 4.043499994424153,
-            "heading": -2.181197470484939e-22,
-            "angularVelocity": -2.1796471324426728e-21,
-            "velocityX": 2.2751291494664054e-33,
-            "velocityY": -0.8344065481228213,
-            "timestamp": 0.10007112931184182
-        },
-        {
-            "x": 7.68,
-            "y": 3.8764999863701526,
-            "heading": -1.0976856482379068e-21,
-            "angularVelocity": -8.789407177499062e-21,
-            "velocityX": 3.967849397509383e-33,
-            "velocityY": -1.668813065290736,
-            "timestamp": 0.20014225862368365
-        },
-        {
-            "x": 7.68,
-            "y": 3.6259999820333837,
-            "heading": -3.357689035831156e-21,
-            "angularVelocity": -2.2583970074080117e-20,
-            "velocityX": 6.1318806903820435e-33,
-            "velocityY": -2.503219520548835,
-            "timestamp": 0.3002133879355255
-        },
-        {
-            "x": 7.68,
-            "y": 3.2919999999999994,
-            "heading": -1.0603568936345678e-20,
-            "angularVelocity": -7.2407296109284e-20,
-            "velocityX": 8.195284274358776e-33,
-            "velocityY": -3.3376257900775017,
-            "timestamp": 0.4002845172473673
-        },
-        {
-            "x": 7.68,
-            "y": 2.9580000179666155,
-            "heading": -4.785144304302103e-21,
-            "angularVelocity": 5.814288954429956e-20,
-            "velocityX": 7.586727405106972e-33,
-            "velocityY": -3.3376257900775013,
-            "timestamp": 0.5003556465592092
-        },
-        {
-            "x": 7.68,
-            "y": 2.7075000136298466,
-            "heading": -2.6182090767908737e-21,
-            "angularVelocity": 2.165395003165498e-20,
-            "velocityX": 5.326134827450793e-33,
-            "velocityY": -2.503219520548835,
-            "timestamp": 0.600426775871051
-        },
-        {
-            "x": 7.68,
-            "y": 2.540500005575846,
-            "heading": -9.668053802454109e-22,
-            "angularVelocity": 1.6502299039719674e-20,
-            "velocityX": 3.4348416075821124e-33,
-            "velocityY": -1.6688130652907358,
-            "timestamp": 0.7004979051828928
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 1.8181769494337196e-40,
-            "angularVelocity": 9.661181877631647e-21,
-            "velocityX": 1.7280403247932335e-33,
-            "velocityY": -0.8344065481228213,
-            "timestamp": 0.8005690344947346
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -1.6883012335149235e-41,
-            "angularVelocity": -1.9855220810412424e-39,
-            "velocityX": -1.2101303998889038e-42,
-            "velocityY": -7.163168700323315e-39,
-            "timestamp": 0.9006401638065764
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.traj
deleted file mode 100644
index 1862a6fc..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -1.6773104267984464e-40,
-            "angularVelocity": -1.9855220924111835e-39,
-            "velocityX": -5.3948128499400994e-40,
-            "velocityY": -2.7550648847397363e-40,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 4.043499994424153,
-            "heading": -2.181197470484939e-22,
-            "angularVelocity": -2.1796471324426728e-21,
-            "velocityX": 2.2751291494664054e-33,
-            "velocityY": -0.8344065481228213,
-            "timestamp": 0.10007112931184182
-        },
-        {
-            "x": 7.68,
-            "y": 3.8764999863701526,
-            "heading": -1.0976856482379068e-21,
-            "angularVelocity": -8.789407177499062e-21,
-            "velocityX": 3.967849397509383e-33,
-            "velocityY": -1.668813065290736,
-            "timestamp": 0.20014225862368365
-        },
-        {
-            "x": 7.68,
-            "y": 3.6259999820333837,
-            "heading": -3.357689035831156e-21,
-            "angularVelocity": -2.2583970074080117e-20,
-            "velocityX": 6.1318806903820435e-33,
-            "velocityY": -2.503219520548835,
-            "timestamp": 0.3002133879355255
-        },
-        {
-            "x": 7.68,
-            "y": 3.2919999999999994,
-            "heading": -1.0603568936345678e-20,
-            "angularVelocity": -7.2407296109284e-20,
-            "velocityX": 8.195284274358776e-33,
-            "velocityY": -3.3376257900775017,
-            "timestamp": 0.4002845172473673
-        },
-        {
-            "x": 7.68,
-            "y": 2.9580000179666155,
-            "heading": -4.785144304302103e-21,
-            "angularVelocity": 5.814288954429956e-20,
-            "velocityX": 7.586727405106972e-33,
-            "velocityY": -3.3376257900775013,
-            "timestamp": 0.5003556465592092
-        },
-        {
-            "x": 7.68,
-            "y": 2.7075000136298466,
-            "heading": -2.6182090767908737e-21,
-            "angularVelocity": 2.165395003165498e-20,
-            "velocityX": 5.326134827450793e-33,
-            "velocityY": -2.503219520548835,
-            "timestamp": 0.600426775871051
-        },
-        {
-            "x": 7.68,
-            "y": 2.540500005575846,
-            "heading": -9.668053802454109e-22,
-            "angularVelocity": 1.6502299039719674e-20,
-            "velocityX": 3.4348416075821124e-33,
-            "velocityY": -1.6688130652907358,
-            "timestamp": 0.7004979051828928
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 1.8181769494337196e-40,
-            "angularVelocity": 9.661181877631647e-21,
-            "velocityX": 1.7280403247932335e-33,
-            "velocityY": -0.8344065481228213,
-            "timestamp": 0.8005690344947346
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -1.6883012335149235e-41,
-            "angularVelocity": -1.9855220810412424e-39,
-            "velocityX": -1.2101303998889038e-42,
-            "velocityY": -7.163168700323315e-39,
-            "timestamp": 0.9006401638065764
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.1.traj
deleted file mode 100644
index 9c555123..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.1.traj	
+++ /dev/null
@@ -1,139 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -1.1842498081533988e-39,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 4.050554627994461,
-            "heading": 1.1070575354189048e-18,
-            "angularVelocity": 1.156189558439314e-17,
-            "velocityX": 1.1339790734570631e-33,
-            "velocityY": -0.7983807350946678,
-            "timestamp": 0.09575052183150978
-        },
-        {
-            "x": 7.68,
-            "y": 3.897663887017311,
-            "heading": 3.320268139408775e-18,
-            "angularVelocity": 2.3114345091427606e-17,
-            "velocityX": 5.50963145740529e-34,
-            "velocityY": -1.596761438503583,
-            "timestamp": 0.19150104366301957
-        },
-        {
-            "x": 7.68,
-            "y": 3.6683277831364007,
-            "heading": 6.638014774282681e-18,
-            "angularVelocity": 3.4649906568297676e-17,
-            "velocityX": 4.767145470149766e-34,
-            "velocityY": -2.3951420785410233,
-            "timestamp": 0.28725156549452935
-        },
-        {
-            "x": 7.68,
-            "y": 3.362546334555254,
-            "heading": 1.1056344671920077e-17,
-            "angularVelocity": 4.614418609717799e-17,
-            "velocityX": -5.447572473811757e-34,
-            "velocityY": -3.19352252846437,
-            "timestamp": 0.38300208732603913
-        },
-        {
-            "x": 7.68,
-            "y": 3.0003278007331793,
-            "heading": 2.1759975564527066e-17,
-            "angularVelocity": 1.11786658577609e-16,
-            "velocityX": -1.9220305893434716e-33,
-            "velocityY": -3.7829405719528495,
-            "timestamp": 0.4787526091575489
-        },
-        {
-            "x": 7.68,
-            "y": 2.63810926691106,
-            "heading": -1.7246492437320545e-17,
-            "angularVelocity": -4.0737603572006183e-16,
-            "velocityX": 5.877602560652089e-33,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.5745031309890587
-        },
-        {
-            "x": 7.68,
-            "y": 2.27589073308894,
-            "heading": -9.722837303851897e-18,
-            "angularVelocity": 7.857560449339153e-17,
-            "velocityX": 3.350655179860002e-33,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.6702536528205685
-        },
-        {
-            "x": 7.68,
-            "y": 1.9136721992668206,
-            "heading": -4.541786403704224e-17,
-            "angularVelocity": -3.727919811862982e-16,
-            "velocityX": 1.4553646654154268e-32,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.7660041746520783
-        },
-        {
-            "x": 7.68,
-            "y": 1.5514536654447455,
-            "heading": -1.11893051502424e-17,
-            "angularVelocity": 3.5747647356983727e-16,
-            "velocityX": -5.35720215261278e-33,
-            "velocityY": -3.7829405719528495,
-            "timestamp": 0.861754696483588
-        },
-        {
-            "x": 7.68,
-            "y": 1.2456722168635992,
-            "heading": -6.7032993083141955e-18,
-            "angularVelocity": 4.6850980602319425e-17,
-            "velocityX": -4.3527390380082413e-33,
-            "velocityY": -3.19352252846437,
-            "timestamp": 0.9575052183150978
-        },
-        {
-            "x": 7.68,
-            "y": 1.016336112982689,
-            "heading": -3.348658686079055e-18,
-            "angularVelocity": 3.5035220254491646e-17,
-            "velocityX": -6.634490282826358e-33,
-            "velocityY": -2.395142078541024,
-            "timestamp": 1.0532557401466076
-        },
-        {
-            "x": 7.68,
-            "y": 0.8634453720055388,
-            "heading": -1.115588951241814e-18,
-            "angularVelocity": 2.3321750022200153e-17,
-            "velocityX": -4.820835446954302e-33,
-            "velocityY": -1.596761438503583,
-            "timestamp": 1.1490062619781174
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 1.1650996045214628e-17,
-            "velocityX": -3.709395222464697e-33,
-            "velocityY": -0.7983807350946678,
-            "timestamp": 1.2447567838096272
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 6.978360472267901e-43,
-            "velocityY": -3.6401965239950315e-38,
-            "timestamp": 1.340507305641137
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.traj
deleted file mode 100644
index 9c555123..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.traj	
+++ /dev/null
@@ -1,139 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -1.1842498081533988e-39,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 4.050554627994461,
-            "heading": 1.1070575354189048e-18,
-            "angularVelocity": 1.156189558439314e-17,
-            "velocityX": 1.1339790734570631e-33,
-            "velocityY": -0.7983807350946678,
-            "timestamp": 0.09575052183150978
-        },
-        {
-            "x": 7.68,
-            "y": 3.897663887017311,
-            "heading": 3.320268139408775e-18,
-            "angularVelocity": 2.3114345091427606e-17,
-            "velocityX": 5.50963145740529e-34,
-            "velocityY": -1.596761438503583,
-            "timestamp": 0.19150104366301957
-        },
-        {
-            "x": 7.68,
-            "y": 3.6683277831364007,
-            "heading": 6.638014774282681e-18,
-            "angularVelocity": 3.4649906568297676e-17,
-            "velocityX": 4.767145470149766e-34,
-            "velocityY": -2.3951420785410233,
-            "timestamp": 0.28725156549452935
-        },
-        {
-            "x": 7.68,
-            "y": 3.362546334555254,
-            "heading": 1.1056344671920077e-17,
-            "angularVelocity": 4.614418609717799e-17,
-            "velocityX": -5.447572473811757e-34,
-            "velocityY": -3.19352252846437,
-            "timestamp": 0.38300208732603913
-        },
-        {
-            "x": 7.68,
-            "y": 3.0003278007331793,
-            "heading": 2.1759975564527066e-17,
-            "angularVelocity": 1.11786658577609e-16,
-            "velocityX": -1.9220305893434716e-33,
-            "velocityY": -3.7829405719528495,
-            "timestamp": 0.4787526091575489
-        },
-        {
-            "x": 7.68,
-            "y": 2.63810926691106,
-            "heading": -1.7246492437320545e-17,
-            "angularVelocity": -4.0737603572006183e-16,
-            "velocityX": 5.877602560652089e-33,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.5745031309890587
-        },
-        {
-            "x": 7.68,
-            "y": 2.27589073308894,
-            "heading": -9.722837303851897e-18,
-            "angularVelocity": 7.857560449339153e-17,
-            "velocityX": 3.350655179860002e-33,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.6702536528205685
-        },
-        {
-            "x": 7.68,
-            "y": 1.9136721992668206,
-            "heading": -4.541786403704224e-17,
-            "angularVelocity": -3.727919811862982e-16,
-            "velocityX": 1.4553646654154268e-32,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.7660041746520783
-        },
-        {
-            "x": 7.68,
-            "y": 1.5514536654447455,
-            "heading": -1.11893051502424e-17,
-            "angularVelocity": 3.5747647356983727e-16,
-            "velocityX": -5.35720215261278e-33,
-            "velocityY": -3.7829405719528495,
-            "timestamp": 0.861754696483588
-        },
-        {
-            "x": 7.68,
-            "y": 1.2456722168635992,
-            "heading": -6.7032993083141955e-18,
-            "angularVelocity": 4.6850980602319425e-17,
-            "velocityX": -4.3527390380082413e-33,
-            "velocityY": -3.19352252846437,
-            "timestamp": 0.9575052183150978
-        },
-        {
-            "x": 7.68,
-            "y": 1.016336112982689,
-            "heading": -3.348658686079055e-18,
-            "angularVelocity": 3.5035220254491646e-17,
-            "velocityX": -6.634490282826358e-33,
-            "velocityY": -2.395142078541024,
-            "timestamp": 1.0532557401466076
-        },
-        {
-            "x": 7.68,
-            "y": 0.8634453720055388,
-            "heading": -1.115588951241814e-18,
-            "angularVelocity": 2.3321750022200153e-17,
-            "velocityX": -4.820835446954302e-33,
-            "velocityY": -1.596761438503583,
-            "timestamp": 1.1490062619781174
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 1.1650996045214628e-17,
-            "velocityX": -3.709395222464697e-33,
-            "velocityY": -0.7983807350946678,
-            "timestamp": 1.2447567838096272
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 6.978360472267901e-43,
-            "velocityY": -3.6401965239950315e-38,
-            "timestamp": 1.340507305641137
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.1.traj
deleted file mode 100644
index e393ae58..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.1.traj	
+++ /dev/null
@@ -1,157 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -7.709535672280055e-33,
-            "angularVelocity": 3.0776034223095e-32,
-            "velocityX": -2.0787724059290825e-32,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.653625318689537,
-            "y": 4.193887618013532,
-            "heading": -0.05699658004084171,
-            "angularVelocity": -0.6065329552716214,
-            "velocityX": -0.28066795214939116,
-            "velocityY": 0.7117891037630026,
-            "timestamp": 0.0939711182145365
-        },
-        {
-            "x": 7.600609429037226,
-            "y": 4.328102182971276,
-            "heading": -0.1617251039431606,
-            "angularVelocity": -1.1144756590341216,
-            "velocityX": -0.5641721697008566,
-            "velocityY": 1.4282533560080801,
-            "timestamp": 0.187942236429073
-        },
-        {
-            "x": 7.520673672603614,
-            "y": 4.5302373715153195,
-            "heading": -0.299017155202882,
-            "angularVelocity": -1.4610026343017766,
-            "velocityX": -0.8506417498525306,
-            "velocityY": 2.151035258328725,
-            "timestamp": 0.2819133546436095
-        },
-        {
-            "x": 7.413680530437971,
-            "y": 4.80084724043498,
-            "heading": -0.43363333911195256,
-            "angularVelocity": -1.4325272111984575,
-            "velocityX": -1.1385747472045327,
-            "velocityY": 2.8797131933862543,
-            "timestamp": 0.375884472858146
-        },
-        {
-            "x": 7.279380160390009,
-            "y": 5.1284214711267,
-            "heading": -0.4371879316114584,
-            "angularVelocity": -0.037826436111898676,
-            "velocityX": -1.4291664566697349,
-            "velocityY": 3.4859032957750506,
-            "timestamp": 0.46985559107268254
-        },
-        {
-            "x": 7.147763767829931,
-            "y": 5.458645997615663,
-            "heading": -0.43718797458387054,
-            "angularVelocity": -4.5729382513446123e-7,
-            "velocityX": -1.4006047289934083,
-            "velocityY": 3.514106597466034,
-            "timestamp": 0.563826709287219
-        },
-        {
-            "x": 6.993957305230812,
-            "y": 5.779137314807096,
-            "heading": -0.43718801851214967,
-            "angularVelocity": -4.6746574877776706e-7,
-            "velocityX": -1.6367418577267254,
-            "velocityY": 3.410529993478958,
-            "timestamp": 0.6577978275017555
-        },
-        {
-            "x": 6.777425503833482,
-            "y": 6.0610683140345785,
-            "heading": -0.43718807107725144,
-            "angularVelocity": -5.593750798607861e-7,
-            "velocityX": -2.3042377861566687,
-            "velocityY": 3.000187766030755,
-            "timestamp": 0.751768945716292
-        },
-        {
-            "x": 6.507457353311083,
-            "y": 6.292343710685516,
-            "heading": -0.4371880918313372,
-            "angularVelocity": -2.2085600557820128e-7,
-            "velocityX": -2.87288430372895,
-            "velocityY": 2.4611327506280603,
-            "timestamp": 0.8457400639308286
-        },
-        {
-            "x": 6.1957042493791805,
-            "y": 6.462993619393473,
-            "heading": -0.4369141825473297,
-            "angularVelocity": 0.0029148241418374306,
-            "velocityX": -3.3175417070186306,
-            "velocityY": 1.8159825268692036,
-            "timestamp": 0.9397111821453651
-        },
-        {
-            "x": 5.885878112264826,
-            "y": 6.564806359719212,
-            "heading": -0.3410861416211598,
-            "angularVelocity": 1.0197605684269313,
-            "velocityX": -3.297035759508787,
-            "velocityY": 1.083447151211916,
-            "timestamp": 1.0336823003599016
-        },
-        {
-            "x": 5.602663899704114,
-            "y": 6.605182844765102,
-            "heading": -0.13545254769192583,
-            "angularVelocity": 2.1882637754695176,
-            "velocityX": -3.0138431673669435,
-            "velocityY": 0.4296690920896744,
-            "timestamp": 1.127653418574438
-        },
-        {
-            "x": 5.388992479168701,
-            "y": 6.621862905384601,
-            "heading": 0.0527880784098822,
-            "angularVelocity": 2.0031753338515554,
-            "velocityX": -2.2737988500636965,
-            "velocityY": 0.17750199142483714,
-            "timestamp": 1.2216245367889744
-        },
-        {
-            "x": 5.246852509457665,
-            "y": 6.628278045699216,
-            "heading": 0.19017469868317557,
-            "angularVelocity": 1.4620089968455965,
-            "velocityX": -1.5125920858633375,
-            "velocityY": 0.06826714884852915,
-            "timestamp": 1.3155956550035108
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": 0.7749753615846516,
-            "velocityX": -0.7539817638001085,
-            "velocityY": 0.01832429296896703,
-            "timestamp": 1.4095667732180472
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": 6.971645129510918e-32,
-            "velocityX": 3.634932162806737e-32,
-            "velocityY": 0,
-            "timestamp": 1.5035378914325837
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.traj
deleted file mode 100644
index e393ae58..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.traj	
+++ /dev/null
@@ -1,157 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -7.709535672280055e-33,
-            "angularVelocity": 3.0776034223095e-32,
-            "velocityX": -2.0787724059290825e-32,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.653625318689537,
-            "y": 4.193887618013532,
-            "heading": -0.05699658004084171,
-            "angularVelocity": -0.6065329552716214,
-            "velocityX": -0.28066795214939116,
-            "velocityY": 0.7117891037630026,
-            "timestamp": 0.0939711182145365
-        },
-        {
-            "x": 7.600609429037226,
-            "y": 4.328102182971276,
-            "heading": -0.1617251039431606,
-            "angularVelocity": -1.1144756590341216,
-            "velocityX": -0.5641721697008566,
-            "velocityY": 1.4282533560080801,
-            "timestamp": 0.187942236429073
-        },
-        {
-            "x": 7.520673672603614,
-            "y": 4.5302373715153195,
-            "heading": -0.299017155202882,
-            "angularVelocity": -1.4610026343017766,
-            "velocityX": -0.8506417498525306,
-            "velocityY": 2.151035258328725,
-            "timestamp": 0.2819133546436095
-        },
-        {
-            "x": 7.413680530437971,
-            "y": 4.80084724043498,
-            "heading": -0.43363333911195256,
-            "angularVelocity": -1.4325272111984575,
-            "velocityX": -1.1385747472045327,
-            "velocityY": 2.8797131933862543,
-            "timestamp": 0.375884472858146
-        },
-        {
-            "x": 7.279380160390009,
-            "y": 5.1284214711267,
-            "heading": -0.4371879316114584,
-            "angularVelocity": -0.037826436111898676,
-            "velocityX": -1.4291664566697349,
-            "velocityY": 3.4859032957750506,
-            "timestamp": 0.46985559107268254
-        },
-        {
-            "x": 7.147763767829931,
-            "y": 5.458645997615663,
-            "heading": -0.43718797458387054,
-            "angularVelocity": -4.5729382513446123e-7,
-            "velocityX": -1.4006047289934083,
-            "velocityY": 3.514106597466034,
-            "timestamp": 0.563826709287219
-        },
-        {
-            "x": 6.993957305230812,
-            "y": 5.779137314807096,
-            "heading": -0.43718801851214967,
-            "angularVelocity": -4.6746574877776706e-7,
-            "velocityX": -1.6367418577267254,
-            "velocityY": 3.410529993478958,
-            "timestamp": 0.6577978275017555
-        },
-        {
-            "x": 6.777425503833482,
-            "y": 6.0610683140345785,
-            "heading": -0.43718807107725144,
-            "angularVelocity": -5.593750798607861e-7,
-            "velocityX": -2.3042377861566687,
-            "velocityY": 3.000187766030755,
-            "timestamp": 0.751768945716292
-        },
-        {
-            "x": 6.507457353311083,
-            "y": 6.292343710685516,
-            "heading": -0.4371880918313372,
-            "angularVelocity": -2.2085600557820128e-7,
-            "velocityX": -2.87288430372895,
-            "velocityY": 2.4611327506280603,
-            "timestamp": 0.8457400639308286
-        },
-        {
-            "x": 6.1957042493791805,
-            "y": 6.462993619393473,
-            "heading": -0.4369141825473297,
-            "angularVelocity": 0.0029148241418374306,
-            "velocityX": -3.3175417070186306,
-            "velocityY": 1.8159825268692036,
-            "timestamp": 0.9397111821453651
-        },
-        {
-            "x": 5.885878112264826,
-            "y": 6.564806359719212,
-            "heading": -0.3410861416211598,
-            "angularVelocity": 1.0197605684269313,
-            "velocityX": -3.297035759508787,
-            "velocityY": 1.083447151211916,
-            "timestamp": 1.0336823003599016
-        },
-        {
-            "x": 5.602663899704114,
-            "y": 6.605182844765102,
-            "heading": -0.13545254769192583,
-            "angularVelocity": 2.1882637754695176,
-            "velocityX": -3.0138431673669435,
-            "velocityY": 0.4296690920896744,
-            "timestamp": 1.127653418574438
-        },
-        {
-            "x": 5.388992479168701,
-            "y": 6.621862905384601,
-            "heading": 0.0527880784098822,
-            "angularVelocity": 2.0031753338515554,
-            "velocityX": -2.2737988500636965,
-            "velocityY": 0.17750199142483714,
-            "timestamp": 1.2216245367889744
-        },
-        {
-            "x": 5.246852509457665,
-            "y": 6.628278045699216,
-            "heading": 0.19017469868317557,
-            "angularVelocity": 1.4620089968455965,
-            "velocityX": -1.5125920858633375,
-            "velocityY": 0.06826714884852915,
-            "timestamp": 1.3155956550035108
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": 0.7749753615846516,
-            "velocityX": -0.7539817638001085,
-            "velocityY": 0.01832429296896703,
-            "timestamp": 1.4095667732180472
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": 6.971645129510918e-32,
-            "velocityX": 3.634932162806737e-32,
-            "velocityY": 0,
-            "timestamp": 1.5035378914325837
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S2.1.traj
deleted file mode 100644
index 67cbd06a..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S2.1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 1.358270377512338e-32,
-            "angularVelocity": -4.2989327725546786e-32,
-            "velocityX": 9.28706448380456e-34,
-            "velocityY": 4.1113328098198935e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 7.585313636391529,
-            "y": 4.127746947940637,
-            "heading": -0.08924586799649764,
-            "angularVelocity": -0.8241960155421162,
-            "velocityX": -0.8744396280098535,
-            "velocityY": 0.006898151480975746,
-            "timestamp": 0.10828233370892486
-        },
-        {
-            "x": 7.394793136195202,
-            "y": 4.129317895600586,
-            "heading": -0.2483761167773939,
-            "angularVelocity": -1.4695864351119023,
-            "velocityX": -1.7594790735529262,
-            "velocityY": 0.014507885138232623,
-            "timestamp": 0.21656466741784972
-        },
-        {
-            "x": 7.106938899563738,
-            "y": 4.131715387581193,
-            "heading": -0.440143741107867,
-            "angularVelocity": -1.7709964106053178,
-            "velocityX": -2.6583674988502586,
-            "velocityY": 0.022141118486160887,
-            "timestamp": 0.32484700112677456
-        },
-        {
-            "x": 6.726884682874397,
-            "y": 4.136782702937484,
-            "heading": -0.5152103460374668,
-            "angularVelocity": -0.6932488648738155,
-            "velocityX": -3.509845084341914,
-            "velocityY": 0.04679724921622947,
-            "timestamp": 0.43312933483569943
-        },
-        {
-            "x": 6.317264615210477,
-            "y": 4.138917325587368,
-            "heading": -0.515210397277194,
-            "angularVelocity": -4.7320486478177106e-7,
-            "velocityX": -3.782889171607859,
-            "velocityY": 0.019713489511795114,
-            "timestamp": 0.5414116685446243
-        },
-        {
-            "x": 5.907644548289307,
-            "y": 4.14105209076814,
-            "heading": -0.5152104485168384,
-            "angularVelocity": -4.732041015646636e-7,
-            "velocityX": -3.7828891647484775,
-            "velocityY": 0.019714805801191258,
-            "timestamp": 0.6496940022535491
-        },
-        {
-            "x": 5.500258277815471,
-            "y": 4.183825709430579,
-            "heading": -0.5152105024899405,
-            "angularVelocity": -4.984479029228616e-7,
-            "velocityX": -3.762259793632967,
-            "velocityY": 0.3950193646307994,
-            "timestamp": 0.7579763359624739
-        },
-        {
-            "x": 5.114610765355458,
-            "y": 4.3219164843017905,
-            "heading": -0.5152109227554116,
-            "angularVelocity": -0.000003881200715936794,
-            "velocityX": -3.5614998241234512,
-            "velocityY": 1.2752844359857862,
-            "timestamp": 0.8662586696713988
-        },
-        {
-            "x": 4.790273412388094,
-            "y": 4.533322369220829,
-            "heading": -0.5901314422767667,
-            "angularVelocity": -0.6918997490647923,
-            "velocityX": -2.9952933397170605,
-            "velocityY": 1.952358041038544,
-            "timestamp": 0.9745410033803236
-        },
-        {
-            "x": 4.547771251602889,
-            "y": 4.790487049727204,
-            "heading": -0.7662492650374113,
-            "angularVelocity": -1.6264686650923976,
-            "velocityX": -2.239535780943542,
-            "velocityY": 2.3749458632620883,
-            "timestamp": 1.0828233370892484
-        },
-        {
-            "x": 4.394005608234175,
-            "y": 5.057835455573767,
-            "heading": -0.8024780004938983,
-            "angularVelocity": -0.3345766037318139,
-            "velocityX": -1.4200436775037824,
-            "velocityY": 2.468993756315091,
-            "timestamp": 1.1911056707981733
-        },
-        {
-            "x": 4.299277084870805,
-            "y": 5.304155354811625,
-            "heading": -0.5153879845736841,
-            "angularVelocity": 2.651309831315093,
-            "velocityX": -0.8748289782709269,
-            "velocityY": 2.2747930414945974,
-            "timestamp": 1.2993880045070982
-        },
-        {
-            "x": 4.268346904630433,
-            "y": 5.476762087961931,
-            "heading": -0.20205360473699635,
-            "angularVelocity": 2.893679597624539,
-            "velocityX": -0.2856438273995415,
-            "velocityY": 1.5940433424190128,
-            "timestamp": 1.4076703382160232
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 2.864290108945775e-30,
-            "angularVelocity": 1.8659886411401074,
-            "velocityX": -0.04937928882107947,
-            "velocityY": 0.7687118404911842,
-            "timestamp": 1.515952671924948
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -1.339793214188515e-31,
-            "velocityX": -1.755749143439243e-31,
-            "velocityY": 0,
-            "timestamp": 1.624235005633873
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S2.traj
deleted file mode 100644
index 67cbd06a..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S2.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 1.358270377512338e-32,
-            "angularVelocity": -4.2989327725546786e-32,
-            "velocityX": 9.28706448380456e-34,
-            "velocityY": 4.1113328098198935e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 7.585313636391529,
-            "y": 4.127746947940637,
-            "heading": -0.08924586799649764,
-            "angularVelocity": -0.8241960155421162,
-            "velocityX": -0.8744396280098535,
-            "velocityY": 0.006898151480975746,
-            "timestamp": 0.10828233370892486
-        },
-        {
-            "x": 7.394793136195202,
-            "y": 4.129317895600586,
-            "heading": -0.2483761167773939,
-            "angularVelocity": -1.4695864351119023,
-            "velocityX": -1.7594790735529262,
-            "velocityY": 0.014507885138232623,
-            "timestamp": 0.21656466741784972
-        },
-        {
-            "x": 7.106938899563738,
-            "y": 4.131715387581193,
-            "heading": -0.440143741107867,
-            "angularVelocity": -1.7709964106053178,
-            "velocityX": -2.6583674988502586,
-            "velocityY": 0.022141118486160887,
-            "timestamp": 0.32484700112677456
-        },
-        {
-            "x": 6.726884682874397,
-            "y": 4.136782702937484,
-            "heading": -0.5152103460374668,
-            "angularVelocity": -0.6932488648738155,
-            "velocityX": -3.509845084341914,
-            "velocityY": 0.04679724921622947,
-            "timestamp": 0.43312933483569943
-        },
-        {
-            "x": 6.317264615210477,
-            "y": 4.138917325587368,
-            "heading": -0.515210397277194,
-            "angularVelocity": -4.7320486478177106e-7,
-            "velocityX": -3.782889171607859,
-            "velocityY": 0.019713489511795114,
-            "timestamp": 0.5414116685446243
-        },
-        {
-            "x": 5.907644548289307,
-            "y": 4.14105209076814,
-            "heading": -0.5152104485168384,
-            "angularVelocity": -4.732041015646636e-7,
-            "velocityX": -3.7828891647484775,
-            "velocityY": 0.019714805801191258,
-            "timestamp": 0.6496940022535491
-        },
-        {
-            "x": 5.500258277815471,
-            "y": 4.183825709430579,
-            "heading": -0.5152105024899405,
-            "angularVelocity": -4.984479029228616e-7,
-            "velocityX": -3.762259793632967,
-            "velocityY": 0.3950193646307994,
-            "timestamp": 0.7579763359624739
-        },
-        {
-            "x": 5.114610765355458,
-            "y": 4.3219164843017905,
-            "heading": -0.5152109227554116,
-            "angularVelocity": -0.000003881200715936794,
-            "velocityX": -3.5614998241234512,
-            "velocityY": 1.2752844359857862,
-            "timestamp": 0.8662586696713988
-        },
-        {
-            "x": 4.790273412388094,
-            "y": 4.533322369220829,
-            "heading": -0.5901314422767667,
-            "angularVelocity": -0.6918997490647923,
-            "velocityX": -2.9952933397170605,
-            "velocityY": 1.952358041038544,
-            "timestamp": 0.9745410033803236
-        },
-        {
-            "x": 4.547771251602889,
-            "y": 4.790487049727204,
-            "heading": -0.7662492650374113,
-            "angularVelocity": -1.6264686650923976,
-            "velocityX": -2.239535780943542,
-            "velocityY": 2.3749458632620883,
-            "timestamp": 1.0828233370892484
-        },
-        {
-            "x": 4.394005608234175,
-            "y": 5.057835455573767,
-            "heading": -0.8024780004938983,
-            "angularVelocity": -0.3345766037318139,
-            "velocityX": -1.4200436775037824,
-            "velocityY": 2.468993756315091,
-            "timestamp": 1.1911056707981733
-        },
-        {
-            "x": 4.299277084870805,
-            "y": 5.304155354811625,
-            "heading": -0.5153879845736841,
-            "angularVelocity": 2.651309831315093,
-            "velocityX": -0.8748289782709269,
-            "velocityY": 2.2747930414945974,
-            "timestamp": 1.2993880045070982
-        },
-        {
-            "x": 4.268346904630433,
-            "y": 5.476762087961931,
-            "heading": -0.20205360473699635,
-            "angularVelocity": 2.893679597624539,
-            "velocityX": -0.2856438273995415,
-            "velocityY": 1.5940433424190128,
-            "timestamp": 1.4076703382160232
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 2.864290108945775e-30,
-            "angularVelocity": 1.8659886411401074,
-            "velocityX": -0.04937928882107947,
-            "velocityY": 0.7687118404911842,
-            "timestamp": 1.515952671924948
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -1.339793214188515e-31,
-            "velocityX": -1.755749143439243e-31,
-            "velocityY": 0,
-            "timestamp": 1.624235005633873
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S3.1.traj
deleted file mode 100644
index c2db4e34..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S3.1.traj	
+++ /dev/null
@@ -1,166 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 4.8594393424661863e-32,
-            "velocityX": 8.070887902586468e-34,
-            "velocityY": -1.5484627527783442e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.613090966537703,
-            "y": 4.130415229492922,
-            "heading": -0.02261774188189381,
-            "angularVelocity": -0.25176724339812206,
-            "velocityX": -0.7447915447554241,
-            "velocityY": 0.038016302406854975,
-            "timestamp": 0.08983591977315897
-        },
-        {
-            "x": 7.479193313628693,
-            "y": 4.137259099498979,
-            "heading": -0.06454119971556918,
-            "angularVelocity": -0.46666698504702897,
-            "velocityX": -1.4904689933291897,
-            "velocityY": 0.0761818883097586,
-            "timestamp": 0.17967183954631794
-        },
-        {
-            "x": 7.278200146895047,
-            "y": 4.147551727639681,
-            "heading": -0.12012980810986089,
-            "angularVelocity": -0.6187793093216658,
-            "velocityX": -2.237336326507015,
-            "velocityY": 0.11457141160316474,
-            "timestamp": 0.2695077593194769
-        },
-        {
-            "x": 7.010006404514947,
-            "y": 4.161329433657577,
-            "heading": -0.1775477987284744,
-            "angularVelocity": -0.6391429036434741,
-            "velocityX": -2.985373145364283,
-            "velocityY": 0.15336522465649807,
-            "timestamp": 0.35934367909263587
-        },
-        {
-            "x": 6.675632733992502,
-            "y": 4.178685588756415,
-            "heading": -0.19285230682653456,
-            "angularVelocity": -0.17036067687364617,
-            "velocityX": -3.7220487235639528,
-            "velocityY": 0.19319839038333309,
-            "timestamp": 0.44917959886579484
-        },
-        {
-            "x": 6.3362226416697665,
-            "y": 4.195852203162663,
-            "heading": -0.19285232652882817,
-            "angularVelocity": -2.193200640663355e-7,
-            "velocityX": -3.7781111740035236,
-            "velocityY": 0.1910885362012055,
-            "timestamp": 0.5390155186389538
-        },
-        {
-            "x": 5.996812544590127,
-            "y": 4.213018723498308,
-            "heading": -0.19285234623213549,
-            "angularVelocity": -2.1933156264800868e-7,
-            "velocityX": -3.7781112269558137,
-            "velocityY": 0.1910874890356692,
-            "timestamp": 0.6288514384121128
-        },
-        {
-            "x": 5.65703115405788,
-            "y": 4.206499194085455,
-            "heading": -0.1928524148916534,
-            "angularVelocity": -7.642946895727943e-7,
-            "velocityX": -3.782244244720454,
-            "velocityY": -0.07257152185432766,
-            "timestamp": 0.7186873581852717
-        },
-        {
-            "x": 5.325195518902672,
-            "y": 4.133157682156235,
-            "heading": -0.192852601151229,
-            "angularVelocity": -0.0000020733763948999505,
-            "velocityX": -3.6937968241559735,
-            "velocityY": -0.8163940672507154,
-            "timestamp": 0.8085232779584307
-        },
-        {
-            "x": 5.022921580906399,
-            "y": 3.999719932132448,
-            "heading": -0.21619143682617745,
-            "angularVelocity": -0.2597940304978627,
-            "velocityX": -3.364733602738329,
-            "velocityY": -1.485349628067107,
-            "timestamp": 0.8983591977315897
-        },
-        {
-            "x": 4.76405537852928,
-            "y": 3.8156128777716756,
-            "heading": -0.2707600447886353,
-            "angularVelocity": -0.6074252715762222,
-            "velocityX": -2.881544520603651,
-            "velocityY": -2.0493701720293274,
-            "timestamp": 0.9881951175047486
-        },
-        {
-            "x": 4.5584097754694755,
-            "y": 3.590752248351908,
-            "heading": -0.3464948618413025,
-            "angularVelocity": -0.8430349156856024,
-            "velocityX": -2.2891244791432657,
-            "velocityY": -2.5030147182477354,
-            "timestamp": 1.0780310372779076
-        },
-        {
-            "x": 4.420003796304514,
-            "y": 3.3682412112408735,
-            "heading": -0.42679529224450924,
-            "angularVelocity": -0.8938566066393836,
-            "velocityX": -1.5406529984228516,
-            "velocityY": -2.476860454851527,
-            "timestamp": 1.1678669570510665
-        },
-        {
-            "x": 4.3340794956684014,
-            "y": 3.187695881574611,
-            "heading": -0.4942860320434666,
-            "angularVelocity": -0.7512667535241839,
-            "velocityX": -0.9564581834687845,
-            "velocityY": -2.009723172222817,
-            "timestamp": 1.2577028768242253
-        },
-        {
-            "x": 4.284833149556787,
-            "y": 3.063288277698013,
-            "heading": -0.5416494132844137,
-            "angularVelocity": -0.5272209752980105,
-            "velocityX": -0.5481810197605272,
-            "velocityY": -1.3848314147664058,
-            "timestamp": 1.3475387965973842
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -0.27105624093568903,
-            "velocityX": -0.24303362855860308,
-            "velocityY": -0.7044874461979944,
-            "timestamp": 1.437374716370543
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 6.221864662981848e-31,
-            "velocityX": 0,
-            "velocityY": 2.6923638006152617e-32,
-            "timestamp": 1.527210636143702
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S3.traj
deleted file mode 100644
index c2db4e34..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S3.traj	
+++ /dev/null
@@ -1,166 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 4.8594393424661863e-32,
-            "velocityX": 8.070887902586468e-34,
-            "velocityY": -1.5484627527783442e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.613090966537703,
-            "y": 4.130415229492922,
-            "heading": -0.02261774188189381,
-            "angularVelocity": -0.25176724339812206,
-            "velocityX": -0.7447915447554241,
-            "velocityY": 0.038016302406854975,
-            "timestamp": 0.08983591977315897
-        },
-        {
-            "x": 7.479193313628693,
-            "y": 4.137259099498979,
-            "heading": -0.06454119971556918,
-            "angularVelocity": -0.46666698504702897,
-            "velocityX": -1.4904689933291897,
-            "velocityY": 0.0761818883097586,
-            "timestamp": 0.17967183954631794
-        },
-        {
-            "x": 7.278200146895047,
-            "y": 4.147551727639681,
-            "heading": -0.12012980810986089,
-            "angularVelocity": -0.6187793093216658,
-            "velocityX": -2.237336326507015,
-            "velocityY": 0.11457141160316474,
-            "timestamp": 0.2695077593194769
-        },
-        {
-            "x": 7.010006404514947,
-            "y": 4.161329433657577,
-            "heading": -0.1775477987284744,
-            "angularVelocity": -0.6391429036434741,
-            "velocityX": -2.985373145364283,
-            "velocityY": 0.15336522465649807,
-            "timestamp": 0.35934367909263587
-        },
-        {
-            "x": 6.675632733992502,
-            "y": 4.178685588756415,
-            "heading": -0.19285230682653456,
-            "angularVelocity": -0.17036067687364617,
-            "velocityX": -3.7220487235639528,
-            "velocityY": 0.19319839038333309,
-            "timestamp": 0.44917959886579484
-        },
-        {
-            "x": 6.3362226416697665,
-            "y": 4.195852203162663,
-            "heading": -0.19285232652882817,
-            "angularVelocity": -2.193200640663355e-7,
-            "velocityX": -3.7781111740035236,
-            "velocityY": 0.1910885362012055,
-            "timestamp": 0.5390155186389538
-        },
-        {
-            "x": 5.996812544590127,
-            "y": 4.213018723498308,
-            "heading": -0.19285234623213549,
-            "angularVelocity": -2.1933156264800868e-7,
-            "velocityX": -3.7781112269558137,
-            "velocityY": 0.1910874890356692,
-            "timestamp": 0.6288514384121128
-        },
-        {
-            "x": 5.65703115405788,
-            "y": 4.206499194085455,
-            "heading": -0.1928524148916534,
-            "angularVelocity": -7.642946895727943e-7,
-            "velocityX": -3.782244244720454,
-            "velocityY": -0.07257152185432766,
-            "timestamp": 0.7186873581852717
-        },
-        {
-            "x": 5.325195518902672,
-            "y": 4.133157682156235,
-            "heading": -0.192852601151229,
-            "angularVelocity": -0.0000020733763948999505,
-            "velocityX": -3.6937968241559735,
-            "velocityY": -0.8163940672507154,
-            "timestamp": 0.8085232779584307
-        },
-        {
-            "x": 5.022921580906399,
-            "y": 3.999719932132448,
-            "heading": -0.21619143682617745,
-            "angularVelocity": -0.2597940304978627,
-            "velocityX": -3.364733602738329,
-            "velocityY": -1.485349628067107,
-            "timestamp": 0.8983591977315897
-        },
-        {
-            "x": 4.76405537852928,
-            "y": 3.8156128777716756,
-            "heading": -0.2707600447886353,
-            "angularVelocity": -0.6074252715762222,
-            "velocityX": -2.881544520603651,
-            "velocityY": -2.0493701720293274,
-            "timestamp": 0.9881951175047486
-        },
-        {
-            "x": 4.5584097754694755,
-            "y": 3.590752248351908,
-            "heading": -0.3464948618413025,
-            "angularVelocity": -0.8430349156856024,
-            "velocityX": -2.2891244791432657,
-            "velocityY": -2.5030147182477354,
-            "timestamp": 1.0780310372779076
-        },
-        {
-            "x": 4.420003796304514,
-            "y": 3.3682412112408735,
-            "heading": -0.42679529224450924,
-            "angularVelocity": -0.8938566066393836,
-            "velocityX": -1.5406529984228516,
-            "velocityY": -2.476860454851527,
-            "timestamp": 1.1678669570510665
-        },
-        {
-            "x": 4.3340794956684014,
-            "y": 3.187695881574611,
-            "heading": -0.4942860320434666,
-            "angularVelocity": -0.7512667535241839,
-            "velocityX": -0.9564581834687845,
-            "velocityY": -2.009723172222817,
-            "timestamp": 1.2577028768242253
-        },
-        {
-            "x": 4.284833149556787,
-            "y": 3.063288277698013,
-            "heading": -0.5416494132844137,
-            "angularVelocity": -0.5272209752980105,
-            "velocityX": -0.5481810197605272,
-            "velocityY": -1.3848314147664058,
-            "timestamp": 1.3475387965973842
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -0.27105624093568903,
-            "velocityX": -0.24303362855860308,
-            "velocityY": -0.7044874461979944,
-            "timestamp": 1.437374716370543
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 6.221864662981848e-31,
-            "velocityX": 0,
-            "velocityY": 2.6923638006152617e-32,
-            "timestamp": 1.527210636143702
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S4.1.traj
deleted file mode 100644
index 2274eedc..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S4.1.traj	
+++ /dev/null
@@ -1,238 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 3.185769476987751e-31,
-            "velocityX": 7.333541525795099e-32,
-            "velocityY": -1.3517842061565195e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 7.679078910576486,
-            "y": 4.080614551713116,
-            "heading": -0.023748331346269917,
-            "angularVelocity": -0.3167839884646853,
-            "velocityX": -0.012286605616975374,
-            "velocityY": -0.6187452541731726,
-            "timestamp": 0.07496695606800095
-        },
-        {
-            "x": 7.6772274783441,
-            "y": 3.987757014244699,
-            "heading": -0.06889026621284731,
-            "angularVelocity": -0.6021577670253255,
-            "velocityX": -0.024696644088144176,
-            "velocityY": -1.2386462294692595,
-            "timestamp": 0.1499339121360019
-        },
-        {
-            "x": 7.674435217742111,
-            "y": 3.848312298051719,
-            "heading": -0.1318457680879735,
-            "angularVelocity": -0.8397766853177616,
-            "velocityX": -0.037246551660047825,
-            "velocityY": -1.8600824083946148,
-            "timestamp": 0.22490086820400285
-        },
-        {
-            "x": 7.670687411814683,
-            "y": 3.6621317365631327,
-            "heading": -0.20653378464827088,
-            "angularVelocity": -0.996279167218362,
-            "velocityX": -0.049992771802326974,
-            "velocityY": -2.483501681990474,
-            "timestamp": 0.2998678242720038
-        },
-        {
-            "x": 7.665952301925262,
-            "y": 3.4291021726627307,
-            "heading": -0.27999342706200125,
-            "angularVelocity": -0.9798936260280143,
-            "velocityX": -0.06316262707958344,
-            "velocityY": -3.1084303821676644,
-            "timestamp": 0.3748347803400048
-        },
-        {
-            "x": 7.659724245559776,
-            "y": 3.1517739981457797,
-            "heading": -0.2973184560258276,
-            "angularVelocity": -0.23110220652567398,
-            "velocityX": -0.0830773542389579,
-            "velocityY": -3.699338869586643,
-            "timestamp": 0.44980173640800575
-        },
-        {
-            "x": 7.61848609595722,
-            "y": 2.8711927485616604,
-            "heading": -0.2973184850947423,
-            "angularVelocity": -3.8775636846908307e-7,
-            "velocityX": -0.5500843540339733,
-            "velocityY": -3.742732322352917,
-            "timestamp": 0.5247686924760067
-        },
-        {
-            "x": 7.5316071002697536,
-            "y": 2.6012326390018865,
-            "heading": -0.2973185190564447,
-            "angularVelocity": -4.530222940715824e-7,
-            "velocityX": -1.1588972027711093,
-            "velocityY": -3.6010547008857787,
-            "timestamp": 0.5997356485440076
-        },
-        {
-            "x": 7.401459139481823,
-            "y": 2.3492645367371554,
-            "heading": -0.29731856454052086,
-            "angularVelocity": -6.067216612944958e-7,
-            "velocityX": -1.7360710320141561,
-            "velocityY": -3.3610555300681284,
-            "timestamp": 0.6747026046120086
-        },
-        {
-            "x": 7.231595664022054,
-            "y": 2.1221680676798096,
-            "heading": -0.2973186386883636,
-            "angularVelocity": -9.89073675332688e-7,
-            "velocityX": -2.265844638345654,
-            "velocityY": -3.0292875817359826,
-            "timestamp": 0.7496695606800096
-        },
-        {
-            "x": 7.026654548839687,
-            "y": 1.9261437915792998,
-            "heading": -0.2973188151543491,
-            "angularVelocity": -0.000002353916908579506,
-            "velocityX": -2.7337526549226907,
-            "velocityY": -2.614809062311352,
-            "timestamp": 0.8246365167480105
-        },
-        {
-            "x": 6.792231559753418,
-            "y": 1.766543984413147,
-            "heading": -0.2973195709317279,
-            "angularVelocity": -0.00001008147347055336,
-            "velocityX": -3.127017573897922,
-            "velocityY": -2.1289354075064555,
-            "timestamp": 0.8996034728160115
-        },
-        {
-            "x": 6.610935711613446,
-            "y": 1.6724691195207,
-            "heading": -0.2973321063250889,
-            "angularVelocity": -0.00023216320074468077,
-            "velocityX": -3.3577106919370485,
-            "velocityY": -1.7423243992220112,
-            "timestamp": 0.9535973587619662
-        },
-        {
-            "x": 6.419882816475647,
-            "y": 1.600658398177247,
-            "heading": -0.29770902924967546,
-            "angularVelocity": -0.006980844552986395,
-            "velocityX": -3.538417207626563,
-            "velocityY": -1.329978757508455,
-            "timestamp": 1.007591244707921
-        },
-        {
-            "x": 6.223144239006287,
-            "y": 1.5524565272909876,
-            "heading": -0.30199019992690734,
-            "angularVelocity": -0.07928991592723345,
-            "velocityX": -3.6437195438439516,
-            "velocityY": -0.892728316211746,
-            "timestamp": 1.0615851306538757
-        },
-        {
-            "x": 6.027009837481886,
-            "y": 1.528287215105613,
-            "heading": -0.31920772077283077,
-            "angularVelocity": -0.31887908314582397,
-            "velocityX": -3.63252983348375,
-            "velocityY": -0.44763053745692566,
-            "timestamp": 1.1155790165998305
-        },
-        {
-            "x": 5.840489459577601,
-            "y": 1.5253276945335545,
-            "heading": -0.3622266817701244,
-            "angularVelocity": -0.7967376350788925,
-            "velocityX": -3.454472198777908,
-            "velocityY": -0.05481214252704887,
-            "timestamp": 1.1695729025457853
-        },
-        {
-            "x": 5.673855769295343,
-            "y": 1.5359985529910274,
-            "heading": -0.41631972448131044,
-            "angularVelocity": -1.0018364443214895,
-            "velocityX": -3.086158504113808,
-            "velocityY": 0.19763086635687102,
-            "timestamp": 1.22356678849174
-        },
-        {
-            "x": 5.530708201179271,
-            "y": 1.5529237053226916,
-            "heading": -0.47196047387840134,
-            "angularVelocity": -1.0305009247305976,
-            "velocityX": -2.651181066303626,
-            "velocityY": 0.3134642383140704,
-            "timestamp": 1.2775606744376948
-        },
-        {
-            "x": 5.411784437145291,
-            "y": 1.57153818466476,
-            "heading": -0.5236032237572267,
-            "angularVelocity": -0.956455512954414,
-            "velocityX": -2.2025413053807186,
-            "velocityY": 0.34475161429762524,
-            "timestamp": 1.3315545603836496
-        },
-        {
-            "x": 5.3170603467926,
-            "y": 1.5890782395606275,
-            "heading": -0.5679992830659408,
-            "angularVelocity": -0.8222423433861833,
-            "velocityX": -1.754348454332506,
-            "velocityY": 0.32485261226475975,
-            "timestamp": 1.3855484463296044
-        },
-        {
-            "x": 5.246334967071984,
-            "y": 1.6037618746917708,
-            "heading": -0.6030801100679523,
-            "angularVelocity": -0.6497185077054303,
-            "velocityX": -1.309877562644932,
-            "velocityY": 0.2719499601462664,
-            "timestamp": 1.4395423322755592
-        },
-        {
-            "x": 5.199384048260516,
-            "y": 1.6143645569232876,
-            "heading": -0.6274260701015184,
-            "angularVelocity": -0.45090216432919045,
-            "velocityX": -0.8695599138477248,
-            "velocityY": 0.19636820069091412,
-            "timestamp": 1.493536218221514
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -0.23287692075121041,
-            "velocityX": -0.4330869662524824,
-            "velocityY": 0.10437187429616973,
-            "timestamp": 1.5475301041674687
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0,
-            "velocityX": 3.9288115092123874e-33,
-            "velocityY": 2.0888895254257856e-32,
-            "timestamp": 1.6015239901134235
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S4.traj
deleted file mode 100644
index 2274eedc..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S4.traj	
+++ /dev/null
@@ -1,238 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 3.185769476987751e-31,
-            "velocityX": 7.333541525795099e-32,
-            "velocityY": -1.3517842061565195e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 7.679078910576486,
-            "y": 4.080614551713116,
-            "heading": -0.023748331346269917,
-            "angularVelocity": -0.3167839884646853,
-            "velocityX": -0.012286605616975374,
-            "velocityY": -0.6187452541731726,
-            "timestamp": 0.07496695606800095
-        },
-        {
-            "x": 7.6772274783441,
-            "y": 3.987757014244699,
-            "heading": -0.06889026621284731,
-            "angularVelocity": -0.6021577670253255,
-            "velocityX": -0.024696644088144176,
-            "velocityY": -1.2386462294692595,
-            "timestamp": 0.1499339121360019
-        },
-        {
-            "x": 7.674435217742111,
-            "y": 3.848312298051719,
-            "heading": -0.1318457680879735,
-            "angularVelocity": -0.8397766853177616,
-            "velocityX": -0.037246551660047825,
-            "velocityY": -1.8600824083946148,
-            "timestamp": 0.22490086820400285
-        },
-        {
-            "x": 7.670687411814683,
-            "y": 3.6621317365631327,
-            "heading": -0.20653378464827088,
-            "angularVelocity": -0.996279167218362,
-            "velocityX": -0.049992771802326974,
-            "velocityY": -2.483501681990474,
-            "timestamp": 0.2998678242720038
-        },
-        {
-            "x": 7.665952301925262,
-            "y": 3.4291021726627307,
-            "heading": -0.27999342706200125,
-            "angularVelocity": -0.9798936260280143,
-            "velocityX": -0.06316262707958344,
-            "velocityY": -3.1084303821676644,
-            "timestamp": 0.3748347803400048
-        },
-        {
-            "x": 7.659724245559776,
-            "y": 3.1517739981457797,
-            "heading": -0.2973184560258276,
-            "angularVelocity": -0.23110220652567398,
-            "velocityX": -0.0830773542389579,
-            "velocityY": -3.699338869586643,
-            "timestamp": 0.44980173640800575
-        },
-        {
-            "x": 7.61848609595722,
-            "y": 2.8711927485616604,
-            "heading": -0.2973184850947423,
-            "angularVelocity": -3.8775636846908307e-7,
-            "velocityX": -0.5500843540339733,
-            "velocityY": -3.742732322352917,
-            "timestamp": 0.5247686924760067
-        },
-        {
-            "x": 7.5316071002697536,
-            "y": 2.6012326390018865,
-            "heading": -0.2973185190564447,
-            "angularVelocity": -4.530222940715824e-7,
-            "velocityX": -1.1588972027711093,
-            "velocityY": -3.6010547008857787,
-            "timestamp": 0.5997356485440076
-        },
-        {
-            "x": 7.401459139481823,
-            "y": 2.3492645367371554,
-            "heading": -0.29731856454052086,
-            "angularVelocity": -6.067216612944958e-7,
-            "velocityX": -1.7360710320141561,
-            "velocityY": -3.3610555300681284,
-            "timestamp": 0.6747026046120086
-        },
-        {
-            "x": 7.231595664022054,
-            "y": 2.1221680676798096,
-            "heading": -0.2973186386883636,
-            "angularVelocity": -9.89073675332688e-7,
-            "velocityX": -2.265844638345654,
-            "velocityY": -3.0292875817359826,
-            "timestamp": 0.7496695606800096
-        },
-        {
-            "x": 7.026654548839687,
-            "y": 1.9261437915792998,
-            "heading": -0.2973188151543491,
-            "angularVelocity": -0.000002353916908579506,
-            "velocityX": -2.7337526549226907,
-            "velocityY": -2.614809062311352,
-            "timestamp": 0.8246365167480105
-        },
-        {
-            "x": 6.792231559753418,
-            "y": 1.766543984413147,
-            "heading": -0.2973195709317279,
-            "angularVelocity": -0.00001008147347055336,
-            "velocityX": -3.127017573897922,
-            "velocityY": -2.1289354075064555,
-            "timestamp": 0.8996034728160115
-        },
-        {
-            "x": 6.610935711613446,
-            "y": 1.6724691195207,
-            "heading": -0.2973321063250889,
-            "angularVelocity": -0.00023216320074468077,
-            "velocityX": -3.3577106919370485,
-            "velocityY": -1.7423243992220112,
-            "timestamp": 0.9535973587619662
-        },
-        {
-            "x": 6.419882816475647,
-            "y": 1.600658398177247,
-            "heading": -0.29770902924967546,
-            "angularVelocity": -0.006980844552986395,
-            "velocityX": -3.538417207626563,
-            "velocityY": -1.329978757508455,
-            "timestamp": 1.007591244707921
-        },
-        {
-            "x": 6.223144239006287,
-            "y": 1.5524565272909876,
-            "heading": -0.30199019992690734,
-            "angularVelocity": -0.07928991592723345,
-            "velocityX": -3.6437195438439516,
-            "velocityY": -0.892728316211746,
-            "timestamp": 1.0615851306538757
-        },
-        {
-            "x": 6.027009837481886,
-            "y": 1.528287215105613,
-            "heading": -0.31920772077283077,
-            "angularVelocity": -0.31887908314582397,
-            "velocityX": -3.63252983348375,
-            "velocityY": -0.44763053745692566,
-            "timestamp": 1.1155790165998305
-        },
-        {
-            "x": 5.840489459577601,
-            "y": 1.5253276945335545,
-            "heading": -0.3622266817701244,
-            "angularVelocity": -0.7967376350788925,
-            "velocityX": -3.454472198777908,
-            "velocityY": -0.05481214252704887,
-            "timestamp": 1.1695729025457853
-        },
-        {
-            "x": 5.673855769295343,
-            "y": 1.5359985529910274,
-            "heading": -0.41631972448131044,
-            "angularVelocity": -1.0018364443214895,
-            "velocityX": -3.086158504113808,
-            "velocityY": 0.19763086635687102,
-            "timestamp": 1.22356678849174
-        },
-        {
-            "x": 5.530708201179271,
-            "y": 1.5529237053226916,
-            "heading": -0.47196047387840134,
-            "angularVelocity": -1.0305009247305976,
-            "velocityX": -2.651181066303626,
-            "velocityY": 0.3134642383140704,
-            "timestamp": 1.2775606744376948
-        },
-        {
-            "x": 5.411784437145291,
-            "y": 1.57153818466476,
-            "heading": -0.5236032237572267,
-            "angularVelocity": -0.956455512954414,
-            "velocityX": -2.2025413053807186,
-            "velocityY": 0.34475161429762524,
-            "timestamp": 1.3315545603836496
-        },
-        {
-            "x": 5.3170603467926,
-            "y": 1.5890782395606275,
-            "heading": -0.5679992830659408,
-            "angularVelocity": -0.8222423433861833,
-            "velocityX": -1.754348454332506,
-            "velocityY": 0.32485261226475975,
-            "timestamp": 1.3855484463296044
-        },
-        {
-            "x": 5.246334967071984,
-            "y": 1.6037618746917708,
-            "heading": -0.6030801100679523,
-            "angularVelocity": -0.6497185077054303,
-            "velocityX": -1.309877562644932,
-            "velocityY": 0.2719499601462664,
-            "timestamp": 1.4395423322755592
-        },
-        {
-            "x": 5.199384048260516,
-            "y": 1.6143645569232876,
-            "heading": -0.6274260701015184,
-            "angularVelocity": -0.45090216432919045,
-            "velocityX": -0.8695599138477248,
-            "velocityY": 0.19636820069091412,
-            "timestamp": 1.493536218221514
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -0.23287692075121041,
-            "velocityX": -0.4330869662524824,
-            "velocityY": 0.10437187429616973,
-            "timestamp": 1.5475301041674687
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0,
-            "velocityX": 3.9288115092123874e-33,
-            "velocityY": 2.0888895254257856e-32,
-            "timestamp": 1.6015239901134235
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.1.traj
deleted file mode 100644
index 93fe8a43..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.1.traj	
+++ /dev/null
@@ -1,265 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -6.709404579113994e-34,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.616979036560275,
-            "y": 4.122277530863763,
-            "heading": 0.08798648745533431,
-            "angularVelocity": 0.9786973137324638,
-            "velocityX": -0.7009990898727472,
-            "velocityY": -0.05252929161611988,
-            "timestamp": 0.08990163375413819
-        },
-        {
-            "x": 7.489636713382173,
-            "y": 4.1124897070611155,
-            "heading": 0.24774232666841967,
-            "angularVelocity": 1.7770070747544424,
-            "velocityX": -1.4164628367750984,
-            "velocityY": -0.10887259100780497,
-            "timestamp": 0.17980326750827638
-        },
-        {
-            "x": 7.296297558859626,
-            "y": 4.097684850959325,
-            "heading": 0.45307882023210666,
-            "angularVelocity": 2.2840129260079944,
-            "velocityX": -2.150563304013888,
-            "velocityY": -0.16467838773963095,
-            "timestamp": 0.2697049012624146
-        },
-        {
-            "x": 7.035802443968742,
-            "y": 4.077851050872189,
-            "heading": 0.6494071349161524,
-            "angularVelocity": 2.183812534719467,
-            "velocityX": -2.8975570744718895,
-            "velocityY": -0.22061668135394896,
-            "timestamp": 0.35960653501655276
-        },
-        {
-            "x": 6.7206589840239435,
-            "y": 4.046579170766799,
-            "heading": 0.7075219081795038,
-            "angularVelocity": 0.6464262198202391,
-            "velocityX": -3.505425283000413,
-            "velocityY": -0.34784551514284123,
-            "timestamp": 0.44950816877069094
-        },
-        {
-            "x": 6.381493052293109,
-            "y": 4.021491444364005,
-            "heading": 0.7075219804762528,
-            "angularVelocity": 8.041761424949666e-7,
-            "velocityX": -3.7726336838147025,
-            "velocityY": -0.27905751380892024,
-            "timestamp": 0.5394098025248292
-        },
-        {
-            "x": 6.042319780035644,
-            "y": 3.9965031541240665,
-            "heading": 0.7075220527697934,
-            "angularVelocity": 8.041404542109454e-7,
-            "velocityX": -3.7727153344624633,
-            "velocityY": -0.27795145868295346,
-            "timestamp": 0.6293114362789674
-        },
-        {
-            "x": 5.7039977727154,
-            "y": 4.0311606356057235,
-            "heading": 0.7075221294336903,
-            "angularVelocity": 8.52753100011537e-7,
-            "velocityX": -3.7632464861037187,
-            "velocityY": 0.3855044678769589,
-            "timestamp": 0.7192130700331055
-        },
-        {
-            "x": 5.379151739194528,
-            "y": 4.1318481159369504,
-            "heading": 0.707522232494757,
-            "angularVelocity": 0.0000011463759035114744,
-            "velocityX": -3.6133496128586025,
-            "velocityY": 1.1199738661767524,
-            "timestamp": 0.8091147037872437
-        },
-        {
-            "x": 5.0805369696371825,
-            "y": 4.294612195205207,
-            "heading": 0.7075222695179495,
-            "angularVelocity": 4.1181890719021464e-7,
-            "velocityX": -3.3215722238596133,
-            "velocityY": 1.8104685362379804,
-            "timestamp": 0.8990163375413819
-        },
-        {
-            "x": 4.819878772182475,
-            "y": 4.513061803267287,
-            "heading": 0.7075222309589103,
-            "angularVelocity": -4.2890254069180694e-7,
-            "velocityX": -2.899371085597298,
-            "velocityY": 2.429873617864304,
-            "timestamp": 0.9889179712955201
-        },
-        {
-            "x": 4.607411800528633,
-            "y": 4.7786191711792085,
-            "heading": 0.7075221980431953,
-            "angularVelocity": -3.6613033230177587e-7,
-            "velocityX": -2.363327147478702,
-            "velocityY": 2.953865873429686,
-            "timestamp": 1.0788196050496583
-        },
-        {
-            "x": 4.4192729102850965,
-            "y": 5.061932933767741,
-            "heading": 0.7075221661049191,
-            "angularVelocity": -3.552580165687493e-7,
-            "velocityX": -2.0927193687943375,
-            "velocityY": 3.151375016869355,
-            "timestamp": 1.1687212388037966
-        },
-        {
-            "x": 4.23999192605832,
-            "y": 5.350933382498888,
-            "heading": 0.7075221408571981,
-            "angularVelocity": -2.808371773455553e-7,
-            "velocityX": -1.9941905028900322,
-            "velocityY": 3.2146295530233306,
-            "timestamp": 1.258622872557935
-        },
-        {
-            "x": 4.059747399496467,
-            "y": 5.639333870205677,
-            "heading": 0.7075221155890865,
-            "angularVelocity": -2.8106398720718004e-7,
-            "velocityX": -2.0049082428773954,
-            "velocityY": 3.2079560255325883,
-            "timestamp": 1.3485245063120732
-        },
-        {
-            "x": 3.82761549949646,
-            "y": 5.887885093688965,
-            "heading": 0.7075220884594695,
-            "angularVelocity": -3.017700109769218e-7,
-            "velocityX": -2.5820654231362323,
-            "velocityY": 2.764701964849852,
-            "timestamp": 1.4384261400662115
-        },
-        {
-            "x": 3.6506709942338365,
-            "y": 6.0327931031008335,
-            "heading": 0.7075220641283702,
-            "angularVelocity": -4.024467762457879e-7,
-            "velocityX": -2.92673770558828,
-            "velocityY": 2.3968403786158894,
-            "timestamp": 1.4988840707255708
-        },
-        {
-            "x": 3.4560308994306905,
-            "y": 6.152888579058325,
-            "heading": 0.7075220417785688,
-            "angularVelocity": -3.696752622643165e-7,
-            "velocityX": -3.2194303159267204,
-            "velocityY": 1.9864304756722417,
-            "timestamp": 1.5593420013849302
-        },
-        {
-            "x": 3.2531901663020784,
-            "y": 6.258545225655943,
-            "heading": 0.7075220198614292,
-            "angularVelocity": -3.625188534499236e-7,
-            "velocityX": -3.3550723770436006,
-            "velocityY": 1.7476060699673126,
-            "timestamp": 1.6197999320442895
-        },
-        {
-            "x": 3.0503686595470367,
-            "y": 6.36423877502023,
-            "heading": 0.7075219979437177,
-            "angularVelocity": -3.625283082596746e-7,
-            "velocityX": -3.354754364616084,
-            "velocityY": 1.7482164574835888,
-            "timestamp": 1.6802578627036489
-        },
-        {
-            "x": 2.862919086961248,
-            "y": 6.462612745515211,
-            "heading": 0.6632456903097049,
-            "angularVelocity": -0.7323490425675934,
-            "velocityX": -3.1004960067524197,
-            "velocityY": 1.6271474961510723,
-            "timestamp": 1.7407157933630082
-        },
-        {
-            "x": 2.701858233172079,
-            "y": 6.5469153850893225,
-            "heading": 0.5979529513983839,
-            "angularVelocity": -1.0799697938593102,
-            "velocityX": -2.6640153249158556,
-            "velocityY": 1.394401671618308,
-            "timestamp": 1.8011737240223675
-        },
-        {
-            "x": 2.567771439572364,
-            "y": 6.617045170730925,
-            "heading": 0.5348900965776917,
-            "angularVelocity": -1.0430865584205178,
-            "velocityX": -2.2178528463909877,
-            "velocityY": 1.1599766131037368,
-            "timestamp": 1.8616316546817269
-        },
-        {
-            "x": 2.4605956436791856,
-            "y": 6.67307612750581,
-            "heading": 0.4804771381393031,
-            "angularVelocity": -0.9000135771262476,
-            "velocityX": -1.7727334482731019,
-            "velocityY": 0.9267759607999744,
-            "timestamp": 1.9220895853410862
-        },
-        {
-            "x": 2.380269344431837,
-            "y": 6.715058292556038,
-            "heading": 0.43766723065506036,
-            "angularVelocity": -0.7080941576620494,
-            "velocityX": -1.3286313039718116,
-            "velocityY": 0.694402944201169,
-            "timestamp": 1.9825475160004455
-        },
-        {
-            "x": 2.3267482247734668,
-            "y": 6.743025081240933,
-            "heading": 0.4081449342684466,
-            "angularVelocity": -0.48831139380108807,
-            "velocityX": -0.8852621827225426,
-            "velocityY": 0.462582631920201,
-            "timestamp": 2.0430054466598047
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": -0.2505036825320822,
-            "velocityX": -0.4424270642703138,
-            "velocityY": 0.23115112619031306,
-            "timestamp": 2.103463377319164
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -2.9211624967517807e-31,
-            "timestamp": 2.1639213079785233
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.traj
deleted file mode 100644
index 93fe8a43..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.traj	
+++ /dev/null
@@ -1,265 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -6.709404579113994e-34,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.616979036560275,
-            "y": 4.122277530863763,
-            "heading": 0.08798648745533431,
-            "angularVelocity": 0.9786973137324638,
-            "velocityX": -0.7009990898727472,
-            "velocityY": -0.05252929161611988,
-            "timestamp": 0.08990163375413819
-        },
-        {
-            "x": 7.489636713382173,
-            "y": 4.1124897070611155,
-            "heading": 0.24774232666841967,
-            "angularVelocity": 1.7770070747544424,
-            "velocityX": -1.4164628367750984,
-            "velocityY": -0.10887259100780497,
-            "timestamp": 0.17980326750827638
-        },
-        {
-            "x": 7.296297558859626,
-            "y": 4.097684850959325,
-            "heading": 0.45307882023210666,
-            "angularVelocity": 2.2840129260079944,
-            "velocityX": -2.150563304013888,
-            "velocityY": -0.16467838773963095,
-            "timestamp": 0.2697049012624146
-        },
-        {
-            "x": 7.035802443968742,
-            "y": 4.077851050872189,
-            "heading": 0.6494071349161524,
-            "angularVelocity": 2.183812534719467,
-            "velocityX": -2.8975570744718895,
-            "velocityY": -0.22061668135394896,
-            "timestamp": 0.35960653501655276
-        },
-        {
-            "x": 6.7206589840239435,
-            "y": 4.046579170766799,
-            "heading": 0.7075219081795038,
-            "angularVelocity": 0.6464262198202391,
-            "velocityX": -3.505425283000413,
-            "velocityY": -0.34784551514284123,
-            "timestamp": 0.44950816877069094
-        },
-        {
-            "x": 6.381493052293109,
-            "y": 4.021491444364005,
-            "heading": 0.7075219804762528,
-            "angularVelocity": 8.041761424949666e-7,
-            "velocityX": -3.7726336838147025,
-            "velocityY": -0.27905751380892024,
-            "timestamp": 0.5394098025248292
-        },
-        {
-            "x": 6.042319780035644,
-            "y": 3.9965031541240665,
-            "heading": 0.7075220527697934,
-            "angularVelocity": 8.041404542109454e-7,
-            "velocityX": -3.7727153344624633,
-            "velocityY": -0.27795145868295346,
-            "timestamp": 0.6293114362789674
-        },
-        {
-            "x": 5.7039977727154,
-            "y": 4.0311606356057235,
-            "heading": 0.7075221294336903,
-            "angularVelocity": 8.52753100011537e-7,
-            "velocityX": -3.7632464861037187,
-            "velocityY": 0.3855044678769589,
-            "timestamp": 0.7192130700331055
-        },
-        {
-            "x": 5.379151739194528,
-            "y": 4.1318481159369504,
-            "heading": 0.707522232494757,
-            "angularVelocity": 0.0000011463759035114744,
-            "velocityX": -3.6133496128586025,
-            "velocityY": 1.1199738661767524,
-            "timestamp": 0.8091147037872437
-        },
-        {
-            "x": 5.0805369696371825,
-            "y": 4.294612195205207,
-            "heading": 0.7075222695179495,
-            "angularVelocity": 4.1181890719021464e-7,
-            "velocityX": -3.3215722238596133,
-            "velocityY": 1.8104685362379804,
-            "timestamp": 0.8990163375413819
-        },
-        {
-            "x": 4.819878772182475,
-            "y": 4.513061803267287,
-            "heading": 0.7075222309589103,
-            "angularVelocity": -4.2890254069180694e-7,
-            "velocityX": -2.899371085597298,
-            "velocityY": 2.429873617864304,
-            "timestamp": 0.9889179712955201
-        },
-        {
-            "x": 4.607411800528633,
-            "y": 4.7786191711792085,
-            "heading": 0.7075221980431953,
-            "angularVelocity": -3.6613033230177587e-7,
-            "velocityX": -2.363327147478702,
-            "velocityY": 2.953865873429686,
-            "timestamp": 1.0788196050496583
-        },
-        {
-            "x": 4.4192729102850965,
-            "y": 5.061932933767741,
-            "heading": 0.7075221661049191,
-            "angularVelocity": -3.552580165687493e-7,
-            "velocityX": -2.0927193687943375,
-            "velocityY": 3.151375016869355,
-            "timestamp": 1.1687212388037966
-        },
-        {
-            "x": 4.23999192605832,
-            "y": 5.350933382498888,
-            "heading": 0.7075221408571981,
-            "angularVelocity": -2.808371773455553e-7,
-            "velocityX": -1.9941905028900322,
-            "velocityY": 3.2146295530233306,
-            "timestamp": 1.258622872557935
-        },
-        {
-            "x": 4.059747399496467,
-            "y": 5.639333870205677,
-            "heading": 0.7075221155890865,
-            "angularVelocity": -2.8106398720718004e-7,
-            "velocityX": -2.0049082428773954,
-            "velocityY": 3.2079560255325883,
-            "timestamp": 1.3485245063120732
-        },
-        {
-            "x": 3.82761549949646,
-            "y": 5.887885093688965,
-            "heading": 0.7075220884594695,
-            "angularVelocity": -3.017700109769218e-7,
-            "velocityX": -2.5820654231362323,
-            "velocityY": 2.764701964849852,
-            "timestamp": 1.4384261400662115
-        },
-        {
-            "x": 3.6506709942338365,
-            "y": 6.0327931031008335,
-            "heading": 0.7075220641283702,
-            "angularVelocity": -4.024467762457879e-7,
-            "velocityX": -2.92673770558828,
-            "velocityY": 2.3968403786158894,
-            "timestamp": 1.4988840707255708
-        },
-        {
-            "x": 3.4560308994306905,
-            "y": 6.152888579058325,
-            "heading": 0.7075220417785688,
-            "angularVelocity": -3.696752622643165e-7,
-            "velocityX": -3.2194303159267204,
-            "velocityY": 1.9864304756722417,
-            "timestamp": 1.5593420013849302
-        },
-        {
-            "x": 3.2531901663020784,
-            "y": 6.258545225655943,
-            "heading": 0.7075220198614292,
-            "angularVelocity": -3.625188534499236e-7,
-            "velocityX": -3.3550723770436006,
-            "velocityY": 1.7476060699673126,
-            "timestamp": 1.6197999320442895
-        },
-        {
-            "x": 3.0503686595470367,
-            "y": 6.36423877502023,
-            "heading": 0.7075219979437177,
-            "angularVelocity": -3.625283082596746e-7,
-            "velocityX": -3.354754364616084,
-            "velocityY": 1.7482164574835888,
-            "timestamp": 1.6802578627036489
-        },
-        {
-            "x": 2.862919086961248,
-            "y": 6.462612745515211,
-            "heading": 0.6632456903097049,
-            "angularVelocity": -0.7323490425675934,
-            "velocityX": -3.1004960067524197,
-            "velocityY": 1.6271474961510723,
-            "timestamp": 1.7407157933630082
-        },
-        {
-            "x": 2.701858233172079,
-            "y": 6.5469153850893225,
-            "heading": 0.5979529513983839,
-            "angularVelocity": -1.0799697938593102,
-            "velocityX": -2.6640153249158556,
-            "velocityY": 1.394401671618308,
-            "timestamp": 1.8011737240223675
-        },
-        {
-            "x": 2.567771439572364,
-            "y": 6.617045170730925,
-            "heading": 0.5348900965776917,
-            "angularVelocity": -1.0430865584205178,
-            "velocityX": -2.2178528463909877,
-            "velocityY": 1.1599766131037368,
-            "timestamp": 1.8616316546817269
-        },
-        {
-            "x": 2.4605956436791856,
-            "y": 6.67307612750581,
-            "heading": 0.4804771381393031,
-            "angularVelocity": -0.9000135771262476,
-            "velocityX": -1.7727334482731019,
-            "velocityY": 0.9267759607999744,
-            "timestamp": 1.9220895853410862
-        },
-        {
-            "x": 2.380269344431837,
-            "y": 6.715058292556038,
-            "heading": 0.43766723065506036,
-            "angularVelocity": -0.7080941576620494,
-            "velocityX": -1.3286313039718116,
-            "velocityY": 0.694402944201169,
-            "timestamp": 1.9825475160004455
-        },
-        {
-            "x": 2.3267482247734668,
-            "y": 6.743025081240933,
-            "heading": 0.4081449342684466,
-            "angularVelocity": -0.48831139380108807,
-            "velocityX": -0.8852621827225426,
-            "velocityY": 0.462582631920201,
-            "timestamp": 2.0430054466598047
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": -0.2505036825320822,
-            "velocityX": -0.4424270642703138,
-            "velocityY": 0.23115112619031306,
-            "timestamp": 2.103463377319164
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -2.9211624967517807e-31,
-            "timestamp": 2.1639213079785233
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.1.traj
deleted file mode 100644
index 2f31d71f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.1.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -8.248091288877123e-33,
-            "velocityX": -1.7499589770527321e-34,
-            "velocityY": 1.243057151683689e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 7.599512207946833,
-            "y": 4.126338330595212,
-            "heading": -0.05672955044148203,
-            "angularVelocity": -0.5720722032058021,
-            "velocityX": -0.8116550928518662,
-            "velocityY": -0.0066724074357217035,
-            "timestamp": 0.09916501819801522
-        },
-        {
-            "x": 7.438184902121127,
-            "y": 4.125091465076226,
-            "heading": -0.16291927611759752,
-            "angularVelocity": -1.070838563898341,
-            "velocityX": -1.6268570183042166,
-            "velocityY": -0.01257364281924666,
-            "timestamp": 0.19833003639603045
-        },
-        {
-            "x": 7.19545269041534,
-            "y": 4.123364615129754,
-            "heading": -0.30423484322627714,
-            "angularVelocity": -1.4250546178138856,
-            "velocityX": -2.4477604715515007,
-            "velocityY": -0.017413902380612675,
-            "timestamp": 0.2974950545940457
-        },
-        {
-            "x": 6.870785634071806,
-            "y": 4.1216134281783505,
-            "heading": -0.4343749203987102,
-            "angularVelocity": -1.3123587282822462,
-            "velocityX": -3.274007933878773,
-            "velocityY": -0.01765932163604032,
-            "timestamp": 0.3966600727920609
-        },
-        {
-            "x": 6.495672194218458,
-            "y": 4.117557524267507,
-            "heading": -0.4343749552288927,
-            "angularVelocity": -3.5123456983963544e-7,
-            "velocityX": -3.782719417287976,
-            "velocityY": -0.040900551268437134,
-            "timestamp": 0.4958250909900761
-        },
-        {
-            "x": 6.120558752684864,
-            "y": 4.113501775750749,
-            "heading": -0.43437499005887026,
-            "angularVelocity": -3.512325030859258e-7,
-            "velocityX": -3.782719434231919,
-            "velocityY": -0.04089898424321494,
-            "timestamp": 0.5949901091880914
-        },
-        {
-            "x": 5.747019040696225,
-            "y": 4.148065138700183,
-            "heading": -0.43437502656757854,
-            "angularVelocity": -3.681611615448704e-7,
-            "velocityX": -3.766849628794958,
-            "velocityY": 0.3485439076955189,
-            "timestamp": 0.6941551273861066
-        },
-        {
-            "x": 5.389911454629062,
-            "y": 4.262959433025878,
-            "heading": -0.434375162279621,
-            "angularVelocity": -0.0000013685475476133038,
-            "velocityX": -3.601144764115114,
-            "velocityY": 1.1586171859140066,
-            "timestamp": 0.7933201455841219
-        },
-        {
-            "x": 5.045913588511523,
-            "y": 4.412598985545015,
-            "heading": -0.4343752985502662,
-            "angularVelocity": -0.0000013741806101348962,
-            "velocityX": -3.468943709873945,
-            "velocityY": 1.5089953618556682,
-            "timestamp": 0.8924851637821372
-        },
-        {
-            "x": 4.713243052034498,
-            "y": 4.585968073714987,
-            "heading": -0.43437543856751193,
-            "angularVelocity": -0.0000014119620841874337,
-            "velocityX": -3.354716638207433,
-            "velocityY": 1.7482887748155678,
-            "timestamp": 0.9916501819801524
-        },
-        {
-            "x": 4.426185676495707,
-            "y": 4.82747296096862,
-            "heading": -0.4343754384530987,
-            "angularVelocity": 1.153766536466057e-9,
-            "velocityX": -2.894744343873237,
-            "velocityY": 2.4353838847827403,
-            "timestamp": 1.0908152001781677
-        },
-        {
-            "x": 4.09351517988242,
-            "y": 5.000842166204109,
-            "heading": -0.4343753660253311,
-            "angularVelocity": 7.303761838407394e-7,
-            "velocityX": -3.354716236213479,
-            "velocityY": 1.748289955327798,
-            "timestamp": 1.189980218376183
-        },
-        {
-            "x": 3.752063829141965,
-            "y": 5.156204697333757,
-            "heading": -0.43437529472941433,
-            "angularVelocity": 7.189623727917814e-7,
-            "velocityX": -3.4432641363372296,
-            "velocityY": 1.5667070298864525,
-            "timestamp": 1.2891452365741982
-        },
-        {
-            "x": 3.4085469650969755,
-            "y": 5.306945220863998,
-            "heading": -0.43437522535251666,
-            "angularVelocity": 6.996105981774619e-7,
-            "velocityX": -3.46409318817495,
-            "velocityY": 1.5200977750968405,
-            "timestamp": 1.3883102547722135
-        },
-        {
-            "x": 3.0417944650952866,
-            "y": 5.3858068126004595,
-            "heading": -0.43437520041032157,
-            "angularVelocity": 2.5152211488411685e-7,
-            "velocityX": -3.6984060172242224,
-            "velocityY": 0.7952561615930772,
-            "timestamp": 1.4874752729702287
-        },
-        {
-            "x": 2.7246409461958954,
-            "y": 5.456187357907523,
-            "heading": -0.2777696543737322,
-            "angularVelocity": 1.5792418423589194,
-            "velocityX": -3.1982399102281316,
-            "velocityY": 0.7097315826285274,
-            "timestamp": 1.586640291168244
-        },
-        {
-            "x": 2.4871805723397817,
-            "y": 5.508279050924135,
-            "heading": -0.14311381126711797,
-            "angularVelocity": 1.3578966207390795,
-            "velocityX": -2.394598197742947,
-            "velocityY": 0.5253031155865293,
-            "timestamp": 1.6858053093662593
-        },
-        {
-            "x": 2.3290265259060328,
-            "y": 5.5428007138640085,
-            "heading": -0.04864902301750531,
-            "angularVelocity": 0.9526019353012469,
-            "velocityX": -1.594857231992267,
-            "velocityY": 0.3481233964071807,
-            "timestamp": 1.7849703275642745
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -2.5681051863102055e-32,
-            "angularVelocity": 0.490586538494469,
-            "velocityX": -0.7969193909512572,
-            "velocityY": 0.1734410626703791,
-            "timestamp": 1.8841353457622898
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 1.14717044306562e-31,
-            "velocityX": 0,
-            "velocityY": 5.846493116157662e-32,
-            "timestamp": 1.983300363960305
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.traj
deleted file mode 100644
index 2f31d71f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -8.248091288877123e-33,
-            "velocityX": -1.7499589770527321e-34,
-            "velocityY": 1.243057151683689e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 7.599512207946833,
-            "y": 4.126338330595212,
-            "heading": -0.05672955044148203,
-            "angularVelocity": -0.5720722032058021,
-            "velocityX": -0.8116550928518662,
-            "velocityY": -0.0066724074357217035,
-            "timestamp": 0.09916501819801522
-        },
-        {
-            "x": 7.438184902121127,
-            "y": 4.125091465076226,
-            "heading": -0.16291927611759752,
-            "angularVelocity": -1.070838563898341,
-            "velocityX": -1.6268570183042166,
-            "velocityY": -0.01257364281924666,
-            "timestamp": 0.19833003639603045
-        },
-        {
-            "x": 7.19545269041534,
-            "y": 4.123364615129754,
-            "heading": -0.30423484322627714,
-            "angularVelocity": -1.4250546178138856,
-            "velocityX": -2.4477604715515007,
-            "velocityY": -0.017413902380612675,
-            "timestamp": 0.2974950545940457
-        },
-        {
-            "x": 6.870785634071806,
-            "y": 4.1216134281783505,
-            "heading": -0.4343749203987102,
-            "angularVelocity": -1.3123587282822462,
-            "velocityX": -3.274007933878773,
-            "velocityY": -0.01765932163604032,
-            "timestamp": 0.3966600727920609
-        },
-        {
-            "x": 6.495672194218458,
-            "y": 4.117557524267507,
-            "heading": -0.4343749552288927,
-            "angularVelocity": -3.5123456983963544e-7,
-            "velocityX": -3.782719417287976,
-            "velocityY": -0.040900551268437134,
-            "timestamp": 0.4958250909900761
-        },
-        {
-            "x": 6.120558752684864,
-            "y": 4.113501775750749,
-            "heading": -0.43437499005887026,
-            "angularVelocity": -3.512325030859258e-7,
-            "velocityX": -3.782719434231919,
-            "velocityY": -0.04089898424321494,
-            "timestamp": 0.5949901091880914
-        },
-        {
-            "x": 5.747019040696225,
-            "y": 4.148065138700183,
-            "heading": -0.43437502656757854,
-            "angularVelocity": -3.681611615448704e-7,
-            "velocityX": -3.766849628794958,
-            "velocityY": 0.3485439076955189,
-            "timestamp": 0.6941551273861066
-        },
-        {
-            "x": 5.389911454629062,
-            "y": 4.262959433025878,
-            "heading": -0.434375162279621,
-            "angularVelocity": -0.0000013685475476133038,
-            "velocityX": -3.601144764115114,
-            "velocityY": 1.1586171859140066,
-            "timestamp": 0.7933201455841219
-        },
-        {
-            "x": 5.045913588511523,
-            "y": 4.412598985545015,
-            "heading": -0.4343752985502662,
-            "angularVelocity": -0.0000013741806101348962,
-            "velocityX": -3.468943709873945,
-            "velocityY": 1.5089953618556682,
-            "timestamp": 0.8924851637821372
-        },
-        {
-            "x": 4.713243052034498,
-            "y": 4.585968073714987,
-            "heading": -0.43437543856751193,
-            "angularVelocity": -0.0000014119620841874337,
-            "velocityX": -3.354716638207433,
-            "velocityY": 1.7482887748155678,
-            "timestamp": 0.9916501819801524
-        },
-        {
-            "x": 4.426185676495707,
-            "y": 4.82747296096862,
-            "heading": -0.4343754384530987,
-            "angularVelocity": 1.153766536466057e-9,
-            "velocityX": -2.894744343873237,
-            "velocityY": 2.4353838847827403,
-            "timestamp": 1.0908152001781677
-        },
-        {
-            "x": 4.09351517988242,
-            "y": 5.000842166204109,
-            "heading": -0.4343753660253311,
-            "angularVelocity": 7.303761838407394e-7,
-            "velocityX": -3.354716236213479,
-            "velocityY": 1.748289955327798,
-            "timestamp": 1.189980218376183
-        },
-        {
-            "x": 3.752063829141965,
-            "y": 5.156204697333757,
-            "heading": -0.43437529472941433,
-            "angularVelocity": 7.189623727917814e-7,
-            "velocityX": -3.4432641363372296,
-            "velocityY": 1.5667070298864525,
-            "timestamp": 1.2891452365741982
-        },
-        {
-            "x": 3.4085469650969755,
-            "y": 5.306945220863998,
-            "heading": -0.43437522535251666,
-            "angularVelocity": 6.996105981774619e-7,
-            "velocityX": -3.46409318817495,
-            "velocityY": 1.5200977750968405,
-            "timestamp": 1.3883102547722135
-        },
-        {
-            "x": 3.0417944650952866,
-            "y": 5.3858068126004595,
-            "heading": -0.43437520041032157,
-            "angularVelocity": 2.5152211488411685e-7,
-            "velocityX": -3.6984060172242224,
-            "velocityY": 0.7952561615930772,
-            "timestamp": 1.4874752729702287
-        },
-        {
-            "x": 2.7246409461958954,
-            "y": 5.456187357907523,
-            "heading": -0.2777696543737322,
-            "angularVelocity": 1.5792418423589194,
-            "velocityX": -3.1982399102281316,
-            "velocityY": 0.7097315826285274,
-            "timestamp": 1.586640291168244
-        },
-        {
-            "x": 2.4871805723397817,
-            "y": 5.508279050924135,
-            "heading": -0.14311381126711797,
-            "angularVelocity": 1.3578966207390795,
-            "velocityX": -2.394598197742947,
-            "velocityY": 0.5253031155865293,
-            "timestamp": 1.6858053093662593
-        },
-        {
-            "x": 2.3290265259060328,
-            "y": 5.5428007138640085,
-            "heading": -0.04864902301750531,
-            "angularVelocity": 0.9526019353012469,
-            "velocityX": -1.594857231992267,
-            "velocityY": 0.3481233964071807,
-            "timestamp": 1.7849703275642745
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -2.5681051863102055e-32,
-            "angularVelocity": 0.490586538494469,
-            "velocityX": -0.7969193909512572,
-            "velocityY": 0.1734410626703791,
-            "timestamp": 1.8841353457622898
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 1.14717044306562e-31,
-            "velocityX": 0,
-            "velocityY": 5.846493116157662e-32,
-            "timestamp": 1.983300363960305
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W3.1.traj
deleted file mode 100644
index 3874f8b2..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W3.1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 5.58164160972529e-32,
-            "velocityX": 1.1273756142039836e-32,
-            "velocityY": 2.4507602066878393e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.588395006557631,
-            "y": 4.128528104169273,
-            "heading": -0.06094869084906696,
-            "angularVelocity": -0.5766457987157873,
-            "velocityX": -0.8666902254019087,
-            "velocityY": 0.01445765014696692,
-            "timestamp": 0.10569519622756653
-        },
-        {
-            "x": 7.4046836501246664,
-            "y": 4.131675342578391,
-            "heading": -0.17161815397097052,
-            "angularVelocity": -1.0470623743734504,
-            "velocityX": -1.7381239922903071,
-            "velocityY": 0.029776551077511915,
-            "timestamp": 0.21139039245513305
-        },
-        {
-            "x": 7.128148507793846,
-            "y": 4.136585421276787,
-            "heading": -0.30937735862178317,
-            "angularVelocity": -1.3033629679272352,
-            "velocityX": -2.6163454177749763,
-            "velocityY": 0.0464550790730766,
-            "timestamp": 0.3170855886826996
-        },
-        {
-            "x": 6.759745872373483,
-            "y": 4.143898882898015,
-            "heading": -0.3925495634756188,
-            "angularVelocity": -0.7869061965196801,
-            "velocityX": -3.485519196418107,
-            "velocityY": 0.06919388848555069,
-            "timestamp": 0.4227807849102661
-        },
-        {
-            "x": 6.3599483672427946,
-            "y": 4.149634642956697,
-            "heading": -0.39254959749689916,
-            "angularVelocity": -3.2188104690533836e-7,
-            "velocityX": -3.782551330619671,
-            "velocityY": 0.05426698907234567,
-            "timestamp": 0.5284759811378327
-        },
-        {
-            "x": 5.960150865134662,
-            "y": 4.155370613693523,
-            "heading": -0.39254963151811834,
-            "angularVelocity": -3.2188046698591794e-7,
-            "velocityX": -3.7825513020227493,
-            "velocityY": 0.05426898233365955,
-            "timestamp": 0.6341711773653992
-        },
-        {
-            "x": 5.56430885680334,
-            "y": 4.211762521698523,
-            "heading": -0.3925496691964927,
-            "angularVelocity": -3.5648142721372326e-7,
-            "velocityX": -3.745127711188092,
-            "velocityY": 0.5335333110464675,
-            "timestamp": 0.7398663735929657
-        },
-        {
-            "x": 5.196451370697568,
-            "y": 4.356905683947299,
-            "heading": -0.407192768864793,
-            "angularVelocity": -0.13854082485237101,
-            "velocityX": -3.4803614472105,
-            "velocityY": 1.3732238306864724,
-            "timestamp": 0.8455615698205322
-        },
-        {
-            "x": 4.853119399386178,
-            "y": 4.529141900436451,
-            "heading": -0.46036689242459117,
-            "angularVelocity": -0.5030893120753747,
-            "velocityX": -3.2483214333807724,
-            "velocityY": 1.6295557663597022,
-            "timestamp": 0.9512567660480987
-        },
-        {
-            "x": 4.514017884763102,
-            "y": 4.740993233583884,
-            "heading": -0.4603674462778229,
-            "angularVelocity": -0.000005240098429403888,
-            "velocityX": -3.208296372268186,
-            "velocityY": 2.0043610372916896,
-            "timestamp": 1.0569519622756653
-        },
-        {
-            "x": 4.21750261375463,
-            "y": 5.009227519029098,
-            "heading": -0.46036738913342323,
-            "angularVelocity": 5.406527608893175e-7,
-            "velocityX": -2.8053807702864813,
-            "velocityY": 2.537809616888297,
-            "timestamp": 1.1626471585032319
-        },
-        {
-            "x": 3.866969646487487,
-            "y": 5.201574981603133,
-            "heading": -0.46036721071098435,
-            "angularVelocity": 0.0000016880846552508654,
-            "velocityX": -3.3164512653198517,
-            "velocityY": 1.8198316426784653,
-            "timestamp": 1.2683423547307984
-        },
-        {
-            "x": 3.5045677865422378,
-            "y": 5.303672838764994,
-            "heading": -0.3887465759877037,
-            "angularVelocity": 0.6776148517580518,
-            "velocityX": -3.4287448519891264,
-            "velocityY": 0.9659649710289621,
-            "timestamp": 1.3740375509583649
-        },
-        {
-            "x": 3.16557460185915,
-            "y": 5.315860378892371,
-            "heading": -0.29320626763004903,
-            "angularVelocity": 0.9039229006392311,
-            "velocityX": -3.207271444515043,
-            "velocityY": 0.1153083636945626,
-            "timestamp": 1.4797327471859314
-        },
-        {
-            "x": 2.8681536233597122,
-            "y": 5.2454821587095966,
-            "heading": -0.23849853505250618,
-            "angularVelocity": 0.5175990445181124,
-            "velocityX": -2.8139498209462337,
-            "velocityY": -0.6658601591622668,
-            "timestamp": 1.585427943413498
-        },
-        {
-            "x": 2.618332286986499,
-            "y": 5.0970676603891505,
-            "heading": -0.24744806192055374,
-            "angularVelocity": -0.08467297651615932,
-            "velocityX": -2.3636016137889215,
-            "velocityY": -1.4041744905880433,
-            "timestamp": 1.6911231396410644
-        },
-        {
-            "x": 2.4325478975720265,
-            "y": 4.8813111498736355,
-            "heading": -0.27927376577641677,
-            "angularVelocity": -0.3011083283987745,
-            "velocityX": -1.7577373054350616,
-            "velocityY": -2.0413085761341616,
-            "timestamp": 1.796818335868631
-        },
-        {
-            "x": 2.3348070320385017,
-            "y": 4.635496280912222,
-            "heading": -0.32770640318337707,
-            "angularVelocity": -0.4582293153861289,
-            "velocityX": -0.9247427416008974,
-            "velocityY": -2.3256957528340565,
-            "timestamp": 1.9025135320961974
-        },
-        {
-            "x": 2.305548232287739,
-            "y": 4.452786448669473,
-            "heading": -0.3683698320727586,
-            "angularVelocity": -0.3847235289845278,
-            "velocityX": -0.2768224176221525,
-            "velocityY": -1.72864840374928,
-            "timestamp": 2.008208728323764
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.2046466509288718,
-            "velocityX": -0.05249275734153,
-            "velocityY": -0.8778681716972112,
-            "timestamp": 2.1139039245513307
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 5.518495272475637e-32,
-            "velocityX": 0,
-            "velocityY": 3.7393231194786592e-34,
-            "timestamp": 2.219599120778897
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W3.traj
deleted file mode 100644
index 3874f8b2..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W3.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 5.58164160972529e-32,
-            "velocityX": 1.1273756142039836e-32,
-            "velocityY": 2.4507602066878393e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.588395006557631,
-            "y": 4.128528104169273,
-            "heading": -0.06094869084906696,
-            "angularVelocity": -0.5766457987157873,
-            "velocityX": -0.8666902254019087,
-            "velocityY": 0.01445765014696692,
-            "timestamp": 0.10569519622756653
-        },
-        {
-            "x": 7.4046836501246664,
-            "y": 4.131675342578391,
-            "heading": -0.17161815397097052,
-            "angularVelocity": -1.0470623743734504,
-            "velocityX": -1.7381239922903071,
-            "velocityY": 0.029776551077511915,
-            "timestamp": 0.21139039245513305
-        },
-        {
-            "x": 7.128148507793846,
-            "y": 4.136585421276787,
-            "heading": -0.30937735862178317,
-            "angularVelocity": -1.3033629679272352,
-            "velocityX": -2.6163454177749763,
-            "velocityY": 0.0464550790730766,
-            "timestamp": 0.3170855886826996
-        },
-        {
-            "x": 6.759745872373483,
-            "y": 4.143898882898015,
-            "heading": -0.3925495634756188,
-            "angularVelocity": -0.7869061965196801,
-            "velocityX": -3.485519196418107,
-            "velocityY": 0.06919388848555069,
-            "timestamp": 0.4227807849102661
-        },
-        {
-            "x": 6.3599483672427946,
-            "y": 4.149634642956697,
-            "heading": -0.39254959749689916,
-            "angularVelocity": -3.2188104690533836e-7,
-            "velocityX": -3.782551330619671,
-            "velocityY": 0.05426698907234567,
-            "timestamp": 0.5284759811378327
-        },
-        {
-            "x": 5.960150865134662,
-            "y": 4.155370613693523,
-            "heading": -0.39254963151811834,
-            "angularVelocity": -3.2188046698591794e-7,
-            "velocityX": -3.7825513020227493,
-            "velocityY": 0.05426898233365955,
-            "timestamp": 0.6341711773653992
-        },
-        {
-            "x": 5.56430885680334,
-            "y": 4.211762521698523,
-            "heading": -0.3925496691964927,
-            "angularVelocity": -3.5648142721372326e-7,
-            "velocityX": -3.745127711188092,
-            "velocityY": 0.5335333110464675,
-            "timestamp": 0.7398663735929657
-        },
-        {
-            "x": 5.196451370697568,
-            "y": 4.356905683947299,
-            "heading": -0.407192768864793,
-            "angularVelocity": -0.13854082485237101,
-            "velocityX": -3.4803614472105,
-            "velocityY": 1.3732238306864724,
-            "timestamp": 0.8455615698205322
-        },
-        {
-            "x": 4.853119399386178,
-            "y": 4.529141900436451,
-            "heading": -0.46036689242459117,
-            "angularVelocity": -0.5030893120753747,
-            "velocityX": -3.2483214333807724,
-            "velocityY": 1.6295557663597022,
-            "timestamp": 0.9512567660480987
-        },
-        {
-            "x": 4.514017884763102,
-            "y": 4.740993233583884,
-            "heading": -0.4603674462778229,
-            "angularVelocity": -0.000005240098429403888,
-            "velocityX": -3.208296372268186,
-            "velocityY": 2.0043610372916896,
-            "timestamp": 1.0569519622756653
-        },
-        {
-            "x": 4.21750261375463,
-            "y": 5.009227519029098,
-            "heading": -0.46036738913342323,
-            "angularVelocity": 5.406527608893175e-7,
-            "velocityX": -2.8053807702864813,
-            "velocityY": 2.537809616888297,
-            "timestamp": 1.1626471585032319
-        },
-        {
-            "x": 3.866969646487487,
-            "y": 5.201574981603133,
-            "heading": -0.46036721071098435,
-            "angularVelocity": 0.0000016880846552508654,
-            "velocityX": -3.3164512653198517,
-            "velocityY": 1.8198316426784653,
-            "timestamp": 1.2683423547307984
-        },
-        {
-            "x": 3.5045677865422378,
-            "y": 5.303672838764994,
-            "heading": -0.3887465759877037,
-            "angularVelocity": 0.6776148517580518,
-            "velocityX": -3.4287448519891264,
-            "velocityY": 0.9659649710289621,
-            "timestamp": 1.3740375509583649
-        },
-        {
-            "x": 3.16557460185915,
-            "y": 5.315860378892371,
-            "heading": -0.29320626763004903,
-            "angularVelocity": 0.9039229006392311,
-            "velocityX": -3.207271444515043,
-            "velocityY": 0.1153083636945626,
-            "timestamp": 1.4797327471859314
-        },
-        {
-            "x": 2.8681536233597122,
-            "y": 5.2454821587095966,
-            "heading": -0.23849853505250618,
-            "angularVelocity": 0.5175990445181124,
-            "velocityX": -2.8139498209462337,
-            "velocityY": -0.6658601591622668,
-            "timestamp": 1.585427943413498
-        },
-        {
-            "x": 2.618332286986499,
-            "y": 5.0970676603891505,
-            "heading": -0.24744806192055374,
-            "angularVelocity": -0.08467297651615932,
-            "velocityX": -2.3636016137889215,
-            "velocityY": -1.4041744905880433,
-            "timestamp": 1.6911231396410644
-        },
-        {
-            "x": 2.4325478975720265,
-            "y": 4.8813111498736355,
-            "heading": -0.27927376577641677,
-            "angularVelocity": -0.3011083283987745,
-            "velocityX": -1.7577373054350616,
-            "velocityY": -2.0413085761341616,
-            "timestamp": 1.796818335868631
-        },
-        {
-            "x": 2.3348070320385017,
-            "y": 4.635496280912222,
-            "heading": -0.32770640318337707,
-            "angularVelocity": -0.4582293153861289,
-            "velocityX": -0.9247427416008974,
-            "velocityY": -2.3256957528340565,
-            "timestamp": 1.9025135320961974
-        },
-        {
-            "x": 2.305548232287739,
-            "y": 4.452786448669473,
-            "heading": -0.3683698320727586,
-            "angularVelocity": -0.3847235289845278,
-            "velocityX": -0.2768224176221525,
-            "velocityY": -1.72864840374928,
-            "timestamp": 2.008208728323764
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.2046466509288718,
-            "velocityX": -0.05249275734153,
-            "velocityY": -0.8778681716972112,
-            "timestamp": 2.1139039245513307
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 5.518495272475637e-32,
-            "velocityX": 0,
-            "velocityY": 3.7393231194786592e-34,
-            "timestamp": 2.219599120778897
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.1.traj
deleted file mode 100644
index 1bf739bf..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.1.traj	
+++ /dev/null
@@ -1,175 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -2.2542235108498663e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 2.538844153951763,
-            "heading": 2.1223723168001845e-18,
-            "angularVelocity": 2.1422107346760384e-17,
-            "velocityX": -4.524824041974102e-19,
-            "velocityY": 0.8260917454276265,
-            "timestamp": 0.09907392781100399
-        },
-        {
-            "x": 7.68,
-            "y": 2.70253245874193,
-            "heading": 6.368502121517421e-18,
-            "angularVelocity": 4.285819588010552e-17,
-            "velocityX": -9.048618887508432e-19,
-            "velocityY": 1.6521834594306501,
-            "timestamp": 0.19814785562200798
-        },
-        {
-            "x": 7.68,
-            "y": 2.9480649081437846,
-            "heading": 1.2741259045974704e-17,
-            "angularVelocity": 6.432324896396774e-17,
-            "velocityX": -1.3570355356096521e-18,
-            "velocityY": 2.47827511058448,
-            "timestamp": 0.297221783433012
-        },
-        {
-            "x": 7.68,
-            "y": 3.2754414834771923,
-            "heading": 2.1247069179422305e-17,
-            "angularVelocity": 8.585316360490521e-17,
-            "velocityX": -1.8085916683463265e-18,
-            "velocityY": 3.3043665731908773,
-            "timestamp": 0.39629571124401597
-        },
-        {
-            "x": 7.68,
-            "y": 3.6502322649266867,
-            "heading": 5.059469084858907e-18,
-            "angularVelocity": -1.633891019789118e-16,
-            "velocityX": -2.0715339797290713e-18,
-            "velocityY": 3.782940575087071,
-            "timestamp": 0.49536963905501996
-        },
-        {
-            "x": 7.68,
-            "y": 4.025023046376204,
-            "heading": -4.370025378852898e-19,
-            "angularVelocity": -5.5478487063848375e-17,
-            "velocityX": -2.0691441879135133e-18,
-            "velocityY": 3.7829405750873115,
-            "timestamp": 0.594443566866024
-        },
-        {
-            "x": 7.68,
-            "y": 4.3998138278257235,
-            "heading": 3.678666355048194e-17,
-            "angularVelocity": 3.7571606285132846e-16,
-            "velocityX": -2.072328507072934e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.693517494677028
-        },
-        {
-            "x": 7.68,
-            "y": 4.774604609275241,
-            "heading": 5.708604515927294e-17,
-            "angularVelocity": 2.04891257037182e-16,
-            "velocityX": -2.07516702935077e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.7925914224880319
-        },
-        {
-            "x": 7.68,
-            "y": 5.14939539072476,
-            "heading": 3.2711500565517004e-17,
-            "angularVelocity": -2.460238039643844e-16,
-            "velocityX": -2.078706211560206e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.8916653502990359
-        },
-        {
-            "x": 7.68,
-            "y": 5.524186172174278,
-            "heading": 3.4723638610714384e-17,
-            "angularVelocity": 2.0309460720999983e-17,
-            "velocityX": -2.0667219645048642e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.9907392781100399
-        },
-        {
-            "x": 7.68,
-            "y": 5.898976953623796,
-            "heading": 1.3963468905806427e-18,
-            "angularVelocity": -3.363881139719203e-16,
-            "velocityX": -2.078319856885538e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 1.089813205921044
-        },
-        {
-            "x": 7.68,
-            "y": 6.273767735073314,
-            "heading": -1.338725801529433e-17,
-            "angularVelocity": -1.492179146704526e-16,
-            "velocityX": -2.0709974384556896e-18,
-            "velocityY": 3.7829405750873115,
-            "timestamp": 1.188887133732048
-        },
-        {
-            "x": 7.68,
-            "y": 6.648558516522808,
-            "heading": -2.1247111946894708e-17,
-            "angularVelocity": -7.933322222365232e-17,
-            "velocityX": -2.070808166139798e-18,
-            "velocityY": 3.782940575087071,
-            "timestamp": 1.287961061543052
-        },
-        {
-            "x": 7.68,
-            "y": 6.975935091856216,
-            "heading": -1.273985947141283e-17,
-            "angularVelocity": 8.586772184549593e-17,
-            "velocityX": -1.8108827209778567e-18,
-            "velocityY": 3.304366573190878,
-            "timestamp": 1.387034989354056
-        },
-        {
-            "x": 7.68,
-            "y": 7.22146754125807,
-            "heading": -6.367750753434283e-18,
-            "angularVelocity": 6.431670631033216e-17,
-            "velocityX": -1.3581621409711223e-18,
-            "velocityY": 2.47827511058448,
-            "timestamp": 1.4861089171650599
-        },
-        {
-            "x": 7.68,
-            "y": 7.385155846048237,
-            "heading": -2.122107432521882e-18,
-            "angularVelocity": 4.285328556951628e-17,
-            "velocityX": -9.05441456368543e-19,
-            "velocityY": 1.65218345943065,
-            "timestamp": 1.5851828449760639
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 2.141943374454498e-17,
-            "velocityX": -4.527207369006345e-19,
-            "velocityY": 0.8260917454276265,
-            "timestamp": 1.6842567727870679
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -3.1368402443636883e-34,
-            "velocityY": 0,
-            "timestamp": 1.7833307005980719
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.traj
deleted file mode 100644
index 1bf739bf..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.traj	
+++ /dev/null
@@ -1,175 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -2.2542235108498663e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 2.538844153951763,
-            "heading": 2.1223723168001845e-18,
-            "angularVelocity": 2.1422107346760384e-17,
-            "velocityX": -4.524824041974102e-19,
-            "velocityY": 0.8260917454276265,
-            "timestamp": 0.09907392781100399
-        },
-        {
-            "x": 7.68,
-            "y": 2.70253245874193,
-            "heading": 6.368502121517421e-18,
-            "angularVelocity": 4.285819588010552e-17,
-            "velocityX": -9.048618887508432e-19,
-            "velocityY": 1.6521834594306501,
-            "timestamp": 0.19814785562200798
-        },
-        {
-            "x": 7.68,
-            "y": 2.9480649081437846,
-            "heading": 1.2741259045974704e-17,
-            "angularVelocity": 6.432324896396774e-17,
-            "velocityX": -1.3570355356096521e-18,
-            "velocityY": 2.47827511058448,
-            "timestamp": 0.297221783433012
-        },
-        {
-            "x": 7.68,
-            "y": 3.2754414834771923,
-            "heading": 2.1247069179422305e-17,
-            "angularVelocity": 8.585316360490521e-17,
-            "velocityX": -1.8085916683463265e-18,
-            "velocityY": 3.3043665731908773,
-            "timestamp": 0.39629571124401597
-        },
-        {
-            "x": 7.68,
-            "y": 3.6502322649266867,
-            "heading": 5.059469084858907e-18,
-            "angularVelocity": -1.633891019789118e-16,
-            "velocityX": -2.0715339797290713e-18,
-            "velocityY": 3.782940575087071,
-            "timestamp": 0.49536963905501996
-        },
-        {
-            "x": 7.68,
-            "y": 4.025023046376204,
-            "heading": -4.370025378852898e-19,
-            "angularVelocity": -5.5478487063848375e-17,
-            "velocityX": -2.0691441879135133e-18,
-            "velocityY": 3.7829405750873115,
-            "timestamp": 0.594443566866024
-        },
-        {
-            "x": 7.68,
-            "y": 4.3998138278257235,
-            "heading": 3.678666355048194e-17,
-            "angularVelocity": 3.7571606285132846e-16,
-            "velocityX": -2.072328507072934e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.693517494677028
-        },
-        {
-            "x": 7.68,
-            "y": 4.774604609275241,
-            "heading": 5.708604515927294e-17,
-            "angularVelocity": 2.04891257037182e-16,
-            "velocityX": -2.07516702935077e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.7925914224880319
-        },
-        {
-            "x": 7.68,
-            "y": 5.14939539072476,
-            "heading": 3.2711500565517004e-17,
-            "angularVelocity": -2.460238039643844e-16,
-            "velocityX": -2.078706211560206e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.8916653502990359
-        },
-        {
-            "x": 7.68,
-            "y": 5.524186172174278,
-            "heading": 3.4723638610714384e-17,
-            "angularVelocity": 2.0309460720999983e-17,
-            "velocityX": -2.0667219645048642e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.9907392781100399
-        },
-        {
-            "x": 7.68,
-            "y": 5.898976953623796,
-            "heading": 1.3963468905806427e-18,
-            "angularVelocity": -3.363881139719203e-16,
-            "velocityX": -2.078319856885538e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 1.089813205921044
-        },
-        {
-            "x": 7.68,
-            "y": 6.273767735073314,
-            "heading": -1.338725801529433e-17,
-            "angularVelocity": -1.492179146704526e-16,
-            "velocityX": -2.0709974384556896e-18,
-            "velocityY": 3.7829405750873115,
-            "timestamp": 1.188887133732048
-        },
-        {
-            "x": 7.68,
-            "y": 6.648558516522808,
-            "heading": -2.1247111946894708e-17,
-            "angularVelocity": -7.933322222365232e-17,
-            "velocityX": -2.070808166139798e-18,
-            "velocityY": 3.782940575087071,
-            "timestamp": 1.287961061543052
-        },
-        {
-            "x": 7.68,
-            "y": 6.975935091856216,
-            "heading": -1.273985947141283e-17,
-            "angularVelocity": 8.586772184549593e-17,
-            "velocityX": -1.8108827209778567e-18,
-            "velocityY": 3.304366573190878,
-            "timestamp": 1.387034989354056
-        },
-        {
-            "x": 7.68,
-            "y": 7.22146754125807,
-            "heading": -6.367750753434283e-18,
-            "angularVelocity": 6.431670631033216e-17,
-            "velocityX": -1.3581621409711223e-18,
-            "velocityY": 2.47827511058448,
-            "timestamp": 1.4861089171650599
-        },
-        {
-            "x": 7.68,
-            "y": 7.385155846048237,
-            "heading": -2.122107432521882e-18,
-            "angularVelocity": 4.285328556951628e-17,
-            "velocityX": -9.05441456368543e-19,
-            "velocityY": 1.65218345943065,
-            "timestamp": 1.5851828449760639
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 2.141943374454498e-17,
-            "velocityX": -4.527207369006345e-19,
-            "velocityY": 0.8260917454276265,
-            "timestamp": 1.6842567727870679
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -3.1368402443636883e-34,
-            "velocityY": 0,
-            "timestamp": 1.7833307005980719
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.1.traj
deleted file mode 100644
index 0dd47978..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.1.traj	
+++ /dev/null
@@ -1,139 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -1.7120676318191985e-39,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 2.5334453720055388,
-            "heading": -1.6066276958150233e-18,
-            "angularVelocity": -1.677931007769515e-17,
-            "velocityX": 1.4398812722194594e-33,
-            "velocityY": 0.7983807350946674,
-            "timestamp": 0.09575052183150976
-        },
-        {
-            "x": 7.68,
-            "y": 2.686336112982689,
-            "heading": -4.816833322743681e-18,
-            "angularVelocity": -3.3526769001814837e-17,
-            "velocityX": 1.5865173753469157e-33,
-            "velocityY": 1.5967614385035827,
-            "timestamp": 0.1915010436630195
-        },
-        {
-            "x": 7.68,
-            "y": 2.915672216863599,
-            "heading": -9.624489104750263e-18,
-            "angularVelocity": -5.0210230614541765e-17,
-            "velocityX": 1.0182538660036936e-34,
-            "velocityY": 2.395142078541023,
-            "timestamp": 0.28725156549452924
-        },
-        {
-            "x": 7.68,
-            "y": 3.221453665444745,
-            "heading": -1.6009153132358185e-17,
-            "angularVelocity": -6.66802008672839e-17,
-            "velocityX": 3.094783239015371e-34,
-            "velocityY": 3.193522528464369,
-            "timestamp": 0.383002087326039
-        },
-        {
-            "x": 7.68,
-            "y": 3.5836721992668203,
-            "heading": -3.473454086768081e-17,
-            "angularVelocity": -1.9556434134398854e-16,
-            "velocityX": 3.082893791022991e-33,
-            "velocityY": 3.7829405719528495,
-            "timestamp": 0.4787526091575488
-        },
-        {
-            "x": 7.68,
-            "y": 3.9458907330889397,
-            "heading": -3.402618832360637e-17,
-            "angularVelocity": 7.397897479044197e-18,
-            "velocityX": 3.015867889332214e-33,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.5745031309890586
-        },
-        {
-            "x": 7.68,
-            "y": 4.308109266911059,
-            "heading": -1.7375698441954866e-17,
-            "angularVelocity": 1.7389450796885638e-16,
-            "velocityX": 5.330140345912211e-33,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.6702536528205684
-        },
-        {
-            "x": 7.68,
-            "y": 4.670327800733179,
-            "heading": 7.579747606314133e-18,
-            "angularVelocity": 2.6062986990486198e-16,
-            "velocityX": 7.600730979130334e-33,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.7660041746520782
-        },
-        {
-            "x": 7.68,
-            "y": 5.032546334555254,
-            "heading": 1.6243735534665814e-17,
-            "angularVelocity": 9.048502047432954e-17,
-            "velocityX": -2.799411465702586e-33,
-            "velocityY": 3.7829405719528495,
-            "timestamp": 0.8617546964835879
-        },
-        {
-            "x": 7.68,
-            "y": 5.3383277831364,
-            "heading": 9.733952786983855e-18,
-            "angularVelocity": -6.798691664538921e-17,
-            "velocityX": -8.030123288193386e-33,
-            "velocityY": 3.193522528464369,
-            "timestamp": 0.9575052183150977
-        },
-        {
-            "x": 7.68,
-            "y": 5.567663887017311,
-            "heading": 4.864296415465134e-18,
-            "angularVelocity": -5.08577528116881e-17,
-            "velocityX": -5.179461626817983e-33,
-            "velocityY": 2.395142078541023,
-            "timestamp": 1.0532557401466074
-        },
-        {
-            "x": 7.68,
-            "y": 5.720554627994461,
-            "heading": 1.6208272352196832e-18,
-            "angularVelocity": -3.3874167136237696e-17,
-            "velocityX": -4.721587370568867e-33,
-            "velocityY": 1.5967614385035827,
-            "timestamp": 1.1490062619781172
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": -1.69276073284528e-17,
-            "velocityX": -2.4599412486527172e-33,
-            "velocityY": 0.7983807350946674,
-            "timestamp": 1.244756783809627
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -3.907521125363084e-43,
-            "velocityY": 8.448865646535191e-39,
-            "timestamp": 1.3405073056411367
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.traj
deleted file mode 100644
index 0dd47978..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.traj	
+++ /dev/null
@@ -1,139 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -1.7120676318191985e-39,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 2.5334453720055388,
-            "heading": -1.6066276958150233e-18,
-            "angularVelocity": -1.677931007769515e-17,
-            "velocityX": 1.4398812722194594e-33,
-            "velocityY": 0.7983807350946674,
-            "timestamp": 0.09575052183150976
-        },
-        {
-            "x": 7.68,
-            "y": 2.686336112982689,
-            "heading": -4.816833322743681e-18,
-            "angularVelocity": -3.3526769001814837e-17,
-            "velocityX": 1.5865173753469157e-33,
-            "velocityY": 1.5967614385035827,
-            "timestamp": 0.1915010436630195
-        },
-        {
-            "x": 7.68,
-            "y": 2.915672216863599,
-            "heading": -9.624489104750263e-18,
-            "angularVelocity": -5.0210230614541765e-17,
-            "velocityX": 1.0182538660036936e-34,
-            "velocityY": 2.395142078541023,
-            "timestamp": 0.28725156549452924
-        },
-        {
-            "x": 7.68,
-            "y": 3.221453665444745,
-            "heading": -1.6009153132358185e-17,
-            "angularVelocity": -6.66802008672839e-17,
-            "velocityX": 3.094783239015371e-34,
-            "velocityY": 3.193522528464369,
-            "timestamp": 0.383002087326039
-        },
-        {
-            "x": 7.68,
-            "y": 3.5836721992668203,
-            "heading": -3.473454086768081e-17,
-            "angularVelocity": -1.9556434134398854e-16,
-            "velocityX": 3.082893791022991e-33,
-            "velocityY": 3.7829405719528495,
-            "timestamp": 0.4787526091575488
-        },
-        {
-            "x": 7.68,
-            "y": 3.9458907330889397,
-            "heading": -3.402618832360637e-17,
-            "angularVelocity": 7.397897479044197e-18,
-            "velocityX": 3.015867889332214e-33,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.5745031309890586
-        },
-        {
-            "x": 7.68,
-            "y": 4.308109266911059,
-            "heading": -1.7375698441954866e-17,
-            "angularVelocity": 1.7389450796885638e-16,
-            "velocityX": 5.330140345912211e-33,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.6702536528205684
-        },
-        {
-            "x": 7.68,
-            "y": 4.670327800733179,
-            "heading": 7.579747606314133e-18,
-            "angularVelocity": 2.6062986990486198e-16,
-            "velocityX": 7.600730979130334e-33,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.7660041746520782
-        },
-        {
-            "x": 7.68,
-            "y": 5.032546334555254,
-            "heading": 1.6243735534665814e-17,
-            "angularVelocity": 9.048502047432954e-17,
-            "velocityX": -2.799411465702586e-33,
-            "velocityY": 3.7829405719528495,
-            "timestamp": 0.8617546964835879
-        },
-        {
-            "x": 7.68,
-            "y": 5.3383277831364,
-            "heading": 9.733952786983855e-18,
-            "angularVelocity": -6.798691664538921e-17,
-            "velocityX": -8.030123288193386e-33,
-            "velocityY": 3.193522528464369,
-            "timestamp": 0.9575052183150977
-        },
-        {
-            "x": 7.68,
-            "y": 5.567663887017311,
-            "heading": 4.864296415465134e-18,
-            "angularVelocity": -5.08577528116881e-17,
-            "velocityX": -5.179461626817983e-33,
-            "velocityY": 2.395142078541023,
-            "timestamp": 1.0532557401466074
-        },
-        {
-            "x": 7.68,
-            "y": 5.720554627994461,
-            "heading": 1.6208272352196832e-18,
-            "angularVelocity": -3.3874167136237696e-17,
-            "velocityX": -4.721587370568867e-33,
-            "velocityY": 1.5967614385035827,
-            "timestamp": 1.1490062619781172
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": -1.69276073284528e-17,
-            "velocityX": -2.4599412486527172e-33,
-            "velocityY": 0.7983807350946674,
-            "timestamp": 1.244756783809627
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -3.907521125363084e-43,
-            "velocityY": 8.448865646535191e-39,
-            "timestamp": 1.3405073056411367
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.1.traj
deleted file mode 100644
index 14b92c55..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.1.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -2.24426408200091e-39,
-            "angularVelocity": 3.072610310721988e-47,
-            "velocityX": -5.239815815905201e-40,
-            "velocityY": -4.5917748078995606e-40,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 2.540500005575846,
-            "heading": -7.160737227180709e-23,
-            "angularVelocity": -7.15564807370433e-22,
-            "velocityX": 1.97902852084548e-33,
-            "velocityY": 0.8344065481228216,
-            "timestamp": 0.10007112931184187
-        },
-        {
-            "x": 7.68,
-            "y": 2.7075000136298466,
-            "heading": 1.9234971876596208e-22,
-            "angularVelocity": 2.6376946581601568e-21,
-            "velocityX": 2.759296119129438e-33,
-            "velocityY": 1.6688130652907365,
-            "timestamp": 0.20014225862368373
-        },
-        {
-            "x": 7.68,
-            "y": 2.9580000179666155,
-            "heading": 1.2715262688933367e-21,
-            "angularVelocity": 1.0784094425251139e-20,
-            "velocityX": 3.360924516770788e-33,
-            "velocityY": 2.5032195205488357,
-            "timestamp": 0.3002133879355256
-        },
-        {
-            "x": 7.68,
-            "y": 3.2919999999999994,
-            "heading": 4.5990005279924634e-21,
-            "angularVelocity": 3.325109025879841e-20,
-            "velocityX": 4.212493187097162e-33,
-            "velocityY": 3.3376257900775026,
-            "timestamp": 0.40028451724736747
-        },
-        {
-            "x": 7.68,
-            "y": 3.6259999820333837,
-            "heading": 2.8774590551268706e-21,
-            "angularVelocity": -1.7203177491039364e-20,
-            "velocityX": -1.8223734723751632e-33,
-            "velocityY": 3.3376257900775026,
-            "timestamp": 0.5003556465592094
-        },
-        {
-            "x": 7.68,
-            "y": 3.8764999863701526,
-            "heading": 1.4966782173181084e-21,
-            "angularVelocity": -1.3797993485663648e-20,
-            "velocityX": -1.777649344678044e-33,
-            "velocityY": 2.5032195205488357,
-            "timestamp": 0.6004267758710513
-        },
-        {
-            "x": 7.68,
-            "y": 4.043499994424153,
-            "heading": 5.147844205579642e-22,
-            "angularVelocity": -9.81195850164629e-21,
-            "velocityX": -2.007686184897088e-33,
-            "velocityY": 1.6688130652907365,
-            "timestamp": 0.7004979051828932
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -4.8498876719579466e-42,
-            "angularVelocity": -5.144185056490002e-21,
-            "velocityX": -2.233495211598662e-33,
-            "velocityY": 0.8344065481228216,
-            "timestamp": 0.8005690344947352
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 9.449822208481667e-41,
-            "angularVelocity": 9.927610425813228e-40,
-            "velocityX": 1.9465664275137074e-41,
-            "velocityY": 5.142787784847508e-39,
-            "timestamp": 0.9006401638065771
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.traj
deleted file mode 100644
index 14b92c55..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -2.24426408200091e-39,
-            "angularVelocity": 3.072610310721988e-47,
-            "velocityX": -5.239815815905201e-40,
-            "velocityY": -4.5917748078995606e-40,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 2.540500005575846,
-            "heading": -7.160737227180709e-23,
-            "angularVelocity": -7.15564807370433e-22,
-            "velocityX": 1.97902852084548e-33,
-            "velocityY": 0.8344065481228216,
-            "timestamp": 0.10007112931184187
-        },
-        {
-            "x": 7.68,
-            "y": 2.7075000136298466,
-            "heading": 1.9234971876596208e-22,
-            "angularVelocity": 2.6376946581601568e-21,
-            "velocityX": 2.759296119129438e-33,
-            "velocityY": 1.6688130652907365,
-            "timestamp": 0.20014225862368373
-        },
-        {
-            "x": 7.68,
-            "y": 2.9580000179666155,
-            "heading": 1.2715262688933367e-21,
-            "angularVelocity": 1.0784094425251139e-20,
-            "velocityX": 3.360924516770788e-33,
-            "velocityY": 2.5032195205488357,
-            "timestamp": 0.3002133879355256
-        },
-        {
-            "x": 7.68,
-            "y": 3.2919999999999994,
-            "heading": 4.5990005279924634e-21,
-            "angularVelocity": 3.325109025879841e-20,
-            "velocityX": 4.212493187097162e-33,
-            "velocityY": 3.3376257900775026,
-            "timestamp": 0.40028451724736747
-        },
-        {
-            "x": 7.68,
-            "y": 3.6259999820333837,
-            "heading": 2.8774590551268706e-21,
-            "angularVelocity": -1.7203177491039364e-20,
-            "velocityX": -1.8223734723751632e-33,
-            "velocityY": 3.3376257900775026,
-            "timestamp": 0.5003556465592094
-        },
-        {
-            "x": 7.68,
-            "y": 3.8764999863701526,
-            "heading": 1.4966782173181084e-21,
-            "angularVelocity": -1.3797993485663648e-20,
-            "velocityX": -1.777649344678044e-33,
-            "velocityY": 2.5032195205488357,
-            "timestamp": 0.6004267758710513
-        },
-        {
-            "x": 7.68,
-            "y": 4.043499994424153,
-            "heading": 5.147844205579642e-22,
-            "angularVelocity": -9.81195850164629e-21,
-            "velocityX": -2.007686184897088e-33,
-            "velocityY": 1.6688130652907365,
-            "timestamp": 0.7004979051828932
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -4.8498876719579466e-42,
-            "angularVelocity": -5.144185056490002e-21,
-            "velocityX": -2.233495211598662e-33,
-            "velocityY": 0.8344065481228216,
-            "timestamp": 0.8005690344947352
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 9.449822208481667e-41,
-            "angularVelocity": 9.927610425813228e-40,
-            "velocityX": 1.9465664275137074e-41,
-            "velocityY": 5.142787784847508e-39,
-            "timestamp": 0.9006401638065771
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C5.1.traj
deleted file mode 100644
index 27f393e3..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C5.1.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -4.712717929333765e-40,
-            "angularVelocity": 9.927649781151023e-40,
-            "velocityX": 1.7772849076267516e-40,
-            "velocityY": -4.543888050052464e-40,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 2.3734999944241535,
-            "heading": 2.8227518572323466e-21,
-            "angularVelocity": 2.820745460648545e-20,
-            "velocityX": 2.2465911667965736e-32,
-            "velocityY": -0.8344065481228212,
-            "timestamp": 0.10007112931184181
-        },
-        {
-            "x": 7.68,
-            "y": 2.206499986370153,
-            "heading": 8.436925006375396e-21,
-            "angularVelocity": 5.6101826293588e-20,
-            "velocityX": 4.4654374072917263e-32,
-            "velocityY": -1.6688130652907358,
-            "timestamp": 0.20014225862368362
-        },
-        {
-            "x": 7.68,
-            "y": 1.955999982033384,
-            "heading": 1.682221146414935e-20,
-            "angularVelocity": 8.379326253074398e-20,
-            "velocityX": 6.797738711780288e-32,
-            "velocityY": -2.503219520548835,
-            "timestamp": 0.30021338793552543
-        },
-        {
-            "x": 7.68,
-            "y": 1.6219999999999999,
-            "heading": 2.78675063552584e-20,
-            "angularVelocity": 1.1037443968745167e-19,
-            "velocityX": 8.936946075377416e-32,
-            "velocityY": -3.3376257900775013,
-            "timestamp": 0.40028451724736724
-        },
-        {
-            "x": 7.68,
-            "y": 1.288000017966616,
-            "heading": 1.0287640204896551e-20,
-            "angularVelocity": -1.75673704816788e-19,
-            "velocityX": 9.060995032099377e-32,
-            "velocityY": -3.3376257900775013,
-            "timestamp": 0.500355646559209
-        },
-        {
-            "x": 7.68,
-            "y": 1.037500013629847,
-            "heading": 3.2969803204781124e-21,
-            "angularVelocity": -6.985690973056607e-20,
-            "velocityX": 6.916062716279199e-32,
-            "velocityY": -2.503219520548835,
-            "timestamp": 0.6004267758710509
-        },
-        {
-            "x": 7.68,
-            "y": 0.8705000055758465,
-            "heading": 6.18120991020044e-22,
-            "angularVelocity": -2.676955216055553e-20,
-            "velocityX": 4.34564572466329e-32,
-            "velocityY": -1.6688130652907358,
-            "timestamp": 0.7004979051828927
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -2.2737142324276756e-40,
-            "angularVelocity": -6.1768164103594875e-21,
-            "velocityX": 2.131975080632403e-32,
-            "velocityY": -0.8344065481228212,
-            "timestamp": 0.8005690344947345
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 2.0993519451368808e-41,
-            "angularVelocity": 2.4819025971975228e-39,
-            "velocityX": -8.732390511178075e-43,
-            "velocityY": -2.571393892423754e-39,
-            "timestamp": 0.9006401638065763
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C5.traj
deleted file mode 100644
index 27f393e3..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C5.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -4.712717929333765e-40,
-            "angularVelocity": 9.927649781151023e-40,
-            "velocityX": 1.7772849076267516e-40,
-            "velocityY": -4.543888050052464e-40,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 2.3734999944241535,
-            "heading": 2.8227518572323466e-21,
-            "angularVelocity": 2.820745460648545e-20,
-            "velocityX": 2.2465911667965736e-32,
-            "velocityY": -0.8344065481228212,
-            "timestamp": 0.10007112931184181
-        },
-        {
-            "x": 7.68,
-            "y": 2.206499986370153,
-            "heading": 8.436925006375396e-21,
-            "angularVelocity": 5.6101826293588e-20,
-            "velocityX": 4.4654374072917263e-32,
-            "velocityY": -1.6688130652907358,
-            "timestamp": 0.20014225862368362
-        },
-        {
-            "x": 7.68,
-            "y": 1.955999982033384,
-            "heading": 1.682221146414935e-20,
-            "angularVelocity": 8.379326253074398e-20,
-            "velocityX": 6.797738711780288e-32,
-            "velocityY": -2.503219520548835,
-            "timestamp": 0.30021338793552543
-        },
-        {
-            "x": 7.68,
-            "y": 1.6219999999999999,
-            "heading": 2.78675063552584e-20,
-            "angularVelocity": 1.1037443968745167e-19,
-            "velocityX": 8.936946075377416e-32,
-            "velocityY": -3.3376257900775013,
-            "timestamp": 0.40028451724736724
-        },
-        {
-            "x": 7.68,
-            "y": 1.288000017966616,
-            "heading": 1.0287640204896551e-20,
-            "angularVelocity": -1.75673704816788e-19,
-            "velocityX": 9.060995032099377e-32,
-            "velocityY": -3.3376257900775013,
-            "timestamp": 0.500355646559209
-        },
-        {
-            "x": 7.68,
-            "y": 1.037500013629847,
-            "heading": 3.2969803204781124e-21,
-            "angularVelocity": -6.985690973056607e-20,
-            "velocityX": 6.916062716279199e-32,
-            "velocityY": -2.503219520548835,
-            "timestamp": 0.6004267758710509
-        },
-        {
-            "x": 7.68,
-            "y": 0.8705000055758465,
-            "heading": 6.18120991020044e-22,
-            "angularVelocity": -2.676955216055553e-20,
-            "velocityX": 4.34564572466329e-32,
-            "velocityY": -1.6688130652907358,
-            "timestamp": 0.7004979051828927
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -2.2737142324276756e-40,
-            "angularVelocity": -6.1768164103594875e-21,
-            "velocityX": 2.131975080632403e-32,
-            "velocityY": -0.8344065481228212,
-            "timestamp": 0.8005690344947345
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 2.0993519451368808e-41,
-            "angularVelocity": 2.4819025971975228e-39,
-            "velocityX": -8.732390511178075e-43,
-            "velocityY": -2.571393892423754e-39,
-            "timestamp": 0.9006401638065763
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S1.1.traj
deleted file mode 100644
index d3b44a34..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S1.1.traj	
+++ /dev/null
@@ -1,256 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -3.908498355076501e-28,
-            "velocityX": -5.285170566576542e-31,
-            "velocityY": -1.5421992925545815e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 7.644672083885401,
-            "y": 2.5062543236054426,
-            "heading": 0.0003371503707738104,
-            "angularVelocity": 0.003954317681676867,
-            "velocityX": -0.4143486570570975,
-            "velocityY": 0.5776865743787781,
-            "timestamp": 0.08526132645270777
-        },
-        {
-            "x": 7.574016248997627,
-            "y": 2.604762966378743,
-            "heading": 0.0010114541718947692,
-            "angularVelocity": 0.00790867124800936,
-            "velocityX": -0.8286973452956875,
-            "velocityY": 1.1553730967106797,
-            "timestamp": 0.17052265290541554
-        },
-        {
-            "x": 7.4680324930779305,
-            "y": 2.7525259224839496,
-            "heading": 0.0020229127878395882,
-            "angularVelocity": 0.01186304105238048,
-            "velocityX": -1.243046060026343,
-            "velocityY": 1.73305955059478,
-            "timestamp": 0.2557839793581233
-        },
-        {
-            "x": 7.326720815099078,
-            "y": 2.949543182754876,
-            "heading": 0.0033715247016779517,
-            "angularVelocity": 0.015817393065211405,
-            "velocityX": -1.657394786805044,
-            "velocityY": 2.310745896971908,
-            "timestamp": 0.3410453058108311
-        },
-        {
-            "x": 7.1500812197312955,
-            "y": 3.1958147252550657,
-            "heading": 0.005057281636821727,
-            "angularVelocity": 0.01977164800486184,
-            "velocityX": -2.0717434588083226,
-            "velocityY": 2.8884319860640817,
-            "timestamp": 0.4263066322635389
-        },
-        {
-            "x": 6.957409028465344,
-            "y": 3.454481547786369,
-            "heading": 0.005057281852786698,
-            "angularVelocity": 2.532976909732762e-9,
-            "velocityX": -2.2597841164587846,
-            "velocityY": 3.0338118499096547,
-            "timestamp": 0.5115679587162466
-        },
-        {
-            "x": 6.719745009541024,
-            "y": 3.6725340903222867,
-            "heading": 0.005057282088140234,
-            "angularVelocity": 2.760378532567681e-9,
-            "velocityX": -2.787477380570894,
-            "velocityY": 2.5574612970252826,
-            "timestamp": 0.5968292851689544
-        },
-        {
-            "x": 6.445480997294438,
-            "y": 3.842270194716782,
-            "heading": 0.005057282389511224,
-            "angularVelocity": 3.534673960970524e-9,
-            "velocityX": -3.2167457821427474,
-            "velocityY": 1.9907748501725318,
-            "timestamp": 0.6820906116216622
-        },
-        {
-            "x": 6.1443030532206215,
-            "y": 3.9576952313720115,
-            "heading": 0.0050573451663417065,
-            "angularVelocity": 7.362872839766494e-7,
-            "velocityX": -3.5324097877003195,
-            "velocityY": 1.3537795089212774,
-            "timestamp": 0.76735193807437
-        },
-        {
-            "x": 5.8268478532983945,
-            "y": 4.0147328665402,
-            "heading": 0.005057406885810731,
-            "angularVelocity": 7.238858646916699e-7,
-            "velocityX": -3.723319975538169,
-            "velocityY": 0.6689742881224171,
-            "timestamp": 0.8526132645270778
-        },
-        {
-            "x": 5.505959202112108,
-            "y": 4.047314610372366,
-            "heading": 0.005057466820580134,
-            "angularVelocity": 7.029537528496964e-7,
-            "velocityX": -3.7635897133768776,
-            "velocityY": 0.3821397717793548,
-            "timestamp": 0.9378745909797855
-        },
-        {
-            "x": 5.196832806272751,
-            "y": 4.139357882135735,
-            "heading": 0.005057539347948128,
-            "angularVelocity": 8.506478964233103e-7,
-            "velocityX": -3.625634372587421,
-            "velocityY": 1.0795430424660248,
-            "timestamp": 1.0231359174324932
-        },
-        {
-            "x": 4.911616541791353,
-            "y": 4.287091740854739,
-            "heading": 0.008468262161445878,
-            "angularVelocity": 0.040003163864724633,
-            "velocityX": -3.3452008823671444,
-            "velocityY": 1.7327182776242007,
-            "timestamp": 1.108397243885201
-        },
-        {
-            "x": 4.672649683479067,
-            "y": 4.471709481894194,
-            "heading": 0.05890642847923189,
-            "angularVelocity": 0.5915714476464851,
-            "velocityX": -2.8027579238375626,
-            "velocityY": 2.165316313039838,
-            "timestamp": 1.1936585703379088
-        },
-        {
-            "x": 4.485112052095124,
-            "y": 4.685980588264911,
-            "heading": 0.1533943172973333,
-            "angularVelocity": 1.1082150929337078,
-            "velocityX": -2.1995626761432403,
-            "velocityY": 2.513110167122247,
-            "timestamp": 1.2789198967906166
-        },
-        {
-            "x": 4.353217601776123,
-            "y": 4.923890113830566,
-            "heading": 0.2635187752392858,
-            "angularVelocity": 1.291610892363634,
-            "velocityX": -1.5469434479432043,
-            "velocityY": 2.7903568413003317,
-            "timestamp": 1.3641812232433244
-        },
-        {
-            "x": 4.2800604900800625,
-            "y": 5.16873168779887,
-            "heading": 0.3704891219101591,
-            "angularVelocity": 1.3052114311350342,
-            "velocityX": -0.8926352155228909,
-            "velocityY": 2.987463639297507,
-            "timestamp": 1.4461375592962549
-        },
-        {
-            "x": 4.26199164107549,
-            "y": 5.42343977988949,
-            "heading": 0.46980573799025893,
-            "angularVelocity": 1.2118235253560785,
-            "velocityX": -0.22046921415441664,
-            "velocityY": 3.107851135829132,
-            "timestamp": 1.5280938953491854
-        },
-        {
-            "x": 4.2996600311730235,
-            "y": 5.679753559001703,
-            "heading": 0.550426603173402,
-            "angularVelocity": 0.9837050930723119,
-            "velocityX": 0.45961535021819666,
-            "velocityY": 3.127443117352554,
-            "timestamp": 1.610050231402116
-        },
-        {
-            "x": 4.391931306983731,
-            "y": 5.927283560440951,
-            "heading": 0.5996302346047028,
-            "angularVelocity": 0.6003639718722656,
-            "velocityX": 1.1258589665496648,
-            "velocityY": 3.0202668074301977,
-            "timestamp": 1.6920065674550464
-        },
-        {
-            "x": 4.533405002431738,
-            "y": 6.1528417215915425,
-            "heading": 0.5943564820218653,
-            "angularVelocity": -0.06434832053139185,
-            "velocityX": 1.7262081525536632,
-            "velocityY": 2.752174780055045,
-            "timestamp": 1.773962903507977
-        },
-        {
-            "x": 4.71098037645994,
-            "y": 6.338671203945781,
-            "heading": 0.5303639205613101,
-            "angularVelocity": -0.7808128638054942,
-            "velocityX": 2.166707085507742,
-            "velocityY": 2.2674205717817246,
-            "timestamp": 1.8559192395609074
-        },
-        {
-            "x": 4.8864330916758485,
-            "y": 6.469038507428809,
-            "heading": 0.43960837441640055,
-            "angularVelocity": -1.107364610427659,
-            "velocityX": 2.1408072110807552,
-            "velocityY": 1.5906921875921145,
-            "timestamp": 1.937875575613838
-        },
-        {
-            "x": 5.028345407892232,
-            "y": 6.554592883552385,
-            "heading": 0.3555321434700494,
-            "angularVelocity": -1.0258661501483803,
-            "velocityX": 1.7315600361235026,
-            "velocityY": 1.0439019146530064,
-            "timestamp": 2.0198319116667687
-        },
-        {
-            "x": 5.126173645194142,
-            "y": 6.6061448414321,
-            "heading": 0.29324170563915075,
-            "angularVelocity": -0.760044199524953,
-            "velocityX": 1.1936628943333356,
-            "velocityY": 0.6290173568329146,
-            "timestamp": 2.1017882477196994
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": -0.405602632332653,
-            "velocityX": 0.6079622053097948,
-            "velocityY": 0.29107155976928517,
-            "timestamp": 2.18374458377263
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 0,
-            "velocityX": 1.6766347007166246e-32,
-            "velocityY": 1.3162553729765107e-31,
-            "timestamp": 2.265700919825561
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S1.traj
deleted file mode 100644
index d3b44a34..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S1.traj	
+++ /dev/null
@@ -1,256 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -3.908498355076501e-28,
-            "velocityX": -5.285170566576542e-31,
-            "velocityY": -1.5421992925545815e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 7.644672083885401,
-            "y": 2.5062543236054426,
-            "heading": 0.0003371503707738104,
-            "angularVelocity": 0.003954317681676867,
-            "velocityX": -0.4143486570570975,
-            "velocityY": 0.5776865743787781,
-            "timestamp": 0.08526132645270777
-        },
-        {
-            "x": 7.574016248997627,
-            "y": 2.604762966378743,
-            "heading": 0.0010114541718947692,
-            "angularVelocity": 0.00790867124800936,
-            "velocityX": -0.8286973452956875,
-            "velocityY": 1.1553730967106797,
-            "timestamp": 0.17052265290541554
-        },
-        {
-            "x": 7.4680324930779305,
-            "y": 2.7525259224839496,
-            "heading": 0.0020229127878395882,
-            "angularVelocity": 0.01186304105238048,
-            "velocityX": -1.243046060026343,
-            "velocityY": 1.73305955059478,
-            "timestamp": 0.2557839793581233
-        },
-        {
-            "x": 7.326720815099078,
-            "y": 2.949543182754876,
-            "heading": 0.0033715247016779517,
-            "angularVelocity": 0.015817393065211405,
-            "velocityX": -1.657394786805044,
-            "velocityY": 2.310745896971908,
-            "timestamp": 0.3410453058108311
-        },
-        {
-            "x": 7.1500812197312955,
-            "y": 3.1958147252550657,
-            "heading": 0.005057281636821727,
-            "angularVelocity": 0.01977164800486184,
-            "velocityX": -2.0717434588083226,
-            "velocityY": 2.8884319860640817,
-            "timestamp": 0.4263066322635389
-        },
-        {
-            "x": 6.957409028465344,
-            "y": 3.454481547786369,
-            "heading": 0.005057281852786698,
-            "angularVelocity": 2.532976909732762e-9,
-            "velocityX": -2.2597841164587846,
-            "velocityY": 3.0338118499096547,
-            "timestamp": 0.5115679587162466
-        },
-        {
-            "x": 6.719745009541024,
-            "y": 3.6725340903222867,
-            "heading": 0.005057282088140234,
-            "angularVelocity": 2.760378532567681e-9,
-            "velocityX": -2.787477380570894,
-            "velocityY": 2.5574612970252826,
-            "timestamp": 0.5968292851689544
-        },
-        {
-            "x": 6.445480997294438,
-            "y": 3.842270194716782,
-            "heading": 0.005057282389511224,
-            "angularVelocity": 3.534673960970524e-9,
-            "velocityX": -3.2167457821427474,
-            "velocityY": 1.9907748501725318,
-            "timestamp": 0.6820906116216622
-        },
-        {
-            "x": 6.1443030532206215,
-            "y": 3.9576952313720115,
-            "heading": 0.0050573451663417065,
-            "angularVelocity": 7.362872839766494e-7,
-            "velocityX": -3.5324097877003195,
-            "velocityY": 1.3537795089212774,
-            "timestamp": 0.76735193807437
-        },
-        {
-            "x": 5.8268478532983945,
-            "y": 4.0147328665402,
-            "heading": 0.005057406885810731,
-            "angularVelocity": 7.238858646916699e-7,
-            "velocityX": -3.723319975538169,
-            "velocityY": 0.6689742881224171,
-            "timestamp": 0.8526132645270778
-        },
-        {
-            "x": 5.505959202112108,
-            "y": 4.047314610372366,
-            "heading": 0.005057466820580134,
-            "angularVelocity": 7.029537528496964e-7,
-            "velocityX": -3.7635897133768776,
-            "velocityY": 0.3821397717793548,
-            "timestamp": 0.9378745909797855
-        },
-        {
-            "x": 5.196832806272751,
-            "y": 4.139357882135735,
-            "heading": 0.005057539347948128,
-            "angularVelocity": 8.506478964233103e-7,
-            "velocityX": -3.625634372587421,
-            "velocityY": 1.0795430424660248,
-            "timestamp": 1.0231359174324932
-        },
-        {
-            "x": 4.911616541791353,
-            "y": 4.287091740854739,
-            "heading": 0.008468262161445878,
-            "angularVelocity": 0.040003163864724633,
-            "velocityX": -3.3452008823671444,
-            "velocityY": 1.7327182776242007,
-            "timestamp": 1.108397243885201
-        },
-        {
-            "x": 4.672649683479067,
-            "y": 4.471709481894194,
-            "heading": 0.05890642847923189,
-            "angularVelocity": 0.5915714476464851,
-            "velocityX": -2.8027579238375626,
-            "velocityY": 2.165316313039838,
-            "timestamp": 1.1936585703379088
-        },
-        {
-            "x": 4.485112052095124,
-            "y": 4.685980588264911,
-            "heading": 0.1533943172973333,
-            "angularVelocity": 1.1082150929337078,
-            "velocityX": -2.1995626761432403,
-            "velocityY": 2.513110167122247,
-            "timestamp": 1.2789198967906166
-        },
-        {
-            "x": 4.353217601776123,
-            "y": 4.923890113830566,
-            "heading": 0.2635187752392858,
-            "angularVelocity": 1.291610892363634,
-            "velocityX": -1.5469434479432043,
-            "velocityY": 2.7903568413003317,
-            "timestamp": 1.3641812232433244
-        },
-        {
-            "x": 4.2800604900800625,
-            "y": 5.16873168779887,
-            "heading": 0.3704891219101591,
-            "angularVelocity": 1.3052114311350342,
-            "velocityX": -0.8926352155228909,
-            "velocityY": 2.987463639297507,
-            "timestamp": 1.4461375592962549
-        },
-        {
-            "x": 4.26199164107549,
-            "y": 5.42343977988949,
-            "heading": 0.46980573799025893,
-            "angularVelocity": 1.2118235253560785,
-            "velocityX": -0.22046921415441664,
-            "velocityY": 3.107851135829132,
-            "timestamp": 1.5280938953491854
-        },
-        {
-            "x": 4.2996600311730235,
-            "y": 5.679753559001703,
-            "heading": 0.550426603173402,
-            "angularVelocity": 0.9837050930723119,
-            "velocityX": 0.45961535021819666,
-            "velocityY": 3.127443117352554,
-            "timestamp": 1.610050231402116
-        },
-        {
-            "x": 4.391931306983731,
-            "y": 5.927283560440951,
-            "heading": 0.5996302346047028,
-            "angularVelocity": 0.6003639718722656,
-            "velocityX": 1.1258589665496648,
-            "velocityY": 3.0202668074301977,
-            "timestamp": 1.6920065674550464
-        },
-        {
-            "x": 4.533405002431738,
-            "y": 6.1528417215915425,
-            "heading": 0.5943564820218653,
-            "angularVelocity": -0.06434832053139185,
-            "velocityX": 1.7262081525536632,
-            "velocityY": 2.752174780055045,
-            "timestamp": 1.773962903507977
-        },
-        {
-            "x": 4.71098037645994,
-            "y": 6.338671203945781,
-            "heading": 0.5303639205613101,
-            "angularVelocity": -0.7808128638054942,
-            "velocityX": 2.166707085507742,
-            "velocityY": 2.2674205717817246,
-            "timestamp": 1.8559192395609074
-        },
-        {
-            "x": 4.8864330916758485,
-            "y": 6.469038507428809,
-            "heading": 0.43960837441640055,
-            "angularVelocity": -1.107364610427659,
-            "velocityX": 2.1408072110807552,
-            "velocityY": 1.5906921875921145,
-            "timestamp": 1.937875575613838
-        },
-        {
-            "x": 5.028345407892232,
-            "y": 6.554592883552385,
-            "heading": 0.3555321434700494,
-            "angularVelocity": -1.0258661501483803,
-            "velocityX": 1.7315600361235026,
-            "velocityY": 1.0439019146530064,
-            "timestamp": 2.0198319116667687
-        },
-        {
-            "x": 5.126173645194142,
-            "y": 6.6061448414321,
-            "heading": 0.29324170563915075,
-            "angularVelocity": -0.760044199524953,
-            "velocityX": 1.1936628943333356,
-            "velocityY": 0.6290173568329146,
-            "timestamp": 2.1017882477196994
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": -0.405602632332653,
-            "velocityX": 0.6079622053097948,
-            "velocityY": 0.29107155976928517,
-            "timestamp": 2.18374458377263
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 0,
-            "velocityX": 1.6766347007166246e-32,
-            "velocityY": 1.3162553729765107e-31,
-            "timestamp": 2.265700919825561
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S2.1.traj
deleted file mode 100644
index 3bd7e899..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S2.1.traj	
+++ /dev/null
@@ -1,211 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -1.8537798939082814e-32,
-            "velocityX": 2.7477856809061307e-34,
-            "velocityY": 6.925951302123011e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 7.6474171775045905,
-            "y": 2.4907449431726314,
-            "heading": -0.04287864331544942,
-            "angularVelocity": -0.5625230567182208,
-            "velocityX": -0.42745263118016213,
-            "velocityY": 0.4426984418000905,
-            "timestamp": 0.07622557476240162
-        },
-        {
-            "x": 7.582037622461459,
-            "y": 2.5583409566544844,
-            "heading": -0.12553871024043178,
-            "angularVelocity": -1.084413822823079,
-            "velocityX": -0.8577115390329612,
-            "velocityY": 0.8867891608892778,
-            "timestamp": 0.15245114952480324
-        },
-        {
-            "x": 7.483606625879984,
-            "y": 2.659997722482693,
-            "heading": -0.2429013405408753,
-            "angularVelocity": -1.5396752424139517,
-            "velocityX": -1.2913119630555505,
-            "velocityY": 1.333630689503849,
-            "timestamp": 0.22867672428720487
-        },
-        {
-            "x": 7.3517522665520065,
-            "y": 2.7960803575645476,
-            "heading": -0.3842395530561246,
-            "angularVelocity": -1.8542098627108627,
-            "velocityX": -1.7297916052318956,
-            "velocityY": 1.785262170944986,
-            "timestamp": 0.3049022990496065
-        },
-        {
-            "x": 7.186045392769314,
-            "y": 2.9668045775727996,
-            "heading": -0.5218107815453775,
-            "angularVelocity": -1.8047909631126855,
-            "velocityX": -2.173901270002986,
-            "velocityY": 2.2397236169147496,
-            "timestamp": 0.3811278738120081
-        },
-        {
-            "x": 6.990773522032128,
-            "y": 3.1645972581511623,
-            "heading": -0.5514162056749212,
-            "angularVelocity": -0.38839227151549954,
-            "velocityX": -2.5617631791673237,
-            "velocityY": 2.59483357383517,
-            "timestamp": 0.4573534485744097
-        },
-        {
-            "x": 6.791088913724043,
-            "y": 3.3726253897030523,
-            "heading": -0.5514162553808045,
-            "angularVelocity": -6.520893213469087e-7,
-            "velocityX": -2.619653691435051,
-            "velocityY": 2.729112009982516,
-            "timestamp": 0.5335790233368113
-        },
-        {
-            "x": 6.578307530314685,
-            "y": 3.567237130643777,
-            "heading": -0.5514163004289083,
-            "angularVelocity": -5.90984115130558e-7,
-            "velocityX": -2.7914697143656473,
-            "velocityY": 2.55310296507882,
-            "timestamp": 0.609804598099213
-        },
-        {
-            "x": 6.335948512349157,
-            "y": 3.7234797461898013,
-            "heading": -0.551416294920752,
-            "angularVelocity": 7.226126339733625e-8,
-            "velocityX": -3.179497415676709,
-            "velocityY": 2.0497400778287207,
-            "timestamp": 0.6860301728616146
-        },
-        {
-            "x": 6.088743524617576,
-            "y": 3.871936280628758,
-            "heading": -0.5514162894475348,
-            "angularVelocity": 7.180289728327137e-8,
-            "velocityX": -3.2430714822699573,
-            "velocityY": 1.9475948184280927,
-            "timestamp": 0.7622557476240163
-        },
-        {
-            "x": 5.841537785943911,
-            "y": 4.020391564616265,
-            "heading": -0.5514162839746023,
-            "angularVelocity": 7.179916458555982e-8,
-            "velocityX": -3.2430813338464923,
-            "velocityY": 1.947578413809903,
-            "timestamp": 0.8384813223864179
-        },
-        {
-            "x": 5.581140041351318,
-            "y": 4.144257068634033,
-            "heading": -0.5514162991479159,
-            "angularVelocity": -1.9905804152585698e-7,
-            "velocityX": -3.416146685731968,
-            "velocityY": 1.6249861598795787,
-            "timestamp": 0.9147068971488196
-        },
-        {
-            "x": 5.261335999197585,
-            "y": 4.231658536911035,
-            "heading": -0.5514164840698423,
-            "angularVelocity": -0.000002110046686592666,
-            "velocityX": -3.6491154543506057,
-            "velocityY": 0.9972921119903242,
-            "timestamp": 1.0023456814389826
-        },
-        {
-            "x": 4.96430123021485,
-            "y": 4.378912897735688,
-            "heading": -0.551417377431188,
-            "angularVelocity": -0.000010193675698949444,
-            "velocityX": -3.3893072728996803,
-            "velocityY": 1.6802419387415284,
-            "timestamp": 1.0899844657291458
-        },
-        {
-            "x": 4.714895393240272,
-            "y": 4.564810737884653,
-            "heading": -0.6189140340648516,
-            "angularVelocity": -0.7701687920520328,
-            "velocityX": -2.8458386203626582,
-            "velocityY": 2.121182324180531,
-            "timestamp": 1.177623250019309
-        },
-        {
-            "x": 4.521287798442847,
-            "y": 4.7820097530438215,
-            "heading": -0.6965700023585185,
-            "angularVelocity": -0.8860913455457793,
-            "velocityX": -2.209154273026079,
-            "velocityY": 2.4783435429688825,
-            "timestamp": 1.265262034309472
-        },
-        {
-            "x": 4.386338131009024,
-            "y": 5.001818806087968,
-            "heading": -0.6848521841568085,
-            "angularVelocity": 0.1337058506301677,
-            "velocityX": -1.539839564490294,
-            "velocityY": 2.5081253103235808,
-            "timestamp": 1.3529008185996352
-        },
-        {
-            "x": 4.301033328473477,
-            "y": 5.211510806429668,
-            "heading": -0.519384386353178,
-            "angularVelocity": 1.888065873389841,
-            "velocityX": -0.9733681637243018,
-            "velocityY": 2.3926849515327757,
-            "timestamp": 1.4405396028897983
-        },
-        {
-            "x": 4.267176527868507,
-            "y": 5.385808771912563,
-            "heading": -0.30251041793580363,
-            "angularVelocity": 2.474634605830756,
-            "velocityX": -0.38632211616348305,
-            "velocityY": 1.988822265103685,
-            "timestamp": 1.5281783871799615
-        },
-        {
-            "x": 4.261077190237836,
-            "y": 5.502861748658668,
-            "heading": -0.11181365176971161,
-            "angularVelocity": 2.175940340919318,
-            "velocityX": -0.06959632861263781,
-            "velocityY": 1.335629854911658,
-            "timestamp": 1.6158171714701246
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 9.015876776981611e-33,
-            "angularVelocity": 1.275846677647969,
-            "velocityX": 0.021940169272516404,
-            "velocityY": 0.6519744860010044,
-            "timestamp": 1.7034559557602877
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -9.923717904588528e-32,
-            "velocityX": -3.152679648787722e-32,
-            "velocityY": -1.9738855170003778e-32,
-            "timestamp": 1.7910947400504509
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S2.traj
deleted file mode 100644
index 3bd7e899..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S2.traj	
+++ /dev/null
@@ -1,211 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -1.8537798939082814e-32,
-            "velocityX": 2.7477856809061307e-34,
-            "velocityY": 6.925951302123011e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 7.6474171775045905,
-            "y": 2.4907449431726314,
-            "heading": -0.04287864331544942,
-            "angularVelocity": -0.5625230567182208,
-            "velocityX": -0.42745263118016213,
-            "velocityY": 0.4426984418000905,
-            "timestamp": 0.07622557476240162
-        },
-        {
-            "x": 7.582037622461459,
-            "y": 2.5583409566544844,
-            "heading": -0.12553871024043178,
-            "angularVelocity": -1.084413822823079,
-            "velocityX": -0.8577115390329612,
-            "velocityY": 0.8867891608892778,
-            "timestamp": 0.15245114952480324
-        },
-        {
-            "x": 7.483606625879984,
-            "y": 2.659997722482693,
-            "heading": -0.2429013405408753,
-            "angularVelocity": -1.5396752424139517,
-            "velocityX": -1.2913119630555505,
-            "velocityY": 1.333630689503849,
-            "timestamp": 0.22867672428720487
-        },
-        {
-            "x": 7.3517522665520065,
-            "y": 2.7960803575645476,
-            "heading": -0.3842395530561246,
-            "angularVelocity": -1.8542098627108627,
-            "velocityX": -1.7297916052318956,
-            "velocityY": 1.785262170944986,
-            "timestamp": 0.3049022990496065
-        },
-        {
-            "x": 7.186045392769314,
-            "y": 2.9668045775727996,
-            "heading": -0.5218107815453775,
-            "angularVelocity": -1.8047909631126855,
-            "velocityX": -2.173901270002986,
-            "velocityY": 2.2397236169147496,
-            "timestamp": 0.3811278738120081
-        },
-        {
-            "x": 6.990773522032128,
-            "y": 3.1645972581511623,
-            "heading": -0.5514162056749212,
-            "angularVelocity": -0.38839227151549954,
-            "velocityX": -2.5617631791673237,
-            "velocityY": 2.59483357383517,
-            "timestamp": 0.4573534485744097
-        },
-        {
-            "x": 6.791088913724043,
-            "y": 3.3726253897030523,
-            "heading": -0.5514162553808045,
-            "angularVelocity": -6.520893213469087e-7,
-            "velocityX": -2.619653691435051,
-            "velocityY": 2.729112009982516,
-            "timestamp": 0.5335790233368113
-        },
-        {
-            "x": 6.578307530314685,
-            "y": 3.567237130643777,
-            "heading": -0.5514163004289083,
-            "angularVelocity": -5.90984115130558e-7,
-            "velocityX": -2.7914697143656473,
-            "velocityY": 2.55310296507882,
-            "timestamp": 0.609804598099213
-        },
-        {
-            "x": 6.335948512349157,
-            "y": 3.7234797461898013,
-            "heading": -0.551416294920752,
-            "angularVelocity": 7.226126339733625e-8,
-            "velocityX": -3.179497415676709,
-            "velocityY": 2.0497400778287207,
-            "timestamp": 0.6860301728616146
-        },
-        {
-            "x": 6.088743524617576,
-            "y": 3.871936280628758,
-            "heading": -0.5514162894475348,
-            "angularVelocity": 7.180289728327137e-8,
-            "velocityX": -3.2430714822699573,
-            "velocityY": 1.9475948184280927,
-            "timestamp": 0.7622557476240163
-        },
-        {
-            "x": 5.841537785943911,
-            "y": 4.020391564616265,
-            "heading": -0.5514162839746023,
-            "angularVelocity": 7.179916458555982e-8,
-            "velocityX": -3.2430813338464923,
-            "velocityY": 1.947578413809903,
-            "timestamp": 0.8384813223864179
-        },
-        {
-            "x": 5.581140041351318,
-            "y": 4.144257068634033,
-            "heading": -0.5514162991479159,
-            "angularVelocity": -1.9905804152585698e-7,
-            "velocityX": -3.416146685731968,
-            "velocityY": 1.6249861598795787,
-            "timestamp": 0.9147068971488196
-        },
-        {
-            "x": 5.261335999197585,
-            "y": 4.231658536911035,
-            "heading": -0.5514164840698423,
-            "angularVelocity": -0.000002110046686592666,
-            "velocityX": -3.6491154543506057,
-            "velocityY": 0.9972921119903242,
-            "timestamp": 1.0023456814389826
-        },
-        {
-            "x": 4.96430123021485,
-            "y": 4.378912897735688,
-            "heading": -0.551417377431188,
-            "angularVelocity": -0.000010193675698949444,
-            "velocityX": -3.3893072728996803,
-            "velocityY": 1.6802419387415284,
-            "timestamp": 1.0899844657291458
-        },
-        {
-            "x": 4.714895393240272,
-            "y": 4.564810737884653,
-            "heading": -0.6189140340648516,
-            "angularVelocity": -0.7701687920520328,
-            "velocityX": -2.8458386203626582,
-            "velocityY": 2.121182324180531,
-            "timestamp": 1.177623250019309
-        },
-        {
-            "x": 4.521287798442847,
-            "y": 4.7820097530438215,
-            "heading": -0.6965700023585185,
-            "angularVelocity": -0.8860913455457793,
-            "velocityX": -2.209154273026079,
-            "velocityY": 2.4783435429688825,
-            "timestamp": 1.265262034309472
-        },
-        {
-            "x": 4.386338131009024,
-            "y": 5.001818806087968,
-            "heading": -0.6848521841568085,
-            "angularVelocity": 0.1337058506301677,
-            "velocityX": -1.539839564490294,
-            "velocityY": 2.5081253103235808,
-            "timestamp": 1.3529008185996352
-        },
-        {
-            "x": 4.301033328473477,
-            "y": 5.211510806429668,
-            "heading": -0.519384386353178,
-            "angularVelocity": 1.888065873389841,
-            "velocityX": -0.9733681637243018,
-            "velocityY": 2.3926849515327757,
-            "timestamp": 1.4405396028897983
-        },
-        {
-            "x": 4.267176527868507,
-            "y": 5.385808771912563,
-            "heading": -0.30251041793580363,
-            "angularVelocity": 2.474634605830756,
-            "velocityX": -0.38632211616348305,
-            "velocityY": 1.988822265103685,
-            "timestamp": 1.5281783871799615
-        },
-        {
-            "x": 4.261077190237836,
-            "y": 5.502861748658668,
-            "heading": -0.11181365176971161,
-            "angularVelocity": 2.175940340919318,
-            "velocityX": -0.06959632861263781,
-            "velocityY": 1.335629854911658,
-            "timestamp": 1.6158171714701246
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 9.015876776981611e-33,
-            "angularVelocity": 1.275846677647969,
-            "velocityX": 0.021940169272516404,
-            "velocityY": 0.6519744860010044,
-            "timestamp": 1.7034559557602877
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -9.923717904588528e-32,
-            "velocityX": -3.152679648787722e-32,
-            "velocityY": -1.9738855170003778e-32,
-            "timestamp": 1.7910947400504509
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S3.1.traj
deleted file mode 100644
index 80fc230a..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S3.1.traj	
+++ /dev/null
@@ -1,229 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 7.203126847267154e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.6459509070752,
-            "y": 2.420050479995149,
-            "heading": -0.026297254997510774,
-            "angularVelocity": -0.33695614983619343,
-            "velocityX": -0.4362832264603014,
-            "velocityY": -0.47344743777665743,
-            "timestamp": 0.078043552581802
-        },
-        {
-            "x": 7.57752123433784,
-            "y": 2.3463325784745406,
-            "heading": -0.07655763531528098,
-            "angularVelocity": -0.6440042598660941,
-            "velocityX": -0.8768139131756897,
-            "velocityY": -0.9445738831958564,
-            "timestamp": 0.156087105163604
-        },
-        {
-            "x": 7.474195561438856,
-            "y": 2.236153787211583,
-            "heading": -0.14701004173730176,
-            "angularVelocity": -0.9027319245646475,
-            "velocityX": -1.3239488654839289,
-            "velocityY": -1.4117603263569152,
-            "timestamp": 0.23413065774540598
-        },
-        {
-            "x": 7.33507656702923,
-            "y": 2.0901340736402396,
-            "heading": -0.23055657416165956,
-            "angularVelocity": -1.0705116522834195,
-            "velocityX": -1.7825815177216175,
-            "velocityY": -1.8710029046700958,
-            "timestamp": 0.312174210327208
-        },
-        {
-            "x": 7.158294386463989,
-            "y": 1.910067298253472,
-            "heading": -0.30975533817585227,
-            "angularVelocity": -1.014802137964151,
-            "velocityX": -2.2651734155741057,
-            "velocityY": -2.3072601057983104,
-            "timestamp": 0.39021776290901
-        },
-        {
-            "x": 6.940360292129115,
-            "y": 1.7108993896694566,
-            "heading": -0.30975560484167247,
-            "angularVelocity": -0.00000341688469228089,
-            "velocityX": -2.7924676302559823,
-            "velocityY": -2.552009768843728,
-            "timestamp": 0.468261315490812
-        },
-        {
-            "x": 6.6915168927330235,
-            "y": 1.5520279357314815,
-            "heading": -0.30975564017638474,
-            "angularVelocity": -4.5275632793576304e-7,
-            "velocityX": -3.1885196299240257,
-            "velocityY": -2.035676858398594,
-            "timestamp": 0.546304868072614
-        },
-        {
-            "x": 6.419127595403782,
-            "y": 1.43815413781978,
-            "heading": -0.30975568823518523,
-            "angularVelocity": -6.157946287917162e-7,
-            "velocityX": -3.490221655962278,
-            "velocityY": -1.4591057703627237,
-            "timestamp": 0.624348420654416
-        },
-        {
-            "x": 6.131252530334066,
-            "y": 1.37264761019134,
-            "heading": -0.30975577587401576,
-            "angularVelocity": -0.0000011229477336220622,
-            "velocityX": -3.6886463461279972,
-            "velocityY": -0.8393586076157934,
-            "timestamp": 0.7023919732362179
-        },
-        {
-            "x": 5.83657654735769,
-            "y": 1.3574693553002635,
-            "heading": -0.31020540722452916,
-            "angularVelocity": -0.005761287587978277,
-            "velocityX": -3.7757889438387497,
-            "velocityY": -0.1944844178532558,
-            "timestamp": 0.7804355258180199
-        },
-        {
-            "x": 5.546474933624268,
-            "y": 1.3928169012069702,
-            "heading": -0.3189695864044355,
-            "angularVelocity": -0.11229856778804584,
-            "velocityX": -3.717175911864151,
-            "velocityY": 0.4529207697160109,
-            "timestamp": 0.8584790783998218
-        },
-        {
-            "x": 5.303276771996363,
-            "y": 1.4607318697473424,
-            "heading": -0.3307229091538368,
-            "angularVelocity": -0.17360472172913416,
-            "velocityX": -3.5922053767043294,
-            "velocityY": 1.0031511484968414,
-            "timestamp": 0.9261807090485242
-        },
-        {
-            "x": 5.073563214559545,
-            "y": 1.564404927734292,
-            "heading": -0.34378229938421223,
-            "angularVelocity": -0.1928962434919207,
-            "velocityX": -3.3930284283517023,
-            "velocityY": 1.5313229089691542,
-            "timestamp": 0.9938823396972266
-        },
-        {
-            "x": 4.862974125111252,
-            "y": 1.7011642845126724,
-            "heading": -0.35827284529055603,
-            "angularVelocity": -0.2140354045770704,
-            "velocityX": -3.1105467834446325,
-            "velocityY": 2.020030469989471,
-            "timestamp": 1.0615839703459289
-        },
-        {
-            "x": 4.677671719496688,
-            "y": 1.8665243414779629,
-            "heading": -0.3791415933598452,
-            "angularVelocity": -0.30824587043792007,
-            "velocityX": -2.737044940262413,
-            "velocityY": 2.442482631227878,
-            "timestamp": 1.1292856009946313
-        },
-        {
-            "x": 4.523629451068858,
-            "y": 2.053477766497112,
-            "heading": -0.41478083919993697,
-            "angularVelocity": -0.5264163580502632,
-            "velocityX": -2.27531105162187,
-            "velocityY": 2.761431640977936,
-            "timestamp": 1.1969872316433336
-        },
-        {
-            "x": 4.406611773563404,
-            "y": 2.2498215105595722,
-            "heading": -0.45494733767836987,
-            "angularVelocity": -0.5932870167770028,
-            "velocityX": -1.7284321867022447,
-            "velocityY": 2.900133160474012,
-            "timestamp": 1.264688862292036
-        },
-        {
-            "x": 4.327688857963163,
-            "y": 2.4438365235550945,
-            "heading": -0.48799769437613305,
-            "angularVelocity": -0.4881766713911416,
-            "velocityX": -1.1657461547677304,
-            "velocityY": 2.865736188870776,
-            "timestamp": 1.3323904929407384
-        },
-        {
-            "x": 4.28360260298108,
-            "y": 2.622213717107768,
-            "heading": -0.5154307830643909,
-            "angularVelocity": -0.40520573028252516,
-            "velocityX": -0.6511845366152605,
-            "velocityY": 2.634754759132393,
-            "timestamp": 1.4000921235894408
-        },
-        {
-            "x": 4.264391797811532,
-            "y": 2.7716303897908317,
-            "heading": -0.5365689504315967,
-            "angularVelocity": -0.3122253801676767,
-            "velocityX": -0.283756904899847,
-            "velocityY": 2.2069877970647664,
-            "timestamp": 1.4677937542381432
-        },
-        {
-            "x": 4.259328393836262,
-            "y": 2.8855840206662466,
-            "heading": -0.5517027167443236,
-            "angularVelocity": -0.22353621571242488,
-            "velocityX": -0.07478998551086755,
-            "velocityY": 1.6831740946789628,
-            "timestamp": 1.5354953848868456
-        },
-        {
-            "x": 4.260599830778067,
-            "y": 2.9618797181043535,
-            "heading": -0.5613571988137664,
-            "angularVelocity": -0.1426033904496253,
-            "velocityX": 0.018780004700190214,
-            "velocityY": 1.1269403219251917,
-            "timestamp": 1.603197015535548
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -0.0685773908510417,
-            "velocityX": 0.035452162657366115,
-            "velocityY": 0.5630629798778553,
-            "timestamp": 1.6708986461842503
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 0,
-            "velocityX": 1.1764020723168042e-36,
-            "velocityY": 0,
-            "timestamp": 1.7386002768329527
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S3.traj
deleted file mode 100644
index 80fc230a..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S3.traj	
+++ /dev/null
@@ -1,229 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 7.203126847267154e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.6459509070752,
-            "y": 2.420050479995149,
-            "heading": -0.026297254997510774,
-            "angularVelocity": -0.33695614983619343,
-            "velocityX": -0.4362832264603014,
-            "velocityY": -0.47344743777665743,
-            "timestamp": 0.078043552581802
-        },
-        {
-            "x": 7.57752123433784,
-            "y": 2.3463325784745406,
-            "heading": -0.07655763531528098,
-            "angularVelocity": -0.6440042598660941,
-            "velocityX": -0.8768139131756897,
-            "velocityY": -0.9445738831958564,
-            "timestamp": 0.156087105163604
-        },
-        {
-            "x": 7.474195561438856,
-            "y": 2.236153787211583,
-            "heading": -0.14701004173730176,
-            "angularVelocity": -0.9027319245646475,
-            "velocityX": -1.3239488654839289,
-            "velocityY": -1.4117603263569152,
-            "timestamp": 0.23413065774540598
-        },
-        {
-            "x": 7.33507656702923,
-            "y": 2.0901340736402396,
-            "heading": -0.23055657416165956,
-            "angularVelocity": -1.0705116522834195,
-            "velocityX": -1.7825815177216175,
-            "velocityY": -1.8710029046700958,
-            "timestamp": 0.312174210327208
-        },
-        {
-            "x": 7.158294386463989,
-            "y": 1.910067298253472,
-            "heading": -0.30975533817585227,
-            "angularVelocity": -1.014802137964151,
-            "velocityX": -2.2651734155741057,
-            "velocityY": -2.3072601057983104,
-            "timestamp": 0.39021776290901
-        },
-        {
-            "x": 6.940360292129115,
-            "y": 1.7108993896694566,
-            "heading": -0.30975560484167247,
-            "angularVelocity": -0.00000341688469228089,
-            "velocityX": -2.7924676302559823,
-            "velocityY": -2.552009768843728,
-            "timestamp": 0.468261315490812
-        },
-        {
-            "x": 6.6915168927330235,
-            "y": 1.5520279357314815,
-            "heading": -0.30975564017638474,
-            "angularVelocity": -4.5275632793576304e-7,
-            "velocityX": -3.1885196299240257,
-            "velocityY": -2.035676858398594,
-            "timestamp": 0.546304868072614
-        },
-        {
-            "x": 6.419127595403782,
-            "y": 1.43815413781978,
-            "heading": -0.30975568823518523,
-            "angularVelocity": -6.157946287917162e-7,
-            "velocityX": -3.490221655962278,
-            "velocityY": -1.4591057703627237,
-            "timestamp": 0.624348420654416
-        },
-        {
-            "x": 6.131252530334066,
-            "y": 1.37264761019134,
-            "heading": -0.30975577587401576,
-            "angularVelocity": -0.0000011229477336220622,
-            "velocityX": -3.6886463461279972,
-            "velocityY": -0.8393586076157934,
-            "timestamp": 0.7023919732362179
-        },
-        {
-            "x": 5.83657654735769,
-            "y": 1.3574693553002635,
-            "heading": -0.31020540722452916,
-            "angularVelocity": -0.005761287587978277,
-            "velocityX": -3.7757889438387497,
-            "velocityY": -0.1944844178532558,
-            "timestamp": 0.7804355258180199
-        },
-        {
-            "x": 5.546474933624268,
-            "y": 1.3928169012069702,
-            "heading": -0.3189695864044355,
-            "angularVelocity": -0.11229856778804584,
-            "velocityX": -3.717175911864151,
-            "velocityY": 0.4529207697160109,
-            "timestamp": 0.8584790783998218
-        },
-        {
-            "x": 5.303276771996363,
-            "y": 1.4607318697473424,
-            "heading": -0.3307229091538368,
-            "angularVelocity": -0.17360472172913416,
-            "velocityX": -3.5922053767043294,
-            "velocityY": 1.0031511484968414,
-            "timestamp": 0.9261807090485242
-        },
-        {
-            "x": 5.073563214559545,
-            "y": 1.564404927734292,
-            "heading": -0.34378229938421223,
-            "angularVelocity": -0.1928962434919207,
-            "velocityX": -3.3930284283517023,
-            "velocityY": 1.5313229089691542,
-            "timestamp": 0.9938823396972266
-        },
-        {
-            "x": 4.862974125111252,
-            "y": 1.7011642845126724,
-            "heading": -0.35827284529055603,
-            "angularVelocity": -0.2140354045770704,
-            "velocityX": -3.1105467834446325,
-            "velocityY": 2.020030469989471,
-            "timestamp": 1.0615839703459289
-        },
-        {
-            "x": 4.677671719496688,
-            "y": 1.8665243414779629,
-            "heading": -0.3791415933598452,
-            "angularVelocity": -0.30824587043792007,
-            "velocityX": -2.737044940262413,
-            "velocityY": 2.442482631227878,
-            "timestamp": 1.1292856009946313
-        },
-        {
-            "x": 4.523629451068858,
-            "y": 2.053477766497112,
-            "heading": -0.41478083919993697,
-            "angularVelocity": -0.5264163580502632,
-            "velocityX": -2.27531105162187,
-            "velocityY": 2.761431640977936,
-            "timestamp": 1.1969872316433336
-        },
-        {
-            "x": 4.406611773563404,
-            "y": 2.2498215105595722,
-            "heading": -0.45494733767836987,
-            "angularVelocity": -0.5932870167770028,
-            "velocityX": -1.7284321867022447,
-            "velocityY": 2.900133160474012,
-            "timestamp": 1.264688862292036
-        },
-        {
-            "x": 4.327688857963163,
-            "y": 2.4438365235550945,
-            "heading": -0.48799769437613305,
-            "angularVelocity": -0.4881766713911416,
-            "velocityX": -1.1657461547677304,
-            "velocityY": 2.865736188870776,
-            "timestamp": 1.3323904929407384
-        },
-        {
-            "x": 4.28360260298108,
-            "y": 2.622213717107768,
-            "heading": -0.5154307830643909,
-            "angularVelocity": -0.40520573028252516,
-            "velocityX": -0.6511845366152605,
-            "velocityY": 2.634754759132393,
-            "timestamp": 1.4000921235894408
-        },
-        {
-            "x": 4.264391797811532,
-            "y": 2.7716303897908317,
-            "heading": -0.5365689504315967,
-            "angularVelocity": -0.3122253801676767,
-            "velocityX": -0.283756904899847,
-            "velocityY": 2.2069877970647664,
-            "timestamp": 1.4677937542381432
-        },
-        {
-            "x": 4.259328393836262,
-            "y": 2.8855840206662466,
-            "heading": -0.5517027167443236,
-            "angularVelocity": -0.22353621571242488,
-            "velocityX": -0.07478998551086755,
-            "velocityY": 1.6831740946789628,
-            "timestamp": 1.5354953848868456
-        },
-        {
-            "x": 4.260599830778067,
-            "y": 2.9618797181043535,
-            "heading": -0.5613571988137664,
-            "angularVelocity": -0.1426033904496253,
-            "velocityX": 0.018780004700190214,
-            "velocityY": 1.1269403219251917,
-            "timestamp": 1.603197015535548
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -0.0685773908510417,
-            "velocityX": 0.035452162657366115,
-            "velocityY": 0.5630629798778553,
-            "timestamp": 1.6708986461842503
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 0,
-            "velocityX": 1.1764020723168042e-36,
-            "velocityY": 0,
-            "timestamp": 1.7386002768329527
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.1.traj
deleted file mode 100644
index e5689e4c..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.1.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 1.0550386775092505e-35,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.6504501923310215,
-            "y": 2.437078665707293,
-            "heading": -0.0029944704440250615,
-            "angularVelocity": -0.0457972069493345,
-            "velocityX": -0.45193254781641656,
-            "velocityY": -0.30467539632270557,
-            "timestamp": 0.0653854381848624
-        },
-        {
-            "x": 7.5913505770823395,
-            "y": 2.3972359988448533,
-            "heading": -0.008983367804687599,
-            "angularVelocity": -0.09159374819405963,
-            "velocityX": -0.9038650942674934,
-            "velocityY": -0.609350766294381,
-            "timestamp": 0.1307708763697248
-        },
-        {
-            "x": 7.50270115478166,
-            "y": 2.337472001372052,
-            "heading": -0.017966623739921285,
-            "angularVelocity": -0.13738924422033513,
-            "velocityX": -1.3557976326478658,
-            "velocityY": -0.91402610629957,
-            "timestamp": 0.19615631455458718
-        },
-        {
-            "x": 7.384501926877228,
-            "y": 2.2577866758321745,
-            "heading": -0.02994413949472175,
-            "angularVelocity": -0.18318322989496186,
-            "velocityX": -1.8077301488788986,
-            "velocityY": -1.2187014074079496,
-            "timestamp": 0.2615417527394496
-        },
-        {
-            "x": 7.2367528971895485,
-            "y": 2.1581800263627264,
-            "heading": -0.044915766222073404,
-            "angularVelocity": -0.2289749391144676,
-            "velocityX": -2.25966260667937,
-            "velocityY": -1.5233766452376303,
-            "timestamp": 0.326927190924312
-        },
-        {
-            "x": 7.059454079201559,
-            "y": 2.0386520636124454,
-            "heading": -0.06288119646669635,
-            "angularVelocity": -0.2747619461359239,
-            "velocityX": -2.711594858272837,
-            "velocityY": -1.82805172020631,
-            "timestamp": 0.3923126291091744
-        },
-        {
-            "x": 6.854358935700705,
-            "y": 1.900385597577332,
-            "heading": -0.06288119947800026,
-            "angularVelocity": -4.6054656889756363e-8,
-            "velocityX": -3.136709781174733,
-            "velocityY": -2.114636987584247,
-            "timestamp": 0.45769806729403684
-        },
-        {
-            "x": 6.636845838072133,
-            "y": 1.782615967982618,
-            "heading": -0.06288120256062167,
-            "angularVelocity": -4.714538109405926e-8,
-            "velocityX": -3.3266290425951324,
-            "velocityY": -1.801159904469057,
-            "timestamp": 0.5230835054788993
-        },
-        {
-            "x": 6.4046630859375,
-            "y": 1.6973352432250977,
-            "heading": -0.06288120593828476,
-            "angularVelocity": -5.1657726572043936e-8,
-            "velocityX": -3.5509856411482486,
-            "velocityY": -1.3042770244409516,
-            "timestamp": 0.5884689436637617
-        },
-        {
-            "x": 6.207746736822867,
-            "y": 1.6503996785686041,
-            "heading": -0.06288121580879107,
-            "angularVelocity": -1.8445407171610623e-7,
-            "velocityX": -3.679854027466254,
-            "velocityY": -0.8771035386810692,
-            "timestamp": 0.6419809437716079
-        },
-        {
-            "x": 6.009908768236521,
-            "y": 1.6271742750630454,
-            "heading": -0.0724170210773865,
-            "angularVelocity": -0.17819938050114592,
-            "velocityX": -3.697076696584537,
-            "velocityY": -0.43402234001253925,
-            "timestamp": 0.695492943879454
-        },
-        {
-            "x": 5.8260492909953046,
-            "y": 1.6186155743000838,
-            "heading": -0.12620190803666176,
-            "angularVelocity": -1.0050995449783064,
-            "velocityX": -3.43585507681777,
-            "velocityY": -0.1599398405163814,
-            "timestamp": 0.7490049439873002
-        },
-        {
-            "x": 5.6625912601386625,
-            "y": 1.614879890442449,
-            "heading": -0.22199295256207294,
-            "angularVelocity": -1.7900852954918034,
-            "velocityX": -3.0546051451490293,
-            "velocityY": -0.06981020799271546,
-            "timestamp": 0.8025169440951464
-        },
-        {
-            "x": 5.5227239807129855,
-            "y": 1.6141995251565013,
-            "heading": -0.32512734843535046,
-            "angularVelocity": -1.9273134187738115,
-            "velocityX": -2.61375540334492,
-            "velocityY": -0.012714256327113658,
-            "timestamp": 0.8560289442029926
-        },
-        {
-            "x": 5.406619958232834,
-            "y": 1.6150039384726416,
-            "heading": -0.42188556792063076,
-            "angularVelocity": -1.8081592781110263,
-            "velocityX": -2.169681982474204,
-            "velocityY": 0.015032391136922594,
-            "timestamp": 0.9095409443108388
-        },
-        {
-            "x": 5.314084878105213,
-            "y": 1.6164421457440925,
-            "heading": -0.5051954003859285,
-            "angularVelocity": -1.5568439284160218,
-            "velocityX": -1.7292397955809797,
-            "velocityY": 0.026876350511144828,
-            "timestamp": 0.9630529444186849
-        },
-        {
-            "x": 5.244912091070787,
-            "y": 1.617980616061371,
-            "heading": -0.5709381839957368,
-            "angularVelocity": -1.2285615091439794,
-            "velocityX": -1.2926593454742539,
-            "velocityY": 0.028750005871169333,
-            "timestamp": 1.016564944526531
-        },
-        {
-            "x": 5.198930392622704,
-            "y": 1.6192554664106382,
-            "heading": -0.6164892293058035,
-            "angularVelocity": -0.851230475748707,
-            "velocityX": -0.8592782619863396,
-            "velocityY": 0.023823634823929046,
-            "timestamp": 1.0700769446343772
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -0.4393551100092281,
-            "velocityX": -0.42850935447171357,
-            "velocityY": 0.013913394899485362,
-            "timestamp": 1.1235889447422234
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 6.2403191565519e-36,
-            "timestamp": 1.1771009448500696
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.traj
deleted file mode 100644
index e5689e4c..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 1.0550386775092505e-35,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.6504501923310215,
-            "y": 2.437078665707293,
-            "heading": -0.0029944704440250615,
-            "angularVelocity": -0.0457972069493345,
-            "velocityX": -0.45193254781641656,
-            "velocityY": -0.30467539632270557,
-            "timestamp": 0.0653854381848624
-        },
-        {
-            "x": 7.5913505770823395,
-            "y": 2.3972359988448533,
-            "heading": -0.008983367804687599,
-            "angularVelocity": -0.09159374819405963,
-            "velocityX": -0.9038650942674934,
-            "velocityY": -0.609350766294381,
-            "timestamp": 0.1307708763697248
-        },
-        {
-            "x": 7.50270115478166,
-            "y": 2.337472001372052,
-            "heading": -0.017966623739921285,
-            "angularVelocity": -0.13738924422033513,
-            "velocityX": -1.3557976326478658,
-            "velocityY": -0.91402610629957,
-            "timestamp": 0.19615631455458718
-        },
-        {
-            "x": 7.384501926877228,
-            "y": 2.2577866758321745,
-            "heading": -0.02994413949472175,
-            "angularVelocity": -0.18318322989496186,
-            "velocityX": -1.8077301488788986,
-            "velocityY": -1.2187014074079496,
-            "timestamp": 0.2615417527394496
-        },
-        {
-            "x": 7.2367528971895485,
-            "y": 2.1581800263627264,
-            "heading": -0.044915766222073404,
-            "angularVelocity": -0.2289749391144676,
-            "velocityX": -2.25966260667937,
-            "velocityY": -1.5233766452376303,
-            "timestamp": 0.326927190924312
-        },
-        {
-            "x": 7.059454079201559,
-            "y": 2.0386520636124454,
-            "heading": -0.06288119646669635,
-            "angularVelocity": -0.2747619461359239,
-            "velocityX": -2.711594858272837,
-            "velocityY": -1.82805172020631,
-            "timestamp": 0.3923126291091744
-        },
-        {
-            "x": 6.854358935700705,
-            "y": 1.900385597577332,
-            "heading": -0.06288119947800026,
-            "angularVelocity": -4.6054656889756363e-8,
-            "velocityX": -3.136709781174733,
-            "velocityY": -2.114636987584247,
-            "timestamp": 0.45769806729403684
-        },
-        {
-            "x": 6.636845838072133,
-            "y": 1.782615967982618,
-            "heading": -0.06288120256062167,
-            "angularVelocity": -4.714538109405926e-8,
-            "velocityX": -3.3266290425951324,
-            "velocityY": -1.801159904469057,
-            "timestamp": 0.5230835054788993
-        },
-        {
-            "x": 6.4046630859375,
-            "y": 1.6973352432250977,
-            "heading": -0.06288120593828476,
-            "angularVelocity": -5.1657726572043936e-8,
-            "velocityX": -3.5509856411482486,
-            "velocityY": -1.3042770244409516,
-            "timestamp": 0.5884689436637617
-        },
-        {
-            "x": 6.207746736822867,
-            "y": 1.6503996785686041,
-            "heading": -0.06288121580879107,
-            "angularVelocity": -1.8445407171610623e-7,
-            "velocityX": -3.679854027466254,
-            "velocityY": -0.8771035386810692,
-            "timestamp": 0.6419809437716079
-        },
-        {
-            "x": 6.009908768236521,
-            "y": 1.6271742750630454,
-            "heading": -0.0724170210773865,
-            "angularVelocity": -0.17819938050114592,
-            "velocityX": -3.697076696584537,
-            "velocityY": -0.43402234001253925,
-            "timestamp": 0.695492943879454
-        },
-        {
-            "x": 5.8260492909953046,
-            "y": 1.6186155743000838,
-            "heading": -0.12620190803666176,
-            "angularVelocity": -1.0050995449783064,
-            "velocityX": -3.43585507681777,
-            "velocityY": -0.1599398405163814,
-            "timestamp": 0.7490049439873002
-        },
-        {
-            "x": 5.6625912601386625,
-            "y": 1.614879890442449,
-            "heading": -0.22199295256207294,
-            "angularVelocity": -1.7900852954918034,
-            "velocityX": -3.0546051451490293,
-            "velocityY": -0.06981020799271546,
-            "timestamp": 0.8025169440951464
-        },
-        {
-            "x": 5.5227239807129855,
-            "y": 1.6141995251565013,
-            "heading": -0.32512734843535046,
-            "angularVelocity": -1.9273134187738115,
-            "velocityX": -2.61375540334492,
-            "velocityY": -0.012714256327113658,
-            "timestamp": 0.8560289442029926
-        },
-        {
-            "x": 5.406619958232834,
-            "y": 1.6150039384726416,
-            "heading": -0.42188556792063076,
-            "angularVelocity": -1.8081592781110263,
-            "velocityX": -2.169681982474204,
-            "velocityY": 0.015032391136922594,
-            "timestamp": 0.9095409443108388
-        },
-        {
-            "x": 5.314084878105213,
-            "y": 1.6164421457440925,
-            "heading": -0.5051954003859285,
-            "angularVelocity": -1.5568439284160218,
-            "velocityX": -1.7292397955809797,
-            "velocityY": 0.026876350511144828,
-            "timestamp": 0.9630529444186849
-        },
-        {
-            "x": 5.244912091070787,
-            "y": 1.617980616061371,
-            "heading": -0.5709381839957368,
-            "angularVelocity": -1.2285615091439794,
-            "velocityX": -1.2926593454742539,
-            "velocityY": 0.028750005871169333,
-            "timestamp": 1.016564944526531
-        },
-        {
-            "x": 5.198930392622704,
-            "y": 1.6192554664106382,
-            "heading": -0.6164892293058035,
-            "angularVelocity": -0.851230475748707,
-            "velocityX": -0.8592782619863396,
-            "velocityY": 0.023823634823929046,
-            "timestamp": 1.0700769446343772
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -0.4393551100092281,
-            "velocityX": -0.42850935447171357,
-            "velocityY": 0.013913394899485362,
-            "timestamp": 1.1235889447422234
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 6.2403191565519e-36,
-            "timestamp": 1.1771009448500696
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.1.traj
deleted file mode 100644
index f850aede..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.1.traj	
+++ /dev/null
@@ -1,238 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 2.322866633708497e-28,
-            "velocityX": -2.1618763976557105e-31,
-            "velocityY": -1.247476705840714e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 7.635666877682845,
-            "y": 2.515362381859407,
-            "heading": -0.0008300252680408195,
-            "angularVelocity": -0.00885317162168204,
-            "velocityX": -0.4728636048938783,
-            "velocityY": 0.6225017511467659,
-            "timestamp": 0.09375456655646779
-        },
-        {
-            "x": 7.547000631163484,
-            "y": 2.632087139923421,
-            "heading": -0.0024900693866490363,
-            "angularVelocity": -0.01770627479364842,
-            "velocityX": -0.9457272298940119,
-            "velocityY": 1.2450034419786018,
-            "timestamp": 0.18750913311293557
-        },
-        {
-            "x": 7.4140012603329,
-            "y": 2.8071742656560237,
-            "heading": -0.004980126198969091,
-            "angularVelocity": -0.026559312295687615,
-            "velocityX": -1.418590856056911,
-            "velocityY": 1.867505041764004,
-            "timestamp": 0.28126369966940334
-        },
-        {
-            "x": 7.236668772838196,
-            "y": 3.0406237394955027,
-            "heading": -0.008300187115435396,
-            "angularVelocity": -0.035412258180156565,
-            "velocityX": -1.891454400654691,
-            "velocityY": 2.4900064329013105,
-            "timestamp": 0.37501826622587114
-        },
-        {
-            "x": 7.022136423676778,
-            "y": 3.3230511197393278,
-            "heading": -0.008300187546286333,
-            "angularVelocity": -4.59551947810532e-9,
-            "velocityX": -2.2882335980104607,
-            "velocityY": 3.012411988206685,
-            "timestamp": 0.46877283278233894
-        },
-        {
-            "x": 6.786560921928144,
-            "y": 3.5881804025568163,
-            "heading": -0.008300187986713265,
-            "angularVelocity": -4.6976584720676315e-9,
-            "velocityX": -2.51268295936015,
-            "velocityY": 2.827907936172931,
-            "timestamp": 0.5625273993388067
-        },
-        {
-            "x": 6.5015204114211205,
-            "y": 3.799228403383322,
-            "heading": -0.008300188501414158,
-            "angularVelocity": -5.489875460815635e-9,
-            "velocityX": -3.0402840200359327,
-            "velocityY": 2.251069026055312,
-            "timestamp": 0.6562819658952744
-        },
-        {
-            "x": 6.179186778264032,
-            "y": 3.9471823835437703,
-            "heading": -0.008300169798343478,
-            "angularVelocity": 1.9948970346527163e-7,
-            "velocityX": -3.4380579527606097,
-            "velocityY": 1.5780989192812942,
-            "timestamp": 0.7500365324517422
-        },
-        {
-            "x": 5.833324835964247,
-            "y": 4.025724182042927,
-            "heading": -0.008299866211032016,
-            "angularVelocity": 0.0000032381069276099122,
-            "velocityX": -3.689014359545628,
-            "velocityY": 0.8377383778085268,
-            "timestamp": 0.8437910990082099
-        },
-        {
-            "x": 5.481092764022396,
-            "y": 4.0672194252777,
-            "heading": -0.008299498284594501,
-            "angularVelocity": 0.000003924357511610969,
-            "velocityX": -3.7569590994770814,
-            "velocityY": 0.44259436909433375,
-            "timestamp": 0.9375456655646777
-        },
-        {
-            "x": 5.14904862351455,
-            "y": 4.179095043376677,
-            "heading": 0.0033651913425959347,
-            "angularVelocity": 0.12441729566489874,
-            "velocityX": -3.5416316527670983,
-            "velocityY": 1.1932818017093059,
-            "timestamp": 1.0313002321211455
-        },
-        {
-            "x": 4.858987953002521,
-            "y": 4.350333701259038,
-            "heading": 0.047887100206889445,
-            "angularVelocity": 0.474877229979814,
-            "velocityX": -3.093829785211866,
-            "velocityY": 1.8264567174893411,
-            "timestamp": 1.1250547986776134
-        },
-        {
-            "x": 4.6181973815778665,
-            "y": 4.575631660267378,
-            "heading": 0.10901822024185409,
-            "angularVelocity": 0.652033519862158,
-            "velocityX": -2.5683076597621346,
-            "velocityY": 2.403061176467013,
-            "timestamp": 1.2188093652340812
-        },
-        {
-            "x": 4.427445933507109,
-            "y": 4.854263962903063,
-            "heading": 0.15298068479753676,
-            "angularVelocity": 0.46891011467909954,
-            "velocityX": -2.0345830083473238,
-            "velocityY": 2.971933132108998,
-            "timestamp": 1.312563931790549
-        },
-        {
-            "x": 4.19709169118396,
-            "y": 5.123942070734021,
-            "heading": 0.15298069409509907,
-            "angularVelocity": 9.916916750047786e-8,
-            "velocityX": -2.4569922381797515,
-            "velocityY": 2.8764263729866686,
-            "timestamp": 1.406318498347017
-        },
-        {
-            "x": 3.9282946631899107,
-            "y": 5.355323824233057,
-            "heading": 0.15298070467968702,
-            "angularVelocity": 1.1289677246612154e-7,
-            "velocityX": -2.8670286458223346,
-            "velocityY": 2.4679518235485323,
-            "timestamp": 1.5000730649034848
-        },
-        {
-            "x": 3.6594937496643456,
-            "y": 5.586701063819283,
-            "heading": 0.1529807152642756,
-            "angularVelocity": 1.1289677885329184e-7,
-            "velocityX": -2.867070089473112,
-            "velocityY": 2.4679036774904017,
-            "timestamp": 1.5938276314599527
-        },
-        {
-            "x": 3.3906928188108107,
-            "y": 5.818078283274842,
-            "heading": 0.1529807258488866,
-            "angularVelocity": 1.1289701863908675e-7,
-            "velocityX": -2.8670702742957928,
-            "velocityY": 2.467903462773746,
-            "timestamp": 1.6875821980164205
-        },
-        {
-            "x": 3.1218918754104785,
-            "y": 6.049455488154229,
-            "heading": 0.15298073643351034,
-            "angularVelocity": 1.1289715379702627e-7,
-            "velocityX": -2.867070408121775,
-            "velocityY": 2.467903307302158,
-            "timestamp": 1.7813367645728884
-        },
-        {
-            "x": 2.853090944613356,
-            "y": 6.280832707674931,
-            "heading": 0.15298074701856604,
-            "angularVelocity": 1.1290176153813175e-7,
-            "velocityX": -2.8670702736940825,
-            "velocityY": 2.4679034634685753,
-            "timestamp": 1.8750913311293562
-        },
-        {
-            "x": 2.631853660621228,
-            "y": 6.471299719512446,
-            "heading": 0.24894777935018825,
-            "angularVelocity": 1.0235984854542721,
-            "velocityX": -2.3597494193403232,
-            "velocityY": 2.0315491696374846,
-            "timestamp": 1.968845897685824
-        },
-        {
-            "x": 2.465926649664366,
-            "y": 6.614149879341798,
-            "heading": 0.32096506390227886,
-            "angularVelocity": 0.7681469521670183,
-            "velocityX": -1.7698019099360354,
-            "velocityY": 1.52366082075921,
-            "timestamp": 2.062600464242292
-        },
-        {
-            "x": 2.355308907973467,
-            "y": 6.709383273555383,
-            "heading": 0.3689895410149908,
-            "angularVelocity": 0.5122361382129277,
-            "velocityX": -1.1798651068828128,
-            "velocityY": 1.0157733933549364,
-            "timestamp": 2.1563550307987596
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.25609908793666974,
-            "velocityX": -0.5899329494543092,
-            "velocityY": 0.5078870095990298,
-            "timestamp": 2.250109597355227
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0,
-            "velocityX": -3.330818616870831e-32,
-            "velocityY": 5.736512740310497e-33,
-            "timestamp": 2.343864163911695
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.traj
deleted file mode 100644
index f850aede..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.traj	
+++ /dev/null
@@ -1,238 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 2.322866633708497e-28,
-            "velocityX": -2.1618763976557105e-31,
-            "velocityY": -1.247476705840714e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 7.635666877682845,
-            "y": 2.515362381859407,
-            "heading": -0.0008300252680408195,
-            "angularVelocity": -0.00885317162168204,
-            "velocityX": -0.4728636048938783,
-            "velocityY": 0.6225017511467659,
-            "timestamp": 0.09375456655646779
-        },
-        {
-            "x": 7.547000631163484,
-            "y": 2.632087139923421,
-            "heading": -0.0024900693866490363,
-            "angularVelocity": -0.01770627479364842,
-            "velocityX": -0.9457272298940119,
-            "velocityY": 1.2450034419786018,
-            "timestamp": 0.18750913311293557
-        },
-        {
-            "x": 7.4140012603329,
-            "y": 2.8071742656560237,
-            "heading": -0.004980126198969091,
-            "angularVelocity": -0.026559312295687615,
-            "velocityX": -1.418590856056911,
-            "velocityY": 1.867505041764004,
-            "timestamp": 0.28126369966940334
-        },
-        {
-            "x": 7.236668772838196,
-            "y": 3.0406237394955027,
-            "heading": -0.008300187115435396,
-            "angularVelocity": -0.035412258180156565,
-            "velocityX": -1.891454400654691,
-            "velocityY": 2.4900064329013105,
-            "timestamp": 0.37501826622587114
-        },
-        {
-            "x": 7.022136423676778,
-            "y": 3.3230511197393278,
-            "heading": -0.008300187546286333,
-            "angularVelocity": -4.59551947810532e-9,
-            "velocityX": -2.2882335980104607,
-            "velocityY": 3.012411988206685,
-            "timestamp": 0.46877283278233894
-        },
-        {
-            "x": 6.786560921928144,
-            "y": 3.5881804025568163,
-            "heading": -0.008300187986713265,
-            "angularVelocity": -4.6976584720676315e-9,
-            "velocityX": -2.51268295936015,
-            "velocityY": 2.827907936172931,
-            "timestamp": 0.5625273993388067
-        },
-        {
-            "x": 6.5015204114211205,
-            "y": 3.799228403383322,
-            "heading": -0.008300188501414158,
-            "angularVelocity": -5.489875460815635e-9,
-            "velocityX": -3.0402840200359327,
-            "velocityY": 2.251069026055312,
-            "timestamp": 0.6562819658952744
-        },
-        {
-            "x": 6.179186778264032,
-            "y": 3.9471823835437703,
-            "heading": -0.008300169798343478,
-            "angularVelocity": 1.9948970346527163e-7,
-            "velocityX": -3.4380579527606097,
-            "velocityY": 1.5780989192812942,
-            "timestamp": 0.7500365324517422
-        },
-        {
-            "x": 5.833324835964247,
-            "y": 4.025724182042927,
-            "heading": -0.008299866211032016,
-            "angularVelocity": 0.0000032381069276099122,
-            "velocityX": -3.689014359545628,
-            "velocityY": 0.8377383778085268,
-            "timestamp": 0.8437910990082099
-        },
-        {
-            "x": 5.481092764022396,
-            "y": 4.0672194252777,
-            "heading": -0.008299498284594501,
-            "angularVelocity": 0.000003924357511610969,
-            "velocityX": -3.7569590994770814,
-            "velocityY": 0.44259436909433375,
-            "timestamp": 0.9375456655646777
-        },
-        {
-            "x": 5.14904862351455,
-            "y": 4.179095043376677,
-            "heading": 0.0033651913425959347,
-            "angularVelocity": 0.12441729566489874,
-            "velocityX": -3.5416316527670983,
-            "velocityY": 1.1932818017093059,
-            "timestamp": 1.0313002321211455
-        },
-        {
-            "x": 4.858987953002521,
-            "y": 4.350333701259038,
-            "heading": 0.047887100206889445,
-            "angularVelocity": 0.474877229979814,
-            "velocityX": -3.093829785211866,
-            "velocityY": 1.8264567174893411,
-            "timestamp": 1.1250547986776134
-        },
-        {
-            "x": 4.6181973815778665,
-            "y": 4.575631660267378,
-            "heading": 0.10901822024185409,
-            "angularVelocity": 0.652033519862158,
-            "velocityX": -2.5683076597621346,
-            "velocityY": 2.403061176467013,
-            "timestamp": 1.2188093652340812
-        },
-        {
-            "x": 4.427445933507109,
-            "y": 4.854263962903063,
-            "heading": 0.15298068479753676,
-            "angularVelocity": 0.46891011467909954,
-            "velocityX": -2.0345830083473238,
-            "velocityY": 2.971933132108998,
-            "timestamp": 1.312563931790549
-        },
-        {
-            "x": 4.19709169118396,
-            "y": 5.123942070734021,
-            "heading": 0.15298069409509907,
-            "angularVelocity": 9.916916750047786e-8,
-            "velocityX": -2.4569922381797515,
-            "velocityY": 2.8764263729866686,
-            "timestamp": 1.406318498347017
-        },
-        {
-            "x": 3.9282946631899107,
-            "y": 5.355323824233057,
-            "heading": 0.15298070467968702,
-            "angularVelocity": 1.1289677246612154e-7,
-            "velocityX": -2.8670286458223346,
-            "velocityY": 2.4679518235485323,
-            "timestamp": 1.5000730649034848
-        },
-        {
-            "x": 3.6594937496643456,
-            "y": 5.586701063819283,
-            "heading": 0.1529807152642756,
-            "angularVelocity": 1.1289677885329184e-7,
-            "velocityX": -2.867070089473112,
-            "velocityY": 2.4679036774904017,
-            "timestamp": 1.5938276314599527
-        },
-        {
-            "x": 3.3906928188108107,
-            "y": 5.818078283274842,
-            "heading": 0.1529807258488866,
-            "angularVelocity": 1.1289701863908675e-7,
-            "velocityX": -2.8670702742957928,
-            "velocityY": 2.467903462773746,
-            "timestamp": 1.6875821980164205
-        },
-        {
-            "x": 3.1218918754104785,
-            "y": 6.049455488154229,
-            "heading": 0.15298073643351034,
-            "angularVelocity": 1.1289715379702627e-7,
-            "velocityX": -2.867070408121775,
-            "velocityY": 2.467903307302158,
-            "timestamp": 1.7813367645728884
-        },
-        {
-            "x": 2.853090944613356,
-            "y": 6.280832707674931,
-            "heading": 0.15298074701856604,
-            "angularVelocity": 1.1290176153813175e-7,
-            "velocityX": -2.8670702736940825,
-            "velocityY": 2.4679034634685753,
-            "timestamp": 1.8750913311293562
-        },
-        {
-            "x": 2.631853660621228,
-            "y": 6.471299719512446,
-            "heading": 0.24894777935018825,
-            "angularVelocity": 1.0235984854542721,
-            "velocityX": -2.3597494193403232,
-            "velocityY": 2.0315491696374846,
-            "timestamp": 1.968845897685824
-        },
-        {
-            "x": 2.465926649664366,
-            "y": 6.614149879341798,
-            "heading": 0.32096506390227886,
-            "angularVelocity": 0.7681469521670183,
-            "velocityX": -1.7698019099360354,
-            "velocityY": 1.52366082075921,
-            "timestamp": 2.062600464242292
-        },
-        {
-            "x": 2.355308907973467,
-            "y": 6.709383273555383,
-            "heading": 0.3689895410149908,
-            "angularVelocity": 0.5122361382129277,
-            "velocityX": -1.1798651068828128,
-            "velocityY": 1.0157733933549364,
-            "timestamp": 2.1563550307987596
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.25609908793666974,
-            "velocityX": -0.5899329494543092,
-            "velocityY": 0.5078870095990298,
-            "timestamp": 2.250109597355227
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0,
-            "velocityX": -3.330818616870831e-32,
-            "velocityY": 5.736512740310497e-33,
-            "timestamp": 2.343864163911695
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.1.traj
deleted file mode 100644
index 086204d2..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.1.traj	
+++ /dev/null
@@ -1,319 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -1.0202939485165084e-30,
-            "velocityX": 2.0196671556133315e-31,
-            "velocityY": 6.267725686062039e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.650233104605526,
-            "y": 2.498844879842391,
-            "heading": 0.006167890922909293,
-            "angularVelocity": 0.07857204140894247,
-            "velocityX": -0.37919700052787514,
-            "velocityY": 0.5330570324317279,
-            "timestamp": 0.07849981764896931
-        },
-        {
-            "x": 7.590699315345095,
-            "y": 2.5825346366761623,
-            "heading": 0.018503878854508167,
-            "angularVelocity": 0.15714670812054757,
-            "velocityX": -0.758393981584137,
-            "velocityY": 1.066114028544761,
-            "timestamp": 0.15699963529793862
-        },
-        {
-            "x": 7.501398636107905,
-            "y": 2.7080692662264956,
-            "heading": 0.037008343975294504,
-            "angularVelocity": 0.23572621790707007,
-            "velocityX": -1.1375909130963788,
-            "velocityY": 1.599170970201378,
-            "timestamp": 0.23549945294690794
-        },
-        {
-            "x": 7.382331074013197,
-            "y": 2.875448760857071,
-            "heading": 0.061681864177524894,
-            "angularVelocity": 0.31431308939549074,
-            "velocityX": -1.5167877539174253,
-            "velocityY": 2.132227814579811,
-            "timestamp": 0.31399927059587723
-        },
-        {
-            "x": 7.2334966444595095,
-            "y": 3.084673101412061,
-            "heading": 0.09252521384117814,
-            "angularVelocity": 0.39290982561993243,
-            "velocityX": -1.8959843985782956,
-            "velocityY": 2.665284414934403,
-            "timestamp": 0.39249908824484653
-        },
-        {
-            "x": 7.061360166565958,
-            "y": 3.326653173894572,
-            "heading": 0.09252521821441809,
-            "angularVelocity": 5.5710192458533506e-8,
-            "velocityX": -2.192826468251186,
-            "velocityY": 3.0825558546464755,
-            "timestamp": 0.4709989058938158
-        },
-        {
-            "x": 6.8697148114409385,
-            "y": 3.553495365981472,
-            "heading": 0.09252522268181232,
-            "angularVelocity": 5.690961282999965e-8,
-            "velocityX": -2.4413477746153216,
-            "velocityY": 2.8897161659824335,
-            "timestamp": 0.5494987235427852
-        },
-        {
-            "x": 6.64183630533757,
-            "y": 3.743907306900521,
-            "heading": 0.09252522772743905,
-            "angularVelocity": 6.427564912453075e-8,
-            "velocityX": -2.902917649087824,
-            "velocityY": 2.425635455237886,
-            "timestamp": 0.6279985411917545
-        },
-        {
-            "x": 6.384546476442235,
-            "y": 3.8921883279204876,
-            "heading": 0.09252523431940601,
-            "angularVelocity": 8.397429651316677e-8,
-            "velocityX": -3.277585051800601,
-            "velocityY": 1.8889345919635134,
-            "timestamp": 0.7064983588407238
-        },
-        {
-            "x": 6.105547891541977,
-            "y": 3.9938992218783795,
-            "heading": 0.09252527901816404,
-            "angularVelocity": 5.694122529822745e-7,
-            "velocityX": -3.5541303566827107,
-            "velocityY": 1.2956831876058104,
-            "timestamp": 0.7849981764896931
-        },
-        {
-            "x": 5.813193382307703,
-            "y": 4.045994945482373,
-            "heading": 0.09252629740574593,
-            "angularVelocity": 0.000012973120350997966,
-            "velocityX": -3.7242699153978625,
-            "velocityY": 0.6636413327346956,
-            "timestamp": 0.8634979941386624
-        },
-        {
-            "x": 5.523572440187452,
-            "y": 4.068080526231475,
-            "heading": 0.11166809511552328,
-            "angularVelocity": 0.24384512325081395,
-            "velocityX": -3.6894473235002847,
-            "velocityY": 0.28134563124547074,
-            "timestamp": 0.9419978117876316
-        },
-        {
-            "x": 5.248612904615211,
-            "y": 4.139233352038044,
-            "heading": 0.1457449942262438,
-            "angularVelocity": 0.43410163400767243,
-            "velocityX": -3.502677379478612,
-            "velocityY": 0.9064075298205869,
-            "timestamp": 1.020497629436601
-        },
-        {
-            "x": 4.992805004119873,
-            "y": 4.258058071136475,
-            "heading": 0.1827080793073441,
-            "angularVelocity": 0.47086841967443066,
-            "velocityX": -3.258706939158039,
-            "velocityY": 1.513694205377446,
-            "timestamp": 1.0989974470855703
-        },
-        {
-            "x": 4.782367562736006,
-            "y": 4.400850786547769,
-            "heading": 0.2087590252622848,
-            "angularVelocity": 0.37195010692359587,
-            "velocityX": -3.004582979783548,
-            "velocityY": 2.0387653429944033,
-            "timestamp": 1.1690362653840851
-        },
-        {
-            "x": 4.593366685154892,
-            "y": 4.57831882888555,
-            "heading": 0.22294244363545143,
-            "angularVelocity": 0.20250796226614393,
-            "velocityX": -2.6985160825467664,
-            "velocityY": 2.5338526070127387,
-            "timestamp": 1.2390750836825999
-        },
-        {
-            "x": 4.4303113780727985,
-            "y": 4.787155425256865,
-            "heading": 0.2229432862167668,
-            "angularVelocity": 0.000012030204616055241,
-            "velocityX": -2.3280705049466333,
-            "velocityY": 2.9817264403466797,
-            "timestamp": 1.3091139019811147
-        },
-        {
-            "x": 4.301341913399859,
-            "y": 5.018600438478609,
-            "heading": 0.22294334445620553,
-            "angularVelocity": 8.315308587772638e-7,
-            "velocityX": -1.8413997809506994,
-            "velocityY": 3.3045248170136206,
-            "timestamp": 1.3791527202796294
-        },
-        {
-            "x": 4.209527987601824,
-            "y": 5.267136397095996,
-            "heading": 0.22294334228224533,
-            "angularVelocity": -3.103936119065208e-8,
-            "velocityX": -1.3109005552544912,
-            "velocityY": 3.5485458586481258,
-            "timestamp": 1.4491915385781442
-        },
-        {
-            "x": 4.095163246631834,
-            "y": 5.506135613505712,
-            "heading": 0.22294334005402738,
-            "angularVelocity": -3.181404259759038e-8,
-            "velocityX": -1.6328765068917115,
-            "velocityY": 3.412382193414732,
-            "timestamp": 1.519230356876659
-        },
-        {
-            "x": 3.9453760475707735,
-            "y": 5.724684722787353,
-            "heading": 0.22294333755566106,
-            "angularVelocity": -3.567116580835512e-8,
-            "velocityX": -2.138631157692488,
-            "velocityY": 3.120399723908581,
-            "timestamp": 1.5892691751751737
-        },
-        {
-            "x": 3.7637358458155963,
-            "y": 5.917575181509908,
-            "heading": 0.22294327041118944,
-            "angularVelocity": -9.586751071941434e-7,
-            "velocityX": -2.5934218504529762,
-            "velocityY": 2.7540507308438293,
-            "timestamp": 1.6593079934736885
-        },
-        {
-            "x": 3.5705120272666107,
-            "y": 6.071441420591724,
-            "heading": 0.20870193370475335,
-            "angularVelocity": -0.20333490844666557,
-            "velocityX": -2.7588103746352264,
-            "velocityY": 2.1968708613263175,
-            "timestamp": 1.7293468117722033
-        },
-        {
-            "x": 3.3793495617108644,
-            "y": 6.184488312917552,
-            "heading": 0.18879422691132347,
-            "angularVelocity": -0.2842381878658059,
-            "velocityX": -2.7293787959290245,
-            "velocityY": 1.614060532031302,
-            "timestamp": 1.799385630070718
-        },
-        {
-            "x": 3.1945436254003248,
-            "y": 6.257137552443532,
-            "heading": 0.1660945073730067,
-            "angularVelocity": -0.3241019778712817,
-            "velocityX": -2.638621564444474,
-            "velocityY": 1.037271063258847,
-            "timestamp": 1.8694244483692328
-        },
-        {
-            "x": 3.018114205815821,
-            "y": 6.289753496686376,
-            "heading": 0.14197845255388317,
-            "angularVelocity": -0.34432412489220054,
-            "velocityX": -2.519023362625089,
-            "velocityY": 0.46568381699052885,
-            "timestamp": 1.9394632666677476
-        },
-        {
-            "x": 2.851226329803467,
-            "y": 6.282596111297607,
-            "heading": 0.11724722927389757,
-            "angularVelocity": -0.3531073750355408,
-            "velocityX": -2.382791144492779,
-            "velocityY": -0.10219169258892027,
-            "timestamp": 2.0095020849662624
-        },
-        {
-            "x": 2.661249908723212,
-            "y": 6.213695182206588,
-            "heading": 0.08663876117142615,
-            "angularVelocity": -0.3547239131111928,
-            "velocityX": -2.2016514926122546,
-            "velocityY": -0.7984982163199495,
-            "timestamp": 2.0957902289879784
-        },
-        {
-            "x": 2.4936356536291973,
-            "y": 6.0868793491721815,
-            "heading": 0.056963129639284946,
-            "angularVelocity": -0.3439131976774355,
-            "velocityX": -1.9424946149239757,
-            "velocityY": -1.4696785343117862,
-            "timestamp": 2.1820783730096944
-        },
-        {
-            "x": 2.3641635269275443,
-            "y": 5.911089762977232,
-            "heading": 0.030889400871559385,
-            "angularVelocity": -0.3021704669089439,
-            "velocityX": -1.500462527842606,
-            "velocityY": -2.037239161740341,
-            "timestamp": 2.2683665170314105
-        },
-        {
-            "x": 2.2965373597407854,
-            "y": 5.740054606624193,
-            "heading": 0.01408623201300084,
-            "angularVelocity": -0.19473322840668433,
-            "velocityX": -0.7837249016474405,
-            "velocityY": -1.9821397051956142,
-            "timestamp": 2.3546546610531265
-        },
-        {
-            "x": 2.263390196849459,
-            "y": 5.620608972310546,
-            "heading": 0.004408633441468959,
-            "angularVelocity": -0.11215444116055903,
-            "velocityX": -0.38414504410927525,
-            "velocityY": -1.384264729156902,
-            "timestamp": 2.4409428050748425
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -1.7581112791577623e-31,
-            "angularVelocity": -0.05109199521503166,
-            "velocityX": -0.1551800308289656,
-            "velocityY": -0.7024020854510996,
-            "timestamp": 2.5272309490965585
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 5.961493131289069e-30,
-            "velocityX": -9.776555553264502e-32,
-            "velocityY": 0,
-            "timestamp": 2.6135190931182746
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.traj
deleted file mode 100644
index 086204d2..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.traj	
+++ /dev/null
@@ -1,319 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -1.0202939485165084e-30,
-            "velocityX": 2.0196671556133315e-31,
-            "velocityY": 6.267725686062039e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.650233104605526,
-            "y": 2.498844879842391,
-            "heading": 0.006167890922909293,
-            "angularVelocity": 0.07857204140894247,
-            "velocityX": -0.37919700052787514,
-            "velocityY": 0.5330570324317279,
-            "timestamp": 0.07849981764896931
-        },
-        {
-            "x": 7.590699315345095,
-            "y": 2.5825346366761623,
-            "heading": 0.018503878854508167,
-            "angularVelocity": 0.15714670812054757,
-            "velocityX": -0.758393981584137,
-            "velocityY": 1.066114028544761,
-            "timestamp": 0.15699963529793862
-        },
-        {
-            "x": 7.501398636107905,
-            "y": 2.7080692662264956,
-            "heading": 0.037008343975294504,
-            "angularVelocity": 0.23572621790707007,
-            "velocityX": -1.1375909130963788,
-            "velocityY": 1.599170970201378,
-            "timestamp": 0.23549945294690794
-        },
-        {
-            "x": 7.382331074013197,
-            "y": 2.875448760857071,
-            "heading": 0.061681864177524894,
-            "angularVelocity": 0.31431308939549074,
-            "velocityX": -1.5167877539174253,
-            "velocityY": 2.132227814579811,
-            "timestamp": 0.31399927059587723
-        },
-        {
-            "x": 7.2334966444595095,
-            "y": 3.084673101412061,
-            "heading": 0.09252521384117814,
-            "angularVelocity": 0.39290982561993243,
-            "velocityX": -1.8959843985782956,
-            "velocityY": 2.665284414934403,
-            "timestamp": 0.39249908824484653
-        },
-        {
-            "x": 7.061360166565958,
-            "y": 3.326653173894572,
-            "heading": 0.09252521821441809,
-            "angularVelocity": 5.5710192458533506e-8,
-            "velocityX": -2.192826468251186,
-            "velocityY": 3.0825558546464755,
-            "timestamp": 0.4709989058938158
-        },
-        {
-            "x": 6.8697148114409385,
-            "y": 3.553495365981472,
-            "heading": 0.09252522268181232,
-            "angularVelocity": 5.690961282999965e-8,
-            "velocityX": -2.4413477746153216,
-            "velocityY": 2.8897161659824335,
-            "timestamp": 0.5494987235427852
-        },
-        {
-            "x": 6.64183630533757,
-            "y": 3.743907306900521,
-            "heading": 0.09252522772743905,
-            "angularVelocity": 6.427564912453075e-8,
-            "velocityX": -2.902917649087824,
-            "velocityY": 2.425635455237886,
-            "timestamp": 0.6279985411917545
-        },
-        {
-            "x": 6.384546476442235,
-            "y": 3.8921883279204876,
-            "heading": 0.09252523431940601,
-            "angularVelocity": 8.397429651316677e-8,
-            "velocityX": -3.277585051800601,
-            "velocityY": 1.8889345919635134,
-            "timestamp": 0.7064983588407238
-        },
-        {
-            "x": 6.105547891541977,
-            "y": 3.9938992218783795,
-            "heading": 0.09252527901816404,
-            "angularVelocity": 5.694122529822745e-7,
-            "velocityX": -3.5541303566827107,
-            "velocityY": 1.2956831876058104,
-            "timestamp": 0.7849981764896931
-        },
-        {
-            "x": 5.813193382307703,
-            "y": 4.045994945482373,
-            "heading": 0.09252629740574593,
-            "angularVelocity": 0.000012973120350997966,
-            "velocityX": -3.7242699153978625,
-            "velocityY": 0.6636413327346956,
-            "timestamp": 0.8634979941386624
-        },
-        {
-            "x": 5.523572440187452,
-            "y": 4.068080526231475,
-            "heading": 0.11166809511552328,
-            "angularVelocity": 0.24384512325081395,
-            "velocityX": -3.6894473235002847,
-            "velocityY": 0.28134563124547074,
-            "timestamp": 0.9419978117876316
-        },
-        {
-            "x": 5.248612904615211,
-            "y": 4.139233352038044,
-            "heading": 0.1457449942262438,
-            "angularVelocity": 0.43410163400767243,
-            "velocityX": -3.502677379478612,
-            "velocityY": 0.9064075298205869,
-            "timestamp": 1.020497629436601
-        },
-        {
-            "x": 4.992805004119873,
-            "y": 4.258058071136475,
-            "heading": 0.1827080793073441,
-            "angularVelocity": 0.47086841967443066,
-            "velocityX": -3.258706939158039,
-            "velocityY": 1.513694205377446,
-            "timestamp": 1.0989974470855703
-        },
-        {
-            "x": 4.782367562736006,
-            "y": 4.400850786547769,
-            "heading": 0.2087590252622848,
-            "angularVelocity": 0.37195010692359587,
-            "velocityX": -3.004582979783548,
-            "velocityY": 2.0387653429944033,
-            "timestamp": 1.1690362653840851
-        },
-        {
-            "x": 4.593366685154892,
-            "y": 4.57831882888555,
-            "heading": 0.22294244363545143,
-            "angularVelocity": 0.20250796226614393,
-            "velocityX": -2.6985160825467664,
-            "velocityY": 2.5338526070127387,
-            "timestamp": 1.2390750836825999
-        },
-        {
-            "x": 4.4303113780727985,
-            "y": 4.787155425256865,
-            "heading": 0.2229432862167668,
-            "angularVelocity": 0.000012030204616055241,
-            "velocityX": -2.3280705049466333,
-            "velocityY": 2.9817264403466797,
-            "timestamp": 1.3091139019811147
-        },
-        {
-            "x": 4.301341913399859,
-            "y": 5.018600438478609,
-            "heading": 0.22294334445620553,
-            "angularVelocity": 8.315308587772638e-7,
-            "velocityX": -1.8413997809506994,
-            "velocityY": 3.3045248170136206,
-            "timestamp": 1.3791527202796294
-        },
-        {
-            "x": 4.209527987601824,
-            "y": 5.267136397095996,
-            "heading": 0.22294334228224533,
-            "angularVelocity": -3.103936119065208e-8,
-            "velocityX": -1.3109005552544912,
-            "velocityY": 3.5485458586481258,
-            "timestamp": 1.4491915385781442
-        },
-        {
-            "x": 4.095163246631834,
-            "y": 5.506135613505712,
-            "heading": 0.22294334005402738,
-            "angularVelocity": -3.181404259759038e-8,
-            "velocityX": -1.6328765068917115,
-            "velocityY": 3.412382193414732,
-            "timestamp": 1.519230356876659
-        },
-        {
-            "x": 3.9453760475707735,
-            "y": 5.724684722787353,
-            "heading": 0.22294333755566106,
-            "angularVelocity": -3.567116580835512e-8,
-            "velocityX": -2.138631157692488,
-            "velocityY": 3.120399723908581,
-            "timestamp": 1.5892691751751737
-        },
-        {
-            "x": 3.7637358458155963,
-            "y": 5.917575181509908,
-            "heading": 0.22294327041118944,
-            "angularVelocity": -9.586751071941434e-7,
-            "velocityX": -2.5934218504529762,
-            "velocityY": 2.7540507308438293,
-            "timestamp": 1.6593079934736885
-        },
-        {
-            "x": 3.5705120272666107,
-            "y": 6.071441420591724,
-            "heading": 0.20870193370475335,
-            "angularVelocity": -0.20333490844666557,
-            "velocityX": -2.7588103746352264,
-            "velocityY": 2.1968708613263175,
-            "timestamp": 1.7293468117722033
-        },
-        {
-            "x": 3.3793495617108644,
-            "y": 6.184488312917552,
-            "heading": 0.18879422691132347,
-            "angularVelocity": -0.2842381878658059,
-            "velocityX": -2.7293787959290245,
-            "velocityY": 1.614060532031302,
-            "timestamp": 1.799385630070718
-        },
-        {
-            "x": 3.1945436254003248,
-            "y": 6.257137552443532,
-            "heading": 0.1660945073730067,
-            "angularVelocity": -0.3241019778712817,
-            "velocityX": -2.638621564444474,
-            "velocityY": 1.037271063258847,
-            "timestamp": 1.8694244483692328
-        },
-        {
-            "x": 3.018114205815821,
-            "y": 6.289753496686376,
-            "heading": 0.14197845255388317,
-            "angularVelocity": -0.34432412489220054,
-            "velocityX": -2.519023362625089,
-            "velocityY": 0.46568381699052885,
-            "timestamp": 1.9394632666677476
-        },
-        {
-            "x": 2.851226329803467,
-            "y": 6.282596111297607,
-            "heading": 0.11724722927389757,
-            "angularVelocity": -0.3531073750355408,
-            "velocityX": -2.382791144492779,
-            "velocityY": -0.10219169258892027,
-            "timestamp": 2.0095020849662624
-        },
-        {
-            "x": 2.661249908723212,
-            "y": 6.213695182206588,
-            "heading": 0.08663876117142615,
-            "angularVelocity": -0.3547239131111928,
-            "velocityX": -2.2016514926122546,
-            "velocityY": -0.7984982163199495,
-            "timestamp": 2.0957902289879784
-        },
-        {
-            "x": 2.4936356536291973,
-            "y": 6.0868793491721815,
-            "heading": 0.056963129639284946,
-            "angularVelocity": -0.3439131976774355,
-            "velocityX": -1.9424946149239757,
-            "velocityY": -1.4696785343117862,
-            "timestamp": 2.1820783730096944
-        },
-        {
-            "x": 2.3641635269275443,
-            "y": 5.911089762977232,
-            "heading": 0.030889400871559385,
-            "angularVelocity": -0.3021704669089439,
-            "velocityX": -1.500462527842606,
-            "velocityY": -2.037239161740341,
-            "timestamp": 2.2683665170314105
-        },
-        {
-            "x": 2.2965373597407854,
-            "y": 5.740054606624193,
-            "heading": 0.01408623201300084,
-            "angularVelocity": -0.19473322840668433,
-            "velocityX": -0.7837249016474405,
-            "velocityY": -1.9821397051956142,
-            "timestamp": 2.3546546610531265
-        },
-        {
-            "x": 2.263390196849459,
-            "y": 5.620608972310546,
-            "heading": 0.004408633441468959,
-            "angularVelocity": -0.11215444116055903,
-            "velocityX": -0.38414504410927525,
-            "velocityY": -1.384264729156902,
-            "timestamp": 2.4409428050748425
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -1.7581112791577623e-31,
-            "angularVelocity": -0.05109199521503166,
-            "velocityX": -0.1551800308289656,
-            "velocityY": -0.7024020854510996,
-            "timestamp": 2.5272309490965585
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 5.961493131289069e-30,
-            "velocityX": -9.776555553264502e-32,
-            "velocityY": 0,
-            "timestamp": 2.6135190931182746
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W3.1.traj
deleted file mode 100644
index 15a1cd3f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W3.1.traj	
+++ /dev/null
@@ -1,274 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -4.62162066936404e-33,
-            "velocityX": 3.0688151507515204e-31,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.623584376302727,
-            "y": 2.4297857949616017,
-            "heading": 0.033539307380936634,
-            "angularVelocity": 0.38483343876921716,
-            "velocityX": -0.6473186288895079,
-            "velocityY": -0.3122585684118786,
-            "timestamp": 0.08715278871867994
-        },
-        {
-            "x": 7.510615150776605,
-            "y": 2.375312721693156,
-            "heading": 0.0969954791736376,
-            "angularVelocity": 0.7281025968948721,
-            "velocityX": -1.2962204329545195,
-            "velocityY": -0.6250296068468768,
-            "timestamp": 0.1743055774373599
-        },
-        {
-            "x": 7.340880293995689,
-            "y": 2.2935273668964844,
-            "heading": 0.1838752961952568,
-            "angularVelocity": 0.9968678948651678,
-            "velocityX": -1.9475550843106406,
-            "velocityY": -0.9384135149210906,
-            "timestamp": 0.26145836615603985
-        },
-        {
-            "x": 7.114069036249808,
-            "y": 2.1844157133503646,
-            "heading": 0.27996244701307155,
-            "angularVelocity": 1.1025137833279668,
-            "velocityX": -2.6024555390648842,
-            "velocityY": -1.251958258023393,
-            "timestamp": 0.3486111548747198
-        },
-        {
-            "x": 6.831104073589339,
-            "y": 2.0490142604857513,
-            "heading": 0.32912237638568254,
-            "angularVelocity": 0.5640660510737534,
-            "velocityX": -3.246768885088134,
-            "velocityY": -1.5536101007814562,
-            "timestamp": 0.4357639435933997
-        },
-        {
-            "x": 6.534528280813305,
-            "y": 1.9049975621731787,
-            "heading": 0.3291224030694369,
-            "angularVelocity": 3.0617212321522066e-7,
-            "velocityX": -3.4029409401155197,
-            "velocityY": -1.6524623070575855,
-            "timestamp": 0.5229167323120797
-        },
-        {
-            "x": 6.237952262222522,
-            "y": 1.7609813288852085,
-            "heading": 0.3291224297529421,
-            "angularVelocity": 3.0616926449614015e-7,
-            "velocityX": -3.402943531137019,
-            "velocityY": -1.6524569713178037,
-            "timestamp": 0.6100695210307596
-        },
-        {
-            "x": 5.936787262454918,
-            "y": 1.6268259970502223,
-            "heading": 0.32912245667190637,
-            "angularVelocity": 3.0887094574020004e-7,
-            "velocityX": -3.4555979699024117,
-            "velocityY": -1.5393119808021931,
-            "timestamp": 0.6972223097494396
-        },
-        {
-            "x": 5.615527521313305,
-            "y": 1.5527301986214108,
-            "heading": 0.32912248658360005,
-            "angularVelocity": 3.4320982850713803e-7,
-            "velocityX": -3.6861670850098154,
-            "velocityY": -0.850182759704744,
-            "timestamp": 0.7843750984681195
-        },
-        {
-            "x": 5.286027461953728,
-            "y": 1.5414290964422628,
-            "heading": 0.32912252585289026,
-            "angularVelocity": 4.50579847209181e-7,
-            "velocityX": -3.780717337951944,
-            "velocityY": -0.12967000075725457,
-            "timestamp": 0.8715278871867994
-        },
-        {
-            "x": 4.960446284223111,
-            "y": 1.593339788205863,
-            "heading": 0.3291233220408656,
-            "angularVelocity": 0.000009135542155699871,
-            "velocityX": -3.7357516898462033,
-            "velocityY": 0.5956285797252272,
-            "timestamp": 0.9586806759054793
-        },
-        {
-            "x": 4.650797848549272,
-            "y": 1.7065466836899894,
-            "heading": 0.3291235959637348,
-            "angularVelocity": 0.0000031430189815062194,
-            "velocityX": -3.5529377800330906,
-            "velocityY": 1.2989474823295228,
-            "timestamp": 1.0458334646241594
-        },
-        {
-            "x": 4.368508202692925,
-            "y": 1.8768714467040708,
-            "heading": 0.32912381701495697,
-            "angularVelocity": 0.000002536364302217629,
-            "velocityX": -3.2390202310972316,
-            "velocityY": 1.9543237286860882,
-            "timestamp": 1.1329862533428394
-        },
-        {
-            "x": 4.092384496633659,
-            "y": 2.0570204638146983,
-            "heading": 0.3291240376025158,
-            "angularVelocity": 0.000002531044182467546,
-            "velocityX": -3.1682716080441704,
-            "velocityY": 2.067048223690569,
-            "timestamp": 1.2201390420615195
-        },
-        {
-            "x": 3.8162567615509033,
-            "y": 2.2371633052825928,
-            "heading": 0.32912425818941665,
-            "angularVelocity": 0.0000025310366317804206,
-            "velocityX": -3.1683178374712258,
-            "velocityY": 2.066977363735053,
-            "timestamp": 1.3072918307801995
-        },
-        {
-            "x": 3.558387727399027,
-            "y": 2.3518646644990793,
-            "heading": 0.32912440943088817,
-            "angularVelocity": 0.0000020272145178599712,
-            "velocityX": -3.4564319203160125,
-            "velocityY": 1.5374371746628512,
-            "timestamp": 1.3818973893726278
-        },
-        {
-            "x": 3.300516056227942,
-            "y": 2.466560095044955,
-            "heading": 0.3291245606718786,
-            "angularVelocity": 0.000002027208070545675,
-            "velocityX": -3.4564672664652627,
-            "velocityY": 1.5373577077876843,
-            "timestamp": 1.4565029479650562
-        },
-        {
-            "x": 3.052458337002912,
-            "y": 2.601171414843865,
-            "heading": 0.329124716605593,
-            "angularVelocity": 0.0000020901085294853097,
-            "velocityX": -3.3249227524744356,
-            "velocityY": 1.804306841723335,
-            "timestamp": 1.5311085065574845
-        },
-        {
-            "x": 2.829814908557799,
-            "y": 2.7746151638451657,
-            "heading": 0.3291249161701291,
-            "angularVelocity": 0.000002674928516490017,
-            "velocityX": -2.984273995740964,
-            "velocityY": 2.3248100044237634,
-            "timestamp": 1.6057140651499129
-        },
-        {
-            "x": 2.638606432152533,
-            "y": 2.982201542308141,
-            "heading": 0.3291253317978473,
-            "angularVelocity": 0.000005571002027267265,
-            "velocityX": -2.562925337103115,
-            "velocityY": 2.7824519027734036,
-            "timestamp": 1.6803196237423412
-        },
-        {
-            "x": 2.4840036736729245,
-            "y": 3.218316897029485,
-            "heading": 0.3291273340604081,
-            "angularVelocity": 0.000026837980957261232,
-            "velocityX": -2.0722686271167303,
-            "velocityY": 3.1648493647939433,
-            "timestamp": 1.7549251823347696
-        },
-        {
-            "x": 2.3733884856918945,
-            "y": 3.4659660107751304,
-            "heading": 0.2954274954004952,
-            "angularVelocity": -0.4517068070492687,
-            "velocityX": -1.4826668423638905,
-            "velocityY": 3.31944587532086,
-            "timestamp": 1.829530740927198
-        },
-        {
-            "x": 2.305023098579148,
-            "y": 3.7064756804968377,
-            "heading": 0.19598318894217118,
-            "angularVelocity": -1.3329342790875696,
-            "velocityX": -0.9163578210871337,
-            "velocityY": 3.2237500028063106,
-            "timestamp": 1.9041362995196263
-        },
-        {
-            "x": 2.272475779606133,
-            "y": 3.9200958593762563,
-            "heading": 0.053473340322528436,
-            "angularVelocity": -1.9101773555262378,
-            "velocityX": -0.43625863256144176,
-            "velocityY": 2.8633279196585013,
-            "timestamp": 1.9787418581120546
-        },
-        {
-            "x": 2.2677016800416907,
-            "y": 4.09662450199971,
-            "heading": -0.09830221598718161,
-            "angularVelocity": -2.034373298360552,
-            "velocityX": -0.06399120460344819,
-            "velocityY": 2.3661593848232187,
-            "timestamp": 2.053347416704483
-        },
-        {
-            "x": 2.2778239174156503,
-            "y": 4.229436050413757,
-            "heading": -0.23380831521279008,
-            "angularVelocity": -1.8163003103546322,
-            "velocityX": 0.13567671853055638,
-            "velocityY": 1.7801830174558415,
-            "timestamp": 2.1279529752969113
-        },
-        {
-            "x": 2.29107059750262,
-            "y": 4.316936633046431,
-            "heading": -0.33501872696397506,
-            "angularVelocity": -1.3566068488823915,
-            "velocityX": 0.1775562081015563,
-            "velocityY": 1.1728426712906341,
-            "timestamp": 2.2025585338893396
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.7369594715641588,
-            "velocityX": 0.11968816621507666,
-            "velocityY": 0.5772139203303202,
-            "timestamp": 2.277164092481768
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -8.092865962806098e-32,
-            "velocityX": 0,
-            "velocityY": 4.648171279746289e-33,
-            "timestamp": 2.3517696510741963
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W3.traj
deleted file mode 100644
index 15a1cd3f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W3.traj	
+++ /dev/null
@@ -1,274 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -4.62162066936404e-33,
-            "velocityX": 3.0688151507515204e-31,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.623584376302727,
-            "y": 2.4297857949616017,
-            "heading": 0.033539307380936634,
-            "angularVelocity": 0.38483343876921716,
-            "velocityX": -0.6473186288895079,
-            "velocityY": -0.3122585684118786,
-            "timestamp": 0.08715278871867994
-        },
-        {
-            "x": 7.510615150776605,
-            "y": 2.375312721693156,
-            "heading": 0.0969954791736376,
-            "angularVelocity": 0.7281025968948721,
-            "velocityX": -1.2962204329545195,
-            "velocityY": -0.6250296068468768,
-            "timestamp": 0.1743055774373599
-        },
-        {
-            "x": 7.340880293995689,
-            "y": 2.2935273668964844,
-            "heading": 0.1838752961952568,
-            "angularVelocity": 0.9968678948651678,
-            "velocityX": -1.9475550843106406,
-            "velocityY": -0.9384135149210906,
-            "timestamp": 0.26145836615603985
-        },
-        {
-            "x": 7.114069036249808,
-            "y": 2.1844157133503646,
-            "heading": 0.27996244701307155,
-            "angularVelocity": 1.1025137833279668,
-            "velocityX": -2.6024555390648842,
-            "velocityY": -1.251958258023393,
-            "timestamp": 0.3486111548747198
-        },
-        {
-            "x": 6.831104073589339,
-            "y": 2.0490142604857513,
-            "heading": 0.32912237638568254,
-            "angularVelocity": 0.5640660510737534,
-            "velocityX": -3.246768885088134,
-            "velocityY": -1.5536101007814562,
-            "timestamp": 0.4357639435933997
-        },
-        {
-            "x": 6.534528280813305,
-            "y": 1.9049975621731787,
-            "heading": 0.3291224030694369,
-            "angularVelocity": 3.0617212321522066e-7,
-            "velocityX": -3.4029409401155197,
-            "velocityY": -1.6524623070575855,
-            "timestamp": 0.5229167323120797
-        },
-        {
-            "x": 6.237952262222522,
-            "y": 1.7609813288852085,
-            "heading": 0.3291224297529421,
-            "angularVelocity": 3.0616926449614015e-7,
-            "velocityX": -3.402943531137019,
-            "velocityY": -1.6524569713178037,
-            "timestamp": 0.6100695210307596
-        },
-        {
-            "x": 5.936787262454918,
-            "y": 1.6268259970502223,
-            "heading": 0.32912245667190637,
-            "angularVelocity": 3.0887094574020004e-7,
-            "velocityX": -3.4555979699024117,
-            "velocityY": -1.5393119808021931,
-            "timestamp": 0.6972223097494396
-        },
-        {
-            "x": 5.615527521313305,
-            "y": 1.5527301986214108,
-            "heading": 0.32912248658360005,
-            "angularVelocity": 3.4320982850713803e-7,
-            "velocityX": -3.6861670850098154,
-            "velocityY": -0.850182759704744,
-            "timestamp": 0.7843750984681195
-        },
-        {
-            "x": 5.286027461953728,
-            "y": 1.5414290964422628,
-            "heading": 0.32912252585289026,
-            "angularVelocity": 4.50579847209181e-7,
-            "velocityX": -3.780717337951944,
-            "velocityY": -0.12967000075725457,
-            "timestamp": 0.8715278871867994
-        },
-        {
-            "x": 4.960446284223111,
-            "y": 1.593339788205863,
-            "heading": 0.3291233220408656,
-            "angularVelocity": 0.000009135542155699871,
-            "velocityX": -3.7357516898462033,
-            "velocityY": 0.5956285797252272,
-            "timestamp": 0.9586806759054793
-        },
-        {
-            "x": 4.650797848549272,
-            "y": 1.7065466836899894,
-            "heading": 0.3291235959637348,
-            "angularVelocity": 0.0000031430189815062194,
-            "velocityX": -3.5529377800330906,
-            "velocityY": 1.2989474823295228,
-            "timestamp": 1.0458334646241594
-        },
-        {
-            "x": 4.368508202692925,
-            "y": 1.8768714467040708,
-            "heading": 0.32912381701495697,
-            "angularVelocity": 0.000002536364302217629,
-            "velocityX": -3.2390202310972316,
-            "velocityY": 1.9543237286860882,
-            "timestamp": 1.1329862533428394
-        },
-        {
-            "x": 4.092384496633659,
-            "y": 2.0570204638146983,
-            "heading": 0.3291240376025158,
-            "angularVelocity": 0.000002531044182467546,
-            "velocityX": -3.1682716080441704,
-            "velocityY": 2.067048223690569,
-            "timestamp": 1.2201390420615195
-        },
-        {
-            "x": 3.8162567615509033,
-            "y": 2.2371633052825928,
-            "heading": 0.32912425818941665,
-            "angularVelocity": 0.0000025310366317804206,
-            "velocityX": -3.1683178374712258,
-            "velocityY": 2.066977363735053,
-            "timestamp": 1.3072918307801995
-        },
-        {
-            "x": 3.558387727399027,
-            "y": 2.3518646644990793,
-            "heading": 0.32912440943088817,
-            "angularVelocity": 0.0000020272145178599712,
-            "velocityX": -3.4564319203160125,
-            "velocityY": 1.5374371746628512,
-            "timestamp": 1.3818973893726278
-        },
-        {
-            "x": 3.300516056227942,
-            "y": 2.466560095044955,
-            "heading": 0.3291245606718786,
-            "angularVelocity": 0.000002027208070545675,
-            "velocityX": -3.4564672664652627,
-            "velocityY": 1.5373577077876843,
-            "timestamp": 1.4565029479650562
-        },
-        {
-            "x": 3.052458337002912,
-            "y": 2.601171414843865,
-            "heading": 0.329124716605593,
-            "angularVelocity": 0.0000020901085294853097,
-            "velocityX": -3.3249227524744356,
-            "velocityY": 1.804306841723335,
-            "timestamp": 1.5311085065574845
-        },
-        {
-            "x": 2.829814908557799,
-            "y": 2.7746151638451657,
-            "heading": 0.3291249161701291,
-            "angularVelocity": 0.000002674928516490017,
-            "velocityX": -2.984273995740964,
-            "velocityY": 2.3248100044237634,
-            "timestamp": 1.6057140651499129
-        },
-        {
-            "x": 2.638606432152533,
-            "y": 2.982201542308141,
-            "heading": 0.3291253317978473,
-            "angularVelocity": 0.000005571002027267265,
-            "velocityX": -2.562925337103115,
-            "velocityY": 2.7824519027734036,
-            "timestamp": 1.6803196237423412
-        },
-        {
-            "x": 2.4840036736729245,
-            "y": 3.218316897029485,
-            "heading": 0.3291273340604081,
-            "angularVelocity": 0.000026837980957261232,
-            "velocityX": -2.0722686271167303,
-            "velocityY": 3.1648493647939433,
-            "timestamp": 1.7549251823347696
-        },
-        {
-            "x": 2.3733884856918945,
-            "y": 3.4659660107751304,
-            "heading": 0.2954274954004952,
-            "angularVelocity": -0.4517068070492687,
-            "velocityX": -1.4826668423638905,
-            "velocityY": 3.31944587532086,
-            "timestamp": 1.829530740927198
-        },
-        {
-            "x": 2.305023098579148,
-            "y": 3.7064756804968377,
-            "heading": 0.19598318894217118,
-            "angularVelocity": -1.3329342790875696,
-            "velocityX": -0.9163578210871337,
-            "velocityY": 3.2237500028063106,
-            "timestamp": 1.9041362995196263
-        },
-        {
-            "x": 2.272475779606133,
-            "y": 3.9200958593762563,
-            "heading": 0.053473340322528436,
-            "angularVelocity": -1.9101773555262378,
-            "velocityX": -0.43625863256144176,
-            "velocityY": 2.8633279196585013,
-            "timestamp": 1.9787418581120546
-        },
-        {
-            "x": 2.2677016800416907,
-            "y": 4.09662450199971,
-            "heading": -0.09830221598718161,
-            "angularVelocity": -2.034373298360552,
-            "velocityX": -0.06399120460344819,
-            "velocityY": 2.3661593848232187,
-            "timestamp": 2.053347416704483
-        },
-        {
-            "x": 2.2778239174156503,
-            "y": 4.229436050413757,
-            "heading": -0.23380831521279008,
-            "angularVelocity": -1.8163003103546322,
-            "velocityX": 0.13567671853055638,
-            "velocityY": 1.7801830174558415,
-            "timestamp": 2.1279529752969113
-        },
-        {
-            "x": 2.29107059750262,
-            "y": 4.316936633046431,
-            "heading": -0.33501872696397506,
-            "angularVelocity": -1.3566068488823915,
-            "velocityX": 0.1775562081015563,
-            "velocityY": 1.1728426712906341,
-            "timestamp": 2.2025585338893396
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.7369594715641588,
-            "velocityX": 0.11968816621507666,
-            "velocityY": 0.5772139203303202,
-            "timestamp": 2.277164092481768
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -8.092865962806098e-32,
-            "velocityX": 0,
-            "velocityY": 4.648171279746289e-33,
-            "timestamp": 2.3517696510741963
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.1.traj
deleted file mode 100644
index 8293ba75..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.1.traj	
+++ /dev/null
@@ -1,220 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -1.1520076473353784e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 0.8649552704871435,
-            "heading": -1.2072196888077206e-18,
-            "angularVelocity": -1.2485272283928241e-17,
-            "velocityX": -1.2865550388289577e-22,
-            "velocityY": 0.8062267267095138,
-            "timestamp": 0.09669149868710694
-        },
-        {
-            "x": 7.68,
-            "y": 1.0208658083668138,
-            "heading": -3.6224298175784965e-18,
-            "angularVelocity": -2.497851580797786e-17,
-            "velocityX": -3.7534290948711127e-22,
-            "velocityY": 1.6124534214139756,
-            "timestamp": 0.19338299737421388
-        },
-        {
-            "x": 7.68,
-            "y": 1.254731607449781,
-            "heading": -7.246204108405477e-18,
-            "angularVelocity": -3.7477692864160655e-17,
-            "velocityX": -8.581224837351263e-22,
-            "velocityY": 2.4186800521083573,
-            "timestamp": 0.29007449606132085
-        },
-        {
-            "x": 7.68,
-            "y": 1.5665526491683786,
-            "heading": -1.208276035533523e-17,
-            "angularVelocity": -5.0020491067317066e-17,
-            "velocityX": -2.0493771641846554e-21,
-            "velocityY": 3.2249064907727645,
-            "timestamp": 0.38676599474842777
-        },
-        {
-            "x": 7.68,
-            "y": 1.9323308421442915,
-            "heading": -4.9593132234185194e-18,
-            "angularVelocity": 7.367190734076976e-17,
-            "velocityX": 8.922575847291113e-21,
-            "velocityY": 3.782940568121389,
-            "timestamp": 0.4834574934355347
-        },
-        {
-            "x": 7.68,
-            "y": 2.298109035120243,
-            "heading": 4.533704301231546e-17,
-            "angularVelocity": 5.201735097557347e-16,
-            "velocityX": 4.097272417616201e-22,
-            "velocityY": 3.782940568121787,
-            "timestamp": 0.5801489921226416
-        },
-        {
-            "x": 7.68,
-            "y": 2.6638872280961943,
-            "heading": 2.5191507323029372e-17,
-            "angularVelocity": -2.083485721374208e-16,
-            "velocityX": 1.8682331175835897e-21,
-            "velocityY": 3.782940568121787,
-            "timestamp": 0.6768404908097485
-        },
-        {
-            "x": 7.68,
-            "y": 3.0296654210721456,
-            "heading": 5.2668856754382746e-17,
-            "angularVelocity": 2.8417544256148633e-16,
-            "velocityX": 2.5170076181953607e-21,
-            "velocityY": 3.782940568121787,
-            "timestamp": 0.7735319894968554
-        },
-        {
-            "x": 7.68,
-            "y": 3.395443614048097,
-            "heading": 4.929611386988728e-17,
-            "angularVelocity": -3.48814831737134e-17,
-            "velocityX": 3.0934195236040766e-21,
-            "velocityY": 3.782940568121787,
-            "timestamp": 0.8702234881839623
-        },
-        {
-            "x": 7.68,
-            "y": 3.7612218070240484,
-            "heading": 4.8005098046144365e-17,
-            "angularVelocity": -1.3351906227498234e-17,
-            "velocityX": -1.4483318353601114e-20,
-            "velocityY": 3.782940568121787,
-            "timestamp": 0.9669149868710692
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.8136822799879804e-17,
-            "angularVelocity": -1.0205938867695289e-16,
-            "velocityX": -1.0465805642283078e-22,
-            "velocityY": 3.782940568121787,
-            "timestamp": 1.0636064855581762
-        },
-        {
-            "x": 7.68,
-            "y": 4.492778192975952,
-            "heading": 4.310486593468924e-17,
-            "angularVelocity": 5.1380350929111975e-17,
-            "velocityX": 2.042774398860212e-21,
-            "velocityY": 3.782940568121787,
-            "timestamp": 1.1602979842452832
-        },
-        {
-            "x": 7.68,
-            "y": 4.8585563859519025,
-            "heading": 1.5339712420766394e-17,
-            "angularVelocity": -2.8715196155735124e-16,
-            "velocityX": 3.895366641644577e-22,
-            "velocityY": 3.782940568121787,
-            "timestamp": 1.2569894829323902
-        },
-        {
-            "x": 7.68,
-            "y": 5.224334578927854,
-            "heading": -2.710858238227145e-17,
-            "angularVelocity": -4.390075175101735e-16,
-            "velocityX": -3.863371094269421e-21,
-            "velocityY": 3.782940568121787,
-            "timestamp": 1.3536809816194972
-        },
-        {
-            "x": 7.68,
-            "y": 5.590112771903805,
-            "heading": -3.378098304971619e-17,
-            "angularVelocity": -6.900710774001486e-17,
-            "velocityX": -4.042329982282679e-22,
-            "velocityY": 3.782940568121787,
-            "timestamp": 1.4503724803066043
-        },
-        {
-            "x": 7.68,
-            "y": 5.955890964879757,
-            "heading": -3.941496183315801e-17,
-            "angularVelocity": -5.826757119208287e-17,
-            "velocityX": 2.6937052538433574e-21,
-            "velocityY": 3.782940568121787,
-            "timestamp": 1.5470639789937113
-        },
-        {
-            "x": 7.68,
-            "y": 6.321669157855708,
-            "heading": -1.9923808577496458e-17,
-            "angularVelocity": 2.0158083720198419e-16,
-            "velocityX": -4.499746264941967e-21,
-            "velocityY": 3.782940568121787,
-            "timestamp": 1.6437554776808183
-        },
-        {
-            "x": 7.68,
-            "y": 6.68744735083162,
-            "heading": 1.1959451428437676e-17,
-            "angularVelocity": 3.297421225035248e-16,
-            "velocityX": 5.431014750114918e-21,
-            "velocityY": 3.782940568121389,
-            "timestamp": 1.7404469763679253
-        },
-        {
-            "x": 7.68,
-            "y": 6.999268392550218,
-            "heading": 7.18685516064291e-18,
-            "angularVelocity": -4.935900605111643e-17,
-            "velocityX": -2.4046856363551417e-22,
-            "velocityY": 3.2249064907727645,
-            "timestamp": 1.8371384750550324
-        },
-        {
-            "x": 7.68,
-            "y": 7.2331341916331855,
-            "heading": 3.596188330325507e-18,
-            "angularVelocity": -3.713528985888002e-17,
-            "velocityX": -1.8035546521541034e-22,
-            "velocityY": 2.4186800521083573,
-            "timestamp": 1.9338299737421394
-        },
-        {
-            "x": 7.68,
-            "y": 7.3890447295128565,
-            "heading": 1.199348678010309e-18,
-            "angularVelocity": -2.4788525206448663e-17,
-            "velocityX": -1.2023814854884031e-22,
-            "velocityY": 1.6124534214139756,
-            "timestamp": 2.030521472429246
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": -1.2403868947555692e-17,
-            "velocityX": -6.011942579686287e-23,
-            "velocityY": 0.8062267267095138,
-            "timestamp": 2.127212971116353
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -2.719452927302828e-33,
-            "velocityY": 0,
-            "timestamp": 2.2239044698034602
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.traj
deleted file mode 100644
index 8293ba75..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.traj	
+++ /dev/null
@@ -1,220 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -1.1520076473353784e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 0.8649552704871435,
-            "heading": -1.2072196888077206e-18,
-            "angularVelocity": -1.2485272283928241e-17,
-            "velocityX": -1.2865550388289577e-22,
-            "velocityY": 0.8062267267095138,
-            "timestamp": 0.09669149868710694
-        },
-        {
-            "x": 7.68,
-            "y": 1.0208658083668138,
-            "heading": -3.6224298175784965e-18,
-            "angularVelocity": -2.497851580797786e-17,
-            "velocityX": -3.7534290948711127e-22,
-            "velocityY": 1.6124534214139756,
-            "timestamp": 0.19338299737421388
-        },
-        {
-            "x": 7.68,
-            "y": 1.254731607449781,
-            "heading": -7.246204108405477e-18,
-            "angularVelocity": -3.7477692864160655e-17,
-            "velocityX": -8.581224837351263e-22,
-            "velocityY": 2.4186800521083573,
-            "timestamp": 0.29007449606132085
-        },
-        {
-            "x": 7.68,
-            "y": 1.5665526491683786,
-            "heading": -1.208276035533523e-17,
-            "angularVelocity": -5.0020491067317066e-17,
-            "velocityX": -2.0493771641846554e-21,
-            "velocityY": 3.2249064907727645,
-            "timestamp": 0.38676599474842777
-        },
-        {
-            "x": 7.68,
-            "y": 1.9323308421442915,
-            "heading": -4.9593132234185194e-18,
-            "angularVelocity": 7.367190734076976e-17,
-            "velocityX": 8.922575847291113e-21,
-            "velocityY": 3.782940568121389,
-            "timestamp": 0.4834574934355347
-        },
-        {
-            "x": 7.68,
-            "y": 2.298109035120243,
-            "heading": 4.533704301231546e-17,
-            "angularVelocity": 5.201735097557347e-16,
-            "velocityX": 4.097272417616201e-22,
-            "velocityY": 3.782940568121787,
-            "timestamp": 0.5801489921226416
-        },
-        {
-            "x": 7.68,
-            "y": 2.6638872280961943,
-            "heading": 2.5191507323029372e-17,
-            "angularVelocity": -2.083485721374208e-16,
-            "velocityX": 1.8682331175835897e-21,
-            "velocityY": 3.782940568121787,
-            "timestamp": 0.6768404908097485
-        },
-        {
-            "x": 7.68,
-            "y": 3.0296654210721456,
-            "heading": 5.2668856754382746e-17,
-            "angularVelocity": 2.8417544256148633e-16,
-            "velocityX": 2.5170076181953607e-21,
-            "velocityY": 3.782940568121787,
-            "timestamp": 0.7735319894968554
-        },
-        {
-            "x": 7.68,
-            "y": 3.395443614048097,
-            "heading": 4.929611386988728e-17,
-            "angularVelocity": -3.48814831737134e-17,
-            "velocityX": 3.0934195236040766e-21,
-            "velocityY": 3.782940568121787,
-            "timestamp": 0.8702234881839623
-        },
-        {
-            "x": 7.68,
-            "y": 3.7612218070240484,
-            "heading": 4.8005098046144365e-17,
-            "angularVelocity": -1.3351906227498234e-17,
-            "velocityX": -1.4483318353601114e-20,
-            "velocityY": 3.782940568121787,
-            "timestamp": 0.9669149868710692
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.8136822799879804e-17,
-            "angularVelocity": -1.0205938867695289e-16,
-            "velocityX": -1.0465805642283078e-22,
-            "velocityY": 3.782940568121787,
-            "timestamp": 1.0636064855581762
-        },
-        {
-            "x": 7.68,
-            "y": 4.492778192975952,
-            "heading": 4.310486593468924e-17,
-            "angularVelocity": 5.1380350929111975e-17,
-            "velocityX": 2.042774398860212e-21,
-            "velocityY": 3.782940568121787,
-            "timestamp": 1.1602979842452832
-        },
-        {
-            "x": 7.68,
-            "y": 4.8585563859519025,
-            "heading": 1.5339712420766394e-17,
-            "angularVelocity": -2.8715196155735124e-16,
-            "velocityX": 3.895366641644577e-22,
-            "velocityY": 3.782940568121787,
-            "timestamp": 1.2569894829323902
-        },
-        {
-            "x": 7.68,
-            "y": 5.224334578927854,
-            "heading": -2.710858238227145e-17,
-            "angularVelocity": -4.390075175101735e-16,
-            "velocityX": -3.863371094269421e-21,
-            "velocityY": 3.782940568121787,
-            "timestamp": 1.3536809816194972
-        },
-        {
-            "x": 7.68,
-            "y": 5.590112771903805,
-            "heading": -3.378098304971619e-17,
-            "angularVelocity": -6.900710774001486e-17,
-            "velocityX": -4.042329982282679e-22,
-            "velocityY": 3.782940568121787,
-            "timestamp": 1.4503724803066043
-        },
-        {
-            "x": 7.68,
-            "y": 5.955890964879757,
-            "heading": -3.941496183315801e-17,
-            "angularVelocity": -5.826757119208287e-17,
-            "velocityX": 2.6937052538433574e-21,
-            "velocityY": 3.782940568121787,
-            "timestamp": 1.5470639789937113
-        },
-        {
-            "x": 7.68,
-            "y": 6.321669157855708,
-            "heading": -1.9923808577496458e-17,
-            "angularVelocity": 2.0158083720198419e-16,
-            "velocityX": -4.499746264941967e-21,
-            "velocityY": 3.782940568121787,
-            "timestamp": 1.6437554776808183
-        },
-        {
-            "x": 7.68,
-            "y": 6.68744735083162,
-            "heading": 1.1959451428437676e-17,
-            "angularVelocity": 3.297421225035248e-16,
-            "velocityX": 5.431014750114918e-21,
-            "velocityY": 3.782940568121389,
-            "timestamp": 1.7404469763679253
-        },
-        {
-            "x": 7.68,
-            "y": 6.999268392550218,
-            "heading": 7.18685516064291e-18,
-            "angularVelocity": -4.935900605111643e-17,
-            "velocityX": -2.4046856363551417e-22,
-            "velocityY": 3.2249064907727645,
-            "timestamp": 1.8371384750550324
-        },
-        {
-            "x": 7.68,
-            "y": 7.2331341916331855,
-            "heading": 3.596188330325507e-18,
-            "angularVelocity": -3.713528985888002e-17,
-            "velocityX": -1.8035546521541034e-22,
-            "velocityY": 2.4186800521083573,
-            "timestamp": 1.9338299737421394
-        },
-        {
-            "x": 7.68,
-            "y": 7.3890447295128565,
-            "heading": 1.199348678010309e-18,
-            "angularVelocity": -2.4788525206448663e-17,
-            "velocityX": -1.2023814854884031e-22,
-            "velocityY": 1.6124534214139756,
-            "timestamp": 2.030521472429246
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": -1.2403868947555692e-17,
-            "velocityX": -6.011942579686287e-23,
-            "velocityY": 0.8062267267095138,
-            "timestamp": 2.127212971116353
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -2.719452927302828e-33,
-            "velocityY": 0,
-            "timestamp": 2.2239044698034602
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.1.traj
deleted file mode 100644
index f16ed7e1..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.1.traj	
+++ /dev/null
@@ -1,175 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.5195168592304314e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 0.8688441539517631,
-            "heading": 1.4487899486085006e-18,
-            "angularVelocity": 1.4623322004407643e-17,
-            "velocityX": 2.341988908112997e-19,
-            "velocityY": 0.8260917454276263,
-            "timestamp": 0.09907392781100396
-        },
-        {
-            "x": 7.68,
-            "y": 1.03253245874193,
-            "heading": 4.3483011920968684e-18,
-            "angularVelocity": 2.926613800008521e-17,
-            "velocityX": 4.684469740721679e-19,
-            "velocityY": 1.6521834594306495,
-            "timestamp": 0.19814785562200793
-        },
-        {
-            "x": 7.68,
-            "y": 1.2780649081437847,
-            "heading": 8.701494230539972e-18,
-            "angularVelocity": 4.393883572228609e-17,
-            "velocityX": 7.027934476495029e-19,
-            "velocityY": 2.4782751105844794,
-            "timestamp": 0.29722178343301187
-        },
-        {
-            "x": 7.68,
-            "y": 1.6054414834771922,
-            "heading": 1.4516368843485863e-17,
-            "angularVelocity": 5.869227900187472e-17,
-            "velocityX": 9.37435089599571e-19,
-            "velocityY": 3.304366573190877,
-            "timestamp": 0.39629571124401586
-        },
-        {
-            "x": 7.68,
-            "y": 1.9802322649266864,
-            "heading": 6.647154079192576e-18,
-            "angularVelocity": -7.942770553424421e-17,
-            "velocityX": 1.0709424285811041e-18,
-            "velocityY": 3.782940575087071,
-            "timestamp": 0.49536963905501985
-        },
-        {
-            "x": 7.68,
-            "y": 2.3550230463762047,
-            "heading": -4.11606275908414e-17,
-            "angularVelocity": -4.825465460619937e-16,
-            "velocityX": 1.073083844244948e-18,
-            "velocityY": 3.7829405750873115,
-            "timestamp": 0.5944435668660238
-        },
-        {
-            "x": 7.68,
-            "y": 2.729813827825723,
-            "heading": -7.635066611173202e-17,
-            "angularVelocity": -3.5518969822232415e-16,
-            "velocityX": 1.0738946259920485e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.6935174946770278
-        },
-        {
-            "x": 7.68,
-            "y": 3.1046046092752415,
-            "heading": -5.864574302033697e-17,
-            "angularVelocity": 1.787041604453991e-16,
-            "velocityX": 1.0691415340399175e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.7925914224880318
-        },
-        {
-            "x": 7.68,
-            "y": 3.4793953907247595,
-            "heading": -2.6391173603704442e-17,
-            "angularVelocity": 3.255606205313886e-16,
-            "velocityX": 1.0732289061667476e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.8916653502990358
-        },
-        {
-            "x": 7.68,
-            "y": 3.854186172174278,
-            "heading": -6.511485011951871e-17,
-            "angularVelocity": -3.9085637736788184e-16,
-            "velocityX": 1.0716697150075795e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.9907392781100398
-        },
-        {
-            "x": 7.68,
-            "y": 4.228976953623796,
-            "heading": -6.241329479526944e-17,
-            "angularVelocity": 2.7268075304360027e-17,
-            "velocityX": 1.0686111075844087e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 1.0898132059210437
-        },
-        {
-            "x": 7.68,
-            "y": 4.603767735073314,
-            "heading": -2.2737833863559818e-17,
-            "angularVelocity": 4.004631875225205e-16,
-            "velocityX": 1.0710197916722359e-18,
-            "velocityY": 3.7829405750873115,
-            "timestamp": 1.1888871337320477
-        },
-        {
-            "x": 7.68,
-            "y": 4.978558516522808,
-            "heading": -1.448944579275074e-17,
-            "angularVelocity": 8.325488100707829e-17,
-            "velocityX": 1.0714504382145178e-18,
-            "velocityY": 3.782940575087071,
-            "timestamp": 1.2879610615430517
-        },
-        {
-            "x": 7.68,
-            "y": 5.305935091856216,
-            "heading": -8.687037170545135e-18,
-            "angularVelocity": 5.856645386228716e-17,
-            "velocityX": 9.36753995288457e-19,
-            "velocityY": 3.304366573190877,
-            "timestamp": 1.3870349893540557
-        },
-        {
-            "x": 7.68,
-            "y": 5.55146754125807,
-            "heading": -4.341905072762387e-18,
-            "angularVelocity": 4.385747283685971e-17,
-            "velocityX": 7.025655481927048e-19,
-            "velocityY": 2.4782751105844794,
-            "timestamp": 1.4861089171650597
-        },
-        {
-            "x": 7.68,
-            "y": 5.715155846048237,
-            "heading": -1.4469357825353783e-18,
-            "angularVelocity": 2.9220293918225814e-17,
-            "velocityX": 4.683770471216647e-19,
-            "velocityY": 1.6521834594306495,
-            "timestamp": 1.5851828449760637
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 1.460460702963924e-17,
-            "velocityX": 2.341885280588165e-19,
-            "velocityY": 0.8260917454276263,
-            "timestamp": 1.6842567727870676
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 3.1812192470207854e-34,
-            "velocityY": 0,
-            "timestamp": 1.7833307005980716
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.traj
deleted file mode 100644
index f16ed7e1..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.traj	
+++ /dev/null
@@ -1,175 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.5195168592304314e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 0.8688441539517631,
-            "heading": 1.4487899486085006e-18,
-            "angularVelocity": 1.4623322004407643e-17,
-            "velocityX": 2.341988908112997e-19,
-            "velocityY": 0.8260917454276263,
-            "timestamp": 0.09907392781100396
-        },
-        {
-            "x": 7.68,
-            "y": 1.03253245874193,
-            "heading": 4.3483011920968684e-18,
-            "angularVelocity": 2.926613800008521e-17,
-            "velocityX": 4.684469740721679e-19,
-            "velocityY": 1.6521834594306495,
-            "timestamp": 0.19814785562200793
-        },
-        {
-            "x": 7.68,
-            "y": 1.2780649081437847,
-            "heading": 8.701494230539972e-18,
-            "angularVelocity": 4.393883572228609e-17,
-            "velocityX": 7.027934476495029e-19,
-            "velocityY": 2.4782751105844794,
-            "timestamp": 0.29722178343301187
-        },
-        {
-            "x": 7.68,
-            "y": 1.6054414834771922,
-            "heading": 1.4516368843485863e-17,
-            "angularVelocity": 5.869227900187472e-17,
-            "velocityX": 9.37435089599571e-19,
-            "velocityY": 3.304366573190877,
-            "timestamp": 0.39629571124401586
-        },
-        {
-            "x": 7.68,
-            "y": 1.9802322649266864,
-            "heading": 6.647154079192576e-18,
-            "angularVelocity": -7.942770553424421e-17,
-            "velocityX": 1.0709424285811041e-18,
-            "velocityY": 3.782940575087071,
-            "timestamp": 0.49536963905501985
-        },
-        {
-            "x": 7.68,
-            "y": 2.3550230463762047,
-            "heading": -4.11606275908414e-17,
-            "angularVelocity": -4.825465460619937e-16,
-            "velocityX": 1.073083844244948e-18,
-            "velocityY": 3.7829405750873115,
-            "timestamp": 0.5944435668660238
-        },
-        {
-            "x": 7.68,
-            "y": 2.729813827825723,
-            "heading": -7.635066611173202e-17,
-            "angularVelocity": -3.5518969822232415e-16,
-            "velocityX": 1.0738946259920485e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.6935174946770278
-        },
-        {
-            "x": 7.68,
-            "y": 3.1046046092752415,
-            "heading": -5.864574302033697e-17,
-            "angularVelocity": 1.787041604453991e-16,
-            "velocityX": 1.0691415340399175e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.7925914224880318
-        },
-        {
-            "x": 7.68,
-            "y": 3.4793953907247595,
-            "heading": -2.6391173603704442e-17,
-            "angularVelocity": 3.255606205313886e-16,
-            "velocityX": 1.0732289061667476e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.8916653502990358
-        },
-        {
-            "x": 7.68,
-            "y": 3.854186172174278,
-            "heading": -6.511485011951871e-17,
-            "angularVelocity": -3.9085637736788184e-16,
-            "velocityX": 1.0716697150075795e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.9907392781100398
-        },
-        {
-            "x": 7.68,
-            "y": 4.228976953623796,
-            "heading": -6.241329479526944e-17,
-            "angularVelocity": 2.7268075304360027e-17,
-            "velocityX": 1.0686111075844087e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 1.0898132059210437
-        },
-        {
-            "x": 7.68,
-            "y": 4.603767735073314,
-            "heading": -2.2737833863559818e-17,
-            "angularVelocity": 4.004631875225205e-16,
-            "velocityX": 1.0710197916722359e-18,
-            "velocityY": 3.7829405750873115,
-            "timestamp": 1.1888871337320477
-        },
-        {
-            "x": 7.68,
-            "y": 4.978558516522808,
-            "heading": -1.448944579275074e-17,
-            "angularVelocity": 8.325488100707829e-17,
-            "velocityX": 1.0714504382145178e-18,
-            "velocityY": 3.782940575087071,
-            "timestamp": 1.2879610615430517
-        },
-        {
-            "x": 7.68,
-            "y": 5.305935091856216,
-            "heading": -8.687037170545135e-18,
-            "angularVelocity": 5.856645386228716e-17,
-            "velocityX": 9.36753995288457e-19,
-            "velocityY": 3.304366573190877,
-            "timestamp": 1.3870349893540557
-        },
-        {
-            "x": 7.68,
-            "y": 5.55146754125807,
-            "heading": -4.341905072762387e-18,
-            "angularVelocity": 4.385747283685971e-17,
-            "velocityX": 7.025655481927048e-19,
-            "velocityY": 2.4782751105844794,
-            "timestamp": 1.4861089171650597
-        },
-        {
-            "x": 7.68,
-            "y": 5.715155846048237,
-            "heading": -1.4469357825353783e-18,
-            "angularVelocity": 2.9220293918225814e-17,
-            "velocityX": 4.683770471216647e-19,
-            "velocityY": 1.6521834594306495,
-            "timestamp": 1.5851828449760637
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 1.460460702963924e-17,
-            "velocityX": 2.341885280588165e-19,
-            "velocityY": 0.8260917454276263,
-            "timestamp": 1.6842567727870676
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 3.1812192470207854e-34,
-            "velocityY": 0,
-            "timestamp": 1.7833307005980716
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C3.1.traj
deleted file mode 100644
index 864eb2e4..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C3.1.traj	
+++ /dev/null
@@ -1,139 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 2.417290808823166e-40,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 0.8634453720055388,
-            "heading": 1.009810999063182e-18,
-            "angularVelocity": 1.0546271494620871e-17,
-            "velocityX": -3.226662245674165e-34,
-            "velocityY": 0.7983807350946677,
-            "timestamp": 0.09575052183150978
-        },
-        {
-            "x": 7.68,
-            "y": 1.0163361129826893,
-            "heading": 3.0323418642587253e-18,
-            "angularVelocity": 2.112292263585315e-17,
-            "velocityX": -6.037770263383471e-34,
-            "velocityY": 1.596761438503583,
-            "timestamp": 0.19150104366301957
-        },
-        {
-            "x": 7.68,
-            "y": 1.2456722168635992,
-            "heading": 6.073900773319643e-18,
-            "angularVelocity": 3.1765455168164525e-17,
-            "velocityX": -4.821612934727609e-34,
-            "velocityY": 2.3951420785410233,
-            "timestamp": 0.28725156549452935
-        },
-        {
-            "x": 7.68,
-            "y": 1.5514536654447455,
-            "heading": 1.0152788689831222e-17,
-            "angularVelocity": 4.2599119433487623e-17,
-            "velocityX": -1.2140491970354773e-33,
-            "velocityY": 3.19352252846437,
-            "timestamp": 0.38300208732603913
-        },
-        {
-            "x": 7.68,
-            "y": 1.9136721992668209,
-            "heading": 4.411917253468209e-18,
-            "angularVelocity": -5.995655508243724e-17,
-            "velocityX": 4.90795348832912e-33,
-            "velocityY": 3.7829405719528495,
-            "timestamp": 0.4787526091575489
-        },
-        {
-            "x": 7.68,
-            "y": 2.27589073308894,
-            "heading": 1.2796107303848674e-17,
-            "angularVelocity": 8.756286535058141e-17,
-            "velocityX": -1.5435756690671252e-33,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.5745031309890587
-        },
-        {
-            "x": 7.68,
-            "y": 2.63810926691106,
-            "heading": 6.994895399550348e-18,
-            "angularVelocity": -6.058673930264937e-17,
-            "velocityX": 4.776897057793067e-34,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.6702536528205685
-        },
-        {
-            "x": 7.68,
-            "y": 3.0003278007331793,
-            "heading": -9.534020306729642e-18,
-            "angularVelocity": -1.7262481070720064e-16,
-            "velocityX": 2.9275045647973183e-33,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.7660041746520783
-        },
-        {
-            "x": 7.68,
-            "y": 3.3625463345552546,
-            "heading": -1.0013819273727212e-17,
-            "angularVelocity": -5.010927959660929e-18,
-            "velocityX": 2.237784317663532e-33,
-            "velocityY": 3.7829405719528495,
-            "timestamp": 0.861754696483588
-        },
-        {
-            "x": 7.68,
-            "y": 3.6683277831364007,
-            "heading": -6.0064636534747415e-18,
-            "angularVelocity": 4.185204993640188e-17,
-            "velocityX": -2.2313398142102424e-33,
-            "velocityY": 3.19352252846437,
-            "timestamp": 0.9575052183150978
-        },
-        {
-            "x": 7.68,
-            "y": 3.897663887017311,
-            "heading": -3.002975312877452e-18,
-            "angularVelocity": 3.136785349810471e-17,
-            "velocityX": -2.8833027864886262e-33,
-            "velocityY": 2.3951420785410233,
-            "timestamp": 1.0532557401466076
-        },
-        {
-            "x": 7.68,
-            "y": 4.050554627994461,
-            "heading": -1.0009557447862449e-18,
-            "angularVelocity": 2.0908706606963636e-17,
-            "velocityX": -3.742421273800505e-33,
-            "velocityY": 1.596761438503583,
-            "timestamp": 1.1490062619781174
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 1.0453788927770355e-17,
-            "velocityX": -1.5032681111068507e-33,
-            "velocityY": 0.7983807350946677,
-            "timestamp": 1.2447567838096272
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 9.665729080515376e-43,
-            "velocityY": 2.80463986608038e-38,
-            "timestamp": 1.340507305641137
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C3.traj
deleted file mode 100644
index 864eb2e4..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C3.traj	
+++ /dev/null
@@ -1,139 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 2.417290808823166e-40,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 0.8634453720055388,
-            "heading": 1.009810999063182e-18,
-            "angularVelocity": 1.0546271494620871e-17,
-            "velocityX": -3.226662245674165e-34,
-            "velocityY": 0.7983807350946677,
-            "timestamp": 0.09575052183150978
-        },
-        {
-            "x": 7.68,
-            "y": 1.0163361129826893,
-            "heading": 3.0323418642587253e-18,
-            "angularVelocity": 2.112292263585315e-17,
-            "velocityX": -6.037770263383471e-34,
-            "velocityY": 1.596761438503583,
-            "timestamp": 0.19150104366301957
-        },
-        {
-            "x": 7.68,
-            "y": 1.2456722168635992,
-            "heading": 6.073900773319643e-18,
-            "angularVelocity": 3.1765455168164525e-17,
-            "velocityX": -4.821612934727609e-34,
-            "velocityY": 2.3951420785410233,
-            "timestamp": 0.28725156549452935
-        },
-        {
-            "x": 7.68,
-            "y": 1.5514536654447455,
-            "heading": 1.0152788689831222e-17,
-            "angularVelocity": 4.2599119433487623e-17,
-            "velocityX": -1.2140491970354773e-33,
-            "velocityY": 3.19352252846437,
-            "timestamp": 0.38300208732603913
-        },
-        {
-            "x": 7.68,
-            "y": 1.9136721992668209,
-            "heading": 4.411917253468209e-18,
-            "angularVelocity": -5.995655508243724e-17,
-            "velocityX": 4.90795348832912e-33,
-            "velocityY": 3.7829405719528495,
-            "timestamp": 0.4787526091575489
-        },
-        {
-            "x": 7.68,
-            "y": 2.27589073308894,
-            "heading": 1.2796107303848674e-17,
-            "angularVelocity": 8.756286535058141e-17,
-            "velocityX": -1.5435756690671252e-33,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.5745031309890587
-        },
-        {
-            "x": 7.68,
-            "y": 2.63810926691106,
-            "heading": 6.994895399550348e-18,
-            "angularVelocity": -6.058673930264937e-17,
-            "velocityX": 4.776897057793067e-34,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.6702536528205685
-        },
-        {
-            "x": 7.68,
-            "y": 3.0003278007331793,
-            "heading": -9.534020306729642e-18,
-            "angularVelocity": -1.7262481070720064e-16,
-            "velocityX": 2.9275045647973183e-33,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.7660041746520783
-        },
-        {
-            "x": 7.68,
-            "y": 3.3625463345552546,
-            "heading": -1.0013819273727212e-17,
-            "angularVelocity": -5.010927959660929e-18,
-            "velocityX": 2.237784317663532e-33,
-            "velocityY": 3.7829405719528495,
-            "timestamp": 0.861754696483588
-        },
-        {
-            "x": 7.68,
-            "y": 3.6683277831364007,
-            "heading": -6.0064636534747415e-18,
-            "angularVelocity": 4.185204993640188e-17,
-            "velocityX": -2.2313398142102424e-33,
-            "velocityY": 3.19352252846437,
-            "timestamp": 0.9575052183150978
-        },
-        {
-            "x": 7.68,
-            "y": 3.897663887017311,
-            "heading": -3.002975312877452e-18,
-            "angularVelocity": 3.136785349810471e-17,
-            "velocityX": -2.8833027864886262e-33,
-            "velocityY": 2.3951420785410233,
-            "timestamp": 1.0532557401466076
-        },
-        {
-            "x": 7.68,
-            "y": 4.050554627994461,
-            "heading": -1.0009557447862449e-18,
-            "angularVelocity": 2.0908706606963636e-17,
-            "velocityX": -3.742421273800505e-33,
-            "velocityY": 1.596761438503583,
-            "timestamp": 1.1490062619781174
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 1.0453788927770355e-17,
-            "velocityX": -1.5032681111068507e-33,
-            "velocityY": 0.7983807350946677,
-            "timestamp": 1.2447567838096272
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 9.665729080515376e-43,
-            "velocityY": 2.80463986608038e-38,
-            "timestamp": 1.340507305641137
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.1.traj
deleted file mode 100644
index 17170fc9..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.1.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 9.069206296242399e-40,
-            "angularVelocity": -9.927610039652183e-40,
-            "velocityX": -8.253947176298157e-40,
-            "velocityY": 1.8367099231598242e-40,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 0.8705000055758465,
-            "heading": 6.490773345690525e-22,
-            "angularVelocity": 6.486159683422905e-21,
-            "velocityX": -7.132918469580153e-33,
-            "velocityY": 0.8344065481228213,
-            "timestamp": 0.10007112931184182
-        },
-        {
-            "x": 7.68,
-            "y": 1.037500013629847,
-            "heading": 2.023593010760001e-21,
-            "angularVelocity": 1.3735386658326633e-20,
-            "velocityX": -1.5648094960655976e-32,
-            "velocityY": 1.668813065290736,
-            "timestamp": 0.20014225862368365
-        },
-        {
-            "x": 7.68,
-            "y": 1.2880000179666162,
-            "heading": 3.787875676222919e-21,
-            "angularVelocity": 1.7630286060953193e-20,
-            "velocityX": -2.3089760494009302e-32,
-            "velocityY": 2.503219520548835,
-            "timestamp": 0.3002133879355255
-        },
-        {
-            "x": 7.68,
-            "y": 1.6219999999999999,
-            "heading": 5.860880748165694e-21,
-            "angularVelocity": 2.0715315514205814e-20,
-            "velocityX": -3.0704903308303665e-32,
-            "velocityY": 3.3376257900775013,
-            "timestamp": 0.4002845172473673
-        },
-        {
-            "x": 7.68,
-            "y": 1.9559999820333838,
-            "heading": 3.934658943161884e-21,
-            "angularVelocity": -1.9248526176341983e-20,
-            "velocityX": -4.040562356218352e-32,
-            "velocityY": 3.3376257900775017,
-            "timestamp": 0.5003556465592092
-        },
-        {
-            "x": 7.68,
-            "y": 2.206499986370153,
-            "heading": 1.6176834885162177e-21,
-            "angularVelocity": -2.3153285409554772e-20,
-            "velocityX": -3.08903871620124e-32,
-            "velocityY": 2.503219520548835,
-            "timestamp": 0.600426775871051
-        },
-        {
-            "x": 7.68,
-            "y": 2.3734999944241535,
-            "heading": 4.992981261257204e-22,
-            "angularVelocity": -1.1175904096118944e-20,
-            "velocityX": -2.3331216062245415e-32,
-            "velocityY": 1.668813065290736,
-            "timestamp": 0.7004979051828928
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 1.7562757360140444e-40,
-            "angularVelocity": -4.9894322348928345e-21,
-            "velocityX": -1.3647931662662733e-32,
-            "velocityY": 0.8344065481228213,
-            "timestamp": 0.8005690344947346
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -7.274275970183695e-41,
-            "angularVelocity": -2.4819026120794767e-39,
-            "velocityX": -8.637198685981915e-42,
-            "velocityY": 5.142787784847508e-39,
-            "timestamp": 0.9006401638065764
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.traj
deleted file mode 100644
index 17170fc9..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 9.069206296242399e-40,
-            "angularVelocity": -9.927610039652183e-40,
-            "velocityX": -8.253947176298157e-40,
-            "velocityY": 1.8367099231598242e-40,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 0.8705000055758465,
-            "heading": 6.490773345690525e-22,
-            "angularVelocity": 6.486159683422905e-21,
-            "velocityX": -7.132918469580153e-33,
-            "velocityY": 0.8344065481228213,
-            "timestamp": 0.10007112931184182
-        },
-        {
-            "x": 7.68,
-            "y": 1.037500013629847,
-            "heading": 2.023593010760001e-21,
-            "angularVelocity": 1.3735386658326633e-20,
-            "velocityX": -1.5648094960655976e-32,
-            "velocityY": 1.668813065290736,
-            "timestamp": 0.20014225862368365
-        },
-        {
-            "x": 7.68,
-            "y": 1.2880000179666162,
-            "heading": 3.787875676222919e-21,
-            "angularVelocity": 1.7630286060953193e-20,
-            "velocityX": -2.3089760494009302e-32,
-            "velocityY": 2.503219520548835,
-            "timestamp": 0.3002133879355255
-        },
-        {
-            "x": 7.68,
-            "y": 1.6219999999999999,
-            "heading": 5.860880748165694e-21,
-            "angularVelocity": 2.0715315514205814e-20,
-            "velocityX": -3.0704903308303665e-32,
-            "velocityY": 3.3376257900775013,
-            "timestamp": 0.4002845172473673
-        },
-        {
-            "x": 7.68,
-            "y": 1.9559999820333838,
-            "heading": 3.934658943161884e-21,
-            "angularVelocity": -1.9248526176341983e-20,
-            "velocityX": -4.040562356218352e-32,
-            "velocityY": 3.3376257900775017,
-            "timestamp": 0.5003556465592092
-        },
-        {
-            "x": 7.68,
-            "y": 2.206499986370153,
-            "heading": 1.6176834885162177e-21,
-            "angularVelocity": -2.3153285409554772e-20,
-            "velocityX": -3.08903871620124e-32,
-            "velocityY": 2.503219520548835,
-            "timestamp": 0.600426775871051
-        },
-        {
-            "x": 7.68,
-            "y": 2.3734999944241535,
-            "heading": 4.992981261257204e-22,
-            "angularVelocity": -1.1175904096118944e-20,
-            "velocityX": -2.3331216062245415e-32,
-            "velocityY": 1.668813065290736,
-            "timestamp": 0.7004979051828928
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 1.7562757360140444e-40,
-            "angularVelocity": -4.9894322348928345e-21,
-            "velocityX": -1.3647931662662733e-32,
-            "velocityY": 0.8344065481228213,
-            "timestamp": 0.8005690344947346
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -7.274275970183695e-41,
-            "angularVelocity": -2.4819026120794767e-39,
-            "velocityX": -8.637198685981915e-42,
-            "velocityY": 5.142787784847508e-39,
-            "timestamp": 0.9006401638065764
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S1.1.traj
deleted file mode 100644
index 73f0ef42..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S1.1.traj	
+++ /dev/null
@@ -1,292 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 4.273608378959021e-31,
-            "velocityX": -7.420825794722196e-33,
-            "velocityY": 7.583788429881001e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 7.678491547493202,
-            "y": 0.8436471873828707,
-            "heading": -0.006289831251442517,
-            "angularVelocity": -0.0762785379812196,
-            "velocityX": -0.018293424295956956,
-            "velocityY": 0.686976241742982,
-            "timestamp": 0.08245872846948342
-        },
-        {
-            "x": 7.67547464227306,
-            "y": 0.9569415605229419,
-            "heading": -0.018869443032064528,
-            "angularVelocity": -0.1525564608394078,
-            "velocityX": -0.03658685109677269,
-            "velocityY": 1.3739524637710059,
-            "timestamp": 0.16491745693896684
-        },
-        {
-            "x": 7.670949285584863,
-            "y": 1.1268831170081794,
-            "heading": -0.03773871517098893,
-            "angularVelocity": -0.22883292635185057,
-            "velocityX": -0.05488026279561822,
-            "velocityY": 2.060928656547622,
-            "timestamp": 0.24737618540845024
-        },
-        {
-            "x": 7.664915480002808,
-            "y": 1.3534718520373763,
-            "heading": -0.06289740953278049,
-            "angularVelocity": -0.3051065039294483,
-            "velocityX": -0.07317364327643945,
-            "velocityY": 2.7479047910986583,
-            "timestamp": 0.3298349138779337
-        },
-        {
-            "x": 7.657373229393316,
-            "y": 1.6367077493690674,
-            "heading": -0.09434508673453766,
-            "angularVelocity": -0.38137475298804396,
-            "velocityX": -0.09146697686811679,
-            "velocityY": 3.4348807286849197,
-            "timestamp": 0.4122936423474171
-        },
-        {
-            "x": 7.649069464629009,
-            "y": 1.9485336685806538,
-            "heading": -0.09434509076634809,
-            "angularVelocity": -4.889488962341613e-8,
-            "velocityX": -0.10070207142933957,
-            "velocityY": 3.781599898511507,
-            "timestamp": 0.49475237081690054
-        },
-        {
-            "x": 7.6407656424773585,
-            "y": 2.260359586264075,
-            "heading": -0.09434509479812381,
-            "angularVelocity": -4.889446872332144e-8,
-            "velocityX": -0.1007027673816724,
-            "velocityY": 3.781599879979021,
-            "timestamp": 0.5772110992863839
-        },
-        {
-            "x": 7.63246175736497,
-            "y": 2.5721855022708704,
-            "heading": -0.09434509882989094,
-            "angularVelocity": -4.889436444733497e-8,
-            "velocityX": -0.10070353092409444,
-            "velocityY": 3.7815998596461093,
-            "timestamp": 0.6596698277558674
-        },
-        {
-            "x": 7.6241578045918486,
-            "y": 2.8840114164758646,
-            "heading": -0.09434510286165439,
-            "angularVelocity": -4.8894319896339695e-8,
-            "velocityX": -0.10070435146468282,
-            "velocityY": 3.781599837795173,
-            "timestamp": 0.7421285562253508
-        },
-        {
-            "x": 7.615853780314312,
-            "y": 3.195837328776685,
-            "heading": -0.09434510689341911,
-            "angularVelocity": -4.889433549543518e-8,
-            "velocityX": -0.10070521861867329,
-            "velocityY": 3.78159981470272,
-            "timestamp": 0.8245872846948342
-        },
-        {
-            "x": 7.607549681647345,
-            "y": 3.5076632390964844,
-            "heading": -0.09434511092518724,
-            "angularVelocity": -4.889437661104965e-8,
-            "velocityX": -0.10070612076003535,
-            "velocityY": 3.7815997906783263,
-            "timestamp": 0.9070460131643177
-        },
-        {
-            "x": 7.599245507241111,
-            "y": 3.819489147399297,
-            "heading": -0.0943451149569584,
-            "angularVelocity": -4.889441348359375e-8,
-            "velocityX": -0.10070703927126574,
-            "velocityY": 3.7815997662177643,
-            "timestamp": 0.9895047416338011
-        },
-        {
-            "x": 7.590941256893013,
-            "y": 4.131315053679708,
-            "heading": -0.09434511898873191,
-            "angularVelocity": -4.88944418703562e-8,
-            "velocityX": -0.10070796023943433,
-            "velocityY": 3.781599741691539,
-            "timestamp": 1.0719634701032845
-        },
-        {
-            "x": 7.582636930618974,
-            "y": 4.443140957938123,
-            "heading": -0.09434512302050634,
-            "angularVelocity": -4.889445311998471e-8,
-            "velocityX": -0.10070888101448454,
-            "velocityY": 3.781599717170225,
-            "timestamp": 1.1544221985727678
-        },
-        {
-            "x": 7.574332505518219,
-            "y": 4.754966859564639,
-            "heading": -0.09434512705227821,
-            "angularVelocity": -4.889442208864286e-8,
-            "velocityX": -0.10071007951363715,
-            "velocityY": 3.78159968525245,
-            "timestamp": 1.2368809270422512
-        },
-        {
-            "x": 7.562081052721255,
-            "y": 5.066662637971005,
-            "heading": -0.09434513109398723,
-            "angularVelocity": -4.901493268640619e-8,
-            "velocityX": -0.1485767853126217,
-            "velocityY": 3.7800216446669976,
-            "timestamp": 1.3193396555117345
-        },
-        {
-            "x": 7.493618412578439,
-            "y": 5.370993415792126,
-            "heading": -0.09434513545729191,
-            "angularVelocity": -5.29150129274595e-8,
-            "velocityX": -0.8302655329951328,
-            "velocityY": 3.6907042282825056,
-            "timestamp": 1.4017983839812178
-        },
-        {
-            "x": 7.37120325739307,
-            "y": 5.657906089273735,
-            "heading": -0.09434514092208332,
-            "angularVelocity": -6.627304961707395e-8,
-            "velocityX": -1.4845627316539582,
-            "velocityY": 3.479470018601967,
-            "timestamp": 1.4842571124507011
-        },
-        {
-            "x": 7.198879241943359,
-            "y": 5.9179229736328125,
-            "heading": -0.09434514894422652,
-            "angularVelocity": -9.728676819194244e-8,
-            "velocityX": -2.089821400938593,
-            "velocityY": 3.153297281988846,
-            "timestamp": 1.5667158409201845
-        },
-        {
-            "x": 7.016660952404083,
-            "y": 6.116497130944734,
-            "heading": -0.09434515627038953,
-            "angularVelocity": -1.028329950777357e-7,
-            "velocityX": -2.5576898068579945,
-            "velocityY": 2.7872673997010713,
-            "timestamp": 1.6379591516026175
-        },
-        {
-            "x": 6.805603401067166,
-            "y": 6.284097595272953,
-            "heading": -0.09434516139165254,
-            "angularVelocity": -7.188412437441302e-8,
-            "velocityX": -2.9624893806199624,
-            "velocityY": 2.352508084236811,
-            "timestamp": 1.7092024622850506
-        },
-        {
-            "x": 6.57091097390028,
-            "y": 6.416591642615984,
-            "heading": -0.09434516555011045,
-            "angularVelocity": -5.83698017255904e-8,
-            "velocityX": -3.2942380824078485,
-            "velocityY": 1.8597401787463752,
-            "timestamp": 1.7804457729674836
-        },
-        {
-            "x": 6.318370883291981,
-            "y": 6.510712371224652,
-            "heading": -0.09434516933867851,
-            "angularVelocity": -5.317787750894471e-8,
-            "velocityX": -3.544755124224867,
-            "velocityY": 1.3211167154795889,
-            "timestamp": 1.8516890836499167
-        },
-        {
-            "x": 6.054210380502568,
-            "y": 6.5641393107028225,
-            "heading": -0.09434515209433943,
-            "angularVelocity": 2.420485363909629e-7,
-            "velocityX": -3.7078639420184567,
-            "velocityY": 0.7499221887135695,
-            "timestamp": 1.9229323943323497
-        },
-        {
-            "x": 5.803536575930751,
-            "y": 6.585943895129495,
-            "heading": -0.03682720962895,
-            "angularVelocity": 0.8073451656643992,
-            "velocityX": -3.5185591765828246,
-            "velocityY": 0.30605798941413415,
-            "timestamp": 1.9941757050147828
-        },
-        {
-            "x": 5.594045383796189,
-            "y": 6.601646508747958,
-            "heading": 0.04817170409455108,
-            "angularVelocity": 1.1930792225867084,
-            "velocityX": -2.9405033276509123,
-            "velocityY": 0.2204082526211729,
-            "timestamp": 2.065419015697216
-        },
-        {
-            "x": 5.426658878791764,
-            "y": 6.613398993876379,
-            "heading": 0.12734083591800166,
-            "angularVelocity": 1.111250039689289,
-            "velocityX": -2.3495048644012306,
-            "velocityY": 0.16496264724146908,
-            "timestamp": 2.136662326379649
-        },
-        {
-            "x": 5.301258489415666,
-            "y": 6.621853943751229,
-            "heading": 0.1915936210653456,
-            "angularVelocity": 0.9018781487254319,
-            "velocityX": -1.7601707188351954,
-            "velocityY": 0.11867710517466466,
-            "timestamp": 2.207905637062082
-        },
-        {
-            "x": 5.217732575552286,
-            "y": 6.627325619944874,
-            "heading": 0.2366430637253094,
-            "angularVelocity": 0.6323322460514409,
-            "velocityX": -1.1724035992052257,
-            "velocityY": 0.07680266597989953,
-            "timestamp": 2.279148947744515
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 0.32784742947732926,
-            "velocityX": -0.5857753542407405,
-            "velocityY": 0.037538682993647744,
-            "timestamp": 2.350392258426948
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": -8.68876182413906e-32,
-            "velocityX": 0,
-            "velocityY": -2.1409750340846636e-32,
-            "timestamp": 2.421635569109381
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S1.traj
deleted file mode 100644
index 73f0ef42..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S1.traj	
+++ /dev/null
@@ -1,292 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 4.273608378959021e-31,
-            "velocityX": -7.420825794722196e-33,
-            "velocityY": 7.583788429881001e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 7.678491547493202,
-            "y": 0.8436471873828707,
-            "heading": -0.006289831251442517,
-            "angularVelocity": -0.0762785379812196,
-            "velocityX": -0.018293424295956956,
-            "velocityY": 0.686976241742982,
-            "timestamp": 0.08245872846948342
-        },
-        {
-            "x": 7.67547464227306,
-            "y": 0.9569415605229419,
-            "heading": -0.018869443032064528,
-            "angularVelocity": -0.1525564608394078,
-            "velocityX": -0.03658685109677269,
-            "velocityY": 1.3739524637710059,
-            "timestamp": 0.16491745693896684
-        },
-        {
-            "x": 7.670949285584863,
-            "y": 1.1268831170081794,
-            "heading": -0.03773871517098893,
-            "angularVelocity": -0.22883292635185057,
-            "velocityX": -0.05488026279561822,
-            "velocityY": 2.060928656547622,
-            "timestamp": 0.24737618540845024
-        },
-        {
-            "x": 7.664915480002808,
-            "y": 1.3534718520373763,
-            "heading": -0.06289740953278049,
-            "angularVelocity": -0.3051065039294483,
-            "velocityX": -0.07317364327643945,
-            "velocityY": 2.7479047910986583,
-            "timestamp": 0.3298349138779337
-        },
-        {
-            "x": 7.657373229393316,
-            "y": 1.6367077493690674,
-            "heading": -0.09434508673453766,
-            "angularVelocity": -0.38137475298804396,
-            "velocityX": -0.09146697686811679,
-            "velocityY": 3.4348807286849197,
-            "timestamp": 0.4122936423474171
-        },
-        {
-            "x": 7.649069464629009,
-            "y": 1.9485336685806538,
-            "heading": -0.09434509076634809,
-            "angularVelocity": -4.889488962341613e-8,
-            "velocityX": -0.10070207142933957,
-            "velocityY": 3.781599898511507,
-            "timestamp": 0.49475237081690054
-        },
-        {
-            "x": 7.6407656424773585,
-            "y": 2.260359586264075,
-            "heading": -0.09434509479812381,
-            "angularVelocity": -4.889446872332144e-8,
-            "velocityX": -0.1007027673816724,
-            "velocityY": 3.781599879979021,
-            "timestamp": 0.5772110992863839
-        },
-        {
-            "x": 7.63246175736497,
-            "y": 2.5721855022708704,
-            "heading": -0.09434509882989094,
-            "angularVelocity": -4.889436444733497e-8,
-            "velocityX": -0.10070353092409444,
-            "velocityY": 3.7815998596461093,
-            "timestamp": 0.6596698277558674
-        },
-        {
-            "x": 7.6241578045918486,
-            "y": 2.8840114164758646,
-            "heading": -0.09434510286165439,
-            "angularVelocity": -4.8894319896339695e-8,
-            "velocityX": -0.10070435146468282,
-            "velocityY": 3.781599837795173,
-            "timestamp": 0.7421285562253508
-        },
-        {
-            "x": 7.615853780314312,
-            "y": 3.195837328776685,
-            "heading": -0.09434510689341911,
-            "angularVelocity": -4.889433549543518e-8,
-            "velocityX": -0.10070521861867329,
-            "velocityY": 3.78159981470272,
-            "timestamp": 0.8245872846948342
-        },
-        {
-            "x": 7.607549681647345,
-            "y": 3.5076632390964844,
-            "heading": -0.09434511092518724,
-            "angularVelocity": -4.889437661104965e-8,
-            "velocityX": -0.10070612076003535,
-            "velocityY": 3.7815997906783263,
-            "timestamp": 0.9070460131643177
-        },
-        {
-            "x": 7.599245507241111,
-            "y": 3.819489147399297,
-            "heading": -0.0943451149569584,
-            "angularVelocity": -4.889441348359375e-8,
-            "velocityX": -0.10070703927126574,
-            "velocityY": 3.7815997662177643,
-            "timestamp": 0.9895047416338011
-        },
-        {
-            "x": 7.590941256893013,
-            "y": 4.131315053679708,
-            "heading": -0.09434511898873191,
-            "angularVelocity": -4.88944418703562e-8,
-            "velocityX": -0.10070796023943433,
-            "velocityY": 3.781599741691539,
-            "timestamp": 1.0719634701032845
-        },
-        {
-            "x": 7.582636930618974,
-            "y": 4.443140957938123,
-            "heading": -0.09434512302050634,
-            "angularVelocity": -4.889445311998471e-8,
-            "velocityX": -0.10070888101448454,
-            "velocityY": 3.781599717170225,
-            "timestamp": 1.1544221985727678
-        },
-        {
-            "x": 7.574332505518219,
-            "y": 4.754966859564639,
-            "heading": -0.09434512705227821,
-            "angularVelocity": -4.889442208864286e-8,
-            "velocityX": -0.10071007951363715,
-            "velocityY": 3.78159968525245,
-            "timestamp": 1.2368809270422512
-        },
-        {
-            "x": 7.562081052721255,
-            "y": 5.066662637971005,
-            "heading": -0.09434513109398723,
-            "angularVelocity": -4.901493268640619e-8,
-            "velocityX": -0.1485767853126217,
-            "velocityY": 3.7800216446669976,
-            "timestamp": 1.3193396555117345
-        },
-        {
-            "x": 7.493618412578439,
-            "y": 5.370993415792126,
-            "heading": -0.09434513545729191,
-            "angularVelocity": -5.29150129274595e-8,
-            "velocityX": -0.8302655329951328,
-            "velocityY": 3.6907042282825056,
-            "timestamp": 1.4017983839812178
-        },
-        {
-            "x": 7.37120325739307,
-            "y": 5.657906089273735,
-            "heading": -0.09434514092208332,
-            "angularVelocity": -6.627304961707395e-8,
-            "velocityX": -1.4845627316539582,
-            "velocityY": 3.479470018601967,
-            "timestamp": 1.4842571124507011
-        },
-        {
-            "x": 7.198879241943359,
-            "y": 5.9179229736328125,
-            "heading": -0.09434514894422652,
-            "angularVelocity": -9.728676819194244e-8,
-            "velocityX": -2.089821400938593,
-            "velocityY": 3.153297281988846,
-            "timestamp": 1.5667158409201845
-        },
-        {
-            "x": 7.016660952404083,
-            "y": 6.116497130944734,
-            "heading": -0.09434515627038953,
-            "angularVelocity": -1.028329950777357e-7,
-            "velocityX": -2.5576898068579945,
-            "velocityY": 2.7872673997010713,
-            "timestamp": 1.6379591516026175
-        },
-        {
-            "x": 6.805603401067166,
-            "y": 6.284097595272953,
-            "heading": -0.09434516139165254,
-            "angularVelocity": -7.188412437441302e-8,
-            "velocityX": -2.9624893806199624,
-            "velocityY": 2.352508084236811,
-            "timestamp": 1.7092024622850506
-        },
-        {
-            "x": 6.57091097390028,
-            "y": 6.416591642615984,
-            "heading": -0.09434516555011045,
-            "angularVelocity": -5.83698017255904e-8,
-            "velocityX": -3.2942380824078485,
-            "velocityY": 1.8597401787463752,
-            "timestamp": 1.7804457729674836
-        },
-        {
-            "x": 6.318370883291981,
-            "y": 6.510712371224652,
-            "heading": -0.09434516933867851,
-            "angularVelocity": -5.317787750894471e-8,
-            "velocityX": -3.544755124224867,
-            "velocityY": 1.3211167154795889,
-            "timestamp": 1.8516890836499167
-        },
-        {
-            "x": 6.054210380502568,
-            "y": 6.5641393107028225,
-            "heading": -0.09434515209433943,
-            "angularVelocity": 2.420485363909629e-7,
-            "velocityX": -3.7078639420184567,
-            "velocityY": 0.7499221887135695,
-            "timestamp": 1.9229323943323497
-        },
-        {
-            "x": 5.803536575930751,
-            "y": 6.585943895129495,
-            "heading": -0.03682720962895,
-            "angularVelocity": 0.8073451656643992,
-            "velocityX": -3.5185591765828246,
-            "velocityY": 0.30605798941413415,
-            "timestamp": 1.9941757050147828
-        },
-        {
-            "x": 5.594045383796189,
-            "y": 6.601646508747958,
-            "heading": 0.04817170409455108,
-            "angularVelocity": 1.1930792225867084,
-            "velocityX": -2.9405033276509123,
-            "velocityY": 0.2204082526211729,
-            "timestamp": 2.065419015697216
-        },
-        {
-            "x": 5.426658878791764,
-            "y": 6.613398993876379,
-            "heading": 0.12734083591800166,
-            "angularVelocity": 1.111250039689289,
-            "velocityX": -2.3495048644012306,
-            "velocityY": 0.16496264724146908,
-            "timestamp": 2.136662326379649
-        },
-        {
-            "x": 5.301258489415666,
-            "y": 6.621853943751229,
-            "heading": 0.1915936210653456,
-            "angularVelocity": 0.9018781487254319,
-            "velocityX": -1.7601707188351954,
-            "velocityY": 0.11867710517466466,
-            "timestamp": 2.207905637062082
-        },
-        {
-            "x": 5.217732575552286,
-            "y": 6.627325619944874,
-            "heading": 0.2366430637253094,
-            "angularVelocity": 0.6323322460514409,
-            "velocityX": -1.1724035992052257,
-            "velocityY": 0.07680266597989953,
-            "timestamp": 2.279148947744515
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 0.32784742947732926,
-            "velocityX": -0.5857753542407405,
-            "velocityY": 0.037538682993647744,
-            "timestamp": 2.350392258426948
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": -8.68876182413906e-32,
-            "velocityX": 0,
-            "velocityY": -2.1409750340846636e-32,
-            "timestamp": 2.421635569109381
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S2.1.traj
deleted file mode 100644
index dfbc040a..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S2.1.traj	
+++ /dev/null
@@ -1,247 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -1.3156383032943886e-37,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.664225467591612,
-            "y": 0.8361290574305452,
-            "heading": -0.011405957532827086,
-            "angularVelocity": -0.14485338302751044,
-            "velocityX": -0.20033341159264173,
-            "velocityY": 0.6239292188565086,
-            "timestamp": 0.07874139557141634
-        },
-        {
-            "x": 7.632676408078482,
-            "y": 0.9343872155736361,
-            "heading": -0.03421427612768491,
-            "angularVelocity": -0.2896610915940815,
-            "velocityX": -0.4006667558300489,
-            "velocityY": 1.2478589873857826,
-            "timestamp": 0.15748279114283267
-        },
-        {
-            "x": 7.585352811707443,
-            "y": 1.0817745497576996,
-            "heading": -0.06841862032572539,
-            "angularVelocity": -0.4343883410984004,
-            "velocityX": -0.6010002239307173,
-            "velocityY": 1.8717897125710332,
-            "timestamp": 0.236224186714249
-        },
-        {
-            "x": 7.522254637419436,
-            "y": 1.278291160108196,
-            "heading": -0.11400992700252059,
-            "angularVelocity": -0.5790004907322871,
-            "velocityX": -0.8013342134732577,
-            "velocityY": 2.4957217093296378,
-            "timestamp": 0.31496558228566535
-        },
-        {
-            "x": 7.443381794868827,
-            "y": 1.5239371518673812,
-            "heading": -0.1709767215367488,
-            "angularVelocity": -0.723466914966739,
-            "velocityX": -1.0016693503872847,
-            "velocityY": 3.1196550426438785,
-            "timestamp": 0.3937069778570817
-        },
-        {
-            "x": 7.3523133020023,
-            "y": 1.807548605142893,
-            "heading": -0.17097672945138376,
-            "angularVelocity": -1.00514283666892e-7,
-            "velocityX": -1.1565516740674315,
-            "velocityY": 3.6018088226323544,
-            "timestamp": 0.47244837342849805
-        },
-        {
-            "x": 7.26124476293705,
-            "y": 2.0911600435840003,
-            "heading": -0.17097673736581726,
-            "angularVelocity": -1.0051172494783456e-7,
-            "velocityX": -1.1565522607819834,
-            "velocityY": 3.6018086342383846,
-            "timestamp": 0.5511897689999143
-        },
-        {
-            "x": 7.170176175790629,
-            "y": 2.3747714665860937,
-            "heading": -0.17097674528023646,
-            "angularVelocity": -1.0051154369078753e-7,
-            "velocityX": -1.1565528714032516,
-            "velocityY": 3.601808438165991,
-            "timestamp": 0.6299311645713307
-        },
-        {
-            "x": 7.079107531002594,
-            "y": 2.6583828710792723,
-            "heading": -0.17097675319464586,
-            "angularVelocity": -1.0051141913755183e-7,
-            "velocityX": -1.1565536034402273,
-            "velocityY": 3.601808203106465,
-            "timestamp": 0.708672560142747
-        },
-        {
-            "x": 6.988038816793172,
-            "y": 2.9419942532809915,
-            "heading": -0.17097676110905413,
-            "angularVelocity": -1.0051140450859988e-7,
-            "velocityX": -1.1565544850779974,
-            "velocityY": 3.601807920009381,
-            "timestamp": 0.7874139557141634
-        },
-        {
-            "x": 6.896968817946089,
-            "y": 3.2256052229773635,
-            "heading": -0.17097676902348397,
-            "angularVelocity": -1.0051167869642574e-7,
-            "velocityX": -1.156570799719756,
-            "velocityY": 3.6018026812738513,
-            "timestamp": 0.8661553512855797
-        },
-        {
-            "x": 6.765153291475094,
-            "y": 3.4927261567236307,
-            "heading": -0.1709767773203105,
-            "angularVelocity": -1.053680400636008e-7,
-            "velocityX": -1.6740308641271406,
-            "velocityY": 3.392382517579277,
-            "timestamp": 0.9448967468569961
-        },
-        {
-            "x": 6.589137393143544,
-            "y": 3.7330328933669676,
-            "heading": -0.17097678717611917,
-            "angularVelocity": -1.2516680215717606e-7,
-            "velocityX": -2.235366760446956,
-            "velocityY": 3.0518475688608455,
-            "timestamp": 1.0236381424284124
-        },
-        {
-            "x": 6.374222917834714,
-            "y": 3.939286833341542,
-            "heading": -0.17097680095561982,
-            "angularVelocity": -1.7499690668870768e-7,
-            "velocityX": -2.7293709204570815,
-            "velocityY": 2.6193838511219654,
-            "timestamp": 1.1023795379998287
-        },
-        {
-            "x": 6.126883506774902,
-            "y": 4.1052751541137695,
-            "heading": -0.17097682837855596,
-            "angularVelocity": -3.482658130630958e-7,
-            "velocityX": -3.1411611296053437,
-            "velocityY": 2.108018527836233,
-            "timestamp": 1.181120933571245
-        },
-        {
-            "x": 5.792856415059812,
-            "y": 4.239084767822678,
-            "heading": -0.17097684795948573,
-            "angularVelocity": -2.0585572281638246e-7,
-            "velocityX": -3.5116508401962974,
-            "velocityY": 1.4067500932170736,
-            "timestamp": 1.2762406100184678
-        },
-        {
-            "x": 5.438271606876556,
-            "y": 4.300308177252639,
-            "heading": -0.1709790019096565,
-            "angularVelocity": -0.000022644633069121558,
-            "velocityX": -3.7277755920458318,
-            "velocityY": 0.6436461068486719,
-            "timestamp": 1.3713602864656906
-        },
-        {
-            "x": 5.110881399181515,
-            "y": 4.377812934831655,
-            "heading": -0.2483895057711766,
-            "angularVelocity": -0.8138221948691259,
-            "velocityX": -3.4418768011337173,
-            "velocityY": 0.8148130909803968,
-            "timestamp": 1.4664799629129135
-        },
-        {
-            "x": 4.831528638348494,
-            "y": 4.508515906317057,
-            "heading": -0.41196599842011083,
-            "angularVelocity": -1.7196914325049715,
-            "velocityX": -2.936855667165985,
-            "velocityY": 1.374089740075198,
-            "timestamp": 1.5615996393601363
-        },
-        {
-            "x": 4.597313308728367,
-            "y": 4.699607232649008,
-            "heading": -0.5851564423497488,
-            "angularVelocity": -1.8207635938052467,
-            "velocityX": -2.4623226063019743,
-            "velocityY": 2.0089568580269215,
-            "timestamp": 1.6567193158073592
-        },
-        {
-            "x": 4.422373656654268,
-            "y": 4.922240435570765,
-            "heading": -0.6321779806524386,
-            "angularVelocity": -0.49434081421397164,
-            "velocityX": -1.8391531448402672,
-            "velocityY": 2.340558875274187,
-            "timestamp": 1.751838992254582
-        },
-        {
-            "x": 4.3095074978210945,
-            "y": 5.147658123857161,
-            "heading": -0.526303235570326,
-            "angularVelocity": 1.11306881012003,
-            "velocityX": -1.1865700457443962,
-            "velocityY": 2.3698323701875594,
-            "timestamp": 1.846958668701805
-        },
-        {
-            "x": 4.263497791071102,
-            "y": 5.351217934034869,
-            "heading": -0.3174810959543699,
-            "angularVelocity": 2.1953621733755666,
-            "velocityX": -0.48370335632419137,
-            "velocityY": 2.1400389254967007,
-            "timestamp": 1.9420783451490278
-        },
-        {
-            "x": 4.258299402660224,
-            "y": 5.491389588363689,
-            "heading": -0.11860771311510487,
-            "angularVelocity": 2.090770177814994,
-            "velocityX": -0.05465103125915711,
-            "velocityY": 1.4736346838458172,
-            "timestamp": 2.0371980215962506
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 6.542074475732666e-36,
-            "angularVelocity": 1.2469314188732994,
-            "velocityX": 0.04941771792488758,
-            "velocityY": 0.7213061923563153,
-            "timestamp": 2.1323176980434737
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -6.642487421560091e-37,
-            "velocityY": 5.186129384888016e-33,
-            "timestamp": 2.227437374490697
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S2.traj
deleted file mode 100644
index dfbc040a..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S2.traj	
+++ /dev/null
@@ -1,247 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -1.3156383032943886e-37,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.664225467591612,
-            "y": 0.8361290574305452,
-            "heading": -0.011405957532827086,
-            "angularVelocity": -0.14485338302751044,
-            "velocityX": -0.20033341159264173,
-            "velocityY": 0.6239292188565086,
-            "timestamp": 0.07874139557141634
-        },
-        {
-            "x": 7.632676408078482,
-            "y": 0.9343872155736361,
-            "heading": -0.03421427612768491,
-            "angularVelocity": -0.2896610915940815,
-            "velocityX": -0.4006667558300489,
-            "velocityY": 1.2478589873857826,
-            "timestamp": 0.15748279114283267
-        },
-        {
-            "x": 7.585352811707443,
-            "y": 1.0817745497576996,
-            "heading": -0.06841862032572539,
-            "angularVelocity": -0.4343883410984004,
-            "velocityX": -0.6010002239307173,
-            "velocityY": 1.8717897125710332,
-            "timestamp": 0.236224186714249
-        },
-        {
-            "x": 7.522254637419436,
-            "y": 1.278291160108196,
-            "heading": -0.11400992700252059,
-            "angularVelocity": -0.5790004907322871,
-            "velocityX": -0.8013342134732577,
-            "velocityY": 2.4957217093296378,
-            "timestamp": 0.31496558228566535
-        },
-        {
-            "x": 7.443381794868827,
-            "y": 1.5239371518673812,
-            "heading": -0.1709767215367488,
-            "angularVelocity": -0.723466914966739,
-            "velocityX": -1.0016693503872847,
-            "velocityY": 3.1196550426438785,
-            "timestamp": 0.3937069778570817
-        },
-        {
-            "x": 7.3523133020023,
-            "y": 1.807548605142893,
-            "heading": -0.17097672945138376,
-            "angularVelocity": -1.00514283666892e-7,
-            "velocityX": -1.1565516740674315,
-            "velocityY": 3.6018088226323544,
-            "timestamp": 0.47244837342849805
-        },
-        {
-            "x": 7.26124476293705,
-            "y": 2.0911600435840003,
-            "heading": -0.17097673736581726,
-            "angularVelocity": -1.0051172494783456e-7,
-            "velocityX": -1.1565522607819834,
-            "velocityY": 3.6018086342383846,
-            "timestamp": 0.5511897689999143
-        },
-        {
-            "x": 7.170176175790629,
-            "y": 2.3747714665860937,
-            "heading": -0.17097674528023646,
-            "angularVelocity": -1.0051154369078753e-7,
-            "velocityX": -1.1565528714032516,
-            "velocityY": 3.601808438165991,
-            "timestamp": 0.6299311645713307
-        },
-        {
-            "x": 7.079107531002594,
-            "y": 2.6583828710792723,
-            "heading": -0.17097675319464586,
-            "angularVelocity": -1.0051141913755183e-7,
-            "velocityX": -1.1565536034402273,
-            "velocityY": 3.601808203106465,
-            "timestamp": 0.708672560142747
-        },
-        {
-            "x": 6.988038816793172,
-            "y": 2.9419942532809915,
-            "heading": -0.17097676110905413,
-            "angularVelocity": -1.0051140450859988e-7,
-            "velocityX": -1.1565544850779974,
-            "velocityY": 3.601807920009381,
-            "timestamp": 0.7874139557141634
-        },
-        {
-            "x": 6.896968817946089,
-            "y": 3.2256052229773635,
-            "heading": -0.17097676902348397,
-            "angularVelocity": -1.0051167869642574e-7,
-            "velocityX": -1.156570799719756,
-            "velocityY": 3.6018026812738513,
-            "timestamp": 0.8661553512855797
-        },
-        {
-            "x": 6.765153291475094,
-            "y": 3.4927261567236307,
-            "heading": -0.1709767773203105,
-            "angularVelocity": -1.053680400636008e-7,
-            "velocityX": -1.6740308641271406,
-            "velocityY": 3.392382517579277,
-            "timestamp": 0.9448967468569961
-        },
-        {
-            "x": 6.589137393143544,
-            "y": 3.7330328933669676,
-            "heading": -0.17097678717611917,
-            "angularVelocity": -1.2516680215717606e-7,
-            "velocityX": -2.235366760446956,
-            "velocityY": 3.0518475688608455,
-            "timestamp": 1.0236381424284124
-        },
-        {
-            "x": 6.374222917834714,
-            "y": 3.939286833341542,
-            "heading": -0.17097680095561982,
-            "angularVelocity": -1.7499690668870768e-7,
-            "velocityX": -2.7293709204570815,
-            "velocityY": 2.6193838511219654,
-            "timestamp": 1.1023795379998287
-        },
-        {
-            "x": 6.126883506774902,
-            "y": 4.1052751541137695,
-            "heading": -0.17097682837855596,
-            "angularVelocity": -3.482658130630958e-7,
-            "velocityX": -3.1411611296053437,
-            "velocityY": 2.108018527836233,
-            "timestamp": 1.181120933571245
-        },
-        {
-            "x": 5.792856415059812,
-            "y": 4.239084767822678,
-            "heading": -0.17097684795948573,
-            "angularVelocity": -2.0585572281638246e-7,
-            "velocityX": -3.5116508401962974,
-            "velocityY": 1.4067500932170736,
-            "timestamp": 1.2762406100184678
-        },
-        {
-            "x": 5.438271606876556,
-            "y": 4.300308177252639,
-            "heading": -0.1709790019096565,
-            "angularVelocity": -0.000022644633069121558,
-            "velocityX": -3.7277755920458318,
-            "velocityY": 0.6436461068486719,
-            "timestamp": 1.3713602864656906
-        },
-        {
-            "x": 5.110881399181515,
-            "y": 4.377812934831655,
-            "heading": -0.2483895057711766,
-            "angularVelocity": -0.8138221948691259,
-            "velocityX": -3.4418768011337173,
-            "velocityY": 0.8148130909803968,
-            "timestamp": 1.4664799629129135
-        },
-        {
-            "x": 4.831528638348494,
-            "y": 4.508515906317057,
-            "heading": -0.41196599842011083,
-            "angularVelocity": -1.7196914325049715,
-            "velocityX": -2.936855667165985,
-            "velocityY": 1.374089740075198,
-            "timestamp": 1.5615996393601363
-        },
-        {
-            "x": 4.597313308728367,
-            "y": 4.699607232649008,
-            "heading": -0.5851564423497488,
-            "angularVelocity": -1.8207635938052467,
-            "velocityX": -2.4623226063019743,
-            "velocityY": 2.0089568580269215,
-            "timestamp": 1.6567193158073592
-        },
-        {
-            "x": 4.422373656654268,
-            "y": 4.922240435570765,
-            "heading": -0.6321779806524386,
-            "angularVelocity": -0.49434081421397164,
-            "velocityX": -1.8391531448402672,
-            "velocityY": 2.340558875274187,
-            "timestamp": 1.751838992254582
-        },
-        {
-            "x": 4.3095074978210945,
-            "y": 5.147658123857161,
-            "heading": -0.526303235570326,
-            "angularVelocity": 1.11306881012003,
-            "velocityX": -1.1865700457443962,
-            "velocityY": 2.3698323701875594,
-            "timestamp": 1.846958668701805
-        },
-        {
-            "x": 4.263497791071102,
-            "y": 5.351217934034869,
-            "heading": -0.3174810959543699,
-            "angularVelocity": 2.1953621733755666,
-            "velocityX": -0.48370335632419137,
-            "velocityY": 2.1400389254967007,
-            "timestamp": 1.9420783451490278
-        },
-        {
-            "x": 4.258299402660224,
-            "y": 5.491389588363689,
-            "heading": -0.11860771311510487,
-            "angularVelocity": 2.090770177814994,
-            "velocityX": -0.05465103125915711,
-            "velocityY": 1.4736346838458172,
-            "timestamp": 2.0371980215962506
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 6.542074475732666e-36,
-            "angularVelocity": 1.2469314188732994,
-            "velocityX": 0.04941771792488758,
-            "velocityY": 0.7213061923563153,
-            "timestamp": 2.1323176980434737
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -6.642487421560091e-37,
-            "velocityY": 5.186129384888016e-33,
-            "timestamp": 2.227437374490697
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.1.traj
deleted file mode 100644
index d9cc7c97..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.1.traj	
+++ /dev/null
@@ -1,247 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -3.982415645335998e-31,
-            "velocityX": 3.346280928312519e-33,
-            "velocityY": 3.541078180282768e-38,
-            "timestamp": 0
-        },
-        {
-            "x": 7.629395539536042,
-            "y": 0.7881897280541793,
-            "heading": -0.02207380596023684,
-            "angularVelocity": -0.2822732032732244,
-            "velocityX": -0.6471146471374848,
-            "velocityY": 0.015213885156899113,
-            "timestamp": 0.0782001468948451
-        },
-        {
-            "x": 7.528184091504648,
-            "y": 0.7905692108612611,
-            "heading": -0.06614402373093474,
-            "angularVelocity": -0.5635567133951742,
-            "velocityX": -1.2942616101155584,
-            "velocityY": 0.030428111729043,
-            "timestamp": 0.1564002937896902
-        },
-        {
-            "x": 7.376360973312952,
-            "y": 0.7941378827255743,
-            "heading": -0.13206235338942335,
-            "angularVelocity": -0.8429438086302324,
-            "velocityX": -1.9414684526853105,
-            "velocityY": 0.04563510435477082,
-            "timestamp": 0.23460044068453528
-        },
-        {
-            "x": 7.173916466766183,
-            "y": 0.7988917309385671,
-            "heading": -0.21951772550448787,
-            "angularVelocity": -1.1183530413716716,
-            "velocityX": -2.588799568612108,
-            "velocityY": 0.060790783668257076,
-            "timestamp": 0.3128005875793804
-        },
-        {
-            "x": 6.920824289019536,
-            "y": 0.8048108023879232,
-            "heading": -0.3276760191057486,
-            "angularVelocity": -1.383095785572821,
-            "velocityX": -3.236466781667369,
-            "velocityY": 0.07569130857196414,
-            "timestamp": 0.3910007344742255
-        },
-        {
-            "x": 6.625087122175782,
-            "y": 0.8120803100358527,
-            "heading": -0.3276760358371292,
-            "angularVelocity": -2.1395587179409026e-7,
-            "velocityX": -3.781798098684327,
-            "velocityY": 0.09296028123113304,
-            "timestamp": 0.4692008813690706
-        },
-        {
-            "x": 6.329343587367458,
-            "y": 0.8190859886904246,
-            "heading": -0.3276760513572638,
-            "angularVelocity": -1.9846682128968708e-7,
-            "velocityX": -3.78187953030339,
-            "velocityY": 0.08958651527359804,
-            "timestamp": 0.5474010282639157
-        },
-        {
-            "x": 6.033600055760057,
-            "y": 0.826091802470254,
-            "heading": -0.3276760668773978,
-            "angularVelocity": -1.9846681457625392e-7,
-            "velocityX": -3.7818794893709464,
-            "velocityY": 0.0895882432148227,
-            "timestamp": 0.6256011751587608
-        },
-        {
-            "x": 5.7383102527596765,
-            "y": 0.8439033174946344,
-            "heading": -0.3276760825327013,
-            "angularVelocity": -2.0019532088764418e-7,
-            "velocityX": -3.7760773441696487,
-            "velocityY": 0.22776830647924015,
-            "timestamp": 0.7038013220536059
-        },
-        {
-            "x": 5.450465299571764,
-            "y": 0.9121572529445121,
-            "heading": -0.32767609968005607,
-            "angularVelocity": -2.1927522433491614e-7,
-            "velocityX": -3.680874839978332,
-            "velocityY": 0.8728108342551084,
-            "timestamp": 0.782001468948451
-        },
-        {
-            "x": 5.178616904201158,
-            "y": 1.028826716826592,
-            "heading": -0.32767612118071976,
-            "angularVelocity": -2.749440322201668e-7,
-            "velocityX": -3.4763156613508643,
-            "velocityY": 1.4919340757663395,
-            "timestamp": 0.8602016158432961
-        },
-        {
-            "x": 4.930841529412244,
-            "y": 1.190445624246428,
-            "heading": -0.3276761534331376,
-            "angularVelocity": -4.1243423626599064e-7,
-            "velocityX": -3.1684771017365216,
-            "velocityY": 2.0667340642903906,
-            "timestamp": 0.9384017627381412
-        },
-        {
-            "x": 4.7145004488669215,
-            "y": 1.3922123936122235,
-            "heading": -0.32767621878386327,
-            "angularVelocity": -8.356854591223944e-7,
-            "velocityX": -2.7665047846524513,
-            "velocityY": 2.5801328690255976,
-            "timestamp": 1.0166019096329864
-        },
-        {
-            "x": 4.536026477813721,
-            "y": 1.628126621246338,
-            "heading": -0.327696539002069,
-            "angularVelocity": -0.00025984882823803823,
-            "velocityX": -2.282271557538034,
-            "velocityY": 3.016800313073996,
-            "timestamp": 1.0948020565278314
-        },
-        {
-            "x": 4.4347589269746495,
-            "y": 1.8009845745755027,
-            "heading": -0.3311064514524199,
-            "angularVelocity": -0.06394544615832723,
-            "velocityX": -1.899051313343304,
-            "velocityY": 3.241572651564751,
-            "timestamp": 1.1481273933038434
-        },
-        {
-            "x": 4.355560152796487,
-            "y": 1.9823487857199633,
-            "heading": -0.3404702376957046,
-            "angularVelocity": -0.17559732033810904,
-            "velocityX": -1.4851996999249673,
-            "velocityY": 3.4010889027473166,
-            "timestamp": 1.2014527300798554
-        },
-        {
-            "x": 4.29961502910342,
-            "y": 2.1668323093896307,
-            "heading": -0.3620606217709223,
-            "angularVelocity": -0.40488040734269476,
-            "velocityX": -1.049128370778876,
-            "velocityY": 3.4595847832064672,
-            "timestamp": 1.2547780668558675
-        },
-        {
-            "x": 4.26624261518445,
-            "y": 2.3445509425061104,
-            "heading": -0.3930653835662206,
-            "angularVelocity": -0.5814264600999738,
-            "velocityX": -0.6258265945712352,
-            "velocityY": 3.332724064417915,
-            "timestamp": 1.3081034036318795
-        },
-        {
-            "x": 4.250245555353669,
-            "y": 2.506179624728893,
-            "heading": -0.42827286207411436,
-            "angularVelocity": -0.6602392152823773,
-            "velocityX": -0.2999898509389163,
-            "velocityY": 3.0309922448647635,
-            "timestamp": 1.3614287404078915
-        },
-        {
-            "x": 4.245253885327146,
-            "y": 2.646807044638539,
-            "heading": -0.46326114250173545,
-            "angularVelocity": -0.6561286349527023,
-            "velocityX": -0.09360784813172378,
-            "velocityY": 2.6371595270071015,
-            "timestamp": 1.4147540771839036
-        },
-        {
-            "x": 4.246491895421447,
-            "y": 2.764573131222025,
-            "heading": -0.49520995766037457,
-            "angularVelocity": -0.5991301150594935,
-            "velocityX": 0.023216170194527277,
-            "velocityY": 2.2084452476729117,
-            "timestamp": 1.4680794139599156
-        },
-        {
-            "x": 4.250760184591696,
-            "y": 2.858861309855261,
-            "heading": -0.522398988898447,
-            "angularVelocity": -0.5098707834110707,
-            "velocityX": 0.08004242313776105,
-            "velocityY": 1.7681684605074017,
-            "timestamp": 1.5214047507359276
-        },
-        {
-            "x": 4.255888225984497,
-            "y": 2.929511890736146,
-            "heading": -0.5437259487715407,
-            "angularVelocity": -0.3999404628668233,
-            "velocityX": 0.09616519468543994,
-            "velocityY": 1.324897040550944,
-            "timestamp": 1.5747300875119397
-        },
-        {
-            "x": 4.260343621579396,
-            "y": 2.9765343465211713,
-            "heading": -0.5584397105575287,
-            "angularVelocity": -0.2759244043319095,
-            "velocityX": 0.08355119468920533,
-            "velocityY": 0.8818032595380478,
-            "timestamp": 1.6280554242879517
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -0.14177668439456792,
-            "velocityX": 0.04981456435476623,
-            "velocityY": 0.4400469813703916,
-            "timestamp": 1.6813807610639637
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 0,
-            "velocityX": 3.549235715029611e-32,
-            "velocityY": -9.598836837089386e-32,
-            "timestamp": 1.7347060978399758
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.traj
deleted file mode 100644
index d9cc7c97..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.traj	
+++ /dev/null
@@ -1,247 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -3.982415645335998e-31,
-            "velocityX": 3.346280928312519e-33,
-            "velocityY": 3.541078180282768e-38,
-            "timestamp": 0
-        },
-        {
-            "x": 7.629395539536042,
-            "y": 0.7881897280541793,
-            "heading": -0.02207380596023684,
-            "angularVelocity": -0.2822732032732244,
-            "velocityX": -0.6471146471374848,
-            "velocityY": 0.015213885156899113,
-            "timestamp": 0.0782001468948451
-        },
-        {
-            "x": 7.528184091504648,
-            "y": 0.7905692108612611,
-            "heading": -0.06614402373093474,
-            "angularVelocity": -0.5635567133951742,
-            "velocityX": -1.2942616101155584,
-            "velocityY": 0.030428111729043,
-            "timestamp": 0.1564002937896902
-        },
-        {
-            "x": 7.376360973312952,
-            "y": 0.7941378827255743,
-            "heading": -0.13206235338942335,
-            "angularVelocity": -0.8429438086302324,
-            "velocityX": -1.9414684526853105,
-            "velocityY": 0.04563510435477082,
-            "timestamp": 0.23460044068453528
-        },
-        {
-            "x": 7.173916466766183,
-            "y": 0.7988917309385671,
-            "heading": -0.21951772550448787,
-            "angularVelocity": -1.1183530413716716,
-            "velocityX": -2.588799568612108,
-            "velocityY": 0.060790783668257076,
-            "timestamp": 0.3128005875793804
-        },
-        {
-            "x": 6.920824289019536,
-            "y": 0.8048108023879232,
-            "heading": -0.3276760191057486,
-            "angularVelocity": -1.383095785572821,
-            "velocityX": -3.236466781667369,
-            "velocityY": 0.07569130857196414,
-            "timestamp": 0.3910007344742255
-        },
-        {
-            "x": 6.625087122175782,
-            "y": 0.8120803100358527,
-            "heading": -0.3276760358371292,
-            "angularVelocity": -2.1395587179409026e-7,
-            "velocityX": -3.781798098684327,
-            "velocityY": 0.09296028123113304,
-            "timestamp": 0.4692008813690706
-        },
-        {
-            "x": 6.329343587367458,
-            "y": 0.8190859886904246,
-            "heading": -0.3276760513572638,
-            "angularVelocity": -1.9846682128968708e-7,
-            "velocityX": -3.78187953030339,
-            "velocityY": 0.08958651527359804,
-            "timestamp": 0.5474010282639157
-        },
-        {
-            "x": 6.033600055760057,
-            "y": 0.826091802470254,
-            "heading": -0.3276760668773978,
-            "angularVelocity": -1.9846681457625392e-7,
-            "velocityX": -3.7818794893709464,
-            "velocityY": 0.0895882432148227,
-            "timestamp": 0.6256011751587608
-        },
-        {
-            "x": 5.7383102527596765,
-            "y": 0.8439033174946344,
-            "heading": -0.3276760825327013,
-            "angularVelocity": -2.0019532088764418e-7,
-            "velocityX": -3.7760773441696487,
-            "velocityY": 0.22776830647924015,
-            "timestamp": 0.7038013220536059
-        },
-        {
-            "x": 5.450465299571764,
-            "y": 0.9121572529445121,
-            "heading": -0.32767609968005607,
-            "angularVelocity": -2.1927522433491614e-7,
-            "velocityX": -3.680874839978332,
-            "velocityY": 0.8728108342551084,
-            "timestamp": 0.782001468948451
-        },
-        {
-            "x": 5.178616904201158,
-            "y": 1.028826716826592,
-            "heading": -0.32767612118071976,
-            "angularVelocity": -2.749440322201668e-7,
-            "velocityX": -3.4763156613508643,
-            "velocityY": 1.4919340757663395,
-            "timestamp": 0.8602016158432961
-        },
-        {
-            "x": 4.930841529412244,
-            "y": 1.190445624246428,
-            "heading": -0.3276761534331376,
-            "angularVelocity": -4.1243423626599064e-7,
-            "velocityX": -3.1684771017365216,
-            "velocityY": 2.0667340642903906,
-            "timestamp": 0.9384017627381412
-        },
-        {
-            "x": 4.7145004488669215,
-            "y": 1.3922123936122235,
-            "heading": -0.32767621878386327,
-            "angularVelocity": -8.356854591223944e-7,
-            "velocityX": -2.7665047846524513,
-            "velocityY": 2.5801328690255976,
-            "timestamp": 1.0166019096329864
-        },
-        {
-            "x": 4.536026477813721,
-            "y": 1.628126621246338,
-            "heading": -0.327696539002069,
-            "angularVelocity": -0.00025984882823803823,
-            "velocityX": -2.282271557538034,
-            "velocityY": 3.016800313073996,
-            "timestamp": 1.0948020565278314
-        },
-        {
-            "x": 4.4347589269746495,
-            "y": 1.8009845745755027,
-            "heading": -0.3311064514524199,
-            "angularVelocity": -0.06394544615832723,
-            "velocityX": -1.899051313343304,
-            "velocityY": 3.241572651564751,
-            "timestamp": 1.1481273933038434
-        },
-        {
-            "x": 4.355560152796487,
-            "y": 1.9823487857199633,
-            "heading": -0.3404702376957046,
-            "angularVelocity": -0.17559732033810904,
-            "velocityX": -1.4851996999249673,
-            "velocityY": 3.4010889027473166,
-            "timestamp": 1.2014527300798554
-        },
-        {
-            "x": 4.29961502910342,
-            "y": 2.1668323093896307,
-            "heading": -0.3620606217709223,
-            "angularVelocity": -0.40488040734269476,
-            "velocityX": -1.049128370778876,
-            "velocityY": 3.4595847832064672,
-            "timestamp": 1.2547780668558675
-        },
-        {
-            "x": 4.26624261518445,
-            "y": 2.3445509425061104,
-            "heading": -0.3930653835662206,
-            "angularVelocity": -0.5814264600999738,
-            "velocityX": -0.6258265945712352,
-            "velocityY": 3.332724064417915,
-            "timestamp": 1.3081034036318795
-        },
-        {
-            "x": 4.250245555353669,
-            "y": 2.506179624728893,
-            "heading": -0.42827286207411436,
-            "angularVelocity": -0.6602392152823773,
-            "velocityX": -0.2999898509389163,
-            "velocityY": 3.0309922448647635,
-            "timestamp": 1.3614287404078915
-        },
-        {
-            "x": 4.245253885327146,
-            "y": 2.646807044638539,
-            "heading": -0.46326114250173545,
-            "angularVelocity": -0.6561286349527023,
-            "velocityX": -0.09360784813172378,
-            "velocityY": 2.6371595270071015,
-            "timestamp": 1.4147540771839036
-        },
-        {
-            "x": 4.246491895421447,
-            "y": 2.764573131222025,
-            "heading": -0.49520995766037457,
-            "angularVelocity": -0.5991301150594935,
-            "velocityX": 0.023216170194527277,
-            "velocityY": 2.2084452476729117,
-            "timestamp": 1.4680794139599156
-        },
-        {
-            "x": 4.250760184591696,
-            "y": 2.858861309855261,
-            "heading": -0.522398988898447,
-            "angularVelocity": -0.5098707834110707,
-            "velocityX": 0.08004242313776105,
-            "velocityY": 1.7681684605074017,
-            "timestamp": 1.5214047507359276
-        },
-        {
-            "x": 4.255888225984497,
-            "y": 2.929511890736146,
-            "heading": -0.5437259487715407,
-            "angularVelocity": -0.3999404628668233,
-            "velocityX": 0.09616519468543994,
-            "velocityY": 1.324897040550944,
-            "timestamp": 1.5747300875119397
-        },
-        {
-            "x": 4.260343621579396,
-            "y": 2.9765343465211713,
-            "heading": -0.5584397105575287,
-            "angularVelocity": -0.2759244043319095,
-            "velocityX": 0.08355119468920533,
-            "velocityY": 0.8818032595380478,
-            "timestamp": 1.6280554242879517
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -0.14177668439456792,
-            "velocityX": 0.04981456435476623,
-            "velocityY": 0.4400469813703916,
-            "timestamp": 1.6813807610639637
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 0,
-            "velocityX": 3.549235715029611e-32,
-            "velocityY": -9.598836837089386e-32,
-            "timestamp": 1.7347060978399758
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.1.traj
deleted file mode 100644
index 773c6f5e..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 2.8467808772943154e-31,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.633526252649942,
-            "y": 0.8024218861992577,
-            "heading": -0.02675508445787236,
-            "angularVelocity": -0.3471453590528024,
-            "velocityX": -0.602993637929559,
-            "velocityY": 0.20009790028293237,
-            "timestamp": 0.07707170428800919
-        },
-        {
-            "x": 7.540488865885011,
-            "y": 0.8333059401759839,
-            "heading": -0.07783349665964928,
-            "angularVelocity": -0.6627388439588938,
-            "velocityX": -1.2071536191448495,
-            "velocityY": 0.400718451240102,
-            "timestamp": 0.15414340857601838
-        },
-        {
-            "x": 7.400762361365792,
-            "y": 0.8797032018771439,
-            "heading": -0.14939031986058435,
-            "angularVelocity": -0.9284448016555393,
-            "velocityX": -1.8129416730824437,
-            "velocityY": 0.6020012419574645,
-            "timestamp": 0.23121511286402757
-        },
-        {
-            "x": 7.214174023420098,
-            "y": 0.9416744881041137,
-            "heading": -0.23458534610519022,
-            "angularVelocity": -1.105399537114693,
-            "velocityX": -2.4209707008480317,
-            "velocityY": 0.8040731264406625,
-            "timestamp": 0.30828681715203676
-        },
-        {
-            "x": 6.98058449689491,
-            "y": 1.0192605514016468,
-            "heading": -0.3182722929391017,
-            "angularVelocity": -1.085832311702643,
-            "velocityX": -3.030807852026836,
-            "velocityY": 1.0066737723562156,
-            "timestamp": 0.38535852144004595
-        },
-        {
-            "x": 6.7044949607857305,
-            "y": 1.1109419048147462,
-            "heading": -0.3204977394277859,
-            "angularVelocity": -0.02887501333002772,
-            "velocityX": -3.5822425189594846,
-            "velocityY": 1.189559180766205,
-            "timestamp": 0.46243022572805514
-        },
-        {
-            "x": 6.427835578289735,
-            "y": 1.202949830409229,
-            "heading": -0.32049776831244,
-            "angularVelocity": -3.7477637567079044e-7,
-            "velocityX": -3.5896362361749183,
-            "velocityY": 1.1937964320946928,
-            "timestamp": 0.5395019300160644
-        },
-        {
-            "x": 6.1511761970625,
-            "y": 1.294957759818759,
-            "heading": -0.3204977971970938,
-            "angularVelocity": -3.7477637370825724e-7,
-            "velocityX": -3.589636219712834,
-            "velocityY": 1.1937964815946673,
-            "timestamp": 0.6165736343040735
-        },
-        {
-            "x": 5.87524941446386,
-            "y": 1.3871813457379307,
-            "heading": -0.32266464886855784,
-            "angularVelocity": -0.028114749653993878,
-            "velocityX": -3.5801308034856563,
-            "velocityY": 1.1965946098005253,
-            "timestamp": 0.6936453385920827
-        },
-        {
-            "x": 5.641703675217654,
-            "y": 1.4649149367510799,
-            "heading": -0.4060390277887848,
-            "angularVelocity": -1.081776764773039,
-            "velocityX": -3.030239715129037,
-            "velocityY": 1.0085879342004245,
-            "timestamp": 0.7707170428800918
-        },
-        {
-            "x": 5.455154020778328,
-            "y": 1.5270189169325792,
-            "heading": -0.49098864699750666,
-            "angularVelocity": -1.1022153979010723,
-            "velocityX": -2.420468785044752,
-            "velocityY": 0.8057948212669979,
-            "timestamp": 0.8477887471681009
-        },
-        {
-            "x": 5.315462796839212,
-            "y": 1.573537321073397,
-            "heading": -0.5623285861822422,
-            "angularVelocity": -0.9256307466375128,
-            "velocityX": -1.8124839100106522,
-            "velocityY": 0.6035730566821683,
-            "timestamp": 0.9248604514561101
-        },
-        {
-            "x": 5.222455052636063,
-            "y": 1.6045200564296747,
-            "heading": -0.613275527154411,
-            "angularVelocity": -0.6610330139033284,
-            "velocityX": -1.2067690089684353,
-            "velocityY": 0.40199883527290925,
-            "timestamp": 1.0019321557441192
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -0.34674817551357506,
-            "velocityX": -0.602751075316368,
-            "velocityY": 0.2008511906325378,
-            "timestamp": 1.0790038600321283
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 8.760742141155177e-29,
-            "velocityX": 3.313743560366677e-30,
-            "velocityY": 1.2206105198294578e-30,
-            "timestamp": 1.1560755643201375
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.traj
deleted file mode 100644
index 773c6f5e..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 2.8467808772943154e-31,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.633526252649942,
-            "y": 0.8024218861992577,
-            "heading": -0.02675508445787236,
-            "angularVelocity": -0.3471453590528024,
-            "velocityX": -0.602993637929559,
-            "velocityY": 0.20009790028293237,
-            "timestamp": 0.07707170428800919
-        },
-        {
-            "x": 7.540488865885011,
-            "y": 0.8333059401759839,
-            "heading": -0.07783349665964928,
-            "angularVelocity": -0.6627388439588938,
-            "velocityX": -1.2071536191448495,
-            "velocityY": 0.400718451240102,
-            "timestamp": 0.15414340857601838
-        },
-        {
-            "x": 7.400762361365792,
-            "y": 0.8797032018771439,
-            "heading": -0.14939031986058435,
-            "angularVelocity": -0.9284448016555393,
-            "velocityX": -1.8129416730824437,
-            "velocityY": 0.6020012419574645,
-            "timestamp": 0.23121511286402757
-        },
-        {
-            "x": 7.214174023420098,
-            "y": 0.9416744881041137,
-            "heading": -0.23458534610519022,
-            "angularVelocity": -1.105399537114693,
-            "velocityX": -2.4209707008480317,
-            "velocityY": 0.8040731264406625,
-            "timestamp": 0.30828681715203676
-        },
-        {
-            "x": 6.98058449689491,
-            "y": 1.0192605514016468,
-            "heading": -0.3182722929391017,
-            "angularVelocity": -1.085832311702643,
-            "velocityX": -3.030807852026836,
-            "velocityY": 1.0066737723562156,
-            "timestamp": 0.38535852144004595
-        },
-        {
-            "x": 6.7044949607857305,
-            "y": 1.1109419048147462,
-            "heading": -0.3204977394277859,
-            "angularVelocity": -0.02887501333002772,
-            "velocityX": -3.5822425189594846,
-            "velocityY": 1.189559180766205,
-            "timestamp": 0.46243022572805514
-        },
-        {
-            "x": 6.427835578289735,
-            "y": 1.202949830409229,
-            "heading": -0.32049776831244,
-            "angularVelocity": -3.7477637567079044e-7,
-            "velocityX": -3.5896362361749183,
-            "velocityY": 1.1937964320946928,
-            "timestamp": 0.5395019300160644
-        },
-        {
-            "x": 6.1511761970625,
-            "y": 1.294957759818759,
-            "heading": -0.3204977971970938,
-            "angularVelocity": -3.7477637370825724e-7,
-            "velocityX": -3.589636219712834,
-            "velocityY": 1.1937964815946673,
-            "timestamp": 0.6165736343040735
-        },
-        {
-            "x": 5.87524941446386,
-            "y": 1.3871813457379307,
-            "heading": -0.32266464886855784,
-            "angularVelocity": -0.028114749653993878,
-            "velocityX": -3.5801308034856563,
-            "velocityY": 1.1965946098005253,
-            "timestamp": 0.6936453385920827
-        },
-        {
-            "x": 5.641703675217654,
-            "y": 1.4649149367510799,
-            "heading": -0.4060390277887848,
-            "angularVelocity": -1.081776764773039,
-            "velocityX": -3.030239715129037,
-            "velocityY": 1.0085879342004245,
-            "timestamp": 0.7707170428800918
-        },
-        {
-            "x": 5.455154020778328,
-            "y": 1.5270189169325792,
-            "heading": -0.49098864699750666,
-            "angularVelocity": -1.1022153979010723,
-            "velocityX": -2.420468785044752,
-            "velocityY": 0.8057948212669979,
-            "timestamp": 0.8477887471681009
-        },
-        {
-            "x": 5.315462796839212,
-            "y": 1.573537321073397,
-            "heading": -0.5623285861822422,
-            "angularVelocity": -0.9256307466375128,
-            "velocityX": -1.8124839100106522,
-            "velocityY": 0.6035730566821683,
-            "timestamp": 0.9248604514561101
-        },
-        {
-            "x": 5.222455052636063,
-            "y": 1.6045200564296747,
-            "heading": -0.613275527154411,
-            "angularVelocity": -0.6610330139033284,
-            "velocityX": -1.2067690089684353,
-            "velocityY": 0.40199883527290925,
-            "timestamp": 1.0019321557441192
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -0.34674817551357506,
-            "velocityX": -0.602751075316368,
-            "velocityY": 0.2008511906325378,
-            "timestamp": 1.0790038600321283
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 8.760742141155177e-29,
-            "velocityX": 3.313743560366677e-30,
-            "velocityY": 1.2206105198294578e-30,
-            "timestamp": 1.1560755643201375
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.1.traj
deleted file mode 100644
index bec33f37..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.1.traj	
+++ /dev/null
@@ -1,310 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -3.516607019090483e-32,
-            "velocityX": -4.092268936373955e-33,
-            "velocityY": -4.396806616029776e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 7.6580594556627535,
-            "y": 0.8642766421574168,
-            "heading": -0.06977499787943942,
-            "angularVelocity": -0.7009716360726537,
-            "velocityX": -0.22041848409624198,
-            "velocityY": 0.7763344454252913,
-            "timestamp": 0.09954040119279155
-        },
-        {
-            "x": 7.613877684624128,
-            "y": 1.0194223152597515,
-            "heading": -0.19807907998906352,
-            "angularVelocity": -1.2889648883484262,
-            "velocityX": -0.4438576749661031,
-            "velocityY": 1.558620130552277,
-            "timestamp": 0.1990808023855831
-        },
-        {
-            "x": 7.54706531781781,
-            "y": 1.253315507091588,
-            "heading": -0.3641253773176819,
-            "angularVelocity": -1.6681296773861405,
-            "velocityX": -0.6712085344815377,
-            "velocityY": 2.349731255139591,
-            "timestamp": 0.2986212035783746
-        },
-        {
-            "x": 7.457648537893544,
-            "y": 1.5658706931334108,
-            "heading": -0.49008580394214096,
-            "angularVelocity": -1.265420122031624,
-            "velocityX": -0.8982963585919527,
-            "velocityY": 3.139983185686188,
-            "timestamp": 0.3981616047711662
-        },
-        {
-            "x": 7.353528135913809,
-            "y": 1.9277448806018234,
-            "heading": -0.490085841920944,
-            "angularVelocity": -3.8154159059291386e-7,
-            "velocityX": -1.0460114760646044,
-            "velocityY": 3.6354503611807685,
-            "timestamp": 0.49770200596395775
-        },
-        {
-            "x": 7.249407681423134,
-            "y": 2.2896190529615703,
-            "heading": -0.4900858798995876,
-            "angularVelocity": -3.8153998933914064e-7,
-            "velocityX": -1.0460120035985523,
-            "velocityY": 3.635450209396512,
-            "timestamp": 0.5972424071567493
-        },
-        {
-            "x": 7.145287117295142,
-            "y": 2.651493193775847,
-            "heading": -0.4900859178782245,
-            "angularVelocity": -3.8153992177326955e-7,
-            "velocityX": -1.0460131050338848,
-            "velocityY": 3.6354498924852887,
-            "timestamp": 0.6967828083495408
-        },
-        {
-            "x": 7.025437006860337,
-            "y": 3.00846649318931,
-            "heading": -0.49008595631381496,
-            "angularVelocity": -3.8613055604326664e-7,
-            "velocityX": -1.204034834083884,
-            "velocityY": 3.586215196401226,
-            "timestamp": 0.7963232095423323
-        },
-        {
-            "x": 6.830624533504276,
-            "y": 3.3307118703307705,
-            "heading": -0.49008600153372534,
-            "angularVelocity": -4.542870012894294e-7,
-            "velocityX": -1.9571196320451347,
-            "velocityY": 3.237332513029861,
-            "timestamp": 0.8958636107351238
-        },
-        {
-            "x": 6.570226686668516,
-            "y": 3.602717269957059,
-            "heading": -0.4900860357215963,
-            "angularVelocity": -3.4345723556551647e-7,
-            "velocityX": -2.6160015804177474,
-            "velocityY": 2.7326130532612987,
-            "timestamp": 0.9954040119279153
-        },
-        {
-            "x": 6.270670526052627,
-            "y": 3.8032215967371097,
-            "heading": -0.4416564414377051,
-            "angularVelocity": 0.4865320382835506,
-            "velocityX": -3.009392739292897,
-            "velocityY": 2.0143009710369753,
-            "timestamp": 1.094944413120707
-        },
-        {
-            "x": 5.9498699270968665,
-            "y": 3.9255658761230467,
-            "heading": -0.3340733830984328,
-            "angularVelocity": 1.0807979177309486,
-            "velocityX": -3.222818022748657,
-            "velocityY": 1.229091684581206,
-            "timestamp": 1.1944848143134985
-        },
-        {
-            "x": 5.637666976812727,
-            "y": 3.9694128449739723,
-            "heading": -0.13726885518093873,
-            "angularVelocity": 1.977132154976145,
-            "velocityX": -3.136444564649281,
-            "velocityY": 0.4404941945733328,
-            "timestamp": 1.2940252155062901
-        },
-        {
-            "x": 5.333615486534618,
-            "y": 3.942111372133816,
-            "heading": 0.08955200005531357,
-            "angularVelocity": 2.278681344642582,
-            "velocityX": -3.0545535946677287,
-            "velocityY": -0.2742752943830166,
-            "timestamp": 1.3935656166990817
-        },
-        {
-            "x": 5.078082684497777,
-            "y": 3.9650753252247246,
-            "heading": 0.19884455131252843,
-            "angularVelocity": 1.0979717777662474,
-            "velocityX": -2.5671265031564414,
-            "velocityY": 0.2306998245509574,
-            "timestamp": 1.4931060178918734
-        },
-        {
-            "x": 4.8543877601623535,
-            "y": 4.064273357391357,
-            "heading": 0.3064368289582943,
-            "angularVelocity": 1.0808905364705061,
-            "velocityX": -2.2472777048805277,
-            "velocityY": 0.9965605018459247,
-            "timestamp": 1.592646419084665
-        },
-        {
-            "x": 4.700013932982481,
-            "y": 4.187105651970831,
-            "heading": 0.3888797273509616,
-            "angularVelocity": 1.0676410431648378,
-            "velocityX": -1.99915137751061,
-            "velocityY": 1.5906864226746236,
-            "timestamp": 1.6698660978526145
-        },
-        {
-            "x": 4.564169886311024,
-            "y": 4.35607150380987,
-            "heading": 0.4692148602751942,
-            "angularVelocity": 1.040345339504005,
-            "velocityX": -1.7591894817340108,
-            "velocityY": 2.1881190719116157,
-            "timestamp": 1.747085776620564
-        },
-        {
-            "x": 4.445019228108457,
-            "y": 4.571847534724939,
-            "heading": 0.5443267073647724,
-            "angularVelocity": 0.9727034389160626,
-            "velocityX": -1.543008985580261,
-            "velocityY": 2.7943140188848803,
-            "timestamp": 1.8243054553885136
-        },
-        {
-            "x": 4.302573716554072,
-            "y": 4.826880596020871,
-            "heading": 0.5443268349443496,
-            "angularVelocity": 0.0000016521640508021742,
-            "velocityX": -1.8446788930894609,
-            "velocityY": 3.302695185541032,
-            "timestamp": 1.9015251341564632
-        },
-        {
-            "x": 4.139811862406642,
-            "y": 5.06945260129317,
-            "heading": 0.544326895208066,
-            "angularVelocity": 7.80419154390702e-7,
-            "velocityX": -2.1077768872432143,
-            "velocityY": 3.141323677365259,
-            "timestamp": 1.9787448129244127
-        },
-        {
-            "x": 3.9382711614032555,
-            "y": 5.28090933180296,
-            "heading": 0.5443268879285122,
-            "angularVelocity": -9.42707078520697e-8,
-            "velocityX": -2.60996554529876,
-            "velocityY": 2.738378790013248,
-            "timestamp": 2.0559644916923623
-        },
-        {
-            "x": 3.721249786731846,
-            "y": 5.476445328956613,
-            "heading": 0.5443268807879367,
-            "angularVelocity": -9.247093001843253e-8,
-            "velocityX": -2.810441303745562,
-            "velocityY": 2.53220422919983,
-            "timestamp": 2.1331841704603116
-        },
-        {
-            "x": 3.5042282470745802,
-            "y": 5.671981142995692,
-            "heading": 0.5443268736473624,
-            "angularVelocity": -9.247091428352055e-8,
-            "velocityX": -2.810443440323428,
-            "velocityY": 2.53220185785385,
-            "timestamp": 2.210403849228261
-        },
-        {
-            "x": 3.287206691377032,
-            "y": 5.867516939231946,
-            "heading": 0.5443268665067905,
-            "angularVelocity": -9.247088327815795e-8,
-            "velocityX": -2.810443648046155,
-            "velocityY": 2.5322016273060566,
-            "timestamp": 2.2876235279962103
-        },
-        {
-            "x": 3.0701851252220784,
-            "y": 6.063052723861702,
-            "heading": 0.5443268593662193,
-            "angularVelocity": -9.247087442211068e-8,
-            "velocityX": -2.8104437834702534,
-            "velocityY": 2.532201477001163,
-            "timestamp": 2.3648432067641596
-        },
-        {
-            "x": 2.853163562032648,
-            "y": 6.258588511782302,
-            "heading": 0.5443268522252032,
-            "angularVelocity": -9.247663534512907e-8,
-            "velocityX": -2.8104437450665345,
-            "velocityY": 2.5322015196178005,
-            "timestamp": 2.442062885532109
-        },
-        {
-            "x": 2.6687757934039245,
-            "y": 6.424725614600548,
-            "heading": 0.4938774787934042,
-            "angularVelocity": -0.6533227570578579,
-            "velocityX": -2.3878339248576954,
-            "velocityY": 2.1514865830703513,
-            "timestamp": 2.5192825643000583
-        },
-        {
-            "x": 2.521265507868239,
-            "y": 6.557635351096379,
-            "heading": 0.4535239115954702,
-            "angularVelocity": -0.522581391709739,
-            "velocityX": -1.9102680545844264,
-            "velocityY": 1.7211899688838848,
-            "timestamp": 2.5965022430680076
-        },
-        {
-            "x": 2.4106327602038906,
-            "y": 6.6573176734827975,
-            "heading": 0.42326153130587874,
-            "angularVelocity": -0.391899846935805,
-            "velocityX": -1.432701474928527,
-            "velocityY": 1.2908927358526117,
-            "timestamp": 2.673721921835957
-        },
-        {
-            "x": 2.336877585524826,
-            "y": 6.723772559032594,
-            "heading": 0.40308732310971707,
-            "angularVelocity": -0.2612573442164476,
-            "velocityX": -0.9551344405446706,
-            "velocityY": 0.8605952085024652,
-            "timestamp": 2.7509416006039062
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": -0.13063150832354717,
-            "velocityX": -0.4775671967717696,
-            "velocityY": 0.43029758084408004,
-            "timestamp": 2.8281612793718556
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": -5.871044564465464e-32,
-            "velocityX": 0,
-            "velocityY": -1.090928833064942e-33,
-            "timestamp": 2.905380958139805
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.traj
deleted file mode 100644
index bec33f37..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.traj	
+++ /dev/null
@@ -1,310 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -3.516607019090483e-32,
-            "velocityX": -4.092268936373955e-33,
-            "velocityY": -4.396806616029776e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 7.6580594556627535,
-            "y": 0.8642766421574168,
-            "heading": -0.06977499787943942,
-            "angularVelocity": -0.7009716360726537,
-            "velocityX": -0.22041848409624198,
-            "velocityY": 0.7763344454252913,
-            "timestamp": 0.09954040119279155
-        },
-        {
-            "x": 7.613877684624128,
-            "y": 1.0194223152597515,
-            "heading": -0.19807907998906352,
-            "angularVelocity": -1.2889648883484262,
-            "velocityX": -0.4438576749661031,
-            "velocityY": 1.558620130552277,
-            "timestamp": 0.1990808023855831
-        },
-        {
-            "x": 7.54706531781781,
-            "y": 1.253315507091588,
-            "heading": -0.3641253773176819,
-            "angularVelocity": -1.6681296773861405,
-            "velocityX": -0.6712085344815377,
-            "velocityY": 2.349731255139591,
-            "timestamp": 0.2986212035783746
-        },
-        {
-            "x": 7.457648537893544,
-            "y": 1.5658706931334108,
-            "heading": -0.49008580394214096,
-            "angularVelocity": -1.265420122031624,
-            "velocityX": -0.8982963585919527,
-            "velocityY": 3.139983185686188,
-            "timestamp": 0.3981616047711662
-        },
-        {
-            "x": 7.353528135913809,
-            "y": 1.9277448806018234,
-            "heading": -0.490085841920944,
-            "angularVelocity": -3.8154159059291386e-7,
-            "velocityX": -1.0460114760646044,
-            "velocityY": 3.6354503611807685,
-            "timestamp": 0.49770200596395775
-        },
-        {
-            "x": 7.249407681423134,
-            "y": 2.2896190529615703,
-            "heading": -0.4900858798995876,
-            "angularVelocity": -3.8153998933914064e-7,
-            "velocityX": -1.0460120035985523,
-            "velocityY": 3.635450209396512,
-            "timestamp": 0.5972424071567493
-        },
-        {
-            "x": 7.145287117295142,
-            "y": 2.651493193775847,
-            "heading": -0.4900859178782245,
-            "angularVelocity": -3.8153992177326955e-7,
-            "velocityX": -1.0460131050338848,
-            "velocityY": 3.6354498924852887,
-            "timestamp": 0.6967828083495408
-        },
-        {
-            "x": 7.025437006860337,
-            "y": 3.00846649318931,
-            "heading": -0.49008595631381496,
-            "angularVelocity": -3.8613055604326664e-7,
-            "velocityX": -1.204034834083884,
-            "velocityY": 3.586215196401226,
-            "timestamp": 0.7963232095423323
-        },
-        {
-            "x": 6.830624533504276,
-            "y": 3.3307118703307705,
-            "heading": -0.49008600153372534,
-            "angularVelocity": -4.542870012894294e-7,
-            "velocityX": -1.9571196320451347,
-            "velocityY": 3.237332513029861,
-            "timestamp": 0.8958636107351238
-        },
-        {
-            "x": 6.570226686668516,
-            "y": 3.602717269957059,
-            "heading": -0.4900860357215963,
-            "angularVelocity": -3.4345723556551647e-7,
-            "velocityX": -2.6160015804177474,
-            "velocityY": 2.7326130532612987,
-            "timestamp": 0.9954040119279153
-        },
-        {
-            "x": 6.270670526052627,
-            "y": 3.8032215967371097,
-            "heading": -0.4416564414377051,
-            "angularVelocity": 0.4865320382835506,
-            "velocityX": -3.009392739292897,
-            "velocityY": 2.0143009710369753,
-            "timestamp": 1.094944413120707
-        },
-        {
-            "x": 5.9498699270968665,
-            "y": 3.9255658761230467,
-            "heading": -0.3340733830984328,
-            "angularVelocity": 1.0807979177309486,
-            "velocityX": -3.222818022748657,
-            "velocityY": 1.229091684581206,
-            "timestamp": 1.1944848143134985
-        },
-        {
-            "x": 5.637666976812727,
-            "y": 3.9694128449739723,
-            "heading": -0.13726885518093873,
-            "angularVelocity": 1.977132154976145,
-            "velocityX": -3.136444564649281,
-            "velocityY": 0.4404941945733328,
-            "timestamp": 1.2940252155062901
-        },
-        {
-            "x": 5.333615486534618,
-            "y": 3.942111372133816,
-            "heading": 0.08955200005531357,
-            "angularVelocity": 2.278681344642582,
-            "velocityX": -3.0545535946677287,
-            "velocityY": -0.2742752943830166,
-            "timestamp": 1.3935656166990817
-        },
-        {
-            "x": 5.078082684497777,
-            "y": 3.9650753252247246,
-            "heading": 0.19884455131252843,
-            "angularVelocity": 1.0979717777662474,
-            "velocityX": -2.5671265031564414,
-            "velocityY": 0.2306998245509574,
-            "timestamp": 1.4931060178918734
-        },
-        {
-            "x": 4.8543877601623535,
-            "y": 4.064273357391357,
-            "heading": 0.3064368289582943,
-            "angularVelocity": 1.0808905364705061,
-            "velocityX": -2.2472777048805277,
-            "velocityY": 0.9965605018459247,
-            "timestamp": 1.592646419084665
-        },
-        {
-            "x": 4.700013932982481,
-            "y": 4.187105651970831,
-            "heading": 0.3888797273509616,
-            "angularVelocity": 1.0676410431648378,
-            "velocityX": -1.99915137751061,
-            "velocityY": 1.5906864226746236,
-            "timestamp": 1.6698660978526145
-        },
-        {
-            "x": 4.564169886311024,
-            "y": 4.35607150380987,
-            "heading": 0.4692148602751942,
-            "angularVelocity": 1.040345339504005,
-            "velocityX": -1.7591894817340108,
-            "velocityY": 2.1881190719116157,
-            "timestamp": 1.747085776620564
-        },
-        {
-            "x": 4.445019228108457,
-            "y": 4.571847534724939,
-            "heading": 0.5443267073647724,
-            "angularVelocity": 0.9727034389160626,
-            "velocityX": -1.543008985580261,
-            "velocityY": 2.7943140188848803,
-            "timestamp": 1.8243054553885136
-        },
-        {
-            "x": 4.302573716554072,
-            "y": 4.826880596020871,
-            "heading": 0.5443268349443496,
-            "angularVelocity": 0.0000016521640508021742,
-            "velocityX": -1.8446788930894609,
-            "velocityY": 3.302695185541032,
-            "timestamp": 1.9015251341564632
-        },
-        {
-            "x": 4.139811862406642,
-            "y": 5.06945260129317,
-            "heading": 0.544326895208066,
-            "angularVelocity": 7.80419154390702e-7,
-            "velocityX": -2.1077768872432143,
-            "velocityY": 3.141323677365259,
-            "timestamp": 1.9787448129244127
-        },
-        {
-            "x": 3.9382711614032555,
-            "y": 5.28090933180296,
-            "heading": 0.5443268879285122,
-            "angularVelocity": -9.42707078520697e-8,
-            "velocityX": -2.60996554529876,
-            "velocityY": 2.738378790013248,
-            "timestamp": 2.0559644916923623
-        },
-        {
-            "x": 3.721249786731846,
-            "y": 5.476445328956613,
-            "heading": 0.5443268807879367,
-            "angularVelocity": -9.247093001843253e-8,
-            "velocityX": -2.810441303745562,
-            "velocityY": 2.53220422919983,
-            "timestamp": 2.1331841704603116
-        },
-        {
-            "x": 3.5042282470745802,
-            "y": 5.671981142995692,
-            "heading": 0.5443268736473624,
-            "angularVelocity": -9.247091428352055e-8,
-            "velocityX": -2.810443440323428,
-            "velocityY": 2.53220185785385,
-            "timestamp": 2.210403849228261
-        },
-        {
-            "x": 3.287206691377032,
-            "y": 5.867516939231946,
-            "heading": 0.5443268665067905,
-            "angularVelocity": -9.247088327815795e-8,
-            "velocityX": -2.810443648046155,
-            "velocityY": 2.5322016273060566,
-            "timestamp": 2.2876235279962103
-        },
-        {
-            "x": 3.0701851252220784,
-            "y": 6.063052723861702,
-            "heading": 0.5443268593662193,
-            "angularVelocity": -9.247087442211068e-8,
-            "velocityX": -2.8104437834702534,
-            "velocityY": 2.532201477001163,
-            "timestamp": 2.3648432067641596
-        },
-        {
-            "x": 2.853163562032648,
-            "y": 6.258588511782302,
-            "heading": 0.5443268522252032,
-            "angularVelocity": -9.247663534512907e-8,
-            "velocityX": -2.8104437450665345,
-            "velocityY": 2.5322015196178005,
-            "timestamp": 2.442062885532109
-        },
-        {
-            "x": 2.6687757934039245,
-            "y": 6.424725614600548,
-            "heading": 0.4938774787934042,
-            "angularVelocity": -0.6533227570578579,
-            "velocityX": -2.3878339248576954,
-            "velocityY": 2.1514865830703513,
-            "timestamp": 2.5192825643000583
-        },
-        {
-            "x": 2.521265507868239,
-            "y": 6.557635351096379,
-            "heading": 0.4535239115954702,
-            "angularVelocity": -0.522581391709739,
-            "velocityX": -1.9102680545844264,
-            "velocityY": 1.7211899688838848,
-            "timestamp": 2.5965022430680076
-        },
-        {
-            "x": 2.4106327602038906,
-            "y": 6.6573176734827975,
-            "heading": 0.42326153130587874,
-            "angularVelocity": -0.391899846935805,
-            "velocityX": -1.432701474928527,
-            "velocityY": 1.2908927358526117,
-            "timestamp": 2.673721921835957
-        },
-        {
-            "x": 2.336877585524826,
-            "y": 6.723772559032594,
-            "heading": 0.40308732310971707,
-            "angularVelocity": -0.2612573442164476,
-            "velocityX": -0.9551344405446706,
-            "velocityY": 0.8605952085024652,
-            "timestamp": 2.7509416006039062
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": -0.13063150832354717,
-            "velocityX": -0.4775671967717696,
-            "velocityY": 0.43029758084408004,
-            "timestamp": 2.8281612793718556
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": -5.871044564465464e-32,
-            "velocityX": 0,
-            "velocityY": -1.090928833064942e-33,
-            "timestamp": 2.905380958139805
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.1.traj
deleted file mode 100644
index 50d9a810..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.1.traj	
+++ /dev/null
@@ -1,346 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -1.6344658510237988e-32,
-            "velocityX": 1.0699335804910329e-32,
-            "velocityY": 2.3378742346984854e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 7.662237951731404,
-            "y": 0.8490986120749341,
-            "heading": -0.06160217222995063,
-            "angularVelocity": -0.6883164695444892,
-            "velocityX": -0.19846557213082008,
-            "velocityY": 0.6938634772078657,
-            "timestamp": 0.0894968738300239
-        },
-        {
-            "x": 7.626433684211601,
-            "y": 0.9738312717265619,
-            "heading": -0.1754105069837687,
-            "angularVelocity": -1.271645923297472,
-            "velocityX": -0.4000616556484858,
-            "velocityY": 1.3937096829608293,
-            "timestamp": 0.1789937476600478
-        },
-        {
-            "x": 7.572249558904937,
-            "y": 1.161886141289839,
-            "heading": -0.32706746606296133,
-            "angularVelocity": -1.6945503523086813,
-            "velocityX": -0.6054303685464192,
-            "velocityY": 2.101245121929494,
-            "timestamp": 0.2684906214900717
-        },
-        {
-            "x": 7.499449827352778,
-            "y": 1.4140559658703253,
-            "heading": -0.4849879770703222,
-            "angularVelocity": -1.7645366173045323,
-            "velocityX": -0.8134332344437395,
-            "velocityY": 2.8176383575075215,
-            "timestamp": 0.3579874953200956
-        },
-        {
-            "x": 7.40801256781841,
-            "y": 1.7212752924886132,
-            "heading": -0.5291211000298751,
-            "angularVelocity": -0.4931247435901767,
-            "velocityX": -1.0216810445024997,
-            "velocityY": 3.432738077553094,
-            "timestamp": 0.4474843691501195
-        },
-        {
-            "x": 7.313226703333856,
-            "y": 2.0462974852478544,
-            "heading": -0.5291211457832385,
-            "angularVelocity": -5.112286210233085e-7,
-            "velocityX": -1.0590969318612773,
-            "velocityY": 3.631659731227445,
-            "timestamp": 0.5369812429801434
-        },
-        {
-            "x": 7.218440797992873,
-            "y": 2.371319666092203,
-            "heading": -0.5291211915365511,
-            "angularVelocity": -5.112280543858435e-7,
-            "velocityX": -1.059097388373643,
-            "velocityY": 3.6316595980955055,
-            "timestamp": 0.6264781168101673
-        },
-        {
-            "x": 7.1236547448927325,
-            "y": 2.6963418038456624,
-            "heading": -0.5291212372898596,
-            "angularVelocity": -5.112280079310825e-7,
-            "velocityX": -1.0590990393716089,
-            "velocityY": 3.631659116616238,
-            "timestamp": 0.7159749906401912
-        },
-        {
-            "x": 7.010927268830994,
-            "y": 3.0155851307576884,
-            "heading": -0.5291212838180788,
-            "angularVelocity": -5.198865285819975e-7,
-            "velocityX": -1.2595688680236534,
-            "velocityY": 3.567089142335248,
-            "timestamp": 0.8054718644702151
-        },
-        {
-            "x": 6.837725577807192,
-            "y": 3.30648882172331,
-            "heading": -0.5291213383056133,
-            "angularVelocity": -6.088205344213367e-7,
-            "velocityX": -1.9352820228419736,
-            "velocityY": 3.2504341047500516,
-            "timestamp": 0.894968738300239
-        },
-        {
-            "x": 6.610788937088648,
-            "y": 3.5577327922782125,
-            "heading": -0.5291214021937537,
-            "angularVelocity": -7.138589055640395e-7,
-            "velocityX": -2.535693494161062,
-            "velocityY": 2.8072932584447,
-            "timestamp": 0.984465612130263
-        },
-        {
-            "x": 6.351860794366729,
-            "y": 3.7516776606884883,
-            "heading": -0.4841599241371699,
-            "angularVelocity": 0.5023804310972586,
-            "velocityX": -2.8931529297178065,
-            "velocityY": 2.167057463690003,
-            "timestamp": 1.0739624859602868
-        },
-        {
-            "x": 6.072750894879938,
-            "y": 3.8834732684056306,
-            "heading": -0.3889789775410024,
-            "angularVelocity": 1.0635114113252733,
-            "velocityX": -3.1186552953445856,
-            "velocityY": 1.4726280603661466,
-            "timestamp": 1.1634593597903107
-        },
-        {
-            "x": 5.7916594444015015,
-            "y": 3.950870142776961,
-            "heading": -0.22937923493241555,
-            "angularVelocity": 1.783299636942698,
-            "velocityX": -3.1407963032574324,
-            "velocityY": 0.7530640064515952,
-            "timestamp": 1.2529562336203346
-        },
-        {
-            "x": 5.498028130376245,
-            "y": 3.9763121038871923,
-            "heading": -0.08643118620464799,
-            "angularVelocity": 1.5972406924431666,
-            "velocityX": -3.280911404603181,
-            "velocityY": 0.28427765151386064,
-            "timestamp": 1.3424531074503585
-        },
-        {
-            "x": 5.202994132142315,
-            "y": 4.066458551574186,
-            "heading": -0.003961984667318494,
-            "angularVelocity": 0.9214757790753437,
-            "velocityX": -3.2965844013084786,
-            "velocityY": 1.0072580619765965,
-            "timestamp": 1.4319499812803824
-        },
-        {
-            "x": 4.937438011169434,
-            "y": 4.2165327072143555,
-            "heading": 0.07947932900863551,
-            "angularVelocity": 0.9323377466170348,
-            "velocityX": -2.967211139433056,
-            "velocityY": 1.6768647799385226,
-            "timestamp": 1.5214468551104063
-        },
-        {
-            "x": 4.74667334234301,
-            "y": 4.373655515147294,
-            "heading": 0.13953709439935258,
-            "angularVelocity": 0.8363208396611413,
-            "velocityX": -2.6564502853656404,
-            "velocityY": 2.187978154123957,
-            "timestamp": 1.5932587274715584
-        },
-        {
-            "x": 4.579753248112816,
-            "y": 4.566217048926455,
-            "heading": 0.1819755811120341,
-            "angularVelocity": 0.5909675561619205,
-            "velocityX": -2.324408050394913,
-            "velocityY": 2.681472122196475,
-            "timestamp": 1.6650705998327104
-        },
-        {
-            "x": 4.438339398678278,
-            "y": 4.792399271574889,
-            "heading": 0.19511904158847473,
-            "angularVelocity": 0.18302628861061485,
-            "velocityX": -1.9692266025783034,
-            "velocityY": 3.1496494271996025,
-            "timestamp": 1.7368824721938625
-        },
-        {
-            "x": 4.33242543936342,
-            "y": 5.0425620352859015,
-            "heading": 0.1951190405932856,
-            "angularVelocity": -1.3858280983712203e-8,
-            "velocityX": -1.4748809052380785,
-            "velocityY": 3.483585032470794,
-            "timestamp": 1.8086943445550145
-        },
-        {
-            "x": 4.2346577127829645,
-            "y": 5.296019422041832,
-            "heading": 0.19511903926490842,
-            "angularVelocity": -1.849801636869094e-8,
-            "velocityX": -1.3614423822396335,
-            "velocityY": 3.5294635611400946,
-            "timestamp": 1.8805062169161666
-        },
-        {
-            "x": 4.124562899973198,
-            "y": 5.544370599514641,
-            "heading": 0.19511903792287785,
-            "angularVelocity": -1.8688143477674107e-8,
-            "velocityX": -1.533100435761989,
-            "velocityY": 3.4583581977060263,
-            "timestamp": 1.9523180892773186
-        },
-        {
-            "x": 3.9766612552398675,
-            "y": 5.772239592130343,
-            "heading": 0.1951190364645304,
-            "angularVelocity": -2.030788798410176e-8,
-            "velocityX": -2.059570929852819,
-            "velocityY": 3.1731381611903378,
-            "timestamp": 2.024129961638471
-        },
-        {
-            "x": 3.7946576924754596,
-            "y": 5.973917226463319,
-            "heading": 0.19511901792814293,
-            "angularVelocity": -2.581242745470272e-7,
-            "velocityX": -2.534449482797023,
-            "velocityY": 2.8084163203364336,
-            "timestamp": 2.095941833999623
-        },
-        {
-            "x": 3.5938514397947183,
-            "y": 6.137095652578835,
-            "heading": 0.18216101901891746,
-            "angularVelocity": -0.1804436854683003,
-            "velocityX": -2.7962820920593496,
-            "velocityY": 2.272304296633136,
-            "timestamp": 2.167753706360775
-        },
-        {
-            "x": 3.3950104775935763,
-            "y": 6.257338696148904,
-            "heading": 0.16464788328752675,
-            "angularVelocity": -0.2438752138826665,
-            "velocityX": -2.768914883616214,
-            "velocityY": 1.6744173298441403,
-            "timestamp": 2.239565578721927
-        },
-        {
-            "x": 3.203569657298003,
-            "y": 6.335228188048846,
-            "heading": 0.14501288414051958,
-            "angularVelocity": -0.273422743362824,
-            "velocityX": -2.6658658798476953,
-            "velocityY": 1.0846325174230818,
-            "timestamp": 2.311377451083079
-        },
-        {
-            "x": 3.0218761949257575,
-            "y": 6.3712387750954615,
-            "heading": 0.12433283555566121,
-            "angularVelocity": -0.2879753431418064,
-            "velocityX": -2.530131249864694,
-            "velocityY": 0.5014572919852904,
-            "timestamp": 2.383189323444231
-        },
-        {
-            "x": 2.851226329803467,
-            "y": 6.365692615509033,
-            "heading": 0.10320964185538832,
-            "angularVelocity": -0.2941462603013797,
-            "velocityX": -2.3763461320722596,
-            "velocityY": -0.07723179195964242,
-            "timestamp": 2.4550011958053832
-        },
-        {
-            "x": 2.6537141886903095,
-            "y": 6.292049225449601,
-            "heading": 0.07629879670595036,
-            "angularVelocity": -0.2952020622183997,
-            "velocityX": -2.1666354603877345,
-            "velocityY": -0.8078408720936208,
-            "timestamp": 2.5461619585290816
-        },
-        {
-            "x": 2.4816525262537397,
-            "y": 6.153957047067382,
-            "heading": 0.05014624127109134,
-            "angularVelocity": -0.2868839032657673,
-            "velocityX": -1.8874530806425645,
-            "velocityY": -1.5148203487586651,
-            "timestamp": 2.63732272125278
-        },
-        {
-            "x": 2.3499481448537543,
-            "y": 5.9595438725680525,
-            "heading": 0.026875208437583037,
-            "angularVelocity": -0.25527466135886906,
-            "velocityX": -1.4447485679685645,
-            "velocityY": -2.1326409377308697,
-            "timestamp": 2.728483483976478
-        },
-        {
-            "x": 2.2874888909600317,
-            "y": 5.764180997032623,
-            "heading": 0.012070368781366221,
-            "angularVelocity": -0.1624036396129025,
-            "velocityX": -0.6851550165615855,
-            "velocityY": -2.1430588083994038,
-            "timestamp": 2.8196442467001765
-        },
-        {
-            "x": 2.260118679493569,
-            "y": 5.628541423814637,
-            "heading": 0.0037496226116310134,
-            "angularVelocity": -0.09127552162935264,
-            "velocityX": -0.3002411415689918,
-            "velocityY": -1.487916173201615,
-            "timestamp": 2.910805009423875
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -4.5794503744404666e-32,
-            "angularVelocity": -0.04113197936919252,
-            "velocityX": -0.11099818815950499,
-            "velocityY": -0.7518741810265671,
-            "timestamp": 3.001965772147573
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -2.1498446437166114e-31,
-            "velocityX": 0,
-            "velocityY": -6.253874378421497e-34,
-            "timestamp": 3.0931265348712715
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.traj
deleted file mode 100644
index 50d9a810..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.traj	
+++ /dev/null
@@ -1,346 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -1.6344658510237988e-32,
-            "velocityX": 1.0699335804910329e-32,
-            "velocityY": 2.3378742346984854e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 7.662237951731404,
-            "y": 0.8490986120749341,
-            "heading": -0.06160217222995063,
-            "angularVelocity": -0.6883164695444892,
-            "velocityX": -0.19846557213082008,
-            "velocityY": 0.6938634772078657,
-            "timestamp": 0.0894968738300239
-        },
-        {
-            "x": 7.626433684211601,
-            "y": 0.9738312717265619,
-            "heading": -0.1754105069837687,
-            "angularVelocity": -1.271645923297472,
-            "velocityX": -0.4000616556484858,
-            "velocityY": 1.3937096829608293,
-            "timestamp": 0.1789937476600478
-        },
-        {
-            "x": 7.572249558904937,
-            "y": 1.161886141289839,
-            "heading": -0.32706746606296133,
-            "angularVelocity": -1.6945503523086813,
-            "velocityX": -0.6054303685464192,
-            "velocityY": 2.101245121929494,
-            "timestamp": 0.2684906214900717
-        },
-        {
-            "x": 7.499449827352778,
-            "y": 1.4140559658703253,
-            "heading": -0.4849879770703222,
-            "angularVelocity": -1.7645366173045323,
-            "velocityX": -0.8134332344437395,
-            "velocityY": 2.8176383575075215,
-            "timestamp": 0.3579874953200956
-        },
-        {
-            "x": 7.40801256781841,
-            "y": 1.7212752924886132,
-            "heading": -0.5291211000298751,
-            "angularVelocity": -0.4931247435901767,
-            "velocityX": -1.0216810445024997,
-            "velocityY": 3.432738077553094,
-            "timestamp": 0.4474843691501195
-        },
-        {
-            "x": 7.313226703333856,
-            "y": 2.0462974852478544,
-            "heading": -0.5291211457832385,
-            "angularVelocity": -5.112286210233085e-7,
-            "velocityX": -1.0590969318612773,
-            "velocityY": 3.631659731227445,
-            "timestamp": 0.5369812429801434
-        },
-        {
-            "x": 7.218440797992873,
-            "y": 2.371319666092203,
-            "heading": -0.5291211915365511,
-            "angularVelocity": -5.112280543858435e-7,
-            "velocityX": -1.059097388373643,
-            "velocityY": 3.6316595980955055,
-            "timestamp": 0.6264781168101673
-        },
-        {
-            "x": 7.1236547448927325,
-            "y": 2.6963418038456624,
-            "heading": -0.5291212372898596,
-            "angularVelocity": -5.112280079310825e-7,
-            "velocityX": -1.0590990393716089,
-            "velocityY": 3.631659116616238,
-            "timestamp": 0.7159749906401912
-        },
-        {
-            "x": 7.010927268830994,
-            "y": 3.0155851307576884,
-            "heading": -0.5291212838180788,
-            "angularVelocity": -5.198865285819975e-7,
-            "velocityX": -1.2595688680236534,
-            "velocityY": 3.567089142335248,
-            "timestamp": 0.8054718644702151
-        },
-        {
-            "x": 6.837725577807192,
-            "y": 3.30648882172331,
-            "heading": -0.5291213383056133,
-            "angularVelocity": -6.088205344213367e-7,
-            "velocityX": -1.9352820228419736,
-            "velocityY": 3.2504341047500516,
-            "timestamp": 0.894968738300239
-        },
-        {
-            "x": 6.610788937088648,
-            "y": 3.5577327922782125,
-            "heading": -0.5291214021937537,
-            "angularVelocity": -7.138589055640395e-7,
-            "velocityX": -2.535693494161062,
-            "velocityY": 2.8072932584447,
-            "timestamp": 0.984465612130263
-        },
-        {
-            "x": 6.351860794366729,
-            "y": 3.7516776606884883,
-            "heading": -0.4841599241371699,
-            "angularVelocity": 0.5023804310972586,
-            "velocityX": -2.8931529297178065,
-            "velocityY": 2.167057463690003,
-            "timestamp": 1.0739624859602868
-        },
-        {
-            "x": 6.072750894879938,
-            "y": 3.8834732684056306,
-            "heading": -0.3889789775410024,
-            "angularVelocity": 1.0635114113252733,
-            "velocityX": -3.1186552953445856,
-            "velocityY": 1.4726280603661466,
-            "timestamp": 1.1634593597903107
-        },
-        {
-            "x": 5.7916594444015015,
-            "y": 3.950870142776961,
-            "heading": -0.22937923493241555,
-            "angularVelocity": 1.783299636942698,
-            "velocityX": -3.1407963032574324,
-            "velocityY": 0.7530640064515952,
-            "timestamp": 1.2529562336203346
-        },
-        {
-            "x": 5.498028130376245,
-            "y": 3.9763121038871923,
-            "heading": -0.08643118620464799,
-            "angularVelocity": 1.5972406924431666,
-            "velocityX": -3.280911404603181,
-            "velocityY": 0.28427765151386064,
-            "timestamp": 1.3424531074503585
-        },
-        {
-            "x": 5.202994132142315,
-            "y": 4.066458551574186,
-            "heading": -0.003961984667318494,
-            "angularVelocity": 0.9214757790753437,
-            "velocityX": -3.2965844013084786,
-            "velocityY": 1.0072580619765965,
-            "timestamp": 1.4319499812803824
-        },
-        {
-            "x": 4.937438011169434,
-            "y": 4.2165327072143555,
-            "heading": 0.07947932900863551,
-            "angularVelocity": 0.9323377466170348,
-            "velocityX": -2.967211139433056,
-            "velocityY": 1.6768647799385226,
-            "timestamp": 1.5214468551104063
-        },
-        {
-            "x": 4.74667334234301,
-            "y": 4.373655515147294,
-            "heading": 0.13953709439935258,
-            "angularVelocity": 0.8363208396611413,
-            "velocityX": -2.6564502853656404,
-            "velocityY": 2.187978154123957,
-            "timestamp": 1.5932587274715584
-        },
-        {
-            "x": 4.579753248112816,
-            "y": 4.566217048926455,
-            "heading": 0.1819755811120341,
-            "angularVelocity": 0.5909675561619205,
-            "velocityX": -2.324408050394913,
-            "velocityY": 2.681472122196475,
-            "timestamp": 1.6650705998327104
-        },
-        {
-            "x": 4.438339398678278,
-            "y": 4.792399271574889,
-            "heading": 0.19511904158847473,
-            "angularVelocity": 0.18302628861061485,
-            "velocityX": -1.9692266025783034,
-            "velocityY": 3.1496494271996025,
-            "timestamp": 1.7368824721938625
-        },
-        {
-            "x": 4.33242543936342,
-            "y": 5.0425620352859015,
-            "heading": 0.1951190405932856,
-            "angularVelocity": -1.3858280983712203e-8,
-            "velocityX": -1.4748809052380785,
-            "velocityY": 3.483585032470794,
-            "timestamp": 1.8086943445550145
-        },
-        {
-            "x": 4.2346577127829645,
-            "y": 5.296019422041832,
-            "heading": 0.19511903926490842,
-            "angularVelocity": -1.849801636869094e-8,
-            "velocityX": -1.3614423822396335,
-            "velocityY": 3.5294635611400946,
-            "timestamp": 1.8805062169161666
-        },
-        {
-            "x": 4.124562899973198,
-            "y": 5.544370599514641,
-            "heading": 0.19511903792287785,
-            "angularVelocity": -1.8688143477674107e-8,
-            "velocityX": -1.533100435761989,
-            "velocityY": 3.4583581977060263,
-            "timestamp": 1.9523180892773186
-        },
-        {
-            "x": 3.9766612552398675,
-            "y": 5.772239592130343,
-            "heading": 0.1951190364645304,
-            "angularVelocity": -2.030788798410176e-8,
-            "velocityX": -2.059570929852819,
-            "velocityY": 3.1731381611903378,
-            "timestamp": 2.024129961638471
-        },
-        {
-            "x": 3.7946576924754596,
-            "y": 5.973917226463319,
-            "heading": 0.19511901792814293,
-            "angularVelocity": -2.581242745470272e-7,
-            "velocityX": -2.534449482797023,
-            "velocityY": 2.8084163203364336,
-            "timestamp": 2.095941833999623
-        },
-        {
-            "x": 3.5938514397947183,
-            "y": 6.137095652578835,
-            "heading": 0.18216101901891746,
-            "angularVelocity": -0.1804436854683003,
-            "velocityX": -2.7962820920593496,
-            "velocityY": 2.272304296633136,
-            "timestamp": 2.167753706360775
-        },
-        {
-            "x": 3.3950104775935763,
-            "y": 6.257338696148904,
-            "heading": 0.16464788328752675,
-            "angularVelocity": -0.2438752138826665,
-            "velocityX": -2.768914883616214,
-            "velocityY": 1.6744173298441403,
-            "timestamp": 2.239565578721927
-        },
-        {
-            "x": 3.203569657298003,
-            "y": 6.335228188048846,
-            "heading": 0.14501288414051958,
-            "angularVelocity": -0.273422743362824,
-            "velocityX": -2.6658658798476953,
-            "velocityY": 1.0846325174230818,
-            "timestamp": 2.311377451083079
-        },
-        {
-            "x": 3.0218761949257575,
-            "y": 6.3712387750954615,
-            "heading": 0.12433283555566121,
-            "angularVelocity": -0.2879753431418064,
-            "velocityX": -2.530131249864694,
-            "velocityY": 0.5014572919852904,
-            "timestamp": 2.383189323444231
-        },
-        {
-            "x": 2.851226329803467,
-            "y": 6.365692615509033,
-            "heading": 0.10320964185538832,
-            "angularVelocity": -0.2941462603013797,
-            "velocityX": -2.3763461320722596,
-            "velocityY": -0.07723179195964242,
-            "timestamp": 2.4550011958053832
-        },
-        {
-            "x": 2.6537141886903095,
-            "y": 6.292049225449601,
-            "heading": 0.07629879670595036,
-            "angularVelocity": -0.2952020622183997,
-            "velocityX": -2.1666354603877345,
-            "velocityY": -0.8078408720936208,
-            "timestamp": 2.5461619585290816
-        },
-        {
-            "x": 2.4816525262537397,
-            "y": 6.153957047067382,
-            "heading": 0.05014624127109134,
-            "angularVelocity": -0.2868839032657673,
-            "velocityX": -1.8874530806425645,
-            "velocityY": -1.5148203487586651,
-            "timestamp": 2.63732272125278
-        },
-        {
-            "x": 2.3499481448537543,
-            "y": 5.9595438725680525,
-            "heading": 0.026875208437583037,
-            "angularVelocity": -0.25527466135886906,
-            "velocityX": -1.4447485679685645,
-            "velocityY": -2.1326409377308697,
-            "timestamp": 2.728483483976478
-        },
-        {
-            "x": 2.2874888909600317,
-            "y": 5.764180997032623,
-            "heading": 0.012070368781366221,
-            "angularVelocity": -0.1624036396129025,
-            "velocityX": -0.6851550165615855,
-            "velocityY": -2.1430588083994038,
-            "timestamp": 2.8196442467001765
-        },
-        {
-            "x": 2.260118679493569,
-            "y": 5.628541423814637,
-            "heading": 0.0037496226116310134,
-            "angularVelocity": -0.09127552162935264,
-            "velocityX": -0.3002411415689918,
-            "velocityY": -1.487916173201615,
-            "timestamp": 2.910805009423875
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -4.5794503744404666e-32,
-            "angularVelocity": -0.04113197936919252,
-            "velocityX": -0.11099818815950499,
-            "velocityY": -0.7518741810265671,
-            "timestamp": 3.001965772147573
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -2.1498446437166114e-31,
-            "velocityX": 0,
-            "velocityY": -6.253874378421497e-34,
-            "timestamp": 3.0931265348712715
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W3.1.traj
deleted file mode 100644
index 62c8640c..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W3.1.traj	
+++ /dev/null
@@ -1,292 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -1.5069359770438582e-30,
-            "velocityX": 5.69693995403372e-32,
-            "velocityY": 3.042278119963715e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 7.627290492953592,
-            "y": 0.8035142159340071,
-            "heading": 0.015175150751654183,
-            "angularVelocity": 0.18617297608634506,
-            "velocityX": -0.6466549133823012,
-            "velocityY": 0.20260100070713255,
-            "timestamp": 0.08151102845675925
-        },
-        {
-            "x": 7.521871327482097,
-            "y": 0.8365426259859824,
-            "heading": 0.045515680376982795,
-            "angularVelocity": 0.3722260680519286,
-            "velocityX": -1.2933116839203953,
-            "velocityY": 0.4052017337690261,
-            "timestamp": 0.1630220569135185
-        },
-        {
-            "x": 7.363742244741542,
-            "y": 0.8860852747673063,
-            "heading": 0.09100430929463568,
-            "angularVelocity": 0.5580671693007231,
-            "velocityX": -1.9399716300284648,
-            "velocityY": 0.6078030141357551,
-            "timestamp": 0.24453308537027776
-        },
-        {
-            "x": 7.152902908699907,
-            "y": 0.95214234591484,
-            "heading": 0.15161661460632753,
-            "angularVelocity": 0.7436086436309897,
-            "velocityX": -2.5866356986709165,
-            "velocityY": 0.8104065474106494,
-            "timestamp": 0.326044113827037
-        },
-        {
-            "x": 6.889352971860368,
-            "y": 1.0347142415767305,
-            "heading": 0.22732297101557483,
-            "angularVelocity": 0.9287866665726976,
-            "velocityX": -3.233304030501219,
-            "velocityY": 1.0130150143510155,
-            "timestamp": 0.4075551422837963
-        },
-        {
-            "x": 6.595109187456524,
-            "y": 1.1269159867790934,
-            "heading": 0.22732298101704992,
-            "angularVelocity": 1.22700882310899e-7,
-            "velocityX": -3.6098647014365244,
-            "velocityY": 1.1311566906713055,
-            "timestamp": 0.4890661707405556
-        },
-        {
-            "x": 6.300865420236581,
-            "y": 1.219117786820698,
-            "heading": 0.2273229910184091,
-            "angularVelocity": 1.2269945998464997e-7,
-            "velocityX": -3.6098644906196586,
-            "velocityY": 1.131157363454394,
-            "timestamp": 0.5705771991973149
-        },
-        {
-            "x": 6.006621672014901,
-            "y": 1.3113196474914972,
-            "heading": 0.2273230010197575,
-            "angularVelocity": 1.2269932759504928e-7,
-            "velocityX": -3.6098642575436695,
-            "velocityY": 1.1311581072702415,
-            "timestamp": 0.6520882276540741
-        },
-        {
-            "x": 5.7123779465456614,
-            "y": 1.4035215807721424,
-            "heading": 0.22732301102110228,
-            "angularVelocity": 1.2269928353992546e-7,
-            "velocityX": -3.6098639784103828,
-            "velocityY": 1.131158998068056,
-            "timestamp": 0.7335992561108334
-        },
-        {
-            "x": 5.418134249112936,
-            "y": 1.4957236035255936,
-            "heading": 0.2273230210224565,
-            "angularVelocity": 1.2269939906393907e-7,
-            "velocityX": -3.60986363445063,
-            "velocityY": 1.1311600957453605,
-            "timestamp": 0.8151102845675927
-        },
-        {
-            "x": 5.123890658648026,
-            "y": 1.5879259676440571,
-            "heading": 0.22732303102385681,
-            "angularVelocity": 1.2269996504792678e-7,
-            "velocityX": -3.609862322139673,
-            "velocityY": 1.1311642837064038,
-            "timestamp": 0.896621313024352
-        },
-        {
-            "x": 4.838467298400658,
-            "y": 1.7046053225590203,
-            "heading": 0.22732304878104165,
-            "angularVelocity": 2.1785008908970575e-7,
-            "velocityX": -3.5016532811726435,
-            "velocityY": 1.4314548242617389,
-            "timestamp": 0.9781323414811113
-        },
-        {
-            "x": 4.5530441892872355,
-            "y": 1.8212852918009321,
-            "heading": 0.2273230665381978,
-            "angularVelocity": 2.1784973753126392e-7,
-            "velocityX": -3.5016502001914627,
-            "velocityY": 1.4314623609958343,
-            "timestamp": 1.0596433699378704
-        },
-        {
-            "x": 4.267621124217833,
-            "y": 1.9379653687834795,
-            "heading": 0.2273230842953322,
-            "angularVelocity": 2.1784947052459537e-7,
-            "velocityX": -3.5016496598471556,
-            "velocityY": 1.4314636827879597,
-            "timestamp": 1.1411543983946297
-        },
-        {
-            "x": 3.9821980933555543,
-            "y": 2.054645529443536,
-            "heading": 0.22732310205245027,
-            "angularVelocity": 2.1784927045604472e-7,
-            "velocityX": -3.5016492401846184,
-            "velocityY": 1.4314647093669521,
-            "timestamp": 1.222665426851389
-        },
-        {
-            "x": 3.6967750938784922,
-            "y": 2.171325766878081,
-            "heading": 0.22732311980956,
-            "angularVelocity": 2.1784916783717796e-7,
-            "velocityX": -3.501648855142053,
-            "velocityY": 1.4314656512577588,
-            "timestamp": 1.3041764553081483
-        },
-        {
-            "x": 3.411352191599502,
-            "y": 2.288006242078097,
-            "heading": 0.2273231375666713,
-            "angularVelocity": 2.178491871140625e-7,
-            "velocityX": -3.501647662689034,
-            "velocityY": 1.4314685682307748,
-            "timestamp": 1.3856874837649076
-        },
-        {
-            "x": 3.135544385824121,
-            "y": 2.425885248705818,
-            "heading": 0.22732315564878777,
-            "angularVelocity": 2.218364413735132e-7,
-            "velocityX": -3.3836869807340535,
-            "velocityY": 1.6915380561155315,
-            "timestamp": 1.4671985122216669
-        },
-        {
-            "x": 2.8888590335845947,
-            "y": 2.6108903884887695,
-            "heading": 0.22732317584620773,
-            "angularVelocity": 2.4778757367040615e-7,
-            "velocityX": -3.026404609412922,
-            "velocityY": 2.2696945834893776,
-            "timestamp": 1.5487095406784261
-        },
-        {
-            "x": 2.714424659127437,
-            "y": 2.7861164379371415,
-            "heading": 0.22732319539367835,
-            "angularVelocity": 2.990800632496739e-7,
-            "velocityX": -2.668879509458687,
-            "velocityY": 2.6809922892290525,
-            "timestamp": 1.6140681954393303
-        },
-        {
-            "x": 2.566977650329029,
-            "y": 2.984587845024715,
-            "heading": 0.2273232142228265,
-            "angularVelocity": 2.880895914276183e-7,
-            "velocityX": -2.2559676195571057,
-            "velocityY": 3.0366507360596735,
-            "timestamp": 1.6794268502002345
-        },
-        {
-            "x": 2.4495779823740462,
-            "y": 3.20218572489824,
-            "heading": 0.22732323403642601,
-            "angularVelocity": 3.0315188694924184e-7,
-            "velocityX": -1.7962375202558922,
-            "velocityY": 3.3292894517113587,
-            "timestamp": 1.7447855049611387
-        },
-        {
-            "x": 2.3646621591221844,
-            "y": 3.4343941523127826,
-            "heading": 0.22732276450933128,
-            "angularVelocity": -0.000007183855179950579,
-            "velocityX": -1.2992284428512828,
-            "velocityY": 3.5528336417572115,
-            "timestamp": 1.810144159722043
-        },
-        {
-            "x": 2.3096843159184894,
-            "y": 3.656196634862642,
-            "heading": 0.16843360905648927,
-            "angularVelocity": -0.9010154151467895,
-            "velocityX": -0.8411715847703609,
-            "velocityY": 3.3936206820850954,
-            "timestamp": 1.875502814482947
-        },
-        {
-            "x": 2.277656929328602,
-            "y": 3.853541817964372,
-            "heading": 0.0672675417750508,
-            "angularVelocity": -1.547860304829062,
-            "velocityX": -0.49002518039951914,
-            "velocityY": 3.0194192922676324,
-            "timestamp": 1.9408614692438513
-        },
-        {
-            "x": 2.265357919525011,
-            "y": 4.021569174077037,
-            "heading": -0.049806740829473055,
-            "angularVelocity": -1.791259061753831,
-            "velocityX": -0.188177217670429,
-            "velocityY": 2.570850895376378,
-            "timestamp": 2.0062201240047557
-        },
-        {
-            "x": 2.269649055536998,
-            "y": 4.158080976089645,
-            "heading": -0.1685192187929243,
-            "angularVelocity": -1.8163237661135343,
-            "velocityX": 0.06565520706762865,
-            "velocityY": 2.088656850603768,
-            "timestamp": 2.07157877876566
-        },
-        {
-            "x": 2.2810300353091644,
-            "y": 4.259951613943982,
-            "heading": -0.2719091294332062,
-            "angularVelocity": -1.581885536330179,
-            "velocityX": 0.1741311814602323,
-            "velocityY": 1.5586403702310236,
-            "timestamp": 2.1369374335265645
-        },
-        {
-            "x": 2.2926691286250658,
-            "y": 4.326989538481201,
-            "heading": -0.34849979264864867,
-            "angularVelocity": -1.1718518916219969,
-            "velocityX": 0.1780803683686499,
-            "velocityY": 1.0256931508529106,
-            "timestamp": 2.202296088287469
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.6349611616573703,
-            "velocityX": 0.11216374329844404,
-            "velocityY": 0.5050664160631704,
-            "timestamp": 2.2676547430483733
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -1.761626207104486e-30,
-            "velocityX": 0,
-            "velocityY": -1.839087679439853e-31,
-            "timestamp": 2.3330133978092777
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W3.traj
deleted file mode 100644
index 62c8640c..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W3.traj	
+++ /dev/null
@@ -1,292 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -1.5069359770438582e-30,
-            "velocityX": 5.69693995403372e-32,
-            "velocityY": 3.042278119963715e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 7.627290492953592,
-            "y": 0.8035142159340071,
-            "heading": 0.015175150751654183,
-            "angularVelocity": 0.18617297608634506,
-            "velocityX": -0.6466549133823012,
-            "velocityY": 0.20260100070713255,
-            "timestamp": 0.08151102845675925
-        },
-        {
-            "x": 7.521871327482097,
-            "y": 0.8365426259859824,
-            "heading": 0.045515680376982795,
-            "angularVelocity": 0.3722260680519286,
-            "velocityX": -1.2933116839203953,
-            "velocityY": 0.4052017337690261,
-            "timestamp": 0.1630220569135185
-        },
-        {
-            "x": 7.363742244741542,
-            "y": 0.8860852747673063,
-            "heading": 0.09100430929463568,
-            "angularVelocity": 0.5580671693007231,
-            "velocityX": -1.9399716300284648,
-            "velocityY": 0.6078030141357551,
-            "timestamp": 0.24453308537027776
-        },
-        {
-            "x": 7.152902908699907,
-            "y": 0.95214234591484,
-            "heading": 0.15161661460632753,
-            "angularVelocity": 0.7436086436309897,
-            "velocityX": -2.5866356986709165,
-            "velocityY": 0.8104065474106494,
-            "timestamp": 0.326044113827037
-        },
-        {
-            "x": 6.889352971860368,
-            "y": 1.0347142415767305,
-            "heading": 0.22732297101557483,
-            "angularVelocity": 0.9287866665726976,
-            "velocityX": -3.233304030501219,
-            "velocityY": 1.0130150143510155,
-            "timestamp": 0.4075551422837963
-        },
-        {
-            "x": 6.595109187456524,
-            "y": 1.1269159867790934,
-            "heading": 0.22732298101704992,
-            "angularVelocity": 1.22700882310899e-7,
-            "velocityX": -3.6098647014365244,
-            "velocityY": 1.1311566906713055,
-            "timestamp": 0.4890661707405556
-        },
-        {
-            "x": 6.300865420236581,
-            "y": 1.219117786820698,
-            "heading": 0.2273229910184091,
-            "angularVelocity": 1.2269945998464997e-7,
-            "velocityX": -3.6098644906196586,
-            "velocityY": 1.131157363454394,
-            "timestamp": 0.5705771991973149
-        },
-        {
-            "x": 6.006621672014901,
-            "y": 1.3113196474914972,
-            "heading": 0.2273230010197575,
-            "angularVelocity": 1.2269932759504928e-7,
-            "velocityX": -3.6098642575436695,
-            "velocityY": 1.1311581072702415,
-            "timestamp": 0.6520882276540741
-        },
-        {
-            "x": 5.7123779465456614,
-            "y": 1.4035215807721424,
-            "heading": 0.22732301102110228,
-            "angularVelocity": 1.2269928353992546e-7,
-            "velocityX": -3.6098639784103828,
-            "velocityY": 1.131158998068056,
-            "timestamp": 0.7335992561108334
-        },
-        {
-            "x": 5.418134249112936,
-            "y": 1.4957236035255936,
-            "heading": 0.2273230210224565,
-            "angularVelocity": 1.2269939906393907e-7,
-            "velocityX": -3.60986363445063,
-            "velocityY": 1.1311600957453605,
-            "timestamp": 0.8151102845675927
-        },
-        {
-            "x": 5.123890658648026,
-            "y": 1.5879259676440571,
-            "heading": 0.22732303102385681,
-            "angularVelocity": 1.2269996504792678e-7,
-            "velocityX": -3.609862322139673,
-            "velocityY": 1.1311642837064038,
-            "timestamp": 0.896621313024352
-        },
-        {
-            "x": 4.838467298400658,
-            "y": 1.7046053225590203,
-            "heading": 0.22732304878104165,
-            "angularVelocity": 2.1785008908970575e-7,
-            "velocityX": -3.5016532811726435,
-            "velocityY": 1.4314548242617389,
-            "timestamp": 0.9781323414811113
-        },
-        {
-            "x": 4.5530441892872355,
-            "y": 1.8212852918009321,
-            "heading": 0.2273230665381978,
-            "angularVelocity": 2.1784973753126392e-7,
-            "velocityX": -3.5016502001914627,
-            "velocityY": 1.4314623609958343,
-            "timestamp": 1.0596433699378704
-        },
-        {
-            "x": 4.267621124217833,
-            "y": 1.9379653687834795,
-            "heading": 0.2273230842953322,
-            "angularVelocity": 2.1784947052459537e-7,
-            "velocityX": -3.5016496598471556,
-            "velocityY": 1.4314636827879597,
-            "timestamp": 1.1411543983946297
-        },
-        {
-            "x": 3.9821980933555543,
-            "y": 2.054645529443536,
-            "heading": 0.22732310205245027,
-            "angularVelocity": 2.1784927045604472e-7,
-            "velocityX": -3.5016492401846184,
-            "velocityY": 1.4314647093669521,
-            "timestamp": 1.222665426851389
-        },
-        {
-            "x": 3.6967750938784922,
-            "y": 2.171325766878081,
-            "heading": 0.22732311980956,
-            "angularVelocity": 2.1784916783717796e-7,
-            "velocityX": -3.501648855142053,
-            "velocityY": 1.4314656512577588,
-            "timestamp": 1.3041764553081483
-        },
-        {
-            "x": 3.411352191599502,
-            "y": 2.288006242078097,
-            "heading": 0.2273231375666713,
-            "angularVelocity": 2.178491871140625e-7,
-            "velocityX": -3.501647662689034,
-            "velocityY": 1.4314685682307748,
-            "timestamp": 1.3856874837649076
-        },
-        {
-            "x": 3.135544385824121,
-            "y": 2.425885248705818,
-            "heading": 0.22732315564878777,
-            "angularVelocity": 2.218364413735132e-7,
-            "velocityX": -3.3836869807340535,
-            "velocityY": 1.6915380561155315,
-            "timestamp": 1.4671985122216669
-        },
-        {
-            "x": 2.8888590335845947,
-            "y": 2.6108903884887695,
-            "heading": 0.22732317584620773,
-            "angularVelocity": 2.4778757367040615e-7,
-            "velocityX": -3.026404609412922,
-            "velocityY": 2.2696945834893776,
-            "timestamp": 1.5487095406784261
-        },
-        {
-            "x": 2.714424659127437,
-            "y": 2.7861164379371415,
-            "heading": 0.22732319539367835,
-            "angularVelocity": 2.990800632496739e-7,
-            "velocityX": -2.668879509458687,
-            "velocityY": 2.6809922892290525,
-            "timestamp": 1.6140681954393303
-        },
-        {
-            "x": 2.566977650329029,
-            "y": 2.984587845024715,
-            "heading": 0.2273232142228265,
-            "angularVelocity": 2.880895914276183e-7,
-            "velocityX": -2.2559676195571057,
-            "velocityY": 3.0366507360596735,
-            "timestamp": 1.6794268502002345
-        },
-        {
-            "x": 2.4495779823740462,
-            "y": 3.20218572489824,
-            "heading": 0.22732323403642601,
-            "angularVelocity": 3.0315188694924184e-7,
-            "velocityX": -1.7962375202558922,
-            "velocityY": 3.3292894517113587,
-            "timestamp": 1.7447855049611387
-        },
-        {
-            "x": 2.3646621591221844,
-            "y": 3.4343941523127826,
-            "heading": 0.22732276450933128,
-            "angularVelocity": -0.000007183855179950579,
-            "velocityX": -1.2992284428512828,
-            "velocityY": 3.5528336417572115,
-            "timestamp": 1.810144159722043
-        },
-        {
-            "x": 2.3096843159184894,
-            "y": 3.656196634862642,
-            "heading": 0.16843360905648927,
-            "angularVelocity": -0.9010154151467895,
-            "velocityX": -0.8411715847703609,
-            "velocityY": 3.3936206820850954,
-            "timestamp": 1.875502814482947
-        },
-        {
-            "x": 2.277656929328602,
-            "y": 3.853541817964372,
-            "heading": 0.0672675417750508,
-            "angularVelocity": -1.547860304829062,
-            "velocityX": -0.49002518039951914,
-            "velocityY": 3.0194192922676324,
-            "timestamp": 1.9408614692438513
-        },
-        {
-            "x": 2.265357919525011,
-            "y": 4.021569174077037,
-            "heading": -0.049806740829473055,
-            "angularVelocity": -1.791259061753831,
-            "velocityX": -0.188177217670429,
-            "velocityY": 2.570850895376378,
-            "timestamp": 2.0062201240047557
-        },
-        {
-            "x": 2.269649055536998,
-            "y": 4.158080976089645,
-            "heading": -0.1685192187929243,
-            "angularVelocity": -1.8163237661135343,
-            "velocityX": 0.06565520706762865,
-            "velocityY": 2.088656850603768,
-            "timestamp": 2.07157877876566
-        },
-        {
-            "x": 2.2810300353091644,
-            "y": 4.259951613943982,
-            "heading": -0.2719091294332062,
-            "angularVelocity": -1.581885536330179,
-            "velocityX": 0.1741311814602323,
-            "velocityY": 1.5586403702310236,
-            "timestamp": 2.1369374335265645
-        },
-        {
-            "x": 2.2926691286250658,
-            "y": 4.326989538481201,
-            "heading": -0.34849979264864867,
-            "angularVelocity": -1.1718518916219969,
-            "velocityX": 0.1780803683686499,
-            "velocityY": 1.0256931508529106,
-            "timestamp": 2.202296088287469
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.6349611616573703,
-            "velocityX": 0.11216374329844404,
-            "velocityY": 0.5050664160631704,
-            "timestamp": 2.2676547430483733
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -1.761626207104486e-30,
-            "velocityX": 0,
-            "velocityY": -1.839087679439853e-31,
-            "timestamp": 2.3330133978092777
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.1.traj
deleted file mode 100644
index 1818c169..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.1.traj	
+++ /dev/null
@@ -1,265 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -1.4340376968599448e-32,
-            "velocityX": -4.9971810322519555e-29,
-            "velocityY": 8.414879145245036e-29,
-            "timestamp": 0
-        },
-        {
-            "x": 1.3816327251489127,
-            "y": 5.576194684745321,
-            "heading": 4.506697153639657e-18,
-            "angularVelocity": 6.903205799499945e-17,
-            "velocityX": 0.4845393494564494,
-            "velocityY": 0.24806468536018447,
-            "timestamp": 0.06528412023584423
-        },
-        {
-            "x": 1.4448981743168023,
-            "y": 5.608584053657482,
-            "heading": 9.944941296273304e-18,
-            "angularVelocity": 8.330117912909241e-17,
-            "velocityX": 0.9690786816049286,
-            "velocityY": 0.4961293618593833,
-            "timestamp": 0.13056824047168847
-        },
-        {
-            "x": 1.5397963458087682,
-            "y": 5.657168105868761,
-            "heading": 6.849951425963079e-18,
-            "angularVelocity": -4.74080026246192e-17,
-            "velocityX": 1.4536179877914959,
-            "velocityY": 0.7441940250671262,
-            "timestamp": 0.1958523607075327
-        },
-        {
-            "x": 1.666327236799984,
-            "y": 5.721946839932961,
-            "heading": -2.8668788112285783e-18,
-            "angularVelocity": -1.4883910439092565e-16,
-            "velocityX": 1.9381572507083278,
-            "velocityY": 0.9922586661225022,
-            "timestamp": 0.26113648094337694
-        },
-        {
-            "x": 1.8244908416408254,
-            "y": 5.802920252957701,
-            "heading": -1.6578007712929266e-17,
-            "angularVelocity": -2.1002241242045617e-16,
-            "velocityX": 2.4226964270861338,
-            "velocityY": 1.2403232628733727,
-            "timestamp": 0.32642060117922117
-        },
-        {
-            "x": 2.0142871433826555,
-            "y": 5.900088336265959,
-            "heading": -2.5925213294324244e-17,
-            "angularVelocity": -1.4317730798527664e-16,
-            "velocityX": 2.9072353438504677,
-            "velocityY": 1.4883877267125722,
-            "timestamp": 0.3917047214150654
-        },
-        {
-            "x": 2.234118517125294,
-            "y": 6.012633170077576,
-            "heading": -1.2540245641654488e-17,
-            "angularVelocity": 2.0502639235211304e-16,
-            "velocityX": 3.3673023845382315,
-            "velocityY": 1.723923572915424,
-            "timestamp": 0.45698884165090964
-        },
-        {
-            "x": 2.4539498908690707,
-            "y": 6.125178003888657,
-            "heading": 1.1802241639861868e-17,
-            "angularVelocity": 3.728699597195476e-16,
-            "velocityX": 3.3673023845556527,
-            "velocityY": 1.72392357290723,
-            "timestamp": 0.5222729618867539
-        },
-        {
-            "x": 2.6737813045614214,
-            "y": 6.237722759669026,
-            "heading": 3.649409202314283e-17,
-            "angularVelocity": 3.78221385080107e-16,
-            "velocityX": 3.367302996474358,
-            "velocityY": 1.7239223776592436,
-            "timestamp": 0.5875570821225982
-        },
-        {
-            "x": 2.896845817565918,
-            "y": 6.343715190887451,
-            "heading": 3.1275322529875387e-17,
-            "angularVelocity": -7.99393399896351e-17,
-            "velocityX": 3.416826514605052,
-            "velocityY": 1.6235560935112305,
-            "timestamp": 0.6528412023584425
-        },
-        {
-            "x": 3.2165948847884085,
-            "y": 6.4256885407325814,
-            "heading": 6.106570262583634e-20,
-            "angularVelocity": -3.577262143903813e-16,
-            "velocityX": 3.664435260812148,
-            "velocityY": 0.9394430333407913,
-            "timestamp": 0.7400985952347529
-        },
-        {
-            "x": 3.5380520485012785,
-            "y": 6.500684460821759,
-            "heading": -4.2865718547534536e-17,
-            "angularVelocity": -4.919558421997663e-16,
-            "velocityX": 3.6840106392881062,
-            "velocityY": 0.8594792672236512,
-            "timestamp": 0.8273559881110633
-        },
-        {
-            "x": 3.8595092199655934,
-            "y": 6.5756803476856875,
-            "heading": -3.198465678230611e-17,
-            "angularVelocity": 1.2470074347342177e-16,
-            "velocityX": 3.684010728122356,
-            "velocityY": 0.8594788864507628,
-            "timestamp": 0.9146133809873737
-        },
-        {
-            "x": 4.180966391429945,
-            "y": 6.6506762345494606,
-            "heading": 1.830329484329921e-18,
-            "angularVelocity": 3.8753147618851267e-16,
-            "velocityX": 3.6840107281227708,
-            "velocityY": 0.8594788864489847,
-            "timestamp": 1.001870773863684
-        },
-        {
-            "x": 4.502423562894296,
-            "y": 6.725672121413234,
-            "heading": 1.519427405027002e-17,
-            "angularVelocity": 1.531554419106218e-16,
-            "velocityX": 3.684010728122771,
-            "velocityY": 0.8594788864489847,
-            "timestamp": 1.0891281667399946
-        },
-        {
-            "x": 4.823880734358647,
-            "y": 6.800668008277006,
-            "heading": 2.800360653678013e-17,
-            "angularVelocity": 1.4679939507274274e-16,
-            "velocityX": 3.6840107281227708,
-            "velocityY": 0.8594788864489846,
-            "timestamp": 1.176385559616305
-        },
-        {
-            "x": 5.1453379058229975,
-            "y": 6.875663895140779,
-            "heading": -9.132380596349043e-18,
-            "angularVelocity": -4.2559129844470954e-16,
-            "velocityX": 3.6840107281227708,
-            "velocityY": 0.8594788864489846,
-            "timestamp": 1.2636429524926156
-        },
-        {
-            "x": 5.466795077287348,
-            "y": 6.950659782004552,
-            "heading": 1.9053427809876355e-18,
-            "angularVelocity": 1.2649613980635305e-16,
-            "velocityX": 3.6840107281227708,
-            "velocityY": 0.8594788864489846,
-            "timestamp": 1.3509003453689261
-        },
-        {
-            "x": 5.788252248751699,
-            "y": 7.025655668868324,
-            "heading": -3.1119644574102345e-18,
-            "angularVelocity": -5.750008191867264e-17,
-            "velocityX": 3.684010728122771,
-            "velocityY": 0.8594788864489846,
-            "timestamp": 1.4381577382452366
-        },
-        {
-            "x": 6.10970942021605,
-            "y": 7.100651555732097,
-            "heading": 3.013239534330982e-17,
-            "angularVelocity": 3.80991898964227e-16,
-            "velocityX": 3.6840107281227708,
-            "velocityY": 0.8594788864489846,
-            "timestamp": 1.5254151311215471
-        },
-        {
-            "x": 6.431166591680401,
-            "y": 7.17564744259587,
-            "heading": 3.192919981892433e-17,
-            "angularVelocity": 2.0592002767735807e-17,
-            "velocityX": 3.6840107281227708,
-            "velocityY": 0.8594788864489847,
-            "timestamp": 1.6126725239978577
-        },
-        {
-            "x": 6.752623763144745,
-            "y": 7.250643329459641,
-            "heading": 5.4408724078031107e-17,
-            "angularVelocity": 2.576231481446227e-16,
-            "velocityX": 3.6840107281226917,
-            "velocityY": 0.8594788864489661,
-            "timestamp": 1.6999299168741682
-        },
-        {
-            "x": 7.061749152944237,
-            "y": 7.322762214393876,
-            "heading": 4.954044927264927e-17,
-            "angularVelocity": -5.579212125566035e-17,
-            "velocityX": 3.5426842312110383,
-            "velocityY": 0.8265074460391663,
-            "timestamp": 1.7871873097504787
-        },
-        {
-            "x": 7.309049484595465,
-            "y": 7.380457326963315,
-            "heading": 3.4625344335160436e-17,
-            "angularVelocity": -1.7093228018551423e-16,
-            "velocityX": 2.834147612017054,
-            "velocityY": 0.6612060098016332,
-            "timestamp": 1.8744447026267892
-        },
-        {
-            "x": 7.49452473986686,
-            "y": 7.4237286629145345,
-            "heading": 2.1983691983745354e-17,
-            "angularVelocity": -1.448777241544276e-16,
-            "velocityX": 2.125610783882924,
-            "velocityY": 0.49590452481841046,
-            "timestamp": 1.9617020955030997
-        },
-        {
-            "x": 7.618174912681235,
-            "y": 7.452576220829731,
-            "heading": 1.0249539478821592e-17,
-            "angularVelocity": -1.3447746039961902e-16,
-            "velocityX": 1.4170738861021466,
-            "velocityY": 0.3306030235866404,
-            "timestamp": 2.04895948837941
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": -1.174632794233726e-16,
-            "velocityX": 0.7085369534980623,
-            "velocityY": 0.16530151423060047,
-            "timestamp": 2.1362168812557205
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -1.0281234266250986e-33,
-            "velocityY": 0,
-            "timestamp": 2.223474274132031
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.traj
deleted file mode 100644
index 1818c169..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.traj	
+++ /dev/null
@@ -1,265 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -1.4340376968599448e-32,
-            "velocityX": -4.9971810322519555e-29,
-            "velocityY": 8.414879145245036e-29,
-            "timestamp": 0
-        },
-        {
-            "x": 1.3816327251489127,
-            "y": 5.576194684745321,
-            "heading": 4.506697153639657e-18,
-            "angularVelocity": 6.903205799499945e-17,
-            "velocityX": 0.4845393494564494,
-            "velocityY": 0.24806468536018447,
-            "timestamp": 0.06528412023584423
-        },
-        {
-            "x": 1.4448981743168023,
-            "y": 5.608584053657482,
-            "heading": 9.944941296273304e-18,
-            "angularVelocity": 8.330117912909241e-17,
-            "velocityX": 0.9690786816049286,
-            "velocityY": 0.4961293618593833,
-            "timestamp": 0.13056824047168847
-        },
-        {
-            "x": 1.5397963458087682,
-            "y": 5.657168105868761,
-            "heading": 6.849951425963079e-18,
-            "angularVelocity": -4.74080026246192e-17,
-            "velocityX": 1.4536179877914959,
-            "velocityY": 0.7441940250671262,
-            "timestamp": 0.1958523607075327
-        },
-        {
-            "x": 1.666327236799984,
-            "y": 5.721946839932961,
-            "heading": -2.8668788112285783e-18,
-            "angularVelocity": -1.4883910439092565e-16,
-            "velocityX": 1.9381572507083278,
-            "velocityY": 0.9922586661225022,
-            "timestamp": 0.26113648094337694
-        },
-        {
-            "x": 1.8244908416408254,
-            "y": 5.802920252957701,
-            "heading": -1.6578007712929266e-17,
-            "angularVelocity": -2.1002241242045617e-16,
-            "velocityX": 2.4226964270861338,
-            "velocityY": 1.2403232628733727,
-            "timestamp": 0.32642060117922117
-        },
-        {
-            "x": 2.0142871433826555,
-            "y": 5.900088336265959,
-            "heading": -2.5925213294324244e-17,
-            "angularVelocity": -1.4317730798527664e-16,
-            "velocityX": 2.9072353438504677,
-            "velocityY": 1.4883877267125722,
-            "timestamp": 0.3917047214150654
-        },
-        {
-            "x": 2.234118517125294,
-            "y": 6.012633170077576,
-            "heading": -1.2540245641654488e-17,
-            "angularVelocity": 2.0502639235211304e-16,
-            "velocityX": 3.3673023845382315,
-            "velocityY": 1.723923572915424,
-            "timestamp": 0.45698884165090964
-        },
-        {
-            "x": 2.4539498908690707,
-            "y": 6.125178003888657,
-            "heading": 1.1802241639861868e-17,
-            "angularVelocity": 3.728699597195476e-16,
-            "velocityX": 3.3673023845556527,
-            "velocityY": 1.72392357290723,
-            "timestamp": 0.5222729618867539
-        },
-        {
-            "x": 2.6737813045614214,
-            "y": 6.237722759669026,
-            "heading": 3.649409202314283e-17,
-            "angularVelocity": 3.78221385080107e-16,
-            "velocityX": 3.367302996474358,
-            "velocityY": 1.7239223776592436,
-            "timestamp": 0.5875570821225982
-        },
-        {
-            "x": 2.896845817565918,
-            "y": 6.343715190887451,
-            "heading": 3.1275322529875387e-17,
-            "angularVelocity": -7.99393399896351e-17,
-            "velocityX": 3.416826514605052,
-            "velocityY": 1.6235560935112305,
-            "timestamp": 0.6528412023584425
-        },
-        {
-            "x": 3.2165948847884085,
-            "y": 6.4256885407325814,
-            "heading": 6.106570262583634e-20,
-            "angularVelocity": -3.577262143903813e-16,
-            "velocityX": 3.664435260812148,
-            "velocityY": 0.9394430333407913,
-            "timestamp": 0.7400985952347529
-        },
-        {
-            "x": 3.5380520485012785,
-            "y": 6.500684460821759,
-            "heading": -4.2865718547534536e-17,
-            "angularVelocity": -4.919558421997663e-16,
-            "velocityX": 3.6840106392881062,
-            "velocityY": 0.8594792672236512,
-            "timestamp": 0.8273559881110633
-        },
-        {
-            "x": 3.8595092199655934,
-            "y": 6.5756803476856875,
-            "heading": -3.198465678230611e-17,
-            "angularVelocity": 1.2470074347342177e-16,
-            "velocityX": 3.684010728122356,
-            "velocityY": 0.8594788864507628,
-            "timestamp": 0.9146133809873737
-        },
-        {
-            "x": 4.180966391429945,
-            "y": 6.6506762345494606,
-            "heading": 1.830329484329921e-18,
-            "angularVelocity": 3.8753147618851267e-16,
-            "velocityX": 3.6840107281227708,
-            "velocityY": 0.8594788864489847,
-            "timestamp": 1.001870773863684
-        },
-        {
-            "x": 4.502423562894296,
-            "y": 6.725672121413234,
-            "heading": 1.519427405027002e-17,
-            "angularVelocity": 1.531554419106218e-16,
-            "velocityX": 3.684010728122771,
-            "velocityY": 0.8594788864489847,
-            "timestamp": 1.0891281667399946
-        },
-        {
-            "x": 4.823880734358647,
-            "y": 6.800668008277006,
-            "heading": 2.800360653678013e-17,
-            "angularVelocity": 1.4679939507274274e-16,
-            "velocityX": 3.6840107281227708,
-            "velocityY": 0.8594788864489846,
-            "timestamp": 1.176385559616305
-        },
-        {
-            "x": 5.1453379058229975,
-            "y": 6.875663895140779,
-            "heading": -9.132380596349043e-18,
-            "angularVelocity": -4.2559129844470954e-16,
-            "velocityX": 3.6840107281227708,
-            "velocityY": 0.8594788864489846,
-            "timestamp": 1.2636429524926156
-        },
-        {
-            "x": 5.466795077287348,
-            "y": 6.950659782004552,
-            "heading": 1.9053427809876355e-18,
-            "angularVelocity": 1.2649613980635305e-16,
-            "velocityX": 3.6840107281227708,
-            "velocityY": 0.8594788864489846,
-            "timestamp": 1.3509003453689261
-        },
-        {
-            "x": 5.788252248751699,
-            "y": 7.025655668868324,
-            "heading": -3.1119644574102345e-18,
-            "angularVelocity": -5.750008191867264e-17,
-            "velocityX": 3.684010728122771,
-            "velocityY": 0.8594788864489846,
-            "timestamp": 1.4381577382452366
-        },
-        {
-            "x": 6.10970942021605,
-            "y": 7.100651555732097,
-            "heading": 3.013239534330982e-17,
-            "angularVelocity": 3.80991898964227e-16,
-            "velocityX": 3.6840107281227708,
-            "velocityY": 0.8594788864489846,
-            "timestamp": 1.5254151311215471
-        },
-        {
-            "x": 6.431166591680401,
-            "y": 7.17564744259587,
-            "heading": 3.192919981892433e-17,
-            "angularVelocity": 2.0592002767735807e-17,
-            "velocityX": 3.6840107281227708,
-            "velocityY": 0.8594788864489847,
-            "timestamp": 1.6126725239978577
-        },
-        {
-            "x": 6.752623763144745,
-            "y": 7.250643329459641,
-            "heading": 5.4408724078031107e-17,
-            "angularVelocity": 2.576231481446227e-16,
-            "velocityX": 3.6840107281226917,
-            "velocityY": 0.8594788864489661,
-            "timestamp": 1.6999299168741682
-        },
-        {
-            "x": 7.061749152944237,
-            "y": 7.322762214393876,
-            "heading": 4.954044927264927e-17,
-            "angularVelocity": -5.579212125566035e-17,
-            "velocityX": 3.5426842312110383,
-            "velocityY": 0.8265074460391663,
-            "timestamp": 1.7871873097504787
-        },
-        {
-            "x": 7.309049484595465,
-            "y": 7.380457326963315,
-            "heading": 3.4625344335160436e-17,
-            "angularVelocity": -1.7093228018551423e-16,
-            "velocityX": 2.834147612017054,
-            "velocityY": 0.6612060098016332,
-            "timestamp": 1.8744447026267892
-        },
-        {
-            "x": 7.49452473986686,
-            "y": 7.4237286629145345,
-            "heading": 2.1983691983745354e-17,
-            "angularVelocity": -1.448777241544276e-16,
-            "velocityX": 2.125610783882924,
-            "velocityY": 0.49590452481841046,
-            "timestamp": 1.9617020955030997
-        },
-        {
-            "x": 7.618174912681235,
-            "y": 7.452576220829731,
-            "heading": 1.0249539478821592e-17,
-            "angularVelocity": -1.3447746039961902e-16,
-            "velocityX": 1.4170738861021466,
-            "velocityY": 0.3306030235866404,
-            "timestamp": 2.04895948837941
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": -1.174632794233726e-16,
-            "velocityX": 0.7085369534980623,
-            "velocityY": 0.16530151423060047,
-            "timestamp": 2.1362168812557205
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -1.0281234266250986e-33,
-            "velocityY": 0,
-            "timestamp": 2.223474274132031
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.1.traj
deleted file mode 100644
index 51c46220..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.1.traj	
+++ /dev/null
@@ -1,265 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -1.3272791352480801e-30,
-            "velocityX": -6.138871086325045e-33,
-            "velocityY": -8.954734510196666e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 1.3835743361511121,
-            "y": 5.575814713247826,
-            "heading": -0.005199936040550204,
-            "angularVelocity": -0.07791207752010428,
-            "velocityX": 0.5030535492153302,
-            "velocityY": 0.23695621540617479,
-            "timestamp": 0.06674107796969504
-        },
-        {
-            "x": 1.4507230090250494,
-            "y": 5.607444140582854,
-            "heading": -0.015599510401707435,
-            "angularVelocity": -0.15581969422009245,
-            "velocityX": 1.0061071069967922,
-            "velocityY": 0.4739124433888902,
-            "timestamp": 0.13348215593939008
-        },
-        {
-            "x": 1.5514460199136038,
-            "y": 5.654888284136176,
-            "heading": -0.03119820909467289,
-            "angularVelocity": -0.23371960968398373,
-            "velocityX": 1.5091606841335288,
-            "velocityY": 0.7108687033024069,
-            "timestamp": 0.20022323390908514
-        },
-        {
-            "x": 1.6857433700165965,
-            "y": 5.718147147394209,
-            "heading": -0.051995311102797384,
-            "angularVelocity": -0.3116087219563307,
-            "velocityX": 2.0122142792475226,
-            "velocityY": 0.9478250154538677,
-            "timestamp": 0.26696431187878017
-        },
-        {
-            "x": 1.853615058416753,
-            "y": 5.79722073482494,
-            "heading": -0.07798989994858517,
-            "angularVelocity": -0.3894841023932979,
-            "velocityX": 2.515267860617741,
-            "velocityY": 1.184781394550373,
-            "timestamp": 0.3337053898484752
-        },
-        {
-            "x": 2.0550610733526136,
-            "y": 5.892109048523229,
-            "heading": -0.10918084380142022,
-            "angularVelocity": -0.4673425242996203,
-            "velocityX": 3.0183212657627503,
-            "velocityY": 1.4217378050347718,
-            "timestamp": 0.4004464678181702
-        },
-        {
-            "x": 2.283467758752316,
-            "y": 5.999697539336407,
-            "heading": -0.1091808488901602,
-            "angularVelocity": -7.624599630551263e-8,
-            "velocityX": 3.422280435797193,
-            "velocityY": 1.6120280655645223,
-            "timestamp": 0.46718754578786525
-        },
-        {
-            "x": 2.511874447545338,
-            "y": 6.107286022946165,
-            "heading": -0.10918085397868219,
-            "angularVelocity": -7.624272995870229e-8,
-            "velocityX": 3.4222804866402323,
-            "velocityY": 1.6120279576336907,
-            "timestamp": 0.5339286237575603
-        },
-        {
-            "x": 2.7402812851669234,
-            "y": 6.214874190597382,
-            "heading": -0.10918085906719958,
-            "angularVelocity": -7.624266119750657e-8,
-            "velocityX": 3.4222827165797036,
-            "velocityY": 1.612023223539615,
-            "timestamp": 0.6006697017272554
-        },
-        {
-            "x": 2.976557731628418,
-            "y": 6.303859233856201,
-            "heading": -0.10918086426900071,
-            "angularVelocity": -7.794002274324629e-8,
-            "velocityX": 3.5401952388119984,
-            "velocityY": 1.3332874740084952,
-            "timestamp": 0.6674107796969505
-        },
-        {
-            "x": 3.298504794987031,
-            "y": 6.35898613680169,
-            "heading": -0.1091808690742104,
-            "angularVelocity": -5.565219068812679e-8,
-            "velocityX": 3.7286737752062145,
-            "velocityY": 0.6384597367556102,
-            "timestamp": 0.753754365316963
-        },
-        {
-            "x": 3.623164348220099,
-            "y": 6.394833894219333,
-            "heading": -0.10918087380844538,
-            "angularVelocity": -5.483018746401896e-8,
-            "velocityX": 3.760088846227146,
-            "velocityY": 0.4151756863029092,
-            "timestamp": 0.8400979509369756
-        },
-        {
-            "x": 3.94782391548866,
-            "y": 6.430681524522641,
-            "heading": -0.10918087854267373,
-            "angularVelocity": -5.483011064647812e-8,
-            "velocityX": 3.7600890087811236,
-            "velocityY": 0.41517421411094135,
-            "timestamp": 0.9264415365569881
-        },
-        {
-            "x": 4.272483486014202,
-            "y": 6.466529125328635,
-            "heading": -0.10918088327689503,
-            "angularVelocity": -5.483002882633892e-8,
-            "velocityX": 3.7600890465022867,
-            "velocityY": 0.4151738724837589,
-            "timestamp": 1.0127851221770008
-        },
-        {
-            "x": 4.597143061517746,
-            "y": 6.502376681050562,
-            "heading": -0.10918088801110931,
-            "angularVelocity": -5.482994771911856e-8,
-            "velocityX": 3.760089104155704,
-            "velocityY": 0.4151733503364993,
-            "timestamp": 1.0991287077970133
-        },
-        {
-            "x": 4.921802670469747,
-            "y": 6.538223933839528,
-            "heading": -0.10918089274532473,
-            "angularVelocity": -5.482996090104271e-8,
-            "velocityX": 3.76008949154355,
-            "velocityY": 0.41516984187713296,
-            "timestamp": 1.1854722934170259
-        },
-        {
-            "x": 5.24839709757225,
-            "y": 6.543220677288062,
-            "heading": -0.10918090874246278,
-            "angularVelocity": -1.8527303358226695e-7,
-            "velocityX": 3.782497851546312,
-            "velocityY": 0.05787046498769136,
-            "timestamp": 1.2718158790370384
-        },
-        {
-            "x": 5.573089202193745,
-            "y": 6.507668986743235,
-            "heading": -0.10918094712419395,
-            "angularVelocity": -4.4452324855688095e-7,
-            "velocityX": 3.760465844566904,
-            "velocityY": -0.4117467474802974,
-            "timestamp": 1.358159464657051
-        },
-        {
-            "x": 5.897772644305168,
-            "y": 6.472038270745821,
-            "heading": -0.10918098550925279,
-            "angularVelocity": -4.4456178841484606e-7,
-            "velocityX": 3.76036551852636,
-            "velocityY": -0.4126619915255808,
-            "timestamp": 1.4445030502770635
-        },
-        {
-            "x": 6.218673539970231,
-            "y": 6.411116178321686,
-            "heading": -0.10918101517811797,
-            "angularVelocity": -3.436140042414391e-7,
-            "velocityX": 3.716557441537206,
-            "velocityY": -0.7055775132184731,
-            "timestamp": 1.530846635897076
-        },
-        {
-            "x": 6.522221927429097,
-            "y": 6.290504141989104,
-            "heading": -0.10918101099769996,
-            "angularVelocity": 4.8416080721557094e-8,
-            "velocityX": 3.5155870037033834,
-            "velocityY": -1.3968847305389978,
-            "timestamp": 1.6171902215170886
-        },
-        {
-            "x": 6.822696463094792,
-            "y": 6.162426273800445,
-            "heading": -0.10918100683974102,
-            "angularVelocity": 4.8155967911031915e-8,
-            "velocityX": 3.479986770390183,
-            "velocityY": -1.4833512792984263,
-            "timestamp": 1.7035338071371011
-        },
-        {
-            "x": 7.1084642887127405,
-            "y": 6.040617535278895,
-            "heading": -0.0727866812513063,
-            "angularVelocity": 0.42150584003543307,
-            "velocityX": 3.309659004382518,
-            "velocityY": -1.4107444999749623,
-            "timestamp": 1.7898773927571137
-        },
-        {
-            "x": 7.337078567452098,
-            "y": 5.943170529564944,
-            "heading": -0.043671737033755105,
-            "angularVelocity": 0.33719869297161775,
-            "velocityX": 2.6477274148129606,
-            "velocityY": -1.1285957725082563,
-            "timestamp": 1.8762209783771262
-        },
-        {
-            "x": 7.508539282574383,
-            "y": 5.870085268029055,
-            "heading": -0.021835852662427283,
-            "angularVelocity": 0.2528952696894573,
-            "velocityX": 1.985795631384405,
-            "velocityY": -0.8464469133529862,
-            "timestamp": 1.9625645639971387
-        },
-        {
-            "x": 7.622846427592322,
-            "y": 5.821361756830362,
-            "heading": -0.007278657657073996,
-            "angularVelocity": 0.16859613717477176,
-            "velocityX": 1.323863772822564,
-            "velocityY": -0.564297982864862,
-            "timestamp": 2.0489081496171515
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0.08429876527373403,
-            "velocityX": 0.6619318852381739,
-            "velocityY": -0.282149005689615,
-            "timestamp": 2.1352517352371643
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -1.8142028796058936e-31,
-            "velocityY": -1.068352585526189e-31,
-            "timestamp": 2.221595320857177
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.traj
deleted file mode 100644
index 51c46220..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.traj	
+++ /dev/null
@@ -1,265 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -1.3272791352480801e-30,
-            "velocityX": -6.138871086325045e-33,
-            "velocityY": -8.954734510196666e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 1.3835743361511121,
-            "y": 5.575814713247826,
-            "heading": -0.005199936040550204,
-            "angularVelocity": -0.07791207752010428,
-            "velocityX": 0.5030535492153302,
-            "velocityY": 0.23695621540617479,
-            "timestamp": 0.06674107796969504
-        },
-        {
-            "x": 1.4507230090250494,
-            "y": 5.607444140582854,
-            "heading": -0.015599510401707435,
-            "angularVelocity": -0.15581969422009245,
-            "velocityX": 1.0061071069967922,
-            "velocityY": 0.4739124433888902,
-            "timestamp": 0.13348215593939008
-        },
-        {
-            "x": 1.5514460199136038,
-            "y": 5.654888284136176,
-            "heading": -0.03119820909467289,
-            "angularVelocity": -0.23371960968398373,
-            "velocityX": 1.5091606841335288,
-            "velocityY": 0.7108687033024069,
-            "timestamp": 0.20022323390908514
-        },
-        {
-            "x": 1.6857433700165965,
-            "y": 5.718147147394209,
-            "heading": -0.051995311102797384,
-            "angularVelocity": -0.3116087219563307,
-            "velocityX": 2.0122142792475226,
-            "velocityY": 0.9478250154538677,
-            "timestamp": 0.26696431187878017
-        },
-        {
-            "x": 1.853615058416753,
-            "y": 5.79722073482494,
-            "heading": -0.07798989994858517,
-            "angularVelocity": -0.3894841023932979,
-            "velocityX": 2.515267860617741,
-            "velocityY": 1.184781394550373,
-            "timestamp": 0.3337053898484752
-        },
-        {
-            "x": 2.0550610733526136,
-            "y": 5.892109048523229,
-            "heading": -0.10918084380142022,
-            "angularVelocity": -0.4673425242996203,
-            "velocityX": 3.0183212657627503,
-            "velocityY": 1.4217378050347718,
-            "timestamp": 0.4004464678181702
-        },
-        {
-            "x": 2.283467758752316,
-            "y": 5.999697539336407,
-            "heading": -0.1091808488901602,
-            "angularVelocity": -7.624599630551263e-8,
-            "velocityX": 3.422280435797193,
-            "velocityY": 1.6120280655645223,
-            "timestamp": 0.46718754578786525
-        },
-        {
-            "x": 2.511874447545338,
-            "y": 6.107286022946165,
-            "heading": -0.10918085397868219,
-            "angularVelocity": -7.624272995870229e-8,
-            "velocityX": 3.4222804866402323,
-            "velocityY": 1.6120279576336907,
-            "timestamp": 0.5339286237575603
-        },
-        {
-            "x": 2.7402812851669234,
-            "y": 6.214874190597382,
-            "heading": -0.10918085906719958,
-            "angularVelocity": -7.624266119750657e-8,
-            "velocityX": 3.4222827165797036,
-            "velocityY": 1.612023223539615,
-            "timestamp": 0.6006697017272554
-        },
-        {
-            "x": 2.976557731628418,
-            "y": 6.303859233856201,
-            "heading": -0.10918086426900071,
-            "angularVelocity": -7.794002274324629e-8,
-            "velocityX": 3.5401952388119984,
-            "velocityY": 1.3332874740084952,
-            "timestamp": 0.6674107796969505
-        },
-        {
-            "x": 3.298504794987031,
-            "y": 6.35898613680169,
-            "heading": -0.1091808690742104,
-            "angularVelocity": -5.565219068812679e-8,
-            "velocityX": 3.7286737752062145,
-            "velocityY": 0.6384597367556102,
-            "timestamp": 0.753754365316963
-        },
-        {
-            "x": 3.623164348220099,
-            "y": 6.394833894219333,
-            "heading": -0.10918087380844538,
-            "angularVelocity": -5.483018746401896e-8,
-            "velocityX": 3.760088846227146,
-            "velocityY": 0.4151756863029092,
-            "timestamp": 0.8400979509369756
-        },
-        {
-            "x": 3.94782391548866,
-            "y": 6.430681524522641,
-            "heading": -0.10918087854267373,
-            "angularVelocity": -5.483011064647812e-8,
-            "velocityX": 3.7600890087811236,
-            "velocityY": 0.41517421411094135,
-            "timestamp": 0.9264415365569881
-        },
-        {
-            "x": 4.272483486014202,
-            "y": 6.466529125328635,
-            "heading": -0.10918088327689503,
-            "angularVelocity": -5.483002882633892e-8,
-            "velocityX": 3.7600890465022867,
-            "velocityY": 0.4151738724837589,
-            "timestamp": 1.0127851221770008
-        },
-        {
-            "x": 4.597143061517746,
-            "y": 6.502376681050562,
-            "heading": -0.10918088801110931,
-            "angularVelocity": -5.482994771911856e-8,
-            "velocityX": 3.760089104155704,
-            "velocityY": 0.4151733503364993,
-            "timestamp": 1.0991287077970133
-        },
-        {
-            "x": 4.921802670469747,
-            "y": 6.538223933839528,
-            "heading": -0.10918089274532473,
-            "angularVelocity": -5.482996090104271e-8,
-            "velocityX": 3.76008949154355,
-            "velocityY": 0.41516984187713296,
-            "timestamp": 1.1854722934170259
-        },
-        {
-            "x": 5.24839709757225,
-            "y": 6.543220677288062,
-            "heading": -0.10918090874246278,
-            "angularVelocity": -1.8527303358226695e-7,
-            "velocityX": 3.782497851546312,
-            "velocityY": 0.05787046498769136,
-            "timestamp": 1.2718158790370384
-        },
-        {
-            "x": 5.573089202193745,
-            "y": 6.507668986743235,
-            "heading": -0.10918094712419395,
-            "angularVelocity": -4.4452324855688095e-7,
-            "velocityX": 3.760465844566904,
-            "velocityY": -0.4117467474802974,
-            "timestamp": 1.358159464657051
-        },
-        {
-            "x": 5.897772644305168,
-            "y": 6.472038270745821,
-            "heading": -0.10918098550925279,
-            "angularVelocity": -4.4456178841484606e-7,
-            "velocityX": 3.76036551852636,
-            "velocityY": -0.4126619915255808,
-            "timestamp": 1.4445030502770635
-        },
-        {
-            "x": 6.218673539970231,
-            "y": 6.411116178321686,
-            "heading": -0.10918101517811797,
-            "angularVelocity": -3.436140042414391e-7,
-            "velocityX": 3.716557441537206,
-            "velocityY": -0.7055775132184731,
-            "timestamp": 1.530846635897076
-        },
-        {
-            "x": 6.522221927429097,
-            "y": 6.290504141989104,
-            "heading": -0.10918101099769996,
-            "angularVelocity": 4.8416080721557094e-8,
-            "velocityX": 3.5155870037033834,
-            "velocityY": -1.3968847305389978,
-            "timestamp": 1.6171902215170886
-        },
-        {
-            "x": 6.822696463094792,
-            "y": 6.162426273800445,
-            "heading": -0.10918100683974102,
-            "angularVelocity": 4.8155967911031915e-8,
-            "velocityX": 3.479986770390183,
-            "velocityY": -1.4833512792984263,
-            "timestamp": 1.7035338071371011
-        },
-        {
-            "x": 7.1084642887127405,
-            "y": 6.040617535278895,
-            "heading": -0.0727866812513063,
-            "angularVelocity": 0.42150584003543307,
-            "velocityX": 3.309659004382518,
-            "velocityY": -1.4107444999749623,
-            "timestamp": 1.7898773927571137
-        },
-        {
-            "x": 7.337078567452098,
-            "y": 5.943170529564944,
-            "heading": -0.043671737033755105,
-            "angularVelocity": 0.33719869297161775,
-            "velocityX": 2.6477274148129606,
-            "velocityY": -1.1285957725082563,
-            "timestamp": 1.8762209783771262
-        },
-        {
-            "x": 7.508539282574383,
-            "y": 5.870085268029055,
-            "heading": -0.021835852662427283,
-            "angularVelocity": 0.2528952696894573,
-            "velocityX": 1.985795631384405,
-            "velocityY": -0.8464469133529862,
-            "timestamp": 1.9625645639971387
-        },
-        {
-            "x": 7.622846427592322,
-            "y": 5.821361756830362,
-            "heading": -0.007278657657073996,
-            "angularVelocity": 0.16859613717477176,
-            "velocityX": 1.323863772822564,
-            "velocityY": -0.564297982864862,
-            "timestamp": 2.0489081496171515
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0.08429876527373403,
-            "velocityX": 0.6619318852381739,
-            "velocityY": -0.282149005689615,
-            "timestamp": 2.1352517352371643
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -1.8142028796058936e-31,
-            "velocityY": -1.068352585526189e-31,
-            "timestamp": 2.221595320857177
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.1.traj
deleted file mode 100644
index b656a1eb..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.1.traj	
+++ /dev/null
@@ -1,274 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -7.013886661360986e-29,
-            "velocityX": -3.078938571640024e-30,
-            "velocityY": -8.674299238088502e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 1.3762618085283742,
-            "y": 5.591108793803365,
-            "heading": -0.012389964804724785,
-            "angularVelocity": -0.17699325527244014,
-            "velocityX": 0.3751554628363854,
-            "velocityY": 0.44439566776177375,
-            "timestamp": 0.07000246865606696
-        },
-        {
-            "x": 1.4301971031295795,
-            "y": 5.652094846604856,
-            "heading": -0.03662832018577037,
-            "angularVelocity": -0.3462500086931304,
-            "velocityX": 0.7704770365480613,
-            "velocityY": 0.871198601597385,
-            "timestamp": 0.14000493731213393
-        },
-        {
-            "x": 1.513800165351683,
-            "y": 5.741006259883096,
-            "heading": -0.07186447572819399,
-            "angularVelocity": -0.5033558989907949,
-            "velocityX": 1.1942873419625006,
-            "velocityY": 1.2701182541879128,
-            "timestamp": 0.2100074059682009
-        },
-        {
-            "x": 1.6300142433743692,
-            "y": 5.854390700910866,
-            "heading": -0.1166088429554257,
-            "angularVelocity": -0.6391827043487479,
-            "velocityX": 1.660142567166188,
-            "velocityY": 1.6197206070673247,
-            "timestamp": 0.28000987462426785
-        },
-        {
-            "x": 1.7831654317988928,
-            "y": 5.985149557302319,
-            "heading": -0.16783055719619017,
-            "angularVelocity": -0.7317129698935645,
-            "velocityX": 2.187796964375065,
-            "velocityY": 1.8679177877841964,
-            "timestamp": 0.3500123432803348
-        },
-        {
-            "x": 1.9771685219359858,
-            "y": 6.116694524674393,
-            "heading": -0.21853613173426145,
-            "angularVelocity": -0.7243398055979559,
-            "velocityX": 2.771374979505968,
-            "velocityY": 1.8791475486148668,
-            "timestamp": 0.4200148119364018
-        },
-        {
-            "x": 2.202537147154592,
-            "y": 6.2222527712605,
-            "heading": -0.25772100339188303,
-            "angularVelocity": -0.5597641398939388,
-            "velocityX": 3.2194382504688988,
-            "velocityY": 1.507921772083263,
-            "timestamp": 0.49001728059246874
-        },
-        {
-            "x": 2.4433286317618386,
-            "y": 6.289980043332844,
-            "heading": -0.29446027058227175,
-            "angularVelocity": -0.5248281652898821,
-            "velocityX": 3.4397570432873823,
-            "velocityY": 0.9674983378806481,
-            "timestamp": 0.5600197492485357
-        },
-        {
-            "x": 2.6903031212328568,
-            "y": 6.317347154244271,
-            "heading": -0.33666392758398794,
-            "angularVelocity": -0.6028881239905549,
-            "velocityX": 3.528082569267318,
-            "velocityY": 0.39094494004013314,
-            "timestamp": 0.6300222179046027
-        },
-        {
-            "x": 2.936701774597168,
-            "y": 6.303859233856201,
-            "heading": -0.37968716420906834,
-            "angularVelocity": -0.6145959914234042,
-            "velocityX": 3.5198566292706706,
-            "velocityY": -0.1926777818987785,
-            "timestamp": 0.7000246865606696
-        },
-        {
-            "x": 3.257320421397908,
-            "y": 6.21346874174157,
-            "heading": -0.4320815750431537,
-            "angularVelocity": -0.5604086145235933,
-            "velocityX": 3.429324784540344,
-            "velocityY": -0.9668132467920023,
-            "timestamp": 0.7935179153869726
-        },
-        {
-            "x": 3.5608980813516515,
-            "y": 6.0524118876009405,
-            "heading": -0.46560329407698275,
-            "angularVelocity": -0.35854702479102396,
-            "velocityX": 3.247055040935253,
-            "velocityY": -1.7226579524796546,
-            "timestamp": 0.8870111442132755
-        },
-        {
-            "x": 3.8328887233978395,
-            "y": 5.82633673900318,
-            "heading": -0.46560402675062595,
-            "angularVelocity": -0.00000783664977303189,
-            "velocityX": 2.909201505399863,
-            "velocityY": -2.418091143453467,
-            "timestamp": 0.9805043730395785
-        },
-        {
-            "x": 4.052764522028411,
-            "y": 5.549310224236533,
-            "heading": -0.46560404923284493,
-            "angularVelocity": -2.404689503787649e-7,
-            "velocityX": 2.3517831332905046,
-            "velocityY": -2.963065007427709,
-            "timestamp": 1.0739976018658814
-        },
-        {
-            "x": 4.211188193978367,
-            "y": 5.233096683639544,
-            "heading": -0.46560406449531083,
-            "angularVelocity": -1.6324674914477828e-7,
-            "velocityX": 1.6944935364707439,
-            "velocityY": -3.382207937052538,
-            "timestamp": 1.1674908306921843
-        },
-        {
-            "x": 4.383758324037826,
-            "y": 4.9243757535417085,
-            "heading": -0.4656040804041062,
-            "angularVelocity": -1.701598650622723e-7,
-            "velocityX": 1.8458035113972522,
-            "velocityY": -3.302067261698653,
-            "timestamp": 1.2609840595184871
-        },
-        {
-            "x": 4.615944430524366,
-            "y": 4.657582435311582,
-            "heading": -0.46560407064369363,
-            "angularVelocity": 1.0439700291593356e-7,
-            "velocityX": 2.4834537153263514,
-            "velocityY": -2.8536111286282813,
-            "timestamp": 1.35447728834479
-        },
-        {
-            "x": 4.89788653877274,
-            "y": 4.444046330431443,
-            "heading": -0.46560381887866387,
-            "angularVelocity": 0.0000026928691303704606,
-            "velocityX": 3.015642007317779,
-            "velocityY": -2.2839740113892115,
-            "timestamp": 1.4479705171710928
-        },
-        {
-            "x": 5.217611687922929,
-            "y": 4.2928350259750445,
-            "heading": -0.4656035898817651,
-            "angularVelocity": 0.000002449342071651111,
-            "velocityX": 3.4197679678406536,
-            "velocityY": -1.6173503295872917,
-            "timestamp": 1.5414637459973957
-        },
-        {
-            "x": 5.553860912374937,
-            "y": 4.183174208089645,
-            "heading": -0.46560339968651515,
-            "angularVelocity": 0.0000020343211207896953,
-            "velocityX": 3.5965088453273077,
-            "velocityY": -1.1729279142677982,
-            "timestamp": 1.6349569748236985
-        },
-        {
-            "x": 5.905448307826856,
-            "y": 4.144763712168745,
-            "heading": -0.4656033544319255,
-            "angularVelocity": 4.840413598760029e-7,
-            "velocityX": 3.7605653357540834,
-            "velocityY": -0.4108371954108264,
-            "timestamp": 1.7284502036500013
-        },
-        {
-            "x": 6.2591141169961295,
-            "y": 4.141672024564684,
-            "heading": -0.4656033129772286,
-            "angularVelocity": 4.433978530009588e-7,
-            "velocityX": 3.7827959693886872,
-            "velocityY": -0.0330685723754928,
-            "timestamp": 1.8219434324763042
-        },
-        {
-            "x": 6.612779928543081,
-            "y": 4.138580608962963,
-            "heading": -0.465603271522502,
-            "angularVelocity": 4.433981701164172e-7,
-            "velocityX": 3.7827959948202516,
-            "velocityY": -0.03306566304887272,
-            "timestamp": 1.915436661302607
-        },
-        {
-            "x": 6.963404638780936,
-            "y": 4.132792400258934,
-            "heading": -0.45779034574220756,
-            "angularVelocity": 0.08356675535090403,
-            "velocityX": 3.7502684915210915,
-            "velocityY": -0.061910458935820756,
-            "timestamp": 2.00892989012891
-        },
-        {
-            "x": 7.251193869015599,
-            "y": 4.13050050188738,
-            "heading": -0.30806732380169355,
-            "angularVelocity": 1.6014317167147898,
-            "velocityX": 3.0781826004676223,
-            "velocityY": -0.024514057331484977,
-            "timestamp": 2.1024231189552127
-        },
-        {
-            "x": 7.466125497714746,
-            "y": 4.12875254378885,
-            "heading": -0.16499798934349721,
-            "angularVelocity": 1.5302641298654869,
-            "velocityX": 2.29890048078732,
-            "velocityY": -0.018696092973500365,
-            "timestamp": 2.1959163477815156
-        },
-        {
-            "x": 7.608856917870355,
-            "y": 4.12758509655783,
-            "heading": -0.05770853615881093,
-            "angularVelocity": 1.147563888118738,
-            "velocityX": 1.5266498114080966,
-            "velocityY": -0.012486970935497041,
-            "timestamp": 2.2894095766078184
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -1.6446149791629635e-35,
-            "angularVelocity": 0.6172482957672322,
-            "velocityX": 0.7609436856846457,
-            "velocityY": -0.0062581704063020965,
-            "timestamp": 2.3829028054341213
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -1.0953722106158818e-30,
-            "velocityX": 0,
-            "velocityY": -1.282624680344039e-29,
-            "timestamp": 2.476396034260424
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.traj
deleted file mode 100644
index b656a1eb..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.traj	
+++ /dev/null
@@ -1,274 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -7.013886661360986e-29,
-            "velocityX": -3.078938571640024e-30,
-            "velocityY": -8.674299238088502e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 1.3762618085283742,
-            "y": 5.591108793803365,
-            "heading": -0.012389964804724785,
-            "angularVelocity": -0.17699325527244014,
-            "velocityX": 0.3751554628363854,
-            "velocityY": 0.44439566776177375,
-            "timestamp": 0.07000246865606696
-        },
-        {
-            "x": 1.4301971031295795,
-            "y": 5.652094846604856,
-            "heading": -0.03662832018577037,
-            "angularVelocity": -0.3462500086931304,
-            "velocityX": 0.7704770365480613,
-            "velocityY": 0.871198601597385,
-            "timestamp": 0.14000493731213393
-        },
-        {
-            "x": 1.513800165351683,
-            "y": 5.741006259883096,
-            "heading": -0.07186447572819399,
-            "angularVelocity": -0.5033558989907949,
-            "velocityX": 1.1942873419625006,
-            "velocityY": 1.2701182541879128,
-            "timestamp": 0.2100074059682009
-        },
-        {
-            "x": 1.6300142433743692,
-            "y": 5.854390700910866,
-            "heading": -0.1166088429554257,
-            "angularVelocity": -0.6391827043487479,
-            "velocityX": 1.660142567166188,
-            "velocityY": 1.6197206070673247,
-            "timestamp": 0.28000987462426785
-        },
-        {
-            "x": 1.7831654317988928,
-            "y": 5.985149557302319,
-            "heading": -0.16783055719619017,
-            "angularVelocity": -0.7317129698935645,
-            "velocityX": 2.187796964375065,
-            "velocityY": 1.8679177877841964,
-            "timestamp": 0.3500123432803348
-        },
-        {
-            "x": 1.9771685219359858,
-            "y": 6.116694524674393,
-            "heading": -0.21853613173426145,
-            "angularVelocity": -0.7243398055979559,
-            "velocityX": 2.771374979505968,
-            "velocityY": 1.8791475486148668,
-            "timestamp": 0.4200148119364018
-        },
-        {
-            "x": 2.202537147154592,
-            "y": 6.2222527712605,
-            "heading": -0.25772100339188303,
-            "angularVelocity": -0.5597641398939388,
-            "velocityX": 3.2194382504688988,
-            "velocityY": 1.507921772083263,
-            "timestamp": 0.49001728059246874
-        },
-        {
-            "x": 2.4433286317618386,
-            "y": 6.289980043332844,
-            "heading": -0.29446027058227175,
-            "angularVelocity": -0.5248281652898821,
-            "velocityX": 3.4397570432873823,
-            "velocityY": 0.9674983378806481,
-            "timestamp": 0.5600197492485357
-        },
-        {
-            "x": 2.6903031212328568,
-            "y": 6.317347154244271,
-            "heading": -0.33666392758398794,
-            "angularVelocity": -0.6028881239905549,
-            "velocityX": 3.528082569267318,
-            "velocityY": 0.39094494004013314,
-            "timestamp": 0.6300222179046027
-        },
-        {
-            "x": 2.936701774597168,
-            "y": 6.303859233856201,
-            "heading": -0.37968716420906834,
-            "angularVelocity": -0.6145959914234042,
-            "velocityX": 3.5198566292706706,
-            "velocityY": -0.1926777818987785,
-            "timestamp": 0.7000246865606696
-        },
-        {
-            "x": 3.257320421397908,
-            "y": 6.21346874174157,
-            "heading": -0.4320815750431537,
-            "angularVelocity": -0.5604086145235933,
-            "velocityX": 3.429324784540344,
-            "velocityY": -0.9668132467920023,
-            "timestamp": 0.7935179153869726
-        },
-        {
-            "x": 3.5608980813516515,
-            "y": 6.0524118876009405,
-            "heading": -0.46560329407698275,
-            "angularVelocity": -0.35854702479102396,
-            "velocityX": 3.247055040935253,
-            "velocityY": -1.7226579524796546,
-            "timestamp": 0.8870111442132755
-        },
-        {
-            "x": 3.8328887233978395,
-            "y": 5.82633673900318,
-            "heading": -0.46560402675062595,
-            "angularVelocity": -0.00000783664977303189,
-            "velocityX": 2.909201505399863,
-            "velocityY": -2.418091143453467,
-            "timestamp": 0.9805043730395785
-        },
-        {
-            "x": 4.052764522028411,
-            "y": 5.549310224236533,
-            "heading": -0.46560404923284493,
-            "angularVelocity": -2.404689503787649e-7,
-            "velocityX": 2.3517831332905046,
-            "velocityY": -2.963065007427709,
-            "timestamp": 1.0739976018658814
-        },
-        {
-            "x": 4.211188193978367,
-            "y": 5.233096683639544,
-            "heading": -0.46560406449531083,
-            "angularVelocity": -1.6324674914477828e-7,
-            "velocityX": 1.6944935364707439,
-            "velocityY": -3.382207937052538,
-            "timestamp": 1.1674908306921843
-        },
-        {
-            "x": 4.383758324037826,
-            "y": 4.9243757535417085,
-            "heading": -0.4656040804041062,
-            "angularVelocity": -1.701598650622723e-7,
-            "velocityX": 1.8458035113972522,
-            "velocityY": -3.302067261698653,
-            "timestamp": 1.2609840595184871
-        },
-        {
-            "x": 4.615944430524366,
-            "y": 4.657582435311582,
-            "heading": -0.46560407064369363,
-            "angularVelocity": 1.0439700291593356e-7,
-            "velocityX": 2.4834537153263514,
-            "velocityY": -2.8536111286282813,
-            "timestamp": 1.35447728834479
-        },
-        {
-            "x": 4.89788653877274,
-            "y": 4.444046330431443,
-            "heading": -0.46560381887866387,
-            "angularVelocity": 0.0000026928691303704606,
-            "velocityX": 3.015642007317779,
-            "velocityY": -2.2839740113892115,
-            "timestamp": 1.4479705171710928
-        },
-        {
-            "x": 5.217611687922929,
-            "y": 4.2928350259750445,
-            "heading": -0.4656035898817651,
-            "angularVelocity": 0.000002449342071651111,
-            "velocityX": 3.4197679678406536,
-            "velocityY": -1.6173503295872917,
-            "timestamp": 1.5414637459973957
-        },
-        {
-            "x": 5.553860912374937,
-            "y": 4.183174208089645,
-            "heading": -0.46560339968651515,
-            "angularVelocity": 0.0000020343211207896953,
-            "velocityX": 3.5965088453273077,
-            "velocityY": -1.1729279142677982,
-            "timestamp": 1.6349569748236985
-        },
-        {
-            "x": 5.905448307826856,
-            "y": 4.144763712168745,
-            "heading": -0.4656033544319255,
-            "angularVelocity": 4.840413598760029e-7,
-            "velocityX": 3.7605653357540834,
-            "velocityY": -0.4108371954108264,
-            "timestamp": 1.7284502036500013
-        },
-        {
-            "x": 6.2591141169961295,
-            "y": 4.141672024564684,
-            "heading": -0.4656033129772286,
-            "angularVelocity": 4.433978530009588e-7,
-            "velocityX": 3.7827959693886872,
-            "velocityY": -0.0330685723754928,
-            "timestamp": 1.8219434324763042
-        },
-        {
-            "x": 6.612779928543081,
-            "y": 4.138580608962963,
-            "heading": -0.465603271522502,
-            "angularVelocity": 4.433981701164172e-7,
-            "velocityX": 3.7827959948202516,
-            "velocityY": -0.03306566304887272,
-            "timestamp": 1.915436661302607
-        },
-        {
-            "x": 6.963404638780936,
-            "y": 4.132792400258934,
-            "heading": -0.45779034574220756,
-            "angularVelocity": 0.08356675535090403,
-            "velocityX": 3.7502684915210915,
-            "velocityY": -0.061910458935820756,
-            "timestamp": 2.00892989012891
-        },
-        {
-            "x": 7.251193869015599,
-            "y": 4.13050050188738,
-            "heading": -0.30806732380169355,
-            "angularVelocity": 1.6014317167147898,
-            "velocityX": 3.0781826004676223,
-            "velocityY": -0.024514057331484977,
-            "timestamp": 2.1024231189552127
-        },
-        {
-            "x": 7.466125497714746,
-            "y": 4.12875254378885,
-            "heading": -0.16499798934349721,
-            "angularVelocity": 1.5302641298654869,
-            "velocityX": 2.29890048078732,
-            "velocityY": -0.018696092973500365,
-            "timestamp": 2.1959163477815156
-        },
-        {
-            "x": 7.608856917870355,
-            "y": 4.12758509655783,
-            "heading": -0.05770853615881093,
-            "angularVelocity": 1.147563888118738,
-            "velocityX": 1.5266498114080966,
-            "velocityY": -0.012486970935497041,
-            "timestamp": 2.2894095766078184
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -1.6446149791629635e-35,
-            "angularVelocity": 0.6172482957672322,
-            "velocityX": 0.7609436856846457,
-            "velocityY": -0.0062581704063020965,
-            "timestamp": 2.3829028054341213
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -1.0953722106158818e-30,
-            "velocityX": 0,
-            "velocityY": -1.282624680344039e-29,
-            "timestamp": 2.476396034260424
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.1.traj
deleted file mode 100644
index 0d7f85e3..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.1.traj	
+++ /dev/null
@@ -1,1453 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 3.439885530040909e-30,
-            "timestamp": 0
-        },
-        {
-            "x": 1.3518715996750759,
-            "y": 5.562488632849977,
-            "heading": -0.00039025483625088287,
-            "angularVelocity": -0.020188245249371933,
-            "velocityX": 0.09681959001984766,
-            "velocityY": 0.12873928941829918,
-            "timestamp": 0.019330795293514086
-        },
-        {
-            "x": 1.3556369831983786,
-            "y": 5.567448998179342,
-            "heading": -0.001175615506548702,
-            "angularVelocity": -0.04062743712160119,
-            "velocityX": 0.19478678792724713,
-            "velocityY": 0.256604306964468,
-            "timestamp": 0.03866159058702817
-        },
-        {
-            "x": 1.3613203761105706,
-            "y": 5.574862277415471,
-            "heading": -0.0023613813379046128,
-            "angularVelocity": -0.061340768103509684,
-            "velocityX": 0.2940071955600798,
-            "velocityY": 0.38349582226532697,
-            "timestamp": 0.05799238588054226
-        },
-        {
-            "x": 1.3689483300707626,
-            "y": 5.58470739953387,
-            "heading": -0.003953362765501964,
-            "angularVelocity": -0.08235467829569511,
-            "velocityX": 0.39460114518679135,
-            "velocityY": 0.5092973138928011,
-            "timestamp": 0.07732318117405634
-        },
-        {
-            "x": 1.3785500603498269,
-            "y": 5.596960627127346,
-            "heading": -0.0059579558445008625,
-            "angularVelocity": -0.10369946236363117,
-            "velocityX": 0.4967064279184545,
-            "velocityY": 0.6338708473927627,
-            "timestamp": 0.09665397646757043
-        },
-        {
-            "x": 1.3901578478597558,
-            "y": 5.611595038412612,
-            "heading": -0.008382231168191269,
-            "angularVelocity": -0.12541001479146147,
-            "velocityX": 0.6004816322183893,
-            "velocityY": 0.7570516920313731,
-            "timestamp": 0.11598477176108452
-        },
-        {
-            "x": 1.4038075203372553,
-            "y": 5.62857987161075,
-            "heading": -0.01123404053815137,
-            "angularVelocity": -0.14752674820972528,
-            "velocityX": 0.7061102386242472,
-            "velocityY": 0.8786412012669059,
-            "timestamp": 0.1353155670545986
-        },
-        {
-            "x": 1.4195390306280726,
-            "y": 5.647879684835849,
-            "heading": -0.014522145551621221,
-            "angularVelocity": -0.17009672719326588,
-            "velocityX": 0.8138056428591633,
-            "velocityY": 0.9983972688167116,
-            "timestamp": 0.1546463623481127
-        },
-        {
-            "x": 1.4373971536587393,
-            "y": 5.669453265066441,
-            "heading": -0.018256373225668265,
-            "angularVelocity": -0.19317506689959266,
-            "velocityX": 0.923817295642181,
-            "velocityY": 1.116021348476537,
-            "timestamp": 0.17397715764162677
-        },
-        {
-            "x": 1.457432326992564,
-            "y": 5.693252190583062,
-            "heading": -0.022447804782031403,
-            "angularVelocity": -0.21682664850159125,
-            "velocityX": 1.0364381304346626,
-            "velocityY": 1.2311405276019616,
-            "timestamp": 0.19330795293514086
-        },
-        {
-            "x": 1.47970166115617,
-            "y": 5.719218907072145,
-            "heading": -0.027109004507978946,
-            "angularVelocity": -0.24112819235695526,
-            "velocityX": 1.1520133458284385,
-            "velocityY": 1.3432823686149544,
-            "timestamp": 0.21263874822865494
-        },
-        {
-            "x": 1.5042701411060604,
-            "y": 5.747284109919373,
-            "heading": -0.03225429550223853,
-            "angularVelocity": -0.2661706834165162,
-            "velocityX": 1.2709502933970824,
-            "velocityY": 1.4518390175412745,
-            "timestamp": 0.23196954352216903
-        },
-        {
-            "x": 1.5312120197646597,
-            "y": 5.777363120811461,
-            "heading": -0.037900086436595266,
-            "angularVelocity": -0.2920620103121596,
-            "velocityX": 1.3937284136281272,
-            "velocityY": 1.5560151786502046,
-            "timestamp": 0.25130033881568314
-        },
-        {
-            "x": 1.5606123466917503,
-            "y": 5.809350786193417,
-            "heading": -0.044065244214455006,
-            "angularVelocity": -0.3189293396494718,
-            "velocityX": 1.5209062266028461,
-            "velocityY": 1.6547516486654796,
-            "timestamp": 0.27063113410919726
-        },
-        {
-            "x": 1.5925684302904153,
-            "y": 5.843114184319679,
-            "heading": -0.050771482089097104,
-            "angularVelocity": -0.34691991575183656,
-            "velocityX": 1.653117893674399,
-            "velocityY": 1.7466119532904336,
-            "timestamp": 0.28996192940271137
-        },
-        {
-            "x": 1.6271906907680806,
-            "y": 5.8784820940308995,
-            "heading": -0.058043668844819325,
-            "angularVelocity": -0.37619697717051565,
-            "velocityX": 1.791041700663091,
-            "velocityY": 1.8296148282676763,
-            "timestamp": 0.3092927246962255
-        },
-        {
-            "x": 1.6646015916598045,
-            "y": 5.915229821006168,
-            "heading": -0.06590980894370155,
-            "angularVelocity": -0.4069227354304089,
-            "velocityX": 1.935300660096276,
-            "velocityY": 1.9009940572697726,
-            "timestamp": 0.3286235199897396
-        },
-        {
-            "x": 1.7049297157110095,
-            "y": 5.953057987948439,
-            "heading": -0.07440007857488001,
-            "angularVelocity": -0.43920953598980744,
-            "velocityX": 2.086211324411271,
-            "velocityY": 1.9568862205562443,
-            "timestamp": 0.3479543152832537
-        },
-        {
-            "x": 1.7482931222517035,
-            "y": 5.991565656180575,
-            "heading": -0.08354352935308812,
-            "angularVelocity": -0.47299920357005454,
-            "velocityX": 2.2432293075517364,
-            "velocityY": 1.9920374535783947,
-            "timestamp": 0.3672851105767678
-        },
-        {
-            "x": 1.7947629094651933,
-            "y": 6.030225122456552,
-            "heading": -0.09335994332155541,
-            "angularVelocity": -0.5078122146252688,
-            "velocityX": 2.4039252657691588,
-            "velocityY": 1.9998901074156783,
-            "timestamp": 0.38661590587028194
-        },
-        {
-            "x": 1.844301928458138,
-            "y": 6.068381679285448,
-            "heading": -0.10384499839936924,
-            "angularVelocity": -0.5424016404194022,
-            "velocityX": 2.562699477220483,
-            "velocityY": 1.9738741344852324,
-            "timestamp": 0.40594670116379605
-        },
-        {
-            "x": 1.8967008920878712,
-            "y": 6.105314232951604,
-            "heading": -0.1149551002367539,
-            "angularVelocity": -0.5747358899978738,
-            "velocityX": 2.710647070341413,
-            "velocityY": 1.9105553137044158,
-            "timestamp": 0.42527749645731017
-        },
-        {
-            "x": 1.951571944971935,
-            "y": 6.140356350737874,
-            "heading": -0.126607814812745,
-            "angularVelocity": -0.6028057510857173,
-            "velocityX": 2.83853054418689,
-            "velocityY": 1.8127613093097652,
-            "timestamp": 0.4446082917508243
-        },
-        {
-            "x": 2.008427579479744,
-            "y": 6.172999646985284,
-            "heading": -0.13870025114422718,
-            "angularVelocity": -0.6255529660251172,
-            "velocityX": 2.9411947953783932,
-            "velocityY": 1.6886680424557017,
-            "timestamp": 0.4639390870443384
-        },
-        {
-            "x": 2.066782288063465,
-            "y": 6.202913115188225,
-            "heading": -0.15112974078850008,
-            "angularVelocity": -0.6429890470384952,
-            "velocityX": 3.018743290054933,
-            "velocityY": 1.5474515015415655,
-            "timestamp": 0.4832698823378525
-        },
-        {
-            "x": 2.126212031523665,
-            "y": 6.229902950259444,
-            "heading": -0.1638060412217236,
-            "angularVelocity": -0.6557567984529158,
-            "velocityX": 3.074355842986947,
-            "velocityY": 1.3962092434072775,
-            "timestamp": 0.5026006776313666
-        },
-        {
-            "x": 2.1863688602207922,
-            "y": 6.253863406934038,
-            "heading": -0.17665473442964857,
-            "angularVelocity": -0.6646748368514358,
-            "velocityX": 3.111968637798953,
-            "velocityY": 1.2394966844759612,
-            "timestamp": 0.5219314729248807
-        },
-        {
-            "x": 2.246973575487724,
-            "y": 6.2747405977079325,
-            "heading": -0.1896159238006329,
-            "angularVelocity": -0.6704943678821678,
-            "velocityX": 3.1351382261683427,
-            "velocityY": 1.0799964748940398,
-            "timestamp": 0.5412622682183948
-        },
-        {
-            "x": 2.30780256528041,
-            "y": 6.292510136082229,
-            "heading": -0.20264154258755296,
-            "angularVelocity": -0.6738273614273119,
-            "velocityX": 3.146740155750102,
-            "velocityY": 0.9192347290676759,
-            "timestamp": 0.560593063511909
-        },
-        {
-            "x": 2.3686753728703707,
-            "y": 6.307164203375093,
-            "heading": -0.2156927432279827,
-            "angularVelocity": -0.6751507344764429,
-            "velocityX": 3.1490068911124522,
-            "velocityY": 0.7580685155659881,
-            "timestamp": 0.5799238588054231
-        },
-        {
-            "x": 2.4294446969864714,
-            "y": 6.318704202261767,
-            "heading": -0.228737776875863,
-            "angularVelocity": -0.6748317102223548,
-            "velocityX": 3.143653594867338,
-            "velocityY": 0.5969748637576952,
-            "timestamp": 0.5992546540989372
-        },
-        {
-            "x": 2.4899887888473393,
-            "y": 6.32713657767754,
-            "heading": -0.24175037423147705,
-            "angularVelocity": -0.6731537506881662,
-            "velocityX": 3.132002121049924,
-            "velocityY": 0.43621461443967424,
-            "timestamp": 0.6185854493924513
-        },
-        {
-            "x": 2.550205774126716,
-            "y": 6.332470420209207,
-            "heading": -0.25470853343764965,
-            "angularVelocity": -0.6703376146412596,
-            "velocityX": 3.1150805936877464,
-            "velocityY": 0.2759246296222352,
-            "timestamp": 0.6379162446859654
-        },
-        {
-            "x": 2.610009417191064,
-            "y": 6.334716080800322,
-            "heading": -0.26759361438505785,
-            "angularVelocity": -0.6665572084213006,
-            "velocityX": 3.09369801688466,
-            "velocityY": 0.11617010873154458,
-            "timestamp": 0.6572470399794795
-        },
-        {
-            "x": 2.669325940254228,
-            "y": 6.333884367249437,
-            "heading": -0.28038965768785973,
-            "angularVelocity": -0.6619512083445117,
-            "velocityX": 3.0684988466597254,
-            "velocityY": -0.04302531469893782,
-            "timestamp": 0.6765778352729936
-        },
-        {
-            "x": 2.7280916104140402,
-            "y": 6.329986080406231,
-            "heading": -0.2930828674707726,
-            "angularVelocity": -0.656631534822156,
-            "velocityX": 3.040002714194029,
-            "velocityY": -0.2016620001410218,
-            "timestamp": 0.6959086305665078
-        },
-        {
-            "x": 2.7862508879843872,
-            "y": 6.323031751356315,
-            "heading": -0.3056612140555153,
-            "angularVelocity": -0.650689554866023,
-            "velocityX": 3.0086334621659443,
-            "velocityY": -0.35975390274029156,
-            "timestamp": 0.7152394258600219
-        },
-        {
-            "x": 2.843754988622848,
-            "y": 6.3130314987287885,
-            "heading": -0.31811412514136855,
-            "angularVelocity": -0.6442006599713835,
-            "velocityX": 2.974740550780866,
-            "velocityY": -0.5173223592555559,
-            "timestamp": 0.734570221153536
-        },
-        {
-            "x": 2.9005607537314586,
-            "y": 6.299994958196607,
-            "heading": -0.3304322429824912,
-            "angularVelocity": -0.6372276801904713,
-            "velocityX": 2.9386150050261657,
-            "velocityY": -0.6743923534566658,
-            "timestamp": 0.7539010164470501
-        },
-        {
-            "x": 2.956629753112793,
-            "y": 6.283931255340576,
-            "heading": -0.34260723133624194,
-            "angularVelocity": -0.6298234588328613,
-            "velocityX": 2.9005014294547,
-            "velocityY": -0.8309902728844937,
-            "timestamp": 0.7732318117405642
-        },
-        {
-            "x": 2.9906169983281425,
-            "y": 6.272983358953205,
-            "heading": -0.34999362222052477,
-            "angularVelocity": -0.625060967472129,
-            "velocityX": 2.8761137487624757,
-            "velocityY": -0.9264474104986867,
-            "timestamp": 0.7850488844241373
-        },
-        {
-            "x": 3.024312445989771,
-            "y": 6.26090838895792,
-            "heading": -0.35731821138357023,
-            "angularVelocity": -0.6198311002374715,
-            "velocityX": 2.8514208691014065,
-            "velocityY": -1.0218241284130187,
-            "timestamp": 0.7968659571077105
-        },
-        {
-            "x": 3.0577122238990846,
-            "y": 6.2477073818247915,
-            "heading": -0.36457506252100974,
-            "angularVelocity": -0.6140988831800235,
-            "velocityX": 2.8264003111145226,
-            "velocityY": -1.1171131367820717,
-            "timestamp": 0.8086830297912836
-        },
-        {
-            "x": 3.090812163934883,
-            "y": 6.233381471982772,
-            "heading": -0.3717577779780659,
-            "angularVelocity": -0.6078252752934934,
-            "velocityX": 2.8010270328463904,
-            "velocityY": -1.2123061459996218,
-            "timestamp": 0.8205001024748567
-        },
-        {
-            "x": 3.1236077670194575,
-            "y": 6.217931905831193,
-            "heading": -0.3788594429566703,
-            "angularVelocity": -0.6009665141923228,
-            "velocityX": 2.7752730276563167,
-            "velocityY": -1.307393680759415,
-            "timestamp": 0.8323171751584298
-        },
-        {
-            "x": 3.1560941623709735,
-            "y": 6.201360058471141,
-            "heading": -0.3858725603738981,
-            "angularVelocity": -0.5934733249949906,
-            "velocityX": 2.749106840704776,
-            "velocityY": -1.4023648498911914,
-            "timestamp": 0.844134247842003
-        },
-        {
-            "x": 3.188266059838492,
-            "y": 6.1836674538325775,
-            "heading": -0.3927889743327873,
-            "angularVelocity": -0.5852899566661331,
-            "velocityX": 2.722492983583009,
-            "velocityY": -1.4972070590002187,
-            "timestamp": 0.8559513205255761
-        },
-        {
-            "x": 3.2201176938016274,
-            "y": 6.164855789080512,
-            "heading": -0.399599779609834,
-            "angularVelocity": -0.576352998701121,
-            "velocityX": 2.695391220484986,
-            "velocityY": -1.5919056483603757,
-            "timestamp": 0.8677683932091492
-        },
-        {
-            "x": 3.251642756703662,
-            "y": 6.144926964467571,
-            "heading": -0.40629521382129,
-            "angularVelocity": -0.5665899153487628,
-            "velocityX": 2.6677556909553144,
-            "velocityY": -1.686443432022221,
-            "timestamp": 0.8795854658927224
-        },
-        {
-            "x": 3.282834319738247,
-            "y": 6.123883120194607,
-            "heading": -0.41286452792903033,
-            "angularVelocity": -0.5559172126335613,
-            "velocityX": 2.6395338227837533,
-            "velocityY": -1.7808001047684463,
-            "timestamp": 0.8914025385762955
-        },
-        {
-            "x": 3.3136847374734915,
-            "y": 6.101726682396313,
-            "heading": -0.4192958293777808,
-            "angularVelocity": -0.5442381223304499,
-            "velocityX": 2.6106649727330486,
-            "velocityY": -1.874951469926556,
-            "timestamp": 0.9032196112598686
-        },
-        {
-            "x": 3.3441855321977045,
-            "y": 6.078460421163092,
-            "heading": -0.42557589025707343,
-            "angularVelocity": -0.5314396422408687,
-            "velocityX": 2.5810787105179256,
-            "velocityY": -1.9688684208198879,
-            "timestamp": 0.9150366839434417
-        },
-        {
-            "x": 3.37432725239814,
-            "y": 6.0540875246663015,
-            "heading": -0.4316899102075926,
-            "angularVelocity": -0.5173887065125765,
-            "velocityX": 2.5506926298537165,
-            "velocityY": -2.0625155780475546,
-            "timestamp": 0.9268537566270149
-        },
-        {
-            "x": 3.4040992978747684,
-            "y": 6.028611695167346,
-            "heading": -0.43762121996231873,
-            "angularVelocity": -0.5019271619587302,
-            "velocityX": 2.519409525001445,
-            "velocityY": -2.1558494375996973,
-            "timestamp": 0.938670829310588
-        },
-        {
-            "x": 3.4334897013019283,
-            "y": 6.00203727528326,
-            "heading": -0.44335090582849124,
-            "angularVelocity": -0.4848650778070557,
-            "velocityX": 2.48711370524239,
-            "velocityY": -2.2488158104525207,
-            "timestamp": 0.9504879019941611
-        },
-        {
-            "x": 3.462484852215012,
-            "y": 5.974369416891247,
-            "heading": -0.4488573271132443,
-            "angularVelocity": -0.46597168623725893,
-            "velocityX": 2.4536661226929253,
-            "velocityY": -2.341346214318684,
-            "timestamp": 0.9623049746777342
-        },
-        {
-            "x": 3.4910691438628887,
-            "y": 5.945614311410449,
-            "heading": -0.45411548589649775,
-            "angularVelocity": -0.4449628875146618,
-            "velocityX": 2.4188978449470153,
-            "velocityY": -2.4333526797013256,
-            "timestamp": 0.9741220473613074
-        },
-        {
-            "x": 3.5192245152965964,
-            "y": 5.915779510545288,
-            "heading": -0.4590961890390477,
-            "angularVelocity": -0.42148366824158007,
-            "velocityX": 2.3826011896200363,
-            "velocityY": -2.524720094735007,
-            "timestamp": 0.9859391200448805
-        },
-        {
-            "x": 3.5469298492407,
-            "y": 5.884874383909108,
-            "heading": -0.46376491150225324,
-            "angularVelocity": -0.39508282535110545,
-            "velocityX": 2.3445175201991417,
-            "velocityY": -2.6152946219194915,
-            "timestamp": 0.9977561927284536
-        },
-        {
-            "x": 3.5741601690568516,
-            "y": 5.852910789899007,
-            "heading": -0.46808022074004524,
-            "angularVelocity": -0.36517582258680764,
-            "velocityX": 2.304320244556386,
-            "velocityY": -2.7048656520944543,
-            "timestamp": 1.0095732654120266
-        },
-        {
-            "x": 3.6008855536785225,
-            "y": 5.819904089620565,
-            "heading": -0.4719915430906053,
-            "angularVelocity": -0.3309891083260508,
-            "velocityX": 2.2615909487314654,
-            "velocityY": -2.7931367744165776,
-            "timestamp": 1.0213903380955998
-        },
-        {
-            "x": 3.62706965768041,
-            "y": 5.785874731899473,
-            "heading": -0.4754359323425916,
-            "angularVelocity": -0.2914756762708573,
-            "velocityX": 2.2157859821143067,
-            "velocityY": -2.8796774490856496,
-            "timestamp": 1.0332074107791729
-        },
-        {
-            "x": 3.652667693323084,
-            "y": 5.750850822222342,
-            "heading": -0.4783333470461038,
-            "angularVelocity": -0.24518887046705806,
-            "velocityX": 2.1661909280000957,
-            "velocityY": -2.9638397439848143,
-            "timestamp": 1.045024483462746
-        },
-        {
-            "x": 3.677623743737937,
-            "y": 5.7148724362586085,
-            "heading": -0.48057991095642455,
-            "angularVelocity": -0.1901117112907446,
-            "velocityX": 2.1118640024567648,
-            "velocityY": -3.044610702424323,
-            "timestamp": 1.0568415561463191
-        },
-        {
-            "x": 3.701867477642102,
-            "y": 5.67799904418439,
-            "heading": -0.48203966919964636,
-            "angularVelocity": -0.12352959843022769,
-            "velocityX": 2.0515854097999866,
-            "velocityY": -3.120349096733213,
-            "timestamp": 1.0686586288298923
-        },
-        {
-            "x": 3.725329217347855,
-            "y": 5.640287080856699,
-            "heading": -0.4827696693750387,
-            "angularVelocity": -0.06177504318881954,
-            "velocityX": 1.9854104594250916,
-            "velocityY": -3.191311785711063,
-            "timestamp": 1.0804757015134654
-        },
-        {
-            "x": 3.74792659306077,
-            "y": 5.60180905514361,
-            "heading": -0.48297166201638797,
-            "angularVelocity": -0.017093289239954308,
-            "velocityX": 1.9122651030425624,
-            "velocityY": -3.2561385330714976,
-            "timestamp": 1.0922927741970385
-        },
-        {
-            "x": 3.769555777117098,
-            "y": 5.56268673154527,
-            "heading": -0.48297173035274543,
-            "angularVelocity": -0.000005782849890954889,
-            "velocityX": 1.8303335043707702,
-            "velocityY": -3.310661163379628,
-            "timestamp": 1.1041098468806116
-        },
-        {
-            "x": 3.7901587344932506,
-            "y": 5.52301434523992,
-            "heading": -0.48297173483145855,
-            "angularVelocity": -3.790036003454015e-7,
-            "velocityX": 1.743490788949144,
-            "velocityY": -3.3572092994315077,
-            "timestamp": 1.1159269195641848
-        },
-        {
-            "x": 3.809721481337777,
-            "y": 5.482818833345491,
-            "heading": -0.4829717389588181,
-            "angularVelocity": -3.4927089311686727e-7,
-            "velocityX": 1.6554647135005363,
-            "velocityY": -3.401477926957743,
-            "timestamp": 1.127743992247758
-        },
-        {
-            "x": 3.8282308263038667,
-            "y": 5.442127501029994,
-            "heading": -0.482971742791846,
-            "angularVelocity": -3.2436357183409427e-7,
-            "velocityX": 1.5663223424036268,
-            "velocityY": -3.443435900336205,
-            "timestamp": 1.139561064931331
-        },
-        {
-            "x": 3.8456768429335884,
-            "y": 5.400969063215312,
-            "heading": -0.48297174636762497,
-            "angularVelocity": -3.0259430956931755e-7,
-            "velocityX": 1.4763399614162735,
-            "velocityY": -3.482963921504598,
-            "timestamp": 1.1513781376149042
-        },
-        {
-            "x": 3.863665898181474,
-            "y": 5.360045056052632,
-            "heading": -0.48297175000247344,
-            "angularVelocity": -3.075929693793739e-7,
-            "velocityX": 1.5222936957043627,
-            "velocityY": -3.463125619898205,
-            "timestamp": 1.1631952102984773
-        },
-        {
-            "x": 3.8827072670006824,
-            "y": 5.319599949808908,
-            "heading": -0.4829717539017329,
-            "angularVelocity": -3.299683104273868e-7,
-            "velocityX": 1.6113439706329509,
-            "velocityY": -3.4225994310711667,
-            "timestamp": 1.1750122829820504
-        },
-        {
-            "x": 3.902794276163034,
-            "y": 5.279663847505986,
-            "heading": -0.48297175811002735,
-            "angularVelocity": -3.5611987502134074e-7,
-            "velocityX": 1.6998295347945822,
-            "velocityY": -3.379525824397886,
-            "timestamp": 1.1868293556656235
-        },
-        {
-            "x": 3.92391456197569,
-            "y": 5.240264436686968,
-            "heading": -0.48297176267932185,
-            "angularVelocity": -3.8666890356003695e-7,
-            "velocityX": 1.78726884214022,
-            "velocityY": -3.3341092057245874,
-            "timestamp": 1.1986464283491967
-        },
-        {
-            "x": 3.9460538007835475,
-            "y": 5.201428448582004,
-            "heading": -0.4829717676728866,
-            "angularVelocity": -4.225720568590589e-7,
-            "velocityX": 1.8734960341433124,
-            "velocityY": -3.286430501434571,
-            "timestamp": 1.2104635010327698
-        },
-        {
-            "x": 3.969196975663963,
-            "y": 5.163182231498957,
-            "heading": -0.4829717731698369,
-            "angularVelocity": -4.651702214495778e-7,
-            "velocityX": 1.9584524441985565,
-            "velocityY": -3.2365221156854496,
-            "timestamp": 1.222280573716343
-        },
-        {
-            "x": 3.993328387583419,
-            "y": 5.125551733327025,
-            "heading": -0.48297177927069973,
-            "angularVelocity": -5.162753076431712e-7,
-            "velocityX": 2.0420803498146496,
-            "velocityY": -3.184417933236803,
-            "timestamp": 1.234097646399916
-        },
-        {
-            "x": 4.018431663513184,
-            "y": 5.088562488555908,
-            "heading": -0.4829717875114574,
-            "angularVelocity": -6.973603296302728e-7,
-            "velocityX": 2.124322715274553,
-            "velocityY": -3.1301529373289303,
-            "timestamp": 1.2459147190834892
-        },
-        {
-            "x": 4.068270320597518,
-            "y": 5.022262107474161,
-            "heading": -0.48297170333499495,
-            "angularVelocity": 0.000003839169547390073,
-            "velocityX": 2.2730707479901486,
-            "velocityY": -3.023866725833157,
-            "timestamp": 1.2678404145896236
-        },
-        {
-            "x": 4.121253956507243,
-            "y": 4.9584470041490425,
-            "heading": -0.48297162876403243,
-            "angularVelocity": 0.0000034010762602073378,
-            "velocityX": 2.4165087896481814,
-            "velocityY": -2.910516718033651,
-            "timestamp": 1.289766110095758
-        },
-        {
-            "x": 4.177258824192737,
-            "y": 4.89726621863139,
-            "heading": -0.4829715614847619,
-            "angularVelocity": 0.0000030685124925835,
-            "velocityX": 2.5543029031769633,
-            "velocityY": -2.7903692040481487,
-            "timestamp": 1.3116918056018925
-        },
-        {
-            "x": 4.236154120064876,
-            "y": 4.83886263701252,
-            "heading": -0.482971499836949,
-            "angularVelocity": 0.000002811669663014854,
-            "velocityX": 2.686131249777681,
-            "velocityY": -2.6637048572771733,
-            "timestamp": 1.333617501108027
-        },
-        {
-            "x": 4.2978022891900425,
-            "y": 4.7833726571672255,
-            "heading": -0.48297144258671054,
-            "angularVelocity": 0.0000026111025032599054,
-            "velocityX": 2.811685910164931,
-            "velocityY": -2.530819596111362,
-            "timestamp": 1.3555431966141613
-        },
-        {
-            "x": 4.362059346053497,
-            "y": 4.730925869094327,
-            "heading": -0.48297138878758666,
-            "angularVelocity": 0.0000024537020442790352,
-            "velocityX": 2.9306735946176077,
-            "velocityY": -2.392023918156931,
-            "timestamp": 1.3774688921202958
-        },
-        {
-            "x": 4.428775209886728,
-            "y": 4.681644750011071,
-            "heading": -0.4829713376913353,
-            "angularVelocity": 0.000002330427847533908,
-            "velocityX": 3.042816307221113,
-            "velocityY": -2.247642227334136,
-            "timestamp": 1.3993945876264302
-        },
-        {
-            "x": 4.497794001617253,
-            "y": 4.635644296035039,
-            "heading": -0.4829712886902182,
-            "angularVelocity": 0.000002234871730357886,
-            "velocityX": 3.147849595521449,
-            "velocityY": -2.098015726031637,
-            "timestamp": 1.4213202831325646
-        },
-        {
-            "x": 4.568948026555497,
-            "y": 4.593021059923411,
-            "heading": -0.4829712412701429,
-            "angularVelocity": 0.0000021627626491587156,
-            "velocityX": 3.2452345659144926,
-            "velocityY": -1.9439855898636562,
-            "timestamp": 1.443245978638699
-        },
-        {
-            "x": 4.64205867202195,
-            "y": 4.55384875924605,
-            "heading": -0.4829711949807044,
-            "angularVelocity": 0.000002111195903735364,
-            "velocityX": 3.3344732643018493,
-            "velocityY": -1.7865932994667033,
-            "timestamp": 1.4651716741448335
-        },
-        {
-            "x": 4.71691342103475,
-            "y": 4.5181219873551415,
-            "heading": -0.4829711494130406,
-            "angularVelocity": 0.000002078276777149451,
-            "velocityX": 3.414019363346236,
-            "velocityY": -1.6294475986389252,
-            "timestamp": 1.487097369650968
-        },
-        {
-            "x": 4.7930985438005,
-            "y": 4.485327738304056,
-            "heading": -0.4829711041848689,
-            "angularVelocity": 0.000002062793021590678,
-            "velocityX": 3.4746958309446994,
-            "velocityY": -1.4956993743693363,
-            "timestamp": 1.5090230651571024
-        },
-        {
-            "x": 4.8695687201769084,
-            "y": 4.453203793512952,
-            "heading": -0.4829710590084327,
-            "angularVelocity": 0.0000020604334399265313,
-            "velocityX": 3.48769672346274,
-            "velocityY": -1.4651277439372516,
-            "timestamp": 1.5309487606632368
-        },
-        {
-            "x": 4.94605170532014,
-            "y": 4.421110356684282,
-            "heading": -0.48297101383350444,
-            "angularVelocity": 0.0000020603646626736416,
-            "velocityX": 3.4882809132254033,
-            "velocityY": -1.463736318862417,
-            "timestamp": 1.5528744561693713
-        },
-        {
-            "x": 5.022532332392591,
-            "y": 4.389011300831823,
-            "heading": -0.48297096865831407,
-            "angularVelocity": 0.000002060376618948169,
-            "velocityX": 3.4881733649487248,
-            "velocityY": -1.463992594598631,
-            "timestamp": 1.5748001516755057
-        },
-        {
-            "x": 5.099012556138694,
-            "y": 4.356911284014184,
-            "heading": -0.48297092348307896,
-            "angularVelocity": 0.000002060378658787562,
-            "velocityX": 3.488154969803465,
-            "velocityY": -1.4640364228636573,
-            "timestamp": 1.5967258471816401
-        },
-        {
-            "x": 5.175492855161546,
-            "y": 4.324811446548553,
-            "heading": -0.48297087830785207,
-            "angularVelocity": 0.0000020603782854914656,
-            "velocityX": 3.48815840306965,
-            "velocityY": -1.46402824287194,
-            "timestamp": 1.6186515426877746
-        },
-        {
-            "x": 5.2519731561146505,
-            "y": 4.292711613681912,
-            "heading": -0.48297083313262523,
-            "angularVelocity": 0.0000020603782806948455,
-            "velocityX": 3.488158491105669,
-            "velocityY": -1.464028033118535,
-            "timestamp": 1.640577238193909
-        },
-        {
-            "x": 5.328453454863713,
-            "y": 4.260611775564003,
-            "heading": -0.48297078795739806,
-            "angularVelocity": 0.0000020603782977173096,
-            "velocityX": 3.488158390582454,
-            "velocityY": -1.4640282726214169,
-            "timestamp": 1.6625029337000434
-        },
-        {
-            "x": 5.404933754197733,
-            "y": 4.228511938839819,
-            "heading": -0.4829707427821709,
-            "angularVelocity": 0.000002060378298482567,
-            "velocityX": 3.4881584172616003,
-            "velocityY": -1.4640282090556718,
-            "timestamp": 1.6844286292061779
-        },
-        {
-            "x": 5.481414053769572,
-            "y": 4.196412102682258,
-            "heading": -0.48297069760694356,
-            "angularVelocity": 0.000002060378303797646,
-            "velocityX": 3.488158428108085,
-            "velocityY": -1.4640281832128117,
-            "timestamp": 1.7063543247123123
-        },
-        {
-            "x": 5.5578943539285115,
-            "y": 4.164312267923512,
-            "heading": -0.4829706524317162,
-            "angularVelocity": 0.000002060378305885037,
-            "velocityX": 3.488158454884962,
-            "velocityY": -1.4640281194148257,
-            "timestamp": 1.7282800202184467
-        },
-        {
-            "x": 5.634374653438418,
-            "y": 4.132212431618396,
-            "heading": -0.4829706072564887,
-            "angularVelocity": 0.0000020603783152723795,
-            "velocityX": 3.4881584252834856,
-            "velocityY": -1.4640281899425134,
-            "timestamp": 1.7502057157245812
-        },
-        {
-            "x": 5.71085495402441,
-            "y": 4.100112597877132,
-            "heading": -0.4829705620812611,
-            "angularVelocity": 0.0000020603783147781934,
-            "velocityX": 3.4881584743622165,
-            "velocityY": -1.4640280730089004,
-            "timestamp": 1.7721314112307156
-        },
-        {
-            "x": 5.7873352532620705,
-            "y": 4.068012760923364,
-            "heading": -0.4829705169060333,
-            "angularVelocity": 0.0000020603783267765002,
-            "velocityX": 3.488158412866673,
-            "velocityY": -1.4640282195266534,
-            "timestamp": 1.79405710673685
-        },
-        {
-            "x": 5.863815552376704,
-            "y": 4.035912923676468,
-            "heading": -0.4829704717308054,
-            "angularVelocity": 0.000002060378332596705,
-            "velocityX": 3.4881584072556215,
-            "velocityY": -1.4640282328957663,
-            "timestamp": 1.8159828022429845
-        },
-        {
-            "x": 5.940295849970619,
-            "y": 4.00381308280633,
-            "heading": -0.48297042655557715,
-            "angularVelocity": 0.0000020603783450065593,
-            "velocityX": 3.4881583378978336,
-            "velocityY": -1.4640283981467839,
-            "timestamp": 1.837908497749119
-        },
-        {
-            "x": 6.016776146567219,
-            "y": 3.971713239559965,
-            "heading": -0.4829703813803487,
-            "angularVelocity": 0.0000020603783564721777,
-            "velocityX": 3.4881582924116907,
-            "velocityY": -1.4640285065230472,
-            "timestamp": 1.8598341932552533
-        },
-        {
-            "x": 6.093256433235532,
-            "y": 3.939613372658632,
-            "heading": -0.48297033620511903,
-            "angularVelocity": 0.000002060378411227836,
-            "velocityX": 3.4881578395965716,
-            "velocityY": -1.464029585392675,
-            "timestamp": 1.8817598887613878
-        },
-        {
-            "x": 6.169736625426417,
-            "y": 3.9075132806585757,
-            "heading": -0.4829702910298788,
-            "angularVelocity": 0.0000020603788925938424,
-            "velocityX": 3.488153530614592,
-            "velocityY": -1.4640398518273146,
-            "timestamp": 1.9036855842675222
-        },
-        {
-            "x": 6.2462171159811755,
-            "y": 3.8754138995336747,
-            "heading": -0.4829702458546715,
-            "angularVelocity": 0.0000020603773887814844,
-            "velocityX": 3.4881671385698874,
-            "velocityY": -1.4640074298145878,
-            "timestamp": 1.9256112797736566
-        },
-        {
-            "x": 6.322701012459848,
-            "y": 3.843322634623867,
-            "heading": -0.4829702006798376,
-            "angularVelocity": 0.0000020603603619090433,
-            "velocityX": 3.4883224779483077,
-            "velocityY": -1.4636372607092853,
-            "timestamp": 1.947536975279791
-        },
-        {
-            "x": 6.399174860669393,
-            "y": 3.811207431927173,
-            "heading": -0.4829701555038708,
-            "angularVelocity": 0.000002060412030184656,
-            "velocityX": 3.4878641905846774,
-            "velocityY": -1.4647290293616972,
-            "timestamp": 1.9694626707859255
-        },
-        {
-            "x": 6.47530722887592,
-            "y": 3.7782908978955247,
-            "heading": -0.4829701102655904,
-            "angularVelocity": 0.0000020632540676234596,
-            "velocityX": 3.4722897700184046,
-            "velocityY": -1.501276619595699,
-            "timestamp": 1.99138836629206
-        },
-        {
-            "x": 6.550061088980348,
-            "y": 3.742353506827061,
-            "heading": -0.48297006466550657,
-            "angularVelocity": 0.0000020797554102610617,
-            "velocityX": 3.409417962749362,
-            "velocityY": -1.6390536418100325,
-            "timestamp": 2.0133140617981944
-        },
-        {
-            "x": 6.623057371447113,
-            "y": 3.702968505083503,
-            "heading": -0.482970018320009,
-            "angularVelocity": 0.0000021137526750992832,
-            "velocityX": 3.3292573294334487,
-            "velocityY": -1.796294294637834,
-            "timestamp": 2.035239757304329
-        },
-        {
-            "x": 6.694087047655727,
-            "y": 3.6601383683558693,
-            "heading": -0.48296997081845616,
-            "angularVelocity": 0.0000021664787259977457,
-            "velocityX": 3.2395631960090396,
-            "velocityY": -1.9534220346951927,
-            "timestamp": 2.0571654528104633
-        },
-        {
-            "x": 6.762971951373863,
-            "y": 3.6139376604858007,
-            "heading": -0.48296992170702124,
-            "angularVelocity": 0.000002239903173580672,
-            "velocityX": 3.141743152404488,
-            "velocityY": -2.1071490232631853,
-            "timestamp": 2.0790911483165977
-        },
-        {
-            "x": 6.829544518243924,
-            "y": 3.564463136615443,
-            "heading": -0.4829698704667692,
-            "angularVelocity": 0.0000023369955126814126,
-            "velocityX": 3.0362807351500685,
-            "velocityY": -2.256463146473092,
-            "timestamp": 2.101016843822732
-        },
-        {
-            "x": 6.893649101257324,
-            "y": 3.5118300914764404,
-            "heading": -0.48296981752333895,
-            "angularVelocity": 0.0000024146750677216304,
-            "velocityX": 2.923719477698919,
-            "velocityY": -2.400518839837981,
-            "timestamp": 2.1229425393288666
-        },
-        {
-            "x": 6.935492251520178,
-            "y": 3.475140361342702,
-            "heading": -0.48296976475625003,
-            "angularVelocity": 0.000003586928740757824,
-            "velocityX": 2.8443562341956334,
-            "velocityY": -2.4940441143007757,
-            "timestamp": 2.1376534780501846
-        },
-        {
-            "x": 6.976123919914837,
-            "y": 3.4371133556518583,
-            "heading": -0.4829697143572031,
-            "angularVelocity": 0.0000034259572361559534,
-            "velocityX": 2.7620037826529793,
-            "velocityY": -2.5849475965623756,
-            "timestamp": 2.1523644167715026
-        },
-        {
-            "x": 7.015501922342544,
-            "y": 3.3977895885157845,
-            "heading": -0.4829696659490819,
-            "angularVelocity": 0.0000032906208164279545,
-            "velocityX": 2.676783798347444,
-            "velocityY": -2.673097066136968,
-            "timestamp": 2.1670753554928206
-        },
-        {
-            "x": 7.053586816421239,
-            "y": 3.357212172853621,
-            "heading": -0.4829696191989941,
-            "angularVelocity": 0.000003177913298539426,
-            "velocityX": 2.5888826539333314,
-            "velocityY": -2.758315864871489,
-            "timestamp": 2.1817862942141386
-        },
-        {
-            "x": 7.090341266441407,
-            "y": 3.315425855846777,
-            "heading": -0.48296957381059136,
-            "angularVelocity": 0.0000030853505387280562,
-            "velocityX": 2.4984435539051053,
-            "velocityY": -2.8404929011286835,
-            "timestamp": 2.1964972329354566
-        },
-        {
-            "x": 7.125730460448936,
-            "y": 3.2724771552259067,
-            "heading": -0.48296952951403316,
-            "angularVelocity": 0.000003011130632666104,
-            "velocityX": 2.405638054643597,
-            "velocityY": -2.9195078189422343,
-            "timestamp": 2.2112081716567746
-        },
-        {
-            "x": 7.1597225937186835,
-            "y": 3.228414468562161,
-            "heading": -0.48296947336189594,
-            "angularVelocity": 0.000003817032908672554,
-            "velocityX": 2.3106705774318574,
-            "velocityY": -2.9952328330951756,
-            "timestamp": 2.2259191103780926
-        },
-        {
-            "x": 7.192119389825632,
-            "y": 3.1836076694729414,
-            "heading": -0.4820443638475483,
-            "angularVelocity": 0.06288582475110666,
-            "velocityX": 2.2022249375566023,
-            "velocityY": -3.0458150861769275,
-            "timestamp": 2.2406300490994107
-        },
-        {
-            "x": 7.223229630823693,
-            "y": 3.1396490395754038,
-            "heading": -0.47743685492285376,
-            "angularVelocity": 0.3132029173651504,
-            "velocityX": 2.11476926030427,
-            "velocityY": -2.988159405071387,
-            "timestamp": 2.2553409878207287
-        },
-        {
-            "x": 7.253205323915578,
-            "y": 3.0967066455951215,
-            "heading": -0.4690721734359265,
-            "angularVelocity": 0.5686028366637977,
-            "velocityX": 2.0376465200313345,
-            "velocityY": -2.9190791147849615,
-            "timestamp": 2.2700519265420467
-        },
-        {
-            "x": 7.28210093319025,
-            "y": 3.0548329265465135,
-            "heading": -0.45697001578434165,
-            "angularVelocity": 0.8226638612835941,
-            "velocityX": 1.9642260648397363,
-            "velocityY": -2.8464341971547444,
-            "timestamp": 2.2847628652633647
-        },
-        {
-            "x": 7.309941019631162,
-            "y": 3.0140523923628573,
-            "heading": -0.44116512849373885,
-            "angularVelocity": 1.0743629342769347,
-            "velocityX": 1.892475182468747,
-            "velocityY": -2.7721231769227326,
-            "timestamp": 2.2994738039846827
-        },
-        {
-            "x": 7.336737361434823,
-            "y": 2.9743781069216433,
-            "heading": -0.42169586611687776,
-            "angularVelocity": 1.3234547941286585,
-            "velocityX": 1.8215249421732278,
-            "velocityY": -2.696924118358335,
-            "timestamp": 2.3141847427060007
-        },
-        {
-            "x": 7.362502163456035,
-            "y": 2.936038971640856,
-            "heading": -0.39992762164634665,
-            "angularVelocity": 1.479731843283817,
-            "velocityX": 1.7514043467447793,
-            "velocityY": -2.606165113394762,
-            "timestamp": 2.3288956814273187
-        },
-        {
-            "x": 7.3872337303102205,
-            "y": 2.8991268417513614,
-            "heading": -0.37676830696947144,
-            "angularVelocity": 1.5742921043722566,
-            "velocityX": 1.6811685047907687,
-            "velocityY": -2.5091620996289765,
-            "timestamp": 2.3436066201486367
-        },
-        {
-            "x": 7.410930819346905,
-            "y": 2.863676250815302,
-            "heading": -0.3528129850042874,
-            "angularVelocity": 1.6284019952084616,
-            "velocityX": 1.6108481916483202,
-            "velocityY": -2.4098116107768903,
-            "timestamp": 2.3583175588699548
-        },
-        {
-            "x": 7.433596368468817,
-            "y": 2.8297026440026514,
-            "heading": -0.328491545307115,
-            "angularVelocity": 1.6532894438563033,
-            "velocityX": 1.5407275872250867,
-            "velocityY": -2.3094112113606737,
-            "timestamp": 2.3730284975912728
-        },
-        {
-            "x": 7.455235422231159,
-            "y": 2.7972130374670496,
-            "heading": -0.30412705199668255,
-            "angularVelocity": 1.6562160832826247,
-            "velocityX": 1.4709498946511312,
-            "velocityY": -2.208533877482683,
-            "timestamp": 2.387739436312591
-        },
-        {
-            "x": 7.475853646381716,
-            "y": 2.766210258525358,
-            "heading": -0.2799689621545816,
-            "angularVelocity": 1.6421854716240782,
-            "velocityX": 1.4015573405031783,
-            "velocityY": -2.1074643521399103,
-            "timestamp": 2.402450375033909
-        },
-        {
-            "x": 7.495456618827617,
-            "y": 2.736694950498885,
-            "heading": -0.2562142168141467,
-            "angularVelocity": 1.6147674727249637,
-            "velocityX": 1.332543953669861,
-            "velocityY": -2.006351095984268,
-            "timestamp": 2.417161313755227
-        },
-        {
-            "x": 7.514049552575163,
-            "y": 2.7086666100983954,
-            "heading": -0.23302134486774898,
-            "angularVelocity": 1.576573214378762,
-            "velocityX": 1.2638849294234806,
-            "velocityY": -1.9052720517334563,
-            "timestamp": 2.431872252476545
-        },
-        {
-            "x": 7.5316372223467996,
-            "y": 2.682124150428342,
-            "heading": -0.2105202201245662,
-            "angularVelocity": 1.5295505724985052,
-            "velocityX": 1.1955504747055303,
-            "velocityY": -1.804266890976241,
-            "timestamp": 2.446583191197863
-        },
-        {
-            "x": 7.5482239779420395,
-            "y": 2.6570662133282936,
-            "heading": -0.18881898554249216,
-            "angularVelocity": 1.4751767370647726,
-            "velocityX": 1.127511704688336,
-            "velocityY": -1.7033540533846026,
-            "timestamp": 2.461294129919181
-        },
-        {
-            "x": 7.563813788966016,
-            "y": 2.6334913430066904,
-            "heading": -0.16800906628713844,
-            "angularVelocity": 1.414588127214289,
-            "velocityX": 1.0597427750401904,
-            "velocityY": -1.602540175593351,
-            "timestamp": 2.476005068640499
-        },
-        {
-            "x": 7.5784102960217234,
-            "y": 2.611398080774854,
-            "heading": -0.1481688563613113,
-            "angularVelocity": 1.3486705574455213,
-            "velocityX": 0.9922213213052737,
-            "velocityY": -1.5018254545389353,
-            "timestamp": 2.490716007361817
-        },
-        {
-            "x": 7.5920168580101715,
-            "y": 2.5907850143215074,
-            "heading": -0.1293664624719475,
-            "angularVelocity": 1.2781233234366292,
-            "velocityX": 0.9249281943327172,
-            "velocityY": -1.401206737641836,
-            "timestamp": 2.505426946083135
-        },
-        {
-            "x": 7.604636591974509,
-            "y": 2.571650800836196,
-            "heading": -0.1116617644111604,
-            "angularVelocity": 1.2035056631111207,
-            "velocityX": 0.8578469534407521,
-            "velocityY": -1.3006793004707733,
-            "timestamp": 2.520137884804453
-        },
-        {
-            "x": 7.616272404986451,
-            "y": 2.5539941753706046,
-            "heading": -0.09510797189421191,
-            "angularVelocity": 1.1252709857977845,
-            "velocityX": 0.790963325479677,
-            "velocityY": -1.20023785021998,
-            "timestamp": 2.534848823525771
-        },
-        {
-            "x": 7.626927018849464,
-            "y": 2.537813951229366,
-            "heading": -0.07975280576069126,
-            "angularVelocity": 1.0437924067530058,
-            "velocityX": 0.7242647165386202,
-            "velocityY": -1.0998770675179592,
-            "timestamp": 2.549559762247089
-        },
-        {
-            "x": 7.636602988821212,
-            "y": 2.523109016437205,
-            "heading": -0.0656393963876239,
-            "angularVelocity": 0.9593819701400241,
-            "velocityX": 0.6577398053956987,
-            "velocityY": -0.9995918731448548,
-            "timestamp": 2.564270700968407
-        },
-        {
-            "x": 7.64530271759392,
-            "y": 2.509878328655525,
-            "heading": -0.05280696792868869,
-            "angularVelocity": 0.8723052078477697,
-            "velocityX": 0.5913782211668082,
-            "velocityY": -0.899377533434203,
-            "timestamp": 2.578981639689725
-        },
-        {
-            "x": 7.653028465638572,
-            "y": 2.498120909886367,
-            "heading": -0.04129135982777762,
-            "angularVelocity": 0.7827922010322448,
-            "velocityX": 0.5251702961317726,
-            "velocityY": -0.799229674726393,
-            "timestamp": 2.593692578411043
-        },
-        {
-            "x": 7.6597823588265515,
-            "y": 2.48783584165749,
-            "heading": -0.03112542462311605,
-            "angularVelocity": 0.6910459894669937,
-            "velocityX": 0.4591068806636599,
-            "velocityY": -0.6991442506638482,
-            "timestamp": 2.608403517132361
-        },
-        {
-            "x": 7.665566394043072,
-            "y": 2.4790222609826817,
-            "heading": -0.02233933188139214,
-            "angularVelocity": 0.5972489525084974,
-            "velocityX": 0.39317920671771145,
-            "velocityY": -0.5991174894934748,
-            "timestamp": 2.623114455853679
-        },
-        {
-            "x": 7.670382443324971,
-            "y": 2.471679357147111,
-            "heading": -0.014960801225629315,
-            "angularVelocity": 0.5015676290643761,
-            "velocityX": 0.3273787875222405,
-            "velocityY": -0.4991458379831521,
-            "timestamp": 2.637825394574997
-        },
-        {
-            "x": 7.674232256901065,
-            "y": 2.4658063692226495,
-            "heading": -0.00901528221740954,
-            "angularVelocity": 0.40415633025538694,
-            "velocityX": 0.2616973429788216,
-            "velocityY": -0.399225911800697,
-            "timestamp": 2.652536333296315
-        },
-        {
-            "x": 7.677117465389097,
-            "y": 2.461402584136771,
-            "heading": -0.004526094899743595,
-            "angularVelocity": 0.30515981357196037,
-            "velocityX": 0.19612674233020058,
-            "velocityY": -0.2993544578835629,
-            "timestamp": 2.667247272017633
-        },
-        {
-            "x": 7.679039581308467,
-            "y": 2.45846733507745,
-            "heading": -0.001514541807715108,
-            "angularVelocity": 0.20471522239871504,
-            "velocityX": 0.13065895764927307,
-            "velocityY": -0.1995283315991124,
-            "timestamp": 2.681958210738951
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 7.235538726171091e-30,
-            "angularVelocity": 0.10295344412796348,
-            "velocityX": 0.06528602353168424,
-            "velocityY": -0.09974448981448966,
-            "timestamp": 2.696669149460269
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 1.9723672087239994e-30,
-            "angularVelocity": -3.5777263674767652e-28,
-            "velocityX": 6.106461964279306e-29,
-            "velocityY": 3.383375524617378e-29,
-            "timestamp": 2.711380088181587
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.traj
deleted file mode 100644
index 0d7f85e3..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.traj	
+++ /dev/null
@@ -1,1453 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 3.439885530040909e-30,
-            "timestamp": 0
-        },
-        {
-            "x": 1.3518715996750759,
-            "y": 5.562488632849977,
-            "heading": -0.00039025483625088287,
-            "angularVelocity": -0.020188245249371933,
-            "velocityX": 0.09681959001984766,
-            "velocityY": 0.12873928941829918,
-            "timestamp": 0.019330795293514086
-        },
-        {
-            "x": 1.3556369831983786,
-            "y": 5.567448998179342,
-            "heading": -0.001175615506548702,
-            "angularVelocity": -0.04062743712160119,
-            "velocityX": 0.19478678792724713,
-            "velocityY": 0.256604306964468,
-            "timestamp": 0.03866159058702817
-        },
-        {
-            "x": 1.3613203761105706,
-            "y": 5.574862277415471,
-            "heading": -0.0023613813379046128,
-            "angularVelocity": -0.061340768103509684,
-            "velocityX": 0.2940071955600798,
-            "velocityY": 0.38349582226532697,
-            "timestamp": 0.05799238588054226
-        },
-        {
-            "x": 1.3689483300707626,
-            "y": 5.58470739953387,
-            "heading": -0.003953362765501964,
-            "angularVelocity": -0.08235467829569511,
-            "velocityX": 0.39460114518679135,
-            "velocityY": 0.5092973138928011,
-            "timestamp": 0.07732318117405634
-        },
-        {
-            "x": 1.3785500603498269,
-            "y": 5.596960627127346,
-            "heading": -0.0059579558445008625,
-            "angularVelocity": -0.10369946236363117,
-            "velocityX": 0.4967064279184545,
-            "velocityY": 0.6338708473927627,
-            "timestamp": 0.09665397646757043
-        },
-        {
-            "x": 1.3901578478597558,
-            "y": 5.611595038412612,
-            "heading": -0.008382231168191269,
-            "angularVelocity": -0.12541001479146147,
-            "velocityX": 0.6004816322183893,
-            "velocityY": 0.7570516920313731,
-            "timestamp": 0.11598477176108452
-        },
-        {
-            "x": 1.4038075203372553,
-            "y": 5.62857987161075,
-            "heading": -0.01123404053815137,
-            "angularVelocity": -0.14752674820972528,
-            "velocityX": 0.7061102386242472,
-            "velocityY": 0.8786412012669059,
-            "timestamp": 0.1353155670545986
-        },
-        {
-            "x": 1.4195390306280726,
-            "y": 5.647879684835849,
-            "heading": -0.014522145551621221,
-            "angularVelocity": -0.17009672719326588,
-            "velocityX": 0.8138056428591633,
-            "velocityY": 0.9983972688167116,
-            "timestamp": 0.1546463623481127
-        },
-        {
-            "x": 1.4373971536587393,
-            "y": 5.669453265066441,
-            "heading": -0.018256373225668265,
-            "angularVelocity": -0.19317506689959266,
-            "velocityX": 0.923817295642181,
-            "velocityY": 1.116021348476537,
-            "timestamp": 0.17397715764162677
-        },
-        {
-            "x": 1.457432326992564,
-            "y": 5.693252190583062,
-            "heading": -0.022447804782031403,
-            "angularVelocity": -0.21682664850159125,
-            "velocityX": 1.0364381304346626,
-            "velocityY": 1.2311405276019616,
-            "timestamp": 0.19330795293514086
-        },
-        {
-            "x": 1.47970166115617,
-            "y": 5.719218907072145,
-            "heading": -0.027109004507978946,
-            "angularVelocity": -0.24112819235695526,
-            "velocityX": 1.1520133458284385,
-            "velocityY": 1.3432823686149544,
-            "timestamp": 0.21263874822865494
-        },
-        {
-            "x": 1.5042701411060604,
-            "y": 5.747284109919373,
-            "heading": -0.03225429550223853,
-            "angularVelocity": -0.2661706834165162,
-            "velocityX": 1.2709502933970824,
-            "velocityY": 1.4518390175412745,
-            "timestamp": 0.23196954352216903
-        },
-        {
-            "x": 1.5312120197646597,
-            "y": 5.777363120811461,
-            "heading": -0.037900086436595266,
-            "angularVelocity": -0.2920620103121596,
-            "velocityX": 1.3937284136281272,
-            "velocityY": 1.5560151786502046,
-            "timestamp": 0.25130033881568314
-        },
-        {
-            "x": 1.5606123466917503,
-            "y": 5.809350786193417,
-            "heading": -0.044065244214455006,
-            "angularVelocity": -0.3189293396494718,
-            "velocityX": 1.5209062266028461,
-            "velocityY": 1.6547516486654796,
-            "timestamp": 0.27063113410919726
-        },
-        {
-            "x": 1.5925684302904153,
-            "y": 5.843114184319679,
-            "heading": -0.050771482089097104,
-            "angularVelocity": -0.34691991575183656,
-            "velocityX": 1.653117893674399,
-            "velocityY": 1.7466119532904336,
-            "timestamp": 0.28996192940271137
-        },
-        {
-            "x": 1.6271906907680806,
-            "y": 5.8784820940308995,
-            "heading": -0.058043668844819325,
-            "angularVelocity": -0.37619697717051565,
-            "velocityX": 1.791041700663091,
-            "velocityY": 1.8296148282676763,
-            "timestamp": 0.3092927246962255
-        },
-        {
-            "x": 1.6646015916598045,
-            "y": 5.915229821006168,
-            "heading": -0.06590980894370155,
-            "angularVelocity": -0.4069227354304089,
-            "velocityX": 1.935300660096276,
-            "velocityY": 1.9009940572697726,
-            "timestamp": 0.3286235199897396
-        },
-        {
-            "x": 1.7049297157110095,
-            "y": 5.953057987948439,
-            "heading": -0.07440007857488001,
-            "angularVelocity": -0.43920953598980744,
-            "velocityX": 2.086211324411271,
-            "velocityY": 1.9568862205562443,
-            "timestamp": 0.3479543152832537
-        },
-        {
-            "x": 1.7482931222517035,
-            "y": 5.991565656180575,
-            "heading": -0.08354352935308812,
-            "angularVelocity": -0.47299920357005454,
-            "velocityX": 2.2432293075517364,
-            "velocityY": 1.9920374535783947,
-            "timestamp": 0.3672851105767678
-        },
-        {
-            "x": 1.7947629094651933,
-            "y": 6.030225122456552,
-            "heading": -0.09335994332155541,
-            "angularVelocity": -0.5078122146252688,
-            "velocityX": 2.4039252657691588,
-            "velocityY": 1.9998901074156783,
-            "timestamp": 0.38661590587028194
-        },
-        {
-            "x": 1.844301928458138,
-            "y": 6.068381679285448,
-            "heading": -0.10384499839936924,
-            "angularVelocity": -0.5424016404194022,
-            "velocityX": 2.562699477220483,
-            "velocityY": 1.9738741344852324,
-            "timestamp": 0.40594670116379605
-        },
-        {
-            "x": 1.8967008920878712,
-            "y": 6.105314232951604,
-            "heading": -0.1149551002367539,
-            "angularVelocity": -0.5747358899978738,
-            "velocityX": 2.710647070341413,
-            "velocityY": 1.9105553137044158,
-            "timestamp": 0.42527749645731017
-        },
-        {
-            "x": 1.951571944971935,
-            "y": 6.140356350737874,
-            "heading": -0.126607814812745,
-            "angularVelocity": -0.6028057510857173,
-            "velocityX": 2.83853054418689,
-            "velocityY": 1.8127613093097652,
-            "timestamp": 0.4446082917508243
-        },
-        {
-            "x": 2.008427579479744,
-            "y": 6.172999646985284,
-            "heading": -0.13870025114422718,
-            "angularVelocity": -0.6255529660251172,
-            "velocityX": 2.9411947953783932,
-            "velocityY": 1.6886680424557017,
-            "timestamp": 0.4639390870443384
-        },
-        {
-            "x": 2.066782288063465,
-            "y": 6.202913115188225,
-            "heading": -0.15112974078850008,
-            "angularVelocity": -0.6429890470384952,
-            "velocityX": 3.018743290054933,
-            "velocityY": 1.5474515015415655,
-            "timestamp": 0.4832698823378525
-        },
-        {
-            "x": 2.126212031523665,
-            "y": 6.229902950259444,
-            "heading": -0.1638060412217236,
-            "angularVelocity": -0.6557567984529158,
-            "velocityX": 3.074355842986947,
-            "velocityY": 1.3962092434072775,
-            "timestamp": 0.5026006776313666
-        },
-        {
-            "x": 2.1863688602207922,
-            "y": 6.253863406934038,
-            "heading": -0.17665473442964857,
-            "angularVelocity": -0.6646748368514358,
-            "velocityX": 3.111968637798953,
-            "velocityY": 1.2394966844759612,
-            "timestamp": 0.5219314729248807
-        },
-        {
-            "x": 2.246973575487724,
-            "y": 6.2747405977079325,
-            "heading": -0.1896159238006329,
-            "angularVelocity": -0.6704943678821678,
-            "velocityX": 3.1351382261683427,
-            "velocityY": 1.0799964748940398,
-            "timestamp": 0.5412622682183948
-        },
-        {
-            "x": 2.30780256528041,
-            "y": 6.292510136082229,
-            "heading": -0.20264154258755296,
-            "angularVelocity": -0.6738273614273119,
-            "velocityX": 3.146740155750102,
-            "velocityY": 0.9192347290676759,
-            "timestamp": 0.560593063511909
-        },
-        {
-            "x": 2.3686753728703707,
-            "y": 6.307164203375093,
-            "heading": -0.2156927432279827,
-            "angularVelocity": -0.6751507344764429,
-            "velocityX": 3.1490068911124522,
-            "velocityY": 0.7580685155659881,
-            "timestamp": 0.5799238588054231
-        },
-        {
-            "x": 2.4294446969864714,
-            "y": 6.318704202261767,
-            "heading": -0.228737776875863,
-            "angularVelocity": -0.6748317102223548,
-            "velocityX": 3.143653594867338,
-            "velocityY": 0.5969748637576952,
-            "timestamp": 0.5992546540989372
-        },
-        {
-            "x": 2.4899887888473393,
-            "y": 6.32713657767754,
-            "heading": -0.24175037423147705,
-            "angularVelocity": -0.6731537506881662,
-            "velocityX": 3.132002121049924,
-            "velocityY": 0.43621461443967424,
-            "timestamp": 0.6185854493924513
-        },
-        {
-            "x": 2.550205774126716,
-            "y": 6.332470420209207,
-            "heading": -0.25470853343764965,
-            "angularVelocity": -0.6703376146412596,
-            "velocityX": 3.1150805936877464,
-            "velocityY": 0.2759246296222352,
-            "timestamp": 0.6379162446859654
-        },
-        {
-            "x": 2.610009417191064,
-            "y": 6.334716080800322,
-            "heading": -0.26759361438505785,
-            "angularVelocity": -0.6665572084213006,
-            "velocityX": 3.09369801688466,
-            "velocityY": 0.11617010873154458,
-            "timestamp": 0.6572470399794795
-        },
-        {
-            "x": 2.669325940254228,
-            "y": 6.333884367249437,
-            "heading": -0.28038965768785973,
-            "angularVelocity": -0.6619512083445117,
-            "velocityX": 3.0684988466597254,
-            "velocityY": -0.04302531469893782,
-            "timestamp": 0.6765778352729936
-        },
-        {
-            "x": 2.7280916104140402,
-            "y": 6.329986080406231,
-            "heading": -0.2930828674707726,
-            "angularVelocity": -0.656631534822156,
-            "velocityX": 3.040002714194029,
-            "velocityY": -0.2016620001410218,
-            "timestamp": 0.6959086305665078
-        },
-        {
-            "x": 2.7862508879843872,
-            "y": 6.323031751356315,
-            "heading": -0.3056612140555153,
-            "angularVelocity": -0.650689554866023,
-            "velocityX": 3.0086334621659443,
-            "velocityY": -0.35975390274029156,
-            "timestamp": 0.7152394258600219
-        },
-        {
-            "x": 2.843754988622848,
-            "y": 6.3130314987287885,
-            "heading": -0.31811412514136855,
-            "angularVelocity": -0.6442006599713835,
-            "velocityX": 2.974740550780866,
-            "velocityY": -0.5173223592555559,
-            "timestamp": 0.734570221153536
-        },
-        {
-            "x": 2.9005607537314586,
-            "y": 6.299994958196607,
-            "heading": -0.3304322429824912,
-            "angularVelocity": -0.6372276801904713,
-            "velocityX": 2.9386150050261657,
-            "velocityY": -0.6743923534566658,
-            "timestamp": 0.7539010164470501
-        },
-        {
-            "x": 2.956629753112793,
-            "y": 6.283931255340576,
-            "heading": -0.34260723133624194,
-            "angularVelocity": -0.6298234588328613,
-            "velocityX": 2.9005014294547,
-            "velocityY": -0.8309902728844937,
-            "timestamp": 0.7732318117405642
-        },
-        {
-            "x": 2.9906169983281425,
-            "y": 6.272983358953205,
-            "heading": -0.34999362222052477,
-            "angularVelocity": -0.625060967472129,
-            "velocityX": 2.8761137487624757,
-            "velocityY": -0.9264474104986867,
-            "timestamp": 0.7850488844241373
-        },
-        {
-            "x": 3.024312445989771,
-            "y": 6.26090838895792,
-            "heading": -0.35731821138357023,
-            "angularVelocity": -0.6198311002374715,
-            "velocityX": 2.8514208691014065,
-            "velocityY": -1.0218241284130187,
-            "timestamp": 0.7968659571077105
-        },
-        {
-            "x": 3.0577122238990846,
-            "y": 6.2477073818247915,
-            "heading": -0.36457506252100974,
-            "angularVelocity": -0.6140988831800235,
-            "velocityX": 2.8264003111145226,
-            "velocityY": -1.1171131367820717,
-            "timestamp": 0.8086830297912836
-        },
-        {
-            "x": 3.090812163934883,
-            "y": 6.233381471982772,
-            "heading": -0.3717577779780659,
-            "angularVelocity": -0.6078252752934934,
-            "velocityX": 2.8010270328463904,
-            "velocityY": -1.2123061459996218,
-            "timestamp": 0.8205001024748567
-        },
-        {
-            "x": 3.1236077670194575,
-            "y": 6.217931905831193,
-            "heading": -0.3788594429566703,
-            "angularVelocity": -0.6009665141923228,
-            "velocityX": 2.7752730276563167,
-            "velocityY": -1.307393680759415,
-            "timestamp": 0.8323171751584298
-        },
-        {
-            "x": 3.1560941623709735,
-            "y": 6.201360058471141,
-            "heading": -0.3858725603738981,
-            "angularVelocity": -0.5934733249949906,
-            "velocityX": 2.749106840704776,
-            "velocityY": -1.4023648498911914,
-            "timestamp": 0.844134247842003
-        },
-        {
-            "x": 3.188266059838492,
-            "y": 6.1836674538325775,
-            "heading": -0.3927889743327873,
-            "angularVelocity": -0.5852899566661331,
-            "velocityX": 2.722492983583009,
-            "velocityY": -1.4972070590002187,
-            "timestamp": 0.8559513205255761
-        },
-        {
-            "x": 3.2201176938016274,
-            "y": 6.164855789080512,
-            "heading": -0.399599779609834,
-            "angularVelocity": -0.576352998701121,
-            "velocityX": 2.695391220484986,
-            "velocityY": -1.5919056483603757,
-            "timestamp": 0.8677683932091492
-        },
-        {
-            "x": 3.251642756703662,
-            "y": 6.144926964467571,
-            "heading": -0.40629521382129,
-            "angularVelocity": -0.5665899153487628,
-            "velocityX": 2.6677556909553144,
-            "velocityY": -1.686443432022221,
-            "timestamp": 0.8795854658927224
-        },
-        {
-            "x": 3.282834319738247,
-            "y": 6.123883120194607,
-            "heading": -0.41286452792903033,
-            "angularVelocity": -0.5559172126335613,
-            "velocityX": 2.6395338227837533,
-            "velocityY": -1.7808001047684463,
-            "timestamp": 0.8914025385762955
-        },
-        {
-            "x": 3.3136847374734915,
-            "y": 6.101726682396313,
-            "heading": -0.4192958293777808,
-            "angularVelocity": -0.5442381223304499,
-            "velocityX": 2.6106649727330486,
-            "velocityY": -1.874951469926556,
-            "timestamp": 0.9032196112598686
-        },
-        {
-            "x": 3.3441855321977045,
-            "y": 6.078460421163092,
-            "heading": -0.42557589025707343,
-            "angularVelocity": -0.5314396422408687,
-            "velocityX": 2.5810787105179256,
-            "velocityY": -1.9688684208198879,
-            "timestamp": 0.9150366839434417
-        },
-        {
-            "x": 3.37432725239814,
-            "y": 6.0540875246663015,
-            "heading": -0.4316899102075926,
-            "angularVelocity": -0.5173887065125765,
-            "velocityX": 2.5506926298537165,
-            "velocityY": -2.0625155780475546,
-            "timestamp": 0.9268537566270149
-        },
-        {
-            "x": 3.4040992978747684,
-            "y": 6.028611695167346,
-            "heading": -0.43762121996231873,
-            "angularVelocity": -0.5019271619587302,
-            "velocityX": 2.519409525001445,
-            "velocityY": -2.1558494375996973,
-            "timestamp": 0.938670829310588
-        },
-        {
-            "x": 3.4334897013019283,
-            "y": 6.00203727528326,
-            "heading": -0.44335090582849124,
-            "angularVelocity": -0.4848650778070557,
-            "velocityX": 2.48711370524239,
-            "velocityY": -2.2488158104525207,
-            "timestamp": 0.9504879019941611
-        },
-        {
-            "x": 3.462484852215012,
-            "y": 5.974369416891247,
-            "heading": -0.4488573271132443,
-            "angularVelocity": -0.46597168623725893,
-            "velocityX": 2.4536661226929253,
-            "velocityY": -2.341346214318684,
-            "timestamp": 0.9623049746777342
-        },
-        {
-            "x": 3.4910691438628887,
-            "y": 5.945614311410449,
-            "heading": -0.45411548589649775,
-            "angularVelocity": -0.4449628875146618,
-            "velocityX": 2.4188978449470153,
-            "velocityY": -2.4333526797013256,
-            "timestamp": 0.9741220473613074
-        },
-        {
-            "x": 3.5192245152965964,
-            "y": 5.915779510545288,
-            "heading": -0.4590961890390477,
-            "angularVelocity": -0.42148366824158007,
-            "velocityX": 2.3826011896200363,
-            "velocityY": -2.524720094735007,
-            "timestamp": 0.9859391200448805
-        },
-        {
-            "x": 3.5469298492407,
-            "y": 5.884874383909108,
-            "heading": -0.46376491150225324,
-            "angularVelocity": -0.39508282535110545,
-            "velocityX": 2.3445175201991417,
-            "velocityY": -2.6152946219194915,
-            "timestamp": 0.9977561927284536
-        },
-        {
-            "x": 3.5741601690568516,
-            "y": 5.852910789899007,
-            "heading": -0.46808022074004524,
-            "angularVelocity": -0.36517582258680764,
-            "velocityX": 2.304320244556386,
-            "velocityY": -2.7048656520944543,
-            "timestamp": 1.0095732654120266
-        },
-        {
-            "x": 3.6008855536785225,
-            "y": 5.819904089620565,
-            "heading": -0.4719915430906053,
-            "angularVelocity": -0.3309891083260508,
-            "velocityX": 2.2615909487314654,
-            "velocityY": -2.7931367744165776,
-            "timestamp": 1.0213903380955998
-        },
-        {
-            "x": 3.62706965768041,
-            "y": 5.785874731899473,
-            "heading": -0.4754359323425916,
-            "angularVelocity": -0.2914756762708573,
-            "velocityX": 2.2157859821143067,
-            "velocityY": -2.8796774490856496,
-            "timestamp": 1.0332074107791729
-        },
-        {
-            "x": 3.652667693323084,
-            "y": 5.750850822222342,
-            "heading": -0.4783333470461038,
-            "angularVelocity": -0.24518887046705806,
-            "velocityX": 2.1661909280000957,
-            "velocityY": -2.9638397439848143,
-            "timestamp": 1.045024483462746
-        },
-        {
-            "x": 3.677623743737937,
-            "y": 5.7148724362586085,
-            "heading": -0.48057991095642455,
-            "angularVelocity": -0.1901117112907446,
-            "velocityX": 2.1118640024567648,
-            "velocityY": -3.044610702424323,
-            "timestamp": 1.0568415561463191
-        },
-        {
-            "x": 3.701867477642102,
-            "y": 5.67799904418439,
-            "heading": -0.48203966919964636,
-            "angularVelocity": -0.12352959843022769,
-            "velocityX": 2.0515854097999866,
-            "velocityY": -3.120349096733213,
-            "timestamp": 1.0686586288298923
-        },
-        {
-            "x": 3.725329217347855,
-            "y": 5.640287080856699,
-            "heading": -0.4827696693750387,
-            "angularVelocity": -0.06177504318881954,
-            "velocityX": 1.9854104594250916,
-            "velocityY": -3.191311785711063,
-            "timestamp": 1.0804757015134654
-        },
-        {
-            "x": 3.74792659306077,
-            "y": 5.60180905514361,
-            "heading": -0.48297166201638797,
-            "angularVelocity": -0.017093289239954308,
-            "velocityX": 1.9122651030425624,
-            "velocityY": -3.2561385330714976,
-            "timestamp": 1.0922927741970385
-        },
-        {
-            "x": 3.769555777117098,
-            "y": 5.56268673154527,
-            "heading": -0.48297173035274543,
-            "angularVelocity": -0.000005782849890954889,
-            "velocityX": 1.8303335043707702,
-            "velocityY": -3.310661163379628,
-            "timestamp": 1.1041098468806116
-        },
-        {
-            "x": 3.7901587344932506,
-            "y": 5.52301434523992,
-            "heading": -0.48297173483145855,
-            "angularVelocity": -3.790036003454015e-7,
-            "velocityX": 1.743490788949144,
-            "velocityY": -3.3572092994315077,
-            "timestamp": 1.1159269195641848
-        },
-        {
-            "x": 3.809721481337777,
-            "y": 5.482818833345491,
-            "heading": -0.4829717389588181,
-            "angularVelocity": -3.4927089311686727e-7,
-            "velocityX": 1.6554647135005363,
-            "velocityY": -3.401477926957743,
-            "timestamp": 1.127743992247758
-        },
-        {
-            "x": 3.8282308263038667,
-            "y": 5.442127501029994,
-            "heading": -0.482971742791846,
-            "angularVelocity": -3.2436357183409427e-7,
-            "velocityX": 1.5663223424036268,
-            "velocityY": -3.443435900336205,
-            "timestamp": 1.139561064931331
-        },
-        {
-            "x": 3.8456768429335884,
-            "y": 5.400969063215312,
-            "heading": -0.48297174636762497,
-            "angularVelocity": -3.0259430956931755e-7,
-            "velocityX": 1.4763399614162735,
-            "velocityY": -3.482963921504598,
-            "timestamp": 1.1513781376149042
-        },
-        {
-            "x": 3.863665898181474,
-            "y": 5.360045056052632,
-            "heading": -0.48297175000247344,
-            "angularVelocity": -3.075929693793739e-7,
-            "velocityX": 1.5222936957043627,
-            "velocityY": -3.463125619898205,
-            "timestamp": 1.1631952102984773
-        },
-        {
-            "x": 3.8827072670006824,
-            "y": 5.319599949808908,
-            "heading": -0.4829717539017329,
-            "angularVelocity": -3.299683104273868e-7,
-            "velocityX": 1.6113439706329509,
-            "velocityY": -3.4225994310711667,
-            "timestamp": 1.1750122829820504
-        },
-        {
-            "x": 3.902794276163034,
-            "y": 5.279663847505986,
-            "heading": -0.48297175811002735,
-            "angularVelocity": -3.5611987502134074e-7,
-            "velocityX": 1.6998295347945822,
-            "velocityY": -3.379525824397886,
-            "timestamp": 1.1868293556656235
-        },
-        {
-            "x": 3.92391456197569,
-            "y": 5.240264436686968,
-            "heading": -0.48297176267932185,
-            "angularVelocity": -3.8666890356003695e-7,
-            "velocityX": 1.78726884214022,
-            "velocityY": -3.3341092057245874,
-            "timestamp": 1.1986464283491967
-        },
-        {
-            "x": 3.9460538007835475,
-            "y": 5.201428448582004,
-            "heading": -0.4829717676728866,
-            "angularVelocity": -4.225720568590589e-7,
-            "velocityX": 1.8734960341433124,
-            "velocityY": -3.286430501434571,
-            "timestamp": 1.2104635010327698
-        },
-        {
-            "x": 3.969196975663963,
-            "y": 5.163182231498957,
-            "heading": -0.4829717731698369,
-            "angularVelocity": -4.651702214495778e-7,
-            "velocityX": 1.9584524441985565,
-            "velocityY": -3.2365221156854496,
-            "timestamp": 1.222280573716343
-        },
-        {
-            "x": 3.993328387583419,
-            "y": 5.125551733327025,
-            "heading": -0.48297177927069973,
-            "angularVelocity": -5.162753076431712e-7,
-            "velocityX": 2.0420803498146496,
-            "velocityY": -3.184417933236803,
-            "timestamp": 1.234097646399916
-        },
-        {
-            "x": 4.018431663513184,
-            "y": 5.088562488555908,
-            "heading": -0.4829717875114574,
-            "angularVelocity": -6.973603296302728e-7,
-            "velocityX": 2.124322715274553,
-            "velocityY": -3.1301529373289303,
-            "timestamp": 1.2459147190834892
-        },
-        {
-            "x": 4.068270320597518,
-            "y": 5.022262107474161,
-            "heading": -0.48297170333499495,
-            "angularVelocity": 0.000003839169547390073,
-            "velocityX": 2.2730707479901486,
-            "velocityY": -3.023866725833157,
-            "timestamp": 1.2678404145896236
-        },
-        {
-            "x": 4.121253956507243,
-            "y": 4.9584470041490425,
-            "heading": -0.48297162876403243,
-            "angularVelocity": 0.0000034010762602073378,
-            "velocityX": 2.4165087896481814,
-            "velocityY": -2.910516718033651,
-            "timestamp": 1.289766110095758
-        },
-        {
-            "x": 4.177258824192737,
-            "y": 4.89726621863139,
-            "heading": -0.4829715614847619,
-            "angularVelocity": 0.0000030685124925835,
-            "velocityX": 2.5543029031769633,
-            "velocityY": -2.7903692040481487,
-            "timestamp": 1.3116918056018925
-        },
-        {
-            "x": 4.236154120064876,
-            "y": 4.83886263701252,
-            "heading": -0.482971499836949,
-            "angularVelocity": 0.000002811669663014854,
-            "velocityX": 2.686131249777681,
-            "velocityY": -2.6637048572771733,
-            "timestamp": 1.333617501108027
-        },
-        {
-            "x": 4.2978022891900425,
-            "y": 4.7833726571672255,
-            "heading": -0.48297144258671054,
-            "angularVelocity": 0.0000026111025032599054,
-            "velocityX": 2.811685910164931,
-            "velocityY": -2.530819596111362,
-            "timestamp": 1.3555431966141613
-        },
-        {
-            "x": 4.362059346053497,
-            "y": 4.730925869094327,
-            "heading": -0.48297138878758666,
-            "angularVelocity": 0.0000024537020442790352,
-            "velocityX": 2.9306735946176077,
-            "velocityY": -2.392023918156931,
-            "timestamp": 1.3774688921202958
-        },
-        {
-            "x": 4.428775209886728,
-            "y": 4.681644750011071,
-            "heading": -0.4829713376913353,
-            "angularVelocity": 0.000002330427847533908,
-            "velocityX": 3.042816307221113,
-            "velocityY": -2.247642227334136,
-            "timestamp": 1.3993945876264302
-        },
-        {
-            "x": 4.497794001617253,
-            "y": 4.635644296035039,
-            "heading": -0.4829712886902182,
-            "angularVelocity": 0.000002234871730357886,
-            "velocityX": 3.147849595521449,
-            "velocityY": -2.098015726031637,
-            "timestamp": 1.4213202831325646
-        },
-        {
-            "x": 4.568948026555497,
-            "y": 4.593021059923411,
-            "heading": -0.4829712412701429,
-            "angularVelocity": 0.0000021627626491587156,
-            "velocityX": 3.2452345659144926,
-            "velocityY": -1.9439855898636562,
-            "timestamp": 1.443245978638699
-        },
-        {
-            "x": 4.64205867202195,
-            "y": 4.55384875924605,
-            "heading": -0.4829711949807044,
-            "angularVelocity": 0.000002111195903735364,
-            "velocityX": 3.3344732643018493,
-            "velocityY": -1.7865932994667033,
-            "timestamp": 1.4651716741448335
-        },
-        {
-            "x": 4.71691342103475,
-            "y": 4.5181219873551415,
-            "heading": -0.4829711494130406,
-            "angularVelocity": 0.000002078276777149451,
-            "velocityX": 3.414019363346236,
-            "velocityY": -1.6294475986389252,
-            "timestamp": 1.487097369650968
-        },
-        {
-            "x": 4.7930985438005,
-            "y": 4.485327738304056,
-            "heading": -0.4829711041848689,
-            "angularVelocity": 0.000002062793021590678,
-            "velocityX": 3.4746958309446994,
-            "velocityY": -1.4956993743693363,
-            "timestamp": 1.5090230651571024
-        },
-        {
-            "x": 4.8695687201769084,
-            "y": 4.453203793512952,
-            "heading": -0.4829710590084327,
-            "angularVelocity": 0.0000020604334399265313,
-            "velocityX": 3.48769672346274,
-            "velocityY": -1.4651277439372516,
-            "timestamp": 1.5309487606632368
-        },
-        {
-            "x": 4.94605170532014,
-            "y": 4.421110356684282,
-            "heading": -0.48297101383350444,
-            "angularVelocity": 0.0000020603646626736416,
-            "velocityX": 3.4882809132254033,
-            "velocityY": -1.463736318862417,
-            "timestamp": 1.5528744561693713
-        },
-        {
-            "x": 5.022532332392591,
-            "y": 4.389011300831823,
-            "heading": -0.48297096865831407,
-            "angularVelocity": 0.000002060376618948169,
-            "velocityX": 3.4881733649487248,
-            "velocityY": -1.463992594598631,
-            "timestamp": 1.5748001516755057
-        },
-        {
-            "x": 5.099012556138694,
-            "y": 4.356911284014184,
-            "heading": -0.48297092348307896,
-            "angularVelocity": 0.000002060378658787562,
-            "velocityX": 3.488154969803465,
-            "velocityY": -1.4640364228636573,
-            "timestamp": 1.5967258471816401
-        },
-        {
-            "x": 5.175492855161546,
-            "y": 4.324811446548553,
-            "heading": -0.48297087830785207,
-            "angularVelocity": 0.0000020603782854914656,
-            "velocityX": 3.48815840306965,
-            "velocityY": -1.46402824287194,
-            "timestamp": 1.6186515426877746
-        },
-        {
-            "x": 5.2519731561146505,
-            "y": 4.292711613681912,
-            "heading": -0.48297083313262523,
-            "angularVelocity": 0.0000020603782806948455,
-            "velocityX": 3.488158491105669,
-            "velocityY": -1.464028033118535,
-            "timestamp": 1.640577238193909
-        },
-        {
-            "x": 5.328453454863713,
-            "y": 4.260611775564003,
-            "heading": -0.48297078795739806,
-            "angularVelocity": 0.0000020603782977173096,
-            "velocityX": 3.488158390582454,
-            "velocityY": -1.4640282726214169,
-            "timestamp": 1.6625029337000434
-        },
-        {
-            "x": 5.404933754197733,
-            "y": 4.228511938839819,
-            "heading": -0.4829707427821709,
-            "angularVelocity": 0.000002060378298482567,
-            "velocityX": 3.4881584172616003,
-            "velocityY": -1.4640282090556718,
-            "timestamp": 1.6844286292061779
-        },
-        {
-            "x": 5.481414053769572,
-            "y": 4.196412102682258,
-            "heading": -0.48297069760694356,
-            "angularVelocity": 0.000002060378303797646,
-            "velocityX": 3.488158428108085,
-            "velocityY": -1.4640281832128117,
-            "timestamp": 1.7063543247123123
-        },
-        {
-            "x": 5.5578943539285115,
-            "y": 4.164312267923512,
-            "heading": -0.4829706524317162,
-            "angularVelocity": 0.000002060378305885037,
-            "velocityX": 3.488158454884962,
-            "velocityY": -1.4640281194148257,
-            "timestamp": 1.7282800202184467
-        },
-        {
-            "x": 5.634374653438418,
-            "y": 4.132212431618396,
-            "heading": -0.4829706072564887,
-            "angularVelocity": 0.0000020603783152723795,
-            "velocityX": 3.4881584252834856,
-            "velocityY": -1.4640281899425134,
-            "timestamp": 1.7502057157245812
-        },
-        {
-            "x": 5.71085495402441,
-            "y": 4.100112597877132,
-            "heading": -0.4829705620812611,
-            "angularVelocity": 0.0000020603783147781934,
-            "velocityX": 3.4881584743622165,
-            "velocityY": -1.4640280730089004,
-            "timestamp": 1.7721314112307156
-        },
-        {
-            "x": 5.7873352532620705,
-            "y": 4.068012760923364,
-            "heading": -0.4829705169060333,
-            "angularVelocity": 0.0000020603783267765002,
-            "velocityX": 3.488158412866673,
-            "velocityY": -1.4640282195266534,
-            "timestamp": 1.79405710673685
-        },
-        {
-            "x": 5.863815552376704,
-            "y": 4.035912923676468,
-            "heading": -0.4829704717308054,
-            "angularVelocity": 0.000002060378332596705,
-            "velocityX": 3.4881584072556215,
-            "velocityY": -1.4640282328957663,
-            "timestamp": 1.8159828022429845
-        },
-        {
-            "x": 5.940295849970619,
-            "y": 4.00381308280633,
-            "heading": -0.48297042655557715,
-            "angularVelocity": 0.0000020603783450065593,
-            "velocityX": 3.4881583378978336,
-            "velocityY": -1.4640283981467839,
-            "timestamp": 1.837908497749119
-        },
-        {
-            "x": 6.016776146567219,
-            "y": 3.971713239559965,
-            "heading": -0.4829703813803487,
-            "angularVelocity": 0.0000020603783564721777,
-            "velocityX": 3.4881582924116907,
-            "velocityY": -1.4640285065230472,
-            "timestamp": 1.8598341932552533
-        },
-        {
-            "x": 6.093256433235532,
-            "y": 3.939613372658632,
-            "heading": -0.48297033620511903,
-            "angularVelocity": 0.000002060378411227836,
-            "velocityX": 3.4881578395965716,
-            "velocityY": -1.464029585392675,
-            "timestamp": 1.8817598887613878
-        },
-        {
-            "x": 6.169736625426417,
-            "y": 3.9075132806585757,
-            "heading": -0.4829702910298788,
-            "angularVelocity": 0.0000020603788925938424,
-            "velocityX": 3.488153530614592,
-            "velocityY": -1.4640398518273146,
-            "timestamp": 1.9036855842675222
-        },
-        {
-            "x": 6.2462171159811755,
-            "y": 3.8754138995336747,
-            "heading": -0.4829702458546715,
-            "angularVelocity": 0.0000020603773887814844,
-            "velocityX": 3.4881671385698874,
-            "velocityY": -1.4640074298145878,
-            "timestamp": 1.9256112797736566
-        },
-        {
-            "x": 6.322701012459848,
-            "y": 3.843322634623867,
-            "heading": -0.4829702006798376,
-            "angularVelocity": 0.0000020603603619090433,
-            "velocityX": 3.4883224779483077,
-            "velocityY": -1.4636372607092853,
-            "timestamp": 1.947536975279791
-        },
-        {
-            "x": 6.399174860669393,
-            "y": 3.811207431927173,
-            "heading": -0.4829701555038708,
-            "angularVelocity": 0.000002060412030184656,
-            "velocityX": 3.4878641905846774,
-            "velocityY": -1.4647290293616972,
-            "timestamp": 1.9694626707859255
-        },
-        {
-            "x": 6.47530722887592,
-            "y": 3.7782908978955247,
-            "heading": -0.4829701102655904,
-            "angularVelocity": 0.0000020632540676234596,
-            "velocityX": 3.4722897700184046,
-            "velocityY": -1.501276619595699,
-            "timestamp": 1.99138836629206
-        },
-        {
-            "x": 6.550061088980348,
-            "y": 3.742353506827061,
-            "heading": -0.48297006466550657,
-            "angularVelocity": 0.0000020797554102610617,
-            "velocityX": 3.409417962749362,
-            "velocityY": -1.6390536418100325,
-            "timestamp": 2.0133140617981944
-        },
-        {
-            "x": 6.623057371447113,
-            "y": 3.702968505083503,
-            "heading": -0.482970018320009,
-            "angularVelocity": 0.0000021137526750992832,
-            "velocityX": 3.3292573294334487,
-            "velocityY": -1.796294294637834,
-            "timestamp": 2.035239757304329
-        },
-        {
-            "x": 6.694087047655727,
-            "y": 3.6601383683558693,
-            "heading": -0.48296997081845616,
-            "angularVelocity": 0.0000021664787259977457,
-            "velocityX": 3.2395631960090396,
-            "velocityY": -1.9534220346951927,
-            "timestamp": 2.0571654528104633
-        },
-        {
-            "x": 6.762971951373863,
-            "y": 3.6139376604858007,
-            "heading": -0.48296992170702124,
-            "angularVelocity": 0.000002239903173580672,
-            "velocityX": 3.141743152404488,
-            "velocityY": -2.1071490232631853,
-            "timestamp": 2.0790911483165977
-        },
-        {
-            "x": 6.829544518243924,
-            "y": 3.564463136615443,
-            "heading": -0.4829698704667692,
-            "angularVelocity": 0.0000023369955126814126,
-            "velocityX": 3.0362807351500685,
-            "velocityY": -2.256463146473092,
-            "timestamp": 2.101016843822732
-        },
-        {
-            "x": 6.893649101257324,
-            "y": 3.5118300914764404,
-            "heading": -0.48296981752333895,
-            "angularVelocity": 0.0000024146750677216304,
-            "velocityX": 2.923719477698919,
-            "velocityY": -2.400518839837981,
-            "timestamp": 2.1229425393288666
-        },
-        {
-            "x": 6.935492251520178,
-            "y": 3.475140361342702,
-            "heading": -0.48296976475625003,
-            "angularVelocity": 0.000003586928740757824,
-            "velocityX": 2.8443562341956334,
-            "velocityY": -2.4940441143007757,
-            "timestamp": 2.1376534780501846
-        },
-        {
-            "x": 6.976123919914837,
-            "y": 3.4371133556518583,
-            "heading": -0.4829697143572031,
-            "angularVelocity": 0.0000034259572361559534,
-            "velocityX": 2.7620037826529793,
-            "velocityY": -2.5849475965623756,
-            "timestamp": 2.1523644167715026
-        },
-        {
-            "x": 7.015501922342544,
-            "y": 3.3977895885157845,
-            "heading": -0.4829696659490819,
-            "angularVelocity": 0.0000032906208164279545,
-            "velocityX": 2.676783798347444,
-            "velocityY": -2.673097066136968,
-            "timestamp": 2.1670753554928206
-        },
-        {
-            "x": 7.053586816421239,
-            "y": 3.357212172853621,
-            "heading": -0.4829696191989941,
-            "angularVelocity": 0.000003177913298539426,
-            "velocityX": 2.5888826539333314,
-            "velocityY": -2.758315864871489,
-            "timestamp": 2.1817862942141386
-        },
-        {
-            "x": 7.090341266441407,
-            "y": 3.315425855846777,
-            "heading": -0.48296957381059136,
-            "angularVelocity": 0.0000030853505387280562,
-            "velocityX": 2.4984435539051053,
-            "velocityY": -2.8404929011286835,
-            "timestamp": 2.1964972329354566
-        },
-        {
-            "x": 7.125730460448936,
-            "y": 3.2724771552259067,
-            "heading": -0.48296952951403316,
-            "angularVelocity": 0.000003011130632666104,
-            "velocityX": 2.405638054643597,
-            "velocityY": -2.9195078189422343,
-            "timestamp": 2.2112081716567746
-        },
-        {
-            "x": 7.1597225937186835,
-            "y": 3.228414468562161,
-            "heading": -0.48296947336189594,
-            "angularVelocity": 0.000003817032908672554,
-            "velocityX": 2.3106705774318574,
-            "velocityY": -2.9952328330951756,
-            "timestamp": 2.2259191103780926
-        },
-        {
-            "x": 7.192119389825632,
-            "y": 3.1836076694729414,
-            "heading": -0.4820443638475483,
-            "angularVelocity": 0.06288582475110666,
-            "velocityX": 2.2022249375566023,
-            "velocityY": -3.0458150861769275,
-            "timestamp": 2.2406300490994107
-        },
-        {
-            "x": 7.223229630823693,
-            "y": 3.1396490395754038,
-            "heading": -0.47743685492285376,
-            "angularVelocity": 0.3132029173651504,
-            "velocityX": 2.11476926030427,
-            "velocityY": -2.988159405071387,
-            "timestamp": 2.2553409878207287
-        },
-        {
-            "x": 7.253205323915578,
-            "y": 3.0967066455951215,
-            "heading": -0.4690721734359265,
-            "angularVelocity": 0.5686028366637977,
-            "velocityX": 2.0376465200313345,
-            "velocityY": -2.9190791147849615,
-            "timestamp": 2.2700519265420467
-        },
-        {
-            "x": 7.28210093319025,
-            "y": 3.0548329265465135,
-            "heading": -0.45697001578434165,
-            "angularVelocity": 0.8226638612835941,
-            "velocityX": 1.9642260648397363,
-            "velocityY": -2.8464341971547444,
-            "timestamp": 2.2847628652633647
-        },
-        {
-            "x": 7.309941019631162,
-            "y": 3.0140523923628573,
-            "heading": -0.44116512849373885,
-            "angularVelocity": 1.0743629342769347,
-            "velocityX": 1.892475182468747,
-            "velocityY": -2.7721231769227326,
-            "timestamp": 2.2994738039846827
-        },
-        {
-            "x": 7.336737361434823,
-            "y": 2.9743781069216433,
-            "heading": -0.42169586611687776,
-            "angularVelocity": 1.3234547941286585,
-            "velocityX": 1.8215249421732278,
-            "velocityY": -2.696924118358335,
-            "timestamp": 2.3141847427060007
-        },
-        {
-            "x": 7.362502163456035,
-            "y": 2.936038971640856,
-            "heading": -0.39992762164634665,
-            "angularVelocity": 1.479731843283817,
-            "velocityX": 1.7514043467447793,
-            "velocityY": -2.606165113394762,
-            "timestamp": 2.3288956814273187
-        },
-        {
-            "x": 7.3872337303102205,
-            "y": 2.8991268417513614,
-            "heading": -0.37676830696947144,
-            "angularVelocity": 1.5742921043722566,
-            "velocityX": 1.6811685047907687,
-            "velocityY": -2.5091620996289765,
-            "timestamp": 2.3436066201486367
-        },
-        {
-            "x": 7.410930819346905,
-            "y": 2.863676250815302,
-            "heading": -0.3528129850042874,
-            "angularVelocity": 1.6284019952084616,
-            "velocityX": 1.6108481916483202,
-            "velocityY": -2.4098116107768903,
-            "timestamp": 2.3583175588699548
-        },
-        {
-            "x": 7.433596368468817,
-            "y": 2.8297026440026514,
-            "heading": -0.328491545307115,
-            "angularVelocity": 1.6532894438563033,
-            "velocityX": 1.5407275872250867,
-            "velocityY": -2.3094112113606737,
-            "timestamp": 2.3730284975912728
-        },
-        {
-            "x": 7.455235422231159,
-            "y": 2.7972130374670496,
-            "heading": -0.30412705199668255,
-            "angularVelocity": 1.6562160832826247,
-            "velocityX": 1.4709498946511312,
-            "velocityY": -2.208533877482683,
-            "timestamp": 2.387739436312591
-        },
-        {
-            "x": 7.475853646381716,
-            "y": 2.766210258525358,
-            "heading": -0.2799689621545816,
-            "angularVelocity": 1.6421854716240782,
-            "velocityX": 1.4015573405031783,
-            "velocityY": -2.1074643521399103,
-            "timestamp": 2.402450375033909
-        },
-        {
-            "x": 7.495456618827617,
-            "y": 2.736694950498885,
-            "heading": -0.2562142168141467,
-            "angularVelocity": 1.6147674727249637,
-            "velocityX": 1.332543953669861,
-            "velocityY": -2.006351095984268,
-            "timestamp": 2.417161313755227
-        },
-        {
-            "x": 7.514049552575163,
-            "y": 2.7086666100983954,
-            "heading": -0.23302134486774898,
-            "angularVelocity": 1.576573214378762,
-            "velocityX": 1.2638849294234806,
-            "velocityY": -1.9052720517334563,
-            "timestamp": 2.431872252476545
-        },
-        {
-            "x": 7.5316372223467996,
-            "y": 2.682124150428342,
-            "heading": -0.2105202201245662,
-            "angularVelocity": 1.5295505724985052,
-            "velocityX": 1.1955504747055303,
-            "velocityY": -1.804266890976241,
-            "timestamp": 2.446583191197863
-        },
-        {
-            "x": 7.5482239779420395,
-            "y": 2.6570662133282936,
-            "heading": -0.18881898554249216,
-            "angularVelocity": 1.4751767370647726,
-            "velocityX": 1.127511704688336,
-            "velocityY": -1.7033540533846026,
-            "timestamp": 2.461294129919181
-        },
-        {
-            "x": 7.563813788966016,
-            "y": 2.6334913430066904,
-            "heading": -0.16800906628713844,
-            "angularVelocity": 1.414588127214289,
-            "velocityX": 1.0597427750401904,
-            "velocityY": -1.602540175593351,
-            "timestamp": 2.476005068640499
-        },
-        {
-            "x": 7.5784102960217234,
-            "y": 2.611398080774854,
-            "heading": -0.1481688563613113,
-            "angularVelocity": 1.3486705574455213,
-            "velocityX": 0.9922213213052737,
-            "velocityY": -1.5018254545389353,
-            "timestamp": 2.490716007361817
-        },
-        {
-            "x": 7.5920168580101715,
-            "y": 2.5907850143215074,
-            "heading": -0.1293664624719475,
-            "angularVelocity": 1.2781233234366292,
-            "velocityX": 0.9249281943327172,
-            "velocityY": -1.401206737641836,
-            "timestamp": 2.505426946083135
-        },
-        {
-            "x": 7.604636591974509,
-            "y": 2.571650800836196,
-            "heading": -0.1116617644111604,
-            "angularVelocity": 1.2035056631111207,
-            "velocityX": 0.8578469534407521,
-            "velocityY": -1.3006793004707733,
-            "timestamp": 2.520137884804453
-        },
-        {
-            "x": 7.616272404986451,
-            "y": 2.5539941753706046,
-            "heading": -0.09510797189421191,
-            "angularVelocity": 1.1252709857977845,
-            "velocityX": 0.790963325479677,
-            "velocityY": -1.20023785021998,
-            "timestamp": 2.534848823525771
-        },
-        {
-            "x": 7.626927018849464,
-            "y": 2.537813951229366,
-            "heading": -0.07975280576069126,
-            "angularVelocity": 1.0437924067530058,
-            "velocityX": 0.7242647165386202,
-            "velocityY": -1.0998770675179592,
-            "timestamp": 2.549559762247089
-        },
-        {
-            "x": 7.636602988821212,
-            "y": 2.523109016437205,
-            "heading": -0.0656393963876239,
-            "angularVelocity": 0.9593819701400241,
-            "velocityX": 0.6577398053956987,
-            "velocityY": -0.9995918731448548,
-            "timestamp": 2.564270700968407
-        },
-        {
-            "x": 7.64530271759392,
-            "y": 2.509878328655525,
-            "heading": -0.05280696792868869,
-            "angularVelocity": 0.8723052078477697,
-            "velocityX": 0.5913782211668082,
-            "velocityY": -0.899377533434203,
-            "timestamp": 2.578981639689725
-        },
-        {
-            "x": 7.653028465638572,
-            "y": 2.498120909886367,
-            "heading": -0.04129135982777762,
-            "angularVelocity": 0.7827922010322448,
-            "velocityX": 0.5251702961317726,
-            "velocityY": -0.799229674726393,
-            "timestamp": 2.593692578411043
-        },
-        {
-            "x": 7.6597823588265515,
-            "y": 2.48783584165749,
-            "heading": -0.03112542462311605,
-            "angularVelocity": 0.6910459894669937,
-            "velocityX": 0.4591068806636599,
-            "velocityY": -0.6991442506638482,
-            "timestamp": 2.608403517132361
-        },
-        {
-            "x": 7.665566394043072,
-            "y": 2.4790222609826817,
-            "heading": -0.02233933188139214,
-            "angularVelocity": 0.5972489525084974,
-            "velocityX": 0.39317920671771145,
-            "velocityY": -0.5991174894934748,
-            "timestamp": 2.623114455853679
-        },
-        {
-            "x": 7.670382443324971,
-            "y": 2.471679357147111,
-            "heading": -0.014960801225629315,
-            "angularVelocity": 0.5015676290643761,
-            "velocityX": 0.3273787875222405,
-            "velocityY": -0.4991458379831521,
-            "timestamp": 2.637825394574997
-        },
-        {
-            "x": 7.674232256901065,
-            "y": 2.4658063692226495,
-            "heading": -0.00901528221740954,
-            "angularVelocity": 0.40415633025538694,
-            "velocityX": 0.2616973429788216,
-            "velocityY": -0.399225911800697,
-            "timestamp": 2.652536333296315
-        },
-        {
-            "x": 7.677117465389097,
-            "y": 2.461402584136771,
-            "heading": -0.004526094899743595,
-            "angularVelocity": 0.30515981357196037,
-            "velocityX": 0.19612674233020058,
-            "velocityY": -0.2993544578835629,
-            "timestamp": 2.667247272017633
-        },
-        {
-            "x": 7.679039581308467,
-            "y": 2.45846733507745,
-            "heading": -0.001514541807715108,
-            "angularVelocity": 0.20471522239871504,
-            "velocityX": 0.13065895764927307,
-            "velocityY": -0.1995283315991124,
-            "timestamp": 2.681958210738951
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 7.235538726171091e-30,
-            "angularVelocity": 0.10295344412796348,
-            "velocityX": 0.06528602353168424,
-            "velocityY": -0.09974448981448966,
-            "timestamp": 2.696669149460269
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 1.9723672087239994e-30,
-            "angularVelocity": -3.5777263674767652e-28,
-            "velocityX": 6.106461964279306e-29,
-            "velocityY": 3.383375524617378e-29,
-            "timestamp": 2.711380088181587
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.1.traj
deleted file mode 100644
index 39d8ef61..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.1.traj	
+++ /dev/null
@@ -1,625 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -4.1566893564125267e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 1.3546830312136449,
-            "y": 5.550921973415328,
-            "heading": -1.4050458772244498e-18,
-            "angularVelocity": -4.014308801297789e-17,
-            "velocityX": 0.13379730014070312,
-            "velocityY": -0.25936522568857817,
-            "timestamp": 0.03500093954603885
-        },
-        {
-            "x": 1.3640490935091651,
-            "y": 5.532765920501419,
-            "heading": -3.603876179363921e-18,
-            "angularVelocity": -6.28220327965604e-17,
-            "velocityX": 0.26759459651667833,
-            "velocityY": -0.5187304440792354,
-            "timestamp": 0.0700018790920777
-        },
-        {
-            "x": 1.3780981867284385,
-            "y": 5.505531841564791,
-            "heading": -4.091508575199503e-18,
-            "angularVelocity": -1.3931950355515604e-17,
-            "velocityX": 0.4013918883749871,
-            "velocityY": -0.7780956537124009,
-            "timestamp": 0.10500281863811654
-        },
-        {
-            "x": 1.3968303106782045,
-            "y": 5.469219736980081,
-            "heading": -4.985304542720085e-18,
-            "angularVelocity": -2.5536326104216816e-17,
-            "velocityX": 0.5351891747117141,
-            "velocityY": -1.0374608526419855,
-            "timestamp": 0.1400037581841554
-        },
-        {
-            "x": 1.4202454651168877,
-            "y": 5.423829607215581,
-            "heading": -4.3424315250592746e-18,
-            "angularVelocity": 1.8367331153854942e-17,
-            "velocityX": 0.6689864541464801,
-            "velocityY": -1.2968260381921248,
-            "timestamp": 0.17500469773019423
-        },
-        {
-            "x": 1.4483436497338926,
-            "y": 5.369361452873384,
-            "heading": -2.0560193041515625e-18,
-            "angularVelocity": 6.532433512064817e-17,
-            "velocityX": 0.8027837247073224,
-            "velocityY": -1.5561912065401715,
-            "timestamp": 0.21000563727623306
-        },
-        {
-            "x": 1.4811248641150936,
-            "y": 5.305815274756268,
-            "heading": 2.1208651292681738e-19,
-            "angularVelocity": 6.480130833433249e-17,
-            "velocityX": 0.9365809834363137,
-            "velocityY": -1.815556351952184,
-            "timestamp": 0.2450065768222719
-        },
-        {
-            "x": 1.518589107680717,
-            "y": 5.233191073988126,
-            "heading": 3.0112738874394488e-18,
-            "angularVelocity": 7.99746695569e-17,
-            "velocityX": 1.0703782256008032,
-            "velocityY": -2.074921465253921,
-            "timestamp": 0.2800075163683107
-        },
-        {
-            "x": 1.5607363795611104,
-            "y": 5.151488852254779,
-            "heading": 5.4707987602321704e-18,
-            "angularVelocity": 7.027028770373186e-17,
-            "velocityX": 1.2041754429187383,
-            "velocityY": -2.334286530390624,
-            "timestamp": 0.31500845591434956
-        },
-        {
-            "x": 1.6075666783068712,
-            "y": 5.060708612365892,
-            "heading": 7.441304754324509e-18,
-            "angularVelocity": 5.629869559433992e-17,
-            "velocityX": 1.3379726188262824,
-            "velocityY": -2.5936515152532893,
-            "timestamp": 0.3500093954603884
-        },
-        {
-            "x": 1.6590800010192646,
-            "y": 4.9608503599406655,
-            "heading": 8.015191331788108e-18,
-            "angularVelocity": 1.639633984973558e-17,
-            "velocityX": 1.4717697119150586,
-            "velocityY": -2.853016339571725,
-            "timestamp": 0.38501033500642723
-        },
-        {
-            "x": 1.7152763390026586,
-            "y": 4.851914111835609,
-            "heading": 8.871725964928404e-18,
-            "angularVelocity": 2.4471795525975083e-17,
-            "velocityX": 1.6055665565638684,
-            "velocityY": -3.112380682288518,
-            "timestamp": 0.42001127455246606
-        },
-        {
-            "x": 1.775979033805623,
-            "y": 4.734242319347404,
-            "heading": 1.526334137039101e-17,
-            "angularVelocity": 1.8261268092684144e-16,
-            "velocityX": 1.7343161523752855,
-            "velocityY": -3.361960964887902,
-            "timestamp": 0.4550122140985049
-        },
-        {
-            "x": 1.8366817286107127,
-            "y": 4.616570526856247,
-            "heading": 2.6738432547349554e-17,
-            "angularVelocity": 3.2785094702569765e-16,
-            "velocityX": 1.734316152436003,
-            "velocityY": -3.3619609649722197,
-            "timestamp": 0.49001315364454373
-        },
-        {
-            "x": 1.8973844234157844,
-            "y": 4.498898734365081,
-            "heading": 1.9852147320466475e-17,
-            "angularVelocity": -1.967457250062152e-16,
-            "velocityX": 1.7343161524354893,
-            "velocityY": -3.3619609649724835,
-            "timestamp": 0.5250140931905826
-        },
-        {
-            "x": 1.9580871182208561,
-            "y": 4.381226941873916,
-            "heading": 2.777024505719557e-17,
-            "angularVelocity": 2.2622529107579697e-16,
-            "velocityX": 1.7343161524354895,
-            "velocityY": -3.361960964972483,
-            "timestamp": 0.5600150327366215
-        },
-        {
-            "x": 2.018789813025928,
-            "y": 4.26355514938275,
-            "heading": 1.7414652311267274e-17,
-            "angularVelocity": -2.9586613617347484e-16,
-            "velocityX": 1.7343161524354893,
-            "velocityY": -3.361960964972483,
-            "timestamp": 0.5950159722826603
-        },
-        {
-            "x": 2.0794925078309996,
-            "y": 4.145883356891583,
-            "heading": 6.771469247853252e-18,
-            "angularVelocity": -3.040827818183264e-16,
-            "velocityX": 1.7343161524354895,
-            "velocityY": -3.361960964972483,
-            "timestamp": 0.6300169118286992
-        },
-        {
-            "x": 2.140195202636071,
-            "y": 4.028211564400419,
-            "heading": 1.7156471914154247e-17,
-            "angularVelocity": 2.9670636674740876e-16,
-            "velocityX": 1.7343161524354882,
-            "velocityY": -3.361960964972482,
-            "timestamp": 0.6650178513747381
-        },
-        {
-            "x": 2.2008978974411426,
-            "y": 3.910539771909253,
-            "heading": 9.427453062324153e-18,
-            "angularVelocity": -2.208231822366884e-16,
-            "velocityX": 1.734316152435471,
-            "velocityY": -3.361960964972468,
-            "timestamp": 0.700018790920777
-        },
-        {
-            "x": 2.261600592246207,
-            "y": 3.7928679794180944,
-            "heading": -2.0403441270686145e-18,
-            "angularVelocity": -3.2764255297515643e-16,
-            "velocityX": 1.7343161524352513,
-            "velocityY": -3.3619609649722837,
-            "timestamp": 0.7350197304668159
-        },
-        {
-            "x": 2.322303287051213,
-            "y": 3.6751961869269927,
-            "heading": 3.357966916114392e-18,
-            "angularVelocity": 1.5423331811082687e-16,
-            "velocityX": 1.7343161524334347,
-            "velocityY": -3.3619609649707387,
-            "timestamp": 0.7700206700128548
-        },
-        {
-            "x": 2.383005981879539,
-            "y": 3.55752439444761,
-            "heading": 1.6083387472754582e-17,
-            "angularVelocity": 3.6357368464068287e-16,
-            "velocityX": 1.7343161531001825,
-            "velocityY": -3.361960964635663,
-            "timestamp": 0.8050216095588937
-        },
-        {
-            "x": 2.4437089810285184,
-            "y": 3.439852758955193,
-            "heading": 2.2946423291224054e-17,
-            "angularVelocity": 1.9608147402560587e-16,
-            "velocityX": 1.7343248477441577,
-            "velocityY": -3.3619564794134043,
-            "timestamp": 0.8400225491049326
-        },
-        {
-            "x": 2.507799181067132,
-            "y": 3.323991142238629,
-            "heading": 9.669545465348722e-18,
-            "angularVelocity": -3.793291836756151e-16,
-            "velocityX": 1.8310994181462568,
-            "velocityY": -3.3102430454713967,
-            "timestamp": 0.8750234886509715
-        },
-        {
-            "x": 2.5806292795037855,
-            "y": 3.213414270581727,
-            "heading": 3.38196516760506e-17,
-            "angularVelocity": 6.899845125281262e-16,
-            "velocityX": 2.0808040978720626,
-            "velocityY": -3.159254382625257,
-            "timestamp": 0.9100244281970103
-        },
-        {
-            "x": 2.661766753531331,
-            "y": 3.1087807297901673,
-            "heading": 5.146705636243318e-17,
-            "angularVelocity": 5.041980276892245e-16,
-            "velocityX": 2.3181513147947848,
-            "velocityY": -2.9894494876003863,
-            "timestamp": 0.9450253677430492
-        },
-        {
-            "x": 2.7507288455963135,
-            "y": 3.0107133388519287,
-            "heading": 6.422105835490463e-17,
-            "angularVelocity": 3.643902751722035e-16,
-            "velocityX": 2.5417058290140595,
-            "velocityY": -2.8018502420229328,
-            "timestamp": 0.9800263072890881
-        },
-        {
-            "x": 2.9215337593923705,
-            "y": 2.8658352980219064,
-            "heading": 5.1318206888106e-17,
-            "angularVelocity": -2.1793112103404205e-16,
-            "velocityX": 2.884920936848368,
-            "velocityY": -2.4470121145413257,
-            "timestamp": 1.0392324060123015
-        },
-        {
-            "x": 3.109750180151373,
-            "y": 2.744432888397421,
-            "heading": 6.648964720130561e-17,
-            "angularVelocity": 2.5624793121609224e-16,
-            "velocityX": 3.179003934013426,
-            "velocityY": -2.0505051378665997,
-            "timestamp": 1.098438504735515
-        },
-        {
-            "x": 3.309758578030313,
-            "y": 2.6436330516151556,
-            "heading": 6.809268296964168e-17,
-            "angularVelocity": 2.7075517621497836e-17,
-            "velocityX": 3.3781722186064402,
-            "velocityY": -1.702524553319066,
-            "timestamp": 1.1576446034587284
-        },
-        {
-            "x": 3.5097675494851996,
-            "y": 2.542834352942729,
-            "heading": 6.982732716570642e-17,
-            "angularVelocity": 2.929840393934814e-17,
-            "velocityX": 3.378181906393955,
-            "velocityY": -1.702505330465446,
-            "timestamp": 1.216850702181942
-        },
-        {
-            "x": 3.709776520948374,
-            "y": 2.4420356542872104,
-            "heading": 8.871000937094426e-17,
-            "angularVelocity": 3.189313704575333e-16,
-            "velocityX": 3.3781819065336705,
-            "velocityY": -1.702505330180368,
-            "timestamp": 1.2760568009051554
-        },
-        {
-            "x": 3.9097854924115545,
-            "y": 2.341236955631689,
-            "heading": 7.04137580064196e-17,
-            "angularVelocity": -3.090264644873572e-16,
-            "velocityX": 3.3781819065337895,
-            "velocityY": -1.7025053301803954,
-            "timestamp": 1.3352628996283689
-        },
-        {
-            "x": 4.109794463874735,
-            "y": 2.240438256976168,
-            "heading": 6.635408667627181e-17,
-            "angularVelocity": -6.856846534504647e-17,
-            "velocityX": 3.3781819065337926,
-            "velocityY": -1.7025053301803963,
-            "timestamp": 1.3944689983515823
-        },
-        {
-            "x": 4.3098034353379155,
-            "y": 2.1396395583206465,
-            "heading": 8.756379552984881e-17,
-            "angularVelocity": 3.5823520399025277e-16,
-            "velocityX": 3.3781819065337926,
-            "velocityY": -1.7025053301803963,
-            "timestamp": 1.4536750970747958
-        },
-        {
-            "x": 4.509812406801097,
-            "y": 2.0388408596651253,
-            "heading": 7.076319345079811e-17,
-            "angularVelocity": -2.837647208878057e-16,
-            "velocityX": 3.3781819065337926,
-            "velocityY": -1.7025053301803963,
-            "timestamp": 1.5128811957980093
-        },
-        {
-            "x": 4.709821378264278,
-            "y": 1.9380421610096037,
-            "heading": 6.652684249010322e-17,
-            "angularVelocity": -7.15526111686399e-17,
-            "velocityX": 3.378181906533802,
-            "velocityY": -1.7025053301803985,
-            "timestamp": 1.5720872945212228
-        },
-        {
-            "x": 4.9098303497278835,
-            "y": 1.837243462354927,
-            "heading": 7.855425123263822e-17,
-            "angularVelocity": 2.0314476045367986e-16,
-            "velocityX": 3.378181906540968,
-            "velocityY": -1.7025053301661324,
-            "timestamp": 1.6312933932444362
-        },
-        {
-            "x": 5.109839349236117,
-            "y": 1.7364448193478421,
-            "heading": 7.31685607290898e-17,
-            "angularVelocity": -9.096513061477316e-17,
-            "velocityX": 3.378182380218786,
-            "velocityY": -1.7025043902702668,
-            "timestamp": 1.6904994919676497
-        },
-        {
-            "x": 5.311619758605957,
-            "y": 1.6392407417297363,
-            "heading": 6.073866217998324e-17,
-            "angularVelocity": -2.0994287441931112e-16,
-            "velocityX": 3.4081017618384997,
-            "velocityY": -1.6417916348765775,
-            "timestamp": 1.7497055906908632
-        },
-        {
-            "x": 5.430192834880249,
-            "y": 1.5926107881863976,
-            "heading": 7.355026472308393e-17,
-            "angularVelocity": 3.8038299911394555e-16,
-            "velocityX": 3.5204949744296554,
-            "velocityY": -1.3844670499027307,
-            "timestamp": 1.78338638760593
-        },
-        {
-            "x": 5.550146685206584,
-            "y": 1.5496573158737061,
-            "heading": 8.480161924468705e-17,
-            "angularVelocity": 3.3405844137167443e-16,
-            "velocityX": 3.5614908586879754,
-            "velocityY": -1.2753104512640399,
-            "timestamp": 1.8170671845209967
-        },
-        {
-            "x": 5.670100671705041,
-            "y": 1.506704223843596,
-            "heading": 8.467696633181045e-17,
-            "angularVelocity": -3.701008417078351e-18,
-            "velocityX": 3.5614949017075612,
-            "velocityY": -1.2752991604806918,
-            "timestamp": 1.8507479814360634
-        },
-        {
-            "x": 5.790054658213967,
-            "y": 1.4637511318444054,
-            "heading": 8.225246246818014e-17,
-            "angularVelocity": -7.198475350046388e-17,
-            "velocityX": 3.561494902018399,
-            "velocityY": -1.275299159562779,
-            "timestamp": 1.8844287783511302
-        },
-        {
-            "x": 5.910008644722922,
-            "y": 1.4207980398452125,
-            "heading": 9.443730259968232e-17,
-            "angularVelocity": 3.617741041647218e-16,
-            "velocityX": 3.5614949020192235,
-            "velocityY": -1.2752991595628402,
-            "timestamp": 1.918109575266197
-        },
-        {
-            "x": 6.02996263123188,
-            "y": 1.3778449478460182,
-            "heading": 8.582959689044548e-17,
-            "angularVelocity": -2.555671628240573e-16,
-            "velocityX": 3.5614949020193487,
-            "velocityY": -1.2752991595628858,
-            "timestamp": 1.9517903721812637
-        },
-        {
-            "x": 6.149916617740839,
-            "y": 1.3348918558468235,
-            "heading": 7.701354643967334e-17,
-            "angularVelocity": -2.6175302422337863e-16,
-            "velocityX": 3.56149490201936,
-            "velocityY": -1.2752991595628895,
-            "timestamp": 1.9854711690963305
-        },
-        {
-            "x": 6.269870604249798,
-            "y": 1.291938763847629,
-            "heading": 8.367010625720842e-17,
-            "angularVelocity": 1.9763664839406868e-16,
-            "velocityX": 3.5614949020193607,
-            "velocityY": -1.27529915956289,
-            "timestamp": 2.019151966011397
-        },
-        {
-            "x": 6.3898245907587565,
-            "y": 1.2489856718484342,
-            "heading": 7.968637324735258e-17,
-            "angularVelocity": -1.1827906031741492e-16,
-            "velocityX": 3.5614949020193607,
-            "velocityY": -1.2752991595628902,
-            "timestamp": 2.0528327629264638
-        },
-        {
-            "x": 6.509778577267715,
-            "y": 1.2060325798492397,
-            "heading": 7.914041276645377e-17,
-            "angularVelocity": -1.6209844508000374e-17,
-            "velocityX": 3.561494902019361,
-            "velocityY": -1.27529915956289,
-            "timestamp": 2.0865135598415305
-        },
-        {
-            "x": 6.629732563776674,
-            "y": 1.1630794878500452,
-            "heading": 7.575912442206911e-17,
-            "angularVelocity": -1.0039217162560839e-16,
-            "velocityX": 3.561494902019361,
-            "velocityY": -1.27529915956289,
-            "timestamp": 2.1201943567565973
-        },
-        {
-            "x": 6.749686550285633,
-            "y": 1.1201263958508503,
-            "heading": 6.889954024340196e-17,
-            "angularVelocity": -2.0366454499176497e-16,
-            "velocityX": 3.5614949020193576,
-            "velocityY": -1.2752991595628989,
-            "timestamp": 2.153875153671664
-        },
-        {
-            "x": 6.869640536794461,
-            "y": 1.077173303851719,
-            "heading": 8.054846416919904e-17,
-            "angularVelocity": 3.458624792985843e-16,
-            "velocityX": 3.5614949020154905,
-            "velocityY": -1.275299159561009,
-            "timestamp": 2.187555950586731
-        },
-        {
-            "x": 6.985406128017608,
-            "y": 1.0357199913369377,
-            "heading": 6.97905590140722e-17,
-            "angularVelocity": -3.1940767538205783e-16,
-            "velocityX": 3.437139314579595,
-            "velocityY": -1.2307699434580792,
-            "timestamp": 2.2212367475017976
-        },
-        {
-            "x": 7.092266700241795,
-            "y": 0.9974553856875888,
-            "heading": 5.947830002893604e-17,
-            "angularVelocity": -3.0617621445548207e-16,
-            "velocityX": 3.172744768885924,
-            "velocityY": -1.1360956139440452,
-            "timestamp": 2.2549175444168643
-        },
-        {
-            "x": 7.190222235939473,
-            "y": 0.9623794931799317,
-            "heading": 5.044588900647289e-17,
-            "angularVelocity": -2.6817687744625345e-16,
-            "velocityX": 2.9083497027904013,
-            "velocityY": -1.0414210980846788,
-            "timestamp": 2.288598341331931
-        },
-        {
-            "x": 7.279272729268112,
-            "y": 0.930492315906057,
-            "heading": 4.27627289839913e-17,
-            "angularVelocity": -2.281169282784766e-16,
-            "velocityX": 2.6439544632272356,
-            "velocityY": -0.9467465201100714,
-            "timestamp": 2.322279138246998
-        },
-        {
-            "x": 7.35941817730645,
-            "y": 0.9017938549120106,
-            "heading": 3.626946964953948e-17,
-            "angularVelocity": -1.9278817213060433e-16,
-            "velocityX": 2.379559136930222,
-            "velocityY": -0.8520719110778341,
-            "timestamp": 2.3559599351620646
-        },
-        {
-            "x": 7.430658578301727,
-            "y": 0.8762841108254199,
-            "heading": 2.9442071838197796e-17,
-            "angularVelocity": -2.0270891340137855e-16,
-            "velocityX": 2.1151637585928986,
-            "velocityY": -0.7573972834110186,
-            "timestamp": 2.3896407320771313
-        },
-        {
-            "x": 7.492993931085436,
-            "y": 0.8539630840647031,
-            "heading": 2.3413209096667504e-17,
-            "angularVelocity": -1.790000012026981e-16,
-            "velocityX": 1.850768345562039,
-            "velocityY": -0.662722643321152,
-            "timestamp": 2.423321528992198
-        },
-        {
-            "x": 7.546424234822929,
-            "y": 0.8348307749287305,
-            "heading": 1.7731266545962487e-17,
-            "angularVelocity": -1.6869976443484408e-16,
-            "velocityX": 1.5863729077500839,
-            "velocityY": -0.5680479943576784,
-            "timestamp": 2.457002325907265
-        },
-        {
-            "x": 7.590949488888224,
-            "y": 0.8188871836416546,
-            "heading": 1.2363765500283435e-17,
-            "angularVelocity": -1.593638362048541e-16,
-            "velocityX": 1.321977451352309,
-            "velocityY": -0.4733733387389999,
-            "timestamp": 2.4906831228223316
-        },
-        {
-            "x": 7.626569692794441,
-            "y": 0.8061323103778162,
-            "heading": 7.88828312817125e-18,
-            "angularVelocity": -1.3287934794211324e-16,
-            "velocityX": 1.057581980498898,
-            "velocityY": -0.37869867794405154,
-            "timestamp": 2.5243639197373984
-        },
-        {
-            "x": 7.653284846152079,
-            "y": 0.7965661552766882,
-            "heading": 4.236479358871306e-18,
-            "angularVelocity": -1.0842391112822455e-16,
-            "velocityX": 0.7931864980809786,
-            "velocityY": -0.28402401300808766,
-            "timestamp": 2.558044716652465
-        },
-        {
-            "x": 7.671094948642455,
-            "y": 0.7901887184523844,
-            "heading": 1.249730071610374e-18,
-            "angularVelocity": -8.867810508445172e-17,
-            "velocityX": 0.5287910062011898,
-            "velocityY": -0.18934934468402032,
-            "timestamp": 2.591725513567532
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -2.906959217388984e-34,
-            "angularVelocity": -3.7105121777105e-17,
-            "velocityX": 0.26439550643651005,
-            "velocityY": -0.09467467353653367,
-            "timestamp": 2.6254063104825986
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -4.078160951605287e-34,
-            "angularVelocity": -3.477050502256718e-33,
-            "velocityX": 7.685636324015334e-33,
-            "velocityY": -2.3747808746440768e-32,
-            "timestamp": 2.6590871073976654
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.traj
deleted file mode 100644
index 39d8ef61..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.traj	
+++ /dev/null
@@ -1,625 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -4.1566893564125267e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 1.3546830312136449,
-            "y": 5.550921973415328,
-            "heading": -1.4050458772244498e-18,
-            "angularVelocity": -4.014308801297789e-17,
-            "velocityX": 0.13379730014070312,
-            "velocityY": -0.25936522568857817,
-            "timestamp": 0.03500093954603885
-        },
-        {
-            "x": 1.3640490935091651,
-            "y": 5.532765920501419,
-            "heading": -3.603876179363921e-18,
-            "angularVelocity": -6.28220327965604e-17,
-            "velocityX": 0.26759459651667833,
-            "velocityY": -0.5187304440792354,
-            "timestamp": 0.0700018790920777
-        },
-        {
-            "x": 1.3780981867284385,
-            "y": 5.505531841564791,
-            "heading": -4.091508575199503e-18,
-            "angularVelocity": -1.3931950355515604e-17,
-            "velocityX": 0.4013918883749871,
-            "velocityY": -0.7780956537124009,
-            "timestamp": 0.10500281863811654
-        },
-        {
-            "x": 1.3968303106782045,
-            "y": 5.469219736980081,
-            "heading": -4.985304542720085e-18,
-            "angularVelocity": -2.5536326104216816e-17,
-            "velocityX": 0.5351891747117141,
-            "velocityY": -1.0374608526419855,
-            "timestamp": 0.1400037581841554
-        },
-        {
-            "x": 1.4202454651168877,
-            "y": 5.423829607215581,
-            "heading": -4.3424315250592746e-18,
-            "angularVelocity": 1.8367331153854942e-17,
-            "velocityX": 0.6689864541464801,
-            "velocityY": -1.2968260381921248,
-            "timestamp": 0.17500469773019423
-        },
-        {
-            "x": 1.4483436497338926,
-            "y": 5.369361452873384,
-            "heading": -2.0560193041515625e-18,
-            "angularVelocity": 6.532433512064817e-17,
-            "velocityX": 0.8027837247073224,
-            "velocityY": -1.5561912065401715,
-            "timestamp": 0.21000563727623306
-        },
-        {
-            "x": 1.4811248641150936,
-            "y": 5.305815274756268,
-            "heading": 2.1208651292681738e-19,
-            "angularVelocity": 6.480130833433249e-17,
-            "velocityX": 0.9365809834363137,
-            "velocityY": -1.815556351952184,
-            "timestamp": 0.2450065768222719
-        },
-        {
-            "x": 1.518589107680717,
-            "y": 5.233191073988126,
-            "heading": 3.0112738874394488e-18,
-            "angularVelocity": 7.99746695569e-17,
-            "velocityX": 1.0703782256008032,
-            "velocityY": -2.074921465253921,
-            "timestamp": 0.2800075163683107
-        },
-        {
-            "x": 1.5607363795611104,
-            "y": 5.151488852254779,
-            "heading": 5.4707987602321704e-18,
-            "angularVelocity": 7.027028770373186e-17,
-            "velocityX": 1.2041754429187383,
-            "velocityY": -2.334286530390624,
-            "timestamp": 0.31500845591434956
-        },
-        {
-            "x": 1.6075666783068712,
-            "y": 5.060708612365892,
-            "heading": 7.441304754324509e-18,
-            "angularVelocity": 5.629869559433992e-17,
-            "velocityX": 1.3379726188262824,
-            "velocityY": -2.5936515152532893,
-            "timestamp": 0.3500093954603884
-        },
-        {
-            "x": 1.6590800010192646,
-            "y": 4.9608503599406655,
-            "heading": 8.015191331788108e-18,
-            "angularVelocity": 1.639633984973558e-17,
-            "velocityX": 1.4717697119150586,
-            "velocityY": -2.853016339571725,
-            "timestamp": 0.38501033500642723
-        },
-        {
-            "x": 1.7152763390026586,
-            "y": 4.851914111835609,
-            "heading": 8.871725964928404e-18,
-            "angularVelocity": 2.4471795525975083e-17,
-            "velocityX": 1.6055665565638684,
-            "velocityY": -3.112380682288518,
-            "timestamp": 0.42001127455246606
-        },
-        {
-            "x": 1.775979033805623,
-            "y": 4.734242319347404,
-            "heading": 1.526334137039101e-17,
-            "angularVelocity": 1.8261268092684144e-16,
-            "velocityX": 1.7343161523752855,
-            "velocityY": -3.361960964887902,
-            "timestamp": 0.4550122140985049
-        },
-        {
-            "x": 1.8366817286107127,
-            "y": 4.616570526856247,
-            "heading": 2.6738432547349554e-17,
-            "angularVelocity": 3.2785094702569765e-16,
-            "velocityX": 1.734316152436003,
-            "velocityY": -3.3619609649722197,
-            "timestamp": 0.49001315364454373
-        },
-        {
-            "x": 1.8973844234157844,
-            "y": 4.498898734365081,
-            "heading": 1.9852147320466475e-17,
-            "angularVelocity": -1.967457250062152e-16,
-            "velocityX": 1.7343161524354893,
-            "velocityY": -3.3619609649724835,
-            "timestamp": 0.5250140931905826
-        },
-        {
-            "x": 1.9580871182208561,
-            "y": 4.381226941873916,
-            "heading": 2.777024505719557e-17,
-            "angularVelocity": 2.2622529107579697e-16,
-            "velocityX": 1.7343161524354895,
-            "velocityY": -3.361960964972483,
-            "timestamp": 0.5600150327366215
-        },
-        {
-            "x": 2.018789813025928,
-            "y": 4.26355514938275,
-            "heading": 1.7414652311267274e-17,
-            "angularVelocity": -2.9586613617347484e-16,
-            "velocityX": 1.7343161524354893,
-            "velocityY": -3.361960964972483,
-            "timestamp": 0.5950159722826603
-        },
-        {
-            "x": 2.0794925078309996,
-            "y": 4.145883356891583,
-            "heading": 6.771469247853252e-18,
-            "angularVelocity": -3.040827818183264e-16,
-            "velocityX": 1.7343161524354895,
-            "velocityY": -3.361960964972483,
-            "timestamp": 0.6300169118286992
-        },
-        {
-            "x": 2.140195202636071,
-            "y": 4.028211564400419,
-            "heading": 1.7156471914154247e-17,
-            "angularVelocity": 2.9670636674740876e-16,
-            "velocityX": 1.7343161524354882,
-            "velocityY": -3.361960964972482,
-            "timestamp": 0.6650178513747381
-        },
-        {
-            "x": 2.2008978974411426,
-            "y": 3.910539771909253,
-            "heading": 9.427453062324153e-18,
-            "angularVelocity": -2.208231822366884e-16,
-            "velocityX": 1.734316152435471,
-            "velocityY": -3.361960964972468,
-            "timestamp": 0.700018790920777
-        },
-        {
-            "x": 2.261600592246207,
-            "y": 3.7928679794180944,
-            "heading": -2.0403441270686145e-18,
-            "angularVelocity": -3.2764255297515643e-16,
-            "velocityX": 1.7343161524352513,
-            "velocityY": -3.3619609649722837,
-            "timestamp": 0.7350197304668159
-        },
-        {
-            "x": 2.322303287051213,
-            "y": 3.6751961869269927,
-            "heading": 3.357966916114392e-18,
-            "angularVelocity": 1.5423331811082687e-16,
-            "velocityX": 1.7343161524334347,
-            "velocityY": -3.3619609649707387,
-            "timestamp": 0.7700206700128548
-        },
-        {
-            "x": 2.383005981879539,
-            "y": 3.55752439444761,
-            "heading": 1.6083387472754582e-17,
-            "angularVelocity": 3.6357368464068287e-16,
-            "velocityX": 1.7343161531001825,
-            "velocityY": -3.361960964635663,
-            "timestamp": 0.8050216095588937
-        },
-        {
-            "x": 2.4437089810285184,
-            "y": 3.439852758955193,
-            "heading": 2.2946423291224054e-17,
-            "angularVelocity": 1.9608147402560587e-16,
-            "velocityX": 1.7343248477441577,
-            "velocityY": -3.3619564794134043,
-            "timestamp": 0.8400225491049326
-        },
-        {
-            "x": 2.507799181067132,
-            "y": 3.323991142238629,
-            "heading": 9.669545465348722e-18,
-            "angularVelocity": -3.793291836756151e-16,
-            "velocityX": 1.8310994181462568,
-            "velocityY": -3.3102430454713967,
-            "timestamp": 0.8750234886509715
-        },
-        {
-            "x": 2.5806292795037855,
-            "y": 3.213414270581727,
-            "heading": 3.38196516760506e-17,
-            "angularVelocity": 6.899845125281262e-16,
-            "velocityX": 2.0808040978720626,
-            "velocityY": -3.159254382625257,
-            "timestamp": 0.9100244281970103
-        },
-        {
-            "x": 2.661766753531331,
-            "y": 3.1087807297901673,
-            "heading": 5.146705636243318e-17,
-            "angularVelocity": 5.041980276892245e-16,
-            "velocityX": 2.3181513147947848,
-            "velocityY": -2.9894494876003863,
-            "timestamp": 0.9450253677430492
-        },
-        {
-            "x": 2.7507288455963135,
-            "y": 3.0107133388519287,
-            "heading": 6.422105835490463e-17,
-            "angularVelocity": 3.643902751722035e-16,
-            "velocityX": 2.5417058290140595,
-            "velocityY": -2.8018502420229328,
-            "timestamp": 0.9800263072890881
-        },
-        {
-            "x": 2.9215337593923705,
-            "y": 2.8658352980219064,
-            "heading": 5.1318206888106e-17,
-            "angularVelocity": -2.1793112103404205e-16,
-            "velocityX": 2.884920936848368,
-            "velocityY": -2.4470121145413257,
-            "timestamp": 1.0392324060123015
-        },
-        {
-            "x": 3.109750180151373,
-            "y": 2.744432888397421,
-            "heading": 6.648964720130561e-17,
-            "angularVelocity": 2.5624793121609224e-16,
-            "velocityX": 3.179003934013426,
-            "velocityY": -2.0505051378665997,
-            "timestamp": 1.098438504735515
-        },
-        {
-            "x": 3.309758578030313,
-            "y": 2.6436330516151556,
-            "heading": 6.809268296964168e-17,
-            "angularVelocity": 2.7075517621497836e-17,
-            "velocityX": 3.3781722186064402,
-            "velocityY": -1.702524553319066,
-            "timestamp": 1.1576446034587284
-        },
-        {
-            "x": 3.5097675494851996,
-            "y": 2.542834352942729,
-            "heading": 6.982732716570642e-17,
-            "angularVelocity": 2.929840393934814e-17,
-            "velocityX": 3.378181906393955,
-            "velocityY": -1.702505330465446,
-            "timestamp": 1.216850702181942
-        },
-        {
-            "x": 3.709776520948374,
-            "y": 2.4420356542872104,
-            "heading": 8.871000937094426e-17,
-            "angularVelocity": 3.189313704575333e-16,
-            "velocityX": 3.3781819065336705,
-            "velocityY": -1.702505330180368,
-            "timestamp": 1.2760568009051554
-        },
-        {
-            "x": 3.9097854924115545,
-            "y": 2.341236955631689,
-            "heading": 7.04137580064196e-17,
-            "angularVelocity": -3.090264644873572e-16,
-            "velocityX": 3.3781819065337895,
-            "velocityY": -1.7025053301803954,
-            "timestamp": 1.3352628996283689
-        },
-        {
-            "x": 4.109794463874735,
-            "y": 2.240438256976168,
-            "heading": 6.635408667627181e-17,
-            "angularVelocity": -6.856846534504647e-17,
-            "velocityX": 3.3781819065337926,
-            "velocityY": -1.7025053301803963,
-            "timestamp": 1.3944689983515823
-        },
-        {
-            "x": 4.3098034353379155,
-            "y": 2.1396395583206465,
-            "heading": 8.756379552984881e-17,
-            "angularVelocity": 3.5823520399025277e-16,
-            "velocityX": 3.3781819065337926,
-            "velocityY": -1.7025053301803963,
-            "timestamp": 1.4536750970747958
-        },
-        {
-            "x": 4.509812406801097,
-            "y": 2.0388408596651253,
-            "heading": 7.076319345079811e-17,
-            "angularVelocity": -2.837647208878057e-16,
-            "velocityX": 3.3781819065337926,
-            "velocityY": -1.7025053301803963,
-            "timestamp": 1.5128811957980093
-        },
-        {
-            "x": 4.709821378264278,
-            "y": 1.9380421610096037,
-            "heading": 6.652684249010322e-17,
-            "angularVelocity": -7.15526111686399e-17,
-            "velocityX": 3.378181906533802,
-            "velocityY": -1.7025053301803985,
-            "timestamp": 1.5720872945212228
-        },
-        {
-            "x": 4.9098303497278835,
-            "y": 1.837243462354927,
-            "heading": 7.855425123263822e-17,
-            "angularVelocity": 2.0314476045367986e-16,
-            "velocityX": 3.378181906540968,
-            "velocityY": -1.7025053301661324,
-            "timestamp": 1.6312933932444362
-        },
-        {
-            "x": 5.109839349236117,
-            "y": 1.7364448193478421,
-            "heading": 7.31685607290898e-17,
-            "angularVelocity": -9.096513061477316e-17,
-            "velocityX": 3.378182380218786,
-            "velocityY": -1.7025043902702668,
-            "timestamp": 1.6904994919676497
-        },
-        {
-            "x": 5.311619758605957,
-            "y": 1.6392407417297363,
-            "heading": 6.073866217998324e-17,
-            "angularVelocity": -2.0994287441931112e-16,
-            "velocityX": 3.4081017618384997,
-            "velocityY": -1.6417916348765775,
-            "timestamp": 1.7497055906908632
-        },
-        {
-            "x": 5.430192834880249,
-            "y": 1.5926107881863976,
-            "heading": 7.355026472308393e-17,
-            "angularVelocity": 3.8038299911394555e-16,
-            "velocityX": 3.5204949744296554,
-            "velocityY": -1.3844670499027307,
-            "timestamp": 1.78338638760593
-        },
-        {
-            "x": 5.550146685206584,
-            "y": 1.5496573158737061,
-            "heading": 8.480161924468705e-17,
-            "angularVelocity": 3.3405844137167443e-16,
-            "velocityX": 3.5614908586879754,
-            "velocityY": -1.2753104512640399,
-            "timestamp": 1.8170671845209967
-        },
-        {
-            "x": 5.670100671705041,
-            "y": 1.506704223843596,
-            "heading": 8.467696633181045e-17,
-            "angularVelocity": -3.701008417078351e-18,
-            "velocityX": 3.5614949017075612,
-            "velocityY": -1.2752991604806918,
-            "timestamp": 1.8507479814360634
-        },
-        {
-            "x": 5.790054658213967,
-            "y": 1.4637511318444054,
-            "heading": 8.225246246818014e-17,
-            "angularVelocity": -7.198475350046388e-17,
-            "velocityX": 3.561494902018399,
-            "velocityY": -1.275299159562779,
-            "timestamp": 1.8844287783511302
-        },
-        {
-            "x": 5.910008644722922,
-            "y": 1.4207980398452125,
-            "heading": 9.443730259968232e-17,
-            "angularVelocity": 3.617741041647218e-16,
-            "velocityX": 3.5614949020192235,
-            "velocityY": -1.2752991595628402,
-            "timestamp": 1.918109575266197
-        },
-        {
-            "x": 6.02996263123188,
-            "y": 1.3778449478460182,
-            "heading": 8.582959689044548e-17,
-            "angularVelocity": -2.555671628240573e-16,
-            "velocityX": 3.5614949020193487,
-            "velocityY": -1.2752991595628858,
-            "timestamp": 1.9517903721812637
-        },
-        {
-            "x": 6.149916617740839,
-            "y": 1.3348918558468235,
-            "heading": 7.701354643967334e-17,
-            "angularVelocity": -2.6175302422337863e-16,
-            "velocityX": 3.56149490201936,
-            "velocityY": -1.2752991595628895,
-            "timestamp": 1.9854711690963305
-        },
-        {
-            "x": 6.269870604249798,
-            "y": 1.291938763847629,
-            "heading": 8.367010625720842e-17,
-            "angularVelocity": 1.9763664839406868e-16,
-            "velocityX": 3.5614949020193607,
-            "velocityY": -1.27529915956289,
-            "timestamp": 2.019151966011397
-        },
-        {
-            "x": 6.3898245907587565,
-            "y": 1.2489856718484342,
-            "heading": 7.968637324735258e-17,
-            "angularVelocity": -1.1827906031741492e-16,
-            "velocityX": 3.5614949020193607,
-            "velocityY": -1.2752991595628902,
-            "timestamp": 2.0528327629264638
-        },
-        {
-            "x": 6.509778577267715,
-            "y": 1.2060325798492397,
-            "heading": 7.914041276645377e-17,
-            "angularVelocity": -1.6209844508000374e-17,
-            "velocityX": 3.561494902019361,
-            "velocityY": -1.27529915956289,
-            "timestamp": 2.0865135598415305
-        },
-        {
-            "x": 6.629732563776674,
-            "y": 1.1630794878500452,
-            "heading": 7.575912442206911e-17,
-            "angularVelocity": -1.0039217162560839e-16,
-            "velocityX": 3.561494902019361,
-            "velocityY": -1.27529915956289,
-            "timestamp": 2.1201943567565973
-        },
-        {
-            "x": 6.749686550285633,
-            "y": 1.1201263958508503,
-            "heading": 6.889954024340196e-17,
-            "angularVelocity": -2.0366454499176497e-16,
-            "velocityX": 3.5614949020193576,
-            "velocityY": -1.2752991595628989,
-            "timestamp": 2.153875153671664
-        },
-        {
-            "x": 6.869640536794461,
-            "y": 1.077173303851719,
-            "heading": 8.054846416919904e-17,
-            "angularVelocity": 3.458624792985843e-16,
-            "velocityX": 3.5614949020154905,
-            "velocityY": -1.275299159561009,
-            "timestamp": 2.187555950586731
-        },
-        {
-            "x": 6.985406128017608,
-            "y": 1.0357199913369377,
-            "heading": 6.97905590140722e-17,
-            "angularVelocity": -3.1940767538205783e-16,
-            "velocityX": 3.437139314579595,
-            "velocityY": -1.2307699434580792,
-            "timestamp": 2.2212367475017976
-        },
-        {
-            "x": 7.092266700241795,
-            "y": 0.9974553856875888,
-            "heading": 5.947830002893604e-17,
-            "angularVelocity": -3.0617621445548207e-16,
-            "velocityX": 3.172744768885924,
-            "velocityY": -1.1360956139440452,
-            "timestamp": 2.2549175444168643
-        },
-        {
-            "x": 7.190222235939473,
-            "y": 0.9623794931799317,
-            "heading": 5.044588900647289e-17,
-            "angularVelocity": -2.6817687744625345e-16,
-            "velocityX": 2.9083497027904013,
-            "velocityY": -1.0414210980846788,
-            "timestamp": 2.288598341331931
-        },
-        {
-            "x": 7.279272729268112,
-            "y": 0.930492315906057,
-            "heading": 4.27627289839913e-17,
-            "angularVelocity": -2.281169282784766e-16,
-            "velocityX": 2.6439544632272356,
-            "velocityY": -0.9467465201100714,
-            "timestamp": 2.322279138246998
-        },
-        {
-            "x": 7.35941817730645,
-            "y": 0.9017938549120106,
-            "heading": 3.626946964953948e-17,
-            "angularVelocity": -1.9278817213060433e-16,
-            "velocityX": 2.379559136930222,
-            "velocityY": -0.8520719110778341,
-            "timestamp": 2.3559599351620646
-        },
-        {
-            "x": 7.430658578301727,
-            "y": 0.8762841108254199,
-            "heading": 2.9442071838197796e-17,
-            "angularVelocity": -2.0270891340137855e-16,
-            "velocityX": 2.1151637585928986,
-            "velocityY": -0.7573972834110186,
-            "timestamp": 2.3896407320771313
-        },
-        {
-            "x": 7.492993931085436,
-            "y": 0.8539630840647031,
-            "heading": 2.3413209096667504e-17,
-            "angularVelocity": -1.790000012026981e-16,
-            "velocityX": 1.850768345562039,
-            "velocityY": -0.662722643321152,
-            "timestamp": 2.423321528992198
-        },
-        {
-            "x": 7.546424234822929,
-            "y": 0.8348307749287305,
-            "heading": 1.7731266545962487e-17,
-            "angularVelocity": -1.6869976443484408e-16,
-            "velocityX": 1.5863729077500839,
-            "velocityY": -0.5680479943576784,
-            "timestamp": 2.457002325907265
-        },
-        {
-            "x": 7.590949488888224,
-            "y": 0.8188871836416546,
-            "heading": 1.2363765500283435e-17,
-            "angularVelocity": -1.593638362048541e-16,
-            "velocityX": 1.321977451352309,
-            "velocityY": -0.4733733387389999,
-            "timestamp": 2.4906831228223316
-        },
-        {
-            "x": 7.626569692794441,
-            "y": 0.8061323103778162,
-            "heading": 7.88828312817125e-18,
-            "angularVelocity": -1.3287934794211324e-16,
-            "velocityX": 1.057581980498898,
-            "velocityY": -0.37869867794405154,
-            "timestamp": 2.5243639197373984
-        },
-        {
-            "x": 7.653284846152079,
-            "y": 0.7965661552766882,
-            "heading": 4.236479358871306e-18,
-            "angularVelocity": -1.0842391112822455e-16,
-            "velocityX": 0.7931864980809786,
-            "velocityY": -0.28402401300808766,
-            "timestamp": 2.558044716652465
-        },
-        {
-            "x": 7.671094948642455,
-            "y": 0.7901887184523844,
-            "heading": 1.249730071610374e-18,
-            "angularVelocity": -8.867810508445172e-17,
-            "velocityX": 0.5287910062011898,
-            "velocityY": -0.18934934468402032,
-            "timestamp": 2.591725513567532
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -2.906959217388984e-34,
-            "angularVelocity": -3.7105121777105e-17,
-            "velocityX": 0.26439550643651005,
-            "velocityY": -0.09467467353653367,
-            "timestamp": 2.6254063104825986
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -4.078160951605287e-34,
-            "angularVelocity": -3.477050502256718e-33,
-            "velocityX": 7.685636324015334e-33,
-            "velocityY": -2.3747808746440768e-32,
-            "timestamp": 2.6590871073976654
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.1.traj
deleted file mode 100644
index 641af2c0..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.1.traj	
+++ /dev/null
@@ -1,112 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -1.3982338019676807e-32,
-            "velocityX": 1.9225496754980614e-32,
-            "velocityY": -8.567103847838605e-38,
-            "timestamp": 0
-        },
-        {
-            "x": 1.3816658535274804,
-            "y": 5.599900984360149,
-            "heading": 0.013082876743969601,
-            "angularVelocity": 0.16716651866961724,
-            "velocityX": 0.4046105912700521,
-            "velocityY": 0.509835013927756,
-            "timestamp": 0.07826254233257365
-        },
-        {
-            "x": 1.4449977124682947,
-            "y": 5.679702872674784,
-            "heading": 0.039246215534929935,
-            "angularVelocity": 0.3343021835373132,
-            "velocityX": 0.8092231232623148,
-            "velocityY": 1.0196690004717623,
-            "timestamp": 0.1565250846651473
-        },
-        {
-            "x": 1.5399958043003432,
-            "y": 5.799405527723277,
-            "heading": 0.07848745324353076,
-            "angularVelocity": 0.501405098007769,
-            "velocityX": 1.2138385618545031,
-            "velocityY": 1.5295012336785618,
-            "timestamp": 0.23478762699772096
-        },
-        {
-            "x": 1.6666604122853452,
-            "y": 5.959008727776544,
-            "heading": 0.1308059995871385,
-            "angularVelocity": 0.6685004701391144,
-            "velocityX": 1.6184576198246376,
-            "velocityY": 2.039330633740969,
-            "timestamp": 0.3130501693302946
-        },
-        {
-            "x": 1.8249918316073899,
-            "y": 6.158512135429991,
-            "heading": 0.19620604644996878,
-            "angularVelocity": 0.8356494040905814,
-            "velocityX": 2.023080449510844,
-            "velocityY": 2.5491557226145924,
-            "timestamp": 0.3913127116628683
-        },
-        {
-            "x": 1.9833278311922586,
-            "y": 6.358008426099132,
-            "heading": 0.26178707056088246,
-            "angularVelocity": 0.8379618417228061,
-            "velocityX": 2.023138973840461,
-            "velocityY": 2.549064785314409,
-            "timestamp": 0.469575253995442
-        },
-        {
-            "x": 2.109996681019138,
-            "y": 6.517605179473628,
-            "heading": 0.31426571552794874,
-            "angularVelocity": 0.6705461310477281,
-            "velocityX": 1.618511819979031,
-            "velocityY": 2.039248261272889,
-            "timestamp": 0.5478377963280157
-        },
-        {
-            "x": 2.204998332006287,
-            "y": 6.637302612224517,
-            "heading": 0.35363163522076196,
-            "angularVelocity": 0.5029982226430836,
-            "velocityX": 1.2138840389754155,
-            "velocityY": 1.5294345057465644,
-            "timestamp": 0.6261003386605893
-        },
-        {
-            "x": 2.268332773183591,
-            "y": 6.717100865272008,
-            "heading": 0.3798775217934225,
-            "angularVelocity": 0.3353569382033373,
-            "velocityX": 0.8092561178011136,
-            "velocityY": 1.0196225508288703,
-            "timestamp": 0.704362880993163
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.1676725265429579,
-            "velocityX": 0.40462813847574725,
-            "velocityY": 0.509811380244222,
-            "timestamp": 0.7826254233257367
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 9.092584352802723e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0.8608879656583104
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.traj
deleted file mode 100644
index 641af2c0..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.traj	
+++ /dev/null
@@ -1,112 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -1.3982338019676807e-32,
-            "velocityX": 1.9225496754980614e-32,
-            "velocityY": -8.567103847838605e-38,
-            "timestamp": 0
-        },
-        {
-            "x": 1.3816658535274804,
-            "y": 5.599900984360149,
-            "heading": 0.013082876743969601,
-            "angularVelocity": 0.16716651866961724,
-            "velocityX": 0.4046105912700521,
-            "velocityY": 0.509835013927756,
-            "timestamp": 0.07826254233257365
-        },
-        {
-            "x": 1.4449977124682947,
-            "y": 5.679702872674784,
-            "heading": 0.039246215534929935,
-            "angularVelocity": 0.3343021835373132,
-            "velocityX": 0.8092231232623148,
-            "velocityY": 1.0196690004717623,
-            "timestamp": 0.1565250846651473
-        },
-        {
-            "x": 1.5399958043003432,
-            "y": 5.799405527723277,
-            "heading": 0.07848745324353076,
-            "angularVelocity": 0.501405098007769,
-            "velocityX": 1.2138385618545031,
-            "velocityY": 1.5295012336785618,
-            "timestamp": 0.23478762699772096
-        },
-        {
-            "x": 1.6666604122853452,
-            "y": 5.959008727776544,
-            "heading": 0.1308059995871385,
-            "angularVelocity": 0.6685004701391144,
-            "velocityX": 1.6184576198246376,
-            "velocityY": 2.039330633740969,
-            "timestamp": 0.3130501693302946
-        },
-        {
-            "x": 1.8249918316073899,
-            "y": 6.158512135429991,
-            "heading": 0.19620604644996878,
-            "angularVelocity": 0.8356494040905814,
-            "velocityX": 2.023080449510844,
-            "velocityY": 2.5491557226145924,
-            "timestamp": 0.3913127116628683
-        },
-        {
-            "x": 1.9833278311922586,
-            "y": 6.358008426099132,
-            "heading": 0.26178707056088246,
-            "angularVelocity": 0.8379618417228061,
-            "velocityX": 2.023138973840461,
-            "velocityY": 2.549064785314409,
-            "timestamp": 0.469575253995442
-        },
-        {
-            "x": 2.109996681019138,
-            "y": 6.517605179473628,
-            "heading": 0.31426571552794874,
-            "angularVelocity": 0.6705461310477281,
-            "velocityX": 1.618511819979031,
-            "velocityY": 2.039248261272889,
-            "timestamp": 0.5478377963280157
-        },
-        {
-            "x": 2.204998332006287,
-            "y": 6.637302612224517,
-            "heading": 0.35363163522076196,
-            "angularVelocity": 0.5029982226430836,
-            "velocityX": 1.2138840389754155,
-            "velocityY": 1.5294345057465644,
-            "timestamp": 0.6261003386605893
-        },
-        {
-            "x": 2.268332773183591,
-            "y": 6.717100865272008,
-            "heading": 0.3798775217934225,
-            "angularVelocity": 0.3353569382033373,
-            "velocityX": 0.8092561178011136,
-            "velocityY": 1.0196225508288703,
-            "timestamp": 0.704362880993163
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.1676725265429579,
-            "velocityX": 0.40462813847574725,
-            "velocityY": 0.509811380244222,
-            "timestamp": 0.7826254233257367
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 9.092584352802723e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0.8608879656583104
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.1.traj
deleted file mode 100644
index bb0e9680..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.1.traj	
+++ /dev/null
@@ -1,76 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 7.039472547405692e-42,
-            "angularVelocity": 1.0705191064262213e-40,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 1.4250000045295101,
-            "y": 5.56,
-            "heading": 8.194631828276944e-23,
-            "angularVelocity": 8.6403876700238e-22,
-            "velocityX": 0.7907971475606789,
-            "velocityY": 8.424071056371489e-35,
-            "timestamp": 0.0948410155004449
-        },
-        {
-            "x": 1.5750000090590195,
-            "y": 5.56,
-            "heading": 2.6954329855279996e-22,
-            "angularVelocity": 1.978015178210688e-21,
-            "velocityX": 1.5815942473623736,
-            "velocityY": 1.2488550168110521e-34,
-            "timestamp": 0.1896820310008898
-        },
-        {
-            "x": 1.7999999999999998,
-            "y": 5.56,
-            "heading": 5.87848829010012e-22,
-            "angularVelocity": 3.3562006983676933e-21,
-            "velocityX": 2.3723912038871497,
-            "velocityY": 2.135553914770072e-34,
-            "timestamp": 0.2845230465013347
-        },
-        {
-            "x": 2.0249999909409806,
-            "y": 5.56,
-            "heading": 4.475372520266276e-22,
-            "angularVelocity": -1.4794393741169351e-21,
-            "velocityX": 2.37239120388715,
-            "velocityY": 2.583104828416982e-34,
-            "timestamp": 0.3793640620017796
-        },
-        {
-            "x": 2.17499999547049,
-            "y": 5.56,
-            "heading": 1.6184867431561565e-22,
-            "angularVelocity": -3.012289189189882e-21,
-            "velocityX": 1.5815942473623736,
-            "velocityY": 1.9272829246954695e-34,
-            "timestamp": 0.47420507750222446
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 1.246024226890136e-41,
-            "angularVelocity": -1.7065260802739357e-21,
-            "velocityX": 0.7907971475606789,
-            "velocityY": -3.1542839688982834e-34,
-            "timestamp": 0.5690460930026694
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 7.131086050293936e-41,
-            "angularVelocity": 6.207915462550707e-40,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0.6638871085031143
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.traj
deleted file mode 100644
index bb0e9680..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.traj	
+++ /dev/null
@@ -1,76 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 7.039472547405692e-42,
-            "angularVelocity": 1.0705191064262213e-40,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 1.4250000045295101,
-            "y": 5.56,
-            "heading": 8.194631828276944e-23,
-            "angularVelocity": 8.6403876700238e-22,
-            "velocityX": 0.7907971475606789,
-            "velocityY": 8.424071056371489e-35,
-            "timestamp": 0.0948410155004449
-        },
-        {
-            "x": 1.5750000090590195,
-            "y": 5.56,
-            "heading": 2.6954329855279996e-22,
-            "angularVelocity": 1.978015178210688e-21,
-            "velocityX": 1.5815942473623736,
-            "velocityY": 1.2488550168110521e-34,
-            "timestamp": 0.1896820310008898
-        },
-        {
-            "x": 1.7999999999999998,
-            "y": 5.56,
-            "heading": 5.87848829010012e-22,
-            "angularVelocity": 3.3562006983676933e-21,
-            "velocityX": 2.3723912038871497,
-            "velocityY": 2.135553914770072e-34,
-            "timestamp": 0.2845230465013347
-        },
-        {
-            "x": 2.0249999909409806,
-            "y": 5.56,
-            "heading": 4.475372520266276e-22,
-            "angularVelocity": -1.4794393741169351e-21,
-            "velocityX": 2.37239120388715,
-            "velocityY": 2.583104828416982e-34,
-            "timestamp": 0.3793640620017796
-        },
-        {
-            "x": 2.17499999547049,
-            "y": 5.56,
-            "heading": 1.6184867431561565e-22,
-            "angularVelocity": -3.012289189189882e-21,
-            "velocityX": 1.5815942473623736,
-            "velocityY": 1.9272829246954695e-34,
-            "timestamp": 0.47420507750222446
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 1.246024226890136e-41,
-            "angularVelocity": -1.7065260802739357e-21,
-            "velocityX": 0.7907971475606789,
-            "velocityY": -3.1542839688982834e-34,
-            "timestamp": 0.5690460930026694
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 7.131086050293936e-41,
-            "angularVelocity": 6.207915462550707e-40,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0.6638871085031143
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.1.traj
deleted file mode 100644
index aceaac81..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.1.traj	
+++ /dev/null
@@ -1,112 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 5.56203910119815e-35,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 1.3816658774685813,
-            "y": 5.519999060819193,
-            "heading": -0.012983997100427386,
-            "angularVelocity": -0.16577951920330813,
-            "velocityX": 0.4043095436088268,
-            "velocityY": -0.5107315115509736,
-            "timestamp": 0.07832087559926142
-        },
-        {
-            "x": 1.4449977779940397,
-            "y": 5.439997258819368,
-            "heading": -0.038949622534069785,
-            "angularVelocity": -0.33152879401526647,
-            "velocityX": 0.8086209460872757,
-            "velocityY": -1.0214620481155445,
-            "timestamp": 0.15664175119852283
-        },
-        {
-            "x": 1.5399959199593471,
-            "y": 5.3199947242673264,
-            "heading": -0.07789432529768456,
-            "angularVelocity": -0.497245497648426,
-            "velocityX": 1.2129351368768808,
-            "velocityY": -1.5321909214351659,
-            "timestamp": 0.23496262679778424
-        },
-        {
-            "x": 1.6666605759505058,
-            "y": 5.159991667949077,
-            "heading": -0.1298174067167751,
-            "angularVelocity": -0.662953280613991,
-            "velocityX": 1.6172528080412967,
-            "velocityY": -2.042917103441561,
-            "timestamp": 0.31328350239704567
-        },
-        {
-            "x": 1.8249920311985446,
-            "y": 4.959988411604181,
-            "heading": -0.19472272142711608,
-            "angularVelocity": -0.828710279522869,
-            "velocityX": 2.0215741210320677,
-            "velocityY": -2.5536391774810774,
-            "timestamp": 0.3916043779963071
-        },
-        {
-            "x": 1.9833279596837488,
-            "y": 4.759991951944026,
-            "heading": -0.259798921868918,
-            "angularVelocity": -0.8308921464919682,
-            "velocityX": 2.021631235270526,
-            "velocityY": -2.553552397491847,
-            "timestamp": 0.4699252535955685
-        },
-        {
-            "x": 2.1099967561163506,
-            "y": 4.599995051908685,
-            "heading": -0.3118731002379449,
-            "angularVelocity": -0.6648824846579983,
-            "velocityX": 1.6173056731479674,
-            "velocityY": -2.042838500095239,
-            "timestamp": 0.54824612919483
-        },
-        {
-            "x": 2.2049983692603425,
-            "y": 4.4799975041635465,
-            "heading": -0.3509353765811358,
-            "angularVelocity": -0.4987466757018699,
-            "velocityX": 1.2129794568446812,
-            "velocityY": -1.532127249944463,
-            "timestamp": 0.6265670047940913
-        },
-        {
-            "x": 2.268332785802464,
-            "y": 4.399999173265512,
-            "heading": -0.3769787554180828,
-            "angularVelocity": -0.33252154853581545,
-            "velocityX": 0.808653070557838,
-            "velocityY": -1.021417729129522,
-            "timestamp": 0.7048878803933527
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.16625509460009477,
-            "velocityX": 0.40432661094808764,
-            "velocityY": -0.5107089643656818,
-            "timestamp": 0.7832087559926141
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -5.703180031220666e-32,
-            "velocityX": 1.0257416304055705e-32,
-            "velocityY": 0,
-            "timestamp": 0.8615296315918755
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.traj
deleted file mode 100644
index aceaac81..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.traj	
+++ /dev/null
@@ -1,112 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 5.56203910119815e-35,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 1.3816658774685813,
-            "y": 5.519999060819193,
-            "heading": -0.012983997100427386,
-            "angularVelocity": -0.16577951920330813,
-            "velocityX": 0.4043095436088268,
-            "velocityY": -0.5107315115509736,
-            "timestamp": 0.07832087559926142
-        },
-        {
-            "x": 1.4449977779940397,
-            "y": 5.439997258819368,
-            "heading": -0.038949622534069785,
-            "angularVelocity": -0.33152879401526647,
-            "velocityX": 0.8086209460872757,
-            "velocityY": -1.0214620481155445,
-            "timestamp": 0.15664175119852283
-        },
-        {
-            "x": 1.5399959199593471,
-            "y": 5.3199947242673264,
-            "heading": -0.07789432529768456,
-            "angularVelocity": -0.497245497648426,
-            "velocityX": 1.2129351368768808,
-            "velocityY": -1.5321909214351659,
-            "timestamp": 0.23496262679778424
-        },
-        {
-            "x": 1.6666605759505058,
-            "y": 5.159991667949077,
-            "heading": -0.1298174067167751,
-            "angularVelocity": -0.662953280613991,
-            "velocityX": 1.6172528080412967,
-            "velocityY": -2.042917103441561,
-            "timestamp": 0.31328350239704567
-        },
-        {
-            "x": 1.8249920311985446,
-            "y": 4.959988411604181,
-            "heading": -0.19472272142711608,
-            "angularVelocity": -0.828710279522869,
-            "velocityX": 2.0215741210320677,
-            "velocityY": -2.5536391774810774,
-            "timestamp": 0.3916043779963071
-        },
-        {
-            "x": 1.9833279596837488,
-            "y": 4.759991951944026,
-            "heading": -0.259798921868918,
-            "angularVelocity": -0.8308921464919682,
-            "velocityX": 2.021631235270526,
-            "velocityY": -2.553552397491847,
-            "timestamp": 0.4699252535955685
-        },
-        {
-            "x": 2.1099967561163506,
-            "y": 4.599995051908685,
-            "heading": -0.3118731002379449,
-            "angularVelocity": -0.6648824846579983,
-            "velocityX": 1.6173056731479674,
-            "velocityY": -2.042838500095239,
-            "timestamp": 0.54824612919483
-        },
-        {
-            "x": 2.2049983692603425,
-            "y": 4.4799975041635465,
-            "heading": -0.3509353765811358,
-            "angularVelocity": -0.4987466757018699,
-            "velocityX": 1.2129794568446812,
-            "velocityY": -1.532127249944463,
-            "timestamp": 0.6265670047940913
-        },
-        {
-            "x": 2.268332785802464,
-            "y": 4.399999173265512,
-            "heading": -0.3769787554180828,
-            "angularVelocity": -0.33252154853581545,
-            "velocityX": 0.808653070557838,
-            "velocityY": -1.021417729129522,
-            "timestamp": 0.7048878803933527
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.16625509460009477,
-            "velocityX": 0.40432661094808764,
-            "velocityY": -0.5107089643656818,
-            "timestamp": 0.7832087559926141
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -5.703180031220666e-32,
-            "velocityX": 1.0257416304055705e-32,
-            "velocityY": 0,
-            "timestamp": 0.8615296315918755
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.1.traj
deleted file mode 100644
index 66955466..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.238209882810899,
-            "y": 6.650800869780916,
-            "heading": 0.2470077179050236,
-            "angularVelocity": -0.14636941117769697,
-            "velocityX": 0.7008486923098889,
-            "velocityY": 0.23433997503541223,
-            "timestamp": 0.08876364255722656
-        },
-        {
-            "x": 5.362652939631404,
-            "y": 6.692404642961414,
-            "heading": 0.22258646311132924,
-            "angularVelocity": -0.2751267759001645,
-            "velocityX": 1.401959780326283,
-            "velocityY": 0.4687028605622118,
-            "timestamp": 0.17752728511445312
-        },
-        {
-            "x": 5.549362223961953,
-            "y": 6.754812642679732,
-            "heading": 0.18937073322571307,
-            "angularVelocity": -0.37420422291202055,
-            "velocityX": 2.1034432449090223,
-            "velocityY": 0.7030806523975865,
-            "timestamp": 0.26629092767167967
-        },
-        {
-            "x": 5.798381050208957,
-            "y": 6.8380191875714225,
-            "heading": 0.15275465831989177,
-            "angularVelocity": -0.4125120809759026,
-            "velocityX": 2.8054146841303256,
-            "velocityY": 0.9373944386978721,
-            "timestamp": 0.35505457022890624
-        },
-        {
-            "x": 6.109622945204874,
-            "y": 6.941929047341865,
-            "heading": 0.1302624797347834,
-            "angularVelocity": -0.25339404667863696,
-            "velocityX": 3.506411927553799,
-            "velocityY": 1.1706353725121932,
-            "timestamp": 0.4438182127861328
-        },
-        {
-            "x": 6.4280450770381,
-            "y": 7.048515369175043,
-            "heading": 0.1302624686930749,
-            "angularVelocity": -1.2439475582812342e-7,
-            "velocityX": 3.587303570016191,
-            "velocityY": 1.2007880564873126,
-            "timestamp": 0.5325818553433593
-        },
-        {
-            "x": 6.746467208870004,
-            "y": 7.1551016910121685,
-            "heading": 0.13026245765136438,
-            "angularVelocity": -1.2439477842907252e-7,
-            "velocityX": 3.5873035700012967,
-            "velocityY": 1.2007880565318134,
-            "timestamp": 0.6213454979005859
-        },
-        {
-            "x": 7.057671454315644,
-            "y": 7.258999412253756,
-            "heading": 0.10797351007767278,
-            "angularVelocity": -0.2511044717394208,
-            "velocityX": 3.5059877724700192,
-            "velocityY": 1.1704986213770205,
-            "timestamp": 0.7101091404578125
-        },
-        {
-            "x": 7.3066745305511045,
-            "y": 7.342200909102172,
-            "heading": 0.07025614763700679,
-            "angularVelocity": -0.4249190474112408,
-            "velocityX": 2.8052372464872497,
-            "velocityY": 0.9373375680789704,
-            "timestamp": 0.7988727830150391
-        },
-        {
-            "x": 7.49336787770315,
-            "y": 7.4046033027166684,
-            "heading": 0.03585076588111199,
-            "angularVelocity": -0.3876066908033862,
-            "velocityX": 2.103263698666567,
-            "velocityY": 0.7030174947390144,
-            "timestamp": 0.8876364255722656
-        },
-        {
-            "x": 7.617797738081694,
-            "y": 7.446202129881464,
-            "heading": 0.010501179707511226,
-            "angularVelocity": -0.2855852401136722,
-            "velocityX": 1.4018111108759532,
-            "velocityY": 0.4686471393732002,
-            "timestamp": 0.9764000681294922
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -0.003,
-            "angularVelocity": -0.15210258748116404,
-            "velocityX": 0.7007628362953491,
-            "velocityY": 0.23430618121741137,
-            "timestamp": 1.0651637106867187
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -0.003,
-            "angularVelocity": -3.7910464085672074e-31,
-            "velocityX": 1.2889893239032156e-32,
-            "velocityY": 1.1630630989344696e-31,
-            "timestamp": 1.1539273532439451
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.traj
deleted file mode 100644
index 66955466..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.238209882810899,
-            "y": 6.650800869780916,
-            "heading": 0.2470077179050236,
-            "angularVelocity": -0.14636941117769697,
-            "velocityX": 0.7008486923098889,
-            "velocityY": 0.23433997503541223,
-            "timestamp": 0.08876364255722656
-        },
-        {
-            "x": 5.362652939631404,
-            "y": 6.692404642961414,
-            "heading": 0.22258646311132924,
-            "angularVelocity": -0.2751267759001645,
-            "velocityX": 1.401959780326283,
-            "velocityY": 0.4687028605622118,
-            "timestamp": 0.17752728511445312
-        },
-        {
-            "x": 5.549362223961953,
-            "y": 6.754812642679732,
-            "heading": 0.18937073322571307,
-            "angularVelocity": -0.37420422291202055,
-            "velocityX": 2.1034432449090223,
-            "velocityY": 0.7030806523975865,
-            "timestamp": 0.26629092767167967
-        },
-        {
-            "x": 5.798381050208957,
-            "y": 6.8380191875714225,
-            "heading": 0.15275465831989177,
-            "angularVelocity": -0.4125120809759026,
-            "velocityX": 2.8054146841303256,
-            "velocityY": 0.9373944386978721,
-            "timestamp": 0.35505457022890624
-        },
-        {
-            "x": 6.109622945204874,
-            "y": 6.941929047341865,
-            "heading": 0.1302624797347834,
-            "angularVelocity": -0.25339404667863696,
-            "velocityX": 3.506411927553799,
-            "velocityY": 1.1706353725121932,
-            "timestamp": 0.4438182127861328
-        },
-        {
-            "x": 6.4280450770381,
-            "y": 7.048515369175043,
-            "heading": 0.1302624686930749,
-            "angularVelocity": -1.2439475582812342e-7,
-            "velocityX": 3.587303570016191,
-            "velocityY": 1.2007880564873126,
-            "timestamp": 0.5325818553433593
-        },
-        {
-            "x": 6.746467208870004,
-            "y": 7.1551016910121685,
-            "heading": 0.13026245765136438,
-            "angularVelocity": -1.2439477842907252e-7,
-            "velocityX": 3.5873035700012967,
-            "velocityY": 1.2007880565318134,
-            "timestamp": 0.6213454979005859
-        },
-        {
-            "x": 7.057671454315644,
-            "y": 7.258999412253756,
-            "heading": 0.10797351007767278,
-            "angularVelocity": -0.2511044717394208,
-            "velocityX": 3.5059877724700192,
-            "velocityY": 1.1704986213770205,
-            "timestamp": 0.7101091404578125
-        },
-        {
-            "x": 7.3066745305511045,
-            "y": 7.342200909102172,
-            "heading": 0.07025614763700679,
-            "angularVelocity": -0.4249190474112408,
-            "velocityX": 2.8052372464872497,
-            "velocityY": 0.9373375680789704,
-            "timestamp": 0.7988727830150391
-        },
-        {
-            "x": 7.49336787770315,
-            "y": 7.4046033027166684,
-            "heading": 0.03585076588111199,
-            "angularVelocity": -0.3876066908033862,
-            "velocityX": 2.103263698666567,
-            "velocityY": 0.7030174947390144,
-            "timestamp": 0.8876364255722656
-        },
-        {
-            "x": 7.617797738081694,
-            "y": 7.446202129881464,
-            "heading": 0.010501179707511226,
-            "angularVelocity": -0.2855852401136722,
-            "velocityX": 1.4018111108759532,
-            "velocityY": 0.4686471393732002,
-            "timestamp": 0.9764000681294922
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -0.003,
-            "angularVelocity": -0.15210258748116404,
-            "velocityX": 0.7007628362953491,
-            "velocityY": 0.23430618121741137,
-            "timestamp": 1.0651637106867187
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -0.003,
-            "angularVelocity": -3.7910464085672074e-31,
-            "velocityX": 1.2889893239032156e-32,
-            "velocityY": 1.1630630989344696e-31,
-            "timestamp": 1.1539273532439451
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.1.traj
deleted file mode 100644
index 63556f1f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.242063425541118,
-            "y": 6.625917379024557,
-            "heading": 0.21680591849920633,
-            "angularVelocity": -0.48090771817524164,
-            "velocityX": 0.735526954803639,
-            "velocityY": -0.04545446665971497,
-            "timestamp": 0.08981781715770647
-        },
-        {
-            "x": 5.374390185654174,
-            "y": 6.616899293663565,
-            "heading": 0.13592925245739512,
-            "angularVelocity": -0.9004523668149724,
-            "velocityX": 1.473279626476675,
-            "velocityY": -0.10040419202302918,
-            "timestamp": 0.17963563431541293
-        },
-        {
-            "x": 5.573225936717345,
-            "y": 6.601485332040172,
-            "heading": 0.027174297785337913,
-            "angularVelocity": -1.2108394315695736,
-            "velocityX": 2.2137673499017074,
-            "velocityY": -0.1716136298027423,
-            "timestamp": 0.2694534514731194
-        },
-        {
-            "x": 5.8386201791233034,
-            "y": 6.576536765329541,
-            "heading": -0.0884788299359431,
-            "angularVelocity": -1.287641265186969,
-            "velocityX": 2.9548061932964313,
-            "velocityY": -0.27776857087079165,
-            "timestamp": 0.35927126863082587
-        },
-        {
-            "x": 6.16462893780028,
-            "y": 6.53182752327052,
-            "heading": -0.12460373299787371,
-            "angularVelocity": -0.4022019706680331,
-            "velocityX": 3.6296669078981743,
-            "velocityY": -0.49777698316268804,
-            "timestamp": 0.44908908578853235
-        },
-        {
-            "x": 6.485747891959683,
-            "y": 6.420787077377064,
-            "heading": -0.12460373462910332,
-            "angularVelocity": -1.816153684215185e-8,
-            "velocityX": 3.5752255434527758,
-            "velocityY": -1.2362852873443355,
-            "timestamp": 0.5389069029462388
-        },
-        {
-            "x": 6.7868856142210525,
-            "y": 6.263423057279399,
-            "heading": -0.12460372221510443,
-            "angularVelocity": 1.382130993675135e-7,
-            "velocityX": 3.352761532075727,
-            "velocityY": -1.7520356770790513,
-            "timestamp": 0.6287247201039452
-        },
-        {
-            "x": 7.0844936481565455,
-            "y": 6.108042080528118,
-            "heading": -0.11377963349551397,
-            "angularVelocity": 0.12051159850149781,
-            "velocityX": 3.313463223148008,
-            "velocityY": -1.7299571696165401,
-            "timestamp": 0.7185425372616516
-        },
-        {
-            "x": 7.322771112714566,
-            "y": 5.983596379959823,
-            "heading": -0.07830444960996434,
-            "angularVelocity": 0.39496822577262786,
-            "velocityX": 2.652897521876331,
-            "velocityY": -1.3855346801602506,
-            "timestamp": 0.808360354419358
-        },
-        {
-            "x": 7.501421172621153,
-            "y": 5.8902827497271355,
-            "heading": -0.04357887494407857,
-            "angularVelocity": 0.3866223402525239,
-            "velocityX": 1.9890269610193678,
-            "velocityY": -1.038921153793384,
-            "timestamp": 0.8981781715770645
-        },
-        {
-            "x": 7.62048387761172,
-            "y": 5.828089668977501,
-            "heading": -0.01724918093308134,
-            "angularVelocity": 0.29314555668577735,
-            "velocityX": 1.3256022998366965,
-            "velocityY": -0.6924358965486016,
-            "timestamp": 0.9879959887347709
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -0.003,
-            "angularVelocity": 0.15864537108558247,
-            "velocityX": 0.6626315832612425,
-            "velocityY": -0.346141444552285,
-            "timestamp": 1.0778138058924773
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -0.003,
-            "angularVelocity": 0,
-            "velocityX": 4.386752331989148e-33,
-            "velocityY": -2.33652586669764e-33,
-            "timestamp": 1.1676316230501838
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.traj
deleted file mode 100644
index 63556f1f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.242063425541118,
-            "y": 6.625917379024557,
-            "heading": 0.21680591849920633,
-            "angularVelocity": -0.48090771817524164,
-            "velocityX": 0.735526954803639,
-            "velocityY": -0.04545446665971497,
-            "timestamp": 0.08981781715770647
-        },
-        {
-            "x": 5.374390185654174,
-            "y": 6.616899293663565,
-            "heading": 0.13592925245739512,
-            "angularVelocity": -0.9004523668149724,
-            "velocityX": 1.473279626476675,
-            "velocityY": -0.10040419202302918,
-            "timestamp": 0.17963563431541293
-        },
-        {
-            "x": 5.573225936717345,
-            "y": 6.601485332040172,
-            "heading": 0.027174297785337913,
-            "angularVelocity": -1.2108394315695736,
-            "velocityX": 2.2137673499017074,
-            "velocityY": -0.1716136298027423,
-            "timestamp": 0.2694534514731194
-        },
-        {
-            "x": 5.8386201791233034,
-            "y": 6.576536765329541,
-            "heading": -0.0884788299359431,
-            "angularVelocity": -1.287641265186969,
-            "velocityX": 2.9548061932964313,
-            "velocityY": -0.27776857087079165,
-            "timestamp": 0.35927126863082587
-        },
-        {
-            "x": 6.16462893780028,
-            "y": 6.53182752327052,
-            "heading": -0.12460373299787371,
-            "angularVelocity": -0.4022019706680331,
-            "velocityX": 3.6296669078981743,
-            "velocityY": -0.49777698316268804,
-            "timestamp": 0.44908908578853235
-        },
-        {
-            "x": 6.485747891959683,
-            "y": 6.420787077377064,
-            "heading": -0.12460373462910332,
-            "angularVelocity": -1.816153684215185e-8,
-            "velocityX": 3.5752255434527758,
-            "velocityY": -1.2362852873443355,
-            "timestamp": 0.5389069029462388
-        },
-        {
-            "x": 6.7868856142210525,
-            "y": 6.263423057279399,
-            "heading": -0.12460372221510443,
-            "angularVelocity": 1.382130993675135e-7,
-            "velocityX": 3.352761532075727,
-            "velocityY": -1.7520356770790513,
-            "timestamp": 0.6287247201039452
-        },
-        {
-            "x": 7.0844936481565455,
-            "y": 6.108042080528118,
-            "heading": -0.11377963349551397,
-            "angularVelocity": 0.12051159850149781,
-            "velocityX": 3.313463223148008,
-            "velocityY": -1.7299571696165401,
-            "timestamp": 0.7185425372616516
-        },
-        {
-            "x": 7.322771112714566,
-            "y": 5.983596379959823,
-            "heading": -0.07830444960996434,
-            "angularVelocity": 0.39496822577262786,
-            "velocityX": 2.652897521876331,
-            "velocityY": -1.3855346801602506,
-            "timestamp": 0.808360354419358
-        },
-        {
-            "x": 7.501421172621153,
-            "y": 5.8902827497271355,
-            "heading": -0.04357887494407857,
-            "angularVelocity": 0.3866223402525239,
-            "velocityX": 1.9890269610193678,
-            "velocityY": -1.038921153793384,
-            "timestamp": 0.8981781715770645
-        },
-        {
-            "x": 7.62048387761172,
-            "y": 5.828089668977501,
-            "heading": -0.01724918093308134,
-            "angularVelocity": 0.29314555668577735,
-            "velocityX": 1.3256022998366965,
-            "velocityY": -0.6924358965486016,
-            "timestamp": 0.9879959887347709
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -0.003,
-            "angularVelocity": 0.15864537108558247,
-            "velocityX": 0.6626315832612425,
-            "velocityY": -0.346141444552285,
-            "timestamp": 1.0778138058924773
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -0.003,
-            "angularVelocity": 0,
-            "velocityX": 4.386752331989148e-33,
-            "velocityY": -2.33652586669764e-33,
-            "timestamp": 1.1676316230501838
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3.1.traj
deleted file mode 100644
index b1b40339..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3.1.traj	
+++ /dev/null
@@ -1,157 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.246662494261999,
-            "y": 6.628692732737936,
-            "heading": 0.18398762968545185,
-            "angularVelocity": -0.8088463398355284,
-            "velocityX": 0.7519183997414132,
-            "velocityY": -0.013910608703967415,
-            "timestamp": 0.09397628025368156
-        },
-        {
-            "x": 5.3885394117424426,
-            "y": 6.6228569584156665,
-            "heading": 0.04133864379940616,
-            "angularVelocity": -1.5179254328962157,
-            "velocityX": 1.509709866122152,
-            "velocityY": -0.0620983753189278,
-            "timestamp": 0.18795256050736311
-        },
-        {
-            "x": 5.602094427902648,
-            "y": 6.606624752166298,
-            "heading": -0.14949344735311892,
-            "angularVelocity": -2.0306410366252936,
-            "velocityX": 2.272435295201409,
-            "velocityY": -0.172726630651381,
-            "timestamp": 0.2819288407610447
-        },
-        {
-            "x": 5.885007714232348,
-            "y": 6.565697735942791,
-            "heading": -0.3456589598447775,
-            "angularVelocity": -2.087393882393782,
-            "velocityX": 3.010475468554385,
-            "velocityY": -0.4355036836213049,
-            "timestamp": 0.37590512101472623
-        },
-        {
-            "x": 6.196762938930662,
-            "y": 6.464967292979978,
-            "heading": -0.4305176558630451,
-            "angularVelocity": -0.9029799412064233,
-            "velocityX": 3.3173820442430326,
-            "velocityY": -1.0718709305252299,
-            "timestamp": 0.4698814012684078
-        },
-        {
-            "x": 6.507653773495931,
-            "y": 6.294193106326001,
-            "heading": -0.4331106719684992,
-            "angularVelocity": -0.027592240280786797,
-            "velocityX": 3.3081840835372915,
-            "velocityY": -1.8172052159649787,
-            "timestamp": 0.5638576815220894
-        },
-        {
-            "x": 6.777249226823148,
-            "y": 6.0624534109685,
-            "heading": -0.433110646067504,
-            "angularVelocity": 2.7561204856502443e-7,
-            "velocityX": 2.8687606340606973,
-            "velocityY": -2.465938157287546,
-            "timestamp": 0.6578339617757709
-        },
-        {
-            "x": 6.99331744885121,
-            "y": 5.780142381909384,
-            "heading": -0.4331105937998759,
-            "angularVelocity": 5.561789419211182e-7,
-            "velocityX": 2.2991782761011916,
-            "velocityY": -3.004066859180208,
-            "timestamp": 0.7518102420294525
-        },
-        {
-            "x": 7.1465883664001515,
-            "y": 5.459372959434879,
-            "heading": -0.43311054984028735,
-            "angularVelocity": 4.677732342413875e-7,
-            "velocityX": 1.6309532270823943,
-            "velocityY": -3.4133019694822235,
-            "timestamp": 0.845786522283134
-        },
-        {
-            "x": 7.2797482024400875,
-            "y": 5.1297467242247805,
-            "heading": -0.4331105067287741,
-            "angularVelocity": 4.5874887908824285e-7,
-            "velocityX": 1.4169515507581365,
-            "velocityY": -3.507547163180951,
-            "timestamp": 0.9397628025368155
-        },
-        {
-            "x": 7.407925031698652,
-            "y": 4.799268903667217,
-            "heading": -0.4305589051420175,
-            "angularVelocity": 0.027151549091629798,
-            "velocityX": 1.3639274603395894,
-            "velocityY": -3.5166088683810885,
-            "timestamp": 1.033739082790497
-        },
-        {
-            "x": 7.517390810231398,
-            "y": 4.529437032753176,
-            "heading": -0.29559925755962413,
-            "angularVelocity": 1.4361033147734772,
-            "velocityX": 1.164823487770018,
-            "velocityY": -2.871276349581525,
-            "timestamp": 1.1277153630441787
-        },
-        {
-            "x": 7.59899827381295,
-            "y": 4.327770783855267,
-            "heading": -0.16069313013903275,
-            "angularVelocity": 1.4355338076419173,
-            "velocityX": 0.8683836321384404,
-            "velocityY": -2.1459271249460685,
-            "timestamp": 1.2216916432978604
-        },
-        {
-            "x": 7.653087401223331,
-            "y": 4.193799869992724,
-            "heading": -0.05829734753961687,
-            "angularVelocity": 1.0895917812772173,
-            "velocityX": 0.5755614849233384,
-            "velocityY": -1.4255822160751557,
-            "timestamp": 1.315667923551542
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -0.003,
-            "angularVelocity": 0.5884181347713066,
-            "velocityX": 0.28637650590148805,
-            "velocityY": -0.7108162805806236,
-            "timestamp": 1.4096442038052237
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -0.003,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.8051389340682807e-33,
-            "timestamp": 1.5036204840589054
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3.traj
deleted file mode 100644
index b1b40339..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3.traj	
+++ /dev/null
@@ -1,157 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.246662494261999,
-            "y": 6.628692732737936,
-            "heading": 0.18398762968545185,
-            "angularVelocity": -0.8088463398355284,
-            "velocityX": 0.7519183997414132,
-            "velocityY": -0.013910608703967415,
-            "timestamp": 0.09397628025368156
-        },
-        {
-            "x": 5.3885394117424426,
-            "y": 6.6228569584156665,
-            "heading": 0.04133864379940616,
-            "angularVelocity": -1.5179254328962157,
-            "velocityX": 1.509709866122152,
-            "velocityY": -0.0620983753189278,
-            "timestamp": 0.18795256050736311
-        },
-        {
-            "x": 5.602094427902648,
-            "y": 6.606624752166298,
-            "heading": -0.14949344735311892,
-            "angularVelocity": -2.0306410366252936,
-            "velocityX": 2.272435295201409,
-            "velocityY": -0.172726630651381,
-            "timestamp": 0.2819288407610447
-        },
-        {
-            "x": 5.885007714232348,
-            "y": 6.565697735942791,
-            "heading": -0.3456589598447775,
-            "angularVelocity": -2.087393882393782,
-            "velocityX": 3.010475468554385,
-            "velocityY": -0.4355036836213049,
-            "timestamp": 0.37590512101472623
-        },
-        {
-            "x": 6.196762938930662,
-            "y": 6.464967292979978,
-            "heading": -0.4305176558630451,
-            "angularVelocity": -0.9029799412064233,
-            "velocityX": 3.3173820442430326,
-            "velocityY": -1.0718709305252299,
-            "timestamp": 0.4698814012684078
-        },
-        {
-            "x": 6.507653773495931,
-            "y": 6.294193106326001,
-            "heading": -0.4331106719684992,
-            "angularVelocity": -0.027592240280786797,
-            "velocityX": 3.3081840835372915,
-            "velocityY": -1.8172052159649787,
-            "timestamp": 0.5638576815220894
-        },
-        {
-            "x": 6.777249226823148,
-            "y": 6.0624534109685,
-            "heading": -0.433110646067504,
-            "angularVelocity": 2.7561204856502443e-7,
-            "velocityX": 2.8687606340606973,
-            "velocityY": -2.465938157287546,
-            "timestamp": 0.6578339617757709
-        },
-        {
-            "x": 6.99331744885121,
-            "y": 5.780142381909384,
-            "heading": -0.4331105937998759,
-            "angularVelocity": 5.561789419211182e-7,
-            "velocityX": 2.2991782761011916,
-            "velocityY": -3.004066859180208,
-            "timestamp": 0.7518102420294525
-        },
-        {
-            "x": 7.1465883664001515,
-            "y": 5.459372959434879,
-            "heading": -0.43311054984028735,
-            "angularVelocity": 4.677732342413875e-7,
-            "velocityX": 1.6309532270823943,
-            "velocityY": -3.4133019694822235,
-            "timestamp": 0.845786522283134
-        },
-        {
-            "x": 7.2797482024400875,
-            "y": 5.1297467242247805,
-            "heading": -0.4331105067287741,
-            "angularVelocity": 4.5874887908824285e-7,
-            "velocityX": 1.4169515507581365,
-            "velocityY": -3.507547163180951,
-            "timestamp": 0.9397628025368155
-        },
-        {
-            "x": 7.407925031698652,
-            "y": 4.799268903667217,
-            "heading": -0.4305589051420175,
-            "angularVelocity": 0.027151549091629798,
-            "velocityX": 1.3639274603395894,
-            "velocityY": -3.5166088683810885,
-            "timestamp": 1.033739082790497
-        },
-        {
-            "x": 7.517390810231398,
-            "y": 4.529437032753176,
-            "heading": -0.29559925755962413,
-            "angularVelocity": 1.4361033147734772,
-            "velocityX": 1.164823487770018,
-            "velocityY": -2.871276349581525,
-            "timestamp": 1.1277153630441787
-        },
-        {
-            "x": 7.59899827381295,
-            "y": 4.327770783855267,
-            "heading": -0.16069313013903275,
-            "angularVelocity": 1.4355338076419173,
-            "velocityX": 0.8683836321384404,
-            "velocityY": -2.1459271249460685,
-            "timestamp": 1.2216916432978604
-        },
-        {
-            "x": 7.653087401223331,
-            "y": 4.193799869992724,
-            "heading": -0.05829734753961687,
-            "angularVelocity": 1.0895917812772173,
-            "velocityX": 0.5755614849233384,
-            "velocityY": -1.4255822160751557,
-            "timestamp": 1.315667923551542
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -0.003,
-            "angularVelocity": 0.5884181347713066,
-            "velocityX": 0.28637650590148805,
-            "velocityY": -0.7108162805806236,
-            "timestamp": 1.4096442038052237
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -0.003,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.8051389340682807e-33,
-            "timestamp": 1.5036204840589054
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C4.1.traj
deleted file mode 100644
index 3f479acb..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C4.1.traj	
+++ /dev/null
@@ -1,247 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 0,
-            "velocityX": -1.7123315632321391e-31,
-            "velocityY": 4.571975133912053e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 5.2081944432473115,
-            "y": 6.63354239454729,
-            "heading": 0.2524699257138384,
-            "angularVelocity": -0.12069194203891866,
-            "velocityX": 0.5160121574790498,
-            "velocityY": 0.0567774581143745,
-            "timestamp": 0.062390861883169
-        },
-        {
-            "x": 5.2726441787772975,
-            "y": 6.640106281427696,
-            "heading": 0.23785051964847434,
-            "angularVelocity": -0.2343196683632906,
-            "velocityX": 1.0329996025807835,
-            "velocityY": 0.1052059016703473,
-            "timestamp": 0.124781723766338
-        },
-        {
-            "x": 5.369418980987113,
-            "y": 6.648966513737627,
-            "heading": 0.21675536507543205,
-            "angularVelocity": -0.33811288923278293,
-            "velocityX": 1.551105390898958,
-            "velocityY": 0.14201169918963874,
-            "timestamp": 0.187172585649507
-        },
-        {
-            "x": 5.498590594297912,
-            "y": 6.659046571836944,
-            "heading": 0.19009632617593986,
-            "angularVelocity": -0.42729076173705777,
-            "velocityX": 2.070361097955043,
-            "velocityY": 0.16156305258605722,
-            "timestamp": 0.249563447532676
-        },
-        {
-            "x": 5.6601948909824875,
-            "y": 6.668590441412665,
-            "heading": 0.15936442465931713,
-            "angularVelocity": -0.49257055583183496,
-            "velocityX": 2.5901917653772815,
-            "velocityY": 0.15296902924013772,
-            "timestamp": 0.311954309415845
-        },
-        {
-            "x": 5.854024302844021,
-            "y": 6.674275452040902,
-            "heading": 0.12740102584180835,
-            "angularVelocity": -0.512308980077189,
-            "velocityX": 3.106695532183737,
-            "velocityY": 0.09111928344386595,
-            "timestamp": 0.374345171299014
-        },
-        {
-            "x": 6.078018113514707,
-            "y": 6.668089747759068,
-            "heading": 0.10126004011963459,
-            "angularVelocity": -0.4189874115076864,
-            "velocityX": 3.590170161299073,
-            "velocityY": -0.09914439543117176,
-            "timestamp": 0.436736033182183
-        },
-        {
-            "x": 6.311203816285634,
-            "y": 6.63161682590639,
-            "heading": 0.10125992071184643,
-            "angularVelocity": -0.000001913866624365403,
-            "velocityX": 3.737497699704501,
-            "velocityY": -0.584587562213517,
-            "timestamp": 0.499126895065352
-        },
-        {
-            "x": 6.53718081427167,
-            "y": 6.563497401833266,
-            "heading": 0.10125989664992657,
-            "angularVelocity": -3.856641685769623e-7,
-            "velocityX": 3.621956664249834,
-            "velocityY": -1.0918173273625358,
-            "timestamp": 0.561517756948521
-        },
-        {
-            "x": 6.7516755548198235,
-            "y": 6.465019697743108,
-            "heading": 0.10125985817262735,
-            "angularVelocity": -6.167136938553595e-7,
-            "velocityX": 3.4379191771674527,
-            "velocityY": -1.5783994821960954,
-            "timestamp": 0.62390861883169
-        },
-        {
-            "x": 6.950631618499756,
-            "y": 6.338046073913574,
-            "heading": 0.10125971935519859,
-            "angularVelocity": -0.000002224964167076665,
-            "velocityX": 3.1888654471946203,
-            "velocityY": -2.035131748416977,
-            "timestamp": 0.686299480714859
-        },
-        {
-            "x": 7.196058677643184,
-            "y": 6.101503366378845,
-            "heading": 0.10125966337035743,
-            "angularVelocity": -6.213285755242982e-7,
-            "velocityX": 2.723788105663008,
-            "velocityY": -2.625188174087521,
-            "timestamp": 0.7764045291337008
-        },
-        {
-            "x": 7.389899318639429,
-            "y": 5.821123760827136,
-            "heading": 0.10125964077104323,
-            "angularVelocity": -2.508107435019506e-7,
-            "velocityX": 2.1512739230237647,
-            "velocityY": -3.1116969633976366,
-            "timestamp": 0.8665095775525427
-        },
-        {
-            "x": 7.524507781478929,
-            "y": 5.507966472295818,
-            "heading": 0.101259626680438,
-            "angularVelocity": -1.5637975290365853e-7,
-            "velocityX": 1.4939058931947202,
-            "velocityY": -3.475468844716046,
-            "timestamp": 0.9566146259713845
-        },
-        {
-            "x": 7.594574766806078,
-            "y": 5.174383579102459,
-            "heading": 0.10125961555168732,
-            "angularVelocity": -1.235086255399969e-7,
-            "velocityX": 0.7776144240159958,
-            "velocityY": -3.702155418004344,
-            "timestamp": 1.0467196743902263
-        },
-        {
-            "x": 7.605242469312774,
-            "y": 4.833688510035603,
-            "heading": 0.10125960514477245,
-            "angularVelocity": -1.1549757817967306e-7,
-            "velocityX": 0.11839184034432584,
-            "velocityY": -3.781087464524514,
-            "timestamp": 1.136824722809068
-        },
-        {
-            "x": 7.615908675824772,
-            "y": 4.492993394135096,
-            "heading": 0.10125959473787158,
-            "angularVelocity": -1.1549742264017462e-7,
-            "velocityX": 0.11837523756082047,
-            "velocityY": -3.781087984291716,
-            "timestamp": 1.2269297712279097
-        },
-        {
-            "x": 7.6265748461946865,
-            "y": 4.152298277103123,
-            "heading": 0.10125958433097336,
-            "angularVelocity": -1.1549739343149193e-7,
-            "velocityX": 0.11837483645025074,
-            "velocityY": -3.781087996848918,
-            "timestamp": 1.3170348196467514
-        },
-        {
-            "x": 7.637240986091898,
-            "y": 3.8116031591171406,
-            "heading": 0.10125957392407539,
-            "angularVelocity": -1.1549739036497336e-7,
-            "velocityX": 0.11837449825941278,
-            "velocityY": -3.78108800743665,
-            "timestamp": 1.407139868065593
-        },
-        {
-            "x": 7.647907101492937,
-            "y": 3.470908040364265,
-            "heading": 0.10125956351717343,
-            "angularVelocity": -1.1549743491773867e-7,
-            "velocityX": 0.11837422639702805,
-            "velocityY": -3.781088015947766,
-            "timestamp": 1.4972449164844348
-        },
-        {
-            "x": 7.658649980665503,
-            "y": 3.1330359890940187,
-            "heading": 0.09207038783752043,
-            "angularVelocity": -0.10198291706072676,
-            "velocityX": 0.11922616280763662,
-            "velocityY": -3.7497571689844853,
-            "timestamp": 1.5873499649032765
-        },
-        {
-            "x": 7.667206120655169,
-            "y": 2.862563871388905,
-            "heading": 0.061604548821908564,
-            "angularVelocity": -0.33811467337536444,
-            "velocityX": 0.09495738740298482,
-            "velocityY": -3.0017421049246735,
-            "timestamp": 1.6774550133221182
-        },
-        {
-            "x": 7.673608699461006,
-            "y": 2.659753859006467,
-            "heading": 0.031813701428875164,
-            "angularVelocity": -0.33062351017837394,
-            "velocityX": 0.07105682665061783,
-            "velocityY": -2.250817417462574,
-            "timestamp": 1.76756006174096
-        },
-        {
-            "x": 7.677870998153504,
-            "y": 2.524576540197495,
-            "heading": 0.009226184845951672,
-            "angularVelocity": -0.25067981183391225,
-            "velocityX": 0.04730366130750142,
-            "velocityY": -1.5002191462193974,
-            "timestamp": 1.8576651101598016
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -0.003,
-            "angularVelocity": -0.1356881224803119,
-            "velocityX": 0.023627997363707604,
-            "velocityY": -0.7499750722442785,
-            "timestamp": 1.9477701585786433
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -0.003,
-            "angularVelocity": 0,
-            "velocityX": -1.899719241029588e-32,
-            "velocityY": -4.254972095731821e-34,
-            "timestamp": 2.037875206997485
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C4.traj
deleted file mode 100644
index 3f479acb..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C4.traj	
+++ /dev/null
@@ -1,247 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 0,
-            "velocityX": -1.7123315632321391e-31,
-            "velocityY": 4.571975133912053e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 5.2081944432473115,
-            "y": 6.63354239454729,
-            "heading": 0.2524699257138384,
-            "angularVelocity": -0.12069194203891866,
-            "velocityX": 0.5160121574790498,
-            "velocityY": 0.0567774581143745,
-            "timestamp": 0.062390861883169
-        },
-        {
-            "x": 5.2726441787772975,
-            "y": 6.640106281427696,
-            "heading": 0.23785051964847434,
-            "angularVelocity": -0.2343196683632906,
-            "velocityX": 1.0329996025807835,
-            "velocityY": 0.1052059016703473,
-            "timestamp": 0.124781723766338
-        },
-        {
-            "x": 5.369418980987113,
-            "y": 6.648966513737627,
-            "heading": 0.21675536507543205,
-            "angularVelocity": -0.33811288923278293,
-            "velocityX": 1.551105390898958,
-            "velocityY": 0.14201169918963874,
-            "timestamp": 0.187172585649507
-        },
-        {
-            "x": 5.498590594297912,
-            "y": 6.659046571836944,
-            "heading": 0.19009632617593986,
-            "angularVelocity": -0.42729076173705777,
-            "velocityX": 2.070361097955043,
-            "velocityY": 0.16156305258605722,
-            "timestamp": 0.249563447532676
-        },
-        {
-            "x": 5.6601948909824875,
-            "y": 6.668590441412665,
-            "heading": 0.15936442465931713,
-            "angularVelocity": -0.49257055583183496,
-            "velocityX": 2.5901917653772815,
-            "velocityY": 0.15296902924013772,
-            "timestamp": 0.311954309415845
-        },
-        {
-            "x": 5.854024302844021,
-            "y": 6.674275452040902,
-            "heading": 0.12740102584180835,
-            "angularVelocity": -0.512308980077189,
-            "velocityX": 3.106695532183737,
-            "velocityY": 0.09111928344386595,
-            "timestamp": 0.374345171299014
-        },
-        {
-            "x": 6.078018113514707,
-            "y": 6.668089747759068,
-            "heading": 0.10126004011963459,
-            "angularVelocity": -0.4189874115076864,
-            "velocityX": 3.590170161299073,
-            "velocityY": -0.09914439543117176,
-            "timestamp": 0.436736033182183
-        },
-        {
-            "x": 6.311203816285634,
-            "y": 6.63161682590639,
-            "heading": 0.10125992071184643,
-            "angularVelocity": -0.000001913866624365403,
-            "velocityX": 3.737497699704501,
-            "velocityY": -0.584587562213517,
-            "timestamp": 0.499126895065352
-        },
-        {
-            "x": 6.53718081427167,
-            "y": 6.563497401833266,
-            "heading": 0.10125989664992657,
-            "angularVelocity": -3.856641685769623e-7,
-            "velocityX": 3.621956664249834,
-            "velocityY": -1.0918173273625358,
-            "timestamp": 0.561517756948521
-        },
-        {
-            "x": 6.7516755548198235,
-            "y": 6.465019697743108,
-            "heading": 0.10125985817262735,
-            "angularVelocity": -6.167136938553595e-7,
-            "velocityX": 3.4379191771674527,
-            "velocityY": -1.5783994821960954,
-            "timestamp": 0.62390861883169
-        },
-        {
-            "x": 6.950631618499756,
-            "y": 6.338046073913574,
-            "heading": 0.10125971935519859,
-            "angularVelocity": -0.000002224964167076665,
-            "velocityX": 3.1888654471946203,
-            "velocityY": -2.035131748416977,
-            "timestamp": 0.686299480714859
-        },
-        {
-            "x": 7.196058677643184,
-            "y": 6.101503366378845,
-            "heading": 0.10125966337035743,
-            "angularVelocity": -6.213285755242982e-7,
-            "velocityX": 2.723788105663008,
-            "velocityY": -2.625188174087521,
-            "timestamp": 0.7764045291337008
-        },
-        {
-            "x": 7.389899318639429,
-            "y": 5.821123760827136,
-            "heading": 0.10125964077104323,
-            "angularVelocity": -2.508107435019506e-7,
-            "velocityX": 2.1512739230237647,
-            "velocityY": -3.1116969633976366,
-            "timestamp": 0.8665095775525427
-        },
-        {
-            "x": 7.524507781478929,
-            "y": 5.507966472295818,
-            "heading": 0.101259626680438,
-            "angularVelocity": -1.5637975290365853e-7,
-            "velocityX": 1.4939058931947202,
-            "velocityY": -3.475468844716046,
-            "timestamp": 0.9566146259713845
-        },
-        {
-            "x": 7.594574766806078,
-            "y": 5.174383579102459,
-            "heading": 0.10125961555168732,
-            "angularVelocity": -1.235086255399969e-7,
-            "velocityX": 0.7776144240159958,
-            "velocityY": -3.702155418004344,
-            "timestamp": 1.0467196743902263
-        },
-        {
-            "x": 7.605242469312774,
-            "y": 4.833688510035603,
-            "heading": 0.10125960514477245,
-            "angularVelocity": -1.1549757817967306e-7,
-            "velocityX": 0.11839184034432584,
-            "velocityY": -3.781087464524514,
-            "timestamp": 1.136824722809068
-        },
-        {
-            "x": 7.615908675824772,
-            "y": 4.492993394135096,
-            "heading": 0.10125959473787158,
-            "angularVelocity": -1.1549742264017462e-7,
-            "velocityX": 0.11837523756082047,
-            "velocityY": -3.781087984291716,
-            "timestamp": 1.2269297712279097
-        },
-        {
-            "x": 7.6265748461946865,
-            "y": 4.152298277103123,
-            "heading": 0.10125958433097336,
-            "angularVelocity": -1.1549739343149193e-7,
-            "velocityX": 0.11837483645025074,
-            "velocityY": -3.781087996848918,
-            "timestamp": 1.3170348196467514
-        },
-        {
-            "x": 7.637240986091898,
-            "y": 3.8116031591171406,
-            "heading": 0.10125957392407539,
-            "angularVelocity": -1.1549739036497336e-7,
-            "velocityX": 0.11837449825941278,
-            "velocityY": -3.78108800743665,
-            "timestamp": 1.407139868065593
-        },
-        {
-            "x": 7.647907101492937,
-            "y": 3.470908040364265,
-            "heading": 0.10125956351717343,
-            "angularVelocity": -1.1549743491773867e-7,
-            "velocityX": 0.11837422639702805,
-            "velocityY": -3.781088015947766,
-            "timestamp": 1.4972449164844348
-        },
-        {
-            "x": 7.658649980665503,
-            "y": 3.1330359890940187,
-            "heading": 0.09207038783752043,
-            "angularVelocity": -0.10198291706072676,
-            "velocityX": 0.11922616280763662,
-            "velocityY": -3.7497571689844853,
-            "timestamp": 1.5873499649032765
-        },
-        {
-            "x": 7.667206120655169,
-            "y": 2.862563871388905,
-            "heading": 0.061604548821908564,
-            "angularVelocity": -0.33811467337536444,
-            "velocityX": 0.09495738740298482,
-            "velocityY": -3.0017421049246735,
-            "timestamp": 1.6774550133221182
-        },
-        {
-            "x": 7.673608699461006,
-            "y": 2.659753859006467,
-            "heading": 0.031813701428875164,
-            "angularVelocity": -0.33062351017837394,
-            "velocityX": 0.07105682665061783,
-            "velocityY": -2.250817417462574,
-            "timestamp": 1.76756006174096
-        },
-        {
-            "x": 7.677870998153504,
-            "y": 2.524576540197495,
-            "heading": 0.009226184845951672,
-            "angularVelocity": -0.25067981183391225,
-            "velocityX": 0.04730366130750142,
-            "velocityY": -1.5002191462193974,
-            "timestamp": 1.8576651101598016
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -0.003,
-            "angularVelocity": -0.1356881224803119,
-            "velocityX": 0.023627997363707604,
-            "velocityY": -0.7499750722442785,
-            "timestamp": 1.9477701585786433
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -0.003,
-            "angularVelocity": 0,
-            "velocityX": -1.899719241029588e-32,
-            "velocityY": -4.254972095731821e-34,
-            "timestamp": 2.037875206997485
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C5.1.traj
deleted file mode 100644
index 7f9f6ab6..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C5.1.traj	
+++ /dev/null
@@ -1,292 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": -8.010592837331029e-32,
-            "velocityX": 9.396271853657066e-34,
-            "velocityY": -2.4298772375808502e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 5.207063492876026,
-            "y": 6.6289176408913075,
-            "heading": 0.24374543730705361,
-            "angularVelocity": -0.264834788594711,
-            "velocityX": 0.5061159579768882,
-            "velocityY": -0.01763482359684202,
-            "timestamp": 0.06137623678217253
-        },
-        {
-            "x": 5.269220913885746,
-            "y": 6.626580685647671,
-            "heading": 0.2121773997323976,
-            "angularVelocity": -0.514336479877237,
-            "velocityX": 1.0127277961065038,
-            "velocityY": -0.03807589657100854,
-            "timestamp": 0.12275247356434506
-        },
-        {
-            "x": 5.36250981468421,
-            "y": 6.6227475288112485,
-            "heading": 0.16662318019476377,
-            "angularVelocity": -0.7422126530713462,
-            "velocityX": 1.5199514615005028,
-            "velocityY": -0.06245343535846346,
-            "timestamp": 0.1841287103465176
-        },
-        {
-            "x": 5.486975524936631,
-            "y": 6.617054798693029,
-            "heading": 0.10910232843404971,
-            "angularVelocity": -0.9371844019185364,
-            "velocityX": 2.0279136808950478,
-            "velocityY": -0.0927513711605005,
-            "timestamp": 0.24550494712869012
-        },
-        {
-            "x": 5.642662315095453,
-            "y": 6.608892934358269,
-            "heading": 0.04300979987381644,
-            "angularVelocity": -1.076842309423355,
-            "velocityX": 2.536597196588689,
-            "velocityY": -0.1329808532205177,
-            "timestamp": 0.30688118391086266
-        },
-        {
-            "x": 5.829535978248522,
-            "y": 6.597025931717244,
-            "heading": -0.024847542112422006,
-            "angularVelocity": -1.1055963275666998,
-            "velocityX": 3.0447233807490215,
-            "velocityY": -0.19334848897856358,
-            "timestamp": 0.3682574206930352
-        },
-        {
-            "x": 6.046434546533161,
-            "y": 6.5777851666622515,
-            "heading": -0.07267029178585922,
-            "angularVelocity": -0.7791737027338695,
-            "velocityX": 3.533917679808606,
-            "velocityY": -0.31348883645767445,
-            "timestamp": 0.4296336574752077
-        },
-        {
-            "x": 6.274079716361941,
-            "y": 6.532107529947078,
-            "heading": -0.07267036448034916,
-            "angularVelocity": -0.0000011844077407082938,
-            "velocityX": 3.709011528952235,
-            "velocityY": -0.7442234830605138,
-            "timestamp": 0.49100989425738023
-        },
-        {
-            "x": 6.493476634626264,
-            "y": 6.456122214314626,
-            "heading": -0.07267035803800323,
-            "angularVelocity": 1.049648246960038e-7,
-            "velocityX": 3.5746231728571716,
-            "velocityY": -1.238025001469686,
-            "timestamp": 0.5523861310395528
-        },
-        {
-            "x": 6.700610073343003,
-            "y": 6.351219816335689,
-            "heading": -0.07267035070446132,
-            "angularVelocity": 1.1948503655702063e-7,
-            "velocityX": 3.3748149051866063,
-            "velocityY": -1.7091695985083282,
-            "timestamp": 0.6137623678217253
-        },
-        {
-            "x": 6.891689244621814,
-            "y": 6.21932014936447,
-            "heading": -0.07267034149477326,
-            "angularVelocity": 1.5005299351249212e-7,
-            "velocityX": 3.1132435173071933,
-            "velocityY": -2.1490347712151214,
-            "timestamp": 0.6751386046038979
-        },
-        {
-            "x": 7.0632171630859375,
-            "y": 6.06283712387085,
-            "heading": -0.07267032569899272,
-            "angularVelocity": 2.573598736854518e-7,
-            "velocityX": 2.7946959190881038,
-            "velocityY": -2.549570219643589,
-            "timestamp": 0.7365148413860704
-        },
-        {
-            "x": 7.263317671759933,
-            "y": 5.7907628781558955,
-            "heading": -0.07267031513885969,
-            "angularVelocity": 1.182832965996254e-7,
-            "velocityX": 2.24131152203967,
-            "velocityY": -3.0474842158680824,
-            "timestamp": 0.8257931530827405
-        },
-        {
-            "x": 7.406264443711533,
-            "y": 5.484771313458312,
-            "heading": -0.07267030869611313,
-            "angularVelocity": 7.216474459034548e-8,
-            "velocityX": 1.6011365944875051,
-            "velocityY": -3.4273896860551396,
-            "timestamp": 0.9150714647794106
-        },
-        {
-            "x": 7.486522259331217,
-            "y": 5.1567114044359705,
-            "heading": -0.07267030368970051,
-            "angularVelocity": 5.607647052205946e-8,
-            "velocityX": 0.8989620669840581,
-            "velocityY": -3.6745756364317224,
-            "timestamp": 1.0043497764760807
-        },
-        {
-            "x": 7.50149841248159,
-            "y": 4.81930906784098,
-            "heading": -0.07267029905812865,
-            "angularVelocity": 5.187790600157932e-8,
-            "velocityX": 0.1677468230047605,
-            "velocityY": -3.7792195011633143,
-            "timestamp": 1.093628088172751
-        },
-        {
-            "x": 7.516449925771596,
-            "y": 4.481905638472984,
-            "heading": -0.0726702944266145,
-            "angularVelocity": 5.1877259457701335e-8,
-            "velocityX": 0.16747083368699095,
-            "velocityY": -3.7792317412357557,
-            "timestamp": 1.1829063998694211
-        },
-        {
-            "x": 7.531401402222247,
-            "y": 4.144502207472513,
-            "heading": -0.07267028979509418,
-            "angularVelocity": 5.187732873980297e-8,
-            "velocityX": 0.16747042105201052,
-            "velocityY": -3.779231759520992,
-            "timestamp": 1.2721847115660914
-        },
-        {
-            "x": 7.546352847973806,
-            "y": 3.807098775111667,
-            "heading": -0.07267028516356967,
-            "angularVelocity": 5.1877375603275234e-8,
-            "velocityX": 0.16747007719365256,
-            "velocityY": -3.7792317747584647,
-            "timestamp": 1.3614630232627616
-        },
-        {
-            "x": 7.561304268557873,
-            "y": 3.4696953416355707,
-            "heading": -0.07267028053204275,
-            "angularVelocity": 5.1877402813012475e-8,
-            "velocityX": 0.16746979529438435,
-            "velocityY": -3.7792317872503007,
-            "timestamp": 1.4507413349594318
-        },
-        {
-            "x": 7.576255668582546,
-            "y": 3.132291907248425,
-            "heading": -0.07267027590051456,
-            "angularVelocity": 5.1877416880821484e-8,
-            "velocityX": 0.16746956501007867,
-            "velocityY": -3.779231797454896,
-            "timestamp": 1.540019646656102
-        },
-        {
-            "x": 7.591207051730301,
-            "y": 2.794888472113414,
-            "heading": -0.07267027126898577,
-            "angularVelocity": 5.1877423431260937e-8,
-            "velocityX": 0.16746937597291248,
-            "velocityY": -3.779231805831691,
-            "timestamp": 1.6292979583527722
-        },
-        {
-            "x": 7.60615842090353,
-            "y": 2.457485036359151,
-            "heading": -0.07267026663745696,
-            "angularVelocity": 5.1877423851232326e-8,
-            "velocityX": 0.16746921944524512,
-            "velocityY": -3.779231812767884,
-            "timestamp": 1.7185762700494425
-        },
-        {
-            "x": 7.621109778388665,
-            "y": 2.1200816000869556,
-            "heading": -0.07267026200592838,
-            "angularVelocity": 5.187742117887202e-8,
-            "velocityX": 0.1674690885277524,
-            "velocityY": -3.779231818569208,
-            "timestamp": 1.8078545817461127
-        },
-        {
-            "x": 7.636061125621916,
-            "y": 1.7826781633604734,
-            "heading": -0.07267025737439563,
-            "angularVelocity": 5.1877468094637746e-8,
-            "velocityX": 0.16746897369713376,
-            "velocityY": -3.7792318236576405,
-            "timestamp": 1.897132893442783
-        },
-        {
-            "x": 7.6506850878742485,
-            "y": 1.4507667606163608,
-            "heading": -0.05536549896524321,
-            "angularVelocity": 0.19382936438075055,
-            "velocityX": 0.16380195788222543,
-            "velocityY": -3.7177159428351065,
-            "timestamp": 1.9864112051394531
-        },
-        {
-            "x": 7.662404359721407,
-            "y": 1.1852476637582392,
-            "heading": -0.0353858682020255,
-            "angularVelocity": 0.2237904187872895,
-            "velocityX": 0.13126672788097452,
-            "velocityY": -2.974060461181691,
-            "timestamp": 2.0756895168361233
-        },
-        {
-            "x": 7.6711999602966845,
-            "y": 0.9861189087552604,
-            "heading": -0.018423007440779238,
-            "angularVelocity": 0.18999979321832794,
-            "velocityX": 0.0985188945458782,
-            "velocityY": -2.2304269784976873,
-            "timestamp": 2.1649678285327933
-        },
-        {
-            "x": 7.677066105120016,
-            "y": 0.8533716446291538,
-            "heading": -0.006322956528169105,
-            "angularVelocity": 0.13553180702747777,
-            "velocityX": 0.06570626966225444,
-            "velocityY": -1.4868926349898441,
-            "timestamp": 2.2542461402294633
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -2.444948224573009e-31,
-            "angularVelocity": 0.0708229849781762,
-            "velocityX": 0.03286234724008497,
-            "velocityY": -0.7434240563896022,
-            "timestamp": 2.3435244519261333
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 1.883272219480887e-30,
-            "velocityX": 0,
-            "velocityY": -3.281240907044904e-32,
-            "timestamp": 2.4328027636228033
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C5.traj
deleted file mode 100644
index 7f9f6ab6..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C5.traj	
+++ /dev/null
@@ -1,292 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": -8.010592837331029e-32,
-            "velocityX": 9.396271853657066e-34,
-            "velocityY": -2.4298772375808502e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 5.207063492876026,
-            "y": 6.6289176408913075,
-            "heading": 0.24374543730705361,
-            "angularVelocity": -0.264834788594711,
-            "velocityX": 0.5061159579768882,
-            "velocityY": -0.01763482359684202,
-            "timestamp": 0.06137623678217253
-        },
-        {
-            "x": 5.269220913885746,
-            "y": 6.626580685647671,
-            "heading": 0.2121773997323976,
-            "angularVelocity": -0.514336479877237,
-            "velocityX": 1.0127277961065038,
-            "velocityY": -0.03807589657100854,
-            "timestamp": 0.12275247356434506
-        },
-        {
-            "x": 5.36250981468421,
-            "y": 6.6227475288112485,
-            "heading": 0.16662318019476377,
-            "angularVelocity": -0.7422126530713462,
-            "velocityX": 1.5199514615005028,
-            "velocityY": -0.06245343535846346,
-            "timestamp": 0.1841287103465176
-        },
-        {
-            "x": 5.486975524936631,
-            "y": 6.617054798693029,
-            "heading": 0.10910232843404971,
-            "angularVelocity": -0.9371844019185364,
-            "velocityX": 2.0279136808950478,
-            "velocityY": -0.0927513711605005,
-            "timestamp": 0.24550494712869012
-        },
-        {
-            "x": 5.642662315095453,
-            "y": 6.608892934358269,
-            "heading": 0.04300979987381644,
-            "angularVelocity": -1.076842309423355,
-            "velocityX": 2.536597196588689,
-            "velocityY": -0.1329808532205177,
-            "timestamp": 0.30688118391086266
-        },
-        {
-            "x": 5.829535978248522,
-            "y": 6.597025931717244,
-            "heading": -0.024847542112422006,
-            "angularVelocity": -1.1055963275666998,
-            "velocityX": 3.0447233807490215,
-            "velocityY": -0.19334848897856358,
-            "timestamp": 0.3682574206930352
-        },
-        {
-            "x": 6.046434546533161,
-            "y": 6.5777851666622515,
-            "heading": -0.07267029178585922,
-            "angularVelocity": -0.7791737027338695,
-            "velocityX": 3.533917679808606,
-            "velocityY": -0.31348883645767445,
-            "timestamp": 0.4296336574752077
-        },
-        {
-            "x": 6.274079716361941,
-            "y": 6.532107529947078,
-            "heading": -0.07267036448034916,
-            "angularVelocity": -0.0000011844077407082938,
-            "velocityX": 3.709011528952235,
-            "velocityY": -0.7442234830605138,
-            "timestamp": 0.49100989425738023
-        },
-        {
-            "x": 6.493476634626264,
-            "y": 6.456122214314626,
-            "heading": -0.07267035803800323,
-            "angularVelocity": 1.049648246960038e-7,
-            "velocityX": 3.5746231728571716,
-            "velocityY": -1.238025001469686,
-            "timestamp": 0.5523861310395528
-        },
-        {
-            "x": 6.700610073343003,
-            "y": 6.351219816335689,
-            "heading": -0.07267035070446132,
-            "angularVelocity": 1.1948503655702063e-7,
-            "velocityX": 3.3748149051866063,
-            "velocityY": -1.7091695985083282,
-            "timestamp": 0.6137623678217253
-        },
-        {
-            "x": 6.891689244621814,
-            "y": 6.21932014936447,
-            "heading": -0.07267034149477326,
-            "angularVelocity": 1.5005299351249212e-7,
-            "velocityX": 3.1132435173071933,
-            "velocityY": -2.1490347712151214,
-            "timestamp": 0.6751386046038979
-        },
-        {
-            "x": 7.0632171630859375,
-            "y": 6.06283712387085,
-            "heading": -0.07267032569899272,
-            "angularVelocity": 2.573598736854518e-7,
-            "velocityX": 2.7946959190881038,
-            "velocityY": -2.549570219643589,
-            "timestamp": 0.7365148413860704
-        },
-        {
-            "x": 7.263317671759933,
-            "y": 5.7907628781558955,
-            "heading": -0.07267031513885969,
-            "angularVelocity": 1.182832965996254e-7,
-            "velocityX": 2.24131152203967,
-            "velocityY": -3.0474842158680824,
-            "timestamp": 0.8257931530827405
-        },
-        {
-            "x": 7.406264443711533,
-            "y": 5.484771313458312,
-            "heading": -0.07267030869611313,
-            "angularVelocity": 7.216474459034548e-8,
-            "velocityX": 1.6011365944875051,
-            "velocityY": -3.4273896860551396,
-            "timestamp": 0.9150714647794106
-        },
-        {
-            "x": 7.486522259331217,
-            "y": 5.1567114044359705,
-            "heading": -0.07267030368970051,
-            "angularVelocity": 5.607647052205946e-8,
-            "velocityX": 0.8989620669840581,
-            "velocityY": -3.6745756364317224,
-            "timestamp": 1.0043497764760807
-        },
-        {
-            "x": 7.50149841248159,
-            "y": 4.81930906784098,
-            "heading": -0.07267029905812865,
-            "angularVelocity": 5.187790600157932e-8,
-            "velocityX": 0.1677468230047605,
-            "velocityY": -3.7792195011633143,
-            "timestamp": 1.093628088172751
-        },
-        {
-            "x": 7.516449925771596,
-            "y": 4.481905638472984,
-            "heading": -0.0726702944266145,
-            "angularVelocity": 5.1877259457701335e-8,
-            "velocityX": 0.16747083368699095,
-            "velocityY": -3.7792317412357557,
-            "timestamp": 1.1829063998694211
-        },
-        {
-            "x": 7.531401402222247,
-            "y": 4.144502207472513,
-            "heading": -0.07267028979509418,
-            "angularVelocity": 5.187732873980297e-8,
-            "velocityX": 0.16747042105201052,
-            "velocityY": -3.779231759520992,
-            "timestamp": 1.2721847115660914
-        },
-        {
-            "x": 7.546352847973806,
-            "y": 3.807098775111667,
-            "heading": -0.07267028516356967,
-            "angularVelocity": 5.1877375603275234e-8,
-            "velocityX": 0.16747007719365256,
-            "velocityY": -3.7792317747584647,
-            "timestamp": 1.3614630232627616
-        },
-        {
-            "x": 7.561304268557873,
-            "y": 3.4696953416355707,
-            "heading": -0.07267028053204275,
-            "angularVelocity": 5.1877402813012475e-8,
-            "velocityX": 0.16746979529438435,
-            "velocityY": -3.7792317872503007,
-            "timestamp": 1.4507413349594318
-        },
-        {
-            "x": 7.576255668582546,
-            "y": 3.132291907248425,
-            "heading": -0.07267027590051456,
-            "angularVelocity": 5.1877416880821484e-8,
-            "velocityX": 0.16746956501007867,
-            "velocityY": -3.779231797454896,
-            "timestamp": 1.540019646656102
-        },
-        {
-            "x": 7.591207051730301,
-            "y": 2.794888472113414,
-            "heading": -0.07267027126898577,
-            "angularVelocity": 5.1877423431260937e-8,
-            "velocityX": 0.16746937597291248,
-            "velocityY": -3.779231805831691,
-            "timestamp": 1.6292979583527722
-        },
-        {
-            "x": 7.60615842090353,
-            "y": 2.457485036359151,
-            "heading": -0.07267026663745696,
-            "angularVelocity": 5.1877423851232326e-8,
-            "velocityX": 0.16746921944524512,
-            "velocityY": -3.779231812767884,
-            "timestamp": 1.7185762700494425
-        },
-        {
-            "x": 7.621109778388665,
-            "y": 2.1200816000869556,
-            "heading": -0.07267026200592838,
-            "angularVelocity": 5.187742117887202e-8,
-            "velocityX": 0.1674690885277524,
-            "velocityY": -3.779231818569208,
-            "timestamp": 1.8078545817461127
-        },
-        {
-            "x": 7.636061125621916,
-            "y": 1.7826781633604734,
-            "heading": -0.07267025737439563,
-            "angularVelocity": 5.1877468094637746e-8,
-            "velocityX": 0.16746897369713376,
-            "velocityY": -3.7792318236576405,
-            "timestamp": 1.897132893442783
-        },
-        {
-            "x": 7.6506850878742485,
-            "y": 1.4507667606163608,
-            "heading": -0.05536549896524321,
-            "angularVelocity": 0.19382936438075055,
-            "velocityX": 0.16380195788222543,
-            "velocityY": -3.7177159428351065,
-            "timestamp": 1.9864112051394531
-        },
-        {
-            "x": 7.662404359721407,
-            "y": 1.1852476637582392,
-            "heading": -0.0353858682020255,
-            "angularVelocity": 0.2237904187872895,
-            "velocityX": 0.13126672788097452,
-            "velocityY": -2.974060461181691,
-            "timestamp": 2.0756895168361233
-        },
-        {
-            "x": 7.6711999602966845,
-            "y": 0.9861189087552604,
-            "heading": -0.018423007440779238,
-            "angularVelocity": 0.18999979321832794,
-            "velocityX": 0.0985188945458782,
-            "velocityY": -2.2304269784976873,
-            "timestamp": 2.1649678285327933
-        },
-        {
-            "x": 7.677066105120016,
-            "y": 0.8533716446291538,
-            "heading": -0.006322956528169105,
-            "angularVelocity": 0.13553180702747777,
-            "velocityX": 0.06570626966225444,
-            "velocityY": -1.4868926349898441,
-            "timestamp": 2.2542461402294633
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -2.444948224573009e-31,
-            "angularVelocity": 0.0708229849781762,
-            "velocityX": 0.03286234724008497,
-            "velocityY": -0.7434240563896022,
-            "timestamp": 2.3435244519261333
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 1.883272219480887e-30,
-            "velocityX": 0,
-            "velocityY": -3.281240907044904e-32,
-            "timestamp": 2.4328027636228033
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj
deleted file mode 100644
index 462787d1..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": -5.95142511939464e-31,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.1029790507443495,
-            "y": 6.633224483607701,
-            "heading": 0.26650125878935643,
-            "angularVelocity": 0.06942713064122713,
-            "velocityX": -0.7797928290155524,
-            "velocityY": 0.034434353705267144,
-            "timestamp": 0.09364147314336706
-        },
-        {
-            "x": 4.9569371541735245,
-            "y": 6.639673450192742,
-            "heading": 0.2795035687162854,
-            "angularVelocity": 0.13885204376302582,
-            "velocityX": -1.559585637308702,
-            "velocityY": 0.06886870067889261,
-            "timestamp": 0.18728294628673411
-        },
-        {
-            "x": 4.737874314376445,
-            "y": 6.649346898931416,
-            "heading": 0.2990065710588269,
-            "angularVelocity": 0.20827312608258508,
-            "velocityX": -2.339378401936154,
-            "velocityY": 0.1033030388561254,
-            "timestamp": 0.28092441943010116
-        },
-        {
-            "x": 4.445790546320906,
-            "y": 6.66224482863892,
-            "heading": 0.3250097249050781,
-            "angularVelocity": 0.2776884319880837,
-            "velocityX": -3.119171006722006,
-            "velocityY": 0.13773736438080994,
-            "timestamp": 0.37456589257346823
-        },
-        {
-            "x": 4.09189529815297,
-            "y": 6.677872415392626,
-            "heading": 0.32500972789735083,
-            "angularVelocity": 3.195452790688951e-8,
-            "velocityX": -3.7792575905562438,
-            "velocityY": 0.16688745092443205,
-            "timestamp": 0.4682073657168353
-        },
-        {
-            "x": 3.7380000499890724,
-            "y": 6.693500002240241,
-            "heading": 0.32500973088947493,
-            "angularVelocity": 3.1952970747065184e-8,
-            "velocityX": -3.7792575905130557,
-            "velocityY": 0.16688745192729154,
-            "timestamp": 0.5618488388602023
-        },
-        {
-            "x": 3.384104801825175,
-            "y": 6.7091275890878554,
-            "heading": 0.3250097338815991,
-            "angularVelocity": 3.195297127919844e-8,
-            "velocityX": -3.7792575905130557,
-            "velocityY": 0.1668874519272919,
-            "timestamp": 0.6554903120035693
-        },
-        {
-            "x": 3.030209553657211,
-            "y": 6.724755175840924,
-            "heading": 0.32500973687387263,
-            "angularVelocity": 3.195453683514771e-8,
-            "velocityX": -3.7792575905565435,
-            "velocityY": 0.16688745091763021,
-            "timestamp": 0.7491317851469363
-        },
-        {
-            "x": 2.7381257502813003,
-            "y": 6.7376531044804615,
-            "heading": 0.35100602630019134,
-            "angularVelocity": 0.2776151266490911,
-            "velocityX": -3.119171383909283,
-            "velocityY": 0.13773735297596457,
-            "timestamp": 0.8427732582903034
-        },
-        {
-            "x": 2.5190628797280783,
-            "y": 6.747326551971013,
-            "heading": 0.37050304728737704,
-            "angularVelocity": 0.20820925101574836,
-            "velocityX": -2.3393787303818834,
-            "velocityY": 0.10330302552738789,
-            "timestamp": 0.9364147314336704
-        },
-        {
-            "x": 2.3730209608288053,
-            "y": 6.7537755172838185,
-            "heading": 0.3835010053218344,
-            "angularVelocity": 0.13880556977745576,
-            "velocityX": -1.559585875754842,
-            "velocityY": 0.06886868709264785,
-            "timestamp": 1.0300562045770374
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0.06940295213233785,
-            "velocityX": -0.7797929526056125,
-            "velocityY": 0.03443434418469861,
-            "timestamp": 1.1236976777204044
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": -1.6940949553652625e-30,
-            "velocityX": -1.6586267397188983e-32,
-            "velocityY": 0,
-            "timestamp": 1.2173391508637714
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.traj
deleted file mode 100644
index 462787d1..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": -5.95142511939464e-31,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.1029790507443495,
-            "y": 6.633224483607701,
-            "heading": 0.26650125878935643,
-            "angularVelocity": 0.06942713064122713,
-            "velocityX": -0.7797928290155524,
-            "velocityY": 0.034434353705267144,
-            "timestamp": 0.09364147314336706
-        },
-        {
-            "x": 4.9569371541735245,
-            "y": 6.639673450192742,
-            "heading": 0.2795035687162854,
-            "angularVelocity": 0.13885204376302582,
-            "velocityX": -1.559585637308702,
-            "velocityY": 0.06886870067889261,
-            "timestamp": 0.18728294628673411
-        },
-        {
-            "x": 4.737874314376445,
-            "y": 6.649346898931416,
-            "heading": 0.2990065710588269,
-            "angularVelocity": 0.20827312608258508,
-            "velocityX": -2.339378401936154,
-            "velocityY": 0.1033030388561254,
-            "timestamp": 0.28092441943010116
-        },
-        {
-            "x": 4.445790546320906,
-            "y": 6.66224482863892,
-            "heading": 0.3250097249050781,
-            "angularVelocity": 0.2776884319880837,
-            "velocityX": -3.119171006722006,
-            "velocityY": 0.13773736438080994,
-            "timestamp": 0.37456589257346823
-        },
-        {
-            "x": 4.09189529815297,
-            "y": 6.677872415392626,
-            "heading": 0.32500972789735083,
-            "angularVelocity": 3.195452790688951e-8,
-            "velocityX": -3.7792575905562438,
-            "velocityY": 0.16688745092443205,
-            "timestamp": 0.4682073657168353
-        },
-        {
-            "x": 3.7380000499890724,
-            "y": 6.693500002240241,
-            "heading": 0.32500973088947493,
-            "angularVelocity": 3.1952970747065184e-8,
-            "velocityX": -3.7792575905130557,
-            "velocityY": 0.16688745192729154,
-            "timestamp": 0.5618488388602023
-        },
-        {
-            "x": 3.384104801825175,
-            "y": 6.7091275890878554,
-            "heading": 0.3250097338815991,
-            "angularVelocity": 3.195297127919844e-8,
-            "velocityX": -3.7792575905130557,
-            "velocityY": 0.1668874519272919,
-            "timestamp": 0.6554903120035693
-        },
-        {
-            "x": 3.030209553657211,
-            "y": 6.724755175840924,
-            "heading": 0.32500973687387263,
-            "angularVelocity": 3.195453683514771e-8,
-            "velocityX": -3.7792575905565435,
-            "velocityY": 0.16688745091763021,
-            "timestamp": 0.7491317851469363
-        },
-        {
-            "x": 2.7381257502813003,
-            "y": 6.7376531044804615,
-            "heading": 0.35100602630019134,
-            "angularVelocity": 0.2776151266490911,
-            "velocityX": -3.119171383909283,
-            "velocityY": 0.13773735297596457,
-            "timestamp": 0.8427732582903034
-        },
-        {
-            "x": 2.5190628797280783,
-            "y": 6.747326551971013,
-            "heading": 0.37050304728737704,
-            "angularVelocity": 0.20820925101574836,
-            "velocityX": -2.3393787303818834,
-            "velocityY": 0.10330302552738789,
-            "timestamp": 0.9364147314336704
-        },
-        {
-            "x": 2.3730209608288053,
-            "y": 6.7537755172838185,
-            "heading": 0.3835010053218344,
-            "angularVelocity": 0.13880556977745576,
-            "velocityX": -1.559585875754842,
-            "velocityY": 0.06886868709264785,
-            "timestamp": 1.0300562045770374
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0.06940295213233785,
-            "velocityX": -0.7797929526056125,
-            "velocityY": 0.03443434418469861,
-            "timestamp": 1.1236976777204044
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": -1.6940949553652625e-30,
-            "velocityX": -1.6586267397188983e-32,
-            "velocityY": 0,
-            "timestamp": 1.2173391508637714
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj
deleted file mode 100644
index 53d96783..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 3.363577949177338e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 5.118924970919473,
-            "y": 6.609128397315466,
-            "heading": 0.25122660793754975,
-            "angularVelocity": -0.10272459100864365,
-            "velocityX": -0.6682716305586017,
-            "velocityY": -0.24437832416494054,
-            "timestamp": 0.08540693106008139
-        },
-        {
-            "x": 5.0047749070529814,
-            "y": 6.567385203608531,
-            "heading": 0.23368024533248913,
-            "angularVelocity": -0.20544424658833893,
-            "velocityX": -1.3365433279201937,
-            "velocityY": -0.48875651178205104,
-            "timestamp": 0.17081386212016278
-        },
-        {
-            "x": 4.833549798383878,
-            "y": 6.504770434941582,
-            "heading": 0.20736177398264571,
-            "angularVelocity": -0.30815381167752187,
-            "velocityX": -2.004815142563213,
-            "velocityY": -0.7331345113302418,
-            "timestamp": 0.25622079318024416
-        },
-        {
-            "x": 4.605249631158782,
-            "y": 6.421284110658292,
-            "heading": 0.17227269285622732,
-            "angularVelocity": -0.4108458258702004,
-            "velocityX": -2.6730871182397737,
-            "velocityY": -0.977512284390117,
-            "timestamp": 0.34162772424032556
-        },
-        {
-            "x": 4.319874394275373,
-            "y": 6.316926253811125,
-            "heading": 0.12841549400557886,
-            "angularVelocity": -0.5135086614902031,
-            "velocityX": -3.3413592239095435,
-            "velocityY": -1.2218897875367427,
-            "timestamp": 0.42703465530040696
-        },
-        {
-            "x": 4.016437653324315,
-            "y": 6.205962904892466,
-            "heading": 0.1284154887581796,
-            "angularVelocity": -6.143996998796484e-8,
-            "velocityX": -3.552835082407984,
-            "velocityY": -1.2992311928477889,
-            "timestamp": 0.5124415863604883
-        },
-        {
-            "x": 3.713000912388581,
-            "y": 6.094999555931864,
-            "heading": 0.12841548351080107,
-            "angularVelocity": -6.143972697353832e-8,
-            "velocityX": -3.5528350822285746,
-            "velocityY": -1.299231193338901,
-            "timestamp": 0.5978485174205697
-        },
-        {
-            "x": 3.4095641714528466,
-            "y": 5.98403620697126,
-            "heading": 0.12841547826342262,
-            "angularVelocity": -6.143972595662007e-8,
-            "velocityX": -3.5528350822285746,
-            "velocityY": -1.299231193338901,
-            "timestamp": 0.683255448480651
-        },
-        {
-            "x": 3.106127430499178,
-            "y": 5.873072858059743,
-            "heading": 0.12841547301602332,
-            "angularVelocity": -6.143997022352915e-8,
-            "velocityX": -3.55283508243856,
-            "velocityY": -1.2992311927641775,
-            "timestamp": 0.7686623795407324
-        },
-        {
-            "x": 2.820751661400481,
-            "y": 5.768715245814204,
-            "heading": 0.08460804498823243,
-            "angularVelocity": -0.5129259122655233,
-            "velocityX": -3.3413654554329186,
-            "velocityY": -1.2218869235814813,
-            "timestamp": 0.8540693106008137
-        },
-        {
-            "x": 2.5924510107246115,
-            "y": 5.685229149936981,
-            "heading": 0.04956396734437731,
-            "angularVelocity": -0.41031889577208425,
-            "velocityX": -2.6730927787964465,
-            "velocityY": -0.977509610063062,
-            "timestamp": 0.939476241660895
-        },
-        {
-            "x": 2.4212255084791887,
-            "y": 5.622614575599846,
-            "heading": 0.02328185696555881,
-            "angularVelocity": -0.30772807373595673,
-            "velocityX": -2.00481975081121,
-            "velocityY": -0.7331322359901695,
-            "timestamp": 1.0248831727209764
-        },
-        {
-            "x": 2.307075169405903,
-            "y": 5.580871525201533,
-            "heading": 0.005760683198102726,
-            "angularVelocity": -0.20514931926462068,
-            "velocityX": -1.3365465502206675,
-            "velocityY": -0.48875483383130996,
-            "timestamp": 1.1102901037810577
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": -0.10257578734376759,
-            "velocityX": -0.6682732735795421,
-            "velocityY": -0.24437741694350015,
-            "timestamp": 1.195697034841139
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": 2.4865352537032555e-34,
-            "velocityX": 0,
-            "velocityY": -6.568641934425214e-34,
-            "timestamp": 1.2811039659012204
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.traj
deleted file mode 100644
index 53d96783..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 3.363577949177338e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 5.118924970919473,
-            "y": 6.609128397315466,
-            "heading": 0.25122660793754975,
-            "angularVelocity": -0.10272459100864365,
-            "velocityX": -0.6682716305586017,
-            "velocityY": -0.24437832416494054,
-            "timestamp": 0.08540693106008139
-        },
-        {
-            "x": 5.0047749070529814,
-            "y": 6.567385203608531,
-            "heading": 0.23368024533248913,
-            "angularVelocity": -0.20544424658833893,
-            "velocityX": -1.3365433279201937,
-            "velocityY": -0.48875651178205104,
-            "timestamp": 0.17081386212016278
-        },
-        {
-            "x": 4.833549798383878,
-            "y": 6.504770434941582,
-            "heading": 0.20736177398264571,
-            "angularVelocity": -0.30815381167752187,
-            "velocityX": -2.004815142563213,
-            "velocityY": -0.7331345113302418,
-            "timestamp": 0.25622079318024416
-        },
-        {
-            "x": 4.605249631158782,
-            "y": 6.421284110658292,
-            "heading": 0.17227269285622732,
-            "angularVelocity": -0.4108458258702004,
-            "velocityX": -2.6730871182397737,
-            "velocityY": -0.977512284390117,
-            "timestamp": 0.34162772424032556
-        },
-        {
-            "x": 4.319874394275373,
-            "y": 6.316926253811125,
-            "heading": 0.12841549400557886,
-            "angularVelocity": -0.5135086614902031,
-            "velocityX": -3.3413592239095435,
-            "velocityY": -1.2218897875367427,
-            "timestamp": 0.42703465530040696
-        },
-        {
-            "x": 4.016437653324315,
-            "y": 6.205962904892466,
-            "heading": 0.1284154887581796,
-            "angularVelocity": -6.143996998796484e-8,
-            "velocityX": -3.552835082407984,
-            "velocityY": -1.2992311928477889,
-            "timestamp": 0.5124415863604883
-        },
-        {
-            "x": 3.713000912388581,
-            "y": 6.094999555931864,
-            "heading": 0.12841548351080107,
-            "angularVelocity": -6.143972697353832e-8,
-            "velocityX": -3.5528350822285746,
-            "velocityY": -1.299231193338901,
-            "timestamp": 0.5978485174205697
-        },
-        {
-            "x": 3.4095641714528466,
-            "y": 5.98403620697126,
-            "heading": 0.12841547826342262,
-            "angularVelocity": -6.143972595662007e-8,
-            "velocityX": -3.5528350822285746,
-            "velocityY": -1.299231193338901,
-            "timestamp": 0.683255448480651
-        },
-        {
-            "x": 3.106127430499178,
-            "y": 5.873072858059743,
-            "heading": 0.12841547301602332,
-            "angularVelocity": -6.143997022352915e-8,
-            "velocityX": -3.55283508243856,
-            "velocityY": -1.2992311927641775,
-            "timestamp": 0.7686623795407324
-        },
-        {
-            "x": 2.820751661400481,
-            "y": 5.768715245814204,
-            "heading": 0.08460804498823243,
-            "angularVelocity": -0.5129259122655233,
-            "velocityX": -3.3413654554329186,
-            "velocityY": -1.2218869235814813,
-            "timestamp": 0.8540693106008137
-        },
-        {
-            "x": 2.5924510107246115,
-            "y": 5.685229149936981,
-            "heading": 0.04956396734437731,
-            "angularVelocity": -0.41031889577208425,
-            "velocityX": -2.6730927787964465,
-            "velocityY": -0.977509610063062,
-            "timestamp": 0.939476241660895
-        },
-        {
-            "x": 2.4212255084791887,
-            "y": 5.622614575599846,
-            "heading": 0.02328185696555881,
-            "angularVelocity": -0.30772807373595673,
-            "velocityX": -2.00481975081121,
-            "velocityY": -0.7331322359901695,
-            "timestamp": 1.0248831727209764
-        },
-        {
-            "x": 2.307075169405903,
-            "y": 5.580871525201533,
-            "heading": 0.005760683198102726,
-            "angularVelocity": -0.20514931926462068,
-            "velocityX": -1.3365465502206675,
-            "velocityY": -0.48875483383130996,
-            "timestamp": 1.1102901037810577
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": -0.10257578734376759,
-            "velocityX": -0.6682732735795421,
-            "velocityY": -0.24437741694350015,
-            "timestamp": 1.195697034841139
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": 2.4865352537032555e-34,
-            "velocityX": 0,
-            "velocityY": -6.568641934425214e-34,
-            "timestamp": 1.2811039659012204
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.1.traj
deleted file mode 100644
index 0a2ace26..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.1.traj	
+++ /dev/null
@@ -1,283 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 1.3458298549409377e-33,
-            "velocityX": 0,
-            "velocityY": 3.5102521636207017e-36,
-            "timestamp": 0
-        },
-        {
-            "x": 5.139424093284861,
-            "y": 6.625864418761083,
-            "heading": 0.2540205855596587,
-            "angularVelocity": -0.08994806739995133,
-            "velocityX": -0.5502097496757357,
-            "velocityY": -0.06221136596695031,
-            "timestamp": 0.06647629696982885
-        },
-        {
-            "x": 5.066272282481334,
-            "y": 6.617593263412388,
-            "heading": 0.24206149463416465,
-            "angularVelocity": -0.17990007672842862,
-            "velocityX": -1.1004194598373498,
-            "velocityY": -0.12442262469054285,
-            "timestamp": 0.1329525939396577
-        },
-        {
-            "x": 4.956544571814103,
-            "y": 6.605186545652003,
-            "heading": 0.22412231737841096,
-            "angularVelocity": -0.26985825133875346,
-            "velocityX": -1.6506291064472642,
-            "velocityY": -0.1866337074403447,
-            "timestamp": 0.19942889090948654
-        },
-        {
-            "x": 4.810240966932036,
-            "y": 6.588644282109891,
-            "heading": 0.2002025750538153,
-            "angularVelocity": -0.35982362759243297,
-            "velocityX": -2.2008386680814915,
-            "velocityY": -0.24884454002634754,
-            "timestamp": 0.2659051878793154
-        },
-        {
-            "x": 4.627361475577568,
-            "y": 6.567966494763936,
-            "heading": 0.17030184062671833,
-            "angularVelocity": -0.4497954276945935,
-            "velocityX": -2.7510481132465947,
-            "velocityY": -0.31105504200002715,
-            "timestamp": 0.3323814848491442
-        },
-        {
-            "x": 4.407906114797318,
-            "y": 6.543153211904946,
-            "heading": 0.13441995513073204,
-            "angularVelocity": -0.5397696191211097,
-            "velocityX": -3.3012573019801734,
-            "velocityY": -0.37326511839627413,
-            "timestamp": 0.398857781818973
-        },
-        {
-            "x": 4.158022417123658,
-            "y": 6.514899899511502,
-            "heading": 0.134419949269586,
-            "angularVelocity": -8.816896134746106e-8,
-            "velocityX": -3.758989430278769,
-            "velocityY": -0.4250133307856765,
-            "timestamp": 0.46533407878880184
-        },
-        {
-            "x": 3.908138721969202,
-            "y": 6.486646564834816,
-            "heading": 0.13441994340875443,
-            "angularVelocity": -8.816423004519242e-8,
-            "velocityX": -3.758989392382515,
-            "velocityY": -0.42501366599148993,
-            "timestamp": 0.5318103757586307
-        },
-        {
-            "x": 3.6582550295868868,
-            "y": 6.45839320564021,
-            "heading": 0.1344199375479281,
-            "angularVelocity": -8.816415176708587e-8,
-            "velocityX": -3.7589893506813095,
-            "velocityY": -0.42501403481347844,
-            "timestamp": 0.5982866727284595
-        },
-        {
-            "x": 3.4083713401101514,
-            "y": 6.430139820747532,
-            "heading": 0.13441993168710784,
-            "angularVelocity": -8.816406061478911e-8,
-            "velocityX": -3.758989306972801,
-            "velocityY": -0.42501442138844536,
-            "timestamp": 0.6647629696982883
-        },
-        {
-            "x": 3.1584877665624913,
-            "y": 6.401885410552911,
-            "heading": 0.1344199258262894,
-            "angularVelocity": -8.816403354486871e-8,
-            "velocityX": -3.7589875630568486,
-            "velocityY": -0.4250298449602976,
-            "timestamp": 0.7312392666681171
-        },
-        {
-            "x": 2.9135491847991943,
-            "y": 6.344918727874756,
-            "heading": 0.13441991981204132,
-            "angularVelocity": -9.047206787345137e-8,
-            "velocityX": -3.684600269994962,
-            "velocityY": -0.8569472921154051,
-            "timestamp": 0.7977155636379459
-        },
-        {
-            "x": 2.741853960900858,
-            "y": 6.285250221506208,
-            "heading": 0.13441991359516897,
-            "angularVelocity": -1.293850671116252e-7,
-            "velocityX": -3.573307735472737,
-            "velocityY": -1.2418163448570063,
-            "timestamp": 0.8457649443822611
-        },
-        {
-            "x": 2.5774323687799447,
-            "y": 6.207758184927692,
-            "heading": 0.1344190973329971,
-            "angularVelocity": -0.000016987985264431854,
-            "velocityX": -3.4219294728447913,
-            "velocityY": -1.6127582786315857,
-            "timestamp": 0.8938143251265763
-        },
-        {
-            "x": 2.4266376481277105,
-            "y": 6.11739996688274,
-            "heading": 0.11881021575379135,
-            "angularVelocity": -0.32485083756365457,
-            "velocityX": -3.1383280765814066,
-            "velocityY": -1.8805282533353447,
-            "timestamp": 0.9418637058708915
-        },
-        {
-            "x": 2.292578699649266,
-            "y": 6.017904789932588,
-            "heading": 0.09375834131328029,
-            "angularVelocity": -0.5213776754755571,
-            "velocityX": -2.7900244790211017,
-            "velocityY": -2.0706859361954097,
-            "timestamp": 0.9899130866152067
-        },
-        {
-            "x": 2.176263979579871,
-            "y": 5.911118232592638,
-            "heading": 0.0629895244075883,
-            "angularVelocity": -0.6403582403990148,
-            "velocityX": -2.420732968200766,
-            "velocityY": -2.2224335815729503,
-            "timestamp": 1.037962467359522
-        },
-        {
-            "x": 2.0781210346390075,
-            "y": 5.798061969403822,
-            "heading": 0.02853978404947447,
-            "angularVelocity": -0.7169653349214,
-            "velocityX": -2.0425433880846535,
-            "velocityY": -2.3529182153339443,
-            "timestamp": 1.0860118481038372
-        },
-        {
-            "x": 1.9983746548109367,
-            "y": 5.679376225369417,
-            "heading": -0.008333608258354223,
-            "angularVelocity": -0.7674061920598474,
-            "velocityX": -1.659675496182245,
-            "velocityY": -2.470078535787315,
-            "timestamp": 1.1340612288481524
-        },
-        {
-            "x": 1.9371600151062012,
-            "y": 5.555497646331787,
-            "heading": -0.04678243253848999,
-            "angularVelocity": -0.8001939605576434,
-            "velocityX": -1.273994352403345,
-            "velocityY": -2.5781514167024167,
-            "timestamp": 1.1821106095924676
-        },
-        {
-            "x": 1.8879136197731217,
-            "y": 5.371859224657917,
-            "heading": -0.10269858839018778,
-            "angularVelocity": -0.8284337962966012,
-            "velocityX": -0.7296170063605633,
-            "velocityY": -2.720721274486196,
-            "timestamp": 1.2496068369333404
-        },
-        {
-            "x": 1.8760341540552343,
-            "y": 5.181395172179021,
-            "heading": -0.15967327513417093,
-            "angularVelocity": -0.8441166119736836,
-            "velocityX": -0.17600192167619955,
-            "velocityY": -2.821847382921196,
-            "timestamp": 1.3171030642742134
-        },
-        {
-            "x": 1.9020919030528052,
-            "y": 4.989002664772072,
-            "heading": -0.21623702759974286,
-            "angularVelocity": -0.8380283564577732,
-            "velocityX": 0.3860623033932351,
-            "velocityY": -2.8504186824445474,
-            "timestamp": 1.3845992916150864
-        },
-        {
-            "x": 1.9652989688718845,
-            "y": 4.8044863939922715,
-            "heading": -0.26949747833980514,
-            "angularVelocity": -0.7890878177691865,
-            "velocityX": 0.9364533146403533,
-            "velocityY": -2.7337271733418085,
-            "timestamp": 1.4520955189559595
-        },
-        {
-            "x": 2.0553904334698565,
-            "y": 4.6466969161112965,
-            "heading": -0.31411027699435934,
-            "angularVelocity": -0.6609672927236123,
-            "velocityX": 1.3347629659801235,
-            "velocityY": -2.337752554436838,
-            "timestamp": 1.5195917462968325
-        },
-        {
-            "x": 2.145781200762326,
-            "y": 4.526760098072049,
-            "heading": -0.34739291091529667,
-            "angularVelocity": -0.49310361826375293,
-            "velocityX": 1.3391973278146792,
-            "velocityY": -1.7769410641803345,
-            "timestamp": 1.5870879736377055
-        },
-        {
-            "x": 2.220455144634577,
-            "y": 4.441296194130274,
-            "heading": -0.37087004938287466,
-            "angularVelocity": -0.3478288993696278,
-            "velocityX": 1.1063424848196184,
-            "velocityY": -1.2662026798943848,
-            "timestamp": 1.6545842009785785
-        },
-        {
-            "x": 2.2728997785605394,
-            "y": 4.3865419949716715,
-            "heading": -0.3858025738529217,
-            "angularVelocity": -0.22123494983851594,
-            "velocityX": 0.7770009671963372,
-            "velocityY": -0.8112186608901896,
-            "timestamp": 1.7220804283194515
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.393,
-            "angularVelocity": -0.1066344954471229,
-            "velocityX": 0.4015072028040527,
-            "velocityY": -0.3932367188113757,
-            "timestamp": 1.7895766556603245
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.393,
-            "angularVelocity": 8.903141052781706e-32,
-            "velocityX": 0,
-            "velocityY": -2.026142404451774e-33,
-            "timestamp": 1.8570728830011975
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.traj
deleted file mode 100644
index 0a2ace26..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.traj	
+++ /dev/null
@@ -1,283 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 1.3458298549409377e-33,
-            "velocityX": 0,
-            "velocityY": 3.5102521636207017e-36,
-            "timestamp": 0
-        },
-        {
-            "x": 5.139424093284861,
-            "y": 6.625864418761083,
-            "heading": 0.2540205855596587,
-            "angularVelocity": -0.08994806739995133,
-            "velocityX": -0.5502097496757357,
-            "velocityY": -0.06221136596695031,
-            "timestamp": 0.06647629696982885
-        },
-        {
-            "x": 5.066272282481334,
-            "y": 6.617593263412388,
-            "heading": 0.24206149463416465,
-            "angularVelocity": -0.17990007672842862,
-            "velocityX": -1.1004194598373498,
-            "velocityY": -0.12442262469054285,
-            "timestamp": 0.1329525939396577
-        },
-        {
-            "x": 4.956544571814103,
-            "y": 6.605186545652003,
-            "heading": 0.22412231737841096,
-            "angularVelocity": -0.26985825133875346,
-            "velocityX": -1.6506291064472642,
-            "velocityY": -0.1866337074403447,
-            "timestamp": 0.19942889090948654
-        },
-        {
-            "x": 4.810240966932036,
-            "y": 6.588644282109891,
-            "heading": 0.2002025750538153,
-            "angularVelocity": -0.35982362759243297,
-            "velocityX": -2.2008386680814915,
-            "velocityY": -0.24884454002634754,
-            "timestamp": 0.2659051878793154
-        },
-        {
-            "x": 4.627361475577568,
-            "y": 6.567966494763936,
-            "heading": 0.17030184062671833,
-            "angularVelocity": -0.4497954276945935,
-            "velocityX": -2.7510481132465947,
-            "velocityY": -0.31105504200002715,
-            "timestamp": 0.3323814848491442
-        },
-        {
-            "x": 4.407906114797318,
-            "y": 6.543153211904946,
-            "heading": 0.13441995513073204,
-            "angularVelocity": -0.5397696191211097,
-            "velocityX": -3.3012573019801734,
-            "velocityY": -0.37326511839627413,
-            "timestamp": 0.398857781818973
-        },
-        {
-            "x": 4.158022417123658,
-            "y": 6.514899899511502,
-            "heading": 0.134419949269586,
-            "angularVelocity": -8.816896134746106e-8,
-            "velocityX": -3.758989430278769,
-            "velocityY": -0.4250133307856765,
-            "timestamp": 0.46533407878880184
-        },
-        {
-            "x": 3.908138721969202,
-            "y": 6.486646564834816,
-            "heading": 0.13441994340875443,
-            "angularVelocity": -8.816423004519242e-8,
-            "velocityX": -3.758989392382515,
-            "velocityY": -0.42501366599148993,
-            "timestamp": 0.5318103757586307
-        },
-        {
-            "x": 3.6582550295868868,
-            "y": 6.45839320564021,
-            "heading": 0.1344199375479281,
-            "angularVelocity": -8.816415176708587e-8,
-            "velocityX": -3.7589893506813095,
-            "velocityY": -0.42501403481347844,
-            "timestamp": 0.5982866727284595
-        },
-        {
-            "x": 3.4083713401101514,
-            "y": 6.430139820747532,
-            "heading": 0.13441993168710784,
-            "angularVelocity": -8.816406061478911e-8,
-            "velocityX": -3.758989306972801,
-            "velocityY": -0.42501442138844536,
-            "timestamp": 0.6647629696982883
-        },
-        {
-            "x": 3.1584877665624913,
-            "y": 6.401885410552911,
-            "heading": 0.1344199258262894,
-            "angularVelocity": -8.816403354486871e-8,
-            "velocityX": -3.7589875630568486,
-            "velocityY": -0.4250298449602976,
-            "timestamp": 0.7312392666681171
-        },
-        {
-            "x": 2.9135491847991943,
-            "y": 6.344918727874756,
-            "heading": 0.13441991981204132,
-            "angularVelocity": -9.047206787345137e-8,
-            "velocityX": -3.684600269994962,
-            "velocityY": -0.8569472921154051,
-            "timestamp": 0.7977155636379459
-        },
-        {
-            "x": 2.741853960900858,
-            "y": 6.285250221506208,
-            "heading": 0.13441991359516897,
-            "angularVelocity": -1.293850671116252e-7,
-            "velocityX": -3.573307735472737,
-            "velocityY": -1.2418163448570063,
-            "timestamp": 0.8457649443822611
-        },
-        {
-            "x": 2.5774323687799447,
-            "y": 6.207758184927692,
-            "heading": 0.1344190973329971,
-            "angularVelocity": -0.000016987985264431854,
-            "velocityX": -3.4219294728447913,
-            "velocityY": -1.6127582786315857,
-            "timestamp": 0.8938143251265763
-        },
-        {
-            "x": 2.4266376481277105,
-            "y": 6.11739996688274,
-            "heading": 0.11881021575379135,
-            "angularVelocity": -0.32485083756365457,
-            "velocityX": -3.1383280765814066,
-            "velocityY": -1.8805282533353447,
-            "timestamp": 0.9418637058708915
-        },
-        {
-            "x": 2.292578699649266,
-            "y": 6.017904789932588,
-            "heading": 0.09375834131328029,
-            "angularVelocity": -0.5213776754755571,
-            "velocityX": -2.7900244790211017,
-            "velocityY": -2.0706859361954097,
-            "timestamp": 0.9899130866152067
-        },
-        {
-            "x": 2.176263979579871,
-            "y": 5.911118232592638,
-            "heading": 0.0629895244075883,
-            "angularVelocity": -0.6403582403990148,
-            "velocityX": -2.420732968200766,
-            "velocityY": -2.2224335815729503,
-            "timestamp": 1.037962467359522
-        },
-        {
-            "x": 2.0781210346390075,
-            "y": 5.798061969403822,
-            "heading": 0.02853978404947447,
-            "angularVelocity": -0.7169653349214,
-            "velocityX": -2.0425433880846535,
-            "velocityY": -2.3529182153339443,
-            "timestamp": 1.0860118481038372
-        },
-        {
-            "x": 1.9983746548109367,
-            "y": 5.679376225369417,
-            "heading": -0.008333608258354223,
-            "angularVelocity": -0.7674061920598474,
-            "velocityX": -1.659675496182245,
-            "velocityY": -2.470078535787315,
-            "timestamp": 1.1340612288481524
-        },
-        {
-            "x": 1.9371600151062012,
-            "y": 5.555497646331787,
-            "heading": -0.04678243253848999,
-            "angularVelocity": -0.8001939605576434,
-            "velocityX": -1.273994352403345,
-            "velocityY": -2.5781514167024167,
-            "timestamp": 1.1821106095924676
-        },
-        {
-            "x": 1.8879136197731217,
-            "y": 5.371859224657917,
-            "heading": -0.10269858839018778,
-            "angularVelocity": -0.8284337962966012,
-            "velocityX": -0.7296170063605633,
-            "velocityY": -2.720721274486196,
-            "timestamp": 1.2496068369333404
-        },
-        {
-            "x": 1.8760341540552343,
-            "y": 5.181395172179021,
-            "heading": -0.15967327513417093,
-            "angularVelocity": -0.8441166119736836,
-            "velocityX": -0.17600192167619955,
-            "velocityY": -2.821847382921196,
-            "timestamp": 1.3171030642742134
-        },
-        {
-            "x": 1.9020919030528052,
-            "y": 4.989002664772072,
-            "heading": -0.21623702759974286,
-            "angularVelocity": -0.8380283564577732,
-            "velocityX": 0.3860623033932351,
-            "velocityY": -2.8504186824445474,
-            "timestamp": 1.3845992916150864
-        },
-        {
-            "x": 1.9652989688718845,
-            "y": 4.8044863939922715,
-            "heading": -0.26949747833980514,
-            "angularVelocity": -0.7890878177691865,
-            "velocityX": 0.9364533146403533,
-            "velocityY": -2.7337271733418085,
-            "timestamp": 1.4520955189559595
-        },
-        {
-            "x": 2.0553904334698565,
-            "y": 4.6466969161112965,
-            "heading": -0.31411027699435934,
-            "angularVelocity": -0.6609672927236123,
-            "velocityX": 1.3347629659801235,
-            "velocityY": -2.337752554436838,
-            "timestamp": 1.5195917462968325
-        },
-        {
-            "x": 2.145781200762326,
-            "y": 4.526760098072049,
-            "heading": -0.34739291091529667,
-            "angularVelocity": -0.49310361826375293,
-            "velocityX": 1.3391973278146792,
-            "velocityY": -1.7769410641803345,
-            "timestamp": 1.5870879736377055
-        },
-        {
-            "x": 2.220455144634577,
-            "y": 4.441296194130274,
-            "heading": -0.37087004938287466,
-            "angularVelocity": -0.3478288993696278,
-            "velocityX": 1.1063424848196184,
-            "velocityY": -1.2662026798943848,
-            "timestamp": 1.6545842009785785
-        },
-        {
-            "x": 2.2728997785605394,
-            "y": 4.3865419949716715,
-            "heading": -0.3858025738529217,
-            "angularVelocity": -0.22123494983851594,
-            "velocityX": 0.7770009671963372,
-            "velocityY": -0.8112186608901896,
-            "timestamp": 1.7220804283194515
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.393,
-            "angularVelocity": -0.1066344954471229,
-            "velocityX": 0.4015072028040527,
-            "velocityY": -0.3932367188113757,
-            "timestamp": 1.7895766556603245
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.393,
-            "angularVelocity": 8.903141052781706e-32,
-            "velocityX": 0,
-            "velocityY": -2.026142404451774e-33,
-            "timestamp": 1.8570728830011975
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.1.traj
deleted file mode 100644
index 29c2d4bc..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 5.380258045140596e-28,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.304265282414897,
-            "y": 5.638698322322049,
-            "heading": -0.04261698283734242,
-            "angularVelocity": -0.41098530784979936,
-            "velocityX": 0.3979499173257931,
-            "velocityY": 0.7589428456312429,
-            "timestamp": 0.10369466261532896
-        },
-        {
-            "x": 4.38866009880019,
-            "y": 5.795292929312062,
-            "heading": -0.1228057028496872,
-            "angularVelocity": -0.7733157907057984,
-            "velocityX": 0.8138781134605525,
-            "velocityY": 1.510151082422871,
-            "timestamp": 0.20738932523065792
-        },
-        {
-            "x": 4.519821720835651,
-            "y": 6.028001091828693,
-            "heading": -0.23019023538729733,
-            "angularVelocity": -1.0355839908170448,
-            "velocityX": 1.264883058851596,
-            "velocityY": 2.2441672179396264,
-            "timestamp": 0.3110839878459869
-        },
-        {
-            "x": 4.707676475524912,
-            "y": 6.330157785456564,
-            "heading": -0.33387571773962255,
-            "angularVelocity": -0.9999114683169592,
-            "velocityX": 1.8116145031122473,
-            "velocityY": 2.9139078715050806,
-            "timestamp": 0.41477865046131585
-        },
-        {
-            "x": 4.978690211282069,
-            "y": 6.613755936139097,
-            "heading": -0.3338757185080882,
-            "angularVelocity": -7.410850442524386e-9,
-            "velocityX": 2.6135745941189206,
-            "velocityY": 2.7349348898948014,
-            "timestamp": 0.5184733130766448
-        },
-        {
-            "x": 5.307019127730767,
-            "y": 6.828410259978494,
-            "heading": -0.3338757060420003,
-            "angularVelocity": 1.2021918642580205e-7,
-            "velocityX": 3.166304881734213,
-            "velocityY": 2.070061451819255,
-            "timestamp": 0.6221679756919738
-        },
-        {
-            "x": 5.675511606586513,
-            "y": 6.962908031631864,
-            "heading": -0.3338756959696621,
-            "angularVelocity": 9.713458668099172e-8,
-            "velocityX": 3.5536301441350284,
-            "velocityY": 1.2970558779125847,
-            "timestamp": 0.7258626383073028
-        },
-        {
-            "x": 6.055844888125957,
-            "y": 7.058944236891981,
-            "heading": -0.3338756861984713,
-            "angularVelocity": 9.423041207795053e-8,
-            "velocityX": 3.6678192680981745,
-            "velocityY": 0.9261441508940896,
-            "timestamp": 0.8295573009226318
-        },
-        {
-            "x": 6.436176952400414,
-            "y": 7.154985262770466,
-            "heading": -0.3338756764272739,
-            "angularVelocity": 9.423047569354886e-8,
-            "velocityX": 3.667807529162388,
-            "velocityY": 0.9261906394812687,
-            "timestamp": 0.9332519635379608
-        },
-        {
-            "x": 6.8165090646020285,
-            "y": 7.2510260988513195,
-            "heading": -0.33387566665551793,
-            "angularVelocity": 9.423586238606553e-8,
-            "velocityX": 3.6678079913574164,
-            "velocityY": 0.9261888091302356,
-            "timestamp": 1.0369466261532898
-        },
-        {
-            "x": 7.162467192455402,
-            "y": 7.33732437680055,
-            "heading": -0.24283297058924916,
-            "angularVelocity": 0.8779882567727307,
-            "velocityX": 3.336315670718354,
-            "velocityY": 0.8322345217454595,
-            "timestamp": 1.1406412887686188
-        },
-        {
-            "x": 7.421550289934561,
-            "y": 7.4021983509049765,
-            "heading": -0.1315877257451376,
-            "angularVelocity": 1.0728155339758783,
-            "velocityX": 2.4985191228237835,
-            "velocityY": 0.6256250077700213,
-            "timestamp": 1.2443359513839478
-        },
-        {
-            "x": 7.593930847137365,
-            "y": 7.445412549605325,
-            "heading": -0.046073342510862675,
-            "angularVelocity": 0.8246748779298502,
-            "velocityX": 1.662386017323544,
-            "velocityY": 0.41674467721309977,
-            "timestamp": 1.3480306139992768
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0.44431739637147044,
-            "velocityX": 0.8300249086292989,
-            "velocityY": 0.20818285001567757,
-            "timestamp": 1.4517252766146058
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 6.677691966309128e-30,
-            "velocityY": 0,
-            "timestamp": 1.5554199392299348
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.traj
deleted file mode 100644
index 29c2d4bc..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 5.380258045140596e-28,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.304265282414897,
-            "y": 5.638698322322049,
-            "heading": -0.04261698283734242,
-            "angularVelocity": -0.41098530784979936,
-            "velocityX": 0.3979499173257931,
-            "velocityY": 0.7589428456312429,
-            "timestamp": 0.10369466261532896
-        },
-        {
-            "x": 4.38866009880019,
-            "y": 5.795292929312062,
-            "heading": -0.1228057028496872,
-            "angularVelocity": -0.7733157907057984,
-            "velocityX": 0.8138781134605525,
-            "velocityY": 1.510151082422871,
-            "timestamp": 0.20738932523065792
-        },
-        {
-            "x": 4.519821720835651,
-            "y": 6.028001091828693,
-            "heading": -0.23019023538729733,
-            "angularVelocity": -1.0355839908170448,
-            "velocityX": 1.264883058851596,
-            "velocityY": 2.2441672179396264,
-            "timestamp": 0.3110839878459869
-        },
-        {
-            "x": 4.707676475524912,
-            "y": 6.330157785456564,
-            "heading": -0.33387571773962255,
-            "angularVelocity": -0.9999114683169592,
-            "velocityX": 1.8116145031122473,
-            "velocityY": 2.9139078715050806,
-            "timestamp": 0.41477865046131585
-        },
-        {
-            "x": 4.978690211282069,
-            "y": 6.613755936139097,
-            "heading": -0.3338757185080882,
-            "angularVelocity": -7.410850442524386e-9,
-            "velocityX": 2.6135745941189206,
-            "velocityY": 2.7349348898948014,
-            "timestamp": 0.5184733130766448
-        },
-        {
-            "x": 5.307019127730767,
-            "y": 6.828410259978494,
-            "heading": -0.3338757060420003,
-            "angularVelocity": 1.2021918642580205e-7,
-            "velocityX": 3.166304881734213,
-            "velocityY": 2.070061451819255,
-            "timestamp": 0.6221679756919738
-        },
-        {
-            "x": 5.675511606586513,
-            "y": 6.962908031631864,
-            "heading": -0.3338756959696621,
-            "angularVelocity": 9.713458668099172e-8,
-            "velocityX": 3.5536301441350284,
-            "velocityY": 1.2970558779125847,
-            "timestamp": 0.7258626383073028
-        },
-        {
-            "x": 6.055844888125957,
-            "y": 7.058944236891981,
-            "heading": -0.3338756861984713,
-            "angularVelocity": 9.423041207795053e-8,
-            "velocityX": 3.6678192680981745,
-            "velocityY": 0.9261441508940896,
-            "timestamp": 0.8295573009226318
-        },
-        {
-            "x": 6.436176952400414,
-            "y": 7.154985262770466,
-            "heading": -0.3338756764272739,
-            "angularVelocity": 9.423047569354886e-8,
-            "velocityX": 3.667807529162388,
-            "velocityY": 0.9261906394812687,
-            "timestamp": 0.9332519635379608
-        },
-        {
-            "x": 6.8165090646020285,
-            "y": 7.2510260988513195,
-            "heading": -0.33387566665551793,
-            "angularVelocity": 9.423586238606553e-8,
-            "velocityX": 3.6678079913574164,
-            "velocityY": 0.9261888091302356,
-            "timestamp": 1.0369466261532898
-        },
-        {
-            "x": 7.162467192455402,
-            "y": 7.33732437680055,
-            "heading": -0.24283297058924916,
-            "angularVelocity": 0.8779882567727307,
-            "velocityX": 3.336315670718354,
-            "velocityY": 0.8322345217454595,
-            "timestamp": 1.1406412887686188
-        },
-        {
-            "x": 7.421550289934561,
-            "y": 7.4021983509049765,
-            "heading": -0.1315877257451376,
-            "angularVelocity": 1.0728155339758783,
-            "velocityX": 2.4985191228237835,
-            "velocityY": 0.6256250077700213,
-            "timestamp": 1.2443359513839478
-        },
-        {
-            "x": 7.593930847137365,
-            "y": 7.445412549605325,
-            "heading": -0.046073342510862675,
-            "angularVelocity": 0.8246748779298502,
-            "velocityX": 1.662386017323544,
-            "velocityY": 0.41674467721309977,
-            "timestamp": 1.3480306139992768
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0.44431739637147044,
-            "velocityX": 0.8300249086292989,
-            "velocityY": 0.20818285001567757,
-            "timestamp": 1.4517252766146058
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 6.677691966309128e-30,
-            "velocityY": 0,
-            "timestamp": 1.5554199392299348
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2.1.traj
deleted file mode 100644
index 05a18e04..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2.1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.272659932573731,
-            "y": 5.605321030989923,
-            "heading": -5.269773313916661e-8,
-            "angularVelocity": -7.069117507058631e-7,
-            "velocityX": 0.1295790656325301,
-            "velocityY": 0.6079397351586845,
-            "timestamp": 0.07454855863477708
-        },
-        {
-            "x": 4.294668340488188,
-            "y": 5.695305527660525,
-            "heading": -1.390931476532318e-7,
-            "angularVelocity": -0.0000011589489187991593,
-            "velocityX": 0.2952224471421504,
-            "velocityY": 1.2070588395619357,
-            "timestamp": 0.14909711726955416
-        },
-        {
-            "x": 4.333068049168991,
-            "y": 5.8286332482919585,
-            "heading": -2.3393980900082764e-7,
-            "angularVelocity": -0.0000012723197345504802,
-            "velocityX": 0.5150965944736963,
-            "velocityY": 1.7884681217524945,
-            "timestamp": 0.22364567590433124
-        },
-        {
-            "x": 4.394361213548298,
-            "y": 6.0022499148295365,
-            "heading": -3.0377830712505715e-7,
-            "angularVelocity": -9.368513338342214e-7,
-            "velocityX": 0.8221911395208825,
-            "velocityY": 2.3289070872699873,
-            "timestamp": 0.2981942345391083
-        },
-        {
-            "x": 4.489214393007107,
-            "y": 6.207820209268236,
-            "heading": -3.104589480689307e-7,
-            "angularVelocity": -8.962358439949817e-8,
-            "velocityX": 1.2723677183517785,
-            "velocityY": 2.75753546703125,
-            "timestamp": 0.37274279317388537
-        },
-        {
-            "x": 4.629403012529928,
-            "y": 6.42298249437822,
-            "heading": -2.4199647648725644e-7,
-            "angularVelocity": 9.18393943226279e-7,
-            "velocityX": 1.880500737963715,
-            "velocityY": 2.8862031546566858,
-            "timestamp": 0.4472913518086624
-        },
-        {
-            "x": 4.811531801184116,
-            "y": 6.618438666310206,
-            "heading": -1.472447710538824e-7,
-            "angularVelocity": 0.0000012710752897138218,
-            "velocityX": 2.443089336474201,
-            "velocityY": 2.6218638627509074,
-            "timestamp": 0.5218399104434395
-        },
-        {
-            "x": 5.024847227590037,
-            "y": 6.779620899070013,
-            "heading": -6.254572131926062e-8,
-            "angularVelocity": 0.000001136235548213432,
-            "velocityX": 2.861429252247402,
-            "velocityY": 2.1621106520395927,
-            "timestamp": 0.5963884690782165
-        },
-        {
-            "x": 5.261847496032715,
-            "y": 6.900974273681641,
-            "heading": 1.6289151195668303e-9,
-            "angularVelocity": 8.609140934997053e-7,
-            "velocityX": 3.1791395133448552,
-            "velocityY": 1.6278433384167708,
-            "timestamp": 0.6709370277129936
-        },
-        {
-            "x": 5.5631639462020495,
-            "y": 6.984947323321856,
-            "heading": 4.965855378058324e-8,
-            "angularVelocity": 5.539886921154719e-7,
-            "velocityX": 3.4757171947403775,
-            "velocityY": 0.9686380297960585,
-            "timestamp": 0.7576289054784
-        },
-        {
-            "x": 5.883696640724643,
-            "y": 7.009274337947639,
-            "heading": 7.188189606013307e-8,
-            "angularVelocity": 2.563216327096045e-7,
-            "velocityX": 3.697378610459799,
-            "velocityY": 0.2806146929817695,
-            "timestamp": 0.8443207832438064
-        },
-        {
-            "x": 6.209423466668327,
-            "y": 6.971151903870608,
-            "heading": 7.193737396357795e-8,
-            "angularVelocity": 6.438452889791163e-10,
-            "velocityX": 3.7572934667332714,
-            "velocityY": -0.4397463179458525,
-            "timestamp": 0.9310126610092129
-        },
-        {
-            "x": 6.521952745942683,
-            "y": 6.871769894938554,
-            "heading": 7.193784305655878e-8,
-            "angularVelocity": 5.630908705768223e-12,
-            "velocityX": 3.6050583669434406,
-            "velocityY": -1.1463820079014435,
-            "timestamp": 1.0177045387746193
-        },
-        {
-            "x": 6.809873374118548,
-            "y": 6.714756988695251,
-            "heading": 7.193831945924844e-8,
-            "angularVelocity": 5.91084758847972e-12,
-            "velocityX": 3.321194967875227,
-            "velocityY": -1.8111605177424126,
-            "timestamp": 1.1043964165400257
-        },
-        {
-            "x": 7.062672913559571,
-            "y": 6.505846154651799,
-            "heading": 7.193893641048392e-8,
-            "angularVelocity": 7.48396872358976e-12,
-            "velocityX": 2.9160694861627596,
-            "velocityY": -2.4098086166543093,
-            "timestamp": 1.1910882943054322
-        },
-        {
-            "x": 7.268448732261103,
-            "y": 6.269564240975314,
-            "heading": 6.958123303548702e-8,
-            "angularVelocity": -2.7156604096657526e-8,
-            "velocityX": 2.3736458816624144,
-            "velocityY": -2.7255369226670942,
-            "timestamp": 1.2777801720708386
-        },
-        {
-            "x": 7.433069281193563,
-            "y": 6.080538590855167,
-            "heading": 5.4913746267249155e-8,
-            "angularVelocity": -1.6915879682512025e-7,
-            "velocityX": 1.8989154825033356,
-            "velocityY": -2.180430912690726,
-            "timestamp": 1.364472049836245
-        },
-        {
-            "x": 7.556534655605292,
-            "y": 5.9387693118426395,
-            "heading": 3.4027865091828515e-8,
-            "angularVelocity": -2.408962130800618e-7,
-            "velocityX": 1.424186181835385,
-            "velocityY": -1.635323662267605,
-            "timestamp": 1.4511639276016515
-        },
-        {
-            "x": 7.638844889158882,
-            "y": 5.8442564415645775,
-            "heading": 1.3493455787871544e-8,
-            "angularVelocity": -2.368497714278514e-7,
-            "velocityX": 0.9494572693998051,
-            "velocityY": -1.0902159778725726,
-            "timestamp": 1.537855805367058
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -2.563219583366834e-29,
-            "angularVelocity": -1.5563981959470815e-7,
-            "velocityX": 0.4747285662416774,
-            "velocityY": -0.5451080630467725,
-            "timestamp": 1.6245476831324643
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 4.328374921762355e-28,
-            "velocityX": 0,
-            "velocityY": 6.636862452110489e-29,
-            "timestamp": 1.7112395608978708
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2.traj
deleted file mode 100644
index 05a18e04..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.272659932573731,
-            "y": 5.605321030989923,
-            "heading": -5.269773313916661e-8,
-            "angularVelocity": -7.069117507058631e-7,
-            "velocityX": 0.1295790656325301,
-            "velocityY": 0.6079397351586845,
-            "timestamp": 0.07454855863477708
-        },
-        {
-            "x": 4.294668340488188,
-            "y": 5.695305527660525,
-            "heading": -1.390931476532318e-7,
-            "angularVelocity": -0.0000011589489187991593,
-            "velocityX": 0.2952224471421504,
-            "velocityY": 1.2070588395619357,
-            "timestamp": 0.14909711726955416
-        },
-        {
-            "x": 4.333068049168991,
-            "y": 5.8286332482919585,
-            "heading": -2.3393980900082764e-7,
-            "angularVelocity": -0.0000012723197345504802,
-            "velocityX": 0.5150965944736963,
-            "velocityY": 1.7884681217524945,
-            "timestamp": 0.22364567590433124
-        },
-        {
-            "x": 4.394361213548298,
-            "y": 6.0022499148295365,
-            "heading": -3.0377830712505715e-7,
-            "angularVelocity": -9.368513338342214e-7,
-            "velocityX": 0.8221911395208825,
-            "velocityY": 2.3289070872699873,
-            "timestamp": 0.2981942345391083
-        },
-        {
-            "x": 4.489214393007107,
-            "y": 6.207820209268236,
-            "heading": -3.104589480689307e-7,
-            "angularVelocity": -8.962358439949817e-8,
-            "velocityX": 1.2723677183517785,
-            "velocityY": 2.75753546703125,
-            "timestamp": 0.37274279317388537
-        },
-        {
-            "x": 4.629403012529928,
-            "y": 6.42298249437822,
-            "heading": -2.4199647648725644e-7,
-            "angularVelocity": 9.18393943226279e-7,
-            "velocityX": 1.880500737963715,
-            "velocityY": 2.8862031546566858,
-            "timestamp": 0.4472913518086624
-        },
-        {
-            "x": 4.811531801184116,
-            "y": 6.618438666310206,
-            "heading": -1.472447710538824e-7,
-            "angularVelocity": 0.0000012710752897138218,
-            "velocityX": 2.443089336474201,
-            "velocityY": 2.6218638627509074,
-            "timestamp": 0.5218399104434395
-        },
-        {
-            "x": 5.024847227590037,
-            "y": 6.779620899070013,
-            "heading": -6.254572131926062e-8,
-            "angularVelocity": 0.000001136235548213432,
-            "velocityX": 2.861429252247402,
-            "velocityY": 2.1621106520395927,
-            "timestamp": 0.5963884690782165
-        },
-        {
-            "x": 5.261847496032715,
-            "y": 6.900974273681641,
-            "heading": 1.6289151195668303e-9,
-            "angularVelocity": 8.609140934997053e-7,
-            "velocityX": 3.1791395133448552,
-            "velocityY": 1.6278433384167708,
-            "timestamp": 0.6709370277129936
-        },
-        {
-            "x": 5.5631639462020495,
-            "y": 6.984947323321856,
-            "heading": 4.965855378058324e-8,
-            "angularVelocity": 5.539886921154719e-7,
-            "velocityX": 3.4757171947403775,
-            "velocityY": 0.9686380297960585,
-            "timestamp": 0.7576289054784
-        },
-        {
-            "x": 5.883696640724643,
-            "y": 7.009274337947639,
-            "heading": 7.188189606013307e-8,
-            "angularVelocity": 2.563216327096045e-7,
-            "velocityX": 3.697378610459799,
-            "velocityY": 0.2806146929817695,
-            "timestamp": 0.8443207832438064
-        },
-        {
-            "x": 6.209423466668327,
-            "y": 6.971151903870608,
-            "heading": 7.193737396357795e-8,
-            "angularVelocity": 6.438452889791163e-10,
-            "velocityX": 3.7572934667332714,
-            "velocityY": -0.4397463179458525,
-            "timestamp": 0.9310126610092129
-        },
-        {
-            "x": 6.521952745942683,
-            "y": 6.871769894938554,
-            "heading": 7.193784305655878e-8,
-            "angularVelocity": 5.630908705768223e-12,
-            "velocityX": 3.6050583669434406,
-            "velocityY": -1.1463820079014435,
-            "timestamp": 1.0177045387746193
-        },
-        {
-            "x": 6.809873374118548,
-            "y": 6.714756988695251,
-            "heading": 7.193831945924844e-8,
-            "angularVelocity": 5.91084758847972e-12,
-            "velocityX": 3.321194967875227,
-            "velocityY": -1.8111605177424126,
-            "timestamp": 1.1043964165400257
-        },
-        {
-            "x": 7.062672913559571,
-            "y": 6.505846154651799,
-            "heading": 7.193893641048392e-8,
-            "angularVelocity": 7.48396872358976e-12,
-            "velocityX": 2.9160694861627596,
-            "velocityY": -2.4098086166543093,
-            "timestamp": 1.1910882943054322
-        },
-        {
-            "x": 7.268448732261103,
-            "y": 6.269564240975314,
-            "heading": 6.958123303548702e-8,
-            "angularVelocity": -2.7156604096657526e-8,
-            "velocityX": 2.3736458816624144,
-            "velocityY": -2.7255369226670942,
-            "timestamp": 1.2777801720708386
-        },
-        {
-            "x": 7.433069281193563,
-            "y": 6.080538590855167,
-            "heading": 5.4913746267249155e-8,
-            "angularVelocity": -1.6915879682512025e-7,
-            "velocityX": 1.8989154825033356,
-            "velocityY": -2.180430912690726,
-            "timestamp": 1.364472049836245
-        },
-        {
-            "x": 7.556534655605292,
-            "y": 5.9387693118426395,
-            "heading": 3.4027865091828515e-8,
-            "angularVelocity": -2.408962130800618e-7,
-            "velocityX": 1.424186181835385,
-            "velocityY": -1.635323662267605,
-            "timestamp": 1.4511639276016515
-        },
-        {
-            "x": 7.638844889158882,
-            "y": 5.8442564415645775,
-            "heading": 1.3493455787871544e-8,
-            "angularVelocity": -2.368497714278514e-7,
-            "velocityX": 0.9494572693998051,
-            "velocityY": -1.0902159778725726,
-            "timestamp": 1.537855805367058
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -2.563219583366834e-29,
-            "angularVelocity": -1.5563981959470815e-7,
-            "velocityX": 0.4747285662416774,
-            "velocityY": -0.5451080630467725,
-            "timestamp": 1.6245476831324643
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 4.328374921762355e-28,
-            "velocityX": 0,
-            "velocityY": 6.636862452110489e-29,
-            "timestamp": 1.7112395608978708
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.1.traj
deleted file mode 100644
index 2b663e69..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -9.522541691773037e-34,
-            "velocityX": 0,
-            "velocityY": -4.2308696063129985e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 4.271262994735721,
-            "y": 5.464085078742271,
-            "heading": 0.013066573413208545,
-            "angularVelocity": 0.12156077000957308,
-            "velocityX": 0.0768721814736735,
-            "velocityY": -0.8923144052220247,
-            "timestamp": 0.10749005137249076
-        },
-        {
-            "x": 4.301424317332477,
-            "y": 5.27437671657762,
-            "heading": 0.033784324267673776,
-            "angularVelocity": 0.192741101059399,
-            "velocityX": 0.2805964106597774,
-            "velocityY": -1.7648922829820315,
-            "timestamp": 0.21498010274498153
-        },
-        {
-            "x": 4.379013311524821,
-            "y": 5.000817118183451,
-            "heading": 0.051063906614390905,
-            "angularVelocity": 0.16075517804747627,
-            "velocityX": 0.7218248870629933,
-            "velocityY": -2.544975975927198,
-            "timestamp": 0.32247015411747226
-        },
-        {
-            "x": 4.5426840546799285,
-            "y": 4.684552935799795,
-            "heading": 0.043724640930798246,
-            "angularVelocity": -0.06827855778168279,
-            "velocityX": 1.5226594560638123,
-            "velocityY": -2.9422646872470954,
-            "timestamp": 0.42996020548996305
-        },
-        {
-            "x": 4.7987371937718075,
-            "y": 4.3933637164950845,
-            "heading": -0.0025480008668042047,
-            "angularVelocity": -0.4304830187237655,
-            "velocityX": 2.382110119238521,
-            "velocityY": -2.7089876280330105,
-            "timestamp": 0.5374502568624538
-        },
-        {
-            "x": 5.129552682758233,
-            "y": 4.161939062753506,
-            "heading": -0.00976861092561694,
-            "angularVelocity": -0.06717468236935505,
-            "velocityX": 3.0776382070935524,
-            "velocityY": -2.152986725623672,
-            "timestamp": 0.6449403082349446
-        },
-        {
-            "x": 5.508331065778466,
-            "y": 4.0140416634822165,
-            "heading": -0.009768639154040644,
-            "angularVelocity": -2.6261429169311795e-7,
-            "velocityX": 3.5238459576843355,
-            "velocityY": -1.3759170954228361,
-            "timestamp": 0.7524303596074354
-        },
-        {
-            "x": 5.911272135069668,
-            "y": 3.959404581326429,
-            "heading": -0.009768652411608816,
-            "angularVelocity": -1.233376298753571e-7,
-            "velocityX": 3.7486359355701584,
-            "velocityY": -0.5082989677477323,
-            "timestamp": 0.8599204109799262
-        },
-        {
-            "x": 6.316087697151271,
-            "y": 3.997759236961094,
-            "heading": -0.009768652034230711,
-            "angularVelocity": 3.51081891151765e-9,
-            "velocityX": 3.7660746916826353,
-            "velocityY": 0.3568205163634492,
-            "timestamp": 0.967410462352417
-        },
-        {
-            "x": 6.720902827559142,
-            "y": 4.036118448432054,
-            "heading": -0.009768651656829377,
-            "angularVelocity": 3.5110350109919825e-9,
-            "velocityX": 3.766070675741356,
-            "velocityY": 0.35686290015837574,
-            "timestamp": 1.0749005137249077
-        },
-        {
-            "x": 7.104541677751467,
-            "y": 4.072471064803872,
-            "heading": -0.005861206189315246,
-            "angularVelocity": 0.03635169411142489,
-            "velocityX": 3.569063790497984,
-            "velocityY": 0.3381951716242375,
-            "timestamp": 1.1823905650973985
-        },
-        {
-            "x": 7.392270834070994,
-            "y": 4.099735530737146,
-            "heading": -0.0029306066828158664,
-            "angularVelocity": 0.027263913907193366,
-            "velocityX": 2.6767980166131413,
-            "velocityY": 0.2536464127158502,
-            "timestamp": 1.2898806164698893
-        },
-        {
-            "x": 7.584090276967534,
-            "y": 4.117911843086064,
-            "heading": -0.0009768696053991616,
-            "angularVelocity": 0.018175980497453854,
-            "velocityX": 1.7845320608492279,
-            "velocityY": 0.16909762454137037,
-            "timestamp": 1.39737066784238
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -6.809535330810639e-32,
-            "angularVelocity": 0.009088000172350516,
-            "velocityX": 0.8922660451626774,
-            "velocityY": 0.08454881915017429,
-            "timestamp": 1.5048607192148709
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -9.489213882041181e-31,
-            "velocityX": 2.198140684032685e-33,
-            "velocityY": -6.756921472052438e-32,
-            "timestamp": 1.6123507705873616
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.traj
deleted file mode 100644
index 2b663e69..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -9.522541691773037e-34,
-            "velocityX": 0,
-            "velocityY": -4.2308696063129985e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 4.271262994735721,
-            "y": 5.464085078742271,
-            "heading": 0.013066573413208545,
-            "angularVelocity": 0.12156077000957308,
-            "velocityX": 0.0768721814736735,
-            "velocityY": -0.8923144052220247,
-            "timestamp": 0.10749005137249076
-        },
-        {
-            "x": 4.301424317332477,
-            "y": 5.27437671657762,
-            "heading": 0.033784324267673776,
-            "angularVelocity": 0.192741101059399,
-            "velocityX": 0.2805964106597774,
-            "velocityY": -1.7648922829820315,
-            "timestamp": 0.21498010274498153
-        },
-        {
-            "x": 4.379013311524821,
-            "y": 5.000817118183451,
-            "heading": 0.051063906614390905,
-            "angularVelocity": 0.16075517804747627,
-            "velocityX": 0.7218248870629933,
-            "velocityY": -2.544975975927198,
-            "timestamp": 0.32247015411747226
-        },
-        {
-            "x": 4.5426840546799285,
-            "y": 4.684552935799795,
-            "heading": 0.043724640930798246,
-            "angularVelocity": -0.06827855778168279,
-            "velocityX": 1.5226594560638123,
-            "velocityY": -2.9422646872470954,
-            "timestamp": 0.42996020548996305
-        },
-        {
-            "x": 4.7987371937718075,
-            "y": 4.3933637164950845,
-            "heading": -0.0025480008668042047,
-            "angularVelocity": -0.4304830187237655,
-            "velocityX": 2.382110119238521,
-            "velocityY": -2.7089876280330105,
-            "timestamp": 0.5374502568624538
-        },
-        {
-            "x": 5.129552682758233,
-            "y": 4.161939062753506,
-            "heading": -0.00976861092561694,
-            "angularVelocity": -0.06717468236935505,
-            "velocityX": 3.0776382070935524,
-            "velocityY": -2.152986725623672,
-            "timestamp": 0.6449403082349446
-        },
-        {
-            "x": 5.508331065778466,
-            "y": 4.0140416634822165,
-            "heading": -0.009768639154040644,
-            "angularVelocity": -2.6261429169311795e-7,
-            "velocityX": 3.5238459576843355,
-            "velocityY": -1.3759170954228361,
-            "timestamp": 0.7524303596074354
-        },
-        {
-            "x": 5.911272135069668,
-            "y": 3.959404581326429,
-            "heading": -0.009768652411608816,
-            "angularVelocity": -1.233376298753571e-7,
-            "velocityX": 3.7486359355701584,
-            "velocityY": -0.5082989677477323,
-            "timestamp": 0.8599204109799262
-        },
-        {
-            "x": 6.316087697151271,
-            "y": 3.997759236961094,
-            "heading": -0.009768652034230711,
-            "angularVelocity": 3.51081891151765e-9,
-            "velocityX": 3.7660746916826353,
-            "velocityY": 0.3568205163634492,
-            "timestamp": 0.967410462352417
-        },
-        {
-            "x": 6.720902827559142,
-            "y": 4.036118448432054,
-            "heading": -0.009768651656829377,
-            "angularVelocity": 3.5110350109919825e-9,
-            "velocityX": 3.766070675741356,
-            "velocityY": 0.35686290015837574,
-            "timestamp": 1.0749005137249077
-        },
-        {
-            "x": 7.104541677751467,
-            "y": 4.072471064803872,
-            "heading": -0.005861206189315246,
-            "angularVelocity": 0.03635169411142489,
-            "velocityX": 3.569063790497984,
-            "velocityY": 0.3381951716242375,
-            "timestamp": 1.1823905650973985
-        },
-        {
-            "x": 7.392270834070994,
-            "y": 4.099735530737146,
-            "heading": -0.0029306066828158664,
-            "angularVelocity": 0.027263913907193366,
-            "velocityX": 2.6767980166131413,
-            "velocityY": 0.2536464127158502,
-            "timestamp": 1.2898806164698893
-        },
-        {
-            "x": 7.584090276967534,
-            "y": 4.117911843086064,
-            "heading": -0.0009768696053991616,
-            "angularVelocity": 0.018175980497453854,
-            "velocityX": 1.7845320608492279,
-            "velocityY": 0.16909762454137037,
-            "timestamp": 1.39737066784238
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -6.809535330810639e-32,
-            "angularVelocity": 0.009088000172350516,
-            "velocityX": 0.8922660451626774,
-            "velocityY": 0.08454881915017429,
-            "timestamp": 1.5048607192148709
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -9.489213882041181e-31,
-            "velocityX": 2.198140684032685e-33,
-            "velocityY": -6.756921472052438e-32,
-            "timestamp": 1.6123507705873616
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C4.1.traj
deleted file mode 100644
index c13783bc..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C4.1.traj	
+++ /dev/null
@@ -1,166 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 7.441191049306378e-32,
-            "velocityX": -6.15829104129807e-34,
-            "velocityY": 2.4252394047543234e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 4.262142393092874,
-            "y": 5.471176724349191,
-            "heading": -0.06580713819401311,
-            "angularVelocity": -0.6311237133024392,
-            "velocityX": -0.008224883662067025,
-            "velocityY": -0.8518601035521746,
-            "timestamp": 0.10426979181255681
-        },
-        {
-            "x": 4.27936690849282,
-            "y": 5.294863623901166,
-            "heading": -0.18680392528158013,
-            "angularVelocity": -1.1604203382805245,
-            "velocityX": 0.16519180771848577,
-            "velocityY": -1.6909317395107013,
-            "timestamp": 0.20853958362511363
-        },
-        {
-            "x": 4.348116425232891,
-            "y": 5.0445566747334585,
-            "heading": -0.3412830267774945,
-            "angularVelocity": -1.4815326549574157,
-            "velocityX": 0.6593426105967491,
-            "velocityY": -2.400570144205118,
-            "timestamp": 0.31280937543767046
-        },
-        {
-            "x": 4.502267029635194,
-            "y": 4.763921050939977,
-            "heading": -0.4878370803946758,
-            "angularVelocity": -1.4055274405902505,
-            "velocityX": 1.4783822018117698,
-            "velocityY": -2.6914374615609895,
-            "timestamp": 0.41707916725022726
-        },
-        {
-            "x": 4.740029978369904,
-            "y": 4.496898841850153,
-            "heading": -0.5207249369577782,
-            "angularVelocity": -0.31541116551017917,
-            "velocityX": 2.280266840487519,
-            "velocityY": -2.5608779345205184,
-            "timestamp": 0.5213489590627841
-        },
-        {
-            "x": 5.0582868126043,
-            "y": 4.2697391199027495,
-            "heading": -0.509978725166827,
-            "angularVelocity": 0.10306160206274768,
-            "velocityX": 3.0522438829312937,
-            "velocityY": -2.1785765368723764,
-            "timestamp": 0.6256187508753409
-        },
-        {
-            "x": 5.423286105863135,
-            "y": 4.1201949004052105,
-            "heading": -0.5099785687835147,
-            "angularVelocity": 0.0000014997949991595825,
-            "velocityX": 3.5005276879710774,
-            "velocityY": -1.4342046425715584,
-            "timestamp": 0.7298885426878977
-        },
-        {
-            "x": 5.805099761015269,
-            "y": 4.021168099470067,
-            "heading": -0.5099784851647622,
-            "angularVelocity": 8.019460959727046e-7,
-            "velocityX": 3.6617859162748747,
-            "velocityY": -0.9497170677501874,
-            "timestamp": 0.8341583345004545
-        },
-        {
-            "x": 6.154222032422552,
-            "y": 3.8375880828272497,
-            "heading": -0.5099784796190555,
-            "angularVelocity": 5.318612994948929e-8,
-            "velocityX": 3.348259024386382,
-            "velocityY": -1.7606251384181746,
-            "timestamp": 0.9384281263130113
-        },
-        {
-            "x": 6.493379431179238,
-            "y": 3.636192348012897,
-            "heading": -0.5099784741544249,
-            "angularVelocity": 5.240856867670856e-8,
-            "velocityX": 3.2526908595576858,
-            "velocityY": -1.9314868795019449,
-            "timestamp": 1.0426979181255682
-        },
-        {
-            "x": 6.795285003697281,
-            "y": 3.3823400148372036,
-            "heading": -0.5099784394461939,
-            "angularVelocity": 3.32869476219209e-7,
-            "velocityX": 2.895427019368865,
-            "velocityY": -2.4345721686299853,
-            "timestamp": 1.146967709938125
-        },
-        {
-            "x": 7.066681771760518,
-            "y": 3.0961027469536373,
-            "heading": -0.509978390292543,
-            "angularVelocity": 4.714083530965781e-7,
-            "velocityX": 2.6028321659174227,
-            "velocityY": -2.7451600593787298,
-            "timestamp": 1.2512375017506818
-        },
-        {
-            "x": 7.312943361341716,
-            "y": 2.8410544331524363,
-            "heading": -0.40533862017886096,
-            "angularVelocity": 1.0035482788897323,
-            "velocityX": 2.361773101301441,
-            "velocityY": -2.446042227260748,
-            "timestamp": 1.3555072935632386
-        },
-        {
-            "x": 7.49731350893092,
-            "y": 2.6484381177603322,
-            "heading": -0.22178416093118458,
-            "angularVelocity": 1.7603800300824186,
-            "velocityX": 1.768202893515326,
-            "velocityY": -1.8472878102448496,
-            "timestamp": 1.4597770853757954
-        },
-        {
-            "x": 7.619307085639959,
-            "y": 2.520663921373434,
-            "heading": -0.0776901780416505,
-            "angularVelocity": 1.3819341190262298,
-            "velocityX": 1.1699800545142,
-            "velocityY": -1.225419118670484,
-            "timestamp": 1.5640468771883522
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 1.429761379305419e-34,
-            "angularVelocity": 0.7450880709660586,
-            "velocityX": 0.5820757220763062,
-            "velocityY": -0.6105691808408042,
-            "timestamp": 1.668316669000909
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -5.61453765178046e-32,
-            "velocityX": 0,
-            "velocityY": 2.318867528557263e-33,
-            "timestamp": 1.7725864608134658
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C4.traj
deleted file mode 100644
index c13783bc..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C4.traj	
+++ /dev/null
@@ -1,166 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 7.441191049306378e-32,
-            "velocityX": -6.15829104129807e-34,
-            "velocityY": 2.4252394047543234e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 4.262142393092874,
-            "y": 5.471176724349191,
-            "heading": -0.06580713819401311,
-            "angularVelocity": -0.6311237133024392,
-            "velocityX": -0.008224883662067025,
-            "velocityY": -0.8518601035521746,
-            "timestamp": 0.10426979181255681
-        },
-        {
-            "x": 4.27936690849282,
-            "y": 5.294863623901166,
-            "heading": -0.18680392528158013,
-            "angularVelocity": -1.1604203382805245,
-            "velocityX": 0.16519180771848577,
-            "velocityY": -1.6909317395107013,
-            "timestamp": 0.20853958362511363
-        },
-        {
-            "x": 4.348116425232891,
-            "y": 5.0445566747334585,
-            "heading": -0.3412830267774945,
-            "angularVelocity": -1.4815326549574157,
-            "velocityX": 0.6593426105967491,
-            "velocityY": -2.400570144205118,
-            "timestamp": 0.31280937543767046
-        },
-        {
-            "x": 4.502267029635194,
-            "y": 4.763921050939977,
-            "heading": -0.4878370803946758,
-            "angularVelocity": -1.4055274405902505,
-            "velocityX": 1.4783822018117698,
-            "velocityY": -2.6914374615609895,
-            "timestamp": 0.41707916725022726
-        },
-        {
-            "x": 4.740029978369904,
-            "y": 4.496898841850153,
-            "heading": -0.5207249369577782,
-            "angularVelocity": -0.31541116551017917,
-            "velocityX": 2.280266840487519,
-            "velocityY": -2.5608779345205184,
-            "timestamp": 0.5213489590627841
-        },
-        {
-            "x": 5.0582868126043,
-            "y": 4.2697391199027495,
-            "heading": -0.509978725166827,
-            "angularVelocity": 0.10306160206274768,
-            "velocityX": 3.0522438829312937,
-            "velocityY": -2.1785765368723764,
-            "timestamp": 0.6256187508753409
-        },
-        {
-            "x": 5.423286105863135,
-            "y": 4.1201949004052105,
-            "heading": -0.5099785687835147,
-            "angularVelocity": 0.0000014997949991595825,
-            "velocityX": 3.5005276879710774,
-            "velocityY": -1.4342046425715584,
-            "timestamp": 0.7298885426878977
-        },
-        {
-            "x": 5.805099761015269,
-            "y": 4.021168099470067,
-            "heading": -0.5099784851647622,
-            "angularVelocity": 8.019460959727046e-7,
-            "velocityX": 3.6617859162748747,
-            "velocityY": -0.9497170677501874,
-            "timestamp": 0.8341583345004545
-        },
-        {
-            "x": 6.154222032422552,
-            "y": 3.8375880828272497,
-            "heading": -0.5099784796190555,
-            "angularVelocity": 5.318612994948929e-8,
-            "velocityX": 3.348259024386382,
-            "velocityY": -1.7606251384181746,
-            "timestamp": 0.9384281263130113
-        },
-        {
-            "x": 6.493379431179238,
-            "y": 3.636192348012897,
-            "heading": -0.5099784741544249,
-            "angularVelocity": 5.240856867670856e-8,
-            "velocityX": 3.2526908595576858,
-            "velocityY": -1.9314868795019449,
-            "timestamp": 1.0426979181255682
-        },
-        {
-            "x": 6.795285003697281,
-            "y": 3.3823400148372036,
-            "heading": -0.5099784394461939,
-            "angularVelocity": 3.32869476219209e-7,
-            "velocityX": 2.895427019368865,
-            "velocityY": -2.4345721686299853,
-            "timestamp": 1.146967709938125
-        },
-        {
-            "x": 7.066681771760518,
-            "y": 3.0961027469536373,
-            "heading": -0.509978390292543,
-            "angularVelocity": 4.714083530965781e-7,
-            "velocityX": 2.6028321659174227,
-            "velocityY": -2.7451600593787298,
-            "timestamp": 1.2512375017506818
-        },
-        {
-            "x": 7.312943361341716,
-            "y": 2.8410544331524363,
-            "heading": -0.40533862017886096,
-            "angularVelocity": 1.0035482788897323,
-            "velocityX": 2.361773101301441,
-            "velocityY": -2.446042227260748,
-            "timestamp": 1.3555072935632386
-        },
-        {
-            "x": 7.49731350893092,
-            "y": 2.6484381177603322,
-            "heading": -0.22178416093118458,
-            "angularVelocity": 1.7603800300824186,
-            "velocityX": 1.768202893515326,
-            "velocityY": -1.8472878102448496,
-            "timestamp": 1.4597770853757954
-        },
-        {
-            "x": 7.619307085639959,
-            "y": 2.520663921373434,
-            "heading": -0.0776901780416505,
-            "angularVelocity": 1.3819341190262298,
-            "velocityX": 1.1699800545142,
-            "velocityY": -1.225419118670484,
-            "timestamp": 1.5640468771883522
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 1.429761379305419e-34,
-            "angularVelocity": 0.7450880709660586,
-            "velocityX": 0.5820757220763062,
-            "velocityY": -0.6105691808408042,
-            "timestamp": 1.668316669000909
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -5.61453765178046e-32,
-            "velocityX": 0,
-            "velocityY": 2.318867528557263e-33,
-            "timestamp": 1.7725864608134658
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C5.1.traj
deleted file mode 100644
index 004abebc..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C5.1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -1.2613927548149136e-32,
-            "velocityX": 4.983015637334932e-33,
-            "velocityY": -3.6905945238681026e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 4.258006222511338,
-            "y": 5.473607841813564,
-            "heading": -0.06530003091818118,
-            "angularVelocity": -0.6342708718414432,
-            "velocityX": -0.04850545332028736,
-            "velocityY": -0.8391424739420397,
-            "timestamp": 0.10295290831912149
-        },
-        {
-            "x": 4.271377859079598,
-            "y": 5.302453682559796,
-            "heading": -0.1916286288786279,
-            "angularVelocity": -1.2270522515873779,
-            "velocityX": 0.12988109599401854,
-            "velocityY": -1.662450940416803,
-            "timestamp": 0.20590581663824298
-        },
-        {
-            "x": 4.340766271943036,
-            "y": 5.0642424556348775,
-            "heading": -0.3653686640748466,
-            "angularVelocity": -1.6875680156375907,
-            "velocityX": 0.673982056420939,
-            "velocityY": -2.3137882242873524,
-            "timestamp": 0.30885872495736444
-        },
-        {
-            "x": 4.494848034080926,
-            "y": 4.801255485663876,
-            "heading": -0.5565822720790902,
-            "angularVelocity": -1.8572919514963226,
-            "velocityX": 1.4966236957608356,
-            "velocityY": -2.5544394448365244,
-            "timestamp": 0.41181163327648596
-        },
-        {
-            "x": 4.73233691115851,
-            "y": 4.546498868370588,
-            "heading": -0.6490495118957932,
-            "angularVelocity": -0.8981508276588281,
-            "velocityX": 2.3067719111095206,
-            "velocityY": -2.4744965582091383,
-            "timestamp": 0.5147645415956075
-        },
-        {
-            "x": 5.049349589860016,
-            "y": 4.3202563768587865,
-            "heading": -0.6490493910522949,
-            "angularVelocity": 0.0000011737744981731646,
-            "velocityX": 3.079200810130272,
-            "velocityY": -2.1975337579636096,
-            "timestamp": 0.617717449914729
-        },
-        {
-            "x": 5.4092082222704905,
-            "y": 4.171311637049865,
-            "heading": -0.6490493274377749,
-            "angularVelocity": 6.178992034513071e-7,
-            "velocityX": 3.4953712166637025,
-            "velocityY": -1.446726879703489,
-            "timestamp": 0.7206703582338505
-        },
-        {
-            "x": 5.789554256648214,
-            "y": 4.087528164867666,
-            "heading": -0.6490492895940018,
-            "angularVelocity": 3.675833320529158e-7,
-            "velocityX": 3.6943690138288297,
-            "velocityY": -0.8138038405141147,
-            "timestamp": 0.823623266552972
-        },
-        {
-            "x": 6.141218018459777,
-            "y": 3.9201502365347105,
-            "heading": -0.6490493503243893,
-            "angularVelocity": -5.898851085689342e-7,
-            "velocityX": 3.4157729738097102,
-            "velocityY": -1.6257717345306835,
-            "timestamp": 0.9265761748720935
-        },
-        {
-            "x": 6.456138339829479,
-            "y": 3.69100407104571,
-            "heading": -0.6490494074378677,
-            "angularVelocity": -5.547534235510974e-7,
-            "velocityX": 3.0588773693847333,
-            "velocityY": -2.2257376622981755,
-            "timestamp": 1.029529083191215
-        },
-        {
-            "x": 6.721775326533507,
-            "y": 3.406188756476768,
-            "heading": -0.6490494689481024,
-            "angularVelocity": -5.974599048336077e-7,
-            "velocityX": 2.580179531020508,
-            "velocityY": -2.7664620574496457,
-            "timestamp": 1.1324819915103363
-        },
-        {
-            "x": 6.916359430113237,
-            "y": 3.0688168532177102,
-            "heading": -0.6490494077682868,
-            "angularVelocity": 5.942504852606566e-7,
-            "velocityX": 1.8900301774533879,
-            "velocityY": -3.276953597204961,
-            "timestamp": 1.2354348998294578
-        },
-        {
-            "x": 7.040594979350531,
-            "y": 2.699698573462511,
-            "heading": -0.6490493528706274,
-            "angularVelocity": 5.332307776860117e-7,
-            "velocityX": 1.206722095234099,
-            "velocityY": -3.585311826364818,
-            "timestamp": 1.3383878081485792
-        },
-        {
-            "x": 7.164812386541997,
-            "y": 2.330574188065911,
-            "heading": -0.649049297974002,
-            "angularVelocity": 5.332207349098274e-7,
-            "velocityX": 1.2065458783003051,
-            "velocityY": -3.585371131551044,
-            "timestamp": 1.4413407164677006
-        },
-        {
-            "x": 7.289029734328084,
-            "y": 1.9614497826782673,
-            "heading": -0.6490492430773671,
-            "angularVelocity": 5.332208261000816e-7,
-            "velocityX": 1.206545301285243,
-            "velocityY": -3.5853713257276327,
-            "timestamp": 1.544293624786822
-        },
-        {
-            "x": 7.413246520819429,
-            "y": 1.5923251884092744,
-            "heading": -0.6490491881699392,
-            "angularVelocity": 5.333256623024379e-7,
-            "velocityX": 1.2065398493290926,
-            "velocityY": -3.585373160365936,
-            "timestamp": 1.6472465331059434
-        },
-        {
-            "x": 7.520074458284658,
-            "y": 1.2704783284902474,
-            "heading": -0.5256216946651037,
-            "angularVelocity": 1.1988733054752465,
-            "velocityX": 1.037638850707321,
-            "velocityY": -3.1261560763432104,
-            "timestamp": 1.7501994414250648
-        },
-        {
-            "x": 7.601104601009551,
-            "y": 1.0271731610238788,
-            "heading": -0.2913266412644364,
-            "angularVelocity": 2.275749730881093,
-            "velocityX": 0.7870602593733868,
-            "velocityY": -2.3632665792422265,
-            "timestamp": 1.8531523497441862
-        },
-        {
-            "x": 7.654009357663848,
-            "y": 0.8666565360101991,
-            "heading": -0.10310334462657153,
-            "angularVelocity": 1.828246522715339,
-            "velocityX": 0.5138733574219042,
-            "velocityY": -1.5591266690216163,
-            "timestamp": 1.9561052580633076
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -1.429149737287701e-32,
-            "angularVelocity": 1.0014612147428026,
-            "velocityX": 0.2524517545010862,
-            "velocityY": -0.7737181718391956,
-            "timestamp": 2.059058166382429
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -8.032741852653566e-32,
-            "velocityX": 0,
-            "velocityY": 2.733638748586157e-32,
-            "timestamp": 2.1620110747015504
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C5.traj
deleted file mode 100644
index 004abebc..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C5.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -1.2613927548149136e-32,
-            "velocityX": 4.983015637334932e-33,
-            "velocityY": -3.6905945238681026e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 4.258006222511338,
-            "y": 5.473607841813564,
-            "heading": -0.06530003091818118,
-            "angularVelocity": -0.6342708718414432,
-            "velocityX": -0.04850545332028736,
-            "velocityY": -0.8391424739420397,
-            "timestamp": 0.10295290831912149
-        },
-        {
-            "x": 4.271377859079598,
-            "y": 5.302453682559796,
-            "heading": -0.1916286288786279,
-            "angularVelocity": -1.2270522515873779,
-            "velocityX": 0.12988109599401854,
-            "velocityY": -1.662450940416803,
-            "timestamp": 0.20590581663824298
-        },
-        {
-            "x": 4.340766271943036,
-            "y": 5.0642424556348775,
-            "heading": -0.3653686640748466,
-            "angularVelocity": -1.6875680156375907,
-            "velocityX": 0.673982056420939,
-            "velocityY": -2.3137882242873524,
-            "timestamp": 0.30885872495736444
-        },
-        {
-            "x": 4.494848034080926,
-            "y": 4.801255485663876,
-            "heading": -0.5565822720790902,
-            "angularVelocity": -1.8572919514963226,
-            "velocityX": 1.4966236957608356,
-            "velocityY": -2.5544394448365244,
-            "timestamp": 0.41181163327648596
-        },
-        {
-            "x": 4.73233691115851,
-            "y": 4.546498868370588,
-            "heading": -0.6490495118957932,
-            "angularVelocity": -0.8981508276588281,
-            "velocityX": 2.3067719111095206,
-            "velocityY": -2.4744965582091383,
-            "timestamp": 0.5147645415956075
-        },
-        {
-            "x": 5.049349589860016,
-            "y": 4.3202563768587865,
-            "heading": -0.6490493910522949,
-            "angularVelocity": 0.0000011737744981731646,
-            "velocityX": 3.079200810130272,
-            "velocityY": -2.1975337579636096,
-            "timestamp": 0.617717449914729
-        },
-        {
-            "x": 5.4092082222704905,
-            "y": 4.171311637049865,
-            "heading": -0.6490493274377749,
-            "angularVelocity": 6.178992034513071e-7,
-            "velocityX": 3.4953712166637025,
-            "velocityY": -1.446726879703489,
-            "timestamp": 0.7206703582338505
-        },
-        {
-            "x": 5.789554256648214,
-            "y": 4.087528164867666,
-            "heading": -0.6490492895940018,
-            "angularVelocity": 3.675833320529158e-7,
-            "velocityX": 3.6943690138288297,
-            "velocityY": -0.8138038405141147,
-            "timestamp": 0.823623266552972
-        },
-        {
-            "x": 6.141218018459777,
-            "y": 3.9201502365347105,
-            "heading": -0.6490493503243893,
-            "angularVelocity": -5.898851085689342e-7,
-            "velocityX": 3.4157729738097102,
-            "velocityY": -1.6257717345306835,
-            "timestamp": 0.9265761748720935
-        },
-        {
-            "x": 6.456138339829479,
-            "y": 3.69100407104571,
-            "heading": -0.6490494074378677,
-            "angularVelocity": -5.547534235510974e-7,
-            "velocityX": 3.0588773693847333,
-            "velocityY": -2.2257376622981755,
-            "timestamp": 1.029529083191215
-        },
-        {
-            "x": 6.721775326533507,
-            "y": 3.406188756476768,
-            "heading": -0.6490494689481024,
-            "angularVelocity": -5.974599048336077e-7,
-            "velocityX": 2.580179531020508,
-            "velocityY": -2.7664620574496457,
-            "timestamp": 1.1324819915103363
-        },
-        {
-            "x": 6.916359430113237,
-            "y": 3.0688168532177102,
-            "heading": -0.6490494077682868,
-            "angularVelocity": 5.942504852606566e-7,
-            "velocityX": 1.8900301774533879,
-            "velocityY": -3.276953597204961,
-            "timestamp": 1.2354348998294578
-        },
-        {
-            "x": 7.040594979350531,
-            "y": 2.699698573462511,
-            "heading": -0.6490493528706274,
-            "angularVelocity": 5.332307776860117e-7,
-            "velocityX": 1.206722095234099,
-            "velocityY": -3.585311826364818,
-            "timestamp": 1.3383878081485792
-        },
-        {
-            "x": 7.164812386541997,
-            "y": 2.330574188065911,
-            "heading": -0.649049297974002,
-            "angularVelocity": 5.332207349098274e-7,
-            "velocityX": 1.2065458783003051,
-            "velocityY": -3.585371131551044,
-            "timestamp": 1.4413407164677006
-        },
-        {
-            "x": 7.289029734328084,
-            "y": 1.9614497826782673,
-            "heading": -0.6490492430773671,
-            "angularVelocity": 5.332208261000816e-7,
-            "velocityX": 1.206545301285243,
-            "velocityY": -3.5853713257276327,
-            "timestamp": 1.544293624786822
-        },
-        {
-            "x": 7.413246520819429,
-            "y": 1.5923251884092744,
-            "heading": -0.6490491881699392,
-            "angularVelocity": 5.333256623024379e-7,
-            "velocityX": 1.2065398493290926,
-            "velocityY": -3.585373160365936,
-            "timestamp": 1.6472465331059434
-        },
-        {
-            "x": 7.520074458284658,
-            "y": 1.2704783284902474,
-            "heading": -0.5256216946651037,
-            "angularVelocity": 1.1988733054752465,
-            "velocityX": 1.037638850707321,
-            "velocityY": -3.1261560763432104,
-            "timestamp": 1.7501994414250648
-        },
-        {
-            "x": 7.601104601009551,
-            "y": 1.0271731610238788,
-            "heading": -0.2913266412644364,
-            "angularVelocity": 2.275749730881093,
-            "velocityX": 0.7870602593733868,
-            "velocityY": -2.3632665792422265,
-            "timestamp": 1.8531523497441862
-        },
-        {
-            "x": 7.654009357663848,
-            "y": 0.8666565360101991,
-            "heading": -0.10310334462657153,
-            "angularVelocity": 1.828246522715339,
-            "velocityX": 0.5138733574219042,
-            "velocityY": -1.5591266690216163,
-            "timestamp": 1.9561052580633076
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -1.429149737287701e-32,
-            "angularVelocity": 1.0014612147428026,
-            "velocityX": 0.2524517545010862,
-            "velocityY": -0.7737181718391956,
-            "timestamp": 2.059058166382429
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -8.032741852653566e-32,
-            "velocityX": 0,
-            "velocityY": 2.733638748586157e-32,
-            "timestamp": 2.1620110747015504
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj
deleted file mode 100644
index c913a793..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 8.330960047205791e-35,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.215224456008129,
-            "y": 5.5891314798928855,
-            "heading": 0.013109691961439362,
-            "angularVelocity": 0.15985781811927008,
-            "velocityX": -0.5825685488618567,
-            "velocityY": 0.35522534228568337,
-            "timestamp": 0.08200845048227925
-        },
-        {
-            "x": 4.11967337131145,
-            "y": 5.647394543004508,
-            "heading": 0.03932481805987993,
-            "angularVelocity": 0.31966371690079975,
-            "velocityX": -1.1651370576417237,
-            "velocityY": 0.7104519445128596,
-            "timestamp": 0.1640169009645585
-        },
-        {
-            "x": 3.976346757700282,
-            "y": 5.734789362523958,
-            "heading": 0.07863903763442207,
-            "angularVelocity": 0.47939229851730114,
-            "velocityX": -1.7477054226519961,
-            "velocityY": 1.0656806585844962,
-            "timestamp": 0.24602535144683776
-        },
-        {
-            "x": 3.7852446629452063,
-            "y": 5.851316183985278,
-            "heading": 0.13104540832787342,
-            "angularVelocity": 0.6390362259652201,
-            "velocityX": -2.3302732051542674,
-            "velocityY": 1.4209123666652002,
-            "timestamp": 0.328033801929117
-        },
-        {
-            "x": 3.5463672091086966,
-            "y": 5.996975313640393,
-            "heading": 0.19653856458379848,
-            "angularVelocity": 0.7986147265406159,
-            "velocityX": -2.912839499243156,
-            "velocityY": 1.77614780914058,
-            "timestamp": 0.41004225241139625
-        },
-        {
-            "x": 3.281494446824011,
-            "y": 6.15848998396632,
-            "heading": 0.19653857286419524,
-            "angularVelocity": 1.009700426817188e-7,
-            "velocityX": -3.229822789322431,
-            "velocityY": 1.9694881366015824,
-            "timestamp": 0.4920507028936755
-        },
-        {
-            "x": 3.016621684855716,
-            "y": 6.320004654811106,
-            "heading": 0.1965385811445921,
-            "angularVelocity": 1.0097004416983622e-7,
-            "velocityX": -3.229822785464406,
-            "velocityY": 1.969488142928474,
-            "timestamp": 0.5740591533759547
-        },
-        {
-            "x": 2.77774755563319,
-            "y": 6.465669559960414,
-            "heading": 0.2620174097526663,
-            "angularVelocity": 0.7984400171324189,
-            "velocityX": -2.9127989593480157,
-            "velocityY": 1.7762182347389206,
-            "timestamp": 0.6560676038582339
-        },
-        {
-            "x": 2.5866484397575076,
-            "y": 6.582201656937504,
-            "heading": 0.3144069758542471,
-            "angularVelocity": 0.6388313130352502,
-            "velocityX": -2.330236881100143,
-            "velocityY": 1.4209766955939638,
-            "timestamp": 0.7380760543405132
-        },
-        {
-            "x": 2.443324199843316,
-            "y": 6.6696008076986475,
-            "heading": 0.35370275100271265,
-            "angularVelocity": 0.4791673896698825,
-            "velocityX": -1.7476764781107736,
-            "velocityY": 1.0657334731623573,
-            "timestamp": 0.8200845048227924
-        },
-        {
-            "x": 2.347774735979553,
-            "y": 6.727866934294423,
-            "heading": 0.37990105353306003,
-            "angularVelocity": 0.3194585725773273,
-            "velocityX": -1.1651172934234333,
-            "velocityY": 0.7104893002260324,
-            "timestamp": 0.9020929553050716
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.15972678900658524,
-            "velocityX": -0.5825586960684825,
-            "velocityY": 0.3552446794720561,
-            "timestamp": 0.9841014057873508
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": -4.59054414969708e-34,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 1.06610985626963
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.traj
deleted file mode 100644
index c913a793..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 8.330960047205791e-35,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.215224456008129,
-            "y": 5.5891314798928855,
-            "heading": 0.013109691961439362,
-            "angularVelocity": 0.15985781811927008,
-            "velocityX": -0.5825685488618567,
-            "velocityY": 0.35522534228568337,
-            "timestamp": 0.08200845048227925
-        },
-        {
-            "x": 4.11967337131145,
-            "y": 5.647394543004508,
-            "heading": 0.03932481805987993,
-            "angularVelocity": 0.31966371690079975,
-            "velocityX": -1.1651370576417237,
-            "velocityY": 0.7104519445128596,
-            "timestamp": 0.1640169009645585
-        },
-        {
-            "x": 3.976346757700282,
-            "y": 5.734789362523958,
-            "heading": 0.07863903763442207,
-            "angularVelocity": 0.47939229851730114,
-            "velocityX": -1.7477054226519961,
-            "velocityY": 1.0656806585844962,
-            "timestamp": 0.24602535144683776
-        },
-        {
-            "x": 3.7852446629452063,
-            "y": 5.851316183985278,
-            "heading": 0.13104540832787342,
-            "angularVelocity": 0.6390362259652201,
-            "velocityX": -2.3302732051542674,
-            "velocityY": 1.4209123666652002,
-            "timestamp": 0.328033801929117
-        },
-        {
-            "x": 3.5463672091086966,
-            "y": 5.996975313640393,
-            "heading": 0.19653856458379848,
-            "angularVelocity": 0.7986147265406159,
-            "velocityX": -2.912839499243156,
-            "velocityY": 1.77614780914058,
-            "timestamp": 0.41004225241139625
-        },
-        {
-            "x": 3.281494446824011,
-            "y": 6.15848998396632,
-            "heading": 0.19653857286419524,
-            "angularVelocity": 1.009700426817188e-7,
-            "velocityX": -3.229822789322431,
-            "velocityY": 1.9694881366015824,
-            "timestamp": 0.4920507028936755
-        },
-        {
-            "x": 3.016621684855716,
-            "y": 6.320004654811106,
-            "heading": 0.1965385811445921,
-            "angularVelocity": 1.0097004416983622e-7,
-            "velocityX": -3.229822785464406,
-            "velocityY": 1.969488142928474,
-            "timestamp": 0.5740591533759547
-        },
-        {
-            "x": 2.77774755563319,
-            "y": 6.465669559960414,
-            "heading": 0.2620174097526663,
-            "angularVelocity": 0.7984400171324189,
-            "velocityX": -2.9127989593480157,
-            "velocityY": 1.7762182347389206,
-            "timestamp": 0.6560676038582339
-        },
-        {
-            "x": 2.5866484397575076,
-            "y": 6.582201656937504,
-            "heading": 0.3144069758542471,
-            "angularVelocity": 0.6388313130352502,
-            "velocityX": -2.330236881100143,
-            "velocityY": 1.4209766955939638,
-            "timestamp": 0.7380760543405132
-        },
-        {
-            "x": 2.443324199843316,
-            "y": 6.6696008076986475,
-            "heading": 0.35370275100271265,
-            "angularVelocity": 0.4791673896698825,
-            "velocityX": -1.7476764781107736,
-            "velocityY": 1.0657334731623573,
-            "timestamp": 0.8200845048227924
-        },
-        {
-            "x": 2.347774735979553,
-            "y": 6.727866934294423,
-            "heading": 0.37990105353306003,
-            "angularVelocity": 0.3194585725773273,
-            "velocityX": -1.1651172934234333,
-            "velocityY": 0.7104893002260324,
-            "timestamp": 0.9020929553050716
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.15972678900658524,
-            "velocityX": -0.5825586960684825,
-            "velocityY": 0.3552446794720561,
-            "timestamp": 0.9841014057873508
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": -4.59054414969708e-34,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 1.06610985626963
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj
deleted file mode 100644
index a4697851..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.1.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.181096344253034,
-            "y": 5.56,
-            "heading": 2.5710705899848466e-19,
-            "angularVelocity": 2.5941602968880693e-18,
-            "velocityX": -0.8263919813283321,
-            "velocityY": -1.4479549544654444e-34,
-            "timestamp": 0.099109935233537
-        },
-        {
-            "x": 4.017289035836238,
-            "y": 5.56,
-            "heading": 7.7182898862159e-19,
-            "angularVelocity": 5.193444314378615e-18,
-            "velocityX": -1.6527839316089838,
-            "velocityY": -7.36495654370163e-34,
-            "timestamp": 0.198219870467074
-        },
-        {
-            "x": 3.771578080903875,
-            "y": 5.56,
-            "heading": 1.545129672215716e-18,
-            "angularVelocity": 7.802453727507411e-18,
-            "velocityX": -2.479175819794286,
-            "velocityY": -8.54847043233614e-34,
-            "timestamp": 0.297329805700611
-        },
-        {
-            "x": 3.4439634979187304,
-            "y": 5.56,
-            "heading": 2.5806642382479214e-18,
-            "angularVelocity": 1.0448342676399075e-17,
-            "velocityX": -3.3055675216936846,
-            "velocityY": -5.0910568375180236e-34,
-            "timestamp": 0.396439740934148
-        },
-        {
-            "x": 3.0690365020812695,
-            "y": 5.56,
-            "heading": -2.5850068638158606e-18,
-            "angularVelocity": -5.212061828001078e-17,
-            "velocityX": -3.7829405796099502,
-            "velocityY": -4.536187828596351e-34,
-            "timestamp": 0.495549676167685
-        },
-        {
-            "x": 2.741421919096125,
-            "y": 5.56,
-            "heading": -1.5478395588580842e-18,
-            "angularVelocity": 1.0464816695997814e-17,
-            "velocityX": -3.305567521693684,
-            "velocityY": -2.872506388563931e-33,
-            "timestamp": 0.594659611401222
-        },
-        {
-            "x": 2.4957109641637625,
-            "y": 5.56,
-            "heading": -7.728706253244835e-19,
-            "angularVelocity": 7.819286045545264e-18,
-            "velocityX": -2.479175819794286,
-            "velocityY": -1.6595590840772702e-33,
-            "timestamp": 0.6937695466347591
-        },
-        {
-            "x": 2.3319036557469652,
-            "y": 5.56,
-            "heading": -2.5738443375571516e-19,
-            "angularVelocity": 5.201155568994596e-18,
-            "velocityX": -1.6527839316089838,
-            "velocityY": -1.4347514653062086e-33,
-            "timestamp": 0.792879481868296
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 2.5969589542999405e-18,
-            "velocityX": -0.8263919813283321,
-            "velocityY": 2.9921012122849815e-33,
-            "timestamp": 0.891989417101833
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 1.808288986442481e-43,
-            "timestamp": 0.9910993523353699
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.traj
deleted file mode 100644
index a4697851..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.181096344253034,
-            "y": 5.56,
-            "heading": 2.5710705899848466e-19,
-            "angularVelocity": 2.5941602968880693e-18,
-            "velocityX": -0.8263919813283321,
-            "velocityY": -1.4479549544654444e-34,
-            "timestamp": 0.099109935233537
-        },
-        {
-            "x": 4.017289035836238,
-            "y": 5.56,
-            "heading": 7.7182898862159e-19,
-            "angularVelocity": 5.193444314378615e-18,
-            "velocityX": -1.6527839316089838,
-            "velocityY": -7.36495654370163e-34,
-            "timestamp": 0.198219870467074
-        },
-        {
-            "x": 3.771578080903875,
-            "y": 5.56,
-            "heading": 1.545129672215716e-18,
-            "angularVelocity": 7.802453727507411e-18,
-            "velocityX": -2.479175819794286,
-            "velocityY": -8.54847043233614e-34,
-            "timestamp": 0.297329805700611
-        },
-        {
-            "x": 3.4439634979187304,
-            "y": 5.56,
-            "heading": 2.5806642382479214e-18,
-            "angularVelocity": 1.0448342676399075e-17,
-            "velocityX": -3.3055675216936846,
-            "velocityY": -5.0910568375180236e-34,
-            "timestamp": 0.396439740934148
-        },
-        {
-            "x": 3.0690365020812695,
-            "y": 5.56,
-            "heading": -2.5850068638158606e-18,
-            "angularVelocity": -5.212061828001078e-17,
-            "velocityX": -3.7829405796099502,
-            "velocityY": -4.536187828596351e-34,
-            "timestamp": 0.495549676167685
-        },
-        {
-            "x": 2.741421919096125,
-            "y": 5.56,
-            "heading": -1.5478395588580842e-18,
-            "angularVelocity": 1.0464816695997814e-17,
-            "velocityX": -3.305567521693684,
-            "velocityY": -2.872506388563931e-33,
-            "timestamp": 0.594659611401222
-        },
-        {
-            "x": 2.4957109641637625,
-            "y": 5.56,
-            "heading": -7.728706253244835e-19,
-            "angularVelocity": 7.819286045545264e-18,
-            "velocityX": -2.479175819794286,
-            "velocityY": -1.6595590840772702e-33,
-            "timestamp": 0.6937695466347591
-        },
-        {
-            "x": 2.3319036557469652,
-            "y": 5.56,
-            "heading": -2.5738443375571516e-19,
-            "angularVelocity": 5.201155568994596e-18,
-            "velocityX": -1.6527839316089838,
-            "velocityY": -1.4347514653062086e-33,
-            "timestamp": 0.792879481868296
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 2.5969589542999405e-18,
-            "velocityX": -0.8263919813283321,
-            "velocityY": 2.9921012122849815e-33,
-            "timestamp": 0.891989417101833
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 1.808288986442481e-43,
-            "timestamp": 0.9910993523353699
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj
deleted file mode 100644
index 4a59e123..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 9.014129666558106e-32,
-            "angularVelocity": 9.039417129994225e-31,
-            "velocityX": 0,
-            "velocityY": -2.779554313855136e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 4.206523478699805,
-            "y": 5.55440930401173,
-            "heading": -0.10760119525493232,
-            "angularVelocity": -1.2344158099898592,
-            "velocityX": -0.6479064718660169,
-            "velocityY": -0.06413723844253004,
-            "timestamp": 0.08716770668695249
-        },
-        {
-            "x": 4.091196989898444,
-            "y": 5.541197271810027,
-            "heading": -0.295913717265277,
-            "angularVelocity": -2.1603473254910437,
-            "velocityX": -1.3230414471673093,
-            "velocityY": -0.15157026270235532,
-            "timestamp": 0.17433541337390498
-        },
-        {
-            "x": 3.9151950960136452,
-            "y": 5.516370757100726,
-            "heading": -0.5340653762567812,
-            "angularVelocity": -2.7321088054637612,
-            "velocityX": -2.019118095155108,
-            "velocityY": -0.2848132141236808,
-            "timestamp": 0.26150312006085746
-        },
-        {
-            "x": 3.678758800597895,
-            "y": 5.473177706772957,
-            "heading": -0.7546654655667131,
-            "angularVelocity": -2.530754767957573,
-            "velocityX": -2.712429917020411,
-            "velocityY": -0.4955166536948123,
-            "timestamp": 0.34867082674780997
-        },
-        {
-            "x": 3.3968844521889907,
-            "y": 5.398848640381046,
-            "heading": -0.8527884173152881,
-            "angularVelocity": -1.1256800881657552,
-            "velocityX": -3.233701552126478,
-            "velocityY": -0.8527133409492049,
-            "timestamp": 0.4358385334347625
-        },
-        {
-            "x": 3.098615494079836,
-            "y": 5.268356714792353,
-            "heading": -0.864385170870599,
-            "angularVelocity": -0.1330395624260059,
-            "velocityX": -3.421782784538955,
-            "velocityY": -1.4970214377364781,
-            "timestamp": 0.5230062401217149
-        },
-        {
-            "x": 2.832096469446971,
-            "y": 5.0836195976119996,
-            "heading": -0.8474300397750907,
-            "angularVelocity": 0.19451161146636953,
-            "velocityX": -3.0575431517318843,
-            "velocityY": -2.1193297862453186,
-            "timestamp": 0.6101739468086674
-        },
-        {
-            "x": 2.620164345186853,
-            "y": 4.879059766080367,
-            "heading": -0.7372424721392209,
-            "angularVelocity": 1.2640870320425897,
-            "velocityX": -2.431314672774813,
-            "velocityY": -2.346738709855847,
-            "timestamp": 0.6973416534956199
-        },
-        {
-            "x": 2.469226841034766,
-            "y": 4.6911608685478585,
-            "heading": -0.6128307853924054,
-            "angularVelocity": 1.427268095897297,
-            "velocityX": -1.7315759458276552,
-            "velocityY": -2.155602168212534,
-            "timestamp": 0.7845093601825724
-        },
-        {
-            "x": 2.3748885049182054,
-            "y": 4.5313089505917965,
-            "heading": -0.5057978702393489,
-            "angularVelocity": 1.2278964219794164,
-            "velocityX": -1.0822624536327627,
-            "velocityY": -1.8338433352404204,
-            "timestamp": 0.871677066869525
-        },
-        {
-            "x": 2.3223904484665,
-            "y": 4.418253055823363,
-            "heading": -0.42949016513376354,
-            "angularVelocity": 0.87541255822675,
-            "velocityX": -0.6022649722820163,
-            "velocityY": -1.2969928780443263,
-            "timestamp": 0.9588447735564775
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0.4530366420626772,
-            "velocityX": -0.256866324898396,
-            "velocityY": -0.6682871218875669,
-            "timestamp": 1.0460124802434299
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -3.4100744325418914e-31,
-            "velocityX": -9.400788860397695e-33,
-            "velocityY": 0,
-            "timestamp": 1.1331801869303824
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.traj
deleted file mode 100644
index 4a59e123..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 9.014129666558106e-32,
-            "angularVelocity": 9.039417129994225e-31,
-            "velocityX": 0,
-            "velocityY": -2.779554313855136e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 4.206523478699805,
-            "y": 5.55440930401173,
-            "heading": -0.10760119525493232,
-            "angularVelocity": -1.2344158099898592,
-            "velocityX": -0.6479064718660169,
-            "velocityY": -0.06413723844253004,
-            "timestamp": 0.08716770668695249
-        },
-        {
-            "x": 4.091196989898444,
-            "y": 5.541197271810027,
-            "heading": -0.295913717265277,
-            "angularVelocity": -2.1603473254910437,
-            "velocityX": -1.3230414471673093,
-            "velocityY": -0.15157026270235532,
-            "timestamp": 0.17433541337390498
-        },
-        {
-            "x": 3.9151950960136452,
-            "y": 5.516370757100726,
-            "heading": -0.5340653762567812,
-            "angularVelocity": -2.7321088054637612,
-            "velocityX": -2.019118095155108,
-            "velocityY": -0.2848132141236808,
-            "timestamp": 0.26150312006085746
-        },
-        {
-            "x": 3.678758800597895,
-            "y": 5.473177706772957,
-            "heading": -0.7546654655667131,
-            "angularVelocity": -2.530754767957573,
-            "velocityX": -2.712429917020411,
-            "velocityY": -0.4955166536948123,
-            "timestamp": 0.34867082674780997
-        },
-        {
-            "x": 3.3968844521889907,
-            "y": 5.398848640381046,
-            "heading": -0.8527884173152881,
-            "angularVelocity": -1.1256800881657552,
-            "velocityX": -3.233701552126478,
-            "velocityY": -0.8527133409492049,
-            "timestamp": 0.4358385334347625
-        },
-        {
-            "x": 3.098615494079836,
-            "y": 5.268356714792353,
-            "heading": -0.864385170870599,
-            "angularVelocity": -0.1330395624260059,
-            "velocityX": -3.421782784538955,
-            "velocityY": -1.4970214377364781,
-            "timestamp": 0.5230062401217149
-        },
-        {
-            "x": 2.832096469446971,
-            "y": 5.0836195976119996,
-            "heading": -0.8474300397750907,
-            "angularVelocity": 0.19451161146636953,
-            "velocityX": -3.0575431517318843,
-            "velocityY": -2.1193297862453186,
-            "timestamp": 0.6101739468086674
-        },
-        {
-            "x": 2.620164345186853,
-            "y": 4.879059766080367,
-            "heading": -0.7372424721392209,
-            "angularVelocity": 1.2640870320425897,
-            "velocityX": -2.431314672774813,
-            "velocityY": -2.346738709855847,
-            "timestamp": 0.6973416534956199
-        },
-        {
-            "x": 2.469226841034766,
-            "y": 4.6911608685478585,
-            "heading": -0.6128307853924054,
-            "angularVelocity": 1.427268095897297,
-            "velocityX": -1.7315759458276552,
-            "velocityY": -2.155602168212534,
-            "timestamp": 0.7845093601825724
-        },
-        {
-            "x": 2.3748885049182054,
-            "y": 4.5313089505917965,
-            "heading": -0.5057978702393489,
-            "angularVelocity": 1.2278964219794164,
-            "velocityX": -1.0822624536327627,
-            "velocityY": -1.8338433352404204,
-            "timestamp": 0.871677066869525
-        },
-        {
-            "x": 2.3223904484665,
-            "y": 4.418253055823363,
-            "heading": -0.42949016513376354,
-            "angularVelocity": 0.87541255822675,
-            "velocityX": -0.6022649722820163,
-            "velocityY": -1.2969928780443263,
-            "timestamp": 0.9588447735564775
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0.4530366420626772,
-            "velocityX": -0.256866324898396,
-            "velocityY": -0.6682871218875669,
-            "timestamp": 1.0460124802434299
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -3.4100744325418914e-31,
-            "velocityX": -9.400788860397695e-33,
-            "velocityY": 0,
-            "timestamp": 1.1331801869303824
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C1.1.traj
deleted file mode 100644
index 97fb7eaf..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C1.1.traj	
+++ /dev/null
@@ -1,220 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 3.2688003829398086e-32,
-            "velocityX": -2.078713781970587e-32,
-            "velocityY": -4.638967376158192e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 4.29154268151456,
-            "y": 3.0623077457400183,
-            "heading": -0.5127665183876435,
-            "angularVelocity": 0.580669777278592,
-            "velocityX": 0.31134301225465,
-            "velocityY": 0.6796516730776717,
-            "timestamp": 0.09167599846825866
-        },
-        {
-            "x": 4.3492977760941525,
-            "y": 3.186661868715021,
-            "heading": -0.4070750475472326,
-            "angularVelocity": 1.1528804987818582,
-            "velocityX": 0.6299914431757085,
-            "velocityY": 1.3564523436094174,
-            "timestamp": 0.18335199693651733
-        },
-        {
-            "x": 4.438053987492854,
-            "y": 3.3723523918382017,
-            "heading": -0.25179906449139605,
-            "angularVelocity": 1.6937473891773143,
-            "velocityX": 0.968151019696088,
-            "velocityY": 2.025508597950785,
-            "timestamp": 0.27502799540477596
-        },
-        {
-            "x": 4.5709698511757795,
-            "y": 3.6117104026755453,
-            "heading": -0.06452556421817643,
-            "angularVelocity": 2.042775681773022,
-            "velocityX": 1.449843643960376,
-            "velocityY": 2.6109125053076734,
-            "timestamp": 0.36670399387303465
-        },
-        {
-            "x": 4.761013106791701,
-            "y": 3.810602792551275,
-            "heading": 0.11153690871840848,
-            "angularVelocity": 1.9204860146414893,
-            "velocityX": 2.0729881189319217,
-            "velocityY": 2.1695143025313537,
-            "timestamp": 0.45837999234129334
-        },
-        {
-            "x": 5.007328053154023,
-            "y": 3.968591564774131,
-            "heading": 0.25752334504560864,
-            "angularVelocity": 1.5924171949733021,
-            "velocityX": 2.6867986220799525,
-            "velocityY": 1.7233384404049508,
-            "timestamp": 0.550055990809552
-        },
-        {
-            "x": 5.305296422655045,
-            "y": 4.081628168088871,
-            "heading": 0.35020186087766364,
-            "angularVelocity": 1.0109354398157564,
-            "velocityX": 3.2502331524012615,
-            "velocityY": 1.2330010602925316,
-            "timestamp": 0.6417319892778107
-        },
-        {
-            "x": 5.6446965706540295,
-            "y": 4.152909996485894,
-            "heading": 0.3502021264888923,
-            "angularVelocity": 0.0000028972820926514052,
-            "velocityX": 3.702170182705961,
-            "velocityY": 0.7775407913523397,
-            "timestamp": 0.7334079877460694
-        },
-        {
-            "x": 5.981889969698284,
-            "y": 4.233991234618027,
-            "heading": 0.3502024148432213,
-            "angularVelocity": 0.0000031453633856654694,
-            "velocityX": 3.6780990082262504,
-            "velocityY": 0.884432561268565,
-            "timestamp": 0.8250839862143281
-        },
-        {
-            "x": 6.295899946950055,
-            "y": 4.381202864314355,
-            "heading": 0.350202827556894,
-            "angularVelocity": 0.000004501872678764248,
-            "velocityX": 3.425214696303447,
-            "velocityY": 1.6057815803042044,
-            "timestamp": 0.9167599846825868
-        },
-        {
-            "x": 6.57390491063703,
-            "y": 4.588535293884794,
-            "heading": 0.350203324900352,
-            "angularVelocity": 0.0000054250127210279085,
-            "velocityX": 3.0324727118541346,
-            "velocityY": 2.2615780906081357,
-            "timestamp": 1.0084359831508454
-        },
-        {
-            "x": 6.804553755441848,
-            "y": 4.847523149382792,
-            "heading": 0.35020327996165895,
-            "angularVelocity": -4.901903857536901e-7,
-            "velocityX": 2.5159130923965525,
-            "velocityY": 2.825034467311176,
-            "timestamp": 1.100111981619104
-        },
-        {
-            "x": 6.978428828519116,
-            "y": 5.147591577808381,
-            "heading": 0.35020324373344786,
-            "angularVelocity": -3.9517661894346874e-7,
-            "velocityX": 1.8966258997164847,
-            "velocityY": 3.273140554116584,
-            "timestamp": 1.1917879800873628
-        },
-        {
-            "x": 7.088431556948234,
-            "y": 5.476488224562697,
-            "heading": 0.3502032113343976,
-            "angularVelocity": -3.5340820734555857e-7,
-            "velocityX": 1.1999076123202526,
-            "velocityY": 3.587598196360961,
-            "timestamp": 1.2834639785556214
-        },
-        {
-            "x": 7.187536215850895,
-            "y": 5.808831226900728,
-            "heading": 0.3502031791295722,
-            "angularVelocity": -3.5128960565806065e-7,
-            "velocityX": 1.0810316828670656,
-            "velocityY": 3.6251909757285055,
-            "timestamp": 1.3751399770238801
-        },
-        {
-            "x": 7.286640767886556,
-            "y": 6.141174261106461,
-            "heading": 0.3502031469247365,
-            "angularVelocity": -3.512897194087527e-7,
-            "velocityX": 1.0810305171639119,
-            "velocityY": 3.6251913233407715,
-            "timestamp": 1.4668159754921388
-        },
-        {
-            "x": 7.3857452620780215,
-            "y": 6.473517312561292,
-            "heading": 0.350203114719879,
-            "angularVelocity": -3.512899557892109e-7,
-            "velocityX": 1.0810298862005645,
-            "velocityY": 3.625191511493593,
-            "timestamp": 1.5584919739603975
-        },
-        {
-            "x": 7.481976228665169,
-            "y": 6.80241098377653,
-            "heading": 0.33998449018149474,
-            "angularVelocity": -0.11146455679915658,
-            "velocityX": 1.0496855032395944,
-            "velocityY": 3.5875657392388547,
-            "timestamp": 1.6501679724286562
-        },
-        {
-            "x": 7.561455250393006,
-            "y": 7.068914029375232,
-            "heading": 0.2316909043200923,
-            "angularVelocity": -1.1812643185871456,
-            "velocityX": 0.8669556160367877,
-            "velocityY": 2.907010014087551,
-            "timestamp": 1.741843970896915
-        },
-        {
-            "x": 7.620860593043084,
-            "y": 7.268277067392774,
-            "heading": 0.12481105079738766,
-            "angularVelocity": -1.1658433538600752,
-            "velocityX": 0.6479923168837385,
-            "velocityY": 2.1746481232660675,
-            "timestamp": 1.8335199693651736
-        },
-        {
-            "x": 7.66032628893014,
-            "y": 7.4008489817464715,
-            "heading": 0.04379045785830094,
-            "angularVelocity": -0.8837710446877722,
-            "velocityX": 0.430491039600946,
-            "velocityY": 1.4460918513977026,
-            "timestamp": 1.9251959678334323
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 6.52409657127002e-31,
-            "angularVelocity": -0.4776654586801488,
-            "velocityX": 0.21460045593800534,
-            "velocityY": 0.7215740145599008,
-            "timestamp": 2.0168719663016907
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": -6.745238901014272e-30,
-            "velocityX": 0,
-            "velocityY": 3.373152313643601e-31,
-            "timestamp": 2.1085479647699494
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C1.traj
deleted file mode 100644
index 97fb7eaf..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C1.traj	
+++ /dev/null
@@ -1,220 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 3.2688003829398086e-32,
-            "velocityX": -2.078713781970587e-32,
-            "velocityY": -4.638967376158192e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 4.29154268151456,
-            "y": 3.0623077457400183,
-            "heading": -0.5127665183876435,
-            "angularVelocity": 0.580669777278592,
-            "velocityX": 0.31134301225465,
-            "velocityY": 0.6796516730776717,
-            "timestamp": 0.09167599846825866
-        },
-        {
-            "x": 4.3492977760941525,
-            "y": 3.186661868715021,
-            "heading": -0.4070750475472326,
-            "angularVelocity": 1.1528804987818582,
-            "velocityX": 0.6299914431757085,
-            "velocityY": 1.3564523436094174,
-            "timestamp": 0.18335199693651733
-        },
-        {
-            "x": 4.438053987492854,
-            "y": 3.3723523918382017,
-            "heading": -0.25179906449139605,
-            "angularVelocity": 1.6937473891773143,
-            "velocityX": 0.968151019696088,
-            "velocityY": 2.025508597950785,
-            "timestamp": 0.27502799540477596
-        },
-        {
-            "x": 4.5709698511757795,
-            "y": 3.6117104026755453,
-            "heading": -0.06452556421817643,
-            "angularVelocity": 2.042775681773022,
-            "velocityX": 1.449843643960376,
-            "velocityY": 2.6109125053076734,
-            "timestamp": 0.36670399387303465
-        },
-        {
-            "x": 4.761013106791701,
-            "y": 3.810602792551275,
-            "heading": 0.11153690871840848,
-            "angularVelocity": 1.9204860146414893,
-            "velocityX": 2.0729881189319217,
-            "velocityY": 2.1695143025313537,
-            "timestamp": 0.45837999234129334
-        },
-        {
-            "x": 5.007328053154023,
-            "y": 3.968591564774131,
-            "heading": 0.25752334504560864,
-            "angularVelocity": 1.5924171949733021,
-            "velocityX": 2.6867986220799525,
-            "velocityY": 1.7233384404049508,
-            "timestamp": 0.550055990809552
-        },
-        {
-            "x": 5.305296422655045,
-            "y": 4.081628168088871,
-            "heading": 0.35020186087766364,
-            "angularVelocity": 1.0109354398157564,
-            "velocityX": 3.2502331524012615,
-            "velocityY": 1.2330010602925316,
-            "timestamp": 0.6417319892778107
-        },
-        {
-            "x": 5.6446965706540295,
-            "y": 4.152909996485894,
-            "heading": 0.3502021264888923,
-            "angularVelocity": 0.0000028972820926514052,
-            "velocityX": 3.702170182705961,
-            "velocityY": 0.7775407913523397,
-            "timestamp": 0.7334079877460694
-        },
-        {
-            "x": 5.981889969698284,
-            "y": 4.233991234618027,
-            "heading": 0.3502024148432213,
-            "angularVelocity": 0.0000031453633856654694,
-            "velocityX": 3.6780990082262504,
-            "velocityY": 0.884432561268565,
-            "timestamp": 0.8250839862143281
-        },
-        {
-            "x": 6.295899946950055,
-            "y": 4.381202864314355,
-            "heading": 0.350202827556894,
-            "angularVelocity": 0.000004501872678764248,
-            "velocityX": 3.425214696303447,
-            "velocityY": 1.6057815803042044,
-            "timestamp": 0.9167599846825868
-        },
-        {
-            "x": 6.57390491063703,
-            "y": 4.588535293884794,
-            "heading": 0.350203324900352,
-            "angularVelocity": 0.0000054250127210279085,
-            "velocityX": 3.0324727118541346,
-            "velocityY": 2.2615780906081357,
-            "timestamp": 1.0084359831508454
-        },
-        {
-            "x": 6.804553755441848,
-            "y": 4.847523149382792,
-            "heading": 0.35020327996165895,
-            "angularVelocity": -4.901903857536901e-7,
-            "velocityX": 2.5159130923965525,
-            "velocityY": 2.825034467311176,
-            "timestamp": 1.100111981619104
-        },
-        {
-            "x": 6.978428828519116,
-            "y": 5.147591577808381,
-            "heading": 0.35020324373344786,
-            "angularVelocity": -3.9517661894346874e-7,
-            "velocityX": 1.8966258997164847,
-            "velocityY": 3.273140554116584,
-            "timestamp": 1.1917879800873628
-        },
-        {
-            "x": 7.088431556948234,
-            "y": 5.476488224562697,
-            "heading": 0.3502032113343976,
-            "angularVelocity": -3.5340820734555857e-7,
-            "velocityX": 1.1999076123202526,
-            "velocityY": 3.587598196360961,
-            "timestamp": 1.2834639785556214
-        },
-        {
-            "x": 7.187536215850895,
-            "y": 5.808831226900728,
-            "heading": 0.3502031791295722,
-            "angularVelocity": -3.5128960565806065e-7,
-            "velocityX": 1.0810316828670656,
-            "velocityY": 3.6251909757285055,
-            "timestamp": 1.3751399770238801
-        },
-        {
-            "x": 7.286640767886556,
-            "y": 6.141174261106461,
-            "heading": 0.3502031469247365,
-            "angularVelocity": -3.512897194087527e-7,
-            "velocityX": 1.0810305171639119,
-            "velocityY": 3.6251913233407715,
-            "timestamp": 1.4668159754921388
-        },
-        {
-            "x": 7.3857452620780215,
-            "y": 6.473517312561292,
-            "heading": 0.350203114719879,
-            "angularVelocity": -3.512899557892109e-7,
-            "velocityX": 1.0810298862005645,
-            "velocityY": 3.625191511493593,
-            "timestamp": 1.5584919739603975
-        },
-        {
-            "x": 7.481976228665169,
-            "y": 6.80241098377653,
-            "heading": 0.33998449018149474,
-            "angularVelocity": -0.11146455679915658,
-            "velocityX": 1.0496855032395944,
-            "velocityY": 3.5875657392388547,
-            "timestamp": 1.6501679724286562
-        },
-        {
-            "x": 7.561455250393006,
-            "y": 7.068914029375232,
-            "heading": 0.2316909043200923,
-            "angularVelocity": -1.1812643185871456,
-            "velocityX": 0.8669556160367877,
-            "velocityY": 2.907010014087551,
-            "timestamp": 1.741843970896915
-        },
-        {
-            "x": 7.620860593043084,
-            "y": 7.268277067392774,
-            "heading": 0.12481105079738766,
-            "angularVelocity": -1.1658433538600752,
-            "velocityX": 0.6479923168837385,
-            "velocityY": 2.1746481232660675,
-            "timestamp": 1.8335199693651736
-        },
-        {
-            "x": 7.66032628893014,
-            "y": 7.4008489817464715,
-            "heading": 0.04379045785830094,
-            "angularVelocity": -0.8837710446877722,
-            "velocityX": 0.430491039600946,
-            "velocityY": 1.4460918513977026,
-            "timestamp": 1.9251959678334323
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 6.52409657127002e-31,
-            "angularVelocity": -0.4776654586801488,
-            "velocityX": 0.21460045593800534,
-            "velocityY": 0.7215740145599008,
-            "timestamp": 2.0168719663016907
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": -6.745238901014272e-30,
-            "velocityX": 0,
-            "velocityY": 3.373152313643601e-31,
-            "timestamp": 2.1085479647699494
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.1.traj
deleted file mode 100644
index 69777c27..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.1.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -2.8196964588027846e-31,
-            "velocityX": -1.564828148658428e-31,
-            "velocityY": 3.07478616058615e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 4.294807569207289,
-            "y": 3.0477321348748183,
-            "heading": -0.5960373953696199,
-            "angularVelocity": -0.36026847684168845,
-            "velocityX": 0.381499939303545,
-            "velocityY": 0.5724991570056517,
-            "timestamp": 0.08337503084628527
-        },
-        {
-            "x": 4.359939756522268,
-            "y": 3.142193816557181,
-            "heading": -0.6551934033575421,
-            "angularVelocity": -0.7095170746861289,
-            "velocityX": 0.7811953609355742,
-            "velocityY": 1.1329732741750633,
-            "timestamp": 0.16675006169257053
-        },
-        {
-            "x": 4.461050205756915,
-            "y": 3.2814565261988764,
-            "heading": -0.7417418377173162,
-            "angularVelocity": -1.038061797174498,
-            "velocityX": 1.2127185826300881,
-            "velocityY": 1.6703167390540123,
-            "timestamp": 0.2501250925388558
-        },
-        {
-            "x": 4.603825208189836,
-            "y": 3.4604941356958814,
-            "heading": -0.8514332359801334,
-            "angularVelocity": -1.315638472926628,
-            "velocityX": 1.7124431737380506,
-            "velocityY": 2.147376830685539,
-            "timestamp": 0.33350012338514107
-        },
-        {
-            "x": 4.8030511460470855,
-            "y": 3.6526461815261504,
-            "heading": -0.9644207302174959,
-            "angularVelocity": -1.3551718433024906,
-            "velocityX": 2.3895156119888465,
-            "velocityY": 2.3046713611960277,
-            "timestamp": 0.41687515423142635
-        },
-        {
-            "x": 5.051097476442208,
-            "y": 3.8467657037250893,
-            "heading": -0.9658198239131095,
-            "angularVelocity": -0.016780727772000164,
-            "velocityX": 2.975067329839247,
-            "velocityY": 2.328269269930804,
-            "timestamp": 0.5002501850777116
-        },
-        {
-            "x": 5.320526470824812,
-            "y": 4.010737925204951,
-            "heading": -0.965820109061322,
-            "angularVelocity": -0.000003420067250854742,
-            "velocityX": 3.2315309709370754,
-            "velocityY": 1.9666825884858703,
-            "timestamp": 0.5836252159239969
-        },
-        {
-            "x": 5.614243645591117,
-            "y": 4.1256688963704224,
-            "heading": -0.9658201941489747,
-            "angularVelocity": -0.000001020541184209393,
-            "velocityX": 3.5228433715103167,
-            "velocityY": 1.37848190278171,
-            "timestamp": 0.6670002467702821
-        },
-        {
-            "x": 5.906557788271019,
-            "y": 4.244122886725936,
-            "heading": -0.965820105112684,
-            "angularVelocity": 0.0000010679011417361832,
-            "velocityX": 3.506015406684877,
-            "velocityY": 1.4207369898777267,
-            "timestamp": 0.7503752776165674
-        },
-        {
-            "x": 6.172253111887642,
-            "y": 4.41406398245588,
-            "heading": -0.9657938677546691,
-            "angularVelocity": 0.0003146908342779248,
-            "velocityX": 3.1867493291423483,
-            "velocityY": 2.0382732576526004,
-            "timestamp": 0.8337503084628527
-        },
-        {
-            "x": 6.426785390452526,
-            "y": 4.5994304003649855,
-            "heading": -0.9640144900160125,
-            "angularVelocity": 0.021341854037057975,
-            "velocityX": 3.0528597828545716,
-            "velocityY": 2.2232845496735987,
-            "timestamp": 0.917125339309138
-        },
-        {
-            "x": 6.681598941757368,
-            "y": 4.785246665794811,
-            "heading": -0.9639002296367303,
-            "angularVelocity": 0.0013704388246982366,
-            "velocityX": 3.0562333676929114,
-            "velocityY": 2.2286800201898096,
-            "timestamp": 1.0005003701554231
-        },
-        {
-            "x": 6.9014822245071255,
-            "y": 5.01136660335564,
-            "heading": -0.9639000238890051,
-            "angularVelocity": 0.000002467737919950576,
-            "velocityX": 2.6372797769052228,
-            "velocityY": 2.7120822057351384,
-            "timestamp": 1.0838754010017084
-        },
-        {
-            "x": 7.122012116423131,
-            "y": 5.23107273201418,
-            "heading": -0.9515772318649645,
-            "angularVelocity": 0.1477995498047812,
-            "velocityX": 2.645035206314798,
-            "velocityY": 2.635154990990089,
-            "timestamp": 1.1672504318479937
-        },
-        {
-            "x": 7.311148990719613,
-            "y": 5.417840497645029,
-            "heading": -0.7917020160601519,
-            "angularVelocity": 1.9175431083146124,
-            "velocityX": 2.2685073981583956,
-            "velocityY": 2.2400923122317544,
-            "timestamp": 1.250625462694279
-        },
-        {
-            "x": 7.462007390054419,
-            "y": 5.572027096128993,
-            "heading": -0.5225549044290918,
-            "angularVelocity": 3.228150069620655,
-            "velocityX": 1.8093954245478587,
-            "velocityY": 1.8493138403538476,
-            "timestamp": 1.3340004935405643
-        },
-        {
-            "x": 7.571928087470151,
-            "y": 5.685474744918936,
-            "heading": -0.27088873612536774,
-            "angularVelocity": 3.018483660506339,
-            "velocityX": 1.318388686636741,
-            "velocityY": 1.3606909363440005,
-            "timestamp": 1.4173755243868496
-        },
-        {
-            "x": 7.6442805622690475,
-            "y": 5.7599789636443175,
-            "heading": -0.0924144798498188,
-            "angularVelocity": 2.1406199729580164,
-            "velocityX": 0.8677954786282445,
-            "velocityY": 0.8936034921863096,
-            "timestamp": 1.5007505552331348
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -2.5332548087748344e-31,
-            "angularVelocity": 1.1084191383412998,
-            "velocityX": 0.42841888474747514,
-            "velocityY": 0.4440302567795869,
-            "timestamp": 1.5841255860794201
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 5.737670210968143e-33,
-            "velocityX": 0,
-            "velocityY": -3.2710687085311585e-31,
-            "timestamp": 1.6675006169257054
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.traj
deleted file mode 100644
index 69777c27..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -2.8196964588027846e-31,
-            "velocityX": -1.564828148658428e-31,
-            "velocityY": 3.07478616058615e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 4.294807569207289,
-            "y": 3.0477321348748183,
-            "heading": -0.5960373953696199,
-            "angularVelocity": -0.36026847684168845,
-            "velocityX": 0.381499939303545,
-            "velocityY": 0.5724991570056517,
-            "timestamp": 0.08337503084628527
-        },
-        {
-            "x": 4.359939756522268,
-            "y": 3.142193816557181,
-            "heading": -0.6551934033575421,
-            "angularVelocity": -0.7095170746861289,
-            "velocityX": 0.7811953609355742,
-            "velocityY": 1.1329732741750633,
-            "timestamp": 0.16675006169257053
-        },
-        {
-            "x": 4.461050205756915,
-            "y": 3.2814565261988764,
-            "heading": -0.7417418377173162,
-            "angularVelocity": -1.038061797174498,
-            "velocityX": 1.2127185826300881,
-            "velocityY": 1.6703167390540123,
-            "timestamp": 0.2501250925388558
-        },
-        {
-            "x": 4.603825208189836,
-            "y": 3.4604941356958814,
-            "heading": -0.8514332359801334,
-            "angularVelocity": -1.315638472926628,
-            "velocityX": 1.7124431737380506,
-            "velocityY": 2.147376830685539,
-            "timestamp": 0.33350012338514107
-        },
-        {
-            "x": 4.8030511460470855,
-            "y": 3.6526461815261504,
-            "heading": -0.9644207302174959,
-            "angularVelocity": -1.3551718433024906,
-            "velocityX": 2.3895156119888465,
-            "velocityY": 2.3046713611960277,
-            "timestamp": 0.41687515423142635
-        },
-        {
-            "x": 5.051097476442208,
-            "y": 3.8467657037250893,
-            "heading": -0.9658198239131095,
-            "angularVelocity": -0.016780727772000164,
-            "velocityX": 2.975067329839247,
-            "velocityY": 2.328269269930804,
-            "timestamp": 0.5002501850777116
-        },
-        {
-            "x": 5.320526470824812,
-            "y": 4.010737925204951,
-            "heading": -0.965820109061322,
-            "angularVelocity": -0.000003420067250854742,
-            "velocityX": 3.2315309709370754,
-            "velocityY": 1.9666825884858703,
-            "timestamp": 0.5836252159239969
-        },
-        {
-            "x": 5.614243645591117,
-            "y": 4.1256688963704224,
-            "heading": -0.9658201941489747,
-            "angularVelocity": -0.000001020541184209393,
-            "velocityX": 3.5228433715103167,
-            "velocityY": 1.37848190278171,
-            "timestamp": 0.6670002467702821
-        },
-        {
-            "x": 5.906557788271019,
-            "y": 4.244122886725936,
-            "heading": -0.965820105112684,
-            "angularVelocity": 0.0000010679011417361832,
-            "velocityX": 3.506015406684877,
-            "velocityY": 1.4207369898777267,
-            "timestamp": 0.7503752776165674
-        },
-        {
-            "x": 6.172253111887642,
-            "y": 4.41406398245588,
-            "heading": -0.9657938677546691,
-            "angularVelocity": 0.0003146908342779248,
-            "velocityX": 3.1867493291423483,
-            "velocityY": 2.0382732576526004,
-            "timestamp": 0.8337503084628527
-        },
-        {
-            "x": 6.426785390452526,
-            "y": 4.5994304003649855,
-            "heading": -0.9640144900160125,
-            "angularVelocity": 0.021341854037057975,
-            "velocityX": 3.0528597828545716,
-            "velocityY": 2.2232845496735987,
-            "timestamp": 0.917125339309138
-        },
-        {
-            "x": 6.681598941757368,
-            "y": 4.785246665794811,
-            "heading": -0.9639002296367303,
-            "angularVelocity": 0.0013704388246982366,
-            "velocityX": 3.0562333676929114,
-            "velocityY": 2.2286800201898096,
-            "timestamp": 1.0005003701554231
-        },
-        {
-            "x": 6.9014822245071255,
-            "y": 5.01136660335564,
-            "heading": -0.9639000238890051,
-            "angularVelocity": 0.000002467737919950576,
-            "velocityX": 2.6372797769052228,
-            "velocityY": 2.7120822057351384,
-            "timestamp": 1.0838754010017084
-        },
-        {
-            "x": 7.122012116423131,
-            "y": 5.23107273201418,
-            "heading": -0.9515772318649645,
-            "angularVelocity": 0.1477995498047812,
-            "velocityX": 2.645035206314798,
-            "velocityY": 2.635154990990089,
-            "timestamp": 1.1672504318479937
-        },
-        {
-            "x": 7.311148990719613,
-            "y": 5.417840497645029,
-            "heading": -0.7917020160601519,
-            "angularVelocity": 1.9175431083146124,
-            "velocityX": 2.2685073981583956,
-            "velocityY": 2.2400923122317544,
-            "timestamp": 1.250625462694279
-        },
-        {
-            "x": 7.462007390054419,
-            "y": 5.572027096128993,
-            "heading": -0.5225549044290918,
-            "angularVelocity": 3.228150069620655,
-            "velocityX": 1.8093954245478587,
-            "velocityY": 1.8493138403538476,
-            "timestamp": 1.3340004935405643
-        },
-        {
-            "x": 7.571928087470151,
-            "y": 5.685474744918936,
-            "heading": -0.27088873612536774,
-            "angularVelocity": 3.018483660506339,
-            "velocityX": 1.318388686636741,
-            "velocityY": 1.3606909363440005,
-            "timestamp": 1.4173755243868496
-        },
-        {
-            "x": 7.6442805622690475,
-            "y": 5.7599789636443175,
-            "heading": -0.0924144798498188,
-            "angularVelocity": 2.1406199729580164,
-            "velocityX": 0.8677954786282445,
-            "velocityY": 0.8936034921863096,
-            "timestamp": 1.5007505552331348
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -2.5332548087748344e-31,
-            "angularVelocity": 1.1084191383412998,
-            "velocityX": 0.42841888474747514,
-            "velocityY": 0.4440302567795869,
-            "timestamp": 1.5841255860794201
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 5.737670210968143e-33,
-            "velocityX": 0,
-            "velocityY": -3.2710687085311585e-31,
-            "timestamp": 1.6675006169257054
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.1.traj
deleted file mode 100644
index 9d039264..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.1.traj	
+++ /dev/null
@@ -1,166 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -1.0027901855857707e-30,
-            "velocityX": -5.17158696002297e-32,
-            "velocityY": 1.9371271312239428e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 4.300239460670815,
-            "y": 3.0530479185441526,
-            "heading": -0.6028935811234369,
-            "angularVelocity": -0.4158951640704737,
-            "velocityX": 0.4197942063083121,
-            "velocityY": 0.5980003056006388,
-            "timestamp": 0.08870884855296232
-        },
-        {
-            "x": 4.375862012794379,
-            "y": 3.1584586937478076,
-            "heading": -0.6740120078870278,
-            "angularVelocity": -0.8017061197804932,
-            "velocityX": 0.8524803709791626,
-            "velocityY": 1.1882780232540273,
-            "timestamp": 0.17741769710592464
-        },
-        {
-            "x": 4.491898262382256,
-            "y": 3.3149103934712074,
-            "heading": -0.7745169367898204,
-            "angularVelocity": -1.1329752391362358,
-            "velocityX": 1.3080572172977813,
-            "velocityY": 1.763653821185524,
-            "timestamp": 0.26612654565888694
-        },
-        {
-            "x": 4.65291407862912,
-            "y": 3.5188640922932937,
-            "heading": -0.893163244153065,
-            "angularVelocity": -1.3374799616794542,
-            "velocityX": 1.8151043427277864,
-            "velocityY": 2.299135905256609,
-            "timestamp": 0.3548353942118493
-        },
-        {
-            "x": 4.870914712338585,
-            "y": 3.7498942741357597,
-            "heading": -0.9455687285938831,
-            "angularVelocity": -0.5907582534963245,
-            "velocityX": 2.457484650804715,
-            "velocityY": 2.6043645657798487,
-            "timestamp": 0.44354424276481164
-        },
-        {
-            "x": 5.142859309500119,
-            "y": 3.9465163862068953,
-            "heading": -0.9455688623878027,
-            "angularVelocity": -0.0000015082364580335284,
-            "velocityX": 3.065585920655626,
-            "velocityY": 2.216488155110533,
-            "timestamp": 0.532253091317774
-        },
-        {
-            "x": 5.447866241414174,
-            "y": 4.086462405550872,
-            "heading": -0.9455689961292948,
-            "angularVelocity": -0.0000015076454528851402,
-            "velocityX": 3.438292085732067,
-            "velocityY": 1.5775880492961702,
-            "timestamp": 0.6209619398707363
-        },
-        {
-            "x": 5.774274975125892,
-            "y": 4.16438201701327,
-            "heading": -0.9455689173593601,
-            "angularVelocity": 8.879602884106285e-7,
-            "velocityX": 3.6795510147653454,
-            "velocityY": 0.8783747363813094,
-            "timestamp": 0.7096707884236987
-        },
-        {
-            "x": 6.109606649873876,
-            "y": 4.1772967927030695,
-            "heading": -0.9455688167433821,
-            "angularVelocity": 0.0000011342270768412302,
-            "velocityX": 3.780137835379285,
-            "velocityY": 0.14558610443566755,
-            "timestamp": 0.798379636976661
-        },
-        {
-            "x": 6.445060320326861,
-            "y": 4.168079453807152,
-            "heading": -0.9455687201101868,
-            "angularVelocity": 0.0000010893298350239914,
-            "velocityX": 3.781513072539862,
-            "velocityY": -0.10390551840399499,
-            "timestamp": 0.8870884855296234
-        },
-        {
-            "x": 6.780513985890507,
-            "y": 4.158861936977066,
-            "heading": -0.9455686234766241,
-            "angularVelocity": 0.0000010893339756447378,
-            "velocityX": 3.781513017423149,
-            "velocityY": -0.10390752422611307,
-            "timestamp": 0.9757973340825857
-        },
-        {
-            "x": 7.079210544299498,
-            "y": 4.144050975293544,
-            "heading": -0.856180476499644,
-            "angularVelocity": 1.0076576174203435,
-            "velocityX": 3.3671563015572095,
-            "velocityY": -0.16696149172400718,
-            "timestamp": 1.064506182635548
-        },
-        {
-            "x": 7.324091725395792,
-            "y": 4.136558502839443,
-            "heading": -0.6317167360096363,
-            "angularVelocity": 2.5303421716267107,
-            "velocityX": 2.760504561730298,
-            "velocityY": -0.08446138774564337,
-            "timestamp": 1.1532150311885103
-        },
-        {
-            "x": 7.50528856962614,
-            "y": 4.13147246408345,
-            "heading": -0.35065072299807204,
-            "angularVelocity": 3.1684101146207286,
-            "velocityX": 2.042601692909661,
-            "velocityY": -0.05733406350050504,
-            "timestamp": 1.2419238797414727
-        },
-        {
-            "x": 7.622685008969739,
-            "y": 4.128418008534293,
-            "heading": -0.12509027443555182,
-            "angularVelocity": 2.5427051781407424,
-            "velocityX": 1.323390408720159,
-            "velocityY": -0.03443236609404116,
-            "timestamp": 1.330632728294435
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.405276849475189e-32,
-            "angularVelocity": 1.4101217237744719,
-            "velocityX": 0.6461022994345517,
-            "velocityY": -0.01598497283444213,
-            "timestamp": 1.4193415768473974
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -2.3485646679922316e-31,
-            "velocityX": 0,
-            "velocityY": -2.6996723431981715e-31,
-            "timestamp": 1.5080504254003597
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.traj
deleted file mode 100644
index 9d039264..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.traj	
+++ /dev/null
@@ -1,166 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -1.0027901855857707e-30,
-            "velocityX": -5.17158696002297e-32,
-            "velocityY": 1.9371271312239428e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 4.300239460670815,
-            "y": 3.0530479185441526,
-            "heading": -0.6028935811234369,
-            "angularVelocity": -0.4158951640704737,
-            "velocityX": 0.4197942063083121,
-            "velocityY": 0.5980003056006388,
-            "timestamp": 0.08870884855296232
-        },
-        {
-            "x": 4.375862012794379,
-            "y": 3.1584586937478076,
-            "heading": -0.6740120078870278,
-            "angularVelocity": -0.8017061197804932,
-            "velocityX": 0.8524803709791626,
-            "velocityY": 1.1882780232540273,
-            "timestamp": 0.17741769710592464
-        },
-        {
-            "x": 4.491898262382256,
-            "y": 3.3149103934712074,
-            "heading": -0.7745169367898204,
-            "angularVelocity": -1.1329752391362358,
-            "velocityX": 1.3080572172977813,
-            "velocityY": 1.763653821185524,
-            "timestamp": 0.26612654565888694
-        },
-        {
-            "x": 4.65291407862912,
-            "y": 3.5188640922932937,
-            "heading": -0.893163244153065,
-            "angularVelocity": -1.3374799616794542,
-            "velocityX": 1.8151043427277864,
-            "velocityY": 2.299135905256609,
-            "timestamp": 0.3548353942118493
-        },
-        {
-            "x": 4.870914712338585,
-            "y": 3.7498942741357597,
-            "heading": -0.9455687285938831,
-            "angularVelocity": -0.5907582534963245,
-            "velocityX": 2.457484650804715,
-            "velocityY": 2.6043645657798487,
-            "timestamp": 0.44354424276481164
-        },
-        {
-            "x": 5.142859309500119,
-            "y": 3.9465163862068953,
-            "heading": -0.9455688623878027,
-            "angularVelocity": -0.0000015082364580335284,
-            "velocityX": 3.065585920655626,
-            "velocityY": 2.216488155110533,
-            "timestamp": 0.532253091317774
-        },
-        {
-            "x": 5.447866241414174,
-            "y": 4.086462405550872,
-            "heading": -0.9455689961292948,
-            "angularVelocity": -0.0000015076454528851402,
-            "velocityX": 3.438292085732067,
-            "velocityY": 1.5775880492961702,
-            "timestamp": 0.6209619398707363
-        },
-        {
-            "x": 5.774274975125892,
-            "y": 4.16438201701327,
-            "heading": -0.9455689173593601,
-            "angularVelocity": 8.879602884106285e-7,
-            "velocityX": 3.6795510147653454,
-            "velocityY": 0.8783747363813094,
-            "timestamp": 0.7096707884236987
-        },
-        {
-            "x": 6.109606649873876,
-            "y": 4.1772967927030695,
-            "heading": -0.9455688167433821,
-            "angularVelocity": 0.0000011342270768412302,
-            "velocityX": 3.780137835379285,
-            "velocityY": 0.14558610443566755,
-            "timestamp": 0.798379636976661
-        },
-        {
-            "x": 6.445060320326861,
-            "y": 4.168079453807152,
-            "heading": -0.9455687201101868,
-            "angularVelocity": 0.0000010893298350239914,
-            "velocityX": 3.781513072539862,
-            "velocityY": -0.10390551840399499,
-            "timestamp": 0.8870884855296234
-        },
-        {
-            "x": 6.780513985890507,
-            "y": 4.158861936977066,
-            "heading": -0.9455686234766241,
-            "angularVelocity": 0.0000010893339756447378,
-            "velocityX": 3.781513017423149,
-            "velocityY": -0.10390752422611307,
-            "timestamp": 0.9757973340825857
-        },
-        {
-            "x": 7.079210544299498,
-            "y": 4.144050975293544,
-            "heading": -0.856180476499644,
-            "angularVelocity": 1.0076576174203435,
-            "velocityX": 3.3671563015572095,
-            "velocityY": -0.16696149172400718,
-            "timestamp": 1.064506182635548
-        },
-        {
-            "x": 7.324091725395792,
-            "y": 4.136558502839443,
-            "heading": -0.6317167360096363,
-            "angularVelocity": 2.5303421716267107,
-            "velocityX": 2.760504561730298,
-            "velocityY": -0.08446138774564337,
-            "timestamp": 1.1532150311885103
-        },
-        {
-            "x": 7.50528856962614,
-            "y": 4.13147246408345,
-            "heading": -0.35065072299807204,
-            "angularVelocity": 3.1684101146207286,
-            "velocityX": 2.042601692909661,
-            "velocityY": -0.05733406350050504,
-            "timestamp": 1.2419238797414727
-        },
-        {
-            "x": 7.622685008969739,
-            "y": 4.128418008534293,
-            "heading": -0.12509027443555182,
-            "angularVelocity": 2.5427051781407424,
-            "velocityX": 1.323390408720159,
-            "velocityY": -0.03443236609404116,
-            "timestamp": 1.330632728294435
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.405276849475189e-32,
-            "angularVelocity": 1.4101217237744719,
-            "velocityX": 0.6461022994345517,
-            "velocityY": -0.01598497283444213,
-            "timestamp": 1.4193415768473974
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -2.3485646679922316e-31,
-            "velocityX": 0,
-            "velocityY": -2.6996723431981715e-31,
-            "timestamp": 1.5080504254003597
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4.1.traj
deleted file mode 100644
index 0b021e4e..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4.1.traj	
+++ /dev/null
@@ -1,238 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 0,
-            "velocityX": 5.832812358477626e-33,
-            "velocityY": -2.114400767819492e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 4.2637473414422145,
-            "y": 2.974020167404826,
-            "heading": -0.5570803398431683,
-            "angularVelocity": 0.15939480257853877,
-            "velocityX": 0.013355031418934325,
-            "velocityY": -0.46426099366130524,
-            "timestamp": 0.05595954204614445
-        },
-        {
-            "x": 4.266239342094047,
-            "y": 2.922097369670203,
-            "heading": -0.5396590874905302,
-            "angularVelocity": 0.31131870840312753,
-            "velocityX": 0.04453218451605929,
-            "velocityY": -0.9278631639230981,
-            "timestamp": 0.1119190840922889
-        },
-        {
-            "x": 4.271816064940073,
-            "y": 2.844342168020101,
-            "heading": -0.5143157133438189,
-            "angularVelocity": 0.4528874472527088,
-            "velocityX": 0.0996563345966598,
-            "velocityY": -1.3894895992176792,
-            "timestamp": 0.16787862613843335
-        },
-        {
-            "x": 4.282354343554946,
-            "y": 2.7410289237168692,
-            "heading": -0.48190029722305777,
-            "angularVelocity": 0.5792652143941603,
-            "velocityX": 0.18831960072480589,
-            "velocityY": -1.8462131841257783,
-            "timestamp": 0.2238381681845778
-        },
-        {
-            "x": 4.3006106845058465,
-            "y": 2.6128237251760056,
-            "heading": -0.4437500478548854,
-            "angularVelocity": 0.6817469902936669,
-            "velocityX": 0.32624178617911886,
-            "velocityY": -2.2910337335345985,
-            "timestamp": 0.27979771023072225
-        },
-        {
-            "x": 4.330801306241698,
-            "y": 2.4614150778969766,
-            "heading": -0.40214700400499553,
-            "angularVelocity": 0.7434486117771366,
-            "velocityX": 0.5395080201148661,
-            "velocityY": -2.705680599640681,
-            "timestamp": 0.3357572522768667
-        },
-        {
-            "x": 4.379172513113433,
-            "y": 2.2912661424924936,
-            "heading": -0.3611794534128463,
-            "angularVelocity": 0.7320923133783194,
-            "velocityX": 0.864396045840541,
-            "velocityY": -3.0405705476320635,
-            "timestamp": 0.39171679432301115
-        },
-        {
-            "x": 4.451949891834961,
-            "y": 2.111933259909663,
-            "heading": -0.32494756197390784,
-            "angularVelocity": 0.6474658318159084,
-            "velocityX": 1.3005356380778725,
-            "velocityY": -3.2046881733762564,
-            "timestamp": 0.4476763363691556
-        },
-        {
-            "x": 4.550285732793113,
-            "y": 1.9373903674143325,
-            "heading": -0.2971520965733793,
-            "angularVelocity": 0.49670644869849423,
-            "velocityX": 1.7572667209653445,
-            "velocityY": -3.1190907951212283,
-            "timestamp": 0.5036358784153001
-        },
-        {
-            "x": 4.671000177640236,
-            "y": 1.7762856565305696,
-            "heading": -0.2715476133805437,
-            "angularVelocity": 0.457553479828869,
-            "velocityX": 2.157173565637473,
-            "velocityY": -2.878949773229245,
-            "timestamp": 0.5595954204614446
-        },
-        {
-            "x": 4.810884164899462,
-            "y": 1.6328902215117949,
-            "heading": -0.24301377044973924,
-            "angularVelocity": 0.5099012945331528,
-            "velocityX": 2.4997343106181424,
-            "velocityY": -2.5624840693036233,
-            "timestamp": 0.6155549625075891
-        },
-        {
-            "x": 4.966899091534391,
-            "y": 1.5100232904396305,
-            "heading": -0.21315700918191657,
-            "angularVelocity": 0.5335419157506132,
-            "velocityX": 2.7879950573269263,
-            "velocityY": -2.1956386092446096,
-            "timestamp": 0.6715145045537336
-        },
-        {
-            "x": 5.136752128601074,
-            "y": 1.409298062324524,
-            "heading": -0.1837509654270181,
-            "angularVelocity": 0.5254875697633724,
-            "velocityX": 3.035282828558936,
-            "velocityY": -1.7999651968567865,
-            "timestamp": 0.7274740465998781
-        },
-        {
-            "x": 5.4382274814533025,
-            "y": 1.3074419969375322,
-            "heading": -0.14111678525881038,
-            "angularVelocity": 0.4782734646509064,
-            "velocityX": 3.3819733590888914,
-            "velocityY": -1.142629061849971,
-            "timestamp": 0.8166158939305274
-        },
-        {
-            "x": 5.764139683697637,
-            "y": 1.267066681764717,
-            "heading": -0.11133632809533998,
-            "angularVelocity": 0.3340794257159682,
-            "velocityX": 3.6561077877984602,
-            "velocityY": -0.4529333459183563,
-            "timestamp": 0.9057577412611766
-        },
-        {
-            "x": 6.100462721846291,
-            "y": 1.2915989985388854,
-            "heading": -0.11133120767369757,
-            "angularVelocity": 0.000057441278584387276,
-            "velocityX": 3.7728973340784897,
-            "velocityY": 0.2752053890377148,
-            "timestamp": 0.9948995885918258
-        },
-        {
-            "x": 6.42549869748969,
-            "y": 1.3814198753662668,
-            "heading": -0.1113311879575033,
-            "angularVelocity": 2.2117776172639892e-7,
-            "velocityX": 3.6462782113742715,
-            "velocityY": 1.0076174043625032,
-            "timestamp": 1.084041435922475
-        },
-        {
-            "x": 6.7266979846048365,
-            "y": 1.533061465523067,
-            "heading": -0.11133117504978503,
-            "angularVelocity": 1.4479976182990285e-7,
-            "velocityX": 3.3788764327254777,
-            "velocityY": 1.7011268522889103,
-            "timestamp": 1.1731832832531242
-        },
-        {
-            "x": 6.992432961420095,
-            "y": 1.740669527522903,
-            "heading": -0.11133116451645807,
-            "angularVelocity": 1.1816366034373416e-7,
-            "velocityX": 2.981035111708839,
-            "velocityY": 2.3289629754897407,
-            "timestamp": 1.2623251305837733
-        },
-        {
-            "x": 7.221595592407175,
-            "y": 1.979432543612985,
-            "heading": -0.09587073631794915,
-            "angularVelocity": 0.17343625537800947,
-            "velocityX": 2.5707637641507293,
-            "velocityY": 2.678461611912118,
-            "timestamp": 1.3514669779144224
-        },
-        {
-            "x": 7.404993837760709,
-            "y": 2.170501480308595,
-            "heading": -0.06411284637687509,
-            "angularVelocity": 0.3562624164974272,
-            "velocityX": 2.0573754173309005,
-            "velocityY": 2.1434258142181197,
-            "timestamp": 1.4406088252450715
-        },
-        {
-            "x": 7.54251389523076,
-            "y": 2.3137697288578556,
-            "heading": -0.0342508837604198,
-            "angularVelocity": 0.33499376006509896,
-            "velocityX": 1.5427104282454527,
-            "velocityY": 1.6071940714649837,
-            "timestamp": 1.5297506725757206
-        },
-        {
-            "x": 7.634176122098128,
-            "y": 2.4092619199619465,
-            "heading": -0.011960250388889534,
-            "angularVelocity": 0.25005801471490263,
-            "velocityX": 1.028273808679041,
-            "velocityY": 1.0712386377846093,
-            "timestamp": 1.6188925199063697
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 9.313254165846089e-33,
-            "angularVelocity": 0.1341709954082624,
-            "velocityX": 0.5140557355952123,
-            "velocityY": 0.5355294002487934,
-            "timestamp": 1.7080343672370188
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -1.1876173054192403e-31,
-            "velocityX": 4.5955148793706025e-32,
-            "velocityY": 0,
-            "timestamp": 1.797176214567668
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4.traj
deleted file mode 100644
index 0b021e4e..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4.traj	
+++ /dev/null
@@ -1,238 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 0,
-            "velocityX": 5.832812358477626e-33,
-            "velocityY": -2.114400767819492e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 4.2637473414422145,
-            "y": 2.974020167404826,
-            "heading": -0.5570803398431683,
-            "angularVelocity": 0.15939480257853877,
-            "velocityX": 0.013355031418934325,
-            "velocityY": -0.46426099366130524,
-            "timestamp": 0.05595954204614445
-        },
-        {
-            "x": 4.266239342094047,
-            "y": 2.922097369670203,
-            "heading": -0.5396590874905302,
-            "angularVelocity": 0.31131870840312753,
-            "velocityX": 0.04453218451605929,
-            "velocityY": -0.9278631639230981,
-            "timestamp": 0.1119190840922889
-        },
-        {
-            "x": 4.271816064940073,
-            "y": 2.844342168020101,
-            "heading": -0.5143157133438189,
-            "angularVelocity": 0.4528874472527088,
-            "velocityX": 0.0996563345966598,
-            "velocityY": -1.3894895992176792,
-            "timestamp": 0.16787862613843335
-        },
-        {
-            "x": 4.282354343554946,
-            "y": 2.7410289237168692,
-            "heading": -0.48190029722305777,
-            "angularVelocity": 0.5792652143941603,
-            "velocityX": 0.18831960072480589,
-            "velocityY": -1.8462131841257783,
-            "timestamp": 0.2238381681845778
-        },
-        {
-            "x": 4.3006106845058465,
-            "y": 2.6128237251760056,
-            "heading": -0.4437500478548854,
-            "angularVelocity": 0.6817469902936669,
-            "velocityX": 0.32624178617911886,
-            "velocityY": -2.2910337335345985,
-            "timestamp": 0.27979771023072225
-        },
-        {
-            "x": 4.330801306241698,
-            "y": 2.4614150778969766,
-            "heading": -0.40214700400499553,
-            "angularVelocity": 0.7434486117771366,
-            "velocityX": 0.5395080201148661,
-            "velocityY": -2.705680599640681,
-            "timestamp": 0.3357572522768667
-        },
-        {
-            "x": 4.379172513113433,
-            "y": 2.2912661424924936,
-            "heading": -0.3611794534128463,
-            "angularVelocity": 0.7320923133783194,
-            "velocityX": 0.864396045840541,
-            "velocityY": -3.0405705476320635,
-            "timestamp": 0.39171679432301115
-        },
-        {
-            "x": 4.451949891834961,
-            "y": 2.111933259909663,
-            "heading": -0.32494756197390784,
-            "angularVelocity": 0.6474658318159084,
-            "velocityX": 1.3005356380778725,
-            "velocityY": -3.2046881733762564,
-            "timestamp": 0.4476763363691556
-        },
-        {
-            "x": 4.550285732793113,
-            "y": 1.9373903674143325,
-            "heading": -0.2971520965733793,
-            "angularVelocity": 0.49670644869849423,
-            "velocityX": 1.7572667209653445,
-            "velocityY": -3.1190907951212283,
-            "timestamp": 0.5036358784153001
-        },
-        {
-            "x": 4.671000177640236,
-            "y": 1.7762856565305696,
-            "heading": -0.2715476133805437,
-            "angularVelocity": 0.457553479828869,
-            "velocityX": 2.157173565637473,
-            "velocityY": -2.878949773229245,
-            "timestamp": 0.5595954204614446
-        },
-        {
-            "x": 4.810884164899462,
-            "y": 1.6328902215117949,
-            "heading": -0.24301377044973924,
-            "angularVelocity": 0.5099012945331528,
-            "velocityX": 2.4997343106181424,
-            "velocityY": -2.5624840693036233,
-            "timestamp": 0.6155549625075891
-        },
-        {
-            "x": 4.966899091534391,
-            "y": 1.5100232904396305,
-            "heading": -0.21315700918191657,
-            "angularVelocity": 0.5335419157506132,
-            "velocityX": 2.7879950573269263,
-            "velocityY": -2.1956386092446096,
-            "timestamp": 0.6715145045537336
-        },
-        {
-            "x": 5.136752128601074,
-            "y": 1.409298062324524,
-            "heading": -0.1837509654270181,
-            "angularVelocity": 0.5254875697633724,
-            "velocityX": 3.035282828558936,
-            "velocityY": -1.7999651968567865,
-            "timestamp": 0.7274740465998781
-        },
-        {
-            "x": 5.4382274814533025,
-            "y": 1.3074419969375322,
-            "heading": -0.14111678525881038,
-            "angularVelocity": 0.4782734646509064,
-            "velocityX": 3.3819733590888914,
-            "velocityY": -1.142629061849971,
-            "timestamp": 0.8166158939305274
-        },
-        {
-            "x": 5.764139683697637,
-            "y": 1.267066681764717,
-            "heading": -0.11133632809533998,
-            "angularVelocity": 0.3340794257159682,
-            "velocityX": 3.6561077877984602,
-            "velocityY": -0.4529333459183563,
-            "timestamp": 0.9057577412611766
-        },
-        {
-            "x": 6.100462721846291,
-            "y": 1.2915989985388854,
-            "heading": -0.11133120767369757,
-            "angularVelocity": 0.000057441278584387276,
-            "velocityX": 3.7728973340784897,
-            "velocityY": 0.2752053890377148,
-            "timestamp": 0.9948995885918258
-        },
-        {
-            "x": 6.42549869748969,
-            "y": 1.3814198753662668,
-            "heading": -0.1113311879575033,
-            "angularVelocity": 2.2117776172639892e-7,
-            "velocityX": 3.6462782113742715,
-            "velocityY": 1.0076174043625032,
-            "timestamp": 1.084041435922475
-        },
-        {
-            "x": 6.7266979846048365,
-            "y": 1.533061465523067,
-            "heading": -0.11133117504978503,
-            "angularVelocity": 1.4479976182990285e-7,
-            "velocityX": 3.3788764327254777,
-            "velocityY": 1.7011268522889103,
-            "timestamp": 1.1731832832531242
-        },
-        {
-            "x": 6.992432961420095,
-            "y": 1.740669527522903,
-            "heading": -0.11133116451645807,
-            "angularVelocity": 1.1816366034373416e-7,
-            "velocityX": 2.981035111708839,
-            "velocityY": 2.3289629754897407,
-            "timestamp": 1.2623251305837733
-        },
-        {
-            "x": 7.221595592407175,
-            "y": 1.979432543612985,
-            "heading": -0.09587073631794915,
-            "angularVelocity": 0.17343625537800947,
-            "velocityX": 2.5707637641507293,
-            "velocityY": 2.678461611912118,
-            "timestamp": 1.3514669779144224
-        },
-        {
-            "x": 7.404993837760709,
-            "y": 2.170501480308595,
-            "heading": -0.06411284637687509,
-            "angularVelocity": 0.3562624164974272,
-            "velocityX": 2.0573754173309005,
-            "velocityY": 2.1434258142181197,
-            "timestamp": 1.4406088252450715
-        },
-        {
-            "x": 7.54251389523076,
-            "y": 2.3137697288578556,
-            "heading": -0.0342508837604198,
-            "angularVelocity": 0.33499376006509896,
-            "velocityX": 1.5427104282454527,
-            "velocityY": 1.6071940714649837,
-            "timestamp": 1.5297506725757206
-        },
-        {
-            "x": 7.634176122098128,
-            "y": 2.4092619199619465,
-            "heading": -0.011960250388889534,
-            "angularVelocity": 0.25005801471490263,
-            "velocityX": 1.028273808679041,
-            "velocityY": 1.0712386377846093,
-            "timestamp": 1.6188925199063697
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 9.313254165846089e-33,
-            "angularVelocity": 0.1341709954082624,
-            "velocityX": 0.5140557355952123,
-            "velocityY": 0.5355294002487934,
-            "timestamp": 1.7080343672370188
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -1.1876173054192403e-31,
-            "velocityX": 4.5955148793706025e-32,
-            "velocityY": 0,
-            "timestamp": 1.797176214567668
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.1.traj
deleted file mode 100644
index d933d22a..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.1.traj	
+++ /dev/null
@@ -1,184 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -2.2118032332823513e-27,
-            "velocityX": -2.2398078489200873e-28,
-            "velocityY": 7.32316842173409e-29,
-            "timestamp": 0
-        },
-        {
-            "x": 4.257275610783411,
-            "y": 2.9390850995701614,
-            "heading": -0.6153074756658753,
-            "angularVelocity": -0.5685220144659363,
-            "velocityX": -0.06600299944485946,
-            "velocityY": -0.7023572274929988,
-            "timestamp": 0.0867292284401611
-        },
-        {
-            "x": 4.259999029271463,
-            "y": 2.8165594202827866,
-            "heading": -0.6936339270516045,
-            "angularVelocity": -0.9031148183195571,
-            "velocityX": 0.03140139186100546,
-            "velocityY": -1.4127380295088343,
-            "timestamp": 0.1734584568803222
-        },
-        {
-            "x": 4.293815085802935,
-            "y": 2.6396142169455077,
-            "heading": -0.7636289676296801,
-            "angularVelocity": -0.8070524993355632,
-            "velocityX": 0.3899038091270867,
-            "velocityY": -2.0402026689233352,
-            "timestamp": 0.2601876853204833
-        },
-        {
-            "x": 4.379794167011224,
-            "y": 2.432035667571809,
-            "heading": -0.7730870281638328,
-            "angularVelocity": -0.10905274616477362,
-            "velocityX": 0.9913506986587586,
-            "velocityY": -2.3934093858210397,
-            "timestamp": 0.3469169137606444
-        },
-        {
-            "x": 4.522000800858502,
-            "y": 2.2059673134468256,
-            "heading": -0.7069342437539652,
-            "angularVelocity": 0.7627507542686203,
-            "velocityX": 1.6396621577856372,
-            "velocityY": -2.6065993920487904,
-            "timestamp": 0.43364614220080544
-        },
-        {
-            "x": 4.726803922559788,
-            "y": 1.9764479224139355,
-            "heading": -0.6474090008701758,
-            "angularVelocity": 0.6863342837743838,
-            "velocityX": 2.3614083208706247,
-            "velocityY": -2.6463903249323897,
-            "timestamp": 0.5203753706409665
-        },
-        {
-            "x": 4.985426543182091,
-            "y": 1.774560944536545,
-            "heading": -0.6474088034070749,
-            "angularVelocity": 0.000002276776866191791,
-            "velocityX": 2.9819545875556583,
-            "velocityY": -2.3277847792302735,
-            "timestamp": 0.6071045990811276
-        },
-        {
-            "x": 5.274431548584119,
-            "y": 1.6192543598542732,
-            "heading": -0.6474086941993331,
-            "angularVelocity": 0.000001259180368575944,
-            "velocityX": 3.3322676864516145,
-            "velocityY": -1.7907064028526984,
-            "timestamp": 0.6938338275212886
-        },
-        {
-            "x": 5.576835908979959,
-            "y": 1.4919922741488194,
-            "heading": -0.6474086295363626,
-            "angularVelocity": 7.455729944345581e-7,
-            "velocityX": 3.4867641028824017,
-            "velocityY": -1.4673494506325018,
-            "timestamp": 0.7805630559614497
-        },
-        {
-            "x": 5.888110144336631,
-            "y": 1.388298557512795,
-            "heading": -0.6474085736154427,
-            "angularVelocity": 6.447759407143774e-7,
-            "velocityX": 3.589034988031027,
-            "velocityY": -1.1956028953671916,
-            "timestamp": 0.8672922844016108
-        },
-        {
-            "x": 6.199390629137113,
-            "y": 1.2846236026664921,
-            "heading": -0.6474085176958322,
-            "angularVelocity": 6.447608435198244e-7,
-            "velocityX": 3.5891070449826046,
-            "velocityY": -1.1953865693365275,
-            "timestamp": 0.9540215128417718
-        },
-        {
-            "x": 6.510671944712167,
-            "y": 1.1809511422971397,
-            "heading": -0.6474084617763125,
-            "angularVelocity": 6.447597973785494e-7,
-            "velocityX": 3.589116623928252,
-            "velocityY": -1.1953578076724478,
-            "timestamp": 1.040750741281933
-        },
-        {
-            "x": 6.8219538378473334,
-            "y": 1.0772804160640193,
-            "heading": -0.6474084058668956,
-            "angularVelocity": 6.446433092317229e-7,
-            "velocityX": 3.5891232832762197,
-            "velocityY": -1.1953378128419065,
-            "timestamp": 1.127479969722094
-        },
-        {
-            "x": 7.104843996542823,
-            "y": 0.9795715918943764,
-            "heading": -0.5659567926753784,
-            "angularVelocity": 0.9391483662017565,
-            "velocityX": 3.2617626581409067,
-            "velocityY": -1.126596257420401,
-            "timestamp": 1.2142091981622551
-        },
-        {
-            "x": 7.336171547926519,
-            "y": 0.9014041069183023,
-            "heading": -0.36771202193482067,
-            "angularVelocity": 2.2857896271651064,
-            "velocityX": 2.6672386638755765,
-            "velocityY": -0.9012819136285574,
-            "timestamp": 1.3009384266024162
-        },
-        {
-            "x": 7.508580909570239,
-            "y": 0.8437450323938049,
-            "heading": -0.19262457792893875,
-            "angularVelocity": 2.018782446873579,
-            "velocityX": 1.9879037868147627,
-            "velocityY": -0.6648171044699364,
-            "timestamp": 1.3876676550425773
-        },
-        {
-            "x": 7.622996353926532,
-            "y": 0.8057914476404057,
-            "heading": -0.06643070405801102,
-            "angularVelocity": 1.4550328204291798,
-            "velocityX": 1.3192258989739947,
-            "velocityY": -0.4376100818144245,
-            "timestamp": 1.4743968834827383
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -1.2156690964354266e-28,
-            "angularVelocity": 0.7659552062526378,
-            "velocityX": 0.6572599237729437,
-            "velocityY": -0.21666799046150395,
-            "timestamp": 1.5611261119228994
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 1.25023472254267e-27,
-            "velocityX": 0,
-            "velocityY": -4.9030509543456705e-29,
-            "timestamp": 1.6478553403630605
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.traj
deleted file mode 100644
index d933d22a..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.traj	
+++ /dev/null
@@ -1,184 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -2.2118032332823513e-27,
-            "velocityX": -2.2398078489200873e-28,
-            "velocityY": 7.32316842173409e-29,
-            "timestamp": 0
-        },
-        {
-            "x": 4.257275610783411,
-            "y": 2.9390850995701614,
-            "heading": -0.6153074756658753,
-            "angularVelocity": -0.5685220144659363,
-            "velocityX": -0.06600299944485946,
-            "velocityY": -0.7023572274929988,
-            "timestamp": 0.0867292284401611
-        },
-        {
-            "x": 4.259999029271463,
-            "y": 2.8165594202827866,
-            "heading": -0.6936339270516045,
-            "angularVelocity": -0.9031148183195571,
-            "velocityX": 0.03140139186100546,
-            "velocityY": -1.4127380295088343,
-            "timestamp": 0.1734584568803222
-        },
-        {
-            "x": 4.293815085802935,
-            "y": 2.6396142169455077,
-            "heading": -0.7636289676296801,
-            "angularVelocity": -0.8070524993355632,
-            "velocityX": 0.3899038091270867,
-            "velocityY": -2.0402026689233352,
-            "timestamp": 0.2601876853204833
-        },
-        {
-            "x": 4.379794167011224,
-            "y": 2.432035667571809,
-            "heading": -0.7730870281638328,
-            "angularVelocity": -0.10905274616477362,
-            "velocityX": 0.9913506986587586,
-            "velocityY": -2.3934093858210397,
-            "timestamp": 0.3469169137606444
-        },
-        {
-            "x": 4.522000800858502,
-            "y": 2.2059673134468256,
-            "heading": -0.7069342437539652,
-            "angularVelocity": 0.7627507542686203,
-            "velocityX": 1.6396621577856372,
-            "velocityY": -2.6065993920487904,
-            "timestamp": 0.43364614220080544
-        },
-        {
-            "x": 4.726803922559788,
-            "y": 1.9764479224139355,
-            "heading": -0.6474090008701758,
-            "angularVelocity": 0.6863342837743838,
-            "velocityX": 2.3614083208706247,
-            "velocityY": -2.6463903249323897,
-            "timestamp": 0.5203753706409665
-        },
-        {
-            "x": 4.985426543182091,
-            "y": 1.774560944536545,
-            "heading": -0.6474088034070749,
-            "angularVelocity": 0.000002276776866191791,
-            "velocityX": 2.9819545875556583,
-            "velocityY": -2.3277847792302735,
-            "timestamp": 0.6071045990811276
-        },
-        {
-            "x": 5.274431548584119,
-            "y": 1.6192543598542732,
-            "heading": -0.6474086941993331,
-            "angularVelocity": 0.000001259180368575944,
-            "velocityX": 3.3322676864516145,
-            "velocityY": -1.7907064028526984,
-            "timestamp": 0.6938338275212886
-        },
-        {
-            "x": 5.576835908979959,
-            "y": 1.4919922741488194,
-            "heading": -0.6474086295363626,
-            "angularVelocity": 7.455729944345581e-7,
-            "velocityX": 3.4867641028824017,
-            "velocityY": -1.4673494506325018,
-            "timestamp": 0.7805630559614497
-        },
-        {
-            "x": 5.888110144336631,
-            "y": 1.388298557512795,
-            "heading": -0.6474085736154427,
-            "angularVelocity": 6.447759407143774e-7,
-            "velocityX": 3.589034988031027,
-            "velocityY": -1.1956028953671916,
-            "timestamp": 0.8672922844016108
-        },
-        {
-            "x": 6.199390629137113,
-            "y": 1.2846236026664921,
-            "heading": -0.6474085176958322,
-            "angularVelocity": 6.447608435198244e-7,
-            "velocityX": 3.5891070449826046,
-            "velocityY": -1.1953865693365275,
-            "timestamp": 0.9540215128417718
-        },
-        {
-            "x": 6.510671944712167,
-            "y": 1.1809511422971397,
-            "heading": -0.6474084617763125,
-            "angularVelocity": 6.447597973785494e-7,
-            "velocityX": 3.589116623928252,
-            "velocityY": -1.1953578076724478,
-            "timestamp": 1.040750741281933
-        },
-        {
-            "x": 6.8219538378473334,
-            "y": 1.0772804160640193,
-            "heading": -0.6474084058668956,
-            "angularVelocity": 6.446433092317229e-7,
-            "velocityX": 3.5891232832762197,
-            "velocityY": -1.1953378128419065,
-            "timestamp": 1.127479969722094
-        },
-        {
-            "x": 7.104843996542823,
-            "y": 0.9795715918943764,
-            "heading": -0.5659567926753784,
-            "angularVelocity": 0.9391483662017565,
-            "velocityX": 3.2617626581409067,
-            "velocityY": -1.126596257420401,
-            "timestamp": 1.2142091981622551
-        },
-        {
-            "x": 7.336171547926519,
-            "y": 0.9014041069183023,
-            "heading": -0.36771202193482067,
-            "angularVelocity": 2.2857896271651064,
-            "velocityX": 2.6672386638755765,
-            "velocityY": -0.9012819136285574,
-            "timestamp": 1.3009384266024162
-        },
-        {
-            "x": 7.508580909570239,
-            "y": 0.8437450323938049,
-            "heading": -0.19262457792893875,
-            "angularVelocity": 2.018782446873579,
-            "velocityX": 1.9879037868147627,
-            "velocityY": -0.6648171044699364,
-            "timestamp": 1.3876676550425773
-        },
-        {
-            "x": 7.622996353926532,
-            "y": 0.8057914476404057,
-            "heading": -0.06643070405801102,
-            "angularVelocity": 1.4550328204291798,
-            "velocityX": 1.3192258989739947,
-            "velocityY": -0.4376100818144245,
-            "timestamp": 1.4743968834827383
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -1.2156690964354266e-28,
-            "angularVelocity": 0.7659552062526378,
-            "velocityX": 0.6572599237729437,
-            "velocityY": -0.21666799046150395,
-            "timestamp": 1.5611261119228994
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 1.25023472254267e-27,
-            "velocityX": 0,
-            "velocityY": -4.9030509543456705e-29,
-            "timestamp": 1.6478553403630605
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W1.1.traj
deleted file mode 100644
index 44ee68f9..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W1.1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -1.6794679447375943e-30,
-            "velocityX": 6.742046899995187e-33,
-            "velocityY": 9.708842875589211e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 4.290023963257714,
-            "y": 3.0497874441329906,
-            "heading": -0.5200869306801483,
-            "angularVelocity": 0.5503538158132694,
-            "velocityX": 0.3239326300245928,
-            "velocityY": 0.5967954280576505,
-            "timestamp": 0.08342464065958187
-        },
-        {
-            "x": 4.342730369898429,
-            "y": 3.150178003649595,
-            "heading": -0.42997936088427535,
-            "angularVelocity": 1.0801073769506666,
-            "velocityX": 0.6317846408926776,
-            "velocityY": 1.2033681982072046,
-            "timestamp": 0.16684928131916374
-        },
-        {
-            "x": 4.418537982231504,
-            "y": 3.3026029150734924,
-            "heading": -0.2990998156933407,
-            "angularVelocity": 1.5688355880967437,
-            "velocityX": 0.9086957010988046,
-            "velocityY": 1.8270970089745318,
-            "timestamp": 0.2502739219787456
-        },
-        {
-            "x": 4.5104462205151705,
-            "y": 3.5100667276192694,
-            "heading": -0.13617594937576347,
-            "angularVelocity": 1.9529465758491624,
-            "velocityX": 1.1016917490685052,
-            "velocityY": 2.4868409489750483,
-            "timestamp": 0.3336985626383275
-        },
-        {
-            "x": 4.573472263152817,
-            "y": 3.7674681923433693,
-            "heading": 0.004287761033867021,
-            "angularVelocity": 1.6837196935950753,
-            "velocityX": 0.7554847361563879,
-            "velocityY": 3.0854369007645883,
-            "timestamp": 0.41712320329790936
-        },
-        {
-            "x": 4.6071871310155705,
-            "y": 4.066840423815047,
-            "heading": 0.04705620405427594,
-            "angularVelocity": 0.5126596013152507,
-            "velocityX": 0.40413560785150865,
-            "velocityY": 3.5885348633778387,
-            "timestamp": 0.5005478439574912
-        },
-        {
-            "x": 4.587417601167403,
-            "y": 4.370555626705338,
-            "heading": 0.08458578048595149,
-            "angularVelocity": 0.4498620088136386,
-            "velocityX": -0.23697470785444333,
-            "velocityY": 3.6405934804036506,
-            "timestamp": 0.5839724846170731
-        },
-        {
-            "x": 4.510022832963655,
-            "y": 4.67116829443229,
-            "heading": 0.1002321163817716,
-            "angularVelocity": 0.1875505338964035,
-            "velocityX": -0.9277207260569706,
-            "velocityY": 3.603403806719601,
-            "timestamp": 0.667397125276655
-        },
-        {
-            "x": 4.376634060333898,
-            "y": 4.957183607465761,
-            "heading": 0.10023219176740035,
-            "angularVelocity": 9.036374403612271e-7,
-            "velocityX": -1.5989133615097586,
-            "velocityY": 3.4284272700744434,
-            "timestamp": 0.7508217659362368
-        },
-        {
-            "x": 4.19313075849175,
-            "y": 5.213940045953902,
-            "heading": 0.10023221692184411,
-            "angularVelocity": 3.015229499928316e-7,
-            "velocityX": -2.1996295146291525,
-            "velocityY": 3.0777050576201734,
-            "timestamp": 0.8342464065958187
-        },
-        {
-            "x": 3.9657176811920825,
-            "y": 5.432756463781841,
-            "heading": 0.10023223856547658,
-            "angularVelocity": 2.5943932502769966e-7,
-            "velocityX": -2.725970115084308,
-            "velocityY": 2.6229231087830502,
-            "timestamp": 0.9176710472554006
-        },
-        {
-            "x": 3.7186426713834653,
-            "y": 5.629101242954768,
-            "heading": 0.10023225962798245,
-            "angularVelocity": 2.524734383899783e-7,
-            "velocityX": -2.961655068037016,
-            "velocityY": 2.3535585843770224,
-            "timestamp": 1.0010956879149824
-        },
-        {
-            "x": 3.4715770348675625,
-            "y": 5.825457816649942,
-            "heading": 0.10023228069061084,
-            "angularVelocity": 2.5247490699416256e-7,
-            "velocityX": -2.961542711631996,
-            "velocityY": 2.353699963736333,
-            "timestamp": 1.0845203285745642
-        },
-        {
-            "x": 3.224511384705754,
-            "y": 6.021814373175139,
-            "heading": 0.10023230175325984,
-            "angularVelocity": 2.5247515406639525e-7,
-            "velocityX": -2.96154287520364,
-            "velocityY": 2.353699757922098,
-            "timestamp": 1.167944969234146
-        },
-        {
-            "x": 2.9774457295747583,
-            "y": 6.218170923447359,
-            "heading": 0.10023232281654228,
-            "angularVelocity": 2.524827469327523e-7,
-            "velocityX": -2.9615429347686173,
-            "velocityY": 2.3536996829685104,
-            "timestamp": 1.2513696098937277
-        },
-        {
-            "x": 2.751412346836958,
-            "y": 6.39797212959724,
-            "heading": 0.1658210049038553,
-            "angularVelocity": 0.7862027521934517,
-            "velocityX": -2.7094319010631414,
-            "velocityY": 2.155252989144637,
-            "timestamp": 1.3347942505533095
-        },
-        {
-            "x": 2.5706626829115913,
-            "y": 6.54172747182469,
-            "heading": 0.24638520673123038,
-            "angularVelocity": 0.9657123026291593,
-            "velocityX": -2.166622025534686,
-            "velocityY": 1.723176043563091,
-            "timestamp": 1.4182188912128912
-        },
-        {
-            "x": 2.435257350873027,
-            "y": 6.649419264379792,
-            "heading": 0.31518856404188306,
-            "angularVelocity": 0.8247366337651717,
-            "velocityX": -1.6230855891977036,
-            "velocityY": 1.2908871012647558,
-            "timestamp": 1.501643531872473
-        },
-        {
-            "x": 2.3450659538089185,
-            "y": 6.721154353305962,
-            "heading": 0.3643262231924431,
-            "angularVelocity": 0.5890065424563048,
-            "velocityX": -1.0811122031935256,
-            "velocityY": 0.8598789081859941,
-            "timestamp": 1.5850681725320548
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0.3077481257883978,
-            "velocityX": -0.5401995555822935,
-            "velocityY": 0.42967696846676384,
-            "timestamp": 1.6684928131916366
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": -2.8369790280137563e-29,
-            "velocityX": 0,
-            "velocityY": -1.9155196850548007e-31,
-            "timestamp": 1.7519174538512183
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W1.traj
deleted file mode 100644
index 44ee68f9..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -1.6794679447375943e-30,
-            "velocityX": 6.742046899995187e-33,
-            "velocityY": 9.708842875589211e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 4.290023963257714,
-            "y": 3.0497874441329906,
-            "heading": -0.5200869306801483,
-            "angularVelocity": 0.5503538158132694,
-            "velocityX": 0.3239326300245928,
-            "velocityY": 0.5967954280576505,
-            "timestamp": 0.08342464065958187
-        },
-        {
-            "x": 4.342730369898429,
-            "y": 3.150178003649595,
-            "heading": -0.42997936088427535,
-            "angularVelocity": 1.0801073769506666,
-            "velocityX": 0.6317846408926776,
-            "velocityY": 1.2033681982072046,
-            "timestamp": 0.16684928131916374
-        },
-        {
-            "x": 4.418537982231504,
-            "y": 3.3026029150734924,
-            "heading": -0.2990998156933407,
-            "angularVelocity": 1.5688355880967437,
-            "velocityX": 0.9086957010988046,
-            "velocityY": 1.8270970089745318,
-            "timestamp": 0.2502739219787456
-        },
-        {
-            "x": 4.5104462205151705,
-            "y": 3.5100667276192694,
-            "heading": -0.13617594937576347,
-            "angularVelocity": 1.9529465758491624,
-            "velocityX": 1.1016917490685052,
-            "velocityY": 2.4868409489750483,
-            "timestamp": 0.3336985626383275
-        },
-        {
-            "x": 4.573472263152817,
-            "y": 3.7674681923433693,
-            "heading": 0.004287761033867021,
-            "angularVelocity": 1.6837196935950753,
-            "velocityX": 0.7554847361563879,
-            "velocityY": 3.0854369007645883,
-            "timestamp": 0.41712320329790936
-        },
-        {
-            "x": 4.6071871310155705,
-            "y": 4.066840423815047,
-            "heading": 0.04705620405427594,
-            "angularVelocity": 0.5126596013152507,
-            "velocityX": 0.40413560785150865,
-            "velocityY": 3.5885348633778387,
-            "timestamp": 0.5005478439574912
-        },
-        {
-            "x": 4.587417601167403,
-            "y": 4.370555626705338,
-            "heading": 0.08458578048595149,
-            "angularVelocity": 0.4498620088136386,
-            "velocityX": -0.23697470785444333,
-            "velocityY": 3.6405934804036506,
-            "timestamp": 0.5839724846170731
-        },
-        {
-            "x": 4.510022832963655,
-            "y": 4.67116829443229,
-            "heading": 0.1002321163817716,
-            "angularVelocity": 0.1875505338964035,
-            "velocityX": -0.9277207260569706,
-            "velocityY": 3.603403806719601,
-            "timestamp": 0.667397125276655
-        },
-        {
-            "x": 4.376634060333898,
-            "y": 4.957183607465761,
-            "heading": 0.10023219176740035,
-            "angularVelocity": 9.036374403612271e-7,
-            "velocityX": -1.5989133615097586,
-            "velocityY": 3.4284272700744434,
-            "timestamp": 0.7508217659362368
-        },
-        {
-            "x": 4.19313075849175,
-            "y": 5.213940045953902,
-            "heading": 0.10023221692184411,
-            "angularVelocity": 3.015229499928316e-7,
-            "velocityX": -2.1996295146291525,
-            "velocityY": 3.0777050576201734,
-            "timestamp": 0.8342464065958187
-        },
-        {
-            "x": 3.9657176811920825,
-            "y": 5.432756463781841,
-            "heading": 0.10023223856547658,
-            "angularVelocity": 2.5943932502769966e-7,
-            "velocityX": -2.725970115084308,
-            "velocityY": 2.6229231087830502,
-            "timestamp": 0.9176710472554006
-        },
-        {
-            "x": 3.7186426713834653,
-            "y": 5.629101242954768,
-            "heading": 0.10023225962798245,
-            "angularVelocity": 2.524734383899783e-7,
-            "velocityX": -2.961655068037016,
-            "velocityY": 2.3535585843770224,
-            "timestamp": 1.0010956879149824
-        },
-        {
-            "x": 3.4715770348675625,
-            "y": 5.825457816649942,
-            "heading": 0.10023228069061084,
-            "angularVelocity": 2.5247490699416256e-7,
-            "velocityX": -2.961542711631996,
-            "velocityY": 2.353699963736333,
-            "timestamp": 1.0845203285745642
-        },
-        {
-            "x": 3.224511384705754,
-            "y": 6.021814373175139,
-            "heading": 0.10023230175325984,
-            "angularVelocity": 2.5247515406639525e-7,
-            "velocityX": -2.96154287520364,
-            "velocityY": 2.353699757922098,
-            "timestamp": 1.167944969234146
-        },
-        {
-            "x": 2.9774457295747583,
-            "y": 6.218170923447359,
-            "heading": 0.10023232281654228,
-            "angularVelocity": 2.524827469327523e-7,
-            "velocityX": -2.9615429347686173,
-            "velocityY": 2.3536996829685104,
-            "timestamp": 1.2513696098937277
-        },
-        {
-            "x": 2.751412346836958,
-            "y": 6.39797212959724,
-            "heading": 0.1658210049038553,
-            "angularVelocity": 0.7862027521934517,
-            "velocityX": -2.7094319010631414,
-            "velocityY": 2.155252989144637,
-            "timestamp": 1.3347942505533095
-        },
-        {
-            "x": 2.5706626829115913,
-            "y": 6.54172747182469,
-            "heading": 0.24638520673123038,
-            "angularVelocity": 0.9657123026291593,
-            "velocityX": -2.166622025534686,
-            "velocityY": 1.723176043563091,
-            "timestamp": 1.4182188912128912
-        },
-        {
-            "x": 2.435257350873027,
-            "y": 6.649419264379792,
-            "heading": 0.31518856404188306,
-            "angularVelocity": 0.8247366337651717,
-            "velocityX": -1.6230855891977036,
-            "velocityY": 1.2908871012647558,
-            "timestamp": 1.501643531872473
-        },
-        {
-            "x": 2.3450659538089185,
-            "y": 6.721154353305962,
-            "heading": 0.3643262231924431,
-            "angularVelocity": 0.5890065424563048,
-            "velocityX": -1.0811122031935256,
-            "velocityY": 0.8598789081859941,
-            "timestamp": 1.5850681725320548
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0.3077481257883978,
-            "velocityX": -0.5401995555822935,
-            "velocityY": 0.42967696846676384,
-            "timestamp": 1.6684928131916366
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": -2.8369790280137563e-29,
-            "velocityX": 0,
-            "velocityY": -1.9155196850548007e-31,
-            "timestamp": 1.7519174538512183
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.1.traj
deleted file mode 100644
index 01699583..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.1.traj	
+++ /dev/null
@@ -1,157 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.1175745999315144e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 4.331197427717942,
-            "y": 3.064203342806604,
-            "heading": -0.5918486780870994,
-            "angularVelocity": -0.24352382070717674,
-            "velocityX": 0.642497001367488,
-            "velocityY": 0.604868198278966,
-            "timestamp": 0.10614435176007272
-        },
-        {
-            "x": 4.457228812945604,
-            "y": 3.20188389088458,
-            "heading": -0.6500618235234895,
-            "angularVelocity": -0.5484337552691863,
-            "velocityX": 1.1873583769444531,
-            "velocityY": 1.2971066834454588,
-            "timestamp": 0.21228870352014545
-        },
-        {
-            "x": 4.610677951553499,
-            "y": 3.4285671398637003,
-            "heading": -0.7523465196948876,
-            "angularVelocity": -0.9636376733695737,
-            "velocityX": 1.4456646638603075,
-            "velocityY": 2.1356129197671536,
-            "timestamp": 0.31843305528021815
-        },
-        {
-            "x": 4.708912918877337,
-            "y": 3.730071149255109,
-            "heading": -0.9027593729452231,
-            "angularVelocity": -1.4170594172577993,
-            "velocityX": 0.9254846413861598,
-            "velocityY": 2.840509215911215,
-            "timestamp": 0.4245774070402909
-        },
-        {
-            "x": 4.717930123488916,
-            "y": 4.059451821332611,
-            "heading": -1.0196863730682848,
-            "angularVelocity": -1.1015847587195386,
-            "velocityX": 0.08495227925044571,
-            "velocityY": 3.10313894819415,
-            "timestamp": 0.5307217588003637
-        },
-        {
-            "x": 4.639893034223715,
-            "y": 4.389369622514019,
-            "heading": -1.0106655427651627,
-            "angularVelocity": 0.08498643737080536,
-            "velocityX": -0.7351977563685651,
-            "velocityY": 3.1081993126412395,
-            "timestamp": 0.6368661105604364
-        },
-        {
-            "x": 4.471904603111427,
-            "y": 4.710090421849215,
-            "heading": -0.9107146957571205,
-            "angularVelocity": 0.9416501712118399,
-            "velocityX": -1.5826412647185075,
-            "velocityY": 3.021553139823678,
-            "timestamp": 0.7430104623205092
-        },
-        {
-            "x": 4.218561716713538,
-            "y": 4.992188206454831,
-            "heading": -0.8355436069817835,
-            "angularVelocity": 0.7081967860640641,
-            "velocityX": -2.3867768957743736,
-            "velocityY": 2.657680601255776,
-            "timestamp": 0.8491548140805819
-        },
-        {
-            "x": 3.8896224481808446,
-            "y": 5.222473200709689,
-            "heading": -0.83554222765338,
-            "angularVelocity": 0.000012994835623464205,
-            "velocityX": -3.098980426921093,
-            "velocityY": 2.169545439171289,
-            "timestamp": 0.9552991658406547
-        },
-        {
-            "x": 3.516178298161353,
-            "y": 5.370026719708121,
-            "heading": -0.8355421446101063,
-            "angularVelocity": 7.823616840140413e-7,
-            "velocityX": -3.518266811442025,
-            "velocityY": 1.390121250464274,
-            "timestamp": 1.0614435176007273
-        },
-        {
-            "x": 3.1189612059977283,
-            "y": 5.42877328608909,
-            "heading": -0.8355420737846555,
-            "angularVelocity": 6.672559542292647e-7,
-            "velocityX": -3.742234848835754,
-            "velocityY": 0.55345918465599,
-            "timestamp": 1.1675878693608
-        },
-        {
-            "x": 2.774416222135212,
-            "y": 5.484170620762628,
-            "heading": -0.7035735462776811,
-            "angularVelocity": 1.243292980914089,
-            "velocityX": -3.2460039384980304,
-            "velocityY": 0.5219056290320314,
-            "timestamp": 1.2737322211208726
-        },
-        {
-            "x": 2.5095166350816602,
-            "y": 5.523131311595933,
-            "heading": -0.3884677902217159,
-            "angularVelocity": 2.9686530732056857,
-            "velocityX": -2.4956541036901063,
-            "velocityY": 0.3670538298766209,
-            "timestamp": 1.3798765728809452
-        },
-        {
-            "x": 2.335800273937377,
-            "y": 5.547984865708467,
-            "heading": -0.13742924633332027,
-            "angularVelocity": 2.365067379711733,
-            "velocityX": -1.6366048523895966,
-            "velocityY": 0.23414862590816604,
-            "timestamp": 1.4860209246410179
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 1.2947391364164484,
-            "velocityX": -0.8083357476365635,
-            "velocityY": 0.11319617193282665,
-            "timestamp": 1.5921652764010905
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -1.986468774957569e-34,
-            "velocityY": 0,
-            "timestamp": 1.6983096281611632
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.traj
deleted file mode 100644
index 01699583..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.traj	
+++ /dev/null
@@ -1,157 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.1175745999315144e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 4.331197427717942,
-            "y": 3.064203342806604,
-            "heading": -0.5918486780870994,
-            "angularVelocity": -0.24352382070717674,
-            "velocityX": 0.642497001367488,
-            "velocityY": 0.604868198278966,
-            "timestamp": 0.10614435176007272
-        },
-        {
-            "x": 4.457228812945604,
-            "y": 3.20188389088458,
-            "heading": -0.6500618235234895,
-            "angularVelocity": -0.5484337552691863,
-            "velocityX": 1.1873583769444531,
-            "velocityY": 1.2971066834454588,
-            "timestamp": 0.21228870352014545
-        },
-        {
-            "x": 4.610677951553499,
-            "y": 3.4285671398637003,
-            "heading": -0.7523465196948876,
-            "angularVelocity": -0.9636376733695737,
-            "velocityX": 1.4456646638603075,
-            "velocityY": 2.1356129197671536,
-            "timestamp": 0.31843305528021815
-        },
-        {
-            "x": 4.708912918877337,
-            "y": 3.730071149255109,
-            "heading": -0.9027593729452231,
-            "angularVelocity": -1.4170594172577993,
-            "velocityX": 0.9254846413861598,
-            "velocityY": 2.840509215911215,
-            "timestamp": 0.4245774070402909
-        },
-        {
-            "x": 4.717930123488916,
-            "y": 4.059451821332611,
-            "heading": -1.0196863730682848,
-            "angularVelocity": -1.1015847587195386,
-            "velocityX": 0.08495227925044571,
-            "velocityY": 3.10313894819415,
-            "timestamp": 0.5307217588003637
-        },
-        {
-            "x": 4.639893034223715,
-            "y": 4.389369622514019,
-            "heading": -1.0106655427651627,
-            "angularVelocity": 0.08498643737080536,
-            "velocityX": -0.7351977563685651,
-            "velocityY": 3.1081993126412395,
-            "timestamp": 0.6368661105604364
-        },
-        {
-            "x": 4.471904603111427,
-            "y": 4.710090421849215,
-            "heading": -0.9107146957571205,
-            "angularVelocity": 0.9416501712118399,
-            "velocityX": -1.5826412647185075,
-            "velocityY": 3.021553139823678,
-            "timestamp": 0.7430104623205092
-        },
-        {
-            "x": 4.218561716713538,
-            "y": 4.992188206454831,
-            "heading": -0.8355436069817835,
-            "angularVelocity": 0.7081967860640641,
-            "velocityX": -2.3867768957743736,
-            "velocityY": 2.657680601255776,
-            "timestamp": 0.8491548140805819
-        },
-        {
-            "x": 3.8896224481808446,
-            "y": 5.222473200709689,
-            "heading": -0.83554222765338,
-            "angularVelocity": 0.000012994835623464205,
-            "velocityX": -3.098980426921093,
-            "velocityY": 2.169545439171289,
-            "timestamp": 0.9552991658406547
-        },
-        {
-            "x": 3.516178298161353,
-            "y": 5.370026719708121,
-            "heading": -0.8355421446101063,
-            "angularVelocity": 7.823616840140413e-7,
-            "velocityX": -3.518266811442025,
-            "velocityY": 1.390121250464274,
-            "timestamp": 1.0614435176007273
-        },
-        {
-            "x": 3.1189612059977283,
-            "y": 5.42877328608909,
-            "heading": -0.8355420737846555,
-            "angularVelocity": 6.672559542292647e-7,
-            "velocityX": -3.742234848835754,
-            "velocityY": 0.55345918465599,
-            "timestamp": 1.1675878693608
-        },
-        {
-            "x": 2.774416222135212,
-            "y": 5.484170620762628,
-            "heading": -0.7035735462776811,
-            "angularVelocity": 1.243292980914089,
-            "velocityX": -3.2460039384980304,
-            "velocityY": 0.5219056290320314,
-            "timestamp": 1.2737322211208726
-        },
-        {
-            "x": 2.5095166350816602,
-            "y": 5.523131311595933,
-            "heading": -0.3884677902217159,
-            "angularVelocity": 2.9686530732056857,
-            "velocityX": -2.4956541036901063,
-            "velocityY": 0.3670538298766209,
-            "timestamp": 1.3798765728809452
-        },
-        {
-            "x": 2.335800273937377,
-            "y": 5.547984865708467,
-            "heading": -0.13742924633332027,
-            "angularVelocity": 2.365067379711733,
-            "velocityX": -1.6366048523895966,
-            "velocityY": 0.23414862590816604,
-            "timestamp": 1.4860209246410179
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 1.2947391364164484,
-            "velocityX": -0.8083357476365635,
-            "velocityY": 0.11319617193282665,
-            "timestamp": 1.5921652764010905
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -1.986468774957569e-34,
-            "velocityY": 0,
-            "timestamp": 1.6983096281611632
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.1.traj
deleted file mode 100644
index 26f24eb2..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.1.traj	
+++ /dev/null
@@ -1,121 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 0,
-            "velocityX": -3.809622996347125e-33,
-            "velocityY": 4.7029981856297106e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 4.1621681366579715,
-            "y": 2.9362991956112916,
-            "heading": -0.5618109581410129,
-            "angularVelocity": 0.03502487973446616,
-            "velocityX": -0.8430624486077395,
-            "velocityY": -0.5326069988815002,
-            "timestamp": 0.11960189130537771
-        },
-        {
-            "x": 3.949371568032143,
-            "y": 2.832059073063443,
-            "heading": -0.5332490097615643,
-            "angularVelocity": 0.2388085010004113,
-            "velocityX": -1.7792073879709687,
-            "velocityY": -0.8715591485229383,
-            "timestamp": 0.23920378261075542
-        },
-        {
-            "x": 3.6196334559490815,
-            "y": 2.7428786025637635,
-            "heading": -0.4400400521459035,
-            "angularVelocity": 0.7793267865446122,
-            "velocityX": -2.756964028613438,
-            "velocityY": -0.7456443165432319,
-            "timestamp": 0.35880567391613316
-        },
-        {
-            "x": 3.2124362068162218,
-            "y": 2.7443781288173086,
-            "heading": -0.34020495702705544,
-            "angularVelocity": 0.8347283979314292,
-            "velocityX": -3.4046054346513173,
-            "velocityY": 0.012537646664080927,
-            "timestamp": 0.47840756522151084
-        },
-        {
-            "x": 2.829910158877178,
-            "y": 2.8625567119312745,
-            "heading": -0.23363822858373923,
-            "angularVelocity": 0.891012067453128,
-            "velocityX": -3.1983277502054186,
-            "velocityY": 0.9880996180254248,
-            "timestamp": 0.5980094565268885
-        },
-        {
-            "x": 2.5228974587582895,
-            "y": 3.073051960500077,
-            "heading": -0.12255766699698727,
-            "angularVelocity": 0.9287525504349555,
-            "velocityX": -2.566955227614215,
-            "velocityY": 1.7599658857512914,
-            "timestamp": 0.7176113478322662
-        },
-        {
-            "x": 2.307806503132395,
-            "y": 3.359545759981809,
-            "heading": -0.008191563913849259,
-            "angularVelocity": 0.9562231987710198,
-            "velocityX": -1.7983909224036065,
-            "velocityY": 2.395395226236259,
-            "timestamp": 0.8372132391376439
-        },
-        {
-            "x": 2.197079056424326,
-            "y": 3.692587049160117,
-            "heading": -0.017161158826022753,
-            "angularVelocity": -0.07499542703130056,
-            "velocityX": -0.9258001315827844,
-            "velocityY": 2.7845821294578066,
-            "timestamp": 0.9568151304430216
-        },
-        {
-            "x": 2.2005818293568207,
-            "y": 4.024472502975606,
-            "heading": -0.15274865723996348,
-            "angularVelocity": -1.133656808718376,
-            "velocityX": 0.029286935969546842,
-            "velocityY": 2.774918107006309,
-            "timestamp": 1.0764170217483993
-        },
-        {
-            "x": 2.2590478504225837,
-            "y": 4.250579625353832,
-            "heading": -0.3027008584759083,
-            "angularVelocity": -1.2537611203242134,
-            "velocityX": 0.48883859968803833,
-            "velocityY": 1.8904978835235247,
-            "timestamp": 1.196018913053777
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.729914389908579,
-            "velocityX": 0.3424038627687911,
-            "velocityY": 0.914871608232253,
-            "timestamp": 1.3156208043591548
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 3.1982065543283105e-28,
-            "velocityX": 4.1170744495691044e-30,
-            "velocityY": 1.8090480101607624e-30,
-            "timestamp": 1.4352226956645326
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.traj
deleted file mode 100644
index 26f24eb2..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.traj	
+++ /dev/null
@@ -1,121 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 0,
-            "velocityX": -3.809622996347125e-33,
-            "velocityY": 4.7029981856297106e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 4.1621681366579715,
-            "y": 2.9362991956112916,
-            "heading": -0.5618109581410129,
-            "angularVelocity": 0.03502487973446616,
-            "velocityX": -0.8430624486077395,
-            "velocityY": -0.5326069988815002,
-            "timestamp": 0.11960189130537771
-        },
-        {
-            "x": 3.949371568032143,
-            "y": 2.832059073063443,
-            "heading": -0.5332490097615643,
-            "angularVelocity": 0.2388085010004113,
-            "velocityX": -1.7792073879709687,
-            "velocityY": -0.8715591485229383,
-            "timestamp": 0.23920378261075542
-        },
-        {
-            "x": 3.6196334559490815,
-            "y": 2.7428786025637635,
-            "heading": -0.4400400521459035,
-            "angularVelocity": 0.7793267865446122,
-            "velocityX": -2.756964028613438,
-            "velocityY": -0.7456443165432319,
-            "timestamp": 0.35880567391613316
-        },
-        {
-            "x": 3.2124362068162218,
-            "y": 2.7443781288173086,
-            "heading": -0.34020495702705544,
-            "angularVelocity": 0.8347283979314292,
-            "velocityX": -3.4046054346513173,
-            "velocityY": 0.012537646664080927,
-            "timestamp": 0.47840756522151084
-        },
-        {
-            "x": 2.829910158877178,
-            "y": 2.8625567119312745,
-            "heading": -0.23363822858373923,
-            "angularVelocity": 0.891012067453128,
-            "velocityX": -3.1983277502054186,
-            "velocityY": 0.9880996180254248,
-            "timestamp": 0.5980094565268885
-        },
-        {
-            "x": 2.5228974587582895,
-            "y": 3.073051960500077,
-            "heading": -0.12255766699698727,
-            "angularVelocity": 0.9287525504349555,
-            "velocityX": -2.566955227614215,
-            "velocityY": 1.7599658857512914,
-            "timestamp": 0.7176113478322662
-        },
-        {
-            "x": 2.307806503132395,
-            "y": 3.359545759981809,
-            "heading": -0.008191563913849259,
-            "angularVelocity": 0.9562231987710198,
-            "velocityX": -1.7983909224036065,
-            "velocityY": 2.395395226236259,
-            "timestamp": 0.8372132391376439
-        },
-        {
-            "x": 2.197079056424326,
-            "y": 3.692587049160117,
-            "heading": -0.017161158826022753,
-            "angularVelocity": -0.07499542703130056,
-            "velocityX": -0.9258001315827844,
-            "velocityY": 2.7845821294578066,
-            "timestamp": 0.9568151304430216
-        },
-        {
-            "x": 2.2005818293568207,
-            "y": 4.024472502975606,
-            "heading": -0.15274865723996348,
-            "angularVelocity": -1.133656808718376,
-            "velocityX": 0.029286935969546842,
-            "velocityY": 2.774918107006309,
-            "timestamp": 1.0764170217483993
-        },
-        {
-            "x": 2.2590478504225837,
-            "y": 4.250579625353832,
-            "heading": -0.3027008584759083,
-            "angularVelocity": -1.2537611203242134,
-            "velocityX": 0.48883859968803833,
-            "velocityY": 1.8904978835235247,
-            "timestamp": 1.196018913053777
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.729914389908579,
-            "velocityX": 0.3424038627687911,
-            "velocityY": 0.914871608232253,
-            "timestamp": 1.3156208043591548
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 3.1982065543283105e-28,
-            "velocityX": 4.1170744495691044e-30,
-            "velocityY": 1.8090480101607624e-30,
-            "timestamp": 1.4352226956645326
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C1.1.traj
deleted file mode 100644
index f8bd91fc..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C1.1.traj	
+++ /dev/null
@@ -1,292 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 1.1411819845129358e-30,
-            "velocityX": 2.0257584812603577e-32,
-            "velocityY": 3.042151659469263e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 5.128157808100627,
-            "y": 1.6381505638691025,
-            "heading": -0.6351424860741187,
-            "angularVelocity": 0.06199634399134786,
-            "velocityX": -0.6106088487961494,
-            "velocityY": 0.23165524966799825,
-            "timestamp": 0.07835161903352221
-        },
-        {
-            "x": 5.034138141125347,
-            "y": 1.6783506940439978,
-            "heading": -0.625731099901857,
-            "angularVelocity": 0.12011731586854772,
-            "velocityX": -1.199970953185457,
-            "velocityY": 0.5130733821555872,
-            "timestamp": 0.15670323806704442
-        },
-        {
-            "x": 4.897649658060979,
-            "y": 1.7471051706265737,
-            "heading": -0.6123788413002932,
-            "angularVelocity": 0.1704145844880519,
-            "velocityX": -1.74199952404266,
-            "velocityY": 0.8775118808095047,
-            "timestamp": 0.23505485710056662
-        },
-        {
-            "x": 4.7291295198915435,
-            "y": 1.855780594555545,
-            "heading": -0.5966040827715752,
-            "angularVelocity": 0.20133289807283902,
-            "velocityX": -2.150818837544829,
-            "velocityY": 1.3870220586313022,
-            "timestamp": 0.31340647613408884
-        },
-        {
-            "x": 4.559409610033689,
-            "y": 2.0156280635815147,
-            "heading": -0.5822183456142529,
-            "angularVelocity": 0.18360484869071408,
-            "velocityX": -2.1661314973624393,
-            "velocityY": 2.0401297509574126,
-            "timestamp": 0.39175809516761106
-        },
-        {
-            "x": 4.423281260574217,
-            "y": 2.2140742145732073,
-            "heading": -0.5727758831618049,
-            "angularVelocity": 0.1205139417528553,
-            "velocityX": -1.7374031467203144,
-            "velocityY": 2.532763884646578,
-            "timestamp": 0.4701097142011333
-        },
-        {
-            "x": 4.3324586171803325,
-            "y": 2.4362881098121476,
-            "heading": -0.5691499320560302,
-            "angularVelocity": 0.04627793465535804,
-            "velocityX": -1.1591674111421555,
-            "velocityY": 2.8361110846205655,
-            "timestamp": 0.5484613332346555
-        },
-        {
-            "x": 4.290356765831772,
-            "y": 2.674111890908862,
-            "heading": -0.5714790836196856,
-            "angularVelocity": -0.02972691046318988,
-            "velocityX": -0.5373450079001806,
-            "velocityY": 3.0353397164002804,
-            "timestamp": 0.6268129522681777
-        },
-        {
-            "x": 4.298268630722052,
-            "y": 2.9211188280591673,
-            "heading": -0.5947192771376703,
-            "angularVelocity": -0.29661408155511804,
-            "velocityX": 0.1009789585444941,
-            "velocityY": 3.1525441362561364,
-            "timestamp": 0.7051645713016998
-        },
-        {
-            "x": 4.356924477851345,
-            "y": 3.1722484967201265,
-            "heading": -0.6368890523591307,
-            "angularVelocity": -0.5382119188043637,
-            "velocityX": 0.7486232939768359,
-            "velocityY": 3.2051624683532776,
-            "timestamp": 0.783516190335222
-        },
-        {
-            "x": 4.466386404708156,
-            "y": 3.418800428561669,
-            "heading": -0.694172117002664,
-            "angularVelocity": -0.7311025011368958,
-            "velocityX": 1.3970601782967371,
-            "velocityY": 3.1467369134523744,
-            "timestamp": 0.8618678093687442
-        },
-        {
-            "x": 4.622742851027307,
-            "y": 3.644917946871822,
-            "heading": -0.7581397869413499,
-            "angularVelocity": -0.8164179723116849,
-            "velocityX": 1.9955739045067467,
-            "velocityY": 2.8859329404975034,
-            "timestamp": 0.9402194284022664
-        },
-        {
-            "x": 4.8130531116785615,
-            "y": 3.833426042269654,
-            "heading": -0.8482959054101304,
-            "angularVelocity": -1.1506605681014435,
-            "velocityX": 2.4289256941816704,
-            "velocityY": 2.4059246984696085,
-            "timestamp": 1.0185710474357885
-        },
-        {
-            "x": 5.034329891204834,
-            "y": 3.981222629547119,
-            "heading": -0.9454274313666489,
-            "angularVelocity": -1.2396875412997066,
-            "velocityX": 2.824150697276595,
-            "velocityY": 1.8863246107809373,
-            "timestamp": 1.0969226664693108
-        },
-        {
-            "x": 5.3307679727813255,
-            "y": 4.095215339701575,
-            "heading": -1.0325645415374454,
-            "angularVelocity": -0.9491279076997933,
-            "velocityX": 3.2289073573561264,
-            "velocityY": 1.241648503948074,
-            "timestamp": 1.1887302189223545
-        },
-        {
-            "x": 5.6706736122505745,
-            "y": 4.158722341660029,
-            "heading": -1.0366531166779425,
-            "angularVelocity": -0.04453419170049298,
-            "velocityX": 3.7023712144281196,
-            "velocityY": 0.691740496958957,
-            "timestamp": 1.2805377713753983
-        },
-        {
-            "x": 5.985432145120126,
-            "y": 4.2874622186845945,
-            "heading": -1.0150469515971576,
-            "angularVelocity": 0.23534191363870294,
-            "velocityX": 3.4284601262030105,
-            "velocityY": 1.402279807975625,
-            "timestamp": 1.372345323828442
-        },
-        {
-            "x": 6.260473058973373,
-            "y": 4.473603429076733,
-            "heading": -0.9641067919300658,
-            "angularVelocity": 0.5548580515001295,
-            "velocityX": 2.9958419160985765,
-            "velocityY": 2.027515225257119,
-            "timestamp": 1.4641528762814857
-        },
-        {
-            "x": 6.487005993334624,
-            "y": 4.6803674717401,
-            "heading": -0.8309910438969466,
-            "angularVelocity": 1.4499433268434323,
-            "velocityX": 2.4674760224886074,
-            "velocityY": 2.2521463337029823,
-            "timestamp": 1.5559604287345294
-        },
-        {
-            "x": 6.71888754498383,
-            "y": 4.9198952355114685,
-            "heading": -0.7843851972685187,
-            "angularVelocity": 0.507647196588375,
-            "velocityX": 2.5257350343568326,
-            "velocityY": 2.609020253468568,
-            "timestamp": 1.6477679811875732
-        },
-        {
-            "x": 6.906293835583718,
-            "y": 5.2122954621107285,
-            "heading": -0.7843850302000089,
-            "angularVelocity": 0.0000018197686939661552,
-            "velocityX": 2.0412949217411986,
-            "velocityY": 3.1849256274293083,
-            "timestamp": 1.739575533640617
-        },
-        {
-            "x": 7.030998336898649,
-            "y": 5.536437116039629,
-            "heading": -0.7843849409482478,
-            "angularVelocity": 9.721614257680115e-7,
-            "velocityX": 1.3583250830993825,
-            "velocityY": 3.530664365490929,
-            "timestamp": 1.8313830860936606
-        },
-        {
-            "x": 7.139916048661206,
-            "y": 5.86621879705104,
-            "heading": -0.7843848550588985,
-            "angularVelocity": 9.355368597772783e-7,
-            "velocityX": 1.1863698448802928,
-            "velocityY": 3.592097514854049,
-            "timestamp": 1.9231906385467044
-        },
-        {
-            "x": 7.248833621341376,
-            "y": 6.196000523997438,
-            "heading": -0.7843847691695015,
-            "angularVelocity": 9.355373786921671e-7,
-            "velocityX": 1.1863683299462482,
-            "velocityY": 3.592098015194004,
-            "timestamp": 2.0149981909997483
-        },
-        {
-            "x": 7.357751176995467,
-            "y": 6.5257822565670125,
-            "heading": -0.7843846832799689,
-            "angularVelocity": 9.355388566444766e-7,
-            "velocityX": 1.1863681444922285,
-            "velocityY": 3.5920980764436137,
-            "timestamp": 2.1068057434527923
-        },
-        {
-            "x": 7.4669549324183055,
-            "y": 6.834400664448086,
-            "heading": -0.7318192896940902,
-            "angularVelocity": 0.5725606682823186,
-            "velocityX": 1.189485532562175,
-            "velocityY": 3.361579735380924,
-            "timestamp": 2.198613295905836
-        },
-        {
-            "x": 7.555413306559794,
-            "y": 7.089894078912264,
-            "heading": -0.5139853289463429,
-            "angularVelocity": 2.3727237566774426,
-            "velocityX": 0.9635195773978621,
-            "velocityY": 2.782923709842436,
-            "timestamp": 2.29042084835888
-        },
-        {
-            "x": 7.619158448834915,
-            "y": 7.279787689667393,
-            "heading": -0.2782484542424907,
-            "angularVelocity": 2.5677285626846773,
-            "velocityX": 0.6943344046528581,
-            "velocityY": 2.0683876835977384,
-            "timestamp": 2.382228400811924
-        },
-        {
-            "x": 7.660128944224933,
-            "y": 7.404974004983973,
-            "heading": -0.09793823848426357,
-            "angularVelocity": 1.964001990472943,
-            "velocityX": 0.44626497815605787,
-            "velocityY": 1.3635731698719276,
-            "timestamp": 2.474035953264968
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 2.533782828853193e-32,
-            "angularVelocity": 1.0667775783953661,
-            "velocityX": 0.21644249567844834,
-            "velocityY": 0.6756088508922024,
-            "timestamp": 2.565843505718012
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 2.4912680070874024e-31,
-            "velocityX": 0,
-            "velocityY": -3.645397861822831e-32,
-            "timestamp": 2.657651058171056
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C1.traj
deleted file mode 100644
index f8bd91fc..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C1.traj	
+++ /dev/null
@@ -1,292 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 1.1411819845129358e-30,
-            "velocityX": 2.0257584812603577e-32,
-            "velocityY": 3.042151659469263e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 5.128157808100627,
-            "y": 1.6381505638691025,
-            "heading": -0.6351424860741187,
-            "angularVelocity": 0.06199634399134786,
-            "velocityX": -0.6106088487961494,
-            "velocityY": 0.23165524966799825,
-            "timestamp": 0.07835161903352221
-        },
-        {
-            "x": 5.034138141125347,
-            "y": 1.6783506940439978,
-            "heading": -0.625731099901857,
-            "angularVelocity": 0.12011731586854772,
-            "velocityX": -1.199970953185457,
-            "velocityY": 0.5130733821555872,
-            "timestamp": 0.15670323806704442
-        },
-        {
-            "x": 4.897649658060979,
-            "y": 1.7471051706265737,
-            "heading": -0.6123788413002932,
-            "angularVelocity": 0.1704145844880519,
-            "velocityX": -1.74199952404266,
-            "velocityY": 0.8775118808095047,
-            "timestamp": 0.23505485710056662
-        },
-        {
-            "x": 4.7291295198915435,
-            "y": 1.855780594555545,
-            "heading": -0.5966040827715752,
-            "angularVelocity": 0.20133289807283902,
-            "velocityX": -2.150818837544829,
-            "velocityY": 1.3870220586313022,
-            "timestamp": 0.31340647613408884
-        },
-        {
-            "x": 4.559409610033689,
-            "y": 2.0156280635815147,
-            "heading": -0.5822183456142529,
-            "angularVelocity": 0.18360484869071408,
-            "velocityX": -2.1661314973624393,
-            "velocityY": 2.0401297509574126,
-            "timestamp": 0.39175809516761106
-        },
-        {
-            "x": 4.423281260574217,
-            "y": 2.2140742145732073,
-            "heading": -0.5727758831618049,
-            "angularVelocity": 0.1205139417528553,
-            "velocityX": -1.7374031467203144,
-            "velocityY": 2.532763884646578,
-            "timestamp": 0.4701097142011333
-        },
-        {
-            "x": 4.3324586171803325,
-            "y": 2.4362881098121476,
-            "heading": -0.5691499320560302,
-            "angularVelocity": 0.04627793465535804,
-            "velocityX": -1.1591674111421555,
-            "velocityY": 2.8361110846205655,
-            "timestamp": 0.5484613332346555
-        },
-        {
-            "x": 4.290356765831772,
-            "y": 2.674111890908862,
-            "heading": -0.5714790836196856,
-            "angularVelocity": -0.02972691046318988,
-            "velocityX": -0.5373450079001806,
-            "velocityY": 3.0353397164002804,
-            "timestamp": 0.6268129522681777
-        },
-        {
-            "x": 4.298268630722052,
-            "y": 2.9211188280591673,
-            "heading": -0.5947192771376703,
-            "angularVelocity": -0.29661408155511804,
-            "velocityX": 0.1009789585444941,
-            "velocityY": 3.1525441362561364,
-            "timestamp": 0.7051645713016998
-        },
-        {
-            "x": 4.356924477851345,
-            "y": 3.1722484967201265,
-            "heading": -0.6368890523591307,
-            "angularVelocity": -0.5382119188043637,
-            "velocityX": 0.7486232939768359,
-            "velocityY": 3.2051624683532776,
-            "timestamp": 0.783516190335222
-        },
-        {
-            "x": 4.466386404708156,
-            "y": 3.418800428561669,
-            "heading": -0.694172117002664,
-            "angularVelocity": -0.7311025011368958,
-            "velocityX": 1.3970601782967371,
-            "velocityY": 3.1467369134523744,
-            "timestamp": 0.8618678093687442
-        },
-        {
-            "x": 4.622742851027307,
-            "y": 3.644917946871822,
-            "heading": -0.7581397869413499,
-            "angularVelocity": -0.8164179723116849,
-            "velocityX": 1.9955739045067467,
-            "velocityY": 2.8859329404975034,
-            "timestamp": 0.9402194284022664
-        },
-        {
-            "x": 4.8130531116785615,
-            "y": 3.833426042269654,
-            "heading": -0.8482959054101304,
-            "angularVelocity": -1.1506605681014435,
-            "velocityX": 2.4289256941816704,
-            "velocityY": 2.4059246984696085,
-            "timestamp": 1.0185710474357885
-        },
-        {
-            "x": 5.034329891204834,
-            "y": 3.981222629547119,
-            "heading": -0.9454274313666489,
-            "angularVelocity": -1.2396875412997066,
-            "velocityX": 2.824150697276595,
-            "velocityY": 1.8863246107809373,
-            "timestamp": 1.0969226664693108
-        },
-        {
-            "x": 5.3307679727813255,
-            "y": 4.095215339701575,
-            "heading": -1.0325645415374454,
-            "angularVelocity": -0.9491279076997933,
-            "velocityX": 3.2289073573561264,
-            "velocityY": 1.241648503948074,
-            "timestamp": 1.1887302189223545
-        },
-        {
-            "x": 5.6706736122505745,
-            "y": 4.158722341660029,
-            "heading": -1.0366531166779425,
-            "angularVelocity": -0.04453419170049298,
-            "velocityX": 3.7023712144281196,
-            "velocityY": 0.691740496958957,
-            "timestamp": 1.2805377713753983
-        },
-        {
-            "x": 5.985432145120126,
-            "y": 4.2874622186845945,
-            "heading": -1.0150469515971576,
-            "angularVelocity": 0.23534191363870294,
-            "velocityX": 3.4284601262030105,
-            "velocityY": 1.402279807975625,
-            "timestamp": 1.372345323828442
-        },
-        {
-            "x": 6.260473058973373,
-            "y": 4.473603429076733,
-            "heading": -0.9641067919300658,
-            "angularVelocity": 0.5548580515001295,
-            "velocityX": 2.9958419160985765,
-            "velocityY": 2.027515225257119,
-            "timestamp": 1.4641528762814857
-        },
-        {
-            "x": 6.487005993334624,
-            "y": 4.6803674717401,
-            "heading": -0.8309910438969466,
-            "angularVelocity": 1.4499433268434323,
-            "velocityX": 2.4674760224886074,
-            "velocityY": 2.2521463337029823,
-            "timestamp": 1.5559604287345294
-        },
-        {
-            "x": 6.71888754498383,
-            "y": 4.9198952355114685,
-            "heading": -0.7843851972685187,
-            "angularVelocity": 0.507647196588375,
-            "velocityX": 2.5257350343568326,
-            "velocityY": 2.609020253468568,
-            "timestamp": 1.6477679811875732
-        },
-        {
-            "x": 6.906293835583718,
-            "y": 5.2122954621107285,
-            "heading": -0.7843850302000089,
-            "angularVelocity": 0.0000018197686939661552,
-            "velocityX": 2.0412949217411986,
-            "velocityY": 3.1849256274293083,
-            "timestamp": 1.739575533640617
-        },
-        {
-            "x": 7.030998336898649,
-            "y": 5.536437116039629,
-            "heading": -0.7843849409482478,
-            "angularVelocity": 9.721614257680115e-7,
-            "velocityX": 1.3583250830993825,
-            "velocityY": 3.530664365490929,
-            "timestamp": 1.8313830860936606
-        },
-        {
-            "x": 7.139916048661206,
-            "y": 5.86621879705104,
-            "heading": -0.7843848550588985,
-            "angularVelocity": 9.355368597772783e-7,
-            "velocityX": 1.1863698448802928,
-            "velocityY": 3.592097514854049,
-            "timestamp": 1.9231906385467044
-        },
-        {
-            "x": 7.248833621341376,
-            "y": 6.196000523997438,
-            "heading": -0.7843847691695015,
-            "angularVelocity": 9.355373786921671e-7,
-            "velocityX": 1.1863683299462482,
-            "velocityY": 3.592098015194004,
-            "timestamp": 2.0149981909997483
-        },
-        {
-            "x": 7.357751176995467,
-            "y": 6.5257822565670125,
-            "heading": -0.7843846832799689,
-            "angularVelocity": 9.355388566444766e-7,
-            "velocityX": 1.1863681444922285,
-            "velocityY": 3.5920980764436137,
-            "timestamp": 2.1068057434527923
-        },
-        {
-            "x": 7.4669549324183055,
-            "y": 6.834400664448086,
-            "heading": -0.7318192896940902,
-            "angularVelocity": 0.5725606682823186,
-            "velocityX": 1.189485532562175,
-            "velocityY": 3.361579735380924,
-            "timestamp": 2.198613295905836
-        },
-        {
-            "x": 7.555413306559794,
-            "y": 7.089894078912264,
-            "heading": -0.5139853289463429,
-            "angularVelocity": 2.3727237566774426,
-            "velocityX": 0.9635195773978621,
-            "velocityY": 2.782923709842436,
-            "timestamp": 2.29042084835888
-        },
-        {
-            "x": 7.619158448834915,
-            "y": 7.279787689667393,
-            "heading": -0.2782484542424907,
-            "angularVelocity": 2.5677285626846773,
-            "velocityX": 0.6943344046528581,
-            "velocityY": 2.0683876835977384,
-            "timestamp": 2.382228400811924
-        },
-        {
-            "x": 7.660128944224933,
-            "y": 7.404974004983973,
-            "heading": -0.09793823848426357,
-            "angularVelocity": 1.964001990472943,
-            "velocityX": 0.44626497815605787,
-            "velocityY": 1.3635731698719276,
-            "timestamp": 2.474035953264968
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 2.533782828853193e-32,
-            "angularVelocity": 1.0667775783953661,
-            "velocityX": 0.21644249567844834,
-            "velocityY": 0.6756088508922024,
-            "timestamp": 2.565843505718012
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 2.4912680070874024e-31,
-            "velocityX": 0,
-            "velocityY": -3.645397861822831e-32,
-            "timestamp": 2.657651058171056
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C2.1.traj
deleted file mode 100644
index 0e0e8d38..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C2.1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -2.491480567253294e-33,
-            "velocityX": -2.0103104784295923e-34,
-            "velocityY": 1.605618083193578e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 5.240158813928264,
-            "y": 1.6204190327819517,
-            "heading": -0.5237338108165747,
-            "angularVelocity": 1.2526389382742513,
-            "velocityX": 0.6912398963489377,
-            "velocityY": 0.004514612397400031,
-            "timestamp": 0.0928170006782674
-        },
-        {
-            "x": 5.369626464101249,
-            "y": 1.6247166229091008,
-            "heading": -0.2974184918320366,
-            "angularVelocity": 2.4382959730515035,
-            "velocityX": 1.394870004707003,
-            "velocityY": 0.046301756097956205,
-            "timestamp": 0.1856340013565348
-        },
-        {
-            "x": 5.5674551093283755,
-            "y": 1.639197763374398,
-            "heading": 0.0009013327241277373,
-            "angularVelocity": 3.214064475001015,
-            "velocityX": 2.1313837312289596,
-            "velocityY": 0.15601819019657054,
-            "timestamp": 0.2784510020348022
-        },
-        {
-            "x": 5.831585704851482,
-            "y": 1.6774487923340957,
-            "heading": 0.2480008567342784,
-            "angularVelocity": 2.6622226769282764,
-            "velocityX": 2.8457135394696165,
-            "velocityY": 0.41211231434085943,
-            "timestamp": 0.3712680027130696
-        },
-        {
-            "x": 6.141513989428359,
-            "y": 1.75469627200855,
-            "heading": 0.3453177753240257,
-            "angularVelocity": 1.0484816130514494,
-            "velocityX": 3.339132726893265,
-            "velocityY": 0.8322557194259916,
-            "timestamp": 0.46408500339133696
-        },
-        {
-            "x": 6.452443961209774,
-            "y": 1.9008832260175597,
-            "heading": 0.36957097138497697,
-            "angularVelocity": 0.2613012258930921,
-            "velocityX": 3.3499247929718696,
-            "velocityY": 1.5750019171136467,
-            "timestamp": 0.5569020040696043
-        },
-        {
-            "x": 6.733109637262523,
-            "y": 2.111863899724239,
-            "heading": 0.36957109822659895,
-            "angularVelocity": 0.0000013665774702958542,
-            "velocityX": 3.023860650546378,
-            "velocityY": 2.273082217319261,
-            "timestamp": 0.6497190047478717
-        },
-        {
-            "x": 6.964965499466835,
-            "y": 2.3755472934997988,
-            "heading": 0.36957101579118784,
-            "angularVelocity": -8.881499138395027e-7,
-            "velocityX": 2.4979891669630336,
-            "velocityY": 2.840895437782658,
-            "timestamp": 0.7425360054261391
-        },
-        {
-            "x": 7.138307566970259,
-            "y": 2.680897277378083,
-            "heading": 0.36957096673523665,
-            "angularVelocity": -5.285233395413412e-7,
-            "velocityX": 1.867568077364202,
-            "velocityY": 3.289806626446832,
-            "timestamp": 0.8353530061044064
-        },
-        {
-            "x": 7.245881008413397,
-            "y": 3.0151337865477195,
-            "heading": 0.3695709277774119,
-            "angularVelocity": -4.197272531605919e-7,
-            "velocityX": 1.1589842448800958,
-            "velocityY": 3.6010268240427665,
-            "timestamp": 0.9281700067826738
-        },
-        {
-            "x": 7.300266060372569,
-            "y": 3.36201756600148,
-            "heading": 0.3695708905369613,
-            "angularVelocity": -4.012244559973519e-7,
-            "velocityX": 0.5859384763755527,
-            "velocityY": 3.7372871017042186,
-            "timestamp": 1.0209870074609413
-        },
-        {
-            "x": 7.354650396010165,
-            "y": 3.708901457760651,
-            "heading": 0.3695708532965109,
-            "angularVelocity": -4.0122445392101915e-7,
-            "velocityX": 0.5859307588068667,
-            "velocityY": 3.7372883116701696,
-            "timestamp": 1.1138040081392087
-        },
-        {
-            "x": 7.409034640641628,
-            "y": 4.055785363787717,
-            "heading": 0.36957081605605097,
-            "angularVelocity": -4.012245569815441e-7,
-            "velocityX": 0.5859297783169686,
-            "velocityY": 3.737288465390881,
-            "timestamp": 1.206621008817476
-        },
-        {
-            "x": 7.463418795891099,
-            "y": 4.402669283828022,
-            "heading": 0.36957077881557854,
-            "angularVelocity": -4.0122469066112833e-7,
-            "velocityX": 0.5859288153253637,
-            "velocityY": 3.7372886163679513,
-            "timestamp": 1.2994380094957434
-        },
-        {
-            "x": 7.517802854177964,
-            "y": 4.7495532190700205,
-            "heading": 0.3695707415750949,
-            "angularVelocity": -4.0122481249990114e-7,
-            "velocityX": 0.5859277706610763,
-            "velocityY": 3.737288780149305,
-            "timestamp": 1.3922550101740108
-        },
-        {
-            "x": 7.56989445358868,
-            "y": 5.095450165697103,
-            "heading": 0.36620989603022114,
-            "angularVelocity": -0.036209374579162214,
-            "velocityX": 0.5612290747390308,
-            "velocityY": 3.7266550750338174,
-            "timestamp": 1.4850720108522781
-        },
-        {
-            "x": 7.614129631330889,
-            "y": 5.376850109081837,
-            "heading": 0.24917441522367106,
-            "angularVelocity": -1.2609272003114111,
-            "velocityX": 0.4765848650458117,
-            "velocityY": 3.0317715647821197,
-            "timestamp": 1.5778890115305455
-        },
-        {
-            "x": 7.6471559939174325,
-            "y": 5.587299345919279,
-            "heading": 0.1342077337923999,
-            "angularVelocity": -1.2386381868746383,
-            "velocityX": 0.35582234229936965,
-            "velocityY": 2.267356575838128,
-            "timestamp": 1.6707060122088129
-        },
-        {
-            "x": 7.669079055841979,
-            "y": 5.72720580380319,
-            "heading": 0.04709984861251457,
-            "angularVelocity": -0.9384906271840042,
-            "velocityX": 0.23619662092442378,
-            "velocityY": 1.5073365532341503,
-            "timestamp": 1.7635230128870802
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -9.59663427624948e-34,
-            "angularVelocity": -0.5074485090913172,
-            "velocityX": 0.11766103276571077,
-            "velocityY": 0.751954875580805,
-            "timestamp": 1.8563400135653476
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 1.9250732473517817e-32,
-            "velocityX": 0,
-            "velocityY": 2.1529996497623123e-34,
-            "timestamp": 1.949157014243615
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C2.traj
deleted file mode 100644
index 0e0e8d38..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C2.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -2.491480567253294e-33,
-            "velocityX": -2.0103104784295923e-34,
-            "velocityY": 1.605618083193578e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 5.240158813928264,
-            "y": 1.6204190327819517,
-            "heading": -0.5237338108165747,
-            "angularVelocity": 1.2526389382742513,
-            "velocityX": 0.6912398963489377,
-            "velocityY": 0.004514612397400031,
-            "timestamp": 0.0928170006782674
-        },
-        {
-            "x": 5.369626464101249,
-            "y": 1.6247166229091008,
-            "heading": -0.2974184918320366,
-            "angularVelocity": 2.4382959730515035,
-            "velocityX": 1.394870004707003,
-            "velocityY": 0.046301756097956205,
-            "timestamp": 0.1856340013565348
-        },
-        {
-            "x": 5.5674551093283755,
-            "y": 1.639197763374398,
-            "heading": 0.0009013327241277373,
-            "angularVelocity": 3.214064475001015,
-            "velocityX": 2.1313837312289596,
-            "velocityY": 0.15601819019657054,
-            "timestamp": 0.2784510020348022
-        },
-        {
-            "x": 5.831585704851482,
-            "y": 1.6774487923340957,
-            "heading": 0.2480008567342784,
-            "angularVelocity": 2.6622226769282764,
-            "velocityX": 2.8457135394696165,
-            "velocityY": 0.41211231434085943,
-            "timestamp": 0.3712680027130696
-        },
-        {
-            "x": 6.141513989428359,
-            "y": 1.75469627200855,
-            "heading": 0.3453177753240257,
-            "angularVelocity": 1.0484816130514494,
-            "velocityX": 3.339132726893265,
-            "velocityY": 0.8322557194259916,
-            "timestamp": 0.46408500339133696
-        },
-        {
-            "x": 6.452443961209774,
-            "y": 1.9008832260175597,
-            "heading": 0.36957097138497697,
-            "angularVelocity": 0.2613012258930921,
-            "velocityX": 3.3499247929718696,
-            "velocityY": 1.5750019171136467,
-            "timestamp": 0.5569020040696043
-        },
-        {
-            "x": 6.733109637262523,
-            "y": 2.111863899724239,
-            "heading": 0.36957109822659895,
-            "angularVelocity": 0.0000013665774702958542,
-            "velocityX": 3.023860650546378,
-            "velocityY": 2.273082217319261,
-            "timestamp": 0.6497190047478717
-        },
-        {
-            "x": 6.964965499466835,
-            "y": 2.3755472934997988,
-            "heading": 0.36957101579118784,
-            "angularVelocity": -8.881499138395027e-7,
-            "velocityX": 2.4979891669630336,
-            "velocityY": 2.840895437782658,
-            "timestamp": 0.7425360054261391
-        },
-        {
-            "x": 7.138307566970259,
-            "y": 2.680897277378083,
-            "heading": 0.36957096673523665,
-            "angularVelocity": -5.285233395413412e-7,
-            "velocityX": 1.867568077364202,
-            "velocityY": 3.289806626446832,
-            "timestamp": 0.8353530061044064
-        },
-        {
-            "x": 7.245881008413397,
-            "y": 3.0151337865477195,
-            "heading": 0.3695709277774119,
-            "angularVelocity": -4.197272531605919e-7,
-            "velocityX": 1.1589842448800958,
-            "velocityY": 3.6010268240427665,
-            "timestamp": 0.9281700067826738
-        },
-        {
-            "x": 7.300266060372569,
-            "y": 3.36201756600148,
-            "heading": 0.3695708905369613,
-            "angularVelocity": -4.012244559973519e-7,
-            "velocityX": 0.5859384763755527,
-            "velocityY": 3.7372871017042186,
-            "timestamp": 1.0209870074609413
-        },
-        {
-            "x": 7.354650396010165,
-            "y": 3.708901457760651,
-            "heading": 0.3695708532965109,
-            "angularVelocity": -4.0122445392101915e-7,
-            "velocityX": 0.5859307588068667,
-            "velocityY": 3.7372883116701696,
-            "timestamp": 1.1138040081392087
-        },
-        {
-            "x": 7.409034640641628,
-            "y": 4.055785363787717,
-            "heading": 0.36957081605605097,
-            "angularVelocity": -4.012245569815441e-7,
-            "velocityX": 0.5859297783169686,
-            "velocityY": 3.737288465390881,
-            "timestamp": 1.206621008817476
-        },
-        {
-            "x": 7.463418795891099,
-            "y": 4.402669283828022,
-            "heading": 0.36957077881557854,
-            "angularVelocity": -4.0122469066112833e-7,
-            "velocityX": 0.5859288153253637,
-            "velocityY": 3.7372886163679513,
-            "timestamp": 1.2994380094957434
-        },
-        {
-            "x": 7.517802854177964,
-            "y": 4.7495532190700205,
-            "heading": 0.3695707415750949,
-            "angularVelocity": -4.0122481249990114e-7,
-            "velocityX": 0.5859277706610763,
-            "velocityY": 3.737288780149305,
-            "timestamp": 1.3922550101740108
-        },
-        {
-            "x": 7.56989445358868,
-            "y": 5.095450165697103,
-            "heading": 0.36620989603022114,
-            "angularVelocity": -0.036209374579162214,
-            "velocityX": 0.5612290747390308,
-            "velocityY": 3.7266550750338174,
-            "timestamp": 1.4850720108522781
-        },
-        {
-            "x": 7.614129631330889,
-            "y": 5.376850109081837,
-            "heading": 0.24917441522367106,
-            "angularVelocity": -1.2609272003114111,
-            "velocityX": 0.4765848650458117,
-            "velocityY": 3.0317715647821197,
-            "timestamp": 1.5778890115305455
-        },
-        {
-            "x": 7.6471559939174325,
-            "y": 5.587299345919279,
-            "heading": 0.1342077337923999,
-            "angularVelocity": -1.2386381868746383,
-            "velocityX": 0.35582234229936965,
-            "velocityY": 2.267356575838128,
-            "timestamp": 1.6707060122088129
-        },
-        {
-            "x": 7.669079055841979,
-            "y": 5.72720580380319,
-            "heading": 0.04709984861251457,
-            "angularVelocity": -0.9384906271840042,
-            "velocityX": 0.23619662092442378,
-            "velocityY": 1.5073365532341503,
-            "timestamp": 1.7635230128870802
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -9.59663427624948e-34,
-            "angularVelocity": -0.5074485090913172,
-            "velocityX": 0.11766103276571077,
-            "velocityY": 0.751954875580805,
-            "timestamp": 1.8563400135653476
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 1.9250732473517817e-32,
-            "velocityX": 0,
-            "velocityY": 2.1529996497623123e-34,
-            "timestamp": 1.949157014243615
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3.1.traj
deleted file mode 100644
index 4d225c1e..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3.1.traj	
+++ /dev/null
@@ -1,238 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0,
-            "velocityX": 2.653642496841617e-31,
-            "velocityY": -1.2761459725684476e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 5.194122324075795,
-            "y": 1.6116215778070426,
-            "heading": -0.6335243646936302,
-            "angularVelocity": 0.13206571622689564,
-            "velocityX": 0.3695911822570569,
-            "velocityY": -0.1708716250073132,
-            "timestamp": 0.049033431926404414
-        },
-        {
-            "x": 5.2306889078335095,
-            "y": 1.5955898422898989,
-            "heading": -0.6207226842720189,
-            "angularVelocity": 0.2610806529090014,
-            "velocityX": 0.745747999295838,
-            "velocityY": -0.3269551994890449,
-            "timestamp": 0.09806686385280883
-        },
-        {
-            "x": 5.286073830286703,
-            "y": 1.5728623577205654,
-            "heading": -0.6018022859553105,
-            "angularVelocity": 0.38586730672362957,
-            "velocityX": 1.1295338767299938,
-            "velocityY": -0.4635099701654054,
-            "timestamp": 0.14710029577921324
-        },
-        {
-            "x": 5.360700050095615,
-            "y": 1.5447484727758143,
-            "heading": -0.577064216904969,
-            "angularVelocity": 0.5045143298894789,
-            "velocityX": 1.5219456782249914,
-            "velocityY": -0.5733615584348081,
-            "timestamp": 0.19613372770561766
-        },
-        {
-            "x": 5.45500431506412,
-            "y": 1.513111103841015,
-            "heading": -0.5469692052395991,
-            "angularVelocity": 0.6137651492712723,
-            "velocityX": 1.9232646230039372,
-            "velocityY": -0.6452203668359155,
-            "timestamp": 0.24516715963202207
-        },
-        {
-            "x": 5.5692993756365,
-            "y": 1.480706991826158,
-            "heading": -0.5122625268421303,
-            "angularVelocity": 0.7078166270271339,
-            "velocityX": 2.330961878090668,
-            "velocityY": -0.6608575158164777,
-            "timestamp": 0.2942005915584265
-        },
-        {
-            "x": 5.703331364670941,
-            "y": 1.4516882815399292,
-            "heading": -0.47420565589215874,
-            "angularVelocity": 0.7761412867722233,
-            "velocityX": 2.733481703578104,
-            "velocityY": -0.5918147913809916,
-            "timestamp": 0.3432340234848309
-        },
-        {
-            "x": 5.855159622936256,
-            "y": 1.4318923943368613,
-            "heading": -0.4349045247507558,
-            "angularVelocity": 0.8015170384282412,
-            "velocityX": 3.096423242273411,
-            "velocityY": -0.40372224470801127,
-            "timestamp": 0.3922674554112353
-        },
-        {
-            "x": 6.019815732483991,
-            "y": 1.4274954608427641,
-            "heading": -0.3972296573283927,
-            "angularVelocity": 0.768350611865634,
-            "velocityX": 3.358037630221609,
-            "velocityY": -0.08967215471871418,
-            "timestamp": 0.4413008873376397
-        },
-        {
-            "x": 6.190507691154086,
-            "y": 1.4421792499476667,
-            "heading": -0.36375877276285506,
-            "angularVelocity": 0.6826135404074718,
-            "velocityX": 3.4811342376834093,
-            "velocityY": 0.2994648452702519,
-            "timestamp": 0.49033431926404414
-        },
-        {
-            "x": 6.361514712702491,
-            "y": 1.4768435219754552,
-            "heading": -0.33601787986089265,
-            "angularVelocity": 0.5657546659914753,
-            "velocityX": 3.487559708345244,
-            "velocityY": 0.7069517809772359,
-            "timestamp": 0.5393677511904486
-        },
-        {
-            "x": 6.529238224029541,
-            "y": 1.531233787536621,
-            "heading": -0.3133159497925571,
-            "angularVelocity": 0.4629888052692109,
-            "velocityX": 3.4205949846286416,
-            "velocityY": 1.1092485968081267,
-            "timestamp": 0.588401183116853
-        },
-        {
-            "x": 6.794330666624461,
-            "y": 1.6764087741891647,
-            "heading": -0.29089151826229404,
-            "angularVelocity": 0.27241251742532613,
-            "velocityX": 3.2203491776466797,
-            "velocityY": 1.7635891250039288,
-            "timestamp": 0.6707191020664587
-        },
-        {
-            "x": 7.034738463760469,
-            "y": 1.8721289187979775,
-            "heading": -0.28740556521383775,
-            "angularVelocity": 0.042347438965477144,
-            "velocityX": 2.9204795286821774,
-            "velocityY": 2.377612883150898,
-            "timestamp": 0.7530370210160644
-        },
-        {
-            "x": 7.236654527584481,
-            "y": 2.109198968129657,
-            "heading": -0.2874055261243408,
-            "angularVelocity": 4.748601214403234e-7,
-            "velocityX": 2.452881054338489,
-            "velocityY": 2.879932490479633,
-            "timestamp": 0.8353549399656701
-        },
-        {
-            "x": 7.392410351066591,
-            "y": 2.3788514084218195,
-            "heading": -0.2874055078665042,
-            "angularVelocity": 2.2179662469221753e-7,
-            "velocityX": 1.8921253776776576,
-            "velocityY": 3.2757441360636306,
-            "timestamp": 0.9176728589152758
-        },
-        {
-            "x": 7.49687844031107,
-            "y": 2.6722091028580075,
-            "heading": -0.28740549372573876,
-            "angularVelocity": 1.7178234776742224e-7,
-            "velocityX": 1.2690807855393922,
-            "velocityY": 3.5637161164844247,
-            "timestamp": 0.9999907778648816
-        },
-        {
-            "x": 7.546620338444338,
-            "y": 2.979614475892834,
-            "heading": -0.28740548096651763,
-            "angularVelocity": 1.5499931641741705e-7,
-            "velocityX": 0.6042657390764874,
-            "velocityY": 3.7343676438541644,
-            "timestamp": 1.0823086968144873
-        },
-        {
-            "x": 7.582493232504898,
-            "y": 3.288945132948743,
-            "heading": -0.2874054683444637,
-            "angularVelocity": 1.5333300623146833e-7,
-            "velocityX": 0.43578475401492967,
-            "velocityY": 3.7577560390622633,
-            "timestamp": 1.164626615764093
-        },
-        {
-            "x": 7.6150468277369106,
-            "y": 3.5683420563080905,
-            "heading": -0.1936159509616472,
-            "angularVelocity": 1.1393572454157397,
-            "velocityX": 0.39546183440153926,
-            "velocityY": 3.3941203437178378,
-            "timestamp": 1.2469445347136987
-        },
-        {
-            "x": 7.641044736890772,
-            "y": 3.791820131624945,
-            "heading": -0.11680359430817472,
-            "angularVelocity": 0.9331183007718732,
-            "velocityX": 0.31582320697043825,
-            "velocityY": 2.714816873027222,
-            "timestamp": 1.3292624536633044
-        },
-        {
-            "x": 7.660527962829294,
-            "y": 3.959415202649684,
-            "heading": -0.05861701943494889,
-            "angularVelocity": 0.7068518691380767,
-            "velocityX": 0.23668268327338735,
-            "velocityY": 2.035948833052459,
-            "timestamp": 1.4115803726129101
-        },
-        {
-            "x": 7.673510674167818,
-            "y": 4.0711397209924485,
-            "heading": -0.019593791766857216,
-            "angularVelocity": 0.4740550801809447,
-            "velocityX": 0.1577142802460941,
-            "velocityY": 1.3572320555281903,
-            "timestamp": 1.4938982915625159
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -2.071552209824535e-33,
-            "angularVelocity": 0.23802583953553896,
-            "velocityX": 0.07883248161483059,
-            "velocityY": 0.6785919726876813,
-            "timestamp": 1.5762162105121216
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -4.785738309812717e-31,
-            "velocityX": -1.2484093100405624e-30,
-            "velocityY": 4.284452606087597e-31,
-            "timestamp": 1.6585341294617273
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3.traj
deleted file mode 100644
index 4d225c1e..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3.traj	
+++ /dev/null
@@ -1,238 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0,
-            "velocityX": 2.653642496841617e-31,
-            "velocityY": -1.2761459725684476e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 5.194122324075795,
-            "y": 1.6116215778070426,
-            "heading": -0.6335243646936302,
-            "angularVelocity": 0.13206571622689564,
-            "velocityX": 0.3695911822570569,
-            "velocityY": -0.1708716250073132,
-            "timestamp": 0.049033431926404414
-        },
-        {
-            "x": 5.2306889078335095,
-            "y": 1.5955898422898989,
-            "heading": -0.6207226842720189,
-            "angularVelocity": 0.2610806529090014,
-            "velocityX": 0.745747999295838,
-            "velocityY": -0.3269551994890449,
-            "timestamp": 0.09806686385280883
-        },
-        {
-            "x": 5.286073830286703,
-            "y": 1.5728623577205654,
-            "heading": -0.6018022859553105,
-            "angularVelocity": 0.38586730672362957,
-            "velocityX": 1.1295338767299938,
-            "velocityY": -0.4635099701654054,
-            "timestamp": 0.14710029577921324
-        },
-        {
-            "x": 5.360700050095615,
-            "y": 1.5447484727758143,
-            "heading": -0.577064216904969,
-            "angularVelocity": 0.5045143298894789,
-            "velocityX": 1.5219456782249914,
-            "velocityY": -0.5733615584348081,
-            "timestamp": 0.19613372770561766
-        },
-        {
-            "x": 5.45500431506412,
-            "y": 1.513111103841015,
-            "heading": -0.5469692052395991,
-            "angularVelocity": 0.6137651492712723,
-            "velocityX": 1.9232646230039372,
-            "velocityY": -0.6452203668359155,
-            "timestamp": 0.24516715963202207
-        },
-        {
-            "x": 5.5692993756365,
-            "y": 1.480706991826158,
-            "heading": -0.5122625268421303,
-            "angularVelocity": 0.7078166270271339,
-            "velocityX": 2.330961878090668,
-            "velocityY": -0.6608575158164777,
-            "timestamp": 0.2942005915584265
-        },
-        {
-            "x": 5.703331364670941,
-            "y": 1.4516882815399292,
-            "heading": -0.47420565589215874,
-            "angularVelocity": 0.7761412867722233,
-            "velocityX": 2.733481703578104,
-            "velocityY": -0.5918147913809916,
-            "timestamp": 0.3432340234848309
-        },
-        {
-            "x": 5.855159622936256,
-            "y": 1.4318923943368613,
-            "heading": -0.4349045247507558,
-            "angularVelocity": 0.8015170384282412,
-            "velocityX": 3.096423242273411,
-            "velocityY": -0.40372224470801127,
-            "timestamp": 0.3922674554112353
-        },
-        {
-            "x": 6.019815732483991,
-            "y": 1.4274954608427641,
-            "heading": -0.3972296573283927,
-            "angularVelocity": 0.768350611865634,
-            "velocityX": 3.358037630221609,
-            "velocityY": -0.08967215471871418,
-            "timestamp": 0.4413008873376397
-        },
-        {
-            "x": 6.190507691154086,
-            "y": 1.4421792499476667,
-            "heading": -0.36375877276285506,
-            "angularVelocity": 0.6826135404074718,
-            "velocityX": 3.4811342376834093,
-            "velocityY": 0.2994648452702519,
-            "timestamp": 0.49033431926404414
-        },
-        {
-            "x": 6.361514712702491,
-            "y": 1.4768435219754552,
-            "heading": -0.33601787986089265,
-            "angularVelocity": 0.5657546659914753,
-            "velocityX": 3.487559708345244,
-            "velocityY": 0.7069517809772359,
-            "timestamp": 0.5393677511904486
-        },
-        {
-            "x": 6.529238224029541,
-            "y": 1.531233787536621,
-            "heading": -0.3133159497925571,
-            "angularVelocity": 0.4629888052692109,
-            "velocityX": 3.4205949846286416,
-            "velocityY": 1.1092485968081267,
-            "timestamp": 0.588401183116853
-        },
-        {
-            "x": 6.794330666624461,
-            "y": 1.6764087741891647,
-            "heading": -0.29089151826229404,
-            "angularVelocity": 0.27241251742532613,
-            "velocityX": 3.2203491776466797,
-            "velocityY": 1.7635891250039288,
-            "timestamp": 0.6707191020664587
-        },
-        {
-            "x": 7.034738463760469,
-            "y": 1.8721289187979775,
-            "heading": -0.28740556521383775,
-            "angularVelocity": 0.042347438965477144,
-            "velocityX": 2.9204795286821774,
-            "velocityY": 2.377612883150898,
-            "timestamp": 0.7530370210160644
-        },
-        {
-            "x": 7.236654527584481,
-            "y": 2.109198968129657,
-            "heading": -0.2874055261243408,
-            "angularVelocity": 4.748601214403234e-7,
-            "velocityX": 2.452881054338489,
-            "velocityY": 2.879932490479633,
-            "timestamp": 0.8353549399656701
-        },
-        {
-            "x": 7.392410351066591,
-            "y": 2.3788514084218195,
-            "heading": -0.2874055078665042,
-            "angularVelocity": 2.2179662469221753e-7,
-            "velocityX": 1.8921253776776576,
-            "velocityY": 3.2757441360636306,
-            "timestamp": 0.9176728589152758
-        },
-        {
-            "x": 7.49687844031107,
-            "y": 2.6722091028580075,
-            "heading": -0.28740549372573876,
-            "angularVelocity": 1.7178234776742224e-7,
-            "velocityX": 1.2690807855393922,
-            "velocityY": 3.5637161164844247,
-            "timestamp": 0.9999907778648816
-        },
-        {
-            "x": 7.546620338444338,
-            "y": 2.979614475892834,
-            "heading": -0.28740548096651763,
-            "angularVelocity": 1.5499931641741705e-7,
-            "velocityX": 0.6042657390764874,
-            "velocityY": 3.7343676438541644,
-            "timestamp": 1.0823086968144873
-        },
-        {
-            "x": 7.582493232504898,
-            "y": 3.288945132948743,
-            "heading": -0.2874054683444637,
-            "angularVelocity": 1.5333300623146833e-7,
-            "velocityX": 0.43578475401492967,
-            "velocityY": 3.7577560390622633,
-            "timestamp": 1.164626615764093
-        },
-        {
-            "x": 7.6150468277369106,
-            "y": 3.5683420563080905,
-            "heading": -0.1936159509616472,
-            "angularVelocity": 1.1393572454157397,
-            "velocityX": 0.39546183440153926,
-            "velocityY": 3.3941203437178378,
-            "timestamp": 1.2469445347136987
-        },
-        {
-            "x": 7.641044736890772,
-            "y": 3.791820131624945,
-            "heading": -0.11680359430817472,
-            "angularVelocity": 0.9331183007718732,
-            "velocityX": 0.31582320697043825,
-            "velocityY": 2.714816873027222,
-            "timestamp": 1.3292624536633044
-        },
-        {
-            "x": 7.660527962829294,
-            "y": 3.959415202649684,
-            "heading": -0.05861701943494889,
-            "angularVelocity": 0.7068518691380767,
-            "velocityX": 0.23668268327338735,
-            "velocityY": 2.035948833052459,
-            "timestamp": 1.4115803726129101
-        },
-        {
-            "x": 7.673510674167818,
-            "y": 4.0711397209924485,
-            "heading": -0.019593791766857216,
-            "angularVelocity": 0.4740550801809447,
-            "velocityX": 0.1577142802460941,
-            "velocityY": 1.3572320555281903,
-            "timestamp": 1.4938982915625159
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -2.071552209824535e-33,
-            "angularVelocity": 0.23802583953553896,
-            "velocityX": 0.07883248161483059,
-            "velocityY": 0.6785919726876813,
-            "timestamp": 1.5762162105121216
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -4.785738309812717e-31,
-            "velocityX": -1.2484093100405624e-30,
-            "velocityY": 4.284452606087597e-31,
-            "timestamp": 1.6585341294617273
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.1.traj
deleted file mode 100644
index 751754fd..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.222401190948388,
-            "y": 1.635609178560311,
-            "heading": -0.6120137040899031,
-            "angularVelocity": 0.36292240482976423,
-            "velocityX": 0.6017242103081577,
-            "velocityY": 0.20241766322785323,
-            "timestamp": 0.07711371780208913
-        },
-        {
-            "x": 5.315327100770923,
-            "y": 1.6668589219810437,
-            "heading": -0.5590312951883875,
-            "angularVelocity": 0.6870685321838068,
-            "velocityX": 1.205050313629373,
-            "velocityY": 0.4052423396435706,
-            "timestamp": 0.15422743560417826
-        },
-        {
-            "x": 5.454938302762681,
-            "y": 1.713800132384211,
-            "heading": -0.4856728473163301,
-            "angularVelocity": 0.9513021802467544,
-            "velocityX": 1.8104587091763231,
-            "velocityY": 0.6087271077195515,
-            "timestamp": 0.23134115340626737
-        },
-        {
-            "x": 5.641435450684747,
-            "y": 1.7765099385015581,
-            "heading": -0.4002091399727602,
-            "angularVelocity": 1.1082815065785667,
-            "velocityX": 2.418469154874715,
-            "velocityY": 0.8132120704942623,
-            "timestamp": 0.3084548712083565
-        },
-        {
-            "x": 5.8748863814192855,
-            "y": 1.8550365409977465,
-            "heading": -0.32154781798640775,
-            "angularVelocity": 1.0200691164734093,
-            "velocityX": 3.0273592998548686,
-            "velocityY": 1.0183220928048784,
-            "timestamp": 0.38556858901044566
-        },
-        {
-            "x": 6.151206632813102,
-            "y": 1.945953456728306,
-            "heading": -0.3195100296755088,
-            "angularVelocity": 0.026425756259478697,
-            "velocityX": 3.583282705976986,
-            "velocityY": 1.178997956808355,
-            "timestamp": 0.4626823068125348
-        },
-        {
-            "x": 6.4277763897122595,
-            "y": 2.0387313847088295,
-            "heading": -0.31950999788997536,
-            "angularVelocity": 4.121903903341971e-7,
-            "velocityX": 3.5865182587742526,
-            "velocityY": 1.2031313056210848,
-            "timestamp": 0.539796024614624
-        },
-        {
-            "x": 6.7043461469693755,
-            "y": 2.1315093116222874,
-            "heading": -0.31950996610444216,
-            "angularVelocity": 4.121903870025697e-7,
-            "velocityX": 3.5865182634161936,
-            "velocityY": 1.2031312917835268,
-            "timestamp": 0.616909742416713
-        },
-        {
-            "x": 6.980831721332989,
-            "y": 2.2223162260189957,
-            "heading": -0.31777519026878787,
-            "angularVelocity": 0.022496332495664188,
-            "velocityX": 3.5854265913259273,
-            "velocityY": 1.1775714747635835,
-            "timestamp": 0.6940234602188021
-        },
-        {
-            "x": 7.214355031558839,
-            "y": 2.3007720927806434,
-            "heading": -0.23870969056825803,
-            "angularVelocity": 1.0253104370281303,
-            "velocityX": 3.028297907062184,
-            "velocityY": 1.0174048016074528,
-            "timestamp": 0.7711371780208912
-        },
-        {
-            "x": 7.400917361819277,
-            "y": 2.3634042679352882,
-            "heading": -0.15373574527352316,
-            "angularVelocity": 1.1019303402387481,
-            "velocityX": 2.419314430400663,
-            "velocityY": 0.8122053629341165,
-            "timestamp": 0.8482508958229803
-        },
-        {
-            "x": 7.540592119323217,
-            "y": 2.4102654434053137,
-            "heading": -0.08076255152981597,
-            "angularVelocity": 0.9463062581289249,
-            "velocityX": 1.8112828882458099,
-            "velocityY": 0.6076892258040765,
-            "timestamp": 0.9253646136250694
-        },
-        {
-            "x": 7.633568850123268,
-            "y": 2.4414404537843537,
-            "heading": -0.02793953661151031,
-            "angularVelocity": 0.6850015331107969,
-            "velocityX": 1.2057093530190521,
-            "velocityY": 0.4042732119212608,
-            "timestamp": 1.0024783314271586
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -3.3818227179407965e-32,
-            "angularVelocity": 0.3623160367292381,
-            "velocityX": 0.6021127135368662,
-            "velocityY": 0.20177403786417583,
-            "timestamp": 1.0795920492292477
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -1.7311819232040874e-32,
-            "angularVelocity": 2.140534191184349e-31,
-            "velocityX": 0,
-            "velocityY": -2.0522160776390996e-31,
-            "timestamp": 1.1567057670313368
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.traj
deleted file mode 100644
index 751754fd..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.222401190948388,
-            "y": 1.635609178560311,
-            "heading": -0.6120137040899031,
-            "angularVelocity": 0.36292240482976423,
-            "velocityX": 0.6017242103081577,
-            "velocityY": 0.20241766322785323,
-            "timestamp": 0.07711371780208913
-        },
-        {
-            "x": 5.315327100770923,
-            "y": 1.6668589219810437,
-            "heading": -0.5590312951883875,
-            "angularVelocity": 0.6870685321838068,
-            "velocityX": 1.205050313629373,
-            "velocityY": 0.4052423396435706,
-            "timestamp": 0.15422743560417826
-        },
-        {
-            "x": 5.454938302762681,
-            "y": 1.713800132384211,
-            "heading": -0.4856728473163301,
-            "angularVelocity": 0.9513021802467544,
-            "velocityX": 1.8104587091763231,
-            "velocityY": 0.6087271077195515,
-            "timestamp": 0.23134115340626737
-        },
-        {
-            "x": 5.641435450684747,
-            "y": 1.7765099385015581,
-            "heading": -0.4002091399727602,
-            "angularVelocity": 1.1082815065785667,
-            "velocityX": 2.418469154874715,
-            "velocityY": 0.8132120704942623,
-            "timestamp": 0.3084548712083565
-        },
-        {
-            "x": 5.8748863814192855,
-            "y": 1.8550365409977465,
-            "heading": -0.32154781798640775,
-            "angularVelocity": 1.0200691164734093,
-            "velocityX": 3.0273592998548686,
-            "velocityY": 1.0183220928048784,
-            "timestamp": 0.38556858901044566
-        },
-        {
-            "x": 6.151206632813102,
-            "y": 1.945953456728306,
-            "heading": -0.3195100296755088,
-            "angularVelocity": 0.026425756259478697,
-            "velocityX": 3.583282705976986,
-            "velocityY": 1.178997956808355,
-            "timestamp": 0.4626823068125348
-        },
-        {
-            "x": 6.4277763897122595,
-            "y": 2.0387313847088295,
-            "heading": -0.31950999788997536,
-            "angularVelocity": 4.121903903341971e-7,
-            "velocityX": 3.5865182587742526,
-            "velocityY": 1.2031313056210848,
-            "timestamp": 0.539796024614624
-        },
-        {
-            "x": 6.7043461469693755,
-            "y": 2.1315093116222874,
-            "heading": -0.31950996610444216,
-            "angularVelocity": 4.121903870025697e-7,
-            "velocityX": 3.5865182634161936,
-            "velocityY": 1.2031312917835268,
-            "timestamp": 0.616909742416713
-        },
-        {
-            "x": 6.980831721332989,
-            "y": 2.2223162260189957,
-            "heading": -0.31777519026878787,
-            "angularVelocity": 0.022496332495664188,
-            "velocityX": 3.5854265913259273,
-            "velocityY": 1.1775714747635835,
-            "timestamp": 0.6940234602188021
-        },
-        {
-            "x": 7.214355031558839,
-            "y": 2.3007720927806434,
-            "heading": -0.23870969056825803,
-            "angularVelocity": 1.0253104370281303,
-            "velocityX": 3.028297907062184,
-            "velocityY": 1.0174048016074528,
-            "timestamp": 0.7711371780208912
-        },
-        {
-            "x": 7.400917361819277,
-            "y": 2.3634042679352882,
-            "heading": -0.15373574527352316,
-            "angularVelocity": 1.1019303402387481,
-            "velocityX": 2.419314430400663,
-            "velocityY": 0.8122053629341165,
-            "timestamp": 0.8482508958229803
-        },
-        {
-            "x": 7.540592119323217,
-            "y": 2.4102654434053137,
-            "heading": -0.08076255152981597,
-            "angularVelocity": 0.9463062581289249,
-            "velocityX": 1.8112828882458099,
-            "velocityY": 0.6076892258040765,
-            "timestamp": 0.9253646136250694
-        },
-        {
-            "x": 7.633568850123268,
-            "y": 2.4414404537843537,
-            "heading": -0.02793953661151031,
-            "angularVelocity": 0.6850015331107969,
-            "velocityX": 1.2057093530190521,
-            "velocityY": 0.4042732119212608,
-            "timestamp": 1.0024783314271586
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -3.3818227179407965e-32,
-            "angularVelocity": 0.3623160367292381,
-            "velocityX": 0.6021127135368662,
-            "velocityY": 0.20177403786417583,
-            "timestamp": 1.0795920492292477
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -1.7311819232040874e-32,
-            "angularVelocity": 2.140534191184349e-31,
-            "velocityX": 0,
-            "velocityY": -2.0522160776390996e-31,
-            "timestamp": 1.1567057670313368
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.1.traj
deleted file mode 100644
index 0bd0e944..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.22245562506892,
-            "y": 1.6045082039129626,
-            "heading": -0.6133597798196372,
-            "angularVelocity": 0.3456549186542876,
-            "velocityX": 0.6027583554308628,
-            "velocityY": -0.20100492713723658,
-            "timestamp": 0.07707172310487943
-        },
-        {
-            "x": 5.31546298986369,
-            "y": 1.573504853521539,
-            "heading": -0.5625082909022232,
-            "angularVelocity": 0.6597943690478437,
-            "velocityX": 1.2067637915426601,
-            "velocityY": -0.4022662157070755,
-            "timestamp": 0.15414344620975887
-        },
-        {
-            "x": 5.455153171011431,
-            "y": 1.526958597742617,
-            "heading": -0.4912836919139272,
-            "angularVelocity": 0.924133989989731,
-            "velocityX": 1.8124699373549893,
-            "velocityY": -0.6039342822993798,
-            "timestamp": 0.2312151693146383
-        },
-        {
-            "x": 5.641701159953251,
-            "y": 1.4648201004985182,
-            "heading": -0.406509898431932,
-            "angularVelocity": 1.0999338027856826,
-            "velocityX": 2.420446584384324,
-            "velocityY": -0.8062424809101578,
-            "timestamp": 0.30828689241951773
-        },
-        {
-            "x": 5.875244890448534,
-            "y": 1.3870507382512116,
-            "heading": -0.323292875073049,
-            "angularVelocity": 1.0797348236997513,
-            "velocityX": 3.03021291190643,
-            "velocityY": -1.0090518171168696,
-            "timestamp": 0.38535861552439715
-        },
-        {
-            "x": 6.151315012657669,
-            "y": 1.295160148354896,
-            "heading": -0.32123146568205607,
-            "angularVelocity": 0.026746636872095383,
-            "velocityX": 3.5819897504232276,
-            "velocityY": -1.192273718485189,
-            "timestamp": 0.46243033862927657
-        },
-        {
-            "x": 6.427941847872596,
-            "y": 1.2030541885235448,
-            "heading": -0.3212314367193528,
-            "angularVelocity": 3.757889680371681e-7,
-            "velocityX": 3.5892130611702004,
-            "velocityY": -1.1950681277232245,
-            "timestamp": 0.539502061734156
-        },
-        {
-            "x": 6.704568684105482,
-            "y": 1.1109482317494896,
-            "heading": -0.3212314077566493,
-            "angularVelocity": 3.7578897090221527e-7,
-            "velocityX": 3.589213074378161,
-            "velocityY": -1.1950680880550335,
-            "timestamp": 0.6165737848390355
-        },
-        {
-            "x": 6.9807077062893965,
-            "y": 1.0195126541212949,
-            "heading": -0.3189025007582472,
-            "angularVelocity": 0.030217398866675224,
-            "velocityX": 3.582883722583232,
-            "velocityY": -1.1863699674103443,
-            "timestamp": 0.6936455079439149
-        },
-        {
-            "x": 7.214256981896637,
-            "y": 0.9418352430707998,
-            "heading": -0.2350676331360113,
-            "angularVelocity": 1.0877513080660361,
-            "velocityX": 3.0302848593306506,
-            "velocityY": -1.0078587570280646,
-            "timestamp": 0.7707172310487943
-        },
-        {
-            "x": 7.4008131962231865,
-            "y": 0.8797969152412516,
-            "heading": -0.14969687190170503,
-            "angularVelocity": 1.1076794159400525,
-            "velocityX": 2.4205533081527695,
-            "velocityY": -0.8049427900441033,
-            "timestamp": 0.8477889541536737
-        },
-        {
-            "x": 7.54051565231881,
-            "y": 0.8333524229780926,
-            "heading": -0.0780215306756512,
-            "angularVelocity": 0.9299823377312819,
-            "velocityX": 1.812629203910703,
-            "velocityY": -0.6026139080860716,
-            "timestamp": 0.9248606772585531
-        },
-        {
-            "x": 7.633536087871107,
-            "y": 0.802437413208921,
-            "heading": -0.026843150662094223,
-            "angularVelocity": 0.6640357572376224,
-            "velocityX": 1.2069333836706095,
-            "velocityY": -0.401120002560503,
-            "timestamp": 1.0019324003634327
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 3.8658238709310755e-33,
-            "angularVelocity": 0.3482879268907221,
-            "velocityX": 0.6028658794310917,
-            "velocityY": -0.20029931325025216,
-            "timestamp": 1.079004123468312
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 4.381697144346615e-34,
-            "angularVelocity": -4.4473487408728983e-32,
-            "velocityX": 0,
-            "velocityY": -1.5918634852021244e-32,
-            "timestamp": 1.1560758465731915
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.traj
deleted file mode 100644
index 0bd0e944..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.22245562506892,
-            "y": 1.6045082039129626,
-            "heading": -0.6133597798196372,
-            "angularVelocity": 0.3456549186542876,
-            "velocityX": 0.6027583554308628,
-            "velocityY": -0.20100492713723658,
-            "timestamp": 0.07707172310487943
-        },
-        {
-            "x": 5.31546298986369,
-            "y": 1.573504853521539,
-            "heading": -0.5625082909022232,
-            "angularVelocity": 0.6597943690478437,
-            "velocityX": 1.2067637915426601,
-            "velocityY": -0.4022662157070755,
-            "timestamp": 0.15414344620975887
-        },
-        {
-            "x": 5.455153171011431,
-            "y": 1.526958597742617,
-            "heading": -0.4912836919139272,
-            "angularVelocity": 0.924133989989731,
-            "velocityX": 1.8124699373549893,
-            "velocityY": -0.6039342822993798,
-            "timestamp": 0.2312151693146383
-        },
-        {
-            "x": 5.641701159953251,
-            "y": 1.4648201004985182,
-            "heading": -0.406509898431932,
-            "angularVelocity": 1.0999338027856826,
-            "velocityX": 2.420446584384324,
-            "velocityY": -0.8062424809101578,
-            "timestamp": 0.30828689241951773
-        },
-        {
-            "x": 5.875244890448534,
-            "y": 1.3870507382512116,
-            "heading": -0.323292875073049,
-            "angularVelocity": 1.0797348236997513,
-            "velocityX": 3.03021291190643,
-            "velocityY": -1.0090518171168696,
-            "timestamp": 0.38535861552439715
-        },
-        {
-            "x": 6.151315012657669,
-            "y": 1.295160148354896,
-            "heading": -0.32123146568205607,
-            "angularVelocity": 0.026746636872095383,
-            "velocityX": 3.5819897504232276,
-            "velocityY": -1.192273718485189,
-            "timestamp": 0.46243033862927657
-        },
-        {
-            "x": 6.427941847872596,
-            "y": 1.2030541885235448,
-            "heading": -0.3212314367193528,
-            "angularVelocity": 3.757889680371681e-7,
-            "velocityX": 3.5892130611702004,
-            "velocityY": -1.1950681277232245,
-            "timestamp": 0.539502061734156
-        },
-        {
-            "x": 6.704568684105482,
-            "y": 1.1109482317494896,
-            "heading": -0.3212314077566493,
-            "angularVelocity": 3.7578897090221527e-7,
-            "velocityX": 3.589213074378161,
-            "velocityY": -1.1950680880550335,
-            "timestamp": 0.6165737848390355
-        },
-        {
-            "x": 6.9807077062893965,
-            "y": 1.0195126541212949,
-            "heading": -0.3189025007582472,
-            "angularVelocity": 0.030217398866675224,
-            "velocityX": 3.582883722583232,
-            "velocityY": -1.1863699674103443,
-            "timestamp": 0.6936455079439149
-        },
-        {
-            "x": 7.214256981896637,
-            "y": 0.9418352430707998,
-            "heading": -0.2350676331360113,
-            "angularVelocity": 1.0877513080660361,
-            "velocityX": 3.0302848593306506,
-            "velocityY": -1.0078587570280646,
-            "timestamp": 0.7707172310487943
-        },
-        {
-            "x": 7.4008131962231865,
-            "y": 0.8797969152412516,
-            "heading": -0.14969687190170503,
-            "angularVelocity": 1.1076794159400525,
-            "velocityX": 2.4205533081527695,
-            "velocityY": -0.8049427900441033,
-            "timestamp": 0.8477889541536737
-        },
-        {
-            "x": 7.54051565231881,
-            "y": 0.8333524229780926,
-            "heading": -0.0780215306756512,
-            "angularVelocity": 0.9299823377312819,
-            "velocityX": 1.812629203910703,
-            "velocityY": -0.6026139080860716,
-            "timestamp": 0.9248606772585531
-        },
-        {
-            "x": 7.633536087871107,
-            "y": 0.802437413208921,
-            "heading": -0.026843150662094223,
-            "angularVelocity": 0.6640357572376224,
-            "velocityX": 1.2069333836706095,
-            "velocityY": -0.401120002560503,
-            "timestamp": 1.0019324003634327
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 3.8658238709310755e-33,
-            "angularVelocity": 0.3482879268907221,
-            "velocityX": 0.6028658794310917,
-            "velocityY": -0.20029931325025216,
-            "timestamp": 1.079004123468312
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 4.381697144346615e-34,
-            "angularVelocity": -4.4473487408728983e-32,
-            "velocityX": 0,
-            "velocityY": -1.5918634852021244e-32,
-            "timestamp": 1.1560758465731915
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W1.1.traj
deleted file mode 100644
index 90e7d73f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W1.1.traj	
+++ /dev/null
@@ -1,247 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0,
-            "velocityX": -4.2485019622555225e-35,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.123544064624381,
-            "y": 1.640479835564366,
-            "heading": -0.5261105113915723,
-            "angularVelocity": 1.3044458865146935,
-            "velocityX": -0.6008098724480858,
-            "velocityY": 0.23456806756139406,
-            "timestamp": 0.08730871076049408
-        },
-        {
-            "x": 5.016418569113309,
-            "y": 1.6815339921073649,
-            "heading": -0.31680443246750034,
-            "angularVelocity": 2.397310384049101,
-            "velocityX": -1.2269737415426907,
-            "velocityY": 0.4702183342922002,
-            "timestamp": 0.17461742152098816
-        },
-        {
-            "x": 4.8522086939147036,
-            "y": 1.7459427773647251,
-            "heading": -0.051891218250848345,
-            "angularVelocity": 3.0342128741697256,
-            "velocityX": -1.8807960141464901,
-            "velocityY": 0.7377131639710823,
-            "timestamp": 0.26192613228148226
-        },
-        {
-            "x": 4.632595451769563,
-            "y": 1.839668047804236,
-            "heading": 0.17554509517769362,
-            "angularVelocity": 2.6049670353333583,
-            "velocityX": -2.515364620920639,
-            "velocityY": 1.073492777789593,
-            "timestamp": 0.34923484304197633
-        },
-        {
-            "x": 4.3704901470738005,
-            "y": 1.9627849470607086,
-            "heading": 0.27538033292006053,
-            "angularVelocity": 1.1434739658020574,
-            "velocityX": -3.002052171114658,
-            "velocityY": 1.4101330575617794,
-            "timestamp": 0.4365435538024704
-        },
-        {
-            "x": 4.075718453010027,
-            "y": 2.1117712847634245,
-            "heading": 0.2753804447713452,
-            "angularVelocity": 0.0000012811010915301473,
-            "velocityX": -3.3762002839830365,
-            "velocityY": 1.7064315393617047,
-            "timestamp": 0.5238522645629645
-        },
-        {
-            "x": 3.780946849777969,
-            "y": 2.260757802178556,
-            "heading": 0.2753805566219007,
-            "angularVelocity": 0.0000012810927397041138,
-            "velocityX": -3.3761992436319233,
-            "velocityY": 1.7064335977177825,
-            "timestamp": 0.6111609753234586
-        },
-        {
-            "x": 3.4861752863833098,
-            "y": 2.409744398412427,
-            "heading": 0.27538066847245973,
-            "angularVelocity": 0.0000012810927810381003,
-            "velocityX": -3.3761987873498507,
-            "velocityY": 1.7064345004769594,
-            "timestamp": 0.6984696860839527
-        },
-        {
-            "x": 3.191403823869153,
-            "y": 2.5587311942392557,
-            "heading": 0.2753807803230405,
-            "angularVelocity": 0.000001281093028963251,
-            "velocityX": -3.3761976319038296,
-            "velocityY": 1.7064367865370367,
-            "timestamp": 0.7857783968444467
-        },
-        {
-            "x": 2.907963654667952,
-            "y": 2.72828634801538,
-            "heading": 0.27538089480545036,
-            "angularVelocity": 0.0000013112369766496227,
-            "velocityX": -3.246413407463273,
-            "velocityY": 1.9420187550501031,
-            "timestamp": 0.8730871076049408
-        },
-        {
-            "x": 2.662249448997808,
-            "y": 2.9489938499602837,
-            "heading": 0.27538103946019826,
-            "angularVelocity": 0.000001656819194794751,
-            "velocityX": -2.81431490088302,
-            "velocityY": 2.527897846875211,
-            "timestamp": 0.9603958183654349
-        },
-        {
-            "x": 2.463361015566044,
-            "y": 3.212680537293485,
-            "heading": 0.27538116270778873,
-            "angularVelocity": 0.0000014116299433562534,
-            "velocityX": -2.277990726232991,
-            "velocityY": 3.0201647125055904,
-            "timestamp": 1.047704529125929
-        },
-        {
-            "x": 2.3186638703555946,
-            "y": 3.509581240155329,
-            "heading": 0.2753811333053821,
-            "angularVelocity": -3.3676372418890626e-7,
-            "velocityX": -1.657304797540575,
-            "velocityY": 3.4005851223287915,
-            "timestamp": 1.1350132398864232
-        },
-        {
-            "x": 2.233516432992889,
-            "y": 3.8287006658490608,
-            "heading": 0.2753811088610493,
-            "angularVelocity": -2.799758765995743e-7,
-            "velocityX": -0.9752456154836949,
-            "velocityY": 3.655069728026828,
-            "timestamp": 1.2223219506469174
-        },
-        {
-            "x": 2.1898375161467403,
-            "y": 4.15608338798651,
-            "heading": 0.27538108549661844,
-            "angularVelocity": -2.6760709935894875e-7,
-            "velocityX": -0.5002813174732252,
-            "velocityY": 3.749714310127982,
-            "timestamp": 1.3096306614074116
-        },
-        {
-            "x": 2.1796859113101297,
-            "y": 4.486211000706444,
-            "heading": 0.2753810837562719,
-            "angularVelocity": -1.9933252293602923e-8,
-            "velocityX": -0.11627253166592438,
-            "velocityY": 3.78115321878412,
-            "timestamp": 1.3969393721679058
-        },
-        {
-            "x": 2.197179274626327,
-            "y": 4.816031069152981,
-            "heading": 0.2753810893961792,
-            "angularVelocity": 6.459730400705482e-8,
-            "velocityX": 0.20036217650934227,
-            "velocityY": 3.7776307263464504,
-            "timestamp": 1.4842480829284
-        },
-        {
-            "x": 2.214672942788018,
-            "y": 5.1458511214306375,
-            "heading": 0.27538109503608554,
-            "angularVelocity": 6.459729260879602e-8,
-            "velocityX": 0.20036566809100828,
-            "velocityY": 3.777630541154376,
-            "timestamp": 1.5715567936888941
-        },
-        {
-            "x": 2.2321666974024836,
-            "y": 5.475671169122818,
-            "heading": 0.2753811006759881,
-            "angularVelocity": 6.459724966098765e-8,
-            "velocityX": 0.20036665828744704,
-            "velocityY": 3.7776304886341237,
-            "timestamp": 1.6588655044493883
-        },
-        {
-            "x": 2.2496605217353887,
-            "y": 5.805491213117091,
-            "heading": 0.2753811063158993,
-            "angularVelocity": 6.459734855412882e-8,
-            "velocityX": 0.20036745681532722,
-            "velocityY": 3.7776304462797263,
-            "timestamp": 1.7461742152098825
-        },
-        {
-            "x": 2.2664262161021513,
-            "y": 6.122693971906954,
-            "heading": 0.30917552472065174,
-            "angularVelocity": 0.3870681185232194,
-            "velocityX": 0.19202773950877436,
-            "velocityY": 3.633116971112041,
-            "timestamp": 1.8334829259703767
-        },
-        {
-            "x": 2.279851444819284,
-            "y": 6.376429629998848,
-            "heading": 0.34011357351362637,
-            "angularVelocity": 0.35435237244361656,
-            "velocityX": 0.15376734578020115,
-            "velocityY": 2.9061894956614607,
-            "timestamp": 1.9207916367308708
-        },
-        {
-            "x": 2.289924296428913,
-            "y": 6.566719611746641,
-            "heading": 0.3645867540299274,
-            "angularVelocity": 0.2803062867740199,
-            "velocityX": 0.11537052284807026,
-            "velocityY": 2.179507406423607,
-            "timestamp": 2.008100347491365
-        },
-        {
-            "x": 2.296641079194263,
-            "y": 6.6935744437684805,
-            "heading": 0.3814115840440414,
-            "angularVelocity": 0.192705056203017,
-            "velocityX": 0.07693141619941274,
-            "velocityY": 1.4529458849739352,
-            "timestamp": 2.095409058251859
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0.09836837448577633,
-            "velocityX": 0.03847177190545204,
-            "velocityY": 0.7264516412973858,
-            "timestamp": 2.182717769012353
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 2.6936232860428826e-33,
-            "velocityX": 0,
-            "velocityY": 5.62663175256514e-35,
-            "timestamp": 2.2700264797728473
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W1.traj
deleted file mode 100644
index 90e7d73f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W1.traj	
+++ /dev/null
@@ -1,247 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0,
-            "velocityX": -4.2485019622555225e-35,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.123544064624381,
-            "y": 1.640479835564366,
-            "heading": -0.5261105113915723,
-            "angularVelocity": 1.3044458865146935,
-            "velocityX": -0.6008098724480858,
-            "velocityY": 0.23456806756139406,
-            "timestamp": 0.08730871076049408
-        },
-        {
-            "x": 5.016418569113309,
-            "y": 1.6815339921073649,
-            "heading": -0.31680443246750034,
-            "angularVelocity": 2.397310384049101,
-            "velocityX": -1.2269737415426907,
-            "velocityY": 0.4702183342922002,
-            "timestamp": 0.17461742152098816
-        },
-        {
-            "x": 4.8522086939147036,
-            "y": 1.7459427773647251,
-            "heading": -0.051891218250848345,
-            "angularVelocity": 3.0342128741697256,
-            "velocityX": -1.8807960141464901,
-            "velocityY": 0.7377131639710823,
-            "timestamp": 0.26192613228148226
-        },
-        {
-            "x": 4.632595451769563,
-            "y": 1.839668047804236,
-            "heading": 0.17554509517769362,
-            "angularVelocity": 2.6049670353333583,
-            "velocityX": -2.515364620920639,
-            "velocityY": 1.073492777789593,
-            "timestamp": 0.34923484304197633
-        },
-        {
-            "x": 4.3704901470738005,
-            "y": 1.9627849470607086,
-            "heading": 0.27538033292006053,
-            "angularVelocity": 1.1434739658020574,
-            "velocityX": -3.002052171114658,
-            "velocityY": 1.4101330575617794,
-            "timestamp": 0.4365435538024704
-        },
-        {
-            "x": 4.075718453010027,
-            "y": 2.1117712847634245,
-            "heading": 0.2753804447713452,
-            "angularVelocity": 0.0000012811010915301473,
-            "velocityX": -3.3762002839830365,
-            "velocityY": 1.7064315393617047,
-            "timestamp": 0.5238522645629645
-        },
-        {
-            "x": 3.780946849777969,
-            "y": 2.260757802178556,
-            "heading": 0.2753805566219007,
-            "angularVelocity": 0.0000012810927397041138,
-            "velocityX": -3.3761992436319233,
-            "velocityY": 1.7064335977177825,
-            "timestamp": 0.6111609753234586
-        },
-        {
-            "x": 3.4861752863833098,
-            "y": 2.409744398412427,
-            "heading": 0.27538066847245973,
-            "angularVelocity": 0.0000012810927810381003,
-            "velocityX": -3.3761987873498507,
-            "velocityY": 1.7064345004769594,
-            "timestamp": 0.6984696860839527
-        },
-        {
-            "x": 3.191403823869153,
-            "y": 2.5587311942392557,
-            "heading": 0.2753807803230405,
-            "angularVelocity": 0.000001281093028963251,
-            "velocityX": -3.3761976319038296,
-            "velocityY": 1.7064367865370367,
-            "timestamp": 0.7857783968444467
-        },
-        {
-            "x": 2.907963654667952,
-            "y": 2.72828634801538,
-            "heading": 0.27538089480545036,
-            "angularVelocity": 0.0000013112369766496227,
-            "velocityX": -3.246413407463273,
-            "velocityY": 1.9420187550501031,
-            "timestamp": 0.8730871076049408
-        },
-        {
-            "x": 2.662249448997808,
-            "y": 2.9489938499602837,
-            "heading": 0.27538103946019826,
-            "angularVelocity": 0.000001656819194794751,
-            "velocityX": -2.81431490088302,
-            "velocityY": 2.527897846875211,
-            "timestamp": 0.9603958183654349
-        },
-        {
-            "x": 2.463361015566044,
-            "y": 3.212680537293485,
-            "heading": 0.27538116270778873,
-            "angularVelocity": 0.0000014116299433562534,
-            "velocityX": -2.277990726232991,
-            "velocityY": 3.0201647125055904,
-            "timestamp": 1.047704529125929
-        },
-        {
-            "x": 2.3186638703555946,
-            "y": 3.509581240155329,
-            "heading": 0.2753811333053821,
-            "angularVelocity": -3.3676372418890626e-7,
-            "velocityX": -1.657304797540575,
-            "velocityY": 3.4005851223287915,
-            "timestamp": 1.1350132398864232
-        },
-        {
-            "x": 2.233516432992889,
-            "y": 3.8287006658490608,
-            "heading": 0.2753811088610493,
-            "angularVelocity": -2.799758765995743e-7,
-            "velocityX": -0.9752456154836949,
-            "velocityY": 3.655069728026828,
-            "timestamp": 1.2223219506469174
-        },
-        {
-            "x": 2.1898375161467403,
-            "y": 4.15608338798651,
-            "heading": 0.27538108549661844,
-            "angularVelocity": -2.6760709935894875e-7,
-            "velocityX": -0.5002813174732252,
-            "velocityY": 3.749714310127982,
-            "timestamp": 1.3096306614074116
-        },
-        {
-            "x": 2.1796859113101297,
-            "y": 4.486211000706444,
-            "heading": 0.2753810837562719,
-            "angularVelocity": -1.9933252293602923e-8,
-            "velocityX": -0.11627253166592438,
-            "velocityY": 3.78115321878412,
-            "timestamp": 1.3969393721679058
-        },
-        {
-            "x": 2.197179274626327,
-            "y": 4.816031069152981,
-            "heading": 0.2753810893961792,
-            "angularVelocity": 6.459730400705482e-8,
-            "velocityX": 0.20036217650934227,
-            "velocityY": 3.7776307263464504,
-            "timestamp": 1.4842480829284
-        },
-        {
-            "x": 2.214672942788018,
-            "y": 5.1458511214306375,
-            "heading": 0.27538109503608554,
-            "angularVelocity": 6.459729260879602e-8,
-            "velocityX": 0.20036566809100828,
-            "velocityY": 3.777630541154376,
-            "timestamp": 1.5715567936888941
-        },
-        {
-            "x": 2.2321666974024836,
-            "y": 5.475671169122818,
-            "heading": 0.2753811006759881,
-            "angularVelocity": 6.459724966098765e-8,
-            "velocityX": 0.20036665828744704,
-            "velocityY": 3.7776304886341237,
-            "timestamp": 1.6588655044493883
-        },
-        {
-            "x": 2.2496605217353887,
-            "y": 5.805491213117091,
-            "heading": 0.2753811063158993,
-            "angularVelocity": 6.459734855412882e-8,
-            "velocityX": 0.20036745681532722,
-            "velocityY": 3.7776304462797263,
-            "timestamp": 1.7461742152098825
-        },
-        {
-            "x": 2.2664262161021513,
-            "y": 6.122693971906954,
-            "heading": 0.30917552472065174,
-            "angularVelocity": 0.3870681185232194,
-            "velocityX": 0.19202773950877436,
-            "velocityY": 3.633116971112041,
-            "timestamp": 1.8334829259703767
-        },
-        {
-            "x": 2.279851444819284,
-            "y": 6.376429629998848,
-            "heading": 0.34011357351362637,
-            "angularVelocity": 0.35435237244361656,
-            "velocityX": 0.15376734578020115,
-            "velocityY": 2.9061894956614607,
-            "timestamp": 1.9207916367308708
-        },
-        {
-            "x": 2.289924296428913,
-            "y": 6.566719611746641,
-            "heading": 0.3645867540299274,
-            "angularVelocity": 0.2803062867740199,
-            "velocityX": 0.11537052284807026,
-            "velocityY": 2.179507406423607,
-            "timestamp": 2.008100347491365
-        },
-        {
-            "x": 2.296641079194263,
-            "y": 6.6935744437684805,
-            "heading": 0.3814115840440414,
-            "angularVelocity": 0.192705056203017,
-            "velocityX": 0.07693141619941274,
-            "velocityY": 1.4529458849739352,
-            "timestamp": 2.095409058251859
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0.09836837448577633,
-            "velocityX": 0.03847177190545204,
-            "velocityY": 0.7264516412973858,
-            "timestamp": 2.182717769012353
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 2.6936232860428826e-33,
-            "velocityX": 0,
-            "velocityY": 5.62663175256514e-35,
-            "timestamp": 2.2700264797728473
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W2.1.traj
deleted file mode 100644
index 97118e90..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W2.1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -2.256840111784074e-31,
-            "velocityX": -2.1308644711626517e-33,
-            "velocityY": -1.0217798620245829e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 5.1132615869041915,
-            "y": 1.6556053336408048,
-            "heading": -0.6461329738139044,
-            "angularVelocity": -0.06592692141286781,
-            "velocityX": -0.6744118848768044,
-            "velocityY": 0.3827425493802767,
-            "timestamp": 0.09302684976743651
-        },
-        {
-            "x": 4.987784771636742,
-            "y": 1.7268160147391884,
-            "heading": -0.6583988506620269,
-            "angularVelocity": -0.131853081973501,
-            "velocityX": -1.348823652323362,
-            "velocityY": 0.7654852472851404,
-            "timestamp": 0.18605369953487302
-        },
-        {
-            "x": 4.799569566809376,
-            "y": 1.8336320552005005,
-            "heading": -0.6767974581881981,
-            "angularVelocity": -0.19777738977689827,
-            "velocityX": -2.02323528419909,
-            "velocityY": 1.148228073167565,
-            "timestamp": 0.2790805493023095
-        },
-        {
-            "x": 4.54861599428641,
-            "y": 1.9760534609244509,
-            "heading": -0.7013284716868837,
-            "angularVelocity": -0.26369820713065356,
-            "velocityX": -2.697646681042515,
-            "velocityY": 1.5309709624694186,
-            "timestamp": 0.37210739906974605
-        },
-        {
-            "x": 4.242554383646119,
-            "y": 2.1497500028424636,
-            "heading": -0.7013284745367188,
-            "angularVelocity": -3.063454187141868e-8,
-            "velocityX": -3.29003520387323,
-            "velocityY": 1.8671656876724017,
-            "timestamp": 0.4651342488371826
-        },
-        {
-            "x": 3.9364928269084327,
-            "y": 2.3234466397396827,
-            "heading": -0.7013284773864732,
-            "angularVelocity": -3.0633677229161635e-8,
-            "velocityX": -3.290034624442601,
-            "velocityY": 1.8671667086594281,
-            "timestamp": 0.558161098604619
-        },
-        {
-            "x": 3.630431318940711,
-            "y": 2.497143362571822,
-            "heading": -0.7013284802362207,
-            "angularVelocity": -3.063360291364155e-8,
-            "velocityX": -3.2900341001857365,
-            "velocityY": 1.867167632424124,
-            "timestamp": 0.6511879483720555
-        },
-        {
-            "x": 3.3243698648507793,
-            "y": 2.6708401803390394,
-            "heading": -0.7013284830859674,
-            "angularVelocity": -3.063359264364299e-8,
-            "velocityX": -3.2900335210218676,
-            "velocityY": 1.8671686529367946,
-            "timestamp": 0.744214798139492
-        },
-        {
-            "x": 3.0183085209148324,
-            "y": 2.844537192202297,
-            "heading": -0.7013284859357423,
-            "angularVelocity": -3.063389607722874e-8,
-            "velocityX": -3.290032336912287,
-            "velocityY": 1.8671707393886097,
-            "timestamp": 0.8372416479069285
-        },
-        {
-            "x": 2.714360997870643,
-            "y": 3.021907148464231,
-            "heading": -0.7013284887932081,
-            "angularVelocity": -3.071657002046057e-8,
-            "velocityX": -3.2673096402172703,
-            "velocityY": 1.9066533662630898,
-            "timestamp": 0.9302684976743649
-        },
-        {
-            "x": 2.4529780195268613,
-            "y": 3.2575406255854364,
-            "heading": -0.7013284919399162,
-            "angularVelocity": -3.382580562568125e-8,
-            "velocityX": -2.8097584621776335,
-            "velocityY": 2.532962018065539,
-            "timestamp": 1.0232953474418014
-        },
-        {
-            "x": 2.2451503238556145,
-            "y": 3.5415333045009083,
-            "heading": -0.7013284961513817,
-            "angularVelocity": -4.527150531202637e-8,
-            "velocityX": -2.2340614155032408,
-            "velocityY": 3.0528033532839474,
-            "timestamp": 1.1163221972092379
-        },
-        {
-            "x": 2.099615704405981,
-            "y": 3.861945316168419,
-            "heading": -0.7013284332934493,
-            "angularVelocity": 6.756966674200994e-7,
-            "velocityX": -1.564436717071087,
-            "velocityY": 3.444296055047848,
-            "timestamp": 1.2093490469766743
-        },
-        {
-            "x": 2.0224929199626733,
-            "y": 4.205305507968674,
-            "heading": -0.7013282219960775,
-            "angularVelocity": 0.0000022713589924444385,
-            "velocityX": -0.8290379028862299,
-            "velocityY": 3.6909794608614943,
-            "timestamp": 1.3023758967441108
-        },
-        {
-            "x": 2.0170244087221176,
-            "y": 4.5571780060792255,
-            "heading": -0.7013280555552984,
-            "angularVelocity": 0.0000017891692504432307,
-            "velocityX": -0.05878422470745493,
-            "velocityY": 3.782483218449498,
-            "timestamp": 1.3954027465115473
-        },
-        {
-            "x": 2.079587370397571,
-            "y": 4.887709205686539,
-            "heading": -0.661535006789308,
-            "angularVelocity": 0.4277587477752017,
-            "velocityX": 0.6725258549747536,
-            "velocityY": 3.5530731227987227,
-            "timestamp": 1.4884295962789837
-        },
-        {
-            "x": 2.143441139356313,
-            "y": 5.160361503306635,
-            "heading": -0.462273406431507,
-            "angularVelocity": 2.1419794484704924,
-            "velocityX": 0.6864014971846709,
-            "velocityY": 2.9308989641347156,
-            "timestamp": 1.5814564460464202
-        },
-        {
-            "x": 2.1958862911911914,
-            "y": 5.3618445595482385,
-            "heading": -0.25085794958853325,
-            "angularVelocity": 2.272628358065485,
-            "velocityX": 0.5637636012182422,
-            "velocityY": 2.165859176628076,
-            "timestamp": 1.6744832958138567
-        },
-        {
-            "x": 2.231834886718389,
-            "y": 5.494391410801975,
-            "heading": -0.08836645960984135,
-            "angularVelocity": 1.7467160328971074,
-            "velocityX": 0.38643247209883785,
-            "velocityY": 1.4248236029178551,
-            "timestamp": 1.7675101455812932
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -3.8128513304390615e-33,
-            "angularVelocity": 0.9499027413134385,
-            "velocityX": 0.19526742362042082,
-            "velocityY": 0.7052650859622128,
-            "timestamp": 1.8605369953487296
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 5.82518331797089e-32,
-            "velocityX": 0,
-            "velocityY": -4.1086986258215814e-33,
-            "timestamp": 1.953563845116166
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W2.traj
deleted file mode 100644
index 97118e90..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W2.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -2.256840111784074e-31,
-            "velocityX": -2.1308644711626517e-33,
-            "velocityY": -1.0217798620245829e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 5.1132615869041915,
-            "y": 1.6556053336408048,
-            "heading": -0.6461329738139044,
-            "angularVelocity": -0.06592692141286781,
-            "velocityX": -0.6744118848768044,
-            "velocityY": 0.3827425493802767,
-            "timestamp": 0.09302684976743651
-        },
-        {
-            "x": 4.987784771636742,
-            "y": 1.7268160147391884,
-            "heading": -0.6583988506620269,
-            "angularVelocity": -0.131853081973501,
-            "velocityX": -1.348823652323362,
-            "velocityY": 0.7654852472851404,
-            "timestamp": 0.18605369953487302
-        },
-        {
-            "x": 4.799569566809376,
-            "y": 1.8336320552005005,
-            "heading": -0.6767974581881981,
-            "angularVelocity": -0.19777738977689827,
-            "velocityX": -2.02323528419909,
-            "velocityY": 1.148228073167565,
-            "timestamp": 0.2790805493023095
-        },
-        {
-            "x": 4.54861599428641,
-            "y": 1.9760534609244509,
-            "heading": -0.7013284716868837,
-            "angularVelocity": -0.26369820713065356,
-            "velocityX": -2.697646681042515,
-            "velocityY": 1.5309709624694186,
-            "timestamp": 0.37210739906974605
-        },
-        {
-            "x": 4.242554383646119,
-            "y": 2.1497500028424636,
-            "heading": -0.7013284745367188,
-            "angularVelocity": -3.063454187141868e-8,
-            "velocityX": -3.29003520387323,
-            "velocityY": 1.8671656876724017,
-            "timestamp": 0.4651342488371826
-        },
-        {
-            "x": 3.9364928269084327,
-            "y": 2.3234466397396827,
-            "heading": -0.7013284773864732,
-            "angularVelocity": -3.0633677229161635e-8,
-            "velocityX": -3.290034624442601,
-            "velocityY": 1.8671667086594281,
-            "timestamp": 0.558161098604619
-        },
-        {
-            "x": 3.630431318940711,
-            "y": 2.497143362571822,
-            "heading": -0.7013284802362207,
-            "angularVelocity": -3.063360291364155e-8,
-            "velocityX": -3.2900341001857365,
-            "velocityY": 1.867167632424124,
-            "timestamp": 0.6511879483720555
-        },
-        {
-            "x": 3.3243698648507793,
-            "y": 2.6708401803390394,
-            "heading": -0.7013284830859674,
-            "angularVelocity": -3.063359264364299e-8,
-            "velocityX": -3.2900335210218676,
-            "velocityY": 1.8671686529367946,
-            "timestamp": 0.744214798139492
-        },
-        {
-            "x": 3.0183085209148324,
-            "y": 2.844537192202297,
-            "heading": -0.7013284859357423,
-            "angularVelocity": -3.063389607722874e-8,
-            "velocityX": -3.290032336912287,
-            "velocityY": 1.8671707393886097,
-            "timestamp": 0.8372416479069285
-        },
-        {
-            "x": 2.714360997870643,
-            "y": 3.021907148464231,
-            "heading": -0.7013284887932081,
-            "angularVelocity": -3.071657002046057e-8,
-            "velocityX": -3.2673096402172703,
-            "velocityY": 1.9066533662630898,
-            "timestamp": 0.9302684976743649
-        },
-        {
-            "x": 2.4529780195268613,
-            "y": 3.2575406255854364,
-            "heading": -0.7013284919399162,
-            "angularVelocity": -3.382580562568125e-8,
-            "velocityX": -2.8097584621776335,
-            "velocityY": 2.532962018065539,
-            "timestamp": 1.0232953474418014
-        },
-        {
-            "x": 2.2451503238556145,
-            "y": 3.5415333045009083,
-            "heading": -0.7013284961513817,
-            "angularVelocity": -4.527150531202637e-8,
-            "velocityX": -2.2340614155032408,
-            "velocityY": 3.0528033532839474,
-            "timestamp": 1.1163221972092379
-        },
-        {
-            "x": 2.099615704405981,
-            "y": 3.861945316168419,
-            "heading": -0.7013284332934493,
-            "angularVelocity": 6.756966674200994e-7,
-            "velocityX": -1.564436717071087,
-            "velocityY": 3.444296055047848,
-            "timestamp": 1.2093490469766743
-        },
-        {
-            "x": 2.0224929199626733,
-            "y": 4.205305507968674,
-            "heading": -0.7013282219960775,
-            "angularVelocity": 0.0000022713589924444385,
-            "velocityX": -0.8290379028862299,
-            "velocityY": 3.6909794608614943,
-            "timestamp": 1.3023758967441108
-        },
-        {
-            "x": 2.0170244087221176,
-            "y": 4.5571780060792255,
-            "heading": -0.7013280555552984,
-            "angularVelocity": 0.0000017891692504432307,
-            "velocityX": -0.05878422470745493,
-            "velocityY": 3.782483218449498,
-            "timestamp": 1.3954027465115473
-        },
-        {
-            "x": 2.079587370397571,
-            "y": 4.887709205686539,
-            "heading": -0.661535006789308,
-            "angularVelocity": 0.4277587477752017,
-            "velocityX": 0.6725258549747536,
-            "velocityY": 3.5530731227987227,
-            "timestamp": 1.4884295962789837
-        },
-        {
-            "x": 2.143441139356313,
-            "y": 5.160361503306635,
-            "heading": -0.462273406431507,
-            "angularVelocity": 2.1419794484704924,
-            "velocityX": 0.6864014971846709,
-            "velocityY": 2.9308989641347156,
-            "timestamp": 1.5814564460464202
-        },
-        {
-            "x": 2.1958862911911914,
-            "y": 5.3618445595482385,
-            "heading": -0.25085794958853325,
-            "angularVelocity": 2.272628358065485,
-            "velocityX": 0.5637636012182422,
-            "velocityY": 2.165859176628076,
-            "timestamp": 1.6744832958138567
-        },
-        {
-            "x": 2.231834886718389,
-            "y": 5.494391410801975,
-            "heading": -0.08836645960984135,
-            "angularVelocity": 1.7467160328971074,
-            "velocityX": 0.38643247209883785,
-            "velocityY": 1.4248236029178551,
-            "timestamp": 1.7675101455812932
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -3.8128513304390615e-33,
-            "angularVelocity": 0.9499027413134385,
-            "velocityX": 0.19526742362042082,
-            "velocityY": 0.7052650859622128,
-            "timestamp": 1.8605369953487296
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 5.82518331797089e-32,
-            "velocityX": 0,
-            "velocityY": -4.1086986258215814e-33,
-            "timestamp": 1.953563845116166
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.1.traj
deleted file mode 100644
index 8a67252f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.1.traj	
+++ /dev/null
@@ -1,256 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -2.137980318394988e-34,
-            "velocityX": 1.3258659962604387e-36,
-            "velocityY": 7.61509960712835e-36,
-            "timestamp": 0
-        },
-        {
-            "x": 5.139985955005962,
-            "y": 1.6266018860262446,
-            "heading": -0.6327443563347177,
-            "angularVelocity": 0.10940864810395588,
-            "velocityX": -0.5430597418126536,
-            "velocityY": 0.09955056482775779,
-            "timestamp": 0.06631691178916982
-        },
-        {
-            "x": 5.067957879081044,
-            "y": 1.6398056681484225,
-            "heading": -0.6182319813670383,
-            "angularVelocity": 0.2188336968074761,
-            "velocityX": -1.0861192715653594,
-            "velocityY": 0.19910128149746392,
-            "timestamp": 0.13263382357833964
-        },
-        {
-            "x": 4.959915797050431,
-            "y": 1.6596113536414707,
-            "heading": -0.5964609094580124,
-            "angularVelocity": 0.328288385596709,
-            "velocityX": -1.6291784269764191,
-            "velocityY": 0.2986521078667379,
-            "timestamp": 0.19895073536750946
-        },
-        {
-            "x": 4.815859744494909,
-            "y": 1.686018943789264,
-            "heading": -0.5674282989321845,
-            "angularVelocity": 0.43778592432238833,
-            "velocityX": -2.172237045860861,
-            "velocityY": 0.39820295359570684,
-            "timestamp": 0.2652676471566793
-        },
-        {
-            "x": 4.635789767966758,
-            "y": 1.719028429378711,
-            "heading": -0.5311304969514992,
-            "angularVelocity": 0.5473385445944894,
-            "velocityX": -2.7152949627783816,
-            "velocityY": 0.49775366039945246,
-            "timestamp": 0.3315845589458491
-        },
-        {
-            "x": 4.41970592970577,
-            "y": 1.758639783689655,
-            "heading": -0.48756329690665473,
-            "angularVelocity": 0.6569545967904882,
-            "velocityX": -3.258351941175237,
-            "velocityY": 0.5973039642870227,
-            "timestamp": 0.3979014707350189
-        },
-        {
-            "x": 4.172944378919847,
-            "y": 1.8038720825822125,
-            "heading": -0.4875632898742738,
-            "angularVelocity": 1.0604204507812919e-7,
-            "velocityX": -3.7209445393116334,
-            "velocityY": 0.6820628052819626,
-            "timestamp": 0.46421838252418873
-        },
-        {
-            "x": 3.9261828364502485,
-            "y": 1.849104426845956,
-            "heading": -0.4875632828424199,
-            "angularVelocity": 1.0603409772331237e-7,
-            "velocityX": -3.7209444139088643,
-            "velocityY": 0.6820634894390606,
-            "timestamp": 0.5305352943133586
-        },
-        {
-            "x": 3.6794213056437455,
-            "y": 1.8943368347367484,
-            "heading": -0.487563275810563,
-            "angularVelocity": 1.0603414195839125e-7,
-            "velocityX": -3.720944238039768,
-            "velocityY": 0.6820644488783205,
-            "timestamp": 0.5968522061025283
-        },
-        {
-            "x": 3.4326599282156307,
-            "y": 1.9395700793621615,
-            "heading": -0.4875632687787025,
-            "angularVelocity": 1.0603419804191013e-7,
-            "velocityX": -3.720941925230195,
-            "velocityY": 0.6820770660916139,
-            "timestamp": 0.6631691178916981
-        },
-        {
-            "x": 3.192328571865682,
-            "y": 2.011528836252569,
-            "heading": -0.4875632615410612,
-            "angularVelocity": 1.0913718718412795e-7,
-            "velocityX": -3.6239829308396314,
-            "velocityY": 1.0850740022269671,
-            "timestamp": 0.7294860296808678
-        },
-        {
-            "x": 2.965054771843869,
-            "y": 2.1177542675713954,
-            "heading": -0.48756325344928275,
-            "angularVelocity": 1.2201681646141545e-7,
-            "velocityX": -3.4270866041583203,
-            "velocityY": 1.6017849512735258,
-            "timestamp": 0.7958029414700376
-        },
-        {
-            "x": 2.7556945515371734,
-            "y": 2.2559769801698577,
-            "heading": -0.48756324342419455,
-            "angularVelocity": 1.5116940667572282e-7,
-            "velocityX": -3.156965767228115,
-            "velocityY": 2.084275471660842,
-            "timestamp": 0.8621198532592074
-        },
-        {
-            "x": 2.5687211592217127,
-            "y": 2.4232437322251608,
-            "heading": -0.4875632292213124,
-            "angularVelocity": 2.1416682100483036e-7,
-            "velocityX": -2.8193923280063653,
-            "velocityY": 2.5222337340898235,
-            "timestamp": 0.9284367650483771
-        },
-        {
-            "x": 2.4081295337898796,
-            "y": 2.6159806661875145,
-            "heading": -0.48756314249031313,
-            "angularVelocity": 0.0000013078262679985986,
-            "velocityX": -2.421578766248568,
-            "velocityY": 2.906301405817713,
-            "timestamp": 0.9947536768375469
-        },
-        {
-            "x": 2.2783797783684188,
-            "y": 2.8285106690600896,
-            "heading": -0.4828540771785898,
-            "angularVelocity": 0.07100851328382284,
-            "velocityX": -1.9565108193510647,
-            "velocityY": 3.2047632668456583,
-            "timestamp": 1.0610705886267167
-        },
-        {
-            "x": 2.1829307079315186,
-            "y": 3.053825616836548,
-            "heading": -0.47038335932986286,
-            "angularVelocity": 0.18804732476646344,
-            "velocityX": -1.4392870213912456,
-            "velocityY": 3.3975488559051734,
-            "timestamp": 1.1273875004158864
-        },
-        {
-            "x": 2.124568521000596,
-            "y": 3.2749812318761844,
-            "heading": -0.4550563591461243,
-            "angularVelocity": 0.24322153086635045,
-            "velocityX": -0.9261395106595871,
-            "velocityY": 3.5094804335357668,
-            "timestamp": 1.1904041261609186
-        },
-        {
-            "x": 2.099309698866093,
-            "y": 3.4965263857952165,
-            "heading": -0.4375068558264715,
-            "angularVelocity": 0.2784900510328627,
-            "velocityX": -0.40082790590377965,
-            "velocityY": 3.5156619590425686,
-            "timestamp": 1.2534207519059508
-        },
-        {
-            "x": 2.105558929434475,
-            "y": 3.707892405315209,
-            "heading": -0.4199092504656253,
-            "angularVelocity": 0.2792533740547583,
-            "velocityX": 0.09916796550908585,
-            "velocityY": 3.354131025281934,
-            "timestamp": 1.316437377650983
-        },
-        {
-            "x": 2.135680280502732,
-            "y": 3.8963278332771982,
-            "heading": -0.40525850037923283,
-            "angularVelocity": 0.23249023433387317,
-            "velocityX": 0.4779905415775436,
-            "velocityY": 2.990249410122441,
-            "timestamp": 1.3794540033960152
-        },
-        {
-            "x": 2.177481885950485,
-            "y": 4.0538159629335215,
-            "heading": -0.39590861500997965,
-            "angularVelocity": 0.14837172347315433,
-            "velocityX": 0.6633424902324003,
-            "velocityY": 2.4991520538330105,
-            "timestamp": 1.4424706291410474
-        },
-        {
-            "x": 2.2205433619064863,
-            "y": 4.17824561445827,
-            "heading": -0.39150641146352744,
-            "angularVelocity": 0.06985781124276201,
-            "velocityX": 0.6833351587282585,
-            "velocityY": 1.9745527478446014,
-            "timestamp": 1.5054872548860796
-        },
-        {
-            "x": 2.2579292824582606,
-            "y": 4.270063380418578,
-            "heading": -0.38992694464863337,
-            "angularVelocity": 0.0250642873403699,
-            "velocityX": 0.5932707457717501,
-            "velocityY": 1.4570403425249505,
-            "timestamp": 1.5685038806311118
-        },
-        {
-            "x": 2.285342089257776,
-            "y": 4.3303097045050025,
-            "heading": -0.38974725509942826,
-            "angularVelocity": 0.002851462563739305,
-            "velocityX": 0.43500911823537725,
-            "velocityY": 0.9560385592555154,
-            "timestamp": 1.631520506376144
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.004010765374750144,
-            "velocityX": 0.23260386555018367,
-            "velocityY": 0.47115019479344145,
-            "timestamp": 1.6945371321211762
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -1.6105528525551922e-32,
-            "velocityX": 0,
-            "velocityY": -4.5276793712997926e-36,
-            "timestamp": 1.7575537578662084
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.traj
deleted file mode 100644
index 8a67252f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.traj	
+++ /dev/null
@@ -1,256 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -2.137980318394988e-34,
-            "velocityX": 1.3258659962604387e-36,
-            "velocityY": 7.61509960712835e-36,
-            "timestamp": 0
-        },
-        {
-            "x": 5.139985955005962,
-            "y": 1.6266018860262446,
-            "heading": -0.6327443563347177,
-            "angularVelocity": 0.10940864810395588,
-            "velocityX": -0.5430597418126536,
-            "velocityY": 0.09955056482775779,
-            "timestamp": 0.06631691178916982
-        },
-        {
-            "x": 5.067957879081044,
-            "y": 1.6398056681484225,
-            "heading": -0.6182319813670383,
-            "angularVelocity": 0.2188336968074761,
-            "velocityX": -1.0861192715653594,
-            "velocityY": 0.19910128149746392,
-            "timestamp": 0.13263382357833964
-        },
-        {
-            "x": 4.959915797050431,
-            "y": 1.6596113536414707,
-            "heading": -0.5964609094580124,
-            "angularVelocity": 0.328288385596709,
-            "velocityX": -1.6291784269764191,
-            "velocityY": 0.2986521078667379,
-            "timestamp": 0.19895073536750946
-        },
-        {
-            "x": 4.815859744494909,
-            "y": 1.686018943789264,
-            "heading": -0.5674282989321845,
-            "angularVelocity": 0.43778592432238833,
-            "velocityX": -2.172237045860861,
-            "velocityY": 0.39820295359570684,
-            "timestamp": 0.2652676471566793
-        },
-        {
-            "x": 4.635789767966758,
-            "y": 1.719028429378711,
-            "heading": -0.5311304969514992,
-            "angularVelocity": 0.5473385445944894,
-            "velocityX": -2.7152949627783816,
-            "velocityY": 0.49775366039945246,
-            "timestamp": 0.3315845589458491
-        },
-        {
-            "x": 4.41970592970577,
-            "y": 1.758639783689655,
-            "heading": -0.48756329690665473,
-            "angularVelocity": 0.6569545967904882,
-            "velocityX": -3.258351941175237,
-            "velocityY": 0.5973039642870227,
-            "timestamp": 0.3979014707350189
-        },
-        {
-            "x": 4.172944378919847,
-            "y": 1.8038720825822125,
-            "heading": -0.4875632898742738,
-            "angularVelocity": 1.0604204507812919e-7,
-            "velocityX": -3.7209445393116334,
-            "velocityY": 0.6820628052819626,
-            "timestamp": 0.46421838252418873
-        },
-        {
-            "x": 3.9261828364502485,
-            "y": 1.849104426845956,
-            "heading": -0.4875632828424199,
-            "angularVelocity": 1.0603409772331237e-7,
-            "velocityX": -3.7209444139088643,
-            "velocityY": 0.6820634894390606,
-            "timestamp": 0.5305352943133586
-        },
-        {
-            "x": 3.6794213056437455,
-            "y": 1.8943368347367484,
-            "heading": -0.487563275810563,
-            "angularVelocity": 1.0603414195839125e-7,
-            "velocityX": -3.720944238039768,
-            "velocityY": 0.6820644488783205,
-            "timestamp": 0.5968522061025283
-        },
-        {
-            "x": 3.4326599282156307,
-            "y": 1.9395700793621615,
-            "heading": -0.4875632687787025,
-            "angularVelocity": 1.0603419804191013e-7,
-            "velocityX": -3.720941925230195,
-            "velocityY": 0.6820770660916139,
-            "timestamp": 0.6631691178916981
-        },
-        {
-            "x": 3.192328571865682,
-            "y": 2.011528836252569,
-            "heading": -0.4875632615410612,
-            "angularVelocity": 1.0913718718412795e-7,
-            "velocityX": -3.6239829308396314,
-            "velocityY": 1.0850740022269671,
-            "timestamp": 0.7294860296808678
-        },
-        {
-            "x": 2.965054771843869,
-            "y": 2.1177542675713954,
-            "heading": -0.48756325344928275,
-            "angularVelocity": 1.2201681646141545e-7,
-            "velocityX": -3.4270866041583203,
-            "velocityY": 1.6017849512735258,
-            "timestamp": 0.7958029414700376
-        },
-        {
-            "x": 2.7556945515371734,
-            "y": 2.2559769801698577,
-            "heading": -0.48756324342419455,
-            "angularVelocity": 1.5116940667572282e-7,
-            "velocityX": -3.156965767228115,
-            "velocityY": 2.084275471660842,
-            "timestamp": 0.8621198532592074
-        },
-        {
-            "x": 2.5687211592217127,
-            "y": 2.4232437322251608,
-            "heading": -0.4875632292213124,
-            "angularVelocity": 2.1416682100483036e-7,
-            "velocityX": -2.8193923280063653,
-            "velocityY": 2.5222337340898235,
-            "timestamp": 0.9284367650483771
-        },
-        {
-            "x": 2.4081295337898796,
-            "y": 2.6159806661875145,
-            "heading": -0.48756314249031313,
-            "angularVelocity": 0.0000013078262679985986,
-            "velocityX": -2.421578766248568,
-            "velocityY": 2.906301405817713,
-            "timestamp": 0.9947536768375469
-        },
-        {
-            "x": 2.2783797783684188,
-            "y": 2.8285106690600896,
-            "heading": -0.4828540771785898,
-            "angularVelocity": 0.07100851328382284,
-            "velocityX": -1.9565108193510647,
-            "velocityY": 3.2047632668456583,
-            "timestamp": 1.0610705886267167
-        },
-        {
-            "x": 2.1829307079315186,
-            "y": 3.053825616836548,
-            "heading": -0.47038335932986286,
-            "angularVelocity": 0.18804732476646344,
-            "velocityX": -1.4392870213912456,
-            "velocityY": 3.3975488559051734,
-            "timestamp": 1.1273875004158864
-        },
-        {
-            "x": 2.124568521000596,
-            "y": 3.2749812318761844,
-            "heading": -0.4550563591461243,
-            "angularVelocity": 0.24322153086635045,
-            "velocityX": -0.9261395106595871,
-            "velocityY": 3.5094804335357668,
-            "timestamp": 1.1904041261609186
-        },
-        {
-            "x": 2.099309698866093,
-            "y": 3.4965263857952165,
-            "heading": -0.4375068558264715,
-            "angularVelocity": 0.2784900510328627,
-            "velocityX": -0.40082790590377965,
-            "velocityY": 3.5156619590425686,
-            "timestamp": 1.2534207519059508
-        },
-        {
-            "x": 2.105558929434475,
-            "y": 3.707892405315209,
-            "heading": -0.4199092504656253,
-            "angularVelocity": 0.2792533740547583,
-            "velocityX": 0.09916796550908585,
-            "velocityY": 3.354131025281934,
-            "timestamp": 1.316437377650983
-        },
-        {
-            "x": 2.135680280502732,
-            "y": 3.8963278332771982,
-            "heading": -0.40525850037923283,
-            "angularVelocity": 0.23249023433387317,
-            "velocityX": 0.4779905415775436,
-            "velocityY": 2.990249410122441,
-            "timestamp": 1.3794540033960152
-        },
-        {
-            "x": 2.177481885950485,
-            "y": 4.0538159629335215,
-            "heading": -0.39590861500997965,
-            "angularVelocity": 0.14837172347315433,
-            "velocityX": 0.6633424902324003,
-            "velocityY": 2.4991520538330105,
-            "timestamp": 1.4424706291410474
-        },
-        {
-            "x": 2.2205433619064863,
-            "y": 4.17824561445827,
-            "heading": -0.39150641146352744,
-            "angularVelocity": 0.06985781124276201,
-            "velocityX": 0.6833351587282585,
-            "velocityY": 1.9745527478446014,
-            "timestamp": 1.5054872548860796
-        },
-        {
-            "x": 2.2579292824582606,
-            "y": 4.270063380418578,
-            "heading": -0.38992694464863337,
-            "angularVelocity": 0.0250642873403699,
-            "velocityX": 0.5932707457717501,
-            "velocityY": 1.4570403425249505,
-            "timestamp": 1.5685038806311118
-        },
-        {
-            "x": 2.285342089257776,
-            "y": 4.3303097045050025,
-            "heading": -0.38974725509942826,
-            "angularVelocity": 0.002851462563739305,
-            "velocityX": 0.43500911823537725,
-            "velocityY": 0.9560385592555154,
-            "timestamp": 1.631520506376144
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.004010765374750144,
-            "velocityX": 0.23260386555018367,
-            "velocityY": 0.47115019479344145,
-            "timestamp": 1.6945371321211762
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -1.6105528525551922e-32,
-            "velocityX": 0,
-            "velocityY": -4.5276793712997926e-36,
-            "timestamp": 1.7575537578662084
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.1.traj
deleted file mode 100644
index b5a8c20b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.1.traj	
+++ /dev/null
@@ -1,427 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": -3.8536087109009435e-31,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7208977987123659,
-            "y": 4.366230614894293,
-            "heading": -1.0125695703411368,
-            "angularVelocity": 0.6607032256639885,
-            "velocityX": 0.262489900865184,
-            "velocityY": 0.1500737469179648,
-            "timestamp": 0.04151702094612918
-        },
-        {
-            "x": 0.7428348149738224,
-            "y": 4.378911218024912,
-            "heading": -0.9596996326030717,
-            "angularVelocity": 1.2734521055122956,
-            "velocityX": 0.5283860874778604,
-            "velocityY": 0.30543143129192235,
-            "timestamp": 0.08303404189225835
-        },
-        {
-            "x": 0.7759938113776359,
-            "y": 4.398288777276395,
-            "heading": -0.8840273814949616,
-            "angularVelocity": 1.8226801775305617,
-            "velocityX": 0.7986843865038376,
-            "velocityY": 0.46673770925264374,
-            "timestamp": 0.12455106283838753
-        },
-        {
-            "x": 0.8206097262268935,
-            "y": 4.424610495629411,
-            "heading": -0.7889470653064761,
-            "angularVelocity": 2.2901526656373563,
-            "velocityX": 1.0746415285232955,
-            "velocityY": 0.6339982434446707,
-            "timestamp": 0.1660680837845167
-        },
-        {
-            "x": 0.876940069443398,
-            "y": 4.458114052595694,
-            "heading": -0.6785120124719297,
-            "angularVelocity": 2.6599946315634684,
-            "velocityX": 1.3568011849799015,
-            "velocityY": 0.8069836467704494,
-            "timestamp": 0.2075851047306459
-        },
-        {
-            "x": 0.9451906760990277,
-            "y": 4.499074634849957,
-            "heading": -0.5574056685886438,
-            "angularVelocity": 2.9170287540888538,
-            "velocityX": 1.6439186892586013,
-            "velocityY": 0.9865973357595108,
-            "timestamp": 0.2491021256767751
-        },
-        {
-            "x": 1.0254198074155758,
-            "y": 4.547882919999799,
-            "heading": -0.43194974381010975,
-            "angularVelocity": 3.0217949631302035,
-            "velocityX": 1.9324395028417536,
-            "velocityY": 1.175621083531942,
-            "timestamp": 0.2906191466229043
-        },
-        {
-            "x": 1.117246624722019,
-            "y": 4.6049782109254505,
-            "heading": -0.31304235816438886,
-            "angularVelocity": 2.8640635319318393,
-            "velocityX": 2.211787243252437,
-            "velocityY": 1.3752261030390898,
-            "timestamp": 0.33213616756903347
-        },
-        {
-            "x": 1.217280699738284,
-            "y": 4.671027805780365,
-            "heading": -0.22202476317889255,
-            "angularVelocity": 2.1922959044630015,
-            "velocityX": 2.409471410436724,
-            "velocityY": 1.5909040039398776,
-            "timestamp": 0.37365318851516266
-        },
-        {
-            "x": 1.3245506705159111,
-            "y": 4.7468577293550815,
-            "heading": -0.15914757808634442,
-            "angularVelocity": 1.5144917351940563,
-            "velocityX": 2.5837588616171234,
-            "velocityY": 1.8264779564293179,
-            "timestamp": 0.41517020946129185
-        },
-        {
-            "x": 1.4375421795348913,
-            "y": 4.833718356767073,
-            "heading": -0.12357000071830693,
-            "angularVelocity": 0.8569395529247944,
-            "velocityX": 2.7215707303608654,
-            "velocityY": 2.0921690774675916,
-            "timestamp": 0.45668723040742104
-        },
-        {
-            "x": 1.5526379689004826,
-            "y": 4.933380534066268,
-            "heading": -0.11179394732799099,
-            "angularVelocity": 0.2836439879911631,
-            "velocityX": 2.7722554928653214,
-            "velocityY": 2.4005136936152596,
-            "timestamp": 0.49820425135355023
-        },
-        {
-            "x": 1.6621482250840773,
-            "y": 5.045879001435295,
-            "heading": -0.11165007196252263,
-            "angularVelocity": 0.0034654549371103873,
-            "velocityX": 2.6377195108883376,
-            "velocityY": 2.709695079403094,
-            "timestamp": 0.5397212722996794
-        },
-        {
-            "x": 1.760950803756714,
-            "y": 5.167963981628418,
-            "heading": -0.11164985662454338,
-            "angularVelocity": 0.000005186739663230103,
-            "velocityX": 2.3798089655981185,
-            "velocityY": 2.940600684033376,
-            "timestamp": 0.5812382932458086
-        },
-        {
-            "x": 1.8636898338676644,
-            "y": 5.328509480252694,
-            "heading": -0.11164971710954129,
-            "angularVelocity": 0.000002768959602908003,
-            "velocityX": 2.039065475146862,
-            "velocityY": 3.1863526751214604,
-            "timestamp": 0.6316236444586746
-        },
-        {
-            "x": 1.9479932631191832,
-            "y": 5.499457142063362,
-            "heading": -0.11164961128911181,
-            "angularVelocity": 0.0000021002221265930453,
-            "velocityX": 1.6731733970781186,
-            "velocityY": 3.3928048072606183,
-            "timestamp": 0.6820089956715406
-        },
-        {
-            "x": 2.043351099956472,
-            "y": 5.66449366180702,
-            "heading": -0.11164949905977092,
-            "angularVelocity": 0.000002227420038927282,
-            "velocityX": 1.8925706488092502,
-            "velocityY": 3.275486143730564,
-            "timestamp": 0.7323943468844066
-        },
-        {
-            "x": 2.1564178056024077,
-            "y": 5.817933945525861,
-            "heading": -0.11163496800446338,
-            "angularVelocity": 0.0002883986749930945,
-            "velocityX": 2.244039247910364,
-            "velocityY": 3.0453352020399613,
-            "timestamp": 0.7827796980972727
-        },
-        {
-            "x": 2.2829725450073877,
-            "y": 5.955279135270152,
-            "heading": -0.10213363222908782,
-            "angularVelocity": 0.1885733760372676,
-            "velocityX": 2.5117367718340784,
-            "velocityY": 2.7258952540120527,
-            "timestamp": 0.8331650493101387
-        },
-        {
-            "x": 2.421245283059438,
-            "y": 6.075180412328107,
-            "heading": -0.08356505866258204,
-            "angularVelocity": 0.36853119252760197,
-            "velocityX": 2.7443043409080725,
-            "velocityY": 2.379685249204375,
-            "timestamp": 0.8835504005230047
-        },
-        {
-            "x": 2.570177190376964,
-            "y": 6.176890921850726,
-            "heading": -0.058269582676031995,
-            "angularVelocity": 0.5020402830847994,
-            "velocityX": 2.9558572825813,
-            "velocityY": 2.018652387526583,
-            "timestamp": 0.9339357517358707
-        },
-        {
-            "x": 2.728959152818659,
-            "y": 6.259893890741026,
-            "heading": -0.029260248722302817,
-            "angularVelocity": 0.5757493647196714,
-            "velocityX": 3.151351704820358,
-            "velocityY": 1.6473631103484576,
-            "timestamp": 0.9843211029487368
-        },
-        {
-            "x": 2.896845817565918,
-            "y": 6.323787212371826,
-            "heading": -5.388333556894659e-33,
-            "angularVelocity": 0.580729279745141,
-            "velocityX": 3.332053081047339,
-            "velocityY": 1.2680932075002167,
-            "timestamp": 1.034706454161603
-        },
-        {
-            "x": 3.0174812814006593,
-            "y": 6.35877580192698,
-            "heading": 0.017969633030989543,
-            "angularVelocity": 0.512929568417621,
-            "velocityX": 3.443447970619488,
-            "velocityY": 0.9987227956847937,
-            "timestamp": 1.069739788442565
-        },
-        {
-            "x": 3.141783379846136,
-            "y": 6.384241994405335,
-            "heading": 0.0331535119160805,
-            "angularVelocity": 0.43341232568386434,
-            "velocityX": 3.5481092792260864,
-            "velocityY": 0.7269131814328351,
-            "timestamp": 1.1047731227235271
-        },
-        {
-            "x": 3.269386252376355,
-            "y": 6.400068652575719,
-            "heading": 0.0449202302050253,
-            "angularVelocity": 0.3358720639272233,
-            "velocityX": 3.642327376147768,
-            "velocityY": 0.45175997361343984,
-            "timestamp": 1.1398064570044892
-        },
-        {
-            "x": 3.3996512457384864,
-            "y": 6.406094302159117,
-            "heading": 0.052174145586997546,
-            "angularVelocity": 0.20705752202617325,
-            "velocityX": 3.7183155995559303,
-            "velocityY": 0.17199760476778314,
-            "timestamp": 1.1748397912854514
-        },
-        {
-            "x": 3.531556253779136,
-            "y": 6.402132340099008,
-            "heading": 0.05397985174916118,
-            "angularVelocity": 0.05154251518314789,
-            "velocityX": 3.7651285767486513,
-            "velocityY": -0.11309120703085081,
-            "timestamp": 1.2098731255664135
-        },
-        {
-            "x": 3.663324219452337,
-            "y": 6.387950136374773,
-            "heading": 0.05397986327261661,
-            "angularVelocity": 3.2892831051068895e-7,
-            "velocityX": 3.7612168061547906,
-            "velocityY": -0.4048202666228151,
-            "timestamp": 1.2449064598473756
-        },
-        {
-            "x": 3.793605089187622,
-            "y": 6.363643169403076,
-            "heading": 0.053979860367415415,
-            "angularVelocity": -8.292676841260746e-8,
-            "velocityX": 3.718768778627541,
-            "velocityY": -0.6938239671072872,
-            "timestamp": 1.2799397941283377
-        },
-        {
-            "x": 4.111061973016203,
-            "y": 6.363890421291427,
-            "heading": 0.05397985774152245,
-            "angularVelocity": -3.129115871783651e-8,
-            "velocityX": 3.7829393158349456,
-            "velocityY": 0.002946349368989899,
-            "timestamp": 1.3638578423242849
-        },
-        {
-            "x": 4.423042820635495,
-            "y": 6.422600616815994,
-            "heading": 0.053979855524806185,
-            "angularVelocity": -2.641525047123925e-8,
-            "velocityX": 3.7176847451316233,
-            "velocityY": 0.6996134536813446,
-            "timestamp": 1.447775890520232
-        },
-        {
-            "x": 4.7253380303410815,
-            "y": 6.519536384153899,
-            "heading": 0.05397985339265137,
-            "angularVelocity": -2.5407583503873633e-8,
-            "velocityX": 3.602266928321921,
-            "velocityY": 1.1551241886794503,
-            "timestamp": 1.5316939387161792
-        },
-        {
-            "x": 5.027633110558163,
-            "y": 6.616472555302148,
-            "heading": 0.053979851260496835,
-            "angularVelocity": -2.5407580227573106e-8,
-            "velocityX": 3.6022653852867035,
-            "velocityY": 1.1551290006400656,
-            "timestamp": 1.6156119869121264
-        },
-        {
-            "x": 5.329928190774575,
-            "y": 6.71340872645249,
-            "heading": 0.05397984912834238,
-            "angularVelocity": -2.540757910781892e-8,
-            "velocityX": 3.602265385278706,
-            "velocityY": 1.1551290006650068,
-            "timestamp": 1.6995300351080735
-        },
-        {
-            "x": 5.632223270990985,
-            "y": 6.810344897602833,
-            "heading": 0.053979846996187915,
-            "angularVelocity": -2.5407579299100972e-8,
-            "velocityX": 3.602265385278706,
-            "velocityY": 1.155129000665007,
-            "timestamp": 1.7834480833040207
-        },
-        {
-            "x": 5.934518351207396,
-            "y": 6.907281068753174,
-            "heading": 0.05397984486403343,
-            "angularVelocity": -2.5407579624099417e-8,
-            "velocityX": 3.602265385278706,
-            "velocityY": 1.1551290006650068,
-            "timestamp": 1.8673661314999679
-        },
-        {
-            "x": 6.236813431423807,
-            "y": 7.004217239903516,
-            "heading": 0.053979842731878905,
-            "angularVelocity": -2.5407579955507513e-8,
-            "velocityX": 3.602265385278706,
-            "velocityY": 1.1551290006650072,
-            "timestamp": 1.951284179695915
-        },
-        {
-            "x": 6.539108511640218,
-            "y": 7.1011534110538586,
-            "heading": 0.053979840599724424,
-            "angularVelocity": -2.5407579566600852e-8,
-            "velocityX": 3.6022653852787063,
-            "velocityY": 1.155129000665007,
-            "timestamp": 2.0352022278918622
-        },
-        {
-            "x": 6.841403591857615,
-            "y": 7.198089582201063,
-            "heading": 0.05397983846756001,
-            "angularVelocity": -2.5407697930922655e-8,
-            "velocityX": 3.6022653852904623,
-            "velocityY": 1.1551290006276251,
-            "timestamp": 2.119120276087809
-        },
-        {
-            "x": 7.120935706136836,
-            "y": 7.287726381323771,
-            "heading": 0.035986533919804255,
-            "angularVelocity": -0.21441519356800662,
-            "velocityX": 3.331013057245073,
-            "velocityY": 1.0681468533849583,
-            "timestamp": 2.203038324283756
-        },
-        {
-            "x": 7.344561416731701,
-            "y": 7.359435826630496,
-            "heading": 0.021591909406666148,
-            "angularVelocity": -0.17153192695269756,
-            "velocityX": 2.664810674250942,
-            "velocityY": 0.8545175543082761,
-            "timestamp": 2.286956372479703
-        },
-        {
-            "x": 7.512280706030795,
-            "y": 7.413217912582047,
-            "heading": 0.010795955954566746,
-            "angularVelocity": -0.12864876727003446,
-            "velocityX": 1.9986080813923648,
-            "velocityY": 0.6408881892244497,
-            "timestamp": 2.37087442067565
-        },
-        {
-            "x": 7.624093568104708,
-            "y": 7.449072637343766,
-            "heading": 0.0035986551256689905,
-            "angularVelocity": -0.08576582729978673,
-            "velocityX": 1.3324054178766367,
-            "velocityY": 0.42725880227814755,
-            "timestamp": 2.454792468871597
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -6.709954361314771e-31,
-            "angularVelocity": -0.04288296979061891,
-            "velocityX": 0.6662027191665668,
-            "velocityY": 0.21362940442053102,
-            "timestamp": 2.538710517067544
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 6.378058636012386e-33,
-            "angularVelocity": 8.071845383822447e-30,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 2.622628565263491
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.traj
deleted file mode 100644
index b5a8c20b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.traj	
+++ /dev/null
@@ -1,427 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": -3.8536087109009435e-31,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7208977987123659,
-            "y": 4.366230614894293,
-            "heading": -1.0125695703411368,
-            "angularVelocity": 0.6607032256639885,
-            "velocityX": 0.262489900865184,
-            "velocityY": 0.1500737469179648,
-            "timestamp": 0.04151702094612918
-        },
-        {
-            "x": 0.7428348149738224,
-            "y": 4.378911218024912,
-            "heading": -0.9596996326030717,
-            "angularVelocity": 1.2734521055122956,
-            "velocityX": 0.5283860874778604,
-            "velocityY": 0.30543143129192235,
-            "timestamp": 0.08303404189225835
-        },
-        {
-            "x": 0.7759938113776359,
-            "y": 4.398288777276395,
-            "heading": -0.8840273814949616,
-            "angularVelocity": 1.8226801775305617,
-            "velocityX": 0.7986843865038376,
-            "velocityY": 0.46673770925264374,
-            "timestamp": 0.12455106283838753
-        },
-        {
-            "x": 0.8206097262268935,
-            "y": 4.424610495629411,
-            "heading": -0.7889470653064761,
-            "angularVelocity": 2.2901526656373563,
-            "velocityX": 1.0746415285232955,
-            "velocityY": 0.6339982434446707,
-            "timestamp": 0.1660680837845167
-        },
-        {
-            "x": 0.876940069443398,
-            "y": 4.458114052595694,
-            "heading": -0.6785120124719297,
-            "angularVelocity": 2.6599946315634684,
-            "velocityX": 1.3568011849799015,
-            "velocityY": 0.8069836467704494,
-            "timestamp": 0.2075851047306459
-        },
-        {
-            "x": 0.9451906760990277,
-            "y": 4.499074634849957,
-            "heading": -0.5574056685886438,
-            "angularVelocity": 2.9170287540888538,
-            "velocityX": 1.6439186892586013,
-            "velocityY": 0.9865973357595108,
-            "timestamp": 0.2491021256767751
-        },
-        {
-            "x": 1.0254198074155758,
-            "y": 4.547882919999799,
-            "heading": -0.43194974381010975,
-            "angularVelocity": 3.0217949631302035,
-            "velocityX": 1.9324395028417536,
-            "velocityY": 1.175621083531942,
-            "timestamp": 0.2906191466229043
-        },
-        {
-            "x": 1.117246624722019,
-            "y": 4.6049782109254505,
-            "heading": -0.31304235816438886,
-            "angularVelocity": 2.8640635319318393,
-            "velocityX": 2.211787243252437,
-            "velocityY": 1.3752261030390898,
-            "timestamp": 0.33213616756903347
-        },
-        {
-            "x": 1.217280699738284,
-            "y": 4.671027805780365,
-            "heading": -0.22202476317889255,
-            "angularVelocity": 2.1922959044630015,
-            "velocityX": 2.409471410436724,
-            "velocityY": 1.5909040039398776,
-            "timestamp": 0.37365318851516266
-        },
-        {
-            "x": 1.3245506705159111,
-            "y": 4.7468577293550815,
-            "heading": -0.15914757808634442,
-            "angularVelocity": 1.5144917351940563,
-            "velocityX": 2.5837588616171234,
-            "velocityY": 1.8264779564293179,
-            "timestamp": 0.41517020946129185
-        },
-        {
-            "x": 1.4375421795348913,
-            "y": 4.833718356767073,
-            "heading": -0.12357000071830693,
-            "angularVelocity": 0.8569395529247944,
-            "velocityX": 2.7215707303608654,
-            "velocityY": 2.0921690774675916,
-            "timestamp": 0.45668723040742104
-        },
-        {
-            "x": 1.5526379689004826,
-            "y": 4.933380534066268,
-            "heading": -0.11179394732799099,
-            "angularVelocity": 0.2836439879911631,
-            "velocityX": 2.7722554928653214,
-            "velocityY": 2.4005136936152596,
-            "timestamp": 0.49820425135355023
-        },
-        {
-            "x": 1.6621482250840773,
-            "y": 5.045879001435295,
-            "heading": -0.11165007196252263,
-            "angularVelocity": 0.0034654549371103873,
-            "velocityX": 2.6377195108883376,
-            "velocityY": 2.709695079403094,
-            "timestamp": 0.5397212722996794
-        },
-        {
-            "x": 1.760950803756714,
-            "y": 5.167963981628418,
-            "heading": -0.11164985662454338,
-            "angularVelocity": 0.000005186739663230103,
-            "velocityX": 2.3798089655981185,
-            "velocityY": 2.940600684033376,
-            "timestamp": 0.5812382932458086
-        },
-        {
-            "x": 1.8636898338676644,
-            "y": 5.328509480252694,
-            "heading": -0.11164971710954129,
-            "angularVelocity": 0.000002768959602908003,
-            "velocityX": 2.039065475146862,
-            "velocityY": 3.1863526751214604,
-            "timestamp": 0.6316236444586746
-        },
-        {
-            "x": 1.9479932631191832,
-            "y": 5.499457142063362,
-            "heading": -0.11164961128911181,
-            "angularVelocity": 0.0000021002221265930453,
-            "velocityX": 1.6731733970781186,
-            "velocityY": 3.3928048072606183,
-            "timestamp": 0.6820089956715406
-        },
-        {
-            "x": 2.043351099956472,
-            "y": 5.66449366180702,
-            "heading": -0.11164949905977092,
-            "angularVelocity": 0.000002227420038927282,
-            "velocityX": 1.8925706488092502,
-            "velocityY": 3.275486143730564,
-            "timestamp": 0.7323943468844066
-        },
-        {
-            "x": 2.1564178056024077,
-            "y": 5.817933945525861,
-            "heading": -0.11163496800446338,
-            "angularVelocity": 0.0002883986749930945,
-            "velocityX": 2.244039247910364,
-            "velocityY": 3.0453352020399613,
-            "timestamp": 0.7827796980972727
-        },
-        {
-            "x": 2.2829725450073877,
-            "y": 5.955279135270152,
-            "heading": -0.10213363222908782,
-            "angularVelocity": 0.1885733760372676,
-            "velocityX": 2.5117367718340784,
-            "velocityY": 2.7258952540120527,
-            "timestamp": 0.8331650493101387
-        },
-        {
-            "x": 2.421245283059438,
-            "y": 6.075180412328107,
-            "heading": -0.08356505866258204,
-            "angularVelocity": 0.36853119252760197,
-            "velocityX": 2.7443043409080725,
-            "velocityY": 2.379685249204375,
-            "timestamp": 0.8835504005230047
-        },
-        {
-            "x": 2.570177190376964,
-            "y": 6.176890921850726,
-            "heading": -0.058269582676031995,
-            "angularVelocity": 0.5020402830847994,
-            "velocityX": 2.9558572825813,
-            "velocityY": 2.018652387526583,
-            "timestamp": 0.9339357517358707
-        },
-        {
-            "x": 2.728959152818659,
-            "y": 6.259893890741026,
-            "heading": -0.029260248722302817,
-            "angularVelocity": 0.5757493647196714,
-            "velocityX": 3.151351704820358,
-            "velocityY": 1.6473631103484576,
-            "timestamp": 0.9843211029487368
-        },
-        {
-            "x": 2.896845817565918,
-            "y": 6.323787212371826,
-            "heading": -5.388333556894659e-33,
-            "angularVelocity": 0.580729279745141,
-            "velocityX": 3.332053081047339,
-            "velocityY": 1.2680932075002167,
-            "timestamp": 1.034706454161603
-        },
-        {
-            "x": 3.0174812814006593,
-            "y": 6.35877580192698,
-            "heading": 0.017969633030989543,
-            "angularVelocity": 0.512929568417621,
-            "velocityX": 3.443447970619488,
-            "velocityY": 0.9987227956847937,
-            "timestamp": 1.069739788442565
-        },
-        {
-            "x": 3.141783379846136,
-            "y": 6.384241994405335,
-            "heading": 0.0331535119160805,
-            "angularVelocity": 0.43341232568386434,
-            "velocityX": 3.5481092792260864,
-            "velocityY": 0.7269131814328351,
-            "timestamp": 1.1047731227235271
-        },
-        {
-            "x": 3.269386252376355,
-            "y": 6.400068652575719,
-            "heading": 0.0449202302050253,
-            "angularVelocity": 0.3358720639272233,
-            "velocityX": 3.642327376147768,
-            "velocityY": 0.45175997361343984,
-            "timestamp": 1.1398064570044892
-        },
-        {
-            "x": 3.3996512457384864,
-            "y": 6.406094302159117,
-            "heading": 0.052174145586997546,
-            "angularVelocity": 0.20705752202617325,
-            "velocityX": 3.7183155995559303,
-            "velocityY": 0.17199760476778314,
-            "timestamp": 1.1748397912854514
-        },
-        {
-            "x": 3.531556253779136,
-            "y": 6.402132340099008,
-            "heading": 0.05397985174916118,
-            "angularVelocity": 0.05154251518314789,
-            "velocityX": 3.7651285767486513,
-            "velocityY": -0.11309120703085081,
-            "timestamp": 1.2098731255664135
-        },
-        {
-            "x": 3.663324219452337,
-            "y": 6.387950136374773,
-            "heading": 0.05397986327261661,
-            "angularVelocity": 3.2892831051068895e-7,
-            "velocityX": 3.7612168061547906,
-            "velocityY": -0.4048202666228151,
-            "timestamp": 1.2449064598473756
-        },
-        {
-            "x": 3.793605089187622,
-            "y": 6.363643169403076,
-            "heading": 0.053979860367415415,
-            "angularVelocity": -8.292676841260746e-8,
-            "velocityX": 3.718768778627541,
-            "velocityY": -0.6938239671072872,
-            "timestamp": 1.2799397941283377
-        },
-        {
-            "x": 4.111061973016203,
-            "y": 6.363890421291427,
-            "heading": 0.05397985774152245,
-            "angularVelocity": -3.129115871783651e-8,
-            "velocityX": 3.7829393158349456,
-            "velocityY": 0.002946349368989899,
-            "timestamp": 1.3638578423242849
-        },
-        {
-            "x": 4.423042820635495,
-            "y": 6.422600616815994,
-            "heading": 0.053979855524806185,
-            "angularVelocity": -2.641525047123925e-8,
-            "velocityX": 3.7176847451316233,
-            "velocityY": 0.6996134536813446,
-            "timestamp": 1.447775890520232
-        },
-        {
-            "x": 4.7253380303410815,
-            "y": 6.519536384153899,
-            "heading": 0.05397985339265137,
-            "angularVelocity": -2.5407583503873633e-8,
-            "velocityX": 3.602266928321921,
-            "velocityY": 1.1551241886794503,
-            "timestamp": 1.5316939387161792
-        },
-        {
-            "x": 5.027633110558163,
-            "y": 6.616472555302148,
-            "heading": 0.053979851260496835,
-            "angularVelocity": -2.5407580227573106e-8,
-            "velocityX": 3.6022653852867035,
-            "velocityY": 1.1551290006400656,
-            "timestamp": 1.6156119869121264
-        },
-        {
-            "x": 5.329928190774575,
-            "y": 6.71340872645249,
-            "heading": 0.05397984912834238,
-            "angularVelocity": -2.540757910781892e-8,
-            "velocityX": 3.602265385278706,
-            "velocityY": 1.1551290006650068,
-            "timestamp": 1.6995300351080735
-        },
-        {
-            "x": 5.632223270990985,
-            "y": 6.810344897602833,
-            "heading": 0.053979846996187915,
-            "angularVelocity": -2.5407579299100972e-8,
-            "velocityX": 3.602265385278706,
-            "velocityY": 1.155129000665007,
-            "timestamp": 1.7834480833040207
-        },
-        {
-            "x": 5.934518351207396,
-            "y": 6.907281068753174,
-            "heading": 0.05397984486403343,
-            "angularVelocity": -2.5407579624099417e-8,
-            "velocityX": 3.602265385278706,
-            "velocityY": 1.1551290006650068,
-            "timestamp": 1.8673661314999679
-        },
-        {
-            "x": 6.236813431423807,
-            "y": 7.004217239903516,
-            "heading": 0.053979842731878905,
-            "angularVelocity": -2.5407579955507513e-8,
-            "velocityX": 3.602265385278706,
-            "velocityY": 1.1551290006650072,
-            "timestamp": 1.951284179695915
-        },
-        {
-            "x": 6.539108511640218,
-            "y": 7.1011534110538586,
-            "heading": 0.053979840599724424,
-            "angularVelocity": -2.5407579566600852e-8,
-            "velocityX": 3.6022653852787063,
-            "velocityY": 1.155129000665007,
-            "timestamp": 2.0352022278918622
-        },
-        {
-            "x": 6.841403591857615,
-            "y": 7.198089582201063,
-            "heading": 0.05397983846756001,
-            "angularVelocity": -2.5407697930922655e-8,
-            "velocityX": 3.6022653852904623,
-            "velocityY": 1.1551290006276251,
-            "timestamp": 2.119120276087809
-        },
-        {
-            "x": 7.120935706136836,
-            "y": 7.287726381323771,
-            "heading": 0.035986533919804255,
-            "angularVelocity": -0.21441519356800662,
-            "velocityX": 3.331013057245073,
-            "velocityY": 1.0681468533849583,
-            "timestamp": 2.203038324283756
-        },
-        {
-            "x": 7.344561416731701,
-            "y": 7.359435826630496,
-            "heading": 0.021591909406666148,
-            "angularVelocity": -0.17153192695269756,
-            "velocityX": 2.664810674250942,
-            "velocityY": 0.8545175543082761,
-            "timestamp": 2.286956372479703
-        },
-        {
-            "x": 7.512280706030795,
-            "y": 7.413217912582047,
-            "heading": 0.010795955954566746,
-            "angularVelocity": -0.12864876727003446,
-            "velocityX": 1.9986080813923648,
-            "velocityY": 0.6408881892244497,
-            "timestamp": 2.37087442067565
-        },
-        {
-            "x": 7.624093568104708,
-            "y": 7.449072637343766,
-            "heading": 0.0035986551256689905,
-            "angularVelocity": -0.08576582729978673,
-            "velocityX": 1.3324054178766367,
-            "velocityY": 0.42725880227814755,
-            "timestamp": 2.454792468871597
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -6.709954361314771e-31,
-            "angularVelocity": -0.04288296979061891,
-            "velocityX": 0.6662027191665668,
-            "velocityY": 0.21362940442053102,
-            "timestamp": 2.538710517067544
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 6.378058636012386e-33,
-            "angularVelocity": 8.071845383822447e-30,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 2.622628565263491
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.1.traj
deleted file mode 100644
index 56b4df9b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.1.traj	
+++ /dev/null
@@ -1,661 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": 0,
-            "velocityX": -4.815368277433195e-31,
-            "velocityY": 1.7671411030208604e-30,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7256756209676614,
-            "y": 4.374259536946564,
-            "heading": -0.9853384433657985,
-            "angularVelocity": 0.9927336490852509,
-            "velocityX": 0.2846921559393299,
-            "velocityY": 0.25897400328871373,
-            "timestamp": 0.05506165393280401
-        },
-        {
-            "x": 0.7571509327835594,
-            "y": 4.40368458563067,
-            "heading": -0.8810495211913171,
-            "angularVelocity": 1.8940390403219183,
-            "velocityX": 0.5716376019938672,
-            "velocityY": 0.5344018310849077,
-            "timestamp": 0.11012330786560802
-        },
-        {
-            "x": 0.8047782151983651,
-            "y": 4.4492655723711865,
-            "heading": -0.7351323547259855,
-            "angularVelocity": 2.6500687146592434,
-            "velocityX": 0.8649809624852585,
-            "velocityY": 0.8278172463956943,
-            "timestamp": 0.16518496179841202
-        },
-        {
-            "x": 0.8693373364829333,
-            "y": 4.511878147553763,
-            "heading": -0.5600607568180229,
-            "angularVelocity": 3.179555741677643,
-            "velocityX": 1.1724878690220806,
-            "velocityY": 1.1371357507548814,
-            "timestamp": 0.22024661573121604
-        },
-        {
-            "x": 0.9516435423656624,
-            "y": 4.592143379904835,
-            "heading": -0.3723491539523138,
-            "angularVelocity": 3.4091166802965285,
-            "velocityX": 1.4948008278694735,
-            "velocityY": 1.4577337696599522,
-            "timestamp": 0.27530826966402006
-        },
-        {
-            "x": 1.0514451430006502,
-            "y": 4.68995617229961,
-            "heading": -0.20247463803020274,
-            "angularVelocity": 3.085169147472857,
-            "velocityX": 1.8125427317637794,
-            "velocityY": 1.7764230713854863,
-            "timestamp": 0.33036992359682404
-        },
-        {
-            "x": 1.163482365369112,
-            "y": 4.8045671318094305,
-            "heading": -0.08453465328971378,
-            "angularVelocity": 2.1419622607883397,
-            "velocityX": 2.034759480799993,
-            "velocityY": 2.0815023037586085,
-            "timestamp": 0.38543157752962803
-        },
-        {
-            "x": 1.290791904505612,
-            "y": 4.932877435439672,
-            "heading": -0.017113580237308034,
-            "angularVelocity": 1.2244650902572813,
-            "velocityX": 2.3121270438301877,
-            "velocityY": 2.330302387705078,
-            "timestamp": 0.440493231462432
-        },
-        {
-            "x": 1.4344972301022125,
-            "y": 5.07384452901374,
-            "heading": -0.000001124802148976757,
-            "angularVelocity": 0.3107871669853269,
-            "velocityX": 2.6098984562178145,
-            "velocityY": 2.560168166154319,
-            "timestamp": 0.495554885395236
-        },
-        {
-            "x": 1.5830888632571813,
-            "y": 5.219814728179112,
-            "heading": -9.965376040774715e-7,
-            "angularVelocity": 0.0000023294713396498257,
-            "velocityX": 2.6986409332390493,
-            "velocityY": 2.6510318659074916,
-            "timestamp": 0.55061653932804
-        },
-        {
-            "x": 1.7316392407262138,
-            "y": 5.365826918457485,
-            "heading": -8.792165857555993e-7,
-            "angularVelocity": 0.000002130720927018926,
-            "velocityX": 2.697891669756242,
-            "velocityY": 2.6517944858193996,
-            "timestamp": 0.605678193260844
-        },
-        {
-            "x": 1.8801815584157808,
-            "y": 5.511847308171012,
-            "heading": -7.618956854973843e-7,
-            "angularVelocity": 0.000002130718782810582,
-            "velocityX": 2.69774529239705,
-            "velocityY": 2.6519433995297197,
-            "timestamp": 0.6607398471936481
-        },
-        {
-            "x": 2.0287247432411455,
-            "y": 5.657866815749449,
-            "heading": -6.445747704723799e-7,
-            "angularVelocity": 0.000002130719050997138,
-            "velocityX": 2.697761040854699,
-            "velocityY": 2.651927378662928,
-            "timestamp": 0.7158015011264521
-        },
-        {
-            "x": 2.1772589110251723,
-            "y": 5.8038954956427204,
-            "heading": -5.272539834240943e-7,
-            "angularVelocity": 0.000002130716726753334,
-            "velocityX": 2.6975972782370956,
-            "velocityY": 2.652093961273629,
-            "timestamp": 0.7708631550592562
-        },
-        {
-            "x": 2.325792733071576,
-            "y": 5.949924527226785,
-            "heading": -4.0993217710172413e-7,
-            "angularVelocity": 0.000002130735238257545,
-            "velocityX": 2.6975909991452314,
-            "velocityY": 2.6521003484836045,
-            "timestamp": 0.8259248089920602
-        },
-        {
-            "x": 2.481459175645397,
-            "y": 6.088325151306142,
-            "heading": -2.908915465695403e-7,
-            "angularVelocity": 0.0000021619515945987695,
-            "velocityX": 2.8271297982536217,
-            "velocityY": 2.5135573342462116,
-            "timestamp": 0.8809864629248643
-        },
-        {
-            "x": 2.6527099574524438,
-            "y": 6.206899777175588,
-            "heading": -1.5899541452377036e-7,
-            "angularVelocity": 0.0000023954262651640655,
-            "velocityX": 3.1101641446636745,
-            "velocityY": 2.1534882699533155,
-            "timestamp": 0.9360481168576683
-        },
-        {
-            "x": 2.837061882019043,
-            "y": 6.303859233856201,
-            "heading": 0,
-            "angularVelocity": 0.0000028875887873359224,
-            "velocityX": 3.348100018792042,
-            "velocityY": 1.760925249319746,
-            "timestamp": 0.9911097707904724
-        },
-        {
-            "x": 2.932152988543463,
-            "y": 6.346714053639414,
-            "heading": 2.0172187169325966e-12,
-            "angularVelocity": 7.316288159012044e-11,
-            "velocityX": 3.4488770423117883,
-            "velocityY": 1.5543094355402358,
-            "timestamp": 1.018681384784641
-        },
-        {
-            "x": 3.0296712234656686,
-            "y": 6.383714706348048,
-            "heading": 2.030359324162707e-12,
-            "angularVelocity": 4.765991109637323e-13,
-            "velocityX": 3.5369070139469976,
-            "velocityY": 1.3419835602475594,
-            "timestamp": 1.0462529987788096
-        },
-        {
-            "x": 3.129255935132477,
-            "y": 6.414726637699066,
-            "heading": 2.0283850653761223e-12,
-            "angularVelocity": -7.160477709378183e-14,
-            "velocityX": 3.611856443643096,
-            "velocityY": 1.1247775106178561,
-            "timestamp": 1.0738246127729782
-        },
-        {
-            "x": 3.2305380260225394,
-            "y": 6.439642353966136,
-            "heading": 2.0263773134602995e-12,
-            "angularVelocity": -7.281954448876825e-14,
-            "velocityX": 3.6734190066355246,
-            "velocityY": 0.9036727510191612,
-            "timestamp": 1.1013962267671469
-        },
-        {
-            "x": 3.3331381427053266,
-            "y": 6.458405909393185,
-            "heading": 2.0244535837319738e-12,
-            "angularVelocity": -6.977212868461473e-14,
-            "velocityX": 3.7212227294451083,
-            "velocityY": 0.6805388843797584,
-            "timestamp": 1.1289678407613155
-        },
-        {
-            "x": 3.43663787164561,
-            "y": 6.471315638605137,
-            "heading": 2.022533482460756e-12,
-            "angularVelocity": -6.96405259775829e-14,
-            "velocityX": 3.753850933863969,
-            "velocityY": 0.4682253717820687,
-            "timestamp": 1.1565394547554841
-        },
-        {
-            "x": 3.5403567621307404,
-            "y": 6.4823268492843,
-            "heading": 2.0206101184182045e-12,
-            "angularVelocity": -6.975886505788547e-14,
-            "velocityX": 3.7617997447283904,
-            "velocityY": 0.39936764973541705,
-            "timestamp": 1.1841110687496528
-        },
-        {
-            "x": 3.6440832149520914,
-            "y": 6.493266592802732,
-            "heading": 2.018711397348351e-12,
-            "angularVelocity": -6.886508434036338e-14,
-            "velocityX": 3.7620740245091877,
-            "velocityY": 0.3967755939732401,
-            "timestamp": 1.2116826827438214
-        },
-        {
-            "x": 3.747806936839693,
-            "y": 6.504232199027323,
-            "heading": 2.016809036122773e-12,
-            "angularVelocity": -6.89971097222047e-14,
-            "velocityX": 3.7619749757661194,
-            "velocityY": 0.39771361326329024,
-            "timestamp": 1.23925429673799
-        },
-        {
-            "x": 3.8515283310140283,
-            "y": 6.515219800794399,
-            "heading": 2.0149121825042556e-12,
-            "angularVelocity": -6.87973533298754e-14,
-            "velocityX": 3.761890551500436,
-            "velocityY": 0.3985113736873028,
-            "timestamp": 1.2668259107321587
-        },
-        {
-            "x": 3.9552475387122907,
-            "y": 6.52622802304771,
-            "heading": 2.0130014782113644e-12,
-            "angularVelocity": -6.92997060168648e-14,
-            "velocityX": 3.7618112497917684,
-            "velocityY": 0.3992592619503583,
-            "timestamp": 1.2943975247263273
-        },
-        {
-            "x": 4.058964880445898,
-            "y": 6.537253812271398,
-            "heading": 2.011079593605957e-12,
-            "angularVelocity": -6.970520685257517e-14,
-            "velocityX": 3.7617435727707904,
-            "velocityY": 0.3998964016833969,
-            "timestamp": 1.321969138720496
-        },
-        {
-            "x": 4.162680825175896,
-            "y": 6.5482927349999205,
-            "heading": 2.009190619489212e-12,
-            "angularVelocity": -6.851157050326969e-14,
-            "velocityX": 3.761692904585482,
-            "velocityY": 0.4003727432280311,
-            "timestamp": 1.3495407527146646
-        },
-        {
-            "x": 4.2663959076467215,
-            "y": 6.559339756077265,
-            "heading": 2.007295781849634e-12,
-            "angularVelocity": -6.87242357042844e-14,
-            "velocityX": 3.7616616311467697,
-            "velocityY": 0.40066646375103865,
-            "timestamp": 1.3771123667088332
-        },
-        {
-            "x": 4.370110729285335,
-            "y": 6.570389225581033,
-            "heading": 2.0053851186045697e-12,
-            "angularVelocity": -6.929821753536312e-14,
-            "velocityX": 3.761652170976742,
-            "velocityY": 0.40075526616572615,
-            "timestamp": 1.4046839807030018
-        },
-        {
-            "x": 4.473826107172322,
-            "y": 6.581433472559441,
-            "heading": 2.003480405786031e-12,
-            "angularVelocity": -6.908240040814547e-14,
-            "velocityX": 3.7616723456572294,
-            "velocityY": 0.4005658493780133,
-            "timestamp": 1.4322555946971705
-        },
-        {
-            "x": 4.577543170052599,
-            "y": 6.592461884602437,
-            "heading": 2.001567952942652e-12,
-            "angularVelocity": -6.936312480979614e-14,
-            "velocityX": 3.7617334589969245,
-            "velocityY": 0.399991529116124,
-            "timestamp": 1.459827208691339
-        },
-        {
-            "x": 4.681263099195036,
-            "y": 6.603463307611616,
-            "heading": 1.9996669629945417e-12,
-            "angularVelocity": -6.894737463787157e-14,
-            "velocityX": 3.761837415995553,
-            "velocityY": 0.3990126587978969,
-            "timestamp": 1.4873988226855077
-        },
-        {
-            "x": 4.784986899417181,
-            "y": 6.61442817334549,
-            "heading": 1.997767883198675e-12,
-            "angularVelocity": -6.88780949705506e-14,
-            "velocityX": 3.7619778169075735,
-            "velocityY": 0.3976867561604882,
-            "timestamp": 1.5149704366796763
-        },
-        {
-            "x": 4.888715211119033,
-            "y": 6.625350277801733,
-            "heading": 1.9958591963322718e-12,
-            "angularVelocity": -6.922653588980761e-14,
-            "velocityX": 3.762141444605259,
-            "velocityY": 0.39613583941828256,
-            "timestamp": 1.542542050673845
-        },
-        {
-            "x": 4.992447807319009,
-            "y": 6.636231614926857,
-            "heading": 1.9939696519007763e-12,
-            "angularVelocity": -6.853225532454514e-14,
-            "velocityX": 3.7622968398643226,
-            "velocityY": 0.3946572415062904,
-            "timestamp": 1.5701136646680136
-        },
-        {
-            "x": 5.09618354900695,
-            "y": 6.6470829236468765,
-            "heading": 1.992073077512808e-12,
-            "angularVelocity": -6.878722613480481e-14,
-            "velocityX": 3.7624109241486314,
-            "velocityY": 0.3935681357313447,
-            "timestamp": 1.5976852786621822
-        },
-        {
-            "x": 5.199926346512542,
-            "y": 6.657866567184633,
-            "heading": 1.990167650654994e-12,
-            "angularVelocity": -6.910829798877678e-14,
-            "velocityX": 3.7626668329101425,
-            "velocityY": 0.3911139746618113,
-            "timestamp": 1.6252568926563509
-        },
-        {
-            "x": 5.303729435372611,
-            "y": 6.668053506977846,
-            "heading": 1.9882412477007472e-12,
-            "angularVelocity": -6.986908358991098e-14,
-            "velocityX": 3.764853551267859,
-            "velocityY": 0.3694720154839007,
-            "timestamp": 1.6528285066505195
-        },
-        {
-            "x": 5.40792279202557,
-            "y": 6.672807389271601,
-            "heading": 1.9863066963577084e-12,
-            "angularVelocity": -7.016461895491627e-14,
-            "velocityX": 3.779008246489486,
-            "velocityY": 0.17241944175311613,
-            "timestamp": 1.6804001206446881
-        },
-        {
-            "x": 5.512213692786393,
-            "y": 6.671303096171843,
-            "heading": 1.984343863250107e-12,
-            "angularVelocity": -7.119037640734205e-14,
-            "velocityX": 3.782546091892586,
-            "velocityY": -0.05455948646048947,
-            "timestamp": 1.7079717346388568
-        },
-        {
-            "x": 5.616221821617454,
-            "y": 6.663482363169505,
-            "heading": 1.9823084185583715e-12,
-            "angularVelocity": -7.382393974706967e-14,
-            "velocityX": 3.772290184136858,
-            "velocityY": -0.2836516209983201,
-            "timestamp": 1.7355433486330254
-        },
-        {
-            "x": 5.7195634841918945,
-            "y": 6.649363040924072,
-            "heading": 1.9801191994112705e-12,
-            "angularVelocity": -7.94012151883752e-14,
-            "velocityX": 3.7481179954212767,
-            "velocityY": -0.5120963266444406,
-            "timestamp": 1.763114962627194
-        },
-        {
-            "x": 5.834816669509981,
-            "y": 6.625475968242897,
-            "heading": 1.9780897732255602e-12,
-            "angularVelocity": -6.522540784880574e-14,
-            "velocityX": 3.704217712719512,
-            "velocityY": -0.7677264405820726,
-            "timestamp": 1.794229004580604
-        },
-        {
-            "x": 5.948163245427426,
-            "y": 6.593751483162918,
-            "heading": 1.9761554326166073e-12,
-            "angularVelocity": -6.216937380396162e-14,
-            "velocityX": 3.6429396118708874,
-            "velocityY": -1.0196195379349016,
-            "timestamp": 1.825343046534014
-        },
-        {
-            "x": 6.0590772896693,
-            "y": 6.554356506595256,
-            "heading": 1.974328708451782e-12,
-            "angularVelocity": -5.871059954922337e-14,
-            "velocityX": 3.564758458833813,
-            "velocityY": -1.2661478256826604,
-            "timestamp": 1.8564570884874239
-        },
-        {
-            "x": 6.167114375070259,
-            "y": 6.507645933449736,
-            "heading": 1.9725156971860347e-12,
-            "angularVelocity": -5.826986956344564e-14,
-            "velocityX": 3.472293492526525,
-            "velocityY": -1.5012698515774907,
-            "timestamp": 1.8875711304408338
-        },
-        {
-            "x": 6.273666318458446,
-            "y": 6.457640205385319,
-            "heading": 1.9706978622350046e-12,
-            "angularVelocity": -5.842490200021065e-14,
-            "velocityX": 3.424561281617029,
-            "velocityY": -1.6071755684828801,
-            "timestamp": 1.9186851723942437
-        },
-        {
-            "x": 6.3802136872176485,
-            "y": 6.4076247308783065,
-            "heading": 1.9688925756308956e-12,
-            "angularVelocity": -5.802160020867771e-14,
-            "velocityX": 3.4244142538170945,
-            "velocityY": -1.6074888174954127,
-            "timestamp": 1.9497992143476537
-        },
-        {
-            "x": 6.486760876374776,
-            "y": 6.3576088737324294,
-            "heading": 1.967091079960254e-12,
-            "angularVelocity": -5.789976010142433e-14,
-            "velocityX": 3.4244084814345546,
-            "velocityY": -1.607501115448455,
-            "timestamp": 1.9809132563010636
-        },
-        {
-            "x": 6.593299366426369,
-            "y": 6.307574489358799,
-            "heading": 1.9652854794360616e-12,
-            "angularVelocity": -5.803168954665514e-14,
-            "velocityX": 3.4241288936675214,
-            "velocityY": -1.6080965773861522,
-            "timestamp": 2.0120272982544734
-        },
-        {
-            "x": 6.6998332452145375,
-            "y": 6.257530287401571,
-            "heading": 1.963481306814657e-12,
-            "angularVelocity": -5.79857968421928e-14,
-            "velocityX": 3.423980688450087,
-            "velocityY": -1.608412112843002,
-            "timestamp": 2.043141340207883
-        },
-        {
-            "x": 6.806367665236628,
-            "y": 6.207487237643806,
-            "heading": 1.9616709532128727e-12,
-            "angularVelocity": -5.818445266643988e-14,
-            "velocityX": 3.4239980836191717,
-            "velocityY": -1.6083750813421427,
-            "timestamp": 2.074255382161293
-        },
-        {
-            "x": 6.912905936531216,
-            "y": 6.157452377320687,
-            "heading": 1.9503301873398137e-12,
-            "angularVelocity": -3.6449015032516537e-13,
-            "velocityX": 3.424121862863347,
-            "velocityY": -1.6081118743030238,
-            "timestamp": 2.1053694241147025
-        },
-        {
-            "x": 7.015182351606042,
-            "y": 6.109386281530938,
-            "heading": 7.52788173701755e-9,
-            "angularVelocity": 2.418821392510085e-7,
-            "velocityX": 3.287146531073864,
-            "velocityY": -1.5448361181009935,
-            "timestamp": 2.1364834660681122
-        },
-        {
-            "x": 7.110154929626141,
-            "y": 6.064756817323096,
-            "heading": 9.121631465411093e-9,
-            "angularVelocity": 5.122284110672045e-8,
-            "velocityX": 3.0524024542461334,
-            "velocityY": -1.4343833653841132,
-            "timestamp": 2.167597508021522
-        },
-        {
-            "x": 7.19782257145347,
-            "y": 6.02356173384038,
-            "heading": 9.19440476065517e-9,
-            "angularVelocity": 2.3389208664172086e-9,
-            "velocityX": 2.8176230513092126,
-            "velocityY": -1.3240029548204029,
-            "timestamp": 2.1987115499749317
-        },
-        {
-            "x": 7.278184934221974,
-            "y": 5.985800317976241,
-            "heading": 8.586659012919787e-9,
-            "angularVelocity": -1.9532843759144805e-8,
-            "velocityX": 2.582832628715854,
-            "velocityY": -1.2136454633793592,
-            "timestamp": 2.2298255919283414
-        },
-        {
-            "x": 7.351241854096833,
-            "y": 5.951472228972044,
-            "heading": 7.633275836953741e-9,
-            "angularVelocity": -3.064157105146478e-8,
-            "velocityX": 2.3480369404994192,
-            "velocityY": -1.1032989238626065,
-            "timestamp": 2.260939633881751
-        },
-        {
-            "x": 7.416993235519351,
-            "y": 5.9205772680990165,
-            "heading": 6.513007078345892e-9,
-            "angularVelocity": -3.600524486341296e-8,
-            "velocityX": 2.1132381810427594,
-            "velocityY": -0.9929587714545899,
-            "timestamp": 2.292053675835161
-        },
-        {
-            "x": 7.47543901596506,
-            "y": 5.89311530534189,
-            "heading": 5.336882897741431e-9,
-            "angularVelocity": -3.780042846669445e-8,
-            "velocityX": 1.8784374120603946,
-            "velocityY": -0.8826227977151127,
-            "timestamp": 2.3231677177885706
-        },
-        {
-            "x": 7.52657915135954,
-            "y": 5.869086249058835,
-            "heading": 4.180544199369271e-9,
-            "angularVelocity": -3.716452645721565e-8,
-            "velocityX": 1.643635226533715,
-            "velocityY": -0.7722897693284123,
-            "timestamp": 2.3542817597419803
-        },
-        {
-            "x": 7.570413608969555,
-            "y": 5.848490031193872,
-            "heading": 3.0988314877379083e-9,
-            "angularVelocity": -3.476606009342686e-8,
-            "velocityX": 1.4088319889664873,
-            "velocityY": -0.6619589282491846,
-            "timestamp": 2.38539580169539
-        },
-        {
-            "x": 7.606942363528039,
-            "y": 5.831326599216932,
-            "heading": 2.1333266690310216e-9,
-            "angularVelocity": -3.103115843804935e-8,
-            "velocityX": 1.1740279393200113,
-            "velocityY": -0.5516297754774387,
-            "timestamp": 2.4165098436487997
-        },
-        {
-            "x": 7.636165394942908,
-            "y": 5.817595911358632,
-            "heading": 1.3166428525752622e-9,
-            "angularVelocity": -2.6248077074736502e-8,
-            "velocityX": 0.9392232439174092,
-            "velocityY": -0.4413019651665061,
-            "timestamp": 2.4476238856022094
-        },
-        {
-            "x": 7.658082686855903,
-            "y": 5.80729793361315,
-            "heading": 6.750439688094791e-10,
-            "angularVelocity": -2.0620877517571588e-8,
-            "velocityX": 0.7044180227631803,
-            "velocityY": -0.3309752477968783,
-            "timestamp": 2.478737927555619
-        },
-        {
-            "x": 7.672694225690866,
-            "y": 5.800432637759072,
-            "heading": 2.301347828245949e-10,
-            "angularVelocity": -1.4299304537044113e-8,
-            "velocityX": 0.46961236527350575,
-            "velocityY": -0.22064943745862983,
-            "timestamp": 2.509851969509029
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -5.36340746801057e-34,
-            "angularVelocity": -7.3964922450186615e-9,
-            "velocityX": 0.23480633985361782,
-            "velocityY": -0.1103243919326242,
-            "timestamp": 2.5409660114624386
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -7.670487347269043e-34,
-            "angularVelocity": -7.414948879954709e-33,
-            "velocityX": 1.169026940913066e-31,
-            "velocityY": 1.7301899688761208e-31,
-            "timestamp": 2.5720800534158483
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.traj
deleted file mode 100644
index 56b4df9b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.traj	
+++ /dev/null
@@ -1,661 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": 0,
-            "velocityX": -4.815368277433195e-31,
-            "velocityY": 1.7671411030208604e-30,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7256756209676614,
-            "y": 4.374259536946564,
-            "heading": -0.9853384433657985,
-            "angularVelocity": 0.9927336490852509,
-            "velocityX": 0.2846921559393299,
-            "velocityY": 0.25897400328871373,
-            "timestamp": 0.05506165393280401
-        },
-        {
-            "x": 0.7571509327835594,
-            "y": 4.40368458563067,
-            "heading": -0.8810495211913171,
-            "angularVelocity": 1.8940390403219183,
-            "velocityX": 0.5716376019938672,
-            "velocityY": 0.5344018310849077,
-            "timestamp": 0.11012330786560802
-        },
-        {
-            "x": 0.8047782151983651,
-            "y": 4.4492655723711865,
-            "heading": -0.7351323547259855,
-            "angularVelocity": 2.6500687146592434,
-            "velocityX": 0.8649809624852585,
-            "velocityY": 0.8278172463956943,
-            "timestamp": 0.16518496179841202
-        },
-        {
-            "x": 0.8693373364829333,
-            "y": 4.511878147553763,
-            "heading": -0.5600607568180229,
-            "angularVelocity": 3.179555741677643,
-            "velocityX": 1.1724878690220806,
-            "velocityY": 1.1371357507548814,
-            "timestamp": 0.22024661573121604
-        },
-        {
-            "x": 0.9516435423656624,
-            "y": 4.592143379904835,
-            "heading": -0.3723491539523138,
-            "angularVelocity": 3.4091166802965285,
-            "velocityX": 1.4948008278694735,
-            "velocityY": 1.4577337696599522,
-            "timestamp": 0.27530826966402006
-        },
-        {
-            "x": 1.0514451430006502,
-            "y": 4.68995617229961,
-            "heading": -0.20247463803020274,
-            "angularVelocity": 3.085169147472857,
-            "velocityX": 1.8125427317637794,
-            "velocityY": 1.7764230713854863,
-            "timestamp": 0.33036992359682404
-        },
-        {
-            "x": 1.163482365369112,
-            "y": 4.8045671318094305,
-            "heading": -0.08453465328971378,
-            "angularVelocity": 2.1419622607883397,
-            "velocityX": 2.034759480799993,
-            "velocityY": 2.0815023037586085,
-            "timestamp": 0.38543157752962803
-        },
-        {
-            "x": 1.290791904505612,
-            "y": 4.932877435439672,
-            "heading": -0.017113580237308034,
-            "angularVelocity": 1.2244650902572813,
-            "velocityX": 2.3121270438301877,
-            "velocityY": 2.330302387705078,
-            "timestamp": 0.440493231462432
-        },
-        {
-            "x": 1.4344972301022125,
-            "y": 5.07384452901374,
-            "heading": -0.000001124802148976757,
-            "angularVelocity": 0.3107871669853269,
-            "velocityX": 2.6098984562178145,
-            "velocityY": 2.560168166154319,
-            "timestamp": 0.495554885395236
-        },
-        {
-            "x": 1.5830888632571813,
-            "y": 5.219814728179112,
-            "heading": -9.965376040774715e-7,
-            "angularVelocity": 0.0000023294713396498257,
-            "velocityX": 2.6986409332390493,
-            "velocityY": 2.6510318659074916,
-            "timestamp": 0.55061653932804
-        },
-        {
-            "x": 1.7316392407262138,
-            "y": 5.365826918457485,
-            "heading": -8.792165857555993e-7,
-            "angularVelocity": 0.000002130720927018926,
-            "velocityX": 2.697891669756242,
-            "velocityY": 2.6517944858193996,
-            "timestamp": 0.605678193260844
-        },
-        {
-            "x": 1.8801815584157808,
-            "y": 5.511847308171012,
-            "heading": -7.618956854973843e-7,
-            "angularVelocity": 0.000002130718782810582,
-            "velocityX": 2.69774529239705,
-            "velocityY": 2.6519433995297197,
-            "timestamp": 0.6607398471936481
-        },
-        {
-            "x": 2.0287247432411455,
-            "y": 5.657866815749449,
-            "heading": -6.445747704723799e-7,
-            "angularVelocity": 0.000002130719050997138,
-            "velocityX": 2.697761040854699,
-            "velocityY": 2.651927378662928,
-            "timestamp": 0.7158015011264521
-        },
-        {
-            "x": 2.1772589110251723,
-            "y": 5.8038954956427204,
-            "heading": -5.272539834240943e-7,
-            "angularVelocity": 0.000002130716726753334,
-            "velocityX": 2.6975972782370956,
-            "velocityY": 2.652093961273629,
-            "timestamp": 0.7708631550592562
-        },
-        {
-            "x": 2.325792733071576,
-            "y": 5.949924527226785,
-            "heading": -4.0993217710172413e-7,
-            "angularVelocity": 0.000002130735238257545,
-            "velocityX": 2.6975909991452314,
-            "velocityY": 2.6521003484836045,
-            "timestamp": 0.8259248089920602
-        },
-        {
-            "x": 2.481459175645397,
-            "y": 6.088325151306142,
-            "heading": -2.908915465695403e-7,
-            "angularVelocity": 0.0000021619515945987695,
-            "velocityX": 2.8271297982536217,
-            "velocityY": 2.5135573342462116,
-            "timestamp": 0.8809864629248643
-        },
-        {
-            "x": 2.6527099574524438,
-            "y": 6.206899777175588,
-            "heading": -1.5899541452377036e-7,
-            "angularVelocity": 0.0000023954262651640655,
-            "velocityX": 3.1101641446636745,
-            "velocityY": 2.1534882699533155,
-            "timestamp": 0.9360481168576683
-        },
-        {
-            "x": 2.837061882019043,
-            "y": 6.303859233856201,
-            "heading": 0,
-            "angularVelocity": 0.0000028875887873359224,
-            "velocityX": 3.348100018792042,
-            "velocityY": 1.760925249319746,
-            "timestamp": 0.9911097707904724
-        },
-        {
-            "x": 2.932152988543463,
-            "y": 6.346714053639414,
-            "heading": 2.0172187169325966e-12,
-            "angularVelocity": 7.316288159012044e-11,
-            "velocityX": 3.4488770423117883,
-            "velocityY": 1.5543094355402358,
-            "timestamp": 1.018681384784641
-        },
-        {
-            "x": 3.0296712234656686,
-            "y": 6.383714706348048,
-            "heading": 2.030359324162707e-12,
-            "angularVelocity": 4.765991109637323e-13,
-            "velocityX": 3.5369070139469976,
-            "velocityY": 1.3419835602475594,
-            "timestamp": 1.0462529987788096
-        },
-        {
-            "x": 3.129255935132477,
-            "y": 6.414726637699066,
-            "heading": 2.0283850653761223e-12,
-            "angularVelocity": -7.160477709378183e-14,
-            "velocityX": 3.611856443643096,
-            "velocityY": 1.1247775106178561,
-            "timestamp": 1.0738246127729782
-        },
-        {
-            "x": 3.2305380260225394,
-            "y": 6.439642353966136,
-            "heading": 2.0263773134602995e-12,
-            "angularVelocity": -7.281954448876825e-14,
-            "velocityX": 3.6734190066355246,
-            "velocityY": 0.9036727510191612,
-            "timestamp": 1.1013962267671469
-        },
-        {
-            "x": 3.3331381427053266,
-            "y": 6.458405909393185,
-            "heading": 2.0244535837319738e-12,
-            "angularVelocity": -6.977212868461473e-14,
-            "velocityX": 3.7212227294451083,
-            "velocityY": 0.6805388843797584,
-            "timestamp": 1.1289678407613155
-        },
-        {
-            "x": 3.43663787164561,
-            "y": 6.471315638605137,
-            "heading": 2.022533482460756e-12,
-            "angularVelocity": -6.96405259775829e-14,
-            "velocityX": 3.753850933863969,
-            "velocityY": 0.4682253717820687,
-            "timestamp": 1.1565394547554841
-        },
-        {
-            "x": 3.5403567621307404,
-            "y": 6.4823268492843,
-            "heading": 2.0206101184182045e-12,
-            "angularVelocity": -6.975886505788547e-14,
-            "velocityX": 3.7617997447283904,
-            "velocityY": 0.39936764973541705,
-            "timestamp": 1.1841110687496528
-        },
-        {
-            "x": 3.6440832149520914,
-            "y": 6.493266592802732,
-            "heading": 2.018711397348351e-12,
-            "angularVelocity": -6.886508434036338e-14,
-            "velocityX": 3.7620740245091877,
-            "velocityY": 0.3967755939732401,
-            "timestamp": 1.2116826827438214
-        },
-        {
-            "x": 3.747806936839693,
-            "y": 6.504232199027323,
-            "heading": 2.016809036122773e-12,
-            "angularVelocity": -6.89971097222047e-14,
-            "velocityX": 3.7619749757661194,
-            "velocityY": 0.39771361326329024,
-            "timestamp": 1.23925429673799
-        },
-        {
-            "x": 3.8515283310140283,
-            "y": 6.515219800794399,
-            "heading": 2.0149121825042556e-12,
-            "angularVelocity": -6.87973533298754e-14,
-            "velocityX": 3.761890551500436,
-            "velocityY": 0.3985113736873028,
-            "timestamp": 1.2668259107321587
-        },
-        {
-            "x": 3.9552475387122907,
-            "y": 6.52622802304771,
-            "heading": 2.0130014782113644e-12,
-            "angularVelocity": -6.92997060168648e-14,
-            "velocityX": 3.7618112497917684,
-            "velocityY": 0.3992592619503583,
-            "timestamp": 1.2943975247263273
-        },
-        {
-            "x": 4.058964880445898,
-            "y": 6.537253812271398,
-            "heading": 2.011079593605957e-12,
-            "angularVelocity": -6.970520685257517e-14,
-            "velocityX": 3.7617435727707904,
-            "velocityY": 0.3998964016833969,
-            "timestamp": 1.321969138720496
-        },
-        {
-            "x": 4.162680825175896,
-            "y": 6.5482927349999205,
-            "heading": 2.009190619489212e-12,
-            "angularVelocity": -6.851157050326969e-14,
-            "velocityX": 3.761692904585482,
-            "velocityY": 0.4003727432280311,
-            "timestamp": 1.3495407527146646
-        },
-        {
-            "x": 4.2663959076467215,
-            "y": 6.559339756077265,
-            "heading": 2.007295781849634e-12,
-            "angularVelocity": -6.87242357042844e-14,
-            "velocityX": 3.7616616311467697,
-            "velocityY": 0.40066646375103865,
-            "timestamp": 1.3771123667088332
-        },
-        {
-            "x": 4.370110729285335,
-            "y": 6.570389225581033,
-            "heading": 2.0053851186045697e-12,
-            "angularVelocity": -6.929821753536312e-14,
-            "velocityX": 3.761652170976742,
-            "velocityY": 0.40075526616572615,
-            "timestamp": 1.4046839807030018
-        },
-        {
-            "x": 4.473826107172322,
-            "y": 6.581433472559441,
-            "heading": 2.003480405786031e-12,
-            "angularVelocity": -6.908240040814547e-14,
-            "velocityX": 3.7616723456572294,
-            "velocityY": 0.4005658493780133,
-            "timestamp": 1.4322555946971705
-        },
-        {
-            "x": 4.577543170052599,
-            "y": 6.592461884602437,
-            "heading": 2.001567952942652e-12,
-            "angularVelocity": -6.936312480979614e-14,
-            "velocityX": 3.7617334589969245,
-            "velocityY": 0.399991529116124,
-            "timestamp": 1.459827208691339
-        },
-        {
-            "x": 4.681263099195036,
-            "y": 6.603463307611616,
-            "heading": 1.9996669629945417e-12,
-            "angularVelocity": -6.894737463787157e-14,
-            "velocityX": 3.761837415995553,
-            "velocityY": 0.3990126587978969,
-            "timestamp": 1.4873988226855077
-        },
-        {
-            "x": 4.784986899417181,
-            "y": 6.61442817334549,
-            "heading": 1.997767883198675e-12,
-            "angularVelocity": -6.88780949705506e-14,
-            "velocityX": 3.7619778169075735,
-            "velocityY": 0.3976867561604882,
-            "timestamp": 1.5149704366796763
-        },
-        {
-            "x": 4.888715211119033,
-            "y": 6.625350277801733,
-            "heading": 1.9958591963322718e-12,
-            "angularVelocity": -6.922653588980761e-14,
-            "velocityX": 3.762141444605259,
-            "velocityY": 0.39613583941828256,
-            "timestamp": 1.542542050673845
-        },
-        {
-            "x": 4.992447807319009,
-            "y": 6.636231614926857,
-            "heading": 1.9939696519007763e-12,
-            "angularVelocity": -6.853225532454514e-14,
-            "velocityX": 3.7622968398643226,
-            "velocityY": 0.3946572415062904,
-            "timestamp": 1.5701136646680136
-        },
-        {
-            "x": 5.09618354900695,
-            "y": 6.6470829236468765,
-            "heading": 1.992073077512808e-12,
-            "angularVelocity": -6.878722613480481e-14,
-            "velocityX": 3.7624109241486314,
-            "velocityY": 0.3935681357313447,
-            "timestamp": 1.5976852786621822
-        },
-        {
-            "x": 5.199926346512542,
-            "y": 6.657866567184633,
-            "heading": 1.990167650654994e-12,
-            "angularVelocity": -6.910829798877678e-14,
-            "velocityX": 3.7626668329101425,
-            "velocityY": 0.3911139746618113,
-            "timestamp": 1.6252568926563509
-        },
-        {
-            "x": 5.303729435372611,
-            "y": 6.668053506977846,
-            "heading": 1.9882412477007472e-12,
-            "angularVelocity": -6.986908358991098e-14,
-            "velocityX": 3.764853551267859,
-            "velocityY": 0.3694720154839007,
-            "timestamp": 1.6528285066505195
-        },
-        {
-            "x": 5.40792279202557,
-            "y": 6.672807389271601,
-            "heading": 1.9863066963577084e-12,
-            "angularVelocity": -7.016461895491627e-14,
-            "velocityX": 3.779008246489486,
-            "velocityY": 0.17241944175311613,
-            "timestamp": 1.6804001206446881
-        },
-        {
-            "x": 5.512213692786393,
-            "y": 6.671303096171843,
-            "heading": 1.984343863250107e-12,
-            "angularVelocity": -7.119037640734205e-14,
-            "velocityX": 3.782546091892586,
-            "velocityY": -0.05455948646048947,
-            "timestamp": 1.7079717346388568
-        },
-        {
-            "x": 5.616221821617454,
-            "y": 6.663482363169505,
-            "heading": 1.9823084185583715e-12,
-            "angularVelocity": -7.382393974706967e-14,
-            "velocityX": 3.772290184136858,
-            "velocityY": -0.2836516209983201,
-            "timestamp": 1.7355433486330254
-        },
-        {
-            "x": 5.7195634841918945,
-            "y": 6.649363040924072,
-            "heading": 1.9801191994112705e-12,
-            "angularVelocity": -7.94012151883752e-14,
-            "velocityX": 3.7481179954212767,
-            "velocityY": -0.5120963266444406,
-            "timestamp": 1.763114962627194
-        },
-        {
-            "x": 5.834816669509981,
-            "y": 6.625475968242897,
-            "heading": 1.9780897732255602e-12,
-            "angularVelocity": -6.522540784880574e-14,
-            "velocityX": 3.704217712719512,
-            "velocityY": -0.7677264405820726,
-            "timestamp": 1.794229004580604
-        },
-        {
-            "x": 5.948163245427426,
-            "y": 6.593751483162918,
-            "heading": 1.9761554326166073e-12,
-            "angularVelocity": -6.216937380396162e-14,
-            "velocityX": 3.6429396118708874,
-            "velocityY": -1.0196195379349016,
-            "timestamp": 1.825343046534014
-        },
-        {
-            "x": 6.0590772896693,
-            "y": 6.554356506595256,
-            "heading": 1.974328708451782e-12,
-            "angularVelocity": -5.871059954922337e-14,
-            "velocityX": 3.564758458833813,
-            "velocityY": -1.2661478256826604,
-            "timestamp": 1.8564570884874239
-        },
-        {
-            "x": 6.167114375070259,
-            "y": 6.507645933449736,
-            "heading": 1.9725156971860347e-12,
-            "angularVelocity": -5.826986956344564e-14,
-            "velocityX": 3.472293492526525,
-            "velocityY": -1.5012698515774907,
-            "timestamp": 1.8875711304408338
-        },
-        {
-            "x": 6.273666318458446,
-            "y": 6.457640205385319,
-            "heading": 1.9706978622350046e-12,
-            "angularVelocity": -5.842490200021065e-14,
-            "velocityX": 3.424561281617029,
-            "velocityY": -1.6071755684828801,
-            "timestamp": 1.9186851723942437
-        },
-        {
-            "x": 6.3802136872176485,
-            "y": 6.4076247308783065,
-            "heading": 1.9688925756308956e-12,
-            "angularVelocity": -5.802160020867771e-14,
-            "velocityX": 3.4244142538170945,
-            "velocityY": -1.6074888174954127,
-            "timestamp": 1.9497992143476537
-        },
-        {
-            "x": 6.486760876374776,
-            "y": 6.3576088737324294,
-            "heading": 1.967091079960254e-12,
-            "angularVelocity": -5.789976010142433e-14,
-            "velocityX": 3.4244084814345546,
-            "velocityY": -1.607501115448455,
-            "timestamp": 1.9809132563010636
-        },
-        {
-            "x": 6.593299366426369,
-            "y": 6.307574489358799,
-            "heading": 1.9652854794360616e-12,
-            "angularVelocity": -5.803168954665514e-14,
-            "velocityX": 3.4241288936675214,
-            "velocityY": -1.6080965773861522,
-            "timestamp": 2.0120272982544734
-        },
-        {
-            "x": 6.6998332452145375,
-            "y": 6.257530287401571,
-            "heading": 1.963481306814657e-12,
-            "angularVelocity": -5.79857968421928e-14,
-            "velocityX": 3.423980688450087,
-            "velocityY": -1.608412112843002,
-            "timestamp": 2.043141340207883
-        },
-        {
-            "x": 6.806367665236628,
-            "y": 6.207487237643806,
-            "heading": 1.9616709532128727e-12,
-            "angularVelocity": -5.818445266643988e-14,
-            "velocityX": 3.4239980836191717,
-            "velocityY": -1.6083750813421427,
-            "timestamp": 2.074255382161293
-        },
-        {
-            "x": 6.912905936531216,
-            "y": 6.157452377320687,
-            "heading": 1.9503301873398137e-12,
-            "angularVelocity": -3.6449015032516537e-13,
-            "velocityX": 3.424121862863347,
-            "velocityY": -1.6081118743030238,
-            "timestamp": 2.1053694241147025
-        },
-        {
-            "x": 7.015182351606042,
-            "y": 6.109386281530938,
-            "heading": 7.52788173701755e-9,
-            "angularVelocity": 2.418821392510085e-7,
-            "velocityX": 3.287146531073864,
-            "velocityY": -1.5448361181009935,
-            "timestamp": 2.1364834660681122
-        },
-        {
-            "x": 7.110154929626141,
-            "y": 6.064756817323096,
-            "heading": 9.121631465411093e-9,
-            "angularVelocity": 5.122284110672045e-8,
-            "velocityX": 3.0524024542461334,
-            "velocityY": -1.4343833653841132,
-            "timestamp": 2.167597508021522
-        },
-        {
-            "x": 7.19782257145347,
-            "y": 6.02356173384038,
-            "heading": 9.19440476065517e-9,
-            "angularVelocity": 2.3389208664172086e-9,
-            "velocityX": 2.8176230513092126,
-            "velocityY": -1.3240029548204029,
-            "timestamp": 2.1987115499749317
-        },
-        {
-            "x": 7.278184934221974,
-            "y": 5.985800317976241,
-            "heading": 8.586659012919787e-9,
-            "angularVelocity": -1.9532843759144805e-8,
-            "velocityX": 2.582832628715854,
-            "velocityY": -1.2136454633793592,
-            "timestamp": 2.2298255919283414
-        },
-        {
-            "x": 7.351241854096833,
-            "y": 5.951472228972044,
-            "heading": 7.633275836953741e-9,
-            "angularVelocity": -3.064157105146478e-8,
-            "velocityX": 2.3480369404994192,
-            "velocityY": -1.1032989238626065,
-            "timestamp": 2.260939633881751
-        },
-        {
-            "x": 7.416993235519351,
-            "y": 5.9205772680990165,
-            "heading": 6.513007078345892e-9,
-            "angularVelocity": -3.600524486341296e-8,
-            "velocityX": 2.1132381810427594,
-            "velocityY": -0.9929587714545899,
-            "timestamp": 2.292053675835161
-        },
-        {
-            "x": 7.47543901596506,
-            "y": 5.89311530534189,
-            "heading": 5.336882897741431e-9,
-            "angularVelocity": -3.780042846669445e-8,
-            "velocityX": 1.8784374120603946,
-            "velocityY": -0.8826227977151127,
-            "timestamp": 2.3231677177885706
-        },
-        {
-            "x": 7.52657915135954,
-            "y": 5.869086249058835,
-            "heading": 4.180544199369271e-9,
-            "angularVelocity": -3.716452645721565e-8,
-            "velocityX": 1.643635226533715,
-            "velocityY": -0.7722897693284123,
-            "timestamp": 2.3542817597419803
-        },
-        {
-            "x": 7.570413608969555,
-            "y": 5.848490031193872,
-            "heading": 3.0988314877379083e-9,
-            "angularVelocity": -3.476606009342686e-8,
-            "velocityX": 1.4088319889664873,
-            "velocityY": -0.6619589282491846,
-            "timestamp": 2.38539580169539
-        },
-        {
-            "x": 7.606942363528039,
-            "y": 5.831326599216932,
-            "heading": 2.1333266690310216e-9,
-            "angularVelocity": -3.103115843804935e-8,
-            "velocityX": 1.1740279393200113,
-            "velocityY": -0.5516297754774387,
-            "timestamp": 2.4165098436487997
-        },
-        {
-            "x": 7.636165394942908,
-            "y": 5.817595911358632,
-            "heading": 1.3166428525752622e-9,
-            "angularVelocity": -2.6248077074736502e-8,
-            "velocityX": 0.9392232439174092,
-            "velocityY": -0.4413019651665061,
-            "timestamp": 2.4476238856022094
-        },
-        {
-            "x": 7.658082686855903,
-            "y": 5.80729793361315,
-            "heading": 6.750439688094791e-10,
-            "angularVelocity": -2.0620877517571588e-8,
-            "velocityX": 0.7044180227631803,
-            "velocityY": -0.3309752477968783,
-            "timestamp": 2.478737927555619
-        },
-        {
-            "x": 7.672694225690866,
-            "y": 5.800432637759072,
-            "heading": 2.301347828245949e-10,
-            "angularVelocity": -1.4299304537044113e-8,
-            "velocityX": 0.46961236527350575,
-            "velocityY": -0.22064943745862983,
-            "timestamp": 2.509851969509029
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -5.36340746801057e-34,
-            "angularVelocity": -7.3964922450186615e-9,
-            "velocityX": 0.23480633985361782,
-            "velocityY": -0.1103243919326242,
-            "timestamp": 2.5409660114624386
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -7.670487347269043e-34,
-            "angularVelocity": -7.414948879954709e-33,
-            "velocityX": 1.169026940913066e-31,
-            "velocityY": 1.7301899688761208e-31,
-            "timestamp": 2.5720800534158483
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.1.traj
deleted file mode 100644
index 9e7360d5..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.1.traj	
+++ /dev/null
@@ -1,391 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": 5.350140627575144e-31,
-            "velocityX": 5.576066956455863e-32,
-            "velocityY": -7.476130633471959e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7244661136787854,
-            "y": 4.377063116032538,
-            "heading": -0.9951964797725402,
-            "angularVelocity": 0.8134823535576418,
-            "velocityX": 0.2626563301835885,
-            "velocityY": 0.30980922299649477,
-            "timestamp": 0.055076204212072884
-        },
-        {
-            "x": 0.7533392249389502,
-            "y": 4.41173941127898,
-            "heading": -0.9083722132270291,
-            "angularVelocity": 1.5764388230385076,
-            "velocityX": 0.524239309393705,
-            "velocityY": 0.6296057570147754,
-            "timestamp": 0.11015240842414577
-        },
-        {
-            "x": 0.7966290780145974,
-            "y": 4.464680613281826,
-            "heading": -0.783928161336893,
-            "angularVelocity": 2.2594885335771653,
-            "velocityX": 0.7859992113646113,
-            "velocityY": 0.9612354874529038,
-            "timestamp": 0.16522861263621866
-        },
-        {
-            "x": 0.8545700129369538,
-            "y": 4.536623492592967,
-            "heading": -0.6296494882874061,
-            "angularVelocity": 2.8011856527989654,
-            "velocityX": 1.0520139459729396,
-            "velocityY": 1.3062425114505738,
-            "timestamp": 0.22030481684829153
-        },
-        {
-            "x": 0.9276258124093708,
-            "y": 4.628245682798297,
-            "heading": -0.4576364570724038,
-            "angularVelocity": 3.1231823920301154,
-            "velocityX": 1.3264494261643394,
-            "velocityY": 1.663553099130404,
-            "timestamp": 0.27538102106036444
-        },
-        {
-            "x": 1.0160400191585532,
-            "y": 4.739959244546144,
-            "heading": -0.28714155882633985,
-            "angularVelocity": 3.095618165506983,
-            "velocityX": 1.605306829220418,
-            "velocityY": 2.0283453325449616,
-            "timestamp": 0.3304572252724373
-        },
-        {
-            "x": 1.114934453543302,
-            "y": 4.869596690219766,
-            "heading": -0.17212037106177128,
-            "angularVelocity": 2.0884007787042775,
-            "velocityX": 1.795592775492439,
-            "velocityY": 2.3537832268624075,
-            "timestamp": 0.3855334294845102
-        },
-        {
-            "x": 1.2246983786188343,
-            "y": 5.017091614146892,
-            "heading": -0.11040110898627345,
-            "angularVelocity": 1.1206157533631977,
-            "velocityX": 1.9929464393166578,
-            "velocityY": 2.678015415862639,
-            "timestamp": 0.44060963369658307
-        },
-        {
-            "x": 1.345743830891035,
-            "y": 5.182217862842374,
-            "heading": -0.10127955705630985,
-            "angularVelocity": 0.16561693131283492,
-            "velocityX": 2.197781310529416,
-            "velocityY": 2.998141412571981,
-            "timestamp": 0.49568583790865595
-        },
-        {
-            "x": 1.4740265309381277,
-            "y": 5.346392338052181,
-            "heading": -0.10127945031307138,
-            "angularVelocity": 0.0000019381008548903397,
-            "velocityX": 2.3291855690186902,
-            "velocityY": 2.9808603835088525,
-            "timestamp": 0.5507620421207289
-        },
-        {
-            "x": 1.6022800251300493,
-            "y": 5.510589629967512,
-            "heading": -0.10127934358262022,
-            "angularVelocity": 0.0000019378686800961446,
-            "velocityX": 2.3286552881910736,
-            "velocityY": 2.9812746587087395,
-            "timestamp": 0.6058382463328018
-        },
-        {
-            "x": 1.7347437736631814,
-            "y": 5.671409606244062,
-            "heading": -0.10127923743109664,
-            "angularVelocity": 0.0000019273572882478923,
-            "velocityX": 2.4050994513545807,
-            "velocityY": 2.9199538816671686,
-            "timestamp": 0.6609144505448747
-        },
-        {
-            "x": 1.8857033856482435,
-            "y": 5.815009412217227,
-            "heading": -0.10127912902413344,
-            "angularVelocity": 0.0000019683085417376046,
-            "velocityX": 2.7409225843485396,
-            "velocityY": 2.607293077428296,
-            "timestamp": 0.7159906547569477
-        },
-        {
-            "x": 2.0529507817587227,
-            "y": 5.93925921930888,
-            "heading": -0.1012790059378887,
-            "angularVelocity": 0.0000022348352886277144,
-            "velocityX": 3.036654368309179,
-            "velocityY": 2.2559616965109663,
-            "timestamp": 0.7710668589690206
-        },
-        {
-            "x": 2.234021022235051,
-            "y": 6.042327659180149,
-            "heading": -0.10127786160272408,
-            "angularVelocity": 0.0000207773062979661,
-            "velocityX": 3.287631075284561,
-            "velocityY": 1.8713787804693067,
-            "timestamp": 0.8261430631810935
-        },
-        {
-            "x": 2.4203468055894013,
-            "y": 6.121035990183916,
-            "heading": -0.0857184634932982,
-            "angularVelocity": 0.28250672558184264,
-            "velocityX": 3.3830541886455214,
-            "velocityY": 1.4290805281478167,
-            "timestamp": 0.8812192673931665
-        },
-        {
-            "x": 2.608481388818861,
-            "y": 6.1750393209771115,
-            "heading": -0.05159654365961516,
-            "angularVelocity": 0.6195401502670522,
-            "velocityX": 3.4158959558113233,
-            "velocityY": 0.9805202004345134,
-            "timestamp": 0.9362954716052394
-        },
-        {
-            "x": 2.797205924987793,
-            "y": 6.204219341278076,
-            "heading": -3.617531000727075e-31,
-            "angularVelocity": 0.9368209809983128,
-            "velocityX": 3.4266075316708653,
-            "velocityY": 0.5298117529778321,
-            "timestamp": 0.9913716758173123
-        },
-        {
-            "x": 2.966124692846499,
-            "y": 6.210295134186947,
-            "heading": 0.05716064392929573,
-            "angularVelocity": 1.15887425245862,
-            "velocityX": 3.4246572006891465,
-            "velocityY": 0.12318055713425537,
-            "timestamp": 1.0406959608361792
-        },
-        {
-            "x": 3.13289635801508,
-            "y": 6.196205704844962,
-            "heading": 0.11248864097572012,
-            "angularVelocity": 1.12171919015676,
-            "velocityX": 3.3811268648859167,
-            "velocityY": -0.2856489320949118,
-            "timestamp": 1.090020245855046
-        },
-        {
-            "x": 3.2974428000278966,
-            "y": 6.1620266805098955,
-            "heading": 0.16165741156418048,
-            "angularVelocity": 0.9968471021861408,
-            "velocityX": 3.336012715640518,
-            "velocityY": -0.6929451551500881,
-            "timestamp": 1.139344530873913
-        },
-        {
-            "x": 3.45979854217056,
-            "y": 6.107879810654357,
-            "heading": 0.20068182384964747,
-            "angularVelocity": 0.7911804959877227,
-            "velocityX": 3.2915984910995078,
-            "velocityY": -1.0977730307662261,
-            "timestamp": 1.1886688158927798
-        },
-        {
-            "x": 3.619952741472391,
-            "y": 6.03393942235358,
-            "heading": 0.22581697371114728,
-            "angularVelocity": 0.509589745738515,
-            "velocityX": 3.2469644362928305,
-            "velocityY": -1.4990666012186962,
-            "timestamp": 1.2379931009116467
-        },
-        {
-            "x": 3.7772983820923596,
-            "y": 5.940432276289927,
-            "heading": 0.23452421295694514,
-            "angularVelocity": 0.17653047058793564,
-            "velocityX": 3.190023749148815,
-            "velocityY": -1.8957628281461119,
-            "timestamp": 1.2873173859305136
-        },
-        {
-            "x": 3.926570083103172,
-            "y": 5.828476756706829,
-            "heading": 0.23452463928791645,
-            "angularVelocity": 0.000008643429319627254,
-            "velocityX": 3.026332788274898,
-            "velocityY": -2.2697849454943535,
-            "timestamp": 1.3366416709493805
-        },
-        {
-            "x": 4.062952838723869,
-            "y": 5.701135334142216,
-            "heading": 0.23452497322201252,
-            "angularVelocity": 0.000006770176109915362,
-            "velocityX": 2.7650224543249706,
-            "velocityY": -2.5817185695831255,
-            "timestamp": 1.3859659559682473
-        },
-        {
-            "x": 4.192164897918701,
-            "y": 5.566523551940918,
-            "heading": 0.23452535083185597,
-            "angularVelocity": 0.000007655657721212718,
-            "velocityX": 2.6196438355955265,
-            "velocityY": -2.7291177591283535,
-            "timestamp": 1.4352902409871142
-        },
-        {
-            "x": 4.379227990202405,
-            "y": 5.2613971819703,
-            "heading": 0.26255662300085747,
-            "angularVelocity": 0.28814889358818807,
-            "velocityX": 1.922924608906585,
-            "velocityY": -3.136562100411476,
-            "timestamp": 1.5325707539189606
-        },
-        {
-            "x": 4.5172116067735955,
-            "y": 4.993723531483578,
-            "heading": 0.47596391395635385,
-            "angularVelocity": 2.193731144335268,
-            "velocityX": 1.4184096322339512,
-            "velocityY": -2.751564958073857,
-            "timestamp": 1.629851266850807
-        },
-        {
-            "x": 4.704361215089976,
-            "y": 4.7238802473185055,
-            "heading": 0.606383868813914,
-            "angularVelocity": 1.3406585854344948,
-            "velocityX": 1.9238139548821627,
-            "velocityY": -2.773867818255899,
-            "timestamp": 1.7271317797826533
-        },
-        {
-            "x": 4.9212614384999,
-            "y": 4.498429708685273,
-            "heading": 0.7825723935837035,
-            "angularVelocity": 1.8111389368724442,
-            "velocityX": 2.229636921855878,
-            "velocityY": -2.3175303237882856,
-            "timestamp": 1.8244122927144997
-        },
-        {
-            "x": 5.185303406160076,
-            "y": 4.293106225066136,
-            "heading": 0.8921386359376694,
-            "angularVelocity": 1.1262917829260124,
-            "velocityX": 2.71423288901818,
-            "velocityY": -2.110633234047443,
-            "timestamp": 1.921692805646346
-        },
-        {
-            "x": 5.506747569472596,
-            "y": 4.137612897565104,
-            "heading": 0.9241000729554552,
-            "angularVelocity": 0.328549223832528,
-            "velocityX": 3.304301690285293,
-            "velocityY": -1.5984015998144245,
-            "timestamp": 2.0189733185781926
-        },
-        {
-            "x": 5.864556115895607,
-            "y": 4.051579577380226,
-            "heading": 0.924099942108789,
-            "angularVelocity": -0.0000013450449852620257,
-            "velocityX": 3.6781112233000655,
-            "velocityY": -0.8843839078557418,
-            "timestamp": 2.116253831510039
-        },
-        {
-            "x": 6.232480317048254,
-            "y": 4.043802644882202,
-            "heading": 0.9240998495204155,
-            "angularVelocity": -9.517669138022849e-7,
-            "velocityX": 3.7820956126167884,
-            "velocityY": -0.07994337471752075,
-            "timestamp": 2.2135343444418853
-        },
-        {
-            "x": 6.59997941709367,
-            "y": 4.063118723469172,
-            "heading": 0.9240997607721795,
-            "angularVelocity": -9.122920231628579e-7,
-            "velocityX": 3.777725764078584,
-            "velocityY": 0.198560616148301,
-            "timestamp": 2.3108148573737317
-        },
-        {
-            "x": 6.956026800754634,
-            "y": 4.090327871531077,
-            "heading": 0.8973557490580134,
-            "angularVelocity": -0.27491643401286797,
-            "velocityX": 3.660007260759475,
-            "velocityY": 0.2796978268501417,
-            "timestamp": 2.408095370305578
-        },
-        {
-            "x": 7.247734347026976,
-            "y": 4.106615231962139,
-            "heading": 0.7104712895715125,
-            "angularVelocity": -1.9210883439464366,
-            "velocityX": 2.9986226170159034,
-            "velocityY": 0.1674267532128742,
-            "timestamp": 2.5053758832374244
-        },
-        {
-            "x": 7.467840517564824,
-            "y": 4.117242335862297,
-            "heading": 0.4016498999018689,
-            "angularVelocity": -3.1745452440819273,
-            "velocityX": 2.262592619058774,
-            "velocityY": 0.1092418571806094,
-            "timestamp": 2.602656396169271
-        },
-        {
-            "x": 7.6104608023102855,
-            "y": 4.123883769495069,
-            "heading": 0.14488296734927883,
-            "angularVelocity": -2.639448794153438,
-            "velocityX": 1.4660724994879464,
-            "velocityY": 0.06827095615156697,
-            "timestamp": 2.699936909101117
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 9.165039287514682e-34,
-            "angularVelocity": -1.4893318608505086,
-            "velocityX": 0.7148317334473038,
-            "velocityY": 0.032033450595745375,
-            "timestamp": 2.7972174220329635
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 3.6915361070859757e-32,
-            "velocityX": 0,
-            "velocityY": 3.3618150907593784e-32,
-            "timestamp": 2.89449793496481
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.traj
deleted file mode 100644
index 9e7360d5..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.traj	
+++ /dev/null
@@ -1,391 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": 5.350140627575144e-31,
-            "velocityX": 5.576066956455863e-32,
-            "velocityY": -7.476130633471959e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7244661136787854,
-            "y": 4.377063116032538,
-            "heading": -0.9951964797725402,
-            "angularVelocity": 0.8134823535576418,
-            "velocityX": 0.2626563301835885,
-            "velocityY": 0.30980922299649477,
-            "timestamp": 0.055076204212072884
-        },
-        {
-            "x": 0.7533392249389502,
-            "y": 4.41173941127898,
-            "heading": -0.9083722132270291,
-            "angularVelocity": 1.5764388230385076,
-            "velocityX": 0.524239309393705,
-            "velocityY": 0.6296057570147754,
-            "timestamp": 0.11015240842414577
-        },
-        {
-            "x": 0.7966290780145974,
-            "y": 4.464680613281826,
-            "heading": -0.783928161336893,
-            "angularVelocity": 2.2594885335771653,
-            "velocityX": 0.7859992113646113,
-            "velocityY": 0.9612354874529038,
-            "timestamp": 0.16522861263621866
-        },
-        {
-            "x": 0.8545700129369538,
-            "y": 4.536623492592967,
-            "heading": -0.6296494882874061,
-            "angularVelocity": 2.8011856527989654,
-            "velocityX": 1.0520139459729396,
-            "velocityY": 1.3062425114505738,
-            "timestamp": 0.22030481684829153
-        },
-        {
-            "x": 0.9276258124093708,
-            "y": 4.628245682798297,
-            "heading": -0.4576364570724038,
-            "angularVelocity": 3.1231823920301154,
-            "velocityX": 1.3264494261643394,
-            "velocityY": 1.663553099130404,
-            "timestamp": 0.27538102106036444
-        },
-        {
-            "x": 1.0160400191585532,
-            "y": 4.739959244546144,
-            "heading": -0.28714155882633985,
-            "angularVelocity": 3.095618165506983,
-            "velocityX": 1.605306829220418,
-            "velocityY": 2.0283453325449616,
-            "timestamp": 0.3304572252724373
-        },
-        {
-            "x": 1.114934453543302,
-            "y": 4.869596690219766,
-            "heading": -0.17212037106177128,
-            "angularVelocity": 2.0884007787042775,
-            "velocityX": 1.795592775492439,
-            "velocityY": 2.3537832268624075,
-            "timestamp": 0.3855334294845102
-        },
-        {
-            "x": 1.2246983786188343,
-            "y": 5.017091614146892,
-            "heading": -0.11040110898627345,
-            "angularVelocity": 1.1206157533631977,
-            "velocityX": 1.9929464393166578,
-            "velocityY": 2.678015415862639,
-            "timestamp": 0.44060963369658307
-        },
-        {
-            "x": 1.345743830891035,
-            "y": 5.182217862842374,
-            "heading": -0.10127955705630985,
-            "angularVelocity": 0.16561693131283492,
-            "velocityX": 2.197781310529416,
-            "velocityY": 2.998141412571981,
-            "timestamp": 0.49568583790865595
-        },
-        {
-            "x": 1.4740265309381277,
-            "y": 5.346392338052181,
-            "heading": -0.10127945031307138,
-            "angularVelocity": 0.0000019381008548903397,
-            "velocityX": 2.3291855690186902,
-            "velocityY": 2.9808603835088525,
-            "timestamp": 0.5507620421207289
-        },
-        {
-            "x": 1.6022800251300493,
-            "y": 5.510589629967512,
-            "heading": -0.10127934358262022,
-            "angularVelocity": 0.0000019378686800961446,
-            "velocityX": 2.3286552881910736,
-            "velocityY": 2.9812746587087395,
-            "timestamp": 0.6058382463328018
-        },
-        {
-            "x": 1.7347437736631814,
-            "y": 5.671409606244062,
-            "heading": -0.10127923743109664,
-            "angularVelocity": 0.0000019273572882478923,
-            "velocityX": 2.4050994513545807,
-            "velocityY": 2.9199538816671686,
-            "timestamp": 0.6609144505448747
-        },
-        {
-            "x": 1.8857033856482435,
-            "y": 5.815009412217227,
-            "heading": -0.10127912902413344,
-            "angularVelocity": 0.0000019683085417376046,
-            "velocityX": 2.7409225843485396,
-            "velocityY": 2.607293077428296,
-            "timestamp": 0.7159906547569477
-        },
-        {
-            "x": 2.0529507817587227,
-            "y": 5.93925921930888,
-            "heading": -0.1012790059378887,
-            "angularVelocity": 0.0000022348352886277144,
-            "velocityX": 3.036654368309179,
-            "velocityY": 2.2559616965109663,
-            "timestamp": 0.7710668589690206
-        },
-        {
-            "x": 2.234021022235051,
-            "y": 6.042327659180149,
-            "heading": -0.10127786160272408,
-            "angularVelocity": 0.0000207773062979661,
-            "velocityX": 3.287631075284561,
-            "velocityY": 1.8713787804693067,
-            "timestamp": 0.8261430631810935
-        },
-        {
-            "x": 2.4203468055894013,
-            "y": 6.121035990183916,
-            "heading": -0.0857184634932982,
-            "angularVelocity": 0.28250672558184264,
-            "velocityX": 3.3830541886455214,
-            "velocityY": 1.4290805281478167,
-            "timestamp": 0.8812192673931665
-        },
-        {
-            "x": 2.608481388818861,
-            "y": 6.1750393209771115,
-            "heading": -0.05159654365961516,
-            "angularVelocity": 0.6195401502670522,
-            "velocityX": 3.4158959558113233,
-            "velocityY": 0.9805202004345134,
-            "timestamp": 0.9362954716052394
-        },
-        {
-            "x": 2.797205924987793,
-            "y": 6.204219341278076,
-            "heading": -3.617531000727075e-31,
-            "angularVelocity": 0.9368209809983128,
-            "velocityX": 3.4266075316708653,
-            "velocityY": 0.5298117529778321,
-            "timestamp": 0.9913716758173123
-        },
-        {
-            "x": 2.966124692846499,
-            "y": 6.210295134186947,
-            "heading": 0.05716064392929573,
-            "angularVelocity": 1.15887425245862,
-            "velocityX": 3.4246572006891465,
-            "velocityY": 0.12318055713425537,
-            "timestamp": 1.0406959608361792
-        },
-        {
-            "x": 3.13289635801508,
-            "y": 6.196205704844962,
-            "heading": 0.11248864097572012,
-            "angularVelocity": 1.12171919015676,
-            "velocityX": 3.3811268648859167,
-            "velocityY": -0.2856489320949118,
-            "timestamp": 1.090020245855046
-        },
-        {
-            "x": 3.2974428000278966,
-            "y": 6.1620266805098955,
-            "heading": 0.16165741156418048,
-            "angularVelocity": 0.9968471021861408,
-            "velocityX": 3.336012715640518,
-            "velocityY": -0.6929451551500881,
-            "timestamp": 1.139344530873913
-        },
-        {
-            "x": 3.45979854217056,
-            "y": 6.107879810654357,
-            "heading": 0.20068182384964747,
-            "angularVelocity": 0.7911804959877227,
-            "velocityX": 3.2915984910995078,
-            "velocityY": -1.0977730307662261,
-            "timestamp": 1.1886688158927798
-        },
-        {
-            "x": 3.619952741472391,
-            "y": 6.03393942235358,
-            "heading": 0.22581697371114728,
-            "angularVelocity": 0.509589745738515,
-            "velocityX": 3.2469644362928305,
-            "velocityY": -1.4990666012186962,
-            "timestamp": 1.2379931009116467
-        },
-        {
-            "x": 3.7772983820923596,
-            "y": 5.940432276289927,
-            "heading": 0.23452421295694514,
-            "angularVelocity": 0.17653047058793564,
-            "velocityX": 3.190023749148815,
-            "velocityY": -1.8957628281461119,
-            "timestamp": 1.2873173859305136
-        },
-        {
-            "x": 3.926570083103172,
-            "y": 5.828476756706829,
-            "heading": 0.23452463928791645,
-            "angularVelocity": 0.000008643429319627254,
-            "velocityX": 3.026332788274898,
-            "velocityY": -2.2697849454943535,
-            "timestamp": 1.3366416709493805
-        },
-        {
-            "x": 4.062952838723869,
-            "y": 5.701135334142216,
-            "heading": 0.23452497322201252,
-            "angularVelocity": 0.000006770176109915362,
-            "velocityX": 2.7650224543249706,
-            "velocityY": -2.5817185695831255,
-            "timestamp": 1.3859659559682473
-        },
-        {
-            "x": 4.192164897918701,
-            "y": 5.566523551940918,
-            "heading": 0.23452535083185597,
-            "angularVelocity": 0.000007655657721212718,
-            "velocityX": 2.6196438355955265,
-            "velocityY": -2.7291177591283535,
-            "timestamp": 1.4352902409871142
-        },
-        {
-            "x": 4.379227990202405,
-            "y": 5.2613971819703,
-            "heading": 0.26255662300085747,
-            "angularVelocity": 0.28814889358818807,
-            "velocityX": 1.922924608906585,
-            "velocityY": -3.136562100411476,
-            "timestamp": 1.5325707539189606
-        },
-        {
-            "x": 4.5172116067735955,
-            "y": 4.993723531483578,
-            "heading": 0.47596391395635385,
-            "angularVelocity": 2.193731144335268,
-            "velocityX": 1.4184096322339512,
-            "velocityY": -2.751564958073857,
-            "timestamp": 1.629851266850807
-        },
-        {
-            "x": 4.704361215089976,
-            "y": 4.7238802473185055,
-            "heading": 0.606383868813914,
-            "angularVelocity": 1.3406585854344948,
-            "velocityX": 1.9238139548821627,
-            "velocityY": -2.773867818255899,
-            "timestamp": 1.7271317797826533
-        },
-        {
-            "x": 4.9212614384999,
-            "y": 4.498429708685273,
-            "heading": 0.7825723935837035,
-            "angularVelocity": 1.8111389368724442,
-            "velocityX": 2.229636921855878,
-            "velocityY": -2.3175303237882856,
-            "timestamp": 1.8244122927144997
-        },
-        {
-            "x": 5.185303406160076,
-            "y": 4.293106225066136,
-            "heading": 0.8921386359376694,
-            "angularVelocity": 1.1262917829260124,
-            "velocityX": 2.71423288901818,
-            "velocityY": -2.110633234047443,
-            "timestamp": 1.921692805646346
-        },
-        {
-            "x": 5.506747569472596,
-            "y": 4.137612897565104,
-            "heading": 0.9241000729554552,
-            "angularVelocity": 0.328549223832528,
-            "velocityX": 3.304301690285293,
-            "velocityY": -1.5984015998144245,
-            "timestamp": 2.0189733185781926
-        },
-        {
-            "x": 5.864556115895607,
-            "y": 4.051579577380226,
-            "heading": 0.924099942108789,
-            "angularVelocity": -0.0000013450449852620257,
-            "velocityX": 3.6781112233000655,
-            "velocityY": -0.8843839078557418,
-            "timestamp": 2.116253831510039
-        },
-        {
-            "x": 6.232480317048254,
-            "y": 4.043802644882202,
-            "heading": 0.9240998495204155,
-            "angularVelocity": -9.517669138022849e-7,
-            "velocityX": 3.7820956126167884,
-            "velocityY": -0.07994337471752075,
-            "timestamp": 2.2135343444418853
-        },
-        {
-            "x": 6.59997941709367,
-            "y": 4.063118723469172,
-            "heading": 0.9240997607721795,
-            "angularVelocity": -9.122920231628579e-7,
-            "velocityX": 3.777725764078584,
-            "velocityY": 0.198560616148301,
-            "timestamp": 2.3108148573737317
-        },
-        {
-            "x": 6.956026800754634,
-            "y": 4.090327871531077,
-            "heading": 0.8973557490580134,
-            "angularVelocity": -0.27491643401286797,
-            "velocityX": 3.660007260759475,
-            "velocityY": 0.2796978268501417,
-            "timestamp": 2.408095370305578
-        },
-        {
-            "x": 7.247734347026976,
-            "y": 4.106615231962139,
-            "heading": 0.7104712895715125,
-            "angularVelocity": -1.9210883439464366,
-            "velocityX": 2.9986226170159034,
-            "velocityY": 0.1674267532128742,
-            "timestamp": 2.5053758832374244
-        },
-        {
-            "x": 7.467840517564824,
-            "y": 4.117242335862297,
-            "heading": 0.4016498999018689,
-            "angularVelocity": -3.1745452440819273,
-            "velocityX": 2.262592619058774,
-            "velocityY": 0.1092418571806094,
-            "timestamp": 2.602656396169271
-        },
-        {
-            "x": 7.6104608023102855,
-            "y": 4.123883769495069,
-            "heading": 0.14488296734927883,
-            "angularVelocity": -2.639448794153438,
-            "velocityX": 1.4660724994879464,
-            "velocityY": 0.06827095615156697,
-            "timestamp": 2.699936909101117
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 9.165039287514682e-34,
-            "angularVelocity": -1.4893318608505086,
-            "velocityX": 0.7148317334473038,
-            "velocityY": 0.032033450595745375,
-            "timestamp": 2.7972174220329635
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 3.6915361070859757e-32,
-            "velocityX": 0,
-            "velocityY": 3.3618150907593784e-32,
-            "timestamp": 2.89449793496481
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.1.traj
deleted file mode 100644
index 00bde21f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.1.traj	
+++ /dev/null
@@ -1,679 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.687708804701003e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7140616140612069,
-            "y": 4.355992841184312,
-            "heading": -1.0347167301054676,
-            "angularVelocity": 0.1988304134727778,
-            "velocityX": 0.15285465616518656,
-            "velocityY": -0.15080528916361516,
-            "timestamp": 0.026571739213607736
-        },
-        {
-            "x": 0.7221925258884458,
-            "y": 4.3479754290630535,
-            "heading": -1.0242624006876555,
-            "angularVelocity": 0.39343790535390527,
-            "velocityX": 0.30599848063671325,
-            "velocityY": -0.3017270362623706,
-            "timestamp": 0.05314347842721547
-        },
-        {
-            "x": 0.7344011981721603,
-            "y": 4.335944497876898,
-            "heading": -1.0087613580778494,
-            "angularVelocity": 0.5833657513042195,
-            "velocityX": 0.45946078973491633,
-            "velocityY": -0.4527716868451825,
-            "timestamp": 0.0797152176408232
-        },
-        {
-            "x": 0.7506970627866689,
-            "y": 4.31989642008046,
-            "heading": -0.9883558514269565,
-            "angularVelocity": 0.7679401971717026,
-            "velocityX": 0.6132780576953482,
-            "velocityY": -0.6039528563572185,
-            "timestamp": 0.10628695685443094
-        },
-        {
-            "x": 0.7710907256657638,
-            "y": 4.29982697901967,
-            "heading": -0.963213241169254,
-            "angularVelocity": 0.9462162057057438,
-            "velocityX": 0.7674944690353961,
-            "velocityY": -0.7552927152962536,
-            "timestamp": 0.13285869606803868
-        },
-        {
-            "x": 0.7955941760833964,
-            "y": 4.27573110299105,
-            "heading": -0.9335350310204893,
-            "angularVelocity": 1.1169088297226009,
-            "velocityX": 0.9221620843352277,
-            "velocityY": -0.9068234425649082,
-            "timestamp": 0.1594304352816464
-        },
-        {
-            "x": 0.8242209914920048,
-            "y": 4.247602572989807,
-            "heading": -0.8995682913456332,
-            "angularVelocity": 1.2783032153748224,
-            "velocityX": 1.0773406730541775,
-            "velocityY": -1.0585882156647894,
-            "timestamp": 0.18600217449525414
-        },
-        {
-            "x": 0.856986533327333,
-            "y": 4.215433742997202,
-            "heading": -0.8616204330063739,
-            "angularVelocity": 1.4281285103018635,
-            "velocityX": 1.2330973735640383,
-            "velocityY": -1.210640738794065,
-            "timestamp": 0.21257391370886186
-        },
-        {
-            "x": 0.893908134971617,
-            "y": 4.179215360896215,
-            "heading": -0.8200791656995261,
-            "angularVelocity": 1.5633627506615773,
-            "velocityX": 1.3895063980372095,
-            "velocityY": -1.3630414558050108,
-            "timestamp": 0.2391456529224696
-        },
-        {
-            "x": 0.9350052770321623,
-            "y": 4.138936674970777,
-            "heading": -0.7754414952973147,
-            "angularVelocity": 1.6798926876172218,
-            "velocityX": 1.546648555074586,
-            "velocityY": -1.5158468025612346,
-            "timestamp": 0.2657173921360773
-        },
-        {
-            "x": 0.9802996637151041,
-            "y": 4.094586228803994,
-            "heading": -0.7283605055619151,
-            "angularVelocity": 1.7718444907546276,
-            "velocityX": 1.7046075275248067,
-            "velocityY": -1.669083299751435,
-            "timestamp": 0.29228913134968504
-        },
-        {
-            "x": 1.0298146765069804,
-            "y": 4.046154350498487,
-            "heading": -0.6797316150493184,
-            "angularVelocity": 1.8300981400454652,
-            "velocityX": 1.8634464381059026,
-            "velocityY": -1.8226837888242364,
-            "timestamp": 0.31886087056329276
-        },
-        {
-            "x": 1.0835715343165668,
-            "y": 3.9936405744754793,
-            "heading": -0.6308802721211105,
-            "angularVelocity": 1.8384699072761677,
-            "velocityX": 2.0230839004342207,
-            "velocityY": -1.9763017994740086,
-            "timestamp": 0.3454326097769005
-        },
-        {
-            "x": 1.1415677463510772,
-            "y": 3.937082152581159,
-            "heading": -0.5840793547040092,
-            "angularVelocity": 1.7613042579137594,
-            "velocityX": 2.1826276243449603,
-            "velocityY": -2.12851787531304,
-            "timestamp": 0.3720043489905082
-        },
-        {
-            "x": 1.203636670620202,
-            "y": 3.876777756204273,
-            "heading": -0.5447644235662668,
-            "angularVelocity": 1.4795768851144226,
-            "velocityX": 2.33589994882,
-            "velocityY": -2.2694937614773476,
-            "timestamp": 0.39857608820411594
-        },
-        {
-            "x": 1.2693974629594884,
-            "y": 3.8133907541919347,
-            "heading": -0.5190150912676422,
-            "angularVelocity": 0.969049563960718,
-            "velocityX": 2.4748395959572558,
-            "velocityY": -2.3855044452595306,
-            "timestamp": 0.42514782741772367
-        },
-        {
-            "x": 1.3388335264794184,
-            "y": 3.7468848835898942,
-            "heading": -0.5067181615747146,
-            "angularVelocity": 0.4627822663045784,
-            "velocityX": 2.613154636275026,
-            "velocityY": -2.5028798479243823,
-            "timestamp": 0.4517195666313314
-        },
-        {
-            "x": 1.410439403600934,
-            "y": 3.676338986398213,
-            "heading": -0.5067181155488927,
-            "angularVelocity": 0.0000017321343363764108,
-            "velocityX": 2.6948133332892708,
-            "velocityY": -2.6549220818618244,
-            "timestamp": 0.4782913058449391
-        },
-        {
-            "x": 1.482040293717765,
-            "y": 3.605788027611728,
-            "heading": -0.5067180695571892,
-            "angularVelocity": 0.0000017308503249172533,
-            "velocityX": 2.694625652511417,
-            "velocityY": -2.655112569761896,
-            "timestamp": 0.5048630450585468
-        },
-        {
-            "x": 1.5536411829944505,
-            "y": 3.5352370679725946,
-            "heading": -0.5067180235654862,
-            "angularVelocity": 0.0000017308503100877725,
-            "velocityX": 2.6946256208934063,
-            "velocityY": -2.6551126018504427,
-            "timestamp": 0.5314347842721546
-        },
-        {
-            "x": 1.6252420722709946,
-            "y": 3.4646861083333174,
-            "heading": -0.5067179775737835,
-            "angularVelocity": 0.0000017308502987237163,
-            "velocityX": 2.6946256208880808,
-            "velocityY": -2.6551126018558486,
-            "timestamp": 0.5580065234857624
-        },
-        {
-            "x": 1.6968429615475384,
-            "y": 3.39413514869404,
-            "heading": -0.506717931582081,
-            "angularVelocity": 0.0000017308502871423956,
-            "velocityX": 2.6946256208880808,
-            "velocityY": -2.65511260185585,
-            "timestamp": 0.5845782626993702
-        },
-        {
-            "x": 1.7684438508240825,
-            "y": 3.323584189054763,
-            "heading": -0.5067178855903789,
-            "angularVelocity": 0.0000017308502762001758,
-            "velocityX": 2.694625620888081,
-            "velocityY": -2.65511260185585,
-            "timestamp": 0.611150001912978
-        },
-        {
-            "x": 1.8400447401006266,
-            "y": 3.2530332294154856,
-            "heading": -0.506717839598677,
-            "angularVelocity": 0.000001730850265567339,
-            "velocityX": 2.6946256208880826,
-            "velocityY": -2.6551126018558495,
-            "timestamp": 0.6377217411265858
-        },
-        {
-            "x": 1.9116456293771706,
-            "y": 3.1824822697762087,
-            "heading": -0.5067177936069754,
-            "angularVelocity": 0.0000017308502535085818,
-            "velocityX": 2.694625620888084,
-            "velocityY": -2.65511260185585,
-            "timestamp": 0.6642934803401935
-        },
-        {
-            "x": 1.9832465186537145,
-            "y": 3.1119313101369315,
-            "heading": -0.5067177476152741,
-            "angularVelocity": 0.0000017308502432657092,
-            "velocityX": 2.6946256208880843,
-            "velocityY": -2.65511260185585,
-            "timestamp": 0.6908652195538013
-        },
-        {
-            "x": 2.0548474079302586,
-            "y": 3.041380350497654,
-            "heading": -0.5067177016235731,
-            "angularVelocity": 0.000001730850231529751,
-            "velocityX": 2.694625620888085,
-            "velocityY": -2.6551126018558504,
-            "timestamp": 0.7174369587674091
-        },
-        {
-            "x": 2.1264482972068026,
-            "y": 2.970829390858377,
-            "heading": -0.5067176556318724,
-            "angularVelocity": 0.0000017308502204807436,
-            "velocityX": 2.6946256208880865,
-            "velocityY": -2.6551126018558495,
-            "timestamp": 0.7440086979810169
-        },
-        {
-            "x": 2.1980491864833867,
-            "y": 2.9002784312191405,
-            "heading": -0.5067176096401721,
-            "angularVelocity": 0.0000017308502087562552,
-            "velocityX": 2.694625620889591,
-            "velocityY": -2.6551126018543236,
-            "timestamp": 0.7705804371946247
-        },
-        {
-            "x": 2.2696500759972245,
-            "y": 2.829727471820689,
-            "heading": -0.506717563648472,
-            "angularVelocity": 0.0000017308501985869009,
-            "velocityX": 2.6946256298184115,
-            "velocityY": -2.6551125927926265,
-            "timestamp": 0.7971521764082324
-        },
-        {
-            "x": 2.3412523738653572,
-            "y": 2.7591779417640674,
-            "heading": -0.5067175176565489,
-            "angularVelocity": 0.0000017308585893832803,
-            "velocityX": 2.694678631779742,
-            "velocityY": -2.6550588009870313,
-            "timestamp": 0.8237239156218402
-        },
-        {
-            "x": 2.4158866145337003,
-            "y": 2.6918440324740667,
-            "heading": -0.5067174710515008,
-            "angularVelocity": 0.000001753932919872258,
-            "velocityX": 2.808782672009746,
-            "velocityY": -2.5340422299311864,
-            "timestamp": 0.850295654835448
-        },
-        {
-            "x": 2.4943342208862305,
-            "y": 2.6289942264556885,
-            "heading": -0.5067174224716422,
-            "angularVelocity": 0.000001828252870098291,
-            "velocityX": 2.952294756541798,
-            "velocityY": -2.365287628074865,
-            "timestamp": 0.8768673940490558
-        },
-        {
-            "x": 2.693828949223235,
-            "y": 2.509453130751911,
-            "heading": -0.5067173754009504,
-            "angularVelocity": 7.656472375459629e-7,
-            "velocityX": 3.244961599231293,
-            "velocityY": -1.9444436869204162,
-            "timestamp": 0.9383456957905196
-        },
-        {
-            "x": 2.900420237054028,
-            "y": 2.4026436735913763,
-            "heading": -0.5067173289743672,
-            "angularVelocity": 7.551702291921459e-7,
-            "velocityX": 3.3603935368868174,
-            "velocityY": -1.7373521085488381,
-            "timestamp": 0.9998239975319835
-        },
-        {
-            "x": 3.1070116472391316,
-            "y": 2.295834453089378,
-            "heading": -0.5067172825477892,
-            "angularVelocity": 7.55170142432061e-7,
-            "velocityX": 3.3603955270899863,
-            "velocityY": -1.7373482590844154,
-            "timestamp": 1.0613022992734473
-        },
-        {
-            "x": 3.3136030574258837,
-            "y": 2.1890252325905677,
-            "heading": -0.5067172361212112,
-            "angularVelocity": 7.551701448333524e-7,
-            "velocityX": 3.3603955271167996,
-            "velocityY": -1.737348259032553,
-            "timestamp": 1.1227806010149113
-        },
-        {
-            "x": 3.5201944676126358,
-            "y": 2.0822160120917577,
-            "heading": -0.5067171896946332,
-            "angularVelocity": 7.551701452492318e-7,
-            "velocityX": 3.3603955271167996,
-            "velocityY": -1.737348259032552,
-            "timestamp": 1.1842589027563752
-        },
-        {
-            "x": 3.726785877799388,
-            "y": 1.9754067915929476,
-            "heading": -0.5067171432680552,
-            "angularVelocity": 7.5517014486464e-7,
-            "velocityX": 3.3603955271168005,
-            "velocityY": -1.7373482590325509,
-            "timestamp": 1.2457372044978392
-        },
-        {
-            "x": 3.93337728798614,
-            "y": 1.8685975710941374,
-            "heading": -0.506717096841477,
-            "angularVelocity": 7.551701472516521e-7,
-            "velocityX": 3.360395527116801,
-            "velocityY": -1.7373482590325502,
-            "timestamp": 1.3072155062393032
-        },
-        {
-            "x": 4.139968698173285,
-            "y": 1.761788350596088,
-            "heading": -0.5067170504148988,
-            "angularVelocity": 7.551701467640916e-7,
-            "velocityX": 3.360395527123197,
-            "velocityY": -1.7373482590201774,
-            "timestamp": 1.3686938079807671
-        },
-        {
-            "x": 4.346560137549085,
-            "y": 1.6549791865550314,
-            "heading": -0.5067170039883208,
-            "angularVelocity": 7.55170145558408e-7,
-            "velocityX": 3.36039600190298,
-            "velocityY": -1.737347340696294,
-            "timestamp": 1.430172109722231
-        },
-        {
-            "x": 4.5552180589852185,
-            "y": 1.552265545137979,
-            "heading": -0.5067169574392034,
-            "angularVelocity": 7.571633584219393e-7,
-            "velocityX": 3.3940091955305984,
-            "velocityY": -1.6707299731374539,
-            "timestamp": 1.491650411463695
-        },
-        {
-            "x": 4.775846094749007,
-            "y": 1.478702717632789,
-            "heading": -0.5067169090858323,
-            "angularVelocity": 7.865111701229533e-7,
-            "velocityX": 3.5887138960279197,
-            "velocityY": -1.1965657056459655,
-            "timestamp": 1.553128713205159
-        },
-        {
-            "x": 5.004393851392972,
-            "y": 1.4356429788646896,
-            "heading": -0.5067168550876733,
-            "angularVelocity": 8.783287361409535e-7,
-            "velocityX": 3.717535295706156,
-            "velocityY": -0.7004054690576753,
-            "timestamp": 1.614607014946623
-        },
-        {
-            "x": 5.236664772033691,
-            "y": 1.4238771200180054,
-            "heading": -0.5067167933216484,
-            "angularVelocity": 0.000001004680076164895,
-            "velocityX": 3.7780959145145716,
-            "velocityY": -0.19138230096471237,
-            "timestamp": 1.676085316688087
-        },
-        {
-            "x": 5.366036550697627,
-            "y": 1.4270871399969536,
-            "heading": -0.5067167282581706,
-            "angularVelocity": 0.000001901925327561791,
-            "velocityX": 3.781775437099809,
-            "velocityY": 0.09383479793163368,
-            "timestamp": 1.7102945871261934
-        },
-        {
-            "x": 5.494798697340742,
-            "y": 1.4400359689986706,
-            "heading": -0.5067166730622293,
-            "angularVelocity": 0.0000016134790500794753,
-            "velocityX": 3.76395476998197,
-            "velocityY": 0.37851812786083194,
-            "timestamp": 1.7445038575643
-        },
-        {
-            "x": 5.6222191444596294,
-            "y": 1.4626499710733982,
-            "heading": -0.5067166242756445,
-            "angularVelocity": 0.0000014261217576278581,
-            "velocityX": 3.724734421022641,
-            "velocityY": 0.6610489433161765,
-            "timestamp": 1.7787131280024064
-        },
-        {
-            "x": 5.747573461036382,
-            "y": 1.4948005438980272,
-            "heading": -0.5067165796372217,
-            "angularVelocity": 0.0000013048633380814483,
-            "velocityX": 3.6643376187618593,
-            "velocityY": 0.9398204759378852,
-            "timestamp": 1.812922398440513
-        },
-        {
-            "x": 5.870148986660447,
-            "y": 1.5363048197489622,
-            "heading": -0.5067165375293654,
-            "angularVelocity": 0.0000012308902156872765,
-            "velocityX": 3.583108439738183,
-            "velocityY": 1.2132464480944425,
-            "timestamp": 1.8471316688786195
-        },
-        {
-            "x": 5.9892489774115445,
-            "y": 1.5869265226922225,
-            "heading": -0.5067164966802475,
-            "angularVelocity": 0.0000011940949728764683,
-            "velocityX": 3.481512152285735,
-            "velocityY": 1.4797656394002383,
-            "timestamp": 1.881340939316726
-        },
-        {
-            "x": 6.104355729464833,
-            "y": 1.6460687267313168,
-            "heading": -0.5067164560018657,
-            "angularVelocity": 0.0000011891040447074693,
-            "velocityX": 3.3647824282469623,
-            "velocityY": 1.7288355840881788,
-            "timestamp": 1.9155502097548325
-        },
-        {
-            "x": 6.219456425834801,
-            "y": 1.705222715286976,
-            "heading": -0.5067164153233215,
-            "angularVelocity": 0.0000011891087921450935,
-            "velocityX": 3.3646054094668667,
-            "velocityY": 1.7291800672184492,
-            "timestamp": 1.949759480192939
-        },
-        {
-            "x": 6.334557121741728,
-            "y": 1.764376704743614,
-            "heading": -0.5067163746447773,
-            "angularVelocity": 0.0000011891087898511233,
-            "velocityX": 3.3646053959312843,
-            "velocityY": 1.7291800935557256,
-            "timestamp": 1.9839687506310455
-        },
-        {
-            "x": 6.449657817648618,
-            "y": 1.8235306942003213,
-            "heading": -0.5067163339662332,
-            "angularVelocity": 0.000001189108787218646,
-            "velocityX": 3.3646053959302495,
-            "velocityY": 1.7291800935577397,
-            "timestamp": 2.018178021069152
-        },
-        {
-            "x": 6.564758513555509,
-            "y": 1.8826846836570283,
-            "heading": -0.5067162932876892,
-            "angularVelocity": 0.0000011891087819705439,
-            "velocityX": 3.3646053959302504,
-            "velocityY": 1.7291800935577386,
-            "timestamp": 2.0523872915072587
-        },
-        {
-            "x": 6.679859209462396,
-            "y": 1.9418386731137416,
-            "heading": -0.5067162526091454,
-            "angularVelocity": 0.0000011891087784797324,
-            "velocityX": 3.3646053959301545,
-            "velocityY": 1.7291800935579253,
-            "timestamp": 2.0865965619453655
-        },
-        {
-            "x": 6.7949599053264,
-            "y": 2.0009926626538963,
-            "heading": -0.5067162119306018,
-            "angularVelocity": 0.0000011891087739781964,
-            "velocityX": 3.3646053946765995,
-            "velocityY": 1.7291800959970696,
-            "timestamp": 2.120805832383472
-        },
-        {
-            "x": 6.910060040297267,
-            "y": 2.06014774355027,
-            "heading": -0.5067161712446878,
-            "angularVelocity": 0.0000011893242211155864,
-            "velocityX": 3.3645889987368625,
-            "velocityY": 1.7292119983499907,
-            "timestamp": 2.155015102821579
-        },
-        {
-            "x": 7.017868923227757,
-            "y": 2.1174564154331827,
-            "heading": -0.4884934235175169,
-            "angularVelocity": 0.5326844885552473,
-            "velocityX": 3.151452268634164,
-            "velocityY": 1.6752380611740627,
-            "timestamp": 2.1892243732596857
-        },
-        {
-            "x": 7.119036711180674,
-            "y": 2.1700871649460596,
-            "heading": -0.45052101262699384,
-            "angularVelocity": 1.1100035284068692,
-            "velocityX": 2.9573208272872957,
-            "velocityY": 1.5384937719762113,
-            "timestamp": 2.2234336436977924
-        },
-        {
-            "x": 7.212945348428852,
-            "y": 2.2182419084801723,
-            "heading": -0.3958429177265348,
-            "angularVelocity": 1.5983414495607555,
-            "velocityX": 2.745122478366898,
-            "velocityY": 1.4076518708938115,
-            "timestamp": 2.257642914135899
-        },
-        {
-            "x": 7.29827375889861,
-            "y": 2.2619654936466005,
-            "heading": -0.3361817714679226,
-            "angularVelocity": 1.7440052212324946,
-            "velocityX": 2.494306641941948,
-            "velocityY": 1.2781209481077944,
-            "timestamp": 2.291852184574006
-        },
-        {
-            "x": 7.374929402835313,
-            "y": 2.3012175879094494,
-            "heading": -0.2767819040095215,
-            "angularVelocity": 1.7363675605380382,
-            "velocityX": 2.2407856979994336,
-            "velocityY": 1.1474110309913224,
-            "timestamp": 2.3260614550121126
-        },
-        {
-            "x": 7.442942536538503,
-            "y": 2.336017519822898,
-            "heading": -0.22036844778019443,
-            "angularVelocity": 1.6490692583284896,
-            "velocityX": 1.9881492014348112,
-            "velocityY": 1.0172661231232827,
-            "timestamp": 2.3602707254502193
-        },
-        {
-            "x": 7.502356073864926,
-            "y": 2.3663915967666074,
-            "heading": -0.16856655782281169,
-            "angularVelocity": 1.5142646801283255,
-            "velocityX": 1.7367671559648723,
-            "velocityY": 0.8878902284298598,
-            "timestamp": 2.394479995888326
-        },
-        {
-            "x": 7.553208169092694,
-            "y": 2.392365594084153,
-            "heading": -0.12246425110299042,
-            "angularVelocity": 1.3476553615264124,
-            "velocityX": 1.4865004303372251,
-            "velocityY": 0.7592677945160818,
-            "timestamp": 2.428689266326433
-        },
-        {
-            "x": 7.595530700913324,
-            "y": 2.4139635888667987,
-            "heading": -0.08285639859822795,
-            "angularVelocity": 1.1578104998299616,
-            "velocityX": 1.2371655775940182,
-            "velocityY": 0.6313491783381474,
-            "timestamp": 2.4628985367645395
-        },
-        {
-            "x": 7.629350339345417,
-            "y": 2.431207580669044,
-            "heading": -0.050358801794747596,
-            "angularVelocity": 0.9499646261757312,
-            "velocityX": 0.988610338629752,
-            "velocityY": 0.5040736496688569,
-            "timestamp": 2.4971078072026462
-        },
-        {
-            "x": 7.654689602970606,
-            "y": 2.444117248020803,
-            "heading": -0.025464280349729525,
-            "angularVelocity": 0.7277127260009457,
-            "velocityX": 0.7407133592934628,
-            "velocityY": 0.3773733606834889,
-            "timestamp": 2.531317077640753
-        },
-        {
-            "x": 7.671567523131761,
-            "y": 2.4527098140869614,
-            "heading": -0.008571412442380029,
-            "angularVelocity": 0.49380965133159116,
-            "velocityX": 0.4933727011714593,
-            "velocityY": 0.2511765365386793,
-            "timestamp": 2.5655263480788597
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 5.0379367595035626e-36,
-            "angularVelocity": 0.2505581771434718,
-            "velocityX": 0.24649683434481964,
-            "velocityY": 0.12541003821758284,
-            "timestamp": 2.5997356185169664
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 2.854280403338002e-36,
-            "angularVelocity": -6.383230494440131e-35,
-            "velocityX": -1.2082929677962258e-35,
-            "velocityY": 1.8766665143888344e-34,
-            "timestamp": 2.633944888955073
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.traj
deleted file mode 100644
index 00bde21f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.traj	
+++ /dev/null
@@ -1,679 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.687708804701003e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7140616140612069,
-            "y": 4.355992841184312,
-            "heading": -1.0347167301054676,
-            "angularVelocity": 0.1988304134727778,
-            "velocityX": 0.15285465616518656,
-            "velocityY": -0.15080528916361516,
-            "timestamp": 0.026571739213607736
-        },
-        {
-            "x": 0.7221925258884458,
-            "y": 4.3479754290630535,
-            "heading": -1.0242624006876555,
-            "angularVelocity": 0.39343790535390527,
-            "velocityX": 0.30599848063671325,
-            "velocityY": -0.3017270362623706,
-            "timestamp": 0.05314347842721547
-        },
-        {
-            "x": 0.7344011981721603,
-            "y": 4.335944497876898,
-            "heading": -1.0087613580778494,
-            "angularVelocity": 0.5833657513042195,
-            "velocityX": 0.45946078973491633,
-            "velocityY": -0.4527716868451825,
-            "timestamp": 0.0797152176408232
-        },
-        {
-            "x": 0.7506970627866689,
-            "y": 4.31989642008046,
-            "heading": -0.9883558514269565,
-            "angularVelocity": 0.7679401971717026,
-            "velocityX": 0.6132780576953482,
-            "velocityY": -0.6039528563572185,
-            "timestamp": 0.10628695685443094
-        },
-        {
-            "x": 0.7710907256657638,
-            "y": 4.29982697901967,
-            "heading": -0.963213241169254,
-            "angularVelocity": 0.9462162057057438,
-            "velocityX": 0.7674944690353961,
-            "velocityY": -0.7552927152962536,
-            "timestamp": 0.13285869606803868
-        },
-        {
-            "x": 0.7955941760833964,
-            "y": 4.27573110299105,
-            "heading": -0.9335350310204893,
-            "angularVelocity": 1.1169088297226009,
-            "velocityX": 0.9221620843352277,
-            "velocityY": -0.9068234425649082,
-            "timestamp": 0.1594304352816464
-        },
-        {
-            "x": 0.8242209914920048,
-            "y": 4.247602572989807,
-            "heading": -0.8995682913456332,
-            "angularVelocity": 1.2783032153748224,
-            "velocityX": 1.0773406730541775,
-            "velocityY": -1.0585882156647894,
-            "timestamp": 0.18600217449525414
-        },
-        {
-            "x": 0.856986533327333,
-            "y": 4.215433742997202,
-            "heading": -0.8616204330063739,
-            "angularVelocity": 1.4281285103018635,
-            "velocityX": 1.2330973735640383,
-            "velocityY": -1.210640738794065,
-            "timestamp": 0.21257391370886186
-        },
-        {
-            "x": 0.893908134971617,
-            "y": 4.179215360896215,
-            "heading": -0.8200791656995261,
-            "angularVelocity": 1.5633627506615773,
-            "velocityX": 1.3895063980372095,
-            "velocityY": -1.3630414558050108,
-            "timestamp": 0.2391456529224696
-        },
-        {
-            "x": 0.9350052770321623,
-            "y": 4.138936674970777,
-            "heading": -0.7754414952973147,
-            "angularVelocity": 1.6798926876172218,
-            "velocityX": 1.546648555074586,
-            "velocityY": -1.5158468025612346,
-            "timestamp": 0.2657173921360773
-        },
-        {
-            "x": 0.9802996637151041,
-            "y": 4.094586228803994,
-            "heading": -0.7283605055619151,
-            "angularVelocity": 1.7718444907546276,
-            "velocityX": 1.7046075275248067,
-            "velocityY": -1.669083299751435,
-            "timestamp": 0.29228913134968504
-        },
-        {
-            "x": 1.0298146765069804,
-            "y": 4.046154350498487,
-            "heading": -0.6797316150493184,
-            "angularVelocity": 1.8300981400454652,
-            "velocityX": 1.8634464381059026,
-            "velocityY": -1.8226837888242364,
-            "timestamp": 0.31886087056329276
-        },
-        {
-            "x": 1.0835715343165668,
-            "y": 3.9936405744754793,
-            "heading": -0.6308802721211105,
-            "angularVelocity": 1.8384699072761677,
-            "velocityX": 2.0230839004342207,
-            "velocityY": -1.9763017994740086,
-            "timestamp": 0.3454326097769005
-        },
-        {
-            "x": 1.1415677463510772,
-            "y": 3.937082152581159,
-            "heading": -0.5840793547040092,
-            "angularVelocity": 1.7613042579137594,
-            "velocityX": 2.1826276243449603,
-            "velocityY": -2.12851787531304,
-            "timestamp": 0.3720043489905082
-        },
-        {
-            "x": 1.203636670620202,
-            "y": 3.876777756204273,
-            "heading": -0.5447644235662668,
-            "angularVelocity": 1.4795768851144226,
-            "velocityX": 2.33589994882,
-            "velocityY": -2.2694937614773476,
-            "timestamp": 0.39857608820411594
-        },
-        {
-            "x": 1.2693974629594884,
-            "y": 3.8133907541919347,
-            "heading": -0.5190150912676422,
-            "angularVelocity": 0.969049563960718,
-            "velocityX": 2.4748395959572558,
-            "velocityY": -2.3855044452595306,
-            "timestamp": 0.42514782741772367
-        },
-        {
-            "x": 1.3388335264794184,
-            "y": 3.7468848835898942,
-            "heading": -0.5067181615747146,
-            "angularVelocity": 0.4627822663045784,
-            "velocityX": 2.613154636275026,
-            "velocityY": -2.5028798479243823,
-            "timestamp": 0.4517195666313314
-        },
-        {
-            "x": 1.410439403600934,
-            "y": 3.676338986398213,
-            "heading": -0.5067181155488927,
-            "angularVelocity": 0.0000017321343363764108,
-            "velocityX": 2.6948133332892708,
-            "velocityY": -2.6549220818618244,
-            "timestamp": 0.4782913058449391
-        },
-        {
-            "x": 1.482040293717765,
-            "y": 3.605788027611728,
-            "heading": -0.5067180695571892,
-            "angularVelocity": 0.0000017308503249172533,
-            "velocityX": 2.694625652511417,
-            "velocityY": -2.655112569761896,
-            "timestamp": 0.5048630450585468
-        },
-        {
-            "x": 1.5536411829944505,
-            "y": 3.5352370679725946,
-            "heading": -0.5067180235654862,
-            "angularVelocity": 0.0000017308503100877725,
-            "velocityX": 2.6946256208934063,
-            "velocityY": -2.6551126018504427,
-            "timestamp": 0.5314347842721546
-        },
-        {
-            "x": 1.6252420722709946,
-            "y": 3.4646861083333174,
-            "heading": -0.5067179775737835,
-            "angularVelocity": 0.0000017308502987237163,
-            "velocityX": 2.6946256208880808,
-            "velocityY": -2.6551126018558486,
-            "timestamp": 0.5580065234857624
-        },
-        {
-            "x": 1.6968429615475384,
-            "y": 3.39413514869404,
-            "heading": -0.506717931582081,
-            "angularVelocity": 0.0000017308502871423956,
-            "velocityX": 2.6946256208880808,
-            "velocityY": -2.65511260185585,
-            "timestamp": 0.5845782626993702
-        },
-        {
-            "x": 1.7684438508240825,
-            "y": 3.323584189054763,
-            "heading": -0.5067178855903789,
-            "angularVelocity": 0.0000017308502762001758,
-            "velocityX": 2.694625620888081,
-            "velocityY": -2.65511260185585,
-            "timestamp": 0.611150001912978
-        },
-        {
-            "x": 1.8400447401006266,
-            "y": 3.2530332294154856,
-            "heading": -0.506717839598677,
-            "angularVelocity": 0.000001730850265567339,
-            "velocityX": 2.6946256208880826,
-            "velocityY": -2.6551126018558495,
-            "timestamp": 0.6377217411265858
-        },
-        {
-            "x": 1.9116456293771706,
-            "y": 3.1824822697762087,
-            "heading": -0.5067177936069754,
-            "angularVelocity": 0.0000017308502535085818,
-            "velocityX": 2.694625620888084,
-            "velocityY": -2.65511260185585,
-            "timestamp": 0.6642934803401935
-        },
-        {
-            "x": 1.9832465186537145,
-            "y": 3.1119313101369315,
-            "heading": -0.5067177476152741,
-            "angularVelocity": 0.0000017308502432657092,
-            "velocityX": 2.6946256208880843,
-            "velocityY": -2.65511260185585,
-            "timestamp": 0.6908652195538013
-        },
-        {
-            "x": 2.0548474079302586,
-            "y": 3.041380350497654,
-            "heading": -0.5067177016235731,
-            "angularVelocity": 0.000001730850231529751,
-            "velocityX": 2.694625620888085,
-            "velocityY": -2.6551126018558504,
-            "timestamp": 0.7174369587674091
-        },
-        {
-            "x": 2.1264482972068026,
-            "y": 2.970829390858377,
-            "heading": -0.5067176556318724,
-            "angularVelocity": 0.0000017308502204807436,
-            "velocityX": 2.6946256208880865,
-            "velocityY": -2.6551126018558495,
-            "timestamp": 0.7440086979810169
-        },
-        {
-            "x": 2.1980491864833867,
-            "y": 2.9002784312191405,
-            "heading": -0.5067176096401721,
-            "angularVelocity": 0.0000017308502087562552,
-            "velocityX": 2.694625620889591,
-            "velocityY": -2.6551126018543236,
-            "timestamp": 0.7705804371946247
-        },
-        {
-            "x": 2.2696500759972245,
-            "y": 2.829727471820689,
-            "heading": -0.506717563648472,
-            "angularVelocity": 0.0000017308501985869009,
-            "velocityX": 2.6946256298184115,
-            "velocityY": -2.6551125927926265,
-            "timestamp": 0.7971521764082324
-        },
-        {
-            "x": 2.3412523738653572,
-            "y": 2.7591779417640674,
-            "heading": -0.5067175176565489,
-            "angularVelocity": 0.0000017308585893832803,
-            "velocityX": 2.694678631779742,
-            "velocityY": -2.6550588009870313,
-            "timestamp": 0.8237239156218402
-        },
-        {
-            "x": 2.4158866145337003,
-            "y": 2.6918440324740667,
-            "heading": -0.5067174710515008,
-            "angularVelocity": 0.000001753932919872258,
-            "velocityX": 2.808782672009746,
-            "velocityY": -2.5340422299311864,
-            "timestamp": 0.850295654835448
-        },
-        {
-            "x": 2.4943342208862305,
-            "y": 2.6289942264556885,
-            "heading": -0.5067174224716422,
-            "angularVelocity": 0.000001828252870098291,
-            "velocityX": 2.952294756541798,
-            "velocityY": -2.365287628074865,
-            "timestamp": 0.8768673940490558
-        },
-        {
-            "x": 2.693828949223235,
-            "y": 2.509453130751911,
-            "heading": -0.5067173754009504,
-            "angularVelocity": 7.656472375459629e-7,
-            "velocityX": 3.244961599231293,
-            "velocityY": -1.9444436869204162,
-            "timestamp": 0.9383456957905196
-        },
-        {
-            "x": 2.900420237054028,
-            "y": 2.4026436735913763,
-            "heading": -0.5067173289743672,
-            "angularVelocity": 7.551702291921459e-7,
-            "velocityX": 3.3603935368868174,
-            "velocityY": -1.7373521085488381,
-            "timestamp": 0.9998239975319835
-        },
-        {
-            "x": 3.1070116472391316,
-            "y": 2.295834453089378,
-            "heading": -0.5067172825477892,
-            "angularVelocity": 7.55170142432061e-7,
-            "velocityX": 3.3603955270899863,
-            "velocityY": -1.7373482590844154,
-            "timestamp": 1.0613022992734473
-        },
-        {
-            "x": 3.3136030574258837,
-            "y": 2.1890252325905677,
-            "heading": -0.5067172361212112,
-            "angularVelocity": 7.551701448333524e-7,
-            "velocityX": 3.3603955271167996,
-            "velocityY": -1.737348259032553,
-            "timestamp": 1.1227806010149113
-        },
-        {
-            "x": 3.5201944676126358,
-            "y": 2.0822160120917577,
-            "heading": -0.5067171896946332,
-            "angularVelocity": 7.551701452492318e-7,
-            "velocityX": 3.3603955271167996,
-            "velocityY": -1.737348259032552,
-            "timestamp": 1.1842589027563752
-        },
-        {
-            "x": 3.726785877799388,
-            "y": 1.9754067915929476,
-            "heading": -0.5067171432680552,
-            "angularVelocity": 7.5517014486464e-7,
-            "velocityX": 3.3603955271168005,
-            "velocityY": -1.7373482590325509,
-            "timestamp": 1.2457372044978392
-        },
-        {
-            "x": 3.93337728798614,
-            "y": 1.8685975710941374,
-            "heading": -0.506717096841477,
-            "angularVelocity": 7.551701472516521e-7,
-            "velocityX": 3.360395527116801,
-            "velocityY": -1.7373482590325502,
-            "timestamp": 1.3072155062393032
-        },
-        {
-            "x": 4.139968698173285,
-            "y": 1.761788350596088,
-            "heading": -0.5067170504148988,
-            "angularVelocity": 7.551701467640916e-7,
-            "velocityX": 3.360395527123197,
-            "velocityY": -1.7373482590201774,
-            "timestamp": 1.3686938079807671
-        },
-        {
-            "x": 4.346560137549085,
-            "y": 1.6549791865550314,
-            "heading": -0.5067170039883208,
-            "angularVelocity": 7.55170145558408e-7,
-            "velocityX": 3.36039600190298,
-            "velocityY": -1.737347340696294,
-            "timestamp": 1.430172109722231
-        },
-        {
-            "x": 4.5552180589852185,
-            "y": 1.552265545137979,
-            "heading": -0.5067169574392034,
-            "angularVelocity": 7.571633584219393e-7,
-            "velocityX": 3.3940091955305984,
-            "velocityY": -1.6707299731374539,
-            "timestamp": 1.491650411463695
-        },
-        {
-            "x": 4.775846094749007,
-            "y": 1.478702717632789,
-            "heading": -0.5067169090858323,
-            "angularVelocity": 7.865111701229533e-7,
-            "velocityX": 3.5887138960279197,
-            "velocityY": -1.1965657056459655,
-            "timestamp": 1.553128713205159
-        },
-        {
-            "x": 5.004393851392972,
-            "y": 1.4356429788646896,
-            "heading": -0.5067168550876733,
-            "angularVelocity": 8.783287361409535e-7,
-            "velocityX": 3.717535295706156,
-            "velocityY": -0.7004054690576753,
-            "timestamp": 1.614607014946623
-        },
-        {
-            "x": 5.236664772033691,
-            "y": 1.4238771200180054,
-            "heading": -0.5067167933216484,
-            "angularVelocity": 0.000001004680076164895,
-            "velocityX": 3.7780959145145716,
-            "velocityY": -0.19138230096471237,
-            "timestamp": 1.676085316688087
-        },
-        {
-            "x": 5.366036550697627,
-            "y": 1.4270871399969536,
-            "heading": -0.5067167282581706,
-            "angularVelocity": 0.000001901925327561791,
-            "velocityX": 3.781775437099809,
-            "velocityY": 0.09383479793163368,
-            "timestamp": 1.7102945871261934
-        },
-        {
-            "x": 5.494798697340742,
-            "y": 1.4400359689986706,
-            "heading": -0.5067166730622293,
-            "angularVelocity": 0.0000016134790500794753,
-            "velocityX": 3.76395476998197,
-            "velocityY": 0.37851812786083194,
-            "timestamp": 1.7445038575643
-        },
-        {
-            "x": 5.6222191444596294,
-            "y": 1.4626499710733982,
-            "heading": -0.5067166242756445,
-            "angularVelocity": 0.0000014261217576278581,
-            "velocityX": 3.724734421022641,
-            "velocityY": 0.6610489433161765,
-            "timestamp": 1.7787131280024064
-        },
-        {
-            "x": 5.747573461036382,
-            "y": 1.4948005438980272,
-            "heading": -0.5067165796372217,
-            "angularVelocity": 0.0000013048633380814483,
-            "velocityX": 3.6643376187618593,
-            "velocityY": 0.9398204759378852,
-            "timestamp": 1.812922398440513
-        },
-        {
-            "x": 5.870148986660447,
-            "y": 1.5363048197489622,
-            "heading": -0.5067165375293654,
-            "angularVelocity": 0.0000012308902156872765,
-            "velocityX": 3.583108439738183,
-            "velocityY": 1.2132464480944425,
-            "timestamp": 1.8471316688786195
-        },
-        {
-            "x": 5.9892489774115445,
-            "y": 1.5869265226922225,
-            "heading": -0.5067164966802475,
-            "angularVelocity": 0.0000011940949728764683,
-            "velocityX": 3.481512152285735,
-            "velocityY": 1.4797656394002383,
-            "timestamp": 1.881340939316726
-        },
-        {
-            "x": 6.104355729464833,
-            "y": 1.6460687267313168,
-            "heading": -0.5067164560018657,
-            "angularVelocity": 0.0000011891040447074693,
-            "velocityX": 3.3647824282469623,
-            "velocityY": 1.7288355840881788,
-            "timestamp": 1.9155502097548325
-        },
-        {
-            "x": 6.219456425834801,
-            "y": 1.705222715286976,
-            "heading": -0.5067164153233215,
-            "angularVelocity": 0.0000011891087921450935,
-            "velocityX": 3.3646054094668667,
-            "velocityY": 1.7291800672184492,
-            "timestamp": 1.949759480192939
-        },
-        {
-            "x": 6.334557121741728,
-            "y": 1.764376704743614,
-            "heading": -0.5067163746447773,
-            "angularVelocity": 0.0000011891087898511233,
-            "velocityX": 3.3646053959312843,
-            "velocityY": 1.7291800935557256,
-            "timestamp": 1.9839687506310455
-        },
-        {
-            "x": 6.449657817648618,
-            "y": 1.8235306942003213,
-            "heading": -0.5067163339662332,
-            "angularVelocity": 0.000001189108787218646,
-            "velocityX": 3.3646053959302495,
-            "velocityY": 1.7291800935577397,
-            "timestamp": 2.018178021069152
-        },
-        {
-            "x": 6.564758513555509,
-            "y": 1.8826846836570283,
-            "heading": -0.5067162932876892,
-            "angularVelocity": 0.0000011891087819705439,
-            "velocityX": 3.3646053959302504,
-            "velocityY": 1.7291800935577386,
-            "timestamp": 2.0523872915072587
-        },
-        {
-            "x": 6.679859209462396,
-            "y": 1.9418386731137416,
-            "heading": -0.5067162526091454,
-            "angularVelocity": 0.0000011891087784797324,
-            "velocityX": 3.3646053959301545,
-            "velocityY": 1.7291800935579253,
-            "timestamp": 2.0865965619453655
-        },
-        {
-            "x": 6.7949599053264,
-            "y": 2.0009926626538963,
-            "heading": -0.5067162119306018,
-            "angularVelocity": 0.0000011891087739781964,
-            "velocityX": 3.3646053946765995,
-            "velocityY": 1.7291800959970696,
-            "timestamp": 2.120805832383472
-        },
-        {
-            "x": 6.910060040297267,
-            "y": 2.06014774355027,
-            "heading": -0.5067161712446878,
-            "angularVelocity": 0.0000011893242211155864,
-            "velocityX": 3.3645889987368625,
-            "velocityY": 1.7292119983499907,
-            "timestamp": 2.155015102821579
-        },
-        {
-            "x": 7.017868923227757,
-            "y": 2.1174564154331827,
-            "heading": -0.4884934235175169,
-            "angularVelocity": 0.5326844885552473,
-            "velocityX": 3.151452268634164,
-            "velocityY": 1.6752380611740627,
-            "timestamp": 2.1892243732596857
-        },
-        {
-            "x": 7.119036711180674,
-            "y": 2.1700871649460596,
-            "heading": -0.45052101262699384,
-            "angularVelocity": 1.1100035284068692,
-            "velocityX": 2.9573208272872957,
-            "velocityY": 1.5384937719762113,
-            "timestamp": 2.2234336436977924
-        },
-        {
-            "x": 7.212945348428852,
-            "y": 2.2182419084801723,
-            "heading": -0.3958429177265348,
-            "angularVelocity": 1.5983414495607555,
-            "velocityX": 2.745122478366898,
-            "velocityY": 1.4076518708938115,
-            "timestamp": 2.257642914135899
-        },
-        {
-            "x": 7.29827375889861,
-            "y": 2.2619654936466005,
-            "heading": -0.3361817714679226,
-            "angularVelocity": 1.7440052212324946,
-            "velocityX": 2.494306641941948,
-            "velocityY": 1.2781209481077944,
-            "timestamp": 2.291852184574006
-        },
-        {
-            "x": 7.374929402835313,
-            "y": 2.3012175879094494,
-            "heading": -0.2767819040095215,
-            "angularVelocity": 1.7363675605380382,
-            "velocityX": 2.2407856979994336,
-            "velocityY": 1.1474110309913224,
-            "timestamp": 2.3260614550121126
-        },
-        {
-            "x": 7.442942536538503,
-            "y": 2.336017519822898,
-            "heading": -0.22036844778019443,
-            "angularVelocity": 1.6490692583284896,
-            "velocityX": 1.9881492014348112,
-            "velocityY": 1.0172661231232827,
-            "timestamp": 2.3602707254502193
-        },
-        {
-            "x": 7.502356073864926,
-            "y": 2.3663915967666074,
-            "heading": -0.16856655782281169,
-            "angularVelocity": 1.5142646801283255,
-            "velocityX": 1.7367671559648723,
-            "velocityY": 0.8878902284298598,
-            "timestamp": 2.394479995888326
-        },
-        {
-            "x": 7.553208169092694,
-            "y": 2.392365594084153,
-            "heading": -0.12246425110299042,
-            "angularVelocity": 1.3476553615264124,
-            "velocityX": 1.4865004303372251,
-            "velocityY": 0.7592677945160818,
-            "timestamp": 2.428689266326433
-        },
-        {
-            "x": 7.595530700913324,
-            "y": 2.4139635888667987,
-            "heading": -0.08285639859822795,
-            "angularVelocity": 1.1578104998299616,
-            "velocityX": 1.2371655775940182,
-            "velocityY": 0.6313491783381474,
-            "timestamp": 2.4628985367645395
-        },
-        {
-            "x": 7.629350339345417,
-            "y": 2.431207580669044,
-            "heading": -0.050358801794747596,
-            "angularVelocity": 0.9499646261757312,
-            "velocityX": 0.988610338629752,
-            "velocityY": 0.5040736496688569,
-            "timestamp": 2.4971078072026462
-        },
-        {
-            "x": 7.654689602970606,
-            "y": 2.444117248020803,
-            "heading": -0.025464280349729525,
-            "angularVelocity": 0.7277127260009457,
-            "velocityX": 0.7407133592934628,
-            "velocityY": 0.3773733606834889,
-            "timestamp": 2.531317077640753
-        },
-        {
-            "x": 7.671567523131761,
-            "y": 2.4527098140869614,
-            "heading": -0.008571412442380029,
-            "angularVelocity": 0.49380965133159116,
-            "velocityX": 0.4933727011714593,
-            "velocityY": 0.2511765365386793,
-            "timestamp": 2.5655263480788597
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 5.0379367595035626e-36,
-            "angularVelocity": 0.2505581771434718,
-            "velocityX": 0.24649683434481964,
-            "velocityY": 0.12541003821758284,
-            "timestamp": 2.5997356185169664
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 2.854280403338002e-36,
-            "angularVelocity": -6.383230494440131e-35,
-            "velocityX": -1.2082929677962258e-35,
-            "velocityY": 1.8766665143888344e-34,
-            "timestamp": 2.633944888955073
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.1.traj
deleted file mode 100644
index 818f5ad4..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.1.traj	
+++ /dev/null
@@ -1,337 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 1.4815405069278332e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7317173879428727,
-            "y": 4.344479531295635,
-            "heading": -1.013582317544266,
-            "angularVelocity": 0.4585462516411977,
-            "velocityX": 0.37696065327942924,
-            "velocityY": -0.26939731598432753,
-            "timestamp": 0.05761181638969176
-        },
-        {
-            "x": 0.7752730705207751,
-            "y": 4.313424298338961,
-            "heading": -0.9621480921974644,
-            "angularVelocity": 0.892772152832256,
-            "velocityX": 0.7560199505477795,
-            "velocityY": -0.5390427676609577,
-            "timestamp": 0.11522363277938352
-        },
-        {
-            "x": 0.8408250898493527,
-            "y": 4.266812487548848,
-            "heading": -0.8877439224939486,
-            "angularVelocity": 1.2914741170498636,
-            "velocityX": 1.1378224717856051,
-            "velocityY": -0.8090668496689294,
-            "timestamp": 0.17283544916907528
-        },
-        {
-            "x": 0.9285847241631874,
-            "y": 4.204600540366139,
-            "heading": -0.7936992773737622,
-            "angularVelocity": 1.6323846567179892,
-            "velocityX": 1.5232922656043415,
-            "velocityY": -1.0798470015578314,
-            "timestamp": 0.23044726555876704
-        },
-        {
-            "x": 1.0388213735909644,
-            "y": 4.1267152282410935,
-            "heading": -0.6856850144743787,
-            "angularVelocity": 1.8748629997840256,
-            "velocityX": 1.9134381857035365,
-            "velocityY": -1.3518982216811457,
-            "timestamp": 0.2880590819484588
-        },
-        {
-            "x": 1.1718232312823271,
-            "y": 4.033123229014291,
-            "heading": -0.5741627817643435,
-            "angularVelocity": 1.935752762171023,
-            "velocityX": 2.3085864328894954,
-            "velocityY": -1.6245278328622093,
-            "timestamp": 0.34567089833815057
-        },
-        {
-            "x": 1.3270687303513053,
-            "y": 3.9246703032584405,
-            "heading": -0.488682735559766,
-            "angularVelocity": 1.4837242003685942,
-            "velocityX": 2.6946815566251776,
-            "velocityY": -1.882477112373353,
-            "timestamp": 0.40328271472784233
-        },
-        {
-            "x": 1.5010681185276094,
-            "y": 3.804139759424889,
-            "heading": -0.46956462349945394,
-            "angularVelocity": 0.3318435914430377,
-            "velocityX": 3.020203129846763,
-            "velocityY": -2.0921149754812625,
-            "timestamp": 0.4608945311175341
-        },
-        {
-            "x": 1.6785494425409546,
-            "y": 3.677652137140934,
-            "heading": -0.4695645649556455,
-            "angularVelocity": 0.0000010161770978802793,
-            "velocityX": 3.080641006227699,
-            "velocityY": -2.195515264236428,
-            "timestamp": 0.5185063475072258
-        },
-        {
-            "x": 1.85603075289391,
-            "y": 3.551164495689284,
-            "heading": -0.4695645064119953,
-            "angularVelocity": 0.000001016174352223477,
-            "velocityX": 3.080640769116793,
-            "velocityY": -2.1955155969406546,
-            "timestamp": 0.5761181638969175
-        },
-        {
-            "x": 2.033512086622271,
-            "y": 3.4246768870368545,
-            "heading": -0.4695644478683731,
-            "angularVelocity": 0.0000010161738659585955,
-            "velocityX": 3.0806411748565776,
-            "velocityY": -2.195515027626543,
-            "timestamp": 0.6337299802866092
-        },
-        {
-            "x": 2.2109934521039283,
-            "y": 3.298189322939119,
-            "heading": -0.46956438932476746,
-            "angularVelocity": 0.0000010161735782211743,
-            "velocityX": 3.080641726016013,
-            "velocityY": -2.1955142542661994,
-            "timestamp": 0.6913417966763009
-        },
-        {
-            "x": 2.3884748618363862,
-            "y": 3.17170182093203,
-            "heading": -0.4695643307811488,
-            "angularVelocity": 0.000001016173805422874,
-            "velocityX": 3.0806424941015065,
-            "velocityY": -2.1955131765246763,
-            "timestamp": 0.7489536130659926
-        },
-        {
-            "x": 2.5659563234551954,
-            "y": 3.045214391729511,
-            "heading": -0.4695642722374779,
-            "angularVelocity": 0.0000010161747112988211,
-            "velocityX": 3.0806433947214558,
-            "velocityY": -2.1955119128156926,
-            "timestamp": 0.8065654294556843
-        },
-        {
-            "x": 2.743437831089148,
-            "y": 2.918727027093402,
-            "heading": -0.46956421369376844,
-            "angularVelocity": 0.000001016175381373278,
-            "velocityX": 3.08064419343162,
-            "velocityY": -2.1955107921009867,
-            "timestamp": 0.864177245845376
-        },
-        {
-            "x": 2.920919379832659,
-            "y": 2.7922397201404365,
-            "heading": -0.4695641551500532,
-            "angularVelocity": 0.0000010161754817816058,
-            "velocityX": 3.080644906992851,
-            "velocityY": -2.195509790862922,
-            "timestamp": 0.9217890622350677
-        },
-        {
-            "x": 3.0984009676669872,
-            "y": 2.6657524680380327,
-            "heading": -0.4695640966063568,
-            "angularVelocity": 0.000001016175153333812,
-            "velocityX": 3.080645585513673,
-            "velocityY": -2.195508838791537,
-            "timestamp": 0.9794008786247594
-        },
-        {
-            "x": 3.275882647398041,
-            "y": 2.5392653448813247,
-            "heading": -0.4695640380626867,
-            "angularVelocity": 0.0000010161746978103879,
-            "velocityX": 3.080647180615714,
-            "velocityY": -2.1955066006101345,
-            "timestamp": 1.0370126950144511
-        },
-        {
-            "x": 3.4563703536987305,
-            "y": 2.417105913162231,
-            "heading": -0.4695639788949644,
-            "angularVelocity": 0.0000010270067149252654,
-            "velocityX": 3.1328244379564545,
-            "velocityY": -2.1203884788634437,
-            "timestamp": 1.0946245114041429
-        },
-        {
-            "x": 3.7578524899325245,
-            "y": 2.288322056940197,
-            "heading": -0.4695639274563231,
-            "angularVelocity": 5.9355601779034e-7,
-            "velocityX": 3.4788348155609183,
-            "velocityY": -1.4860507766867785,
-            "timestamp": 1.1812863255510104
-        },
-        {
-            "x": 4.061243078218464,
-            "y": 2.1641008385808727,
-            "heading": -0.46956387627292245,
-            "angularVelocity": 5.906107685245515e-7,
-            "velocityX": 3.5008566491785738,
-            "velocityY": -1.43340200735705,
-            "timestamp": 1.267948139697878
-        },
-        {
-            "x": 4.364633703659485,
-            "y": 2.039879710966971,
-            "heading": -0.46956382508952316,
-            "angularVelocity": 5.906107531304183e-7,
-            "velocityX": 3.5008570779150574,
-            "velocityY": -1.433400960235855,
-            "timestamp": 1.3546099538447456
-        },
-        {
-            "x": 4.668024366282006,
-            "y": 1.9156586741630988,
-            "heading": -0.46956377390610504,
-            "angularVelocity": 5.906109701114576e-7,
-            "velocityX": 3.5008575069563967,
-            "velocityY": -1.4333999123691519,
-            "timestamp": 1.4412717679916132
-        },
-        {
-            "x": 4.971415087165123,
-            "y": 1.7914377796518506,
-            "heading": -0.4695637227226054,
-            "angularVelocity": 5.906119107964098e-7,
-            "velocityX": 3.5008581792316793,
-            "velocityY": -1.4333982704392505,
-            "timestamp": 1.5279335821384807
-        },
-        {
-            "x": 5.2748058711699946,
-            "y": 1.6672170393060612,
-            "heading": -0.4695636715390663,
-            "angularVelocity": 5.906123657897232e-7,
-            "velocityX": 3.5008589076003997,
-            "velocityY": -1.4333964915074275,
-            "timestamp": 1.6145953962853483
-        },
-        {
-            "x": 5.5781967938554144,
-            "y": 1.5429966376674569,
-            "heading": -0.4695636203556594,
-            "angularVelocity": 5.906108405690599e-7,
-            "velocityX": 3.5008605078501667,
-            "velocityY": -1.4333925831287677,
-            "timestamp": 1.701257210432216
-        },
-        {
-            "x": 5.886988948403778,
-            "y": 1.4328868446307816,
-            "heading": -0.4695635793301605,
-            "angularVelocity": 4.733976467078746e-7,
-            "velocityX": 3.5631859035981908,
-            "velocityY": -1.2705687518849984,
-            "timestamp": 1.7879190245790835
-        },
-        {
-            "x": 6.195781169913944,
-            "y": 1.3227772393821355,
-            "heading": -0.4695635383046595,
-            "angularVelocity": 4.7339767079441426e-7,
-            "velocityX": 3.5631866762776085,
-            "velocityY": -1.2705665849787193,
-            "timestamp": 1.874580838725951
-        },
-        {
-            "x": 6.5045734104803365,
-            "y": 1.212667687574965,
-            "heading": -0.46956349727913566,
-            "angularVelocity": 4.733979354519185e-7,
-            "velocityX": 3.5631868961694693,
-            "velocityY": -1.270565968311784,
-            "timestamp": 1.9612426528728186
-        },
-        {
-            "x": 6.813365656566466,
-            "y": 1.1025581512474965,
-            "heading": -0.46956345625352064,
-            "angularVelocity": 4.733989867793667e-7,
-            "velocityX": 3.563186959862305,
-            "velocityY": -1.270565789689833,
-            "timestamp": 2.0479044670196864
-        },
-        {
-            "x": 7.101065618447083,
-            "y": 0.9964425714529646,
-            "heading": -0.4012732100130825,
-            "angularVelocity": 0.7880085007764233,
-            "velocityX": 3.3198008224596522,
-            "velocityY": -1.2244790954259896,
-            "timestamp": 2.134566281166554
-        },
-        {
-            "x": 7.333266238045498,
-            "y": 0.911992130350644,
-            "heading": -0.2613611242014092,
-            "angularVelocity": 1.6144606155435655,
-            "velocityX": 2.6793879390165927,
-            "velocityY": -0.9744827284506232,
-            "timestamp": 2.2212280953134216
-        },
-        {
-            "x": 7.506890250099573,
-            "y": 0.8492417427348896,
-            "heading": -0.1371754084070899,
-            "angularVelocity": 1.43299233943868,
-            "velocityX": 2.0034661605379225,
-            "velocityY": -0.7240834759056616,
-            "timestamp": 2.307889909460289
-        },
-        {
-            "x": 7.622366585478705,
-            "y": 0.8076799304753898,
-            "heading": -0.04732610702177306,
-            "angularVelocity": 1.0367807582823931,
-            "velocityX": 1.332493861522807,
-            "velocityY": -0.4795862245517285,
-            "timestamp": 2.3945517236071567
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 2.9489608951695904e-31,
-            "angularVelocity": 0.5461010421679925,
-            "velocityX": 0.6650381726791765,
-            "velocityY": -0.2386279433331854,
-            "timestamp": 2.4812135377540243
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -7.162012659830636e-32,
-            "angularVelocity": -4.0878892657023166e-30,
-            "velocityX": 2.3668785600931567e-32,
-            "velocityY": 6.81692291678628e-32,
-            "timestamp": 2.567875351900892
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.traj
deleted file mode 100644
index 818f5ad4..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.traj	
+++ /dev/null
@@ -1,337 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 1.4815405069278332e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7317173879428727,
-            "y": 4.344479531295635,
-            "heading": -1.013582317544266,
-            "angularVelocity": 0.4585462516411977,
-            "velocityX": 0.37696065327942924,
-            "velocityY": -0.26939731598432753,
-            "timestamp": 0.05761181638969176
-        },
-        {
-            "x": 0.7752730705207751,
-            "y": 4.313424298338961,
-            "heading": -0.9621480921974644,
-            "angularVelocity": 0.892772152832256,
-            "velocityX": 0.7560199505477795,
-            "velocityY": -0.5390427676609577,
-            "timestamp": 0.11522363277938352
-        },
-        {
-            "x": 0.8408250898493527,
-            "y": 4.266812487548848,
-            "heading": -0.8877439224939486,
-            "angularVelocity": 1.2914741170498636,
-            "velocityX": 1.1378224717856051,
-            "velocityY": -0.8090668496689294,
-            "timestamp": 0.17283544916907528
-        },
-        {
-            "x": 0.9285847241631874,
-            "y": 4.204600540366139,
-            "heading": -0.7936992773737622,
-            "angularVelocity": 1.6323846567179892,
-            "velocityX": 1.5232922656043415,
-            "velocityY": -1.0798470015578314,
-            "timestamp": 0.23044726555876704
-        },
-        {
-            "x": 1.0388213735909644,
-            "y": 4.1267152282410935,
-            "heading": -0.6856850144743787,
-            "angularVelocity": 1.8748629997840256,
-            "velocityX": 1.9134381857035365,
-            "velocityY": -1.3518982216811457,
-            "timestamp": 0.2880590819484588
-        },
-        {
-            "x": 1.1718232312823271,
-            "y": 4.033123229014291,
-            "heading": -0.5741627817643435,
-            "angularVelocity": 1.935752762171023,
-            "velocityX": 2.3085864328894954,
-            "velocityY": -1.6245278328622093,
-            "timestamp": 0.34567089833815057
-        },
-        {
-            "x": 1.3270687303513053,
-            "y": 3.9246703032584405,
-            "heading": -0.488682735559766,
-            "angularVelocity": 1.4837242003685942,
-            "velocityX": 2.6946815566251776,
-            "velocityY": -1.882477112373353,
-            "timestamp": 0.40328271472784233
-        },
-        {
-            "x": 1.5010681185276094,
-            "y": 3.804139759424889,
-            "heading": -0.46956462349945394,
-            "angularVelocity": 0.3318435914430377,
-            "velocityX": 3.020203129846763,
-            "velocityY": -2.0921149754812625,
-            "timestamp": 0.4608945311175341
-        },
-        {
-            "x": 1.6785494425409546,
-            "y": 3.677652137140934,
-            "heading": -0.4695645649556455,
-            "angularVelocity": 0.0000010161770978802793,
-            "velocityX": 3.080641006227699,
-            "velocityY": -2.195515264236428,
-            "timestamp": 0.5185063475072258
-        },
-        {
-            "x": 1.85603075289391,
-            "y": 3.551164495689284,
-            "heading": -0.4695645064119953,
-            "angularVelocity": 0.000001016174352223477,
-            "velocityX": 3.080640769116793,
-            "velocityY": -2.1955155969406546,
-            "timestamp": 0.5761181638969175
-        },
-        {
-            "x": 2.033512086622271,
-            "y": 3.4246768870368545,
-            "heading": -0.4695644478683731,
-            "angularVelocity": 0.0000010161738659585955,
-            "velocityX": 3.0806411748565776,
-            "velocityY": -2.195515027626543,
-            "timestamp": 0.6337299802866092
-        },
-        {
-            "x": 2.2109934521039283,
-            "y": 3.298189322939119,
-            "heading": -0.46956438932476746,
-            "angularVelocity": 0.0000010161735782211743,
-            "velocityX": 3.080641726016013,
-            "velocityY": -2.1955142542661994,
-            "timestamp": 0.6913417966763009
-        },
-        {
-            "x": 2.3884748618363862,
-            "y": 3.17170182093203,
-            "heading": -0.4695643307811488,
-            "angularVelocity": 0.000001016173805422874,
-            "velocityX": 3.0806424941015065,
-            "velocityY": -2.1955131765246763,
-            "timestamp": 0.7489536130659926
-        },
-        {
-            "x": 2.5659563234551954,
-            "y": 3.045214391729511,
-            "heading": -0.4695642722374779,
-            "angularVelocity": 0.0000010161747112988211,
-            "velocityX": 3.0806433947214558,
-            "velocityY": -2.1955119128156926,
-            "timestamp": 0.8065654294556843
-        },
-        {
-            "x": 2.743437831089148,
-            "y": 2.918727027093402,
-            "heading": -0.46956421369376844,
-            "angularVelocity": 0.000001016175381373278,
-            "velocityX": 3.08064419343162,
-            "velocityY": -2.1955107921009867,
-            "timestamp": 0.864177245845376
-        },
-        {
-            "x": 2.920919379832659,
-            "y": 2.7922397201404365,
-            "heading": -0.4695641551500532,
-            "angularVelocity": 0.0000010161754817816058,
-            "velocityX": 3.080644906992851,
-            "velocityY": -2.195509790862922,
-            "timestamp": 0.9217890622350677
-        },
-        {
-            "x": 3.0984009676669872,
-            "y": 2.6657524680380327,
-            "heading": -0.4695640966063568,
-            "angularVelocity": 0.000001016175153333812,
-            "velocityX": 3.080645585513673,
-            "velocityY": -2.195508838791537,
-            "timestamp": 0.9794008786247594
-        },
-        {
-            "x": 3.275882647398041,
-            "y": 2.5392653448813247,
-            "heading": -0.4695640380626867,
-            "angularVelocity": 0.0000010161746978103879,
-            "velocityX": 3.080647180615714,
-            "velocityY": -2.1955066006101345,
-            "timestamp": 1.0370126950144511
-        },
-        {
-            "x": 3.4563703536987305,
-            "y": 2.417105913162231,
-            "heading": -0.4695639788949644,
-            "angularVelocity": 0.0000010270067149252654,
-            "velocityX": 3.1328244379564545,
-            "velocityY": -2.1203884788634437,
-            "timestamp": 1.0946245114041429
-        },
-        {
-            "x": 3.7578524899325245,
-            "y": 2.288322056940197,
-            "heading": -0.4695639274563231,
-            "angularVelocity": 5.9355601779034e-7,
-            "velocityX": 3.4788348155609183,
-            "velocityY": -1.4860507766867785,
-            "timestamp": 1.1812863255510104
-        },
-        {
-            "x": 4.061243078218464,
-            "y": 2.1641008385808727,
-            "heading": -0.46956387627292245,
-            "angularVelocity": 5.906107685245515e-7,
-            "velocityX": 3.5008566491785738,
-            "velocityY": -1.43340200735705,
-            "timestamp": 1.267948139697878
-        },
-        {
-            "x": 4.364633703659485,
-            "y": 2.039879710966971,
-            "heading": -0.46956382508952316,
-            "angularVelocity": 5.906107531304183e-7,
-            "velocityX": 3.5008570779150574,
-            "velocityY": -1.433400960235855,
-            "timestamp": 1.3546099538447456
-        },
-        {
-            "x": 4.668024366282006,
-            "y": 1.9156586741630988,
-            "heading": -0.46956377390610504,
-            "angularVelocity": 5.906109701114576e-7,
-            "velocityX": 3.5008575069563967,
-            "velocityY": -1.4333999123691519,
-            "timestamp": 1.4412717679916132
-        },
-        {
-            "x": 4.971415087165123,
-            "y": 1.7914377796518506,
-            "heading": -0.4695637227226054,
-            "angularVelocity": 5.906119107964098e-7,
-            "velocityX": 3.5008581792316793,
-            "velocityY": -1.4333982704392505,
-            "timestamp": 1.5279335821384807
-        },
-        {
-            "x": 5.2748058711699946,
-            "y": 1.6672170393060612,
-            "heading": -0.4695636715390663,
-            "angularVelocity": 5.906123657897232e-7,
-            "velocityX": 3.5008589076003997,
-            "velocityY": -1.4333964915074275,
-            "timestamp": 1.6145953962853483
-        },
-        {
-            "x": 5.5781967938554144,
-            "y": 1.5429966376674569,
-            "heading": -0.4695636203556594,
-            "angularVelocity": 5.906108405690599e-7,
-            "velocityX": 3.5008605078501667,
-            "velocityY": -1.4333925831287677,
-            "timestamp": 1.701257210432216
-        },
-        {
-            "x": 5.886988948403778,
-            "y": 1.4328868446307816,
-            "heading": -0.4695635793301605,
-            "angularVelocity": 4.733976467078746e-7,
-            "velocityX": 3.5631859035981908,
-            "velocityY": -1.2705687518849984,
-            "timestamp": 1.7879190245790835
-        },
-        {
-            "x": 6.195781169913944,
-            "y": 1.3227772393821355,
-            "heading": -0.4695635383046595,
-            "angularVelocity": 4.7339767079441426e-7,
-            "velocityX": 3.5631866762776085,
-            "velocityY": -1.2705665849787193,
-            "timestamp": 1.874580838725951
-        },
-        {
-            "x": 6.5045734104803365,
-            "y": 1.212667687574965,
-            "heading": -0.46956349727913566,
-            "angularVelocity": 4.733979354519185e-7,
-            "velocityX": 3.5631868961694693,
-            "velocityY": -1.270565968311784,
-            "timestamp": 1.9612426528728186
-        },
-        {
-            "x": 6.813365656566466,
-            "y": 1.1025581512474965,
-            "heading": -0.46956345625352064,
-            "angularVelocity": 4.733989867793667e-7,
-            "velocityX": 3.563186959862305,
-            "velocityY": -1.270565789689833,
-            "timestamp": 2.0479044670196864
-        },
-        {
-            "x": 7.101065618447083,
-            "y": 0.9964425714529646,
-            "heading": -0.4012732100130825,
-            "angularVelocity": 0.7880085007764233,
-            "velocityX": 3.3198008224596522,
-            "velocityY": -1.2244790954259896,
-            "timestamp": 2.134566281166554
-        },
-        {
-            "x": 7.333266238045498,
-            "y": 0.911992130350644,
-            "heading": -0.2613611242014092,
-            "angularVelocity": 1.6144606155435655,
-            "velocityX": 2.6793879390165927,
-            "velocityY": -0.9744827284506232,
-            "timestamp": 2.2212280953134216
-        },
-        {
-            "x": 7.506890250099573,
-            "y": 0.8492417427348896,
-            "heading": -0.1371754084070899,
-            "angularVelocity": 1.43299233943868,
-            "velocityX": 2.0034661605379225,
-            "velocityY": -0.7240834759056616,
-            "timestamp": 2.307889909460289
-        },
-        {
-            "x": 7.622366585478705,
-            "y": 0.8076799304753898,
-            "heading": -0.04732610702177306,
-            "angularVelocity": 1.0367807582823931,
-            "velocityX": 1.332493861522807,
-            "velocityY": -0.4795862245517285,
-            "timestamp": 2.3945517236071567
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 2.9489608951695904e-31,
-            "angularVelocity": 0.5461010421679925,
-            "velocityX": 0.6650381726791765,
-            "velocityY": -0.2386279433331854,
-            "timestamp": 2.4812135377540243
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -7.162012659830636e-32,
-            "angularVelocity": -4.0878892657023166e-30,
-            "velocityX": 2.3668785600931567e-32,
-            "velocityY": 6.81692291678628e-32,
-            "timestamp": 2.567875351900892
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.1.traj
deleted file mode 100644
index fccc0721..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.1.traj	
+++ /dev/null
@@ -1,238 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": 3.226894361928835e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.724288196557008,
-            "y": 4.368098642385211,
-            "heading": -1.0157828821188275,
-            "angularVelocity": 0.526633856876912,
-            "velocityX": 0.3107160851076989,
-            "velocityY": 0.17611589024403787,
-            "timestamp": 0.0459847341088113
-        },
-        {
-            "x": 0.7529777435005003,
-            "y": 4.384403118337835,
-            "heading": -0.9690307892055188,
-            "angularVelocity": 1.0166872510929454,
-            "velocityX": 0.6238928526933836,
-            "velocityY": 0.3545627971674321,
-            "timestamp": 0.0919694682176226
-        },
-        {
-            "x": 0.7962133909539325,
-            "y": 4.4090308090891845,
-            "heading": -0.9019535610081195,
-            "angularVelocity": 1.4586847025950622,
-            "velocityX": 0.9402174067404033,
-            "velocityY": 0.5355623171174385,
-            "timestamp": 0.1379542023264339
-        },
-        {
-            "x": 0.8541780196778807,
-            "y": 4.442096684944602,
-            "heading": -0.8174192357524789,
-            "angularVelocity": 1.8383127986701173,
-            "velocityX": 1.2605189493276492,
-            "velocityY": 0.7190620212605457,
-            "timestamp": 0.1839389364352452
-        },
-        {
-            "x": 0.92708218377146,
-            "y": 4.483710427261066,
-            "heading": -0.7190735575075958,
-            "angularVelocity": 2.1386592779291416,
-            "velocityX": 1.585399274486893,
-            "velocityY": 0.9049468942886179,
-            "timestamp": 0.2299236705440565
-        },
-        {
-            "x": 1.015132605778429,
-            "y": 4.5339957868438265,
-            "heading": -0.6117681897735129,
-            "angularVelocity": 2.333499797567627,
-            "velocityX": 1.914775059884505,
-            "velocityY": 1.0935228953105474,
-            "timestamp": 0.2759084046528678
-        },
-        {
-            "x": 1.1184514719230099,
-            "y": 4.593093468568762,
-            "heading": -0.5033518248752775,
-            "angularVelocity": 2.357659927786404,
-            "velocityX": 2.246807949353511,
-            "velocityY": 1.2851587134349265,
-            "timestamp": 0.32189313876167913
-        },
-        {
-            "x": 1.2364935087746176,
-            "y": 4.660915178231087,
-            "heading": -0.4100171287779816,
-            "angularVelocity": 2.029688719661721,
-            "velocityX": 2.566983133408849,
-            "velocityY": 1.4748744551145279,
-            "timestamp": 0.36787787287049045
-        },
-        {
-            "x": 1.3657550031232673,
-            "y": 4.73811911608404,
-            "heading": -0.35248337281854053,
-            "angularVelocity": 1.2511490405338719,
-            "velocityX": 2.8109653530405243,
-            "velocityY": 1.6789036481164823,
-            "timestamp": 0.4138626069793018
-        },
-        {
-            "x": 1.5059170232916794,
-            "y": 4.82520017003363,
-            "heading": -0.3304805961437169,
-            "angularVelocity": 0.47848002388715516,
-            "velocityX": 3.0480119736421156,
-            "velocityY": 1.8936948454141527,
-            "timestamp": 0.4598473410881131
-        },
-        {
-            "x": 1.6456219690029539,
-            "y": 4.9288521484832435,
-            "heading": -0.3304805109628974,
-            "angularVelocity": 0.0000018523716874218418,
-            "velocityX": 3.038072273739655,
-            "velocityY": 2.2540519252400664,
-            "timestamp": 0.5058320751969244
-        },
-        {
-            "x": 1.7741171019961581,
-            "y": 5.046113419715312,
-            "heading": -0.3304804288379279,
-            "angularVelocity": 0.000001785918111046389,
-            "velocityX": 2.794299792821407,
-            "velocityY": 2.5500043330598703,
-            "timestamp": 0.5518168093057357
-        },
-        {
-            "x": 1.8900823069364519,
-            "y": 5.175779429346925,
-            "heading": -0.330480329845049,
-            "angularVelocity": 0.000002152733527656335,
-            "velocityX": 2.5218196253105503,
-            "velocityY": 2.8197620828859327,
-            "timestamp": 0.597801543414547
-        },
-        {
-            "x": 1.992326225217661,
-            "y": 5.31651812358221,
-            "heading": -0.3304801981354216,
-            "angularVelocity": 0.0000028642033047051737,
-            "velocityX": 2.2234317597502637,
-            "velocityY": 3.0605525281991963,
-            "timestamp": 0.6437862775233584
-        },
-        {
-            "x": 2.079798460006714,
-            "y": 5.466883659362793,
-            "heading": -0.3304799560936257,
-            "angularVelocity": 0.000005263524964464214,
-            "velocityX": 1.9022015998192814,
-            "velocityY": 3.2699011681743237,
-            "timestamp": 0.6897710116321697
-        },
-        {
-            "x": 2.16894705974312,
-            "y": 5.683675424803782,
-            "heading": -0.33027266768763375,
-            "angularVelocity": 0.0033444025204108644,
-            "velocityX": 1.4383284005893175,
-            "velocityY": 3.49773024107632,
-            "timestamp": 0.751751710685232
-        },
-        {
-            "x": 2.226835014262279,
-            "y": 5.903744633222095,
-            "heading": -0.30773378760994813,
-            "angularVelocity": 0.3636435280988788,
-            "velocityX": 0.9339674350815139,
-            "velocityY": 3.550608685938014,
-            "timestamp": 0.8137324097382944
-        },
-        {
-            "x": 2.2584550007033997,
-            "y": 6.112330245271792,
-            "heading": -0.23499422209122486,
-            "angularVelocity": 1.1735841419993758,
-            "velocityX": 0.5101585965339779,
-            "velocityY": 3.3653317119112898,
-            "timestamp": 0.8757131087913568
-        },
-        {
-            "x": 2.277604108948135,
-            "y": 6.297377159682904,
-            "heading": -0.09486114062468509,
-            "angularVelocity": 2.260914826833697,
-            "velocityX": 0.30895276331652,
-            "velocityY": 2.985557072415336,
-            "timestamp": 0.9376938078444191
-        },
-        {
-            "x": 2.288511286424087,
-            "y": 6.451498238253786,
-            "heading": 0.04992678964099742,
-            "angularVelocity": 2.336016412815077,
-            "velocityX": 0.1759769999789598,
-            "velocityY": 2.48659793977057,
-            "timestamp": 0.9996745068974815
-        },
-        {
-            "x": 2.294708988562371,
-            "y": 6.5742371130661414,
-            "heading": 0.17908010356662757,
-            "angularVelocity": 2.083766654762434,
-            "velocityX": 0.09999406642681147,
-            "velocityY": 1.9802757420867263,
-            "timestamp": 1.061655205950544
-        },
-        {
-            "x": 2.2980089897573315,
-            "y": 6.66587189325695,
-            "heading": 0.2827001929036045,
-            "angularVelocity": 1.6718122079950113,
-            "velocityX": 0.05324240038246661,
-            "velocityY": 1.478440572481471,
-            "timestamp": 1.1236359050036064
-        },
-        {
-            "x": 2.2995212581337525,
-            "y": 6.72670613551003,
-            "heading": 0.35522073089449935,
-            "angularVelocity": 1.1700503398453694,
-            "velocityX": 0.024399020978011547,
-            "velocityY": 0.9815030030719727,
-            "timestamp": 1.185616604056669
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.609532801060419,
-            "velocityX": 0.007724047543277551,
-            "velocityY": 0.48876287219728576,
-            "timestamp": 1.2475973031097314
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 2.340826570383063e-30,
-            "velocityX": -1.14239845433479e-30,
-            "velocityY": 0,
-            "timestamp": 1.3095780021627939
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.traj
deleted file mode 100644
index fccc0721..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.traj	
+++ /dev/null
@@ -1,238 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": 3.226894361928835e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.724288196557008,
-            "y": 4.368098642385211,
-            "heading": -1.0157828821188275,
-            "angularVelocity": 0.526633856876912,
-            "velocityX": 0.3107160851076989,
-            "velocityY": 0.17611589024403787,
-            "timestamp": 0.0459847341088113
-        },
-        {
-            "x": 0.7529777435005003,
-            "y": 4.384403118337835,
-            "heading": -0.9690307892055188,
-            "angularVelocity": 1.0166872510929454,
-            "velocityX": 0.6238928526933836,
-            "velocityY": 0.3545627971674321,
-            "timestamp": 0.0919694682176226
-        },
-        {
-            "x": 0.7962133909539325,
-            "y": 4.4090308090891845,
-            "heading": -0.9019535610081195,
-            "angularVelocity": 1.4586847025950622,
-            "velocityX": 0.9402174067404033,
-            "velocityY": 0.5355623171174385,
-            "timestamp": 0.1379542023264339
-        },
-        {
-            "x": 0.8541780196778807,
-            "y": 4.442096684944602,
-            "heading": -0.8174192357524789,
-            "angularVelocity": 1.8383127986701173,
-            "velocityX": 1.2605189493276492,
-            "velocityY": 0.7190620212605457,
-            "timestamp": 0.1839389364352452
-        },
-        {
-            "x": 0.92708218377146,
-            "y": 4.483710427261066,
-            "heading": -0.7190735575075958,
-            "angularVelocity": 2.1386592779291416,
-            "velocityX": 1.585399274486893,
-            "velocityY": 0.9049468942886179,
-            "timestamp": 0.2299236705440565
-        },
-        {
-            "x": 1.015132605778429,
-            "y": 4.5339957868438265,
-            "heading": -0.6117681897735129,
-            "angularVelocity": 2.333499797567627,
-            "velocityX": 1.914775059884505,
-            "velocityY": 1.0935228953105474,
-            "timestamp": 0.2759084046528678
-        },
-        {
-            "x": 1.1184514719230099,
-            "y": 4.593093468568762,
-            "heading": -0.5033518248752775,
-            "angularVelocity": 2.357659927786404,
-            "velocityX": 2.246807949353511,
-            "velocityY": 1.2851587134349265,
-            "timestamp": 0.32189313876167913
-        },
-        {
-            "x": 1.2364935087746176,
-            "y": 4.660915178231087,
-            "heading": -0.4100171287779816,
-            "angularVelocity": 2.029688719661721,
-            "velocityX": 2.566983133408849,
-            "velocityY": 1.4748744551145279,
-            "timestamp": 0.36787787287049045
-        },
-        {
-            "x": 1.3657550031232673,
-            "y": 4.73811911608404,
-            "heading": -0.35248337281854053,
-            "angularVelocity": 1.2511490405338719,
-            "velocityX": 2.8109653530405243,
-            "velocityY": 1.6789036481164823,
-            "timestamp": 0.4138626069793018
-        },
-        {
-            "x": 1.5059170232916794,
-            "y": 4.82520017003363,
-            "heading": -0.3304805961437169,
-            "angularVelocity": 0.47848002388715516,
-            "velocityX": 3.0480119736421156,
-            "velocityY": 1.8936948454141527,
-            "timestamp": 0.4598473410881131
-        },
-        {
-            "x": 1.6456219690029539,
-            "y": 4.9288521484832435,
-            "heading": -0.3304805109628974,
-            "angularVelocity": 0.0000018523716874218418,
-            "velocityX": 3.038072273739655,
-            "velocityY": 2.2540519252400664,
-            "timestamp": 0.5058320751969244
-        },
-        {
-            "x": 1.7741171019961581,
-            "y": 5.046113419715312,
-            "heading": -0.3304804288379279,
-            "angularVelocity": 0.000001785918111046389,
-            "velocityX": 2.794299792821407,
-            "velocityY": 2.5500043330598703,
-            "timestamp": 0.5518168093057357
-        },
-        {
-            "x": 1.8900823069364519,
-            "y": 5.175779429346925,
-            "heading": -0.330480329845049,
-            "angularVelocity": 0.000002152733527656335,
-            "velocityX": 2.5218196253105503,
-            "velocityY": 2.8197620828859327,
-            "timestamp": 0.597801543414547
-        },
-        {
-            "x": 1.992326225217661,
-            "y": 5.31651812358221,
-            "heading": -0.3304801981354216,
-            "angularVelocity": 0.0000028642033047051737,
-            "velocityX": 2.2234317597502637,
-            "velocityY": 3.0605525281991963,
-            "timestamp": 0.6437862775233584
-        },
-        {
-            "x": 2.079798460006714,
-            "y": 5.466883659362793,
-            "heading": -0.3304799560936257,
-            "angularVelocity": 0.000005263524964464214,
-            "velocityX": 1.9022015998192814,
-            "velocityY": 3.2699011681743237,
-            "timestamp": 0.6897710116321697
-        },
-        {
-            "x": 2.16894705974312,
-            "y": 5.683675424803782,
-            "heading": -0.33027266768763375,
-            "angularVelocity": 0.0033444025204108644,
-            "velocityX": 1.4383284005893175,
-            "velocityY": 3.49773024107632,
-            "timestamp": 0.751751710685232
-        },
-        {
-            "x": 2.226835014262279,
-            "y": 5.903744633222095,
-            "heading": -0.30773378760994813,
-            "angularVelocity": 0.3636435280988788,
-            "velocityX": 0.9339674350815139,
-            "velocityY": 3.550608685938014,
-            "timestamp": 0.8137324097382944
-        },
-        {
-            "x": 2.2584550007033997,
-            "y": 6.112330245271792,
-            "heading": -0.23499422209122486,
-            "angularVelocity": 1.1735841419993758,
-            "velocityX": 0.5101585965339779,
-            "velocityY": 3.3653317119112898,
-            "timestamp": 0.8757131087913568
-        },
-        {
-            "x": 2.277604108948135,
-            "y": 6.297377159682904,
-            "heading": -0.09486114062468509,
-            "angularVelocity": 2.260914826833697,
-            "velocityX": 0.30895276331652,
-            "velocityY": 2.985557072415336,
-            "timestamp": 0.9376938078444191
-        },
-        {
-            "x": 2.288511286424087,
-            "y": 6.451498238253786,
-            "heading": 0.04992678964099742,
-            "angularVelocity": 2.336016412815077,
-            "velocityX": 0.1759769999789598,
-            "velocityY": 2.48659793977057,
-            "timestamp": 0.9996745068974815
-        },
-        {
-            "x": 2.294708988562371,
-            "y": 6.5742371130661414,
-            "heading": 0.17908010356662757,
-            "angularVelocity": 2.083766654762434,
-            "velocityX": 0.09999406642681147,
-            "velocityY": 1.9802757420867263,
-            "timestamp": 1.061655205950544
-        },
-        {
-            "x": 2.2980089897573315,
-            "y": 6.66587189325695,
-            "heading": 0.2827001929036045,
-            "angularVelocity": 1.6718122079950113,
-            "velocityX": 0.05324240038246661,
-            "velocityY": 1.478440572481471,
-            "timestamp": 1.1236359050036064
-        },
-        {
-            "x": 2.2995212581337525,
-            "y": 6.72670613551003,
-            "heading": 0.35522073089449935,
-            "angularVelocity": 1.1700503398453694,
-            "velocityX": 0.024399020978011547,
-            "velocityY": 0.9815030030719727,
-            "timestamp": 1.185616604056669
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.609532801060419,
-            "velocityX": 0.007724047543277551,
-            "velocityY": 0.48876287219728576,
-            "timestamp": 1.2475973031097314
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 2.340826570383063e-30,
-            "velocityX": -1.14239845433479e-30,
-            "velocityY": 0,
-            "timestamp": 1.3095780021627939
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.1.traj
deleted file mode 100644
index ab8f454f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.1.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": -1.335008934228872e-34,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7208748853953925,
-            "y": 4.367346013203549,
-            "heading": -1.0266172190247755,
-            "angularVelocity": 0.3311622793955978,
-            "velocityX": 0.2691033980434437,
-            "velocityY": 0.1817800411932904,
-            "timestamp": 0.040411549889224324
-        },
-        {
-            "x": 0.7426520542538281,
-            "y": 4.382068365397769,
-            "heading": -1.0004187382318452,
-            "angularVelocity": 0.6482919082476458,
-            "velocityX": 0.5388847722527592,
-            "velocityY": 0.3643105061443391,
-            "timestamp": 0.08082309977844865
-        },
-        {
-            "x": 0.7753644718100721,
-            "y": 4.404202344534013,
-            "heading": -0.9621111981448724,
-            "angularVelocity": 0.9479354341019116,
-            "velocityX": 0.8094818844096516,
-            "velocityY": 0.5477141855958662,
-            "timestamp": 0.12123464966767297
-        },
-        {
-            "x": 0.8190534716968692,
-            "y": 4.4337878475210974,
-            "heading": -0.9126009199564967,
-            "angularVelocity": 1.2251516787674004,
-            "velocityX": 1.0811018138764033,
-            "velocityY": 0.7321051300478436,
-            "timestamp": 0.1616461995568973
-        },
-        {
-            "x": 0.8737711662497897,
-            "y": 4.470868927169861,
-            "heading": -0.8530717234087362,
-            "angularVelocity": 1.4730738294111758,
-            "velocityX": 1.3540112839748029,
-            "velocityY": 0.9175861789614466,
-            "timestamp": 0.20205774944612162
-        },
-        {
-            "x": 0.9395812969768862,
-            "y": 4.515493926508116,
-            "heading": -0.7850998870787868,
-            "angularVelocity": 1.681990334849146,
-            "velocityX": 1.6284980632392125,
-            "velocityY": 1.104263495475429,
-            "timestamp": 0.24246929933534594
-        },
-        {
-            "x": 1.0165562063250766,
-            "y": 4.567717120220609,
-            "heading": -0.7108767199363526,
-            "angularVelocity": 1.8366820214986515,
-            "velocityX": 1.9047749853493152,
-            "velocityY": 1.292283860818163,
-            "timestamp": 0.28288084922457024
-        },
-        {
-            "x": 1.104761821592339,
-            "y": 4.627598200600785,
-            "heading": -0.633776622573869,
-            "angularVelocity": 1.9078728129415852,
-            "velocityX": 2.1826833048732497,
-            "velocityY": 1.481781335888433,
-            "timestamp": 0.3232923991137946
-        },
-        {
-            "x": 1.2041714779159751,
-            "y": 4.695156297767735,
-            "heading": -0.5601638063396884,
-            "angularVelocity": 1.8215786436295356,
-            "velocityX": 2.4599317916817567,
-            "velocityY": 1.6717521934233843,
-            "timestamp": 0.36370394900301894
-        },
-        {
-            "x": 1.3134031887504467,
-            "y": 4.770142726541239,
-            "heading": -0.5079341517285784,
-            "angularVelocity": 1.2924437383441445,
-            "velocityX": 2.7029824674850675,
-            "velocityY": 1.8555692364943988,
-            "timestamp": 0.4041154988922433
-        },
-        {
-            "x": 1.4312329739367307,
-            "y": 4.852476770750293,
-            "heading": -0.48455184709669846,
-            "angularVelocity": 0.5786045003464412,
-            "velocityX": 2.9157452636505585,
-            "velocityY": 2.0373889255608297,
-            "timestamp": 0.44452704878146765
-        },
-        {
-            "x": 1.55675883861541,
-            "y": 4.939734199489085,
-            "heading": -0.4845517983636149,
-            "angularVelocity": 0.0000012059196871647072,
-            "velocityX": 3.10618783547697,
-            "velocityY": 2.1592200491686473,
-            "timestamp": 0.484938598670692
-        },
-        {
-            "x": 1.6733111473031865,
-            "y": 5.037206282378932,
-            "heading": -0.48207265962656115,
-            "angularVelocity": 0.061347281751120815,
-            "velocityX": 2.884133595649469,
-            "velocityY": 2.4119857604332493,
-            "timestamp": 0.5253501485599164
-        },
-        {
-            "x": 1.7808787822723389,
-            "y": 5.128108024597168,
-            "heading": -0.4506827811882082,
-            "angularVelocity": 0.7767551238296608,
-            "velocityX": 2.661804243193232,
-            "velocityY": 2.2494000469522843,
-            "timestamp": 0.5657616984491407
-        },
-        {
-            "x": 1.9395772431433458,
-            "y": 5.270947819082425,
-            "heading": -0.3258768071798875,
-            "angularVelocity": 1.7295324507132326,
-            "velocityX": 2.1992067297704168,
-            "velocityY": 1.9794409825205124,
-            "timestamp": 0.6379233823521242
-        },
-        {
-            "x": 2.0647143213126102,
-            "y": 5.386258683660538,
-            "heading": -0.20467464704644728,
-            "angularVelocity": 1.6795916278282566,
-            "velocityX": 1.7341208159374817,
-            "velocityY": 1.5979514105178143,
-            "timestamp": 0.7100850662551077
-        },
-        {
-            "x": 2.157719068276139,
-            "y": 5.473035188211263,
-            "heading": -0.10556396576995164,
-            "angularVelocity": 1.373453000483514,
-            "velocityX": 1.2888383686911677,
-            "velocityY": 1.2025288194132213,
-            "timestamp": 0.7822467501580912
-        },
-        {
-            "x": 2.219336350583297,
-            "y": 5.530988274582093,
-            "heading": -0.03600663550492556,
-            "angularVelocity": 0.9639094669484316,
-            "velocityX": 0.853878110577325,
-            "velocityY": 0.8031005269880169,
-            "timestamp": 0.8544084340610747
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 7.006093267694686e-35,
-            "angularVelocity": 0.49897166414982813,
-            "velocityX": 0.4249297931839902,
-            "velocityY": 0.40203781077103196,
-            "timestamp": 0.9265701179640582
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 2.908643221674668e-34,
-            "velocityX": -1.689604381582848e-35,
-            "velocityY": 1.5152673195076234e-35,
-            "timestamp": 0.9987318018670417
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.traj
deleted file mode 100644
index ab8f454f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": -1.335008934228872e-34,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7208748853953925,
-            "y": 4.367346013203549,
-            "heading": -1.0266172190247755,
-            "angularVelocity": 0.3311622793955978,
-            "velocityX": 0.2691033980434437,
-            "velocityY": 0.1817800411932904,
-            "timestamp": 0.040411549889224324
-        },
-        {
-            "x": 0.7426520542538281,
-            "y": 4.382068365397769,
-            "heading": -1.0004187382318452,
-            "angularVelocity": 0.6482919082476458,
-            "velocityX": 0.5388847722527592,
-            "velocityY": 0.3643105061443391,
-            "timestamp": 0.08082309977844865
-        },
-        {
-            "x": 0.7753644718100721,
-            "y": 4.404202344534013,
-            "heading": -0.9621111981448724,
-            "angularVelocity": 0.9479354341019116,
-            "velocityX": 0.8094818844096516,
-            "velocityY": 0.5477141855958662,
-            "timestamp": 0.12123464966767297
-        },
-        {
-            "x": 0.8190534716968692,
-            "y": 4.4337878475210974,
-            "heading": -0.9126009199564967,
-            "angularVelocity": 1.2251516787674004,
-            "velocityX": 1.0811018138764033,
-            "velocityY": 0.7321051300478436,
-            "timestamp": 0.1616461995568973
-        },
-        {
-            "x": 0.8737711662497897,
-            "y": 4.470868927169861,
-            "heading": -0.8530717234087362,
-            "angularVelocity": 1.4730738294111758,
-            "velocityX": 1.3540112839748029,
-            "velocityY": 0.9175861789614466,
-            "timestamp": 0.20205774944612162
-        },
-        {
-            "x": 0.9395812969768862,
-            "y": 4.515493926508116,
-            "heading": -0.7850998870787868,
-            "angularVelocity": 1.681990334849146,
-            "velocityX": 1.6284980632392125,
-            "velocityY": 1.104263495475429,
-            "timestamp": 0.24246929933534594
-        },
-        {
-            "x": 1.0165562063250766,
-            "y": 4.567717120220609,
-            "heading": -0.7108767199363526,
-            "angularVelocity": 1.8366820214986515,
-            "velocityX": 1.9047749853493152,
-            "velocityY": 1.292283860818163,
-            "timestamp": 0.28288084922457024
-        },
-        {
-            "x": 1.104761821592339,
-            "y": 4.627598200600785,
-            "heading": -0.633776622573869,
-            "angularVelocity": 1.9078728129415852,
-            "velocityX": 2.1826833048732497,
-            "velocityY": 1.481781335888433,
-            "timestamp": 0.3232923991137946
-        },
-        {
-            "x": 1.2041714779159751,
-            "y": 4.695156297767735,
-            "heading": -0.5601638063396884,
-            "angularVelocity": 1.8215786436295356,
-            "velocityX": 2.4599317916817567,
-            "velocityY": 1.6717521934233843,
-            "timestamp": 0.36370394900301894
-        },
-        {
-            "x": 1.3134031887504467,
-            "y": 4.770142726541239,
-            "heading": -0.5079341517285784,
-            "angularVelocity": 1.2924437383441445,
-            "velocityX": 2.7029824674850675,
-            "velocityY": 1.8555692364943988,
-            "timestamp": 0.4041154988922433
-        },
-        {
-            "x": 1.4312329739367307,
-            "y": 4.852476770750293,
-            "heading": -0.48455184709669846,
-            "angularVelocity": 0.5786045003464412,
-            "velocityX": 2.9157452636505585,
-            "velocityY": 2.0373889255608297,
-            "timestamp": 0.44452704878146765
-        },
-        {
-            "x": 1.55675883861541,
-            "y": 4.939734199489085,
-            "heading": -0.4845517983636149,
-            "angularVelocity": 0.0000012059196871647072,
-            "velocityX": 3.10618783547697,
-            "velocityY": 2.1592200491686473,
-            "timestamp": 0.484938598670692
-        },
-        {
-            "x": 1.6733111473031865,
-            "y": 5.037206282378932,
-            "heading": -0.48207265962656115,
-            "angularVelocity": 0.061347281751120815,
-            "velocityX": 2.884133595649469,
-            "velocityY": 2.4119857604332493,
-            "timestamp": 0.5253501485599164
-        },
-        {
-            "x": 1.7808787822723389,
-            "y": 5.128108024597168,
-            "heading": -0.4506827811882082,
-            "angularVelocity": 0.7767551238296608,
-            "velocityX": 2.661804243193232,
-            "velocityY": 2.2494000469522843,
-            "timestamp": 0.5657616984491407
-        },
-        {
-            "x": 1.9395772431433458,
-            "y": 5.270947819082425,
-            "heading": -0.3258768071798875,
-            "angularVelocity": 1.7295324507132326,
-            "velocityX": 2.1992067297704168,
-            "velocityY": 1.9794409825205124,
-            "timestamp": 0.6379233823521242
-        },
-        {
-            "x": 2.0647143213126102,
-            "y": 5.386258683660538,
-            "heading": -0.20467464704644728,
-            "angularVelocity": 1.6795916278282566,
-            "velocityX": 1.7341208159374817,
-            "velocityY": 1.5979514105178143,
-            "timestamp": 0.7100850662551077
-        },
-        {
-            "x": 2.157719068276139,
-            "y": 5.473035188211263,
-            "heading": -0.10556396576995164,
-            "angularVelocity": 1.373453000483514,
-            "velocityX": 1.2888383686911677,
-            "velocityY": 1.2025288194132213,
-            "timestamp": 0.7822467501580912
-        },
-        {
-            "x": 2.219336350583297,
-            "y": 5.530988274582093,
-            "heading": -0.03600663550492556,
-            "angularVelocity": 0.9639094669484316,
-            "velocityX": 0.853878110577325,
-            "velocityY": 0.8031005269880169,
-            "timestamp": 0.8544084340610747
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 7.006093267694686e-35,
-            "angularVelocity": 0.49897166414982813,
-            "velocityX": 0.4249297931839902,
-            "velocityY": 0.40203781077103196,
-            "timestamp": 0.9265701179640582
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 2.908643221674668e-34,
-            "velocityX": -1.689604381582848e-35,
-            "velocityY": 1.5152673195076234e-35,
-            "timestamp": 0.9987318018670417
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.1.traj
deleted file mode 100644
index ff6d2947..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.1.traj	
+++ /dev/null
@@ -1,121 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": -2.204776300650663e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7541176231747116,
-            "y": 4.360036687345491,
-            "heading": -1.0162545684059647,
-            "angularVelocity": 0.32461244675544165,
-            "velocityX": 0.6031109414489596,
-            "velocityY": 0.0005015351663374791,
-            "timestamp": 0.07315009584923313
-        },
-        {
-            "x": 0.8424088865359771,
-            "y": 4.360100324606402,
-            "heading": -0.9701479863262087,
-            "angularVelocity": 0.630301048063541,
-            "velocityX": 1.2069876646948012,
-            "velocityY": 0.0008699545799604022,
-            "timestamp": 0.14630019169846625
-        },
-        {
-            "x": 0.974955221752511,
-            "y": 4.3601789275184695,
-            "heading": -0.9038423136459668,
-            "angularVelocity": 0.9064331619859869,
-            "velocityX": 1.8119776013652948,
-            "velocityY": 0.0010745428444254071,
-            "timestamp": 0.21945028754769938
-        },
-        {
-            "x": 1.151884261990258,
-            "y": 4.360259672825357,
-            "heading": -0.8212724833638784,
-            "angularVelocity": 1.128772687492223,
-            "velocityX": 2.4187123500481573,
-            "velocityY": 0.0011038305000959348,
-            "timestamp": 0.2926003833969325
-        },
-        {
-            "x": 1.3733948258568207,
-            "y": 4.360324542882412,
-            "heading": -0.7323749312717828,
-            "angularVelocity": 1.215275948172809,
-            "velocityX": 3.028165052894887,
-            "velocityY": 0.0008868075468144582,
-            "timestamp": 0.36575047924616566
-        },
-        {
-            "x": 1.6366160230983884,
-            "y": 4.359789775146019,
-            "heading": -0.6992109641912426,
-            "angularVelocity": 0.4533687440268879,
-            "velocityX": 3.5983711871554758,
-            "velocityY": -0.007310554144235059,
-            "timestamp": 0.4389005750953988
-        },
-        {
-            "x": 1.8581232786849116,
-            "y": 4.3598960912826,
-            "heading": -0.6098314600860953,
-            "angularVelocity": 1.2218644838062247,
-            "velocityX": 3.0281198269795637,
-            "velocityY": 0.001453397091854858,
-            "timestamp": 0.512050670944632
-        },
-        {
-            "x": 2.035050245808528,
-            "y": 4.359956526837273,
-            "heading": -0.5268891037054299,
-            "angularVelocity": 1.1338653137464598,
-            "velocityX": 2.4186840094956525,
-            "velocityY": 0.000826185584123381,
-            "timestamp": 0.5852007667938651
-        },
-        {
-            "x": 2.1675937895751547,
-            "y": 4.359986275638159,
-            "heading": -0.46022423203591045,
-            "angularVelocity": 0.9113435996982586,
-            "velocityX": 1.8119394407877087,
-            "velocityY": 0.00040668163915013244,
-            "timestamp": 0.6583508626430983
-        },
-        {
-            "x": 2.255882919836075,
-            "y": 4.359997704013322,
-            "heading": -0.41385948514831483,
-            "angularVelocity": 0.6338302957681125,
-            "velocityX": 1.2069585040994306,
-            "velocityY": 0.0001562318549097459,
-            "timestamp": 0.7315009584923314
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0.32617161838698483,
-            "velocityX": 0.6031035182080025,
-            "velocityY": 0.00003138733655167869,
-            "timestamp": 0.8046510543415646
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -2.2755735462869925e-32,
-            "velocityX": -9.780003655586715e-35,
-            "velocityY": 0,
-            "timestamp": 0.8778011501907977
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.traj
deleted file mode 100644
index ff6d2947..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.traj	
+++ /dev/null
@@ -1,121 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": -2.204776300650663e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7541176231747116,
-            "y": 4.360036687345491,
-            "heading": -1.0162545684059647,
-            "angularVelocity": 0.32461244675544165,
-            "velocityX": 0.6031109414489596,
-            "velocityY": 0.0005015351663374791,
-            "timestamp": 0.07315009584923313
-        },
-        {
-            "x": 0.8424088865359771,
-            "y": 4.360100324606402,
-            "heading": -0.9701479863262087,
-            "angularVelocity": 0.630301048063541,
-            "velocityX": 1.2069876646948012,
-            "velocityY": 0.0008699545799604022,
-            "timestamp": 0.14630019169846625
-        },
-        {
-            "x": 0.974955221752511,
-            "y": 4.3601789275184695,
-            "heading": -0.9038423136459668,
-            "angularVelocity": 0.9064331619859869,
-            "velocityX": 1.8119776013652948,
-            "velocityY": 0.0010745428444254071,
-            "timestamp": 0.21945028754769938
-        },
-        {
-            "x": 1.151884261990258,
-            "y": 4.360259672825357,
-            "heading": -0.8212724833638784,
-            "angularVelocity": 1.128772687492223,
-            "velocityX": 2.4187123500481573,
-            "velocityY": 0.0011038305000959348,
-            "timestamp": 0.2926003833969325
-        },
-        {
-            "x": 1.3733948258568207,
-            "y": 4.360324542882412,
-            "heading": -0.7323749312717828,
-            "angularVelocity": 1.215275948172809,
-            "velocityX": 3.028165052894887,
-            "velocityY": 0.0008868075468144582,
-            "timestamp": 0.36575047924616566
-        },
-        {
-            "x": 1.6366160230983884,
-            "y": 4.359789775146019,
-            "heading": -0.6992109641912426,
-            "angularVelocity": 0.4533687440268879,
-            "velocityX": 3.5983711871554758,
-            "velocityY": -0.007310554144235059,
-            "timestamp": 0.4389005750953988
-        },
-        {
-            "x": 1.8581232786849116,
-            "y": 4.3598960912826,
-            "heading": -0.6098314600860953,
-            "angularVelocity": 1.2218644838062247,
-            "velocityX": 3.0281198269795637,
-            "velocityY": 0.001453397091854858,
-            "timestamp": 0.512050670944632
-        },
-        {
-            "x": 2.035050245808528,
-            "y": 4.359956526837273,
-            "heading": -0.5268891037054299,
-            "angularVelocity": 1.1338653137464598,
-            "velocityX": 2.4186840094956525,
-            "velocityY": 0.000826185584123381,
-            "timestamp": 0.5852007667938651
-        },
-        {
-            "x": 2.1675937895751547,
-            "y": 4.359986275638159,
-            "heading": -0.46022423203591045,
-            "angularVelocity": 0.9113435996982586,
-            "velocityX": 1.8119394407877087,
-            "velocityY": 0.00040668163915013244,
-            "timestamp": 0.6583508626430983
-        },
-        {
-            "x": 2.255882919836075,
-            "y": 4.359997704013322,
-            "heading": -0.41385948514831483,
-            "angularVelocity": 0.6338302957681125,
-            "velocityX": 1.2069585040994306,
-            "velocityY": 0.0001562318549097459,
-            "timestamp": 0.7315009584923314
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0.32617161838698483,
-            "velocityX": 0.6031035182080025,
-            "velocityY": 0.00003138733655167869,
-            "timestamp": 0.8046510543415646
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -2.2755735462869925e-32,
-            "velocityX": -9.780003655586715e-35,
-            "velocityY": 0,
-            "timestamp": 0.8778011501907977
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.1.traj
deleted file mode 100644
index bfcd0c92..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.1.traj	
+++ /dev/null
@@ -1,256 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 4.0882515102287785e-31,
-            "velocityX": 3.918479950533569e-32,
-            "velocityY": 1.7984537919855076e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 2.31173856270728,
-            "y": 6.7441134449795,
-            "heading": 0.37806624882590184,
-            "angularVelocity": -0.25868952140989293,
-            "velocityX": 0.2544583948907238,
-            "velocityY": -0.279343577911277,
-            "timestamp": 0.04613155998378907
-        },
-        {
-            "x": 2.335747257207826,
-            "y": 6.718800139650514,
-            "heading": 0.35485995839909595,
-            "angularVelocity": -0.5030458635034395,
-            "velocityX": 0.5204396840033778,
-            "velocityY": -0.5487199075401175,
-            "timestamp": 0.09226311996757813
-        },
-        {
-            "x": 2.3726756647692695,
-            "y": 6.681679707204912,
-            "heading": 0.3212123448310367,
-            "angularVelocity": -0.7293838226993238,
-            "velocityX": 0.8005020331941938,
-            "velocityY": -0.8046645823086491,
-            "timestamp": 0.1383946799513672
-        },
-        {
-            "x": 2.4233218216100387,
-            "y": 6.6336167386650216,
-            "heading": 0.2781690598764637,
-            "angularVelocity": -0.9330550488580632,
-            "velocityX": 1.097863520300784,
-            "velocityY": -1.0418674017696439,
-            "timestamp": 0.18452623993515627
-        },
-        {
-            "x": 2.488661485197386,
-            "y": 6.57586429791715,
-            "heading": 0.22709762602154188,
-            "angularVelocity": -1.1070823070554872,
-            "velocityX": 1.416376632620007,
-            "velocityY": -1.2519073876575257,
-            "timestamp": 0.23065779991894533
-        },
-        {
-            "x": 2.5698490572295953,
-            "y": 6.510313323679299,
-            "heading": 0.16995873595290442,
-            "angularVelocity": -1.2386073674663585,
-            "velocityX": 1.7599138650576658,
-            "velocityY": -1.4209572418727212,
-            "timestamp": 0.2767893599027344
-        },
-        {
-            "x": 2.6680837365477243,
-            "y": 6.439909324941273,
-            "heading": 0.10980534967726638,
-            "angularVelocity": -1.3039530052045993,
-            "velocityX": 2.1294462912732417,
-            "velocityY": -1.5261569034900655,
-            "timestamp": 0.3229209198865235
-        },
-        {
-            "x": 2.784053925984014,
-            "y": 6.36914274527536,
-            "heading": 0.05120415857101511,
-            "angularVelocity": -1.2703058627725585,
-            "velocityX": 2.5139013178189233,
-            "velocityY": -1.5340166187915734,
-            "timestamp": 0.36905247987031253
-        },
-        {
-            "x": 2.916773557662964,
-            "y": 6.303858757019043,
-            "heading": 0,
-            "angularVelocity": -1.1099593984900726,
-            "velocityX": 2.8769812190523862,
-            "velocityY": -1.4151697510176988,
-            "timestamp": 0.41518403985410157
-        },
-        {
-            "x": 3.221120615978503,
-            "y": 6.218848325437687,
-            "heading": -0.053482212757092314,
-            "angularVelocity": -0.6045874919246804,
-            "velocityX": 3.4404789027216576,
-            "velocityY": -0.9609969552052925,
-            "timestamp": 0.5036447067905502
-        },
-        {
-            "x": 3.554544204530565,
-            "y": 6.190324835643033,
-            "heading": -0.053482256222138806,
-            "angularVelocity": -4.913488445928212e-7,
-            "velocityX": 3.769173352396249,
-            "velocityY": -0.32244262656470185,
-            "timestamp": 0.5921053737269988
-        },
-        {
-            "x": 3.8871649424892416,
-            "y": 6.227044557920147,
-            "heading": -0.05348225420791585,
-            "angularVelocity": 2.276970118304524e-8,
-            "velocityX": 3.760097560621335,
-            "velocityY": 0.41509660223897277,
-            "timestamp": 0.6805660406634474
-        },
-        {
-            "x": 4.206338511891951,
-            "y": 6.327608602444822,
-            "heading": -0.05348225236183454,
-            "angularVelocity": 2.0868950806255528e-8,
-            "velocityX": 3.608084592353424,
-            "velocityY": 1.1368221381026216,
-            "timestamp": 0.769026707599896
-        },
-        {
-            "x": 4.5243112033895745,
-            "y": 6.4319075091933104,
-            "heading": -0.05348225052024108,
-            "angularVelocity": 2.0818218151781936e-8,
-            "velocityX": 3.5945093170737707,
-            "velocityY": 1.1790427357213897,
-            "timestamp": 0.8574873745363446
-        },
-        {
-            "x": 4.842283928689835,
-            "y": 6.536206312888755,
-            "heading": -0.053482248678638385,
-            "angularVelocity": 2.0818322537631325e-8,
-            "velocityX": 3.59450969919429,
-            "velocityY": 1.1790415707624449,
-            "timestamp": 0.9459480414727932
-        },
-        {
-            "x": 5.160256832233298,
-            "y": 6.640504573177801,
-            "heading": -0.05348224683713029,
-            "angularVelocity": 2.0817253121924564e-8,
-            "velocityX": 3.5945117141373055,
-            "velocityY": 1.1790354278469999,
-            "timestamp": 1.034408708409242
-        },
-        {
-            "x": 5.478229837776281,
-            "y": 6.744802522501547,
-            "heading": -0.05348224499575697,
-            "angularVelocity": 2.081572963279097e-8,
-            "velocityX": 3.594512867186711,
-            "velocityY": 1.1790319125524373,
-            "timestamp": 1.1228693753456906
-        },
-        {
-            "x": 5.79620288183899,
-            "y": 6.849100354390164,
-            "heading": -0.05348224315440185,
-            "angularVelocity": 2.0815523766741823e-8,
-            "velocityX": 3.5945133026313907,
-            "velocityY": 1.1790305850118254,
-            "timestamp": 1.2113300422821394
-        },
-        {
-            "x": 6.114175956134068,
-            "y": 6.9533980941092235,
-            "heading": -0.053482241313040024,
-            "angularVelocity": 2.081559959710574e-8,
-            "velocityX": 3.5945136443919665,
-            "velocityY": 1.179029543084821,
-            "timestamp": 1.299790709218588
-        },
-        {
-            "x": 6.432149055252676,
-            "y": 7.057695758148605,
-            "heading": -0.05348223947166161,
-            "angularVelocity": 2.0815787162382163e-8,
-            "velocityX": 3.594513925008545,
-            "velocityY": 1.1790286875669842,
-            "timestamp": 1.3882513761550368
-        },
-        {
-            "x": 6.750122174049358,
-            "y": 7.16199336219523,
-            "heading": -0.05348223763026628,
-            "angularVelocity": 2.081597839391937e-8,
-            "velocityX": 3.594514147458541,
-            "velocityY": 1.1790280093813261,
-            "timestamp": 1.4767120430914855
-        },
-        {
-            "x": 7.060081426834384,
-            "y": 7.263662245896702,
-            "heading": -0.036655668946107355,
-            "angularVelocity": 0.1902152591303347,
-            "velocityX": 3.503921726112515,
-            "velocityY": 1.1493117474969148,
-            "timestamp": 1.5651727100279342
-        },
-        {
-            "x": 7.308048847978653,
-            "y": 7.344997351864717,
-            "heading": -0.023193676123292754,
-            "angularVelocity": 0.15218054858760893,
-            "velocityX": 2.8031375947279678,
-            "velocityY": 0.9194493867703608,
-            "timestamp": 1.653633376964383
-        },
-        {
-            "x": 7.4940244209186755,
-            "y": 7.405998678341617,
-            "heading": -0.013096928746230797,
-            "angularVelocity": 0.11413826875525969,
-            "velocityX": 2.102353276102148,
-            "velocityY": 0.6895870061744361,
-            "timestamp": 1.7420940439008317
-        },
-        {
-            "x": 7.618008139456229,
-            "y": 7.4466662269505,
-            "heading": -0.006365663946800556,
-            "angularVelocity": 0.07609330827525898,
-            "velocityX": 1.4015688874087386,
-            "velocityY": 0.4597246439267693,
-            "timestamp": 1.8305547108372804
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -0.003,
-            "angularVelocity": 0.038047010760369614,
-            "velocityX": 0.7007844581174931,
-            "velocityY": 0.2298623077769412,
-            "timestamp": 1.919015377773729
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -0.003,
-            "angularVelocity": -2.311552385941746e-29,
-            "velocityX": 0,
-            "velocityY": -6.724711823689692e-32,
-            "timestamp": 2.007476044710178
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.traj
deleted file mode 100644
index bfcd0c92..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.traj	
+++ /dev/null
@@ -1,256 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 4.0882515102287785e-31,
-            "velocityX": 3.918479950533569e-32,
-            "velocityY": 1.7984537919855076e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 2.31173856270728,
-            "y": 6.7441134449795,
-            "heading": 0.37806624882590184,
-            "angularVelocity": -0.25868952140989293,
-            "velocityX": 0.2544583948907238,
-            "velocityY": -0.279343577911277,
-            "timestamp": 0.04613155998378907
-        },
-        {
-            "x": 2.335747257207826,
-            "y": 6.718800139650514,
-            "heading": 0.35485995839909595,
-            "angularVelocity": -0.5030458635034395,
-            "velocityX": 0.5204396840033778,
-            "velocityY": -0.5487199075401175,
-            "timestamp": 0.09226311996757813
-        },
-        {
-            "x": 2.3726756647692695,
-            "y": 6.681679707204912,
-            "heading": 0.3212123448310367,
-            "angularVelocity": -0.7293838226993238,
-            "velocityX": 0.8005020331941938,
-            "velocityY": -0.8046645823086491,
-            "timestamp": 0.1383946799513672
-        },
-        {
-            "x": 2.4233218216100387,
-            "y": 6.6336167386650216,
-            "heading": 0.2781690598764637,
-            "angularVelocity": -0.9330550488580632,
-            "velocityX": 1.097863520300784,
-            "velocityY": -1.0418674017696439,
-            "timestamp": 0.18452623993515627
-        },
-        {
-            "x": 2.488661485197386,
-            "y": 6.57586429791715,
-            "heading": 0.22709762602154188,
-            "angularVelocity": -1.1070823070554872,
-            "velocityX": 1.416376632620007,
-            "velocityY": -1.2519073876575257,
-            "timestamp": 0.23065779991894533
-        },
-        {
-            "x": 2.5698490572295953,
-            "y": 6.510313323679299,
-            "heading": 0.16995873595290442,
-            "angularVelocity": -1.2386073674663585,
-            "velocityX": 1.7599138650576658,
-            "velocityY": -1.4209572418727212,
-            "timestamp": 0.2767893599027344
-        },
-        {
-            "x": 2.6680837365477243,
-            "y": 6.439909324941273,
-            "heading": 0.10980534967726638,
-            "angularVelocity": -1.3039530052045993,
-            "velocityX": 2.1294462912732417,
-            "velocityY": -1.5261569034900655,
-            "timestamp": 0.3229209198865235
-        },
-        {
-            "x": 2.784053925984014,
-            "y": 6.36914274527536,
-            "heading": 0.05120415857101511,
-            "angularVelocity": -1.2703058627725585,
-            "velocityX": 2.5139013178189233,
-            "velocityY": -1.5340166187915734,
-            "timestamp": 0.36905247987031253
-        },
-        {
-            "x": 2.916773557662964,
-            "y": 6.303858757019043,
-            "heading": 0,
-            "angularVelocity": -1.1099593984900726,
-            "velocityX": 2.8769812190523862,
-            "velocityY": -1.4151697510176988,
-            "timestamp": 0.41518403985410157
-        },
-        {
-            "x": 3.221120615978503,
-            "y": 6.218848325437687,
-            "heading": -0.053482212757092314,
-            "angularVelocity": -0.6045874919246804,
-            "velocityX": 3.4404789027216576,
-            "velocityY": -0.9609969552052925,
-            "timestamp": 0.5036447067905502
-        },
-        {
-            "x": 3.554544204530565,
-            "y": 6.190324835643033,
-            "heading": -0.053482256222138806,
-            "angularVelocity": -4.913488445928212e-7,
-            "velocityX": 3.769173352396249,
-            "velocityY": -0.32244262656470185,
-            "timestamp": 0.5921053737269988
-        },
-        {
-            "x": 3.8871649424892416,
-            "y": 6.227044557920147,
-            "heading": -0.05348225420791585,
-            "angularVelocity": 2.276970118304524e-8,
-            "velocityX": 3.760097560621335,
-            "velocityY": 0.41509660223897277,
-            "timestamp": 0.6805660406634474
-        },
-        {
-            "x": 4.206338511891951,
-            "y": 6.327608602444822,
-            "heading": -0.05348225236183454,
-            "angularVelocity": 2.0868950806255528e-8,
-            "velocityX": 3.608084592353424,
-            "velocityY": 1.1368221381026216,
-            "timestamp": 0.769026707599896
-        },
-        {
-            "x": 4.5243112033895745,
-            "y": 6.4319075091933104,
-            "heading": -0.05348225052024108,
-            "angularVelocity": 2.0818218151781936e-8,
-            "velocityX": 3.5945093170737707,
-            "velocityY": 1.1790427357213897,
-            "timestamp": 0.8574873745363446
-        },
-        {
-            "x": 4.842283928689835,
-            "y": 6.536206312888755,
-            "heading": -0.053482248678638385,
-            "angularVelocity": 2.0818322537631325e-8,
-            "velocityX": 3.59450969919429,
-            "velocityY": 1.1790415707624449,
-            "timestamp": 0.9459480414727932
-        },
-        {
-            "x": 5.160256832233298,
-            "y": 6.640504573177801,
-            "heading": -0.05348224683713029,
-            "angularVelocity": 2.0817253121924564e-8,
-            "velocityX": 3.5945117141373055,
-            "velocityY": 1.1790354278469999,
-            "timestamp": 1.034408708409242
-        },
-        {
-            "x": 5.478229837776281,
-            "y": 6.744802522501547,
-            "heading": -0.05348224499575697,
-            "angularVelocity": 2.081572963279097e-8,
-            "velocityX": 3.594512867186711,
-            "velocityY": 1.1790319125524373,
-            "timestamp": 1.1228693753456906
-        },
-        {
-            "x": 5.79620288183899,
-            "y": 6.849100354390164,
-            "heading": -0.05348224315440185,
-            "angularVelocity": 2.0815523766741823e-8,
-            "velocityX": 3.5945133026313907,
-            "velocityY": 1.1790305850118254,
-            "timestamp": 1.2113300422821394
-        },
-        {
-            "x": 6.114175956134068,
-            "y": 6.9533980941092235,
-            "heading": -0.053482241313040024,
-            "angularVelocity": 2.081559959710574e-8,
-            "velocityX": 3.5945136443919665,
-            "velocityY": 1.179029543084821,
-            "timestamp": 1.299790709218588
-        },
-        {
-            "x": 6.432149055252676,
-            "y": 7.057695758148605,
-            "heading": -0.05348223947166161,
-            "angularVelocity": 2.0815787162382163e-8,
-            "velocityX": 3.594513925008545,
-            "velocityY": 1.1790286875669842,
-            "timestamp": 1.3882513761550368
-        },
-        {
-            "x": 6.750122174049358,
-            "y": 7.16199336219523,
-            "heading": -0.05348223763026628,
-            "angularVelocity": 2.081597839391937e-8,
-            "velocityX": 3.594514147458541,
-            "velocityY": 1.1790280093813261,
-            "timestamp": 1.4767120430914855
-        },
-        {
-            "x": 7.060081426834384,
-            "y": 7.263662245896702,
-            "heading": -0.036655668946107355,
-            "angularVelocity": 0.1902152591303347,
-            "velocityX": 3.503921726112515,
-            "velocityY": 1.1493117474969148,
-            "timestamp": 1.5651727100279342
-        },
-        {
-            "x": 7.308048847978653,
-            "y": 7.344997351864717,
-            "heading": -0.023193676123292754,
-            "angularVelocity": 0.15218054858760893,
-            "velocityX": 2.8031375947279678,
-            "velocityY": 0.9194493867703608,
-            "timestamp": 1.653633376964383
-        },
-        {
-            "x": 7.4940244209186755,
-            "y": 7.405998678341617,
-            "heading": -0.013096928746230797,
-            "angularVelocity": 0.11413826875525969,
-            "velocityX": 2.102353276102148,
-            "velocityY": 0.6895870061744361,
-            "timestamp": 1.7420940439008317
-        },
-        {
-            "x": 7.618008139456229,
-            "y": 7.4466662269505,
-            "heading": -0.006365663946800556,
-            "angularVelocity": 0.07609330827525898,
-            "velocityX": 1.4015688874087386,
-            "velocityY": 0.4597246439267693,
-            "timestamp": 1.8305547108372804
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -0.003,
-            "angularVelocity": 0.038047010760369614,
-            "velocityX": 0.7007844581174931,
-            "velocityY": 0.2298623077769412,
-            "timestamp": 1.919015377773729
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -0.003,
-            "angularVelocity": -2.311552385941746e-29,
-            "velocityX": 0,
-            "velocityY": -6.724711823689692e-32,
-            "timestamp": 2.007476044710178
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.1.traj
deleted file mode 100644
index 3bdf526e..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.1.traj	
+++ /dev/null
@@ -1,310 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3119316870686255,
-            "y": 6.7398994499669,
-            "heading": 0.3779608263213178,
-            "angularVelocity": -0.23922354190742906,
-            "velocityX": 0.23708773688854562,
-            "velocityY": -0.3397952597632521,
-            "timestamp": 0.05032604058401226
-        },
-        {
-            "x": 2.3368398722232935,
-            "y": 6.706448700500007,
-            "heading": 0.3544233266085098,
-            "angularVelocity": -0.4677002092687843,
-            "velocityX": 0.49493631657919795,
-            "velocityY": -0.6646807314605355,
-            "timestamp": 0.10065208116802452
-        },
-        {
-            "x": 2.3760366485894098,
-            "y": 6.657739626207859,
-            "heading": 0.32015686455898285,
-            "angularVelocity": -0.6808892901544442,
-            "velocityX": 0.7788567491352839,
-            "velocityY": -0.9678701866250936,
-            "timestamp": 0.1509781217520368
-        },
-        {
-            "x": 2.431161593733442,
-            "y": 6.595431091068774,
-            "heading": 0.27624213154004224,
-            "angularVelocity": -0.8726045703046355,
-            "velocityX": 1.0953562907856245,
-            "velocityY": -1.2380973034243006,
-            "timestamp": 0.20130416233604903
-        },
-        {
-            "x": 2.504176998220011,
-            "y": 6.522134358575033,
-            "heading": 0.22423189981090158,
-            "angularVelocity": -1.0334656000271036,
-            "velocityX": 1.450847387143028,
-            "velocityY": -1.456437495244302,
-            "timestamp": 0.2516302029200613
-        },
-        {
-            "x": 2.597103761438428,
-            "y": 6.441982348073212,
-            "heading": 0.16656086207364196,
-            "angularVelocity": -1.1459482420634122,
-            "velocityX": 1.8464946206783215,
-            "velocityY": -1.5926548079620442,
-            "timestamp": 0.3019562435040735
-        },
-        {
-            "x": 2.711127221877276,
-            "y": 6.361040590595544,
-            "heading": 0.10700626596614256,
-            "angularVelocity": -1.1833753543173746,
-            "velocityX": 2.265695038108593,
-            "velocityY": -1.6083474189181441,
-            "timestamp": 0.35228228408808576
-        },
-        {
-            "x": 2.8452820247148356,
-            "y": 6.2864334030632,
-            "heading": 0.05015861793730577,
-            "angularVelocity": -1.1295871355889644,
-            "velocityX": 2.6657134414062895,
-            "velocityY": -1.482476798622636,
-            "timestamp": 0.402608324672098
-        },
-        {
-            "x": 2.996485471725464,
-            "y": 6.224146842956543,
-            "heading": 0,
-            "angularVelocity": -0.9966732402395141,
-            "velocityX": 3.004477309479889,
-            "velocityY": -1.237660650109664,
-            "timestamp": 0.45293436525611025
-        },
-        {
-            "x": 3.2087154283406467,
-            "y": 6.171188923340848,
-            "heading": -0.04850064735870574,
-            "angularVelocity": -0.763447552582083,
-            "velocityX": 3.340706769624593,
-            "velocityY": -0.8336093706421461,
-            "timestamp": 0.5164628253247648
-        },
-        {
-            "x": 3.43894020563635,
-            "y": 6.146133056520132,
-            "heading": -0.0773967080078905,
-            "angularVelocity": -0.45485221297638595,
-            "velocityX": 3.623962819921881,
-            "velocityY": -0.3944038119865702,
-            "timestamp": 0.5799912853934195
-        },
-        {
-            "x": 3.6791864985656395,
-            "y": 6.1522055528567385,
-            "heading": -0.07740096735197836,
-            "angularVelocity": -0.00006704623623761945,
-            "velocityX": 3.781711262474177,
-            "velocityY": 0.09558702241564195,
-            "timestamp": 0.6435197454620741
-        },
-        {
-            "x": 3.9162306085794563,
-            "y": 6.191776893986129,
-            "heading": -0.0774009621424873,
-            "angularVelocity": 8.200247652314305e-8,
-            "velocityX": 3.731305776303155,
-            "velocityY": 0.6228915526463914,
-            "timestamp": 0.7070482055307288
-        },
-        {
-            "x": 4.1454234559206995,
-            "y": 6.264071031070342,
-            "heading": -0.0774009579813014,
-            "angularVelocity": 6.550112963869378e-8,
-            "velocityX": 3.607719234710775,
-            "velocityY": 1.1379803163195765,
-            "timestamp": 0.7705766655993834
-        },
-        {
-            "x": 4.362271301821872,
-            "y": 6.367670338469585,
-            "heading": -0.07740095431740261,
-            "angularVelocity": 5.7673344791685836e-8,
-            "velocityX": 3.4133968565714166,
-            "velocityY": 1.630754268044481,
-            "timestamp": 0.8341051256680381
-        },
-        {
-            "x": 4.562525576285781,
-            "y": 6.500538810004899,
-            "heading": -0.07740095080920495,
-            "angularVelocity": 5.522245710837058e-8,
-            "velocityX": 3.152197837748554,
-            "velocityY": 2.091479494256989,
-            "timestamp": 0.8976335857366927
-        },
-        {
-            "x": 4.76192015617459,
-            "y": 6.634693969014619,
-            "heading": -0.07740094730433215,
-            "angularVelocity": 5.517012038625121e-8,
-            "velocityX": 3.138665405604548,
-            "velocityY": 2.111733211614557,
-            "timestamp": 0.9611620458053474
-        },
-        {
-            "x": 4.961315208646695,
-            "y": 6.76884842562192,
-            "heading": -0.07740094379946605,
-            "angularVelocity": 5.5170015187013835e-8,
-            "velocityX": 3.138672844527154,
-            "velocityY": 2.1117221551145704,
-            "timestamp": 1.024690505874002
-        },
-        {
-            "x": 5.173514155427892,
-            "y": 6.881664257521493,
-            "heading": -0.07740094020412008,
-            "angularVelocity": 5.659425663991619e-8,
-            "velocityX": 3.3402186445551516,
-            "velocityY": 1.7758313640477164,
-            "timestamp": 1.0882189659426564
-        },
-        {
-            "x": 5.399391012764095,
-            "y": 6.963734040630747,
-            "heading": -0.07740093622781413,
-            "angularVelocity": 6.259093865144696e-8,
-            "velocityX": 3.5555223138118204,
-            "velocityY": 1.2918585311301378,
-            "timestamp": 1.151747426011311
-        },
-        {
-            "x": 5.634517116552883,
-            "y": 7.013448397757994,
-            "heading": -0.07740093140263109,
-            "angularVelocity": 7.595309327466021e-8,
-            "velocityX": 3.7011144852982407,
-            "velocityY": 0.7825525295830946,
-            "timestamp": 1.2152758860799655
-        },
-        {
-            "x": 5.874282332827973,
-            "y": 7.02983251834796,
-            "heading": -0.07740092481326658,
-            "angularVelocity": 1.0372303211074395e-7,
-            "velocityX": 3.7741386461434376,
-            "velocityY": 0.2579020579478282,
-            "timestamp": 1.27880434614862
-        },
-        {
-            "x": 6.113985538482666,
-            "y": 7.0125651359558105,
-            "heading": -0.07740091266766538,
-            "angularVelocity": 1.9118362343988029e-7,
-            "velocityX": 3.773162538422118,
-            "velocityY": -0.27180546126086014,
-            "timestamp": 1.3423328062172746
-        },
-        {
-            "x": 6.409905183229014,
-            "y": 6.936971954690801,
-            "heading": -0.07740090470499697,
-            "angularVelocity": 9.862508855382814e-8,
-            "velocityX": 3.66524129970236,
-            "velocityY": -0.9362921822438863,
-            "timestamp": 1.4230695491897984
-        },
-        {
-            "x": 6.687740416068466,
-            "y": 6.8101241855802925,
-            "heading": -0.0774008999193729,
-            "angularVelocity": 5.9274425678096215e-8,
-            "velocityX": 3.441248960637469,
-            "velocityY": -1.5711281436467635,
-            "timestamp": 1.5038062921623223
-        },
-        {
-            "x": 6.938692776760068,
-            "y": 6.63603889201252,
-            "heading": -0.0774008963221294,
-            "angularVelocity": 4.455522177089406e-8,
-            "velocityX": 3.108279470438973,
-            "velocityY": -2.156209021548162,
-            "timestamp": 1.5845430351348462
-        },
-        {
-            "x": 7.154815342738161,
-            "y": 6.420229219532517,
-            "heading": -0.07740089316210408,
-            "angularVelocity": 3.913986631676039e-8,
-            "velocityX": 2.6768799188697083,
-            "velocityY": -2.6730044405364133,
-            "timestamp": 1.66527977810737
-        },
-        {
-            "x": 7.329876929572681,
-            "y": 6.2124862031192585,
-            "heading": -0.05260075245643602,
-            "angularVelocity": 0.3071729152375715,
-            "velocityX": 2.1683013258795385,
-            "velocityY": -2.5730913678789142,
-            "timestamp": 1.746016521079894
-        },
-        {
-            "x": 7.469926171014247,
-            "y": 6.046291741893866,
-            "heading": -0.03276050638448697,
-            "angularVelocity": 0.2457399858041329,
-            "velocityX": 1.7346407135748194,
-            "velocityY": -2.0584736900019895,
-            "timestamp": 1.8267532640524178
-        },
-        {
-            "x": 7.574963090588109,
-            "y": 5.921645878284834,
-            "heading": -0.01788026453634592,
-            "angularVelocity": 0.184305698995044,
-            "velocityX": 1.300980392652287,
-            "velocityY": -1.5438554866085237,
-            "timestamp": 1.9074900070249416
-        },
-        {
-            "x": 7.644987698235257,
-            "y": 5.8385486280487005,
-            "heading": -0.007960086700925195,
-            "angularVelocity": 0.12287067164441078,
-            "velocityX": 0.8673201948582164,
-            "velocityY": -1.0292370880556136,
-            "timestamp": 1.9882267499974655
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -0.003,
-            "angularVelocity": 0.06143530836524976,
-            "velocityX": 0.43366007192855915,
-            "velocityY": -0.5146185803264369,
-            "timestamp": 2.0689634929699894
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -0.003,
-            "angularVelocity": -2.7405624218065555e-29,
-            "velocityX": 1.4947392660092065e-31,
-            "velocityY": 0,
-            "timestamp": 2.1497002359425132
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.traj
deleted file mode 100644
index 3bdf526e..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.traj	
+++ /dev/null
@@ -1,310 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3119316870686255,
-            "y": 6.7398994499669,
-            "heading": 0.3779608263213178,
-            "angularVelocity": -0.23922354190742906,
-            "velocityX": 0.23708773688854562,
-            "velocityY": -0.3397952597632521,
-            "timestamp": 0.05032604058401226
-        },
-        {
-            "x": 2.3368398722232935,
-            "y": 6.706448700500007,
-            "heading": 0.3544233266085098,
-            "angularVelocity": -0.4677002092687843,
-            "velocityX": 0.49493631657919795,
-            "velocityY": -0.6646807314605355,
-            "timestamp": 0.10065208116802452
-        },
-        {
-            "x": 2.3760366485894098,
-            "y": 6.657739626207859,
-            "heading": 0.32015686455898285,
-            "angularVelocity": -0.6808892901544442,
-            "velocityX": 0.7788567491352839,
-            "velocityY": -0.9678701866250936,
-            "timestamp": 0.1509781217520368
-        },
-        {
-            "x": 2.431161593733442,
-            "y": 6.595431091068774,
-            "heading": 0.27624213154004224,
-            "angularVelocity": -0.8726045703046355,
-            "velocityX": 1.0953562907856245,
-            "velocityY": -1.2380973034243006,
-            "timestamp": 0.20130416233604903
-        },
-        {
-            "x": 2.504176998220011,
-            "y": 6.522134358575033,
-            "heading": 0.22423189981090158,
-            "angularVelocity": -1.0334656000271036,
-            "velocityX": 1.450847387143028,
-            "velocityY": -1.456437495244302,
-            "timestamp": 0.2516302029200613
-        },
-        {
-            "x": 2.597103761438428,
-            "y": 6.441982348073212,
-            "heading": 0.16656086207364196,
-            "angularVelocity": -1.1459482420634122,
-            "velocityX": 1.8464946206783215,
-            "velocityY": -1.5926548079620442,
-            "timestamp": 0.3019562435040735
-        },
-        {
-            "x": 2.711127221877276,
-            "y": 6.361040590595544,
-            "heading": 0.10700626596614256,
-            "angularVelocity": -1.1833753543173746,
-            "velocityX": 2.265695038108593,
-            "velocityY": -1.6083474189181441,
-            "timestamp": 0.35228228408808576
-        },
-        {
-            "x": 2.8452820247148356,
-            "y": 6.2864334030632,
-            "heading": 0.05015861793730577,
-            "angularVelocity": -1.1295871355889644,
-            "velocityX": 2.6657134414062895,
-            "velocityY": -1.482476798622636,
-            "timestamp": 0.402608324672098
-        },
-        {
-            "x": 2.996485471725464,
-            "y": 6.224146842956543,
-            "heading": 0,
-            "angularVelocity": -0.9966732402395141,
-            "velocityX": 3.004477309479889,
-            "velocityY": -1.237660650109664,
-            "timestamp": 0.45293436525611025
-        },
-        {
-            "x": 3.2087154283406467,
-            "y": 6.171188923340848,
-            "heading": -0.04850064735870574,
-            "angularVelocity": -0.763447552582083,
-            "velocityX": 3.340706769624593,
-            "velocityY": -0.8336093706421461,
-            "timestamp": 0.5164628253247648
-        },
-        {
-            "x": 3.43894020563635,
-            "y": 6.146133056520132,
-            "heading": -0.0773967080078905,
-            "angularVelocity": -0.45485221297638595,
-            "velocityX": 3.623962819921881,
-            "velocityY": -0.3944038119865702,
-            "timestamp": 0.5799912853934195
-        },
-        {
-            "x": 3.6791864985656395,
-            "y": 6.1522055528567385,
-            "heading": -0.07740096735197836,
-            "angularVelocity": -0.00006704623623761945,
-            "velocityX": 3.781711262474177,
-            "velocityY": 0.09558702241564195,
-            "timestamp": 0.6435197454620741
-        },
-        {
-            "x": 3.9162306085794563,
-            "y": 6.191776893986129,
-            "heading": -0.0774009621424873,
-            "angularVelocity": 8.200247652314305e-8,
-            "velocityX": 3.731305776303155,
-            "velocityY": 0.6228915526463914,
-            "timestamp": 0.7070482055307288
-        },
-        {
-            "x": 4.1454234559206995,
-            "y": 6.264071031070342,
-            "heading": -0.0774009579813014,
-            "angularVelocity": 6.550112963869378e-8,
-            "velocityX": 3.607719234710775,
-            "velocityY": 1.1379803163195765,
-            "timestamp": 0.7705766655993834
-        },
-        {
-            "x": 4.362271301821872,
-            "y": 6.367670338469585,
-            "heading": -0.07740095431740261,
-            "angularVelocity": 5.7673344791685836e-8,
-            "velocityX": 3.4133968565714166,
-            "velocityY": 1.630754268044481,
-            "timestamp": 0.8341051256680381
-        },
-        {
-            "x": 4.562525576285781,
-            "y": 6.500538810004899,
-            "heading": -0.07740095080920495,
-            "angularVelocity": 5.522245710837058e-8,
-            "velocityX": 3.152197837748554,
-            "velocityY": 2.091479494256989,
-            "timestamp": 0.8976335857366927
-        },
-        {
-            "x": 4.76192015617459,
-            "y": 6.634693969014619,
-            "heading": -0.07740094730433215,
-            "angularVelocity": 5.517012038625121e-8,
-            "velocityX": 3.138665405604548,
-            "velocityY": 2.111733211614557,
-            "timestamp": 0.9611620458053474
-        },
-        {
-            "x": 4.961315208646695,
-            "y": 6.76884842562192,
-            "heading": -0.07740094379946605,
-            "angularVelocity": 5.5170015187013835e-8,
-            "velocityX": 3.138672844527154,
-            "velocityY": 2.1117221551145704,
-            "timestamp": 1.024690505874002
-        },
-        {
-            "x": 5.173514155427892,
-            "y": 6.881664257521493,
-            "heading": -0.07740094020412008,
-            "angularVelocity": 5.659425663991619e-8,
-            "velocityX": 3.3402186445551516,
-            "velocityY": 1.7758313640477164,
-            "timestamp": 1.0882189659426564
-        },
-        {
-            "x": 5.399391012764095,
-            "y": 6.963734040630747,
-            "heading": -0.07740093622781413,
-            "angularVelocity": 6.259093865144696e-8,
-            "velocityX": 3.5555223138118204,
-            "velocityY": 1.2918585311301378,
-            "timestamp": 1.151747426011311
-        },
-        {
-            "x": 5.634517116552883,
-            "y": 7.013448397757994,
-            "heading": -0.07740093140263109,
-            "angularVelocity": 7.595309327466021e-8,
-            "velocityX": 3.7011144852982407,
-            "velocityY": 0.7825525295830946,
-            "timestamp": 1.2152758860799655
-        },
-        {
-            "x": 5.874282332827973,
-            "y": 7.02983251834796,
-            "heading": -0.07740092481326658,
-            "angularVelocity": 1.0372303211074395e-7,
-            "velocityX": 3.7741386461434376,
-            "velocityY": 0.2579020579478282,
-            "timestamp": 1.27880434614862
-        },
-        {
-            "x": 6.113985538482666,
-            "y": 7.0125651359558105,
-            "heading": -0.07740091266766538,
-            "angularVelocity": 1.9118362343988029e-7,
-            "velocityX": 3.773162538422118,
-            "velocityY": -0.27180546126086014,
-            "timestamp": 1.3423328062172746
-        },
-        {
-            "x": 6.409905183229014,
-            "y": 6.936971954690801,
-            "heading": -0.07740090470499697,
-            "angularVelocity": 9.862508855382814e-8,
-            "velocityX": 3.66524129970236,
-            "velocityY": -0.9362921822438863,
-            "timestamp": 1.4230695491897984
-        },
-        {
-            "x": 6.687740416068466,
-            "y": 6.8101241855802925,
-            "heading": -0.0774008999193729,
-            "angularVelocity": 5.9274425678096215e-8,
-            "velocityX": 3.441248960637469,
-            "velocityY": -1.5711281436467635,
-            "timestamp": 1.5038062921623223
-        },
-        {
-            "x": 6.938692776760068,
-            "y": 6.63603889201252,
-            "heading": -0.0774008963221294,
-            "angularVelocity": 4.455522177089406e-8,
-            "velocityX": 3.108279470438973,
-            "velocityY": -2.156209021548162,
-            "timestamp": 1.5845430351348462
-        },
-        {
-            "x": 7.154815342738161,
-            "y": 6.420229219532517,
-            "heading": -0.07740089316210408,
-            "angularVelocity": 3.913986631676039e-8,
-            "velocityX": 2.6768799188697083,
-            "velocityY": -2.6730044405364133,
-            "timestamp": 1.66527977810737
-        },
-        {
-            "x": 7.329876929572681,
-            "y": 6.2124862031192585,
-            "heading": -0.05260075245643602,
-            "angularVelocity": 0.3071729152375715,
-            "velocityX": 2.1683013258795385,
-            "velocityY": -2.5730913678789142,
-            "timestamp": 1.746016521079894
-        },
-        {
-            "x": 7.469926171014247,
-            "y": 6.046291741893866,
-            "heading": -0.03276050638448697,
-            "angularVelocity": 0.2457399858041329,
-            "velocityX": 1.7346407135748194,
-            "velocityY": -2.0584736900019895,
-            "timestamp": 1.8267532640524178
-        },
-        {
-            "x": 7.574963090588109,
-            "y": 5.921645878284834,
-            "heading": -0.01788026453634592,
-            "angularVelocity": 0.184305698995044,
-            "velocityX": 1.300980392652287,
-            "velocityY": -1.5438554866085237,
-            "timestamp": 1.9074900070249416
-        },
-        {
-            "x": 7.644987698235257,
-            "y": 5.8385486280487005,
-            "heading": -0.007960086700925195,
-            "angularVelocity": 0.12287067164441078,
-            "velocityX": 0.8673201948582164,
-            "velocityY": -1.0292370880556136,
-            "timestamp": 1.9882267499974655
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -0.003,
-            "angularVelocity": 0.06143530836524976,
-            "velocityX": 0.43366007192855915,
-            "velocityY": -0.5146185803264369,
-            "timestamp": 2.0689634929699894
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -0.003,
-            "angularVelocity": -2.7405624218065555e-29,
-            "velocityX": 1.4947392660092065e-31,
-            "velocityY": 0,
-            "timestamp": 2.1497002359425132
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.1.traj
deleted file mode 100644
index 9479c37b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.1.traj	
+++ /dev/null
@@ -1,373 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.308859553563438,
-            "y": 6.744970476661412,
-            "heading": 0.38132184535212243,
-            "angularVelocity": -0.20370231005101772,
-            "velocityX": 0.20796028650336576,
-            "velocityY": -0.2823689819220092,
-            "timestamp": 0.04260214155501803
-        },
-        {
-            "x": 2.3271744503017344,
-            "y": 6.7213636093363665,
-            "heading": 0.36416244677454657,
-            "angularVelocity": -0.4027825351318442,
-            "velocityX": 0.4299055415945176,
-            "velocityY": -0.554123958640874,
-            "timestamp": 0.08520428311003606
-        },
-        {
-            "x": 2.355668020695042,
-            "y": 6.686794512902429,
-            "heading": 0.33880572426100314,
-            "angularVelocity": -0.5951983066578187,
-            "velocityX": 0.6688295318795241,
-            "velocityY": -0.8114403448308736,
-            "timestamp": 0.1278064246650541
-        },
-        {
-            "x": 2.3952194813587413,
-            "y": 6.64212767454663,
-            "heading": 0.305649210011626,
-            "angularVelocity": -0.7782828054913067,
-            "velocityX": 0.9283913723590802,
-            "velocityY": -1.0484646246742158,
-            "timestamp": 0.1704085662200721
-        },
-        {
-            "x": 2.4468808744427974,
-            "y": 6.58861694166169,
-            "heading": 0.2652376454484053,
-            "angularVelocity": -0.9485805898051318,
-            "velocityX": 1.2126477965277631,
-            "velocityY": -1.2560573466907532,
-            "timestamp": 0.21301070777509012
-        },
-        {
-            "x": 2.5118430486187977,
-            "y": 6.5281213977332095,
-            "heading": 0.2183393877421899,
-            "angularVelocity": -1.100842727487048,
-            "velocityX": 1.5248570096436473,
-            "velocityY": -1.4200118050486843,
-            "timestamp": 0.25561284933010814
-        },
-        {
-            "x": 2.5912567436103777,
-            "y": 6.463385022311091,
-            "heading": 0.16616355843360497,
-            "angularVelocity": -1.2247231571962895,
-            "velocityX": 1.864077534436207,
-            "velocityY": -1.5195568358579936,
-            "timestamp": 0.29821499088512615
-        },
-        {
-            "x": 2.6857686356352475,
-            "y": 6.398206753066201,
-            "heading": 0.11066645077656016,
-            "angularVelocity": -1.3026835185121792,
-            "velocityX": 2.21847748904391,
-            "velocityY": -1.5299294088471524,
-            "timestamp": 0.34081713244014417
-        },
-        {
-            "x": 2.794870523929059,
-            "y": 6.337041243081873,
-            "heading": 0.05441835033855986,
-            "angularVelocity": -1.3203115708481314,
-            "velocityX": 2.5609484479298716,
-            "velocityY": -1.43573791719686,
-            "timestamp": 0.3834192739951622
-        },
-        {
-            "x": 2.916773796081543,
-            "y": 6.28393030166626,
-            "heading": 0,
-            "angularVelocity": -1.2773618497155108,
-            "velocityX": 2.8614353105947288,
-            "velocityY": -1.246673042176136,
-            "timestamp": 0.4260214155501802
-        },
-        {
-            "x": 3.0391710673922954,
-            "y": 6.243974094587608,
-            "heading": -0.047264422869834,
-            "angularVelocity": -1.1928681768219465,
-            "velocityX": 3.0890847917160027,
-            "velocityY": -1.0084220857175146,
-            "timestamp": 0.4656439185125414
-        },
-        {
-            "x": 3.1702962457898094,
-            "y": 6.213670351410701,
-            "heading": -0.08947327835139807,
-            "angularVelocity": -1.0652748394431244,
-            "velocityX": 3.309361312233972,
-            "velocityY": -0.7648114306582993,
-            "timestamp": 0.5052664214749026
-        },
-        {
-            "x": 3.3090884442062665,
-            "y": 6.193565770320959,
-            "heading": -0.12122975141158922,
-            "angularVelocity": -0.8014756940104903,
-            "velocityX": 3.502862970274757,
-            "velocityY": -0.5074031064832265,
-            "timestamp": 0.5448889244372639
-        },
-        {
-            "x": 3.4545421039318374,
-            "y": 6.168639069925164,
-            "heading": -0.11367597167549683,
-            "angularVelocity": 0.19064367900402435,
-            "velocityX": 3.6709861530893746,
-            "velocityY": -0.6291046383282068,
-            "timestamp": 0.5845114273996251
-        },
-        {
-            "x": 3.5909551218669695,
-            "y": 6.134279746914322,
-            "heading": -0.10324626427429143,
-            "angularVelocity": 0.26322686911306303,
-            "velocityX": 3.442816776737095,
-            "velocityY": -0.8671668986555798,
-            "timestamp": 0.6241339303619864
-        },
-        {
-            "x": 3.7180733265010666,
-            "y": 6.090709420708735,
-            "heading": -0.09157048631886859,
-            "angularVelocity": 0.2946754263988333,
-            "velocityX": 3.208232573163052,
-            "velocityY": -1.0996358873889647,
-            "timestamp": 0.6637564333243476
-        },
-        {
-            "x": 3.835817587911086,
-            "y": 6.038004134131813,
-            "heading": -0.07918178962672723,
-            "angularVelocity": 0.31266820028784686,
-            "velocityX": 2.9716512740719234,
-            "velocityY": -1.3301856933921488,
-            "timestamp": 0.7033789362867089
-        },
-        {
-            "x": 3.9441493716247042,
-            "y": 5.9762022678896844,
-            "heading": -0.06634465187684353,
-            "angularVelocity": 0.3239860379864983,
-            "velocityX": 2.734097434897688,
-            "velocityY": -1.5597668400917784,
-            "timestamp": 0.7430014392490701
-        },
-        {
-            "x": 4.0430458841375625,
-            "y": 5.905326959185002,
-            "heading": -0.05321706206750646,
-            "angularVelocity": 0.3313165203573185,
-            "velocityX": 2.4959683290781256,
-            "velocityY": -1.7887640458253868,
-            "timestamp": 0.7826239422114314
-        },
-        {
-            "x": 4.1324920654296875,
-            "y": 5.8253936767578125,
-            "heading": -0.03990404951279715,
-            "angularVelocity": 0.33599625362778834,
-            "velocityX": 2.2574591357111773,
-            "velocityY": -2.017370848659449,
-            "timestamp": 0.8222464451737926
-        },
-        {
-            "x": 4.241428434426057,
-            "y": 5.691951873954815,
-            "heading": -0.020558340356236417,
-            "angularVelocity": 0.3400028267859621,
-            "velocityX": 1.9145678816329739,
-            "velocityY": -2.345253399462351,
-            "timestamp": 0.8791451166920127
-        },
-        {
-            "x": 4.330837596388679,
-            "y": 5.539871857431794,
-            "heading": -0.0011062192039580838,
-            "angularVelocity": 0.34187302854776463,
-            "velocityX": 1.571375211001045,
-            "velocityY": -2.6728219212345348,
-            "timestamp": 0.9360437882102327
-        },
-        {
-            "x": 4.400691025411647,
-            "y": 5.369183558238886,
-            "heading": 0.018249380247094018,
-            "angularVelocity": 0.34017664972817613,
-            "velocityX": 1.2276811946408939,
-            "velocityY": -2.999864401724217,
-            "timestamp": 0.9929424597284527
-        },
-        {
-            "x": 4.450931892836431,
-            "y": 5.179947405025881,
-            "heading": 0.037101343157351356,
-            "angularVelocity": 0.3313251857597514,
-            "velocityX": 0.8829884087662048,
-            "velocityY": -3.3258448424829754,
-            "timestamp": 1.0498411312466729
-        },
-        {
-            "x": 4.481391591894744,
-            "y": 4.9723492336445485,
-            "heading": 0.054218740493726776,
-            "angularVelocity": 0.3008400175194617,
-            "velocityX": 0.5353323416094045,
-            "velocityY": -3.6485591990465953,
-            "timestamp": 1.106739802764893
-        },
-        {
-            "x": 4.496396423991072,
-            "y": 4.780842919446315,
-            "heading": 0.0400753404119281,
-            "angularVelocity": -0.248571710101697,
-            "velocityX": 0.26371146629537756,
-            "velocityY": -3.3657431551263746,
-            "timestamp": 1.163638474283113
-        },
-        {
-            "x": 4.530633514554346,
-            "y": 4.608273372061934,
-            "heading": 0.027581622688211512,
-            "angularVelocity": -0.21957837310341866,
-            "velocityX": 0.6017203855508131,
-            "velocityY": -3.032927532044774,
-            "timestamp": 1.220537145801333
-        },
-        {
-            "x": 4.584102924369241,
-            "y": 4.454640545632531,
-            "heading": 0.016737783854959394,
-            "angularVelocity": -0.1905815820283204,
-            "velocityX": 0.9397303731032456,
-            "velocityY": -2.7001127149375717,
-            "timestamp": 1.277435817319553
-        },
-        {
-            "x": 4.656804671911905,
-            "y": 4.319944426632471,
-            "heading": 0.007543896915478472,
-            "angularVelocity": -0.1615835079126032,
-            "velocityX": 1.277740685375807,
-            "velocityY": -2.3672981355448273,
-            "timestamp": 1.334334488837773
-        },
-        {
-            "x": 4.748738765716553,
-            "y": 4.2041850090026855,
-            "heading": 0,
-            "angularVelocity": -0.13258476365415262,
-            "velocityX": 1.6157511476381003,
-            "velocityY": -2.0344836626408243,
-            "timestamp": 1.391233160355993
-        },
-        {
-            "x": 5.007932614843106,
-            "y": 4.0487951090615,
-            "heading": -0.008478617739268947,
-            "angularVelocity": -0.07486216912690727,
-            "velocityX": 2.288558626731898,
-            "velocityY": -1.372019039864535,
-            "timestamp": 1.5044895332096289
-        },
-        {
-            "x": 5.344428643764395,
-            "y": 3.9673190703928256,
-            "heading": -0.018205683784992727,
-            "angularVelocity": -0.08588537492980046,
-            "velocityX": 2.971100172492302,
-            "velocityY": -0.7193947379364598,
-            "timestamp": 1.6177459060632646
-        },
-        {
-            "x": 5.761576858213634,
-            "y": 3.95566669131606,
-            "heading": -0.05529230717881331,
-            "angularVelocity": -0.3274572764373147,
-            "velocityX": 3.6832206783483272,
-            "velocityY": -0.10288497488635052,
-            "timestamp": 1.7310022789169004
-        },
-        {
-            "x": 6.1883195012502945,
-            "y": 3.9937898497766295,
-            "heading": -0.05529230006422074,
-            "angularVelocity": 6.281847442205705e-8,
-            "velocityX": 3.767934927495438,
-            "velocityY": 0.33660938894658915,
-            "timestamp": 1.8442586517705362
-        },
-        {
-            "x": 6.615062139687873,
-            "y": 4.031913059718256,
-            "heading": -0.05529229294956677,
-            "angularVelocity": 6.281901664491759e-8,
-            "velocityX": 3.7679348868877285,
-            "velocityY": 0.3366098434998829,
-            "timestamp": 1.957515024624172
-        },
-        {
-            "x": 7.0409980156903185,
-            "y": 4.069937864184792,
-            "heading": -0.05280872948360114,
-            "angularVelocity": 0.021928686248633502,
-            "velocityX": 3.7608115576232954,
-            "velocityY": 0.33574096987616325,
-            "timestamp": 2.0707713974778077
-        },
-        {
-            "x": 7.360520062907597,
-            "y": 4.098469259621894,
-            "heading": -0.032478143170072554,
-            "angularVelocity": 0.17950942451425975,
-            "velocityX": 2.821227973018394,
-            "velocityY": 0.25191867546363345,
-            "timestamp": 2.1840277703314435
-        },
-        {
-            "x": 7.573513556720189,
-            "y": 4.1174900304811795,
-            "heading": -0.013829576144119837,
-            "angularVelocity": 0.16465799280057086,
-            "velocityX": 1.88063142449255,
-            "velocityY": 0.1679443759325155,
-            "timestamp": 2.2972841431850792
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -0.003,
-            "angularVelocity": 0.09562001564464007,
-            "velocityX": 0.9402247361163992,
-            "velocityY": 0.08396851567117221,
-            "timestamp": 2.410540516038715
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -0.003,
-            "angularVelocity": 0,
-            "velocityX": 2.757511206578506e-35,
-            "velocityY": 0,
-            "timestamp": 2.5237968888923508
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.traj
deleted file mode 100644
index 9479c37b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.traj	
+++ /dev/null
@@ -1,373 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.308859553563438,
-            "y": 6.744970476661412,
-            "heading": 0.38132184535212243,
-            "angularVelocity": -0.20370231005101772,
-            "velocityX": 0.20796028650336576,
-            "velocityY": -0.2823689819220092,
-            "timestamp": 0.04260214155501803
-        },
-        {
-            "x": 2.3271744503017344,
-            "y": 6.7213636093363665,
-            "heading": 0.36416244677454657,
-            "angularVelocity": -0.4027825351318442,
-            "velocityX": 0.4299055415945176,
-            "velocityY": -0.554123958640874,
-            "timestamp": 0.08520428311003606
-        },
-        {
-            "x": 2.355668020695042,
-            "y": 6.686794512902429,
-            "heading": 0.33880572426100314,
-            "angularVelocity": -0.5951983066578187,
-            "velocityX": 0.6688295318795241,
-            "velocityY": -0.8114403448308736,
-            "timestamp": 0.1278064246650541
-        },
-        {
-            "x": 2.3952194813587413,
-            "y": 6.64212767454663,
-            "heading": 0.305649210011626,
-            "angularVelocity": -0.7782828054913067,
-            "velocityX": 0.9283913723590802,
-            "velocityY": -1.0484646246742158,
-            "timestamp": 0.1704085662200721
-        },
-        {
-            "x": 2.4468808744427974,
-            "y": 6.58861694166169,
-            "heading": 0.2652376454484053,
-            "angularVelocity": -0.9485805898051318,
-            "velocityX": 1.2126477965277631,
-            "velocityY": -1.2560573466907532,
-            "timestamp": 0.21301070777509012
-        },
-        {
-            "x": 2.5118430486187977,
-            "y": 6.5281213977332095,
-            "heading": 0.2183393877421899,
-            "angularVelocity": -1.100842727487048,
-            "velocityX": 1.5248570096436473,
-            "velocityY": -1.4200118050486843,
-            "timestamp": 0.25561284933010814
-        },
-        {
-            "x": 2.5912567436103777,
-            "y": 6.463385022311091,
-            "heading": 0.16616355843360497,
-            "angularVelocity": -1.2247231571962895,
-            "velocityX": 1.864077534436207,
-            "velocityY": -1.5195568358579936,
-            "timestamp": 0.29821499088512615
-        },
-        {
-            "x": 2.6857686356352475,
-            "y": 6.398206753066201,
-            "heading": 0.11066645077656016,
-            "angularVelocity": -1.3026835185121792,
-            "velocityX": 2.21847748904391,
-            "velocityY": -1.5299294088471524,
-            "timestamp": 0.34081713244014417
-        },
-        {
-            "x": 2.794870523929059,
-            "y": 6.337041243081873,
-            "heading": 0.05441835033855986,
-            "angularVelocity": -1.3203115708481314,
-            "velocityX": 2.5609484479298716,
-            "velocityY": -1.43573791719686,
-            "timestamp": 0.3834192739951622
-        },
-        {
-            "x": 2.916773796081543,
-            "y": 6.28393030166626,
-            "heading": 0,
-            "angularVelocity": -1.2773618497155108,
-            "velocityX": 2.8614353105947288,
-            "velocityY": -1.246673042176136,
-            "timestamp": 0.4260214155501802
-        },
-        {
-            "x": 3.0391710673922954,
-            "y": 6.243974094587608,
-            "heading": -0.047264422869834,
-            "angularVelocity": -1.1928681768219465,
-            "velocityX": 3.0890847917160027,
-            "velocityY": -1.0084220857175146,
-            "timestamp": 0.4656439185125414
-        },
-        {
-            "x": 3.1702962457898094,
-            "y": 6.213670351410701,
-            "heading": -0.08947327835139807,
-            "angularVelocity": -1.0652748394431244,
-            "velocityX": 3.309361312233972,
-            "velocityY": -0.7648114306582993,
-            "timestamp": 0.5052664214749026
-        },
-        {
-            "x": 3.3090884442062665,
-            "y": 6.193565770320959,
-            "heading": -0.12122975141158922,
-            "angularVelocity": -0.8014756940104903,
-            "velocityX": 3.502862970274757,
-            "velocityY": -0.5074031064832265,
-            "timestamp": 0.5448889244372639
-        },
-        {
-            "x": 3.4545421039318374,
-            "y": 6.168639069925164,
-            "heading": -0.11367597167549683,
-            "angularVelocity": 0.19064367900402435,
-            "velocityX": 3.6709861530893746,
-            "velocityY": -0.6291046383282068,
-            "timestamp": 0.5845114273996251
-        },
-        {
-            "x": 3.5909551218669695,
-            "y": 6.134279746914322,
-            "heading": -0.10324626427429143,
-            "angularVelocity": 0.26322686911306303,
-            "velocityX": 3.442816776737095,
-            "velocityY": -0.8671668986555798,
-            "timestamp": 0.6241339303619864
-        },
-        {
-            "x": 3.7180733265010666,
-            "y": 6.090709420708735,
-            "heading": -0.09157048631886859,
-            "angularVelocity": 0.2946754263988333,
-            "velocityX": 3.208232573163052,
-            "velocityY": -1.0996358873889647,
-            "timestamp": 0.6637564333243476
-        },
-        {
-            "x": 3.835817587911086,
-            "y": 6.038004134131813,
-            "heading": -0.07918178962672723,
-            "angularVelocity": 0.31266820028784686,
-            "velocityX": 2.9716512740719234,
-            "velocityY": -1.3301856933921488,
-            "timestamp": 0.7033789362867089
-        },
-        {
-            "x": 3.9441493716247042,
-            "y": 5.9762022678896844,
-            "heading": -0.06634465187684353,
-            "angularVelocity": 0.3239860379864983,
-            "velocityX": 2.734097434897688,
-            "velocityY": -1.5597668400917784,
-            "timestamp": 0.7430014392490701
-        },
-        {
-            "x": 4.0430458841375625,
-            "y": 5.905326959185002,
-            "heading": -0.05321706206750646,
-            "angularVelocity": 0.3313165203573185,
-            "velocityX": 2.4959683290781256,
-            "velocityY": -1.7887640458253868,
-            "timestamp": 0.7826239422114314
-        },
-        {
-            "x": 4.1324920654296875,
-            "y": 5.8253936767578125,
-            "heading": -0.03990404951279715,
-            "angularVelocity": 0.33599625362778834,
-            "velocityX": 2.2574591357111773,
-            "velocityY": -2.017370848659449,
-            "timestamp": 0.8222464451737926
-        },
-        {
-            "x": 4.241428434426057,
-            "y": 5.691951873954815,
-            "heading": -0.020558340356236417,
-            "angularVelocity": 0.3400028267859621,
-            "velocityX": 1.9145678816329739,
-            "velocityY": -2.345253399462351,
-            "timestamp": 0.8791451166920127
-        },
-        {
-            "x": 4.330837596388679,
-            "y": 5.539871857431794,
-            "heading": -0.0011062192039580838,
-            "angularVelocity": 0.34187302854776463,
-            "velocityX": 1.571375211001045,
-            "velocityY": -2.6728219212345348,
-            "timestamp": 0.9360437882102327
-        },
-        {
-            "x": 4.400691025411647,
-            "y": 5.369183558238886,
-            "heading": 0.018249380247094018,
-            "angularVelocity": 0.34017664972817613,
-            "velocityX": 1.2276811946408939,
-            "velocityY": -2.999864401724217,
-            "timestamp": 0.9929424597284527
-        },
-        {
-            "x": 4.450931892836431,
-            "y": 5.179947405025881,
-            "heading": 0.037101343157351356,
-            "angularVelocity": 0.3313251857597514,
-            "velocityX": 0.8829884087662048,
-            "velocityY": -3.3258448424829754,
-            "timestamp": 1.0498411312466729
-        },
-        {
-            "x": 4.481391591894744,
-            "y": 4.9723492336445485,
-            "heading": 0.054218740493726776,
-            "angularVelocity": 0.3008400175194617,
-            "velocityX": 0.5353323416094045,
-            "velocityY": -3.6485591990465953,
-            "timestamp": 1.106739802764893
-        },
-        {
-            "x": 4.496396423991072,
-            "y": 4.780842919446315,
-            "heading": 0.0400753404119281,
-            "angularVelocity": -0.248571710101697,
-            "velocityX": 0.26371146629537756,
-            "velocityY": -3.3657431551263746,
-            "timestamp": 1.163638474283113
-        },
-        {
-            "x": 4.530633514554346,
-            "y": 4.608273372061934,
-            "heading": 0.027581622688211512,
-            "angularVelocity": -0.21957837310341866,
-            "velocityX": 0.6017203855508131,
-            "velocityY": -3.032927532044774,
-            "timestamp": 1.220537145801333
-        },
-        {
-            "x": 4.584102924369241,
-            "y": 4.454640545632531,
-            "heading": 0.016737783854959394,
-            "angularVelocity": -0.1905815820283204,
-            "velocityX": 0.9397303731032456,
-            "velocityY": -2.7001127149375717,
-            "timestamp": 1.277435817319553
-        },
-        {
-            "x": 4.656804671911905,
-            "y": 4.319944426632471,
-            "heading": 0.007543896915478472,
-            "angularVelocity": -0.1615835079126032,
-            "velocityX": 1.277740685375807,
-            "velocityY": -2.3672981355448273,
-            "timestamp": 1.334334488837773
-        },
-        {
-            "x": 4.748738765716553,
-            "y": 4.2041850090026855,
-            "heading": 0,
-            "angularVelocity": -0.13258476365415262,
-            "velocityX": 1.6157511476381003,
-            "velocityY": -2.0344836626408243,
-            "timestamp": 1.391233160355993
-        },
-        {
-            "x": 5.007932614843106,
-            "y": 4.0487951090615,
-            "heading": -0.008478617739268947,
-            "angularVelocity": -0.07486216912690727,
-            "velocityX": 2.288558626731898,
-            "velocityY": -1.372019039864535,
-            "timestamp": 1.5044895332096289
-        },
-        {
-            "x": 5.344428643764395,
-            "y": 3.9673190703928256,
-            "heading": -0.018205683784992727,
-            "angularVelocity": -0.08588537492980046,
-            "velocityX": 2.971100172492302,
-            "velocityY": -0.7193947379364598,
-            "timestamp": 1.6177459060632646
-        },
-        {
-            "x": 5.761576858213634,
-            "y": 3.95566669131606,
-            "heading": -0.05529230717881331,
-            "angularVelocity": -0.3274572764373147,
-            "velocityX": 3.6832206783483272,
-            "velocityY": -0.10288497488635052,
-            "timestamp": 1.7310022789169004
-        },
-        {
-            "x": 6.1883195012502945,
-            "y": 3.9937898497766295,
-            "heading": -0.05529230006422074,
-            "angularVelocity": 6.281847442205705e-8,
-            "velocityX": 3.767934927495438,
-            "velocityY": 0.33660938894658915,
-            "timestamp": 1.8442586517705362
-        },
-        {
-            "x": 6.615062139687873,
-            "y": 4.031913059718256,
-            "heading": -0.05529229294956677,
-            "angularVelocity": 6.281901664491759e-8,
-            "velocityX": 3.7679348868877285,
-            "velocityY": 0.3366098434998829,
-            "timestamp": 1.957515024624172
-        },
-        {
-            "x": 7.0409980156903185,
-            "y": 4.069937864184792,
-            "heading": -0.05280872948360114,
-            "angularVelocity": 0.021928686248633502,
-            "velocityX": 3.7608115576232954,
-            "velocityY": 0.33574096987616325,
-            "timestamp": 2.0707713974778077
-        },
-        {
-            "x": 7.360520062907597,
-            "y": 4.098469259621894,
-            "heading": -0.032478143170072554,
-            "angularVelocity": 0.17950942451425975,
-            "velocityX": 2.821227973018394,
-            "velocityY": 0.25191867546363345,
-            "timestamp": 2.1840277703314435
-        },
-        {
-            "x": 7.573513556720189,
-            "y": 4.1174900304811795,
-            "heading": -0.013829576144119837,
-            "angularVelocity": 0.16465799280057086,
-            "velocityX": 1.88063142449255,
-            "velocityY": 0.1679443759325155,
-            "timestamp": 2.2972841431850792
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -0.003,
-            "angularVelocity": 0.09562001564464007,
-            "velocityX": 0.9402247361163992,
-            "velocityY": 0.08396851567117221,
-            "timestamp": 2.410540516038715
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -0.003,
-            "angularVelocity": 0,
-            "velocityX": 2.757511206578506e-35,
-            "velocityY": 0,
-            "timestamp": 2.5237968888923508
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.1.traj
deleted file mode 100644
index b2b4ba4b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.1.traj	
+++ /dev/null
@@ -1,472 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.285270702118571,
-            "y": 6.762383358598232,
-            "heading": 0.3634342922015001,
-            "angularVelocity": -0.5809173586087107,
-            "velocityX": -0.3220883435269421,
-            "velocityY": 0.11771892098839686,
-            "timestamp": 0.04573061452687351
-        },
-        {
-            "x": 2.2599794598619596,
-            "y": 6.767831656358603,
-            "heading": 0.2777726454574198,
-            "angularVelocity": -1.8731794363656886,
-            "velocityX": -0.5530483794778248,
-            "velocityY": 0.11913895793336499,
-            "timestamp": 0.09146122905374703
-        },
-        {
-            "x": 2.2450548028766546,
-            "y": 6.7710281276739375,
-            "heading": 0.1344358774143225,
-            "angularVelocity": -3.134372225827269,
-            "velocityX": -0.3263602980128451,
-            "velocityY": 0.06989784301807915,
-            "timestamp": 0.13719184358062053
-        },
-        {
-            "x": 2.2457261085510254,
-            "y": 6.7668328285217285,
-            "heading": 0,
-            "angularVelocity": -2.939734766416754,
-            "velocityX": 0.014679568191384731,
-            "velocityY": -0.09173940030374239,
-            "timestamp": 0.18292245810749405
-        },
-        {
-            "x": 2.262817902753147,
-            "y": 6.755151488221845,
-            "heading": -0.1222613161134929,
-            "angularVelocity": -2.5936618258096975,
-            "velocityX": 0.3625867573309899,
-            "velocityY": -0.24780893395554135,
-            "timestamp": 0.230060953533885
-        },
-        {
-            "x": 2.2962801062686125,
-            "y": 6.73609161362754,
-            "heading": -0.22784823217966704,
-            "angularVelocity": -2.2399297031255614,
-            "velocityX": 0.7098699950598514,
-            "velocityY": -0.40433778002244786,
-            "timestamp": 0.277199448960276
-        },
-        {
-            "x": 2.3460880772414248,
-            "y": 6.709619977067709,
-            "heading": -0.31657680000453947,
-            "angularVelocity": -1.882295288008833,
-            "velocityX": 1.056630478386546,
-            "velocityY": -0.561571520694102,
-            "timestamp": 0.32433794438666697
-        },
-        {
-            "x": 2.4122219538028076,
-            "y": 6.675695367304005,
-            "heading": -0.3884103898720453,
-            "angularVelocity": -1.5238838070197687,
-            "velocityX": 1.4029696103613034,
-            "velocityY": -0.7196795200363996,
-            "timestamp": 0.37147643981305795
-        },
-        {
-            "x": 2.494664207392629,
-            "y": 6.634268490634927,
-            "heading": -0.4434154648490763,
-            "angularVelocity": -1.1668822791122553,
-            "velocityX": 1.7489368899896385,
-            "velocityY": -0.8788332401012526,
-            "timestamp": 0.4186149352394489
-        },
-        {
-            "x": 2.593395417678489,
-            "y": 6.585276348843886,
-            "heading": -0.4817298536594349,
-            "angularVelocity": -0.8128046613242179,
-            "velocityX": 2.094492185055806,
-            "velocityY": -1.0393234096224127,
-            "timestamp": 0.4657534306658399
-        },
-        {
-            "x": 2.7083870607833584,
-            "y": 6.52862575501488,
-            "heading": -0.5035659675601283,
-            "angularVelocity": -0.46323315377732527,
-            "velocityX": 2.4394423721997165,
-            "velocityY": -1.2017904542040352,
-            "timestamp": 0.5128919260922309
-        },
-        {
-            "x": 2.839583375891639,
-            "y": 6.464143202111409,
-            "heading": -0.5093019910748294,
-            "angularVelocity": -0.12168448446963974,
-            "velocityX": 2.78320964471958,
-            "velocityY": -1.367938291626405,
-            "timestamp": 0.5600304215186218
-        },
-        {
-            "x": 2.9868119835174247,
-            "y": 6.391337595074747,
-            "heading": -0.5000000631264199,
-            "angularVelocity": 0.197331880546237,
-            "velocityX": 3.1233200443517304,
-            "velocityY": -1.5445042608591435,
-            "timestamp": 0.6071689169450127
-        },
-        {
-            "x": 3.140901786552832,
-            "y": 6.301587367083572,
-            "heading": -0.5000000197789056,
-            "angularVelocity": 9.19577807856047e-7,
-            "velocityX": 3.2688740198760886,
-            "velocityY": -1.903968872561393,
-            "timestamp": 0.6543074123714037
-        },
-        {
-            "x": 3.284848213195801,
-            "y": 6.196334362030029,
-            "heading": -0.5,
-            "angularVelocity": 4.1959136469938736e-7,
-            "velocityX": 3.0536915813881054,
-            "velocityY": -2.2328460868682716,
-            "timestamp": 0.7014459077977946
-        },
-        {
-            "x": 3.3993231490932048,
-            "y": 6.0978407623199296,
-            "heading": -0.5170802154644099,
-            "angularVelocity": -0.41193186552987154,
-            "velocityX": 2.7608477187370504,
-            "velocityY": -2.375418059313161,
-            "timestamp": 0.7429095985042958
-        },
-        {
-            "x": 3.501098994358481,
-            "y": 5.994630951270013,
-            "heading": -0.5506635755138305,
-            "angularVelocity": -0.8099462319284754,
-            "velocityX": 2.4545775721162104,
-            "velocityY": -2.489161222534419,
-            "timestamp": 0.7843732892107971
-        },
-        {
-            "x": 3.5900181651549583,
-            "y": 5.890084132174874,
-            "heading": -0.6049860857837608,
-            "angularVelocity": -1.3101224069596595,
-            "velocityX": 2.144506899445626,
-            "velocityY": -2.521406495990601,
-            "timestamp": 0.8258369799172983
-        },
-        {
-            "x": 3.6727603343537245,
-            "y": 5.794679716077001,
-            "heading": -0.692814707704561,
-            "angularVelocity": -2.1182056016767716,
-            "velocityX": 1.9955331469267976,
-            "velocityY": -2.3009147153167167,
-            "timestamp": 0.8673006706237996
-        },
-        {
-            "x": 3.757696306555582,
-            "y": 5.694133573084931,
-            "heading": -0.7686614093789431,
-            "angularVelocity": -1.8292318021376788,
-            "velocityX": 2.04844216119069,
-            "velocityY": -2.424920244168773,
-            "timestamp": 0.9087643613303008
-        },
-        {
-            "x": 3.844342447773648,
-            "y": 5.582483853612312,
-            "heading": -0.8164085841781574,
-            "angularVelocity": -1.1515418426489148,
-            "velocityX": 2.0896871393165974,
-            "velocityY": -2.6927105998111585,
-            "timestamp": 0.950228052036802
-        },
-        {
-            "x": 3.9316331213435283,
-            "y": 5.459771926422603,
-            "heading": -0.8357045871440653,
-            "angularVelocity": -0.4653710906373953,
-            "velocityX": 2.105231639596513,
-            "velocityY": -2.9595032448602216,
-            "timestamp": 0.9916917427433033
-        },
-        {
-            "x": 4.018219294626101,
-            "y": 5.3289825292048665,
-            "heading": -0.8357080754934182,
-            "angularVelocity": -0.00008413021836472818,
-            "velocityX": 2.088240863443359,
-            "velocityY": -3.1543115190486835,
-            "timestamp": 1.0331554334498045
-        },
-        {
-            "x": 4.10449796775504,
-            "y": 5.197989757794864,
-            "heading": -0.8357107471897811,
-            "angularVelocity": -0.00006443460091393523,
-            "velocityX": 2.0808247326516955,
-            "velocityY": -3.159216393376985,
-            "timestamp": 1.0746191241563057
-        },
-        {
-            "x": 4.190712810420311,
-            "y": 5.066954954654073,
-            "heading": -0.8357133876254744,
-            "angularVelocity": -0.00006368067213797694,
-            "velocityX": 2.0792853023028366,
-            "velocityY": -3.160230093078832,
-            "timestamp": 1.1160828148628068
-        },
-        {
-            "x": 4.274456024169922,
-            "y": 4.934326648712158,
-            "heading": -0.8357152799414105,
-            "angularVelocity": -0.000045637904003859675,
-            "velocityX": 2.0196758253473424,
-            "velocityY": -3.198661375339825,
-            "timestamp": 1.157546505569308
-        },
-        {
-            "x": 4.34828200363932,
-            "y": 4.789661343281582,
-            "heading": -0.8357152360129204,
-            "angularVelocity": 0.0000010231801667591938,
-            "velocityX": 1.7195509731200054,
-            "velocityY": -3.36953696405698,
-            "timestamp": 1.2004797949125405
-        },
-        {
-            "x": 4.435451827820083,
-            "y": 4.652622129190998,
-            "heading": -0.8357151829793803,
-            "angularVelocity": 0.0000012352545290785843,
-            "velocityX": 2.0303551280191665,
-            "velocityY": -3.1919104309715474,
-            "timestamp": 1.2434130842557731
-        },
-        {
-            "x": 4.535184717372419,
-            "y": 4.524436546290902,
-            "heading": -0.8357137971440172,
-            "angularVelocity": 0.000032278807056995315,
-            "velocityX": 2.322973410097133,
-            "velocityY": -2.985692101886486,
-            "timestamp": 1.2863463735990057
-        },
-        {
-            "x": 4.644325012525673,
-            "y": 4.4080959710874374,
-            "heading": -0.8258667739389303,
-            "angularVelocity": 0.22935636555504585,
-            "velocityX": 2.542090224690803,
-            "velocityY": -2.7097987827900663,
-            "timestamp": 1.3292796629422383
-        },
-        {
-            "x": 4.760978449232698,
-            "y": 4.3048079626404245,
-            "heading": -0.804942882637706,
-            "angularVelocity": 0.4873582159975465,
-            "velocityX": 2.717085937078615,
-            "velocityY": -2.405779059257989,
-            "timestamp": 1.372212952285471
-        },
-        {
-            "x": 4.883873293679825,
-            "y": 4.215276634825419,
-            "heading": -0.7738338273486849,
-            "angularVelocity": 0.7245905395302493,
-            "velocityX": 2.8624604899158337,
-            "velocityY": -2.085359151013129,
-            "timestamp": 1.4151462416287035
-        },
-        {
-            "x": 5.012023716462111,
-            "y": 4.13998910043764,
-            "heading": -0.7341562289641704,
-            "angularVelocity": 0.9241686111518187,
-            "velocityX": 2.9848731542039757,
-            "velocityY": -1.7535934362235386,
-            "timestamp": 1.458079530971936
-        },
-        {
-            "x": 5.144586044931236,
-            "y": 4.079308119917677,
-            "heading": -0.6878803412234658,
-            "angularVelocity": 1.0778556325081365,
-            "velocityX": 3.0876350379153963,
-            "velocityY": -1.4133783236324275,
-            "timestamp": 1.5010128203151687
-        },
-        {
-            "x": 5.2807921990430025,
-            "y": 4.033507276074697,
-            "heading": -0.6371968549741135,
-            "angularVelocity": 1.1805171936433834,
-            "velocityX": 3.1725068401551706,
-            "velocityY": -1.0667909341122943,
-            "timestamp": 1.5439461096584013
-        },
-        {
-            "x": 5.419841766333411,
-            "y": 4.00280308410274,
-            "heading": -0.5848216118259447,
-            "angularVelocity": 1.2199215096111542,
-            "velocityX": 3.2387354758394924,
-            "velocityY": -0.7151604836631573,
-            "timestamp": 1.5868793990016339
-        },
-        {
-            "x": 5.561074826834354,
-            "y": 3.9873115142730846,
-            "heading": -0.5330346380339317,
-            "angularVelocity": 1.2062195695745372,
-            "velocityX": 3.289593289064067,
-            "velocityY": -0.36082885953151356,
-            "timestamp": 1.6298126883448665
-        },
-        {
-            "x": 5.703982353210449,
-            "y": 3.987083911895752,
-            "heading": -0.4835776002687649,
-            "angularVelocity": 1.1519508176925284,
-            "velocityX": 3.328594863384743,
-            "velocityY": -0.005301303040469438,
-            "timestamp": 1.672745977688099
-        },
-        {
-            "x": 5.958820715702131,
-            "y": 3.94375176713671,
-            "heading": -0.42940135113015304,
-            "angularVelocity": 0.7375353070821884,
-            "velocityX": 3.4692746900164138,
-            "velocityY": -0.5899077030899651,
-            "timestamp": 1.7462017819301305
-        },
-        {
-            "x": 6.2169360873076736,
-            "y": 3.85673688488041,
-            "heading": -0.41197233440562925,
-            "angularVelocity": 0.2372721516614769,
-            "velocityX": 3.513886673339918,
-            "velocityY": -1.184588245329031,
-            "timestamp": 1.819657586172162
-        },
-        {
-            "x": 6.462551803453154,
-            "y": 3.726776847306083,
-            "heading": -0.411972258677301,
-            "angularVelocity": 0.0000010309372967585872,
-            "velocityX": 3.343721012654018,
-            "velocityY": -1.76922761809427,
-            "timestamp": 1.8931133904141935
-        },
-        {
-            "x": 6.683975982637168,
-            "y": 3.558883929748973,
-            "heading": -0.4119722206140506,
-            "angularVelocity": 5.181789344885153e-7,
-            "velocityX": 3.0143864255360504,
-            "velocityY": -2.285631738561,
-            "timestamp": 1.966569194656225
-        },
-        {
-            "x": 6.875523396649561,
-            "y": 3.357572509103632,
-            "heading": -0.4119721861737391,
-            "angularVelocity": 4.688575916806749e-7,
-            "velocityX": 2.6076552559586523,
-            "velocityY": -2.7405788109272424,
-            "timestamp": 2.0400249988982564
-        },
-        {
-            "x": 7.060174216266159,
-            "y": 3.149917085871916,
-            "heading": -0.4119721521055279,
-            "angularVelocity": 4.637919568273184e-7,
-            "velocityX": 2.513767584766965,
-            "velocityY": -2.826943702739948,
-            "timestamp": 2.1134808031402876
-        },
-        {
-            "x": 7.236976316543012,
-            "y": 2.954068221435557,
-            "heading": -0.3758421094017394,
-            "angularVelocity": 0.4918609642437705,
-            "velocityX": 2.40691803869303,
-            "velocityY": -2.666213602277847,
-            "timestamp": 2.186936607382319
-        },
-        {
-            "x": 7.385172224991548,
-            "y": 2.787906831681346,
-            "heading": -0.27501394829018777,
-            "angularVelocity": 1.3726370863672974,
-            "velocityX": 2.0174839820722426,
-            "velocityY": -2.262059363024933,
-            "timestamp": 2.26039241162435
-        },
-        {
-            "x": 7.503381996539121,
-            "y": 2.655285343615516,
-            "heading": -0.17500745818288596,
-            "angularVelocity": 1.3614511629031412,
-            "velocityX": 1.6092638664478731,
-            "velocityY": -1.8054596152654476,
-            "timestamp": 2.3338482158663814
-        },
-        {
-            "x": 7.59180643545552,
-            "y": 2.55603678143989,
-            "heading": -0.09217156397731928,
-            "angularVelocity": 1.1276970562139184,
-            "velocityX": 1.2037774254713123,
-            "velocityY": -1.3511330139223812,
-            "timestamp": 2.4073040201084126
-        },
-        {
-            "x": 7.650633785300877,
-            "y": 2.489982671853645,
-            "heading": -0.033538816068743524,
-            "angularVelocity": 0.7982044239197745,
-            "velocityX": 0.8008536623127058,
-            "velocityY": -0.8992360817206846,
-            "timestamp": 2.480759824350444
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -0.003,
-            "angularVelocity": 0.41574408426759185,
-            "velocityX": 0.3997807253237885,
-            "velocityY": -0.44901382802875656,
-            "timestamp": 2.554215628592475
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -0.003,
-            "angularVelocity": 5.454413556338505e-31,
-            "velocityX": 0,
-            "velocityY": -3.390988673475489e-31,
-            "timestamp": 2.6276714328345063
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.traj
deleted file mode 100644
index b2b4ba4b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.traj	
+++ /dev/null
@@ -1,472 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.285270702118571,
-            "y": 6.762383358598232,
-            "heading": 0.3634342922015001,
-            "angularVelocity": -0.5809173586087107,
-            "velocityX": -0.3220883435269421,
-            "velocityY": 0.11771892098839686,
-            "timestamp": 0.04573061452687351
-        },
-        {
-            "x": 2.2599794598619596,
-            "y": 6.767831656358603,
-            "heading": 0.2777726454574198,
-            "angularVelocity": -1.8731794363656886,
-            "velocityX": -0.5530483794778248,
-            "velocityY": 0.11913895793336499,
-            "timestamp": 0.09146122905374703
-        },
-        {
-            "x": 2.2450548028766546,
-            "y": 6.7710281276739375,
-            "heading": 0.1344358774143225,
-            "angularVelocity": -3.134372225827269,
-            "velocityX": -0.3263602980128451,
-            "velocityY": 0.06989784301807915,
-            "timestamp": 0.13719184358062053
-        },
-        {
-            "x": 2.2457261085510254,
-            "y": 6.7668328285217285,
-            "heading": 0,
-            "angularVelocity": -2.939734766416754,
-            "velocityX": 0.014679568191384731,
-            "velocityY": -0.09173940030374239,
-            "timestamp": 0.18292245810749405
-        },
-        {
-            "x": 2.262817902753147,
-            "y": 6.755151488221845,
-            "heading": -0.1222613161134929,
-            "angularVelocity": -2.5936618258096975,
-            "velocityX": 0.3625867573309899,
-            "velocityY": -0.24780893395554135,
-            "timestamp": 0.230060953533885
-        },
-        {
-            "x": 2.2962801062686125,
-            "y": 6.73609161362754,
-            "heading": -0.22784823217966704,
-            "angularVelocity": -2.2399297031255614,
-            "velocityX": 0.7098699950598514,
-            "velocityY": -0.40433778002244786,
-            "timestamp": 0.277199448960276
-        },
-        {
-            "x": 2.3460880772414248,
-            "y": 6.709619977067709,
-            "heading": -0.31657680000453947,
-            "angularVelocity": -1.882295288008833,
-            "velocityX": 1.056630478386546,
-            "velocityY": -0.561571520694102,
-            "timestamp": 0.32433794438666697
-        },
-        {
-            "x": 2.4122219538028076,
-            "y": 6.675695367304005,
-            "heading": -0.3884103898720453,
-            "angularVelocity": -1.5238838070197687,
-            "velocityX": 1.4029696103613034,
-            "velocityY": -0.7196795200363996,
-            "timestamp": 0.37147643981305795
-        },
-        {
-            "x": 2.494664207392629,
-            "y": 6.634268490634927,
-            "heading": -0.4434154648490763,
-            "angularVelocity": -1.1668822791122553,
-            "velocityX": 1.7489368899896385,
-            "velocityY": -0.8788332401012526,
-            "timestamp": 0.4186149352394489
-        },
-        {
-            "x": 2.593395417678489,
-            "y": 6.585276348843886,
-            "heading": -0.4817298536594349,
-            "angularVelocity": -0.8128046613242179,
-            "velocityX": 2.094492185055806,
-            "velocityY": -1.0393234096224127,
-            "timestamp": 0.4657534306658399
-        },
-        {
-            "x": 2.7083870607833584,
-            "y": 6.52862575501488,
-            "heading": -0.5035659675601283,
-            "angularVelocity": -0.46323315377732527,
-            "velocityX": 2.4394423721997165,
-            "velocityY": -1.2017904542040352,
-            "timestamp": 0.5128919260922309
-        },
-        {
-            "x": 2.839583375891639,
-            "y": 6.464143202111409,
-            "heading": -0.5093019910748294,
-            "angularVelocity": -0.12168448446963974,
-            "velocityX": 2.78320964471958,
-            "velocityY": -1.367938291626405,
-            "timestamp": 0.5600304215186218
-        },
-        {
-            "x": 2.9868119835174247,
-            "y": 6.391337595074747,
-            "heading": -0.5000000631264199,
-            "angularVelocity": 0.197331880546237,
-            "velocityX": 3.1233200443517304,
-            "velocityY": -1.5445042608591435,
-            "timestamp": 0.6071689169450127
-        },
-        {
-            "x": 3.140901786552832,
-            "y": 6.301587367083572,
-            "heading": -0.5000000197789056,
-            "angularVelocity": 9.19577807856047e-7,
-            "velocityX": 3.2688740198760886,
-            "velocityY": -1.903968872561393,
-            "timestamp": 0.6543074123714037
-        },
-        {
-            "x": 3.284848213195801,
-            "y": 6.196334362030029,
-            "heading": -0.5,
-            "angularVelocity": 4.1959136469938736e-7,
-            "velocityX": 3.0536915813881054,
-            "velocityY": -2.2328460868682716,
-            "timestamp": 0.7014459077977946
-        },
-        {
-            "x": 3.3993231490932048,
-            "y": 6.0978407623199296,
-            "heading": -0.5170802154644099,
-            "angularVelocity": -0.41193186552987154,
-            "velocityX": 2.7608477187370504,
-            "velocityY": -2.375418059313161,
-            "timestamp": 0.7429095985042958
-        },
-        {
-            "x": 3.501098994358481,
-            "y": 5.994630951270013,
-            "heading": -0.5506635755138305,
-            "angularVelocity": -0.8099462319284754,
-            "velocityX": 2.4545775721162104,
-            "velocityY": -2.489161222534419,
-            "timestamp": 0.7843732892107971
-        },
-        {
-            "x": 3.5900181651549583,
-            "y": 5.890084132174874,
-            "heading": -0.6049860857837608,
-            "angularVelocity": -1.3101224069596595,
-            "velocityX": 2.144506899445626,
-            "velocityY": -2.521406495990601,
-            "timestamp": 0.8258369799172983
-        },
-        {
-            "x": 3.6727603343537245,
-            "y": 5.794679716077001,
-            "heading": -0.692814707704561,
-            "angularVelocity": -2.1182056016767716,
-            "velocityX": 1.9955331469267976,
-            "velocityY": -2.3009147153167167,
-            "timestamp": 0.8673006706237996
-        },
-        {
-            "x": 3.757696306555582,
-            "y": 5.694133573084931,
-            "heading": -0.7686614093789431,
-            "angularVelocity": -1.8292318021376788,
-            "velocityX": 2.04844216119069,
-            "velocityY": -2.424920244168773,
-            "timestamp": 0.9087643613303008
-        },
-        {
-            "x": 3.844342447773648,
-            "y": 5.582483853612312,
-            "heading": -0.8164085841781574,
-            "angularVelocity": -1.1515418426489148,
-            "velocityX": 2.0896871393165974,
-            "velocityY": -2.6927105998111585,
-            "timestamp": 0.950228052036802
-        },
-        {
-            "x": 3.9316331213435283,
-            "y": 5.459771926422603,
-            "heading": -0.8357045871440653,
-            "angularVelocity": -0.4653710906373953,
-            "velocityX": 2.105231639596513,
-            "velocityY": -2.9595032448602216,
-            "timestamp": 0.9916917427433033
-        },
-        {
-            "x": 4.018219294626101,
-            "y": 5.3289825292048665,
-            "heading": -0.8357080754934182,
-            "angularVelocity": -0.00008413021836472818,
-            "velocityX": 2.088240863443359,
-            "velocityY": -3.1543115190486835,
-            "timestamp": 1.0331554334498045
-        },
-        {
-            "x": 4.10449796775504,
-            "y": 5.197989757794864,
-            "heading": -0.8357107471897811,
-            "angularVelocity": -0.00006443460091393523,
-            "velocityX": 2.0808247326516955,
-            "velocityY": -3.159216393376985,
-            "timestamp": 1.0746191241563057
-        },
-        {
-            "x": 4.190712810420311,
-            "y": 5.066954954654073,
-            "heading": -0.8357133876254744,
-            "angularVelocity": -0.00006368067213797694,
-            "velocityX": 2.0792853023028366,
-            "velocityY": -3.160230093078832,
-            "timestamp": 1.1160828148628068
-        },
-        {
-            "x": 4.274456024169922,
-            "y": 4.934326648712158,
-            "heading": -0.8357152799414105,
-            "angularVelocity": -0.000045637904003859675,
-            "velocityX": 2.0196758253473424,
-            "velocityY": -3.198661375339825,
-            "timestamp": 1.157546505569308
-        },
-        {
-            "x": 4.34828200363932,
-            "y": 4.789661343281582,
-            "heading": -0.8357152360129204,
-            "angularVelocity": 0.0000010231801667591938,
-            "velocityX": 1.7195509731200054,
-            "velocityY": -3.36953696405698,
-            "timestamp": 1.2004797949125405
-        },
-        {
-            "x": 4.435451827820083,
-            "y": 4.652622129190998,
-            "heading": -0.8357151829793803,
-            "angularVelocity": 0.0000012352545290785843,
-            "velocityX": 2.0303551280191665,
-            "velocityY": -3.1919104309715474,
-            "timestamp": 1.2434130842557731
-        },
-        {
-            "x": 4.535184717372419,
-            "y": 4.524436546290902,
-            "heading": -0.8357137971440172,
-            "angularVelocity": 0.000032278807056995315,
-            "velocityX": 2.322973410097133,
-            "velocityY": -2.985692101886486,
-            "timestamp": 1.2863463735990057
-        },
-        {
-            "x": 4.644325012525673,
-            "y": 4.4080959710874374,
-            "heading": -0.8258667739389303,
-            "angularVelocity": 0.22935636555504585,
-            "velocityX": 2.542090224690803,
-            "velocityY": -2.7097987827900663,
-            "timestamp": 1.3292796629422383
-        },
-        {
-            "x": 4.760978449232698,
-            "y": 4.3048079626404245,
-            "heading": -0.804942882637706,
-            "angularVelocity": 0.4873582159975465,
-            "velocityX": 2.717085937078615,
-            "velocityY": -2.405779059257989,
-            "timestamp": 1.372212952285471
-        },
-        {
-            "x": 4.883873293679825,
-            "y": 4.215276634825419,
-            "heading": -0.7738338273486849,
-            "angularVelocity": 0.7245905395302493,
-            "velocityX": 2.8624604899158337,
-            "velocityY": -2.085359151013129,
-            "timestamp": 1.4151462416287035
-        },
-        {
-            "x": 5.012023716462111,
-            "y": 4.13998910043764,
-            "heading": -0.7341562289641704,
-            "angularVelocity": 0.9241686111518187,
-            "velocityX": 2.9848731542039757,
-            "velocityY": -1.7535934362235386,
-            "timestamp": 1.458079530971936
-        },
-        {
-            "x": 5.144586044931236,
-            "y": 4.079308119917677,
-            "heading": -0.6878803412234658,
-            "angularVelocity": 1.0778556325081365,
-            "velocityX": 3.0876350379153963,
-            "velocityY": -1.4133783236324275,
-            "timestamp": 1.5010128203151687
-        },
-        {
-            "x": 5.2807921990430025,
-            "y": 4.033507276074697,
-            "heading": -0.6371968549741135,
-            "angularVelocity": 1.1805171936433834,
-            "velocityX": 3.1725068401551706,
-            "velocityY": -1.0667909341122943,
-            "timestamp": 1.5439461096584013
-        },
-        {
-            "x": 5.419841766333411,
-            "y": 4.00280308410274,
-            "heading": -0.5848216118259447,
-            "angularVelocity": 1.2199215096111542,
-            "velocityX": 3.2387354758394924,
-            "velocityY": -0.7151604836631573,
-            "timestamp": 1.5868793990016339
-        },
-        {
-            "x": 5.561074826834354,
-            "y": 3.9873115142730846,
-            "heading": -0.5330346380339317,
-            "angularVelocity": 1.2062195695745372,
-            "velocityX": 3.289593289064067,
-            "velocityY": -0.36082885953151356,
-            "timestamp": 1.6298126883448665
-        },
-        {
-            "x": 5.703982353210449,
-            "y": 3.987083911895752,
-            "heading": -0.4835776002687649,
-            "angularVelocity": 1.1519508176925284,
-            "velocityX": 3.328594863384743,
-            "velocityY": -0.005301303040469438,
-            "timestamp": 1.672745977688099
-        },
-        {
-            "x": 5.958820715702131,
-            "y": 3.94375176713671,
-            "heading": -0.42940135113015304,
-            "angularVelocity": 0.7375353070821884,
-            "velocityX": 3.4692746900164138,
-            "velocityY": -0.5899077030899651,
-            "timestamp": 1.7462017819301305
-        },
-        {
-            "x": 6.2169360873076736,
-            "y": 3.85673688488041,
-            "heading": -0.41197233440562925,
-            "angularVelocity": 0.2372721516614769,
-            "velocityX": 3.513886673339918,
-            "velocityY": -1.184588245329031,
-            "timestamp": 1.819657586172162
-        },
-        {
-            "x": 6.462551803453154,
-            "y": 3.726776847306083,
-            "heading": -0.411972258677301,
-            "angularVelocity": 0.0000010309372967585872,
-            "velocityX": 3.343721012654018,
-            "velocityY": -1.76922761809427,
-            "timestamp": 1.8931133904141935
-        },
-        {
-            "x": 6.683975982637168,
-            "y": 3.558883929748973,
-            "heading": -0.4119722206140506,
-            "angularVelocity": 5.181789344885153e-7,
-            "velocityX": 3.0143864255360504,
-            "velocityY": -2.285631738561,
-            "timestamp": 1.966569194656225
-        },
-        {
-            "x": 6.875523396649561,
-            "y": 3.357572509103632,
-            "heading": -0.4119721861737391,
-            "angularVelocity": 4.688575916806749e-7,
-            "velocityX": 2.6076552559586523,
-            "velocityY": -2.7405788109272424,
-            "timestamp": 2.0400249988982564
-        },
-        {
-            "x": 7.060174216266159,
-            "y": 3.149917085871916,
-            "heading": -0.4119721521055279,
-            "angularVelocity": 4.637919568273184e-7,
-            "velocityX": 2.513767584766965,
-            "velocityY": -2.826943702739948,
-            "timestamp": 2.1134808031402876
-        },
-        {
-            "x": 7.236976316543012,
-            "y": 2.954068221435557,
-            "heading": -0.3758421094017394,
-            "angularVelocity": 0.4918609642437705,
-            "velocityX": 2.40691803869303,
-            "velocityY": -2.666213602277847,
-            "timestamp": 2.186936607382319
-        },
-        {
-            "x": 7.385172224991548,
-            "y": 2.787906831681346,
-            "heading": -0.27501394829018777,
-            "angularVelocity": 1.3726370863672974,
-            "velocityX": 2.0174839820722426,
-            "velocityY": -2.262059363024933,
-            "timestamp": 2.26039241162435
-        },
-        {
-            "x": 7.503381996539121,
-            "y": 2.655285343615516,
-            "heading": -0.17500745818288596,
-            "angularVelocity": 1.3614511629031412,
-            "velocityX": 1.6092638664478731,
-            "velocityY": -1.8054596152654476,
-            "timestamp": 2.3338482158663814
-        },
-        {
-            "x": 7.59180643545552,
-            "y": 2.55603678143989,
-            "heading": -0.09217156397731928,
-            "angularVelocity": 1.1276970562139184,
-            "velocityX": 1.2037774254713123,
-            "velocityY": -1.3511330139223812,
-            "timestamp": 2.4073040201084126
-        },
-        {
-            "x": 7.650633785300877,
-            "y": 2.489982671853645,
-            "heading": -0.033538816068743524,
-            "angularVelocity": 0.7982044239197745,
-            "velocityX": 0.8008536623127058,
-            "velocityY": -0.8992360817206846,
-            "timestamp": 2.480759824350444
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -0.003,
-            "angularVelocity": 0.41574408426759185,
-            "velocityX": 0.3997807253237885,
-            "velocityY": -0.44901382802875656,
-            "timestamp": 2.554215628592475
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -0.003,
-            "angularVelocity": 5.454413556338505e-31,
-            "velocityX": 0,
-            "velocityY": -3.390988673475489e-31,
-            "timestamp": 2.6276714328345063
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.1.traj
deleted file mode 100644
index 95620590..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.1.traj	
+++ /dev/null
@@ -1,616 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 4.884595084783414e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 2.298507420370097,
-            "y": 6.738728491162364,
-            "heading": 0.38429180377773414,
-            "angularVelocity": -0.12150700609071861,
-            "velocityX": -0.031771662206367984,
-            "velocityY": -0.3889348314553929,
-            "timestamp": 0.04697832994094238
-        },
-        {
-            "x": 2.295523042319471,
-            "y": 6.702182630298916,
-            "heading": 0.3729896107436911,
-            "angularVelocity": -0.2405831166891495,
-            "velocityX": -0.06352669527371438,
-            "velocityY": -0.7779301841804652,
-            "timestamp": 0.09395665988188476
-        },
-        {
-            "x": 2.2910478858134478,
-            "y": 6.647358841243747,
-            "heading": 0.35624034482133005,
-            "angularVelocity": -0.3565317443897418,
-            "velocityX": -0.09526001694076108,
-            "velocityY": -1.1670016606398963,
-            "timestamp": 0.14093498982282715
-        },
-        {
-            "x": 2.285083332518943,
-            "y": 6.574252474793301,
-            "heading": 0.33424119989038353,
-            "angularVelocity": -0.46828282228427764,
-            "velocityX": -0.12696392787914923,
-            "velocityY": -1.556172101953168,
-            "timestamp": 0.18791331976376952
-        },
-        {
-            "x": 2.2776313636635503,
-            "y": 6.48285724279221,
-            "heading": 0.3072714314404645,
-            "angularVelocity": -0.5740895532860236,
-            "velocityX": -0.15862566559434335,
-            "velocityY": -1.9454763955207768,
-            "timestamp": 0.23489164970471188
-        },
-        {
-            "x": 2.268695072443392,
-            "y": 6.373164221156064,
-            "heading": 0.2757564747192681,
-            "angularVelocity": -0.6708402951065854,
-            "velocityX": -0.1902215602681591,
-            "velocityY": -2.334970650809517,
-            "timestamp": 0.28186997964565425
-        },
-        {
-            "x": 2.2582799597491072,
-            "y": 6.245160021300657,
-            "heading": 0.24041955506127732,
-            "angularVelocity": -0.752196165815466,
-            "velocityX": -0.22170036072754723,
-            "velocityY": -2.7247499009931837,
-            "timestamp": 0.3288483095865966
-        },
-        {
-            "x": 2.2463982479830333,
-            "y": 6.098824071496616,
-            "heading": 0.20274918029785605,
-            "angularVelocity": -0.8018670482917891,
-            "velocityX": -0.25291899011757896,
-            "velocityY": -3.114967049446051,
-            "timestamp": 0.375826639527539
-        },
-        {
-            "x": 2.2330953714575887,
-            "y": 5.934154199517091,
-            "heading": 0.1675471140087416,
-            "angularVelocity": -0.7493256216934009,
-            "velocityX": -0.28317048609790474,
-            "velocityY": -3.505230436810659,
-            "timestamp": 0.42280496946848134
-        },
-        {
-            "x": 2.218530566609672,
-            "y": 5.757035827571947,
-            "heading": 0.16754710135001605,
-            "angularVelocity": -2.6945882371217095e-7,
-            "velocityX": -0.3100324099691601,
-            "velocityY": -3.770214313020578,
-            "timestamp": 0.4697832994094237
-        },
-        {
-            "x": 2.203965673260931,
-            "y": 5.579917462904045,
-            "heading": 0.16754708869211127,
-            "angularVelocity": -2.694413527531555e-7,
-            "velocityX": -0.3100342938339967,
-            "velocityY": -3.7702141581142206,
-            "timestamp": 0.5167616293503661
-        },
-        {
-            "x": 2.1894007799094983,
-            "y": 5.402799098236364,
-            "heading": 0.16754707603420654,
-            "angularVelocity": -2.694413521606316e-7,
-            "velocityX": -0.31003429389130516,
-            "velocityY": -3.7702141581095083,
-            "timestamp": 0.5637399592913085
-        },
-        {
-            "x": 2.1748358865580655,
-            "y": 5.225680733568683,
-            "heading": 0.1675470633763018,
-            "angularVelocity": -2.6944135250249435e-7,
-            "velocityX": -0.3100342938913069,
-            "velocityY": -3.770214158109508,
-            "timestamp": 0.6107182892322509
-        },
-        {
-            "x": 2.160270993206632,
-            "y": 5.048562368901002,
-            "heading": 0.16754705071839698,
-            "angularVelocity": -2.694413536359467e-7,
-            "velocityX": -0.31003429389130693,
-            "velocityY": -3.7702141581095083,
-            "timestamp": 0.6576966191731933
-        },
-        {
-            "x": 2.1457060998551993,
-            "y": 4.871444004233322,
-            "heading": 0.1675470380604922,
-            "angularVelocity": -2.6944135247689087e-7,
-            "velocityX": -0.3100342938913068,
-            "velocityY": -3.7702141581095083,
-            "timestamp": 0.7046749491141358
-        },
-        {
-            "x": 2.131141206503766,
-            "y": 4.6943256395656405,
-            "heading": 0.16754702540258745,
-            "angularVelocity": -2.694413528693067e-7,
-            "velocityX": -0.31003429389130693,
-            "velocityY": -3.7702141581095083,
-            "timestamp": 0.7516532790550782
-        },
-        {
-            "x": 2.1165763131523327,
-            "y": 4.51720727489796,
-            "heading": 0.16754701274468267,
-            "angularVelocity": -2.694413529636153e-7,
-            "velocityX": -0.31003429389130693,
-            "velocityY": -3.7702141581095083,
-            "timestamp": 0.7986316089960206
-        },
-        {
-            "x": 2.1020114198009,
-            "y": 4.340088910230279,
-            "heading": 0.16754700008677786,
-            "angularVelocity": -2.694413530501801e-7,
-            "velocityX": -0.3100342938913069,
-            "velocityY": -3.770214158109508,
-            "timestamp": 0.845609938936963
-        },
-        {
-            "x": 2.0874465264494666,
-            "y": 4.162970545562599,
-            "heading": 0.16754698742887306,
-            "angularVelocity": -2.694413534833812e-7,
-            "velocityX": -0.31003429389130704,
-            "velocityY": -3.7702141581095083,
-            "timestamp": 0.8925882688779054
-        },
-        {
-            "x": 2.0728816330980333,
-            "y": 3.9858521808949186,
-            "heading": 0.16754697477096828,
-            "angularVelocity": -2.6944135303696276e-7,
-            "velocityX": -0.31003429389130693,
-            "velocityY": -3.770214158109508,
-            "timestamp": 0.9395665988188479
-        },
-        {
-            "x": 2.0583167397466005,
-            "y": 3.8087338162272375,
-            "heading": 0.16754696211306347,
-            "angularVelocity": -2.694413537071192e-7,
-            "velocityX": -0.31003429389130704,
-            "velocityY": -3.7702141581095083,
-            "timestamp": 0.9865449287597903
-        },
-        {
-            "x": 2.043751846395167,
-            "y": 3.631615451559557,
-            "heading": 0.1675469494551587,
-            "angularVelocity": -2.694413527332354e-7,
-            "velocityX": -0.31003429389130716,
-            "velocityY": -3.770214158109508,
-            "timestamp": 1.0335232587007326
-        },
-        {
-            "x": 2.0291869530437365,
-            "y": 3.4544970868918763,
-            "heading": 0.16754693679725388,
-            "angularVelocity": -2.694413532802898e-7,
-            "velocityX": -0.31003429389125564,
-            "velocityY": -3.7702141581095123,
-            "timestamp": 1.080501588641675
-        },
-        {
-            "x": 2.0146220597715567,
-            "y": 3.2773787222176782,
-            "heading": 0.1675469241393491,
-            "angularVelocity": -2.6944135307750017e-7,
-            "velocityX": -0.31003429220428774,
-            "velocityY": -3.7702141582482356,
-            "timestamp": 1.1274799185826172
-        },
-        {
-            "x": 2.0000597716588953,
-            "y": 3.1002601433334096,
-            "heading": 0.16754691148142445,
-            "angularVelocity": -2.6944177624221033e-7,
-            "velocityX": -0.30997883771037654,
-            "velocityY": -3.770218718011654,
-            "timestamp": 1.1744582485235595
-        },
-        {
-            "x": 2.001608158601577,
-            "y": 2.922550679334096,
-            "heading": 0.16754689857982888,
-            "angularVelocity": -2.7462865480192305e-7,
-            "velocityX": 0.032959599556405594,
-            "velocityY": -3.7827965409310345,
-            "timestamp": 1.2214365784645018
-        },
-        {
-            "x": 2.021524410839774,
-            "y": 2.7459539818451146,
-            "heading": 0.1675468848578902,
-            "angularVelocity": -2.9209081516381327e-7,
-            "velocityX": 0.4239455140962882,
-            "velocityY": -3.7591097365739743,
-            "timestamp": 1.2684149084054441
-        },
-        {
-            "x": 2.0595952756969864,
-            "y": 2.572363489269638,
-            "heading": 0.16754686955661854,
-            "angularVelocity": -3.257091448717022e-7,
-            "velocityX": 0.8103920446953395,
-            "velocityY": -3.6951184257444134,
-            "timestamp": 1.3153932383463864
-        },
-        {
-            "x": 2.1154126290237643,
-            "y": 2.4036404273247642,
-            "heading": 0.16754685154625998,
-            "angularVelocity": -3.8337587986161977e-7,
-            "velocityX": 1.1881510772508834,
-            "velocityY": -3.5915083008906312,
-            "timestamp": 1.3623715682873287
-        },
-        {
-            "x": 2.1883780268761943,
-            "y": 2.2415938375833533,
-            "heading": 0.1675468289010436,
-            "angularVelocity": -4.82035363826859e-7,
-            "velocityX": 1.5531713865554018,
-            "velocityY": -3.449390175111029,
-            "timestamp": 1.409349898228271
-        },
-        {
-            "x": 2.2777091497247044,
-            "y": 2.0879611790897656,
-            "heading": 0.16754679784052873,
-            "angularVelocity": -6.611668595012802e-7,
-            "velocityX": 1.9015389214731706,
-            "velocityY": -3.270287783467881,
-            "timestamp": 1.4563282281692134
-        },
-        {
-            "x": 2.382448196411133,
-            "y": 1.9443897008895872,
-            "heading": 0.16754674836915184,
-            "angularVelocity": -0.0000010530680204285256,
-            "velocityX": 2.2295183080815923,
-            "velocityY": -3.0561213729961265,
-            "timestamp": 1.5033065581101557
-        },
-        {
-            "x": 2.5069799221636964,
-            "y": 1.8074962230648766,
-            "heading": 0.1675467031876573,
-            "angularVelocity": -9.235757728412815e-7,
-            "velocityX": 2.545610454081449,
-            "velocityY": -2.798302730812182,
-            "timestamp": 1.55222673973217
-        },
-        {
-            "x": 2.645527049823341,
-            "y": 1.6848068830394283,
-            "heading": 0.16754667440242366,
-            "angularVelocity": -5.884122397185895e-7,
-            "velocityX": 2.8321057499364835,
-            "velocityY": -2.5079493975189484,
-            "timestamp": 1.6011469213541845
-        },
-        {
-            "x": 2.7964787325047507,
-            "y": 1.577748144471377,
-            "heading": 0.1675466534117406,
-            "angularVelocity": -4.2908023560369677e-7,
-            "velocityX": 3.0856729815058617,
-            "velocityY": -2.1884370625450487,
-            "timestamp": 1.6500671029761989
-        },
-        {
-            "x": 2.958079900807193,
-            "y": 1.4875647290195357,
-            "heading": 0.1675466366326211,
-            "angularVelocity": -3.429897224036844e-7,
-            "velocityX": 3.303364029820377,
-            "velocityY": -1.8434807979383812,
-            "timestamp": 1.6989872845982132
-        },
-        {
-            "x": 3.1284516660647324,
-            "y": 1.4153051353233332,
-            "heading": 0.16754662225394246,
-            "angularVelocity": -2.9392120351406106e-7,
-            "velocityX": 3.4826478481607013,
-            "velocityY": -1.4770916889581902,
-            "timestamp": 1.7479074662202276
-        },
-        {
-            "x": 3.3056131601804433,
-            "y": 1.3618094062641966,
-            "heading": 0.16754660920712522,
-            "angularVelocity": -2.666960100615425e-7,
-            "velocityX": 3.6214398279336546,
-            "velocityY": -1.0935308759169382,
-            "timestamp": 1.796827647842242
-        },
-        {
-            "x": 3.487504529935422,
-            "y": 1.3276990268102373,
-            "heading": 0.16754659675699374,
-            "angularVelocity": -2.544988811687778e-7,
-            "velocityX": 3.7181253978240782,
-            "velocityY": -0.6972660019440657,
-            "timestamp": 1.8457478294642564
-        },
-        {
-            "x": 3.671043389503124,
-            "y": 1.304003553111977,
-            "heading": 0.16754658441216208,
-            "angularVelocity": -2.5234639867690264e-7,
-            "velocityX": 3.751802497092686,
-            "velocityY": -0.48437010887133,
-            "timestamp": 1.8946680110862708
-        },
-        {
-            "x": 3.8545823032662914,
-            "y": 1.2803084992011935,
-            "heading": 0.16754657206733162,
-            "angularVelocity": -2.523463739841797e-7,
-            "velocityX": 3.751803604927206,
-            "velocityY": -0.4843615278018566,
-            "timestamp": 1.9435881927082852
-        },
-        {
-            "x": 4.038121217030901,
-            "y": 1.2566134453015836,
-            "heading": 0.16754655972250113,
-            "angularVelocity": -2.5234637491728997e-7,
-            "velocityX": 3.7518036049566934,
-            "velocityY": -0.48436152757345374,
-            "timestamp": 1.9925083743302996
-        },
-        {
-            "x": 4.221660130795511,
-            "y": 1.232918391401974,
-            "heading": 0.16754654737767066,
-            "angularVelocity": -2.5234637402611237e-7,
-            "velocityX": 3.7518036049566943,
-            "velocityY": -0.4843615275734478,
-            "timestamp": 2.041428555952314
-        },
-        {
-            "x": 4.4051990445601215,
-            "y": 1.2092233375023644,
-            "heading": 0.16754653503284023,
-            "angularVelocity": -2.5234637391390173e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.4843615275734475,
-            "timestamp": 2.090348737574329
-        },
-        {
-            "x": 4.588737958324731,
-            "y": 1.1855282836027548,
-            "heading": 0.16754652268800974,
-            "angularVelocity": -2.523463747404588e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.4843615275734478,
-            "timestamp": 2.1392689191963434
-        },
-        {
-            "x": 4.77227687208934,
-            "y": 1.1618332297031453,
-            "heading": 0.16754651034317924,
-            "angularVelocity": -2.523463747849071e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.48436152757344797,
-            "timestamp": 2.188189100818358
-        },
-        {
-            "x": 4.95581578585395,
-            "y": 1.1381381758035356,
-            "heading": 0.16754649799834875,
-            "angularVelocity": -2.52346374599026e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.4843615275734478,
-            "timestamp": 2.2371092824403727
-        },
-        {
-            "x": 5.1393546996185595,
-            "y": 1.114443121903926,
-            "heading": 0.1675464856535183,
-            "angularVelocity": -2.523463743979587e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.48436152757344764,
-            "timestamp": 2.2860294640623873
-        },
-        {
-            "x": 5.32289361338317,
-            "y": 1.0907480680043165,
-            "heading": 0.16754647330868783,
-            "angularVelocity": -2.523463747332428e-7,
-            "velocityX": 3.7518036049566943,
-            "velocityY": -0.48436152757344797,
-            "timestamp": 2.334949645684402
-        },
-        {
-            "x": 5.506432527147779,
-            "y": 1.0670530141047068,
-            "heading": 0.1675464609638573,
-            "angularVelocity": -2.5234637523589347e-7,
-            "velocityX": 3.7518036049566943,
-            "velocityY": -0.48436152757344764,
-            "timestamp": 2.3838698273064165
-        },
-        {
-            "x": 5.689971440912389,
-            "y": 1.043357960205097,
-            "heading": 0.16754644861902682,
-            "angularVelocity": -2.523463745889681e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.4843615275734477,
-            "timestamp": 2.432790008928431
-        },
-        {
-            "x": 5.8735103546769984,
-            "y": 1.0196629063054876,
-            "heading": 0.16754643627419633,
-            "angularVelocity": -2.5234637515727394e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.4843615275734478,
-            "timestamp": 2.4817101905504457
-        },
-        {
-            "x": 6.057049268441609,
-            "y": 0.995967852405878,
-            "heading": 0.16754642392936583,
-            "angularVelocity": -2.523463750187176e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.4843615275734476,
-            "timestamp": 2.5306303721724603
-        },
-        {
-            "x": 6.240588182206218,
-            "y": 0.9722727985062686,
-            "heading": 0.16754641158453532,
-            "angularVelocity": -2.523463753175626e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.48436152757344764,
-            "timestamp": 2.579550553794475
-        },
-        {
-            "x": 6.424127095970827,
-            "y": 0.948577744606659,
-            "heading": 0.1675463992397048,
-            "angularVelocity": -2.5234637555305e-7,
-            "velocityX": 3.7518036049566934,
-            "velocityY": -0.4843615275734476,
-            "timestamp": 2.6284707354164896
-        },
-        {
-            "x": 6.607666009735459,
-            "y": 0.9248826907072107,
-            "heading": 0.1675463868948743,
-            "angularVelocity": -2.5234637524939476e-7,
-            "velocityX": 3.7518036049571197,
-            "velocityY": -0.48436152757015266,
-            "timestamp": 2.677390917038504
-        },
-        {
-            "x": 6.79120492428193,
-            "y": 0.9011876428641385,
-            "heading": 0.16754637454996243,
-            "angularVelocity": -2.523480385625403e-7,
-            "velocityX": 3.7518036209390857,
-            "velocityY": -0.4843614037689742,
-            "timestamp": 2.726311098660519
-        },
-        {
-            "x": 6.9688913787459,
-            "y": 0.8784350451744124,
-            "heading": 0.1510406339197301,
-            "angularVelocity": -0.33740145851798203,
-            "velocityX": 3.6321707845828914,
-            "velocityY": -0.4650963454209109,
-            "timestamp": 2.7752312802825334
-        },
-        {
-            "x": 7.1269915101358645,
-            "y": 0.8581344466937117,
-            "heading": 0.12402265057850995,
-            "angularVelocity": -0.5522870611964742,
-            "velocityX": 3.2317977192222487,
-            "velocityY": -0.4149738984526803,
-            "timestamp": 2.824151461904548
-        },
-        {
-            "x": 7.265299167873697,
-            "y": 0.8403581253912503,
-            "heading": 0.0960275261585182,
-            "angularVelocity": -0.5722612527545016,
-            "velocityX": 2.8272106347943935,
-            "velocityY": -0.3633739841730647,
-            "timestamp": 2.8730716435265626
-        },
-        {
-            "x": 7.383819928892336,
-            "y": 0.8251162922066029,
-            "heading": 0.06979646830524168,
-            "angularVelocity": -0.5362011542792869,
-            "velocityX": 2.4227375510254454,
-            "velocityY": -0.3115653433671744,
-            "timestamp": 2.9219918251485772
-        },
-        {
-            "x": 7.482566860925546,
-            "y": 0.8124124441531555,
-            "heading": 0.04664460944238867,
-            "angularVelocity": -0.47325782724474763,
-            "velocityX": 2.01853159083065,
-            "velocityY": -0.2596852184974389,
-            "timestamp": 2.970912006770592
-        },
-        {
-            "x": 7.5615507925764724,
-            "y": 0.8022481812485672,
-            "heading": 0.027346506217350703,
-            "angularVelocity": -0.3944814304686405,
-            "velocityX": 1.6145469831081642,
-            "velocityY": -0.20777238692863387,
-            "timestamp": 3.0198321883926065
-        },
-        {
-            "x": 7.620780133649161,
-            "y": 0.7946243691967044,
-            "heading": 0.012413272398069258,
-            "angularVelocity": -0.3052571213791523,
-            "velocityX": 1.2107342840696753,
-            "velocityY": -0.15584185910773468,
-            "timestamp": 3.068752370014621
-        },
-        {
-            "x": 7.660261458521305,
-            "y": 0.7895415301241143,
-            "heading": 0.002207571288741933,
-            "angularVelocity": -0.2086194443876447,
-            "velocityX": 0.8070559749184711,
-            "velocityY": -0.10390065825721975,
-            "timestamp": 3.1176725516366357
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -0.003,
-            "angularVelocity": -0.10645036702804238,
-            "velocityX": 0.40348463199107626,
-            "velocityY": -0.05195258970523994,
-            "timestamp": 3.1665927332586503
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -0.003,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -1.0902826040652685e-32,
-            "timestamp": 3.215512914880665
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.traj
deleted file mode 100644
index 95620590..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.traj	
+++ /dev/null
@@ -1,616 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 4.884595084783414e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 2.298507420370097,
-            "y": 6.738728491162364,
-            "heading": 0.38429180377773414,
-            "angularVelocity": -0.12150700609071861,
-            "velocityX": -0.031771662206367984,
-            "velocityY": -0.3889348314553929,
-            "timestamp": 0.04697832994094238
-        },
-        {
-            "x": 2.295523042319471,
-            "y": 6.702182630298916,
-            "heading": 0.3729896107436911,
-            "angularVelocity": -0.2405831166891495,
-            "velocityX": -0.06352669527371438,
-            "velocityY": -0.7779301841804652,
-            "timestamp": 0.09395665988188476
-        },
-        {
-            "x": 2.2910478858134478,
-            "y": 6.647358841243747,
-            "heading": 0.35624034482133005,
-            "angularVelocity": -0.3565317443897418,
-            "velocityX": -0.09526001694076108,
-            "velocityY": -1.1670016606398963,
-            "timestamp": 0.14093498982282715
-        },
-        {
-            "x": 2.285083332518943,
-            "y": 6.574252474793301,
-            "heading": 0.33424119989038353,
-            "angularVelocity": -0.46828282228427764,
-            "velocityX": -0.12696392787914923,
-            "velocityY": -1.556172101953168,
-            "timestamp": 0.18791331976376952
-        },
-        {
-            "x": 2.2776313636635503,
-            "y": 6.48285724279221,
-            "heading": 0.3072714314404645,
-            "angularVelocity": -0.5740895532860236,
-            "velocityX": -0.15862566559434335,
-            "velocityY": -1.9454763955207768,
-            "timestamp": 0.23489164970471188
-        },
-        {
-            "x": 2.268695072443392,
-            "y": 6.373164221156064,
-            "heading": 0.2757564747192681,
-            "angularVelocity": -0.6708402951065854,
-            "velocityX": -0.1902215602681591,
-            "velocityY": -2.334970650809517,
-            "timestamp": 0.28186997964565425
-        },
-        {
-            "x": 2.2582799597491072,
-            "y": 6.245160021300657,
-            "heading": 0.24041955506127732,
-            "angularVelocity": -0.752196165815466,
-            "velocityX": -0.22170036072754723,
-            "velocityY": -2.7247499009931837,
-            "timestamp": 0.3288483095865966
-        },
-        {
-            "x": 2.2463982479830333,
-            "y": 6.098824071496616,
-            "heading": 0.20274918029785605,
-            "angularVelocity": -0.8018670482917891,
-            "velocityX": -0.25291899011757896,
-            "velocityY": -3.114967049446051,
-            "timestamp": 0.375826639527539
-        },
-        {
-            "x": 2.2330953714575887,
-            "y": 5.934154199517091,
-            "heading": 0.1675471140087416,
-            "angularVelocity": -0.7493256216934009,
-            "velocityX": -0.28317048609790474,
-            "velocityY": -3.505230436810659,
-            "timestamp": 0.42280496946848134
-        },
-        {
-            "x": 2.218530566609672,
-            "y": 5.757035827571947,
-            "heading": 0.16754710135001605,
-            "angularVelocity": -2.6945882371217095e-7,
-            "velocityX": -0.3100324099691601,
-            "velocityY": -3.770214313020578,
-            "timestamp": 0.4697832994094237
-        },
-        {
-            "x": 2.203965673260931,
-            "y": 5.579917462904045,
-            "heading": 0.16754708869211127,
-            "angularVelocity": -2.694413527531555e-7,
-            "velocityX": -0.3100342938339967,
-            "velocityY": -3.7702141581142206,
-            "timestamp": 0.5167616293503661
-        },
-        {
-            "x": 2.1894007799094983,
-            "y": 5.402799098236364,
-            "heading": 0.16754707603420654,
-            "angularVelocity": -2.694413521606316e-7,
-            "velocityX": -0.31003429389130516,
-            "velocityY": -3.7702141581095083,
-            "timestamp": 0.5637399592913085
-        },
-        {
-            "x": 2.1748358865580655,
-            "y": 5.225680733568683,
-            "heading": 0.1675470633763018,
-            "angularVelocity": -2.6944135250249435e-7,
-            "velocityX": -0.3100342938913069,
-            "velocityY": -3.770214158109508,
-            "timestamp": 0.6107182892322509
-        },
-        {
-            "x": 2.160270993206632,
-            "y": 5.048562368901002,
-            "heading": 0.16754705071839698,
-            "angularVelocity": -2.694413536359467e-7,
-            "velocityX": -0.31003429389130693,
-            "velocityY": -3.7702141581095083,
-            "timestamp": 0.6576966191731933
-        },
-        {
-            "x": 2.1457060998551993,
-            "y": 4.871444004233322,
-            "heading": 0.1675470380604922,
-            "angularVelocity": -2.6944135247689087e-7,
-            "velocityX": -0.3100342938913068,
-            "velocityY": -3.7702141581095083,
-            "timestamp": 0.7046749491141358
-        },
-        {
-            "x": 2.131141206503766,
-            "y": 4.6943256395656405,
-            "heading": 0.16754702540258745,
-            "angularVelocity": -2.694413528693067e-7,
-            "velocityX": -0.31003429389130693,
-            "velocityY": -3.7702141581095083,
-            "timestamp": 0.7516532790550782
-        },
-        {
-            "x": 2.1165763131523327,
-            "y": 4.51720727489796,
-            "heading": 0.16754701274468267,
-            "angularVelocity": -2.694413529636153e-7,
-            "velocityX": -0.31003429389130693,
-            "velocityY": -3.7702141581095083,
-            "timestamp": 0.7986316089960206
-        },
-        {
-            "x": 2.1020114198009,
-            "y": 4.340088910230279,
-            "heading": 0.16754700008677786,
-            "angularVelocity": -2.694413530501801e-7,
-            "velocityX": -0.3100342938913069,
-            "velocityY": -3.770214158109508,
-            "timestamp": 0.845609938936963
-        },
-        {
-            "x": 2.0874465264494666,
-            "y": 4.162970545562599,
-            "heading": 0.16754698742887306,
-            "angularVelocity": -2.694413534833812e-7,
-            "velocityX": -0.31003429389130704,
-            "velocityY": -3.7702141581095083,
-            "timestamp": 0.8925882688779054
-        },
-        {
-            "x": 2.0728816330980333,
-            "y": 3.9858521808949186,
-            "heading": 0.16754697477096828,
-            "angularVelocity": -2.6944135303696276e-7,
-            "velocityX": -0.31003429389130693,
-            "velocityY": -3.770214158109508,
-            "timestamp": 0.9395665988188479
-        },
-        {
-            "x": 2.0583167397466005,
-            "y": 3.8087338162272375,
-            "heading": 0.16754696211306347,
-            "angularVelocity": -2.694413537071192e-7,
-            "velocityX": -0.31003429389130704,
-            "velocityY": -3.7702141581095083,
-            "timestamp": 0.9865449287597903
-        },
-        {
-            "x": 2.043751846395167,
-            "y": 3.631615451559557,
-            "heading": 0.1675469494551587,
-            "angularVelocity": -2.694413527332354e-7,
-            "velocityX": -0.31003429389130716,
-            "velocityY": -3.770214158109508,
-            "timestamp": 1.0335232587007326
-        },
-        {
-            "x": 2.0291869530437365,
-            "y": 3.4544970868918763,
-            "heading": 0.16754693679725388,
-            "angularVelocity": -2.694413532802898e-7,
-            "velocityX": -0.31003429389125564,
-            "velocityY": -3.7702141581095123,
-            "timestamp": 1.080501588641675
-        },
-        {
-            "x": 2.0146220597715567,
-            "y": 3.2773787222176782,
-            "heading": 0.1675469241393491,
-            "angularVelocity": -2.6944135307750017e-7,
-            "velocityX": -0.31003429220428774,
-            "velocityY": -3.7702141582482356,
-            "timestamp": 1.1274799185826172
-        },
-        {
-            "x": 2.0000597716588953,
-            "y": 3.1002601433334096,
-            "heading": 0.16754691148142445,
-            "angularVelocity": -2.6944177624221033e-7,
-            "velocityX": -0.30997883771037654,
-            "velocityY": -3.770218718011654,
-            "timestamp": 1.1744582485235595
-        },
-        {
-            "x": 2.001608158601577,
-            "y": 2.922550679334096,
-            "heading": 0.16754689857982888,
-            "angularVelocity": -2.7462865480192305e-7,
-            "velocityX": 0.032959599556405594,
-            "velocityY": -3.7827965409310345,
-            "timestamp": 1.2214365784645018
-        },
-        {
-            "x": 2.021524410839774,
-            "y": 2.7459539818451146,
-            "heading": 0.1675468848578902,
-            "angularVelocity": -2.9209081516381327e-7,
-            "velocityX": 0.4239455140962882,
-            "velocityY": -3.7591097365739743,
-            "timestamp": 1.2684149084054441
-        },
-        {
-            "x": 2.0595952756969864,
-            "y": 2.572363489269638,
-            "heading": 0.16754686955661854,
-            "angularVelocity": -3.257091448717022e-7,
-            "velocityX": 0.8103920446953395,
-            "velocityY": -3.6951184257444134,
-            "timestamp": 1.3153932383463864
-        },
-        {
-            "x": 2.1154126290237643,
-            "y": 2.4036404273247642,
-            "heading": 0.16754685154625998,
-            "angularVelocity": -3.8337587986161977e-7,
-            "velocityX": 1.1881510772508834,
-            "velocityY": -3.5915083008906312,
-            "timestamp": 1.3623715682873287
-        },
-        {
-            "x": 2.1883780268761943,
-            "y": 2.2415938375833533,
-            "heading": 0.1675468289010436,
-            "angularVelocity": -4.82035363826859e-7,
-            "velocityX": 1.5531713865554018,
-            "velocityY": -3.449390175111029,
-            "timestamp": 1.409349898228271
-        },
-        {
-            "x": 2.2777091497247044,
-            "y": 2.0879611790897656,
-            "heading": 0.16754679784052873,
-            "angularVelocity": -6.611668595012802e-7,
-            "velocityX": 1.9015389214731706,
-            "velocityY": -3.270287783467881,
-            "timestamp": 1.4563282281692134
-        },
-        {
-            "x": 2.382448196411133,
-            "y": 1.9443897008895872,
-            "heading": 0.16754674836915184,
-            "angularVelocity": -0.0000010530680204285256,
-            "velocityX": 2.2295183080815923,
-            "velocityY": -3.0561213729961265,
-            "timestamp": 1.5033065581101557
-        },
-        {
-            "x": 2.5069799221636964,
-            "y": 1.8074962230648766,
-            "heading": 0.1675467031876573,
-            "angularVelocity": -9.235757728412815e-7,
-            "velocityX": 2.545610454081449,
-            "velocityY": -2.798302730812182,
-            "timestamp": 1.55222673973217
-        },
-        {
-            "x": 2.645527049823341,
-            "y": 1.6848068830394283,
-            "heading": 0.16754667440242366,
-            "angularVelocity": -5.884122397185895e-7,
-            "velocityX": 2.8321057499364835,
-            "velocityY": -2.5079493975189484,
-            "timestamp": 1.6011469213541845
-        },
-        {
-            "x": 2.7964787325047507,
-            "y": 1.577748144471377,
-            "heading": 0.1675466534117406,
-            "angularVelocity": -4.2908023560369677e-7,
-            "velocityX": 3.0856729815058617,
-            "velocityY": -2.1884370625450487,
-            "timestamp": 1.6500671029761989
-        },
-        {
-            "x": 2.958079900807193,
-            "y": 1.4875647290195357,
-            "heading": 0.1675466366326211,
-            "angularVelocity": -3.429897224036844e-7,
-            "velocityX": 3.303364029820377,
-            "velocityY": -1.8434807979383812,
-            "timestamp": 1.6989872845982132
-        },
-        {
-            "x": 3.1284516660647324,
-            "y": 1.4153051353233332,
-            "heading": 0.16754662225394246,
-            "angularVelocity": -2.9392120351406106e-7,
-            "velocityX": 3.4826478481607013,
-            "velocityY": -1.4770916889581902,
-            "timestamp": 1.7479074662202276
-        },
-        {
-            "x": 3.3056131601804433,
-            "y": 1.3618094062641966,
-            "heading": 0.16754660920712522,
-            "angularVelocity": -2.666960100615425e-7,
-            "velocityX": 3.6214398279336546,
-            "velocityY": -1.0935308759169382,
-            "timestamp": 1.796827647842242
-        },
-        {
-            "x": 3.487504529935422,
-            "y": 1.3276990268102373,
-            "heading": 0.16754659675699374,
-            "angularVelocity": -2.544988811687778e-7,
-            "velocityX": 3.7181253978240782,
-            "velocityY": -0.6972660019440657,
-            "timestamp": 1.8457478294642564
-        },
-        {
-            "x": 3.671043389503124,
-            "y": 1.304003553111977,
-            "heading": 0.16754658441216208,
-            "angularVelocity": -2.5234639867690264e-7,
-            "velocityX": 3.751802497092686,
-            "velocityY": -0.48437010887133,
-            "timestamp": 1.8946680110862708
-        },
-        {
-            "x": 3.8545823032662914,
-            "y": 1.2803084992011935,
-            "heading": 0.16754657206733162,
-            "angularVelocity": -2.523463739841797e-7,
-            "velocityX": 3.751803604927206,
-            "velocityY": -0.4843615278018566,
-            "timestamp": 1.9435881927082852
-        },
-        {
-            "x": 4.038121217030901,
-            "y": 1.2566134453015836,
-            "heading": 0.16754655972250113,
-            "angularVelocity": -2.5234637491728997e-7,
-            "velocityX": 3.7518036049566934,
-            "velocityY": -0.48436152757345374,
-            "timestamp": 1.9925083743302996
-        },
-        {
-            "x": 4.221660130795511,
-            "y": 1.232918391401974,
-            "heading": 0.16754654737767066,
-            "angularVelocity": -2.5234637402611237e-7,
-            "velocityX": 3.7518036049566943,
-            "velocityY": -0.4843615275734478,
-            "timestamp": 2.041428555952314
-        },
-        {
-            "x": 4.4051990445601215,
-            "y": 1.2092233375023644,
-            "heading": 0.16754653503284023,
-            "angularVelocity": -2.5234637391390173e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.4843615275734475,
-            "timestamp": 2.090348737574329
-        },
-        {
-            "x": 4.588737958324731,
-            "y": 1.1855282836027548,
-            "heading": 0.16754652268800974,
-            "angularVelocity": -2.523463747404588e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.4843615275734478,
-            "timestamp": 2.1392689191963434
-        },
-        {
-            "x": 4.77227687208934,
-            "y": 1.1618332297031453,
-            "heading": 0.16754651034317924,
-            "angularVelocity": -2.523463747849071e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.48436152757344797,
-            "timestamp": 2.188189100818358
-        },
-        {
-            "x": 4.95581578585395,
-            "y": 1.1381381758035356,
-            "heading": 0.16754649799834875,
-            "angularVelocity": -2.52346374599026e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.4843615275734478,
-            "timestamp": 2.2371092824403727
-        },
-        {
-            "x": 5.1393546996185595,
-            "y": 1.114443121903926,
-            "heading": 0.1675464856535183,
-            "angularVelocity": -2.523463743979587e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.48436152757344764,
-            "timestamp": 2.2860294640623873
-        },
-        {
-            "x": 5.32289361338317,
-            "y": 1.0907480680043165,
-            "heading": 0.16754647330868783,
-            "angularVelocity": -2.523463747332428e-7,
-            "velocityX": 3.7518036049566943,
-            "velocityY": -0.48436152757344797,
-            "timestamp": 2.334949645684402
-        },
-        {
-            "x": 5.506432527147779,
-            "y": 1.0670530141047068,
-            "heading": 0.1675464609638573,
-            "angularVelocity": -2.5234637523589347e-7,
-            "velocityX": 3.7518036049566943,
-            "velocityY": -0.48436152757344764,
-            "timestamp": 2.3838698273064165
-        },
-        {
-            "x": 5.689971440912389,
-            "y": 1.043357960205097,
-            "heading": 0.16754644861902682,
-            "angularVelocity": -2.523463745889681e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.4843615275734477,
-            "timestamp": 2.432790008928431
-        },
-        {
-            "x": 5.8735103546769984,
-            "y": 1.0196629063054876,
-            "heading": 0.16754643627419633,
-            "angularVelocity": -2.5234637515727394e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.4843615275734478,
-            "timestamp": 2.4817101905504457
-        },
-        {
-            "x": 6.057049268441609,
-            "y": 0.995967852405878,
-            "heading": 0.16754642392936583,
-            "angularVelocity": -2.523463750187176e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.4843615275734476,
-            "timestamp": 2.5306303721724603
-        },
-        {
-            "x": 6.240588182206218,
-            "y": 0.9722727985062686,
-            "heading": 0.16754641158453532,
-            "angularVelocity": -2.523463753175626e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.48436152757344764,
-            "timestamp": 2.579550553794475
-        },
-        {
-            "x": 6.424127095970827,
-            "y": 0.948577744606659,
-            "heading": 0.1675463992397048,
-            "angularVelocity": -2.5234637555305e-7,
-            "velocityX": 3.7518036049566934,
-            "velocityY": -0.4843615275734476,
-            "timestamp": 2.6284707354164896
-        },
-        {
-            "x": 6.607666009735459,
-            "y": 0.9248826907072107,
-            "heading": 0.1675463868948743,
-            "angularVelocity": -2.5234637524939476e-7,
-            "velocityX": 3.7518036049571197,
-            "velocityY": -0.48436152757015266,
-            "timestamp": 2.677390917038504
-        },
-        {
-            "x": 6.79120492428193,
-            "y": 0.9011876428641385,
-            "heading": 0.16754637454996243,
-            "angularVelocity": -2.523480385625403e-7,
-            "velocityX": 3.7518036209390857,
-            "velocityY": -0.4843614037689742,
-            "timestamp": 2.726311098660519
-        },
-        {
-            "x": 6.9688913787459,
-            "y": 0.8784350451744124,
-            "heading": 0.1510406339197301,
-            "angularVelocity": -0.33740145851798203,
-            "velocityX": 3.6321707845828914,
-            "velocityY": -0.4650963454209109,
-            "timestamp": 2.7752312802825334
-        },
-        {
-            "x": 7.1269915101358645,
-            "y": 0.8581344466937117,
-            "heading": 0.12402265057850995,
-            "angularVelocity": -0.5522870611964742,
-            "velocityX": 3.2317977192222487,
-            "velocityY": -0.4149738984526803,
-            "timestamp": 2.824151461904548
-        },
-        {
-            "x": 7.265299167873697,
-            "y": 0.8403581253912503,
-            "heading": 0.0960275261585182,
-            "angularVelocity": -0.5722612527545016,
-            "velocityX": 2.8272106347943935,
-            "velocityY": -0.3633739841730647,
-            "timestamp": 2.8730716435265626
-        },
-        {
-            "x": 7.383819928892336,
-            "y": 0.8251162922066029,
-            "heading": 0.06979646830524168,
-            "angularVelocity": -0.5362011542792869,
-            "velocityX": 2.4227375510254454,
-            "velocityY": -0.3115653433671744,
-            "timestamp": 2.9219918251485772
-        },
-        {
-            "x": 7.482566860925546,
-            "y": 0.8124124441531555,
-            "heading": 0.04664460944238867,
-            "angularVelocity": -0.47325782724474763,
-            "velocityX": 2.01853159083065,
-            "velocityY": -0.2596852184974389,
-            "timestamp": 2.970912006770592
-        },
-        {
-            "x": 7.5615507925764724,
-            "y": 0.8022481812485672,
-            "heading": 0.027346506217350703,
-            "angularVelocity": -0.3944814304686405,
-            "velocityX": 1.6145469831081642,
-            "velocityY": -0.20777238692863387,
-            "timestamp": 3.0198321883926065
-        },
-        {
-            "x": 7.620780133649161,
-            "y": 0.7946243691967044,
-            "heading": 0.012413272398069258,
-            "angularVelocity": -0.3052571213791523,
-            "velocityX": 1.2107342840696753,
-            "velocityY": -0.15584185910773468,
-            "timestamp": 3.068752370014621
-        },
-        {
-            "x": 7.660261458521305,
-            "y": 0.7895415301241143,
-            "heading": 0.002207571288741933,
-            "angularVelocity": -0.2086194443876447,
-            "velocityX": 0.8070559749184711,
-            "velocityY": -0.10390065825721975,
-            "timestamp": 3.1176725516366357
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -0.003,
-            "angularVelocity": -0.10645036702804238,
-            "velocityX": 0.40348463199107626,
-            "velocityY": -0.05195258970523994,
-            "timestamp": 3.1665927332586503
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -0.003,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -1.0902826040652685e-32,
-            "timestamp": 3.215512914880665
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.1.traj
deleted file mode 100644
index 5360be44..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3161416084367663,
-            "y": 6.743433917099698,
-            "heading": 0.3883998707887416,
-            "angularVelocity": -0.031816354442463515,
-            "velocityX": 0.32095354030299916,
-            "velocityY": -0.26974278009236136,
-            "timestamp": 0.05029266360959302
-        },
-        {
-            "x": 2.3488347297971495,
-            "y": 6.716804087923855,
-            "heading": 0.385297889956982,
-            "angularVelocity": -0.06167859503006947,
-            "velocityX": 0.6500574639309191,
-            "velocityY": -0.5294972917434406,
-            "timestamp": 0.10058532721918605
-        },
-        {
-            "x": 2.398581891811533,
-            "y": 6.677772540045158,
-            "heading": 0.38082205724745694,
-            "angularVelocity": -0.08899573791258333,
-            "velocityX": 0.9891534558709463,
-            "velocityY": -0.7760883015003333,
-            "timestamp": 0.15087799082877906
-        },
-        {
-            "x": 2.4660071913042128,
-            "y": 6.627244851321165,
-            "heading": 0.3751449326788243,
-            "angularVelocity": -0.11288176368431184,
-            "velocityX": 1.3406587492776654,
-            "velocityY": -1.004673149074478,
-            "timestamp": 0.2011706544383721
-        },
-        {
-            "x": 2.551890499441383,
-            "y": 6.566518101686868,
-            "heading": 0.3685091874361843,
-            "angularVelocity": -0.13194260884949985,
-            "velocityX": 1.7076707013145411,
-            "velocityY": -1.207467357579225,
-            "timestamp": 0.2514633180479651
-        },
-        {
-            "x": 2.6571889825280843,
-            "y": 6.497556440570772,
-            "heading": 0.3612736159712354,
-            "angularVelocity": -0.14386932299145247,
-            "velocityX": 2.093714580402863,
-            "velocityY": -1.3712071735039586,
-            "timestamp": 0.3017559816575581
-        },
-        {
-            "x": 2.7829575448660773,
-            "y": 6.423518691483174,
-            "heading": 0.3539965965907385,
-            "angularVelocity": -0.144693457419283,
-            "velocityX": 2.50073377131696,
-            "velocityY": -1.4721381564184353,
-            "timestamp": 0.35204864526715113
-        },
-        {
-            "x": 2.929814044412336,
-            "y": 6.349664492887633,
-            "heading": 0.3475640372215199,
-            "angularVelocity": -0.1279025390095239,
-            "velocityX": 2.9200382124570217,
-            "velocityY": -1.468488508957291,
-            "timestamp": 0.40234130887674413
-        },
-        {
-            "x": 3.096125602722168,
-            "y": 6.283931255340576,
-            "heading": 0.3432166982946709,
-            "angularVelocity": -0.08644081690714242,
-            "velocityX": 3.3068751259797926,
-            "velocityY": -1.3070144396670602,
-            "timestamp": 0.45263397248633713
-        },
-        {
-            "x": 3.3513045644934745,
-            "y": 6.2237675636491945,
-            "heading": 0.3432161755192605,
-            "angularVelocity": -0.0000075431393477149555,
-            "velocityX": 3.681983552011348,
-            "velocityY": -0.8681033957434081,
-            "timestamp": 0.5219387257984235
-        },
-        {
-            "x": 3.6126701345327414,
-            "y": 6.203172404760708,
-            "heading": 0.34321617069844046,
-            "angularVelocity": -6.955973168022537e-8,
-            "velocityX": 3.771250275753975,
-            "velocityY": -0.2971680570846853,
-            "timestamp": 0.5912434791105099
-        },
-        {
-            "x": 3.8741231287698,
-            "y": 6.222626349014288,
-            "heading": 0.3432161668255195,
-            "angularVelocity": -5.588247279499863e-8,
-            "velocityX": 3.7725117216666155,
-            "velocityY": 0.2807014428862732,
-            "timestamp": 0.6605482324225962
-        },
-        {
-            "x": 4.129562675072727,
-            "y": 6.28167517120369,
-            "heading": 0.34321616334811994,
-            "angularVelocity": -5.017548406400545e-8,
-            "velocityX": 3.68574353266443,
-            "velocityY": 0.8520169161197405,
-            "timestamp": 0.7298529857346825
-        },
-        {
-            "x": 4.378319272574482,
-            "y": 6.3644778320450675,
-            "heading": 0.34321615995103527,
-            "angularVelocity": -4.901661807423297e-8,
-            "velocityX": 3.5893150990896703,
-            "velocityY": 1.1947616416511535,
-            "timestamp": 0.7991577390467688
-        },
-        {
-            "x": 4.606228025075672,
-            "y": 6.4403413045373235,
-            "heading": 0.31944011002240325,
-            "angularVelocity": -0.34306521259177225,
-            "velocityX": 3.2885010278428317,
-            "velocityY": 1.094635921300162,
-            "timestamp": 0.8684624923588551
-        },
-        {
-            "x": 4.796152006740457,
-            "y": 6.503560868780938,
-            "heading": 0.2996267220530126,
-            "angularVelocity": -0.2858878651536105,
-            "velocityX": 2.74041782977768,
-            "velocityY": 0.9121966563957079,
-            "timestamp": 0.9377672456709414
-        },
-        {
-            "x": 4.9480911999499915,
-            "y": 6.554136520865842,
-            "heading": 0.2837760268788632,
-            "angularVelocity": -0.22871007278204386,
-            "velocityX": 2.1923343774897495,
-            "velocityY": 0.7297573350727579,
-            "timestamp": 1.0070719989830277
-        },
-        {
-            "x": 5.062045598488017,
-            "y": 6.592068260177227,
-            "heading": 0.27188801539640783,
-            "angularVelocity": -0.1715324117657857,
-            "velocityX": 1.644250835507307,
-            "velocityY": 0.5473180048787404,
-            "timestamp": 1.076376752295114
-        },
-        {
-            "x": 5.138015199132455,
-            "y": 6.617356086604739,
-            "heading": 0.26396267467345363,
-            "angularVelocity": -0.11435493734845723,
-            "velocityX": 1.0961672470333998,
-            "velocityY": 0.364878673092444,
-            "timestamp": 1.1456815056072003
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": -0.05717753089184424,
-            "velocityX": 0.548083631385222,
-            "velocityY": 0.1824393391651535,
-            "timestamp": 1.2149862589192866
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": -1.0031982410108139e-28,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 1.2842910122313729
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.traj
deleted file mode 100644
index 5360be44..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3161416084367663,
-            "y": 6.743433917099698,
-            "heading": 0.3883998707887416,
-            "angularVelocity": -0.031816354442463515,
-            "velocityX": 0.32095354030299916,
-            "velocityY": -0.26974278009236136,
-            "timestamp": 0.05029266360959302
-        },
-        {
-            "x": 2.3488347297971495,
-            "y": 6.716804087923855,
-            "heading": 0.385297889956982,
-            "angularVelocity": -0.06167859503006947,
-            "velocityX": 0.6500574639309191,
-            "velocityY": -0.5294972917434406,
-            "timestamp": 0.10058532721918605
-        },
-        {
-            "x": 2.398581891811533,
-            "y": 6.677772540045158,
-            "heading": 0.38082205724745694,
-            "angularVelocity": -0.08899573791258333,
-            "velocityX": 0.9891534558709463,
-            "velocityY": -0.7760883015003333,
-            "timestamp": 0.15087799082877906
-        },
-        {
-            "x": 2.4660071913042128,
-            "y": 6.627244851321165,
-            "heading": 0.3751449326788243,
-            "angularVelocity": -0.11288176368431184,
-            "velocityX": 1.3406587492776654,
-            "velocityY": -1.004673149074478,
-            "timestamp": 0.2011706544383721
-        },
-        {
-            "x": 2.551890499441383,
-            "y": 6.566518101686868,
-            "heading": 0.3685091874361843,
-            "angularVelocity": -0.13194260884949985,
-            "velocityX": 1.7076707013145411,
-            "velocityY": -1.207467357579225,
-            "timestamp": 0.2514633180479651
-        },
-        {
-            "x": 2.6571889825280843,
-            "y": 6.497556440570772,
-            "heading": 0.3612736159712354,
-            "angularVelocity": -0.14386932299145247,
-            "velocityX": 2.093714580402863,
-            "velocityY": -1.3712071735039586,
-            "timestamp": 0.3017559816575581
-        },
-        {
-            "x": 2.7829575448660773,
-            "y": 6.423518691483174,
-            "heading": 0.3539965965907385,
-            "angularVelocity": -0.144693457419283,
-            "velocityX": 2.50073377131696,
-            "velocityY": -1.4721381564184353,
-            "timestamp": 0.35204864526715113
-        },
-        {
-            "x": 2.929814044412336,
-            "y": 6.349664492887633,
-            "heading": 0.3475640372215199,
-            "angularVelocity": -0.1279025390095239,
-            "velocityX": 2.9200382124570217,
-            "velocityY": -1.468488508957291,
-            "timestamp": 0.40234130887674413
-        },
-        {
-            "x": 3.096125602722168,
-            "y": 6.283931255340576,
-            "heading": 0.3432166982946709,
-            "angularVelocity": -0.08644081690714242,
-            "velocityX": 3.3068751259797926,
-            "velocityY": -1.3070144396670602,
-            "timestamp": 0.45263397248633713
-        },
-        {
-            "x": 3.3513045644934745,
-            "y": 6.2237675636491945,
-            "heading": 0.3432161755192605,
-            "angularVelocity": -0.0000075431393477149555,
-            "velocityX": 3.681983552011348,
-            "velocityY": -0.8681033957434081,
-            "timestamp": 0.5219387257984235
-        },
-        {
-            "x": 3.6126701345327414,
-            "y": 6.203172404760708,
-            "heading": 0.34321617069844046,
-            "angularVelocity": -6.955973168022537e-8,
-            "velocityX": 3.771250275753975,
-            "velocityY": -0.2971680570846853,
-            "timestamp": 0.5912434791105099
-        },
-        {
-            "x": 3.8741231287698,
-            "y": 6.222626349014288,
-            "heading": 0.3432161668255195,
-            "angularVelocity": -5.588247279499863e-8,
-            "velocityX": 3.7725117216666155,
-            "velocityY": 0.2807014428862732,
-            "timestamp": 0.6605482324225962
-        },
-        {
-            "x": 4.129562675072727,
-            "y": 6.28167517120369,
-            "heading": 0.34321616334811994,
-            "angularVelocity": -5.017548406400545e-8,
-            "velocityX": 3.68574353266443,
-            "velocityY": 0.8520169161197405,
-            "timestamp": 0.7298529857346825
-        },
-        {
-            "x": 4.378319272574482,
-            "y": 6.3644778320450675,
-            "heading": 0.34321615995103527,
-            "angularVelocity": -4.901661807423297e-8,
-            "velocityX": 3.5893150990896703,
-            "velocityY": 1.1947616416511535,
-            "timestamp": 0.7991577390467688
-        },
-        {
-            "x": 4.606228025075672,
-            "y": 6.4403413045373235,
-            "heading": 0.31944011002240325,
-            "angularVelocity": -0.34306521259177225,
-            "velocityX": 3.2885010278428317,
-            "velocityY": 1.094635921300162,
-            "timestamp": 0.8684624923588551
-        },
-        {
-            "x": 4.796152006740457,
-            "y": 6.503560868780938,
-            "heading": 0.2996267220530126,
-            "angularVelocity": -0.2858878651536105,
-            "velocityX": 2.74041782977768,
-            "velocityY": 0.9121966563957079,
-            "timestamp": 0.9377672456709414
-        },
-        {
-            "x": 4.9480911999499915,
-            "y": 6.554136520865842,
-            "heading": 0.2837760268788632,
-            "angularVelocity": -0.22871007278204386,
-            "velocityX": 2.1923343774897495,
-            "velocityY": 0.7297573350727579,
-            "timestamp": 1.0070719989830277
-        },
-        {
-            "x": 5.062045598488017,
-            "y": 6.592068260177227,
-            "heading": 0.27188801539640783,
-            "angularVelocity": -0.1715324117657857,
-            "velocityX": 1.644250835507307,
-            "velocityY": 0.5473180048787404,
-            "timestamp": 1.076376752295114
-        },
-        {
-            "x": 5.138015199132455,
-            "y": 6.617356086604739,
-            "heading": 0.26396267467345363,
-            "angularVelocity": -0.11435493734845723,
-            "velocityX": 1.0961672470333998,
-            "velocityY": 0.364878673092444,
-            "timestamp": 1.1456815056072003
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": -0.05717753089184424,
-            "velocityX": 0.548083631385222,
-            "velocityY": 0.1824393391651535,
-            "timestamp": 1.2149862589192866
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": -1.0031982410108139e-28,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 1.2842910122313729
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.1.traj
deleted file mode 100644
index 08c314c4..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": -4.207802187241835e-33,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3149122846118377,
-            "y": 6.742667851221533,
-            "heading": 0.387869941582996,
-            "angularVelocity": -0.04275911471750884,
-            "velocityX": 0.2993514559631543,
-            "velocityY": -0.28770572153040486,
-            "timestamp": 0.049815306773293704
-        },
-        {
-            "x": 2.3455895487746456,
-            "y": 6.714948533605446,
-            "heading": 0.38350547033909727,
-            "angularVelocity": -0.08761305563705182,
-            "velocityX": 0.6158200390577908,
-            "velocityY": -0.5564417728518571,
-            "timestamp": 0.09963061354658741
-        },
-        {
-            "x": 2.393100480232861,
-            "y": 6.675215395028912,
-            "heading": 0.37677783190043923,
-            "angularVelocity": -0.13505163120415611,
-            "velocityX": 0.9537416215146959,
-            "velocityY": -0.7976090312433709,
-            "timestamp": 0.1494459203198811
-        },
-        {
-            "x": 2.4587594870185163,
-            "y": 6.62556801103228,
-            "heading": 0.3675326753009839,
-            "angularVelocity": -0.18558867140063587,
-            "velocityX": 1.318048829538744,
-            "velocityY": -0.996629092792259,
-            "timestamp": 0.19926122709317481
-        },
-        {
-            "x": 2.5440298105146963,
-            "y": 6.569365137451258,
-            "heading": 0.35560686650228135,
-            "angularVelocity": -0.23940048895004992,
-            "velocityX": 1.7117293663218704,
-            "velocityY": -1.1282249818675432,
-            "timestamp": 0.24907653386646852
-        },
-        {
-            "x": 2.649943674204089,
-            "y": 6.511985811495891,
-            "heading": 0.34090990459713355,
-            "angularVelocity": -0.2950290353938729,
-            "velocityX": 2.126130913363638,
-            "velocityY": -1.1518412647039136,
-            "timestamp": 0.2988918406397622
-        },
-        {
-            "x": 2.7755461673317816,
-            "y": 6.460929479776747,
-            "heading": 0.3236377937131317,
-            "angularVelocity": -0.3467229653455557,
-            "velocityX": 2.5213634375333727,
-            "velocityY": -1.0249125223997653,
-            "timestamp": 0.3487071474130559
-        },
-        {
-            "x": 2.916773796081543,
-            "y": 6.423426628112793,
-            "heading": 0.3044078287975214,
-            "angularVelocity": -0.38602522319421667,
-            "velocityX": 2.835024772455506,
-            "velocityY": -0.7528379145513634,
-            "timestamp": 0.39852245418634963
-        },
-        {
-            "x": 3.0358362362248608,
-            "y": 6.404912706911414,
-            "heading": 0.2883220118823199,
-            "angularVelocity": -0.4054270239814924,
-            "velocityX": 3.0008504404627585,
-            "velocityY": -0.46662497866622815,
-            "timestamp": 0.43819868680957713
-        },
-        {
-            "x": 3.161507467990405,
-            "y": 6.397735862008237,
-            "heading": 0.27120244348460076,
-            "angularVelocity": -0.43148170241679884,
-            "velocityX": 3.1674184633137656,
-            "velocityY": -0.1808852410792362,
-            "timestamp": 0.47787491943280463
-        },
-        {
-            "x": 3.294397924367021,
-            "y": 6.401341217255427,
-            "heading": 0.24704597239250262,
-            "angularVelocity": -0.6088398392423459,
-            "velocityX": 3.349371842799791,
-            "velocityY": 0.09086939481941644,
-            "timestamp": 0.5175511520560321
-        },
-        {
-            "x": 3.420816904565574,
-            "y": 6.393526992528847,
-            "heading": 0.22271106774313612,
-            "angularVelocity": -0.6133370796675788,
-            "velocityX": 3.186264719209789,
-            "velocityY": -0.19694976589104882,
-            "timestamp": 0.5572273846792596
-        },
-        {
-            "x": 3.540724707702168,
-            "y": 6.374315752768638,
-            "heading": 0.19854345438454005,
-            "angularVelocity": -0.6091206689927945,
-            "velocityX": 3.0221569743089773,
-            "velocityY": -0.48420019971772477,
-            "timestamp": 0.5969036173024871
-        },
-        {
-            "x": 3.654109001159668,
-            "y": 6.343714714050293,
-            "heading": 0.17465129572461457,
-            "angularVelocity": -0.602178107150174,
-            "velocityX": 2.857738398053468,
-            "velocityY": -0.7712687595352792,
-            "timestamp": 0.6365798499257146
-        },
-        {
-            "x": 3.8638905650676802,
-            "y": 6.228810110896502,
-            "heading": 0.12580249974502505,
-            "angularVelocity": -0.584756466458008,
-            "velocityX": 2.5112415481035373,
-            "velocityY": -1.375493671286808,
-            "timestamp": 0.7201168414454817
-        },
-        {
-            "x": 4.035438177373403,
-            "y": 6.070055732251625,
-            "heading": 0.08069123606907251,
-            "angularVelocity": -0.5400154213754852,
-            "velocityX": 2.0535526739088925,
-            "velocityY": -1.90040813963597,
-            "timestamp": 0.8036538329652487
-        },
-        {
-            "x": 4.153723645387517,
-            "y": 5.887986786487416,
-            "heading": 0.04380352514936875,
-            "angularVelocity": -0.4415733706546099,
-            "velocityX": 1.4159651414562335,
-            "velocityY": -2.179500870834227,
-            "timestamp": 0.8871908244850157
-        },
-        {
-            "x": 4.218422318430694,
-            "y": 5.7283456029173445,
-            "heading": 0.01885386609449773,
-            "angularVelocity": -0.29866599934929844,
-            "velocityX": 0.7744912985986951,
-            "velocityY": -1.9110238550103982,
-            "timestamp": 0.9707278160047828
-        },
-        {
-            "x": 4.250477583142556,
-            "y": 5.616790769692197,
-            "heading": 0.0038966042591740443,
-            "angularVelocity": -0.17904956311230244,
-            "velocityX": 0.38372539073628414,
-            "velocityY": -1.33539443060689,
-            "timestamp": 1.0542648075245498
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": -0.08255748900826321,
-            "velocityX": 0.14990265545387924,
-            "velocityY": -0.6798278063289014,
-            "timestamp": 1.1378017990443168
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": 2.6015085807950757e-30,
-            "velocityX": -2.4770702901017708e-31,
-            "velocityY": 1.2993067663186955e-31,
-            "timestamp": 1.2213387905640838
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.traj
deleted file mode 100644
index 08c314c4..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": -4.207802187241835e-33,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3149122846118377,
-            "y": 6.742667851221533,
-            "heading": 0.387869941582996,
-            "angularVelocity": -0.04275911471750884,
-            "velocityX": 0.2993514559631543,
-            "velocityY": -0.28770572153040486,
-            "timestamp": 0.049815306773293704
-        },
-        {
-            "x": 2.3455895487746456,
-            "y": 6.714948533605446,
-            "heading": 0.38350547033909727,
-            "angularVelocity": -0.08761305563705182,
-            "velocityX": 0.6158200390577908,
-            "velocityY": -0.5564417728518571,
-            "timestamp": 0.09963061354658741
-        },
-        {
-            "x": 2.393100480232861,
-            "y": 6.675215395028912,
-            "heading": 0.37677783190043923,
-            "angularVelocity": -0.13505163120415611,
-            "velocityX": 0.9537416215146959,
-            "velocityY": -0.7976090312433709,
-            "timestamp": 0.1494459203198811
-        },
-        {
-            "x": 2.4587594870185163,
-            "y": 6.62556801103228,
-            "heading": 0.3675326753009839,
-            "angularVelocity": -0.18558867140063587,
-            "velocityX": 1.318048829538744,
-            "velocityY": -0.996629092792259,
-            "timestamp": 0.19926122709317481
-        },
-        {
-            "x": 2.5440298105146963,
-            "y": 6.569365137451258,
-            "heading": 0.35560686650228135,
-            "angularVelocity": -0.23940048895004992,
-            "velocityX": 1.7117293663218704,
-            "velocityY": -1.1282249818675432,
-            "timestamp": 0.24907653386646852
-        },
-        {
-            "x": 2.649943674204089,
-            "y": 6.511985811495891,
-            "heading": 0.34090990459713355,
-            "angularVelocity": -0.2950290353938729,
-            "velocityX": 2.126130913363638,
-            "velocityY": -1.1518412647039136,
-            "timestamp": 0.2988918406397622
-        },
-        {
-            "x": 2.7755461673317816,
-            "y": 6.460929479776747,
-            "heading": 0.3236377937131317,
-            "angularVelocity": -0.3467229653455557,
-            "velocityX": 2.5213634375333727,
-            "velocityY": -1.0249125223997653,
-            "timestamp": 0.3487071474130559
-        },
-        {
-            "x": 2.916773796081543,
-            "y": 6.423426628112793,
-            "heading": 0.3044078287975214,
-            "angularVelocity": -0.38602522319421667,
-            "velocityX": 2.835024772455506,
-            "velocityY": -0.7528379145513634,
-            "timestamp": 0.39852245418634963
-        },
-        {
-            "x": 3.0358362362248608,
-            "y": 6.404912706911414,
-            "heading": 0.2883220118823199,
-            "angularVelocity": -0.4054270239814924,
-            "velocityX": 3.0008504404627585,
-            "velocityY": -0.46662497866622815,
-            "timestamp": 0.43819868680957713
-        },
-        {
-            "x": 3.161507467990405,
-            "y": 6.397735862008237,
-            "heading": 0.27120244348460076,
-            "angularVelocity": -0.43148170241679884,
-            "velocityX": 3.1674184633137656,
-            "velocityY": -0.1808852410792362,
-            "timestamp": 0.47787491943280463
-        },
-        {
-            "x": 3.294397924367021,
-            "y": 6.401341217255427,
-            "heading": 0.24704597239250262,
-            "angularVelocity": -0.6088398392423459,
-            "velocityX": 3.349371842799791,
-            "velocityY": 0.09086939481941644,
-            "timestamp": 0.5175511520560321
-        },
-        {
-            "x": 3.420816904565574,
-            "y": 6.393526992528847,
-            "heading": 0.22271106774313612,
-            "angularVelocity": -0.6133370796675788,
-            "velocityX": 3.186264719209789,
-            "velocityY": -0.19694976589104882,
-            "timestamp": 0.5572273846792596
-        },
-        {
-            "x": 3.540724707702168,
-            "y": 6.374315752768638,
-            "heading": 0.19854345438454005,
-            "angularVelocity": -0.6091206689927945,
-            "velocityX": 3.0221569743089773,
-            "velocityY": -0.48420019971772477,
-            "timestamp": 0.5969036173024871
-        },
-        {
-            "x": 3.654109001159668,
-            "y": 6.343714714050293,
-            "heading": 0.17465129572461457,
-            "angularVelocity": -0.602178107150174,
-            "velocityX": 2.857738398053468,
-            "velocityY": -0.7712687595352792,
-            "timestamp": 0.6365798499257146
-        },
-        {
-            "x": 3.8638905650676802,
-            "y": 6.228810110896502,
-            "heading": 0.12580249974502505,
-            "angularVelocity": -0.584756466458008,
-            "velocityX": 2.5112415481035373,
-            "velocityY": -1.375493671286808,
-            "timestamp": 0.7201168414454817
-        },
-        {
-            "x": 4.035438177373403,
-            "y": 6.070055732251625,
-            "heading": 0.08069123606907251,
-            "angularVelocity": -0.5400154213754852,
-            "velocityX": 2.0535526739088925,
-            "velocityY": -1.90040813963597,
-            "timestamp": 0.8036538329652487
-        },
-        {
-            "x": 4.153723645387517,
-            "y": 5.887986786487416,
-            "heading": 0.04380352514936875,
-            "angularVelocity": -0.4415733706546099,
-            "velocityX": 1.4159651414562335,
-            "velocityY": -2.179500870834227,
-            "timestamp": 0.8871908244850157
-        },
-        {
-            "x": 4.218422318430694,
-            "y": 5.7283456029173445,
-            "heading": 0.01885386609449773,
-            "angularVelocity": -0.29866599934929844,
-            "velocityX": 0.7744912985986951,
-            "velocityY": -1.9110238550103982,
-            "timestamp": 0.9707278160047828
-        },
-        {
-            "x": 4.250477583142556,
-            "y": 5.616790769692197,
-            "heading": 0.0038966042591740443,
-            "angularVelocity": -0.17904956311230244,
-            "velocityX": 0.38372539073628414,
-            "velocityY": -1.33539443060689,
-            "timestamp": 1.0542648075245498
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": -0.08255748900826321,
-            "velocityX": 0.14990265545387924,
-            "velocityY": -0.6798278063289014,
-            "timestamp": 1.1378017990443168
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": 2.6015085807950757e-30,
-            "velocityX": -2.4770702901017708e-31,
-            "velocityY": 1.2993067663186955e-31,
-            "timestamp": 1.2213387905640838
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.1.traj
deleted file mode 100644
index 8360b86f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.1.traj	
+++ /dev/null
@@ -1,544 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.542786541644192e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2937082670823616,
-            "y": 6.738516922494428,
-            "heading": 0.3838170687349535,
-            "angularVelocity": -0.12752172771189255,
-            "velocityX": -0.1297657401618881,
-            "velocityY": -0.3812097977411177,
-            "timestamp": 0.04848531599949176
-        },
-        {
-            "x": 2.281123433600762,
-            "y": 6.70154498788335,
-            "heading": 0.37169259737448246,
-            "angularVelocity": -0.2500648105569625,
-            "velocityX": -0.25955968775719646,
-            "velocityY": -0.7625387985814647,
-            "timestamp": 0.09697063199898352
-        },
-        {
-            "x": 2.262243832879689,
-            "y": 6.646077048114969,
-            "heading": 0.3539392998096368,
-            "angularVelocity": -0.36615823159004923,
-            "velocityX": -0.3893880102034898,
-            "velocityY": -1.1440152265685524,
-            "timestamp": 0.14545594799847528
-        },
-        {
-            "x": 2.2370673940379584,
-            "y": 6.572104077537847,
-            "heading": 0.3309787534387302,
-            "angularVelocity": -0.4735567026240549,
-            "velocityX": -0.5192590441623445,
-            "velocityY": -1.5256778068215682,
-            "timestamp": 0.19394126399796704
-        },
-        {
-            "x": 2.2055915440305824,
-            "y": 6.479614437749851,
-            "heading": 0.303410444299779,
-            "angularVelocity": -0.5685908933466691,
-            "velocityX": -0.6491831466623577,
-            "velocityY": -1.9075804268039651,
-            "timestamp": 0.2424265799974588
-        },
-        {
-            "x": 2.167813255965874,
-            "y": 6.3685929704461826,
-            "heading": 0.2721534840825349,
-            "angularVelocity": -0.6446685882563702,
-            "velocityX": -0.7791696782014628,
-            "velocityY": -2.2897956838082623,
-            "timestamp": 0.29091189599695055
-        },
-        {
-            "x": 2.123730000007902,
-            "y": 6.239021112222755,
-            "heading": 0.23878941688886735,
-            "angularVelocity": -0.6881272505944181,
-            "velocityX": -0.909208387103519,
-            "velocityY": -2.6723938073299895,
-            "timestamp": 0.3393972119964423
-        },
-        {
-            "x": 2.0733472424162183,
-            "y": 6.090890823809695,
-            "heading": 0.2066566550746183,
-            "angularVelocity": -0.6627318220196932,
-            "velocityX": -1.0391343554894152,
-            "velocityY": -3.0551577391940747,
-            "timestamp": 0.38788252799593403
-        },
-        {
-            "x": 2.0168156590767947,
-            "y": 5.924489369921669,
-            "heading": 0.187194468928521,
-            "angularVelocity": -0.4014037187247154,
-            "velocityX": -1.1659526636929207,
-            "velocityY": -3.431996893448088,
-            "timestamp": 0.4363678439954258
-        },
-        {
-            "x": 1.9576664650342146,
-            "y": 5.750871436780747,
-            "heading": 0.18719445417707173,
-            "angularVelocity": -3.042457077435804e-7,
-            "velocityX": -1.2199403638687387,
-            "velocityY": -3.580835342863344,
-            "timestamp": 0.4848531599949175
-        },
-        {
-            "x": 1.8985172819179166,
-            "y": 5.577253499917319,
-            "heading": 0.18719443942575115,
-            "angularVelocity": -3.042430533078605e-7,
-            "velocityX": -1.2199401385163373,
-            "velocityY": -3.5808354196392784,
-            "timestamp": 0.5333384759944093
-        },
-        {
-            "x": 1.8393681174712941,
-            "y": 5.403635556693396,
-            "heading": 0.18719442467442912,
-            "angularVelocity": -3.0424308271607693e-7,
-            "velocityX": -1.219939753457998,
-            "velocityY": -3.5808355508232466,
-            "timestamp": 0.5818237919939011
-        },
-        {
-            "x": 1.780218973506269,
-            "y": 5.230017606491683,
-            "heading": 0.18719440992310651,
-            "angularVelocity": -3.0424309544377536e-7,
-            "velocityX": -1.2199393310291196,
-            "velocityY": -3.580835694738771,
-            "timestamp": 0.6303091079933929
-        },
-        {
-            "x": 1.7210698517072343,
-            "y": 5.056399648738334,
-            "heading": 0.1871943951717844,
-            "angularVelocity": -3.042430844451726e-7,
-            "velocityX": -1.2199388738599835,
-            "velocityY": -3.580835850489736,
-            "timestamp": 0.6787944239928847
-        },
-        {
-            "x": 1.661920753431811,
-            "y": 4.882781682970833,
-            "heading": 0.18719438042046438,
-            "angularVelocity": -3.042430423387178e-7,
-            "velocityX": -1.2199383886901667,
-            "velocityY": -3.580836015780049,
-            "timestamp": 0.7272797399923765
-        },
-        {
-            "x": 1.6027716795006517,
-            "y": 4.709163708909599,
-            "heading": 0.18719436566914785,
-            "angularVelocity": -3.042429697472901e-7,
-            "velocityX": -1.2199378865945683,
-            "velocityY": -3.5808361868366427,
-            "timestamp": 0.7757650559918683
-        },
-        {
-            "x": 1.5436226301529286,
-            "y": 4.5355457264731545,
-            "heading": 0.18719435091783626,
-            "angularVelocity": -3.042428676797686e-7,
-            "velocityX": -1.219937379566087,
-            "velocityY": -3.5808363595737087,
-            "timestamp": 0.8242503719913601
-        },
-        {
-            "x": 1.4844769425124074,
-            "y": 4.361926598741778,
-            "heading": 0.18719433616649686,
-            "angularVelocity": -3.0424344145116126e-7,
-            "velocityX": -1.2198680450801807,
-            "velocityY": -3.58085998103429,
-            "timestamp": 0.8727356879908519
-        },
-        {
-            "x": 1.4424355744034427,
-            "y": 4.183392744566331,
-            "heading": 0.18719432108286724,
-            "angularVelocity": -3.110968611299024e-7,
-            "velocityX": -0.8670948562865489,
-            "velocityY": -3.682225236552195,
-            "timestamp": 0.9212210039903437
-        },
-        {
-            "x": 1.4196862795998648,
-            "y": 4.0013919676043574,
-            "heading": 0.18719430492048,
-            "angularVelocity": -3.3334602243497606e-7,
-            "velocityX": -0.4691996810625877,
-            "velocityY": -3.753729829541186,
-            "timestamp": 0.9697063199898355
-        },
-        {
-            "x": 1.4164891295727136,
-            "y": 3.81800278977475,
-            "heading": 0.18719428668091828,
-            "angularVelocity": -3.761873328628986e-7,
-            "velocityX": -0.06594058347012394,
-            "velocityY": -3.7823653213178425,
-            "timestamp": 1.0181916359893273
-        },
-        {
-            "x": 1.432880701620732,
-            "y": 3.635319656493888,
-            "heading": 0.1871942648241357,
-            "angularVelocity": -4.507917942103899e-7,
-            "velocityX": 0.3380729136345315,
-            "velocityY": -3.767803292913241,
-            "timestamp": 1.066676951988819
-        },
-        {
-            "x": 1.4686738067578264,
-            "y": 3.455429018696199,
-            "heading": 0.18719405074105777,
-            "angularVelocity": -0.0000044154209051021175,
-            "velocityX": 0.7382256751280081,
-            "velocityY": -3.710208628927779,
-            "timestamp": 1.1151622679883109
-        },
-        {
-            "x": 1.5224561218642167,
-            "y": 3.2829543962052377,
-            "heading": 0.17869632281657946,
-            "angularVelocity": -0.17526394846200616,
-            "velocityX": 1.10924955315069,
-            "velocityY": -3.557254788081333,
-            "timestamp": 1.1636475839878027
-        },
-        {
-            "x": 1.5921416539328836,
-            "y": 3.121386413493124,
-            "heading": 0.1576086367549348,
-            "angularVelocity": -0.43492933122045885,
-            "velocityX": 1.4372502402541338,
-            "velocityY": -3.33230751169348,
-            "timestamp": 1.2121328999872945
-        },
-        {
-            "x": 1.6761645416915945,
-            "y": 2.9727322431612,
-            "heading": 0.12423651758368265,
-            "angularVelocity": -0.6882933210462135,
-            "velocityX": 1.7329553500274961,
-            "velocityY": -3.0659626995783453,
-            "timestamp": 1.2606182159867863
-        },
-        {
-            "x": 1.7731411061925635,
-            "y": 2.838565031466778,
-            "heading": 0.08175388007293943,
-            "angularVelocity": -0.8761959499529206,
-            "velocityX": 2.0001223566816617,
-            "velocityY": -2.767172058765521,
-            "timestamp": 1.309103531986278
-        },
-        {
-            "x": 1.8819216264400211,
-            "y": 2.719965378093697,
-            "heading": 0.033593056188374194,
-            "angularVelocity": -0.9933074146707891,
-            "velocityX": 2.2435765964482535,
-            "velocityY": -2.4460942643771193,
-            "timestamp": 1.3575888479857698
-        },
-        {
-            "x": 2.0016674106146493,
-            "y": 2.617587012881804,
-            "heading": -0.0178597683247242,
-            "angularVelocity": -1.0612042729434041,
-            "velocityX": 2.4697329842357973,
-            "velocityY": -2.1115334220519952,
-            "timestamp": 1.4060741639852616
-        },
-        {
-            "x": 2.131748914718628,
-            "y": 2.531857967376709,
-            "heading": -0.07087516069292028,
-            "angularVelocity": -1.0934319242038333,
-            "velocityX": 2.682905152268552,
-            "velocityY": -1.7681445142231829,
-            "timestamp": 1.4545594799847534
-        },
-        {
-            "x": 2.218821194191698,
-            "y": 2.484029060579093,
-            "heading": -0.10483282223316899,
-            "angularVelocity": -1.096901896110445,
-            "velocityX": 2.812612651242981,
-            "velocityY": -1.5449714785171498,
-            "timestamp": 1.4855172727096597
-        },
-        {
-            "x": 2.3097767814437278,
-            "y": 2.4431840829179357,
-            "heading": -0.13859592156415457,
-            "angularVelocity": -1.0906171389530375,
-            "velocityX": 2.938051432149048,
-            "velocityY": -1.3193762883647426,
-            "timestamp": 1.516475065434566
-        },
-        {
-            "x": 2.4044569077078632,
-            "y": 2.4094074987036276,
-            "heading": -0.1718069818405491,
-            "angularVelocity": -1.0727851488087659,
-            "velocityX": 3.0583616572737307,
-            "velocityY": -1.0910527282945428,
-            "timestamp": 1.5474328581594723
-        },
-        {
-            "x": 2.5026677503931767,
-            "y": 2.3827944816766236,
-            "heading": -0.20403633414457553,
-            "angularVelocity": -1.0410739741365005,
-            "velocityX": 3.1724110164298485,
-            "velocityY": -0.859654861819398,
-            "timestamp": 1.5783906508843786
-        },
-        {
-            "x": 2.6041682782784212,
-            "y": 2.363451815109364,
-            "heading": -0.23475758891885987,
-            "angularVelocity": -0.9923593405260972,
-            "velocityX": 3.2786745743372427,
-            "velocityY": -0.6248076773199155,
-            "timestamp": 1.6093484436092849
-        },
-        {
-            "x": 2.7086523080371263,
-            "y": 2.3514976196224144,
-            "heading": -0.26331031649970843,
-            "angularVelocity": -0.9223114784903548,
-            "velocityX": 3.3750477847899405,
-            "velocityY": -0.38614495527174003,
-            "timestamp": 1.6403062363341911
-        },
-        {
-            "x": 2.8157215031828033,
-            "y": 2.3470577985799963,
-            "heading": -0.288841887845313,
-            "angularVelocity": -0.8247219551529203,
-            "velocityX": 3.458553912306755,
-            "velocityY": -0.14341529714262294,
-            "timestamp": 1.6712640290590974
-        },
-        {
-            "x": 2.9248447015312338,
-            "y": 2.3502540561946077,
-            "heading": -0.3102182612745416,
-            "angularVelocity": -0.6905005668986338,
-            "velocityX": 3.524902415281217,
-            "velocityY": 0.10324565588123534,
-            "timestamp": 1.7022218217840037
-        },
-        {
-            "x": 3.0353304312162184,
-            "y": 2.361176073397281,
-            "heading": -0.326082506948615,
-            "angularVelocity": -0.5124475706430824,
-            "velocityX": 3.568914963243171,
-            "velocityY": 0.3528034863374462,
-            "timestamp": 1.73317961450891
-        },
-        {
-            "x": 3.1466093544989446,
-            "y": 2.379927120590619,
-            "heading": -0.3369287204650216,
-            "angularVelocity": -0.35035487229182166,
-            "velocityX": 3.594536738181179,
-            "velocityY": 0.605697161937562,
-            "timestamp": 1.7641374072338163
-        },
-        {
-            "x": 3.2578769593015027,
-            "y": 2.406586601459548,
-            "heading": -0.34366338052401463,
-            "angularVelocity": -0.21754328930658381,
-            "velocityX": 3.5941711281316278,
-            "velocityY": 0.8611557389062998,
-            "timestamp": 1.7950951999587226
-        },
-        {
-            "x": 3.3679824864036325,
-            "y": 2.4411213005139327,
-            "heading": -0.34790283328354016,
-            "angularVelocity": -0.13694299193573645,
-            "velocityX": 3.556633642474382,
-            "velocityY": 1.1155413876329803,
-            "timestamp": 1.8260529926836289
-        },
-        {
-            "x": 3.475304114496225,
-            "y": 2.4831460278092354,
-            "heading": -0.3524511595124565,
-            "angularVelocity": -0.14692023650373393,
-            "velocityX": 3.466708012625811,
-            "velocityY": 1.3574846136083376,
-            "timestamp": 1.8570107854085351
-        },
-        {
-            "x": 3.5778542240444473,
-            "y": 2.531447362567012,
-            "heading": -0.3616437420636369,
-            "angularVelocity": -0.29693921116020117,
-            "velocityX": 3.312578207999158,
-            "velocityY": 1.560231867531513,
-            "timestamp": 1.8879685781334414
-        },
-        {
-            "x": 3.673558919058804,
-            "y": 2.583451056106446,
-            "heading": -0.37745622232401294,
-            "angularVelocity": -0.5107754419475534,
-            "velocityX": 3.091457322713648,
-            "velocityY": 1.6798256258655577,
-            "timestamp": 1.9189263708583477
-        },
-        {
-            "x": 3.7613706261508923,
-            "y": 2.636186152260406,
-            "heading": -0.3968957996596081,
-            "angularVelocity": -0.6279380932955558,
-            "velocityX": 2.836497675157173,
-            "velocityY": 1.7034514257095354,
-            "timestamp": 1.949884163583254
-        },
-        {
-            "x": 3.841306735175595,
-            "y": 2.6876448087218088,
-            "heading": -0.4179459699473055,
-            "angularVelocity": -0.6799635386030257,
-            "velocityX": 2.582099755467769,
-            "velocityY": 1.6622198138851267,
-            "timestamp": 1.9808419563081603
-        },
-        {
-            "x": 3.913645075614716,
-            "y": 2.736629100397619,
-            "heading": -0.43934657697326235,
-            "angularVelocity": -0.6912833617387507,
-            "velocityX": 2.336676296079001,
-            "velocityY": 1.582292772328868,
-            "timestamp": 2.0117997490330666
-        },
-        {
-            "x": 3.9786691618750996,
-            "y": 2.7823974078733573,
-            "heading": -0.46027689817629036,
-            "angularVelocity": -0.6760921681319296,
-            "velocityX": 2.100410931692797,
-            "velocityY": 1.4784099074015198,
-            "timestamp": 2.0427575417579726
-        },
-        {
-            "x": 4.036618763930541,
-            "y": 2.824460782764613,
-            "heading": -0.48017689659950147,
-            "angularVelocity": -0.6428106357887643,
-            "velocityX": 1.8718906276836884,
-            "velocityY": 1.3587330099747585,
-            "timestamp": 2.0737153344828787
-        },
-        {
-            "x": 4.08768908254176,
-            "y": 2.862477844590781,
-            "heading": -0.49864492956893264,
-            "angularVelocity": -0.5965552238942878,
-            "velocityX": 1.6496757073449355,
-            "velocityY": 1.2280288250525657,
-            "timestamp": 2.1046731272077848
-        },
-        {
-            "x": 4.132038835388808,
-            "y": 2.8961988047734253,
-            "heading": -0.5153800748468587,
-            "angularVelocity": -0.5405794084679139,
-            "velocityX": 1.4325876925792385,
-            "velocityY": 1.0892559583398733,
-            "timestamp": 2.135630919932691
-        },
-        {
-            "x": 4.169798307734006,
-            "y": 2.9254338939938918,
-            "heading": -0.5301487101337384,
-            "angularVelocity": -0.47705711511170307,
-            "velocityX": 1.2197081581622198,
-            "velocityY": 0.944353154640467,
-            "timestamp": 2.166588712657597
-        },
-        {
-            "x": 4.201075770562755,
-            "y": 2.9500345091711524,
-            "heading": -0.5427642633485259,
-            "angularVelocity": -0.4075081620800149,
-            "velocityX": 1.0103259979397883,
-            "velocityY": 0.7946501675963605,
-            "timestamp": 2.197546505382503
-        },
-        {
-            "x": 4.225962326087245,
-            "y": 2.9698813733488647,
-            "heading": -0.5530743601373866,
-            "angularVelocity": -0.3330372058810695,
-            "velocityX": 0.8038866254348108,
-            "velocityY": 0.6410942909939014,
-            "timestamp": 2.228504298107409
-        },
-        {
-            "x": 4.24453552816763,
-            "y": 2.984876771487546,
-            "heading": -0.5609523103628415,
-            "angularVelocity": -0.2544738992160421,
-            "velocityX": 0.5999524011761236,
-            "velocityY": 0.48438201883216997,
-            "timestamp": 2.259462090832315
-        },
-        {
-            "x": 4.256862100646349,
-            "y": 2.9949392682905573,
-            "heading": -0.5662912533122896,
-            "angularVelocity": -0.17245877305093363,
-            "velocityX": 0.398173493448939,
-            "velocityY": 0.32503922008988784,
-            "timestamp": 2.290419883557221
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.569,
-            "angularVelocity": -0.08749805620543083,
-            "velocityX": 0.19826669841112912,
-            "velocityY": 0.16347198117160036,
-            "timestamp": 2.321377676282127
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.569,
-            "angularVelocity": 1.3974941712649426e-29,
-            "velocityX": 2.480011116993708e-30,
-            "velocityY": -4.1979337006948246e-30,
-            "timestamp": 2.3523354690070333
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.traj
deleted file mode 100644
index 8360b86f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.traj	
+++ /dev/null
@@ -1,544 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.542786541644192e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2937082670823616,
-            "y": 6.738516922494428,
-            "heading": 0.3838170687349535,
-            "angularVelocity": -0.12752172771189255,
-            "velocityX": -0.1297657401618881,
-            "velocityY": -0.3812097977411177,
-            "timestamp": 0.04848531599949176
-        },
-        {
-            "x": 2.281123433600762,
-            "y": 6.70154498788335,
-            "heading": 0.37169259737448246,
-            "angularVelocity": -0.2500648105569625,
-            "velocityX": -0.25955968775719646,
-            "velocityY": -0.7625387985814647,
-            "timestamp": 0.09697063199898352
-        },
-        {
-            "x": 2.262243832879689,
-            "y": 6.646077048114969,
-            "heading": 0.3539392998096368,
-            "angularVelocity": -0.36615823159004923,
-            "velocityX": -0.3893880102034898,
-            "velocityY": -1.1440152265685524,
-            "timestamp": 0.14545594799847528
-        },
-        {
-            "x": 2.2370673940379584,
-            "y": 6.572104077537847,
-            "heading": 0.3309787534387302,
-            "angularVelocity": -0.4735567026240549,
-            "velocityX": -0.5192590441623445,
-            "velocityY": -1.5256778068215682,
-            "timestamp": 0.19394126399796704
-        },
-        {
-            "x": 2.2055915440305824,
-            "y": 6.479614437749851,
-            "heading": 0.303410444299779,
-            "angularVelocity": -0.5685908933466691,
-            "velocityX": -0.6491831466623577,
-            "velocityY": -1.9075804268039651,
-            "timestamp": 0.2424265799974588
-        },
-        {
-            "x": 2.167813255965874,
-            "y": 6.3685929704461826,
-            "heading": 0.2721534840825349,
-            "angularVelocity": -0.6446685882563702,
-            "velocityX": -0.7791696782014628,
-            "velocityY": -2.2897956838082623,
-            "timestamp": 0.29091189599695055
-        },
-        {
-            "x": 2.123730000007902,
-            "y": 6.239021112222755,
-            "heading": 0.23878941688886735,
-            "angularVelocity": -0.6881272505944181,
-            "velocityX": -0.909208387103519,
-            "velocityY": -2.6723938073299895,
-            "timestamp": 0.3393972119964423
-        },
-        {
-            "x": 2.0733472424162183,
-            "y": 6.090890823809695,
-            "heading": 0.2066566550746183,
-            "angularVelocity": -0.6627318220196932,
-            "velocityX": -1.0391343554894152,
-            "velocityY": -3.0551577391940747,
-            "timestamp": 0.38788252799593403
-        },
-        {
-            "x": 2.0168156590767947,
-            "y": 5.924489369921669,
-            "heading": 0.187194468928521,
-            "angularVelocity": -0.4014037187247154,
-            "velocityX": -1.1659526636929207,
-            "velocityY": -3.431996893448088,
-            "timestamp": 0.4363678439954258
-        },
-        {
-            "x": 1.9576664650342146,
-            "y": 5.750871436780747,
-            "heading": 0.18719445417707173,
-            "angularVelocity": -3.042457077435804e-7,
-            "velocityX": -1.2199403638687387,
-            "velocityY": -3.580835342863344,
-            "timestamp": 0.4848531599949175
-        },
-        {
-            "x": 1.8985172819179166,
-            "y": 5.577253499917319,
-            "heading": 0.18719443942575115,
-            "angularVelocity": -3.042430533078605e-7,
-            "velocityX": -1.2199401385163373,
-            "velocityY": -3.5808354196392784,
-            "timestamp": 0.5333384759944093
-        },
-        {
-            "x": 1.8393681174712941,
-            "y": 5.403635556693396,
-            "heading": 0.18719442467442912,
-            "angularVelocity": -3.0424308271607693e-7,
-            "velocityX": -1.219939753457998,
-            "velocityY": -3.5808355508232466,
-            "timestamp": 0.5818237919939011
-        },
-        {
-            "x": 1.780218973506269,
-            "y": 5.230017606491683,
-            "heading": 0.18719440992310651,
-            "angularVelocity": -3.0424309544377536e-7,
-            "velocityX": -1.2199393310291196,
-            "velocityY": -3.580835694738771,
-            "timestamp": 0.6303091079933929
-        },
-        {
-            "x": 1.7210698517072343,
-            "y": 5.056399648738334,
-            "heading": 0.1871943951717844,
-            "angularVelocity": -3.042430844451726e-7,
-            "velocityX": -1.2199388738599835,
-            "velocityY": -3.580835850489736,
-            "timestamp": 0.6787944239928847
-        },
-        {
-            "x": 1.661920753431811,
-            "y": 4.882781682970833,
-            "heading": 0.18719438042046438,
-            "angularVelocity": -3.042430423387178e-7,
-            "velocityX": -1.2199383886901667,
-            "velocityY": -3.580836015780049,
-            "timestamp": 0.7272797399923765
-        },
-        {
-            "x": 1.6027716795006517,
-            "y": 4.709163708909599,
-            "heading": 0.18719436566914785,
-            "angularVelocity": -3.042429697472901e-7,
-            "velocityX": -1.2199378865945683,
-            "velocityY": -3.5808361868366427,
-            "timestamp": 0.7757650559918683
-        },
-        {
-            "x": 1.5436226301529286,
-            "y": 4.5355457264731545,
-            "heading": 0.18719435091783626,
-            "angularVelocity": -3.042428676797686e-7,
-            "velocityX": -1.219937379566087,
-            "velocityY": -3.5808363595737087,
-            "timestamp": 0.8242503719913601
-        },
-        {
-            "x": 1.4844769425124074,
-            "y": 4.361926598741778,
-            "heading": 0.18719433616649686,
-            "angularVelocity": -3.0424344145116126e-7,
-            "velocityX": -1.2198680450801807,
-            "velocityY": -3.58085998103429,
-            "timestamp": 0.8727356879908519
-        },
-        {
-            "x": 1.4424355744034427,
-            "y": 4.183392744566331,
-            "heading": 0.18719432108286724,
-            "angularVelocity": -3.110968611299024e-7,
-            "velocityX": -0.8670948562865489,
-            "velocityY": -3.682225236552195,
-            "timestamp": 0.9212210039903437
-        },
-        {
-            "x": 1.4196862795998648,
-            "y": 4.0013919676043574,
-            "heading": 0.18719430492048,
-            "angularVelocity": -3.3334602243497606e-7,
-            "velocityX": -0.4691996810625877,
-            "velocityY": -3.753729829541186,
-            "timestamp": 0.9697063199898355
-        },
-        {
-            "x": 1.4164891295727136,
-            "y": 3.81800278977475,
-            "heading": 0.18719428668091828,
-            "angularVelocity": -3.761873328628986e-7,
-            "velocityX": -0.06594058347012394,
-            "velocityY": -3.7823653213178425,
-            "timestamp": 1.0181916359893273
-        },
-        {
-            "x": 1.432880701620732,
-            "y": 3.635319656493888,
-            "heading": 0.1871942648241357,
-            "angularVelocity": -4.507917942103899e-7,
-            "velocityX": 0.3380729136345315,
-            "velocityY": -3.767803292913241,
-            "timestamp": 1.066676951988819
-        },
-        {
-            "x": 1.4686738067578264,
-            "y": 3.455429018696199,
-            "heading": 0.18719405074105777,
-            "angularVelocity": -0.0000044154209051021175,
-            "velocityX": 0.7382256751280081,
-            "velocityY": -3.710208628927779,
-            "timestamp": 1.1151622679883109
-        },
-        {
-            "x": 1.5224561218642167,
-            "y": 3.2829543962052377,
-            "heading": 0.17869632281657946,
-            "angularVelocity": -0.17526394846200616,
-            "velocityX": 1.10924955315069,
-            "velocityY": -3.557254788081333,
-            "timestamp": 1.1636475839878027
-        },
-        {
-            "x": 1.5921416539328836,
-            "y": 3.121386413493124,
-            "heading": 0.1576086367549348,
-            "angularVelocity": -0.43492933122045885,
-            "velocityX": 1.4372502402541338,
-            "velocityY": -3.33230751169348,
-            "timestamp": 1.2121328999872945
-        },
-        {
-            "x": 1.6761645416915945,
-            "y": 2.9727322431612,
-            "heading": 0.12423651758368265,
-            "angularVelocity": -0.6882933210462135,
-            "velocityX": 1.7329553500274961,
-            "velocityY": -3.0659626995783453,
-            "timestamp": 1.2606182159867863
-        },
-        {
-            "x": 1.7731411061925635,
-            "y": 2.838565031466778,
-            "heading": 0.08175388007293943,
-            "angularVelocity": -0.8761959499529206,
-            "velocityX": 2.0001223566816617,
-            "velocityY": -2.767172058765521,
-            "timestamp": 1.309103531986278
-        },
-        {
-            "x": 1.8819216264400211,
-            "y": 2.719965378093697,
-            "heading": 0.033593056188374194,
-            "angularVelocity": -0.9933074146707891,
-            "velocityX": 2.2435765964482535,
-            "velocityY": -2.4460942643771193,
-            "timestamp": 1.3575888479857698
-        },
-        {
-            "x": 2.0016674106146493,
-            "y": 2.617587012881804,
-            "heading": -0.0178597683247242,
-            "angularVelocity": -1.0612042729434041,
-            "velocityX": 2.4697329842357973,
-            "velocityY": -2.1115334220519952,
-            "timestamp": 1.4060741639852616
-        },
-        {
-            "x": 2.131748914718628,
-            "y": 2.531857967376709,
-            "heading": -0.07087516069292028,
-            "angularVelocity": -1.0934319242038333,
-            "velocityX": 2.682905152268552,
-            "velocityY": -1.7681445142231829,
-            "timestamp": 1.4545594799847534
-        },
-        {
-            "x": 2.218821194191698,
-            "y": 2.484029060579093,
-            "heading": -0.10483282223316899,
-            "angularVelocity": -1.096901896110445,
-            "velocityX": 2.812612651242981,
-            "velocityY": -1.5449714785171498,
-            "timestamp": 1.4855172727096597
-        },
-        {
-            "x": 2.3097767814437278,
-            "y": 2.4431840829179357,
-            "heading": -0.13859592156415457,
-            "angularVelocity": -1.0906171389530375,
-            "velocityX": 2.938051432149048,
-            "velocityY": -1.3193762883647426,
-            "timestamp": 1.516475065434566
-        },
-        {
-            "x": 2.4044569077078632,
-            "y": 2.4094074987036276,
-            "heading": -0.1718069818405491,
-            "angularVelocity": -1.0727851488087659,
-            "velocityX": 3.0583616572737307,
-            "velocityY": -1.0910527282945428,
-            "timestamp": 1.5474328581594723
-        },
-        {
-            "x": 2.5026677503931767,
-            "y": 2.3827944816766236,
-            "heading": -0.20403633414457553,
-            "angularVelocity": -1.0410739741365005,
-            "velocityX": 3.1724110164298485,
-            "velocityY": -0.859654861819398,
-            "timestamp": 1.5783906508843786
-        },
-        {
-            "x": 2.6041682782784212,
-            "y": 2.363451815109364,
-            "heading": -0.23475758891885987,
-            "angularVelocity": -0.9923593405260972,
-            "velocityX": 3.2786745743372427,
-            "velocityY": -0.6248076773199155,
-            "timestamp": 1.6093484436092849
-        },
-        {
-            "x": 2.7086523080371263,
-            "y": 2.3514976196224144,
-            "heading": -0.26331031649970843,
-            "angularVelocity": -0.9223114784903548,
-            "velocityX": 3.3750477847899405,
-            "velocityY": -0.38614495527174003,
-            "timestamp": 1.6403062363341911
-        },
-        {
-            "x": 2.8157215031828033,
-            "y": 2.3470577985799963,
-            "heading": -0.288841887845313,
-            "angularVelocity": -0.8247219551529203,
-            "velocityX": 3.458553912306755,
-            "velocityY": -0.14341529714262294,
-            "timestamp": 1.6712640290590974
-        },
-        {
-            "x": 2.9248447015312338,
-            "y": 2.3502540561946077,
-            "heading": -0.3102182612745416,
-            "angularVelocity": -0.6905005668986338,
-            "velocityX": 3.524902415281217,
-            "velocityY": 0.10324565588123534,
-            "timestamp": 1.7022218217840037
-        },
-        {
-            "x": 3.0353304312162184,
-            "y": 2.361176073397281,
-            "heading": -0.326082506948615,
-            "angularVelocity": -0.5124475706430824,
-            "velocityX": 3.568914963243171,
-            "velocityY": 0.3528034863374462,
-            "timestamp": 1.73317961450891
-        },
-        {
-            "x": 3.1466093544989446,
-            "y": 2.379927120590619,
-            "heading": -0.3369287204650216,
-            "angularVelocity": -0.35035487229182166,
-            "velocityX": 3.594536738181179,
-            "velocityY": 0.605697161937562,
-            "timestamp": 1.7641374072338163
-        },
-        {
-            "x": 3.2578769593015027,
-            "y": 2.406586601459548,
-            "heading": -0.34366338052401463,
-            "angularVelocity": -0.21754328930658381,
-            "velocityX": 3.5941711281316278,
-            "velocityY": 0.8611557389062998,
-            "timestamp": 1.7950951999587226
-        },
-        {
-            "x": 3.3679824864036325,
-            "y": 2.4411213005139327,
-            "heading": -0.34790283328354016,
-            "angularVelocity": -0.13694299193573645,
-            "velocityX": 3.556633642474382,
-            "velocityY": 1.1155413876329803,
-            "timestamp": 1.8260529926836289
-        },
-        {
-            "x": 3.475304114496225,
-            "y": 2.4831460278092354,
-            "heading": -0.3524511595124565,
-            "angularVelocity": -0.14692023650373393,
-            "velocityX": 3.466708012625811,
-            "velocityY": 1.3574846136083376,
-            "timestamp": 1.8570107854085351
-        },
-        {
-            "x": 3.5778542240444473,
-            "y": 2.531447362567012,
-            "heading": -0.3616437420636369,
-            "angularVelocity": -0.29693921116020117,
-            "velocityX": 3.312578207999158,
-            "velocityY": 1.560231867531513,
-            "timestamp": 1.8879685781334414
-        },
-        {
-            "x": 3.673558919058804,
-            "y": 2.583451056106446,
-            "heading": -0.37745622232401294,
-            "angularVelocity": -0.5107754419475534,
-            "velocityX": 3.091457322713648,
-            "velocityY": 1.6798256258655577,
-            "timestamp": 1.9189263708583477
-        },
-        {
-            "x": 3.7613706261508923,
-            "y": 2.636186152260406,
-            "heading": -0.3968957996596081,
-            "angularVelocity": -0.6279380932955558,
-            "velocityX": 2.836497675157173,
-            "velocityY": 1.7034514257095354,
-            "timestamp": 1.949884163583254
-        },
-        {
-            "x": 3.841306735175595,
-            "y": 2.6876448087218088,
-            "heading": -0.4179459699473055,
-            "angularVelocity": -0.6799635386030257,
-            "velocityX": 2.582099755467769,
-            "velocityY": 1.6622198138851267,
-            "timestamp": 1.9808419563081603
-        },
-        {
-            "x": 3.913645075614716,
-            "y": 2.736629100397619,
-            "heading": -0.43934657697326235,
-            "angularVelocity": -0.6912833617387507,
-            "velocityX": 2.336676296079001,
-            "velocityY": 1.582292772328868,
-            "timestamp": 2.0117997490330666
-        },
-        {
-            "x": 3.9786691618750996,
-            "y": 2.7823974078733573,
-            "heading": -0.46027689817629036,
-            "angularVelocity": -0.6760921681319296,
-            "velocityX": 2.100410931692797,
-            "velocityY": 1.4784099074015198,
-            "timestamp": 2.0427575417579726
-        },
-        {
-            "x": 4.036618763930541,
-            "y": 2.824460782764613,
-            "heading": -0.48017689659950147,
-            "angularVelocity": -0.6428106357887643,
-            "velocityX": 1.8718906276836884,
-            "velocityY": 1.3587330099747585,
-            "timestamp": 2.0737153344828787
-        },
-        {
-            "x": 4.08768908254176,
-            "y": 2.862477844590781,
-            "heading": -0.49864492956893264,
-            "angularVelocity": -0.5965552238942878,
-            "velocityX": 1.6496757073449355,
-            "velocityY": 1.2280288250525657,
-            "timestamp": 2.1046731272077848
-        },
-        {
-            "x": 4.132038835388808,
-            "y": 2.8961988047734253,
-            "heading": -0.5153800748468587,
-            "angularVelocity": -0.5405794084679139,
-            "velocityX": 1.4325876925792385,
-            "velocityY": 1.0892559583398733,
-            "timestamp": 2.135630919932691
-        },
-        {
-            "x": 4.169798307734006,
-            "y": 2.9254338939938918,
-            "heading": -0.5301487101337384,
-            "angularVelocity": -0.47705711511170307,
-            "velocityX": 1.2197081581622198,
-            "velocityY": 0.944353154640467,
-            "timestamp": 2.166588712657597
-        },
-        {
-            "x": 4.201075770562755,
-            "y": 2.9500345091711524,
-            "heading": -0.5427642633485259,
-            "angularVelocity": -0.4075081620800149,
-            "velocityX": 1.0103259979397883,
-            "velocityY": 0.7946501675963605,
-            "timestamp": 2.197546505382503
-        },
-        {
-            "x": 4.225962326087245,
-            "y": 2.9698813733488647,
-            "heading": -0.5530743601373866,
-            "angularVelocity": -0.3330372058810695,
-            "velocityX": 0.8038866254348108,
-            "velocityY": 0.6410942909939014,
-            "timestamp": 2.228504298107409
-        },
-        {
-            "x": 4.24453552816763,
-            "y": 2.984876771487546,
-            "heading": -0.5609523103628415,
-            "angularVelocity": -0.2544738992160421,
-            "velocityX": 0.5999524011761236,
-            "velocityY": 0.48438201883216997,
-            "timestamp": 2.259462090832315
-        },
-        {
-            "x": 4.256862100646349,
-            "y": 2.9949392682905573,
-            "heading": -0.5662912533122896,
-            "angularVelocity": -0.17245877305093363,
-            "velocityX": 0.398173493448939,
-            "velocityY": 0.32503922008988784,
-            "timestamp": 2.290419883557221
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.569,
-            "angularVelocity": -0.08749805620543083,
-            "velocityX": 0.19826669841112912,
-            "velocityY": 0.16347198117160036,
-            "timestamp": 2.321377676282127
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.569,
-            "angularVelocity": 1.3974941712649426e-29,
-            "velocityX": 2.480011116993708e-30,
-            "velocityY": -4.1979337006948246e-30,
-            "timestamp": 2.3523354690070333
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.1.traj
deleted file mode 100644
index 1ef15c59..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.1.traj	
+++ /dev/null
@@ -1,562 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 9.203343680058695e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.298008161759761,
-            "y": 6.742798044155305,
-            "heading": 0.37730350882287184,
-            "angularVelocity": -0.30162955292479776,
-            "velocityX": -0.04731994607959398,
-            "velocityY": -0.3373947598852993,
-            "timestamp": 0.0420929947149233
-        },
-        {
-            "x": 2.2940179124306126,
-            "y": 6.714368498753073,
-            "heading": 0.3522801711626307,
-            "angularVelocity": -0.5944774856175671,
-            "velocityX": -0.09479604281359731,
-            "velocityY": -0.6753984978919232,
-            "timestamp": 0.0841859894298466
-        },
-        {
-            "x": 2.28802134748838,
-            "y": 6.671681118579196,
-            "heading": 0.31538217591497186,
-            "angularVelocity": -0.8765828019021266,
-            "velocityX": -0.14245992671333493,
-            "velocityY": -1.014120769096589,
-            "timestamp": 0.1262789841447699
-        },
-        {
-            "x": 2.2800080756588827,
-            "y": 6.61469814393844,
-            "heading": 0.2672110996394858,
-            "angularVelocity": -1.1443965106718308,
-            "velocityX": -0.19037067530517543,
-            "velocityY": -1.3537400944426796,
-            "timestamp": 0.1683719788596932
-        },
-        {
-            "x": 2.2699650128963897,
-            "y": 6.543370292056319,
-            "heading": 0.20862519634351256,
-            "angularVelocity": -1.391820745773707,
-            "velocityX": -0.23859226055333851,
-            "velocityY": -1.694530226827328,
-            "timestamp": 0.21046497357461652
-        },
-        {
-            "x": 2.2578780077181495,
-            "y": 6.4576319980717,
-            "heading": 0.14091301714440063,
-            "angularVelocity": -1.608632972248606,
-            "velocityX": -0.2871500414760241,
-            "velocityY": -2.036877978516017,
-            "timestamp": 0.2525579682895398
-        },
-        {
-            "x": 2.2437361567314555,
-            "y": 6.357398182016401,
-            "heading": 0.06608849726258184,
-            "angularVelocity": -1.7776002964049293,
-            "velocityX": -0.3359668534508049,
-            "velocityY": -2.3812469683883926,
-            "timestamp": 0.2946509630044631
-        },
-        {
-            "x": 2.227539619778089,
-            "y": 6.242572554004172,
-            "heading": -0.012517898946656567,
-            "angularVelocity": -1.8674460380308828,
-            "velocityX": -0.3847798680768185,
-            "velocityY": -2.727903509595609,
-            "timestamp": 0.33674395771938637
-        },
-        {
-            "x": 2.209319968778144,
-            "y": 6.1131309245806085,
-            "heading": -0.08849823738903302,
-            "angularVelocity": -1.8050589880087344,
-            "velocityX": -0.4328428310539146,
-            "velocityY": -3.0751347177888695,
-            "timestamp": 0.37883695243430965
-        },
-        {
-            "x": 2.189363625025874,
-            "y": 5.970570114356374,
-            "heading": -0.13907359717832485,
-            "angularVelocity": -1.201514887021361,
-            "velocityX": -0.4741013056311528,
-            "velocityY": -3.38680607520881,
-            "timestamp": 0.42092994714923293
-        },
-        {
-            "x": 2.1674827409245396,
-            "y": 5.816618965425972,
-            "heading": -0.15173248317406665,
-            "angularVelocity": -0.3007361695568274,
-            "velocityX": -0.5198224609468644,
-            "velocityY": -3.657405465518488,
-            "timestamp": 0.4630229418641562
-        },
-        {
-            "x": 2.1445157307124236,
-            "y": 5.659048701686038,
-            "heading": -0.1517325271793822,
-            "angularVelocity": -0.0000010454308573871989,
-            "velocityX": -0.5456254744443967,
-            "velocityY": -3.7433844944292183,
-            "timestamp": 0.5051159365790795
-        },
-        {
-            "x": 2.1215487153797565,
-            "y": 5.501478438692377,
-            "heading": -0.1517325711844689,
-            "angularVelocity": -0.0000010454254199880688,
-            "velocityX": -0.5456255960929445,
-            "velocityY": -3.7433844767000393,
-            "timestamp": 0.5472089312940028
-        },
-        {
-            "x": 2.0985817251133763,
-            "y": 5.343908172045118,
-            "heading": -0.15173261518953962,
-            "angularVelocity": -0.000001045425040533604,
-            "velocityX": -0.5456250005951166,
-            "velocityY": -3.7433845634983034,
-            "timestamp": 0.5893019260089262
-        },
-        {
-            "x": 2.075614762728362,
-            "y": 5.186337901333946,
-            "heading": -0.15173265919459192,
-            "angularVelocity": -0.0000010454246033549915,
-            "velocityX": -0.5456243382196686,
-            "velocityY": -3.743384660044355,
-            "timestamp": 0.6313949207238495
-        },
-        {
-            "x": 2.0526478312972185,
-            "y": 5.028767626111028,
-            "heading": -0.15173270319962293,
-            "angularVelocity": -0.0000010454240970480152,
-            "velocityX": -0.5456236028509798,
-            "velocityY": -3.7433847672295513,
-            "timestamp": 0.6734879154387728
-        },
-        {
-            "x": 2.0296809339445536,
-            "y": 4.871197345920939,
-            "heading": -0.15173274720462945,
-            "angularVelocity": -0.0000010454235162635837,
-            "velocityX": -0.5456227932512114,
-            "velocityY": -3.743384885234266,
-            "timestamp": 0.7155809101536962
-        },
-        {
-            "x": 2.0067140734575584,
-            "y": 4.7136270603574335,
-            "heading": -0.15173279120960867,
-            "angularVelocity": -0.0000010454228670132247,
-            "velocityX": -0.5456219174363699,
-            "velocityY": -3.7433850128900636,
-            "timestamp": 0.7576739048686195
-        },
-        {
-            "x": 1.983747251812091,
-            "y": 4.55605676913253,
-            "heading": -0.15173283521455908,
-            "angularVelocity": -0.000001045422182833939,
-            "velocityX": -0.545620994681206,
-            "velocityY": -3.743385147387475,
-            "timestamp": 0.7997668995835429
-        },
-        {
-            "x": 1.9607804702256346,
-            "y": 4.398486472068785,
-            "heading": -0.15173287921948736,
-            "angularVelocity": -0.0000010454216570158517,
-            "velocityX": -0.5456200430024101,
-            "velocityY": -3.7433852861004024,
-            "timestamp": 0.8418598942984662
-        },
-        {
-            "x": 1.9378163633559966,
-            "y": 4.240915785172817,
-            "heading": -0.15173292322446258,
-            "angularVelocity": -0.0000010454227717672262,
-            "velocityX": -0.5455564999630786,
-            "velocityY": -3.7433945473141725,
-            "timestamp": 0.8839528890133895
-        },
-        {
-            "x": 1.9275487572437193,
-            "y": 4.082011890541997,
-            "heading": -0.15173296776197548,
-            "angularVelocity": -0.000001058074228463605,
-            "velocityX": -0.2439267194414494,
-            "velocityY": -3.775067459728259,
-            "timestamp": 0.9260458837283129
-        },
-        {
-            "x": 1.9320519221515333,
-            "y": 3.9228403083205765,
-            "heading": -0.15173301420750987,
-            "angularVelocity": -0.0000011034029462076059,
-            "velocityX": 0.10698133830372927,
-            "velocityY": -3.781426892988182,
-            "timestamp": 0.9681388784432362
-        },
-        {
-            "x": 1.9512874233679367,
-            "y": 3.764771127717043,
-            "heading": -0.15173306433513473,
-            "angularVelocity": -0.0000011908780826817618,
-            "velocityX": 0.45697630559851177,
-            "velocityY": -3.7552372235348,
-            "timestamp": 1.0102318731581594
-        },
-        {
-            "x": 1.9850897622274482,
-            "y": 3.609164986787025,
-            "heading": -0.15173312063283023,
-            "angularVelocity": -0.0000013374599716310608,
-            "velocityX": 0.8030395339756486,
-            "velocityY": -3.6967229816711127,
-            "timestamp": 1.0523248678730828
-        },
-        {
-            "x": 2.0331680016062417,
-            "y": 3.4573613296965586,
-            "heading": -0.1517331870158403,
-            "angularVelocity": -0.0000015770560044658274,
-            "velocityX": 1.142190991741159,
-            "velocityY": -3.606387668982997,
-            "timestamp": 1.094417862588006
-        },
-        {
-            "x": 2.0951083019579966,
-            "y": 3.3106668726225337,
-            "heading": -0.15173327041919607,
-            "angularVelocity": -0.0000019814070334134687,
-            "velocityX": 1.4715108956074094,
-            "velocityY": -3.4850088017619067,
-            "timestamp": 1.1365108573029294
-        },
-        {
-            "x": 2.170377492904663,
-            "y": 3.170344352722168,
-            "heading": -0.1517333779044128,
-            "angularVelocity": -0.000002553517929494838,
-            "velocityX": 1.7881643122906952,
-            "velocityY": -3.333631186155945,
-            "timestamp": 1.1786038520178528
-        },
-        {
-            "x": 2.2421653916580504,
-            "y": 3.058317386598826,
-            "heading": -0.1517334835913584,
-            "angularVelocity": -0.0000030048345176781924,
-            "velocityX": 2.041035009880172,
-            "velocityY": -3.1850905776457417,
-            "timestamp": 1.213776153524627
-        },
-        {
-            "x": 2.322415894828655,
-            "y": 2.9521882028356305,
-            "heading": -0.15173356325163515,
-            "angularVelocity": -0.000002264858236060329,
-            "velocityX": 2.2816392369190814,
-            "velocityY": -3.0174079948323826,
-            "timestamp": 1.2489484550314012
-        },
-        {
-            "x": 2.4106466853387967,
-            "y": 2.8525946456824203,
-            "heading": -0.1517336276365959,
-            "angularVelocity": -0.0000018305586497777263,
-            "velocityX": 2.5085304836576707,
-            "velocityY": -2.8315905666289245,
-            "timestamp": 1.2841207565381754
-        },
-        {
-            "x": 2.5063274836460994,
-            "y": 2.7601352747554455,
-            "heading": -0.15173368243148194,
-            "angularVelocity": -0.0000015578987915956282,
-            "velocityX": 2.720345107040385,
-            "velocityY": -2.628755212654101,
-            "timestamp": 1.3192930580449496
-        },
-        {
-            "x": 2.6088832332920924,
-            "y": 2.67536576592715,
-            "heading": -0.15173373101035148,
-            "angularVelocity": -0.000001381168347773627,
-            "velocityX": 2.915810033819959,
-            "velocityY": -2.410121180496788,
-            "timestamp": 1.3544653595517238
-        },
-        {
-            "x": 2.7176975542058304,
-            "y": 2.598795565150263,
-            "heading": -0.1517337755722331,
-            "angularVelocity": -0.0000012669595030854418,
-            "velocityX": 3.0937503732242226,
-            "velocityY": -2.1770028544233733,
-            "timestamp": 1.389637661058498
-        },
-        {
-            "x": 2.8321164386506434,
-            "y": 2.5308848045360426,
-            "heading": -0.15173381767917685,
-            "angularVelocity": -0.0000011971620259359605,
-            "velocityX": 3.2530963156555406,
-            "velocityY": -1.930802299108592,
-            "timestamp": 1.4248099625652721
-        },
-        {
-            "x": 2.951452140465328,
-            "y": 2.4720414272198683,
-            "heading": -0.15173385854226368,
-            "angularVelocity": -0.0000011617973536146762,
-            "velocityX": 3.3928886283344455,
-            "velocityY": -1.6730033235055968,
-            "timestamp": 1.4599822640720463
-        },
-        {
-            "x": 3.0749866313948626,
-            "y": 2.4226169148084327,
-            "heading": -0.151733899192453,
-            "angularVelocity": -0.000001155744366153098,
-            "velocityX": 3.512266347021429,
-            "velocityY": -1.4052112114959712,
-            "timestamp": 1.4951545655788205
-        },
-        {
-            "x": 3.1992028583958065,
-            "y": 2.3749316777596947,
-            "heading": -0.1517339398689439,
-            "angularVelocity": -0.0000011564921588237654,
-            "velocityX": 3.5316491011263524,
-            "velocityY": -1.3557610678264387,
-            "timestamp": 1.5303268670855947
-        },
-        {
-            "x": 3.3234191450837622,
-            "y": 2.3272465961911335,
-            "heading": -0.15173398054544393,
-            "angularVelocity": -0.0000011564924185481092,
-            "velocityX": 3.5316507981154315,
-            "velocityY": -1.355756647297499,
-            "timestamp": 1.565499168592369
-        },
-        {
-            "x": 3.4476354415156054,
-            "y": 2.2795615400047207,
-            "heading": -0.15173402122195195,
-            "angularVelocity": -0.0000011564926460273957,
-            "velocityX": 3.5316510751484063,
-            "velocityY": -1.3557559256458895,
-            "timestamp": 1.6006714700991431
-        },
-        {
-            "x": 3.57185174767356,
-            "y": 2.231876509154167,
-            "heading": -0.15173406189847174,
-            "angularVelocity": -0.0000011564929795008858,
-            "velocityX": 3.531651351675977,
-            "velocityY": -1.355755205310334,
-            "timestamp": 1.6358437716059173
-        },
-        {
-            "x": 3.6960680635749434,
-            "y": 2.1841915036846027,
-            "heading": -0.15173410257500647,
-            "angularVelocity": -0.000001156493404477995,
-            "velocityX": 3.5316516286959314,
-            "velocityY": -1.3557544836916666,
-            "timestamp": 1.6710160731126915
-        },
-        {
-            "x": 3.820284389246338,
-            "y": 2.136506523665285,
-            "heading": -0.15173414325155568,
-            "angularVelocity": -0.0000011564938164652249,
-            "velocityX": 3.5316519064716276,
-            "velocityY": -1.3557537601039011,
-            "timestamp": 1.7061883746194657
-        },
-        {
-            "x": 3.9445007247793735,
-            "y": 2.0888215693349235,
-            "heading": -0.15173418392811813,
-            "angularVelocity": -0.0000011564941935950735,
-            "velocityX": 3.5316521868525257,
-            "velocityY": -1.3557530297293134,
-            "timestamp": 1.74136067612624
-        },
-        {
-            "x": 4.068717070394662,
-            "y": 2.0411366412682117,
-            "heading": -0.1517342246046924,
-            "angularVelocity": -0.0000011564945294151802,
-            "velocityX": 3.531652473505735,
-            "velocityY": -1.3557522830153055,
-            "timestamp": 1.776532977633014
-        },
-        {
-            "x": 4.192933426524707,
-            "y": 1.9934517405918115,
-            "heading": -0.15173426528127656,
-            "angularVelocity": -0.0000011564948108738022,
-            "velocityX": 3.5316527724556495,
-            "velocityY": -1.3557515042686648,
-            "timestamp": 1.8117052791397883
-        },
-        {
-            "x": 4.3171498402859925,
-            "y": 1.9457669900421575,
-            "heading": -0.1517343059583456,
-            "angularVelocity": -0.0000011565085966842365,
-            "velocityX": 3.531654410996122,
-            "velocityY": -1.3557472359455842,
-            "timestamp": 1.8468775806465625
-        },
-        {
-            "x": 4.438199866372425,
-            "y": 1.9006463371040783,
-            "heading": -0.16306820061092758,
-            "angularVelocity": -0.3222392100329015,
-            "velocityX": 3.441629375976964,
-            "velocityY": -1.282846188765579,
-            "timestamp": 1.8820498821533367
-        },
-        {
-            "x": 4.551333178258229,
-            "y": 1.8581105785728176,
-            "heading": -0.19901052601489952,
-            "angularVelocity": -1.0218929061849813,
-            "velocityX": 3.2165456066050515,
-            "velocityY": -1.2093538582645278,
-            "timestamp": 1.9172221836601109
-        },
-        {
-            "x": 4.6559660625940325,
-            "y": 1.8184646011809935,
-            "heading": -0.2548690023306322,
-            "angularVelocity": -1.5881382202121255,
-            "velocityX": 2.9748660125539748,
-            "velocityY": -1.1271931518097666,
-            "timestamp": 1.952394485166885
-        },
-        {
-            "x": 4.750968691238499,
-            "y": 1.7823676048635404,
-            "heading": -0.3144227593810797,
-            "angularVelocity": -1.693200458860429,
-            "velocityX": 2.7010637511499986,
-            "velocityY": -1.0262904265875403,
-            "timestamp": 1.9875667866736593
-        },
-        {
-            "x": 4.836310937621718,
-            "y": 1.749878315803868,
-            "heading": -0.37309459063768413,
-            "angularVelocity": -1.6681260180060764,
-            "velocityX": 2.426404947278819,
-            "velocityY": -0.9237180300360781,
-            "timestamp": 2.0227390881804337
-        },
-        {
-            "x": 4.912035358987092,
-            "y": 1.7210053742684595,
-            "heading": -0.4285254423369764,
-            "angularVelocity": -1.5759802266171352,
-            "velocityX": 2.1529561081121917,
-            "velocityY": -0.8208999780650579,
-            "timestamp": 2.057911389687208
-        },
-        {
-            "x": 4.978188578205751,
-            "y": 1.6957473341798728,
-            "heading": -0.4792367761737124,
-            "angularVelocity": -1.4417974276425698,
-            "velocityX": 1.8808328253957978,
-            "velocityY": -0.71812304019178,
-            "timestamp": 2.0930836911939825
-        },
-        {
-            "x": 5.034811621143831,
-            "y": 1.6741013653395114,
-            "heading": -0.5242175587960242,
-            "angularVelocity": -1.278869471013956,
-            "velocityX": 1.6098759680874073,
-            "velocityY": -0.6154265690060781,
-            "timestamp": 2.128255992700757
-        },
-        {
-            "x": 5.081938937252143,
-            "y": 1.656065298518901,
-            "heading": -0.5627406592667319,
-            "angularVelocity": -1.0952681178195878,
-            "velocityX": 1.3398985590759445,
-            "velocityY": -0.5127917721601607,
-            "timestamp": 2.1634282942075314
-        },
-        {
-            "x": 5.11959917834615,
-            "y": 1.6416378108353973,
-            "heading": -0.5942629145071194,
-            "angularVelocity": -0.8962238434785516,
-            "velocityX": 1.0707357631047747,
-            "velocityY": -0.41019458680362747,
-            "timestamp": 2.1986005957143058
-        },
-        {
-            "x": 5.147816310703936,
-            "y": 1.63081813815951,
-            "heading": -0.6183637008514038,
-            "angularVelocity": -0.6852206228143011,
-            "velocityX": 0.8022543634897267,
-            "velocityY": -0.3076191267666608,
-            "timestamp": 2.23377289722108
-        },
-        {
-            "x": 5.1666106911988186,
-            "y": 1.6236057376407542,
-            "heading": -0.6347043261398958,
-            "angularVelocity": -0.4645878884367218,
-            "velocityX": 0.5343517395716112,
-            "velocityY": -0.20505910076332604,
-            "timestamp": 2.2689451987278546
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.643,
-            "angularVelocity": -0.23585814702817923,
-            "velocityX": 0.26695178873562564,
-            "velocityY": -0.10251639745723456,
-            "timestamp": 2.304117500234629
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.643,
-            "angularVelocity": -2.979902278450704e-33,
-            "velocityX": -7.596111021745797e-35,
-            "velocityY": -9.748244176889607e-35,
-            "timestamp": 2.3392898017414034
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.traj
deleted file mode 100644
index 1ef15c59..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.traj	
+++ /dev/null
@@ -1,562 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 9.203343680058695e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.298008161759761,
-            "y": 6.742798044155305,
-            "heading": 0.37730350882287184,
-            "angularVelocity": -0.30162955292479776,
-            "velocityX": -0.04731994607959398,
-            "velocityY": -0.3373947598852993,
-            "timestamp": 0.0420929947149233
-        },
-        {
-            "x": 2.2940179124306126,
-            "y": 6.714368498753073,
-            "heading": 0.3522801711626307,
-            "angularVelocity": -0.5944774856175671,
-            "velocityX": -0.09479604281359731,
-            "velocityY": -0.6753984978919232,
-            "timestamp": 0.0841859894298466
-        },
-        {
-            "x": 2.28802134748838,
-            "y": 6.671681118579196,
-            "heading": 0.31538217591497186,
-            "angularVelocity": -0.8765828019021266,
-            "velocityX": -0.14245992671333493,
-            "velocityY": -1.014120769096589,
-            "timestamp": 0.1262789841447699
-        },
-        {
-            "x": 2.2800080756588827,
-            "y": 6.61469814393844,
-            "heading": 0.2672110996394858,
-            "angularVelocity": -1.1443965106718308,
-            "velocityX": -0.19037067530517543,
-            "velocityY": -1.3537400944426796,
-            "timestamp": 0.1683719788596932
-        },
-        {
-            "x": 2.2699650128963897,
-            "y": 6.543370292056319,
-            "heading": 0.20862519634351256,
-            "angularVelocity": -1.391820745773707,
-            "velocityX": -0.23859226055333851,
-            "velocityY": -1.694530226827328,
-            "timestamp": 0.21046497357461652
-        },
-        {
-            "x": 2.2578780077181495,
-            "y": 6.4576319980717,
-            "heading": 0.14091301714440063,
-            "angularVelocity": -1.608632972248606,
-            "velocityX": -0.2871500414760241,
-            "velocityY": -2.036877978516017,
-            "timestamp": 0.2525579682895398
-        },
-        {
-            "x": 2.2437361567314555,
-            "y": 6.357398182016401,
-            "heading": 0.06608849726258184,
-            "angularVelocity": -1.7776002964049293,
-            "velocityX": -0.3359668534508049,
-            "velocityY": -2.3812469683883926,
-            "timestamp": 0.2946509630044631
-        },
-        {
-            "x": 2.227539619778089,
-            "y": 6.242572554004172,
-            "heading": -0.012517898946656567,
-            "angularVelocity": -1.8674460380308828,
-            "velocityX": -0.3847798680768185,
-            "velocityY": -2.727903509595609,
-            "timestamp": 0.33674395771938637
-        },
-        {
-            "x": 2.209319968778144,
-            "y": 6.1131309245806085,
-            "heading": -0.08849823738903302,
-            "angularVelocity": -1.8050589880087344,
-            "velocityX": -0.4328428310539146,
-            "velocityY": -3.0751347177888695,
-            "timestamp": 0.37883695243430965
-        },
-        {
-            "x": 2.189363625025874,
-            "y": 5.970570114356374,
-            "heading": -0.13907359717832485,
-            "angularVelocity": -1.201514887021361,
-            "velocityX": -0.4741013056311528,
-            "velocityY": -3.38680607520881,
-            "timestamp": 0.42092994714923293
-        },
-        {
-            "x": 2.1674827409245396,
-            "y": 5.816618965425972,
-            "heading": -0.15173248317406665,
-            "angularVelocity": -0.3007361695568274,
-            "velocityX": -0.5198224609468644,
-            "velocityY": -3.657405465518488,
-            "timestamp": 0.4630229418641562
-        },
-        {
-            "x": 2.1445157307124236,
-            "y": 5.659048701686038,
-            "heading": -0.1517325271793822,
-            "angularVelocity": -0.0000010454308573871989,
-            "velocityX": -0.5456254744443967,
-            "velocityY": -3.7433844944292183,
-            "timestamp": 0.5051159365790795
-        },
-        {
-            "x": 2.1215487153797565,
-            "y": 5.501478438692377,
-            "heading": -0.1517325711844689,
-            "angularVelocity": -0.0000010454254199880688,
-            "velocityX": -0.5456255960929445,
-            "velocityY": -3.7433844767000393,
-            "timestamp": 0.5472089312940028
-        },
-        {
-            "x": 2.0985817251133763,
-            "y": 5.343908172045118,
-            "heading": -0.15173261518953962,
-            "angularVelocity": -0.000001045425040533604,
-            "velocityX": -0.5456250005951166,
-            "velocityY": -3.7433845634983034,
-            "timestamp": 0.5893019260089262
-        },
-        {
-            "x": 2.075614762728362,
-            "y": 5.186337901333946,
-            "heading": -0.15173265919459192,
-            "angularVelocity": -0.0000010454246033549915,
-            "velocityX": -0.5456243382196686,
-            "velocityY": -3.743384660044355,
-            "timestamp": 0.6313949207238495
-        },
-        {
-            "x": 2.0526478312972185,
-            "y": 5.028767626111028,
-            "heading": -0.15173270319962293,
-            "angularVelocity": -0.0000010454240970480152,
-            "velocityX": -0.5456236028509798,
-            "velocityY": -3.7433847672295513,
-            "timestamp": 0.6734879154387728
-        },
-        {
-            "x": 2.0296809339445536,
-            "y": 4.871197345920939,
-            "heading": -0.15173274720462945,
-            "angularVelocity": -0.0000010454235162635837,
-            "velocityX": -0.5456227932512114,
-            "velocityY": -3.743384885234266,
-            "timestamp": 0.7155809101536962
-        },
-        {
-            "x": 2.0067140734575584,
-            "y": 4.7136270603574335,
-            "heading": -0.15173279120960867,
-            "angularVelocity": -0.0000010454228670132247,
-            "velocityX": -0.5456219174363699,
-            "velocityY": -3.7433850128900636,
-            "timestamp": 0.7576739048686195
-        },
-        {
-            "x": 1.983747251812091,
-            "y": 4.55605676913253,
-            "heading": -0.15173283521455908,
-            "angularVelocity": -0.000001045422182833939,
-            "velocityX": -0.545620994681206,
-            "velocityY": -3.743385147387475,
-            "timestamp": 0.7997668995835429
-        },
-        {
-            "x": 1.9607804702256346,
-            "y": 4.398486472068785,
-            "heading": -0.15173287921948736,
-            "angularVelocity": -0.0000010454216570158517,
-            "velocityX": -0.5456200430024101,
-            "velocityY": -3.7433852861004024,
-            "timestamp": 0.8418598942984662
-        },
-        {
-            "x": 1.9378163633559966,
-            "y": 4.240915785172817,
-            "heading": -0.15173292322446258,
-            "angularVelocity": -0.0000010454227717672262,
-            "velocityX": -0.5455564999630786,
-            "velocityY": -3.7433945473141725,
-            "timestamp": 0.8839528890133895
-        },
-        {
-            "x": 1.9275487572437193,
-            "y": 4.082011890541997,
-            "heading": -0.15173296776197548,
-            "angularVelocity": -0.000001058074228463605,
-            "velocityX": -0.2439267194414494,
-            "velocityY": -3.775067459728259,
-            "timestamp": 0.9260458837283129
-        },
-        {
-            "x": 1.9320519221515333,
-            "y": 3.9228403083205765,
-            "heading": -0.15173301420750987,
-            "angularVelocity": -0.0000011034029462076059,
-            "velocityX": 0.10698133830372927,
-            "velocityY": -3.781426892988182,
-            "timestamp": 0.9681388784432362
-        },
-        {
-            "x": 1.9512874233679367,
-            "y": 3.764771127717043,
-            "heading": -0.15173306433513473,
-            "angularVelocity": -0.0000011908780826817618,
-            "velocityX": 0.45697630559851177,
-            "velocityY": -3.7552372235348,
-            "timestamp": 1.0102318731581594
-        },
-        {
-            "x": 1.9850897622274482,
-            "y": 3.609164986787025,
-            "heading": -0.15173312063283023,
-            "angularVelocity": -0.0000013374599716310608,
-            "velocityX": 0.8030395339756486,
-            "velocityY": -3.6967229816711127,
-            "timestamp": 1.0523248678730828
-        },
-        {
-            "x": 2.0331680016062417,
-            "y": 3.4573613296965586,
-            "heading": -0.1517331870158403,
-            "angularVelocity": -0.0000015770560044658274,
-            "velocityX": 1.142190991741159,
-            "velocityY": -3.606387668982997,
-            "timestamp": 1.094417862588006
-        },
-        {
-            "x": 2.0951083019579966,
-            "y": 3.3106668726225337,
-            "heading": -0.15173327041919607,
-            "angularVelocity": -0.0000019814070334134687,
-            "velocityX": 1.4715108956074094,
-            "velocityY": -3.4850088017619067,
-            "timestamp": 1.1365108573029294
-        },
-        {
-            "x": 2.170377492904663,
-            "y": 3.170344352722168,
-            "heading": -0.1517333779044128,
-            "angularVelocity": -0.000002553517929494838,
-            "velocityX": 1.7881643122906952,
-            "velocityY": -3.333631186155945,
-            "timestamp": 1.1786038520178528
-        },
-        {
-            "x": 2.2421653916580504,
-            "y": 3.058317386598826,
-            "heading": -0.1517334835913584,
-            "angularVelocity": -0.0000030048345176781924,
-            "velocityX": 2.041035009880172,
-            "velocityY": -3.1850905776457417,
-            "timestamp": 1.213776153524627
-        },
-        {
-            "x": 2.322415894828655,
-            "y": 2.9521882028356305,
-            "heading": -0.15173356325163515,
-            "angularVelocity": -0.000002264858236060329,
-            "velocityX": 2.2816392369190814,
-            "velocityY": -3.0174079948323826,
-            "timestamp": 1.2489484550314012
-        },
-        {
-            "x": 2.4106466853387967,
-            "y": 2.8525946456824203,
-            "heading": -0.1517336276365959,
-            "angularVelocity": -0.0000018305586497777263,
-            "velocityX": 2.5085304836576707,
-            "velocityY": -2.8315905666289245,
-            "timestamp": 1.2841207565381754
-        },
-        {
-            "x": 2.5063274836460994,
-            "y": 2.7601352747554455,
-            "heading": -0.15173368243148194,
-            "angularVelocity": -0.0000015578987915956282,
-            "velocityX": 2.720345107040385,
-            "velocityY": -2.628755212654101,
-            "timestamp": 1.3192930580449496
-        },
-        {
-            "x": 2.6088832332920924,
-            "y": 2.67536576592715,
-            "heading": -0.15173373101035148,
-            "angularVelocity": -0.000001381168347773627,
-            "velocityX": 2.915810033819959,
-            "velocityY": -2.410121180496788,
-            "timestamp": 1.3544653595517238
-        },
-        {
-            "x": 2.7176975542058304,
-            "y": 2.598795565150263,
-            "heading": -0.1517337755722331,
-            "angularVelocity": -0.0000012669595030854418,
-            "velocityX": 3.0937503732242226,
-            "velocityY": -2.1770028544233733,
-            "timestamp": 1.389637661058498
-        },
-        {
-            "x": 2.8321164386506434,
-            "y": 2.5308848045360426,
-            "heading": -0.15173381767917685,
-            "angularVelocity": -0.0000011971620259359605,
-            "velocityX": 3.2530963156555406,
-            "velocityY": -1.930802299108592,
-            "timestamp": 1.4248099625652721
-        },
-        {
-            "x": 2.951452140465328,
-            "y": 2.4720414272198683,
-            "heading": -0.15173385854226368,
-            "angularVelocity": -0.0000011617973536146762,
-            "velocityX": 3.3928886283344455,
-            "velocityY": -1.6730033235055968,
-            "timestamp": 1.4599822640720463
-        },
-        {
-            "x": 3.0749866313948626,
-            "y": 2.4226169148084327,
-            "heading": -0.151733899192453,
-            "angularVelocity": -0.000001155744366153098,
-            "velocityX": 3.512266347021429,
-            "velocityY": -1.4052112114959712,
-            "timestamp": 1.4951545655788205
-        },
-        {
-            "x": 3.1992028583958065,
-            "y": 2.3749316777596947,
-            "heading": -0.1517339398689439,
-            "angularVelocity": -0.0000011564921588237654,
-            "velocityX": 3.5316491011263524,
-            "velocityY": -1.3557610678264387,
-            "timestamp": 1.5303268670855947
-        },
-        {
-            "x": 3.3234191450837622,
-            "y": 2.3272465961911335,
-            "heading": -0.15173398054544393,
-            "angularVelocity": -0.0000011564924185481092,
-            "velocityX": 3.5316507981154315,
-            "velocityY": -1.355756647297499,
-            "timestamp": 1.565499168592369
-        },
-        {
-            "x": 3.4476354415156054,
-            "y": 2.2795615400047207,
-            "heading": -0.15173402122195195,
-            "angularVelocity": -0.0000011564926460273957,
-            "velocityX": 3.5316510751484063,
-            "velocityY": -1.3557559256458895,
-            "timestamp": 1.6006714700991431
-        },
-        {
-            "x": 3.57185174767356,
-            "y": 2.231876509154167,
-            "heading": -0.15173406189847174,
-            "angularVelocity": -0.0000011564929795008858,
-            "velocityX": 3.531651351675977,
-            "velocityY": -1.355755205310334,
-            "timestamp": 1.6358437716059173
-        },
-        {
-            "x": 3.6960680635749434,
-            "y": 2.1841915036846027,
-            "heading": -0.15173410257500647,
-            "angularVelocity": -0.000001156493404477995,
-            "velocityX": 3.5316516286959314,
-            "velocityY": -1.3557544836916666,
-            "timestamp": 1.6710160731126915
-        },
-        {
-            "x": 3.820284389246338,
-            "y": 2.136506523665285,
-            "heading": -0.15173414325155568,
-            "angularVelocity": -0.0000011564938164652249,
-            "velocityX": 3.5316519064716276,
-            "velocityY": -1.3557537601039011,
-            "timestamp": 1.7061883746194657
-        },
-        {
-            "x": 3.9445007247793735,
-            "y": 2.0888215693349235,
-            "heading": -0.15173418392811813,
-            "angularVelocity": -0.0000011564941935950735,
-            "velocityX": 3.5316521868525257,
-            "velocityY": -1.3557530297293134,
-            "timestamp": 1.74136067612624
-        },
-        {
-            "x": 4.068717070394662,
-            "y": 2.0411366412682117,
-            "heading": -0.1517342246046924,
-            "angularVelocity": -0.0000011564945294151802,
-            "velocityX": 3.531652473505735,
-            "velocityY": -1.3557522830153055,
-            "timestamp": 1.776532977633014
-        },
-        {
-            "x": 4.192933426524707,
-            "y": 1.9934517405918115,
-            "heading": -0.15173426528127656,
-            "angularVelocity": -0.0000011564948108738022,
-            "velocityX": 3.5316527724556495,
-            "velocityY": -1.3557515042686648,
-            "timestamp": 1.8117052791397883
-        },
-        {
-            "x": 4.3171498402859925,
-            "y": 1.9457669900421575,
-            "heading": -0.1517343059583456,
-            "angularVelocity": -0.0000011565085966842365,
-            "velocityX": 3.531654410996122,
-            "velocityY": -1.3557472359455842,
-            "timestamp": 1.8468775806465625
-        },
-        {
-            "x": 4.438199866372425,
-            "y": 1.9006463371040783,
-            "heading": -0.16306820061092758,
-            "angularVelocity": -0.3222392100329015,
-            "velocityX": 3.441629375976964,
-            "velocityY": -1.282846188765579,
-            "timestamp": 1.8820498821533367
-        },
-        {
-            "x": 4.551333178258229,
-            "y": 1.8581105785728176,
-            "heading": -0.19901052601489952,
-            "angularVelocity": -1.0218929061849813,
-            "velocityX": 3.2165456066050515,
-            "velocityY": -1.2093538582645278,
-            "timestamp": 1.9172221836601109
-        },
-        {
-            "x": 4.6559660625940325,
-            "y": 1.8184646011809935,
-            "heading": -0.2548690023306322,
-            "angularVelocity": -1.5881382202121255,
-            "velocityX": 2.9748660125539748,
-            "velocityY": -1.1271931518097666,
-            "timestamp": 1.952394485166885
-        },
-        {
-            "x": 4.750968691238499,
-            "y": 1.7823676048635404,
-            "heading": -0.3144227593810797,
-            "angularVelocity": -1.693200458860429,
-            "velocityX": 2.7010637511499986,
-            "velocityY": -1.0262904265875403,
-            "timestamp": 1.9875667866736593
-        },
-        {
-            "x": 4.836310937621718,
-            "y": 1.749878315803868,
-            "heading": -0.37309459063768413,
-            "angularVelocity": -1.6681260180060764,
-            "velocityX": 2.426404947278819,
-            "velocityY": -0.9237180300360781,
-            "timestamp": 2.0227390881804337
-        },
-        {
-            "x": 4.912035358987092,
-            "y": 1.7210053742684595,
-            "heading": -0.4285254423369764,
-            "angularVelocity": -1.5759802266171352,
-            "velocityX": 2.1529561081121917,
-            "velocityY": -0.8208999780650579,
-            "timestamp": 2.057911389687208
-        },
-        {
-            "x": 4.978188578205751,
-            "y": 1.6957473341798728,
-            "heading": -0.4792367761737124,
-            "angularVelocity": -1.4417974276425698,
-            "velocityX": 1.8808328253957978,
-            "velocityY": -0.71812304019178,
-            "timestamp": 2.0930836911939825
-        },
-        {
-            "x": 5.034811621143831,
-            "y": 1.6741013653395114,
-            "heading": -0.5242175587960242,
-            "angularVelocity": -1.278869471013956,
-            "velocityX": 1.6098759680874073,
-            "velocityY": -0.6154265690060781,
-            "timestamp": 2.128255992700757
-        },
-        {
-            "x": 5.081938937252143,
-            "y": 1.656065298518901,
-            "heading": -0.5627406592667319,
-            "angularVelocity": -1.0952681178195878,
-            "velocityX": 1.3398985590759445,
-            "velocityY": -0.5127917721601607,
-            "timestamp": 2.1634282942075314
-        },
-        {
-            "x": 5.11959917834615,
-            "y": 1.6416378108353973,
-            "heading": -0.5942629145071194,
-            "angularVelocity": -0.8962238434785516,
-            "velocityX": 1.0707357631047747,
-            "velocityY": -0.41019458680362747,
-            "timestamp": 2.1986005957143058
-        },
-        {
-            "x": 5.147816310703936,
-            "y": 1.63081813815951,
-            "heading": -0.6183637008514038,
-            "angularVelocity": -0.6852206228143011,
-            "velocityX": 0.8022543634897267,
-            "velocityY": -0.3076191267666608,
-            "timestamp": 2.23377289722108
-        },
-        {
-            "x": 5.1666106911988186,
-            "y": 1.6236057376407542,
-            "heading": -0.6347043261398958,
-            "angularVelocity": -0.4645878884367218,
-            "velocityX": 0.5343517395716112,
-            "velocityY": -0.20505910076332604,
-            "timestamp": 2.2689451987278546
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.643,
-            "angularVelocity": -0.23585814702817923,
-            "velocityX": 0.26695178873562564,
-            "velocityY": -0.10251639745723456,
-            "timestamp": 2.304117500234629
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.643,
-            "angularVelocity": -2.979902278450704e-33,
-            "velocityX": -7.596111021745797e-35,
-            "velocityY": -9.748244176889607e-35,
-            "timestamp": 2.3392898017414034
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.1.traj
deleted file mode 100644
index a5fd0bb0..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.1.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 1.792103156985809e-33,
-            "velocityX": -7.27073415334495e-34,
-            "velocityY": -7.853383039035825e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.298001341213542,
-            "y": 6.709118496985434,
-            "heading": 0.374359095500822,
-            "angularVelocity": -0.2058832051525312,
-            "velocityX": -0.02630859852023658,
-            "velocityY": -0.6302702831973344,
-            "timestamp": 0.07596979310473732
-        },
-        {
-            "x": 2.294004039374054,
-            "y": 6.613355753676287,
-            "heading": 0.3430620070158405,
-            "angularVelocity": -0.4119675361210102,
-            "velocityX": -0.0526169899393711,
-            "velocityY": -1.2605371081782701,
-            "timestamp": 0.15193958620947465
-        },
-        {
-            "x": 2.2880080040813597,
-            "y": 6.469712250400375,
-            "heading": 0.296082691458658,
-            "angularVelocity": -0.6183946755312791,
-            "velocityX": -0.07892657130746197,
-            "velocityY": -1.8907976105433808,
-            "timestamp": 0.22790937931421196
-        },
-        {
-            "x": 2.280012894319733,
-            "y": 6.278188663497132,
-            "heading": 0.23338596481601495,
-            "angularVelocity": -0.8252849465603376,
-            "velocityX": -0.10524064150870725,
-            "velocityY": -2.5210492101669324,
-            "timestamp": 0.3038791724189493
-        },
-        {
-            "x": 2.270017931664709,
-            "y": 6.038785816229407,
-            "heading": 0.1549313176962556,
-            "angularVelocity": -1.0327084478378958,
-            "velocityX": -0.13156495820969516,
-            "velocityY": -3.1512899730774246,
-            "timestamp": 0.37984896552368663
-        },
-        {
-            "x": 2.2620113109689415,
-            "y": 5.847271344563018,
-            "heading": 0.09176539516362914,
-            "angularVelocity": -0.8314610314331311,
-            "velocityX": -0.10539216139142096,
-            "velocityY": -2.5209292251507525,
-            "timestamp": 0.45581875862842397
-        },
-        {
-            "x": 2.2560057963370115,
-            "y": 5.703635646814395,
-            "heading": 0.0443838554377995,
-            "angularVelocity": -0.6236892031613949,
-            "velocityX": -0.079051349049357,
-            "velocityY": -1.8906948653999391,
-            "timestamp": 0.5317885517331613
-        },
-        {
-            "x": 2.252001935551058,
-            "y": 5.607878550007709,
-            "heading": 0.012794605457377246,
-            "angularVelocity": -0.4158132948561684,
-            "velocityX": -0.05270332618167983,
-            "velocityY": -1.2604627825518706,
-            "timestamp": 0.6077583448378986
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": -0.20790639031492017,
-            "velocityX": -0.026351730987313093,
-            "velocityY": -0.6302314123944018,
-            "timestamp": 0.6837281379426359
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": -6.008173298568887e-34,
-            "velocityX": -7.305982513747354e-33,
-            "velocityY": -3.8813573012664696e-35,
-            "timestamp": 0.7596979310473732
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.traj
deleted file mode 100644
index a5fd0bb0..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 1.792103156985809e-33,
-            "velocityX": -7.27073415334495e-34,
-            "velocityY": -7.853383039035825e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.298001341213542,
-            "y": 6.709118496985434,
-            "heading": 0.374359095500822,
-            "angularVelocity": -0.2058832051525312,
-            "velocityX": -0.02630859852023658,
-            "velocityY": -0.6302702831973344,
-            "timestamp": 0.07596979310473732
-        },
-        {
-            "x": 2.294004039374054,
-            "y": 6.613355753676287,
-            "heading": 0.3430620070158405,
-            "angularVelocity": -0.4119675361210102,
-            "velocityX": -0.0526169899393711,
-            "velocityY": -1.2605371081782701,
-            "timestamp": 0.15193958620947465
-        },
-        {
-            "x": 2.2880080040813597,
-            "y": 6.469712250400375,
-            "heading": 0.296082691458658,
-            "angularVelocity": -0.6183946755312791,
-            "velocityX": -0.07892657130746197,
-            "velocityY": -1.8907976105433808,
-            "timestamp": 0.22790937931421196
-        },
-        {
-            "x": 2.280012894319733,
-            "y": 6.278188663497132,
-            "heading": 0.23338596481601495,
-            "angularVelocity": -0.8252849465603376,
-            "velocityX": -0.10524064150870725,
-            "velocityY": -2.5210492101669324,
-            "timestamp": 0.3038791724189493
-        },
-        {
-            "x": 2.270017931664709,
-            "y": 6.038785816229407,
-            "heading": 0.1549313176962556,
-            "angularVelocity": -1.0327084478378958,
-            "velocityX": -0.13156495820969516,
-            "velocityY": -3.1512899730774246,
-            "timestamp": 0.37984896552368663
-        },
-        {
-            "x": 2.2620113109689415,
-            "y": 5.847271344563018,
-            "heading": 0.09176539516362914,
-            "angularVelocity": -0.8314610314331311,
-            "velocityX": -0.10539216139142096,
-            "velocityY": -2.5209292251507525,
-            "timestamp": 0.45581875862842397
-        },
-        {
-            "x": 2.2560057963370115,
-            "y": 5.703635646814395,
-            "heading": 0.0443838554377995,
-            "angularVelocity": -0.6236892031613949,
-            "velocityX": -0.079051349049357,
-            "velocityY": -1.8906948653999391,
-            "timestamp": 0.5317885517331613
-        },
-        {
-            "x": 2.252001935551058,
-            "y": 5.607878550007709,
-            "heading": 0.012794605457377246,
-            "angularVelocity": -0.4158132948561684,
-            "velocityX": -0.05270332618167983,
-            "velocityY": -1.2604627825518706,
-            "timestamp": 0.6077583448378986
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": -0.20790639031492017,
-            "velocityX": -0.026351730987313093,
-            "velocityY": -0.6302314123944018,
-            "timestamp": 0.6837281379426359
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": -6.008173298568887e-34,
-            "velocityX": -7.305982513747354e-33,
-            "velocityY": -3.8813573012664696e-35,
-            "timestamp": 0.7596979310473732
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.1.traj
deleted file mode 100644
index 5aab1ee6..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": -2.4663146867319515e-36,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3001378208847427,
-            "y": 6.713171893956039,
-            "heading": 0.36291265694481545,
-            "angularVelocity": -0.37089801282350154,
-            "velocityX": 0.0018871357066083216,
-            "velocityY": -0.6001237332286558,
-            "timestamp": 0.0730317826428328
-        },
-        {
-            "x": 2.3004082222445,
-            "y": 6.625427179140099,
-            "heading": 0.3106389081779897,
-            "angularVelocity": -0.7157671204942956,
-            "velocityX": 0.0037025162192713427,
-            "velocityY": -1.201459304985902,
-            "timestamp": 0.1460635652856656
-        },
-        {
-            "x": 2.300808308388558,
-            "y": 6.493639269598709,
-            "heading": 0.23621859479744584,
-            "angularVelocity": -1.019012691289514,
-            "velocityX": 0.005478246998500719,
-            "velocityY": -1.804528176258669,
-            "timestamp": 0.2190953479284984
-        },
-        {
-            "x": 2.30134752968001,
-            "y": 6.317621999647275,
-            "heading": 0.14515321533996162,
-            "angularVelocity": -1.2469280655909227,
-            "velocityX": 0.007383378468093947,
-            "velocityY": -2.4101461525629078,
-            "timestamp": 0.2921271305713312
-        },
-        {
-            "x": 2.3020704977057354,
-            "y": 6.097150895386178,
-            "heading": 0.04952404086318575,
-            "angularVelocity": -1.309418598536164,
-            "velocityX": 0.00989936161439861,
-            "velocityY": -3.0188377755932185,
-            "timestamp": 0.36515891321416405
-        },
-        {
-            "x": 2.3031517302727234,
-            "y": 5.834309739739576,
-            "heading": 0.004115026920641748,
-            "angularVelocity": -0.6217705812361183,
-            "velocityX": 0.014804959263771778,
-            "velocityY": -3.598996849522972,
-            "timestamp": 0.4381906958569969
-        },
-        {
-            "x": 2.303521267718626,
-            "y": 5.558035105167069,
-            "heading": 0.00411499765356137,
-            "angularVelocity": -4.007444337013033e-7,
-            "velocityX": 0.005059953797233799,
-            "velocityY": -3.7829370251531125,
-            "timestamp": 0.5112224784998297
-        },
-        {
-            "x": 2.3038907744757537,
-            "y": 5.281760470553516,
-            "heading": 0.004114968386478979,
-            "angularVelocity": -4.0074446127092794e-7,
-            "velocityX": 0.005059533586019425,
-            "velocityY": -3.782937025715139,
-            "timestamp": 0.5842542611426625
-        },
-        {
-            "x": 2.3020058670727765,
-            "y": 5.019317688425813,
-            "heading": -0.04121766182368216,
-            "angularVelocity": -0.6207246840990261,
-            "velocityX": -0.02580941248819387,
-            "velocityY": -3.5935420529332935,
-            "timestamp": 0.6572860437854954
-        },
-        {
-            "x": 2.301170882315387,
-            "y": 4.798980955675817,
-            "heading": -0.139726223563735,
-            "angularVelocity": -1.348845094221732,
-            "velocityX": -0.011433169603334988,
-            "velocityY": -3.016997871016921,
-            "timestamp": 0.7303178264283282
-        },
-        {
-            "x": 2.3006513716532107,
-            "y": 4.623098256624685,
-            "heading": -0.23378951684706242,
-            "angularVelocity": -1.2879775062228829,
-            "velocityX": -0.007113487352719777,
-            "velocityY": -2.4083035178163246,
-            "timestamp": 0.803349609071161
-        },
-        {
-            "x": 2.300310703422249,
-            "y": 4.491429967185545,
-            "heading": -0.31073488166813007,
-            "angularVelocity": -1.0535873839664371,
-            "velocityX": -0.004664657203118652,
-            "velocityY": -1.8028902578357346,
-            "timestamp": 0.8763813917139939
-        },
-        {
-            "x": 2.3001003821722423,
-            "y": 4.403776446553788,
-            "heading": -0.36487144716496317,
-            "angularVelocity": -0.7412740527174575,
-            "velocityX": -0.0028798591845282696,
-            "velocityY": -1.2002106132398922,
-            "timestamp": 0.9494131743568267
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.393,
-            "angularVelocity": -0.3851549533249317,
-            "velocityX": -0.0013744998219966374,
-            "velocityY": -0.5994163769475577,
-            "timestamp": 1.0224449569996594
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.393,
-            "angularVelocity": -7.841220088894993e-36,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 1.0954767396424923
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.traj
deleted file mode 100644
index 5aab1ee6..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": -2.4663146867319515e-36,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3001378208847427,
-            "y": 6.713171893956039,
-            "heading": 0.36291265694481545,
-            "angularVelocity": -0.37089801282350154,
-            "velocityX": 0.0018871357066083216,
-            "velocityY": -0.6001237332286558,
-            "timestamp": 0.0730317826428328
-        },
-        {
-            "x": 2.3004082222445,
-            "y": 6.625427179140099,
-            "heading": 0.3106389081779897,
-            "angularVelocity": -0.7157671204942956,
-            "velocityX": 0.0037025162192713427,
-            "velocityY": -1.201459304985902,
-            "timestamp": 0.1460635652856656
-        },
-        {
-            "x": 2.300808308388558,
-            "y": 6.493639269598709,
-            "heading": 0.23621859479744584,
-            "angularVelocity": -1.019012691289514,
-            "velocityX": 0.005478246998500719,
-            "velocityY": -1.804528176258669,
-            "timestamp": 0.2190953479284984
-        },
-        {
-            "x": 2.30134752968001,
-            "y": 6.317621999647275,
-            "heading": 0.14515321533996162,
-            "angularVelocity": -1.2469280655909227,
-            "velocityX": 0.007383378468093947,
-            "velocityY": -2.4101461525629078,
-            "timestamp": 0.2921271305713312
-        },
-        {
-            "x": 2.3020704977057354,
-            "y": 6.097150895386178,
-            "heading": 0.04952404086318575,
-            "angularVelocity": -1.309418598536164,
-            "velocityX": 0.00989936161439861,
-            "velocityY": -3.0188377755932185,
-            "timestamp": 0.36515891321416405
-        },
-        {
-            "x": 2.3031517302727234,
-            "y": 5.834309739739576,
-            "heading": 0.004115026920641748,
-            "angularVelocity": -0.6217705812361183,
-            "velocityX": 0.014804959263771778,
-            "velocityY": -3.598996849522972,
-            "timestamp": 0.4381906958569969
-        },
-        {
-            "x": 2.303521267718626,
-            "y": 5.558035105167069,
-            "heading": 0.00411499765356137,
-            "angularVelocity": -4.007444337013033e-7,
-            "velocityX": 0.005059953797233799,
-            "velocityY": -3.7829370251531125,
-            "timestamp": 0.5112224784998297
-        },
-        {
-            "x": 2.3038907744757537,
-            "y": 5.281760470553516,
-            "heading": 0.004114968386478979,
-            "angularVelocity": -4.0074446127092794e-7,
-            "velocityX": 0.005059533586019425,
-            "velocityY": -3.782937025715139,
-            "timestamp": 0.5842542611426625
-        },
-        {
-            "x": 2.3020058670727765,
-            "y": 5.019317688425813,
-            "heading": -0.04121766182368216,
-            "angularVelocity": -0.6207246840990261,
-            "velocityX": -0.02580941248819387,
-            "velocityY": -3.5935420529332935,
-            "timestamp": 0.6572860437854954
-        },
-        {
-            "x": 2.301170882315387,
-            "y": 4.798980955675817,
-            "heading": -0.139726223563735,
-            "angularVelocity": -1.348845094221732,
-            "velocityX": -0.011433169603334988,
-            "velocityY": -3.016997871016921,
-            "timestamp": 0.7303178264283282
-        },
-        {
-            "x": 2.3006513716532107,
-            "y": 4.623098256624685,
-            "heading": -0.23378951684706242,
-            "angularVelocity": -1.2879775062228829,
-            "velocityX": -0.007113487352719777,
-            "velocityY": -2.4083035178163246,
-            "timestamp": 0.803349609071161
-        },
-        {
-            "x": 2.300310703422249,
-            "y": 4.491429967185545,
-            "heading": -0.31073488166813007,
-            "angularVelocity": -1.0535873839664371,
-            "velocityX": -0.004664657203118652,
-            "velocityY": -1.8028902578357346,
-            "timestamp": 0.8763813917139939
-        },
-        {
-            "x": 2.3001003821722423,
-            "y": 4.403776446553788,
-            "heading": -0.36487144716496317,
-            "angularVelocity": -0.7412740527174575,
-            "velocityX": -0.0028798591845282696,
-            "velocityY": -1.2002106132398922,
-            "timestamp": 0.9494131743568267
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.393,
-            "angularVelocity": -0.3851549533249317,
-            "velocityX": -0.0013744998219966374,
-            "velocityY": -0.5994163769475577,
-            "timestamp": 1.0224449569996594
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.393,
-            "angularVelocity": -7.841220088894993e-36,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 1.0954767396424923
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.1.traj
deleted file mode 100644
index f6df5a01..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.1.traj	
+++ /dev/null
@@ -1,292 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -5.349509897792121e-32,
-            "angularVelocity": -4.120122643775229e-32,
-            "velocityX": 8.47273432885147e-31,
-            "velocityY": -1.4633096492322666e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 2.237003637990514,
-            "y": 5.5993039448743,
-            "heading": -2.4015234029354595e-10,
-            "angularVelocity": -3.4082853646292762e-9,
-            "velocityX": -0.1844472530475389,
-            "velocityY": 0.5578103057326728,
-            "timestamp": 0.070461130729763
-        },
-        {
-            "x": 2.214179391204466,
-            "y": 5.678821295080181,
-            "heading": -7.831932260526302e-10,
-            "angularVelocity": -7.70693396452574e-9,
-            "velocityX": -0.32392677423960453,
-            "velocityY": 1.1285278760503061,
-            "timestamp": 0.140922261459526
-        },
-        {
-            "x": 2.1888413071411663,
-            "y": 5.799659166026344,
-            "heading": -1.7630537350139963e-9,
-            "angularVelocity": -1.3906355788165327e-8,
-            "velocityX": -0.3596037106073644,
-            "velocityY": 1.714957873876257,
-            "timestamp": 0.21138339218928898
-        },
-        {
-            "x": 2.184731423610446,
-            "y": 5.956036692159833,
-            "heading": -3.5093546140825654e-9,
-            "angularVelocity": -2.4783811808465378e-8,
-            "velocityX": -0.0583283788886903,
-            "velocityY": 2.219344545207332,
-            "timestamp": 0.281844522919052
-        },
-        {
-            "x": 2.2210562385399535,
-            "y": 6.10354071192916,
-            "heading": -5.82104685853391e-9,
-            "angularVelocity": -3.2807944511631915e-8,
-            "velocityX": 0.5155298325992919,
-            "velocityY": 2.0934097741478808,
-            "timestamp": 0.352305653648815
-        },
-        {
-            "x": 2.290250351402012,
-            "y": 6.2258789967414945,
-            "heading": -8.381712631190757e-9,
-            "angularVelocity": -3.6341419943068046e-8,
-            "velocityX": 0.9820182013148953,
-            "velocityY": 1.7362520802853605,
-            "timestamp": 0.422766784378578
-        },
-        {
-            "x": 2.38835072517395,
-            "y": 6.318583965301514,
-            "heading": -1.1074238763723672e-8,
-            "angularVelocity": -3.821280465498069e-8,
-            "velocityX": 1.392262269357751,
-            "velocityY": 1.315689481497163,
-            "timestamp": 0.493227915108341
-        },
-        {
-            "x": 2.4799014983496455,
-            "y": 6.371301429692481,
-            "heading": -1.319458225812021e-8,
-            "angularVelocity": -3.903749361340101e-8,
-            "velocityX": 1.6855317920452093,
-            "velocityY": 0.9705757706637901,
-            "timestamp": 0.5475435759628242
-        },
-        {
-            "x": 2.5874215406057264,
-            "y": 6.405308001046697,
-            "heading": -1.535577963780212e-8,
-            "angularVelocity": -3.978965207197302e-8,
-            "velocityX": 1.979540349233062,
-            "velocityY": 0.626091459097589,
-            "timestamp": 0.6018592368173074
-        },
-        {
-            "x": 2.7109670001208555,
-            "y": 6.420651747906481,
-            "heading": -1.7552309308039307e-8,
-            "angularVelocity": -4.044015305772357e-8,
-            "velocityX": 2.2745826447141577,
-            "velocityY": 0.2824921324780203,
-            "timestamp": 0.6561748976717905
-        },
-        {
-            "x": 2.850621987061303,
-            "y": 6.417405226954699,
-            "heading": -1.9775875083794707e-8,
-            "angularVelocity": -4.093791389017331e-8,
-            "velocityX": 2.571173483741952,
-            "velocityY": -0.059771360579454216,
-            "timestamp": 0.7104905585262736
-        },
-        {
-            "x": 3.006526369448995,
-            "y": 6.395690573007363,
-            "heading": -2.2012612720253862e-8,
-            "angularVelocity": -4.1180421619075874e-8,
-            "velocityX": 2.8703394184132693,
-            "velocityY": -0.39978624220811715,
-            "timestamp": 0.7648062193807568
-        },
-        {
-            "x": 3.178958586879986,
-            "y": 6.35575655651524,
-            "heading": -2.4234645855460274e-8,
-            "angularVelocity": -4.090970062204104e-8,
-            "velocityX": 3.1746316756057436,
-            "velocityY": -0.7352210368724164,
-            "timestamp": 0.8191218802352399
-        },
-        {
-            "x": 3.368741313034932,
-            "y": 6.298384837056506,
-            "heading": -2.6356859092562007e-8,
-            "angularVelocity": -3.9071928501624834e-8,
-            "velocityX": 3.494070092598876,
-            "velocityY": -1.0562647781255272,
-            "timestamp": 0.873437541089723
-        },
-        {
-            "x": 3.5700972080230713,
-            "y": 6.2574591636657715,
-            "heading": -2.635685976829305e-8,
-            "angularVelocity": -1.2440816646696897e-14,
-            "velocityX": 3.7071425040054167,
-            "velocityY": -0.7534783293344505,
-            "timestamp": 0.9277532019442062
-        },
-        {
-            "x": 3.903312814324664,
-            "y": 6.255439725872096,
-            "heading": -2.6356859842955344e-8,
-            "angularVelocity": -8.476143761744691e-16,
-            "velocityX": 3.7828709995267413,
-            "velocityY": -0.02292591506032139,
-            "timestamp": 1.0158385758158621
-        },
-        {
-            "x": 4.231215212285405,
-            "y": 6.314741822504602,
-            "heading": -2.6356859970624137e-8,
-            "angularVelocity": -1.4493758784250442e-15,
-            "velocityX": 3.722552150839135,
-            "velocityY": 0.6732343181536394,
-            "timestamp": 1.103923949687518
-        },
-        {
-            "x": 4.548594281876572,
-            "y": 6.416266440930298,
-            "heading": -2.6357038616847798e-8,
-            "angularVelocity": -2.0281055655946166e-12,
-            "velocityX": 3.603084776124604,
-            "velocityY": 1.1525706703347043,
-            "timestamp": 1.1920093235591738
-        },
-        {
-            "x": 4.864492921080643,
-            "y": 6.522307835013861,
-            "heading": -2.6356860621748925e-8,
-            "angularVelocity": 2.02071358534527e-12,
-            "velocityX": 3.5862780084505173,
-            "velocityY": 1.2038479203550279,
-            "timestamp": 1.2800946974308296
-        },
-        {
-            "x": 5.180407145227995,
-            "y": 6.628302763705304,
-            "heading": -2.6356859722122177e-8,
-            "angularVelocity": 1.021313250736518e-14,
-            "velocityX": 3.586454938732789,
-            "velocityY": 1.2033204153921748,
-            "timestamp": 1.3681800713024854
-        },
-        {
-            "x": 5.4963196348481045,
-            "y": 6.734302860663818,
-            "heading": -2.6356858783882045e-8,
-            "angularVelocity": 1.0651495820397439e-14,
-            "velocityX": 3.58643524725062,
-            "velocityY": 1.20337908892381,
-            "timestamp": 1.4562654451741412
-        },
-        {
-            "x": 5.81223242108415,
-            "y": 6.8403020736225555,
-            "heading": -2.6356857847826716e-8,
-            "angularVelocity": 1.0626692486301617e-14,
-            "velocityX": 3.586438614621235,
-            "velocityY": 1.2033690532024592,
-            "timestamp": 1.544350819045797
-        },
-        {
-            "x": 6.128145213013902,
-            "y": 6.946301269622288,
-            "heading": -2.6356856889534724e-8,
-            "angularVelocity": 1.0879135888139458e-14,
-            "velocityX": 3.5864386792612257,
-            "velocityY": 1.2033688606688582,
-            "timestamp": 1.632436192917453
-        },
-        {
-            "x": 6.444057998192988,
-            "y": 7.052300485742325,
-            "heading": -2.6356856001580027e-8,
-            "angularVelocity": 1.0080624290280854e-14,
-            "velocityX": 3.5864386026236104,
-            "velocityY": 1.2033690890866424,
-            "timestamp": 1.7205215667891087
-        },
-        {
-            "x": 6.759970783516966,
-            "y": 7.158299701416182,
-            "heading": -2.635685511579164e-8,
-            "angularVelocity": 1.0056027630784321e-14,
-            "velocityX": 3.586438604268464,
-            "velocityY": 1.2033690840214906,
-            "timestamp": 1.8086069406607646
-        },
-        {
-            "x": 7.0666471679768685,
-            "y": 7.261199790661366,
-            "heading": -1.748457582258916e-8,
-            "angularVelocity": 1.0072373848515286e-7,
-            "velocityX": 3.4815812317231813,
-            "velocityY": 1.1681858715050373,
-            "timestamp": 1.8966923145324204
-        },
-        {
-            "x": 7.311988294075674,
-            "y": 7.343519871122703,
-            "heading": -1.0463867035430883e-8,
-            "angularVelocity": 7.970353325086049e-8,
-            "velocityX": 2.7852651957526704,
-            "velocityY": 0.9345488001177432,
-            "timestamp": 1.9847776884040762
-        },
-        {
-            "x": 7.495994144762843,
-            "y": 7.405259934452133,
-            "heading": -5.222908649794349e-9,
-            "angularVelocity": 5.94986783168245e-8,
-            "velocityX": 2.088948966213813,
-            "velocityY": 0.7009116339580341,
-            "timestamp": 2.072863062275732
-        },
-        {
-            "x": 7.618664714352163,
-            "y": 7.446419977873495,
-            "heading": -1.738724662079323e-9,
-            "angularVelocity": 3.955466249815561e-8,
-            "velocityX": 1.3926326721215396,
-            "velocityY": 0.4672744362816174,
-            "timestamp": 2.160948436147388
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -8.289937690891028e-31,
-            "angularVelocity": 1.9739103024170296e-8,
-            "velocityX": 0.6963163457465601,
-            "velocityY": 0.23363722286151184,
-            "timestamp": 2.2490338100190437
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 1.1211425849455439e-31,
-            "angularVelocity": 1.0684035256675139e-29,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 2.3371191838906995
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.traj
deleted file mode 100644
index f6df5a01..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.traj	
+++ /dev/null
@@ -1,292 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -5.349509897792121e-32,
-            "angularVelocity": -4.120122643775229e-32,
-            "velocityX": 8.47273432885147e-31,
-            "velocityY": -1.4633096492322666e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 2.237003637990514,
-            "y": 5.5993039448743,
-            "heading": -2.4015234029354595e-10,
-            "angularVelocity": -3.4082853646292762e-9,
-            "velocityX": -0.1844472530475389,
-            "velocityY": 0.5578103057326728,
-            "timestamp": 0.070461130729763
-        },
-        {
-            "x": 2.214179391204466,
-            "y": 5.678821295080181,
-            "heading": -7.831932260526302e-10,
-            "angularVelocity": -7.70693396452574e-9,
-            "velocityX": -0.32392677423960453,
-            "velocityY": 1.1285278760503061,
-            "timestamp": 0.140922261459526
-        },
-        {
-            "x": 2.1888413071411663,
-            "y": 5.799659166026344,
-            "heading": -1.7630537350139963e-9,
-            "angularVelocity": -1.3906355788165327e-8,
-            "velocityX": -0.3596037106073644,
-            "velocityY": 1.714957873876257,
-            "timestamp": 0.21138339218928898
-        },
-        {
-            "x": 2.184731423610446,
-            "y": 5.956036692159833,
-            "heading": -3.5093546140825654e-9,
-            "angularVelocity": -2.4783811808465378e-8,
-            "velocityX": -0.0583283788886903,
-            "velocityY": 2.219344545207332,
-            "timestamp": 0.281844522919052
-        },
-        {
-            "x": 2.2210562385399535,
-            "y": 6.10354071192916,
-            "heading": -5.82104685853391e-9,
-            "angularVelocity": -3.2807944511631915e-8,
-            "velocityX": 0.5155298325992919,
-            "velocityY": 2.0934097741478808,
-            "timestamp": 0.352305653648815
-        },
-        {
-            "x": 2.290250351402012,
-            "y": 6.2258789967414945,
-            "heading": -8.381712631190757e-9,
-            "angularVelocity": -3.6341419943068046e-8,
-            "velocityX": 0.9820182013148953,
-            "velocityY": 1.7362520802853605,
-            "timestamp": 0.422766784378578
-        },
-        {
-            "x": 2.38835072517395,
-            "y": 6.318583965301514,
-            "heading": -1.1074238763723672e-8,
-            "angularVelocity": -3.821280465498069e-8,
-            "velocityX": 1.392262269357751,
-            "velocityY": 1.315689481497163,
-            "timestamp": 0.493227915108341
-        },
-        {
-            "x": 2.4799014983496455,
-            "y": 6.371301429692481,
-            "heading": -1.319458225812021e-8,
-            "angularVelocity": -3.903749361340101e-8,
-            "velocityX": 1.6855317920452093,
-            "velocityY": 0.9705757706637901,
-            "timestamp": 0.5475435759628242
-        },
-        {
-            "x": 2.5874215406057264,
-            "y": 6.405308001046697,
-            "heading": -1.535577963780212e-8,
-            "angularVelocity": -3.978965207197302e-8,
-            "velocityX": 1.979540349233062,
-            "velocityY": 0.626091459097589,
-            "timestamp": 0.6018592368173074
-        },
-        {
-            "x": 2.7109670001208555,
-            "y": 6.420651747906481,
-            "heading": -1.7552309308039307e-8,
-            "angularVelocity": -4.044015305772357e-8,
-            "velocityX": 2.2745826447141577,
-            "velocityY": 0.2824921324780203,
-            "timestamp": 0.6561748976717905
-        },
-        {
-            "x": 2.850621987061303,
-            "y": 6.417405226954699,
-            "heading": -1.9775875083794707e-8,
-            "angularVelocity": -4.093791389017331e-8,
-            "velocityX": 2.571173483741952,
-            "velocityY": -0.059771360579454216,
-            "timestamp": 0.7104905585262736
-        },
-        {
-            "x": 3.006526369448995,
-            "y": 6.395690573007363,
-            "heading": -2.2012612720253862e-8,
-            "angularVelocity": -4.1180421619075874e-8,
-            "velocityX": 2.8703394184132693,
-            "velocityY": -0.39978624220811715,
-            "timestamp": 0.7648062193807568
-        },
-        {
-            "x": 3.178958586879986,
-            "y": 6.35575655651524,
-            "heading": -2.4234645855460274e-8,
-            "angularVelocity": -4.090970062204104e-8,
-            "velocityX": 3.1746316756057436,
-            "velocityY": -0.7352210368724164,
-            "timestamp": 0.8191218802352399
-        },
-        {
-            "x": 3.368741313034932,
-            "y": 6.298384837056506,
-            "heading": -2.6356859092562007e-8,
-            "angularVelocity": -3.9071928501624834e-8,
-            "velocityX": 3.494070092598876,
-            "velocityY": -1.0562647781255272,
-            "timestamp": 0.873437541089723
-        },
-        {
-            "x": 3.5700972080230713,
-            "y": 6.2574591636657715,
-            "heading": -2.635685976829305e-8,
-            "angularVelocity": -1.2440816646696897e-14,
-            "velocityX": 3.7071425040054167,
-            "velocityY": -0.7534783293344505,
-            "timestamp": 0.9277532019442062
-        },
-        {
-            "x": 3.903312814324664,
-            "y": 6.255439725872096,
-            "heading": -2.6356859842955344e-8,
-            "angularVelocity": -8.476143761744691e-16,
-            "velocityX": 3.7828709995267413,
-            "velocityY": -0.02292591506032139,
-            "timestamp": 1.0158385758158621
-        },
-        {
-            "x": 4.231215212285405,
-            "y": 6.314741822504602,
-            "heading": -2.6356859970624137e-8,
-            "angularVelocity": -1.4493758784250442e-15,
-            "velocityX": 3.722552150839135,
-            "velocityY": 0.6732343181536394,
-            "timestamp": 1.103923949687518
-        },
-        {
-            "x": 4.548594281876572,
-            "y": 6.416266440930298,
-            "heading": -2.6357038616847798e-8,
-            "angularVelocity": -2.0281055655946166e-12,
-            "velocityX": 3.603084776124604,
-            "velocityY": 1.1525706703347043,
-            "timestamp": 1.1920093235591738
-        },
-        {
-            "x": 4.864492921080643,
-            "y": 6.522307835013861,
-            "heading": -2.6356860621748925e-8,
-            "angularVelocity": 2.02071358534527e-12,
-            "velocityX": 3.5862780084505173,
-            "velocityY": 1.2038479203550279,
-            "timestamp": 1.2800946974308296
-        },
-        {
-            "x": 5.180407145227995,
-            "y": 6.628302763705304,
-            "heading": -2.6356859722122177e-8,
-            "angularVelocity": 1.021313250736518e-14,
-            "velocityX": 3.586454938732789,
-            "velocityY": 1.2033204153921748,
-            "timestamp": 1.3681800713024854
-        },
-        {
-            "x": 5.4963196348481045,
-            "y": 6.734302860663818,
-            "heading": -2.6356858783882045e-8,
-            "angularVelocity": 1.0651495820397439e-14,
-            "velocityX": 3.58643524725062,
-            "velocityY": 1.20337908892381,
-            "timestamp": 1.4562654451741412
-        },
-        {
-            "x": 5.81223242108415,
-            "y": 6.8403020736225555,
-            "heading": -2.6356857847826716e-8,
-            "angularVelocity": 1.0626692486301617e-14,
-            "velocityX": 3.586438614621235,
-            "velocityY": 1.2033690532024592,
-            "timestamp": 1.544350819045797
-        },
-        {
-            "x": 6.128145213013902,
-            "y": 6.946301269622288,
-            "heading": -2.6356856889534724e-8,
-            "angularVelocity": 1.0879135888139458e-14,
-            "velocityX": 3.5864386792612257,
-            "velocityY": 1.2033688606688582,
-            "timestamp": 1.632436192917453
-        },
-        {
-            "x": 6.444057998192988,
-            "y": 7.052300485742325,
-            "heading": -2.6356856001580027e-8,
-            "angularVelocity": 1.0080624290280854e-14,
-            "velocityX": 3.5864386026236104,
-            "velocityY": 1.2033690890866424,
-            "timestamp": 1.7205215667891087
-        },
-        {
-            "x": 6.759970783516966,
-            "y": 7.158299701416182,
-            "heading": -2.635685511579164e-8,
-            "angularVelocity": 1.0056027630784321e-14,
-            "velocityX": 3.586438604268464,
-            "velocityY": 1.2033690840214906,
-            "timestamp": 1.8086069406607646
-        },
-        {
-            "x": 7.0666471679768685,
-            "y": 7.261199790661366,
-            "heading": -1.748457582258916e-8,
-            "angularVelocity": 1.0072373848515286e-7,
-            "velocityX": 3.4815812317231813,
-            "velocityY": 1.1681858715050373,
-            "timestamp": 1.8966923145324204
-        },
-        {
-            "x": 7.311988294075674,
-            "y": 7.343519871122703,
-            "heading": -1.0463867035430883e-8,
-            "angularVelocity": 7.970353325086049e-8,
-            "velocityX": 2.7852651957526704,
-            "velocityY": 0.9345488001177432,
-            "timestamp": 1.9847776884040762
-        },
-        {
-            "x": 7.495994144762843,
-            "y": 7.405259934452133,
-            "heading": -5.222908649794349e-9,
-            "angularVelocity": 5.94986783168245e-8,
-            "velocityX": 2.088948966213813,
-            "velocityY": 0.7009116339580341,
-            "timestamp": 2.072863062275732
-        },
-        {
-            "x": 7.618664714352163,
-            "y": 7.446419977873495,
-            "heading": -1.738724662079323e-9,
-            "angularVelocity": 3.955466249815561e-8,
-            "velocityX": 1.3926326721215396,
-            "velocityY": 0.4672744362816174,
-            "timestamp": 2.160948436147388
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -8.289937690891028e-31,
-            "angularVelocity": 1.9739103024170296e-8,
-            "velocityX": 0.6963163457465601,
-            "velocityY": 0.23363722286151184,
-            "timestamp": 2.2490338100190437
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 1.1211425849455439e-31,
-            "angularVelocity": 1.0684035256675139e-29,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 2.3371191838906995
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.1.traj
deleted file mode 100644
index 7a28104f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.1.traj	
+++ /dev/null
@@ -1,616 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 5.330261523342824e-32,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.216523220338838,
-            "y": 5.608939409819235,
-            "heading": -0.000395848810607471,
-            "angularVelocity": -0.004694167537861253,
-            "velocityX": -0.3969839194829395,
-            "velocityY": 0.5803473011402309,
-            "timestamp": 0.08432779772229798
-        },
-        {
-            "x": 2.1530740053514283,
-            "y": 5.7090393916763,
-            "heading": -0.0013136261454272055,
-            "angularVelocity": -0.010883449581379904,
-            "velocityX": -0.7524116210926717,
-            "velocityY": 1.1870342231242381,
-            "timestamp": 0.16865559544459596
-        },
-        {
-            "x": 2.071477218025725,
-            "y": 5.86558734321221,
-            "heading": -0.0031284327789465032,
-            "angularVelocity": -0.02152085886904205,
-            "velocityX": -0.9676143517279073,
-            "velocityY": 1.8564216754650953,
-            "timestamp": 0.25298339316689394
-        },
-        {
-            "x": 2.0458269370451427,
-            "y": 6.041770495606775,
-            "heading": -0.006595224749493174,
-            "angularVelocity": -0.04111090368993278,
-            "velocityX": -0.30417349525777404,
-            "velocityY": 2.089265427928733,
-            "timestamp": 0.3373111908891919
-        },
-        {
-            "x": 2.0715234655116643,
-            "y": 6.188301290095102,
-            "heading": -0.01049068188832321,
-            "angularVelocity": -0.046194223542478534,
-            "velocityX": 0.30472192041755103,
-            "velocityY": 1.7376333598900595,
-            "timestamp": 0.4216389886114899
-        },
-        {
-            "x": 2.1438515186309814,
-            "y": 6.298208713531494,
-            "heading": -0.014554322717054646,
-            "angularVelocity": -0.04818862745724922,
-            "velocityX": 0.8577011978600798,
-            "velocityY": 1.303335630776598,
-            "timestamp": 0.5059667863337879
-        },
-        {
-            "x": 2.2162950329491684,
-            "y": 6.355974505839093,
-            "heading": -0.017431077998725047,
-            "angularVelocity": -0.04874949538291163,
-            "velocityX": 1.2276278031981451,
-            "velocityY": 0.978899124069331,
-            "timestamp": 0.5649777619156877
-        },
-        {
-            "x": 2.310686009240105,
-            "y": 6.394730061346725,
-            "heading": -0.020327199149560698,
-            "angularVelocity": -0.0490776694043422,
-            "velocityX": 1.5995494966853068,
-            "velocityY": 0.6567516487478545,
-            "timestamp": 0.6239887374975874
-        },
-        {
-            "x": 2.4271886355684367,
-            "y": 6.414666608347948,
-            "heading": -0.023223370555002502,
-            "angularVelocity": -0.04907852101890474,
-            "velocityX": 1.9742535211376202,
-            "velocityY": 0.33784472811424154,
-            "timestamp": 0.6829997130794871
-        },
-        {
-            "x": 2.5660476863967387,
-            "y": 6.416075601325133,
-            "heading": -0.02609042518710973,
-            "angularVelocity": -0.048585108174133695,
-            "velocityX": 2.35310549366504,
-            "velocityY": 0.02387679517735602,
-            "timestamp": 0.7420106886613869
-        },
-        {
-            "x": 2.72766661902605,
-            "y": 6.399454975329908,
-            "heading": -0.02887926513240676,
-            "angularVelocity": -0.04725968208112396,
-            "velocityX": 2.738794453669127,
-            "velocityY": -0.281653130308932,
-            "timestamp": 0.8010216642432866
-        },
-        {
-            "x": 2.912832200300888,
-            "y": 6.365845166245685,
-            "heading": -0.031490096116184224,
-            "angularVelocity": -0.04424314219571676,
-            "velocityX": 3.1378159647242496,
-            "velocityY": -0.5695518291775498,
-            "timestamp": 0.8600326398251863
-        },
-        {
-            "x": 3.123679163604623,
-            "y": 6.318688161285352,
-            "heading": -0.033616023768519236,
-            "angularVelocity": -0.03602597027708684,
-            "velocityX": 3.5730126679757386,
-            "velocityY": -0.7991226122822729,
-            "timestamp": 0.919043615407086
-        },
-        {
-            "x": 3.345972776412964,
-            "y": 6.298208713531494,
-            "heading": -0.03361602546428586,
-            "angularVelocity": -2.8736461454443448e-8,
-            "velocityX": 3.766987591991692,
-            "velocityY": -0.34704472434005124,
-            "timestamp": 0.9780545909889857
-        },
-        {
-            "x": 3.4285825306560853,
-            "y": 6.294597584481868,
-            "heading": -0.033616025709707474,
-            "angularVelocity": -1.1227840838251836e-8,
-            "velocityX": 3.779329612797739,
-            "velocityY": -0.16520624081174923,
-            "timestamp": 0.9999128999238256
-        },
-        {
-            "x": 3.5112703400714786,
-            "y": 6.2949695138762145,
-            "heading": -0.033616025930811776,
-            "angularVelocity": -1.0115343355589466e-8,
-            "velocityX": 3.7829005739596693,
-            "velocityY": 0.01701546974451014,
-            "timestamp": 1.0217712088586655
-        },
-        {
-            "x": 3.5938442713679413,
-            "y": 6.299323633866166,
-            "heading": -0.03361602613301592,
-            "angularVelocity": -9.2506762233922e-9,
-            "velocityX": 3.7776907418875507,
-            "velocityY": 0.19919747693799758,
-            "timestamp": 1.0436295177935053
-        },
-        {
-            "x": 3.6761126565475006,
-            "y": 6.3076498312062705,
-            "heading": -0.03361602632044191,
-            "angularVelocity": -8.574588182780684e-9,
-            "velocityX": 3.763712253532677,
-            "velocityY": 0.3809168113107395,
-            "timestamp": 1.0654878267283452
-        },
-        {
-            "x": 3.7578845383740482,
-            "y": 6.31992876945557,
-            "heading": -0.033616026496316675,
-            "angularVelocity": -8.046128410971048e-9,
-            "velocityX": 3.7409976256768736,
-            "velocityY": 0.5617515191089957,
-            "timestamp": 1.087346135663185
-        },
-        {
-            "x": 3.8389701148011586,
-            "y": 6.336131931270735,
-            "heading": -0.03361602666323835,
-            "angularVelocity": -7.63653221094489e-9,
-            "velocityX": 3.709599707316274,
-            "velocityY": 0.741281581455633,
-            "timestamp": 1.109204444598025
-        },
-        {
-            "x": 3.919181182263655,
-            "y": 6.356221678659879,
-            "heading": -0.03361602682336018,
-            "angularVelocity": -7.3254443467155425e-9,
-            "velocityX": 3.6695916276783906,
-            "velocityY": 0.9190897360373237,
-            "timestamp": 1.1310627535328648
-        },
-        {
-            "x": 3.998331580016101,
-            "y": 6.3801513240916305,
-            "heading": -0.03361602697852209,
-            "angularVelocity": -7.098532288064829e-9,
-            "velocityX": 3.621066844118403,
-            "velocityY": 1.0947619737229577,
-            "timestamp": 1.1529210624677046
-        },
-        {
-            "x": 4.076237649565588,
-            "y": 6.407865179672378,
-            "heading": -0.03361602713034875,
-            "angularVelocity": -6.945947165054901e-9,
-            "velocityX": 3.5641398326708003,
-            "velocityY": 1.2678865352009827,
-            "timestamp": 1.1747793714025445
-        },
-        {
-            "x": 4.152718837418924,
-            "y": 6.439298305619894,
-            "heading": -0.033616027280326394,
-            "angularVelocity": -6.861356032634536e-9,
-            "velocityX": 3.4989526445677317,
-            "velocityY": 1.4380401540310954,
-            "timestamp": 1.1966376803373844
-        },
-        {
-            "x": 4.227916828679672,
-            "y": 6.473688356846056,
-            "heading": -0.03361602742978782,
-            "angularVelocity": -6.8377396934186395e-9,
-            "velocityX": 3.4402474356508805,
-            "velocityY": 1.5733171000866744,
-            "timestamp": 1.2184959892722242
-        },
-        {
-            "x": 4.303113417879144,
-            "y": 6.50808147368302,
-            "heading": -0.033616027579243535,
-            "angularVelocity": -6.837478280080917e-9,
-            "velocityX": 3.4401832924786557,
-            "velocityY": 1.5734573493077775,
-            "timestamp": 1.240354298207064
-        },
-        {
-            "x": 4.378310012144908,
-            "y": 6.542474579443129,
-            "heading": -0.03361602772869459,
-            "angularVelocity": -6.837264944013175e-9,
-            "velocityX": 3.4401835242573666,
-            "velocityY": 1.573456842550575,
-            "timestamp": 1.262212607141904
-        },
-        {
-            "x": 4.453506612352025,
-            "y": 6.576867672213151,
-            "heading": -0.03361602787814215,
-            "angularVelocity": -6.837105393969464e-9,
-            "velocityX": 3.4401837960695145,
-            "velocityY": 1.5734562482645857,
-            "timestamp": 1.2840709160767438
-        },
-        {
-            "x": 4.528703218974218,
-            "y": 6.6112607509573404,
-            "heading": -0.03361602802758787,
-            "angularVelocity": -6.837020955916861e-9,
-            "velocityX": 3.440184089554037,
-            "velocityY": 1.5734556065940832,
-            "timestamp": 1.3059292250115837
-        },
-        {
-            "x": 4.603899833102385,
-            "y": 6.645653813290562,
-            "heading": -0.03361602817703386,
-            "angularVelocity": -6.837033689050042e-9,
-            "velocityX": 3.440184432946286,
-            "velocityY": 1.5734548558055208,
-            "timestamp": 1.3277875339464236
-        },
-        {
-            "x": 4.679098505825837,
-            "y": 6.680042374380203,
-            "heading": -0.033616028326483124,
-            "angularVelocity": -6.83718315967473e-9,
-            "velocityX": 3.4402786120196116,
-            "velocityY": 1.573248927543637,
-            "timestamp": 1.3496458428812634
-        },
-        {
-            "x": 4.755659310004632,
-            "y": 6.7112810805754854,
-            "heading": -0.033616028476497646,
-            "angularVelocity": -6.8630434261390676e-9,
-            "velocityX": 3.502595027228562,
-            "velocityY": 1.4291456072107467,
-            "timestamp": 1.3715041518161033
-        },
-        {
-            "x": 4.833635561443148,
-            "y": 6.738796851922075,
-            "heading": -0.0336160286284395,
-            "angularVelocity": -6.951217281059994e-9,
-            "velocityX": 3.5673505974759867,
-            "velocityY": 1.258824341288896,
-            "timestamp": 1.3933624607509432
-        },
-        {
-            "x": 4.9128465230031875,
-            "y": 6.762525251770201,
-            "heading": -0.033616028783803605,
-            "angularVelocity": -7.107782622056134e-9,
-            "velocityX": 3.623837589457144,
-            "velocityY": 1.0855551506230685,
-            "timestamp": 1.415220769685783
-        },
-        {
-            "x": 4.993108392358412,
-            "y": 6.78241106619245,
-            "heading": -0.033616028944227425,
-            "angularVelocity": -7.33926038728002e-9,
-            "velocityX": 3.6719157732872607,
-            "velocityY": 0.9097599673208229,
-            "timestamp": 1.437079078620623
-        },
-        {
-            "x": 5.0742348898890315,
-            "y": 6.798408083033217,
-            "heading": -0.03361602911156847,
-            "angularVelocity": -7.655717557583249e-9,
-            "velocityX": 3.711471814789518,
-            "velocityY": 0.7318506151803719,
-            "timestamp": 1.4589373875554628
-        },
-        {
-            "x": 5.156037716665883,
-            "y": 6.810479143599293,
-            "heading": -0.033616029288003786,
-            "angularVelocity": -8.071773672975751e-9,
-            "velocityX": 3.742413332189024,
-            "velocityY": 0.5522412828028046,
-            "timestamp": 1.4807956964903026
-        },
-        {
-            "x": 5.238326998632702,
-            "y": 6.818596213450421,
-            "heading": -0.033616029476163836,
-            "angularVelocity": -8.608170503938931e-9,
-            "velocityX": 3.764668264691683,
-            "velocityY": 0.37134939739963974,
-            "timestamp": 1.5026540054251425
-        },
-        {
-            "x": 5.3209117299494695,
-            "y": 6.822740441774992,
-            "heading": -0.033616029679319044,
-            "angularVelocity": -9.29418657606336e-9,
-            "velocityX": 3.778184834103796,
-            "velocityY": 0.18959510257284434,
-            "timestamp": 1.5245123143599824
-        },
-        {
-            "x": 5.403600215911865,
-            "y": 6.822902202606201,
-            "heading": -0.03361602991707219,
-            "angularVelocity": -1.087701477803747e-8,
-            "velocityX": 3.7829315254392575,
-            "velocityY": 0.00740042753033698,
-            "timestamp": 1.5463706232948222
-        },
-        {
-            "x": 5.530709925548806,
-            "y": 6.813694137770519,
-            "heading": -0.03361603012559057,
-            "angularVelocity": -6.189541308885362e-9,
-            "velocityX": 3.773052419307907,
-            "velocityY": -0.2733269661668366,
-            "timestamp": 1.5800594501413512
-        },
-        {
-            "x": 5.656785942150605,
-            "y": 6.795079480224979,
-            "heading": -0.03361603030947626,
-            "angularVelocity": -5.458358559418031e-9,
-            "velocityX": 3.7423688624167126,
-            "velocityY": -0.5525469209818521,
-            "timestamp": 1.6137482769878801
-        },
-        {
-            "x": 5.781133118086899,
-            "y": 6.767160888262499,
-            "heading": -0.033616030476517275,
-            "angularVelocity": -4.958350514129881e-9,
-            "velocityX": 3.6910509381274874,
-            "velocityY": -0.8287196253423904,
-            "timestamp": 1.6474371038344091
-        },
-        {
-            "x": 5.903065850616414,
-            "y": 6.730092343263882,
-            "heading": -0.03361603063230265,
-            "angularVelocity": -4.624244623037642e-9,
-            "velocityX": 3.619381971506033,
-            "velocityY": -1.100321633860491,
-            "timestamp": 1.681125930680938
-        },
-        {
-            "x": 6.0219118917079,
-            "y": 6.684078355941691,
-            "heading": -0.033616030781137125,
-            "angularVelocity": -4.4179180116936e-9,
-            "velocityX": 3.5277583761789866,
-            "velocityY": -1.3658530625542256,
-            "timestamp": 1.714814757527467
-        },
-        {
-            "x": 6.137016291187876,
-            "y": 6.629373275360583,
-            "heading": -0.033616030926600995,
-            "angularVelocity": -4.3178668766413575e-9,
-            "velocityX": 3.4166936119307345,
-            "velocityY": -1.623834538089393,
-            "timestamp": 1.748503584373996
-        },
-        {
-            "x": 6.249923393155274,
-            "y": 6.570266127170688,
-            "heading": -0.0336160310713635,
-            "angularVelocity": -4.297048049789566e-9,
-            "velocityX": 3.3514702806883885,
-            "velocityY": -1.7545030125020786,
-            "timestamp": 1.782192411220525
-        },
-        {
-            "x": 6.362830075113665,
-            "y": 6.51115817668204,
-            "heading": -0.03361603121596069,
-            "angularVelocity": -4.292140831453005e-9,
-            "velocityX": 3.351457813379591,
-            "velocityY": -1.7545268274825585,
-            "timestamp": 1.815881238067054
-        },
-        {
-            "x": 6.474865356079977,
-            "y": 6.45041474520773,
-            "heading": -0.033616029845538055,
-            "angularVelocity": 4.06788471811577e-8,
-            "velocityX": 3.32559164130869,
-            "velocityY": -1.8030735160660076,
-            "timestamp": 1.849570064913583
-        },
-        {
-            "x": 6.586900214909621,
-            "y": 6.389670535150393,
-            "heading": -0.03361602847479329,
-            "angularVelocity": 4.068840886286375e-8,
-            "velocityX": 3.3255791108435817,
-            "velocityY": -1.8030966270823277,
-            "timestamp": 1.8832588917601119
-        },
-        {
-            "x": 6.6989350261872,
-            "y": 6.328926237389509,
-            "heading": -0.03361602710404103,
-            "angularVelocity": 4.068863135772345e-8,
-            "velocityX": 3.325577699335118,
-            "velocityY": -1.8030992304246154,
-            "timestamp": 1.9169477186066408
-        },
-        {
-            "x": 6.810969814733213,
-            "y": 6.268181897703298,
-            "heading": -0.03361602573329801,
-            "angularVelocity": 4.068835717596455e-8,
-            "velocityX": 3.3255770245842737,
-            "velocityY": -1.8031004749122261,
-            "timestamp": 1.9506365454531698
-        },
-        {
-            "x": 6.92300458832202,
-            "y": 6.207437530430852,
-            "heading": -0.03361602436246198,
-            "angularVelocity": 4.069111827209e-8,
-            "velocityX": 3.3255765806030024,
-            "velocityY": -1.803101293766319,
-            "timestamp": 1.9843253722996987
-        },
-        {
-            "x": 7.031146753085382,
-            "y": 6.14880363204561,
-            "heading": -0.02881387895998304,
-            "angularVelocity": 0.1425441563861497,
-            "velocityX": 3.210030591329563,
-            "velocityY": -1.7404553341186793,
-            "timestamp": 2.0180141991462275
-        },
-        {
-            "x": 7.130970312732727,
-            "y": 6.094680017318762,
-            "heading": -0.024381039069153343,
-            "angularVelocity": 0.13158190135330103,
-            "velocityX": 2.9631058422454424,
-            "velocityY": -1.6065746359589999,
-            "timestamp": 2.0517030259927562
-        },
-        {
-            "x": 7.222475251377904,
-            "y": 6.04506669568433,
-            "heading": -0.02031756633641395,
-            "angularVelocity": 0.12061781644251243,
-            "velocityX": 2.716180621605835,
-            "velocityY": -1.4726936577651888,
-            "timestamp": 2.085391852839285
-        },
-        {
-            "x": 7.305661563921799,
-            "y": 5.999963670644433,
-            "heading": -0.016623481896744244,
-            "angularVelocity": 0.10965310417300114,
-            "velocityX": 2.469255249607073,
-            "velocityY": -1.33881257561641,
-            "timestamp": 2.1190806796858137
-        },
-        {
-            "x": 7.380529247941795,
-            "y": 5.959370944187338,
-            "heading": -0.013298795761870387,
-            "angularVelocity": 0.09868809472109723,
-            "velocityX": 2.222329805696702,
-            "velocityY": -1.2049314344491047,
-            "timestamp": 2.1527695065323424
-        },
-        {
-            "x": 7.447078302075198,
-            "y": 5.923288517676822,
-            "heading": -0.010343513390295376,
-            "angularVelocity": 0.08772292324209098,
-            "velocityX": 1.9754043213368524,
-            "velocityY": -1.0710502528001746,
-            "timestamp": 2.186458333378871
-        },
-        {
-            "x": 7.505308725482669,
-            "y": 5.891716392152232,
-            "heading": -0.007757637975370072,
-            "angularVelocity": 0.07675765697347174,
-            "velocityX": 1.7284788120626102,
-            "velocityY": -0.9371690402998547,
-            "timestamp": 2.2201471602254
-        },
-        {
-            "x": 7.555220517619577,
-            "y": 5.86465456845934,
-            "heading": -0.005541171447259703,
-            "angularVelocity": 0.06579233341092419,
-            "velocityX": 1.481553286621783,
-            "velocityY": -0.8032878026941531,
-            "timestamp": 2.2538359870719287
-        },
-        {
-            "x": 7.596813678122498,
-            "y": 5.842103047317274,
-            "heading": -0.0036941149792240496,
-            "angularVelocity": 0.054826975021999756,
-            "velocityX": 1.234627750393344,
-            "velocityY": -0.6694065437422472,
-            "timestamp": 2.2875248139184574
-        },
-        {
-            "x": 7.630088206746783,
-            "y": 5.824061829356817,
-            "heading": -0.0022164692716530373,
-            "angularVelocity": 0.04386159584309864,
-            "velocityX": 0.9877022069028755,
-            "velocityY": -0.535525266066523,
-            "timestamp": 2.321213640764986
-        },
-        {
-            "x": 7.655044103329761,
-            "y": 5.81053091514454,
-            "heading": -0.0011082347348293014,
-            "angularVelocity": 0.0328962044856008,
-            "velocityX": 0.7407766585837434,
-            "velocityY": -0.4016439715731056,
-            "timestamp": 2.354902467611515
-        },
-        {
-            "x": 7.671681367767583,
-            "y": 5.8015103051986525,
-            "heading": -0.0003694116016791507,
-            "angularVelocity": 0.021930806214050736,
-            "velocityX": 0.49385110718203246,
-            "velocityY": -0.2677626616973202,
-            "timestamp": 2.3885912944580436
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -1.3509198173227418e-31,
-            "angularVelocity": 0.010965404149034802,
-            "velocityX": 0.24692555399191934,
-            "velocityY": -0.13388133754848214,
-            "timestamp": 2.4222801213045724
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 2.046162663204931e-31,
-            "angularVelocity": 9.569528449856341e-30,
-            "velocityX": -2.6100111398714645e-32,
-            "velocityY": -7.871960721903e-33,
-            "timestamp": 2.455968948151101
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.traj
deleted file mode 100644
index 7a28104f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.traj	
+++ /dev/null
@@ -1,616 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 5.330261523342824e-32,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.216523220338838,
-            "y": 5.608939409819235,
-            "heading": -0.000395848810607471,
-            "angularVelocity": -0.004694167537861253,
-            "velocityX": -0.3969839194829395,
-            "velocityY": 0.5803473011402309,
-            "timestamp": 0.08432779772229798
-        },
-        {
-            "x": 2.1530740053514283,
-            "y": 5.7090393916763,
-            "heading": -0.0013136261454272055,
-            "angularVelocity": -0.010883449581379904,
-            "velocityX": -0.7524116210926717,
-            "velocityY": 1.1870342231242381,
-            "timestamp": 0.16865559544459596
-        },
-        {
-            "x": 2.071477218025725,
-            "y": 5.86558734321221,
-            "heading": -0.0031284327789465032,
-            "angularVelocity": -0.02152085886904205,
-            "velocityX": -0.9676143517279073,
-            "velocityY": 1.8564216754650953,
-            "timestamp": 0.25298339316689394
-        },
-        {
-            "x": 2.0458269370451427,
-            "y": 6.041770495606775,
-            "heading": -0.006595224749493174,
-            "angularVelocity": -0.04111090368993278,
-            "velocityX": -0.30417349525777404,
-            "velocityY": 2.089265427928733,
-            "timestamp": 0.3373111908891919
-        },
-        {
-            "x": 2.0715234655116643,
-            "y": 6.188301290095102,
-            "heading": -0.01049068188832321,
-            "angularVelocity": -0.046194223542478534,
-            "velocityX": 0.30472192041755103,
-            "velocityY": 1.7376333598900595,
-            "timestamp": 0.4216389886114899
-        },
-        {
-            "x": 2.1438515186309814,
-            "y": 6.298208713531494,
-            "heading": -0.014554322717054646,
-            "angularVelocity": -0.04818862745724922,
-            "velocityX": 0.8577011978600798,
-            "velocityY": 1.303335630776598,
-            "timestamp": 0.5059667863337879
-        },
-        {
-            "x": 2.2162950329491684,
-            "y": 6.355974505839093,
-            "heading": -0.017431077998725047,
-            "angularVelocity": -0.04874949538291163,
-            "velocityX": 1.2276278031981451,
-            "velocityY": 0.978899124069331,
-            "timestamp": 0.5649777619156877
-        },
-        {
-            "x": 2.310686009240105,
-            "y": 6.394730061346725,
-            "heading": -0.020327199149560698,
-            "angularVelocity": -0.0490776694043422,
-            "velocityX": 1.5995494966853068,
-            "velocityY": 0.6567516487478545,
-            "timestamp": 0.6239887374975874
-        },
-        {
-            "x": 2.4271886355684367,
-            "y": 6.414666608347948,
-            "heading": -0.023223370555002502,
-            "angularVelocity": -0.04907852101890474,
-            "velocityX": 1.9742535211376202,
-            "velocityY": 0.33784472811424154,
-            "timestamp": 0.6829997130794871
-        },
-        {
-            "x": 2.5660476863967387,
-            "y": 6.416075601325133,
-            "heading": -0.02609042518710973,
-            "angularVelocity": -0.048585108174133695,
-            "velocityX": 2.35310549366504,
-            "velocityY": 0.02387679517735602,
-            "timestamp": 0.7420106886613869
-        },
-        {
-            "x": 2.72766661902605,
-            "y": 6.399454975329908,
-            "heading": -0.02887926513240676,
-            "angularVelocity": -0.04725968208112396,
-            "velocityX": 2.738794453669127,
-            "velocityY": -0.281653130308932,
-            "timestamp": 0.8010216642432866
-        },
-        {
-            "x": 2.912832200300888,
-            "y": 6.365845166245685,
-            "heading": -0.031490096116184224,
-            "angularVelocity": -0.04424314219571676,
-            "velocityX": 3.1378159647242496,
-            "velocityY": -0.5695518291775498,
-            "timestamp": 0.8600326398251863
-        },
-        {
-            "x": 3.123679163604623,
-            "y": 6.318688161285352,
-            "heading": -0.033616023768519236,
-            "angularVelocity": -0.03602597027708684,
-            "velocityX": 3.5730126679757386,
-            "velocityY": -0.7991226122822729,
-            "timestamp": 0.919043615407086
-        },
-        {
-            "x": 3.345972776412964,
-            "y": 6.298208713531494,
-            "heading": -0.03361602546428586,
-            "angularVelocity": -2.8736461454443448e-8,
-            "velocityX": 3.766987591991692,
-            "velocityY": -0.34704472434005124,
-            "timestamp": 0.9780545909889857
-        },
-        {
-            "x": 3.4285825306560853,
-            "y": 6.294597584481868,
-            "heading": -0.033616025709707474,
-            "angularVelocity": -1.1227840838251836e-8,
-            "velocityX": 3.779329612797739,
-            "velocityY": -0.16520624081174923,
-            "timestamp": 0.9999128999238256
-        },
-        {
-            "x": 3.5112703400714786,
-            "y": 6.2949695138762145,
-            "heading": -0.033616025930811776,
-            "angularVelocity": -1.0115343355589466e-8,
-            "velocityX": 3.7829005739596693,
-            "velocityY": 0.01701546974451014,
-            "timestamp": 1.0217712088586655
-        },
-        {
-            "x": 3.5938442713679413,
-            "y": 6.299323633866166,
-            "heading": -0.03361602613301592,
-            "angularVelocity": -9.2506762233922e-9,
-            "velocityX": 3.7776907418875507,
-            "velocityY": 0.19919747693799758,
-            "timestamp": 1.0436295177935053
-        },
-        {
-            "x": 3.6761126565475006,
-            "y": 6.3076498312062705,
-            "heading": -0.03361602632044191,
-            "angularVelocity": -8.574588182780684e-9,
-            "velocityX": 3.763712253532677,
-            "velocityY": 0.3809168113107395,
-            "timestamp": 1.0654878267283452
-        },
-        {
-            "x": 3.7578845383740482,
-            "y": 6.31992876945557,
-            "heading": -0.033616026496316675,
-            "angularVelocity": -8.046128410971048e-9,
-            "velocityX": 3.7409976256768736,
-            "velocityY": 0.5617515191089957,
-            "timestamp": 1.087346135663185
-        },
-        {
-            "x": 3.8389701148011586,
-            "y": 6.336131931270735,
-            "heading": -0.03361602666323835,
-            "angularVelocity": -7.63653221094489e-9,
-            "velocityX": 3.709599707316274,
-            "velocityY": 0.741281581455633,
-            "timestamp": 1.109204444598025
-        },
-        {
-            "x": 3.919181182263655,
-            "y": 6.356221678659879,
-            "heading": -0.03361602682336018,
-            "angularVelocity": -7.3254443467155425e-9,
-            "velocityX": 3.6695916276783906,
-            "velocityY": 0.9190897360373237,
-            "timestamp": 1.1310627535328648
-        },
-        {
-            "x": 3.998331580016101,
-            "y": 6.3801513240916305,
-            "heading": -0.03361602697852209,
-            "angularVelocity": -7.098532288064829e-9,
-            "velocityX": 3.621066844118403,
-            "velocityY": 1.0947619737229577,
-            "timestamp": 1.1529210624677046
-        },
-        {
-            "x": 4.076237649565588,
-            "y": 6.407865179672378,
-            "heading": -0.03361602713034875,
-            "angularVelocity": -6.945947165054901e-9,
-            "velocityX": 3.5641398326708003,
-            "velocityY": 1.2678865352009827,
-            "timestamp": 1.1747793714025445
-        },
-        {
-            "x": 4.152718837418924,
-            "y": 6.439298305619894,
-            "heading": -0.033616027280326394,
-            "angularVelocity": -6.861356032634536e-9,
-            "velocityX": 3.4989526445677317,
-            "velocityY": 1.4380401540310954,
-            "timestamp": 1.1966376803373844
-        },
-        {
-            "x": 4.227916828679672,
-            "y": 6.473688356846056,
-            "heading": -0.03361602742978782,
-            "angularVelocity": -6.8377396934186395e-9,
-            "velocityX": 3.4402474356508805,
-            "velocityY": 1.5733171000866744,
-            "timestamp": 1.2184959892722242
-        },
-        {
-            "x": 4.303113417879144,
-            "y": 6.50808147368302,
-            "heading": -0.033616027579243535,
-            "angularVelocity": -6.837478280080917e-9,
-            "velocityX": 3.4401832924786557,
-            "velocityY": 1.5734573493077775,
-            "timestamp": 1.240354298207064
-        },
-        {
-            "x": 4.378310012144908,
-            "y": 6.542474579443129,
-            "heading": -0.03361602772869459,
-            "angularVelocity": -6.837264944013175e-9,
-            "velocityX": 3.4401835242573666,
-            "velocityY": 1.573456842550575,
-            "timestamp": 1.262212607141904
-        },
-        {
-            "x": 4.453506612352025,
-            "y": 6.576867672213151,
-            "heading": -0.03361602787814215,
-            "angularVelocity": -6.837105393969464e-9,
-            "velocityX": 3.4401837960695145,
-            "velocityY": 1.5734562482645857,
-            "timestamp": 1.2840709160767438
-        },
-        {
-            "x": 4.528703218974218,
-            "y": 6.6112607509573404,
-            "heading": -0.03361602802758787,
-            "angularVelocity": -6.837020955916861e-9,
-            "velocityX": 3.440184089554037,
-            "velocityY": 1.5734556065940832,
-            "timestamp": 1.3059292250115837
-        },
-        {
-            "x": 4.603899833102385,
-            "y": 6.645653813290562,
-            "heading": -0.03361602817703386,
-            "angularVelocity": -6.837033689050042e-9,
-            "velocityX": 3.440184432946286,
-            "velocityY": 1.5734548558055208,
-            "timestamp": 1.3277875339464236
-        },
-        {
-            "x": 4.679098505825837,
-            "y": 6.680042374380203,
-            "heading": -0.033616028326483124,
-            "angularVelocity": -6.83718315967473e-9,
-            "velocityX": 3.4402786120196116,
-            "velocityY": 1.573248927543637,
-            "timestamp": 1.3496458428812634
-        },
-        {
-            "x": 4.755659310004632,
-            "y": 6.7112810805754854,
-            "heading": -0.033616028476497646,
-            "angularVelocity": -6.8630434261390676e-9,
-            "velocityX": 3.502595027228562,
-            "velocityY": 1.4291456072107467,
-            "timestamp": 1.3715041518161033
-        },
-        {
-            "x": 4.833635561443148,
-            "y": 6.738796851922075,
-            "heading": -0.0336160286284395,
-            "angularVelocity": -6.951217281059994e-9,
-            "velocityX": 3.5673505974759867,
-            "velocityY": 1.258824341288896,
-            "timestamp": 1.3933624607509432
-        },
-        {
-            "x": 4.9128465230031875,
-            "y": 6.762525251770201,
-            "heading": -0.033616028783803605,
-            "angularVelocity": -7.107782622056134e-9,
-            "velocityX": 3.623837589457144,
-            "velocityY": 1.0855551506230685,
-            "timestamp": 1.415220769685783
-        },
-        {
-            "x": 4.993108392358412,
-            "y": 6.78241106619245,
-            "heading": -0.033616028944227425,
-            "angularVelocity": -7.33926038728002e-9,
-            "velocityX": 3.6719157732872607,
-            "velocityY": 0.9097599673208229,
-            "timestamp": 1.437079078620623
-        },
-        {
-            "x": 5.0742348898890315,
-            "y": 6.798408083033217,
-            "heading": -0.03361602911156847,
-            "angularVelocity": -7.655717557583249e-9,
-            "velocityX": 3.711471814789518,
-            "velocityY": 0.7318506151803719,
-            "timestamp": 1.4589373875554628
-        },
-        {
-            "x": 5.156037716665883,
-            "y": 6.810479143599293,
-            "heading": -0.033616029288003786,
-            "angularVelocity": -8.071773672975751e-9,
-            "velocityX": 3.742413332189024,
-            "velocityY": 0.5522412828028046,
-            "timestamp": 1.4807956964903026
-        },
-        {
-            "x": 5.238326998632702,
-            "y": 6.818596213450421,
-            "heading": -0.033616029476163836,
-            "angularVelocity": -8.608170503938931e-9,
-            "velocityX": 3.764668264691683,
-            "velocityY": 0.37134939739963974,
-            "timestamp": 1.5026540054251425
-        },
-        {
-            "x": 5.3209117299494695,
-            "y": 6.822740441774992,
-            "heading": -0.033616029679319044,
-            "angularVelocity": -9.29418657606336e-9,
-            "velocityX": 3.778184834103796,
-            "velocityY": 0.18959510257284434,
-            "timestamp": 1.5245123143599824
-        },
-        {
-            "x": 5.403600215911865,
-            "y": 6.822902202606201,
-            "heading": -0.03361602991707219,
-            "angularVelocity": -1.087701477803747e-8,
-            "velocityX": 3.7829315254392575,
-            "velocityY": 0.00740042753033698,
-            "timestamp": 1.5463706232948222
-        },
-        {
-            "x": 5.530709925548806,
-            "y": 6.813694137770519,
-            "heading": -0.03361603012559057,
-            "angularVelocity": -6.189541308885362e-9,
-            "velocityX": 3.773052419307907,
-            "velocityY": -0.2733269661668366,
-            "timestamp": 1.5800594501413512
-        },
-        {
-            "x": 5.656785942150605,
-            "y": 6.795079480224979,
-            "heading": -0.03361603030947626,
-            "angularVelocity": -5.458358559418031e-9,
-            "velocityX": 3.7423688624167126,
-            "velocityY": -0.5525469209818521,
-            "timestamp": 1.6137482769878801
-        },
-        {
-            "x": 5.781133118086899,
-            "y": 6.767160888262499,
-            "heading": -0.033616030476517275,
-            "angularVelocity": -4.958350514129881e-9,
-            "velocityX": 3.6910509381274874,
-            "velocityY": -0.8287196253423904,
-            "timestamp": 1.6474371038344091
-        },
-        {
-            "x": 5.903065850616414,
-            "y": 6.730092343263882,
-            "heading": -0.03361603063230265,
-            "angularVelocity": -4.624244623037642e-9,
-            "velocityX": 3.619381971506033,
-            "velocityY": -1.100321633860491,
-            "timestamp": 1.681125930680938
-        },
-        {
-            "x": 6.0219118917079,
-            "y": 6.684078355941691,
-            "heading": -0.033616030781137125,
-            "angularVelocity": -4.4179180116936e-9,
-            "velocityX": 3.5277583761789866,
-            "velocityY": -1.3658530625542256,
-            "timestamp": 1.714814757527467
-        },
-        {
-            "x": 6.137016291187876,
-            "y": 6.629373275360583,
-            "heading": -0.033616030926600995,
-            "angularVelocity": -4.3178668766413575e-9,
-            "velocityX": 3.4166936119307345,
-            "velocityY": -1.623834538089393,
-            "timestamp": 1.748503584373996
-        },
-        {
-            "x": 6.249923393155274,
-            "y": 6.570266127170688,
-            "heading": -0.0336160310713635,
-            "angularVelocity": -4.297048049789566e-9,
-            "velocityX": 3.3514702806883885,
-            "velocityY": -1.7545030125020786,
-            "timestamp": 1.782192411220525
-        },
-        {
-            "x": 6.362830075113665,
-            "y": 6.51115817668204,
-            "heading": -0.03361603121596069,
-            "angularVelocity": -4.292140831453005e-9,
-            "velocityX": 3.351457813379591,
-            "velocityY": -1.7545268274825585,
-            "timestamp": 1.815881238067054
-        },
-        {
-            "x": 6.474865356079977,
-            "y": 6.45041474520773,
-            "heading": -0.033616029845538055,
-            "angularVelocity": 4.06788471811577e-8,
-            "velocityX": 3.32559164130869,
-            "velocityY": -1.8030735160660076,
-            "timestamp": 1.849570064913583
-        },
-        {
-            "x": 6.586900214909621,
-            "y": 6.389670535150393,
-            "heading": -0.03361602847479329,
-            "angularVelocity": 4.068840886286375e-8,
-            "velocityX": 3.3255791108435817,
-            "velocityY": -1.8030966270823277,
-            "timestamp": 1.8832588917601119
-        },
-        {
-            "x": 6.6989350261872,
-            "y": 6.328926237389509,
-            "heading": -0.03361602710404103,
-            "angularVelocity": 4.068863135772345e-8,
-            "velocityX": 3.325577699335118,
-            "velocityY": -1.8030992304246154,
-            "timestamp": 1.9169477186066408
-        },
-        {
-            "x": 6.810969814733213,
-            "y": 6.268181897703298,
-            "heading": -0.03361602573329801,
-            "angularVelocity": 4.068835717596455e-8,
-            "velocityX": 3.3255770245842737,
-            "velocityY": -1.8031004749122261,
-            "timestamp": 1.9506365454531698
-        },
-        {
-            "x": 6.92300458832202,
-            "y": 6.207437530430852,
-            "heading": -0.03361602436246198,
-            "angularVelocity": 4.069111827209e-8,
-            "velocityX": 3.3255765806030024,
-            "velocityY": -1.803101293766319,
-            "timestamp": 1.9843253722996987
-        },
-        {
-            "x": 7.031146753085382,
-            "y": 6.14880363204561,
-            "heading": -0.02881387895998304,
-            "angularVelocity": 0.1425441563861497,
-            "velocityX": 3.210030591329563,
-            "velocityY": -1.7404553341186793,
-            "timestamp": 2.0180141991462275
-        },
-        {
-            "x": 7.130970312732727,
-            "y": 6.094680017318762,
-            "heading": -0.024381039069153343,
-            "angularVelocity": 0.13158190135330103,
-            "velocityX": 2.9631058422454424,
-            "velocityY": -1.6065746359589999,
-            "timestamp": 2.0517030259927562
-        },
-        {
-            "x": 7.222475251377904,
-            "y": 6.04506669568433,
-            "heading": -0.02031756633641395,
-            "angularVelocity": 0.12061781644251243,
-            "velocityX": 2.716180621605835,
-            "velocityY": -1.4726936577651888,
-            "timestamp": 2.085391852839285
-        },
-        {
-            "x": 7.305661563921799,
-            "y": 5.999963670644433,
-            "heading": -0.016623481896744244,
-            "angularVelocity": 0.10965310417300114,
-            "velocityX": 2.469255249607073,
-            "velocityY": -1.33881257561641,
-            "timestamp": 2.1190806796858137
-        },
-        {
-            "x": 7.380529247941795,
-            "y": 5.959370944187338,
-            "heading": -0.013298795761870387,
-            "angularVelocity": 0.09868809472109723,
-            "velocityX": 2.222329805696702,
-            "velocityY": -1.2049314344491047,
-            "timestamp": 2.1527695065323424
-        },
-        {
-            "x": 7.447078302075198,
-            "y": 5.923288517676822,
-            "heading": -0.010343513390295376,
-            "angularVelocity": 0.08772292324209098,
-            "velocityX": 1.9754043213368524,
-            "velocityY": -1.0710502528001746,
-            "timestamp": 2.186458333378871
-        },
-        {
-            "x": 7.505308725482669,
-            "y": 5.891716392152232,
-            "heading": -0.007757637975370072,
-            "angularVelocity": 0.07675765697347174,
-            "velocityX": 1.7284788120626102,
-            "velocityY": -0.9371690402998547,
-            "timestamp": 2.2201471602254
-        },
-        {
-            "x": 7.555220517619577,
-            "y": 5.86465456845934,
-            "heading": -0.005541171447259703,
-            "angularVelocity": 0.06579233341092419,
-            "velocityX": 1.481553286621783,
-            "velocityY": -0.8032878026941531,
-            "timestamp": 2.2538359870719287
-        },
-        {
-            "x": 7.596813678122498,
-            "y": 5.842103047317274,
-            "heading": -0.0036941149792240496,
-            "angularVelocity": 0.054826975021999756,
-            "velocityX": 1.234627750393344,
-            "velocityY": -0.6694065437422472,
-            "timestamp": 2.2875248139184574
-        },
-        {
-            "x": 7.630088206746783,
-            "y": 5.824061829356817,
-            "heading": -0.0022164692716530373,
-            "angularVelocity": 0.04386159584309864,
-            "velocityX": 0.9877022069028755,
-            "velocityY": -0.535525266066523,
-            "timestamp": 2.321213640764986
-        },
-        {
-            "x": 7.655044103329761,
-            "y": 5.81053091514454,
-            "heading": -0.0011082347348293014,
-            "angularVelocity": 0.0328962044856008,
-            "velocityX": 0.7407766585837434,
-            "velocityY": -0.4016439715731056,
-            "timestamp": 2.354902467611515
-        },
-        {
-            "x": 7.671681367767583,
-            "y": 5.8015103051986525,
-            "heading": -0.0003694116016791507,
-            "angularVelocity": 0.021930806214050736,
-            "velocityX": 0.49385110718203246,
-            "velocityY": -0.2677626616973202,
-            "timestamp": 2.3885912944580436
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -1.3509198173227418e-31,
-            "angularVelocity": 0.010965404149034802,
-            "velocityX": 0.24692555399191934,
-            "velocityY": -0.13388133754848214,
-            "timestamp": 2.4222801213045724
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 2.046162663204931e-31,
-            "angularVelocity": 9.569528449856341e-30,
-            "velocityX": -2.6100111398714645e-32,
-            "velocityY": -7.871960721903e-33,
-            "timestamp": 2.455968948151101
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.1.traj
deleted file mode 100644
index 11fcc4a7..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.1.traj	
+++ /dev/null
@@ -1,625 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 3.8917516758910124e-38,
-            "velocityY": -7.197289285396863e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2621897956017167,
-            "y": 5.610801651791703,
-            "heading": 3.480716271732917e-18,
-            "angularVelocity": 4.397302355093269e-17,
-            "velocityX": 0.15399765670735568,
-            "velocityY": 0.6417938075748972,
-            "timestamp": 0.07915572134244127
-        },
-        {
-            "x": 2.2905203388747193,
-            "y": 5.711291077569501,
-            "heading": 3.7329161294500195e-18,
-            "angularVelocity": 3.1861229325774303e-18,
-            "velocityX": 0.35790897729855475,
-            "velocityY": 1.2695156341645175,
-            "timestamp": 0.15831144268488254
-        },
-        {
-            "x": 2.3436883815778624,
-            "y": 5.857742406339618,
-            "heading": 7.49671396174733e-18,
-            "angularVelocity": 4.754928420848352e-17,
-            "velocityX": 0.6716891944314332,
-            "velocityY": 1.8501673193848232,
-            "timestamp": 0.23746716402732382
-        },
-        {
-            "x": 2.444399562681219,
-            "y": 6.0258512044397365,
-            "heading": 1.0345374349306828e-17,
-            "angularVelocity": 3.5988057740869555e-17,
-            "velocityX": 1.2723171413935193,
-            "velocityY": 2.1237731808778637,
-            "timestamp": 0.3166228853697651
-        },
-        {
-            "x": 2.5795093648860483,
-            "y": 6.154639121411333,
-            "heading": 6.772254039775596e-18,
-            "angularVelocity": -4.5140391310627574e-17,
-            "velocityX": 1.706886121602264,
-            "velocityY": 1.6270196871106504,
-            "timestamp": 0.3957786067122063
-        },
-        {
-            "x": 2.7311723872223266,
-            "y": 6.233875155156547,
-            "heading": -7.527039882559706e-19,
-            "angularVelocity": -9.506524797608463e-17,
-            "velocityX": 1.916008341079448,
-            "velocityY": 1.00101461273312,
-            "timestamp": 0.4749343280546476
-        },
-        {
-            "x": 2.892774820327759,
-            "y": 6.261821746826172,
-            "heading": -9.856118161245683e-18,
-            "angularVelocity": -1.1500639891239944e-16,
-            "velocityX": 2.041576153495111,
-            "velocityY": 0.3530583916824199,
-            "timestamp": 0.5540900493970888
-        },
-        {
-            "x": 2.9421812890727397,
-            "y": 6.2655363212140305,
-            "heading": -1.2690994415516207e-17,
-            "angularVelocity": -1.1860957835731596e-16,
-            "velocityX": 2.067137973857932,
-            "velocityY": 0.15541563622967255,
-            "timestamp": 0.5779909545953872
-        },
-        {
-            "x": 2.992183666499164,
-            "y": 6.264525133875901,
-            "heading": -1.5700573063161643e-17,
-            "angularVelocity": -1.2591902515397632e-16,
-            "velocityX": 2.092070447188938,
-            "velocityY": -0.042307491274448855,
-            "timestamp": 0.6018918597936855
-        },
-        {
-            "x": 3.0427650175812166,
-            "y": 6.258786080293358,
-            "heading": -1.8888665404790212e-17,
-            "angularVelocity": -1.333879346368914e-16,
-            "velocityX": 2.1162943688699283,
-            "velocityY": -0.24011867060800707,
-            "timestamp": 0.6257927649919839
-        },
-        {
-            "x": 3.093906133667358,
-            "y": 6.248316847924188,
-            "heading": -2.226054198779156e-17,
-            "angularVelocity": -1.4107736063473173e-16,
-            "velocityX": 2.139714611720349,
-            "velocityY": -0.4380266053653904,
-            "timestamp": 0.6496936701902822
-        },
-        {
-            "x": 3.1455850428469736,
-            "y": 6.233114888531311,
-            "heading": -2.51599280773308e-17,
-            "angularVelocity": -1.2130863242066928e-16,
-            "velocityX": 2.162215562584425,
-            "velocityY": -0.6360411568829696,
-            "timestamp": 0.6735945753885806
-        },
-        {
-            "x": 3.197776369275469,
-            "y": 6.213177387662576,
-            "heading": -2.8136873311901383e-17,
-            "angularVelocity": -1.2455366258796381e-16,
-            "velocityX": 2.1836548028403113,
-            "velocityY": -0.8341734634450271,
-            "timestamp": 0.6974954805868789
-        },
-        {
-            "x": 3.2504504785577715,
-            "y": 6.188501232669928,
-            "heading": -3.110871206110287e-17,
-            "angularVelocity": -1.2434001017092495e-16,
-            "velocityX": 2.203854157207935,
-            "velocityY": -1.0324360013948046,
-            "timestamp": 0.7213963857851773
-        },
-        {
-            "x": 3.3035723121371006,
-            "y": 6.159082982949556,
-            "heading": -3.205106262321403e-17,
-            "angularVelocity": -3.942740156809933e-17,
-            "velocityX": 2.222586681909921,
-            "velocityY": -1.2308424922109886,
-            "timestamp": 0.7452972909834756
-        },
-        {
-            "x": 3.3570997558109252,
-            "y": 6.124918851159325,
-            "heading": -3.3149188675890806e-17,
-            "angularVelocity": -4.594495761931231e-17,
-            "velocityX": 2.239557172823534,
-            "velocityY": -1.429407443223672,
-            "timestamp": 0.769198196181774
-        },
-        {
-            "x": 3.4109812853154073,
-            "y": 6.086004715644374,
-            "heading": -3.4399834546372206e-17,
-            "angularVelocity": -5.232629825878709e-17,
-            "velocityX": 2.2543719184459183,
-            "velocityY": -1.6281448418833084,
-            "timestamp": 0.7930991013800723
-        },
-        {
-            "x": 3.4651524420580837,
-            "y": 6.042336211273467,
-            "heading": -3.565040387084894e-17,
-            "angularVelocity": -5.2323095531564346e-17,
-            "velocityX": 2.2664897539752324,
-            "velocityY": -1.827064875100061,
-            "timestamp": 0.8170000065783707
-        },
-        {
-            "x": 3.5195303175635244,
-            "y": 5.993909012954667,
-            "heading": -3.501524266469143e-17,
-            "angularVelocity": 2.6574776159275344e-17,
-            "velocityX": 2.2751387470175053,
-            "velocityY": -2.0261658676529293,
-            "timestamp": 0.840900911776669
-        },
-        {
-            "x": 3.5740044311548194,
-            "y": 5.940719606067823,
-            "heading": -3.37448298204232e-17,
-            "angularVelocity": 5.315333634409861e-17,
-            "velocityX": 2.2791652926673844,
-            "velocityY": -2.225413909872768,
-            "timestamp": 0.8648018169749674
-        },
-        {
-            "x": 3.628420546352513,
-            "y": 5.8827673726499405,
-            "heading": -3.046153632575651e-17,
-            "angularVelocity": 1.3737109446355186e-16,
-            "velocityX": 2.2767386735448056,
-            "velocityY": -2.4246878073056584,
-            "timestamp": 0.8887027221732657
-        },
-        {
-            "x": 3.6825492209054214,
-            "y": 5.820060642331504,
-            "heading": -2.5159851350702206e-17,
-            "angularVelocity": 2.2181942251709706e-16,
-            "velocityX": 2.264712323814494,
-            "velocityY": -2.6236131978340897,
-            "timestamp": 0.9126036273715641
-        },
-        {
-            "x": 3.736016878561411,
-            "y": 5.75263682695494,
-            "heading": -1.874886441281884e-17,
-            "angularVelocity": 2.6823197281004355e-16,
-            "velocityX": 2.237055760540655,
-            "velocityY": -2.8209732985913787,
-            "timestamp": 0.9365045325698624
-        },
-        {
-            "x": 3.788130152189531,
-            "y": 5.680646456241098,
-            "heading": -1.0550078419468136e-17,
-            "angularVelocity": 3.4303244721005534e-16,
-            "velocityX": 2.1803891189790523,
-            "velocityY": -3.012035323204781,
-            "timestamp": 0.9604054377681608
-        },
-        {
-            "x": 3.837400250784585,
-            "y": 5.604834562813646,
-            "heading": 5.433473713273816e-19,
-            "angularVelocity": 4.641424958072882e-16,
-            "velocityX": 2.061432325942265,
-            "velocityY": -3.171925615304708,
-            "timestamp": 0.9843063429664591
-        },
-        {
-            "x": 3.882609579100313,
-            "y": 5.526533156121729,
-            "heading": 5.968487826268029e-18,
-            "angularVelocity": 2.269847275628469e-16,
-            "velocityX": 1.8915320545661611,
-            "velocityY": -3.2760854052293937,
-            "timestamp": 1.0082072481647575
-        },
-        {
-            "x": 3.923632674014372,
-            "y": 5.445959589727867,
-            "heading": 1.2150851940113108e-17,
-            "angularVelocity": 2.586665259140546e-16,
-            "velocityX": 1.716382478977385,
-            "velocityY": -3.3711512482631147,
-            "timestamp": 1.0321081533630558
-        },
-        {
-            "x": 3.960355768910532,
-            "y": 5.363337507789521,
-            "heading": 1.8690802571261126e-17,
-            "angularVelocity": 2.736277382336804e-16,
-            "velocityX": 1.5364729742024472,
-            "velocityY": -3.456859949564964,
-            "timestamp": 1.0560090585613542
-        },
-        {
-            "x": 4.001130960542707,
-            "y": 5.282638203170642,
-            "heading": 2.7305426078750436e-17,
-            "angularVelocity": 3.6043084711714437e-16,
-            "velocityX": 1.7060103495610794,
-            "velocityY": -3.376412062611935,
-            "timestamp": 1.0799099637596525
-        },
-        {
-            "x": 4.046099298269293,
-            "y": 5.204198147271007,
-            "heading": 3.276849439101543e-17,
-            "angularVelocity": 2.285716070976872e-16,
-            "velocityX": 1.8814491482016318,
-            "velocityY": -3.281886407600119,
-            "timestamp": 1.1038108689579509
-        },
-        {
-            "x": 4.095136049561521,
-            "y": 5.128235061004533,
-            "heading": 2.8631224607755774e-17,
-            "angularVelocity": -1.7310096621588863e-16,
-            "velocityX": 2.051669210240635,
-            "velocityY": -3.178251435928111,
-            "timestamp": 1.1277117741562492
-        },
-        {
-            "x": 4.148105144500732,
-            "y": 5.054959774017334,
-            "heading": 1.6930966409187752e-17,
-            "angularVelocity": -4.895320115071494e-16,
-            "velocityX": 2.2161961858659356,
-            "velocityY": -3.065795474240693,
-            "timestamp": 1.1516126793545476
-        },
-        {
-            "x": 4.265239674165176,
-            "y": 4.923669357008322,
-            "heading": 4.8393962188151695e-17,
-            "angularVelocity": 6.764650813120357e-16,
-            "velocityX": 2.5184321191325005,
-            "velocityY": -2.8227884986351293,
-            "timestamp": 1.1981235734837035
-        },
-        {
-            "x": 4.395200374134661,
-            "y": 4.805061224730511,
-            "heading": 5.597930320273315e-17,
-            "angularVelocity": 1.6308740471674414e-16,
-            "velocityX": 2.7941991312528214,
-            "velocityY": -2.550115075157559,
-            "timestamp": 1.2446344676128593
-        },
-        {
-            "x": 4.536621337779305,
-            "y": 4.700381789438767,
-            "heading": 7.583650477943765e-17,
-            "angularVelocity": 4.269365693457232e-16,
-            "velocityX": 3.040598687523153,
-            "velocityY": -2.250643365424577,
-            "timestamp": 1.2911453617420152
-        },
-        {
-            "x": 4.6880158468821485,
-            "y": 4.610730375722861,
-            "heading": 8.702958111273957e-17,
-            "angularVelocity": 2.4065493779199536e-16,
-            "velocityX": 3.2550332978427186,
-            "velocityY": -1.9275358041271147,
-            "timestamp": 1.337656255871171
-        },
-        {
-            "x": 4.842298757317137,
-            "y": 4.526146352730071,
-            "heading": 1.0187276641692621e-16,
-            "angularVelocity": 3.1913351876162675e-16,
-            "velocityX": 3.317134906212745,
-            "velocityY": -1.81858518475069,
-            "timestamp": 1.384167150000327
-        },
-        {
-            "x": 4.996581769126158,
-            "y": 4.441562514645757,
-            "heading": 8.403781588381205e-17,
-            "angularVelocity": -3.8345748597281753e-16,
-            "velocityX": 3.3171370857888243,
-            "velocityY": -1.818581209155721,
-            "timestamp": 1.4306780441294829
-        },
-        {
-            "x": 5.15086732735743,
-            "y": 4.356983321464328,
-            "heading": 9.522247870814417e-17,
-            "angularVelocity": 2.4047404444902996e-16,
-            "velocityX": 3.317191834731107,
-            "velocityY": -1.818481342167903,
-            "timestamp": 1.4771889382586387
-        },
-        {
-            "x": 5.312538078249567,
-            "y": 4.287555361855474,
-            "heading": 1.0159270601289723e-16,
-            "angularVelocity": 1.369620477960438e-16,
-            "velocityX": 3.4759759819537814,
-            "velocityY": -1.4927246811478623,
-            "timestamp": 1.5236998323877946
-        },
-        {
-            "x": 5.480467319488525,
-            "y": 4.235044002532959,
-            "heading": 8.575039074051001e-17,
-            "angularVelocity": -3.4061515197699265e-16,
-            "velocityX": 3.6105356472537267,
-            "velocityY": -1.1290120369797083,
-            "timestamp": 1.5702107265169505
-        },
-        {
-            "x": 5.592904232264291,
-            "y": 4.2080430634395825,
-            "heading": 8.77612564470466e-17,
-            "angularVelocity": 6.578528400068062e-17,
-            "velocityX": 3.6783631124997167,
-            "velocityY": -0.8833332035895192,
-            "timestamp": 1.6007778318670682
-        },
-        {
-            "x": 5.706904061688869,
-            "y": 4.1886742955111504,
-            "heading": 8.48190648175679e-17,
-            "angularVelocity": -9.625352468867059e-17,
-            "velocityX": 3.7294937848649896,
-            "velocityY": -0.6336474359145746,
-            "timestamp": 1.631344937217186
-        },
-        {
-            "x": 5.821949322476148,
-            "y": 4.1770253308728975,
-            "heading": 6.614143850361712e-17,
-            "angularVelocity": -6.110368020256626e-16,
-            "velocityX": 3.7636949743701025,
-            "velocityY": -0.3810947914375838,
-            "timestamp": 1.6619120425673037
-        },
-        {
-            "x": 5.937517692705647,
-            "y": 4.173144318562267,
-            "heading": 5.289613863539961e-17,
-            "angularVelocity": -4.3331874941909564e-16,
-            "velocityX": 3.7808084509727644,
-            "velocityY": -0.12696695569208769,
-            "timestamp": 1.6924791479174215
-        },
-        {
-            "x": 6.05311068319219,
-            "y": 4.170083108620971,
-            "heading": 6.789574595295852e-17,
-            "angularVelocity": 4.90710754117939e-16,
-            "velocityX": 3.7816139003851448,
-            "velocityY": -0.1001471976568426,
-            "timestamp": 1.7230462532675392
-        },
-        {
-            "x": 6.168703676027133,
-            "y": 4.167021987357848,
-            "heading": 5.651709601242885e-17,
-            "angularVelocity": -3.7225147131887107e-16,
-            "velocityX": 3.7816139772128325,
-            "velocityY": -0.1001442965586977,
-            "timestamp": 1.753613358617657
-        },
-        {
-            "x": 6.284296668862326,
-            "y": 4.163960866104177,
-            "heading": 5.090280597716889e-17,
-            "angularVelocity": -1.8367097476040197e-16,
-            "velocityX": 3.7816139772210215,
-            "velocityY": -0.10014429624946593,
-            "timestamp": 1.7841804639677747
-        },
-        {
-            "x": 6.399889661697518,
-            "y": 4.160899744850507,
-            "heading": 6.173261084277577e-17,
-            "angularVelocity": 3.542960558928146e-16,
-            "velocityX": 3.7816139772210224,
-            "velocityY": -0.10014429624943298,
-            "timestamp": 1.8147475693178925
-        },
-        {
-            "x": 6.515482654532711,
-            "y": 4.157838623596838,
-            "heading": 8.001124700396023e-17,
-            "angularVelocity": 5.979838768447224e-16,
-            "velocityX": 3.7816139772210224,
-            "velocityY": -0.10014429624943295,
-            "timestamp": 1.8453146746680102
-        },
-        {
-            "x": 6.631075647367904,
-            "y": 4.154777502343168,
-            "heading": 7.855690079559015e-17,
-            "angularVelocity": -4.757880053117145e-17,
-            "velocityX": 3.7816139772210224,
-            "velocityY": -0.10014429624943294,
-            "timestamp": 1.875881780018128
-        },
-        {
-            "x": 6.746668640203097,
-            "y": 4.151716381089498,
-            "heading": 7.690538849750639e-17,
-            "angularVelocity": -5.4029070766342576e-17,
-            "velocityX": 3.7816139772210224,
-            "velocityY": -0.10014429624943297,
-            "timestamp": 1.9064488853682457
-        },
-        {
-            "x": 6.862261633037354,
-            "y": 4.148655259835854,
-            "heading": 7.874213255028351e-17,
-            "angularVelocity": 6.00889103413278e-17,
-            "velocityX": 3.781613977190394,
-            "velocityY": -0.10014429624862185,
-            "timestamp": 1.9370159907183635
-        },
-        {
-            "x": 6.971293364430436,
-            "y": 4.1457678932048365,
-            "heading": 7.034301862221845e-17,
-            "angularVelocity": -2.747762270062427e-16,
-            "velocityX": 3.56696292122612,
-            "velocityY": -0.09445992997849091,
-            "timestamp": 1.9675830960684813
-        },
-        {
-            "x": 7.072537143209538,
-            "y": 4.14308676630068,
-            "heading": 6.196577755471392e-17,
-            "angularVelocity": -2.740606584102676e-16,
-            "velocityX": 3.3121807779784156,
-            "velocityY": -0.08771281655386487,
-            "timestamp": 1.998150201418599
-        },
-        {
-            "x": 7.165992951009495,
-            "y": 4.140611879609727,
-            "heading": 5.371713190017333e-17,
-            "angularVelocity": -2.6985367144933565e-16,
-            "velocityX": 3.057398033916134,
-            "velocityY": -0.08096568721853159,
-            "timestamp": 2.0287173067687165
-        },
-        {
-            "x": 7.251660781708473,
-            "y": 4.138343233294095,
-            "heading": 4.5597032779539255e-17,
-            "angularVelocity": -2.6564828431304464e-16,
-            "velocityX": 2.802615089578594,
-            "velocityY": -0.07421855257953036,
-            "timestamp": 2.059284412118834
-        },
-        {
-            "x": 7.329540632245536,
-            "y": 4.136280827434844,
-            "heading": 3.7578034898451724e-17,
-            "angularVelocity": -2.623407674507654e-16,
-            "velocityX": 2.5478320451027607,
-            "velocityY": -0.06747141528867756,
-            "timestamp": 2.0898515174689516
-        },
-        {
-            "x": 7.399632500784112,
-            "y": 4.134424662080611,
-            "heading": 2.960093117982924e-17,
-            "angularVelocity": -2.609702038052913e-16,
-            "velocityX": 2.293048940543746,
-            "velocityY": -0.06072427640670841,
-            "timestamp": 2.120418622819069
-        },
-        {
-            "x": 7.461936386099821,
-            "y": 4.132774737263818,
-            "heading": 2.4038424741612248e-17,
-            "angularVelocity": -1.8197687815335696e-16,
-            "velocityX": 2.0382657959291963,
-            "velocityY": -0.05397713646399285,
-            "timestamp": 2.1509857281691867
-        },
-        {
-            "x": 7.516452287318104,
-            "y": 4.131331053007627,
-            "heading": 1.8665464618967105e-17,
-            "angularVelocity": -1.757758880397929e-16,
-            "velocityX": 1.7834826227035099,
-            "velocityY": -0.04722999576360025,
-            "timestamp": 2.181552833519304
-        },
-        {
-            "x": 7.5631802037830385,
-            "y": 4.130093609329407,
-            "heading": 1.3420986699374698e-17,
-            "angularVelocity": -1.7157260548142223e-16,
-            "velocityX": 1.5286994280194506,
-            "velocityY": -0.04048285449494931,
-            "timestamp": 2.2121199388694217
-        },
-        {
-            "x": 7.6021201349844665,
-            "y": 4.129062406242667,
-            "heading": 8.352476313992174e-18,
-            "angularVelocity": -1.6581584391601633e-16,
-            "velocityX": 1.2739162166455336,
-            "velocityY": -0.033735712784319354,
-            "timestamp": 2.2426870442195392
-        },
-        {
-            "x": 7.633272080514257,
-            "y": 4.128237443758217,
-            "heading": 5.582680805550875e-18,
-            "angularVelocity": -9.061360111293989e-17,
-            "velocityX": 1.019132991919723,
-            "velocityY": -0.026988570720105975,
-            "timestamp": 2.2732541495696568
-        },
-        {
-            "x": 7.656636040038488,
-            "y": 4.127618721884898,
-            "heading": 2.786250494304085e-18,
-            "angularVelocity": -9.148495626615499e-17,
-            "velocityX": 0.7643497562696309,
-            "velocityY": -0.020241428366596953,
-            "timestamp": 2.3038212549197743
-        },
-        {
-            "x": 7.672212013278888,
-            "y": 4.1272062406300805,
-            "heading": 1.8952405540316967e-19,
-            "angularVelocity": -8.495166341555795e-17,
-            "velocityX": 0.5095665115159678,
-            "velocityY": -0.013494285772008131,
-            "timestamp": 2.334388360269892
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 6.998640646947103e-35,
-            "angularVelocity": -6.20026191774287e-18,
-            "velocityX": 0.25478325905928073,
-            "velocityY": -0.006747142973428655,
-            "timestamp": 2.3649554656200094
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.616659043590546e-35,
-            "angularVelocity": -1.1064131915191804e-33,
-            "velocityX": -5.781348077610764e-36,
-            "velocityY": -1.0341970128847813e-32,
-            "timestamp": 2.395522570970127
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.traj
deleted file mode 100644
index 11fcc4a7..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.traj	
+++ /dev/null
@@ -1,625 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 3.8917516758910124e-38,
-            "velocityY": -7.197289285396863e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2621897956017167,
-            "y": 5.610801651791703,
-            "heading": 3.480716271732917e-18,
-            "angularVelocity": 4.397302355093269e-17,
-            "velocityX": 0.15399765670735568,
-            "velocityY": 0.6417938075748972,
-            "timestamp": 0.07915572134244127
-        },
-        {
-            "x": 2.2905203388747193,
-            "y": 5.711291077569501,
-            "heading": 3.7329161294500195e-18,
-            "angularVelocity": 3.1861229325774303e-18,
-            "velocityX": 0.35790897729855475,
-            "velocityY": 1.2695156341645175,
-            "timestamp": 0.15831144268488254
-        },
-        {
-            "x": 2.3436883815778624,
-            "y": 5.857742406339618,
-            "heading": 7.49671396174733e-18,
-            "angularVelocity": 4.754928420848352e-17,
-            "velocityX": 0.6716891944314332,
-            "velocityY": 1.8501673193848232,
-            "timestamp": 0.23746716402732382
-        },
-        {
-            "x": 2.444399562681219,
-            "y": 6.0258512044397365,
-            "heading": 1.0345374349306828e-17,
-            "angularVelocity": 3.5988057740869555e-17,
-            "velocityX": 1.2723171413935193,
-            "velocityY": 2.1237731808778637,
-            "timestamp": 0.3166228853697651
-        },
-        {
-            "x": 2.5795093648860483,
-            "y": 6.154639121411333,
-            "heading": 6.772254039775596e-18,
-            "angularVelocity": -4.5140391310627574e-17,
-            "velocityX": 1.706886121602264,
-            "velocityY": 1.6270196871106504,
-            "timestamp": 0.3957786067122063
-        },
-        {
-            "x": 2.7311723872223266,
-            "y": 6.233875155156547,
-            "heading": -7.527039882559706e-19,
-            "angularVelocity": -9.506524797608463e-17,
-            "velocityX": 1.916008341079448,
-            "velocityY": 1.00101461273312,
-            "timestamp": 0.4749343280546476
-        },
-        {
-            "x": 2.892774820327759,
-            "y": 6.261821746826172,
-            "heading": -9.856118161245683e-18,
-            "angularVelocity": -1.1500639891239944e-16,
-            "velocityX": 2.041576153495111,
-            "velocityY": 0.3530583916824199,
-            "timestamp": 0.5540900493970888
-        },
-        {
-            "x": 2.9421812890727397,
-            "y": 6.2655363212140305,
-            "heading": -1.2690994415516207e-17,
-            "angularVelocity": -1.1860957835731596e-16,
-            "velocityX": 2.067137973857932,
-            "velocityY": 0.15541563622967255,
-            "timestamp": 0.5779909545953872
-        },
-        {
-            "x": 2.992183666499164,
-            "y": 6.264525133875901,
-            "heading": -1.5700573063161643e-17,
-            "angularVelocity": -1.2591902515397632e-16,
-            "velocityX": 2.092070447188938,
-            "velocityY": -0.042307491274448855,
-            "timestamp": 0.6018918597936855
-        },
-        {
-            "x": 3.0427650175812166,
-            "y": 6.258786080293358,
-            "heading": -1.8888665404790212e-17,
-            "angularVelocity": -1.333879346368914e-16,
-            "velocityX": 2.1162943688699283,
-            "velocityY": -0.24011867060800707,
-            "timestamp": 0.6257927649919839
-        },
-        {
-            "x": 3.093906133667358,
-            "y": 6.248316847924188,
-            "heading": -2.226054198779156e-17,
-            "angularVelocity": -1.4107736063473173e-16,
-            "velocityX": 2.139714611720349,
-            "velocityY": -0.4380266053653904,
-            "timestamp": 0.6496936701902822
-        },
-        {
-            "x": 3.1455850428469736,
-            "y": 6.233114888531311,
-            "heading": -2.51599280773308e-17,
-            "angularVelocity": -1.2130863242066928e-16,
-            "velocityX": 2.162215562584425,
-            "velocityY": -0.6360411568829696,
-            "timestamp": 0.6735945753885806
-        },
-        {
-            "x": 3.197776369275469,
-            "y": 6.213177387662576,
-            "heading": -2.8136873311901383e-17,
-            "angularVelocity": -1.2455366258796381e-16,
-            "velocityX": 2.1836548028403113,
-            "velocityY": -0.8341734634450271,
-            "timestamp": 0.6974954805868789
-        },
-        {
-            "x": 3.2504504785577715,
-            "y": 6.188501232669928,
-            "heading": -3.110871206110287e-17,
-            "angularVelocity": -1.2434001017092495e-16,
-            "velocityX": 2.203854157207935,
-            "velocityY": -1.0324360013948046,
-            "timestamp": 0.7213963857851773
-        },
-        {
-            "x": 3.3035723121371006,
-            "y": 6.159082982949556,
-            "heading": -3.205106262321403e-17,
-            "angularVelocity": -3.942740156809933e-17,
-            "velocityX": 2.222586681909921,
-            "velocityY": -1.2308424922109886,
-            "timestamp": 0.7452972909834756
-        },
-        {
-            "x": 3.3570997558109252,
-            "y": 6.124918851159325,
-            "heading": -3.3149188675890806e-17,
-            "angularVelocity": -4.594495761931231e-17,
-            "velocityX": 2.239557172823534,
-            "velocityY": -1.429407443223672,
-            "timestamp": 0.769198196181774
-        },
-        {
-            "x": 3.4109812853154073,
-            "y": 6.086004715644374,
-            "heading": -3.4399834546372206e-17,
-            "angularVelocity": -5.232629825878709e-17,
-            "velocityX": 2.2543719184459183,
-            "velocityY": -1.6281448418833084,
-            "timestamp": 0.7930991013800723
-        },
-        {
-            "x": 3.4651524420580837,
-            "y": 6.042336211273467,
-            "heading": -3.565040387084894e-17,
-            "angularVelocity": -5.2323095531564346e-17,
-            "velocityX": 2.2664897539752324,
-            "velocityY": -1.827064875100061,
-            "timestamp": 0.8170000065783707
-        },
-        {
-            "x": 3.5195303175635244,
-            "y": 5.993909012954667,
-            "heading": -3.501524266469143e-17,
-            "angularVelocity": 2.6574776159275344e-17,
-            "velocityX": 2.2751387470175053,
-            "velocityY": -2.0261658676529293,
-            "timestamp": 0.840900911776669
-        },
-        {
-            "x": 3.5740044311548194,
-            "y": 5.940719606067823,
-            "heading": -3.37448298204232e-17,
-            "angularVelocity": 5.315333634409861e-17,
-            "velocityX": 2.2791652926673844,
-            "velocityY": -2.225413909872768,
-            "timestamp": 0.8648018169749674
-        },
-        {
-            "x": 3.628420546352513,
-            "y": 5.8827673726499405,
-            "heading": -3.046153632575651e-17,
-            "angularVelocity": 1.3737109446355186e-16,
-            "velocityX": 2.2767386735448056,
-            "velocityY": -2.4246878073056584,
-            "timestamp": 0.8887027221732657
-        },
-        {
-            "x": 3.6825492209054214,
-            "y": 5.820060642331504,
-            "heading": -2.5159851350702206e-17,
-            "angularVelocity": 2.2181942251709706e-16,
-            "velocityX": 2.264712323814494,
-            "velocityY": -2.6236131978340897,
-            "timestamp": 0.9126036273715641
-        },
-        {
-            "x": 3.736016878561411,
-            "y": 5.75263682695494,
-            "heading": -1.874886441281884e-17,
-            "angularVelocity": 2.6823197281004355e-16,
-            "velocityX": 2.237055760540655,
-            "velocityY": -2.8209732985913787,
-            "timestamp": 0.9365045325698624
-        },
-        {
-            "x": 3.788130152189531,
-            "y": 5.680646456241098,
-            "heading": -1.0550078419468136e-17,
-            "angularVelocity": 3.4303244721005534e-16,
-            "velocityX": 2.1803891189790523,
-            "velocityY": -3.012035323204781,
-            "timestamp": 0.9604054377681608
-        },
-        {
-            "x": 3.837400250784585,
-            "y": 5.604834562813646,
-            "heading": 5.433473713273816e-19,
-            "angularVelocity": 4.641424958072882e-16,
-            "velocityX": 2.061432325942265,
-            "velocityY": -3.171925615304708,
-            "timestamp": 0.9843063429664591
-        },
-        {
-            "x": 3.882609579100313,
-            "y": 5.526533156121729,
-            "heading": 5.968487826268029e-18,
-            "angularVelocity": 2.269847275628469e-16,
-            "velocityX": 1.8915320545661611,
-            "velocityY": -3.2760854052293937,
-            "timestamp": 1.0082072481647575
-        },
-        {
-            "x": 3.923632674014372,
-            "y": 5.445959589727867,
-            "heading": 1.2150851940113108e-17,
-            "angularVelocity": 2.586665259140546e-16,
-            "velocityX": 1.716382478977385,
-            "velocityY": -3.3711512482631147,
-            "timestamp": 1.0321081533630558
-        },
-        {
-            "x": 3.960355768910532,
-            "y": 5.363337507789521,
-            "heading": 1.8690802571261126e-17,
-            "angularVelocity": 2.736277382336804e-16,
-            "velocityX": 1.5364729742024472,
-            "velocityY": -3.456859949564964,
-            "timestamp": 1.0560090585613542
-        },
-        {
-            "x": 4.001130960542707,
-            "y": 5.282638203170642,
-            "heading": 2.7305426078750436e-17,
-            "angularVelocity": 3.6043084711714437e-16,
-            "velocityX": 1.7060103495610794,
-            "velocityY": -3.376412062611935,
-            "timestamp": 1.0799099637596525
-        },
-        {
-            "x": 4.046099298269293,
-            "y": 5.204198147271007,
-            "heading": 3.276849439101543e-17,
-            "angularVelocity": 2.285716070976872e-16,
-            "velocityX": 1.8814491482016318,
-            "velocityY": -3.281886407600119,
-            "timestamp": 1.1038108689579509
-        },
-        {
-            "x": 4.095136049561521,
-            "y": 5.128235061004533,
-            "heading": 2.8631224607755774e-17,
-            "angularVelocity": -1.7310096621588863e-16,
-            "velocityX": 2.051669210240635,
-            "velocityY": -3.178251435928111,
-            "timestamp": 1.1277117741562492
-        },
-        {
-            "x": 4.148105144500732,
-            "y": 5.054959774017334,
-            "heading": 1.6930966409187752e-17,
-            "angularVelocity": -4.895320115071494e-16,
-            "velocityX": 2.2161961858659356,
-            "velocityY": -3.065795474240693,
-            "timestamp": 1.1516126793545476
-        },
-        {
-            "x": 4.265239674165176,
-            "y": 4.923669357008322,
-            "heading": 4.8393962188151695e-17,
-            "angularVelocity": 6.764650813120357e-16,
-            "velocityX": 2.5184321191325005,
-            "velocityY": -2.8227884986351293,
-            "timestamp": 1.1981235734837035
-        },
-        {
-            "x": 4.395200374134661,
-            "y": 4.805061224730511,
-            "heading": 5.597930320273315e-17,
-            "angularVelocity": 1.6308740471674414e-16,
-            "velocityX": 2.7941991312528214,
-            "velocityY": -2.550115075157559,
-            "timestamp": 1.2446344676128593
-        },
-        {
-            "x": 4.536621337779305,
-            "y": 4.700381789438767,
-            "heading": 7.583650477943765e-17,
-            "angularVelocity": 4.269365693457232e-16,
-            "velocityX": 3.040598687523153,
-            "velocityY": -2.250643365424577,
-            "timestamp": 1.2911453617420152
-        },
-        {
-            "x": 4.6880158468821485,
-            "y": 4.610730375722861,
-            "heading": 8.702958111273957e-17,
-            "angularVelocity": 2.4065493779199536e-16,
-            "velocityX": 3.2550332978427186,
-            "velocityY": -1.9275358041271147,
-            "timestamp": 1.337656255871171
-        },
-        {
-            "x": 4.842298757317137,
-            "y": 4.526146352730071,
-            "heading": 1.0187276641692621e-16,
-            "angularVelocity": 3.1913351876162675e-16,
-            "velocityX": 3.317134906212745,
-            "velocityY": -1.81858518475069,
-            "timestamp": 1.384167150000327
-        },
-        {
-            "x": 4.996581769126158,
-            "y": 4.441562514645757,
-            "heading": 8.403781588381205e-17,
-            "angularVelocity": -3.8345748597281753e-16,
-            "velocityX": 3.3171370857888243,
-            "velocityY": -1.818581209155721,
-            "timestamp": 1.4306780441294829
-        },
-        {
-            "x": 5.15086732735743,
-            "y": 4.356983321464328,
-            "heading": 9.522247870814417e-17,
-            "angularVelocity": 2.4047404444902996e-16,
-            "velocityX": 3.317191834731107,
-            "velocityY": -1.818481342167903,
-            "timestamp": 1.4771889382586387
-        },
-        {
-            "x": 5.312538078249567,
-            "y": 4.287555361855474,
-            "heading": 1.0159270601289723e-16,
-            "angularVelocity": 1.369620477960438e-16,
-            "velocityX": 3.4759759819537814,
-            "velocityY": -1.4927246811478623,
-            "timestamp": 1.5236998323877946
-        },
-        {
-            "x": 5.480467319488525,
-            "y": 4.235044002532959,
-            "heading": 8.575039074051001e-17,
-            "angularVelocity": -3.4061515197699265e-16,
-            "velocityX": 3.6105356472537267,
-            "velocityY": -1.1290120369797083,
-            "timestamp": 1.5702107265169505
-        },
-        {
-            "x": 5.592904232264291,
-            "y": 4.2080430634395825,
-            "heading": 8.77612564470466e-17,
-            "angularVelocity": 6.578528400068062e-17,
-            "velocityX": 3.6783631124997167,
-            "velocityY": -0.8833332035895192,
-            "timestamp": 1.6007778318670682
-        },
-        {
-            "x": 5.706904061688869,
-            "y": 4.1886742955111504,
-            "heading": 8.48190648175679e-17,
-            "angularVelocity": -9.625352468867059e-17,
-            "velocityX": 3.7294937848649896,
-            "velocityY": -0.6336474359145746,
-            "timestamp": 1.631344937217186
-        },
-        {
-            "x": 5.821949322476148,
-            "y": 4.1770253308728975,
-            "heading": 6.614143850361712e-17,
-            "angularVelocity": -6.110368020256626e-16,
-            "velocityX": 3.7636949743701025,
-            "velocityY": -0.3810947914375838,
-            "timestamp": 1.6619120425673037
-        },
-        {
-            "x": 5.937517692705647,
-            "y": 4.173144318562267,
-            "heading": 5.289613863539961e-17,
-            "angularVelocity": -4.3331874941909564e-16,
-            "velocityX": 3.7808084509727644,
-            "velocityY": -0.12696695569208769,
-            "timestamp": 1.6924791479174215
-        },
-        {
-            "x": 6.05311068319219,
-            "y": 4.170083108620971,
-            "heading": 6.789574595295852e-17,
-            "angularVelocity": 4.90710754117939e-16,
-            "velocityX": 3.7816139003851448,
-            "velocityY": -0.1001471976568426,
-            "timestamp": 1.7230462532675392
-        },
-        {
-            "x": 6.168703676027133,
-            "y": 4.167021987357848,
-            "heading": 5.651709601242885e-17,
-            "angularVelocity": -3.7225147131887107e-16,
-            "velocityX": 3.7816139772128325,
-            "velocityY": -0.1001442965586977,
-            "timestamp": 1.753613358617657
-        },
-        {
-            "x": 6.284296668862326,
-            "y": 4.163960866104177,
-            "heading": 5.090280597716889e-17,
-            "angularVelocity": -1.8367097476040197e-16,
-            "velocityX": 3.7816139772210215,
-            "velocityY": -0.10014429624946593,
-            "timestamp": 1.7841804639677747
-        },
-        {
-            "x": 6.399889661697518,
-            "y": 4.160899744850507,
-            "heading": 6.173261084277577e-17,
-            "angularVelocity": 3.542960558928146e-16,
-            "velocityX": 3.7816139772210224,
-            "velocityY": -0.10014429624943298,
-            "timestamp": 1.8147475693178925
-        },
-        {
-            "x": 6.515482654532711,
-            "y": 4.157838623596838,
-            "heading": 8.001124700396023e-17,
-            "angularVelocity": 5.979838768447224e-16,
-            "velocityX": 3.7816139772210224,
-            "velocityY": -0.10014429624943295,
-            "timestamp": 1.8453146746680102
-        },
-        {
-            "x": 6.631075647367904,
-            "y": 4.154777502343168,
-            "heading": 7.855690079559015e-17,
-            "angularVelocity": -4.757880053117145e-17,
-            "velocityX": 3.7816139772210224,
-            "velocityY": -0.10014429624943294,
-            "timestamp": 1.875881780018128
-        },
-        {
-            "x": 6.746668640203097,
-            "y": 4.151716381089498,
-            "heading": 7.690538849750639e-17,
-            "angularVelocity": -5.4029070766342576e-17,
-            "velocityX": 3.7816139772210224,
-            "velocityY": -0.10014429624943297,
-            "timestamp": 1.9064488853682457
-        },
-        {
-            "x": 6.862261633037354,
-            "y": 4.148655259835854,
-            "heading": 7.874213255028351e-17,
-            "angularVelocity": 6.00889103413278e-17,
-            "velocityX": 3.781613977190394,
-            "velocityY": -0.10014429624862185,
-            "timestamp": 1.9370159907183635
-        },
-        {
-            "x": 6.971293364430436,
-            "y": 4.1457678932048365,
-            "heading": 7.034301862221845e-17,
-            "angularVelocity": -2.747762270062427e-16,
-            "velocityX": 3.56696292122612,
-            "velocityY": -0.09445992997849091,
-            "timestamp": 1.9675830960684813
-        },
-        {
-            "x": 7.072537143209538,
-            "y": 4.14308676630068,
-            "heading": 6.196577755471392e-17,
-            "angularVelocity": -2.740606584102676e-16,
-            "velocityX": 3.3121807779784156,
-            "velocityY": -0.08771281655386487,
-            "timestamp": 1.998150201418599
-        },
-        {
-            "x": 7.165992951009495,
-            "y": 4.140611879609727,
-            "heading": 5.371713190017333e-17,
-            "angularVelocity": -2.6985367144933565e-16,
-            "velocityX": 3.057398033916134,
-            "velocityY": -0.08096568721853159,
-            "timestamp": 2.0287173067687165
-        },
-        {
-            "x": 7.251660781708473,
-            "y": 4.138343233294095,
-            "heading": 4.5597032779539255e-17,
-            "angularVelocity": -2.6564828431304464e-16,
-            "velocityX": 2.802615089578594,
-            "velocityY": -0.07421855257953036,
-            "timestamp": 2.059284412118834
-        },
-        {
-            "x": 7.329540632245536,
-            "y": 4.136280827434844,
-            "heading": 3.7578034898451724e-17,
-            "angularVelocity": -2.623407674507654e-16,
-            "velocityX": 2.5478320451027607,
-            "velocityY": -0.06747141528867756,
-            "timestamp": 2.0898515174689516
-        },
-        {
-            "x": 7.399632500784112,
-            "y": 4.134424662080611,
-            "heading": 2.960093117982924e-17,
-            "angularVelocity": -2.609702038052913e-16,
-            "velocityX": 2.293048940543746,
-            "velocityY": -0.06072427640670841,
-            "timestamp": 2.120418622819069
-        },
-        {
-            "x": 7.461936386099821,
-            "y": 4.132774737263818,
-            "heading": 2.4038424741612248e-17,
-            "angularVelocity": -1.8197687815335696e-16,
-            "velocityX": 2.0382657959291963,
-            "velocityY": -0.05397713646399285,
-            "timestamp": 2.1509857281691867
-        },
-        {
-            "x": 7.516452287318104,
-            "y": 4.131331053007627,
-            "heading": 1.8665464618967105e-17,
-            "angularVelocity": -1.757758880397929e-16,
-            "velocityX": 1.7834826227035099,
-            "velocityY": -0.04722999576360025,
-            "timestamp": 2.181552833519304
-        },
-        {
-            "x": 7.5631802037830385,
-            "y": 4.130093609329407,
-            "heading": 1.3420986699374698e-17,
-            "angularVelocity": -1.7157260548142223e-16,
-            "velocityX": 1.5286994280194506,
-            "velocityY": -0.04048285449494931,
-            "timestamp": 2.2121199388694217
-        },
-        {
-            "x": 7.6021201349844665,
-            "y": 4.129062406242667,
-            "heading": 8.352476313992174e-18,
-            "angularVelocity": -1.6581584391601633e-16,
-            "velocityX": 1.2739162166455336,
-            "velocityY": -0.033735712784319354,
-            "timestamp": 2.2426870442195392
-        },
-        {
-            "x": 7.633272080514257,
-            "y": 4.128237443758217,
-            "heading": 5.582680805550875e-18,
-            "angularVelocity": -9.061360111293989e-17,
-            "velocityX": 1.019132991919723,
-            "velocityY": -0.026988570720105975,
-            "timestamp": 2.2732541495696568
-        },
-        {
-            "x": 7.656636040038488,
-            "y": 4.127618721884898,
-            "heading": 2.786250494304085e-18,
-            "angularVelocity": -9.148495626615499e-17,
-            "velocityX": 0.7643497562696309,
-            "velocityY": -0.020241428366596953,
-            "timestamp": 2.3038212549197743
-        },
-        {
-            "x": 7.672212013278888,
-            "y": 4.1272062406300805,
-            "heading": 1.8952405540316967e-19,
-            "angularVelocity": -8.495166341555795e-17,
-            "velocityX": 0.5095665115159678,
-            "velocityY": -0.013494285772008131,
-            "timestamp": 2.334388360269892
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 6.998640646947103e-35,
-            "angularVelocity": -6.20026191774287e-18,
-            "velocityX": 0.25478325905928073,
-            "velocityY": -0.006747142973428655,
-            "timestamp": 2.3649554656200094
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.616659043590546e-35,
-            "angularVelocity": -1.1064131915191804e-33,
-            "velocityX": -5.781348077610764e-36,
-            "velocityY": -1.0341970128847813e-32,
-            "timestamp": 2.395522570970127
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.1.traj
deleted file mode 100644
index 84ac552d..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.1.traj	
+++ /dev/null
@@ -1,589 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -8.663522895809945e-37,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2925575950089905,
-            "y": 5.560958672147739,
-            "heading": -0.030620645935394094,
-            "angularVelocity": -0.4244497974010598,
-            "velocityX": 0.5899144850684857,
-            "velocityY": 0.0132886876305762,
-            "timestamp": 0.07214197326253131
-        },
-        {
-            "x": 2.3778169001331375,
-            "y": 5.562706698566846,
-            "heading": -0.08906407022187784,
-            "angularVelocity": -0.8101167966920272,
-            "velocityX": 1.181826629746885,
-            "velocityY": 0.02423036604148944,
-            "timestamp": 0.14428394652506263
-        },
-        {
-            "x": 2.5059697216292194,
-            "y": 5.56499180242334,
-            "heading": -0.17113009935557016,
-            "angularVelocity": -1.1375628558848325,
-            "velocityX": 1.7763975075885672,
-            "velocityY": 0.031675094998839956,
-            "timestamp": 0.21642591978759396
-        },
-        {
-            "x": 2.6772662600517445,
-            "y": 5.56737716078426,
-            "heading": -0.269909808145774,
-            "angularVelocity": -1.3692404618700307,
-            "velocityX": 2.3744365544197357,
-            "velocityY": 0.033064778422963216,
-            "timestamp": 0.28856789305012526
-        },
-        {
-            "x": 2.891943869686031,
-            "y": 5.568884231737924,
-            "heading": -0.3711645487758272,
-            "angularVelocity": -1.4035482542399815,
-            "velocityX": 2.975765700961003,
-            "velocityY": 0.020890348371527884,
-            "timestamp": 0.36070986631265656
-        },
-        {
-            "x": 3.1471065368375797,
-            "y": 5.565499357743455,
-            "heading": -0.4176508519135821,
-            "angularVelocity": -0.644372492676169,
-            "velocityX": 3.5369515915927057,
-            "velocityY": -0.04691962031799383,
-            "timestamp": 0.43285183957518786
-        },
-        {
-            "x": 3.4167471065440913,
-            "y": 5.523390468620082,
-            "heading": -0.41765089729124494,
-            "angularVelocity": -6.290050140591191e-7,
-            "velocityX": 3.7376378481534562,
-            "velocityY": -0.5836947233219467,
-            "timestamp": 0.5049938128377192
-        },
-        {
-            "x": 3.6595394652992312,
-            "y": 5.451715121543425,
-            "heading": -0.35516305517194063,
-            "angularVelocity": 0.8661787208384958,
-            "velocityX": 3.3654798694179893,
-            "velocityY": -0.9935318350085096,
-            "timestamp": 0.5771357861002505
-        },
-        {
-            "x": 3.865437953511339,
-            "y": 5.371729850961066,
-            "heading": -0.21394633729907842,
-            "angularVelocity": 1.957483438371744,
-            "velocityX": 2.8540734180200986,
-            "velocityY": -1.1087203047702217,
-            "timestamp": 0.6492777593627819
-        },
-        {
-            "x": 4.032804489135742,
-            "y": 5.285560607910156,
-            "heading": 0,
-            "angularVelocity": 2.9656291285588754,
-            "velocityX": 2.319960600680286,
-            "velocityY": -1.1944397852458482,
-            "timestamp": 0.7214197326253132
-        },
-        {
-            "x": 4.094074407124677,
-            "y": 5.249842876180867,
-            "heading": 0.09747269322901686,
-            "angularVelocity": 3.3393263465697696,
-            "velocityX": 2.099051997177287,
-            "velocityY": -1.2236571972325518,
-            "timestamp": 0.7506090609846522
-        },
-        {
-            "x": 4.1489483150763835,
-            "y": 5.213294373235887,
-            "heading": 0.20635910635937496,
-            "angularVelocity": 3.7303500714335676,
-            "velocityX": 1.8799304758291948,
-            "velocityY": -1.2521186679955063,
-            "timestamp": 0.7797983893439911
-        },
-        {
-            "x": 4.197569720233833,
-            "y": 5.1758877650462445,
-            "heading": 0.3277826636682362,
-            "angularVelocity": 4.159861296363549,
-            "velocityX": 1.6657253828827165,
-            "velocityY": -1.2815165778788733,
-            "timestamp": 0.8089877177033301
-        },
-        {
-            "x": 4.240207786511328,
-            "y": 5.137521274665071,
-            "heading": 0.4634475312713068,
-            "angularVelocity": 4.64775571170911,
-            "velocityX": 1.460741602293634,
-            "velocityY": -1.3144012739470166,
-            "timestamp": 0.8381770460626691
-        },
-        {
-            "x": 4.277338609770458,
-            "y": 5.097982888442082,
-            "heading": 0.6156568342246089,
-            "angularVelocity": 5.214553109256573,
-            "velocityX": 1.27206843549211,
-            "velocityY": -1.3545493659959265,
-            "timestamp": 0.867366374422008
-        },
-        {
-            "x": 4.3089076073694805,
-            "y": 5.060359750747959,
-            "heading": 0.7826775871522255,
-            "angularVelocity": 5.721979994588639,
-            "velocityX": 1.0815253167318162,
-            "velocityY": -1.2889346829415846,
-            "timestamp": 0.896555702781347
-        },
-        {
-            "x": 4.3340758913958135,
-            "y": 5.025518984665433,
-            "heading": 0.9544942987723,
-            "angularVelocity": 5.886285203444992,
-            "velocityX": 0.8622426565111364,
-            "velocityY": -1.1936131470246671,
-            "timestamp": 0.925745031140686
-        },
-        {
-            "x": 4.353180255759456,
-            "y": 4.9936006932957335,
-            "heading": 1.134787723551591,
-            "angularVelocity": 6.17668973262302,
-            "velocityX": 0.6544982511572759,
-            "velocityY": -1.0934918055244334,
-            "timestamp": 0.9549343595000249
-        },
-        {
-            "x": 4.3789611483924675,
-            "y": 4.9633684481765155,
-            "heading": 1.3088307728290547,
-            "angularVelocity": 5.962557518791945,
-            "velocityX": 0.8832300735266126,
-            "velocityY": -1.0357293853095562,
-            "timestamp": 0.9841236878593639
-        },
-        {
-            "x": 4.410526959907193,
-            "y": 4.931127063214471,
-            "heading": 1.468917613249843,
-            "angularVelocity": 5.4844304209407735,
-            "velocityX": 1.0814161643642388,
-            "velocityY": -1.104560699894603,
-            "timestamp": 1.013313016218703
-        },
-        {
-            "x": 4.4435007518243745,
-            "y": 4.892801223101424,
-            "heading": 1.6159090393580717,
-            "angularVelocity": 5.035793365940867,
-            "velocityX": 1.1296523000205303,
-            "velocityY": -1.3130086325122463,
-            "timestamp": 1.042502344578042
-        },
-        {
-            "x": 4.478070868346437,
-            "y": 4.848243847113431,
-            "heading": 1.7513264495243417,
-            "angularVelocity": 4.639278043646516,
-            "velocityX": 1.1843409377729661,
-            "velocityY": -1.5264954177589778,
-            "timestamp": 1.0716916729373809
-        },
-        {
-            "x": 4.514676965101799,
-            "y": 4.797506620196597,
-            "heading": 1.8758559761070788,
-            "angularVelocity": 4.266268995630874,
-            "velocityX": 1.2540917798696012,
-            "velocityY": -1.738211523479656,
-            "timestamp": 1.1008810012967198
-        },
-        {
-            "x": 4.554041400948399,
-            "y": 4.740897535447995,
-            "heading": 1.989313365398794,
-            "angularVelocity": 3.886947582177413,
-            "velocityX": 1.3485899833664619,
-            "velocityY": -1.939376064146053,
-            "timestamp": 1.1300703296560588
-        },
-        {
-            "x": 4.597166214346246,
-            "y": 4.6791223056158096,
-            "heading": 2.090829457044177,
-            "angularVelocity": 3.477849520744562,
-            "velocityX": 1.4774171185768328,
-            "velocityY": -2.1163635240829413,
-            "timestamp": 1.1592596580153978
-        },
-        {
-            "x": 4.645233022395554,
-            "y": 4.613520579676299,
-            "heading": 2.179525396238879,
-            "angularVelocity": 3.0386426882728963,
-            "velocityX": 1.646725387359914,
-            "velocityY": -2.2474558212478137,
-            "timestamp": 1.1884489863747367
-        },
-        {
-            "x": 4.699569055464054,
-            "y": 4.547167372776357,
-            "heading": 2.256949250765549,
-            "angularVelocity": 2.6524712584521715,
-            "velocityX": 1.8615033686143418,
-            "velocityY": -2.2732008795506418,
-            "timestamp": 1.2176383147340757
-        },
-        {
-            "x": 4.760368188580363,
-            "y": 4.482555153301175,
-            "heading": 2.3258054605639615,
-            "angularVelocity": 2.358951495928562,
-            "velocityX": 2.082923333069965,
-            "velocityY": -2.213556224376411,
-            "timestamp": 1.2468276430934147
-        },
-        {
-            "x": 4.827470532546925,
-            "y": 4.4190609396421525,
-            "heading": 2.3838171568684117,
-            "angularVelocity": 1.9874282679714124,
-            "velocityX": 2.2988656381705637,
-            "velocityY": -2.175254355885453,
-            "timestamp": 1.2760169714527536
-        },
-        {
-            "x": 4.899988963730756,
-            "y": 4.356794264470432,
-            "heading": 2.432111827676766,
-            "angularVelocity": 1.6545317594778726,
-            "velocityX": 2.484415889639006,
-            "velocityY": -2.1331999971078073,
-            "timestamp": 1.3052062998120926
-        },
-        {
-            "x": 4.972562290971905,
-            "y": 4.300811163566705,
-            "heading": 2.4919687455960093,
-            "angularVelocity": 2.050643892259759,
-            "velocityX": 2.486296578932036,
-            "velocityY": -1.9179304235623322,
-            "timestamp": 1.3343956281714315
-        },
-        {
-            "x": 5.045616975616562,
-            "y": 4.25136299402395,
-            "heading": 2.5615851998228103,
-            "angularVelocity": 2.384996782720671,
-            "velocityX": 2.5027874483889563,
-            "velocityY": -1.6940495832592366,
-            "timestamp": 1.3635849565307705
-        },
-        {
-            "x": 5.124307057757338,
-            "y": 4.205650619988245,
-            "heading": 2.6233534867290937,
-            "angularVelocity": 2.116125665718561,
-            "velocityX": 2.6958510717359463,
-            "velocityY": -1.5660646066588564,
-            "timestamp": 1.3927742848901095
-        },
-        {
-            "x": 5.208588203701103,
-            "y": 4.163420547805767,
-            "heading": 2.6756025601758724,
-            "angularVelocity": 1.7900060187599918,
-            "velocityX": 2.8873958628376504,
-            "velocityY": -1.4467640934590689,
-            "timestamp": 1.4219636132494484
-        },
-        {
-            "x": 5.297563997608419,
-            "y": 4.1261005954207315,
-            "heading": 2.720276777571372,
-            "angularVelocity": 1.5304982987457612,
-            "velocityX": 3.048230257714994,
-            "velocityY": -1.2785478283571272,
-            "timestamp": 1.4511529416087874
-        },
-        {
-            "x": 5.390789031982422,
-            "y": 4.09412145614624,
-            "heading": 2.757748451080741,
-            "angularVelocity": 1.2837456569082273,
-            "velocityX": 3.1938053944353855,
-            "velocityY": -1.0955763997309995,
-            "timestamp": 1.4803422699681263
-        },
-        {
-            "x": 5.532416911390752,
-            "y": 4.059853570653742,
-            "heading": 2.796595493071039,
-            "angularVelocity": 0.9277997104504593,
-            "velocityX": 3.3825562713263437,
-            "velocityY": -0.818433852585974,
-            "timestamp": 1.522212343382395
-        },
-        {
-            "x": 5.681515537517707,
-            "y": 4.021534249773671,
-            "heading": 2.782742222711128,
-            "angularVelocity": -0.3308632928068908,
-            "velocityX": 3.5609831550031092,
-            "velocityY": -0.9151959324488058,
-            "timestamp": 1.5640824167966638
-        },
-        {
-            "x": 5.826340094588043,
-            "y": 3.969241008458542,
-            "heading": 2.7680381624736805,
-            "angularVelocity": -0.351183053632681,
-            "velocityX": 3.4589038246343846,
-            "velocityY": -1.2489407600921083,
-            "timestamp": 1.6059524902109326
-        },
-        {
-            "x": 5.965226819905552,
-            "y": 3.9129528098786466,
-            "heading": 2.7396888789742415,
-            "angularVelocity": -0.6770774729470177,
-            "velocityX": 3.317088172818383,
-            "velocityY": -1.3443539499673571,
-            "timestamp": 1.6478225636252013
-        },
-        {
-            "x": 6.097371617524639,
-            "y": 3.854212614126153,
-            "heading": 2.694967938521201,
-            "angularVelocity": -1.0680884174853156,
-            "velocityX": 3.1560679703526437,
-            "velocityY": -1.402915996141418,
-            "timestamp": 1.68969263703947
-        },
-        {
-            "x": 6.217931948857121,
-            "y": 3.789923024350862,
-            "heading": 2.6256097109731904,
-            "angularVelocity": -1.6565107699184056,
-            "velocityX": 2.8793914483893763,
-            "velocityY": -1.535454431598441,
-            "timestamp": 1.7315627104537388
-        },
-        {
-            "x": 6.32897922975643,
-            "y": 3.7148340577600942,
-            "heading": 2.547026684843302,
-            "angularVelocity": -1.8768303879569581,
-            "velocityX": 2.652187394098668,
-            "velocityY": -1.7933803422751633,
-            "timestamp": 1.7734327838680075
-        },
-        {
-            "x": 6.440862810984381,
-            "y": 3.629355422988249,
-            "heading": 2.48919642870263,
-            "angularVelocity": -1.381183538144076,
-            "velocityX": 2.672161095132505,
-            "velocityY": -2.0415210149288825,
-            "timestamp": 1.8153028572822762
-        },
-        {
-            "x": 6.556707402178449,
-            "y": 3.5350910926988375,
-            "heading": 2.4587564932761583,
-            "angularVelocity": -0.7270093635923316,
-            "velocityX": 2.766763507861174,
-            "velocityY": -2.2513533558144507,
-            "timestamp": 1.857172930696545
-        },
-        {
-            "x": 6.674645090324846,
-            "y": 3.429370276775565,
-            "heading": 2.4587373064570563,
-            "angularVelocity": -0.00045824660759140524,
-            "velocityX": 2.816753794040547,
-            "velocityY": -2.5249732637737297,
-            "timestamp": 1.8990430041108137
-        },
-        {
-            "x": 6.791280321048844,
-            "y": 3.3222067883695963,
-            "heading": 2.45873486083509,
-            "angularVelocity": -0.00005840978453659185,
-            "velocityX": 2.785646673460325,
-            "velocityY": -2.559429197691564,
-            "timestamp": 1.9409130775250825
-        },
-        {
-            "x": 6.907916035753451,
-            "y": 3.2150438274589583,
-            "heading": 2.4587324135695865,
-            "angularVelocity": -0.000058449037785704465,
-            "velocityX": 2.78565823256613,
-            "velocityY": -2.5594165993059286,
-            "timestamp": 1.9827831509393512
-        },
-        {
-            "x": 7.020962902689577,
-            "y": 3.1172887235192603,
-            "heading": 2.428615335233058,
-            "angularVelocity": -0.7192984363448716,
-            "velocityX": 2.6999443210338705,
-            "velocityY": -2.334724923276202,
-            "timestamp": 2.02465322435362
-        },
-        {
-            "x": 7.121900964071713,
-            "y": 3.0192154080188542,
-            "heading": 2.3705886859446603,
-            "angularVelocity": -1.385874075602246,
-            "velocityX": 2.4107447909976107,
-            "velocityY": -2.3423248994587165,
-            "timestamp": 2.0665232977678887
-        },
-        {
-            "x": 7.210190441218643,
-            "y": 2.92089200334631,
-            "heading": 2.287196223783446,
-            "angularVelocity": -1.9916961056197946,
-            "velocityX": 2.10865350708562,
-            "velocityY": -2.348297880916461,
-            "timestamp": 2.1083933711821574
-        },
-        {
-            "x": 7.2890247338542125,
-            "y": 2.830773686809754,
-            "heading": 2.1745743754916034,
-            "angularVelocity": -2.689793427815253,
-            "velocityX": 1.8828314881507633,
-            "velocityY": -2.1523324223703058,
-            "timestamp": 2.150263444596426
-        },
-        {
-            "x": 7.359953440317656,
-            "y": 2.7509272688501483,
-            "heading": 2.035955336818745,
-            "angularVelocity": -3.3106949037643396,
-            "velocityX": 1.6940191568728353,
-            "velocityY": -1.9070044890916185,
-            "timestamp": 2.192133518010695
-        },
-        {
-            "x": 7.4240572342258595,
-            "y": 2.6822786221440054,
-            "heading": 1.872800890040008,
-            "angularVelocity": -3.896684038847076,
-            "velocityX": 1.5310169933057254,
-            "velocityY": -1.6395635619484732,
-            "timestamp": 2.2340035914249636
-        },
-        {
-            "x": 7.482217323317858,
-            "y": 2.6251237224270416,
-            "heading": 1.684191854325982,
-            "angularVelocity": -4.504626343687046,
-            "velocityX": 1.3890610727273844,
-            "velocityY": -1.3650537258787387,
-            "timestamp": 2.2758736648392324
-        },
-        {
-            "x": 7.535468730024687,
-            "y": 2.5792763511542147,
-            "heading": 1.466995470236355,
-            "angularVelocity": -5.187389616938403,
-            "velocityX": 1.2718250140130243,
-            "velocityY": -1.0949914230912903,
-            "timestamp": 2.317743738253501
-        },
-        {
-            "x": 7.580274182888533,
-            "y": 2.5411410245640176,
-            "heading": 1.2146613001768354,
-            "angularVelocity": -6.02659965658257,
-            "velocityX": 1.0701068617801175,
-            "velocityY": -0.91080152195771,
-            "timestamp": 2.35961381166777
-        },
-        {
-            "x": 7.616169603666072,
-            "y": 2.509680561991982,
-            "heading": 0.922946754094809,
-            "angularVelocity": -6.967137200734248,
-            "velocityX": 0.8573049400316086,
-            "velocityY": -0.7513830286553573,
-            "timestamp": 2.4014838850820386
-        },
-        {
-            "x": 7.640414148022587,
-            "y": 2.4865073161659272,
-            "heading": 0.6422191973798425,
-            "angularVelocity": -6.704730463149791,
-            "velocityX": 0.5790423178062244,
-            "velocityY": -0.5534560590991822,
-            "timestamp": 2.4433539584963073
-        },
-        {
-            "x": 7.656220323606262,
-            "y": 2.4718614980447855,
-            "heading": 0.3932969564478367,
-            "angularVelocity": -5.9451111649394885,
-            "velocityX": 0.37750532288984423,
-            "velocityY": -0.34979203347062976,
-            "timestamp": 2.485224031910576
-        },
-        {
-            "x": 7.668143711832241,
-            "y": 2.4642737767821443,
-            "heading": 0.1972336516813064,
-            "angularVelocity": -4.682659684559189,
-            "velocityX": 0.28477113254632147,
-            "velocityY": -0.18122063430765137,
-            "timestamp": 2.527094105324845
-        },
-        {
-            "x": 7.676158456210791,
-            "y": 2.459440783193878,
-            "heading": 0.06602780599823356,
-            "angularVelocity": -3.1336425992116688,
-            "velocityX": 0.19141940113768102,
-            "velocityY": -0.11542835238065828,
-            "timestamp": 2.5689641787391135
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 1.4385689598890085e-34,
-            "angularVelocity": -1.576968956919288,
-            "velocityX": 0.09174915341561693,
-            "velocityY": -0.05829421815741107,
-            "timestamp": 2.6108342521533823
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 5.954582727394106e-35,
-            "angularVelocity": -1.9052580558070114e-33,
-            "velocityX": -2.275662344047759e-34,
-            "velocityY": 3.071010312134925e-34,
-            "timestamp": 2.652704325567651
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.traj
deleted file mode 100644
index 84ac552d..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C4.traj	
+++ /dev/null
@@ -1,589 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -8.663522895809945e-37,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2925575950089905,
-            "y": 5.560958672147739,
-            "heading": -0.030620645935394094,
-            "angularVelocity": -0.4244497974010598,
-            "velocityX": 0.5899144850684857,
-            "velocityY": 0.0132886876305762,
-            "timestamp": 0.07214197326253131
-        },
-        {
-            "x": 2.3778169001331375,
-            "y": 5.562706698566846,
-            "heading": -0.08906407022187784,
-            "angularVelocity": -0.8101167966920272,
-            "velocityX": 1.181826629746885,
-            "velocityY": 0.02423036604148944,
-            "timestamp": 0.14428394652506263
-        },
-        {
-            "x": 2.5059697216292194,
-            "y": 5.56499180242334,
-            "heading": -0.17113009935557016,
-            "angularVelocity": -1.1375628558848325,
-            "velocityX": 1.7763975075885672,
-            "velocityY": 0.031675094998839956,
-            "timestamp": 0.21642591978759396
-        },
-        {
-            "x": 2.6772662600517445,
-            "y": 5.56737716078426,
-            "heading": -0.269909808145774,
-            "angularVelocity": -1.3692404618700307,
-            "velocityX": 2.3744365544197357,
-            "velocityY": 0.033064778422963216,
-            "timestamp": 0.28856789305012526
-        },
-        {
-            "x": 2.891943869686031,
-            "y": 5.568884231737924,
-            "heading": -0.3711645487758272,
-            "angularVelocity": -1.4035482542399815,
-            "velocityX": 2.975765700961003,
-            "velocityY": 0.020890348371527884,
-            "timestamp": 0.36070986631265656
-        },
-        {
-            "x": 3.1471065368375797,
-            "y": 5.565499357743455,
-            "heading": -0.4176508519135821,
-            "angularVelocity": -0.644372492676169,
-            "velocityX": 3.5369515915927057,
-            "velocityY": -0.04691962031799383,
-            "timestamp": 0.43285183957518786
-        },
-        {
-            "x": 3.4167471065440913,
-            "y": 5.523390468620082,
-            "heading": -0.41765089729124494,
-            "angularVelocity": -6.290050140591191e-7,
-            "velocityX": 3.7376378481534562,
-            "velocityY": -0.5836947233219467,
-            "timestamp": 0.5049938128377192
-        },
-        {
-            "x": 3.6595394652992312,
-            "y": 5.451715121543425,
-            "heading": -0.35516305517194063,
-            "angularVelocity": 0.8661787208384958,
-            "velocityX": 3.3654798694179893,
-            "velocityY": -0.9935318350085096,
-            "timestamp": 0.5771357861002505
-        },
-        {
-            "x": 3.865437953511339,
-            "y": 5.371729850961066,
-            "heading": -0.21394633729907842,
-            "angularVelocity": 1.957483438371744,
-            "velocityX": 2.8540734180200986,
-            "velocityY": -1.1087203047702217,
-            "timestamp": 0.6492777593627819
-        },
-        {
-            "x": 4.032804489135742,
-            "y": 5.285560607910156,
-            "heading": 0,
-            "angularVelocity": 2.9656291285588754,
-            "velocityX": 2.319960600680286,
-            "velocityY": -1.1944397852458482,
-            "timestamp": 0.7214197326253132
-        },
-        {
-            "x": 4.094074407124677,
-            "y": 5.249842876180867,
-            "heading": 0.09747269322901686,
-            "angularVelocity": 3.3393263465697696,
-            "velocityX": 2.099051997177287,
-            "velocityY": -1.2236571972325518,
-            "timestamp": 0.7506090609846522
-        },
-        {
-            "x": 4.1489483150763835,
-            "y": 5.213294373235887,
-            "heading": 0.20635910635937496,
-            "angularVelocity": 3.7303500714335676,
-            "velocityX": 1.8799304758291948,
-            "velocityY": -1.2521186679955063,
-            "timestamp": 0.7797983893439911
-        },
-        {
-            "x": 4.197569720233833,
-            "y": 5.1758877650462445,
-            "heading": 0.3277826636682362,
-            "angularVelocity": 4.159861296363549,
-            "velocityX": 1.6657253828827165,
-            "velocityY": -1.2815165778788733,
-            "timestamp": 0.8089877177033301
-        },
-        {
-            "x": 4.240207786511328,
-            "y": 5.137521274665071,
-            "heading": 0.4634475312713068,
-            "angularVelocity": 4.64775571170911,
-            "velocityX": 1.460741602293634,
-            "velocityY": -1.3144012739470166,
-            "timestamp": 0.8381770460626691
-        },
-        {
-            "x": 4.277338609770458,
-            "y": 5.097982888442082,
-            "heading": 0.6156568342246089,
-            "angularVelocity": 5.214553109256573,
-            "velocityX": 1.27206843549211,
-            "velocityY": -1.3545493659959265,
-            "timestamp": 0.867366374422008
-        },
-        {
-            "x": 4.3089076073694805,
-            "y": 5.060359750747959,
-            "heading": 0.7826775871522255,
-            "angularVelocity": 5.721979994588639,
-            "velocityX": 1.0815253167318162,
-            "velocityY": -1.2889346829415846,
-            "timestamp": 0.896555702781347
-        },
-        {
-            "x": 4.3340758913958135,
-            "y": 5.025518984665433,
-            "heading": 0.9544942987723,
-            "angularVelocity": 5.886285203444992,
-            "velocityX": 0.8622426565111364,
-            "velocityY": -1.1936131470246671,
-            "timestamp": 0.925745031140686
-        },
-        {
-            "x": 4.353180255759456,
-            "y": 4.9936006932957335,
-            "heading": 1.134787723551591,
-            "angularVelocity": 6.17668973262302,
-            "velocityX": 0.6544982511572759,
-            "velocityY": -1.0934918055244334,
-            "timestamp": 0.9549343595000249
-        },
-        {
-            "x": 4.3789611483924675,
-            "y": 4.9633684481765155,
-            "heading": 1.3088307728290547,
-            "angularVelocity": 5.962557518791945,
-            "velocityX": 0.8832300735266126,
-            "velocityY": -1.0357293853095562,
-            "timestamp": 0.9841236878593639
-        },
-        {
-            "x": 4.410526959907193,
-            "y": 4.931127063214471,
-            "heading": 1.468917613249843,
-            "angularVelocity": 5.4844304209407735,
-            "velocityX": 1.0814161643642388,
-            "velocityY": -1.104560699894603,
-            "timestamp": 1.013313016218703
-        },
-        {
-            "x": 4.4435007518243745,
-            "y": 4.892801223101424,
-            "heading": 1.6159090393580717,
-            "angularVelocity": 5.035793365940867,
-            "velocityX": 1.1296523000205303,
-            "velocityY": -1.3130086325122463,
-            "timestamp": 1.042502344578042
-        },
-        {
-            "x": 4.478070868346437,
-            "y": 4.848243847113431,
-            "heading": 1.7513264495243417,
-            "angularVelocity": 4.639278043646516,
-            "velocityX": 1.1843409377729661,
-            "velocityY": -1.5264954177589778,
-            "timestamp": 1.0716916729373809
-        },
-        {
-            "x": 4.514676965101799,
-            "y": 4.797506620196597,
-            "heading": 1.8758559761070788,
-            "angularVelocity": 4.266268995630874,
-            "velocityX": 1.2540917798696012,
-            "velocityY": -1.738211523479656,
-            "timestamp": 1.1008810012967198
-        },
-        {
-            "x": 4.554041400948399,
-            "y": 4.740897535447995,
-            "heading": 1.989313365398794,
-            "angularVelocity": 3.886947582177413,
-            "velocityX": 1.3485899833664619,
-            "velocityY": -1.939376064146053,
-            "timestamp": 1.1300703296560588
-        },
-        {
-            "x": 4.597166214346246,
-            "y": 4.6791223056158096,
-            "heading": 2.090829457044177,
-            "angularVelocity": 3.477849520744562,
-            "velocityX": 1.4774171185768328,
-            "velocityY": -2.1163635240829413,
-            "timestamp": 1.1592596580153978
-        },
-        {
-            "x": 4.645233022395554,
-            "y": 4.613520579676299,
-            "heading": 2.179525396238879,
-            "angularVelocity": 3.0386426882728963,
-            "velocityX": 1.646725387359914,
-            "velocityY": -2.2474558212478137,
-            "timestamp": 1.1884489863747367
-        },
-        {
-            "x": 4.699569055464054,
-            "y": 4.547167372776357,
-            "heading": 2.256949250765549,
-            "angularVelocity": 2.6524712584521715,
-            "velocityX": 1.8615033686143418,
-            "velocityY": -2.2732008795506418,
-            "timestamp": 1.2176383147340757
-        },
-        {
-            "x": 4.760368188580363,
-            "y": 4.482555153301175,
-            "heading": 2.3258054605639615,
-            "angularVelocity": 2.358951495928562,
-            "velocityX": 2.082923333069965,
-            "velocityY": -2.213556224376411,
-            "timestamp": 1.2468276430934147
-        },
-        {
-            "x": 4.827470532546925,
-            "y": 4.4190609396421525,
-            "heading": 2.3838171568684117,
-            "angularVelocity": 1.9874282679714124,
-            "velocityX": 2.2988656381705637,
-            "velocityY": -2.175254355885453,
-            "timestamp": 1.2760169714527536
-        },
-        {
-            "x": 4.899988963730756,
-            "y": 4.356794264470432,
-            "heading": 2.432111827676766,
-            "angularVelocity": 1.6545317594778726,
-            "velocityX": 2.484415889639006,
-            "velocityY": -2.1331999971078073,
-            "timestamp": 1.3052062998120926
-        },
-        {
-            "x": 4.972562290971905,
-            "y": 4.300811163566705,
-            "heading": 2.4919687455960093,
-            "angularVelocity": 2.050643892259759,
-            "velocityX": 2.486296578932036,
-            "velocityY": -1.9179304235623322,
-            "timestamp": 1.3343956281714315
-        },
-        {
-            "x": 5.045616975616562,
-            "y": 4.25136299402395,
-            "heading": 2.5615851998228103,
-            "angularVelocity": 2.384996782720671,
-            "velocityX": 2.5027874483889563,
-            "velocityY": -1.6940495832592366,
-            "timestamp": 1.3635849565307705
-        },
-        {
-            "x": 5.124307057757338,
-            "y": 4.205650619988245,
-            "heading": 2.6233534867290937,
-            "angularVelocity": 2.116125665718561,
-            "velocityX": 2.6958510717359463,
-            "velocityY": -1.5660646066588564,
-            "timestamp": 1.3927742848901095
-        },
-        {
-            "x": 5.208588203701103,
-            "y": 4.163420547805767,
-            "heading": 2.6756025601758724,
-            "angularVelocity": 1.7900060187599918,
-            "velocityX": 2.8873958628376504,
-            "velocityY": -1.4467640934590689,
-            "timestamp": 1.4219636132494484
-        },
-        {
-            "x": 5.297563997608419,
-            "y": 4.1261005954207315,
-            "heading": 2.720276777571372,
-            "angularVelocity": 1.5304982987457612,
-            "velocityX": 3.048230257714994,
-            "velocityY": -1.2785478283571272,
-            "timestamp": 1.4511529416087874
-        },
-        {
-            "x": 5.390789031982422,
-            "y": 4.09412145614624,
-            "heading": 2.757748451080741,
-            "angularVelocity": 1.2837456569082273,
-            "velocityX": 3.1938053944353855,
-            "velocityY": -1.0955763997309995,
-            "timestamp": 1.4803422699681263
-        },
-        {
-            "x": 5.532416911390752,
-            "y": 4.059853570653742,
-            "heading": 2.796595493071039,
-            "angularVelocity": 0.9277997104504593,
-            "velocityX": 3.3825562713263437,
-            "velocityY": -0.818433852585974,
-            "timestamp": 1.522212343382395
-        },
-        {
-            "x": 5.681515537517707,
-            "y": 4.021534249773671,
-            "heading": 2.782742222711128,
-            "angularVelocity": -0.3308632928068908,
-            "velocityX": 3.5609831550031092,
-            "velocityY": -0.9151959324488058,
-            "timestamp": 1.5640824167966638
-        },
-        {
-            "x": 5.826340094588043,
-            "y": 3.969241008458542,
-            "heading": 2.7680381624736805,
-            "angularVelocity": -0.351183053632681,
-            "velocityX": 3.4589038246343846,
-            "velocityY": -1.2489407600921083,
-            "timestamp": 1.6059524902109326
-        },
-        {
-            "x": 5.965226819905552,
-            "y": 3.9129528098786466,
-            "heading": 2.7396888789742415,
-            "angularVelocity": -0.6770774729470177,
-            "velocityX": 3.317088172818383,
-            "velocityY": -1.3443539499673571,
-            "timestamp": 1.6478225636252013
-        },
-        {
-            "x": 6.097371617524639,
-            "y": 3.854212614126153,
-            "heading": 2.694967938521201,
-            "angularVelocity": -1.0680884174853156,
-            "velocityX": 3.1560679703526437,
-            "velocityY": -1.402915996141418,
-            "timestamp": 1.68969263703947
-        },
-        {
-            "x": 6.217931948857121,
-            "y": 3.789923024350862,
-            "heading": 2.6256097109731904,
-            "angularVelocity": -1.6565107699184056,
-            "velocityX": 2.8793914483893763,
-            "velocityY": -1.535454431598441,
-            "timestamp": 1.7315627104537388
-        },
-        {
-            "x": 6.32897922975643,
-            "y": 3.7148340577600942,
-            "heading": 2.547026684843302,
-            "angularVelocity": -1.8768303879569581,
-            "velocityX": 2.652187394098668,
-            "velocityY": -1.7933803422751633,
-            "timestamp": 1.7734327838680075
-        },
-        {
-            "x": 6.440862810984381,
-            "y": 3.629355422988249,
-            "heading": 2.48919642870263,
-            "angularVelocity": -1.381183538144076,
-            "velocityX": 2.672161095132505,
-            "velocityY": -2.0415210149288825,
-            "timestamp": 1.8153028572822762
-        },
-        {
-            "x": 6.556707402178449,
-            "y": 3.5350910926988375,
-            "heading": 2.4587564932761583,
-            "angularVelocity": -0.7270093635923316,
-            "velocityX": 2.766763507861174,
-            "velocityY": -2.2513533558144507,
-            "timestamp": 1.857172930696545
-        },
-        {
-            "x": 6.674645090324846,
-            "y": 3.429370276775565,
-            "heading": 2.4587373064570563,
-            "angularVelocity": -0.00045824660759140524,
-            "velocityX": 2.816753794040547,
-            "velocityY": -2.5249732637737297,
-            "timestamp": 1.8990430041108137
-        },
-        {
-            "x": 6.791280321048844,
-            "y": 3.3222067883695963,
-            "heading": 2.45873486083509,
-            "angularVelocity": -0.00005840978453659185,
-            "velocityX": 2.785646673460325,
-            "velocityY": -2.559429197691564,
-            "timestamp": 1.9409130775250825
-        },
-        {
-            "x": 6.907916035753451,
-            "y": 3.2150438274589583,
-            "heading": 2.4587324135695865,
-            "angularVelocity": -0.000058449037785704465,
-            "velocityX": 2.78565823256613,
-            "velocityY": -2.5594165993059286,
-            "timestamp": 1.9827831509393512
-        },
-        {
-            "x": 7.020962902689577,
-            "y": 3.1172887235192603,
-            "heading": 2.428615335233058,
-            "angularVelocity": -0.7192984363448716,
-            "velocityX": 2.6999443210338705,
-            "velocityY": -2.334724923276202,
-            "timestamp": 2.02465322435362
-        },
-        {
-            "x": 7.121900964071713,
-            "y": 3.0192154080188542,
-            "heading": 2.3705886859446603,
-            "angularVelocity": -1.385874075602246,
-            "velocityX": 2.4107447909976107,
-            "velocityY": -2.3423248994587165,
-            "timestamp": 2.0665232977678887
-        },
-        {
-            "x": 7.210190441218643,
-            "y": 2.92089200334631,
-            "heading": 2.287196223783446,
-            "angularVelocity": -1.9916961056197946,
-            "velocityX": 2.10865350708562,
-            "velocityY": -2.348297880916461,
-            "timestamp": 2.1083933711821574
-        },
-        {
-            "x": 7.2890247338542125,
-            "y": 2.830773686809754,
-            "heading": 2.1745743754916034,
-            "angularVelocity": -2.689793427815253,
-            "velocityX": 1.8828314881507633,
-            "velocityY": -2.1523324223703058,
-            "timestamp": 2.150263444596426
-        },
-        {
-            "x": 7.359953440317656,
-            "y": 2.7509272688501483,
-            "heading": 2.035955336818745,
-            "angularVelocity": -3.3106949037643396,
-            "velocityX": 1.6940191568728353,
-            "velocityY": -1.9070044890916185,
-            "timestamp": 2.192133518010695
-        },
-        {
-            "x": 7.4240572342258595,
-            "y": 2.6822786221440054,
-            "heading": 1.872800890040008,
-            "angularVelocity": -3.896684038847076,
-            "velocityX": 1.5310169933057254,
-            "velocityY": -1.6395635619484732,
-            "timestamp": 2.2340035914249636
-        },
-        {
-            "x": 7.482217323317858,
-            "y": 2.6251237224270416,
-            "heading": 1.684191854325982,
-            "angularVelocity": -4.504626343687046,
-            "velocityX": 1.3890610727273844,
-            "velocityY": -1.3650537258787387,
-            "timestamp": 2.2758736648392324
-        },
-        {
-            "x": 7.535468730024687,
-            "y": 2.5792763511542147,
-            "heading": 1.466995470236355,
-            "angularVelocity": -5.187389616938403,
-            "velocityX": 1.2718250140130243,
-            "velocityY": -1.0949914230912903,
-            "timestamp": 2.317743738253501
-        },
-        {
-            "x": 7.580274182888533,
-            "y": 2.5411410245640176,
-            "heading": 1.2146613001768354,
-            "angularVelocity": -6.02659965658257,
-            "velocityX": 1.0701068617801175,
-            "velocityY": -0.91080152195771,
-            "timestamp": 2.35961381166777
-        },
-        {
-            "x": 7.616169603666072,
-            "y": 2.509680561991982,
-            "heading": 0.922946754094809,
-            "angularVelocity": -6.967137200734248,
-            "velocityX": 0.8573049400316086,
-            "velocityY": -0.7513830286553573,
-            "timestamp": 2.4014838850820386
-        },
-        {
-            "x": 7.640414148022587,
-            "y": 2.4865073161659272,
-            "heading": 0.6422191973798425,
-            "angularVelocity": -6.704730463149791,
-            "velocityX": 0.5790423178062244,
-            "velocityY": -0.5534560590991822,
-            "timestamp": 2.4433539584963073
-        },
-        {
-            "x": 7.656220323606262,
-            "y": 2.4718614980447855,
-            "heading": 0.3932969564478367,
-            "angularVelocity": -5.9451111649394885,
-            "velocityX": 0.37750532288984423,
-            "velocityY": -0.34979203347062976,
-            "timestamp": 2.485224031910576
-        },
-        {
-            "x": 7.668143711832241,
-            "y": 2.4642737767821443,
-            "heading": 0.1972336516813064,
-            "angularVelocity": -4.682659684559189,
-            "velocityX": 0.28477113254632147,
-            "velocityY": -0.18122063430765137,
-            "timestamp": 2.527094105324845
-        },
-        {
-            "x": 7.676158456210791,
-            "y": 2.459440783193878,
-            "heading": 0.06602780599823356,
-            "angularVelocity": -3.1336425992116688,
-            "velocityX": 0.19141940113768102,
-            "velocityY": -0.11542835238065828,
-            "timestamp": 2.5689641787391135
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 1.4385689598890085e-34,
-            "angularVelocity": -1.576968956919288,
-            "velocityX": 0.09174915341561693,
-            "velocityY": -0.05829421815741107,
-            "timestamp": 2.6108342521533823
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 5.954582727394106e-35,
-            "angularVelocity": -1.9052580558070114e-33,
-            "velocityX": -2.275662344047759e-34,
-            "velocityY": 3.071010312134925e-34,
-            "timestamp": 2.652704325567651
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.1.traj
deleted file mode 100644
index cfa6b046..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.1.traj	
+++ /dev/null
@@ -1,625 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 6.892560738534845e-27,
-            "timestamp": 0
-        },
-        {
-            "x": 2.24821488195175,
-            "y": 5.550858605907564,
-            "heading": -1.0860081979744884e-8,
-            "angularVelocity": -3.249360952601713e-7,
-            "velocityX": -0.053411135206172675,
-            "velocityY": -0.27351257600550494,
-            "timestamp": 0.0334222075852833
-        },
-        {
-            "x": 2.2446446457975586,
-            "y": 5.532575817984525,
-            "heading": -3.253781151640391e-8,
-            "angularVelocity": -6.486025430910346e-7,
-            "velocityX": -0.10682227213842765,
-            "velocityY": -0.5470251441769277,
-            "timestamp": 0.0668444151705666
-        },
-        {
-            "x": 2.2392892913368425,
-            "y": 5.505151636566201,
-            "heading": -6.492851000422694e-8,
-            "angularVelocity": -9.691369834959166e-7,
-            "velocityX": -0.1602334150722015,
-            "velocityY": -0.8205377023156053,
-            "timestamp": 0.10026662275584991
-        },
-        {
-            "x": 2.2321488181807467,
-            "y": 5.468586062083933,
-            "heading": -1.0785000725838172e-7,
-            "angularVelocity": -0.000001284220850944888,
-            "velocityX": -0.21364456964054193,
-            "velocityY": -1.0940502475484135,
-            "timestamp": 0.1336888303411332
-        },
-        {
-            "x": 2.2232232256851456,
-            "y": 5.42287909509794,
-            "heading": -1.6102300741445552e-7,
-            "angularVelocity": -0.0000015909481231905412,
-            "velocityX": -0.26705574348127187,
-            "velocityY": -1.3675627760193463,
-            "timestamp": 0.16711103792641652
-        },
-        {
-            "x": 2.212512512848178,
-            "y": 5.368030736346982,
-            "heading": -2.2404505707142586e-7,
-            "angularVelocity": -0.0000018856338989468898,
-            "velocityX": -0.32046694729784664,
-            "velocityY": -1.6410752823864576,
-            "timestamp": 0.20053324551169982
-        },
-        {
-            "x": 2.200016678146418,
-            "y": 5.304040986826963,
-            "heading": -2.963548114247805e-7,
-            "angularVelocity": -0.000002163524111560841,
-            "velocityX": -0.3738781966956197,
-            "velocityY": -1.9145877589558649,
-            "timestamp": 0.23395545309698312
-        },
-        {
-            "x": 2.1857357192573095,
-            "y": 5.230909847922857,
-            "heading": -3.7718073304542066e-7,
-            "angularVelocity": -0.0000024183297502514874,
-            "velocityX": -0.4272895155847271,
-            "velocityY": -2.1881001940865983,
-            "timestamp": 0.2673776606822664
-        },
-        {
-            "x": 2.169669632551276,
-            "y": 5.148637321647842,
-            "heading": -4.654625982128869e-7,
-            "angularVelocity": -0.0000026414132559778984,
-            "velocityX": -0.48070094307146977,
-            "velocityY": -2.4616125689825754,
-            "timestamp": 0.30079986826754973
-        },
-        {
-            "x": 2.15181841205741,
-            "y": 5.057223411125392,
-            "heading": -5.597189955047307e-7,
-            "angularVelocity": -0.0000028201725975039026,
-            "velocityX": -0.5341125492089794,
-            "velocityY": -2.735124850423542,
-            "timestamp": 0.33422207585283303
-        },
-        {
-            "x": 2.1321820469941515,
-            "y": 4.956668121725741,
-            "heading": -6.57784320236276e-7,
-            "angularVelocity": -0.0000029341365576412115,
-            "velocityX": -0.5875244779329194,
-            "velocityY": -3.008636971185932,
-            "timestamp": 0.36764428343811634
-        },
-        {
-            "x": 2.1107605140888954,
-            "y": 4.846971464532025,
-            "heading": -7.56114109936986e-7,
-            "angularVelocity": -0.0000029420493990798148,
-            "velocityX": -0.6409371029779476,
-            "velocityY": -3.282148760338516,
-            "timestamp": 0.40106649102339964
-        },
-        {
-            "x": 2.087553735835974,
-            "y": 4.728133474365802,
-            "heading": -8.465523402036467e-7,
-            "angularVelocity": -0.0000027059321418483588,
-            "velocityX": -0.6943520470083785,
-            "velocityY": -3.5556595076225634,
-            "timestamp": 0.43448869860868294
-        },
-        {
-            "x": 2.0633251233427066,
-            "y": 4.604042454619442,
-            "heading": -8.465523885072994e-7,
-            "angularVelocity": -1.4452562228577176e-12,
-            "velocityX": -0.724925558303899,
-            "velocityY": -3.712831339152857,
-            "timestamp": 0.46791090619396625
-        },
-        {
-            "x": 2.039088654512122,
-            "y": 4.479952969280304,
-            "heading": -8.465523654496814e-7,
-            "angularVelocity": 6.898891300327124e-13,
-            "velocityX": -0.725160621670614,
-            "velocityY": -3.7127854293461913,
-            "timestamp": 0.5013331137792496
-        },
-        {
-            "x": 2.0148473415026222,
-            "y": 4.355864430051467,
-            "heading": -8.465523174606992e-7,
-            "angularVelocity": 1.4358411666699279e-12,
-            "velocityX": -0.7253055606758912,
-            "velocityY": -3.712757121497041,
-            "timestamp": 0.5347553213645329
-        },
-        {
-            "x": 1.9907968545860597,
-            "y": 4.231738764257757,
-            "heading": -8.465522534990754e-7,
-            "angularVelocity": 1.9137462023851052e-12,
-            "velocityX": -0.7195960008785613,
-            "velocityY": -3.7138679567028667,
-            "timestamp": 0.5681775289498161
-        },
-        {
-            "x": 1.9669031646192998,
-            "y": 4.107582820499775,
-            "heading": -8.465521766194915e-7,
-            "angularVelocity": 2.3002544763334585e-12,
-            "velocityX": -0.7149046006511871,
-            "velocityY": -3.7147738802470687,
-            "timestamp": 0.6015997365350993
-        },
-        {
-            "x": 1.9504985155072192,
-            "y": 3.982217377752346,
-            "heading": -8.465520941748615e-7,
-            "angularVelocity": 2.4667619198655976e-12,
-            "velocityX": -0.4908308067350749,
-            "velocityY": -3.7509623631996885,
-            "timestamp": 0.6350219441203826
-        },
-        {
-            "x": 1.94330794196602,
-            "y": 3.855987816020002,
-            "heading": -8.465520093116816e-7,
-            "angularVelocity": 2.5391254773053977e-12,
-            "velocityX": -0.2151435844765948,
-            "velocityY": -3.7768169984060798,
-            "timestamp": 0.6684441517056658
-        },
-        {
-            "x": 1.9454293697222904,
-            "y": 3.7295714172218672,
-            "heading": -8.465519250884413e-7,
-            "angularVelocity": 2.519978339764665e-12,
-            "velocityX": 0.0634735976376205,
-            "velocityY": -3.7824072056149025,
-            "timestamp": 0.7018663592909491
-        },
-        {
-            "x": 1.9568513812515056,
-            "y": 3.603654207925799,
-            "heading": -8.465518451486746e-7,
-            "angularVelocity": 2.391815832767542e-12,
-            "velocityX": 0.34174916483985474,
-            "velocityY": -3.767471342961152,
-            "timestamp": 0.7352885668762323
-        },
-        {
-            "x": 1.9775120291721782,
-            "y": 3.4789195174340257,
-            "heading": -8.465517739710846e-7,
-            "angularVelocity": 2.129649524174011e-12,
-            "velocityX": 0.6181712523959566,
-            "velocityY": -3.7320901132416737,
-            "timestamp": 0.7687107744615156
-        },
-        {
-            "x": 2.0072992100178335,
-            "y": 3.3560442616931043,
-            "heading": -8.465517105809759e-7,
-            "angularVelocity": 1.8966463157464128e-12,
-            "velocityX": 0.8912391789143821,
-            "velocityY": -3.6764554055067418,
-            "timestamp": 0.8021329820467988
-        },
-        {
-            "x": 2.046051283468891,
-            "y": 3.235695267642527,
-            "heading": -8.465516527951994e-7,
-            "angularVelocity": 1.7289634182989629e-12,
-            "velocityX": 1.1594707905613406,
-            "velocityY": -3.600869085126494,
-            "timestamp": 0.8355551896320821
-        },
-        {
-            "x": 2.0935579534966013,
-            "y": 3.1185256537724477,
-            "heading": -8.46551596321545e-7,
-            "angularVelocity": 1.6897044173558387e-12,
-            "velocityX": 1.421410297528723,
-            "velocityY": -3.5057413120014775,
-            "timestamp": 0.8689773972173653
-        },
-        {
-            "x": 2.1495614051818848,
-            "y": 3.005171298980713,
-            "heading": -8.465515057383081e-7,
-            "angularVelocity": 2.7102707524537993e-12,
-            "velocityX": 1.6756359238847198,
-            "velocityY": -3.3915878986274275,
-            "timestamp": 0.9023996048026486
-        },
-        {
-            "x": 2.288743031901554,
-            "y": 2.8026748842621148,
-            "heading": -8.46551464739476e-7,
-            "angularVelocity": 6.312009526868921e-13,
-            "velocityX": 2.142782210187666,
-            "velocityY": -3.117550249366458,
-            "timestamp": 0.9673533019435205
-        },
-        {
-            "x": 2.455414708004788,
-            "y": 2.6221288200825,
-            "heading": -8.46551435217149e-7,
-            "angularVelocity": 4.5451345461171864e-13,
-            "velocityX": 2.566007532127087,
-            "velocityY": -2.779611817758129,
-            "timestamp": 1.0323069990843925
-        },
-        {
-            "x": 2.646160177401748,
-            "y": 2.4672337011827246,
-            "heading": -8.465514075797998e-7,
-            "angularVelocity": 4.254931139136813e-13,
-            "velocityX": 2.9366376017527633,
-            "velocityY": -2.384700574689416,
-            "timestamp": 1.0972606962252645
-        },
-        {
-            "x": 2.857069726290126,
-            "y": 2.341164292711998,
-            "heading": -8.465513787578765e-7,
-            "angularVelocity": 4.4373032078954734e-13,
-            "velocityX": 3.2470753501649696,
-            "velocityY": -1.9409119729950552,
-            "timestamp": 1.1622143933661364
-        },
-        {
-            "x": 3.0838173053505575,
-            "y": 2.2464971410417167,
-            "heading": -8.465513471236816e-7,
-            "angularVelocity": 4.870268741320482e-13,
-            "velocityX": 3.490911049579749,
-            "velocityY": -1.457455939200772,
-            "timestamp": 1.2271680905070084
-        },
-        {
-            "x": 3.3145112489857773,
-            "y": 2.1619002222864983,
-            "heading": -8.465513138187862e-7,
-            "angularVelocity": 5.127482864627404e-13,
-            "velocityX": 3.5516676307835056,
-            "velocityY": -1.3024188380298123,
-            "timestamp": 1.2921217876478803
-        },
-        {
-            "x": 3.5452013267301767,
-            "y": 2.0772927620920076,
-            "heading": -8.465512839726007e-7,
-            "angularVelocity": 4.594994306997602e-13,
-            "velocityX": 3.5516081131487938,
-            "velocityY": -1.302581129610464,
-            "timestamp": 1.3570754847887523
-        },
-        {
-            "x": 3.77589133724735,
-            "y": 1.9926851185956267,
-            "heading": -8.465512625373994e-7,
-            "angularVelocity": 3.3000743037279784e-13,
-            "velocityX": 3.5516070781466733,
-            "velocityY": -1.302583951649345,
-            "timestamp": 1.4220291819296242
-        },
-        {
-            "x": 4.006581361028944,
-            "y": 1.9080775112659911,
-            "heading": -8.465512535495126e-7,
-            "angularVelocity": 1.3837377280671698e-13,
-            "velocityX": 3.5516072823601217,
-            "velocityY": -1.302583394841325,
-            "timestamp": 1.4869828790704962
-        },
-        {
-            "x": 4.2372714040114055,
-            "y": 1.823469956289169,
-            "heading": -8.465512575529088e-7,
-            "angularVelocity": -6.163459506326445e-14,
-            "velocityX": 3.551607577968653,
-            "velocityY": -1.3025825888392473,
-            "timestamp": 1.5519365762113682
-        },
-        {
-            "x": 4.467961461705261,
-            "y": 1.7388624414245573,
-            "heading": -8.465512722492451e-7,
-            "angularVelocity": -2.2625863777166434e-13,
-            "velocityX": 3.5516078044591044,
-            "velocityY": -1.3025819712885556,
-            "timestamp": 1.6168902733522401
-        },
-        {
-            "x": 4.698651553926265,
-            "y": 1.6542550207002664,
-            "heading": -8.465512926518018e-7,
-            "angularVelocity": -3.141092229421582e-13,
-            "velocityX": 3.551608336024854,
-            "velocityY": -1.3025805219428788,
-            "timestamp": 1.681843970493112
-        },
-        {
-            "x": 4.929586887359619,
-            "y": 1.5703192949295044,
-            "heading": -8.465513064459796e-7,
-            "angularVelocity": -2.1236938120360531e-13,
-            "velocityX": 3.555383967330626,
-            "velocityY": -1.2922393869021438,
-            "timestamp": 1.746797667633984
-        },
-        {
-            "x": 5.059495672066219,
-            "y": 1.5327262216225606,
-            "heading": -8.465512750899748e-7,
-            "angularVelocity": 8.770995258558909e-13,
-            "velocityX": 3.633847296905686,
-            "velocityY": -1.051564666141291,
-            "timestamp": 1.7825473219513848
-        },
-        {
-            "x": 5.189548300230332,
-            "y": 1.4956338313822377,
-            "heading": -8.465512186056332e-7,
-            "angularVelocity": 1.579996862146135e-12,
-            "velocityX": 3.637870929029155,
-            "velocityY": -1.0375594099736098,
-            "timestamp": 1.8182969762687855
-        },
-        {
-            "x": 5.319623170079142,
-            "y": 1.4586195133360007,
-            "heading": -8.46551136487956e-7,
-            "angularVelocity": 2.2970201754243682e-12,
-            "velocityX": 3.638493080071459,
-            "velocityY": -1.0353755512600933,
-            "timestamp": 1.8540466305861862
-        },
-        {
-            "x": 5.4496989055100356,
-            "y": 1.42160823723968,
-            "heading": -8.465510387564672e-7,
-            "angularVelocity": 2.7337743725450157e-12,
-            "velocityX": 3.6385172923918394,
-            "velocityY": -1.0352904609291835,
-            "timestamp": 1.889796284903587
-        },
-        {
-            "x": 5.57977446957584,
-            "y": 1.3845963588884114,
-            "heading": -8.465509335637302e-7,
-            "angularVelocity": 2.9424826347181554e-12,
-            "velocityX": 3.6385124989164392,
-            "velocityY": -1.03530730738433,
-            "timestamp": 1.9255459392209877
-        },
-        {
-            "x": 5.709850057975644,
-            "y": 1.3475845660573411,
-            "heading": -8.465508277220688e-7,
-            "angularVelocity": 2.960634540295608e-12,
-            "velocityX": 3.6385131795943324,
-            "velocityY": -1.0353049151878095,
-            "timestamp": 1.9612955935383884
-        },
-        {
-            "x": 5.839925662966751,
-            "y": 1.3105728315354166,
-            "heading": -8.465507264753305e-7,
-            "angularVelocity": 2.832103981671134e-12,
-            "velocityX": 3.6385136436912036,
-            "velocityY": -1.0353032841469862,
-            "timestamp": 1.9970452478557892
-        },
-        {
-            "x": 5.970001275498046,
-            "y": 1.273561123513075,
-            "heading": -8.465506337302748e-7,
-            "angularVelocity": 2.594292378130792e-12,
-            "velocityX": 3.638513854607602,
-            "velocityY": -1.0353025428927405,
-            "timestamp": 2.03279490217319
-        },
-        {
-            "x": 6.100076894564937,
-            "y": 1.2365494384597497,
-            "heading": -8.46550552278632e-7,
-            "angularVelocity": 2.2783896618382965e-12,
-            "velocityX": 3.6385140374232323,
-            "velocityY": -1.0353019003965735,
-            "timestamp": 2.0685445564905907
-        },
-        {
-            "x": 6.230152520105601,
-            "y": 1.1995377761581834,
-            "heading": -8.465504825990077e-7,
-            "angularVelocity": 1.9490992420524453e-12,
-            "velocityX": 3.6385142185095396,
-            "velocityY": -1.03530126397757,
-            "timestamp": 2.1042942108079914
-        },
-        {
-            "x": 6.3602281516940815,
-            "y": 1.16252613511139,
-            "heading": -8.465504225140753e-7,
-            "angularVelocity": 1.680713660601537e-12,
-            "velocityX": 3.6385143876808734,
-            "velocityY": -1.0353006694327234,
-            "timestamp": 2.140043865125392
-        },
-        {
-            "x": 6.490303788775648,
-            "y": 1.1255145133698101,
-            "heading": -8.465503700121188e-7,
-            "angularVelocity": 1.4686003981542915e-12,
-            "velocityX": 3.638514541335137,
-            "velocityY": -1.0353001294215285,
-            "timestamp": 2.175793519442793
-        },
-        {
-            "x": 6.62037943067546,
-            "y": 1.0885029085617548,
-            "heading": -8.465503232940394e-7,
-            "angularVelocity": 1.306812060469801e-12,
-            "velocityX": 3.638514676112538,
-            "velocityY": -1.0352996557519205,
-            "timestamp": 2.2115431737601936
-        },
-        {
-            "x": 6.7504550766965234,
-            "y": 1.0514913182376744,
-            "heading": -8.465502808582129e-7,
-            "angularVelocity": 1.187027603302574e-12,
-            "velocityX": 3.638514791393393,
-            "velocityY": -1.0352992506018621,
-            "timestamp": 2.2472928280775943
-        },
-        {
-            "x": 6.880530727542945,
-            "y": 1.0144797448710672,
-            "heading": -8.465502433332782e-7,
-            "angularVelocity": 1.0496586699357012e-12,
-            "velocityX": 3.6385149263697616,
-            "velocityY": -1.0352987762623522,
-            "timestamp": 2.283042482394995
-        },
-        {
-            "x": 7.003525953233288,
-            "y": 0.9794828656529073,
-            "heading": -7.145353891597533e-7,
-            "angularVelocity": 0.000003692758900357643,
-            "velocityX": 3.440459160761053,
-            "velocityY": -0.9789431502593731,
-            "timestamp": 2.318792136712396
-        },
-        {
-            "x": 7.116271603762692,
-            "y": 0.9474023877264399,
-            "heading": -5.944969754078434e-7,
-            "angularVelocity": 0.0000033577503345576707,
-            "velocityX": 3.153754985388111,
-            "velocityY": -0.8973647029323183,
-            "timestamp": 2.3545417910297965
-        },
-        {
-            "x": 7.218767661219051,
-            "y": 0.9182383156679585,
-            "heading": -4.859087151281335e-7,
-            "angularVelocity": 0.0000030374632243726353,
-            "velocityX": 2.867050308972391,
-            "velocityY": -0.8157861275958178,
-            "timestamp": 2.3902914453471973
-        },
-        {
-            "x": 7.311014119553375,
-            "y": 0.8919906507276494,
-            "heading": -3.8850367492824467e-7,
-            "angularVelocity": 0.000002724642854703664,
-            "velocityX": 2.580345463352478,
-            "velocityY": -0.7342075172887301,
-            "timestamp": 2.426041099664598
-        },
-        {
-            "x": 7.393010975686544,
-            "y": 0.8686593933386082,
-            "heading": -3.0210953963079075e-7,
-            "angularVelocity": 0.0000024166425366030095,
-            "velocityX": 2.293640531602499,
-            "velocityY": -0.6526288948669626,
-            "timestamp": 2.4617907539819988
-        },
-        {
-            "x": 7.464758227730078,
-            "y": 0.8482445436165693,
-            "heading": -2.2661033918382436e-7,
-            "angularVelocity": 0.000002111886167406066,
-            "velocityX": 2.006935547027439,
-            "velocityY": -0.5710502692078399,
-            "timestamp": 2.4975404082993995
-        },
-        {
-            "x": 7.526255874392654,
-            "y": 0.8307461015250325,
-            "heading": -1.6193015142138781e-7,
-            "angularVelocity": 0.0000018092535156938384,
-            "velocityX": 1.7202305263310733,
-            "velocityY": -0.48947164456971237,
-            "timestamp": 2.5332900626168002
-        },
-        {
-            "x": 7.577503914725456,
-            "y": 0.8161640669449958,
-            "heading": -1.0802379214331377e-7,
-            "angularVelocity": 0.0000015078847699136293,
-            "velocityX": 1.4335254790941394,
-            "velocityY": -0.4078930232603403,
-            "timestamp": 2.569039716934201
-        },
-        {
-            "x": 7.618502347994633,
-            "y": 0.8044984397090525,
-            "heading": -6.487055743300418e-8,
-            "angularVelocity": 0.0000012070951602673883,
-            "velocityX": 1.1468204113297347,
-            "velocityY": -0.3263144066337203,
-            "timestamp": 2.6047893712516017
-        },
-        {
-            "x": 7.649251173610278,
-            "y": 0.7957492196197445,
-            "heading": -3.246962719134405e-8,
-            "angularVelocity": 9.063284906782187e-7,
-            "velocityX": 0.8601153270642448,
-            "velocityY": -0.24473579553046104,
-            "timestamp": 2.6405390255690024
-        },
-        {
-            "x": 7.669750391083677,
-            "y": 0.7899164064601075,
-            "heading": -1.0836483282929717e-8,
-            "angularVelocity": 6.051287577519143e-7,
-            "velocityX": 0.573410229128317,
-            "velocityY": -0.16315719049619,
-            "timestamp": 2.676288679886403
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 2.0494644799121527e-30,
-            "angularVelocity": 3.031213445055282e-7,
-            "velocityX": 0.28670511958863415,
-            "velocityY": -0.08157859189950141,
-            "timestamp": 2.712038334203804
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -1.7051999701665118e-29,
-            "angularVelocity": -2.5846596071248955e-28,
-            "velocityX": 4.524644517512267e-29,
-            "velocityY": 1.710870028377411e-29,
-            "timestamp": 2.7477879885212046
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.traj
deleted file mode 100644
index cfa6b046..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.traj	
+++ /dev/null
@@ -1,625 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 6.892560738534845e-27,
-            "timestamp": 0
-        },
-        {
-            "x": 2.24821488195175,
-            "y": 5.550858605907564,
-            "heading": -1.0860081979744884e-8,
-            "angularVelocity": -3.249360952601713e-7,
-            "velocityX": -0.053411135206172675,
-            "velocityY": -0.27351257600550494,
-            "timestamp": 0.0334222075852833
-        },
-        {
-            "x": 2.2446446457975586,
-            "y": 5.532575817984525,
-            "heading": -3.253781151640391e-8,
-            "angularVelocity": -6.486025430910346e-7,
-            "velocityX": -0.10682227213842765,
-            "velocityY": -0.5470251441769277,
-            "timestamp": 0.0668444151705666
-        },
-        {
-            "x": 2.2392892913368425,
-            "y": 5.505151636566201,
-            "heading": -6.492851000422694e-8,
-            "angularVelocity": -9.691369834959166e-7,
-            "velocityX": -0.1602334150722015,
-            "velocityY": -0.8205377023156053,
-            "timestamp": 0.10026662275584991
-        },
-        {
-            "x": 2.2321488181807467,
-            "y": 5.468586062083933,
-            "heading": -1.0785000725838172e-7,
-            "angularVelocity": -0.000001284220850944888,
-            "velocityX": -0.21364456964054193,
-            "velocityY": -1.0940502475484135,
-            "timestamp": 0.1336888303411332
-        },
-        {
-            "x": 2.2232232256851456,
-            "y": 5.42287909509794,
-            "heading": -1.6102300741445552e-7,
-            "angularVelocity": -0.0000015909481231905412,
-            "velocityX": -0.26705574348127187,
-            "velocityY": -1.3675627760193463,
-            "timestamp": 0.16711103792641652
-        },
-        {
-            "x": 2.212512512848178,
-            "y": 5.368030736346982,
-            "heading": -2.2404505707142586e-7,
-            "angularVelocity": -0.0000018856338989468898,
-            "velocityX": -0.32046694729784664,
-            "velocityY": -1.6410752823864576,
-            "timestamp": 0.20053324551169982
-        },
-        {
-            "x": 2.200016678146418,
-            "y": 5.304040986826963,
-            "heading": -2.963548114247805e-7,
-            "angularVelocity": -0.000002163524111560841,
-            "velocityX": -0.3738781966956197,
-            "velocityY": -1.9145877589558649,
-            "timestamp": 0.23395545309698312
-        },
-        {
-            "x": 2.1857357192573095,
-            "y": 5.230909847922857,
-            "heading": -3.7718073304542066e-7,
-            "angularVelocity": -0.0000024183297502514874,
-            "velocityX": -0.4272895155847271,
-            "velocityY": -2.1881001940865983,
-            "timestamp": 0.2673776606822664
-        },
-        {
-            "x": 2.169669632551276,
-            "y": 5.148637321647842,
-            "heading": -4.654625982128869e-7,
-            "angularVelocity": -0.0000026414132559778984,
-            "velocityX": -0.48070094307146977,
-            "velocityY": -2.4616125689825754,
-            "timestamp": 0.30079986826754973
-        },
-        {
-            "x": 2.15181841205741,
-            "y": 5.057223411125392,
-            "heading": -5.597189955047307e-7,
-            "angularVelocity": -0.0000028201725975039026,
-            "velocityX": -0.5341125492089794,
-            "velocityY": -2.735124850423542,
-            "timestamp": 0.33422207585283303
-        },
-        {
-            "x": 2.1321820469941515,
-            "y": 4.956668121725741,
-            "heading": -6.57784320236276e-7,
-            "angularVelocity": -0.0000029341365576412115,
-            "velocityX": -0.5875244779329194,
-            "velocityY": -3.008636971185932,
-            "timestamp": 0.36764428343811634
-        },
-        {
-            "x": 2.1107605140888954,
-            "y": 4.846971464532025,
-            "heading": -7.56114109936986e-7,
-            "angularVelocity": -0.0000029420493990798148,
-            "velocityX": -0.6409371029779476,
-            "velocityY": -3.282148760338516,
-            "timestamp": 0.40106649102339964
-        },
-        {
-            "x": 2.087553735835974,
-            "y": 4.728133474365802,
-            "heading": -8.465523402036467e-7,
-            "angularVelocity": -0.0000027059321418483588,
-            "velocityX": -0.6943520470083785,
-            "velocityY": -3.5556595076225634,
-            "timestamp": 0.43448869860868294
-        },
-        {
-            "x": 2.0633251233427066,
-            "y": 4.604042454619442,
-            "heading": -8.465523885072994e-7,
-            "angularVelocity": -1.4452562228577176e-12,
-            "velocityX": -0.724925558303899,
-            "velocityY": -3.712831339152857,
-            "timestamp": 0.46791090619396625
-        },
-        {
-            "x": 2.039088654512122,
-            "y": 4.479952969280304,
-            "heading": -8.465523654496814e-7,
-            "angularVelocity": 6.898891300327124e-13,
-            "velocityX": -0.725160621670614,
-            "velocityY": -3.7127854293461913,
-            "timestamp": 0.5013331137792496
-        },
-        {
-            "x": 2.0148473415026222,
-            "y": 4.355864430051467,
-            "heading": -8.465523174606992e-7,
-            "angularVelocity": 1.4358411666699279e-12,
-            "velocityX": -0.7253055606758912,
-            "velocityY": -3.712757121497041,
-            "timestamp": 0.5347553213645329
-        },
-        {
-            "x": 1.9907968545860597,
-            "y": 4.231738764257757,
-            "heading": -8.465522534990754e-7,
-            "angularVelocity": 1.9137462023851052e-12,
-            "velocityX": -0.7195960008785613,
-            "velocityY": -3.7138679567028667,
-            "timestamp": 0.5681775289498161
-        },
-        {
-            "x": 1.9669031646192998,
-            "y": 4.107582820499775,
-            "heading": -8.465521766194915e-7,
-            "angularVelocity": 2.3002544763334585e-12,
-            "velocityX": -0.7149046006511871,
-            "velocityY": -3.7147738802470687,
-            "timestamp": 0.6015997365350993
-        },
-        {
-            "x": 1.9504985155072192,
-            "y": 3.982217377752346,
-            "heading": -8.465520941748615e-7,
-            "angularVelocity": 2.4667619198655976e-12,
-            "velocityX": -0.4908308067350749,
-            "velocityY": -3.7509623631996885,
-            "timestamp": 0.6350219441203826
-        },
-        {
-            "x": 1.94330794196602,
-            "y": 3.855987816020002,
-            "heading": -8.465520093116816e-7,
-            "angularVelocity": 2.5391254773053977e-12,
-            "velocityX": -0.2151435844765948,
-            "velocityY": -3.7768169984060798,
-            "timestamp": 0.6684441517056658
-        },
-        {
-            "x": 1.9454293697222904,
-            "y": 3.7295714172218672,
-            "heading": -8.465519250884413e-7,
-            "angularVelocity": 2.519978339764665e-12,
-            "velocityX": 0.0634735976376205,
-            "velocityY": -3.7824072056149025,
-            "timestamp": 0.7018663592909491
-        },
-        {
-            "x": 1.9568513812515056,
-            "y": 3.603654207925799,
-            "heading": -8.465518451486746e-7,
-            "angularVelocity": 2.391815832767542e-12,
-            "velocityX": 0.34174916483985474,
-            "velocityY": -3.767471342961152,
-            "timestamp": 0.7352885668762323
-        },
-        {
-            "x": 1.9775120291721782,
-            "y": 3.4789195174340257,
-            "heading": -8.465517739710846e-7,
-            "angularVelocity": 2.129649524174011e-12,
-            "velocityX": 0.6181712523959566,
-            "velocityY": -3.7320901132416737,
-            "timestamp": 0.7687107744615156
-        },
-        {
-            "x": 2.0072992100178335,
-            "y": 3.3560442616931043,
-            "heading": -8.465517105809759e-7,
-            "angularVelocity": 1.8966463157464128e-12,
-            "velocityX": 0.8912391789143821,
-            "velocityY": -3.6764554055067418,
-            "timestamp": 0.8021329820467988
-        },
-        {
-            "x": 2.046051283468891,
-            "y": 3.235695267642527,
-            "heading": -8.465516527951994e-7,
-            "angularVelocity": 1.7289634182989629e-12,
-            "velocityX": 1.1594707905613406,
-            "velocityY": -3.600869085126494,
-            "timestamp": 0.8355551896320821
-        },
-        {
-            "x": 2.0935579534966013,
-            "y": 3.1185256537724477,
-            "heading": -8.46551596321545e-7,
-            "angularVelocity": 1.6897044173558387e-12,
-            "velocityX": 1.421410297528723,
-            "velocityY": -3.5057413120014775,
-            "timestamp": 0.8689773972173653
-        },
-        {
-            "x": 2.1495614051818848,
-            "y": 3.005171298980713,
-            "heading": -8.465515057383081e-7,
-            "angularVelocity": 2.7102707524537993e-12,
-            "velocityX": 1.6756359238847198,
-            "velocityY": -3.3915878986274275,
-            "timestamp": 0.9023996048026486
-        },
-        {
-            "x": 2.288743031901554,
-            "y": 2.8026748842621148,
-            "heading": -8.46551464739476e-7,
-            "angularVelocity": 6.312009526868921e-13,
-            "velocityX": 2.142782210187666,
-            "velocityY": -3.117550249366458,
-            "timestamp": 0.9673533019435205
-        },
-        {
-            "x": 2.455414708004788,
-            "y": 2.6221288200825,
-            "heading": -8.46551435217149e-7,
-            "angularVelocity": 4.5451345461171864e-13,
-            "velocityX": 2.566007532127087,
-            "velocityY": -2.779611817758129,
-            "timestamp": 1.0323069990843925
-        },
-        {
-            "x": 2.646160177401748,
-            "y": 2.4672337011827246,
-            "heading": -8.465514075797998e-7,
-            "angularVelocity": 4.254931139136813e-13,
-            "velocityX": 2.9366376017527633,
-            "velocityY": -2.384700574689416,
-            "timestamp": 1.0972606962252645
-        },
-        {
-            "x": 2.857069726290126,
-            "y": 2.341164292711998,
-            "heading": -8.465513787578765e-7,
-            "angularVelocity": 4.4373032078954734e-13,
-            "velocityX": 3.2470753501649696,
-            "velocityY": -1.9409119729950552,
-            "timestamp": 1.1622143933661364
-        },
-        {
-            "x": 3.0838173053505575,
-            "y": 2.2464971410417167,
-            "heading": -8.465513471236816e-7,
-            "angularVelocity": 4.870268741320482e-13,
-            "velocityX": 3.490911049579749,
-            "velocityY": -1.457455939200772,
-            "timestamp": 1.2271680905070084
-        },
-        {
-            "x": 3.3145112489857773,
-            "y": 2.1619002222864983,
-            "heading": -8.465513138187862e-7,
-            "angularVelocity": 5.127482864627404e-13,
-            "velocityX": 3.5516676307835056,
-            "velocityY": -1.3024188380298123,
-            "timestamp": 1.2921217876478803
-        },
-        {
-            "x": 3.5452013267301767,
-            "y": 2.0772927620920076,
-            "heading": -8.465512839726007e-7,
-            "angularVelocity": 4.594994306997602e-13,
-            "velocityX": 3.5516081131487938,
-            "velocityY": -1.302581129610464,
-            "timestamp": 1.3570754847887523
-        },
-        {
-            "x": 3.77589133724735,
-            "y": 1.9926851185956267,
-            "heading": -8.465512625373994e-7,
-            "angularVelocity": 3.3000743037279784e-13,
-            "velocityX": 3.5516070781466733,
-            "velocityY": -1.302583951649345,
-            "timestamp": 1.4220291819296242
-        },
-        {
-            "x": 4.006581361028944,
-            "y": 1.9080775112659911,
-            "heading": -8.465512535495126e-7,
-            "angularVelocity": 1.3837377280671698e-13,
-            "velocityX": 3.5516072823601217,
-            "velocityY": -1.302583394841325,
-            "timestamp": 1.4869828790704962
-        },
-        {
-            "x": 4.2372714040114055,
-            "y": 1.823469956289169,
-            "heading": -8.465512575529088e-7,
-            "angularVelocity": -6.163459506326445e-14,
-            "velocityX": 3.551607577968653,
-            "velocityY": -1.3025825888392473,
-            "timestamp": 1.5519365762113682
-        },
-        {
-            "x": 4.467961461705261,
-            "y": 1.7388624414245573,
-            "heading": -8.465512722492451e-7,
-            "angularVelocity": -2.2625863777166434e-13,
-            "velocityX": 3.5516078044591044,
-            "velocityY": -1.3025819712885556,
-            "timestamp": 1.6168902733522401
-        },
-        {
-            "x": 4.698651553926265,
-            "y": 1.6542550207002664,
-            "heading": -8.465512926518018e-7,
-            "angularVelocity": -3.141092229421582e-13,
-            "velocityX": 3.551608336024854,
-            "velocityY": -1.3025805219428788,
-            "timestamp": 1.681843970493112
-        },
-        {
-            "x": 4.929586887359619,
-            "y": 1.5703192949295044,
-            "heading": -8.465513064459796e-7,
-            "angularVelocity": -2.1236938120360531e-13,
-            "velocityX": 3.555383967330626,
-            "velocityY": -1.2922393869021438,
-            "timestamp": 1.746797667633984
-        },
-        {
-            "x": 5.059495672066219,
-            "y": 1.5327262216225606,
-            "heading": -8.465512750899748e-7,
-            "angularVelocity": 8.770995258558909e-13,
-            "velocityX": 3.633847296905686,
-            "velocityY": -1.051564666141291,
-            "timestamp": 1.7825473219513848
-        },
-        {
-            "x": 5.189548300230332,
-            "y": 1.4956338313822377,
-            "heading": -8.465512186056332e-7,
-            "angularVelocity": 1.579996862146135e-12,
-            "velocityX": 3.637870929029155,
-            "velocityY": -1.0375594099736098,
-            "timestamp": 1.8182969762687855
-        },
-        {
-            "x": 5.319623170079142,
-            "y": 1.4586195133360007,
-            "heading": -8.46551136487956e-7,
-            "angularVelocity": 2.2970201754243682e-12,
-            "velocityX": 3.638493080071459,
-            "velocityY": -1.0353755512600933,
-            "timestamp": 1.8540466305861862
-        },
-        {
-            "x": 5.4496989055100356,
-            "y": 1.42160823723968,
-            "heading": -8.465510387564672e-7,
-            "angularVelocity": 2.7337743725450157e-12,
-            "velocityX": 3.6385172923918394,
-            "velocityY": -1.0352904609291835,
-            "timestamp": 1.889796284903587
-        },
-        {
-            "x": 5.57977446957584,
-            "y": 1.3845963588884114,
-            "heading": -8.465509335637302e-7,
-            "angularVelocity": 2.9424826347181554e-12,
-            "velocityX": 3.6385124989164392,
-            "velocityY": -1.03530730738433,
-            "timestamp": 1.9255459392209877
-        },
-        {
-            "x": 5.709850057975644,
-            "y": 1.3475845660573411,
-            "heading": -8.465508277220688e-7,
-            "angularVelocity": 2.960634540295608e-12,
-            "velocityX": 3.6385131795943324,
-            "velocityY": -1.0353049151878095,
-            "timestamp": 1.9612955935383884
-        },
-        {
-            "x": 5.839925662966751,
-            "y": 1.3105728315354166,
-            "heading": -8.465507264753305e-7,
-            "angularVelocity": 2.832103981671134e-12,
-            "velocityX": 3.6385136436912036,
-            "velocityY": -1.0353032841469862,
-            "timestamp": 1.9970452478557892
-        },
-        {
-            "x": 5.970001275498046,
-            "y": 1.273561123513075,
-            "heading": -8.465506337302748e-7,
-            "angularVelocity": 2.594292378130792e-12,
-            "velocityX": 3.638513854607602,
-            "velocityY": -1.0353025428927405,
-            "timestamp": 2.03279490217319
-        },
-        {
-            "x": 6.100076894564937,
-            "y": 1.2365494384597497,
-            "heading": -8.46550552278632e-7,
-            "angularVelocity": 2.2783896618382965e-12,
-            "velocityX": 3.6385140374232323,
-            "velocityY": -1.0353019003965735,
-            "timestamp": 2.0685445564905907
-        },
-        {
-            "x": 6.230152520105601,
-            "y": 1.1995377761581834,
-            "heading": -8.465504825990077e-7,
-            "angularVelocity": 1.9490992420524453e-12,
-            "velocityX": 3.6385142185095396,
-            "velocityY": -1.03530126397757,
-            "timestamp": 2.1042942108079914
-        },
-        {
-            "x": 6.3602281516940815,
-            "y": 1.16252613511139,
-            "heading": -8.465504225140753e-7,
-            "angularVelocity": 1.680713660601537e-12,
-            "velocityX": 3.6385143876808734,
-            "velocityY": -1.0353006694327234,
-            "timestamp": 2.140043865125392
-        },
-        {
-            "x": 6.490303788775648,
-            "y": 1.1255145133698101,
-            "heading": -8.465503700121188e-7,
-            "angularVelocity": 1.4686003981542915e-12,
-            "velocityX": 3.638514541335137,
-            "velocityY": -1.0353001294215285,
-            "timestamp": 2.175793519442793
-        },
-        {
-            "x": 6.62037943067546,
-            "y": 1.0885029085617548,
-            "heading": -8.465503232940394e-7,
-            "angularVelocity": 1.306812060469801e-12,
-            "velocityX": 3.638514676112538,
-            "velocityY": -1.0352996557519205,
-            "timestamp": 2.2115431737601936
-        },
-        {
-            "x": 6.7504550766965234,
-            "y": 1.0514913182376744,
-            "heading": -8.465502808582129e-7,
-            "angularVelocity": 1.187027603302574e-12,
-            "velocityX": 3.638514791393393,
-            "velocityY": -1.0352992506018621,
-            "timestamp": 2.2472928280775943
-        },
-        {
-            "x": 6.880530727542945,
-            "y": 1.0144797448710672,
-            "heading": -8.465502433332782e-7,
-            "angularVelocity": 1.0496586699357012e-12,
-            "velocityX": 3.6385149263697616,
-            "velocityY": -1.0352987762623522,
-            "timestamp": 2.283042482394995
-        },
-        {
-            "x": 7.003525953233288,
-            "y": 0.9794828656529073,
-            "heading": -7.145353891597533e-7,
-            "angularVelocity": 0.000003692758900357643,
-            "velocityX": 3.440459160761053,
-            "velocityY": -0.9789431502593731,
-            "timestamp": 2.318792136712396
-        },
-        {
-            "x": 7.116271603762692,
-            "y": 0.9474023877264399,
-            "heading": -5.944969754078434e-7,
-            "angularVelocity": 0.0000033577503345576707,
-            "velocityX": 3.153754985388111,
-            "velocityY": -0.8973647029323183,
-            "timestamp": 2.3545417910297965
-        },
-        {
-            "x": 7.218767661219051,
-            "y": 0.9182383156679585,
-            "heading": -4.859087151281335e-7,
-            "angularVelocity": 0.0000030374632243726353,
-            "velocityX": 2.867050308972391,
-            "velocityY": -0.8157861275958178,
-            "timestamp": 2.3902914453471973
-        },
-        {
-            "x": 7.311014119553375,
-            "y": 0.8919906507276494,
-            "heading": -3.8850367492824467e-7,
-            "angularVelocity": 0.000002724642854703664,
-            "velocityX": 2.580345463352478,
-            "velocityY": -0.7342075172887301,
-            "timestamp": 2.426041099664598
-        },
-        {
-            "x": 7.393010975686544,
-            "y": 0.8686593933386082,
-            "heading": -3.0210953963079075e-7,
-            "angularVelocity": 0.0000024166425366030095,
-            "velocityX": 2.293640531602499,
-            "velocityY": -0.6526288948669626,
-            "timestamp": 2.4617907539819988
-        },
-        {
-            "x": 7.464758227730078,
-            "y": 0.8482445436165693,
-            "heading": -2.2661033918382436e-7,
-            "angularVelocity": 0.000002111886167406066,
-            "velocityX": 2.006935547027439,
-            "velocityY": -0.5710502692078399,
-            "timestamp": 2.4975404082993995
-        },
-        {
-            "x": 7.526255874392654,
-            "y": 0.8307461015250325,
-            "heading": -1.6193015142138781e-7,
-            "angularVelocity": 0.0000018092535156938384,
-            "velocityX": 1.7202305263310733,
-            "velocityY": -0.48947164456971237,
-            "timestamp": 2.5332900626168002
-        },
-        {
-            "x": 7.577503914725456,
-            "y": 0.8161640669449958,
-            "heading": -1.0802379214331377e-7,
-            "angularVelocity": 0.0000015078847699136293,
-            "velocityX": 1.4335254790941394,
-            "velocityY": -0.4078930232603403,
-            "timestamp": 2.569039716934201
-        },
-        {
-            "x": 7.618502347994633,
-            "y": 0.8044984397090525,
-            "heading": -6.487055743300418e-8,
-            "angularVelocity": 0.0000012070951602673883,
-            "velocityX": 1.1468204113297347,
-            "velocityY": -0.3263144066337203,
-            "timestamp": 2.6047893712516017
-        },
-        {
-            "x": 7.649251173610278,
-            "y": 0.7957492196197445,
-            "heading": -3.246962719134405e-8,
-            "angularVelocity": 9.063284906782187e-7,
-            "velocityX": 0.8601153270642448,
-            "velocityY": -0.24473579553046104,
-            "timestamp": 2.6405390255690024
-        },
-        {
-            "x": 7.669750391083677,
-            "y": 0.7899164064601075,
-            "heading": -1.0836483282929717e-8,
-            "angularVelocity": 6.051287577519143e-7,
-            "velocityX": 0.573410229128317,
-            "velocityY": -0.16315719049619,
-            "timestamp": 2.676288679886403
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 2.0494644799121527e-30,
-            "angularVelocity": 3.031213445055282e-7,
-            "velocityX": 0.28670511958863415,
-            "velocityY": -0.08157859189950141,
-            "timestamp": 2.712038334203804
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -1.7051999701665118e-29,
-            "angularVelocity": -2.5846596071248955e-28,
-            "velocityX": 4.524644517512267e-29,
-            "velocityY": 1.710870028377411e-29,
-            "timestamp": 2.7477879885212046
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.1.traj
deleted file mode 100644
index bd47bcab..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -9.427656700789375e-18,
-            "angularVelocity": -1.3516738833302546e-18,
-            "velocityX": 8.730641062152612e-18,
-            "velocityY": -7.295689550902649e-17,
-            "timestamp": 0
-        },
-        {
-            "x": 2.220337966696873,
-            "y": 5.62696613740055,
-            "heading": -0.005158227148275901,
-            "angularVelocity": -0.05503188081512997,
-            "velocityX": -0.31645707615215996,
-            "velocityY": 0.7144455616516493,
-            "timestamp": 0.09373161650796653
-        },
-        {
-            "x": 2.1631184668461043,
-            "y": 5.761793353879717,
-            "heading": -0.015168353452777405,
-            "angularVelocity": -0.10679562219702789,
-            "velocityX": -0.6104610374014564,
-            "velocityY": 1.4384390401258744,
-            "timestamp": 0.18746323301593307
-        },
-        {
-            "x": 2.0853065565036237,
-            "y": 5.966914141221312,
-            "heading": -0.029010799448120003,
-            "angularVelocity": -0.14768171627730414,
-            "velocityX": -0.8301564961900216,
-            "velocityY": 2.1883841865051155,
-            "timestamp": 0.2811948495238996
-        },
-        {
-            "x": 2.0799249337959744,
-            "y": 6.161512978496455,
-            "heading": -0.03183861533006777,
-            "angularVelocity": -0.0301692853201504,
-            "velocityX": -0.05741523413491959,
-            "velocityY": 2.076128040089899,
-            "timestamp": 0.37492646603186613
-        },
-        {
-            "x": 2.1174615185646393,
-            "y": 6.2967780571782885,
-            "heading": -0.027602730416951004,
-            "angularVelocity": 0.045191634060389424,
-            "velocityX": 0.4004687656856407,
-            "velocityY": 1.443110486314265,
-            "timestamp": 0.46865808253983265
-        },
-        {
-            "x": 2.193636869673838,
-            "y": 6.369832500172102,
-            "heading": -0.016913451231696384,
-            "angularVelocity": 0.11404134040882687,
-            "velocityX": 0.8126964406159006,
-            "velocityY": 0.7794002249774918,
-            "timestamp": 0.5623896990477992
-        },
-        {
-            "x": 2.306850910186767,
-            "y": 6.379708766937256,
-            "heading": 7.831941019246608e-17,
-            "angularVelocity": 0.1804455301403977,
-            "velocityX": 1.20785328079034,
-            "velocityY": 0.10536750707073723,
-            "timestamp": 0.6561213155557658
-        },
-        {
-            "x": 2.454400050444712,
-            "y": 6.392580268241816,
-            "heading": 0.024770297736306776,
-            "angularVelocity": 0.3129674474840377,
-            "velocityX": 1.864252028642711,
-            "velocityY": 0.1626286834118752,
-            "timestamp": 0.7352678783546651
-        },
-        {
-            "x": 2.653900880802349,
-            "y": 6.4099837935603645,
-            "heading": 0.06003005706528144,
-            "angularVelocity": 0.445499565389401,
-            "velocityX": 2.520650591795653,
-            "velocityY": 0.2198898436406934,
-            "timestamp": 0.8144144411535644
-        },
-        {
-            "x": 2.905353386807521,
-            "y": 6.431919341632162,
-            "heading": 0.10577908294347244,
-            "angularVelocity": 0.5780292189622053,
-            "velocityX": 3.177048972348669,
-            "velocityY": 0.2771509879403426,
-            "timestamp": 0.8935610039524637
-        },
-        {
-            "x": 3.203627341926949,
-            "y": 6.457939375335675,
-            "heading": 0.10577908974967822,
-            "angularVelocity": 8.599496273628574e-8,
-            "velocityX": 3.768628030977189,
-            "velocityY": 0.3287575958241692,
-            "timestamp": 0.972707566751363
-        },
-        {
-            "x": 3.501901297052475,
-            "y": 6.48395940903972,
-            "heading": 0.10577909654992135,
-            "angularVelocity": 8.591962579346019e-8,
-            "velocityX": 3.7686280310542424,
-            "velocityY": 0.328757595830891,
-            "timestamp": 1.0518541295502624
-        },
-        {
-            "x": 3.8001752521780374,
-            "y": 6.509979442743767,
-            "heading": 0.10577910335027264,
-            "angularVelocity": 8.592099253060419e-8,
-            "velocityX": 3.768628031054697,
-            "velocityY": 0.32875759583093067,
-            "timestamp": 1.1310006923491618
-        },
-        {
-            "x": 4.0984492073035454,
-            "y": 6.53599947644781,
-            "heading": 0.1057791101506597,
-            "angularVelocity": 8.592144417290625e-8,
-            "velocityX": 3.7686280310540092,
-            "velocityY": 0.32875759583087066,
-            "timestamp": 1.2101472551480612
-        },
-        {
-            "x": 4.396723162417087,
-            "y": 6.562019510150809,
-            "heading": 0.10577911696012733,
-            "angularVelocity": 8.603617553663074e-8,
-            "velocityX": 3.7686280309028084,
-            "velocityY": 0.3287575958176806,
-            "timestamp": 1.2892938179469606
-        },
-        {
-            "x": 4.656482038612004,
-            "y": 6.5846796673565295,
-            "heading": 0.15819030641666595,
-            "angularVelocity": 0.6622042398696235,
-            "velocityX": 3.2819981943490006,
-            "velocityY": 0.28630627034678896,
-            "timestamp": 1.36844038074586
-        },
-        {
-            "x": 4.864289197548449,
-            "y": 6.6028077981790565,
-            "heading": 0.2001158851669991,
-            "angularVelocity": 0.5297207770962873,
-            "velocityX": 2.6255992880506356,
-            "velocityY": 0.22904508018355874,
-            "timestamp": 1.4475869435447595
-        },
-        {
-            "x": 5.020144593431014,
-            "y": 6.616403898623411,
-            "heading": 0.2315582663704034,
-            "angularVelocity": 0.39726780407754475,
-            "velocityX": 1.9691998031370395,
-            "velocityY": 0.17178383954462473,
-            "timestamp": 1.526733506343659
-        },
-        {
-            "x": 5.124048198578653,
-            "y": 6.625467966274828,
-            "heading": 0.2525193347871735,
-            "angularVelocity": 0.2648386446045577,
-            "velocityX": 1.3127999684793037,
-            "velocityY": 0.1145225683956373,
-            "timestamp": 1.6058800691425583
-        },
-        {
-            "x": 5.176000000000001,
-            "y": 6.63,
-            "heading": 0.26299999999999985,
-            "angularVelocity": 0.13242097751555232,
-            "velocityX": 0.6563999696784293,
-            "velocityY": 0.05726128292756526,
-            "timestamp": 1.6850266319414577
-        },
-        {
-            "x": 5.176000000000001,
-            "y": 6.63,
-            "heading": 0.26299999999999996,
-            "angularVelocity": 1.6428599245730974e-17,
-            "velocityX": 1.9833104251084488e-16,
-            "velocityY": 1.8748049703629392e-17,
-            "timestamp": 1.7641731947403572
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.traj
deleted file mode 100644
index bd47bcab..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -9.427656700789375e-18,
-            "angularVelocity": -1.3516738833302546e-18,
-            "velocityX": 8.730641062152612e-18,
-            "velocityY": -7.295689550902649e-17,
-            "timestamp": 0
-        },
-        {
-            "x": 2.220337966696873,
-            "y": 5.62696613740055,
-            "heading": -0.005158227148275901,
-            "angularVelocity": -0.05503188081512997,
-            "velocityX": -0.31645707615215996,
-            "velocityY": 0.7144455616516493,
-            "timestamp": 0.09373161650796653
-        },
-        {
-            "x": 2.1631184668461043,
-            "y": 5.761793353879717,
-            "heading": -0.015168353452777405,
-            "angularVelocity": -0.10679562219702789,
-            "velocityX": -0.6104610374014564,
-            "velocityY": 1.4384390401258744,
-            "timestamp": 0.18746323301593307
-        },
-        {
-            "x": 2.0853065565036237,
-            "y": 5.966914141221312,
-            "heading": -0.029010799448120003,
-            "angularVelocity": -0.14768171627730414,
-            "velocityX": -0.8301564961900216,
-            "velocityY": 2.1883841865051155,
-            "timestamp": 0.2811948495238996
-        },
-        {
-            "x": 2.0799249337959744,
-            "y": 6.161512978496455,
-            "heading": -0.03183861533006777,
-            "angularVelocity": -0.0301692853201504,
-            "velocityX": -0.05741523413491959,
-            "velocityY": 2.076128040089899,
-            "timestamp": 0.37492646603186613
-        },
-        {
-            "x": 2.1174615185646393,
-            "y": 6.2967780571782885,
-            "heading": -0.027602730416951004,
-            "angularVelocity": 0.045191634060389424,
-            "velocityX": 0.4004687656856407,
-            "velocityY": 1.443110486314265,
-            "timestamp": 0.46865808253983265
-        },
-        {
-            "x": 2.193636869673838,
-            "y": 6.369832500172102,
-            "heading": -0.016913451231696384,
-            "angularVelocity": 0.11404134040882687,
-            "velocityX": 0.8126964406159006,
-            "velocityY": 0.7794002249774918,
-            "timestamp": 0.5623896990477992
-        },
-        {
-            "x": 2.306850910186767,
-            "y": 6.379708766937256,
-            "heading": 7.831941019246608e-17,
-            "angularVelocity": 0.1804455301403977,
-            "velocityX": 1.20785328079034,
-            "velocityY": 0.10536750707073723,
-            "timestamp": 0.6561213155557658
-        },
-        {
-            "x": 2.454400050444712,
-            "y": 6.392580268241816,
-            "heading": 0.024770297736306776,
-            "angularVelocity": 0.3129674474840377,
-            "velocityX": 1.864252028642711,
-            "velocityY": 0.1626286834118752,
-            "timestamp": 0.7352678783546651
-        },
-        {
-            "x": 2.653900880802349,
-            "y": 6.4099837935603645,
-            "heading": 0.06003005706528144,
-            "angularVelocity": 0.445499565389401,
-            "velocityX": 2.520650591795653,
-            "velocityY": 0.2198898436406934,
-            "timestamp": 0.8144144411535644
-        },
-        {
-            "x": 2.905353386807521,
-            "y": 6.431919341632162,
-            "heading": 0.10577908294347244,
-            "angularVelocity": 0.5780292189622053,
-            "velocityX": 3.177048972348669,
-            "velocityY": 0.2771509879403426,
-            "timestamp": 0.8935610039524637
-        },
-        {
-            "x": 3.203627341926949,
-            "y": 6.457939375335675,
-            "heading": 0.10577908974967822,
-            "angularVelocity": 8.599496273628574e-8,
-            "velocityX": 3.768628030977189,
-            "velocityY": 0.3287575958241692,
-            "timestamp": 0.972707566751363
-        },
-        {
-            "x": 3.501901297052475,
-            "y": 6.48395940903972,
-            "heading": 0.10577909654992135,
-            "angularVelocity": 8.591962579346019e-8,
-            "velocityX": 3.7686280310542424,
-            "velocityY": 0.328757595830891,
-            "timestamp": 1.0518541295502624
-        },
-        {
-            "x": 3.8001752521780374,
-            "y": 6.509979442743767,
-            "heading": 0.10577910335027264,
-            "angularVelocity": 8.592099253060419e-8,
-            "velocityX": 3.768628031054697,
-            "velocityY": 0.32875759583093067,
-            "timestamp": 1.1310006923491618
-        },
-        {
-            "x": 4.0984492073035454,
-            "y": 6.53599947644781,
-            "heading": 0.1057791101506597,
-            "angularVelocity": 8.592144417290625e-8,
-            "velocityX": 3.7686280310540092,
-            "velocityY": 0.32875759583087066,
-            "timestamp": 1.2101472551480612
-        },
-        {
-            "x": 4.396723162417087,
-            "y": 6.562019510150809,
-            "heading": 0.10577911696012733,
-            "angularVelocity": 8.603617553663074e-8,
-            "velocityX": 3.7686280309028084,
-            "velocityY": 0.3287575958176806,
-            "timestamp": 1.2892938179469606
-        },
-        {
-            "x": 4.656482038612004,
-            "y": 6.5846796673565295,
-            "heading": 0.15819030641666595,
-            "angularVelocity": 0.6622042398696235,
-            "velocityX": 3.2819981943490006,
-            "velocityY": 0.28630627034678896,
-            "timestamp": 1.36844038074586
-        },
-        {
-            "x": 4.864289197548449,
-            "y": 6.6028077981790565,
-            "heading": 0.2001158851669991,
-            "angularVelocity": 0.5297207770962873,
-            "velocityX": 2.6255992880506356,
-            "velocityY": 0.22904508018355874,
-            "timestamp": 1.4475869435447595
-        },
-        {
-            "x": 5.020144593431014,
-            "y": 6.616403898623411,
-            "heading": 0.2315582663704034,
-            "angularVelocity": 0.39726780407754475,
-            "velocityX": 1.9691998031370395,
-            "velocityY": 0.17178383954462473,
-            "timestamp": 1.526733506343659
-        },
-        {
-            "x": 5.124048198578653,
-            "y": 6.625467966274828,
-            "heading": 0.2525193347871735,
-            "angularVelocity": 0.2648386446045577,
-            "velocityX": 1.3127999684793037,
-            "velocityY": 0.1145225683956373,
-            "timestamp": 1.6058800691425583
-        },
-        {
-            "x": 5.176000000000001,
-            "y": 6.63,
-            "heading": 0.26299999999999985,
-            "angularVelocity": 0.13242097751555232,
-            "velocityX": 0.6563999696784293,
-            "velocityY": 0.05726128292756526,
-            "timestamp": 1.6850266319414577
-        },
-        {
-            "x": 5.176000000000001,
-            "y": 6.63,
-            "heading": 0.26299999999999996,
-            "angularVelocity": 1.6428599245730974e-17,
-            "velocityX": 1.9833104251084488e-16,
-            "velocityY": 1.8748049703629392e-17,
-            "timestamp": 1.7641731947403572
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.1.traj
deleted file mode 100644
index 96b0e72c..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.1.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3319036557469652,
-            "y": 5.56,
-            "heading": -1.815106931159875e-19,
-            "angularVelocity": -1.831407645203353e-18,
-            "velocityX": 0.8263919813283321,
-            "velocityY": 8.319303153097966e-34,
-            "timestamp": 0.099109935233537
-        },
-        {
-            "x": 2.4957109641637625,
-            "y": 5.56,
-            "heading": -5.438547016393454e-19,
-            "angularVelocity": -3.655980680963076e-18,
-            "velocityX": 1.6527839316089838,
-            "velocityY": 1.3987786573815764e-33,
-            "timestamp": 0.198219870467074
-        },
-        {
-            "x": 2.741421919096125,
-            "y": 5.56,
-            "heading": -1.0860329369861945e-18,
-            "angularVelocity": -5.4704731072674304e-18,
-            "velocityX": 2.4791758197942864,
-            "velocityY": 2.272018181833636e-33,
-            "timestamp": 0.297329805700611
-        },
-        {
-            "x": 3.0690365020812695,
-            "y": 5.56,
-            "heading": -1.80338172905721e-18,
-            "angularVelocity": -7.237910009033217e-18,
-            "velocityX": 3.3055675216936846,
-            "velocityY": 3.0574601440031788e-33,
-            "timestamp": 0.396439740934148
-        },
-        {
-            "x": 3.4439634979187304,
-            "y": 5.56,
-            "heading": 1.7928981709293245e-18,
-            "angularVelocity": 3.6285765816640505e-17,
-            "velocityX": 3.7829405796099502,
-            "velocityY": 2.997011014948752e-33,
-            "timestamp": 0.495549676167685
-        },
-        {
-            "x": 3.771578080903875,
-            "y": 5.56,
-            "heading": 1.0808328664707184e-18,
-            "angularVelocity": -7.184600643588333e-18,
-            "velocityX": 3.305567521693685,
-            "velocityY": -4.3766441461200414e-33,
-            "timestamp": 0.594659611401222
-        },
-        {
-            "x": 4.017289035836238,
-            "y": 5.56,
-            "heading": 5.416591039281022e-19,
-            "angularVelocity": -5.4401585595028644e-18,
-            "velocityX": 2.479175819794286,
-            "velocityY": -5.119323199602909e-33,
-            "timestamp": 0.6937695466347591
-        },
-        {
-            "x": 4.181096344253034,
-            "y": 5.56,
-            "heading": 1.8086099379948042e-19,
-            "angularVelocity": -3.64038286599245e-18,
-            "velocityX": 1.6527839316089838,
-            "velocityY": -5.9637817525889115e-33,
-            "timestamp": 0.792879481868296
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -1.8248523050897816e-18,
-            "velocityX": 0.8263919813283321,
-            "velocityY": -3.8230331267939286e-33,
-            "timestamp": 0.891989417101833
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.0684896902092895e-40,
-            "timestamp": 0.9910993523353699
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.traj
deleted file mode 100644
index 96b0e72c..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3319036557469652,
-            "y": 5.56,
-            "heading": -1.815106931159875e-19,
-            "angularVelocity": -1.831407645203353e-18,
-            "velocityX": 0.8263919813283321,
-            "velocityY": 8.319303153097966e-34,
-            "timestamp": 0.099109935233537
-        },
-        {
-            "x": 2.4957109641637625,
-            "y": 5.56,
-            "heading": -5.438547016393454e-19,
-            "angularVelocity": -3.655980680963076e-18,
-            "velocityX": 1.6527839316089838,
-            "velocityY": 1.3987786573815764e-33,
-            "timestamp": 0.198219870467074
-        },
-        {
-            "x": 2.741421919096125,
-            "y": 5.56,
-            "heading": -1.0860329369861945e-18,
-            "angularVelocity": -5.4704731072674304e-18,
-            "velocityX": 2.4791758197942864,
-            "velocityY": 2.272018181833636e-33,
-            "timestamp": 0.297329805700611
-        },
-        {
-            "x": 3.0690365020812695,
-            "y": 5.56,
-            "heading": -1.80338172905721e-18,
-            "angularVelocity": -7.237910009033217e-18,
-            "velocityX": 3.3055675216936846,
-            "velocityY": 3.0574601440031788e-33,
-            "timestamp": 0.396439740934148
-        },
-        {
-            "x": 3.4439634979187304,
-            "y": 5.56,
-            "heading": 1.7928981709293245e-18,
-            "angularVelocity": 3.6285765816640505e-17,
-            "velocityX": 3.7829405796099502,
-            "velocityY": 2.997011014948752e-33,
-            "timestamp": 0.495549676167685
-        },
-        {
-            "x": 3.771578080903875,
-            "y": 5.56,
-            "heading": 1.0808328664707184e-18,
-            "angularVelocity": -7.184600643588333e-18,
-            "velocityX": 3.305567521693685,
-            "velocityY": -4.3766441461200414e-33,
-            "timestamp": 0.594659611401222
-        },
-        {
-            "x": 4.017289035836238,
-            "y": 5.56,
-            "heading": 5.416591039281022e-19,
-            "angularVelocity": -5.4401585595028644e-18,
-            "velocityX": 2.479175819794286,
-            "velocityY": -5.119323199602909e-33,
-            "timestamp": 0.6937695466347591
-        },
-        {
-            "x": 4.181096344253034,
-            "y": 5.56,
-            "heading": 1.8086099379948042e-19,
-            "angularVelocity": -3.64038286599245e-18,
-            "velocityX": 1.6527839316089838,
-            "velocityY": -5.9637817525889115e-33,
-            "timestamp": 0.792879481868296
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -1.8248523050897816e-18,
-            "velocityX": 0.8263919813283321,
-            "velocityY": -3.8230331267939286e-33,
-            "timestamp": 0.891989417101833
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.0684896902092895e-40,
-            "timestamp": 0.9910993523353699
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.1.traj
deleted file mode 100644
index 7d72e43b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.1.traj	
+++ /dev/null
@@ -1,526 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -4.99624931676796e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2447003077480563,
-            "y": 5.548424158968356,
-            "heading": -0.001651505925702968,
-            "angularVelocity": -0.042250731032121805,
-            "velocityX": -0.13558284496887443,
-            "velocityY": -0.2961465280181771,
-            "timestamp": 0.03908822132445813
-        },
-        {
-            "x": 2.234100924566356,
-            "y": 5.525272476708423,
-            "heading": -0.0049545293965937804,
-            "angularVelocity": -0.08450175932731885,
-            "velocityX": -0.27116565611205723,
-            "velocityY": -0.5922930610671583,
-            "timestamp": 0.07817644264891627
-        },
-        {
-            "x": 2.2182018518980375,
-            "y": 5.490544953203296,
-            "heading": -0.009909100000268336,
-            "angularVelocity": -0.12675354456654908,
-            "velocityX": -0.4067484303351874,
-            "velocityY": -0.8884395945486457,
-            "timestamp": 0.1172646639733744
-        },
-        {
-            "x": 2.1970030913105703,
-            "y": 5.444241588640874,
-            "heading": -0.016515264878529855,
-            "angularVelocity": -0.16900653584180803,
-            "velocityX": -0.5423311644575062,
-            "velocityY": -1.1845861232230646,
-            "timestamp": 0.15635288529783253
-        },
-        {
-            "x": 2.170504644501127,
-            "y": 5.386362383452732,
-            "heading": -0.024773088013563174,
-            "angularVelocity": -0.21126116398317848,
-            "velocityX": -0.6779138551608025,
-            "velocityY": -1.480732640856411,
-            "timestamp": 0.19544110662229067
-        },
-        {
-            "x": 2.1387065133083243,
-            "y": 5.31690733837683,
-            "heading": -0.03468264893636917,
-            "angularVelocity": -0.2535178267784212,
-            "velocityX": -0.8134964988264785,
-            "velocityY": -1.7768791396104282,
-            "timestamp": 0.2345293279467488
-        },
-        {
-            "x": 2.1016086997403236,
-            "y": 5.235876454566401,
-            "heading": -0.04624404029676127,
-            "angularVelocity": -0.2957768598481422,
-            "velocityX": -0.9490790911170489,
-            "velocityY": -2.0730256088611454,
-            "timestamp": 0.27361754927120696
-        },
-        {
-            "x": 2.0592112060450187,
-            "y": 5.143269733801358,
-            "heading": -0.05945736298631538,
-            "angularVelocity": -0.33803847404262444,
-            "velocityX": -1.0846616258994186,
-            "velocityY": -2.369172032575987,
-            "timestamp": 0.31270577059566507
-        },
-        {
-            "x": 2.011514034901109,
-            "y": 5.039087178972333,
-            "heading": -0.07432271508161363,
-            "angularVelocity": -0.3803025973454465,
-            "velocityX": -1.2202440921521842,
-            "velocityY": -2.6653183823394446,
-            "timestamp": 0.3517939919201232
-        },
-        {
-            "x": 1.9585171900495353,
-            "y": 4.923328795522167,
-            "heading": -0.09084016031123929,
-            "angularVelocity": -0.4225683510264515,
-            "velocityX": -1.3558264626999135,
-            "velocityY": -2.9614645928578263,
-            "timestamp": 0.3908822132445813
-        },
-        {
-            "x": 1.9002206796290688,
-            "y": 4.795994598697166,
-            "heading": -0.10900958241827326,
-            "angularVelocity": -0.46483113048874997,
-            "velocityX": -1.491408625032185,
-            "velocityY": -3.257610413327559,
-            "timestamp": 0.4299704345690394
-        },
-        {
-            "x": 1.8386671955063139,
-            "y": 4.661546724325594,
-            "heading": -0.10900958703871873,
-            "angularVelocity": -1.182055697467884e-7,
-            "velocityX": -1.5747322860207698,
-            "velocityY": -3.439600724104782,
-            "timestamp": 0.4690586558934975
-        },
-        {
-            "x": 1.7777943297173011,
-            "y": 4.526789325478927,
-            "heading": -0.10900959166091434,
-            "angularVelocity": -1.1825034389504455e-7,
-            "velocityX": -1.5573199221351706,
-            "velocityY": -3.447519336526256,
-            "timestamp": 0.5081468772179556
-        },
-        {
-            "x": 1.728739704549767,
-            "y": 4.38729485969642,
-            "heading": -0.10900959636164997,
-            "angularVelocity": -1.2025964513922604e-7,
-            "velocityX": -1.2549720479816096,
-            "velocityY": -3.5687084511883413,
-            "timestamp": 0.5472350985424137
-        },
-        {
-            "x": 1.6918738865194354,
-            "y": 4.244095790476986,
-            "heading": -0.10900960128999353,
-            "angularVelocity": -1.2608257379035844e-7,
-            "velocityX": -0.9431439134643449,
-            "velocityY": -3.6634838927766116,
-            "timestamp": 0.5863233198668718
-        },
-        {
-            "x": 1.6674707596733624,
-            "y": 4.098254956364247,
-            "heading": -0.10900960662650709,
-            "angularVelocity": -1.3652485001388814e-7,
-            "velocityX": -0.6243089610931135,
-            "velocityY": -3.7310685718382324,
-            "timestamp": 0.6254115411913299
-        },
-        {
-            "x": 1.6557115284805652,
-            "y": 3.9508548842430677,
-            "heading": -0.1090096126146074,
-            "angularVelocity": -1.531944945007552e-7,
-            "velocityX": -0.3008382268193907,
-            "velocityY": -3.7709587985000237,
-            "timestamp": 0.664499762515788
-        },
-        {
-            "x": 1.6566835066189385,
-            "y": 3.8029896922639588,
-            "heading": -0.10900961961577929,
-            "angularVelocity": -1.79112061302596e-7,
-            "velocityX": 0.024866266754091815,
-            "velocityY": -3.7828580316236344,
-            "timestamp": 0.7035879838402461
-        },
-        {
-            "x": 1.6703794917330834,
-            "y": 3.6557569809887474,
-            "heading": -0.10900969829200789,
-            "angularVelocity": -0.000002012786100988185,
-            "velocityX": 0.3503865013576149,
-            "velocityY": -3.766677180142832,
-            "timestamp": 0.7426762051647042
-        },
-        {
-            "x": 1.6964297342683634,
-            "y": 3.5114772969924033,
-            "heading": -0.1124967438020359,
-            "angularVelocity": -0.0892096235606636,
-            "velocityX": 0.6664473760284892,
-            "velocityY": -3.6911294274233937,
-            "timestamp": 0.7817644264891623
-        },
-        {
-            "x": 1.7337062034817727,
-            "y": 3.3729422790966246,
-            "heading": -0.12243166775651844,
-            "angularVelocity": -0.2541666931326176,
-            "velocityX": 0.9536496660724848,
-            "velocityY": -3.54416274779685,
-            "timestamp": 0.8208526478136204
-        },
-        {
-            "x": 1.7809992442628195,
-            "y": 3.2421845220219927,
-            "heading": -0.1367067464310455,
-            "angularVelocity": -0.3652015413039525,
-            "velocityX": 1.2099051626962352,
-            "velocityY": -3.3451958836719715,
-            "timestamp": 0.8599408691380785
-        },
-        {
-            "x": 1.8374227958909133,
-            "y": 3.120270451905998,
-            "heading": -0.154016140064332,
-            "angularVelocity": -0.4428288892862246,
-            "velocityX": 1.443492431128254,
-            "velocityY": -3.118946475052685,
-            "timestamp": 0.8990290904625367
-        },
-        {
-            "x": 1.9023445230612375,
-            "y": 3.007828092475748,
-            "heading": -0.17349166731783877,
-            "angularVelocity": -0.4982454200677405,
-            "velocityX": 1.660902567846671,
-            "velocityY": -2.8766302384778886,
-            "timestamp": 0.9381173117869948
-        },
-        {
-            "x": 1.9753011558084586,
-            "y": 2.90526251520286,
-            "heading": -0.19452421295735298,
-            "angularVelocity": -0.5380788617862533,
-            "velocityX": 1.866460797529241,
-            "velocityY": -2.623950995915806,
-            "timestamp": 0.9772055331114529
-        },
-        {
-            "x": 2.0559421744907826,
-            "y": 2.8128534355639654,
-            "heading": -0.21666661660279088,
-            "angularVelocity": -0.5664725304751266,
-            "velocityX": 2.0630516291077963,
-            "velocityY": -2.3641157491367464,
-            "timestamp": 1.016293754435911
-        },
-        {
-            "x": 2.1439944855858766,
-            "y": 2.7308042438703497,
-            "heading": -0.2395781801271676,
-            "angularVelocity": -0.586150066389446,
-            "velocityX": 2.252655866948419,
-            "velocityY": -2.099077136627731,
-            "timestamp": 1.055381975760369
-        },
-        {
-            "x": 2.2392399311065674,
-            "y": 2.659268856048584,
-            "heading": -0.2629913490343549,
-            "angularVelocity": -0.5989827143282399,
-            "velocityX": 2.4366789353272003,
-            "velocityY": -1.830100869209989,
-            "timestamp": 1.0944701970848272
-        },
-        {
-            "x": 2.3166964235934535,
-            "y": 2.610513248595422,
-            "heading": -0.2811797949710667,
-            "angularVelocity": -0.6046182408202432,
-            "velocityX": 2.5747998696792043,
-            "velocityY": -1.620728330008343,
-            "timestamp": 1.124552726371468
-        },
-        {
-            "x": 2.398213254515867,
-            "y": 2.5681176642029904,
-            "heading": -0.29946946527559826,
-            "angularVelocity": -0.6079831296849904,
-            "velocityX": 2.7097731758410535,
-            "velocityY": -1.409309170398157,
-            "timestamp": 1.154635255658109
-        },
-        {
-            "x": 2.483678189488588,
-            "y": 2.5321524360945276,
-            "heading": -0.31778023201379,
-            "angularVelocity": -0.6086844149213316,
-            "velocityX": 2.8410155993982453,
-            "velocityY": -1.1955520018204469,
-            "timestamp": 1.1847177849447499
-        },
-        {
-            "x": 2.5729562458095865,
-            "y": 2.5026984837196036,
-            "heading": -0.33601688083426684,
-            "angularVelocity": -0.6062205955726221,
-            "velocityX": 2.9677709434035733,
-            "velocityY": -0.9791049181495252,
-            "timestamp": 1.2148003142313908
-        },
-        {
-            "x": 2.665882293188928,
-            "y": 2.479849558811985,
-            "heading": -0.35406456410202075,
-            "angularVelocity": -0.5999390242685017,
-            "velocityX": 3.08903704518594,
-            "velocityY": -0.7595413500609144,
-            "timestamp": 1.2448828435180317
-        },
-        {
-            "x": 2.7622502226261916,
-            "y": 2.4637149411085275,
-            "heading": -0.37178230702948656,
-            "angularVelocity": -0.5889711851900935,
-            "velocityX": 3.2034516951353296,
-            "velocityY": -0.5363451174508257,
-            "timestamp": 1.2749653728046726
-        },
-        {
-            "x": 2.8617965704136163,
-            "y": 2.4544224370162078,
-            "heading": -0.3889934776908366,
-            "angularVelocity": -0.57213176782302,
-            "velocityX": 3.309108314626244,
-            "velocityY": -0.30890035886875983,
-            "timestamp": 1.3050479020913135
-        },
-        {
-            "x": 2.96417486527032,
-            "y": 2.452120989561553,
-            "heading": -0.40547141981393114,
-            "angularVelocity": -0.5477578685651567,
-            "velocityX": 3.4032475754010525,
-            "velocityY": -0.07650445322366382,
-            "timestamp": 1.3351304313779544
-        },
-        {
-            "x": 3.068913958464018,
-            "y": 2.456980552046256,
-            "heading": -0.4209173641629785,
-            "angularVelocity": -0.5134523165215639,
-            "velocityX": 3.4817249638713506,
-            "velocityY": 0.16154102065001244,
-            "timestamp": 1.3652129606645953
-        },
-        {
-            "x": 3.175348297900835,
-            "y": 2.469181868617686,
-            "heading": -0.43492680962689906,
-            "angularVelocity": -0.4657003847794821,
-            "velocityX": 3.5380781457121575,
-            "velocityY": 0.4055947708108251,
-            "timestamp": 1.3952954899512362
-        },
-        {
-            "x": 3.2825012821379658,
-            "y": 2.488873812540701,
-            "heading": -0.44694408076253883,
-            "angularVelocity": -0.3994767534718848,
-            "velocityX": 3.5619672540204803,
-            "velocityY": 0.6545973490252659,
-            "timestamp": 1.425378019237877
-        },
-        {
-            "x": 3.3889723525651156,
-            "y": 2.516050236824646,
-            "heading": -0.4565561347315361,
-            "angularVelocity": -0.3195227993435668,
-            "velocityX": 3.539299153094488,
-            "velocityY": 0.9033955896790181,
-            "timestamp": 1.455460548524518
-        },
-        {
-            "x": 3.4932758480756925,
-            "y": 2.5504542246131905,
-            "heading": -0.4662703499641935,
-            "angularVelocity": -0.32291883239253294,
-            "velocityX": 3.4672448754798637,
-            "velocityY": 1.1436534295611822,
-            "timestamp": 1.485543077811159
-        },
-        {
-            "x": 3.5930108067726363,
-            "y": 2.5908608883384305,
-            "heading": -0.4766346317588353,
-            "angularVelocity": -0.3445282707409342,
-            "velocityX": 3.315378096922126,
-            "velocityY": 1.3431936969204072,
-            "timestamp": 1.5156256070977998
-        },
-        {
-            "x": 3.686096614652251,
-            "y": 2.634834387797071,
-            "heading": -0.48720467819938723,
-            "angularVelocity": -0.3513682755805826,
-            "velocityX": 3.094347785475507,
-            "velocityY": 1.4617620426674243,
-            "timestamp": 1.5457081363844407
-        },
-        {
-            "x": 3.771715868998783,
-            "y": 2.679896340499022,
-            "heading": -0.4975881116213702,
-            "angularVelocity": -0.34516490694787005,
-            "velocityX": 2.8461454663839936,
-            "velocityY": 1.497944280967274,
-            "timestamp": 1.5757906656710816
-        },
-        {
-            "x": 3.8498203373891897,
-            "y": 2.724287723719235,
-            "heading": -0.507530883254748,
-            "angularVelocity": -0.33051647814035945,
-            "velocityX": 2.596339810598824,
-            "velocityY": 1.475653286903962,
-            "timestamp": 1.6058731949577225
-        },
-        {
-            "x": 3.920599661686896,
-            "y": 2.7668759161025953,
-            "heading": -0.5168796397769617,
-            "angularVelocity": -0.3107702957131982,
-            "velocityX": 2.35283820796086,
-            "velocityY": 1.415711823216824,
-            "timestamp": 1.6359557242443634
-        },
-        {
-            "x": 3.984278053809844,
-            "y": 2.806919241781621,
-            "heading": -0.525538519539268,
-            "angularVelocity": -0.287837491315874,
-            "velocityX": 2.11678983227083,
-            "velocityY": 1.3311156551191636,
-            "timestamp": 1.6660382535310043
-        },
-        {
-            "x": 4.041059171114166,
-            "y": 2.843909698702473,
-            "heading": -0.5334440710807248,
-            "angularVelocity": -0.26279544070665484,
-            "velocityX": 1.8875114111345854,
-            "velocityY": 1.229632540814369,
-            "timestamp": 1.6961207828176452
-        },
-        {
-            "x": 4.091116341717457,
-            "y": 2.8774832028649056,
-            "heading": -0.5405519914461838,
-            "angularVelocity": -0.23628067632639732,
-            "velocityX": 1.6639947434713853,
-            "velocityY": 1.11604658778956,
-            "timestamp": 1.7262033121042861
-        },
-        {
-            "x": 4.1345948250930045,
-            "y": 2.9073683770064043,
-            "heading": -0.5468299348831553,
-            "angularVelocity": -0.20869067813911088,
-            "velocityX": 1.445306774615446,
-            "velocityY": 0.9934395428236782,
-            "timestamp": 1.756285841390927
-        },
-        {
-            "x": 4.171616484177909,
-            "y": 2.933356232054327,
-            "heading": -0.552253409487953,
-            "angularVelocity": -0.18028652288901045,
-            "velocityX": 1.2306697595852323,
-            "velocityY": 0.863885306993297,
-            "timestamp": 1.786368370677568
-        },
-        {
-            "x": 4.202284260430688,
-            "y": 2.9552814196152344,
-            "heading": -0.5568033106587852,
-            "angularVelocity": -0.15124729464987938,
-            "velocityX": 1.0194547127524736,
-            "velocityY": 0.7288345787681281,
-            "timestamp": 1.8164508999642088
-        },
-        {
-            "x": 4.226685878711894,
-            "y": 2.973010149818714,
-            "heading": -0.5604643670760648,
-            "angularVelocity": -0.12170041895060278,
-            "velocityX": 0.8111558057069341,
-            "velocityY": 0.5893364229633967,
-            "timestamp": 1.8465334292508497
-        },
-        {
-            "x": 4.244896778926994,
-            "y": 2.98643211128414,
-            "heading": -0.5632241207030344,
-            "angularVelocity": -0.09173941461743668,
-            "velocityX": 0.6053646633758383,
-            "velocityY": 0.4461713088529227,
-            "timestamp": 1.8766159585374906
-        },
-        {
-            "x": 4.256982406189367,
-            "y": 2.9954548894256625,
-            "heading": -0.565072233243941,
-            "angularVelocity": -0.06143474583855434,
-            "velocityX": 0.4017490400230914,
-            "velocityY": 0.2999341596429636,
-            "timestamp": 1.9066984878241315
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -0.030840716457660636,
-            "velocityX": 0.20003616561938206,
-            "velocityY": 0.15108804618886046,
-            "timestamp": 1.9367810171107724
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -4.9732030554004765e-30,
-            "velocityX": -4.223945033236224e-32,
-            "velocityY": 9.969983495378875e-32,
-            "timestamp": 1.9668635463974133
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.traj
deleted file mode 100644
index 7d72e43b..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.traj	
+++ /dev/null
@@ -1,526 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -4.99624931676796e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2447003077480563,
-            "y": 5.548424158968356,
-            "heading": -0.001651505925702968,
-            "angularVelocity": -0.042250731032121805,
-            "velocityX": -0.13558284496887443,
-            "velocityY": -0.2961465280181771,
-            "timestamp": 0.03908822132445813
-        },
-        {
-            "x": 2.234100924566356,
-            "y": 5.525272476708423,
-            "heading": -0.0049545293965937804,
-            "angularVelocity": -0.08450175932731885,
-            "velocityX": -0.27116565611205723,
-            "velocityY": -0.5922930610671583,
-            "timestamp": 0.07817644264891627
-        },
-        {
-            "x": 2.2182018518980375,
-            "y": 5.490544953203296,
-            "heading": -0.009909100000268336,
-            "angularVelocity": -0.12675354456654908,
-            "velocityX": -0.4067484303351874,
-            "velocityY": -0.8884395945486457,
-            "timestamp": 0.1172646639733744
-        },
-        {
-            "x": 2.1970030913105703,
-            "y": 5.444241588640874,
-            "heading": -0.016515264878529855,
-            "angularVelocity": -0.16900653584180803,
-            "velocityX": -0.5423311644575062,
-            "velocityY": -1.1845861232230646,
-            "timestamp": 0.15635288529783253
-        },
-        {
-            "x": 2.170504644501127,
-            "y": 5.386362383452732,
-            "heading": -0.024773088013563174,
-            "angularVelocity": -0.21126116398317848,
-            "velocityX": -0.6779138551608025,
-            "velocityY": -1.480732640856411,
-            "timestamp": 0.19544110662229067
-        },
-        {
-            "x": 2.1387065133083243,
-            "y": 5.31690733837683,
-            "heading": -0.03468264893636917,
-            "angularVelocity": -0.2535178267784212,
-            "velocityX": -0.8134964988264785,
-            "velocityY": -1.7768791396104282,
-            "timestamp": 0.2345293279467488
-        },
-        {
-            "x": 2.1016086997403236,
-            "y": 5.235876454566401,
-            "heading": -0.04624404029676127,
-            "angularVelocity": -0.2957768598481422,
-            "velocityX": -0.9490790911170489,
-            "velocityY": -2.0730256088611454,
-            "timestamp": 0.27361754927120696
-        },
-        {
-            "x": 2.0592112060450187,
-            "y": 5.143269733801358,
-            "heading": -0.05945736298631538,
-            "angularVelocity": -0.33803847404262444,
-            "velocityX": -1.0846616258994186,
-            "velocityY": -2.369172032575987,
-            "timestamp": 0.31270577059566507
-        },
-        {
-            "x": 2.011514034901109,
-            "y": 5.039087178972333,
-            "heading": -0.07432271508161363,
-            "angularVelocity": -0.3803025973454465,
-            "velocityX": -1.2202440921521842,
-            "velocityY": -2.6653183823394446,
-            "timestamp": 0.3517939919201232
-        },
-        {
-            "x": 1.9585171900495353,
-            "y": 4.923328795522167,
-            "heading": -0.09084016031123929,
-            "angularVelocity": -0.4225683510264515,
-            "velocityX": -1.3558264626999135,
-            "velocityY": -2.9614645928578263,
-            "timestamp": 0.3908822132445813
-        },
-        {
-            "x": 1.9002206796290688,
-            "y": 4.795994598697166,
-            "heading": -0.10900958241827326,
-            "angularVelocity": -0.46483113048874997,
-            "velocityX": -1.491408625032185,
-            "velocityY": -3.257610413327559,
-            "timestamp": 0.4299704345690394
-        },
-        {
-            "x": 1.8386671955063139,
-            "y": 4.661546724325594,
-            "heading": -0.10900958703871873,
-            "angularVelocity": -1.182055697467884e-7,
-            "velocityX": -1.5747322860207698,
-            "velocityY": -3.439600724104782,
-            "timestamp": 0.4690586558934975
-        },
-        {
-            "x": 1.7777943297173011,
-            "y": 4.526789325478927,
-            "heading": -0.10900959166091434,
-            "angularVelocity": -1.1825034389504455e-7,
-            "velocityX": -1.5573199221351706,
-            "velocityY": -3.447519336526256,
-            "timestamp": 0.5081468772179556
-        },
-        {
-            "x": 1.728739704549767,
-            "y": 4.38729485969642,
-            "heading": -0.10900959636164997,
-            "angularVelocity": -1.2025964513922604e-7,
-            "velocityX": -1.2549720479816096,
-            "velocityY": -3.5687084511883413,
-            "timestamp": 0.5472350985424137
-        },
-        {
-            "x": 1.6918738865194354,
-            "y": 4.244095790476986,
-            "heading": -0.10900960128999353,
-            "angularVelocity": -1.2608257379035844e-7,
-            "velocityX": -0.9431439134643449,
-            "velocityY": -3.6634838927766116,
-            "timestamp": 0.5863233198668718
-        },
-        {
-            "x": 1.6674707596733624,
-            "y": 4.098254956364247,
-            "heading": -0.10900960662650709,
-            "angularVelocity": -1.3652485001388814e-7,
-            "velocityX": -0.6243089610931135,
-            "velocityY": -3.7310685718382324,
-            "timestamp": 0.6254115411913299
-        },
-        {
-            "x": 1.6557115284805652,
-            "y": 3.9508548842430677,
-            "heading": -0.1090096126146074,
-            "angularVelocity": -1.531944945007552e-7,
-            "velocityX": -0.3008382268193907,
-            "velocityY": -3.7709587985000237,
-            "timestamp": 0.664499762515788
-        },
-        {
-            "x": 1.6566835066189385,
-            "y": 3.8029896922639588,
-            "heading": -0.10900961961577929,
-            "angularVelocity": -1.79112061302596e-7,
-            "velocityX": 0.024866266754091815,
-            "velocityY": -3.7828580316236344,
-            "timestamp": 0.7035879838402461
-        },
-        {
-            "x": 1.6703794917330834,
-            "y": 3.6557569809887474,
-            "heading": -0.10900969829200789,
-            "angularVelocity": -0.000002012786100988185,
-            "velocityX": 0.3503865013576149,
-            "velocityY": -3.766677180142832,
-            "timestamp": 0.7426762051647042
-        },
-        {
-            "x": 1.6964297342683634,
-            "y": 3.5114772969924033,
-            "heading": -0.1124967438020359,
-            "angularVelocity": -0.0892096235606636,
-            "velocityX": 0.6664473760284892,
-            "velocityY": -3.6911294274233937,
-            "timestamp": 0.7817644264891623
-        },
-        {
-            "x": 1.7337062034817727,
-            "y": 3.3729422790966246,
-            "heading": -0.12243166775651844,
-            "angularVelocity": -0.2541666931326176,
-            "velocityX": 0.9536496660724848,
-            "velocityY": -3.54416274779685,
-            "timestamp": 0.8208526478136204
-        },
-        {
-            "x": 1.7809992442628195,
-            "y": 3.2421845220219927,
-            "heading": -0.1367067464310455,
-            "angularVelocity": -0.3652015413039525,
-            "velocityX": 1.2099051626962352,
-            "velocityY": -3.3451958836719715,
-            "timestamp": 0.8599408691380785
-        },
-        {
-            "x": 1.8374227958909133,
-            "y": 3.120270451905998,
-            "heading": -0.154016140064332,
-            "angularVelocity": -0.4428288892862246,
-            "velocityX": 1.443492431128254,
-            "velocityY": -3.118946475052685,
-            "timestamp": 0.8990290904625367
-        },
-        {
-            "x": 1.9023445230612375,
-            "y": 3.007828092475748,
-            "heading": -0.17349166731783877,
-            "angularVelocity": -0.4982454200677405,
-            "velocityX": 1.660902567846671,
-            "velocityY": -2.8766302384778886,
-            "timestamp": 0.9381173117869948
-        },
-        {
-            "x": 1.9753011558084586,
-            "y": 2.90526251520286,
-            "heading": -0.19452421295735298,
-            "angularVelocity": -0.5380788617862533,
-            "velocityX": 1.866460797529241,
-            "velocityY": -2.623950995915806,
-            "timestamp": 0.9772055331114529
-        },
-        {
-            "x": 2.0559421744907826,
-            "y": 2.8128534355639654,
-            "heading": -0.21666661660279088,
-            "angularVelocity": -0.5664725304751266,
-            "velocityX": 2.0630516291077963,
-            "velocityY": -2.3641157491367464,
-            "timestamp": 1.016293754435911
-        },
-        {
-            "x": 2.1439944855858766,
-            "y": 2.7308042438703497,
-            "heading": -0.2395781801271676,
-            "angularVelocity": -0.586150066389446,
-            "velocityX": 2.252655866948419,
-            "velocityY": -2.099077136627731,
-            "timestamp": 1.055381975760369
-        },
-        {
-            "x": 2.2392399311065674,
-            "y": 2.659268856048584,
-            "heading": -0.2629913490343549,
-            "angularVelocity": -0.5989827143282399,
-            "velocityX": 2.4366789353272003,
-            "velocityY": -1.830100869209989,
-            "timestamp": 1.0944701970848272
-        },
-        {
-            "x": 2.3166964235934535,
-            "y": 2.610513248595422,
-            "heading": -0.2811797949710667,
-            "angularVelocity": -0.6046182408202432,
-            "velocityX": 2.5747998696792043,
-            "velocityY": -1.620728330008343,
-            "timestamp": 1.124552726371468
-        },
-        {
-            "x": 2.398213254515867,
-            "y": 2.5681176642029904,
-            "heading": -0.29946946527559826,
-            "angularVelocity": -0.6079831296849904,
-            "velocityX": 2.7097731758410535,
-            "velocityY": -1.409309170398157,
-            "timestamp": 1.154635255658109
-        },
-        {
-            "x": 2.483678189488588,
-            "y": 2.5321524360945276,
-            "heading": -0.31778023201379,
-            "angularVelocity": -0.6086844149213316,
-            "velocityX": 2.8410155993982453,
-            "velocityY": -1.1955520018204469,
-            "timestamp": 1.1847177849447499
-        },
-        {
-            "x": 2.5729562458095865,
-            "y": 2.5026984837196036,
-            "heading": -0.33601688083426684,
-            "angularVelocity": -0.6062205955726221,
-            "velocityX": 2.9677709434035733,
-            "velocityY": -0.9791049181495252,
-            "timestamp": 1.2148003142313908
-        },
-        {
-            "x": 2.665882293188928,
-            "y": 2.479849558811985,
-            "heading": -0.35406456410202075,
-            "angularVelocity": -0.5999390242685017,
-            "velocityX": 3.08903704518594,
-            "velocityY": -0.7595413500609144,
-            "timestamp": 1.2448828435180317
-        },
-        {
-            "x": 2.7622502226261916,
-            "y": 2.4637149411085275,
-            "heading": -0.37178230702948656,
-            "angularVelocity": -0.5889711851900935,
-            "velocityX": 3.2034516951353296,
-            "velocityY": -0.5363451174508257,
-            "timestamp": 1.2749653728046726
-        },
-        {
-            "x": 2.8617965704136163,
-            "y": 2.4544224370162078,
-            "heading": -0.3889934776908366,
-            "angularVelocity": -0.57213176782302,
-            "velocityX": 3.309108314626244,
-            "velocityY": -0.30890035886875983,
-            "timestamp": 1.3050479020913135
-        },
-        {
-            "x": 2.96417486527032,
-            "y": 2.452120989561553,
-            "heading": -0.40547141981393114,
-            "angularVelocity": -0.5477578685651567,
-            "velocityX": 3.4032475754010525,
-            "velocityY": -0.07650445322366382,
-            "timestamp": 1.3351304313779544
-        },
-        {
-            "x": 3.068913958464018,
-            "y": 2.456980552046256,
-            "heading": -0.4209173641629785,
-            "angularVelocity": -0.5134523165215639,
-            "velocityX": 3.4817249638713506,
-            "velocityY": 0.16154102065001244,
-            "timestamp": 1.3652129606645953
-        },
-        {
-            "x": 3.175348297900835,
-            "y": 2.469181868617686,
-            "heading": -0.43492680962689906,
-            "angularVelocity": -0.4657003847794821,
-            "velocityX": 3.5380781457121575,
-            "velocityY": 0.4055947708108251,
-            "timestamp": 1.3952954899512362
-        },
-        {
-            "x": 3.2825012821379658,
-            "y": 2.488873812540701,
-            "heading": -0.44694408076253883,
-            "angularVelocity": -0.3994767534718848,
-            "velocityX": 3.5619672540204803,
-            "velocityY": 0.6545973490252659,
-            "timestamp": 1.425378019237877
-        },
-        {
-            "x": 3.3889723525651156,
-            "y": 2.516050236824646,
-            "heading": -0.4565561347315361,
-            "angularVelocity": -0.3195227993435668,
-            "velocityX": 3.539299153094488,
-            "velocityY": 0.9033955896790181,
-            "timestamp": 1.455460548524518
-        },
-        {
-            "x": 3.4932758480756925,
-            "y": 2.5504542246131905,
-            "heading": -0.4662703499641935,
-            "angularVelocity": -0.32291883239253294,
-            "velocityX": 3.4672448754798637,
-            "velocityY": 1.1436534295611822,
-            "timestamp": 1.485543077811159
-        },
-        {
-            "x": 3.5930108067726363,
-            "y": 2.5908608883384305,
-            "heading": -0.4766346317588353,
-            "angularVelocity": -0.3445282707409342,
-            "velocityX": 3.315378096922126,
-            "velocityY": 1.3431936969204072,
-            "timestamp": 1.5156256070977998
-        },
-        {
-            "x": 3.686096614652251,
-            "y": 2.634834387797071,
-            "heading": -0.48720467819938723,
-            "angularVelocity": -0.3513682755805826,
-            "velocityX": 3.094347785475507,
-            "velocityY": 1.4617620426674243,
-            "timestamp": 1.5457081363844407
-        },
-        {
-            "x": 3.771715868998783,
-            "y": 2.679896340499022,
-            "heading": -0.4975881116213702,
-            "angularVelocity": -0.34516490694787005,
-            "velocityX": 2.8461454663839936,
-            "velocityY": 1.497944280967274,
-            "timestamp": 1.5757906656710816
-        },
-        {
-            "x": 3.8498203373891897,
-            "y": 2.724287723719235,
-            "heading": -0.507530883254748,
-            "angularVelocity": -0.33051647814035945,
-            "velocityX": 2.596339810598824,
-            "velocityY": 1.475653286903962,
-            "timestamp": 1.6058731949577225
-        },
-        {
-            "x": 3.920599661686896,
-            "y": 2.7668759161025953,
-            "heading": -0.5168796397769617,
-            "angularVelocity": -0.3107702957131982,
-            "velocityX": 2.35283820796086,
-            "velocityY": 1.415711823216824,
-            "timestamp": 1.6359557242443634
-        },
-        {
-            "x": 3.984278053809844,
-            "y": 2.806919241781621,
-            "heading": -0.525538519539268,
-            "angularVelocity": -0.287837491315874,
-            "velocityX": 2.11678983227083,
-            "velocityY": 1.3311156551191636,
-            "timestamp": 1.6660382535310043
-        },
-        {
-            "x": 4.041059171114166,
-            "y": 2.843909698702473,
-            "heading": -0.5334440710807248,
-            "angularVelocity": -0.26279544070665484,
-            "velocityX": 1.8875114111345854,
-            "velocityY": 1.229632540814369,
-            "timestamp": 1.6961207828176452
-        },
-        {
-            "x": 4.091116341717457,
-            "y": 2.8774832028649056,
-            "heading": -0.5405519914461838,
-            "angularVelocity": -0.23628067632639732,
-            "velocityX": 1.6639947434713853,
-            "velocityY": 1.11604658778956,
-            "timestamp": 1.7262033121042861
-        },
-        {
-            "x": 4.1345948250930045,
-            "y": 2.9073683770064043,
-            "heading": -0.5468299348831553,
-            "angularVelocity": -0.20869067813911088,
-            "velocityX": 1.445306774615446,
-            "velocityY": 0.9934395428236782,
-            "timestamp": 1.756285841390927
-        },
-        {
-            "x": 4.171616484177909,
-            "y": 2.933356232054327,
-            "heading": -0.552253409487953,
-            "angularVelocity": -0.18028652288901045,
-            "velocityX": 1.2306697595852323,
-            "velocityY": 0.863885306993297,
-            "timestamp": 1.786368370677568
-        },
-        {
-            "x": 4.202284260430688,
-            "y": 2.9552814196152344,
-            "heading": -0.5568033106587852,
-            "angularVelocity": -0.15124729464987938,
-            "velocityX": 1.0194547127524736,
-            "velocityY": 0.7288345787681281,
-            "timestamp": 1.8164508999642088
-        },
-        {
-            "x": 4.226685878711894,
-            "y": 2.973010149818714,
-            "heading": -0.5604643670760648,
-            "angularVelocity": -0.12170041895060278,
-            "velocityX": 0.8111558057069341,
-            "velocityY": 0.5893364229633967,
-            "timestamp": 1.8465334292508497
-        },
-        {
-            "x": 4.244896778926994,
-            "y": 2.98643211128414,
-            "heading": -0.5632241207030344,
-            "angularVelocity": -0.09173941461743668,
-            "velocityX": 0.6053646633758383,
-            "velocityY": 0.4461713088529227,
-            "timestamp": 1.8766159585374906
-        },
-        {
-            "x": 4.256982406189367,
-            "y": 2.9954548894256625,
-            "heading": -0.565072233243941,
-            "angularVelocity": -0.06143474583855434,
-            "velocityX": 0.4017490400230914,
-            "velocityY": 0.2999341596429636,
-            "timestamp": 1.9066984878241315
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -0.030840716457660636,
-            "velocityX": 0.20003616561938206,
-            "velocityY": 0.15108804618886046,
-            "timestamp": 1.9367810171107724
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -4.9732030554004765e-30,
-            "velocityX": -4.223945033236224e-32,
-            "velocityY": 9.969983495378875e-32,
-            "timestamp": 1.9668635463974133
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.1.traj
deleted file mode 100644
index 848b8926..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.1.traj	
+++ /dev/null
@@ -1,553 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 1.4317938010297452e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2473468901462774,
-            "y": 5.551784954334613,
-            "heading": -0.004046039178700474,
-            "angularVelocity": -0.12521248615625988,
-            "velocityX": -0.08210560159156222,
-            "velocityY": -0.2542304328304919,
-            "timestamp": 0.032313384255078216
-        },
-        {
-            "x": 2.2420392078855844,
-            "y": 5.535352460743813,
-            "heading": -0.012062241078351911,
-            "angularVelocity": -0.24807682898124367,
-            "velocityX": -0.16425646471427996,
-            "velocityY": -0.5085352082308542,
-            "timestamp": 0.06462676851015643
-        },
-        {
-            "x": 2.234075272195187,
-            "y": 5.510699784095841,
-            "heading": -0.023960373728225737,
-            "angularVelocity": -0.36821066329520014,
-            "velocityX": -0.24645935032774877,
-            "velocityY": -0.7629246275588465,
-            "timestamp": 0.09694015276523466
-        },
-        {
-            "x": 2.2234531308615284,
-            "y": 5.477823761292619,
-            "heading": -0.03963572188039383,
-            "angularVelocity": -0.4851038822931769,
-            "velocityX": -0.32872265095504705,
-            "velocityY": -1.017411935057708,
-            "timestamp": 0.12925353702031286
-        },
-        {
-            "x": 2.2101704924088987,
-            "y": 5.436720678146679,
-            "heading": -0.05896132607089341,
-            "angularVelocity": -0.5980680958065965,
-            "velocityX": -0.4110568657178767,
-            "velocityY": -1.27201418525146,
-            "timestamp": 0.16156692127539107
-        },
-        {
-            "x": 2.1942246366351648,
-            "y": 5.38738610923438,
-            "heading": -0.08177969135834087,
-            "angularVelocity": -0.7061583245915709,
-            "velocityX": -0.4934752623822669,
-            "velocityY": -1.5267533887152571,
-            "timestamp": 0.19388030553046928
-        },
-        {
-            "x": 2.1756122932404036,
-            "y": 5.329814710705053,
-            "heading": -0.10789037911156549,
-            "angularVelocity": -0.8080455933402793,
-            "velocityX": -0.5759948647853634,
-            "velocityY": -1.7816579679449487,
-            "timestamp": 0.2261936897855475
-        },
-        {
-            "x": 2.1543294701171383,
-            "y": 5.263999960188385,
-            "heading": -0.1370304714941401,
-            "angularVelocity": -0.9017963625397062,
-            "velocityX": -0.6586380106540685,
-            "velocityY": -2.0367643945039555,
-            "timestamp": 0.25850707404062573
-        },
-        {
-            "x": 2.1303711997350336,
-            "y": 5.1899338649176165,
-            "heading": -0.1688416775308267,
-            "angularVelocity": -0.9844591264590365,
-            "velocityX": -0.7414348863300811,
-            "velocityY": -2.2921181726463207,
-            "timestamp": 0.29082045829570397
-        },
-        {
-            "x": 2.1037311567595207,
-            "y": 5.107606785885568,
-            "heading": -0.20280965108227242,
-            "angularVelocity": -1.051204457054369,
-            "velocityX": -0.8244275116843016,
-            "velocityY": -2.5477702484570597,
-            "timestamp": 0.3231338425507822
-        },
-        {
-            "x": 2.074401135228672,
-            "y": 5.017008143765364,
-            "heading": -0.23813648207052604,
-            "angularVelocity": -1.0932569213236978,
-            "velocityX": -0.9076740863575303,
-            "velocityY": -2.8037497219427316,
-            "timestamp": 0.35544722680586044
-        },
-        {
-            "x": 2.0423710011934597,
-            "y": 4.918132506074763,
-            "heading": -0.27341197795784133,
-            "angularVelocity": -1.0916682576129122,
-            "velocityX": -0.9912342756292476,
-            "velocityY": -3.0598973140692034,
-            "timestamp": 0.3877606110609387
-        },
-        {
-            "x": 2.00763899967256,
-            "y": 4.811032729430715,
-            "heading": -0.3053550995537523,
-            "angularVelocity": -0.9885415078704507,
-            "velocityX": -1.0748487761829064,
-            "velocityY": -3.3144091562374,
-            "timestamp": 0.4200739953160169
-        },
-        {
-            "x": 1.9707337568018743,
-            "y": 4.697468766706309,
-            "heading": -0.3150299299334663,
-            "angularVelocity": -0.2994062863655037,
-            "velocityX": -1.1421039213769328,
-            "velocityY": -3.514455862250325,
-            "timestamp": 0.45238737957109515
-        },
-        {
-            "x": 1.9331993710024595,
-            "y": 4.581134395027877,
-            "heading": -0.3150299515537849,
-            "angularVelocity": -6.690824588937528e-7,
-            "velocityX": -1.1615739627617825,
-            "velocityY": -3.6001915107405935,
-            "timestamp": 0.4847007638261734
-        },
-        {
-            "x": 1.8956658385726595,
-            "y": 4.464799748019651,
-            "heading": -0.3150299731737692,
-            "angularVelocity": -6.690721139443934e-7,
-            "velocityX": -1.1615475535931208,
-            "velocityY": -3.600200031352131,
-            "timestamp": 0.5170141480812516
-        },
-        {
-            "x": 1.8630740613066095,
-            "y": 4.346985096208152,
-            "heading": -0.31502999489570194,
-            "angularVelocity": -6.722271053919432e-7,
-            "velocityX": -1.0086154086732113,
-            "velocityY": -3.64600163453891,
-            "timestamp": 0.5493275323363298
-        },
-        {
-            "x": 1.8389498768606893,
-            "y": 4.227149626761845,
-            "heading": -0.3150300170939308,
-            "angularVelocity": -6.869670074324573e-7,
-            "velocityX": -0.7465694170405304,
-            "velocityY": -3.708539733886721,
-            "timestamp": 0.581640916591408
-        },
-        {
-            "x": 1.8234162959667812,
-            "y": 4.10590102408459,
-            "heading": -0.31503004024876186,
-            "angularVelocity": -7.165709048863888e-7,
-            "velocityX": -0.4807166210536207,
-            "velocityY": -3.7522718672898008,
-            "timestamp": 0.6139543008464863
-        },
-        {
-            "x": 1.816552246405149,
-            "y": 3.98385431066073,
-            "heading": -0.315030064931292,
-            "angularVelocity": -7.63848502164674e-7,
-            "velocityX": -0.21242125267501,
-            "velocityY": -3.776970943694307,
-            "timestamp": 0.6462676851015645
-        },
-        {
-            "x": 1.818392595454208,
-            "y": 3.861628584870307,
-            "heading": -0.315030091871416,
-            "angularVelocity": -8.337140982566696e-7,
-            "velocityX": 0.05695315088421205,
-            "velocityY": -3.782510826646546,
-            "timestamp": 0.6785810693566428
-        },
-        {
-            "x": 1.828928030254088,
-            "y": 3.739843861498511,
-            "heading": -0.3150301220653929,
-            "angularVelocity": -9.344108498281474e-7,
-            "velocityX": 0.32603935003261814,
-            "velocityY": -3.7688631562216113,
-            "timestamp": 0.710894453611721
-        },
-        {
-            "x": 1.8481051197294465,
-            "y": 3.619117921789427,
-            "heading": -0.3150301569615331,
-            "angularVelocity": -0.0000010799283629257,
-            "velocityX": 0.593472021499706,
-            "velocityY": -3.7360970536569806,
-            "timestamp": 0.7432078378667992
-        },
-        {
-            "x": 1.875826590719106,
-            "y": 3.500063178055521,
-            "heading": -0.31503019880828736,
-            "angularVelocity": -0.0000012950285239246719,
-            "velocityX": 0.8578943873792161,
-            "velocityY": -3.684378670896923,
-            "timestamp": 0.7755212221218775
-        },
-        {
-            "x": 1.9119518234377388,
-            "y": 3.3832835666467993,
-            "heading": -0.31503025136937535,
-            "angularVelocity": -0.0000016266042432729645,
-            "velocityX": 1.1179650027822445,
-            "velocityY": -3.61397031294762,
-            "timestamp": 0.8078346063769557
-        },
-        {
-            "x": 1.9562975654606443,
-            "y": 3.269371484637439,
-            "heading": -0.31503032157308203,
-            "angularVelocity": -0.0000021725891085409373,
-            "velocityX": 1.3723645184560365,
-            "velocityY": -3.5252290849559675,
-            "timestamp": 0.8401479906320339
-        },
-        {
-            "x": 2.008638858795166,
-            "y": 3.158904790878296,
-            "heading": -0.3150304332740777,
-            "angularVelocity": -0.0000034568027529519608,
-            "velocityX": 1.6198022751608165,
-            "velocityY": -3.4186049002831713,
-            "timestamp": 0.8724613748871122
-        },
-        {
-            "x": 2.0775165641117233,
-            "y": 3.0394044472847606,
-            "heading": -0.3150305189714284,
-            "angularVelocity": -0.000002350392551393865,
-            "velocityX": 1.8890857675343722,
-            "velocityY": -3.277495922091173,
-            "timestamp": 0.9089222420961518
-        },
-        {
-            "x": 2.155767726009908,
-            "y": 2.9258208622381168,
-            "heading": -0.3150305770370975,
-            "angularVelocity": -0.00000159254767063084,
-            "velocityX": 2.1461684235196876,
-            "velocityY": -3.1152189660064753,
-            "timestamp": 0.9453831093051914
-        },
-        {
-            "x": 2.242886953120307,
-            "y": 2.818887620265158,
-            "heading": -0.3150306203630764,
-            "angularVelocity": -0.0000011882871183876306,
-            "velocityX": 2.389389879591231,
-            "velocityY": -2.932822232666113,
-            "timestamp": 0.981843976514231
-        },
-        {
-            "x": 2.338311580267833,
-            "y": 2.719295349641276,
-            "heading": -0.3150306549359537,
-            "angularVelocity": -9.482187337248934e-7,
-            "velocityX": 2.617179306252704,
-            "velocityY": -2.731483868798106,
-            "timestamp": 1.0183048437232707
-        },
-        {
-            "x": 2.4414253013619946,
-            "y": 2.6276872624602983,
-            "heading": -0.31503068395926054,
-            "angularVelocity": -7.960125215618009e-7,
-            "velocityX": 2.8280655120730875,
-            "velocityY": -2.512504342142078,
-            "timestamp": 1.0547657109323103
-        },
-        {
-            "x": 2.5515621486647997,
-            "y": 2.5446549983777076,
-            "heading": -0.3150307093352344,
-            "angularVelocity": -6.959783403374032e-7,
-            "velocityX": 3.020686443670201,
-            "velocityY": -2.2772981127010774,
-            "timestamp": 1.09122657814135
-        },
-        {
-            "x": 2.6680107915990137,
-            "y": 2.4707347968336117,
-            "heading": -0.31503073229503964,
-            "angularVelocity": -6.297108928988803e-7,
-            "velocityX": 3.1937979496369038,
-            "velocityY": -2.0273846236374995,
-            "timestamp": 1.1276874453503896
-        },
-        {
-            "x": 2.7900191236562386,
-            "y": 2.4064040113359084,
-            "heading": -0.3150307537024253,
-            "angularVelocity": -5.871332010760787e-7,
-            "velocityX": 3.3462816821585726,
-            "velocityY": -1.764378919702536,
-            "timestamp": 1.1641483125594292
-        },
-        {
-            "x": 2.9167990841955884,
-            "y": 2.352077913506683,
-            "heading": -0.3150307742165802,
-            "angularVelocity": -5.626348607586924e-7,
-            "velocityX": 3.477151539278742,
-            "velocityY": -1.4899836999970275,
-            "timestamp": 1.2006091797684688
-        },
-        {
-            "x": 3.0475311021850517,
-            "y": 2.308104934680324,
-            "heading": -0.31503079438993686,
-            "angularVelocity": -5.532878997498012e-7,
-            "velocityX": 3.585543296047864,
-            "velocityY": -1.2060321707174542,
-            "timestamp": 1.2370700469775084
-        },
-        {
-            "x": 3.178817728061225,
-            "y": 2.2658165761507623,
-            "heading": -0.3150308145386853,
-            "angularVelocity": -5.526129783451472e-7,
-            "velocityX": 3.600754340906743,
-            "velocityY": -1.159828653748452,
-            "timestamp": 1.273530914186548
-        },
-        {
-            "x": 3.3101043979960063,
-            "y": 2.2235283544039026,
-            "heading": -0.3150308346874411,
-            "angularVelocity": -5.526131795163358e-7,
-            "velocityX": 3.600755549287413,
-            "velocityY": -1.1598249022550946,
-            "timestamp": 1.3099917813955877
-        },
-        {
-            "x": 3.4413910762638236,
-            "y": 2.1812401585275256,
-            "heading": -0.31503085483620696,
-            "angularVelocity": -5.52613457009699e-7,
-            "velocityX": 3.60075577783481,
-            "velocityY": -1.159824192714005,
-            "timestamp": 1.3464526486046273
-        },
-        {
-            "x": 3.572677762945226,
-            "y": 2.138951988771722,
-            "heading": -0.31503087498498206,
-            "angularVelocity": -5.52613710159953e-7,
-            "velocityX": 3.6007560085914068,
-            "velocityY": -1.1598234763137998,
-            "timestamp": 1.382913515813667
-        },
-        {
-            "x": 3.7039644581047573,
-            "y": 2.0966638453368867,
-            "heading": -0.31503089513376514,
-            "angularVelocity": -5.526139295643368e-7,
-            "velocityX": 3.6007562411181984,
-            "velocityY": -1.1598227544173942,
-            "timestamp": 1.4193743830227066
-        },
-        {
-            "x": 3.8352511618245644,
-            "y": 2.05437572847807,
-            "heading": -0.3150309152825546,
-            "angularVelocity": -5.526141044747151e-7,
-            "velocityX": 3.600756475898009,
-            "velocityY": -1.1598220255258234,
-            "timestamp": 1.4558352502317462
-        },
-        {
-            "x": 3.966537874250263,
-            "y": 2.01208763864737,
-            "heading": -0.3150309354313486,
-            "angularVelocity": -5.526142273042023e-7,
-            "velocityX": 3.6007567146716046,
-            "velocityY": -1.159821284234734,
-            "timestamp": 1.4922961174407858
-        },
-        {
-            "x": 4.097824595645338,
-            "y": 1.9697995766628096,
-            "heading": -0.3150309555801447,
-            "angularVelocity": -5.526142864672618e-7,
-            "velocityX": 3.600756960671674,
-            "velocityY": -1.159820520507965,
-            "timestamp": 1.5287569846498255
-        },
-        {
-            "x": 4.229111326453571,
-            "y": 1.9275115439021684,
-            "heading": -0.3150309757289402,
-            "angularVelocity": -5.5261426880692e-7,
-            "velocityX": 3.600757218843208,
-            "velocityY": -1.1598197189933317,
-            "timestamp": 1.565217851858865
-        },
-        {
-            "x": 4.360398019008411,
-            "y": 1.885223392382287,
-            "heading": -0.31503099587881916,
-            "angularVelocity": -5.52643987126892e-7,
-            "velocityX": 3.6007561696802117,
-            "velocityY": -1.1598229761632481,
-            "timestamp": 1.6016787190679047
-        },
-        {
-            "x": 4.485161526388635,
-            "y": 1.844329974236374,
-            "heading": -0.3370638509456068,
-            "angularVelocity": -0.6042877406197901,
-            "velocityX": 3.4218469534726492,
-            "velocityY": -1.1215700907896822,
-            "timestamp": 1.6381395862769443
-        },
-        {
-            "x": 4.60044458089581,
-            "y": 1.8067761222092775,
-            "heading": -0.3770388194107599,
-            "angularVelocity": -1.0963800788381923,
-            "velocityX": 3.1618297460185825,
-            "velocityY": -1.0299769287381584,
-            "timestamp": 1.674600453485984
-        },
-        {
-            "x": 4.705219804759057,
-            "y": 1.772711102552083,
-            "heading": -0.4189139196427851,
-            "angularVelocity": -1.148494356756437,
-            "velocityX": 2.8736349923479074,
-            "velocityY": -0.9342898911836356,
-            "timestamp": 1.7110613206950236
-        },
-        {
-            "x": 4.799468970200479,
-            "y": 1.7421012617230478,
-            "heading": -0.45948472427125614,
-            "angularVelocity": -1.1127218778387968,
-            "velocityX": 2.5849403115144614,
-            "velocityY": -0.8395258580532651,
-            "timestamp": 1.7475221879040632
-        },
-        {
-            "x": 4.883206648654635,
-            "y": 1.7149242324789875,
-            "heading": -0.49729979894596704,
-            "angularVelocity": -1.037141394852447,
-            "velocityX": 2.296645276538986,
-            "velocityY": -0.7453752838144925,
-            "timestamp": 1.7839830551131028
-        },
-        {
-            "x": 4.956447680828415,
-            "y": 1.6911655730288653,
-            "heading": -0.5315269969459382,
-            "angularVelocity": -0.9387379023033031,
-            "velocityX": 2.0087572726635705,
-            "velocityY": -0.6516207997442213,
-            "timestamp": 1.8204439223221425
-        },
-        {
-            "x": 5.019204214404415,
-            "y": 1.6708154773305386,
-            "heading": -0.5616288277349688,
-            "angularVelocity": -0.825592836737831,
-            "velocityX": 1.7212024392124468,
-            "velocityY": -0.5581352627093101,
-            "timestamp": 1.856904789531182
-        },
-        {
-            "x": 5.071485921099441,
-            "y": 1.6538669506708767,
-            "heading": -0.587231204588931,
-            "angularVelocity": -0.7021878198118158,
-            "velocityX": 1.4339128687006073,
-            "velocityY": -0.46484156733002446,
-            "timestamp": 1.8933656567402217
-        },
-        {
-            "x": 5.11330055140079,
-            "y": 1.6403147844077772,
-            "heading": -0.6080594902506735,
-            "angularVelocity": -0.5712504187662296,
-            "velocityX": 1.1468358682094448,
-            "velocityY": -0.37169072763413913,
-            "timestamp": 1.9298265239492614
-        },
-        {
-            "x": 5.1446544158470235,
-            "y": 1.6301549523614358,
-            "heading": -0.6239033665029361,
-            "angularVelocity": -0.43454469037794713,
-            "velocityX": 0.859931944747064,
-            "velocityY": -0.2786503126240163,
-            "timestamp": 1.966287391158301
-        },
-        {
-            "x": 5.165552752490355,
-            "y": 1.6233842390024875,
-            "heading": -0.6345958555798376,
-            "angularVelocity": -0.29325931869912014,
-            "velocityX": 0.5731716835893133,
-            "velocityY": -0.18569808886140712,
-            "timestamp": 2.0027482583673404
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -0.14821766002380998,
-            "velocityX": 0.2865331603263514,
-            "velocityY": -0.09281839027812194,
-            "timestamp": 2.03920912557638
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 2.046404654845657e-31,
-            "velocityX": 2.793064044552828e-33,
-            "velocityY": 5.221456528298987e-32,
-            "timestamp": 2.0756699927854196
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.traj
deleted file mode 100644
index 848b8926..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.traj	
+++ /dev/null
@@ -1,553 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 1.4317938010297452e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2473468901462774,
-            "y": 5.551784954334613,
-            "heading": -0.004046039178700474,
-            "angularVelocity": -0.12521248615625988,
-            "velocityX": -0.08210560159156222,
-            "velocityY": -0.2542304328304919,
-            "timestamp": 0.032313384255078216
-        },
-        {
-            "x": 2.2420392078855844,
-            "y": 5.535352460743813,
-            "heading": -0.012062241078351911,
-            "angularVelocity": -0.24807682898124367,
-            "velocityX": -0.16425646471427996,
-            "velocityY": -0.5085352082308542,
-            "timestamp": 0.06462676851015643
-        },
-        {
-            "x": 2.234075272195187,
-            "y": 5.510699784095841,
-            "heading": -0.023960373728225737,
-            "angularVelocity": -0.36821066329520014,
-            "velocityX": -0.24645935032774877,
-            "velocityY": -0.7629246275588465,
-            "timestamp": 0.09694015276523466
-        },
-        {
-            "x": 2.2234531308615284,
-            "y": 5.477823761292619,
-            "heading": -0.03963572188039383,
-            "angularVelocity": -0.4851038822931769,
-            "velocityX": -0.32872265095504705,
-            "velocityY": -1.017411935057708,
-            "timestamp": 0.12925353702031286
-        },
-        {
-            "x": 2.2101704924088987,
-            "y": 5.436720678146679,
-            "heading": -0.05896132607089341,
-            "angularVelocity": -0.5980680958065965,
-            "velocityX": -0.4110568657178767,
-            "velocityY": -1.27201418525146,
-            "timestamp": 0.16156692127539107
-        },
-        {
-            "x": 2.1942246366351648,
-            "y": 5.38738610923438,
-            "heading": -0.08177969135834087,
-            "angularVelocity": -0.7061583245915709,
-            "velocityX": -0.4934752623822669,
-            "velocityY": -1.5267533887152571,
-            "timestamp": 0.19388030553046928
-        },
-        {
-            "x": 2.1756122932404036,
-            "y": 5.329814710705053,
-            "heading": -0.10789037911156549,
-            "angularVelocity": -0.8080455933402793,
-            "velocityX": -0.5759948647853634,
-            "velocityY": -1.7816579679449487,
-            "timestamp": 0.2261936897855475
-        },
-        {
-            "x": 2.1543294701171383,
-            "y": 5.263999960188385,
-            "heading": -0.1370304714941401,
-            "angularVelocity": -0.9017963625397062,
-            "velocityX": -0.6586380106540685,
-            "velocityY": -2.0367643945039555,
-            "timestamp": 0.25850707404062573
-        },
-        {
-            "x": 2.1303711997350336,
-            "y": 5.1899338649176165,
-            "heading": -0.1688416775308267,
-            "angularVelocity": -0.9844591264590365,
-            "velocityX": -0.7414348863300811,
-            "velocityY": -2.2921181726463207,
-            "timestamp": 0.29082045829570397
-        },
-        {
-            "x": 2.1037311567595207,
-            "y": 5.107606785885568,
-            "heading": -0.20280965108227242,
-            "angularVelocity": -1.051204457054369,
-            "velocityX": -0.8244275116843016,
-            "velocityY": -2.5477702484570597,
-            "timestamp": 0.3231338425507822
-        },
-        {
-            "x": 2.074401135228672,
-            "y": 5.017008143765364,
-            "heading": -0.23813648207052604,
-            "angularVelocity": -1.0932569213236978,
-            "velocityX": -0.9076740863575303,
-            "velocityY": -2.8037497219427316,
-            "timestamp": 0.35544722680586044
-        },
-        {
-            "x": 2.0423710011934597,
-            "y": 4.918132506074763,
-            "heading": -0.27341197795784133,
-            "angularVelocity": -1.0916682576129122,
-            "velocityX": -0.9912342756292476,
-            "velocityY": -3.0598973140692034,
-            "timestamp": 0.3877606110609387
-        },
-        {
-            "x": 2.00763899967256,
-            "y": 4.811032729430715,
-            "heading": -0.3053550995537523,
-            "angularVelocity": -0.9885415078704507,
-            "velocityX": -1.0748487761829064,
-            "velocityY": -3.3144091562374,
-            "timestamp": 0.4200739953160169
-        },
-        {
-            "x": 1.9707337568018743,
-            "y": 4.697468766706309,
-            "heading": -0.3150299299334663,
-            "angularVelocity": -0.2994062863655037,
-            "velocityX": -1.1421039213769328,
-            "velocityY": -3.514455862250325,
-            "timestamp": 0.45238737957109515
-        },
-        {
-            "x": 1.9331993710024595,
-            "y": 4.581134395027877,
-            "heading": -0.3150299515537849,
-            "angularVelocity": -6.690824588937528e-7,
-            "velocityX": -1.1615739627617825,
-            "velocityY": -3.6001915107405935,
-            "timestamp": 0.4847007638261734
-        },
-        {
-            "x": 1.8956658385726595,
-            "y": 4.464799748019651,
-            "heading": -0.3150299731737692,
-            "angularVelocity": -6.690721139443934e-7,
-            "velocityX": -1.1615475535931208,
-            "velocityY": -3.600200031352131,
-            "timestamp": 0.5170141480812516
-        },
-        {
-            "x": 1.8630740613066095,
-            "y": 4.346985096208152,
-            "heading": -0.31502999489570194,
-            "angularVelocity": -6.722271053919432e-7,
-            "velocityX": -1.0086154086732113,
-            "velocityY": -3.64600163453891,
-            "timestamp": 0.5493275323363298
-        },
-        {
-            "x": 1.8389498768606893,
-            "y": 4.227149626761845,
-            "heading": -0.3150300170939308,
-            "angularVelocity": -6.869670074324573e-7,
-            "velocityX": -0.7465694170405304,
-            "velocityY": -3.708539733886721,
-            "timestamp": 0.581640916591408
-        },
-        {
-            "x": 1.8234162959667812,
-            "y": 4.10590102408459,
-            "heading": -0.31503004024876186,
-            "angularVelocity": -7.165709048863888e-7,
-            "velocityX": -0.4807166210536207,
-            "velocityY": -3.7522718672898008,
-            "timestamp": 0.6139543008464863
-        },
-        {
-            "x": 1.816552246405149,
-            "y": 3.98385431066073,
-            "heading": -0.315030064931292,
-            "angularVelocity": -7.63848502164674e-7,
-            "velocityX": -0.21242125267501,
-            "velocityY": -3.776970943694307,
-            "timestamp": 0.6462676851015645
-        },
-        {
-            "x": 1.818392595454208,
-            "y": 3.861628584870307,
-            "heading": -0.315030091871416,
-            "angularVelocity": -8.337140982566696e-7,
-            "velocityX": 0.05695315088421205,
-            "velocityY": -3.782510826646546,
-            "timestamp": 0.6785810693566428
-        },
-        {
-            "x": 1.828928030254088,
-            "y": 3.739843861498511,
-            "heading": -0.3150301220653929,
-            "angularVelocity": -9.344108498281474e-7,
-            "velocityX": 0.32603935003261814,
-            "velocityY": -3.7688631562216113,
-            "timestamp": 0.710894453611721
-        },
-        {
-            "x": 1.8481051197294465,
-            "y": 3.619117921789427,
-            "heading": -0.3150301569615331,
-            "angularVelocity": -0.0000010799283629257,
-            "velocityX": 0.593472021499706,
-            "velocityY": -3.7360970536569806,
-            "timestamp": 0.7432078378667992
-        },
-        {
-            "x": 1.875826590719106,
-            "y": 3.500063178055521,
-            "heading": -0.31503019880828736,
-            "angularVelocity": -0.0000012950285239246719,
-            "velocityX": 0.8578943873792161,
-            "velocityY": -3.684378670896923,
-            "timestamp": 0.7755212221218775
-        },
-        {
-            "x": 1.9119518234377388,
-            "y": 3.3832835666467993,
-            "heading": -0.31503025136937535,
-            "angularVelocity": -0.0000016266042432729645,
-            "velocityX": 1.1179650027822445,
-            "velocityY": -3.61397031294762,
-            "timestamp": 0.8078346063769557
-        },
-        {
-            "x": 1.9562975654606443,
-            "y": 3.269371484637439,
-            "heading": -0.31503032157308203,
-            "angularVelocity": -0.0000021725891085409373,
-            "velocityX": 1.3723645184560365,
-            "velocityY": -3.5252290849559675,
-            "timestamp": 0.8401479906320339
-        },
-        {
-            "x": 2.008638858795166,
-            "y": 3.158904790878296,
-            "heading": -0.3150304332740777,
-            "angularVelocity": -0.0000034568027529519608,
-            "velocityX": 1.6198022751608165,
-            "velocityY": -3.4186049002831713,
-            "timestamp": 0.8724613748871122
-        },
-        {
-            "x": 2.0775165641117233,
-            "y": 3.0394044472847606,
-            "heading": -0.3150305189714284,
-            "angularVelocity": -0.000002350392551393865,
-            "velocityX": 1.8890857675343722,
-            "velocityY": -3.277495922091173,
-            "timestamp": 0.9089222420961518
-        },
-        {
-            "x": 2.155767726009908,
-            "y": 2.9258208622381168,
-            "heading": -0.3150305770370975,
-            "angularVelocity": -0.00000159254767063084,
-            "velocityX": 2.1461684235196876,
-            "velocityY": -3.1152189660064753,
-            "timestamp": 0.9453831093051914
-        },
-        {
-            "x": 2.242886953120307,
-            "y": 2.818887620265158,
-            "heading": -0.3150306203630764,
-            "angularVelocity": -0.0000011882871183876306,
-            "velocityX": 2.389389879591231,
-            "velocityY": -2.932822232666113,
-            "timestamp": 0.981843976514231
-        },
-        {
-            "x": 2.338311580267833,
-            "y": 2.719295349641276,
-            "heading": -0.3150306549359537,
-            "angularVelocity": -9.482187337248934e-7,
-            "velocityX": 2.617179306252704,
-            "velocityY": -2.731483868798106,
-            "timestamp": 1.0183048437232707
-        },
-        {
-            "x": 2.4414253013619946,
-            "y": 2.6276872624602983,
-            "heading": -0.31503068395926054,
-            "angularVelocity": -7.960125215618009e-7,
-            "velocityX": 2.8280655120730875,
-            "velocityY": -2.512504342142078,
-            "timestamp": 1.0547657109323103
-        },
-        {
-            "x": 2.5515621486647997,
-            "y": 2.5446549983777076,
-            "heading": -0.3150307093352344,
-            "angularVelocity": -6.959783403374032e-7,
-            "velocityX": 3.020686443670201,
-            "velocityY": -2.2772981127010774,
-            "timestamp": 1.09122657814135
-        },
-        {
-            "x": 2.6680107915990137,
-            "y": 2.4707347968336117,
-            "heading": -0.31503073229503964,
-            "angularVelocity": -6.297108928988803e-7,
-            "velocityX": 3.1937979496369038,
-            "velocityY": -2.0273846236374995,
-            "timestamp": 1.1276874453503896
-        },
-        {
-            "x": 2.7900191236562386,
-            "y": 2.4064040113359084,
-            "heading": -0.3150307537024253,
-            "angularVelocity": -5.871332010760787e-7,
-            "velocityX": 3.3462816821585726,
-            "velocityY": -1.764378919702536,
-            "timestamp": 1.1641483125594292
-        },
-        {
-            "x": 2.9167990841955884,
-            "y": 2.352077913506683,
-            "heading": -0.3150307742165802,
-            "angularVelocity": -5.626348607586924e-7,
-            "velocityX": 3.477151539278742,
-            "velocityY": -1.4899836999970275,
-            "timestamp": 1.2006091797684688
-        },
-        {
-            "x": 3.0475311021850517,
-            "y": 2.308104934680324,
-            "heading": -0.31503079438993686,
-            "angularVelocity": -5.532878997498012e-7,
-            "velocityX": 3.585543296047864,
-            "velocityY": -1.2060321707174542,
-            "timestamp": 1.2370700469775084
-        },
-        {
-            "x": 3.178817728061225,
-            "y": 2.2658165761507623,
-            "heading": -0.3150308145386853,
-            "angularVelocity": -5.526129783451472e-7,
-            "velocityX": 3.600754340906743,
-            "velocityY": -1.159828653748452,
-            "timestamp": 1.273530914186548
-        },
-        {
-            "x": 3.3101043979960063,
-            "y": 2.2235283544039026,
-            "heading": -0.3150308346874411,
-            "angularVelocity": -5.526131795163358e-7,
-            "velocityX": 3.600755549287413,
-            "velocityY": -1.1598249022550946,
-            "timestamp": 1.3099917813955877
-        },
-        {
-            "x": 3.4413910762638236,
-            "y": 2.1812401585275256,
-            "heading": -0.31503085483620696,
-            "angularVelocity": -5.52613457009699e-7,
-            "velocityX": 3.60075577783481,
-            "velocityY": -1.159824192714005,
-            "timestamp": 1.3464526486046273
-        },
-        {
-            "x": 3.572677762945226,
-            "y": 2.138951988771722,
-            "heading": -0.31503087498498206,
-            "angularVelocity": -5.52613710159953e-7,
-            "velocityX": 3.6007560085914068,
-            "velocityY": -1.1598234763137998,
-            "timestamp": 1.382913515813667
-        },
-        {
-            "x": 3.7039644581047573,
-            "y": 2.0966638453368867,
-            "heading": -0.31503089513376514,
-            "angularVelocity": -5.526139295643368e-7,
-            "velocityX": 3.6007562411181984,
-            "velocityY": -1.1598227544173942,
-            "timestamp": 1.4193743830227066
-        },
-        {
-            "x": 3.8352511618245644,
-            "y": 2.05437572847807,
-            "heading": -0.3150309152825546,
-            "angularVelocity": -5.526141044747151e-7,
-            "velocityX": 3.600756475898009,
-            "velocityY": -1.1598220255258234,
-            "timestamp": 1.4558352502317462
-        },
-        {
-            "x": 3.966537874250263,
-            "y": 2.01208763864737,
-            "heading": -0.3150309354313486,
-            "angularVelocity": -5.526142273042023e-7,
-            "velocityX": 3.6007567146716046,
-            "velocityY": -1.159821284234734,
-            "timestamp": 1.4922961174407858
-        },
-        {
-            "x": 4.097824595645338,
-            "y": 1.9697995766628096,
-            "heading": -0.3150309555801447,
-            "angularVelocity": -5.526142864672618e-7,
-            "velocityX": 3.600756960671674,
-            "velocityY": -1.159820520507965,
-            "timestamp": 1.5287569846498255
-        },
-        {
-            "x": 4.229111326453571,
-            "y": 1.9275115439021684,
-            "heading": -0.3150309757289402,
-            "angularVelocity": -5.5261426880692e-7,
-            "velocityX": 3.600757218843208,
-            "velocityY": -1.1598197189933317,
-            "timestamp": 1.565217851858865
-        },
-        {
-            "x": 4.360398019008411,
-            "y": 1.885223392382287,
-            "heading": -0.31503099587881916,
-            "angularVelocity": -5.52643987126892e-7,
-            "velocityX": 3.6007561696802117,
-            "velocityY": -1.1598229761632481,
-            "timestamp": 1.6016787190679047
-        },
-        {
-            "x": 4.485161526388635,
-            "y": 1.844329974236374,
-            "heading": -0.3370638509456068,
-            "angularVelocity": -0.6042877406197901,
-            "velocityX": 3.4218469534726492,
-            "velocityY": -1.1215700907896822,
-            "timestamp": 1.6381395862769443
-        },
-        {
-            "x": 4.60044458089581,
-            "y": 1.8067761222092775,
-            "heading": -0.3770388194107599,
-            "angularVelocity": -1.0963800788381923,
-            "velocityX": 3.1618297460185825,
-            "velocityY": -1.0299769287381584,
-            "timestamp": 1.674600453485984
-        },
-        {
-            "x": 4.705219804759057,
-            "y": 1.772711102552083,
-            "heading": -0.4189139196427851,
-            "angularVelocity": -1.148494356756437,
-            "velocityX": 2.8736349923479074,
-            "velocityY": -0.9342898911836356,
-            "timestamp": 1.7110613206950236
-        },
-        {
-            "x": 4.799468970200479,
-            "y": 1.7421012617230478,
-            "heading": -0.45948472427125614,
-            "angularVelocity": -1.1127218778387968,
-            "velocityX": 2.5849403115144614,
-            "velocityY": -0.8395258580532651,
-            "timestamp": 1.7475221879040632
-        },
-        {
-            "x": 4.883206648654635,
-            "y": 1.7149242324789875,
-            "heading": -0.49729979894596704,
-            "angularVelocity": -1.037141394852447,
-            "velocityX": 2.296645276538986,
-            "velocityY": -0.7453752838144925,
-            "timestamp": 1.7839830551131028
-        },
-        {
-            "x": 4.956447680828415,
-            "y": 1.6911655730288653,
-            "heading": -0.5315269969459382,
-            "angularVelocity": -0.9387379023033031,
-            "velocityX": 2.0087572726635705,
-            "velocityY": -0.6516207997442213,
-            "timestamp": 1.8204439223221425
-        },
-        {
-            "x": 5.019204214404415,
-            "y": 1.6708154773305386,
-            "heading": -0.5616288277349688,
-            "angularVelocity": -0.825592836737831,
-            "velocityX": 1.7212024392124468,
-            "velocityY": -0.5581352627093101,
-            "timestamp": 1.856904789531182
-        },
-        {
-            "x": 5.071485921099441,
-            "y": 1.6538669506708767,
-            "heading": -0.587231204588931,
-            "angularVelocity": -0.7021878198118158,
-            "velocityX": 1.4339128687006073,
-            "velocityY": -0.46484156733002446,
-            "timestamp": 1.8933656567402217
-        },
-        {
-            "x": 5.11330055140079,
-            "y": 1.6403147844077772,
-            "heading": -0.6080594902506735,
-            "angularVelocity": -0.5712504187662296,
-            "velocityX": 1.1468358682094448,
-            "velocityY": -0.37169072763413913,
-            "timestamp": 1.9298265239492614
-        },
-        {
-            "x": 5.1446544158470235,
-            "y": 1.6301549523614358,
-            "heading": -0.6239033665029361,
-            "angularVelocity": -0.43454469037794713,
-            "velocityX": 0.859931944747064,
-            "velocityY": -0.2786503126240163,
-            "timestamp": 1.966287391158301
-        },
-        {
-            "x": 5.165552752490355,
-            "y": 1.6233842390024875,
-            "heading": -0.6345958555798376,
-            "angularVelocity": -0.29325931869912014,
-            "velocityX": 0.5731716835893133,
-            "velocityY": -0.18569808886140712,
-            "timestamp": 2.0027482583673404
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -0.14821766002380998,
-            "velocityX": 0.2865331603263514,
-            "velocityY": -0.09281839027812194,
-            "timestamp": 2.03920912557638
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 2.046404654845657e-31,
-            "velocityX": 2.793064044552828e-33,
-            "velocityY": 5.221456528298987e-32,
-            "timestamp": 2.0756699927854196
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj
deleted file mode 100644
index 35bba061..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.1.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -5.451838641793523e-34,
-            "angularVelocity": -2.0131697388730357e-33,
-            "velocityX": -3.0924959567962797e-34,
-            "velocityY": 4.789643826321005e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2520020156777094,
-            "y": 5.6078780285533805,
-            "heading": 0.01582108665485387,
-            "angularVelocity": 0.208254594271014,
-            "velocityX": 0.026352738707599337,
-            "velocityY": 0.630223424496655,
-            "timestamp": 0.07596992858781765
-        },
-        {
-            "x": 2.2560055471462697,
-            "y": 5.703634194455967,
-            "heading": 0.04745824180691435,
-            "angularVelocity": 0.4164431340157103,
-            "velocityX": 0.05269889735295001,
-            "velocityY": 1.2604482810839739,
-            "timestamp": 0.1519398571756353
-        },
-        {
-            "x": 2.262009891384212,
-            "y": 5.847268762594484,
-            "heading": 0.09489870943903907,
-            "angularVelocity": 0.6244637649920359,
-            "velocityX": 0.07903580205425154,
-            "velocityY": 1.8906766244025437,
-            "timestamp": 0.22790978576345294
-        },
-        {
-            "x": 2.2700142224347877,
-            "y": 6.038782263911744,
-            "heading": 0.1581161098401003,
-            "angularVelocity": 0.8321371571119077,
-            "velocityX": 0.1053618346017344,
-            "velocityY": 2.5209119565760743,
-            "timestamp": 0.3038797143512706
-        },
-        {
-            "x": 2.280009491675263,
-            "y": 6.278187226323642,
-            "heading": 0.23645965423367718,
-            "angularVelocity": 1.0312441494928544,
-            "velocityX": 0.13156875919557368,
-            "velocityY": 3.151312194997739,
-            "timestamp": 0.37984964293908824
-        },
-        {
-            "x": 2.288005657404111,
-            "y": 6.469712008115255,
-            "heading": 0.2990934539478391,
-            "angularVelocity": 0.8244551611202343,
-            "velocityX": 0.10525435363026732,
-            "velocityY": 2.52106044262263,
-            "timestamp": 0.4558195715269059
-        },
-        {
-            "x": 2.294002815862431,
-            "y": 6.613355952122642,
-            "heading": 0.3460499471183657,
-            "angularVelocity": 0.6180931592721867,
-            "velocityX": 0.07894121489645621,
-            "velocityY": 1.8908000399308182,
-            "timestamp": 0.5317895001147235
-        },
-        {
-            "x": 2.2980009414583966,
-            "y": 6.709118669367686,
-            "heading": 0.3773492268804029,
-            "angularVelocity": 0.4119956454330042,
-            "velocityX": 0.05262773929482027,
-            "velocityY": 1.260534517079955,
-            "timestamp": 0.6077594287025412
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.20601273965271094,
-            "velocityX": 0.026313813620245378,
-            "velocityY": 0.6302668901019914,
-            "timestamp": 0.6837293572903589
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 9.82651204832558e-34,
-            "velocityX": 0,
-            "velocityY": 3.8258931367738186e-36,
-            "timestamp": 0.7596992858781766
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.traj
deleted file mode 100644
index 35bba061..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -5.451838641793523e-34,
-            "angularVelocity": -2.0131697388730357e-33,
-            "velocityX": -3.0924959567962797e-34,
-            "velocityY": 4.789643826321005e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2520020156777094,
-            "y": 5.6078780285533805,
-            "heading": 0.01582108665485387,
-            "angularVelocity": 0.208254594271014,
-            "velocityX": 0.026352738707599337,
-            "velocityY": 0.630223424496655,
-            "timestamp": 0.07596992858781765
-        },
-        {
-            "x": 2.2560055471462697,
-            "y": 5.703634194455967,
-            "heading": 0.04745824180691435,
-            "angularVelocity": 0.4164431340157103,
-            "velocityX": 0.05269889735295001,
-            "velocityY": 1.2604482810839739,
-            "timestamp": 0.1519398571756353
-        },
-        {
-            "x": 2.262009891384212,
-            "y": 5.847268762594484,
-            "heading": 0.09489870943903907,
-            "angularVelocity": 0.6244637649920359,
-            "velocityX": 0.07903580205425154,
-            "velocityY": 1.8906766244025437,
-            "timestamp": 0.22790978576345294
-        },
-        {
-            "x": 2.2700142224347877,
-            "y": 6.038782263911744,
-            "heading": 0.1581161098401003,
-            "angularVelocity": 0.8321371571119077,
-            "velocityX": 0.1053618346017344,
-            "velocityY": 2.5209119565760743,
-            "timestamp": 0.3038797143512706
-        },
-        {
-            "x": 2.280009491675263,
-            "y": 6.278187226323642,
-            "heading": 0.23645965423367718,
-            "angularVelocity": 1.0312441494928544,
-            "velocityX": 0.13156875919557368,
-            "velocityY": 3.151312194997739,
-            "timestamp": 0.37984964293908824
-        },
-        {
-            "x": 2.288005657404111,
-            "y": 6.469712008115255,
-            "heading": 0.2990934539478391,
-            "angularVelocity": 0.8244551611202343,
-            "velocityX": 0.10525435363026732,
-            "velocityY": 2.52106044262263,
-            "timestamp": 0.4558195715269059
-        },
-        {
-            "x": 2.294002815862431,
-            "y": 6.613355952122642,
-            "heading": 0.3460499471183657,
-            "angularVelocity": 0.6180931592721867,
-            "velocityX": 0.07894121489645621,
-            "velocityY": 1.8908000399308182,
-            "timestamp": 0.5317895001147235
-        },
-        {
-            "x": 2.2980009414583966,
-            "y": 6.709118669367686,
-            "heading": 0.3773492268804029,
-            "angularVelocity": 0.4119956454330042,
-            "velocityX": 0.05262773929482027,
-            "velocityY": 1.260534517079955,
-            "timestamp": 0.6077594287025412
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.20601273965271094,
-            "velocityX": 0.026313813620245378,
-            "velocityY": 0.6302668901019914,
-            "timestamp": 0.6837293572903589
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 9.82651204832558e-34,
-            "velocityX": 0,
-            "velocityY": 3.8258931367738186e-36,
-            "timestamp": 0.7596992858781766
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj
deleted file mode 100644
index f46e6706..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.1.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 5.7052362935559275e-34,
-            "velocityX": 0,
-            "velocityY": -1.2494033787640354e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2520019447146944,
-            "y": 5.512001877264397,
-            "heading": -0.01569721301150974,
-            "angularVelocity": -0.20637467835572687,
-            "velocityX": 0.02632000319280478,
-            "velocityY": -0.6310417737196751,
-            "timestamp": 0.07606172005488261
-        },
-        {
-            "x": 2.2560053559202022,
-            "y": 5.416005527882177,
-            "heading": -0.04708679827781808,
-            "angularVelocity": -0.41268571422864314,
-            "velocityX": 0.05263371907208186,
-            "velocityY": -1.262084913580087,
-            "timestamp": 0.15212344010976522
-        },
-        {
-            "x": 2.262009560249172,
-            "y": 5.272010699235591,
-            "heading": -0.0941564711571504,
-            "angularVelocity": -0.6188352412405218,
-            "velocityX": 0.07893858204412474,
-            "velocityY": -1.8931313746610776,
-            "timestamp": 0.22818516016464785
-        },
-        {
-            "x": 2.2700137636187,
-            "y": 5.080016884298785,
-            "heading": -0.15688079244309955,
-            "angularVelocity": -0.8246503134650409,
-            "velocityX": 0.10523300503528431,
-            "velocityY": -2.5241845017213995,
-            "timestamp": 0.30424688021953045
-        },
-        {
-            "x": 2.2800092014745617,
-            "y": 4.840012168782865,
-            "heading": -0.23463455266722952,
-            "angularVelocity": -1.0222456206358,
-            "velocityX": 0.13141219852311892,
-            "velocityY": -3.155394268532757,
-            "timestamp": 0.38030860027441304
-        },
-        {
-            "x": 2.2880054914438257,
-            "y": 4.648007616430203,
-            "heading": -0.2967978427641432,
-            "angularVelocity": -0.8172743142287549,
-            "velocityX": 0.10512896583845424,
-            "velocityY": -2.524325668866294,
-            "timestamp": 0.45637032032929564
-        },
-        {
-            "x": 2.2940027351611283,
-            "y": 4.5040038585170965,
-            "heading": -0.3434020516948695,
-            "angularVelocity": -0.6127156853289512,
-            "velocityX": 0.07884706936650254,
-            "velocityY": -1.8932487696728393,
-            "timestamp": 0.5324320403841782
-        },
-        {
-            "x": 2.2980009144821483,
-            "y": 4.408001268437703,
-            "heading": -0.37446662411182524,
-            "angularVelocity": -0.40841269950956927,
-            "velocityX": 0.05256493434720428,
-            "velocityY": -1.2621669613850792,
-            "timestamp": 0.6084937604390609
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.20422067601109492,
-            "velocityX": 0.026282412709165117,
-            "velocityY": -0.6310831309503169,
-            "timestamp": 0.6845554804939435
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 8.777318885868316e-35,
-            "velocityX": -1.1404320857258046e-34,
-            "velocityY": 1.134511753128188e-36,
-            "timestamp": 0.7606172005488262
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.traj
deleted file mode 100644
index f46e6706..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 5.7052362935559275e-34,
-            "velocityX": 0,
-            "velocityY": -1.2494033787640354e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2520019447146944,
-            "y": 5.512001877264397,
-            "heading": -0.01569721301150974,
-            "angularVelocity": -0.20637467835572687,
-            "velocityX": 0.02632000319280478,
-            "velocityY": -0.6310417737196751,
-            "timestamp": 0.07606172005488261
-        },
-        {
-            "x": 2.2560053559202022,
-            "y": 5.416005527882177,
-            "heading": -0.04708679827781808,
-            "angularVelocity": -0.41268571422864314,
-            "velocityX": 0.05263371907208186,
-            "velocityY": -1.262084913580087,
-            "timestamp": 0.15212344010976522
-        },
-        {
-            "x": 2.262009560249172,
-            "y": 5.272010699235591,
-            "heading": -0.0941564711571504,
-            "angularVelocity": -0.6188352412405218,
-            "velocityX": 0.07893858204412474,
-            "velocityY": -1.8931313746610776,
-            "timestamp": 0.22818516016464785
-        },
-        {
-            "x": 2.2700137636187,
-            "y": 5.080016884298785,
-            "heading": -0.15688079244309955,
-            "angularVelocity": -0.8246503134650409,
-            "velocityX": 0.10523300503528431,
-            "velocityY": -2.5241845017213995,
-            "timestamp": 0.30424688021953045
-        },
-        {
-            "x": 2.2800092014745617,
-            "y": 4.840012168782865,
-            "heading": -0.23463455266722952,
-            "angularVelocity": -1.0222456206358,
-            "velocityX": 0.13141219852311892,
-            "velocityY": -3.155394268532757,
-            "timestamp": 0.38030860027441304
-        },
-        {
-            "x": 2.2880054914438257,
-            "y": 4.648007616430203,
-            "heading": -0.2967978427641432,
-            "angularVelocity": -0.8172743142287549,
-            "velocityX": 0.10512896583845424,
-            "velocityY": -2.524325668866294,
-            "timestamp": 0.45637032032929564
-        },
-        {
-            "x": 2.2940027351611283,
-            "y": 4.5040038585170965,
-            "heading": -0.3434020516948695,
-            "angularVelocity": -0.6127156853289512,
-            "velocityX": 0.07884706936650254,
-            "velocityY": -1.8932487696728393,
-            "timestamp": 0.5324320403841782
-        },
-        {
-            "x": 2.2980009144821483,
-            "y": 4.408001268437703,
-            "heading": -0.37446662411182524,
-            "angularVelocity": -0.40841269950956927,
-            "velocityX": 0.05256493434720428,
-            "velocityY": -1.2621669613850792,
-            "timestamp": 0.6084937604390609
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.20422067601109492,
-            "velocityX": 0.026282412709165117,
-            "velocityY": -0.6310831309503169,
-            "timestamp": 0.6845554804939435
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 8.777318885868316e-35,
-            "velocityX": -1.1404320857258046e-34,
-            "velocityY": 1.134511753128188e-36,
-            "timestamp": 0.7606172005488262
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.1.traj
deleted file mode 100644
index 8dba26f9..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.1.traj	
+++ /dev/null
@@ -1,634 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.2999999999310377,
-            "y": 4.359999999993798,
-            "heading": -0.3900000000006091,
-            "angularVelocity": -1.7156392127364986e-12,
-            "velocityX": -4.6004235672195495e-11,
-            "velocityY": -2.541589729484622e-10,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2830691122580777,
-            "y": 4.4226688824706715,
-            "heading": -0.3961360368292833,
-            "angularVelocity": -0.04017087715885992,
-            "velocityX": -0.11084167601962927,
-            "velocityY": 0.4102752396150543,
-            "timestamp": 0.1527483904267538
-        },
-        {
-            "x": 2.2499539829007165,
-            "y": 4.551962677072163,
-            "heading": -0.39368124618911415,
-            "angularVelocity": 0.016070811835100392,
-            "velocityX": -0.21679527487321065,
-            "velocityY": 0.8464494731937718,
-            "timestamp": 0.3054967808535076
-        },
-        {
-            "x": 2.208537052758008,
-            "y": 4.723131432963358,
-            "heading": -0.3887340677228252,
-            "angularVelocity": 0.032387761683442845,
-            "velocityX": -0.27114478888671906,
-            "velocityY": 1.120592861369484,
-            "timestamp": 0.45824517128026143
-        },
-        {
-            "x": 2.1635048676051025,
-            "y": 4.925567881437362,
-            "heading": -0.37513416937632715,
-            "angularVelocity": 0.08903464258184278,
-            "velocityX": -0.294812828865451,
-            "velocityY": 1.3252934968125494,
-            "timestamp": 0.6109935617070152
-        },
-        {
-            "x": 2.119479495251874,
-            "y": 5.143090740041083,
-            "heading": -0.3554033967273168,
-            "angularVelocity": 0.12917172215836753,
-            "velocityX": -0.28822151359768483,
-            "velocityY": 1.4240599066467436,
-            "timestamp": 0.763741952133769
-        },
-        {
-            "x": 2.08196432941898,
-            "y": 5.365761039079461,
-            "heading": -0.32917795560380303,
-            "angularVelocity": 0.1716904580681245,
-            "velocityX": -0.24560105455579473,
-            "velocityY": 1.4577587261323537,
-            "timestamp": 0.9164903425605229
-        },
-        {
-            "x": 2.052346293594587,
-            "y": 5.582587972754811,
-            "heading": -0.29825458565802576,
-            "angularVelocity": 0.20244645364142877,
-            "velocityX": -0.19390080434302895,
-            "velocityY": 1.419503885313628,
-            "timestamp": 1.0692387329872766
-        },
-        {
-            "x": 2.030803808580604,
-            "y": 5.789012383645482,
-            "heading": -0.2654877856320327,
-            "angularVelocity": 0.2145148628152058,
-            "velocityX": -0.14103248420843936,
-            "velocityY": 1.3514015455604398,
-            "timestamp": 1.2219871234140305
-        },
-        {
-            "x": 2.0185215947450232,
-            "y": 5.980748966625485,
-            "heading": -0.23040407839794677,
-            "angularVelocity": 0.2296829912414591,
-            "velocityX": -0.08040813886408783,
-            "velocityY": 1.2552445394842306,
-            "timestamp": 1.3747355138407844
-        },
-        {
-            "x": 2.017537204929899,
-            "y": 6.152494473346335,
-            "heading": -0.19497588875865757,
-            "angularVelocity": 0.23193821893113326,
-            "velocityX": -0.006444518152378274,
-            "velocityY": 1.1243686841111533,
-            "timestamp": 1.5274839042675383
-        },
-        {
-            "x": 2.0415938446669073,
-            "y": 6.276321571691329,
-            "heading": -0.16155868009406138,
-            "angularVelocity": 0.21877290194991583,
-            "velocityX": 0.15749193627933206,
-            "velocityY": 0.8106605773582652,
-            "timestamp": 1.6802322946942922
-        },
-        {
-            "x": 2.1241281032079797,
-            "y": 6.2825961111606565,
-            "heading": -0.060349903928264906,
-            "angularVelocity": 0.6625848944275851,
-            "velocityX": 0.5403281719922621,
-            "velocityY": 0.04107761453342325,
-            "timestamp": 1.8329806851210462
-        },
-        {
-            "x": 2.1855581068088026,
-            "y": 6.287266231959223,
-            "heading": 0.012078838000808553,
-            "angularVelocity": 0.7458652653001706,
-            "velocityX": 0.6326011577496066,
-            "velocityY": 0.048092524107779686,
-            "timestamp": 1.9300876899626762
-        },
-        {
-            "x": 2.2551653620091354,
-            "y": 6.2925580157259535,
-            "heading": 0.08826667660975476,
-            "angularVelocity": 0.784576135772582,
-            "velocityX": 0.7168098264960764,
-            "velocityY": 0.05449435852189788,
-            "timestamp": 2.0271946948043063
-        },
-        {
-            "x": 2.3319122711008053,
-            "y": 6.29839258064769,
-            "heading": 0.16657489622034788,
-            "angularVelocity": 0.8064116460755234,
-            "velocityX": 0.7903333988427459,
-            "velocityY": 0.06008387443491743,
-            "timestamp": 2.1243016996459363
-        },
-        {
-            "x": 2.4149946395663093,
-            "y": 6.304708789079692,
-            "heading": 0.2457212902089042,
-            "angularVelocity": 0.8150430973252747,
-            "velocityX": 0.8555754417689245,
-            "velocityY": 0.06504379984951533,
-            "timestamp": 2.2214087044875663
-        },
-        {
-            "x": 2.5038373372195277,
-            "y": 6.311462917641332,
-            "heading": 0.32495163133637983,
-            "angularVelocity": 0.8159075780845946,
-            "velocityX": 0.9148948410382448,
-            "velocityY": 0.06955346543875748,
-            "timestamp": 2.3185157093291964
-        },
-        {
-            "x": 2.598073257877524,
-            "y": 6.3186270576568155,
-            "heading": 0.40431668105413066,
-            "angularVelocity": 0.8172947961305416,
-            "velocityX": 0.9704338108518987,
-            "velocityY": 0.0737757297255109,
-            "timestamp": 2.4156227141708264
-        },
-        {
-            "x": 2.697382502344975,
-            "y": 6.326176889286233,
-            "heading": 0.48416024389571777,
-            "angularVelocity": 0.8222224852016302,
-            "velocityX": 1.0226784849096606,
-            "velocityY": 0.07774755027256483,
-            "timestamp": 2.5127297190124565
-        },
-        {
-            "x": 2.8014852531337993,
-            "y": 6.334091139837385,
-            "heading": 0.5646614354219063,
-            "angularVelocity": 0.8289946917215433,
-            "velocityX": 1.0720416213516444,
-            "velocityY": 0.08150030637886811,
-            "timestamp": 2.6098367238540865
-        },
-        {
-            "x": 2.910148663901152,
-            "y": 6.342352107524804,
-            "heading": 0.6457203378655492,
-            "angularVelocity": 0.8347379532340086,
-            "velocityX": 1.119006925166821,
-            "velocityY": 0.0850707709678732,
-            "timestamp": 2.7069437286957165
-        },
-        {
-            "x": 3.0231702594110863,
-            "y": 6.350944399479571,
-            "heading": 0.7270565797219191,
-            "angularVelocity": 0.8375939718950361,
-            "velocityX": 1.1638871555335075,
-            "velocityY": 0.08848272107248593,
-            "timestamp": 2.8040507335373466
-        },
-        {
-            "x": 3.140351376164449,
-            "y": 6.359852912614292,
-            "heading": 0.8083350217640537,
-            "angularVelocity": 0.83699875417797,
-            "velocityX": 1.2067215654083316,
-            "velocityY": 0.09173914083795442,
-            "timestamp": 2.9011577383789766
-        },
-        {
-            "x": 3.261472243862432,
-            "y": 6.3690609392298585,
-            "heading": 0.8891526159463483,
-            "angularVelocity": 0.8322529806195065,
-            "velocityX": 1.2472927986960072,
-            "velocityY": 0.09482350610318373,
-            "timestamp": 2.9982647432206067
-        },
-        {
-            "x": 3.386280226554517,
-            "y": 6.378549273066906,
-            "heading": 0.9690024828336246,
-            "angularVelocity": 0.8222874040565998,
-            "velocityX": 1.2852624076036416,
-            "velocityY": 0.09771008689812195,
-            "timestamp": 3.0953717480622367
-        },
-        {
-            "x": 3.5144942146424594,
-            "y": 6.388296543201246,
-            "heading": 1.0472679478096496,
-            "angularVelocity": 0.8059713626886263,
-            "velocityX": 1.3203371728362745,
-            "velocityY": 0.10037659168026988,
-            "timestamp": 3.1924787529038667
-        },
-        {
-            "x": 3.64579126909874,
-            "y": 6.398278198691388,
-            "heading": 1.1233630936332748,
-            "angularVelocity": 0.783621593451087,
-            "velocityX": 1.3520863383515835,
-            "velocityY": 0.10279027288912505,
-            "timestamp": 3.2895857577454968
-        },
-        {
-            "x": 3.779855631903331,
-            "y": 6.408470234520327,
-            "heading": 1.1966377830577097,
-            "angularVelocity": 0.7545767639735752,
-            "velocityX": 1.3805838535787394,
-            "velocityY": 0.10495675240615632,
-            "timestamp": 3.386692762587127
-        },
-        {
-            "x": 3.916399996401096,
-            "y": 6.418850808641588,
-            "heading": 1.2663868507253695,
-            "angularVelocity": 0.7182701992663435,
-            "velocityX": 1.406122708347489,
-            "velocityY": 0.10689830431744617,
-            "timestamp": 3.483799767428757
-        },
-        {
-            "x": 4.055149110841729,
-            "y": 6.429398995480507,
-            "heading": 1.3319162676094702,
-            "angularVelocity": 0.6748165803464835,
-            "velocityX": 1.4288270430729209,
-            "velocityY": 0.10862436634220823,
-            "timestamp": 3.580906772270387
-        },
-        {
-            "x": 4.195643021249515,
-            "y": 6.440079827739076,
-            "heading": 1.392465239071673,
-            "angularVelocity": 0.6235283604234658,
-            "velocityX": 1.4467948093350342,
-            "velocityY": 0.10999033795341527,
-            "timestamp": 3.678013777112017
-        },
-        {
-            "x": 4.337478451024624,
-            "y": 6.45086264692987,
-            "heading": 1.4461855651355748,
-            "angularVelocity": 0.5532075282023133,
-            "velocityX": 1.4606096661273347,
-            "velocityY": 0.11104059096144768,
-            "timestamp": 3.775120781953647
-        },
-        {
-            "x": 4.47965775917095,
-            "y": 6.461671608979337,
-            "heading": 1.49351259060778,
-            "angularVelocity": 0.48736984118650195,
-            "velocityX": 1.4641508974652757,
-            "velocityY": 0.11130980792840979,
-            "timestamp": 3.872227786795277
-        },
-        {
-            "x": 4.621471854948602,
-            "y": 6.4724528062914635,
-            "heading": 1.5350394838354429,
-            "angularVelocity": 0.42764055328666206,
-            "velocityX": 1.4603899702927075,
-            "velocityY": 0.11102388888102613,
-            "timestamp": 3.969334791636907
-        },
-        {
-            "x": 4.762134624452166,
-            "y": 6.483146475819268,
-            "heading": 1.5711187532761213,
-            "angularVelocity": 0.37154136862160403,
-            "velocityX": 1.4485337063274482,
-            "velocityY": 0.11012253482900584,
-            "timestamp": 4.0664417964785375
-        },
-        {
-            "x": 4.901310758087736,
-            "y": 6.493727126167039,
-            "heading": 1.6012523506615222,
-            "angularVelocity": 0.3103133235343918,
-            "velocityX": 1.433224451514287,
-            "velocityY": 0.10895867241503827,
-            "timestamp": 4.163548801320168
-        },
-        {
-            "x": 5.0385652541984065,
-            "y": 6.504161687000508,
-            "heading": 1.6187441808494436,
-            "angularVelocity": 0.18012943788636607,
-            "velocityX": 1.413435585164251,
-            "velocityY": 0.1074542543793728,
-            "timestamp": 4.2606558061617985
-        },
-        {
-            "x": 5.1726571421935885,
-            "y": 6.5143558154807435,
-            "heading": 1.6310181673546418,
-            "angularVelocity": 0.12639650992409435,
-            "velocityX": 1.3808673050498854,
-            "velocityY": 0.104978301436634,
-            "timestamp": 4.357762811003429
-        },
-        {
-            "x": 5.303636473288953,
-            "y": 6.524313316659959,
-            "heading": 1.6376756877148975,
-            "angularVelocity": 0.06855860046187665,
-            "velocityX": 1.3488144481980282,
-            "velocityY": 0.1025415325257505,
-            "timestamp": 4.4548698158450595
-        },
-        {
-            "x": 5.4313983902221485,
-            "y": 6.53402621894025,
-            "heading": 1.6386463375950402,
-            "angularVelocity": 0.009995672907119661,
-            "velocityX": 1.3156817797402116,
-            "velocityY": 0.1000226726538996,
-            "timestamp": 4.55197682068669
-        },
-        {
-            "x": 5.555758593734715,
-            "y": 6.5434805112143435,
-            "heading": 1.6339151298589827,
-            "angularVelocity": -0.048721590949420676,
-            "velocityX": 1.2806512124922533,
-            "velocityY": 0.09735952745677612,
-            "timestamp": 4.6490838255283204
-        },
-        {
-            "x": 5.676522731590665,
-            "y": 6.552661418256687,
-            "heading": 1.6211253547169766,
-            "angularVelocity": -0.13170805962068485,
-            "velocityX": 1.2436192217305724,
-            "velocityY": 0.0945442281350229,
-            "timestamp": 4.746190830369951
-        },
-        {
-            "x": 5.8040475153273645,
-            "y": 6.568758338060969,
-            "heading": 1.594817916740329,
-            "angularVelocity": -0.24917360707174604,
-            "velocityX": 1.2078641151592902,
-            "velocityY": 0.15246363592342288,
-            "timestamp": 4.851769580629666
-        },
-        {
-            "x": 5.9283875752902055,
-            "y": 6.58985033542802,
-            "heading": 1.559498986776326,
-            "angularVelocity": -0.33452688097090233,
-            "velocityX": 1.1776996757735678,
-            "velocityY": 0.19977502680380196,
-            "timestamp": 4.957348330889381
-        },
-        {
-            "x": 6.049721376535089,
-            "y": 6.61532142807792,
-            "heading": 1.5174190958344496,
-            "angularVelocity": -0.3985640187289682,
-            "velocityX": 1.1492255865917638,
-            "velocityY": 0.2412520781772084,
-            "timestamp": 5.0629270811490965
-        },
-        {
-            "x": 6.167915961542227,
-            "y": 6.644893212673861,
-            "heading": 1.4694436889435494,
-            "angularVelocity": -0.45440400479138554,
-            "velocityX": 1.119492176613303,
-            "velocityY": 0.280092203729064,
-            "timestamp": 5.168505831408812
-        },
-        {
-            "x": 6.2827599678090085,
-            "y": 6.678289224223258,
-            "heading": 1.4161083022197893,
-            "angularVelocity": -0.5051716053929952,
-            "velocityX": 1.087756824956101,
-            "velocityY": 0.31631376267017064,
-            "timestamp": 5.274084581668527
-        },
-        {
-            "x": 6.394018817655917,
-            "y": 6.715173356117064,
-            "heading": 1.3577889107275223,
-            "angularVelocity": -0.5523781194839231,
-            "velocityX": 1.0537996466964168,
-            "velocityY": 0.34935185331081736,
-            "timestamp": 5.379663331928242
-        },
-        {
-            "x": 6.501464569355021,
-            "y": 6.755149800954155,
-            "heading": 1.2948750452839908,
-            "angularVelocity": -0.5958951523626541,
-            "velocityX": 1.0176834949234126,
-            "velocityY": 0.3786410134453019,
-            "timestamp": 5.485242082187957
-        },
-        {
-            "x": 6.6048776092971675,
-            "y": 6.797770420027359,
-            "heading": 1.22768881668393,
-            "angularVelocity": -0.6363612797096425,
-            "velocityX": 0.9794872502530397,
-            "velocityY": 0.4036855817031342,
-            "timestamp": 5.5908208324476725
-        },
-        {
-            "x": 6.70404339771049,
-            "y": 6.8425429323292,
-            "heading": 1.1564439654233045,
-            "angularVelocity": -0.6748029419532755,
-            "velocityX": 0.939258970068964,
-            "velocityY": 0.4240674597490353,
-            "timestamp": 5.696399582707388
-        },
-        {
-            "x": 6.79877778418725,
-            "y": 6.888932915699562,
-            "heading": 1.0813771004184407,
-            "angularVelocity": -0.7110035384939538,
-            "velocityX": 0.8972864903430491,
-            "velocityY": 0.4393875033009405,
-            "timestamp": 5.801978332967103
-        },
-        {
-            "x": 6.88894352921239,
-            "y": 6.936373258053616,
-            "heading": 1.0028508284852347,
-            "angularVelocity": -0.7437696672526416,
-            "velocityX": 0.8540141324253476,
-            "velocityY": 0.44933608637180433,
-            "timestamp": 5.907557083226818
-        },
-        {
-            "x": 6.974457896759412,
-            "y": 6.984282785155121,
-            "heading": 0.9213875618911144,
-            "angularVelocity": -0.7715877144447874,
-            "velocityX": 0.809958131723675,
-            "velocityY": 0.45378001785130806,
-            "timestamp": 6.013135833486533
-        },
-        {
-            "x": 7.055294443748765,
-            "y": 7.032089281864471,
-            "heading": 0.8376815844807217,
-            "angularVelocity": -0.792829780737162,
-            "velocityX": 0.7656516726874414,
-            "velocityY": 0.45280415475213037,
-            "timestamp": 6.118714583746248
-        },
-        {
-            "x": 7.131475603500295,
-            "y": 7.0792526481673885,
-            "heading": 0.752591160441856,
-            "angularVelocity": -0.805942709434023,
-            "velocityX": 0.7215576931722669,
-            "velocityY": 0.44671267879627613,
-            "timestamp": 6.224293334005964
-        },
-        {
-            "x": 7.203056823429108,
-            "y": 7.125284419454981,
-            "heading": 0.6670903070961578,
-            "angularVelocity": -0.8098301328249344,
-            "velocityX": 0.6779888895797812,
-            "velocityY": 0.4359946600433992,
-            "timestamp": 6.329872084265679
-        },
-        {
-            "x": 7.270104931236086,
-            "y": 7.169760877903185,
-            "heading": 0.5821807537401323,
-            "angularVelocity": -0.8042295741051197,
-            "velocityX": 0.6350530514787667,
-            "velocityY": 0.4212633543513555,
-            "timestamp": 6.435450834525394
-        },
-        {
-            "x": 7.332675465601759,
-            "y": 7.212330496694183,
-            "heading": 0.49884759114131233,
-            "angularVelocity": -0.789298626772507,
-            "velocityX": 0.592643253784208,
-            "velocityY": 0.4032025262817465,
-            "timestamp": 6.541029584785109
-        },
-        {
-            "x": 7.390806680523043,
-            "y": 7.252712685445242,
-            "heading": 0.4183197118253645,
-            "angularVelocity": -0.7627280973825078,
-            "velocityX": 0.5505957828143127,
-            "velocityY": 0.38248405756315146,
-            "timestamp": 6.646608335044824
-        },
-        {
-            "x": 7.444527736090263,
-            "y": 7.290684851015479,
-            "heading": 0.34198396458440167,
-            "angularVelocity": -0.7230218869110173,
-            "velocityX": 0.5088245046789369,
-            "velocityY": 0.3596572750296197,
-            "timestamp": 6.75218708530454
-        },
-        {
-            "x": 7.4938504955203875,
-            "y": 7.326060981562411,
-            "heading": 0.27119241907250147,
-            "angularVelocity": -0.6705094095505424,
-            "velocityX": 0.46716559024619986,
-            "velocityY": 0.3350686622032283,
-            "timestamp": 6.857765835564255
-        },
-        {
-            "x": 7.538762197286186,
-            "y": 7.358679050922846,
-            "heading": 0.2071469006235064,
-            "angularVelocity": -0.6066137198463124,
-            "velocityX": 0.4253858037038331,
-            "velocityY": 0.3089454019040213,
-            "timestamp": 6.96334458582397
-        },
-        {
-            "x": 7.57923356269503,
-            "y": 7.3883979284340455,
-            "heading": 0.15090824268085423,
-            "angularVelocity": -0.5326702370042972,
-            "velocityX": 0.38332870100058564,
-            "velocityY": 0.28148540800086536,
-            "timestamp": 7.068923336083685
-        },
-        {
-            "x": 7.612613012416259,
-            "y": 7.4137927319874946,
-            "heading": 0.0952672119425909,
-            "angularVelocity": -0.5270097494122625,
-            "velocityX": 0.316156891737545,
-            "velocityY": 0.2405294965409898,
-            "timestamp": 7.1745020863434
-        },
-        {
-            "x": 7.641161660602441,
-            "y": 7.4358681154587,
-            "heading": 0.04946855188730579,
-            "angularVelocity": -0.43378672242570576,
-            "velocityX": 0.27040145789287734,
-            "velocityY": 0.20908926766306235,
-            "timestamp": 7.280080836603116
-        },
-        {
-            "x": 7.664737893553612,
-            "y": 7.454419635462092,
-            "heading": 0.014221249681079469,
-            "angularVelocity": -0.3338484505242331,
-            "velocityX": 0.22330471529975876,
-            "velocityY": 0.17571263220528385,
-            "timestamp": 7.385659586862831
-        },
-        {
-            "x": 7.679999999654181,
-            "y": 7.467000000250119,
-            "heading": 5.571589197087409e-11,
-            "angularVelocity": -0.13469802934148392,
-            "velocityX": 0.1445566071683118,
-            "velocityY": 0.11915622105321995,
-            "timestamp": 7.491238337122546
-        },
-        {
-            "x": 7.6799999998383255,
-            "y": 7.467000000125839,
-            "heading": 2.732434354976469e-11,
-            "angularVelocity": -1.9015024944340445e-11,
-            "velocityX": 1.5544110263140067e-10,
-            "velocityY": -2.911726567326558e-11,
-            "timestamp": 7.596817087382261
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.traj
deleted file mode 100644
index 8dba26f9..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.traj	
+++ /dev/null
@@ -1,634 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.2999999999310377,
-            "y": 4.359999999993798,
-            "heading": -0.3900000000006091,
-            "angularVelocity": -1.7156392127364986e-12,
-            "velocityX": -4.6004235672195495e-11,
-            "velocityY": -2.541589729484622e-10,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2830691122580777,
-            "y": 4.4226688824706715,
-            "heading": -0.3961360368292833,
-            "angularVelocity": -0.04017087715885992,
-            "velocityX": -0.11084167601962927,
-            "velocityY": 0.4102752396150543,
-            "timestamp": 0.1527483904267538
-        },
-        {
-            "x": 2.2499539829007165,
-            "y": 4.551962677072163,
-            "heading": -0.39368124618911415,
-            "angularVelocity": 0.016070811835100392,
-            "velocityX": -0.21679527487321065,
-            "velocityY": 0.8464494731937718,
-            "timestamp": 0.3054967808535076
-        },
-        {
-            "x": 2.208537052758008,
-            "y": 4.723131432963358,
-            "heading": -0.3887340677228252,
-            "angularVelocity": 0.032387761683442845,
-            "velocityX": -0.27114478888671906,
-            "velocityY": 1.120592861369484,
-            "timestamp": 0.45824517128026143
-        },
-        {
-            "x": 2.1635048676051025,
-            "y": 4.925567881437362,
-            "heading": -0.37513416937632715,
-            "angularVelocity": 0.08903464258184278,
-            "velocityX": -0.294812828865451,
-            "velocityY": 1.3252934968125494,
-            "timestamp": 0.6109935617070152
-        },
-        {
-            "x": 2.119479495251874,
-            "y": 5.143090740041083,
-            "heading": -0.3554033967273168,
-            "angularVelocity": 0.12917172215836753,
-            "velocityX": -0.28822151359768483,
-            "velocityY": 1.4240599066467436,
-            "timestamp": 0.763741952133769
-        },
-        {
-            "x": 2.08196432941898,
-            "y": 5.365761039079461,
-            "heading": -0.32917795560380303,
-            "angularVelocity": 0.1716904580681245,
-            "velocityX": -0.24560105455579473,
-            "velocityY": 1.4577587261323537,
-            "timestamp": 0.9164903425605229
-        },
-        {
-            "x": 2.052346293594587,
-            "y": 5.582587972754811,
-            "heading": -0.29825458565802576,
-            "angularVelocity": 0.20244645364142877,
-            "velocityX": -0.19390080434302895,
-            "velocityY": 1.419503885313628,
-            "timestamp": 1.0692387329872766
-        },
-        {
-            "x": 2.030803808580604,
-            "y": 5.789012383645482,
-            "heading": -0.2654877856320327,
-            "angularVelocity": 0.2145148628152058,
-            "velocityX": -0.14103248420843936,
-            "velocityY": 1.3514015455604398,
-            "timestamp": 1.2219871234140305
-        },
-        {
-            "x": 2.0185215947450232,
-            "y": 5.980748966625485,
-            "heading": -0.23040407839794677,
-            "angularVelocity": 0.2296829912414591,
-            "velocityX": -0.08040813886408783,
-            "velocityY": 1.2552445394842306,
-            "timestamp": 1.3747355138407844
-        },
-        {
-            "x": 2.017537204929899,
-            "y": 6.152494473346335,
-            "heading": -0.19497588875865757,
-            "angularVelocity": 0.23193821893113326,
-            "velocityX": -0.006444518152378274,
-            "velocityY": 1.1243686841111533,
-            "timestamp": 1.5274839042675383
-        },
-        {
-            "x": 2.0415938446669073,
-            "y": 6.276321571691329,
-            "heading": -0.16155868009406138,
-            "angularVelocity": 0.21877290194991583,
-            "velocityX": 0.15749193627933206,
-            "velocityY": 0.8106605773582652,
-            "timestamp": 1.6802322946942922
-        },
-        {
-            "x": 2.1241281032079797,
-            "y": 6.2825961111606565,
-            "heading": -0.060349903928264906,
-            "angularVelocity": 0.6625848944275851,
-            "velocityX": 0.5403281719922621,
-            "velocityY": 0.04107761453342325,
-            "timestamp": 1.8329806851210462
-        },
-        {
-            "x": 2.1855581068088026,
-            "y": 6.287266231959223,
-            "heading": 0.012078838000808553,
-            "angularVelocity": 0.7458652653001706,
-            "velocityX": 0.6326011577496066,
-            "velocityY": 0.048092524107779686,
-            "timestamp": 1.9300876899626762
-        },
-        {
-            "x": 2.2551653620091354,
-            "y": 6.2925580157259535,
-            "heading": 0.08826667660975476,
-            "angularVelocity": 0.784576135772582,
-            "velocityX": 0.7168098264960764,
-            "velocityY": 0.05449435852189788,
-            "timestamp": 2.0271946948043063
-        },
-        {
-            "x": 2.3319122711008053,
-            "y": 6.29839258064769,
-            "heading": 0.16657489622034788,
-            "angularVelocity": 0.8064116460755234,
-            "velocityX": 0.7903333988427459,
-            "velocityY": 0.06008387443491743,
-            "timestamp": 2.1243016996459363
-        },
-        {
-            "x": 2.4149946395663093,
-            "y": 6.304708789079692,
-            "heading": 0.2457212902089042,
-            "angularVelocity": 0.8150430973252747,
-            "velocityX": 0.8555754417689245,
-            "velocityY": 0.06504379984951533,
-            "timestamp": 2.2214087044875663
-        },
-        {
-            "x": 2.5038373372195277,
-            "y": 6.311462917641332,
-            "heading": 0.32495163133637983,
-            "angularVelocity": 0.8159075780845946,
-            "velocityX": 0.9148948410382448,
-            "velocityY": 0.06955346543875748,
-            "timestamp": 2.3185157093291964
-        },
-        {
-            "x": 2.598073257877524,
-            "y": 6.3186270576568155,
-            "heading": 0.40431668105413066,
-            "angularVelocity": 0.8172947961305416,
-            "velocityX": 0.9704338108518987,
-            "velocityY": 0.0737757297255109,
-            "timestamp": 2.4156227141708264
-        },
-        {
-            "x": 2.697382502344975,
-            "y": 6.326176889286233,
-            "heading": 0.48416024389571777,
-            "angularVelocity": 0.8222224852016302,
-            "velocityX": 1.0226784849096606,
-            "velocityY": 0.07774755027256483,
-            "timestamp": 2.5127297190124565
-        },
-        {
-            "x": 2.8014852531337993,
-            "y": 6.334091139837385,
-            "heading": 0.5646614354219063,
-            "angularVelocity": 0.8289946917215433,
-            "velocityX": 1.0720416213516444,
-            "velocityY": 0.08150030637886811,
-            "timestamp": 2.6098367238540865
-        },
-        {
-            "x": 2.910148663901152,
-            "y": 6.342352107524804,
-            "heading": 0.6457203378655492,
-            "angularVelocity": 0.8347379532340086,
-            "velocityX": 1.119006925166821,
-            "velocityY": 0.0850707709678732,
-            "timestamp": 2.7069437286957165
-        },
-        {
-            "x": 3.0231702594110863,
-            "y": 6.350944399479571,
-            "heading": 0.7270565797219191,
-            "angularVelocity": 0.8375939718950361,
-            "velocityX": 1.1638871555335075,
-            "velocityY": 0.08848272107248593,
-            "timestamp": 2.8040507335373466
-        },
-        {
-            "x": 3.140351376164449,
-            "y": 6.359852912614292,
-            "heading": 0.8083350217640537,
-            "angularVelocity": 0.83699875417797,
-            "velocityX": 1.2067215654083316,
-            "velocityY": 0.09173914083795442,
-            "timestamp": 2.9011577383789766
-        },
-        {
-            "x": 3.261472243862432,
-            "y": 6.3690609392298585,
-            "heading": 0.8891526159463483,
-            "angularVelocity": 0.8322529806195065,
-            "velocityX": 1.2472927986960072,
-            "velocityY": 0.09482350610318373,
-            "timestamp": 2.9982647432206067
-        },
-        {
-            "x": 3.386280226554517,
-            "y": 6.378549273066906,
-            "heading": 0.9690024828336246,
-            "angularVelocity": 0.8222874040565998,
-            "velocityX": 1.2852624076036416,
-            "velocityY": 0.09771008689812195,
-            "timestamp": 3.0953717480622367
-        },
-        {
-            "x": 3.5144942146424594,
-            "y": 6.388296543201246,
-            "heading": 1.0472679478096496,
-            "angularVelocity": 0.8059713626886263,
-            "velocityX": 1.3203371728362745,
-            "velocityY": 0.10037659168026988,
-            "timestamp": 3.1924787529038667
-        },
-        {
-            "x": 3.64579126909874,
-            "y": 6.398278198691388,
-            "heading": 1.1233630936332748,
-            "angularVelocity": 0.783621593451087,
-            "velocityX": 1.3520863383515835,
-            "velocityY": 0.10279027288912505,
-            "timestamp": 3.2895857577454968
-        },
-        {
-            "x": 3.779855631903331,
-            "y": 6.408470234520327,
-            "heading": 1.1966377830577097,
-            "angularVelocity": 0.7545767639735752,
-            "velocityX": 1.3805838535787394,
-            "velocityY": 0.10495675240615632,
-            "timestamp": 3.386692762587127
-        },
-        {
-            "x": 3.916399996401096,
-            "y": 6.418850808641588,
-            "heading": 1.2663868507253695,
-            "angularVelocity": 0.7182701992663435,
-            "velocityX": 1.406122708347489,
-            "velocityY": 0.10689830431744617,
-            "timestamp": 3.483799767428757
-        },
-        {
-            "x": 4.055149110841729,
-            "y": 6.429398995480507,
-            "heading": 1.3319162676094702,
-            "angularVelocity": 0.6748165803464835,
-            "velocityX": 1.4288270430729209,
-            "velocityY": 0.10862436634220823,
-            "timestamp": 3.580906772270387
-        },
-        {
-            "x": 4.195643021249515,
-            "y": 6.440079827739076,
-            "heading": 1.392465239071673,
-            "angularVelocity": 0.6235283604234658,
-            "velocityX": 1.4467948093350342,
-            "velocityY": 0.10999033795341527,
-            "timestamp": 3.678013777112017
-        },
-        {
-            "x": 4.337478451024624,
-            "y": 6.45086264692987,
-            "heading": 1.4461855651355748,
-            "angularVelocity": 0.5532075282023133,
-            "velocityX": 1.4606096661273347,
-            "velocityY": 0.11104059096144768,
-            "timestamp": 3.775120781953647
-        },
-        {
-            "x": 4.47965775917095,
-            "y": 6.461671608979337,
-            "heading": 1.49351259060778,
-            "angularVelocity": 0.48736984118650195,
-            "velocityX": 1.4641508974652757,
-            "velocityY": 0.11130980792840979,
-            "timestamp": 3.872227786795277
-        },
-        {
-            "x": 4.621471854948602,
-            "y": 6.4724528062914635,
-            "heading": 1.5350394838354429,
-            "angularVelocity": 0.42764055328666206,
-            "velocityX": 1.4603899702927075,
-            "velocityY": 0.11102388888102613,
-            "timestamp": 3.969334791636907
-        },
-        {
-            "x": 4.762134624452166,
-            "y": 6.483146475819268,
-            "heading": 1.5711187532761213,
-            "angularVelocity": 0.37154136862160403,
-            "velocityX": 1.4485337063274482,
-            "velocityY": 0.11012253482900584,
-            "timestamp": 4.0664417964785375
-        },
-        {
-            "x": 4.901310758087736,
-            "y": 6.493727126167039,
-            "heading": 1.6012523506615222,
-            "angularVelocity": 0.3103133235343918,
-            "velocityX": 1.433224451514287,
-            "velocityY": 0.10895867241503827,
-            "timestamp": 4.163548801320168
-        },
-        {
-            "x": 5.0385652541984065,
-            "y": 6.504161687000508,
-            "heading": 1.6187441808494436,
-            "angularVelocity": 0.18012943788636607,
-            "velocityX": 1.413435585164251,
-            "velocityY": 0.1074542543793728,
-            "timestamp": 4.2606558061617985
-        },
-        {
-            "x": 5.1726571421935885,
-            "y": 6.5143558154807435,
-            "heading": 1.6310181673546418,
-            "angularVelocity": 0.12639650992409435,
-            "velocityX": 1.3808673050498854,
-            "velocityY": 0.104978301436634,
-            "timestamp": 4.357762811003429
-        },
-        {
-            "x": 5.303636473288953,
-            "y": 6.524313316659959,
-            "heading": 1.6376756877148975,
-            "angularVelocity": 0.06855860046187665,
-            "velocityX": 1.3488144481980282,
-            "velocityY": 0.1025415325257505,
-            "timestamp": 4.4548698158450595
-        },
-        {
-            "x": 5.4313983902221485,
-            "y": 6.53402621894025,
-            "heading": 1.6386463375950402,
-            "angularVelocity": 0.009995672907119661,
-            "velocityX": 1.3156817797402116,
-            "velocityY": 0.1000226726538996,
-            "timestamp": 4.55197682068669
-        },
-        {
-            "x": 5.555758593734715,
-            "y": 6.5434805112143435,
-            "heading": 1.6339151298589827,
-            "angularVelocity": -0.048721590949420676,
-            "velocityX": 1.2806512124922533,
-            "velocityY": 0.09735952745677612,
-            "timestamp": 4.6490838255283204
-        },
-        {
-            "x": 5.676522731590665,
-            "y": 6.552661418256687,
-            "heading": 1.6211253547169766,
-            "angularVelocity": -0.13170805962068485,
-            "velocityX": 1.2436192217305724,
-            "velocityY": 0.0945442281350229,
-            "timestamp": 4.746190830369951
-        },
-        {
-            "x": 5.8040475153273645,
-            "y": 6.568758338060969,
-            "heading": 1.594817916740329,
-            "angularVelocity": -0.24917360707174604,
-            "velocityX": 1.2078641151592902,
-            "velocityY": 0.15246363592342288,
-            "timestamp": 4.851769580629666
-        },
-        {
-            "x": 5.9283875752902055,
-            "y": 6.58985033542802,
-            "heading": 1.559498986776326,
-            "angularVelocity": -0.33452688097090233,
-            "velocityX": 1.1776996757735678,
-            "velocityY": 0.19977502680380196,
-            "timestamp": 4.957348330889381
-        },
-        {
-            "x": 6.049721376535089,
-            "y": 6.61532142807792,
-            "heading": 1.5174190958344496,
-            "angularVelocity": -0.3985640187289682,
-            "velocityX": 1.1492255865917638,
-            "velocityY": 0.2412520781772084,
-            "timestamp": 5.0629270811490965
-        },
-        {
-            "x": 6.167915961542227,
-            "y": 6.644893212673861,
-            "heading": 1.4694436889435494,
-            "angularVelocity": -0.45440400479138554,
-            "velocityX": 1.119492176613303,
-            "velocityY": 0.280092203729064,
-            "timestamp": 5.168505831408812
-        },
-        {
-            "x": 6.2827599678090085,
-            "y": 6.678289224223258,
-            "heading": 1.4161083022197893,
-            "angularVelocity": -0.5051716053929952,
-            "velocityX": 1.087756824956101,
-            "velocityY": 0.31631376267017064,
-            "timestamp": 5.274084581668527
-        },
-        {
-            "x": 6.394018817655917,
-            "y": 6.715173356117064,
-            "heading": 1.3577889107275223,
-            "angularVelocity": -0.5523781194839231,
-            "velocityX": 1.0537996466964168,
-            "velocityY": 0.34935185331081736,
-            "timestamp": 5.379663331928242
-        },
-        {
-            "x": 6.501464569355021,
-            "y": 6.755149800954155,
-            "heading": 1.2948750452839908,
-            "angularVelocity": -0.5958951523626541,
-            "velocityX": 1.0176834949234126,
-            "velocityY": 0.3786410134453019,
-            "timestamp": 5.485242082187957
-        },
-        {
-            "x": 6.6048776092971675,
-            "y": 6.797770420027359,
-            "heading": 1.22768881668393,
-            "angularVelocity": -0.6363612797096425,
-            "velocityX": 0.9794872502530397,
-            "velocityY": 0.4036855817031342,
-            "timestamp": 5.5908208324476725
-        },
-        {
-            "x": 6.70404339771049,
-            "y": 6.8425429323292,
-            "heading": 1.1564439654233045,
-            "angularVelocity": -0.6748029419532755,
-            "velocityX": 0.939258970068964,
-            "velocityY": 0.4240674597490353,
-            "timestamp": 5.696399582707388
-        },
-        {
-            "x": 6.79877778418725,
-            "y": 6.888932915699562,
-            "heading": 1.0813771004184407,
-            "angularVelocity": -0.7110035384939538,
-            "velocityX": 0.8972864903430491,
-            "velocityY": 0.4393875033009405,
-            "timestamp": 5.801978332967103
-        },
-        {
-            "x": 6.88894352921239,
-            "y": 6.936373258053616,
-            "heading": 1.0028508284852347,
-            "angularVelocity": -0.7437696672526416,
-            "velocityX": 0.8540141324253476,
-            "velocityY": 0.44933608637180433,
-            "timestamp": 5.907557083226818
-        },
-        {
-            "x": 6.974457896759412,
-            "y": 6.984282785155121,
-            "heading": 0.9213875618911144,
-            "angularVelocity": -0.7715877144447874,
-            "velocityX": 0.809958131723675,
-            "velocityY": 0.45378001785130806,
-            "timestamp": 6.013135833486533
-        },
-        {
-            "x": 7.055294443748765,
-            "y": 7.032089281864471,
-            "heading": 0.8376815844807217,
-            "angularVelocity": -0.792829780737162,
-            "velocityX": 0.7656516726874414,
-            "velocityY": 0.45280415475213037,
-            "timestamp": 6.118714583746248
-        },
-        {
-            "x": 7.131475603500295,
-            "y": 7.0792526481673885,
-            "heading": 0.752591160441856,
-            "angularVelocity": -0.805942709434023,
-            "velocityX": 0.7215576931722669,
-            "velocityY": 0.44671267879627613,
-            "timestamp": 6.224293334005964
-        },
-        {
-            "x": 7.203056823429108,
-            "y": 7.125284419454981,
-            "heading": 0.6670903070961578,
-            "angularVelocity": -0.8098301328249344,
-            "velocityX": 0.6779888895797812,
-            "velocityY": 0.4359946600433992,
-            "timestamp": 6.329872084265679
-        },
-        {
-            "x": 7.270104931236086,
-            "y": 7.169760877903185,
-            "heading": 0.5821807537401323,
-            "angularVelocity": -0.8042295741051197,
-            "velocityX": 0.6350530514787667,
-            "velocityY": 0.4212633543513555,
-            "timestamp": 6.435450834525394
-        },
-        {
-            "x": 7.332675465601759,
-            "y": 7.212330496694183,
-            "heading": 0.49884759114131233,
-            "angularVelocity": -0.789298626772507,
-            "velocityX": 0.592643253784208,
-            "velocityY": 0.4032025262817465,
-            "timestamp": 6.541029584785109
-        },
-        {
-            "x": 7.390806680523043,
-            "y": 7.252712685445242,
-            "heading": 0.4183197118253645,
-            "angularVelocity": -0.7627280973825078,
-            "velocityX": 0.5505957828143127,
-            "velocityY": 0.38248405756315146,
-            "timestamp": 6.646608335044824
-        },
-        {
-            "x": 7.444527736090263,
-            "y": 7.290684851015479,
-            "heading": 0.34198396458440167,
-            "angularVelocity": -0.7230218869110173,
-            "velocityX": 0.5088245046789369,
-            "velocityY": 0.3596572750296197,
-            "timestamp": 6.75218708530454
-        },
-        {
-            "x": 7.4938504955203875,
-            "y": 7.326060981562411,
-            "heading": 0.27119241907250147,
-            "angularVelocity": -0.6705094095505424,
-            "velocityX": 0.46716559024619986,
-            "velocityY": 0.3350686622032283,
-            "timestamp": 6.857765835564255
-        },
-        {
-            "x": 7.538762197286186,
-            "y": 7.358679050922846,
-            "heading": 0.2071469006235064,
-            "angularVelocity": -0.6066137198463124,
-            "velocityX": 0.4253858037038331,
-            "velocityY": 0.3089454019040213,
-            "timestamp": 6.96334458582397
-        },
-        {
-            "x": 7.57923356269503,
-            "y": 7.3883979284340455,
-            "heading": 0.15090824268085423,
-            "angularVelocity": -0.5326702370042972,
-            "velocityX": 0.38332870100058564,
-            "velocityY": 0.28148540800086536,
-            "timestamp": 7.068923336083685
-        },
-        {
-            "x": 7.612613012416259,
-            "y": 7.4137927319874946,
-            "heading": 0.0952672119425909,
-            "angularVelocity": -0.5270097494122625,
-            "velocityX": 0.316156891737545,
-            "velocityY": 0.2405294965409898,
-            "timestamp": 7.1745020863434
-        },
-        {
-            "x": 7.641161660602441,
-            "y": 7.4358681154587,
-            "heading": 0.04946855188730579,
-            "angularVelocity": -0.43378672242570576,
-            "velocityX": 0.27040145789287734,
-            "velocityY": 0.20908926766306235,
-            "timestamp": 7.280080836603116
-        },
-        {
-            "x": 7.664737893553612,
-            "y": 7.454419635462092,
-            "heading": 0.014221249681079469,
-            "angularVelocity": -0.3338484505242331,
-            "velocityX": 0.22330471529975876,
-            "velocityY": 0.17571263220528385,
-            "timestamp": 7.385659586862831
-        },
-        {
-            "x": 7.679999999654181,
-            "y": 7.467000000250119,
-            "heading": 5.571589197087409e-11,
-            "angularVelocity": -0.13469802934148392,
-            "velocityX": 0.1445566071683118,
-            "velocityY": 0.11915622105321995,
-            "timestamp": 7.491238337122546
-        },
-        {
-            "x": 7.6799999998383255,
-            "y": 7.467000000125839,
-            "heading": 2.732434354976469e-11,
-            "angularVelocity": -1.9015024944340445e-11,
-            "velocityX": 1.5544110263140067e-10,
-            "velocityY": -2.911726567326558e-11,
-            "timestamp": 7.596817087382261
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.1.traj
deleted file mode 100644
index de7450e6..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.1.traj	
+++ /dev/null
@@ -1,634 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": -2.538537990246625e-31,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2837844841867163,
-            "y": 4.387687502531162,
-            "heading": -0.3845108824411683,
-            "angularVelocity": 0.0884354987329395,
-            "velocityX": -0.26124913754506907,
-            "velocityY": 0.4460749963360829,
-            "timestamp": 0.06206916495779045
-        },
-        {
-            "x": 2.252431932443338,
-            "y": 4.44366896754607,
-            "heading": -0.37369458929412197,
-            "angularVelocity": 0.17426194076711193,
-            "velocityX": -0.5051228216917889,
-            "velocityY": 0.9019207049574874,
-            "timestamp": 0.1241383299155809
-        },
-        {
-            "x": 2.2074803876744995,
-            "y": 4.528718281766582,
-            "heading": -0.3577885652255258,
-            "angularVelocity": 0.2562628976859123,
-            "velocityX": -0.724217005329779,
-            "velocityY": 1.3702345484796434,
-            "timestamp": 0.18620749487337135
-        },
-        {
-            "x": 2.1512700092503536,
-            "y": 4.643824218738039,
-            "heading": -0.3371682293163389,
-            "angularVelocity": 0.3322154555127859,
-            "velocityX": -0.905608742482958,
-            "velocityY": 1.8544785812676818,
-            "timestamp": 0.2482766598311618
-        },
-        {
-            "x": 2.087674884006875,
-            "y": 4.7901724592691535,
-            "heading": -0.3124919375355934,
-            "angularVelocity": 0.3975612012535503,
-            "velocityX": -1.0245848367214008,
-            "velocityY": 2.357825187926933,
-            "timestamp": 0.31034582478895223
-        },
-        {
-            "x": 2.0237484850052736,
-            "y": 4.968633105417482,
-            "heading": -0.2850705208032402,
-            "angularVelocity": 0.44178807225385713,
-            "velocityX": -1.029922007895696,
-            "velocityY": 2.8751900604698215,
-            "timestamp": 0.3724149897467427
-        },
-        {
-            "x": 1.9724195760606846,
-            "y": 5.1766438961084935,
-            "heading": -0.25765844821795814,
-            "angularVelocity": 0.4416375281458893,
-            "velocityX": -0.8269630980122488,
-            "velocityY": 3.351274192787545,
-            "timestamp": 0.43448415470453317
-        },
-        {
-            "x": 1.9484287592243066,
-            "y": 5.401486281993408,
-            "heading": -0.23420864800659155,
-            "angularVelocity": 0.3778011227958159,
-            "velocityX": -0.38651747373332257,
-            "velocityY": 3.6224490217904175,
-            "timestamp": 0.49655331966232363
-        },
-        {
-            "x": 1.9564542032821701,
-            "y": 5.628890453578366,
-            "heading": -0.21272720707613346,
-            "angularVelocity": 0.34608876961188295,
-            "velocityX": 0.12929840547219124,
-            "velocityY": 3.663722103228105,
-            "timestamp": 0.5586224846201141
-        },
-        {
-            "x": 1.99619273658118,
-            "y": 5.8512380599288125,
-            "heading": -0.18843692284079885,
-            "angularVelocity": 0.39134221077502945,
-            "velocityX": 0.6402298681838031,
-            "velocityY": 3.5822554806643043,
-            "timestamp": 0.6206916495779046
-        },
-        {
-            "x": 2.0663437221615903,
-            "y": 6.063242956223716,
-            "heading": -0.16364621155748288,
-            "angularVelocity": 0.3994046206362532,
-            "velocityX": 1.130206691653697,
-            "velocityY": 3.4156234651942974,
-            "timestamp": 0.682760814535695
-        },
-        {
-            "x": 2.1656765937805176,
-            "y": 6.261821746826172,
-            "heading": -0.13977158803318993,
-            "angularVelocity": 0.38464547638239754,
-            "velocityX": 1.6003577893593972,
-            "velocityY": 3.1993146796407252,
-            "timestamp": 0.7448299794934855
-        },
-        {
-            "x": 2.225814155156259,
-            "y": 6.362141570414172,
-            "heading": -0.12770962458788607,
-            "angularVelocity": 0.3689752694416071,
-            "velocityX": 1.8396070434406133,
-            "velocityY": 3.068781803712599,
-            "timestamp": 0.7775204182423698
-        },
-        {
-            "x": 2.2936460979223643,
-            "y": 6.4579721340232545,
-            "heading": -0.11638786741998439,
-            "angularVelocity": 0.3463323712222401,
-            "velocityX": 2.074978047465827,
-            "velocityY": 2.9314554125487073,
-            "timestamp": 0.8102108569912542
-        },
-        {
-            "x": 2.3690076803030626,
-            "y": 6.549046337769426,
-            "heading": -0.10608509188235166,
-            "angularVelocity": 0.31516173940473013,
-            "velocityX": 2.3053096031972573,
-            "velocityY": 2.7859584401928488,
-            "timestamp": 0.8429012957401385
-        },
-        {
-            "x": 2.4516786592945876,
-            "y": 6.635038598364521,
-            "heading": -0.09714822011858378,
-            "angularVelocity": 0.27337876474752265,
-            "velocityX": 2.528903928955548,
-            "velocityY": 2.6305018802439832,
-            "timestamp": 0.8755917344890228
-        },
-        {
-            "x": 2.5413546021547084,
-            "y": 6.715546857152131,
-            "heading": -0.09001694608551258,
-            "angularVelocity": 0.21814555896345397,
-            "velocityX": 2.7431856619836545,
-            "velocityY": 2.462746352407108,
-            "timestamp": 0.9082821732379072
-        },
-        {
-            "x": 2.6375981860796323,
-            "y": 6.79006985383775,
-            "heading": -0.08525828181365991,
-            "angularVelocity": 0.14556746421386957,
-            "velocityX": 2.9440896974181805,
-            "velocityY": 2.2796572801553694,
-            "timestamp": 0.9409726119867915
-        },
-        {
-            "x": 2.73984178672467,
-            "y": 6.858044565031264,
-            "heading": -0.08306946201564708,
-            "angularVelocity": 0.06695596274838787,
-            "velocityX": 3.1276301131997113,
-            "velocityY": 2.079345331378129,
-            "timestamp": 0.9736630507356758
-        },
-        {
-            "x": 2.847369129600613,
-            "y": 6.918863246924361,
-            "heading": -0.08274471887343776,
-            "angularVelocity": 0.009933887583435854,
-            "velocityX": 3.2892597037781757,
-            "velocityY": 1.8604425091962247,
-            "timestamp": 1.0063534894845603
-        },
-        {
-            "x": 2.9591159211859868,
-            "y": 6.971834136920771,
-            "heading": -0.08274467756065068,
-            "angularVelocity": 0.0000012637574442965605,
-            "velocityX": 3.418332572524967,
-            "velocityY": 1.6203786802362972,
-            "timestamp": 1.0390439282334447
-        },
-        {
-            "x": 3.0743869446602656,
-            "y": 7.0166209198254075,
-            "heading": -0.0827446619031275,
-            "angularVelocity": 4.789633846658565e-7,
-            "velocityX": 3.526138769803835,
-            "velocityY": 1.3700269748141811,
-            "timestamp": 1.0717343669823292
-        },
-        {
-            "x": 3.192583723464696,
-            "y": 7.052991069898469,
-            "heading": -0.08274464937093684,
-            "angularVelocity": 3.833595125790563e-7,
-            "velocityX": 3.61563757869459,
-            "velocityY": 1.1125623107197633,
-            "timestamp": 1.1044248057312136
-        },
-        {
-            "x": 3.313092599420772,
-            "y": 7.080755764680444,
-            "heading": -0.08274463888121661,
-            "angularVelocity": 3.208803751652956e-7,
-            "velocityX": 3.6863645936910636,
-            "velocityY": 0.8493215706001247,
-            "timestamp": 1.137115244480098
-        },
-        {
-            "x": 3.4352879115666495,
-            "y": 7.0997708632875725,
-            "heading": -0.08274462977736693,
-            "angularVelocity": 2.7848661636087506e-7,
-            "velocityX": 3.7379526498424447,
-            "velocityY": 0.5816715631449295,
-            "timestamp": 1.1698056832289825
-        },
-        {
-            "x": 3.5585352449602814,
-            "y": 7.109937657437644,
-            "heading": -0.08274462163132143,
-            "angularVelocity": 2.491874023135916e-7,
-            "velocityX": 3.770133963033834,
-            "velocityY": 0.31100207091115845,
-            "timestamp": 1.202496121977867
-        },
-        {
-            "x": 3.6821947259931562,
-            "y": 7.1112033907952945,
-            "heading": -0.08274461414532981,
-            "angularVelocity": 2.2899636479565772e-7,
-            "velocityX": 3.782741552745206,
-            "velocityY": 0.03871876321990418,
-            "timestamp": 1.2351865607267514
-        },
-        {
-            "x": 3.8056243495254822,
-            "y": 7.103561554173401,
-            "heading": -0.08274460709859606,
-            "angularVelocity": 2.1555947310703947e-7,
-            "velocityX": 3.775710215468651,
-            "velocityY": -0.23376366040158306,
-            "timestamp": 1.2678769994756358
-        },
-        {
-            "x": 3.9281833350664277,
-            "y": 7.087052015079957,
-            "heading": -0.08274460031581693,
-            "angularVelocity": 2.0748510569540933e-7,
-            "velocityX": 3.7490774131963343,
-            "velocityY": -0.5050265375893709,
-            "timestamp": 1.3005674382245203
-        },
-        {
-            "x": 4.0492357154991785,
-            "y": 7.0617618768818975,
-            "heading": -0.08274459364684544,
-            "angularVelocity": 2.0400373145008536e-7,
-            "velocityX": 3.7029903869628296,
-            "velocityY": -0.7736249241717706,
-            "timestamp": 1.3332578769734047
-        },
-        {
-            "x": 4.169706806774688,
-            "y": 7.033833646086136,
-            "heading": -0.0827445869919534,
-            "angularVelocity": 2.0357304165195936e-7,
-            "velocityX": 3.6852087609158533,
-            "velocityY": -0.8543241346506245,
-            "timestamp": 1.3659483157222891
-        },
-        {
-            "x": 4.290177832890272,
-            "y": 7.00590513421804,
-            "heading": -0.08274458033705716,
-            "angularVelocity": 2.0357317018224517e-7,
-            "velocityX": 3.6852067676748597,
-            "velocityY": -0.8543327326486478,
-            "timestamp": 1.3986387544711736
-        },
-        {
-            "x": 4.410648856777724,
-            "y": 6.977976612738755,
-            "heading": -0.08274457368215653,
-            "angularVelocity": 2.0357330400976238e-7,
-            "velocityX": 3.685206699516314,
-            "velocityY": -0.8543330266547546,
-            "timestamp": 1.431329193220058
-        },
-        {
-            "x": 4.531119878449975,
-            "y": 6.950048081704109,
-            "heading": -0.08274456702725173,
-            "angularVelocity": 2.0357343141081232e-7,
-            "velocityX": 3.6852066317533563,
-            "velocityY": -0.8543333189530982,
-            "timestamp": 1.4640196319689425
-        },
-        {
-            "x": 4.651590897922017,
-            "y": 6.922119541178764,
-            "heading": -0.082744560372343,
-            "angularVelocity": 2.0357355233562972e-7,
-            "velocityX": 3.6852065644490066,
-            "velocityY": -0.8543336092734171,
-            "timestamp": 1.496710070717827
-        },
-        {
-            "x": 4.772061915216332,
-            "y": 6.894190991259702,
-            "heading": -0.08274455371743049,
-            "angularVelocity": 2.0357366776279015e-7,
-            "velocityX": 3.68520649783238,
-            "velocityY": -0.8543338966271132,
-            "timestamp": 1.5294005094667114
-        },
-        {
-            "x": 4.892532930362486,
-            "y": 6.866262432074455,
-            "heading": -0.08274454706251447,
-            "angularVelocity": 2.0357377502591166e-7,
-            "velocityX": 3.6852064321201583,
-            "velocityY": -0.8543341800795031,
-            "timestamp": 1.5620909482155958
-        },
-        {
-            "x": 5.0130039433965745,
-            "y": 6.838333863778735,
-            "heading": -0.08274454040759514,
-            "angularVelocity": 2.0357387593869802e-7,
-            "velocityX": 3.685206367512119,
-            "velocityY": -0.854334458768776,
-            "timestamp": 1.5947813869644802
-        },
-        {
-            "x": 5.133474954360567,
-            "y": 6.810405286553609,
-            "heading": -0.08274453375267282,
-            "angularVelocity": 2.0357396812707888e-7,
-            "velocityX": 3.6852063041879153,
-            "velocityY": -0.8543347319191732,
-            "timestamp": 1.6274718257133647
-        },
-        {
-            "x": 5.253945963272805,
-            "y": 6.782476700477742,
-            "heading": -0.08274452709774775,
-            "angularVelocity": 2.035740515232947e-7,
-            "velocityX": 3.6852062414248348,
-            "velocityY": -0.8543350026628553,
-            "timestamp": 1.6601622644622491
-        },
-        {
-            "x": 5.374416652936201,
-            "y": 6.754546737348104,
-            "heading": -0.08274452044281651,
-            "angularVelocity": 2.0357424100563973e-7,
-            "velocityX": 3.685196475607398,
-            "velocityY": -0.8543771267177642,
-            "timestamp": 1.6928527032111336
-        },
-        {
-            "x": 5.493278503417969,
-            "y": 6.720412731170654,
-            "heading": -0.08274451374626977,
-            "angularVelocity": 2.0484725764732438e-7,
-            "velocityX": 3.6359821106976624,
-            "velocityY": -1.0441587046150123,
-            "timestamp": 1.725543141960018
-        },
-        {
-            "x": 5.6078450645507765,
-            "y": 6.6784969077855525,
-            "heading": -0.0827445073182203,
-            "angularVelocity": 1.9932954157588389e-7,
-            "velocityX": 3.5526329088991675,
-            "velocityY": -1.2997818219306485,
-            "timestamp": 1.7577914952435287
-        },
-        {
-            "x": 5.720102544926552,
-            "y": 6.630740323854381,
-            "heading": -0.08274450093221485,
-            "angularVelocity": 1.980257845223315e-7,
-            "velocityX": 3.481029849458213,
-            "velocityY": -1.4808999241394636,
-            "timestamp": 1.7900398485270395
-        },
-        {
-            "x": 5.832359462510262,
-            "y": 6.582982417035901,
-            "heading": -0.08274449454621227,
-            "angularVelocity": 1.9802569455219195e-7,
-            "velocityX": 3.481012397650423,
-            "velocityY": -1.480940945995501,
-            "timestamp": 1.8222882018105502
-        },
-        {
-            "x": 5.944616376948871,
-            "y": 6.535224502824747,
-            "heading": -0.08274448816020867,
-            "angularVelocity": 1.9802572730548282e-7,
-            "velocityX": 3.4810123001228868,
-            "velocityY": -1.4809411752374333,
-            "timestamp": 1.854536555094061
-        },
-        {
-            "x": 6.0568732883758765,
-            "y": 6.487466581534697,
-            "heading": -0.0827444817742042,
-            "angularVelocity": 1.9802575385283685e-7,
-            "velocityX": 3.4810122067350586,
-            "velocityY": -1.4809413947492354,
-            "timestamp": 1.8867849083775716
-        },
-        {
-            "x": 6.169130196876316,
-            "y": 6.439708653365641,
-            "heading": -0.08274447538819907,
-            "angularVelocity": 1.980257744166014e-7,
-            "velocityX": 3.4810121159842042,
-            "velocityY": -1.4809416080626625,
-            "timestamp": 1.9190332616610823
-        },
-        {
-            "x": 6.281387102537832,
-            "y": 6.391950718523587,
-            "heading": -0.08274446900219351,
-            "angularVelocity": 1.9802578764982062e-7,
-            "velocityX": 3.481012027951073,
-            "velocityY": -1.4809418149879348,
-            "timestamp": 1.951281614944593
-        },
-        {
-            "x": 6.393644005449787,
-            "y": 6.344192777218584,
-            "heading": -0.08274446261618765,
-            "angularVelocity": 1.9802579747327125e-7,
-            "velocityX": 3.481011942689016,
-            "velocityY": -1.4809420153996509,
-            "timestamp": 1.9835299682281038
-        },
-        {
-            "x": 6.505900905702341,
-            "y": 6.296434829662559,
-            "heading": -0.08274445623018166,
-            "angularVelocity": 1.9802580083930334e-7,
-            "velocityX": 3.4810118602227385,
-            "velocityY": -1.4809422092397408,
-            "timestamp": 2.0157783215116143
-        },
-        {
-            "x": 6.618157803385852,
-            "y": 6.2486768760679015,
-            "heading": -0.08274444984417571,
-            "angularVelocity": 1.9802579953185327e-7,
-            "velocityX": 3.4810117805584304,
-            "velocityY": -1.4809423964936643,
-            "timestamp": 2.048026674795125
-        },
-        {
-            "x": 6.730414698590335,
-            "y": 6.200918916646197,
-            "heading": -0.08274444345816992,
-            "angularVelocity": 1.9802579502845229e-7,
-            "velocityX": 3.4810117036854167,
-            "velocityY": -1.4809425771865148,
-            "timestamp": 2.0802750280786353
-        },
-        {
-            "x": 6.842671592155369,
-            "y": 6.153160953370972,
-            "heading": -0.08274443707212797,
-            "angularVelocity": 1.980269160510039e-7,
-            "velocityX": 3.4810116528472412,
-            "velocityY": -1.4809426966816255,
-            "timestamp": 2.112523381362146
-        },
-        {
-            "x": 6.954235049253699,
-            "y": 6.105720053055757,
-            "heading": -0.08066018407058251,
-            "angularVelocity": 0.06463130018523208,
-            "velocityX": 3.459508648938503,
-            "velocityY": -1.4711107850419303,
-            "timestamp": 2.1447717346456563
-        },
-        {
-            "x": 7.05792262192444,
-            "y": 6.061617369901677,
-            "heading": -0.07347223812764758,
-            "angularVelocity": 0.2228934258982459,
-            "velocityX": 3.215282707900597,
-            "velocityY": -1.3675948897716332,
-            "timestamp": 2.177020087929167
-        },
-        {
-            "x": 7.153639251658588,
-            "y": 6.020903567478063,
-            "heading": -0.06480991744029473,
-            "angularVelocity": 0.2686128067130366,
-            "velocityX": 2.9681090656833784,
-            "velocityY": -1.2625079508922343,
-            "timestamp": 2.2092684412126773
-        },
-        {
-            "x": 7.241376863236811,
-            "y": 5.983582900712933,
-            "heading": -0.05573995757660787,
-            "angularVelocity": 0.28125342661540725,
-            "velocityX": 2.7206850162822076,
-            "velocityY": -1.1572890695232254,
-            "timestamp": 2.241516794496188
-        },
-        {
-            "x": 7.321134939051253,
-            "y": 5.949655957408732,
-            "heading": -0.04677486060395727,
-            "angularVelocity": 0.2780016980662186,
-            "velocityX": 2.4732449162054873,
-            "velocityY": -1.0520519607910965,
-            "timestamp": 2.2737651477796983
-        },
-        {
-            "x": 7.392914197093691,
-            "y": 5.919122647933878,
-            "heading": -0.03821600737741098,
-            "angularVelocity": 0.26540434952773495,
-            "velocityX": 2.2258270805765674,
-            "velocityY": -0.946817631474723,
-            "timestamp": 2.306013501063209
-        },
-        {
-            "x": 7.456715523277294,
-            "y": 5.891982736437705,
-            "heading": -0.030261865024148932,
-            "angularVelocity": 0.24665266729552143,
-            "velocityX": 1.9784367165261438,
-            "velocityY": -0.8415906157307577,
-            "timestamp": 2.3382618543467193
-        },
-        {
-            "x": 7.512539743308544,
-            "y": 5.868235970303718,
-            "heading": -0.023053012750234723,
-            "angularVelocity": 0.2235417173255794,
-            "velocityX": 1.7310719570848203,
-            "velocityY": -0.736371433456386,
-            "timestamp": 2.37051020763023
-        },
-        {
-            "x": 7.560387579465668,
-            "y": 5.847882114762634,
-            "heading": -0.016694240391467878,
-            "angularVelocity": 0.19718130420078028,
-            "velocityX": 1.4837295950113008,
-            "velocityY": -0.6311595312214304,
-            "timestamp": 2.4027585609137403
-        },
-        {
-            "x": 7.600259653459209,
-            "y": 5.830920960662069,
-            "heading": -0.011266668966831511,
-            "angularVelocity": 0.1683053821980557,
-            "velocityX": 1.2364065117684333,
-            "velocityY": -0.5259541146628961,
-            "timestamp": 2.435006914197251
-        },
-        {
-            "x": 7.632156499892169,
-            "y": 5.817352324088122,
-            "heading": -0.006834955017927562,
-            "angularVelocity": 0.1374245038171387,
-            "velocityX": 0.9891000062092736,
-            "velocityY": -0.4207544011521409,
-            "timestamp": 2.4672552674807613
-        },
-        {
-            "x": 7.656078580521334,
-            "y": 5.807176043662066,
-            "heading": -0.0034518424131983775,
-            "angularVelocity": 0.10490807313441347,
-            "velocityX": 0.7418078194211917,
-            "velocityY": -0.31555969188850075,
-            "timestamp": 2.499503620764272
-        },
-        {
-            "x": 7.672026296758129,
-            "y": 5.800391977434502,
-            "heading": -0.0011611797809329624,
-            "angularVelocity": 0.07103192563441285,
-            "velocityX": 0.49452808013452626,
-            "velocityY": -0.21036938438134845,
-            "timestamp": 2.5317519740477823
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 1.9934013230001416e-31,
-            "angularVelocity": 0.036007413176266045,
-            "velocityX": 0.2472592374491084,
-            "velocityY": -0.10518296561320875,
-            "timestamp": 2.564000327331293
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 1.0109342582388432e-32,
-            "angularVelocity": -5.855934233748412e-30,
-            "velocityX": 8.513095472636782e-32,
-            "velocityY": -2.1397535642954256e-34,
-            "timestamp": 2.5962486806148033
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.traj
deleted file mode 100644
index de7450e6..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.traj	
+++ /dev/null
@@ -1,634 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": -2.538537990246625e-31,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2837844841867163,
-            "y": 4.387687502531162,
-            "heading": -0.3845108824411683,
-            "angularVelocity": 0.0884354987329395,
-            "velocityX": -0.26124913754506907,
-            "velocityY": 0.4460749963360829,
-            "timestamp": 0.06206916495779045
-        },
-        {
-            "x": 2.252431932443338,
-            "y": 4.44366896754607,
-            "heading": -0.37369458929412197,
-            "angularVelocity": 0.17426194076711193,
-            "velocityX": -0.5051228216917889,
-            "velocityY": 0.9019207049574874,
-            "timestamp": 0.1241383299155809
-        },
-        {
-            "x": 2.2074803876744995,
-            "y": 4.528718281766582,
-            "heading": -0.3577885652255258,
-            "angularVelocity": 0.2562628976859123,
-            "velocityX": -0.724217005329779,
-            "velocityY": 1.3702345484796434,
-            "timestamp": 0.18620749487337135
-        },
-        {
-            "x": 2.1512700092503536,
-            "y": 4.643824218738039,
-            "heading": -0.3371682293163389,
-            "angularVelocity": 0.3322154555127859,
-            "velocityX": -0.905608742482958,
-            "velocityY": 1.8544785812676818,
-            "timestamp": 0.2482766598311618
-        },
-        {
-            "x": 2.087674884006875,
-            "y": 4.7901724592691535,
-            "heading": -0.3124919375355934,
-            "angularVelocity": 0.3975612012535503,
-            "velocityX": -1.0245848367214008,
-            "velocityY": 2.357825187926933,
-            "timestamp": 0.31034582478895223
-        },
-        {
-            "x": 2.0237484850052736,
-            "y": 4.968633105417482,
-            "heading": -0.2850705208032402,
-            "angularVelocity": 0.44178807225385713,
-            "velocityX": -1.029922007895696,
-            "velocityY": 2.8751900604698215,
-            "timestamp": 0.3724149897467427
-        },
-        {
-            "x": 1.9724195760606846,
-            "y": 5.1766438961084935,
-            "heading": -0.25765844821795814,
-            "angularVelocity": 0.4416375281458893,
-            "velocityX": -0.8269630980122488,
-            "velocityY": 3.351274192787545,
-            "timestamp": 0.43448415470453317
-        },
-        {
-            "x": 1.9484287592243066,
-            "y": 5.401486281993408,
-            "heading": -0.23420864800659155,
-            "angularVelocity": 0.3778011227958159,
-            "velocityX": -0.38651747373332257,
-            "velocityY": 3.6224490217904175,
-            "timestamp": 0.49655331966232363
-        },
-        {
-            "x": 1.9564542032821701,
-            "y": 5.628890453578366,
-            "heading": -0.21272720707613346,
-            "angularVelocity": 0.34608876961188295,
-            "velocityX": 0.12929840547219124,
-            "velocityY": 3.663722103228105,
-            "timestamp": 0.5586224846201141
-        },
-        {
-            "x": 1.99619273658118,
-            "y": 5.8512380599288125,
-            "heading": -0.18843692284079885,
-            "angularVelocity": 0.39134221077502945,
-            "velocityX": 0.6402298681838031,
-            "velocityY": 3.5822554806643043,
-            "timestamp": 0.6206916495779046
-        },
-        {
-            "x": 2.0663437221615903,
-            "y": 6.063242956223716,
-            "heading": -0.16364621155748288,
-            "angularVelocity": 0.3994046206362532,
-            "velocityX": 1.130206691653697,
-            "velocityY": 3.4156234651942974,
-            "timestamp": 0.682760814535695
-        },
-        {
-            "x": 2.1656765937805176,
-            "y": 6.261821746826172,
-            "heading": -0.13977158803318993,
-            "angularVelocity": 0.38464547638239754,
-            "velocityX": 1.6003577893593972,
-            "velocityY": 3.1993146796407252,
-            "timestamp": 0.7448299794934855
-        },
-        {
-            "x": 2.225814155156259,
-            "y": 6.362141570414172,
-            "heading": -0.12770962458788607,
-            "angularVelocity": 0.3689752694416071,
-            "velocityX": 1.8396070434406133,
-            "velocityY": 3.068781803712599,
-            "timestamp": 0.7775204182423698
-        },
-        {
-            "x": 2.2936460979223643,
-            "y": 6.4579721340232545,
-            "heading": -0.11638786741998439,
-            "angularVelocity": 0.3463323712222401,
-            "velocityX": 2.074978047465827,
-            "velocityY": 2.9314554125487073,
-            "timestamp": 0.8102108569912542
-        },
-        {
-            "x": 2.3690076803030626,
-            "y": 6.549046337769426,
-            "heading": -0.10608509188235166,
-            "angularVelocity": 0.31516173940473013,
-            "velocityX": 2.3053096031972573,
-            "velocityY": 2.7859584401928488,
-            "timestamp": 0.8429012957401385
-        },
-        {
-            "x": 2.4516786592945876,
-            "y": 6.635038598364521,
-            "heading": -0.09714822011858378,
-            "angularVelocity": 0.27337876474752265,
-            "velocityX": 2.528903928955548,
-            "velocityY": 2.6305018802439832,
-            "timestamp": 0.8755917344890228
-        },
-        {
-            "x": 2.5413546021547084,
-            "y": 6.715546857152131,
-            "heading": -0.09001694608551258,
-            "angularVelocity": 0.21814555896345397,
-            "velocityX": 2.7431856619836545,
-            "velocityY": 2.462746352407108,
-            "timestamp": 0.9082821732379072
-        },
-        {
-            "x": 2.6375981860796323,
-            "y": 6.79006985383775,
-            "heading": -0.08525828181365991,
-            "angularVelocity": 0.14556746421386957,
-            "velocityX": 2.9440896974181805,
-            "velocityY": 2.2796572801553694,
-            "timestamp": 0.9409726119867915
-        },
-        {
-            "x": 2.73984178672467,
-            "y": 6.858044565031264,
-            "heading": -0.08306946201564708,
-            "angularVelocity": 0.06695596274838787,
-            "velocityX": 3.1276301131997113,
-            "velocityY": 2.079345331378129,
-            "timestamp": 0.9736630507356758
-        },
-        {
-            "x": 2.847369129600613,
-            "y": 6.918863246924361,
-            "heading": -0.08274471887343776,
-            "angularVelocity": 0.009933887583435854,
-            "velocityX": 3.2892597037781757,
-            "velocityY": 1.8604425091962247,
-            "timestamp": 1.0063534894845603
-        },
-        {
-            "x": 2.9591159211859868,
-            "y": 6.971834136920771,
-            "heading": -0.08274467756065068,
-            "angularVelocity": 0.0000012637574442965605,
-            "velocityX": 3.418332572524967,
-            "velocityY": 1.6203786802362972,
-            "timestamp": 1.0390439282334447
-        },
-        {
-            "x": 3.0743869446602656,
-            "y": 7.0166209198254075,
-            "heading": -0.0827446619031275,
-            "angularVelocity": 4.789633846658565e-7,
-            "velocityX": 3.526138769803835,
-            "velocityY": 1.3700269748141811,
-            "timestamp": 1.0717343669823292
-        },
-        {
-            "x": 3.192583723464696,
-            "y": 7.052991069898469,
-            "heading": -0.08274464937093684,
-            "angularVelocity": 3.833595125790563e-7,
-            "velocityX": 3.61563757869459,
-            "velocityY": 1.1125623107197633,
-            "timestamp": 1.1044248057312136
-        },
-        {
-            "x": 3.313092599420772,
-            "y": 7.080755764680444,
-            "heading": -0.08274463888121661,
-            "angularVelocity": 3.208803751652956e-7,
-            "velocityX": 3.6863645936910636,
-            "velocityY": 0.8493215706001247,
-            "timestamp": 1.137115244480098
-        },
-        {
-            "x": 3.4352879115666495,
-            "y": 7.0997708632875725,
-            "heading": -0.08274462977736693,
-            "angularVelocity": 2.7848661636087506e-7,
-            "velocityX": 3.7379526498424447,
-            "velocityY": 0.5816715631449295,
-            "timestamp": 1.1698056832289825
-        },
-        {
-            "x": 3.5585352449602814,
-            "y": 7.109937657437644,
-            "heading": -0.08274462163132143,
-            "angularVelocity": 2.491874023135916e-7,
-            "velocityX": 3.770133963033834,
-            "velocityY": 0.31100207091115845,
-            "timestamp": 1.202496121977867
-        },
-        {
-            "x": 3.6821947259931562,
-            "y": 7.1112033907952945,
-            "heading": -0.08274461414532981,
-            "angularVelocity": 2.2899636479565772e-7,
-            "velocityX": 3.782741552745206,
-            "velocityY": 0.03871876321990418,
-            "timestamp": 1.2351865607267514
-        },
-        {
-            "x": 3.8056243495254822,
-            "y": 7.103561554173401,
-            "heading": -0.08274460709859606,
-            "angularVelocity": 2.1555947310703947e-7,
-            "velocityX": 3.775710215468651,
-            "velocityY": -0.23376366040158306,
-            "timestamp": 1.2678769994756358
-        },
-        {
-            "x": 3.9281833350664277,
-            "y": 7.087052015079957,
-            "heading": -0.08274460031581693,
-            "angularVelocity": 2.0748510569540933e-7,
-            "velocityX": 3.7490774131963343,
-            "velocityY": -0.5050265375893709,
-            "timestamp": 1.3005674382245203
-        },
-        {
-            "x": 4.0492357154991785,
-            "y": 7.0617618768818975,
-            "heading": -0.08274459364684544,
-            "angularVelocity": 2.0400373145008536e-7,
-            "velocityX": 3.7029903869628296,
-            "velocityY": -0.7736249241717706,
-            "timestamp": 1.3332578769734047
-        },
-        {
-            "x": 4.169706806774688,
-            "y": 7.033833646086136,
-            "heading": -0.0827445869919534,
-            "angularVelocity": 2.0357304165195936e-7,
-            "velocityX": 3.6852087609158533,
-            "velocityY": -0.8543241346506245,
-            "timestamp": 1.3659483157222891
-        },
-        {
-            "x": 4.290177832890272,
-            "y": 7.00590513421804,
-            "heading": -0.08274458033705716,
-            "angularVelocity": 2.0357317018224517e-7,
-            "velocityX": 3.6852067676748597,
-            "velocityY": -0.8543327326486478,
-            "timestamp": 1.3986387544711736
-        },
-        {
-            "x": 4.410648856777724,
-            "y": 6.977976612738755,
-            "heading": -0.08274457368215653,
-            "angularVelocity": 2.0357330400976238e-7,
-            "velocityX": 3.685206699516314,
-            "velocityY": -0.8543330266547546,
-            "timestamp": 1.431329193220058
-        },
-        {
-            "x": 4.531119878449975,
-            "y": 6.950048081704109,
-            "heading": -0.08274456702725173,
-            "angularVelocity": 2.0357343141081232e-7,
-            "velocityX": 3.6852066317533563,
-            "velocityY": -0.8543333189530982,
-            "timestamp": 1.4640196319689425
-        },
-        {
-            "x": 4.651590897922017,
-            "y": 6.922119541178764,
-            "heading": -0.082744560372343,
-            "angularVelocity": 2.0357355233562972e-7,
-            "velocityX": 3.6852065644490066,
-            "velocityY": -0.8543336092734171,
-            "timestamp": 1.496710070717827
-        },
-        {
-            "x": 4.772061915216332,
-            "y": 6.894190991259702,
-            "heading": -0.08274455371743049,
-            "angularVelocity": 2.0357366776279015e-7,
-            "velocityX": 3.68520649783238,
-            "velocityY": -0.8543338966271132,
-            "timestamp": 1.5294005094667114
-        },
-        {
-            "x": 4.892532930362486,
-            "y": 6.866262432074455,
-            "heading": -0.08274454706251447,
-            "angularVelocity": 2.0357377502591166e-7,
-            "velocityX": 3.6852064321201583,
-            "velocityY": -0.8543341800795031,
-            "timestamp": 1.5620909482155958
-        },
-        {
-            "x": 5.0130039433965745,
-            "y": 6.838333863778735,
-            "heading": -0.08274454040759514,
-            "angularVelocity": 2.0357387593869802e-7,
-            "velocityX": 3.685206367512119,
-            "velocityY": -0.854334458768776,
-            "timestamp": 1.5947813869644802
-        },
-        {
-            "x": 5.133474954360567,
-            "y": 6.810405286553609,
-            "heading": -0.08274453375267282,
-            "angularVelocity": 2.0357396812707888e-7,
-            "velocityX": 3.6852063041879153,
-            "velocityY": -0.8543347319191732,
-            "timestamp": 1.6274718257133647
-        },
-        {
-            "x": 5.253945963272805,
-            "y": 6.782476700477742,
-            "heading": -0.08274452709774775,
-            "angularVelocity": 2.035740515232947e-7,
-            "velocityX": 3.6852062414248348,
-            "velocityY": -0.8543350026628553,
-            "timestamp": 1.6601622644622491
-        },
-        {
-            "x": 5.374416652936201,
-            "y": 6.754546737348104,
-            "heading": -0.08274452044281651,
-            "angularVelocity": 2.0357424100563973e-7,
-            "velocityX": 3.685196475607398,
-            "velocityY": -0.8543771267177642,
-            "timestamp": 1.6928527032111336
-        },
-        {
-            "x": 5.493278503417969,
-            "y": 6.720412731170654,
-            "heading": -0.08274451374626977,
-            "angularVelocity": 2.0484725764732438e-7,
-            "velocityX": 3.6359821106976624,
-            "velocityY": -1.0441587046150123,
-            "timestamp": 1.725543141960018
-        },
-        {
-            "x": 5.6078450645507765,
-            "y": 6.6784969077855525,
-            "heading": -0.0827445073182203,
-            "angularVelocity": 1.9932954157588389e-7,
-            "velocityX": 3.5526329088991675,
-            "velocityY": -1.2997818219306485,
-            "timestamp": 1.7577914952435287
-        },
-        {
-            "x": 5.720102544926552,
-            "y": 6.630740323854381,
-            "heading": -0.08274450093221485,
-            "angularVelocity": 1.980257845223315e-7,
-            "velocityX": 3.481029849458213,
-            "velocityY": -1.4808999241394636,
-            "timestamp": 1.7900398485270395
-        },
-        {
-            "x": 5.832359462510262,
-            "y": 6.582982417035901,
-            "heading": -0.08274449454621227,
-            "angularVelocity": 1.9802569455219195e-7,
-            "velocityX": 3.481012397650423,
-            "velocityY": -1.480940945995501,
-            "timestamp": 1.8222882018105502
-        },
-        {
-            "x": 5.944616376948871,
-            "y": 6.535224502824747,
-            "heading": -0.08274448816020867,
-            "angularVelocity": 1.9802572730548282e-7,
-            "velocityX": 3.4810123001228868,
-            "velocityY": -1.4809411752374333,
-            "timestamp": 1.854536555094061
-        },
-        {
-            "x": 6.0568732883758765,
-            "y": 6.487466581534697,
-            "heading": -0.0827444817742042,
-            "angularVelocity": 1.9802575385283685e-7,
-            "velocityX": 3.4810122067350586,
-            "velocityY": -1.4809413947492354,
-            "timestamp": 1.8867849083775716
-        },
-        {
-            "x": 6.169130196876316,
-            "y": 6.439708653365641,
-            "heading": -0.08274447538819907,
-            "angularVelocity": 1.980257744166014e-7,
-            "velocityX": 3.4810121159842042,
-            "velocityY": -1.4809416080626625,
-            "timestamp": 1.9190332616610823
-        },
-        {
-            "x": 6.281387102537832,
-            "y": 6.391950718523587,
-            "heading": -0.08274446900219351,
-            "angularVelocity": 1.9802578764982062e-7,
-            "velocityX": 3.481012027951073,
-            "velocityY": -1.4809418149879348,
-            "timestamp": 1.951281614944593
-        },
-        {
-            "x": 6.393644005449787,
-            "y": 6.344192777218584,
-            "heading": -0.08274446261618765,
-            "angularVelocity": 1.9802579747327125e-7,
-            "velocityX": 3.481011942689016,
-            "velocityY": -1.4809420153996509,
-            "timestamp": 1.9835299682281038
-        },
-        {
-            "x": 6.505900905702341,
-            "y": 6.296434829662559,
-            "heading": -0.08274445623018166,
-            "angularVelocity": 1.9802580083930334e-7,
-            "velocityX": 3.4810118602227385,
-            "velocityY": -1.4809422092397408,
-            "timestamp": 2.0157783215116143
-        },
-        {
-            "x": 6.618157803385852,
-            "y": 6.2486768760679015,
-            "heading": -0.08274444984417571,
-            "angularVelocity": 1.9802579953185327e-7,
-            "velocityX": 3.4810117805584304,
-            "velocityY": -1.4809423964936643,
-            "timestamp": 2.048026674795125
-        },
-        {
-            "x": 6.730414698590335,
-            "y": 6.200918916646197,
-            "heading": -0.08274444345816992,
-            "angularVelocity": 1.9802579502845229e-7,
-            "velocityX": 3.4810117036854167,
-            "velocityY": -1.4809425771865148,
-            "timestamp": 2.0802750280786353
-        },
-        {
-            "x": 6.842671592155369,
-            "y": 6.153160953370972,
-            "heading": -0.08274443707212797,
-            "angularVelocity": 1.980269160510039e-7,
-            "velocityX": 3.4810116528472412,
-            "velocityY": -1.4809426966816255,
-            "timestamp": 2.112523381362146
-        },
-        {
-            "x": 6.954235049253699,
-            "y": 6.105720053055757,
-            "heading": -0.08066018407058251,
-            "angularVelocity": 0.06463130018523208,
-            "velocityX": 3.459508648938503,
-            "velocityY": -1.4711107850419303,
-            "timestamp": 2.1447717346456563
-        },
-        {
-            "x": 7.05792262192444,
-            "y": 6.061617369901677,
-            "heading": -0.07347223812764758,
-            "angularVelocity": 0.2228934258982459,
-            "velocityX": 3.215282707900597,
-            "velocityY": -1.3675948897716332,
-            "timestamp": 2.177020087929167
-        },
-        {
-            "x": 7.153639251658588,
-            "y": 6.020903567478063,
-            "heading": -0.06480991744029473,
-            "angularVelocity": 0.2686128067130366,
-            "velocityX": 2.9681090656833784,
-            "velocityY": -1.2625079508922343,
-            "timestamp": 2.2092684412126773
-        },
-        {
-            "x": 7.241376863236811,
-            "y": 5.983582900712933,
-            "heading": -0.05573995757660787,
-            "angularVelocity": 0.28125342661540725,
-            "velocityX": 2.7206850162822076,
-            "velocityY": -1.1572890695232254,
-            "timestamp": 2.241516794496188
-        },
-        {
-            "x": 7.321134939051253,
-            "y": 5.949655957408732,
-            "heading": -0.04677486060395727,
-            "angularVelocity": 0.2780016980662186,
-            "velocityX": 2.4732449162054873,
-            "velocityY": -1.0520519607910965,
-            "timestamp": 2.2737651477796983
-        },
-        {
-            "x": 7.392914197093691,
-            "y": 5.919122647933878,
-            "heading": -0.03821600737741098,
-            "angularVelocity": 0.26540434952773495,
-            "velocityX": 2.2258270805765674,
-            "velocityY": -0.946817631474723,
-            "timestamp": 2.306013501063209
-        },
-        {
-            "x": 7.456715523277294,
-            "y": 5.891982736437705,
-            "heading": -0.030261865024148932,
-            "angularVelocity": 0.24665266729552143,
-            "velocityX": 1.9784367165261438,
-            "velocityY": -0.8415906157307577,
-            "timestamp": 2.3382618543467193
-        },
-        {
-            "x": 7.512539743308544,
-            "y": 5.868235970303718,
-            "heading": -0.023053012750234723,
-            "angularVelocity": 0.2235417173255794,
-            "velocityX": 1.7310719570848203,
-            "velocityY": -0.736371433456386,
-            "timestamp": 2.37051020763023
-        },
-        {
-            "x": 7.560387579465668,
-            "y": 5.847882114762634,
-            "heading": -0.016694240391467878,
-            "angularVelocity": 0.19718130420078028,
-            "velocityX": 1.4837295950113008,
-            "velocityY": -0.6311595312214304,
-            "timestamp": 2.4027585609137403
-        },
-        {
-            "x": 7.600259653459209,
-            "y": 5.830920960662069,
-            "heading": -0.011266668966831511,
-            "angularVelocity": 0.1683053821980557,
-            "velocityX": 1.2364065117684333,
-            "velocityY": -0.5259541146628961,
-            "timestamp": 2.435006914197251
-        },
-        {
-            "x": 7.632156499892169,
-            "y": 5.817352324088122,
-            "heading": -0.006834955017927562,
-            "angularVelocity": 0.1374245038171387,
-            "velocityX": 0.9891000062092736,
-            "velocityY": -0.4207544011521409,
-            "timestamp": 2.4672552674807613
-        },
-        {
-            "x": 7.656078580521334,
-            "y": 5.807176043662066,
-            "heading": -0.0034518424131983775,
-            "angularVelocity": 0.10490807313441347,
-            "velocityX": 0.7418078194211917,
-            "velocityY": -0.31555969188850075,
-            "timestamp": 2.499503620764272
-        },
-        {
-            "x": 7.672026296758129,
-            "y": 5.800391977434502,
-            "heading": -0.0011611797809329624,
-            "angularVelocity": 0.07103192563441285,
-            "velocityX": 0.49452808013452626,
-            "velocityY": -0.21036938438134845,
-            "timestamp": 2.5317519740477823
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 1.9934013230001416e-31,
-            "angularVelocity": 0.036007413176266045,
-            "velocityX": 0.2472592374491084,
-            "velocityY": -0.10518296561320875,
-            "timestamp": 2.564000327331293
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 1.0109342582388432e-32,
-            "angularVelocity": -5.855934233748412e-30,
-            "velocityX": 8.513095472636782e-32,
-            "velocityY": -2.1397535642954256e-34,
-            "timestamp": 2.5962486806148033
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.1.traj
deleted file mode 100644
index 9c22ef7a..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.1.traj	
+++ /dev/null
@@ -1,400 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 1.5561411635921053e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2879322251049214,
-            "y": 4.397976336389526,
-            "heading": -0.38546801638598305,
-            "angularVelocity": 0.06554072441421986,
-            "velocityX": -0.17452197008732642,
-            "velocityY": 0.5492068837058847,
-            "timestamp": 0.06914759722835051
-        },
-        {
-            "x": 2.264556974735031,
-            "y": 4.474163552813777,
-            "heading": -0.3765424472118302,
-            "angularVelocity": 0.12907996129426796,
-            "velocityX": -0.3380486279621587,
-            "velocityY": 1.1018056950361832,
-            "timestamp": 0.13829519445670102
-        },
-        {
-            "x": 2.231004605798313,
-            "y": 4.588879932488501,
-            "heading": -0.3634307413762601,
-            "angularVelocity": 0.18961910985573333,
-            "velocityX": -0.48522826940697783,
-            "velocityY": 1.6590074604604372,
-            "timestamp": 0.20744279168505153
-        },
-        {
-            "x": 2.1891253320707884,
-            "y": 4.7425700174397605,
-            "heading": -0.3464768905998804,
-            "angularVelocity": 0.24518351259275634,
-            "velocityX": -0.6056504550588213,
-            "velocityY": 2.2226381119759067,
-            "timestamp": 0.27659038891340204
-        },
-        {
-            "x": 2.142455487111455,
-            "y": 4.9358291258163405,
-            "heading": -0.3263533789303141,
-            "angularVelocity": 0.29102257309767376,
-            "velocityX": -0.6749308266671239,
-            "velocityY": 2.7948781465017833,
-            "timestamp": 0.34573798614175255
-        },
-        {
-            "x": 2.099906819940071,
-            "y": 5.168740583772433,
-            "heading": -0.30483662973794956,
-            "angularVelocity": 0.3111713212639373,
-            "velocityX": -0.6153311015404023,
-            "velocityY": 3.3683232287448175,
-            "timestamp": 0.41488558337010306
-        },
-        {
-            "x": 2.090216091849313,
-            "y": 5.4241817814008675,
-            "heading": -0.28862249413017305,
-            "angularVelocity": 0.23448588609832868,
-            "velocityX": -0.14014555066512774,
-            "velocityY": 3.69414423446863,
-            "timestamp": 0.48403318059845357
-        },
-        {
-            "x": 2.1169158362249996,
-            "y": 5.665111810235099,
-            "heading": -0.24564536248803095,
-            "angularVelocity": 0.6215274769324497,
-            "velocityX": 0.3861268568382121,
-            "velocityY": 3.484286345315426,
-            "timestamp": 0.5531807778268041
-        },
-        {
-            "x": 2.1721169779622786,
-            "y": 5.878339660700231,
-            "heading": -0.1915552026069021,
-            "angularVelocity": 0.7822420741739773,
-            "velocityX": 0.7983088921367857,
-            "velocityY": 3.08366247001803,
-            "timestamp": 0.6223283750551546
-        },
-        {
-            "x": 2.2518565055578685,
-            "y": 6.060184802876031,
-            "heading": -0.1318933186992544,
-            "angularVelocity": 0.8628193357100991,
-            "velocityX": 1.1531785744091572,
-            "velocityY": 2.629811439074749,
-            "timestamp": 0.6914759722835051
-        },
-        {
-            "x": 2.3540132373645406,
-            "y": 6.209070989598032,
-            "heading": -0.06941508964740774,
-            "angularVelocity": 0.9035488079854361,
-            "velocityX": 1.4773721127192665,
-            "velocityY": 2.153165007746782,
-            "timestamp": 0.7606235695118556
-        },
-        {
-            "x": 2.477290153503418,
-            "y": 6.3241448402404785,
-            "heading": -0.005748673882586236,
-            "angularVelocity": 0.9207321485592678,
-            "velocityX": 1.782808384965753,
-            "velocityY": 1.6641771407079455,
-            "timestamp": 0.8297711667402061
-        },
-        {
-            "x": 2.6074231512941006,
-            "y": 6.400809151237731,
-            "heading": 0.05275689385662717,
-            "angularVelocity": 0.9223034785287629,
-            "velocityX": 2.051464863433381,
-            "velocityY": 1.20856464509814,
-            "timestamp": 0.8932053498501396
-        },
-        {
-            "x": 2.7533092580755354,
-            "y": 6.447850328813001,
-            "heading": 0.11062964933050726,
-            "angularVelocity": 0.9123275911695883,
-            "velocityX": 2.2998027188043615,
-            "velocityY": 0.7415745780760178,
-            "timestamp": 0.956639532960073
-        },
-        {
-            "x": 2.9132889651123484,
-            "y": 6.4644464131499,
-            "heading": 0.16695291665745207,
-            "angularVelocity": 0.8879008850767368,
-            "velocityX": 2.5219794627047807,
-            "velocityY": 0.26162683151755056,
-            "timestamp": 1.0200737160700064
-        },
-        {
-            "x": 3.0851729747755345,
-            "y": 6.449682823375658,
-            "heading": 0.22055798508605634,
-            "angularVelocity": 0.8450501890441832,
-            "velocityX": 2.7096433064356162,
-            "velocityY": -0.23273870727739393,
-            "timestamp": 1.0835078991799398
-        },
-        {
-            "x": 3.266000929743579,
-            "y": 6.40260327654133,
-            "heading": 0.269929363572885,
-            "angularVelocity": 0.7783087298743283,
-            "velocityX": 2.8506389789001907,
-            "velocityY": -0.7421794453112333,
-            "timestamp": 1.1469420822898733
-        },
-        {
-            "x": 3.4516899170753974,
-            "y": 6.322371527535034,
-            "heading": 0.31309009761375267,
-            "angularVelocity": 0.6804018263443473,
-            "velocityX": 2.9272700967841274,
-            "velocityY": -1.2648030615800407,
-            "timestamp": 1.2103762653998067
-        },
-        {
-            "x": 3.636598494450249,
-            "y": 6.208685958737944,
-            "heading": 0.3475353077130109,
-            "angularVelocity": 0.5430070730006203,
-            "velocityX": 2.9149674246500377,
-            "velocityY": -1.7921814899081216,
-            "timestamp": 1.2738104485097401
-        },
-        {
-            "x": 3.8132641934440024,
-            "y": 6.062639398740876,
-            "heading": 0.3704029865204341,
-            "angularVelocity": 0.36049457383033545,
-            "velocityX": 2.785023631307647,
-            "velocityY": -2.3023321628339066,
-            "timestamp": 1.3372446316196736
-        },
-        {
-            "x": 3.9730355739593506,
-            "y": 5.887885093688965,
-            "heading": 0.37894290950178183,
-            "angularVelocity": 0.1346265146219495,
-            "velocityX": 2.5186953261297353,
-            "velocityY": -2.754891707981806,
-            "timestamp": 1.400678814729607
-        },
-        {
-            "x": 4.101195684299111,
-            "y": 5.704704428790184,
-            "heading": 0.3789451050812407,
-            "angularVelocity": 0.000037151619885842266,
-            "velocityX": 2.1686126132704535,
-            "velocityY": -3.099622022424022,
-            "timestamp": 1.459776557478272
-        },
-        {
-            "x": 4.204458426061581,
-            "y": 5.506418480888323,
-            "heading": 0.3789451145933903,
-            "angularVelocity": 1.6095622464276883e-7,
-            "velocityX": 1.7473212505255857,
-            "velocityY": -3.3552203295641,
-            "timestamp": 1.518874300226937
-        },
-        {
-            "x": 4.281071316930325,
-            "y": 5.296392335982766,
-            "heading": 0.3789451203460425,
-            "angularVelocity": 9.73413180945881e-8,
-            "velocityX": 1.2963759241186865,
-            "velocityY": -3.5538776125279528,
-            "timestamp": 1.577972042975602
-        },
-        {
-            "x": 4.344371609432443,
-            "y": 5.081977806335691,
-            "heading": 0.37894512554853116,
-            "angularVelocity": 8.803193422611526e-8,
-            "velocityX": 1.0711118489099893,
-            "velocityY": -3.628133997587826,
-            "timestamp": 1.6370697857242669
-        },
-        {
-            "x": 4.4350051048376455,
-            "y": 4.877610290730759,
-            "heading": 0.3789451268821095,
-            "angularVelocity": 2.2565639514634894e-8,
-            "velocityX": 1.5336202567072863,
-            "velocityY": -3.4581272667918515,
-            "timestamp": 1.6961675284729318
-        },
-        {
-            "x": 4.551081441257298,
-            "y": 4.6873967879425225,
-            "heading": 0.3767623699353526,
-            "angularVelocity": -0.03693469235752831,
-            "velocityX": 1.964141624038618,
-            "velocityY": -3.218625516664622,
-            "timestamp": 1.7552652712215968
-        },
-        {
-            "x": 4.687723026941447,
-            "y": 4.517482449028109,
-            "heading": 0.36165339974597627,
-            "angularVelocity": -0.25566069848849177,
-            "velocityX": 2.312128675793254,
-            "velocityY": -2.875140927749109,
-            "timestamp": 1.8143630139702618
-        },
-        {
-            "x": 4.8416817092622,
-            "y": 4.370738170800582,
-            "heading": 0.3345492154544829,
-            "angularVelocity": -0.45863315638965824,
-            "velocityX": 2.6051533469768047,
-            "velocityY": -2.483077549196378,
-            "timestamp": 1.8734607567189268
-        },
-        {
-            "x": 5.0108564738059584,
-            "y": 4.248670932935222,
-            "heading": 0.2976430739087661,
-            "angularVelocity": -0.6244932518551631,
-            "velocityX": 2.8626265010319796,
-            "velocityY": -2.065514386638646,
-            "timestamp": 1.9325584994675917
-        },
-        {
-            "x": 5.194237326297875,
-            "y": 4.152011519418658,
-            "heading": 0.2577973678603408,
-            "angularVelocity": -0.6742339757109184,
-            "velocityX": 3.103009420708511,
-            "velocityY": -1.635585540509506,
-            "timestamp": 1.9916562422162567
-        },
-        {
-            "x": 5.390789031982422,
-            "y": 4.081310272216797,
-            "heading": 0.22104423660220585,
-            "angularVelocity": -0.6219041464028032,
-            "velocityX": 3.3258750088027327,
-            "velocityY": -1.1963442919045442,
-            "timestamp": 2.050753984964922
-        },
-        {
-            "x": 5.67793189235796,
-            "y": 4.03407988691648,
-            "heading": 0.19119773345256094,
-            "angularVelocity": -0.37371337139653454,
-            "velocityX": 3.5953667967399574,
-            "velocityY": -0.5913800499300379,
-            "timestamp": 2.130618676555184
-        },
-        {
-            "x": 5.980039011502812,
-            "y": 4.037197033313024,
-            "heading": 0.19119714274274896,
-            "angularVelocity": -0.000007396382559589573,
-            "velocityX": 3.782736940809626,
-            "velocityY": 0.039030344129324,
-            "timestamp": 2.210483368145446
-        },
-        {
-            "x": 6.281741681649569,
-            "y": 4.053135447217484,
-            "heading": 0.19119713453456852,
-            "angularVelocity": -1.0277608664279704e-7,
-            "velocityX": 3.777672763010384,
-            "velocityY": 0.1995677136803443,
-            "timestamp": 2.290348059735708
-        },
-        {
-            "x": 6.583444349801777,
-            "y": 4.069073898877065,
-            "heading": 0.19119712632638652,
-            "angularVelocity": -1.0277610566434499e-7,
-            "velocityX": 3.777672738036278,
-            "velocityY": 0.1995681864189316,
-            "timestamp": 2.3702127513259703
-        },
-        {
-            "x": 6.885147017932413,
-            "y": 4.08501235094329,
-            "heading": 0.1911971181180728,
-            "angularVelocity": -1.0277775515776054e-7,
-            "velocityX": 3.7776727377661725,
-            "velocityY": 0.1995681915106077,
-            "timestamp": 2.4500774429162324
-        },
-        {
-            "x": 7.150097958068984,
-            "y": 4.099008563262217,
-            "heading": 0.12746367267042205,
-            "angularVelocity": -0.7980177995885095,
-            "velocityX": 3.317497818634029,
-            "velocityY": 0.17524906238576554,
-            "timestamp": 2.5299421345064945
-        },
-        {
-            "x": 7.362058748903403,
-            "y": 4.110205268667911,
-            "heading": 0.07647732188798662,
-            "angularVelocity": -0.6384091613855594,
-            "velocityX": 2.6539987397918514,
-            "velocityY": 0.14019593869007996,
-            "timestamp": 2.6098068260967566
-        },
-        {
-            "x": 7.521029367100823,
-            "y": 4.118602666583934,
-            "heading": 0.03823840686026882,
-            "angularVelocity": -0.47879625234031903,
-            "velocityX": 1.9904993687699384,
-            "velocityY": 0.10514531201226385,
-            "timestamp": 2.6896715176870187
-        },
-        {
-            "x": 7.627009789128585,
-            "y": 4.124200888756926,
-            "heading": 0.012746172901683662,
-            "angularVelocity": -0.31919279284744,
-            "velocityX": 1.3269997030913787,
-            "velocityY": 0.07009633495752653,
-            "timestamp": 2.769536209277281
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 1.6188506035771725e-33,
-            "angularVelocity": -0.15959709663791943,
-            "velocityX": 0.6634998497618533,
-            "velocityY": 0.03504816943929136,
-            "timestamp": 2.849400900867543
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -1.4796214296723179e-31,
-            "velocityX": -2.3067017155365004e-33,
-            "velocityY": 0,
-            "timestamp": 2.929265592457805
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.traj
deleted file mode 100644
index 9c22ef7a..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.traj	
+++ /dev/null
@@ -1,400 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 1.5561411635921053e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2879322251049214,
-            "y": 4.397976336389526,
-            "heading": -0.38546801638598305,
-            "angularVelocity": 0.06554072441421986,
-            "velocityX": -0.17452197008732642,
-            "velocityY": 0.5492068837058847,
-            "timestamp": 0.06914759722835051
-        },
-        {
-            "x": 2.264556974735031,
-            "y": 4.474163552813777,
-            "heading": -0.3765424472118302,
-            "angularVelocity": 0.12907996129426796,
-            "velocityX": -0.3380486279621587,
-            "velocityY": 1.1018056950361832,
-            "timestamp": 0.13829519445670102
-        },
-        {
-            "x": 2.231004605798313,
-            "y": 4.588879932488501,
-            "heading": -0.3634307413762601,
-            "angularVelocity": 0.18961910985573333,
-            "velocityX": -0.48522826940697783,
-            "velocityY": 1.6590074604604372,
-            "timestamp": 0.20744279168505153
-        },
-        {
-            "x": 2.1891253320707884,
-            "y": 4.7425700174397605,
-            "heading": -0.3464768905998804,
-            "angularVelocity": 0.24518351259275634,
-            "velocityX": -0.6056504550588213,
-            "velocityY": 2.2226381119759067,
-            "timestamp": 0.27659038891340204
-        },
-        {
-            "x": 2.142455487111455,
-            "y": 4.9358291258163405,
-            "heading": -0.3263533789303141,
-            "angularVelocity": 0.29102257309767376,
-            "velocityX": -0.6749308266671239,
-            "velocityY": 2.7948781465017833,
-            "timestamp": 0.34573798614175255
-        },
-        {
-            "x": 2.099906819940071,
-            "y": 5.168740583772433,
-            "heading": -0.30483662973794956,
-            "angularVelocity": 0.3111713212639373,
-            "velocityX": -0.6153311015404023,
-            "velocityY": 3.3683232287448175,
-            "timestamp": 0.41488558337010306
-        },
-        {
-            "x": 2.090216091849313,
-            "y": 5.4241817814008675,
-            "heading": -0.28862249413017305,
-            "angularVelocity": 0.23448588609832868,
-            "velocityX": -0.14014555066512774,
-            "velocityY": 3.69414423446863,
-            "timestamp": 0.48403318059845357
-        },
-        {
-            "x": 2.1169158362249996,
-            "y": 5.665111810235099,
-            "heading": -0.24564536248803095,
-            "angularVelocity": 0.6215274769324497,
-            "velocityX": 0.3861268568382121,
-            "velocityY": 3.484286345315426,
-            "timestamp": 0.5531807778268041
-        },
-        {
-            "x": 2.1721169779622786,
-            "y": 5.878339660700231,
-            "heading": -0.1915552026069021,
-            "angularVelocity": 0.7822420741739773,
-            "velocityX": 0.7983088921367857,
-            "velocityY": 3.08366247001803,
-            "timestamp": 0.6223283750551546
-        },
-        {
-            "x": 2.2518565055578685,
-            "y": 6.060184802876031,
-            "heading": -0.1318933186992544,
-            "angularVelocity": 0.8628193357100991,
-            "velocityX": 1.1531785744091572,
-            "velocityY": 2.629811439074749,
-            "timestamp": 0.6914759722835051
-        },
-        {
-            "x": 2.3540132373645406,
-            "y": 6.209070989598032,
-            "heading": -0.06941508964740774,
-            "angularVelocity": 0.9035488079854361,
-            "velocityX": 1.4773721127192665,
-            "velocityY": 2.153165007746782,
-            "timestamp": 0.7606235695118556
-        },
-        {
-            "x": 2.477290153503418,
-            "y": 6.3241448402404785,
-            "heading": -0.005748673882586236,
-            "angularVelocity": 0.9207321485592678,
-            "velocityX": 1.782808384965753,
-            "velocityY": 1.6641771407079455,
-            "timestamp": 0.8297711667402061
-        },
-        {
-            "x": 2.6074231512941006,
-            "y": 6.400809151237731,
-            "heading": 0.05275689385662717,
-            "angularVelocity": 0.9223034785287629,
-            "velocityX": 2.051464863433381,
-            "velocityY": 1.20856464509814,
-            "timestamp": 0.8932053498501396
-        },
-        {
-            "x": 2.7533092580755354,
-            "y": 6.447850328813001,
-            "heading": 0.11062964933050726,
-            "angularVelocity": 0.9123275911695883,
-            "velocityX": 2.2998027188043615,
-            "velocityY": 0.7415745780760178,
-            "timestamp": 0.956639532960073
-        },
-        {
-            "x": 2.9132889651123484,
-            "y": 6.4644464131499,
-            "heading": 0.16695291665745207,
-            "angularVelocity": 0.8879008850767368,
-            "velocityX": 2.5219794627047807,
-            "velocityY": 0.26162683151755056,
-            "timestamp": 1.0200737160700064
-        },
-        {
-            "x": 3.0851729747755345,
-            "y": 6.449682823375658,
-            "heading": 0.22055798508605634,
-            "angularVelocity": 0.8450501890441832,
-            "velocityX": 2.7096433064356162,
-            "velocityY": -0.23273870727739393,
-            "timestamp": 1.0835078991799398
-        },
-        {
-            "x": 3.266000929743579,
-            "y": 6.40260327654133,
-            "heading": 0.269929363572885,
-            "angularVelocity": 0.7783087298743283,
-            "velocityX": 2.8506389789001907,
-            "velocityY": -0.7421794453112333,
-            "timestamp": 1.1469420822898733
-        },
-        {
-            "x": 3.4516899170753974,
-            "y": 6.322371527535034,
-            "heading": 0.31309009761375267,
-            "angularVelocity": 0.6804018263443473,
-            "velocityX": 2.9272700967841274,
-            "velocityY": -1.2648030615800407,
-            "timestamp": 1.2103762653998067
-        },
-        {
-            "x": 3.636598494450249,
-            "y": 6.208685958737944,
-            "heading": 0.3475353077130109,
-            "angularVelocity": 0.5430070730006203,
-            "velocityX": 2.9149674246500377,
-            "velocityY": -1.7921814899081216,
-            "timestamp": 1.2738104485097401
-        },
-        {
-            "x": 3.8132641934440024,
-            "y": 6.062639398740876,
-            "heading": 0.3704029865204341,
-            "angularVelocity": 0.36049457383033545,
-            "velocityX": 2.785023631307647,
-            "velocityY": -2.3023321628339066,
-            "timestamp": 1.3372446316196736
-        },
-        {
-            "x": 3.9730355739593506,
-            "y": 5.887885093688965,
-            "heading": 0.37894290950178183,
-            "angularVelocity": 0.1346265146219495,
-            "velocityX": 2.5186953261297353,
-            "velocityY": -2.754891707981806,
-            "timestamp": 1.400678814729607
-        },
-        {
-            "x": 4.101195684299111,
-            "y": 5.704704428790184,
-            "heading": 0.3789451050812407,
-            "angularVelocity": 0.000037151619885842266,
-            "velocityX": 2.1686126132704535,
-            "velocityY": -3.099622022424022,
-            "timestamp": 1.459776557478272
-        },
-        {
-            "x": 4.204458426061581,
-            "y": 5.506418480888323,
-            "heading": 0.3789451145933903,
-            "angularVelocity": 1.6095622464276883e-7,
-            "velocityX": 1.7473212505255857,
-            "velocityY": -3.3552203295641,
-            "timestamp": 1.518874300226937
-        },
-        {
-            "x": 4.281071316930325,
-            "y": 5.296392335982766,
-            "heading": 0.3789451203460425,
-            "angularVelocity": 9.73413180945881e-8,
-            "velocityX": 1.2963759241186865,
-            "velocityY": -3.5538776125279528,
-            "timestamp": 1.577972042975602
-        },
-        {
-            "x": 4.344371609432443,
-            "y": 5.081977806335691,
-            "heading": 0.37894512554853116,
-            "angularVelocity": 8.803193422611526e-8,
-            "velocityX": 1.0711118489099893,
-            "velocityY": -3.628133997587826,
-            "timestamp": 1.6370697857242669
-        },
-        {
-            "x": 4.4350051048376455,
-            "y": 4.877610290730759,
-            "heading": 0.3789451268821095,
-            "angularVelocity": 2.2565639514634894e-8,
-            "velocityX": 1.5336202567072863,
-            "velocityY": -3.4581272667918515,
-            "timestamp": 1.6961675284729318
-        },
-        {
-            "x": 4.551081441257298,
-            "y": 4.6873967879425225,
-            "heading": 0.3767623699353526,
-            "angularVelocity": -0.03693469235752831,
-            "velocityX": 1.964141624038618,
-            "velocityY": -3.218625516664622,
-            "timestamp": 1.7552652712215968
-        },
-        {
-            "x": 4.687723026941447,
-            "y": 4.517482449028109,
-            "heading": 0.36165339974597627,
-            "angularVelocity": -0.25566069848849177,
-            "velocityX": 2.312128675793254,
-            "velocityY": -2.875140927749109,
-            "timestamp": 1.8143630139702618
-        },
-        {
-            "x": 4.8416817092622,
-            "y": 4.370738170800582,
-            "heading": 0.3345492154544829,
-            "angularVelocity": -0.45863315638965824,
-            "velocityX": 2.6051533469768047,
-            "velocityY": -2.483077549196378,
-            "timestamp": 1.8734607567189268
-        },
-        {
-            "x": 5.0108564738059584,
-            "y": 4.248670932935222,
-            "heading": 0.2976430739087661,
-            "angularVelocity": -0.6244932518551631,
-            "velocityX": 2.8626265010319796,
-            "velocityY": -2.065514386638646,
-            "timestamp": 1.9325584994675917
-        },
-        {
-            "x": 5.194237326297875,
-            "y": 4.152011519418658,
-            "heading": 0.2577973678603408,
-            "angularVelocity": -0.6742339757109184,
-            "velocityX": 3.103009420708511,
-            "velocityY": -1.635585540509506,
-            "timestamp": 1.9916562422162567
-        },
-        {
-            "x": 5.390789031982422,
-            "y": 4.081310272216797,
-            "heading": 0.22104423660220585,
-            "angularVelocity": -0.6219041464028032,
-            "velocityX": 3.3258750088027327,
-            "velocityY": -1.1963442919045442,
-            "timestamp": 2.050753984964922
-        },
-        {
-            "x": 5.67793189235796,
-            "y": 4.03407988691648,
-            "heading": 0.19119773345256094,
-            "angularVelocity": -0.37371337139653454,
-            "velocityX": 3.5953667967399574,
-            "velocityY": -0.5913800499300379,
-            "timestamp": 2.130618676555184
-        },
-        {
-            "x": 5.980039011502812,
-            "y": 4.037197033313024,
-            "heading": 0.19119714274274896,
-            "angularVelocity": -0.000007396382559589573,
-            "velocityX": 3.782736940809626,
-            "velocityY": 0.039030344129324,
-            "timestamp": 2.210483368145446
-        },
-        {
-            "x": 6.281741681649569,
-            "y": 4.053135447217484,
-            "heading": 0.19119713453456852,
-            "angularVelocity": -1.0277608664279704e-7,
-            "velocityX": 3.777672763010384,
-            "velocityY": 0.1995677136803443,
-            "timestamp": 2.290348059735708
-        },
-        {
-            "x": 6.583444349801777,
-            "y": 4.069073898877065,
-            "heading": 0.19119712632638652,
-            "angularVelocity": -1.0277610566434499e-7,
-            "velocityX": 3.777672738036278,
-            "velocityY": 0.1995681864189316,
-            "timestamp": 2.3702127513259703
-        },
-        {
-            "x": 6.885147017932413,
-            "y": 4.08501235094329,
-            "heading": 0.1911971181180728,
-            "angularVelocity": -1.0277775515776054e-7,
-            "velocityX": 3.7776727377661725,
-            "velocityY": 0.1995681915106077,
-            "timestamp": 2.4500774429162324
-        },
-        {
-            "x": 7.150097958068984,
-            "y": 4.099008563262217,
-            "heading": 0.12746367267042205,
-            "angularVelocity": -0.7980177995885095,
-            "velocityX": 3.317497818634029,
-            "velocityY": 0.17524906238576554,
-            "timestamp": 2.5299421345064945
-        },
-        {
-            "x": 7.362058748903403,
-            "y": 4.110205268667911,
-            "heading": 0.07647732188798662,
-            "angularVelocity": -0.6384091613855594,
-            "velocityX": 2.6539987397918514,
-            "velocityY": 0.14019593869007996,
-            "timestamp": 2.6098068260967566
-        },
-        {
-            "x": 7.521029367100823,
-            "y": 4.118602666583934,
-            "heading": 0.03823840686026882,
-            "angularVelocity": -0.47879625234031903,
-            "velocityX": 1.9904993687699384,
-            "velocityY": 0.10514531201226385,
-            "timestamp": 2.6896715176870187
-        },
-        {
-            "x": 7.627009789128585,
-            "y": 4.124200888756926,
-            "heading": 0.012746172901683662,
-            "angularVelocity": -0.31919279284744,
-            "velocityX": 1.3269997030913787,
-            "velocityY": 0.07009633495752653,
-            "timestamp": 2.769536209277281
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 1.6188506035771725e-33,
-            "angularVelocity": -0.15959709663791943,
-            "velocityX": 0.6634998497618533,
-            "velocityY": 0.03504816943929136,
-            "timestamp": 2.849400900867543
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -1.4796214296723179e-31,
-            "velocityX": -2.3067017155365004e-33,
-            "velocityY": 0,
-            "timestamp": 2.929265592457805
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.1.traj
deleted file mode 100644
index ce59bb95..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.1.traj	
+++ /dev/null
@@ -1,625 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.298402468716654,
-            "y": 4.3526843009931255,
-            "heading": -0.37533408415570846,
-            "angularVelocity": 0.4620245693037669,
-            "velocityX": -0.050327487964169394,
-            "velocityY": -0.2304685720750862,
-            "timestamp": 0.03174271850172795
-        },
-        {
-            "x": 2.2952744131725273,
-            "y": 4.338010072410149,
-            "heading": -0.34621912169362346,
-            "angularVelocity": 0.9172170449263883,
-            "velocityX": -0.09854403440450425,
-            "velocityY": -0.462286447903903,
-            "timestamp": 0.0634854370034559
-        },
-        {
-            "x": 2.2907034035575107,
-            "y": 4.3159283160610205,
-            "heading": -0.3028943022400045,
-            "angularVelocity": 1.364874260887912,
-            "velocityX": -0.14400183194038138,
-            "velocityY": -0.6956479278208441,
-            "timestamp": 0.09522815550518385
-        },
-        {
-            "x": 2.2847999846220546,
-            "y": 4.286378308280002,
-            "heading": -0.24568561151458213,
-            "angularVelocity": 1.8022618548662834,
-            "velocityX": -0.1859771063759033,
-            "velocityY": -0.9309224028625668,
-            "timestamp": 0.1269708740069118
-        },
-        {
-            "x": 2.2777013130983144,
-            "y": 4.249280679670162,
-            "heading": -0.1750916915555501,
-            "angularVelocity": 2.223940585151498,
-            "velocityX": -0.22363149278956135,
-            "velocityY": -1.1686972748670326,
-            "timestamp": 0.15871359250863973
-        },
-        {
-            "x": 2.2695785209744463,
-            "y": 4.204529290238701,
-            "heading": -0.09189311777806415,
-            "angularVelocity": 2.6210286233977427,
-            "velocityX": -0.25589465890975394,
-            "velocityY": -1.409815905623355,
-            "timestamp": 0.19045631101036767
-        },
-        {
-            "x": 2.2606569711878857,
-            "y": 4.151982062664549,
-            "heading": 0.002706319559794192,
-            "angularVelocity": 2.9801933105606198,
-            "velocityX": -0.2810581515277304,
-            "velocityY": -1.6554104391308149,
-            "timestamp": 0.2221990295120956
-        },
-        {
-            "x": 2.251280347316294,
-            "y": 4.091451352956859,
-            "heading": 0.10685098530745366,
-            "angularVelocity": 3.2808993893194844,
-            "velocityX": -0.2953944814487593,
-            "velocityY": -1.90691637530654,
-            "timestamp": 0.2539417480138236
-        },
-        {
-            "x": 2.242133674900212,
-            "y": 4.022709367456616,
-            "heading": 0.21739538615105536,
-            "angularVelocity": 3.4825120865934673,
-            "velocityX": -0.28815025454054716,
-            "velocityY": -2.165598560706175,
-            "timestamp": 0.2856844665155515
-        },
-        {
-            "x": 2.2351422077578347,
-            "y": 3.945850682832356,
-            "heading": 0.32707167243976176,
-            "angularVelocity": 3.455163623831899,
-            "velocityX": -0.22025420229828702,
-            "velocityY": -2.4213012700873704,
-            "timestamp": 0.31742718501727946
-        },
-        {
-            "x": 2.2344515310594897,
-            "y": 3.864378163543378,
-            "heading": 0.4256089749961829,
-            "angularVelocity": 3.1042490122910227,
-            "velocityX": -0.021758586880562782,
-            "velocityY": -2.566652231898267,
-            "timestamp": 0.3491699035190074
-        },
-        {
-            "x": 2.2416429003575074,
-            "y": 3.7806290345563274,
-            "heading": 0.5177286971400864,
-            "angularVelocity": 2.9020741288711216,
-            "velocityX": 0.22655177746121863,
-            "velocityY": -2.638372922674897,
-            "timestamp": 0.38091262202073534
-        },
-        {
-            "x": 2.256915898193716,
-            "y": 3.695399323053411,
-            "heading": 0.6036240465401915,
-            "angularVelocity": 2.705985922265222,
-            "velocityX": 0.4811496480800017,
-            "velocityY": -2.6850161399464656,
-            "timestamp": 0.4126553405224633
-        },
-        {
-            "x": 2.280071045497703,
-            "y": 3.6084777900686467,
-            "heading": 0.6811513337134051,
-            "angularVelocity": 2.4423644486842004,
-            "velocityX": 0.729463272111561,
-            "velocityY": -2.7383140791811433,
-            "timestamp": 0.4443980590241912
-        },
-        {
-            "x": 2.3096855944046597,
-            "y": 3.5244760350530098,
-            "heading": 0.76088689172136,
-            "angularVelocity": 2.511932240574098,
-            "velocityX": 0.932955660534971,
-            "velocityY": -2.6463314731869354,
-            "timestamp": 0.47614077752591916
-        },
-        {
-            "x": 2.3348942368332115,
-            "y": 3.446420745412462,
-            "heading": 0.8559069862879931,
-            "angularVelocity": 2.993445396350149,
-            "velocityX": 0.7941551202420774,
-            "velocityY": -2.458998262429792,
-            "timestamp": 0.5078834960276472
-        },
-        {
-            "x": 2.355075569752848,
-            "y": 3.373868279566975,
-            "heading": 0.9657696902827837,
-            "angularVelocity": 3.4610363944982643,
-            "velocityX": 0.6357783413709821,
-            "velocityY": -2.285641220097014,
-            "timestamp": 0.5396262145293751
-        },
-        {
-            "x": 2.370025604730125,
-            "y": 3.306495756130267,
-            "heading": 1.091029644988018,
-            "angularVelocity": 3.9461004166489144,
-            "velocityX": 0.4709752561508733,
-            "velocityY": -2.1224560030370347,
-            "timestamp": 0.571368933031103
-        },
-        {
-            "x": 2.3800631598495494,
-            "y": 3.2441375159647747,
-            "heading": 1.2334110423596254,
-            "angularVelocity": 4.485482154399005,
-            "velocityX": 0.31621598883778346,
-            "velocityY": -1.9644895934825695,
-            "timestamp": 0.603111651532831
-        },
-        {
-            "x": 2.3890320547539976,
-            "y": 3.1882487557001293,
-            "heading": 1.3963738258608078,
-            "angularVelocity": 5.133863487221998,
-            "velocityX": 0.28254967840764567,
-            "velocityY": -1.760679705539431,
-            "timestamp": 0.6348543700345589
-        },
-        {
-            "x": 2.399614951757013,
-            "y": 3.1248820965808815,
-            "heading": 1.5465533770663946,
-            "angularVelocity": 4.7311496397956585,
-            "velocityX": 0.33339605120587323,
-            "velocityY": -1.9962581061165,
-            "timestamp": 0.6665970885362869
-        },
-        {
-            "x": 2.4156041612276775,
-            "y": 3.056078363317597,
-            "heading": 1.6846738705269053,
-            "angularVelocity": 4.351249671731503,
-            "velocityX": 0.5037126694046519,
-            "velocityY": -2.167543818263053,
-            "timestamp": 0.6983398070380148
-        },
-        {
-            "x": 2.4386535219180887,
-            "y": 2.9838461830329104,
-            "heading": 1.8122491249375776,
-            "angularVelocity": 4.019039969866676,
-            "velocityX": 0.7261306459671242,
-            "velocityY": -2.275551171861821,
-            "timestamp": 0.7300825255397427
-        },
-        {
-            "x": 2.4695800740474128,
-            "y": 2.910121142766613,
-            "heading": 1.9308538821800434,
-            "angularVelocity": 3.736439814882526,
-            "velocityX": 0.9742880757878827,
-            "velocityY": -2.3225811696715355,
-            "timestamp": 0.7618252440414707
-        },
-        {
-            "x": 2.508646228883482,
-            "y": 2.8366969767650785,
-            "heading": 2.0420489888957944,
-            "angularVelocity": 3.503011461028351,
-            "velocityX": 1.2307123233299608,
-            "velocityY": -2.313102641084062,
-            "timestamp": 0.7935679625431986
-        },
-        {
-            "x": 2.555734299118471,
-            "y": 2.7651139971749186,
-            "heading": 2.147169748045839,
-            "angularVelocity": 3.3116495408016253,
-            "velocityX": 1.4834290337302587,
-            "velocityY": -2.255099215470886,
-            "timestamp": 0.8253106810449266
-        },
-        {
-            "x": 2.6104869842529297,
-            "y": 2.6966493129730225,
-            "heading": 2.247054267099527,
-            "angularVelocity": 3.1466907614812087,
-            "velocityX": 1.7248896036260801,
-            "velocityY": -2.1568626580665695,
-            "timestamp": 0.8570533995466545
-        },
-        {
-            "x": 2.751018773357776,
-            "y": 2.5737681266542305,
-            "heading": 2.423101617636397,
-            "angularVelocity": 2.7414576820608,
-            "velocityX": 2.188399607493837,
-            "velocityY": -1.9135395743649004,
-            "timestamp": 0.9212700940770386
-        },
-        {
-            "x": 2.920945282929504,
-            "y": 2.4615612619997704,
-            "heading": 2.5486951706813996,
-            "angularVelocity": 1.9557772937936253,
-            "velocityX": 2.646142265876451,
-            "velocityY": -1.7473161064272718,
-            "timestamp": 0.9854867886074228
-        },
-        {
-            "x": 3.118708267205298,
-            "y": 2.3583606370011725,
-            "heading": 2.6122511958810777,
-            "angularVelocity": 0.9897118757741349,
-            "velocityX": 3.079619493373657,
-            "velocityY": -1.6070684695514719,
-            "timestamp": 1.049703483137807
-        },
-        {
-            "x": 3.3434611246912245,
-            "y": 2.26616504003377,
-            "heading": 2.6122519169792926,
-            "angularVelocity": 0.000011229139401682067,
-            "velocityX": 3.499913209945475,
-            "velocityY": -1.4356951512629466,
-            "timestamp": 1.113920177668191
-        },
-        {
-            "x": 3.5722970045795748,
-            "y": 2.184629282343257,
-            "heading": 2.6122522165022306,
-            "angularVelocity": 0.000004664253435080619,
-            "velocityX": 3.5634951559220656,
-            "velocityY": -1.269697207039109,
-            "timestamp": 1.1781368721985752
-        },
-        {
-            "x": 3.80113295504209,
-            "y": 2.103093722723815,
-            "heading": 2.6122525160244625,
-            "angularVelocity": 0.000004664242440601037,
-            "velocityX": 3.56349625492233,
-            "velocityY": -1.2696941226220015,
-            "timestamp": 1.2423535667289594
-        },
-        {
-            "x": 4.029968923086795,
-            "y": 2.0215582124500746,
-            "heading": 2.612252815546594,
-            "angularVelocity": 0.00000466424087155042,
-            "velocityX": 3.563496528717025,
-            "velocityY": -1.2696933541971844,
-            "timestamp": 1.3065702612593435
-        },
-        {
-            "x": 4.258804909490432,
-            "y": 1.940022753702107,
-            "heading": 2.6122531150686714,
-            "angularVelocity": 0.000004664240033500949,
-            "velocityX": 3.5634968146073587,
-            "velocityY": -1.2696925518237052,
-            "timestamp": 1.3707869557897276
-        },
-        {
-            "x": 4.487640916105967,
-            "y": 1.8584873516805105,
-            "heading": 2.6122534145907514,
-            "angularVelocity": 0.000004664240074560689,
-            "velocityX": 3.563497129352575,
-            "velocityY": -1.2696916684650783,
-            "timestamp": 1.4350036503201118
-        },
-        {
-            "x": 4.716476945604857,
-            "y": 1.7769520138830965,
-            "heading": 2.6122537141129194,
-            "angularVelocity": 0.000004664241438058459,
-            "velocityX": 3.5634974856984654,
-            "velocityY": -1.2696906683484759,
-            "timestamp": 1.499220344850496
-        },
-        {
-            "x": 4.945312999982612,
-            "y": 1.6954167459104819,
-            "heading": 2.612254013635151,
-            "angularVelocity": 0.000004664242435524347,
-            "velocityX": 3.563497873118986,
-            "velocityY": -1.269689581017522,
-            "timestamp": 1.56343703938088
-        },
-        {
-            "x": 5.174149135873993,
-            "y": 1.613881706713074,
-            "heading": 2.6122543131567744,
-            "angularVelocity": 0.0000046642329655920085,
-            "velocityX": 3.563499142471556,
-            "velocityY": -1.269686018467187,
-            "timestamp": 1.6276537339112642
-        },
-        {
-            "x": 5.406651475687904,
-            "y": 1.5434785463415874,
-            "heading": 2.61225458420666,
-            "angularVelocity": 0.000004220863245612629,
-            "velocityX": 3.6205902766281897,
-            "velocityY": -1.096337344772168,
-            "timestamp": 1.6918704284416484
-        },
-        {
-            "x": 5.646764755249023,
-            "y": 1.5066063404083252,
-            "heading": 2.6122548161665473,
-            "angularVelocity": 0.0000036121430551488755,
-            "velocityX": 3.7391099202016678,
-            "velocityY": -0.5741841152508433,
-            "timestamp": 1.7560871229720325
-        },
-        {
-            "x": 5.787436421990979,
-            "y": 1.496717214982701,
-            "heading": 2.612254832075579,
-            "angularVelocity": 4.2677202387530557e-7,
-            "velocityX": 3.773625708136921,
-            "velocityY": -0.26528339929021244,
-            "timestamp": 1.7933647115888978
-        },
-        {
-            "x": 5.926771623718837,
-            "y": 1.4982765413849686,
-            "heading": 2.6080354125638485,
-            "angularVelocity": -0.11318917527356848,
-            "velocityX": 3.7377740057150746,
-            "velocityY": 0.04183013065287323,
-            "timestamp": 1.830642300205763
-        },
-        {
-            "x": 6.064421169286406,
-            "y": 1.511276136415188,
-            "heading": 2.60121232475526,
-            "angularVelocity": -0.18303458087688948,
-            "velocityX": 3.6925549820916683,
-            "velocityY": 0.34872413996054913,
-            "timestamp": 1.8679198888226283
-        },
-        {
-            "x": 6.200023778520788,
-            "y": 1.5356760041928015,
-            "heading": 2.5932728220304684,
-            "angularVelocity": -0.2129832700927332,
-            "velocityX": 3.6376443398227996,
-            "velocityY": 0.6545452289951588,
-            "timestamp": 1.9051974774394935
-        },
-        {
-            "x": 6.333175231231244,
-            "y": 1.5714002898334831,
-            "heading": 2.5855391112934316,
-            "angularVelocity": -0.20746274166290535,
-            "velocityX": 3.5718901798871894,
-            "velocityY": 0.9583314523868992,
-            "timestamp": 1.9424750660563588
-        },
-        {
-            "x": 6.463368734714764,
-            "y": 1.6183219287773098,
-            "heading": 2.5790588590244545,
-            "angularVelocity": -0.17383775371256402,
-            "velocityX": 3.4925409157130343,
-            "velocityY": 1.258709071181673,
-            "timestamp": 1.979752654673224
-        },
-        {
-            "x": 6.589861303526444,
-            "y": 1.6762129661852994,
-            "heading": 2.574283804517795,
-            "angularVelocity": -0.12809451157737914,
-            "velocityX": 3.393260495247066,
-            "velocityY": 1.5529716260079764,
-            "timestamp": 2.017030243290089
-        },
-        {
-            "x": 6.711340738368746,
-            "y": 1.7445488061219798,
-            "heading": 2.570091844488755,
-            "angularVelocity": -0.11245255352014773,
-            "velocityX": 3.258779318878531,
-            "velocityY": 1.8331614911851861,
-            "timestamp": 2.054307831906954
-        },
-        {
-            "x": 6.825305386557433,
-            "y": 1.8215960437325822,
-            "heading": 2.560973344137785,
-            "angularVelocity": -0.24461078866147912,
-            "velocityX": 3.057189384216965,
-            "velocityY": 2.0668514372666396,
-            "timestamp": 2.091585420523819
-        },
-        {
-            "x": 6.929260415482823,
-            "y": 1.9025407238929235,
-            "heading": 2.5356026772435865,
-            "angularVelocity": -0.680587662333947,
-            "velocityX": 2.788673645010342,
-            "velocityY": 2.171403332771371,
-            "timestamp": 2.128863009140684
-        },
-        {
-            "x": 7.023455735639986,
-            "y": 1.9838668818491878,
-            "heading": 2.488274688562586,
-            "angularVelocity": -1.2696097155701718,
-            "velocityX": 2.5268619471418052,
-            "velocityY": 2.1816367681967113,
-            "timestamp": 2.166140597757549
-        },
-        {
-            "x": 7.110101814938688,
-            "y": 2.060020595789651,
-            "heading": 2.4125158730705962,
-            "angularVelocity": -2.0322885224854694,
-            "velocityX": 2.3243477519224953,
-            "velocityY": 2.0428819772427436,
-            "timestamp": 2.2034181863744142
-        },
-        {
-            "x": 7.191835709933807,
-            "y": 2.1285351921147058,
-            "heading": 2.3080325133818067,
-            "angularVelocity": -2.8028465242925935,
-            "velocityX": 2.1925746280203615,
-            "velocityY": 1.8379567688575535,
-            "timestamp": 2.2406957749912793
-        },
-        {
-            "x": 7.274315415796274,
-            "y": 2.1867028423762775,
-            "heading": 2.1850264648931392,
-            "angularVelocity": -3.2997319046816447,
-            "velocityX": 2.2125815784434106,
-            "velocityY": 1.5603919786607532,
-            "timestamp": 2.2779733636081443
-        },
-        {
-            "x": 7.351412179448119,
-            "y": 2.2360068924730196,
-            "heading": 2.043511695780604,
-            "angularVelocity": -3.796242578000636,
-            "velocityX": 2.0681800114335913,
-            "velocityY": 1.322619083639869,
-            "timestamp": 2.3152509522250093
-        },
-        {
-            "x": 7.420172175937219,
-            "y": 2.2789013315216375,
-            "heading": 1.8838780266002726,
-            "angularVelocity": -4.282296014933475,
-            "velocityX": 1.8445398170951257,
-            "velocityY": 1.1506763350355667,
-            "timestamp": 2.3525285408418743
-        },
-        {
-            "x": 7.479537718839642,
-            "y": 2.3170603162912466,
-            "heading": 1.7061327210648876,
-            "angularVelocity": -4.768154597182533,
-            "velocityX": 1.592526370537947,
-            "velocityY": 1.0236441300375574,
-            "timestamp": 2.3898061294587394
-        },
-        {
-            "x": 7.529281054923766,
-            "y": 2.3515994880332096,
-            "heading": 1.5086439015102746,
-            "angularVelocity": -5.2977895535137876,
-            "velocityX": 1.3344032682848619,
-            "velocityY": 0.9265398601007353,
-            "timestamp": 2.4270837180756044
-        },
-        {
-            "x": 7.569719945861703,
-            "y": 2.383438536522646,
-            "heading": 1.288404786035775,
-            "angularVelocity": -5.908083748068905,
-            "velocityX": 1.0848043674059311,
-            "velocityY": 0.8541069760888933,
-            "timestamp": 2.4643613066924694
-        },
-        {
-            "x": 7.603202598509127,
-            "y": 2.4091886515491456,
-            "heading": 1.0408456353949866,
-            "angularVelocity": -6.640964714353526,
-            "velocityX": 0.8981979223912477,
-            "velocityY": 0.6907666504707783,
-            "timestamp": 2.5016388953093345
-        },
-        {
-            "x": 7.630132008026638,
-            "y": 2.427100499568469,
-            "heading": 0.7720308301798764,
-            "angularVelocity": -7.211164004677401,
-            "velocityX": 0.7224021326670985,
-            "velocityY": 0.48049910640464033,
-            "timestamp": 2.5389164839261995
-        },
-        {
-            "x": 7.649623319500903,
-            "y": 2.438227448776781,
-            "heading": 0.5254269726030665,
-            "angularVelocity": -6.615338242807932,
-            "velocityX": 0.5228694289911082,
-            "velocityY": 0.29848897477445496,
-            "timestamp": 2.5761940725430645
-        },
-        {
-            "x": 7.66290885295536,
-            "y": 2.4458359453096237,
-            "heading": 0.3183730749926382,
-            "angularVelocity": -5.554380132752273,
-            "velocityX": 0.3563946582222695,
-            "velocityY": 0.2041037742822349,
-            "timestamp": 2.6134716611599296
-        },
-        {
-            "x": 7.671732744074589,
-            "y": 2.451418069701132,
-            "heading": 0.15988099310288126,
-            "angularVelocity": -4.251672057404801,
-            "velocityX": 0.23670766931625417,
-            "velocityY": 0.14974478228408286,
-            "timestamp": 2.6507492497767946
-        },
-        {
-            "x": 7.677275184086734,
-            "y": 2.4551558590330185,
-            "heading": 0.05341064749493927,
-            "angularVelocity": -2.8561489505727446,
-            "velocityX": 0.14868021827027583,
-            "velocityY": 0.10026907508163654,
-            "timestamp": 2.6880268383936596
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 1.0247603884557974e-38,
-            "angularVelocity": -1.4327817189005392,
-            "velocityX": 0.07309528363731636,
-            "velocityY": 0.04947050051802475,
-            "timestamp": 2.7253044270105247
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -2.6319896813971663e-37,
-            "velocityX": 5.227147746061896e-36,
-            "velocityY": -6.467829854732196e-35,
-            "timestamp": 2.7625820156273897
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.traj
deleted file mode 100644
index ce59bb95..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.traj	
+++ /dev/null
@@ -1,625 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.298402468716654,
-            "y": 4.3526843009931255,
-            "heading": -0.37533408415570846,
-            "angularVelocity": 0.4620245693037669,
-            "velocityX": -0.050327487964169394,
-            "velocityY": -0.2304685720750862,
-            "timestamp": 0.03174271850172795
-        },
-        {
-            "x": 2.2952744131725273,
-            "y": 4.338010072410149,
-            "heading": -0.34621912169362346,
-            "angularVelocity": 0.9172170449263883,
-            "velocityX": -0.09854403440450425,
-            "velocityY": -0.462286447903903,
-            "timestamp": 0.0634854370034559
-        },
-        {
-            "x": 2.2907034035575107,
-            "y": 4.3159283160610205,
-            "heading": -0.3028943022400045,
-            "angularVelocity": 1.364874260887912,
-            "velocityX": -0.14400183194038138,
-            "velocityY": -0.6956479278208441,
-            "timestamp": 0.09522815550518385
-        },
-        {
-            "x": 2.2847999846220546,
-            "y": 4.286378308280002,
-            "heading": -0.24568561151458213,
-            "angularVelocity": 1.8022618548662834,
-            "velocityX": -0.1859771063759033,
-            "velocityY": -0.9309224028625668,
-            "timestamp": 0.1269708740069118
-        },
-        {
-            "x": 2.2777013130983144,
-            "y": 4.249280679670162,
-            "heading": -0.1750916915555501,
-            "angularVelocity": 2.223940585151498,
-            "velocityX": -0.22363149278956135,
-            "velocityY": -1.1686972748670326,
-            "timestamp": 0.15871359250863973
-        },
-        {
-            "x": 2.2695785209744463,
-            "y": 4.204529290238701,
-            "heading": -0.09189311777806415,
-            "angularVelocity": 2.6210286233977427,
-            "velocityX": -0.25589465890975394,
-            "velocityY": -1.409815905623355,
-            "timestamp": 0.19045631101036767
-        },
-        {
-            "x": 2.2606569711878857,
-            "y": 4.151982062664549,
-            "heading": 0.002706319559794192,
-            "angularVelocity": 2.9801933105606198,
-            "velocityX": -0.2810581515277304,
-            "velocityY": -1.6554104391308149,
-            "timestamp": 0.2221990295120956
-        },
-        {
-            "x": 2.251280347316294,
-            "y": 4.091451352956859,
-            "heading": 0.10685098530745366,
-            "angularVelocity": 3.2808993893194844,
-            "velocityX": -0.2953944814487593,
-            "velocityY": -1.90691637530654,
-            "timestamp": 0.2539417480138236
-        },
-        {
-            "x": 2.242133674900212,
-            "y": 4.022709367456616,
-            "heading": 0.21739538615105536,
-            "angularVelocity": 3.4825120865934673,
-            "velocityX": -0.28815025454054716,
-            "velocityY": -2.165598560706175,
-            "timestamp": 0.2856844665155515
-        },
-        {
-            "x": 2.2351422077578347,
-            "y": 3.945850682832356,
-            "heading": 0.32707167243976176,
-            "angularVelocity": 3.455163623831899,
-            "velocityX": -0.22025420229828702,
-            "velocityY": -2.4213012700873704,
-            "timestamp": 0.31742718501727946
-        },
-        {
-            "x": 2.2344515310594897,
-            "y": 3.864378163543378,
-            "heading": 0.4256089749961829,
-            "angularVelocity": 3.1042490122910227,
-            "velocityX": -0.021758586880562782,
-            "velocityY": -2.566652231898267,
-            "timestamp": 0.3491699035190074
-        },
-        {
-            "x": 2.2416429003575074,
-            "y": 3.7806290345563274,
-            "heading": 0.5177286971400864,
-            "angularVelocity": 2.9020741288711216,
-            "velocityX": 0.22655177746121863,
-            "velocityY": -2.638372922674897,
-            "timestamp": 0.38091262202073534
-        },
-        {
-            "x": 2.256915898193716,
-            "y": 3.695399323053411,
-            "heading": 0.6036240465401915,
-            "angularVelocity": 2.705985922265222,
-            "velocityX": 0.4811496480800017,
-            "velocityY": -2.6850161399464656,
-            "timestamp": 0.4126553405224633
-        },
-        {
-            "x": 2.280071045497703,
-            "y": 3.6084777900686467,
-            "heading": 0.6811513337134051,
-            "angularVelocity": 2.4423644486842004,
-            "velocityX": 0.729463272111561,
-            "velocityY": -2.7383140791811433,
-            "timestamp": 0.4443980590241912
-        },
-        {
-            "x": 2.3096855944046597,
-            "y": 3.5244760350530098,
-            "heading": 0.76088689172136,
-            "angularVelocity": 2.511932240574098,
-            "velocityX": 0.932955660534971,
-            "velocityY": -2.6463314731869354,
-            "timestamp": 0.47614077752591916
-        },
-        {
-            "x": 2.3348942368332115,
-            "y": 3.446420745412462,
-            "heading": 0.8559069862879931,
-            "angularVelocity": 2.993445396350149,
-            "velocityX": 0.7941551202420774,
-            "velocityY": -2.458998262429792,
-            "timestamp": 0.5078834960276472
-        },
-        {
-            "x": 2.355075569752848,
-            "y": 3.373868279566975,
-            "heading": 0.9657696902827837,
-            "angularVelocity": 3.4610363944982643,
-            "velocityX": 0.6357783413709821,
-            "velocityY": -2.285641220097014,
-            "timestamp": 0.5396262145293751
-        },
-        {
-            "x": 2.370025604730125,
-            "y": 3.306495756130267,
-            "heading": 1.091029644988018,
-            "angularVelocity": 3.9461004166489144,
-            "velocityX": 0.4709752561508733,
-            "velocityY": -2.1224560030370347,
-            "timestamp": 0.571368933031103
-        },
-        {
-            "x": 2.3800631598495494,
-            "y": 3.2441375159647747,
-            "heading": 1.2334110423596254,
-            "angularVelocity": 4.485482154399005,
-            "velocityX": 0.31621598883778346,
-            "velocityY": -1.9644895934825695,
-            "timestamp": 0.603111651532831
-        },
-        {
-            "x": 2.3890320547539976,
-            "y": 3.1882487557001293,
-            "heading": 1.3963738258608078,
-            "angularVelocity": 5.133863487221998,
-            "velocityX": 0.28254967840764567,
-            "velocityY": -1.760679705539431,
-            "timestamp": 0.6348543700345589
-        },
-        {
-            "x": 2.399614951757013,
-            "y": 3.1248820965808815,
-            "heading": 1.5465533770663946,
-            "angularVelocity": 4.7311496397956585,
-            "velocityX": 0.33339605120587323,
-            "velocityY": -1.9962581061165,
-            "timestamp": 0.6665970885362869
-        },
-        {
-            "x": 2.4156041612276775,
-            "y": 3.056078363317597,
-            "heading": 1.6846738705269053,
-            "angularVelocity": 4.351249671731503,
-            "velocityX": 0.5037126694046519,
-            "velocityY": -2.167543818263053,
-            "timestamp": 0.6983398070380148
-        },
-        {
-            "x": 2.4386535219180887,
-            "y": 2.9838461830329104,
-            "heading": 1.8122491249375776,
-            "angularVelocity": 4.019039969866676,
-            "velocityX": 0.7261306459671242,
-            "velocityY": -2.275551171861821,
-            "timestamp": 0.7300825255397427
-        },
-        {
-            "x": 2.4695800740474128,
-            "y": 2.910121142766613,
-            "heading": 1.9308538821800434,
-            "angularVelocity": 3.736439814882526,
-            "velocityX": 0.9742880757878827,
-            "velocityY": -2.3225811696715355,
-            "timestamp": 0.7618252440414707
-        },
-        {
-            "x": 2.508646228883482,
-            "y": 2.8366969767650785,
-            "heading": 2.0420489888957944,
-            "angularVelocity": 3.503011461028351,
-            "velocityX": 1.2307123233299608,
-            "velocityY": -2.313102641084062,
-            "timestamp": 0.7935679625431986
-        },
-        {
-            "x": 2.555734299118471,
-            "y": 2.7651139971749186,
-            "heading": 2.147169748045839,
-            "angularVelocity": 3.3116495408016253,
-            "velocityX": 1.4834290337302587,
-            "velocityY": -2.255099215470886,
-            "timestamp": 0.8253106810449266
-        },
-        {
-            "x": 2.6104869842529297,
-            "y": 2.6966493129730225,
-            "heading": 2.247054267099527,
-            "angularVelocity": 3.1466907614812087,
-            "velocityX": 1.7248896036260801,
-            "velocityY": -2.1568626580665695,
-            "timestamp": 0.8570533995466545
-        },
-        {
-            "x": 2.751018773357776,
-            "y": 2.5737681266542305,
-            "heading": 2.423101617636397,
-            "angularVelocity": 2.7414576820608,
-            "velocityX": 2.188399607493837,
-            "velocityY": -1.9135395743649004,
-            "timestamp": 0.9212700940770386
-        },
-        {
-            "x": 2.920945282929504,
-            "y": 2.4615612619997704,
-            "heading": 2.5486951706813996,
-            "angularVelocity": 1.9557772937936253,
-            "velocityX": 2.646142265876451,
-            "velocityY": -1.7473161064272718,
-            "timestamp": 0.9854867886074228
-        },
-        {
-            "x": 3.118708267205298,
-            "y": 2.3583606370011725,
-            "heading": 2.6122511958810777,
-            "angularVelocity": 0.9897118757741349,
-            "velocityX": 3.079619493373657,
-            "velocityY": -1.6070684695514719,
-            "timestamp": 1.049703483137807
-        },
-        {
-            "x": 3.3434611246912245,
-            "y": 2.26616504003377,
-            "heading": 2.6122519169792926,
-            "angularVelocity": 0.000011229139401682067,
-            "velocityX": 3.499913209945475,
-            "velocityY": -1.4356951512629466,
-            "timestamp": 1.113920177668191
-        },
-        {
-            "x": 3.5722970045795748,
-            "y": 2.184629282343257,
-            "heading": 2.6122522165022306,
-            "angularVelocity": 0.000004664253435080619,
-            "velocityX": 3.5634951559220656,
-            "velocityY": -1.269697207039109,
-            "timestamp": 1.1781368721985752
-        },
-        {
-            "x": 3.80113295504209,
-            "y": 2.103093722723815,
-            "heading": 2.6122525160244625,
-            "angularVelocity": 0.000004664242440601037,
-            "velocityX": 3.56349625492233,
-            "velocityY": -1.2696941226220015,
-            "timestamp": 1.2423535667289594
-        },
-        {
-            "x": 4.029968923086795,
-            "y": 2.0215582124500746,
-            "heading": 2.612252815546594,
-            "angularVelocity": 0.00000466424087155042,
-            "velocityX": 3.563496528717025,
-            "velocityY": -1.2696933541971844,
-            "timestamp": 1.3065702612593435
-        },
-        {
-            "x": 4.258804909490432,
-            "y": 1.940022753702107,
-            "heading": 2.6122531150686714,
-            "angularVelocity": 0.000004664240033500949,
-            "velocityX": 3.5634968146073587,
-            "velocityY": -1.2696925518237052,
-            "timestamp": 1.3707869557897276
-        },
-        {
-            "x": 4.487640916105967,
-            "y": 1.8584873516805105,
-            "heading": 2.6122534145907514,
-            "angularVelocity": 0.000004664240074560689,
-            "velocityX": 3.563497129352575,
-            "velocityY": -1.2696916684650783,
-            "timestamp": 1.4350036503201118
-        },
-        {
-            "x": 4.716476945604857,
-            "y": 1.7769520138830965,
-            "heading": 2.6122537141129194,
-            "angularVelocity": 0.000004664241438058459,
-            "velocityX": 3.5634974856984654,
-            "velocityY": -1.2696906683484759,
-            "timestamp": 1.499220344850496
-        },
-        {
-            "x": 4.945312999982612,
-            "y": 1.6954167459104819,
-            "heading": 2.612254013635151,
-            "angularVelocity": 0.000004664242435524347,
-            "velocityX": 3.563497873118986,
-            "velocityY": -1.269689581017522,
-            "timestamp": 1.56343703938088
-        },
-        {
-            "x": 5.174149135873993,
-            "y": 1.613881706713074,
-            "heading": 2.6122543131567744,
-            "angularVelocity": 0.0000046642329655920085,
-            "velocityX": 3.563499142471556,
-            "velocityY": -1.269686018467187,
-            "timestamp": 1.6276537339112642
-        },
-        {
-            "x": 5.406651475687904,
-            "y": 1.5434785463415874,
-            "heading": 2.61225458420666,
-            "angularVelocity": 0.000004220863245612629,
-            "velocityX": 3.6205902766281897,
-            "velocityY": -1.096337344772168,
-            "timestamp": 1.6918704284416484
-        },
-        {
-            "x": 5.646764755249023,
-            "y": 1.5066063404083252,
-            "heading": 2.6122548161665473,
-            "angularVelocity": 0.0000036121430551488755,
-            "velocityX": 3.7391099202016678,
-            "velocityY": -0.5741841152508433,
-            "timestamp": 1.7560871229720325
-        },
-        {
-            "x": 5.787436421990979,
-            "y": 1.496717214982701,
-            "heading": 2.612254832075579,
-            "angularVelocity": 4.2677202387530557e-7,
-            "velocityX": 3.773625708136921,
-            "velocityY": -0.26528339929021244,
-            "timestamp": 1.7933647115888978
-        },
-        {
-            "x": 5.926771623718837,
-            "y": 1.4982765413849686,
-            "heading": 2.6080354125638485,
-            "angularVelocity": -0.11318917527356848,
-            "velocityX": 3.7377740057150746,
-            "velocityY": 0.04183013065287323,
-            "timestamp": 1.830642300205763
-        },
-        {
-            "x": 6.064421169286406,
-            "y": 1.511276136415188,
-            "heading": 2.60121232475526,
-            "angularVelocity": -0.18303458087688948,
-            "velocityX": 3.6925549820916683,
-            "velocityY": 0.34872413996054913,
-            "timestamp": 1.8679198888226283
-        },
-        {
-            "x": 6.200023778520788,
-            "y": 1.5356760041928015,
-            "heading": 2.5932728220304684,
-            "angularVelocity": -0.2129832700927332,
-            "velocityX": 3.6376443398227996,
-            "velocityY": 0.6545452289951588,
-            "timestamp": 1.9051974774394935
-        },
-        {
-            "x": 6.333175231231244,
-            "y": 1.5714002898334831,
-            "heading": 2.5855391112934316,
-            "angularVelocity": -0.20746274166290535,
-            "velocityX": 3.5718901798871894,
-            "velocityY": 0.9583314523868992,
-            "timestamp": 1.9424750660563588
-        },
-        {
-            "x": 6.463368734714764,
-            "y": 1.6183219287773098,
-            "heading": 2.5790588590244545,
-            "angularVelocity": -0.17383775371256402,
-            "velocityX": 3.4925409157130343,
-            "velocityY": 1.258709071181673,
-            "timestamp": 1.979752654673224
-        },
-        {
-            "x": 6.589861303526444,
-            "y": 1.6762129661852994,
-            "heading": 2.574283804517795,
-            "angularVelocity": -0.12809451157737914,
-            "velocityX": 3.393260495247066,
-            "velocityY": 1.5529716260079764,
-            "timestamp": 2.017030243290089
-        },
-        {
-            "x": 6.711340738368746,
-            "y": 1.7445488061219798,
-            "heading": 2.570091844488755,
-            "angularVelocity": -0.11245255352014773,
-            "velocityX": 3.258779318878531,
-            "velocityY": 1.8331614911851861,
-            "timestamp": 2.054307831906954
-        },
-        {
-            "x": 6.825305386557433,
-            "y": 1.8215960437325822,
-            "heading": 2.560973344137785,
-            "angularVelocity": -0.24461078866147912,
-            "velocityX": 3.057189384216965,
-            "velocityY": 2.0668514372666396,
-            "timestamp": 2.091585420523819
-        },
-        {
-            "x": 6.929260415482823,
-            "y": 1.9025407238929235,
-            "heading": 2.5356026772435865,
-            "angularVelocity": -0.680587662333947,
-            "velocityX": 2.788673645010342,
-            "velocityY": 2.171403332771371,
-            "timestamp": 2.128863009140684
-        },
-        {
-            "x": 7.023455735639986,
-            "y": 1.9838668818491878,
-            "heading": 2.488274688562586,
-            "angularVelocity": -1.2696097155701718,
-            "velocityX": 2.5268619471418052,
-            "velocityY": 2.1816367681967113,
-            "timestamp": 2.166140597757549
-        },
-        {
-            "x": 7.110101814938688,
-            "y": 2.060020595789651,
-            "heading": 2.4125158730705962,
-            "angularVelocity": -2.0322885224854694,
-            "velocityX": 2.3243477519224953,
-            "velocityY": 2.0428819772427436,
-            "timestamp": 2.2034181863744142
-        },
-        {
-            "x": 7.191835709933807,
-            "y": 2.1285351921147058,
-            "heading": 2.3080325133818067,
-            "angularVelocity": -2.8028465242925935,
-            "velocityX": 2.1925746280203615,
-            "velocityY": 1.8379567688575535,
-            "timestamp": 2.2406957749912793
-        },
-        {
-            "x": 7.274315415796274,
-            "y": 2.1867028423762775,
-            "heading": 2.1850264648931392,
-            "angularVelocity": -3.2997319046816447,
-            "velocityX": 2.2125815784434106,
-            "velocityY": 1.5603919786607532,
-            "timestamp": 2.2779733636081443
-        },
-        {
-            "x": 7.351412179448119,
-            "y": 2.2360068924730196,
-            "heading": 2.043511695780604,
-            "angularVelocity": -3.796242578000636,
-            "velocityX": 2.0681800114335913,
-            "velocityY": 1.322619083639869,
-            "timestamp": 2.3152509522250093
-        },
-        {
-            "x": 7.420172175937219,
-            "y": 2.2789013315216375,
-            "heading": 1.8838780266002726,
-            "angularVelocity": -4.282296014933475,
-            "velocityX": 1.8445398170951257,
-            "velocityY": 1.1506763350355667,
-            "timestamp": 2.3525285408418743
-        },
-        {
-            "x": 7.479537718839642,
-            "y": 2.3170603162912466,
-            "heading": 1.7061327210648876,
-            "angularVelocity": -4.768154597182533,
-            "velocityX": 1.592526370537947,
-            "velocityY": 1.0236441300375574,
-            "timestamp": 2.3898061294587394
-        },
-        {
-            "x": 7.529281054923766,
-            "y": 2.3515994880332096,
-            "heading": 1.5086439015102746,
-            "angularVelocity": -5.2977895535137876,
-            "velocityX": 1.3344032682848619,
-            "velocityY": 0.9265398601007353,
-            "timestamp": 2.4270837180756044
-        },
-        {
-            "x": 7.569719945861703,
-            "y": 2.383438536522646,
-            "heading": 1.288404786035775,
-            "angularVelocity": -5.908083748068905,
-            "velocityX": 1.0848043674059311,
-            "velocityY": 0.8541069760888933,
-            "timestamp": 2.4643613066924694
-        },
-        {
-            "x": 7.603202598509127,
-            "y": 2.4091886515491456,
-            "heading": 1.0408456353949866,
-            "angularVelocity": -6.640964714353526,
-            "velocityX": 0.8981979223912477,
-            "velocityY": 0.6907666504707783,
-            "timestamp": 2.5016388953093345
-        },
-        {
-            "x": 7.630132008026638,
-            "y": 2.427100499568469,
-            "heading": 0.7720308301798764,
-            "angularVelocity": -7.211164004677401,
-            "velocityX": 0.7224021326670985,
-            "velocityY": 0.48049910640464033,
-            "timestamp": 2.5389164839261995
-        },
-        {
-            "x": 7.649623319500903,
-            "y": 2.438227448776781,
-            "heading": 0.5254269726030665,
-            "angularVelocity": -6.615338242807932,
-            "velocityX": 0.5228694289911082,
-            "velocityY": 0.29848897477445496,
-            "timestamp": 2.5761940725430645
-        },
-        {
-            "x": 7.66290885295536,
-            "y": 2.4458359453096237,
-            "heading": 0.3183730749926382,
-            "angularVelocity": -5.554380132752273,
-            "velocityX": 0.3563946582222695,
-            "velocityY": 0.2041037742822349,
-            "timestamp": 2.6134716611599296
-        },
-        {
-            "x": 7.671732744074589,
-            "y": 2.451418069701132,
-            "heading": 0.15988099310288126,
-            "angularVelocity": -4.251672057404801,
-            "velocityX": 0.23670766931625417,
-            "velocityY": 0.14974478228408286,
-            "timestamp": 2.6507492497767946
-        },
-        {
-            "x": 7.677275184086734,
-            "y": 2.4551558590330185,
-            "heading": 0.05341064749493927,
-            "angularVelocity": -2.8561489505727446,
-            "velocityX": 0.14868021827027583,
-            "velocityY": 0.10026907508163654,
-            "timestamp": 2.6880268383936596
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 1.0247603884557974e-38,
-            "angularVelocity": -1.4327817189005392,
-            "velocityX": 0.07309528363731636,
-            "velocityY": 0.04947050051802475,
-            "timestamp": 2.7253044270105247
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -2.6319896813971663e-37,
-            "velocityX": 5.227147746061896e-36,
-            "velocityY": -6.467829854732196e-35,
-            "timestamp": 2.7625820156273897
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.1.traj
deleted file mode 100644
index 8826d851..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.1.traj	
+++ /dev/null
@@ -1,571 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -4.609910300520975e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.298808348352727,
-            "y": 4.35514296819253,
-            "heading": -0.3837399750205753,
-            "angularVelocity": 0.24884159467028183,
-            "velocityX": -0.047369219319970686,
-            "velocityY": -0.19307136062676852,
-            "timestamp": 0.02515666638336474
-        },
-        {
-            "x": 2.2965596540188837,
-            "y": 4.3453750993410925,
-            "heading": -0.37149402212882315,
-            "angularVelocity": 0.48678758564965857,
-            "velocityX": -0.08938761199815681,
-            "velocityY": -0.38828152755153583,
-            "timestamp": 0.05031333276672948
-        },
-        {
-            "x": 2.2934096466067952,
-            "y": 4.330641358670927,
-            "heading": -0.3535645425221108,
-            "angularVelocity": 0.7127128584321681,
-            "velocityX": -0.1252156133919033,
-            "velocityY": -0.5856793760205414,
-            "timestamp": 0.07546999915009422
-        },
-        {
-            "x": 2.289538880665139,
-            "y": 4.310886769515238,
-            "heading": -0.33028900557963653,
-            "angularVelocity": 0.9252234214095049,
-            "velocityX": -0.15386640990779057,
-            "velocityY": -0.7852625961900879,
-            "timestamp": 0.10062666553345896
-        },
-        {
-            "x": 2.2851573432175654,
-            "y": 4.286058471237428,
-            "heading": -0.3020481691352866,
-            "angularVelocity": 1.1225985197714665,
-            "velocityX": -0.17417003432819553,
-            "velocityY": -0.9869470739663532,
-            "timestamp": 0.1257833319168237
-        },
-        {
-            "x": 2.280510060472114,
-            "y": 4.25610900707676,
-            "heading": -0.26927550139535,
-            "angularVelocity": 1.3027428690475438,
-            "velocityX": -0.18473364771909595,
-            "velocityY": -1.1905180004483138,
-            "timestamp": 0.15093999830018845
-        },
-        {
-            "x": 2.2758836751536,
-            "y": 4.2210014881511375,
-            "heading": -0.23246752663964654,
-            "angularVelocity": 1.463149933889621,
-            "velocityX": -0.1839029562984313,
-            "velocityY": -1.3955552930033008,
-            "timestamp": 0.1760966646835532
-        },
-        {
-            "x": 2.271613693612169,
-            "y": 4.180717494993525,
-            "heading": -0.19219487185489256,
-            "angularVelocity": 1.6008740653882818,
-            "velocityX": -0.16973558723403512,
-            "velocityY": -1.6013247758554696,
-            "timestamp": 0.20125333106691795
-        },
-        {
-            "x": 2.268091592763015,
-            "y": 4.135268758759812,
-            "heading": -0.1491141249151469,
-            "angularVelocity": 1.7124982413501877,
-            "velocityX": -0.1400066604803655,
-            "velocityY": -1.8066279347634933,
-            "timestamp": 0.2264099974502827
-        },
-        {
-            "x": 2.2657701315686967,
-            "y": 4.084713672798125,
-            "heading": -0.10398137252966654,
-            "angularVelocity": 1.7940672940403972,
-            "velocityX": -0.09228015981695388,
-            "velocityY": -2.0096099058307106,
-            "timestamp": 0.25156666383364745
-        },
-        {
-            "x": 2.265163909208295,
-            "y": 4.029179243747037,
-            "heading": -0.057668550112221675,
-            "angularVelocity": 1.8409761337881332,
-            "velocityX": -0.02409788129965802,
-            "velocityY": -2.2075432493635576,
-            "timestamp": 0.2767233302170122
-        },
-        {
-            "x": 2.266840363517046,
-            "y": 3.9688874958283327,
-            "heading": -0.011179237419756825,
-            "angularVelocity": 1.8479917801512262,
-            "velocityX": 0.06664055893588815,
-            "velocityY": -2.3966509314037507,
-            "timestamp": 0.3018799966003769
-        },
-        {
-            "x": 2.2713096946462064,
-            "y": 3.9041248350083095,
-            "heading": 0.03446639703260334,
-            "angularVelocity": 1.8144548151476838,
-            "velocityX": 0.1776599117328043,
-            "velocityY": -2.574373719995273,
-            "timestamp": 0.3270366629837416
-        },
-        {
-            "x": 2.2787160490905882,
-            "y": 3.8349972418338227,
-            "heading": 0.07865637806724814,
-            "angularVelocity": 1.7565912892125506,
-            "velocityX": 0.29440921668697884,
-            "velocityY": -2.74788368701343,
-            "timestamp": 0.35219332936710634
-        },
-        {
-            "x": 2.289237241442295,
-            "y": 3.7616571782461574,
-            "heading": 0.12059430680114623,
-            "angularVelocity": 1.667070195025133,
-            "velocityX": 0.41822681079334173,
-            "velocityY": -2.915333155436007,
-            "timestamp": 0.37734999575047107
-        },
-        {
-            "x": 2.303092280593543,
-            "y": 3.684333317171128,
-            "heading": 0.15922184993862323,
-            "angularVelocity": 1.5354794052927534,
-            "velocityX": 0.550750204343837,
-            "velocityY": -3.073692670431113,
-            "timestamp": 0.4025066621338358
-        },
-        {
-            "x": 2.3205444941015814,
-            "y": 3.6033821039148743,
-            "heading": 0.19314125064776663,
-            "angularVelocity": 1.3483265307192482,
-            "velocityX": 0.6937411039317692,
-            "velocityY": -3.217883165544707,
-            "timestamp": 0.4276633285172005
-        },
-        {
-            "x": 2.341904587688066,
-            "y": 3.5193842561187316,
-            "heading": 0.22066827104785883,
-            "angularVelocity": 1.0942236932591152,
-            "velocityX": 0.8490828339882706,
-            "velocityY": -3.3389896147642064,
-            "timestamp": 0.45281999490056524
-        },
-        {
-            "x": 2.3675851289681646,
-            "y": 3.4337423994061407,
-            "heading": 0.23918679984335672,
-            "angularVelocity": 0.7361280907928144,
-            "velocityX": 1.0208244959308244,
-            "velocityY": -3.40434044032252,
-            "timestamp": 0.47797666128392996
-        },
-        {
-            "x": 2.3978885412195137,
-            "y": 3.3469640370766824,
-            "heading": 0.24945072521405584,
-            "angularVelocity": 0.40800021808479553,
-            "velocityX": 1.2045877537807594,
-            "velocityY": -3.4495175555868487,
-            "timestamp": 0.5031333276672947
-        },
-        {
-            "x": 2.4331457577160354,
-            "y": 3.2598611919071945,
-            "heading": 0.25313177220508776,
-            "angularVelocity": 0.1463249118518414,
-            "velocityX": 1.4015059054023244,
-            "velocityY": -3.462416038839161,
-            "timestamp": 0.5282899940506595
-        },
-        {
-            "x": 2.473498104501041,
-            "y": 3.173715186571369,
-            "heading": 0.2532432496800442,
-            "angularVelocity": 0.004431329384332257,
-            "velocityX": 1.604041893710102,
-            "velocityY": -3.424380799229854,
-            "timestamp": 0.5534466604340242
-        },
-        {
-            "x": 2.5185817721335604,
-            "y": 3.089905547060991,
-            "heading": 0.25324321969105484,
-            "angularVelocity": -0.0000011920891614956396,
-            "velocityX": 1.7921161311870697,
-            "velocityY": -3.3315081669882356,
-            "timestamp": 0.578603326817389
-        },
-        {
-            "x": 2.568241478882672,
-            "y": 3.0087236123099395,
-            "heading": 0.2532431890633549,
-            "angularVelocity": -0.0000012174784783024135,
-            "velocityX": 1.97401778090718,
-            "velocityY": -3.2270545514223747,
-            "timestamp": 0.6037599932007538
-        },
-        {
-            "x": 2.6223245423144714,
-            "y": 2.9304189809174512,
-            "heading": 0.25324315880142845,
-            "angularVelocity": -0.0000012029386552177117,
-            "velocityX": 2.1498501672527754,
-            "velocityY": -3.1126791681853656,
-            "timestamp": 0.6289166595841186
-        },
-        {
-            "x": 2.6806646810204167,
-            "y": 2.855232404738762,
-            "heading": 0.25324312847978453,
-            "angularVelocity": -0.0000012053124772832063,
-            "velocityX": 2.319072718813166,
-            "velocityY": -2.9887336832676343,
-            "timestamp": 0.6540733259674834
-        },
-        {
-            "x": 2.7430825233459473,
-            "y": 2.7833950519561768,
-            "heading": 0.2532430937019675,
-            "angularVelocity": -0.0000013824493495341482,
-            "velocityX": 2.4811650865953045,
-            "velocityY": -2.8555990562441647,
-            "timestamp": 0.6792299923508481
-        },
-        {
-            "x": 2.8747857816965947,
-            "y": 2.6605842580771375,
-            "heading": 0.2532430684120768,
-            "angularVelocity": -5.312693002073383e-7,
-            "velocityX": 2.7667141273306664,
-            "velocityY": -2.579908520631989,
-            "timestamp": 0.7268327631068262
-        },
-        {
-            "x": 3.0186320127358077,
-            "y": 2.5522491183421203,
-            "heading": 0.2532430469824583,
-            "angularVelocity": -4.5017586399077444e-7,
-            "velocityX": 3.021803747025554,
-            "velocityY": -2.2758158404343,
-            "timestamp": 0.7744355338628043
-        },
-        {
-            "x": 3.1730376030001257,
-            "y": 2.4595822138278454,
-            "heading": 0.2532430277420137,
-            "angularVelocity": -4.041874945252969e-7,
-            "velocityX": 3.2436261127705017,
-            "velocityY": -1.946670394236201,
-            "timestamp": 0.8220383046187824
-        },
-        {
-            "x": 3.336302611834685,
-            "y": 2.38360340882859,
-            "heading": 0.25324300957525164,
-            "angularVelocity": -3.816324501860361e-7,
-            "velocityX": 3.4297375182526793,
-            "velocityY": -1.5961004746706646,
-            "timestamp": 0.8696410753747604
-        },
-        {
-            "x": 3.505316504811254,
-            "y": 2.3214540241877932,
-            "heading": 0.25324299166355524,
-            "angularVelocity": -3.7627424124720095e-7,
-            "velocityX": 3.550505365390803,
-            "velocityY": -1.3055833442844713,
-            "timestamp": 0.9172438461307385
-        },
-        {
-            "x": 3.6743308817857088,
-            "y": 2.2593059557844626,
-            "heading": 0.25324297375187266,
-            "angularVelocity": -3.76273948990246e-7,
-            "velocityX": 3.5505155328218008,
-            "velocityY": -1.3055556938463668,
-            "timestamp": 0.9648466168867166
-        },
-        {
-            "x": 3.843345266545321,
-            "y": 2.1971579085532174,
-            "heading": 0.2532429558401917,
-            "angularVelocity": -3.762739157561073e-7,
-            "velocityX": 3.5505156963660025,
-            "velocityY": -1.3055552490805507,
-            "timestamp": 1.0124493876426945
-        },
-        {
-            "x": 4.012359658311276,
-            "y": 2.135009880376043,
-            "heading": 0.25324293792850894,
-            "angularVelocity": -3.7627395316214525e-7,
-            "velocityX": 3.5505158435494915,
-            "velocityY": -1.3055548488082471,
-            "timestamp": 1.0600521583986726
-        },
-        {
-            "x": 4.181374056391421,
-            "y": 2.07286186937061,
-            "heading": 0.25324292001682175,
-            "angularVelocity": -3.7627404706616014e-7,
-            "velocityX": 3.5505159761928384,
-            "velocityY": -1.30555448807838,
-            "timestamp": 1.1076549291546507
-        },
-        {
-            "x": 4.3503884601627245,
-            "y": 2.010713873842556,
-            "heading": 0.25324290210512795,
-            "angularVelocity": -3.7627418619029544e-7,
-            "velocityX": 3.5505160957480255,
-            "velocityY": -1.30555416294226,
-            "timestamp": 1.1552576999106288
-        },
-        {
-            "x": 4.51940286907005,
-            "y": 1.9485658922821711,
-            "heading": 0.2532428841934256,
-            "angularVelocity": -3.7627436389492574e-7,
-            "velocityX": 3.5505162036413855,
-            "velocityY": -1.3055538695209365,
-            "timestamp": 1.2028604706666068
-        },
-        {
-            "x": 4.688417282561973,
-            "y": 1.8864179231898264,
-            "heading": 0.25324286628171494,
-            "angularVelocity": -3.7627454024029507e-7,
-            "velocityX": 3.550516299950831,
-            "velocityY": -1.3055536076025602,
-            "timestamp": 1.250463241422585
-        },
-        {
-            "x": 4.857431700124039,
-            "y": 1.824269965166445,
-            "heading": 0.2532428483699966,
-            "angularVelocity": -3.7627470129766985e-7,
-            "velocityX": 3.5505163854530717,
-            "velocityY": -1.3055533750748494,
-            "timestamp": 1.298066012178563
-        },
-        {
-            "x": 5.02644612130461,
-            "y": 1.7621220169837726,
-            "heading": 0.2532428304582713,
-            "angularVelocity": -3.76274846815446e-7,
-            "velocityX": 3.5505164614676405,
-            "velocityY": -1.3055531683492934,
-            "timestamp": 1.345668782934541
-        },
-        {
-            "x": 5.1954605457099055,
-            "y": 1.6999740775709098,
-            "heading": 0.25324281254653974,
-            "angularVelocity": -3.7627497879654404e-7,
-            "velocityX": 3.5505165292100247,
-            "velocityY": -1.3055529841203253,
-            "timestamp": 1.3932715536905191
-        },
-        {
-            "x": 5.364474972997734,
-            "y": 1.6378261459972472,
-            "heading": 0.25324279463480254,
-            "angularVelocity": -3.7627509767034604e-7,
-            "velocityX": 3.550516589763906,
-            "velocityY": -1.3055528194408317,
-            "timestamp": 1.4408743244464972
-        },
-        {
-            "x": 5.533489402870974,
-            "y": 1.5756782214547507,
-            "heading": 0.2532427767230601,
-            "angularVelocity": -3.762752068122207e-7,
-            "velocityX": 3.5505166440761182,
-            "velocityY": -1.3055526717358554,
-            "timestamp": 1.4884770952024753
-        },
-        {
-            "x": 5.702503835071411,
-            "y": 1.513530303241192,
-            "heading": 0.25324275881131303,
-            "angularVelocity": -3.7627530496428226e-7,
-            "velocityX": 3.550516692963962,
-            "velocityY": -1.3055525387827163,
-            "timestamp": 1.5360798659584534
-        },
-        {
-            "x": 5.871518269374186,
-            "y": 1.4513823907450571,
-            "heading": 0.2532427408995616,
-            "angularVelocity": -3.762753964251323e-7,
-            "velocityX": 3.5505167371281625,
-            "velocityY": -1.3055524186757583,
-            "timestamp": 1.5836826367144314
-        },
-        {
-            "x": 6.040532705582966,
-            "y": 1.3892344834324082,
-            "heading": 0.25324272298780626,
-            "angularVelocity": -3.7627547903960503e-7,
-            "velocityX": 3.550516777167954,
-            "velocityY": -1.3055523097853368,
-            "timestamp": 1.6312854074704095
-        },
-        {
-            "x": 6.20954714352589,
-            "y": 1.3270865808358587,
-            "heading": 0.2532427050760471,
-            "angularVelocity": -3.7627555795965075e-7,
-            "velocityX": 3.5505168135974197,
-            "velocityY": -1.3055522107133783,
-            "timestamp": 1.6788881782263876
-        },
-        {
-            "x": 6.378561583051887,
-            "y": 1.2649386825445668,
-            "heading": 0.25324268716428455,
-            "angularVelocity": -3.7627562961930894e-7,
-            "velocityX": 3.5505168468533315,
-            "velocityY": -1.3055521202720615,
-            "timestamp": 1.7264909489823657
-        },
-        {
-            "x": 6.547576024027691,
-            "y": 1.2027907881961044,
-            "heading": 0.25324266925251887,
-            "angularVelocity": -3.7627569544162935e-7,
-            "velocityX": 3.5505168773096716,
-            "velocityY": -1.3055520374443248,
-            "timestamp": 1.7740937197383437
-        },
-        {
-            "x": 6.716590466335514,
-            "y": 1.1406428974701475,
-            "heading": 0.25324265134075025,
-            "angularVelocity": -3.762757576316506e-7,
-            "velocityX": 3.550516905291665,
-            "velocityY": -1.3055519613457025,
-            "timestamp": 1.8216964904943218
-        },
-        {
-            "x": 6.88560491686231,
-            "y": 1.0784950290966298,
-            "heading": 0.253242633428611,
-            "angularVelocity": -3.7628354454135297e-7,
-            "velocityX": 3.550517077949107,
-            "velocityY": -1.3055514917839766,
-            "timestamp": 1.8692992612502999
-        },
-        {
-            "x": 7.0444139114910005,
-            "y": 1.0203058482073266,
-            "heading": 0.2259027129119336,
-            "angularVelocity": -0.574334646544581,
-            "velocityX": 3.336129223292024,
-            "velocityY": -1.2223906290579827,
-            "timestamp": 1.916902032006278
-        },
-        {
-            "x": 7.185810534595712,
-            "y": 0.9684234171781619,
-            "heading": 0.18518533646408028,
-            "angularVelocity": -0.8553572786042075,
-            "velocityX": 2.9703443908662663,
-            "velocityY": -1.089903596055891,
-            "timestamp": 1.964504802762256
-        },
-        {
-            "x": 7.3094655243228,
-            "y": 0.9230369546143523,
-            "heading": 0.14380134057962407,
-            "angularVelocity": -0.8693610734677483,
-            "velocityX": 2.59764269523238,
-            "velocityY": -0.953441613650398,
-            "timestamp": 2.012107573518234
-        },
-        {
-            "x": 7.415398593682274,
-            "y": 0.8841490900710189,
-            "heading": 0.10540313701630608,
-            "angularVelocity": -0.8066379950897271,
-            "velocityX": 2.2253551143589885,
-            "velocityY": -0.8169243917057022,
-            "timestamp": 2.059710344274212
-        },
-        {
-            "x": 7.503637039429732,
-            "y": 0.8517540010892736,
-            "heading": 0.07171800478429124,
-            "angularVelocity": -0.70762965468317,
-            "velocityX": 1.8536409613588987,
-            "velocityY": -0.680529483206126,
-            "timestamp": 2.1073131150301903
-        },
-        {
-            "x": 7.574202084965293,
-            "y": 0.8258459262172375,
-            "heading": 0.04375776286271558,
-            "angularVelocity": -0.5873658502969467,
-            "velocityX": 1.482372652157004,
-            "velocityY": -0.5442556065663985,
-            "timestamp": 2.1549158857861683
-        },
-        {
-            "x": 7.627109788299601,
-            "y": 0.8064200554899893,
-            "heading": 0.022189516524924582,
-            "angularVelocity": -0.4530880449870122,
-            "velocityX": 1.1114416764840027,
-            "velocityY": -0.40808277372822227,
-            "timestamp": 2.2025186565421464
-        },
-        {
-            "x": 7.66237251850946,
-            "y": 0.7934724766877013,
-            "heading": 0.0074863600733853135,
-            "angularVelocity": -0.3088718622474896,
-            "velocityX": 0.7407705402406998,
-            "velocityY": -0.27199212559831976,
-            "timestamp": 2.2501214272981245
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -1.0670900456397286e-32,
-            "angularVelocity": -0.1572673177316084,
-            "velocityX": 0.37030368633165434,
-            "velocityY": -0.13596848639085585,
-            "timestamp": 2.2977241980541026
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 2.230662363627099e-31,
-            "velocityX": 2.3924515989499496e-33,
-            "velocityY": 1.2585115119762557e-33,
-            "timestamp": 2.3453269688100806
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.traj
deleted file mode 100644
index 8826d851..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.traj	
+++ /dev/null
@@ -1,571 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -4.609910300520975e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.298808348352727,
-            "y": 4.35514296819253,
-            "heading": -0.3837399750205753,
-            "angularVelocity": 0.24884159467028183,
-            "velocityX": -0.047369219319970686,
-            "velocityY": -0.19307136062676852,
-            "timestamp": 0.02515666638336474
-        },
-        {
-            "x": 2.2965596540188837,
-            "y": 4.3453750993410925,
-            "heading": -0.37149402212882315,
-            "angularVelocity": 0.48678758564965857,
-            "velocityX": -0.08938761199815681,
-            "velocityY": -0.38828152755153583,
-            "timestamp": 0.05031333276672948
-        },
-        {
-            "x": 2.2934096466067952,
-            "y": 4.330641358670927,
-            "heading": -0.3535645425221108,
-            "angularVelocity": 0.7127128584321681,
-            "velocityX": -0.1252156133919033,
-            "velocityY": -0.5856793760205414,
-            "timestamp": 0.07546999915009422
-        },
-        {
-            "x": 2.289538880665139,
-            "y": 4.310886769515238,
-            "heading": -0.33028900557963653,
-            "angularVelocity": 0.9252234214095049,
-            "velocityX": -0.15386640990779057,
-            "velocityY": -0.7852625961900879,
-            "timestamp": 0.10062666553345896
-        },
-        {
-            "x": 2.2851573432175654,
-            "y": 4.286058471237428,
-            "heading": -0.3020481691352866,
-            "angularVelocity": 1.1225985197714665,
-            "velocityX": -0.17417003432819553,
-            "velocityY": -0.9869470739663532,
-            "timestamp": 0.1257833319168237
-        },
-        {
-            "x": 2.280510060472114,
-            "y": 4.25610900707676,
-            "heading": -0.26927550139535,
-            "angularVelocity": 1.3027428690475438,
-            "velocityX": -0.18473364771909595,
-            "velocityY": -1.1905180004483138,
-            "timestamp": 0.15093999830018845
-        },
-        {
-            "x": 2.2758836751536,
-            "y": 4.2210014881511375,
-            "heading": -0.23246752663964654,
-            "angularVelocity": 1.463149933889621,
-            "velocityX": -0.1839029562984313,
-            "velocityY": -1.3955552930033008,
-            "timestamp": 0.1760966646835532
-        },
-        {
-            "x": 2.271613693612169,
-            "y": 4.180717494993525,
-            "heading": -0.19219487185489256,
-            "angularVelocity": 1.6008740653882818,
-            "velocityX": -0.16973558723403512,
-            "velocityY": -1.6013247758554696,
-            "timestamp": 0.20125333106691795
-        },
-        {
-            "x": 2.268091592763015,
-            "y": 4.135268758759812,
-            "heading": -0.1491141249151469,
-            "angularVelocity": 1.7124982413501877,
-            "velocityX": -0.1400066604803655,
-            "velocityY": -1.8066279347634933,
-            "timestamp": 0.2264099974502827
-        },
-        {
-            "x": 2.2657701315686967,
-            "y": 4.084713672798125,
-            "heading": -0.10398137252966654,
-            "angularVelocity": 1.7940672940403972,
-            "velocityX": -0.09228015981695388,
-            "velocityY": -2.0096099058307106,
-            "timestamp": 0.25156666383364745
-        },
-        {
-            "x": 2.265163909208295,
-            "y": 4.029179243747037,
-            "heading": -0.057668550112221675,
-            "angularVelocity": 1.8409761337881332,
-            "velocityX": -0.02409788129965802,
-            "velocityY": -2.2075432493635576,
-            "timestamp": 0.2767233302170122
-        },
-        {
-            "x": 2.266840363517046,
-            "y": 3.9688874958283327,
-            "heading": -0.011179237419756825,
-            "angularVelocity": 1.8479917801512262,
-            "velocityX": 0.06664055893588815,
-            "velocityY": -2.3966509314037507,
-            "timestamp": 0.3018799966003769
-        },
-        {
-            "x": 2.2713096946462064,
-            "y": 3.9041248350083095,
-            "heading": 0.03446639703260334,
-            "angularVelocity": 1.8144548151476838,
-            "velocityX": 0.1776599117328043,
-            "velocityY": -2.574373719995273,
-            "timestamp": 0.3270366629837416
-        },
-        {
-            "x": 2.2787160490905882,
-            "y": 3.8349972418338227,
-            "heading": 0.07865637806724814,
-            "angularVelocity": 1.7565912892125506,
-            "velocityX": 0.29440921668697884,
-            "velocityY": -2.74788368701343,
-            "timestamp": 0.35219332936710634
-        },
-        {
-            "x": 2.289237241442295,
-            "y": 3.7616571782461574,
-            "heading": 0.12059430680114623,
-            "angularVelocity": 1.667070195025133,
-            "velocityX": 0.41822681079334173,
-            "velocityY": -2.915333155436007,
-            "timestamp": 0.37734999575047107
-        },
-        {
-            "x": 2.303092280593543,
-            "y": 3.684333317171128,
-            "heading": 0.15922184993862323,
-            "angularVelocity": 1.5354794052927534,
-            "velocityX": 0.550750204343837,
-            "velocityY": -3.073692670431113,
-            "timestamp": 0.4025066621338358
-        },
-        {
-            "x": 2.3205444941015814,
-            "y": 3.6033821039148743,
-            "heading": 0.19314125064776663,
-            "angularVelocity": 1.3483265307192482,
-            "velocityX": 0.6937411039317692,
-            "velocityY": -3.217883165544707,
-            "timestamp": 0.4276633285172005
-        },
-        {
-            "x": 2.341904587688066,
-            "y": 3.5193842561187316,
-            "heading": 0.22066827104785883,
-            "angularVelocity": 1.0942236932591152,
-            "velocityX": 0.8490828339882706,
-            "velocityY": -3.3389896147642064,
-            "timestamp": 0.45281999490056524
-        },
-        {
-            "x": 2.3675851289681646,
-            "y": 3.4337423994061407,
-            "heading": 0.23918679984335672,
-            "angularVelocity": 0.7361280907928144,
-            "velocityX": 1.0208244959308244,
-            "velocityY": -3.40434044032252,
-            "timestamp": 0.47797666128392996
-        },
-        {
-            "x": 2.3978885412195137,
-            "y": 3.3469640370766824,
-            "heading": 0.24945072521405584,
-            "angularVelocity": 0.40800021808479553,
-            "velocityX": 1.2045877537807594,
-            "velocityY": -3.4495175555868487,
-            "timestamp": 0.5031333276672947
-        },
-        {
-            "x": 2.4331457577160354,
-            "y": 3.2598611919071945,
-            "heading": 0.25313177220508776,
-            "angularVelocity": 0.1463249118518414,
-            "velocityX": 1.4015059054023244,
-            "velocityY": -3.462416038839161,
-            "timestamp": 0.5282899940506595
-        },
-        {
-            "x": 2.473498104501041,
-            "y": 3.173715186571369,
-            "heading": 0.2532432496800442,
-            "angularVelocity": 0.004431329384332257,
-            "velocityX": 1.604041893710102,
-            "velocityY": -3.424380799229854,
-            "timestamp": 0.5534466604340242
-        },
-        {
-            "x": 2.5185817721335604,
-            "y": 3.089905547060991,
-            "heading": 0.25324321969105484,
-            "angularVelocity": -0.0000011920891614956396,
-            "velocityX": 1.7921161311870697,
-            "velocityY": -3.3315081669882356,
-            "timestamp": 0.578603326817389
-        },
-        {
-            "x": 2.568241478882672,
-            "y": 3.0087236123099395,
-            "heading": 0.2532431890633549,
-            "angularVelocity": -0.0000012174784783024135,
-            "velocityX": 1.97401778090718,
-            "velocityY": -3.2270545514223747,
-            "timestamp": 0.6037599932007538
-        },
-        {
-            "x": 2.6223245423144714,
-            "y": 2.9304189809174512,
-            "heading": 0.25324315880142845,
-            "angularVelocity": -0.0000012029386552177117,
-            "velocityX": 2.1498501672527754,
-            "velocityY": -3.1126791681853656,
-            "timestamp": 0.6289166595841186
-        },
-        {
-            "x": 2.6806646810204167,
-            "y": 2.855232404738762,
-            "heading": 0.25324312847978453,
-            "angularVelocity": -0.0000012053124772832063,
-            "velocityX": 2.319072718813166,
-            "velocityY": -2.9887336832676343,
-            "timestamp": 0.6540733259674834
-        },
-        {
-            "x": 2.7430825233459473,
-            "y": 2.7833950519561768,
-            "heading": 0.2532430937019675,
-            "angularVelocity": -0.0000013824493495341482,
-            "velocityX": 2.4811650865953045,
-            "velocityY": -2.8555990562441647,
-            "timestamp": 0.6792299923508481
-        },
-        {
-            "x": 2.8747857816965947,
-            "y": 2.6605842580771375,
-            "heading": 0.2532430684120768,
-            "angularVelocity": -5.312693002073383e-7,
-            "velocityX": 2.7667141273306664,
-            "velocityY": -2.579908520631989,
-            "timestamp": 0.7268327631068262
-        },
-        {
-            "x": 3.0186320127358077,
-            "y": 2.5522491183421203,
-            "heading": 0.2532430469824583,
-            "angularVelocity": -4.5017586399077444e-7,
-            "velocityX": 3.021803747025554,
-            "velocityY": -2.2758158404343,
-            "timestamp": 0.7744355338628043
-        },
-        {
-            "x": 3.1730376030001257,
-            "y": 2.4595822138278454,
-            "heading": 0.2532430277420137,
-            "angularVelocity": -4.041874945252969e-7,
-            "velocityX": 3.2436261127705017,
-            "velocityY": -1.946670394236201,
-            "timestamp": 0.8220383046187824
-        },
-        {
-            "x": 3.336302611834685,
-            "y": 2.38360340882859,
-            "heading": 0.25324300957525164,
-            "angularVelocity": -3.816324501860361e-7,
-            "velocityX": 3.4297375182526793,
-            "velocityY": -1.5961004746706646,
-            "timestamp": 0.8696410753747604
-        },
-        {
-            "x": 3.505316504811254,
-            "y": 2.3214540241877932,
-            "heading": 0.25324299166355524,
-            "angularVelocity": -3.7627424124720095e-7,
-            "velocityX": 3.550505365390803,
-            "velocityY": -1.3055833442844713,
-            "timestamp": 0.9172438461307385
-        },
-        {
-            "x": 3.6743308817857088,
-            "y": 2.2593059557844626,
-            "heading": 0.25324297375187266,
-            "angularVelocity": -3.76273948990246e-7,
-            "velocityX": 3.5505155328218008,
-            "velocityY": -1.3055556938463668,
-            "timestamp": 0.9648466168867166
-        },
-        {
-            "x": 3.843345266545321,
-            "y": 2.1971579085532174,
-            "heading": 0.2532429558401917,
-            "angularVelocity": -3.762739157561073e-7,
-            "velocityX": 3.5505156963660025,
-            "velocityY": -1.3055552490805507,
-            "timestamp": 1.0124493876426945
-        },
-        {
-            "x": 4.012359658311276,
-            "y": 2.135009880376043,
-            "heading": 0.25324293792850894,
-            "angularVelocity": -3.7627395316214525e-7,
-            "velocityX": 3.5505158435494915,
-            "velocityY": -1.3055548488082471,
-            "timestamp": 1.0600521583986726
-        },
-        {
-            "x": 4.181374056391421,
-            "y": 2.07286186937061,
-            "heading": 0.25324292001682175,
-            "angularVelocity": -3.7627404706616014e-7,
-            "velocityX": 3.5505159761928384,
-            "velocityY": -1.30555448807838,
-            "timestamp": 1.1076549291546507
-        },
-        {
-            "x": 4.3503884601627245,
-            "y": 2.010713873842556,
-            "heading": 0.25324290210512795,
-            "angularVelocity": -3.7627418619029544e-7,
-            "velocityX": 3.5505160957480255,
-            "velocityY": -1.30555416294226,
-            "timestamp": 1.1552576999106288
-        },
-        {
-            "x": 4.51940286907005,
-            "y": 1.9485658922821711,
-            "heading": 0.2532428841934256,
-            "angularVelocity": -3.7627436389492574e-7,
-            "velocityX": 3.5505162036413855,
-            "velocityY": -1.3055538695209365,
-            "timestamp": 1.2028604706666068
-        },
-        {
-            "x": 4.688417282561973,
-            "y": 1.8864179231898264,
-            "heading": 0.25324286628171494,
-            "angularVelocity": -3.7627454024029507e-7,
-            "velocityX": 3.550516299950831,
-            "velocityY": -1.3055536076025602,
-            "timestamp": 1.250463241422585
-        },
-        {
-            "x": 4.857431700124039,
-            "y": 1.824269965166445,
-            "heading": 0.2532428483699966,
-            "angularVelocity": -3.7627470129766985e-7,
-            "velocityX": 3.5505163854530717,
-            "velocityY": -1.3055533750748494,
-            "timestamp": 1.298066012178563
-        },
-        {
-            "x": 5.02644612130461,
-            "y": 1.7621220169837726,
-            "heading": 0.2532428304582713,
-            "angularVelocity": -3.76274846815446e-7,
-            "velocityX": 3.5505164614676405,
-            "velocityY": -1.3055531683492934,
-            "timestamp": 1.345668782934541
-        },
-        {
-            "x": 5.1954605457099055,
-            "y": 1.6999740775709098,
-            "heading": 0.25324281254653974,
-            "angularVelocity": -3.7627497879654404e-7,
-            "velocityX": 3.5505165292100247,
-            "velocityY": -1.3055529841203253,
-            "timestamp": 1.3932715536905191
-        },
-        {
-            "x": 5.364474972997734,
-            "y": 1.6378261459972472,
-            "heading": 0.25324279463480254,
-            "angularVelocity": -3.7627509767034604e-7,
-            "velocityX": 3.550516589763906,
-            "velocityY": -1.3055528194408317,
-            "timestamp": 1.4408743244464972
-        },
-        {
-            "x": 5.533489402870974,
-            "y": 1.5756782214547507,
-            "heading": 0.2532427767230601,
-            "angularVelocity": -3.762752068122207e-7,
-            "velocityX": 3.5505166440761182,
-            "velocityY": -1.3055526717358554,
-            "timestamp": 1.4884770952024753
-        },
-        {
-            "x": 5.702503835071411,
-            "y": 1.513530303241192,
-            "heading": 0.25324275881131303,
-            "angularVelocity": -3.7627530496428226e-7,
-            "velocityX": 3.550516692963962,
-            "velocityY": -1.3055525387827163,
-            "timestamp": 1.5360798659584534
-        },
-        {
-            "x": 5.871518269374186,
-            "y": 1.4513823907450571,
-            "heading": 0.2532427408995616,
-            "angularVelocity": -3.762753964251323e-7,
-            "velocityX": 3.5505167371281625,
-            "velocityY": -1.3055524186757583,
-            "timestamp": 1.5836826367144314
-        },
-        {
-            "x": 6.040532705582966,
-            "y": 1.3892344834324082,
-            "heading": 0.25324272298780626,
-            "angularVelocity": -3.7627547903960503e-7,
-            "velocityX": 3.550516777167954,
-            "velocityY": -1.3055523097853368,
-            "timestamp": 1.6312854074704095
-        },
-        {
-            "x": 6.20954714352589,
-            "y": 1.3270865808358587,
-            "heading": 0.2532427050760471,
-            "angularVelocity": -3.7627555795965075e-7,
-            "velocityX": 3.5505168135974197,
-            "velocityY": -1.3055522107133783,
-            "timestamp": 1.6788881782263876
-        },
-        {
-            "x": 6.378561583051887,
-            "y": 1.2649386825445668,
-            "heading": 0.25324268716428455,
-            "angularVelocity": -3.7627562961930894e-7,
-            "velocityX": 3.5505168468533315,
-            "velocityY": -1.3055521202720615,
-            "timestamp": 1.7264909489823657
-        },
-        {
-            "x": 6.547576024027691,
-            "y": 1.2027907881961044,
-            "heading": 0.25324266925251887,
-            "angularVelocity": -3.7627569544162935e-7,
-            "velocityX": 3.5505168773096716,
-            "velocityY": -1.3055520374443248,
-            "timestamp": 1.7740937197383437
-        },
-        {
-            "x": 6.716590466335514,
-            "y": 1.1406428974701475,
-            "heading": 0.25324265134075025,
-            "angularVelocity": -3.762757576316506e-7,
-            "velocityX": 3.550516905291665,
-            "velocityY": -1.3055519613457025,
-            "timestamp": 1.8216964904943218
-        },
-        {
-            "x": 6.88560491686231,
-            "y": 1.0784950290966298,
-            "heading": 0.253242633428611,
-            "angularVelocity": -3.7628354454135297e-7,
-            "velocityX": 3.550517077949107,
-            "velocityY": -1.3055514917839766,
-            "timestamp": 1.8692992612502999
-        },
-        {
-            "x": 7.0444139114910005,
-            "y": 1.0203058482073266,
-            "heading": 0.2259027129119336,
-            "angularVelocity": -0.574334646544581,
-            "velocityX": 3.336129223292024,
-            "velocityY": -1.2223906290579827,
-            "timestamp": 1.916902032006278
-        },
-        {
-            "x": 7.185810534595712,
-            "y": 0.9684234171781619,
-            "heading": 0.18518533646408028,
-            "angularVelocity": -0.8553572786042075,
-            "velocityX": 2.9703443908662663,
-            "velocityY": -1.089903596055891,
-            "timestamp": 1.964504802762256
-        },
-        {
-            "x": 7.3094655243228,
-            "y": 0.9230369546143523,
-            "heading": 0.14380134057962407,
-            "angularVelocity": -0.8693610734677483,
-            "velocityX": 2.59764269523238,
-            "velocityY": -0.953441613650398,
-            "timestamp": 2.012107573518234
-        },
-        {
-            "x": 7.415398593682274,
-            "y": 0.8841490900710189,
-            "heading": 0.10540313701630608,
-            "angularVelocity": -0.8066379950897271,
-            "velocityX": 2.2253551143589885,
-            "velocityY": -0.8169243917057022,
-            "timestamp": 2.059710344274212
-        },
-        {
-            "x": 7.503637039429732,
-            "y": 0.8517540010892736,
-            "heading": 0.07171800478429124,
-            "angularVelocity": -0.70762965468317,
-            "velocityX": 1.8536409613588987,
-            "velocityY": -0.680529483206126,
-            "timestamp": 2.1073131150301903
-        },
-        {
-            "x": 7.574202084965293,
-            "y": 0.8258459262172375,
-            "heading": 0.04375776286271558,
-            "angularVelocity": -0.5873658502969467,
-            "velocityX": 1.482372652157004,
-            "velocityY": -0.5442556065663985,
-            "timestamp": 2.1549158857861683
-        },
-        {
-            "x": 7.627109788299601,
-            "y": 0.8064200554899893,
-            "heading": 0.022189516524924582,
-            "angularVelocity": -0.4530880449870122,
-            "velocityX": 1.1114416764840027,
-            "velocityY": -0.40808277372822227,
-            "timestamp": 2.2025186565421464
-        },
-        {
-            "x": 7.66237251850946,
-            "y": 0.7934724766877013,
-            "heading": 0.0074863600733853135,
-            "angularVelocity": -0.3088718622474896,
-            "velocityX": 0.7407705402406998,
-            "velocityY": -0.27199212559831976,
-            "timestamp": 2.2501214272981245
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -1.0670900456397286e-32,
-            "angularVelocity": -0.1572673177316084,
-            "velocityX": 0.37030368633165434,
-            "velocityY": -0.13596848639085585,
-            "timestamp": 2.2977241980541026
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 2.230662363627099e-31,
-            "velocityX": 2.3924515989499496e-33,
-            "velocityY": 1.2585115119762557e-33,
-            "timestamp": 2.3453269688100806
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.1.traj
deleted file mode 100644
index 297f1b3f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.1.traj	
+++ /dev/null
@@ -1,256 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -4.289844309028223e-18,
-            "velocityX": -1.3032340882001426e-17,
-            "velocityY": -3.394190195762378e-17,
-            "timestamp": 0
-        },
-        {
-            "x": 2.271980215040446,
-            "y": 4.410826628196332,
-            "heading": -0.3872050871501322,
-            "angularVelocity": 0.033498492691473286,
-            "velocityX": -0.3358317815939762,
-            "velocityY": 0.6091837294305799,
-            "timestamp": 0.0834339883696639
-        },
-        {
-            "x": 2.216375801038755,
-            "y": 4.512717243328352,
-            "heading": -0.38158865604258857,
-            "angularVelocity": 0.06731586511476574,
-            "velocityX": -0.6664479918611741,
-            "velocityY": 1.2212123275291613,
-            "timestamp": 0.1668679767393278
-        },
-        {
-            "x": 2.133913144213912,
-            "y": 4.66605742038127,
-            "heading": -0.3731070083031397,
-            "angularVelocity": 0.10165698541488709,
-            "velocityX": -0.9883580832768961,
-            "velocityY": 1.837862243543364,
-            "timestamp": 0.2503019651089917
-        },
-        {
-            "x": 2.026047479965626,
-            "y": 4.871580643769176,
-            "heading": -0.3616752437087536,
-            "angularVelocity": 0.13701567930528194,
-            "velocityX": -1.2928264171046289,
-            "velocityY": 2.4633033539909763,
-            "timestamp": 0.3337359534786556
-        },
-        {
-            "x": 1.8971303225339013,
-            "y": 5.131188087013384,
-            "heading": -0.34705934044989933,
-            "angularVelocity": 0.1751792470161148,
-            "velocityX": -1.5451395762230817,
-            "velocityY": 3.111531023711595,
-            "timestamp": 0.41716994184831946
-        },
-        {
-            "x": 1.812941926504828,
-            "y": 5.427772716701387,
-            "heading": -0.3288482106163032,
-            "angularVelocity": 0.21826991841509946,
-            "velocityX": -1.0090419704726812,
-            "velocityY": 3.5547219482539485,
-            "timestamp": 0.5006039302179833
-        },
-        {
-            "x": 1.7681664127359014,
-            "y": 5.682327706651028,
-            "heading": -0.2856043053433686,
-            "angularVelocity": 0.5183008282076023,
-            "velocityX": -0.5366579573130098,
-            "velocityY": 3.0509747277288763,
-            "timestamp": 0.5840379185876472
-        },
-        {
-            "x": 1.7580426073358177,
-            "y": 5.890413977811611,
-            "heading": -0.23166998700826372,
-            "angularVelocity": 0.6464310215597402,
-            "velocityX": -0.12133910409739124,
-            "velocityY": 2.4940228224314014,
-            "timestamp": 0.6674719069573111
-        },
-        {
-            "x": 1.7809381631803756,
-            "y": 6.0507944700387695,
-            "heading": -0.17161076742769205,
-            "angularVelocity": 0.7198411672948628,
-            "velocityX": 0.27441521485536685,
-            "velocityY": 1.9222441041228078,
-            "timestamp": 0.7509058953269749
-        },
-        {
-            "x": 1.8360347454575048,
-            "y": 6.162894796860879,
-            "heading": -0.10766394629623156,
-            "angularVelocity": 0.7664361057242964,
-            "velocityX": 0.6603613629651003,
-            "velocityY": 1.34358106345549,
-            "timestamp": 0.8343398836966388
-        },
-        {
-            "x": 1.9228414263613316,
-            "y": 6.226384193614056,
-            "heading": -0.041156065591054634,
-            "angularVelocity": 0.797131744563275,
-            "velocityX": 1.0404234844874343,
-            "velocityY": 0.760953635248516,
-            "timestamp": 0.9177738720663027
-        },
-        {
-            "x": 2.041031122207642,
-            "y": 6.2410478591918945,
-            "heading": 0.027035456766794728,
-            "angularVelocity": 0.8173110705883178,
-            "velocityX": 1.4165653369372246,
-            "velocityY": 0.17575170340583585,
-            "timestamp": 1.0012078604359667
-        },
-        {
-            "x": 2.201330972597037,
-            "y": 6.260936085330867,
-            "heading": 0.09277658873792281,
-            "angularVelocity": 0.8449644804697937,
-            "velocityX": 2.060318642882427,
-            "velocityY": 0.2556214680702958,
-            "timestamp": 1.0790112870770416
-        },
-        {
-            "x": 2.4117170311040814,
-            "y": 6.287038452225851,
-            "heading": 0.16066894258910833,
-            "angularVelocity": 0.8726139294965615,
-            "velocityX": 2.7040718846096095,
-            "velocityY": 0.33549122476729665,
-            "timestamp": 1.1568147137181166
-        },
-        {
-            "x": 2.672189282535082,
-            "y": 6.319354957991783,
-            "heading": 0.23071153953234388,
-            "angularVelocity": 0.9002507982373139,
-            "velocityX": 3.3478249310538675,
-            "velocityY": 0.4153609572357597,
-            "timestamp": 1.2346181403591916
-        },
-        {
-            "x": 2.9642755449736304,
-            "y": 6.355593779334123,
-            "heading": 0.23071154098757585,
-            "angularVelocity": 1.8703321803502023e-8,
-            "velocityX": 3.7541567903686675,
-            "velocityY": 0.4657741041344694,
-            "timestamp": 1.3124215670002666
-        },
-        {
-            "x": 3.256361807413129,
-            "y": 6.39183260067658,
-            "heading": 0.23071154244151956,
-            "angularVelocity": 1.868721301612308e-8,
-            "velocityX": 3.7541567903805326,
-            "velocityY": 0.46577410413594156,
-            "timestamp": 1.3902249936413416
-        },
-        {
-            "x": 3.5484480698526375,
-            "y": 6.4280714220190385,
-            "heading": 0.23071154389546672,
-            "angularVelocity": 1.868722261906901e-8,
-            "velocityX": 3.7541567903806516,
-            "velocityY": 0.4657741041359563,
-            "timestamp": 1.4680284202824165
-        },
-        {
-            "x": 3.8405343322921546,
-            "y": 6.464310243361498,
-            "heading": 0.23071154534942268,
-            "angularVelocity": 1.868730306107698e-8,
-            "velocityX": 3.754156790380741,
-            "velocityY": 0.46577410413596737,
-            "timestamp": 1.5458318469234915
-        },
-        {
-            "x": 4.132620594731676,
-            "y": 6.500549064703958,
-            "heading": 0.23071154680338032,
-            "angularVelocity": 1.86873022983191e-8,
-            "velocityX": 3.754156790380778,
-            "velocityY": 0.465774104135972,
-            "timestamp": 1.6236352735645665
-        },
-        {
-            "x": 4.424706857172369,
-            "y": 6.536787886046564,
-            "heading": 0.23071154826355325,
-            "angularVelocity": 1.876697436713731e-8,
-            "velocityX": 3.7541567903965425,
-            "velocityY": 0.46577410413792786,
-            "timestamp": 1.7014387002056415
-        },
-        {
-            "x": 4.6751378783325555,
-            "y": 6.567858587416207,
-            "heading": 0.24147534520321134,
-            "angularVelocity": 0.13834605240872366,
-            "velocityX": 3.2187659589270945,
-            "velocityY": 0.39934875250390567,
-            "timestamp": 1.7792421268467165
-        },
-        {
-            "x": 4.875482718759792,
-            "y": 6.592715151427428,
-            "heading": 0.2500855408103817,
-            "angularVelocity": 0.11066602050088868,
-            "velocityX": 2.575013069173925,
-            "velocityY": 0.31947903947593354,
-            "timestamp": 1.8570455534877914
-        },
-        {
-            "x": 5.025741356581141,
-            "y": 6.611357575366475,
-            "heading": 0.25654288837544836,
-            "angularVelocity": 0.08299567055584532,
-            "velocityX": 1.931259898289217,
-            "velocityY": 0.23960929156828065,
-            "timestamp": 1.9348489801288664
-        },
-        {
-            "x": 5.125913784825429,
-            "y": 6.623785858368443,
-            "heading": 0.2608476597362627,
-            "angularVelocity": 0.05532881451478021,
-            "velocityX": 1.2875066378045663,
-            "velocityY": 0.15973953254406137,
-            "timestamp": 2.0126524067699414
-        },
-        {
-            "x": 5.175999999999999,
-            "y": 6.63,
-            "heading": 0.26300000000000007,
-            "angularVelocity": 0.02766382352150404,
-            "velocityX": 0.6437533324290226,
-            "velocityY": 0.07986976795027836,
-            "timestamp": 2.090455833411016
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": -5.112167395133813e-18,
-            "velocityX": -1.2520036782163602e-16,
-            "velocityY": -1.3106574021367763e-17,
-            "timestamp": 2.168259260052091
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.traj
deleted file mode 100644
index 297f1b3f..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.traj	
+++ /dev/null
@@ -1,256 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -4.289844309028223e-18,
-            "velocityX": -1.3032340882001426e-17,
-            "velocityY": -3.394190195762378e-17,
-            "timestamp": 0
-        },
-        {
-            "x": 2.271980215040446,
-            "y": 4.410826628196332,
-            "heading": -0.3872050871501322,
-            "angularVelocity": 0.033498492691473286,
-            "velocityX": -0.3358317815939762,
-            "velocityY": 0.6091837294305799,
-            "timestamp": 0.0834339883696639
-        },
-        {
-            "x": 2.216375801038755,
-            "y": 4.512717243328352,
-            "heading": -0.38158865604258857,
-            "angularVelocity": 0.06731586511476574,
-            "velocityX": -0.6664479918611741,
-            "velocityY": 1.2212123275291613,
-            "timestamp": 0.1668679767393278
-        },
-        {
-            "x": 2.133913144213912,
-            "y": 4.66605742038127,
-            "heading": -0.3731070083031397,
-            "angularVelocity": 0.10165698541488709,
-            "velocityX": -0.9883580832768961,
-            "velocityY": 1.837862243543364,
-            "timestamp": 0.2503019651089917
-        },
-        {
-            "x": 2.026047479965626,
-            "y": 4.871580643769176,
-            "heading": -0.3616752437087536,
-            "angularVelocity": 0.13701567930528194,
-            "velocityX": -1.2928264171046289,
-            "velocityY": 2.4633033539909763,
-            "timestamp": 0.3337359534786556
-        },
-        {
-            "x": 1.8971303225339013,
-            "y": 5.131188087013384,
-            "heading": -0.34705934044989933,
-            "angularVelocity": 0.1751792470161148,
-            "velocityX": -1.5451395762230817,
-            "velocityY": 3.111531023711595,
-            "timestamp": 0.41716994184831946
-        },
-        {
-            "x": 1.812941926504828,
-            "y": 5.427772716701387,
-            "heading": -0.3288482106163032,
-            "angularVelocity": 0.21826991841509946,
-            "velocityX": -1.0090419704726812,
-            "velocityY": 3.5547219482539485,
-            "timestamp": 0.5006039302179833
-        },
-        {
-            "x": 1.7681664127359014,
-            "y": 5.682327706651028,
-            "heading": -0.2856043053433686,
-            "angularVelocity": 0.5183008282076023,
-            "velocityX": -0.5366579573130098,
-            "velocityY": 3.0509747277288763,
-            "timestamp": 0.5840379185876472
-        },
-        {
-            "x": 1.7580426073358177,
-            "y": 5.890413977811611,
-            "heading": -0.23166998700826372,
-            "angularVelocity": 0.6464310215597402,
-            "velocityX": -0.12133910409739124,
-            "velocityY": 2.4940228224314014,
-            "timestamp": 0.6674719069573111
-        },
-        {
-            "x": 1.7809381631803756,
-            "y": 6.0507944700387695,
-            "heading": -0.17161076742769205,
-            "angularVelocity": 0.7198411672948628,
-            "velocityX": 0.27441521485536685,
-            "velocityY": 1.9222441041228078,
-            "timestamp": 0.7509058953269749
-        },
-        {
-            "x": 1.8360347454575048,
-            "y": 6.162894796860879,
-            "heading": -0.10766394629623156,
-            "angularVelocity": 0.7664361057242964,
-            "velocityX": 0.6603613629651003,
-            "velocityY": 1.34358106345549,
-            "timestamp": 0.8343398836966388
-        },
-        {
-            "x": 1.9228414263613316,
-            "y": 6.226384193614056,
-            "heading": -0.041156065591054634,
-            "angularVelocity": 0.797131744563275,
-            "velocityX": 1.0404234844874343,
-            "velocityY": 0.760953635248516,
-            "timestamp": 0.9177738720663027
-        },
-        {
-            "x": 2.041031122207642,
-            "y": 6.2410478591918945,
-            "heading": 0.027035456766794728,
-            "angularVelocity": 0.8173110705883178,
-            "velocityX": 1.4165653369372246,
-            "velocityY": 0.17575170340583585,
-            "timestamp": 1.0012078604359667
-        },
-        {
-            "x": 2.201330972597037,
-            "y": 6.260936085330867,
-            "heading": 0.09277658873792281,
-            "angularVelocity": 0.8449644804697937,
-            "velocityX": 2.060318642882427,
-            "velocityY": 0.2556214680702958,
-            "timestamp": 1.0790112870770416
-        },
-        {
-            "x": 2.4117170311040814,
-            "y": 6.287038452225851,
-            "heading": 0.16066894258910833,
-            "angularVelocity": 0.8726139294965615,
-            "velocityX": 2.7040718846096095,
-            "velocityY": 0.33549122476729665,
-            "timestamp": 1.1568147137181166
-        },
-        {
-            "x": 2.672189282535082,
-            "y": 6.319354957991783,
-            "heading": 0.23071153953234388,
-            "angularVelocity": 0.9002507982373139,
-            "velocityX": 3.3478249310538675,
-            "velocityY": 0.4153609572357597,
-            "timestamp": 1.2346181403591916
-        },
-        {
-            "x": 2.9642755449736304,
-            "y": 6.355593779334123,
-            "heading": 0.23071154098757585,
-            "angularVelocity": 1.8703321803502023e-8,
-            "velocityX": 3.7541567903686675,
-            "velocityY": 0.4657741041344694,
-            "timestamp": 1.3124215670002666
-        },
-        {
-            "x": 3.256361807413129,
-            "y": 6.39183260067658,
-            "heading": 0.23071154244151956,
-            "angularVelocity": 1.868721301612308e-8,
-            "velocityX": 3.7541567903805326,
-            "velocityY": 0.46577410413594156,
-            "timestamp": 1.3902249936413416
-        },
-        {
-            "x": 3.5484480698526375,
-            "y": 6.4280714220190385,
-            "heading": 0.23071154389546672,
-            "angularVelocity": 1.868722261906901e-8,
-            "velocityX": 3.7541567903806516,
-            "velocityY": 0.4657741041359563,
-            "timestamp": 1.4680284202824165
-        },
-        {
-            "x": 3.8405343322921546,
-            "y": 6.464310243361498,
-            "heading": 0.23071154534942268,
-            "angularVelocity": 1.868730306107698e-8,
-            "velocityX": 3.754156790380741,
-            "velocityY": 0.46577410413596737,
-            "timestamp": 1.5458318469234915
-        },
-        {
-            "x": 4.132620594731676,
-            "y": 6.500549064703958,
-            "heading": 0.23071154680338032,
-            "angularVelocity": 1.86873022983191e-8,
-            "velocityX": 3.754156790380778,
-            "velocityY": 0.465774104135972,
-            "timestamp": 1.6236352735645665
-        },
-        {
-            "x": 4.424706857172369,
-            "y": 6.536787886046564,
-            "heading": 0.23071154826355325,
-            "angularVelocity": 1.876697436713731e-8,
-            "velocityX": 3.7541567903965425,
-            "velocityY": 0.46577410413792786,
-            "timestamp": 1.7014387002056415
-        },
-        {
-            "x": 4.6751378783325555,
-            "y": 6.567858587416207,
-            "heading": 0.24147534520321134,
-            "angularVelocity": 0.13834605240872366,
-            "velocityX": 3.2187659589270945,
-            "velocityY": 0.39934875250390567,
-            "timestamp": 1.7792421268467165
-        },
-        {
-            "x": 4.875482718759792,
-            "y": 6.592715151427428,
-            "heading": 0.2500855408103817,
-            "angularVelocity": 0.11066602050088868,
-            "velocityX": 2.575013069173925,
-            "velocityY": 0.31947903947593354,
-            "timestamp": 1.8570455534877914
-        },
-        {
-            "x": 5.025741356581141,
-            "y": 6.611357575366475,
-            "heading": 0.25654288837544836,
-            "angularVelocity": 0.08299567055584532,
-            "velocityX": 1.931259898289217,
-            "velocityY": 0.23960929156828065,
-            "timestamp": 1.9348489801288664
-        },
-        {
-            "x": 5.125913784825429,
-            "y": 6.623785858368443,
-            "heading": 0.2608476597362627,
-            "angularVelocity": 0.05532881451478021,
-            "velocityX": 1.2875066378045663,
-            "velocityY": 0.15973953254406137,
-            "timestamp": 2.0126524067699414
-        },
-        {
-            "x": 5.175999999999999,
-            "y": 6.63,
-            "heading": 0.26300000000000007,
-            "angularVelocity": 0.02766382352150404,
-            "velocityX": 0.6437533324290226,
-            "velocityY": 0.07986976795027836,
-            "timestamp": 2.090455833411016
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": -5.112167395133813e-18,
-            "velocityX": -1.2520036782163602e-16,
-            "velocityY": -1.3106574021367763e-17,
-            "timestamp": 2.168259260052091
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.1.traj
deleted file mode 100644
index 253677d3..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.1.traj	
+++ /dev/null
@@ -1,265 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -7.3580792676211765e-19,
-            "velocityX": -1.2600069549143939e-17,
-            "velocityY": 7.063684033512061e-18,
-            "timestamp": 0
-        },
-        {
-            "x": 2.277488982590109,
-            "y": 4.417838766017227,
-            "heading": -0.3894665385190853,
-            "angularVelocity": 0.006183201184696677,
-            "velocityX": -0.26091883762393897,
-            "velocityY": 0.6703927825220056,
-            "timestamp": 0.08627593781609397
-        },
-        {
-            "x": 2.232568196785243,
-            "y": 4.5335556038331974,
-            "heading": -0.3884042841517919,
-            "angularVelocity": 0.012312290010209009,
-            "velocityX": -0.520664126544967,
-            "velocityY": 1.3412411472435377,
-            "timestamp": 0.17255187563218793
-        },
-        {
-            "x": 2.165406410226816,
-            "y": 4.707215578005854,
-            "heading": -0.38682100523451757,
-            "angularVelocity": 0.018351338245094856,
-            "velocityX": -0.7784532774547995,
-            "velocityY": 2.012843656858665,
-            "timestamp": 0.2588278134482819
-        },
-        {
-            "x": 2.0763412317308223,
-            "y": 4.9389471550535315,
-            "heading": -0.3847322299034305,
-            "angularVelocity": 0.024210404244363496,
-            "velocityX": -1.0323293000400813,
-            "velocityY": 2.685935185562845,
-            "timestamp": 0.34510375126437587
-        },
-        {
-            "x": 1.9663857850872422,
-            "y": 5.229122444622766,
-            "heading": -0.3821844673054777,
-            "angularVelocity": 0.029530395872206988,
-            "velocityX": -1.2744624912447062,
-            "velocityY": 3.3633397319630034,
-            "timestamp": 0.43137968908046986
-        },
-        {
-            "x": 1.9100206214147308,
-            "y": 5.550595198428695,
-            "heading": -0.3821844591902738,
-            "angularVelocity": 9.406103402067565e-8,
-            "velocityX": -0.653312674417546,
-            "velocityY": 3.7260997903167445,
-            "timestamp": 0.5176556268965639
-        },
-        {
-            "x": 1.8984249514650446,
-            "y": 5.829240348205722,
-            "heading": -0.3690339918971659,
-            "angularVelocity": 0.15242334799236965,
-            "velocityX": -0.13440213161644662,
-            "velocityY": 3.229697141872688,
-            "timestamp": 0.6039315647126579
-        },
-        {
-            "x": 1.916554046418155,
-            "y": 6.053409726810643,
-            "heading": -0.3524665993885735,
-            "angularVelocity": 0.1920279620015428,
-            "velocityX": 0.2101292134517386,
-            "velocityY": 2.5982838816862235,
-            "timestamp": 0.6902075025287518
-        },
-        {
-            "x": 1.9615881089279723,
-            "y": 6.221650822615583,
-            "heading": -0.33415152161256073,
-            "angularVelocity": 0.21228488776393065,
-            "velocityX": 0.5219770848021326,
-            "velocityY": 1.9500349699305692,
-            "timestamp": 0.7764834403448458
-        },
-        {
-            "x": 2.0323565347582764,
-            "y": 6.333414099671539,
-            "heading": -0.3147709502575115,
-            "angularVelocity": 0.22463472256153053,
-            "velocityX": 0.8202568134484147,
-            "velocityY": 1.2954165423758215,
-            "timestamp": 0.8627593781609398
-        },
-        {
-            "x": 2.1282194334415006,
-            "y": 6.388411890330483,
-            "heading": -0.2946952221482423,
-            "angularVelocity": 0.232692087938443,
-            "velocityX": 1.111119752619376,
-            "velocityY": 0.6374638404531465,
-            "timestamp": 0.9490353159770338
-        },
-        {
-            "x": 2.2487735748291016,
-            "y": 6.386467456817627,
-            "heading": -0.27415678824354056,
-            "angularVelocity": 0.23805517997945658,
-            "velocityX": 1.3973089651552055,
-            "velocityY": -0.022537379043057543,
-            "timestamp": 1.0353112537931277
-        },
-        {
-            "x": 2.4244121861833303,
-            "y": 6.383634558684574,
-            "heading": -0.2535069736087744,
-            "angularVelocity": 0.24641991696382376,
-            "velocityX": 2.0959438518486566,
-            "velocityY": -0.03380575250000802,
-            "timestamp": 1.1191105460305186
-        },
-        {
-            "x": 2.658595728295013,
-            "y": 6.37985738170666,
-            "heading": -0.2319955871365083,
-            "angularVelocity": 0.2567012906424869,
-            "velocityX": 2.79457661108014,
-            "velocityY": -0.04507409164284618,
-            "timestamp": 1.2029098382679095
-        },
-        {
-            "x": 2.9513225155845477,
-            "y": 6.37513595307082,
-            "heading": -0.20870362422898678,
-            "angularVelocity": 0.27794939892259674,
-            "velocityX": 3.493189255826692,
-            "velocityY": -0.05634210635652628,
-            "timestamp": 1.2867091305053004
-        },
-        {
-            "x": 3.1855040701620707,
-            "y": 6.371358808150105,
-            "heading": -0.1849223646442006,
-            "angularVelocity": 0.2837883107343998,
-            "velocityX": 2.794552893288424,
-            "velocityY": -0.04507370909548172,
-            "timestamp": 1.3705084227426914
-        },
-        {
-            "x": 3.3611402034786066,
-            "y": 6.368525949985646,
-            "heading": -0.1612618078271142,
-            "angularVelocity": 0.28234793141284903,
-            "velocityX": 2.0959142807433877,
-            "velocityY": -0.03380527554378375,
-            "timestamp": 1.4543077149800823
-        },
-        {
-            "x": 3.478230941686313,
-            "y": 6.366637378155631,
-            "heading": -0.13785841940389754,
-            "angularVelocity": 0.2792790702446555,
-            "velocityX": 1.3972759802792332,
-            "velocityY": -0.022536847025679124,
-            "timestamp": 1.5381070072174732
-        },
-        {
-            "x": 3.536776304244995,
-            "y": 6.365693092346191,
-            "heading": -0.11477234815509793,
-            "angularVelocity": 0.2754924371366116,
-            "velocityX": 0.6986379120342898,
-            "velocityY": -0.01126842225306853,
-            "timestamp": 1.621906299454864
-        },
-        {
-            "x": 3.536776304244995,
-            "y": 6.365693092346191,
-            "heading": -0.09203745526508908,
-            "angularVelocity": 0.27130172920323553,
-            "velocityX": -5.971595919811331e-16,
-            "velocityY": 2.294938009446505e-16,
-            "timestamp": 1.705705591692255
-        },
-        {
-            "x": 3.5821674028193073,
-            "y": 6.315334924760779,
-            "heading": -0.06814456679249249,
-            "angularVelocity": 0.26497230333398186,
-            "velocityX": 0.5033876064793726,
-            "velocityY": -0.558472437189569,
-            "timestamp": 1.7958768593000762
-        },
-        {
-            "x": 3.6729493357217975,
-            "y": 6.214618882752082,
-            "heading": -0.0452520324499122,
-            "angularVelocity": 0.2538783689072417,
-            "velocityX": 1.006772282467237,
-            "velocityY": -1.1169416232090381,
-            "timestamp": 1.8860481269078975
-        },
-        {
-            "x": 3.809120897123708,
-            "y": 6.063546304100605,
-            "heading": -0.024382898724538877,
-            "angularVelocity": 0.23143884165108508,
-            "velocityX": 1.5101435858055878,
-            "velocityY": -1.6753959732332042,
-            "timestamp": 1.9762193945157187
-        },
-        {
-            "x": 3.9906611673735113,
-            "y": 5.862140397658975,
-            "heading": -0.011123644415836652,
-            "angularVelocity": 0.14704522472038728,
-            "velocityX": 2.0132828900595015,
-            "velocityY": -2.2335929369164194,
-            "timestamp": 2.06639066212354
-        },
-        {
-            "x": 4.126829478795296,
-            "y": 5.711071424627299,
-            "heading": -0.0049044530235942295,
-            "angularVelocity": 0.06897087683516459,
-            "velocityX": 1.5101075435028497,
-            "velocityY": -1.6753559868839316,
-            "timestamp": 2.1565619297313616
-        },
-        {
-            "x": 4.217609649564756,
-            "y": 5.610357337578787,
-            "heading": -0.0015015365594071913,
-            "angularVelocity": 0.03773836782456074,
-            "velocityX": 1.0067527403993772,
-            "velocityY": -1.1169199426866636,
-            "timestamp": 2.246733197339183
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 4.052308831945937e-18,
-            "angularVelocity": 0.016652051138278397,
-            "velocityX": 0.5033793096117788,
-            "velocityY": -0.5584632324101773,
-            "timestamp": 2.3369044649470045
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 1.897041849287005e-18,
-            "angularVelocity": -2.8636978124151836e-18,
-            "velocityX": 2.1901737652514486e-16,
-            "velocityY": -2.4329458809860463e-16,
-            "timestamp": 2.427075732554826
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.traj
deleted file mode 100644
index 253677d3..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.traj	
+++ /dev/null
@@ -1,265 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -7.3580792676211765e-19,
-            "velocityX": -1.2600069549143939e-17,
-            "velocityY": 7.063684033512061e-18,
-            "timestamp": 0
-        },
-        {
-            "x": 2.277488982590109,
-            "y": 4.417838766017227,
-            "heading": -0.3894665385190853,
-            "angularVelocity": 0.006183201184696677,
-            "velocityX": -0.26091883762393897,
-            "velocityY": 0.6703927825220056,
-            "timestamp": 0.08627593781609397
-        },
-        {
-            "x": 2.232568196785243,
-            "y": 4.5335556038331974,
-            "heading": -0.3884042841517919,
-            "angularVelocity": 0.012312290010209009,
-            "velocityX": -0.520664126544967,
-            "velocityY": 1.3412411472435377,
-            "timestamp": 0.17255187563218793
-        },
-        {
-            "x": 2.165406410226816,
-            "y": 4.707215578005854,
-            "heading": -0.38682100523451757,
-            "angularVelocity": 0.018351338245094856,
-            "velocityX": -0.7784532774547995,
-            "velocityY": 2.012843656858665,
-            "timestamp": 0.2588278134482819
-        },
-        {
-            "x": 2.0763412317308223,
-            "y": 4.9389471550535315,
-            "heading": -0.3847322299034305,
-            "angularVelocity": 0.024210404244363496,
-            "velocityX": -1.0323293000400813,
-            "velocityY": 2.685935185562845,
-            "timestamp": 0.34510375126437587
-        },
-        {
-            "x": 1.9663857850872422,
-            "y": 5.229122444622766,
-            "heading": -0.3821844673054777,
-            "angularVelocity": 0.029530395872206988,
-            "velocityX": -1.2744624912447062,
-            "velocityY": 3.3633397319630034,
-            "timestamp": 0.43137968908046986
-        },
-        {
-            "x": 1.9100206214147308,
-            "y": 5.550595198428695,
-            "heading": -0.3821844591902738,
-            "angularVelocity": 9.406103402067565e-8,
-            "velocityX": -0.653312674417546,
-            "velocityY": 3.7260997903167445,
-            "timestamp": 0.5176556268965639
-        },
-        {
-            "x": 1.8984249514650446,
-            "y": 5.829240348205722,
-            "heading": -0.3690339918971659,
-            "angularVelocity": 0.15242334799236965,
-            "velocityX": -0.13440213161644662,
-            "velocityY": 3.229697141872688,
-            "timestamp": 0.6039315647126579
-        },
-        {
-            "x": 1.916554046418155,
-            "y": 6.053409726810643,
-            "heading": -0.3524665993885735,
-            "angularVelocity": 0.1920279620015428,
-            "velocityX": 0.2101292134517386,
-            "velocityY": 2.5982838816862235,
-            "timestamp": 0.6902075025287518
-        },
-        {
-            "x": 1.9615881089279723,
-            "y": 6.221650822615583,
-            "heading": -0.33415152161256073,
-            "angularVelocity": 0.21228488776393065,
-            "velocityX": 0.5219770848021326,
-            "velocityY": 1.9500349699305692,
-            "timestamp": 0.7764834403448458
-        },
-        {
-            "x": 2.0323565347582764,
-            "y": 6.333414099671539,
-            "heading": -0.3147709502575115,
-            "angularVelocity": 0.22463472256153053,
-            "velocityX": 0.8202568134484147,
-            "velocityY": 1.2954165423758215,
-            "timestamp": 0.8627593781609398
-        },
-        {
-            "x": 2.1282194334415006,
-            "y": 6.388411890330483,
-            "heading": -0.2946952221482423,
-            "angularVelocity": 0.232692087938443,
-            "velocityX": 1.111119752619376,
-            "velocityY": 0.6374638404531465,
-            "timestamp": 0.9490353159770338
-        },
-        {
-            "x": 2.2487735748291016,
-            "y": 6.386467456817627,
-            "heading": -0.27415678824354056,
-            "angularVelocity": 0.23805517997945658,
-            "velocityX": 1.3973089651552055,
-            "velocityY": -0.022537379043057543,
-            "timestamp": 1.0353112537931277
-        },
-        {
-            "x": 2.4244121861833303,
-            "y": 6.383634558684574,
-            "heading": -0.2535069736087744,
-            "angularVelocity": 0.24641991696382376,
-            "velocityX": 2.0959438518486566,
-            "velocityY": -0.03380575250000802,
-            "timestamp": 1.1191105460305186
-        },
-        {
-            "x": 2.658595728295013,
-            "y": 6.37985738170666,
-            "heading": -0.2319955871365083,
-            "angularVelocity": 0.2567012906424869,
-            "velocityX": 2.79457661108014,
-            "velocityY": -0.04507409164284618,
-            "timestamp": 1.2029098382679095
-        },
-        {
-            "x": 2.9513225155845477,
-            "y": 6.37513595307082,
-            "heading": -0.20870362422898678,
-            "angularVelocity": 0.27794939892259674,
-            "velocityX": 3.493189255826692,
-            "velocityY": -0.05634210635652628,
-            "timestamp": 1.2867091305053004
-        },
-        {
-            "x": 3.1855040701620707,
-            "y": 6.371358808150105,
-            "heading": -0.1849223646442006,
-            "angularVelocity": 0.2837883107343998,
-            "velocityX": 2.794552893288424,
-            "velocityY": -0.04507370909548172,
-            "timestamp": 1.3705084227426914
-        },
-        {
-            "x": 3.3611402034786066,
-            "y": 6.368525949985646,
-            "heading": -0.1612618078271142,
-            "angularVelocity": 0.28234793141284903,
-            "velocityX": 2.0959142807433877,
-            "velocityY": -0.03380527554378375,
-            "timestamp": 1.4543077149800823
-        },
-        {
-            "x": 3.478230941686313,
-            "y": 6.366637378155631,
-            "heading": -0.13785841940389754,
-            "angularVelocity": 0.2792790702446555,
-            "velocityX": 1.3972759802792332,
-            "velocityY": -0.022536847025679124,
-            "timestamp": 1.5381070072174732
-        },
-        {
-            "x": 3.536776304244995,
-            "y": 6.365693092346191,
-            "heading": -0.11477234815509793,
-            "angularVelocity": 0.2754924371366116,
-            "velocityX": 0.6986379120342898,
-            "velocityY": -0.01126842225306853,
-            "timestamp": 1.621906299454864
-        },
-        {
-            "x": 3.536776304244995,
-            "y": 6.365693092346191,
-            "heading": -0.09203745526508908,
-            "angularVelocity": 0.27130172920323553,
-            "velocityX": -5.971595919811331e-16,
-            "velocityY": 2.294938009446505e-16,
-            "timestamp": 1.705705591692255
-        },
-        {
-            "x": 3.5821674028193073,
-            "y": 6.315334924760779,
-            "heading": -0.06814456679249249,
-            "angularVelocity": 0.26497230333398186,
-            "velocityX": 0.5033876064793726,
-            "velocityY": -0.558472437189569,
-            "timestamp": 1.7958768593000762
-        },
-        {
-            "x": 3.6729493357217975,
-            "y": 6.214618882752082,
-            "heading": -0.0452520324499122,
-            "angularVelocity": 0.2538783689072417,
-            "velocityX": 1.006772282467237,
-            "velocityY": -1.1169416232090381,
-            "timestamp": 1.8860481269078975
-        },
-        {
-            "x": 3.809120897123708,
-            "y": 6.063546304100605,
-            "heading": -0.024382898724538877,
-            "angularVelocity": 0.23143884165108508,
-            "velocityX": 1.5101435858055878,
-            "velocityY": -1.6753959732332042,
-            "timestamp": 1.9762193945157187
-        },
-        {
-            "x": 3.9906611673735113,
-            "y": 5.862140397658975,
-            "heading": -0.011123644415836652,
-            "angularVelocity": 0.14704522472038728,
-            "velocityX": 2.0132828900595015,
-            "velocityY": -2.2335929369164194,
-            "timestamp": 2.06639066212354
-        },
-        {
-            "x": 4.126829478795296,
-            "y": 5.711071424627299,
-            "heading": -0.0049044530235942295,
-            "angularVelocity": 0.06897087683516459,
-            "velocityX": 1.5101075435028497,
-            "velocityY": -1.6753559868839316,
-            "timestamp": 2.1565619297313616
-        },
-        {
-            "x": 4.217609649564756,
-            "y": 5.610357337578787,
-            "heading": -0.0015015365594071913,
-            "angularVelocity": 0.03773836782456074,
-            "velocityX": 1.0067527403993772,
-            "velocityY": -1.1169199426866636,
-            "timestamp": 2.246733197339183
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 4.052308831945937e-18,
-            "angularVelocity": 0.016652051138278397,
-            "velocityX": 0.5033793096117788,
-            "velocityY": -0.5584632324101773,
-            "timestamp": 2.3369044649470045
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 1.897041849287005e-18,
-            "angularVelocity": -2.8636978124151836e-18,
-            "velocityX": 2.1901737652514486e-16,
-            "velocityY": -2.4329458809860463e-16,
-            "timestamp": 2.427075732554826
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.1.traj
deleted file mode 100644
index 4ff6b6c8..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.1.traj	
+++ /dev/null
@@ -1,229 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.284359298751307,
-            "y": 4.3328670624584875,
-            "heading": -0.3913603360985968,
-            "angularVelocity": -0.02219557362709146,
-            "velocityX": -0.2551974739939649,
-            "velocityY": -0.44270758788444614,
-            "timestamp": 0.061288620940906494
-        },
-        {
-            "x": 2.2541133789559487,
-            "y": 4.2780305351291235,
-            "heading": -0.3941996942145126,
-            "angularVelocity": -0.04632765548197678,
-            "velocityX": -0.4934997611468709,
-            "velocityY": -0.89472607618691,
-            "timestamp": 0.12257724188181299
-        },
-        {
-            "x": 2.2108693669713233,
-            "y": 4.194701771021148,
-            "heading": -0.39869373638946143,
-            "angularVelocity": -0.0733258818024626,
-            "velocityX": -0.7055797849705345,
-            "velocityY": -1.3596123200148318,
-            "timestamp": 0.1838658628227195
-        },
-        {
-            "x": 2.157413012714945,
-            "y": 4.081769184110331,
-            "heading": -0.40512443208979565,
-            "angularVelocity": -0.1049247903054391,
-            "velocityX": -0.8722068376757797,
-            "velocityY": -1.8426354709417345,
-            "timestamp": 0.24515448376362597
-        },
-        {
-            "x": 2.099439422754663,
-            "y": 3.9378510886177804,
-            "heading": -0.41398926531008917,
-            "angularVelocity": -0.1446407682894496,
-            "velocityX": -0.9459111507204496,
-            "velocityY": -2.348202542056109,
-            "timestamp": 0.30644310470453245
-        },
-        {
-            "x": 2.05084134036617,
-            "y": 3.764062564560248,
-            "heading": -0.42610260644735526,
-            "angularVelocity": -0.1976442111325286,
-            "velocityX": -0.7929380958881483,
-            "velocityY": -2.835575697242347,
-            "timestamp": 0.36773172564543893
-        },
-        {
-            "x": 2.0313297442556637,
-            "y": 3.578686605657539,
-            "heading": -0.44115755081157715,
-            "angularVelocity": -0.24564012263773397,
-            "velocityX": -0.3183559331400068,
-            "velocityY": -3.024639093795978,
-            "timestamp": 0.4290203465863454
-        },
-        {
-            "x": 2.0424292488902682,
-            "y": 3.3999266361315956,
-            "heading": -0.45764648760363913,
-            "angularVelocity": -0.2690374907923018,
-            "velocityX": 0.18110220892890863,
-            "velocityY": -2.9166910069375023,
-            "timestamp": 0.4903089675272519
-        },
-        {
-            "x": 2.0816987250314285,
-            "y": 3.234855796713624,
-            "heading": -0.4747853916237668,
-            "angularVelocity": -0.27964251368378285,
-            "velocityX": 0.6407302944379066,
-            "velocityY": -2.6933358408754287,
-            "timestamp": 0.5515975884681584
-        },
-        {
-            "x": 2.1473776050382467,
-            "y": 3.086622748034336,
-            "heading": -0.49218081575104855,
-            "angularVelocity": -0.2838279579508591,
-            "velocityX": 1.071632531431,
-            "velocityY": -2.418606364502998,
-            "timestamp": 0.6128862094090649
-        },
-        {
-            "x": 2.2382969856262207,
-            "y": 2.9569339752197266,
-            "heading": -0.5096053711064815,
-            "angularVelocity": -0.2843032701328595,
-            "velocityX": 1.4834626590087072,
-            "velocityY": -2.116033463041253,
-            "timestamp": 0.6741748303499714
-        },
-        {
-            "x": 2.3750291535583634,
-            "y": 2.833789120602945,
-            "heading": -0.5294729435704262,
-            "angularVelocity": -0.282105779182567,
-            "velocityX": 1.9415021560293924,
-            "velocityY": -1.7485717103605045,
-            "timestamp": 0.7446007979480589
-        },
-        {
-            "x": 2.5431654959507464,
-            "y": 2.737552434720476,
-            "heading": -0.5488979650801319,
-            "angularVelocity": -0.2758218619098283,
-            "velocityX": 2.3874196993915153,
-            "velocityY": -1.366494336743516,
-            "timestamp": 0.8150267655461465
-        },
-        {
-            "x": 2.7413669689925393,
-            "y": 2.669711436345154,
-            "heading": -0.5674506234763109,
-            "angularVelocity": -0.2634349094364851,
-            "velocityX": 2.8143237473555907,
-            "velocityY": -0.9632952260234809,
-            "timestamp": 0.8854527331442341
-        },
-        {
-            "x": 2.9672720655352354,
-            "y": 2.6325722400915463,
-            "heading": -0.5844267226661307,
-            "angularVelocity": -0.24104885979984425,
-            "velocityX": 3.207696028145596,
-            "velocityY": -0.5273508837756647,
-            "timestamp": 0.9558787007423217
-        },
-        {
-            "x": 3.21586763898004,
-            "y": 2.629998478445332,
-            "heading": -0.5985057428976699,
-            "angularVelocity": -0.19991234358165255,
-            "velocityX": 3.529885096694866,
-            "velocityY": -0.03654563414595668,
-            "timestamp": 1.0263046683404093
-        },
-        {
-            "x": 3.4730987079905056,
-            "y": 2.6678365706373897,
-            "heading": -0.6068355688677516,
-            "angularVelocity": -0.11827776392962061,
-            "velocityX": 3.652503157336117,
-            "velocityY": 0.5372747224148254,
-            "timestamp": 1.096730635938497
-        },
-        {
-            "x": 3.704150591962856,
-            "y": 2.737591563426048,
-            "heading": -0.6062385008661115,
-            "angularVelocity": 0.00847795240879764,
-            "velocityX": 3.28077684769537,
-            "velocityY": 0.9904726220694814,
-            "timestamp": 1.1671566035365846
-        },
-        {
-            "x": 3.894448488053554,
-            "y": 2.8141322684658854,
-            "heading": -0.5991077413907381,
-            "angularVelocity": 0.10125184954600595,
-            "velocityX": 2.702098424500253,
-            "velocityY": 1.086825039829707,
-            "timestamp": 1.2375825711346722
-        },
-        {
-            "x": 4.043960756386648,
-            "y": 2.883897596305393,
-            "heading": -0.5885679211534333,
-            "angularVelocity": 0.1496581530473843,
-            "velocityX": 2.122970736963697,
-            "velocityY": 0.9906193726389452,
-            "timestamp": 1.3080085387327598
-        },
-        {
-            "x": 4.154363809487208,
-            "y": 2.940218619761556,
-            "heading": -0.5783208756256007,
-            "angularVelocity": 0.14550095479427785,
-            "velocityX": 1.5676469470837475,
-            "velocityY": 0.7997195548321091,
-            "timestamp": 1.3784345063308474
-        },
-        {
-            "x": 4.227038806897989,
-            "y": 2.9796144762337837,
-            "heading": -0.5703846528919979,
-            "angularVelocity": 0.11268887037369305,
-            "velocityX": 1.0319346668480025,
-            "velocityY": 0.5593938971070241,
-            "timestamp": 1.448860473928935
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 0.06225903656760019,
-            "velocityX": 0.5106240542868632,
-            "velocityY": 0.2894603292148455,
-            "timestamp": 1.5192864415270226
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 9.24329495349137e-32,
-            "velocityX": 0,
-            "velocityY": 1.2740379664097394e-33,
-            "timestamp": 1.5897124091251102
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.traj
deleted file mode 100644
index 4ff6b6c8..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.traj	
+++ /dev/null
@@ -1,229 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.284359298751307,
-            "y": 4.3328670624584875,
-            "heading": -0.3913603360985968,
-            "angularVelocity": -0.02219557362709146,
-            "velocityX": -0.2551974739939649,
-            "velocityY": -0.44270758788444614,
-            "timestamp": 0.061288620940906494
-        },
-        {
-            "x": 2.2541133789559487,
-            "y": 4.2780305351291235,
-            "heading": -0.3941996942145126,
-            "angularVelocity": -0.04632765548197678,
-            "velocityX": -0.4934997611468709,
-            "velocityY": -0.89472607618691,
-            "timestamp": 0.12257724188181299
-        },
-        {
-            "x": 2.2108693669713233,
-            "y": 4.194701771021148,
-            "heading": -0.39869373638946143,
-            "angularVelocity": -0.0733258818024626,
-            "velocityX": -0.7055797849705345,
-            "velocityY": -1.3596123200148318,
-            "timestamp": 0.1838658628227195
-        },
-        {
-            "x": 2.157413012714945,
-            "y": 4.081769184110331,
-            "heading": -0.40512443208979565,
-            "angularVelocity": -0.1049247903054391,
-            "velocityX": -0.8722068376757797,
-            "velocityY": -1.8426354709417345,
-            "timestamp": 0.24515448376362597
-        },
-        {
-            "x": 2.099439422754663,
-            "y": 3.9378510886177804,
-            "heading": -0.41398926531008917,
-            "angularVelocity": -0.1446407682894496,
-            "velocityX": -0.9459111507204496,
-            "velocityY": -2.348202542056109,
-            "timestamp": 0.30644310470453245
-        },
-        {
-            "x": 2.05084134036617,
-            "y": 3.764062564560248,
-            "heading": -0.42610260644735526,
-            "angularVelocity": -0.1976442111325286,
-            "velocityX": -0.7929380958881483,
-            "velocityY": -2.835575697242347,
-            "timestamp": 0.36773172564543893
-        },
-        {
-            "x": 2.0313297442556637,
-            "y": 3.578686605657539,
-            "heading": -0.44115755081157715,
-            "angularVelocity": -0.24564012263773397,
-            "velocityX": -0.3183559331400068,
-            "velocityY": -3.024639093795978,
-            "timestamp": 0.4290203465863454
-        },
-        {
-            "x": 2.0424292488902682,
-            "y": 3.3999266361315956,
-            "heading": -0.45764648760363913,
-            "angularVelocity": -0.2690374907923018,
-            "velocityX": 0.18110220892890863,
-            "velocityY": -2.9166910069375023,
-            "timestamp": 0.4903089675272519
-        },
-        {
-            "x": 2.0816987250314285,
-            "y": 3.234855796713624,
-            "heading": -0.4747853916237668,
-            "angularVelocity": -0.27964251368378285,
-            "velocityX": 0.6407302944379066,
-            "velocityY": -2.6933358408754287,
-            "timestamp": 0.5515975884681584
-        },
-        {
-            "x": 2.1473776050382467,
-            "y": 3.086622748034336,
-            "heading": -0.49218081575104855,
-            "angularVelocity": -0.2838279579508591,
-            "velocityX": 1.071632531431,
-            "velocityY": -2.418606364502998,
-            "timestamp": 0.6128862094090649
-        },
-        {
-            "x": 2.2382969856262207,
-            "y": 2.9569339752197266,
-            "heading": -0.5096053711064815,
-            "angularVelocity": -0.2843032701328595,
-            "velocityX": 1.4834626590087072,
-            "velocityY": -2.116033463041253,
-            "timestamp": 0.6741748303499714
-        },
-        {
-            "x": 2.3750291535583634,
-            "y": 2.833789120602945,
-            "heading": -0.5294729435704262,
-            "angularVelocity": -0.282105779182567,
-            "velocityX": 1.9415021560293924,
-            "velocityY": -1.7485717103605045,
-            "timestamp": 0.7446007979480589
-        },
-        {
-            "x": 2.5431654959507464,
-            "y": 2.737552434720476,
-            "heading": -0.5488979650801319,
-            "angularVelocity": -0.2758218619098283,
-            "velocityX": 2.3874196993915153,
-            "velocityY": -1.366494336743516,
-            "timestamp": 0.8150267655461465
-        },
-        {
-            "x": 2.7413669689925393,
-            "y": 2.669711436345154,
-            "heading": -0.5674506234763109,
-            "angularVelocity": -0.2634349094364851,
-            "velocityX": 2.8143237473555907,
-            "velocityY": -0.9632952260234809,
-            "timestamp": 0.8854527331442341
-        },
-        {
-            "x": 2.9672720655352354,
-            "y": 2.6325722400915463,
-            "heading": -0.5844267226661307,
-            "angularVelocity": -0.24104885979984425,
-            "velocityX": 3.207696028145596,
-            "velocityY": -0.5273508837756647,
-            "timestamp": 0.9558787007423217
-        },
-        {
-            "x": 3.21586763898004,
-            "y": 2.629998478445332,
-            "heading": -0.5985057428976699,
-            "angularVelocity": -0.19991234358165255,
-            "velocityX": 3.529885096694866,
-            "velocityY": -0.03654563414595668,
-            "timestamp": 1.0263046683404093
-        },
-        {
-            "x": 3.4730987079905056,
-            "y": 2.6678365706373897,
-            "heading": -0.6068355688677516,
-            "angularVelocity": -0.11827776392962061,
-            "velocityX": 3.652503157336117,
-            "velocityY": 0.5372747224148254,
-            "timestamp": 1.096730635938497
-        },
-        {
-            "x": 3.704150591962856,
-            "y": 2.737591563426048,
-            "heading": -0.6062385008661115,
-            "angularVelocity": 0.00847795240879764,
-            "velocityX": 3.28077684769537,
-            "velocityY": 0.9904726220694814,
-            "timestamp": 1.1671566035365846
-        },
-        {
-            "x": 3.894448488053554,
-            "y": 2.8141322684658854,
-            "heading": -0.5991077413907381,
-            "angularVelocity": 0.10125184954600595,
-            "velocityX": 2.702098424500253,
-            "velocityY": 1.086825039829707,
-            "timestamp": 1.2375825711346722
-        },
-        {
-            "x": 4.043960756386648,
-            "y": 2.883897596305393,
-            "heading": -0.5885679211534333,
-            "angularVelocity": 0.1496581530473843,
-            "velocityX": 2.122970736963697,
-            "velocityY": 0.9906193726389452,
-            "timestamp": 1.3080085387327598
-        },
-        {
-            "x": 4.154363809487208,
-            "y": 2.940218619761556,
-            "heading": -0.5783208756256007,
-            "angularVelocity": 0.14550095479427785,
-            "velocityX": 1.5676469470837475,
-            "velocityY": 0.7997195548321091,
-            "timestamp": 1.3784345063308474
-        },
-        {
-            "x": 4.227038806897989,
-            "y": 2.9796144762337837,
-            "heading": -0.5703846528919979,
-            "angularVelocity": 0.11268887037369305,
-            "velocityX": 1.0319346668480025,
-            "velocityY": 0.5593938971070241,
-            "timestamp": 1.448860473928935
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 0.06225903656760019,
-            "velocityX": 0.5106240542868632,
-            "velocityY": 0.2894603292148455,
-            "timestamp": 1.5192864415270226
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 9.24329495349137e-32,
-            "velocityX": 0,
-            "velocityY": 1.2740379664097394e-33,
-            "timestamp": 1.5897124091251102
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.1.traj
deleted file mode 100644
index 2b0725c8..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.1.traj	
+++ /dev/null
@@ -1,256 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 9.57151893871228e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2927892107770984,
-            "y": 4.332309394440523,
-            "heading": -0.3722824674321399,
-            "angularVelocity": 0.3002660031020358,
-            "velocityX": -0.12220408518393268,
-            "velocityY": -0.4692835993371687,
-            "timestamp": 0.059006122520770025
-        },
-        {
-            "x": 2.2795380946752672,
-            "y": 4.276530177721837,
-            "heading": -0.3395678297253276,
-            "angularVelocity": 0.5544278510301563,
-            "velocityX": -0.224571883996739,
-            "velocityY": -0.9453123563415277,
-            "timestamp": 0.11801224504154005
-        },
-        {
-            "x": 2.2618545093830753,
-            "y": 4.192237257969867,
-            "heading": -0.2954402922678697,
-            "angularVelocity": 0.7478467584770477,
-            "velocityX": -0.29969068524994186,
-            "velocityY": -1.4285453127732544,
-            "timestamp": 0.17701836756231007
-        },
-        {
-            "x": 2.2420434066604593,
-            "y": 4.079048226765037,
-            "heading": -0.24482967906698166,
-            "angularVelocity": 0.8577179966887506,
-            "velocityX": -0.33574656114103973,
-            "velocityY": -1.9182590953165608,
-            "timestamp": 0.2360244900830801
-        },
-        {
-            "x": 2.2234541825747725,
-            "y": 3.936854095556344,
-            "heading": -0.19463836205748358,
-            "angularVelocity": 0.8506120189787272,
-            "velocityX": -0.3150389026010499,
-            "velocityY": -2.40981994976269,
-            "timestamp": 0.2950306126038501
-        },
-        {
-            "x": 2.211142623620853,
-            "y": 3.7664761504723194,
-            "heading": -0.15521354512815658,
-            "angularVelocity": 0.6681479013546628,
-            "velocityX": -0.2086488389333828,
-            "velocityY": -2.8874621446961175,
-            "timestamp": 0.35403673512462014
-        },
-        {
-            "x": 2.2125400151692816,
-            "y": 3.5715383450180926,
-            "heading": -0.14131017152189063,
-            "angularVelocity": 0.23562594883900537,
-            "velocityX": 0.023682144983125607,
-            "velocityY": -3.303687772156347,
-            "timestamp": 0.41304285764539017
-        },
-        {
-            "x": 2.235363094184648,
-            "y": 3.3576277458759463,
-            "heading": -0.144791073330025,
-            "angularVelocity": -0.058992214018278255,
-            "velocityX": 0.38679170974728244,
-            "velocityY": -3.6252271798888405,
-            "timestamp": 0.4720489801661602
-        },
-        {
-            "x": 2.287018648077521,
-            "y": 3.1404706845748738,
-            "heading": -0.14479216409186968,
-            "angularVelocity": -0.000018485570624385508,
-            "velocityX": 0.8754270181825738,
-            "velocityY": -3.6802462528296687,
-            "timestamp": 0.5310551026869302
-        },
-        {
-            "x": 2.3664206535422005,
-            "y": 2.9318538879811458,
-            "heading": -0.14479229465212481,
-            "angularVelocity": -0.000002212656069584398,
-            "velocityX": 1.3456570618876043,
-            "velocityY": -3.53551102295012,
-            "timestamp": 0.5900612252077002
-        },
-        {
-            "x": 2.4722259044647217,
-            "y": 2.73530650138855,
-            "heading": -0.14479252387880184,
-            "angularVelocity": -0.000003884794784189704,
-            "velocityX": 1.7931232625102267,
-            "velocityY": -3.330965977698533,
-            "timestamp": 0.6490673477284702
-        },
-        {
-            "x": 2.6222628757424284,
-            "y": 2.533912575740545,
-            "heading": -0.14479267276909355,
-            "angularVelocity": -0.0000022427572708026126,
-            "velocityX": 2.260029880862475,
-            "velocityY": -3.033627551346843,
-            "timestamp": 0.7154545103709612
-        },
-        {
-            "x": 2.800083925097921,
-            "y": 2.356570260194387,
-            "heading": -0.14479275333723068,
-            "angularVelocity": -0.0000012136101911737535,
-            "velocityX": 2.678545704884188,
-            "velocityY": -2.671334464182248,
-            "timestamp": 0.7818416730134522
-        },
-        {
-            "x": 3.001881619185789,
-            "y": 2.207076716019015,
-            "heading": -0.14479280967120484,
-            "angularVelocity": -8.485672817028763e-7,
-            "velocityX": 3.0397095772053357,
-            "velocityY": -2.2518441551784094,
-            "timestamp": 0.8482288356559432
-        },
-        {
-            "x": 3.2233351494929763,
-            "y": 2.0886327789398735,
-            "heading": -0.14479285532879432,
-            "angularVelocity": -6.877472642883405e-7,
-            "velocityX": 3.3357884490373766,
-            "velocityY": -1.7841391673415428,
-            "timestamp": 0.9146159982984342
-        },
-        {
-            "x": 3.4597028157497376,
-            "y": 2.003774307484127,
-            "heading": -0.14479289641721121,
-            "angularVelocity": -6.189211176196774e-7,
-            "velocityX": 3.5604423633775646,
-            "velocityY": -1.2782361540698368,
-            "timestamp": 0.9810031609409252
-        },
-        {
-            "x": 3.704835569204283,
-            "y": 1.949179734375487,
-            "heading": -0.14479293648657837,
-            "angularVelocity": -6.035710154638316e-7,
-            "velocityX": 3.692472214464675,
-            "velocityY": -0.8223664174750668,
-            "timestamp": 1.047390323583416
-        },
-        {
-            "x": 3.9499686718209266,
-            "y": 1.8945867290481686,
-            "heading": -0.1447929765559331,
-            "angularVelocity": -6.035708281824289e-7,
-            "velocityX": 3.692477473946853,
-            "velocityY": -0.8223428017448724,
-            "timestamp": 1.113777486225907
-        },
-        {
-            "x": 4.195101784677066,
-            "y": 1.8399937696982192,
-            "heading": -0.14479301662532418,
-            "angularVelocity": -6.035713756667977e-7,
-            "velocityX": 3.692477628185933,
-            "velocityY": -0.8223421091807183,
-            "timestamp": 1.1801646488683981
-        },
-        {
-            "x": 4.435904668374261,
-            "y": 1.7860806487644325,
-            "heading": -0.15898872227240893,
-            "angularVelocity": -0.21383208864538455,
-            "velocityX": 3.627250723064772,
-            "velocityY": -0.8121015989811171,
-            "timestamp": 1.2465518115108891
-        },
-        {
-            "x": 4.648245693564204,
-            "y": 1.7385415440023928,
-            "heading": -0.26462575063007965,
-            "angularVelocity": -1.5912267395211415,
-            "velocityX": 3.1985253886123286,
-            "velocityY": -0.7160888170209653,
-            "timestamp": 1.3129389741533801
-        },
-        {
-            "x": 4.824757883271093,
-            "y": 1.6989793937314068,
-            "heading": -0.37654836939407044,
-            "angularVelocity": -1.6859075506317058,
-            "velocityX": 2.6588301515075172,
-            "velocityY": -0.5959307296206723,
-            "timestamp": 1.3793261367958711
-        },
-        {
-            "x": 4.96555652453039,
-            "y": 1.6673723665346096,
-            "heading": -0.4762340168485147,
-            "angularVelocity": -1.501580177349536,
-            "velocityX": 2.1208715006774397,
-            "velocityY": -0.47610149219673153,
-            "timestamp": 1.4457132994383621
-        },
-        {
-            "x": 5.070906714490224,
-            "y": 1.643682366519874,
-            "heading": -0.5558849180153355,
-            "angularVelocity": -1.1997937251175768,
-            "velocityX": 1.5869060487969315,
-            "velocityY": -0.3568460990313944,
-            "timestamp": 1.5121004620808531
-        },
-        {
-            "x": 5.141006607602348,
-            "y": 1.6278935430132826,
-            "heading": -0.6113350442973207,
-            "angularVelocity": -0.8352537459779052,
-            "velocityX": 1.055925427776252,
-            "velocityY": -0.23782946699526453,
-            "timestamp": 1.5784876247233441
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -0.4317846186174008,
-            "velocityX": 0.5271108299370921,
-            "velocityY": -0.11890164753374041,
-            "timestamp": 1.6448747873658351
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 1.0662498702136072e-32,
-            "velocityX": 0,
-            "velocityY": 8.479039452208929e-33,
-            "timestamp": 1.7112619500083261
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.traj
deleted file mode 100644
index 2b0725c8..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.traj	
+++ /dev/null
@@ -1,256 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 9.57151893871228e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2927892107770984,
-            "y": 4.332309394440523,
-            "heading": -0.3722824674321399,
-            "angularVelocity": 0.3002660031020358,
-            "velocityX": -0.12220408518393268,
-            "velocityY": -0.4692835993371687,
-            "timestamp": 0.059006122520770025
-        },
-        {
-            "x": 2.2795380946752672,
-            "y": 4.276530177721837,
-            "heading": -0.3395678297253276,
-            "angularVelocity": 0.5544278510301563,
-            "velocityX": -0.224571883996739,
-            "velocityY": -0.9453123563415277,
-            "timestamp": 0.11801224504154005
-        },
-        {
-            "x": 2.2618545093830753,
-            "y": 4.192237257969867,
-            "heading": -0.2954402922678697,
-            "angularVelocity": 0.7478467584770477,
-            "velocityX": -0.29969068524994186,
-            "velocityY": -1.4285453127732544,
-            "timestamp": 0.17701836756231007
-        },
-        {
-            "x": 2.2420434066604593,
-            "y": 4.079048226765037,
-            "heading": -0.24482967906698166,
-            "angularVelocity": 0.8577179966887506,
-            "velocityX": -0.33574656114103973,
-            "velocityY": -1.9182590953165608,
-            "timestamp": 0.2360244900830801
-        },
-        {
-            "x": 2.2234541825747725,
-            "y": 3.936854095556344,
-            "heading": -0.19463836205748358,
-            "angularVelocity": 0.8506120189787272,
-            "velocityX": -0.3150389026010499,
-            "velocityY": -2.40981994976269,
-            "timestamp": 0.2950306126038501
-        },
-        {
-            "x": 2.211142623620853,
-            "y": 3.7664761504723194,
-            "heading": -0.15521354512815658,
-            "angularVelocity": 0.6681479013546628,
-            "velocityX": -0.2086488389333828,
-            "velocityY": -2.8874621446961175,
-            "timestamp": 0.35403673512462014
-        },
-        {
-            "x": 2.2125400151692816,
-            "y": 3.5715383450180926,
-            "heading": -0.14131017152189063,
-            "angularVelocity": 0.23562594883900537,
-            "velocityX": 0.023682144983125607,
-            "velocityY": -3.303687772156347,
-            "timestamp": 0.41304285764539017
-        },
-        {
-            "x": 2.235363094184648,
-            "y": 3.3576277458759463,
-            "heading": -0.144791073330025,
-            "angularVelocity": -0.058992214018278255,
-            "velocityX": 0.38679170974728244,
-            "velocityY": -3.6252271798888405,
-            "timestamp": 0.4720489801661602
-        },
-        {
-            "x": 2.287018648077521,
-            "y": 3.1404706845748738,
-            "heading": -0.14479216409186968,
-            "angularVelocity": -0.000018485570624385508,
-            "velocityX": 0.8754270181825738,
-            "velocityY": -3.6802462528296687,
-            "timestamp": 0.5310551026869302
-        },
-        {
-            "x": 2.3664206535422005,
-            "y": 2.9318538879811458,
-            "heading": -0.14479229465212481,
-            "angularVelocity": -0.000002212656069584398,
-            "velocityX": 1.3456570618876043,
-            "velocityY": -3.53551102295012,
-            "timestamp": 0.5900612252077002
-        },
-        {
-            "x": 2.4722259044647217,
-            "y": 2.73530650138855,
-            "heading": -0.14479252387880184,
-            "angularVelocity": -0.000003884794784189704,
-            "velocityX": 1.7931232625102267,
-            "velocityY": -3.330965977698533,
-            "timestamp": 0.6490673477284702
-        },
-        {
-            "x": 2.6222628757424284,
-            "y": 2.533912575740545,
-            "heading": -0.14479267276909355,
-            "angularVelocity": -0.0000022427572708026126,
-            "velocityX": 2.260029880862475,
-            "velocityY": -3.033627551346843,
-            "timestamp": 0.7154545103709612
-        },
-        {
-            "x": 2.800083925097921,
-            "y": 2.356570260194387,
-            "heading": -0.14479275333723068,
-            "angularVelocity": -0.0000012136101911737535,
-            "velocityX": 2.678545704884188,
-            "velocityY": -2.671334464182248,
-            "timestamp": 0.7818416730134522
-        },
-        {
-            "x": 3.001881619185789,
-            "y": 2.207076716019015,
-            "heading": -0.14479280967120484,
-            "angularVelocity": -8.485672817028763e-7,
-            "velocityX": 3.0397095772053357,
-            "velocityY": -2.2518441551784094,
-            "timestamp": 0.8482288356559432
-        },
-        {
-            "x": 3.2233351494929763,
-            "y": 2.0886327789398735,
-            "heading": -0.14479285532879432,
-            "angularVelocity": -6.877472642883405e-7,
-            "velocityX": 3.3357884490373766,
-            "velocityY": -1.7841391673415428,
-            "timestamp": 0.9146159982984342
-        },
-        {
-            "x": 3.4597028157497376,
-            "y": 2.003774307484127,
-            "heading": -0.14479289641721121,
-            "angularVelocity": -6.189211176196774e-7,
-            "velocityX": 3.5604423633775646,
-            "velocityY": -1.2782361540698368,
-            "timestamp": 0.9810031609409252
-        },
-        {
-            "x": 3.704835569204283,
-            "y": 1.949179734375487,
-            "heading": -0.14479293648657837,
-            "angularVelocity": -6.035710154638316e-7,
-            "velocityX": 3.692472214464675,
-            "velocityY": -0.8223664174750668,
-            "timestamp": 1.047390323583416
-        },
-        {
-            "x": 3.9499686718209266,
-            "y": 1.8945867290481686,
-            "heading": -0.1447929765559331,
-            "angularVelocity": -6.035708281824289e-7,
-            "velocityX": 3.692477473946853,
-            "velocityY": -0.8223428017448724,
-            "timestamp": 1.113777486225907
-        },
-        {
-            "x": 4.195101784677066,
-            "y": 1.8399937696982192,
-            "heading": -0.14479301662532418,
-            "angularVelocity": -6.035713756667977e-7,
-            "velocityX": 3.692477628185933,
-            "velocityY": -0.8223421091807183,
-            "timestamp": 1.1801646488683981
-        },
-        {
-            "x": 4.435904668374261,
-            "y": 1.7860806487644325,
-            "heading": -0.15898872227240893,
-            "angularVelocity": -0.21383208864538455,
-            "velocityX": 3.627250723064772,
-            "velocityY": -0.8121015989811171,
-            "timestamp": 1.2465518115108891
-        },
-        {
-            "x": 4.648245693564204,
-            "y": 1.7385415440023928,
-            "heading": -0.26462575063007965,
-            "angularVelocity": -1.5912267395211415,
-            "velocityX": 3.1985253886123286,
-            "velocityY": -0.7160888170209653,
-            "timestamp": 1.3129389741533801
-        },
-        {
-            "x": 4.824757883271093,
-            "y": 1.6989793937314068,
-            "heading": -0.37654836939407044,
-            "angularVelocity": -1.6859075506317058,
-            "velocityX": 2.6588301515075172,
-            "velocityY": -0.5959307296206723,
-            "timestamp": 1.3793261367958711
-        },
-        {
-            "x": 4.96555652453039,
-            "y": 1.6673723665346096,
-            "heading": -0.4762340168485147,
-            "angularVelocity": -1.501580177349536,
-            "velocityX": 2.1208715006774397,
-            "velocityY": -0.47610149219673153,
-            "timestamp": 1.4457132994383621
-        },
-        {
-            "x": 5.070906714490224,
-            "y": 1.643682366519874,
-            "heading": -0.5558849180153355,
-            "angularVelocity": -1.1997937251175768,
-            "velocityX": 1.5869060487969315,
-            "velocityY": -0.3568460990313944,
-            "timestamp": 1.5121004620808531
-        },
-        {
-            "x": 5.141006607602348,
-            "y": 1.6278935430132826,
-            "heading": -0.6113350442973207,
-            "angularVelocity": -0.8352537459779052,
-            "velocityX": 1.055925427776252,
-            "velocityY": -0.23782946699526453,
-            "timestamp": 1.5784876247233441
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -0.4317846186174008,
-            "velocityX": 0.5271108299370921,
-            "velocityY": -0.11890164753374041,
-            "timestamp": 1.6448747873658351
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 1.0662498702136072e-32,
-            "velocityX": 0,
-            "velocityY": 8.479039452208929e-33,
-            "timestamp": 1.7112619500083261
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.1.traj
deleted file mode 100644
index 087304c1..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 1.4326217036715317e-32,
-            "velocityX": -1.74824017803789e-33,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3001377838932537,
-            "y": 4.403828106793783,
-            "heading": -0.3629126711329447,
-            "angularVelocity": 0.37089781855009624,
-            "velocityX": 0.0018866291944089578,
-            "velocityY": 0.600123743495633,
-            "timestamp": 0.07303178264284323
-        },
-        {
-            "x": 2.300408122825978,
-            "y": 4.49157282256222,
-            "heading": -0.3106389398567459,
-            "angularVelocity": 0.7157668810008344,
-            "velocityX": 0.0037016614265912673,
-            "velocityY": 1.2014593180279707,
-            "timestamp": 0.14606356528568645
-        },
-        {
-            "x": 2.3008081314200264,
-            "y": 4.623360732427042,
-            "heading": -0.2362186265841639,
-            "angularVelocity": 1.019012689811082,
-            "velocityX": 0.005477185131915077,
-            "velocityY": 1.8045281806870437,
-            "timestamp": 0.2190953479285297
-        },
-        {
-            "x": 2.301347271135313,
-            "y": 4.79937800153835,
-            "heading": -0.14515320669009546,
-            "angularVelocity": 1.2469286192754925,
-            "velocityX": 0.007382261472700921,
-            "velocityY": 2.410146141058999,
-            "timestamp": 0.2921271305713729
-        },
-        {
-            "x": 2.302070167703843,
-            "y": 5.019849104537176,
-            "heading": -0.04952394454697364,
-            "angularVelocity": 1.3094197989222025,
-            "velocityX": 0.009898383174695338,
-            "velocityY": 3.018837758308937,
-            "timestamp": 0.3651589132142161
-        },
-        {
-            "x": 2.3031513688730865,
-            "y": 5.282690253403799,
-            "heading": -0.004114907781753178,
-            "angularVelocity": 0.6217708937393756,
-            "velocityX": 0.014804529344862738,
-            "velocityY": 3.598996756686451,
-            "timestamp": 0.43819069585705933
-        },
-        {
-            "x": 2.303520931398292,
-            "y": 5.5589648879428,
-            "heading": -0.0041148785146688655,
-            "angularVelocity": 4.00744487575221e-7,
-            "velocityX": 0.005060297199824665,
-            "velocityY": 3.7829370246937892,
-            "timestamp": 0.5112224784999025
-        },
-        {
-            "x": 2.3038904865521834,
-            "y": 5.835239522491655,
-            "heading": -0.004114849247579104,
-            "angularVelocity": 4.0074456218901027e-7,
-            "velocityX": 0.005060196266858216,
-            "velocityY": 3.7829370248287195,
-            "timestamp": 0.5842542611427458
-        },
-        {
-            "x": 2.302005656251481,
-            "y": 6.097682306820458,
-            "heading": 0.0412177605138735,
-            "angularVelocity": 0.6207244041015477,
-            "velocityX": -0.025808356752290516,
-            "velocityY": 3.5935420830717044,
-            "timestamp": 0.6572860437855891
-        },
-        {
-            "x": 2.3011707390613565,
-            "y": 6.318019040898657,
-            "heading": 0.13972629361592653,
-            "angularVelocity": 1.3488447020908976,
-            "velocityX": -0.01143224442716972,
-            "velocityY": 3.016997889203128,
-            "timestamp": 0.7303178264284323
-        },
-        {
-            "x": 2.3006512841511224,
-            "y": 6.493901741171369,
-            "heading": 0.23378956116370217,
-            "angularVelocity": 1.2879771538343168,
-            "velocityX": -0.007112723959845375,
-            "velocityY": 2.408303534542668,
-            "timestamp": 0.8033496090712756
-        },
-        {
-            "x": 2.300310658917655,
-            "y": 6.625570031648912,
-            "heading": 0.310734904704345,
-            "angularVelocity": 1.053587092580481,
-            "velocityX": -0.00466406845267762,
-            "velocityY": 1.802890272053981,
-            "timestamp": 0.8763813917141189
-        },
-        {
-            "x": 2.3001003670918414,
-            "y": 6.7132235530415985,
-            "heading": 0.3648714550351714,
-            "angularVelocity": 0.7412738450542471,
-            "velocityX": -0.0028794562888088465,
-            "velocityY": 1.2002106236588805,
-            "timestamp": 0.9494131743569622
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.38515484556072405,
-            "velocityX": -0.0013742933310585549,
-            "velocityY": 0.5994163824877027,
-            "timestamp": 1.0224449569998053
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 2.8585488767662063e-32,
-            "velocityX": -1.0732467562704116e-31,
-            "velocityY": 0,
-            "timestamp": 1.0954767396426486
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.traj
deleted file mode 100644
index 087304c1..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 1.4326217036715317e-32,
-            "velocityX": -1.74824017803789e-33,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3001377838932537,
-            "y": 4.403828106793783,
-            "heading": -0.3629126711329447,
-            "angularVelocity": 0.37089781855009624,
-            "velocityX": 0.0018866291944089578,
-            "velocityY": 0.600123743495633,
-            "timestamp": 0.07303178264284323
-        },
-        {
-            "x": 2.300408122825978,
-            "y": 4.49157282256222,
-            "heading": -0.3106389398567459,
-            "angularVelocity": 0.7157668810008344,
-            "velocityX": 0.0037016614265912673,
-            "velocityY": 1.2014593180279707,
-            "timestamp": 0.14606356528568645
-        },
-        {
-            "x": 2.3008081314200264,
-            "y": 4.623360732427042,
-            "heading": -0.2362186265841639,
-            "angularVelocity": 1.019012689811082,
-            "velocityX": 0.005477185131915077,
-            "velocityY": 1.8045281806870437,
-            "timestamp": 0.2190953479285297
-        },
-        {
-            "x": 2.301347271135313,
-            "y": 4.79937800153835,
-            "heading": -0.14515320669009546,
-            "angularVelocity": 1.2469286192754925,
-            "velocityX": 0.007382261472700921,
-            "velocityY": 2.410146141058999,
-            "timestamp": 0.2921271305713729
-        },
-        {
-            "x": 2.302070167703843,
-            "y": 5.019849104537176,
-            "heading": -0.04952394454697364,
-            "angularVelocity": 1.3094197989222025,
-            "velocityX": 0.009898383174695338,
-            "velocityY": 3.018837758308937,
-            "timestamp": 0.3651589132142161
-        },
-        {
-            "x": 2.3031513688730865,
-            "y": 5.282690253403799,
-            "heading": -0.004114907781753178,
-            "angularVelocity": 0.6217708937393756,
-            "velocityX": 0.014804529344862738,
-            "velocityY": 3.598996756686451,
-            "timestamp": 0.43819069585705933
-        },
-        {
-            "x": 2.303520931398292,
-            "y": 5.5589648879428,
-            "heading": -0.0041148785146688655,
-            "angularVelocity": 4.00744487575221e-7,
-            "velocityX": 0.005060297199824665,
-            "velocityY": 3.7829370246937892,
-            "timestamp": 0.5112224784999025
-        },
-        {
-            "x": 2.3038904865521834,
-            "y": 5.835239522491655,
-            "heading": -0.004114849247579104,
-            "angularVelocity": 4.0074456218901027e-7,
-            "velocityX": 0.005060196266858216,
-            "velocityY": 3.7829370248287195,
-            "timestamp": 0.5842542611427458
-        },
-        {
-            "x": 2.302005656251481,
-            "y": 6.097682306820458,
-            "heading": 0.0412177605138735,
-            "angularVelocity": 0.6207244041015477,
-            "velocityX": -0.025808356752290516,
-            "velocityY": 3.5935420830717044,
-            "timestamp": 0.6572860437855891
-        },
-        {
-            "x": 2.3011707390613565,
-            "y": 6.318019040898657,
-            "heading": 0.13972629361592653,
-            "angularVelocity": 1.3488447020908976,
-            "velocityX": -0.01143224442716972,
-            "velocityY": 3.016997889203128,
-            "timestamp": 0.7303178264284323
-        },
-        {
-            "x": 2.3006512841511224,
-            "y": 6.493901741171369,
-            "heading": 0.23378956116370217,
-            "angularVelocity": 1.2879771538343168,
-            "velocityX": -0.007112723959845375,
-            "velocityY": 2.408303534542668,
-            "timestamp": 0.8033496090712756
-        },
-        {
-            "x": 2.300310658917655,
-            "y": 6.625570031648912,
-            "heading": 0.310734904704345,
-            "angularVelocity": 1.053587092580481,
-            "velocityX": -0.00466406845267762,
-            "velocityY": 1.802890272053981,
-            "timestamp": 0.8763813917141189
-        },
-        {
-            "x": 2.3001003670918414,
-            "y": 6.7132235530415985,
-            "heading": 0.3648714550351714,
-            "angularVelocity": 0.7412738450542471,
-            "velocityX": -0.0028794562888088465,
-            "velocityY": 1.2002106236588805,
-            "timestamp": 0.9494131743569622
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.38515484556072405,
-            "velocityX": -0.0013742933310585549,
-            "velocityY": 0.5994163824877027,
-            "timestamp": 1.0224449569998053
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 2.8585488767662063e-32,
-            "velocityX": -1.0732467562704116e-31,
-            "velocityY": 0,
-            "timestamp": 1.0954767396426486
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj
deleted file mode 100644
index 4345e451..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.1.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 2.0150450292109907e-34,
-            "velocityY": 3.039389955204995e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2980012520823867,
-            "y": 4.4080014432651184,
-            "heading": -0.3744767713085023,
-            "angularVelocity": 0.2040875990087458,
-            "velocityX": -0.02627801674742958,
-            "velocityY": 0.6310864511258738,
-            "timestamp": 0.07606159691668693
-        },
-        {
-            "x": 2.294003780157748,
-            "y": 4.504004078538053,
-            "heading": -0.34341554427294535,
-            "angularVelocity": 0.40836937817095076,
-            "velocityX": -0.05255571913664827,
-            "velocityY": 1.262169598911913,
-            "timestamp": 0.15212319383337386
-        },
-        {
-            "x": 2.2880075055108158,
-            "y": 4.648007445856594,
-            "heading": -0.296791131111893,
-            "angularVelocity": 0.6129823071177671,
-            "velocityX": -0.07883445641432706,
-            "velocityY": 1.8932466994648223,
-            "timestamp": 0.2281847907500608
-        },
-        {
-            "x": 2.2800121106149036,
-            "y": 4.840010896492559,
-            "heading": -0.2345694561462187,
-            "angularVelocity": 0.8180432371651087,
-            "velocityX": -0.10511736829125365,
-            "velocityY": 2.5243152710332057,
-            "timestamp": 0.3042463876667477
-        },
-        {
-            "x": 2.270016860554186,
-            "y": 5.080013636963748,
-            "heading": -0.15671112019223,
-            "angularVelocity": 1.023622157700289,
-            "velocityX": -0.13140994228224737,
-            "velocityY": 3.1553734105013973,
-            "timestamp": 0.3803079845834346
-        },
-        {
-            "x": 2.2620106394348882,
-            "y": 5.2720083260038075,
-            "heading": -0.09403333701953483,
-            "angularVelocity": 0.82403980081234,
-            "velocityX": -0.10525970323851294,
-            "velocityY": 2.524200080237073,
-            "timestamp": 0.4563695815001215
-        },
-        {
-            "x": 2.256005452365101,
-            "y": 5.416004188429854,
-            "heading": -0.0470178538159679,
-            "angularVelocity": 0.6181237984664553,
-            "velocityX": -0.07895163016843816,
-            "velocityY": 1.8931480308488688,
-            "timestamp": 0.5324311784168084
-        },
-        {
-            "x": 2.2520018201736485,
-            "y": 5.5120013950535185,
-            "heading": -0.01567260522346991,
-            "angularVelocity": 0.4121034774859067,
-            "velocityX": -0.05263670963729669,
-            "velocityY": 1.262098227162051,
-            "timestamp": 0.6084927753334953
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 1.3656663814986182e-35,
-            "angularVelocity": 0.20605148798856665,
-            "velocityX": -0.026318408432062,
-            "velocityY": 0.6310491350721452,
-            "timestamp": 0.6845543722501822
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 6.6551444751621536e-34,
-            "velocityX": 3.9479513180483775e-34,
-            "velocityY": 0,
-            "timestamp": 0.7606159691668691
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.traj
deleted file mode 100644
index 4345e451..00000000
--- a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 2.0150450292109907e-34,
-            "velocityY": 3.039389955204995e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2980012520823867,
-            "y": 4.4080014432651184,
-            "heading": -0.3744767713085023,
-            "angularVelocity": 0.2040875990087458,
-            "velocityX": -0.02627801674742958,
-            "velocityY": 0.6310864511258738,
-            "timestamp": 0.07606159691668693
-        },
-        {
-            "x": 2.294003780157748,
-            "y": 4.504004078538053,
-            "heading": -0.34341554427294535,
-            "angularVelocity": 0.40836937817095076,
-            "velocityX": -0.05255571913664827,
-            "velocityY": 1.262169598911913,
-            "timestamp": 0.15212319383337386
-        },
-        {
-            "x": 2.2880075055108158,
-            "y": 4.648007445856594,
-            "heading": -0.296791131111893,
-            "angularVelocity": 0.6129823071177671,
-            "velocityX": -0.07883445641432706,
-            "velocityY": 1.8932466994648223,
-            "timestamp": 0.2281847907500608
-        },
-        {
-            "x": 2.2800121106149036,
-            "y": 4.840010896492559,
-            "heading": -0.2345694561462187,
-            "angularVelocity": 0.8180432371651087,
-            "velocityX": -0.10511736829125365,
-            "velocityY": 2.5243152710332057,
-            "timestamp": 0.3042463876667477
-        },
-        {
-            "x": 2.270016860554186,
-            "y": 5.080013636963748,
-            "heading": -0.15671112019223,
-            "angularVelocity": 1.023622157700289,
-            "velocityX": -0.13140994228224737,
-            "velocityY": 3.1553734105013973,
-            "timestamp": 0.3803079845834346
-        },
-        {
-            "x": 2.2620106394348882,
-            "y": 5.2720083260038075,
-            "heading": -0.09403333701953483,
-            "angularVelocity": 0.82403980081234,
-            "velocityX": -0.10525970323851294,
-            "velocityY": 2.524200080237073,
-            "timestamp": 0.4563695815001215
-        },
-        {
-            "x": 2.256005452365101,
-            "y": 5.416004188429854,
-            "heading": -0.0470178538159679,
-            "angularVelocity": 0.6181237984664553,
-            "velocityX": -0.07895163016843816,
-            "velocityY": 1.8931480308488688,
-            "timestamp": 0.5324311784168084
-        },
-        {
-            "x": 2.2520018201736485,
-            "y": 5.5120013950535185,
-            "heading": -0.01567260522346991,
-            "angularVelocity": 0.4121034774859067,
-            "velocityX": -0.05263670963729669,
-            "velocityY": 1.262098227162051,
-            "timestamp": 0.6084927753334953
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 1.3656663814986182e-35,
-            "angularVelocity": 0.20605148798856665,
-            "velocityX": -0.026318408432062,
-            "velocityY": 0.6310491350721452,
-            "timestamp": 0.6845543722501822
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 6.6551444751621536e-34,
-            "velocityX": 3.9479513180483775e-34,
-            "velocityY": 0,
-            "timestamp": 0.7606159691668691
-        }
-    ]
-}
\ No newline at end of file

From 4d0b37b623f1d57ebfe6d807a44d1a7f8bcba3ad Mon Sep 17 00:00:00 2001
From: Awesomeplayer165 <dev@jacob2.dev>
Date: Wed, 28 Feb 2024 22:22:12 -0800
Subject: [PATCH 62/62] remove other pre-generated trajectories

---
 ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat |  Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat |  Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat |  Bin 2048 -> 2048 bytes
 ctre_sim/Talon FX vers. C - 018 - 0 - ext.dat |  Bin 2048 -> 2048 bytes
 src/main/deploy/choreo/Amp Side To C1.traj    |  274 -----
 src/main/deploy/choreo/Amp Side To C2.traj    |  319 -----
 src/main/deploy/choreo/Amp Side To C3.traj    |  526 --------
 src/main/deploy/choreo/Amp Side To C4.traj    |  400 ------
 src/main/deploy/choreo/Amp Side To C5.traj    |  805 ------------
 src/main/deploy/choreo/Amp Side To W1.traj    |  121 --
 src/main/deploy/choreo/Amp Side To W2.traj    |  148 ---
 src/main/deploy/choreo/Amp Side To W3.traj    |  193 ---
 src/main/deploy/choreo/C1 To C2.traj          |   94 --
 src/main/deploy/choreo/C1 To C3.traj          |  139 ---
 src/main/deploy/choreo/C1 To C4.traj          |  175 ---
 src/main/deploy/choreo/C1 To C5.traj          |  220 ----
 src/main/deploy/choreo/C1 To S1.traj          |  130 --
 src/main/deploy/choreo/C1 To S2.traj          |  148 ---
 src/main/deploy/choreo/C1 To S3.traj          |  265 ----
 src/main/deploy/choreo/C1 To S4.traj          |  301 -----
 src/main/deploy/choreo/C1 To W1.traj          |  202 ---
 src/main/deploy/choreo/C1 To W2.traj          |  193 ---
 src/main/deploy/choreo/C1 To W3.traj          |  274 -----
 src/main/deploy/choreo/C2 To C1.traj          |   94 --
 src/main/deploy/choreo/C2 To C3.traj          |   94 --
 src/main/deploy/choreo/C2 To C4.traj          |  139 ---
 src/main/deploy/choreo/C2 To C5.traj          |  175 ---
 src/main/deploy/choreo/C2 To S1.traj          |  130 --
 src/main/deploy/choreo/C2 To S2.traj          |  193 ---
 src/main/deploy/choreo/C2 To S3.traj          |  193 ---
 src/main/deploy/choreo/C2 To S4.traj          |  256 ----
 src/main/deploy/choreo/C2 To W1.traj          |  202 ---
 src/main/deploy/choreo/C2 To W2.traj          |  184 ---
 src/main/deploy/choreo/C2 To W3.traj          |  346 ------
 src/main/deploy/choreo/C3 To C1.traj          |  139 ---
 src/main/deploy/choreo/C3 To C2.traj          |   94 --
 src/main/deploy/choreo/C3 To C4.traj          |   94 --
 src/main/deploy/choreo/C3 To C5.traj          |  139 ---
 src/main/deploy/choreo/C3 To S1.traj          |  157 ---
 src/main/deploy/choreo/C3 To S2.traj          |  148 ---
 src/main/deploy/choreo/C3 To S3.traj          |  166 ---
 src/main/deploy/choreo/C3 To S4.traj          |  238 ----
 src/main/deploy/choreo/C3 To W1.traj          |  220 ----
 src/main/deploy/choreo/C3 To W2.traj          |  193 ---
 src/main/deploy/choreo/C3 To W3.traj          |  202 ---
 src/main/deploy/choreo/C4 To C1.traj          |  175 ---
 src/main/deploy/choreo/C4 To C2.traj          |  139 ---
 src/main/deploy/choreo/C4 To C3.traj          |   94 --
 src/main/deploy/choreo/C4 To C5.traj          |   94 --
 src/main/deploy/choreo/C4 To S1.traj          |  256 ----
 src/main/deploy/choreo/C4 To S2.traj          |  211 ----
 src/main/deploy/choreo/C4 To S3.traj          |  229 ----
 src/main/deploy/choreo/C4 To S4.traj          |  211 ----
 src/main/deploy/choreo/C4 To W1.traj          |  238 ----
 src/main/deploy/choreo/C4 To W2.traj          |  256 ----
 src/main/deploy/choreo/C4 To W3.traj          |  274 -----
 src/main/deploy/choreo/C5 To C1.traj          |  220 ----
 src/main/deploy/choreo/C5 To C2.traj          |  175 ---
 src/main/deploy/choreo/C5 To C3.traj          |  139 ---
 src/main/deploy/choreo/C5 To C4.traj          |   94 --
 src/main/deploy/choreo/C5 To S1.traj          |  292 -----
 src/main/deploy/choreo/C5 To S2.traj          |  247 ----
 src/main/deploy/choreo/C5 To S3.traj          |  247 ----
 src/main/deploy/choreo/C5 To S4.traj          |  148 ---
 src/main/deploy/choreo/C5 To W1.traj          |  310 -----
 src/main/deploy/choreo/C5 To W2.traj          |  283 -----
 src/main/deploy/choreo/C5 To W3.traj          |  292 -----
 src/main/deploy/choreo/Center To C1.traj      |  220 ----
 src/main/deploy/choreo/Center To C2.traj      |  211 ----
 src/main/deploy/choreo/Center To C3.traj      |  211 ----
 src/main/deploy/choreo/Center To C4.traj      | 1093 -----------------
 src/main/deploy/choreo/Center To C5.traj      |  625 ----------
 src/main/deploy/choreo/Center To W1.traj      |  112 --
 src/main/deploy/choreo/Center To W2.traj      |   76 --
 src/main/deploy/choreo/Center To W3.traj      |  112 --
 src/main/deploy/choreo/S1 To C1.traj          |  130 --
 src/main/deploy/choreo/S1 To C2.traj          |  130 --
 src/main/deploy/choreo/S1 To C3.traj          |  157 ---
 src/main/deploy/choreo/S1 To C4.traj          |  247 ----
 src/main/deploy/choreo/S1 To C5.traj          |  292 -----
 src/main/deploy/choreo/S1 To W1.traj          |  130 --
 src/main/deploy/choreo/S1 To W2.traj          |  148 ---
 src/main/deploy/choreo/S1 To W3.traj          |  175 ---
 src/main/deploy/choreo/S2 To C1.traj          |  148 ---
 src/main/deploy/choreo/S2 To C2.traj          |  202 ---
 src/main/deploy/choreo/S2 To C3.traj          |  148 ---
 src/main/deploy/choreo/S2 To C4.traj          |  166 ---
 src/main/deploy/choreo/S2 To C5.traj          |  202 ---
 src/main/deploy/choreo/S2 To W1.traj          |  130 --
 src/main/deploy/choreo/S2 To W2.traj          |  103 --
 src/main/deploy/choreo/S2 To W3.traj          |  130 --
 src/main/deploy/choreo/S3 To C1.traj          |  220 ----
 src/main/deploy/choreo/S3 To C2.traj          |  193 ---
 src/main/deploy/choreo/S3 To C3.traj          |  166 ---
 src/main/deploy/choreo/S3 To C4.traj          |  238 ----
 src/main/deploy/choreo/S3 To C5.traj          |  184 ---
 src/main/deploy/choreo/S3 To W1.traj          |  202 ---
 src/main/deploy/choreo/S3 To W2.traj          |  157 ---
 src/main/deploy/choreo/S3 To W3.traj          |  121 --
 src/main/deploy/choreo/S4 To C1.traj          |  292 -----
 src/main/deploy/choreo/S4 To C2.traj          |  202 ---
 src/main/deploy/choreo/S4 To C3.traj          |  238 ----
 src/main/deploy/choreo/S4 To C4.traj          |  148 ---
 src/main/deploy/choreo/S4 To C5.traj          |  148 ---
 src/main/deploy/choreo/S4 To W1.traj          |  247 ----
 src/main/deploy/choreo/S4 To W2.traj          |  202 ---
 src/main/deploy/choreo/S4 To W3.traj          |  256 ----
 src/main/deploy/choreo/Stage Side To C1.traj  |  616 ----------
 src/main/deploy/choreo/Stage Side To C2.traj  |  607 ---------
 src/main/deploy/choreo/Stage Side To C3.traj  |  274 -----
 src/main/deploy/choreo/Stage Side To C4.traj  |  679 ----------
 src/main/deploy/choreo/Stage Side To C5.traj  |  337 -----
 src/main/deploy/choreo/Stage Side To W1.traj  |  193 ---
 src/main/deploy/choreo/Stage Side To W2.traj  |  148 ---
 src/main/deploy/choreo/Stage Side To W3.traj  |  121 --
 src/main/deploy/choreo/W1 To C1.traj          |  202 ---
 src/main/deploy/choreo/W1 To C2.traj          |  256 ----
 src/main/deploy/choreo/W1 To C3.traj          |  193 ---
 src/main/deploy/choreo/W1 To C4.traj          |  400 ------
 src/main/deploy/choreo/W1 To C5.traj          |  616 ----------
 src/main/deploy/choreo/W1 To S1.traj          |  130 --
 src/main/deploy/choreo/W1 To S2.traj          |  130 --
 src/main/deploy/choreo/W1 To S3.traj          |  544 --------
 src/main/deploy/choreo/W1 To S4.traj          |  562 ---------
 src/main/deploy/choreo/W1 To W2.traj          |  103 --
 src/main/deploy/choreo/W1 To W3.traj          |  148 ---
 src/main/deploy/choreo/W2 To C1.traj          |  193 ---
 src/main/deploy/choreo/W2 To C2.traj          |  526 --------
 src/main/deploy/choreo/W2 To C3.traj          |  571 ---------
 src/main/deploy/choreo/W2 To C4.traj          |  589 ---------
 src/main/deploy/choreo/W2 To C5.traj          |  625 ----------
 src/main/deploy/choreo/W2 To S1.traj          |  148 ---
 src/main/deploy/choreo/W2 To S2.traj          |  103 --
 src/main/deploy/choreo/W2 To S3.traj          |  526 --------
 src/main/deploy/choreo/W2 To S4.traj          |  553 ---------
 src/main/deploy/choreo/W2 To W1.traj          |  103 --
 src/main/deploy/choreo/W2 To W3.traj          |  103 --
 src/main/deploy/choreo/W3 To C1.traj          |  553 ---------
 src/main/deploy/choreo/W3 To C2.traj          |  553 ---------
 src/main/deploy/choreo/W3 To C3.traj          |  310 -----
 src/main/deploy/choreo/W3 To C4.traj          |  625 ----------
 src/main/deploy/choreo/W3 To C5.traj          |  571 ---------
 src/main/deploy/choreo/W3 To S1.traj          |  175 ---
 src/main/deploy/choreo/W3 To S2.traj          |  130 --
 src/main/deploy/choreo/W3 To S3.traj          |  229 ----
 src/main/deploy/choreo/W3 To S4.traj          |  256 ----
 src/main/deploy/choreo/W3 To W1.traj          |  148 ---
 src/main/deploy/choreo/W3 To W2.traj          |  103 --
 .../deploy/choreo/amp 4 local sgmt 1.traj     |  337 -----
 .../deploy/choreo/amp 4 local sgmt 2.traj     |  193 ---
 .../deploy/choreo/amp 4 local sgmt 3.traj     |  229 ----
 src/main/deploy/choreo/amp 4 local.traj       |  760 ------------
 src/main/deploy/choreo/test.traj              | 1084 ----------------
 153 files changed, 38414 deletions(-)
 delete mode 100644 src/main/deploy/choreo/Amp Side To C1.traj
 delete mode 100644 src/main/deploy/choreo/Amp Side To C2.traj
 delete mode 100644 src/main/deploy/choreo/Amp Side To C3.traj
 delete mode 100644 src/main/deploy/choreo/Amp Side To C4.traj
 delete mode 100644 src/main/deploy/choreo/Amp Side To C5.traj
 delete mode 100644 src/main/deploy/choreo/Amp Side To W1.traj
 delete mode 100644 src/main/deploy/choreo/Amp Side To W2.traj
 delete mode 100644 src/main/deploy/choreo/Amp Side To W3.traj
 delete mode 100644 src/main/deploy/choreo/C1 To C2.traj
 delete mode 100644 src/main/deploy/choreo/C1 To C3.traj
 delete mode 100644 src/main/deploy/choreo/C1 To C4.traj
 delete mode 100644 src/main/deploy/choreo/C1 To C5.traj
 delete mode 100644 src/main/deploy/choreo/C1 To S1.traj
 delete mode 100644 src/main/deploy/choreo/C1 To S2.traj
 delete mode 100644 src/main/deploy/choreo/C1 To S3.traj
 delete mode 100644 src/main/deploy/choreo/C1 To S4.traj
 delete mode 100644 src/main/deploy/choreo/C1 To W1.traj
 delete mode 100644 src/main/deploy/choreo/C1 To W2.traj
 delete mode 100644 src/main/deploy/choreo/C1 To W3.traj
 delete mode 100644 src/main/deploy/choreo/C2 To C1.traj
 delete mode 100644 src/main/deploy/choreo/C2 To C3.traj
 delete mode 100644 src/main/deploy/choreo/C2 To C4.traj
 delete mode 100644 src/main/deploy/choreo/C2 To C5.traj
 delete mode 100644 src/main/deploy/choreo/C2 To S1.traj
 delete mode 100644 src/main/deploy/choreo/C2 To S2.traj
 delete mode 100644 src/main/deploy/choreo/C2 To S3.traj
 delete mode 100644 src/main/deploy/choreo/C2 To S4.traj
 delete mode 100644 src/main/deploy/choreo/C2 To W1.traj
 delete mode 100644 src/main/deploy/choreo/C2 To W2.traj
 delete mode 100644 src/main/deploy/choreo/C2 To W3.traj
 delete mode 100644 src/main/deploy/choreo/C3 To C1.traj
 delete mode 100644 src/main/deploy/choreo/C3 To C2.traj
 delete mode 100644 src/main/deploy/choreo/C3 To C4.traj
 delete mode 100644 src/main/deploy/choreo/C3 To C5.traj
 delete mode 100644 src/main/deploy/choreo/C3 To S1.traj
 delete mode 100644 src/main/deploy/choreo/C3 To S2.traj
 delete mode 100644 src/main/deploy/choreo/C3 To S3.traj
 delete mode 100644 src/main/deploy/choreo/C3 To S4.traj
 delete mode 100644 src/main/deploy/choreo/C3 To W1.traj
 delete mode 100644 src/main/deploy/choreo/C3 To W2.traj
 delete mode 100644 src/main/deploy/choreo/C3 To W3.traj
 delete mode 100644 src/main/deploy/choreo/C4 To C1.traj
 delete mode 100644 src/main/deploy/choreo/C4 To C2.traj
 delete mode 100644 src/main/deploy/choreo/C4 To C3.traj
 delete mode 100644 src/main/deploy/choreo/C4 To C5.traj
 delete mode 100644 src/main/deploy/choreo/C4 To S1.traj
 delete mode 100644 src/main/deploy/choreo/C4 To S2.traj
 delete mode 100644 src/main/deploy/choreo/C4 To S3.traj
 delete mode 100644 src/main/deploy/choreo/C4 To S4.traj
 delete mode 100644 src/main/deploy/choreo/C4 To W1.traj
 delete mode 100644 src/main/deploy/choreo/C4 To W2.traj
 delete mode 100644 src/main/deploy/choreo/C4 To W3.traj
 delete mode 100644 src/main/deploy/choreo/C5 To C1.traj
 delete mode 100644 src/main/deploy/choreo/C5 To C2.traj
 delete mode 100644 src/main/deploy/choreo/C5 To C3.traj
 delete mode 100644 src/main/deploy/choreo/C5 To C4.traj
 delete mode 100644 src/main/deploy/choreo/C5 To S1.traj
 delete mode 100644 src/main/deploy/choreo/C5 To S2.traj
 delete mode 100644 src/main/deploy/choreo/C5 To S3.traj
 delete mode 100644 src/main/deploy/choreo/C5 To S4.traj
 delete mode 100644 src/main/deploy/choreo/C5 To W1.traj
 delete mode 100644 src/main/deploy/choreo/C5 To W2.traj
 delete mode 100644 src/main/deploy/choreo/C5 To W3.traj
 delete mode 100644 src/main/deploy/choreo/Center To C1.traj
 delete mode 100644 src/main/deploy/choreo/Center To C2.traj
 delete mode 100644 src/main/deploy/choreo/Center To C3.traj
 delete mode 100644 src/main/deploy/choreo/Center To C4.traj
 delete mode 100644 src/main/deploy/choreo/Center To C5.traj
 delete mode 100644 src/main/deploy/choreo/Center To W1.traj
 delete mode 100644 src/main/deploy/choreo/Center To W2.traj
 delete mode 100644 src/main/deploy/choreo/Center To W3.traj
 delete mode 100644 src/main/deploy/choreo/S1 To C1.traj
 delete mode 100644 src/main/deploy/choreo/S1 To C2.traj
 delete mode 100644 src/main/deploy/choreo/S1 To C3.traj
 delete mode 100644 src/main/deploy/choreo/S1 To C4.traj
 delete mode 100644 src/main/deploy/choreo/S1 To C5.traj
 delete mode 100644 src/main/deploy/choreo/S1 To W1.traj
 delete mode 100644 src/main/deploy/choreo/S1 To W2.traj
 delete mode 100644 src/main/deploy/choreo/S1 To W3.traj
 delete mode 100644 src/main/deploy/choreo/S2 To C1.traj
 delete mode 100644 src/main/deploy/choreo/S2 To C2.traj
 delete mode 100644 src/main/deploy/choreo/S2 To C3.traj
 delete mode 100644 src/main/deploy/choreo/S2 To C4.traj
 delete mode 100644 src/main/deploy/choreo/S2 To C5.traj
 delete mode 100644 src/main/deploy/choreo/S2 To W1.traj
 delete mode 100644 src/main/deploy/choreo/S2 To W2.traj
 delete mode 100644 src/main/deploy/choreo/S2 To W3.traj
 delete mode 100644 src/main/deploy/choreo/S3 To C1.traj
 delete mode 100644 src/main/deploy/choreo/S3 To C2.traj
 delete mode 100644 src/main/deploy/choreo/S3 To C3.traj
 delete mode 100644 src/main/deploy/choreo/S3 To C4.traj
 delete mode 100644 src/main/deploy/choreo/S3 To C5.traj
 delete mode 100644 src/main/deploy/choreo/S3 To W1.traj
 delete mode 100644 src/main/deploy/choreo/S3 To W2.traj
 delete mode 100644 src/main/deploy/choreo/S3 To W3.traj
 delete mode 100644 src/main/deploy/choreo/S4 To C1.traj
 delete mode 100644 src/main/deploy/choreo/S4 To C2.traj
 delete mode 100644 src/main/deploy/choreo/S4 To C3.traj
 delete mode 100644 src/main/deploy/choreo/S4 To C4.traj
 delete mode 100644 src/main/deploy/choreo/S4 To C5.traj
 delete mode 100644 src/main/deploy/choreo/S4 To W1.traj
 delete mode 100644 src/main/deploy/choreo/S4 To W2.traj
 delete mode 100644 src/main/deploy/choreo/S4 To W3.traj
 delete mode 100644 src/main/deploy/choreo/Stage Side To C1.traj
 delete mode 100644 src/main/deploy/choreo/Stage Side To C2.traj
 delete mode 100644 src/main/deploy/choreo/Stage Side To C3.traj
 delete mode 100644 src/main/deploy/choreo/Stage Side To C4.traj
 delete mode 100644 src/main/deploy/choreo/Stage Side To C5.traj
 delete mode 100644 src/main/deploy/choreo/Stage Side To W1.traj
 delete mode 100644 src/main/deploy/choreo/Stage Side To W2.traj
 delete mode 100644 src/main/deploy/choreo/Stage Side To W3.traj
 delete mode 100644 src/main/deploy/choreo/W1 To C1.traj
 delete mode 100644 src/main/deploy/choreo/W1 To C2.traj
 delete mode 100644 src/main/deploy/choreo/W1 To C3.traj
 delete mode 100644 src/main/deploy/choreo/W1 To C4.traj
 delete mode 100644 src/main/deploy/choreo/W1 To C5.traj
 delete mode 100644 src/main/deploy/choreo/W1 To S1.traj
 delete mode 100644 src/main/deploy/choreo/W1 To S2.traj
 delete mode 100644 src/main/deploy/choreo/W1 To S3.traj
 delete mode 100644 src/main/deploy/choreo/W1 To S4.traj
 delete mode 100644 src/main/deploy/choreo/W1 To W2.traj
 delete mode 100644 src/main/deploy/choreo/W1 To W3.traj
 delete mode 100644 src/main/deploy/choreo/W2 To C1.traj
 delete mode 100644 src/main/deploy/choreo/W2 To C2.traj
 delete mode 100644 src/main/deploy/choreo/W2 To C3.traj
 delete mode 100644 src/main/deploy/choreo/W2 To C4.traj
 delete mode 100644 src/main/deploy/choreo/W2 To C5.traj
 delete mode 100644 src/main/deploy/choreo/W2 To S1.traj
 delete mode 100644 src/main/deploy/choreo/W2 To S2.traj
 delete mode 100644 src/main/deploy/choreo/W2 To S3.traj
 delete mode 100644 src/main/deploy/choreo/W2 To S4.traj
 delete mode 100644 src/main/deploy/choreo/W2 To W1.traj
 delete mode 100644 src/main/deploy/choreo/W2 To W3.traj
 delete mode 100644 src/main/deploy/choreo/W3 To C1.traj
 delete mode 100644 src/main/deploy/choreo/W3 To C2.traj
 delete mode 100644 src/main/deploy/choreo/W3 To C3.traj
 delete mode 100644 src/main/deploy/choreo/W3 To C4.traj
 delete mode 100644 src/main/deploy/choreo/W3 To C5.traj
 delete mode 100644 src/main/deploy/choreo/W3 To S1.traj
 delete mode 100644 src/main/deploy/choreo/W3 To S2.traj
 delete mode 100644 src/main/deploy/choreo/W3 To S3.traj
 delete mode 100644 src/main/deploy/choreo/W3 To S4.traj
 delete mode 100644 src/main/deploy/choreo/W3 To W1.traj
 delete mode 100644 src/main/deploy/choreo/W3 To W2.traj
 delete mode 100644 src/main/deploy/choreo/amp 4 local sgmt 1.traj
 delete mode 100644 src/main/deploy/choreo/amp 4 local sgmt 2.traj
 delete mode 100644 src/main/deploy/choreo/amp 4 local sgmt 3.traj
 delete mode 100644 src/main/deploy/choreo/amp 4 local.traj
 delete mode 100644 src/main/deploy/choreo/test.traj

diff --git a/ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 013 - 0 - ext.dat
index 56d956c14b7edfdbc500f3750efb8165c04949bf..9c319613a084a8f4fb41df7090d9bc3dae2dde25 100644
GIT binary patch
delta 65
zcmZn=Xb@nz&oH5CqG0Mo1>T8eUMv?F<|BAq=NUxr$;dM>FfdING~+tQAaPF$$}`{0
I$n>8T0HGTaqyPW_

delta 65
zcmZn=Xb@nz%`l~EqG0Mo1>T8eUMzPQCL?%U2N~k;J&<EyU|^akXvTGbA?e;dD9?N|
IBh!CY0L!}+q5uE@

diff --git a/ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 014 - 0 - ext.dat
index f59bb310f0c8ede190539d2679870e53f786ccfc..7aa8c02b95f6e47b49c2305f135206cc911837fc 100644
GIT binary patch
delta 47
xcmZn=Xb@n&%+NNGc`x%7hSrIP_j5G9*dofnz`!_B(TwX9L(7XzFy102HUNrG4*CE9

delta 47
xcmZn=Xb@n&&rmgyc`x$?hR%tH_j4S4Q6kE~z`!_B(TwX1!@d_~Fy102HUNuT4*CE9

diff --git a/ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 015 - 0 - ext.dat
index 06d3cf562a6acbd67d795fa5cca4840613e967e0..0795cb7d86841372fecd6204583ed9b8f95a6107 100644
GIT binary patch
delta 49
zcmZn=Xb@n&%8)&gc`x%dhOCK)_j5?zSCMC6U|^i6XvTGjLFT>^lxMz~k?B7x0C+eJ
A;{X5v

delta 49
zcmZn=Xb@n&$dEsgc`x%NhP;V~_j5$ve<H`gz`!_B(TwW|L)iUiP@ef_MyCI)0HXX4
A;Q#;t

diff --git a/ctre_sim/Talon FX vers. C - 018 - 0 - ext.dat b/ctre_sim/Talon FX vers. C - 018 - 0 - ext.dat
index abeb3b49bc95e09fca083e2c313cc286753a55c7..062dc8c8993415a593b38737efd119ed0434e8fc 100644
GIT binary patch
delta 63
zcmZn=Xb@nz&!EsSQ80C)0`J5!FO~}o`UoE9SqAP067mcT0uu$zInOb0JdlELEH*PT
H{bvOLT7D7G

delta 63
zcmZn=Xb@nz&7jgSQ80C)0`J5!FP1wD$_O6kc?Q=9cjOot1SSfaa~@y_dT<ZIvDnPW
H^q&;~f<Y6`

diff --git a/src/main/deploy/choreo/Amp Side To C1.traj b/src/main/deploy/choreo/Amp Side To C1.traj
deleted file mode 100644
index b2886244..00000000
--- a/src/main/deploy/choreo/Amp Side To C1.traj	
+++ /dev/null
@@ -1,274 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": 0,
-            "velocityX": -1.8098951472235817e-30,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7613816362208097,
-            "y": 6.725489442192221,
-            "heading": 0.9931061799728784,
-            "angularVelocity": -0.5863053623193526,
-            "velocityX": 0.6424157559264662,
-            "velocityY": 0.06863355110715653,
-            "timestamp": 0.07998190540440472
-        },
-        {
-            "x": 0.8643774617687672,
-            "y": 6.736651681634022,
-            "heading": 0.9029654144342619,
-            "angularVelocity": -1.127014480123304,
-            "velocityX": 1.287739083323781,
-            "velocityY": 0.1395595589447693,
-            "timestamp": 0.15996381080880945
-        },
-        {
-            "x": 1.019343320389449,
-            "y": 6.753669544326843,
-            "heading": 0.7755395878520636,
-            "angularVelocity": -1.5931831823449016,
-            "velocityX": 1.9375114638385158,
-            "velocityY": 0.21277140881771536,
-            "timestamp": 0.23994571621321417
-        },
-        {
-            "x": 1.2269178654194646,
-            "y": 6.7767659053404685,
-            "heading": 0.62538736801047,
-            "angularVelocity": -1.8773273665136312,
-            "velocityX": 2.5952688171215317,
-            "velocityY": 0.2887698273359004,
-            "timestamp": 0.3199276216176189
-        },
-        {
-            "x": 1.4861217663901574,
-            "y": 6.806716511012647,
-            "heading": 0.5166970392894589,
-            "angularVelocity": -1.3589364765874345,
-            "velocityX": 3.240781770077933,
-            "velocityY": 0.37446726882464465,
-            "timestamp": 0.39990952702202365
-        },
-        {
-            "x": 1.7870125383049598,
-            "y": 6.838519066862702,
-            "heading": 0.5166969975980924,
-            "angularVelocity": -5.212599803241117e-7,
-            "velocityX": 3.76198554402321,
-            "velocityY": 0.39762188321538583,
-            "timestamp": 0.4798914324264284
-        },
-        {
-            "x": 2.0879033719171214,
-            "y": 6.870321038982823,
-            "heading": 0.5166969559088411,
-            "angularVelocity": -5.212335340785354e-7,
-            "velocityX": 3.761986315414672,
-            "velocityY": 0.39761458494048757,
-            "timestamp": 0.5598733378308332
-        },
-        {
-            "x": 2.3887942055296523,
-            "y": 6.902123011099449,
-            "heading": 0.5166969142195897,
-            "angularVelocity": -5.212335365150009e-7,
-            "velocityX": 3.7619863154192914,
-            "velocityY": 0.3976145848967806,
-            "timestamp": 0.6398552432352379
-        },
-        {
-            "x": 2.689685039142183,
-            "y": 6.933924983216074,
-            "heading": 0.516696872530338,
-            "angularVelocity": -5.212335398189914e-7,
-            "velocityX": 3.7619863154192914,
-            "velocityY": 0.3976145848967804,
-            "timestamp": 0.7198371486396427
-        },
-        {
-            "x": 2.990575872754714,
-            "y": 6.9657269553327,
-            "heading": 0.5166968308410861,
-            "angularVelocity": -5.212335433606452e-7,
-            "velocityX": 3.7619863154192914,
-            "velocityY": 0.39761458489678053,
-            "timestamp": 0.7998190540440474
-        },
-        {
-            "x": 3.291466706367245,
-            "y": 6.997528927449325,
-            "heading": 0.5166967891518339,
-            "angularVelocity": -5.212335460884143e-7,
-            "velocityX": 3.761986315419291,
-            "velocityY": 0.3976145848967808,
-            "timestamp": 0.8798009594484522
-        },
-        {
-            "x": 3.5923575399797762,
-            "y": 7.0293308995659505,
-            "heading": 0.5166967474625817,
-            "angularVelocity": -5.212335479348436e-7,
-            "velocityX": 3.761986315419291,
-            "velocityY": 0.3976145848967805,
-            "timestamp": 0.9597828648528569
-        },
-        {
-            "x": 3.893248373592307,
-            "y": 7.0611328716825765,
-            "heading": 0.516696705773329,
-            "angularVelocity": -5.212335516712931e-7,
-            "velocityX": 3.7619863154192905,
-            "velocityY": 0.3976145848967809,
-            "timestamp": 1.0397647702572617
-        },
-        {
-            "x": 4.194139207204837,
-            "y": 7.092934843799203,
-            "heading": 0.5166966640840763,
-            "angularVelocity": -5.212335535473894e-7,
-            "velocityX": 3.7619863154192905,
-            "velocityY": 0.3976145848967809,
-            "timestamp": 1.1197466756616663
-        },
-        {
-            "x": 4.495030040817368,
-            "y": 7.1247368159158295,
-            "heading": 0.5166966223948233,
-            "angularVelocity": -5.212335568545237e-7,
-            "velocityX": 3.7619863154192905,
-            "velocityY": 0.3976145848967808,
-            "timestamp": 1.199728581066071
-        },
-        {
-            "x": 4.7959208744299,
-            "y": 7.156538788032456,
-            "heading": 0.51669658070557,
-            "angularVelocity": -5.212335599678648e-7,
-            "velocityX": 3.7619863154192905,
-            "velocityY": 0.397614584896781,
-            "timestamp": 1.2797104864704756
-        },
-        {
-            "x": 5.096811708042431,
-            "y": 7.1883407601490825,
-            "heading": 0.5166965390163165,
-            "angularVelocity": -5.212335628762146e-7,
-            "velocityX": 3.7619863154192905,
-            "velocityY": 0.3976145848967809,
-            "timestamp": 1.3596923918748802
-        },
-        {
-            "x": 5.3977025416549615,
-            "y": 7.220142732265709,
-            "heading": 0.5166964973270628,
-            "angularVelocity": -5.212335655841188e-7,
-            "velocityX": 3.76198631541929,
-            "velocityY": 0.3976145848967813,
-            "timestamp": 1.4396742972792849
-        },
-        {
-            "x": 5.698593375267492,
-            "y": 7.251944704382335,
-            "heading": 0.5166964556378089,
-            "angularVelocity": -5.212335680260379e-7,
-            "velocityX": 3.7619863154192896,
-            "velocityY": 0.3976145848967809,
-            "timestamp": 1.5196562026836895
-        },
-        {
-            "x": 5.999484208880023,
-            "y": 7.283746676498962,
-            "heading": 0.5166964139485547,
-            "angularVelocity": -5.212335712969897e-7,
-            "velocityX": 3.7619863154192896,
-            "velocityY": 0.3976145848967812,
-            "timestamp": 1.5996381080880941
-        },
-        {
-            "x": 6.300375042492554,
-            "y": 7.315548648615588,
-            "heading": 0.5166963722593003,
-            "angularVelocity": -5.21233573548864e-7,
-            "velocityX": 3.7619863154192896,
-            "velocityY": 0.39761458489678125,
-            "timestamp": 1.6796200134924988
-        },
-        {
-            "x": 6.6012658761048035,
-            "y": 7.347350620734872,
-            "heading": 0.5166963305700458,
-            "angularVelocity": -5.212335762453642e-7,
-            "velocityX": 3.7619863154157773,
-            "velocityY": 0.3976145849300086,
-            "timestamp": 1.7596019188969034
-        },
-        {
-            "x": 6.902156662813089,
-            "y": 7.3791530366221005,
-            "heading": 0.5166962888792155,
-            "angularVelocity": -5.212532766918364e-7,
-            "velocityX": 3.761985728983584,
-            "velocityY": 0.3976201332842598,
-            "timestamp": 1.839583824301308
-        },
-        {
-            "x": 7.162091022778527,
-            "y": 7.409610892063227,
-            "heading": 0.4051307797997224,
-            "angularVelocity": -1.3948843618490365,
-            "velocityX": 3.249914573191993,
-            "velocityY": 0.3808093253983604,
-            "timestamp": 1.9195657297057127
-        },
-        {
-            "x": 7.370036606479622,
-            "y": 7.432974685540162,
-            "heading": 0.25885269617169443,
-            "angularVelocity": -1.8288897080960538,
-            "velocityX": 2.5999078497777726,
-            "velocityY": 0.2921134894049214,
-            "timestamp": 1.9995476351101173
-        },
-        {
-            "x": 7.525326975450784,
-            "y": 7.45013556763307,
-            "heading": 0.1335302309738367,
-            "angularVelocity": -1.5668852169025227,
-            "velocityX": 1.9415687609088876,
-            "velocityY": 0.21455955576625652,
-            "timestamp": 2.079529540514522
-        },
-        {
-            "x": 7.628525640862077,
-            "y": 7.461415071089343,
-            "heading": 0.043910704401711645,
-            "angularVelocity": -1.1204975190199653,
-            "velocityX": 1.2902751552304177,
-            "velocityY": 0.14102569073894916,
-            "timestamp": 2.159511445918927
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -0.003,
-            "angularVelocity": -0.5865164647493907,
-            "velocityX": 0.6435750546033896,
-            "velocityY": 0.0698274051164111,
-            "timestamp": 2.2394933513233317
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -0.003,
-            "angularVelocity": 0,
-            "velocityX": 4.312091780947568e-36,
-            "velocityY": 0,
-            "timestamp": 2.3194752567277366
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Amp Side To C2.traj b/src/main/deploy/choreo/Amp Side To C2.traj
deleted file mode 100644
index 8c7890ab..00000000
--- a/src/main/deploy/choreo/Amp Side To C2.traj	
+++ /dev/null
@@ -1,319 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": 0,
-            "velocityX": 2.674890564447798e-33,
-            "velocityY": 3.559444452002106e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 0.744719766454751,
-            "y": 6.720759968199705,
-            "heading": 1.006760857121406,
-            "angularVelocity": -0.5061419234074507,
-            "velocityX": 0.5286878015432446,
-            "velocityY": 0.011572252862695013,
-            "timestamp": 0.06567158605400746
-        },
-        {
-            "x": 0.8143576636925598,
-            "y": 6.722372451876338,
-            "heading": 0.9431499891720047,
-            "angularVelocity": -0.9686208567749168,
-            "velocityX": 1.0603961533765867,
-            "velocityY": 0.024553749551892837,
-            "timestamp": 0.1313431721080149
-        },
-        {
-            "x": 0.9191612228651693,
-            "y": 6.724923861576486,
-            "heading": 0.8530685118915011,
-            "angularVelocity": -1.3716963864162066,
-            "velocityX": 1.5958737327650472,
-            "velocityY": 0.03885104431723037,
-            "timestamp": 0.19701475816202235
-        },
-        {
-            "x": 1.059454644842809,
-            "y": 6.7284663342548425,
-            "heading": 0.7426650921374723,
-            "angularVelocity": -1.6811444094442096,
-            "velocityX": 2.1362880114128,
-            "velocityY": 0.05394224338464699,
-            "timestamp": 0.2626863442160298
-        },
-        {
-            "x": 1.2356244745863587,
-            "y": 6.733039509062333,
-            "heading": 0.6243118436370656,
-            "angularVelocity": -1.8021987226420155,
-            "velocityX": 2.682588320596835,
-            "velocityY": 0.06963703912619325,
-            "timestamp": 0.3283579302700373
-        },
-        {
-            "x": 1.4470569399809903,
-            "y": 6.738777367450543,
-            "heading": 0.5308220213971302,
-            "angularVelocity": -1.4235962287107038,
-            "velocityX": 3.219542546463745,
-            "velocityY": 0.08737200870294055,
-            "timestamp": 0.39402951632404476
-        },
-        {
-            "x": 1.688226065456379,
-            "y": 6.748371696739962,
-            "heading": 0.5126606987274956,
-            "angularVelocity": -0.2765476480908373,
-            "velocityX": 3.6723511638207493,
-            "velocityY": 0.14609559272058092,
-            "timestamp": 0.45970110237805223
-        },
-        {
-            "x": 1.9365780121804386,
-            "y": 6.754665679712547,
-            "heading": 0.5126606479364623,
-            "angularVelocity": -7.734095747362075e-7,
-            "velocityX": 3.7817260347544686,
-            "velocityY": 0.09584027660688858,
-            "timestamp": 0.5253726884320596
-        },
-        {
-            "x": 2.1849299738494317,
-            "y": 6.760959072962077,
-            "heading": 0.5126605971462416,
-            "angularVelocity": -7.733971982996713e-7,
-            "velocityX": 3.7817262623252414,
-            "velocityY": 0.09583129672545204,
-            "timestamp": 0.5910442744860671
-        },
-        {
-            "x": 2.433281935680659,
-            "y": 6.767252459809602,
-            "heading": 0.5126605463560266,
-            "angularVelocity": -7.733971099299294e-7,
-            "velocityX": 3.7817262647956325,
-            "velocityY": 0.09583119924030911,
-            "timestamp": 0.6567158605400746
-        },
-        {
-            "x": 2.6816338977021794,
-            "y": 6.773545839147862,
-            "heading": 0.5126604955658177,
-            "angularVelocity": -7.733970196193196e-7,
-            "velocityX": 3.78172626769327,
-            "velocityY": 0.09583108489457716,
-            "timestamp": 0.722387446594082
-        },
-        {
-            "x": 2.9299858599438156,
-            "y": 6.779839209799928,
-            "heading": 0.5126604447756148,
-            "angularVelocity": -7.733969286509421e-7,
-            "velocityX": 3.7817262710450463,
-            "velocityY": 0.09583095262742326,
-            "timestamp": 0.7880590326480895
-        },
-        {
-            "x": 3.1783378224424883,
-            "y": 6.786132570308856,
-            "heading": 0.5126603939854177,
-            "angularVelocity": -7.733968385113031e-7,
-            "velocityX": 3.781726274959007,
-            "velocityY": 0.09583079817497217,
-            "timestamp": 0.853730618702097
-        },
-        {
-            "x": 3.426689785244194,
-            "y": 6.792425918859452,
-            "heading": 0.5126603431952265,
-            "angularVelocity": -7.733967515033378e-7,
-            "velocityX": 3.7817262795733857,
-            "velocityY": 0.09583061608201786,
-            "timestamp": 0.9194022047561045
-        },
-        {
-            "x": 3.6750417484067377,
-            "y": 6.798719253170605,
-            "heading": 0.5126602924050404,
-            "angularVelocity": -7.733966712943809e-7,
-            "velocityX": 3.781726285067957,
-            "velocityY": 0.09583039925395942,
-            "timestamp": 0.9850737908101119
-        },
-        {
-            "x": 3.9233937120031053,
-            "y": 6.805012570362013,
-            "heading": 0.5126602416148589,
-            "angularVelocity": -7.733966025007139e-7,
-            "velocityX": 3.7817262916739214,
-            "velocityY": 0.095830138566711,
-            "timestamp": 1.0507453768641193
-        },
-        {
-            "x": 4.171745676125189,
-            "y": 6.811305866807266,
-            "heading": 0.5126601908246804,
-            "angularVelocity": -7.733965561950186e-7,
-            "velocityX": 3.7817262996791623,
-            "velocityY": 0.09582982265909266,
-            "timestamp": 1.1164169629181266
-        },
-        {
-            "x": 4.420097640886595,
-            "y": 6.817599138023107,
-            "heading": 0.5126601400345029,
-            "angularVelocity": -7.733965406200526e-7,
-            "velocityX": 3.781726309414286,
-            "velocityY": 0.09582943848362173,
-            "timestamp": 1.182088548972134
-        },
-        {
-            "x": 4.6684496064150185,
-            "y": 6.8238923789700765,
-            "heading": 0.5126600892443262,
-            "angularVelocity": -7.73396529455525e-7,
-            "velocityX": 3.7817263210938923,
-            "velocityY": 0.09582897757087808,
-            "timestamp": 1.2477601350261414
-        },
-        {
-            "x": 4.916801572841165,
-            "y": 6.830185584489968,
-            "heading": 0.5126600384541553,
-            "angularVelocity": -7.733964406232834e-7,
-            "velocityX": 3.7817263347637717,
-            "velocityY": 0.09582843811261686,
-            "timestamp": 1.3134317210801487
-        },
-        {
-            "x": 5.165153539254683,
-            "y": 6.8364787905062485,
-            "heading": 0.5126599876639959,
-            "angularVelocity": -7.733962639482034e-7,
-            "velocityX": 3.781726334571479,
-            "velocityY": 0.09582844567126618,
-            "timestamp": 1.379103307134156
-        },
-        {
-            "x": 5.413504500375102,
-            "y": 6.842811545538402,
-            "heading": 0.5126599368726894,
-            "angularVelocity": -7.734137320837285e-7,
-            "velocityX": 3.781711026686433,
-            "velocityY": 0.09643066982787928,
-            "timestamp": 1.4447748931881634
-        },
-        {
-            "x": 5.661808131063952,
-            "y": 6.834835924305975,
-            "heading": 0.5126598860199024,
-            "angularVelocity": -7.743499133059843e-7,
-            "velocityX": 3.780990312684674,
-            "velocityY": -0.12144706275568991,
-            "timestamp": 1.5104464792421708
-        },
-        {
-            "x": 5.9063591957092285,
-            "y": 6.791097164154053,
-            "heading": 0.5126598314406843,
-            "angularVelocity": -8.310933457001734e-7,
-            "velocityX": 3.723848917614642,
-            "velocityY": -0.6660225948568639,
-            "timestamp": 1.5761180652961782
-        },
-        {
-            "x": 6.194622556481831,
-            "y": 6.683762426219015,
-            "heading": 0.5126597887806125,
-            "angularVelocity": -5.246475178274693e-7,
-            "velocityX": 3.5451571072929595,
-            "velocityY": -1.3200377183874616,
-            "timestamp": 1.65742993725234
-        },
-        {
-            "x": 6.459547388261047,
-            "y": 6.527457130068993,
-            "heading": 0.5126597482300577,
-            "angularVelocity": -4.987039864896594e-7,
-            "velocityX": 3.2581322432449182,
-            "velocityY": -1.9222936625379825,
-            "timestamp": 1.7387418092085019
-        },
-        {
-            "x": 6.723521508223802,
-            "y": 6.3695515377198895,
-            "heading": 0.5126597076877473,
-            "angularVelocity": -4.98602596336278e-7,
-            "velocityX": 3.2464400783305223,
-            "velocityY": -1.9419746286809643,
-            "timestamp": 1.8200536811646637
-        },
-        {
-            "x": 6.987495332330779,
-            "y": 6.211645450805702,
-            "heading": 0.5126596671480953,
-            "angularVelocity": -4.985699012832353e-7,
-            "velocityX": 3.246436439802837,
-            "velocityY": -1.9419807109984206,
-            "timestamp": 1.9013655531208256
-        },
-        {
-            "x": 7.218138260463332,
-            "y": 6.072816355413413,
-            "heading": 0.4174347514218684,
-            "angularVelocity": -1.1711071635093024,
-            "velocityX": 2.8365221779297074,
-            "velocityY": -1.7073656288101424,
-            "timestamp": 1.9826774250769874
-        },
-        {
-            "x": 7.40359391202109,
-            "y": 5.961763553999033,
-            "heading": 0.26911254592387807,
-            "angularVelocity": -1.8241150022712385,
-            "velocityX": 2.2807942689821186,
-            "velocityY": -1.365763679308134,
-            "timestamp": 2.063989297033149
-        },
-        {
-            "x": 7.542069024042313,
-            "y": 5.879081457057784,
-            "heading": 0.13934312775190755,
-            "angularVelocity": -1.595946754760921,
-            "velocityX": 1.7030122255201257,
-            "velocityY": -1.0168514750927211,
-            "timestamp": 2.145301168989311
-        },
-        {
-            "x": 7.63409233729861,
-            "y": 5.824280880315082,
-            "heading": 0.04592275308495019,
-            "angularVelocity": -1.1489143272623823,
-            "velocityX": 1.1317328090287153,
-            "velocityY": -0.6739554191077514,
-            "timestamp": 2.2266130409454727
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -0.003,
-            "angularVelocity": -0.6016680210157614,
-            "velocityX": 0.5645874531846384,
-            "velocityY": -0.3355091902172136,
-            "timestamp": 2.3079249129016346
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -0.003,
-            "angularVelocity": 3.440382093642737e-30,
-            "velocityX": 0,
-            "velocityY": 2.1122019835026996e-31,
-            "timestamp": 2.3892367848577964
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Amp Side To C3.traj b/src/main/deploy/choreo/Amp Side To C3.traj
deleted file mode 100644
index 463b42ef..00000000
--- a/src/main/deploy/choreo/Amp Side To C3.traj	
+++ /dev/null
@@ -1,526 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 1.3928609100313025e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 0.729279758485921,
-            "y": 6.711628327586588,
-            "heading": 1.093428142295198,
-            "angularVelocity": 0.9770378814200095,
-            "velocityX": 0.352568020824986,
-            "velocityY": -0.15309237280887605,
-            "timestamp": 0.05468379815278649
-        },
-        {
-            "x": 0.7686493967347447,
-            "y": 6.69501140584797,
-            "heading": 1.1949690599508662,
-            "angularVelocity": 1.8568739020644303,
-            "velocityX": 0.7199506906748694,
-            "velocityY": -0.30387285265355396,
-            "timestamp": 0.10936759630557298
-        },
-        {
-            "x": 0.8290790315170126,
-            "y": 6.670059182269693,
-            "heading": 1.3362954219692313,
-            "angularVelocity": 2.584428419245852,
-            "velocityX": 1.1050738394839994,
-            "velocityY": -0.4563001185206737,
-            "timestamp": 0.16405139445835948
-        },
-        {
-            "x": 0.9115429372168562,
-            "y": 6.636285478537955,
-            "heading": 1.5055888656223098,
-            "angularVelocity": 3.0958611027725738,
-            "velocityX": 1.508013497333151,
-            "velocityY": -0.6176181039468854,
-            "timestamp": 0.21873519261114596
-        },
-        {
-            "x": 1.0168329273736771,
-            "y": 6.593191021214006,
-            "heading": 1.6881423696003794,
-            "angularVelocity": 3.3383471913932383,
-            "velocityX": 1.9254330114861617,
-            "velocityY": -0.7880662788554578,
-            "timestamp": 0.27341899076393245
-        },
-        {
-            "x": 1.1449422871140338,
-            "y": 6.540878714080721,
-            "heading": 1.8564615478628756,
-            "angularVelocity": 3.078044758197163,
-            "velocityX": 2.3427297310698694,
-            "velocityY": -0.9566326572109004,
-            "timestamp": 0.32810278891671896
-        },
-        {
-            "x": 1.2912409780613738,
-            "y": 6.479735868066328,
-            "heading": 1.974688906048384,
-            "angularVelocity": 2.1620180415263293,
-            "velocityX": 2.675357160425865,
-            "velocityY": -1.1181162991560991,
-            "timestamp": 0.38278658706950547
-        },
-        {
-            "x": 1.4553681499836075,
-            "y": 6.408144093279595,
-            "heading": 2.0430197361851268,
-            "angularVelocity": 1.2495626208301445,
-            "velocityX": 3.001385738855639,
-            "velocityY": -1.3091953595963746,
-            "timestamp": 0.437470385222292
-        },
-        {
-            "x": 1.6374055328463037,
-            "y": 6.326368400043195,
-            "heading": 2.0610350683831213,
-            "angularVelocity": 0.3294455178051113,
-            "velocityX": 3.328908909254691,
-            "velocityY": -1.4954281889476506,
-            "timestamp": 0.4921541833750785
-        },
-        {
-            "x": 1.8283093127689483,
-            "y": 6.246687216839916,
-            "heading": 2.061035169590908,
-            "angularVelocity": 0.000001850781943801841,
-            "velocityX": 3.4910482880003957,
-            "velocityY": -1.45712598420193,
-            "timestamp": 0.546837981527865
-        },
-        {
-            "x": 2.01921313853121,
-            "y": 6.167006143460992,
-            "heading": 2.0610352707983473,
-            "angularVelocity": 0.0000018507755948234292,
-            "velocityX": 3.491049126267287,
-            "velocityY": -1.4571239758492094,
-            "timestamp": 0.6015217796806515
-        },
-        {
-            "x": 2.210116959480156,
-            "y": 6.087325058550032,
-            "heading": 2.061035372005735,
-            "angularVelocity": 0.000001850774661522582,
-            "velocityX": 3.4910490382464,
-            "velocityY": -1.4571241867349893,
-            "timestamp": 0.6562055778334379
-        },
-        {
-            "x": 2.40102077504018,
-            "y": 6.007643960727976,
-            "heading": 2.0610354732130722,
-            "angularVelocity": 0.0000018507737318277825,
-            "velocityX": 3.49104893969945,
-            "velocityY": -1.4571244228395928,
-            "timestamp": 0.7108893759862244
-        },
-        {
-            "x": 2.5919245845305627,
-            "y": 5.927962848363926,
-            "heading": 2.0610355744203592,
-            "angularVelocity": 0.0000018507728139380304,
-            "velocityX": 3.491048828704212,
-            "velocityY": -1.457124688768356,
-            "timestamp": 0.7655731741390108
-        },
-        {
-            "x": 2.78282838714305,
-            "y": 5.848281719521431,
-            "heading": 2.0610356756275974,
-            "angularVelocity": 0.0000018507719179726602,
-            "velocityX": 3.4910487029284663,
-            "velocityY": -1.457124990108895,
-            "timestamp": 0.8202569722917973
-        },
-        {
-            "x": 2.973732181912291,
-            "y": 5.768600571887663,
-            "heading": 2.061035776834788,
-            "angularVelocity": 0.0000018507710613284672,
-            "velocityX": 3.491048559499401,
-            "velocityY": -1.4571253337439838,
-            "timestamp": 0.8749407704445837
-        },
-        {
-            "x": 3.16463596766188,
-            "y": 5.688919402644143,
-            "heading": 2.061035878041936,
-            "angularVelocity": 0.000001850770272196717,
-            "velocityX": 3.4910483945574473,
-            "velocityY": -1.4571257289205004,
-            "timestamp": 0.9296245685973702
-        },
-        {
-            "x": 3.3555397425941185,
-            "y": 5.609238207483871,
-            "heading": 2.061035979249042,
-            "angularVelocity": 0.000001850769506382635,
-            "velocityX": 3.4910481967410822,
-            "velocityY": -1.457126202858873,
-            "timestamp": 0.9843083667501566
-        },
-        {
-            "x": 3.546443239175115,
-            "y": 5.529556345440524,
-            "heading": 2.0610360804562036,
-            "angularVelocity": 0.0000018507705174022697,
-            "velocityX": 3.491043106545231,
-            "velocityY": -1.4571383981177848,
-            "timestamp": 1.0389921649029432
-        },
-        {
-            "x": 3.7301891812097523,
-            "y": 5.434525969364101,
-            "heading": 2.0610361860102304,
-            "angularVelocity": 0.0000019302614304565207,
-            "velocityX": 3.360153249071168,
-            "velocityY": -1.7378159397580357,
-            "timestamp": 1.0936759630557298
-        },
-        {
-            "x": 3.9011674048607876,
-            "y": 5.318079465137689,
-            "heading": 2.0610363082632275,
-            "angularVelocity": 0.0000022356347107367405,
-            "velocityX": 3.126670593972313,
-            "velocityY": -2.129451650396711,
-            "timestamp": 1.1483597612085163
-        },
-        {
-            "x": 4.05689386248419,
-            "y": 5.181908226701039,
-            "heading": 2.0610364709885136,
-            "angularVelocity": 0.0000029757495174652245,
-            "velocityX": 2.8477622784778513,
-            "velocityY": -2.490156920998575,
-            "timestamp": 1.203043559361303
-        },
-        {
-            "x": 4.195106185477333,
-            "y": 5.027990436285506,
-            "heading": 2.061036659879246,
-            "angularVelocity": 0.0000034542357869786317,
-            "velocityX": 2.52748213660978,
-            "velocityY": -2.814687267798894,
-            "timestamp": 1.2577273575140895
-        },
-        {
-            "x": 4.313711649853664,
-            "y": 4.858669951905228,
-            "heading": 2.0606108116890636,
-            "angularVelocity": -0.007787465475470693,
-            "velocityX": 2.168932451344132,
-            "velocityY": -3.096355595256869,
-            "timestamp": 1.312411155666876
-        },
-        {
-            "x": 4.40996698448611,
-            "y": 4.67856050613378,
-            "heading": 2.051588761211157,
-            "angularVelocity": -0.16498580535132465,
-            "velocityX": 1.7602166982532683,
-            "velocityY": -3.293652815925874,
-            "timestamp": 1.3670949538196626
-        },
-        {
-            "x": 4.510742037442817,
-            "y": 4.51918682060045,
-            "heading": 1.996778499947209,
-            "angularVelocity": -1.002312624862085,
-            "velocityX": 1.8428685709639585,
-            "velocityY": -2.9144589607334965,
-            "timestamp": 1.4217787519724492
-        },
-        {
-            "x": 4.62612450472694,
-            "y": 4.379617585881877,
-            "heading": 1.9276644376664414,
-            "angularVelocity": -1.263885549567404,
-            "velocityX": 2.1099936577511538,
-            "velocityY": -2.5522959163995464,
-            "timestamp": 1.4764625501252358
-        },
-        {
-            "x": 4.756154634809633,
-            "y": 4.26013511834241,
-            "heading": 1.8517335763640235,
-            "angularVelocity": -1.3885440270675238,
-            "velocityX": 2.3778547664042544,
-            "velocityY": -2.1849701662205963,
-            "timestamp": 1.5311463482780223
-        },
-        {
-            "x": 4.900439739227295,
-            "y": 4.161101341247559,
-            "heading": 1.7737408289368037,
-            "angularVelocity": -1.4262496399629772,
-            "velocityX": 2.638534799915143,
-            "velocityY": -1.8110259426046127,
-            "timestamp": 1.5858301464308089
-        },
-        {
-            "x": 5.008276114908647,
-            "y": 4.1019129343639005,
-            "heading": 1.719841693932476,
-            "angularVelocity": -1.4074809230288232,
-            "velocityX": 2.815956908545699,
-            "velocityY": -1.5456009367595303,
-            "timestamp": 1.6241248999315097
-        },
-        {
-            "x": 5.122676890234505,
-            "y": 4.053024888243358,
-            "heading": 1.6679900254477509,
-            "angularVelocity": -1.354014943163857,
-            "velocityX": 2.987374636678728,
-            "velocityY": -1.276625167978901,
-            "timestamp": 1.6624196534322104
-        },
-        {
-            "x": 5.243382451295884,
-            "y": 4.014566134854959,
-            "heading": 1.6197010617895011,
-            "angularVelocity": -1.260981185252083,
-            "velocityX": 3.1520130051018556,
-            "velocityY": -1.0042825680467018,
-            "timestamp": 1.7007144069329112
-        },
-        {
-            "x": 5.370099790784461,
-            "y": 3.9866453379407707,
-            "heading": 1.576858310766692,
-            "angularVelocity": -1.1187629402556432,
-            "velocityX": 3.3090000040412137,
-            "velocityY": -0.7291024059908604,
-            "timestamp": 1.739009160433612
-        },
-        {
-            "x": 5.502536097274536,
-            "y": 3.9692960416426737,
-            "heading": 1.5420636410872666,
-            "angularVelocity": -0.9086014792806747,
-            "velocityX": 3.4583407486263615,
-            "velocityY": -0.45304629778540073,
-            "timestamp": 1.7773039139343128
-        },
-        {
-            "x": 5.640613600815518,
-            "y": 3.9622331858505495,
-            "heading": 1.5196895147171037,
-            "angularVelocity": -0.5842608797508935,
-            "velocityX": 3.6056506680074274,
-            "velocityY": -0.18443403198808295,
-            "timestamp": 1.8155986674350135
-        },
-        {
-            "x": 5.785301546169388,
-            "y": 3.963377277975263,
-            "heading": 1.519119424890785,
-            "angularVelocity": -0.014886891132695255,
-            "velocityX": 3.778270706226711,
-            "velocityY": 0.029875949578638192,
-            "timestamp": 1.8538934209357143
-        },
-        {
-            "x": 5.929550760809323,
-            "y": 3.9767385598159253,
-            "heading": 1.5191191822149777,
-            "angularVelocity": -0.000006337051042848807,
-            "velocityX": 3.7668140268168893,
-            "velocityY": 0.3489063283934661,
-            "timestamp": 1.892188174436415
-        },
-        {
-            "x": 6.073462686179593,
-            "y": 3.9933433315789473,
-            "heading": 1.5191189525644635,
-            "angularVelocity": -0.0000059969184602345635,
-            "velocityX": 3.7580063119517533,
-            "velocityY": 0.4336043516435729,
-            "timestamp": 1.9304829279371158
-        },
-        {
-            "x": 6.217374593429904,
-            "y": 4.009948260386706,
-            "heading": 1.519118722914699,
-            "angularVelocity": -0.000005996898884562842,
-            "velocityX": 3.7580058387809343,
-            "velocityY": 0.43360845259010783,
-            "timestamp": 1.9687776814378166
-        },
-        {
-            "x": 6.361286503020876,
-            "y": 4.026553168908082,
-            "heading": 1.5191184932648618,
-            "angularVelocity": -0.000005996900776462565,
-            "velocityX": 3.7580058999032113,
-            "velocityY": 0.4336079228469906,
-            "timestamp": 2.0070724349385176
-        },
-        {
-            "x": 6.5051984133944005,
-            "y": 4.04315807064667,
-            "heading": 1.5191182636148604,
-            "angularVelocity": -0.000005996905070522029,
-            "velocityX": 3.7580059203381437,
-            "velocityY": 0.4336077457264339,
-            "timestamp": 2.0453671884392186
-        },
-        {
-            "x": 6.6491104352583505,
-            "y": 4.059762006067292,
-            "heading": 1.5191180339567425,
-            "angularVelocity": -0.000005997117020238007,
-            "velocityX": 3.7580088317141747,
-            "velocityY": 0.4335825120357026,
-            "timestamp": 2.0836619419399196
-        },
-        {
-            "x": 6.790280334982046,
-            "y": 4.068588148423426,
-            "heading": 1.5085875786821552,
-            "angularVelocity": -0.27498428144717973,
-            "velocityX": 3.6864031445224414,
-            "velocityY": 0.23047915312922457,
-            "timestamp": 2.1219566954406206
-        },
-        {
-            "x": 6.921716704437578,
-            "y": 4.075833761978748,
-            "heading": 1.4694677109965053,
-            "angularVelocity": -1.0215464028234083,
-            "velocityX": 3.43222915517987,
-            "velocityY": 0.18920642889606504,
-            "timestamp": 2.1602514489413216
-        },
-        {
-            "x": 7.043039125371844,
-            "y": 4.08211395301362,
-            "heading": 1.4037305732475935,
-            "angularVelocity": -1.716609502335841,
-            "velocityX": 3.1681212135768497,
-            "velocityY": 0.16399612115943957,
-            "timestamp": 2.1985462024420226
-        },
-        {
-            "x": 7.153946697571625,
-            "y": 4.087967374487544,
-            "heading": 1.3134036899027293,
-            "angularVelocity": -2.3587273735346383,
-            "velocityX": 2.896155793188521,
-            "velocityY": 0.1528517861805002,
-            "timestamp": 2.2368409559427236
-        },
-        {
-            "x": 7.254278338500056,
-            "y": 4.0938696715910385,
-            "heading": 1.199938125135736,
-            "angularVelocity": -2.9629532610757465,
-            "velocityX": 2.619983986229206,
-            "velocityY": 0.1541280871121784,
-            "timestamp": 2.2751357094434246
-        },
-        {
-            "x": 7.34404947628579,
-            "y": 4.100268813517142,
-            "heading": 1.0638507127340207,
-            "angularVelocity": -3.5536829450860603,
-            "velocityX": 2.344215057660344,
-            "velocityY": 0.16710231405423262,
-            "timestamp": 2.3134304629441256
-        },
-        {
-            "x": 7.4235066081317465,
-            "y": 4.107648882181887,
-            "heading": 0.9045749783818139,
-            "angularVelocity": -4.159205107542786,
-            "velocityX": 2.0748829691383666,
-            "velocityY": 0.192717487125464,
-            "timestamp": 2.3517252164448266
-        },
-        {
-            "x": 7.492760083799764,
-            "y": 4.113297423743801,
-            "heading": 0.7219171880631509,
-            "angularVelocity": -4.769786292404788,
-            "velocityX": 1.808432470175071,
-            "velocityY": 0.1475017083426549,
-            "timestamp": 2.3900199699455276
-        },
-        {
-            "x": 7.549820937161571,
-            "y": 4.1180620764150975,
-            "heading": 0.5402485968001275,
-            "angularVelocity": -4.7439551023536115,
-            "velocityX": 1.4900436259698675,
-            "velocityY": 0.1244205076606485,
-            "timestamp": 2.4283147234462286
-        },
-        {
-            "x": 7.595294548959434,
-            "y": 4.121747510359626,
-            "heading": 0.3723680212509228,
-            "angularVelocity": -4.383905370904984,
-            "velocityX": 1.1874632329734431,
-            "velocityY": 0.09623861254154589,
-            "timestamp": 2.4666094769469296
-        },
-        {
-            "x": 7.630234770559726,
-            "y": 4.124236647711556,
-            "heading": 0.22836852122657442,
-            "angularVelocity": -3.7602931697083015,
-            "velocityX": 0.912402311184801,
-            "velocityY": 0.06499943528517171,
-            "timestamp": 2.5049042304476306
-        },
-        {
-            "x": 7.655576852148959,
-            "y": 4.125750923706981,
-            "heading": 0.11519495737029227,
-            "angularVelocity": -2.9553281718920323,
-            "velocityX": 0.6617637998053468,
-            "velocityY": 0.03954264897925492,
-            "timestamp": 2.5431989839483315
-        },
-        {
-            "x": 7.671995118301485,
-            "y": 4.1266005721732935,
-            "heading": 0.03711275718074835,
-            "angularVelocity": -2.0389790520029063,
-            "velocityX": 0.4287340863083462,
-            "velocityY": 0.02218707234390751,
-            "timestamp": 2.5814937374490325
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -0.003,
-            "angularVelocity": -1.047473962197309,
-            "velocityX": 0.20903337838088354,
-            "velocityY": 0.010430353774151994,
-            "timestamp": 2.6197884909497335
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -0.003,
-            "angularVelocity": 7.250240493925158e-33,
-            "velocityX": 3.887711813813661e-34,
-            "velocityY": -4.900942603355191e-34,
-            "timestamp": 2.6580832444504345
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Amp Side To C4.traj b/src/main/deploy/choreo/Amp Side To C4.traj
deleted file mode 100644
index ca6ee59b..00000000
--- a/src/main/deploy/choreo/Amp Side To C4.traj	
+++ /dev/null
@@ -1,400 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7271139423991175,
-            "y": 6.709732017673653,
-            "heading": 0.9305052411067223,
-            "angularVelocity": -1.767895408627961,
-            "velocityX": 0.2763206248110281,
-            "velocityY": -0.16578618916650492,
-            "timestamp": 0.06193508867035021
-        },
-        {
-            "x": 0.7615532933104725,
-            "y": 6.686914726598375,
-            "heading": 0.7206788937202419,
-            "angularVelocity": -3.387842851138587,
-            "velocityX": 0.5560555680263672,
-            "velocityY": -0.36840652956392367,
-            "timestamp": 0.12387017734070042
-        },
-        {
-            "x": 0.8202948153873781,
-            "y": 6.650989300229197,
-            "heading": 0.4540271404715134,
-            "angularVelocity": -4.305342237709284,
-            "velocityX": 0.9484368770271357,
-            "velocityY": -0.5800496477915925,
-            "timestamp": 0.18580526601105063
-        },
-        {
-            "x": 0.9068549514543394,
-            "y": 6.604915057765644,
-            "heading": 0.1437481054753499,
-            "angularVelocity": -5.009745552276917,
-            "velocityX": 1.3975944480789861,
-            "velocityY": -0.7439117865606645,
-            "timestamp": 0.24774035468140085
-        },
-        {
-            "x": 1.022793030858839,
-            "y": 6.550181878656492,
-            "heading": -0.13251218552677876,
-            "angularVelocity": -4.460481076769346,
-            "velocityX": 1.871928851536495,
-            "velocityY": -0.8837184265686551,
-            "timestamp": 0.30967544335175107
-        },
-        {
-            "x": 1.1650239086947571,
-            "y": 6.483494014593417,
-            "heading": -0.35594040398018906,
-            "angularVelocity": -3.6074577957352734,
-            "velocityX": 2.296450701684509,
-            "velocityY": -1.0767380090149254,
-            "timestamp": 0.37161053202210126
-        },
-        {
-            "x": 1.3271753983931303,
-            "y": 6.398307258999271,
-            "heading": -0.5136957877872723,
-            "angularVelocity": -2.5471083870846947,
-            "velocityX": 2.61808763302859,
-            "velocityY": -1.3754199343695408,
-            "timestamp": 0.43354562069245145
-        },
-        {
-            "x": 1.510544798478812,
-            "y": 6.2989823057862875,
-            "heading": -0.5925289533905876,
-            "angularVelocity": -1.2728352747326335,
-            "velocityX": 2.96067066379232,
-            "velocityY": -1.6036943733404823,
-            "timestamp": 0.49548070936280164
-        },
-        {
-            "x": 1.7178378905511122,
-            "y": 6.190936000456915,
-            "heading": -0.5942013489806535,
-            "angularVelocity": -0.027002392762643195,
-            "velocityX": 3.3469410720572084,
-            "velocityY": -1.7445087695675872,
-            "timestamp": 0.5574157980331519
-        },
-        {
-            "x": 1.927701821836241,
-            "y": 6.086763021965953,
-            "heading": -0.5942019920614379,
-            "angularVelocity": -0.000010383141417040622,
-            "velocityX": 3.388449678374247,
-            "velocityY": -1.6819702809408126,
-            "timestamp": 0.6193508867035021
-        },
-        {
-            "x": 2.1375657815463693,
-            "y": 5.982590100737573,
-            "heading": -0.594202635139582,
-            "angularVelocity": -0.0000103830987882079,
-            "velocityX": 3.3884501373224802,
-            "velocityY": -1.6819693563827811,
-            "timestamp": 0.6812859753738524
-        },
-        {
-            "x": 2.347429741257157,
-            "y": 5.878417179509917,
-            "heading": -0.5942032782166262,
-            "angularVelocity": -0.000010383081029176535,
-            "velocityX": 3.3884501373331317,
-            "velocityY": -1.6819693563710962,
-            "timestamp": 0.7432210640442026
-        },
-        {
-            "x": 2.5572937009682177,
-            "y": 5.774244258282216,
-            "heading": -0.5942039212925836,
-            "angularVelocity": -0.000010383063479788062,
-            "velocityX": 3.3884501373375415,
-            "velocityY": -1.6819693563718265,
-            "timestamp": 0.8051561527145529
-        },
-        {
-            "x": 2.7671576606795463,
-            "y": 5.670071337054474,
-            "heading": -0.594204564367474,
-            "angularVelocity": -0.00001038304625561267,
-            "velocityX": 3.388450137341862,
-            "velocityY": -1.68196935637249,
-            "timestamp": 0.8670912413849031
-        },
-        {
-            "x": 2.977021620391137,
-            "y": 5.5658984158266955,
-            "heading": -0.5942052074413191,
-            "angularVelocity": -0.000010383029375253952,
-            "velocityX": 3.388450137346089,
-            "velocityY": -1.6819693563730826,
-            "timestamp": 0.9290263300552534
-        },
-        {
-            "x": 3.186885580102983,
-            "y": 5.461725494598884,
-            "heading": -0.5942058505141404,
-            "angularVelocity": -0.000010383012846871076,
-            "velocityX": 3.3884501373502207,
-            "velocityY": -1.6819693563736025,
-            "timestamp": 0.9909614187256036
-        },
-        {
-            "x": 3.3967495398150795,
-            "y": 5.357552573371046,
-            "heading": -0.594206493585961,
-            "angularVelocity": -0.000010382996688205053,
-            "velocityX": 3.3884501373542517,
-            "velocityY": -1.681969356374048,
-            "timestamp": 1.0528965073959538
-        },
-        {
-            "x": 3.6066134995274184,
-            "y": 5.253379652143185,
-            "heading": -0.5942071366568044,
-            "angularVelocity": -0.000010382980910469912,
-            "velocityX": 3.38845013735818,
-            "velocityY": -1.6819693563744145,
-            "timestamp": 1.114831596066304
-        },
-        {
-            "x": 3.8164774592399944,
-            "y": 5.149206730915306,
-            "heading": -0.5942077797266946,
-            "angularVelocity": -0.000010382965523044611,
-            "velocityX": 3.388450137362003,
-            "velocityY": -1.681969356374701,
-            "timestamp": 1.1767666847366542
-        },
-        {
-            "x": 4.026341418952801,
-            "y": 5.045033809687414,
-            "heading": -0.594208422795657,
-            "angularVelocity": -0.00001038295054018878,
-            "velocityX": 3.388450137365716,
-            "velocityY": -1.6819693563749047,
-            "timestamp": 1.2387017734070045
-        },
-        {
-            "x": 4.236205378665829,
-            "y": 4.940860888459515,
-            "heading": -0.594209065863717,
-            "angularVelocity": -0.000010382935971395078,
-            "velocityX": 3.388450137369317,
-            "velocityY": -1.681969356375023,
-            "timestamp": 1.3006368620773547
-        },
-        {
-            "x": 4.446069338379075,
-            "y": 4.836687967231613,
-            "heading": -0.5942097089309012,
-            "angularVelocity": -0.000010382921828372961,
-            "velocityX": 3.3884501373728035,
-            "velocityY": -1.6819693563750544,
-            "timestamp": 1.362571950747705
-        },
-        {
-            "x": 4.6559332980925285,
-            "y": 4.732515046003717,
-            "heading": -0.5942103519972364,
-            "angularVelocity": -0.000010382908121631514,
-            "velocityX": 3.388450137376173,
-            "velocityY": -1.6819693563749973,
-            "timestamp": 1.4245070394180552
-        },
-        {
-            "x": 4.865797257806183,
-            "y": 4.628342124775828,
-            "heading": -0.5942109950627501,
-            "angularVelocity": -0.00001038289485891648,
-            "velocityX": 3.388450137379422,
-            "velocityY": -1.6819693563748486,
-            "timestamp": 1.4864421280884055
-        },
-        {
-            "x": 5.075661217518705,
-            "y": 4.524169203545277,
-            "heading": -0.594211638127464,
-            "angularVelocity": -0.000010382881945026341,
-            "velocityX": 3.3884501373611275,
-            "velocityY": -1.6819693564178315,
-            "timestamp": 1.5483772167587557
-        },
-        {
-            "x": 5.285525078653758,
-            "y": 4.419996083725492,
-            "heading": -0.5942122811951368,
-            "angularVelocity": -0.000010382929719206097,
-            "velocityX": 3.3884485457355824,
-            "velocityY": -1.6819725628269917,
-            "timestamp": 1.610312305429106
-        },
-        {
-            "x": 5.489266395568848,
-            "y": 4.304305553436279,
-            "heading": -0.5942133396987443,
-            "angularVelocity": -0.000017090531880803138,
-            "velocityX": 3.2895943364108793,
-            "velocityY": -1.8679319392755818,
-            "timestamp": 1.6722473940994562
-        },
-        {
-            "x": 5.681024172067428,
-            "y": 4.156179263559471,
-            "heading": -0.5942132880529166,
-            "angularVelocity": 8.063055616728172e-7,
-            "velocityX": 2.9937628707653796,
-            "velocityY": -2.312578894659331,
-            "timestamp": 1.7362998206864544
-        },
-        {
-            "x": 5.864967854996084,
-            "y": 3.998454299605836,
-            "heading": -0.5942132372714615,
-            "angularVelocity": 7.928107921043856e-7,
-            "velocityX": 2.871767593048443,
-            "velocityY": -2.4624354198261003,
-            "timestamp": 1.8003522472734526
-        },
-        {
-            "x": 6.0489114264021975,
-            "y": 3.840729205591288,
-            "heading": -0.5942131864900138,
-            "angularVelocity": 7.928106775338953e-7,
-            "velocityX": 2.871765851934987,
-            "velocityY": -2.4624374503645132,
-            "timestamp": 1.8644046738604507
-        },
-        {
-            "x": 6.232854997807022,
-            "y": 3.683004111575237,
-            "heading": -0.5942131357085663,
-            "angularVelocity": 7.928106743629292e-7,
-            "velocityX": 2.87176585191486,
-            "velocityY": -2.462437450387986,
-            "timestamp": 1.928457100447449
-        },
-        {
-            "x": 6.4167985692118465,
-            "y": 3.5252790175591864,
-            "heading": -0.594213084927119,
-            "angularVelocity": 7.928106702567426e-7,
-            "velocityX": 2.871765851914861,
-            "velocityY": -2.462437450387986,
-            "timestamp": 1.992509527034447
-        },
-        {
-            "x": 6.6007421406166715,
-            "y": 3.367553923543135,
-            "heading": -0.5942130341456721,
-            "angularVelocity": 7.928106666471698e-7,
-            "velocityX": 2.8717658519148612,
-            "velocityY": -2.462437450387985,
-            "timestamp": 2.0565619536214452
-        },
-        {
-            "x": 6.784685712022088,
-            "y": 3.2098288295277735,
-            "heading": -0.5942129833642253,
-            "angularVelocity": 7.928106614555171e-7,
-            "velocityX": 2.8717658519240943,
-            "velocityY": -2.4624374503772177,
-            "timestamp": 2.120614380208443
-        },
-        {
-            "x": 6.968629334581004,
-            "y": 3.052103795169223,
-            "heading": -0.594212932582592,
-            "angularVelocity": 7.928135744203515e-7,
-            "velocityX": 2.871766650543336,
-            "velocityY": -2.462436519002493,
-            "timestamp": 2.184666806795441
-        },
-        {
-            "x": 7.14554518368725,
-            "y": 2.9064126516878526,
-            "heading": -0.554455950225117,
-            "angularVelocity": 0.6206943979472176,
-            "velocityX": 2.7620475684235433,
-            "velocityY": -2.2745608752775093,
-            "timestamp": 2.248719233382439
-        },
-        {
-            "x": 7.299470766691596,
-            "y": 2.7775737815312107,
-            "heading": -0.43989859569345574,
-            "angularVelocity": 1.7884935924490803,
-            "velocityX": 2.4031186827134454,
-            "velocityY": -2.0114596280228136,
-            "timestamp": 2.312771659969437
-        },
-        {
-            "x": 7.4271138269599435,
-            "y": 2.670230084319628,
-            "heading": -0.3103960824325961,
-            "angularVelocity": 2.021820564205559,
-            "velocityX": 1.9927903917110543,
-            "velocityY": -1.6758724521667427,
-            "timestamp": 2.376824086556435
-        },
-        {
-            "x": 7.52865057013146,
-            "y": 2.5846878922538004,
-            "heading": -0.1939655985882249,
-            "angularVelocity": 1.8177372825404448,
-            "velocityX": 1.5852130603296692,
-            "velocityY": -1.3355027533522315,
-            "timestamp": 2.440876513143433
-        },
-        {
-            "x": 7.604480145165485,
-            "y": 2.520737422138354,
-            "heading": -0.10094853997619462,
-            "angularVelocity": 1.4522019471922978,
-            "velocityX": 1.1838673267285131,
-            "velocityY": -0.9984082340516305,
-            "timestamp": 2.504928939730431
-        },
-        {
-            "x": 7.654868393442648,
-            "y": 2.4782123742697575,
-            "heading": -0.03632146129528005,
-            "angularVelocity": 1.008971589751342,
-            "velocityX": 0.7866719648587251,
-            "velocityY": -0.6639100208145539,
-            "timestamp": 2.568981366317429
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -0.003,
-            "angularVelocity": 0.5202216851226045,
-            "velocityX": 0.39235994475896174,
-            "velocityY": -0.3311720632620588,
-            "timestamp": 2.6330337929044267
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -0.003,
-            "angularVelocity": -9.133242422473854e-34,
-            "velocityX": 1.8081868884139647e-34,
-            "velocityY": 0,
-            "timestamp": 2.6970862194914247
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Amp Side To C5.traj b/src/main/deploy/choreo/Amp Side To C5.traj
deleted file mode 100644
index 6231149d..00000000
--- a/src/main/deploy/choreo/Amp Side To C5.traj	
+++ /dev/null
@@ -1,805 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7237404107060486,
-            "y": 6.712071337633356,
-            "heading": 0.9558489426883627,
-            "angularVelocity": -1.537881157714612,
-            "velocityX": 0.25110936688338653,
-            "velocityY": -0.14489824428928566,
-            "timestamp": 0.05471882979351295
-        },
-        {
-            "x": 0.7514918072858734,
-            "y": 6.694799584586779,
-            "heading": 0.7935491608961526,
-            "angularVelocity": -2.966068214628546,
-            "velocityX": 0.5071635611460984,
-            "velocityY": -0.31564551200661234,
-            "timestamp": 0.1094376595870259
-        },
-        {
-            "x": 0.7959632333479747,
-            "y": 6.666655428800684,
-            "heading": 0.574279005326375,
-            "angularVelocity": -4.007215731718184,
-            "velocityX": 0.8127261900504715,
-            "velocityY": -0.514341331718894,
-            "timestamp": 0.16415648938053887
-        },
-        {
-            "x": 0.8617224500002636,
-            "y": 6.63045184710829,
-            "heading": 0.31905346805964874,
-            "angularVelocity": -4.664309127769047,
-            "velocityX": 1.2017657705846174,
-            "velocityY": -0.6616293116832328,
-            "timestamp": 0.2188753191740518
-        },
-        {
-            "x": 0.9507709996198539,
-            "y": 6.585947725507511,
-            "heading": 0.05142997722620149,
-            "angularVelocity": -4.890884762034389,
-            "velocityX": 1.627384027685255,
-            "velocityY": -0.8133237090178916,
-            "timestamp": 0.2735941489675648
-        },
-        {
-            "x": 1.0614190010918925,
-            "y": 6.535018660742887,
-            "heading": -0.17495854287380355,
-            "angularVelocity": -4.137305584828904,
-            "velocityX": 2.0221192940269503,
-            "velocityY": -0.9307411170306262,
-            "timestamp": 0.32831297876107773
-        },
-        {
-            "x": 1.1921384640832198,
-            "y": 6.474205616338843,
-            "heading": -0.35862575739678126,
-            "angularVelocity": -3.356563274764148,
-            "velocityX": 2.388930163247485,
-            "velocityY": -1.111373262796896,
-            "timestamp": 0.3830318085545907
-        },
-        {
-            "x": 1.3384031857193457,
-            "y": 6.398894346098027,
-            "heading": -0.49152044440666937,
-            "angularVelocity": -2.4286829142980233,
-            "velocityX": 2.6730235677201417,
-            "velocityY": -1.376331886573771,
-            "timestamp": 0.4377506383481036
-        },
-        {
-            "x": 1.500957547301863,
-            "y": 6.312259668262383,
-            "heading": -0.5624501430724521,
-            "angularVelocity": -1.2962575942036991,
-            "velocityX": 2.9707207225727275,
-            "velocityY": -1.5832699303433067,
-            "timestamp": 0.4924694681416166
-        },
-        {
-            "x": 1.6817069101290123,
-            "y": 6.217739107445247,
-            "heading": -0.5720464000476684,
-            "angularVelocity": -0.17537394369443057,
-            "velocityX": 3.3032388212471107,
-            "velocityY": -1.7273863709040396,
-            "timestamp": 0.5471882979351296
-        },
-        {
-            "x": 1.8673643551319996,
-            "y": 6.126199810501224,
-            "heading": -0.5720469984173726,
-            "angularVelocity": -0.000010935352720012522,
-            "velocityX": 3.392935223643913,
-            "velocityY": -1.6729030443351731,
-            "timestamp": 0.6019071277286425
-        },
-        {
-            "x": 2.0530162492759167,
-            "y": 6.034649256644459,
-            "heading": -0.5720475972653951,
-            "angularVelocity": -0.000010944094102204816,
-            "velocityX": 3.3928337803365705,
-            "velocityY": -1.6731087671691436,
-            "timestamp": 0.6566259575221555
-        },
-        {
-            "x": 2.23866813267496,
-            "y": 5.943098680998449,
-            "heading": -0.5720481961130315,
-            "angularVelocity": -0.000010944087046546937,
-            "velocityX": 3.3928335839713664,
-            "velocityY": -1.673109165372953,
-            "timestamp": 0.7113447873156684
-        },
-        {
-            "x": 2.4243200161477754,
-            "y": 5.851548105501653,
-            "heading": -0.5720487949598734,
-            "angularVelocity": -0.000010944072528266654,
-            "velocityX": 3.392833585319575,
-            "velocityY": -1.673109162646019,
-            "timestamp": 0.7660636171091814
-        },
-        {
-            "x": 2.6099718996232633,
-            "y": 5.759997530009892,
-            "heading": -0.5720493938059237,
-            "angularVelocity": -0.000010944058060277418,
-            "velocityX": 3.3928335853684146,
-            "velocityY": -1.673109162553994,
-            "timestamp": 0.8207824469026943
-        },
-        {
-            "x": 2.7956237830994004,
-            "y": 5.668446954519065,
-            "heading": -0.5720499926511823,
-            "angularVelocity": -0.000010944043595406558,
-            "velocityX": 3.3928335853802802,
-            "velocityY": -1.6731091625369476,
-            "timestamp": 0.8755012766962073
-        },
-        {
-            "x": 2.9812756665756717,
-            "y": 5.576896379028125,
-            "heading": -0.5720505914956495,
-            "angularVelocity": -0.000010944029129446314,
-            "velocityX": 3.3928335853827263,
-            "velocityY": -1.6731091625390042,
-            "timestamp": 0.9302201064897202
-        },
-        {
-            "x": 3.166927550036217,
-            "y": 5.485345803504912,
-            "heading": -0.5720511903393257,
-            "angularVelocity": -0.000010944014673786385,
-            "velocityX": 3.392833585095338,
-            "velocityY": -1.673109163128796,
-            "timestamp": 0.9849389362832331
-        },
-        {
-            "x": 3.3525794334560626,
-            "y": 5.393795227898782,
-            "heading": -0.5720517891822119,
-            "angularVelocity": -0.000010944000236842356,
-            "velocityX": 3.392833584351539,
-            "velocityY": -1.6731091646441256,
-            "timestamp": 1.039657766076746
-        },
-        {
-            "x": 3.538231313226632,
-            "y": 5.302244644892125,
-            "heading": -0.5720523880244444,
-            "angularVelocity": -0.000010943988288632917,
-            "velocityX": 3.392833517660122,
-            "velocityY": -1.673109299890567,
-            "timestamp": 1.0943765958702592
-        },
-        {
-            "x": 3.723883292814953,
-            "y": 5.21069426429933,
-            "heading": -0.5720529868621156,
-            "angularVelocity": -0.000010943904932797782,
-            "velocityX": 3.3928353418539645,
-            "velocityY": -1.673105600727685,
-            "timestamp": 1.1490954256637722
-        },
-        {
-            "x": 3.909492929164672,
-            "y": 5.1190580682844296,
-            "heading": -0.5720535873273735,
-            "angularVelocity": -0.000010973649477031932,
-            "velocityX": 3.3920615088102064,
-            "velocityY": -1.6746738985594591,
-            "timestamp": 1.2038142554572853
-        },
-        {
-            "x": 4.092995643615723,
-            "y": 5.023272514343262,
-            "heading": -0.5720542830492642,
-            "angularVelocity": -0.000012714487741704976,
-            "velocityX": 3.353556995709023,
-            "velocityY": -1.7505044296932655,
-            "timestamp": 1.2585330852507983
-        },
-        {
-            "x": 4.152699494738863,
-            "y": 4.989124335806784,
-            "heading": -0.5720542274621303,
-            "angularVelocity": 0.0000030573365885939726,
-            "velocityX": 3.283759307267904,
-            "velocityY": -1.8781769850015708,
-            "timestamp": 1.2767146404552234
-        },
-        {
-            "x": 4.211236123155136,
-            "y": 4.953011857528808,
-            "heading": -0.5720541720789936,
-            "angularVelocity": 0.000003046116576982166,
-            "velocityX": 3.2195611298433677,
-            "velocityY": -1.9862150334199096,
-            "timestamp": 1.2948961956596485
-        },
-        {
-            "x": 4.269217408443118,
-            "y": 4.916014310130808,
-            "heading": -0.5720541167402081,
-            "angularVelocity": 0.000003043677217870583,
-            "velocityX": 3.1890168160017764,
-            "velocityY": -2.034894539109796,
-            "timestamp": 1.3130777508640736
-        },
-        {
-            "x": 4.327172181408397,
-            "y": 4.8789752463128,
-            "heading": -0.5720540614037476,
-            "angularVelocity": 0.000003043549349895456,
-            "velocityX": 3.187558617163689,
-            "velocityY": -2.0371779752347874,
-            "timestamp": 1.3312593060684987
-        },
-        {
-            "x": 4.385108415981365,
-            "y": 4.841907191568928,
-            "heading": -0.5720540060672449,
-            "angularVelocity": 0.0000030435516711711228,
-            "velocityX": 3.186538990839725,
-            "velocityY": -2.0387724992190503,
-            "timestamp": 1.3494408612729238
-        },
-        {
-            "x": 4.443034277698237,
-            "y": 4.804822929374826,
-            "heading": -0.5720539507307192,
-            "angularVelocity": 0.000003043552934036031,
-            "velocityX": 3.1859684755020403,
-            "velocityY": -2.0396639218758956,
-            "timestamp": 1.367622416477349
-        },
-        {
-            "x": 4.500957639641057,
-            "y": 4.767734762799275,
-            "heading": -0.5720538953941279,
-            "angularVelocity": 0.0000030435565398491907,
-            "velocityX": 3.1858309859372986,
-            "velocityY": -2.039878665966735,
-            "timestamp": 1.385803971681774
-        },
-        {
-            "x": 4.558883202891902,
-            "y": 4.7306500344023386,
-            "heading": -0.5720538400575734,
-            "angularVelocity": 0.000003043554516316428,
-            "velocityX": 3.1859520596302042,
-            "velocityY": -2.03968956340728,
-            "timestamp": 1.4039855268861992
-        },
-        {
-            "x": 4.616809360440233,
-            "y": 4.693566234312182,
-            "heading": -0.572053784721022,
-            "angularVelocity": 0.000003043554346667464,
-            "velocityX": 3.1859847464654854,
-            "velocityY": -2.039638505794633,
-            "timestamp": 1.4221670820906243
-        },
-        {
-            "x": 4.674735447300298,
-            "y": 4.656482323806888,
-            "heading": -0.5720537293844703,
-            "angularVelocity": 0.00000304355436453364,
-            "velocityX": 3.185980858554995,
-            "velocityY": -2.039644578714451,
-            "timestamp": 1.4403486372950494
-        },
-        {
-            "x": 4.732661331177625,
-            "y": 4.619398096238942,
-            "heading": -0.5720536740479176,
-            "angularVelocity": 0.0000030435544255368,
-            "velocityX": 3.1859696943429383,
-            "velocityY": -2.0396620174125277,
-            "timestamp": 1.4585301924994745
-        },
-        {
-            "x": 4.790587117727679,
-            "y": 4.582313716645222,
-            "heading": -0.5720536187113643,
-            "angularVelocity": 0.0000030435544545803464,
-            "velocityX": 3.1859643412654055,
-            "velocityY": -2.039670378951063,
-            "timestamp": 1.4767117477038996
-        },
-        {
-            "x": 4.848512993160408,
-            "y": 4.5452294758867104,
-            "heading": -0.5720535633748114,
-            "angularVelocity": 0.0000030435544295946876,
-            "velocityX": 3.1859692298833355,
-            "velocityY": -2.0396627429041696,
-            "timestamp": 1.4948933029083247
-        },
-        {
-            "x": 4.906438819485911,
-            "y": 4.508145158422396,
-            "heading": -0.5720535080382583,
-            "angularVelocity": 0.0000030435544442083865,
-            "velocityX": 3.1859665289468815,
-            "velocityY": -2.039666961784329,
-            "timestamp": 1.5130748581127498
-        },
-        {
-            "x": 4.9643646433421065,
-            "y": 4.471060837101057,
-            "heading": -0.5720534527017052,
-            "angularVelocity": 0.0000030435544458805193,
-            "velocityX": 3.185966393132936,
-            "velocityY": -2.039667173923745,
-            "timestamp": 1.5312564133171749
-        },
-        {
-            "x": 5.022290480818567,
-            "y": 4.433976537054754,
-            "heading": -0.572053397365152,
-            "angularVelocity": 0.0000030435544415454932,
-            "velocityX": 3.185967142258403,
-            "velocityY": -2.039666003779759,
-            "timestamp": 1.5494379685216
-        },
-        {
-            "x": 5.080216347467466,
-            "y": 4.396892282576102,
-            "heading": -0.572053342028599,
-            "angularVelocity": 0.000003043554433808661,
-            "velocityX": 3.1859687467658464,
-            "velocityY": -2.039663497522643,
-            "timestamp": 1.567619523726025
-        },
-        {
-            "x": 5.138142382220086,
-            "y": 4.359808290677983,
-            "heading": -0.5720532866920471,
-            "angularVelocity": 0.000003043554377469469,
-            "velocityX": 3.185977992604117,
-            "velocityY": -2.039649055384386,
-            "timestamp": 1.5858010789304502
-        },
-        {
-            "x": 5.196068138494407,
-            "y": 4.322723863792993,
-            "heading": -0.5720532313554916,
-            "angularVelocity": 0.000003043554576021048,
-            "velocityX": 3.1859626760759787,
-            "velocityY": -2.0396729800088242,
-            "timestamp": 1.6039826341348753
-        },
-        {
-            "x": 5.253993690845546,
-            "y": 4.285639118381674,
-            "heading": -0.5720531760189423,
-            "angularVelocity": 0.0000030435542278177066,
-            "velocityX": 3.1859514601384813,
-            "velocityY": -2.0396904992104243,
-            "timestamp": 1.6221641893393004
-        },
-        {
-            "x": 5.3119198680466875,
-            "y": 4.248555348991913,
-            "heading": -0.5720531206823879,
-            "angularVelocity": 0.000003043554518420827,
-            "velocityX": 3.1859858273860655,
-            "velocityY": -2.0396368172479113,
-            "timestamp": 1.6403457445437255
-        },
-        {
-            "x": 5.369845451022558,
-            "y": 4.211470651416335,
-            "heading": -0.5720530653458327,
-            "angularVelocity": 0.0000030435545594837867,
-            "velocityX": 3.1859531445232334,
-            "velocityY": -2.039687868206526,
-            "timestamp": 1.6585272997481506
-        },
-        {
-            "x": 5.427769955136907,
-            "y": 4.17438426873464,
-            "heading": -0.5720530100092767,
-            "angularVelocity": 0.0000030435545998300077,
-            "velocityX": 3.185893806281465,
-            "velocityY": -2.039780550383344,
-            "timestamp": 1.6767088549525757
-        },
-        {
-            "x": 5.485698211873678,
-            "y": 4.137303747848898,
-            "heading": -0.5720529546727399,
-            "angularVelocity": 0.000003043553546723704,
-            "velocityX": 3.1861002035013715,
-            "velocityY": -2.0394581469404574,
-            "timestamp": 1.6948904101570008
-        },
-        {
-            "x": 5.5436372749681775,
-            "y": 4.100240114388813,
-            "heading": -0.5720528993360909,
-            "angularVelocity": 0.0000030435597092212927,
-            "velocityX": 3.186694561772871,
-            "velocityY": -2.0385293250953618,
-            "timestamp": 1.713071965361426
-        },
-        {
-            "x": 5.601833960036281,
-            "y": 4.063582321171135,
-            "heading": -0.5720528439855975,
-            "angularVelocity": 0.0000030443211669066955,
-            "velocityX": 3.2008639752640304,
-            "velocityY": -2.0162077889097265,
-            "timestamp": 1.731253520565851
-        },
-        {
-            "x": 5.661045954546234,
-            "y": 4.0285882123488195,
-            "heading": -0.5720527885077281,
-            "angularVelocity": 0.000003051326955110969,
-            "velocityX": 3.2567068022627845,
-            "velocityY": -1.9247038236737246,
-            "timestamp": 1.7494350757702761
-        },
-        {
-            "x": 5.721517200862813,
-            "y": 3.9958181607132874,
-            "heading": -0.5720527326566293,
-            "angularVelocity": 0.0000030718548612359243,
-            "velocityX": 3.3259666533847816,
-            "velocityY": -1.8023789091228484,
-            "timestamp": 1.7676166309747012
-        },
-        {
-            "x": 5.7832255363464355,
-            "y": 3.9654417037963863,
-            "heading": -0.572052675208486,
-            "angularVelocity": 0.000003159693577578397,
-            "velocityX": 3.3940075416988695,
-            "velocityY": -1.670729295450284,
-            "timestamp": 1.7857981861791263
-        },
-        {
-            "x": 5.945861586676733,
-            "y": 3.9048707731215235,
-            "heading": -0.5720526214657033,
-            "angularVelocity": 0.0000011714587578241761,
-            "velocityX": 3.5450606755342,
-            "velocityY": -1.3202953710429959,
-            "timestamp": 1.831674989206145
-        },
-        {
-            "x": 6.113092819895856,
-            "y": 3.858469939627616,
-            "heading": -0.5720525698257588,
-            "angularVelocity": 0.0000011256221281562495,
-            "velocityX": 3.6452242132183756,
-            "velocityY": -1.0114225585101817,
-            "timestamp": 1.8775517922331635
-        },
-        {
-            "x": 6.280428909416468,
-            "y": 3.8124486851902244,
-            "heading": -0.5720525182264563,
-            "angularVelocity": 0.0000011247362300949913,
-            "velocityX": 3.6475098193329205,
-            "velocityY": -1.0031486808299002,
-            "timestamp": 1.923428595260182
-        },
-        {
-            "x": 6.444900242470224,
-            "y": 3.7570546132296903,
-            "heading": -0.5720524653781475,
-            "angularVelocity": 0.000001151961455361842,
-            "velocityX": 3.585065266142057,
-            "velocityY": -1.2074527496557126,
-            "timestamp": 1.9693053982872004
-        },
-        {
-            "x": 6.602974533610246,
-            "y": 3.685417746961916,
-            "heading": -0.5720524086130027,
-            "angularVelocity": 0.0000012373387213911858,
-            "velocityX": 3.445625691199315,
-            "velocityY": -1.561505195243581,
-            "timestamp": 2.015182201314219
-        },
-        {
-            "x": 6.753006171084989,
-            "y": 3.5981834135738024,
-            "heading": -0.5720523449049553,
-            "angularVelocity": 0.0000013886766999770178,
-            "velocityX": 3.27031588025751,
-            "velocityY": -1.9014911160397099,
-            "timestamp": 2.0610590043412373
-        },
-        {
-            "x": 6.893461047058093,
-            "y": 3.4962434932531963,
-            "heading": -0.5720522700606003,
-            "angularVelocity": 0.0000016314204553722347,
-            "velocityX": 3.0615663408441725,
-            "velocityY": -2.2220362709370285,
-            "timestamp": 2.106935807368256
-        },
-        {
-            "x": 7.0229029918463715,
-            "y": 3.380640293435665,
-            "heading": -0.5720521774325067,
-            "angularVelocity": 0.000002019061650871652,
-            "velocityX": 2.8215118806773165,
-            "velocityY": -2.519861720737319,
-            "timestamp": 2.1528126103952743
-        },
-        {
-            "x": 7.140008449554443,
-            "y": 3.2525558471679688,
-            "heading": -0.5720520639012779,
-            "angularVelocity": 0.0000024746979161572956,
-            "velocityX": 2.5526071997441684,
-            "velocityY": -2.7919217952549285,
-            "timestamp": 2.1986894134222927
-        },
-        {
-            "x": 7.220415929770595,
-            "y": 3.149946383889573,
-            "heading": -0.5720519473102574,
-            "angularVelocity": 0.000003383341158681086,
-            "velocityX": 2.333335244140948,
-            "velocityY": -2.977611988413324,
-            "timestamp": 2.2331497338662096
-        },
-        {
-            "x": 7.2928033634314815,
-            "y": 3.0415299314010706,
-            "heading": -0.5720518591979534,
-            "angularVelocity": 0.000002556920622107498,
-            "velocityX": 2.100602453152653,
-            "velocityY": -3.1461243276865574,
-            "timestamp": 2.2676100543101265
-        },
-        {
-            "x": 7.356753135957797,
-            "y": 2.927931973099431,
-            "heading": -0.5720517877574303,
-            "angularVelocity": 0.000002073124165462267,
-            "velocityX": 1.855750953634494,
-            "velocityY": -3.2964858375736252,
-            "timestamp": 2.3020703747540434
-        },
-        {
-            "x": 7.411896317838347,
-            "y": 2.8098078848104033,
-            "heading": -0.5720517267503814,
-            "angularVelocity": 0.0000017703564026272917,
-            "velocityX": 1.6001935318717442,
-            "velocityY": -3.427829073187373,
-            "timestamp": 2.3365306951979603
-        },
-        {
-            "x": 7.457914797691639,
-            "y": 2.687839154921363,
-            "heading": -0.572051672451037,
-            "angularVelocity": 0.0000015757063075533039,
-            "velocityX": 1.3354048732140973,
-            "velocityY": -3.5393962771625227,
-            "timestamp": 2.370991015641877
-        },
-        {
-            "x": 7.494543133171347,
-            "y": 2.5627294544923394,
-            "heading": -0.5720516223974168,
-            "angularVelocity": 0.000001452500141635371,
-            "velocityX": 1.0629133742199746,
-            "velocityY": -3.630543733121263,
-            "timestamp": 2.405451336085794
-        },
-        {
-            "x": 7.521592976236692,
-            "y": 2.435205423722826,
-            "heading": -0.572051574793162,
-            "angularVelocity": 0.0000013814222897397843,
-            "velocityX": 0.7849562255098341,
-            "velocityY": -3.70060490229679,
-            "timestamp": 2.439911656529711
-        },
-        {
-            "x": 7.539069360193098,
-            "y": 2.3060208712744585,
-            "heading": -0.572051528188319,
-            "angularVelocity": 0.000001352420480159191,
-            "velocityX": 0.5071451376970121,
-            "velocityY": -3.748791386272309,
-            "timestamp": 2.474371976973628
-        },
-        {
-            "x": 7.551504596507269,
-            "y": 2.176254012981795,
-            "heading": -0.5720514816592456,
-            "angularVelocity": 0.0000013502217280486013,
-            "velocityX": 0.3608566651205708,
-            "velocityY": -3.7656892513183533,
-            "timestamp": 2.5088322974175448
-        },
-        {
-            "x": 7.5639699142109125,
-            "y": 2.0464900408376128,
-            "heading": -0.5720514351306527,
-            "angularVelocity": 0.0000013502077822692335,
-            "velocityX": 0.36172959342888805,
-            "velocityY": -3.765605498514506,
-            "timestamp": 2.5432926178614617
-        },
-        {
-            "x": 7.576441327813496,
-            "y": 1.916726654418564,
-            "heading": -0.5720513886022516,
-            "angularVelocity": 0.00000135020222116771,
-            "velocityX": 0.36190648960876737,
-            "velocityY": -3.7655885014252446,
-            "timestamp": 2.5777529383053785
-        },
-        {
-            "x": 7.58884833483585,
-            "y": 1.786957094038731,
-            "heading": -0.5720513420716767,
-            "angularVelocity": 0.000001350265295572762,
-            "velocityX": 0.36003748258495927,
-            "velocityY": -3.765767662871606,
-            "timestamp": 2.6122132587492954
-        },
-        {
-            "x": 7.601019357724818,
-            "y": 1.6571651890693204,
-            "heading": -0.5720512933827582,
-            "angularVelocity": 0.0000014128980208836598,
-            "velocityX": 0.3531894866947264,
-            "velocityY": -3.7664160778959355,
-            "timestamp": 2.6466735791932123
-        },
-        {
-            "x": 7.610550391987499,
-            "y": 1.5345810963054864,
-            "heading": -0.5537181215211259,
-            "angularVelocity": 0.5320081654919462,
-            "velocityX": 0.2765799661741953,
-            "velocityY": -3.5572534203020942,
-            "timestamp": 2.6811338996371292
-        },
-        {
-            "x": 7.620334086621719,
-            "y": 1.4202071871878734,
-            "heading": -0.5150322166693104,
-            "angularVelocity": 1.122621738667962,
-            "velocityX": 0.2839118878810709,
-            "velocityY": -3.319003063356803,
-            "timestamp": 2.715594220081046
-        },
-        {
-            "x": 7.630352820350644,
-            "y": 1.314046573370508,
-            "heading": -0.4556179078059868,
-            "angularVelocity": 1.7241368651811046,
-            "velocityX": 0.290732459821803,
-            "velocityY": -3.0806624096878554,
-            "timestamp": 2.750054540524963
-        },
-        {
-            "x": 7.63947235322732,
-            "y": 1.2175511973082003,
-            "heading": -0.3892109145347841,
-            "angularVelocity": 1.9270567544269157,
-            "velocityX": 0.2646386556821465,
-            "velocityY": -2.8001880080992603,
-            "timestamp": 2.78451486096888
-        },
-        {
-            "x": 7.647652812069227,
-            "y": 1.1309118710627388,
-            "heading": -0.32231407882554747,
-            "angularVelocity": 1.9412714347239703,
-            "velocityX": 0.23738777633239685,
-            "velocityY": -2.5141764536539966,
-            "timestamp": 2.818975181412797
-        },
-        {
-            "x": 7.654895533140534,
-            "y": 1.0540989766744202,
-            "heading": -0.25819483301023066,
-            "angularVelocity": 1.8606688791433752,
-            "velocityX": 0.21017567387602423,
-            "velocityY": -2.22902437930995,
-            "timestamp": 2.8534355018567137
-        },
-        {
-            "x": 7.66120815675186,
-            "y": 0.9870533783479266,
-            "heading": -0.19885832997172487,
-            "angularVelocity": 1.7218790270703905,
-            "velocityX": 0.18318528469800885,
-            "velocityY": -1.9455883596791401,
-            "timestamp": 2.8878958223006306
-        },
-        {
-            "x": 7.666600355589264,
-            "y": 0.929716153410391,
-            "heading": -0.14570825222338094,
-            "angularVelocity": 1.542355876662632,
-            "velocityX": 0.15647558606309142,
-            "velocityY": -1.663862210186161,
-            "timestamp": 2.9223561427445475
-        },
-        {
-            "x": 7.671080940668327,
-            "y": 0.8820338579186632,
-            "heading": -0.09980369912266952,
-            "angularVelocity": 1.3320988461333123,
-            "velocityX": 0.1300215732566484,
-            "velocityY": -1.383686944215496,
-            "timestamp": 2.9568164631884644
-        },
-        {
-            "x": 7.674656709905022,
-            "y": 0.8439593502971015,
-            "heading": -0.06197555143316066,
-            "angularVelocity": 1.0977305841102087,
-            "velocityX": 0.10376482837700403,
-            "velocityY": -1.1048796741031013,
-            "timestamp": 2.9912767836323813
-        },
-        {
-            "x": 7.67733258738128,
-            "y": 0.8154516156506164,
-            "heading": -0.032889080662387146,
-            "angularVelocity": 0.8440568861838474,
-            "velocityX": 0.07765097485389878,
-            "velocityY": -0.8272626104241091,
-            "timestamp": 3.025737104076298
-        },
-        {
-            "x": 7.679112422858347,
-            "y": 0.7964752743679322,
-            "heading": -0.013083588528854375,
-            "angularVelocity": 0.5747332548975859,
-            "velocityX": 0.05164883710108163,
-            "velocityY": -0.5506722235380279,
-            "timestamp": 3.060197424520215
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -0.003,
-            "angularVelocity": 0.2926144736603049,
-            "velocityX": 0.025756497043992084,
-            "velocityY": -0.27496187632245256,
-            "timestamp": 3.094657744964132
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -0.003,
-            "angularVelocity": 9.445490920898867e-33,
-            "velocityX": 0,
-            "velocityY": 3.8255947334103544e-33,
-            "timestamp": 3.129118065408049
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Amp Side To W1.traj b/src/main/deploy/choreo/Amp Side To W1.traj
deleted file mode 100644
index 59066a5b..00000000
--- a/src/main/deploy/choreo/Amp Side To W1.traj	
+++ /dev/null
@@ -1,121 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": -3.0520704892108665e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7541195079597249,
-            "y": 6.720987197392724,
-            "heading": 1.0162961519009226,
-            "angularVelocity": -0.3240021115694771,
-            "velocityX": 0.6030587810307456,
-            "velocityY": 0.013493760103527496,
-            "timestamp": 0.07315954820244353
-        },
-        {
-            "x": 0.8424136303815098,
-            "y": 6.722972492793459,
-            "heading": 0.9702543819568796,
-            "angularVelocity": -0.6293337107086672,
-            "velocityX": 1.2068707993857855,
-            "velocityY": 0.027136518055622637,
-            "timestamp": 0.14631909640488705
-        },
-        {
-            "x": 0.9749626751701804,
-            "y": 6.7259693430054925,
-            "heading": 0.9040114694947134,
-            "angularVelocity": -0.9054581949963141,
-            "velocityX": 1.8117805268819878,
-            "velocityY": 0.0409632137659495,
-            "timestamp": 0.21947864460733058
-        },
-        {
-            "x": 1.1518931657803464,
-            "y": 6.729992727527654,
-            "heading": 0.8214709979365993,
-            "angularVelocity": -1.1282255506783914,
-            "velocityX": 2.4184196725842533,
-            "velocityY": 0.0549946605880228,
-            "timestamp": 0.2926381928097741
-        },
-        {
-            "x": 1.3734034446931793,
-            "y": 6.735064446892539,
-            "heading": 0.7325177816386138,
-            "angularVelocity": -1.215879792639604,
-            "velocityX": 3.0277699132296103,
-            "velocityY": 0.06932409356680827,
-            "timestamp": 0.3657977410122176
-        },
-        {
-            "x": 1.6366130282089124,
-            "y": 6.741790657706068,
-            "heading": 0.6993734786052862,
-            "angularVelocity": -0.4530413848603092,
-            "velocityX": 3.597747525550208,
-            "velocityY": 0.09193893317775545,
-            "timestamp": 0.43895728921466115
-        },
-        {
-            "x": 1.858120509684905,
-            "y": 6.746838490569188,
-            "heading": 0.6098796286126051,
-            "angularVelocity": -1.2232695825988174,
-            "velocityX": 3.0277316757485147,
-            "velocityY": 0.0689975948068392,
-            "timestamp": 0.5121168374171047
-        },
-        {
-            "x": 2.035048195511774,
-            "y": 6.750890505964207,
-            "heading": 0.5269052635268205,
-            "angularVelocity": -1.1341563353588457,
-            "velocityX": 2.418381334686234,
-            "velocityY": 0.0553860089978706,
-            "timestamp": 0.5852763856195482
-        },
-        {
-            "x": 2.1675926440263233,
-            "y": 6.753939666061267,
-            "heading": 0.4602292179018529,
-            "angularVelocity": -0.9113785864349585,
-            "velocityX": 1.811717701533345,
-            "velocityY": 0.041678224811154865,
-            "timestamp": 0.6584359338219917
-        },
-        {
-            "x": 2.2558825150080364,
-            "y": 6.755978237986949,
-            "heading": 0.41386063697230147,
-            "angularVelocity": -0.6338008102679221,
-            "velocityX": 1.206812687489562,
-            "velocityY": 0.027864741865846625,
-            "timestamp": 0.7315954820244353
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": -0.3261452203924221,
-            "velocityX": 0.6030311295783809,
-            "velocityY": 0.013966215458624026,
-            "timestamp": 0.8047550302268788
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 9.371559495672325e-33,
-            "velocityX": 2.5226214414524116e-33,
-            "velocityY": -5.278780681595662e-33,
-            "timestamp": 0.8779145784293223
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Amp Side To W2.traj b/src/main/deploy/choreo/Amp Side To W2.traj
deleted file mode 100644
index fb2e2531..00000000
--- a/src/main/deploy/choreo/Amp Side To W2.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": 3.547724392998225e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.737435339105297,
-            "y": 6.6995140898970185,
-            "heading": 1.00766177496316,
-            "angularVelocity": -0.49661186995303425,
-            "velocityX": 0.4213191985755434,
-            "velocityY": -0.3145981609176562,
-            "timestamp": 0.06511770457661178
-        },
-        {
-            "x": 0.7924238483684921,
-            "y": 6.658398285939555,
-            "heading": 0.945783535120445,
-            "angularVelocity": -0.95025216636614,
-            "velocityX": 0.8444479058456402,
-            "velocityY": -0.6314074524707902,
-            "timestamp": 0.13023540915322357
-        },
-        {
-            "x": 0.8751312092590968,
-            "y": 6.596482141852294,
-            "heading": 0.8584519121155169,
-            "angularVelocity": -1.3411348506945964,
-            "velocityX": 1.2701209514118896,
-            "velocityY": -0.9508342545216032,
-            "timestamp": 0.19535311372983535
-        },
-        {
-            "x": 0.9857818913354864,
-            "y": 6.513571279138628,
-            "heading": 0.7519056782658311,
-            "angularVelocity": -1.6362099146835547,
-            "velocityX": 1.6992411325894867,
-            "velocityY": -1.273246089565702,
-            "timestamp": 0.26047081830644714
-        },
-        {
-            "x": 1.1246151480908613,
-            "y": 6.409448227792029,
-            "heading": 0.6367147770436085,
-            "angularVelocity": -1.7689644002530829,
-            "velocityX": 2.132035483407367,
-            "velocityY": -1.5989975694566836,
-            "timestamp": 0.32558852288305895
-        },
-        {
-            "x": 1.2913898404503776,
-            "y": 6.284148794801154,
-            "heading": 0.5378900995711877,
-            "angularVelocity": -1.5176314662036392,
-            "velocityX": 2.5611267080722016,
-            "velocityY": -1.9241991683453519,
-            "timestamp": 0.39070622745967076
-        },
-        {
-            "x": 1.479902092849603,
-            "y": 6.139172935751937,
-            "heading": 0.5154398140678585,
-            "angularVelocity": -0.3447646941687853,
-            "velocityX": 2.8949462150871557,
-            "velocityY": -2.2263662392867607,
-            "timestamp": 0.4558239320362826
-        },
-        {
-            "x": 1.6682099030328927,
-            "y": 5.994133301373275,
-            "heading": 0.4928532472983225,
-            "angularVelocity": -0.34685753922672635,
-            "velocityX": 2.8918066355017875,
-            "velocityY": -2.2273456246913312,
-            "timestamp": 0.5209416366128944
-        },
-        {
-            "x": 1.8350205910276742,
-            "y": 5.8693381796986746,
-            "heading": 0.39723540571375665,
-            "angularVelocity": -1.4683847074503305,
-            "velocityX": 2.561679486083952,
-            "velocityY": -1.9164545569596636,
-            "timestamp": 0.5860593411895062
-        },
-        {
-            "x": 1.9739536597530634,
-            "y": 5.765582600106914,
-            "heading": 0.2820330722411117,
-            "angularVelocity": -1.7691399631126123,
-            "velocityX": 2.1335682765342634,
-            "velocityY": -1.5933543767607987,
-            "timestamp": 0.651177045766118
-        },
-        {
-            "x": 2.08468090414567,
-            "y": 5.683000750382208,
-            "heading": 0.1756895135649168,
-            "angularVelocity": -1.6330974712273538,
-            "velocityX": 1.7004168852778658,
-            "velocityY": -1.2681935007022147,
-            "timestamp": 0.7162947503427298
-        },
-        {
-            "x": 2.167467858188685,
-            "y": 5.6213477608369935,
-            "heading": 0.08927498136102419,
-            "angularVelocity": -1.3270512645639152,
-            "velocityX": 1.2713432480657338,
-            "velocityY": -0.946793041094972,
-            "timestamp": 0.7814124549193416
-        },
-        {
-            "x": 2.2225251033432207,
-            "y": 5.5804064125777195,
-            "heading": 0.028526912579098198,
-            "angularVelocity": -0.9328963478811614,
-            "velocityX": 0.8455034696402836,
-            "velocityY": -0.628728370041152,
-            "timestamp": 0.8465301594959534
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": -0.4841527013902356,
-            "velocityX": 0.42192667624600166,
-            "velocityY": -0.31337733279145685,
-            "timestamp": 0.9116478640725653
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": -3.2592320489843035e-28,
-            "velocityX": -3.6408651267401044e-33,
-            "velocityY": 2.9208085569078165e-32,
-            "timestamp": 0.9767655686491771
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Amp Side To W3.traj b/src/main/deploy/choreo/Amp Side To W3.traj
deleted file mode 100644
index dd55da0b..00000000
--- a/src/main/deploy/choreo/Amp Side To W3.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 6.72,
-            "heading": 1.04,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7269019070910594,
-            "y": 6.695506477136938,
-            "heading": 1.0019343442516249,
-            "angularVelocity": -0.6189075555866055,
-            "velocityX": 0.2748072454505401,
-            "velocityY": -0.3982389391394925,
-            "timestamp": 0.06150459047522247
-        },
-        {
-            "x": 0.7606735605009036,
-            "y": 6.646281390208111,
-            "heading": 0.9275075561119867,
-            "angularVelocity": -1.2101013528351434,
-            "velocityX": 0.5490915905447641,
-            "velocityY": -0.8003481780543988,
-            "timestamp": 0.12300918095044494
-        },
-        {
-            "x": 0.8112865108119746,
-            "y": 6.571983401826325,
-            "heading": 0.8196848361236891,
-            "angularVelocity": -1.7530841056771946,
-            "velocityX": 0.8229133780097379,
-            "velocityY": -1.2080072041406207,
-            "timestamp": 0.1845137714256674
-        },
-        {
-            "x": 0.8787770707149783,
-            "y": 6.472119277485466,
-            "heading": 0.6843886163577808,
-            "angularVelocity": -2.199774337501089,
-            "velocityX": 1.0973255716610744,
-            "velocityY": -1.6236857049083067,
-            "timestamp": 0.24601836190088988
-        },
-        {
-            "x": 0.9632585614164454,
-            "y": 6.3460508217508655,
-            "heading": 0.5331826539280757,
-            "angularVelocity": -2.4584500321259677,
-            "velocityX": 1.373580249030372,
-            "velocityY": -2.0497405927024026,
-            "timestamp": 0.3075229523761124
-        },
-        {
-            "x": 1.0643372089754837,
-            "y": 6.193350092274746,
-            "heading": 0.39338059647753487,
-            "angularVelocity": -2.273034522632274,
-            "velocityX": 1.6434325759759127,
-            "velocityY": -2.4827533733053033,
-            "timestamp": 0.3690275428513349
-        },
-        {
-            "x": 1.1776993188507112,
-            "y": 6.018028259717655,
-            "heading": 0.3265019863738018,
-            "angularVelocity": -1.087375911082207,
-            "velocityX": 1.843148763357691,
-            "velocityY": -2.85054873469518,
-            "timestamp": 0.4305321333265574
-        },
-        {
-            "x": 1.3085730997897271,
-            "y": 5.825657422218107,
-            "heading": 0.3265019166878518,
-            "angularVelocity": -0.0000011330203075824657,
-            "velocityX": 2.1278701301448275,
-            "velocityY": -3.1277476366100623,
-            "timestamp": 0.4920367238017799
-        },
-        {
-            "x": 1.4394483094166801,
-            "y": 5.633287556689,
-            "heading": 0.32650184700859936,
-            "angularVelocity": -0.00000113291141146818,
-            "velocityX": 2.1278933591091382,
-            "velocityY": -3.127731833392264,
-            "timestamp": 0.5535413142770024
-        },
-        {
-            "x": 1.5703235190623746,
-            "y": 5.4409176911726425,
-            "heading": 0.32650177732934843,
-            "angularVelocity": -0.0000011329113877746253,
-            "velocityX": 2.127893359413853,
-            "velocityY": -3.1277318331849595,
-            "timestamp": 0.6150459047522249
-        },
-        {
-            "x": 1.7011987286842072,
-            "y": 5.24854782564005,
-            "heading": 0.32650170765009895,
-            "angularVelocity": -0.0000011329113643360664,
-            "velocityX": 2.127893359025885,
-            "velocityY": -3.1277318334489093,
-            "timestamp": 0.6765504952274474
-        },
-        {
-            "x": 1.8320721192807423,
-            "y": 5.056176722584032,
-            "heading": 0.32650163796198384,
-            "angularVelocity": -0.000001133055510645955,
-            "velocityX": 2.1278637835863385,
-            "velocityY": -3.1277519542791743,
-            "timestamp": 0.7380550857026699
-        },
-        {
-            "x": 1.9454000953072241,
-            "y": 4.881259770298511,
-            "heading": 0.26077643177319376,
-            "angularVelocity": -1.0686227756490447,
-            "velocityX": 1.8425937828516514,
-            "velocityY": -2.843965807007333,
-            "timestamp": 0.7995596761778924
-        },
-        {
-            "x": 2.046443192812682,
-            "y": 4.7300582879948045,
-            "heading": 0.13106133632258532,
-            "angularVelocity": -2.1090311218780498,
-            "velocityX": 1.6428545694676804,
-            "velocityY": -2.4583771899858244,
-            "timestamp": 0.8610642666531149
-        },
-        {
-            "x": 2.131123897414369,
-            "y": 4.604963233646066,
-            "heading": -0.022718028385538325,
-            "angularVelocity": -2.50029084853552,
-            "velocityX": 1.376819257674782,
-            "velocityY": -2.0339141092100017,
-            "timestamp": 0.9225688571283374
-        },
-        {
-            "x": 2.198697305455373,
-            "y": 4.506052899677577,
-            "heading": -0.16200255999945734,
-            "angularVelocity": -2.264620096446796,
-            "velocityX": 1.0986725953118257,
-            "velocityY": -1.6081780758842066,
-            "timestamp": 0.9840734476035599
-        },
-        {
-            "x": 2.249342546290196,
-            "y": 4.432626729162034,
-            "heading": -0.2740984297313893,
-            "angularVelocity": -1.822561029442031,
-            "velocityX": 0.8234383879887088,
-            "velocityY": -1.1938323619132651,
-            "timestamp": 1.0455780380787822
-        },
-        {
-            "x": 2.283112085041758,
-            "y": 4.38410205128396,
-            "heading": -0.35252590091377134,
-            "angularVelocity": -1.2751482544051254,
-            "velocityX": 0.549057208423595,
-            "velocityY": -0.788960263016835,
-            "timestamp": 1.1070826285540047
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.393,
-            "angularVelocity": -0.6580663129938888,
-            "velocityX": 0.2745797480766085,
-            "velocityY": -0.39187402269867344,
-            "timestamp": 1.1685872190292272
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.393,
-            "angularVelocity": 0,
-            "velocityX": 1.1335977805694889e-32,
-            "velocityY": 0,
-            "timestamp": 1.2300918095044497
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C1 To C2.traj b/src/main/deploy/choreo/C1 To C2.traj
deleted file mode 100644
index 50686485..00000000
--- a/src/main/deploy/choreo/C1 To C2.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -5.382102664363315e-41,
-            "angularVelocity": -2.8652892976895567e-45,
-            "velocityX": 1.592735858156421e-40,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 7.383499994424153,
-            "heading": 1.659495816830368e-21,
-            "angularVelocity": 1.658316243120447e-20,
-            "velocityX": 2.780383744489643e-33,
-            "velocityY": -0.8344065481228213,
-            "timestamp": 0.10007112931184184
-        },
-        {
-            "x": 7.68,
-            "y": 7.216499986370153,
-            "heading": 4.9512387638524226e-21,
-            "angularVelocity": 3.289403167370527e-20,
-            "velocityX": 3.618624574631574e-33,
-            "velocityY": -1.668813065290736,
-            "timestamp": 0.20014225862368368
-        },
-        {
-            "x": 7.68,
-            "y": 6.9659999820333836,
-            "heading": 9.801760970151702e-21,
-            "angularVelocity": 4.8470744390330394e-20,
-            "velocityX": 3.439292531132709e-33,
-            "velocityY": -2.503219520548835,
-            "timestamp": 0.30021338793552554
-        },
-        {
-            "x": 7.68,
-            "y": 6.632,
-            "heading": 1.506109654033251e-20,
-            "angularVelocity": 5.255597191401202e-20,
-            "velocityX": 4.639675238422859e-33,
-            "velocityY": -3.3376257900775017,
-            "timestamp": 0.40028451724736736
-        },
-        {
-            "x": 7.68,
-            "y": 6.298000017966616,
-            "heading": 5.3586032763760104e-21,
-            "angularVelocity": -9.69559666912617e-20,
-            "velocityX": 4.935439546802769e-33,
-            "velocityY": -3.3376257900775017,
-            "timestamp": 0.5003556465592092
-        },
-        {
-            "x": 7.68,
-            "y": 6.047500013629846,
-            "heading": 1.8760590350448333e-21,
-            "angularVelocity": -3.480068842301423e-20,
-            "velocityX": 4.701730454770122e-33,
-            "velocityY": -2.5032195205488352,
-            "timestamp": 0.600426775871051
-        },
-        {
-            "x": 7.68,
-            "y": 5.880500005575846,
-            "heading": 3.168722428707802e-22,
-            "angularVelocity": -1.5580785203142817e-20,
-            "velocityX": 3.49539235617723e-33,
-            "velocityY": -1.668813065290736,
-            "timestamp": 0.7004979051828928
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 5.881496870004905e-41,
-            "angularVelocity": -3.1664700918333573e-21,
-            "velocityX": 3.364378386644386e-33,
-            "velocityY": -0.8344065481228213,
-            "timestamp": 0.8005690344947346
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 9.145026397391945e-42,
-            "angularVelocity": -4.963805231001231e-40,
-            "velocityX": -4.870998572760777e-42,
-            "velocityY": 3.489748854003666e-39,
-            "timestamp": 0.9006401638065764
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C1 To C3.traj b/src/main/deploy/choreo/C1 To C3.traj
deleted file mode 100644
index 688fbdaa..00000000
--- a/src/main/deploy/choreo/C1 To C3.traj	
+++ /dev/null
@@ -1,139 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 1.0127320738607452e-39,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 7.390554627994461,
-            "heading": -1.4011773737891353e-18,
-            "angularVelocity": -1.4633626501290335e-17,
-            "velocityX": 9.653222160408549e-34,
-            "velocityY": -0.7983807350946673,
-            "timestamp": 0.09575052183150974
-        },
-        {
-            "x": 7.68,
-            "y": 7.237663887017311,
-            "heading": -4.2066729097937526e-18,
-            "angularVelocity": -2.93000547907741e-17,
-            "velocityX": 5.229262005564601e-34,
-            "velocityY": -1.5967614385035824,
-            "timestamp": 0.19150104366301948
-        },
-        {
-            "x": 7.68,
-            "y": 7.0083277831364,
-            "heading": -8.423669065336014e-18,
-            "angularVelocity": -4.4041495279144365e-17,
-            "velocityX": -1.080014219031917e-33,
-            "velocityY": -2.3951420785410225,
-            "timestamp": 0.28725156549452924
-        },
-        {
-            "x": 7.68,
-            "y": 6.702546334555254,
-            "heading": -1.4072329853209785e-17,
-            "angularVelocity": -5.899352482411367e-17,
-            "velocityX": -1.5994734307756055e-33,
-            "velocityY": -3.1935225284643685,
-            "timestamp": 0.38300208732603896
-        },
-        {
-            "x": 7.68,
-            "y": 6.340327800733179,
-            "heading": -3.9014262815434015e-17,
-            "angularVelocity": -2.604887418380216e-16,
-            "velocityX": 4.89416509473128e-33,
-            "velocityY": -3.7829405719528495,
-            "timestamp": 0.4787526091575487
-        },
-        {
-            "x": 7.68,
-            "y": 5.978109266911059,
-            "heading": -3.149786356993182e-17,
-            "angularVelocity": 7.849982539759592e-17,
-            "velocityX": -6.0692119772378415e-33,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.5745031309890585
-        },
-        {
-            "x": 7.68,
-            "y": 5.61589073308894,
-            "heading": -3.435394827038493e-17,
-            "angularVelocity": -2.98283982773363e-17,
-            "velocityX": 7.480170031140856e-33,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.6702536528205683
-        },
-        {
-            "x": 7.68,
-            "y": 5.253672199266821,
-            "heading": -8.183196831726821e-18,
-            "angularVelocity": 2.7332228522691796e-16,
-            "velocityX": -1.1202872015905622e-32,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.766004174652078
-        },
-        {
-            "x": 7.68,
-            "y": 4.891453665444746,
-            "heading": 1.4014689490668664e-17,
-            "angularVelocity": 2.318304474774222e-16,
-            "velocityX": 5.155427490211361e-33,
-            "velocityY": -3.7829405719528495,
-            "timestamp": 0.8617546964835878
-        },
-        {
-            "x": 7.68,
-            "y": 4.5856722168636,
-            "heading": 8.395813027039913e-18,
-            "angularVelocity": -5.868246309956073e-17,
-            "velocityX": -4.168254505442575e-33,
-            "velocityY": -3.1935225284643685,
-            "timestamp": 0.9575052183150976
-        },
-        {
-            "x": 7.68,
-            "y": 4.356336112982689,
-            "heading": 4.1945848336017725e-18,
-            "angularVelocity": -4.387681773072387e-17,
-            "velocityX": 2.1988846402360483e-33,
-            "velocityY": -2.3951420785410225,
-            "timestamp": 1.0532557401466074
-        },
-        {
-            "x": 7.68,
-            "y": 4.203445372005539,
-            "heading": 1.3975677775871127e-18,
-            "angularVelocity": -2.9211507184809557e-17,
-            "velocityX": 3.5559383909284404e-33,
-            "velocityY": -1.5967614385035824,
-            "timestamp": 1.1490062619781172
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -1.4595928576161535e-17,
-            "velocityX": -6.471812078129625e-34,
-            "velocityY": -0.7983807350946673,
-            "timestamp": 1.244756783809627
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 8.546727720642512e-43,
-            "velocityY": 6.233759417908357e-39,
-            "timestamp": 1.3405073056411367
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C1 To C4.traj b/src/main/deploy/choreo/C1 To C4.traj
deleted file mode 100644
index d59fe62f..00000000
--- a/src/main/deploy/choreo/C1 To C4.traj	
+++ /dev/null
@@ -1,175 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 3.5626996813915863e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 7.385155846048237,
-            "heading": 1.781123128781041e-18,
-            "angularVelocity": 1.7977717932873702e-17,
-            "velocityX": 2.837123063321612e-19,
-            "velocityY": -0.8260917454276266,
-            "timestamp": 0.099073927811004
-        },
-        {
-            "x": 7.68,
-            "y": 7.22146754125807,
-            "heading": 5.344190150368438e-18,
-            "angularVelocity": 3.5963720225105045e-17,
-            "velocityX": 5.673750850451309e-19,
-            "velocityY": -1.65218345943065,
-            "timestamp": 0.198147855622008
-        },
-        {
-            "x": 7.68,
-            "y": 6.975935091856216,
-            "heading": 1.0690981078424305e-17,
-            "angularVelocity": 5.396768903659916e-17,
-            "velocityX": 8.509388117154945e-19,
-            "velocityY": -2.47827511058448,
-            "timestamp": 0.29722178343301203
-        },
-        {
-            "x": 7.68,
-            "y": 6.648558516522808,
-            "heading": 1.7824905328944302e-17,
-            "angularVelocity": 7.200607069613354e-17,
-            "velocityX": 1.134205375948437e-18,
-            "velocityY": -3.304366573190878,
-            "timestamp": 0.396295711244016
-        },
-        {
-            "x": 7.68,
-            "y": 6.273767735073314,
-            "heading": 3.2118763957552995e-17,
-            "angularVelocity": 1.4427467391699638e-16,
-            "velocityX": 1.2981920193437937e-18,
-            "velocityY": -3.782940575087071,
-            "timestamp": 0.49536963905502
-        },
-        {
-            "x": 7.68,
-            "y": 5.898976953623795,
-            "heading": 3.194072142377472e-17,
-            "angularVelocity": -1.7970674806511035e-18,
-            "velocityX": 1.2970624694923187e-18,
-            "velocityY": -3.7829405750873115,
-            "timestamp": 0.5944435668660241
-        },
-        {
-            "x": 7.68,
-            "y": 5.524186172174278,
-            "heading": 3.523836351949418e-17,
-            "angularVelocity": 3.3284660945411024e-17,
-            "velocityX": 1.2988038949897902e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.6935174946770281
-        },
-        {
-            "x": 7.68,
-            "y": 5.149395390724759,
-            "heading": 4.1591047407718544e-17,
-            "angularVelocity": 6.412064231794501e-17,
-            "velocityX": 1.299093645390414e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.792591422488032
-        },
-        {
-            "x": 7.68,
-            "y": 4.774604609275242,
-            "heading": 3.0405238206332753e-17,
-            "angularVelocity": -1.1290366142265185e-16,
-            "velocityX": 1.2929821768433727e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.891665350299036
-        },
-        {
-            "x": 7.68,
-            "y": 4.3998138278257235,
-            "heading": 3.8390344063969795e-17,
-            "angularVelocity": 8.059744913789504e-17,
-            "velocityX": 1.304793755296235e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.99073927811004
-        },
-        {
-            "x": 7.68,
-            "y": 4.025023046376205,
-            "heading": 1.0979594277826173e-17,
-            "angularVelocity": -2.766696586253564e-16,
-            "velocityX": 1.307186742413441e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 1.0898132059210441
-        },
-        {
-            "x": 7.68,
-            "y": 3.650232264926687,
-            "heading": -2.14668342522941e-17,
-            "angularVelocity": -3.2749714528425564e-16,
-            "velocityX": 1.3056923664932481e-18,
-            "velocityY": -3.7829405750873115,
-            "timestamp": 1.1888871337320481
-        },
-        {
-            "x": 7.68,
-            "y": 3.2754414834771928,
-            "heading": -1.7689075007663427e-17,
-            "angularVelocity": 3.813071035032691e-17,
-            "velocityX": 1.3012288375156972e-18,
-            "velocityY": -3.782940575087071,
-            "timestamp": 1.2879610615430521
-        },
-        {
-            "x": 7.68,
-            "y": 2.948064908143785,
-            "heading": -1.0623620375651477e-17,
-            "angularVelocity": 7.131497447110055e-17,
-            "velocityX": 1.136256754825581e-18,
-            "velocityY": -3.304366573190878,
-            "timestamp": 1.3870349893540561
-        },
-        {
-            "x": 7.68,
-            "y": 2.70253245874193,
-            "heading": -5.31498515459649e-18,
-            "angularVelocity": 5.35825654501949e-17,
-            "velocityX": 8.521926280380639e-19,
-            "velocityY": -2.4782751105844802,
-            "timestamp": 1.48610891716506
-        },
-        {
-            "x": 7.68,
-            "y": 2.538844153951763,
-            "heading": -1.7723662552803497e-18,
-            "angularVelocity": 3.575732766130284e-17,
-            "velocityX": 5.681284366395801e-19,
-            "velocityY": -1.6521834594306501,
-            "timestamp": 1.585182844976064
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 1.7889330669293064e-17,
-            "velocityX": 2.8406422370407584e-19,
-            "velocityY": -0.8260917454276266,
-            "timestamp": 1.684256772787068
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 1.0461754707969188e-34,
-            "velocityY": 0,
-            "timestamp": 1.783330700598072
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C1 To C5.traj b/src/main/deploy/choreo/C1 To C5.traj
deleted file mode 100644
index 6f64999b..00000000
--- a/src/main/deploy/choreo/C1 To C5.traj	
+++ /dev/null
@@ -1,220 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -8.452820466081124e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 7.3890447295128565,
-            "heading": -6.111572142469688e-20,
-            "angularVelocity": -6.320692328199756e-19,
-            "velocityX": -1.9797111149811777e-21,
-            "velocityY": -0.8062267267095136,
-            "timestamp": 0.09669149868710691
-        },
-        {
-            "x": 7.68,
-            "y": 7.2331341916331855,
-            "heading": -1.8226742072400974e-19,
-            "angularVelocity": -1.2529715733337834e-18,
-            "velocityX": -3.753564400558873e-21,
-            "velocityY": -1.6124534214139752,
-            "timestamp": 0.19338299737421383
-        },
-        {
-            "x": 7.68,
-            "y": 6.999268392550218,
-            "heading": -3.611632829024713e-19,
-            "angularVelocity": -1.850171571559818e-18,
-            "velocityX": -5.1157697830729356e-21,
-            "velocityY": -2.418680052108357,
-            "timestamp": 0.29007449606132074
-        },
-        {
-            "x": 7.68,
-            "y": 6.687447350831621,
-            "heading": -5.912144198792796e-19,
-            "angularVelocity": -2.3792281637439373e-18,
-            "velocityX": -5.242727749103024e-21,
-            "velocityY": -3.224906490772764,
-            "timestamp": 0.38676599474842766
-        },
-        {
-            "x": 7.68,
-            "y": 6.321669157855709,
-            "heading": -4.100561450064452e-17,
-            "angularVelocity": -4.179726307846966e-16,
-            "velocityX": -1.166132844762965e-20,
-            "velocityY": -3.782940568121389,
-            "timestamp": 0.48345749343553457
-        },
-        {
-            "x": 7.68,
-            "y": 5.955890964879757,
-            "heading": -6.040572418063157e-17,
-            "angularVelocity": -2.0063924898701218e-16,
-            "velocityX": -1.9392305823460385e-22,
-            "velocityY": -3.782940568121787,
-            "timestamp": 0.5801489921226415
-        },
-        {
-            "x": 7.68,
-            "y": 5.590112771903805,
-            "heading": -8.327536807485113e-17,
-            "angularVelocity": -2.365217646302656e-16,
-            "velocityX": 3.675624932403311e-22,
-            "velocityY": -3.782940568121787,
-            "timestamp": 0.6768404908097484
-        },
-        {
-            "x": 7.68,
-            "y": 5.224334578927854,
-            "heading": -7.575439540555723e-17,
-            "angularVelocity": 7.778318436801094e-17,
-            "velocityX": -2.1057162419585226e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 0.7735319894968553
-        },
-        {
-            "x": 7.68,
-            "y": 4.858556385951902,
-            "heading": -4.8431591576069396e-17,
-            "angularVelocity": 2.82577105541659e-16,
-            "velocityX": 1.016892835365518e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 0.8702234881839622
-        },
-        {
-            "x": 7.68,
-            "y": 4.49277819297595,
-            "heading": -7.892411277004063e-17,
-            "angularVelocity": -3.1535886409915893e-16,
-            "velocityX": 1.448731313682566e-20,
-            "velocityY": -3.782940568121787,
-            "timestamp": 0.9669149868710691
-        },
-        {
-            "x": 7.68,
-            "y": 4.126999999999999,
-            "heading": -8.668140475632877e-17,
-            "angularVelocity": -8.022723912254832e-17,
-            "velocityX": 2.655349226445793e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 1.063606485558176
-        },
-        {
-            "x": 7.68,
-            "y": 3.7612218070240475,
-            "heading": -4.236036305527668e-17,
-            "angularVelocity": 4.583757859030679e-16,
-            "velocityX": 7.500561292003624e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 1.160297984245283
-        },
-        {
-            "x": 7.68,
-            "y": 3.395443614048096,
-            "heading": -2.060715967072721e-17,
-            "angularVelocity": 2.249753461257506e-16,
-            "velocityX": 1.3311646441840309e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 1.25698948293239
-        },
-        {
-            "x": 7.68,
-            "y": 3.0296654210721448,
-            "heading": -1.7378554757194933e-17,
-            "angularVelocity": 3.3390783650741015e-17,
-            "velocityX": 1.5677048108756238e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 1.353680981619497
-        },
-        {
-            "x": 7.68,
-            "y": 2.663887228096194,
-            "heading": -2.7058170522872628e-18,
-            "angularVelocity": 1.5174796030816139e-16,
-            "velocityX": 2.708497475030379e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 1.450372480306604
-        },
-        {
-            "x": 7.68,
-            "y": 2.2981090351202424,
-            "heading": -9.881529062229235e-18,
-            "angularVelocity": -7.421243963841227e-17,
-            "velocityX": 2.3345387345088647e-21,
-            "velocityY": -3.782940568121787,
-            "timestamp": 1.547063978993711
-        },
-        {
-            "x": 7.68,
-            "y": 1.9323308421442913,
-            "heading": -6.5997498705085955e-18,
-            "angularVelocity": 3.39407211211041e-17,
-            "velocityX": -6.072343928076645e-20,
-            "velocityY": -3.782940568121787,
-            "timestamp": 1.643755477680818
-        },
-        {
-            "x": 7.68,
-            "y": 1.5665526491683786,
-            "heading": 6.487893960505722e-19,
-            "angularVelocity": 7.496563157826373e-17,
-            "velocityX": 5.228734565126193e-20,
-            "velocityY": -3.782940568121389,
-            "timestamp": 1.7404469763679251
-        },
-        {
-            "x": 7.68,
-            "y": 1.254731607449781,
-            "heading": 3.9009642634108723e-19,
-            "angularVelocity": -2.6754468948553036e-18,
-            "velocityX": 1.8077268140623735e-21,
-            "velocityY": -3.224906490772764,
-            "timestamp": 1.8371384750550321
-        },
-        {
-            "x": 7.68,
-            "y": 1.020865808366814,
-            "heading": 1.9475770164363563e-19,
-            "angularVelocity": -2.020226466917104e-18,
-            "velocityX": 1.3557598034014801e-21,
-            "velocityY": -2.418680052108357,
-            "timestamp": 1.9338299737421392
-        },
-        {
-            "x": 7.68,
-            "y": 0.8649552704871435,
-            "heading": 6.488190339479676e-20,
-            "angularVelocity": -1.3431976959152128e-18,
-            "velocityX": 9.038296351119003e-22,
-            "velocityY": -1.6124534214139752,
-            "timestamp": 2.030521472429246
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -6.710197305720325e-19,
-            "velocityX": 4.519117474728098e-22,
-            "velocityY": -0.8062267267095136,
-            "timestamp": 2.127212971116353
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 1.0656726462878492e-32,
-            "velocityY": 0,
-            "timestamp": 2.2239044698034602
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C1 To S1.traj b/src/main/deploy/choreo/C1 To S1.traj
deleted file mode 100644
index ce3947a6..00000000
--- a/src/main/deploy/choreo/C1 To S1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -7.649625564275916e-33,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.617797764168564,
-            "y": 7.446201717117849,
-            "heading": 0.013442479376395878,
-            "angularVelocity": 0.15144225686963014,
-            "velocityX": -0.7007670767707749,
-            "velocityY": -0.23431234749607086,
-            "timestamp": 0.08876306820531243
-        },
-        {
-            "x": 7.493368305965553,
-            "y": 7.404602586388856,
-            "heading": 0.03870227011836747,
-            "angularVelocity": 0.28457545746724416,
-            "velocityX": -1.401815650572125,
-            "velocityY": -0.4686535917478179,
-            "timestamp": 0.17752613641062487
-        },
-        {
-            "x": 7.30667604944407,
-            "y": 7.3422008161122045,
-            "heading": 0.0730276629215222,
-            "angularVelocity": 0.38670804756608035,
-            "velocityX": -2.1032650210965835,
-            "velocityY": -0.7030150212056434,
-            "timestamp": 0.2662892046159373
-        },
-        {
-            "x": 7.057674574154849,
-            "y": 7.259002273186937,
-            "heading": 0.11075765125141449,
-            "angularVelocity": 0.42506403948785515,
-            "velocityX": -2.8052373619302045,
-            "velocityY": -0.9373103544914424,
-            "timestamp": 0.35505227282124974
-        },
-        {
-            "x": 6.746464870184761,
-            "y": 7.155111739606062,
-            "heading": 0.13339737013954753,
-            "angularVelocity": 0.25505786749611997,
-            "velocityX": -3.5060719538216505,
-            "velocityY": -1.170425219424191,
-            "timestamp": 0.44381534102656217
-        },
-        {
-            "x": 6.428047828205506,
-            "y": 7.048517057389171,
-            "heading": 0.13339738106725169,
-            "angularVelocity": 1.2311157711164836e-7,
-            "velocityX": -3.5872694400628884,
-            "velocityY": -1.2008900139638257,
-            "timestamp": 0.5325784092318746
-        },
-        {
-            "x": 6.109630786224927,
-            "y": 6.941922375176235,
-            "heading": 0.1333973919949535,
-            "angularVelocity": 1.23111550991103e-7,
-            "velocityX": -3.587269440077816,
-            "velocityY": -1.200890013919235,
-            "timestamp": 0.621341477437187
-        },
-        {
-            "x": 5.798381397539494,
-            "y": 6.838018370585249,
-            "heading": 0.15626352052401435,
-            "angularVelocity": 0.2576085864583032,
-            "velocityX": -3.5065190397107555,
-            "velocityY": -1.1705769831075976,
-            "timestamp": 0.7101045456424995
-        },
-        {
-            "x": 5.5493631968567065,
-            "y": 6.754814060290535,
-            "heading": 0.1928079538664876,
-            "angularVelocity": 0.4117076401528937,
-            "velocityX": -2.8054257893230465,
-            "velocityY": -0.9373753293689796,
-            "timestamp": 0.7988676138478119
-        },
-        {
-            "x": 5.362653942099421,
-            "y": 6.692406067264648,
-            "heading": 0.22585011955614995,
-            "angularVelocity": 0.37225127926004886,
-            "velocityX": -2.1034565223155943,
-            "velocityY": -0.703085126367409,
-            "timestamp": 0.8876306820531243
-        },
-        {
-            "x": 5.238210383284219,
-            "y": 6.650801525067523,
-            "heading": 0.25010704549903756,
-            "angularVelocity": 0.2732772360607878,
-            "velocityX": -1.4019745073178984,
-            "velocityY": -0.4687145570597535,
-            "timestamp": 0.9763937502584368
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": 0.1452513388947291,
-            "velocityX": -0.7008588655398115,
-            "velocityY": -0.2343488737838604,
-            "timestamp": 1.0651568184637492
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": -2.2743517080702824e-31,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 1.1539198866690616
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C1 To S2.traj b/src/main/deploy/choreo/C1 To S2.traj
deleted file mode 100644
index f95228f2..00000000
--- a/src/main/deploy/choreo/C1 To S2.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -6.443666081756986e-29,
-            "angularVelocity": 1.4558689567742243e-28,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.593939912495806,
-            "y": 7.445359803187238,
-            "heading": -0.04618112566514864,
-            "angularVelocity": -0.44533178373248467,
-            "velocityX": -0.8298908206418381,
-            "velocityY": -0.2086797865609081,
-            "timestamp": 0.10370049332228601
-        },
-        {
-            "x": 7.421570715628637,
-            "y": 7.402025139488172,
-            "heading": -0.13159426902714605,
-            "angularVelocity": -0.823652237569843,
-            "velocityX": -1.662182997832739,
-            "velocityY": -0.41788290788915955,
-            "timestamp": 0.20740098664457202
-        },
-        {
-            "x": 7.162505124622784,
-            "y": 7.336932458231965,
-            "heading": -0.24235041158328322,
-            "angularVelocity": -1.0680387239038096,
-            "velocityX": -2.4982098224038025,
-            "velocityY": -0.627698858229146,
-            "timestamp": 0.311101479966858
-        },
-        {
-            "x": 6.816624735331939,
-            "y": 7.250238713519174,
-            "heading": -0.3315039987658666,
-            "angularVelocity": -0.8597219196007858,
-            "velocityX": -3.3353784365894983,
-            "velocityY": -0.8360012757447058,
-            "timestamp": 0.41480197328914403
-        },
-        {
-            "x": 6.436297385949381,
-            "y": 7.154089090014683,
-            "heading": -0.3315040257643058,
-            "angularVelocity": -2.603501519346576e-7,
-            "velocityX": -3.6675558350581356,
-            "velocityY": -0.927185786914993,
-            "timestamp": 0.5185024666114301
-        },
-        {
-            "x": 6.055970043958053,
-            "y": 7.05793943727301,
-            "heading": -0.33150405276269507,
-            "angularVelocity": -2.60349670609762e-7,
-            "velocityX": -3.6675557637833287,
-            "velocityY": -0.9271860688537472,
-            "timestamp": 0.6222029599337161
-        },
-        {
-            "x": 5.675642817406181,
-            "y": 6.961789327902759,
-            "heading": -0.33150407976108204,
-            "angularVelocity": -2.6034964813703677e-7,
-            "velocityX": -3.6675546505826837,
-            "velocityY": -0.9271904721942118,
-            "timestamp": 0.7259034532560021
-        },
-        {
-            "x": 5.306931756724127,
-            "y": 6.827827408155551,
-            "heading": -0.331504107569482,
-            "angularVelocity": -2.6816073038858223e-7,
-            "velocityX": -3.5555381548297533,
-            "velocityY": -1.2918156457642633,
-            "timestamp": 0.8296039465782882
-        },
-        {
-            "x": 4.97827117667767,
-            "y": 6.613640897314209,
-            "heading": -0.3315041419146958,
-            "angularVelocity": -3.311962425493206e-7,
-            "velocityX": -3.1693251354651193,
-            "velocityY": -2.065433866121419,
-            "timestamp": 0.9333044399005742
-        },
-        {
-            "x": 4.706831845168329,
-            "y": 6.330419557819514,
-            "heading": -0.3315041418010325,
-            "angularVelocity": 1.0960729474265148e-9,
-            "velocityX": -2.6175317282796997,
-            "velocityY": -2.7311474653691747,
-            "timestamp": 1.0370049332228601
-        },
-        {
-            "x": 4.518922560808715,
-            "y": 6.028421366315536,
-            "heading": -0.2325817741817243,
-            "angularVelocity": 0.9539237900428917,
-            "velocityX": -1.8120384806234937,
-            "velocityY": -2.912215572257732,
-            "timestamp": 1.1407054265451462
-        },
-        {
-            "x": 4.388098262013575,
-            "y": 5.795527878516759,
-            "heading": -0.12496957799500227,
-            "angularVelocity": 1.0377211596504115,
-            "velocityX": -1.2615590784949995,
-            "velocityY": -2.245828157007679,
-            "timestamp": 1.2444059198674322
-        },
-        {
-            "x": 4.304052100915554,
-            "y": 5.638778154553602,
-            "heading": -0.0435889655527213,
-            "angularVelocity": 0.7847659141732163,
-            "velocityX": -0.8104702148022851,
-            "velocityY": -1.5115619891604988,
-            "timestamp": 1.3481064131897182
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 7.875266725539043e-28,
-            "angularVelocity": 0.42033518025077415,
-            "velocityX": -0.39587179964482383,
-            "velocityY": -0.7596700076322139,
-            "timestamp": 1.4518069065120043
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -1.3117957600781845e-26,
-            "velocityX": -2.8963805377195657e-28,
-            "velocityY": 0,
-            "timestamp": 1.5555073998342903
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C1 To S3.traj b/src/main/deploy/choreo/C1 To S3.traj
deleted file mode 100644
index f3e004b9..00000000
--- a/src/main/deploy/choreo/C1 To S3.traj	
+++ /dev/null
@@ -1,265 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 3.237981257278863e-31,
-            "velocityX": 3.6994687797817426e-32,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.663124715083084,
-            "y": 7.408544235319849,
-            "heading": 0.027485198347684953,
-            "angularVelocity": 0.3204918925418427,
-            "velocityX": -0.19677471240300645,
-            "velocityY": -0.6816250119548423,
-            "timestamp": 0.08575941852911824
-        },
-        {
-            "x": 7.629347887601235,
-            "y": 7.291532761279695,
-            "heading": 0.07929730291499706,
-            "angularVelocity": 0.6041564350126787,
-            "velocityX": -0.3938556028149893,
-            "velocityY": -1.3644154315298311,
-            "timestamp": 0.17151883705823648
-        },
-        {
-            "x": 7.578636110956943,
-            "y": 7.115823334781324,
-            "heading": 0.15015093083574435,
-            "angularVelocity": 0.8261906288076145,
-            "velocityX": -0.5913260317532774,
-            "velocityY": -2.048864480566788,
-            "timestamp": 0.25727825558735473
-        },
-        {
-            "x": 7.510961813450331,
-            "y": 6.881225838812792,
-            "heading": 0.22908991381513497,
-            "angularVelocity": 0.9204701283345157,
-            "velocityX": -0.7891179612374983,
-            "velocityY": -2.7355303941208287,
-            "timestamp": 0.34303767411647296
-        },
-        {
-            "x": 7.426596388399054,
-            "y": 6.588221528517866,
-            "heading": 0.2780284674480044,
-            "angularVelocity": 0.5706493172671544,
-            "velocityX": -0.9837453016618997,
-            "velocityY": -3.4165846191627605,
-            "timestamp": 0.4287970926455912
-        },
-        {
-            "x": 7.336364964950643,
-            "y": 6.276599275391096,
-            "heading": 0.2780284905344079,
-            "angularVelocity": 2.691996271350766e-7,
-            "velocityX": -1.0521459333096546,
-            "velocityY": -3.6336796409243712,
-            "timestamp": 0.5145565111747095
-        },
-        {
-            "x": 7.246133490581996,
-            "y": 5.964977037008454,
-            "heading": 0.27802851362075814,
-            "angularVelocity": 2.6919900573505555e-7,
-            "velocityX": -1.0521465270664079,
-            "velocityY": -3.633679469000075,
-            "timestamp": 0.6003159297038277
-        },
-        {
-            "x": 7.155901958204915,
-            "y": 5.653354815422387,
-            "heading": 0.2780285367070952,
-            "angularVelocity": 2.6919885218329885e-7,
-            "velocityX": -1.0521472034753196,
-            "velocityY": -3.633679273143183,
-            "timestamp": 0.686075348232946
-        },
-        {
-            "x": 7.06567006903972,
-            "y": 5.341732697146042,
-            "heading": 0.27802855979342844,
-            "angularVelocity": 2.6919880800587287e-7,
-            "velocityX": -1.052151363812688,
-            "velocityY": -3.6336780684973866,
-            "timestamp": 0.7718347667620643
-        },
-        {
-            "x": 6.941702537617662,
-            "y": 5.041929112902265,
-            "heading": 0.2780285835706332,
-            "angularVelocity": 2.772547333115093e-7,
-            "velocityX": -1.445526725207054,
-            "velocityY": -3.495867735413615,
-            "timestamp": 0.8575941852911826
-        },
-        {
-            "x": 6.763533066668612,
-            "y": 4.770809627708675,
-            "heading": 0.2780286115416881,
-            "angularVelocity": 3.2615723612466423e-7,
-            "velocityX": -2.0775498948673006,
-            "velocityY": -3.1613960290733045,
-            "timestamp": 0.9433536038203009
-        },
-        {
-            "x": 6.537527544905599,
-            "y": 4.538061625407243,
-            "heading": 0.2780286466069666,
-            "angularVelocity": 4.0887962009830865e-7,
-            "velocityX": -2.6353434484432303,
-            "velocityY": -2.713964323608448,
-            "timestamp": 1.0291130223494191
-        },
-        {
-            "x": 6.274440682462666,
-            "y": 4.354088811409382,
-            "heading": 0.26884633764727317,
-            "angularVelocity": -0.10707055991260324,
-            "velocityX": -3.0677314160380695,
-            "velocityY": -2.145219932145364,
-            "timestamp": 1.1148724408785373
-        },
-        {
-            "x": 5.982323590323029,
-            "y": 4.22411616098336,
-            "heading": 0.2547621599051439,
-            "angularVelocity": -0.16422893232822805,
-            "velocityX": -3.40623918806603,
-            "velocityY": -1.5155495764222278,
-            "timestamp": 1.2006318594076555
-        },
-        {
-            "x": 5.6751302181547745,
-            "y": 4.153390532297334,
-            "heading": 0.22346642432676106,
-            "angularVelocity": -0.3649247641266729,
-            "velocityX": -3.582036555716069,
-            "velocityY": -0.8246980902979518,
-            "timestamp": 1.2863912779367737
-        },
-        {
-            "x": 5.366531848907471,
-            "y": 4.105798244476318,
-            "heading": 0.18319904977282217,
-            "angularVelocity": -0.4695388010388101,
-            "velocityX": -3.5984195618411468,
-            "velocityY": -0.5549511486584576,
-            "timestamp": 1.3721506964658918
-        },
-        {
-            "x": 5.15360011207683,
-            "y": 4.041515222282991,
-            "heading": 0.15622154454817708,
-            "angularVelocity": -0.44121073940112593,
-            "velocityX": -3.482448368249538,
-            "velocityY": -1.051333676582719,
-            "timestamp": 1.4332949588681712
-        },
-        {
-            "x": 4.961585993321137,
-            "y": 3.955708543808847,
-            "heading": 0.09940090208560654,
-            "angularVelocity": -0.9292882149552651,
-            "velocityX": -3.1403456548775974,
-            "velocityY": -1.403348001969601,
-            "timestamp": 1.4944392212704505
-        },
-        {
-            "x": 4.792433392014809,
-            "y": 3.849704139398821,
-            "heading": 0.01980990286006859,
-            "angularVelocity": -1.301692032882722,
-            "velocityX": -2.766450925410501,
-            "velocityY": -1.7336770490843985,
-            "timestamp": 1.5555834836727298
-        },
-        {
-            "x": 4.64716063510186,
-            "y": 3.7242164869664776,
-            "heading": -0.07646566712737501,
-            "angularVelocity": -1.5745642551712946,
-            "velocityX": -2.3759016987918233,
-            "velocityY": -2.0523209783239644,
-            "timestamp": 1.6167277460750091
-        },
-        {
-            "x": 4.526373969972362,
-            "y": 3.5797426645365276,
-            "heading": -0.18496768957846801,
-            "angularVelocity": -1.7745250034621045,
-            "velocityX": -1.975437438999914,
-            "velocityY": -2.362835313629761,
-            "timestamp": 1.6778720084772885
-        },
-        {
-            "x": 4.435318488505818,
-            "y": 3.4263170797911884,
-            "heading": -0.3033986089581413,
-            "angularVelocity": -1.936909772506439,
-            "velocityX": -1.489190937777188,
-            "velocityY": -2.5092392763841707,
-            "timestamp": 1.7390162708795678
-        },
-        {
-            "x": 4.37042278768838,
-            "y": 3.2883178140362905,
-            "heading": -0.39683380701899557,
-            "angularVelocity": -1.5281106417823267,
-            "velocityX": -1.0613538910728308,
-            "velocityY": -2.2569454652502787,
-            "timestamp": 1.800160533281847
-        },
-        {
-            "x": 4.3241368483863285,
-            "y": 3.1745112953889034,
-            "heading": -0.4672868225779202,
-            "angularVelocity": -1.1522424638210744,
-            "velocityX": -0.7569956277749857,
-            "velocityY": -1.8612787884925825,
-            "timestamp": 1.8613047956841264
-        },
-        {
-            "x": 4.292271472981756,
-            "y": 3.08778416242541,
-            "heading": -0.5177338397629753,
-            "angularVelocity": -0.8250490757931602,
-            "velocityX": -0.5211507041318677,
-            "velocityY": -1.418401818193496,
-            "timestamp": 1.9224490580864058
-        },
-        {
-            "x": 4.272405555044274,
-            "y": 3.0293931910282037,
-            "heading": -0.5502025950282511,
-            "angularVelocity": -0.5310188395381721,
-            "velocityX": -0.3249024055075056,
-            "velocityY": -0.9549705745576195,
-            "timestamp": 1.983593320488685
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -0.25836283489389134,
-            "velocityX": -0.15382563587721435,
-            "velocityY": -0.48071871134564137,
-            "timestamp": 2.0447375828909644
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 0,
-            "velocityX": 3.3381626772503296e-32,
-            "velocityY": -2.986960154995716e-30,
-            "timestamp": 2.105881845293244
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C1 To S4.traj b/src/main/deploy/choreo/C1 To S4.traj
deleted file mode 100644
index 6d9a6add..00000000
--- a/src/main/deploy/choreo/C1 To S4.traj	
+++ /dev/null
@@ -1,301 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 9.776523821034258e-32,
-            "velocityX": -4.560355515853987e-33,
-            "velocityY": 2.3850698853192112e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 7.67836355889096,
-            "y": 7.400720406813562,
-            "heading": -0.028945523101477338,
-            "angularVelocity": -0.3234212568317029,
-            "velocityX": -0.018284687354286195,
-            "velocityY": -0.7405714954779761,
-            "timestamp": 0.0894978993806123
-        },
-        {
-            "x": 7.675074058245281,
-            "y": 7.268033905727148,
-            "heading": -0.0827237859595526,
-            "angularVelocity": -0.6008885485609575,
-            "velocityX": -0.03675505982201489,
-            "velocityY": -1.4825655351097347,
-            "timestamp": 0.1789957987612246
-        },
-        {
-            "x": 7.670107822881543,
-            "y": 7.068767684711926,
-            "heading": -0.1543450175390465,
-            "angularVelocity": -0.8002560068466026,
-            "velocityX": -0.05548996566520156,
-            "velocityY": -2.226490480718353,
-            "timestamp": 0.2684936981418369
-        },
-        {
-            "x": 7.663417838093003,
-            "y": 6.802742204879413,
-            "heading": -0.22912917227010834,
-            "angularVelocity": -0.8355967597967521,
-            "velocityX": -0.07475018782384167,
-            "velocityY": -2.9724214945109684,
-            "timestamp": 0.3579915975224492
-        },
-        {
-            "x": 7.654753648982881,
-            "y": 6.471615180012679,
-            "heading": -0.2502157544796387,
-            "angularVelocity": -0.23560980040272358,
-            "velocityX": -0.0968088543986332,
-            "velocityY": -3.6998301318619107,
-            "timestamp": 0.4474894969030615
-        },
-        {
-            "x": 7.646619499365586,
-            "y": 6.133147678353669,
-            "heading": -0.250215779137968,
-            "angularVelocity": -2.7551852588992195e-7,
-            "velocityX": -0.09088648642693918,
-            "velocityY": -3.78184855735655,
-            "timestamp": 0.5369873962836738
-        },
-        {
-            "x": 7.638485287213415,
-            "y": 5.7946801781975115,
-            "heading": -0.25021580379628167,
-            "angularVelocity": -2.755183512776458e-7,
-            "velocityX": -0.09088718515702116,
-            "velocityY": -3.7818485405645093,
-            "timestamp": 0.626485295664286
-        },
-        {
-            "x": 7.630351002745582,
-            "y": 5.456212679779282,
-            "heading": -0.2502158284545949,
-            "angularVelocity": -2.7551834628402704e-7,
-            "velocityX": -0.09088799317222593,
-            "velocityY": -3.781848521145859,
-            "timestamp": 0.7159831950448983
-        },
-        {
-            "x": 7.622216641973054,
-            "y": 5.117745183194865,
-            "heading": -0.25021585311291117,
-            "angularVelocity": -2.7551838019723297e-7,
-            "velocityX": -0.09088884575864668,
-            "velocityY": -3.781848500655846,
-            "timestamp": 0.8054810944255105
-        },
-        {
-            "x": 7.614082202627753,
-            "y": 4.779277688498788,
-            "heading": -0.25021587777122717,
-            "angularVelocity": -2.755183772950637e-7,
-            "velocityX": -0.09088972368733664,
-            "velocityY": -3.7818484795565834,
-            "timestamp": 0.8949789938061228
-        },
-        {
-            "x": 7.605947683187911,
-            "y": 4.440810195727643,
-            "heading": -0.2502159024295383,
-            "angularVelocity": -2.755183223713384e-7,
-            "velocityX": -0.09089061861940764,
-            "velocityY": -3.7818484580484664,
-            "timestamp": 0.984476893186735
-        },
-        {
-            "x": 7.597813082423135,
-            "y": 4.1023427049110195,
-            "heading": -0.25021592708784146,
-            "angularVelocity": -2.755182337798815e-7,
-            "velocityX": -0.09089152729921718,
-            "velocityY": -3.781848436209727,
-            "timestamp": 1.0739747925673473
-        },
-        {
-            "x": 7.589678399478048,
-            "y": 3.7638752160694957,
-            "heading": -0.2502159517461411,
-            "angularVelocity": -2.755181940361221e-7,
-            "velocityX": -0.09089244553653594,
-            "velocityY": -3.781848414141052,
-            "timestamp": 1.1634726919479597
-        },
-        {
-            "x": 7.581543633206817,
-            "y": 3.4254077292306317,
-            "heading": -0.2502159764044425,
-            "angularVelocity": -2.7551821391136536e-7,
-            "velocityX": -0.09089337657676429,
-            "velocityY": -3.7818483917644334,
-            "timestamp": 1.252970591328572
-        },
-        {
-            "x": 7.573407384740196,
-            "y": 3.0869402780182864,
-            "heading": -0.2502160010627749,
-            "angularVelocity": -2.7551856112041726e-7,
-            "velocityX": -0.09090993780780313,
-            "velocityY": -3.7818479936934355,
-            "timestamp": 1.3424684907091844
-        },
-        {
-            "x": 7.507089727245593,
-            "y": 2.754933679576005,
-            "heading": -0.25021602759782946,
-            "angularVelocity": -2.964880153822211e-7,
-            "velocityX": -0.7409968049928212,
-            "velocityY": -3.709658000243552,
-            "timestamp": 1.4319663900897968
-        },
-        {
-            "x": 7.3768883522731565,
-            "y": 2.4424052627551904,
-            "heading": -0.2502160612558337,
-            "angularVelocity": -3.7607591242222915e-7,
-            "velocityX": -1.454798111168213,
-            "velocityY": -3.492019577931195,
-            "timestamp": 1.5214642894704091
-        },
-        {
-            "x": 7.1878697455519145,
-            "y": 2.1615167326419513,
-            "heading": -0.2502161148657548,
-            "angularVelocity": -5.990075908840489e-7,
-            "velocityX": -2.111989309574672,
-            "velocityY": -3.1384929932119427,
-            "timestamp": 1.6109621888510215
-        },
-        {
-            "x": 6.94738938516625,
-            "y": 1.9231986374023808,
-            "heading": -0.25021645217340494,
-            "angularVelocity": -0.000003768889018580663,
-            "velocityX": -2.686994466350113,
-            "velocityY": -2.6628345121941095,
-            "timestamp": 1.7004600882316339
-        },
-        {
-            "x": 6.667655944824219,
-            "y": 1.738860011100769,
-            "heading": -0.25893161706240975,
-            "angularVelocity": -0.09737842954215001,
-            "velocityX": -3.125586659329227,
-            "velocityY": -2.059697798242895,
-            "timestamp": 1.7899579876122462
-        },
-        {
-            "x": 6.480451878153193,
-            "y": 1.646736643588724,
-            "heading": -0.268441983388845,
-            "angularVelocity": -0.16928620019223647,
-            "velocityX": -3.3322654479868237,
-            "velocityY": -1.6398122111953808,
-            "timestamp": 1.846137205502623
-        },
-        {
-            "x": 6.284980922842322,
-            "y": 1.5795583069683912,
-            "heading": -0.28291661109989463,
-            "angularVelocity": -0.25765092955355146,
-            "velocityX": -3.47941752575298,
-            "velocityY": -1.1957862558966221,
-            "timestamp": 1.9023164233929999
-        },
-        {
-            "x": 6.086191677834544,
-            "y": 1.5383458560749974,
-            "heading": -0.3070406119029933,
-            "angularVelocity": -0.429411474723066,
-            "velocityX": -3.53848366838567,
-            "velocityY": -0.7335889042423714,
-            "timestamp": 1.9584956412833767
-        },
-        {
-            "x": 5.891572555778559,
-            "y": 1.5225106097486452,
-            "heading": -0.35143250880916643,
-            "angularVelocity": -0.7901836047765775,
-            "velocityX": -3.4642547433776456,
-            "velocityY": -0.2818701811985366,
-            "timestamp": 2.0146748591737533
-        },
-        {
-            "x": 5.713348199791213,
-            "y": 1.5270062964148978,
-            "heading": -0.407385332387855,
-            "angularVelocity": -0.9959701412692558,
-            "velocityX": -3.172425011952234,
-            "velocityY": 0.08002401662169539,
-            "timestamp": 2.07085407706413
-        },
-        {
-            "x": 5.558872974287929,
-            "y": 1.5428236796222032,
-            "heading": -0.4653888399418059,
-            "angularVelocity": -1.0324726781908529,
-            "velocityX": -2.7496862951120815,
-            "velocityY": 0.2815522145247918,
-            "timestamp": 2.1270332949545065
-        },
-        {
-            "x": 5.4303297125098124,
-            "y": 1.5629685460150702,
-            "heading": -0.519265179295127,
-            "angularVelocity": -0.9590083553396718,
-            "velocityX": -2.2880927610801525,
-            "velocityY": 0.35858217948453525,
-            "timestamp": 2.183212512844883
-        },
-        {
-            "x": 5.3280145107455015,
-            "y": 1.583083454211334,
-            "heading": -0.5654730488836437,
-            "angularVelocity": -0.8225082392332849,
-            "velocityX": -1.8212286608183241,
-            "velocityY": 0.3580489182942135,
-            "timestamp": 2.2393917307352598
-        },
-        {
-            "x": 5.251723840078425,
-            "y": 1.6004232571954151,
-            "heading": -0.6018676415285119,
-            "angularVelocity": -0.6478301765590008,
-            "velocityX": -1.3579874112157035,
-            "velocityY": 0.3086515554188953,
-            "timestamp": 2.2955709486256364
-        },
-        {
-            "x": 5.20115218978415,
-            "y": 1.613158164546165,
-            "heading": -0.6270407807035692,
-            "angularVelocity": -0.44808632302748147,
-            "velocityX": -0.9001843064628601,
-            "velocityY": 0.22668359989631479,
-            "timestamp": 2.351750166516013
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -0.2306763921441399,
-            "velocityX": -0.44771342016952703,
-            "velocityY": 0.12178587938311458,
-            "timestamp": 2.4079293844063896
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 3.4924263364045203e-31,
-            "velocityX": 0,
-            "velocityY": 4.738614736798697e-32,
-            "timestamp": 2.464108602296766
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C1 To W1.traj b/src/main/deploy/choreo/C1 To W1.traj
deleted file mode 100644
index 1a381a4d..00000000
--- a/src/main/deploy/choreo/C1 To W1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 3.136513796099116e-33,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.613270105003341,
-            "y": 7.458186274184093,
-            "heading": 0.022686687743237347,
-            "angularVelocity": 0.25194782208363986,
-            "velocityX": -0.7410712353675238,
-            "velocityY": -0.0978811472565877,
-            "timestamp": 0.09004518298914281
-        },
-        {
-            "x": 7.479732366880497,
-            "y": 7.440537508755927,
-            "heading": 0.06475240251532689,
-            "angularVelocity": 0.4671622998107697,
-            "velocityX": -1.483008126475186,
-            "velocityY": -0.19599899564082535,
-            "timestamp": 0.18009036597828562
-        },
-        {
-            "x": 7.2792817573683966,
-            "y": 7.414022951557291,
-            "heading": 0.12053408331695856,
-            "angularVelocity": 0.6194854510802376,
-            "velocityX": -2.226111412714547,
-            "velocityY": -0.2944583632178676,
-            "timestamp": 0.2701355489674284
-        },
-        {
-            "x": 7.01181645042657,
-            "y": 7.378594749595234,
-            "heading": 0.17807816149187164,
-            "angularVelocity": 0.6390578181383835,
-            "velocityX": -2.9703455316879728,
-            "velocityY": -0.39344916391950197,
-            "timestamp": 0.36018073195657124
-        },
-        {
-            "x": 6.678421151155383,
-            "y": 7.334256434894407,
-            "heading": 0.1920911587056688,
-            "angularVelocity": 0.15562184170903165,
-            "velocityX": -3.7025334193766524,
-            "velocityY": -0.49240073959507324,
-            "timestamp": 0.45022591494571407
-        },
-        {
-            "x": 6.340697514229505,
-            "y": 7.289811670135368,
-            "heading": 0.1920911781237931,
-            "angularVelocity": 2.1564867368321945e-7,
-            "velocityX": -3.7506019279965175,
-            "velocityY": -0.49358292452353475,
-            "timestamp": 0.5402710979348568
-        },
-        {
-            "x": 6.002973877037569,
-            "y": 7.245366907398008,
-            "heading": 0.19209119754191162,
-            "angularVelocity": 2.1564861007875057e-7,
-            "velocityX": -3.750601930951228,
-            "velocityY": -0.49358290207171196,
-            "timestamp": 0.6303162809239996
-        },
-        {
-            "x": 5.665250239845632,
-            "y": 7.200922144660658,
-            "heading": 0.19209121696003015,
-            "angularVelocity": 2.1564860966767442e-7,
-            "velocityX": -3.7506019309512406,
-            "velocityY": -0.493582902071613,
-            "timestamp": 0.7203614639131424
-        },
-        {
-            "x": 5.327526602653695,
-            "y": 7.156477381923307,
-            "heading": 0.19209123637814873,
-            "angularVelocity": 2.1564861026621434e-7,
-            "velocityX": -3.7506019309512415,
-            "velocityY": -0.493582902071613,
-            "timestamp": 0.8104066469022851
-        },
-        {
-            "x": 4.989802965461758,
-            "y": 7.112032619185957,
-            "heading": 0.19209125579626718,
-            "angularVelocity": 2.156486089426075e-7,
-            "velocityX": -3.7506019309512415,
-            "velocityY": -0.49358290207161315,
-            "timestamp": 0.9004518298914279
-        },
-        {
-            "x": 4.652079328269822,
-            "y": 7.067587856448606,
-            "heading": 0.19209127521438563,
-            "angularVelocity": 2.156486089612396e-7,
-            "velocityX": -3.750601930951241,
-            "velocityY": -0.4935829020716131,
-            "timestamp": 0.9904970128805707
-        },
-        {
-            "x": 4.314355691077885,
-            "y": 7.023143093711255,
-            "heading": 0.19209129463250418,
-            "angularVelocity": 2.1564860999442945e-7,
-            "velocityX": -3.7506019309512415,
-            "velocityY": -0.4935829020716131,
-            "timestamp": 1.0805421958697135
-        },
-        {
-            "x": 3.976632053885947,
-            "y": 6.978698330973905,
-            "heading": 0.1920913140506227,
-            "angularVelocity": 2.1564860984546372e-7,
-            "velocityX": -3.750601930951241,
-            "velocityY": -0.49358290207161315,
-            "timestamp": 1.1705873788588563
-        },
-        {
-            "x": 3.638908416694012,
-            "y": 6.934253568236545,
-            "heading": 0.19209133346874124,
-            "angularVelocity": 2.1564860987115903e-7,
-            "velocityX": -3.750601930951227,
-            "velocityY": -0.4935829020717237,
-            "timestamp": 1.2606325618479992
-        },
-        {
-            "x": 3.3011847797995815,
-            "y": 6.8898088032385445,
-            "heading": 0.19209135288686577,
-            "angularVelocity": 2.1564867640744468e-7,
-            "velocityX": -3.7506019276472724,
-            "velocityY": -0.4935829271773418,
-            "timestamp": 1.350677744837142
-        },
-        {
-            "x": 2.967975868183031,
-            "y": 6.845440652800829,
-            "heading": 0.2065131865620181,
-            "angularVelocity": 0.16016218965195775,
-            "velocityX": -3.7004634846122446,
-            "velocityY": -0.492732081438091,
-            "timestamp": 1.440722927826285
-        },
-        {
-            "x": 2.7005740636525677,
-            "y": 6.810001078107782,
-            "heading": 0.266815731322117,
-            "angularVelocity": 0.6696920674520688,
-            "velocityX": -2.9696403033875236,
-            "velocityY": -0.39357546419024264,
-            "timestamp": 1.5307681108154279
-        },
-        {
-            "x": 2.5001864407535797,
-            "y": 6.78347721885534,
-            "heading": 0.32524318999567287,
-            "angularVelocity": 0.6488682318586739,
-            "velocityX": -2.2254119126300136,
-            "velocityY": -0.29456166750908425,
-            "timestamp": 1.6208132938045707
-        },
-        {
-            "x": 2.366700230698382,
-            "y": 6.765819830273621,
-            "heading": 0.3692681816828941,
-            "angularVelocity": 0.48892111966199864,
-            "velocityX": -1.4824358796771253,
-            "velocityY": -0.19609476038099796,
-            "timestamp": 1.7108584767937136
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.26355455704906927,
-            "velocityX": -0.740741797442122,
-            "velocityY": -0.09794894053004785,
-            "timestamp": 1.8009036597828565
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0,
-            "velocityX": 6.927431243507319e-35,
-            "velocityY": 0,
-            "timestamp": 1.8909488427719994
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C1 To W2.traj b/src/main/deploy/choreo/C1 To W2.traj
deleted file mode 100644
index 28a53b93..00000000
--- a/src/main/deploy/choreo/C1 To W2.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -2.4374604803646449e-32,
-            "velocityY": 2.350988701644575e-38,
-            "timestamp": 0
-        },
-        {
-            "x": 7.602872461914855,
-            "y": 7.439913035887961,
-            "heading": 5.3325436298288445e-18,
-            "angularVelocity": 5.3856284215509377e-17,
-            "velocityX": -0.7789533333445217,
-            "velocityY": -0.2735661154121556,
-            "timestamp": 0.09901432445765257
-        },
-        {
-            "x": 7.448617388685645,
-            "y": 7.385739108696781,
-            "heading": -4.4022234029758545e-18,
-            "angularVelocity": -9.831675377471393e-17,
-            "velocityX": -1.557906636985473,
-            "velocityY": -0.5471322203925048,
-            "timestamp": 0.19802864891530514
-        },
-        {
-            "x": 7.217234786194527,
-            "y": 7.304478220492258,
-            "heading": -7.053417683545123e-18,
-            "angularVelocity": -2.6775864730895626e-17,
-            "velocityX": -2.3368598812192953,
-            "velocityY": -0.8206983045092451,
-            "timestamp": 0.2970429733729577
-        },
-        {
-            "x": 6.9087246720879545,
-            "y": 7.196130377471774,
-            "heading": 2.6673179344101e-18,
-            "angularVelocity": 9.817504407722252e-17,
-            "velocityX": -3.1158129472318725,
-            "velocityY": -1.0942643260352094,
-            "timestamp": 0.3960572978306103
-        },
-        {
-            "x": 6.5553201862538,
-            "y": 7.072015763386003,
-            "heading": 8.41895708879283e-18,
-            "angularVelocity": 5.808896021749495e-17,
-            "velocityX": -3.5692258445423475,
-            "velocityY": -1.2535015995473784,
-            "timestamp": 0.49507162228826285
-        },
-        {
-            "x": 6.2019157004196215,
-            "y": 6.9479011493002245,
-            "heading": -3.69857396739816e-19,
-            "angularVelocity": -8.876306064099344e-17,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.2535015995474599,
-            "timestamp": 0.5940859467459154
-        },
-        {
-            "x": 5.848511214585444,
-            "y": 6.823786535214446,
-            "heading": -8.032367976884792e-18,
-            "angularVelocity": -7.738789939855791e-17,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.2535015995474599,
-            "timestamp": 0.693100271203568
-        },
-        {
-            "x": 5.495106728751266,
-            "y": 6.699671921128667,
-            "heading": -3.490709714785344e-17,
-            "angularVelocity": -2.714226382747745e-16,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.2535015995474599,
-            "timestamp": 0.7921145956612206
-        },
-        {
-            "x": 5.141702242917089,
-            "y": 6.575557307042889,
-            "heading": -1.2741337636064699e-17,
-            "angularVelocity": 2.238641694845762e-16,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.2535015995474599,
-            "timestamp": 0.8911289201188731
-        },
-        {
-            "x": 4.788297757082911,
-            "y": 6.4514426929571105,
-            "heading": -2.2645871045802615e-17,
-            "angularVelocity": -1.0003131833516608e-16,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.2535015995474597,
-            "timestamp": 0.9901432445765257
-        },
-        {
-            "x": 4.4348932712487334,
-            "y": 6.327328078871332,
-            "heading": -7.505924377434873e-18,
-            "angularVelocity": 1.529066299375997e-16,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.25350159954746,
-            "timestamp": 1.0891575690341782
-        },
-        {
-            "x": 4.081488785414556,
-            "y": 6.203213464785553,
-            "heading": -3.3249179069580955e-17,
-            "angularVelocity": -2.5999525658031657e-16,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.25350159954746,
-            "timestamp": 1.1881718934918308
-        },
-        {
-            "x": 3.728084299580378,
-            "y": 6.079098850699775,
-            "heading": -3.31250701341244e-17,
-            "angularVelocity": 1.2534442479544336e-18,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.2535015995474597,
-            "timestamp": 1.2871862179494835
-        },
-        {
-            "x": 3.3746798137461993,
-            "y": 5.9549842366139965,
-            "heading": -3.474935359347948e-17,
-            "angularVelocity": -1.6404530025230332e-17,
-            "velocityX": -3.569225844542579,
-            "velocityY": -1.2535015995474599,
-            "timestamp": 1.3862005424071362
-        },
-        {
-            "x": 3.021275327912045,
-            "y": 5.830869622528225,
-            "heading": -2.4652412430964293e-17,
-            "angularVelocity": 1.0197454982216833e-16,
-            "velocityX": -3.5692258445423475,
-            "velocityY": -1.2535015995473784,
-            "timestamp": 1.4852148668647889
-        },
-        {
-            "x": 2.7127652138054734,
-            "y": 5.722521779507741,
-            "heading": 1.414690860074665e-18,
-            "angularVelocity": 2.632659789801892e-16,
-            "velocityX": -3.115812947231872,
-            "velocityY": -1.0942643260352094,
-            "timestamp": 1.5842291913224416
-        },
-        {
-            "x": 2.4813826113143547,
-            "y": 5.641260891303218,
-            "heading": 8.307803002216867e-18,
-            "angularVelocity": 6.961732115429119e-17,
-            "velocityX": -2.336859881219295,
-            "velocityY": -0.8206983045092451,
-            "timestamp": 1.6832435157800942
-        },
-        {
-            "x": 2.3271275380851444,
-            "y": 5.587086964112038,
-            "heading": -3.972937436005726e-18,
-            "angularVelocity": -1.2402993582700068e-16,
-            "velocityX": -1.557906636985473,
-            "velocityY": -0.5471322203925048,
-            "timestamp": 1.782257840237747
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 4.012487544797503e-17,
-            "velocityX": -0.7789533333445219,
-            "velocityY": -0.2735661154121556,
-            "timestamp": 1.8812721646953996
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -5.265297803216144e-35,
-            "velocityY": 0,
-            "timestamp": 1.9802864891530523
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C1 To W3.traj b/src/main/deploy/choreo/C1 To W3.traj
deleted file mode 100644
index 96b5b24b..00000000
--- a/src/main/deploy/choreo/C1 To W3.traj	
+++ /dev/null
@@ -1,274 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": -1.8418609272007183e-28,
-            "velocityX": -1.1316944174618986e-32,
-            "velocityY": -1.0903166764521522e-29,
-            "timestamp": 0
-        },
-        {
-            "x": 7.615058283165484,
-            "y": 7.4500526040017725,
-            "heading": -0.011528934363875379,
-            "angularVelocity": -0.12842736126317786,
-            "velocityX": -0.7234227436435784,
-            "velocityY": -0.18878668917675853,
-            "timestamp": 0.08977007898235294
-        },
-        {
-            "x": 7.485153650546927,
-            "y": 7.41615410854214,
-            "heading": -0.0328638632229319,
-            "angularVelocity": -0.23766191476385995,
-            "velocityX": -1.4470816344507456,
-            "velocityY": -0.3776146333375848,
-            "timestamp": 0.17954015796470588
-        },
-        {
-            "x": 7.290257872847477,
-            "y": 7.365300387326372,
-            "heading": -0.06109799257845879,
-            "angularVelocity": -0.3145160355888266,
-            "velocityX": -2.17105498746146,
-            "velocityY": -0.5664885426438214,
-            "timestamp": 0.2693102369470588
-        },
-        {
-            "x": 7.030342832843462,
-            "y": 7.297491054945299,
-            "heading": -0.0902650461777,
-            "angularVelocity": -0.32490840968750134,
-            "velocityX": -2.895341554228826,
-            "velocityY": -0.7553667452421856,
-            "timestamp": 0.35908031592941175
-        },
-        {
-            "x": 6.705594231461252,
-            "y": 7.212800171241254,
-            "heading": -0.10097705422378679,
-            "angularVelocity": -0.11932715407194204,
-            "velocityX": -3.617559492690666,
-            "velocityY": -0.9434199531077018,
-            "timestamp": 0.4488503949117647
-        },
-        {
-            "x": 6.377014064882571,
-            "y": 7.1270111914036525,
-            "heading": -0.10097706428057987,
-            "angularVelocity": -1.120283753063034e-7,
-            "velocityX": -3.6602414780458985,
-            "velocityY": -0.9556522708907337,
-            "timestamp": 0.5386204738941176
-        },
-        {
-            "x": 6.048433901032778,
-            "y": 7.0412222011141505,
-            "heading": -0.10097707433737516,
-            "angularVelocity": -1.1202840770590317e-7,
-            "velocityX": -3.660241447647229,
-            "velocityY": -0.9556523873204973,
-            "timestamp": 0.6283905528764705
-        },
-        {
-            "x": 5.719853740215168,
-            "y": 6.955433199211206,
-            "heading": -0.10097708439415136,
-            "angularVelocity": -1.1202819821797078e-7,
-            "velocityX": -3.6602414138700015,
-            "velocityY": -0.9556525166893554,
-            "timestamp": 0.7181606318588234
-        },
-        {
-            "x": 5.391273582926602,
-            "y": 6.86964418379178,
-            "heading": -0.100977094450907,
-            "angularVelocity": -1.1202797265919916e-7,
-            "velocityX": -3.660241374557919,
-            "velocityY": -0.9556526672572584,
-            "timestamp": 0.8079307108411763
-        },
-        {
-            "x": 5.062693429765362,
-            "y": 6.783855152564382,
-            "heading": -0.10097710450764096,
-            "angularVelocity": -1.1202773594839044e-7,
-            "velocityX": -3.6602413285812334,
-            "velocityY": -0.9556528433514181,
-            "timestamp": 0.8977007898235292
-        },
-        {
-            "x": 4.734113281444647,
-            "y": 6.698066102797359,
-            "heading": -0.10097711456435234,
-            "angularVelocity": -1.1202748993058552e-7,
-            "velocityX": -3.6602412746598207,
-            "velocityY": -0.9556530498750426,
-            "timestamp": 0.9874708688058821
-        },
-        {
-            "x": 4.4055331387963745,
-            "y": 6.612277031304317,
-            "heading": -0.10097712462104143,
-            "angularVelocity": -1.1202724880866421e-7,
-            "velocityX": -3.660241211471189,
-            "velocityY": -0.9556532918937597,
-            "timestamp": 1.077240947788235
-        },
-        {
-            "x": 4.076953002755506,
-            "y": 6.526487934504167,
-            "heading": -0.10097713467770983,
-            "angularVelocity": -1.1202702659530083e-7,
-            "velocityX": -3.6602411378675055,
-            "velocityY": -0.9556535738043043,
-            "timestamp": 1.167011026770588
-        },
-        {
-            "x": 3.748372874246301,
-            "y": 6.440698808855463,
-            "heading": -0.10097714473442117,
-            "angularVelocity": -1.1202751392033635e-7,
-            "velocityX": -3.6602410539684915,
-            "velocityY": -0.9556538951629009,
-            "timestamp": 1.256781105752941
-        },
-        {
-            "x": 3.4197928479820465,
-            "y": 6.354909291415878,
-            "heading": -0.10097715479114391,
-            "angularVelocity": -1.120276421337113e-7,
-            "velocityX": -3.6602399150000573,
-            "velocityY": -0.9556582595606669,
-            "timestamp": 1.346551184735294
-        },
-        {
-            "x": 3.1047619528127512,
-            "y": 6.228101525660344,
-            "heading": -0.10097716526412484,
-            "angularVelocity": -1.1666457370126234e-7,
-            "velocityX": -3.5093084326123236,
-            "velocityY": -1.412583871971558,
-            "timestamp": 1.436321263717647
-        },
-        {
-            "x": 2.8208656132222933,
-            "y": 6.041748259045348,
-            "heading": -0.1009771779112871,
-            "angularVelocity": -1.4088397962304153e-7,
-            "velocityX": -3.1624828986324123,
-            "velocityY": -2.0758950947495576,
-            "timestamp": 1.5260913427
-        },
-        {
-            "x": 2.5792187192315943,
-            "y": 5.803145197833315,
-            "heading": -0.10097728674036179,
-            "angularVelocity": -0.0000012123108078932896,
-            "velocityX": -2.691842278966871,
-            "velocityY": -2.65793529332427,
-            "timestamp": 1.615861421682353
-        },
-        {
-            "x": 2.3963290188287294,
-            "y": 5.533479989641919,
-            "heading": -0.13644335482170092,
-            "angularVelocity": -0.3950767169047361,
-            "velocityX": -2.037312459521214,
-            "velocityY": -3.003954226707092,
-            "timestamp": 1.705631500664706
-        },
-        {
-            "x": 2.279822826385498,
-            "y": 5.254662990570068,
-            "heading": -0.18962986506951118,
-            "angularVelocity": -0.5924748073105779,
-            "velocityX": -1.2978287839789375,
-            "velocityY": -3.105901233821009,
-            "timestamp": 1.795401579647059
-        },
-        {
-            "x": 2.2328990354823257,
-            "y": 5.0773593431323425,
-            "heading": -0.225508532252886,
-            "angularVelocity": -0.6284306472549296,
-            "velocityX": -0.8218908506157174,
-            "velocityY": -3.1055514229095325,
-            "timestamp": 1.8524940644563022
-        },
-        {
-            "x": 2.2122650091837905,
-            "y": 4.906948206481987,
-            "heading": -0.2618449581428327,
-            "angularVelocity": -0.636448492486558,
-            "velocityX": -0.3614140524419242,
-            "velocityY": -2.9848260628208765,
-            "timestamp": 1.9095865492655453
-        },
-        {
-            "x": 2.2138710973194917,
-            "y": 4.752151212947765,
-            "heading": -0.29639527524079207,
-            "angularVelocity": -0.6051640108688915,
-            "velocityX": 0.02813134059521416,
-            "velocityY": -2.7113374737719043,
-            "timestamp": 1.9666790340747884
-        },
-        {
-            "x": 2.229883400142308,
-            "y": 4.620372877512837,
-            "heading": -0.32691110500805654,
-            "angularVelocity": -0.5344981895462751,
-            "velocityX": 0.280462531565201,
-            "velocityY": -2.308155545781648,
-            "timestamp": 2.0237715188840317
-        },
-        {
-            "x": 2.251883940293385,
-            "y": 4.51506115985361,
-            "heading": -0.3519685302004407,
-            "angularVelocity": -0.4388918309689961,
-            "velocityX": 0.38534914402167997,
-            "velocityY": -1.844581086479138,
-            "timestamp": 2.080864003693275
-        },
-        {
-            "x": 2.273515445313811,
-            "y": 4.436872238113893,
-            "heading": -0.37095335253129014,
-            "angularVelocity": -0.33252751906728534,
-            "velocityX": 0.3788853312819565,
-            "velocityY": -1.3695133781784317,
-            "timestamp": 2.1379564885025184
-        },
-        {
-            "x": 2.2905088224909966,
-            "y": 4.385405157743103,
-            "heading": -0.3836505931421081,
-            "angularVelocity": -0.22239775783770355,
-            "velocityX": 0.2976464806908316,
-            "velocityY": -0.9014685653065743,
-            "timestamp": 2.1950489733117617
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.11121265573212626,
-            "velocityX": 0.16624215149881386,
-            "velocityY": -0.4449825196428863,
-            "timestamp": 2.252141458121005
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -5.463060715454085e-29,
-            "velocityX": 0,
-            "velocityY": -5.448481045128469e-30,
-            "timestamp": 2.3092339429302484
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C2 To C1.traj b/src/main/deploy/choreo/C2 To C1.traj
deleted file mode 100644
index 90a593eb..00000000
--- a/src/main/deploy/choreo/C2 To C1.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -3.817834980565894e-40,
-            "angularVelocity": -1.9855221195343998e-39,
-            "velocityX": -9.621145750064654e-40,
-            "velocityY": 1.4575297691012433e-39,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 5.880500005575846,
-            "heading": 3.479114156010015e-22,
-            "angularVelocity": 3.4766412561552746e-21,
-            "velocityX": 2.7068848151306367e-33,
-            "velocityY": 0.8344065481228213,
-            "timestamp": 0.10007112931184182
-        },
-        {
-            "x": 7.68,
-            "y": 6.047500013629847,
-            "heading": 1.081691491825702e-21,
-            "angularVelocity": 7.332585052389997e-21,
-            "velocityX": 5.1589068200097806e-33,
-            "velocityY": 1.6688130652907358,
-            "timestamp": 0.20014225862368365
-        },
-        {
-            "x": 7.68,
-            "y": 6.298000017966616,
-            "heading": 2.2511586271387426e-21,
-            "angularVelocity": 1.1686358860616172e-20,
-            "velocityX": 5.8498916949463956e-33,
-            "velocityY": 2.503219520548835,
-            "timestamp": 0.3002133879355255
-        },
-        {
-            "x": 7.68,
-            "y": 6.632,
-            "heading": 4.481673966100434e-21,
-            "angularVelocity": 2.228929929167172e-20,
-            "velocityX": 6.576791251337367e-33,
-            "velocityY": 3.3376257900775013,
-            "timestamp": 0.4002845172473673
-        },
-        {
-            "x": 7.68,
-            "y": 6.9659999820333836,
-            "heading": 1.4391637904192512e-21,
-            "angularVelocity": -3.0403475801105514e-20,
-            "velocityX": 1.0509973010204301e-32,
-            "velocityY": 3.3376257900775013,
-            "timestamp": 0.5003556465592092
-        },
-        {
-            "x": 7.68,
-            "y": 7.216499986370152,
-            "heading": 4.7457500532831525e-22,
-            "angularVelocity": -9.63903176962576e-21,
-            "velocityX": 7.983324746463204e-33,
-            "velocityY": 2.503219520548835,
-            "timestamp": 0.600426775871051
-        },
-        {
-            "x": 7.68,
-            "y": 7.383499994424153,
-            "heading": 1.0909640932435146e-22,
-            "angularVelocity": -3.652188228166023e-21,
-            "velocityX": 7.341551448907972e-33,
-            "velocityY": 1.6688130652907358,
-            "timestamp": 0.7004979051828928
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -9.42965601107167e-40,
-            "angularVelocity": -1.09018866193586e-21,
-            "velocityX": 7.44470103227488e-33,
-            "velocityY": 0.8344065481228213,
-            "timestamp": 0.8005690344947346
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 8.249839679439821e-43,
-            "angularVelocity": 9.431229878048832e-39,
-            "velocityX": 1.012759498983901e-41,
-            "velocityY": 5.142787784847508e-39,
-            "timestamp": 0.9006401638065764
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C2 To C3.traj b/src/main/deploy/choreo/C2 To C3.traj
deleted file mode 100644
index 74357de2..00000000
--- a/src/main/deploy/choreo/C2 To C3.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -5.382102664363315e-41,
-            "angularVelocity": -2.8652892976895567e-45,
-            "velocityX": 1.592735858156421e-40,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 5.713499994424153,
-            "heading": 1.659495816830368e-21,
-            "angularVelocity": 1.658316243120447e-20,
-            "velocityX": 2.780383744489643e-33,
-            "velocityY": -0.8344065481228213,
-            "timestamp": 0.10007112931184184
-        },
-        {
-            "x": 7.68,
-            "y": 5.546499986370153,
-            "heading": 4.9512387638524226e-21,
-            "angularVelocity": 3.289403167370527e-20,
-            "velocityX": 3.618624574631574e-33,
-            "velocityY": -1.668813065290736,
-            "timestamp": 0.20014225862368368
-        },
-        {
-            "x": 7.68,
-            "y": 5.295999982033384,
-            "heading": 9.801760970151702e-21,
-            "angularVelocity": 4.8470744390330394e-20,
-            "velocityX": 3.439292531132709e-33,
-            "velocityY": -2.503219520548835,
-            "timestamp": 0.30021338793552554
-        },
-        {
-            "x": 7.68,
-            "y": 4.962,
-            "heading": 1.506109654033251e-20,
-            "angularVelocity": 5.255597191401202e-20,
-            "velocityX": 4.639675238422859e-33,
-            "velocityY": -3.3376257900775017,
-            "timestamp": 0.40028451724736736
-        },
-        {
-            "x": 7.68,
-            "y": 4.628000017966616,
-            "heading": 5.3586032763760104e-21,
-            "angularVelocity": -9.69559666912617e-20,
-            "velocityX": 4.935439546802769e-33,
-            "velocityY": -3.3376257900775017,
-            "timestamp": 0.5003556465592092
-        },
-        {
-            "x": 7.68,
-            "y": 4.377500013629846,
-            "heading": 1.8760590350448333e-21,
-            "angularVelocity": -3.480068842301423e-20,
-            "velocityX": 4.701730454770122e-33,
-            "velocityY": -2.5032195205488352,
-            "timestamp": 0.600426775871051
-        },
-        {
-            "x": 7.68,
-            "y": 4.210500005575846,
-            "heading": 3.168722428707802e-22,
-            "angularVelocity": -1.5580785203142817e-20,
-            "velocityX": 3.49539235617723e-33,
-            "velocityY": -1.668813065290736,
-            "timestamp": 0.7004979051828928
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 5.881496870004905e-41,
-            "angularVelocity": -3.1664700918333573e-21,
-            "velocityX": 3.364378386644386e-33,
-            "velocityY": -0.8344065481228213,
-            "timestamp": 0.8005690344947346
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 9.145026397391945e-42,
-            "angularVelocity": -4.963805231001231e-40,
-            "velocityX": -4.870998572760777e-42,
-            "velocityY": 3.489748854003666e-39,
-            "timestamp": 0.9006401638065764
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C2 To C4.traj b/src/main/deploy/choreo/C2 To C4.traj
deleted file mode 100644
index bcede3cb..00000000
--- a/src/main/deploy/choreo/C2 To C4.traj	
+++ /dev/null
@@ -1,139 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 7.067561628822357e-40,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 5.720554627994461,
-            "heading": -3.8243496398495774e-19,
-            "angularVelocity": -3.994077070459528e-18,
-            "velocityX": -4.592295404090674e-34,
-            "velocityY": -0.7983807350946677,
-            "timestamp": 0.09575052183150977
-        },
-        {
-            "x": 7.68,
-            "y": 5.567663887017311,
-            "heading": -1.1460790562861162e-18,
-            "angularVelocity": -7.975351749671857e-18,
-            "velocityX": 3.216033275420413e-35,
-            "velocityY": -1.5967614385035829,
-            "timestamp": 0.19150104366301954
-        },
-        {
-            "x": 7.68,
-            "y": 5.3383277831364,
-            "heading": -2.289011643788579e-18,
-            "angularVelocity": -1.1936567706665239e-17,
-            "velocityX": -6.99395718741277e-36,
-            "velocityY": -2.3951420785410233,
-            "timestamp": 0.2872515654945293
-        },
-        {
-            "x": 7.68,
-            "y": 5.032546334555254,
-            "heading": -3.801510097022925e-18,
-            "angularVelocity": -1.5796242396621782e-17,
-            "velocityX": -1.9389720200265424e-34,
-            "velocityY": -3.1935225284643693,
-            "timestamp": 0.3830020873260391
-        },
-        {
-            "x": 7.68,
-            "y": 4.670327800733179,
-            "heading": -7.244345840319028e-19,
-            "angularVelocity": 3.2136383741131935e-17,
-            "velocityX": -8.432373407035536e-35,
-            "velocityY": -3.7829405719528495,
-            "timestamp": 0.47875260915754886
-        },
-        {
-            "x": 7.68,
-            "y": 4.308109266911059,
-            "heading": -3.757861819981246e-18,
-            "angularVelocity": -3.168052954622205e-17,
-            "velocityX": -1.6004910806259417e-34,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.5745031309890586
-        },
-        {
-            "x": 7.68,
-            "y": 3.9458907330889397,
-            "heading": -5.3976563030855884e-18,
-            "angularVelocity": -1.712569761227889e-17,
-            "velocityX": -2.8603575042124878e-33,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.6702536528205684
-        },
-        {
-            "x": 7.68,
-            "y": 3.5836721992668203,
-            "heading": -4.35645304230582e-18,
-            "angularVelocity": 1.0874126227865062e-17,
-            "velocityX": -1.9073271103237224e-34,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.7660041746520782
-        },
-        {
-            "x": 7.68,
-            "y": 3.221453665444745,
-            "heading": 3.661990694649205e-18,
-            "angularVelocity": 8.37430813282138e-17,
-            "velocityX": -3.48109229998139e-33,
-            "velocityY": -3.7829405719528495,
-            "timestamp": 0.8617546964835879
-        },
-        {
-            "x": 7.68,
-            "y": 2.915672216863599,
-            "heading": 2.222559506115206e-18,
-            "angularVelocity": -1.503314198751682e-17,
-            "velocityX": 7.233727398507661e-34,
-            "velocityY": -3.1935225284643693,
-            "timestamp": 0.9575052183150977
-        },
-        {
-            "x": 7.68,
-            "y": 2.686336112982689,
-            "heading": 1.1174854461234148e-18,
-            "angularVelocity": -1.1541180553049724e-17,
-            "velocityX": 7.218959232022663e-34,
-            "velocityY": -2.3951420785410233,
-            "timestamp": 1.0532557401466074
-        },
-        {
-            "x": 7.68,
-            "y": 2.5334453720055388,
-            "heading": 3.7383227049768543e-19,
-            "angularVelocity": -7.766570474567622e-18,
-            "velocityX": 1.7872274250399634e-33,
-            "velocityY": -1.5967614385035829,
-            "timestamp": 1.1490062619781172
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -3.904232200157263e-18,
-            "velocityX": 6.089132400706592e-33,
-            "velocityY": -0.7983807350946677,
-            "timestamp": 1.244756783809627
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 3.5763678522980893e-43,
-            "velocityY": -5.882608067207678e-38,
-            "timestamp": 1.3405073056411367
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C2 To C5.traj b/src/main/deploy/choreo/C2 To C5.traj
deleted file mode 100644
index 3fd40c6e..00000000
--- a/src/main/deploy/choreo/C2 To C5.traj	
+++ /dev/null
@@ -1,175 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -1.2882449649302714e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 5.715155846048237,
-            "heading": -2.6675793503802873e-18,
-            "angularVelocity": -2.692513973443352e-17,
-            "velocityX": 2.411941793716939e-19,
-            "velocityY": -0.8260917454276263,
-            "timestamp": 0.09907392781100396
-        },
-        {
-            "x": 7.68,
-            "y": 5.55146754125807,
-            "heading": -8.002991694377614e-18,
-            "angularVelocity": -5.385283961043983e-17,
-            "velocityX": 4.822926900034719e-19,
-            "velocityY": -1.6521834594306495,
-            "timestamp": 0.19814785562200793
-        },
-        {
-            "x": 7.68,
-            "y": 5.305935091856216,
-            "heading": -1.6007101079545214e-17,
-            "angularVelocity": -8.07892607262788e-17,
-            "velocityX": 7.23199861883135e-19,
-            "velocityY": -2.478275110584479,
-            "timestamp": 0.29722178343301187
-        },
-        {
-            "x": 7.68,
-            "y": 4.978558516522808,
-            "heading": -2.6680861930648114e-17,
-            "angularVelocity": -1.0773531530269989e-16,
-            "velocityX": 9.635330187135107e-19,
-            "velocityY": -3.3043665731908765,
-            "timestamp": 0.39629571124401586
-        },
-        {
-            "x": 7.68,
-            "y": 4.603767735073314,
-            "heading": -3.033567109313287e-17,
-            "angularVelocity": -3.688971703490728e-17,
-            "velocityX": 1.1102275506466142e-18,
-            "velocityY": -3.782940575087071,
-            "timestamp": 0.49536963905501985
-        },
-        {
-            "x": 7.68,
-            "y": 4.228976953623796,
-            "heading": -3.5529792774719166e-17,
-            "angularVelocity": -5.2426726146536295e-17,
-            "velocityX": 1.1087814850277944e-18,
-            "velocityY": -3.7829405750873115,
-            "timestamp": 0.5944435668660238
-        },
-        {
-            "x": 7.68,
-            "y": 3.8541861721742774,
-            "heading": -4.8775545061581196e-17,
-            "angularVelocity": -1.3369564101798844e-16,
-            "velocityX": 1.1066129122730268e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.6935174946770278
-        },
-        {
-            "x": 7.68,
-            "y": 3.4793953907247595,
-            "heading": -3.3383639047364345e-17,
-            "angularVelocity": 1.5535778538404392e-16,
-            "velocityX": 1.113993391664377e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.7925914224880318
-        },
-        {
-            "x": 7.68,
-            "y": 3.104604609275241,
-            "heading": -4.339521377299963e-18,
-            "angularVelocity": 2.9315601300747485e-16,
-            "velocityX": 1.114171335036007e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.8916653502990358
-        },
-        {
-            "x": 7.68,
-            "y": 2.7298138278257227,
-            "heading": 1.3174711336089981e-17,
-            "angularVelocity": 1.7677943229992181e-16,
-            "velocityX": 1.1158489583207238e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 0.9907392781100398
-        },
-        {
-            "x": 7.68,
-            "y": 2.3550230463762043,
-            "heading": 2.965421117619106e-17,
-            "angularVelocity": 1.6633538413393434e-16,
-            "velocityX": 1.1181357718618657e-18,
-            "velocityY": -3.782940575087312,
-            "timestamp": 1.0898132059210437
-        },
-        {
-            "x": 7.68,
-            "y": 1.9802322649266861,
-            "heading": 2.902288280090785e-17,
-            "angularVelocity": -6.372295813531433e-18,
-            "velocityX": 1.121394742098521e-18,
-            "velocityY": -3.7829405750873115,
-            "timestamp": 1.1888871337320477
-        },
-        {
-            "x": 7.68,
-            "y": 1.605441483477192,
-            "heading": 2.667314494747493e-17,
-            "angularVelocity": -2.3717015216307205e-17,
-            "velocityX": 1.1050727300415562e-18,
-            "velocityY": -3.782940575087071,
-            "timestamp": 1.2879610615430517
-        },
-        {
-            "x": 7.68,
-            "y": 1.2780649081437845,
-            "heading": 1.6004201006274906e-17,
-            "angularVelocity": -1.0768669595616384e-16,
-            "velocityX": 9.663666503471993e-19,
-            "velocityY": -3.304366573190877,
-            "timestamp": 1.3870349893540557
-        },
-        {
-            "x": 7.68,
-            "y": 1.0325324587419298,
-            "heading": 8.002125258512226e-18,
-            "angularVelocity": -8.076873426351199e-17,
-            "velocityX": 7.247750412790524e-19,
-            "velocityY": -2.478275110584479,
-            "timestamp": 1.4861089171650597
-        },
-        {
-            "x": 7.68,
-            "y": 0.868844153951763,
-            "heading": 2.6673738748743342e-18,
-            "angularVelocity": -5.384616822543564e-17,
-            "velocityX": 4.831833763651305e-19,
-            "velocityY": -1.6521834594306495,
-            "timestamp": 1.5851828449760637
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -2.6923065773077948e-17,
-            "velocityX": 2.415916928361511e-19,
-            "velocityY": -0.8260917454276263,
-            "timestamp": 1.6842567727870676
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -1.1487176577652646e-33,
-            "velocityY": 0,
-            "timestamp": 1.7833307005980716
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C2 To S1.traj b/src/main/deploy/choreo/C2 To S1.traj
deleted file mode 100644
index 6cbc4c1d..00000000
--- a/src/main/deploy/choreo/C2 To S1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 7.776001513534033e-33,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.620476769374415,
-            "y": 5.82809535402629,
-            "heading": -0.014618874829729222,
-            "angularVelocity": -0.16274311325472623,
-            "velocityX": -0.6626362135126213,
-            "velocityY": 0.34616581514913597,
-            "timestamp": 0.0898279167539809
-        },
-        {
-            "x": 7.5013982904113785,
-            "y": 5.890300493780534,
-            "heading": -0.04163561529349077,
-            "angularVelocity": -0.30076107116849365,
-            "velocityX": -1.3256288608937272,
-            "velocityY": 0.6924922897255835,
-            "timestamp": 0.1796558335079618
-        },
-        {
-            "x": 7.3227216232188646,
-            "y": 5.983633458070523,
-            "heading": -0.07726722819497155,
-            "angularVelocity": -0.39666524827763777,
-            "velocityX": -1.9890995321851956,
-            "velocityY": 1.0390195794655717,
-            "timestamp": 0.2694837502619427
-        },
-        {
-            "x": 7.084404558965627,
-            "y": 6.108106280619183,
-            "heading": -0.11365614248079976,
-            "angularVelocity": -0.40509582767559393,
-            "velocityX": -2.6530400889284445,
-            "velocityY": 1.3856808333823862,
-            "timestamp": 0.3593116670159236
-        },
-        {
-            "x": 6.786764320803032,
-            "y": 6.263508027124416,
-            "heading": -0.12453213515982267,
-            "angularVelocity": -0.12107586452004557,
-            "velocityX": -3.313449191722499,
-            "velocityY": 1.7299938829800985,
-            "timestamp": 0.4491395837699045
-        },
-        {
-            "x": 6.485598483527544,
-            "y": 6.42090073842616,
-            "heading": -0.1245321478638912,
-            "angularVelocity": -1.4142672994383305e-7,
-            "velocityX": -3.3526975594938513,
-            "velocityY": 1.7521580928210598,
-            "timestamp": 0.5389675005238854
-        },
-        {
-            "x": 6.164419225817841,
-            "y": 6.531883674964022,
-            "heading": -0.12453214704403566,
-            "angularVelocity": 9.126957138704023e-9,
-            "velocityX": -3.575494894191328,
-            "velocityY": 1.2355060714790946,
-            "timestamp": 0.6287954172778664
-        },
-        {
-            "x": 5.838530026083828,
-            "y": 6.576482035403971,
-            "heading": -0.08897109734241883,
-            "angularVelocity": 0.39587971074750417,
-            "velocityX": -3.62792783702814,
-            "velocityY": 0.4964866385813549,
-            "timestamp": 0.7186233340318473
-        },
-        {
-            "x": 5.57314678987786,
-            "y": 6.601508855183474,
-            "heading": 0.02708371106201491,
-            "angularVelocity": 1.2919681608812392,
-            "velocityX": -2.9543514510393742,
-            "velocityY": 0.27860848479928674,
-            "timestamp": 0.8084512507858282
-        },
-        {
-            "x": 5.3743330310505755,
-            "y": 6.616929267325104,
-            "heading": 0.1370261836602847,
-            "angularVelocity": 1.2239232141983023,
-            "velocityX": -2.213273623742076,
-            "velocityY": 0.17166614454461948,
-            "timestamp": 0.8982791675398092
-        },
-        {
-            "x": 5.242037120273562,
-            "y": 6.625930657483227,
-            "heading": 0.21902488355780253,
-            "angularVelocity": 0.9128420524556349,
-            "velocityX": -1.4727705546076917,
-            "velocityY": 0.10020704568687104,
-            "timestamp": 0.9881070842937901
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": 0.48954843918551255,
-            "velocityX": -0.7351514168409599,
-            "velocityY": 0.0453015350218741,
-            "timestamp": 1.077935001047771
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": -4.780109867389741e-33,
-            "velocityX": 0,
-            "velocityY": -5.233682494366388e-34,
-            "timestamp": 1.167762917801752
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C2 To S2.traj b/src/main/deploy/choreo/C2 To S2.traj
deleted file mode 100644
index 4e0731ce..00000000
--- a/src/main/deploy/choreo/C2 To S2.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 1.3211551091100433e-31,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.6375302942972665,
-            "y": 5.822387711771769,
-            "heading": 0.010500881646553203,
-            "angularVelocity": 0.13619602846032355,
-            "velocityX": -0.5508304389364825,
-            "velocityY": 0.32927763890619355,
-            "timestamp": 0.07710123243140166
-        },
-        {
-            "x": 7.5525908811480225,
-            "y": 5.873163180716977,
-            "heading": 0.031500279151306286,
-            "angularVelocity": 0.27236137273728567,
-            "velocityX": -1.1016609004897018,
-            "velocityY": 0.6585558666702785,
-            "timestamp": 0.1542024648628033
-        },
-        {
-            "x": 7.4251817593524025,
-            "y": 5.949326484276257,
-            "heading": 0.06299451206522548,
-            "angularVelocity": 0.40847898173275193,
-            "velocityX": -1.6524913776051289,
-            "velocityY": 0.987835098836381,
-            "timestamp": 0.23130369729420497
-        },
-        {
-            "x": 7.255302939480135,
-            "y": 6.050877733979633,
-            "heading": 0.10497913316622995,
-            "angularVelocity": 0.5445389104299847,
-            "velocityX": -2.203321717631576,
-            "velocityY": 1.3171157775425744,
-            "timestamp": 0.3084049297256066
-        },
-        {
-            "x": 7.042954462706059,
-            "y": 6.177817072431174,
-            "heading": 0.1574496496087124,
-            "angularVelocity": 0.6805405670936119,
-            "velocityX": -2.754151523621958,
-            "velocityY": 1.6463983058180132,
-            "timestamp": 0.3855061621570083
-        },
-        {
-            "x": 6.792607460054783,
-            "y": 6.3274746833219355,
-            "heading": 0.15744965721454035,
-            "angularVelocity": 9.864729365588338e-8,
-            "velocityX": -3.246990933303352,
-            "velocityY": 1.941053419916652,
-            "timestamp": 0.46260739458840994
-        },
-        {
-            "x": 6.542259846259959,
-            "y": 6.4771312718889495,
-            "heading": 0.15744966481969572,
-            "angularVelocity": 9.863857053631023e-8,
-            "velocityX": -3.246998859811527,
-            "velocityY": 1.9410401604172332,
-            "timestamp": 0.5397086270198116
-        },
-        {
-            "x": 6.273256747030451,
-            "y": 6.589862608843688,
-            "heading": 0.1574496727775284,
-            "angularVelocity": 1.0321278187268036e-7,
-            "velocityX": -3.4889597837342676,
-            "velocityY": 1.4621210764047974,
-            "timestamp": 0.6168098594512132
-        },
-        {
-            "x": 5.98904964850956,
-            "y": 6.655416753006686,
-            "heading": 0.15744969119167346,
-            "angularVelocity": 2.3883074855376557e-7,
-            "velocityX": -3.686155065987494,
-            "velocityY": 0.8502347121535809,
-            "timestamp": 0.6939110918826149
-        },
-        {
-            "x": 5.6978465137603145,
-            "y": 6.6719003531074685,
-            "heading": 0.15744999446473232,
-            "angularVelocity": 0.00000393343983387992,
-            "velocityX": -3.7768933850484796,
-            "velocityY": 0.21379165521703997,
-            "timestamp": 0.7710123243140166
-        },
-        {
-            "x": 5.40805721282959,
-            "y": 6.638837814331055,
-            "heading": 0.15745035498223803,
-            "angularVelocity": 0.00000467589809324531,
-            "velocityX": -3.7585560151526085,
-            "velocityY": -0.428819848059242,
-            "timestamp": 0.8481135567454182
-        },
-        {
-            "x": 5.104307335648147,
-            "y": 6.562247743535093,
-            "heading": 0.18966206475475966,
-            "angularVelocity": 0.37702586381684944,
-            "velocityX": -3.555277277652879,
-            "velocityY": -0.896457772827513,
-            "timestamp": 0.9335498964342235
-        },
-        {
-            "x": 4.836190414730584,
-            "y": 6.438293970082571,
-            "heading": 0.27176288986041636,
-            "angularVelocity": 0.9609590650149814,
-            "velocityX": -3.1382070193334055,
-            "velocityY": -1.4508319750590115,
-            "timestamp": 1.0189862361230289
-        },
-        {
-            "x": 4.616497381460216,
-            "y": 6.278221072521664,
-            "heading": 0.3274582685749672,
-            "angularVelocity": 0.6518933151562509,
-            "velocityX": -2.5714237532949347,
-            "velocityY": -1.8735926438791781,
-            "timestamp": 1.1044225758118342
-        },
-        {
-            "x": 4.449006665280017,
-            "y": 6.09487376587715,
-            "heading": 0.3096216512620487,
-            "angularVelocity": -0.2087708506460702,
-            "velocityX": -1.960415401575849,
-            "velocityY": -2.1460107878256482,
-            "timestamp": 1.1898589155006396
-        },
-        {
-            "x": 4.339814814870372,
-            "y": 5.903698766708507,
-            "heading": 0.23521329843339078,
-            "angularVelocity": -0.8709215902704173,
-            "velocityX": -1.2780492564097095,
-            "velocityY": -2.237630964352897,
-            "timestamp": 1.275295255189445
-        },
-        {
-            "x": 4.286146359443328,
-            "y": 5.736442488850756,
-            "heading": 0.1356278137676922,
-            "angularVelocity": -1.1656103834554516,
-            "velocityX": -0.628168945703036,
-            "velocityY": -1.957671389796991,
-            "timestamp": 1.3607315948782504
-        },
-        {
-            "x": 4.266951931468271,
-            "y": 5.619203136466468,
-            "heading": 0.049466079264437586,
-            "angularVelocity": -1.0084904715849423,
-            "velocityX": -0.2246635102225946,
-            "velocityY": -1.372242219309977,
-            "timestamp": 1.4461679345670557
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 4.182976778726496e-34,
-            "angularVelocity": -0.578981724224301,
-            "velocityX": -0.046255861178807596,
-            "velocityY": -0.6929502912005617,
-            "timestamp": 1.531604274255861
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -6.30298269203181e-33,
-            "timestamp": 1.6170406139446665
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C2 To S3.traj b/src/main/deploy/choreo/C2 To S3.traj
deleted file mode 100644
index 90749c58..00000000
--- a/src/main/deploy/choreo/C2 To S3.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 7.446350164213638e-30,
-            "velocityX": -3.476389258222187e-32,
-            "velocityY": 2.4606016306403766e-37,
-            "timestamp": 0
-        },
-        {
-            "x": 7.6436015698643995,
-            "y": 5.74806653588022,
-            "heading": -0.0038807165751560974,
-            "angularVelocity": -0.045372809336175934,
-            "velocityX": -0.4255654848002544,
-            "velocityY": -0.572123393880186,
-            "timestamp": 0.08552956345292874
-        },
-        {
-            "x": 7.570804707716712,
-            "y": 5.6501996114987145,
-            "heading": -0.011642164214821673,
-            "angularVelocity": -0.09074578807989701,
-            "velocityX": -0.8511309915401587,
-            "velocityY": -1.1442467426525054,
-            "timestamp": 0.17105912690585748
-        },
-        {
-            "x": 7.461609412342726,
-            "y": 5.503399231866818,
-            "heading": -0.02328436718357624,
-            "angularVelocity": -0.136119050521775,
-            "velocityX": -1.2766965124764296,
-            "velocityY": -1.7163700328330225,
-            "timestamp": 0.25658869035878623
-        },
-        {
-            "x": 7.3160156842382476,
-            "y": 5.307665404737857,
-            "heading": -0.038807352623976644,
-            "angularVelocity": -0.18149263031072865,
-            "velocityX": -1.7022620276158245,
-            "velocityY": -2.288493232362672,
-            "timestamp": 0.34211825381171496
-        },
-        {
-            "x": 7.134023531165023,
-            "y": 5.062998148299065,
-            "heading": -0.05821111114579378,
-            "angularVelocity": -0.22686610031040333,
-            "velocityX": -2.127827452005934,
-            "velocityY": -2.8606162192496707,
-            "timestamp": 0.4276478172646437
-        },
-        {
-            "x": 6.9409164317403365,
-            "y": 4.803390071813602,
-            "heading": -0.058211113467083825,
-            "angularVelocity": -2.714020682501112e-8,
-            "velocityX": -2.2577818900123794,
-            "velocityY": -3.0353022511138943,
-            "timestamp": 0.5131773807175725
-        },
-        {
-            "x": 6.70968017178257,
-            "y": 4.577079452070077,
-            "heading": -0.05821111591888997,
-            "angularVelocity": -2.866618337367041e-8,
-            "velocityX": -2.7035828387575864,
-            "velocityY": -2.645992924634468,
-            "timestamp": 0.5987069441705012
-        },
-        {
-            "x": 6.4400791107327935,
-            "y": 4.398188508585333,
-            "heading": -0.05821111892081074,
-            "angularVelocity": -3.509804859460385e-8,
-            "velocityX": -3.152138864804934,
-            "velocityY": -2.091568532127445,
-            "timestamp": 0.6842365076234299
-        },
-        {
-            "x": 6.141694589328426,
-            "y": 4.273075039260189,
-            "heading": -0.058211144902897714,
-            "angularVelocity": -3.0377902004237926e-7,
-            "velocityX": -3.488671160686832,
-            "velocityY": -1.462809632999017,
-            "timestamp": 0.7697660710763586
-        },
-        {
-            "x": 5.825131041705898,
-            "y": 4.206185466847777,
-            "heading": -0.05821124348692034,
-            "angularVelocity": -0.000001152630957627144,
-            "velocityX": -3.7012178578082944,
-            "velocityY": -0.7820637649954203,
-            "timestamp": 0.8552956345292874
-        },
-        {
-            "x": 5.503318785454965,
-            "y": 4.1726658015863975,
-            "heading": -0.058211338856237596,
-            "angularVelocity": -0.0000011150450605169934,
-            "velocityX": -3.76258504380235,
-            "velocityY": -0.39190735820634004,
-            "timestamp": 0.9408251979822161
-        },
-        {
-            "x": 5.1935160355708865,
-            "y": 4.079344267897855,
-            "heading": -0.0582116136415855,
-            "angularVelocity": -0.0000032127528402629677,
-            "velocityX": -3.622171532005757,
-            "velocityY": -1.091102654112135,
-            "timestamp": 1.026354761435145
-        },
-        {
-            "x": 4.918641828284195,
-            "y": 3.9369980738928456,
-            "heading": -0.09326111746178506,
-            "angularVelocity": -0.4097940221510551,
-            "velocityX": -3.2137917719873403,
-            "velocityY": -1.664292301495839,
-            "timestamp": 1.1118843248880736
-        },
-        {
-            "x": 4.689025118769517,
-            "y": 3.755353610578967,
-            "heading": -0.16867561276178636,
-            "angularVelocity": -0.8817360016283226,
-            "velocityX": -2.6846472756878708,
-            "velocityY": -2.1237623107224977,
-            "timestamp": 1.1974138883410024
-        },
-        {
-            "x": 4.510985991586203,
-            "y": 3.542062267326439,
-            "heading": -0.2715725395002121,
-            "angularVelocity": -1.2030568447254515,
-            "velocityX": -2.0816092120158958,
-            "velocityY": -2.493773318156982,
-            "timestamp": 1.282943451793931
-        },
-        {
-            "x": 4.39344009542189,
-            "y": 3.3364721307805043,
-            "heading": -0.3792171957634167,
-            "angularVelocity": -1.2585666513128753,
-            "velocityX": -1.3743305989045989,
-            "velocityY": -2.4037318588569803,
-            "timestamp": 1.3684730152468598
-        },
-        {
-            "x": 4.321460732883776,
-            "y": 3.1711461674458645,
-            "heading": -0.4697306425856333,
-            "angularVelocity": -1.0582708851545903,
-            "velocityX": -0.8415728975131295,
-            "velocityY": -1.932968632835679,
-            "timestamp": 1.4540025786997886
-        },
-        {
-            "x": 4.280782976343733,
-            "y": 3.057632956228743,
-            "heading": -0.5333100471714295,
-            "angularVelocity": -0.7433617338733072,
-            "velocityX": -0.475598785938263,
-            "velocityY": -1.3271809960727023,
-            "timestamp": 1.5395321421527173
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -0.38220647351440606,
-            "velocityX": -0.20791613596297567,
-            "velocityY": -0.6738366700592523,
-            "timestamp": 1.625061705605646
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -1.9453350650223036e-32,
-            "velocityX": 0,
-            "velocityY": 3.675541041669649e-33,
-            "timestamp": 1.7105912690585747
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C2 To S4.traj b/src/main/deploy/choreo/C2 To S4.traj
deleted file mode 100644
index 4efefc66..00000000
--- a/src/main/deploy/choreo/C2 To S4.traj	
+++ /dev/null
@@ -1,256 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": -1.007826409753455e-39,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.661226431831969,
-            "y": 5.757681823957774,
-            "heading": -0.0001846118691686639,
-            "angularVelocity": -0.0025538708536768893,
-            "velocityX": -0.25970848342391806,
-            "velocityY": -0.5439170529292338,
-            "timestamp": 0.07228708096295171
-        },
-        {
-            "x": 7.623679292701499,
-            "y": 5.679045474684997,
-            "heading": -0.0005538375109882563,
-            "angularVelocity": -0.005107768039614528,
-            "velocityX": -0.5194170055049473,
-            "velocityY": -1.0878340669625215,
-            "timestamp": 0.14457416192590342
-        },
-        {
-            "x": 7.567358579979895,
-            "y": 5.561090955652203,
-            "heading": -0.0011076778331591435,
-            "angularVelocity": -0.0076616777824344475,
-            "velocityX": -0.779125563950612,
-            "velocityY": -1.6317510329853882,
-            "timestamp": 0.2168612428888551
-        },
-        {
-            "x": 7.492264291454711,
-            "y": 5.403818271607319,
-            "heading": -0.001846132228095339,
-            "angularVelocity": -0.010215579120073546,
-            "velocityX": -1.0388341530026832,
-            "velocityY": -2.175667933326667,
-            "timestamp": 0.28914832385180683
-        },
-        {
-            "x": 7.39839642615194,
-            "y": 5.207227430397753,
-            "heading": -0.0027691974892554232,
-            "angularVelocity": -0.012769436099282666,
-            "velocityX": -1.298542755528879,
-            "velocityY": -2.719584725109078,
-            "timestamp": 0.36143540481475855
-        },
-        {
-            "x": 7.285754988600711,
-            "y": 4.97131845200793,
-            "heading": -0.0038768638943260385,
-            "angularVelocity": -0.015323158582628504,
-            "velocityX": -1.5582512954003445,
-            "velocityY": -3.263501240432299,
-            "timestamp": 0.4337224857777103
-        },
-        {
-            "x": 7.167927209247298,
-            "y": 4.7245478329210355,
-            "heading": -0.0038768640537450113,
-            "angularVelocity": -2.205359112715029e-9,
-            "velocityX": -1.629997750411335,
-            "velocityY": -3.4137582511232942,
-            "timestamp": 0.506009566740662
-        },
-        {
-            "x": 7.050099366875978,
-            "y": 4.477777243923851,
-            "heading": -0.003876864213154645,
-            "angularVelocity": -2.2052299170164517e-9,
-            "velocityX": -1.6299986221840805,
-            "velocityY": -3.413757834870371,
-            "timestamp": 0.5782966477036137
-        },
-        {
-            "x": 6.932271561638543,
-            "y": 4.2310066371960025,
-            "heading": -0.0038768643725604667,
-            "angularVelocity": -2.2051771914474314e-9,
-            "velocityX": -1.629998108483918,
-            "velocityY": -3.413758080151585,
-            "timestamp": 0.6505837286665653
-        },
-        {
-            "x": 6.8252544845676155,
-            "y": 3.9793591274718993,
-            "heading": -0.0038768645333797187,
-            "angularVelocity": -2.2247301918048757e-9,
-            "velocityX": -1.4804454080221456,
-            "velocityY": -3.4812238421008592,
-            "timestamp": 0.722870809629517
-        },
-        {
-            "x": 6.7595626994044276,
-            "y": 3.7139091088268694,
-            "heading": -0.003876864707631206,
-            "angularVelocity": -2.410548124556829e-9,
-            "velocityX": -0.9087624550347577,
-            "velocityY": -3.6721640313720423,
-            "timestamp": 0.7951578905924687
-        },
-        {
-            "x": 6.7368645668029785,
-            "y": 3.4413950443267822,
-            "heading": -0.0038768649214938546,
-            "angularVelocity": -2.9585182521427424e-9,
-            "velocityX": -0.31399985030634436,
-            "velocityY": -3.7698861382956523,
-            "timestamp": 0.8674449715554203
-        },
-        {
-            "x": 6.763602448674763,
-            "y": 3.1456190677825386,
-            "heading": -0.0038769163309766456,
-            "angularVelocity": -6.548510424231184e-7,
-            "velocityX": 0.340585605327871,
-            "velocityY": -3.7675774205236117,
-            "timestamp": 0.9459505849856912
-        },
-        {
-            "x": 6.763837716877819,
-            "y": 2.855181760345028,
-            "heading": -0.02573615579182971,
-            "angularVelocity": -0.27844173818562123,
-            "velocityX": 0.0029968328731749647,
-            "velocityY": -3.699573759722024,
-            "timestamp": 1.0244561984159621
-        },
-        {
-            "x": 6.746691140594586,
-            "y": 2.576419895706607,
-            "heading": -0.08408576500639096,
-            "angularVelocity": -0.7432539746522423,
-            "velocityX": -0.2184121049950426,
-            "velocityY": -3.5508526391685153,
-            "timestamp": 1.102961811846233
-        },
-        {
-            "x": 6.69086397201793,
-            "y": 2.322743038831463,
-            "heading": -0.2046015958190934,
-            "angularVelocity": -1.5351237388870964,
-            "velocityX": -0.7111232705192682,
-            "velocityY": -3.2313212494092722,
-            "timestamp": 1.1814674252765038
-        },
-        {
-            "x": 6.586917702656428,
-            "y": 2.0871040226876985,
-            "heading": -0.32491541634118276,
-            "angularVelocity": -1.5325505433946174,
-            "velocityX": -1.324061615719081,
-            "velocityY": -3.001556269005664,
-            "timestamp": 1.2599730387067747
-        },
-        {
-            "x": 6.439847045017927,
-            "y": 1.879401635438828,
-            "heading": -0.448122685606705,
-            "angularVelocity": -1.5694071274910255,
-            "velocityX": -1.8733775995410846,
-            "velocityY": -2.6457010928696563,
-            "timestamp": 1.3384786521370455
-        },
-        {
-            "x": 6.25456019704424,
-            "y": 1.7060438180074393,
-            "heading": -0.5681346722700569,
-            "angularVelocity": -1.528705801018258,
-            "velocityX": -2.3601732395640638,
-            "velocityY": -2.2082219328859343,
-            "timestamp": 1.4169842655673164
-        },
-        {
-            "x": 6.049779045368227,
-            "y": 1.57972317182915,
-            "heading": -0.6633309119306381,
-            "angularVelocity": -1.2126042393788183,
-            "velocityX": -2.6084905617341763,
-            "velocityY": -1.6090651439911137,
-            "timestamp": 1.4954898789975872
-        },
-        {
-            "x": 5.840944041976222,
-            "y": 1.503518626439596,
-            "heading": -0.7208467997653518,
-            "angularVelocity": -0.7326340795464229,
-            "velocityX": -2.660128292322611,
-            "velocityY": -0.9706891273098467,
-            "timestamp": 1.573995492427858
-        },
-        {
-            "x": 5.636524773547887,
-            "y": 1.4770268115818306,
-            "heading": -0.7348631463149735,
-            "angularVelocity": -0.17853941822989652,
-            "velocityX": -2.603880913686534,
-            "velocityY": -0.33745121782018417,
-            "timestamp": 1.652501105858129
-        },
-        {
-            "x": 5.451777868175388,
-            "y": 1.4973120784666072,
-            "heading": -0.7197568228935092,
-            "angularVelocity": 0.19242348109134802,
-            "velocityX": -2.353295481686686,
-            "velocityY": 0.25839256581051945,
-            "timestamp": 1.7310067192883998
-        },
-        {
-            "x": 5.309771976878378,
-            "y": 1.5457401851903851,
-            "heading": -0.687868735789328,
-            "angularVelocity": 0.4061886241103559,
-            "velocityX": -1.808862896449324,
-            "velocityY": 0.6168744451222204,
-            "timestamp": 1.8095123327186706
-        },
-        {
-            "x": 5.2191255322417085,
-            "y": 1.592492981200916,
-            "heading": -0.6576952711638115,
-            "angularVelocity": 0.38434786134518467,
-            "velocityX": -1.154649211386418,
-            "velocityY": 0.5955344333696231,
-            "timestamp": 1.8880179461489415
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0.22540134890517805,
-            "velocityX": -0.5493305555788384,
-            "velocityY": 0.35038282738234927,
-            "timestamp": 1.9665235595792123
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0,
-            "velocityX": 6.617232561290069e-35,
-            "velocityY": 1.0472745338276337e-32,
-            "timestamp": 2.045029173009483
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C2 To W1.traj b/src/main/deploy/choreo/C2 To W1.traj
deleted file mode 100644
index 0428af29..00000000
--- a/src/main/deploy/choreo/C2 To W1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 2.477402495362019e-32,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.61783610738437,
-            "y": 5.830159445563511,
-            "heading": -0.009839426193567341,
-            "angularVelocity": -0.10699963479066096,
-            "velocityX": -0.6760062707098423,
-            "velocityY": 0.3605950687933442,
-            "timestamp": 0.09195756801242966
-        },
-        {
-            "x": 7.493508328314676,
-            "y": 5.8964783245931205,
-            "heading": -0.029519357600464063,
-            "angularVelocity": -0.21401100346886773,
-            "velocityX": -1.3520124744152573,
-            "velocityY": 0.7211900060324206,
-            "timestamp": 0.18391513602485932
-        },
-        {
-            "x": 7.307016677169599,
-            "y": 5.995956620127639,
-            "heading": -0.05904164496194892,
-            "angularVelocity": -0.32104249818236175,
-            "velocityX": -2.028018521758536,
-            "velocityY": 1.0817847588256284,
-            "timestamp": 0.275872704037289
-        },
-        {
-            "x": 7.058361185168299,
-            "y": 6.1285943089313815,
-            "heading": -0.09840880514573262,
-            "angularVelocity": -0.42810136277704247,
-            "velocityX": -2.7040242296065236,
-            "velocityY": 1.4423792589405482,
-            "timestamp": 0.36783027204971863
-        },
-        {
-            "x": 6.751428461707863,
-            "y": 6.292319225940392,
-            "heading": -0.09840881013370367,
-            "angularVelocity": -5.424209394751822e-8,
-            "velocityX": -3.3377646896767494,
-            "velocityY": 1.7804398327158815,
-            "timestamp": 0.4597878400621483
-        },
-        {
-            "x": 6.443018732830252,
-            "y": 6.45324462570272,
-            "heading": -0.09840881513118924,
-            "angularVelocity": -5.434556026601013e-8,
-            "velocityX": -3.3538265043712756,
-            "velocityY": 1.7499962563230937,
-            "timestamp": 0.551745408074578
-        },
-        {
-            "x": 6.108495593860041,
-            "y": 6.548679499028843,
-            "heading": -0.09840879814777771,
-            "angularVelocity": 1.8468748020115896e-7,
-            "velocityX": -3.637798891386455,
-            "velocityY": 1.0378142374668238,
-            "timestamp": 0.6437029760870077
-        },
-        {
-            "x": 5.7623127419169835,
-            "y": 6.582899007638951,
-            "heading": -0.09840874396641834,
-            "angularVelocity": 5.891995683553032e-7,
-            "velocityX": -3.764593381767812,
-            "velocityY": 0.37212281000605224,
-            "timestamp": 0.7356605440994373
-        },
-        {
-            "x": 5.416121749902788,
-            "y": 6.617036066627776,
-            "heading": -0.09840868977891702,
-            "angularVelocity": 5.892663593006056e-7,
-            "velocityX": -3.7646819016288298,
-            "velocityY": 0.37122620494064046,
-            "timestamp": 0.8276181121118669
-        },
-        {
-            "x": 5.068594601971295,
-            "y": 6.632477066079001,
-            "heading": -0.0984086463109951,
-            "angularVelocity": 4.72695427552955e-7,
-            "velocityX": -3.7792120370616824,
-            "velocityY": 0.1679143955735957,
-            "timestamp": 0.9195756801242965
-        },
-        {
-            "x": 4.721067428734796,
-            "y": 6.647917495985935,
-            "heading": -0.09840860284326601,
-            "angularVelocity": 4.7269333042542933e-7,
-            "velocityX": -3.7792123122430223,
-            "velocityY": 0.16790820201819795,
-            "timestamp": 1.011533248136726
-        },
-        {
-            "x": 4.3735402523552525,
-            "y": 6.663357855150138,
-            "heading": -0.09840855937552709,
-            "angularVelocity": 4.726934374928119e-7,
-            "velocityX": -3.779212346422309,
-            "velocityY": 0.16790743272066663,
-            "timestamp": 1.1034908161491557
-        },
-        {
-            "x": 4.026013074283842,
-            "y": 6.678798176234082,
-            "heading": -0.09840851590777203,
-            "angularVelocity": 4.7269361285314424e-7,
-            "velocityX": -3.779212364820652,
-            "velocityY": 0.1679070186138197,
-            "timestamp": 1.1954483841615853
-        },
-        {
-            "x": 3.6784858950907164,
-            "y": 6.69423847207055,
-            "heading": -0.09840847244000124,
-            "angularVelocity": 4.7269378404370134e-7,
-            "velocityX": -3.7792123770188395,
-            "velocityY": 0.16790674405809006,
-            "timestamp": 1.2874059521740149
-        },
-        {
-            "x": 3.330958715199463,
-            "y": 6.709678752193432,
-            "heading": -0.09840842897219951,
-            "angularVelocity": 4.7269412044437995e-7,
-            "velocityX": -3.7792123846106827,
-            "velocityY": 0.16790657317944163,
-            "timestamp": 1.3793635201864445
-        },
-        {
-            "x": 2.991492162018093,
-            "y": 6.7253542779205,
-            "heading": -0.07153342912076269,
-            "angularVelocity": 0.2922543563549246,
-            "velocityX": -3.6915564484642,
-            "velocityY": 0.17046477050097905,
-            "timestamp": 1.471321088198874
-        },
-        {
-            "x": 2.713831676976805,
-            "y": 6.738399816428541,
-            "heading": 0.08699262812295999,
-            "angularVelocity": 1.7239044123295555,
-            "velocityX": -3.0194413689122035,
-            "velocityY": 0.14186476208546492,
-            "timestamp": 1.5632786562113037
-        },
-        {
-            "x": 2.5064819943185284,
-            "y": 6.7478572981535265,
-            "heading": 0.2316797715135602,
-            "angularVelocity": 1.5734120259796696,
-            "velocityX": -2.2548408699787488,
-            "velocityY": 0.10284614882059591,
-            "timestamp": 1.6552362242237333
-        },
-        {
-            "x": 2.368710598807467,
-            "y": 6.7539967145328434,
-            "heading": 0.3372042735506754,
-            "angularVelocity": 1.147534719739997,
-            "velocityX": -1.4982061671361233,
-            "velocityY": 0.06676357924654308,
-            "timestamp": 1.747193792236163
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.6067551334304849,
-            "velocityX": -0.7471989559160562,
-            "velocityY": 0.03265947036300673,
-            "timestamp": 1.8391513602485925
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0,
-            "velocityX": -4.676461204476467e-32,
-            "velocityY": 0,
-            "timestamp": 1.931108928261022
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C2 To W2.traj b/src/main/deploy/choreo/C2 To W2.traj
deleted file mode 100644
index 5429265f..00000000
--- a/src/main/deploy/choreo/C2 To W2.traj	
+++ /dev/null
@@ -1,184 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -7.048442713490885e-38,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.598761685357721,
-            "y": 5.839328892781844,
-            "heading": -0.010877614975927053,
-            "angularVelocity": -0.10375029848266179,
-            "velocityX": -0.774848108801196,
-            "velocityY": 0.4037314494285639,
-            "timestamp": 0.10484418006512992
-        },
-        {
-            "x": 7.436285061134455,
-            "y": 5.923986667743732,
-            "heading": -0.03263383242489801,
-            "angularVelocity": -0.20751001567713226,
-            "velocityX": -1.5496961693280003,
-            "velocityY": 0.8074627977375359,
-            "timestamp": 0.20968836013025985
-        },
-        {
-            "x": 7.192570139480125,
-            "y": 6.050973310343331,
-            "heading": -0.06527034306657675,
-            "angularVelocity": -0.31128585889464805,
-            "velocityX": -2.3245441139692504,
-            "velocityY": 1.2111940073422762,
-            "timestamp": 0.3145325401953898
-        },
-        {
-            "x": 6.867616948374112,
-            "y": 6.220288799737083,
-            "heading": -0.10878948434468085,
-            "angularVelocity": -0.41508399656585776,
-            "velocityX": -3.0993917917441864,
-            "velocityY": 1.6149250181418864,
-            "timestamp": 0.4193767202605197
-        },
-        {
-            "x": 6.5158804421759,
-            "y": 6.403559919066418,
-            "heading": -0.10878948853448296,
-            "angularVelocity": -3.996218113987742e-8,
-            "velocityX": -3.3548500830442913,
-            "velocityY": 1.7480333120587623,
-            "timestamp": 0.5242209003256496
-        },
-        {
-            "x": 6.14377891604402,
-            "y": 6.540845489176542,
-            "heading": -0.10878948531130445,
-            "angularVelocity": 3.074256021150283e-8,
-            "velocityX": -3.549090907103546,
-            "velocityY": 1.309424805695852,
-            "timestamp": 0.6290650803907796
-        },
-        {
-            "x": 5.750100268474419,
-            "y": 6.5890532439035745,
-            "heading": -0.10878941324219087,
-            "angularVelocity": 6.873926003335433e-7,
-            "velocityX": -3.754892711498581,
-            "velocityY": 0.4598038221776824,
-            "timestamp": 0.7339092604559095
-        },
-        {
-            "x": 5.35529697720587,
-            "y": 6.626964014921626,
-            "heading": -0.10878934177449004,
-            "angularVelocity": 6.816563474487017e-7,
-            "velocityX": -3.7656195224503146,
-            "velocityY": 0.3615915637329853,
-            "timestamp": 0.8387534405210395
-        },
-        {
-            "x": 4.9606240931120595,
-            "y": 6.587718813133039,
-            "heading": -0.10878931155021784,
-            "angularVelocity": 2.882780157548741e-7,
-            "velocityX": -3.7643757035310585,
-            "velocityY": -0.3743193161910223,
-            "timestamp": 0.9435976205861695
-        },
-        {
-            "x": 4.585497747567555,
-            "y": 6.458927912145357,
-            "heading": -0.10878930710495487,
-            "angularVelocity": 4.239875765382173e-8,
-            "velocityX": -3.5779415253328617,
-            "velocityY": -1.2284029586351546,
-            "timestamp": 1.0484418006512994
-        },
-        {
-            "x": 4.215349929173217,
-            "y": 6.316458743577884,
-            "heading": -0.10878930270384918,
-            "angularVelocity": 4.197758707259389e-8,
-            "velocityX": -3.530456513317188,
-            "velocityY": -1.3588657804273945,
-            "timestamp": 1.1532859807164293
-        },
-        {
-            "x": 3.8452021463353314,
-            "y": 6.173989482631574,
-            "heading": -0.10878929830273087,
-            "angularVelocity": 4.197770719831182e-8,
-            "velocityX": -3.530456174181034,
-            "velocityY": -1.3588666615333942,
-            "timestamp": 1.2581301607815591
-        },
-        {
-            "x": 3.475054376591526,
-            "y": 6.0315201876656985,
-            "heading": -0.10878929390160265,
-            "angularVelocity": 4.197780205439048e-8,
-            "velocityX": -3.530456049290165,
-            "velocityY": -1.3588669860107905,
-            "timestamp": 1.362974340846689
-        },
-        {
-            "x": 3.104906616234201,
-            "y": 5.889050868312964,
-            "heading": -0.10878928950045777,
-            "angularVelocity": 4.19779608468647e-8,
-            "velocityX": -3.530455959762256,
-            "velocityY": -1.358867218611777,
-            "timestamp": 1.4678185209118189
-        },
-        {
-            "x": 2.7629439773689555,
-            "y": 5.757430528072475,
-            "heading": -0.06527490613894259,
-            "angularVelocity": 0.4150386157294016,
-            "velocityX": -3.2616272896866185,
-            "velocityY": -1.2553900479618894,
-            "timestamp": 1.5726627009769487
-        },
-        {
-            "x": 2.506471990883214,
-            "y": 5.658715266844828,
-            "heading": -0.03263773447849684,
-            "angularVelocity": 0.311292163667756,
-            "velocityX": -2.446220537243172,
-            "velocityY": -0.9415425936501655,
-            "timestamp": 1.6775068810420786
-        },
-        {
-            "x": 2.335490664707938,
-            "y": 5.592905089907167,
-            "heading": -0.010879217132893882,
-            "angularVelocity": 0.20753195200808147,
-            "velocityX": -1.6308137091544919,
-            "velocityY": -0.6276950890052175,
-            "timestamp": 1.7823510611072084
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0.10376557979790346,
-            "velocityX": -0.8154068700316104,
-            "velocityY": -0.3138475582214142,
-            "timestamp": 1.8871952411723383
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 2.158258486540926e-34,
-            "velocityY": 0,
-            "timestamp": 1.9920394212374681
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C2 To W3.traj b/src/main/deploy/choreo/C2 To W3.traj
deleted file mode 100644
index cf63c7e3..00000000
--- a/src/main/deploy/choreo/C2 To W3.traj	
+++ /dev/null
@@ -1,346 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": -2.981242670977675e-31,
-            "velocityX": -5.763030958279131e-32,
-            "velocityY": 3.5928963704249306e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.6464505983184665,
-            "y": 5.761517456765686,
-            "heading": 0.04441523610418216,
-            "angularVelocity": 0.571175712894524,
-            "velocityX": -0.43144211544178024,
-            "velocityY": -0.4563021319898165,
-            "timestamp": 0.07776107264628128
-        },
-        {
-            "x": 7.57910774134765,
-            "y": 5.690410071738714,
-            "heading": 0.1294441379757556,
-            "angularVelocity": 1.0934635927458562,
-            "velocityX": -0.8660227370723712,
-            "velocityY": -0.914434209908928,
-            "timestamp": 0.15552214529256256
-        },
-        {
-            "x": 7.4776816969238515,
-            "y": 5.583411802725407,
-            "heading": 0.24873158333085876,
-            "angularVelocity": 1.5340252043296483,
-            "velocityX": -1.3043292867777738,
-            "velocityY": -1.3759875651409672,
-            "timestamp": 0.23328321793884382
-        },
-        {
-            "x": 7.341761998810456,
-            "y": 5.440105324158156,
-            "heading": 0.38885380810835574,
-            "angularVelocity": 1.8019584865410045,
-            "velocityX": -1.747914393255167,
-            "velocityY": -1.842907687489359,
-            "timestamp": 0.3110442905851251
-        },
-        {
-            "x": 7.171108690296084,
-            "y": 5.260470754663091,
-            "heading": 0.5147184350397563,
-            "angularVelocity": 1.6186071339824826,
-            "velocityX": -2.1945853202236263,
-            "velocityY": -2.3100834824152265,
-            "timestamp": 0.3888053632314064
-        },
-        {
-            "x": 6.970033766909185,
-            "y": 5.052418524467747,
-            "heading": 0.5282282166004705,
-            "angularVelocity": 0.17373450623768458,
-            "velocityX": -2.585804394720051,
-            "velocityY": -2.67553189681076,
-            "timestamp": 0.4665664358776877
-        },
-        {
-            "x": 6.768438732548067,
-            "y": 4.838192511419719,
-            "heading": 0.5282282675213732,
-            "angularVelocity": 6.548379670470849e-7,
-            "velocityX": -2.592492972391604,
-            "velocityY": -2.7549261572367425,
-            "timestamp": 0.5443275085239689
-        },
-        {
-            "x": 6.5423880710685225,
-            "y": 4.649952125007829,
-            "heading": 0.5282283002658353,
-            "angularVelocity": 4.210906690145718e-7,
-            "velocityX": -2.9069900116707847,
-            "velocityY": -2.42075346964613,
-            "timestamp": 0.6220885811702502
-        },
-        {
-            "x": 6.292219183977312,
-            "y": 4.49519788912002,
-            "heading": 0.5282283052888641,
-            "angularVelocity": 6.459567356551782e-8,
-            "velocityX": -3.217148099656167,
-            "velocityY": -1.9901247580747212,
-            "timestamp": 0.6998496538165315
-        },
-        {
-            "x": 6.0420493950115075,
-            "y": 4.3404451111724525,
-            "heading": 0.5282283103117955,
-            "angularVelocity": 6.459441933764687e-8,
-            "velocityX": -3.217159697677684,
-            "velocityY": -1.990106009101862,
-            "timestamp": 0.7776107264628128
-        },
-        {
-            "x": 5.780250458218128,
-            "y": 4.206300927357815,
-            "heading": 0.5282283154384368,
-            "angularVelocity": 6.592812032299238e-8,
-            "velocityX": -3.366709432935023,
-            "velocityY": -1.7250814482051704,
-            "timestamp": 0.8553717991090941
-        },
-        {
-            "x": 5.499389832161033,
-            "y": 4.11883296395985,
-            "heading": 0.5282284192975907,
-            "angularVelocity": 0.0000013356188425542589,
-            "velocityX": -3.611840944307328,
-            "velocityY": -1.1248296920470104,
-            "timestamp": 0.9331328717553754
-        },
-        {
-            "x": 5.2375954046767585,
-            "y": 3.9846908767107623,
-            "heading": 0.5282448470390027,
-            "angularVelocity": 0.0002112591919891304,
-            "velocityX": -3.3666514436488617,
-            "velocityY": -1.7250544865716144,
-            "timestamp": 1.0108939444016567
-        },
-        {
-            "x": 4.9990709441074195,
-            "y": 3.8334805845295827,
-            "heading": 0.56802081206443,
-            "angularVelocity": 0.5115151279660829,
-            "velocityX": -3.067401881842984,
-            "velocityY": -1.944549978997939,
-            "timestamp": 1.0886550170479379
-        },
-        {
-            "x": 4.779271759525853,
-            "y": 3.6735037985529155,
-            "heading": 0.6417219019727908,
-            "angularVelocity": 0.9477890080506808,
-            "velocityX": -2.826596613210087,
-            "velocityY": -2.057286255609729,
-            "timestamp": 1.166416089694219
-        },
-        {
-            "x": 4.5625385904493,
-            "y": 3.4912294193947493,
-            "heading": 0.6783171685956464,
-            "angularVelocity": 0.47061164895862556,
-            "velocityX": -2.7871679453603333,
-            "velocityY": -2.3440311836655594,
-            "timestamp": 1.2441771623405002
-        },
-        {
-            "x": 4.372028277853454,
-            "y": 3.267088484433369,
-            "heading": 0.6783172621445687,
-            "angularVelocity": 0.0000012030302442626518,
-            "velocityX": -2.4499445045265382,
-            "velocityY": -2.8824311102413667,
-            "timestamp": 1.3219382349867814
-        },
-        {
-            "x": 4.1999208824099785,
-            "y": 3.028525386131778,
-            "heading": 0.678317151805174,
-            "angularVelocity": -0.0000014189541221835924,
-            "velocityX": -2.213284739864076,
-            "velocityY": -3.067898759405809,
-            "timestamp": 1.3996993076330626
-        },
-        {
-            "x": 4.015537599692405,
-            "y": 2.8383590216563426,
-            "heading": 0.6532286408155417,
-            "angularVelocity": -0.32263586568249475,
-            "velocityX": -2.371151482905778,
-            "velocityY": -2.4455213644037763,
-            "timestamp": 1.4774603802793438
-        },
-        {
-            "x": 3.8266113642227695,
-            "y": 2.6983370404519986,
-            "heading": 0.6187227841336836,
-            "angularVelocity": -0.4437420357974964,
-            "velocityX": -2.429573423311953,
-            "velocityY": -1.8006693637223163,
-            "timestamp": 1.555221452925625
-        },
-        {
-            "x": 3.635277950307217,
-            "y": 2.6086556112169066,
-            "heading": 0.5790475746735078,
-            "angularVelocity": -0.5102194209775099,
-            "velocityX": -2.460529509229992,
-            "velocityY": -1.1532946522360021,
-            "timestamp": 1.6329825255719062
-        },
-        {
-            "x": 3.4425296783447266,
-            "y": 2.5693652629852295,
-            "heading": 0.5361762744054714,
-            "angularVelocity": -0.5513208448531661,
-            "velocityX": -2.4787244491760223,
-            "velocityY": -0.5052701421751292,
-            "timestamp": 1.7107435982181873
-        },
-        {
-            "x": 3.273952117174966,
-            "y": 2.5733990398146904,
-            "heading": 0.49726118807033537,
-            "angularVelocity": -0.5743772146540196,
-            "velocityX": -2.4881638242809143,
-            "velocityY": 0.05953756545439798,
-            "timestamp": 1.7784953915385004
-        },
-        {
-            "x": 3.1059881626789445,
-            "y": 2.6157002802478813,
-            "heading": 0.456882179671172,
-            "angularVelocity": -0.5959843484623638,
-            "velocityX": -2.4791071389347814,
-            "velocityY": 0.6243560260198714,
-            "timestamp": 1.8462471848588136
-        },
-        {
-            "x": 2.9403803042277463,
-            "y": 2.6962017103764078,
-            "heading": 0.41517848324514556,
-            "angularVelocity": -0.6155364217277942,
-            "velocityX": -2.444331734043506,
-            "velocityY": 1.1881815400506988,
-            "timestamp": 1.9139989781791267
-        },
-        {
-            "x": 2.7796953039379972,
-            "y": 2.8146585273254012,
-            "heading": 0.3723609476398507,
-            "angularVelocity": -0.6319764172568042,
-            "velocityX": -2.3716715442508174,
-            "velocityY": 1.7483938231562413,
-            "timestamp": 1.9817507714994398
-        },
-        {
-            "x": 2.628009703912687,
-            "y": 2.970316258543535,
-            "heading": 0.3287788631859365,
-            "angularVelocity": -0.6432609724124856,
-            "velocityX": -2.23884258396202,
-            "velocityY": 2.2974702748047298,
-            "timestamp": 2.0495025648197527
-        },
-        {
-            "x": 2.4923790399755665,
-            "y": 3.160718496478581,
-            "heading": 0.28508218813026037,
-            "angularVelocity": -0.6449523018392979,
-            "velocityX": -2.001875630005736,
-            "velocityY": 2.8102907480968544,
-            "timestamp": 2.1172543581400656
-        },
-        {
-            "x": 2.384956097652455,
-            "y": 3.376988776463332,
-            "heading": 0.24257394609876834,
-            "angularVelocity": -0.627411319291935,
-            "velocityX": -1.5855365158416335,
-            "velocityY": 3.192096760631575,
-            "timestamp": 2.1850061514603785
-        },
-        {
-            "x": 2.3144199991578422,
-            "y": 3.6018152629752547,
-            "heading": 0.17996830737750358,
-            "angularVelocity": -0.9240440090682317,
-            "velocityX": -1.0410956675513523,
-            "velocityY": 3.3183842890918167,
-            "timestamp": 2.2527579447806914
-        },
-        {
-            "x": 2.277461106698391,
-            "y": 3.8129334421080285,
-            "heading": 0.07321465766316056,
-            "angularVelocity": -1.57565791963083,
-            "velocityX": -0.5455042685692306,
-            "velocityY": 3.1160530044520236,
-            "timestamp": 2.3205097381010042
-        },
-        {
-            "x": 2.2642620699526455,
-            "y": 3.9942911908489713,
-            "heading": -0.04721496632936169,
-            "angularVelocity": -1.7775119755599884,
-            "velocityX": -0.1948145738865332,
-            "velocityY": 2.676796286166625,
-            "timestamp": 2.388261531421317
-        },
-        {
-            "x": 2.2685016834700633,
-            "y": 4.1415779064179725,
-            "heading": -0.1677701441610935,
-            "angularVelocity": -1.7793651197065368,
-            "velocityX": 0.06257566493294611,
-            "velocityY": 2.173916118687331,
-            "timestamp": 2.45601332474163
-        },
-        {
-            "x": 2.2803498467055876,
-            "y": 4.251629868834139,
-            "heading": -0.2721018795383186,
-            "angularVelocity": -1.5399110527447033,
-            "velocityX": 0.17487600925202312,
-            "velocityY": 1.624340213341216,
-            "timestamp": 2.523765118061943
-        },
-        {
-            "x": 2.2924250535428135,
-            "y": 4.3241863896986095,
-            "heading": -0.3487735727111741,
-            "angularVelocity": -1.1316555535345214,
-            "velocityX": 0.17822711762236845,
-            "velocityY": 1.0709166105971757,
-            "timestamp": 2.591516911382256
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.6084920452794201,
-            "velocityX": 0.1118043683563332,
-            "velocityY": 0.5286001823165565,
-            "timestamp": 2.6592687047025687
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -7.720140427747849e-31,
-            "velocityX": 0,
-            "velocityY": -1.167304580662352e-31,
-            "timestamp": 2.7270204980228816
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C3 To C1.traj b/src/main/deploy/choreo/C3 To C1.traj
deleted file mode 100644
index 57334e24..00000000
--- a/src/main/deploy/choreo/C3 To C1.traj	
+++ /dev/null
@@ -1,139 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -2.1749467140472343e-39,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 4.203445372005539,
-            "heading": -7.837837605587582e-19,
-            "angularVelocity": -8.185686568731239e-18,
-            "velocityX": 1.377912473786286e-33,
-            "velocityY": 0.7983807350946673,
-            "timestamp": 0.09575052183150974
-        },
-        {
-            "x": 7.68,
-            "y": 4.356336112982689,
-            "heading": -2.3565356003209568e-18,
-            "angularVelocity": -1.6425517161035916e-17,
-            "velocityX": 3.260516050321855e-34,
-            "velocityY": 1.5967614385035824,
-            "timestamp": 0.19150104366301948
-        },
-        {
-            "x": 7.68,
-            "y": 4.5856722168636,
-            "heading": -4.728579560129296e-18,
-            "angularVelocity": -2.4773170043438095e-17,
-            "velocityX": -1.693775140889528e-33,
-            "velocityY": 2.3951420785410225,
-            "timestamp": 0.28725156549452924
-        },
-        {
-            "x": 7.68,
-            "y": 4.891453665444746,
-            "heading": -7.932188825954499e-18,
-            "angularVelocity": -3.345787785122934e-17,
-            "velocityX": -9.607950389092107e-34,
-            "velocityY": 3.1935225284643685,
-            "timestamp": 0.38300208732603896
-        },
-        {
-            "x": 7.68,
-            "y": 5.253672199266821,
-            "heading": -4.35728250993897e-17,
-            "angularVelocity": -3.722239377050204e-16,
-            "velocityX": -3.660377688643743e-33,
-            "velocityY": 3.7829405719528495,
-            "timestamp": 0.4787526091575487
-        },
-        {
-            "x": 7.68,
-            "y": 5.61589073308894,
-            "heading": -3.405116375197899e-17,
-            "angularVelocity": 9.944239639932065e-17,
-            "velocityX": -5.791062266320423e-33,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.5745031309890585
-        },
-        {
-            "x": 7.68,
-            "y": 5.978109266911059,
-            "heading": 5.876472199762529e-18,
-            "angularVelocity": 4.169965362904391e-16,
-            "velocityX": -1.6171107443968654e-32,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.6702536528205683
-        },
-        {
-            "x": 7.68,
-            "y": 6.340327800733179,
-            "heading": 2.547705878984039e-17,
-            "angularVelocity": 2.047047495424479e-16,
-            "velocityX": -4.3724642588945734e-33,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.766004174652078
-        },
-        {
-            "x": 7.68,
-            "y": 6.702546334555254,
-            "heading": 7.687233753276096e-18,
-            "angularVelocity": -1.8579350479017399e-16,
-            "velocityX": -2.6103711792186515e-33,
-            "velocityY": 3.7829405719528495,
-            "timestamp": 0.8617546964835878
-        },
-        {
-            "x": 7.68,
-            "y": 7.0083277831364,
-            "heading": 4.613320516549708e-18,
-            "angularVelocity": -3.210335754994756e-17,
-            "velocityX": 1.2273832122580678e-32,
-            "velocityY": 3.1935225284643685,
-            "timestamp": 0.9575052183150976
-        },
-        {
-            "x": 7.68,
-            "y": 7.237663887017311,
-            "heading": 2.3068261479777784e-18,
-            "angularVelocity": -2.4088582761110343e-17,
-            "velocityX": 8.179933156696061e-33,
-            "velocityY": 2.3951420785410225,
-            "timestamp": 1.0532557401466074
-        },
-        {
-            "x": 7.68,
-            "y": 7.390554627994461,
-            "heading": 7.689558058679801e-19,
-            "angularVelocity": -1.6061221530256843e-17,
-            "velocityX": 9.843160057308665e-33,
-            "velocityY": 1.5967614385035824,
-            "timestamp": 1.1490062619781172
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": -8.030826271263815e-18,
-            "velocityX": 3.1854559448557055e-33,
-            "velocityY": 0.7983807350946673,
-            "timestamp": 1.244756783809627
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 2.257351446612169e-43,
-            "velocityY": -1.1946741683702123e-38,
-            "timestamp": 1.3405073056411367
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C3 To C2.traj b/src/main/deploy/choreo/C3 To C2.traj
deleted file mode 100644
index 5ffe4bce..00000000
--- a/src/main/deploy/choreo/C3 To C2.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -3.817834980565894e-40,
-            "angularVelocity": -1.9855221195343998e-39,
-            "velocityX": -9.621145750064654e-40,
-            "velocityY": 1.4575297691012433e-39,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 4.210500005575846,
-            "heading": 3.479114156010015e-22,
-            "angularVelocity": 3.4766412561552746e-21,
-            "velocityX": 2.7068848151306367e-33,
-            "velocityY": 0.8344065481228213,
-            "timestamp": 0.10007112931184182
-        },
-        {
-            "x": 7.68,
-            "y": 4.377500013629847,
-            "heading": 1.081691491825702e-21,
-            "angularVelocity": 7.332585052389997e-21,
-            "velocityX": 5.1589068200097806e-33,
-            "velocityY": 1.6688130652907358,
-            "timestamp": 0.20014225862368365
-        },
-        {
-            "x": 7.68,
-            "y": 4.628000017966616,
-            "heading": 2.2511586271387426e-21,
-            "angularVelocity": 1.1686358860616172e-20,
-            "velocityX": 5.8498916949463956e-33,
-            "velocityY": 2.503219520548835,
-            "timestamp": 0.3002133879355255
-        },
-        {
-            "x": 7.68,
-            "y": 4.962,
-            "heading": 4.481673966100434e-21,
-            "angularVelocity": 2.228929929167172e-20,
-            "velocityX": 6.576791251337367e-33,
-            "velocityY": 3.3376257900775013,
-            "timestamp": 0.4002845172473673
-        },
-        {
-            "x": 7.68,
-            "y": 5.295999982033384,
-            "heading": 1.4391637904192512e-21,
-            "angularVelocity": -3.0403475801105514e-20,
-            "velocityX": 1.0509973010204301e-32,
-            "velocityY": 3.3376257900775013,
-            "timestamp": 0.5003556465592092
-        },
-        {
-            "x": 7.68,
-            "y": 5.5464999863701525,
-            "heading": 4.7457500532831525e-22,
-            "angularVelocity": -9.63903176962576e-21,
-            "velocityX": 7.983324746463204e-33,
-            "velocityY": 2.503219520548835,
-            "timestamp": 0.600426775871051
-        },
-        {
-            "x": 7.68,
-            "y": 5.713499994424153,
-            "heading": 1.0909640932435146e-22,
-            "angularVelocity": -3.652188228166023e-21,
-            "velocityX": 7.341551448907972e-33,
-            "velocityY": 1.6688130652907358,
-            "timestamp": 0.7004979051828928
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -9.42965601107167e-40,
-            "angularVelocity": -1.09018866193586e-21,
-            "velocityX": 7.44470103227488e-33,
-            "velocityY": 0.8344065481228213,
-            "timestamp": 0.8005690344947346
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 8.249839679439821e-43,
-            "angularVelocity": 9.431229878048832e-39,
-            "velocityX": 1.012759498983901e-41,
-            "velocityY": 5.142787784847508e-39,
-            "timestamp": 0.9006401638065764
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C3 To C4.traj b/src/main/deploy/choreo/C3 To C4.traj
deleted file mode 100644
index 1862a6fc..00000000
--- a/src/main/deploy/choreo/C3 To C4.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -1.6773104267984464e-40,
-            "angularVelocity": -1.9855220924111835e-39,
-            "velocityX": -5.3948128499400994e-40,
-            "velocityY": -2.7550648847397363e-40,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 4.043499994424153,
-            "heading": -2.181197470484939e-22,
-            "angularVelocity": -2.1796471324426728e-21,
-            "velocityX": 2.2751291494664054e-33,
-            "velocityY": -0.8344065481228213,
-            "timestamp": 0.10007112931184182
-        },
-        {
-            "x": 7.68,
-            "y": 3.8764999863701526,
-            "heading": -1.0976856482379068e-21,
-            "angularVelocity": -8.789407177499062e-21,
-            "velocityX": 3.967849397509383e-33,
-            "velocityY": -1.668813065290736,
-            "timestamp": 0.20014225862368365
-        },
-        {
-            "x": 7.68,
-            "y": 3.6259999820333837,
-            "heading": -3.357689035831156e-21,
-            "angularVelocity": -2.2583970074080117e-20,
-            "velocityX": 6.1318806903820435e-33,
-            "velocityY": -2.503219520548835,
-            "timestamp": 0.3002133879355255
-        },
-        {
-            "x": 7.68,
-            "y": 3.2919999999999994,
-            "heading": -1.0603568936345678e-20,
-            "angularVelocity": -7.2407296109284e-20,
-            "velocityX": 8.195284274358776e-33,
-            "velocityY": -3.3376257900775017,
-            "timestamp": 0.4002845172473673
-        },
-        {
-            "x": 7.68,
-            "y": 2.9580000179666155,
-            "heading": -4.785144304302103e-21,
-            "angularVelocity": 5.814288954429956e-20,
-            "velocityX": 7.586727405106972e-33,
-            "velocityY": -3.3376257900775013,
-            "timestamp": 0.5003556465592092
-        },
-        {
-            "x": 7.68,
-            "y": 2.7075000136298466,
-            "heading": -2.6182090767908737e-21,
-            "angularVelocity": 2.165395003165498e-20,
-            "velocityX": 5.326134827450793e-33,
-            "velocityY": -2.503219520548835,
-            "timestamp": 0.600426775871051
-        },
-        {
-            "x": 7.68,
-            "y": 2.540500005575846,
-            "heading": -9.668053802454109e-22,
-            "angularVelocity": 1.6502299039719674e-20,
-            "velocityX": 3.4348416075821124e-33,
-            "velocityY": -1.6688130652907358,
-            "timestamp": 0.7004979051828928
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 1.8181769494337196e-40,
-            "angularVelocity": 9.661181877631647e-21,
-            "velocityX": 1.7280403247932335e-33,
-            "velocityY": -0.8344065481228213,
-            "timestamp": 0.8005690344947346
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -1.6883012335149235e-41,
-            "angularVelocity": -1.9855220810412424e-39,
-            "velocityX": -1.2101303998889038e-42,
-            "velocityY": -7.163168700323315e-39,
-            "timestamp": 0.9006401638065764
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C3 To C5.traj b/src/main/deploy/choreo/C3 To C5.traj
deleted file mode 100644
index 9c555123..00000000
--- a/src/main/deploy/choreo/C3 To C5.traj	
+++ /dev/null
@@ -1,139 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -1.1842498081533988e-39,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 4.050554627994461,
-            "heading": 1.1070575354189048e-18,
-            "angularVelocity": 1.156189558439314e-17,
-            "velocityX": 1.1339790734570631e-33,
-            "velocityY": -0.7983807350946678,
-            "timestamp": 0.09575052183150978
-        },
-        {
-            "x": 7.68,
-            "y": 3.897663887017311,
-            "heading": 3.320268139408775e-18,
-            "angularVelocity": 2.3114345091427606e-17,
-            "velocityX": 5.50963145740529e-34,
-            "velocityY": -1.596761438503583,
-            "timestamp": 0.19150104366301957
-        },
-        {
-            "x": 7.68,
-            "y": 3.6683277831364007,
-            "heading": 6.638014774282681e-18,
-            "angularVelocity": 3.4649906568297676e-17,
-            "velocityX": 4.767145470149766e-34,
-            "velocityY": -2.3951420785410233,
-            "timestamp": 0.28725156549452935
-        },
-        {
-            "x": 7.68,
-            "y": 3.362546334555254,
-            "heading": 1.1056344671920077e-17,
-            "angularVelocity": 4.614418609717799e-17,
-            "velocityX": -5.447572473811757e-34,
-            "velocityY": -3.19352252846437,
-            "timestamp": 0.38300208732603913
-        },
-        {
-            "x": 7.68,
-            "y": 3.0003278007331793,
-            "heading": 2.1759975564527066e-17,
-            "angularVelocity": 1.11786658577609e-16,
-            "velocityX": -1.9220305893434716e-33,
-            "velocityY": -3.7829405719528495,
-            "timestamp": 0.4787526091575489
-        },
-        {
-            "x": 7.68,
-            "y": 2.63810926691106,
-            "heading": -1.7246492437320545e-17,
-            "angularVelocity": -4.0737603572006183e-16,
-            "velocityX": 5.877602560652089e-33,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.5745031309890587
-        },
-        {
-            "x": 7.68,
-            "y": 2.27589073308894,
-            "heading": -9.722837303851897e-18,
-            "angularVelocity": 7.857560449339153e-17,
-            "velocityX": 3.350655179860002e-33,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.6702536528205685
-        },
-        {
-            "x": 7.68,
-            "y": 1.9136721992668206,
-            "heading": -4.541786403704224e-17,
-            "angularVelocity": -3.727919811862982e-16,
-            "velocityX": 1.4553646654154268e-32,
-            "velocityY": -3.782940571953311,
-            "timestamp": 0.7660041746520783
-        },
-        {
-            "x": 7.68,
-            "y": 1.5514536654447455,
-            "heading": -1.11893051502424e-17,
-            "angularVelocity": 3.5747647356983727e-16,
-            "velocityX": -5.35720215261278e-33,
-            "velocityY": -3.7829405719528495,
-            "timestamp": 0.861754696483588
-        },
-        {
-            "x": 7.68,
-            "y": 1.2456722168635992,
-            "heading": -6.7032993083141955e-18,
-            "angularVelocity": 4.6850980602319425e-17,
-            "velocityX": -4.3527390380082413e-33,
-            "velocityY": -3.19352252846437,
-            "timestamp": 0.9575052183150978
-        },
-        {
-            "x": 7.68,
-            "y": 1.016336112982689,
-            "heading": -3.348658686079055e-18,
-            "angularVelocity": 3.5035220254491646e-17,
-            "velocityX": -6.634490282826358e-33,
-            "velocityY": -2.395142078541024,
-            "timestamp": 1.0532557401466076
-        },
-        {
-            "x": 7.68,
-            "y": 0.8634453720055388,
-            "heading": -1.115588951241814e-18,
-            "angularVelocity": 2.3321750022200153e-17,
-            "velocityX": -4.820835446954302e-33,
-            "velocityY": -1.596761438503583,
-            "timestamp": 1.1490062619781174
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 1.1650996045214628e-17,
-            "velocityX": -3.709395222464697e-33,
-            "velocityY": -0.7983807350946678,
-            "timestamp": 1.2447567838096272
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 6.978360472267901e-43,
-            "velocityY": -3.6401965239950315e-38,
-            "timestamp": 1.340507305641137
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C3 To S1.traj b/src/main/deploy/choreo/C3 To S1.traj
deleted file mode 100644
index e393ae58..00000000
--- a/src/main/deploy/choreo/C3 To S1.traj	
+++ /dev/null
@@ -1,157 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -7.709535672280055e-33,
-            "angularVelocity": 3.0776034223095e-32,
-            "velocityX": -2.0787724059290825e-32,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.653625318689537,
-            "y": 4.193887618013532,
-            "heading": -0.05699658004084171,
-            "angularVelocity": -0.6065329552716214,
-            "velocityX": -0.28066795214939116,
-            "velocityY": 0.7117891037630026,
-            "timestamp": 0.0939711182145365
-        },
-        {
-            "x": 7.600609429037226,
-            "y": 4.328102182971276,
-            "heading": -0.1617251039431606,
-            "angularVelocity": -1.1144756590341216,
-            "velocityX": -0.5641721697008566,
-            "velocityY": 1.4282533560080801,
-            "timestamp": 0.187942236429073
-        },
-        {
-            "x": 7.520673672603614,
-            "y": 4.5302373715153195,
-            "heading": -0.299017155202882,
-            "angularVelocity": -1.4610026343017766,
-            "velocityX": -0.8506417498525306,
-            "velocityY": 2.151035258328725,
-            "timestamp": 0.2819133546436095
-        },
-        {
-            "x": 7.413680530437971,
-            "y": 4.80084724043498,
-            "heading": -0.43363333911195256,
-            "angularVelocity": -1.4325272111984575,
-            "velocityX": -1.1385747472045327,
-            "velocityY": 2.8797131933862543,
-            "timestamp": 0.375884472858146
-        },
-        {
-            "x": 7.279380160390009,
-            "y": 5.1284214711267,
-            "heading": -0.4371879316114584,
-            "angularVelocity": -0.037826436111898676,
-            "velocityX": -1.4291664566697349,
-            "velocityY": 3.4859032957750506,
-            "timestamp": 0.46985559107268254
-        },
-        {
-            "x": 7.147763767829931,
-            "y": 5.458645997615663,
-            "heading": -0.43718797458387054,
-            "angularVelocity": -4.5729382513446123e-7,
-            "velocityX": -1.4006047289934083,
-            "velocityY": 3.514106597466034,
-            "timestamp": 0.563826709287219
-        },
-        {
-            "x": 6.993957305230812,
-            "y": 5.779137314807096,
-            "heading": -0.43718801851214967,
-            "angularVelocity": -4.6746574877776706e-7,
-            "velocityX": -1.6367418577267254,
-            "velocityY": 3.410529993478958,
-            "timestamp": 0.6577978275017555
-        },
-        {
-            "x": 6.777425503833482,
-            "y": 6.0610683140345785,
-            "heading": -0.43718807107725144,
-            "angularVelocity": -5.593750798607861e-7,
-            "velocityX": -2.3042377861566687,
-            "velocityY": 3.000187766030755,
-            "timestamp": 0.751768945716292
-        },
-        {
-            "x": 6.507457353311083,
-            "y": 6.292343710685516,
-            "heading": -0.4371880918313372,
-            "angularVelocity": -2.2085600557820128e-7,
-            "velocityX": -2.87288430372895,
-            "velocityY": 2.4611327506280603,
-            "timestamp": 0.8457400639308286
-        },
-        {
-            "x": 6.1957042493791805,
-            "y": 6.462993619393473,
-            "heading": -0.4369141825473297,
-            "angularVelocity": 0.0029148241418374306,
-            "velocityX": -3.3175417070186306,
-            "velocityY": 1.8159825268692036,
-            "timestamp": 0.9397111821453651
-        },
-        {
-            "x": 5.885878112264826,
-            "y": 6.564806359719212,
-            "heading": -0.3410861416211598,
-            "angularVelocity": 1.0197605684269313,
-            "velocityX": -3.297035759508787,
-            "velocityY": 1.083447151211916,
-            "timestamp": 1.0336823003599016
-        },
-        {
-            "x": 5.602663899704114,
-            "y": 6.605182844765102,
-            "heading": -0.13545254769192583,
-            "angularVelocity": 2.1882637754695176,
-            "velocityX": -3.0138431673669435,
-            "velocityY": 0.4296690920896744,
-            "timestamp": 1.127653418574438
-        },
-        {
-            "x": 5.388992479168701,
-            "y": 6.621862905384601,
-            "heading": 0.0527880784098822,
-            "angularVelocity": 2.0031753338515554,
-            "velocityX": -2.2737988500636965,
-            "velocityY": 0.17750199142483714,
-            "timestamp": 1.2216245367889744
-        },
-        {
-            "x": 5.246852509457665,
-            "y": 6.628278045699216,
-            "heading": 0.19017469868317557,
-            "angularVelocity": 1.4620089968455965,
-            "velocityX": -1.5125920858633375,
-            "velocityY": 0.06826714884852915,
-            "timestamp": 1.3155956550035108
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": 0.7749753615846516,
-            "velocityX": -0.7539817638001085,
-            "velocityY": 0.01832429296896703,
-            "timestamp": 1.4095667732180472
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": 6.971645129510918e-32,
-            "velocityX": 3.634932162806737e-32,
-            "velocityY": 0,
-            "timestamp": 1.5035378914325837
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C3 To S2.traj b/src/main/deploy/choreo/C3 To S2.traj
deleted file mode 100644
index 67cbd06a..00000000
--- a/src/main/deploy/choreo/C3 To S2.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 1.358270377512338e-32,
-            "angularVelocity": -4.2989327725546786e-32,
-            "velocityX": 9.28706448380456e-34,
-            "velocityY": 4.1113328098198935e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 7.585313636391529,
-            "y": 4.127746947940637,
-            "heading": -0.08924586799649764,
-            "angularVelocity": -0.8241960155421162,
-            "velocityX": -0.8744396280098535,
-            "velocityY": 0.006898151480975746,
-            "timestamp": 0.10828233370892486
-        },
-        {
-            "x": 7.394793136195202,
-            "y": 4.129317895600586,
-            "heading": -0.2483761167773939,
-            "angularVelocity": -1.4695864351119023,
-            "velocityX": -1.7594790735529262,
-            "velocityY": 0.014507885138232623,
-            "timestamp": 0.21656466741784972
-        },
-        {
-            "x": 7.106938899563738,
-            "y": 4.131715387581193,
-            "heading": -0.440143741107867,
-            "angularVelocity": -1.7709964106053178,
-            "velocityX": -2.6583674988502586,
-            "velocityY": 0.022141118486160887,
-            "timestamp": 0.32484700112677456
-        },
-        {
-            "x": 6.726884682874397,
-            "y": 4.136782702937484,
-            "heading": -0.5152103460374668,
-            "angularVelocity": -0.6932488648738155,
-            "velocityX": -3.509845084341914,
-            "velocityY": 0.04679724921622947,
-            "timestamp": 0.43312933483569943
-        },
-        {
-            "x": 6.317264615210477,
-            "y": 4.138917325587368,
-            "heading": -0.515210397277194,
-            "angularVelocity": -4.7320486478177106e-7,
-            "velocityX": -3.782889171607859,
-            "velocityY": 0.019713489511795114,
-            "timestamp": 0.5414116685446243
-        },
-        {
-            "x": 5.907644548289307,
-            "y": 4.14105209076814,
-            "heading": -0.5152104485168384,
-            "angularVelocity": -4.732041015646636e-7,
-            "velocityX": -3.7828891647484775,
-            "velocityY": 0.019714805801191258,
-            "timestamp": 0.6496940022535491
-        },
-        {
-            "x": 5.500258277815471,
-            "y": 4.183825709430579,
-            "heading": -0.5152105024899405,
-            "angularVelocity": -4.984479029228616e-7,
-            "velocityX": -3.762259793632967,
-            "velocityY": 0.3950193646307994,
-            "timestamp": 0.7579763359624739
-        },
-        {
-            "x": 5.114610765355458,
-            "y": 4.3219164843017905,
-            "heading": -0.5152109227554116,
-            "angularVelocity": -0.000003881200715936794,
-            "velocityX": -3.5614998241234512,
-            "velocityY": 1.2752844359857862,
-            "timestamp": 0.8662586696713988
-        },
-        {
-            "x": 4.790273412388094,
-            "y": 4.533322369220829,
-            "heading": -0.5901314422767667,
-            "angularVelocity": -0.6918997490647923,
-            "velocityX": -2.9952933397170605,
-            "velocityY": 1.952358041038544,
-            "timestamp": 0.9745410033803236
-        },
-        {
-            "x": 4.547771251602889,
-            "y": 4.790487049727204,
-            "heading": -0.7662492650374113,
-            "angularVelocity": -1.6264686650923976,
-            "velocityX": -2.239535780943542,
-            "velocityY": 2.3749458632620883,
-            "timestamp": 1.0828233370892484
-        },
-        {
-            "x": 4.394005608234175,
-            "y": 5.057835455573767,
-            "heading": -0.8024780004938983,
-            "angularVelocity": -0.3345766037318139,
-            "velocityX": -1.4200436775037824,
-            "velocityY": 2.468993756315091,
-            "timestamp": 1.1911056707981733
-        },
-        {
-            "x": 4.299277084870805,
-            "y": 5.304155354811625,
-            "heading": -0.5153879845736841,
-            "angularVelocity": 2.651309831315093,
-            "velocityX": -0.8748289782709269,
-            "velocityY": 2.2747930414945974,
-            "timestamp": 1.2993880045070982
-        },
-        {
-            "x": 4.268346904630433,
-            "y": 5.476762087961931,
-            "heading": -0.20205360473699635,
-            "angularVelocity": 2.893679597624539,
-            "velocityX": -0.2856438273995415,
-            "velocityY": 1.5940433424190128,
-            "timestamp": 1.4076703382160232
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 2.864290108945775e-30,
-            "angularVelocity": 1.8659886411401074,
-            "velocityX": -0.04937928882107947,
-            "velocityY": 0.7687118404911842,
-            "timestamp": 1.515952671924948
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -1.339793214188515e-31,
-            "velocityX": -1.755749143439243e-31,
-            "velocityY": 0,
-            "timestamp": 1.624235005633873
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C3 To S3.traj b/src/main/deploy/choreo/C3 To S3.traj
deleted file mode 100644
index c2db4e34..00000000
--- a/src/main/deploy/choreo/C3 To S3.traj	
+++ /dev/null
@@ -1,166 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 4.8594393424661863e-32,
-            "velocityX": 8.070887902586468e-34,
-            "velocityY": -1.5484627527783442e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.613090966537703,
-            "y": 4.130415229492922,
-            "heading": -0.02261774188189381,
-            "angularVelocity": -0.25176724339812206,
-            "velocityX": -0.7447915447554241,
-            "velocityY": 0.038016302406854975,
-            "timestamp": 0.08983591977315897
-        },
-        {
-            "x": 7.479193313628693,
-            "y": 4.137259099498979,
-            "heading": -0.06454119971556918,
-            "angularVelocity": -0.46666698504702897,
-            "velocityX": -1.4904689933291897,
-            "velocityY": 0.0761818883097586,
-            "timestamp": 0.17967183954631794
-        },
-        {
-            "x": 7.278200146895047,
-            "y": 4.147551727639681,
-            "heading": -0.12012980810986089,
-            "angularVelocity": -0.6187793093216658,
-            "velocityX": -2.237336326507015,
-            "velocityY": 0.11457141160316474,
-            "timestamp": 0.2695077593194769
-        },
-        {
-            "x": 7.010006404514947,
-            "y": 4.161329433657577,
-            "heading": -0.1775477987284744,
-            "angularVelocity": -0.6391429036434741,
-            "velocityX": -2.985373145364283,
-            "velocityY": 0.15336522465649807,
-            "timestamp": 0.35934367909263587
-        },
-        {
-            "x": 6.675632733992502,
-            "y": 4.178685588756415,
-            "heading": -0.19285230682653456,
-            "angularVelocity": -0.17036067687364617,
-            "velocityX": -3.7220487235639528,
-            "velocityY": 0.19319839038333309,
-            "timestamp": 0.44917959886579484
-        },
-        {
-            "x": 6.3362226416697665,
-            "y": 4.195852203162663,
-            "heading": -0.19285232652882817,
-            "angularVelocity": -2.193200640663355e-7,
-            "velocityX": -3.7781111740035236,
-            "velocityY": 0.1910885362012055,
-            "timestamp": 0.5390155186389538
-        },
-        {
-            "x": 5.996812544590127,
-            "y": 4.213018723498308,
-            "heading": -0.19285234623213549,
-            "angularVelocity": -2.1933156264800868e-7,
-            "velocityX": -3.7781112269558137,
-            "velocityY": 0.1910874890356692,
-            "timestamp": 0.6288514384121128
-        },
-        {
-            "x": 5.65703115405788,
-            "y": 4.206499194085455,
-            "heading": -0.1928524148916534,
-            "angularVelocity": -7.642946895727943e-7,
-            "velocityX": -3.782244244720454,
-            "velocityY": -0.07257152185432766,
-            "timestamp": 0.7186873581852717
-        },
-        {
-            "x": 5.325195518902672,
-            "y": 4.133157682156235,
-            "heading": -0.192852601151229,
-            "angularVelocity": -0.0000020733763948999505,
-            "velocityX": -3.6937968241559735,
-            "velocityY": -0.8163940672507154,
-            "timestamp": 0.8085232779584307
-        },
-        {
-            "x": 5.022921580906399,
-            "y": 3.999719932132448,
-            "heading": -0.21619143682617745,
-            "angularVelocity": -0.2597940304978627,
-            "velocityX": -3.364733602738329,
-            "velocityY": -1.485349628067107,
-            "timestamp": 0.8983591977315897
-        },
-        {
-            "x": 4.76405537852928,
-            "y": 3.8156128777716756,
-            "heading": -0.2707600447886353,
-            "angularVelocity": -0.6074252715762222,
-            "velocityX": -2.881544520603651,
-            "velocityY": -2.0493701720293274,
-            "timestamp": 0.9881951175047486
-        },
-        {
-            "x": 4.5584097754694755,
-            "y": 3.590752248351908,
-            "heading": -0.3464948618413025,
-            "angularVelocity": -0.8430349156856024,
-            "velocityX": -2.2891244791432657,
-            "velocityY": -2.5030147182477354,
-            "timestamp": 1.0780310372779076
-        },
-        {
-            "x": 4.420003796304514,
-            "y": 3.3682412112408735,
-            "heading": -0.42679529224450924,
-            "angularVelocity": -0.8938566066393836,
-            "velocityX": -1.5406529984228516,
-            "velocityY": -2.476860454851527,
-            "timestamp": 1.1678669570510665
-        },
-        {
-            "x": 4.3340794956684014,
-            "y": 3.187695881574611,
-            "heading": -0.4942860320434666,
-            "angularVelocity": -0.7512667535241839,
-            "velocityX": -0.9564581834687845,
-            "velocityY": -2.009723172222817,
-            "timestamp": 1.2577028768242253
-        },
-        {
-            "x": 4.284833149556787,
-            "y": 3.063288277698013,
-            "heading": -0.5416494132844137,
-            "angularVelocity": -0.5272209752980105,
-            "velocityX": -0.5481810197605272,
-            "velocityY": -1.3848314147664058,
-            "timestamp": 1.3475387965973842
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -0.27105624093568903,
-            "velocityX": -0.24303362855860308,
-            "velocityY": -0.7044874461979944,
-            "timestamp": 1.437374716370543
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 6.221864662981848e-31,
-            "velocityX": 0,
-            "velocityY": 2.6923638006152617e-32,
-            "timestamp": 1.527210636143702
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C3 To S4.traj b/src/main/deploy/choreo/C3 To S4.traj
deleted file mode 100644
index 2274eedc..00000000
--- a/src/main/deploy/choreo/C3 To S4.traj	
+++ /dev/null
@@ -1,238 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 3.185769476987751e-31,
-            "velocityX": 7.333541525795099e-32,
-            "velocityY": -1.3517842061565195e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 7.679078910576486,
-            "y": 4.080614551713116,
-            "heading": -0.023748331346269917,
-            "angularVelocity": -0.3167839884646853,
-            "velocityX": -0.012286605616975374,
-            "velocityY": -0.6187452541731726,
-            "timestamp": 0.07496695606800095
-        },
-        {
-            "x": 7.6772274783441,
-            "y": 3.987757014244699,
-            "heading": -0.06889026621284731,
-            "angularVelocity": -0.6021577670253255,
-            "velocityX": -0.024696644088144176,
-            "velocityY": -1.2386462294692595,
-            "timestamp": 0.1499339121360019
-        },
-        {
-            "x": 7.674435217742111,
-            "y": 3.848312298051719,
-            "heading": -0.1318457680879735,
-            "angularVelocity": -0.8397766853177616,
-            "velocityX": -0.037246551660047825,
-            "velocityY": -1.8600824083946148,
-            "timestamp": 0.22490086820400285
-        },
-        {
-            "x": 7.670687411814683,
-            "y": 3.6621317365631327,
-            "heading": -0.20653378464827088,
-            "angularVelocity": -0.996279167218362,
-            "velocityX": -0.049992771802326974,
-            "velocityY": -2.483501681990474,
-            "timestamp": 0.2998678242720038
-        },
-        {
-            "x": 7.665952301925262,
-            "y": 3.4291021726627307,
-            "heading": -0.27999342706200125,
-            "angularVelocity": -0.9798936260280143,
-            "velocityX": -0.06316262707958344,
-            "velocityY": -3.1084303821676644,
-            "timestamp": 0.3748347803400048
-        },
-        {
-            "x": 7.659724245559776,
-            "y": 3.1517739981457797,
-            "heading": -0.2973184560258276,
-            "angularVelocity": -0.23110220652567398,
-            "velocityX": -0.0830773542389579,
-            "velocityY": -3.699338869586643,
-            "timestamp": 0.44980173640800575
-        },
-        {
-            "x": 7.61848609595722,
-            "y": 2.8711927485616604,
-            "heading": -0.2973184850947423,
-            "angularVelocity": -3.8775636846908307e-7,
-            "velocityX": -0.5500843540339733,
-            "velocityY": -3.742732322352917,
-            "timestamp": 0.5247686924760067
-        },
-        {
-            "x": 7.5316071002697536,
-            "y": 2.6012326390018865,
-            "heading": -0.2973185190564447,
-            "angularVelocity": -4.530222940715824e-7,
-            "velocityX": -1.1588972027711093,
-            "velocityY": -3.6010547008857787,
-            "timestamp": 0.5997356485440076
-        },
-        {
-            "x": 7.401459139481823,
-            "y": 2.3492645367371554,
-            "heading": -0.29731856454052086,
-            "angularVelocity": -6.067216612944958e-7,
-            "velocityX": -1.7360710320141561,
-            "velocityY": -3.3610555300681284,
-            "timestamp": 0.6747026046120086
-        },
-        {
-            "x": 7.231595664022054,
-            "y": 2.1221680676798096,
-            "heading": -0.2973186386883636,
-            "angularVelocity": -9.89073675332688e-7,
-            "velocityX": -2.265844638345654,
-            "velocityY": -3.0292875817359826,
-            "timestamp": 0.7496695606800096
-        },
-        {
-            "x": 7.026654548839687,
-            "y": 1.9261437915792998,
-            "heading": -0.2973188151543491,
-            "angularVelocity": -0.000002353916908579506,
-            "velocityX": -2.7337526549226907,
-            "velocityY": -2.614809062311352,
-            "timestamp": 0.8246365167480105
-        },
-        {
-            "x": 6.792231559753418,
-            "y": 1.766543984413147,
-            "heading": -0.2973195709317279,
-            "angularVelocity": -0.00001008147347055336,
-            "velocityX": -3.127017573897922,
-            "velocityY": -2.1289354075064555,
-            "timestamp": 0.8996034728160115
-        },
-        {
-            "x": 6.610935711613446,
-            "y": 1.6724691195207,
-            "heading": -0.2973321063250889,
-            "angularVelocity": -0.00023216320074468077,
-            "velocityX": -3.3577106919370485,
-            "velocityY": -1.7423243992220112,
-            "timestamp": 0.9535973587619662
-        },
-        {
-            "x": 6.419882816475647,
-            "y": 1.600658398177247,
-            "heading": -0.29770902924967546,
-            "angularVelocity": -0.006980844552986395,
-            "velocityX": -3.538417207626563,
-            "velocityY": -1.329978757508455,
-            "timestamp": 1.007591244707921
-        },
-        {
-            "x": 6.223144239006287,
-            "y": 1.5524565272909876,
-            "heading": -0.30199019992690734,
-            "angularVelocity": -0.07928991592723345,
-            "velocityX": -3.6437195438439516,
-            "velocityY": -0.892728316211746,
-            "timestamp": 1.0615851306538757
-        },
-        {
-            "x": 6.027009837481886,
-            "y": 1.528287215105613,
-            "heading": -0.31920772077283077,
-            "angularVelocity": -0.31887908314582397,
-            "velocityX": -3.63252983348375,
-            "velocityY": -0.44763053745692566,
-            "timestamp": 1.1155790165998305
-        },
-        {
-            "x": 5.840489459577601,
-            "y": 1.5253276945335545,
-            "heading": -0.3622266817701244,
-            "angularVelocity": -0.7967376350788925,
-            "velocityX": -3.454472198777908,
-            "velocityY": -0.05481214252704887,
-            "timestamp": 1.1695729025457853
-        },
-        {
-            "x": 5.673855769295343,
-            "y": 1.5359985529910274,
-            "heading": -0.41631972448131044,
-            "angularVelocity": -1.0018364443214895,
-            "velocityX": -3.086158504113808,
-            "velocityY": 0.19763086635687102,
-            "timestamp": 1.22356678849174
-        },
-        {
-            "x": 5.530708201179271,
-            "y": 1.5529237053226916,
-            "heading": -0.47196047387840134,
-            "angularVelocity": -1.0305009247305976,
-            "velocityX": -2.651181066303626,
-            "velocityY": 0.3134642383140704,
-            "timestamp": 1.2775606744376948
-        },
-        {
-            "x": 5.411784437145291,
-            "y": 1.57153818466476,
-            "heading": -0.5236032237572267,
-            "angularVelocity": -0.956455512954414,
-            "velocityX": -2.2025413053807186,
-            "velocityY": 0.34475161429762524,
-            "timestamp": 1.3315545603836496
-        },
-        {
-            "x": 5.3170603467926,
-            "y": 1.5890782395606275,
-            "heading": -0.5679992830659408,
-            "angularVelocity": -0.8222423433861833,
-            "velocityX": -1.754348454332506,
-            "velocityY": 0.32485261226475975,
-            "timestamp": 1.3855484463296044
-        },
-        {
-            "x": 5.246334967071984,
-            "y": 1.6037618746917708,
-            "heading": -0.6030801100679523,
-            "angularVelocity": -0.6497185077054303,
-            "velocityX": -1.309877562644932,
-            "velocityY": 0.2719499601462664,
-            "timestamp": 1.4395423322755592
-        },
-        {
-            "x": 5.199384048260516,
-            "y": 1.6143645569232876,
-            "heading": -0.6274260701015184,
-            "angularVelocity": -0.45090216432919045,
-            "velocityX": -0.8695599138477248,
-            "velocityY": 0.19636820069091412,
-            "timestamp": 1.493536218221514
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -0.23287692075121041,
-            "velocityX": -0.4330869662524824,
-            "velocityY": 0.10437187429616973,
-            "timestamp": 1.5475301041674687
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0,
-            "velocityX": 3.9288115092123874e-33,
-            "velocityY": 2.0888895254257856e-32,
-            "timestamp": 1.6015239901134235
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C3 To W1.traj b/src/main/deploy/choreo/C3 To W1.traj
deleted file mode 100644
index 47660d82..00000000
--- a/src/main/deploy/choreo/C3 To W1.traj	
+++ /dev/null
@@ -1,220 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 1.832751405105417e-34,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.608571974046609,
-            "y": 4.129750388677263,
-            "heading": -0.02440208863451916,
-            "angularVelocity": -0.26295738388657636,
-            "velocityX": -0.7697098032139116,
-            "velocityY": 0.029638242122484983,
-            "timestamp": 0.09279864392416992
-        },
-        {
-            "x": 7.46571594445247,
-            "y": 4.135252523590067,
-            "heading": -0.07320447592180841,
-            "angularVelocity": -0.5258954788948677,
-            "velocityX": -1.5394193659863489,
-            "velocityY": 0.05929111331948823,
-            "timestamp": 0.18559728784833984
-        },
-        {
-            "x": 7.25143160309568,
-            "y": 4.143508368339934,
-            "heading": -0.1463869418885076,
-            "angularVelocity": -0.7886156830752087,
-            "velocityX": -2.3091322490864354,
-            "velocityY": 0.08896514432486853,
-            "timestamp": 0.27839593177250976
-        },
-        {
-            "x": 6.965717910609067,
-            "y": 4.154520147774138,
-            "heading": -0.24389107124776152,
-            "angularVelocity": -1.050706402983198,
-            "velocityX": -3.078856332427471,
-            "velocityY": 0.1186631503279318,
-            "timestamp": 0.3711945756966797
-        },
-        {
-            "x": 6.614926201978592,
-            "y": 4.168029747138803,
-            "heading": -0.24389108357148506,
-            "angularVelocity": -1.3280068591073525e-7,
-            "velocityX": -3.780138305869253,
-            "velocityY": 0.1455797067002448,
-            "timestamp": 0.4639932196208496
-        },
-        {
-            "x": 6.264134491970206,
-            "y": 4.181539310738243,
-            "heading": -0.2438910958936344,
-            "angularVelocity": -1.3278372185354406e-7,
-            "velocityX": -3.7801383207176356,
-            "velocityY": 0.14557932129349355,
-            "timestamp": 0.5567918635450195
-        },
-        {
-            "x": 5.913342805476888,
-            "y": 4.195049484934711,
-            "heading": -0.2438911082157909,
-            "angularVelocity": -1.32783799697389e-7,
-            "velocityX": -3.780138067318815,
-            "velocityY": 0.14558590109896277,
-            "timestamp": 0.6495905074691894
-        },
-        {
-            "x": 5.568285864608835,
-            "y": 4.259648543779433,
-            "heading": -0.24389114467773698,
-            "angularVelocity": -3.929146432741106e-7,
-            "velocityX": -3.718340336417024,
-            "velocityY": 0.6961207202291138,
-            "timestamp": 0.7423891513933594
-        },
-        {
-            "x": 5.223229300660808,
-            "y": 4.324249615558004,
-            "heading": -0.2438911811402195,
-            "angularVelocity": -3.929204239211486e-7,
-            "velocityX": -3.7183362747195923,
-            "velocityY": 0.6961424116429128,
-            "timestamp": 0.8351877953175293
-        },
-        {
-            "x": 4.889178228675737,
-            "y": 4.432171862538056,
-            "heading": -0.2438912192984801,
-            "angularVelocity": -4.111941617608203e-7,
-            "velocityX": -3.5997408783046705,
-            "velocityY": 1.1629722420106654,
-            "timestamp": 0.9279864392416992
-        },
-        {
-            "x": 4.584073695522117,
-            "y": 4.605805251226935,
-            "heading": -0.24389116821114407,
-            "angularVelocity": 5.505181312773355e-7,
-            "velocityX": -3.287812410307864,
-            "velocityY": 1.8710767889103372,
-            "timestamp": 1.0207850831658691
-        },
-        {
-            "x": 4.320679858830311,
-            "y": 4.837885010533044,
-            "heading": -0.24389110572504064,
-            "angularVelocity": 6.733514712028696e-7,
-            "velocityX": -2.8383371303038154,
-            "velocityY": 2.5008960205899418,
-            "timestamp": 1.113583727090039
-        },
-        {
-            "x": 4.066259556785684,
-            "y": 5.079768532204951,
-            "heading": -0.24389104453640137,
-            "angularVelocity": 6.593699721479525e-7,
-            "velocityX": -2.7416381456233947,
-            "velocityY": 2.606541555386963,
-            "timestamp": 1.206382371014209
-        },
-        {
-            "x": 3.8118393280605467,
-            "y": 5.321652130996455,
-            "heading": -0.243890983347767,
-            "angularVelocity": 6.593699195322184e-7,
-            "velocityX": -2.741637355531139,
-            "velocityY": 2.606542386429263,
-            "timestamp": 1.299181014938379
-        },
-        {
-            "x": 3.557419123484207,
-            "y": 5.563535755188363,
-            "heading": -0.24389092215913197,
-            "angularVelocity": 6.593699267467298e-7,
-            "velocityX": -2.7416370953032203,
-            "velocityY": 2.606542660144498,
-            "timestamp": 1.3919796588625488
-        },
-        {
-            "x": 3.3029989365055474,
-            "y": 5.805419397890013,
-            "heading": -0.24389086097049373,
-            "angularVelocity": 6.593699609263459e-7,
-            "velocityX": -2.7416369056702816,
-            "velocityY": 2.606542859605842,
-            "timestamp": 1.4847783027867187
-        },
-        {
-            "x": 3.0485787435735707,
-            "y": 6.04730303432977,
-            "heading": -0.24389079978182,
-            "angularVelocity": 6.593703437871394e-7,
-            "velocityX": -2.741636969823341,
-            "velocityY": 2.6065427921275584,
-            "timestamp": 1.5775769467108887
-        },
-        {
-            "x": 2.7984729856087975,
-            "y": 6.275491677578651,
-            "heading": -0.21231080129448254,
-            "angularVelocity": 0.3403066807004949,
-            "velocityX": -2.6951445343225506,
-            "velocityY": 2.45896527793385,
-            "timestamp": 1.6703755906350586
-        },
-        {
-            "x": 2.596978431795094,
-            "y": 6.469397893884434,
-            "heading": -0.03732218251039644,
-            "angularVelocity": 1.8856807748945335,
-            "velocityX": -2.171309248638947,
-            "velocityY": 2.089537175394848,
-            "timestamp": 1.7631742345592285
-        },
-        {
-            "x": 2.4472498512530994,
-            "y": 6.613867328015145,
-            "heading": 0.15789573469158452,
-            "angularVelocity": 2.1036720898801096,
-            "velocityX": -1.6134781092743464,
-            "velocityY": 1.5568054447946784,
-            "timestamp": 1.8559728784833984
-        },
-        {
-            "x": 2.3487140682465175,
-            "y": 6.7094536627437265,
-            "heading": 0.3099829221841124,
-            "angularVelocity": 1.6388945038552638,
-            "velocityX": -1.0618235228426263,
-            "velocityY": 1.0300402105734285,
-            "timestamp": 1.9487715224075683
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.8945936525078311,
-            "velocityX": -0.5249437511858832,
-            "velocityY": 0.5123602592202229,
-            "timestamp": 2.0415701663317383
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0,
-            "velocityX": 1.4564927652554838e-31,
-            "velocityY": 0,
-            "timestamp": 2.134368810255908
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C3 To W2.traj b/src/main/deploy/choreo/C3 To W2.traj
deleted file mode 100644
index 2f31d71f..00000000
--- a/src/main/deploy/choreo/C3 To W2.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -8.248091288877123e-33,
-            "velocityX": -1.7499589770527321e-34,
-            "velocityY": 1.243057151683689e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 7.599512207946833,
-            "y": 4.126338330595212,
-            "heading": -0.05672955044148203,
-            "angularVelocity": -0.5720722032058021,
-            "velocityX": -0.8116550928518662,
-            "velocityY": -0.0066724074357217035,
-            "timestamp": 0.09916501819801522
-        },
-        {
-            "x": 7.438184902121127,
-            "y": 4.125091465076226,
-            "heading": -0.16291927611759752,
-            "angularVelocity": -1.070838563898341,
-            "velocityX": -1.6268570183042166,
-            "velocityY": -0.01257364281924666,
-            "timestamp": 0.19833003639603045
-        },
-        {
-            "x": 7.19545269041534,
-            "y": 4.123364615129754,
-            "heading": -0.30423484322627714,
-            "angularVelocity": -1.4250546178138856,
-            "velocityX": -2.4477604715515007,
-            "velocityY": -0.017413902380612675,
-            "timestamp": 0.2974950545940457
-        },
-        {
-            "x": 6.870785634071806,
-            "y": 4.1216134281783505,
-            "heading": -0.4343749203987102,
-            "angularVelocity": -1.3123587282822462,
-            "velocityX": -3.274007933878773,
-            "velocityY": -0.01765932163604032,
-            "timestamp": 0.3966600727920609
-        },
-        {
-            "x": 6.495672194218458,
-            "y": 4.117557524267507,
-            "heading": -0.4343749552288927,
-            "angularVelocity": -3.5123456983963544e-7,
-            "velocityX": -3.782719417287976,
-            "velocityY": -0.040900551268437134,
-            "timestamp": 0.4958250909900761
-        },
-        {
-            "x": 6.120558752684864,
-            "y": 4.113501775750749,
-            "heading": -0.43437499005887026,
-            "angularVelocity": -3.512325030859258e-7,
-            "velocityX": -3.782719434231919,
-            "velocityY": -0.04089898424321494,
-            "timestamp": 0.5949901091880914
-        },
-        {
-            "x": 5.747019040696225,
-            "y": 4.148065138700183,
-            "heading": -0.43437502656757854,
-            "angularVelocity": -3.681611615448704e-7,
-            "velocityX": -3.766849628794958,
-            "velocityY": 0.3485439076955189,
-            "timestamp": 0.6941551273861066
-        },
-        {
-            "x": 5.389911454629062,
-            "y": 4.262959433025878,
-            "heading": -0.434375162279621,
-            "angularVelocity": -0.0000013685475476133038,
-            "velocityX": -3.601144764115114,
-            "velocityY": 1.1586171859140066,
-            "timestamp": 0.7933201455841219
-        },
-        {
-            "x": 5.045913588511523,
-            "y": 4.412598985545015,
-            "heading": -0.4343752985502662,
-            "angularVelocity": -0.0000013741806101348962,
-            "velocityX": -3.468943709873945,
-            "velocityY": 1.5089953618556682,
-            "timestamp": 0.8924851637821372
-        },
-        {
-            "x": 4.713243052034498,
-            "y": 4.585968073714987,
-            "heading": -0.43437543856751193,
-            "angularVelocity": -0.0000014119620841874337,
-            "velocityX": -3.354716638207433,
-            "velocityY": 1.7482887748155678,
-            "timestamp": 0.9916501819801524
-        },
-        {
-            "x": 4.426185676495707,
-            "y": 4.82747296096862,
-            "heading": -0.4343754384530987,
-            "angularVelocity": 1.153766536466057e-9,
-            "velocityX": -2.894744343873237,
-            "velocityY": 2.4353838847827403,
-            "timestamp": 1.0908152001781677
-        },
-        {
-            "x": 4.09351517988242,
-            "y": 5.000842166204109,
-            "heading": -0.4343753660253311,
-            "angularVelocity": 7.303761838407394e-7,
-            "velocityX": -3.354716236213479,
-            "velocityY": 1.748289955327798,
-            "timestamp": 1.189980218376183
-        },
-        {
-            "x": 3.752063829141965,
-            "y": 5.156204697333757,
-            "heading": -0.43437529472941433,
-            "angularVelocity": 7.189623727917814e-7,
-            "velocityX": -3.4432641363372296,
-            "velocityY": 1.5667070298864525,
-            "timestamp": 1.2891452365741982
-        },
-        {
-            "x": 3.4085469650969755,
-            "y": 5.306945220863998,
-            "heading": -0.43437522535251666,
-            "angularVelocity": 6.996105981774619e-7,
-            "velocityX": -3.46409318817495,
-            "velocityY": 1.5200977750968405,
-            "timestamp": 1.3883102547722135
-        },
-        {
-            "x": 3.0417944650952866,
-            "y": 5.3858068126004595,
-            "heading": -0.43437520041032157,
-            "angularVelocity": 2.5152211488411685e-7,
-            "velocityX": -3.6984060172242224,
-            "velocityY": 0.7952561615930772,
-            "timestamp": 1.4874752729702287
-        },
-        {
-            "x": 2.7246409461958954,
-            "y": 5.456187357907523,
-            "heading": -0.2777696543737322,
-            "angularVelocity": 1.5792418423589194,
-            "velocityX": -3.1982399102281316,
-            "velocityY": 0.7097315826285274,
-            "timestamp": 1.586640291168244
-        },
-        {
-            "x": 2.4871805723397817,
-            "y": 5.508279050924135,
-            "heading": -0.14311381126711797,
-            "angularVelocity": 1.3578966207390795,
-            "velocityX": -2.394598197742947,
-            "velocityY": 0.5253031155865293,
-            "timestamp": 1.6858053093662593
-        },
-        {
-            "x": 2.3290265259060328,
-            "y": 5.5428007138640085,
-            "heading": -0.04864902301750531,
-            "angularVelocity": 0.9526019353012469,
-            "velocityX": -1.594857231992267,
-            "velocityY": 0.3481233964071807,
-            "timestamp": 1.7849703275642745
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -2.5681051863102055e-32,
-            "angularVelocity": 0.490586538494469,
-            "velocityX": -0.7969193909512572,
-            "velocityY": 0.1734410626703791,
-            "timestamp": 1.8841353457622898
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 1.14717044306562e-31,
-            "velocityX": 0,
-            "velocityY": 5.846493116157662e-32,
-            "timestamp": 1.983300363960305
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C3 To W3.traj b/src/main/deploy/choreo/C3 To W3.traj
deleted file mode 100644
index 3874f8b2..00000000
--- a/src/main/deploy/choreo/C3 To W3.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 5.58164160972529e-32,
-            "velocityX": 1.1273756142039836e-32,
-            "velocityY": 2.4507602066878393e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.588395006557631,
-            "y": 4.128528104169273,
-            "heading": -0.06094869084906696,
-            "angularVelocity": -0.5766457987157873,
-            "velocityX": -0.8666902254019087,
-            "velocityY": 0.01445765014696692,
-            "timestamp": 0.10569519622756653
-        },
-        {
-            "x": 7.4046836501246664,
-            "y": 4.131675342578391,
-            "heading": -0.17161815397097052,
-            "angularVelocity": -1.0470623743734504,
-            "velocityX": -1.7381239922903071,
-            "velocityY": 0.029776551077511915,
-            "timestamp": 0.21139039245513305
-        },
-        {
-            "x": 7.128148507793846,
-            "y": 4.136585421276787,
-            "heading": -0.30937735862178317,
-            "angularVelocity": -1.3033629679272352,
-            "velocityX": -2.6163454177749763,
-            "velocityY": 0.0464550790730766,
-            "timestamp": 0.3170855886826996
-        },
-        {
-            "x": 6.759745872373483,
-            "y": 4.143898882898015,
-            "heading": -0.3925495634756188,
-            "angularVelocity": -0.7869061965196801,
-            "velocityX": -3.485519196418107,
-            "velocityY": 0.06919388848555069,
-            "timestamp": 0.4227807849102661
-        },
-        {
-            "x": 6.3599483672427946,
-            "y": 4.149634642956697,
-            "heading": -0.39254959749689916,
-            "angularVelocity": -3.2188104690533836e-7,
-            "velocityX": -3.782551330619671,
-            "velocityY": 0.05426698907234567,
-            "timestamp": 0.5284759811378327
-        },
-        {
-            "x": 5.960150865134662,
-            "y": 4.155370613693523,
-            "heading": -0.39254963151811834,
-            "angularVelocity": -3.2188046698591794e-7,
-            "velocityX": -3.7825513020227493,
-            "velocityY": 0.05426898233365955,
-            "timestamp": 0.6341711773653992
-        },
-        {
-            "x": 5.56430885680334,
-            "y": 4.211762521698523,
-            "heading": -0.3925496691964927,
-            "angularVelocity": -3.5648142721372326e-7,
-            "velocityX": -3.745127711188092,
-            "velocityY": 0.5335333110464675,
-            "timestamp": 0.7398663735929657
-        },
-        {
-            "x": 5.196451370697568,
-            "y": 4.356905683947299,
-            "heading": -0.407192768864793,
-            "angularVelocity": -0.13854082485237101,
-            "velocityX": -3.4803614472105,
-            "velocityY": 1.3732238306864724,
-            "timestamp": 0.8455615698205322
-        },
-        {
-            "x": 4.853119399386178,
-            "y": 4.529141900436451,
-            "heading": -0.46036689242459117,
-            "angularVelocity": -0.5030893120753747,
-            "velocityX": -3.2483214333807724,
-            "velocityY": 1.6295557663597022,
-            "timestamp": 0.9512567660480987
-        },
-        {
-            "x": 4.514017884763102,
-            "y": 4.740993233583884,
-            "heading": -0.4603674462778229,
-            "angularVelocity": -0.000005240098429403888,
-            "velocityX": -3.208296372268186,
-            "velocityY": 2.0043610372916896,
-            "timestamp": 1.0569519622756653
-        },
-        {
-            "x": 4.21750261375463,
-            "y": 5.009227519029098,
-            "heading": -0.46036738913342323,
-            "angularVelocity": 5.406527608893175e-7,
-            "velocityX": -2.8053807702864813,
-            "velocityY": 2.537809616888297,
-            "timestamp": 1.1626471585032319
-        },
-        {
-            "x": 3.866969646487487,
-            "y": 5.201574981603133,
-            "heading": -0.46036721071098435,
-            "angularVelocity": 0.0000016880846552508654,
-            "velocityX": -3.3164512653198517,
-            "velocityY": 1.8198316426784653,
-            "timestamp": 1.2683423547307984
-        },
-        {
-            "x": 3.5045677865422378,
-            "y": 5.303672838764994,
-            "heading": -0.3887465759877037,
-            "angularVelocity": 0.6776148517580518,
-            "velocityX": -3.4287448519891264,
-            "velocityY": 0.9659649710289621,
-            "timestamp": 1.3740375509583649
-        },
-        {
-            "x": 3.16557460185915,
-            "y": 5.315860378892371,
-            "heading": -0.29320626763004903,
-            "angularVelocity": 0.9039229006392311,
-            "velocityX": -3.207271444515043,
-            "velocityY": 0.1153083636945626,
-            "timestamp": 1.4797327471859314
-        },
-        {
-            "x": 2.8681536233597122,
-            "y": 5.2454821587095966,
-            "heading": -0.23849853505250618,
-            "angularVelocity": 0.5175990445181124,
-            "velocityX": -2.8139498209462337,
-            "velocityY": -0.6658601591622668,
-            "timestamp": 1.585427943413498
-        },
-        {
-            "x": 2.618332286986499,
-            "y": 5.0970676603891505,
-            "heading": -0.24744806192055374,
-            "angularVelocity": -0.08467297651615932,
-            "velocityX": -2.3636016137889215,
-            "velocityY": -1.4041744905880433,
-            "timestamp": 1.6911231396410644
-        },
-        {
-            "x": 2.4325478975720265,
-            "y": 4.8813111498736355,
-            "heading": -0.27927376577641677,
-            "angularVelocity": -0.3011083283987745,
-            "velocityX": -1.7577373054350616,
-            "velocityY": -2.0413085761341616,
-            "timestamp": 1.796818335868631
-        },
-        {
-            "x": 2.3348070320385017,
-            "y": 4.635496280912222,
-            "heading": -0.32770640318337707,
-            "angularVelocity": -0.4582293153861289,
-            "velocityX": -0.9247427416008974,
-            "velocityY": -2.3256957528340565,
-            "timestamp": 1.9025135320961974
-        },
-        {
-            "x": 2.305548232287739,
-            "y": 4.452786448669473,
-            "heading": -0.3683698320727586,
-            "angularVelocity": -0.3847235289845278,
-            "velocityX": -0.2768224176221525,
-            "velocityY": -1.72864840374928,
-            "timestamp": 2.008208728323764
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.2046466509288718,
-            "velocityX": -0.05249275734153,
-            "velocityY": -0.8778681716972112,
-            "timestamp": 2.1139039245513307
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 5.518495272475637e-32,
-            "velocityX": 0,
-            "velocityY": 3.7393231194786592e-34,
-            "timestamp": 2.219599120778897
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C4 To C1.traj b/src/main/deploy/choreo/C4 To C1.traj
deleted file mode 100644
index 1bf739bf..00000000
--- a/src/main/deploy/choreo/C4 To C1.traj	
+++ /dev/null
@@ -1,175 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -2.2542235108498663e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 2.538844153951763,
-            "heading": 2.1223723168001845e-18,
-            "angularVelocity": 2.1422107346760384e-17,
-            "velocityX": -4.524824041974102e-19,
-            "velocityY": 0.8260917454276265,
-            "timestamp": 0.09907392781100399
-        },
-        {
-            "x": 7.68,
-            "y": 2.70253245874193,
-            "heading": 6.368502121517421e-18,
-            "angularVelocity": 4.285819588010552e-17,
-            "velocityX": -9.048618887508432e-19,
-            "velocityY": 1.6521834594306501,
-            "timestamp": 0.19814785562200798
-        },
-        {
-            "x": 7.68,
-            "y": 2.9480649081437846,
-            "heading": 1.2741259045974704e-17,
-            "angularVelocity": 6.432324896396774e-17,
-            "velocityX": -1.3570355356096521e-18,
-            "velocityY": 2.47827511058448,
-            "timestamp": 0.297221783433012
-        },
-        {
-            "x": 7.68,
-            "y": 3.2754414834771923,
-            "heading": 2.1247069179422305e-17,
-            "angularVelocity": 8.585316360490521e-17,
-            "velocityX": -1.8085916683463265e-18,
-            "velocityY": 3.3043665731908773,
-            "timestamp": 0.39629571124401597
-        },
-        {
-            "x": 7.68,
-            "y": 3.6502322649266867,
-            "heading": 5.059469084858907e-18,
-            "angularVelocity": -1.633891019789118e-16,
-            "velocityX": -2.0715339797290713e-18,
-            "velocityY": 3.782940575087071,
-            "timestamp": 0.49536963905501996
-        },
-        {
-            "x": 7.68,
-            "y": 4.025023046376204,
-            "heading": -4.370025378852898e-19,
-            "angularVelocity": -5.5478487063848375e-17,
-            "velocityX": -2.0691441879135133e-18,
-            "velocityY": 3.7829405750873115,
-            "timestamp": 0.594443566866024
-        },
-        {
-            "x": 7.68,
-            "y": 4.3998138278257235,
-            "heading": 3.678666355048194e-17,
-            "angularVelocity": 3.7571606285132846e-16,
-            "velocityX": -2.072328507072934e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.693517494677028
-        },
-        {
-            "x": 7.68,
-            "y": 4.774604609275241,
-            "heading": 5.708604515927294e-17,
-            "angularVelocity": 2.04891257037182e-16,
-            "velocityX": -2.07516702935077e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.7925914224880319
-        },
-        {
-            "x": 7.68,
-            "y": 5.14939539072476,
-            "heading": 3.2711500565517004e-17,
-            "angularVelocity": -2.460238039643844e-16,
-            "velocityX": -2.078706211560206e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.8916653502990359
-        },
-        {
-            "x": 7.68,
-            "y": 5.524186172174278,
-            "heading": 3.4723638610714384e-17,
-            "angularVelocity": 2.0309460720999983e-17,
-            "velocityX": -2.0667219645048642e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.9907392781100399
-        },
-        {
-            "x": 7.68,
-            "y": 5.898976953623796,
-            "heading": 1.3963468905806427e-18,
-            "angularVelocity": -3.363881139719203e-16,
-            "velocityX": -2.078319856885538e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 1.089813205921044
-        },
-        {
-            "x": 7.68,
-            "y": 6.273767735073314,
-            "heading": -1.338725801529433e-17,
-            "angularVelocity": -1.492179146704526e-16,
-            "velocityX": -2.0709974384556896e-18,
-            "velocityY": 3.7829405750873115,
-            "timestamp": 1.188887133732048
-        },
-        {
-            "x": 7.68,
-            "y": 6.648558516522808,
-            "heading": -2.1247111946894708e-17,
-            "angularVelocity": -7.933322222365232e-17,
-            "velocityX": -2.070808166139798e-18,
-            "velocityY": 3.782940575087071,
-            "timestamp": 1.287961061543052
-        },
-        {
-            "x": 7.68,
-            "y": 6.975935091856216,
-            "heading": -1.273985947141283e-17,
-            "angularVelocity": 8.586772184549593e-17,
-            "velocityX": -1.8108827209778567e-18,
-            "velocityY": 3.304366573190878,
-            "timestamp": 1.387034989354056
-        },
-        {
-            "x": 7.68,
-            "y": 7.22146754125807,
-            "heading": -6.367750753434283e-18,
-            "angularVelocity": 6.431670631033216e-17,
-            "velocityX": -1.3581621409711223e-18,
-            "velocityY": 2.47827511058448,
-            "timestamp": 1.4861089171650599
-        },
-        {
-            "x": 7.68,
-            "y": 7.385155846048237,
-            "heading": -2.122107432521882e-18,
-            "angularVelocity": 4.285328556951628e-17,
-            "velocityX": -9.05441456368543e-19,
-            "velocityY": 1.65218345943065,
-            "timestamp": 1.5851828449760639
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 2.141943374454498e-17,
-            "velocityX": -4.527207369006345e-19,
-            "velocityY": 0.8260917454276265,
-            "timestamp": 1.6842567727870679
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -3.1368402443636883e-34,
-            "velocityY": 0,
-            "timestamp": 1.7833307005980719
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C4 To C2.traj b/src/main/deploy/choreo/C4 To C2.traj
deleted file mode 100644
index 0dd47978..00000000
--- a/src/main/deploy/choreo/C4 To C2.traj	
+++ /dev/null
@@ -1,139 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -1.7120676318191985e-39,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 2.5334453720055388,
-            "heading": -1.6066276958150233e-18,
-            "angularVelocity": -1.677931007769515e-17,
-            "velocityX": 1.4398812722194594e-33,
-            "velocityY": 0.7983807350946674,
-            "timestamp": 0.09575052183150976
-        },
-        {
-            "x": 7.68,
-            "y": 2.686336112982689,
-            "heading": -4.816833322743681e-18,
-            "angularVelocity": -3.3526769001814837e-17,
-            "velocityX": 1.5865173753469157e-33,
-            "velocityY": 1.5967614385035827,
-            "timestamp": 0.1915010436630195
-        },
-        {
-            "x": 7.68,
-            "y": 2.915672216863599,
-            "heading": -9.624489104750263e-18,
-            "angularVelocity": -5.0210230614541765e-17,
-            "velocityX": 1.0182538660036936e-34,
-            "velocityY": 2.395142078541023,
-            "timestamp": 0.28725156549452924
-        },
-        {
-            "x": 7.68,
-            "y": 3.221453665444745,
-            "heading": -1.6009153132358185e-17,
-            "angularVelocity": -6.66802008672839e-17,
-            "velocityX": 3.094783239015371e-34,
-            "velocityY": 3.193522528464369,
-            "timestamp": 0.383002087326039
-        },
-        {
-            "x": 7.68,
-            "y": 3.5836721992668203,
-            "heading": -3.473454086768081e-17,
-            "angularVelocity": -1.9556434134398854e-16,
-            "velocityX": 3.082893791022991e-33,
-            "velocityY": 3.7829405719528495,
-            "timestamp": 0.4787526091575488
-        },
-        {
-            "x": 7.68,
-            "y": 3.9458907330889397,
-            "heading": -3.402618832360637e-17,
-            "angularVelocity": 7.397897479044197e-18,
-            "velocityX": 3.015867889332214e-33,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.5745031309890586
-        },
-        {
-            "x": 7.68,
-            "y": 4.308109266911059,
-            "heading": -1.7375698441954866e-17,
-            "angularVelocity": 1.7389450796885638e-16,
-            "velocityX": 5.330140345912211e-33,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.6702536528205684
-        },
-        {
-            "x": 7.68,
-            "y": 4.670327800733179,
-            "heading": 7.579747606314133e-18,
-            "angularVelocity": 2.6062986990486198e-16,
-            "velocityX": 7.600730979130334e-33,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.7660041746520782
-        },
-        {
-            "x": 7.68,
-            "y": 5.032546334555254,
-            "heading": 1.6243735534665814e-17,
-            "angularVelocity": 9.048502047432954e-17,
-            "velocityX": -2.799411465702586e-33,
-            "velocityY": 3.7829405719528495,
-            "timestamp": 0.8617546964835879
-        },
-        {
-            "x": 7.68,
-            "y": 5.3383277831364,
-            "heading": 9.733952786983855e-18,
-            "angularVelocity": -6.798691664538921e-17,
-            "velocityX": -8.030123288193386e-33,
-            "velocityY": 3.193522528464369,
-            "timestamp": 0.9575052183150977
-        },
-        {
-            "x": 7.68,
-            "y": 5.567663887017311,
-            "heading": 4.864296415465134e-18,
-            "angularVelocity": -5.08577528116881e-17,
-            "velocityX": -5.179461626817983e-33,
-            "velocityY": 2.395142078541023,
-            "timestamp": 1.0532557401466074
-        },
-        {
-            "x": 7.68,
-            "y": 5.720554627994461,
-            "heading": 1.6208272352196832e-18,
-            "angularVelocity": -3.3874167136237696e-17,
-            "velocityX": -4.721587370568867e-33,
-            "velocityY": 1.5967614385035827,
-            "timestamp": 1.1490062619781172
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": -1.69276073284528e-17,
-            "velocityX": -2.4599412486527172e-33,
-            "velocityY": 0.7983807350946674,
-            "timestamp": 1.244756783809627
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -3.907521125363084e-43,
-            "velocityY": 8.448865646535191e-39,
-            "timestamp": 1.3405073056411367
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C4 To C3.traj b/src/main/deploy/choreo/C4 To C3.traj
deleted file mode 100644
index 14b92c55..00000000
--- a/src/main/deploy/choreo/C4 To C3.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -2.24426408200091e-39,
-            "angularVelocity": 3.072610310721988e-47,
-            "velocityX": -5.239815815905201e-40,
-            "velocityY": -4.5917748078995606e-40,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 2.540500005575846,
-            "heading": -7.160737227180709e-23,
-            "angularVelocity": -7.15564807370433e-22,
-            "velocityX": 1.97902852084548e-33,
-            "velocityY": 0.8344065481228216,
-            "timestamp": 0.10007112931184187
-        },
-        {
-            "x": 7.68,
-            "y": 2.7075000136298466,
-            "heading": 1.9234971876596208e-22,
-            "angularVelocity": 2.6376946581601568e-21,
-            "velocityX": 2.759296119129438e-33,
-            "velocityY": 1.6688130652907365,
-            "timestamp": 0.20014225862368373
-        },
-        {
-            "x": 7.68,
-            "y": 2.9580000179666155,
-            "heading": 1.2715262688933367e-21,
-            "angularVelocity": 1.0784094425251139e-20,
-            "velocityX": 3.360924516770788e-33,
-            "velocityY": 2.5032195205488357,
-            "timestamp": 0.3002133879355256
-        },
-        {
-            "x": 7.68,
-            "y": 3.2919999999999994,
-            "heading": 4.5990005279924634e-21,
-            "angularVelocity": 3.325109025879841e-20,
-            "velocityX": 4.212493187097162e-33,
-            "velocityY": 3.3376257900775026,
-            "timestamp": 0.40028451724736747
-        },
-        {
-            "x": 7.68,
-            "y": 3.6259999820333837,
-            "heading": 2.8774590551268706e-21,
-            "angularVelocity": -1.7203177491039364e-20,
-            "velocityX": -1.8223734723751632e-33,
-            "velocityY": 3.3376257900775026,
-            "timestamp": 0.5003556465592094
-        },
-        {
-            "x": 7.68,
-            "y": 3.8764999863701526,
-            "heading": 1.4966782173181084e-21,
-            "angularVelocity": -1.3797993485663648e-20,
-            "velocityX": -1.777649344678044e-33,
-            "velocityY": 2.5032195205488357,
-            "timestamp": 0.6004267758710513
-        },
-        {
-            "x": 7.68,
-            "y": 4.043499994424153,
-            "heading": 5.147844205579642e-22,
-            "angularVelocity": -9.81195850164629e-21,
-            "velocityX": -2.007686184897088e-33,
-            "velocityY": 1.6688130652907365,
-            "timestamp": 0.7004979051828932
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -4.8498876719579466e-42,
-            "angularVelocity": -5.144185056490002e-21,
-            "velocityX": -2.233495211598662e-33,
-            "velocityY": 0.8344065481228216,
-            "timestamp": 0.8005690344947352
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 9.449822208481667e-41,
-            "angularVelocity": 9.927610425813228e-40,
-            "velocityX": 1.9465664275137074e-41,
-            "velocityY": 5.142787784847508e-39,
-            "timestamp": 0.9006401638065771
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C4 To C5.traj b/src/main/deploy/choreo/C4 To C5.traj
deleted file mode 100644
index 27f393e3..00000000
--- a/src/main/deploy/choreo/C4 To C5.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -4.712717929333765e-40,
-            "angularVelocity": 9.927649781151023e-40,
-            "velocityX": 1.7772849076267516e-40,
-            "velocityY": -4.543888050052464e-40,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 2.3734999944241535,
-            "heading": 2.8227518572323466e-21,
-            "angularVelocity": 2.820745460648545e-20,
-            "velocityX": 2.2465911667965736e-32,
-            "velocityY": -0.8344065481228212,
-            "timestamp": 0.10007112931184181
-        },
-        {
-            "x": 7.68,
-            "y": 2.206499986370153,
-            "heading": 8.436925006375396e-21,
-            "angularVelocity": 5.6101826293588e-20,
-            "velocityX": 4.4654374072917263e-32,
-            "velocityY": -1.6688130652907358,
-            "timestamp": 0.20014225862368362
-        },
-        {
-            "x": 7.68,
-            "y": 1.955999982033384,
-            "heading": 1.682221146414935e-20,
-            "angularVelocity": 8.379326253074398e-20,
-            "velocityX": 6.797738711780288e-32,
-            "velocityY": -2.503219520548835,
-            "timestamp": 0.30021338793552543
-        },
-        {
-            "x": 7.68,
-            "y": 1.6219999999999999,
-            "heading": 2.78675063552584e-20,
-            "angularVelocity": 1.1037443968745167e-19,
-            "velocityX": 8.936946075377416e-32,
-            "velocityY": -3.3376257900775013,
-            "timestamp": 0.40028451724736724
-        },
-        {
-            "x": 7.68,
-            "y": 1.288000017966616,
-            "heading": 1.0287640204896551e-20,
-            "angularVelocity": -1.75673704816788e-19,
-            "velocityX": 9.060995032099377e-32,
-            "velocityY": -3.3376257900775013,
-            "timestamp": 0.500355646559209
-        },
-        {
-            "x": 7.68,
-            "y": 1.037500013629847,
-            "heading": 3.2969803204781124e-21,
-            "angularVelocity": -6.985690973056607e-20,
-            "velocityX": 6.916062716279199e-32,
-            "velocityY": -2.503219520548835,
-            "timestamp": 0.6004267758710509
-        },
-        {
-            "x": 7.68,
-            "y": 0.8705000055758465,
-            "heading": 6.18120991020044e-22,
-            "angularVelocity": -2.676955216055553e-20,
-            "velocityX": 4.34564572466329e-32,
-            "velocityY": -1.6688130652907358,
-            "timestamp": 0.7004979051828927
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -2.2737142324276756e-40,
-            "angularVelocity": -6.1768164103594875e-21,
-            "velocityX": 2.131975080632403e-32,
-            "velocityY": -0.8344065481228212,
-            "timestamp": 0.8005690344947345
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 2.0993519451368808e-41,
-            "angularVelocity": 2.4819025971975228e-39,
-            "velocityX": -8.732390511178075e-43,
-            "velocityY": -2.571393892423754e-39,
-            "timestamp": 0.9006401638065763
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C4 To S1.traj b/src/main/deploy/choreo/C4 To S1.traj
deleted file mode 100644
index d3b44a34..00000000
--- a/src/main/deploy/choreo/C4 To S1.traj	
+++ /dev/null
@@ -1,256 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -3.908498355076501e-28,
-            "velocityX": -5.285170566576542e-31,
-            "velocityY": -1.5421992925545815e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 7.644672083885401,
-            "y": 2.5062543236054426,
-            "heading": 0.0003371503707738104,
-            "angularVelocity": 0.003954317681676867,
-            "velocityX": -0.4143486570570975,
-            "velocityY": 0.5776865743787781,
-            "timestamp": 0.08526132645270777
-        },
-        {
-            "x": 7.574016248997627,
-            "y": 2.604762966378743,
-            "heading": 0.0010114541718947692,
-            "angularVelocity": 0.00790867124800936,
-            "velocityX": -0.8286973452956875,
-            "velocityY": 1.1553730967106797,
-            "timestamp": 0.17052265290541554
-        },
-        {
-            "x": 7.4680324930779305,
-            "y": 2.7525259224839496,
-            "heading": 0.0020229127878395882,
-            "angularVelocity": 0.01186304105238048,
-            "velocityX": -1.243046060026343,
-            "velocityY": 1.73305955059478,
-            "timestamp": 0.2557839793581233
-        },
-        {
-            "x": 7.326720815099078,
-            "y": 2.949543182754876,
-            "heading": 0.0033715247016779517,
-            "angularVelocity": 0.015817393065211405,
-            "velocityX": -1.657394786805044,
-            "velocityY": 2.310745896971908,
-            "timestamp": 0.3410453058108311
-        },
-        {
-            "x": 7.1500812197312955,
-            "y": 3.1958147252550657,
-            "heading": 0.005057281636821727,
-            "angularVelocity": 0.01977164800486184,
-            "velocityX": -2.0717434588083226,
-            "velocityY": 2.8884319860640817,
-            "timestamp": 0.4263066322635389
-        },
-        {
-            "x": 6.957409028465344,
-            "y": 3.454481547786369,
-            "heading": 0.005057281852786698,
-            "angularVelocity": 2.532976909732762e-9,
-            "velocityX": -2.2597841164587846,
-            "velocityY": 3.0338118499096547,
-            "timestamp": 0.5115679587162466
-        },
-        {
-            "x": 6.719745009541024,
-            "y": 3.6725340903222867,
-            "heading": 0.005057282088140234,
-            "angularVelocity": 2.760378532567681e-9,
-            "velocityX": -2.787477380570894,
-            "velocityY": 2.5574612970252826,
-            "timestamp": 0.5968292851689544
-        },
-        {
-            "x": 6.445480997294438,
-            "y": 3.842270194716782,
-            "heading": 0.005057282389511224,
-            "angularVelocity": 3.534673960970524e-9,
-            "velocityX": -3.2167457821427474,
-            "velocityY": 1.9907748501725318,
-            "timestamp": 0.6820906116216622
-        },
-        {
-            "x": 6.1443030532206215,
-            "y": 3.9576952313720115,
-            "heading": 0.0050573451663417065,
-            "angularVelocity": 7.362872839766494e-7,
-            "velocityX": -3.5324097877003195,
-            "velocityY": 1.3537795089212774,
-            "timestamp": 0.76735193807437
-        },
-        {
-            "x": 5.8268478532983945,
-            "y": 4.0147328665402,
-            "heading": 0.005057406885810731,
-            "angularVelocity": 7.238858646916699e-7,
-            "velocityX": -3.723319975538169,
-            "velocityY": 0.6689742881224171,
-            "timestamp": 0.8526132645270778
-        },
-        {
-            "x": 5.505959202112108,
-            "y": 4.047314610372366,
-            "heading": 0.005057466820580134,
-            "angularVelocity": 7.029537528496964e-7,
-            "velocityX": -3.7635897133768776,
-            "velocityY": 0.3821397717793548,
-            "timestamp": 0.9378745909797855
-        },
-        {
-            "x": 5.196832806272751,
-            "y": 4.139357882135735,
-            "heading": 0.005057539347948128,
-            "angularVelocity": 8.506478964233103e-7,
-            "velocityX": -3.625634372587421,
-            "velocityY": 1.0795430424660248,
-            "timestamp": 1.0231359174324932
-        },
-        {
-            "x": 4.911616541791353,
-            "y": 4.287091740854739,
-            "heading": 0.008468262161445878,
-            "angularVelocity": 0.040003163864724633,
-            "velocityX": -3.3452008823671444,
-            "velocityY": 1.7327182776242007,
-            "timestamp": 1.108397243885201
-        },
-        {
-            "x": 4.672649683479067,
-            "y": 4.471709481894194,
-            "heading": 0.05890642847923189,
-            "angularVelocity": 0.5915714476464851,
-            "velocityX": -2.8027579238375626,
-            "velocityY": 2.165316313039838,
-            "timestamp": 1.1936585703379088
-        },
-        {
-            "x": 4.485112052095124,
-            "y": 4.685980588264911,
-            "heading": 0.1533943172973333,
-            "angularVelocity": 1.1082150929337078,
-            "velocityX": -2.1995626761432403,
-            "velocityY": 2.513110167122247,
-            "timestamp": 1.2789198967906166
-        },
-        {
-            "x": 4.353217601776123,
-            "y": 4.923890113830566,
-            "heading": 0.2635187752392858,
-            "angularVelocity": 1.291610892363634,
-            "velocityX": -1.5469434479432043,
-            "velocityY": 2.7903568413003317,
-            "timestamp": 1.3641812232433244
-        },
-        {
-            "x": 4.2800604900800625,
-            "y": 5.16873168779887,
-            "heading": 0.3704891219101591,
-            "angularVelocity": 1.3052114311350342,
-            "velocityX": -0.8926352155228909,
-            "velocityY": 2.987463639297507,
-            "timestamp": 1.4461375592962549
-        },
-        {
-            "x": 4.26199164107549,
-            "y": 5.42343977988949,
-            "heading": 0.46980573799025893,
-            "angularVelocity": 1.2118235253560785,
-            "velocityX": -0.22046921415441664,
-            "velocityY": 3.107851135829132,
-            "timestamp": 1.5280938953491854
-        },
-        {
-            "x": 4.2996600311730235,
-            "y": 5.679753559001703,
-            "heading": 0.550426603173402,
-            "angularVelocity": 0.9837050930723119,
-            "velocityX": 0.45961535021819666,
-            "velocityY": 3.127443117352554,
-            "timestamp": 1.610050231402116
-        },
-        {
-            "x": 4.391931306983731,
-            "y": 5.927283560440951,
-            "heading": 0.5996302346047028,
-            "angularVelocity": 0.6003639718722656,
-            "velocityX": 1.1258589665496648,
-            "velocityY": 3.0202668074301977,
-            "timestamp": 1.6920065674550464
-        },
-        {
-            "x": 4.533405002431738,
-            "y": 6.1528417215915425,
-            "heading": 0.5943564820218653,
-            "angularVelocity": -0.06434832053139185,
-            "velocityX": 1.7262081525536632,
-            "velocityY": 2.752174780055045,
-            "timestamp": 1.773962903507977
-        },
-        {
-            "x": 4.71098037645994,
-            "y": 6.338671203945781,
-            "heading": 0.5303639205613101,
-            "angularVelocity": -0.7808128638054942,
-            "velocityX": 2.166707085507742,
-            "velocityY": 2.2674205717817246,
-            "timestamp": 1.8559192395609074
-        },
-        {
-            "x": 4.8864330916758485,
-            "y": 6.469038507428809,
-            "heading": 0.43960837441640055,
-            "angularVelocity": -1.107364610427659,
-            "velocityX": 2.1408072110807552,
-            "velocityY": 1.5906921875921145,
-            "timestamp": 1.937875575613838
-        },
-        {
-            "x": 5.028345407892232,
-            "y": 6.554592883552385,
-            "heading": 0.3555321434700494,
-            "angularVelocity": -1.0258661501483803,
-            "velocityX": 1.7315600361235026,
-            "velocityY": 1.0439019146530064,
-            "timestamp": 2.0198319116667687
-        },
-        {
-            "x": 5.126173645194142,
-            "y": 6.6061448414321,
-            "heading": 0.29324170563915075,
-            "angularVelocity": -0.760044199524953,
-            "velocityX": 1.1936628943333356,
-            "velocityY": 0.6290173568329146,
-            "timestamp": 2.1017882477196994
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": -0.405602632332653,
-            "velocityX": 0.6079622053097948,
-            "velocityY": 0.29107155976928517,
-            "timestamp": 2.18374458377263
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 0,
-            "velocityX": 1.6766347007166246e-32,
-            "velocityY": 1.3162553729765107e-31,
-            "timestamp": 2.265700919825561
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C4 To S2.traj b/src/main/deploy/choreo/C4 To S2.traj
deleted file mode 100644
index 3bd7e899..00000000
--- a/src/main/deploy/choreo/C4 To S2.traj	
+++ /dev/null
@@ -1,211 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -1.8537798939082814e-32,
-            "velocityX": 2.7477856809061307e-34,
-            "velocityY": 6.925951302123011e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 7.6474171775045905,
-            "y": 2.4907449431726314,
-            "heading": -0.04287864331544942,
-            "angularVelocity": -0.5625230567182208,
-            "velocityX": -0.42745263118016213,
-            "velocityY": 0.4426984418000905,
-            "timestamp": 0.07622557476240162
-        },
-        {
-            "x": 7.582037622461459,
-            "y": 2.5583409566544844,
-            "heading": -0.12553871024043178,
-            "angularVelocity": -1.084413822823079,
-            "velocityX": -0.8577115390329612,
-            "velocityY": 0.8867891608892778,
-            "timestamp": 0.15245114952480324
-        },
-        {
-            "x": 7.483606625879984,
-            "y": 2.659997722482693,
-            "heading": -0.2429013405408753,
-            "angularVelocity": -1.5396752424139517,
-            "velocityX": -1.2913119630555505,
-            "velocityY": 1.333630689503849,
-            "timestamp": 0.22867672428720487
-        },
-        {
-            "x": 7.3517522665520065,
-            "y": 2.7960803575645476,
-            "heading": -0.3842395530561246,
-            "angularVelocity": -1.8542098627108627,
-            "velocityX": -1.7297916052318956,
-            "velocityY": 1.785262170944986,
-            "timestamp": 0.3049022990496065
-        },
-        {
-            "x": 7.186045392769314,
-            "y": 2.9668045775727996,
-            "heading": -0.5218107815453775,
-            "angularVelocity": -1.8047909631126855,
-            "velocityX": -2.173901270002986,
-            "velocityY": 2.2397236169147496,
-            "timestamp": 0.3811278738120081
-        },
-        {
-            "x": 6.990773522032128,
-            "y": 3.1645972581511623,
-            "heading": -0.5514162056749212,
-            "angularVelocity": -0.38839227151549954,
-            "velocityX": -2.5617631791673237,
-            "velocityY": 2.59483357383517,
-            "timestamp": 0.4573534485744097
-        },
-        {
-            "x": 6.791088913724043,
-            "y": 3.3726253897030523,
-            "heading": -0.5514162553808045,
-            "angularVelocity": -6.520893213469087e-7,
-            "velocityX": -2.619653691435051,
-            "velocityY": 2.729112009982516,
-            "timestamp": 0.5335790233368113
-        },
-        {
-            "x": 6.578307530314685,
-            "y": 3.567237130643777,
-            "heading": -0.5514163004289083,
-            "angularVelocity": -5.90984115130558e-7,
-            "velocityX": -2.7914697143656473,
-            "velocityY": 2.55310296507882,
-            "timestamp": 0.609804598099213
-        },
-        {
-            "x": 6.335948512349157,
-            "y": 3.7234797461898013,
-            "heading": -0.551416294920752,
-            "angularVelocity": 7.226126339733625e-8,
-            "velocityX": -3.179497415676709,
-            "velocityY": 2.0497400778287207,
-            "timestamp": 0.6860301728616146
-        },
-        {
-            "x": 6.088743524617576,
-            "y": 3.871936280628758,
-            "heading": -0.5514162894475348,
-            "angularVelocity": 7.180289728327137e-8,
-            "velocityX": -3.2430714822699573,
-            "velocityY": 1.9475948184280927,
-            "timestamp": 0.7622557476240163
-        },
-        {
-            "x": 5.841537785943911,
-            "y": 4.020391564616265,
-            "heading": -0.5514162839746023,
-            "angularVelocity": 7.179916458555982e-8,
-            "velocityX": -3.2430813338464923,
-            "velocityY": 1.947578413809903,
-            "timestamp": 0.8384813223864179
-        },
-        {
-            "x": 5.581140041351318,
-            "y": 4.144257068634033,
-            "heading": -0.5514162991479159,
-            "angularVelocity": -1.9905804152585698e-7,
-            "velocityX": -3.416146685731968,
-            "velocityY": 1.6249861598795787,
-            "timestamp": 0.9147068971488196
-        },
-        {
-            "x": 5.261335999197585,
-            "y": 4.231658536911035,
-            "heading": -0.5514164840698423,
-            "angularVelocity": -0.000002110046686592666,
-            "velocityX": -3.6491154543506057,
-            "velocityY": 0.9972921119903242,
-            "timestamp": 1.0023456814389826
-        },
-        {
-            "x": 4.96430123021485,
-            "y": 4.378912897735688,
-            "heading": -0.551417377431188,
-            "angularVelocity": -0.000010193675698949444,
-            "velocityX": -3.3893072728996803,
-            "velocityY": 1.6802419387415284,
-            "timestamp": 1.0899844657291458
-        },
-        {
-            "x": 4.714895393240272,
-            "y": 4.564810737884653,
-            "heading": -0.6189140340648516,
-            "angularVelocity": -0.7701687920520328,
-            "velocityX": -2.8458386203626582,
-            "velocityY": 2.121182324180531,
-            "timestamp": 1.177623250019309
-        },
-        {
-            "x": 4.521287798442847,
-            "y": 4.7820097530438215,
-            "heading": -0.6965700023585185,
-            "angularVelocity": -0.8860913455457793,
-            "velocityX": -2.209154273026079,
-            "velocityY": 2.4783435429688825,
-            "timestamp": 1.265262034309472
-        },
-        {
-            "x": 4.386338131009024,
-            "y": 5.001818806087968,
-            "heading": -0.6848521841568085,
-            "angularVelocity": 0.1337058506301677,
-            "velocityX": -1.539839564490294,
-            "velocityY": 2.5081253103235808,
-            "timestamp": 1.3529008185996352
-        },
-        {
-            "x": 4.301033328473477,
-            "y": 5.211510806429668,
-            "heading": -0.519384386353178,
-            "angularVelocity": 1.888065873389841,
-            "velocityX": -0.9733681637243018,
-            "velocityY": 2.3926849515327757,
-            "timestamp": 1.4405396028897983
-        },
-        {
-            "x": 4.267176527868507,
-            "y": 5.385808771912563,
-            "heading": -0.30251041793580363,
-            "angularVelocity": 2.474634605830756,
-            "velocityX": -0.38632211616348305,
-            "velocityY": 1.988822265103685,
-            "timestamp": 1.5281783871799615
-        },
-        {
-            "x": 4.261077190237836,
-            "y": 5.502861748658668,
-            "heading": -0.11181365176971161,
-            "angularVelocity": 2.175940340919318,
-            "velocityX": -0.06959632861263781,
-            "velocityY": 1.335629854911658,
-            "timestamp": 1.6158171714701246
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 9.015876776981611e-33,
-            "angularVelocity": 1.275846677647969,
-            "velocityX": 0.021940169272516404,
-            "velocityY": 0.6519744860010044,
-            "timestamp": 1.7034559557602877
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -9.923717904588528e-32,
-            "velocityX": -3.152679648787722e-32,
-            "velocityY": -1.9738855170003778e-32,
-            "timestamp": 1.7910947400504509
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C4 To S3.traj b/src/main/deploy/choreo/C4 To S3.traj
deleted file mode 100644
index 80fc230a..00000000
--- a/src/main/deploy/choreo/C4 To S3.traj	
+++ /dev/null
@@ -1,229 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 7.203126847267154e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.6459509070752,
-            "y": 2.420050479995149,
-            "heading": -0.026297254997510774,
-            "angularVelocity": -0.33695614983619343,
-            "velocityX": -0.4362832264603014,
-            "velocityY": -0.47344743777665743,
-            "timestamp": 0.078043552581802
-        },
-        {
-            "x": 7.57752123433784,
-            "y": 2.3463325784745406,
-            "heading": -0.07655763531528098,
-            "angularVelocity": -0.6440042598660941,
-            "velocityX": -0.8768139131756897,
-            "velocityY": -0.9445738831958564,
-            "timestamp": 0.156087105163604
-        },
-        {
-            "x": 7.474195561438856,
-            "y": 2.236153787211583,
-            "heading": -0.14701004173730176,
-            "angularVelocity": -0.9027319245646475,
-            "velocityX": -1.3239488654839289,
-            "velocityY": -1.4117603263569152,
-            "timestamp": 0.23413065774540598
-        },
-        {
-            "x": 7.33507656702923,
-            "y": 2.0901340736402396,
-            "heading": -0.23055657416165956,
-            "angularVelocity": -1.0705116522834195,
-            "velocityX": -1.7825815177216175,
-            "velocityY": -1.8710029046700958,
-            "timestamp": 0.312174210327208
-        },
-        {
-            "x": 7.158294386463989,
-            "y": 1.910067298253472,
-            "heading": -0.30975533817585227,
-            "angularVelocity": -1.014802137964151,
-            "velocityX": -2.2651734155741057,
-            "velocityY": -2.3072601057983104,
-            "timestamp": 0.39021776290901
-        },
-        {
-            "x": 6.940360292129115,
-            "y": 1.7108993896694566,
-            "heading": -0.30975560484167247,
-            "angularVelocity": -0.00000341688469228089,
-            "velocityX": -2.7924676302559823,
-            "velocityY": -2.552009768843728,
-            "timestamp": 0.468261315490812
-        },
-        {
-            "x": 6.6915168927330235,
-            "y": 1.5520279357314815,
-            "heading": -0.30975564017638474,
-            "angularVelocity": -4.5275632793576304e-7,
-            "velocityX": -3.1885196299240257,
-            "velocityY": -2.035676858398594,
-            "timestamp": 0.546304868072614
-        },
-        {
-            "x": 6.419127595403782,
-            "y": 1.43815413781978,
-            "heading": -0.30975568823518523,
-            "angularVelocity": -6.157946287917162e-7,
-            "velocityX": -3.490221655962278,
-            "velocityY": -1.4591057703627237,
-            "timestamp": 0.624348420654416
-        },
-        {
-            "x": 6.131252530334066,
-            "y": 1.37264761019134,
-            "heading": -0.30975577587401576,
-            "angularVelocity": -0.0000011229477336220622,
-            "velocityX": -3.6886463461279972,
-            "velocityY": -0.8393586076157934,
-            "timestamp": 0.7023919732362179
-        },
-        {
-            "x": 5.83657654735769,
-            "y": 1.3574693553002635,
-            "heading": -0.31020540722452916,
-            "angularVelocity": -0.005761287587978277,
-            "velocityX": -3.7757889438387497,
-            "velocityY": -0.1944844178532558,
-            "timestamp": 0.7804355258180199
-        },
-        {
-            "x": 5.546474933624268,
-            "y": 1.3928169012069702,
-            "heading": -0.3189695864044355,
-            "angularVelocity": -0.11229856778804584,
-            "velocityX": -3.717175911864151,
-            "velocityY": 0.4529207697160109,
-            "timestamp": 0.8584790783998218
-        },
-        {
-            "x": 5.303276771996363,
-            "y": 1.4607318697473424,
-            "heading": -0.3307229091538368,
-            "angularVelocity": -0.17360472172913416,
-            "velocityX": -3.5922053767043294,
-            "velocityY": 1.0031511484968414,
-            "timestamp": 0.9261807090485242
-        },
-        {
-            "x": 5.073563214559545,
-            "y": 1.564404927734292,
-            "heading": -0.34378229938421223,
-            "angularVelocity": -0.1928962434919207,
-            "velocityX": -3.3930284283517023,
-            "velocityY": 1.5313229089691542,
-            "timestamp": 0.9938823396972266
-        },
-        {
-            "x": 4.862974125111252,
-            "y": 1.7011642845126724,
-            "heading": -0.35827284529055603,
-            "angularVelocity": -0.2140354045770704,
-            "velocityX": -3.1105467834446325,
-            "velocityY": 2.020030469989471,
-            "timestamp": 1.0615839703459289
-        },
-        {
-            "x": 4.677671719496688,
-            "y": 1.8665243414779629,
-            "heading": -0.3791415933598452,
-            "angularVelocity": -0.30824587043792007,
-            "velocityX": -2.737044940262413,
-            "velocityY": 2.442482631227878,
-            "timestamp": 1.1292856009946313
-        },
-        {
-            "x": 4.523629451068858,
-            "y": 2.053477766497112,
-            "heading": -0.41478083919993697,
-            "angularVelocity": -0.5264163580502632,
-            "velocityX": -2.27531105162187,
-            "velocityY": 2.761431640977936,
-            "timestamp": 1.1969872316433336
-        },
-        {
-            "x": 4.406611773563404,
-            "y": 2.2498215105595722,
-            "heading": -0.45494733767836987,
-            "angularVelocity": -0.5932870167770028,
-            "velocityX": -1.7284321867022447,
-            "velocityY": 2.900133160474012,
-            "timestamp": 1.264688862292036
-        },
-        {
-            "x": 4.327688857963163,
-            "y": 2.4438365235550945,
-            "heading": -0.48799769437613305,
-            "angularVelocity": -0.4881766713911416,
-            "velocityX": -1.1657461547677304,
-            "velocityY": 2.865736188870776,
-            "timestamp": 1.3323904929407384
-        },
-        {
-            "x": 4.28360260298108,
-            "y": 2.622213717107768,
-            "heading": -0.5154307830643909,
-            "angularVelocity": -0.40520573028252516,
-            "velocityX": -0.6511845366152605,
-            "velocityY": 2.634754759132393,
-            "timestamp": 1.4000921235894408
-        },
-        {
-            "x": 4.264391797811532,
-            "y": 2.7716303897908317,
-            "heading": -0.5365689504315967,
-            "angularVelocity": -0.3122253801676767,
-            "velocityX": -0.283756904899847,
-            "velocityY": 2.2069877970647664,
-            "timestamp": 1.4677937542381432
-        },
-        {
-            "x": 4.259328393836262,
-            "y": 2.8855840206662466,
-            "heading": -0.5517027167443236,
-            "angularVelocity": -0.22353621571242488,
-            "velocityX": -0.07478998551086755,
-            "velocityY": 1.6831740946789628,
-            "timestamp": 1.5354953848868456
-        },
-        {
-            "x": 4.260599830778067,
-            "y": 2.9618797181043535,
-            "heading": -0.5613571988137664,
-            "angularVelocity": -0.1426033904496253,
-            "velocityX": 0.018780004700190214,
-            "velocityY": 1.1269403219251917,
-            "timestamp": 1.603197015535548
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -0.0685773908510417,
-            "velocityX": 0.035452162657366115,
-            "velocityY": 0.5630629798778553,
-            "timestamp": 1.6708986461842503
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 0,
-            "velocityX": 1.1764020723168042e-36,
-            "velocityY": 0,
-            "timestamp": 1.7386002768329527
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C4 To S4.traj b/src/main/deploy/choreo/C4 To S4.traj
deleted file mode 100644
index 1d7c5583..00000000
--- a/src/main/deploy/choreo/C4 To S4.traj	
+++ /dev/null
@@ -1,211 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -6.19176557444807e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.676458612092857,
-            "y": 2.4118388742702104,
-            "heading": -0.005126526914020665,
-            "angularVelocity": -0.06953454590838633,
-            "velocityX": -0.04803423529000399,
-            "velocityY": -0.6125508405591464,
-            "timestamp": 0.07372633051742336
-        },
-        {
-            "x": 7.666722992989933,
-            "y": 2.3218088358271687,
-            "heading": -0.015946167020864214,
-            "angularVelocity": -0.1467540840688735,
-            "velocityX": -0.1320507752738741,
-            "velocityY": -1.221138198676054,
-            "timestamp": 0.14745266103484672
-        },
-        {
-            "x": 7.646538544706082,
-            "y": 2.1877155788672784,
-            "heading": -0.03333492073697428,
-            "angularVelocity": -0.23585540734325022,
-            "velocityX": -0.2737753003871665,
-            "velocityY": -1.8187973824114398,
-            "timestamp": 0.22117899155227008
-        },
-        {
-            "x": 7.6083275574144436,
-            "y": 2.012099664253373,
-            "heading": -0.058740519234419,
-            "angularVelocity": -0.34459328599625155,
-            "velocityX": -0.5182814202669078,
-            "velocityY": -2.3819972238059934,
-            "timestamp": 0.29490532206969344
-        },
-        {
-            "x": 7.537878490375706,
-            "y": 1.8047562055419635,
-            "heading": -0.09435741731352677,
-            "angularVelocity": -0.4830960367773968,
-            "velocityX": -0.9555482626670088,
-            "velocityY": -2.812339326482677,
-            "timestamp": 0.36863165258711683
-        },
-        {
-            "x": 7.422206286264909,
-            "y": 1.5965261152652936,
-            "heading": -0.14021536920865627,
-            "angularVelocity": -0.6220023643288746,
-            "velocityX": -1.5689402049307317,
-            "velocityY": -2.8243653090459993,
-            "timestamp": 0.4423579831045402
-        },
-        {
-            "x": 7.269711858201117,
-            "y": 1.4146723813359454,
-            "heading": -0.19176235416262147,
-            "angularVelocity": -0.6991665608772336,
-            "velocityX": -2.068384890358183,
-            "velocityY": -2.4666049788870437,
-            "timestamp": 0.5160843136219636
-        },
-        {
-            "x": 7.089538504757627,
-            "y": 1.268699285071451,
-            "heading": -0.24604454859029715,
-            "angularVelocity": -0.7362660537519551,
-            "velocityX": -2.4438128437832747,
-            "velocityY": -1.9799316640341589,
-            "timestamp": 0.589810644139387
-        },
-        {
-            "x": 6.887025833129883,
-            "y": 1.1621593236923218,
-            "heading": -0.30153109883638657,
-            "angularVelocity": -0.7526015448846538,
-            "velocityX": -2.746816099573618,
-            "velocityY": -1.4450734307731645,
-            "timestamp": 0.6635369746568104
-        },
-        {
-            "x": 6.705802863807946,
-            "y": 1.102049450480248,
-            "heading": -0.3478019503216008,
-            "angularVelocity": -0.7562242219318657,
-            "velocityX": -2.9618041287927626,
-            "velocityY": -0.982401244869012,
-            "timestamp": 0.7247236573142499
-        },
-        {
-            "x": 6.5128639776244945,
-            "y": 1.0708739549800326,
-            "heading": -0.3939137418380571,
-            "angularVelocity": -0.7536246371554138,
-            "velocityX": -3.153282345173718,
-            "velocityY": -0.5095143934302662,
-            "timestamp": 0.7859103399716895
-        },
-        {
-            "x": 6.310456930732261,
-            "y": 1.0694436089370258,
-            "heading": -0.43928747590731515,
-            "angularVelocity": -0.741562250127008,
-            "velocityX": -3.308024525948419,
-            "velocityY": -0.023376754236126687,
-            "timestamp": 0.847097022629129
-        },
-        {
-            "x": 6.102475562929418,
-            "y": 1.0987242176578527,
-            "heading": -0.48296228626459325,
-            "angularVelocity": -0.7137960167214212,
-            "velocityX": -3.399128025411204,
-            "velocityY": 0.47854545219843847,
-            "timestamp": 0.9082837052865685
-        },
-        {
-            "x": 5.896578065500401,
-            "y": 1.1591365694762163,
-            "heading": -0.523188323608364,
-            "angularVelocity": -0.6574312513227849,
-            "velocityX": -3.3650704448508275,
-            "velocityY": 0.9873447814876488,
-            "timestamp": 0.9694703879440081
-        },
-        {
-            "x": 5.707810116262367,
-            "y": 1.2455849986940297,
-            "heading": -0.5570591599834606,
-            "angularVelocity": -0.5535654966739442,
-            "velocityX": -3.0851149472324435,
-            "velocityY": 1.4128634772014765,
-            "timestamp": 1.0306570706014475
-        },
-        {
-            "x": 5.549198847906331,
-            "y": 1.3398221505031715,
-            "heading": -0.5834741067263961,
-            "angularVelocity": -0.431710718667697,
-            "velocityX": -2.5922514747864094,
-            "velocityY": 1.5401578859363727,
-            "timestamp": 1.091843753258887
-        },
-        {
-            "x": 5.421048809950807,
-            "y": 1.4274575794538809,
-            "heading": -0.6037133289661716,
-            "angularVelocity": -0.3307782242924832,
-            "velocityX": -2.094410619921754,
-            "velocityY": 1.4322631191062654,
-            "timestamp": 1.1530304359163264
-        },
-        {
-            "x": 5.321165420085697,
-            "y": 1.5019229236562417,
-            "heading": -0.6188925850446988,
-            "angularVelocity": -0.24808104344388057,
-            "velocityX": -1.6324367579186874,
-            "velocityY": 1.2170188179552628,
-            "timestamp": 1.2142171185737658
-        },
-        {
-            "x": 5.247799603622136,
-            "y": 1.559962853683462,
-            "heading": -0.6297183288548622,
-            "angularVelocity": -0.1769297392828535,
-            "velocityX": -1.19904876808419,
-            "velocityY": 0.9485712822864297,
-            "timestamp": 1.2754038012312052
-        },
-        {
-            "x": 5.199709983819949,
-            "y": 1.599716552093825,
-            "heading": -0.6366489788204296,
-            "angularVelocity": -0.11327056255638758,
-            "velocityX": -0.7859491267310955,
-            "velocityY": 0.6497116150736374,
-            "timestamp": 1.3365904838886447
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -0.054767165566589775,
-            "velocityX": -0.3875023581960166,
-            "velocityY": 0.3315010231839871,
-            "timestamp": 1.397777166546084
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 7.169943122382087e-31,
-            "velocityX": 0,
-            "velocityY": -4.0382085379705675e-33,
-            "timestamp": 1.4589638492035235
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C4 To W1.traj b/src/main/deploy/choreo/C4 To W1.traj
deleted file mode 100644
index f850aede..00000000
--- a/src/main/deploy/choreo/C4 To W1.traj	
+++ /dev/null
@@ -1,238 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 2.322866633708497e-28,
-            "velocityX": -2.1618763976557105e-31,
-            "velocityY": -1.247476705840714e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 7.635666877682845,
-            "y": 2.515362381859407,
-            "heading": -0.0008300252680408195,
-            "angularVelocity": -0.00885317162168204,
-            "velocityX": -0.4728636048938783,
-            "velocityY": 0.6225017511467659,
-            "timestamp": 0.09375456655646779
-        },
-        {
-            "x": 7.547000631163484,
-            "y": 2.632087139923421,
-            "heading": -0.0024900693866490363,
-            "angularVelocity": -0.01770627479364842,
-            "velocityX": -0.9457272298940119,
-            "velocityY": 1.2450034419786018,
-            "timestamp": 0.18750913311293557
-        },
-        {
-            "x": 7.4140012603329,
-            "y": 2.8071742656560237,
-            "heading": -0.004980126198969091,
-            "angularVelocity": -0.026559312295687615,
-            "velocityX": -1.418590856056911,
-            "velocityY": 1.867505041764004,
-            "timestamp": 0.28126369966940334
-        },
-        {
-            "x": 7.236668772838196,
-            "y": 3.0406237394955027,
-            "heading": -0.008300187115435396,
-            "angularVelocity": -0.035412258180156565,
-            "velocityX": -1.891454400654691,
-            "velocityY": 2.4900064329013105,
-            "timestamp": 0.37501826622587114
-        },
-        {
-            "x": 7.022136423676778,
-            "y": 3.3230511197393278,
-            "heading": -0.008300187546286333,
-            "angularVelocity": -4.59551947810532e-9,
-            "velocityX": -2.2882335980104607,
-            "velocityY": 3.012411988206685,
-            "timestamp": 0.46877283278233894
-        },
-        {
-            "x": 6.786560921928144,
-            "y": 3.5881804025568163,
-            "heading": -0.008300187986713265,
-            "angularVelocity": -4.6976584720676315e-9,
-            "velocityX": -2.51268295936015,
-            "velocityY": 2.827907936172931,
-            "timestamp": 0.5625273993388067
-        },
-        {
-            "x": 6.5015204114211205,
-            "y": 3.799228403383322,
-            "heading": -0.008300188501414158,
-            "angularVelocity": -5.489875460815635e-9,
-            "velocityX": -3.0402840200359327,
-            "velocityY": 2.251069026055312,
-            "timestamp": 0.6562819658952744
-        },
-        {
-            "x": 6.179186778264032,
-            "y": 3.9471823835437703,
-            "heading": -0.008300169798343478,
-            "angularVelocity": 1.9948970346527163e-7,
-            "velocityX": -3.4380579527606097,
-            "velocityY": 1.5780989192812942,
-            "timestamp": 0.7500365324517422
-        },
-        {
-            "x": 5.833324835964247,
-            "y": 4.025724182042927,
-            "heading": -0.008299866211032016,
-            "angularVelocity": 0.0000032381069276099122,
-            "velocityX": -3.689014359545628,
-            "velocityY": 0.8377383778085268,
-            "timestamp": 0.8437910990082099
-        },
-        {
-            "x": 5.481092764022396,
-            "y": 4.0672194252777,
-            "heading": -0.008299498284594501,
-            "angularVelocity": 0.000003924357511610969,
-            "velocityX": -3.7569590994770814,
-            "velocityY": 0.44259436909433375,
-            "timestamp": 0.9375456655646777
-        },
-        {
-            "x": 5.14904862351455,
-            "y": 4.179095043376677,
-            "heading": 0.0033651913425959347,
-            "angularVelocity": 0.12441729566489874,
-            "velocityX": -3.5416316527670983,
-            "velocityY": 1.1932818017093059,
-            "timestamp": 1.0313002321211455
-        },
-        {
-            "x": 4.858987953002521,
-            "y": 4.350333701259038,
-            "heading": 0.047887100206889445,
-            "angularVelocity": 0.474877229979814,
-            "velocityX": -3.093829785211866,
-            "velocityY": 1.8264567174893411,
-            "timestamp": 1.1250547986776134
-        },
-        {
-            "x": 4.6181973815778665,
-            "y": 4.575631660267378,
-            "heading": 0.10901822024185409,
-            "angularVelocity": 0.652033519862158,
-            "velocityX": -2.5683076597621346,
-            "velocityY": 2.403061176467013,
-            "timestamp": 1.2188093652340812
-        },
-        {
-            "x": 4.427445933507109,
-            "y": 4.854263962903063,
-            "heading": 0.15298068479753676,
-            "angularVelocity": 0.46891011467909954,
-            "velocityX": -2.0345830083473238,
-            "velocityY": 2.971933132108998,
-            "timestamp": 1.312563931790549
-        },
-        {
-            "x": 4.19709169118396,
-            "y": 5.123942070734021,
-            "heading": 0.15298069409509907,
-            "angularVelocity": 9.916916750047786e-8,
-            "velocityX": -2.4569922381797515,
-            "velocityY": 2.8764263729866686,
-            "timestamp": 1.406318498347017
-        },
-        {
-            "x": 3.9282946631899107,
-            "y": 5.355323824233057,
-            "heading": 0.15298070467968702,
-            "angularVelocity": 1.1289677246612154e-7,
-            "velocityX": -2.8670286458223346,
-            "velocityY": 2.4679518235485323,
-            "timestamp": 1.5000730649034848
-        },
-        {
-            "x": 3.6594937496643456,
-            "y": 5.586701063819283,
-            "heading": 0.1529807152642756,
-            "angularVelocity": 1.1289677885329184e-7,
-            "velocityX": -2.867070089473112,
-            "velocityY": 2.4679036774904017,
-            "timestamp": 1.5938276314599527
-        },
-        {
-            "x": 3.3906928188108107,
-            "y": 5.818078283274842,
-            "heading": 0.1529807258488866,
-            "angularVelocity": 1.1289701863908675e-7,
-            "velocityX": -2.8670702742957928,
-            "velocityY": 2.467903462773746,
-            "timestamp": 1.6875821980164205
-        },
-        {
-            "x": 3.1218918754104785,
-            "y": 6.049455488154229,
-            "heading": 0.15298073643351034,
-            "angularVelocity": 1.1289715379702627e-7,
-            "velocityX": -2.867070408121775,
-            "velocityY": 2.467903307302158,
-            "timestamp": 1.7813367645728884
-        },
-        {
-            "x": 2.853090944613356,
-            "y": 6.280832707674931,
-            "heading": 0.15298074701856604,
-            "angularVelocity": 1.1290176153813175e-7,
-            "velocityX": -2.8670702736940825,
-            "velocityY": 2.4679034634685753,
-            "timestamp": 1.8750913311293562
-        },
-        {
-            "x": 2.631853660621228,
-            "y": 6.471299719512446,
-            "heading": 0.24894777935018825,
-            "angularVelocity": 1.0235984854542721,
-            "velocityX": -2.3597494193403232,
-            "velocityY": 2.0315491696374846,
-            "timestamp": 1.968845897685824
-        },
-        {
-            "x": 2.465926649664366,
-            "y": 6.614149879341798,
-            "heading": 0.32096506390227886,
-            "angularVelocity": 0.7681469521670183,
-            "velocityX": -1.7698019099360354,
-            "velocityY": 1.52366082075921,
-            "timestamp": 2.062600464242292
-        },
-        {
-            "x": 2.355308907973467,
-            "y": 6.709383273555383,
-            "heading": 0.3689895410149908,
-            "angularVelocity": 0.5122361382129277,
-            "velocityX": -1.1798651068828128,
-            "velocityY": 1.0157733933549364,
-            "timestamp": 2.1563550307987596
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.25609908793666974,
-            "velocityX": -0.5899329494543092,
-            "velocityY": 0.5078870095990298,
-            "timestamp": 2.250109597355227
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0,
-            "velocityX": -3.330818616870831e-32,
-            "velocityY": 5.736512740310497e-33,
-            "timestamp": 2.343864163911695
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C4 To W2.traj b/src/main/deploy/choreo/C4 To W2.traj
deleted file mode 100644
index f49877f8..00000000
--- a/src/main/deploy/choreo/C4 To W2.traj	
+++ /dev/null
@@ -1,256 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": 6.426641305668581e-31,
-            "velocityX": -5.011351780601486e-32,
-            "velocityY": -1.0551805148640748e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.6491080594765135,
-            "y": 2.4958059548034544,
-            "heading": -0.02134777834454554,
-            "angularVelocity": -0.2757848804434067,
-            "velocityX": -0.3990827516771383,
-            "velocityY": 0.5013212819261742,
-            "timestamp": 0.07740735572675958
-        },
-        {
-            "x": 7.587308326375472,
-            "y": 2.5734484495799954,
-            "heading": -0.06302080337998518,
-            "angularVelocity": -0.538360013001088,
-            "velocityX": -0.7983702907923742,
-            "velocityY": 1.0030376835324535,
-            "timestamp": 0.15481471145351916
-        },
-        {
-            "x": 7.4945787588029695,
-            "y": 2.6899753845455243,
-            "heading": -0.12336744193035674,
-            "angularVelocity": -0.7795982433941759,
-            "velocityX": -1.1979425818371583,
-            "velocityY": 1.5053728921687748,
-            "timestamp": 0.23222206718027874
-        },
-        {
-            "x": 7.370887761034633,
-            "y": 2.845471450080925,
-            "heading": -0.19922081386712712,
-            "angularVelocity": -0.9799245979222482,
-            "velocityX": -1.5979230475841766,
-            "velocityY": 2.0088021877957765,
-            "timestamp": 0.3096294229070383
-        },
-        {
-            "x": 7.216202117777622,
-            "y": 3.0401051125017453,
-            "heading": -0.28194072984640106,
-            "angularVelocity": -1.0686312069781692,
-            "velocityX": -1.9983326107022177,
-            "velocityY": 2.5144078439762083,
-            "timestamp": 0.38703677863379793
-        },
-        {
-            "x": 7.032473858656331,
-            "y": 3.268122248690632,
-            "heading": -0.2819407809538647,
-            "angularVelocity": -6.602404015783628e-7,
-            "velocityX": -2.3735245493959742,
-            "velocityY": 2.9456778887237176,
-            "timestamp": 0.46444413436055754
-        },
-        {
-            "x": 6.834683469529352,
-            "y": 3.4840547774927524,
-            "heading": -0.28194080060331267,
-            "angularVelocity": -2.538447128540298e-7,
-            "velocityX": -2.5551885511392456,
-            "velocityY": 2.7895608469606037,
-            "timestamp": 0.5418514900873171
-        },
-        {
-            "x": 6.603064880376339,
-            "y": 3.663221523268011,
-            "heading": -0.2819408226641663,
-            "angularVelocity": -2.8499686418440244e-7,
-            "velocityX": -2.992203867170001,
-            "velocityY": 2.314595868740169,
-            "timestamp": 0.6192588458140768
-        },
-        {
-            "x": 6.344360158554223,
-            "y": 3.8004066501958644,
-            "heading": -0.2819407635550332,
-            "angularVelocity": 7.636113202840995e-7,
-            "velocityX": -3.342120647233029,
-            "velocityY": 1.7722492344539278,
-            "timestamp": 0.6966662015408364
-        },
-        {
-            "x": 6.0661003337524155,
-            "y": 3.891617196013079,
-            "heading": -0.28194070942615473,
-            "angularVelocity": 6.992730598438293e-7,
-            "velocityX": -3.59474654817093,
-            "velocityY": 1.1783188427102327,
-            "timestamp": 0.774073557267596
-        },
-        {
-            "x": 5.7830200242104235,
-            "y": 3.966540038544743,
-            "heading": -0.2819406560965727,
-            "angularVelocity": 6.889472132026456e-7,
-            "velocityX": -3.6570207945254936,
-            "velocityY": 0.967903396624646,
-            "timestamp": 0.8514809129943556
-        },
-        {
-            "x": 5.499939604590639,
-            "y": 4.041462465169378,
-            "heading": -0.2819406027672389,
-            "angularVelocity": 6.889440059098256e-7,
-            "velocityX": -3.657022216584049,
-            "velocityY": 0.9678980236594484,
-            "timestamp": 0.9288882687211152
-        },
-        {
-            "x": 5.234359644348809,
-            "y": 4.1279159927443425,
-            "heading": -0.3272205989423401,
-            "angularVelocity": -0.5849572789301486,
-            "velocityX": -3.430939576069496,
-            "velocityY": 1.1168644990295,
-            "timestamp": 1.0062956244478747
-        },
-        {
-            "x": 4.992805004119873,
-            "y": 4.258058071136475,
-            "heading": -0.38392632852173914,
-            "angularVelocity": -0.7325625458589549,
-            "velocityX": -3.120564421314147,
-            "velocityY": 1.6812624222886623,
-            "timestamp": 1.0837029801746343
-        },
-        {
-            "x": 4.760679474369801,
-            "y": 4.451318301231298,
-            "heading": -0.43925940371465827,
-            "angularVelocity": -0.650567727765224,
-            "velocityX": -2.7291701738847216,
-            "velocityY": 2.272219071899485,
-            "timestamp": 1.1687565000948184
-        },
-        {
-            "x": 4.558064597648056,
-            "y": 4.695759350557987,
-            "heading": -0.45036550730749886,
-            "angularVelocity": -0.13057782444839394,
-            "velocityX": -2.3822044862091225,
-            "velocityY": 2.8739674684372165,
-            "timestamp": 1.2538100200150024
-        },
-        {
-            "x": 4.30999794561686,
-            "y": 4.900667947770545,
-            "heading": -0.4503653988140509,
-            "angularVelocity": 0.0000012755903345298267,
-            "velocityX": -2.9165947777821004,
-            "velocityY": 2.4091724528843765,
-            "timestamp": 1.3388635399351865
-        },
-        {
-            "x": 4.028045524182051,
-            "y": 5.05567579900462,
-            "heading": -0.4503653036134663,
-            "angularVelocity": 0.0000011193021141522574,
-            "velocityX": -3.3150000340890857,
-            "velocityY": 1.8224742653747859,
-            "timestamp": 1.4239170598553705
-        },
-        {
-            "x": 3.731575050469511,
-            "y": 5.180695241574213,
-            "heading": -0.45036521232637566,
-            "angularVelocity": 0.000001073289978617534,
-            "velocityX": -3.485693172848094,
-            "velocityY": 1.4698914599542185,
-            "timestamp": 1.5089705797755546
-        },
-        {
-            "x": 3.4348291016010033,
-            "y": 5.305059402921897,
-            "heading": -0.4503651210307298,
-            "angularVelocity": 0.000001073390565931622,
-            "velocityX": -3.488932017711167,
-            "velocityY": 1.4621871201127652,
-            "timestamp": 1.5940240996957387
-        },
-        {
-            "x": 3.120089238118937,
-            "y": 5.371868296261872,
-            "heading": -0.45036508370615375,
-            "angularVelocity": 4.3883634791364236e-7,
-            "velocityX": -3.7004919229372337,
-            "velocityY": 0.785492398232055,
-            "timestamp": 1.6790776196159227
-        },
-        {
-            "x": 2.8304866761911076,
-            "y": 5.435580552385651,
-            "heading": -0.37567464629819297,
-            "angularVelocity": 0.8781580994889028,
-            "velocityX": -3.404945053415936,
-            "velocityY": 0.7490842963759552,
-            "timestamp": 1.7641311395361068
-        },
-        {
-            "x": 2.5976382602327472,
-            "y": 5.485810295855226,
-            "heading": -0.2445738789443125,
-            "angularVelocity": 1.5413914377313083,
-            "velocityX": -2.7376693660283826,
-            "velocityY": 0.5905663106789283,
-            "timestamp": 1.8491846594562908
-        },
-        {
-            "x": 2.4235493783626687,
-            "y": 5.523078200691987,
-            "heading": -0.12850773623159037,
-            "angularVelocity": 1.3646248012033382,
-            "velocityX": -2.0468157230112984,
-            "velocityY": 0.43817004718549557,
-            "timestamp": 1.934238179376475
-        },
-        {
-            "x": 2.3077761259926057,
-            "y": 5.547737838884683,
-            "heading": -0.04437897835044122,
-            "angularVelocity": 0.9891272925576834,
-            "velocityX": -1.3611812007158846,
-            "velocityY": 0.2899308366756611,
-            "timestamp": 2.0192916992966587
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 1.7803711242706045e-30,
-            "angularVelocity": 0.521777092730431,
-            "velocityX": -0.6792914161204248,
-            "velocityY": 0.14416994295838573,
-            "timestamp": 2.104345219216843
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 1.3562125763475036e-31,
-            "velocityY": 2.290518308039125e-31,
-            "timestamp": 2.189398739137027
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C4 To W3.traj b/src/main/deploy/choreo/C4 To W3.traj
deleted file mode 100644
index 15a1cd3f..00000000
--- a/src/main/deploy/choreo/C4 To W3.traj	
+++ /dev/null
@@ -1,274 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -4.62162066936404e-33,
-            "velocityX": 3.0688151507515204e-31,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.623584376302727,
-            "y": 2.4297857949616017,
-            "heading": 0.033539307380936634,
-            "angularVelocity": 0.38483343876921716,
-            "velocityX": -0.6473186288895079,
-            "velocityY": -0.3122585684118786,
-            "timestamp": 0.08715278871867994
-        },
-        {
-            "x": 7.510615150776605,
-            "y": 2.375312721693156,
-            "heading": 0.0969954791736376,
-            "angularVelocity": 0.7281025968948721,
-            "velocityX": -1.2962204329545195,
-            "velocityY": -0.6250296068468768,
-            "timestamp": 0.1743055774373599
-        },
-        {
-            "x": 7.340880293995689,
-            "y": 2.2935273668964844,
-            "heading": 0.1838752961952568,
-            "angularVelocity": 0.9968678948651678,
-            "velocityX": -1.9475550843106406,
-            "velocityY": -0.9384135149210906,
-            "timestamp": 0.26145836615603985
-        },
-        {
-            "x": 7.114069036249808,
-            "y": 2.1844157133503646,
-            "heading": 0.27996244701307155,
-            "angularVelocity": 1.1025137833279668,
-            "velocityX": -2.6024555390648842,
-            "velocityY": -1.251958258023393,
-            "timestamp": 0.3486111548747198
-        },
-        {
-            "x": 6.831104073589339,
-            "y": 2.0490142604857513,
-            "heading": 0.32912237638568254,
-            "angularVelocity": 0.5640660510737534,
-            "velocityX": -3.246768885088134,
-            "velocityY": -1.5536101007814562,
-            "timestamp": 0.4357639435933997
-        },
-        {
-            "x": 6.534528280813305,
-            "y": 1.9049975621731787,
-            "heading": 0.3291224030694369,
-            "angularVelocity": 3.0617212321522066e-7,
-            "velocityX": -3.4029409401155197,
-            "velocityY": -1.6524623070575855,
-            "timestamp": 0.5229167323120797
-        },
-        {
-            "x": 6.237952262222522,
-            "y": 1.7609813288852085,
-            "heading": 0.3291224297529421,
-            "angularVelocity": 3.0616926449614015e-7,
-            "velocityX": -3.402943531137019,
-            "velocityY": -1.6524569713178037,
-            "timestamp": 0.6100695210307596
-        },
-        {
-            "x": 5.936787262454918,
-            "y": 1.6268259970502223,
-            "heading": 0.32912245667190637,
-            "angularVelocity": 3.0887094574020004e-7,
-            "velocityX": -3.4555979699024117,
-            "velocityY": -1.5393119808021931,
-            "timestamp": 0.6972223097494396
-        },
-        {
-            "x": 5.615527521313305,
-            "y": 1.5527301986214108,
-            "heading": 0.32912248658360005,
-            "angularVelocity": 3.4320982850713803e-7,
-            "velocityX": -3.6861670850098154,
-            "velocityY": -0.850182759704744,
-            "timestamp": 0.7843750984681195
-        },
-        {
-            "x": 5.286027461953728,
-            "y": 1.5414290964422628,
-            "heading": 0.32912252585289026,
-            "angularVelocity": 4.50579847209181e-7,
-            "velocityX": -3.780717337951944,
-            "velocityY": -0.12967000075725457,
-            "timestamp": 0.8715278871867994
-        },
-        {
-            "x": 4.960446284223111,
-            "y": 1.593339788205863,
-            "heading": 0.3291233220408656,
-            "angularVelocity": 0.000009135542155699871,
-            "velocityX": -3.7357516898462033,
-            "velocityY": 0.5956285797252272,
-            "timestamp": 0.9586806759054793
-        },
-        {
-            "x": 4.650797848549272,
-            "y": 1.7065466836899894,
-            "heading": 0.3291235959637348,
-            "angularVelocity": 0.0000031430189815062194,
-            "velocityX": -3.5529377800330906,
-            "velocityY": 1.2989474823295228,
-            "timestamp": 1.0458334646241594
-        },
-        {
-            "x": 4.368508202692925,
-            "y": 1.8768714467040708,
-            "heading": 0.32912381701495697,
-            "angularVelocity": 0.000002536364302217629,
-            "velocityX": -3.2390202310972316,
-            "velocityY": 1.9543237286860882,
-            "timestamp": 1.1329862533428394
-        },
-        {
-            "x": 4.092384496633659,
-            "y": 2.0570204638146983,
-            "heading": 0.3291240376025158,
-            "angularVelocity": 0.000002531044182467546,
-            "velocityX": -3.1682716080441704,
-            "velocityY": 2.067048223690569,
-            "timestamp": 1.2201390420615195
-        },
-        {
-            "x": 3.8162567615509033,
-            "y": 2.2371633052825928,
-            "heading": 0.32912425818941665,
-            "angularVelocity": 0.0000025310366317804206,
-            "velocityX": -3.1683178374712258,
-            "velocityY": 2.066977363735053,
-            "timestamp": 1.3072918307801995
-        },
-        {
-            "x": 3.558387727399027,
-            "y": 2.3518646644990793,
-            "heading": 0.32912440943088817,
-            "angularVelocity": 0.0000020272145178599712,
-            "velocityX": -3.4564319203160125,
-            "velocityY": 1.5374371746628512,
-            "timestamp": 1.3818973893726278
-        },
-        {
-            "x": 3.300516056227942,
-            "y": 2.466560095044955,
-            "heading": 0.3291245606718786,
-            "angularVelocity": 0.000002027208070545675,
-            "velocityX": -3.4564672664652627,
-            "velocityY": 1.5373577077876843,
-            "timestamp": 1.4565029479650562
-        },
-        {
-            "x": 3.052458337002912,
-            "y": 2.601171414843865,
-            "heading": 0.329124716605593,
-            "angularVelocity": 0.0000020901085294853097,
-            "velocityX": -3.3249227524744356,
-            "velocityY": 1.804306841723335,
-            "timestamp": 1.5311085065574845
-        },
-        {
-            "x": 2.829814908557799,
-            "y": 2.7746151638451657,
-            "heading": 0.3291249161701291,
-            "angularVelocity": 0.000002674928516490017,
-            "velocityX": -2.984273995740964,
-            "velocityY": 2.3248100044237634,
-            "timestamp": 1.6057140651499129
-        },
-        {
-            "x": 2.638606432152533,
-            "y": 2.982201542308141,
-            "heading": 0.3291253317978473,
-            "angularVelocity": 0.000005571002027267265,
-            "velocityX": -2.562925337103115,
-            "velocityY": 2.7824519027734036,
-            "timestamp": 1.6803196237423412
-        },
-        {
-            "x": 2.4840036736729245,
-            "y": 3.218316897029485,
-            "heading": 0.3291273340604081,
-            "angularVelocity": 0.000026837980957261232,
-            "velocityX": -2.0722686271167303,
-            "velocityY": 3.1648493647939433,
-            "timestamp": 1.7549251823347696
-        },
-        {
-            "x": 2.3733884856918945,
-            "y": 3.4659660107751304,
-            "heading": 0.2954274954004952,
-            "angularVelocity": -0.4517068070492687,
-            "velocityX": -1.4826668423638905,
-            "velocityY": 3.31944587532086,
-            "timestamp": 1.829530740927198
-        },
-        {
-            "x": 2.305023098579148,
-            "y": 3.7064756804968377,
-            "heading": 0.19598318894217118,
-            "angularVelocity": -1.3329342790875696,
-            "velocityX": -0.9163578210871337,
-            "velocityY": 3.2237500028063106,
-            "timestamp": 1.9041362995196263
-        },
-        {
-            "x": 2.272475779606133,
-            "y": 3.9200958593762563,
-            "heading": 0.053473340322528436,
-            "angularVelocity": -1.9101773555262378,
-            "velocityX": -0.43625863256144176,
-            "velocityY": 2.8633279196585013,
-            "timestamp": 1.9787418581120546
-        },
-        {
-            "x": 2.2677016800416907,
-            "y": 4.09662450199971,
-            "heading": -0.09830221598718161,
-            "angularVelocity": -2.034373298360552,
-            "velocityX": -0.06399120460344819,
-            "velocityY": 2.3661593848232187,
-            "timestamp": 2.053347416704483
-        },
-        {
-            "x": 2.2778239174156503,
-            "y": 4.229436050413757,
-            "heading": -0.23380831521279008,
-            "angularVelocity": -1.8163003103546322,
-            "velocityX": 0.13567671853055638,
-            "velocityY": 1.7801830174558415,
-            "timestamp": 2.1279529752969113
-        },
-        {
-            "x": 2.29107059750262,
-            "y": 4.316936633046431,
-            "heading": -0.33501872696397506,
-            "angularVelocity": -1.3566068488823915,
-            "velocityX": 0.1775562081015563,
-            "velocityY": 1.1728426712906341,
-            "timestamp": 2.2025585338893396
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.7369594715641588,
-            "velocityX": 0.11968816621507666,
-            "velocityY": 0.5772139203303202,
-            "timestamp": 2.277164092481768
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -8.092865962806098e-32,
-            "velocityX": 0,
-            "velocityY": 4.648171279746289e-33,
-            "timestamp": 2.3517696510741963
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C5 To C1.traj b/src/main/deploy/choreo/C5 To C1.traj
deleted file mode 100644
index 8293ba75..00000000
--- a/src/main/deploy/choreo/C5 To C1.traj	
+++ /dev/null
@@ -1,220 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -1.1520076473353784e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 0.8649552704871435,
-            "heading": -1.2072196888077206e-18,
-            "angularVelocity": -1.2485272283928241e-17,
-            "velocityX": -1.2865550388289577e-22,
-            "velocityY": 0.8062267267095138,
-            "timestamp": 0.09669149868710694
-        },
-        {
-            "x": 7.68,
-            "y": 1.0208658083668138,
-            "heading": -3.6224298175784965e-18,
-            "angularVelocity": -2.497851580797786e-17,
-            "velocityX": -3.7534290948711127e-22,
-            "velocityY": 1.6124534214139756,
-            "timestamp": 0.19338299737421388
-        },
-        {
-            "x": 7.68,
-            "y": 1.254731607449781,
-            "heading": -7.246204108405477e-18,
-            "angularVelocity": -3.7477692864160655e-17,
-            "velocityX": -8.581224837351263e-22,
-            "velocityY": 2.4186800521083573,
-            "timestamp": 0.29007449606132085
-        },
-        {
-            "x": 7.68,
-            "y": 1.5665526491683786,
-            "heading": -1.208276035533523e-17,
-            "angularVelocity": -5.0020491067317066e-17,
-            "velocityX": -2.0493771641846554e-21,
-            "velocityY": 3.2249064907727645,
-            "timestamp": 0.38676599474842777
-        },
-        {
-            "x": 7.68,
-            "y": 1.9323308421442915,
-            "heading": -4.9593132234185194e-18,
-            "angularVelocity": 7.367190734076976e-17,
-            "velocityX": 8.922575847291113e-21,
-            "velocityY": 3.782940568121389,
-            "timestamp": 0.4834574934355347
-        },
-        {
-            "x": 7.68,
-            "y": 2.298109035120243,
-            "heading": 4.533704301231546e-17,
-            "angularVelocity": 5.201735097557347e-16,
-            "velocityX": 4.097272417616201e-22,
-            "velocityY": 3.782940568121787,
-            "timestamp": 0.5801489921226416
-        },
-        {
-            "x": 7.68,
-            "y": 2.6638872280961943,
-            "heading": 2.5191507323029372e-17,
-            "angularVelocity": -2.083485721374208e-16,
-            "velocityX": 1.8682331175835897e-21,
-            "velocityY": 3.782940568121787,
-            "timestamp": 0.6768404908097485
-        },
-        {
-            "x": 7.68,
-            "y": 3.0296654210721456,
-            "heading": 5.2668856754382746e-17,
-            "angularVelocity": 2.8417544256148633e-16,
-            "velocityX": 2.5170076181953607e-21,
-            "velocityY": 3.782940568121787,
-            "timestamp": 0.7735319894968554
-        },
-        {
-            "x": 7.68,
-            "y": 3.395443614048097,
-            "heading": 4.929611386988728e-17,
-            "angularVelocity": -3.48814831737134e-17,
-            "velocityX": 3.0934195236040766e-21,
-            "velocityY": 3.782940568121787,
-            "timestamp": 0.8702234881839623
-        },
-        {
-            "x": 7.68,
-            "y": 3.7612218070240484,
-            "heading": 4.8005098046144365e-17,
-            "angularVelocity": -1.3351906227498234e-17,
-            "velocityX": -1.4483318353601114e-20,
-            "velocityY": 3.782940568121787,
-            "timestamp": 0.9669149868710692
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.8136822799879804e-17,
-            "angularVelocity": -1.0205938867695289e-16,
-            "velocityX": -1.0465805642283078e-22,
-            "velocityY": 3.782940568121787,
-            "timestamp": 1.0636064855581762
-        },
-        {
-            "x": 7.68,
-            "y": 4.492778192975952,
-            "heading": 4.310486593468924e-17,
-            "angularVelocity": 5.1380350929111975e-17,
-            "velocityX": 2.042774398860212e-21,
-            "velocityY": 3.782940568121787,
-            "timestamp": 1.1602979842452832
-        },
-        {
-            "x": 7.68,
-            "y": 4.8585563859519025,
-            "heading": 1.5339712420766394e-17,
-            "angularVelocity": -2.8715196155735124e-16,
-            "velocityX": 3.895366641644577e-22,
-            "velocityY": 3.782940568121787,
-            "timestamp": 1.2569894829323902
-        },
-        {
-            "x": 7.68,
-            "y": 5.224334578927854,
-            "heading": -2.710858238227145e-17,
-            "angularVelocity": -4.390075175101735e-16,
-            "velocityX": -3.863371094269421e-21,
-            "velocityY": 3.782940568121787,
-            "timestamp": 1.3536809816194972
-        },
-        {
-            "x": 7.68,
-            "y": 5.590112771903805,
-            "heading": -3.378098304971619e-17,
-            "angularVelocity": -6.900710774001486e-17,
-            "velocityX": -4.042329982282679e-22,
-            "velocityY": 3.782940568121787,
-            "timestamp": 1.4503724803066043
-        },
-        {
-            "x": 7.68,
-            "y": 5.955890964879757,
-            "heading": -3.941496183315801e-17,
-            "angularVelocity": -5.826757119208287e-17,
-            "velocityX": 2.6937052538433574e-21,
-            "velocityY": 3.782940568121787,
-            "timestamp": 1.5470639789937113
-        },
-        {
-            "x": 7.68,
-            "y": 6.321669157855708,
-            "heading": -1.9923808577496458e-17,
-            "angularVelocity": 2.0158083720198419e-16,
-            "velocityX": -4.499746264941967e-21,
-            "velocityY": 3.782940568121787,
-            "timestamp": 1.6437554776808183
-        },
-        {
-            "x": 7.68,
-            "y": 6.68744735083162,
-            "heading": 1.1959451428437676e-17,
-            "angularVelocity": 3.297421225035248e-16,
-            "velocityX": 5.431014750114918e-21,
-            "velocityY": 3.782940568121389,
-            "timestamp": 1.7404469763679253
-        },
-        {
-            "x": 7.68,
-            "y": 6.999268392550218,
-            "heading": 7.18685516064291e-18,
-            "angularVelocity": -4.935900605111643e-17,
-            "velocityX": -2.4046856363551417e-22,
-            "velocityY": 3.2249064907727645,
-            "timestamp": 1.8371384750550324
-        },
-        {
-            "x": 7.68,
-            "y": 7.2331341916331855,
-            "heading": 3.596188330325507e-18,
-            "angularVelocity": -3.713528985888002e-17,
-            "velocityX": -1.8035546521541034e-22,
-            "velocityY": 2.4186800521083573,
-            "timestamp": 1.9338299737421394
-        },
-        {
-            "x": 7.68,
-            "y": 7.3890447295128565,
-            "heading": 1.199348678010309e-18,
-            "angularVelocity": -2.4788525206448663e-17,
-            "velocityX": -1.2023814854884031e-22,
-            "velocityY": 1.6124534214139756,
-            "timestamp": 2.030521472429246
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": -1.2403868947555692e-17,
-            "velocityX": -6.011942579686287e-23,
-            "velocityY": 0.8062267267095138,
-            "timestamp": 2.127212971116353
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -2.719452927302828e-33,
-            "velocityY": 0,
-            "timestamp": 2.2239044698034602
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C5 To C2.traj b/src/main/deploy/choreo/C5 To C2.traj
deleted file mode 100644
index f16ed7e1..00000000
--- a/src/main/deploy/choreo/C5 To C2.traj	
+++ /dev/null
@@ -1,175 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.5195168592304314e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 0.8688441539517631,
-            "heading": 1.4487899486085006e-18,
-            "angularVelocity": 1.4623322004407643e-17,
-            "velocityX": 2.341988908112997e-19,
-            "velocityY": 0.8260917454276263,
-            "timestamp": 0.09907392781100396
-        },
-        {
-            "x": 7.68,
-            "y": 1.03253245874193,
-            "heading": 4.3483011920968684e-18,
-            "angularVelocity": 2.926613800008521e-17,
-            "velocityX": 4.684469740721679e-19,
-            "velocityY": 1.6521834594306495,
-            "timestamp": 0.19814785562200793
-        },
-        {
-            "x": 7.68,
-            "y": 1.2780649081437847,
-            "heading": 8.701494230539972e-18,
-            "angularVelocity": 4.393883572228609e-17,
-            "velocityX": 7.027934476495029e-19,
-            "velocityY": 2.4782751105844794,
-            "timestamp": 0.29722178343301187
-        },
-        {
-            "x": 7.68,
-            "y": 1.6054414834771922,
-            "heading": 1.4516368843485863e-17,
-            "angularVelocity": 5.869227900187472e-17,
-            "velocityX": 9.37435089599571e-19,
-            "velocityY": 3.304366573190877,
-            "timestamp": 0.39629571124401586
-        },
-        {
-            "x": 7.68,
-            "y": 1.9802322649266864,
-            "heading": 6.647154079192576e-18,
-            "angularVelocity": -7.942770553424421e-17,
-            "velocityX": 1.0709424285811041e-18,
-            "velocityY": 3.782940575087071,
-            "timestamp": 0.49536963905501985
-        },
-        {
-            "x": 7.68,
-            "y": 2.3550230463762047,
-            "heading": -4.11606275908414e-17,
-            "angularVelocity": -4.825465460619937e-16,
-            "velocityX": 1.073083844244948e-18,
-            "velocityY": 3.7829405750873115,
-            "timestamp": 0.5944435668660238
-        },
-        {
-            "x": 7.68,
-            "y": 2.729813827825723,
-            "heading": -7.635066611173202e-17,
-            "angularVelocity": -3.5518969822232415e-16,
-            "velocityX": 1.0738946259920485e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.6935174946770278
-        },
-        {
-            "x": 7.68,
-            "y": 3.1046046092752415,
-            "heading": -5.864574302033697e-17,
-            "angularVelocity": 1.787041604453991e-16,
-            "velocityX": 1.0691415340399175e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.7925914224880318
-        },
-        {
-            "x": 7.68,
-            "y": 3.4793953907247595,
-            "heading": -2.6391173603704442e-17,
-            "angularVelocity": 3.255606205313886e-16,
-            "velocityX": 1.0732289061667476e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.8916653502990358
-        },
-        {
-            "x": 7.68,
-            "y": 3.854186172174278,
-            "heading": -6.511485011951871e-17,
-            "angularVelocity": -3.9085637736788184e-16,
-            "velocityX": 1.0716697150075795e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 0.9907392781100398
-        },
-        {
-            "x": 7.68,
-            "y": 4.228976953623796,
-            "heading": -6.241329479526944e-17,
-            "angularVelocity": 2.7268075304360027e-17,
-            "velocityX": 1.0686111075844087e-18,
-            "velocityY": 3.782940575087312,
-            "timestamp": 1.0898132059210437
-        },
-        {
-            "x": 7.68,
-            "y": 4.603767735073314,
-            "heading": -2.2737833863559818e-17,
-            "angularVelocity": 4.004631875225205e-16,
-            "velocityX": 1.0710197916722359e-18,
-            "velocityY": 3.7829405750873115,
-            "timestamp": 1.1888871337320477
-        },
-        {
-            "x": 7.68,
-            "y": 4.978558516522808,
-            "heading": -1.448944579275074e-17,
-            "angularVelocity": 8.325488100707829e-17,
-            "velocityX": 1.0714504382145178e-18,
-            "velocityY": 3.782940575087071,
-            "timestamp": 1.2879610615430517
-        },
-        {
-            "x": 7.68,
-            "y": 5.305935091856216,
-            "heading": -8.687037170545135e-18,
-            "angularVelocity": 5.856645386228716e-17,
-            "velocityX": 9.36753995288457e-19,
-            "velocityY": 3.304366573190877,
-            "timestamp": 1.3870349893540557
-        },
-        {
-            "x": 7.68,
-            "y": 5.55146754125807,
-            "heading": -4.341905072762387e-18,
-            "angularVelocity": 4.385747283685971e-17,
-            "velocityX": 7.025655481927048e-19,
-            "velocityY": 2.4782751105844794,
-            "timestamp": 1.4861089171650597
-        },
-        {
-            "x": 7.68,
-            "y": 5.715155846048237,
-            "heading": -1.4469357825353783e-18,
-            "angularVelocity": 2.9220293918225814e-17,
-            "velocityX": 4.683770471216647e-19,
-            "velocityY": 1.6521834594306495,
-            "timestamp": 1.5851828449760637
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 1.460460702963924e-17,
-            "velocityX": 2.341885280588165e-19,
-            "velocityY": 0.8260917454276263,
-            "timestamp": 1.6842567727870676
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 3.1812192470207854e-34,
-            "velocityY": 0,
-            "timestamp": 1.7833307005980716
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C5 To C3.traj b/src/main/deploy/choreo/C5 To C3.traj
deleted file mode 100644
index 864eb2e4..00000000
--- a/src/main/deploy/choreo/C5 To C3.traj	
+++ /dev/null
@@ -1,139 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 2.417290808823166e-40,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 0.8634453720055388,
-            "heading": 1.009810999063182e-18,
-            "angularVelocity": 1.0546271494620871e-17,
-            "velocityX": -3.226662245674165e-34,
-            "velocityY": 0.7983807350946677,
-            "timestamp": 0.09575052183150978
-        },
-        {
-            "x": 7.68,
-            "y": 1.0163361129826893,
-            "heading": 3.0323418642587253e-18,
-            "angularVelocity": 2.112292263585315e-17,
-            "velocityX": -6.037770263383471e-34,
-            "velocityY": 1.596761438503583,
-            "timestamp": 0.19150104366301957
-        },
-        {
-            "x": 7.68,
-            "y": 1.2456722168635992,
-            "heading": 6.073900773319643e-18,
-            "angularVelocity": 3.1765455168164525e-17,
-            "velocityX": -4.821612934727609e-34,
-            "velocityY": 2.3951420785410233,
-            "timestamp": 0.28725156549452935
-        },
-        {
-            "x": 7.68,
-            "y": 1.5514536654447455,
-            "heading": 1.0152788689831222e-17,
-            "angularVelocity": 4.2599119433487623e-17,
-            "velocityX": -1.2140491970354773e-33,
-            "velocityY": 3.19352252846437,
-            "timestamp": 0.38300208732603913
-        },
-        {
-            "x": 7.68,
-            "y": 1.9136721992668209,
-            "heading": 4.411917253468209e-18,
-            "angularVelocity": -5.995655508243724e-17,
-            "velocityX": 4.90795348832912e-33,
-            "velocityY": 3.7829405719528495,
-            "timestamp": 0.4787526091575489
-        },
-        {
-            "x": 7.68,
-            "y": 2.27589073308894,
-            "heading": 1.2796107303848674e-17,
-            "angularVelocity": 8.756286535058141e-17,
-            "velocityX": -1.5435756690671252e-33,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.5745031309890587
-        },
-        {
-            "x": 7.68,
-            "y": 2.63810926691106,
-            "heading": 6.994895399550348e-18,
-            "angularVelocity": -6.058673930264937e-17,
-            "velocityX": 4.776897057793067e-34,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.6702536528205685
-        },
-        {
-            "x": 7.68,
-            "y": 3.0003278007331793,
-            "heading": -9.534020306729642e-18,
-            "angularVelocity": -1.7262481070720064e-16,
-            "velocityX": 2.9275045647973183e-33,
-            "velocityY": 3.782940571953311,
-            "timestamp": 0.7660041746520783
-        },
-        {
-            "x": 7.68,
-            "y": 3.3625463345552546,
-            "heading": -1.0013819273727212e-17,
-            "angularVelocity": -5.010927959660929e-18,
-            "velocityX": 2.237784317663532e-33,
-            "velocityY": 3.7829405719528495,
-            "timestamp": 0.861754696483588
-        },
-        {
-            "x": 7.68,
-            "y": 3.6683277831364007,
-            "heading": -6.0064636534747415e-18,
-            "angularVelocity": 4.185204993640188e-17,
-            "velocityX": -2.2313398142102424e-33,
-            "velocityY": 3.19352252846437,
-            "timestamp": 0.9575052183150978
-        },
-        {
-            "x": 7.68,
-            "y": 3.897663887017311,
-            "heading": -3.002975312877452e-18,
-            "angularVelocity": 3.136785349810471e-17,
-            "velocityX": -2.8833027864886262e-33,
-            "velocityY": 2.3951420785410233,
-            "timestamp": 1.0532557401466076
-        },
-        {
-            "x": 7.68,
-            "y": 4.050554627994461,
-            "heading": -1.0009557447862449e-18,
-            "angularVelocity": 2.0908706606963636e-17,
-            "velocityX": -3.742421273800505e-33,
-            "velocityY": 1.596761438503583,
-            "timestamp": 1.1490062619781174
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 1.0453788927770355e-17,
-            "velocityX": -1.5032681111068507e-33,
-            "velocityY": 0.7983807350946677,
-            "timestamp": 1.2447567838096272
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 9.665729080515376e-43,
-            "velocityY": 2.80463986608038e-38,
-            "timestamp": 1.340507305641137
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C5 To C4.traj b/src/main/deploy/choreo/C5 To C4.traj
deleted file mode 100644
index 17170fc9..00000000
--- a/src/main/deploy/choreo/C5 To C4.traj	
+++ /dev/null
@@ -1,94 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 9.069206296242399e-40,
-            "angularVelocity": -9.927610039652183e-40,
-            "velocityX": -8.253947176298157e-40,
-            "velocityY": 1.8367099231598242e-40,
-            "timestamp": 0
-        },
-        {
-            "x": 7.68,
-            "y": 0.8705000055758465,
-            "heading": 6.490773345690525e-22,
-            "angularVelocity": 6.486159683422905e-21,
-            "velocityX": -7.132918469580153e-33,
-            "velocityY": 0.8344065481228213,
-            "timestamp": 0.10007112931184182
-        },
-        {
-            "x": 7.68,
-            "y": 1.037500013629847,
-            "heading": 2.023593010760001e-21,
-            "angularVelocity": 1.3735386658326633e-20,
-            "velocityX": -1.5648094960655976e-32,
-            "velocityY": 1.668813065290736,
-            "timestamp": 0.20014225862368365
-        },
-        {
-            "x": 7.68,
-            "y": 1.2880000179666162,
-            "heading": 3.787875676222919e-21,
-            "angularVelocity": 1.7630286060953193e-20,
-            "velocityX": -2.3089760494009302e-32,
-            "velocityY": 2.503219520548835,
-            "timestamp": 0.3002133879355255
-        },
-        {
-            "x": 7.68,
-            "y": 1.6219999999999999,
-            "heading": 5.860880748165694e-21,
-            "angularVelocity": 2.0715315514205814e-20,
-            "velocityX": -3.0704903308303665e-32,
-            "velocityY": 3.3376257900775013,
-            "timestamp": 0.4002845172473673
-        },
-        {
-            "x": 7.68,
-            "y": 1.9559999820333838,
-            "heading": 3.934658943161884e-21,
-            "angularVelocity": -1.9248526176341983e-20,
-            "velocityX": -4.040562356218352e-32,
-            "velocityY": 3.3376257900775017,
-            "timestamp": 0.5003556465592092
-        },
-        {
-            "x": 7.68,
-            "y": 2.206499986370153,
-            "heading": 1.6176834885162177e-21,
-            "angularVelocity": -2.3153285409554772e-20,
-            "velocityX": -3.08903871620124e-32,
-            "velocityY": 2.503219520548835,
-            "timestamp": 0.600426775871051
-        },
-        {
-            "x": 7.68,
-            "y": 2.3734999944241535,
-            "heading": 4.992981261257204e-22,
-            "angularVelocity": -1.1175904096118944e-20,
-            "velocityX": -2.3331216062245415e-32,
-            "velocityY": 1.668813065290736,
-            "timestamp": 0.7004979051828928
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 1.7562757360140444e-40,
-            "angularVelocity": -4.9894322348928345e-21,
-            "velocityX": -1.3647931662662733e-32,
-            "velocityY": 0.8344065481228213,
-            "timestamp": 0.8005690344947346
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -7.274275970183695e-41,
-            "angularVelocity": -2.4819026120794767e-39,
-            "velocityX": -8.637198685981915e-42,
-            "velocityY": 5.142787784847508e-39,
-            "timestamp": 0.9006401638065764
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C5 To S1.traj b/src/main/deploy/choreo/C5 To S1.traj
deleted file mode 100644
index 73f0ef42..00000000
--- a/src/main/deploy/choreo/C5 To S1.traj	
+++ /dev/null
@@ -1,292 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 4.273608378959021e-31,
-            "velocityX": -7.420825794722196e-33,
-            "velocityY": 7.583788429881001e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 7.678491547493202,
-            "y": 0.8436471873828707,
-            "heading": -0.006289831251442517,
-            "angularVelocity": -0.0762785379812196,
-            "velocityX": -0.018293424295956956,
-            "velocityY": 0.686976241742982,
-            "timestamp": 0.08245872846948342
-        },
-        {
-            "x": 7.67547464227306,
-            "y": 0.9569415605229419,
-            "heading": -0.018869443032064528,
-            "angularVelocity": -0.1525564608394078,
-            "velocityX": -0.03658685109677269,
-            "velocityY": 1.3739524637710059,
-            "timestamp": 0.16491745693896684
-        },
-        {
-            "x": 7.670949285584863,
-            "y": 1.1268831170081794,
-            "heading": -0.03773871517098893,
-            "angularVelocity": -0.22883292635185057,
-            "velocityX": -0.05488026279561822,
-            "velocityY": 2.060928656547622,
-            "timestamp": 0.24737618540845024
-        },
-        {
-            "x": 7.664915480002808,
-            "y": 1.3534718520373763,
-            "heading": -0.06289740953278049,
-            "angularVelocity": -0.3051065039294483,
-            "velocityX": -0.07317364327643945,
-            "velocityY": 2.7479047910986583,
-            "timestamp": 0.3298349138779337
-        },
-        {
-            "x": 7.657373229393316,
-            "y": 1.6367077493690674,
-            "heading": -0.09434508673453766,
-            "angularVelocity": -0.38137475298804396,
-            "velocityX": -0.09146697686811679,
-            "velocityY": 3.4348807286849197,
-            "timestamp": 0.4122936423474171
-        },
-        {
-            "x": 7.649069464629009,
-            "y": 1.9485336685806538,
-            "heading": -0.09434509076634809,
-            "angularVelocity": -4.889488962341613e-8,
-            "velocityX": -0.10070207142933957,
-            "velocityY": 3.781599898511507,
-            "timestamp": 0.49475237081690054
-        },
-        {
-            "x": 7.6407656424773585,
-            "y": 2.260359586264075,
-            "heading": -0.09434509479812381,
-            "angularVelocity": -4.889446872332144e-8,
-            "velocityX": -0.1007027673816724,
-            "velocityY": 3.781599879979021,
-            "timestamp": 0.5772110992863839
-        },
-        {
-            "x": 7.63246175736497,
-            "y": 2.5721855022708704,
-            "heading": -0.09434509882989094,
-            "angularVelocity": -4.889436444733497e-8,
-            "velocityX": -0.10070353092409444,
-            "velocityY": 3.7815998596461093,
-            "timestamp": 0.6596698277558674
-        },
-        {
-            "x": 7.6241578045918486,
-            "y": 2.8840114164758646,
-            "heading": -0.09434510286165439,
-            "angularVelocity": -4.8894319896339695e-8,
-            "velocityX": -0.10070435146468282,
-            "velocityY": 3.781599837795173,
-            "timestamp": 0.7421285562253508
-        },
-        {
-            "x": 7.615853780314312,
-            "y": 3.195837328776685,
-            "heading": -0.09434510689341911,
-            "angularVelocity": -4.889433549543518e-8,
-            "velocityX": -0.10070521861867329,
-            "velocityY": 3.78159981470272,
-            "timestamp": 0.8245872846948342
-        },
-        {
-            "x": 7.607549681647345,
-            "y": 3.5076632390964844,
-            "heading": -0.09434511092518724,
-            "angularVelocity": -4.889437661104965e-8,
-            "velocityX": -0.10070612076003535,
-            "velocityY": 3.7815997906783263,
-            "timestamp": 0.9070460131643177
-        },
-        {
-            "x": 7.599245507241111,
-            "y": 3.819489147399297,
-            "heading": -0.0943451149569584,
-            "angularVelocity": -4.889441348359375e-8,
-            "velocityX": -0.10070703927126574,
-            "velocityY": 3.7815997662177643,
-            "timestamp": 0.9895047416338011
-        },
-        {
-            "x": 7.590941256893013,
-            "y": 4.131315053679708,
-            "heading": -0.09434511898873191,
-            "angularVelocity": -4.88944418703562e-8,
-            "velocityX": -0.10070796023943433,
-            "velocityY": 3.781599741691539,
-            "timestamp": 1.0719634701032845
-        },
-        {
-            "x": 7.582636930618974,
-            "y": 4.443140957938123,
-            "heading": -0.09434512302050634,
-            "angularVelocity": -4.889445311998471e-8,
-            "velocityX": -0.10070888101448454,
-            "velocityY": 3.781599717170225,
-            "timestamp": 1.1544221985727678
-        },
-        {
-            "x": 7.574332505518219,
-            "y": 4.754966859564639,
-            "heading": -0.09434512705227821,
-            "angularVelocity": -4.889442208864286e-8,
-            "velocityX": -0.10071007951363715,
-            "velocityY": 3.78159968525245,
-            "timestamp": 1.2368809270422512
-        },
-        {
-            "x": 7.562081052721255,
-            "y": 5.066662637971005,
-            "heading": -0.09434513109398723,
-            "angularVelocity": -4.901493268640619e-8,
-            "velocityX": -0.1485767853126217,
-            "velocityY": 3.7800216446669976,
-            "timestamp": 1.3193396555117345
-        },
-        {
-            "x": 7.493618412578439,
-            "y": 5.370993415792126,
-            "heading": -0.09434513545729191,
-            "angularVelocity": -5.29150129274595e-8,
-            "velocityX": -0.8302655329951328,
-            "velocityY": 3.6907042282825056,
-            "timestamp": 1.4017983839812178
-        },
-        {
-            "x": 7.37120325739307,
-            "y": 5.657906089273735,
-            "heading": -0.09434514092208332,
-            "angularVelocity": -6.627304961707395e-8,
-            "velocityX": -1.4845627316539582,
-            "velocityY": 3.479470018601967,
-            "timestamp": 1.4842571124507011
-        },
-        {
-            "x": 7.198879241943359,
-            "y": 5.9179229736328125,
-            "heading": -0.09434514894422652,
-            "angularVelocity": -9.728676819194244e-8,
-            "velocityX": -2.089821400938593,
-            "velocityY": 3.153297281988846,
-            "timestamp": 1.5667158409201845
-        },
-        {
-            "x": 7.016660952404083,
-            "y": 6.116497130944734,
-            "heading": -0.09434515627038953,
-            "angularVelocity": -1.028329950777357e-7,
-            "velocityX": -2.5576898068579945,
-            "velocityY": 2.7872673997010713,
-            "timestamp": 1.6379591516026175
-        },
-        {
-            "x": 6.805603401067166,
-            "y": 6.284097595272953,
-            "heading": -0.09434516139165254,
-            "angularVelocity": -7.188412437441302e-8,
-            "velocityX": -2.9624893806199624,
-            "velocityY": 2.352508084236811,
-            "timestamp": 1.7092024622850506
-        },
-        {
-            "x": 6.57091097390028,
-            "y": 6.416591642615984,
-            "heading": -0.09434516555011045,
-            "angularVelocity": -5.83698017255904e-8,
-            "velocityX": -3.2942380824078485,
-            "velocityY": 1.8597401787463752,
-            "timestamp": 1.7804457729674836
-        },
-        {
-            "x": 6.318370883291981,
-            "y": 6.510712371224652,
-            "heading": -0.09434516933867851,
-            "angularVelocity": -5.317787750894471e-8,
-            "velocityX": -3.544755124224867,
-            "velocityY": 1.3211167154795889,
-            "timestamp": 1.8516890836499167
-        },
-        {
-            "x": 6.054210380502568,
-            "y": 6.5641393107028225,
-            "heading": -0.09434515209433943,
-            "angularVelocity": 2.420485363909629e-7,
-            "velocityX": -3.7078639420184567,
-            "velocityY": 0.7499221887135695,
-            "timestamp": 1.9229323943323497
-        },
-        {
-            "x": 5.803536575930751,
-            "y": 6.585943895129495,
-            "heading": -0.03682720962895,
-            "angularVelocity": 0.8073451656643992,
-            "velocityX": -3.5185591765828246,
-            "velocityY": 0.30605798941413415,
-            "timestamp": 1.9941757050147828
-        },
-        {
-            "x": 5.594045383796189,
-            "y": 6.601646508747958,
-            "heading": 0.04817170409455108,
-            "angularVelocity": 1.1930792225867084,
-            "velocityX": -2.9405033276509123,
-            "velocityY": 0.2204082526211729,
-            "timestamp": 2.065419015697216
-        },
-        {
-            "x": 5.426658878791764,
-            "y": 6.613398993876379,
-            "heading": 0.12734083591800166,
-            "angularVelocity": 1.111250039689289,
-            "velocityX": -2.3495048644012306,
-            "velocityY": 0.16496264724146908,
-            "timestamp": 2.136662326379649
-        },
-        {
-            "x": 5.301258489415666,
-            "y": 6.621853943751229,
-            "heading": 0.1915936210653456,
-            "angularVelocity": 0.9018781487254319,
-            "velocityX": -1.7601707188351954,
-            "velocityY": 0.11867710517466466,
-            "timestamp": 2.207905637062082
-        },
-        {
-            "x": 5.217732575552286,
-            "y": 6.627325619944874,
-            "heading": 0.2366430637253094,
-            "angularVelocity": 0.6323322460514409,
-            "velocityX": -1.1724035992052257,
-            "velocityY": 0.07680266597989953,
-            "timestamp": 2.279148947744515
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 0.32784742947732926,
-            "velocityX": -0.5857753542407405,
-            "velocityY": 0.037538682993647744,
-            "timestamp": 2.350392258426948
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": -8.68876182413906e-32,
-            "velocityX": 0,
-            "velocityY": -2.1409750340846636e-32,
-            "timestamp": 2.421635569109381
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C5 To S2.traj b/src/main/deploy/choreo/C5 To S2.traj
deleted file mode 100644
index dfbc040a..00000000
--- a/src/main/deploy/choreo/C5 To S2.traj	
+++ /dev/null
@@ -1,247 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -1.3156383032943886e-37,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.664225467591612,
-            "y": 0.8361290574305452,
-            "heading": -0.011405957532827086,
-            "angularVelocity": -0.14485338302751044,
-            "velocityX": -0.20033341159264173,
-            "velocityY": 0.6239292188565086,
-            "timestamp": 0.07874139557141634
-        },
-        {
-            "x": 7.632676408078482,
-            "y": 0.9343872155736361,
-            "heading": -0.03421427612768491,
-            "angularVelocity": -0.2896610915940815,
-            "velocityX": -0.4006667558300489,
-            "velocityY": 1.2478589873857826,
-            "timestamp": 0.15748279114283267
-        },
-        {
-            "x": 7.585352811707443,
-            "y": 1.0817745497576996,
-            "heading": -0.06841862032572539,
-            "angularVelocity": -0.4343883410984004,
-            "velocityX": -0.6010002239307173,
-            "velocityY": 1.8717897125710332,
-            "timestamp": 0.236224186714249
-        },
-        {
-            "x": 7.522254637419436,
-            "y": 1.278291160108196,
-            "heading": -0.11400992700252059,
-            "angularVelocity": -0.5790004907322871,
-            "velocityX": -0.8013342134732577,
-            "velocityY": 2.4957217093296378,
-            "timestamp": 0.31496558228566535
-        },
-        {
-            "x": 7.443381794868827,
-            "y": 1.5239371518673812,
-            "heading": -0.1709767215367488,
-            "angularVelocity": -0.723466914966739,
-            "velocityX": -1.0016693503872847,
-            "velocityY": 3.1196550426438785,
-            "timestamp": 0.3937069778570817
-        },
-        {
-            "x": 7.3523133020023,
-            "y": 1.807548605142893,
-            "heading": -0.17097672945138376,
-            "angularVelocity": -1.00514283666892e-7,
-            "velocityX": -1.1565516740674315,
-            "velocityY": 3.6018088226323544,
-            "timestamp": 0.47244837342849805
-        },
-        {
-            "x": 7.26124476293705,
-            "y": 2.0911600435840003,
-            "heading": -0.17097673736581726,
-            "angularVelocity": -1.0051172494783456e-7,
-            "velocityX": -1.1565522607819834,
-            "velocityY": 3.6018086342383846,
-            "timestamp": 0.5511897689999143
-        },
-        {
-            "x": 7.170176175790629,
-            "y": 2.3747714665860937,
-            "heading": -0.17097674528023646,
-            "angularVelocity": -1.0051154369078753e-7,
-            "velocityX": -1.1565528714032516,
-            "velocityY": 3.601808438165991,
-            "timestamp": 0.6299311645713307
-        },
-        {
-            "x": 7.079107531002594,
-            "y": 2.6583828710792723,
-            "heading": -0.17097675319464586,
-            "angularVelocity": -1.0051141913755183e-7,
-            "velocityX": -1.1565536034402273,
-            "velocityY": 3.601808203106465,
-            "timestamp": 0.708672560142747
-        },
-        {
-            "x": 6.988038816793172,
-            "y": 2.9419942532809915,
-            "heading": -0.17097676110905413,
-            "angularVelocity": -1.0051140450859988e-7,
-            "velocityX": -1.1565544850779974,
-            "velocityY": 3.601807920009381,
-            "timestamp": 0.7874139557141634
-        },
-        {
-            "x": 6.896968817946089,
-            "y": 3.2256052229773635,
-            "heading": -0.17097676902348397,
-            "angularVelocity": -1.0051167869642574e-7,
-            "velocityX": -1.156570799719756,
-            "velocityY": 3.6018026812738513,
-            "timestamp": 0.8661553512855797
-        },
-        {
-            "x": 6.765153291475094,
-            "y": 3.4927261567236307,
-            "heading": -0.1709767773203105,
-            "angularVelocity": -1.053680400636008e-7,
-            "velocityX": -1.6740308641271406,
-            "velocityY": 3.392382517579277,
-            "timestamp": 0.9448967468569961
-        },
-        {
-            "x": 6.589137393143544,
-            "y": 3.7330328933669676,
-            "heading": -0.17097678717611917,
-            "angularVelocity": -1.2516680215717606e-7,
-            "velocityX": -2.235366760446956,
-            "velocityY": 3.0518475688608455,
-            "timestamp": 1.0236381424284124
-        },
-        {
-            "x": 6.374222917834714,
-            "y": 3.939286833341542,
-            "heading": -0.17097680095561982,
-            "angularVelocity": -1.7499690668870768e-7,
-            "velocityX": -2.7293709204570815,
-            "velocityY": 2.6193838511219654,
-            "timestamp": 1.1023795379998287
-        },
-        {
-            "x": 6.126883506774902,
-            "y": 4.1052751541137695,
-            "heading": -0.17097682837855596,
-            "angularVelocity": -3.482658130630958e-7,
-            "velocityX": -3.1411611296053437,
-            "velocityY": 2.108018527836233,
-            "timestamp": 1.181120933571245
-        },
-        {
-            "x": 5.792856415059812,
-            "y": 4.239084767822678,
-            "heading": -0.17097684795948573,
-            "angularVelocity": -2.0585572281638246e-7,
-            "velocityX": -3.5116508401962974,
-            "velocityY": 1.4067500932170736,
-            "timestamp": 1.2762406100184678
-        },
-        {
-            "x": 5.438271606876556,
-            "y": 4.300308177252639,
-            "heading": -0.1709790019096565,
-            "angularVelocity": -0.000022644633069121558,
-            "velocityX": -3.7277755920458318,
-            "velocityY": 0.6436461068486719,
-            "timestamp": 1.3713602864656906
-        },
-        {
-            "x": 5.110881399181515,
-            "y": 4.377812934831655,
-            "heading": -0.2483895057711766,
-            "angularVelocity": -0.8138221948691259,
-            "velocityX": -3.4418768011337173,
-            "velocityY": 0.8148130909803968,
-            "timestamp": 1.4664799629129135
-        },
-        {
-            "x": 4.831528638348494,
-            "y": 4.508515906317057,
-            "heading": -0.41196599842011083,
-            "angularVelocity": -1.7196914325049715,
-            "velocityX": -2.936855667165985,
-            "velocityY": 1.374089740075198,
-            "timestamp": 1.5615996393601363
-        },
-        {
-            "x": 4.597313308728367,
-            "y": 4.699607232649008,
-            "heading": -0.5851564423497488,
-            "angularVelocity": -1.8207635938052467,
-            "velocityX": -2.4623226063019743,
-            "velocityY": 2.0089568580269215,
-            "timestamp": 1.6567193158073592
-        },
-        {
-            "x": 4.422373656654268,
-            "y": 4.922240435570765,
-            "heading": -0.6321779806524386,
-            "angularVelocity": -0.49434081421397164,
-            "velocityX": -1.8391531448402672,
-            "velocityY": 2.340558875274187,
-            "timestamp": 1.751838992254582
-        },
-        {
-            "x": 4.3095074978210945,
-            "y": 5.147658123857161,
-            "heading": -0.526303235570326,
-            "angularVelocity": 1.11306881012003,
-            "velocityX": -1.1865700457443962,
-            "velocityY": 2.3698323701875594,
-            "timestamp": 1.846958668701805
-        },
-        {
-            "x": 4.263497791071102,
-            "y": 5.351217934034869,
-            "heading": -0.3174810959543699,
-            "angularVelocity": 2.1953621733755666,
-            "velocityX": -0.48370335632419137,
-            "velocityY": 2.1400389254967007,
-            "timestamp": 1.9420783451490278
-        },
-        {
-            "x": 4.258299402660224,
-            "y": 5.491389588363689,
-            "heading": -0.11860771311510487,
-            "angularVelocity": 2.090770177814994,
-            "velocityX": -0.05465103125915711,
-            "velocityY": 1.4736346838458172,
-            "timestamp": 2.0371980215962506
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 6.542074475732666e-36,
-            "angularVelocity": 1.2469314188732994,
-            "velocityX": 0.04941771792488758,
-            "velocityY": 0.7213061923563153,
-            "timestamp": 2.1323176980434737
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -6.642487421560091e-37,
-            "velocityY": 5.186129384888016e-33,
-            "timestamp": 2.227437374490697
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C5 To S3.traj b/src/main/deploy/choreo/C5 To S3.traj
deleted file mode 100644
index d9cc7c97..00000000
--- a/src/main/deploy/choreo/C5 To S3.traj	
+++ /dev/null
@@ -1,247 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -3.982415645335998e-31,
-            "velocityX": 3.346280928312519e-33,
-            "velocityY": 3.541078180282768e-38,
-            "timestamp": 0
-        },
-        {
-            "x": 7.629395539536042,
-            "y": 0.7881897280541793,
-            "heading": -0.02207380596023684,
-            "angularVelocity": -0.2822732032732244,
-            "velocityX": -0.6471146471374848,
-            "velocityY": 0.015213885156899113,
-            "timestamp": 0.0782001468948451
-        },
-        {
-            "x": 7.528184091504648,
-            "y": 0.7905692108612611,
-            "heading": -0.06614402373093474,
-            "angularVelocity": -0.5635567133951742,
-            "velocityX": -1.2942616101155584,
-            "velocityY": 0.030428111729043,
-            "timestamp": 0.1564002937896902
-        },
-        {
-            "x": 7.376360973312952,
-            "y": 0.7941378827255743,
-            "heading": -0.13206235338942335,
-            "angularVelocity": -0.8429438086302324,
-            "velocityX": -1.9414684526853105,
-            "velocityY": 0.04563510435477082,
-            "timestamp": 0.23460044068453528
-        },
-        {
-            "x": 7.173916466766183,
-            "y": 0.7988917309385671,
-            "heading": -0.21951772550448787,
-            "angularVelocity": -1.1183530413716716,
-            "velocityX": -2.588799568612108,
-            "velocityY": 0.060790783668257076,
-            "timestamp": 0.3128005875793804
-        },
-        {
-            "x": 6.920824289019536,
-            "y": 0.8048108023879232,
-            "heading": -0.3276760191057486,
-            "angularVelocity": -1.383095785572821,
-            "velocityX": -3.236466781667369,
-            "velocityY": 0.07569130857196414,
-            "timestamp": 0.3910007344742255
-        },
-        {
-            "x": 6.625087122175782,
-            "y": 0.8120803100358527,
-            "heading": -0.3276760358371292,
-            "angularVelocity": -2.1395587179409026e-7,
-            "velocityX": -3.781798098684327,
-            "velocityY": 0.09296028123113304,
-            "timestamp": 0.4692008813690706
-        },
-        {
-            "x": 6.329343587367458,
-            "y": 0.8190859886904246,
-            "heading": -0.3276760513572638,
-            "angularVelocity": -1.9846682128968708e-7,
-            "velocityX": -3.78187953030339,
-            "velocityY": 0.08958651527359804,
-            "timestamp": 0.5474010282639157
-        },
-        {
-            "x": 6.033600055760057,
-            "y": 0.826091802470254,
-            "heading": -0.3276760668773978,
-            "angularVelocity": -1.9846681457625392e-7,
-            "velocityX": -3.7818794893709464,
-            "velocityY": 0.0895882432148227,
-            "timestamp": 0.6256011751587608
-        },
-        {
-            "x": 5.7383102527596765,
-            "y": 0.8439033174946344,
-            "heading": -0.3276760825327013,
-            "angularVelocity": -2.0019532088764418e-7,
-            "velocityX": -3.7760773441696487,
-            "velocityY": 0.22776830647924015,
-            "timestamp": 0.7038013220536059
-        },
-        {
-            "x": 5.450465299571764,
-            "y": 0.9121572529445121,
-            "heading": -0.32767609968005607,
-            "angularVelocity": -2.1927522433491614e-7,
-            "velocityX": -3.680874839978332,
-            "velocityY": 0.8728108342551084,
-            "timestamp": 0.782001468948451
-        },
-        {
-            "x": 5.178616904201158,
-            "y": 1.028826716826592,
-            "heading": -0.32767612118071976,
-            "angularVelocity": -2.749440322201668e-7,
-            "velocityX": -3.4763156613508643,
-            "velocityY": 1.4919340757663395,
-            "timestamp": 0.8602016158432961
-        },
-        {
-            "x": 4.930841529412244,
-            "y": 1.190445624246428,
-            "heading": -0.3276761534331376,
-            "angularVelocity": -4.1243423626599064e-7,
-            "velocityX": -3.1684771017365216,
-            "velocityY": 2.0667340642903906,
-            "timestamp": 0.9384017627381412
-        },
-        {
-            "x": 4.7145004488669215,
-            "y": 1.3922123936122235,
-            "heading": -0.32767621878386327,
-            "angularVelocity": -8.356854591223944e-7,
-            "velocityX": -2.7665047846524513,
-            "velocityY": 2.5801328690255976,
-            "timestamp": 1.0166019096329864
-        },
-        {
-            "x": 4.536026477813721,
-            "y": 1.628126621246338,
-            "heading": -0.327696539002069,
-            "angularVelocity": -0.00025984882823803823,
-            "velocityX": -2.282271557538034,
-            "velocityY": 3.016800313073996,
-            "timestamp": 1.0948020565278314
-        },
-        {
-            "x": 4.4347589269746495,
-            "y": 1.8009845745755027,
-            "heading": -0.3311064514524199,
-            "angularVelocity": -0.06394544615832723,
-            "velocityX": -1.899051313343304,
-            "velocityY": 3.241572651564751,
-            "timestamp": 1.1481273933038434
-        },
-        {
-            "x": 4.355560152796487,
-            "y": 1.9823487857199633,
-            "heading": -0.3404702376957046,
-            "angularVelocity": -0.17559732033810904,
-            "velocityX": -1.4851996999249673,
-            "velocityY": 3.4010889027473166,
-            "timestamp": 1.2014527300798554
-        },
-        {
-            "x": 4.29961502910342,
-            "y": 2.1668323093896307,
-            "heading": -0.3620606217709223,
-            "angularVelocity": -0.40488040734269476,
-            "velocityX": -1.049128370778876,
-            "velocityY": 3.4595847832064672,
-            "timestamp": 1.2547780668558675
-        },
-        {
-            "x": 4.26624261518445,
-            "y": 2.3445509425061104,
-            "heading": -0.3930653835662206,
-            "angularVelocity": -0.5814264600999738,
-            "velocityX": -0.6258265945712352,
-            "velocityY": 3.332724064417915,
-            "timestamp": 1.3081034036318795
-        },
-        {
-            "x": 4.250245555353669,
-            "y": 2.506179624728893,
-            "heading": -0.42827286207411436,
-            "angularVelocity": -0.6602392152823773,
-            "velocityX": -0.2999898509389163,
-            "velocityY": 3.0309922448647635,
-            "timestamp": 1.3614287404078915
-        },
-        {
-            "x": 4.245253885327146,
-            "y": 2.646807044638539,
-            "heading": -0.46326114250173545,
-            "angularVelocity": -0.6561286349527023,
-            "velocityX": -0.09360784813172378,
-            "velocityY": 2.6371595270071015,
-            "timestamp": 1.4147540771839036
-        },
-        {
-            "x": 4.246491895421447,
-            "y": 2.764573131222025,
-            "heading": -0.49520995766037457,
-            "angularVelocity": -0.5991301150594935,
-            "velocityX": 0.023216170194527277,
-            "velocityY": 2.2084452476729117,
-            "timestamp": 1.4680794139599156
-        },
-        {
-            "x": 4.250760184591696,
-            "y": 2.858861309855261,
-            "heading": -0.522398988898447,
-            "angularVelocity": -0.5098707834110707,
-            "velocityX": 0.08004242313776105,
-            "velocityY": 1.7681684605074017,
-            "timestamp": 1.5214047507359276
-        },
-        {
-            "x": 4.255888225984497,
-            "y": 2.929511890736146,
-            "heading": -0.5437259487715407,
-            "angularVelocity": -0.3999404628668233,
-            "velocityX": 0.09616519468543994,
-            "velocityY": 1.324897040550944,
-            "timestamp": 1.5747300875119397
-        },
-        {
-            "x": 4.260343621579396,
-            "y": 2.9765343465211713,
-            "heading": -0.5584397105575287,
-            "angularVelocity": -0.2759244043319095,
-            "velocityX": 0.08355119468920533,
-            "velocityY": 0.8818032595380478,
-            "timestamp": 1.6280554242879517
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -0.14177668439456792,
-            "velocityX": 0.04981456435476623,
-            "velocityY": 0.4400469813703916,
-            "timestamp": 1.6813807610639637
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 0,
-            "velocityX": 3.549235715029611e-32,
-            "velocityY": -9.598836837089386e-32,
-            "timestamp": 1.7347060978399758
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C5 To S4.traj b/src/main/deploy/choreo/C5 To S4.traj
deleted file mode 100644
index 773c6f5e..00000000
--- a/src/main/deploy/choreo/C5 To S4.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 2.8467808772943154e-31,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 7.633526252649942,
-            "y": 0.8024218861992577,
-            "heading": -0.02675508445787236,
-            "angularVelocity": -0.3471453590528024,
-            "velocityX": -0.602993637929559,
-            "velocityY": 0.20009790028293237,
-            "timestamp": 0.07707170428800919
-        },
-        {
-            "x": 7.540488865885011,
-            "y": 0.8333059401759839,
-            "heading": -0.07783349665964928,
-            "angularVelocity": -0.6627388439588938,
-            "velocityX": -1.2071536191448495,
-            "velocityY": 0.400718451240102,
-            "timestamp": 0.15414340857601838
-        },
-        {
-            "x": 7.400762361365792,
-            "y": 0.8797032018771439,
-            "heading": -0.14939031986058435,
-            "angularVelocity": -0.9284448016555393,
-            "velocityX": -1.8129416730824437,
-            "velocityY": 0.6020012419574645,
-            "timestamp": 0.23121511286402757
-        },
-        {
-            "x": 7.214174023420098,
-            "y": 0.9416744881041137,
-            "heading": -0.23458534610519022,
-            "angularVelocity": -1.105399537114693,
-            "velocityX": -2.4209707008480317,
-            "velocityY": 0.8040731264406625,
-            "timestamp": 0.30828681715203676
-        },
-        {
-            "x": 6.98058449689491,
-            "y": 1.0192605514016468,
-            "heading": -0.3182722929391017,
-            "angularVelocity": -1.085832311702643,
-            "velocityX": -3.030807852026836,
-            "velocityY": 1.0066737723562156,
-            "timestamp": 0.38535852144004595
-        },
-        {
-            "x": 6.7044949607857305,
-            "y": 1.1109419048147462,
-            "heading": -0.3204977394277859,
-            "angularVelocity": -0.02887501333002772,
-            "velocityX": -3.5822425189594846,
-            "velocityY": 1.189559180766205,
-            "timestamp": 0.46243022572805514
-        },
-        {
-            "x": 6.427835578289735,
-            "y": 1.202949830409229,
-            "heading": -0.32049776831244,
-            "angularVelocity": -3.7477637567079044e-7,
-            "velocityX": -3.5896362361749183,
-            "velocityY": 1.1937964320946928,
-            "timestamp": 0.5395019300160644
-        },
-        {
-            "x": 6.1511761970625,
-            "y": 1.294957759818759,
-            "heading": -0.3204977971970938,
-            "angularVelocity": -3.7477637370825724e-7,
-            "velocityX": -3.589636219712834,
-            "velocityY": 1.1937964815946673,
-            "timestamp": 0.6165736343040735
-        },
-        {
-            "x": 5.87524941446386,
-            "y": 1.3871813457379307,
-            "heading": -0.32266464886855784,
-            "angularVelocity": -0.028114749653993878,
-            "velocityX": -3.5801308034856563,
-            "velocityY": 1.1965946098005253,
-            "timestamp": 0.6936453385920827
-        },
-        {
-            "x": 5.641703675217654,
-            "y": 1.4649149367510799,
-            "heading": -0.4060390277887848,
-            "angularVelocity": -1.081776764773039,
-            "velocityX": -3.030239715129037,
-            "velocityY": 1.0085879342004245,
-            "timestamp": 0.7707170428800918
-        },
-        {
-            "x": 5.455154020778328,
-            "y": 1.5270189169325792,
-            "heading": -0.49098864699750666,
-            "angularVelocity": -1.1022153979010723,
-            "velocityX": -2.420468785044752,
-            "velocityY": 0.8057948212669979,
-            "timestamp": 0.8477887471681009
-        },
-        {
-            "x": 5.315462796839212,
-            "y": 1.573537321073397,
-            "heading": -0.5623285861822422,
-            "angularVelocity": -0.9256307466375128,
-            "velocityX": -1.8124839100106522,
-            "velocityY": 0.6035730566821683,
-            "timestamp": 0.9248604514561101
-        },
-        {
-            "x": 5.222455052636063,
-            "y": 1.6045200564296747,
-            "heading": -0.613275527154411,
-            "angularVelocity": -0.6610330139033284,
-            "velocityX": -1.2067690089684353,
-            "velocityY": 0.40199883527290925,
-            "timestamp": 1.0019321557441192
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -0.34674817551357506,
-            "velocityX": -0.602751075316368,
-            "velocityY": 0.2008511906325378,
-            "timestamp": 1.0790038600321283
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 8.760742141155177e-29,
-            "velocityX": 3.313743560366677e-30,
-            "velocityY": 1.2206105198294578e-30,
-            "timestamp": 1.1560755643201375
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C5 To W1.traj b/src/main/deploy/choreo/C5 To W1.traj
deleted file mode 100644
index bec33f37..00000000
--- a/src/main/deploy/choreo/C5 To W1.traj	
+++ /dev/null
@@ -1,310 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -3.516607019090483e-32,
-            "velocityX": -4.092268936373955e-33,
-            "velocityY": -4.396806616029776e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 7.6580594556627535,
-            "y": 0.8642766421574168,
-            "heading": -0.06977499787943942,
-            "angularVelocity": -0.7009716360726537,
-            "velocityX": -0.22041848409624198,
-            "velocityY": 0.7763344454252913,
-            "timestamp": 0.09954040119279155
-        },
-        {
-            "x": 7.613877684624128,
-            "y": 1.0194223152597515,
-            "heading": -0.19807907998906352,
-            "angularVelocity": -1.2889648883484262,
-            "velocityX": -0.4438576749661031,
-            "velocityY": 1.558620130552277,
-            "timestamp": 0.1990808023855831
-        },
-        {
-            "x": 7.54706531781781,
-            "y": 1.253315507091588,
-            "heading": -0.3641253773176819,
-            "angularVelocity": -1.6681296773861405,
-            "velocityX": -0.6712085344815377,
-            "velocityY": 2.349731255139591,
-            "timestamp": 0.2986212035783746
-        },
-        {
-            "x": 7.457648537893544,
-            "y": 1.5658706931334108,
-            "heading": -0.49008580394214096,
-            "angularVelocity": -1.265420122031624,
-            "velocityX": -0.8982963585919527,
-            "velocityY": 3.139983185686188,
-            "timestamp": 0.3981616047711662
-        },
-        {
-            "x": 7.353528135913809,
-            "y": 1.9277448806018234,
-            "heading": -0.490085841920944,
-            "angularVelocity": -3.8154159059291386e-7,
-            "velocityX": -1.0460114760646044,
-            "velocityY": 3.6354503611807685,
-            "timestamp": 0.49770200596395775
-        },
-        {
-            "x": 7.249407681423134,
-            "y": 2.2896190529615703,
-            "heading": -0.4900858798995876,
-            "angularVelocity": -3.8153998933914064e-7,
-            "velocityX": -1.0460120035985523,
-            "velocityY": 3.635450209396512,
-            "timestamp": 0.5972424071567493
-        },
-        {
-            "x": 7.145287117295142,
-            "y": 2.651493193775847,
-            "heading": -0.4900859178782245,
-            "angularVelocity": -3.8153992177326955e-7,
-            "velocityX": -1.0460131050338848,
-            "velocityY": 3.6354498924852887,
-            "timestamp": 0.6967828083495408
-        },
-        {
-            "x": 7.025437006860337,
-            "y": 3.00846649318931,
-            "heading": -0.49008595631381496,
-            "angularVelocity": -3.8613055604326664e-7,
-            "velocityX": -1.204034834083884,
-            "velocityY": 3.586215196401226,
-            "timestamp": 0.7963232095423323
-        },
-        {
-            "x": 6.830624533504276,
-            "y": 3.3307118703307705,
-            "heading": -0.49008600153372534,
-            "angularVelocity": -4.542870012894294e-7,
-            "velocityX": -1.9571196320451347,
-            "velocityY": 3.237332513029861,
-            "timestamp": 0.8958636107351238
-        },
-        {
-            "x": 6.570226686668516,
-            "y": 3.602717269957059,
-            "heading": -0.4900860357215963,
-            "angularVelocity": -3.4345723556551647e-7,
-            "velocityX": -2.6160015804177474,
-            "velocityY": 2.7326130532612987,
-            "timestamp": 0.9954040119279153
-        },
-        {
-            "x": 6.270670526052627,
-            "y": 3.8032215967371097,
-            "heading": -0.4416564414377051,
-            "angularVelocity": 0.4865320382835506,
-            "velocityX": -3.009392739292897,
-            "velocityY": 2.0143009710369753,
-            "timestamp": 1.094944413120707
-        },
-        {
-            "x": 5.9498699270968665,
-            "y": 3.9255658761230467,
-            "heading": -0.3340733830984328,
-            "angularVelocity": 1.0807979177309486,
-            "velocityX": -3.222818022748657,
-            "velocityY": 1.229091684581206,
-            "timestamp": 1.1944848143134985
-        },
-        {
-            "x": 5.637666976812727,
-            "y": 3.9694128449739723,
-            "heading": -0.13726885518093873,
-            "angularVelocity": 1.977132154976145,
-            "velocityX": -3.136444564649281,
-            "velocityY": 0.4404941945733328,
-            "timestamp": 1.2940252155062901
-        },
-        {
-            "x": 5.333615486534618,
-            "y": 3.942111372133816,
-            "heading": 0.08955200005531357,
-            "angularVelocity": 2.278681344642582,
-            "velocityX": -3.0545535946677287,
-            "velocityY": -0.2742752943830166,
-            "timestamp": 1.3935656166990817
-        },
-        {
-            "x": 5.078082684497777,
-            "y": 3.9650753252247246,
-            "heading": 0.19884455131252843,
-            "angularVelocity": 1.0979717777662474,
-            "velocityX": -2.5671265031564414,
-            "velocityY": 0.2306998245509574,
-            "timestamp": 1.4931060178918734
-        },
-        {
-            "x": 4.8543877601623535,
-            "y": 4.064273357391357,
-            "heading": 0.3064368289582943,
-            "angularVelocity": 1.0808905364705061,
-            "velocityX": -2.2472777048805277,
-            "velocityY": 0.9965605018459247,
-            "timestamp": 1.592646419084665
-        },
-        {
-            "x": 4.700013932982481,
-            "y": 4.187105651970831,
-            "heading": 0.3888797273509616,
-            "angularVelocity": 1.0676410431648378,
-            "velocityX": -1.99915137751061,
-            "velocityY": 1.5906864226746236,
-            "timestamp": 1.6698660978526145
-        },
-        {
-            "x": 4.564169886311024,
-            "y": 4.35607150380987,
-            "heading": 0.4692148602751942,
-            "angularVelocity": 1.040345339504005,
-            "velocityX": -1.7591894817340108,
-            "velocityY": 2.1881190719116157,
-            "timestamp": 1.747085776620564
-        },
-        {
-            "x": 4.445019228108457,
-            "y": 4.571847534724939,
-            "heading": 0.5443267073647724,
-            "angularVelocity": 0.9727034389160626,
-            "velocityX": -1.543008985580261,
-            "velocityY": 2.7943140188848803,
-            "timestamp": 1.8243054553885136
-        },
-        {
-            "x": 4.302573716554072,
-            "y": 4.826880596020871,
-            "heading": 0.5443268349443496,
-            "angularVelocity": 0.0000016521640508021742,
-            "velocityX": -1.8446788930894609,
-            "velocityY": 3.302695185541032,
-            "timestamp": 1.9015251341564632
-        },
-        {
-            "x": 4.139811862406642,
-            "y": 5.06945260129317,
-            "heading": 0.544326895208066,
-            "angularVelocity": 7.80419154390702e-7,
-            "velocityX": -2.1077768872432143,
-            "velocityY": 3.141323677365259,
-            "timestamp": 1.9787448129244127
-        },
-        {
-            "x": 3.9382711614032555,
-            "y": 5.28090933180296,
-            "heading": 0.5443268879285122,
-            "angularVelocity": -9.42707078520697e-8,
-            "velocityX": -2.60996554529876,
-            "velocityY": 2.738378790013248,
-            "timestamp": 2.0559644916923623
-        },
-        {
-            "x": 3.721249786731846,
-            "y": 5.476445328956613,
-            "heading": 0.5443268807879367,
-            "angularVelocity": -9.247093001843253e-8,
-            "velocityX": -2.810441303745562,
-            "velocityY": 2.53220422919983,
-            "timestamp": 2.1331841704603116
-        },
-        {
-            "x": 3.5042282470745802,
-            "y": 5.671981142995692,
-            "heading": 0.5443268736473624,
-            "angularVelocity": -9.247091428352055e-8,
-            "velocityX": -2.810443440323428,
-            "velocityY": 2.53220185785385,
-            "timestamp": 2.210403849228261
-        },
-        {
-            "x": 3.287206691377032,
-            "y": 5.867516939231946,
-            "heading": 0.5443268665067905,
-            "angularVelocity": -9.247088327815795e-8,
-            "velocityX": -2.810443648046155,
-            "velocityY": 2.5322016273060566,
-            "timestamp": 2.2876235279962103
-        },
-        {
-            "x": 3.0701851252220784,
-            "y": 6.063052723861702,
-            "heading": 0.5443268593662193,
-            "angularVelocity": -9.247087442211068e-8,
-            "velocityX": -2.8104437834702534,
-            "velocityY": 2.532201477001163,
-            "timestamp": 2.3648432067641596
-        },
-        {
-            "x": 2.853163562032648,
-            "y": 6.258588511782302,
-            "heading": 0.5443268522252032,
-            "angularVelocity": -9.247663534512907e-8,
-            "velocityX": -2.8104437450665345,
-            "velocityY": 2.5322015196178005,
-            "timestamp": 2.442062885532109
-        },
-        {
-            "x": 2.6687757934039245,
-            "y": 6.424725614600548,
-            "heading": 0.4938774787934042,
-            "angularVelocity": -0.6533227570578579,
-            "velocityX": -2.3878339248576954,
-            "velocityY": 2.1514865830703513,
-            "timestamp": 2.5192825643000583
-        },
-        {
-            "x": 2.521265507868239,
-            "y": 6.557635351096379,
-            "heading": 0.4535239115954702,
-            "angularVelocity": -0.522581391709739,
-            "velocityX": -1.9102680545844264,
-            "velocityY": 1.7211899688838848,
-            "timestamp": 2.5965022430680076
-        },
-        {
-            "x": 2.4106327602038906,
-            "y": 6.6573176734827975,
-            "heading": 0.42326153130587874,
-            "angularVelocity": -0.391899846935805,
-            "velocityX": -1.432701474928527,
-            "velocityY": 1.2908927358526117,
-            "timestamp": 2.673721921835957
-        },
-        {
-            "x": 2.336877585524826,
-            "y": 6.723772559032594,
-            "heading": 0.40308732310971707,
-            "angularVelocity": -0.2612573442164476,
-            "velocityX": -0.9551344405446706,
-            "velocityY": 0.8605952085024652,
-            "timestamp": 2.7509416006039062
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": -0.13063150832354717,
-            "velocityX": -0.4775671967717696,
-            "velocityY": 0.43029758084408004,
-            "timestamp": 2.8281612793718556
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": -5.871044564465464e-32,
-            "velocityX": 0,
-            "velocityY": -1.090928833064942e-33,
-            "timestamp": 2.905380958139805
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C5 To W2.traj b/src/main/deploy/choreo/C5 To W2.traj
deleted file mode 100644
index d94da80a..00000000
--- a/src/main/deploy/choreo/C5 To W2.traj	
+++ /dev/null
@@ -1,283 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -2.906139149989869e-32,
-            "velocityX": 1.181772084740038e-33,
-            "velocityY": 2.7502272506846476e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 7.666043555718406,
-            "y": 0.8524996120976488,
-            "heading": 0.004999198621269784,
-            "angularVelocity": 0.05577597491216818,
-            "velocityX": -0.15571181404981524,
-            "velocityY": 0.7307780702234378,
-            "timestamp": 0.08962996396104499
-        },
-        {
-            "x": 7.638131101049187,
-            "y": 0.9835016263770707,
-            "heading": 0.014524734067527434,
-            "angularVelocity": 0.10627623871853438,
-            "velocityX": -0.31141878715192206,
-            "velocityY": 1.4615872693685117,
-            "timestamp": 0.17925992792208997
-        },
-        {
-            "x": 7.596263489657983,
-            "y": 1.180010140566484,
-            "heading": 0.02778512041931341,
-            "angularVelocity": 0.14794590743726296,
-            "velocityX": -0.46711623592083296,
-            "velocityY": 2.1924421867983774,
-            "timestamp": 0.26888989188313495
-        },
-        {
-            "x": 7.540442918712468,
-            "y": 1.4420312351509346,
-            "heading": 0.04318451816280708,
-            "angularVelocity": 0.17181082154832236,
-            "velocityX": -0.6227891709269913,
-            "velocityY": 2.9233649440976053,
-            "timestamp": 0.35851985584417995
-        },
-        {
-            "x": 7.470680035396563,
-            "y": 1.7695653388184471,
-            "heading": 0.05582613302662815,
-            "angularVelocity": 0.1410422843561041,
-            "velocityX": -0.7783433154812639,
-            "velocityY": 3.654292484261909,
-            "timestamp": 0.44814981980522495
-        },
-        {
-            "x": 7.399999368806171,
-            "y": 2.101181376235394,
-            "heading": 0.05582613685023459,
-            "angularVelocity": 4.265991273772723e-8,
-            "velocityX": -0.7885830080341364,
-            "velocityY": 3.6998345504308543,
-            "timestamp": 0.5377797837662699
-        },
-        {
-            "x": 7.329318638814834,
-            "y": 2.4327974001390613,
-            "heading": 0.05582614067384277,
-            "angularVelocity": 4.265993214747779e-8,
-            "velocityX": -0.788583715397405,
-            "velocityY": 3.6998343996634295,
-            "timestamp": 0.6274097477273148
-        },
-        {
-            "x": 7.258637432835217,
-            "y": 2.764413322590103,
-            "heading": 0.05582614449746126,
-            "angularVelocity": 4.266004720083137e-8,
-            "velocityX": -0.7885890259906463,
-            "velocityY": 3.699833267758185,
-            "timestamp": 0.7170397116883598
-        },
-        {
-            "x": 7.145396314308057,
-            "y": 3.084009006822374,
-            "heading": 0.05582614848984762,
-            "angularVelocity": 4.4542987394171874e-8,
-            "velocityX": -1.2634292542656518,
-            "velocityY": 3.5657236721770227,
-            "timestamp": 0.8066696756494047
-        },
-        {
-            "x": 6.971535677728531,
-            "y": 3.375105946924151,
-            "heading": 0.05582615332930866,
-            "angularVelocity": 5.39937854332775e-8,
-            "velocityX": -1.9397601973274063,
-            "velocityY": 3.2477636633692444,
-            "timestamp": 0.8962996396104497
-        },
-        {
-            "x": 6.743840876905014,
-            "y": 3.626342927147427,
-            "heading": 0.0558261606002303,
-            "angularVelocity": 8.112155042111421e-8,
-            "velocityX": -2.5403870621043505,
-            "velocityY": 2.8030467615993793,
-            "timestamp": 0.9859296035714946
-        },
-        {
-            "x": 6.471198532281258,
-            "y": 3.8279144449501397,
-            "heading": 0.0558261762788256,
-            "angularVelocity": 1.7492582391526053e-7,
-            "velocityX": -3.041866052096724,
-            "velocityY": 2.2489300329331785,
-            "timestamp": 1.0755595675325396
-        },
-        {
-            "x": 6.164249513947577,
-            "y": 3.9719534129902763,
-            "heading": 0.05582625958870765,
-            "angularVelocity": 9.294869524458774e-7,
-            "velocityX": -3.4246250335109787,
-            "velocityY": 1.6070403431461682,
-            "timestamp": 1.1651895314935845
-        },
-        {
-            "x": 5.836252922691461,
-            "y": 4.05240893270077,
-            "heading": 0.05954498226559844,
-            "angularVelocity": 0.04148972634315725,
-            "velocityX": -3.6594524505071675,
-            "velocityY": 0.8976408798452788,
-            "timestamp": 1.2548194954546295
-        },
-        {
-            "x": 5.497492188342187,
-            "y": 4.066758597782368,
-            "heading": 0.05954602093201425,
-            "angularVelocity": 0.000011588383726853398,
-            "velocityX": -3.779547813904135,
-            "velocityY": 0.16009897190000646,
-            "timestamp": 1.3444494594156744
-        },
-        {
-            "x": 5.204246867986104,
-            "y": 4.110932074419811,
-            "heading": 0.17401114275805463,
-            "angularVelocity": 1.2770854384789139,
-            "velocityX": -3.2717331057226993,
-            "velocityY": 0.4928427356797903,
-            "timestamp": 1.4340794233767193
-        },
-        {
-            "x": 4.937438011169434,
-            "y": 4.2165327072143555,
-            "heading": 0.3021760231951433,
-            "angularVelocity": 1.4299334148208713,
-            "velocityX": -2.976781926774297,
-            "velocityY": 1.1781844834885962,
-            "timestamp": 1.5237093873377643
-        },
-        {
-            "x": 4.703348327317259,
-            "y": 4.378813716693983,
-            "heading": 0.4253911826920849,
-            "angularVelocity": 1.3960058537628992,
-            "velocityX": -2.6521945051027034,
-            "velocityY": 1.8386149895276152,
-            "timestamp": 1.61197202546698
-        },
-        {
-            "x": 4.500555108729279,
-            "y": 4.597666686435954,
-            "heading": 0.5261840356750895,
-            "angularVelocity": 1.1419651068603307,
-            "velocityX": -2.297611117074157,
-            "velocityY": 2.479565242787912,
-            "timestamp": 1.7002346635961958
-        },
-        {
-            "x": 4.326794639818804,
-            "y": 4.871425359866347,
-            "heading": 0.5588913487111756,
-            "angularVelocity": 0.37056804248478087,
-            "velocityX": -1.968675224233522,
-            "velocityY": 3.1016371052677254,
-            "timestamp": 1.7884973017254115
-        },
-        {
-            "x": 4.0969805858398445,
-            "y": 5.11364296690146,
-            "heading": 0.5588923269288569,
-            "angularVelocity": 0.00001108303243636796,
-            "velocityX": -2.603752378696331,
-            "velocityY": 2.744282429905488,
-            "timestamp": 1.8767599398546273
-        },
-        {
-            "x": 3.8246168380678105,
-            "y": 5.306780372295281,
-            "heading": 0.5588922897097742,
-            "angularVelocity": -4.2168559122096544e-7,
-            "velocityX": -3.0858328455274044,
-            "velocityY": 2.188212470049549,
-            "timestamp": 1.965022577983843
-        },
-        {
-            "x": 3.520011793550478,
-            "y": 5.443527703198532,
-            "heading": 0.5588922240132788,
-            "angularVelocity": -7.44329614921164e-7,
-            "velocityX": -3.451121006278936,
-            "velocityY": 1.549322950250525,
-            "timestamp": 2.053285216113059
-        },
-        {
-            "x": 3.194693864220709,
-            "y": 5.518709582083228,
-            "heading": 0.5588921711301718,
-            "angularVelocity": -5.991562005821763e-7,
-            "velocityX": -3.6857943091787817,
-            "velocityY": 0.851797323060201,
-            "timestamp": 2.1415478542422743
-        },
-        {
-            "x": 2.8828791952129387,
-            "y": 5.532174882793972,
-            "heading": 0.5041280478943673,
-            "angularVelocity": -0.6204677811196886,
-            "velocityX": -3.5328047701370098,
-            "velocityY": 0.15255946339412532,
-            "timestamp": 2.22981049237149
-        },
-        {
-            "x": 2.6283322264519793,
-            "y": 5.543894951157175,
-            "heading": 0.3346872638422791,
-            "angularVelocity": -1.9197339626765784,
-            "velocityX": -2.883971906530878,
-            "velocityY": 0.132786291137652,
-            "timestamp": 2.3180731305007054
-        },
-        {
-            "x": 2.4385492640365714,
-            "y": 5.5521384887253,
-            "heading": 0.1780668377028913,
-            "angularVelocity": -1.7744815865360473,
-            "velocityX": -2.150207227406541,
-            "velocityY": 0.09339781523475092,
-            "timestamp": 2.406335768629921
-        },
-        {
-            "x": 2.31267915340354,
-            "y": 5.55742889143524,
-            "heading": 0.062002293486176877,
-            "angularVelocity": -1.3149906537667377,
-            "velocityX": -1.4260859781775244,
-            "velocityY": 0.0599393222554144,
-            "timestamp": 2.4945984067591365
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 9.107235017738231e-34,
-            "angularVelocity": -0.7024749633633888,
-            "velocityX": -0.7101436659051381,
-            "velocityY": 0.029130202985730554,
-            "timestamp": 2.582861044888352
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -2.2232006496445675e-32,
-            "velocityX": 0,
-            "velocityY": -2.9660881962078245e-33,
-            "timestamp": 2.6711236830175675
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/C5 To W3.traj b/src/main/deploy/choreo/C5 To W3.traj
deleted file mode 100644
index 62c8640c..00000000
--- a/src/main/deploy/choreo/C5 To W3.traj	
+++ /dev/null
@@ -1,292 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -1.5069359770438582e-30,
-            "velocityX": 5.69693995403372e-32,
-            "velocityY": 3.042278119963715e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 7.627290492953592,
-            "y": 0.8035142159340071,
-            "heading": 0.015175150751654183,
-            "angularVelocity": 0.18617297608634506,
-            "velocityX": -0.6466549133823012,
-            "velocityY": 0.20260100070713255,
-            "timestamp": 0.08151102845675925
-        },
-        {
-            "x": 7.521871327482097,
-            "y": 0.8365426259859824,
-            "heading": 0.045515680376982795,
-            "angularVelocity": 0.3722260680519286,
-            "velocityX": -1.2933116839203953,
-            "velocityY": 0.4052017337690261,
-            "timestamp": 0.1630220569135185
-        },
-        {
-            "x": 7.363742244741542,
-            "y": 0.8860852747673063,
-            "heading": 0.09100430929463568,
-            "angularVelocity": 0.5580671693007231,
-            "velocityX": -1.9399716300284648,
-            "velocityY": 0.6078030141357551,
-            "timestamp": 0.24453308537027776
-        },
-        {
-            "x": 7.152902908699907,
-            "y": 0.95214234591484,
-            "heading": 0.15161661460632753,
-            "angularVelocity": 0.7436086436309897,
-            "velocityX": -2.5866356986709165,
-            "velocityY": 0.8104065474106494,
-            "timestamp": 0.326044113827037
-        },
-        {
-            "x": 6.889352971860368,
-            "y": 1.0347142415767305,
-            "heading": 0.22732297101557483,
-            "angularVelocity": 0.9287866665726976,
-            "velocityX": -3.233304030501219,
-            "velocityY": 1.0130150143510155,
-            "timestamp": 0.4075551422837963
-        },
-        {
-            "x": 6.595109187456524,
-            "y": 1.1269159867790934,
-            "heading": 0.22732298101704992,
-            "angularVelocity": 1.22700882310899e-7,
-            "velocityX": -3.6098647014365244,
-            "velocityY": 1.1311566906713055,
-            "timestamp": 0.4890661707405556
-        },
-        {
-            "x": 6.300865420236581,
-            "y": 1.219117786820698,
-            "heading": 0.2273229910184091,
-            "angularVelocity": 1.2269945998464997e-7,
-            "velocityX": -3.6098644906196586,
-            "velocityY": 1.131157363454394,
-            "timestamp": 0.5705771991973149
-        },
-        {
-            "x": 6.006621672014901,
-            "y": 1.3113196474914972,
-            "heading": 0.2273230010197575,
-            "angularVelocity": 1.2269932759504928e-7,
-            "velocityX": -3.6098642575436695,
-            "velocityY": 1.1311581072702415,
-            "timestamp": 0.6520882276540741
-        },
-        {
-            "x": 5.7123779465456614,
-            "y": 1.4035215807721424,
-            "heading": 0.22732301102110228,
-            "angularVelocity": 1.2269928353992546e-7,
-            "velocityX": -3.6098639784103828,
-            "velocityY": 1.131158998068056,
-            "timestamp": 0.7335992561108334
-        },
-        {
-            "x": 5.418134249112936,
-            "y": 1.4957236035255936,
-            "heading": 0.2273230210224565,
-            "angularVelocity": 1.2269939906393907e-7,
-            "velocityX": -3.60986363445063,
-            "velocityY": 1.1311600957453605,
-            "timestamp": 0.8151102845675927
-        },
-        {
-            "x": 5.123890658648026,
-            "y": 1.5879259676440571,
-            "heading": 0.22732303102385681,
-            "angularVelocity": 1.2269996504792678e-7,
-            "velocityX": -3.609862322139673,
-            "velocityY": 1.1311642837064038,
-            "timestamp": 0.896621313024352
-        },
-        {
-            "x": 4.838467298400658,
-            "y": 1.7046053225590203,
-            "heading": 0.22732304878104165,
-            "angularVelocity": 2.1785008908970575e-7,
-            "velocityX": -3.5016532811726435,
-            "velocityY": 1.4314548242617389,
-            "timestamp": 0.9781323414811113
-        },
-        {
-            "x": 4.5530441892872355,
-            "y": 1.8212852918009321,
-            "heading": 0.2273230665381978,
-            "angularVelocity": 2.1784973753126392e-7,
-            "velocityX": -3.5016502001914627,
-            "velocityY": 1.4314623609958343,
-            "timestamp": 1.0596433699378704
-        },
-        {
-            "x": 4.267621124217833,
-            "y": 1.9379653687834795,
-            "heading": 0.2273230842953322,
-            "angularVelocity": 2.1784947052459537e-7,
-            "velocityX": -3.5016496598471556,
-            "velocityY": 1.4314636827879597,
-            "timestamp": 1.1411543983946297
-        },
-        {
-            "x": 3.9821980933555543,
-            "y": 2.054645529443536,
-            "heading": 0.22732310205245027,
-            "angularVelocity": 2.1784927045604472e-7,
-            "velocityX": -3.5016492401846184,
-            "velocityY": 1.4314647093669521,
-            "timestamp": 1.222665426851389
-        },
-        {
-            "x": 3.6967750938784922,
-            "y": 2.171325766878081,
-            "heading": 0.22732311980956,
-            "angularVelocity": 2.1784916783717796e-7,
-            "velocityX": -3.501648855142053,
-            "velocityY": 1.4314656512577588,
-            "timestamp": 1.3041764553081483
-        },
-        {
-            "x": 3.411352191599502,
-            "y": 2.288006242078097,
-            "heading": 0.2273231375666713,
-            "angularVelocity": 2.178491871140625e-7,
-            "velocityX": -3.501647662689034,
-            "velocityY": 1.4314685682307748,
-            "timestamp": 1.3856874837649076
-        },
-        {
-            "x": 3.135544385824121,
-            "y": 2.425885248705818,
-            "heading": 0.22732315564878777,
-            "angularVelocity": 2.218364413735132e-7,
-            "velocityX": -3.3836869807340535,
-            "velocityY": 1.6915380561155315,
-            "timestamp": 1.4671985122216669
-        },
-        {
-            "x": 2.8888590335845947,
-            "y": 2.6108903884887695,
-            "heading": 0.22732317584620773,
-            "angularVelocity": 2.4778757367040615e-7,
-            "velocityX": -3.026404609412922,
-            "velocityY": 2.2696945834893776,
-            "timestamp": 1.5487095406784261
-        },
-        {
-            "x": 2.714424659127437,
-            "y": 2.7861164379371415,
-            "heading": 0.22732319539367835,
-            "angularVelocity": 2.990800632496739e-7,
-            "velocityX": -2.668879509458687,
-            "velocityY": 2.6809922892290525,
-            "timestamp": 1.6140681954393303
-        },
-        {
-            "x": 2.566977650329029,
-            "y": 2.984587845024715,
-            "heading": 0.2273232142228265,
-            "angularVelocity": 2.880895914276183e-7,
-            "velocityX": -2.2559676195571057,
-            "velocityY": 3.0366507360596735,
-            "timestamp": 1.6794268502002345
-        },
-        {
-            "x": 2.4495779823740462,
-            "y": 3.20218572489824,
-            "heading": 0.22732323403642601,
-            "angularVelocity": 3.0315188694924184e-7,
-            "velocityX": -1.7962375202558922,
-            "velocityY": 3.3292894517113587,
-            "timestamp": 1.7447855049611387
-        },
-        {
-            "x": 2.3646621591221844,
-            "y": 3.4343941523127826,
-            "heading": 0.22732276450933128,
-            "angularVelocity": -0.000007183855179950579,
-            "velocityX": -1.2992284428512828,
-            "velocityY": 3.5528336417572115,
-            "timestamp": 1.810144159722043
-        },
-        {
-            "x": 2.3096843159184894,
-            "y": 3.656196634862642,
-            "heading": 0.16843360905648927,
-            "angularVelocity": -0.9010154151467895,
-            "velocityX": -0.8411715847703609,
-            "velocityY": 3.3936206820850954,
-            "timestamp": 1.875502814482947
-        },
-        {
-            "x": 2.277656929328602,
-            "y": 3.853541817964372,
-            "heading": 0.0672675417750508,
-            "angularVelocity": -1.547860304829062,
-            "velocityX": -0.49002518039951914,
-            "velocityY": 3.0194192922676324,
-            "timestamp": 1.9408614692438513
-        },
-        {
-            "x": 2.265357919525011,
-            "y": 4.021569174077037,
-            "heading": -0.049806740829473055,
-            "angularVelocity": -1.791259061753831,
-            "velocityX": -0.188177217670429,
-            "velocityY": 2.570850895376378,
-            "timestamp": 2.0062201240047557
-        },
-        {
-            "x": 2.269649055536998,
-            "y": 4.158080976089645,
-            "heading": -0.1685192187929243,
-            "angularVelocity": -1.8163237661135343,
-            "velocityX": 0.06565520706762865,
-            "velocityY": 2.088656850603768,
-            "timestamp": 2.07157877876566
-        },
-        {
-            "x": 2.2810300353091644,
-            "y": 4.259951613943982,
-            "heading": -0.2719091294332062,
-            "angularVelocity": -1.581885536330179,
-            "velocityX": 0.1741311814602323,
-            "velocityY": 1.5586403702310236,
-            "timestamp": 2.1369374335265645
-        },
-        {
-            "x": 2.2926691286250658,
-            "y": 4.326989538481201,
-            "heading": -0.34849979264864867,
-            "angularVelocity": -1.1718518916219969,
-            "velocityX": 0.1780803683686499,
-            "velocityY": 1.0256931508529106,
-            "timestamp": 2.202296088287469
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.6349611616573703,
-            "velocityX": 0.11216374329844404,
-            "velocityY": 0.5050664160631704,
-            "timestamp": 2.2676547430483733
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -1.761626207104486e-30,
-            "velocityX": 0,
-            "velocityY": -1.839087679439853e-31,
-            "timestamp": 2.3330133978092777
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Center To C1.traj b/src/main/deploy/choreo/Center To C1.traj
deleted file mode 100644
index 1fe0cdb8..00000000
--- a/src/main/deploy/choreo/Center To C1.traj	
+++ /dev/null
@@ -1,220 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -2.1086675817598662e-32,
-            "velocityY": -7.052966104933725e-38,
-            "timestamp": 0
-        },
-        {
-            "x": 1.4233949672121562,
-            "y": 5.582111248416047,
-            "heading": 6.989955400155978e-19,
-            "angularVelocity": 7.290266672073875e-18,
-            "velocityX": 0.7654825584964474,
-            "velocityY": 0.23061220206204183,
-            "timestamp": 0.09588065253415788
-        },
-        {
-            "x": 1.5701848986828006,
-            "y": 5.626333744358309,
-            "heading": -5.64955528358279e-19,
-            "angularVelocity": -1.3182545571253806e-17,
-            "velocityX": 1.530965086187225,
-            "velocityY": 0.4612243948434498,
-            "timestamp": 0.19176130506831576
-        },
-        {
-            "x": 1.7903697885046006,
-            "y": 5.69266748604712,
-            "heading": -1.1328157855510893e-18,
-            "angularVelocity": -5.922574643212496e-18,
-            "velocityX": 2.2964475522666925,
-            "velocityY": 0.6918365690635988,
-            "timestamp": 0.28764195760247363
-        },
-        {
-            "x": 2.08394961895559,
-            "y": 5.781112468143492,
-            "heading": -1.2347885191323161e-17,
-            "angularVelocity": -1.1696905584501664e-16,
-            "velocityX": 3.061929833512548,
-            "velocityY": 0.9224486876000673,
-            "timestamp": 0.3835226101366315
-        },
-        {
-            "x": 2.4312425305333245,
-            "y": 5.885739258408696,
-            "heading": -1.8571615119787707e-17,
-            "angularVelocity": -6.491121789400923e-17,
-            "velocityX": 3.6221375470302584,
-            "velocityY": 1.0912190050847865,
-            "timestamp": 0.4794032626707894
-        },
-        {
-            "x": 2.7785354421111035,
-            "y": 5.990366048673914,
-            "heading": -2.5105020741934312e-17,
-            "angularVelocity": -6.814102166319416e-17,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849244,
-            "timestamp": 0.5752839152049473
-        },
-        {
-            "x": 3.125828353688883,
-            "y": 6.094992838939131,
-            "heading": -3.974939977960392e-17,
-            "angularVelocity": -1.527354961675144e-16,
-            "velocityX": 3.622137547030716,
-            "velocityY": 1.0912190050849244,
-            "timestamp": 0.6711645677391052
-        },
-        {
-            "x": 3.473121265266662,
-            "y": 6.199619629204348,
-            "heading": -4.6160884572564577e-17,
-            "angularVelocity": -6.68694321901595e-17,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849242,
-            "timestamp": 0.767045220273263
-        },
-        {
-            "x": 3.8204141768444413,
-            "y": 6.304246419469565,
-            "heading": -5.207468497060529e-17,
-            "angularVelocity": -6.167876669314405e-17,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849244,
-            "timestamp": 0.862925872807421
-        },
-        {
-            "x": 4.16770708842222,
-            "y": 6.408873209734782,
-            "heading": -3.0597254964935646e-17,
-            "angularVelocity": 2.24001708769928e-16,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849242,
-            "timestamp": 0.9588065253415788
-        },
-        {
-            "x": 4.515,
-            "y": 6.5135,
-            "heading": -2.0783137065340813e-18,
-            "angularVelocity": 2.974420855995074e-16,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849244,
-            "timestamp": 1.0546871778757367
-        },
-        {
-            "x": 4.862292911577779,
-            "y": 6.618126790265217,
-            "heading": -2.7789619178473824e-17,
-            "angularVelocity": -2.681594752696605e-16,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849244,
-            "timestamp": 1.1505678304098945
-        },
-        {
-            "x": 5.209585823155558,
-            "y": 6.722753580530434,
-            "heading": 2.2712942266126744e-18,
-            "angularVelocity": 3.135242889004865e-16,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849242,
-            "timestamp": 1.2464484829440523
-        },
-        {
-            "x": 5.5568787347333375,
-            "y": 6.827380370795651,
-            "heading": 3.7041174502192387e-17,
-            "angularVelocity": 3.6263708429820527e-16,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849244,
-            "timestamp": 1.34232913547821
-        },
-        {
-            "x": 5.9041716463111165,
-            "y": 6.932007161060868,
-            "heading": 6.494346998398645e-17,
-            "angularVelocity": 2.910106965725311e-16,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849244,
-            "timestamp": 1.4382097880123679
-        },
-        {
-            "x": 6.251464557888895,
-            "y": 7.036633951326086,
-            "heading": 5.759102638783417e-17,
-            "angularVelocity": -7.668328700140211e-17,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849244,
-            "timestamp": 1.5340904405465257
-        },
-        {
-            "x": 6.598757469466675,
-            "y": 7.141260741591303,
-            "heading": 2.5440135069531877e-17,
-            "angularVelocity": -3.353219911269207e-16,
-            "velocityX": 3.6221375470307158,
-            "velocityY": 1.0912190050849244,
-            "timestamp": 1.6299710930806834
-        },
-        {
-            "x": 6.94605038104441,
-            "y": 7.245887531856507,
-            "heading": 2.2711688502269004e-17,
-            "angularVelocity": -2.84566958514003e-17,
-            "velocityX": 3.6221375470302584,
-            "velocityY": 1.0912190050847868,
-            "timestamp": 1.7258517456148412
-        },
-        {
-            "x": 7.239630211495399,
-            "y": 7.334332513952879,
-            "heading": 1.1340524791180054e-17,
-            "angularVelocity": -1.1859706178712186e-16,
-            "velocityX": 3.061929833512548,
-            "velocityY": 0.9224486876000674,
-            "timestamp": 1.821732398148999
-        },
-        {
-            "x": 7.4598151013172,
-            "y": 7.40066625564169,
-            "heading": 5.896173994762203e-18,
-            "angularVelocity": -5.67825796795303e-17,
-            "velocityX": 2.2964475522666925,
-            "velocityY": 0.6918365690635988,
-            "timestamp": 1.9176130506831568
-        },
-        {
-            "x": 7.606605032787844,
-            "y": 7.444888751583952,
-            "heading": 1.9652682638036787e-18,
-            "angularVelocity": -4.099790316709085e-17,
-            "velocityX": 1.530965086187225,
-            "velocityY": 0.46122439484344985,
-            "timestamp": 2.0134937032173146
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": -2.0497026255693862e-17,
-            "velocityX": 0.7654825584964475,
-            "velocityY": 0.23061220206204183,
-            "timestamp": 2.1093743557514726
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 2.8084467268312453e-34,
-            "velocityY": 0,
-            "timestamp": 2.2052550082856306
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Center To C2.traj b/src/main/deploy/choreo/Center To C2.traj
deleted file mode 100644
index a76ce3f5..00000000
--- a/src/main/deploy/choreo/Center To C2.traj	
+++ /dev/null
@@ -1,211 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 4.006684802425094e-34,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 1.4307759237486215,
-            "y": 5.581186236587336,
-            "heading": -0.002104963011526108,
-            "angularVelocity": -0.021033383553861286,
-            "velocityX": 0.8071357913745165,
-            "velocityY": 0.21169884590096585,
-            "timestamp": 0.1000772417873624
-        },
-        {
-            "x": 1.592327768448065,
-            "y": 5.623558708255768,
-            "heading": -0.006314883423264977,
-            "angularVelocity": -0.04206671103789844,
-            "velocityX": 1.6142715547926287,
-            "velocityY": 0.42339767675114676,
-            "timestamp": 0.2001544835747248
-        },
-        {
-            "x": 1.8346555283201378,
-            "y": 5.6871174126709505,
-            "heading": -0.012629752552513055,
-            "angularVelocity": -0.06309995176191506,
-            "velocityX": 2.4214072604734147,
-            "velocityY": 0.6350964842758995,
-            "timestamp": 0.3002317253620872
-        },
-        {
-            "x": 2.1577591856288034,
-            "y": 5.77186234429474,
-            "heading": -0.02104955396970995,
-            "angularVelocity": -0.08413302831713466,
-            "velocityX": 3.228542788930726,
-            "velocityY": 0.8467952364619561,
-            "timestamp": 0.4003089671494496
-        },
-        {
-            "x": 2.5239589562380984,
-            "y": 5.867910684343787,
-            "heading": -0.021049554851793276,
-            "angularVelocity": -8.814025154574047e-9,
-            "velocityX": 3.6591712967806616,
-            "velocityY": 0.9597420785549242,
-            "timestamp": 0.500386208936812
-        },
-        {
-            "x": 2.8901587297210503,
-            "y": 5.963959013436644,
-            "heading": -0.021049555733868682,
-            "angularVelocity": -8.813945991956408e-9,
-            "velocityX": 3.659171325495056,
-            "velocityY": 0.9597419690775807,
-            "timestamp": 0.6004634507241744
-        },
-        {
-            "x": 3.2563585069426413,
-            "y": 6.060007328275347,
-            "heading": -0.02104955661593894,
-            "angularVelocity": -8.813894597525165e-9,
-            "velocityX": 3.659171362852588,
-            "velocityY": 0.9597418266460623,
-            "timestamp": 0.7005406925115368
-        },
-        {
-            "x": 3.622558289188601,
-            "y": 6.15605562395784,
-            "heading": -0.021049557498003604,
-            "angularVelocity": -8.813838677794314e-9,
-            "velocityX": 3.6591714130574977,
-            "velocityY": 0.9597416352318217,
-            "timestamp": 0.8006179342988993
-        },
-        {
-            "x": 3.9887580784892815,
-            "y": 6.252103892743073,
-            "heading": -0.02104955838006166,
-            "angularVelocity": -8.81377261667878e-9,
-            "velocityX": 3.659171483550255,
-            "velocityY": 0.9597413664668089,
-            "timestamp": 0.9006951760862617
-        },
-        {
-            "x": 4.354957878489299,
-            "y": 6.3481521207353335,
-            "heading": -0.021049559262112345,
-            "angularVelocity": -8.813699036774153e-9,
-            "velocityX": 3.6591715904610522,
-            "velocityY": 0.9597409588519333,
-            "timestamp": 1.0007724178736241
-        },
-        {
-            "x": 4.721157716449094,
-            "y": 6.444200203999558,
-            "heading": -0.021049560144156942,
-            "angularVelocity": -8.81363814954091e-9,
-            "velocityX": 3.659171969765827,
-            "velocityY": 0.9597395126886197,
-            "timestamp": 1.1008496596609865
-        },
-        {
-            "x": 5.092065894328832,
-            "y": 6.520059807846628,
-            "heading": -0.02104956519242719,
-            "angularVelocity": -5.0443738888274115e-8,
-            "velocityX": 3.70621902897584,
-            "velocityY": 0.7580105375830706,
-            "timestamp": 1.2009269014483488
-        },
-        {
-            "x": 5.47058189966855,
-            "y": 6.512767035208857,
-            "heading": -0.021049620221549888,
-            "angularVelocity": -5.498665002090286e-7,
-            "velocityX": 3.7822385847120277,
-            "velocityY": -0.07287143917560433,
-            "timestamp": 1.3010041432357111
-        },
-        {
-            "x": 5.849069453983326,
-            "y": 6.50412284194313,
-            "heading": -0.02104967520004647,
-            "angularVelocity": -5.493606298291521e-7,
-            "velocityX": 3.7819542940538056,
-            "velocityY": -0.08637521489745259,
-            "timestamp": 1.4010813850230734
-        },
-        {
-            "x": 6.2213754396920145,
-            "y": 6.435451104158958,
-            "heading": -0.021049699334872447,
-            "angularVelocity": -2.4116198195152956e-7,
-            "velocityX": 3.720186318681132,
-            "velocityY": -0.6861873544644725,
-            "timestamp": 1.5011586268104358
-        },
-        {
-            "x": 6.5695694546238315,
-            "y": 6.286829159405189,
-            "heading": -0.02104969841623417,
-            "angularVelocity": 9.179292491243133e-9,
-            "velocityX": 3.4792527123362977,
-            "velocityY": -1.4850723511100814,
-            "timestamp": 1.601235868597798
-        },
-        {
-            "x": 6.915952468622566,
-            "y": 6.134033995374984,
-            "heading": -0.021049697500180466,
-            "angularVelocity": 9.153466759383175e-9,
-            "velocityX": 3.4611566807037675,
-            "velocityY": -1.5267723340622583,
-            "timestamp": 1.7013131103851604
-        },
-        {
-            "x": 7.221571466882516,
-            "y": 5.999220411113471,
-            "heading": -0.012629808861323527,
-            "angularVelocity": 0.0841338998605394,
-            "velocityX": 3.053831148837094,
-            "velocityY": -1.347095322110863,
-            "timestamp": 1.8013903521725227
-        },
-        {
-            "x": 7.450785730315982,
-            "y": 5.898110210460681,
-            "heading": -0.0063148981970481905,
-            "angularVelocity": 0.0631003667916113,
-            "velocityX": 2.290373508899117,
-            "velocityY": -1.0103216160535444,
-            "timestamp": 1.901467593959885
-        },
-        {
-            "x": 7.603595242863294,
-            "y": 5.830703404850431,
-            "heading": -0.0021049639599730564,
-            "angularVelocity": 0.04206684918455414,
-            "velocityX": 1.5269157084883698,
-            "velocityY": -0.673547795746317,
-            "timestamp": 2.0015448357472474
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0.02103339303101045,
-            "velocityX": 0.7634578628680123,
-            "velocityY": -0.33677391831044023,
-            "timestamp": 2.1016220775346097
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 1.7450911582816108e-33,
-            "velocityY": 0,
-            "timestamp": 2.201699319321972
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Center To C3.traj b/src/main/deploy/choreo/Center To C3.traj
deleted file mode 100644
index 4db64f81..00000000
--- a/src/main/deploy/choreo/Center To C3.traj	
+++ /dev/null
@@ -1,211 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 2.6743025901632004e-33,
-            "velocityX": 2.068786142168461e-33,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 1.4007220754094205,
-            "y": 5.545584664540576,
-            "heading": -0.37232774466736046,
-            "angularVelocity": -3.437582019103784,
-            "velocityX": 0.4683005682394807,
-            "velocityY": -0.13309214444637307,
-            "timestamp": 0.10831094141120465
-        },
-        {
-            "x": 1.5327694511201624,
-            "y": 5.533363500300387,
-            "heading": -0.9362708794209632,
-            "angularVelocity": -5.206705134364786,
-            "velocityX": 1.2191508446909478,
-            "velocityY": -0.11283406903270367,
-            "timestamp": 0.2166218828224093
-        },
-        {
-            "x": 1.760106911270163,
-            "y": 5.516367699163042,
-            "heading": -1.4218044496098983,
-            "angularVelocity": -4.482774905866594,
-            "velocityX": 2.0989334705060814,
-            "velocityY": -0.1569167520465022,
-            "timestamp": 0.3249328242336139
-        },
-        {
-            "x": 2.067387230758427,
-            "y": 5.467280597023701,
-            "heading": -1.7292049865361503,
-            "angularVelocity": -2.838130044121764,
-            "velocityX": 2.83702011527781,
-            "velocityY": -0.453205387191498,
-            "timestamp": 0.4332437656448186
-        },
-        {
-            "x": 2.4488593918735733,
-            "y": 5.393167697716653,
-            "heading": -1.7983473850598053,
-            "angularVelocity": -0.6383694723984937,
-            "velocityX": 3.5220094677866336,
-            "velocityY": -0.6842605035226905,
-            "timestamp": 0.5415547070560233
-        },
-        {
-            "x": 2.8531804118778052,
-            "y": 5.326790341993422,
-            "heading": -1.7983490370676205,
-            "angularVelocity": -0.000015252455509683049,
-            "velocityX": 3.7329656148885193,
-            "velocityY": -0.6128407237384188,
-            "timestamp": 0.6498656484672279
-        },
-        {
-            "x": 3.257501404598323,
-            "y": 5.2604128200678275,
-            "heading": -1.7983506890680847,
-            "angularVelocity": -0.00001525238764202395,
-            "velocityX": 3.732965362986781,
-            "velocityY": -0.6128422582312392,
-            "timestamp": 0.7581765898784326
-        },
-        {
-            "x": 3.6466232677858144,
-            "y": 5.172310745117625,
-            "heading": -1.8337945854134767,
-            "angularVelocity": -0.32724206699329256,
-            "velocityX": 3.5926367005728674,
-            "velocityY": -0.8134180517896293,
-            "timestamp": 0.8664875312896373
-        },
-        {
-            "x": 4.014238623942389,
-            "y": 5.062846847355338,
-            "heading": -1.917513965365452,
-            "angularVelocity": -0.7729540419571551,
-            "velocityX": 3.394074055370967,
-            "velocityY": -1.0106448742487064,
-            "timestamp": 0.974798472700842
-        },
-        {
-            "x": 4.354761888880782,
-            "y": 4.880183720994759,
-            "heading": -1.9914398534549198,
-            "angularVelocity": -0.6825338892476871,
-            "velocityX": 3.143941512294591,
-            "velocityY": -1.6864697507068689,
-            "timestamp": 1.0831094141120465
-        },
-        {
-            "x": 4.661144802240214,
-            "y": 4.608133464126214,
-            "heading": -1.9914398486412812,
-            "angularVelocity": 4.444277523534959e-8,
-            "velocityX": 2.8287346538355926,
-            "velocityY": -2.5117523061285136,
-            "timestamp": 1.1914203555232512
-        },
-        {
-            "x": 5.019380877625839,
-            "y": 4.418922250546549,
-            "heading": -1.9767149939198712,
-            "angularVelocity": 0.13594983599585594,
-            "velocityX": 3.3074781801182462,
-            "velocityY": -1.7469261287400477,
-            "timestamp": 1.2997312969344559
-        },
-        {
-            "x": 5.376950498025801,
-            "y": 4.3069722509438675,
-            "heading": -1.8614424797513431,
-            "angularVelocity": 1.0642739566900585,
-            "velocityX": 3.3013250161167162,
-            "velocityY": -1.0335982509621156,
-            "timestamp": 1.4080422383456606
-        },
-        {
-            "x": 5.749629435463048,
-            "y": 4.240661073830716,
-            "heading": -1.7582912287295662,
-            "angularVelocity": 0.9523622422425545,
-            "velocityX": 3.4408244687151703,
-            "velocityY": -0.6122297179691145,
-            "timestamp": 1.5163531797568652
-        },
-        {
-            "x": 6.158173088993434,
-            "y": 4.209455444526307,
-            "heading": -1.7582906674190417,
-            "angularVelocity": 0.000005182399092709644,
-            "velocityX": 3.771951828756997,
-            "velocityY": -0.2881115139230142,
-            "timestamp": 1.62466412116807
-        },
-        {
-            "x": 6.566716775041419,
-            "y": 4.178250240929697,
-            "heading": -1.7582901061123783,
-            "angularVelocity": 0.000005182363444203767,
-            "velocityX": 3.771952128981517,
-            "velocityY": -0.2881075835001592,
-            "timestamp": 1.7329750625792746
-        },
-        {
-            "x": 6.948239527399369,
-            "y": 4.140315578246664,
-            "heading": -1.6699172621402418,
-            "angularVelocity": 0.8159179748667066,
-            "velocityX": 3.5224765604196056,
-            "velocityY": -0.35023850950582514,
-            "timestamp": 1.8412860039904793
-        },
-        {
-            "x": 7.2588942750640015,
-            "y": 4.14084991195257,
-            "heading": -1.390296709754989,
-            "angularVelocity": 2.5816464037891405,
-            "velocityX": 2.86817512263351,
-            "velocityY": 0.004933330824608595,
-            "timestamp": 1.949596945401684
-        },
-        {
-            "x": 7.4846066646178775,
-            "y": 4.133171401549216,
-            "heading": -0.9384850139510863,
-            "angularVelocity": 4.17143171241205,
-            "velocityX": 2.083929717653862,
-            "velocityY": -0.07089321081793584,
-            "timestamp": 2.0579078868128886
-        },
-        {
-            "x": 7.620450068903259,
-            "y": 4.128683952785512,
-            "heading": -0.34333254233125515,
-            "angularVelocity": 5.494850879010672,
-            "velocityX": 1.2541983525897882,
-            "velocityY": -0.04143116757398245,
-            "timestamp": 2.166218828224093
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 9.09513036114211e-34,
-            "angularVelocity": 3.1698786646843584,
-            "velocityX": 0.5498053134877472,
-            "velocityY": -0.015547393121808115,
-            "timestamp": 2.2745297696352975
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -7.032634787663221e-33,
-            "velocityX": 0,
-            "velocityY": -1.950631326224301e-32,
-            "timestamp": 2.382840711046502
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Center To C4.traj b/src/main/deploy/choreo/Center To C4.traj
deleted file mode 100644
index 1f520266..00000000
--- a/src/main/deploy/choreo/Center To C4.traj	
+++ /dev/null
@@ -1,1093 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": -1.9296664556128306e-33,
-            "angularVelocity": 0,
-            "velocityX": -7.813211441980168e-34,
-            "velocityY": -2.2407271587906947e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 1.35439418502164,
-            "y": 5.559271848784049,
-            "heading": -0.0037613483747314606,
-            "angularVelocity": -0.16062668985124567,
-            "velocityX": 0.18765169410035007,
-            "velocityY": -0.031095370031394967,
-            "timestamp": 0.023416708507251784
-        },
-        {
-            "x": 1.3631871179840578,
-            "y": 5.557813607446842,
-            "heading": -0.011205273313316288,
-            "angularVelocity": -0.317889464963847,
-            "velocityX": 0.37549824560930783,
-            "velocityY": -0.06227354014150002,
-            "timestamp": 0.04683341701450357
-        },
-        {
-            "x": 1.3763837173940232,
-            "y": 5.5556231238892515,
-            "heading": -0.02224504855076525,
-            "angularVelocity": -0.47144863395424863,
-            "velocityX": 0.563554839736692,
-            "velocityY": -0.09354361467639355,
-            "timestamp": 0.07025012552175536
-        },
-        {
-            "x": 1.3939893513502628,
-            "y": 5.552698044632534,
-            "heading": -0.03678367986427699,
-            "angularVelocity": -0.6208657083043512,
-            "velocityX": 0.7518406761047259,
-            "velocityY": -0.12491419346204678,
-            "timestamp": 0.09366683402900713
-        },
-        {
-            "x": 1.4160099408355828,
-            "y": 5.549035829570074,
-            "heading": -0.0547111148835232,
-            "angularVelocity": -0.7655830456997818,
-            "velocityX": 0.9403793653792271,
-            "velocityY": -0.15639324635770782,
-            "timestamp": 0.11708354253625891
-        },
-        {
-            "x": 1.4424520698548373,
-            "y": 5.544633771060564,
-            "heading": -0.07590088077413953,
-            "angularVelocity": -0.9048994176125892,
-            "velocityX": 1.129199221618411,
-            "velocityY": -0.18798792785714832,
-            "timestamp": 0.1405002510435107
-        },
-        {
-            "x": 1.4733230980825243,
-            "y": 5.539489016814175,
-            "heading": -0.10020600379509263,
-            "angularVelocity": -1.0379393420482537,
-            "velocityX": 1.3183333694454236,
-            "velocityY": -0.2197044151102235,
-            "timestamp": 0.16391695955076246
-        },
-        {
-            "x": 1.5086312686123933,
-            "y": 5.533598592872994,
-            "heading": -0.12745397379473564,
-            "angularVelocity": -1.163612298082128,
-            "velocityX": 1.5078195348818708,
-            "velocityY": -0.25154790389774145,
-            "timestamp": 0.18733366805801424
-        },
-        {
-            "x": 1.5483857981367395,
-            "y": 5.526959418437362,
-            "heading": -0.15744034724886685,
-            "angularVelocity": -1.2805545854082991,
-            "velocityX": 1.6976992950155392,
-            "velocityY": -0.28352295684851286,
-            "timestamp": 0.21075037656526602
-        },
-        {
-            "x": 1.5925969268012463,
-            "y": 5.5195682985391485,
-            "heading": -0.18992026053497643,
-            "angularVelocity": -1.3870400819163256,
-            "velocityX": 1.888016355962865,
-            "velocityY": -0.31563444947560276,
-            "timestamp": 0.2341670850725178
-        },
-        {
-            "x": 1.6412758830317984,
-            "y": 5.511421875068327,
-            "heading": -0.22459650029504458,
-            "angularVelocity": -1.4808332157070316,
-            "velocityX": 2.0788129217851563,
-            "velocityY": -0.34788934868040844,
-            "timestamp": 0.25758379357976957
-        },
-        {
-            "x": 1.6944346649736404,
-            "y": 5.502516516620167,
-            "heading": -0.26110152083126115,
-            "angularVelocity": -1.5589304758570732,
-            "velocityX": 2.2701218630013136,
-            "velocityY": -0.38029932539011047,
-            "timestamp": 0.2810005020870214
-        },
-        {
-            "x": 1.7520853949904163,
-            "y": 5.492848149517658,
-            "heading": -0.298968117565775,
-            "angularVelocity": -1.6170759747377448,
-            "velocityX": 2.4619484843022064,
-            "velocityY": -0.4128832666435571,
-            "timestamp": 0.3044172105942732
-        },
-        {
-            "x": 1.8142385663960479,
-            "y": 5.482412126535505,
-            "heading": -0.337577206769387,
-            "angularVelocity": -1.6487837815317001,
-            "velocityX": 2.654223217852448,
-            "velocityY": -0.44566566556215986,
-            "timestamp": 0.327833919101525
-        },
-        {
-            "x": 1.8808979849725118,
-            "y": 5.471203531590798,
-            "heading": -0.3760544549024592,
-            "angularVelocity": -1.6431535679387552,
-            "velocityX": 2.8466604756095246,
-            "velocityY": -0.47865800358925525,
-            "timestamp": 0.3512506276087768
-        },
-        {
-            "x": 1.9520436894250033,
-            "y": 5.459218834920382,
-            "heading": -0.41303307424029884,
-            "angularVelocity": -1.5791552995754587,
-            "velocityX": 3.0382452952539354,
-            "velocityY": -0.5118010785637118,
-            "timestamp": 0.3746673361160286
-        },
-        {
-            "x": 2.027554046757936,
-            "y": 5.446452385802885,
-            "heading": -0.44595627669268334,
-            "angularVelocity": -1.4059705462956014,
-            "velocityX": 3.2246358325529645,
-            "velocityY": -0.5451854650516427,
-            "timestamp": 0.3980840446232804
-        },
-        {
-            "x": 2.106704107988529,
-            "y": 5.4327632732947455,
-            "heading": -0.46854805770485686,
-            "angularVelocity": -0.9647718425148408,
-            "velocityX": 3.3800677497471567,
-            "velocityY": -0.5845873899786794,
-            "timestamp": 0.4215007531305322
-        },
-        {
-            "x": 2.1895060473348913,
-            "y": 5.418170740153337,
-            "heading": -0.4808901513114338,
-            "angularVelocity": -0.5270635539044761,
-            "velocityX": 3.5360195614476866,
-            "velocityY": -0.6231675616108724,
-            "timestamp": 0.444917461637784
-        },
-        {
-            "x": 2.275961949822614,
-            "y": 5.402702256569038,
-            "heading": -0.4829709400280361,
-            "angularVelocity": -0.0888591458512548,
-            "velocityX": 3.6920604132278863,
-            "velocityY": -0.6605746311236733,
-            "timestamp": 0.4683341701450358
-        },
-        {
-            "x": 2.3633379318433296,
-            "y": 5.388122823546896,
-            "heading": -0.48297098226531077,
-            "angularVelocity": -0.0000018037238101032338,
-            "velocityX": 3.7313519956767345,
-            "velocityY": -0.6226081269122786,
-            "timestamp": 0.4917508786522876
-        },
-        {
-            "x": 2.4507217300981603,
-            "y": 5.373590311340196,
-            "heading": -0.48297102439627604,
-            "angularVelocity": -0.0000017991839149984119,
-            "velocityX": 3.7316857844376363,
-            "velocityY": -0.6206043946012527,
-            "timestamp": 0.5151675871595394
-        },
-        {
-            "x": 2.5381084152434026,
-            "y": 5.359075168632344,
-            "heading": -0.4829710665257657,
-            "angularVelocity": -0.0000017991208983915868,
-            "velocityX": 3.7318090677936033,
-            "velocityY": -0.6198626379688947,
-            "timestamp": 0.5385842956667911
-        },
-        {
-            "x": 2.6254950830921073,
-            "y": 5.344559921791865,
-            "heading": -0.482971108655264,
-            "angularVelocity": -0.0000017991212673817243,
-            "velocityX": 3.7318083291527238,
-            "velocityY": -0.6198670849061145,
-            "timestamp": 0.5620010041740429
-        },
-        {
-            "x": 2.712881673840345,
-            "y": 5.330044210788612,
-            "heading": -0.4829711507848006,
-            "angularVelocity": -0.0000017991229009417428,
-            "velocityX": 3.731805036611997,
-            "velocityY": -0.6198869067684731,
-            "timestamp": 0.5854177126812946
-        },
-        {
-            "x": 2.800268245782709,
-            "y": 5.315528386572539,
-            "heading": -0.4829711929143463,
-            "angularVelocity": -0.0000017991232935293638,
-            "velocityX": 3.7318042335155903,
-            "velocityY": -0.6198917414709255,
-            "timestamp": 0.6088344211885464
-        },
-        {
-            "x": 2.8876548130761686,
-            "y": 5.301012534370082,
-            "heading": -0.4829712350438941,
-            "angularVelocity": -0.0000017991233825733017,
-            "velocityX": 3.731804034986268,
-            "velocityY": -0.6198929366167053,
-            "timestamp": 0.6322511296957981
-        },
-        {
-            "x": 2.975041345179212,
-            "y": 5.286496470320027,
-            "heading": -0.48297127717345933,
-            "angularVelocity": -0.000001799124124149535,
-            "velocityX": 3.7318025321954105,
-            "velocityY": -0.6199019834729332,
-            "timestamp": 0.6556678382030499
-        },
-        {
-            "x": 3.0624277772479402,
-            "y": 5.271979804077107,
-            "heading": -0.4829713193030743,
-            "angularVelocity": -0.0000017991262516148556,
-            "velocityX": 3.7317982602749975,
-            "velocityY": -0.6199276998481766,
-            "timestamp": 0.6790845467103016
-        },
-        {
-            "x": 3.149814115388955,
-            "y": 5.257462572426413,
-            "heading": -0.482971361432736,
-            "angularVelocity": -0.0000017991282469783394,
-            "velocityX": 3.731794249134225,
-            "velocityY": -0.619951845332987,
-            "timestamp": 0.7025012552175534
-        },
-        {
-            "x": 3.2372009926614522,
-            "y": 5.242948586436876,
-            "heading": -0.48297140356212753,
-            "angularVelocity": -0.0000017991167072996656,
-            "velocityX": 3.7318172725014778,
-            "velocityY": -0.6198132408343549,
-            "timestamp": 0.7259179637248051
-        },
-        {
-            "x": 3.3245908005140246,
-            "y": 5.228452256133779,
-            "heading": -0.4829714456900571,
-            "angularVelocity": -0.0000017990542795775989,
-            "velocityX": 3.7319424216049217,
-            "velocityY": -0.6190592626894866,
-            "timestamp": 0.7493346722320569
-        },
-        {
-            "x": 3.4119833565424647,
-            "y": 5.213972502683899,
-            "heading": -0.48297148781659605,
-            "angularVelocity": -0.0000017989948869691892,
-            "velocityX": 3.7320597812190472,
-            "velocityY": -0.6183513556300411,
-            "timestamp": 0.7727513807393086
-        },
-        {
-            "x": 3.4993677183765524,
-            "y": 5.199443379550061,
-            "heading": -0.48297152994719206,
-            "angularVelocity": -0.0000017991681461203675,
-            "velocityX": 3.731709851836638,
-            "velocityY": -0.6204596657670539,
-            "timestamp": 0.7961680892465604
-        },
-        {
-            "x": 3.5867330233447126,
-            "y": 5.184800101210533,
-            "heading": -0.48297157208742414,
-            "angularVelocity": -0.0000017995796480013664,
-            "velocityX": 3.7308960369522555,
-            "velocityY": -0.6253346124593939,
-            "timestamp": 0.8195847977538121
-        },
-        {
-            "x": 3.6740864894013328,
-            "y": 5.170086363489935,
-            "heading": -0.48297161423379187,
-            "angularVelocity": -0.0000017998416699478457,
-            "velocityX": 3.7303904615618975,
-            "velocityY": -0.6283435486246598,
-            "timestamp": 0.8430015062610638
-        },
-        {
-            "x": 3.7614250146254347,
-            "y": 5.155284197469098,
-            "heading": -0.482971656398251,
-            "angularVelocity": -0.0000018006142553635733,
-            "velocityX": 3.729752420031712,
-            "velocityY": -0.6321198393988197,
-            "timestamp": 0.8664182147683156
-        },
-        {
-            "x": 3.848175799548237,
-            "y": 5.137355912505346,
-            "heading": -0.48297169899264486,
-            "angularVelocity": -0.000001818974422860047,
-            "velocityX": 3.704653234929829,
-            "velocityY": -0.7656193421980864,
-            "timestamp": 0.8898349232755673
-        },
-        {
-            "x": 3.9339293985898425,
-            "y": 5.115142351916783,
-            "heading": -0.48297174253890224,
-            "angularVelocity": -0.0000018596233270255789,
-            "velocityX": 3.6620688605764276,
-            "velocityY": -0.9486201095127581,
-            "timestamp": 0.9132516317828191
-        },
-        {
-            "x": 4.018431663513184,
-            "y": 5.088562488555908,
-            "heading": -0.4829717875114574,
-            "angularVelocity": -0.000001920532733427299,
-            "velocityX": 3.6086312001179417,
-            "velocityY": -1.135081104701801,
-            "timestamp": 0.9366683402900708
-        },
-        {
-            "x": 4.095489617333613,
-            "y": 5.060208566496142,
-            "heading": -0.48297174072333765,
-            "angularVelocity": 0.00000215563208981735,
-            "velocityX": 3.550230245224857,
-            "velocityY": -1.306327856847248,
-            "timestamp": 0.9583733984124503
-        },
-        {
-            "x": 4.1711135466589555,
-            "y": 5.028225843813575,
-            "heading": -0.482971694875483,
-            "angularVelocity": 0.0000021123119922022846,
-            "velocityX": 3.4841615672696387,
-            "velocityY": -1.4735147218783384,
-            "timestamp": 0.9800784565348297
-        },
-        {
-            "x": 4.245175973739862,
-            "y": 4.9927768637698655,
-            "heading": -0.48297164958118555,
-            "angularVelocity": 0.0000020868083942836762,
-            "velocityX": 3.4122197076519116,
-            "velocityY": -1.633212859593547,
-            "timestamp": 1.0017835146572092
-        },
-        {
-            "x": 4.317835043195099,
-            "y": 4.954533786792328,
-            "heading": -0.4829716044969262,
-            "angularVelocity": 0.0000020771314744606885,
-            "velocityX": 3.3475639201504173,
-            "velocityY": -1.7619430808199938,
-            "timestamp": 1.0234885727795886
-        },
-        {
-            "x": 4.39018173013391,
-            "y": 4.915702997439957,
-            "heading": -0.48297155943721365,
-            "angularVelocity": 0.000002076000546478681,
-            "velocityX": 3.333171767193961,
-            "velocityY": -1.789020288884605,
-            "timestamp": 1.045193630901968
-        },
-        {
-            "x": 4.462524157787397,
-            "y": 4.876864273483723,
-            "heading": -0.4829715143782692,
-            "angularVelocity": 0.000002075965162802355,
-            "velocityX": 3.3329755325063237,
-            "velocityY": -1.7893858536194671,
-            "timestamp": 1.0668986890243475
-        },
-        {
-            "x": 4.534839835738717,
-            "y": 4.8379757656949245,
-            "heading": -0.48297146931947993,
-            "angularVelocity": 0.0000020759580074306272,
-            "velocityX": 3.331743114605303,
-            "velocityY": -1.7916795048180625,
-            "timestamp": 1.088603747146727
-        },
-        {
-            "x": 4.60713879668486,
-            "y": 4.7990561876016065,
-            "heading": -0.48297142426091105,
-            "angularVelocity": 0.000002075947856864141,
-            "velocityX": 3.3309729252282607,
-            "velocityY": -1.793110982419974,
-            "timestamp": 1.1103088052691064
-        },
-        {
-            "x": 4.679445820973998,
-            "y": 4.760151592099993,
-            "heading": -0.48297137920221006,
-            "angularVelocity": 0.0000020759539417382525,
-            "velocityX": 3.331344421260875,
-            "velocityY": -1.7924207013070022,
-            "timestamp": 1.1320138633914858
-        },
-        {
-            "x": 4.75175835900604,
-            "y": 4.72125724602898,
-            "heading": -0.4829713341434503,
-            "angularVelocity": 0.000002075956651977989,
-            "velocityX": 3.3315984515826855,
-            "velocityY": -1.791948487386363,
-            "timestamp": 1.1537189215138652
-        },
-        {
-            "x": 4.824072564767644,
-            "y": 4.68236600077068,
-            "heading": -0.48297128908467135,
-            "angularVelocity": 0.0000020759575350386475,
-            "velocityX": 3.331675287569862,
-            "velocityY": -1.7918056260909452,
-            "timestamp": 1.1754239796362447
-        },
-        {
-            "x": 4.8963869623773055,
-            "y": 4.643475112237639,
-            "heading": -0.48297124402589003,
-            "angularVelocity": 0.000002075957644932215,
-            "velocityX": 3.3316841264339563,
-            "velocityY": -1.79178919096958,
-            "timestamp": 1.1971290377586241
-        },
-        {
-            "x": 4.968701267365325,
-            "y": 4.604584051483172,
-            "heading": -0.4829711989671097,
-            "angularVelocity": 0.000002075957594698055,
-            "velocityX": 3.331679859150302,
-            "velocityY": -1.7917971255911598,
-            "timestamp": 1.2188340958810036
-        },
-        {
-            "x": 5.0410154847403765,
-            "y": 4.565692827821048,
-            "heading": -0.4829711539083305,
-            "angularVelocity": 0.0000020759575472868184,
-            "velocityX": 3.331675822627313,
-            "velocityY": -1.7918046311069282,
-            "timestamp": 1.240539154003383
-        },
-        {
-            "x": 5.113329654064444,
-            "y": 4.526801514813212,
-            "heading": -0.4829711088495519,
-            "angularVelocity": 0.000002075957520958662,
-            "velocityX": 3.331673608812237,
-            "velocityY": -1.7918087474613025,
-            "timestamp": 1.2622442121257624
-        },
-        {
-            "x": 5.185643801945598,
-            "y": 4.487910161934636,
-            "heading": -0.48297106379077354,
-            "angularVelocity": 0.0000020759575090327586,
-            "velocityX": 3.3316726208897984,
-            "velocityY": -1.7918105843944965,
-            "timestamp": 1.2839492702481419
-        },
-        {
-            "x": 5.257957938797454,
-            "y": 4.4490187885483135,
-            "heading": -0.48297101873199527,
-            "angularVelocity": 0.0000020759575026231074,
-            "velocityX": 3.331672112745739,
-            "velocityY": -1.7918115292316654,
-            "timestamp": 1.3056543283705213
-        },
-        {
-            "x": 5.330272065176824,
-            "y": 4.410127395689581,
-            "heading": -0.4829709736732172,
-            "angularVelocity": 0.000002075957497099591,
-            "velocityX": 3.3316716302550873,
-            "velocityY": -1.7918124263686461,
-            "timestamp": 1.3273593864929008
-        },
-        {
-            "x": 5.402586187963667,
-            "y": 4.371235996150952,
-            "heading": -0.4829709286144391,
-            "angularVelocity": 0.00000207595749402331,
-            "velocityX": 3.3316714647394585,
-            "velocityY": -1.7918127341261798,
-            "timestamp": 1.3490644446152802
-        },
-        {
-            "x": 5.474900315939471,
-            "y": 4.332344606260608,
-            "heading": -0.482970883555661,
-            "angularVelocity": 0.0000020759574957870433,
-            "velocityX": 3.331671703806323,
-            "velocityY": -1.7918122896084423,
-            "timestamp": 1.3707695027376596
-        },
-        {
-            "x": 5.547214451711641,
-            "y": 4.293453230866729,
-            "heading": -0.4829708384968828,
-            "angularVelocity": 0.0000020759574997718594,
-            "velocityX": 3.331672063002153,
-            "velocityY": -1.7918116217242377,
-            "timestamp": 1.392474560860039
-        },
-        {
-            "x": 5.619528591620017,
-            "y": 4.254561863163663,
-            "heading": -0.4829707934381046,
-            "angularVelocity": 0.0000020759575011693265,
-            "velocityX": 3.3316722535663335,
-            "velocityY": -1.7918112673914555,
-            "timestamp": 1.4141796189824185
-        },
-        {
-            "x": 5.6918427353771435,
-            "y": 4.215670502616919,
-            "heading": -0.4829707483793263,
-            "angularVelocity": 0.000002075957502624911,
-            "velocityX": 3.3316724308867816,
-            "velocityY": -1.7918109376838844,
-            "timestamp": 1.435884677104798
-        },
-        {
-            "x": 5.764156898177578,
-            "y": 4.176779177479102,
-            "heading": -0.4829707033205479,
-            "angularVelocity": 0.0000020759575111736717,
-            "velocityX": 3.3316733082540817,
-            "velocityY": -1.7918093063163627,
-            "timestamp": 1.4575897352271774
-        },
-        {
-            "x": 5.836471080177112,
-            "y": 4.137887888039928,
-            "heading": -0.48297065826176927,
-            "angularVelocity": 0.0000020759575207599675,
-            "velocityX": 3.331674192798964,
-            "velocityY": -1.7918076616010294,
-            "timestamp": 1.4792947933495568
-        },
-        {
-            "x": 5.908785251268908,
-            "y": 4.098996578318978,
-            "heading": -0.48297061320299073,
-            "angularVelocity": 0.0000020759575149169034,
-            "velocityX": 3.331673690255448,
-            "velocityY": -1.7918085960272527,
-            "timestamp": 1.5009998514719363
-        },
-        {
-            "x": 5.981099339043632,
-            "y": 4.0601051136793025,
-            "heading": -0.48297056814421313,
-            "angularVelocity": 0.000002075957470564626,
-            "velocityX": 3.3316698516536873,
-            "velocityY": -1.7918157334753977,
-            "timestamp": 1.5227049095943157
-        },
-        {
-            "x": 6.053413282760847,
-            "y": 4.021213381182831,
-            "heading": -0.48297052308543725,
-            "angularVelocity": 0.00000207595739406405,
-            "velocityX": 3.3316632146058507,
-            "velocityY": -1.7918280742300665,
-            "timestamp": 1.5444099677166951
-        },
-        {
-            "x": 6.125727369888532,
-            "y": 3.982321915340118,
-            "heading": -0.4829704780266597,
-            "angularVelocity": 0.0000020759574679524,
-            "velocityX": 3.3316698218431826,
-            "velocityY": -1.7918157889019397,
-            "timestamp": 1.5661150258390746
-        },
-        {
-            "x": 6.198042995964267,
-            "y": 3.9434333111252817,
-            "heading": -0.4829704329678644,
-            "angularVelocity": 0.000002075958288103883,
-            "velocityX": 3.3317407245813113,
-            "velocityY": -1.7916839473808401,
-            "timestamp": 1.587820083961454
-        },
-        {
-            "x": 6.270359121166566,
-            "y": 3.9045456350763237,
-            "heading": -0.4829703879090624,
-            "angularVelocity": 0.0000020759585995923127,
-            "velocityX": 3.331763720444871,
-            "velocityY": -1.791641184728093,
-            "timestamp": 1.6095251420838335
-        },
-        {
-            "x": 6.342677550055512,
-            "y": 3.8656622432711547,
-            "heading": -0.48297034285023976,
-            "angularVelocity": 0.0000020759595452668008,
-            "velocityX": 3.331869856379567,
-            "velocityY": -1.791443800149147,
-            "timestamp": 1.631230200206213
-        },
-        {
-            "x": 6.414977737835259,
-            "y": 3.8267449442462484,
-            "heading": -0.4829702977916256,
-            "angularVelocity": 0.0000020759499429522823,
-            "velocityX": 3.3310294481622025,
-            "velocityY": -1.7930059807042389,
-            "timestamp": 1.6529352583285923
-        },
-        {
-            "x": 6.487268603977522,
-            "y": 3.787810332568674,
-            "heading": -0.4829702527330872,
-            "angularVelocity": 0.00000207594645249869,
-            "velocityX": 3.330599979721225,
-            "velocityY": -1.793803612876013,
-            "timestamp": 1.6746403164509718
-        },
-        {
-            "x": 6.559543297837492,
-            "y": 3.7488457083598847,
-            "heading": -0.4829702076720725,
-            "angularVelocity": 0.000002076060537305802,
-            "velocityX": 3.3298548869330626,
-            "velocityY": -1.7951863565208734,
-            "timestamp": 1.6963453745733512
-        },
-        {
-            "x": 6.630628961599716,
-            "y": 3.7077518923991635,
-            "heading": -0.48297016252441444,
-            "angularVelocity": 0.000002080052392839484,
-            "velocityX": 3.2750736423474676,
-            "velocityY": -1.893282926450661,
-            "timestamp": 1.7180504326957307
-        },
-        {
-            "x": 6.699799614538253,
-            "y": 3.6635105764693066,
-            "heading": -0.4829701170038858,
-            "angularVelocity": 0.0000020972313632639956,
-            "velocityX": 3.186844861162469,
-            "velocityY": -2.0382952066016227,
-            "timestamp": 1.73975549081811
-        },
-        {
-            "x": 6.766806665010948,
-            "y": 3.616056089090901,
-            "heading": -0.48297007073382403,
-            "angularVelocity": 0.000002131764013783056,
-            "velocityX": 3.0871629135886995,
-            "velocityY": -2.1863331169555953,
-            "timestamp": 1.7614605489404895
-        },
-        {
-            "x": 6.831474779993965,
-            "y": 3.565460555976799,
-            "heading": -0.48297002329923955,
-            "angularVelocity": 0.0000021854161466442962,
-            "velocityX": 2.9794029860873867,
-            "velocityY": -2.331048036306895,
-            "timestamp": 1.783165607062869
-        },
-        {
-            "x": 6.893649101257324,
-            "y": 3.5118300914764404,
-            "heading": -0.48296997476148124,
-            "angularVelocity": 0.00000223624180421772,
-            "velocityX": 2.8645083976649595,
-            "velocityY": -2.470874033046681,
-            "timestamp": 1.8048706651852484
-        },
-        {
-            "x": 6.934561334359113,
-            "y": 3.474160479757131,
-            "heading": -0.4829699264500687,
-            "angularVelocity": 0.0000032862628672599958,
-            "velocityX": 2.782952215912443,
-            "velocityY": -2.5623810156238895,
-            "timestamp": 1.8195716840772946
-        },
-        {
-            "x": 6.974231870495054,
-            "y": 3.435185370851295,
-            "heading": -0.4829698798888691,
-            "angularVelocity": 0.0000031672090196575476,
-            "velocityX": 2.6984888889168737,
-            "velocityY": -2.6511841928806597,
-            "timestamp": 1.8342727029693409
-        },
-        {
-            "x": 7.012622614846585,
-            "y": 3.394949051793438,
-            "heading": -0.482969834780297,
-            "angularVelocity": 0.0000030683976682796836,
-            "velocityX": 2.611434257274787,
-            "velocityY": -2.736974855506367,
-            "timestamp": 1.848973721861387
-        },
-        {
-            "x": 7.049699862488227,
-            "y": 3.3534992220248374,
-            "heading": -0.4829697908553284,
-            "angularVelocity": 0.0000029878860039287864,
-            "velocityX": 2.5220869324714577,
-            "velocityY": -2.8195208830745364,
-            "timestamp": 1.8636747407534333
-        },
-        {
-            "x": 7.085438739774718,
-            "y": 3.310890028476691,
-            "heading": -0.48296974786836877,
-            "angularVelocity": 0.0000029240802913465185,
-            "velocityX": 2.4310476402306533,
-            "velocityY": -2.8983837012274143,
-            "timestamp": 1.8783757596454795
-        },
-        {
-            "x": 7.119836677140904,
-            "y": 3.2671911422114968,
-            "heading": -0.4829697055890564,
-            "angularVelocity": 0.0000028759443619428334,
-            "velocityX": 2.339833559754191,
-            "velocityY": -2.9725073198044742,
-            "timestamp": 1.8930767785375258
-        },
-        {
-            "x": 7.1529563800127525,
-            "y": 3.222515689615774,
-            "heading": -0.48296966398118896,
-            "angularVelocity": 0.000002830270996857658,
-            "velocityX": 2.2528848588700137,
-            "velocityY": -3.0389357991976094,
-            "timestamp": 1.907777797429572
-        },
-        {
-            "x": 7.184894346745796,
-            "y": 3.177381185858669,
-            "heading": -0.4821430167851757,
-            "angularVelocity": 0.056230605652804695,
-            "velocityX": 2.17250021699707,
-            "velocityY": -3.0701616050246474,
-            "timestamp": 1.9224788163216182
-        },
-        {
-            "x": 7.2158791345376,
-            "y": 3.1333943550888805,
-            "heading": -0.47750834116458535,
-            "angularVelocity": 0.31526220424747137,
-            "velocityX": 2.107662606199986,
-            "velocityY": -2.992094023740475,
-            "timestamp": 1.9371798352136644
-        },
-        {
-            "x": 7.245895346179036,
-            "y": 3.090550131820991,
-            "heading": -0.46908945700490695,
-            "angularVelocity": 0.5726735147747635,
-            "velocityX": 2.041777638805421,
-            "velocityY": -2.9143710094182476,
-            "timestamp": 1.9518808541057107
-        },
-        {
-            "x": 7.274933319615603,
-            "y": 3.048846636162261,
-            "heading": -0.45692051225089847,
-            "angularVelocity": 0.827761996863506,
-            "velocityX": 1.9752354343465999,
-            "velocityY": -2.8367758700924766,
-            "timestamp": 1.9665818729977569
-        },
-        {
-            "x": 7.3029845213490745,
-            "y": 3.008282422680816,
-            "heading": -0.4410407652781448,
-            "angularVelocity": 1.08018002625281,
-            "velocityX": 1.90811276003785,
-            "velocityY": -2.7592790526506894,
-            "timestamp": 1.981282891889803
-        },
-        {
-            "x": 7.330040747407425,
-            "y": 2.9688555797343485,
-            "heading": -0.42149121219843927,
-            "angularVelocity": 1.3298093977882255,
-            "velocityX": 1.8404320310743014,
-            "velocityY": -2.6819122698903146,
-            "timestamp": 1.9959839107818493
-        },
-        {
-            "x": 7.356112777651364,
-            "y": 2.9307983654229495,
-            "heading": -0.3996724931243264,
-            "angularVelocity": 1.4841637327544344,
-            "velocityX": 1.7734845751435842,
-            "velocityY": -2.5887467114262686,
-            "timestamp": 2.0106849296738956
-        },
-        {
-            "x": 7.38118332940277,
-            "y": 2.8941879380599844,
-            "heading": -0.37646864699164706,
-            "angularVelocity": 1.578383532670194,
-            "velocityX": 1.7053615083080305,
-            "velocityY": -2.4903326518935414,
-            "timestamp": 2.025385948565942
-        },
-        {
-            "x": 7.40523963577506,
-            "y": 2.8590504496296347,
-            "heading": -0.3524751392807844,
-            "angularVelocity": 1.6320982842790503,
-            "velocityX": 1.6363700059799433,
-            "velocityY": -2.390139669119207,
-            "timestamp": 2.040086967457988
-        },
-        {
-            "x": 7.428276340738969,
-            "y": 2.825395406477394,
-            "heading": -0.3281233875567818,
-            "angularVelocity": 1.656466936259613,
-            "velocityX": 1.5670141731720877,
-            "velocityY": -2.289300040995877,
-            "timestamp": 2.0547879863500342
-        },
-        {
-            "x": 7.450292281624529,
-            "y": 2.793225377698084,
-            "heading": -0.3037369697425905,
-            "angularVelocity": 1.658825010243686,
-            "velocityX": 1.497579252650967,
-            "velocityY": -2.1882856566299744,
-            "timestamp": 2.0694890052420805
-        },
-        {
-            "x": 7.471288319158757,
-            "y": 2.7625397684205146,
-            "heading": -0.27956516086770095,
-            "angularVelocity": 1.6442267745106667,
-            "velocityX": 1.4282028809299567,
-            "velocityY": -2.0873117368873455,
-            "timestamp": 2.0841900241341267
-        },
-        {
-            "x": 7.491266209557209,
-            "y": 2.733336527235843,
-            "heading": -0.2558043926700512,
-            "angularVelocity": 1.6162667616531794,
-            "velocityX": 1.3589459713748013,
-            "velocityY": -1.9864773590946931,
-            "timestamp": 2.098891043026173
-        },
-        {
-            "x": 7.510228057377649,
-            "y": 2.7056129838220437,
-            "heading": -0.2326125564520332,
-            "angularVelocity": 1.577566588297197,
-            "velocityX": 1.28983221909181,
-            "velocityY": -1.885824623271541,
-            "timestamp": 2.113592061918219
-        },
-        {
-            "x": 7.528176060430696,
-            "y": 2.679366275566135,
-            "heading": -0.21011884554685348,
-            "angularVelocity": 1.5300783619392286,
-            "velocityX": 1.2208679673730278,
-            "velocityY": -1.7853666095287006,
-            "timestamp": 2.1282930808102654
-        },
-        {
-            "x": 7.545112396890263,
-            "y": 2.654593565371254,
-            "heading": -0.18843070859149175,
-            "angularVelocity": 1.4752812110931899,
-            "velocityX": 1.152051880480808,
-            "velocityY": -1.6851015821961506,
-            "timestamp": 2.1429940997023116
-        },
-        {
-            "x": 7.561039180049459,
-            "y": 2.631292148556631,
-            "heading": -0.1676388680105614,
-            "angularVelocity": 1.4143128944742427,
-            "velocityX": 1.0833795450607198,
-            "velocityY": -1.5850205340005987,
-            "timestamp": 2.157695118594358
-        },
-        {
-            "x": 7.575958444025175,
-            "y": 2.6094594995019675,
-            "heading": -0.14782100448684538,
-            "angularVelocity": 1.3480605439149753,
-            "velocityX": 1.0148455753489192,
-            "velocityY": -1.485111284801969,
-            "timestamp": 2.172396137486404
-        },
-        {
-            "x": 7.589872142500143,
-            "y": 2.5890932855015207,
-            "heading": -0.12904449811203259,
-            "angularVelocity": 1.2772248313327248,
-            "velocityX": 0.9464445000131985,
-            "velocityY": -1.3853607120705473,
-            "timestamp": 2.1870971563784503
-        },
-        {
-            "x": 7.602782151943804,
-            "y": 2.570191363211945,
-            "heading": -0.11136848807718236,
-            "angularVelocity": 1.2023663233582889,
-            "velocityX": 0.8781710668125251,
-            "velocityY": -1.2857559349035097,
-            "timestamp": 2.2017981752704965
-        },
-        {
-            "x": 7.614690275426409,
-            "y": 2.552751766479238,
-            "heading": -0.09484543137092857,
-            "angularVelocity": 1.1239395600799826,
-            "velocityX": 0.8100202829510676,
-            "velocityY": -1.186284900439311,
-            "timestamp": 2.2164991941625427
-        },
-        {
-            "x": 7.62559824546286,
-            "y": 2.536772690588083,
-            "heading": -0.07952228805027751,
-            "angularVelocity": 1.042318456507909,
-            "velocityX": 0.7419873490775946,
-            "velocityY": -1.086936627215761,
-            "timestamp": 2.231200213054589
-        },
-        {
-            "x": 7.635507725447758,
-            "y": 2.522252475791086,
-            "heading": -0.06544142530276546,
-            "angularVelocity": 0.9578154310876013,
-            "velocityX": 0.6740675634570735,
-            "velocityY": -0.9877012541528096,
-            "timestamp": 2.245901231946635
-        },
-        {
-            "x": 7.644420309753795,
-            "y": 2.509189591670685,
-            "heading": -0.05264130826624494,
-            "angularVelocity": 0.8706959109783889,
-            "velocityX": 0.6062562310465265,
-            "velocityY": -0.8885699839124228,
-            "timestamp": 2.2606022508386814
-        },
-        {
-            "x": 7.652337522761704,
-            "y": 2.497582623097395,
-            "heading": -0.04115702850586921,
-            "angularVelocity": 0.7811893750159932,
-            "velocityX": 0.538548590818527,
-            "velocityY": -0.7895349743118973,
-            "timestamp": 2.2753032697307276
-        },
-        {
-            "x": 7.6592608171290975,
-            "y": 2.487430258069545,
-            "heading": -0.031020708755951136,
-            "angularVelocity": 0.6894977704846257,
-            "velocityX": 0.4709397639872257,
-            "velocityY": -0.6905892103399419,
-            "timestamp": 2.290004288622774
-        },
-        {
-            "x": 7.665191571570965,
-            "y": 2.4787312774326344,
-            "heading": -0.022261813501037925,
-            "angularVelocity": 0.5958019181685587,
-            "velocityX": 0.40342472079099084,
-            "velocityY": -0.5917263763001924,
-            "timestamp": 2.30470530751482
-        },
-        {
-            "x": 7.670131088361328,
-            "y": 2.4714845463094046,
-            "heading": -0.014907388223763604,
-            "angularVelocity": 0.5002663646159557,
-            "velocityX": 0.33599826152420176,
-            "velocityY": -0.49294073944433947,
-            "timestamp": 2.3194063264068663
-        },
-        {
-            "x": 7.674080590697013,
-            "y": 2.465689006980458,
-            "heading": -0.008982245050778671,
-            "angularVelocity": 0.40304302827545885,
-            "velocityX": 0.26865500715889973,
-            "velocityY": -0.3942270513020354,
-            "timestamp": 2.3341073452989125
-        },
-        {
-            "x": 7.677041220003002,
-            "y": 2.461343672911727,
-            "heading": -0.0045091086563273375,
-            "angularVelocity": 0.304273902870198,
-            "velocityX": 0.20138939536970257,
-            "velocityY": -0.29558046966947404,
-            "timestamp": 2.3488083641909587
-        },
-        {
-            "x": 7.679014033209882,
-            "y": 2.458447623611078,
-            "heading": -0.00150873334272897,
-            "angularVelocity": 0.20409301801671295,
-            "velocityX": 0.1341956786374654,
-            "velocityY": -0.1969965022095263,
-            "timestamp": 2.363509383083005
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -1.1328442579469643e-34,
-            "angularVelocity": 0.10262780789604178,
-            "velocityX": 0.06706792211866815,
-            "velocityY": -0.0984709714141921,
-            "timestamp": 2.378210401975051
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -2.155134012002005e-33,
-            "angularVelocity": -1.3889170145055665e-31,
-            "velocityX": 1.4892305241424507e-32,
-            "velocityY": 6.576507695252416e-33,
-            "timestamp": 2.3929114208670974
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Center To C5.traj b/src/main/deploy/choreo/Center To C5.traj
deleted file mode 100644
index 39d8ef61..00000000
--- a/src/main/deploy/choreo/Center To C5.traj	
+++ /dev/null
@@ -1,625 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -4.1566893564125267e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 1.3546830312136449,
-            "y": 5.550921973415328,
-            "heading": -1.4050458772244498e-18,
-            "angularVelocity": -4.014308801297789e-17,
-            "velocityX": 0.13379730014070312,
-            "velocityY": -0.25936522568857817,
-            "timestamp": 0.03500093954603885
-        },
-        {
-            "x": 1.3640490935091651,
-            "y": 5.532765920501419,
-            "heading": -3.603876179363921e-18,
-            "angularVelocity": -6.28220327965604e-17,
-            "velocityX": 0.26759459651667833,
-            "velocityY": -0.5187304440792354,
-            "timestamp": 0.0700018790920777
-        },
-        {
-            "x": 1.3780981867284385,
-            "y": 5.505531841564791,
-            "heading": -4.091508575199503e-18,
-            "angularVelocity": -1.3931950355515604e-17,
-            "velocityX": 0.4013918883749871,
-            "velocityY": -0.7780956537124009,
-            "timestamp": 0.10500281863811654
-        },
-        {
-            "x": 1.3968303106782045,
-            "y": 5.469219736980081,
-            "heading": -4.985304542720085e-18,
-            "angularVelocity": -2.5536326104216816e-17,
-            "velocityX": 0.5351891747117141,
-            "velocityY": -1.0374608526419855,
-            "timestamp": 0.1400037581841554
-        },
-        {
-            "x": 1.4202454651168877,
-            "y": 5.423829607215581,
-            "heading": -4.3424315250592746e-18,
-            "angularVelocity": 1.8367331153854942e-17,
-            "velocityX": 0.6689864541464801,
-            "velocityY": -1.2968260381921248,
-            "timestamp": 0.17500469773019423
-        },
-        {
-            "x": 1.4483436497338926,
-            "y": 5.369361452873384,
-            "heading": -2.0560193041515625e-18,
-            "angularVelocity": 6.532433512064817e-17,
-            "velocityX": 0.8027837247073224,
-            "velocityY": -1.5561912065401715,
-            "timestamp": 0.21000563727623306
-        },
-        {
-            "x": 1.4811248641150936,
-            "y": 5.305815274756268,
-            "heading": 2.1208651292681738e-19,
-            "angularVelocity": 6.480130833433249e-17,
-            "velocityX": 0.9365809834363137,
-            "velocityY": -1.815556351952184,
-            "timestamp": 0.2450065768222719
-        },
-        {
-            "x": 1.518589107680717,
-            "y": 5.233191073988126,
-            "heading": 3.0112738874394488e-18,
-            "angularVelocity": 7.99746695569e-17,
-            "velocityX": 1.0703782256008032,
-            "velocityY": -2.074921465253921,
-            "timestamp": 0.2800075163683107
-        },
-        {
-            "x": 1.5607363795611104,
-            "y": 5.151488852254779,
-            "heading": 5.4707987602321704e-18,
-            "angularVelocity": 7.027028770373186e-17,
-            "velocityX": 1.2041754429187383,
-            "velocityY": -2.334286530390624,
-            "timestamp": 0.31500845591434956
-        },
-        {
-            "x": 1.6075666783068712,
-            "y": 5.060708612365892,
-            "heading": 7.441304754324509e-18,
-            "angularVelocity": 5.629869559433992e-17,
-            "velocityX": 1.3379726188262824,
-            "velocityY": -2.5936515152532893,
-            "timestamp": 0.3500093954603884
-        },
-        {
-            "x": 1.6590800010192646,
-            "y": 4.9608503599406655,
-            "heading": 8.015191331788108e-18,
-            "angularVelocity": 1.639633984973558e-17,
-            "velocityX": 1.4717697119150586,
-            "velocityY": -2.853016339571725,
-            "timestamp": 0.38501033500642723
-        },
-        {
-            "x": 1.7152763390026586,
-            "y": 4.851914111835609,
-            "heading": 8.871725964928404e-18,
-            "angularVelocity": 2.4471795525975083e-17,
-            "velocityX": 1.6055665565638684,
-            "velocityY": -3.112380682288518,
-            "timestamp": 0.42001127455246606
-        },
-        {
-            "x": 1.775979033805623,
-            "y": 4.734242319347404,
-            "heading": 1.526334137039101e-17,
-            "angularVelocity": 1.8261268092684144e-16,
-            "velocityX": 1.7343161523752855,
-            "velocityY": -3.361960964887902,
-            "timestamp": 0.4550122140985049
-        },
-        {
-            "x": 1.8366817286107127,
-            "y": 4.616570526856247,
-            "heading": 2.6738432547349554e-17,
-            "angularVelocity": 3.2785094702569765e-16,
-            "velocityX": 1.734316152436003,
-            "velocityY": -3.3619609649722197,
-            "timestamp": 0.49001315364454373
-        },
-        {
-            "x": 1.8973844234157844,
-            "y": 4.498898734365081,
-            "heading": 1.9852147320466475e-17,
-            "angularVelocity": -1.967457250062152e-16,
-            "velocityX": 1.7343161524354893,
-            "velocityY": -3.3619609649724835,
-            "timestamp": 0.5250140931905826
-        },
-        {
-            "x": 1.9580871182208561,
-            "y": 4.381226941873916,
-            "heading": 2.777024505719557e-17,
-            "angularVelocity": 2.2622529107579697e-16,
-            "velocityX": 1.7343161524354895,
-            "velocityY": -3.361960964972483,
-            "timestamp": 0.5600150327366215
-        },
-        {
-            "x": 2.018789813025928,
-            "y": 4.26355514938275,
-            "heading": 1.7414652311267274e-17,
-            "angularVelocity": -2.9586613617347484e-16,
-            "velocityX": 1.7343161524354893,
-            "velocityY": -3.361960964972483,
-            "timestamp": 0.5950159722826603
-        },
-        {
-            "x": 2.0794925078309996,
-            "y": 4.145883356891583,
-            "heading": 6.771469247853252e-18,
-            "angularVelocity": -3.040827818183264e-16,
-            "velocityX": 1.7343161524354895,
-            "velocityY": -3.361960964972483,
-            "timestamp": 0.6300169118286992
-        },
-        {
-            "x": 2.140195202636071,
-            "y": 4.028211564400419,
-            "heading": 1.7156471914154247e-17,
-            "angularVelocity": 2.9670636674740876e-16,
-            "velocityX": 1.7343161524354882,
-            "velocityY": -3.361960964972482,
-            "timestamp": 0.6650178513747381
-        },
-        {
-            "x": 2.2008978974411426,
-            "y": 3.910539771909253,
-            "heading": 9.427453062324153e-18,
-            "angularVelocity": -2.208231822366884e-16,
-            "velocityX": 1.734316152435471,
-            "velocityY": -3.361960964972468,
-            "timestamp": 0.700018790920777
-        },
-        {
-            "x": 2.261600592246207,
-            "y": 3.7928679794180944,
-            "heading": -2.0403441270686145e-18,
-            "angularVelocity": -3.2764255297515643e-16,
-            "velocityX": 1.7343161524352513,
-            "velocityY": -3.3619609649722837,
-            "timestamp": 0.7350197304668159
-        },
-        {
-            "x": 2.322303287051213,
-            "y": 3.6751961869269927,
-            "heading": 3.357966916114392e-18,
-            "angularVelocity": 1.5423331811082687e-16,
-            "velocityX": 1.7343161524334347,
-            "velocityY": -3.3619609649707387,
-            "timestamp": 0.7700206700128548
-        },
-        {
-            "x": 2.383005981879539,
-            "y": 3.55752439444761,
-            "heading": 1.6083387472754582e-17,
-            "angularVelocity": 3.6357368464068287e-16,
-            "velocityX": 1.7343161531001825,
-            "velocityY": -3.361960964635663,
-            "timestamp": 0.8050216095588937
-        },
-        {
-            "x": 2.4437089810285184,
-            "y": 3.439852758955193,
-            "heading": 2.2946423291224054e-17,
-            "angularVelocity": 1.9608147402560587e-16,
-            "velocityX": 1.7343248477441577,
-            "velocityY": -3.3619564794134043,
-            "timestamp": 0.8400225491049326
-        },
-        {
-            "x": 2.507799181067132,
-            "y": 3.323991142238629,
-            "heading": 9.669545465348722e-18,
-            "angularVelocity": -3.793291836756151e-16,
-            "velocityX": 1.8310994181462568,
-            "velocityY": -3.3102430454713967,
-            "timestamp": 0.8750234886509715
-        },
-        {
-            "x": 2.5806292795037855,
-            "y": 3.213414270581727,
-            "heading": 3.38196516760506e-17,
-            "angularVelocity": 6.899845125281262e-16,
-            "velocityX": 2.0808040978720626,
-            "velocityY": -3.159254382625257,
-            "timestamp": 0.9100244281970103
-        },
-        {
-            "x": 2.661766753531331,
-            "y": 3.1087807297901673,
-            "heading": 5.146705636243318e-17,
-            "angularVelocity": 5.041980276892245e-16,
-            "velocityX": 2.3181513147947848,
-            "velocityY": -2.9894494876003863,
-            "timestamp": 0.9450253677430492
-        },
-        {
-            "x": 2.7507288455963135,
-            "y": 3.0107133388519287,
-            "heading": 6.422105835490463e-17,
-            "angularVelocity": 3.643902751722035e-16,
-            "velocityX": 2.5417058290140595,
-            "velocityY": -2.8018502420229328,
-            "timestamp": 0.9800263072890881
-        },
-        {
-            "x": 2.9215337593923705,
-            "y": 2.8658352980219064,
-            "heading": 5.1318206888106e-17,
-            "angularVelocity": -2.1793112103404205e-16,
-            "velocityX": 2.884920936848368,
-            "velocityY": -2.4470121145413257,
-            "timestamp": 1.0392324060123015
-        },
-        {
-            "x": 3.109750180151373,
-            "y": 2.744432888397421,
-            "heading": 6.648964720130561e-17,
-            "angularVelocity": 2.5624793121609224e-16,
-            "velocityX": 3.179003934013426,
-            "velocityY": -2.0505051378665997,
-            "timestamp": 1.098438504735515
-        },
-        {
-            "x": 3.309758578030313,
-            "y": 2.6436330516151556,
-            "heading": 6.809268296964168e-17,
-            "angularVelocity": 2.7075517621497836e-17,
-            "velocityX": 3.3781722186064402,
-            "velocityY": -1.702524553319066,
-            "timestamp": 1.1576446034587284
-        },
-        {
-            "x": 3.5097675494851996,
-            "y": 2.542834352942729,
-            "heading": 6.982732716570642e-17,
-            "angularVelocity": 2.929840393934814e-17,
-            "velocityX": 3.378181906393955,
-            "velocityY": -1.702505330465446,
-            "timestamp": 1.216850702181942
-        },
-        {
-            "x": 3.709776520948374,
-            "y": 2.4420356542872104,
-            "heading": 8.871000937094426e-17,
-            "angularVelocity": 3.189313704575333e-16,
-            "velocityX": 3.3781819065336705,
-            "velocityY": -1.702505330180368,
-            "timestamp": 1.2760568009051554
-        },
-        {
-            "x": 3.9097854924115545,
-            "y": 2.341236955631689,
-            "heading": 7.04137580064196e-17,
-            "angularVelocity": -3.090264644873572e-16,
-            "velocityX": 3.3781819065337895,
-            "velocityY": -1.7025053301803954,
-            "timestamp": 1.3352628996283689
-        },
-        {
-            "x": 4.109794463874735,
-            "y": 2.240438256976168,
-            "heading": 6.635408667627181e-17,
-            "angularVelocity": -6.856846534504647e-17,
-            "velocityX": 3.3781819065337926,
-            "velocityY": -1.7025053301803963,
-            "timestamp": 1.3944689983515823
-        },
-        {
-            "x": 4.3098034353379155,
-            "y": 2.1396395583206465,
-            "heading": 8.756379552984881e-17,
-            "angularVelocity": 3.5823520399025277e-16,
-            "velocityX": 3.3781819065337926,
-            "velocityY": -1.7025053301803963,
-            "timestamp": 1.4536750970747958
-        },
-        {
-            "x": 4.509812406801097,
-            "y": 2.0388408596651253,
-            "heading": 7.076319345079811e-17,
-            "angularVelocity": -2.837647208878057e-16,
-            "velocityX": 3.3781819065337926,
-            "velocityY": -1.7025053301803963,
-            "timestamp": 1.5128811957980093
-        },
-        {
-            "x": 4.709821378264278,
-            "y": 1.9380421610096037,
-            "heading": 6.652684249010322e-17,
-            "angularVelocity": -7.15526111686399e-17,
-            "velocityX": 3.378181906533802,
-            "velocityY": -1.7025053301803985,
-            "timestamp": 1.5720872945212228
-        },
-        {
-            "x": 4.9098303497278835,
-            "y": 1.837243462354927,
-            "heading": 7.855425123263822e-17,
-            "angularVelocity": 2.0314476045367986e-16,
-            "velocityX": 3.378181906540968,
-            "velocityY": -1.7025053301661324,
-            "timestamp": 1.6312933932444362
-        },
-        {
-            "x": 5.109839349236117,
-            "y": 1.7364448193478421,
-            "heading": 7.31685607290898e-17,
-            "angularVelocity": -9.096513061477316e-17,
-            "velocityX": 3.378182380218786,
-            "velocityY": -1.7025043902702668,
-            "timestamp": 1.6904994919676497
-        },
-        {
-            "x": 5.311619758605957,
-            "y": 1.6392407417297363,
-            "heading": 6.073866217998324e-17,
-            "angularVelocity": -2.0994287441931112e-16,
-            "velocityX": 3.4081017618384997,
-            "velocityY": -1.6417916348765775,
-            "timestamp": 1.7497055906908632
-        },
-        {
-            "x": 5.430192834880249,
-            "y": 1.5926107881863976,
-            "heading": 7.355026472308393e-17,
-            "angularVelocity": 3.8038299911394555e-16,
-            "velocityX": 3.5204949744296554,
-            "velocityY": -1.3844670499027307,
-            "timestamp": 1.78338638760593
-        },
-        {
-            "x": 5.550146685206584,
-            "y": 1.5496573158737061,
-            "heading": 8.480161924468705e-17,
-            "angularVelocity": 3.3405844137167443e-16,
-            "velocityX": 3.5614908586879754,
-            "velocityY": -1.2753104512640399,
-            "timestamp": 1.8170671845209967
-        },
-        {
-            "x": 5.670100671705041,
-            "y": 1.506704223843596,
-            "heading": 8.467696633181045e-17,
-            "angularVelocity": -3.701008417078351e-18,
-            "velocityX": 3.5614949017075612,
-            "velocityY": -1.2752991604806918,
-            "timestamp": 1.8507479814360634
-        },
-        {
-            "x": 5.790054658213967,
-            "y": 1.4637511318444054,
-            "heading": 8.225246246818014e-17,
-            "angularVelocity": -7.198475350046388e-17,
-            "velocityX": 3.561494902018399,
-            "velocityY": -1.275299159562779,
-            "timestamp": 1.8844287783511302
-        },
-        {
-            "x": 5.910008644722922,
-            "y": 1.4207980398452125,
-            "heading": 9.443730259968232e-17,
-            "angularVelocity": 3.617741041647218e-16,
-            "velocityX": 3.5614949020192235,
-            "velocityY": -1.2752991595628402,
-            "timestamp": 1.918109575266197
-        },
-        {
-            "x": 6.02996263123188,
-            "y": 1.3778449478460182,
-            "heading": 8.582959689044548e-17,
-            "angularVelocity": -2.555671628240573e-16,
-            "velocityX": 3.5614949020193487,
-            "velocityY": -1.2752991595628858,
-            "timestamp": 1.9517903721812637
-        },
-        {
-            "x": 6.149916617740839,
-            "y": 1.3348918558468235,
-            "heading": 7.701354643967334e-17,
-            "angularVelocity": -2.6175302422337863e-16,
-            "velocityX": 3.56149490201936,
-            "velocityY": -1.2752991595628895,
-            "timestamp": 1.9854711690963305
-        },
-        {
-            "x": 6.269870604249798,
-            "y": 1.291938763847629,
-            "heading": 8.367010625720842e-17,
-            "angularVelocity": 1.9763664839406868e-16,
-            "velocityX": 3.5614949020193607,
-            "velocityY": -1.27529915956289,
-            "timestamp": 2.019151966011397
-        },
-        {
-            "x": 6.3898245907587565,
-            "y": 1.2489856718484342,
-            "heading": 7.968637324735258e-17,
-            "angularVelocity": -1.1827906031741492e-16,
-            "velocityX": 3.5614949020193607,
-            "velocityY": -1.2752991595628902,
-            "timestamp": 2.0528327629264638
-        },
-        {
-            "x": 6.509778577267715,
-            "y": 1.2060325798492397,
-            "heading": 7.914041276645377e-17,
-            "angularVelocity": -1.6209844508000374e-17,
-            "velocityX": 3.561494902019361,
-            "velocityY": -1.27529915956289,
-            "timestamp": 2.0865135598415305
-        },
-        {
-            "x": 6.629732563776674,
-            "y": 1.1630794878500452,
-            "heading": 7.575912442206911e-17,
-            "angularVelocity": -1.0039217162560839e-16,
-            "velocityX": 3.561494902019361,
-            "velocityY": -1.27529915956289,
-            "timestamp": 2.1201943567565973
-        },
-        {
-            "x": 6.749686550285633,
-            "y": 1.1201263958508503,
-            "heading": 6.889954024340196e-17,
-            "angularVelocity": -2.0366454499176497e-16,
-            "velocityX": 3.5614949020193576,
-            "velocityY": -1.2752991595628989,
-            "timestamp": 2.153875153671664
-        },
-        {
-            "x": 6.869640536794461,
-            "y": 1.077173303851719,
-            "heading": 8.054846416919904e-17,
-            "angularVelocity": 3.458624792985843e-16,
-            "velocityX": 3.5614949020154905,
-            "velocityY": -1.275299159561009,
-            "timestamp": 2.187555950586731
-        },
-        {
-            "x": 6.985406128017608,
-            "y": 1.0357199913369377,
-            "heading": 6.97905590140722e-17,
-            "angularVelocity": -3.1940767538205783e-16,
-            "velocityX": 3.437139314579595,
-            "velocityY": -1.2307699434580792,
-            "timestamp": 2.2212367475017976
-        },
-        {
-            "x": 7.092266700241795,
-            "y": 0.9974553856875888,
-            "heading": 5.947830002893604e-17,
-            "angularVelocity": -3.0617621445548207e-16,
-            "velocityX": 3.172744768885924,
-            "velocityY": -1.1360956139440452,
-            "timestamp": 2.2549175444168643
-        },
-        {
-            "x": 7.190222235939473,
-            "y": 0.9623794931799317,
-            "heading": 5.044588900647289e-17,
-            "angularVelocity": -2.6817687744625345e-16,
-            "velocityX": 2.9083497027904013,
-            "velocityY": -1.0414210980846788,
-            "timestamp": 2.288598341331931
-        },
-        {
-            "x": 7.279272729268112,
-            "y": 0.930492315906057,
-            "heading": 4.27627289839913e-17,
-            "angularVelocity": -2.281169282784766e-16,
-            "velocityX": 2.6439544632272356,
-            "velocityY": -0.9467465201100714,
-            "timestamp": 2.322279138246998
-        },
-        {
-            "x": 7.35941817730645,
-            "y": 0.9017938549120106,
-            "heading": 3.626946964953948e-17,
-            "angularVelocity": -1.9278817213060433e-16,
-            "velocityX": 2.379559136930222,
-            "velocityY": -0.8520719110778341,
-            "timestamp": 2.3559599351620646
-        },
-        {
-            "x": 7.430658578301727,
-            "y": 0.8762841108254199,
-            "heading": 2.9442071838197796e-17,
-            "angularVelocity": -2.0270891340137855e-16,
-            "velocityX": 2.1151637585928986,
-            "velocityY": -0.7573972834110186,
-            "timestamp": 2.3896407320771313
-        },
-        {
-            "x": 7.492993931085436,
-            "y": 0.8539630840647031,
-            "heading": 2.3413209096667504e-17,
-            "angularVelocity": -1.790000012026981e-16,
-            "velocityX": 1.850768345562039,
-            "velocityY": -0.662722643321152,
-            "timestamp": 2.423321528992198
-        },
-        {
-            "x": 7.546424234822929,
-            "y": 0.8348307749287305,
-            "heading": 1.7731266545962487e-17,
-            "angularVelocity": -1.6869976443484408e-16,
-            "velocityX": 1.5863729077500839,
-            "velocityY": -0.5680479943576784,
-            "timestamp": 2.457002325907265
-        },
-        {
-            "x": 7.590949488888224,
-            "y": 0.8188871836416546,
-            "heading": 1.2363765500283435e-17,
-            "angularVelocity": -1.593638362048541e-16,
-            "velocityX": 1.321977451352309,
-            "velocityY": -0.4733733387389999,
-            "timestamp": 2.4906831228223316
-        },
-        {
-            "x": 7.626569692794441,
-            "y": 0.8061323103778162,
-            "heading": 7.88828312817125e-18,
-            "angularVelocity": -1.3287934794211324e-16,
-            "velocityX": 1.057581980498898,
-            "velocityY": -0.37869867794405154,
-            "timestamp": 2.5243639197373984
-        },
-        {
-            "x": 7.653284846152079,
-            "y": 0.7965661552766882,
-            "heading": 4.236479358871306e-18,
-            "angularVelocity": -1.0842391112822455e-16,
-            "velocityX": 0.7931864980809786,
-            "velocityY": -0.28402401300808766,
-            "timestamp": 2.558044716652465
-        },
-        {
-            "x": 7.671094948642455,
-            "y": 0.7901887184523844,
-            "heading": 1.249730071610374e-18,
-            "angularVelocity": -8.867810508445172e-17,
-            "velocityX": 0.5287910062011898,
-            "velocityY": -0.18934934468402032,
-            "timestamp": 2.591725513567532
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -2.906959217388984e-34,
-            "angularVelocity": -3.7105121777105e-17,
-            "velocityX": 0.26439550643651005,
-            "velocityY": -0.09467467353653367,
-            "timestamp": 2.6254063104825986
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -4.078160951605287e-34,
-            "angularVelocity": -3.477050502256718e-33,
-            "velocityX": 7.685636324015334e-33,
-            "velocityY": -2.3747808746440768e-32,
-            "timestamp": 2.6590871073976654
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Center To W1.traj b/src/main/deploy/choreo/Center To W1.traj
deleted file mode 100644
index 641af2c0..00000000
--- a/src/main/deploy/choreo/Center To W1.traj	
+++ /dev/null
@@ -1,112 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -1.3982338019676807e-32,
-            "velocityX": 1.9225496754980614e-32,
-            "velocityY": -8.567103847838605e-38,
-            "timestamp": 0
-        },
-        {
-            "x": 1.3816658535274804,
-            "y": 5.599900984360149,
-            "heading": 0.013082876743969601,
-            "angularVelocity": 0.16716651866961724,
-            "velocityX": 0.4046105912700521,
-            "velocityY": 0.509835013927756,
-            "timestamp": 0.07826254233257365
-        },
-        {
-            "x": 1.4449977124682947,
-            "y": 5.679702872674784,
-            "heading": 0.039246215534929935,
-            "angularVelocity": 0.3343021835373132,
-            "velocityX": 0.8092231232623148,
-            "velocityY": 1.0196690004717623,
-            "timestamp": 0.1565250846651473
-        },
-        {
-            "x": 1.5399958043003432,
-            "y": 5.799405527723277,
-            "heading": 0.07848745324353076,
-            "angularVelocity": 0.501405098007769,
-            "velocityX": 1.2138385618545031,
-            "velocityY": 1.5295012336785618,
-            "timestamp": 0.23478762699772096
-        },
-        {
-            "x": 1.6666604122853452,
-            "y": 5.959008727776544,
-            "heading": 0.1308059995871385,
-            "angularVelocity": 0.6685004701391144,
-            "velocityX": 1.6184576198246376,
-            "velocityY": 2.039330633740969,
-            "timestamp": 0.3130501693302946
-        },
-        {
-            "x": 1.8249918316073899,
-            "y": 6.158512135429991,
-            "heading": 0.19620604644996878,
-            "angularVelocity": 0.8356494040905814,
-            "velocityX": 2.023080449510844,
-            "velocityY": 2.5491557226145924,
-            "timestamp": 0.3913127116628683
-        },
-        {
-            "x": 1.9833278311922586,
-            "y": 6.358008426099132,
-            "heading": 0.26178707056088246,
-            "angularVelocity": 0.8379618417228061,
-            "velocityX": 2.023138973840461,
-            "velocityY": 2.549064785314409,
-            "timestamp": 0.469575253995442
-        },
-        {
-            "x": 2.109996681019138,
-            "y": 6.517605179473628,
-            "heading": 0.31426571552794874,
-            "angularVelocity": 0.6705461310477281,
-            "velocityX": 1.618511819979031,
-            "velocityY": 2.039248261272889,
-            "timestamp": 0.5478377963280157
-        },
-        {
-            "x": 2.204998332006287,
-            "y": 6.637302612224517,
-            "heading": 0.35363163522076196,
-            "angularVelocity": 0.5029982226430836,
-            "velocityX": 1.2138840389754155,
-            "velocityY": 1.5294345057465644,
-            "timestamp": 0.6261003386605893
-        },
-        {
-            "x": 2.268332773183591,
-            "y": 6.717100865272008,
-            "heading": 0.3798775217934225,
-            "angularVelocity": 0.3353569382033373,
-            "velocityX": 0.8092561178011136,
-            "velocityY": 1.0196225508288703,
-            "timestamp": 0.704362880993163
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.1676725265429579,
-            "velocityX": 0.40462813847574725,
-            "velocityY": 0.509811380244222,
-            "timestamp": 0.7826254233257367
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 9.092584352802723e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0.8608879656583104
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Center To W2.traj b/src/main/deploy/choreo/Center To W2.traj
deleted file mode 100644
index bb0e9680..00000000
--- a/src/main/deploy/choreo/Center To W2.traj	
+++ /dev/null
@@ -1,76 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 7.039472547405692e-42,
-            "angularVelocity": 1.0705191064262213e-40,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 1.4250000045295101,
-            "y": 5.56,
-            "heading": 8.194631828276944e-23,
-            "angularVelocity": 8.6403876700238e-22,
-            "velocityX": 0.7907971475606789,
-            "velocityY": 8.424071056371489e-35,
-            "timestamp": 0.0948410155004449
-        },
-        {
-            "x": 1.5750000090590195,
-            "y": 5.56,
-            "heading": 2.6954329855279996e-22,
-            "angularVelocity": 1.978015178210688e-21,
-            "velocityX": 1.5815942473623736,
-            "velocityY": 1.2488550168110521e-34,
-            "timestamp": 0.1896820310008898
-        },
-        {
-            "x": 1.7999999999999998,
-            "y": 5.56,
-            "heading": 5.87848829010012e-22,
-            "angularVelocity": 3.3562006983676933e-21,
-            "velocityX": 2.3723912038871497,
-            "velocityY": 2.135553914770072e-34,
-            "timestamp": 0.2845230465013347
-        },
-        {
-            "x": 2.0249999909409806,
-            "y": 5.56,
-            "heading": 4.475372520266276e-22,
-            "angularVelocity": -1.4794393741169351e-21,
-            "velocityX": 2.37239120388715,
-            "velocityY": 2.583104828416982e-34,
-            "timestamp": 0.3793640620017796
-        },
-        {
-            "x": 2.17499999547049,
-            "y": 5.56,
-            "heading": 1.6184867431561565e-22,
-            "angularVelocity": -3.012289189189882e-21,
-            "velocityX": 1.5815942473623736,
-            "velocityY": 1.9272829246954695e-34,
-            "timestamp": 0.47420507750222446
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 1.246024226890136e-41,
-            "angularVelocity": -1.7065260802739357e-21,
-            "velocityX": 0.7907971475606789,
-            "velocityY": -3.1542839688982834e-34,
-            "timestamp": 0.5690460930026694
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 7.131086050293936e-41,
-            "angularVelocity": 6.207915462550707e-40,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0.6638871085031143
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Center To W3.traj b/src/main/deploy/choreo/Center To W3.traj
deleted file mode 100644
index aceaac81..00000000
--- a/src/main/deploy/choreo/Center To W3.traj	
+++ /dev/null
@@ -1,112 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.35,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 5.56203910119815e-35,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 1.3816658774685813,
-            "y": 5.519999060819193,
-            "heading": -0.012983997100427386,
-            "angularVelocity": -0.16577951920330813,
-            "velocityX": 0.4043095436088268,
-            "velocityY": -0.5107315115509736,
-            "timestamp": 0.07832087559926142
-        },
-        {
-            "x": 1.4449977779940397,
-            "y": 5.439997258819368,
-            "heading": -0.038949622534069785,
-            "angularVelocity": -0.33152879401526647,
-            "velocityX": 0.8086209460872757,
-            "velocityY": -1.0214620481155445,
-            "timestamp": 0.15664175119852283
-        },
-        {
-            "x": 1.5399959199593471,
-            "y": 5.3199947242673264,
-            "heading": -0.07789432529768456,
-            "angularVelocity": -0.497245497648426,
-            "velocityX": 1.2129351368768808,
-            "velocityY": -1.5321909214351659,
-            "timestamp": 0.23496262679778424
-        },
-        {
-            "x": 1.6666605759505058,
-            "y": 5.159991667949077,
-            "heading": -0.1298174067167751,
-            "angularVelocity": -0.662953280613991,
-            "velocityX": 1.6172528080412967,
-            "velocityY": -2.042917103441561,
-            "timestamp": 0.31328350239704567
-        },
-        {
-            "x": 1.8249920311985446,
-            "y": 4.959988411604181,
-            "heading": -0.19472272142711608,
-            "angularVelocity": -0.828710279522869,
-            "velocityX": 2.0215741210320677,
-            "velocityY": -2.5536391774810774,
-            "timestamp": 0.3916043779963071
-        },
-        {
-            "x": 1.9833279596837488,
-            "y": 4.759991951944026,
-            "heading": -0.259798921868918,
-            "angularVelocity": -0.8308921464919682,
-            "velocityX": 2.021631235270526,
-            "velocityY": -2.553552397491847,
-            "timestamp": 0.4699252535955685
-        },
-        {
-            "x": 2.1099967561163506,
-            "y": 4.599995051908685,
-            "heading": -0.3118731002379449,
-            "angularVelocity": -0.6648824846579983,
-            "velocityX": 1.6173056731479674,
-            "velocityY": -2.042838500095239,
-            "timestamp": 0.54824612919483
-        },
-        {
-            "x": 2.2049983692603425,
-            "y": 4.4799975041635465,
-            "heading": -0.3509353765811358,
-            "angularVelocity": -0.4987466757018699,
-            "velocityX": 1.2129794568446812,
-            "velocityY": -1.532127249944463,
-            "timestamp": 0.6265670047940913
-        },
-        {
-            "x": 2.268332785802464,
-            "y": 4.399999173265512,
-            "heading": -0.3769787554180828,
-            "angularVelocity": -0.33252154853581545,
-            "velocityX": 0.808653070557838,
-            "velocityY": -1.021417729129522,
-            "timestamp": 0.7048878803933527
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.16625509460009477,
-            "velocityX": 0.40432661094808764,
-            "velocityY": -0.5107089643656818,
-            "timestamp": 0.7832087559926141
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -5.703180031220666e-32,
-            "velocityX": 1.0257416304055705e-32,
-            "velocityY": 0,
-            "timestamp": 0.8615296315918755
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S1 To C1.traj b/src/main/deploy/choreo/S1 To C1.traj
deleted file mode 100644
index 66955466..00000000
--- a/src/main/deploy/choreo/S1 To C1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.238209882810899,
-            "y": 6.650800869780916,
-            "heading": 0.2470077179050236,
-            "angularVelocity": -0.14636941117769697,
-            "velocityX": 0.7008486923098889,
-            "velocityY": 0.23433997503541223,
-            "timestamp": 0.08876364255722656
-        },
-        {
-            "x": 5.362652939631404,
-            "y": 6.692404642961414,
-            "heading": 0.22258646311132924,
-            "angularVelocity": -0.2751267759001645,
-            "velocityX": 1.401959780326283,
-            "velocityY": 0.4687028605622118,
-            "timestamp": 0.17752728511445312
-        },
-        {
-            "x": 5.549362223961953,
-            "y": 6.754812642679732,
-            "heading": 0.18937073322571307,
-            "angularVelocity": -0.37420422291202055,
-            "velocityX": 2.1034432449090223,
-            "velocityY": 0.7030806523975865,
-            "timestamp": 0.26629092767167967
-        },
-        {
-            "x": 5.798381050208957,
-            "y": 6.8380191875714225,
-            "heading": 0.15275465831989177,
-            "angularVelocity": -0.4125120809759026,
-            "velocityX": 2.8054146841303256,
-            "velocityY": 0.9373944386978721,
-            "timestamp": 0.35505457022890624
-        },
-        {
-            "x": 6.109622945204874,
-            "y": 6.941929047341865,
-            "heading": 0.1302624797347834,
-            "angularVelocity": -0.25339404667863696,
-            "velocityX": 3.506411927553799,
-            "velocityY": 1.1706353725121932,
-            "timestamp": 0.4438182127861328
-        },
-        {
-            "x": 6.4280450770381,
-            "y": 7.048515369175043,
-            "heading": 0.1302624686930749,
-            "angularVelocity": -1.2439475582812342e-7,
-            "velocityX": 3.587303570016191,
-            "velocityY": 1.2007880564873126,
-            "timestamp": 0.5325818553433593
-        },
-        {
-            "x": 6.746467208870004,
-            "y": 7.1551016910121685,
-            "heading": 0.13026245765136438,
-            "angularVelocity": -1.2439477842907252e-7,
-            "velocityX": 3.5873035700012967,
-            "velocityY": 1.2007880565318134,
-            "timestamp": 0.6213454979005859
-        },
-        {
-            "x": 7.057671454315644,
-            "y": 7.258999412253756,
-            "heading": 0.10797351007767278,
-            "angularVelocity": -0.2511044717394208,
-            "velocityX": 3.5059877724700192,
-            "velocityY": 1.1704986213770205,
-            "timestamp": 0.7101091404578125
-        },
-        {
-            "x": 7.3066745305511045,
-            "y": 7.342200909102172,
-            "heading": 0.07025614763700679,
-            "angularVelocity": -0.4249190474112408,
-            "velocityX": 2.8052372464872497,
-            "velocityY": 0.9373375680789704,
-            "timestamp": 0.7988727830150391
-        },
-        {
-            "x": 7.49336787770315,
-            "y": 7.4046033027166684,
-            "heading": 0.03585076588111199,
-            "angularVelocity": -0.3876066908033862,
-            "velocityX": 2.103263698666567,
-            "velocityY": 0.7030174947390144,
-            "timestamp": 0.8876364255722656
-        },
-        {
-            "x": 7.617797738081694,
-            "y": 7.446202129881464,
-            "heading": 0.010501179707511226,
-            "angularVelocity": -0.2855852401136722,
-            "velocityX": 1.4018111108759532,
-            "velocityY": 0.4686471393732002,
-            "timestamp": 0.9764000681294922
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -0.003,
-            "angularVelocity": -0.15210258748116404,
-            "velocityX": 0.7007628362953491,
-            "velocityY": 0.23430618121741137,
-            "timestamp": 1.0651637106867187
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -0.003,
-            "angularVelocity": -3.7910464085672074e-31,
-            "velocityX": 1.2889893239032156e-32,
-            "velocityY": 1.1630630989344696e-31,
-            "timestamp": 1.1539273532439451
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S1 To C2.traj b/src/main/deploy/choreo/S1 To C2.traj
deleted file mode 100644
index 63556f1f..00000000
--- a/src/main/deploy/choreo/S1 To C2.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.242063425541118,
-            "y": 6.625917379024557,
-            "heading": 0.21680591849920633,
-            "angularVelocity": -0.48090771817524164,
-            "velocityX": 0.735526954803639,
-            "velocityY": -0.04545446665971497,
-            "timestamp": 0.08981781715770647
-        },
-        {
-            "x": 5.374390185654174,
-            "y": 6.616899293663565,
-            "heading": 0.13592925245739512,
-            "angularVelocity": -0.9004523668149724,
-            "velocityX": 1.473279626476675,
-            "velocityY": -0.10040419202302918,
-            "timestamp": 0.17963563431541293
-        },
-        {
-            "x": 5.573225936717345,
-            "y": 6.601485332040172,
-            "heading": 0.027174297785337913,
-            "angularVelocity": -1.2108394315695736,
-            "velocityX": 2.2137673499017074,
-            "velocityY": -0.1716136298027423,
-            "timestamp": 0.2694534514731194
-        },
-        {
-            "x": 5.8386201791233034,
-            "y": 6.576536765329541,
-            "heading": -0.0884788299359431,
-            "angularVelocity": -1.287641265186969,
-            "velocityX": 2.9548061932964313,
-            "velocityY": -0.27776857087079165,
-            "timestamp": 0.35927126863082587
-        },
-        {
-            "x": 6.16462893780028,
-            "y": 6.53182752327052,
-            "heading": -0.12460373299787371,
-            "angularVelocity": -0.4022019706680331,
-            "velocityX": 3.6296669078981743,
-            "velocityY": -0.49777698316268804,
-            "timestamp": 0.44908908578853235
-        },
-        {
-            "x": 6.485747891959683,
-            "y": 6.420787077377064,
-            "heading": -0.12460373462910332,
-            "angularVelocity": -1.816153684215185e-8,
-            "velocityX": 3.5752255434527758,
-            "velocityY": -1.2362852873443355,
-            "timestamp": 0.5389069029462388
-        },
-        {
-            "x": 6.7868856142210525,
-            "y": 6.263423057279399,
-            "heading": -0.12460372221510443,
-            "angularVelocity": 1.382130993675135e-7,
-            "velocityX": 3.352761532075727,
-            "velocityY": -1.7520356770790513,
-            "timestamp": 0.6287247201039452
-        },
-        {
-            "x": 7.0844936481565455,
-            "y": 6.108042080528118,
-            "heading": -0.11377963349551397,
-            "angularVelocity": 0.12051159850149781,
-            "velocityX": 3.313463223148008,
-            "velocityY": -1.7299571696165401,
-            "timestamp": 0.7185425372616516
-        },
-        {
-            "x": 7.322771112714566,
-            "y": 5.983596379959823,
-            "heading": -0.07830444960996434,
-            "angularVelocity": 0.39496822577262786,
-            "velocityX": 2.652897521876331,
-            "velocityY": -1.3855346801602506,
-            "timestamp": 0.808360354419358
-        },
-        {
-            "x": 7.501421172621153,
-            "y": 5.8902827497271355,
-            "heading": -0.04357887494407857,
-            "angularVelocity": 0.3866223402525239,
-            "velocityX": 1.9890269610193678,
-            "velocityY": -1.038921153793384,
-            "timestamp": 0.8981781715770645
-        },
-        {
-            "x": 7.62048387761172,
-            "y": 5.828089668977501,
-            "heading": -0.01724918093308134,
-            "angularVelocity": 0.29314555668577735,
-            "velocityX": 1.3256022998366965,
-            "velocityY": -0.6924358965486016,
-            "timestamp": 0.9879959887347709
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -0.003,
-            "angularVelocity": 0.15864537108558247,
-            "velocityX": 0.6626315832612425,
-            "velocityY": -0.346141444552285,
-            "timestamp": 1.0778138058924773
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -0.003,
-            "angularVelocity": 0,
-            "velocityX": 4.386752331989148e-33,
-            "velocityY": -2.33652586669764e-33,
-            "timestamp": 1.1676316230501838
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S1 To C3.traj b/src/main/deploy/choreo/S1 To C3.traj
deleted file mode 100644
index b1b40339..00000000
--- a/src/main/deploy/choreo/S1 To C3.traj	
+++ /dev/null
@@ -1,157 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.246662494261999,
-            "y": 6.628692732737936,
-            "heading": 0.18398762968545185,
-            "angularVelocity": -0.8088463398355284,
-            "velocityX": 0.7519183997414132,
-            "velocityY": -0.013910608703967415,
-            "timestamp": 0.09397628025368156
-        },
-        {
-            "x": 5.3885394117424426,
-            "y": 6.6228569584156665,
-            "heading": 0.04133864379940616,
-            "angularVelocity": -1.5179254328962157,
-            "velocityX": 1.509709866122152,
-            "velocityY": -0.0620983753189278,
-            "timestamp": 0.18795256050736311
-        },
-        {
-            "x": 5.602094427902648,
-            "y": 6.606624752166298,
-            "heading": -0.14949344735311892,
-            "angularVelocity": -2.0306410366252936,
-            "velocityX": 2.272435295201409,
-            "velocityY": -0.172726630651381,
-            "timestamp": 0.2819288407610447
-        },
-        {
-            "x": 5.885007714232348,
-            "y": 6.565697735942791,
-            "heading": -0.3456589598447775,
-            "angularVelocity": -2.087393882393782,
-            "velocityX": 3.010475468554385,
-            "velocityY": -0.4355036836213049,
-            "timestamp": 0.37590512101472623
-        },
-        {
-            "x": 6.196762938930662,
-            "y": 6.464967292979978,
-            "heading": -0.4305176558630451,
-            "angularVelocity": -0.9029799412064233,
-            "velocityX": 3.3173820442430326,
-            "velocityY": -1.0718709305252299,
-            "timestamp": 0.4698814012684078
-        },
-        {
-            "x": 6.507653773495931,
-            "y": 6.294193106326001,
-            "heading": -0.4331106719684992,
-            "angularVelocity": -0.027592240280786797,
-            "velocityX": 3.3081840835372915,
-            "velocityY": -1.8172052159649787,
-            "timestamp": 0.5638576815220894
-        },
-        {
-            "x": 6.777249226823148,
-            "y": 6.0624534109685,
-            "heading": -0.433110646067504,
-            "angularVelocity": 2.7561204856502443e-7,
-            "velocityX": 2.8687606340606973,
-            "velocityY": -2.465938157287546,
-            "timestamp": 0.6578339617757709
-        },
-        {
-            "x": 6.99331744885121,
-            "y": 5.780142381909384,
-            "heading": -0.4331105937998759,
-            "angularVelocity": 5.561789419211182e-7,
-            "velocityX": 2.2991782761011916,
-            "velocityY": -3.004066859180208,
-            "timestamp": 0.7518102420294525
-        },
-        {
-            "x": 7.1465883664001515,
-            "y": 5.459372959434879,
-            "heading": -0.43311054984028735,
-            "angularVelocity": 4.677732342413875e-7,
-            "velocityX": 1.6309532270823943,
-            "velocityY": -3.4133019694822235,
-            "timestamp": 0.845786522283134
-        },
-        {
-            "x": 7.2797482024400875,
-            "y": 5.1297467242247805,
-            "heading": -0.4331105067287741,
-            "angularVelocity": 4.5874887908824285e-7,
-            "velocityX": 1.4169515507581365,
-            "velocityY": -3.507547163180951,
-            "timestamp": 0.9397628025368155
-        },
-        {
-            "x": 7.407925031698652,
-            "y": 4.799268903667217,
-            "heading": -0.4305589051420175,
-            "angularVelocity": 0.027151549091629798,
-            "velocityX": 1.3639274603395894,
-            "velocityY": -3.5166088683810885,
-            "timestamp": 1.033739082790497
-        },
-        {
-            "x": 7.517390810231398,
-            "y": 4.529437032753176,
-            "heading": -0.29559925755962413,
-            "angularVelocity": 1.4361033147734772,
-            "velocityX": 1.164823487770018,
-            "velocityY": -2.871276349581525,
-            "timestamp": 1.1277153630441787
-        },
-        {
-            "x": 7.59899827381295,
-            "y": 4.327770783855267,
-            "heading": -0.16069313013903275,
-            "angularVelocity": 1.4355338076419173,
-            "velocityX": 0.8683836321384404,
-            "velocityY": -2.1459271249460685,
-            "timestamp": 1.2216916432978604
-        },
-        {
-            "x": 7.653087401223331,
-            "y": 4.193799869992724,
-            "heading": -0.05829734753961687,
-            "angularVelocity": 1.0895917812772173,
-            "velocityX": 0.5755614849233384,
-            "velocityY": -1.4255822160751557,
-            "timestamp": 1.315667923551542
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -0.003,
-            "angularVelocity": 0.5884181347713066,
-            "velocityX": 0.28637650590148805,
-            "velocityY": -0.7108162805806236,
-            "timestamp": 1.4096442038052237
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -0.003,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.8051389340682807e-33,
-            "timestamp": 1.5036204840589054
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S1 To C4.traj b/src/main/deploy/choreo/S1 To C4.traj
deleted file mode 100644
index 3f479acb..00000000
--- a/src/main/deploy/choreo/S1 To C4.traj	
+++ /dev/null
@@ -1,247 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 0,
-            "velocityX": -1.7123315632321391e-31,
-            "velocityY": 4.571975133912053e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 5.2081944432473115,
-            "y": 6.63354239454729,
-            "heading": 0.2524699257138384,
-            "angularVelocity": -0.12069194203891866,
-            "velocityX": 0.5160121574790498,
-            "velocityY": 0.0567774581143745,
-            "timestamp": 0.062390861883169
-        },
-        {
-            "x": 5.2726441787772975,
-            "y": 6.640106281427696,
-            "heading": 0.23785051964847434,
-            "angularVelocity": -0.2343196683632906,
-            "velocityX": 1.0329996025807835,
-            "velocityY": 0.1052059016703473,
-            "timestamp": 0.124781723766338
-        },
-        {
-            "x": 5.369418980987113,
-            "y": 6.648966513737627,
-            "heading": 0.21675536507543205,
-            "angularVelocity": -0.33811288923278293,
-            "velocityX": 1.551105390898958,
-            "velocityY": 0.14201169918963874,
-            "timestamp": 0.187172585649507
-        },
-        {
-            "x": 5.498590594297912,
-            "y": 6.659046571836944,
-            "heading": 0.19009632617593986,
-            "angularVelocity": -0.42729076173705777,
-            "velocityX": 2.070361097955043,
-            "velocityY": 0.16156305258605722,
-            "timestamp": 0.249563447532676
-        },
-        {
-            "x": 5.6601948909824875,
-            "y": 6.668590441412665,
-            "heading": 0.15936442465931713,
-            "angularVelocity": -0.49257055583183496,
-            "velocityX": 2.5901917653772815,
-            "velocityY": 0.15296902924013772,
-            "timestamp": 0.311954309415845
-        },
-        {
-            "x": 5.854024302844021,
-            "y": 6.674275452040902,
-            "heading": 0.12740102584180835,
-            "angularVelocity": -0.512308980077189,
-            "velocityX": 3.106695532183737,
-            "velocityY": 0.09111928344386595,
-            "timestamp": 0.374345171299014
-        },
-        {
-            "x": 6.078018113514707,
-            "y": 6.668089747759068,
-            "heading": 0.10126004011963459,
-            "angularVelocity": -0.4189874115076864,
-            "velocityX": 3.590170161299073,
-            "velocityY": -0.09914439543117176,
-            "timestamp": 0.436736033182183
-        },
-        {
-            "x": 6.311203816285634,
-            "y": 6.63161682590639,
-            "heading": 0.10125992071184643,
-            "angularVelocity": -0.000001913866624365403,
-            "velocityX": 3.737497699704501,
-            "velocityY": -0.584587562213517,
-            "timestamp": 0.499126895065352
-        },
-        {
-            "x": 6.53718081427167,
-            "y": 6.563497401833266,
-            "heading": 0.10125989664992657,
-            "angularVelocity": -3.856641685769623e-7,
-            "velocityX": 3.621956664249834,
-            "velocityY": -1.0918173273625358,
-            "timestamp": 0.561517756948521
-        },
-        {
-            "x": 6.7516755548198235,
-            "y": 6.465019697743108,
-            "heading": 0.10125985817262735,
-            "angularVelocity": -6.167136938553595e-7,
-            "velocityX": 3.4379191771674527,
-            "velocityY": -1.5783994821960954,
-            "timestamp": 0.62390861883169
-        },
-        {
-            "x": 6.950631618499756,
-            "y": 6.338046073913574,
-            "heading": 0.10125971935519859,
-            "angularVelocity": -0.000002224964167076665,
-            "velocityX": 3.1888654471946203,
-            "velocityY": -2.035131748416977,
-            "timestamp": 0.686299480714859
-        },
-        {
-            "x": 7.196058677643184,
-            "y": 6.101503366378845,
-            "heading": 0.10125966337035743,
-            "angularVelocity": -6.213285755242982e-7,
-            "velocityX": 2.723788105663008,
-            "velocityY": -2.625188174087521,
-            "timestamp": 0.7764045291337008
-        },
-        {
-            "x": 7.389899318639429,
-            "y": 5.821123760827136,
-            "heading": 0.10125964077104323,
-            "angularVelocity": -2.508107435019506e-7,
-            "velocityX": 2.1512739230237647,
-            "velocityY": -3.1116969633976366,
-            "timestamp": 0.8665095775525427
-        },
-        {
-            "x": 7.524507781478929,
-            "y": 5.507966472295818,
-            "heading": 0.101259626680438,
-            "angularVelocity": -1.5637975290365853e-7,
-            "velocityX": 1.4939058931947202,
-            "velocityY": -3.475468844716046,
-            "timestamp": 0.9566146259713845
-        },
-        {
-            "x": 7.594574766806078,
-            "y": 5.174383579102459,
-            "heading": 0.10125961555168732,
-            "angularVelocity": -1.235086255399969e-7,
-            "velocityX": 0.7776144240159958,
-            "velocityY": -3.702155418004344,
-            "timestamp": 1.0467196743902263
-        },
-        {
-            "x": 7.605242469312774,
-            "y": 4.833688510035603,
-            "heading": 0.10125960514477245,
-            "angularVelocity": -1.1549757817967306e-7,
-            "velocityX": 0.11839184034432584,
-            "velocityY": -3.781087464524514,
-            "timestamp": 1.136824722809068
-        },
-        {
-            "x": 7.615908675824772,
-            "y": 4.492993394135096,
-            "heading": 0.10125959473787158,
-            "angularVelocity": -1.1549742264017462e-7,
-            "velocityX": 0.11837523756082047,
-            "velocityY": -3.781087984291716,
-            "timestamp": 1.2269297712279097
-        },
-        {
-            "x": 7.6265748461946865,
-            "y": 4.152298277103123,
-            "heading": 0.10125958433097336,
-            "angularVelocity": -1.1549739343149193e-7,
-            "velocityX": 0.11837483645025074,
-            "velocityY": -3.781087996848918,
-            "timestamp": 1.3170348196467514
-        },
-        {
-            "x": 7.637240986091898,
-            "y": 3.8116031591171406,
-            "heading": 0.10125957392407539,
-            "angularVelocity": -1.1549739036497336e-7,
-            "velocityX": 0.11837449825941278,
-            "velocityY": -3.78108800743665,
-            "timestamp": 1.407139868065593
-        },
-        {
-            "x": 7.647907101492937,
-            "y": 3.470908040364265,
-            "heading": 0.10125956351717343,
-            "angularVelocity": -1.1549743491773867e-7,
-            "velocityX": 0.11837422639702805,
-            "velocityY": -3.781088015947766,
-            "timestamp": 1.4972449164844348
-        },
-        {
-            "x": 7.658649980665503,
-            "y": 3.1330359890940187,
-            "heading": 0.09207038783752043,
-            "angularVelocity": -0.10198291706072676,
-            "velocityX": 0.11922616280763662,
-            "velocityY": -3.7497571689844853,
-            "timestamp": 1.5873499649032765
-        },
-        {
-            "x": 7.667206120655169,
-            "y": 2.862563871388905,
-            "heading": 0.061604548821908564,
-            "angularVelocity": -0.33811467337536444,
-            "velocityX": 0.09495738740298482,
-            "velocityY": -3.0017421049246735,
-            "timestamp": 1.6774550133221182
-        },
-        {
-            "x": 7.673608699461006,
-            "y": 2.659753859006467,
-            "heading": 0.031813701428875164,
-            "angularVelocity": -0.33062351017837394,
-            "velocityX": 0.07105682665061783,
-            "velocityY": -2.250817417462574,
-            "timestamp": 1.76756006174096
-        },
-        {
-            "x": 7.677870998153504,
-            "y": 2.524576540197495,
-            "heading": 0.009226184845951672,
-            "angularVelocity": -0.25067981183391225,
-            "velocityX": 0.04730366130750142,
-            "velocityY": -1.5002191462193974,
-            "timestamp": 1.8576651101598016
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -0.003,
-            "angularVelocity": -0.1356881224803119,
-            "velocityX": 0.023627997363707604,
-            "velocityY": -0.7499750722442785,
-            "timestamp": 1.9477701585786433
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -0.003,
-            "angularVelocity": 0,
-            "velocityX": -1.899719241029588e-32,
-            "velocityY": -4.254972095731821e-34,
-            "timestamp": 2.037875206997485
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S1 To C5.traj b/src/main/deploy/choreo/S1 To C5.traj
deleted file mode 100644
index 7f9f6ab6..00000000
--- a/src/main/deploy/choreo/S1 To C5.traj	
+++ /dev/null
@@ -1,292 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": -8.010592837331029e-32,
-            "velocityX": 9.396271853657066e-34,
-            "velocityY": -2.4298772375808502e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 5.207063492876026,
-            "y": 6.6289176408913075,
-            "heading": 0.24374543730705361,
-            "angularVelocity": -0.264834788594711,
-            "velocityX": 0.5061159579768882,
-            "velocityY": -0.01763482359684202,
-            "timestamp": 0.06137623678217253
-        },
-        {
-            "x": 5.269220913885746,
-            "y": 6.626580685647671,
-            "heading": 0.2121773997323976,
-            "angularVelocity": -0.514336479877237,
-            "velocityX": 1.0127277961065038,
-            "velocityY": -0.03807589657100854,
-            "timestamp": 0.12275247356434506
-        },
-        {
-            "x": 5.36250981468421,
-            "y": 6.6227475288112485,
-            "heading": 0.16662318019476377,
-            "angularVelocity": -0.7422126530713462,
-            "velocityX": 1.5199514615005028,
-            "velocityY": -0.06245343535846346,
-            "timestamp": 0.1841287103465176
-        },
-        {
-            "x": 5.486975524936631,
-            "y": 6.617054798693029,
-            "heading": 0.10910232843404971,
-            "angularVelocity": -0.9371844019185364,
-            "velocityX": 2.0279136808950478,
-            "velocityY": -0.0927513711605005,
-            "timestamp": 0.24550494712869012
-        },
-        {
-            "x": 5.642662315095453,
-            "y": 6.608892934358269,
-            "heading": 0.04300979987381644,
-            "angularVelocity": -1.076842309423355,
-            "velocityX": 2.536597196588689,
-            "velocityY": -0.1329808532205177,
-            "timestamp": 0.30688118391086266
-        },
-        {
-            "x": 5.829535978248522,
-            "y": 6.597025931717244,
-            "heading": -0.024847542112422006,
-            "angularVelocity": -1.1055963275666998,
-            "velocityX": 3.0447233807490215,
-            "velocityY": -0.19334848897856358,
-            "timestamp": 0.3682574206930352
-        },
-        {
-            "x": 6.046434546533161,
-            "y": 6.5777851666622515,
-            "heading": -0.07267029178585922,
-            "angularVelocity": -0.7791737027338695,
-            "velocityX": 3.533917679808606,
-            "velocityY": -0.31348883645767445,
-            "timestamp": 0.4296336574752077
-        },
-        {
-            "x": 6.274079716361941,
-            "y": 6.532107529947078,
-            "heading": -0.07267036448034916,
-            "angularVelocity": -0.0000011844077407082938,
-            "velocityX": 3.709011528952235,
-            "velocityY": -0.7442234830605138,
-            "timestamp": 0.49100989425738023
-        },
-        {
-            "x": 6.493476634626264,
-            "y": 6.456122214314626,
-            "heading": -0.07267035803800323,
-            "angularVelocity": 1.049648246960038e-7,
-            "velocityX": 3.5746231728571716,
-            "velocityY": -1.238025001469686,
-            "timestamp": 0.5523861310395528
-        },
-        {
-            "x": 6.700610073343003,
-            "y": 6.351219816335689,
-            "heading": -0.07267035070446132,
-            "angularVelocity": 1.1948503655702063e-7,
-            "velocityX": 3.3748149051866063,
-            "velocityY": -1.7091695985083282,
-            "timestamp": 0.6137623678217253
-        },
-        {
-            "x": 6.891689244621814,
-            "y": 6.21932014936447,
-            "heading": -0.07267034149477326,
-            "angularVelocity": 1.5005299351249212e-7,
-            "velocityX": 3.1132435173071933,
-            "velocityY": -2.1490347712151214,
-            "timestamp": 0.6751386046038979
-        },
-        {
-            "x": 7.0632171630859375,
-            "y": 6.06283712387085,
-            "heading": -0.07267032569899272,
-            "angularVelocity": 2.573598736854518e-7,
-            "velocityX": 2.7946959190881038,
-            "velocityY": -2.549570219643589,
-            "timestamp": 0.7365148413860704
-        },
-        {
-            "x": 7.263317671759933,
-            "y": 5.7907628781558955,
-            "heading": -0.07267031513885969,
-            "angularVelocity": 1.182832965996254e-7,
-            "velocityX": 2.24131152203967,
-            "velocityY": -3.0474842158680824,
-            "timestamp": 0.8257931530827405
-        },
-        {
-            "x": 7.406264443711533,
-            "y": 5.484771313458312,
-            "heading": -0.07267030869611313,
-            "angularVelocity": 7.216474459034548e-8,
-            "velocityX": 1.6011365944875051,
-            "velocityY": -3.4273896860551396,
-            "timestamp": 0.9150714647794106
-        },
-        {
-            "x": 7.486522259331217,
-            "y": 5.1567114044359705,
-            "heading": -0.07267030368970051,
-            "angularVelocity": 5.607647052205946e-8,
-            "velocityX": 0.8989620669840581,
-            "velocityY": -3.6745756364317224,
-            "timestamp": 1.0043497764760807
-        },
-        {
-            "x": 7.50149841248159,
-            "y": 4.81930906784098,
-            "heading": -0.07267029905812865,
-            "angularVelocity": 5.187790600157932e-8,
-            "velocityX": 0.1677468230047605,
-            "velocityY": -3.7792195011633143,
-            "timestamp": 1.093628088172751
-        },
-        {
-            "x": 7.516449925771596,
-            "y": 4.481905638472984,
-            "heading": -0.0726702944266145,
-            "angularVelocity": 5.1877259457701335e-8,
-            "velocityX": 0.16747083368699095,
-            "velocityY": -3.7792317412357557,
-            "timestamp": 1.1829063998694211
-        },
-        {
-            "x": 7.531401402222247,
-            "y": 4.144502207472513,
-            "heading": -0.07267028979509418,
-            "angularVelocity": 5.187732873980297e-8,
-            "velocityX": 0.16747042105201052,
-            "velocityY": -3.779231759520992,
-            "timestamp": 1.2721847115660914
-        },
-        {
-            "x": 7.546352847973806,
-            "y": 3.807098775111667,
-            "heading": -0.07267028516356967,
-            "angularVelocity": 5.1877375603275234e-8,
-            "velocityX": 0.16747007719365256,
-            "velocityY": -3.7792317747584647,
-            "timestamp": 1.3614630232627616
-        },
-        {
-            "x": 7.561304268557873,
-            "y": 3.4696953416355707,
-            "heading": -0.07267028053204275,
-            "angularVelocity": 5.1877402813012475e-8,
-            "velocityX": 0.16746979529438435,
-            "velocityY": -3.7792317872503007,
-            "timestamp": 1.4507413349594318
-        },
-        {
-            "x": 7.576255668582546,
-            "y": 3.132291907248425,
-            "heading": -0.07267027590051456,
-            "angularVelocity": 5.1877416880821484e-8,
-            "velocityX": 0.16746956501007867,
-            "velocityY": -3.779231797454896,
-            "timestamp": 1.540019646656102
-        },
-        {
-            "x": 7.591207051730301,
-            "y": 2.794888472113414,
-            "heading": -0.07267027126898577,
-            "angularVelocity": 5.1877423431260937e-8,
-            "velocityX": 0.16746937597291248,
-            "velocityY": -3.779231805831691,
-            "timestamp": 1.6292979583527722
-        },
-        {
-            "x": 7.60615842090353,
-            "y": 2.457485036359151,
-            "heading": -0.07267026663745696,
-            "angularVelocity": 5.1877423851232326e-8,
-            "velocityX": 0.16746921944524512,
-            "velocityY": -3.779231812767884,
-            "timestamp": 1.7185762700494425
-        },
-        {
-            "x": 7.621109778388665,
-            "y": 2.1200816000869556,
-            "heading": -0.07267026200592838,
-            "angularVelocity": 5.187742117887202e-8,
-            "velocityX": 0.1674690885277524,
-            "velocityY": -3.779231818569208,
-            "timestamp": 1.8078545817461127
-        },
-        {
-            "x": 7.636061125621916,
-            "y": 1.7826781633604734,
-            "heading": -0.07267025737439563,
-            "angularVelocity": 5.1877468094637746e-8,
-            "velocityX": 0.16746897369713376,
-            "velocityY": -3.7792318236576405,
-            "timestamp": 1.897132893442783
-        },
-        {
-            "x": 7.6506850878742485,
-            "y": 1.4507667606163608,
-            "heading": -0.05536549896524321,
-            "angularVelocity": 0.19382936438075055,
-            "velocityX": 0.16380195788222543,
-            "velocityY": -3.7177159428351065,
-            "timestamp": 1.9864112051394531
-        },
-        {
-            "x": 7.662404359721407,
-            "y": 1.1852476637582392,
-            "heading": -0.0353858682020255,
-            "angularVelocity": 0.2237904187872895,
-            "velocityX": 0.13126672788097452,
-            "velocityY": -2.974060461181691,
-            "timestamp": 2.0756895168361233
-        },
-        {
-            "x": 7.6711999602966845,
-            "y": 0.9861189087552604,
-            "heading": -0.018423007440779238,
-            "angularVelocity": 0.18999979321832794,
-            "velocityX": 0.0985188945458782,
-            "velocityY": -2.2304269784976873,
-            "timestamp": 2.1649678285327933
-        },
-        {
-            "x": 7.677066105120016,
-            "y": 0.8533716446291538,
-            "heading": -0.006322956528169105,
-            "angularVelocity": 0.13553180702747777,
-            "velocityX": 0.06570626966225444,
-            "velocityY": -1.4868926349898441,
-            "timestamp": 2.2542461402294633
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -2.444948224573009e-31,
-            "angularVelocity": 0.0708229849781762,
-            "velocityX": 0.03286234724008497,
-            "velocityY": -0.7434240563896022,
-            "timestamp": 2.3435244519261333
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 1.883272219480887e-30,
-            "velocityX": 0,
-            "velocityY": -3.281240907044904e-32,
-            "timestamp": 2.4328027636228033
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S1 To W1.traj b/src/main/deploy/choreo/S1 To W1.traj
deleted file mode 100644
index 462787d1..00000000
--- a/src/main/deploy/choreo/S1 To W1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": -5.95142511939464e-31,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.1029790507443495,
-            "y": 6.633224483607701,
-            "heading": 0.26650125878935643,
-            "angularVelocity": 0.06942713064122713,
-            "velocityX": -0.7797928290155524,
-            "velocityY": 0.034434353705267144,
-            "timestamp": 0.09364147314336706
-        },
-        {
-            "x": 4.9569371541735245,
-            "y": 6.639673450192742,
-            "heading": 0.2795035687162854,
-            "angularVelocity": 0.13885204376302582,
-            "velocityX": -1.559585637308702,
-            "velocityY": 0.06886870067889261,
-            "timestamp": 0.18728294628673411
-        },
-        {
-            "x": 4.737874314376445,
-            "y": 6.649346898931416,
-            "heading": 0.2990065710588269,
-            "angularVelocity": 0.20827312608258508,
-            "velocityX": -2.339378401936154,
-            "velocityY": 0.1033030388561254,
-            "timestamp": 0.28092441943010116
-        },
-        {
-            "x": 4.445790546320906,
-            "y": 6.66224482863892,
-            "heading": 0.3250097249050781,
-            "angularVelocity": 0.2776884319880837,
-            "velocityX": -3.119171006722006,
-            "velocityY": 0.13773736438080994,
-            "timestamp": 0.37456589257346823
-        },
-        {
-            "x": 4.09189529815297,
-            "y": 6.677872415392626,
-            "heading": 0.32500972789735083,
-            "angularVelocity": 3.195452790688951e-8,
-            "velocityX": -3.7792575905562438,
-            "velocityY": 0.16688745092443205,
-            "timestamp": 0.4682073657168353
-        },
-        {
-            "x": 3.7380000499890724,
-            "y": 6.693500002240241,
-            "heading": 0.32500973088947493,
-            "angularVelocity": 3.1952970747065184e-8,
-            "velocityX": -3.7792575905130557,
-            "velocityY": 0.16688745192729154,
-            "timestamp": 0.5618488388602023
-        },
-        {
-            "x": 3.384104801825175,
-            "y": 6.7091275890878554,
-            "heading": 0.3250097338815991,
-            "angularVelocity": 3.195297127919844e-8,
-            "velocityX": -3.7792575905130557,
-            "velocityY": 0.1668874519272919,
-            "timestamp": 0.6554903120035693
-        },
-        {
-            "x": 3.030209553657211,
-            "y": 6.724755175840924,
-            "heading": 0.32500973687387263,
-            "angularVelocity": 3.195453683514771e-8,
-            "velocityX": -3.7792575905565435,
-            "velocityY": 0.16688745091763021,
-            "timestamp": 0.7491317851469363
-        },
-        {
-            "x": 2.7381257502813003,
-            "y": 6.7376531044804615,
-            "heading": 0.35100602630019134,
-            "angularVelocity": 0.2776151266490911,
-            "velocityX": -3.119171383909283,
-            "velocityY": 0.13773735297596457,
-            "timestamp": 0.8427732582903034
-        },
-        {
-            "x": 2.5190628797280783,
-            "y": 6.747326551971013,
-            "heading": 0.37050304728737704,
-            "angularVelocity": 0.20820925101574836,
-            "velocityX": -2.3393787303818834,
-            "velocityY": 0.10330302552738789,
-            "timestamp": 0.9364147314336704
-        },
-        {
-            "x": 2.3730209608288053,
-            "y": 6.7537755172838185,
-            "heading": 0.3835010053218344,
-            "angularVelocity": 0.13880556977745576,
-            "velocityX": -1.559585875754842,
-            "velocityY": 0.06886868709264785,
-            "timestamp": 1.0300562045770374
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0.06940295213233785,
-            "velocityX": -0.7797929526056125,
-            "velocityY": 0.03443434418469861,
-            "timestamp": 1.1236976777204044
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": -1.6940949553652625e-30,
-            "velocityX": -1.6586267397188983e-32,
-            "velocityY": 0,
-            "timestamp": 1.2173391508637714
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S1 To W2.traj b/src/main/deploy/choreo/S1 To W2.traj
deleted file mode 100644
index 53d96783..00000000
--- a/src/main/deploy/choreo/S1 To W2.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 3.363577949177338e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 5.118924970919473,
-            "y": 6.609128397315466,
-            "heading": 0.25122660793754975,
-            "angularVelocity": -0.10272459100864365,
-            "velocityX": -0.6682716305586017,
-            "velocityY": -0.24437832416494054,
-            "timestamp": 0.08540693106008139
-        },
-        {
-            "x": 5.0047749070529814,
-            "y": 6.567385203608531,
-            "heading": 0.23368024533248913,
-            "angularVelocity": -0.20544424658833893,
-            "velocityX": -1.3365433279201937,
-            "velocityY": -0.48875651178205104,
-            "timestamp": 0.17081386212016278
-        },
-        {
-            "x": 4.833549798383878,
-            "y": 6.504770434941582,
-            "heading": 0.20736177398264571,
-            "angularVelocity": -0.30815381167752187,
-            "velocityX": -2.004815142563213,
-            "velocityY": -0.7331345113302418,
-            "timestamp": 0.25622079318024416
-        },
-        {
-            "x": 4.605249631158782,
-            "y": 6.421284110658292,
-            "heading": 0.17227269285622732,
-            "angularVelocity": -0.4108458258702004,
-            "velocityX": -2.6730871182397737,
-            "velocityY": -0.977512284390117,
-            "timestamp": 0.34162772424032556
-        },
-        {
-            "x": 4.319874394275373,
-            "y": 6.316926253811125,
-            "heading": 0.12841549400557886,
-            "angularVelocity": -0.5135086614902031,
-            "velocityX": -3.3413592239095435,
-            "velocityY": -1.2218897875367427,
-            "timestamp": 0.42703465530040696
-        },
-        {
-            "x": 4.016437653324315,
-            "y": 6.205962904892466,
-            "heading": 0.1284154887581796,
-            "angularVelocity": -6.143996998796484e-8,
-            "velocityX": -3.552835082407984,
-            "velocityY": -1.2992311928477889,
-            "timestamp": 0.5124415863604883
-        },
-        {
-            "x": 3.713000912388581,
-            "y": 6.094999555931864,
-            "heading": 0.12841548351080107,
-            "angularVelocity": -6.143972697353832e-8,
-            "velocityX": -3.5528350822285746,
-            "velocityY": -1.299231193338901,
-            "timestamp": 0.5978485174205697
-        },
-        {
-            "x": 3.4095641714528466,
-            "y": 5.98403620697126,
-            "heading": 0.12841547826342262,
-            "angularVelocity": -6.143972595662007e-8,
-            "velocityX": -3.5528350822285746,
-            "velocityY": -1.299231193338901,
-            "timestamp": 0.683255448480651
-        },
-        {
-            "x": 3.106127430499178,
-            "y": 5.873072858059743,
-            "heading": 0.12841547301602332,
-            "angularVelocity": -6.143997022352915e-8,
-            "velocityX": -3.55283508243856,
-            "velocityY": -1.2992311927641775,
-            "timestamp": 0.7686623795407324
-        },
-        {
-            "x": 2.820751661400481,
-            "y": 5.768715245814204,
-            "heading": 0.08460804498823243,
-            "angularVelocity": -0.5129259122655233,
-            "velocityX": -3.3413654554329186,
-            "velocityY": -1.2218869235814813,
-            "timestamp": 0.8540693106008137
-        },
-        {
-            "x": 2.5924510107246115,
-            "y": 5.685229149936981,
-            "heading": 0.04956396734437731,
-            "angularVelocity": -0.41031889577208425,
-            "velocityX": -2.6730927787964465,
-            "velocityY": -0.977509610063062,
-            "timestamp": 0.939476241660895
-        },
-        {
-            "x": 2.4212255084791887,
-            "y": 5.622614575599846,
-            "heading": 0.02328185696555881,
-            "angularVelocity": -0.30772807373595673,
-            "velocityX": -2.00481975081121,
-            "velocityY": -0.7331322359901695,
-            "timestamp": 1.0248831727209764
-        },
-        {
-            "x": 2.307075169405903,
-            "y": 5.580871525201533,
-            "heading": 0.005760683198102726,
-            "angularVelocity": -0.20514931926462068,
-            "velocityX": -1.3365465502206675,
-            "velocityY": -0.48875483383130996,
-            "timestamp": 1.1102901037810577
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": -0.10257578734376759,
-            "velocityX": -0.6682732735795421,
-            "velocityY": -0.24437741694350015,
-            "timestamp": 1.195697034841139
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": 2.4865352537032555e-34,
-            "velocityX": 0,
-            "velocityY": -6.568641934425214e-34,
-            "timestamp": 1.2811039659012204
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S1 To W3.traj b/src/main/deploy/choreo/S1 To W3.traj
deleted file mode 100644
index 359cb1db..00000000
--- a/src/main/deploy/choreo/S1 To W3.traj	
+++ /dev/null
@@ -1,175 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 2.521971410376041e-31,
-            "velocityX": -3.46085485178236e-32,
-            "velocityY": -2.8734015412732535e-37,
-            "timestamp": 0
-        },
-        {
-            "x": 5.132269964046874,
-            "y": 6.606195610268081,
-            "heading": 0.16914444930535336,
-            "angularVelocity": -1.1148212857556328,
-            "velocityX": -0.5365789380469493,
-            "velocityY": -0.29208606589986635,
-            "timestamp": 0.08149786145594107
-        },
-        {
-            "x": 5.043666795437562,
-            "y": 6.5570487224925476,
-            "heading": 0.005335724612659364,
-            "angularVelocity": -2.0099757437347154,
-            "velocityX": -1.0871839705537814,
-            "velocityY": -0.6030451216453527,
-            "timestamp": 0.16299572291188213
-        },
-        {
-            "x": 4.909391168326261,
-            "y": 6.4806746643853055,
-            "heading": -0.2131663273161798,
-            "angularVelocity": -2.681077123072297,
-            "velocityX": -1.647596939508567,
-            "velocityY": -0.9371295975481627,
-            "timestamp": 0.2444935843678232
-        },
-        {
-            "x": 4.728031239930722,
-            "y": 6.375889843725219,
-            "heading": -0.4550458803770597,
-            "angularVelocity": -2.967925154596154,
-            "velocityX": -2.225333587355345,
-            "velocityY": -1.2857370584715861,
-            "timestamp": 0.32599144582376427
-        },
-        {
-            "x": 4.50844214888531,
-            "y": 6.243100931440797,
-            "heading": -0.5880935326141025,
-            "angularVelocity": -1.6325293677671568,
-            "velocityX": -2.6944153763362864,
-            "velocityY": -1.6293545611158133,
-            "timestamp": 0.40748930727970534
-        },
-        {
-            "x": 4.251218650398233,
-            "y": 6.083818287155582,
-            "heading": -0.6031902279140122,
-            "angularVelocity": -0.1852403858237583,
-            "velocityX": -3.15619936390767,
-            "velocityY": -1.9544395575499292,
-            "timestamp": 0.4889871687356464
-        },
-        {
-            "x": 3.9855391766757995,
-            "y": 5.92740773635831,
-            "heading": -0.6031903133870022,
-            "angularVelocity": -0.0000010487758630839914,
-            "velocityX": -3.259956383837921,
-            "velocityY": -1.9191982219291623,
-            "timestamp": 0.5704850301915875
-        },
-        {
-            "x": 3.719859695364488,
-            "y": 5.770997198451498,
-            "heading": -0.603190398859952,
-            "angularVelocity": -0.0000010487753700599598,
-            "velocityX": -3.259956476955436,
-            "velocityY": -1.919198063759863,
-            "timestamp": 0.6519828916475285
-        },
-        {
-            "x": 3.4541802321431443,
-            "y": 5.614586629816999,
-            "heading": -0.6031904843329001,
-            "angularVelocity": -0.000001048775349610677,
-            "velocityX": -3.259956254986808,
-            "velocityY": -1.9191984407966034,
-            "timestamp": 0.7334807531034696
-        },
-        {
-            "x": 3.1885009063173535,
-            "y": 5.458175827802239,
-            "heading": -0.6031905698058837,
-            "angularVelocity": -0.000001048775783641969,
-            "velocityX": -3.259954569107567,
-            "velocityY": -1.9192013044332228,
-            "timestamp": 0.8149786145594107
-        },
-        {
-            "x": 2.937386380849553,
-            "y": 5.279314604480798,
-            "heading": -0.603190661977047,
-            "angularVelocity": -0.0000011309641953921016,
-            "velocityX": -3.0812406728433848,
-            "velocityY": -2.1946738248847923,
-            "timestamp": 0.8964764760153517
-        },
-        {
-            "x": 2.722322578778117,
-            "y": 5.058413395221267,
-            "heading": -0.6031910253459357,
-            "angularVelocity": -0.00000445863096683889,
-            "velocityX": -2.6388889012468426,
-            "velocityY": -2.710515408787179,
-            "timestamp": 0.9779743374712928
-        },
-        {
-            "x": 2.561151389441887,
-            "y": 4.8410288515668665,
-            "heading": -0.5595876383179096,
-            "angularVelocity": 0.5350249227287813,
-            "velocityX": -1.9776124975176395,
-            "velocityY": -2.667365005300435,
-            "timestamp": 1.0594721989272338
-        },
-        {
-            "x": 2.4444023928247787,
-            "y": 4.656624455699537,
-            "heading": -0.5072145719014195,
-            "angularVelocity": 0.6426311743750899,
-            "velocityX": -1.432540615562337,
-            "velocityY": -2.262689996681954,
-            "timestamp": 1.1409700603831747
-        },
-        {
-            "x": 2.367465069003143,
-            "y": 4.510717874734717,
-            "heading": -0.4552059179368463,
-            "angularVelocity": 0.6381597386170672,
-            "velocityX": -0.9440410146618312,
-            "velocityY": -1.7903117745451358,
-            "timestamp": 1.2224679218391157
-        },
-        {
-            "x": 2.321242179529733,
-            "y": 4.410775659264178,
-            "heading": -0.4150276653385325,
-            "angularVelocity": 0.49299763061923907,
-            "velocityX": -0.5671669004271778,
-            "velocityY": -1.2263170307182782,
-            "timestamp": 1.3039657832950566
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.393,
-            "angularVelocity": 0.27028519454392347,
-            "velocityX": -0.26064707895700284,
-            "velocityY": -0.6230305723006956,
-            "timestamp": 1.3854636447509976
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.393,
-            "angularVelocity": -9.678618240747916e-31,
-            "velocityX": 0,
-            "velocityY": -3.108003047332484e-31,
-            "timestamp": 1.4669615062069385
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S2 To C1.traj b/src/main/deploy/choreo/S2 To C1.traj
deleted file mode 100644
index 29c2d4bc..00000000
--- a/src/main/deploy/choreo/S2 To C1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 5.380258045140596e-28,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.304265282414897,
-            "y": 5.638698322322049,
-            "heading": -0.04261698283734242,
-            "angularVelocity": -0.41098530784979936,
-            "velocityX": 0.3979499173257931,
-            "velocityY": 0.7589428456312429,
-            "timestamp": 0.10369466261532896
-        },
-        {
-            "x": 4.38866009880019,
-            "y": 5.795292929312062,
-            "heading": -0.1228057028496872,
-            "angularVelocity": -0.7733157907057984,
-            "velocityX": 0.8138781134605525,
-            "velocityY": 1.510151082422871,
-            "timestamp": 0.20738932523065792
-        },
-        {
-            "x": 4.519821720835651,
-            "y": 6.028001091828693,
-            "heading": -0.23019023538729733,
-            "angularVelocity": -1.0355839908170448,
-            "velocityX": 1.264883058851596,
-            "velocityY": 2.2441672179396264,
-            "timestamp": 0.3110839878459869
-        },
-        {
-            "x": 4.707676475524912,
-            "y": 6.330157785456564,
-            "heading": -0.33387571773962255,
-            "angularVelocity": -0.9999114683169592,
-            "velocityX": 1.8116145031122473,
-            "velocityY": 2.9139078715050806,
-            "timestamp": 0.41477865046131585
-        },
-        {
-            "x": 4.978690211282069,
-            "y": 6.613755936139097,
-            "heading": -0.3338757185080882,
-            "angularVelocity": -7.410850442524386e-9,
-            "velocityX": 2.6135745941189206,
-            "velocityY": 2.7349348898948014,
-            "timestamp": 0.5184733130766448
-        },
-        {
-            "x": 5.307019127730767,
-            "y": 6.828410259978494,
-            "heading": -0.3338757060420003,
-            "angularVelocity": 1.2021918642580205e-7,
-            "velocityX": 3.166304881734213,
-            "velocityY": 2.070061451819255,
-            "timestamp": 0.6221679756919738
-        },
-        {
-            "x": 5.675511606586513,
-            "y": 6.962908031631864,
-            "heading": -0.3338756959696621,
-            "angularVelocity": 9.713458668099172e-8,
-            "velocityX": 3.5536301441350284,
-            "velocityY": 1.2970558779125847,
-            "timestamp": 0.7258626383073028
-        },
-        {
-            "x": 6.055844888125957,
-            "y": 7.058944236891981,
-            "heading": -0.3338756861984713,
-            "angularVelocity": 9.423041207795053e-8,
-            "velocityX": 3.6678192680981745,
-            "velocityY": 0.9261441508940896,
-            "timestamp": 0.8295573009226318
-        },
-        {
-            "x": 6.436176952400414,
-            "y": 7.154985262770466,
-            "heading": -0.3338756764272739,
-            "angularVelocity": 9.423047569354886e-8,
-            "velocityX": 3.667807529162388,
-            "velocityY": 0.9261906394812687,
-            "timestamp": 0.9332519635379608
-        },
-        {
-            "x": 6.8165090646020285,
-            "y": 7.2510260988513195,
-            "heading": -0.33387566665551793,
-            "angularVelocity": 9.423586238606553e-8,
-            "velocityX": 3.6678079913574164,
-            "velocityY": 0.9261888091302356,
-            "timestamp": 1.0369466261532898
-        },
-        {
-            "x": 7.162467192455402,
-            "y": 7.33732437680055,
-            "heading": -0.24283297058924916,
-            "angularVelocity": 0.8779882567727307,
-            "velocityX": 3.336315670718354,
-            "velocityY": 0.8322345217454595,
-            "timestamp": 1.1406412887686188
-        },
-        {
-            "x": 7.421550289934561,
-            "y": 7.4021983509049765,
-            "heading": -0.1315877257451376,
-            "angularVelocity": 1.0728155339758783,
-            "velocityX": 2.4985191228237835,
-            "velocityY": 0.6256250077700213,
-            "timestamp": 1.2443359513839478
-        },
-        {
-            "x": 7.593930847137365,
-            "y": 7.445412549605325,
-            "heading": -0.046073342510862675,
-            "angularVelocity": 0.8246748779298502,
-            "velocityX": 1.662386017323544,
-            "velocityY": 0.41674467721309977,
-            "timestamp": 1.3480306139992768
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0.44431739637147044,
-            "velocityX": 0.8300249086292989,
-            "velocityY": 0.20818285001567757,
-            "timestamp": 1.4517252766146058
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 6.677691966309128e-30,
-            "velocityY": 0,
-            "timestamp": 1.5554199392299348
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S2 To C2.traj b/src/main/deploy/choreo/S2 To C2.traj
deleted file mode 100644
index 05a18e04..00000000
--- a/src/main/deploy/choreo/S2 To C2.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.272659932573731,
-            "y": 5.605321030989923,
-            "heading": -5.269773313916661e-8,
-            "angularVelocity": -7.069117507058631e-7,
-            "velocityX": 0.1295790656325301,
-            "velocityY": 0.6079397351586845,
-            "timestamp": 0.07454855863477708
-        },
-        {
-            "x": 4.294668340488188,
-            "y": 5.695305527660525,
-            "heading": -1.390931476532318e-7,
-            "angularVelocity": -0.0000011589489187991593,
-            "velocityX": 0.2952224471421504,
-            "velocityY": 1.2070588395619357,
-            "timestamp": 0.14909711726955416
-        },
-        {
-            "x": 4.333068049168991,
-            "y": 5.8286332482919585,
-            "heading": -2.3393980900082764e-7,
-            "angularVelocity": -0.0000012723197345504802,
-            "velocityX": 0.5150965944736963,
-            "velocityY": 1.7884681217524945,
-            "timestamp": 0.22364567590433124
-        },
-        {
-            "x": 4.394361213548298,
-            "y": 6.0022499148295365,
-            "heading": -3.0377830712505715e-7,
-            "angularVelocity": -9.368513338342214e-7,
-            "velocityX": 0.8221911395208825,
-            "velocityY": 2.3289070872699873,
-            "timestamp": 0.2981942345391083
-        },
-        {
-            "x": 4.489214393007107,
-            "y": 6.207820209268236,
-            "heading": -3.104589480689307e-7,
-            "angularVelocity": -8.962358439949817e-8,
-            "velocityX": 1.2723677183517785,
-            "velocityY": 2.75753546703125,
-            "timestamp": 0.37274279317388537
-        },
-        {
-            "x": 4.629403012529928,
-            "y": 6.42298249437822,
-            "heading": -2.4199647648725644e-7,
-            "angularVelocity": 9.18393943226279e-7,
-            "velocityX": 1.880500737963715,
-            "velocityY": 2.8862031546566858,
-            "timestamp": 0.4472913518086624
-        },
-        {
-            "x": 4.811531801184116,
-            "y": 6.618438666310206,
-            "heading": -1.472447710538824e-7,
-            "angularVelocity": 0.0000012710752897138218,
-            "velocityX": 2.443089336474201,
-            "velocityY": 2.6218638627509074,
-            "timestamp": 0.5218399104434395
-        },
-        {
-            "x": 5.024847227590037,
-            "y": 6.779620899070013,
-            "heading": -6.254572131926062e-8,
-            "angularVelocity": 0.000001136235548213432,
-            "velocityX": 2.861429252247402,
-            "velocityY": 2.1621106520395927,
-            "timestamp": 0.5963884690782165
-        },
-        {
-            "x": 5.261847496032715,
-            "y": 6.900974273681641,
-            "heading": 1.6289151195668303e-9,
-            "angularVelocity": 8.609140934997053e-7,
-            "velocityX": 3.1791395133448552,
-            "velocityY": 1.6278433384167708,
-            "timestamp": 0.6709370277129936
-        },
-        {
-            "x": 5.5631639462020495,
-            "y": 6.984947323321856,
-            "heading": 4.965855378058324e-8,
-            "angularVelocity": 5.539886921154719e-7,
-            "velocityX": 3.4757171947403775,
-            "velocityY": 0.9686380297960585,
-            "timestamp": 0.7576289054784
-        },
-        {
-            "x": 5.883696640724643,
-            "y": 7.009274337947639,
-            "heading": 7.188189606013307e-8,
-            "angularVelocity": 2.563216327096045e-7,
-            "velocityX": 3.697378610459799,
-            "velocityY": 0.2806146929817695,
-            "timestamp": 0.8443207832438064
-        },
-        {
-            "x": 6.209423466668327,
-            "y": 6.971151903870608,
-            "heading": 7.193737396357795e-8,
-            "angularVelocity": 6.438452889791163e-10,
-            "velocityX": 3.7572934667332714,
-            "velocityY": -0.4397463179458525,
-            "timestamp": 0.9310126610092129
-        },
-        {
-            "x": 6.521952745942683,
-            "y": 6.871769894938554,
-            "heading": 7.193784305655878e-8,
-            "angularVelocity": 5.630908705768223e-12,
-            "velocityX": 3.6050583669434406,
-            "velocityY": -1.1463820079014435,
-            "timestamp": 1.0177045387746193
-        },
-        {
-            "x": 6.809873374118548,
-            "y": 6.714756988695251,
-            "heading": 7.193831945924844e-8,
-            "angularVelocity": 5.91084758847972e-12,
-            "velocityX": 3.321194967875227,
-            "velocityY": -1.8111605177424126,
-            "timestamp": 1.1043964165400257
-        },
-        {
-            "x": 7.062672913559571,
-            "y": 6.505846154651799,
-            "heading": 7.193893641048392e-8,
-            "angularVelocity": 7.48396872358976e-12,
-            "velocityX": 2.9160694861627596,
-            "velocityY": -2.4098086166543093,
-            "timestamp": 1.1910882943054322
-        },
-        {
-            "x": 7.268448732261103,
-            "y": 6.269564240975314,
-            "heading": 6.958123303548702e-8,
-            "angularVelocity": -2.7156604096657526e-8,
-            "velocityX": 2.3736458816624144,
-            "velocityY": -2.7255369226670942,
-            "timestamp": 1.2777801720708386
-        },
-        {
-            "x": 7.433069281193563,
-            "y": 6.080538590855167,
-            "heading": 5.4913746267249155e-8,
-            "angularVelocity": -1.6915879682512025e-7,
-            "velocityX": 1.8989154825033356,
-            "velocityY": -2.180430912690726,
-            "timestamp": 1.364472049836245
-        },
-        {
-            "x": 7.556534655605292,
-            "y": 5.9387693118426395,
-            "heading": 3.4027865091828515e-8,
-            "angularVelocity": -2.408962130800618e-7,
-            "velocityX": 1.424186181835385,
-            "velocityY": -1.635323662267605,
-            "timestamp": 1.4511639276016515
-        },
-        {
-            "x": 7.638844889158882,
-            "y": 5.8442564415645775,
-            "heading": 1.3493455787871544e-8,
-            "angularVelocity": -2.368497714278514e-7,
-            "velocityX": 0.9494572693998051,
-            "velocityY": -1.0902159778725726,
-            "timestamp": 1.537855805367058
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -2.563219583366834e-29,
-            "angularVelocity": -1.5563981959470815e-7,
-            "velocityX": 0.4747285662416774,
-            "velocityY": -0.5451080630467725,
-            "timestamp": 1.6245476831324643
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 4.328374921762355e-28,
-            "velocityX": 0,
-            "velocityY": 6.636862452110489e-29,
-            "timestamp": 1.7112395608978708
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S2 To C3.traj b/src/main/deploy/choreo/S2 To C3.traj
deleted file mode 100644
index 2b663e69..00000000
--- a/src/main/deploy/choreo/S2 To C3.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -9.522541691773037e-34,
-            "velocityX": 0,
-            "velocityY": -4.2308696063129985e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 4.271262994735721,
-            "y": 5.464085078742271,
-            "heading": 0.013066573413208545,
-            "angularVelocity": 0.12156077000957308,
-            "velocityX": 0.0768721814736735,
-            "velocityY": -0.8923144052220247,
-            "timestamp": 0.10749005137249076
-        },
-        {
-            "x": 4.301424317332477,
-            "y": 5.27437671657762,
-            "heading": 0.033784324267673776,
-            "angularVelocity": 0.192741101059399,
-            "velocityX": 0.2805964106597774,
-            "velocityY": -1.7648922829820315,
-            "timestamp": 0.21498010274498153
-        },
-        {
-            "x": 4.379013311524821,
-            "y": 5.000817118183451,
-            "heading": 0.051063906614390905,
-            "angularVelocity": 0.16075517804747627,
-            "velocityX": 0.7218248870629933,
-            "velocityY": -2.544975975927198,
-            "timestamp": 0.32247015411747226
-        },
-        {
-            "x": 4.5426840546799285,
-            "y": 4.684552935799795,
-            "heading": 0.043724640930798246,
-            "angularVelocity": -0.06827855778168279,
-            "velocityX": 1.5226594560638123,
-            "velocityY": -2.9422646872470954,
-            "timestamp": 0.42996020548996305
-        },
-        {
-            "x": 4.7987371937718075,
-            "y": 4.3933637164950845,
-            "heading": -0.0025480008668042047,
-            "angularVelocity": -0.4304830187237655,
-            "velocityX": 2.382110119238521,
-            "velocityY": -2.7089876280330105,
-            "timestamp": 0.5374502568624538
-        },
-        {
-            "x": 5.129552682758233,
-            "y": 4.161939062753506,
-            "heading": -0.00976861092561694,
-            "angularVelocity": -0.06717468236935505,
-            "velocityX": 3.0776382070935524,
-            "velocityY": -2.152986725623672,
-            "timestamp": 0.6449403082349446
-        },
-        {
-            "x": 5.508331065778466,
-            "y": 4.0140416634822165,
-            "heading": -0.009768639154040644,
-            "angularVelocity": -2.6261429169311795e-7,
-            "velocityX": 3.5238459576843355,
-            "velocityY": -1.3759170954228361,
-            "timestamp": 0.7524303596074354
-        },
-        {
-            "x": 5.911272135069668,
-            "y": 3.959404581326429,
-            "heading": -0.009768652411608816,
-            "angularVelocity": -1.233376298753571e-7,
-            "velocityX": 3.7486359355701584,
-            "velocityY": -0.5082989677477323,
-            "timestamp": 0.8599204109799262
-        },
-        {
-            "x": 6.316087697151271,
-            "y": 3.997759236961094,
-            "heading": -0.009768652034230711,
-            "angularVelocity": 3.51081891151765e-9,
-            "velocityX": 3.7660746916826353,
-            "velocityY": 0.3568205163634492,
-            "timestamp": 0.967410462352417
-        },
-        {
-            "x": 6.720902827559142,
-            "y": 4.036118448432054,
-            "heading": -0.009768651656829377,
-            "angularVelocity": 3.5110350109919825e-9,
-            "velocityX": 3.766070675741356,
-            "velocityY": 0.35686290015837574,
-            "timestamp": 1.0749005137249077
-        },
-        {
-            "x": 7.104541677751467,
-            "y": 4.072471064803872,
-            "heading": -0.005861206189315246,
-            "angularVelocity": 0.03635169411142489,
-            "velocityX": 3.569063790497984,
-            "velocityY": 0.3381951716242375,
-            "timestamp": 1.1823905650973985
-        },
-        {
-            "x": 7.392270834070994,
-            "y": 4.099735530737146,
-            "heading": -0.0029306066828158664,
-            "angularVelocity": 0.027263913907193366,
-            "velocityX": 2.6767980166131413,
-            "velocityY": 0.2536464127158502,
-            "timestamp": 1.2898806164698893
-        },
-        {
-            "x": 7.584090276967534,
-            "y": 4.117911843086064,
-            "heading": -0.0009768696053991616,
-            "angularVelocity": 0.018175980497453854,
-            "velocityX": 1.7845320608492279,
-            "velocityY": 0.16909762454137037,
-            "timestamp": 1.39737066784238
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -6.809535330810639e-32,
-            "angularVelocity": 0.009088000172350516,
-            "velocityX": 0.8922660451626774,
-            "velocityY": 0.08454881915017429,
-            "timestamp": 1.5048607192148709
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -9.489213882041181e-31,
-            "velocityX": 2.198140684032685e-33,
-            "velocityY": -6.756921472052438e-32,
-            "timestamp": 1.6123507705873616
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S2 To C4.traj b/src/main/deploy/choreo/S2 To C4.traj
deleted file mode 100644
index c13783bc..00000000
--- a/src/main/deploy/choreo/S2 To C4.traj	
+++ /dev/null
@@ -1,166 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 7.441191049306378e-32,
-            "velocityX": -6.15829104129807e-34,
-            "velocityY": 2.4252394047543234e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 4.262142393092874,
-            "y": 5.471176724349191,
-            "heading": -0.06580713819401311,
-            "angularVelocity": -0.6311237133024392,
-            "velocityX": -0.008224883662067025,
-            "velocityY": -0.8518601035521746,
-            "timestamp": 0.10426979181255681
-        },
-        {
-            "x": 4.27936690849282,
-            "y": 5.294863623901166,
-            "heading": -0.18680392528158013,
-            "angularVelocity": -1.1604203382805245,
-            "velocityX": 0.16519180771848577,
-            "velocityY": -1.6909317395107013,
-            "timestamp": 0.20853958362511363
-        },
-        {
-            "x": 4.348116425232891,
-            "y": 5.0445566747334585,
-            "heading": -0.3412830267774945,
-            "angularVelocity": -1.4815326549574157,
-            "velocityX": 0.6593426105967491,
-            "velocityY": -2.400570144205118,
-            "timestamp": 0.31280937543767046
-        },
-        {
-            "x": 4.502267029635194,
-            "y": 4.763921050939977,
-            "heading": -0.4878370803946758,
-            "angularVelocity": -1.4055274405902505,
-            "velocityX": 1.4783822018117698,
-            "velocityY": -2.6914374615609895,
-            "timestamp": 0.41707916725022726
-        },
-        {
-            "x": 4.740029978369904,
-            "y": 4.496898841850153,
-            "heading": -0.5207249369577782,
-            "angularVelocity": -0.31541116551017917,
-            "velocityX": 2.280266840487519,
-            "velocityY": -2.5608779345205184,
-            "timestamp": 0.5213489590627841
-        },
-        {
-            "x": 5.0582868126043,
-            "y": 4.2697391199027495,
-            "heading": -0.509978725166827,
-            "angularVelocity": 0.10306160206274768,
-            "velocityX": 3.0522438829312937,
-            "velocityY": -2.1785765368723764,
-            "timestamp": 0.6256187508753409
-        },
-        {
-            "x": 5.423286105863135,
-            "y": 4.1201949004052105,
-            "heading": -0.5099785687835147,
-            "angularVelocity": 0.0000014997949991595825,
-            "velocityX": 3.5005276879710774,
-            "velocityY": -1.4342046425715584,
-            "timestamp": 0.7298885426878977
-        },
-        {
-            "x": 5.805099761015269,
-            "y": 4.021168099470067,
-            "heading": -0.5099784851647622,
-            "angularVelocity": 8.019460959727046e-7,
-            "velocityX": 3.6617859162748747,
-            "velocityY": -0.9497170677501874,
-            "timestamp": 0.8341583345004545
-        },
-        {
-            "x": 6.154222032422552,
-            "y": 3.8375880828272497,
-            "heading": -0.5099784796190555,
-            "angularVelocity": 5.318612994948929e-8,
-            "velocityX": 3.348259024386382,
-            "velocityY": -1.7606251384181746,
-            "timestamp": 0.9384281263130113
-        },
-        {
-            "x": 6.493379431179238,
-            "y": 3.636192348012897,
-            "heading": -0.5099784741544249,
-            "angularVelocity": 5.240856867670856e-8,
-            "velocityX": 3.2526908595576858,
-            "velocityY": -1.9314868795019449,
-            "timestamp": 1.0426979181255682
-        },
-        {
-            "x": 6.795285003697281,
-            "y": 3.3823400148372036,
-            "heading": -0.5099784394461939,
-            "angularVelocity": 3.32869476219209e-7,
-            "velocityX": 2.895427019368865,
-            "velocityY": -2.4345721686299853,
-            "timestamp": 1.146967709938125
-        },
-        {
-            "x": 7.066681771760518,
-            "y": 3.0961027469536373,
-            "heading": -0.509978390292543,
-            "angularVelocity": 4.714083530965781e-7,
-            "velocityX": 2.6028321659174227,
-            "velocityY": -2.7451600593787298,
-            "timestamp": 1.2512375017506818
-        },
-        {
-            "x": 7.312943361341716,
-            "y": 2.8410544331524363,
-            "heading": -0.40533862017886096,
-            "angularVelocity": 1.0035482788897323,
-            "velocityX": 2.361773101301441,
-            "velocityY": -2.446042227260748,
-            "timestamp": 1.3555072935632386
-        },
-        {
-            "x": 7.49731350893092,
-            "y": 2.6484381177603322,
-            "heading": -0.22178416093118458,
-            "angularVelocity": 1.7603800300824186,
-            "velocityX": 1.768202893515326,
-            "velocityY": -1.8472878102448496,
-            "timestamp": 1.4597770853757954
-        },
-        {
-            "x": 7.619307085639959,
-            "y": 2.520663921373434,
-            "heading": -0.0776901780416505,
-            "angularVelocity": 1.3819341190262298,
-            "velocityX": 1.1699800545142,
-            "velocityY": -1.225419118670484,
-            "timestamp": 1.5640468771883522
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 1.429761379305419e-34,
-            "angularVelocity": 0.7450880709660586,
-            "velocityX": 0.5820757220763062,
-            "velocityY": -0.6105691808408042,
-            "timestamp": 1.668316669000909
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -5.61453765178046e-32,
-            "velocityX": 0,
-            "velocityY": 2.318867528557263e-33,
-            "timestamp": 1.7725864608134658
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S2 To C5.traj b/src/main/deploy/choreo/S2 To C5.traj
deleted file mode 100644
index 004abebc..00000000
--- a/src/main/deploy/choreo/S2 To C5.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -1.2613927548149136e-32,
-            "velocityX": 4.983015637334932e-33,
-            "velocityY": -3.6905945238681026e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 4.258006222511338,
-            "y": 5.473607841813564,
-            "heading": -0.06530003091818118,
-            "angularVelocity": -0.6342708718414432,
-            "velocityX": -0.04850545332028736,
-            "velocityY": -0.8391424739420397,
-            "timestamp": 0.10295290831912149
-        },
-        {
-            "x": 4.271377859079598,
-            "y": 5.302453682559796,
-            "heading": -0.1916286288786279,
-            "angularVelocity": -1.2270522515873779,
-            "velocityX": 0.12988109599401854,
-            "velocityY": -1.662450940416803,
-            "timestamp": 0.20590581663824298
-        },
-        {
-            "x": 4.340766271943036,
-            "y": 5.0642424556348775,
-            "heading": -0.3653686640748466,
-            "angularVelocity": -1.6875680156375907,
-            "velocityX": 0.673982056420939,
-            "velocityY": -2.3137882242873524,
-            "timestamp": 0.30885872495736444
-        },
-        {
-            "x": 4.494848034080926,
-            "y": 4.801255485663876,
-            "heading": -0.5565822720790902,
-            "angularVelocity": -1.8572919514963226,
-            "velocityX": 1.4966236957608356,
-            "velocityY": -2.5544394448365244,
-            "timestamp": 0.41181163327648596
-        },
-        {
-            "x": 4.73233691115851,
-            "y": 4.546498868370588,
-            "heading": -0.6490495118957932,
-            "angularVelocity": -0.8981508276588281,
-            "velocityX": 2.3067719111095206,
-            "velocityY": -2.4744965582091383,
-            "timestamp": 0.5147645415956075
-        },
-        {
-            "x": 5.049349589860016,
-            "y": 4.3202563768587865,
-            "heading": -0.6490493910522949,
-            "angularVelocity": 0.0000011737744981731646,
-            "velocityX": 3.079200810130272,
-            "velocityY": -2.1975337579636096,
-            "timestamp": 0.617717449914729
-        },
-        {
-            "x": 5.4092082222704905,
-            "y": 4.171311637049865,
-            "heading": -0.6490493274377749,
-            "angularVelocity": 6.178992034513071e-7,
-            "velocityX": 3.4953712166637025,
-            "velocityY": -1.446726879703489,
-            "timestamp": 0.7206703582338505
-        },
-        {
-            "x": 5.789554256648214,
-            "y": 4.087528164867666,
-            "heading": -0.6490492895940018,
-            "angularVelocity": 3.675833320529158e-7,
-            "velocityX": 3.6943690138288297,
-            "velocityY": -0.8138038405141147,
-            "timestamp": 0.823623266552972
-        },
-        {
-            "x": 6.141218018459777,
-            "y": 3.9201502365347105,
-            "heading": -0.6490493503243893,
-            "angularVelocity": -5.898851085689342e-7,
-            "velocityX": 3.4157729738097102,
-            "velocityY": -1.6257717345306835,
-            "timestamp": 0.9265761748720935
-        },
-        {
-            "x": 6.456138339829479,
-            "y": 3.69100407104571,
-            "heading": -0.6490494074378677,
-            "angularVelocity": -5.547534235510974e-7,
-            "velocityX": 3.0588773693847333,
-            "velocityY": -2.2257376622981755,
-            "timestamp": 1.029529083191215
-        },
-        {
-            "x": 6.721775326533507,
-            "y": 3.406188756476768,
-            "heading": -0.6490494689481024,
-            "angularVelocity": -5.974599048336077e-7,
-            "velocityX": 2.580179531020508,
-            "velocityY": -2.7664620574496457,
-            "timestamp": 1.1324819915103363
-        },
-        {
-            "x": 6.916359430113237,
-            "y": 3.0688168532177102,
-            "heading": -0.6490494077682868,
-            "angularVelocity": 5.942504852606566e-7,
-            "velocityX": 1.8900301774533879,
-            "velocityY": -3.276953597204961,
-            "timestamp": 1.2354348998294578
-        },
-        {
-            "x": 7.040594979350531,
-            "y": 2.699698573462511,
-            "heading": -0.6490493528706274,
-            "angularVelocity": 5.332307776860117e-7,
-            "velocityX": 1.206722095234099,
-            "velocityY": -3.585311826364818,
-            "timestamp": 1.3383878081485792
-        },
-        {
-            "x": 7.164812386541997,
-            "y": 2.330574188065911,
-            "heading": -0.649049297974002,
-            "angularVelocity": 5.332207349098274e-7,
-            "velocityX": 1.2065458783003051,
-            "velocityY": -3.585371131551044,
-            "timestamp": 1.4413407164677006
-        },
-        {
-            "x": 7.289029734328084,
-            "y": 1.9614497826782673,
-            "heading": -0.6490492430773671,
-            "angularVelocity": 5.332208261000816e-7,
-            "velocityX": 1.206545301285243,
-            "velocityY": -3.5853713257276327,
-            "timestamp": 1.544293624786822
-        },
-        {
-            "x": 7.413246520819429,
-            "y": 1.5923251884092744,
-            "heading": -0.6490491881699392,
-            "angularVelocity": 5.333256623024379e-7,
-            "velocityX": 1.2065398493290926,
-            "velocityY": -3.585373160365936,
-            "timestamp": 1.6472465331059434
-        },
-        {
-            "x": 7.520074458284658,
-            "y": 1.2704783284902474,
-            "heading": -0.5256216946651037,
-            "angularVelocity": 1.1988733054752465,
-            "velocityX": 1.037638850707321,
-            "velocityY": -3.1261560763432104,
-            "timestamp": 1.7501994414250648
-        },
-        {
-            "x": 7.601104601009551,
-            "y": 1.0271731610238788,
-            "heading": -0.2913266412644364,
-            "angularVelocity": 2.275749730881093,
-            "velocityX": 0.7870602593733868,
-            "velocityY": -2.3632665792422265,
-            "timestamp": 1.8531523497441862
-        },
-        {
-            "x": 7.654009357663848,
-            "y": 0.8666565360101991,
-            "heading": -0.10310334462657153,
-            "angularVelocity": 1.828246522715339,
-            "velocityX": 0.5138733574219042,
-            "velocityY": -1.5591266690216163,
-            "timestamp": 1.9561052580633076
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -1.429149737287701e-32,
-            "angularVelocity": 1.0014612147428026,
-            "velocityX": 0.2524517545010862,
-            "velocityY": -0.7737181718391956,
-            "timestamp": 2.059058166382429
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": -8.032741852653566e-32,
-            "velocityX": 0,
-            "velocityY": 2.733638748586157e-32,
-            "timestamp": 2.1620110747015504
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S2 To W1.traj b/src/main/deploy/choreo/S2 To W1.traj
deleted file mode 100644
index c913a793..00000000
--- a/src/main/deploy/choreo/S2 To W1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 8.330960047205791e-35,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.215224456008129,
-            "y": 5.5891314798928855,
-            "heading": 0.013109691961439362,
-            "angularVelocity": 0.15985781811927008,
-            "velocityX": -0.5825685488618567,
-            "velocityY": 0.35522534228568337,
-            "timestamp": 0.08200845048227925
-        },
-        {
-            "x": 4.11967337131145,
-            "y": 5.647394543004508,
-            "heading": 0.03932481805987993,
-            "angularVelocity": 0.31966371690079975,
-            "velocityX": -1.1651370576417237,
-            "velocityY": 0.7104519445128596,
-            "timestamp": 0.1640169009645585
-        },
-        {
-            "x": 3.976346757700282,
-            "y": 5.734789362523958,
-            "heading": 0.07863903763442207,
-            "angularVelocity": 0.47939229851730114,
-            "velocityX": -1.7477054226519961,
-            "velocityY": 1.0656806585844962,
-            "timestamp": 0.24602535144683776
-        },
-        {
-            "x": 3.7852446629452063,
-            "y": 5.851316183985278,
-            "heading": 0.13104540832787342,
-            "angularVelocity": 0.6390362259652201,
-            "velocityX": -2.3302732051542674,
-            "velocityY": 1.4209123666652002,
-            "timestamp": 0.328033801929117
-        },
-        {
-            "x": 3.5463672091086966,
-            "y": 5.996975313640393,
-            "heading": 0.19653856458379848,
-            "angularVelocity": 0.7986147265406159,
-            "velocityX": -2.912839499243156,
-            "velocityY": 1.77614780914058,
-            "timestamp": 0.41004225241139625
-        },
-        {
-            "x": 3.281494446824011,
-            "y": 6.15848998396632,
-            "heading": 0.19653857286419524,
-            "angularVelocity": 1.009700426817188e-7,
-            "velocityX": -3.229822789322431,
-            "velocityY": 1.9694881366015824,
-            "timestamp": 0.4920507028936755
-        },
-        {
-            "x": 3.016621684855716,
-            "y": 6.320004654811106,
-            "heading": 0.1965385811445921,
-            "angularVelocity": 1.0097004416983622e-7,
-            "velocityX": -3.229822785464406,
-            "velocityY": 1.969488142928474,
-            "timestamp": 0.5740591533759547
-        },
-        {
-            "x": 2.77774755563319,
-            "y": 6.465669559960414,
-            "heading": 0.2620174097526663,
-            "angularVelocity": 0.7984400171324189,
-            "velocityX": -2.9127989593480157,
-            "velocityY": 1.7762182347389206,
-            "timestamp": 0.6560676038582339
-        },
-        {
-            "x": 2.5866484397575076,
-            "y": 6.582201656937504,
-            "heading": 0.3144069758542471,
-            "angularVelocity": 0.6388313130352502,
-            "velocityX": -2.330236881100143,
-            "velocityY": 1.4209766955939638,
-            "timestamp": 0.7380760543405132
-        },
-        {
-            "x": 2.443324199843316,
-            "y": 6.6696008076986475,
-            "heading": 0.35370275100271265,
-            "angularVelocity": 0.4791673896698825,
-            "velocityX": -1.7476764781107736,
-            "velocityY": 1.0657334731623573,
-            "timestamp": 0.8200845048227924
-        },
-        {
-            "x": 2.347774735979553,
-            "y": 6.727866934294423,
-            "heading": 0.37990105353306003,
-            "angularVelocity": 0.3194585725773273,
-            "velocityX": -1.1651172934234333,
-            "velocityY": 0.7104893002260324,
-            "timestamp": 0.9020929553050716
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.15972678900658524,
-            "velocityX": -0.5825586960684825,
-            "velocityY": 0.3552446794720561,
-            "timestamp": 0.9841014057873508
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": -4.59054414969708e-34,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 1.06610985626963
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S2 To W2.traj b/src/main/deploy/choreo/S2 To W2.traj
deleted file mode 100644
index a4697851..00000000
--- a/src/main/deploy/choreo/S2 To W2.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 4.181096344253034,
-            "y": 5.56,
-            "heading": 2.5710705899848466e-19,
-            "angularVelocity": 2.5941602968880693e-18,
-            "velocityX": -0.8263919813283321,
-            "velocityY": -1.4479549544654444e-34,
-            "timestamp": 0.099109935233537
-        },
-        {
-            "x": 4.017289035836238,
-            "y": 5.56,
-            "heading": 7.7182898862159e-19,
-            "angularVelocity": 5.193444314378615e-18,
-            "velocityX": -1.6527839316089838,
-            "velocityY": -7.36495654370163e-34,
-            "timestamp": 0.198219870467074
-        },
-        {
-            "x": 3.771578080903875,
-            "y": 5.56,
-            "heading": 1.545129672215716e-18,
-            "angularVelocity": 7.802453727507411e-18,
-            "velocityX": -2.479175819794286,
-            "velocityY": -8.54847043233614e-34,
-            "timestamp": 0.297329805700611
-        },
-        {
-            "x": 3.4439634979187304,
-            "y": 5.56,
-            "heading": 2.5806642382479214e-18,
-            "angularVelocity": 1.0448342676399075e-17,
-            "velocityX": -3.3055675216936846,
-            "velocityY": -5.0910568375180236e-34,
-            "timestamp": 0.396439740934148
-        },
-        {
-            "x": 3.0690365020812695,
-            "y": 5.56,
-            "heading": -2.5850068638158606e-18,
-            "angularVelocity": -5.212061828001078e-17,
-            "velocityX": -3.7829405796099502,
-            "velocityY": -4.536187828596351e-34,
-            "timestamp": 0.495549676167685
-        },
-        {
-            "x": 2.741421919096125,
-            "y": 5.56,
-            "heading": -1.5478395588580842e-18,
-            "angularVelocity": 1.0464816695997814e-17,
-            "velocityX": -3.305567521693684,
-            "velocityY": -2.872506388563931e-33,
-            "timestamp": 0.594659611401222
-        },
-        {
-            "x": 2.4957109641637625,
-            "y": 5.56,
-            "heading": -7.728706253244835e-19,
-            "angularVelocity": 7.819286045545264e-18,
-            "velocityX": -2.479175819794286,
-            "velocityY": -1.6595590840772702e-33,
-            "timestamp": 0.6937695466347591
-        },
-        {
-            "x": 2.3319036557469652,
-            "y": 5.56,
-            "heading": -2.5738443375571516e-19,
-            "angularVelocity": 5.201155568994596e-18,
-            "velocityX": -1.6527839316089838,
-            "velocityY": -1.4347514653062086e-33,
-            "timestamp": 0.792879481868296
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 2.5969589542999405e-18,
-            "velocityX": -0.8263919813283321,
-            "velocityY": 2.9921012122849815e-33,
-            "timestamp": 0.891989417101833
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 1.808288986442481e-43,
-            "timestamp": 0.9910993523353699
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S2 To W3.traj b/src/main/deploy/choreo/S2 To W3.traj
deleted file mode 100644
index 4a59e123..00000000
--- a/src/main/deploy/choreo/S2 To W3.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 9.014129666558106e-32,
-            "angularVelocity": 9.039417129994225e-31,
-            "velocityX": 0,
-            "velocityY": -2.779554313855136e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 4.206523478699805,
-            "y": 5.55440930401173,
-            "heading": -0.10760119525493232,
-            "angularVelocity": -1.2344158099898592,
-            "velocityX": -0.6479064718660169,
-            "velocityY": -0.06413723844253004,
-            "timestamp": 0.08716770668695249
-        },
-        {
-            "x": 4.091196989898444,
-            "y": 5.541197271810027,
-            "heading": -0.295913717265277,
-            "angularVelocity": -2.1603473254910437,
-            "velocityX": -1.3230414471673093,
-            "velocityY": -0.15157026270235532,
-            "timestamp": 0.17433541337390498
-        },
-        {
-            "x": 3.9151950960136452,
-            "y": 5.516370757100726,
-            "heading": -0.5340653762567812,
-            "angularVelocity": -2.7321088054637612,
-            "velocityX": -2.019118095155108,
-            "velocityY": -0.2848132141236808,
-            "timestamp": 0.26150312006085746
-        },
-        {
-            "x": 3.678758800597895,
-            "y": 5.473177706772957,
-            "heading": -0.7546654655667131,
-            "angularVelocity": -2.530754767957573,
-            "velocityX": -2.712429917020411,
-            "velocityY": -0.4955166536948123,
-            "timestamp": 0.34867082674780997
-        },
-        {
-            "x": 3.3968844521889907,
-            "y": 5.398848640381046,
-            "heading": -0.8527884173152881,
-            "angularVelocity": -1.1256800881657552,
-            "velocityX": -3.233701552126478,
-            "velocityY": -0.8527133409492049,
-            "timestamp": 0.4358385334347625
-        },
-        {
-            "x": 3.098615494079836,
-            "y": 5.268356714792353,
-            "heading": -0.864385170870599,
-            "angularVelocity": -0.1330395624260059,
-            "velocityX": -3.421782784538955,
-            "velocityY": -1.4970214377364781,
-            "timestamp": 0.5230062401217149
-        },
-        {
-            "x": 2.832096469446971,
-            "y": 5.0836195976119996,
-            "heading": -0.8474300397750907,
-            "angularVelocity": 0.19451161146636953,
-            "velocityX": -3.0575431517318843,
-            "velocityY": -2.1193297862453186,
-            "timestamp": 0.6101739468086674
-        },
-        {
-            "x": 2.620164345186853,
-            "y": 4.879059766080367,
-            "heading": -0.7372424721392209,
-            "angularVelocity": 1.2640870320425897,
-            "velocityX": -2.431314672774813,
-            "velocityY": -2.346738709855847,
-            "timestamp": 0.6973416534956199
-        },
-        {
-            "x": 2.469226841034766,
-            "y": 4.6911608685478585,
-            "heading": -0.6128307853924054,
-            "angularVelocity": 1.427268095897297,
-            "velocityX": -1.7315759458276552,
-            "velocityY": -2.155602168212534,
-            "timestamp": 0.7845093601825724
-        },
-        {
-            "x": 2.3748885049182054,
-            "y": 4.5313089505917965,
-            "heading": -0.5057978702393489,
-            "angularVelocity": 1.2278964219794164,
-            "velocityX": -1.0822624536327627,
-            "velocityY": -1.8338433352404204,
-            "timestamp": 0.871677066869525
-        },
-        {
-            "x": 2.3223904484665,
-            "y": 4.418253055823363,
-            "heading": -0.42949016513376354,
-            "angularVelocity": 0.87541255822675,
-            "velocityX": -0.6022649722820163,
-            "velocityY": -1.2969928780443263,
-            "timestamp": 0.9588447735564775
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0.4530366420626772,
-            "velocityX": -0.256866324898396,
-            "velocityY": -0.6682871218875669,
-            "timestamp": 1.0460124802434299
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -3.4100744325418914e-31,
-            "velocityX": -9.400788860397695e-33,
-            "velocityY": 0,
-            "timestamp": 1.1331801869303824
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S3 To C1.traj b/src/main/deploy/choreo/S3 To C1.traj
deleted file mode 100644
index 97fb7eaf..00000000
--- a/src/main/deploy/choreo/S3 To C1.traj	
+++ /dev/null
@@ -1,220 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 3.2688003829398086e-32,
-            "velocityX": -2.078713781970587e-32,
-            "velocityY": -4.638967376158192e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 4.29154268151456,
-            "y": 3.0623077457400183,
-            "heading": -0.5127665183876435,
-            "angularVelocity": 0.580669777278592,
-            "velocityX": 0.31134301225465,
-            "velocityY": 0.6796516730776717,
-            "timestamp": 0.09167599846825866
-        },
-        {
-            "x": 4.3492977760941525,
-            "y": 3.186661868715021,
-            "heading": -0.4070750475472326,
-            "angularVelocity": 1.1528804987818582,
-            "velocityX": 0.6299914431757085,
-            "velocityY": 1.3564523436094174,
-            "timestamp": 0.18335199693651733
-        },
-        {
-            "x": 4.438053987492854,
-            "y": 3.3723523918382017,
-            "heading": -0.25179906449139605,
-            "angularVelocity": 1.6937473891773143,
-            "velocityX": 0.968151019696088,
-            "velocityY": 2.025508597950785,
-            "timestamp": 0.27502799540477596
-        },
-        {
-            "x": 4.5709698511757795,
-            "y": 3.6117104026755453,
-            "heading": -0.06452556421817643,
-            "angularVelocity": 2.042775681773022,
-            "velocityX": 1.449843643960376,
-            "velocityY": 2.6109125053076734,
-            "timestamp": 0.36670399387303465
-        },
-        {
-            "x": 4.761013106791701,
-            "y": 3.810602792551275,
-            "heading": 0.11153690871840848,
-            "angularVelocity": 1.9204860146414893,
-            "velocityX": 2.0729881189319217,
-            "velocityY": 2.1695143025313537,
-            "timestamp": 0.45837999234129334
-        },
-        {
-            "x": 5.007328053154023,
-            "y": 3.968591564774131,
-            "heading": 0.25752334504560864,
-            "angularVelocity": 1.5924171949733021,
-            "velocityX": 2.6867986220799525,
-            "velocityY": 1.7233384404049508,
-            "timestamp": 0.550055990809552
-        },
-        {
-            "x": 5.305296422655045,
-            "y": 4.081628168088871,
-            "heading": 0.35020186087766364,
-            "angularVelocity": 1.0109354398157564,
-            "velocityX": 3.2502331524012615,
-            "velocityY": 1.2330010602925316,
-            "timestamp": 0.6417319892778107
-        },
-        {
-            "x": 5.6446965706540295,
-            "y": 4.152909996485894,
-            "heading": 0.3502021264888923,
-            "angularVelocity": 0.0000028972820926514052,
-            "velocityX": 3.702170182705961,
-            "velocityY": 0.7775407913523397,
-            "timestamp": 0.7334079877460694
-        },
-        {
-            "x": 5.981889969698284,
-            "y": 4.233991234618027,
-            "heading": 0.3502024148432213,
-            "angularVelocity": 0.0000031453633856654694,
-            "velocityX": 3.6780990082262504,
-            "velocityY": 0.884432561268565,
-            "timestamp": 0.8250839862143281
-        },
-        {
-            "x": 6.295899946950055,
-            "y": 4.381202864314355,
-            "heading": 0.350202827556894,
-            "angularVelocity": 0.000004501872678764248,
-            "velocityX": 3.425214696303447,
-            "velocityY": 1.6057815803042044,
-            "timestamp": 0.9167599846825868
-        },
-        {
-            "x": 6.57390491063703,
-            "y": 4.588535293884794,
-            "heading": 0.350203324900352,
-            "angularVelocity": 0.0000054250127210279085,
-            "velocityX": 3.0324727118541346,
-            "velocityY": 2.2615780906081357,
-            "timestamp": 1.0084359831508454
-        },
-        {
-            "x": 6.804553755441848,
-            "y": 4.847523149382792,
-            "heading": 0.35020327996165895,
-            "angularVelocity": -4.901903857536901e-7,
-            "velocityX": 2.5159130923965525,
-            "velocityY": 2.825034467311176,
-            "timestamp": 1.100111981619104
-        },
-        {
-            "x": 6.978428828519116,
-            "y": 5.147591577808381,
-            "heading": 0.35020324373344786,
-            "angularVelocity": -3.9517661894346874e-7,
-            "velocityX": 1.8966258997164847,
-            "velocityY": 3.273140554116584,
-            "timestamp": 1.1917879800873628
-        },
-        {
-            "x": 7.088431556948234,
-            "y": 5.476488224562697,
-            "heading": 0.3502032113343976,
-            "angularVelocity": -3.5340820734555857e-7,
-            "velocityX": 1.1999076123202526,
-            "velocityY": 3.587598196360961,
-            "timestamp": 1.2834639785556214
-        },
-        {
-            "x": 7.187536215850895,
-            "y": 5.808831226900728,
-            "heading": 0.3502031791295722,
-            "angularVelocity": -3.5128960565806065e-7,
-            "velocityX": 1.0810316828670656,
-            "velocityY": 3.6251909757285055,
-            "timestamp": 1.3751399770238801
-        },
-        {
-            "x": 7.286640767886556,
-            "y": 6.141174261106461,
-            "heading": 0.3502031469247365,
-            "angularVelocity": -3.512897194087527e-7,
-            "velocityX": 1.0810305171639119,
-            "velocityY": 3.6251913233407715,
-            "timestamp": 1.4668159754921388
-        },
-        {
-            "x": 7.3857452620780215,
-            "y": 6.473517312561292,
-            "heading": 0.350203114719879,
-            "angularVelocity": -3.512899557892109e-7,
-            "velocityX": 1.0810298862005645,
-            "velocityY": 3.625191511493593,
-            "timestamp": 1.5584919739603975
-        },
-        {
-            "x": 7.481976228665169,
-            "y": 6.80241098377653,
-            "heading": 0.33998449018149474,
-            "angularVelocity": -0.11146455679915658,
-            "velocityX": 1.0496855032395944,
-            "velocityY": 3.5875657392388547,
-            "timestamp": 1.6501679724286562
-        },
-        {
-            "x": 7.561455250393006,
-            "y": 7.068914029375232,
-            "heading": 0.2316909043200923,
-            "angularVelocity": -1.1812643185871456,
-            "velocityX": 0.8669556160367877,
-            "velocityY": 2.907010014087551,
-            "timestamp": 1.741843970896915
-        },
-        {
-            "x": 7.620860593043084,
-            "y": 7.268277067392774,
-            "heading": 0.12481105079738766,
-            "angularVelocity": -1.1658433538600752,
-            "velocityX": 0.6479923168837385,
-            "velocityY": 2.1746481232660675,
-            "timestamp": 1.8335199693651736
-        },
-        {
-            "x": 7.66032628893014,
-            "y": 7.4008489817464715,
-            "heading": 0.04379045785830094,
-            "angularVelocity": -0.8837710446877722,
-            "velocityX": 0.430491039600946,
-            "velocityY": 1.4460918513977026,
-            "timestamp": 1.9251959678334323
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 6.52409657127002e-31,
-            "angularVelocity": -0.4776654586801488,
-            "velocityX": 0.21460045593800534,
-            "velocityY": 0.7215740145599008,
-            "timestamp": 2.0168719663016907
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": -6.745238901014272e-30,
-            "velocityX": 0,
-            "velocityY": 3.373152313643601e-31,
-            "timestamp": 2.1085479647699494
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S3 To C2.traj b/src/main/deploy/choreo/S3 To C2.traj
deleted file mode 100644
index 69777c27..00000000
--- a/src/main/deploy/choreo/S3 To C2.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -2.8196964588027846e-31,
-            "velocityX": -1.564828148658428e-31,
-            "velocityY": 3.07478616058615e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 4.294807569207289,
-            "y": 3.0477321348748183,
-            "heading": -0.5960373953696199,
-            "angularVelocity": -0.36026847684168845,
-            "velocityX": 0.381499939303545,
-            "velocityY": 0.5724991570056517,
-            "timestamp": 0.08337503084628527
-        },
-        {
-            "x": 4.359939756522268,
-            "y": 3.142193816557181,
-            "heading": -0.6551934033575421,
-            "angularVelocity": -0.7095170746861289,
-            "velocityX": 0.7811953609355742,
-            "velocityY": 1.1329732741750633,
-            "timestamp": 0.16675006169257053
-        },
-        {
-            "x": 4.461050205756915,
-            "y": 3.2814565261988764,
-            "heading": -0.7417418377173162,
-            "angularVelocity": -1.038061797174498,
-            "velocityX": 1.2127185826300881,
-            "velocityY": 1.6703167390540123,
-            "timestamp": 0.2501250925388558
-        },
-        {
-            "x": 4.603825208189836,
-            "y": 3.4604941356958814,
-            "heading": -0.8514332359801334,
-            "angularVelocity": -1.315638472926628,
-            "velocityX": 1.7124431737380506,
-            "velocityY": 2.147376830685539,
-            "timestamp": 0.33350012338514107
-        },
-        {
-            "x": 4.8030511460470855,
-            "y": 3.6526461815261504,
-            "heading": -0.9644207302174959,
-            "angularVelocity": -1.3551718433024906,
-            "velocityX": 2.3895156119888465,
-            "velocityY": 2.3046713611960277,
-            "timestamp": 0.41687515423142635
-        },
-        {
-            "x": 5.051097476442208,
-            "y": 3.8467657037250893,
-            "heading": -0.9658198239131095,
-            "angularVelocity": -0.016780727772000164,
-            "velocityX": 2.975067329839247,
-            "velocityY": 2.328269269930804,
-            "timestamp": 0.5002501850777116
-        },
-        {
-            "x": 5.320526470824812,
-            "y": 4.010737925204951,
-            "heading": -0.965820109061322,
-            "angularVelocity": -0.000003420067250854742,
-            "velocityX": 3.2315309709370754,
-            "velocityY": 1.9666825884858703,
-            "timestamp": 0.5836252159239969
-        },
-        {
-            "x": 5.614243645591117,
-            "y": 4.1256688963704224,
-            "heading": -0.9658201941489747,
-            "angularVelocity": -0.000001020541184209393,
-            "velocityX": 3.5228433715103167,
-            "velocityY": 1.37848190278171,
-            "timestamp": 0.6670002467702821
-        },
-        {
-            "x": 5.906557788271019,
-            "y": 4.244122886725936,
-            "heading": -0.965820105112684,
-            "angularVelocity": 0.0000010679011417361832,
-            "velocityX": 3.506015406684877,
-            "velocityY": 1.4207369898777267,
-            "timestamp": 0.7503752776165674
-        },
-        {
-            "x": 6.172253111887642,
-            "y": 4.41406398245588,
-            "heading": -0.9657938677546691,
-            "angularVelocity": 0.0003146908342779248,
-            "velocityX": 3.1867493291423483,
-            "velocityY": 2.0382732576526004,
-            "timestamp": 0.8337503084628527
-        },
-        {
-            "x": 6.426785390452526,
-            "y": 4.5994304003649855,
-            "heading": -0.9640144900160125,
-            "angularVelocity": 0.021341854037057975,
-            "velocityX": 3.0528597828545716,
-            "velocityY": 2.2232845496735987,
-            "timestamp": 0.917125339309138
-        },
-        {
-            "x": 6.681598941757368,
-            "y": 4.785246665794811,
-            "heading": -0.9639002296367303,
-            "angularVelocity": 0.0013704388246982366,
-            "velocityX": 3.0562333676929114,
-            "velocityY": 2.2286800201898096,
-            "timestamp": 1.0005003701554231
-        },
-        {
-            "x": 6.9014822245071255,
-            "y": 5.01136660335564,
-            "heading": -0.9639000238890051,
-            "angularVelocity": 0.000002467737919950576,
-            "velocityX": 2.6372797769052228,
-            "velocityY": 2.7120822057351384,
-            "timestamp": 1.0838754010017084
-        },
-        {
-            "x": 7.122012116423131,
-            "y": 5.23107273201418,
-            "heading": -0.9515772318649645,
-            "angularVelocity": 0.1477995498047812,
-            "velocityX": 2.645035206314798,
-            "velocityY": 2.635154990990089,
-            "timestamp": 1.1672504318479937
-        },
-        {
-            "x": 7.311148990719613,
-            "y": 5.417840497645029,
-            "heading": -0.7917020160601519,
-            "angularVelocity": 1.9175431083146124,
-            "velocityX": 2.2685073981583956,
-            "velocityY": 2.2400923122317544,
-            "timestamp": 1.250625462694279
-        },
-        {
-            "x": 7.462007390054419,
-            "y": 5.572027096128993,
-            "heading": -0.5225549044290918,
-            "angularVelocity": 3.228150069620655,
-            "velocityX": 1.8093954245478587,
-            "velocityY": 1.8493138403538476,
-            "timestamp": 1.3340004935405643
-        },
-        {
-            "x": 7.571928087470151,
-            "y": 5.685474744918936,
-            "heading": -0.27088873612536774,
-            "angularVelocity": 3.018483660506339,
-            "velocityX": 1.318388686636741,
-            "velocityY": 1.3606909363440005,
-            "timestamp": 1.4173755243868496
-        },
-        {
-            "x": 7.6442805622690475,
-            "y": 5.7599789636443175,
-            "heading": -0.0924144798498188,
-            "angularVelocity": 2.1406199729580164,
-            "velocityX": 0.8677954786282445,
-            "velocityY": 0.8936034921863096,
-            "timestamp": 1.5007505552331348
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -2.5332548087748344e-31,
-            "angularVelocity": 1.1084191383412998,
-            "velocityX": 0.42841888474747514,
-            "velocityY": 0.4440302567795869,
-            "timestamp": 1.5841255860794201
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 5.737670210968143e-33,
-            "velocityX": 0,
-            "velocityY": -3.2710687085311585e-31,
-            "timestamp": 1.6675006169257054
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S3 To C3.traj b/src/main/deploy/choreo/S3 To C3.traj
deleted file mode 100644
index 9d039264..00000000
--- a/src/main/deploy/choreo/S3 To C3.traj	
+++ /dev/null
@@ -1,166 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -1.0027901855857707e-30,
-            "velocityX": -5.17158696002297e-32,
-            "velocityY": 1.9371271312239428e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 4.300239460670815,
-            "y": 3.0530479185441526,
-            "heading": -0.6028935811234369,
-            "angularVelocity": -0.4158951640704737,
-            "velocityX": 0.4197942063083121,
-            "velocityY": 0.5980003056006388,
-            "timestamp": 0.08870884855296232
-        },
-        {
-            "x": 4.375862012794379,
-            "y": 3.1584586937478076,
-            "heading": -0.6740120078870278,
-            "angularVelocity": -0.8017061197804932,
-            "velocityX": 0.8524803709791626,
-            "velocityY": 1.1882780232540273,
-            "timestamp": 0.17741769710592464
-        },
-        {
-            "x": 4.491898262382256,
-            "y": 3.3149103934712074,
-            "heading": -0.7745169367898204,
-            "angularVelocity": -1.1329752391362358,
-            "velocityX": 1.3080572172977813,
-            "velocityY": 1.763653821185524,
-            "timestamp": 0.26612654565888694
-        },
-        {
-            "x": 4.65291407862912,
-            "y": 3.5188640922932937,
-            "heading": -0.893163244153065,
-            "angularVelocity": -1.3374799616794542,
-            "velocityX": 1.8151043427277864,
-            "velocityY": 2.299135905256609,
-            "timestamp": 0.3548353942118493
-        },
-        {
-            "x": 4.870914712338585,
-            "y": 3.7498942741357597,
-            "heading": -0.9455687285938831,
-            "angularVelocity": -0.5907582534963245,
-            "velocityX": 2.457484650804715,
-            "velocityY": 2.6043645657798487,
-            "timestamp": 0.44354424276481164
-        },
-        {
-            "x": 5.142859309500119,
-            "y": 3.9465163862068953,
-            "heading": -0.9455688623878027,
-            "angularVelocity": -0.0000015082364580335284,
-            "velocityX": 3.065585920655626,
-            "velocityY": 2.216488155110533,
-            "timestamp": 0.532253091317774
-        },
-        {
-            "x": 5.447866241414174,
-            "y": 4.086462405550872,
-            "heading": -0.9455689961292948,
-            "angularVelocity": -0.0000015076454528851402,
-            "velocityX": 3.438292085732067,
-            "velocityY": 1.5775880492961702,
-            "timestamp": 0.6209619398707363
-        },
-        {
-            "x": 5.774274975125892,
-            "y": 4.16438201701327,
-            "heading": -0.9455689173593601,
-            "angularVelocity": 8.879602884106285e-7,
-            "velocityX": 3.6795510147653454,
-            "velocityY": 0.8783747363813094,
-            "timestamp": 0.7096707884236987
-        },
-        {
-            "x": 6.109606649873876,
-            "y": 4.1772967927030695,
-            "heading": -0.9455688167433821,
-            "angularVelocity": 0.0000011342270768412302,
-            "velocityX": 3.780137835379285,
-            "velocityY": 0.14558610443566755,
-            "timestamp": 0.798379636976661
-        },
-        {
-            "x": 6.445060320326861,
-            "y": 4.168079453807152,
-            "heading": -0.9455687201101868,
-            "angularVelocity": 0.0000010893298350239914,
-            "velocityX": 3.781513072539862,
-            "velocityY": -0.10390551840399499,
-            "timestamp": 0.8870884855296234
-        },
-        {
-            "x": 6.780513985890507,
-            "y": 4.158861936977066,
-            "heading": -0.9455686234766241,
-            "angularVelocity": 0.0000010893339756447378,
-            "velocityX": 3.781513017423149,
-            "velocityY": -0.10390752422611307,
-            "timestamp": 0.9757973340825857
-        },
-        {
-            "x": 7.079210544299498,
-            "y": 4.144050975293544,
-            "heading": -0.856180476499644,
-            "angularVelocity": 1.0076576174203435,
-            "velocityX": 3.3671563015572095,
-            "velocityY": -0.16696149172400718,
-            "timestamp": 1.064506182635548
-        },
-        {
-            "x": 7.324091725395792,
-            "y": 4.136558502839443,
-            "heading": -0.6317167360096363,
-            "angularVelocity": 2.5303421716267107,
-            "velocityX": 2.760504561730298,
-            "velocityY": -0.08446138774564337,
-            "timestamp": 1.1532150311885103
-        },
-        {
-            "x": 7.50528856962614,
-            "y": 4.13147246408345,
-            "heading": -0.35065072299807204,
-            "angularVelocity": 3.1684101146207286,
-            "velocityX": 2.042601692909661,
-            "velocityY": -0.05733406350050504,
-            "timestamp": 1.2419238797414727
-        },
-        {
-            "x": 7.622685008969739,
-            "y": 4.128418008534293,
-            "heading": -0.12509027443555182,
-            "angularVelocity": 2.5427051781407424,
-            "velocityX": 1.323390408720159,
-            "velocityY": -0.03443236609404116,
-            "timestamp": 1.330632728294435
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 3.405276849475189e-32,
-            "angularVelocity": 1.4101217237744719,
-            "velocityX": 0.6461022994345517,
-            "velocityY": -0.01598497283444213,
-            "timestamp": 1.4193415768473974
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -2.3485646679922316e-31,
-            "velocityX": 0,
-            "velocityY": -2.6996723431981715e-31,
-            "timestamp": 1.5080504254003597
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S3 To C4.traj b/src/main/deploy/choreo/S3 To C4.traj
deleted file mode 100644
index 0b021e4e..00000000
--- a/src/main/deploy/choreo/S3 To C4.traj	
+++ /dev/null
@@ -1,238 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 0,
-            "velocityX": 5.832812358477626e-33,
-            "velocityY": -2.114400767819492e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 4.2637473414422145,
-            "y": 2.974020167404826,
-            "heading": -0.5570803398431683,
-            "angularVelocity": 0.15939480257853877,
-            "velocityX": 0.013355031418934325,
-            "velocityY": -0.46426099366130524,
-            "timestamp": 0.05595954204614445
-        },
-        {
-            "x": 4.266239342094047,
-            "y": 2.922097369670203,
-            "heading": -0.5396590874905302,
-            "angularVelocity": 0.31131870840312753,
-            "velocityX": 0.04453218451605929,
-            "velocityY": -0.9278631639230981,
-            "timestamp": 0.1119190840922889
-        },
-        {
-            "x": 4.271816064940073,
-            "y": 2.844342168020101,
-            "heading": -0.5143157133438189,
-            "angularVelocity": 0.4528874472527088,
-            "velocityX": 0.0996563345966598,
-            "velocityY": -1.3894895992176792,
-            "timestamp": 0.16787862613843335
-        },
-        {
-            "x": 4.282354343554946,
-            "y": 2.7410289237168692,
-            "heading": -0.48190029722305777,
-            "angularVelocity": 0.5792652143941603,
-            "velocityX": 0.18831960072480589,
-            "velocityY": -1.8462131841257783,
-            "timestamp": 0.2238381681845778
-        },
-        {
-            "x": 4.3006106845058465,
-            "y": 2.6128237251760056,
-            "heading": -0.4437500478548854,
-            "angularVelocity": 0.6817469902936669,
-            "velocityX": 0.32624178617911886,
-            "velocityY": -2.2910337335345985,
-            "timestamp": 0.27979771023072225
-        },
-        {
-            "x": 4.330801306241698,
-            "y": 2.4614150778969766,
-            "heading": -0.40214700400499553,
-            "angularVelocity": 0.7434486117771366,
-            "velocityX": 0.5395080201148661,
-            "velocityY": -2.705680599640681,
-            "timestamp": 0.3357572522768667
-        },
-        {
-            "x": 4.379172513113433,
-            "y": 2.2912661424924936,
-            "heading": -0.3611794534128463,
-            "angularVelocity": 0.7320923133783194,
-            "velocityX": 0.864396045840541,
-            "velocityY": -3.0405705476320635,
-            "timestamp": 0.39171679432301115
-        },
-        {
-            "x": 4.451949891834961,
-            "y": 2.111933259909663,
-            "heading": -0.32494756197390784,
-            "angularVelocity": 0.6474658318159084,
-            "velocityX": 1.3005356380778725,
-            "velocityY": -3.2046881733762564,
-            "timestamp": 0.4476763363691556
-        },
-        {
-            "x": 4.550285732793113,
-            "y": 1.9373903674143325,
-            "heading": -0.2971520965733793,
-            "angularVelocity": 0.49670644869849423,
-            "velocityX": 1.7572667209653445,
-            "velocityY": -3.1190907951212283,
-            "timestamp": 0.5036358784153001
-        },
-        {
-            "x": 4.671000177640236,
-            "y": 1.7762856565305696,
-            "heading": -0.2715476133805437,
-            "angularVelocity": 0.457553479828869,
-            "velocityX": 2.157173565637473,
-            "velocityY": -2.878949773229245,
-            "timestamp": 0.5595954204614446
-        },
-        {
-            "x": 4.810884164899462,
-            "y": 1.6328902215117949,
-            "heading": -0.24301377044973924,
-            "angularVelocity": 0.5099012945331528,
-            "velocityX": 2.4997343106181424,
-            "velocityY": -2.5624840693036233,
-            "timestamp": 0.6155549625075891
-        },
-        {
-            "x": 4.966899091534391,
-            "y": 1.5100232904396305,
-            "heading": -0.21315700918191657,
-            "angularVelocity": 0.5335419157506132,
-            "velocityX": 2.7879950573269263,
-            "velocityY": -2.1956386092446096,
-            "timestamp": 0.6715145045537336
-        },
-        {
-            "x": 5.136752128601074,
-            "y": 1.409298062324524,
-            "heading": -0.1837509654270181,
-            "angularVelocity": 0.5254875697633724,
-            "velocityX": 3.035282828558936,
-            "velocityY": -1.7999651968567865,
-            "timestamp": 0.7274740465998781
-        },
-        {
-            "x": 5.4382274814533025,
-            "y": 1.3074419969375322,
-            "heading": -0.14111678525881038,
-            "angularVelocity": 0.4782734646509064,
-            "velocityX": 3.3819733590888914,
-            "velocityY": -1.142629061849971,
-            "timestamp": 0.8166158939305274
-        },
-        {
-            "x": 5.764139683697637,
-            "y": 1.267066681764717,
-            "heading": -0.11133632809533998,
-            "angularVelocity": 0.3340794257159682,
-            "velocityX": 3.6561077877984602,
-            "velocityY": -0.4529333459183563,
-            "timestamp": 0.9057577412611766
-        },
-        {
-            "x": 6.100462721846291,
-            "y": 1.2915989985388854,
-            "heading": -0.11133120767369757,
-            "angularVelocity": 0.000057441278584387276,
-            "velocityX": 3.7728973340784897,
-            "velocityY": 0.2752053890377148,
-            "timestamp": 0.9948995885918258
-        },
-        {
-            "x": 6.42549869748969,
-            "y": 1.3814198753662668,
-            "heading": -0.1113311879575033,
-            "angularVelocity": 2.2117776172639892e-7,
-            "velocityX": 3.6462782113742715,
-            "velocityY": 1.0076174043625032,
-            "timestamp": 1.084041435922475
-        },
-        {
-            "x": 6.7266979846048365,
-            "y": 1.533061465523067,
-            "heading": -0.11133117504978503,
-            "angularVelocity": 1.4479976182990285e-7,
-            "velocityX": 3.3788764327254777,
-            "velocityY": 1.7011268522889103,
-            "timestamp": 1.1731832832531242
-        },
-        {
-            "x": 6.992432961420095,
-            "y": 1.740669527522903,
-            "heading": -0.11133116451645807,
-            "angularVelocity": 1.1816366034373416e-7,
-            "velocityX": 2.981035111708839,
-            "velocityY": 2.3289629754897407,
-            "timestamp": 1.2623251305837733
-        },
-        {
-            "x": 7.221595592407175,
-            "y": 1.979432543612985,
-            "heading": -0.09587073631794915,
-            "angularVelocity": 0.17343625537800947,
-            "velocityX": 2.5707637641507293,
-            "velocityY": 2.678461611912118,
-            "timestamp": 1.3514669779144224
-        },
-        {
-            "x": 7.404993837760709,
-            "y": 2.170501480308595,
-            "heading": -0.06411284637687509,
-            "angularVelocity": 0.3562624164974272,
-            "velocityX": 2.0573754173309005,
-            "velocityY": 2.1434258142181197,
-            "timestamp": 1.4406088252450715
-        },
-        {
-            "x": 7.54251389523076,
-            "y": 2.3137697288578556,
-            "heading": -0.0342508837604198,
-            "angularVelocity": 0.33499376006509896,
-            "velocityX": 1.5427104282454527,
-            "velocityY": 1.6071940714649837,
-            "timestamp": 1.5297506725757206
-        },
-        {
-            "x": 7.634176122098128,
-            "y": 2.4092619199619465,
-            "heading": -0.011960250388889534,
-            "angularVelocity": 0.25005801471490263,
-            "velocityX": 1.028273808679041,
-            "velocityY": 1.0712386377846093,
-            "timestamp": 1.6188925199063697
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 9.313254165846089e-33,
-            "angularVelocity": 0.1341709954082624,
-            "velocityX": 0.5140557355952123,
-            "velocityY": 0.5355294002487934,
-            "timestamp": 1.7080343672370188
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -1.1876173054192403e-31,
-            "velocityX": 4.5955148793706025e-32,
-            "velocityY": 0,
-            "timestamp": 1.797176214567668
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S3 To C5.traj b/src/main/deploy/choreo/S3 To C5.traj
deleted file mode 100644
index d933d22a..00000000
--- a/src/main/deploy/choreo/S3 To C5.traj	
+++ /dev/null
@@ -1,184 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -2.2118032332823513e-27,
-            "velocityX": -2.2398078489200873e-28,
-            "velocityY": 7.32316842173409e-29,
-            "timestamp": 0
-        },
-        {
-            "x": 4.257275610783411,
-            "y": 2.9390850995701614,
-            "heading": -0.6153074756658753,
-            "angularVelocity": -0.5685220144659363,
-            "velocityX": -0.06600299944485946,
-            "velocityY": -0.7023572274929988,
-            "timestamp": 0.0867292284401611
-        },
-        {
-            "x": 4.259999029271463,
-            "y": 2.8165594202827866,
-            "heading": -0.6936339270516045,
-            "angularVelocity": -0.9031148183195571,
-            "velocityX": 0.03140139186100546,
-            "velocityY": -1.4127380295088343,
-            "timestamp": 0.1734584568803222
-        },
-        {
-            "x": 4.293815085802935,
-            "y": 2.6396142169455077,
-            "heading": -0.7636289676296801,
-            "angularVelocity": -0.8070524993355632,
-            "velocityX": 0.3899038091270867,
-            "velocityY": -2.0402026689233352,
-            "timestamp": 0.2601876853204833
-        },
-        {
-            "x": 4.379794167011224,
-            "y": 2.432035667571809,
-            "heading": -0.7730870281638328,
-            "angularVelocity": -0.10905274616477362,
-            "velocityX": 0.9913506986587586,
-            "velocityY": -2.3934093858210397,
-            "timestamp": 0.3469169137606444
-        },
-        {
-            "x": 4.522000800858502,
-            "y": 2.2059673134468256,
-            "heading": -0.7069342437539652,
-            "angularVelocity": 0.7627507542686203,
-            "velocityX": 1.6396621577856372,
-            "velocityY": -2.6065993920487904,
-            "timestamp": 0.43364614220080544
-        },
-        {
-            "x": 4.726803922559788,
-            "y": 1.9764479224139355,
-            "heading": -0.6474090008701758,
-            "angularVelocity": 0.6863342837743838,
-            "velocityX": 2.3614083208706247,
-            "velocityY": -2.6463903249323897,
-            "timestamp": 0.5203753706409665
-        },
-        {
-            "x": 4.985426543182091,
-            "y": 1.774560944536545,
-            "heading": -0.6474088034070749,
-            "angularVelocity": 0.000002276776866191791,
-            "velocityX": 2.9819545875556583,
-            "velocityY": -2.3277847792302735,
-            "timestamp": 0.6071045990811276
-        },
-        {
-            "x": 5.274431548584119,
-            "y": 1.6192543598542732,
-            "heading": -0.6474086941993331,
-            "angularVelocity": 0.000001259180368575944,
-            "velocityX": 3.3322676864516145,
-            "velocityY": -1.7907064028526984,
-            "timestamp": 0.6938338275212886
-        },
-        {
-            "x": 5.576835908979959,
-            "y": 1.4919922741488194,
-            "heading": -0.6474086295363626,
-            "angularVelocity": 7.455729944345581e-7,
-            "velocityX": 3.4867641028824017,
-            "velocityY": -1.4673494506325018,
-            "timestamp": 0.7805630559614497
-        },
-        {
-            "x": 5.888110144336631,
-            "y": 1.388298557512795,
-            "heading": -0.6474085736154427,
-            "angularVelocity": 6.447759407143774e-7,
-            "velocityX": 3.589034988031027,
-            "velocityY": -1.1956028953671916,
-            "timestamp": 0.8672922844016108
-        },
-        {
-            "x": 6.199390629137113,
-            "y": 1.2846236026664921,
-            "heading": -0.6474085176958322,
-            "angularVelocity": 6.447608435198244e-7,
-            "velocityX": 3.5891070449826046,
-            "velocityY": -1.1953865693365275,
-            "timestamp": 0.9540215128417718
-        },
-        {
-            "x": 6.510671944712167,
-            "y": 1.1809511422971397,
-            "heading": -0.6474084617763125,
-            "angularVelocity": 6.447597973785494e-7,
-            "velocityX": 3.589116623928252,
-            "velocityY": -1.1953578076724478,
-            "timestamp": 1.040750741281933
-        },
-        {
-            "x": 6.8219538378473334,
-            "y": 1.0772804160640193,
-            "heading": -0.6474084058668956,
-            "angularVelocity": 6.446433092317229e-7,
-            "velocityX": 3.5891232832762197,
-            "velocityY": -1.1953378128419065,
-            "timestamp": 1.127479969722094
-        },
-        {
-            "x": 7.104843996542823,
-            "y": 0.9795715918943764,
-            "heading": -0.5659567926753784,
-            "angularVelocity": 0.9391483662017565,
-            "velocityX": 3.2617626581409067,
-            "velocityY": -1.126596257420401,
-            "timestamp": 1.2142091981622551
-        },
-        {
-            "x": 7.336171547926519,
-            "y": 0.9014041069183023,
-            "heading": -0.36771202193482067,
-            "angularVelocity": 2.2857896271651064,
-            "velocityX": 2.6672386638755765,
-            "velocityY": -0.9012819136285574,
-            "timestamp": 1.3009384266024162
-        },
-        {
-            "x": 7.508580909570239,
-            "y": 0.8437450323938049,
-            "heading": -0.19262457792893875,
-            "angularVelocity": 2.018782446873579,
-            "velocityX": 1.9879037868147627,
-            "velocityY": -0.6648171044699364,
-            "timestamp": 1.3876676550425773
-        },
-        {
-            "x": 7.622996353926532,
-            "y": 0.8057914476404057,
-            "heading": -0.06643070405801102,
-            "angularVelocity": 1.4550328204291798,
-            "velocityX": 1.3192258989739947,
-            "velocityY": -0.4376100818144245,
-            "timestamp": 1.4743968834827383
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -1.2156690964354266e-28,
-            "angularVelocity": 0.7659552062526378,
-            "velocityX": 0.6572599237729437,
-            "velocityY": -0.21666799046150395,
-            "timestamp": 1.5611261119228994
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 1.25023472254267e-27,
-            "velocityX": 0,
-            "velocityY": -4.9030509543456705e-29,
-            "timestamp": 1.6478553403630605
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S3 To W1.traj b/src/main/deploy/choreo/S3 To W1.traj
deleted file mode 100644
index 44ee68f9..00000000
--- a/src/main/deploy/choreo/S3 To W1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -1.6794679447375943e-30,
-            "velocityX": 6.742046899995187e-33,
-            "velocityY": 9.708842875589211e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 4.290023963257714,
-            "y": 3.0497874441329906,
-            "heading": -0.5200869306801483,
-            "angularVelocity": 0.5503538158132694,
-            "velocityX": 0.3239326300245928,
-            "velocityY": 0.5967954280576505,
-            "timestamp": 0.08342464065958187
-        },
-        {
-            "x": 4.342730369898429,
-            "y": 3.150178003649595,
-            "heading": -0.42997936088427535,
-            "angularVelocity": 1.0801073769506666,
-            "velocityX": 0.6317846408926776,
-            "velocityY": 1.2033681982072046,
-            "timestamp": 0.16684928131916374
-        },
-        {
-            "x": 4.418537982231504,
-            "y": 3.3026029150734924,
-            "heading": -0.2990998156933407,
-            "angularVelocity": 1.5688355880967437,
-            "velocityX": 0.9086957010988046,
-            "velocityY": 1.8270970089745318,
-            "timestamp": 0.2502739219787456
-        },
-        {
-            "x": 4.5104462205151705,
-            "y": 3.5100667276192694,
-            "heading": -0.13617594937576347,
-            "angularVelocity": 1.9529465758491624,
-            "velocityX": 1.1016917490685052,
-            "velocityY": 2.4868409489750483,
-            "timestamp": 0.3336985626383275
-        },
-        {
-            "x": 4.573472263152817,
-            "y": 3.7674681923433693,
-            "heading": 0.004287761033867021,
-            "angularVelocity": 1.6837196935950753,
-            "velocityX": 0.7554847361563879,
-            "velocityY": 3.0854369007645883,
-            "timestamp": 0.41712320329790936
-        },
-        {
-            "x": 4.6071871310155705,
-            "y": 4.066840423815047,
-            "heading": 0.04705620405427594,
-            "angularVelocity": 0.5126596013152507,
-            "velocityX": 0.40413560785150865,
-            "velocityY": 3.5885348633778387,
-            "timestamp": 0.5005478439574912
-        },
-        {
-            "x": 4.587417601167403,
-            "y": 4.370555626705338,
-            "heading": 0.08458578048595149,
-            "angularVelocity": 0.4498620088136386,
-            "velocityX": -0.23697470785444333,
-            "velocityY": 3.6405934804036506,
-            "timestamp": 0.5839724846170731
-        },
-        {
-            "x": 4.510022832963655,
-            "y": 4.67116829443229,
-            "heading": 0.1002321163817716,
-            "angularVelocity": 0.1875505338964035,
-            "velocityX": -0.9277207260569706,
-            "velocityY": 3.603403806719601,
-            "timestamp": 0.667397125276655
-        },
-        {
-            "x": 4.376634060333898,
-            "y": 4.957183607465761,
-            "heading": 0.10023219176740035,
-            "angularVelocity": 9.036374403612271e-7,
-            "velocityX": -1.5989133615097586,
-            "velocityY": 3.4284272700744434,
-            "timestamp": 0.7508217659362368
-        },
-        {
-            "x": 4.19313075849175,
-            "y": 5.213940045953902,
-            "heading": 0.10023221692184411,
-            "angularVelocity": 3.015229499928316e-7,
-            "velocityX": -2.1996295146291525,
-            "velocityY": 3.0777050576201734,
-            "timestamp": 0.8342464065958187
-        },
-        {
-            "x": 3.9657176811920825,
-            "y": 5.432756463781841,
-            "heading": 0.10023223856547658,
-            "angularVelocity": 2.5943932502769966e-7,
-            "velocityX": -2.725970115084308,
-            "velocityY": 2.6229231087830502,
-            "timestamp": 0.9176710472554006
-        },
-        {
-            "x": 3.7186426713834653,
-            "y": 5.629101242954768,
-            "heading": 0.10023225962798245,
-            "angularVelocity": 2.524734383899783e-7,
-            "velocityX": -2.961655068037016,
-            "velocityY": 2.3535585843770224,
-            "timestamp": 1.0010956879149824
-        },
-        {
-            "x": 3.4715770348675625,
-            "y": 5.825457816649942,
-            "heading": 0.10023228069061084,
-            "angularVelocity": 2.5247490699416256e-7,
-            "velocityX": -2.961542711631996,
-            "velocityY": 2.353699963736333,
-            "timestamp": 1.0845203285745642
-        },
-        {
-            "x": 3.224511384705754,
-            "y": 6.021814373175139,
-            "heading": 0.10023230175325984,
-            "angularVelocity": 2.5247515406639525e-7,
-            "velocityX": -2.96154287520364,
-            "velocityY": 2.353699757922098,
-            "timestamp": 1.167944969234146
-        },
-        {
-            "x": 2.9774457295747583,
-            "y": 6.218170923447359,
-            "heading": 0.10023232281654228,
-            "angularVelocity": 2.524827469327523e-7,
-            "velocityX": -2.9615429347686173,
-            "velocityY": 2.3536996829685104,
-            "timestamp": 1.2513696098937277
-        },
-        {
-            "x": 2.751412346836958,
-            "y": 6.39797212959724,
-            "heading": 0.1658210049038553,
-            "angularVelocity": 0.7862027521934517,
-            "velocityX": -2.7094319010631414,
-            "velocityY": 2.155252989144637,
-            "timestamp": 1.3347942505533095
-        },
-        {
-            "x": 2.5706626829115913,
-            "y": 6.54172747182469,
-            "heading": 0.24638520673123038,
-            "angularVelocity": 0.9657123026291593,
-            "velocityX": -2.166622025534686,
-            "velocityY": 1.723176043563091,
-            "timestamp": 1.4182188912128912
-        },
-        {
-            "x": 2.435257350873027,
-            "y": 6.649419264379792,
-            "heading": 0.31518856404188306,
-            "angularVelocity": 0.8247366337651717,
-            "velocityX": -1.6230855891977036,
-            "velocityY": 1.2908871012647558,
-            "timestamp": 1.501643531872473
-        },
-        {
-            "x": 2.3450659538089185,
-            "y": 6.721154353305962,
-            "heading": 0.3643262231924431,
-            "angularVelocity": 0.5890065424563048,
-            "velocityX": -1.0811122031935256,
-            "velocityY": 0.8598789081859941,
-            "timestamp": 1.5850681725320548
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0.3077481257883978,
-            "velocityX": -0.5401995555822935,
-            "velocityY": 0.42967696846676384,
-            "timestamp": 1.6684928131916366
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": -2.8369790280137563e-29,
-            "velocityX": 0,
-            "velocityY": -1.9155196850548007e-31,
-            "timestamp": 1.7519174538512183
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S3 To W2.traj b/src/main/deploy/choreo/S3 To W2.traj
deleted file mode 100644
index 01699583..00000000
--- a/src/main/deploy/choreo/S3 To W2.traj	
+++ /dev/null
@@ -1,157 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.1175745999315144e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 4.331197427717942,
-            "y": 3.064203342806604,
-            "heading": -0.5918486780870994,
-            "angularVelocity": -0.24352382070717674,
-            "velocityX": 0.642497001367488,
-            "velocityY": 0.604868198278966,
-            "timestamp": 0.10614435176007272
-        },
-        {
-            "x": 4.457228812945604,
-            "y": 3.20188389088458,
-            "heading": -0.6500618235234895,
-            "angularVelocity": -0.5484337552691863,
-            "velocityX": 1.1873583769444531,
-            "velocityY": 1.2971066834454588,
-            "timestamp": 0.21228870352014545
-        },
-        {
-            "x": 4.610677951553499,
-            "y": 3.4285671398637003,
-            "heading": -0.7523465196948876,
-            "angularVelocity": -0.9636376733695737,
-            "velocityX": 1.4456646638603075,
-            "velocityY": 2.1356129197671536,
-            "timestamp": 0.31843305528021815
-        },
-        {
-            "x": 4.708912918877337,
-            "y": 3.730071149255109,
-            "heading": -0.9027593729452231,
-            "angularVelocity": -1.4170594172577993,
-            "velocityX": 0.9254846413861598,
-            "velocityY": 2.840509215911215,
-            "timestamp": 0.4245774070402909
-        },
-        {
-            "x": 4.717930123488916,
-            "y": 4.059451821332611,
-            "heading": -1.0196863730682848,
-            "angularVelocity": -1.1015847587195386,
-            "velocityX": 0.08495227925044571,
-            "velocityY": 3.10313894819415,
-            "timestamp": 0.5307217588003637
-        },
-        {
-            "x": 4.639893034223715,
-            "y": 4.389369622514019,
-            "heading": -1.0106655427651627,
-            "angularVelocity": 0.08498643737080536,
-            "velocityX": -0.7351977563685651,
-            "velocityY": 3.1081993126412395,
-            "timestamp": 0.6368661105604364
-        },
-        {
-            "x": 4.471904603111427,
-            "y": 4.710090421849215,
-            "heading": -0.9107146957571205,
-            "angularVelocity": 0.9416501712118399,
-            "velocityX": -1.5826412647185075,
-            "velocityY": 3.021553139823678,
-            "timestamp": 0.7430104623205092
-        },
-        {
-            "x": 4.218561716713538,
-            "y": 4.992188206454831,
-            "heading": -0.8355436069817835,
-            "angularVelocity": 0.7081967860640641,
-            "velocityX": -2.3867768957743736,
-            "velocityY": 2.657680601255776,
-            "timestamp": 0.8491548140805819
-        },
-        {
-            "x": 3.8896224481808446,
-            "y": 5.222473200709689,
-            "heading": -0.83554222765338,
-            "angularVelocity": 0.000012994835623464205,
-            "velocityX": -3.098980426921093,
-            "velocityY": 2.169545439171289,
-            "timestamp": 0.9552991658406547
-        },
-        {
-            "x": 3.516178298161353,
-            "y": 5.370026719708121,
-            "heading": -0.8355421446101063,
-            "angularVelocity": 7.823616840140413e-7,
-            "velocityX": -3.518266811442025,
-            "velocityY": 1.390121250464274,
-            "timestamp": 1.0614435176007273
-        },
-        {
-            "x": 3.1189612059977283,
-            "y": 5.42877328608909,
-            "heading": -0.8355420737846555,
-            "angularVelocity": 6.672559542292647e-7,
-            "velocityX": -3.742234848835754,
-            "velocityY": 0.55345918465599,
-            "timestamp": 1.1675878693608
-        },
-        {
-            "x": 2.774416222135212,
-            "y": 5.484170620762628,
-            "heading": -0.7035735462776811,
-            "angularVelocity": 1.243292980914089,
-            "velocityX": -3.2460039384980304,
-            "velocityY": 0.5219056290320314,
-            "timestamp": 1.2737322211208726
-        },
-        {
-            "x": 2.5095166350816602,
-            "y": 5.523131311595933,
-            "heading": -0.3884677902217159,
-            "angularVelocity": 2.9686530732056857,
-            "velocityX": -2.4956541036901063,
-            "velocityY": 0.3670538298766209,
-            "timestamp": 1.3798765728809452
-        },
-        {
-            "x": 2.335800273937377,
-            "y": 5.547984865708467,
-            "heading": -0.13742924633332027,
-            "angularVelocity": 2.365067379711733,
-            "velocityX": -1.6366048523895966,
-            "velocityY": 0.23414862590816604,
-            "timestamp": 1.4860209246410179
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 1.2947391364164484,
-            "velocityX": -0.8083357476365635,
-            "velocityY": 0.11319617193282665,
-            "timestamp": 1.5921652764010905
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -1.986468774957569e-34,
-            "velocityY": 0,
-            "timestamp": 1.6983096281611632
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S3 To W3.traj b/src/main/deploy/choreo/S3 To W3.traj
deleted file mode 100644
index 26f24eb2..00000000
--- a/src/main/deploy/choreo/S3 To W3.traj	
+++ /dev/null
@@ -1,121 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 0,
-            "velocityX": -3.809622996347125e-33,
-            "velocityY": 4.7029981856297106e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 4.1621681366579715,
-            "y": 2.9362991956112916,
-            "heading": -0.5618109581410129,
-            "angularVelocity": 0.03502487973446616,
-            "velocityX": -0.8430624486077395,
-            "velocityY": -0.5326069988815002,
-            "timestamp": 0.11960189130537771
-        },
-        {
-            "x": 3.949371568032143,
-            "y": 2.832059073063443,
-            "heading": -0.5332490097615643,
-            "angularVelocity": 0.2388085010004113,
-            "velocityX": -1.7792073879709687,
-            "velocityY": -0.8715591485229383,
-            "timestamp": 0.23920378261075542
-        },
-        {
-            "x": 3.6196334559490815,
-            "y": 2.7428786025637635,
-            "heading": -0.4400400521459035,
-            "angularVelocity": 0.7793267865446122,
-            "velocityX": -2.756964028613438,
-            "velocityY": -0.7456443165432319,
-            "timestamp": 0.35880567391613316
-        },
-        {
-            "x": 3.2124362068162218,
-            "y": 2.7443781288173086,
-            "heading": -0.34020495702705544,
-            "angularVelocity": 0.8347283979314292,
-            "velocityX": -3.4046054346513173,
-            "velocityY": 0.012537646664080927,
-            "timestamp": 0.47840756522151084
-        },
-        {
-            "x": 2.829910158877178,
-            "y": 2.8625567119312745,
-            "heading": -0.23363822858373923,
-            "angularVelocity": 0.891012067453128,
-            "velocityX": -3.1983277502054186,
-            "velocityY": 0.9880996180254248,
-            "timestamp": 0.5980094565268885
-        },
-        {
-            "x": 2.5228974587582895,
-            "y": 3.073051960500077,
-            "heading": -0.12255766699698727,
-            "angularVelocity": 0.9287525504349555,
-            "velocityX": -2.566955227614215,
-            "velocityY": 1.7599658857512914,
-            "timestamp": 0.7176113478322662
-        },
-        {
-            "x": 2.307806503132395,
-            "y": 3.359545759981809,
-            "heading": -0.008191563913849259,
-            "angularVelocity": 0.9562231987710198,
-            "velocityX": -1.7983909224036065,
-            "velocityY": 2.395395226236259,
-            "timestamp": 0.8372132391376439
-        },
-        {
-            "x": 2.197079056424326,
-            "y": 3.692587049160117,
-            "heading": -0.017161158826022753,
-            "angularVelocity": -0.07499542703130056,
-            "velocityX": -0.9258001315827844,
-            "velocityY": 2.7845821294578066,
-            "timestamp": 0.9568151304430216
-        },
-        {
-            "x": 2.2005818293568207,
-            "y": 4.024472502975606,
-            "heading": -0.15274865723996348,
-            "angularVelocity": -1.133656808718376,
-            "velocityX": 0.029286935969546842,
-            "velocityY": 2.774918107006309,
-            "timestamp": 1.0764170217483993
-        },
-        {
-            "x": 2.2590478504225837,
-            "y": 4.250579625353832,
-            "heading": -0.3027008584759083,
-            "angularVelocity": -1.2537611203242134,
-            "velocityX": 0.48883859968803833,
-            "velocityY": 1.8904978835235247,
-            "timestamp": 1.196018913053777
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.729914389908579,
-            "velocityX": 0.3424038627687911,
-            "velocityY": 0.914871608232253,
-            "timestamp": 1.3156208043591548
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 3.1982065543283105e-28,
-            "velocityX": 4.1170744495691044e-30,
-            "velocityY": 1.8090480101607624e-30,
-            "timestamp": 1.4352226956645326
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S4 To C1.traj b/src/main/deploy/choreo/S4 To C1.traj
deleted file mode 100644
index f8bd91fc..00000000
--- a/src/main/deploy/choreo/S4 To C1.traj	
+++ /dev/null
@@ -1,292 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 1.1411819845129358e-30,
-            "velocityX": 2.0257584812603577e-32,
-            "velocityY": 3.042151659469263e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 5.128157808100627,
-            "y": 1.6381505638691025,
-            "heading": -0.6351424860741187,
-            "angularVelocity": 0.06199634399134786,
-            "velocityX": -0.6106088487961494,
-            "velocityY": 0.23165524966799825,
-            "timestamp": 0.07835161903352221
-        },
-        {
-            "x": 5.034138141125347,
-            "y": 1.6783506940439978,
-            "heading": -0.625731099901857,
-            "angularVelocity": 0.12011731586854772,
-            "velocityX": -1.199970953185457,
-            "velocityY": 0.5130733821555872,
-            "timestamp": 0.15670323806704442
-        },
-        {
-            "x": 4.897649658060979,
-            "y": 1.7471051706265737,
-            "heading": -0.6123788413002932,
-            "angularVelocity": 0.1704145844880519,
-            "velocityX": -1.74199952404266,
-            "velocityY": 0.8775118808095047,
-            "timestamp": 0.23505485710056662
-        },
-        {
-            "x": 4.7291295198915435,
-            "y": 1.855780594555545,
-            "heading": -0.5966040827715752,
-            "angularVelocity": 0.20133289807283902,
-            "velocityX": -2.150818837544829,
-            "velocityY": 1.3870220586313022,
-            "timestamp": 0.31340647613408884
-        },
-        {
-            "x": 4.559409610033689,
-            "y": 2.0156280635815147,
-            "heading": -0.5822183456142529,
-            "angularVelocity": 0.18360484869071408,
-            "velocityX": -2.1661314973624393,
-            "velocityY": 2.0401297509574126,
-            "timestamp": 0.39175809516761106
-        },
-        {
-            "x": 4.423281260574217,
-            "y": 2.2140742145732073,
-            "heading": -0.5727758831618049,
-            "angularVelocity": 0.1205139417528553,
-            "velocityX": -1.7374031467203144,
-            "velocityY": 2.532763884646578,
-            "timestamp": 0.4701097142011333
-        },
-        {
-            "x": 4.3324586171803325,
-            "y": 2.4362881098121476,
-            "heading": -0.5691499320560302,
-            "angularVelocity": 0.04627793465535804,
-            "velocityX": -1.1591674111421555,
-            "velocityY": 2.8361110846205655,
-            "timestamp": 0.5484613332346555
-        },
-        {
-            "x": 4.290356765831772,
-            "y": 2.674111890908862,
-            "heading": -0.5714790836196856,
-            "angularVelocity": -0.02972691046318988,
-            "velocityX": -0.5373450079001806,
-            "velocityY": 3.0353397164002804,
-            "timestamp": 0.6268129522681777
-        },
-        {
-            "x": 4.298268630722052,
-            "y": 2.9211188280591673,
-            "heading": -0.5947192771376703,
-            "angularVelocity": -0.29661408155511804,
-            "velocityX": 0.1009789585444941,
-            "velocityY": 3.1525441362561364,
-            "timestamp": 0.7051645713016998
-        },
-        {
-            "x": 4.356924477851345,
-            "y": 3.1722484967201265,
-            "heading": -0.6368890523591307,
-            "angularVelocity": -0.5382119188043637,
-            "velocityX": 0.7486232939768359,
-            "velocityY": 3.2051624683532776,
-            "timestamp": 0.783516190335222
-        },
-        {
-            "x": 4.466386404708156,
-            "y": 3.418800428561669,
-            "heading": -0.694172117002664,
-            "angularVelocity": -0.7311025011368958,
-            "velocityX": 1.3970601782967371,
-            "velocityY": 3.1467369134523744,
-            "timestamp": 0.8618678093687442
-        },
-        {
-            "x": 4.622742851027307,
-            "y": 3.644917946871822,
-            "heading": -0.7581397869413499,
-            "angularVelocity": -0.8164179723116849,
-            "velocityX": 1.9955739045067467,
-            "velocityY": 2.8859329404975034,
-            "timestamp": 0.9402194284022664
-        },
-        {
-            "x": 4.8130531116785615,
-            "y": 3.833426042269654,
-            "heading": -0.8482959054101304,
-            "angularVelocity": -1.1506605681014435,
-            "velocityX": 2.4289256941816704,
-            "velocityY": 2.4059246984696085,
-            "timestamp": 1.0185710474357885
-        },
-        {
-            "x": 5.034329891204834,
-            "y": 3.981222629547119,
-            "heading": -0.9454274313666489,
-            "angularVelocity": -1.2396875412997066,
-            "velocityX": 2.824150697276595,
-            "velocityY": 1.8863246107809373,
-            "timestamp": 1.0969226664693108
-        },
-        {
-            "x": 5.3307679727813255,
-            "y": 4.095215339701575,
-            "heading": -1.0325645415374454,
-            "angularVelocity": -0.9491279076997933,
-            "velocityX": 3.2289073573561264,
-            "velocityY": 1.241648503948074,
-            "timestamp": 1.1887302189223545
-        },
-        {
-            "x": 5.6706736122505745,
-            "y": 4.158722341660029,
-            "heading": -1.0366531166779425,
-            "angularVelocity": -0.04453419170049298,
-            "velocityX": 3.7023712144281196,
-            "velocityY": 0.691740496958957,
-            "timestamp": 1.2805377713753983
-        },
-        {
-            "x": 5.985432145120126,
-            "y": 4.2874622186845945,
-            "heading": -1.0150469515971576,
-            "angularVelocity": 0.23534191363870294,
-            "velocityX": 3.4284601262030105,
-            "velocityY": 1.402279807975625,
-            "timestamp": 1.372345323828442
-        },
-        {
-            "x": 6.260473058973373,
-            "y": 4.473603429076733,
-            "heading": -0.9641067919300658,
-            "angularVelocity": 0.5548580515001295,
-            "velocityX": 2.9958419160985765,
-            "velocityY": 2.027515225257119,
-            "timestamp": 1.4641528762814857
-        },
-        {
-            "x": 6.487005993334624,
-            "y": 4.6803674717401,
-            "heading": -0.8309910438969466,
-            "angularVelocity": 1.4499433268434323,
-            "velocityX": 2.4674760224886074,
-            "velocityY": 2.2521463337029823,
-            "timestamp": 1.5559604287345294
-        },
-        {
-            "x": 6.71888754498383,
-            "y": 4.9198952355114685,
-            "heading": -0.7843851972685187,
-            "angularVelocity": 0.507647196588375,
-            "velocityX": 2.5257350343568326,
-            "velocityY": 2.609020253468568,
-            "timestamp": 1.6477679811875732
-        },
-        {
-            "x": 6.906293835583718,
-            "y": 5.2122954621107285,
-            "heading": -0.7843850302000089,
-            "angularVelocity": 0.0000018197686939661552,
-            "velocityX": 2.0412949217411986,
-            "velocityY": 3.1849256274293083,
-            "timestamp": 1.739575533640617
-        },
-        {
-            "x": 7.030998336898649,
-            "y": 5.536437116039629,
-            "heading": -0.7843849409482478,
-            "angularVelocity": 9.721614257680115e-7,
-            "velocityX": 1.3583250830993825,
-            "velocityY": 3.530664365490929,
-            "timestamp": 1.8313830860936606
-        },
-        {
-            "x": 7.139916048661206,
-            "y": 5.86621879705104,
-            "heading": -0.7843848550588985,
-            "angularVelocity": 9.355368597772783e-7,
-            "velocityX": 1.1863698448802928,
-            "velocityY": 3.592097514854049,
-            "timestamp": 1.9231906385467044
-        },
-        {
-            "x": 7.248833621341376,
-            "y": 6.196000523997438,
-            "heading": -0.7843847691695015,
-            "angularVelocity": 9.355373786921671e-7,
-            "velocityX": 1.1863683299462482,
-            "velocityY": 3.592098015194004,
-            "timestamp": 2.0149981909997483
-        },
-        {
-            "x": 7.357751176995467,
-            "y": 6.5257822565670125,
-            "heading": -0.7843846832799689,
-            "angularVelocity": 9.355388566444766e-7,
-            "velocityX": 1.1863681444922285,
-            "velocityY": 3.5920980764436137,
-            "timestamp": 2.1068057434527923
-        },
-        {
-            "x": 7.4669549324183055,
-            "y": 6.834400664448086,
-            "heading": -0.7318192896940902,
-            "angularVelocity": 0.5725606682823186,
-            "velocityX": 1.189485532562175,
-            "velocityY": 3.361579735380924,
-            "timestamp": 2.198613295905836
-        },
-        {
-            "x": 7.555413306559794,
-            "y": 7.089894078912264,
-            "heading": -0.5139853289463429,
-            "angularVelocity": 2.3727237566774426,
-            "velocityX": 0.9635195773978621,
-            "velocityY": 2.782923709842436,
-            "timestamp": 2.29042084835888
-        },
-        {
-            "x": 7.619158448834915,
-            "y": 7.279787689667393,
-            "heading": -0.2782484542424907,
-            "angularVelocity": 2.5677285626846773,
-            "velocityX": 0.6943344046528581,
-            "velocityY": 2.0683876835977384,
-            "timestamp": 2.382228400811924
-        },
-        {
-            "x": 7.660128944224933,
-            "y": 7.404974004983973,
-            "heading": -0.09793823848426357,
-            "angularVelocity": 1.964001990472943,
-            "velocityX": 0.44626497815605787,
-            "velocityY": 1.3635731698719276,
-            "timestamp": 2.474035953264968
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 2.533782828853193e-32,
-            "angularVelocity": 1.0667775783953661,
-            "velocityX": 0.21644249567844834,
-            "velocityY": 0.6756088508922024,
-            "timestamp": 2.565843505718012
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 2.4912680070874024e-31,
-            "velocityX": 0,
-            "velocityY": -3.645397861822831e-32,
-            "timestamp": 2.657651058171056
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S4 To C2.traj b/src/main/deploy/choreo/S4 To C2.traj
deleted file mode 100644
index 0e0e8d38..00000000
--- a/src/main/deploy/choreo/S4 To C2.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -2.491480567253294e-33,
-            "velocityX": -2.0103104784295923e-34,
-            "velocityY": 1.605618083193578e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 5.240158813928264,
-            "y": 1.6204190327819517,
-            "heading": -0.5237338108165747,
-            "angularVelocity": 1.2526389382742513,
-            "velocityX": 0.6912398963489377,
-            "velocityY": 0.004514612397400031,
-            "timestamp": 0.0928170006782674
-        },
-        {
-            "x": 5.369626464101249,
-            "y": 1.6247166229091008,
-            "heading": -0.2974184918320366,
-            "angularVelocity": 2.4382959730515035,
-            "velocityX": 1.394870004707003,
-            "velocityY": 0.046301756097956205,
-            "timestamp": 0.1856340013565348
-        },
-        {
-            "x": 5.5674551093283755,
-            "y": 1.639197763374398,
-            "heading": 0.0009013327241277373,
-            "angularVelocity": 3.214064475001015,
-            "velocityX": 2.1313837312289596,
-            "velocityY": 0.15601819019657054,
-            "timestamp": 0.2784510020348022
-        },
-        {
-            "x": 5.831585704851482,
-            "y": 1.6774487923340957,
-            "heading": 0.2480008567342784,
-            "angularVelocity": 2.6622226769282764,
-            "velocityX": 2.8457135394696165,
-            "velocityY": 0.41211231434085943,
-            "timestamp": 0.3712680027130696
-        },
-        {
-            "x": 6.141513989428359,
-            "y": 1.75469627200855,
-            "heading": 0.3453177753240257,
-            "angularVelocity": 1.0484816130514494,
-            "velocityX": 3.339132726893265,
-            "velocityY": 0.8322557194259916,
-            "timestamp": 0.46408500339133696
-        },
-        {
-            "x": 6.452443961209774,
-            "y": 1.9008832260175597,
-            "heading": 0.36957097138497697,
-            "angularVelocity": 0.2613012258930921,
-            "velocityX": 3.3499247929718696,
-            "velocityY": 1.5750019171136467,
-            "timestamp": 0.5569020040696043
-        },
-        {
-            "x": 6.733109637262523,
-            "y": 2.111863899724239,
-            "heading": 0.36957109822659895,
-            "angularVelocity": 0.0000013665774702958542,
-            "velocityX": 3.023860650546378,
-            "velocityY": 2.273082217319261,
-            "timestamp": 0.6497190047478717
-        },
-        {
-            "x": 6.964965499466835,
-            "y": 2.3755472934997988,
-            "heading": 0.36957101579118784,
-            "angularVelocity": -8.881499138395027e-7,
-            "velocityX": 2.4979891669630336,
-            "velocityY": 2.840895437782658,
-            "timestamp": 0.7425360054261391
-        },
-        {
-            "x": 7.138307566970259,
-            "y": 2.680897277378083,
-            "heading": 0.36957096673523665,
-            "angularVelocity": -5.285233395413412e-7,
-            "velocityX": 1.867568077364202,
-            "velocityY": 3.289806626446832,
-            "timestamp": 0.8353530061044064
-        },
-        {
-            "x": 7.245881008413397,
-            "y": 3.0151337865477195,
-            "heading": 0.3695709277774119,
-            "angularVelocity": -4.197272531605919e-7,
-            "velocityX": 1.1589842448800958,
-            "velocityY": 3.6010268240427665,
-            "timestamp": 0.9281700067826738
-        },
-        {
-            "x": 7.300266060372569,
-            "y": 3.36201756600148,
-            "heading": 0.3695708905369613,
-            "angularVelocity": -4.012244559973519e-7,
-            "velocityX": 0.5859384763755527,
-            "velocityY": 3.7372871017042186,
-            "timestamp": 1.0209870074609413
-        },
-        {
-            "x": 7.354650396010165,
-            "y": 3.708901457760651,
-            "heading": 0.3695708532965109,
-            "angularVelocity": -4.0122445392101915e-7,
-            "velocityX": 0.5859307588068667,
-            "velocityY": 3.7372883116701696,
-            "timestamp": 1.1138040081392087
-        },
-        {
-            "x": 7.409034640641628,
-            "y": 4.055785363787717,
-            "heading": 0.36957081605605097,
-            "angularVelocity": -4.012245569815441e-7,
-            "velocityX": 0.5859297783169686,
-            "velocityY": 3.737288465390881,
-            "timestamp": 1.206621008817476
-        },
-        {
-            "x": 7.463418795891099,
-            "y": 4.402669283828022,
-            "heading": 0.36957077881557854,
-            "angularVelocity": -4.0122469066112833e-7,
-            "velocityX": 0.5859288153253637,
-            "velocityY": 3.7372886163679513,
-            "timestamp": 1.2994380094957434
-        },
-        {
-            "x": 7.517802854177964,
-            "y": 4.7495532190700205,
-            "heading": 0.3695707415750949,
-            "angularVelocity": -4.0122481249990114e-7,
-            "velocityX": 0.5859277706610763,
-            "velocityY": 3.737288780149305,
-            "timestamp": 1.3922550101740108
-        },
-        {
-            "x": 7.56989445358868,
-            "y": 5.095450165697103,
-            "heading": 0.36620989603022114,
-            "angularVelocity": -0.036209374579162214,
-            "velocityX": 0.5612290747390308,
-            "velocityY": 3.7266550750338174,
-            "timestamp": 1.4850720108522781
-        },
-        {
-            "x": 7.614129631330889,
-            "y": 5.376850109081837,
-            "heading": 0.24917441522367106,
-            "angularVelocity": -1.2609272003114111,
-            "velocityX": 0.4765848650458117,
-            "velocityY": 3.0317715647821197,
-            "timestamp": 1.5778890115305455
-        },
-        {
-            "x": 7.6471559939174325,
-            "y": 5.587299345919279,
-            "heading": 0.1342077337923999,
-            "angularVelocity": -1.2386381868746383,
-            "velocityX": 0.35582234229936965,
-            "velocityY": 2.267356575838128,
-            "timestamp": 1.6707060122088129
-        },
-        {
-            "x": 7.669079055841979,
-            "y": 5.72720580380319,
-            "heading": 0.04709984861251457,
-            "angularVelocity": -0.9384906271840042,
-            "velocityX": 0.23619662092442378,
-            "velocityY": 1.5073365532341503,
-            "timestamp": 1.7635230128870802
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -9.59663427624948e-34,
-            "angularVelocity": -0.5074485090913172,
-            "velocityX": 0.11766103276571077,
-            "velocityY": 0.751954875580805,
-            "timestamp": 1.8563400135653476
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 0,
-            "angularVelocity": 1.9250732473517817e-32,
-            "velocityX": 0,
-            "velocityY": 2.1529996497623123e-34,
-            "timestamp": 1.949157014243615
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S4 To C3.traj b/src/main/deploy/choreo/S4 To C3.traj
deleted file mode 100644
index 4d225c1e..00000000
--- a/src/main/deploy/choreo/S4 To C3.traj	
+++ /dev/null
@@ -1,238 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0,
-            "velocityX": 2.653642496841617e-31,
-            "velocityY": -1.2761459725684476e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 5.194122324075795,
-            "y": 1.6116215778070426,
-            "heading": -0.6335243646936302,
-            "angularVelocity": 0.13206571622689564,
-            "velocityX": 0.3695911822570569,
-            "velocityY": -0.1708716250073132,
-            "timestamp": 0.049033431926404414
-        },
-        {
-            "x": 5.2306889078335095,
-            "y": 1.5955898422898989,
-            "heading": -0.6207226842720189,
-            "angularVelocity": 0.2610806529090014,
-            "velocityX": 0.745747999295838,
-            "velocityY": -0.3269551994890449,
-            "timestamp": 0.09806686385280883
-        },
-        {
-            "x": 5.286073830286703,
-            "y": 1.5728623577205654,
-            "heading": -0.6018022859553105,
-            "angularVelocity": 0.38586730672362957,
-            "velocityX": 1.1295338767299938,
-            "velocityY": -0.4635099701654054,
-            "timestamp": 0.14710029577921324
-        },
-        {
-            "x": 5.360700050095615,
-            "y": 1.5447484727758143,
-            "heading": -0.577064216904969,
-            "angularVelocity": 0.5045143298894789,
-            "velocityX": 1.5219456782249914,
-            "velocityY": -0.5733615584348081,
-            "timestamp": 0.19613372770561766
-        },
-        {
-            "x": 5.45500431506412,
-            "y": 1.513111103841015,
-            "heading": -0.5469692052395991,
-            "angularVelocity": 0.6137651492712723,
-            "velocityX": 1.9232646230039372,
-            "velocityY": -0.6452203668359155,
-            "timestamp": 0.24516715963202207
-        },
-        {
-            "x": 5.5692993756365,
-            "y": 1.480706991826158,
-            "heading": -0.5122625268421303,
-            "angularVelocity": 0.7078166270271339,
-            "velocityX": 2.330961878090668,
-            "velocityY": -0.6608575158164777,
-            "timestamp": 0.2942005915584265
-        },
-        {
-            "x": 5.703331364670941,
-            "y": 1.4516882815399292,
-            "heading": -0.47420565589215874,
-            "angularVelocity": 0.7761412867722233,
-            "velocityX": 2.733481703578104,
-            "velocityY": -0.5918147913809916,
-            "timestamp": 0.3432340234848309
-        },
-        {
-            "x": 5.855159622936256,
-            "y": 1.4318923943368613,
-            "heading": -0.4349045247507558,
-            "angularVelocity": 0.8015170384282412,
-            "velocityX": 3.096423242273411,
-            "velocityY": -0.40372224470801127,
-            "timestamp": 0.3922674554112353
-        },
-        {
-            "x": 6.019815732483991,
-            "y": 1.4274954608427641,
-            "heading": -0.3972296573283927,
-            "angularVelocity": 0.768350611865634,
-            "velocityX": 3.358037630221609,
-            "velocityY": -0.08967215471871418,
-            "timestamp": 0.4413008873376397
-        },
-        {
-            "x": 6.190507691154086,
-            "y": 1.4421792499476667,
-            "heading": -0.36375877276285506,
-            "angularVelocity": 0.6826135404074718,
-            "velocityX": 3.4811342376834093,
-            "velocityY": 0.2994648452702519,
-            "timestamp": 0.49033431926404414
-        },
-        {
-            "x": 6.361514712702491,
-            "y": 1.4768435219754552,
-            "heading": -0.33601787986089265,
-            "angularVelocity": 0.5657546659914753,
-            "velocityX": 3.487559708345244,
-            "velocityY": 0.7069517809772359,
-            "timestamp": 0.5393677511904486
-        },
-        {
-            "x": 6.529238224029541,
-            "y": 1.531233787536621,
-            "heading": -0.3133159497925571,
-            "angularVelocity": 0.4629888052692109,
-            "velocityX": 3.4205949846286416,
-            "velocityY": 1.1092485968081267,
-            "timestamp": 0.588401183116853
-        },
-        {
-            "x": 6.794330666624461,
-            "y": 1.6764087741891647,
-            "heading": -0.29089151826229404,
-            "angularVelocity": 0.27241251742532613,
-            "velocityX": 3.2203491776466797,
-            "velocityY": 1.7635891250039288,
-            "timestamp": 0.6707191020664587
-        },
-        {
-            "x": 7.034738463760469,
-            "y": 1.8721289187979775,
-            "heading": -0.28740556521383775,
-            "angularVelocity": 0.042347438965477144,
-            "velocityX": 2.9204795286821774,
-            "velocityY": 2.377612883150898,
-            "timestamp": 0.7530370210160644
-        },
-        {
-            "x": 7.236654527584481,
-            "y": 2.109198968129657,
-            "heading": -0.2874055261243408,
-            "angularVelocity": 4.748601214403234e-7,
-            "velocityX": 2.452881054338489,
-            "velocityY": 2.879932490479633,
-            "timestamp": 0.8353549399656701
-        },
-        {
-            "x": 7.392410351066591,
-            "y": 2.3788514084218195,
-            "heading": -0.2874055078665042,
-            "angularVelocity": 2.2179662469221753e-7,
-            "velocityX": 1.8921253776776576,
-            "velocityY": 3.2757441360636306,
-            "timestamp": 0.9176728589152758
-        },
-        {
-            "x": 7.49687844031107,
-            "y": 2.6722091028580075,
-            "heading": -0.28740549372573876,
-            "angularVelocity": 1.7178234776742224e-7,
-            "velocityX": 1.2690807855393922,
-            "velocityY": 3.5637161164844247,
-            "timestamp": 0.9999907778648816
-        },
-        {
-            "x": 7.546620338444338,
-            "y": 2.979614475892834,
-            "heading": -0.28740548096651763,
-            "angularVelocity": 1.5499931641741705e-7,
-            "velocityX": 0.6042657390764874,
-            "velocityY": 3.7343676438541644,
-            "timestamp": 1.0823086968144873
-        },
-        {
-            "x": 7.582493232504898,
-            "y": 3.288945132948743,
-            "heading": -0.2874054683444637,
-            "angularVelocity": 1.5333300623146833e-7,
-            "velocityX": 0.43578475401492967,
-            "velocityY": 3.7577560390622633,
-            "timestamp": 1.164626615764093
-        },
-        {
-            "x": 7.6150468277369106,
-            "y": 3.5683420563080905,
-            "heading": -0.1936159509616472,
-            "angularVelocity": 1.1393572454157397,
-            "velocityX": 0.39546183440153926,
-            "velocityY": 3.3941203437178378,
-            "timestamp": 1.2469445347136987
-        },
-        {
-            "x": 7.641044736890772,
-            "y": 3.791820131624945,
-            "heading": -0.11680359430817472,
-            "angularVelocity": 0.9331183007718732,
-            "velocityX": 0.31582320697043825,
-            "velocityY": 2.714816873027222,
-            "timestamp": 1.3292624536633044
-        },
-        {
-            "x": 7.660527962829294,
-            "y": 3.959415202649684,
-            "heading": -0.05861701943494889,
-            "angularVelocity": 0.7068518691380767,
-            "velocityX": 0.23668268327338735,
-            "velocityY": 2.035948833052459,
-            "timestamp": 1.4115803726129101
-        },
-        {
-            "x": 7.673510674167818,
-            "y": 4.0711397209924485,
-            "heading": -0.019593791766857216,
-            "angularVelocity": 0.4740550801809447,
-            "velocityX": 0.1577142802460941,
-            "velocityY": 1.3572320555281903,
-            "timestamp": 1.4938982915625159
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -2.071552209824535e-33,
-            "angularVelocity": 0.23802583953553896,
-            "velocityX": 0.07883248161483059,
-            "velocityY": 0.6785919726876813,
-            "timestamp": 1.5762162105121216
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -4.785738309812717e-31,
-            "velocityX": -1.2484093100405624e-30,
-            "velocityY": 4.284452606087597e-31,
-            "timestamp": 1.6585341294617273
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S4 To C4.traj b/src/main/deploy/choreo/S4 To C4.traj
deleted file mode 100644
index 751754fd..00000000
--- a/src/main/deploy/choreo/S4 To C4.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.222401190948388,
-            "y": 1.635609178560311,
-            "heading": -0.6120137040899031,
-            "angularVelocity": 0.36292240482976423,
-            "velocityX": 0.6017242103081577,
-            "velocityY": 0.20241766322785323,
-            "timestamp": 0.07711371780208913
-        },
-        {
-            "x": 5.315327100770923,
-            "y": 1.6668589219810437,
-            "heading": -0.5590312951883875,
-            "angularVelocity": 0.6870685321838068,
-            "velocityX": 1.205050313629373,
-            "velocityY": 0.4052423396435706,
-            "timestamp": 0.15422743560417826
-        },
-        {
-            "x": 5.454938302762681,
-            "y": 1.713800132384211,
-            "heading": -0.4856728473163301,
-            "angularVelocity": 0.9513021802467544,
-            "velocityX": 1.8104587091763231,
-            "velocityY": 0.6087271077195515,
-            "timestamp": 0.23134115340626737
-        },
-        {
-            "x": 5.641435450684747,
-            "y": 1.7765099385015581,
-            "heading": -0.4002091399727602,
-            "angularVelocity": 1.1082815065785667,
-            "velocityX": 2.418469154874715,
-            "velocityY": 0.8132120704942623,
-            "timestamp": 0.3084548712083565
-        },
-        {
-            "x": 5.8748863814192855,
-            "y": 1.8550365409977465,
-            "heading": -0.32154781798640775,
-            "angularVelocity": 1.0200691164734093,
-            "velocityX": 3.0273592998548686,
-            "velocityY": 1.0183220928048784,
-            "timestamp": 0.38556858901044566
-        },
-        {
-            "x": 6.151206632813102,
-            "y": 1.945953456728306,
-            "heading": -0.3195100296755088,
-            "angularVelocity": 0.026425756259478697,
-            "velocityX": 3.583282705976986,
-            "velocityY": 1.178997956808355,
-            "timestamp": 0.4626823068125348
-        },
-        {
-            "x": 6.4277763897122595,
-            "y": 2.0387313847088295,
-            "heading": -0.31950999788997536,
-            "angularVelocity": 4.121903903341971e-7,
-            "velocityX": 3.5865182587742526,
-            "velocityY": 1.2031313056210848,
-            "timestamp": 0.539796024614624
-        },
-        {
-            "x": 6.7043461469693755,
-            "y": 2.1315093116222874,
-            "heading": -0.31950996610444216,
-            "angularVelocity": 4.121903870025697e-7,
-            "velocityX": 3.5865182634161936,
-            "velocityY": 1.2031312917835268,
-            "timestamp": 0.616909742416713
-        },
-        {
-            "x": 6.980831721332989,
-            "y": 2.2223162260189957,
-            "heading": -0.31777519026878787,
-            "angularVelocity": 0.022496332495664188,
-            "velocityX": 3.5854265913259273,
-            "velocityY": 1.1775714747635835,
-            "timestamp": 0.6940234602188021
-        },
-        {
-            "x": 7.214355031558839,
-            "y": 2.3007720927806434,
-            "heading": -0.23870969056825803,
-            "angularVelocity": 1.0253104370281303,
-            "velocityX": 3.028297907062184,
-            "velocityY": 1.0174048016074528,
-            "timestamp": 0.7711371780208912
-        },
-        {
-            "x": 7.400917361819277,
-            "y": 2.3634042679352882,
-            "heading": -0.15373574527352316,
-            "angularVelocity": 1.1019303402387481,
-            "velocityX": 2.419314430400663,
-            "velocityY": 0.8122053629341165,
-            "timestamp": 0.8482508958229803
-        },
-        {
-            "x": 7.540592119323217,
-            "y": 2.4102654434053137,
-            "heading": -0.08076255152981597,
-            "angularVelocity": 0.9463062581289249,
-            "velocityX": 1.8112828882458099,
-            "velocityY": 0.6076892258040765,
-            "timestamp": 0.9253646136250694
-        },
-        {
-            "x": 7.633568850123268,
-            "y": 2.4414404537843537,
-            "heading": -0.02793953661151031,
-            "angularVelocity": 0.6850015331107969,
-            "velocityX": 1.2057093530190521,
-            "velocityY": 0.4042732119212608,
-            "timestamp": 1.0024783314271586
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -3.3818227179407965e-32,
-            "angularVelocity": 0.3623160367292381,
-            "velocityX": 0.6021127135368662,
-            "velocityY": 0.20177403786417583,
-            "timestamp": 1.0795920492292477
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -1.7311819232040874e-32,
-            "angularVelocity": 2.140534191184349e-31,
-            "velocityX": 0,
-            "velocityY": -2.0522160776390996e-31,
-            "timestamp": 1.1567057670313368
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S4 To C5.traj b/src/main/deploy/choreo/S4 To C5.traj
deleted file mode 100644
index 0bd0e944..00000000
--- a/src/main/deploy/choreo/S4 To C5.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.22245562506892,
-            "y": 1.6045082039129626,
-            "heading": -0.6133597798196372,
-            "angularVelocity": 0.3456549186542876,
-            "velocityX": 0.6027583554308628,
-            "velocityY": -0.20100492713723658,
-            "timestamp": 0.07707172310487943
-        },
-        {
-            "x": 5.31546298986369,
-            "y": 1.573504853521539,
-            "heading": -0.5625082909022232,
-            "angularVelocity": 0.6597943690478437,
-            "velocityX": 1.2067637915426601,
-            "velocityY": -0.4022662157070755,
-            "timestamp": 0.15414344620975887
-        },
-        {
-            "x": 5.455153171011431,
-            "y": 1.526958597742617,
-            "heading": -0.4912836919139272,
-            "angularVelocity": 0.924133989989731,
-            "velocityX": 1.8124699373549893,
-            "velocityY": -0.6039342822993798,
-            "timestamp": 0.2312151693146383
-        },
-        {
-            "x": 5.641701159953251,
-            "y": 1.4648201004985182,
-            "heading": -0.406509898431932,
-            "angularVelocity": 1.0999338027856826,
-            "velocityX": 2.420446584384324,
-            "velocityY": -0.8062424809101578,
-            "timestamp": 0.30828689241951773
-        },
-        {
-            "x": 5.875244890448534,
-            "y": 1.3870507382512116,
-            "heading": -0.323292875073049,
-            "angularVelocity": 1.0797348236997513,
-            "velocityX": 3.03021291190643,
-            "velocityY": -1.0090518171168696,
-            "timestamp": 0.38535861552439715
-        },
-        {
-            "x": 6.151315012657669,
-            "y": 1.295160148354896,
-            "heading": -0.32123146568205607,
-            "angularVelocity": 0.026746636872095383,
-            "velocityX": 3.5819897504232276,
-            "velocityY": -1.192273718485189,
-            "timestamp": 0.46243033862927657
-        },
-        {
-            "x": 6.427941847872596,
-            "y": 1.2030541885235448,
-            "heading": -0.3212314367193528,
-            "angularVelocity": 3.757889680371681e-7,
-            "velocityX": 3.5892130611702004,
-            "velocityY": -1.1950681277232245,
-            "timestamp": 0.539502061734156
-        },
-        {
-            "x": 6.704568684105482,
-            "y": 1.1109482317494896,
-            "heading": -0.3212314077566493,
-            "angularVelocity": 3.7578897090221527e-7,
-            "velocityX": 3.589213074378161,
-            "velocityY": -1.1950680880550335,
-            "timestamp": 0.6165737848390355
-        },
-        {
-            "x": 6.9807077062893965,
-            "y": 1.0195126541212949,
-            "heading": -0.3189025007582472,
-            "angularVelocity": 0.030217398866675224,
-            "velocityX": 3.582883722583232,
-            "velocityY": -1.1863699674103443,
-            "timestamp": 0.6936455079439149
-        },
-        {
-            "x": 7.214256981896637,
-            "y": 0.9418352430707998,
-            "heading": -0.2350676331360113,
-            "angularVelocity": 1.0877513080660361,
-            "velocityX": 3.0302848593306506,
-            "velocityY": -1.0078587570280646,
-            "timestamp": 0.7707172310487943
-        },
-        {
-            "x": 7.4008131962231865,
-            "y": 0.8797969152412516,
-            "heading": -0.14969687190170503,
-            "angularVelocity": 1.1076794159400525,
-            "velocityX": 2.4205533081527695,
-            "velocityY": -0.8049427900441033,
-            "timestamp": 0.8477889541536737
-        },
-        {
-            "x": 7.54051565231881,
-            "y": 0.8333524229780926,
-            "heading": -0.0780215306756512,
-            "angularVelocity": 0.9299823377312819,
-            "velocityX": 1.812629203910703,
-            "velocityY": -0.6026139080860716,
-            "timestamp": 0.9248606772585531
-        },
-        {
-            "x": 7.633536087871107,
-            "y": 0.802437413208921,
-            "heading": -0.026843150662094223,
-            "angularVelocity": 0.6640357572376224,
-            "velocityX": 1.2069333836706095,
-            "velocityY": -0.401120002560503,
-            "timestamp": 1.0019324003634327
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 3.8658238709310755e-33,
-            "angularVelocity": 0.3482879268907221,
-            "velocityX": 0.6028658794310917,
-            "velocityY": -0.20029931325025216,
-            "timestamp": 1.079004123468312
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 4.381697144346615e-34,
-            "angularVelocity": -4.4473487408728983e-32,
-            "velocityX": 0,
-            "velocityY": -1.5918634852021244e-32,
-            "timestamp": 1.1560758465731915
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S4 To W1.traj b/src/main/deploy/choreo/S4 To W1.traj
deleted file mode 100644
index 90e7d73f..00000000
--- a/src/main/deploy/choreo/S4 To W1.traj	
+++ /dev/null
@@ -1,247 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 0,
-            "velocityX": -4.2485019622555225e-35,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 5.123544064624381,
-            "y": 1.640479835564366,
-            "heading": -0.5261105113915723,
-            "angularVelocity": 1.3044458865146935,
-            "velocityX": -0.6008098724480858,
-            "velocityY": 0.23456806756139406,
-            "timestamp": 0.08730871076049408
-        },
-        {
-            "x": 5.016418569113309,
-            "y": 1.6815339921073649,
-            "heading": -0.31680443246750034,
-            "angularVelocity": 2.397310384049101,
-            "velocityX": -1.2269737415426907,
-            "velocityY": 0.4702183342922002,
-            "timestamp": 0.17461742152098816
-        },
-        {
-            "x": 4.8522086939147036,
-            "y": 1.7459427773647251,
-            "heading": -0.051891218250848345,
-            "angularVelocity": 3.0342128741697256,
-            "velocityX": -1.8807960141464901,
-            "velocityY": 0.7377131639710823,
-            "timestamp": 0.26192613228148226
-        },
-        {
-            "x": 4.632595451769563,
-            "y": 1.839668047804236,
-            "heading": 0.17554509517769362,
-            "angularVelocity": 2.6049670353333583,
-            "velocityX": -2.515364620920639,
-            "velocityY": 1.073492777789593,
-            "timestamp": 0.34923484304197633
-        },
-        {
-            "x": 4.3704901470738005,
-            "y": 1.9627849470607086,
-            "heading": 0.27538033292006053,
-            "angularVelocity": 1.1434739658020574,
-            "velocityX": -3.002052171114658,
-            "velocityY": 1.4101330575617794,
-            "timestamp": 0.4365435538024704
-        },
-        {
-            "x": 4.075718453010027,
-            "y": 2.1117712847634245,
-            "heading": 0.2753804447713452,
-            "angularVelocity": 0.0000012811010915301473,
-            "velocityX": -3.3762002839830365,
-            "velocityY": 1.7064315393617047,
-            "timestamp": 0.5238522645629645
-        },
-        {
-            "x": 3.780946849777969,
-            "y": 2.260757802178556,
-            "heading": 0.2753805566219007,
-            "angularVelocity": 0.0000012810927397041138,
-            "velocityX": -3.3761992436319233,
-            "velocityY": 1.7064335977177825,
-            "timestamp": 0.6111609753234586
-        },
-        {
-            "x": 3.4861752863833098,
-            "y": 2.409744398412427,
-            "heading": 0.27538066847245973,
-            "angularVelocity": 0.0000012810927810381003,
-            "velocityX": -3.3761987873498507,
-            "velocityY": 1.7064345004769594,
-            "timestamp": 0.6984696860839527
-        },
-        {
-            "x": 3.191403823869153,
-            "y": 2.5587311942392557,
-            "heading": 0.2753807803230405,
-            "angularVelocity": 0.000001281093028963251,
-            "velocityX": -3.3761976319038296,
-            "velocityY": 1.7064367865370367,
-            "timestamp": 0.7857783968444467
-        },
-        {
-            "x": 2.907963654667952,
-            "y": 2.72828634801538,
-            "heading": 0.27538089480545036,
-            "angularVelocity": 0.0000013112369766496227,
-            "velocityX": -3.246413407463273,
-            "velocityY": 1.9420187550501031,
-            "timestamp": 0.8730871076049408
-        },
-        {
-            "x": 2.662249448997808,
-            "y": 2.9489938499602837,
-            "heading": 0.27538103946019826,
-            "angularVelocity": 0.000001656819194794751,
-            "velocityX": -2.81431490088302,
-            "velocityY": 2.527897846875211,
-            "timestamp": 0.9603958183654349
-        },
-        {
-            "x": 2.463361015566044,
-            "y": 3.212680537293485,
-            "heading": 0.27538116270778873,
-            "angularVelocity": 0.0000014116299433562534,
-            "velocityX": -2.277990726232991,
-            "velocityY": 3.0201647125055904,
-            "timestamp": 1.047704529125929
-        },
-        {
-            "x": 2.3186638703555946,
-            "y": 3.509581240155329,
-            "heading": 0.2753811333053821,
-            "angularVelocity": -3.3676372418890626e-7,
-            "velocityX": -1.657304797540575,
-            "velocityY": 3.4005851223287915,
-            "timestamp": 1.1350132398864232
-        },
-        {
-            "x": 2.233516432992889,
-            "y": 3.8287006658490608,
-            "heading": 0.2753811088610493,
-            "angularVelocity": -2.799758765995743e-7,
-            "velocityX": -0.9752456154836949,
-            "velocityY": 3.655069728026828,
-            "timestamp": 1.2223219506469174
-        },
-        {
-            "x": 2.1898375161467403,
-            "y": 4.15608338798651,
-            "heading": 0.27538108549661844,
-            "angularVelocity": -2.6760709935894875e-7,
-            "velocityX": -0.5002813174732252,
-            "velocityY": 3.749714310127982,
-            "timestamp": 1.3096306614074116
-        },
-        {
-            "x": 2.1796859113101297,
-            "y": 4.486211000706444,
-            "heading": 0.2753810837562719,
-            "angularVelocity": -1.9933252293602923e-8,
-            "velocityX": -0.11627253166592438,
-            "velocityY": 3.78115321878412,
-            "timestamp": 1.3969393721679058
-        },
-        {
-            "x": 2.197179274626327,
-            "y": 4.816031069152981,
-            "heading": 0.2753810893961792,
-            "angularVelocity": 6.459730400705482e-8,
-            "velocityX": 0.20036217650934227,
-            "velocityY": 3.7776307263464504,
-            "timestamp": 1.4842480829284
-        },
-        {
-            "x": 2.214672942788018,
-            "y": 5.1458511214306375,
-            "heading": 0.27538109503608554,
-            "angularVelocity": 6.459729260879602e-8,
-            "velocityX": 0.20036566809100828,
-            "velocityY": 3.777630541154376,
-            "timestamp": 1.5715567936888941
-        },
-        {
-            "x": 2.2321666974024836,
-            "y": 5.475671169122818,
-            "heading": 0.2753811006759881,
-            "angularVelocity": 6.459724966098765e-8,
-            "velocityX": 0.20036665828744704,
-            "velocityY": 3.7776304886341237,
-            "timestamp": 1.6588655044493883
-        },
-        {
-            "x": 2.2496605217353887,
-            "y": 5.805491213117091,
-            "heading": 0.2753811063158993,
-            "angularVelocity": 6.459734855412882e-8,
-            "velocityX": 0.20036745681532722,
-            "velocityY": 3.7776304462797263,
-            "timestamp": 1.7461742152098825
-        },
-        {
-            "x": 2.2664262161021513,
-            "y": 6.122693971906954,
-            "heading": 0.30917552472065174,
-            "angularVelocity": 0.3870681185232194,
-            "velocityX": 0.19202773950877436,
-            "velocityY": 3.633116971112041,
-            "timestamp": 1.8334829259703767
-        },
-        {
-            "x": 2.279851444819284,
-            "y": 6.376429629998848,
-            "heading": 0.34011357351362637,
-            "angularVelocity": 0.35435237244361656,
-            "velocityX": 0.15376734578020115,
-            "velocityY": 2.9061894956614607,
-            "timestamp": 1.9207916367308708
-        },
-        {
-            "x": 2.289924296428913,
-            "y": 6.566719611746641,
-            "heading": 0.3645867540299274,
-            "angularVelocity": 0.2803062867740199,
-            "velocityX": 0.11537052284807026,
-            "velocityY": 2.179507406423607,
-            "timestamp": 2.008100347491365
-        },
-        {
-            "x": 2.296641079194263,
-            "y": 6.6935744437684805,
-            "heading": 0.3814115840440414,
-            "angularVelocity": 0.192705056203017,
-            "velocityX": 0.07693141619941274,
-            "velocityY": 1.4529458849739352,
-            "timestamp": 2.095409058251859
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0.09836837448577633,
-            "velocityX": 0.03847177190545204,
-            "velocityY": 0.7264516412973858,
-            "timestamp": 2.182717769012353
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 2.6936232860428826e-33,
-            "velocityX": 0,
-            "velocityY": 5.62663175256514e-35,
-            "timestamp": 2.2700264797728473
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S4 To W2.traj b/src/main/deploy/choreo/S4 To W2.traj
deleted file mode 100644
index 97118e90..00000000
--- a/src/main/deploy/choreo/S4 To W2.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -2.256840111784074e-31,
-            "velocityX": -2.1308644711626517e-33,
-            "velocityY": -1.0217798620245829e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 5.1132615869041915,
-            "y": 1.6556053336408048,
-            "heading": -0.6461329738139044,
-            "angularVelocity": -0.06592692141286781,
-            "velocityX": -0.6744118848768044,
-            "velocityY": 0.3827425493802767,
-            "timestamp": 0.09302684976743651
-        },
-        {
-            "x": 4.987784771636742,
-            "y": 1.7268160147391884,
-            "heading": -0.6583988506620269,
-            "angularVelocity": -0.131853081973501,
-            "velocityX": -1.348823652323362,
-            "velocityY": 0.7654852472851404,
-            "timestamp": 0.18605369953487302
-        },
-        {
-            "x": 4.799569566809376,
-            "y": 1.8336320552005005,
-            "heading": -0.6767974581881981,
-            "angularVelocity": -0.19777738977689827,
-            "velocityX": -2.02323528419909,
-            "velocityY": 1.148228073167565,
-            "timestamp": 0.2790805493023095
-        },
-        {
-            "x": 4.54861599428641,
-            "y": 1.9760534609244509,
-            "heading": -0.7013284716868837,
-            "angularVelocity": -0.26369820713065356,
-            "velocityX": -2.697646681042515,
-            "velocityY": 1.5309709624694186,
-            "timestamp": 0.37210739906974605
-        },
-        {
-            "x": 4.242554383646119,
-            "y": 2.1497500028424636,
-            "heading": -0.7013284745367188,
-            "angularVelocity": -3.063454187141868e-8,
-            "velocityX": -3.29003520387323,
-            "velocityY": 1.8671656876724017,
-            "timestamp": 0.4651342488371826
-        },
-        {
-            "x": 3.9364928269084327,
-            "y": 2.3234466397396827,
-            "heading": -0.7013284773864732,
-            "angularVelocity": -3.0633677229161635e-8,
-            "velocityX": -3.290034624442601,
-            "velocityY": 1.8671667086594281,
-            "timestamp": 0.558161098604619
-        },
-        {
-            "x": 3.630431318940711,
-            "y": 2.497143362571822,
-            "heading": -0.7013284802362207,
-            "angularVelocity": -3.063360291364155e-8,
-            "velocityX": -3.2900341001857365,
-            "velocityY": 1.867167632424124,
-            "timestamp": 0.6511879483720555
-        },
-        {
-            "x": 3.3243698648507793,
-            "y": 2.6708401803390394,
-            "heading": -0.7013284830859674,
-            "angularVelocity": -3.063359264364299e-8,
-            "velocityX": -3.2900335210218676,
-            "velocityY": 1.8671686529367946,
-            "timestamp": 0.744214798139492
-        },
-        {
-            "x": 3.0183085209148324,
-            "y": 2.844537192202297,
-            "heading": -0.7013284859357423,
-            "angularVelocity": -3.063389607722874e-8,
-            "velocityX": -3.290032336912287,
-            "velocityY": 1.8671707393886097,
-            "timestamp": 0.8372416479069285
-        },
-        {
-            "x": 2.714360997870643,
-            "y": 3.021907148464231,
-            "heading": -0.7013284887932081,
-            "angularVelocity": -3.071657002046057e-8,
-            "velocityX": -3.2673096402172703,
-            "velocityY": 1.9066533662630898,
-            "timestamp": 0.9302684976743649
-        },
-        {
-            "x": 2.4529780195268613,
-            "y": 3.2575406255854364,
-            "heading": -0.7013284919399162,
-            "angularVelocity": -3.382580562568125e-8,
-            "velocityX": -2.8097584621776335,
-            "velocityY": 2.532962018065539,
-            "timestamp": 1.0232953474418014
-        },
-        {
-            "x": 2.2451503238556145,
-            "y": 3.5415333045009083,
-            "heading": -0.7013284961513817,
-            "angularVelocity": -4.527150531202637e-8,
-            "velocityX": -2.2340614155032408,
-            "velocityY": 3.0528033532839474,
-            "timestamp": 1.1163221972092379
-        },
-        {
-            "x": 2.099615704405981,
-            "y": 3.861945316168419,
-            "heading": -0.7013284332934493,
-            "angularVelocity": 6.756966674200994e-7,
-            "velocityX": -1.564436717071087,
-            "velocityY": 3.444296055047848,
-            "timestamp": 1.2093490469766743
-        },
-        {
-            "x": 2.0224929199626733,
-            "y": 4.205305507968674,
-            "heading": -0.7013282219960775,
-            "angularVelocity": 0.0000022713589924444385,
-            "velocityX": -0.8290379028862299,
-            "velocityY": 3.6909794608614943,
-            "timestamp": 1.3023758967441108
-        },
-        {
-            "x": 2.0170244087221176,
-            "y": 4.5571780060792255,
-            "heading": -0.7013280555552984,
-            "angularVelocity": 0.0000017891692504432307,
-            "velocityX": -0.05878422470745493,
-            "velocityY": 3.782483218449498,
-            "timestamp": 1.3954027465115473
-        },
-        {
-            "x": 2.079587370397571,
-            "y": 4.887709205686539,
-            "heading": -0.661535006789308,
-            "angularVelocity": 0.4277587477752017,
-            "velocityX": 0.6725258549747536,
-            "velocityY": 3.5530731227987227,
-            "timestamp": 1.4884295962789837
-        },
-        {
-            "x": 2.143441139356313,
-            "y": 5.160361503306635,
-            "heading": -0.462273406431507,
-            "angularVelocity": 2.1419794484704924,
-            "velocityX": 0.6864014971846709,
-            "velocityY": 2.9308989641347156,
-            "timestamp": 1.5814564460464202
-        },
-        {
-            "x": 2.1958862911911914,
-            "y": 5.3618445595482385,
-            "heading": -0.25085794958853325,
-            "angularVelocity": 2.272628358065485,
-            "velocityX": 0.5637636012182422,
-            "velocityY": 2.165859176628076,
-            "timestamp": 1.6744832958138567
-        },
-        {
-            "x": 2.231834886718389,
-            "y": 5.494391410801975,
-            "heading": -0.08836645960984135,
-            "angularVelocity": 1.7467160328971074,
-            "velocityX": 0.38643247209883785,
-            "velocityY": 1.4248236029178551,
-            "timestamp": 1.7675101455812932
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -3.8128513304390615e-33,
-            "angularVelocity": 0.9499027413134385,
-            "velocityX": 0.19526742362042082,
-            "velocityY": 0.7052650859622128,
-            "timestamp": 1.8605369953487296
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 5.82518331797089e-32,
-            "velocityX": 0,
-            "velocityY": -4.1086986258215814e-33,
-            "timestamp": 1.953563845116166
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/S4 To W3.traj b/src/main/deploy/choreo/S4 To W3.traj
deleted file mode 100644
index 8a67252f..00000000
--- a/src/main/deploy/choreo/S4 To W3.traj	
+++ /dev/null
@@ -1,256 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -2.137980318394988e-34,
-            "velocityX": 1.3258659962604387e-36,
-            "velocityY": 7.61509960712835e-36,
-            "timestamp": 0
-        },
-        {
-            "x": 5.139985955005962,
-            "y": 1.6266018860262446,
-            "heading": -0.6327443563347177,
-            "angularVelocity": 0.10940864810395588,
-            "velocityX": -0.5430597418126536,
-            "velocityY": 0.09955056482775779,
-            "timestamp": 0.06631691178916982
-        },
-        {
-            "x": 5.067957879081044,
-            "y": 1.6398056681484225,
-            "heading": -0.6182319813670383,
-            "angularVelocity": 0.2188336968074761,
-            "velocityX": -1.0861192715653594,
-            "velocityY": 0.19910128149746392,
-            "timestamp": 0.13263382357833964
-        },
-        {
-            "x": 4.959915797050431,
-            "y": 1.6596113536414707,
-            "heading": -0.5964609094580124,
-            "angularVelocity": 0.328288385596709,
-            "velocityX": -1.6291784269764191,
-            "velocityY": 0.2986521078667379,
-            "timestamp": 0.19895073536750946
-        },
-        {
-            "x": 4.815859744494909,
-            "y": 1.686018943789264,
-            "heading": -0.5674282989321845,
-            "angularVelocity": 0.43778592432238833,
-            "velocityX": -2.172237045860861,
-            "velocityY": 0.39820295359570684,
-            "timestamp": 0.2652676471566793
-        },
-        {
-            "x": 4.635789767966758,
-            "y": 1.719028429378711,
-            "heading": -0.5311304969514992,
-            "angularVelocity": 0.5473385445944894,
-            "velocityX": -2.7152949627783816,
-            "velocityY": 0.49775366039945246,
-            "timestamp": 0.3315845589458491
-        },
-        {
-            "x": 4.41970592970577,
-            "y": 1.758639783689655,
-            "heading": -0.48756329690665473,
-            "angularVelocity": 0.6569545967904882,
-            "velocityX": -3.258351941175237,
-            "velocityY": 0.5973039642870227,
-            "timestamp": 0.3979014707350189
-        },
-        {
-            "x": 4.172944378919847,
-            "y": 1.8038720825822125,
-            "heading": -0.4875632898742738,
-            "angularVelocity": 1.0604204507812919e-7,
-            "velocityX": -3.7209445393116334,
-            "velocityY": 0.6820628052819626,
-            "timestamp": 0.46421838252418873
-        },
-        {
-            "x": 3.9261828364502485,
-            "y": 1.849104426845956,
-            "heading": -0.4875632828424199,
-            "angularVelocity": 1.0603409772331237e-7,
-            "velocityX": -3.7209444139088643,
-            "velocityY": 0.6820634894390606,
-            "timestamp": 0.5305352943133586
-        },
-        {
-            "x": 3.6794213056437455,
-            "y": 1.8943368347367484,
-            "heading": -0.487563275810563,
-            "angularVelocity": 1.0603414195839125e-7,
-            "velocityX": -3.720944238039768,
-            "velocityY": 0.6820644488783205,
-            "timestamp": 0.5968522061025283
-        },
-        {
-            "x": 3.4326599282156307,
-            "y": 1.9395700793621615,
-            "heading": -0.4875632687787025,
-            "angularVelocity": 1.0603419804191013e-7,
-            "velocityX": -3.720941925230195,
-            "velocityY": 0.6820770660916139,
-            "timestamp": 0.6631691178916981
-        },
-        {
-            "x": 3.192328571865682,
-            "y": 2.011528836252569,
-            "heading": -0.4875632615410612,
-            "angularVelocity": 1.0913718718412795e-7,
-            "velocityX": -3.6239829308396314,
-            "velocityY": 1.0850740022269671,
-            "timestamp": 0.7294860296808678
-        },
-        {
-            "x": 2.965054771843869,
-            "y": 2.1177542675713954,
-            "heading": -0.48756325344928275,
-            "angularVelocity": 1.2201681646141545e-7,
-            "velocityX": -3.4270866041583203,
-            "velocityY": 1.6017849512735258,
-            "timestamp": 0.7958029414700376
-        },
-        {
-            "x": 2.7556945515371734,
-            "y": 2.2559769801698577,
-            "heading": -0.48756324342419455,
-            "angularVelocity": 1.5116940667572282e-7,
-            "velocityX": -3.156965767228115,
-            "velocityY": 2.084275471660842,
-            "timestamp": 0.8621198532592074
-        },
-        {
-            "x": 2.5687211592217127,
-            "y": 2.4232437322251608,
-            "heading": -0.4875632292213124,
-            "angularVelocity": 2.1416682100483036e-7,
-            "velocityX": -2.8193923280063653,
-            "velocityY": 2.5222337340898235,
-            "timestamp": 0.9284367650483771
-        },
-        {
-            "x": 2.4081295337898796,
-            "y": 2.6159806661875145,
-            "heading": -0.48756314249031313,
-            "angularVelocity": 0.0000013078262679985986,
-            "velocityX": -2.421578766248568,
-            "velocityY": 2.906301405817713,
-            "timestamp": 0.9947536768375469
-        },
-        {
-            "x": 2.2783797783684188,
-            "y": 2.8285106690600896,
-            "heading": -0.4828540771785898,
-            "angularVelocity": 0.07100851328382284,
-            "velocityX": -1.9565108193510647,
-            "velocityY": 3.2047632668456583,
-            "timestamp": 1.0610705886267167
-        },
-        {
-            "x": 2.1829307079315186,
-            "y": 3.053825616836548,
-            "heading": -0.47038335932986286,
-            "angularVelocity": 0.18804732476646344,
-            "velocityX": -1.4392870213912456,
-            "velocityY": 3.3975488559051734,
-            "timestamp": 1.1273875004158864
-        },
-        {
-            "x": 2.124568521000596,
-            "y": 3.2749812318761844,
-            "heading": -0.4550563591461243,
-            "angularVelocity": 0.24322153086635045,
-            "velocityX": -0.9261395106595871,
-            "velocityY": 3.5094804335357668,
-            "timestamp": 1.1904041261609186
-        },
-        {
-            "x": 2.099309698866093,
-            "y": 3.4965263857952165,
-            "heading": -0.4375068558264715,
-            "angularVelocity": 0.2784900510328627,
-            "velocityX": -0.40082790590377965,
-            "velocityY": 3.5156619590425686,
-            "timestamp": 1.2534207519059508
-        },
-        {
-            "x": 2.105558929434475,
-            "y": 3.707892405315209,
-            "heading": -0.4199092504656253,
-            "angularVelocity": 0.2792533740547583,
-            "velocityX": 0.09916796550908585,
-            "velocityY": 3.354131025281934,
-            "timestamp": 1.316437377650983
-        },
-        {
-            "x": 2.135680280502732,
-            "y": 3.8963278332771982,
-            "heading": -0.40525850037923283,
-            "angularVelocity": 0.23249023433387317,
-            "velocityX": 0.4779905415775436,
-            "velocityY": 2.990249410122441,
-            "timestamp": 1.3794540033960152
-        },
-        {
-            "x": 2.177481885950485,
-            "y": 4.0538159629335215,
-            "heading": -0.39590861500997965,
-            "angularVelocity": 0.14837172347315433,
-            "velocityX": 0.6633424902324003,
-            "velocityY": 2.4991520538330105,
-            "timestamp": 1.4424706291410474
-        },
-        {
-            "x": 2.2205433619064863,
-            "y": 4.17824561445827,
-            "heading": -0.39150641146352744,
-            "angularVelocity": 0.06985781124276201,
-            "velocityX": 0.6833351587282585,
-            "velocityY": 1.9745527478446014,
-            "timestamp": 1.5054872548860796
-        },
-        {
-            "x": 2.2579292824582606,
-            "y": 4.270063380418578,
-            "heading": -0.38992694464863337,
-            "angularVelocity": 0.0250642873403699,
-            "velocityX": 0.5932707457717501,
-            "velocityY": 1.4570403425249505,
-            "timestamp": 1.5685038806311118
-        },
-        {
-            "x": 2.285342089257776,
-            "y": 4.3303097045050025,
-            "heading": -0.38974725509942826,
-            "angularVelocity": 0.002851462563739305,
-            "velocityX": 0.43500911823537725,
-            "velocityY": 0.9560385592555154,
-            "timestamp": 1.631520506376144
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.004010765374750144,
-            "velocityX": 0.23260386555018367,
-            "velocityY": 0.47115019479344145,
-            "timestamp": 1.6945371321211762
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -1.6105528525551922e-32,
-            "velocityX": 0,
-            "velocityY": -4.5276793712997926e-36,
-            "timestamp": 1.7575537578662084
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Stage Side To C1.traj b/src/main/deploy/choreo/Stage Side To C1.traj
deleted file mode 100644
index a684ff7a..00000000
--- a/src/main/deploy/choreo/Stage Side To C1.traj	
+++ /dev/null
@@ -1,616 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 5.951430910881942e-24,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7172662637575132,
-            "y": 4.364294720467062,
-            "heading": -1.0315470131742317,
-            "angularVelocity": 0.26098153509522004,
-            "velocityX": 0.22434208273715575,
-            "velocityY": 0.13259724206381326,
-            "timestamp": 0.03238921413610362
-        },
-        {
-            "x": 0.7318144654254066,
-            "y": 4.3728967855708385,
-            "heading": -1.0149312072454106,
-            "angularVelocity": 0.5130042939294357,
-            "velocityX": 0.4491680967238058,
-            "velocityY": 0.2655842487449578,
-            "timestamp": 0.06477842827220724
-        },
-        {
-            "x": 0.7536624687467911,
-            "y": 4.385820612299006,
-            "heading": -0.9904958605622504,
-            "angularVelocity": 0.7544285137786853,
-            "velocityX": 0.6745456444104029,
-            "velocityY": 0.39901637235962667,
-            "timestamp": 0.09716764240831086
-        },
-        {
-            "x": 0.7828310955502448,
-            "y": 4.4030821899572965,
-            "heading": -0.9586530378385134,
-            "angularVelocity": 0.9831304516969581,
-            "velocityX": 0.9005660551343861,
-            "velocityY": 0.5329421574032179,
-            "timestamp": 0.12955685654441448
-        },
-        {
-            "x": 0.8193448564336872,
-            "y": 4.424698840047832,
-            "heading": -0.919902879162077,
-            "angularVelocity": 1.1963908266993857,
-            "velocityX": 1.1273432177146,
-            "velocityY": 0.6674027347406944,
-            "timestamp": 0.1619460706805181
-        },
-        {
-            "x": 0.8632324943492647,
-            "y": 4.450689040734006,
-            "heading": -0.8748582540701377,
-            "angularVelocity": 1.3907291761589549,
-            "velocityX": 1.3550078038681697,
-            "velocityY": 0.8024338156819394,
-            "timestamp": 0.19433528481662174
-        },
-        {
-            "x": 0.9145271224696989,
-            "y": 4.481072488182425,
-            "heading": -0.8242801160685745,
-            "angularVelocity": 1.5615734852049024,
-            "velocityX": 1.5836947418633758,
-            "velocityY": 0.9380730054376618,
-            "timestamp": 0.22672449895272537
-        },
-        {
-            "x": 0.9732655893831887,
-            "y": 4.515870568727518,
-            "heading": -0.7691375276578237,
-            "angularVelocity": 1.7024984977725797,
-            "velocityX": 1.8135193606940647,
-            "velocityY": 1.0743724870528544,
-            "timestamp": 0.25911371308882897
-        },
-        {
-            "x": 1.0394861119792222,
-            "y": 4.555106942812012,
-            "heading": -0.7107271690719036,
-            "angularVelocity": 1.8033891881560444,
-            "velocityX": 2.044523905944935,
-            "velocityY": 1.2114024724285466,
-            "timestamp": 0.2915029272249326
-        },
-        {
-            "x": 1.1132202582415271,
-            "y": 4.598805444215395,
-            "heading": -0.6509416110612904,
-            "angularVelocity": 1.8458477491731227,
-            "velocityX": 2.276503096137642,
-            "velocityY": 1.3491683132463759,
-            "timestamp": 0.3238921413610362
-        },
-        {
-            "x": 1.194458181637227,
-            "y": 4.646971254862031,
-            "heading": -0.5929406477835183,
-            "angularVelocity": 1.790749322723445,
-            "velocityX": 2.508178279791774,
-            "velocityY": 1.48709414326129,
-            "timestamp": 0.35628135549713985
-        },
-        {
-            "x": 1.2829200414273145,
-            "y": 4.699514481647374,
-            "heading": -0.5430956567390244,
-            "angularVelocity": 1.5389379574026985,
-            "velocityX": 2.7312135273909255,
-            "velocityY": 1.6222445708176134,
-            "timestamp": 0.3886705696332435
-        },
-        {
-            "x": 1.3771379621947135,
-            "y": 4.756486240363935,
-            "heading": -0.5116699283377882,
-            "angularVelocity": 0.9702528832339478,
-            "velocityX": 2.908928891311882,
-            "velocityY": 1.758973171660129,
-            "timestamp": 0.4210597837693471
-        },
-        {
-            "x": 1.4771406301349097,
-            "y": 4.8178582459332,
-            "heading": -0.4985254849792972,
-            "angularVelocity": 0.40582779511896505,
-            "velocityX": 3.087529926474048,
-            "velocityY": 1.894828485537579,
-            "timestamp": 0.45344899790545073
-        },
-        {
-            "x": 1.5826858211522454,
-            "y": 4.880091207833619,
-            "heading": -0.49852543793275184,
-            "angularVelocity": 0.000001452537415626245,
-            "velocityX": 3.2586524197166784,
-            "velocityY": 1.9214100607352578,
-            "timestamp": 0.48583821204155436
-        },
-        {
-            "x": 1.6882312828144117,
-            "y": 4.9423237107273525,
-            "heading": -0.4985253908851185,
-            "angularVelocity": 0.0000014525710071923091,
-            "velocityX": 3.2586607757338757,
-            "velocityY": 1.9213958891446032,
-            "timestamp": 0.5182274261776579
-        },
-        {
-            "x": 1.7937767445018398,
-            "y": 5.004556213578243,
-            "heading": -0.49852534383749064,
-            "angularVelocity": 0.0000014525708350435827,
-            "velocityX": 3.2586607765138327,
-            "velocityY": 1.921395887821837,
-            "timestamp": 0.5506166403137616
-        },
-        {
-            "x": 1.8993222061892705,
-            "y": 5.06678871642913,
-            "heading": -0.49852529678986307,
-            "angularVelocity": 0.0000014525708270038968,
-            "velocityX": 3.2586607765139086,
-            "velocityY": 1.9213958878217097,
-            "timestamp": 0.5830058544498652
-        },
-        {
-            "x": 2.0048676678767015,
-            "y": 5.129021219280016,
-            "heading": -0.4985252497422358,
-            "angularVelocity": 0.0000014525708183379938,
-            "velocityX": 3.258660776513909,
-            "velocityY": 1.9213958878217101,
-            "timestamp": 0.6153950685859688
-        },
-        {
-            "x": 2.1104131295641317,
-            "y": 5.191253722130902,
-            "heading": -0.49852520269460865,
-            "angularVelocity": 0.000001452570810915054,
-            "velocityX": 3.2586607765139104,
-            "velocityY": 1.9213958878217086,
-            "timestamp": 0.6477842827220724
-        },
-        {
-            "x": 2.2159585912515625,
-            "y": 5.253486224981789,
-            "heading": -0.4985251556469819,
-            "angularVelocity": 0.0000014525708028649706,
-            "velocityX": 3.2586607765139086,
-            "velocityY": 1.9213958878217123,
-            "timestamp": 0.6801734968581761
-        },
-        {
-            "x": 2.321504052938993,
-            "y": 5.315718727832675,
-            "heading": -0.4985251085993554,
-            "angularVelocity": 0.000001452570794356237,
-            "velocityX": 3.2586607765139117,
-            "velocityY": 1.9213958878217081,
-            "timestamp": 0.7125627109942797
-        },
-        {
-            "x": 2.427049514626424,
-            "y": 5.3779512306835615,
-            "heading": -0.49852506155172915,
-            "angularVelocity": 0.0000014525707870212498,
-            "velocityX": 3.258660776513912,
-            "velocityY": 1.9213958878217077,
-            "timestamp": 0.7449519251303833
-        },
-        {
-            "x": 2.5325949763138547,
-            "y": 5.440183733534448,
-            "heading": -0.4985250145041031,
-            "angularVelocity": 0.000001452570779107594,
-            "velocityX": 3.258660776513913,
-            "velocityY": 1.9213958878217068,
-            "timestamp": 0.777341139266487
-        },
-        {
-            "x": 2.6381404380012854,
-            "y": 5.502416236385335,
-            "heading": -0.49852496745647734,
-            "angularVelocity": 0.0000014525707705186945,
-            "velocityX": 3.258660776513915,
-            "velocityY": 1.9213958878217055,
-            "timestamp": 0.8097303534025906
-        },
-        {
-            "x": 2.743685899688716,
-            "y": 5.564648739236221,
-            "heading": -0.4985249204088519,
-            "angularVelocity": 0.00000145257076360687,
-            "velocityX": 3.2586607765139153,
-            "velocityY": 1.9213958878217066,
-            "timestamp": 0.8421195675386942
-        },
-        {
-            "x": 2.849231361376147,
-            "y": 5.626881242087108,
-            "heading": -0.4985248733612266,
-            "angularVelocity": 0.0000014525707544682094,
-            "velocityX": 3.2586607765139153,
-            "velocityY": 1.9213958878217066,
-            "timestamp": 0.8745087816747978
-        },
-        {
-            "x": 2.954776823063578,
-            "y": 5.689113744937994,
-            "heading": -0.4985248263136016,
-            "angularVelocity": 0.0000014525707471414822,
-            "velocityX": 3.2586607765139166,
-            "velocityY": 1.921395887821705,
-            "timestamp": 0.9068979958109015
-        },
-        {
-            "x": 3.0603222847510096,
-            "y": 5.751346247788881,
-            "heading": -0.49852477926597694,
-            "angularVelocity": 0.0000014525707385370494,
-            "velocityX": 3.258660776513917,
-            "velocityY": 1.9213958878217057,
-            "timestamp": 0.9392872099470051
-        },
-        {
-            "x": 3.1658677464384404,
-            "y": 5.813578750639766,
-            "heading": -0.4985247322183525,
-            "angularVelocity": 0.0000014525707310188908,
-            "velocityX": 3.2586607765139175,
-            "velocityY": 1.9213958878217063,
-            "timestamp": 0.9716764240831087
-        },
-        {
-            "x": 3.271413208125871,
-            "y": 5.875811253490653,
-            "heading": -0.4985246851707283,
-            "angularVelocity": 0.0000014525707228586851,
-            "velocityX": 3.258660776513917,
-            "velocityY": 1.921395887821708,
-            "timestamp": 1.0040656382192124
-        },
-        {
-            "x": 3.376958669813326,
-            "y": 5.938043756341499,
-            "heading": -0.4985246381231044,
-            "angularVelocity": 0.000001452570714543944,
-            "velocityX": 3.258660776514645,
-            "velocityY": 1.921395887820474,
-            "timestamp": 1.0364548523553159
-        },
-        {
-            "x": 3.482504131751534,
-            "y": 6.000276258767071,
-            "heading": -0.49852459107548075,
-            "angularVelocity": 0.0000014525707064344823,
-            "velocityX": 3.258660784256519,
-            "velocityY": 1.9213958746903632,
-            "timestamp": 1.0688440664914194
-        },
-        {
-            "x": 3.588052280044538,
-            "y": 6.0625042049472775,
-            "heading": -0.4985245440280508,
-            "angularVelocity": 0.0000014525647243024202,
-            "velocityX": 3.258743724052004,
-            "velocityY": 1.921255203004168,
-            "timestamp": 1.101233280627523
-        },
-        {
-            "x": 3.69740891456604,
-            "y": 6.1177659034729,
-            "heading": -0.4985244969178424,
-            "angularVelocity": 0.0000014545029788824699,
-            "velocityX": 3.3763287390046313,
-            "velocityY": 1.7061759601022248,
-            "timestamp": 1.1336224947636264
-        },
-        {
-            "x": 3.8477077097405896,
-            "y": 6.176720729441604,
-            "heading": -0.4985244519972541,
-            "angularVelocity": 0.0000010525498975792195,
-            "velocityX": 3.5217032391581933,
-            "velocityY": 1.3813909907718895,
-            "timestamp": 1.1763003653755966
-        },
-        {
-            "x": 4.000682189533232,
-            "y": 6.228336695265556,
-            "heading": -0.49852440748602983,
-            "angularVelocity": 0.000001042957945295385,
-            "velocityX": 3.58439813418752,
-            "velocityY": 1.2094316113670969,
-            "timestamp": 1.2189782359875667
-        },
-        {
-            "x": 4.153656816760498,
-            "y": 6.279952224134777,
-            "heading": -0.4985243629748153,
-            "angularVelocity": 0.00000104295771772215,
-            "velocityX": 3.5844015887793454,
-            "velocityY": 1.2094213729291292,
-            "timestamp": 1.2616561065995369
-        },
-        {
-            "x": 4.306631443993679,
-            "y": 6.331567752986466,
-            "heading": -0.4985243184636008,
-            "angularVelocity": 0.0000010429577168479619,
-            "velocityX": 3.584401588917953,
-            "velocityY": 1.209421372518333,
-            "timestamp": 1.304333977211507
-        },
-        {
-            "x": 4.459606071226857,
-            "y": 6.38318328183816,
-            "heading": -0.4985242739523863,
-            "angularVelocity": 0.000001042957716433214,
-            "velocityX": 3.584401588917909,
-            "velocityY": 1.2094213725184633,
-            "timestamp": 1.3470118478234772
-        },
-        {
-            "x": 4.612580698460036,
-            "y": 6.434798810689855,
-            "heading": -0.4985242294411719,
-            "angularVelocity": 0.0000010429577140470852,
-            "velocityX": 3.5844015889179057,
-            "velocityY": 1.209421372518471,
-            "timestamp": 1.3896897184354473
-        },
-        {
-            "x": 4.765555325693214,
-            "y": 6.4864143395415494,
-            "heading": -0.49852418492995754,
-            "angularVelocity": 0.0000010429577143791187,
-            "velocityX": 3.584401588917906,
-            "velocityY": 1.2094213725184708,
-            "timestamp": 1.4323675890474175
-        },
-        {
-            "x": 4.918529952926393,
-            "y": 6.5380298683932425,
-            "heading": -0.4985241404187432,
-            "angularVelocity": 0.0000010429577134079309,
-            "velocityX": 3.584401588917911,
-            "velocityY": 1.2094213725184557,
-            "timestamp": 1.4750454596593876
-        },
-        {
-            "x": 5.071504580159572,
-            "y": 6.589645397244938,
-            "heading": -0.49852409590752894,
-            "angularVelocity": 0.000001042957711441133,
-            "velocityX": 3.584401588917906,
-            "velocityY": 1.20942137251847,
-            "timestamp": 1.5177233302713578
-        },
-        {
-            "x": 5.224479207392751,
-            "y": 6.641260926096632,
-            "heading": -0.49852405139631467,
-            "angularVelocity": 0.0000010429577109856096,
-            "velocityX": 3.5844015889179066,
-            "velocityY": 1.2094213725184684,
-            "timestamp": 1.560401200883328
-        },
-        {
-            "x": 5.377453834625929,
-            "y": 6.692876454948327,
-            "heading": -0.4985240068851004,
-            "angularVelocity": 0.000001042957710628888,
-            "velocityX": 3.584401588917908,
-            "velocityY": 1.209421372518467,
-            "timestamp": 1.603079071495298
-        },
-        {
-            "x": 5.530428461859109,
-            "y": 6.744491983800022,
-            "heading": -0.4985239623738862,
-            "angularVelocity": 0.0000010429577097054993,
-            "velocityX": 3.5844015889179075,
-            "velocityY": 1.2094213725184686,
-            "timestamp": 1.6457569421072682
-        },
-        {
-            "x": 5.683403089092287,
-            "y": 6.796107512651717,
-            "heading": -0.4985239178626721,
-            "angularVelocity": 0.0000010429577084041104,
-            "velocityX": 3.584401588917908,
-            "velocityY": 1.2094213725184642,
-            "timestamp": 1.6884348127192383
-        },
-        {
-            "x": 5.836377716325465,
-            "y": 6.847723041503412,
-            "heading": -0.4985238733514579,
-            "angularVelocity": 0.000001042957708437487,
-            "velocityX": 3.5844015889179084,
-            "velocityY": 1.209421372518465,
-            "timestamp": 1.7311126833312085
-        },
-        {
-            "x": 5.989352343558645,
-            "y": 6.899338570355106,
-            "heading": -0.4985238288402439,
-            "angularVelocity": 0.0000010429577061050693,
-            "velocityX": 3.5844015889179084,
-            "velocityY": 1.2094213725184642,
-            "timestamp": 1.7737905539431786
-        },
-        {
-            "x": 6.142326970791823,
-            "y": 6.9509540992067995,
-            "heading": -0.49852378432902983,
-            "angularVelocity": 0.0000010429577060929704,
-            "velocityX": 3.584401588917909,
-            "velocityY": 1.2094213725184646,
-            "timestamp": 1.8164684245551488
-        },
-        {
-            "x": 6.295301598025002,
-            "y": 7.002569628058494,
-            "heading": -0.4985237398178158,
-            "angularVelocity": 0.0000010429577057751362,
-            "velocityX": 3.58440158891791,
-            "velocityY": 1.2094213725184613,
-            "timestamp": 1.859146295167119
-        },
-        {
-            "x": 6.44827622525818,
-            "y": 7.054185156910189,
-            "heading": -0.49852369530660184,
-            "angularVelocity": 0.0000010429577050460837,
-            "velocityX": 3.5844015889179097,
-            "velocityY": 1.2094213725184615,
-            "timestamp": 1.901824165779089
-        },
-        {
-            "x": 6.60125085249139,
-            "y": 7.105800685761796,
-            "heading": -0.4985236507953879,
-            "angularVelocity": 0.0000010429577031279755,
-            "velocityX": 3.584401588918602,
-            "velocityY": 1.2094213725164094,
-            "timestamp": 1.9445020363910592
-        },
-        {
-            "x": 6.754225480461122,
-            "y": 7.15741621243045,
-            "heading": -0.49852360628410947,
-            "angularVelocity": 0.000001042959214800434,
-            "velocityX": 3.584401606176385,
-            "velocityY": 1.209421321366894,
-            "timestamp": 1.9871799070030294
-        },
-        {
-            "x": 6.905473027890085,
-            "y": 7.208390081717835,
-            "heading": -0.49401401466016337,
-            "angularVelocity": 0.10566580664128356,
-            "velocityX": 3.543933782547739,
-            "velocityY": 1.1943864245440503,
-            "timestamp": 2.0298577776149997
-        },
-        {
-            "x": 7.045508663452743,
-            "y": 7.253974234854833,
-            "heading": -0.45930885159823864,
-            "angularVelocity": 0.8131887220303519,
-            "velocityX": 3.2812235839006783,
-            "velocityY": 1.0680981146283737,
-            "timestamp": 2.07253564822697
-        },
-        {
-            "x": 7.17296615196941,
-            "y": 7.296903697598188,
-            "heading": -0.39602280772923965,
-            "angularVelocity": 1.4828772607799376,
-            "velocityX": 2.986500654531669,
-            "velocityY": 1.0058951425592673,
-            "timestamp": 2.1152135188389405
-        },
-        {
-            "x": 7.286216905364156,
-            "y": 7.335009151903716,
-            "heading": -0.3243524220820172,
-            "angularVelocity": 1.6793336832302157,
-            "velocityX": 2.6536177126649325,
-            "velocityY": 0.8928621264164087,
-            "timestamp": 2.157891389450911
-        },
-        {
-            "x": 7.38508554947564,
-            "y": 7.368236895639727,
-            "heading": -0.2530399098538711,
-            "angularVelocity": 1.6709482269282783,
-            "velocityX": 2.31662551795061,
-            "velocityY": 0.7785707969856192,
-            "timestamp": 2.2005692600628812
-        },
-        {
-            "x": 7.469627579006154,
-            "y": 7.396613050645165,
-            "heading": -0.18659143584410423,
-            "angularVelocity": 1.5569772591027433,
-            "velocityX": 1.9809336388681473,
-            "velocityY": 0.6648915374301301,
-            "timestamp": 2.2432471306748516
-        },
-        {
-            "x": 7.539923583097788,
-            "y": 7.420174905803342,
-            "heading": -0.12774543015301906,
-            "angularVelocity": 1.3788411850749698,
-            "velocityX": 1.6471300719468636,
-            "velocityY": 0.5520860066427349,
-            "timestamp": 2.285925001286822
-        },
-        {
-            "x": 7.596048224439389,
-            "y": 7.4389607760624,
-            "heading": -0.07839593087005978,
-            "angularVelocity": 1.1563252471438406,
-            "velocityX": 1.315075952403783,
-            "velocityY": 0.4401782467044836,
-            "timestamp": 2.3286028718987923
-        },
-        {
-            "x": 7.638066692552635,
-            "y": 7.453006402993039,
-            "heading": -0.03996068961729096,
-            "angularVelocity": 0.9005894788478175,
-            "velocityX": 0.9845493111706484,
-            "velocityY": 0.3291079599154316,
-            "timestamp": 2.3712807425107627
-        },
-        {
-            "x": 7.666034942577791,
-            "y": 7.46234355914255,
-            "heading": -0.013541291231896178,
-            "angularVelocity": 0.6190420938664342,
-            "velocityX": 0.655333774251397,
-            "velocityY": 0.2187821467102802,
-            "timestamp": 2.413958613122733
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0.31729069510085045,
-            "velocityX": 0.3272201077973249,
-            "velocityY": 0.10910668200355174,
-            "timestamp": 2.4566364837347034
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 5.5875375349932e-23,
-            "velocityY": -3.709612464642882e-23,
-            "timestamp": 2.499314354346674
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Stage Side To C2.traj b/src/main/deploy/choreo/Stage Side To C2.traj
deleted file mode 100644
index a6fd22bb..00000000
--- a/src/main/deploy/choreo/Stage Side To C2.traj	
+++ /dev/null
@@ -1,607 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7226799898554153,
-            "y": 4.366381680411806,
-            "heading": -1.026991220057009,
-            "angularVelocity": 0.3105337423596819,
-            "velocityX": 0.3026851649532591,
-            "velocityY": 0.15233766037290783,
-            "timestamp": 0.04189167928786116
-        },
-        {
-            "x": 0.7480755502425919,
-            "y": 4.379165077654378,
-            "heading": -1.001621000893582,
-            "angularVelocity": 0.6056147567896238,
-            "velocityX": 0.6062196793943158,
-            "velocityY": 0.3051536118838673,
-            "timestamp": 0.08378335857572232
-        },
-        {
-            "x": 0.7862289943958503,
-            "y": 4.398373013543058,
-            "heading": -0.964694126725153,
-            "angularVelocity": 0.8814846956762871,
-            "velocityX": 0.9107642568130221,
-            "velocityY": 0.4585143449774818,
-            "timestamp": 0.12567503786358347
-        },
-        {
-            "x": 0.8371909372554502,
-            "y": 4.424030289360705,
-            "heading": -0.9172220550063085,
-            "angularVelocity": 1.1332100437568369,
-            "velocityX": 1.2165170679697972,
-            "velocityY": 0.6124671116987632,
-            "timestamp": 0.16756671715144464
-        },
-        {
-            "x": 0.9010212440299602,
-            "y": 4.456163358075127,
-            "heading": -0.8604944995891576,
-            "angularVelocity": 1.354148517832005,
-            "velocityX": 1.523698926841682,
-            "velocityY": 0.767051339565964,
-            "timestamp": 0.2094583964393058
-        },
-        {
-            "x": 0.9777886193671033,
-            "y": 4.494801080149917,
-            "heading": -0.796206518536166,
-            "angularVelocity": 1.5346241102256366,
-            "velocityX": 1.8325208404664681,
-            "velocityY": 0.9223244981249834,
-            "timestamp": 0.25135007572716694
-        },
-        {
-            "x": 1.0675667159355893,
-            "y": 4.539976112871383,
-            "heading": -0.7267376359019178,
-            "angularVelocity": 1.658297872398206,
-            "velocityX": 2.143100923492954,
-            "velocityY": 1.07837722166836,
-            "timestamp": 0.2932417550150281
-        },
-        {
-            "x": 1.1704147696438687,
-            "y": 4.591719023811467,
-            "heading": -0.6558650979752687,
-            "angularVelocity": 1.6918046526529587,
-            "velocityX": 2.4550950321555027,
-            "velocityY": 1.23515962643868,
-            "timestamp": 0.3351334343028892
-        },
-        {
-            "x": 1.286251945776662,
-            "y": 4.64999734523705,
-            "heading": -0.5908433350340152,
-            "angularVelocity": 1.5521402829056499,
-            "velocityX": 2.765159528144268,
-            "velocityY": 1.3911669910657207,
-            "timestamp": 0.37702511359075036
-        },
-        {
-            "x": 1.4132089603720126,
-            "y": 4.714842752574763,
-            "heading": -0.5508840698501896,
-            "angularVelocity": 0.9538711711517488,
-            "velocityX": 3.0306021805179513,
-            "velocityY": 1.547930482617412,
-            "timestamp": 0.4189167928786115
-        },
-        {
-            "x": 1.550306465812279,
-            "y": 4.786404286835006,
-            "heading": -0.541265046364201,
-            "angularVelocity": 0.22961656466170327,
-            "velocityX": 3.272666738857444,
-            "velocityY": 1.7082517453765305,
-            "timestamp": 0.46080847216647264
-        },
-        {
-            "x": 1.6919561664396436,
-            "y": 4.857462558344818,
-            "heading": -0.5412650016128938,
-            "angularVelocity": 0.000001068262432444083,
-            "velocityX": 3.3813325948098276,
-            "velocityY": 1.6962383155263616,
-            "timestamp": 0.5027001514543338
-        },
-        {
-            "x": 1.8336059249814771,
-            "y": 4.928520714406238,
-            "heading": -0.541264956861776,
-            "angularVelocity": 0.0000010682579123299043,
-            "velocityX": 3.3813339772912605,
-            "velocityY": 1.6962355596475316,
-            "timestamp": 0.544591830742195
-        },
-        {
-            "x": 1.9752556835258195,
-            "y": 4.999578870462656,
-            "heading": -0.5412649121106583,
-            "angularVelocity": 0.0000010682579080478101,
-            "velocityX": 3.3813339773511535,
-            "velocityY": 1.696235559528139,
-            "timestamp": 0.5864835100300562
-        },
-        {
-            "x": 2.1169054420701623,
-            "y": 5.0706370265190746,
-            "heading": -0.5412648673595408,
-            "angularVelocity": 0.0000010682579032833847,
-            "velocityX": 3.381333977351157,
-            "velocityY": 1.6962355595281329,
-            "timestamp": 0.6283751893179174
-        },
-        {
-            "x": 2.258555200614505,
-            "y": 5.141695182575493,
-            "heading": -0.5412648226084235,
-            "angularVelocity": 0.0000010682578992307337,
-            "velocityX": 3.381333977351157,
-            "velocityY": 1.6962355595281327,
-            "timestamp": 0.6702668686057786
-        },
-        {
-            "x": 2.400204959158848,
-            "y": 5.212753338631911,
-            "heading": -0.5412647778573064,
-            "angularVelocity": 0.0000010682578937479676,
-            "velocityX": 3.3813339773511575,
-            "velocityY": 1.696235559528132,
-            "timestamp": 0.7121585478936397
-        },
-        {
-            "x": 2.5418547177031905,
-            "y": 5.2838114946883294,
-            "heading": -0.5412647331061895,
-            "angularVelocity": 0.0000010682578893581349,
-            "velocityX": 3.3813339773511584,
-            "velocityY": 1.696235559528132,
-            "timestamp": 0.7540502271815009
-        },
-        {
-            "x": 2.6835044762475335,
-            "y": 5.354869650744748,
-            "heading": -0.5412646883550728,
-            "angularVelocity": 0.0000010682578845109594,
-            "velocityX": 3.3813339773511584,
-            "velocityY": 1.6962355595281313,
-            "timestamp": 0.7959419064693621
-        },
-        {
-            "x": 2.8251542347918766,
-            "y": 5.425927806801167,
-            "heading": -0.5412646436039563,
-            "angularVelocity": 0.0000010682578796288815,
-            "velocityX": 3.3813339773511593,
-            "velocityY": 1.6962355595281313,
-            "timestamp": 0.8378335857572233
-        },
-        {
-            "x": 2.9668039933362196,
-            "y": 5.496985962857585,
-            "heading": -0.5412645988528401,
-            "angularVelocity": 0.0000010682578747880459,
-            "velocityX": 3.38133397735116,
-            "velocityY": 1.6962355595281309,
-            "timestamp": 0.8797252650450845
-        },
-        {
-            "x": 3.108453751880562,
-            "y": 5.5680441189140035,
-            "heading": -0.541264554101724,
-            "angularVelocity": 0.0000010682578699288912,
-            "velocityX": 3.38133397735116,
-            "velocityY": 1.69623555952813,
-            "timestamp": 0.9216169443329457
-        },
-        {
-            "x": 3.250103510424905,
-            "y": 5.639102274970423,
-            "heading": -0.5412645093506081,
-            "angularVelocity": 0.000001068257865104235,
-            "velocityX": 3.3813339773511606,
-            "velocityY": 1.69623555952813,
-            "timestamp": 0.9635086236208069
-        },
-        {
-            "x": 3.3917532689692482,
-            "y": 5.71016043102684,
-            "heading": -0.5412644645994924,
-            "angularVelocity": 0.0000010682578613220535,
-            "velocityX": 3.381333977351161,
-            "velocityY": 1.6962355595281293,
-            "timestamp": 1.005400302908668
-        },
-        {
-            "x": 3.5334030275135913,
-            "y": 5.7812185870832575,
-            "heading": -0.5412644198483769,
-            "angularVelocity": 0.0000010682578558700584,
-            "velocityX": 3.381333977351162,
-            "velocityY": 1.6962355595281287,
-            "timestamp": 1.0472919821965292
-        },
-        {
-            "x": 3.675052786057934,
-            "y": 5.852276743139676,
-            "heading": -0.5412643750972616,
-            "angularVelocity": 0.0000010682578513840427,
-            "velocityX": 3.381333977351162,
-            "velocityY": 1.6962355595281287,
-            "timestamp": 1.0891836614843904
-        },
-        {
-            "x": 3.816702544602277,
-            "y": 5.923334899196093,
-            "heading": -0.5412643303461464,
-            "angularVelocity": 0.0000010682578474926016,
-            "velocityX": 3.381333977351163,
-            "velocityY": 1.6962355595281278,
-            "timestamp": 1.1310753407722516
-        },
-        {
-            "x": 3.95835230314662,
-            "y": 5.994393055252511,
-            "heading": -0.5412642855950315,
-            "angularVelocity": 0.000001068257842976485,
-            "velocityX": 3.3813339773511637,
-            "velocityY": 1.6962355595281275,
-            "timestamp": 1.1729670200601128
-        },
-        {
-            "x": 4.100002061690963,
-            "y": 6.065451211308929,
-            "heading": -0.5412642408439168,
-            "angularVelocity": 0.0000010682578369198447,
-            "velocityX": 3.3813339773511637,
-            "velocityY": 1.6962355595281269,
-            "timestamp": 1.214858699347974
-        },
-        {
-            "x": 4.2416518202353055,
-            "y": 6.136509367365346,
-            "heading": -0.5412641960928023,
-            "angularVelocity": 0.0000010682578319796932,
-            "velocityX": 3.3813339773511646,
-            "velocityY": 1.696235559528127,
-            "timestamp": 1.2567503786358352
-        },
-        {
-            "x": 4.383301578779649,
-            "y": 6.207567523421765,
-            "heading": -0.5412641513416879,
-            "angularVelocity": 0.0000010682578277344439,
-            "velocityX": 3.381333977351165,
-            "velocityY": 1.6962355595281264,
-            "timestamp": 1.2986420579236964
-        },
-        {
-            "x": 4.524951337323992,
-            "y": 6.278625679478183,
-            "heading": -0.5412641065905739,
-            "angularVelocity": 0.0000010682578220378627,
-            "velocityX": 3.3813339773511655,
-            "velocityY": 1.6962355595281264,
-            "timestamp": 1.3405337372115576
-        },
-        {
-            "x": 4.666601095868337,
-            "y": 6.349683835534597,
-            "heading": -0.54126406183946,
-            "angularVelocity": 0.0000010682578180389232,
-            "velocityX": 3.381333977351213,
-            "velocityY": 1.6962355595280314,
-            "timestamp": 1.3824254164994187
-        },
-        {
-            "x": 4.8082508544580715,
-            "y": 6.420741991500528,
-            "heading": -0.5412640170883463,
-            "angularVelocity": 0.000001068257814314864,
-            "velocityX": 3.3813339784347316,
-            "velocityY": 1.6962355573681103,
-            "timestamp": 1.42431709578728
-        },
-        {
-            "x": 4.949901660762853,
-            "y": 6.491798058877407,
-            "heading": -0.54126397233727,
-            "angularVelocity": 0.0000010682569195340074,
-            "velocityX": 3.3813589885337056,
-            "velocityY": 1.6961857004731962,
-            "timestamp": 1.4662087750751411
-        },
-        {
-            "x": 5.0966757016987385,
-            "y": 6.551558383908352,
-            "heading": -0.5412639273749705,
-            "angularVelocity": 0.000001073299046263079,
-            "velocityX": 3.503656177813225,
-            "velocityY": 1.4265440308634512,
-            "timestamp": 1.5081004543630023
-        },
-        {
-            "x": 5.2483360143420885,
-            "y": 6.597526467706988,
-            "heading": -0.5412638804954443,
-            "angularVelocity": 0.000001119065339105663,
-            "velocityX": 3.6202968040790893,
-            "velocityY": 1.097308214425216,
-            "timestamp": 1.5499921336508635
-        },
-        {
-            "x": 5.403589728009299,
-            "y": 6.629310092724924,
-            "heading": -0.5412638294474256,
-            "angularVelocity": 0.0000012185717939310704,
-            "velocityX": 3.7060751993343666,
-            "velocityY": 0.7587097380253649,
-            "timestamp": 1.5918838129387247
-        },
-        {
-            "x": 5.561113217492597,
-            "y": 6.646638205385644,
-            "heading": -0.54126377105557,
-            "angularVelocity": 0.0000013938771748607603,
-            "velocityX": 3.760257219598817,
-            "velocityY": 0.4136409176067717,
-            "timestamp": 1.633775492226586
-        },
-        {
-            "x": 5.7195634841918945,
-            "y": 6.649363040924072,
-            "heading": -0.5412637028328066,
-            "angularVelocity": 0.000001628551649307522,
-            "velocityX": 3.7823804009024578,
-            "velocityY": 0.06504479134637085,
-            "timestamp": 1.675667171514447
-        },
-        {
-            "x": 5.8386728592662305,
-            "y": 6.64312635201416,
-            "heading": -0.5412636336326876,
-            "angularVelocity": 0.000002194803906563273,
-            "velocityX": 3.777764046962115,
-            "velocityY": -0.19780759592812253,
-            "timestamp": 1.7071962383364254
-        },
-        {
-            "x": 5.95706146956484,
-            "y": 6.62863229973747,
-            "heading": -0.5412635727602775,
-            "angularVelocity": 0.0000019306759164775456,
-            "velocityX": 3.7549037200201067,
-            "velocityY": -0.459704448549871,
-            "timestamp": 1.7387253051584037
-        },
-        {
-            "x": 6.0741575637122,
-            "y": 6.605950895283392,
-            "heading": -0.5412635175985968,
-            "angularVelocity": 0.000001749550058024173,
-            "velocityX": 3.713909289117771,
-            "velocityY": -0.7193807727372066,
-            "timestamp": 1.770254371980382
-        },
-        {
-            "x": 6.189395638406918,
-            "y": 6.575191700510336,
-            "heading": -0.5412634663439824,
-            "angularVelocity": 0.0000016256305579593937,
-            "velocityX": 3.654978922953296,
-            "velocityY": -0.9755821492190131,
-            "timestamp": 1.8017834388023604
-        },
-        {
-            "x": 6.302219177732501,
-            "y": 6.536503313694644,
-            "heading": -0.5412634176727114,
-            "angularVelocity": 0.0000015436952600896132,
-            "velocityX": 3.578397672300798,
-            "velocityY": -1.2270704691051513,
-            "timestamp": 1.8333125056243387
-        },
-        {
-            "x": 6.412083374588637,
-            "y": 6.49007271186475,
-            "heading": -0.5412633705487488,
-            "angularVelocity": 0.0000014946196396789507,
-            "velocityX": 3.484536902930231,
-            "velocityY": -1.4726284825381617,
-            "timestamp": 1.864841572446317
-        },
-        {
-            "x": 6.518458035998333,
-            "y": 6.436124829047777,
-            "heading": -0.5412633241017745,
-            "angularVelocity": 0.0000014731477658308475,
-            "velocityX": 3.3738601275551834,
-            "velocityY": -1.7110523163142606,
-            "timestamp": 1.8963706392682953
-        },
-        {
-            "x": 6.622870515934444,
-            "y": 6.378470889852871,
-            "heading": -0.5412632777559726,
-            "angularVelocity": 0.0000014699389024112294,
-            "velocityX": 3.3116260790606757,
-            "velocityY": -1.8285964351699866,
-            "timestamp": 1.9278997060902736
-        },
-        {
-            "x": 6.7272827038996414,
-            "y": 6.320816421895741,
-            "heading": -0.5412632314101712,
-            "angularVelocity": 0.0000014699388892769531,
-            "velocityX": 3.311616818687876,
-            "velocityY": -1.8286132057971307,
-            "timestamp": 1.959428772912252
-        },
-        {
-            "x": 6.831694893880165,
-            "y": 6.263161957588452,
-            "heading": -0.5412631850642508,
-            "angularVelocity": 0.0000014699426583817394,
-            "velocityX": 3.311616882607505,
-            "velocityY": -1.828613090035986,
-            "timestamp": 1.9909578397342302
-        },
-        {
-            "x": 6.935462774507961,
-            "y": 6.205910954727142,
-            "heading": -0.5387292297574777,
-            "angularVelocity": 0.08036886473933494,
-            "velocityX": 3.2911814743423053,
-            "velocityY": -1.815816598206477,
-            "timestamp": 2.0224869065562086
-        },
-        {
-            "x": 7.033610221214887,
-            "y": 6.151825220973909,
-            "heading": -0.514724354544779,
-            "angularVelocity": 0.7613569836442236,
-            "velocityX": 3.11291949302188,
-            "velocityY": -1.715424502051262,
-            "timestamp": 2.054015973378187
-        },
-        {
-            "x": 7.126006777010342,
-            "y": 6.101041958255997,
-            "heading": -0.47044950446923295,
-            "angularVelocity": 1.4042550109565184,
-            "velocityX": 2.930519838000589,
-            "velocityY": -1.610680804625417,
-            "timestamp": 2.0855450402001656
-        },
-        {
-            "x": 7.2116589199714145,
-            "y": 6.053937431211348,
-            "heading": -0.4144026036835486,
-            "angularVelocity": 1.7776263757548019,
-            "velocityX": 2.7166088817245364,
-            "velocityY": -1.4940032101366643,
-            "timestamp": 2.117074107022144
-        },
-        {
-            "x": 7.290084520639544,
-            "y": 6.01082680132011,
-            "heading": -0.35575031779267857,
-            "angularVelocity": 1.8602607626174494,
-            "velocityX": 2.4874063387585092,
-            "velocityY": -1.3673297130756235,
-            "timestamp": 2.1486031738441227
-        },
-        {
-            "x": 7.3612581622075055,
-            "y": 5.971723406014521,
-            "heading": -0.2979353118382045,
-            "angularVelocity": 1.8337049517168786,
-            "velocityX": 2.2573976568931324,
-            "velocityY": -1.2402331958119186,
-            "timestamp": 2.1801322406661012
-        },
-        {
-            "x": 7.425208576949436,
-            "y": 5.936609973900717,
-            "heading": -0.2429131400794928,
-            "angularVelocity": 1.7451252861171487,
-            "velocityX": 2.028300269811719,
-            "velocityY": -1.1136844712868785,
-            "timestamp": 2.2116613074880798
-        },
-        {
-            "x": 7.481970224994158,
-            "y": 5.90546408447042,
-            "heading": -0.1919554556036592,
-            "angularVelocity": 1.616212898515343,
-            "velocityX": 1.8002958465346786,
-            "velocityY": -0.9878468527519235,
-            "timestamp": 2.2431903743100583
-        },
-        {
-            "x": 7.531573575573894,
-            "y": 5.8782638409384305,
-            "heading": -0.14593987362659777,
-            "angularVelocity": 1.4594653954358323,
-            "velocityX": 1.5732578087328088,
-            "velocityY": -0.8627037294052898,
-            "timestamp": 2.274719441132037
-        },
-        {
-            "x": 7.574043708984396,
-            "y": 5.854989674792864,
-            "heading": -0.10549518757833008,
-            "angularVelocity": 1.2827745989638484,
-            "velocityX": 1.347015236774987,
-            "velocityY": -0.7381812559495933,
-            "timestamp": 2.3062485079540154
-        },
-        {
-            "x": 7.60940100035129,
-            "y": 5.835624772967359,
-            "heading": -0.07108683169331761,
-            "angularVelocity": 1.091321734299697,
-            "velocityX": 1.1214188978865451,
-            "velocityY": -0.614192038566993,
-            "timestamp": 2.337777574775994
-        },
-        {
-            "x": 7.637662176953836,
-            "y": 5.820154874880602,
-            "heading": -0.043071374084194446,
-            "angularVelocity": 0.8885596826352652,
-            "velocityX": 0.8963530941818176,
-            "velocityY": -0.4906551206892414,
-            "timestamp": 2.3693066415979724
-        },
-        {
-            "x": 7.65884126375683,
-            "y": 5.808567782468078,
-            "heading": -0.021732750340195603,
-            "angularVelocity": 0.6767921126395045,
-            "velocityX": 0.6717321169883477,
-            "velocityY": -0.3675050859559891,
-            "timestamp": 2.400835708419951
-        },
-        {
-            "x": 7.672950320288622,
-            "y": 5.8008527701995645,
-            "heading": -0.007307045765720019,
-            "angularVelocity": 0.45753668054709684,
-            "velocityX": 0.44749362902029066,
-            "velocityY": -0.24469523034329732,
-            "timestamp": 2.4323647752419295
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 5.476349034092433e-35,
-            "angularVelocity": 0.23175585268595406,
-            "velocityX": 0.22359303404641942,
-            "velocityY": -0.12219740664444498,
-            "timestamp": 2.463893842063908
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 1.465348958332225e-34,
-            "angularVelocity": 2.9106988947740147e-33,
-            "velocityX": 3.969530612508356e-33,
-            "velocityY": 4.7510565840732025e-33,
-            "timestamp": 2.4954229088858866
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Stage Side To C3.traj b/src/main/deploy/choreo/Stage Side To C3.traj
deleted file mode 100644
index 49c665b8..00000000
--- a/src/main/deploy/choreo/Stage Side To C3.traj	
+++ /dev/null
@@ -1,274 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": 0,
-            "velocityX": -1.418990828065155e-30,
-            "velocityY": 1.8798559266210403e-30,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7612279700458028,
-            "y": 4.384337560333447,
-            "heading": -0.9750502376080931,
-            "angularVelocity": 0.7696859194569483,
-            "velocityX": 0.6070760811828038,
-            "velocityY": 0.28841179417367857,
-            "timestamp": 0.0843847610434429
-        },
-        {
-            "x": 0.8642667385584091,
-            "y": 4.433290258850185,
-            "heading": -0.8542284973914237,
-            "angularVelocity": 1.4317957261788106,
-            "velocityX": 1.2210589594435701,
-            "velocityY": 0.5801130193585202,
-            "timestamp": 0.1687695220868858
-        },
-        {
-            "x": 1.0199495140663002,
-            "y": 4.507092326164681,
-            "heading": -0.6915219404263555,
-            "angularVelocity": 1.9281509475538106,
-            "velocityX": 1.8449157594667183,
-            "velocityY": 0.8745899899690757,
-            "timestamp": 0.25315428313032873
-        },
-        {
-            "x": 1.2292345273519185,
-            "y": 4.606077015104397,
-            "heading": -0.5137189126301165,
-            "angularVelocity": 2.1070513869757743,
-            "velocityX": 2.480128055086372,
-            "velocityY": 1.173016166850306,
-            "timestamp": 0.3375390441737716
-        },
-        {
-            "x": 1.485641598676529,
-            "y": 4.729632973105035,
-            "heading": -0.42871970592241276,
-            "angularVelocity": 1.007281476615537,
-            "velocityX": 3.038547104406527,
-            "velocityY": 1.4641975218379706,
-            "timestamp": 0.4219238052172145
-        },
-        {
-            "x": 1.7745745246623985,
-            "y": 4.865356167675212,
-            "heading": -0.42871965586223265,
-            "angularVelocity": 5.932372081830595e-7,
-            "velocityX": 3.4239941242128182,
-            "velocityY": 1.6083851265555307,
-            "timestamp": 0.5063085662606575
-        },
-        {
-            "x": 2.063508085893233,
-            "y": 5.001078009918069,
-            "heading": -0.4287196058112515,
-            "angularVelocity": 5.931281972756146e-7,
-            "velocityX": 3.4240016521710777,
-            "velocityY": 1.6083691008265977,
-            "timestamp": 0.5906933273041004
-        },
-        {
-            "x": 2.3524294773436862,
-            "y": 5.136825756955775,
-            "heading": -0.4287195557601797,
-            "angularVelocity": 5.931292703362374e-7,
-            "velocityX": 3.423857434421327,
-            "velocityY": 1.6086760851027126,
-            "timestamp": 0.6750780883475433
-        },
-        {
-            "x": 2.647360543237261,
-            "y": 5.258967835966316,
-            "heading": -0.4287195052637089,
-            "angularVelocity": 5.984074633873495e-7,
-            "velocityX": 3.4950749666965533,
-            "velocityY": 1.447442375852424,
-            "timestamp": 0.7594628493909863
-        },
-        {
-            "x": 2.959809428031792,
-            "y": 5.324379711821066,
-            "heading": -0.42871944828896014,
-            "angularVelocity": 6.751781731100131e-7,
-            "velocityX": 3.702669545200082,
-            "velocityY": 0.7751621862272086,
-            "timestamp": 0.8438476104344292
-        },
-        {
-            "x": 3.2789674137401184,
-            "y": 5.330797896945005,
-            "heading": -0.4287193686010616,
-            "angularVelocity": 9.443399202542765e-7,
-            "velocityX": 3.782175617514861,
-            "velocityY": 0.0760585802995927,
-            "timestamp": 0.9282323714778722
-        },
-        {
-            "x": 3.593793413911528,
-            "y": 5.278000284107237,
-            "heading": -0.42871953037751775,
-            "angularVelocity": -0.000001917128805011847,
-            "velocityX": 3.7308395055989028,
-            "velocityY": -0.625677103128148,
-            "timestamp": 1.0126171325213151
-        },
-        {
-            "x": 3.893396265271842,
-            "y": 5.167813441772834,
-            "heading": -0.42871965571467335,
-            "angularVelocity": -0.0000014853055701792625,
-            "velocityX": 3.550437871193987,
-            "velocityY": -1.3057670718262655,
-            "timestamp": 1.097001893564758
-        },
-        {
-            "x": 4.167411681517661,
-            "y": 5.0040496950416875,
-            "heading": -0.4287197602752934,
-            "angularVelocity": -0.0000012390936318239587,
-            "velocityX": 3.247214459785327,
-            "velocityY": -1.9406791546980178,
-            "timestamp": 1.181386654608201
-        },
-        {
-            "x": 4.4320204493985145,
-            "y": 4.825485382292427,
-            "heading": -0.42871985986303984,
-            "angularVelocity": -0.0000011801626881351292,
-            "velocityX": 3.1357411528921184,
-            "velocityY": -2.1160729797797915,
-            "timestamp": 1.265771415651644
-        },
-        {
-            "x": 4.666313927400726,
-            "y": 4.608668432226026,
-            "heading": -0.4287198793836171,
-            "angularVelocity": -2.3132822841838536e-7,
-            "velocityX": 2.77649038884734,
-            "velocityY": -2.5693851281356843,
-            "timestamp": 1.350156176695087
-        },
-        {
-            "x": 4.936706954544898,
-            "y": 4.438990462685779,
-            "heading": -0.4287197198154701,
-            "angularVelocity": 0.0000018909592816311697,
-            "velocityX": 3.204287406881873,
-            "velocityY": -2.010765539204243,
-            "timestamp": 1.4345409377385299
-        },
-        {
-            "x": 5.233776210787073,
-            "y": 4.322144997944839,
-            "heading": -0.42871957094251306,
-            "angularVelocity": 0.0000017642161351814778,
-            "velocityX": 3.5204135506067917,
-            "velocityY": -1.384674949557486,
-            "timestamp": 1.5189256987819728
-        },
-        {
-            "x": 5.532138329985476,
-            "y": 4.208641466951728,
-            "heading": -0.42871942351676046,
-            "angularVelocity": 0.0000017470660671471722,
-            "velocityX": 3.5357345983929718,
-            "velocityY": -1.3450714274656952,
-            "timestamp": 1.6033104598254158
-        },
-        {
-            "x": 5.845046534221127,
-            "y": 4.145463344127181,
-            "heading": -0.42871936362457447,
-            "angularVelocity": 7.097512069029325e-7,
-            "velocityX": 3.708112701469523,
-            "velocityY": -0.7486911385809435,
-            "timestamp": 1.6876952208688587
-        },
-        {
-            "x": 6.164234411105391,
-            "y": 4.140760073731674,
-            "heading": -0.4287193263126739,
-            "angularVelocity": 4.4216396519651743e-7,
-            "velocityX": 3.7825298423246796,
-            "velocityY": -0.055736016049057806,
-            "timestamp": 1.7720799819123016
-        },
-        {
-            "x": 6.483450051427475,
-            "y": 4.138663317552119,
-            "heading": -0.42871928910258295,
-            "angularVelocity": 4.4095747260310005e-7,
-            "velocityX": 3.782858852414673,
-            "velocityY": -0.024847569082867782,
-            "timestamp": 1.8564647429557446
-        },
-        {
-            "x": 6.802665691714121,
-            "y": 4.136566553568004,
-            "heading": -0.4287192518958351,
-            "angularVelocity": 4.4091785477775863e-7,
-            "velocityX": 3.7828588519947024,
-            "velocityY": -0.024847661569458286,
-            "timestamp": 1.9408495039991875
-        },
-        {
-            "x": 7.0950029322992965,
-            "y": 4.132696093446151,
-            "heading": -0.35666839777179793,
-            "angularVelocity": 0.853837271482519,
-            "velocityX": 3.4643368893901423,
-            "velocityY": -0.04586681379319509,
-            "timestamp": 2.0252342650426303
-        },
-        {
-            "x": 7.329695147990892,
-            "y": 4.13025195968354,
-            "heading": -0.23302792235716066,
-            "angularVelocity": 1.465199093839195,
-            "velocityX": 2.7812156222231823,
-            "velocityY": -0.02896416049909081,
-            "timestamp": 2.109619026086073
-        },
-        {
-            "x": 7.505137224924894,
-            "y": 4.1285705338055685,
-            "heading": -0.12281843303988041,
-            "angularVelocity": 1.3060354494646,
-            "velocityX": 2.0790729838492856,
-            "velocityY": -0.019925705271038107,
-            "timestamp": 2.194003787129516
-        },
-        {
-            "x": 7.621791455543556,
-            "y": 4.127510522704197,
-            "heading": -0.04250049300917318,
-            "angularVelocity": 0.9518062152170125,
-            "velocityX": 1.3824087332380297,
-            "velocityY": -0.012561641322782502,
-            "timestamp": 2.278388548172959
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -1.5179160305718205e-31,
-            "angularVelocity": 0.5036512811515541,
-            "velocityX": 0.6897992449901723,
-            "velocityY": -0.0060499395609808024,
-            "timestamp": 2.362773309216402
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": 2.3304076579629275e-30,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 2.447158070259845
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Stage Side To C4.traj b/src/main/deploy/choreo/Stage Side To C4.traj
deleted file mode 100644
index 00bde21f..00000000
--- a/src/main/deploy/choreo/Stage Side To C4.traj	
+++ /dev/null
@@ -1,679 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.687708804701003e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7140616140612069,
-            "y": 4.355992841184312,
-            "heading": -1.0347167301054676,
-            "angularVelocity": 0.1988304134727778,
-            "velocityX": 0.15285465616518656,
-            "velocityY": -0.15080528916361516,
-            "timestamp": 0.026571739213607736
-        },
-        {
-            "x": 0.7221925258884458,
-            "y": 4.3479754290630535,
-            "heading": -1.0242624006876555,
-            "angularVelocity": 0.39343790535390527,
-            "velocityX": 0.30599848063671325,
-            "velocityY": -0.3017270362623706,
-            "timestamp": 0.05314347842721547
-        },
-        {
-            "x": 0.7344011981721603,
-            "y": 4.335944497876898,
-            "heading": -1.0087613580778494,
-            "angularVelocity": 0.5833657513042195,
-            "velocityX": 0.45946078973491633,
-            "velocityY": -0.4527716868451825,
-            "timestamp": 0.0797152176408232
-        },
-        {
-            "x": 0.7506970627866689,
-            "y": 4.31989642008046,
-            "heading": -0.9883558514269565,
-            "angularVelocity": 0.7679401971717026,
-            "velocityX": 0.6132780576953482,
-            "velocityY": -0.6039528563572185,
-            "timestamp": 0.10628695685443094
-        },
-        {
-            "x": 0.7710907256657638,
-            "y": 4.29982697901967,
-            "heading": -0.963213241169254,
-            "angularVelocity": 0.9462162057057438,
-            "velocityX": 0.7674944690353961,
-            "velocityY": -0.7552927152962536,
-            "timestamp": 0.13285869606803868
-        },
-        {
-            "x": 0.7955941760833964,
-            "y": 4.27573110299105,
-            "heading": -0.9335350310204893,
-            "angularVelocity": 1.1169088297226009,
-            "velocityX": 0.9221620843352277,
-            "velocityY": -0.9068234425649082,
-            "timestamp": 0.1594304352816464
-        },
-        {
-            "x": 0.8242209914920048,
-            "y": 4.247602572989807,
-            "heading": -0.8995682913456332,
-            "angularVelocity": 1.2783032153748224,
-            "velocityX": 1.0773406730541775,
-            "velocityY": -1.0585882156647894,
-            "timestamp": 0.18600217449525414
-        },
-        {
-            "x": 0.856986533327333,
-            "y": 4.215433742997202,
-            "heading": -0.8616204330063739,
-            "angularVelocity": 1.4281285103018635,
-            "velocityX": 1.2330973735640383,
-            "velocityY": -1.210640738794065,
-            "timestamp": 0.21257391370886186
-        },
-        {
-            "x": 0.893908134971617,
-            "y": 4.179215360896215,
-            "heading": -0.8200791656995261,
-            "angularVelocity": 1.5633627506615773,
-            "velocityX": 1.3895063980372095,
-            "velocityY": -1.3630414558050108,
-            "timestamp": 0.2391456529224696
-        },
-        {
-            "x": 0.9350052770321623,
-            "y": 4.138936674970777,
-            "heading": -0.7754414952973147,
-            "angularVelocity": 1.6798926876172218,
-            "velocityX": 1.546648555074586,
-            "velocityY": -1.5158468025612346,
-            "timestamp": 0.2657173921360773
-        },
-        {
-            "x": 0.9802996637151041,
-            "y": 4.094586228803994,
-            "heading": -0.7283605055619151,
-            "angularVelocity": 1.7718444907546276,
-            "velocityX": 1.7046075275248067,
-            "velocityY": -1.669083299751435,
-            "timestamp": 0.29228913134968504
-        },
-        {
-            "x": 1.0298146765069804,
-            "y": 4.046154350498487,
-            "heading": -0.6797316150493184,
-            "angularVelocity": 1.8300981400454652,
-            "velocityX": 1.8634464381059026,
-            "velocityY": -1.8226837888242364,
-            "timestamp": 0.31886087056329276
-        },
-        {
-            "x": 1.0835715343165668,
-            "y": 3.9936405744754793,
-            "heading": -0.6308802721211105,
-            "angularVelocity": 1.8384699072761677,
-            "velocityX": 2.0230839004342207,
-            "velocityY": -1.9763017994740086,
-            "timestamp": 0.3454326097769005
-        },
-        {
-            "x": 1.1415677463510772,
-            "y": 3.937082152581159,
-            "heading": -0.5840793547040092,
-            "angularVelocity": 1.7613042579137594,
-            "velocityX": 2.1826276243449603,
-            "velocityY": -2.12851787531304,
-            "timestamp": 0.3720043489905082
-        },
-        {
-            "x": 1.203636670620202,
-            "y": 3.876777756204273,
-            "heading": -0.5447644235662668,
-            "angularVelocity": 1.4795768851144226,
-            "velocityX": 2.33589994882,
-            "velocityY": -2.2694937614773476,
-            "timestamp": 0.39857608820411594
-        },
-        {
-            "x": 1.2693974629594884,
-            "y": 3.8133907541919347,
-            "heading": -0.5190150912676422,
-            "angularVelocity": 0.969049563960718,
-            "velocityX": 2.4748395959572558,
-            "velocityY": -2.3855044452595306,
-            "timestamp": 0.42514782741772367
-        },
-        {
-            "x": 1.3388335264794184,
-            "y": 3.7468848835898942,
-            "heading": -0.5067181615747146,
-            "angularVelocity": 0.4627822663045784,
-            "velocityX": 2.613154636275026,
-            "velocityY": -2.5028798479243823,
-            "timestamp": 0.4517195666313314
-        },
-        {
-            "x": 1.410439403600934,
-            "y": 3.676338986398213,
-            "heading": -0.5067181155488927,
-            "angularVelocity": 0.0000017321343363764108,
-            "velocityX": 2.6948133332892708,
-            "velocityY": -2.6549220818618244,
-            "timestamp": 0.4782913058449391
-        },
-        {
-            "x": 1.482040293717765,
-            "y": 3.605788027611728,
-            "heading": -0.5067180695571892,
-            "angularVelocity": 0.0000017308503249172533,
-            "velocityX": 2.694625652511417,
-            "velocityY": -2.655112569761896,
-            "timestamp": 0.5048630450585468
-        },
-        {
-            "x": 1.5536411829944505,
-            "y": 3.5352370679725946,
-            "heading": -0.5067180235654862,
-            "angularVelocity": 0.0000017308503100877725,
-            "velocityX": 2.6946256208934063,
-            "velocityY": -2.6551126018504427,
-            "timestamp": 0.5314347842721546
-        },
-        {
-            "x": 1.6252420722709946,
-            "y": 3.4646861083333174,
-            "heading": -0.5067179775737835,
-            "angularVelocity": 0.0000017308502987237163,
-            "velocityX": 2.6946256208880808,
-            "velocityY": -2.6551126018558486,
-            "timestamp": 0.5580065234857624
-        },
-        {
-            "x": 1.6968429615475384,
-            "y": 3.39413514869404,
-            "heading": -0.506717931582081,
-            "angularVelocity": 0.0000017308502871423956,
-            "velocityX": 2.6946256208880808,
-            "velocityY": -2.65511260185585,
-            "timestamp": 0.5845782626993702
-        },
-        {
-            "x": 1.7684438508240825,
-            "y": 3.323584189054763,
-            "heading": -0.5067178855903789,
-            "angularVelocity": 0.0000017308502762001758,
-            "velocityX": 2.694625620888081,
-            "velocityY": -2.65511260185585,
-            "timestamp": 0.611150001912978
-        },
-        {
-            "x": 1.8400447401006266,
-            "y": 3.2530332294154856,
-            "heading": -0.506717839598677,
-            "angularVelocity": 0.000001730850265567339,
-            "velocityX": 2.6946256208880826,
-            "velocityY": -2.6551126018558495,
-            "timestamp": 0.6377217411265858
-        },
-        {
-            "x": 1.9116456293771706,
-            "y": 3.1824822697762087,
-            "heading": -0.5067177936069754,
-            "angularVelocity": 0.0000017308502535085818,
-            "velocityX": 2.694625620888084,
-            "velocityY": -2.65511260185585,
-            "timestamp": 0.6642934803401935
-        },
-        {
-            "x": 1.9832465186537145,
-            "y": 3.1119313101369315,
-            "heading": -0.5067177476152741,
-            "angularVelocity": 0.0000017308502432657092,
-            "velocityX": 2.6946256208880843,
-            "velocityY": -2.65511260185585,
-            "timestamp": 0.6908652195538013
-        },
-        {
-            "x": 2.0548474079302586,
-            "y": 3.041380350497654,
-            "heading": -0.5067177016235731,
-            "angularVelocity": 0.000001730850231529751,
-            "velocityX": 2.694625620888085,
-            "velocityY": -2.6551126018558504,
-            "timestamp": 0.7174369587674091
-        },
-        {
-            "x": 2.1264482972068026,
-            "y": 2.970829390858377,
-            "heading": -0.5067176556318724,
-            "angularVelocity": 0.0000017308502204807436,
-            "velocityX": 2.6946256208880865,
-            "velocityY": -2.6551126018558495,
-            "timestamp": 0.7440086979810169
-        },
-        {
-            "x": 2.1980491864833867,
-            "y": 2.9002784312191405,
-            "heading": -0.5067176096401721,
-            "angularVelocity": 0.0000017308502087562552,
-            "velocityX": 2.694625620889591,
-            "velocityY": -2.6551126018543236,
-            "timestamp": 0.7705804371946247
-        },
-        {
-            "x": 2.2696500759972245,
-            "y": 2.829727471820689,
-            "heading": -0.506717563648472,
-            "angularVelocity": 0.0000017308501985869009,
-            "velocityX": 2.6946256298184115,
-            "velocityY": -2.6551125927926265,
-            "timestamp": 0.7971521764082324
-        },
-        {
-            "x": 2.3412523738653572,
-            "y": 2.7591779417640674,
-            "heading": -0.5067175176565489,
-            "angularVelocity": 0.0000017308585893832803,
-            "velocityX": 2.694678631779742,
-            "velocityY": -2.6550588009870313,
-            "timestamp": 0.8237239156218402
-        },
-        {
-            "x": 2.4158866145337003,
-            "y": 2.6918440324740667,
-            "heading": -0.5067174710515008,
-            "angularVelocity": 0.000001753932919872258,
-            "velocityX": 2.808782672009746,
-            "velocityY": -2.5340422299311864,
-            "timestamp": 0.850295654835448
-        },
-        {
-            "x": 2.4943342208862305,
-            "y": 2.6289942264556885,
-            "heading": -0.5067174224716422,
-            "angularVelocity": 0.000001828252870098291,
-            "velocityX": 2.952294756541798,
-            "velocityY": -2.365287628074865,
-            "timestamp": 0.8768673940490558
-        },
-        {
-            "x": 2.693828949223235,
-            "y": 2.509453130751911,
-            "heading": -0.5067173754009504,
-            "angularVelocity": 7.656472375459629e-7,
-            "velocityX": 3.244961599231293,
-            "velocityY": -1.9444436869204162,
-            "timestamp": 0.9383456957905196
-        },
-        {
-            "x": 2.900420237054028,
-            "y": 2.4026436735913763,
-            "heading": -0.5067173289743672,
-            "angularVelocity": 7.551702291921459e-7,
-            "velocityX": 3.3603935368868174,
-            "velocityY": -1.7373521085488381,
-            "timestamp": 0.9998239975319835
-        },
-        {
-            "x": 3.1070116472391316,
-            "y": 2.295834453089378,
-            "heading": -0.5067172825477892,
-            "angularVelocity": 7.55170142432061e-7,
-            "velocityX": 3.3603955270899863,
-            "velocityY": -1.7373482590844154,
-            "timestamp": 1.0613022992734473
-        },
-        {
-            "x": 3.3136030574258837,
-            "y": 2.1890252325905677,
-            "heading": -0.5067172361212112,
-            "angularVelocity": 7.551701448333524e-7,
-            "velocityX": 3.3603955271167996,
-            "velocityY": -1.737348259032553,
-            "timestamp": 1.1227806010149113
-        },
-        {
-            "x": 3.5201944676126358,
-            "y": 2.0822160120917577,
-            "heading": -0.5067171896946332,
-            "angularVelocity": 7.551701452492318e-7,
-            "velocityX": 3.3603955271167996,
-            "velocityY": -1.737348259032552,
-            "timestamp": 1.1842589027563752
-        },
-        {
-            "x": 3.726785877799388,
-            "y": 1.9754067915929476,
-            "heading": -0.5067171432680552,
-            "angularVelocity": 7.5517014486464e-7,
-            "velocityX": 3.3603955271168005,
-            "velocityY": -1.7373482590325509,
-            "timestamp": 1.2457372044978392
-        },
-        {
-            "x": 3.93337728798614,
-            "y": 1.8685975710941374,
-            "heading": -0.506717096841477,
-            "angularVelocity": 7.551701472516521e-7,
-            "velocityX": 3.360395527116801,
-            "velocityY": -1.7373482590325502,
-            "timestamp": 1.3072155062393032
-        },
-        {
-            "x": 4.139968698173285,
-            "y": 1.761788350596088,
-            "heading": -0.5067170504148988,
-            "angularVelocity": 7.551701467640916e-7,
-            "velocityX": 3.360395527123197,
-            "velocityY": -1.7373482590201774,
-            "timestamp": 1.3686938079807671
-        },
-        {
-            "x": 4.346560137549085,
-            "y": 1.6549791865550314,
-            "heading": -0.5067170039883208,
-            "angularVelocity": 7.55170145558408e-7,
-            "velocityX": 3.36039600190298,
-            "velocityY": -1.737347340696294,
-            "timestamp": 1.430172109722231
-        },
-        {
-            "x": 4.5552180589852185,
-            "y": 1.552265545137979,
-            "heading": -0.5067169574392034,
-            "angularVelocity": 7.571633584219393e-7,
-            "velocityX": 3.3940091955305984,
-            "velocityY": -1.6707299731374539,
-            "timestamp": 1.491650411463695
-        },
-        {
-            "x": 4.775846094749007,
-            "y": 1.478702717632789,
-            "heading": -0.5067169090858323,
-            "angularVelocity": 7.865111701229533e-7,
-            "velocityX": 3.5887138960279197,
-            "velocityY": -1.1965657056459655,
-            "timestamp": 1.553128713205159
-        },
-        {
-            "x": 5.004393851392972,
-            "y": 1.4356429788646896,
-            "heading": -0.5067168550876733,
-            "angularVelocity": 8.783287361409535e-7,
-            "velocityX": 3.717535295706156,
-            "velocityY": -0.7004054690576753,
-            "timestamp": 1.614607014946623
-        },
-        {
-            "x": 5.236664772033691,
-            "y": 1.4238771200180054,
-            "heading": -0.5067167933216484,
-            "angularVelocity": 0.000001004680076164895,
-            "velocityX": 3.7780959145145716,
-            "velocityY": -0.19138230096471237,
-            "timestamp": 1.676085316688087
-        },
-        {
-            "x": 5.366036550697627,
-            "y": 1.4270871399969536,
-            "heading": -0.5067167282581706,
-            "angularVelocity": 0.000001901925327561791,
-            "velocityX": 3.781775437099809,
-            "velocityY": 0.09383479793163368,
-            "timestamp": 1.7102945871261934
-        },
-        {
-            "x": 5.494798697340742,
-            "y": 1.4400359689986706,
-            "heading": -0.5067166730622293,
-            "angularVelocity": 0.0000016134790500794753,
-            "velocityX": 3.76395476998197,
-            "velocityY": 0.37851812786083194,
-            "timestamp": 1.7445038575643
-        },
-        {
-            "x": 5.6222191444596294,
-            "y": 1.4626499710733982,
-            "heading": -0.5067166242756445,
-            "angularVelocity": 0.0000014261217576278581,
-            "velocityX": 3.724734421022641,
-            "velocityY": 0.6610489433161765,
-            "timestamp": 1.7787131280024064
-        },
-        {
-            "x": 5.747573461036382,
-            "y": 1.4948005438980272,
-            "heading": -0.5067165796372217,
-            "angularVelocity": 0.0000013048633380814483,
-            "velocityX": 3.6643376187618593,
-            "velocityY": 0.9398204759378852,
-            "timestamp": 1.812922398440513
-        },
-        {
-            "x": 5.870148986660447,
-            "y": 1.5363048197489622,
-            "heading": -0.5067165375293654,
-            "angularVelocity": 0.0000012308902156872765,
-            "velocityX": 3.583108439738183,
-            "velocityY": 1.2132464480944425,
-            "timestamp": 1.8471316688786195
-        },
-        {
-            "x": 5.9892489774115445,
-            "y": 1.5869265226922225,
-            "heading": -0.5067164966802475,
-            "angularVelocity": 0.0000011940949728764683,
-            "velocityX": 3.481512152285735,
-            "velocityY": 1.4797656394002383,
-            "timestamp": 1.881340939316726
-        },
-        {
-            "x": 6.104355729464833,
-            "y": 1.6460687267313168,
-            "heading": -0.5067164560018657,
-            "angularVelocity": 0.0000011891040447074693,
-            "velocityX": 3.3647824282469623,
-            "velocityY": 1.7288355840881788,
-            "timestamp": 1.9155502097548325
-        },
-        {
-            "x": 6.219456425834801,
-            "y": 1.705222715286976,
-            "heading": -0.5067164153233215,
-            "angularVelocity": 0.0000011891087921450935,
-            "velocityX": 3.3646054094668667,
-            "velocityY": 1.7291800672184492,
-            "timestamp": 1.949759480192939
-        },
-        {
-            "x": 6.334557121741728,
-            "y": 1.764376704743614,
-            "heading": -0.5067163746447773,
-            "angularVelocity": 0.0000011891087898511233,
-            "velocityX": 3.3646053959312843,
-            "velocityY": 1.7291800935557256,
-            "timestamp": 1.9839687506310455
-        },
-        {
-            "x": 6.449657817648618,
-            "y": 1.8235306942003213,
-            "heading": -0.5067163339662332,
-            "angularVelocity": 0.000001189108787218646,
-            "velocityX": 3.3646053959302495,
-            "velocityY": 1.7291800935577397,
-            "timestamp": 2.018178021069152
-        },
-        {
-            "x": 6.564758513555509,
-            "y": 1.8826846836570283,
-            "heading": -0.5067162932876892,
-            "angularVelocity": 0.0000011891087819705439,
-            "velocityX": 3.3646053959302504,
-            "velocityY": 1.7291800935577386,
-            "timestamp": 2.0523872915072587
-        },
-        {
-            "x": 6.679859209462396,
-            "y": 1.9418386731137416,
-            "heading": -0.5067162526091454,
-            "angularVelocity": 0.0000011891087784797324,
-            "velocityX": 3.3646053959301545,
-            "velocityY": 1.7291800935579253,
-            "timestamp": 2.0865965619453655
-        },
-        {
-            "x": 6.7949599053264,
-            "y": 2.0009926626538963,
-            "heading": -0.5067162119306018,
-            "angularVelocity": 0.0000011891087739781964,
-            "velocityX": 3.3646053946765995,
-            "velocityY": 1.7291800959970696,
-            "timestamp": 2.120805832383472
-        },
-        {
-            "x": 6.910060040297267,
-            "y": 2.06014774355027,
-            "heading": -0.5067161712446878,
-            "angularVelocity": 0.0000011893242211155864,
-            "velocityX": 3.3645889987368625,
-            "velocityY": 1.7292119983499907,
-            "timestamp": 2.155015102821579
-        },
-        {
-            "x": 7.017868923227757,
-            "y": 2.1174564154331827,
-            "heading": -0.4884934235175169,
-            "angularVelocity": 0.5326844885552473,
-            "velocityX": 3.151452268634164,
-            "velocityY": 1.6752380611740627,
-            "timestamp": 2.1892243732596857
-        },
-        {
-            "x": 7.119036711180674,
-            "y": 2.1700871649460596,
-            "heading": -0.45052101262699384,
-            "angularVelocity": 1.1100035284068692,
-            "velocityX": 2.9573208272872957,
-            "velocityY": 1.5384937719762113,
-            "timestamp": 2.2234336436977924
-        },
-        {
-            "x": 7.212945348428852,
-            "y": 2.2182419084801723,
-            "heading": -0.3958429177265348,
-            "angularVelocity": 1.5983414495607555,
-            "velocityX": 2.745122478366898,
-            "velocityY": 1.4076518708938115,
-            "timestamp": 2.257642914135899
-        },
-        {
-            "x": 7.29827375889861,
-            "y": 2.2619654936466005,
-            "heading": -0.3361817714679226,
-            "angularVelocity": 1.7440052212324946,
-            "velocityX": 2.494306641941948,
-            "velocityY": 1.2781209481077944,
-            "timestamp": 2.291852184574006
-        },
-        {
-            "x": 7.374929402835313,
-            "y": 2.3012175879094494,
-            "heading": -0.2767819040095215,
-            "angularVelocity": 1.7363675605380382,
-            "velocityX": 2.2407856979994336,
-            "velocityY": 1.1474110309913224,
-            "timestamp": 2.3260614550121126
-        },
-        {
-            "x": 7.442942536538503,
-            "y": 2.336017519822898,
-            "heading": -0.22036844778019443,
-            "angularVelocity": 1.6490692583284896,
-            "velocityX": 1.9881492014348112,
-            "velocityY": 1.0172661231232827,
-            "timestamp": 2.3602707254502193
-        },
-        {
-            "x": 7.502356073864926,
-            "y": 2.3663915967666074,
-            "heading": -0.16856655782281169,
-            "angularVelocity": 1.5142646801283255,
-            "velocityX": 1.7367671559648723,
-            "velocityY": 0.8878902284298598,
-            "timestamp": 2.394479995888326
-        },
-        {
-            "x": 7.553208169092694,
-            "y": 2.392365594084153,
-            "heading": -0.12246425110299042,
-            "angularVelocity": 1.3476553615264124,
-            "velocityX": 1.4865004303372251,
-            "velocityY": 0.7592677945160818,
-            "timestamp": 2.428689266326433
-        },
-        {
-            "x": 7.595530700913324,
-            "y": 2.4139635888667987,
-            "heading": -0.08285639859822795,
-            "angularVelocity": 1.1578104998299616,
-            "velocityX": 1.2371655775940182,
-            "velocityY": 0.6313491783381474,
-            "timestamp": 2.4628985367645395
-        },
-        {
-            "x": 7.629350339345417,
-            "y": 2.431207580669044,
-            "heading": -0.050358801794747596,
-            "angularVelocity": 0.9499646261757312,
-            "velocityX": 0.988610338629752,
-            "velocityY": 0.5040736496688569,
-            "timestamp": 2.4971078072026462
-        },
-        {
-            "x": 7.654689602970606,
-            "y": 2.444117248020803,
-            "heading": -0.025464280349729525,
-            "angularVelocity": 0.7277127260009457,
-            "velocityX": 0.7407133592934628,
-            "velocityY": 0.3773733606834889,
-            "timestamp": 2.531317077640753
-        },
-        {
-            "x": 7.671567523131761,
-            "y": 2.4527098140869614,
-            "heading": -0.008571412442380029,
-            "angularVelocity": 0.49380965133159116,
-            "velocityX": 0.4933727011714593,
-            "velocityY": 0.2511765365386793,
-            "timestamp": 2.5655263480788597
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 5.0379367595035626e-36,
-            "angularVelocity": 0.2505581771434718,
-            "velocityX": 0.24649683434481964,
-            "velocityY": 0.12541003821758284,
-            "timestamp": 2.5997356185169664
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 2.854280403338002e-36,
-            "angularVelocity": -6.383230494440131e-35,
-            "velocityX": -1.2082929677962258e-35,
-            "velocityY": 1.8766665143888344e-34,
-            "timestamp": 2.633944888955073
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Stage Side To C5.traj b/src/main/deploy/choreo/Stage Side To C5.traj
deleted file mode 100644
index 818f5ad4..00000000
--- a/src/main/deploy/choreo/Stage Side To C5.traj	
+++ /dev/null
@@ -1,337 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 1.4815405069278332e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7317173879428727,
-            "y": 4.344479531295635,
-            "heading": -1.013582317544266,
-            "angularVelocity": 0.4585462516411977,
-            "velocityX": 0.37696065327942924,
-            "velocityY": -0.26939731598432753,
-            "timestamp": 0.05761181638969176
-        },
-        {
-            "x": 0.7752730705207751,
-            "y": 4.313424298338961,
-            "heading": -0.9621480921974644,
-            "angularVelocity": 0.892772152832256,
-            "velocityX": 0.7560199505477795,
-            "velocityY": -0.5390427676609577,
-            "timestamp": 0.11522363277938352
-        },
-        {
-            "x": 0.8408250898493527,
-            "y": 4.266812487548848,
-            "heading": -0.8877439224939486,
-            "angularVelocity": 1.2914741170498636,
-            "velocityX": 1.1378224717856051,
-            "velocityY": -0.8090668496689294,
-            "timestamp": 0.17283544916907528
-        },
-        {
-            "x": 0.9285847241631874,
-            "y": 4.204600540366139,
-            "heading": -0.7936992773737622,
-            "angularVelocity": 1.6323846567179892,
-            "velocityX": 1.5232922656043415,
-            "velocityY": -1.0798470015578314,
-            "timestamp": 0.23044726555876704
-        },
-        {
-            "x": 1.0388213735909644,
-            "y": 4.1267152282410935,
-            "heading": -0.6856850144743787,
-            "angularVelocity": 1.8748629997840256,
-            "velocityX": 1.9134381857035365,
-            "velocityY": -1.3518982216811457,
-            "timestamp": 0.2880590819484588
-        },
-        {
-            "x": 1.1718232312823271,
-            "y": 4.033123229014291,
-            "heading": -0.5741627817643435,
-            "angularVelocity": 1.935752762171023,
-            "velocityX": 2.3085864328894954,
-            "velocityY": -1.6245278328622093,
-            "timestamp": 0.34567089833815057
-        },
-        {
-            "x": 1.3270687303513053,
-            "y": 3.9246703032584405,
-            "heading": -0.488682735559766,
-            "angularVelocity": 1.4837242003685942,
-            "velocityX": 2.6946815566251776,
-            "velocityY": -1.882477112373353,
-            "timestamp": 0.40328271472784233
-        },
-        {
-            "x": 1.5010681185276094,
-            "y": 3.804139759424889,
-            "heading": -0.46956462349945394,
-            "angularVelocity": 0.3318435914430377,
-            "velocityX": 3.020203129846763,
-            "velocityY": -2.0921149754812625,
-            "timestamp": 0.4608945311175341
-        },
-        {
-            "x": 1.6785494425409546,
-            "y": 3.677652137140934,
-            "heading": -0.4695645649556455,
-            "angularVelocity": 0.0000010161770978802793,
-            "velocityX": 3.080641006227699,
-            "velocityY": -2.195515264236428,
-            "timestamp": 0.5185063475072258
-        },
-        {
-            "x": 1.85603075289391,
-            "y": 3.551164495689284,
-            "heading": -0.4695645064119953,
-            "angularVelocity": 0.000001016174352223477,
-            "velocityX": 3.080640769116793,
-            "velocityY": -2.1955155969406546,
-            "timestamp": 0.5761181638969175
-        },
-        {
-            "x": 2.033512086622271,
-            "y": 3.4246768870368545,
-            "heading": -0.4695644478683731,
-            "angularVelocity": 0.0000010161738659585955,
-            "velocityX": 3.0806411748565776,
-            "velocityY": -2.195515027626543,
-            "timestamp": 0.6337299802866092
-        },
-        {
-            "x": 2.2109934521039283,
-            "y": 3.298189322939119,
-            "heading": -0.46956438932476746,
-            "angularVelocity": 0.0000010161735782211743,
-            "velocityX": 3.080641726016013,
-            "velocityY": -2.1955142542661994,
-            "timestamp": 0.6913417966763009
-        },
-        {
-            "x": 2.3884748618363862,
-            "y": 3.17170182093203,
-            "heading": -0.4695643307811488,
-            "angularVelocity": 0.000001016173805422874,
-            "velocityX": 3.0806424941015065,
-            "velocityY": -2.1955131765246763,
-            "timestamp": 0.7489536130659926
-        },
-        {
-            "x": 2.5659563234551954,
-            "y": 3.045214391729511,
-            "heading": -0.4695642722374779,
-            "angularVelocity": 0.0000010161747112988211,
-            "velocityX": 3.0806433947214558,
-            "velocityY": -2.1955119128156926,
-            "timestamp": 0.8065654294556843
-        },
-        {
-            "x": 2.743437831089148,
-            "y": 2.918727027093402,
-            "heading": -0.46956421369376844,
-            "angularVelocity": 0.000001016175381373278,
-            "velocityX": 3.08064419343162,
-            "velocityY": -2.1955107921009867,
-            "timestamp": 0.864177245845376
-        },
-        {
-            "x": 2.920919379832659,
-            "y": 2.7922397201404365,
-            "heading": -0.4695641551500532,
-            "angularVelocity": 0.0000010161754817816058,
-            "velocityX": 3.080644906992851,
-            "velocityY": -2.195509790862922,
-            "timestamp": 0.9217890622350677
-        },
-        {
-            "x": 3.0984009676669872,
-            "y": 2.6657524680380327,
-            "heading": -0.4695640966063568,
-            "angularVelocity": 0.000001016175153333812,
-            "velocityX": 3.080645585513673,
-            "velocityY": -2.195508838791537,
-            "timestamp": 0.9794008786247594
-        },
-        {
-            "x": 3.275882647398041,
-            "y": 2.5392653448813247,
-            "heading": -0.4695640380626867,
-            "angularVelocity": 0.0000010161746978103879,
-            "velocityX": 3.080647180615714,
-            "velocityY": -2.1955066006101345,
-            "timestamp": 1.0370126950144511
-        },
-        {
-            "x": 3.4563703536987305,
-            "y": 2.417105913162231,
-            "heading": -0.4695639788949644,
-            "angularVelocity": 0.0000010270067149252654,
-            "velocityX": 3.1328244379564545,
-            "velocityY": -2.1203884788634437,
-            "timestamp": 1.0946245114041429
-        },
-        {
-            "x": 3.7578524899325245,
-            "y": 2.288322056940197,
-            "heading": -0.4695639274563231,
-            "angularVelocity": 5.9355601779034e-7,
-            "velocityX": 3.4788348155609183,
-            "velocityY": -1.4860507766867785,
-            "timestamp": 1.1812863255510104
-        },
-        {
-            "x": 4.061243078218464,
-            "y": 2.1641008385808727,
-            "heading": -0.46956387627292245,
-            "angularVelocity": 5.906107685245515e-7,
-            "velocityX": 3.5008566491785738,
-            "velocityY": -1.43340200735705,
-            "timestamp": 1.267948139697878
-        },
-        {
-            "x": 4.364633703659485,
-            "y": 2.039879710966971,
-            "heading": -0.46956382508952316,
-            "angularVelocity": 5.906107531304183e-7,
-            "velocityX": 3.5008570779150574,
-            "velocityY": -1.433400960235855,
-            "timestamp": 1.3546099538447456
-        },
-        {
-            "x": 4.668024366282006,
-            "y": 1.9156586741630988,
-            "heading": -0.46956377390610504,
-            "angularVelocity": 5.906109701114576e-7,
-            "velocityX": 3.5008575069563967,
-            "velocityY": -1.4333999123691519,
-            "timestamp": 1.4412717679916132
-        },
-        {
-            "x": 4.971415087165123,
-            "y": 1.7914377796518506,
-            "heading": -0.4695637227226054,
-            "angularVelocity": 5.906119107964098e-7,
-            "velocityX": 3.5008581792316793,
-            "velocityY": -1.4333982704392505,
-            "timestamp": 1.5279335821384807
-        },
-        {
-            "x": 5.2748058711699946,
-            "y": 1.6672170393060612,
-            "heading": -0.4695636715390663,
-            "angularVelocity": 5.906123657897232e-7,
-            "velocityX": 3.5008589076003997,
-            "velocityY": -1.4333964915074275,
-            "timestamp": 1.6145953962853483
-        },
-        {
-            "x": 5.5781967938554144,
-            "y": 1.5429966376674569,
-            "heading": -0.4695636203556594,
-            "angularVelocity": 5.906108405690599e-7,
-            "velocityX": 3.5008605078501667,
-            "velocityY": -1.4333925831287677,
-            "timestamp": 1.701257210432216
-        },
-        {
-            "x": 5.886988948403778,
-            "y": 1.4328868446307816,
-            "heading": -0.4695635793301605,
-            "angularVelocity": 4.733976467078746e-7,
-            "velocityX": 3.5631859035981908,
-            "velocityY": -1.2705687518849984,
-            "timestamp": 1.7879190245790835
-        },
-        {
-            "x": 6.195781169913944,
-            "y": 1.3227772393821355,
-            "heading": -0.4695635383046595,
-            "angularVelocity": 4.7339767079441426e-7,
-            "velocityX": 3.5631866762776085,
-            "velocityY": -1.2705665849787193,
-            "timestamp": 1.874580838725951
-        },
-        {
-            "x": 6.5045734104803365,
-            "y": 1.212667687574965,
-            "heading": -0.46956349727913566,
-            "angularVelocity": 4.733979354519185e-7,
-            "velocityX": 3.5631868961694693,
-            "velocityY": -1.270565968311784,
-            "timestamp": 1.9612426528728186
-        },
-        {
-            "x": 6.813365656566466,
-            "y": 1.1025581512474965,
-            "heading": -0.46956345625352064,
-            "angularVelocity": 4.733989867793667e-7,
-            "velocityX": 3.563186959862305,
-            "velocityY": -1.270565789689833,
-            "timestamp": 2.0479044670196864
-        },
-        {
-            "x": 7.101065618447083,
-            "y": 0.9964425714529646,
-            "heading": -0.4012732100130825,
-            "angularVelocity": 0.7880085007764233,
-            "velocityX": 3.3198008224596522,
-            "velocityY": -1.2244790954259896,
-            "timestamp": 2.134566281166554
-        },
-        {
-            "x": 7.333266238045498,
-            "y": 0.911992130350644,
-            "heading": -0.2613611242014092,
-            "angularVelocity": 1.6144606155435655,
-            "velocityX": 2.6793879390165927,
-            "velocityY": -0.9744827284506232,
-            "timestamp": 2.2212280953134216
-        },
-        {
-            "x": 7.506890250099573,
-            "y": 0.8492417427348896,
-            "heading": -0.1371754084070899,
-            "angularVelocity": 1.43299233943868,
-            "velocityX": 2.0034661605379225,
-            "velocityY": -0.7240834759056616,
-            "timestamp": 2.307889909460289
-        },
-        {
-            "x": 7.622366585478705,
-            "y": 0.8076799304753898,
-            "heading": -0.04732610702177306,
-            "angularVelocity": 1.0367807582823931,
-            "velocityX": 1.332493861522807,
-            "velocityY": -0.4795862245517285,
-            "timestamp": 2.3945517236071567
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 2.9489608951695904e-31,
-            "angularVelocity": 0.5461010421679925,
-            "velocityX": 0.6650381726791765,
-            "velocityY": -0.2386279433331854,
-            "timestamp": 2.4812135377540243
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -7.162012659830636e-32,
-            "angularVelocity": -4.0878892657023166e-30,
-            "velocityX": 2.3668785600931567e-32,
-            "velocityY": 6.81692291678628e-32,
-            "timestamp": 2.567875351900892
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Stage Side To W1.traj b/src/main/deploy/choreo/Stage Side To W1.traj
deleted file mode 100644
index 9649eb8c..00000000
--- a/src/main/deploy/choreo/Stage Side To W1.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7269596910839872,
-            "y": 4.384958038291164,
-            "heading": -1.001571192673364,
-            "angularVelocity": 0.6206074004453302,
-            "velocityX": 0.27389113865872694,
-            "velocityY": 0.40306073338263126,
-            "timestamp": 0.06192128437247234
-        },
-        {
-            "x": 0.760846036232504,
-            "y": 4.4351182711466235,
-            "heading": -0.9264796681168371,
-            "angularVelocity": 1.2126932656117428,
-            "velocityX": 0.5472487447883301,
-            "velocityY": 0.8100644772439347,
-            "timestamp": 0.12384256874494468
-        },
-        {
-            "x": 0.8116293344011996,
-            "y": 4.510831947021006,
-            "heading": -0.817810113065978,
-            "angularVelocity": 1.7549628718484593,
-            "velocityX": 0.8201266928382989,
-            "velocityY": 1.2227407206049843,
-            "timestamp": 0.185763853117417
-        },
-        {
-            "x": 0.8793448344180163,
-            "y": 4.612606899775036,
-            "heading": -0.6817312187907495,
-            "angularVelocity": 2.197610977457753,
-            "velocityX": 1.0935738930977383,
-            "velocityY": 1.6436182450904597,
-            "timestamp": 0.24768513748988935
-        },
-        {
-            "x": 0.9640984700059907,
-            "y": 4.74109575406918,
-            "heading": -0.5303096586797065,
-            "angularVelocity": 2.4453879089491037,
-            "velocityX": 1.3687318738119136,
-            "velocityY": 2.075035354907217,
-            "timestamp": 0.3096064218623617
-        },
-        {
-            "x": 1.0654137163163762,
-            "y": 4.896663616218439,
-            "heading": -0.3925803264267699,
-            "angularVelocity": 2.224264784697608,
-            "velocityX": 1.6361941994120859,
-            "velocityY": 2.5123487622362273,
-            "timestamp": 0.37152770623483405
-        },
-        {
-            "x": 1.179096696888173,
-            "y": 5.075186082347611,
-            "heading": -0.3289347906925715,
-            "angularVelocity": 1.0278458591290565,
-            "velocityX": 1.8359273668802587,
-            "velocityY": 2.88305496144612,
-            "timestamp": 0.4334489906073064
-        },
-        {
-            "x": 1.3095067112863388,
-            "y": 5.269772108097061,
-            "heading": -0.32893471990297235,
-            "angularVelocity": 0.0000011432191669124052,
-            "velocityX": 2.1060611988232667,
-            "velocityY": 3.142473992932148,
-            "timestamp": 0.49537027497977876
-        },
-        {
-            "x": 1.4399171936865056,
-            "y": 5.464357820195416,
-            "heading": -0.32893464911575754,
-            "angularVelocity": 0.0000011431806608935866,
-            "velocityX": 2.106068756838354,
-            "velocityY": 3.1424689276125553,
-            "timestamp": 0.557291559352251
-        },
-        {
-            "x": 1.570327676092705,
-            "y": 5.6589435322897295,
-            "heading": -0.3289345783285442,
-            "angularVelocity": 0.0000011431806365404781,
-            "velocityX": 2.106068756935774,
-            "velocityY": 3.1424689275472675,
-            "timestamp": 0.6192128437247234
-        },
-        {
-            "x": 1.7007381584918697,
-            "y": 5.853529244388756,
-            "heading": -0.32893450754133247,
-            "angularVelocity": 0.000001143180612539941,
-            "velocityX": 2.1060687568221708,
-            "velocityY": 3.1424689276234057,
-            "timestamp": 0.6811341280971958
-        },
-        {
-            "x": 1.8311480951447636,
-            "y": 6.048115322242248,
-            "heading": -0.3289344367512267,
-            "angularVelocity": 0.0000011432273482693713,
-            "velocityX": 2.1060599432732174,
-            "velocityY": 3.1424748343882203,
-            "timestamp": 0.7430554124696681
-        },
-        {
-            "x": 1.9446381116074218,
-            "y": 6.226114434087632,
-            "heading": -0.2658064774598036,
-            "angularVelocity": 1.0194872398268149,
-            "velocityX": 1.8328110860877296,
-            "velocityY": 2.8746030326934724,
-            "timestamp": 0.8049766968421405
-        },
-        {
-            "x": 2.0457809471284243,
-            "y": 6.380047389358405,
-            "heading": -0.1375002927378408,
-            "angularVelocity": 2.0720853261080365,
-            "velocityX": 1.6334098451931771,
-            "velocityY": 2.485945775039604,
-            "timestamp": 0.8668979812146128
-        },
-        {
-            "x": 2.1306775060879315,
-            "y": 6.50749359169558,
-            "heading": 0.01763716177237307,
-            "angularVelocity": 2.5053978786522335,
-            "velocityX": 1.3710400199200075,
-            "velocityY": 2.0581970097802293,
-            "timestamp": 0.9288192655870852
-        },
-        {
-            "x": 2.198423978678441,
-            "y": 6.608249711273454,
-            "heading": 0.15869859646801654,
-            "angularVelocity": 2.278076692452355,
-            "velocityX": 1.0940740857860296,
-            "velocityY": 1.6271645622174213,
-            "timestamp": 0.9907405499595575
-        },
-        {
-            "x": 2.24920191473134,
-            "y": 6.683037424747583,
-            "heading": 0.27235604329703667,
-            "angularVelocity": 1.8355150087866652,
-            "velocityX": 0.8200400971571714,
-            "velocityY": 1.2077868576540292,
-            "timestamp": 1.0526618343320298
-        },
-        {
-            "x": 2.2830642302273354,
-            "y": 6.732456156660043,
-            "heading": 0.35192243545091534,
-            "angularVelocity": 1.2849603001653922,
-            "velocityX": 0.5468606770541943,
-            "velocityY": 0.7980895812043076,
-            "timestamp": 1.114583118704502
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.6633836000880191,
-            "velocityX": 0.2735048205846554,
-            "velocityY": 0.3963716771816175,
-            "timestamp": 1.1765044030769745
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0,
-            "velocityX": 2.8187230537991962e-33,
-            "velocityY": 0,
-            "timestamp": 1.2384256874494468
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Stage Side To W2.traj b/src/main/deploy/choreo/Stage Side To W2.traj
deleted file mode 100644
index f9120604..00000000
--- a/src/main/deploy/choreo/Stage Side To W2.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": 2.3377521162047767e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.737441410859622,
-            "y": 4.3811882786622816,
-            "heading": -1.0071421705955907,
-            "angularVelocity": 0.501398563530153,
-            "velocityX": 0.41874598035403643,
-            "velocityY": 0.32332545020514236,
-            "timestamp": 0.06553235648117978
-        },
-        {
-            "x": 0.7924436512054522,
-            "y": 4.423719236654154,
-            "heading": -0.9443396668116766,
-            "angularVelocity": 0.9583434375956188,
-            "velocityX": 0.8393142456524665,
-            "velocityY": 0.6490069986127681,
-            "timestamp": 0.13106471296235955
-        },
-        {
-            "x": 0.8751754235694627,
-            "y": 4.487775832809834,
-            "heading": -0.8558630826468019,
-            "angularVelocity": 1.350120595621899,
-            "velocityX": 1.2624568504227318,
-            "velocityY": 0.9774804324956131,
-            "timestamp": 0.19659706944353933
-        },
-        {
-            "x": 0.9858651777004729,
-            "y": 4.5735665588886505,
-            "heading": -0.748264717894899,
-            "angularVelocity": 1.641912034443748,
-            "velocityX": 1.6890855155315267,
-            "velocityY": 1.3091353750334698,
-            "timestamp": 0.2621294259247191
-        },
-        {
-            "x": 1.1247487447497322,
-            "y": 4.6813201098081265,
-            "heading": -0.6327126866941822,
-            "angularVelocity": 1.7632821007116781,
-            "velocityX": 2.1193128785036808,
-            "velocityY": 1.644280119095407,
-            "timestamp": 0.3276617824058989
-        },
-        {
-            "x": 1.291489149115039,
-            "y": 4.810947116825512,
-            "heading": -0.5360472598717524,
-            "angularVelocity": 1.4750793655678647,
-            "velocityX": 2.5443981159626317,
-            "velocityY": 1.9780611285451524,
-            "timestamp": 0.39319413888707866
-        },
-        {
-            "x": 1.4799949106081978,
-            "y": 4.9607414161097925,
-            "heading": -0.5171264547525753,
-            "angularVelocity": 0.288724626049585,
-            "velocityX": 2.876529574322522,
-            "velocityY": 2.285806696533185,
-            "timestamp": 0.45872649536825844
-        },
-        {
-            "x": 1.6682619137790426,
-            "y": 5.11061369240764,
-            "heading": -0.49798057816746544,
-            "angularVelocity": 0.29215913501614493,
-            "velocityX": 2.8728862088900566,
-            "velocityY": 2.2869965974882214,
-            "timestamp": 0.5242588518494382
-        },
-        {
-            "x": 1.8349963336194852,
-            "y": 5.239757443274396,
-            "heading": -0.40436368242247644,
-            "angularVelocity": 1.4285598866244509,
-            "velocityX": 2.54430679428301,
-            "velocityY": 1.970686814899508,
-            "timestamp": 0.589791208330618
-        },
-        {
-            "x": 1.9739513715778099,
-            "y": 5.347175023297594,
-            "heading": -0.28864983484418905,
-            "angularVelocity": 1.7657513599639834,
-            "velocityX": 2.120403498662996,
-            "velocityY": 1.6391533250303247,
-            "timestamp": 0.6553235648117979
-        },
-        {
-            "x": 2.0846889840994356,
-            "y": 5.432668431655269,
-            "heading": -0.18116230899252247,
-            "angularVelocity": 1.6402206730126385,
-            "velocityX": 1.6898158172204303,
-            "velocityY": 1.304598414406612,
-            "timestamp": 0.7208559212929777
-        },
-        {
-            "x": 2.167475833751633,
-            "y": 5.496492915332372,
-            "heading": -0.09361421148490373,
-            "angularVelocity": 1.3359522258711838,
-            "velocityX": 1.2632973098712375,
-            "velocityY": 0.973938480228949,
-            "timestamp": 0.7863882777741575
-        },
-        {
-            "x": 2.2225290058379836,
-            "y": 5.538875443360028,
-            "heading": -0.032000086006940494,
-            "angularVelocity": 0.9402092155141033,
-            "velocityX": 0.8400914455466154,
-            "velocityY": 0.6467420111747162,
-            "timestamp": 0.8519206342553374
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 2.4308347188047162e-30,
-            "angularVelocity": 0.48830971027463277,
-            "velocityX": 0.41919741082263007,
-            "velocityY": 0.3223530752482305,
-            "timestamp": 0.9174529907365172
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 1.7485008040489027e-29,
-            "angularVelocity": 1.1123032826758857e-30,
-            "velocityX": 0,
-            "velocityY": 1.931278949788811e-31,
-            "timestamp": 0.982985347217697
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/Stage Side To W3.traj b/src/main/deploy/choreo/Stage Side To W3.traj
deleted file mode 100644
index ff6d2947..00000000
--- a/src/main/deploy/choreo/Stage Side To W3.traj	
+++ /dev/null
@@ -1,121 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.71,
-            "y": 4.36,
-            "heading": -1.04,
-            "angularVelocity": -2.204776300650663e-32,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7541176231747116,
-            "y": 4.360036687345491,
-            "heading": -1.0162545684059647,
-            "angularVelocity": 0.32461244675544165,
-            "velocityX": 0.6031109414489596,
-            "velocityY": 0.0005015351663374791,
-            "timestamp": 0.07315009584923313
-        },
-        {
-            "x": 0.8424088865359771,
-            "y": 4.360100324606402,
-            "heading": -0.9701479863262087,
-            "angularVelocity": 0.630301048063541,
-            "velocityX": 1.2069876646948012,
-            "velocityY": 0.0008699545799604022,
-            "timestamp": 0.14630019169846625
-        },
-        {
-            "x": 0.974955221752511,
-            "y": 4.3601789275184695,
-            "heading": -0.9038423136459668,
-            "angularVelocity": 0.9064331619859869,
-            "velocityX": 1.8119776013652948,
-            "velocityY": 0.0010745428444254071,
-            "timestamp": 0.21945028754769938
-        },
-        {
-            "x": 1.151884261990258,
-            "y": 4.360259672825357,
-            "heading": -0.8212724833638784,
-            "angularVelocity": 1.128772687492223,
-            "velocityX": 2.4187123500481573,
-            "velocityY": 0.0011038305000959348,
-            "timestamp": 0.2926003833969325
-        },
-        {
-            "x": 1.3733948258568207,
-            "y": 4.360324542882412,
-            "heading": -0.7323749312717828,
-            "angularVelocity": 1.215275948172809,
-            "velocityX": 3.028165052894887,
-            "velocityY": 0.0008868075468144582,
-            "timestamp": 0.36575047924616566
-        },
-        {
-            "x": 1.6366160230983884,
-            "y": 4.359789775146019,
-            "heading": -0.6992109641912426,
-            "angularVelocity": 0.4533687440268879,
-            "velocityX": 3.5983711871554758,
-            "velocityY": -0.007310554144235059,
-            "timestamp": 0.4389005750953988
-        },
-        {
-            "x": 1.8581232786849116,
-            "y": 4.3598960912826,
-            "heading": -0.6098314600860953,
-            "angularVelocity": 1.2218644838062247,
-            "velocityX": 3.0281198269795637,
-            "velocityY": 0.001453397091854858,
-            "timestamp": 0.512050670944632
-        },
-        {
-            "x": 2.035050245808528,
-            "y": 4.359956526837273,
-            "heading": -0.5268891037054299,
-            "angularVelocity": 1.1338653137464598,
-            "velocityX": 2.4186840094956525,
-            "velocityY": 0.000826185584123381,
-            "timestamp": 0.5852007667938651
-        },
-        {
-            "x": 2.1675937895751547,
-            "y": 4.359986275638159,
-            "heading": -0.46022423203591045,
-            "angularVelocity": 0.9113435996982586,
-            "velocityX": 1.8119394407877087,
-            "velocityY": 0.00040668163915013244,
-            "timestamp": 0.6583508626430983
-        },
-        {
-            "x": 2.255882919836075,
-            "y": 4.359997704013322,
-            "heading": -0.41385948514831483,
-            "angularVelocity": 0.6338302957681125,
-            "velocityX": 1.2069585040994306,
-            "velocityY": 0.0001562318549097459,
-            "timestamp": 0.7315009584923314
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0.32617161838698483,
-            "velocityX": 0.6031035182080025,
-            "velocityY": 0.00003138733655167869,
-            "timestamp": 0.8046510543415646
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -2.2755735462869925e-32,
-            "velocityX": -9.780003655586715e-35,
-            "velocityY": 0,
-            "timestamp": 0.8778011501907977
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W1 To C1.traj b/src/main/deploy/choreo/W1 To C1.traj
deleted file mode 100644
index b6c78aaa..00000000
--- a/src/main/deploy/choreo/W1 To C1.traj	
+++ /dev/null
@@ -1,202 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 6.154725095706714e-32,
-            "velocityX": 1.2115062621827067e-33,
-            "velocityY": 6.540799795634554e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3666999539229265,
-            "y": 6.765820977804648,
-            "heading": 0.36628259128993745,
-            "angularVelocity": -0.26339586361920003,
-            "velocityX": 0.7407424724032508,
-            "velocityY": 0.09796218023747214,
-            "timestamp": 0.09004472729439511
-        },
-        {
-            "x": 2.5001846245776895,
-            "y": 6.783480891306791,
-            "heading": 0.3222458902940237,
-            "angularVelocity": -0.4890536327789505,
-            "velocityX": 1.4824262859761226,
-            "velocityY": 0.1961237935054848,
-            "timestamp": 0.18008945458879022
-        },
-        {
-            "x": 2.70056880915549,
-            "y": 6.810010034997764,
-            "heading": 0.26375227569196347,
-            "angularVelocity": -0.6496062163731077,
-            "velocityX": 2.225384990313294,
-            "velocityY": 0.2946218450330504,
-            "timestamp": 0.27013418188318533
-        },
-        {
-            "x": 2.967965472322839,
-            "y": 6.8454603445891635,
-            "heading": 0.20325713210858826,
-            "angularVelocity": -0.6718343805472418,
-            "velocityX": 2.9695982341432847,
-            "velocityY": 0.3936966733820705,
-            "timestamp": 0.36017890917758044
-        },
-        {
-            "x": 3.3011740475809854,
-            "y": 6.889827568786931,
-            "heading": 0.1886164627156386,
-            "angularVelocity": -0.16259330038374148,
-            "velocityX": 3.700478476310374,
-            "velocityY": 0.4927242885939573,
-            "timestamp": 0.45022363647197555
-        },
-        {
-            "x": 3.6388963576772766,
-            "y": 6.934269203744266,
-            "heading": 0.18861644342798295,
-            "angularVelocity": -2.1420083336262225e-7,
-            "velocityX": 3.7506061736644574,
-            "velocityY": 0.4935506641275844,
-            "timestamp": 0.5402683637663707
-        },
-        {
-            "x": 3.9766186774172247,
-            "y": 6.978710765417191,
-            "heading": 0.1886164241403376,
-            "angularVelocity": -2.1420071941558326e-7,
-            "velocityX": 3.750606280762983,
-            "velocityY": 0.4935498502608198,
-            "timestamp": 0.6303130910607657
-        },
-        {
-            "x": 4.314341007224031,
-            "y": 7.023152250589549,
-            "heading": 0.18861640485269274,
-            "angularVelocity": -2.1420071403103001e-7,
-            "velocityX": 3.750606392561398,
-            "velocityY": 0.4935490006767261,
-            "timestamp": 0.7203578183551609
-        },
-        {
-            "x": 4.652063347871557,
-            "y": 7.067593653380398,
-            "heading": 0.1886163855650432,
-            "angularVelocity": -2.1420076555553473e-7,
-            "velocityX": 3.750606512954019,
-            "velocityY": 0.49354808578132814,
-            "timestamp": 0.810402545649556
-        },
-        {
-            "x": 4.989785700043158,
-            "y": 7.112034968596579,
-            "heading": 0.18861636627738598,
-            "angularVelocity": -2.1420085095836123e-7,
-            "velocityX": 3.7506066409356835,
-            "velocityY": 0.4935471132127838,
-            "timestamp": 0.9004472729439512
-        },
-        {
-            "x": 5.327508064058316,
-            "y": 7.15647619381006,
-            "heading": 0.18861634698972354,
-            "angularVelocity": -2.1420090940402864e-7,
-            "velocityX": 3.7506067724653955,
-            "velocityY": 0.4935461136795145,
-            "timestamp": 0.9904920002383464
-        },
-        {
-            "x": 5.6652304398823485,
-            "y": 7.200917329284216,
-            "heading": 0.18861632770206457,
-            "angularVelocity": -2.1420087020950116e-7,
-            "velocityX": 3.750606903609938,
-            "velocityY": 0.4935451170711973,
-            "timestamp": 1.0805367275327415
-        },
-        {
-            "x": 6.002952827157885,
-            "y": 7.245358377734637,
-            "heading": 0.18861630841441618,
-            "angularVelocity": -2.1420075320811155e-7,
-            "velocityX": 3.750607030785673,
-            "velocityY": 0.49354415062109597,
-            "timestamp": 1.1705814548271367
-        },
-        {
-            "x": 6.340675225087303,
-            "y": 7.289799345222555,
-            "heading": 0.1886162891267749,
-            "angularVelocity": -2.1420067405327174e-7,
-            "velocityX": 3.750607149103328,
-            "velocityY": 0.4935432514845822,
-            "timestamp": 1.2606261821215319
-        },
-        {
-            "x": 6.678397632133336,
-            "y": 7.334240243430023,
-            "heading": 0.18861626983912938,
-            "angularVelocity": -2.1420072105835628e-7,
-            "velocityX": 3.7506072503487453,
-            "velocityY": 0.4935424820841753,
-            "timestamp": 1.350670909415927
-        },
-        {
-            "x": 7.011808183476814,
-            "y": 7.378579327724673,
-            "heading": 0.17443906284640512,
-            "angularVelocity": -0.15744627607536488,
-            "velocityX": 3.7027215403008924,
-            "velocityY": 0.49241177831197624,
-            "timestamp": 1.4407156367103222
-        },
-        {
-            "x": 7.279274281127278,
-            "y": 7.414016049929063,
-            "heading": 0.11701095642225132,
-            "angularVelocity": -0.637773117313093,
-            "velocityX": 2.9703693451811146,
-            "velocityY": 0.3935457774060549,
-            "timestamp": 1.5307603640047174
-        },
-        {
-            "x": 7.479727163444043,
-            "y": 7.440534681549159,
-            "heading": 0.061439635089441606,
-            "angularVelocity": -0.6171524197205129,
-            "velocityX": 2.226147919371209,
-            "velocityY": 0.294505102263186,
-            "timestamp": 1.6208050912991125
-        },
-        {
-            "x": 7.613267962945412,
-            "y": 7.458185352165826,
-            "heading": 0.019571051398075176,
-            "angularVelocity": -0.46497540666073534,
-            "velocityX": 1.4830496300440337,
-            "velocityY": 0.19602114579078314,
-            "timestamp": 1.7108498185935077
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -0.003,
-            "angularVelocity": -0.2506648870652985,
-            "velocityX": 0.7410987745724689,
-            "velocityY": 0.09789188216823574,
-            "timestamp": 1.8008945458879029
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -0.003,
-            "angularVelocity": 5.335131961970383e-32,
-            "velocityX": 0,
-            "velocityY": -3.2330177098423496e-35,
-            "timestamp": 1.890939273182298
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W1 To C2.traj b/src/main/deploy/choreo/W1 To C2.traj
deleted file mode 100644
index cd2bebdf..00000000
--- a/src/main/deploy/choreo/W1 To C2.traj	
+++ /dev/null
@@ -1,256 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 2.274971502954598e-31,
-            "velocityX": 3.487335890979115e-33,
-            "velocityY": 2.862188318424697e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3417817158351957,
-            "y": 6.761449131523058,
-            "heading": 0.37953631669999827,
-            "angularVelocity": -0.14722299547472617,
-            "velocityX": 0.5878646347531417,
-            "velocityY": 0.06259884318982863,
-            "timestamp": 0.07107370194626751
-        },
-        {
-            "x": 2.42534939307215,
-            "y": 6.770350816518497,
-            "heading": 0.3588464620969251,
-            "angularVelocity": -0.2911042204995314,
-            "velocityX": 1.1757890042104941,
-            "velocityY": 0.12524583287024788,
-            "timestamp": 0.14214740389253502
-        },
-        {
-            "x": 2.5507092244237364,
-            "y": 6.783710205716013,
-            "heading": 0.3282884162089386,
-            "angularVelocity": -0.42994870185747563,
-            "velocityX": 1.7638005045292426,
-            "velocityY": 0.18796529281121122,
-            "timestamp": 0.21322110583880255
-        },
-        {
-            "x": 2.717871083967313,
-            "y": 6.80153596037414,
-            "heading": 0.2884653946252666,
-            "angularVelocity": -0.5603059991696991,
-            "velocityX": 2.351950932145827,
-            "velocityY": 0.25080661580833774,
-            "timestamp": 0.28429480778507005
-        },
-        {
-            "x": 2.926852904271386,
-            "y": 6.82384561239607,
-            "heading": 0.2406022074409234,
-            "angularVelocity": -0.673430338841082,
-            "velocityX": 2.9403536692385535,
-            "velocityY": 0.31389461096009597,
-            "timestamp": 0.35536850973133755
-        },
-        {
-            "x": 3.1776924244019935,
-            "y": 6.85069218229951,
-            "heading": 0.1884744580400949,
-            "angularVelocity": -0.7334323100298853,
-            "velocityX": 3.529287391280776,
-            "velocityY": 0.3777285995842137,
-            "timestamp": 0.42644221167760504
-        },
-        {
-            "x": 3.4450596021465354,
-            "y": 6.879057072545224,
-            "heading": 0.1884744481252109,
-            "angularVelocity": -1.3950144320454384e-7,
-            "velocityX": 3.7618299092775445,
-            "velocityY": 0.39909121755204374,
-            "timestamp": 0.49751591362387254
-        },
-        {
-            "x": 3.712426777772062,
-            "y": 6.907421982754425,
-            "heading": 0.18847443820909762,
-            "angularVelocity": -1.3951873937625683e-7,
-            "velocityX": 3.7618298794631864,
-            "velocityY": 0.39909149843660235,
-            "timestamp": 0.5685896155701401
-        },
-        {
-            "x": 3.979793955045072,
-            "y": 6.93578687743453,
-            "heading": 0.188474428292985,
-            "angularVelocity": -1.3951872987555026e-7,
-            "velocityX": 3.7618299026431106,
-            "velocityY": 0.3990912799437423,
-            "timestamp": 0.6396633175164076
-        },
-        {
-            "x": 4.247161134956238,
-            "y": 6.964151747247427,
-            "heading": 0.18847441837687146,
-            "angularVelocity": -1.395187425580786e-7,
-            "velocityX": 3.761829939761708,
-            "velocityY": 0.399090930064572,
-            "timestamp": 0.7107370194626751
-        },
-        {
-            "x": 4.514528318048531,
-            "y": 6.992516587075052,
-            "heading": 0.18847440846075478,
-            "angularVelocity": -1.3951878716159625e-7,
-            "velocityX": 3.7618299845198404,
-            "velocityY": 0.39909050817474234,
-            "timestamp": 0.7818107214089426
-        },
-        {
-            "x": 4.781895504744899,
-            "y": 7.020881392930694,
-            "heading": 0.18847439854463183,
-            "angularVelocity": -1.395188750932112e-7,
-            "velocityX": 3.761830035228834,
-            "velocityY": 0.3990900301922755,
-            "timestamp": 0.8528844233552101
-        },
-        {
-            "x": 5.049262689335958,
-            "y": 7.049246218630208,
-            "heading": 0.18847438862849922,
-            "angularVelocity": -1.3951901124406535e-7,
-            "velocityX": 3.7618300056073353,
-            "velocityY": 0.3990903093936624,
-            "timestamp": 0.9239581253014776
-        },
-        {
-            "x": 5.3166271708653765,
-            "y": 7.077636511969196,
-            "heading": 0.18847437871238637,
-            "angularVelocity": -1.3951873300413244e-7,
-            "velocityX": 3.761791973795463,
-            "velocityY": 0.3994486365804798,
-            "timestamp": 0.9950318272477451
-        },
-        {
-            "x": 5.584732592713635,
-            "y": 7.097866624574175,
-            "heading": 0.18847436874075538,
-            "angularVelocity": -1.4029986815741496e-7,
-            "velocityX": 3.772216931248832,
-            "velocityY": 0.28463569578063386,
-            "timestamp": 1.0661055291940127
-        },
-        {
-            "x": 5.85271010689652,
-            "y": 7.076007397472714,
-            "heading": 0.188474358062239,
-            "angularVelocity": -1.5024567569917294e-7,
-            "velocityX": 3.7704172829702145,
-            "velocityY": -0.3075571766057785,
-            "timestamp": 1.1371792311402802
-        },
-        {
-            "x": 6.113985538482666,
-            "y": 7.0125651359558105,
-            "heading": 0.18847434515977443,
-            "angularVelocity": -1.8153640819270364e-7,
-            "velocityX": 3.6761196396335167,
-            "velocityY": -0.8926263833128883,
-            "timestamp": 1.2082529330865477
-        },
-        {
-            "x": 6.38971197835489,
-            "y": 6.891444245730791,
-            "heading": 0.18847433517810044,
-            "angularVelocity": -1.2538350348338093e-7,
-            "velocityX": 3.4635019217336325,
-            "velocityY": -1.521444357133088,
-            "timestamp": 1.2878620818911364
-        },
-        {
-            "x": 6.640022696468843,
-            "y": 6.723993100546061,
-            "heading": 0.18847432674418188,
-            "angularVelocity": -1.0594157434839283e-7,
-            "velocityX": 3.1442456284571,
-            "velocityY": -2.103415847287377,
-            "timestamp": 1.367471230695725
-        },
-        {
-            "x": 6.8648306229514935,
-            "y": 6.523601637914116,
-            "heading": 0.18847431867026868,
-            "angularVelocity": -1.014194138550125e-7,
-            "velocityX": 2.8238956182605124,
-            "velocityY": -2.5171913736164258,
-            "timestamp": 1.4470803795003138
-        },
-        {
-            "x": 7.089642217529903,
-            "y": 6.3232142903896476,
-            "heading": 0.1884743105959038,
-            "angularVelocity": -1.0142508708190059e-7,
-            "velocityX": 2.823941694568014,
-            "velocityY": -2.517139682229462,
-            "timestamp": 1.5266895283049025
-        },
-        {
-            "x": 7.2864279243188,
-            "y": 6.147809259471006,
-            "heading": 0.12465186300240168,
-            "angularVelocity": -0.8016973997575559,
-            "velocityX": 2.471898139143281,
-            "velocityY": -2.203327551576542,
-            "timestamp": 1.6062986771094911
-        },
-        {
-            "x": 7.443856673448633,
-            "y": 6.007485445692029,
-            "heading": 0.07359246308164451,
-            "angularVelocity": -0.6413760313670541,
-            "velocityX": 1.9775208188222257,
-            "velocityY": -1.7626593913656032,
-            "timestamp": 1.6859078259140798
-        },
-        {
-            "x": 7.561928317357885,
-            "y": 5.902242696623479,
-            "heading": 0.0352966131369852,
-            "angularVelocity": -0.4810483533581169,
-            "velocityX": 1.483141644926293,
-            "velocityY": -1.3219931458747511,
-            "timestamp": 1.7655169747186685
-        },
-        {
-            "x": 7.640642772199918,
-            "y": 5.832080900256126,
-            "heading": 0.009765510491971057,
-            "angularVelocity": -0.320705635324694,
-            "velocityX": 0.9887614178017027,
-            "velocityY": -0.8813283073733266,
-            "timestamp": 1.8451261235232572
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -0.003,
-            "angularVelocity": -0.1603523047748632,
-            "velocityX": 0.4943807136624207,
-            "velocityY": -0.44066417971947147,
-            "timestamp": 1.924735272327846
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -0.003,
-            "angularVelocity": 9.952815155734959e-29,
-            "velocityX": 0,
-            "velocityY": 1.270429811278963e-30,
-            "timestamp": 2.004344421132435
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W1 To C3.traj b/src/main/deploy/choreo/W1 To C3.traj
deleted file mode 100644
index a91e184b..00000000
--- a/src/main/deploy/choreo/W1 To C3.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3668131239789423,
-            "y": 6.6799532352680275,
-            "heading": 0.31688592464549686,
-            "angularVelocity": -0.6546901107445974,
-            "velocityX": 0.5982690928509629,
-            "velocityY": -0.6899048465062367,
-            "timestamp": 0.11167737858654542
-        },
-        {
-            "x": 2.5010623988117353,
-            "y": 6.525329092787392,
-            "heading": 0.1875032700506318,
-            "angularVelocity": -1.1585395022018103,
-            "velocityX": 1.202116995688263,
-            "velocityY": -1.3845609955896643,
-            "timestamp": 0.22335475717309083
-        },
-        {
-            "x": 2.7034647042549174,
-            "y": 6.29238592031536,
-            "heading": 0.03684167640916454,
-            "angularVelocity": -1.3490788873122188,
-            "velocityX": 1.8123841014618285,
-            "velocityY": -2.0858581694904785,
-            "timestamp": 0.33503213575963625
-        },
-        {
-            "x": 2.970402662431944,
-            "y": 5.984552676414473,
-            "heading": 2.692880535790558e-7,
-            "angularVelocity": -0.32989140314179594,
-            "velocityX": 2.39025988571325,
-            "velocityY": -2.7564511971627637,
-            "timestamp": 0.44670951434618167
-        },
-        {
-            "x": 3.247779632307318,
-            "y": 5.665896160181276,
-            "heading": 2.2674846776964554e-7,
-            "angularVelocity": -3.809149744305609e-7,
-            "velocityX": 2.4837346057547505,
-            "velocityY": -2.853366727141142,
-            "timestamp": 0.5583868929327271
-        },
-        {
-            "x": 3.5251566242922663,
-            "y": 5.347239663193555,
-            "heading": 1.842089099097335e-7,
-            "angularVelocity": -3.8091472416520575e-7,
-            "velocityX": 2.4837348037318896,
-            "velocityY": -2.853366554810183,
-            "timestamp": 0.6700642715192726
-        },
-        {
-            "x": 3.8025335437012195,
-            "y": 5.0285831030297485,
-            "heading": 1.4166936288049968e-7,
-            "angularVelocity": -3.8091462718327505e-7,
-            "velocityX": 2.4837341538598574,
-            "velocityY": -2.8533671205119875,
-            "timestamp": 0.7817416501058181
-        },
-        {
-            "x": 4.079910865419093,
-            "y": 4.7099268930580935,
-            "heading": 9.912982095474378e-8,
-            "angularVelocity": -3.809145814844431e-7,
-            "velocityX": 2.4837377562807075,
-            "velocityY": -2.853363984763686,
-            "timestamp": 0.8934190286923636
-        },
-        {
-            "x": 4.382964098552189,
-            "y": 4.415582373772677,
-            "heading": 5.542611481257948e-8,
-            "angularVelocity": -3.9133893269333644e-7,
-            "velocityX": 2.713649236476739,
-            "velocityY": -2.635668234792112,
-            "timestamp": 1.005096407278909
-        },
-        {
-            "x": 4.748738765716553,
-            "y": 4.2041850090026855,
-            "heading": 0,
-            "angularVelocity": -4.963056575446211e-7,
-            "velocityX": 3.27527984443961,
-            "velocityY": -1.8929291450565913,
-            "timestamp": 1.1167737858654545
-        },
-        {
-            "x": 5.133803316447558,
-            "y": 4.089523361787239,
-            "heading": -1.387645596172588e-10,
-            "angularVelocity": -1.3065520492479572e-9,
-            "velocityX": 3.625615065458012,
-            "velocityY": -1.0796085871456598,
-            "timestamp": 1.2229804690844188
-        },
-        {
-            "x": 5.534974194059096,
-            "y": 4.0675249151482635,
-            "heading": -2.526284831735191e-10,
-            "angularVelocity": -1.0720975375429818e-9,
-            "velocityX": 3.7772658504404184,
-            "velocityY": -0.20712864739049783,
-            "timestamp": 1.3291871523033831
-        },
-        {
-            "x": 5.9365934232146325,
-            "y": 4.0786604378791305,
-            "heading": -3.6352969398434843e-10,
-            "angularVelocity": -1.0442018095219078e-9,
-            "velocityX": 3.781487350730336,
-            "velocityY": 0.10484766488641149,
-            "timestamp": 1.4353938355223474
-        },
-        {
-            "x": 6.3382126442540345,
-            "y": 4.089796253283455,
-            "heading": -4.744184411388132e-10,
-            "angularVelocity": -1.044084456790862e-9,
-            "velocityX": 3.781487274312031,
-            "velocityY": 0.1048504205837075,
-            "timestamp": 1.5416005187413118
-        },
-        {
-            "x": 6.739831868633552,
-            "y": 4.100931948201603,
-            "heading": -5.853040774969199e-10,
-            "angularVelocity": -1.0440551668485128e-9,
-            "velocityX": 3.7814873057612264,
-            "velocityY": 0.10484928613381475,
-            "timestamp": 1.647807201960276
-        },
-        {
-            "x": 7.11589910333758,
-            "y": 4.111359168329562,
-            "heading": -0.0011999964635320353,
-            "angularVelocity": -0.011298685184913941,
-            "velocityX": 3.5408999067290075,
-            "velocityY": 0.09817856854143046,
-            "timestamp": 1.7540138851792404
-        },
-        {
-            "x": 7.397949547057075,
-            "y": 4.119179583905487,
-            "heading": -0.002099997192055183,
-            "angularVelocity": -0.008474049854922212,
-            "velocityX": 2.6556750966226486,
-            "velocityY": 0.0736339309252122,
-            "timestamp": 1.8602205683982047
-        },
-        {
-            "x": 7.585983181328134,
-            "y": 4.124393194553852,
-            "heading": -0.0026999988369627403,
-            "angularVelocity": -0.005649377484767236,
-            "velocityX": 1.7704501126675238,
-            "velocityY": 0.049089289773120645,
-            "timestamp": 1.966427251617169
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -0.003,
-            "angularVelocity": -0.0028246919491873987,
-            "velocityX": 0.8852250707992916,
-            "velocityY": 0.024544646034960003,
-            "timestamp": 2.072633934836133
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -0.003,
-            "angularVelocity": 0,
-            "velocityX": 1.6661554323284825e-27,
-            "velocityY": 0,
-            "timestamp": 2.1788406180550974
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W1 To C4.traj b/src/main/deploy/choreo/W1 To C4.traj
deleted file mode 100644
index 785ed7b8..00000000
--- a/src/main/deploy/choreo/W1 To C4.traj	
+++ /dev/null
@@ -1,400 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 1.592522039385819e-32,
-            "velocityX": 0,
-            "velocityY": -2.1348488243236776e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 2.311435758812473,
-            "y": 6.743296066265471,
-            "heading": 0.3325883455828011,
-            "angularVelocity": -1.0899722636212008,
-            "velocityX": 0.2171102722189281,
-            "velocityY": -0.26017204737900074,
-            "timestamp": 0.052672582902671385
-        },
-        {
-            "x": 2.3359939039892414,
-            "y": 6.71538911279917,
-            "heading": 0.2289766220305027,
-            "angularVelocity": -1.9670902363712421,
-            "velocityX": 0.46624152117519013,
-            "velocityY": -0.529819346772266,
-            "timestamp": 0.10534516580534277
-        },
-        {
-            "x": 2.3766647023789824,
-            "y": 6.674615688796359,
-            "heading": 0.08902544501665462,
-            "angularVelocity": -2.6570023587499336,
-            "velocityX": 0.7721436115045406,
-            "velocityY": -0.7740919802272123,
-            "timestamp": 0.15801774870801416
-        },
-        {
-            "x": 2.4341603545379895,
-            "y": 6.621640281380937,
-            "heading": -0.08739056422595769,
-            "angularVelocity": -3.349294823240299,
-            "velocityX": 1.0915669783129407,
-            "velocityY": -1.0057491866937662,
-            "timestamp": 0.21069033161068554
-        },
-        {
-            "x": 2.508963105088746,
-            "y": 6.555426903517877,
-            "heading": -0.2922468734649172,
-            "angularVelocity": -3.8892398654057443,
-            "velocityX": 1.4201458601143102,
-            "velocityY": -1.2570748236404126,
-            "timestamp": 0.2633629145133569
-        },
-        {
-            "x": 2.602244928931498,
-            "y": 6.475307627430128,
-            "heading": -0.4990571722352239,
-            "angularVelocity": -3.9263367652285313,
-            "velocityX": 1.7709749304513536,
-            "velocityY": -1.5210812090949455,
-            "timestamp": 0.3160354974160283
-        },
-        {
-            "x": 2.7117676530035184,
-            "y": 6.382297804514516,
-            "heading": -0.6662019146269795,
-            "angularVelocity": -3.173277883497873,
-            "velocityX": 2.0793118172009177,
-            "velocityY": -1.7658109359755114,
-            "timestamp": 0.3687080803186997
-        },
-        {
-            "x": 2.833611348488951,
-            "y": 6.27498915923685,
-            "heading": -0.7793427065081729,
-            "angularVelocity": -2.1480015910792667,
-            "velocityX": 2.3132280357425485,
-            "velocityY": -2.0372770683364947,
-            "timestamp": 0.42138066322137113
-        },
-        {
-            "x": 2.968521622075034,
-            "y": 6.1546184694434425,
-            "heading": -0.8356933529016568,
-            "angularVelocity": -1.0698288044390758,
-            "velocityX": 2.5612997531442825,
-            "velocityY": -2.2852627146807634,
-            "timestamp": 0.47405324612404254
-        },
-        {
-            "x": 3.1177887530073343,
-            "y": 6.022632523021301,
-            "heading": -0.8357129106693775,
-            "angularVelocity": -0.00037130830959681015,
-            "velocityX": 2.8338676918144654,
-            "velocityY": -2.5057807904736045,
-            "timestamp": 0.5267258290267139
-        },
-        {
-            "x": 3.269422043665646,
-            "y": 5.893362401601714,
-            "heading": -0.8357131785361996,
-            "angularVelocity": -0.000005085507627644201,
-            "velocityX": 2.8787897289658386,
-            "velocityY": -2.45422028493365,
-            "timestamp": 0.5793984119293853
-        },
-        {
-            "x": 3.4210553878779164,
-            "y": 5.764092343000596,
-            "heading": -0.8357134464026715,
-            "angularVelocity": -0.000005085500977911915,
-            "velocityX": 2.8787907456989505,
-            "velocityY": -2.4542190923119205,
-            "timestamp": 0.6320709948320568
-        },
-        {
-            "x": 3.572688732091381,
-            "y": 5.634822284400852,
-            "heading": -0.8357137142690544,
-            "angularVelocity": -0.000005085499289329294,
-            "velocityX": 2.878790745721617,
-            "velocityY": -2.4542190922857987,
-            "timestamp": 0.6847435777347282
-        },
-        {
-            "x": 3.7243220763038787,
-            "y": 5.50555222579995,
-            "heading": -0.8357139821353484,
-            "angularVelocity": -0.000005085497600843703,
-            "velocityX": 2.8787907457032484,
-            "velocityY": -2.4542190923078118,
-            "timestamp": 0.7374161606373996
-        },
-        {
-            "x": 3.875955376681643,
-            "y": 5.376282115781124,
-            "heading": -0.8357142500017658,
-            "angularVelocity": -0.000005085499943938472,
-            "velocityX": 2.878789913491687,
-            "velocityY": -2.4542200684878277,
-            "timestamp": 0.790088743540071
-        },
-        {
-            "x": 4.025639474822054,
-            "y": 5.244759916762783,
-            "heading": -0.8357145286285158,
-            "angularVelocity": -0.000005289787108985714,
-            "velocityX": 2.8417839014463553,
-            "velocityY": -2.4969764490450044,
-            "timestamp": 0.8427613264427424
-        },
-        {
-            "x": 4.159073025842643,
-            "y": 5.096777164207917,
-            "heading": -0.835714908739186,
-            "angularVelocity": -0.000007216480551859566,
-            "velocityX": 2.5332638664625207,
-            "velocityY": -2.809483499761346,
-            "timestamp": 0.8954339093454138
-        },
-        {
-            "x": 4.274456024169922,
-            "y": 4.934326648712158,
-            "heading": -0.8357152799414105,
-            "angularVelocity": -0.000007047351849068254,
-            "velocityX": 2.19057034929318,
-            "velocityY": -3.0841570043363045,
-            "timestamp": 0.9481064922480852
-        },
-        {
-            "x": 4.3556906912761315,
-            "y": 4.793345794539918,
-            "heading": -0.8357152378475459,
-            "angularVelocity": 9.786625189030426e-7,
-            "velocityX": 1.8886677315079998,
-            "velocityY": -3.2777384277013035,
-            "timestamp": 0.9911181166598327
-        },
-        {
-            "x": 4.442446525481621,
-            "y": 4.655693706781043,
-            "heading": -0.8357151935315337,
-            "angularVelocity": 0.0000010303263992905167,
-            "velocityX": 2.0170322649286843,
-            "velocityY": -3.200346177143663,
-            "timestamp": 1.0341297410715804
-        },
-        {
-            "x": 4.541847641111602,
-            "y": 4.5268757636785075,
-            "heading": -0.8357148699573956,
-            "angularVelocity": 0.000007522946236810339,
-            "velocityX": 2.311029099445763,
-            "velocityY": -2.9949564766345187,
-            "timestamp": 1.077141365483328
-        },
-        {
-            "x": 4.65067034095477,
-            "y": 4.409944405402937,
-            "heading": -0.8255512517653536,
-            "angularVelocity": 0.23629933375095027,
-            "velocityX": 2.530076492843306,
-            "velocityY": -2.7185989805033612,
-            "timestamp": 1.1201529898950755
-        },
-        {
-            "x": 4.7668457664948924,
-            "y": 4.306168868140065,
-            "heading": -0.803496324956771,
-            "angularVelocity": 0.5127666557638517,
-            "velocityX": 2.701023900608423,
-            "velocityY": -2.4127323411326107,
-            "timestamp": 1.163164614306823
-        },
-        {
-            "x": 4.88912359332006,
-            "y": 4.216234980411927,
-            "heading": -0.7705722789003837,
-            "angularVelocity": 0.7654685566210554,
-            "velocityX": 2.8429018549638743,
-            "velocityY": -2.0909205117948377,
-            "timestamp": 1.2061762387185706
-        },
-        {
-            "x": 5.016560626655114,
-            "y": 4.140615450479534,
-            "heading": -0.7285529163528696,
-            "angularVelocity": 0.9769303792218019,
-            "velocityX": 2.9628509752411754,
-            "velocityY": -1.7581184381341082,
-            "timestamp": 1.2491878631303182
-        },
-        {
-            "x": 5.148349456504816,
-            "y": 4.079665503809577,
-            "heading": -0.6795461936491729,
-            "angularVelocity": 1.1393832103284065,
-            "velocityX": 3.0640281935900804,
-            "velocityY": -1.4170575397591803,
-            "timestamp": 1.2921994875420657
-        },
-        {
-            "x": 5.28369745675312,
-            "y": 4.033673765712218,
-            "heading": -0.6261187752553977,
-            "angularVelocity": 1.2421623020399764,
-            "velocityX": 3.1467772282354725,
-            "velocityY": -1.0692862389265216,
-            "timestamp": 1.3352111119538133
-        },
-        {
-            "x": 5.421794981888129,
-            "y": 4.0028549700404525,
-            "heading": -0.5712230670658915,
-            "angularVelocity": 1.2762993479156466,
-            "velocityX": 3.2107023862435504,
-            "velocityY": -0.7165224771968601,
-            "timestamp": 1.3782227363655608
-        },
-        {
-            "x": 5.5620425748748135,
-            "y": 3.9873099729843906,
-            "heading": -0.5170015662419462,
-            "angularVelocity": 1.2606243443606422,
-            "velocityX": 3.260690450658235,
-            "velocityY": -0.3614138565716755,
-            "timestamp": 1.4212343607773084
-        },
-        {
-            "x": 5.703982353210449,
-            "y": 3.987083911895752,
-            "heading": -0.46507695218435896,
-            "angularVelocity": 1.207222809362328,
-            "velocityX": 3.3000329626441363,
-            "velocityY": -0.005255813788261737,
-            "timestamp": 1.464245985189056
-        },
-        {
-            "x": 5.957415351644791,
-            "y": 3.94389686592464,
-            "heading": -0.40627705964587574,
-            "angularVelocity": 0.8004851872173009,
-            "velocityX": 3.450165509502976,
-            "velocityY": -0.5879362884366123,
-            "timestamp": 1.5377013013865826
-        },
-        {
-            "x": 6.214659603999478,
-            "y": 3.857155103461619,
-            "heading": -0.3855287596647175,
-            "angularVelocity": 0.2824615161326723,
-            "velocityX": 3.502050847660077,
-            "velocityY": -1.180877939858924,
-            "timestamp": 1.6111566175841092
-        },
-        {
-            "x": 6.460339332530576,
-            "y": 3.727320070546642,
-            "heading": -0.38552868222223635,
-            "angularVelocity": 0.0000010542801416145007,
-            "velocityX": 3.3446146752734305,
-            "velocityY": -1.7675375947717866,
-            "timestamp": 1.6846119337816359
-        },
-        {
-            "x": 6.681847040612612,
-            "y": 3.5595404285567858,
-            "heading": -0.3855286464284339,
-            "angularVelocity": 4.872867523834452e-7,
-            "velocityX": 3.0155435923301614,
-            "velocityY": -2.2841048228379357,
-            "timestamp": 1.7580672499791625
-        },
-        {
-            "x": 6.873376982747171,
-            "y": 3.358214932798401,
-            "heading": -0.3855286139444744,
-            "angularVelocity": 4.422274806376815e-7,
-            "velocityX": 2.607434724254966,
-            "velocityY": -2.740788634235891,
-            "timestamp": 1.8315225661766892
-        },
-        {
-            "x": 7.058369006072979,
-            "y": 3.1508658904491242,
-            "heading": -0.3855285817822777,
-            "angularVelocity": 4.3784709334515964e-7,
-            "velocityX": 2.518429337753462,
-            "velocityY": -2.8227915021385286,
-            "timestamp": 1.9049778823742158
-        },
-        {
-            "x": 7.235640342207717,
-            "y": 2.9543486280986375,
-            "heading": -0.3517392740895451,
-            "angularVelocity": 0.4599981245995971,
-            "velocityX": 2.413322075396734,
-            "velocityY": -2.6753306979448217,
-            "timestamp": 1.9784331985717425
-        },
-        {
-            "x": 7.3842205894533075,
-            "y": 2.7881464841241645,
-            "heading": -0.2579536451103976,
-            "angularVelocity": 1.2767711560447332,
-            "velocityX": 2.0227296666458727,
-            "velocityY": -2.2626292088586646,
-            "timestamp": 2.051888514769269
-        },
-        {
-            "x": 7.502781206912582,
-            "y": 2.6554541566341396,
-            "heading": -0.1644602811973769,
-            "angularVelocity": 1.2727923416953275,
-            "velocityX": 1.6140508760517314,
-            "velocityY": -1.806435998903144,
-            "timestamp": 2.125343830966796
-        },
-        {
-            "x": 7.591494122264987,
-            "y": 2.5561305844890163,
-            "heading": -0.0867965293639426,
-            "angularVelocity": 1.0572924582421084,
-            "velocityX": 1.2077126604948574,
-            "velocityY": -1.3521631555983635,
-            "timestamp": 2.1987991471643227
-        },
-        {
-            "x": 7.6505265885867635,
-            "y": 2.4900164938031613,
-            "heading": -0.031723308715129774,
-            "angularVelocity": 0.7497513250193758,
-            "velocityX": 0.8036513812429008,
-            "velocityY": -0.900058622143418,
-            "timestamp": 2.2722544633618496
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -0.003,
-            "angularVelocity": 0.39103104039319253,
-            "velocityX": 0.401242727401519,
-            "velocityY": -0.4494772538229548,
-            "timestamp": 2.3457097795593764
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -0.003,
-            "angularVelocity": -1.5819357203294863e-32,
-            "velocityX": 5.378736393413154e-34,
-            "velocityY": 0,
-            "timestamp": 2.4191650957569033
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W1 To C5.traj b/src/main/deploy/choreo/W1 To C5.traj
deleted file mode 100644
index 95620590..00000000
--- a/src/main/deploy/choreo/W1 To C5.traj	
+++ /dev/null
@@ -1,616 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 4.884595084783414e-34,
-            "timestamp": 0
-        },
-        {
-            "x": 2.298507420370097,
-            "y": 6.738728491162364,
-            "heading": 0.38429180377773414,
-            "angularVelocity": -0.12150700609071861,
-            "velocityX": -0.031771662206367984,
-            "velocityY": -0.3889348314553929,
-            "timestamp": 0.04697832994094238
-        },
-        {
-            "x": 2.295523042319471,
-            "y": 6.702182630298916,
-            "heading": 0.3729896107436911,
-            "angularVelocity": -0.2405831166891495,
-            "velocityX": -0.06352669527371438,
-            "velocityY": -0.7779301841804652,
-            "timestamp": 0.09395665988188476
-        },
-        {
-            "x": 2.2910478858134478,
-            "y": 6.647358841243747,
-            "heading": 0.35624034482133005,
-            "angularVelocity": -0.3565317443897418,
-            "velocityX": -0.09526001694076108,
-            "velocityY": -1.1670016606398963,
-            "timestamp": 0.14093498982282715
-        },
-        {
-            "x": 2.285083332518943,
-            "y": 6.574252474793301,
-            "heading": 0.33424119989038353,
-            "angularVelocity": -0.46828282228427764,
-            "velocityX": -0.12696392787914923,
-            "velocityY": -1.556172101953168,
-            "timestamp": 0.18791331976376952
-        },
-        {
-            "x": 2.2776313636635503,
-            "y": 6.48285724279221,
-            "heading": 0.3072714314404645,
-            "angularVelocity": -0.5740895532860236,
-            "velocityX": -0.15862566559434335,
-            "velocityY": -1.9454763955207768,
-            "timestamp": 0.23489164970471188
-        },
-        {
-            "x": 2.268695072443392,
-            "y": 6.373164221156064,
-            "heading": 0.2757564747192681,
-            "angularVelocity": -0.6708402951065854,
-            "velocityX": -0.1902215602681591,
-            "velocityY": -2.334970650809517,
-            "timestamp": 0.28186997964565425
-        },
-        {
-            "x": 2.2582799597491072,
-            "y": 6.245160021300657,
-            "heading": 0.24041955506127732,
-            "angularVelocity": -0.752196165815466,
-            "velocityX": -0.22170036072754723,
-            "velocityY": -2.7247499009931837,
-            "timestamp": 0.3288483095865966
-        },
-        {
-            "x": 2.2463982479830333,
-            "y": 6.098824071496616,
-            "heading": 0.20274918029785605,
-            "angularVelocity": -0.8018670482917891,
-            "velocityX": -0.25291899011757896,
-            "velocityY": -3.114967049446051,
-            "timestamp": 0.375826639527539
-        },
-        {
-            "x": 2.2330953714575887,
-            "y": 5.934154199517091,
-            "heading": 0.1675471140087416,
-            "angularVelocity": -0.7493256216934009,
-            "velocityX": -0.28317048609790474,
-            "velocityY": -3.505230436810659,
-            "timestamp": 0.42280496946848134
-        },
-        {
-            "x": 2.218530566609672,
-            "y": 5.757035827571947,
-            "heading": 0.16754710135001605,
-            "angularVelocity": -2.6945882371217095e-7,
-            "velocityX": -0.3100324099691601,
-            "velocityY": -3.770214313020578,
-            "timestamp": 0.4697832994094237
-        },
-        {
-            "x": 2.203965673260931,
-            "y": 5.579917462904045,
-            "heading": 0.16754708869211127,
-            "angularVelocity": -2.694413527531555e-7,
-            "velocityX": -0.3100342938339967,
-            "velocityY": -3.7702141581142206,
-            "timestamp": 0.5167616293503661
-        },
-        {
-            "x": 2.1894007799094983,
-            "y": 5.402799098236364,
-            "heading": 0.16754707603420654,
-            "angularVelocity": -2.694413521606316e-7,
-            "velocityX": -0.31003429389130516,
-            "velocityY": -3.7702141581095083,
-            "timestamp": 0.5637399592913085
-        },
-        {
-            "x": 2.1748358865580655,
-            "y": 5.225680733568683,
-            "heading": 0.1675470633763018,
-            "angularVelocity": -2.6944135250249435e-7,
-            "velocityX": -0.3100342938913069,
-            "velocityY": -3.770214158109508,
-            "timestamp": 0.6107182892322509
-        },
-        {
-            "x": 2.160270993206632,
-            "y": 5.048562368901002,
-            "heading": 0.16754705071839698,
-            "angularVelocity": -2.694413536359467e-7,
-            "velocityX": -0.31003429389130693,
-            "velocityY": -3.7702141581095083,
-            "timestamp": 0.6576966191731933
-        },
-        {
-            "x": 2.1457060998551993,
-            "y": 4.871444004233322,
-            "heading": 0.1675470380604922,
-            "angularVelocity": -2.6944135247689087e-7,
-            "velocityX": -0.3100342938913068,
-            "velocityY": -3.7702141581095083,
-            "timestamp": 0.7046749491141358
-        },
-        {
-            "x": 2.131141206503766,
-            "y": 4.6943256395656405,
-            "heading": 0.16754702540258745,
-            "angularVelocity": -2.694413528693067e-7,
-            "velocityX": -0.31003429389130693,
-            "velocityY": -3.7702141581095083,
-            "timestamp": 0.7516532790550782
-        },
-        {
-            "x": 2.1165763131523327,
-            "y": 4.51720727489796,
-            "heading": 0.16754701274468267,
-            "angularVelocity": -2.694413529636153e-7,
-            "velocityX": -0.31003429389130693,
-            "velocityY": -3.7702141581095083,
-            "timestamp": 0.7986316089960206
-        },
-        {
-            "x": 2.1020114198009,
-            "y": 4.340088910230279,
-            "heading": 0.16754700008677786,
-            "angularVelocity": -2.694413530501801e-7,
-            "velocityX": -0.3100342938913069,
-            "velocityY": -3.770214158109508,
-            "timestamp": 0.845609938936963
-        },
-        {
-            "x": 2.0874465264494666,
-            "y": 4.162970545562599,
-            "heading": 0.16754698742887306,
-            "angularVelocity": -2.694413534833812e-7,
-            "velocityX": -0.31003429389130704,
-            "velocityY": -3.7702141581095083,
-            "timestamp": 0.8925882688779054
-        },
-        {
-            "x": 2.0728816330980333,
-            "y": 3.9858521808949186,
-            "heading": 0.16754697477096828,
-            "angularVelocity": -2.6944135303696276e-7,
-            "velocityX": -0.31003429389130693,
-            "velocityY": -3.770214158109508,
-            "timestamp": 0.9395665988188479
-        },
-        {
-            "x": 2.0583167397466005,
-            "y": 3.8087338162272375,
-            "heading": 0.16754696211306347,
-            "angularVelocity": -2.694413537071192e-7,
-            "velocityX": -0.31003429389130704,
-            "velocityY": -3.7702141581095083,
-            "timestamp": 0.9865449287597903
-        },
-        {
-            "x": 2.043751846395167,
-            "y": 3.631615451559557,
-            "heading": 0.1675469494551587,
-            "angularVelocity": -2.694413527332354e-7,
-            "velocityX": -0.31003429389130716,
-            "velocityY": -3.770214158109508,
-            "timestamp": 1.0335232587007326
-        },
-        {
-            "x": 2.0291869530437365,
-            "y": 3.4544970868918763,
-            "heading": 0.16754693679725388,
-            "angularVelocity": -2.694413532802898e-7,
-            "velocityX": -0.31003429389125564,
-            "velocityY": -3.7702141581095123,
-            "timestamp": 1.080501588641675
-        },
-        {
-            "x": 2.0146220597715567,
-            "y": 3.2773787222176782,
-            "heading": 0.1675469241393491,
-            "angularVelocity": -2.6944135307750017e-7,
-            "velocityX": -0.31003429220428774,
-            "velocityY": -3.7702141582482356,
-            "timestamp": 1.1274799185826172
-        },
-        {
-            "x": 2.0000597716588953,
-            "y": 3.1002601433334096,
-            "heading": 0.16754691148142445,
-            "angularVelocity": -2.6944177624221033e-7,
-            "velocityX": -0.30997883771037654,
-            "velocityY": -3.770218718011654,
-            "timestamp": 1.1744582485235595
-        },
-        {
-            "x": 2.001608158601577,
-            "y": 2.922550679334096,
-            "heading": 0.16754689857982888,
-            "angularVelocity": -2.7462865480192305e-7,
-            "velocityX": 0.032959599556405594,
-            "velocityY": -3.7827965409310345,
-            "timestamp": 1.2214365784645018
-        },
-        {
-            "x": 2.021524410839774,
-            "y": 2.7459539818451146,
-            "heading": 0.1675468848578902,
-            "angularVelocity": -2.9209081516381327e-7,
-            "velocityX": 0.4239455140962882,
-            "velocityY": -3.7591097365739743,
-            "timestamp": 1.2684149084054441
-        },
-        {
-            "x": 2.0595952756969864,
-            "y": 2.572363489269638,
-            "heading": 0.16754686955661854,
-            "angularVelocity": -3.257091448717022e-7,
-            "velocityX": 0.8103920446953395,
-            "velocityY": -3.6951184257444134,
-            "timestamp": 1.3153932383463864
-        },
-        {
-            "x": 2.1154126290237643,
-            "y": 2.4036404273247642,
-            "heading": 0.16754685154625998,
-            "angularVelocity": -3.8337587986161977e-7,
-            "velocityX": 1.1881510772508834,
-            "velocityY": -3.5915083008906312,
-            "timestamp": 1.3623715682873287
-        },
-        {
-            "x": 2.1883780268761943,
-            "y": 2.2415938375833533,
-            "heading": 0.1675468289010436,
-            "angularVelocity": -4.82035363826859e-7,
-            "velocityX": 1.5531713865554018,
-            "velocityY": -3.449390175111029,
-            "timestamp": 1.409349898228271
-        },
-        {
-            "x": 2.2777091497247044,
-            "y": 2.0879611790897656,
-            "heading": 0.16754679784052873,
-            "angularVelocity": -6.611668595012802e-7,
-            "velocityX": 1.9015389214731706,
-            "velocityY": -3.270287783467881,
-            "timestamp": 1.4563282281692134
-        },
-        {
-            "x": 2.382448196411133,
-            "y": 1.9443897008895872,
-            "heading": 0.16754674836915184,
-            "angularVelocity": -0.0000010530680204285256,
-            "velocityX": 2.2295183080815923,
-            "velocityY": -3.0561213729961265,
-            "timestamp": 1.5033065581101557
-        },
-        {
-            "x": 2.5069799221636964,
-            "y": 1.8074962230648766,
-            "heading": 0.1675467031876573,
-            "angularVelocity": -9.235757728412815e-7,
-            "velocityX": 2.545610454081449,
-            "velocityY": -2.798302730812182,
-            "timestamp": 1.55222673973217
-        },
-        {
-            "x": 2.645527049823341,
-            "y": 1.6848068830394283,
-            "heading": 0.16754667440242366,
-            "angularVelocity": -5.884122397185895e-7,
-            "velocityX": 2.8321057499364835,
-            "velocityY": -2.5079493975189484,
-            "timestamp": 1.6011469213541845
-        },
-        {
-            "x": 2.7964787325047507,
-            "y": 1.577748144471377,
-            "heading": 0.1675466534117406,
-            "angularVelocity": -4.2908023560369677e-7,
-            "velocityX": 3.0856729815058617,
-            "velocityY": -2.1884370625450487,
-            "timestamp": 1.6500671029761989
-        },
-        {
-            "x": 2.958079900807193,
-            "y": 1.4875647290195357,
-            "heading": 0.1675466366326211,
-            "angularVelocity": -3.429897224036844e-7,
-            "velocityX": 3.303364029820377,
-            "velocityY": -1.8434807979383812,
-            "timestamp": 1.6989872845982132
-        },
-        {
-            "x": 3.1284516660647324,
-            "y": 1.4153051353233332,
-            "heading": 0.16754662225394246,
-            "angularVelocity": -2.9392120351406106e-7,
-            "velocityX": 3.4826478481607013,
-            "velocityY": -1.4770916889581902,
-            "timestamp": 1.7479074662202276
-        },
-        {
-            "x": 3.3056131601804433,
-            "y": 1.3618094062641966,
-            "heading": 0.16754660920712522,
-            "angularVelocity": -2.666960100615425e-7,
-            "velocityX": 3.6214398279336546,
-            "velocityY": -1.0935308759169382,
-            "timestamp": 1.796827647842242
-        },
-        {
-            "x": 3.487504529935422,
-            "y": 1.3276990268102373,
-            "heading": 0.16754659675699374,
-            "angularVelocity": -2.544988811687778e-7,
-            "velocityX": 3.7181253978240782,
-            "velocityY": -0.6972660019440657,
-            "timestamp": 1.8457478294642564
-        },
-        {
-            "x": 3.671043389503124,
-            "y": 1.304003553111977,
-            "heading": 0.16754658441216208,
-            "angularVelocity": -2.5234639867690264e-7,
-            "velocityX": 3.751802497092686,
-            "velocityY": -0.48437010887133,
-            "timestamp": 1.8946680110862708
-        },
-        {
-            "x": 3.8545823032662914,
-            "y": 1.2803084992011935,
-            "heading": 0.16754657206733162,
-            "angularVelocity": -2.523463739841797e-7,
-            "velocityX": 3.751803604927206,
-            "velocityY": -0.4843615278018566,
-            "timestamp": 1.9435881927082852
-        },
-        {
-            "x": 4.038121217030901,
-            "y": 1.2566134453015836,
-            "heading": 0.16754655972250113,
-            "angularVelocity": -2.5234637491728997e-7,
-            "velocityX": 3.7518036049566934,
-            "velocityY": -0.48436152757345374,
-            "timestamp": 1.9925083743302996
-        },
-        {
-            "x": 4.221660130795511,
-            "y": 1.232918391401974,
-            "heading": 0.16754654737767066,
-            "angularVelocity": -2.5234637402611237e-7,
-            "velocityX": 3.7518036049566943,
-            "velocityY": -0.4843615275734478,
-            "timestamp": 2.041428555952314
-        },
-        {
-            "x": 4.4051990445601215,
-            "y": 1.2092233375023644,
-            "heading": 0.16754653503284023,
-            "angularVelocity": -2.5234637391390173e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.4843615275734475,
-            "timestamp": 2.090348737574329
-        },
-        {
-            "x": 4.588737958324731,
-            "y": 1.1855282836027548,
-            "heading": 0.16754652268800974,
-            "angularVelocity": -2.523463747404588e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.4843615275734478,
-            "timestamp": 2.1392689191963434
-        },
-        {
-            "x": 4.77227687208934,
-            "y": 1.1618332297031453,
-            "heading": 0.16754651034317924,
-            "angularVelocity": -2.523463747849071e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.48436152757344797,
-            "timestamp": 2.188189100818358
-        },
-        {
-            "x": 4.95581578585395,
-            "y": 1.1381381758035356,
-            "heading": 0.16754649799834875,
-            "angularVelocity": -2.52346374599026e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.4843615275734478,
-            "timestamp": 2.2371092824403727
-        },
-        {
-            "x": 5.1393546996185595,
-            "y": 1.114443121903926,
-            "heading": 0.1675464856535183,
-            "angularVelocity": -2.523463743979587e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.48436152757344764,
-            "timestamp": 2.2860294640623873
-        },
-        {
-            "x": 5.32289361338317,
-            "y": 1.0907480680043165,
-            "heading": 0.16754647330868783,
-            "angularVelocity": -2.523463747332428e-7,
-            "velocityX": 3.7518036049566943,
-            "velocityY": -0.48436152757344797,
-            "timestamp": 2.334949645684402
-        },
-        {
-            "x": 5.506432527147779,
-            "y": 1.0670530141047068,
-            "heading": 0.1675464609638573,
-            "angularVelocity": -2.5234637523589347e-7,
-            "velocityX": 3.7518036049566943,
-            "velocityY": -0.48436152757344764,
-            "timestamp": 2.3838698273064165
-        },
-        {
-            "x": 5.689971440912389,
-            "y": 1.043357960205097,
-            "heading": 0.16754644861902682,
-            "angularVelocity": -2.523463745889681e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.4843615275734477,
-            "timestamp": 2.432790008928431
-        },
-        {
-            "x": 5.8735103546769984,
-            "y": 1.0196629063054876,
-            "heading": 0.16754643627419633,
-            "angularVelocity": -2.5234637515727394e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.4843615275734478,
-            "timestamp": 2.4817101905504457
-        },
-        {
-            "x": 6.057049268441609,
-            "y": 0.995967852405878,
-            "heading": 0.16754642392936583,
-            "angularVelocity": -2.523463750187176e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.4843615275734476,
-            "timestamp": 2.5306303721724603
-        },
-        {
-            "x": 6.240588182206218,
-            "y": 0.9722727985062686,
-            "heading": 0.16754641158453532,
-            "angularVelocity": -2.523463753175626e-7,
-            "velocityX": 3.751803604956694,
-            "velocityY": -0.48436152757344764,
-            "timestamp": 2.579550553794475
-        },
-        {
-            "x": 6.424127095970827,
-            "y": 0.948577744606659,
-            "heading": 0.1675463992397048,
-            "angularVelocity": -2.5234637555305e-7,
-            "velocityX": 3.7518036049566934,
-            "velocityY": -0.4843615275734476,
-            "timestamp": 2.6284707354164896
-        },
-        {
-            "x": 6.607666009735459,
-            "y": 0.9248826907072107,
-            "heading": 0.1675463868948743,
-            "angularVelocity": -2.5234637524939476e-7,
-            "velocityX": 3.7518036049571197,
-            "velocityY": -0.48436152757015266,
-            "timestamp": 2.677390917038504
-        },
-        {
-            "x": 6.79120492428193,
-            "y": 0.9011876428641385,
-            "heading": 0.16754637454996243,
-            "angularVelocity": -2.523480385625403e-7,
-            "velocityX": 3.7518036209390857,
-            "velocityY": -0.4843614037689742,
-            "timestamp": 2.726311098660519
-        },
-        {
-            "x": 6.9688913787459,
-            "y": 0.8784350451744124,
-            "heading": 0.1510406339197301,
-            "angularVelocity": -0.33740145851798203,
-            "velocityX": 3.6321707845828914,
-            "velocityY": -0.4650963454209109,
-            "timestamp": 2.7752312802825334
-        },
-        {
-            "x": 7.1269915101358645,
-            "y": 0.8581344466937117,
-            "heading": 0.12402265057850995,
-            "angularVelocity": -0.5522870611964742,
-            "velocityX": 3.2317977192222487,
-            "velocityY": -0.4149738984526803,
-            "timestamp": 2.824151461904548
-        },
-        {
-            "x": 7.265299167873697,
-            "y": 0.8403581253912503,
-            "heading": 0.0960275261585182,
-            "angularVelocity": -0.5722612527545016,
-            "velocityX": 2.8272106347943935,
-            "velocityY": -0.3633739841730647,
-            "timestamp": 2.8730716435265626
-        },
-        {
-            "x": 7.383819928892336,
-            "y": 0.8251162922066029,
-            "heading": 0.06979646830524168,
-            "angularVelocity": -0.5362011542792869,
-            "velocityX": 2.4227375510254454,
-            "velocityY": -0.3115653433671744,
-            "timestamp": 2.9219918251485772
-        },
-        {
-            "x": 7.482566860925546,
-            "y": 0.8124124441531555,
-            "heading": 0.04664460944238867,
-            "angularVelocity": -0.47325782724474763,
-            "velocityX": 2.01853159083065,
-            "velocityY": -0.2596852184974389,
-            "timestamp": 2.970912006770592
-        },
-        {
-            "x": 7.5615507925764724,
-            "y": 0.8022481812485672,
-            "heading": 0.027346506217350703,
-            "angularVelocity": -0.3944814304686405,
-            "velocityX": 1.6145469831081642,
-            "velocityY": -0.20777238692863387,
-            "timestamp": 3.0198321883926065
-        },
-        {
-            "x": 7.620780133649161,
-            "y": 0.7946243691967044,
-            "heading": 0.012413272398069258,
-            "angularVelocity": -0.3052571213791523,
-            "velocityX": 1.2107342840696753,
-            "velocityY": -0.15584185910773468,
-            "timestamp": 3.068752370014621
-        },
-        {
-            "x": 7.660261458521305,
-            "y": 0.7895415301241143,
-            "heading": 0.002207571288741933,
-            "angularVelocity": -0.2086194443876447,
-            "velocityX": 0.8070559749184711,
-            "velocityY": -0.10390065825721975,
-            "timestamp": 3.1176725516366357
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -0.003,
-            "angularVelocity": -0.10645036702804238,
-            "velocityX": 0.40348463199107626,
-            "velocityY": -0.05195258970523994,
-            "timestamp": 3.1665927332586503
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -0.003,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -1.0902826040652685e-32,
-            "timestamp": 3.215512914880665
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W1 To S1.traj b/src/main/deploy/choreo/W1 To S1.traj
deleted file mode 100644
index 6d3241e6..00000000
--- a/src/main/deploy/choreo/W1 To S1.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": -4.966859894029903e-31,
-            "velocityX": 7.6981862107013014e-34,
-            "velocityY": 1.676179535721191e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3730209621579745,
-            "y": 6.753775517490119,
-            "heading": 0.38350122654498725,
-            "angularVelocity": -0.06940058989840046,
-            "velocityX": 0.7797929692101098,
-            "velocityY": -0.03443434208804683,
-            "timestamp": 0.09364147285393071
-        },
-        {
-            "x": 2.5190628825903247,
-            "y": 6.747326552846718,
-            "heading": 0.37050348328006333,
-            "angularVelocity": -0.13880327667642306,
-            "velocityX": 1.5595858969471437,
-            "velocityY": -0.06886868015693057,
-            "timestamp": 0.18728294570786141
-        },
-        {
-            "x": 2.73812575373285,
-            "y": 6.7376531065331395,
-            "heading": 0.35100644039340695,
-            "angularVelocity": -0.2082094855243831,
-            "velocityX": 2.3393787439058484,
-            "velocityY": -0.10330301327774684,
-            "timestamp": 0.2809244185617921
-        },
-        {
-            "x": 3.0302095555912687,
-            "y": 6.7247551792664035,
-            "heading": 0.3250096622235805,
-            "angularVelocity": -0.27762034681324244,
-            "velocityX": 3.1191713773449834,
-            "velocityY": -0.13773733874150373,
-            "timestamp": 0.37456589141572283
-        },
-        {
-            "x": 3.3841048026937948,
-            "y": 6.709127593205063,
-            "heading": 0.3250096592315294,
-            "angularVelocity": -3.195216141250517e-8,
-            "velocityX": 3.779257590860006,
-            "velocityY": -0.16688744404647712,
-            "timestamp": 0.46820736426965354
-        },
-        {
-            "x": 3.7380000497922845,
-            "y": 6.693500007049851,
-            "heading": 0.3250096562396269,
-            "angularVelocity": -3.195060442591983e-8,
-            "velocityX": 3.7792575908168353,
-            "velocityY": -0.16688744504894126,
-            "timestamp": 0.5618488371235842
-        },
-        {
-            "x": 4.091895296890773,
-            "y": 6.677872420894639,
-            "heading": 0.3250096532477245,
-            "angularVelocity": -3.195060369258429e-8,
-            "velocityX": 3.7792575908168353,
-            "velocityY": -0.16688744504893968,
-            "timestamp": 0.6554903099775149
-        },
-        {
-            "x": 4.445790543993225,
-            "y": 6.6622448348316095,
-            "heading": 0.3250096502556742,
-            "angularVelocity": -3.195215253798021e-8,
-            "velocityX": 3.7792575908592094,
-            "velocityY": -0.1668874440645195,
-            "timestamp": 0.7491317828314457
-        },
-        {
-            "x": 4.737874310687331,
-            "y": 6.649346902155386,
-            "heading": 0.29900607193864137,
-            "angularVelocity": -0.27769296578215763,
-            "velocityX": 3.119171001824275,
-            "velocityY": -0.1377373965095818,
-            "timestamp": 0.8427732556853764
-        },
-        {
-            "x": 4.956937151198913,
-            "y": 6.639673451411702,
-            "heading": 0.27950309637569815,
-            "angularVelocity": -0.20827284074623165,
-            "velocityX": 2.339378416797119,
-            "velocityY": -0.10330306058697603,
-            "timestamp": 0.9364147285393072
-        },
-        {
-            "x": 5.10297904938153,
-            "y": 6.633224483848421,
-            "heading": 0.2665010284217395,
-            "angularVelocity": -0.1388494601558462,
-            "velocityX": 1.5595856593415998,
-            "velocityY": -0.06886871133840435,
-            "timestamp": 1.030056201393238
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": -0.06942467075336838,
-            "velocityX": 0.7797928459794067,
-            "velocityY": -0.03443435638235996,
-            "timestamp": 1.1236976742471687
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.26,
-            "angularVelocity": 2.7813300894409408e-31,
-            "velocityX": -8.985802616555554e-33,
-            "velocityY": -4.9789977648629575e-31,
-            "timestamp": 1.2173391471010995
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W1 To S2.traj b/src/main/deploy/choreo/W1 To S2.traj
deleted file mode 100644
index 15f8fbd9..00000000
--- a/src/main/deploy/choreo/W1 To S2.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": -5.274350592003984e-33,
-            "velocityX": 6.842220334639481e-35,
-            "velocityY": 4.754828012658472e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3477747650996332,
-            "y": 6.727866811231813,
-            "heading": 0.37690695075838176,
-            "angularVelocity": -0.1596548904259805,
-            "velocityX": 0.5825590927179829,
-            "velocityY": -0.3552462054262789,
-            "timestamp": 0.08200844463131846
-        },
-        {
-            "x": 2.4433243855136606,
-            "y": 6.669600518122756,
-            "heading": 0.35072364334029893,
-            "angularVelocity": -0.3192757469769597,
-            "velocityX": 1.16511928550267,
-            "velocityY": -0.7104913813571495,
-            "timestamp": 0.1640168892626369
-        },
-        {
-            "x": 2.5866489974259967,
-            "y": 6.582201260145896,
-            "heading": 0.3114536293241488,
-            "angularVelocity": -0.47885329605621113,
-            "velocityX": 1.7476811388958082,
-            "velocityY": -1.0657348565721152,
-            "timestamp": 0.24602533389395537
-        },
-        {
-            "x": 2.7777487646808345,
-            "y": 6.465669250143918,
-            "heading": 0.2590995246277934,
-            "angularVelocity": -0.638398946983098,
-            "velocityX": 2.330244990183096,
-            "velocityY": -1.4209757364118492,
-            "timestamp": 0.3280337785252738
-        },
-        {
-            "x": 3.0166238424465086,
-            "y": 6.320004790854863,
-            "heading": 0.19366077275903337,
-            "angularVelocity": -0.797951383701398,
-            "velocityX": 2.912810733572304,
-            "velocityY": -1.7762129247044165,
-            "timestamp": 0.4100422231565923
-        },
-        {
-            "x": 3.281496122930573,
-            "y": 6.158489372926003,
-            "heading": 0.19366076448026312,
-            "angularVelocity": -1.0095021672660313e-7,
-            "velocityX": 3.2298171447445223,
-            "velocityY": -1.9694973932864837,
-            "timestamp": 0.49205066778791073
-        },
-        {
-            "x": 3.546368403715213,
-            "y": 5.996973955490063,
-            "heading": 0.193660756201493,
-            "angularVelocity": -1.0095021513248387e-7,
-            "velocityX": 3.2298171484097047,
-            "velocityY": -1.9694973872758792,
-            "timestamp": 0.5740591124192291
-        },
-        {
-            "x": 3.785245611761769,
-            "y": 5.851315694507869,
-            "heading": 0.1281200370686441,
-            "angularVelocity": -0.7991947588751046,
-            "velocityX": 2.912836709932308,
-            "velocityY": -1.7761373433799759,
-            "timestamp": 0.6560675570505476
-        },
-        {
-            "x": 3.9763473726141734,
-            "y": 5.734789313275287,
-            "heading": 0.07567704387560718,
-            "angularVelocity": -0.6394828414171544,
-            "velocityX": 2.3302692998450527,
-            "velocityY": -1.4209070999510438,
-            "timestamp": 0.7380760016818662
-        },
-        {
-            "x": 4.119673683324307,
-            "y": 5.647394635056781,
-            "heading": 0.036339503004366135,
-            "angularVelocity": -0.47967670949118707,
-            "velocityX": 1.7477018538088223,
-            "velocityY": -1.0656790116115789,
-            "timestamp": 0.8200844463131847
-        },
-        {
-            "x": 4.2152245571841105,
-            "y": 5.589131547509505,
-            "heading": 0.010112949222819228,
-            "angularVelocity": -0.3198030873460947,
-            "velocityX": 1.1651345698527396,
-            "velocityY": -0.710452293165722,
-            "timestamp": 0.9020928909445032
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": -0.15989754813385992,
-            "velocityX": 0.5825673566993228,
-            "velocityY": -0.3552261921375451,
-            "timestamp": 0.9841013355758217
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": 2.1439897413450708e-33,
-            "velocityX": 3.9230953244452345e-34,
-            "velocityY": -1.8972025919917558e-33,
-            "timestamp": 1.0661097802071402
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W1 To S3.traj b/src/main/deploy/choreo/W1 To S3.traj
deleted file mode 100644
index 8360b86f..00000000
--- a/src/main/deploy/choreo/W1 To S3.traj	
+++ /dev/null
@@ -1,544 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.542786541644192e-31,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2937082670823616,
-            "y": 6.738516922494428,
-            "heading": 0.3838170687349535,
-            "angularVelocity": -0.12752172771189255,
-            "velocityX": -0.1297657401618881,
-            "velocityY": -0.3812097977411177,
-            "timestamp": 0.04848531599949176
-        },
-        {
-            "x": 2.281123433600762,
-            "y": 6.70154498788335,
-            "heading": 0.37169259737448246,
-            "angularVelocity": -0.2500648105569625,
-            "velocityX": -0.25955968775719646,
-            "velocityY": -0.7625387985814647,
-            "timestamp": 0.09697063199898352
-        },
-        {
-            "x": 2.262243832879689,
-            "y": 6.646077048114969,
-            "heading": 0.3539392998096368,
-            "angularVelocity": -0.36615823159004923,
-            "velocityX": -0.3893880102034898,
-            "velocityY": -1.1440152265685524,
-            "timestamp": 0.14545594799847528
-        },
-        {
-            "x": 2.2370673940379584,
-            "y": 6.572104077537847,
-            "heading": 0.3309787534387302,
-            "angularVelocity": -0.4735567026240549,
-            "velocityX": -0.5192590441623445,
-            "velocityY": -1.5256778068215682,
-            "timestamp": 0.19394126399796704
-        },
-        {
-            "x": 2.2055915440305824,
-            "y": 6.479614437749851,
-            "heading": 0.303410444299779,
-            "angularVelocity": -0.5685908933466691,
-            "velocityX": -0.6491831466623577,
-            "velocityY": -1.9075804268039651,
-            "timestamp": 0.2424265799974588
-        },
-        {
-            "x": 2.167813255965874,
-            "y": 6.3685929704461826,
-            "heading": 0.2721534840825349,
-            "angularVelocity": -0.6446685882563702,
-            "velocityX": -0.7791696782014628,
-            "velocityY": -2.2897956838082623,
-            "timestamp": 0.29091189599695055
-        },
-        {
-            "x": 2.123730000007902,
-            "y": 6.239021112222755,
-            "heading": 0.23878941688886735,
-            "angularVelocity": -0.6881272505944181,
-            "velocityX": -0.909208387103519,
-            "velocityY": -2.6723938073299895,
-            "timestamp": 0.3393972119964423
-        },
-        {
-            "x": 2.0733472424162183,
-            "y": 6.090890823809695,
-            "heading": 0.2066566550746183,
-            "angularVelocity": -0.6627318220196932,
-            "velocityX": -1.0391343554894152,
-            "velocityY": -3.0551577391940747,
-            "timestamp": 0.38788252799593403
-        },
-        {
-            "x": 2.0168156590767947,
-            "y": 5.924489369921669,
-            "heading": 0.187194468928521,
-            "angularVelocity": -0.4014037187247154,
-            "velocityX": -1.1659526636929207,
-            "velocityY": -3.431996893448088,
-            "timestamp": 0.4363678439954258
-        },
-        {
-            "x": 1.9576664650342146,
-            "y": 5.750871436780747,
-            "heading": 0.18719445417707173,
-            "angularVelocity": -3.042457077435804e-7,
-            "velocityX": -1.2199403638687387,
-            "velocityY": -3.580835342863344,
-            "timestamp": 0.4848531599949175
-        },
-        {
-            "x": 1.8985172819179166,
-            "y": 5.577253499917319,
-            "heading": 0.18719443942575115,
-            "angularVelocity": -3.042430533078605e-7,
-            "velocityX": -1.2199401385163373,
-            "velocityY": -3.5808354196392784,
-            "timestamp": 0.5333384759944093
-        },
-        {
-            "x": 1.8393681174712941,
-            "y": 5.403635556693396,
-            "heading": 0.18719442467442912,
-            "angularVelocity": -3.0424308271607693e-7,
-            "velocityX": -1.219939753457998,
-            "velocityY": -3.5808355508232466,
-            "timestamp": 0.5818237919939011
-        },
-        {
-            "x": 1.780218973506269,
-            "y": 5.230017606491683,
-            "heading": 0.18719440992310651,
-            "angularVelocity": -3.0424309544377536e-7,
-            "velocityX": -1.2199393310291196,
-            "velocityY": -3.580835694738771,
-            "timestamp": 0.6303091079933929
-        },
-        {
-            "x": 1.7210698517072343,
-            "y": 5.056399648738334,
-            "heading": 0.1871943951717844,
-            "angularVelocity": -3.042430844451726e-7,
-            "velocityX": -1.2199388738599835,
-            "velocityY": -3.580835850489736,
-            "timestamp": 0.6787944239928847
-        },
-        {
-            "x": 1.661920753431811,
-            "y": 4.882781682970833,
-            "heading": 0.18719438042046438,
-            "angularVelocity": -3.042430423387178e-7,
-            "velocityX": -1.2199383886901667,
-            "velocityY": -3.580836015780049,
-            "timestamp": 0.7272797399923765
-        },
-        {
-            "x": 1.6027716795006517,
-            "y": 4.709163708909599,
-            "heading": 0.18719436566914785,
-            "angularVelocity": -3.042429697472901e-7,
-            "velocityX": -1.2199378865945683,
-            "velocityY": -3.5808361868366427,
-            "timestamp": 0.7757650559918683
-        },
-        {
-            "x": 1.5436226301529286,
-            "y": 4.5355457264731545,
-            "heading": 0.18719435091783626,
-            "angularVelocity": -3.042428676797686e-7,
-            "velocityX": -1.219937379566087,
-            "velocityY": -3.5808363595737087,
-            "timestamp": 0.8242503719913601
-        },
-        {
-            "x": 1.4844769425124074,
-            "y": 4.361926598741778,
-            "heading": 0.18719433616649686,
-            "angularVelocity": -3.0424344145116126e-7,
-            "velocityX": -1.2198680450801807,
-            "velocityY": -3.58085998103429,
-            "timestamp": 0.8727356879908519
-        },
-        {
-            "x": 1.4424355744034427,
-            "y": 4.183392744566331,
-            "heading": 0.18719432108286724,
-            "angularVelocity": -3.110968611299024e-7,
-            "velocityX": -0.8670948562865489,
-            "velocityY": -3.682225236552195,
-            "timestamp": 0.9212210039903437
-        },
-        {
-            "x": 1.4196862795998648,
-            "y": 4.0013919676043574,
-            "heading": 0.18719430492048,
-            "angularVelocity": -3.3334602243497606e-7,
-            "velocityX": -0.4691996810625877,
-            "velocityY": -3.753729829541186,
-            "timestamp": 0.9697063199898355
-        },
-        {
-            "x": 1.4164891295727136,
-            "y": 3.81800278977475,
-            "heading": 0.18719428668091828,
-            "angularVelocity": -3.761873328628986e-7,
-            "velocityX": -0.06594058347012394,
-            "velocityY": -3.7823653213178425,
-            "timestamp": 1.0181916359893273
-        },
-        {
-            "x": 1.432880701620732,
-            "y": 3.635319656493888,
-            "heading": 0.1871942648241357,
-            "angularVelocity": -4.507917942103899e-7,
-            "velocityX": 0.3380729136345315,
-            "velocityY": -3.767803292913241,
-            "timestamp": 1.066676951988819
-        },
-        {
-            "x": 1.4686738067578264,
-            "y": 3.455429018696199,
-            "heading": 0.18719405074105777,
-            "angularVelocity": -0.0000044154209051021175,
-            "velocityX": 0.7382256751280081,
-            "velocityY": -3.710208628927779,
-            "timestamp": 1.1151622679883109
-        },
-        {
-            "x": 1.5224561218642167,
-            "y": 3.2829543962052377,
-            "heading": 0.17869632281657946,
-            "angularVelocity": -0.17526394846200616,
-            "velocityX": 1.10924955315069,
-            "velocityY": -3.557254788081333,
-            "timestamp": 1.1636475839878027
-        },
-        {
-            "x": 1.5921416539328836,
-            "y": 3.121386413493124,
-            "heading": 0.1576086367549348,
-            "angularVelocity": -0.43492933122045885,
-            "velocityX": 1.4372502402541338,
-            "velocityY": -3.33230751169348,
-            "timestamp": 1.2121328999872945
-        },
-        {
-            "x": 1.6761645416915945,
-            "y": 2.9727322431612,
-            "heading": 0.12423651758368265,
-            "angularVelocity": -0.6882933210462135,
-            "velocityX": 1.7329553500274961,
-            "velocityY": -3.0659626995783453,
-            "timestamp": 1.2606182159867863
-        },
-        {
-            "x": 1.7731411061925635,
-            "y": 2.838565031466778,
-            "heading": 0.08175388007293943,
-            "angularVelocity": -0.8761959499529206,
-            "velocityX": 2.0001223566816617,
-            "velocityY": -2.767172058765521,
-            "timestamp": 1.309103531986278
-        },
-        {
-            "x": 1.8819216264400211,
-            "y": 2.719965378093697,
-            "heading": 0.033593056188374194,
-            "angularVelocity": -0.9933074146707891,
-            "velocityX": 2.2435765964482535,
-            "velocityY": -2.4460942643771193,
-            "timestamp": 1.3575888479857698
-        },
-        {
-            "x": 2.0016674106146493,
-            "y": 2.617587012881804,
-            "heading": -0.0178597683247242,
-            "angularVelocity": -1.0612042729434041,
-            "velocityX": 2.4697329842357973,
-            "velocityY": -2.1115334220519952,
-            "timestamp": 1.4060741639852616
-        },
-        {
-            "x": 2.131748914718628,
-            "y": 2.531857967376709,
-            "heading": -0.07087516069292028,
-            "angularVelocity": -1.0934319242038333,
-            "velocityX": 2.682905152268552,
-            "velocityY": -1.7681445142231829,
-            "timestamp": 1.4545594799847534
-        },
-        {
-            "x": 2.218821194191698,
-            "y": 2.484029060579093,
-            "heading": -0.10483282223316899,
-            "angularVelocity": -1.096901896110445,
-            "velocityX": 2.812612651242981,
-            "velocityY": -1.5449714785171498,
-            "timestamp": 1.4855172727096597
-        },
-        {
-            "x": 2.3097767814437278,
-            "y": 2.4431840829179357,
-            "heading": -0.13859592156415457,
-            "angularVelocity": -1.0906171389530375,
-            "velocityX": 2.938051432149048,
-            "velocityY": -1.3193762883647426,
-            "timestamp": 1.516475065434566
-        },
-        {
-            "x": 2.4044569077078632,
-            "y": 2.4094074987036276,
-            "heading": -0.1718069818405491,
-            "angularVelocity": -1.0727851488087659,
-            "velocityX": 3.0583616572737307,
-            "velocityY": -1.0910527282945428,
-            "timestamp": 1.5474328581594723
-        },
-        {
-            "x": 2.5026677503931767,
-            "y": 2.3827944816766236,
-            "heading": -0.20403633414457553,
-            "angularVelocity": -1.0410739741365005,
-            "velocityX": 3.1724110164298485,
-            "velocityY": -0.859654861819398,
-            "timestamp": 1.5783906508843786
-        },
-        {
-            "x": 2.6041682782784212,
-            "y": 2.363451815109364,
-            "heading": -0.23475758891885987,
-            "angularVelocity": -0.9923593405260972,
-            "velocityX": 3.2786745743372427,
-            "velocityY": -0.6248076773199155,
-            "timestamp": 1.6093484436092849
-        },
-        {
-            "x": 2.7086523080371263,
-            "y": 2.3514976196224144,
-            "heading": -0.26331031649970843,
-            "angularVelocity": -0.9223114784903548,
-            "velocityX": 3.3750477847899405,
-            "velocityY": -0.38614495527174003,
-            "timestamp": 1.6403062363341911
-        },
-        {
-            "x": 2.8157215031828033,
-            "y": 2.3470577985799963,
-            "heading": -0.288841887845313,
-            "angularVelocity": -0.8247219551529203,
-            "velocityX": 3.458553912306755,
-            "velocityY": -0.14341529714262294,
-            "timestamp": 1.6712640290590974
-        },
-        {
-            "x": 2.9248447015312338,
-            "y": 2.3502540561946077,
-            "heading": -0.3102182612745416,
-            "angularVelocity": -0.6905005668986338,
-            "velocityX": 3.524902415281217,
-            "velocityY": 0.10324565588123534,
-            "timestamp": 1.7022218217840037
-        },
-        {
-            "x": 3.0353304312162184,
-            "y": 2.361176073397281,
-            "heading": -0.326082506948615,
-            "angularVelocity": -0.5124475706430824,
-            "velocityX": 3.568914963243171,
-            "velocityY": 0.3528034863374462,
-            "timestamp": 1.73317961450891
-        },
-        {
-            "x": 3.1466093544989446,
-            "y": 2.379927120590619,
-            "heading": -0.3369287204650216,
-            "angularVelocity": -0.35035487229182166,
-            "velocityX": 3.594536738181179,
-            "velocityY": 0.605697161937562,
-            "timestamp": 1.7641374072338163
-        },
-        {
-            "x": 3.2578769593015027,
-            "y": 2.406586601459548,
-            "heading": -0.34366338052401463,
-            "angularVelocity": -0.21754328930658381,
-            "velocityX": 3.5941711281316278,
-            "velocityY": 0.8611557389062998,
-            "timestamp": 1.7950951999587226
-        },
-        {
-            "x": 3.3679824864036325,
-            "y": 2.4411213005139327,
-            "heading": -0.34790283328354016,
-            "angularVelocity": -0.13694299193573645,
-            "velocityX": 3.556633642474382,
-            "velocityY": 1.1155413876329803,
-            "timestamp": 1.8260529926836289
-        },
-        {
-            "x": 3.475304114496225,
-            "y": 2.4831460278092354,
-            "heading": -0.3524511595124565,
-            "angularVelocity": -0.14692023650373393,
-            "velocityX": 3.466708012625811,
-            "velocityY": 1.3574846136083376,
-            "timestamp": 1.8570107854085351
-        },
-        {
-            "x": 3.5778542240444473,
-            "y": 2.531447362567012,
-            "heading": -0.3616437420636369,
-            "angularVelocity": -0.29693921116020117,
-            "velocityX": 3.312578207999158,
-            "velocityY": 1.560231867531513,
-            "timestamp": 1.8879685781334414
-        },
-        {
-            "x": 3.673558919058804,
-            "y": 2.583451056106446,
-            "heading": -0.37745622232401294,
-            "angularVelocity": -0.5107754419475534,
-            "velocityX": 3.091457322713648,
-            "velocityY": 1.6798256258655577,
-            "timestamp": 1.9189263708583477
-        },
-        {
-            "x": 3.7613706261508923,
-            "y": 2.636186152260406,
-            "heading": -0.3968957996596081,
-            "angularVelocity": -0.6279380932955558,
-            "velocityX": 2.836497675157173,
-            "velocityY": 1.7034514257095354,
-            "timestamp": 1.949884163583254
-        },
-        {
-            "x": 3.841306735175595,
-            "y": 2.6876448087218088,
-            "heading": -0.4179459699473055,
-            "angularVelocity": -0.6799635386030257,
-            "velocityX": 2.582099755467769,
-            "velocityY": 1.6622198138851267,
-            "timestamp": 1.9808419563081603
-        },
-        {
-            "x": 3.913645075614716,
-            "y": 2.736629100397619,
-            "heading": -0.43934657697326235,
-            "angularVelocity": -0.6912833617387507,
-            "velocityX": 2.336676296079001,
-            "velocityY": 1.582292772328868,
-            "timestamp": 2.0117997490330666
-        },
-        {
-            "x": 3.9786691618750996,
-            "y": 2.7823974078733573,
-            "heading": -0.46027689817629036,
-            "angularVelocity": -0.6760921681319296,
-            "velocityX": 2.100410931692797,
-            "velocityY": 1.4784099074015198,
-            "timestamp": 2.0427575417579726
-        },
-        {
-            "x": 4.036618763930541,
-            "y": 2.824460782764613,
-            "heading": -0.48017689659950147,
-            "angularVelocity": -0.6428106357887643,
-            "velocityX": 1.8718906276836884,
-            "velocityY": 1.3587330099747585,
-            "timestamp": 2.0737153344828787
-        },
-        {
-            "x": 4.08768908254176,
-            "y": 2.862477844590781,
-            "heading": -0.49864492956893264,
-            "angularVelocity": -0.5965552238942878,
-            "velocityX": 1.6496757073449355,
-            "velocityY": 1.2280288250525657,
-            "timestamp": 2.1046731272077848
-        },
-        {
-            "x": 4.132038835388808,
-            "y": 2.8961988047734253,
-            "heading": -0.5153800748468587,
-            "angularVelocity": -0.5405794084679139,
-            "velocityX": 1.4325876925792385,
-            "velocityY": 1.0892559583398733,
-            "timestamp": 2.135630919932691
-        },
-        {
-            "x": 4.169798307734006,
-            "y": 2.9254338939938918,
-            "heading": -0.5301487101337384,
-            "angularVelocity": -0.47705711511170307,
-            "velocityX": 1.2197081581622198,
-            "velocityY": 0.944353154640467,
-            "timestamp": 2.166588712657597
-        },
-        {
-            "x": 4.201075770562755,
-            "y": 2.9500345091711524,
-            "heading": -0.5427642633485259,
-            "angularVelocity": -0.4075081620800149,
-            "velocityX": 1.0103259979397883,
-            "velocityY": 0.7946501675963605,
-            "timestamp": 2.197546505382503
-        },
-        {
-            "x": 4.225962326087245,
-            "y": 2.9698813733488647,
-            "heading": -0.5530743601373866,
-            "angularVelocity": -0.3330372058810695,
-            "velocityX": 0.8038866254348108,
-            "velocityY": 0.6410942909939014,
-            "timestamp": 2.228504298107409
-        },
-        {
-            "x": 4.24453552816763,
-            "y": 2.984876771487546,
-            "heading": -0.5609523103628415,
-            "angularVelocity": -0.2544738992160421,
-            "velocityX": 0.5999524011761236,
-            "velocityY": 0.48438201883216997,
-            "timestamp": 2.259462090832315
-        },
-        {
-            "x": 4.256862100646349,
-            "y": 2.9949392682905573,
-            "heading": -0.5662912533122896,
-            "angularVelocity": -0.17245877305093363,
-            "velocityX": 0.398173493448939,
-            "velocityY": 0.32503922008988784,
-            "timestamp": 2.290419883557221
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.569,
-            "angularVelocity": -0.08749805620543083,
-            "velocityX": 0.19826669841112912,
-            "velocityY": 0.16347198117160036,
-            "timestamp": 2.321377676282127
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.569,
-            "angularVelocity": 1.3974941712649426e-29,
-            "velocityX": 2.480011116993708e-30,
-            "velocityY": -4.1979337006948246e-30,
-            "timestamp": 2.3523354690070333
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W1 To S4.traj b/src/main/deploy/choreo/W1 To S4.traj
deleted file mode 100644
index 1ef15c59..00000000
--- a/src/main/deploy/choreo/W1 To S4.traj	
+++ /dev/null
@@ -1,562 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 9.203343680058695e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.298008161759761,
-            "y": 6.742798044155305,
-            "heading": 0.37730350882287184,
-            "angularVelocity": -0.30162955292479776,
-            "velocityX": -0.04731994607959398,
-            "velocityY": -0.3373947598852993,
-            "timestamp": 0.0420929947149233
-        },
-        {
-            "x": 2.2940179124306126,
-            "y": 6.714368498753073,
-            "heading": 0.3522801711626307,
-            "angularVelocity": -0.5944774856175671,
-            "velocityX": -0.09479604281359731,
-            "velocityY": -0.6753984978919232,
-            "timestamp": 0.0841859894298466
-        },
-        {
-            "x": 2.28802134748838,
-            "y": 6.671681118579196,
-            "heading": 0.31538217591497186,
-            "angularVelocity": -0.8765828019021266,
-            "velocityX": -0.14245992671333493,
-            "velocityY": -1.014120769096589,
-            "timestamp": 0.1262789841447699
-        },
-        {
-            "x": 2.2800080756588827,
-            "y": 6.61469814393844,
-            "heading": 0.2672110996394858,
-            "angularVelocity": -1.1443965106718308,
-            "velocityX": -0.19037067530517543,
-            "velocityY": -1.3537400944426796,
-            "timestamp": 0.1683719788596932
-        },
-        {
-            "x": 2.2699650128963897,
-            "y": 6.543370292056319,
-            "heading": 0.20862519634351256,
-            "angularVelocity": -1.391820745773707,
-            "velocityX": -0.23859226055333851,
-            "velocityY": -1.694530226827328,
-            "timestamp": 0.21046497357461652
-        },
-        {
-            "x": 2.2578780077181495,
-            "y": 6.4576319980717,
-            "heading": 0.14091301714440063,
-            "angularVelocity": -1.608632972248606,
-            "velocityX": -0.2871500414760241,
-            "velocityY": -2.036877978516017,
-            "timestamp": 0.2525579682895398
-        },
-        {
-            "x": 2.2437361567314555,
-            "y": 6.357398182016401,
-            "heading": 0.06608849726258184,
-            "angularVelocity": -1.7776002964049293,
-            "velocityX": -0.3359668534508049,
-            "velocityY": -2.3812469683883926,
-            "timestamp": 0.2946509630044631
-        },
-        {
-            "x": 2.227539619778089,
-            "y": 6.242572554004172,
-            "heading": -0.012517898946656567,
-            "angularVelocity": -1.8674460380308828,
-            "velocityX": -0.3847798680768185,
-            "velocityY": -2.727903509595609,
-            "timestamp": 0.33674395771938637
-        },
-        {
-            "x": 2.209319968778144,
-            "y": 6.1131309245806085,
-            "heading": -0.08849823738903302,
-            "angularVelocity": -1.8050589880087344,
-            "velocityX": -0.4328428310539146,
-            "velocityY": -3.0751347177888695,
-            "timestamp": 0.37883695243430965
-        },
-        {
-            "x": 2.189363625025874,
-            "y": 5.970570114356374,
-            "heading": -0.13907359717832485,
-            "angularVelocity": -1.201514887021361,
-            "velocityX": -0.4741013056311528,
-            "velocityY": -3.38680607520881,
-            "timestamp": 0.42092994714923293
-        },
-        {
-            "x": 2.1674827409245396,
-            "y": 5.816618965425972,
-            "heading": -0.15173248317406665,
-            "angularVelocity": -0.3007361695568274,
-            "velocityX": -0.5198224609468644,
-            "velocityY": -3.657405465518488,
-            "timestamp": 0.4630229418641562
-        },
-        {
-            "x": 2.1445157307124236,
-            "y": 5.659048701686038,
-            "heading": -0.1517325271793822,
-            "angularVelocity": -0.0000010454308573871989,
-            "velocityX": -0.5456254744443967,
-            "velocityY": -3.7433844944292183,
-            "timestamp": 0.5051159365790795
-        },
-        {
-            "x": 2.1215487153797565,
-            "y": 5.501478438692377,
-            "heading": -0.1517325711844689,
-            "angularVelocity": -0.0000010454254199880688,
-            "velocityX": -0.5456255960929445,
-            "velocityY": -3.7433844767000393,
-            "timestamp": 0.5472089312940028
-        },
-        {
-            "x": 2.0985817251133763,
-            "y": 5.343908172045118,
-            "heading": -0.15173261518953962,
-            "angularVelocity": -0.000001045425040533604,
-            "velocityX": -0.5456250005951166,
-            "velocityY": -3.7433845634983034,
-            "timestamp": 0.5893019260089262
-        },
-        {
-            "x": 2.075614762728362,
-            "y": 5.186337901333946,
-            "heading": -0.15173265919459192,
-            "angularVelocity": -0.0000010454246033549915,
-            "velocityX": -0.5456243382196686,
-            "velocityY": -3.743384660044355,
-            "timestamp": 0.6313949207238495
-        },
-        {
-            "x": 2.0526478312972185,
-            "y": 5.028767626111028,
-            "heading": -0.15173270319962293,
-            "angularVelocity": -0.0000010454240970480152,
-            "velocityX": -0.5456236028509798,
-            "velocityY": -3.7433847672295513,
-            "timestamp": 0.6734879154387728
-        },
-        {
-            "x": 2.0296809339445536,
-            "y": 4.871197345920939,
-            "heading": -0.15173274720462945,
-            "angularVelocity": -0.0000010454235162635837,
-            "velocityX": -0.5456227932512114,
-            "velocityY": -3.743384885234266,
-            "timestamp": 0.7155809101536962
-        },
-        {
-            "x": 2.0067140734575584,
-            "y": 4.7136270603574335,
-            "heading": -0.15173279120960867,
-            "angularVelocity": -0.0000010454228670132247,
-            "velocityX": -0.5456219174363699,
-            "velocityY": -3.7433850128900636,
-            "timestamp": 0.7576739048686195
-        },
-        {
-            "x": 1.983747251812091,
-            "y": 4.55605676913253,
-            "heading": -0.15173283521455908,
-            "angularVelocity": -0.000001045422182833939,
-            "velocityX": -0.545620994681206,
-            "velocityY": -3.743385147387475,
-            "timestamp": 0.7997668995835429
-        },
-        {
-            "x": 1.9607804702256346,
-            "y": 4.398486472068785,
-            "heading": -0.15173287921948736,
-            "angularVelocity": -0.0000010454216570158517,
-            "velocityX": -0.5456200430024101,
-            "velocityY": -3.7433852861004024,
-            "timestamp": 0.8418598942984662
-        },
-        {
-            "x": 1.9378163633559966,
-            "y": 4.240915785172817,
-            "heading": -0.15173292322446258,
-            "angularVelocity": -0.0000010454227717672262,
-            "velocityX": -0.5455564999630786,
-            "velocityY": -3.7433945473141725,
-            "timestamp": 0.8839528890133895
-        },
-        {
-            "x": 1.9275487572437193,
-            "y": 4.082011890541997,
-            "heading": -0.15173296776197548,
-            "angularVelocity": -0.000001058074228463605,
-            "velocityX": -0.2439267194414494,
-            "velocityY": -3.775067459728259,
-            "timestamp": 0.9260458837283129
-        },
-        {
-            "x": 1.9320519221515333,
-            "y": 3.9228403083205765,
-            "heading": -0.15173301420750987,
-            "angularVelocity": -0.0000011034029462076059,
-            "velocityX": 0.10698133830372927,
-            "velocityY": -3.781426892988182,
-            "timestamp": 0.9681388784432362
-        },
-        {
-            "x": 1.9512874233679367,
-            "y": 3.764771127717043,
-            "heading": -0.15173306433513473,
-            "angularVelocity": -0.0000011908780826817618,
-            "velocityX": 0.45697630559851177,
-            "velocityY": -3.7552372235348,
-            "timestamp": 1.0102318731581594
-        },
-        {
-            "x": 1.9850897622274482,
-            "y": 3.609164986787025,
-            "heading": -0.15173312063283023,
-            "angularVelocity": -0.0000013374599716310608,
-            "velocityX": 0.8030395339756486,
-            "velocityY": -3.6967229816711127,
-            "timestamp": 1.0523248678730828
-        },
-        {
-            "x": 2.0331680016062417,
-            "y": 3.4573613296965586,
-            "heading": -0.1517331870158403,
-            "angularVelocity": -0.0000015770560044658274,
-            "velocityX": 1.142190991741159,
-            "velocityY": -3.606387668982997,
-            "timestamp": 1.094417862588006
-        },
-        {
-            "x": 2.0951083019579966,
-            "y": 3.3106668726225337,
-            "heading": -0.15173327041919607,
-            "angularVelocity": -0.0000019814070334134687,
-            "velocityX": 1.4715108956074094,
-            "velocityY": -3.4850088017619067,
-            "timestamp": 1.1365108573029294
-        },
-        {
-            "x": 2.170377492904663,
-            "y": 3.170344352722168,
-            "heading": -0.1517333779044128,
-            "angularVelocity": -0.000002553517929494838,
-            "velocityX": 1.7881643122906952,
-            "velocityY": -3.333631186155945,
-            "timestamp": 1.1786038520178528
-        },
-        {
-            "x": 2.2421653916580504,
-            "y": 3.058317386598826,
-            "heading": -0.1517334835913584,
-            "angularVelocity": -0.0000030048345176781924,
-            "velocityX": 2.041035009880172,
-            "velocityY": -3.1850905776457417,
-            "timestamp": 1.213776153524627
-        },
-        {
-            "x": 2.322415894828655,
-            "y": 2.9521882028356305,
-            "heading": -0.15173356325163515,
-            "angularVelocity": -0.000002264858236060329,
-            "velocityX": 2.2816392369190814,
-            "velocityY": -3.0174079948323826,
-            "timestamp": 1.2489484550314012
-        },
-        {
-            "x": 2.4106466853387967,
-            "y": 2.8525946456824203,
-            "heading": -0.1517336276365959,
-            "angularVelocity": -0.0000018305586497777263,
-            "velocityX": 2.5085304836576707,
-            "velocityY": -2.8315905666289245,
-            "timestamp": 1.2841207565381754
-        },
-        {
-            "x": 2.5063274836460994,
-            "y": 2.7601352747554455,
-            "heading": -0.15173368243148194,
-            "angularVelocity": -0.0000015578987915956282,
-            "velocityX": 2.720345107040385,
-            "velocityY": -2.628755212654101,
-            "timestamp": 1.3192930580449496
-        },
-        {
-            "x": 2.6088832332920924,
-            "y": 2.67536576592715,
-            "heading": -0.15173373101035148,
-            "angularVelocity": -0.000001381168347773627,
-            "velocityX": 2.915810033819959,
-            "velocityY": -2.410121180496788,
-            "timestamp": 1.3544653595517238
-        },
-        {
-            "x": 2.7176975542058304,
-            "y": 2.598795565150263,
-            "heading": -0.1517337755722331,
-            "angularVelocity": -0.0000012669595030854418,
-            "velocityX": 3.0937503732242226,
-            "velocityY": -2.1770028544233733,
-            "timestamp": 1.389637661058498
-        },
-        {
-            "x": 2.8321164386506434,
-            "y": 2.5308848045360426,
-            "heading": -0.15173381767917685,
-            "angularVelocity": -0.0000011971620259359605,
-            "velocityX": 3.2530963156555406,
-            "velocityY": -1.930802299108592,
-            "timestamp": 1.4248099625652721
-        },
-        {
-            "x": 2.951452140465328,
-            "y": 2.4720414272198683,
-            "heading": -0.15173385854226368,
-            "angularVelocity": -0.0000011617973536146762,
-            "velocityX": 3.3928886283344455,
-            "velocityY": -1.6730033235055968,
-            "timestamp": 1.4599822640720463
-        },
-        {
-            "x": 3.0749866313948626,
-            "y": 2.4226169148084327,
-            "heading": -0.151733899192453,
-            "angularVelocity": -0.000001155744366153098,
-            "velocityX": 3.512266347021429,
-            "velocityY": -1.4052112114959712,
-            "timestamp": 1.4951545655788205
-        },
-        {
-            "x": 3.1992028583958065,
-            "y": 2.3749316777596947,
-            "heading": -0.1517339398689439,
-            "angularVelocity": -0.0000011564921588237654,
-            "velocityX": 3.5316491011263524,
-            "velocityY": -1.3557610678264387,
-            "timestamp": 1.5303268670855947
-        },
-        {
-            "x": 3.3234191450837622,
-            "y": 2.3272465961911335,
-            "heading": -0.15173398054544393,
-            "angularVelocity": -0.0000011564924185481092,
-            "velocityX": 3.5316507981154315,
-            "velocityY": -1.355756647297499,
-            "timestamp": 1.565499168592369
-        },
-        {
-            "x": 3.4476354415156054,
-            "y": 2.2795615400047207,
-            "heading": -0.15173402122195195,
-            "angularVelocity": -0.0000011564926460273957,
-            "velocityX": 3.5316510751484063,
-            "velocityY": -1.3557559256458895,
-            "timestamp": 1.6006714700991431
-        },
-        {
-            "x": 3.57185174767356,
-            "y": 2.231876509154167,
-            "heading": -0.15173406189847174,
-            "angularVelocity": -0.0000011564929795008858,
-            "velocityX": 3.531651351675977,
-            "velocityY": -1.355755205310334,
-            "timestamp": 1.6358437716059173
-        },
-        {
-            "x": 3.6960680635749434,
-            "y": 2.1841915036846027,
-            "heading": -0.15173410257500647,
-            "angularVelocity": -0.000001156493404477995,
-            "velocityX": 3.5316516286959314,
-            "velocityY": -1.3557544836916666,
-            "timestamp": 1.6710160731126915
-        },
-        {
-            "x": 3.820284389246338,
-            "y": 2.136506523665285,
-            "heading": -0.15173414325155568,
-            "angularVelocity": -0.0000011564938164652249,
-            "velocityX": 3.5316519064716276,
-            "velocityY": -1.3557537601039011,
-            "timestamp": 1.7061883746194657
-        },
-        {
-            "x": 3.9445007247793735,
-            "y": 2.0888215693349235,
-            "heading": -0.15173418392811813,
-            "angularVelocity": -0.0000011564941935950735,
-            "velocityX": 3.5316521868525257,
-            "velocityY": -1.3557530297293134,
-            "timestamp": 1.74136067612624
-        },
-        {
-            "x": 4.068717070394662,
-            "y": 2.0411366412682117,
-            "heading": -0.1517342246046924,
-            "angularVelocity": -0.0000011564945294151802,
-            "velocityX": 3.531652473505735,
-            "velocityY": -1.3557522830153055,
-            "timestamp": 1.776532977633014
-        },
-        {
-            "x": 4.192933426524707,
-            "y": 1.9934517405918115,
-            "heading": -0.15173426528127656,
-            "angularVelocity": -0.0000011564948108738022,
-            "velocityX": 3.5316527724556495,
-            "velocityY": -1.3557515042686648,
-            "timestamp": 1.8117052791397883
-        },
-        {
-            "x": 4.3171498402859925,
-            "y": 1.9457669900421575,
-            "heading": -0.1517343059583456,
-            "angularVelocity": -0.0000011565085966842365,
-            "velocityX": 3.531654410996122,
-            "velocityY": -1.3557472359455842,
-            "timestamp": 1.8468775806465625
-        },
-        {
-            "x": 4.438199866372425,
-            "y": 1.9006463371040783,
-            "heading": -0.16306820061092758,
-            "angularVelocity": -0.3222392100329015,
-            "velocityX": 3.441629375976964,
-            "velocityY": -1.282846188765579,
-            "timestamp": 1.8820498821533367
-        },
-        {
-            "x": 4.551333178258229,
-            "y": 1.8581105785728176,
-            "heading": -0.19901052601489952,
-            "angularVelocity": -1.0218929061849813,
-            "velocityX": 3.2165456066050515,
-            "velocityY": -1.2093538582645278,
-            "timestamp": 1.9172221836601109
-        },
-        {
-            "x": 4.6559660625940325,
-            "y": 1.8184646011809935,
-            "heading": -0.2548690023306322,
-            "angularVelocity": -1.5881382202121255,
-            "velocityX": 2.9748660125539748,
-            "velocityY": -1.1271931518097666,
-            "timestamp": 1.952394485166885
-        },
-        {
-            "x": 4.750968691238499,
-            "y": 1.7823676048635404,
-            "heading": -0.3144227593810797,
-            "angularVelocity": -1.693200458860429,
-            "velocityX": 2.7010637511499986,
-            "velocityY": -1.0262904265875403,
-            "timestamp": 1.9875667866736593
-        },
-        {
-            "x": 4.836310937621718,
-            "y": 1.749878315803868,
-            "heading": -0.37309459063768413,
-            "angularVelocity": -1.6681260180060764,
-            "velocityX": 2.426404947278819,
-            "velocityY": -0.9237180300360781,
-            "timestamp": 2.0227390881804337
-        },
-        {
-            "x": 4.912035358987092,
-            "y": 1.7210053742684595,
-            "heading": -0.4285254423369764,
-            "angularVelocity": -1.5759802266171352,
-            "velocityX": 2.1529561081121917,
-            "velocityY": -0.8208999780650579,
-            "timestamp": 2.057911389687208
-        },
-        {
-            "x": 4.978188578205751,
-            "y": 1.6957473341798728,
-            "heading": -0.4792367761737124,
-            "angularVelocity": -1.4417974276425698,
-            "velocityX": 1.8808328253957978,
-            "velocityY": -0.71812304019178,
-            "timestamp": 2.0930836911939825
-        },
-        {
-            "x": 5.034811621143831,
-            "y": 1.6741013653395114,
-            "heading": -0.5242175587960242,
-            "angularVelocity": -1.278869471013956,
-            "velocityX": 1.6098759680874073,
-            "velocityY": -0.6154265690060781,
-            "timestamp": 2.128255992700757
-        },
-        {
-            "x": 5.081938937252143,
-            "y": 1.656065298518901,
-            "heading": -0.5627406592667319,
-            "angularVelocity": -1.0952681178195878,
-            "velocityX": 1.3398985590759445,
-            "velocityY": -0.5127917721601607,
-            "timestamp": 2.1634282942075314
-        },
-        {
-            "x": 5.11959917834615,
-            "y": 1.6416378108353973,
-            "heading": -0.5942629145071194,
-            "angularVelocity": -0.8962238434785516,
-            "velocityX": 1.0707357631047747,
-            "velocityY": -0.41019458680362747,
-            "timestamp": 2.1986005957143058
-        },
-        {
-            "x": 5.147816310703936,
-            "y": 1.63081813815951,
-            "heading": -0.6183637008514038,
-            "angularVelocity": -0.6852206228143011,
-            "velocityX": 0.8022543634897267,
-            "velocityY": -0.3076191267666608,
-            "timestamp": 2.23377289722108
-        },
-        {
-            "x": 5.1666106911988186,
-            "y": 1.6236057376407542,
-            "heading": -0.6347043261398958,
-            "angularVelocity": -0.4645878884367218,
-            "velocityX": 0.5343517395716112,
-            "velocityY": -0.20505910076332604,
-            "timestamp": 2.2689451987278546
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.643,
-            "angularVelocity": -0.23585814702817923,
-            "velocityX": 0.26695178873562564,
-            "velocityY": -0.10251639745723456,
-            "timestamp": 2.304117500234629
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.643,
-            "angularVelocity": -2.979902278450704e-33,
-            "velocityX": -7.596111021745797e-35,
-            "velocityY": -9.748244176889607e-35,
-            "timestamp": 2.3392898017414034
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W1 To W2.traj b/src/main/deploy/choreo/W1 To W2.traj
deleted file mode 100644
index a5fd0bb0..00000000
--- a/src/main/deploy/choreo/W1 To W2.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": 1.792103156985809e-33,
-            "velocityX": -7.27073415334495e-34,
-            "velocityY": -7.853383039035825e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.298001341213542,
-            "y": 6.709118496985434,
-            "heading": 0.374359095500822,
-            "angularVelocity": -0.2058832051525312,
-            "velocityX": -0.02630859852023658,
-            "velocityY": -0.6302702831973344,
-            "timestamp": 0.07596979310473732
-        },
-        {
-            "x": 2.294004039374054,
-            "y": 6.613355753676287,
-            "heading": 0.3430620070158405,
-            "angularVelocity": -0.4119675361210102,
-            "velocityX": -0.0526169899393711,
-            "velocityY": -1.2605371081782701,
-            "timestamp": 0.15193958620947465
-        },
-        {
-            "x": 2.2880080040813597,
-            "y": 6.469712250400375,
-            "heading": 0.296082691458658,
-            "angularVelocity": -0.6183946755312791,
-            "velocityX": -0.07892657130746197,
-            "velocityY": -1.8907976105433808,
-            "timestamp": 0.22790937931421196
-        },
-        {
-            "x": 2.280012894319733,
-            "y": 6.278188663497132,
-            "heading": 0.23338596481601495,
-            "angularVelocity": -0.8252849465603376,
-            "velocityX": -0.10524064150870725,
-            "velocityY": -2.5210492101669324,
-            "timestamp": 0.3038791724189493
-        },
-        {
-            "x": 2.270017931664709,
-            "y": 6.038785816229407,
-            "heading": 0.1549313176962556,
-            "angularVelocity": -1.0327084478378958,
-            "velocityX": -0.13156495820969516,
-            "velocityY": -3.1512899730774246,
-            "timestamp": 0.37984896552368663
-        },
-        {
-            "x": 2.2620113109689415,
-            "y": 5.847271344563018,
-            "heading": 0.09176539516362914,
-            "angularVelocity": -0.8314610314331311,
-            "velocityX": -0.10539216139142096,
-            "velocityY": -2.5209292251507525,
-            "timestamp": 0.45581875862842397
-        },
-        {
-            "x": 2.2560057963370115,
-            "y": 5.703635646814395,
-            "heading": 0.0443838554377995,
-            "angularVelocity": -0.6236892031613949,
-            "velocityX": -0.079051349049357,
-            "velocityY": -1.8906948653999391,
-            "timestamp": 0.5317885517331613
-        },
-        {
-            "x": 2.252001935551058,
-            "y": 5.607878550007709,
-            "heading": 0.012794605457377246,
-            "angularVelocity": -0.4158132948561684,
-            "velocityX": -0.05270332618167983,
-            "velocityY": -1.2604627825518706,
-            "timestamp": 0.6077583448378986
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": -0.20790639031492017,
-            "velocityX": -0.026351730987313093,
-            "velocityY": -0.6302314123944018,
-            "timestamp": 0.6837281379426359
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -0.003,
-            "angularVelocity": -6.008173298568887e-34,
-            "velocityX": -7.305982513747354e-33,
-            "velocityY": -3.8813573012664696e-35,
-            "timestamp": 0.7596979310473732
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W1 To W3.traj b/src/main/deploy/choreo/W1 To W3.traj
deleted file mode 100644
index 5aab1ee6..00000000
--- a/src/main/deploy/choreo/W1 To W3.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.39,
-            "angularVelocity": -2.4663146867319515e-36,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3001378208847427,
-            "y": 6.713171893956039,
-            "heading": 0.36291265694481545,
-            "angularVelocity": -0.37089801282350154,
-            "velocityX": 0.0018871357066083216,
-            "velocityY": -0.6001237332286558,
-            "timestamp": 0.0730317826428328
-        },
-        {
-            "x": 2.3004082222445,
-            "y": 6.625427179140099,
-            "heading": 0.3106389081779897,
-            "angularVelocity": -0.7157671204942956,
-            "velocityX": 0.0037025162192713427,
-            "velocityY": -1.201459304985902,
-            "timestamp": 0.1460635652856656
-        },
-        {
-            "x": 2.300808308388558,
-            "y": 6.493639269598709,
-            "heading": 0.23621859479744584,
-            "angularVelocity": -1.019012691289514,
-            "velocityX": 0.005478246998500719,
-            "velocityY": -1.804528176258669,
-            "timestamp": 0.2190953479284984
-        },
-        {
-            "x": 2.30134752968001,
-            "y": 6.317621999647275,
-            "heading": 0.14515321533996162,
-            "angularVelocity": -1.2469280655909227,
-            "velocityX": 0.007383378468093947,
-            "velocityY": -2.4101461525629078,
-            "timestamp": 0.2921271305713312
-        },
-        {
-            "x": 2.3020704977057354,
-            "y": 6.097150895386178,
-            "heading": 0.04952404086318575,
-            "angularVelocity": -1.309418598536164,
-            "velocityX": 0.00989936161439861,
-            "velocityY": -3.0188377755932185,
-            "timestamp": 0.36515891321416405
-        },
-        {
-            "x": 2.3031517302727234,
-            "y": 5.834309739739576,
-            "heading": 0.004115026920641748,
-            "angularVelocity": -0.6217705812361183,
-            "velocityX": 0.014804959263771778,
-            "velocityY": -3.598996849522972,
-            "timestamp": 0.4381906958569969
-        },
-        {
-            "x": 2.303521267718626,
-            "y": 5.558035105167069,
-            "heading": 0.00411499765356137,
-            "angularVelocity": -4.007444337013033e-7,
-            "velocityX": 0.005059953797233799,
-            "velocityY": -3.7829370251531125,
-            "timestamp": 0.5112224784998297
-        },
-        {
-            "x": 2.3038907744757537,
-            "y": 5.281760470553516,
-            "heading": 0.004114968386478979,
-            "angularVelocity": -4.0074446127092794e-7,
-            "velocityX": 0.005059533586019425,
-            "velocityY": -3.782937025715139,
-            "timestamp": 0.5842542611426625
-        },
-        {
-            "x": 2.3020058670727765,
-            "y": 5.019317688425813,
-            "heading": -0.04121766182368216,
-            "angularVelocity": -0.6207246840990261,
-            "velocityX": -0.02580941248819387,
-            "velocityY": -3.5935420529332935,
-            "timestamp": 0.6572860437854954
-        },
-        {
-            "x": 2.301170882315387,
-            "y": 4.798980955675817,
-            "heading": -0.139726223563735,
-            "angularVelocity": -1.348845094221732,
-            "velocityX": -0.011433169603334988,
-            "velocityY": -3.016997871016921,
-            "timestamp": 0.7303178264283282
-        },
-        {
-            "x": 2.3006513716532107,
-            "y": 4.623098256624685,
-            "heading": -0.23378951684706242,
-            "angularVelocity": -1.2879775062228829,
-            "velocityX": -0.007113487352719777,
-            "velocityY": -2.4083035178163246,
-            "timestamp": 0.803349609071161
-        },
-        {
-            "x": 2.300310703422249,
-            "y": 4.491429967185545,
-            "heading": -0.31073488166813007,
-            "angularVelocity": -1.0535873839664371,
-            "velocityX": -0.004664657203118652,
-            "velocityY": -1.8028902578357346,
-            "timestamp": 0.8763813917139939
-        },
-        {
-            "x": 2.3001003821722423,
-            "y": 4.403776446553788,
-            "heading": -0.36487144716496317,
-            "angularVelocity": -0.7412740527174575,
-            "velocityX": -0.0028798591845282696,
-            "velocityY": -1.2002106132398922,
-            "timestamp": 0.9494131743568267
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.393,
-            "angularVelocity": -0.3851549533249317,
-            "velocityX": -0.0013744998219966374,
-            "velocityY": -0.5994163769475577,
-            "timestamp": 1.0224449569996594
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.393,
-            "angularVelocity": -7.841220088894993e-36,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 1.0954767396424923
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W2 To C1.traj b/src/main/deploy/choreo/W2 To C1.traj
deleted file mode 100644
index 70287ec9..00000000
--- a/src/main/deploy/choreo/W2 To C1.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": -3.731399929679607e-32,
-            "velocityY": 5.877471754111438e-38,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3271275380851444,
-            "y": 5.587086964112038,
-            "heading": 3.953847862678034e-18,
-            "angularVelocity": 3.9932078666702264e-17,
-            "velocityX": 0.7789533333445218,
-            "velocityY": 0.2735661154121556,
-            "timestamp": 0.09901432445765257
-        },
-        {
-            "x": 2.4813826113143547,
-            "y": 5.641260891303218,
-            "heading": 1.1398162294527873e-19,
-            "angularVelocity": -3.8780916638560175e-17,
-            "velocityX": 1.557906636985473,
-            "velocityY": 0.5471322203925046,
-            "timestamp": 0.19802864891530514
-        },
-        {
-            "x": 2.7127652138054734,
-            "y": 5.722521779507741,
-            "heading": 2.1553394674505063e-18,
-            "angularVelocity": 2.0616792512846884e-17,
-            "velocityX": 2.336859881219295,
-            "velocityY": 0.8206983045092451,
-            "timestamp": 0.2970429733729577
-        },
-        {
-            "x": 3.0212753279120443,
-            "y": 5.830869622528225,
-            "heading": -1.0672476097054788e-17,
-            "angularVelocity": -1.295551486240031e-16,
-            "velocityX": 3.1158129472318716,
-            "velocityY": 1.0942643260352094,
-            "timestamp": 0.3960572978306103
-        },
-        {
-            "x": 3.3746798137461993,
-            "y": 5.9549842366139965,
-            "heading": 1.8895880094915182e-18,
-            "angularVelocity": 1.2687117924961395e-16,
-            "velocityX": 3.5692258445423475,
-            "velocityY": 1.2535015995473786,
-            "timestamp": 0.49507162228826285
-        },
-        {
-            "x": 3.7280842995803773,
-            "y": 6.079098850699775,
-            "heading": 1.0049319654091473e-17,
-            "angularVelocity": 8.240960778697987e-17,
-            "velocityX": 3.569225844542579,
-            "velocityY": 1.25350159954746,
-            "timestamp": 0.5940859467459154
-        },
-        {
-            "x": 4.081488785414555,
-            "y": 6.203213464785553,
-            "heading": 8.136834826577652e-18,
-            "angularVelocity": -1.931523381075779e-17,
-            "velocityX": 3.569225844542579,
-            "velocityY": 1.25350159954746,
-            "timestamp": 0.693100271203568
-        },
-        {
-            "x": 4.434893271248733,
-            "y": 6.327328078871332,
-            "heading": -2.4381204969164037e-18,
-            "angularVelocity": -1.0680227715899811e-16,
-            "velocityX": 3.5692258445425793,
-            "velocityY": 1.2535015995474599,
-            "timestamp": 0.7921145956612206
-        },
-        {
-            "x": 4.788297757082911,
-            "y": 6.4514426929571105,
-            "heading": 3.192038150785043e-17,
-            "angularVelocity": 3.47005367081625e-16,
-            "velocityX": 3.569225844542579,
-            "velocityY": 1.25350159954746,
-            "timestamp": 0.8911289201188731
-        },
-        {
-            "x": 5.141702242917089,
-            "y": 6.575557307042889,
-            "heading": 6.016026907559514e-17,
-            "angularVelocity": 2.8521012209839933e-16,
-            "velocityX": 3.569225844542579,
-            "velocityY": 1.2535015995474599,
-            "timestamp": 0.9901432445765257
-        },
-        {
-            "x": 5.495106728751266,
-            "y": 6.699671921128667,
-            "heading": 2.4186102644429564e-17,
-            "angularVelocity": -3.63322848771759e-16,
-            "velocityX": 3.569225844542579,
-            "velocityY": 1.2535015995474599,
-            "timestamp": 1.0891575690341782
-        },
-        {
-            "x": 5.848511214585444,
-            "y": 6.823786535214446,
-            "heading": -1.2421929874703125e-18,
-            "angularVelocity": -2.568143121812034e-16,
-            "velocityX": 3.5692258445425793,
-            "velocityY": 1.2535015995474599,
-            "timestamp": 1.1881718934918308
-        },
-        {
-            "x": 6.2019157004196215,
-            "y": 6.9479011493002245,
-            "heading": 2.3184212364315377e-18,
-            "angularVelocity": 3.596059704901295e-17,
-            "velocityX": 3.569225844542579,
-            "velocityY": 1.2535015995474599,
-            "timestamp": 1.2871862179494835
-        },
-        {
-            "x": 6.5553201862538,
-            "y": 7.072015763386003,
-            "heading": 3.1459219033425486e-17,
-            "angularVelocity": 2.9430890890068574e-16,
-            "velocityX": 3.569225844542579,
-            "velocityY": 1.25350159954746,
-            "timestamp": 1.3862005424071362
-        },
-        {
-            "x": 6.9087246720879545,
-            "y": 7.196130377471773,
-            "heading": 2.9741502939674576e-17,
-            "angularVelocity": -1.7348157482865667e-17,
-            "velocityX": 3.5692258445423475,
-            "velocityY": 1.2535015995473786,
-            "timestamp": 1.4852148668647889
-        },
-        {
-            "x": 7.217234786194527,
-            "y": 7.304478220492258,
-            "heading": 1.2383304928032073e-17,
-            "angularVelocity": -1.753099667759744e-16,
-            "velocityX": 3.115812947231872,
-            "velocityY": 1.0942643260352094,
-            "timestamp": 1.5842291913224416
-        },
-        {
-            "x": 7.448617388685645,
-            "y": 7.385739108696781,
-            "heading": -3.803725574168796e-18,
-            "angularVelocity": -1.6348170333424006e-16,
-            "velocityX": 2.336859881219295,
-            "velocityY": 0.8206983045092451,
-            "timestamp": 1.6832435157800942
-        },
-        {
-            "x": 7.602872461914855,
-            "y": 7.439913035887961,
-            "heading": -2.4779119751079125e-18,
-            "angularVelocity": 1.3390118812558602e-17,
-            "velocityX": 1.557906636985473,
-            "velocityY": 0.5471322203925049,
-            "timestamp": 1.782257840237747
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 2.5025792615041696e-17,
-            "velocityX": 0.7789533333445218,
-            "velocityY": 0.27356611541215564,
-            "timestamp": 1.8812721646953996
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 1.3533974507170236e-34,
-            "velocityY": 0,
-            "timestamp": 1.9802864891530523
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W2 To C2.traj b/src/main/deploy/choreo/W2 To C2.traj
deleted file mode 100644
index 168ae5bd..00000000
--- a/src/main/deploy/choreo/W2 To C2.traj	
+++ /dev/null
@@ -1,526 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -1.058013890027075e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 2.26040474690827,
-            "y": 5.564834298284281,
-            "heading": -0.00020682462174428096,
-            "angularVelocity": -0.005575654083684234,
-            "velocityX": 0.28049498700644215,
-            "velocityY": 0.1303247879443207,
-            "timestamp": 0.037094234800108185
-        },
-        {
-            "x": 2.2812142405469196,
-            "y": 5.574502894544252,
-            "heading": -0.0006204719876615776,
-            "angularVelocity": -0.011151257551113,
-            "velocityX": 0.5609899692172368,
-            "velocityY": 0.26064956756953994,
-            "timestamp": 0.07418846960021637
-        },
-        {
-            "x": 2.312428480686622,
-            "y": 5.589005788443611,
-            "heading": -0.0012409398807264826,
-            "angularVelocity": -0.016726801251156914,
-            "velocityX": 0.8414849452457597,
-            "velocityY": 0.39097433812857635,
-            "timestamp": 0.11128270440032456
-        },
-        {
-            "x": 2.3540474670292637,
-            "y": 5.608342979609529,
-            "heading": -0.002068225636714691,
-            "angularVelocity": -0.022302273128063925,
-            "velocityX": 1.1219799132376411,
-            "velocityY": 0.5212990986367875,
-            "timestamp": 0.14837693920043274
-        },
-        {
-            "x": 2.406071199181798,
-            "y": 5.632514467619691,
-            "heading": -0.0031023259847413323,
-            "angularVelocity": -0.02787765682725579,
-            "velocityX": 1.4024748706336019,
-            "velocityY": 0.6516238477600773,
-            "timestamp": 0.18547117400054092
-        },
-        {
-            "x": 2.4684996766150022,
-            "y": 5.66152025198224,
-            "heading": -0.004343236799506005,
-            "angularVelocity": -0.03345292931508493,
-            "velocityX": 1.682969813762594,
-            "velocityY": 0.7819485836236739,
-            "timestamp": 0.2225654088006491
-        },
-        {
-            "x": 2.5413328985945514,
-            "y": 5.69536033210275,
-            "heading": -0.005790952692459236,
-            "angularVelocity": -0.03902805653641911,
-            "velocityX": 1.9634647370954057,
-            "velocityY": 0.9122733034625292,
-            "timestamp": 0.2596596436007573
-        },
-        {
-            "x": 2.6245708640566887,
-            "y": 5.7340347072253275,
-            "heading": -0.00744546628275827,
-            "angularVelocity": -0.04460298478228464,
-            "velocityX": 2.2439596317510366,
-            "velocityY": 1.0425980029237354,
-            "timestamp": 0.2967538784008655
-        },
-        {
-            "x": 2.7182135713591755,
-            "y": 5.777543376315584,
-            "heading": -0.009306766753500902,
-            "angularVelocity": -0.05017762142212937,
-            "velocityX": 2.5244544821346007,
-            "velocityY": 1.1729226744995158,
-            "timestamp": 0.3338481132009737
-        },
-        {
-            "x": 2.8222610176995655,
-            "y": 5.8258863377887575,
-            "heading": -0.011374836524793099,
-            "angularVelocity": -0.055751784136736884,
-            "velocityX": 2.8049492569688192,
-            "velocityY": 1.303247303352706,
-            "timestamp": 0.3709423480010819
-        },
-        {
-            "x": 2.9367131973687606,
-            "y": 5.879063588696104,
-            "heading": -0.013649641477510719,
-            "angularVelocity": -0.061325027055469855,
-            "velocityX": 3.0854438778950355,
-            "velocityY": 1.4335718527125696,
-            "timestamp": 0.4080365828011901
-        },
-        {
-            "x": 3.06157009301301,
-            "y": 5.937075120660461,
-            "heading": -0.01613108463550858,
-            "angularVelocity": -0.06689565565567898,
-            "velocityX": 3.3659380309924987,
-            "velocityY": 1.5638961762378367,
-            "timestamp": 0.4451308176012983
-        },
-        {
-            "x": 3.1888299168589427,
-            "y": 5.996203084762226,
-            "heading": -0.01613108529402469,
-            "angularVelocity": -1.7752519083825314e-8,
-            "velocityX": 3.430717051630934,
-            "velocityY": 1.5939933636693306,
-            "timestamp": 0.4822250524014065
-        },
-        {
-            "x": 3.316089744181463,
-            "y": 6.055331041381533,
-            "heading": -0.016131085952509703,
-            "angularVelocity": -1.7751680763659775e-8,
-            "velocityX": 3.4307171453540595,
-            "velocityY": 1.5939931619544851,
-            "timestamp": 0.5193192872015147
-        },
-        {
-            "x": 3.443349575355442,
-            "y": 6.114458989711434,
-            "heading": -0.01613108661098856,
-            "angularVelocity": -1.7751514817327164e-8,
-            "velocityX": 3.430717249183109,
-            "velocityY": 1.5939929384856546,
-            "timestamp": 0.5564135220016229
-        },
-        {
-            "x": 3.5706094108109987,
-            "y": 6.173586928826195,
-            "heading": -0.016131087269461084,
-            "angularVelocity": -1.7751344035751635e-8,
-            "velocityX": 3.4307173646074394,
-            "velocityY": 1.5939926900605101,
-            "timestamp": 0.5935077568017311
-        },
-        {
-            "x": 3.6978692510432434,
-            "y": 6.232714857660196,
-            "heading": -0.01613108792792704,
-            "angularVelocity": -1.7751166882414398e-8,
-            "velocityX": 3.4307174933791567,
-            "velocityY": 1.5939924129080143,
-            "timestamp": 0.6306019916018393
-        },
-        {
-            "x": 3.825129096624906,
-            "y": 6.291842774980762,
-            "heading": -0.016131088586386323,
-            "angularVelocity": -1.775098711225461e-8,
-            "velocityX": 3.430717637590709,
-            "velocityY": 1.5939921025246122,
-            "timestamp": 0.6676962264019475
-        },
-        {
-            "x": 3.952388948222243,
-            "y": 6.3509706793539165,
-            "heading": -0.016131089244838845,
-            "angularVelocity": -1.775080474504649e-8,
-            "velocityX": 3.4307177997634843,
-            "velocityY": 1.5939917534835466,
-            "timestamp": 0.7047904612020557
-        },
-        {
-            "x": 4.07964880661482,
-            "y": 6.410098569101814,
-            "heading": -0.016131089903284688,
-            "angularVelocity": -1.775062484471552e-8,
-            "velocityX": 3.430717982952055,
-            "velocityY": 1.5939913592104822,
-            "timestamp": 0.7418846960021639
-        },
-        {
-            "x": 4.206908672718448,
-            "y": 6.469226442253365,
-            "heading": -0.016131090561724272,
-            "angularVelocity": -1.7750456035932977e-8,
-            "velocityX": 3.4307181908294035,
-            "velocityY": 1.5939909118000826,
-            "timestamp": 0.7789789308022721
-        },
-        {
-            "x": 4.33416854760619,
-            "y": 6.52835429649903,
-            "heading": -0.016131091220158388,
-            "angularVelocity": -1.775030869099235e-8,
-            "velocityX": 3.43071842763476,
-            "velocityY": 1.593990402128264,
-            "timestamp": 0.8160731656023803
-        },
-        {
-            "x": 4.461428432515134,
-            "y": 6.587482129176235,
-            "heading": -0.016131091878588796,
-            "angularVelocity": -1.7750208582663698e-8,
-            "velocityX": 3.430718697790006,
-            "velocityY": 1.5939898206778003,
-            "timestamp": 0.8531674004024885
-        },
-        {
-            "x": 4.588688397859823,
-            "y": 6.646609788732575,
-            "heading": -0.01613109253701839,
-            "angularVelocity": -1.7750186861935105e-8,
-            "velocityX": 3.4307208662063466,
-            "velocityY": 1.5939851536219738,
-            "timestamp": 0.8902616352025967
-        },
-        {
-            "x": 4.7169608374986005,
-            "y": 6.703507592945478,
-            "heading": -0.016131093196594153,
-            "angularVelocity": -1.7781085471466767e-8,
-            "velocityX": 3.4580155199319775,
-            "velocityY": 1.533871894635887,
-            "timestamp": 0.9273558700027049
-        },
-        {
-            "x": 4.84945193271468,
-            "y": 6.749738410352506,
-            "heading": -0.016131093868950192,
-            "angularVelocity": -1.8125621008136676e-8,
-            "velocityX": 3.571743585763167,
-            "velocityY": 1.2463073481945173,
-            "timestamp": 0.9644501048028131
-        },
-        {
-            "x": 4.985276817345946,
-            "y": 6.784991484609654,
-            "heading": -0.0161310945734632,
-            "angularVelocity": -1.8992520332334574e-8,
-            "velocityX": 3.661617104738627,
-            "velocityY": 0.9503653181449264,
-            "timestamp": 1.0015443396029213
-        },
-        {
-            "x": 5.123527608694991,
-            "y": 6.809030968493646,
-            "heading": -0.01613109533353302,
-            "angularVelocity": -2.0490241268499195e-8,
-            "velocityX": 3.7270155886499414,
-            "velocityY": 0.6480652320645928,
-            "timestamp": 1.0386385744030295
-        },
-        {
-            "x": 5.263280146337045,
-            "y": 6.821696104409015,
-            "heading": -0.016131096180260384,
-            "angularVelocity": -2.2826387114294202e-8,
-            "velocityX": 3.767500216547063,
-            "velocityY": 0.34143138370740883,
-            "timestamp": 1.0757328092031377
-        },
-        {
-            "x": 5.403600215911865,
-            "y": 6.822902202606201,
-            "heading": -0.0161310971470982,
-            "angularVelocity": -2.6064368823146806e-8,
-            "velocityX": 3.7827999507462917,
-            "velocityY": 0.032514437989762196,
-            "timestamp": 1.112827044003246
-        },
-        {
-            "x": 5.530811605245593,
-            "y": 6.814532094483614,
-            "heading": -0.016131098042384447,
-            "angularVelocity": -2.6566067685833464e-8,
-            "velocityX": 3.7747775034488793,
-            "velocityY": -0.2483684519763222,
-            "timestamp": 1.1465274115659696
-        },
-        {
-            "x": 5.657050747526861,
-            "y": 6.796742322973506,
-            "heading": -0.016131098828254187,
-            "angularVelocity": -2.3319322572510494e-8,
-            "velocityX": 3.745927757206515,
-            "velocityY": -0.5278806374143462,
-            "timestamp": 1.1802277791286933
-        },
-        {
-            "x": 5.781621117814577,
-            "y": 6.76963106353518,
-            "heading": -0.016131099539274135,
-            "angularVelocity": -2.109828463295381e-8,
-            "velocityX": 3.6964098405118033,
-            "velocityY": -0.8044796362492181,
-            "timestamp": 1.213928146691417
-        },
-        {
-            "x": 5.903835409646495,
-            "y": 6.733347943713289,
-            "heading": -0.016131100200026813,
-            "angularVelocity": -1.96066906061588e-8,
-            "velocityX": 3.626497295747634,
-            "velocityY": -1.0766386970219277,
-            "timestamp": 1.2476285142541406
-        },
-        {
-            "x": 6.023019351333038,
-            "y": 6.68809326224078,
-            "heading": -0.016131100829259,
-            "angularVelocity": -1.86713746487578e-8,
-            "velocityX": 3.5365769072018636,
-            "velocityY": -1.3428542400399708,
-            "timestamp": 1.2813288818168642
-        },
-        {
-            "x": 6.138515596479519,
-            "y": 6.634117196875684,
-            "heading": -0.016131101442406693,
-            "angularVelocity": -1.81940951537302e-8,
-            "velocityX": 3.4271509036665004,
-            "velocityY": -1.601646191681265,
-            "timestamp": 1.3150292493795879
-        },
-        {
-            "x": 6.2512259314426855,
-            "y": 6.5745422130600915,
-            "heading": -0.016131102051778846,
-            "angularVelocity": -1.80820624580199e-8,
-            "velocityX": 3.3444838473464196,
-            "velocityY": -1.7677843929954529,
-            "timestamp": 1.3487296169423115
-        },
-        {
-            "x": 6.363935646775815,
-            "y": 6.514966056976746,
-            "heading": -0.01613110266098242,
-            "angularVelocity": -1.807706019437417e-8,
-            "velocityX": 3.344465460898959,
-            "velocityY": -1.7678191780093255,
-            "timestamp": 1.3824299845050352
-        },
-        {
-            "x": 6.4758517936606514,
-            "y": 6.453912192428435,
-            "heading": -0.016131102003666662,
-            "angularVelocity": 1.950470585863807e-8,
-            "velocityX": 3.320917692560386,
-            "velocityY": -1.811667615632877,
-            "timestamp": 1.4161303520677588
-        },
-        {
-            "x": 6.587767622018337,
-            "y": 6.392857744000494,
-            "heading": -0.016131101346154864,
-            "angularVelocity": 1.951052313957626e-8,
-            "velocityX": 3.32090824081921,
-            "velocityY": -1.8116849412489389,
-            "timestamp": 1.4498307196304825
-        },
-        {
-            "x": 6.699683405881235,
-            "y": 6.331803214011495,
-            "heading": -0.016131100688641712,
-            "angularVelocity": 1.9510563155904532e-8,
-            "velocityX": 3.3209069205135386,
-            "velocityY": -1.811687361432026,
-            "timestamp": 1.4835310871932061
-        },
-        {
-            "x": 6.811599167638959,
-            "y": 6.270748643502715,
-            "heading": -0.016131100031139815,
-            "angularVelocity": 1.9510229240352237e-8,
-            "velocityX": 3.320906264580792,
-            "velocityY": -1.8116885637862563,
-            "timestamp": 1.5172314547559298
-        },
-        {
-            "x": 6.923514914598752,
-            "y": 6.209694045868981,
-            "heading": -0.016131099373601954,
-            "angularVelocity": 1.95112965056638e-8,
-            "velocityX": 3.3209058254778503,
-            "velocityY": -1.8116893686722635,
-            "timestamp": 1.5509318223186535
-        },
-        {
-            "x": 7.031584175346113,
-            "y": 6.1507377884047765,
-            "heading": -0.013826704963615632,
-            "angularVelocity": 0.0683789102803133,
-            "velocityX": 3.2067680136194574,
-            "velocityY": -1.7494247608568367,
-            "timestamp": 1.584632189881377
-        },
-        {
-            "x": 7.131340439149152,
-            "y": 6.096316611291437,
-            "heading": -0.011699535798216875,
-            "angularVelocity": 0.06312005830317652,
-            "velocityX": 2.960094236876553,
-            "velocityY": -1.614854111369865,
-            "timestamp": 1.6183325574441008
-        },
-        {
-            "x": 7.222783690004588,
-            "y": 6.046430524071638,
-            "heading": -0.00974961791861361,
-            "angularVelocity": 0.057860433598327515,
-            "velocityX": 2.713419985263968,
-            "velocityY": -1.4802831787205646,
-            "timestamp": 1.6520329250068244
-        },
-        {
-            "x": 7.305913922772626,
-            "y": 6.001079530283105,
-            "heading": -0.00797695998479322,
-            "angularVelocity": 0.05260055192336173,
-            "velocityX": 2.466745581136908,
-            "velocityY": -1.3457121410953312,
-            "timestamp": 1.685733292569548
-        },
-        {
-            "x": 7.380731135011445,
-            "y": 5.960263631930958,
-            "heading": -0.00638156583911186,
-            "angularVelocity": 0.04734055623313652,
-            "velocityX": 2.2200711045530213,
-            "velocityY": -1.2111410439717167,
-            "timestamp": 1.7194336601322717
-        },
-        {
-            "x": 7.447235325348076,
-            "y": 5.923982830388488,
-            "heading": -0.004963437343393232,
-            "angularVelocity": 0.042080505296519764,
-            "velocityX": 1.9733965872286794,
-            "velocityY": -1.0765699060980205,
-            "timestamp": 1.7531340276949954
-        },
-        {
-            "x": 7.505426492937412,
-            "y": 5.892237126701046,
-            "heading": -0.0037225753686417596,
-            "angularVelocity": 0.0368204285143562,
-            "velocityX": 1.72672204482727,
-            "velocityY": -0.9419987372053683,
-            "timestamp": 1.786834395257719
-        },
-        {
-            "x": 7.555304637231595,
-            "y": 5.8650265217185185,
-            "heading": -0.002658980227466916,
-            "angularVelocity": 0.03156034245604838,
-            "velocityX": 1.4800474861690802,
-            "velocityY": -0.8074275430937743,
-            "timestamp": 1.8205347628204427
-        },
-        {
-            "x": 7.596869757865521,
-            "y": 5.842351016163066,
-            "heading": -0.0017726518914738514,
-            "angularVelocity": 0.02630025724030852,
-            "velocityX": 1.233372916677093,
-            "velocityY": -0.6728563275533557,
-            "timestamp": 1.8542351303831663
-        },
-        {
-            "x": 7.630121854593904,
-            "y": 5.824210610667954,
-            "heading": -0.001063590116536395,
-            "angularVelocity": 0.021040179268589657,
-            "velocityX": 0.9866983399066315,
-            "velocityY": -0.5382850932218669,
-            "timestamp": 1.88793549794589
-        },
-        {
-            "x": 7.655060927254085,
-            "y": 5.810605305801891,
-            "heading": -0.000531794525727047,
-            "angularVelocity": 0.01578011248154109,
-            "velocityX": 0.7400237583095566,
-            "velocityY": -0.4037138420149424,
-            "timestamp": 1.9216358655086136
-        },
-        {
-            "x": 7.671686975742638,
-            "y": 5.8015351020850225,
-            "heading": -0.00017726465841382563,
-            "angularVelocity": 0.010520059362948236,
-            "velocityX": 0.49334917364355607,
-            "velocityY": -0.26914257537363395,
-            "timestamp": 1.9553362330713373
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 3.6111480760753294e-31,
-            "angularVelocity": 0.005260021514121506,
-            "velocityX": 0.24667458721003926,
-            "velocityY": -0.1345712944104092,
-            "timestamp": 1.989036600634061
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": -8.452347473573201e-32,
-            "angularVelocity": -1.5098225699671103e-29,
-            "velocityX": 2.1451735090517096e-32,
-            "velocityY": 9.12030277003484e-33,
-            "timestamp": 2.0227369681967846
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W2 To C3.traj b/src/main/deploy/choreo/W2 To C3.traj
deleted file mode 100644
index 72a99c84..00000000
--- a/src/main/deploy/choreo/W2 To C3.traj	
+++ /dev/null
@@ -1,571 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 8.750120685231394e-32,
-            "velocityX": 3.618098191401073e-32,
-            "velocityY": 7.301888602720515e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 2.256418341264449,
-            "y": 5.558391022160101,
-            "heading": -2.5296452464584536e-15,
-            "angularVelocity": -8.979773235851594e-14,
-            "velocityX": 0.22783925602590605,
-            "velocityY": -0.05711574048502746,
-            "timestamp": 0.0281704802605187
-        },
-        {
-            "x": 2.2692550247581535,
-            "y": 5.555173071000616,
-            "heading": -7.593688933611638e-15,
-            "angularVelocity": -1.7976419432911875e-13,
-            "velocityX": 0.4556785463006615,
-            "velocityY": -0.11423132050732354,
-            "timestamp": 0.0563409605210374
-        },
-        {
-            "x": 2.2885100515708303,
-            "y": 5.55034615163608,
-            "heading": -1.5195474783343342e-14,
-            "angularVelocity": -2.6984935224723246e-13,
-            "velocityX": 0.6835178752583485,
-            "velocityY": -0.1713467189730698,
-            "timestamp": 0.0845114407815561
-        },
-        {
-            "x": 2.314183422940896,
-            "y": 5.54391026989229,
-            "heading": -2.533887633402318e-14,
-            "angularVelocity": -3.6007201361891127e-13,
-            "velocityX": 0.9113572481775378,
-            "velocityY": -0.2284619106337897,
-            "timestamp": 0.1126819210420748
-        },
-        {
-            "x": 2.346275140284958,
-            "y": 5.535865432453464,
-            "heading": -3.8024880419905343e-14,
-            "angularVelocity": -4.5032970578145327e-13,
-            "velocityX": 1.139196671383666,
-            "velocityY": -0.2855768650171331,
-            "timestamp": 0.14085240130259352
-        },
-        {
-            "x": 2.3847852052344276,
-            "y": 5.526211647048542,
-            "heading": -5.3249348657163737e-14,
-            "angularVelocity": -5.404404929621474e-13,
-            "velocityX": 1.3670361525018668,
-            "velocityY": -0.3426915450373883,
-            "timestamp": 0.16902288156311224
-        },
-        {
-            "x": 2.4297136196807942,
-            "y": 5.514948922688138,
-            "heading": -7.10022499667577e-14,
-            "angularVelocity": -6.301951953680268e-13,
-            "velocityX": 1.594875700764475,
-            "velocityY": -0.3998059051974526,
-            "timestamp": 0.19719336182363095
-        },
-        {
-            "x": 2.4810603858307716,
-            "y": 5.502077269965895,
-            "heading": -9.12659901579941e-14,
-            "angularVelocity": -7.193253365088161e-13,
-            "velocityX": 1.8227153273613368,
-            "velocityY": -0.45691988930278143,
-            "timestamp": 0.22536384208414967
-        },
-        {
-            "x": 2.5388255062707983,
-            "y": 5.4875967014378215,
-            "heading": -1.1400857239830863e-13,
-            "angularVelocity": -8.073196492084524e-13,
-            "velocityX": 2.050555045771992,
-            "velocityY": -0.5140334276930385,
-            "timestamp": 0.2535343223446684
-        },
-        {
-            "x": 2.603008984034667,
-            "y": 5.4715072320838045,
-            "heading": -1.3917722607752625e-13,
-            "angularVelocity": -8.934407027436765e-13,
-            "velocityX": 2.2783948718766736,
-            "velocityY": -0.571146434324935,
-            "timestamp": 0.2817048026051871
-        },
-        {
-            "x": 2.673610822650838,
-            "y": 5.453808879815544,
-            "heading": -1.6668826583849139e-13,
-            "angularVelocity": -9.76591080715578e-13,
-            "velocityX": 2.5062348232351805,
-            "velocityY": -0.6282588051246059,
-            "timestamp": 0.3098752828657058
-        },
-        {
-            "x": 2.7506310260932763,
-            "y": 5.434501665856061,
-            "heading": -1.9641690622397207e-13,
-            "angularVelocity": -1.0553118055097966e-12,
-            "velocityX": 2.7340749156620854,
-            "velocityY": -0.6853704225462842,
-            "timestamp": 0.33804576312622453
-        },
-        {
-            "x": 2.8340695983911575,
-            "y": 5.413585614341132,
-            "heading": -2.282023162882806e-13,
-            "angularVelocity": -1.128323328886794e-12,
-            "velocityX": 2.9619151511173074,
-            "velocityY": -0.7424811831924288,
-            "timestamp": 0.36621624338674325
-        },
-        {
-            "x": 2.9239265420414853,
-            "y": 5.391060748788907,
-            "heading": -2.61980248572899e-13,
-            "angularVelocity": -1.1990541861941579e-12,
-            "velocityX": 3.189755475211519,
-            "velocityY": -0.7995911089877218,
-            "timestamp": 0.39438672364726196
-        },
-        {
-            "x": 3.020201851829833,
-            "y": 5.366927076274376,
-            "heading": -2.9867647823192555e-13,
-            "angularVelocity": -1.302648350982896e-12,
-            "velocityX": 3.417595614203248,
-            "velocityY": -0.8567007836339452,
-            "timestamp": 0.4225572039077807
-        },
-        {
-            "x": 3.12289548969471,
-            "y": 5.341184519140017,
-            "heading": -3.474157859958988e-13,
-            "angularVelocity": -1.7301553723681501e-12,
-            "velocityX": 3.6454344020823566,
-            "velocityY": -0.9138132149787032,
-            "timestamp": 0.4507276841682994
-        },
-        {
-            "x": 3.2262642847742486,
-            "y": 5.315271868799975,
-            "heading": -3.474349355086831e-13,
-            "angularVelocity": -6.797723212352284e-16,
-            "velocityX": 3.6694012357471815,
-            "velocityY": -0.9198512095073661,
-            "timestamp": 0.4788981644288181
-        },
-        {
-            "x": 3.3296318094304937,
-            "y": 5.289354150477899,
-            "heading": -3.47428956348503e-13,
-            "angularVelocity": 2.1224913898406702e-16,
-            "velocityX": 3.669356138067555,
-            "velocityY": -0.920031113505745,
-            "timestamp": 0.5070686446893368
-        },
-        {
-            "x": 3.4329975109325144,
-            "y": 5.263429161946644,
-            "heading": -3.4742781851400116e-13,
-            "angularVelocity": 4.0391022494337805e-17,
-            "velocityX": 3.669291419461124,
-            "velocityY": -0.9202891924987843,
-            "timestamp": 0.5352391249498555
-        },
-        {
-            "x": 3.536360974747324,
-            "y": 5.237495253145949,
-            "heading": -3.474322284236003e-13,
-            "angularVelocity": -1.565436427905273e-16,
-            "velocityX": 3.669211985699591,
-            "velocityY": -0.9206058455823695,
-            "timestamp": 0.5634096052103742
-        },
-        {
-            "x": 3.6397250084501422,
-            "y": 5.211563615847215,
-            "heading": -3.474363074560882e-13,
-            "angularVelocity": -1.4479811668640362e-16,
-            "velocityX": 3.6692322156709736,
-            "velocityY": -0.9205252114596718,
-            "timestamp": 0.5915800854708929
-        },
-        {
-            "x": 3.7430830091153613,
-            "y": 5.185607942589789,
-            "heading": -3.474405753435316e-13,
-            "angularVelocity": -1.515021186684875e-16,
-            "velocityX": 3.669018053983158,
-            "velocityY": -0.9213784435831122,
-            "timestamp": 0.6197505657314116
-        },
-        {
-            "x": 3.8464299042958676,
-            "y": 5.1596080862072276,
-            "heading": -3.474364187384818e-13,
-            "angularVelocity": 1.4755179936907513e-16,
-            "velocityX": 3.668623829794913,
-            "velocityY": -0.92294686289042,
-            "timestamp": 0.6479210459919303
-        },
-        {
-            "x": 3.9490305912287407,
-            "y": 5.130804053406161,
-            "heading": -3.4744608105895375e-13,
-            "angularVelocity": -3.429945241470833e-16,
-            "velocityX": 3.642134815737201,
-            "velocityY": -1.0224899446047346,
-            "timestamp": 0.6760915262524491
-        },
-        {
-            "x": 4.049738686089906,
-            "y": 5.09595505292046,
-            "heading": -3.4745592092862155e-13,
-            "angularVelocity": -3.4929719255187864e-16,
-            "velocityX": 3.57495129404341,
-            "velocityY": -1.237075128411728,
-            "timestamp": 0.7042620065129678
-        },
-        {
-            "x": 4.148105144500732,
-            "y": 5.054959774017334,
-            "heading": -3.474586084587748e-13,
-            "angularVelocity": -9.540235481877701e-17,
-            "velocityX": 3.491827526585996,
-            "velocityY": -1.4552566560457803,
-            "timestamp": 0.7324324867734865
-        },
-        {
-            "x": 4.301558125072517,
-            "y": 4.97182210350292,
-            "heading": -3.4745624581283064e-13,
-            "angularVelocity": 5.1211249699831376e-17,
-            "velocityX": 3.3261517465815813,
-            "velocityY": -1.8020406443565171,
-            "timestamp": 0.7785677783246081
-        },
-        {
-            "x": 4.445898119107738,
-            "y": 4.873711676741488,
-            "heading": -3.4747329562610653e-13,
-            "angularVelocity": -3.6956119081305154e-16,
-            "velocityX": 3.1286242956827928,
-            "velocityY": -2.1265808335182323,
-            "timestamp": 0.8247030698757297
-        },
-        {
-            "x": 4.587571576014754,
-            "y": 4.771788561847154,
-            "heading": -3.4746251546031876e-13,
-            "angularVelocity": 2.3366419539300746e-16,
-            "velocityX": 3.0708260887450245,
-            "velocityY": -2.2092222996219535,
-            "timestamp": 0.8708383614268513
-        },
-        {
-            "x": 4.729219222551993,
-            "y": 4.66982957987028,
-            "heading": -3.474587781166732e-13,
-            "angularVelocity": 8.100834549122579e-17,
-            "velocityX": 3.070266639108493,
-            "velocityY": -2.209999732353969,
-            "timestamp": 0.916973652977973
-        },
-        {
-            "x": 4.870877206231954,
-            "y": 4.567884960544782,
-            "heading": -3.47487867941798e-13,
-            "angularVelocity": -6.305330291624225e-16,
-            "velocityX": 3.070490700636254,
-            "velocityY": -2.209688416384298,
-            "timestamp": 0.9631089445290946
-        },
-        {
-            "x": 5.012587244979431,
-            "y": 4.466012713767847,
-            "heading": -3.4750411523417436e-13,
-            "angularVelocity": -3.5216624493767476e-16,
-            "velocityX": 3.0716190140743462,
-            "velocityY": -2.208119713797033,
-            "timestamp": 1.009244236080216
-        },
-        {
-            "x": 5.160453670184351,
-            "y": 4.373302358522453,
-            "heading": -3.47491376477911e-13,
-            "angularVelocity": 2.7611738924244047e-16,
-            "velocityX": 3.2050610331965634,
-            "velocityY": -2.009532228521384,
-            "timestamp": 1.0553795276313376
-        },
-        {
-            "x": 5.316942928228011,
-            "y": 4.29603139716118,
-            "heading": -3.4750002331748596e-13,
-            "angularVelocity": -1.8742353831509497e-16,
-            "velocityX": 3.3919642161767567,
-            "velocityY": -1.6748774910066955,
-            "timestamp": 1.101514819182459
-        },
-        {
-            "x": 5.480467319488525,
-            "y": 4.235044002532959,
-            "heading": -3.475163170309309e-13,
-            "angularVelocity": -3.5317243928660403e-16,
-            "velocityX": 3.5444534056932553,
-            "velocityY": -1.321924985762245,
-            "timestamp": 1.1476501107335806
-        },
-        {
-            "x": 5.5914176787721175,
-            "y": 4.201991970759951,
-            "heading": -3.4752963658489967e-13,
-            "angularVelocity": -4.352387405096259e-16,
-            "velocityX": 3.625488497149835,
-            "velocityY": -1.0800304007696078,
-            "timestamp": 1.1782529824212593
-        },
-        {
-            "x": 5.704342870003183,
-            "y": 4.176490587918632,
-            "heading": -3.4752519346531394e-13,
-            "angularVelocity": 1.4518636145588672e-16,
-            "velocityX": 3.6900194329281493,
-            "velocityY": -0.8333003223399679,
-            "timestamp": 1.208855854108938
-        },
-        {
-            "x": 5.818728492967466,
-            "y": 4.15864825583045,
-            "heading": -3.47528320752536e-13,
-            "angularVelocity": -1.0218933861720526e-16,
-            "velocityX": 3.737741481635173,
-            "velocityY": -0.5830280331479286,
-            "timestamp": 1.2394587257966168
-        },
-        {
-            "x": 5.934052260797228,
-            "y": 4.148506842880417,
-            "heading": -3.475166383708343e-13,
-            "angularVelocity": 3.817413550372978e-16,
-            "velocityX": 3.768396933681978,
-            "velocityY": -0.3313876244719343,
-            "timestamp": 1.2700615974842955
-        },
-        {
-            "x": 6.049784939896788,
-            "y": 4.145614314337883,
-            "heading": -3.4752553093783573e-13,
-            "angularVelocity": -2.9057949488904467e-16,
-            "velocityX": 3.7817587931181613,
-            "velocityY": -0.0945182064161531,
-            "timestamp": 1.3006644691719742
-        },
-        {
-            "x": 6.1655453259831,
-            "y": 4.144216905783092,
-            "heading": -3.4753489515012823e-13,
-            "angularVelocity": -3.0599129336581387e-16,
-            "velocityX": 3.7826641652362696,
-            "velocityY": -0.04566266099543126,
-            "timestamp": 1.331267340859653
-        },
-        {
-            "x": 6.281306292241875,
-            "y": 4.142868414150292,
-            "heading": -3.4754223223995867e-13,
-            "angularVelocity": -2.397516775868154e-16,
-            "velocityX": 3.7826831233414593,
-            "velocityY": -0.04406421878574171,
-            "timestamp": 1.3618702125473316
-        },
-        {
-            "x": 6.39706739301833,
-            "y": 4.141531519398385,
-            "heading": -3.475633687164305e-13,
-            "angularVelocity": -6.906697084916283e-16,
-            "velocityX": 3.7826875189315166,
-            "velocityY": -0.04368527131792852,
-            "timestamp": 1.3924730842350104
-        },
-        {
-            "x": 6.512828617962261,
-            "y": 4.14020542040497,
-            "heading": -3.4758398313992887e-13,
-            "angularVelocity": -6.73610754719036e-16,
-            "velocityX": 3.7826915763114313,
-            "velocityY": -0.043332501854336236,
-            "timestamp": 1.423075955922689
-        },
-        {
-            "x": 6.628589931637748,
-            "y": 4.138887090943481,
-            "heading": -3.4757335612235213e-13,
-            "angularVelocity": 3.472555676974266e-16,
-            "velocityX": 3.7826944757635745,
-            "velocityY": -0.04307861939704374,
-            "timestamp": 1.4536788276103678
-        },
-        {
-            "x": 6.744351296535766,
-            "y": 4.137573267392239,
-            "heading": -3.475819448429605e-13,
-            "angularVelocity": -2.806508055564177e-16,
-            "velocityX": 3.782696149545524,
-            "velocityY": -0.042931381231176444,
-            "timestamp": 1.4842816992980465
-        },
-        {
-            "x": 6.860112695906494,
-            "y": 4.136261713498386,
-            "heading": -3.475979530506806e-13,
-            "angularVelocity": -5.230943272138926e-16,
-            "velocityX": 3.7826972759991557,
-            "velocityY": -0.04285721636551622,
-            "timestamp": 1.5148845709857253
-        },
-        {
-            "x": 6.9694302247553,
-            "y": 4.135029589899985,
-            "heading": -2.392571520797574e-13,
-            "angularVelocity": 3.5402168172320343e-12,
-            "velocityX": 3.5721330326269505,
-            "velocityY": -0.04026169865772549,
-            "timestamp": 1.545487442673404
-        },
-        {
-            "x": 7.070940187048315,
-            "y": 4.133884255746116,
-            "heading": -1.6833543298116243e-13,
-            "angularVelocity": 2.317485846705618e-12,
-            "velocityX": 3.3170077412667367,
-            "velocityY": -0.03742570844047695,
-            "timestamp": 1.5760903143610827
-        },
-        {
-            "x": 7.164641690009617,
-            "y": 4.132826276383854,
-            "heading": -1.1981581485168293e-13,
-            "angularVelocity": 1.5854597325485145e-12,
-            "velocityX": 3.061853276960511,
-            "velocityY": -0.034571244567840914,
-            "timestamp": 1.6066931860487614
-        },
-        {
-            "x": 7.250534738513507,
-            "y": 4.131855992058627,
-            "heading": -8.523066967233492e-14,
-            "angularVelocity": 1.1301275060622065e-12,
-            "velocityX": 2.8066989719298543,
-            "velocityY": -0.03170566260882202,
-            "timestamp": 1.6372960577364402
-        },
-        {
-            "x": 7.328619329487729,
-            "y": 4.1309736027819035,
-            "heading": -6.003549662940244e-14,
-            "angularVelocity": 8.232944437274219e-13,
-            "velocityX": 2.5515445665080714,
-            "velocityY": -0.028833544941282405,
-            "timestamp": 1.6678989294241189
-        },
-        {
-            "x": 7.398895462248961,
-            "y": 4.130179235828021,
-            "heading": -4.153185117983844e-14,
-            "angularVelocity": 6.046375734752488e-13,
-            "velocityX": 2.2963901387575416,
-            "velocityY": -0.02595726837221309,
-            "timestamp": 1.6985018011117976
-        },
-        {
-            "x": 7.461363136410412,
-            "y": 4.129472977946104,
-            "heading": -2.797073854746349e-14,
-            "angularVelocity": 4.431320366500053e-13,
-            "velocityX": 2.041235698368024,
-            "velocityY": -0.0230781571305896,
-            "timestamp": 1.7291046727994763
-        },
-        {
-            "x": 7.516022351719397,
-            "y": 4.1288548915008905,
-            "heading": -1.8141238812912564e-14,
-            "angularVelocity": 3.211953369611228e-13,
-            "velocityX": 1.7860812497215437,
-            "velocityY": -0.02019700801679021,
-            "timestamp": 1.759707544487155
-        },
-        {
-            "x": 7.5628731079961895,
-            "y": 4.128325023227497,
-            "heading": -1.1163529343344889e-14,
-            "angularVelocity": 2.2800831618830555e-13,
-            "velocityX": 1.5309267952022354,
-            "velocityY": -0.017314331754962387,
-            "timestamp": 1.7903104161748338
-        },
-        {
-            "x": 7.601915405105201,
-            "y": 4.127883409320133,
-            "heading": -6.372564950074351e-15,
-            "angularVelocity": 1.5655276164796086e-13,
-            "velocityX": 1.2757723362523365,
-            "velocityY": -0.014430472786738478,
-            "timestamp": 1.8209132878625125
-        },
-        {
-            "x": 7.63314924293974,
-            "y": 4.127530078564582,
-            "heading": -3.2435183420078772e-15,
-            "angularVelocity": 1.0224681844586284e-13,
-            "velocityX": 1.020617873816125,
-            "velocityY": -0.011545673194894595,
-            "timestamp": 1.8515161595501912
-        },
-        {
-            "x": 7.656574621413203,
-            "y": 4.127265054360105,
-            "heading": -1.3581702257498648e-15,
-            "angularVelocity": 6.160689169092944e-14,
-            "velocityX": 0.7654634085499833,
-            "velocityY": -0.008660108992989924,
-            "timestamp": 1.88211903123787
-        },
-        {
-            "x": 7.672191540453606,
-            "y": 4.127088356077157,
-            "heading": -3.7222995775515084e-16,
-            "angularVelocity": 3.2217238765593925e-14,
-            "velocityX": 0.5103089409316736,
-            "velocityY": -0.0057739118290542285,
-            "timestamp": 1.9127219029255487
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 2.9643987227812266e-34,
-            "angularVelocity": 1.2163231774130929e-14,
-            "velocityX": 0.25515447132169783,
-            "velocityY": -0.0028871825495910644,
-            "timestamp": 1.9433247746132274
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": -1.7511816257564745e-34,
-            "angularVelocity": -1.536305146096975e-32,
-            "velocityX": 2.290424541792184e-34,
-            "velocityY": 7.21466542895957e-33,
-            "timestamp": 1.9739276463009061
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W2 To C4.traj b/src/main/deploy/choreo/W2 To C4.traj
deleted file mode 100644
index 5f6c2964..00000000
--- a/src/main/deploy/choreo/W2 To C4.traj	
+++ /dev/null
@@ -1,589 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.253608767824088,
-            "y": 5.559959646221142,
-            "heading": 0.04333845773860565,
-            "angularVelocity": 1.2657316926377489,
-            "velocityX": 0.10539673178657698,
-            "velocityY": -0.0011785619397610312,
-            "timestamp": 0.03423984560921402
-        },
-        {
-            "x": 2.260842860117559,
-            "y": 5.560038303784881,
-            "heading": 0.12991520296053788,
-            "angularVelocity": 2.5285378389274697,
-            "velocityX": 0.211277012637124,
-            "velocityY": 0.002297252290136546,
-            "timestamp": 0.06847969121842803
-        },
-        {
-            "x": 2.271629728646886,
-            "y": 5.560441749821537,
-            "heading": 0.2597959154554935,
-            "angularVelocity": 3.7932622120236568,
-            "velocityX": 0.3150384687021929,
-            "velocityY": 0.011782939714764799,
-            "timestamp": 0.10271953682764205
-        },
-        {
-            "x": 2.285721166472015,
-            "y": 5.561272024294721,
-            "heading": 0.433416913958427,
-            "angularVelocity": 5.070729596286839,
-            "velocityX": 0.41155085761651194,
-            "velocityY": 0.024248779701277118,
-            "timestamp": 0.13695938243685607
-        },
-        {
-            "x": 2.302746549134424,
-            "y": 5.561830584813036,
-            "heading": 0.6513804515506975,
-            "angularVelocity": 6.36578622695705,
-            "velocityX": 0.497238885266109,
-            "velocityY": 0.01631317280720512,
-            "timestamp": 0.17119922804607007
-        },
-        {
-            "x": 2.325552284167229,
-            "y": 5.559809655953846,
-            "heading": 0.9008347442106217,
-            "angularVelocity": 7.285497005652254,
-            "velocityX": 0.6660583488924382,
-            "velocityY": -0.05902272113767896,
-            "timestamp": 0.20543907365528408
-        },
-        {
-            "x": 2.357593597831257,
-            "y": 5.5547755319598515,
-            "heading": 1.1463891087836293,
-            "angularVelocity": 7.171596723173552,
-            "velocityX": 0.9357902494573596,
-            "velocityY": -0.14702531230571617,
-            "timestamp": 0.23967891926449808
-        },
-        {
-            "x": 2.398826095012215,
-            "y": 5.54977146589545,
-            "heading": 1.3795454342192353,
-            "angularVelocity": 6.809502825937482,
-            "velocityX": 1.2042255578939407,
-            "velocityY": -0.14614744825411982,
-            "timestamp": 0.2739187648737121
-        },
-        {
-            "x": 2.4467424620473803,
-            "y": 5.551292722520974,
-            "heading": 1.6025344347532522,
-            "angularVelocity": 6.512558586829906,
-            "velocityX": 1.399432917485785,
-            "velocityY": 0.04442942421196648,
-            "timestamp": 0.3081586104829261
-        },
-        {
-            "x": 2.499282041125752,
-            "y": 5.560003512593039,
-            "heading": 1.8091707816342142,
-            "angularVelocity": 6.034967249541443,
-            "velocityX": 1.5344572425359657,
-            "velocityY": 0.2544050627880495,
-            "timestamp": 0.3423984560921401
-        },
-        {
-            "x": 2.558927262082263,
-            "y": 5.571143049666431,
-            "heading": 1.9899644077603311,
-            "angularVelocity": 5.2802114877955235,
-            "velocityX": 1.7419827658469296,
-            "velocityY": 0.3253384142127747,
-            "timestamp": 0.3766383017013541
-        },
-        {
-            "x": 2.624771333399717,
-            "y": 5.5772713811756995,
-            "heading": 2.148730759711285,
-            "angularVelocity": 4.63688866366352,
-            "velocityX": 1.9230247726273466,
-            "velocityY": 0.17898245159199333,
-            "timestamp": 0.4108781473105681
-        },
-        {
-            "x": 2.6962684360037885,
-            "y": 5.576686266483743,
-            "heading": 2.2908744479705683,
-            "angularVelocity": 4.151411483614645,
-            "velocityX": 2.0881257298902987,
-            "velocityY": -0.0170887070763856,
-            "timestamp": 0.4451179929197821
-        },
-        {
-            "x": 2.773687793639176,
-            "y": 5.569334398339137,
-            "heading": 2.4184902740666434,
-            "angularVelocity": 3.727114530613817,
-            "velocityX": 2.2610895656186547,
-            "velocityY": -0.2147167434256298,
-            "timestamp": 0.4793578385289961
-        },
-        {
-            "x": 2.8582271385194487,
-            "y": 5.556743437974915,
-            "heading": 2.5300657438997165,
-            "angularVelocity": 3.258644069441941,
-            "velocityX": 2.4690340559690878,
-            "velocityY": -0.36772830426646747,
-            "timestamp": 0.5135976841382102
-        },
-        {
-            "x": 2.9508927177061977,
-            "y": 5.541492370817128,
-            "heading": 2.6233336392816216,
-            "angularVelocity": 2.7239578252305745,
-            "velocityX": 2.706366735538439,
-            "velocityY": -0.445418689437171,
-            "timestamp": 0.5478375297474242
-        },
-        {
-            "x": 3.0518533392296137,
-            "y": 5.525810656425357,
-            "heading": 2.697205719709221,
-            "angularVelocity": 2.1574887127329996,
-            "velocityX": 2.948629578406965,
-            "velocityY": -0.45799606022611716,
-            "timestamp": 0.5820773753566382
-        },
-        {
-            "x": 3.1608544462331407,
-            "y": 5.511189976920693,
-            "heading": 2.751249379299885,
-            "angularVelocity": 1.5783850256649679,
-            "velocityX": 3.183457900119567,
-            "velocityY": -0.42700775206563796,
-            "timestamp": 0.6163172209658522
-        },
-        {
-            "x": 3.2775207255393237,
-            "y": 5.498694402974223,
-            "heading": 2.785399702937351,
-            "angularVelocity": 0.9973854446433661,
-            "velocityX": 3.40732492306531,
-            "velocityY": -0.36494247342945346,
-            "timestamp": 0.6505570665750662
-        },
-        {
-            "x": 3.401135401206032,
-            "y": 5.488837187261409,
-            "heading": 2.800896234569334,
-            "angularVelocity": 0.4525876608454414,
-            "velocityX": 3.610257974803583,
-            "velocityY": -0.28788727102673717,
-            "timestamp": 0.6847969121842802
-        },
-        {
-            "x": 3.5179017921083777,
-            "y": 5.474779629986763,
-            "heading": 2.835487088185811,
-            "angularVelocity": 1.0102514483058458,
-            "velocityX": 3.410248756230483,
-            "velocityY": -0.4105613510962388,
-            "timestamp": 0.7190367577934942
-        },
-        {
-            "x": 3.6287098119916026,
-            "y": 5.4546983370085105,
-            "heading": 2.8876881760682602,
-            "angularVelocity": 1.5245713569573334,
-            "velocityX": 3.2362301263825226,
-            "velocityY": -0.5864890048700768,
-            "timestamp": 0.7532766034027082
-        },
-        {
-            "x": 3.7356372636451365,
-            "y": 5.425785607175325,
-            "heading": 2.9455736884009496,
-            "angularVelocity": 1.6905891747687094,
-            "velocityX": 3.1228952628442794,
-            "velocityY": -0.8444176461299453,
-            "timestamp": 0.7875164490119222
-        },
-        {
-            "x": 3.8388362586877394,
-            "y": 5.387864157402447,
-            "heading": 3.0009390226743027,
-            "angularVelocity": 1.6169855117119507,
-            "velocityX": 3.0140029315678847,
-            "velocityY": -1.1075239709221774,
-            "timestamp": 0.8217562946211362
-        },
-        {
-            "x": 3.9379980862804507,
-            "y": 5.341070528950818,
-            "heading": 3.053673275383994,
-            "angularVelocity": 1.5401428298350917,
-            "velocityX": 2.896094472050628,
-            "velocityY": -1.3666425072616961,
-            "timestamp": 0.8559961402303502
-        },
-        {
-            "x": 4.032804489135742,
-            "y": 5.285560607910156,
-            "heading": 3.1036105469083837,
-            "angularVelocity": 1.4584549268806075,
-            "velocityX": 2.76889107320562,
-            "velocityY": -1.62120827512502,
-            "timestamp": 0.8902359858395642
-        },
-        {
-            "x": 4.173725003261308,
-            "y": 5.173637687034127,
-            "heading": 3.175484729095798,
-            "angularVelocity": 1.2994086167535415,
-            "velocityX": 2.547692714675012,
-            "velocityY": -2.023447131813128,
-            "timestamp": 0.9455489807364162
-        },
-        {
-            "x": 4.301872286760783,
-            "y": 5.039921697201636,
-            "heading": 3.2346580028937466,
-            "angularVelocity": 1.0697897285853943,
-            "velocityX": 2.3167663175433564,
-            "velocityY": -2.4174425934058767,
-            "timestamp": 1.0008619756332682
-        },
-        {
-            "x": 4.416442024233276,
-            "y": 4.885133369968262,
-            "heading": 3.2770092965554323,
-            "angularVelocity": 0.765666255111711,
-            "velocityX": 2.0712987551323803,
-            "velocityY": -2.7984079965661373,
-            "timestamp": 1.0561749705301202
-        },
-        {
-            "x": 4.517908742039174,
-            "y": 4.711097539386313,
-            "heading": 3.282986865137276,
-            "angularVelocity": 0.10806806959180061,
-            "velocityX": 1.834410123608644,
-            "velocityY": -3.146382344808711,
-            "timestamp": 1.1114879654269723
-        },
-        {
-            "x": 4.627451956204509,
-            "y": 4.5560902871099245,
-            "heading": 3.2338916761611958,
-            "angularVelocity": -0.8875886953442504,
-            "velocityX": 1.9804245705663106,
-            "velocityY": -2.802365928032756,
-            "timestamp": 1.1668009603238243
-        },
-        {
-            "x": 4.751713919325069,
-            "y": 4.421499213962681,
-            "heading": 3.169974564154514,
-            "angularVelocity": -1.1555532678328913,
-            "velocityX": 2.246523865726009,
-            "velocityY": -2.433263167149585,
-            "timestamp": 1.2221139552206763
-        },
-        {
-            "x": 4.891049798654671,
-            "y": 4.307366204928411,
-            "heading": 3.098048256171184,
-            "angularVelocity": -1.3003509955925967,
-            "velocityX": 2.5190442063286667,
-            "velocityY": -2.063403170396147,
-            "timestamp": 1.2774269501175284
-        },
-        {
-            "x": 5.0446094980944505,
-            "y": 4.214363163694223,
-            "heading": 3.021068837736617,
-            "angularVelocity": -1.3917058473893738,
-            "velocityX": 2.7761957154216415,
-            "velocityY": -1.6813958710357464,
-            "timestamp": 1.3327399450143804
-        },
-        {
-            "x": 5.211468562298102,
-            "y": 4.143120621239898,
-            "heading": 2.9418200950482523,
-            "angularVelocity": -1.432732811451417,
-            "velocityX": 3.016634057056783,
-            "velocityY": -1.2879892435254758,
-            "timestamp": 1.3880529399112325
-        },
-        {
-            "x": 5.390789031982422,
-            "y": 4.09412145614624,
-            "heading": 2.865640793449113,
-            "angularVelocity": -1.3772405876991196,
-            "velocityX": 3.2419229878750393,
-            "velocityY": -0.8858526858838836,
-            "timestamp": 1.4433659348080845
-        },
-        {
-            "x": 5.535592658236132,
-            "y": 4.049337172847784,
-            "heading": 2.841642220937184,
-            "angularVelocity": -0.5722130304709164,
-            "velocityX": 3.452643767066762,
-            "velocityY": -1.0678197818199107,
-            "timestamp": 1.4853058614332775
-        },
-        {
-            "x": 5.681734895850587,
-            "y": 4.003264514213434,
-            "heading": 2.8233979117656802,
-            "angularVelocity": -0.4350105171749157,
-            "velocityX": 3.484561118108144,
-            "velocityY": -1.098539323783022,
-            "timestamp": 1.5272457880584704
-        },
-        {
-            "x": 5.827742551539107,
-            "y": 3.9572785916007636,
-            "heading": 2.804954082691072,
-            "angularVelocity": -0.4397677954812897,
-            "velocityX": 3.481352196758831,
-            "velocityY": -1.096471222366128,
-            "timestamp": 1.5691857146836634
-        },
-        {
-            "x": 5.9679573209760495,
-            "y": 3.908501280249766,
-            "heading": 2.7717324952229117,
-            "angularVelocity": -0.7921231661908814,
-            "velocityX": 3.3432287731451744,
-            "velocityY": -1.1630280564605782,
-            "timestamp": 1.6111256413088564
-        },
-        {
-            "x": 6.096940014064459,
-            "y": 3.8543767396001813,
-            "heading": 2.711734513417447,
-            "angularVelocity": -1.4305695463335533,
-            "velocityX": 3.07541532537948,
-            "velocityY": -1.2905254015650245,
-            "timestamp": 1.6530655679340494
-        },
-        {
-            "x": 6.215088187160598,
-            "y": 3.7918967611554555,
-            "heading": 2.632459037184743,
-            "angularVelocity": -1.890214948184595,
-            "velocityX": 2.8170810633981405,
-            "velocityY": -1.4897493503766595,
-            "timestamp": 1.6950054945592423
-        },
-        {
-            "x": 6.32424952389229,
-            "y": 3.717983083464812,
-            "heading": 2.5463998013121922,
-            "angularVelocity": -2.0519643880553406,
-            "velocityX": 2.6028022821127137,
-            "velocityY": -1.7623702194615716,
-            "timestamp": 1.7369454211844353
-        },
-        {
-            "x": 6.43630340548619,
-            "y": 3.6322698094657313,
-            "heading": 2.4886072976482807,
-            "angularVelocity": -1.3779829464268867,
-            "velocityX": 2.6717710451737124,
-            "velocityY": -2.043715401914737,
-            "timestamp": 1.7788853478096283
-        },
-        {
-            "x": 6.552059359988985,
-            "y": 3.5378523447078845,
-            "heading": 2.4573864362362134,
-            "angularVelocity": -0.7444185987992016,
-            "velocityX": 2.7600418936656252,
-            "velocityY": -2.251254886581771,
-            "timestamp": 1.8208252744348212
-        },
-        {
-            "x": 6.67032629558411,
-            "y": 3.432112823682954,
-            "heading": 2.4573457680499984,
-            "angularVelocity": -0.0009696770950155036,
-            "velocityX": 2.8199127922194083,
-            "velocityY": -2.521213782033988,
-            "timestamp": 1.8627652010600142
-        },
-        {
-            "x": 6.787202921818474,
-            "y": 3.3248215481814336,
-            "heading": 2.4573432508654287,
-            "angularVelocity": -0.00006001881195361165,
-            "velocityX": 2.7867627733081557,
-            "velocityY": -2.558213238195593,
-            "timestamp": 1.9047051276852072
-        },
-        {
-            "x": 6.904079764497574,
-            "y": 3.217530508972575,
-            "heading": 2.4573407325391345,
-            "angularVelocity": -0.000060046034804859665,
-            "velocityX": 2.7867679341358973,
-            "velocityY": -2.5582076041213115,
-            "timestamp": 1.9466450543104001
-        },
-        {
-            "x": 7.017539426879104,
-            "y": 3.1193532157248125,
-            "heading": 2.428286840001792,
-            "angularVelocity": -0.6927501995172902,
-            "velocityX": 2.7052899590285975,
-            "velocityY": -2.3409028376503684,
-            "timestamp": 1.988584980935593
-        },
-        {
-            "x": 7.118887938132189,
-            "y": 3.020968055873212,
-            "heading": 2.3710302150679574,
-            "angularVelocity": -1.365205653446252,
-            "velocityX": 2.4165161794108876,
-            "velocityY": -2.345859131582302,
-            "timestamp": 2.030524907560786
-        },
-        {
-            "x": 7.207551237715759,
-            "y": 2.9223074295194613,
-            "heading": 2.2882909026899103,
-            "angularVelocity": -1.9728053679604207,
-            "velocityX": 2.1140547139229047,
-            "velocityY": -2.3524272523282552,
-            "timestamp": 2.0724648341859786
-        },
-        {
-            "x": 7.28673413242529,
-            "y": 2.8318719270188626,
-            "heading": 2.1761461762136682,
-            "angularVelocity": -2.6739371167349164,
-            "velocityX": 1.8880074688058086,
-            "velocityY": -2.1563104606452623,
-            "timestamp": 2.1144047608111713
-        },
-        {
-            "x": 7.3579924776067,
-            "y": 2.7517426977667294,
-            "heading": 2.0378729016459984,
-            "angularVelocity": -3.2969364921256217,
-            "velocityX": 1.6990574594521515,
-            "velocityY": -1.9105715174046114,
-            "timestamp": 2.156344687436364
-        },
-        {
-            "x": 7.4224137082831785,
-            "y": 2.6828558926645387,
-            "heading": 1.874977709266495,
-            "angularVelocity": -3.884012335912244,
-            "velocityX": 1.5360358460374843,
-            "velocityY": -1.6425113405136766,
-            "timestamp": 2.198284614061557
-        },
-        {
-            "x": 7.480883603746462,
-            "y": 2.6255140291502124,
-            "heading": 1.6865689006450588,
-            "angularVelocity": -4.4923495051672475,
-            "velocityX": 1.3941344243592637,
-            "velocityY": -1.3672380504328716,
-            "timestamp": 2.2402245406867496
-        },
-        {
-            "x": 7.534437188323511,
-            "y": 2.579534136686106,
-            "heading": 1.4695142507662173,
-            "angularVelocity": -5.175370281846384,
-            "velocityX": 1.27691173748694,
-            "velocityY": -1.0963274417482318,
-            "timestamp": 2.2821644673119423
-        },
-        {
-            "x": 7.579534895925645,
-            "y": 2.5413039154996566,
-            "heading": 1.2172257697546927,
-            "angularVelocity": -6.015472636997345,
-            "velocityX": 1.0752929542572058,
-            "velocityY": -0.9115471643072651,
-            "timestamp": 2.324104393937135
-        },
-        {
-            "x": 7.615712715030757,
-            "y": 2.5097826728946813,
-            "heading": 0.9253708991340255,
-            "angularVelocity": -6.958878903840344,
-            "velocityX": 0.8626104530039771,
-            "velocityY": -0.7515807761580874,
-            "timestamp": 2.366044320562328
-        },
-        {
-            "x": 7.6401514150011085,
-            "y": 2.4865499215178333,
-            "heading": 0.6440959436378817,
-            "angularVelocity": -6.706615345559152,
-            "velocityX": 0.5827072657697844,
-            "velocityY": -0.5539530763721487,
-            "timestamp": 2.4079842471875206
-        },
-        {
-            "x": 7.656070937434432,
-            "y": 2.471863188851966,
-            "heading": 0.3945229348796906,
-            "angularVelocity": -5.950725927314207,
-            "velocityX": 0.37957916749813,
-            "velocityY": -0.3501849871393242,
-            "timestamp": 2.4499241738127133
-        },
-        {
-            "x": 7.668072859318959,
-            "y": 2.4642781587251137,
-            "heading": 0.19785094472919199,
-            "angularVelocity": -4.689373729909185,
-            "velocityX": 0.28616935818189254,
-            "velocityY": -0.18085463512222474,
-            "timestamp": 2.491864100437906
-        },
-        {
-            "x": 7.676136280306529,
-            "y": 2.459442710773055,
-            "heading": 0.06623696286278732,
-            "angularVelocity": -3.1381547956105695,
-            "velocityX": 0.19226120874341576,
-            "velocityY": -0.11529462116784107,
-            "timestamp": 2.533804027063099
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 2.1057025676027217e-33,
-            "angularVelocity": -1.57932948845455,
-            "velocityX": 0.09212509425682942,
-            "velocityY": -0.058243086471875524,
-            "timestamp": 2.5757439536882916
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": -7.524501141381425e-34,
-            "angularVelocity": -5.907340858361994e-32,
-            "velocityX": -7.634877430876997e-33,
-            "velocityY": -7.90956322394541e-33,
-            "timestamp": 2.6176838803134843
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W2 To C5.traj b/src/main/deploy/choreo/W2 To C5.traj
deleted file mode 100644
index cfa6b046..00000000
--- a/src/main/deploy/choreo/W2 To C5.traj	
+++ /dev/null
@@ -1,625 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 6.892560738534845e-27,
-            "timestamp": 0
-        },
-        {
-            "x": 2.24821488195175,
-            "y": 5.550858605907564,
-            "heading": -1.0860081979744884e-8,
-            "angularVelocity": -3.249360952601713e-7,
-            "velocityX": -0.053411135206172675,
-            "velocityY": -0.27351257600550494,
-            "timestamp": 0.0334222075852833
-        },
-        {
-            "x": 2.2446446457975586,
-            "y": 5.532575817984525,
-            "heading": -3.253781151640391e-8,
-            "angularVelocity": -6.486025430910346e-7,
-            "velocityX": -0.10682227213842765,
-            "velocityY": -0.5470251441769277,
-            "timestamp": 0.0668444151705666
-        },
-        {
-            "x": 2.2392892913368425,
-            "y": 5.505151636566201,
-            "heading": -6.492851000422694e-8,
-            "angularVelocity": -9.691369834959166e-7,
-            "velocityX": -0.1602334150722015,
-            "velocityY": -0.8205377023156053,
-            "timestamp": 0.10026662275584991
-        },
-        {
-            "x": 2.2321488181807467,
-            "y": 5.468586062083933,
-            "heading": -1.0785000725838172e-7,
-            "angularVelocity": -0.000001284220850944888,
-            "velocityX": -0.21364456964054193,
-            "velocityY": -1.0940502475484135,
-            "timestamp": 0.1336888303411332
-        },
-        {
-            "x": 2.2232232256851456,
-            "y": 5.42287909509794,
-            "heading": -1.6102300741445552e-7,
-            "angularVelocity": -0.0000015909481231905412,
-            "velocityX": -0.26705574348127187,
-            "velocityY": -1.3675627760193463,
-            "timestamp": 0.16711103792641652
-        },
-        {
-            "x": 2.212512512848178,
-            "y": 5.368030736346982,
-            "heading": -2.2404505707142586e-7,
-            "angularVelocity": -0.0000018856338989468898,
-            "velocityX": -0.32046694729784664,
-            "velocityY": -1.6410752823864576,
-            "timestamp": 0.20053324551169982
-        },
-        {
-            "x": 2.200016678146418,
-            "y": 5.304040986826963,
-            "heading": -2.963548114247805e-7,
-            "angularVelocity": -0.000002163524111560841,
-            "velocityX": -0.3738781966956197,
-            "velocityY": -1.9145877589558649,
-            "timestamp": 0.23395545309698312
-        },
-        {
-            "x": 2.1857357192573095,
-            "y": 5.230909847922857,
-            "heading": -3.7718073304542066e-7,
-            "angularVelocity": -0.0000024183297502514874,
-            "velocityX": -0.4272895155847271,
-            "velocityY": -2.1881001940865983,
-            "timestamp": 0.2673776606822664
-        },
-        {
-            "x": 2.169669632551276,
-            "y": 5.148637321647842,
-            "heading": -4.654625982128869e-7,
-            "angularVelocity": -0.0000026414132559778984,
-            "velocityX": -0.48070094307146977,
-            "velocityY": -2.4616125689825754,
-            "timestamp": 0.30079986826754973
-        },
-        {
-            "x": 2.15181841205741,
-            "y": 5.057223411125392,
-            "heading": -5.597189955047307e-7,
-            "angularVelocity": -0.0000028201725975039026,
-            "velocityX": -0.5341125492089794,
-            "velocityY": -2.735124850423542,
-            "timestamp": 0.33422207585283303
-        },
-        {
-            "x": 2.1321820469941515,
-            "y": 4.956668121725741,
-            "heading": -6.57784320236276e-7,
-            "angularVelocity": -0.0000029341365576412115,
-            "velocityX": -0.5875244779329194,
-            "velocityY": -3.008636971185932,
-            "timestamp": 0.36764428343811634
-        },
-        {
-            "x": 2.1107605140888954,
-            "y": 4.846971464532025,
-            "heading": -7.56114109936986e-7,
-            "angularVelocity": -0.0000029420493990798148,
-            "velocityX": -0.6409371029779476,
-            "velocityY": -3.282148760338516,
-            "timestamp": 0.40106649102339964
-        },
-        {
-            "x": 2.087553735835974,
-            "y": 4.728133474365802,
-            "heading": -8.465523402036467e-7,
-            "angularVelocity": -0.0000027059321418483588,
-            "velocityX": -0.6943520470083785,
-            "velocityY": -3.5556595076225634,
-            "timestamp": 0.43448869860868294
-        },
-        {
-            "x": 2.0633251233427066,
-            "y": 4.604042454619442,
-            "heading": -8.465523885072994e-7,
-            "angularVelocity": -1.4452562228577176e-12,
-            "velocityX": -0.724925558303899,
-            "velocityY": -3.712831339152857,
-            "timestamp": 0.46791090619396625
-        },
-        {
-            "x": 2.039088654512122,
-            "y": 4.479952969280304,
-            "heading": -8.465523654496814e-7,
-            "angularVelocity": 6.898891300327124e-13,
-            "velocityX": -0.725160621670614,
-            "velocityY": -3.7127854293461913,
-            "timestamp": 0.5013331137792496
-        },
-        {
-            "x": 2.0148473415026222,
-            "y": 4.355864430051467,
-            "heading": -8.465523174606992e-7,
-            "angularVelocity": 1.4358411666699279e-12,
-            "velocityX": -0.7253055606758912,
-            "velocityY": -3.712757121497041,
-            "timestamp": 0.5347553213645329
-        },
-        {
-            "x": 1.9907968545860597,
-            "y": 4.231738764257757,
-            "heading": -8.465522534990754e-7,
-            "angularVelocity": 1.9137462023851052e-12,
-            "velocityX": -0.7195960008785613,
-            "velocityY": -3.7138679567028667,
-            "timestamp": 0.5681775289498161
-        },
-        {
-            "x": 1.9669031646192998,
-            "y": 4.107582820499775,
-            "heading": -8.465521766194915e-7,
-            "angularVelocity": 2.3002544763334585e-12,
-            "velocityX": -0.7149046006511871,
-            "velocityY": -3.7147738802470687,
-            "timestamp": 0.6015997365350993
-        },
-        {
-            "x": 1.9504985155072192,
-            "y": 3.982217377752346,
-            "heading": -8.465520941748615e-7,
-            "angularVelocity": 2.4667619198655976e-12,
-            "velocityX": -0.4908308067350749,
-            "velocityY": -3.7509623631996885,
-            "timestamp": 0.6350219441203826
-        },
-        {
-            "x": 1.94330794196602,
-            "y": 3.855987816020002,
-            "heading": -8.465520093116816e-7,
-            "angularVelocity": 2.5391254773053977e-12,
-            "velocityX": -0.2151435844765948,
-            "velocityY": -3.7768169984060798,
-            "timestamp": 0.6684441517056658
-        },
-        {
-            "x": 1.9454293697222904,
-            "y": 3.7295714172218672,
-            "heading": -8.465519250884413e-7,
-            "angularVelocity": 2.519978339764665e-12,
-            "velocityX": 0.0634735976376205,
-            "velocityY": -3.7824072056149025,
-            "timestamp": 0.7018663592909491
-        },
-        {
-            "x": 1.9568513812515056,
-            "y": 3.603654207925799,
-            "heading": -8.465518451486746e-7,
-            "angularVelocity": 2.391815832767542e-12,
-            "velocityX": 0.34174916483985474,
-            "velocityY": -3.767471342961152,
-            "timestamp": 0.7352885668762323
-        },
-        {
-            "x": 1.9775120291721782,
-            "y": 3.4789195174340257,
-            "heading": -8.465517739710846e-7,
-            "angularVelocity": 2.129649524174011e-12,
-            "velocityX": 0.6181712523959566,
-            "velocityY": -3.7320901132416737,
-            "timestamp": 0.7687107744615156
-        },
-        {
-            "x": 2.0072992100178335,
-            "y": 3.3560442616931043,
-            "heading": -8.465517105809759e-7,
-            "angularVelocity": 1.8966463157464128e-12,
-            "velocityX": 0.8912391789143821,
-            "velocityY": -3.6764554055067418,
-            "timestamp": 0.8021329820467988
-        },
-        {
-            "x": 2.046051283468891,
-            "y": 3.235695267642527,
-            "heading": -8.465516527951994e-7,
-            "angularVelocity": 1.7289634182989629e-12,
-            "velocityX": 1.1594707905613406,
-            "velocityY": -3.600869085126494,
-            "timestamp": 0.8355551896320821
-        },
-        {
-            "x": 2.0935579534966013,
-            "y": 3.1185256537724477,
-            "heading": -8.46551596321545e-7,
-            "angularVelocity": 1.6897044173558387e-12,
-            "velocityX": 1.421410297528723,
-            "velocityY": -3.5057413120014775,
-            "timestamp": 0.8689773972173653
-        },
-        {
-            "x": 2.1495614051818848,
-            "y": 3.005171298980713,
-            "heading": -8.465515057383081e-7,
-            "angularVelocity": 2.7102707524537993e-12,
-            "velocityX": 1.6756359238847198,
-            "velocityY": -3.3915878986274275,
-            "timestamp": 0.9023996048026486
-        },
-        {
-            "x": 2.288743031901554,
-            "y": 2.8026748842621148,
-            "heading": -8.46551464739476e-7,
-            "angularVelocity": 6.312009526868921e-13,
-            "velocityX": 2.142782210187666,
-            "velocityY": -3.117550249366458,
-            "timestamp": 0.9673533019435205
-        },
-        {
-            "x": 2.455414708004788,
-            "y": 2.6221288200825,
-            "heading": -8.46551435217149e-7,
-            "angularVelocity": 4.5451345461171864e-13,
-            "velocityX": 2.566007532127087,
-            "velocityY": -2.779611817758129,
-            "timestamp": 1.0323069990843925
-        },
-        {
-            "x": 2.646160177401748,
-            "y": 2.4672337011827246,
-            "heading": -8.465514075797998e-7,
-            "angularVelocity": 4.254931139136813e-13,
-            "velocityX": 2.9366376017527633,
-            "velocityY": -2.384700574689416,
-            "timestamp": 1.0972606962252645
-        },
-        {
-            "x": 2.857069726290126,
-            "y": 2.341164292711998,
-            "heading": -8.465513787578765e-7,
-            "angularVelocity": 4.4373032078954734e-13,
-            "velocityX": 3.2470753501649696,
-            "velocityY": -1.9409119729950552,
-            "timestamp": 1.1622143933661364
-        },
-        {
-            "x": 3.0838173053505575,
-            "y": 2.2464971410417167,
-            "heading": -8.465513471236816e-7,
-            "angularVelocity": 4.870268741320482e-13,
-            "velocityX": 3.490911049579749,
-            "velocityY": -1.457455939200772,
-            "timestamp": 1.2271680905070084
-        },
-        {
-            "x": 3.3145112489857773,
-            "y": 2.1619002222864983,
-            "heading": -8.465513138187862e-7,
-            "angularVelocity": 5.127482864627404e-13,
-            "velocityX": 3.5516676307835056,
-            "velocityY": -1.3024188380298123,
-            "timestamp": 1.2921217876478803
-        },
-        {
-            "x": 3.5452013267301767,
-            "y": 2.0772927620920076,
-            "heading": -8.465512839726007e-7,
-            "angularVelocity": 4.594994306997602e-13,
-            "velocityX": 3.5516081131487938,
-            "velocityY": -1.302581129610464,
-            "timestamp": 1.3570754847887523
-        },
-        {
-            "x": 3.77589133724735,
-            "y": 1.9926851185956267,
-            "heading": -8.465512625373994e-7,
-            "angularVelocity": 3.3000743037279784e-13,
-            "velocityX": 3.5516070781466733,
-            "velocityY": -1.302583951649345,
-            "timestamp": 1.4220291819296242
-        },
-        {
-            "x": 4.006581361028944,
-            "y": 1.9080775112659911,
-            "heading": -8.465512535495126e-7,
-            "angularVelocity": 1.3837377280671698e-13,
-            "velocityX": 3.5516072823601217,
-            "velocityY": -1.302583394841325,
-            "timestamp": 1.4869828790704962
-        },
-        {
-            "x": 4.2372714040114055,
-            "y": 1.823469956289169,
-            "heading": -8.465512575529088e-7,
-            "angularVelocity": -6.163459506326445e-14,
-            "velocityX": 3.551607577968653,
-            "velocityY": -1.3025825888392473,
-            "timestamp": 1.5519365762113682
-        },
-        {
-            "x": 4.467961461705261,
-            "y": 1.7388624414245573,
-            "heading": -8.465512722492451e-7,
-            "angularVelocity": -2.2625863777166434e-13,
-            "velocityX": 3.5516078044591044,
-            "velocityY": -1.3025819712885556,
-            "timestamp": 1.6168902733522401
-        },
-        {
-            "x": 4.698651553926265,
-            "y": 1.6542550207002664,
-            "heading": -8.465512926518018e-7,
-            "angularVelocity": -3.141092229421582e-13,
-            "velocityX": 3.551608336024854,
-            "velocityY": -1.3025805219428788,
-            "timestamp": 1.681843970493112
-        },
-        {
-            "x": 4.929586887359619,
-            "y": 1.5703192949295044,
-            "heading": -8.465513064459796e-7,
-            "angularVelocity": -2.1236938120360531e-13,
-            "velocityX": 3.555383967330626,
-            "velocityY": -1.2922393869021438,
-            "timestamp": 1.746797667633984
-        },
-        {
-            "x": 5.059495672066219,
-            "y": 1.5327262216225606,
-            "heading": -8.465512750899748e-7,
-            "angularVelocity": 8.770995258558909e-13,
-            "velocityX": 3.633847296905686,
-            "velocityY": -1.051564666141291,
-            "timestamp": 1.7825473219513848
-        },
-        {
-            "x": 5.189548300230332,
-            "y": 1.4956338313822377,
-            "heading": -8.465512186056332e-7,
-            "angularVelocity": 1.579996862146135e-12,
-            "velocityX": 3.637870929029155,
-            "velocityY": -1.0375594099736098,
-            "timestamp": 1.8182969762687855
-        },
-        {
-            "x": 5.319623170079142,
-            "y": 1.4586195133360007,
-            "heading": -8.46551136487956e-7,
-            "angularVelocity": 2.2970201754243682e-12,
-            "velocityX": 3.638493080071459,
-            "velocityY": -1.0353755512600933,
-            "timestamp": 1.8540466305861862
-        },
-        {
-            "x": 5.4496989055100356,
-            "y": 1.42160823723968,
-            "heading": -8.465510387564672e-7,
-            "angularVelocity": 2.7337743725450157e-12,
-            "velocityX": 3.6385172923918394,
-            "velocityY": -1.0352904609291835,
-            "timestamp": 1.889796284903587
-        },
-        {
-            "x": 5.57977446957584,
-            "y": 1.3845963588884114,
-            "heading": -8.465509335637302e-7,
-            "angularVelocity": 2.9424826347181554e-12,
-            "velocityX": 3.6385124989164392,
-            "velocityY": -1.03530730738433,
-            "timestamp": 1.9255459392209877
-        },
-        {
-            "x": 5.709850057975644,
-            "y": 1.3475845660573411,
-            "heading": -8.465508277220688e-7,
-            "angularVelocity": 2.960634540295608e-12,
-            "velocityX": 3.6385131795943324,
-            "velocityY": -1.0353049151878095,
-            "timestamp": 1.9612955935383884
-        },
-        {
-            "x": 5.839925662966751,
-            "y": 1.3105728315354166,
-            "heading": -8.465507264753305e-7,
-            "angularVelocity": 2.832103981671134e-12,
-            "velocityX": 3.6385136436912036,
-            "velocityY": -1.0353032841469862,
-            "timestamp": 1.9970452478557892
-        },
-        {
-            "x": 5.970001275498046,
-            "y": 1.273561123513075,
-            "heading": -8.465506337302748e-7,
-            "angularVelocity": 2.594292378130792e-12,
-            "velocityX": 3.638513854607602,
-            "velocityY": -1.0353025428927405,
-            "timestamp": 2.03279490217319
-        },
-        {
-            "x": 6.100076894564937,
-            "y": 1.2365494384597497,
-            "heading": -8.46550552278632e-7,
-            "angularVelocity": 2.2783896618382965e-12,
-            "velocityX": 3.6385140374232323,
-            "velocityY": -1.0353019003965735,
-            "timestamp": 2.0685445564905907
-        },
-        {
-            "x": 6.230152520105601,
-            "y": 1.1995377761581834,
-            "heading": -8.465504825990077e-7,
-            "angularVelocity": 1.9490992420524453e-12,
-            "velocityX": 3.6385142185095396,
-            "velocityY": -1.03530126397757,
-            "timestamp": 2.1042942108079914
-        },
-        {
-            "x": 6.3602281516940815,
-            "y": 1.16252613511139,
-            "heading": -8.465504225140753e-7,
-            "angularVelocity": 1.680713660601537e-12,
-            "velocityX": 3.6385143876808734,
-            "velocityY": -1.0353006694327234,
-            "timestamp": 2.140043865125392
-        },
-        {
-            "x": 6.490303788775648,
-            "y": 1.1255145133698101,
-            "heading": -8.465503700121188e-7,
-            "angularVelocity": 1.4686003981542915e-12,
-            "velocityX": 3.638514541335137,
-            "velocityY": -1.0353001294215285,
-            "timestamp": 2.175793519442793
-        },
-        {
-            "x": 6.62037943067546,
-            "y": 1.0885029085617548,
-            "heading": -8.465503232940394e-7,
-            "angularVelocity": 1.306812060469801e-12,
-            "velocityX": 3.638514676112538,
-            "velocityY": -1.0352996557519205,
-            "timestamp": 2.2115431737601936
-        },
-        {
-            "x": 6.7504550766965234,
-            "y": 1.0514913182376744,
-            "heading": -8.465502808582129e-7,
-            "angularVelocity": 1.187027603302574e-12,
-            "velocityX": 3.638514791393393,
-            "velocityY": -1.0352992506018621,
-            "timestamp": 2.2472928280775943
-        },
-        {
-            "x": 6.880530727542945,
-            "y": 1.0144797448710672,
-            "heading": -8.465502433332782e-7,
-            "angularVelocity": 1.0496586699357012e-12,
-            "velocityX": 3.6385149263697616,
-            "velocityY": -1.0352987762623522,
-            "timestamp": 2.283042482394995
-        },
-        {
-            "x": 7.003525953233288,
-            "y": 0.9794828656529073,
-            "heading": -7.145353891597533e-7,
-            "angularVelocity": 0.000003692758900357643,
-            "velocityX": 3.440459160761053,
-            "velocityY": -0.9789431502593731,
-            "timestamp": 2.318792136712396
-        },
-        {
-            "x": 7.116271603762692,
-            "y": 0.9474023877264399,
-            "heading": -5.944969754078434e-7,
-            "angularVelocity": 0.0000033577503345576707,
-            "velocityX": 3.153754985388111,
-            "velocityY": -0.8973647029323183,
-            "timestamp": 2.3545417910297965
-        },
-        {
-            "x": 7.218767661219051,
-            "y": 0.9182383156679585,
-            "heading": -4.859087151281335e-7,
-            "angularVelocity": 0.0000030374632243726353,
-            "velocityX": 2.867050308972391,
-            "velocityY": -0.8157861275958178,
-            "timestamp": 2.3902914453471973
-        },
-        {
-            "x": 7.311014119553375,
-            "y": 0.8919906507276494,
-            "heading": -3.8850367492824467e-7,
-            "angularVelocity": 0.000002724642854703664,
-            "velocityX": 2.580345463352478,
-            "velocityY": -0.7342075172887301,
-            "timestamp": 2.426041099664598
-        },
-        {
-            "x": 7.393010975686544,
-            "y": 0.8686593933386082,
-            "heading": -3.0210953963079075e-7,
-            "angularVelocity": 0.0000024166425366030095,
-            "velocityX": 2.293640531602499,
-            "velocityY": -0.6526288948669626,
-            "timestamp": 2.4617907539819988
-        },
-        {
-            "x": 7.464758227730078,
-            "y": 0.8482445436165693,
-            "heading": -2.2661033918382436e-7,
-            "angularVelocity": 0.000002111886167406066,
-            "velocityX": 2.006935547027439,
-            "velocityY": -0.5710502692078399,
-            "timestamp": 2.4975404082993995
-        },
-        {
-            "x": 7.526255874392654,
-            "y": 0.8307461015250325,
-            "heading": -1.6193015142138781e-7,
-            "angularVelocity": 0.0000018092535156938384,
-            "velocityX": 1.7202305263310733,
-            "velocityY": -0.48947164456971237,
-            "timestamp": 2.5332900626168002
-        },
-        {
-            "x": 7.577503914725456,
-            "y": 0.8161640669449958,
-            "heading": -1.0802379214331377e-7,
-            "angularVelocity": 0.0000015078847699136293,
-            "velocityX": 1.4335254790941394,
-            "velocityY": -0.4078930232603403,
-            "timestamp": 2.569039716934201
-        },
-        {
-            "x": 7.618502347994633,
-            "y": 0.8044984397090525,
-            "heading": -6.487055743300418e-8,
-            "angularVelocity": 0.0000012070951602673883,
-            "velocityX": 1.1468204113297347,
-            "velocityY": -0.3263144066337203,
-            "timestamp": 2.6047893712516017
-        },
-        {
-            "x": 7.649251173610278,
-            "y": 0.7957492196197445,
-            "heading": -3.246962719134405e-8,
-            "angularVelocity": 9.063284906782187e-7,
-            "velocityX": 0.8601153270642448,
-            "velocityY": -0.24473579553046104,
-            "timestamp": 2.6405390255690024
-        },
-        {
-            "x": 7.669750391083677,
-            "y": 0.7899164064601075,
-            "heading": -1.0836483282929717e-8,
-            "angularVelocity": 6.051287577519143e-7,
-            "velocityX": 0.573410229128317,
-            "velocityY": -0.16315719049619,
-            "timestamp": 2.676288679886403
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 2.0494644799121527e-30,
-            "angularVelocity": 3.031213445055282e-7,
-            "velocityX": 0.28670511958863415,
-            "velocityY": -0.08157859189950141,
-            "timestamp": 2.712038334203804
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -1.7051999701665118e-29,
-            "angularVelocity": -2.5846596071248955e-28,
-            "velocityX": 4.524644517512267e-29,
-            "velocityY": 1.710870028377411e-29,
-            "timestamp": 2.7477879885212046
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W2 To S1.traj b/src/main/deploy/choreo/W2 To S1.traj
deleted file mode 100644
index 2aaf78f3..00000000
--- a/src/main/deploy/choreo/W2 To S1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 4.4337460853781175e-35,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3070751783284065,
-            "y": 5.580871519986164,
-            "heading": 0.00875997642576413,
-            "angularVelocity": 0.10256751513529143,
-            "velocityX": 0.6682733985254588,
-            "velocityY": 0.24437736336610263,
-            "timestamp": 0.08540692844327274
-        },
-        {
-            "x": 2.421225525991661,
-            "y": 5.622614560146346,
-            "heading": 0.026280802198455293,
-            "angularVelocity": 0.20514525100066663,
-            "velocityX": 1.3365466917484683,
-            "velocityY": 0.48875472893171557,
-            "timestamp": 0.1708138568865455
-        },
-        {
-            "x": 2.592451026856299,
-            "y": 5.685229121649564,
-            "heading": 0.052563939294491345,
-            "angularVelocity": 0.30774010463909035,
-            "velocityX": 2.0048197960703633,
-            "velocityY": 0.7331321081849562,
-            "timestamp": 0.25622078532981823
-        },
-        {
-            "x": 2.8207516562305845,
-            "y": 5.768715208168554,
-            "heading": 0.0876113178799828,
-            "angularVelocity": 0.41035755792072426,
-            "velocityX": 2.673092611285307,
-            "velocityY": 0.9775095304409794,
-            "timestamp": 0.341627713773091
-        },
-        {
-            "x": 3.106127374237044,
-            "y": 5.873072825511933,
-            "heading": 0.13142500230326418,
-            "angularVelocity": 0.5129991819385286,
-            "velocityX": 3.341364959588794,
-            "velocityY": 1.221887020708095,
-            "timestamp": 0.4270346422163637
-        },
-        {
-            "x": 3.40956412390744,
-            "y": 5.984036121763709,
-            "heading": 0.13142500754936084,
-            "angularVelocity": 6.142471997127927e-8,
-            "velocityX": 3.5528352933560723,
-            "velocityY": 1.299230615997127,
-            "timestamp": 0.5124415706596365
-        },
-        {
-            "x": 3.7130008735619784,
-            "y": 6.094999418058892,
-            "heading": 0.1314250127954367,
-            "angularVelocity": 6.142447634082945e-8,
-            "velocityX": 3.5528352931703995,
-            "velocityY": 1.2992306165053664,
-            "timestamp": 0.5978484991029092
-        },
-        {
-            "x": 4.016437623216516,
-            "y": 6.205962714354075,
-            "heading": 0.13142501804151255,
-            "angularVelocity": 6.142447606684555e-8,
-            "velocityX": 3.552835293170399,
-            "velocityY": 1.2992306165053664,
-            "timestamp": 0.683255427546182
-        },
-        {
-            "x": 4.3198743728836435,
-            "y": 6.316926010614789,
-            "heading": 0.13142502328760913,
-            "angularVelocity": 6.142471942956278e-8,
-            "velocityX": 3.5528352933178016,
-            "velocityY": 1.2992306161017806,
-            "timestamp": 0.7686623559894548
-        },
-        {
-            "x": 4.605249584503867,
-            "y": 6.421283973215489,
-            "heading": 0.17528171773503587,
-            "angularVelocity": 0.5135027713419801,
-            "velocityX": 3.3413590304886087,
-            "velocityY": 1.2218910632059061,
-            "timestamp": 0.8540692844327276
-        },
-        {
-            "x": 4.83354975240058,
-            "y": 6.5047703704067485,
-            "heading": 0.21036850612417926,
-            "angularVelocity": 0.4108189935954435,
-            "velocityX": 2.6730872080050085,
-            "velocityY": 0.9775131679944534,
-            "timestamp": 0.9394762128760004
-        },
-        {
-            "x": 5.004774875721697,
-            "y": 6.567385181417454,
-            "heading": 0.23668415165777779,
-            "angularVelocity": 0.3081207346202291,
-            "velocityX": 2.0048153755446823,
-            "velocityY": 0.7331350295812797,
-            "timestamp": 1.0248831413192732
-        },
-        {
-            "x": 5.118924957871047,
-            "y": 6.6091283935214395,
-            "heading": 0.25422806054578806,
-            "angularVelocity": 0.20541552316405948,
-            "velocityX": 1.3365435829385781,
-            "velocityY": 0.48875674216187737,
-            "timestamp": 1.110290069762546
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": 0.10270758607175808,
-            "velocityX": 0.6682718038134577,
-            "velocityY": 0.2443783760754632,
-            "timestamp": 1.1956969982058188
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": 1.86588452749739e-32,
-            "velocityX": -1.0770205854918045e-34,
-            "velocityY": 0,
-            "timestamp": 1.2811039266490916
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W2 To S2.traj b/src/main/deploy/choreo/W2 To S2.traj
deleted file mode 100644
index 96b0e72c..00000000
--- a/src/main/deploy/choreo/W2 To S2.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3319036557469652,
-            "y": 5.56,
-            "heading": -1.815106931159875e-19,
-            "angularVelocity": -1.831407645203353e-18,
-            "velocityX": 0.8263919813283321,
-            "velocityY": 8.319303153097966e-34,
-            "timestamp": 0.099109935233537
-        },
-        {
-            "x": 2.4957109641637625,
-            "y": 5.56,
-            "heading": -5.438547016393454e-19,
-            "angularVelocity": -3.655980680963076e-18,
-            "velocityX": 1.6527839316089838,
-            "velocityY": 1.3987786573815764e-33,
-            "timestamp": 0.198219870467074
-        },
-        {
-            "x": 2.741421919096125,
-            "y": 5.56,
-            "heading": -1.0860329369861945e-18,
-            "angularVelocity": -5.4704731072674304e-18,
-            "velocityX": 2.4791758197942864,
-            "velocityY": 2.272018181833636e-33,
-            "timestamp": 0.297329805700611
-        },
-        {
-            "x": 3.0690365020812695,
-            "y": 5.56,
-            "heading": -1.80338172905721e-18,
-            "angularVelocity": -7.237910009033217e-18,
-            "velocityX": 3.3055675216936846,
-            "velocityY": 3.0574601440031788e-33,
-            "timestamp": 0.396439740934148
-        },
-        {
-            "x": 3.4439634979187304,
-            "y": 5.56,
-            "heading": 1.7928981709293245e-18,
-            "angularVelocity": 3.6285765816640505e-17,
-            "velocityX": 3.7829405796099502,
-            "velocityY": 2.997011014948752e-33,
-            "timestamp": 0.495549676167685
-        },
-        {
-            "x": 3.771578080903875,
-            "y": 5.56,
-            "heading": 1.0808328664707184e-18,
-            "angularVelocity": -7.184600643588333e-18,
-            "velocityX": 3.305567521693685,
-            "velocityY": -4.3766441461200414e-33,
-            "timestamp": 0.594659611401222
-        },
-        {
-            "x": 4.017289035836238,
-            "y": 5.56,
-            "heading": 5.416591039281022e-19,
-            "angularVelocity": -5.4401585595028644e-18,
-            "velocityX": 2.479175819794286,
-            "velocityY": -5.119323199602909e-33,
-            "timestamp": 0.6937695466347591
-        },
-        {
-            "x": 4.181096344253034,
-            "y": 5.56,
-            "heading": 1.8086099379948042e-19,
-            "angularVelocity": -3.64038286599245e-18,
-            "velocityX": 1.6527839316089838,
-            "velocityY": -5.9637817525889115e-33,
-            "timestamp": 0.792879481868296
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": -1.8248523050897816e-18,
-            "velocityX": 0.8263919813283321,
-            "velocityY": -3.8230331267939286e-33,
-            "timestamp": 0.891989417101833
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 2.0684896902092895e-40,
-            "timestamp": 0.9910993523353699
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W2 To S3.traj b/src/main/deploy/choreo/W2 To S3.traj
deleted file mode 100644
index 7d72e43b..00000000
--- a/src/main/deploy/choreo/W2 To S3.traj	
+++ /dev/null
@@ -1,526 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -4.99624931676796e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2447003077480563,
-            "y": 5.548424158968356,
-            "heading": -0.001651505925702968,
-            "angularVelocity": -0.042250731032121805,
-            "velocityX": -0.13558284496887443,
-            "velocityY": -0.2961465280181771,
-            "timestamp": 0.03908822132445813
-        },
-        {
-            "x": 2.234100924566356,
-            "y": 5.525272476708423,
-            "heading": -0.0049545293965937804,
-            "angularVelocity": -0.08450175932731885,
-            "velocityX": -0.27116565611205723,
-            "velocityY": -0.5922930610671583,
-            "timestamp": 0.07817644264891627
-        },
-        {
-            "x": 2.2182018518980375,
-            "y": 5.490544953203296,
-            "heading": -0.009909100000268336,
-            "angularVelocity": -0.12675354456654908,
-            "velocityX": -0.4067484303351874,
-            "velocityY": -0.8884395945486457,
-            "timestamp": 0.1172646639733744
-        },
-        {
-            "x": 2.1970030913105703,
-            "y": 5.444241588640874,
-            "heading": -0.016515264878529855,
-            "angularVelocity": -0.16900653584180803,
-            "velocityX": -0.5423311644575062,
-            "velocityY": -1.1845861232230646,
-            "timestamp": 0.15635288529783253
-        },
-        {
-            "x": 2.170504644501127,
-            "y": 5.386362383452732,
-            "heading": -0.024773088013563174,
-            "angularVelocity": -0.21126116398317848,
-            "velocityX": -0.6779138551608025,
-            "velocityY": -1.480732640856411,
-            "timestamp": 0.19544110662229067
-        },
-        {
-            "x": 2.1387065133083243,
-            "y": 5.31690733837683,
-            "heading": -0.03468264893636917,
-            "angularVelocity": -0.2535178267784212,
-            "velocityX": -0.8134964988264785,
-            "velocityY": -1.7768791396104282,
-            "timestamp": 0.2345293279467488
-        },
-        {
-            "x": 2.1016086997403236,
-            "y": 5.235876454566401,
-            "heading": -0.04624404029676127,
-            "angularVelocity": -0.2957768598481422,
-            "velocityX": -0.9490790911170489,
-            "velocityY": -2.0730256088611454,
-            "timestamp": 0.27361754927120696
-        },
-        {
-            "x": 2.0592112060450187,
-            "y": 5.143269733801358,
-            "heading": -0.05945736298631538,
-            "angularVelocity": -0.33803847404262444,
-            "velocityX": -1.0846616258994186,
-            "velocityY": -2.369172032575987,
-            "timestamp": 0.31270577059566507
-        },
-        {
-            "x": 2.011514034901109,
-            "y": 5.039087178972333,
-            "heading": -0.07432271508161363,
-            "angularVelocity": -0.3803025973454465,
-            "velocityX": -1.2202440921521842,
-            "velocityY": -2.6653183823394446,
-            "timestamp": 0.3517939919201232
-        },
-        {
-            "x": 1.9585171900495353,
-            "y": 4.923328795522167,
-            "heading": -0.09084016031123929,
-            "angularVelocity": -0.4225683510264515,
-            "velocityX": -1.3558264626999135,
-            "velocityY": -2.9614645928578263,
-            "timestamp": 0.3908822132445813
-        },
-        {
-            "x": 1.9002206796290688,
-            "y": 4.795994598697166,
-            "heading": -0.10900958241827326,
-            "angularVelocity": -0.46483113048874997,
-            "velocityX": -1.491408625032185,
-            "velocityY": -3.257610413327559,
-            "timestamp": 0.4299704345690394
-        },
-        {
-            "x": 1.8386671955063139,
-            "y": 4.661546724325594,
-            "heading": -0.10900958703871873,
-            "angularVelocity": -1.182055697467884e-7,
-            "velocityX": -1.5747322860207698,
-            "velocityY": -3.439600724104782,
-            "timestamp": 0.4690586558934975
-        },
-        {
-            "x": 1.7777943297173011,
-            "y": 4.526789325478927,
-            "heading": -0.10900959166091434,
-            "angularVelocity": -1.1825034389504455e-7,
-            "velocityX": -1.5573199221351706,
-            "velocityY": -3.447519336526256,
-            "timestamp": 0.5081468772179556
-        },
-        {
-            "x": 1.728739704549767,
-            "y": 4.38729485969642,
-            "heading": -0.10900959636164997,
-            "angularVelocity": -1.2025964513922604e-7,
-            "velocityX": -1.2549720479816096,
-            "velocityY": -3.5687084511883413,
-            "timestamp": 0.5472350985424137
-        },
-        {
-            "x": 1.6918738865194354,
-            "y": 4.244095790476986,
-            "heading": -0.10900960128999353,
-            "angularVelocity": -1.2608257379035844e-7,
-            "velocityX": -0.9431439134643449,
-            "velocityY": -3.6634838927766116,
-            "timestamp": 0.5863233198668718
-        },
-        {
-            "x": 1.6674707596733624,
-            "y": 4.098254956364247,
-            "heading": -0.10900960662650709,
-            "angularVelocity": -1.3652485001388814e-7,
-            "velocityX": -0.6243089610931135,
-            "velocityY": -3.7310685718382324,
-            "timestamp": 0.6254115411913299
-        },
-        {
-            "x": 1.6557115284805652,
-            "y": 3.9508548842430677,
-            "heading": -0.1090096126146074,
-            "angularVelocity": -1.531944945007552e-7,
-            "velocityX": -0.3008382268193907,
-            "velocityY": -3.7709587985000237,
-            "timestamp": 0.664499762515788
-        },
-        {
-            "x": 1.6566835066189385,
-            "y": 3.8029896922639588,
-            "heading": -0.10900961961577929,
-            "angularVelocity": -1.79112061302596e-7,
-            "velocityX": 0.024866266754091815,
-            "velocityY": -3.7828580316236344,
-            "timestamp": 0.7035879838402461
-        },
-        {
-            "x": 1.6703794917330834,
-            "y": 3.6557569809887474,
-            "heading": -0.10900969829200789,
-            "angularVelocity": -0.000002012786100988185,
-            "velocityX": 0.3503865013576149,
-            "velocityY": -3.766677180142832,
-            "timestamp": 0.7426762051647042
-        },
-        {
-            "x": 1.6964297342683634,
-            "y": 3.5114772969924033,
-            "heading": -0.1124967438020359,
-            "angularVelocity": -0.0892096235606636,
-            "velocityX": 0.6664473760284892,
-            "velocityY": -3.6911294274233937,
-            "timestamp": 0.7817644264891623
-        },
-        {
-            "x": 1.7337062034817727,
-            "y": 3.3729422790966246,
-            "heading": -0.12243166775651844,
-            "angularVelocity": -0.2541666931326176,
-            "velocityX": 0.9536496660724848,
-            "velocityY": -3.54416274779685,
-            "timestamp": 0.8208526478136204
-        },
-        {
-            "x": 1.7809992442628195,
-            "y": 3.2421845220219927,
-            "heading": -0.1367067464310455,
-            "angularVelocity": -0.3652015413039525,
-            "velocityX": 1.2099051626962352,
-            "velocityY": -3.3451958836719715,
-            "timestamp": 0.8599408691380785
-        },
-        {
-            "x": 1.8374227958909133,
-            "y": 3.120270451905998,
-            "heading": -0.154016140064332,
-            "angularVelocity": -0.4428288892862246,
-            "velocityX": 1.443492431128254,
-            "velocityY": -3.118946475052685,
-            "timestamp": 0.8990290904625367
-        },
-        {
-            "x": 1.9023445230612375,
-            "y": 3.007828092475748,
-            "heading": -0.17349166731783877,
-            "angularVelocity": -0.4982454200677405,
-            "velocityX": 1.660902567846671,
-            "velocityY": -2.8766302384778886,
-            "timestamp": 0.9381173117869948
-        },
-        {
-            "x": 1.9753011558084586,
-            "y": 2.90526251520286,
-            "heading": -0.19452421295735298,
-            "angularVelocity": -0.5380788617862533,
-            "velocityX": 1.866460797529241,
-            "velocityY": -2.623950995915806,
-            "timestamp": 0.9772055331114529
-        },
-        {
-            "x": 2.0559421744907826,
-            "y": 2.8128534355639654,
-            "heading": -0.21666661660279088,
-            "angularVelocity": -0.5664725304751266,
-            "velocityX": 2.0630516291077963,
-            "velocityY": -2.3641157491367464,
-            "timestamp": 1.016293754435911
-        },
-        {
-            "x": 2.1439944855858766,
-            "y": 2.7308042438703497,
-            "heading": -0.2395781801271676,
-            "angularVelocity": -0.586150066389446,
-            "velocityX": 2.252655866948419,
-            "velocityY": -2.099077136627731,
-            "timestamp": 1.055381975760369
-        },
-        {
-            "x": 2.2392399311065674,
-            "y": 2.659268856048584,
-            "heading": -0.2629913490343549,
-            "angularVelocity": -0.5989827143282399,
-            "velocityX": 2.4366789353272003,
-            "velocityY": -1.830100869209989,
-            "timestamp": 1.0944701970848272
-        },
-        {
-            "x": 2.3166964235934535,
-            "y": 2.610513248595422,
-            "heading": -0.2811797949710667,
-            "angularVelocity": -0.6046182408202432,
-            "velocityX": 2.5747998696792043,
-            "velocityY": -1.620728330008343,
-            "timestamp": 1.124552726371468
-        },
-        {
-            "x": 2.398213254515867,
-            "y": 2.5681176642029904,
-            "heading": -0.29946946527559826,
-            "angularVelocity": -0.6079831296849904,
-            "velocityX": 2.7097731758410535,
-            "velocityY": -1.409309170398157,
-            "timestamp": 1.154635255658109
-        },
-        {
-            "x": 2.483678189488588,
-            "y": 2.5321524360945276,
-            "heading": -0.31778023201379,
-            "angularVelocity": -0.6086844149213316,
-            "velocityX": 2.8410155993982453,
-            "velocityY": -1.1955520018204469,
-            "timestamp": 1.1847177849447499
-        },
-        {
-            "x": 2.5729562458095865,
-            "y": 2.5026984837196036,
-            "heading": -0.33601688083426684,
-            "angularVelocity": -0.6062205955726221,
-            "velocityX": 2.9677709434035733,
-            "velocityY": -0.9791049181495252,
-            "timestamp": 1.2148003142313908
-        },
-        {
-            "x": 2.665882293188928,
-            "y": 2.479849558811985,
-            "heading": -0.35406456410202075,
-            "angularVelocity": -0.5999390242685017,
-            "velocityX": 3.08903704518594,
-            "velocityY": -0.7595413500609144,
-            "timestamp": 1.2448828435180317
-        },
-        {
-            "x": 2.7622502226261916,
-            "y": 2.4637149411085275,
-            "heading": -0.37178230702948656,
-            "angularVelocity": -0.5889711851900935,
-            "velocityX": 3.2034516951353296,
-            "velocityY": -0.5363451174508257,
-            "timestamp": 1.2749653728046726
-        },
-        {
-            "x": 2.8617965704136163,
-            "y": 2.4544224370162078,
-            "heading": -0.3889934776908366,
-            "angularVelocity": -0.57213176782302,
-            "velocityX": 3.309108314626244,
-            "velocityY": -0.30890035886875983,
-            "timestamp": 1.3050479020913135
-        },
-        {
-            "x": 2.96417486527032,
-            "y": 2.452120989561553,
-            "heading": -0.40547141981393114,
-            "angularVelocity": -0.5477578685651567,
-            "velocityX": 3.4032475754010525,
-            "velocityY": -0.07650445322366382,
-            "timestamp": 1.3351304313779544
-        },
-        {
-            "x": 3.068913958464018,
-            "y": 2.456980552046256,
-            "heading": -0.4209173641629785,
-            "angularVelocity": -0.5134523165215639,
-            "velocityX": 3.4817249638713506,
-            "velocityY": 0.16154102065001244,
-            "timestamp": 1.3652129606645953
-        },
-        {
-            "x": 3.175348297900835,
-            "y": 2.469181868617686,
-            "heading": -0.43492680962689906,
-            "angularVelocity": -0.4657003847794821,
-            "velocityX": 3.5380781457121575,
-            "velocityY": 0.4055947708108251,
-            "timestamp": 1.3952954899512362
-        },
-        {
-            "x": 3.2825012821379658,
-            "y": 2.488873812540701,
-            "heading": -0.44694408076253883,
-            "angularVelocity": -0.3994767534718848,
-            "velocityX": 3.5619672540204803,
-            "velocityY": 0.6545973490252659,
-            "timestamp": 1.425378019237877
-        },
-        {
-            "x": 3.3889723525651156,
-            "y": 2.516050236824646,
-            "heading": -0.4565561347315361,
-            "angularVelocity": -0.3195227993435668,
-            "velocityX": 3.539299153094488,
-            "velocityY": 0.9033955896790181,
-            "timestamp": 1.455460548524518
-        },
-        {
-            "x": 3.4932758480756925,
-            "y": 2.5504542246131905,
-            "heading": -0.4662703499641935,
-            "angularVelocity": -0.32291883239253294,
-            "velocityX": 3.4672448754798637,
-            "velocityY": 1.1436534295611822,
-            "timestamp": 1.485543077811159
-        },
-        {
-            "x": 3.5930108067726363,
-            "y": 2.5908608883384305,
-            "heading": -0.4766346317588353,
-            "angularVelocity": -0.3445282707409342,
-            "velocityX": 3.315378096922126,
-            "velocityY": 1.3431936969204072,
-            "timestamp": 1.5156256070977998
-        },
-        {
-            "x": 3.686096614652251,
-            "y": 2.634834387797071,
-            "heading": -0.48720467819938723,
-            "angularVelocity": -0.3513682755805826,
-            "velocityX": 3.094347785475507,
-            "velocityY": 1.4617620426674243,
-            "timestamp": 1.5457081363844407
-        },
-        {
-            "x": 3.771715868998783,
-            "y": 2.679896340499022,
-            "heading": -0.4975881116213702,
-            "angularVelocity": -0.34516490694787005,
-            "velocityX": 2.8461454663839936,
-            "velocityY": 1.497944280967274,
-            "timestamp": 1.5757906656710816
-        },
-        {
-            "x": 3.8498203373891897,
-            "y": 2.724287723719235,
-            "heading": -0.507530883254748,
-            "angularVelocity": -0.33051647814035945,
-            "velocityX": 2.596339810598824,
-            "velocityY": 1.475653286903962,
-            "timestamp": 1.6058731949577225
-        },
-        {
-            "x": 3.920599661686896,
-            "y": 2.7668759161025953,
-            "heading": -0.5168796397769617,
-            "angularVelocity": -0.3107702957131982,
-            "velocityX": 2.35283820796086,
-            "velocityY": 1.415711823216824,
-            "timestamp": 1.6359557242443634
-        },
-        {
-            "x": 3.984278053809844,
-            "y": 2.806919241781621,
-            "heading": -0.525538519539268,
-            "angularVelocity": -0.287837491315874,
-            "velocityX": 2.11678983227083,
-            "velocityY": 1.3311156551191636,
-            "timestamp": 1.6660382535310043
-        },
-        {
-            "x": 4.041059171114166,
-            "y": 2.843909698702473,
-            "heading": -0.5334440710807248,
-            "angularVelocity": -0.26279544070665484,
-            "velocityX": 1.8875114111345854,
-            "velocityY": 1.229632540814369,
-            "timestamp": 1.6961207828176452
-        },
-        {
-            "x": 4.091116341717457,
-            "y": 2.8774832028649056,
-            "heading": -0.5405519914461838,
-            "angularVelocity": -0.23628067632639732,
-            "velocityX": 1.6639947434713853,
-            "velocityY": 1.11604658778956,
-            "timestamp": 1.7262033121042861
-        },
-        {
-            "x": 4.1345948250930045,
-            "y": 2.9073683770064043,
-            "heading": -0.5468299348831553,
-            "angularVelocity": -0.20869067813911088,
-            "velocityX": 1.445306774615446,
-            "velocityY": 0.9934395428236782,
-            "timestamp": 1.756285841390927
-        },
-        {
-            "x": 4.171616484177909,
-            "y": 2.933356232054327,
-            "heading": -0.552253409487953,
-            "angularVelocity": -0.18028652288901045,
-            "velocityX": 1.2306697595852323,
-            "velocityY": 0.863885306993297,
-            "timestamp": 1.786368370677568
-        },
-        {
-            "x": 4.202284260430688,
-            "y": 2.9552814196152344,
-            "heading": -0.5568033106587852,
-            "angularVelocity": -0.15124729464987938,
-            "velocityX": 1.0194547127524736,
-            "velocityY": 0.7288345787681281,
-            "timestamp": 1.8164508999642088
-        },
-        {
-            "x": 4.226685878711894,
-            "y": 2.973010149818714,
-            "heading": -0.5604643670760648,
-            "angularVelocity": -0.12170041895060278,
-            "velocityX": 0.8111558057069341,
-            "velocityY": 0.5893364229633967,
-            "timestamp": 1.8465334292508497
-        },
-        {
-            "x": 4.244896778926994,
-            "y": 2.98643211128414,
-            "heading": -0.5632241207030344,
-            "angularVelocity": -0.09173941461743668,
-            "velocityX": 0.6053646633758383,
-            "velocityY": 0.4461713088529227,
-            "timestamp": 1.8766159585374906
-        },
-        {
-            "x": 4.256982406189367,
-            "y": 2.9954548894256625,
-            "heading": -0.565072233243941,
-            "angularVelocity": -0.06143474583855434,
-            "velocityX": 0.4017490400230914,
-            "velocityY": 0.2999341596429636,
-            "timestamp": 1.9066984878241315
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -0.030840716457660636,
-            "velocityX": 0.20003616561938206,
-            "velocityY": 0.15108804618886046,
-            "timestamp": 1.9367810171107724
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": -4.9732030554004765e-30,
-            "velocityX": -4.223945033236224e-32,
-            "velocityY": 9.969983495378875e-32,
-            "timestamp": 1.9668635463974133
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W2 To S4.traj b/src/main/deploy/choreo/W2 To S4.traj
deleted file mode 100644
index 848b8926..00000000
--- a/src/main/deploy/choreo/W2 To S4.traj	
+++ /dev/null
@@ -1,553 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 1.4317938010297452e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2473468901462774,
-            "y": 5.551784954334613,
-            "heading": -0.004046039178700474,
-            "angularVelocity": -0.12521248615625988,
-            "velocityX": -0.08210560159156222,
-            "velocityY": -0.2542304328304919,
-            "timestamp": 0.032313384255078216
-        },
-        {
-            "x": 2.2420392078855844,
-            "y": 5.535352460743813,
-            "heading": -0.012062241078351911,
-            "angularVelocity": -0.24807682898124367,
-            "velocityX": -0.16425646471427996,
-            "velocityY": -0.5085352082308542,
-            "timestamp": 0.06462676851015643
-        },
-        {
-            "x": 2.234075272195187,
-            "y": 5.510699784095841,
-            "heading": -0.023960373728225737,
-            "angularVelocity": -0.36821066329520014,
-            "velocityX": -0.24645935032774877,
-            "velocityY": -0.7629246275588465,
-            "timestamp": 0.09694015276523466
-        },
-        {
-            "x": 2.2234531308615284,
-            "y": 5.477823761292619,
-            "heading": -0.03963572188039383,
-            "angularVelocity": -0.4851038822931769,
-            "velocityX": -0.32872265095504705,
-            "velocityY": -1.017411935057708,
-            "timestamp": 0.12925353702031286
-        },
-        {
-            "x": 2.2101704924088987,
-            "y": 5.436720678146679,
-            "heading": -0.05896132607089341,
-            "angularVelocity": -0.5980680958065965,
-            "velocityX": -0.4110568657178767,
-            "velocityY": -1.27201418525146,
-            "timestamp": 0.16156692127539107
-        },
-        {
-            "x": 2.1942246366351648,
-            "y": 5.38738610923438,
-            "heading": -0.08177969135834087,
-            "angularVelocity": -0.7061583245915709,
-            "velocityX": -0.4934752623822669,
-            "velocityY": -1.5267533887152571,
-            "timestamp": 0.19388030553046928
-        },
-        {
-            "x": 2.1756122932404036,
-            "y": 5.329814710705053,
-            "heading": -0.10789037911156549,
-            "angularVelocity": -0.8080455933402793,
-            "velocityX": -0.5759948647853634,
-            "velocityY": -1.7816579679449487,
-            "timestamp": 0.2261936897855475
-        },
-        {
-            "x": 2.1543294701171383,
-            "y": 5.263999960188385,
-            "heading": -0.1370304714941401,
-            "angularVelocity": -0.9017963625397062,
-            "velocityX": -0.6586380106540685,
-            "velocityY": -2.0367643945039555,
-            "timestamp": 0.25850707404062573
-        },
-        {
-            "x": 2.1303711997350336,
-            "y": 5.1899338649176165,
-            "heading": -0.1688416775308267,
-            "angularVelocity": -0.9844591264590365,
-            "velocityX": -0.7414348863300811,
-            "velocityY": -2.2921181726463207,
-            "timestamp": 0.29082045829570397
-        },
-        {
-            "x": 2.1037311567595207,
-            "y": 5.107606785885568,
-            "heading": -0.20280965108227242,
-            "angularVelocity": -1.051204457054369,
-            "velocityX": -0.8244275116843016,
-            "velocityY": -2.5477702484570597,
-            "timestamp": 0.3231338425507822
-        },
-        {
-            "x": 2.074401135228672,
-            "y": 5.017008143765364,
-            "heading": -0.23813648207052604,
-            "angularVelocity": -1.0932569213236978,
-            "velocityX": -0.9076740863575303,
-            "velocityY": -2.8037497219427316,
-            "timestamp": 0.35544722680586044
-        },
-        {
-            "x": 2.0423710011934597,
-            "y": 4.918132506074763,
-            "heading": -0.27341197795784133,
-            "angularVelocity": -1.0916682576129122,
-            "velocityX": -0.9912342756292476,
-            "velocityY": -3.0598973140692034,
-            "timestamp": 0.3877606110609387
-        },
-        {
-            "x": 2.00763899967256,
-            "y": 4.811032729430715,
-            "heading": -0.3053550995537523,
-            "angularVelocity": -0.9885415078704507,
-            "velocityX": -1.0748487761829064,
-            "velocityY": -3.3144091562374,
-            "timestamp": 0.4200739953160169
-        },
-        {
-            "x": 1.9707337568018743,
-            "y": 4.697468766706309,
-            "heading": -0.3150299299334663,
-            "angularVelocity": -0.2994062863655037,
-            "velocityX": -1.1421039213769328,
-            "velocityY": -3.514455862250325,
-            "timestamp": 0.45238737957109515
-        },
-        {
-            "x": 1.9331993710024595,
-            "y": 4.581134395027877,
-            "heading": -0.3150299515537849,
-            "angularVelocity": -6.690824588937528e-7,
-            "velocityX": -1.1615739627617825,
-            "velocityY": -3.6001915107405935,
-            "timestamp": 0.4847007638261734
-        },
-        {
-            "x": 1.8956658385726595,
-            "y": 4.464799748019651,
-            "heading": -0.3150299731737692,
-            "angularVelocity": -6.690721139443934e-7,
-            "velocityX": -1.1615475535931208,
-            "velocityY": -3.600200031352131,
-            "timestamp": 0.5170141480812516
-        },
-        {
-            "x": 1.8630740613066095,
-            "y": 4.346985096208152,
-            "heading": -0.31502999489570194,
-            "angularVelocity": -6.722271053919432e-7,
-            "velocityX": -1.0086154086732113,
-            "velocityY": -3.64600163453891,
-            "timestamp": 0.5493275323363298
-        },
-        {
-            "x": 1.8389498768606893,
-            "y": 4.227149626761845,
-            "heading": -0.3150300170939308,
-            "angularVelocity": -6.869670074324573e-7,
-            "velocityX": -0.7465694170405304,
-            "velocityY": -3.708539733886721,
-            "timestamp": 0.581640916591408
-        },
-        {
-            "x": 1.8234162959667812,
-            "y": 4.10590102408459,
-            "heading": -0.31503004024876186,
-            "angularVelocity": -7.165709048863888e-7,
-            "velocityX": -0.4807166210536207,
-            "velocityY": -3.7522718672898008,
-            "timestamp": 0.6139543008464863
-        },
-        {
-            "x": 1.816552246405149,
-            "y": 3.98385431066073,
-            "heading": -0.315030064931292,
-            "angularVelocity": -7.63848502164674e-7,
-            "velocityX": -0.21242125267501,
-            "velocityY": -3.776970943694307,
-            "timestamp": 0.6462676851015645
-        },
-        {
-            "x": 1.818392595454208,
-            "y": 3.861628584870307,
-            "heading": -0.315030091871416,
-            "angularVelocity": -8.337140982566696e-7,
-            "velocityX": 0.05695315088421205,
-            "velocityY": -3.782510826646546,
-            "timestamp": 0.6785810693566428
-        },
-        {
-            "x": 1.828928030254088,
-            "y": 3.739843861498511,
-            "heading": -0.3150301220653929,
-            "angularVelocity": -9.344108498281474e-7,
-            "velocityX": 0.32603935003261814,
-            "velocityY": -3.7688631562216113,
-            "timestamp": 0.710894453611721
-        },
-        {
-            "x": 1.8481051197294465,
-            "y": 3.619117921789427,
-            "heading": -0.3150301569615331,
-            "angularVelocity": -0.0000010799283629257,
-            "velocityX": 0.593472021499706,
-            "velocityY": -3.7360970536569806,
-            "timestamp": 0.7432078378667992
-        },
-        {
-            "x": 1.875826590719106,
-            "y": 3.500063178055521,
-            "heading": -0.31503019880828736,
-            "angularVelocity": -0.0000012950285239246719,
-            "velocityX": 0.8578943873792161,
-            "velocityY": -3.684378670896923,
-            "timestamp": 0.7755212221218775
-        },
-        {
-            "x": 1.9119518234377388,
-            "y": 3.3832835666467993,
-            "heading": -0.31503025136937535,
-            "angularVelocity": -0.0000016266042432729645,
-            "velocityX": 1.1179650027822445,
-            "velocityY": -3.61397031294762,
-            "timestamp": 0.8078346063769557
-        },
-        {
-            "x": 1.9562975654606443,
-            "y": 3.269371484637439,
-            "heading": -0.31503032157308203,
-            "angularVelocity": -0.0000021725891085409373,
-            "velocityX": 1.3723645184560365,
-            "velocityY": -3.5252290849559675,
-            "timestamp": 0.8401479906320339
-        },
-        {
-            "x": 2.008638858795166,
-            "y": 3.158904790878296,
-            "heading": -0.3150304332740777,
-            "angularVelocity": -0.0000034568027529519608,
-            "velocityX": 1.6198022751608165,
-            "velocityY": -3.4186049002831713,
-            "timestamp": 0.8724613748871122
-        },
-        {
-            "x": 2.0775165641117233,
-            "y": 3.0394044472847606,
-            "heading": -0.3150305189714284,
-            "angularVelocity": -0.000002350392551393865,
-            "velocityX": 1.8890857675343722,
-            "velocityY": -3.277495922091173,
-            "timestamp": 0.9089222420961518
-        },
-        {
-            "x": 2.155767726009908,
-            "y": 2.9258208622381168,
-            "heading": -0.3150305770370975,
-            "angularVelocity": -0.00000159254767063084,
-            "velocityX": 2.1461684235196876,
-            "velocityY": -3.1152189660064753,
-            "timestamp": 0.9453831093051914
-        },
-        {
-            "x": 2.242886953120307,
-            "y": 2.818887620265158,
-            "heading": -0.3150306203630764,
-            "angularVelocity": -0.0000011882871183876306,
-            "velocityX": 2.389389879591231,
-            "velocityY": -2.932822232666113,
-            "timestamp": 0.981843976514231
-        },
-        {
-            "x": 2.338311580267833,
-            "y": 2.719295349641276,
-            "heading": -0.3150306549359537,
-            "angularVelocity": -9.482187337248934e-7,
-            "velocityX": 2.617179306252704,
-            "velocityY": -2.731483868798106,
-            "timestamp": 1.0183048437232707
-        },
-        {
-            "x": 2.4414253013619946,
-            "y": 2.6276872624602983,
-            "heading": -0.31503068395926054,
-            "angularVelocity": -7.960125215618009e-7,
-            "velocityX": 2.8280655120730875,
-            "velocityY": -2.512504342142078,
-            "timestamp": 1.0547657109323103
-        },
-        {
-            "x": 2.5515621486647997,
-            "y": 2.5446549983777076,
-            "heading": -0.3150307093352344,
-            "angularVelocity": -6.959783403374032e-7,
-            "velocityX": 3.020686443670201,
-            "velocityY": -2.2772981127010774,
-            "timestamp": 1.09122657814135
-        },
-        {
-            "x": 2.6680107915990137,
-            "y": 2.4707347968336117,
-            "heading": -0.31503073229503964,
-            "angularVelocity": -6.297108928988803e-7,
-            "velocityX": 3.1937979496369038,
-            "velocityY": -2.0273846236374995,
-            "timestamp": 1.1276874453503896
-        },
-        {
-            "x": 2.7900191236562386,
-            "y": 2.4064040113359084,
-            "heading": -0.3150307537024253,
-            "angularVelocity": -5.871332010760787e-7,
-            "velocityX": 3.3462816821585726,
-            "velocityY": -1.764378919702536,
-            "timestamp": 1.1641483125594292
-        },
-        {
-            "x": 2.9167990841955884,
-            "y": 2.352077913506683,
-            "heading": -0.3150307742165802,
-            "angularVelocity": -5.626348607586924e-7,
-            "velocityX": 3.477151539278742,
-            "velocityY": -1.4899836999970275,
-            "timestamp": 1.2006091797684688
-        },
-        {
-            "x": 3.0475311021850517,
-            "y": 2.308104934680324,
-            "heading": -0.31503079438993686,
-            "angularVelocity": -5.532878997498012e-7,
-            "velocityX": 3.585543296047864,
-            "velocityY": -1.2060321707174542,
-            "timestamp": 1.2370700469775084
-        },
-        {
-            "x": 3.178817728061225,
-            "y": 2.2658165761507623,
-            "heading": -0.3150308145386853,
-            "angularVelocity": -5.526129783451472e-7,
-            "velocityX": 3.600754340906743,
-            "velocityY": -1.159828653748452,
-            "timestamp": 1.273530914186548
-        },
-        {
-            "x": 3.3101043979960063,
-            "y": 2.2235283544039026,
-            "heading": -0.3150308346874411,
-            "angularVelocity": -5.526131795163358e-7,
-            "velocityX": 3.600755549287413,
-            "velocityY": -1.1598249022550946,
-            "timestamp": 1.3099917813955877
-        },
-        {
-            "x": 3.4413910762638236,
-            "y": 2.1812401585275256,
-            "heading": -0.31503085483620696,
-            "angularVelocity": -5.52613457009699e-7,
-            "velocityX": 3.60075577783481,
-            "velocityY": -1.159824192714005,
-            "timestamp": 1.3464526486046273
-        },
-        {
-            "x": 3.572677762945226,
-            "y": 2.138951988771722,
-            "heading": -0.31503087498498206,
-            "angularVelocity": -5.52613710159953e-7,
-            "velocityX": 3.6007560085914068,
-            "velocityY": -1.1598234763137998,
-            "timestamp": 1.382913515813667
-        },
-        {
-            "x": 3.7039644581047573,
-            "y": 2.0966638453368867,
-            "heading": -0.31503089513376514,
-            "angularVelocity": -5.526139295643368e-7,
-            "velocityX": 3.6007562411181984,
-            "velocityY": -1.1598227544173942,
-            "timestamp": 1.4193743830227066
-        },
-        {
-            "x": 3.8352511618245644,
-            "y": 2.05437572847807,
-            "heading": -0.3150309152825546,
-            "angularVelocity": -5.526141044747151e-7,
-            "velocityX": 3.600756475898009,
-            "velocityY": -1.1598220255258234,
-            "timestamp": 1.4558352502317462
-        },
-        {
-            "x": 3.966537874250263,
-            "y": 2.01208763864737,
-            "heading": -0.3150309354313486,
-            "angularVelocity": -5.526142273042023e-7,
-            "velocityX": 3.6007567146716046,
-            "velocityY": -1.159821284234734,
-            "timestamp": 1.4922961174407858
-        },
-        {
-            "x": 4.097824595645338,
-            "y": 1.9697995766628096,
-            "heading": -0.3150309555801447,
-            "angularVelocity": -5.526142864672618e-7,
-            "velocityX": 3.600756960671674,
-            "velocityY": -1.159820520507965,
-            "timestamp": 1.5287569846498255
-        },
-        {
-            "x": 4.229111326453571,
-            "y": 1.9275115439021684,
-            "heading": -0.3150309757289402,
-            "angularVelocity": -5.5261426880692e-7,
-            "velocityX": 3.600757218843208,
-            "velocityY": -1.1598197189933317,
-            "timestamp": 1.565217851858865
-        },
-        {
-            "x": 4.360398019008411,
-            "y": 1.885223392382287,
-            "heading": -0.31503099587881916,
-            "angularVelocity": -5.52643987126892e-7,
-            "velocityX": 3.6007561696802117,
-            "velocityY": -1.1598229761632481,
-            "timestamp": 1.6016787190679047
-        },
-        {
-            "x": 4.485161526388635,
-            "y": 1.844329974236374,
-            "heading": -0.3370638509456068,
-            "angularVelocity": -0.6042877406197901,
-            "velocityX": 3.4218469534726492,
-            "velocityY": -1.1215700907896822,
-            "timestamp": 1.6381395862769443
-        },
-        {
-            "x": 4.60044458089581,
-            "y": 1.8067761222092775,
-            "heading": -0.3770388194107599,
-            "angularVelocity": -1.0963800788381923,
-            "velocityX": 3.1618297460185825,
-            "velocityY": -1.0299769287381584,
-            "timestamp": 1.674600453485984
-        },
-        {
-            "x": 4.705219804759057,
-            "y": 1.772711102552083,
-            "heading": -0.4189139196427851,
-            "angularVelocity": -1.148494356756437,
-            "velocityX": 2.8736349923479074,
-            "velocityY": -0.9342898911836356,
-            "timestamp": 1.7110613206950236
-        },
-        {
-            "x": 4.799468970200479,
-            "y": 1.7421012617230478,
-            "heading": -0.45948472427125614,
-            "angularVelocity": -1.1127218778387968,
-            "velocityX": 2.5849403115144614,
-            "velocityY": -0.8395258580532651,
-            "timestamp": 1.7475221879040632
-        },
-        {
-            "x": 4.883206648654635,
-            "y": 1.7149242324789875,
-            "heading": -0.49729979894596704,
-            "angularVelocity": -1.037141394852447,
-            "velocityX": 2.296645276538986,
-            "velocityY": -0.7453752838144925,
-            "timestamp": 1.7839830551131028
-        },
-        {
-            "x": 4.956447680828415,
-            "y": 1.6911655730288653,
-            "heading": -0.5315269969459382,
-            "angularVelocity": -0.9387379023033031,
-            "velocityX": 2.0087572726635705,
-            "velocityY": -0.6516207997442213,
-            "timestamp": 1.8204439223221425
-        },
-        {
-            "x": 5.019204214404415,
-            "y": 1.6708154773305386,
-            "heading": -0.5616288277349688,
-            "angularVelocity": -0.825592836737831,
-            "velocityX": 1.7212024392124468,
-            "velocityY": -0.5581352627093101,
-            "timestamp": 1.856904789531182
-        },
-        {
-            "x": 5.071485921099441,
-            "y": 1.6538669506708767,
-            "heading": -0.587231204588931,
-            "angularVelocity": -0.7021878198118158,
-            "velocityX": 1.4339128687006073,
-            "velocityY": -0.46484156733002446,
-            "timestamp": 1.8933656567402217
-        },
-        {
-            "x": 5.11330055140079,
-            "y": 1.6403147844077772,
-            "heading": -0.6080594902506735,
-            "angularVelocity": -0.5712504187662296,
-            "velocityX": 1.1468358682094448,
-            "velocityY": -0.37169072763413913,
-            "timestamp": 1.9298265239492614
-        },
-        {
-            "x": 5.1446544158470235,
-            "y": 1.6301549523614358,
-            "heading": -0.6239033665029361,
-            "angularVelocity": -0.43454469037794713,
-            "velocityX": 0.859931944747064,
-            "velocityY": -0.2786503126240163,
-            "timestamp": 1.966287391158301
-        },
-        {
-            "x": 5.165552752490355,
-            "y": 1.6233842390024875,
-            "heading": -0.6345958555798376,
-            "angularVelocity": -0.29325931869912014,
-            "velocityX": 0.5731716835893133,
-            "velocityY": -0.18569808886140712,
-            "timestamp": 2.0027482583673404
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -0.14821766002380998,
-            "velocityX": 0.2865331603263514,
-            "velocityY": -0.09281839027812194,
-            "timestamp": 2.03920912557638
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 2.046404654845657e-31,
-            "velocityX": 2.793064044552828e-33,
-            "velocityY": 5.221456528298987e-32,
-            "timestamp": 2.0756699927854196
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W2 To W1.traj b/src/main/deploy/choreo/W2 To W1.traj
deleted file mode 100644
index 35bba061..00000000
--- a/src/main/deploy/choreo/W2 To W1.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": -5.451838641793523e-34,
-            "angularVelocity": -2.0131697388730357e-33,
-            "velocityX": -3.0924959567962797e-34,
-            "velocityY": 4.789643826321005e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2520020156777094,
-            "y": 5.6078780285533805,
-            "heading": 0.01582108665485387,
-            "angularVelocity": 0.208254594271014,
-            "velocityX": 0.026352738707599337,
-            "velocityY": 0.630223424496655,
-            "timestamp": 0.07596992858781765
-        },
-        {
-            "x": 2.2560055471462697,
-            "y": 5.703634194455967,
-            "heading": 0.04745824180691435,
-            "angularVelocity": 0.4164431340157103,
-            "velocityX": 0.05269889735295001,
-            "velocityY": 1.2604482810839739,
-            "timestamp": 0.1519398571756353
-        },
-        {
-            "x": 2.262009891384212,
-            "y": 5.847268762594484,
-            "heading": 0.09489870943903907,
-            "angularVelocity": 0.6244637649920359,
-            "velocityX": 0.07903580205425154,
-            "velocityY": 1.8906766244025437,
-            "timestamp": 0.22790978576345294
-        },
-        {
-            "x": 2.2700142224347877,
-            "y": 6.038782263911744,
-            "heading": 0.1581161098401003,
-            "angularVelocity": 0.8321371571119077,
-            "velocityX": 0.1053618346017344,
-            "velocityY": 2.5209119565760743,
-            "timestamp": 0.3038797143512706
-        },
-        {
-            "x": 2.280009491675263,
-            "y": 6.278187226323642,
-            "heading": 0.23645965423367718,
-            "angularVelocity": 1.0312441494928544,
-            "velocityX": 0.13156875919557368,
-            "velocityY": 3.151312194997739,
-            "timestamp": 0.37984964293908824
-        },
-        {
-            "x": 2.288005657404111,
-            "y": 6.469712008115255,
-            "heading": 0.2990934539478391,
-            "angularVelocity": 0.8244551611202343,
-            "velocityX": 0.10525435363026732,
-            "velocityY": 2.52106044262263,
-            "timestamp": 0.4558195715269059
-        },
-        {
-            "x": 2.294002815862431,
-            "y": 6.613355952122642,
-            "heading": 0.3460499471183657,
-            "angularVelocity": 0.6180931592721867,
-            "velocityX": 0.07894121489645621,
-            "velocityY": 1.8908000399308182,
-            "timestamp": 0.5317895001147235
-        },
-        {
-            "x": 2.2980009414583966,
-            "y": 6.709118669367686,
-            "heading": 0.3773492268804029,
-            "angularVelocity": 0.4119956454330042,
-            "velocityX": 0.05262773929482027,
-            "velocityY": 1.260534517079955,
-            "timestamp": 0.6077594287025412
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.20601273965271094,
-            "velocityX": 0.026313813620245378,
-            "velocityY": 0.6302668901019914,
-            "timestamp": 0.6837293572903589
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 9.82651204832558e-34,
-            "velocityX": 0,
-            "velocityY": 3.8258931367738186e-36,
-            "timestamp": 0.7596992858781766
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W2 To W3.traj b/src/main/deploy/choreo/W2 To W3.traj
deleted file mode 100644
index f46e6706..00000000
--- a/src/main/deploy/choreo/W2 To W3.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 5.7052362935559275e-34,
-            "velocityX": 0,
-            "velocityY": -1.2494033787640354e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2520019447146944,
-            "y": 5.512001877264397,
-            "heading": -0.01569721301150974,
-            "angularVelocity": -0.20637467835572687,
-            "velocityX": 0.02632000319280478,
-            "velocityY": -0.6310417737196751,
-            "timestamp": 0.07606172005488261
-        },
-        {
-            "x": 2.2560053559202022,
-            "y": 5.416005527882177,
-            "heading": -0.04708679827781808,
-            "angularVelocity": -0.41268571422864314,
-            "velocityX": 0.05263371907208186,
-            "velocityY": -1.262084913580087,
-            "timestamp": 0.15212344010976522
-        },
-        {
-            "x": 2.262009560249172,
-            "y": 5.272010699235591,
-            "heading": -0.0941564711571504,
-            "angularVelocity": -0.6188352412405218,
-            "velocityX": 0.07893858204412474,
-            "velocityY": -1.8931313746610776,
-            "timestamp": 0.22818516016464785
-        },
-        {
-            "x": 2.2700137636187,
-            "y": 5.080016884298785,
-            "heading": -0.15688079244309955,
-            "angularVelocity": -0.8246503134650409,
-            "velocityX": 0.10523300503528431,
-            "velocityY": -2.5241845017213995,
-            "timestamp": 0.30424688021953045
-        },
-        {
-            "x": 2.2800092014745617,
-            "y": 4.840012168782865,
-            "heading": -0.23463455266722952,
-            "angularVelocity": -1.0222456206358,
-            "velocityX": 0.13141219852311892,
-            "velocityY": -3.155394268532757,
-            "timestamp": 0.38030860027441304
-        },
-        {
-            "x": 2.2880054914438257,
-            "y": 4.648007616430203,
-            "heading": -0.2967978427641432,
-            "angularVelocity": -0.8172743142287549,
-            "velocityX": 0.10512896583845424,
-            "velocityY": -2.524325668866294,
-            "timestamp": 0.45637032032929564
-        },
-        {
-            "x": 2.2940027351611283,
-            "y": 4.5040038585170965,
-            "heading": -0.3434020516948695,
-            "angularVelocity": -0.6127156853289512,
-            "velocityX": 0.07884706936650254,
-            "velocityY": -1.8932487696728393,
-            "timestamp": 0.5324320403841782
-        },
-        {
-            "x": 2.2980009144821483,
-            "y": 4.408001268437703,
-            "heading": -0.37446662411182524,
-            "angularVelocity": -0.40841269950956927,
-            "velocityX": 0.05256493434720428,
-            "velocityY": -1.2621669613850792,
-            "timestamp": 0.6084937604390609
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": -0.20422067601109492,
-            "velocityX": 0.026282412709165117,
-            "velocityY": -0.6310831309503169,
-            "timestamp": 0.6845554804939435
-        },
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 8.777318885868316e-35,
-            "velocityX": -1.1404320857258046e-34,
-            "velocityY": 1.134511753128188e-36,
-            "timestamp": 0.7606172005488262
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W3 To C1.traj b/src/main/deploy/choreo/W3 To C1.traj
deleted file mode 100644
index 5529ce29..00000000
--- a/src/main/deploy/choreo/W3 To C1.traj	
+++ /dev/null
@@ -1,553 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 1.095053106710567e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3031602212725026,
-            "y": 4.367935645195281,
-            "heading": -0.3933962364434778,
-            "angularVelocity": -0.10578752662991632,
-            "velocityX": 0.0984360180997807,
-            "velocityY": 0.24718310735800264,
-            "timestamp": 0.032104318454850914
-        },
-        {
-            "x": 2.309652031024921,
-            "y": 4.383743043384139,
-            "heading": -0.39998627296935113,
-            "angularVelocity": -0.20526947286363117,
-            "velocityX": 0.20220986038211264,
-            "velocityY": 0.4923760711845366,
-            "timestamp": 0.06420863690970183
-        },
-        {
-            "x": 2.3196739941431104,
-            "y": 4.407342390021744,
-            "heading": -0.40953275493580865,
-            "angularVelocity": -0.29735818811675285,
-            "velocityX": 0.31216869257896596,
-            "velocityY": 0.7350832465356143,
-            "timestamp": 0.09631295536455274
-        },
-        {
-            "x": 2.333458307096076,
-            "y": 4.438632228164309,
-            "heading": -0.4217541925110665,
-            "angularVelocity": -0.3806789292987277,
-            "velocityX": 0.4293600866297902,
-            "velocityY": 0.9746301945817274,
-            "timestamp": 0.12841727381940365
-        },
-        {
-            "x": 2.3512791398330983,
-            "y": 4.477480960055527,
-            "heading": -0.4363123371700568,
-            "angularVelocity": -0.4534637506622021,
-            "velocityX": 0.5550914517025046,
-            "velocityY": 1.21007807550416,
-            "timestamp": 0.16052159227425455
-        },
-        {
-            "x": 2.3734634122157945,
-            "y": 4.523713969255995,
-            "heading": -0.45279460426513807,
-            "angularVelocity": -0.5133971966500652,
-            "velocityX": 0.691005866201282,
-            "velocityY": 1.4400869236793286,
-            "timestamp": 0.19262591072910545
-        },
-        {
-            "x": 2.4004044405613056,
-            "y": 4.577093572392249,
-            "heading": -0.4706891378053212,
-            "angularVelocity": -0.5573871180398601,
-            "velocityX": 0.8391714773013875,
-            "velocityY": 1.6626923013900174,
-            "timestamp": 0.22473022918395635
-        },
-        {
-            "x": 2.432578225951104,
-            "y": 4.63728707308353,
-            "heading": -0.48934877664951465,
-            "angularVelocity": -0.581218968109697,
-            "velocityX": 1.002163787873114,
-            "velocityY": 1.8749347000134435,
-            "timestamp": 0.25683454763880725
-        },
-        {
-            "x": 2.4705256917588616,
-            "y": 4.703846202675345,
-            "heading": -0.5079484621261662,
-            "angularVelocity": -0.5793515131868818,
-            "velocityX": 1.1820050271780043,
-            "velocityY": 2.073214221489191,
-            "timestamp": 0.28893886609365815
-        },
-        {
-            "x": 2.5143889510907504,
-            "y": 4.77663720854218,
-            "heading": -0.5259955708448517,
-            "angularVelocity": -0.5621395995079442,
-            "velocityX": 1.366272870535308,
-            "velocityY": 2.267327554989265,
-            "timestamp": 0.32104318454850905
-        },
-        {
-            "x": 2.564366566619072,
-            "y": 4.855456871813396,
-            "heading": -0.5427715655324792,
-            "angularVelocity": -0.5225463580925951,
-            "velocityX": 1.5567256348583118,
-            "velocityY": 2.4551109341275055,
-            "timestamp": 0.35314750300335995
-        },
-        {
-            "x": 2.620751841763659,
-            "y": 4.939963572730219,
-            "heading": -0.5571329301868887,
-            "angularVelocity": -0.44733435704626356,
-            "velocityX": 1.7563143482981505,
-            "velocityY": 2.6322533847172953,
-            "timestamp": 0.38525182145821085
-        },
-        {
-            "x": 2.6840019979529464,
-            "y": 5.029487269137319,
-            "heading": -0.5670183536644556,
-            "angularVelocity": -0.30791569338152147,
-            "velocityX": 1.9701448039844656,
-            "velocityY": 2.788525055686805,
-            "timestamp": 0.41735613991306175
-        },
-        {
-            "x": 2.7547918742113016,
-            "y": 5.122370588176559,
-            "heading": -0.568384562504894,
-            "angularVelocity": -0.0425552980468726,
-            "velocityX": 2.2049954543625816,
-            "velocityY": 2.893172118569162,
-            "timestamp": 0.44946045836791265
-        },
-        {
-            "x": 2.8341176789452183,
-            "y": 5.214333601098647,
-            "heading": -0.5683844391321649,
-            "angularVelocity": 0.0000038428702119811485,
-            "velocityX": 2.4708764599838617,
-            "velocityY": 2.864506002562167,
-            "timestamp": 0.48156477682276355
-        },
-        {
-            "x": 2.9197482153809866,
-            "y": 5.3004567509455125,
-            "heading": -0.5683843814173083,
-            "angularVelocity": 0.0000017977287583592433,
-            "velocityX": 2.6672591276526463,
-            "velocityY": 2.6826032755681406,
-            "timestamp": 0.5136690952776145
-        },
-        {
-            "x": 3.0112544304014244,
-            "y": 5.380309105627853,
-            "heading": -0.5683843265105415,
-            "angularVelocity": 0.0000017102610943456471,
-            "velocityX": 2.850277452521701,
-            "velocityY": 2.487277678691065,
-            "timestamp": 0.5457734137324655
-        },
-        {
-            "x": 3.107551803887462,
-            "y": 5.454313172250514,
-            "heading": -0.5683842730108181,
-            "angularVelocity": 0.0000016664338651344868,
-            "velocityX": 2.999514648518804,
-            "velocityY": 2.305112526426488,
-            "timestamp": 0.5778777321873164
-        },
-        {
-            "x": 3.2038516202489813,
-            "y": 5.528314059986814,
-            "heading": -0.5683842195116714,
-            "angularVelocity": 0.000001666415897857932,
-            "velocityX": 2.9995907403219917,
-            "velocityY": 2.305013509019602,
-            "timestamp": 0.6099820506421674
-        },
-        {
-            "x": 3.3001514533185667,
-            "y": 5.602314925980335,
-            "heading": -0.5683841660125195,
-            "angularVelocity": 0.000001666416059709123,
-            "velocityX": 2.9995912607524815,
-            "velocityY": 2.305012831765639,
-            "timestamp": 0.6420863690970183
-        },
-        {
-            "x": 3.3964512995777554,
-            "y": 5.676315774809768,
-            "heading": -0.568384112513365,
-            "angularVelocity": 0.0000016664161452516387,
-            "velocityX": 2.999591671588281,
-            "velocityY": 2.30501229713074,
-            "timestamp": 0.6741906875518693
-        },
-        {
-            "x": 3.492751156794414,
-            "y": 5.750316609379858,
-            "heading": -0.5683840590142086,
-            "angularVelocity": 0.000001666416198927138,
-            "velocityX": 2.999592012896559,
-            "velocityY": 2.3050118529742454,
-            "timestamp": 0.7062950060067202
-        },
-        {
-            "x": 3.5890510233460025,
-            "y": 5.824317431802072,
-            "heading": -0.5683840055150509,
-            "angularVelocity": 0.0000016664162437925357,
-            "velocityX": 2.999592303665221,
-            "velocityY": 2.305011474586604,
-            "timestamp": 0.7383993244615712
-        },
-        {
-            "x": 3.6853508979893648,
-            "y": 5.898318243694168,
-            "heading": -0.5683839520158918,
-            "angularVelocity": 0.0000016664162889267752,
-            "velocityX": 2.9995925557115117,
-            "velocityY": 2.3050111465896648,
-            "timestamp": 0.7705036429164221
-        },
-        {
-            "x": 3.7816507797340138,
-            "y": 5.972319046345101,
-            "heading": -0.5683838985167311,
-            "angularVelocity": 0.0000016664163350532929,
-            "velocityX": 2.9995927769056334,
-            "velocityY": 2.305010858741686,
-            "timestamp": 0.8026079613712731
-        },
-        {
-            "x": 3.877950667767949,
-            "y": 6.046319840811553,
-            "heading": -0.5683838450175687,
-            "angularVelocity": 0.0000016664163859759036,
-            "velocityX": 2.999592972807203,
-            "velocityY": 2.305010603807763,
-            "timestamp": 0.8347122798261241
-        },
-        {
-            "x": 3.9742505615842325,
-            "y": 6.120320627753221,
-            "heading": -0.5683837915184047,
-            "angularVelocity": 0.0000016664164376020863,
-            "velocityX": 2.999593152918432,
-            "velocityY": 2.3050103694223405,
-            "timestamp": 0.866816598280975
-        },
-        {
-            "x": 4.070552264919673,
-            "y": 6.194319059842733,
-            "heading": -0.5683837380195041,
-            "angularVelocity": 0.0000016664082331173329,
-            "velocityX": 2.999649516649054,
-            "velocityY": 2.304937019409932,
-            "timestamp": 0.898920916735826
-        },
-        {
-            "x": 4.171102213813095,
-            "y": 6.262432889296395,
-            "heading": -0.5683836849080734,
-            "angularVelocity": 0.0000016543391427080076,
-            "velocityX": 3.131975812999348,
-            "velocityY": 2.1216407241116846,
-            "timestamp": 0.9310252351906769
-        },
-        {
-            "x": 4.276216983795166,
-            "y": 6.32326602935791,
-            "heading": -0.5683836309724583,
-            "angularVelocity": 0.0000016800112198234746,
-            "velocityX": 3.274162948822482,
-            "velocityY": 1.8948584797732355,
-            "timestamp": 0.9631295536455279
-        },
-        {
-            "x": 4.413834682670106,
-            "y": 6.38740625436239,
-            "heading": -0.5683835821873748,
-            "angularVelocity": 0.0000012155044166425132,
-            "velocityX": 3.428812837046335,
-            "velocityY": 1.5980853383274327,
-            "timestamp": 1.0032652231659112
-        },
-        {
-            "x": 4.5565820466376845,
-            "y": 6.439134291384663,
-            "heading": -0.5683835352510233,
-            "angularVelocity": 0.0000011694423477450916,
-            "velocityX": 3.556620972650851,
-            "velocityY": 1.2888295533727718,
-            "timestamp": 1.0434008926862943
-        },
-        {
-            "x": 4.7007810920363475,
-            "y": 6.4866657027970485,
-            "heading": -0.5683834885999294,
-            "angularVelocity": 0.000001162335011018839,
-            "velocityX": 3.5927903314389917,
-            "velocityY": 1.184268556632563,
-            "timestamp": 1.0835365622066775
-        },
-        {
-            "x": 4.84498021653845,
-            "y": 6.534196874227661,
-            "heading": -0.5683834419488406,
-            "angularVelocity": 0.0000011623348856187121,
-            "velocityX": 3.5927923023401758,
-            "velocityY": 1.1842625773683353,
-            "timestamp": 1.1236722317270607
-        },
-        {
-            "x": 4.989179343628637,
-            "y": 6.581728037806586,
-            "heading": -0.5683833952977476,
-            "angularVelocity": 0.0000011623349868626642,
-            "velocityX": 3.5927923668235673,
-            "velocityY": 1.1842623817396558,
-            "timestamp": 1.1638079012474438
-        },
-        {
-            "x": 5.133378473125504,
-            "y": 6.629259194084157,
-            "heading": -0.5683833486466496,
-            "angularVelocity": 0.0000011623351115999292,
-            "velocityX": 3.592792426787221,
-            "velocityY": 1.1842621998228242,
-            "timestamp": 1.203943570767827
-        },
-        {
-            "x": 5.277577604867278,
-            "y": 6.676790343551162,
-            "heading": -0.5683833019955459,
-            "angularVelocity": 0.0000011623352597981392,
-            "velocityX": 3.5927924827201663,
-            "velocityY": 1.184262030134241,
-            "timestamp": 1.2440792402882102
-        },
-        {
-            "x": 5.421776738706852,
-            "y": 6.7243214866538885,
-            "heading": -0.5683832553444353,
-            "angularVelocity": 0.0000011623354265532326,
-            "velocityX": 3.5927925349879053,
-            "velocityY": 1.1842618715650601,
-            "timestamp": 1.2842149098085933
-        },
-        {
-            "x": 5.565975874510482,
-            "y": 6.771852623798087,
-            "heading": -0.5683832086933173,
-            "angularVelocity": 0.0000011623356113966992,
-            "velocityX": 3.5927925839233144,
-            "velocityY": 1.18426172310541,
-            "timestamp": 1.3243505793289765
-        },
-        {
-            "x": 5.710175012156514,
-            "y": 6.819383755352828,
-            "heading": -0.5683831620421912,
-            "angularVelocity": 0.0000011623358144790056,
-            "velocityX": 3.5927926298276605,
-            "velocityY": 1.1842615838413222,
-            "timestamp": 1.3644862488493597
-        },
-        {
-            "x": 5.854374151534237,
-            "y": 6.866914881653982,
-            "heading": -0.5683831153910563,
-            "angularVelocity": 0.000001162336033125057,
-            "velocityX": 3.592792672973599,
-            "velocityY": 1.1842614529456208,
-            "timestamp": 1.4046219183697428
-        },
-        {
-            "x": 5.998573292523273,
-            "y": 6.9144460030667485,
-            "heading": -0.568383068739913,
-            "angularVelocity": 0.000001162336242705986,
-            "velocityX": 3.59279271312027,
-            "velocityY": 1.184261331149038,
-            "timestamp": 1.444757587890126
-        },
-        {
-            "x": 6.142772435013044,
-            "y": 6.9619771199265985,
-            "heading": -0.5683830220887615,
-            "angularVelocity": 0.0000011623364467362213,
-            "velocityX": 3.5927927505118338,
-            "velocityY": 1.1842612177108516,
-            "timestamp": 1.4848932574105091
-        },
-        {
-            "x": 6.286971578902684,
-            "y": 7.00950823253954,
-            "heading": -0.5683829754376022,
-            "angularVelocity": 0.0000011623366413601916,
-            "velocityX": 3.59279278539024,
-            "velocityY": 1.1842611118970372,
-            "timestamp": 1.5250289269308923
-        },
-        {
-            "x": 6.431170724100105,
-            "y": 7.057039341184945,
-            "heading": -0.5683829287864354,
-            "angularVelocity": 0.0000011623368281432719,
-            "velocityX": 3.592792817974252,
-            "velocityY": 1.1842610130439195,
-            "timestamp": 1.5651645964512755
-        },
-        {
-            "x": 6.575369870521147,
-            "y": 7.10457044611814,
-            "heading": -0.5683828821352613,
-            "angularVelocity": 0.0000011623370098744323,
-            "velocityX": 3.5927928484613587,
-            "velocityY": 1.1842609205523604,
-            "timestamp": 1.6053002659716586
-        },
-        {
-            "x": 6.719569018075523,
-            "y": 7.152101547613029,
-            "heading": -0.5683828354840802,
-            "angularVelocity": 0.0000011623371832362076,
-            "velocityX": 3.5927928766989634,
-            "velocityY": 1.1842608348853054,
-            "timestamp": 1.6454359354920418
-        },
-        {
-            "x": 6.8637678827817385,
-            "y": 7.1996335071853474,
-            "heading": -0.5683827888209949,
-            "angularVelocity": 0.0000011626337817909648,
-            "velocityX": 3.592785829397527,
-            "velocityY": 1.1842822143076228,
-            "timestamp": 1.685571605012425
-        },
-        {
-            "x": 6.99756534811195,
-            "y": 7.244965845733408,
-            "heading": -0.5424482126349314,
-            "angularVelocity": 0.6461727559544621,
-            "velocityX": 3.3336298342366253,
-            "velocityY": 1.1294775716906411,
-            "timestamp": 1.7257072745328081
-        },
-        {
-            "x": 7.121516587135396,
-            "y": 7.285176196921745,
-            "heading": -0.489772109644447,
-            "angularVelocity": 1.3124510845329824,
-            "velocityX": 3.088306249893159,
-            "velocityY": 1.0018607305881775,
-            "timestamp": 1.7658429440531913
-        },
-        {
-            "x": 7.233956124618328,
-            "y": 7.321947450112657,
-            "heading": -0.4161805837381191,
-            "angularVelocity": 1.8335691614401448,
-            "velocityX": 2.8014865287304875,
-            "velocityY": 0.9161739079059802,
-            "timestamp": 1.8059786135735745
-        },
-        {
-            "x": 7.333707629732149,
-            "y": 7.35452494917019,
-            "heading": -0.3377597360881954,
-            "angularVelocity": 1.953894094381483,
-            "velocityX": 2.4853579448365832,
-            "velocityY": 0.8116844554190893,
-            "timestamp": 1.8461142830939576
-        },
-        {
-            "x": 7.420722566948463,
-            "y": 7.382895867936689,
-            "heading": -0.26184604843223563,
-            "angularVelocity": 1.8914269666638126,
-            "velocityX": 2.1680200743162716,
-            "velocityY": 0.7068754328887092,
-            "timestamp": 1.8862499526143408
-        },
-        {
-            "x": 7.495090208476241,
-            "y": 7.40709623575076,
-            "heading": -0.19218016370533045,
-            "angularVelocity": 1.7357598754276362,
-            "velocityX": 1.8529064649092093,
-            "velocityY": 0.6029640990984653,
-            "timestamp": 1.926385622134724
-        },
-        {
-            "x": 7.556901497509452,
-            "y": 7.427169942598205,
-            "heading": -0.13107853540878284,
-            "angularVelocity": 1.522377202790068,
-            "velocityX": 1.5400587500308252,
-            "velocityY": 0.500146305949861,
-            "timestamp": 1.9665212916551071
-        },
-        {
-            "x": 7.606235479245265,
-            "y": 7.443160506733181,
-            "heading": -0.08018624320935208,
-            "angularVelocity": 1.2680065589434022,
-            "velocityX": 1.2291804852229526,
-            "velocityY": 0.3984127915657456,
-            "timestamp": 2.0066569611754903
-        },
-        {
-            "x": 7.643159935464096,
-            "y": 7.455107253348189,
-            "heading": -0.040758426269481916,
-            "angularVelocity": 0.982363503861482,
-            "velocityX": 0.9199910369024534,
-            "velocityY": 0.29765908374694494,
-            "timestamp": 2.0467926306958737
-        },
-        {
-            "x": 7.667732329164437,
-            "y": 7.4630440972663505,
-            "heading": -0.013776014916518935,
-            "angularVelocity": 0.6722800859036306,
-            "velocityX": 0.6122333075286244,
-            "velocityY": 0.1977503804721968,
-            "timestamp": 2.086928300216257
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": 2.662292212835229e-35,
-            "angularVelocity": 0.3432362056280802,
-            "velocityX": 0.30565506897381545,
-            "velocityY": 0.09856326756028964,
-            "timestamp": 2.1270639697366405
-        },
-        {
-            "x": 7.68,
-            "y": 7.467,
-            "heading": -1.5591265591207004e-41,
-            "angularVelocity": 3.563315968840566e-34,
-            "velocityX": -7.620676928265867e-35,
-            "velocityY": -1.1745137620932586e-33,
-            "timestamp": 2.167199639257024
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W3 To C2.traj b/src/main/deploy/choreo/W3 To C2.traj
deleted file mode 100644
index 0646af70..00000000
--- a/src/main/deploy/choreo/W3 To C2.traj	
+++ /dev/null
@@ -1,553 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -3.138879459525789e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 2.304641315068496,
-            "y": 4.371901353899003,
-            "heading": -0.39554860460981267,
-            "angularVelocity": -0.1412451028521288,
-            "velocityX": 0.11814916908296796,
-            "velocityY": 0.3029604871416256,
-            "timestamp": 0.039283518492098594
-        },
-        {
-            "x": 2.314310378834228,
-            "y": 4.395560050627161,
-            "heading": -0.4062436571914542,
-            "angularVelocity": -0.27225291909092403,
-            "velocityX": 0.24613538035494495,
-            "velocityY": 0.6022550330596644,
-            "timestamp": 0.07856703698419719
-        },
-        {
-            "x": 2.3294686593461313,
-            "y": 4.43078385259534,
-            "heading": -0.42159423390969814,
-            "angularVelocity": -0.3907637937605655,
-            "velocityX": 0.385868707635076,
-            "velocityY": 0.8966559850097278,
-            "timestamp": 0.11785055547629578
-        },
-        {
-            "x": 2.3506733572393026,
-            "y": 4.477309554936099,
-            "heading": -0.44099148005781696,
-            "angularVelocity": -0.49377568233887975,
-            "velocityX": 0.5397861165984255,
-            "velocityY": 1.1843568022074944,
-            "timestamp": 0.15713407396839438
-        },
-        {
-            "x": 2.3786040121360466,
-            "y": 4.534766426387325,
-            "heading": -0.4636665715847995,
-            "angularVelocity": -0.5772164102747018,
-            "velocityX": 0.7110018646207296,
-            "velocityY": 1.462620296162777,
-            "timestamp": 0.19641759246049298
-        },
-        {
-            "x": 2.4140943920114943,
-            "y": 4.602616046670193,
-            "heading": -0.48862953085625704,
-            "angularVelocity": -0.6354562989682879,
-            "velocityX": 0.903441983756771,
-            "velocityY": 1.727177780588029,
-            "timestamp": 0.23570111095259158
-        },
-        {
-            "x": 2.458163120855333,
-            "y": 4.68005234098581,
-            "heading": -0.5145799632934547,
-            "angularVelocity": -0.6605933845360225,
-            "velocityX": 1.1218121628463973,
-            "velocityY": 1.9712158505149888,
-            "timestamp": 0.27498462944469015
-        },
-        {
-            "x": 2.5115097740388146,
-            "y": 4.766403162212065,
-            "heading": -0.5400224328341426,
-            "angularVelocity": -0.6476626971635644,
-            "velocityX": 1.3579907103842537,
-            "velocityY": 2.1981437646332993,
-            "timestamp": 0.31426814793678876
-        },
-        {
-            "x": 2.5744195202560527,
-            "y": 4.861351309968927,
-            "heading": -0.563860333505168,
-            "angularVelocity": -0.6068168429413633,
-            "velocityX": 1.6014285031491418,
-            "velocityY": 2.416997035944537,
-            "timestamp": 0.35355166642888736
-        },
-        {
-            "x": 2.6473242968990576,
-            "y": 4.964346697364103,
-            "heading": -0.5842892856317753,
-            "angularVelocity": -0.5200387569839264,
-            "velocityX": 1.8558616804566808,
-            "velocityY": 2.6218473127835553,
-            "timestamp": 0.39283518492098596
-        },
-        {
-            "x": 2.7309171688764238,
-            "y": 5.07424676618957,
-            "heading": -0.5978668700067779,
-            "angularVelocity": -0.3456305569399124,
-            "velocityX": 2.127937496081736,
-            "velocityY": 2.7976126641403014,
-            "timestamp": 0.43211870341308456
-        },
-        {
-            "x": 2.826173576921219,
-            "y": 5.187875652325513,
-            "heading": -0.5974605263008922,
-            "angularVelocity": 0.010343872481393205,
-            "velocityX": 2.4248441000503926,
-            "velocityY": 2.8925333192550147,
-            "timestamp": 0.47140222190518316
-        },
-        {
-            "x": 2.9311441629204515,
-            "y": 5.293067244130248,
-            "heading": -0.5974604636085413,
-            "angularVelocity": 0.000001595894492248037,
-            "velocityX": 2.672127905761066,
-            "velocityY": 2.677753822532693,
-            "timestamp": 0.5106857403972818
-        },
-        {
-            "x": 3.044820699228354,
-            "y": 5.388784226639372,
-            "heading": -0.597460407679312,
-            "angularVelocity": 0.0000014237326879146737,
-            "velocityX": 2.8937463005192323,
-            "velocityY": 2.436568468997834,
-            "timestamp": 0.5499692588893803
-        },
-        {
-            "x": 3.1663502143390825,
-            "y": 5.474310087323753,
-            "heading": -0.5974603544159754,
-            "angularVelocity": 0.0000013558698005366095,
-            "velocityX": 3.0936514796952492,
-            "velocityY": 2.177143595261183,
-            "timestamp": 0.5892527773814789
-        },
-        {
-            "x": 3.289918343405979,
-            "y": 5.556863181758907,
-            "heading": -0.5974603016382944,
-            "angularVelocity": 0.0000013435069733969365,
-            "velocityX": 3.145546371864123,
-            "velocityY": 2.1014689519673895,
-            "timestamp": 0.6285362958735774
-        },
-        {
-            "x": 3.413486607435696,
-            "y": 5.639416074177005,
-            "heading": -0.5974602488606314,
-            "angularVelocity": 0.0000013435065140042136,
-            "velocityX": 3.145549807473418,
-            "velocityY": 2.101463809427605,
-            "timestamp": 0.667819814365676
-        },
-        {
-            "x": 3.5370548851900594,
-            "y": 5.721968946051526,
-            "heading": -0.5974601960829673,
-            "angularVelocity": 0.0000013435065431603766,
-            "velocityX": 3.145550156847558,
-            "velocityY": 2.1014632864709872,
-            "timestamp": 0.7071033328577745
-        },
-        {
-            "x": 3.6606231741521,
-            "y": 5.804521801149959,
-            "heading": -0.5974601433053026,
-            "angularVelocity": 0.0000013435065598734946,
-            "velocityX": 3.1455504421498475,
-            "velocityY": 2.1014628594194082,
-            "timestamp": 0.746386851349873
-        },
-        {
-            "x": 3.7841914725218415,
-            "y": 5.887074642166574,
-            "heading": -0.597460090527637,
-            "angularVelocity": 0.0000013435065802330538,
-            "velocityX": 3.1455506816319834,
-            "velocityY": 2.101462500953099,
-            "timestamp": 0.7856703698419716
-        },
-        {
-            "x": 3.9077597789317933,
-            "y": 5.969627471148282,
-            "heading": -0.5974600377499704,
-            "angularVelocity": 0.0000013435066065618314,
-            "velocityX": 3.14555088630331,
-            "velocityY": 2.1014621945928877,
-            "timestamp": 0.8249538883340701
-        },
-        {
-            "x": 4.031328092300017,
-            "y": 6.052180289714568,
-            "heading": -0.5974599849723026,
-            "angularVelocity": 0.0000013435066364860128,
-            "velocityX": 3.1455510634328734,
-            "velocityY": 2.101461929458217,
-            "timestamp": 0.8642374068261687
-        },
-        {
-            "x": 4.154896411746533,
-            "y": 6.134733099182619,
-            "heading": -0.5974599321946334,
-            "angularVelocity": 0.0000013435066718879358,
-            "velocityX": 3.14555121816168,
-            "velocityY": 2.1014616978538454,
-            "timestamp": 0.9035209253182672
-        },
-        {
-            "x": 4.27846473654201,
-            "y": 6.217285900644126,
-            "heading": -0.5974598794169629,
-            "angularVelocity": 0.000001343506708988157,
-            "velocityX": 3.14555135432466,
-            "velocityY": 2.1014614940395124,
-            "timestamp": 0.9428044438103658
-        },
-        {
-            "x": 4.4020330660739395,
-            "y": 6.299838695015915,
-            "heading": -0.5974598266392908,
-            "angularVelocity": 0.0000013435067467217265,
-            "velocityX": 3.1455514748956417,
-            "velocityY": 2.1014613135638847,
-            "timestamp": 0.9820879623024643
-        },
-        {
-            "x": 4.525601399950297,
-            "y": 6.382391482884783,
-            "heading": -0.5974597738616171,
-            "angularVelocity": 0.0000013435067857429384,
-            "velocityX": 3.1455515854872873,
-            "velocityY": 2.101461148025713,
-            "timestamp": 1.0213714807945629
-        },
-        {
-            "x": 4.6491721527053596,
-            "y": 6.464940649963802,
-            "heading": -0.5974597210842042,
-            "angularVelocity": 0.0000013435001464771913,
-            "velocityX": 3.1456131603868194,
-            "velocityY": 2.101368977314248,
-            "timestamp": 1.0606549992866614
-        },
-        {
-            "x": 4.778837800088318,
-            "y": 6.537541722502267,
-            "heading": -0.5974596685786843,
-            "angularVelocity": 0.0000013365788503576918,
-            "velocityX": 3.300764604603488,
-            "velocityY": 1.8481305984081453,
-            "timestamp": 1.09993851777876
-        },
-        {
-            "x": 4.914297553492409,
-            "y": 6.59865432938236,
-            "heading": -0.5974596146272461,
-            "angularVelocity": 0.0000013733860988547744,
-            "velocityX": 3.4482591835906553,
-            "velocityY": 1.5556805812161723,
-            "timestamp": 1.1392220362708585
-        },
-        {
-            "x": 5.054536029054771,
-            "y": 6.647820017049671,
-            "heading": -0.5974595570647036,
-            "angularVelocity": 0.0000014653102566024163,
-            "velocityX": 3.5699061831898264,
-            "velocityY": 1.2515601848929776,
-            "timestamp": 1.178505554762957
-        },
-        {
-            "x": 5.198501873691458,
-            "y": 6.684670110841039,
-            "heading": -0.5974594930097576,
-            "angularVelocity": 0.0000016305806703071774,
-            "velocityX": 3.6647899720507366,
-            "velocityY": 0.9380548180449542,
-            "timestamp": 1.2177890732550556
-        },
-        {
-            "x": 5.345115761176417,
-            "y": 6.7089283098414425,
-            "heading": -0.5974594180971905,
-            "angularVelocity": 0.0000019069719319939646,
-            "velocityX": 3.7321984667555648,
-            "velocityY": 0.6175159438731035,
-            "timestamp": 1.2570725917471541
-        },
-        {
-            "x": 5.493278503417969,
-            "y": 6.720412731170654,
-            "heading": -0.5974593280342978,
-            "angularVelocity": 0.0000022926381384477543,
-            "velocityX": 3.7716260642834416,
-            "velocityY": 0.2923470648746024,
-            "timestamp": 1.2963561102392527
-        },
-        {
-            "x": 5.617528419554277,
-            "y": 6.721027319451783,
-            "heading": -0.5974592417476945,
-            "angularVelocity": 0.0000026270679336343826,
-            "velocityX": 3.7828927995748978,
-            "velocityY": 0.018711655155499447,
-            "timestamp": 1.3292013222818118
-        },
-        {
-            "x": 5.7414972044827435,
-            "y": 6.712651079745108,
-            "heading": -0.5974591685782128,
-            "angularVelocity": 0.0000022277061710620633,
-            "velocityX": 3.7743335244066216,
-            "velocityY": -0.25502163589934396,
-            "timestamp": 1.3620465343243708
-        },
-        {
-            "x": 5.864535126960113,
-            "y": 6.695327921921503,
-            "heading": -0.597459104000633,
-            "angularVelocity": 0.000001966118527479946,
-            "velocityX": 3.745992637160019,
-            "velocityY": -0.5274180541456184,
-            "timestamp": 1.39489174636693
-        },
-        {
-            "x": 5.985997338800097,
-            "y": 6.669148654285341,
-            "heading": -0.5974590451258569,
-            "angularVelocity": 0.0000017924918823644274,
-            "velocityX": 3.6980188065957362,
-            "velocityY": -0.7970497374814797,
-            "timestamp": 1.427736958409489
-        },
-        {
-            "x": 6.105247258928482,
-            "y": 6.634250515985412,
-            "heading": -0.5974589899613981,
-            "angularVelocity": 0.0000016795281661896423,
-            "velocityX": 3.6306637318675383,
-            "velocityY": -1.062503059946642,
-            "timestamp": 1.460582170452048
-        },
-        {
-            "x": 6.221659925637703,
-            "y": 6.5908164867561565,
-            "heading": -0.5974589370314296,
-            "angularVelocity": 0.0000016114972403778573,
-            "velocityX": 3.544281174328253,
-            "velocityY": -1.3223854110871922,
-            "timestamp": 1.4934273824946072
-        },
-        {
-            "x": 6.334625365293315,
-            "y": 6.539074496952364,
-            "heading": -0.5974588851619594,
-            "angularVelocity": 0.000001579209477235118,
-            "velocityX": 3.439327458421827,
-            "velocityY": -1.5753282316046957,
-            "timestamp": 1.5262725945371662
-        },
-        {
-            "x": 6.443562887659525,
-            "y": 6.4793163144818955,
-            "heading": -0.5974588333468978,
-            "angularVelocity": 0.000001577552964572491,
-            "velocityX": 3.3166941417319027,
-            "velocityY": -1.819387933752184,
-            "timestamp": 1.5591178065797253
-        },
-        {
-            "x": 6.552407332287041,
-            "y": 6.419388766569548,
-            "heading": -0.5974587815290724,
-            "angularVelocity": 0.0000015776371105880298,
-            "velocityX": 3.313860312014723,
-            "velocityY": -1.82454440649205,
-            "timestamp": 1.5919630186222844
-        },
-        {
-            "x": 6.661251765496281,
-            "y": 6.359461197918566,
-            "heading": -0.5974587297112393,
-            "angularVelocity": 0.0000015776373457032493,
-            "velocityX": 3.3138599643757076,
-            "velocityY": -1.8245450378971961,
-            "timestamp": 1.6248082306648435
-        },
-        {
-            "x": 6.7700961954772865,
-            "y": 6.299533623404254,
-            "heading": -0.5974586778933993,
-            "angularVelocity": 0.0000015776375522202912,
-            "velocityX": 3.3138598660894165,
-            "velocityY": -1.8245452164111537,
-            "timestamp": 1.6576534427074026
-        },
-        {
-            "x": 6.878940549868732,
-            "y": 6.239605911600315,
-            "heading": -0.5974586260742238,
-            "angularVelocity": 0.0000015776782163840074,
-            "velocityX": 3.313857564701912,
-            "velocityY": -1.8245493963088628,
-            "timestamp": 1.6904986547499616
-        },
-        {
-            "x": 6.983465419726436,
-            "y": 6.181138244955353,
-            "heading": -0.5831453971579332,
-            "angularVelocity": 0.4357782466962171,
-            "velocityX": 3.182347238991671,
-            "velocityY": -1.7800970981449478,
-            "timestamp": 1.7233438667925207
-        },
-        {
-            "x": 7.08190934617205,
-            "y": 6.126284607678917,
-            "heading": -0.5460039664027865,
-            "angularVelocity": 1.1308019782912204,
-            "velocityX": 2.997207821891462,
-            "velocityY": -1.67006494600528,
-            "timestamp": 1.7561890788350798
-        },
-        {
-            "x": 7.174234078294548,
-            "y": 6.075101338854014,
-            "heading": -0.4864629486170815,
-            "angularVelocity": 1.8127761729340726,
-            "velocityX": 2.8109038237566843,
-            "velocityY": -1.5583175032799648,
-            "timestamp": 1.7890342908776389
-        },
-        {
-            "x": 7.258956033298187,
-            "y": 6.028257642494675,
-            "heading": -0.41936488473987615,
-            "angularVelocity": 2.0428567728619895,
-            "velocityX": 2.5794309043853687,
-            "velocityY": -1.4261955836547433,
-            "timestamp": 1.821879502920198
-        },
-        {
-            "x": 7.335844716602702,
-            "y": 5.985834491701992,
-            "heading": -0.35186229403264746,
-            "angularVelocity": 2.0551729311350044,
-            "velocityX": 2.3409403843979617,
-            "velocityY": -1.2916083701234404,
-            "timestamp": 1.854724714962757
-        },
-        {
-            "x": 7.404915151007572,
-            "y": 5.947794038432469,
-            "heading": -0.2870870788105502,
-            "angularVelocity": 1.9721356993583767,
-            "velocityX": 2.1029072461275145,
-            "velocityY": -1.1581734719873862,
-            "timestamp": 1.8875699270053161
-        },
-        {
-            "x": 7.466209510665893,
-            "y": 5.914092129035952,
-            "heading": -0.22688750133470567,
-            "angularVelocity": 1.8328265744751362,
-            "velocityX": 1.866158135283738,
-            "velocityY": -1.0260828687243908,
-            "timestamp": 1.9204151390478752
-        },
-        {
-            "x": 7.519766848297347,
-            "y": 5.8846886900937605,
-            "heading": -0.17246123732016092,
-            "angularVelocity": 1.657053208974533,
-            "velocityX": 1.6305980172109753,
-            "velocityY": -0.8952123342686217,
-            "timestamp": 1.9532603510904343
-        },
-        {
-            "x": 7.565618503926309,
-            "y": 5.859549729252428,
-            "heading": -0.12462132977080041,
-            "angularVelocity": 1.4565260680124767,
-            "velocityX": 1.395992072438578,
-            "velocityY": -0.7653767254946843,
-            "timestamp": 1.9861055631329934
-        },
-        {
-            "x": 7.603788954978725,
-            "y": 5.838647308998552,
-            "heading": -0.08394075693399232,
-            "angularVelocity": 1.2385541242393951,
-            "velocityX": 1.1621313634075436,
-            "velocityY": -0.6363916977236806,
-            "timestamp": 2.0189507751755524
-        },
-        {
-            "x": 7.634297586862898,
-            "y": 5.821958675739452,
-            "heading": -0.050840342274680116,
-            "angularVelocity": 1.0077698574891136,
-            "velocityX": 0.928860859374263,
-            "velocityY": -0.5080994221459559,
-            "timestamp": 2.0517959872181115
-        },
-        {
-            "x": 7.657160251333572,
-            "y": 5.809465047264352,
-            "heading": -0.025644988874275507,
-            "angularVelocity": 0.7670936442051631,
-            "velocityX": 0.6960729752956952,
-            "velocityY": -0.3803789867120345,
-            "timestamp": 2.0846411992606706
-        },
-        {
-            "x": 7.672390418071531,
-            "y": 5.801150340997017,
-            "heading": -0.00862072816733467,
-            "angularVelocity": 0.5183178810016766,
-            "velocityX": 0.46369518693407513,
-            "velocityY": -0.2531481987862983,
-            "timestamp": 2.1174864113032297
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 8.329701949714689e-33,
-            "angularVelocity": 0.26246529193262896,
-            "velocityX": 0.2316800975013136,
-            "velocityY": -0.12636060901771562,
-            "timestamp": 2.1503316233457888
-        },
-        {
-            "x": 7.68,
-            "y": 5.797,
-            "heading": 5.789840595031671e-40,
-            "angularVelocity": 1.2947513524749855e-30,
-            "velocityX": 9.16355000594536e-32,
-            "velocityY": -1.9486684829089695e-31,
-            "timestamp": 2.183176835388348
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W3 To C3.traj b/src/main/deploy/choreo/W3 To C3.traj
deleted file mode 100644
index ea265506..00000000
--- a/src/main/deploy/choreo/W3 To C3.traj	
+++ /dev/null
@@ -1,310 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 1.9823938961383557e-29,
-            "timestamp": 0
-        },
-        {
-            "x": 2.312764146242865,
-            "y": 4.398513079462153,
-            "heading": -0.39804543849617763,
-            "angularVelocity": -0.11524733034669515,
-            "velocityX": 0.18284072142228638,
-            "velocityY": 0.5516827447028667,
-            "timestamp": 0.06981019405093988
-        },
-        {
-            "x": 2.339895425931373,
-            "y": 4.474992910905513,
-            "heading": -0.4126392714825622,
-            "angularVelocity": -0.20905017072795415,
-            "velocityX": 0.3886435220157258,
-            "velocityY": 1.0955395910772,
-            "timestamp": 0.13962038810187977
-        },
-        {
-            "x": 2.3838571031189573,
-            "y": 4.588440455462814,
-            "heading": -0.43142102712692415,
-            "angularVelocity": -0.2690403013169746,
-            "velocityX": 0.6297314852832604,
-            "velocityY": 1.6250856497386854,
-            "timestamp": 0.20943058215281965
-        },
-        {
-            "x": 2.448821264845437,
-            "y": 4.736675156040894,
-            "heading": -0.45019318323311186,
-            "angularVelocity": -0.26890279219946406,
-            "velocityX": 0.9305827409539079,
-            "velocityY": 2.1233961972720627,
-            "timestamp": 0.27924077620375953
-        },
-        {
-            "x": 2.542623225560298,
-            "y": 4.913429983875953,
-            "heading": -0.4600490129319591,
-            "angularVelocity": -0.14118037962391586,
-            "velocityX": 1.343671393412285,
-            "velocityY": 2.5319343433823196,
-            "timestamp": 0.34905097025469944
-        },
-        {
-            "x": 2.675927389267578,
-            "y": 5.095797559489528,
-            "heading": -0.44245218860567326,
-            "angularVelocity": 0.25206668688824874,
-            "velocityX": 1.9095228930350086,
-            "velocityY": 2.6123344605070336,
-            "timestamp": 0.41886116430563936
-        },
-        {
-            "x": 2.83772713712192,
-            "y": 5.250967901017311,
-            "heading": -0.38879601501351296,
-            "angularVelocity": 0.7686008371275667,
-            "velocityX": 2.3177094699953265,
-            "velocityY": 2.2227461710524636,
-            "timestamp": 0.48867135835657927
-        },
-        {
-            "x": 3.013235940099558,
-            "y": 5.368218424760011,
-            "heading": -0.31718431053283597,
-            "angularVelocity": 1.0258058360734288,
-            "velocityX": 2.514085591121856,
-            "velocityY": 1.6795616361728827,
-            "timestamp": 0.5584815524075192
-        },
-        {
-            "x": 3.1945050869972476,
-            "y": 5.445323486359522,
-            "heading": -0.23653218942173773,
-            "angularVelocity": 1.1553057859494003,
-            "velocityX": 2.5965999573717062,
-            "velocityY": 1.1044957351381008,
-            "timestamp": 0.6282917464584591
-        },
-        {
-            "x": 3.377225953863421,
-            "y": 5.481834951968023,
-            "heading": -0.15180568644047004,
-            "angularVelocity": 1.2136694952369833,
-            "velocityX": 2.6173952006491676,
-            "velocityY": 0.523010515923758,
-            "timestamp": 0.698101940509399
-        },
-        {
-            "x": 3.558790922164917,
-            "y": 5.477728366851807,
-            "heading": -0.06601521216801902,
-            "angularVelocity": 1.2289104110775417,
-            "velocityX": 2.6008374674955865,
-            "velocityY": -0.05882500648124433,
-            "timestamp": 0.7679121345603389
-        },
-        {
-            "x": 3.7207755893743903,
-            "y": 5.440802283745667,
-            "heading": 0.01087618717506099,
-            "angularVelocity": 1.2168377614801031,
-            "velocityX": 2.5634734381227786,
-            "velocityY": -0.5843703286511746,
-            "timestamp": 0.8311016588990232
-        },
-        {
-            "x": 3.8800031263189014,
-            "y": 5.370704697701765,
-            "heading": 0.08523277253593173,
-            "angularVelocity": 1.1767232961398173,
-            "velocityX": 2.519840726939224,
-            "velocityY": -1.1093228945227975,
-            "timestamp": 0.8942911832377075
-        },
-        {
-            "x": 4.035880089753818,
-            "y": 5.267514783368049,
-            "heading": 0.15438353932229767,
-            "angularVelocity": 1.0943390935099226,
-            "velocityX": 2.4668165343191313,
-            "velocityY": -1.633022489291642,
-            "timestamp": 0.9574807075763918
-        },
-        {
-            "x": 4.187424190703861,
-            "y": 5.131425904894565,
-            "heading": 0.2138534475851711,
-            "angularVelocity": 0.9411355581283213,
-            "velocityX": 2.3982472179488545,
-            "velocityY": -2.153662017503815,
-            "timestamp": 1.0206702319150762
-        },
-        {
-            "x": 4.332733928007212,
-            "y": 4.96304651892829,
-            "heading": 0.25476003945729764,
-            "angularVelocity": 0.6473635034690916,
-            "velocityX": 2.2995858699928062,
-            "velocityY": -2.664672471059999,
-            "timestamp": 1.0838597562537606
-        },
-        {
-            "x": 4.46716744223408,
-            "y": 4.765388489141315,
-            "heading": 0.2547605945579538,
-            "angularVelocity": 0.000008784690993212257,
-            "velocityX": 2.12746520302083,
-            "velocityY": -3.1280189533989655,
-            "timestamp": 1.147049280592445
-        },
-        {
-            "x": 4.612743650855858,
-            "y": 4.575786978724626,
-            "heading": 0.2547605407972418,
-            "angularVelocity": -8.507852266556338e-7,
-            "velocityX": 2.303802887345736,
-            "velocityY": -3.0005212478052368,
-            "timestamp": 1.2102388049311295
-        },
-        {
-            "x": 4.782527461386278,
-            "y": 4.409030186652972,
-            "heading": 0.25208197397278675,
-            "angularVelocity": -0.04238941267495421,
-            "velocityX": 2.6868980627285057,
-            "velocityY": -2.638994260793031,
-            "timestamp": 1.2734283292698139
-        },
-        {
-            "x": 4.970403968588328,
-            "y": 4.270066434839271,
-            "heading": 0.23891743895373094,
-            "angularVelocity": -0.2083341369570075,
-            "velocityX": 2.9732223682415455,
-            "velocityY": -2.1991580609051504,
-            "timestamp": 1.3366178536084983
-        },
-        {
-            "x": 5.173968377386639,
-            "y": 4.1604195633547185,
-            "heading": 0.21971651393307054,
-            "angularVelocity": -0.3038624711945638,
-            "velocityX": 3.2214898106793903,
-            "velocityY": -1.7352064702582655,
-            "timestamp": 1.3998073779471827
-        },
-        {
-            "x": 5.390789031982422,
-            "y": 4.081310272216797,
-            "heading": 0.19899217158132915,
-            "angularVelocity": -0.3279711719205329,
-            "velocityX": 3.4312753081359326,
-            "velocityY": -1.25193680386463,
-            "timestamp": 1.462996902285867
-        },
-        {
-            "x": 5.681530534379024,
-            "y": 4.031634954440038,
-            "heading": 0.1807325834147537,
-            "angularVelocity": -0.22892527550150313,
-            "velocityX": 3.645102941474752,
-            "velocityY": -0.6227925681552634,
-            "timestamp": 1.542759122191511
-        },
-        {
-            "x": 5.983259555492767,
-            "y": 4.033604453850848,
-            "heading": 0.1807316659794543,
-            "angularVelocity": -0.000011502133581259988,
-            "velocityX": 3.7828563632073102,
-            "velocityY": 0.024692133859281144,
-            "timestamp": 1.6225213420971547
-        },
-        {
-            "x": 6.284539191755923,
-            "y": 4.050188554583329,
-            "heading": 0.18073165818528908,
-            "angularVelocity": -9.771750653524246e-8,
-            "velocityX": 3.777222306745724,
-            "velocityY": 0.20791924738433482,
-            "timestamp": 1.7022835620027985
-        },
-        {
-            "x": 6.585818825161056,
-            "y": 4.0667727072366,
-            "heading": 0.18073165039112304,
-            "angularVelocity": -9.77175162921636e-8,
-            "velocityX": 3.777222270913928,
-            "velocityY": 0.20791989832899174,
-            "timestamp": 1.7820457819084423
-        },
-        {
-            "x": 6.88709845856161,
-            "y": 4.083356859971397,
-            "heading": 0.180731642596831,
-            "angularVelocity": -9.771909620772691e-8,
-            "velocityX": 3.777222270856537,
-            "velocityY": 0.20791989935110283,
-            "timestamp": 1.8618080018140861
-        },
-        {
-            "x": 7.15139892307148,
-            "y": 4.097904823415219,
-            "heading": 0.12048670978727856,
-            "angularVelocity": -0.7553066211144209,
-            "velocityX": 3.313604671767403,
-            "velocityY": 0.1823916568650699,
-            "timestamp": 1.94157022171973
-        },
-        {
-            "x": 7.3628393316310445,
-            "y": 4.109542993215659,
-            "heading": 0.07229126938267637,
-            "angularVelocity": -0.6042389550038586,
-            "velocityX": 2.650884200686756,
-            "velocityY": 0.1459108060709806,
-            "timestamp": 2.0213324416253737
-        },
-        {
-            "x": 7.521419659615098,
-            "y": 4.118271521022388,
-            "heading": 0.03614542857914898,
-            "angularVelocity": -0.4531699449471921,
-            "velocityX": 1.98816342087339,
-            "velocityY": 0.10943185654883955,
-            "timestamp": 2.1010946615310178
-        },
-        {
-            "x": 7.627139886542953,
-            "y": 4.12409050689369,
-            "heading": 0.012048510432709781,
-            "angularVelocity": -0.302109421915929,
-            "velocityX": 1.3254423842882344,
-            "velocityY": 0.07295416148382058,
-            "timestamp": 2.180856881436662
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 2.791295127527068e-32,
-            "angularVelocity": -0.15105535486544666,
-            "velocityX": 0.6627211920578459,
-            "velocityY": 0.03647708288149429,
-            "timestamp": 2.260619101342306
-        },
-        {
-            "x": 7.68,
-            "y": 4.127,
-            "heading": 0,
-            "angularVelocity": -1.938584239537671e-30,
-            "velocityX": -1.689352589413745e-32,
-            "velocityY": 0,
-            "timestamp": 2.34038132124795
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W3 To C4.traj b/src/main/deploy/choreo/W3 To C4.traj
deleted file mode 100644
index ce59bb95..00000000
--- a/src/main/deploy/choreo/W3 To C4.traj	
+++ /dev/null
@@ -1,625 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.298402468716654,
-            "y": 4.3526843009931255,
-            "heading": -0.37533408415570846,
-            "angularVelocity": 0.4620245693037669,
-            "velocityX": -0.050327487964169394,
-            "velocityY": -0.2304685720750862,
-            "timestamp": 0.03174271850172795
-        },
-        {
-            "x": 2.2952744131725273,
-            "y": 4.338010072410149,
-            "heading": -0.34621912169362346,
-            "angularVelocity": 0.9172170449263883,
-            "velocityX": -0.09854403440450425,
-            "velocityY": -0.462286447903903,
-            "timestamp": 0.0634854370034559
-        },
-        {
-            "x": 2.2907034035575107,
-            "y": 4.3159283160610205,
-            "heading": -0.3028943022400045,
-            "angularVelocity": 1.364874260887912,
-            "velocityX": -0.14400183194038138,
-            "velocityY": -0.6956479278208441,
-            "timestamp": 0.09522815550518385
-        },
-        {
-            "x": 2.2847999846220546,
-            "y": 4.286378308280002,
-            "heading": -0.24568561151458213,
-            "angularVelocity": 1.8022618548662834,
-            "velocityX": -0.1859771063759033,
-            "velocityY": -0.9309224028625668,
-            "timestamp": 0.1269708740069118
-        },
-        {
-            "x": 2.2777013130983144,
-            "y": 4.249280679670162,
-            "heading": -0.1750916915555501,
-            "angularVelocity": 2.223940585151498,
-            "velocityX": -0.22363149278956135,
-            "velocityY": -1.1686972748670326,
-            "timestamp": 0.15871359250863973
-        },
-        {
-            "x": 2.2695785209744463,
-            "y": 4.204529290238701,
-            "heading": -0.09189311777806415,
-            "angularVelocity": 2.6210286233977427,
-            "velocityX": -0.25589465890975394,
-            "velocityY": -1.409815905623355,
-            "timestamp": 0.19045631101036767
-        },
-        {
-            "x": 2.2606569711878857,
-            "y": 4.151982062664549,
-            "heading": 0.002706319559794192,
-            "angularVelocity": 2.9801933105606198,
-            "velocityX": -0.2810581515277304,
-            "velocityY": -1.6554104391308149,
-            "timestamp": 0.2221990295120956
-        },
-        {
-            "x": 2.251280347316294,
-            "y": 4.091451352956859,
-            "heading": 0.10685098530745366,
-            "angularVelocity": 3.2808993893194844,
-            "velocityX": -0.2953944814487593,
-            "velocityY": -1.90691637530654,
-            "timestamp": 0.2539417480138236
-        },
-        {
-            "x": 2.242133674900212,
-            "y": 4.022709367456616,
-            "heading": 0.21739538615105536,
-            "angularVelocity": 3.4825120865934673,
-            "velocityX": -0.28815025454054716,
-            "velocityY": -2.165598560706175,
-            "timestamp": 0.2856844665155515
-        },
-        {
-            "x": 2.2351422077578347,
-            "y": 3.945850682832356,
-            "heading": 0.32707167243976176,
-            "angularVelocity": 3.455163623831899,
-            "velocityX": -0.22025420229828702,
-            "velocityY": -2.4213012700873704,
-            "timestamp": 0.31742718501727946
-        },
-        {
-            "x": 2.2344515310594897,
-            "y": 3.864378163543378,
-            "heading": 0.4256089749961829,
-            "angularVelocity": 3.1042490122910227,
-            "velocityX": -0.021758586880562782,
-            "velocityY": -2.566652231898267,
-            "timestamp": 0.3491699035190074
-        },
-        {
-            "x": 2.2416429003575074,
-            "y": 3.7806290345563274,
-            "heading": 0.5177286971400864,
-            "angularVelocity": 2.9020741288711216,
-            "velocityX": 0.22655177746121863,
-            "velocityY": -2.638372922674897,
-            "timestamp": 0.38091262202073534
-        },
-        {
-            "x": 2.256915898193716,
-            "y": 3.695399323053411,
-            "heading": 0.6036240465401915,
-            "angularVelocity": 2.705985922265222,
-            "velocityX": 0.4811496480800017,
-            "velocityY": -2.6850161399464656,
-            "timestamp": 0.4126553405224633
-        },
-        {
-            "x": 2.280071045497703,
-            "y": 3.6084777900686467,
-            "heading": 0.6811513337134051,
-            "angularVelocity": 2.4423644486842004,
-            "velocityX": 0.729463272111561,
-            "velocityY": -2.7383140791811433,
-            "timestamp": 0.4443980590241912
-        },
-        {
-            "x": 2.3096855944046597,
-            "y": 3.5244760350530098,
-            "heading": 0.76088689172136,
-            "angularVelocity": 2.511932240574098,
-            "velocityX": 0.932955660534971,
-            "velocityY": -2.6463314731869354,
-            "timestamp": 0.47614077752591916
-        },
-        {
-            "x": 2.3348942368332115,
-            "y": 3.446420745412462,
-            "heading": 0.8559069862879931,
-            "angularVelocity": 2.993445396350149,
-            "velocityX": 0.7941551202420774,
-            "velocityY": -2.458998262429792,
-            "timestamp": 0.5078834960276472
-        },
-        {
-            "x": 2.355075569752848,
-            "y": 3.373868279566975,
-            "heading": 0.9657696902827837,
-            "angularVelocity": 3.4610363944982643,
-            "velocityX": 0.6357783413709821,
-            "velocityY": -2.285641220097014,
-            "timestamp": 0.5396262145293751
-        },
-        {
-            "x": 2.370025604730125,
-            "y": 3.306495756130267,
-            "heading": 1.091029644988018,
-            "angularVelocity": 3.9461004166489144,
-            "velocityX": 0.4709752561508733,
-            "velocityY": -2.1224560030370347,
-            "timestamp": 0.571368933031103
-        },
-        {
-            "x": 2.3800631598495494,
-            "y": 3.2441375159647747,
-            "heading": 1.2334110423596254,
-            "angularVelocity": 4.485482154399005,
-            "velocityX": 0.31621598883778346,
-            "velocityY": -1.9644895934825695,
-            "timestamp": 0.603111651532831
-        },
-        {
-            "x": 2.3890320547539976,
-            "y": 3.1882487557001293,
-            "heading": 1.3963738258608078,
-            "angularVelocity": 5.133863487221998,
-            "velocityX": 0.28254967840764567,
-            "velocityY": -1.760679705539431,
-            "timestamp": 0.6348543700345589
-        },
-        {
-            "x": 2.399614951757013,
-            "y": 3.1248820965808815,
-            "heading": 1.5465533770663946,
-            "angularVelocity": 4.7311496397956585,
-            "velocityX": 0.33339605120587323,
-            "velocityY": -1.9962581061165,
-            "timestamp": 0.6665970885362869
-        },
-        {
-            "x": 2.4156041612276775,
-            "y": 3.056078363317597,
-            "heading": 1.6846738705269053,
-            "angularVelocity": 4.351249671731503,
-            "velocityX": 0.5037126694046519,
-            "velocityY": -2.167543818263053,
-            "timestamp": 0.6983398070380148
-        },
-        {
-            "x": 2.4386535219180887,
-            "y": 2.9838461830329104,
-            "heading": 1.8122491249375776,
-            "angularVelocity": 4.019039969866676,
-            "velocityX": 0.7261306459671242,
-            "velocityY": -2.275551171861821,
-            "timestamp": 0.7300825255397427
-        },
-        {
-            "x": 2.4695800740474128,
-            "y": 2.910121142766613,
-            "heading": 1.9308538821800434,
-            "angularVelocity": 3.736439814882526,
-            "velocityX": 0.9742880757878827,
-            "velocityY": -2.3225811696715355,
-            "timestamp": 0.7618252440414707
-        },
-        {
-            "x": 2.508646228883482,
-            "y": 2.8366969767650785,
-            "heading": 2.0420489888957944,
-            "angularVelocity": 3.503011461028351,
-            "velocityX": 1.2307123233299608,
-            "velocityY": -2.313102641084062,
-            "timestamp": 0.7935679625431986
-        },
-        {
-            "x": 2.555734299118471,
-            "y": 2.7651139971749186,
-            "heading": 2.147169748045839,
-            "angularVelocity": 3.3116495408016253,
-            "velocityX": 1.4834290337302587,
-            "velocityY": -2.255099215470886,
-            "timestamp": 0.8253106810449266
-        },
-        {
-            "x": 2.6104869842529297,
-            "y": 2.6966493129730225,
-            "heading": 2.247054267099527,
-            "angularVelocity": 3.1466907614812087,
-            "velocityX": 1.7248896036260801,
-            "velocityY": -2.1568626580665695,
-            "timestamp": 0.8570533995466545
-        },
-        {
-            "x": 2.751018773357776,
-            "y": 2.5737681266542305,
-            "heading": 2.423101617636397,
-            "angularVelocity": 2.7414576820608,
-            "velocityX": 2.188399607493837,
-            "velocityY": -1.9135395743649004,
-            "timestamp": 0.9212700940770386
-        },
-        {
-            "x": 2.920945282929504,
-            "y": 2.4615612619997704,
-            "heading": 2.5486951706813996,
-            "angularVelocity": 1.9557772937936253,
-            "velocityX": 2.646142265876451,
-            "velocityY": -1.7473161064272718,
-            "timestamp": 0.9854867886074228
-        },
-        {
-            "x": 3.118708267205298,
-            "y": 2.3583606370011725,
-            "heading": 2.6122511958810777,
-            "angularVelocity": 0.9897118757741349,
-            "velocityX": 3.079619493373657,
-            "velocityY": -1.6070684695514719,
-            "timestamp": 1.049703483137807
-        },
-        {
-            "x": 3.3434611246912245,
-            "y": 2.26616504003377,
-            "heading": 2.6122519169792926,
-            "angularVelocity": 0.000011229139401682067,
-            "velocityX": 3.499913209945475,
-            "velocityY": -1.4356951512629466,
-            "timestamp": 1.113920177668191
-        },
-        {
-            "x": 3.5722970045795748,
-            "y": 2.184629282343257,
-            "heading": 2.6122522165022306,
-            "angularVelocity": 0.000004664253435080619,
-            "velocityX": 3.5634951559220656,
-            "velocityY": -1.269697207039109,
-            "timestamp": 1.1781368721985752
-        },
-        {
-            "x": 3.80113295504209,
-            "y": 2.103093722723815,
-            "heading": 2.6122525160244625,
-            "angularVelocity": 0.000004664242440601037,
-            "velocityX": 3.56349625492233,
-            "velocityY": -1.2696941226220015,
-            "timestamp": 1.2423535667289594
-        },
-        {
-            "x": 4.029968923086795,
-            "y": 2.0215582124500746,
-            "heading": 2.612252815546594,
-            "angularVelocity": 0.00000466424087155042,
-            "velocityX": 3.563496528717025,
-            "velocityY": -1.2696933541971844,
-            "timestamp": 1.3065702612593435
-        },
-        {
-            "x": 4.258804909490432,
-            "y": 1.940022753702107,
-            "heading": 2.6122531150686714,
-            "angularVelocity": 0.000004664240033500949,
-            "velocityX": 3.5634968146073587,
-            "velocityY": -1.2696925518237052,
-            "timestamp": 1.3707869557897276
-        },
-        {
-            "x": 4.487640916105967,
-            "y": 1.8584873516805105,
-            "heading": 2.6122534145907514,
-            "angularVelocity": 0.000004664240074560689,
-            "velocityX": 3.563497129352575,
-            "velocityY": -1.2696916684650783,
-            "timestamp": 1.4350036503201118
-        },
-        {
-            "x": 4.716476945604857,
-            "y": 1.7769520138830965,
-            "heading": 2.6122537141129194,
-            "angularVelocity": 0.000004664241438058459,
-            "velocityX": 3.5634974856984654,
-            "velocityY": -1.2696906683484759,
-            "timestamp": 1.499220344850496
-        },
-        {
-            "x": 4.945312999982612,
-            "y": 1.6954167459104819,
-            "heading": 2.612254013635151,
-            "angularVelocity": 0.000004664242435524347,
-            "velocityX": 3.563497873118986,
-            "velocityY": -1.269689581017522,
-            "timestamp": 1.56343703938088
-        },
-        {
-            "x": 5.174149135873993,
-            "y": 1.613881706713074,
-            "heading": 2.6122543131567744,
-            "angularVelocity": 0.0000046642329655920085,
-            "velocityX": 3.563499142471556,
-            "velocityY": -1.269686018467187,
-            "timestamp": 1.6276537339112642
-        },
-        {
-            "x": 5.406651475687904,
-            "y": 1.5434785463415874,
-            "heading": 2.61225458420666,
-            "angularVelocity": 0.000004220863245612629,
-            "velocityX": 3.6205902766281897,
-            "velocityY": -1.096337344772168,
-            "timestamp": 1.6918704284416484
-        },
-        {
-            "x": 5.646764755249023,
-            "y": 1.5066063404083252,
-            "heading": 2.6122548161665473,
-            "angularVelocity": 0.0000036121430551488755,
-            "velocityX": 3.7391099202016678,
-            "velocityY": -0.5741841152508433,
-            "timestamp": 1.7560871229720325
-        },
-        {
-            "x": 5.787436421990979,
-            "y": 1.496717214982701,
-            "heading": 2.612254832075579,
-            "angularVelocity": 4.2677202387530557e-7,
-            "velocityX": 3.773625708136921,
-            "velocityY": -0.26528339929021244,
-            "timestamp": 1.7933647115888978
-        },
-        {
-            "x": 5.926771623718837,
-            "y": 1.4982765413849686,
-            "heading": 2.6080354125638485,
-            "angularVelocity": -0.11318917527356848,
-            "velocityX": 3.7377740057150746,
-            "velocityY": 0.04183013065287323,
-            "timestamp": 1.830642300205763
-        },
-        {
-            "x": 6.064421169286406,
-            "y": 1.511276136415188,
-            "heading": 2.60121232475526,
-            "angularVelocity": -0.18303458087688948,
-            "velocityX": 3.6925549820916683,
-            "velocityY": 0.34872413996054913,
-            "timestamp": 1.8679198888226283
-        },
-        {
-            "x": 6.200023778520788,
-            "y": 1.5356760041928015,
-            "heading": 2.5932728220304684,
-            "angularVelocity": -0.2129832700927332,
-            "velocityX": 3.6376443398227996,
-            "velocityY": 0.6545452289951588,
-            "timestamp": 1.9051974774394935
-        },
-        {
-            "x": 6.333175231231244,
-            "y": 1.5714002898334831,
-            "heading": 2.5855391112934316,
-            "angularVelocity": -0.20746274166290535,
-            "velocityX": 3.5718901798871894,
-            "velocityY": 0.9583314523868992,
-            "timestamp": 1.9424750660563588
-        },
-        {
-            "x": 6.463368734714764,
-            "y": 1.6183219287773098,
-            "heading": 2.5790588590244545,
-            "angularVelocity": -0.17383775371256402,
-            "velocityX": 3.4925409157130343,
-            "velocityY": 1.258709071181673,
-            "timestamp": 1.979752654673224
-        },
-        {
-            "x": 6.589861303526444,
-            "y": 1.6762129661852994,
-            "heading": 2.574283804517795,
-            "angularVelocity": -0.12809451157737914,
-            "velocityX": 3.393260495247066,
-            "velocityY": 1.5529716260079764,
-            "timestamp": 2.017030243290089
-        },
-        {
-            "x": 6.711340738368746,
-            "y": 1.7445488061219798,
-            "heading": 2.570091844488755,
-            "angularVelocity": -0.11245255352014773,
-            "velocityX": 3.258779318878531,
-            "velocityY": 1.8331614911851861,
-            "timestamp": 2.054307831906954
-        },
-        {
-            "x": 6.825305386557433,
-            "y": 1.8215960437325822,
-            "heading": 2.560973344137785,
-            "angularVelocity": -0.24461078866147912,
-            "velocityX": 3.057189384216965,
-            "velocityY": 2.0668514372666396,
-            "timestamp": 2.091585420523819
-        },
-        {
-            "x": 6.929260415482823,
-            "y": 1.9025407238929235,
-            "heading": 2.5356026772435865,
-            "angularVelocity": -0.680587662333947,
-            "velocityX": 2.788673645010342,
-            "velocityY": 2.171403332771371,
-            "timestamp": 2.128863009140684
-        },
-        {
-            "x": 7.023455735639986,
-            "y": 1.9838668818491878,
-            "heading": 2.488274688562586,
-            "angularVelocity": -1.2696097155701718,
-            "velocityX": 2.5268619471418052,
-            "velocityY": 2.1816367681967113,
-            "timestamp": 2.166140597757549
-        },
-        {
-            "x": 7.110101814938688,
-            "y": 2.060020595789651,
-            "heading": 2.4125158730705962,
-            "angularVelocity": -2.0322885224854694,
-            "velocityX": 2.3243477519224953,
-            "velocityY": 2.0428819772427436,
-            "timestamp": 2.2034181863744142
-        },
-        {
-            "x": 7.191835709933807,
-            "y": 2.1285351921147058,
-            "heading": 2.3080325133818067,
-            "angularVelocity": -2.8028465242925935,
-            "velocityX": 2.1925746280203615,
-            "velocityY": 1.8379567688575535,
-            "timestamp": 2.2406957749912793
-        },
-        {
-            "x": 7.274315415796274,
-            "y": 2.1867028423762775,
-            "heading": 2.1850264648931392,
-            "angularVelocity": -3.2997319046816447,
-            "velocityX": 2.2125815784434106,
-            "velocityY": 1.5603919786607532,
-            "timestamp": 2.2779733636081443
-        },
-        {
-            "x": 7.351412179448119,
-            "y": 2.2360068924730196,
-            "heading": 2.043511695780604,
-            "angularVelocity": -3.796242578000636,
-            "velocityX": 2.0681800114335913,
-            "velocityY": 1.322619083639869,
-            "timestamp": 2.3152509522250093
-        },
-        {
-            "x": 7.420172175937219,
-            "y": 2.2789013315216375,
-            "heading": 1.8838780266002726,
-            "angularVelocity": -4.282296014933475,
-            "velocityX": 1.8445398170951257,
-            "velocityY": 1.1506763350355667,
-            "timestamp": 2.3525285408418743
-        },
-        {
-            "x": 7.479537718839642,
-            "y": 2.3170603162912466,
-            "heading": 1.7061327210648876,
-            "angularVelocity": -4.768154597182533,
-            "velocityX": 1.592526370537947,
-            "velocityY": 1.0236441300375574,
-            "timestamp": 2.3898061294587394
-        },
-        {
-            "x": 7.529281054923766,
-            "y": 2.3515994880332096,
-            "heading": 1.5086439015102746,
-            "angularVelocity": -5.2977895535137876,
-            "velocityX": 1.3344032682848619,
-            "velocityY": 0.9265398601007353,
-            "timestamp": 2.4270837180756044
-        },
-        {
-            "x": 7.569719945861703,
-            "y": 2.383438536522646,
-            "heading": 1.288404786035775,
-            "angularVelocity": -5.908083748068905,
-            "velocityX": 1.0848043674059311,
-            "velocityY": 0.8541069760888933,
-            "timestamp": 2.4643613066924694
-        },
-        {
-            "x": 7.603202598509127,
-            "y": 2.4091886515491456,
-            "heading": 1.0408456353949866,
-            "angularVelocity": -6.640964714353526,
-            "velocityX": 0.8981979223912477,
-            "velocityY": 0.6907666504707783,
-            "timestamp": 2.5016388953093345
-        },
-        {
-            "x": 7.630132008026638,
-            "y": 2.427100499568469,
-            "heading": 0.7720308301798764,
-            "angularVelocity": -7.211164004677401,
-            "velocityX": 0.7224021326670985,
-            "velocityY": 0.48049910640464033,
-            "timestamp": 2.5389164839261995
-        },
-        {
-            "x": 7.649623319500903,
-            "y": 2.438227448776781,
-            "heading": 0.5254269726030665,
-            "angularVelocity": -6.615338242807932,
-            "velocityX": 0.5228694289911082,
-            "velocityY": 0.29848897477445496,
-            "timestamp": 2.5761940725430645
-        },
-        {
-            "x": 7.66290885295536,
-            "y": 2.4458359453096237,
-            "heading": 0.3183730749926382,
-            "angularVelocity": -5.554380132752273,
-            "velocityX": 0.3563946582222695,
-            "velocityY": 0.2041037742822349,
-            "timestamp": 2.6134716611599296
-        },
-        {
-            "x": 7.671732744074589,
-            "y": 2.451418069701132,
-            "heading": 0.15988099310288126,
-            "angularVelocity": -4.251672057404801,
-            "velocityX": 0.23670766931625417,
-            "velocityY": 0.14974478228408286,
-            "timestamp": 2.6507492497767946
-        },
-        {
-            "x": 7.677275184086734,
-            "y": 2.4551558590330185,
-            "heading": 0.05341064749493927,
-            "angularVelocity": -2.8561489505727446,
-            "velocityX": 0.14868021827027583,
-            "velocityY": 0.10026907508163654,
-            "timestamp": 2.6880268383936596
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 1.0247603884557974e-38,
-            "angularVelocity": -1.4327817189005392,
-            "velocityX": 0.07309528363731636,
-            "velocityY": 0.04947050051802475,
-            "timestamp": 2.7253044270105247
-        },
-        {
-            "x": 7.68,
-            "y": 2.457,
-            "heading": 0,
-            "angularVelocity": -2.6319896813971663e-37,
-            "velocityX": 5.227147746061896e-36,
-            "velocityY": -6.467829854732196e-35,
-            "timestamp": 2.7625820156273897
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W3 To C5.traj b/src/main/deploy/choreo/W3 To C5.traj
deleted file mode 100644
index 8826d851..00000000
--- a/src/main/deploy/choreo/W3 To C5.traj	
+++ /dev/null
@@ -1,571 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": -4.609910300520975e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.298808348352727,
-            "y": 4.35514296819253,
-            "heading": -0.3837399750205753,
-            "angularVelocity": 0.24884159467028183,
-            "velocityX": -0.047369219319970686,
-            "velocityY": -0.19307136062676852,
-            "timestamp": 0.02515666638336474
-        },
-        {
-            "x": 2.2965596540188837,
-            "y": 4.3453750993410925,
-            "heading": -0.37149402212882315,
-            "angularVelocity": 0.48678758564965857,
-            "velocityX": -0.08938761199815681,
-            "velocityY": -0.38828152755153583,
-            "timestamp": 0.05031333276672948
-        },
-        {
-            "x": 2.2934096466067952,
-            "y": 4.330641358670927,
-            "heading": -0.3535645425221108,
-            "angularVelocity": 0.7127128584321681,
-            "velocityX": -0.1252156133919033,
-            "velocityY": -0.5856793760205414,
-            "timestamp": 0.07546999915009422
-        },
-        {
-            "x": 2.289538880665139,
-            "y": 4.310886769515238,
-            "heading": -0.33028900557963653,
-            "angularVelocity": 0.9252234214095049,
-            "velocityX": -0.15386640990779057,
-            "velocityY": -0.7852625961900879,
-            "timestamp": 0.10062666553345896
-        },
-        {
-            "x": 2.2851573432175654,
-            "y": 4.286058471237428,
-            "heading": -0.3020481691352866,
-            "angularVelocity": 1.1225985197714665,
-            "velocityX": -0.17417003432819553,
-            "velocityY": -0.9869470739663532,
-            "timestamp": 0.1257833319168237
-        },
-        {
-            "x": 2.280510060472114,
-            "y": 4.25610900707676,
-            "heading": -0.26927550139535,
-            "angularVelocity": 1.3027428690475438,
-            "velocityX": -0.18473364771909595,
-            "velocityY": -1.1905180004483138,
-            "timestamp": 0.15093999830018845
-        },
-        {
-            "x": 2.2758836751536,
-            "y": 4.2210014881511375,
-            "heading": -0.23246752663964654,
-            "angularVelocity": 1.463149933889621,
-            "velocityX": -0.1839029562984313,
-            "velocityY": -1.3955552930033008,
-            "timestamp": 0.1760966646835532
-        },
-        {
-            "x": 2.271613693612169,
-            "y": 4.180717494993525,
-            "heading": -0.19219487185489256,
-            "angularVelocity": 1.6008740653882818,
-            "velocityX": -0.16973558723403512,
-            "velocityY": -1.6013247758554696,
-            "timestamp": 0.20125333106691795
-        },
-        {
-            "x": 2.268091592763015,
-            "y": 4.135268758759812,
-            "heading": -0.1491141249151469,
-            "angularVelocity": 1.7124982413501877,
-            "velocityX": -0.1400066604803655,
-            "velocityY": -1.8066279347634933,
-            "timestamp": 0.2264099974502827
-        },
-        {
-            "x": 2.2657701315686967,
-            "y": 4.084713672798125,
-            "heading": -0.10398137252966654,
-            "angularVelocity": 1.7940672940403972,
-            "velocityX": -0.09228015981695388,
-            "velocityY": -2.0096099058307106,
-            "timestamp": 0.25156666383364745
-        },
-        {
-            "x": 2.265163909208295,
-            "y": 4.029179243747037,
-            "heading": -0.057668550112221675,
-            "angularVelocity": 1.8409761337881332,
-            "velocityX": -0.02409788129965802,
-            "velocityY": -2.2075432493635576,
-            "timestamp": 0.2767233302170122
-        },
-        {
-            "x": 2.266840363517046,
-            "y": 3.9688874958283327,
-            "heading": -0.011179237419756825,
-            "angularVelocity": 1.8479917801512262,
-            "velocityX": 0.06664055893588815,
-            "velocityY": -2.3966509314037507,
-            "timestamp": 0.3018799966003769
-        },
-        {
-            "x": 2.2713096946462064,
-            "y": 3.9041248350083095,
-            "heading": 0.03446639703260334,
-            "angularVelocity": 1.8144548151476838,
-            "velocityX": 0.1776599117328043,
-            "velocityY": -2.574373719995273,
-            "timestamp": 0.3270366629837416
-        },
-        {
-            "x": 2.2787160490905882,
-            "y": 3.8349972418338227,
-            "heading": 0.07865637806724814,
-            "angularVelocity": 1.7565912892125506,
-            "velocityX": 0.29440921668697884,
-            "velocityY": -2.74788368701343,
-            "timestamp": 0.35219332936710634
-        },
-        {
-            "x": 2.289237241442295,
-            "y": 3.7616571782461574,
-            "heading": 0.12059430680114623,
-            "angularVelocity": 1.667070195025133,
-            "velocityX": 0.41822681079334173,
-            "velocityY": -2.915333155436007,
-            "timestamp": 0.37734999575047107
-        },
-        {
-            "x": 2.303092280593543,
-            "y": 3.684333317171128,
-            "heading": 0.15922184993862323,
-            "angularVelocity": 1.5354794052927534,
-            "velocityX": 0.550750204343837,
-            "velocityY": -3.073692670431113,
-            "timestamp": 0.4025066621338358
-        },
-        {
-            "x": 2.3205444941015814,
-            "y": 3.6033821039148743,
-            "heading": 0.19314125064776663,
-            "angularVelocity": 1.3483265307192482,
-            "velocityX": 0.6937411039317692,
-            "velocityY": -3.217883165544707,
-            "timestamp": 0.4276633285172005
-        },
-        {
-            "x": 2.341904587688066,
-            "y": 3.5193842561187316,
-            "heading": 0.22066827104785883,
-            "angularVelocity": 1.0942236932591152,
-            "velocityX": 0.8490828339882706,
-            "velocityY": -3.3389896147642064,
-            "timestamp": 0.45281999490056524
-        },
-        {
-            "x": 2.3675851289681646,
-            "y": 3.4337423994061407,
-            "heading": 0.23918679984335672,
-            "angularVelocity": 0.7361280907928144,
-            "velocityX": 1.0208244959308244,
-            "velocityY": -3.40434044032252,
-            "timestamp": 0.47797666128392996
-        },
-        {
-            "x": 2.3978885412195137,
-            "y": 3.3469640370766824,
-            "heading": 0.24945072521405584,
-            "angularVelocity": 0.40800021808479553,
-            "velocityX": 1.2045877537807594,
-            "velocityY": -3.4495175555868487,
-            "timestamp": 0.5031333276672947
-        },
-        {
-            "x": 2.4331457577160354,
-            "y": 3.2598611919071945,
-            "heading": 0.25313177220508776,
-            "angularVelocity": 0.1463249118518414,
-            "velocityX": 1.4015059054023244,
-            "velocityY": -3.462416038839161,
-            "timestamp": 0.5282899940506595
-        },
-        {
-            "x": 2.473498104501041,
-            "y": 3.173715186571369,
-            "heading": 0.2532432496800442,
-            "angularVelocity": 0.004431329384332257,
-            "velocityX": 1.604041893710102,
-            "velocityY": -3.424380799229854,
-            "timestamp": 0.5534466604340242
-        },
-        {
-            "x": 2.5185817721335604,
-            "y": 3.089905547060991,
-            "heading": 0.25324321969105484,
-            "angularVelocity": -0.0000011920891614956396,
-            "velocityX": 1.7921161311870697,
-            "velocityY": -3.3315081669882356,
-            "timestamp": 0.578603326817389
-        },
-        {
-            "x": 2.568241478882672,
-            "y": 3.0087236123099395,
-            "heading": 0.2532431890633549,
-            "angularVelocity": -0.0000012174784783024135,
-            "velocityX": 1.97401778090718,
-            "velocityY": -3.2270545514223747,
-            "timestamp": 0.6037599932007538
-        },
-        {
-            "x": 2.6223245423144714,
-            "y": 2.9304189809174512,
-            "heading": 0.25324315880142845,
-            "angularVelocity": -0.0000012029386552177117,
-            "velocityX": 2.1498501672527754,
-            "velocityY": -3.1126791681853656,
-            "timestamp": 0.6289166595841186
-        },
-        {
-            "x": 2.6806646810204167,
-            "y": 2.855232404738762,
-            "heading": 0.25324312847978453,
-            "angularVelocity": -0.0000012053124772832063,
-            "velocityX": 2.319072718813166,
-            "velocityY": -2.9887336832676343,
-            "timestamp": 0.6540733259674834
-        },
-        {
-            "x": 2.7430825233459473,
-            "y": 2.7833950519561768,
-            "heading": 0.2532430937019675,
-            "angularVelocity": -0.0000013824493495341482,
-            "velocityX": 2.4811650865953045,
-            "velocityY": -2.8555990562441647,
-            "timestamp": 0.6792299923508481
-        },
-        {
-            "x": 2.8747857816965947,
-            "y": 2.6605842580771375,
-            "heading": 0.2532430684120768,
-            "angularVelocity": -5.312693002073383e-7,
-            "velocityX": 2.7667141273306664,
-            "velocityY": -2.579908520631989,
-            "timestamp": 0.7268327631068262
-        },
-        {
-            "x": 3.0186320127358077,
-            "y": 2.5522491183421203,
-            "heading": 0.2532430469824583,
-            "angularVelocity": -4.5017586399077444e-7,
-            "velocityX": 3.021803747025554,
-            "velocityY": -2.2758158404343,
-            "timestamp": 0.7744355338628043
-        },
-        {
-            "x": 3.1730376030001257,
-            "y": 2.4595822138278454,
-            "heading": 0.2532430277420137,
-            "angularVelocity": -4.041874945252969e-7,
-            "velocityX": 3.2436261127705017,
-            "velocityY": -1.946670394236201,
-            "timestamp": 0.8220383046187824
-        },
-        {
-            "x": 3.336302611834685,
-            "y": 2.38360340882859,
-            "heading": 0.25324300957525164,
-            "angularVelocity": -3.816324501860361e-7,
-            "velocityX": 3.4297375182526793,
-            "velocityY": -1.5961004746706646,
-            "timestamp": 0.8696410753747604
-        },
-        {
-            "x": 3.505316504811254,
-            "y": 2.3214540241877932,
-            "heading": 0.25324299166355524,
-            "angularVelocity": -3.7627424124720095e-7,
-            "velocityX": 3.550505365390803,
-            "velocityY": -1.3055833442844713,
-            "timestamp": 0.9172438461307385
-        },
-        {
-            "x": 3.6743308817857088,
-            "y": 2.2593059557844626,
-            "heading": 0.25324297375187266,
-            "angularVelocity": -3.76273948990246e-7,
-            "velocityX": 3.5505155328218008,
-            "velocityY": -1.3055556938463668,
-            "timestamp": 0.9648466168867166
-        },
-        {
-            "x": 3.843345266545321,
-            "y": 2.1971579085532174,
-            "heading": 0.2532429558401917,
-            "angularVelocity": -3.762739157561073e-7,
-            "velocityX": 3.5505156963660025,
-            "velocityY": -1.3055552490805507,
-            "timestamp": 1.0124493876426945
-        },
-        {
-            "x": 4.012359658311276,
-            "y": 2.135009880376043,
-            "heading": 0.25324293792850894,
-            "angularVelocity": -3.7627395316214525e-7,
-            "velocityX": 3.5505158435494915,
-            "velocityY": -1.3055548488082471,
-            "timestamp": 1.0600521583986726
-        },
-        {
-            "x": 4.181374056391421,
-            "y": 2.07286186937061,
-            "heading": 0.25324292001682175,
-            "angularVelocity": -3.7627404706616014e-7,
-            "velocityX": 3.5505159761928384,
-            "velocityY": -1.30555448807838,
-            "timestamp": 1.1076549291546507
-        },
-        {
-            "x": 4.3503884601627245,
-            "y": 2.010713873842556,
-            "heading": 0.25324290210512795,
-            "angularVelocity": -3.7627418619029544e-7,
-            "velocityX": 3.5505160957480255,
-            "velocityY": -1.30555416294226,
-            "timestamp": 1.1552576999106288
-        },
-        {
-            "x": 4.51940286907005,
-            "y": 1.9485658922821711,
-            "heading": 0.2532428841934256,
-            "angularVelocity": -3.7627436389492574e-7,
-            "velocityX": 3.5505162036413855,
-            "velocityY": -1.3055538695209365,
-            "timestamp": 1.2028604706666068
-        },
-        {
-            "x": 4.688417282561973,
-            "y": 1.8864179231898264,
-            "heading": 0.25324286628171494,
-            "angularVelocity": -3.7627454024029507e-7,
-            "velocityX": 3.550516299950831,
-            "velocityY": -1.3055536076025602,
-            "timestamp": 1.250463241422585
-        },
-        {
-            "x": 4.857431700124039,
-            "y": 1.824269965166445,
-            "heading": 0.2532428483699966,
-            "angularVelocity": -3.7627470129766985e-7,
-            "velocityX": 3.5505163854530717,
-            "velocityY": -1.3055533750748494,
-            "timestamp": 1.298066012178563
-        },
-        {
-            "x": 5.02644612130461,
-            "y": 1.7621220169837726,
-            "heading": 0.2532428304582713,
-            "angularVelocity": -3.76274846815446e-7,
-            "velocityX": 3.5505164614676405,
-            "velocityY": -1.3055531683492934,
-            "timestamp": 1.345668782934541
-        },
-        {
-            "x": 5.1954605457099055,
-            "y": 1.6999740775709098,
-            "heading": 0.25324281254653974,
-            "angularVelocity": -3.7627497879654404e-7,
-            "velocityX": 3.5505165292100247,
-            "velocityY": -1.3055529841203253,
-            "timestamp": 1.3932715536905191
-        },
-        {
-            "x": 5.364474972997734,
-            "y": 1.6378261459972472,
-            "heading": 0.25324279463480254,
-            "angularVelocity": -3.7627509767034604e-7,
-            "velocityX": 3.550516589763906,
-            "velocityY": -1.3055528194408317,
-            "timestamp": 1.4408743244464972
-        },
-        {
-            "x": 5.533489402870974,
-            "y": 1.5756782214547507,
-            "heading": 0.2532427767230601,
-            "angularVelocity": -3.762752068122207e-7,
-            "velocityX": 3.5505166440761182,
-            "velocityY": -1.3055526717358554,
-            "timestamp": 1.4884770952024753
-        },
-        {
-            "x": 5.702503835071411,
-            "y": 1.513530303241192,
-            "heading": 0.25324275881131303,
-            "angularVelocity": -3.7627530496428226e-7,
-            "velocityX": 3.550516692963962,
-            "velocityY": -1.3055525387827163,
-            "timestamp": 1.5360798659584534
-        },
-        {
-            "x": 5.871518269374186,
-            "y": 1.4513823907450571,
-            "heading": 0.2532427408995616,
-            "angularVelocity": -3.762753964251323e-7,
-            "velocityX": 3.5505167371281625,
-            "velocityY": -1.3055524186757583,
-            "timestamp": 1.5836826367144314
-        },
-        {
-            "x": 6.040532705582966,
-            "y": 1.3892344834324082,
-            "heading": 0.25324272298780626,
-            "angularVelocity": -3.7627547903960503e-7,
-            "velocityX": 3.550516777167954,
-            "velocityY": -1.3055523097853368,
-            "timestamp": 1.6312854074704095
-        },
-        {
-            "x": 6.20954714352589,
-            "y": 1.3270865808358587,
-            "heading": 0.2532427050760471,
-            "angularVelocity": -3.7627555795965075e-7,
-            "velocityX": 3.5505168135974197,
-            "velocityY": -1.3055522107133783,
-            "timestamp": 1.6788881782263876
-        },
-        {
-            "x": 6.378561583051887,
-            "y": 1.2649386825445668,
-            "heading": 0.25324268716428455,
-            "angularVelocity": -3.7627562961930894e-7,
-            "velocityX": 3.5505168468533315,
-            "velocityY": -1.3055521202720615,
-            "timestamp": 1.7264909489823657
-        },
-        {
-            "x": 6.547576024027691,
-            "y": 1.2027907881961044,
-            "heading": 0.25324266925251887,
-            "angularVelocity": -3.7627569544162935e-7,
-            "velocityX": 3.5505168773096716,
-            "velocityY": -1.3055520374443248,
-            "timestamp": 1.7740937197383437
-        },
-        {
-            "x": 6.716590466335514,
-            "y": 1.1406428974701475,
-            "heading": 0.25324265134075025,
-            "angularVelocity": -3.762757576316506e-7,
-            "velocityX": 3.550516905291665,
-            "velocityY": -1.3055519613457025,
-            "timestamp": 1.8216964904943218
-        },
-        {
-            "x": 6.88560491686231,
-            "y": 1.0784950290966298,
-            "heading": 0.253242633428611,
-            "angularVelocity": -3.7628354454135297e-7,
-            "velocityX": 3.550517077949107,
-            "velocityY": -1.3055514917839766,
-            "timestamp": 1.8692992612502999
-        },
-        {
-            "x": 7.0444139114910005,
-            "y": 1.0203058482073266,
-            "heading": 0.2259027129119336,
-            "angularVelocity": -0.574334646544581,
-            "velocityX": 3.336129223292024,
-            "velocityY": -1.2223906290579827,
-            "timestamp": 1.916902032006278
-        },
-        {
-            "x": 7.185810534595712,
-            "y": 0.9684234171781619,
-            "heading": 0.18518533646408028,
-            "angularVelocity": -0.8553572786042075,
-            "velocityX": 2.9703443908662663,
-            "velocityY": -1.089903596055891,
-            "timestamp": 1.964504802762256
-        },
-        {
-            "x": 7.3094655243228,
-            "y": 0.9230369546143523,
-            "heading": 0.14380134057962407,
-            "angularVelocity": -0.8693610734677483,
-            "velocityX": 2.59764269523238,
-            "velocityY": -0.953441613650398,
-            "timestamp": 2.012107573518234
-        },
-        {
-            "x": 7.415398593682274,
-            "y": 0.8841490900710189,
-            "heading": 0.10540313701630608,
-            "angularVelocity": -0.8066379950897271,
-            "velocityX": 2.2253551143589885,
-            "velocityY": -0.8169243917057022,
-            "timestamp": 2.059710344274212
-        },
-        {
-            "x": 7.503637039429732,
-            "y": 0.8517540010892736,
-            "heading": 0.07171800478429124,
-            "angularVelocity": -0.70762965468317,
-            "velocityX": 1.8536409613588987,
-            "velocityY": -0.680529483206126,
-            "timestamp": 2.1073131150301903
-        },
-        {
-            "x": 7.574202084965293,
-            "y": 0.8258459262172375,
-            "heading": 0.04375776286271558,
-            "angularVelocity": -0.5873658502969467,
-            "velocityX": 1.482372652157004,
-            "velocityY": -0.5442556065663985,
-            "timestamp": 2.1549158857861683
-        },
-        {
-            "x": 7.627109788299601,
-            "y": 0.8064200554899893,
-            "heading": 0.022189516524924582,
-            "angularVelocity": -0.4530880449870122,
-            "velocityX": 1.1114416764840027,
-            "velocityY": -0.40808277372822227,
-            "timestamp": 2.2025186565421464
-        },
-        {
-            "x": 7.66237251850946,
-            "y": 0.7934724766877013,
-            "heading": 0.0074863600733853135,
-            "angularVelocity": -0.3088718622474896,
-            "velocityX": 0.7407705402406998,
-            "velocityY": -0.27199212559831976,
-            "timestamp": 2.2501214272981245
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": -1.0670900456397286e-32,
-            "angularVelocity": -0.1572673177316084,
-            "velocityX": 0.37030368633165434,
-            "velocityY": -0.13596848639085585,
-            "timestamp": 2.2977241980541026
-        },
-        {
-            "x": 7.68,
-            "y": 0.787,
-            "heading": 0,
-            "angularVelocity": 2.230662363627099e-31,
-            "velocityX": 2.3924515989499496e-33,
-            "velocityY": 1.2585115119762557e-33,
-            "timestamp": 2.3453269688100806
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W3 To S1.traj b/src/main/deploy/choreo/W3 To S1.traj
deleted file mode 100644
index 714e9458..00000000
--- a/src/main/deploy/choreo/W3 To S1.traj	
+++ /dev/null
@@ -1,175 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 5.020065556819226e-31,
-            "velocityX": -2.911156118414847e-31,
-            "velocityY": 8.635491730351355e-32,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3221690164859408,
-            "y": 4.411229757983623,
-            "heading": -0.4048176043167742,
-            "angularVelocity": -0.1808500047435309,
-            "velocityX": 0.27057455786553103,
-            "velocityY": 0.6252631516043967,
-            "timestamp": 0.08193311544453755
-        },
-        {
-            "x": 2.3687303275118676,
-            "y": 4.5127321003230465,
-            "heading": -0.4313760168590284,
-            "angularVelocity": -0.32414747563532126,
-            "velocityX": 0.5682843960383053,
-            "velocityY": 1.2388439251785637,
-            "timestamp": 0.1638662308890751
-        },
-        {
-            "x": 2.4431953998066236,
-            "y": 4.662720767025029,
-            "heading": -0.4646753197408115,
-            "angularVelocity": -0.40642056268856935,
-            "velocityX": 0.9088519567543129,
-            "velocityY": 1.830623257619443,
-            "timestamp": 0.24579934633361264
-        },
-        {
-            "x": 2.551740018878988,
-            "y": 4.857106976368847,
-            "heading": -0.4953245435557511,
-            "angularVelocity": -0.3740761430663416,
-            "velocityX": 1.3247954564335083,
-            "velocityY": 2.3724986934666186,
-            "timestamp": 0.3277324617781502
-        },
-        {
-            "x": 2.701293241756868,
-            "y": 5.089437731072216,
-            "heading": -0.5138028115710582,
-            "angularVelocity": -0.2255286878210994,
-            "velocityX": 1.8253086321258618,
-            "velocityY": 2.835614799252222,
-            "timestamp": 0.4096655772226877
-        },
-        {
-            "x": 2.9065804913658706,
-            "y": 5.321655101693131,
-            "heading": -0.5138024716246538,
-            "angularVelocity": 0.000004149072112901325,
-            "velocityX": 2.5055467315650244,
-            "velocityY": 2.8342309377715424,
-            "timestamp": 0.4915986926672252
-        },
-        {
-            "x": 3.1502852438919966,
-            "y": 5.513164387705616,
-            "heading": -0.5138023715427595,
-            "angularVelocity": 0.0000012215072474193347,
-            "velocityX": 2.974435320857471,
-            "velocityY": 2.3373856220824885,
-            "timestamp": 0.5735318081117627
-        },
-        {
-            "x": 3.4075351073224858,
-            "y": 5.686052564856294,
-            "heading": -0.5138022777253332,
-            "angularVelocity": 0.0000011450489327031063,
-            "velocityX": 3.1397544452539083,
-            "velocityY": 2.110113550701131,
-            "timestamp": 0.6554649235563003
-        },
-        {
-            "x": 3.664785106053728,
-            "y": 5.858940540685368,
-            "heading": -0.5138021839079745,
-            "angularVelocity": 0.0000011450481052658978,
-            "velocityX": 3.13975609661006,
-            "velocityY": 2.110111093555414,
-            "timestamp": 0.7373980390008378
-        },
-        {
-            "x": 3.922035141067721,
-            "y": 6.031828462527248,
-            "heading": -0.5138020900906538,
-            "angularVelocity": 0.000001145047641454271,
-            "velocityX": 3.1397565394438236,
-            "velocityY": 2.1101104346375243,
-            "timestamp": 0.8193311544453753
-        },
-        {
-            "x": 4.179285313417939,
-            "y": 6.204716180018366,
-            "heading": -0.5138019962733545,
-            "angularVelocity": 0.00000114504738127865,
-            "velocityX": 3.139758215643051,
-            "velocityY": 2.110107940520708,
-            "timestamp": 0.9012642698899128
-        },
-        {
-            "x": 4.443584109358565,
-            "y": 6.354084975447565,
-            "heading": -0.4978326665645833,
-            "angularVelocity": 0.19490690207652947,
-            "velocityX": 3.225787210783358,
-            "velocityY": 1.8230576808752068,
-            "timestamp": 0.9831973853344503
-        },
-        {
-            "x": 4.674709010100242,
-            "y": 6.4693171293955105,
-            "heading": -0.3712044647358083,
-            "angularVelocity": 1.5455069801964878,
-            "velocityX": 2.8208972585465766,
-            "velocityY": 1.406417335929929,
-            "timestamp": 1.065130500778988
-        },
-        {
-            "x": 4.870816853917273,
-            "y": 6.548974021375514,
-            "heading": -0.15522232897542676,
-            "angularVelocity": 2.636078642787472,
-            "velocityX": 2.3935113751384223,
-            "velocityY": 0.9722185168697003,
-            "timestamp": 1.1470636162235255
-        },
-        {
-            "x": 5.022935942406859,
-            "y": 6.594453289671109,
-            "heading": 0.041233782182786366,
-            "angularVelocity": 2.3977620049271122,
-            "velocityX": 1.856625220025449,
-            "velocityY": 0.5550779809707214,
-            "timestamp": 1.228996731668063
-        },
-        {
-            "x": 5.124959992690689,
-            "y": 6.619307485763033,
-            "heading": 0.18624088374392397,
-            "angularVelocity": 1.769822870452196,
-            "velocityX": 1.2452114109208137,
-            "velocityY": 0.3033473822772884,
-            "timestamp": 1.3109298471126005
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": 0.9368509404239479,
-            "velocityX": 0.6229472299739557,
-            "velocityY": 0.13050296182384175,
-            "timestamp": 1.392862962557138
-        },
-        {
-            "x": 5.176,
-            "y": 6.63,
-            "heading": 0.263,
-            "angularVelocity": 1.5668772442975024e-30,
-            "velocityX": 0,
-            "velocityY": -2.8584411322391187e-31,
-            "timestamp": 1.4747960780016756
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W3 To S2.traj b/src/main/deploy/choreo/W3 To S2.traj
deleted file mode 100644
index 72f3f88a..00000000
--- a/src/main/deploy/choreo/W3 To S2.traj	
+++ /dev/null
@@ -1,130 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3223907681513127,
-            "y": 4.418261357040706,
-            "heading": -0.429219543622513,
-            "angularVelocity": -0.4499475285553182,
-            "velocityX": 0.2568788379871729,
-            "velocityY": 0.6684053711347925,
-            "timestamp": 0.08716470506781053
-        },
-        {
-            "x": 2.374909672838796,
-            "y": 4.531319644218887,
-            "heading": -0.5050759499486299,
-            "angularVelocity": -0.8702651637162607,
-            "velocityX": 0.6025248940683728,
-            "velocityY": 1.297064988520603,
-            "timestamp": 0.17432941013562106
-        },
-        {
-            "x": 2.469260718438912,
-            "y": 4.691180971030917,
-            "heading": -0.6116327562753044,
-            "angularVelocity": -1.2224765315706085,
-            "velocityX": 1.082445532589298,
-            "velocityY": 1.834014429208093,
-            "timestamp": 0.2614941152034316
-        },
-        {
-            "x": 2.620331804373909,
-            "y": 4.8788277526908095,
-            "heading": -0.7357228691689549,
-            "angularVelocity": -1.4236279787456398,
-            "velocityX": 1.7331680961631113,
-            "velocityY": 2.1527839911109723,
-            "timestamp": 0.3486588202712421
-        },
-        {
-            "x": 2.832381156144981,
-            "y": 5.083303561006889,
-            "heading": -0.84759450212177,
-            "angularVelocity": -1.2834510581523242,
-            "velocityX": 2.432743294503276,
-            "velocityY": 2.3458555634072913,
-            "timestamp": 0.43582352533905266
-        },
-        {
-            "x": 3.098611796220168,
-            "y": 5.267387874253087,
-            "heading": -0.8666590247194526,
-            "angularVelocity": -0.21871837440225492,
-            "velocityX": 3.0543399403241303,
-            "velocityY": 2.111913452847551,
-            "timestamp": 0.5229882304068632
-        },
-        {
-            "x": 3.3974595011437105,
-            "y": 5.397776588765608,
-            "heading": -0.8564937886773937,
-            "angularVelocity": 0.11662101115527058,
-            "velocityX": 3.42854030987716,
-            "velocityY": 1.4958888969002289,
-            "timestamp": 0.6101529354746736
-        },
-        {
-            "x": 3.678181484629676,
-            "y": 5.472509187071852,
-            "heading": -0.7581230365630254,
-            "angularVelocity": 1.1285617502845884,
-            "velocityX": 3.220592363245829,
-            "velocityY": 0.8573722385466142,
-            "timestamp": 0.6973176405424841
-        },
-        {
-            "x": 3.915033154173224,
-            "y": 5.514676302538828,
-            "heading": -0.5395209628894968,
-            "angularVelocity": 2.5079196161274817,
-            "velocityX": 2.7172887163363586,
-            "velocityY": 0.4837636453214852,
-            "timestamp": 0.7844823456102946
-        },
-        {
-            "x": 4.091189385673594,
-            "y": 5.53976294658506,
-            "heading": -0.29904398170320484,
-            "angularVelocity": 2.7588802256510867,
-            "velocityX": 2.0209582693284847,
-            "velocityY": 0.2878073645372324,
-            "timestamp": 0.8716470506781051
-        },
-        {
-            "x": 4.2064124532392215,
-            "y": 5.553849604220975,
-            "heading": -0.10702579424844161,
-            "angularVelocity": 2.2029350905894844,
-            "velocityX": 1.3219005040628353,
-            "velocityY": 0.161609651807531,
-            "timestamp": 0.9588117557459156
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 2.9287791201697906e-29,
-            "angularVelocity": 1.2278570112200775,
-            "velocityX": 0.6492025266047246,
-            "velocityY": 0.0705606216901654,
-            "timestamp": 1.045976460813726
-        },
-        {
-            "x": 4.263,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 0,
-            "velocityX": 3.0874616220611523e-31,
-            "velocityY": -3.4071713040637054e-30,
-            "timestamp": 1.1331411658815367
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W3 To S3.traj b/src/main/deploy/choreo/W3 To S3.traj
deleted file mode 100644
index 4ff6b6c8..00000000
--- a/src/main/deploy/choreo/W3 To S3.traj	
+++ /dev/null
@@ -1,229 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.284359298751307,
-            "y": 4.3328670624584875,
-            "heading": -0.3913603360985968,
-            "angularVelocity": -0.02219557362709146,
-            "velocityX": -0.2551974739939649,
-            "velocityY": -0.44270758788444614,
-            "timestamp": 0.061288620940906494
-        },
-        {
-            "x": 2.2541133789559487,
-            "y": 4.2780305351291235,
-            "heading": -0.3941996942145126,
-            "angularVelocity": -0.04632765548197678,
-            "velocityX": -0.4934997611468709,
-            "velocityY": -0.89472607618691,
-            "timestamp": 0.12257724188181299
-        },
-        {
-            "x": 2.2108693669713233,
-            "y": 4.194701771021148,
-            "heading": -0.39869373638946143,
-            "angularVelocity": -0.0733258818024626,
-            "velocityX": -0.7055797849705345,
-            "velocityY": -1.3596123200148318,
-            "timestamp": 0.1838658628227195
-        },
-        {
-            "x": 2.157413012714945,
-            "y": 4.081769184110331,
-            "heading": -0.40512443208979565,
-            "angularVelocity": -0.1049247903054391,
-            "velocityX": -0.8722068376757797,
-            "velocityY": -1.8426354709417345,
-            "timestamp": 0.24515448376362597
-        },
-        {
-            "x": 2.099439422754663,
-            "y": 3.9378510886177804,
-            "heading": -0.41398926531008917,
-            "angularVelocity": -0.1446407682894496,
-            "velocityX": -0.9459111507204496,
-            "velocityY": -2.348202542056109,
-            "timestamp": 0.30644310470453245
-        },
-        {
-            "x": 2.05084134036617,
-            "y": 3.764062564560248,
-            "heading": -0.42610260644735526,
-            "angularVelocity": -0.1976442111325286,
-            "velocityX": -0.7929380958881483,
-            "velocityY": -2.835575697242347,
-            "timestamp": 0.36773172564543893
-        },
-        {
-            "x": 2.0313297442556637,
-            "y": 3.578686605657539,
-            "heading": -0.44115755081157715,
-            "angularVelocity": -0.24564012263773397,
-            "velocityX": -0.3183559331400068,
-            "velocityY": -3.024639093795978,
-            "timestamp": 0.4290203465863454
-        },
-        {
-            "x": 2.0424292488902682,
-            "y": 3.3999266361315956,
-            "heading": -0.45764648760363913,
-            "angularVelocity": -0.2690374907923018,
-            "velocityX": 0.18110220892890863,
-            "velocityY": -2.9166910069375023,
-            "timestamp": 0.4903089675272519
-        },
-        {
-            "x": 2.0816987250314285,
-            "y": 3.234855796713624,
-            "heading": -0.4747853916237668,
-            "angularVelocity": -0.27964251368378285,
-            "velocityX": 0.6407302944379066,
-            "velocityY": -2.6933358408754287,
-            "timestamp": 0.5515975884681584
-        },
-        {
-            "x": 2.1473776050382467,
-            "y": 3.086622748034336,
-            "heading": -0.49218081575104855,
-            "angularVelocity": -0.2838279579508591,
-            "velocityX": 1.071632531431,
-            "velocityY": -2.418606364502998,
-            "timestamp": 0.6128862094090649
-        },
-        {
-            "x": 2.2382969856262207,
-            "y": 2.9569339752197266,
-            "heading": -0.5096053711064815,
-            "angularVelocity": -0.2843032701328595,
-            "velocityX": 1.4834626590087072,
-            "velocityY": -2.116033463041253,
-            "timestamp": 0.6741748303499714
-        },
-        {
-            "x": 2.3750291535583634,
-            "y": 2.833789120602945,
-            "heading": -0.5294729435704262,
-            "angularVelocity": -0.282105779182567,
-            "velocityX": 1.9415021560293924,
-            "velocityY": -1.7485717103605045,
-            "timestamp": 0.7446007979480589
-        },
-        {
-            "x": 2.5431654959507464,
-            "y": 2.737552434720476,
-            "heading": -0.5488979650801319,
-            "angularVelocity": -0.2758218619098283,
-            "velocityX": 2.3874196993915153,
-            "velocityY": -1.366494336743516,
-            "timestamp": 0.8150267655461465
-        },
-        {
-            "x": 2.7413669689925393,
-            "y": 2.669711436345154,
-            "heading": -0.5674506234763109,
-            "angularVelocity": -0.2634349094364851,
-            "velocityX": 2.8143237473555907,
-            "velocityY": -0.9632952260234809,
-            "timestamp": 0.8854527331442341
-        },
-        {
-            "x": 2.9672720655352354,
-            "y": 2.6325722400915463,
-            "heading": -0.5844267226661307,
-            "angularVelocity": -0.24104885979984425,
-            "velocityX": 3.207696028145596,
-            "velocityY": -0.5273508837756647,
-            "timestamp": 0.9558787007423217
-        },
-        {
-            "x": 3.21586763898004,
-            "y": 2.629998478445332,
-            "heading": -0.5985057428976699,
-            "angularVelocity": -0.19991234358165255,
-            "velocityX": 3.529885096694866,
-            "velocityY": -0.03654563414595668,
-            "timestamp": 1.0263046683404093
-        },
-        {
-            "x": 3.4730987079905056,
-            "y": 2.6678365706373897,
-            "heading": -0.6068355688677516,
-            "angularVelocity": -0.11827776392962061,
-            "velocityX": 3.652503157336117,
-            "velocityY": 0.5372747224148254,
-            "timestamp": 1.096730635938497
-        },
-        {
-            "x": 3.704150591962856,
-            "y": 2.737591563426048,
-            "heading": -0.6062385008661115,
-            "angularVelocity": 0.00847795240879764,
-            "velocityX": 3.28077684769537,
-            "velocityY": 0.9904726220694814,
-            "timestamp": 1.1671566035365846
-        },
-        {
-            "x": 3.894448488053554,
-            "y": 2.8141322684658854,
-            "heading": -0.5991077413907381,
-            "angularVelocity": 0.10125184954600595,
-            "velocityX": 2.702098424500253,
-            "velocityY": 1.086825039829707,
-            "timestamp": 1.2375825711346722
-        },
-        {
-            "x": 4.043960756386648,
-            "y": 2.883897596305393,
-            "heading": -0.5885679211534333,
-            "angularVelocity": 0.1496581530473843,
-            "velocityX": 2.122970736963697,
-            "velocityY": 0.9906193726389452,
-            "timestamp": 1.3080085387327598
-        },
-        {
-            "x": 4.154363809487208,
-            "y": 2.940218619761556,
-            "heading": -0.5783208756256007,
-            "angularVelocity": 0.14550095479427785,
-            "velocityX": 1.5676469470837475,
-            "velocityY": 0.7997195548321091,
-            "timestamp": 1.3784345063308474
-        },
-        {
-            "x": 4.227038806897989,
-            "y": 2.9796144762337837,
-            "heading": -0.5703846528919979,
-            "angularVelocity": 0.11268887037369305,
-            "velocityX": 1.0319346668480025,
-            "velocityY": 0.5593938971070241,
-            "timestamp": 1.448860473928935
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 0.06225903656760019,
-            "velocityX": 0.5106240542868632,
-            "velocityY": 0.2894603292148455,
-            "timestamp": 1.5192864415270226
-        },
-        {
-            "x": 4.263,
-            "y": 3,
-            "heading": -0.566,
-            "angularVelocity": 9.24329495349137e-32,
-            "velocityX": 0,
-            "velocityY": 1.2740379664097394e-33,
-            "timestamp": 1.5897124091251102
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W3 To S4.traj b/src/main/deploy/choreo/W3 To S4.traj
deleted file mode 100644
index 2b0725c8..00000000
--- a/src/main/deploy/choreo/W3 To S4.traj	
+++ /dev/null
@@ -1,256 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 9.57151893871228e-33,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2927892107770984,
-            "y": 4.332309394440523,
-            "heading": -0.3722824674321399,
-            "angularVelocity": 0.3002660031020358,
-            "velocityX": -0.12220408518393268,
-            "velocityY": -0.4692835993371687,
-            "timestamp": 0.059006122520770025
-        },
-        {
-            "x": 2.2795380946752672,
-            "y": 4.276530177721837,
-            "heading": -0.3395678297253276,
-            "angularVelocity": 0.5544278510301563,
-            "velocityX": -0.224571883996739,
-            "velocityY": -0.9453123563415277,
-            "timestamp": 0.11801224504154005
-        },
-        {
-            "x": 2.2618545093830753,
-            "y": 4.192237257969867,
-            "heading": -0.2954402922678697,
-            "angularVelocity": 0.7478467584770477,
-            "velocityX": -0.29969068524994186,
-            "velocityY": -1.4285453127732544,
-            "timestamp": 0.17701836756231007
-        },
-        {
-            "x": 2.2420434066604593,
-            "y": 4.079048226765037,
-            "heading": -0.24482967906698166,
-            "angularVelocity": 0.8577179966887506,
-            "velocityX": -0.33574656114103973,
-            "velocityY": -1.9182590953165608,
-            "timestamp": 0.2360244900830801
-        },
-        {
-            "x": 2.2234541825747725,
-            "y": 3.936854095556344,
-            "heading": -0.19463836205748358,
-            "angularVelocity": 0.8506120189787272,
-            "velocityX": -0.3150389026010499,
-            "velocityY": -2.40981994976269,
-            "timestamp": 0.2950306126038501
-        },
-        {
-            "x": 2.211142623620853,
-            "y": 3.7664761504723194,
-            "heading": -0.15521354512815658,
-            "angularVelocity": 0.6681479013546628,
-            "velocityX": -0.2086488389333828,
-            "velocityY": -2.8874621446961175,
-            "timestamp": 0.35403673512462014
-        },
-        {
-            "x": 2.2125400151692816,
-            "y": 3.5715383450180926,
-            "heading": -0.14131017152189063,
-            "angularVelocity": 0.23562594883900537,
-            "velocityX": 0.023682144983125607,
-            "velocityY": -3.303687772156347,
-            "timestamp": 0.41304285764539017
-        },
-        {
-            "x": 2.235363094184648,
-            "y": 3.3576277458759463,
-            "heading": -0.144791073330025,
-            "angularVelocity": -0.058992214018278255,
-            "velocityX": 0.38679170974728244,
-            "velocityY": -3.6252271798888405,
-            "timestamp": 0.4720489801661602
-        },
-        {
-            "x": 2.287018648077521,
-            "y": 3.1404706845748738,
-            "heading": -0.14479216409186968,
-            "angularVelocity": -0.000018485570624385508,
-            "velocityX": 0.8754270181825738,
-            "velocityY": -3.6802462528296687,
-            "timestamp": 0.5310551026869302
-        },
-        {
-            "x": 2.3664206535422005,
-            "y": 2.9318538879811458,
-            "heading": -0.14479229465212481,
-            "angularVelocity": -0.000002212656069584398,
-            "velocityX": 1.3456570618876043,
-            "velocityY": -3.53551102295012,
-            "timestamp": 0.5900612252077002
-        },
-        {
-            "x": 2.4722259044647217,
-            "y": 2.73530650138855,
-            "heading": -0.14479252387880184,
-            "angularVelocity": -0.000003884794784189704,
-            "velocityX": 1.7931232625102267,
-            "velocityY": -3.330965977698533,
-            "timestamp": 0.6490673477284702
-        },
-        {
-            "x": 2.6222628757424284,
-            "y": 2.533912575740545,
-            "heading": -0.14479267276909355,
-            "angularVelocity": -0.0000022427572708026126,
-            "velocityX": 2.260029880862475,
-            "velocityY": -3.033627551346843,
-            "timestamp": 0.7154545103709612
-        },
-        {
-            "x": 2.800083925097921,
-            "y": 2.356570260194387,
-            "heading": -0.14479275333723068,
-            "angularVelocity": -0.0000012136101911737535,
-            "velocityX": 2.678545704884188,
-            "velocityY": -2.671334464182248,
-            "timestamp": 0.7818416730134522
-        },
-        {
-            "x": 3.001881619185789,
-            "y": 2.207076716019015,
-            "heading": -0.14479280967120484,
-            "angularVelocity": -8.485672817028763e-7,
-            "velocityX": 3.0397095772053357,
-            "velocityY": -2.2518441551784094,
-            "timestamp": 0.8482288356559432
-        },
-        {
-            "x": 3.2233351494929763,
-            "y": 2.0886327789398735,
-            "heading": -0.14479285532879432,
-            "angularVelocity": -6.877472642883405e-7,
-            "velocityX": 3.3357884490373766,
-            "velocityY": -1.7841391673415428,
-            "timestamp": 0.9146159982984342
-        },
-        {
-            "x": 3.4597028157497376,
-            "y": 2.003774307484127,
-            "heading": -0.14479289641721121,
-            "angularVelocity": -6.189211176196774e-7,
-            "velocityX": 3.5604423633775646,
-            "velocityY": -1.2782361540698368,
-            "timestamp": 0.9810031609409252
-        },
-        {
-            "x": 3.704835569204283,
-            "y": 1.949179734375487,
-            "heading": -0.14479293648657837,
-            "angularVelocity": -6.035710154638316e-7,
-            "velocityX": 3.692472214464675,
-            "velocityY": -0.8223664174750668,
-            "timestamp": 1.047390323583416
-        },
-        {
-            "x": 3.9499686718209266,
-            "y": 1.8945867290481686,
-            "heading": -0.1447929765559331,
-            "angularVelocity": -6.035708281824289e-7,
-            "velocityX": 3.692477473946853,
-            "velocityY": -0.8223428017448724,
-            "timestamp": 1.113777486225907
-        },
-        {
-            "x": 4.195101784677066,
-            "y": 1.8399937696982192,
-            "heading": -0.14479301662532418,
-            "angularVelocity": -6.035713756667977e-7,
-            "velocityX": 3.692477628185933,
-            "velocityY": -0.8223421091807183,
-            "timestamp": 1.1801646488683981
-        },
-        {
-            "x": 4.435904668374261,
-            "y": 1.7860806487644325,
-            "heading": -0.15898872227240893,
-            "angularVelocity": -0.21383208864538455,
-            "velocityX": 3.627250723064772,
-            "velocityY": -0.8121015989811171,
-            "timestamp": 1.2465518115108891
-        },
-        {
-            "x": 4.648245693564204,
-            "y": 1.7385415440023928,
-            "heading": -0.26462575063007965,
-            "angularVelocity": -1.5912267395211415,
-            "velocityX": 3.1985253886123286,
-            "velocityY": -0.7160888170209653,
-            "timestamp": 1.3129389741533801
-        },
-        {
-            "x": 4.824757883271093,
-            "y": 1.6989793937314068,
-            "heading": -0.37654836939407044,
-            "angularVelocity": -1.6859075506317058,
-            "velocityX": 2.6588301515075172,
-            "velocityY": -0.5959307296206723,
-            "timestamp": 1.3793261367958711
-        },
-        {
-            "x": 4.96555652453039,
-            "y": 1.6673723665346096,
-            "heading": -0.4762340168485147,
-            "angularVelocity": -1.501580177349536,
-            "velocityX": 2.1208715006774397,
-            "velocityY": -0.47610149219673153,
-            "timestamp": 1.4457132994383621
-        },
-        {
-            "x": 5.070906714490224,
-            "y": 1.643682366519874,
-            "heading": -0.5558849180153355,
-            "angularVelocity": -1.1997937251175768,
-            "velocityX": 1.5869060487969315,
-            "velocityY": -0.3568460990313944,
-            "timestamp": 1.5121004620808531
-        },
-        {
-            "x": 5.141006607602348,
-            "y": 1.6278935430132826,
-            "heading": -0.6113350442973207,
-            "angularVelocity": -0.8352537459779052,
-            "velocityX": 1.055925427776252,
-            "velocityY": -0.23782946699526453,
-            "timestamp": 1.5784876247233441
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": -0.4317846186174008,
-            "velocityX": 0.5271108299370921,
-            "velocityY": -0.11890164753374041,
-            "timestamp": 1.6448747873658351
-        },
-        {
-            "x": 5.176,
-            "y": 1.62,
-            "heading": -0.64,
-            "angularVelocity": 1.0662498702136072e-32,
-            "velocityX": 0,
-            "velocityY": 8.479039452208929e-33,
-            "timestamp": 1.7112619500083261
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W3 To W1.traj b/src/main/deploy/choreo/W3 To W1.traj
deleted file mode 100644
index 087304c1..00000000
--- a/src/main/deploy/choreo/W3 To W1.traj	
+++ /dev/null
@@ -1,148 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 1.4326217036715317e-32,
-            "velocityX": -1.74824017803789e-33,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.3001377838932537,
-            "y": 4.403828106793783,
-            "heading": -0.3629126711329447,
-            "angularVelocity": 0.37089781855009624,
-            "velocityX": 0.0018866291944089578,
-            "velocityY": 0.600123743495633,
-            "timestamp": 0.07303178264284323
-        },
-        {
-            "x": 2.300408122825978,
-            "y": 4.49157282256222,
-            "heading": -0.3106389398567459,
-            "angularVelocity": 0.7157668810008344,
-            "velocityX": 0.0037016614265912673,
-            "velocityY": 1.2014593180279707,
-            "timestamp": 0.14606356528568645
-        },
-        {
-            "x": 2.3008081314200264,
-            "y": 4.623360732427042,
-            "heading": -0.2362186265841639,
-            "angularVelocity": 1.019012689811082,
-            "velocityX": 0.005477185131915077,
-            "velocityY": 1.8045281806870437,
-            "timestamp": 0.2190953479285297
-        },
-        {
-            "x": 2.301347271135313,
-            "y": 4.79937800153835,
-            "heading": -0.14515320669009546,
-            "angularVelocity": 1.2469286192754925,
-            "velocityX": 0.007382261472700921,
-            "velocityY": 2.410146141058999,
-            "timestamp": 0.2921271305713729
-        },
-        {
-            "x": 2.302070167703843,
-            "y": 5.019849104537176,
-            "heading": -0.04952394454697364,
-            "angularVelocity": 1.3094197989222025,
-            "velocityX": 0.009898383174695338,
-            "velocityY": 3.018837758308937,
-            "timestamp": 0.3651589132142161
-        },
-        {
-            "x": 2.3031513688730865,
-            "y": 5.282690253403799,
-            "heading": -0.004114907781753178,
-            "angularVelocity": 0.6217708937393756,
-            "velocityX": 0.014804529344862738,
-            "velocityY": 3.598996756686451,
-            "timestamp": 0.43819069585705933
-        },
-        {
-            "x": 2.303520931398292,
-            "y": 5.5589648879428,
-            "heading": -0.0041148785146688655,
-            "angularVelocity": 4.00744487575221e-7,
-            "velocityX": 0.005060297199824665,
-            "velocityY": 3.7829370246937892,
-            "timestamp": 0.5112224784999025
-        },
-        {
-            "x": 2.3038904865521834,
-            "y": 5.835239522491655,
-            "heading": -0.004114849247579104,
-            "angularVelocity": 4.0074456218901027e-7,
-            "velocityX": 0.005060196266858216,
-            "velocityY": 3.7829370248287195,
-            "timestamp": 0.5842542611427458
-        },
-        {
-            "x": 2.302005656251481,
-            "y": 6.097682306820458,
-            "heading": 0.0412177605138735,
-            "angularVelocity": 0.6207244041015477,
-            "velocityX": -0.025808356752290516,
-            "velocityY": 3.5935420830717044,
-            "timestamp": 0.6572860437855891
-        },
-        {
-            "x": 2.3011707390613565,
-            "y": 6.318019040898657,
-            "heading": 0.13972629361592653,
-            "angularVelocity": 1.3488447020908976,
-            "velocityX": -0.01143224442716972,
-            "velocityY": 3.016997889203128,
-            "timestamp": 0.7303178264284323
-        },
-        {
-            "x": 2.3006512841511224,
-            "y": 6.493901741171369,
-            "heading": 0.23378956116370217,
-            "angularVelocity": 1.2879771538343168,
-            "velocityX": -0.007112723959845375,
-            "velocityY": 2.408303534542668,
-            "timestamp": 0.8033496090712756
-        },
-        {
-            "x": 2.300310658917655,
-            "y": 6.625570031648912,
-            "heading": 0.310734904704345,
-            "angularVelocity": 1.053587092580481,
-            "velocityX": -0.00466406845267762,
-            "velocityY": 1.802890272053981,
-            "timestamp": 0.8763813917141189
-        },
-        {
-            "x": 2.3001003670918414,
-            "y": 6.7132235530415985,
-            "heading": 0.3648714550351714,
-            "angularVelocity": 0.7412738450542471,
-            "velocityX": -0.0028794562888088465,
-            "velocityY": 1.2002106236588805,
-            "timestamp": 0.9494131743569622
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 0.38515484556072405,
-            "velocityX": -0.0013742933310585549,
-            "velocityY": 0.5994163824877027,
-            "timestamp": 1.0224449569998053
-        },
-        {
-            "x": 2.3,
-            "y": 6.757,
-            "heading": 0.393,
-            "angularVelocity": 2.8585488767662063e-32,
-            "velocityX": -1.0732467562704116e-31,
-            "velocityY": 0,
-            "timestamp": 1.0954767396426486
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/W3 To W2.traj b/src/main/deploy/choreo/W3 To W2.traj
deleted file mode 100644
index 4345e451..00000000
--- a/src/main/deploy/choreo/W3 To W2.traj	
+++ /dev/null
@@ -1,103 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.3,
-            "y": 4.36,
-            "heading": -0.39,
-            "angularVelocity": 0,
-            "velocityX": 2.0150450292109907e-34,
-            "velocityY": 3.039389955204995e-35,
-            "timestamp": 0
-        },
-        {
-            "x": 2.2980012520823867,
-            "y": 4.4080014432651184,
-            "heading": -0.3744767713085023,
-            "angularVelocity": 0.2040875990087458,
-            "velocityX": -0.02627801674742958,
-            "velocityY": 0.6310864511258738,
-            "timestamp": 0.07606159691668693
-        },
-        {
-            "x": 2.294003780157748,
-            "y": 4.504004078538053,
-            "heading": -0.34341554427294535,
-            "angularVelocity": 0.40836937817095076,
-            "velocityX": -0.05255571913664827,
-            "velocityY": 1.262169598911913,
-            "timestamp": 0.15212319383337386
-        },
-        {
-            "x": 2.2880075055108158,
-            "y": 4.648007445856594,
-            "heading": -0.296791131111893,
-            "angularVelocity": 0.6129823071177671,
-            "velocityX": -0.07883445641432706,
-            "velocityY": 1.8932466994648223,
-            "timestamp": 0.2281847907500608
-        },
-        {
-            "x": 2.2800121106149036,
-            "y": 4.840010896492559,
-            "heading": -0.2345694561462187,
-            "angularVelocity": 0.8180432371651087,
-            "velocityX": -0.10511736829125365,
-            "velocityY": 2.5243152710332057,
-            "timestamp": 0.3042463876667477
-        },
-        {
-            "x": 2.270016860554186,
-            "y": 5.080013636963748,
-            "heading": -0.15671112019223,
-            "angularVelocity": 1.023622157700289,
-            "velocityX": -0.13140994228224737,
-            "velocityY": 3.1553734105013973,
-            "timestamp": 0.3803079845834346
-        },
-        {
-            "x": 2.2620106394348882,
-            "y": 5.2720083260038075,
-            "heading": -0.09403333701953483,
-            "angularVelocity": 0.82403980081234,
-            "velocityX": -0.10525970323851294,
-            "velocityY": 2.524200080237073,
-            "timestamp": 0.4563695815001215
-        },
-        {
-            "x": 2.256005452365101,
-            "y": 5.416004188429854,
-            "heading": -0.0470178538159679,
-            "angularVelocity": 0.6181237984664553,
-            "velocityX": -0.07895163016843816,
-            "velocityY": 1.8931480308488688,
-            "timestamp": 0.5324311784168084
-        },
-        {
-            "x": 2.2520018201736485,
-            "y": 5.5120013950535185,
-            "heading": -0.01567260522346991,
-            "angularVelocity": 0.4121034774859067,
-            "velocityX": -0.05263670963729669,
-            "velocityY": 1.262098227162051,
-            "timestamp": 0.6084927753334953
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 1.3656663814986182e-35,
-            "angularVelocity": 0.20605148798856665,
-            "velocityX": -0.026318408432062,
-            "velocityY": 0.6310491350721452,
-            "timestamp": 0.6845543722501822
-        },
-        {
-            "x": 2.25,
-            "y": 5.56,
-            "heading": 0,
-            "angularVelocity": 6.6551444751621536e-34,
-            "velocityX": 3.9479513180483775e-34,
-            "velocityY": 0,
-            "timestamp": 0.7606159691668691
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/amp 4 local sgmt 1.traj b/src/main/deploy/choreo/amp 4 local sgmt 1.traj
deleted file mode 100644
index 1dbcc825..00000000
--- a/src/main/deploy/choreo/amp 4 local sgmt 1.traj	
+++ /dev/null
@@ -1,337 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.7470832465334709,
-            "y": 6.625458718233556,
-            "heading": 1.0564954979067924,
-            "angularVelocity": -5.684301329861994e-12,
-            "velocityX": -3.602456269610706e-10,
-            "velocityY": 4.1137821262407583e-10,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7589569772775385,
-            "y": 6.629936126338657,
-            "heading": 1.0434265097431823,
-            "angularVelocity": -0.12778392744309977,
-            "velocityX": 0.11609712676013798,
-            "velocityY": 0.04377849867759475,
-            "timestamp": 0.10227411521599863
-        },
-        {
-            "x": 0.7822022838432713,
-            "y": 6.638629979602176,
-            "heading": 1.0183346599429852,
-            "angularVelocity": -0.2453392014361959,
-            "velocityX": 0.2272843592055021,
-            "velocityY": 0.08500540295845989,
-            "timestamp": 0.20454823043199727
-        },
-        {
-            "x": 0.816345318524229,
-            "y": 6.6512661070932495,
-            "heading": 0.9822355978698938,
-            "angularVelocity": -0.35296381641965074,
-            "velocityX": 0.33383847695279084,
-            "velocityY": 0.12355156118457596,
-            "timestamp": 0.30682234564799593
-        },
-        {
-            "x": 0.8609432271767217,
-            "y": 6.667555740683275,
-            "heading": 0.9361120278121537,
-            "angularVelocity": -0.45097989764859636,
-            "velocityX": 0.4360625240585109,
-            "velocityY": 0.15927424713723962,
-            "timestamp": 0.40909646086399454
-        },
-        {
-            "x": 0.9155883048461597,
-            "y": 6.687191502411657,
-            "heading": 0.8809291072748399,
-            "angularVelocity": -0.5395590113414345,
-            "velocityX": 0.5343001768187209,
-            "velocityY": 0.19199150048411856,
-            "timestamp": 0.5113705760799931
-        },
-        {
-            "x": 0.9799143577111046,
-            "y": 6.709840447255909,
-            "heading": 0.817662056557047,
-            "angularVelocity": -0.6186027670698295,
-            "velocityX": 0.6289573163604619,
-            "velocityY": 0.22145333556813593,
-            "timestamp": 0.6136446912959918
-        },
-        {
-            "x": 1.0536065024635548,
-            "y": 6.735133328172977,
-            "heading": 0.7473295527982347,
-            "angularVelocity": -0.687686259272288,
-            "velocityX": 0.7205356386732681,
-            "velocityY": 0.24730480411634378,
-            "timestamp": 0.7159188065119904
-        },
-        {
-            "x": 1.1364169197016813,
-            "y": 6.762648211314195,
-            "heading": 0.6710254557943266,
-            "angularVelocity": -0.7460743786173882,
-            "velocityX": 0.8096908708297301,
-            "velocityY": 0.2690307532427302,
-            "timestamp": 0.818192921727989
-        },
-        {
-            "x": 1.2281932228229642,
-            "y": 6.7918845460908575,
-            "heading": 0.5899378478266369,
-            "angularVelocity": -0.7928458510838684,
-            "velocityX": 0.8973561208709863,
-            "velocityY": 0.28586249636547506,
-            "timestamp": 0.9204670369439876
-        },
-        {
-            "x": 1.3289409008958384,
-            "y": 6.822217824239773,
-            "heading": 0.5053285615301966,
-            "angularVelocity": -0.8272795718879136,
-            "velocityX": 0.985075041075245,
-            "velocityY": 0.296588019337818,
-            "timestamp": 1.0227411521599863
-        },
-        {
-            "x": 1.4390031913745853,
-            "y": 6.852792823991799,
-            "heading": 0.4183893727045359,
-            "angularVelocity": -0.8500605319021087,
-            "velocityX": 1.076150015287458,
-            "velocityY": 0.29895148738224403,
-            "timestamp": 1.125015267375985
-        },
-        {
-            "x": 1.5597885498551032,
-            "y": 6.882059112916124,
-            "heading": 0.32979940511385675,
-            "angularVelocity": -0.8662012607776253,
-            "velocityX": 1.1809963684348201,
-            "velocityY": 0.28615537738935176,
-            "timestamp": 1.2272893825919837
-        },
-        {
-            "x": 1.700662064049805,
-            "y": 6.9025972281907375,
-            "heading": 0.2406449492427404,
-            "angularVelocity": -0.8717206268168672,
-            "velocityX": 1.3774112254273,
-            "velocityY": 0.20081439319892427,
-            "timestamp": 1.3295634978079824
-        },
-        {
-            "x": 1.8510164004103928,
-            "y": 6.913603640101383,
-            "heading": 0.18435642050200896,
-            "angularVelocity": -0.5503692556839603,
-            "velocityX": 1.4701113396826178,
-            "velocityY": 0.10761678064700267,
-            "timestamp": 1.4318376130239812
-        },
-        {
-            "x": 2.0039488544462833,
-            "y": 6.918843013673937,
-            "heading": 0.15617120081925578,
-            "angularVelocity": -0.2755850738942481,
-            "velocityX": 1.4953192592999736,
-            "velocityY": 0.051228726583383746,
-            "timestamp": 1.5341117282399799
-        },
-        {
-            "x": 2.1557445524327394,
-            "y": 6.919295783765447,
-            "heading": 0.15617120073466711,
-            "angularVelocity": -8.01993499988118e-11,
-            "velocityX": 1.484204462099971,
-            "velocityY": 0.0044270165813428694,
-            "timestamp": 1.6363858434559786
-        },
-        {
-            "x": 2.232501376452394,
-            "y": 6.918644981823818,
-            "heading": 0.15617120066422163,
-            "angularVelocity": -2.7881719949913897e-12,
-            "velocityX": 1.4631774376072004,
-            "velocityY": -0.012405855819009865,
-            "timestamp": 1.6888448428916174
-        },
-        {
-            "x": 2.3078780299226054,
-            "y": 6.916920762093343,
-            "heading": 0.1561712005937278,
-            "angularVelocity": -3.698975819730093e-12,
-            "velocityX": 1.436867928003674,
-            "velocityY": -0.032867889033463694,
-            "timestamp": 1.7413038423272562
-        },
-        {
-            "x": 2.381659462849091,
-            "y": 6.91403766609857,
-            "heading": 0.15617120052323402,
-            "angularVelocity": -3.687857737825465e-12,
-            "velocityX": 1.4064590265589882,
-            "velocityY": -0.05495897496281437,
-            "timestamp": 1.793762841762895
-        },
-        {
-            "x": 2.453627858185915,
-            "y": 6.909923503320222,
-            "heading": 0.15617120045273983,
-            "angularVelocity": -3.68806627753873e-12,
-            "velocityX": 1.3718979863339387,
-            "velocityY": -0.07842619311071317,
-            "timestamp": 1.8462218411985338
-        },
-        {
-            "x": 2.5235476514141553,
-            "y": 6.90450568335544,
-            "heading": 0.15617120038224525,
-            "angularVelocity": -3.688143550349055e-12,
-            "velocityX": 1.3328464960153679,
-            "velocityY": -0.10327716626043287,
-            "timestamp": 1.8986808406341726
-        },
-        {
-            "x": 2.5911718313337544,
-            "y": 6.897715977103231,
-            "heading": 0.1561712003117504,
-            "angularVelocity": -3.687560329325201e-12,
-            "velocityX": 1.2890863531735766,
-            "velocityY": -0.12942875706821344,
-            "timestamp": 1.9511398400698114
-        },
-        {
-            "x": 2.65624736130467,
-            "y": 6.889492702059127,
-            "heading": 0.15617120024125528,
-            "angularVelocity": -3.68944362324301e-12,
-            "velocityX": 1.240502697821755,
-            "velocityY": -0.1567561709974853,
-            "timestamp": 2.0035988395054503
-        },
-        {
-            "x": 2.7185182573768354,
-            "y": 6.879780772610566,
-            "heading": 0.1561712001707086,
-            "angularVelocity": -4.669569996251489e-12,
-            "velocityX": 1.187039346844362,
-            "velocityY": -0.1851336547556834,
-            "timestamp": 2.056057838941089
-        },
-        {
-            "x": 2.844727229024539,
-            "y": 6.826831262995891,
-            "heading": 0.36522912798339696,
-            "angularVelocity": 1.2815424227266763,
-            "velocityX": 0.7736714562084378,
-            "velocityY": -0.32458488264739044,
-            "timestamp": 2.219187770584854
-        },
-        {
-            "x": 2.905891887894138,
-            "y": 6.746880408353911,
-            "heading": 0.5636352419280429,
-            "angularVelocity": 1.2162459209838357,
-            "velocityX": 0.3749444284785087,
-            "velocityY": -0.490105365340026,
-            "timestamp": 2.382317702228619
-        },
-        {
-            "x": 2.909090532344001,
-            "y": 6.6442122814929245,
-            "heading": 0.7377210107974022,
-            "angularVelocity": 1.0671601901772823,
-            "velocityX": 0.0196079562833389,
-            "velocityY": -0.6293641266849203,
-            "timestamp": 2.5454476338723837
-        },
-        {
-            "x": 2.8528015320306115,
-            "y": 6.520167648469734,
-            "heading": 0.8617887991289785,
-            "angularVelocity": 0.7605458243522792,
-            "velocityX": -0.34505623601923857,
-            "velocityY": -0.7604038809886424,
-            "timestamp": 2.7085775655161486
-        },
-        {
-            "x": 2.6673775887021587,
-            "y": 6.358333521535233,
-            "heading": 0.8824380102910534,
-            "angularVelocity": 0.1265813757217392,
-            "velocityX": -1.1366641384675562,
-            "velocityY": -0.9920566103835737,
-            "timestamp": 2.8717074971599135
-        },
-        {
-            "x": 2.433334548523251,
-            "y": 6.191208743378355,
-            "heading": 0.7641593769894643,
-            "angularVelocity": -0.7250578241718747,
-            "velocityX": -1.4347032313583656,
-            "velocityY": -1.0244887408273557,
-            "timestamp": 3.0348374288036783
-        },
-        {
-            "x": 2.190845809692594,
-            "y": 6.024313186516238,
-            "heading": 0.6297541443819642,
-            "angularVelocity": -0.823915214372992,
-            "velocityX": -1.486476064011715,
-            "velocityY": -1.0230835952941921,
-            "timestamp": 3.1979673604474432
-        },
-        {
-            "x": 1.9580404786548902,
-            "y": 5.870547470854263,
-            "heading": 0.47066977289213696,
-            "angularVelocity": -0.9752003809508628,
-            "velocityX": -1.4271159717504125,
-            "velocityY": -0.9425965806313563,
-            "timestamp": 3.361097292091208
-        },
-        {
-            "x": 1.7498273467354928,
-            "y": 5.73486661370809,
-            "heading": 0.32338313207816166,
-            "angularVelocity": -0.9028793141986351,
-            "velocityX": -1.2763637531323493,
-            "velocityY": -0.831734899187721,
-            "timestamp": 3.524227223734973
-        },
-        {
-            "x": 1.577014249967444,
-            "y": 5.624033816048722,
-            "heading": 0.19882585300104294,
-            "angularVelocity": -0.7635464431083702,
-            "velocityX": -1.0593586039568081,
-            "velocityY": -0.6794142353068856,
-            "timestamp": 3.687357155378738
-        },
-        {
-            "x": 1.4506486184747536,
-            "y": 5.5441849844920235,
-            "heading": 0.1067515452077215,
-            "angularVelocity": -0.5644231371867715,
-            "velocityX": -0.7746317925903302,
-            "velocityY": -0.4894799553714506,
-            "timestamp": 3.8504870870225028
-        },
-        {
-            "x": 1.3818649056319274,
-            "y": 5.50129938101995,
-            "heading": 0.05628563946839357,
-            "angularVelocity": -0.30936018439404245,
-            "velocityX": -0.4216498592955241,
-            "velocityY": -0.2628923039688523,
-            "timestamp": 4.013617018666268
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/amp 4 local sgmt 2.traj b/src/main/deploy/choreo/amp 4 local sgmt 2.traj
deleted file mode 100644
index ad2ef04b..00000000
--- a/src/main/deploy/choreo/amp 4 local sgmt 2.traj	
+++ /dev/null
@@ -1,193 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.3818649054659777,
-            "y": 5.501299381110093,
-            "heading": 0.05628563946121549,
-            "angularVelocity": -8.20711890061584e-12,
-            "velocityX": -2.918186949691226e-10,
-            "velocityY": -8.516677331367793e-11,
-            "timestamp": 0
-        },
-        {
-            "x": 1.4254749911225055,
-            "y": 5.4996771535618185,
-            "heading": 0.052286234874463405,
-            "angularVelocity": -0.03818204740477268,
-            "velocityX": 0.41634256351427257,
-            "velocityY": -0.015487297221703413,
-            "timestamp": 0.10474568177260579
-        },
-        {
-            "x": 1.5054277602383004,
-            "y": 5.496460245033646,
-            "heading": 0.04534031368940726,
-            "angularVelocity": -0.06631224378557163,
-            "velocityX": 0.763303725485265,
-            "velocityY": -0.030711609603001202,
-            "timestamp": 0.20949136354521158
-        },
-        {
-            "x": 1.614706352195534,
-            "y": 5.491742221647794,
-            "heading": 0.036622661203774276,
-            "angularVelocity": -0.08322684370600701,
-            "velocityX": 1.0432753896642522,
-            "velocityY": -0.045042652489690874,
-            "timestamp": 0.3142370453178174
-        },
-        {
-            "x": 1.7468754648697162,
-            "y": 5.485736649446459,
-            "heading": 0.02746720464328704,
-            "angularVelocity": -0.08740652985101428,
-            "velocityX": 1.2618096559898304,
-            "velocityY": -0.05733479469253905,
-            "timestamp": 0.41898272709042317
-        },
-        {
-            "x": 1.8974421226741423,
-            "y": 5.478814465512338,
-            "heading": 0.0192537076414664,
-            "angularVelocity": -0.07841370510897432,
-            "velocityX": 1.43744978567254,
-            "velocityY": -0.06608562544064354,
-            "timestamp": 0.523728408863029
-        },
-        {
-            "x": 2.0478771487383725,
-            "y": 5.472018862260595,
-            "heading": 0.011842428389913316,
-            "angularVelocity": -0.07075498604111852,
-            "velocityX": 1.436193106275662,
-            "velocityY": -0.06487716815402857,
-            "timestamp": 0.6284740906356348
-        },
-        {
-            "x": 2.1991998312667134,
-            "y": 5.465297323381209,
-            "heading": 0.007319920190768939,
-            "angularVelocity": -0.04317608251475354,
-            "velocityX": 1.4446675030153622,
-            "velocityY": -0.06417008055763802,
-            "timestamp": 0.7332197724082405
-        },
-        {
-            "x": 2.337086103631991,
-            "y": 5.4645313180146555,
-            "heading": 0.0021217678335590726,
-            "angularVelocity": -0.04962641198543019,
-            "velocityX": 1.316390996187784,
-            "velocityY": -0.007313001468104138,
-            "timestamp": 0.8379654541808463
-        },
-        {
-            "x": 2.45341531313919,
-            "y": 5.464492524135499,
-            "heading": -0.0044756440905307565,
-            "angularVelocity": -0.06298504924092899,
-            "velocityX": 1.1105871626237722,
-            "velocityY": -0.0003703621622037101,
-            "timestamp": 0.9427111359534521
-        },
-        {
-            "x": 2.541111145262716,
-            "y": 5.463618987693384,
-            "heading": -0.012133903650681083,
-            "angularVelocity": -0.07311289048562652,
-            "velocityX": 0.8372262287970834,
-            "velocityY": -0.008339593435811936,
-            "timestamp": 1.047456817726058
-        },
-        {
-            "x": 2.592916727068705,
-            "y": 5.461223602405139,
-            "heading": -0.020267736871702392,
-            "angularVelocity": -0.07765316033563659,
-            "velocityX": 0.4945844156915881,
-            "velocityY": -0.022868582319897673,
-            "timestamp": 1.1522024994986637
-        },
-        {
-            "x": 2.573983182200651,
-            "y": 5.456183859404588,
-            "heading": -0.02941680850548689,
-            "angularVelocity": -0.07029420270393813,
-            "velocityX": -0.14547032678251723,
-            "velocityY": -0.03872138453206129,
-            "timestamp": 1.2823564991476335
-        },
-        {
-            "x": 2.4879155076147748,
-            "y": 5.449739339031148,
-            "heading": -0.03668510065770142,
-            "angularVelocity": -0.05584378637679435,
-            "velocityX": -0.6612756796636089,
-            "velocityY": -0.04951457856894505,
-            "timestamp": 1.4125104987966033
-        },
-        {
-            "x": 2.35046961428428,
-            "y": 5.4436438239165845,
-            "heading": -0.03977633600135786,
-            "angularVelocity": -0.02375059815495546,
-            "velocityX": -1.056025121733672,
-            "velocityY": -0.046833099245860266,
-            "timestamp": 1.542664498445573
-        },
-        {
-            "x": 2.1764781877651598,
-            "y": 5.443212327064482,
-            "heading": -0.032264347817817784,
-            "angularVelocity": 0.05771615319957162,
-            "velocityX": -1.336811984121226,
-            "velocityY": -0.0033152797571240995,
-            "timestamp": 1.6728184980945429
-        },
-        {
-            "x": 1.9752868983122995,
-            "y": 5.4534701667830054,
-            "heading": -0.009494098721117665,
-            "angularVelocity": 0.174948516049147,
-            "velocityX": -1.5457941360881122,
-            "velocityY": 0.07881309585526655,
-            "timestamp": 1.8029724977435126
-        },
-        {
-            "x": 1.7725080662079153,
-            "y": 5.4634789711909475,
-            "heading": 0.004869525328448035,
-            "angularVelocity": 0.11035868346800264,
-            "velocityX": -1.5579915534229911,
-            "velocityY": 0.07689970624561784,
-            "timestamp": 1.9331264973924824
-        },
-        {
-            "x": 1.5970859275969194,
-            "y": 5.473760403676058,
-            "heading": 0.019510408429697427,
-            "angularVelocity": 0.11248892189634652,
-            "velocityX": -1.347804439948399,
-            "velocityY": 0.07899436394267238,
-            "timestamp": 2.063280497041452
-        },
-        {
-            "x": 1.463998994938607,
-            "y": 5.481958295434183,
-            "heading": 0.031093440957313423,
-            "angularVelocity": 0.0889948258110792,
-            "velocityX": -1.0225343286428261,
-            "velocityY": 0.06298609118419185,
-            "timestamp": 2.193434496690422
-        },
-        {
-            "x": 1.3892531402609951,
-            "y": 5.486522674152253,
-            "heading": 0.03752685288213878,
-            "angularVelocity": 0.049429229542960515,
-            "velocityX": -0.5742878042328909,
-            "velocityY": 0.03506906174326123,
-            "timestamp": 2.323588496339392
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/amp 4 local sgmt 3.traj b/src/main/deploy/choreo/amp 4 local sgmt 3.traj
deleted file mode 100644
index 104394ca..00000000
--- a/src/main/deploy/choreo/amp 4 local sgmt 3.traj	
+++ /dev/null
@@ -1,229 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 1.3892531401370203,
-            "y": 5.486522674374907,
-            "heading": 0.03752685288891443,
-            "angularVelocity": 5.018347137854837e-11,
-            "velocityX": -8.981333628450582e-10,
-            "velocityY": 1.1863234183511445e-9,
-            "timestamp": 0
-        },
-        {
-            "x": 1.410785208000178,
-            "y": 5.466798705442517,
-            "heading": 0.02538794622123859,
-            "angularVelocity": -0.12074981514388294,
-            "velocityX": 0.21418676347263918,
-            "velocityY": -0.1962009963268651,
-            "timestamp": 0.1005294019105829
-        },
-        {
-            "x": 1.4512665581597533,
-            "y": 5.429718184658836,
-            "heading": 0.002445244265967334,
-            "angularVelocity": -0.2282188250282345,
-            "velocityX": 0.4026816906907998,
-            "velocityY": -0.36885249450974994,
-            "timestamp": 0.2010588038211658
-        },
-        {
-            "x": 1.5081514675148215,
-            "y": 5.3776444456377,
-            "heading": -0.029986935931297812,
-            "angularVelocity": -0.3226138782330449,
-            "velocityX": 0.5658534482437384,
-            "velocityY": -0.5179951130124598,
-            "timestamp": 0.3015882057317487
-        },
-        {
-            "x": 1.578941487701644,
-            "y": 5.312948064530918,
-            "heading": -0.07059739560458454,
-            "angularVelocity": -0.4039659933419245,
-            "velocityX": 0.7041722939494359,
-            "velocityY": -0.6435568078574709,
-            "timestamp": 0.4021176076423316
-        },
-        {
-            "x": 1.6611946188865518,
-            "y": 5.238029568056684,
-            "heading": -0.11800108662062794,
-            "angularVelocity": -0.4715405656932785,
-            "velocityX": 0.8181997404499256,
-            "velocityY": -0.7452396506196601,
-            "timestamp": 0.5026470095529145
-        },
-        {
-            "x": 1.7524893345353523,
-            "y": 5.155358397604862,
-            "heading": -0.17044488274189246,
-            "angularVelocity": -0.521676197753198,
-            "velocityX": 0.9081394425873396,
-            "velocityY": -0.8223581237255294,
-            "timestamp": 0.6031764114634974
-        },
-        {
-            "x": 1.8499500532222615,
-            "y": 5.067431436588914,
-            "heading": -0.22499900927916092,
-            "angularVelocity": -0.5426683689609221,
-            "velocityX": 0.9694747626122975,
-            "velocityY": -0.8746392520744162,
-            "timestamp": 0.7037058133740803
-        },
-        {
-            "x": 1.9473638091809122,
-            "y": 4.979075974351683,
-            "heading": -0.27982409115325835,
-            "angularVelocity": -0.5453636534436577,
-            "velocityX": 0.9690076084526664,
-            "velocityY": -0.8789016988127862,
-            "timestamp": 0.8042352152846632
-        },
-        {
-            "x": 2.05818821772957,
-            "y": 4.854821291737014,
-            "heading": -0.34544281347225186,
-            "angularVelocity": -0.6527316496725523,
-            "velocityX": 1.1024079106043232,
-            "velocityY": -1.2360033993617228,
-            "timestamp": 0.9047646171952461
-        },
-        {
-            "x": 2.1664425710584037,
-            "y": 4.732231232720307,
-            "heading": -0.4041393246178968,
-            "angularVelocity": -0.5838740711290391,
-            "velocityX": 1.076842701113489,
-            "velocityY": -1.2194448247922516,
-            "timestamp": 1.005294019105829
-        },
-        {
-            "x": 2.2666201121554423,
-            "y": 4.61627032113389,
-            "heading": -0.4602072656334145,
-            "angularVelocity": -0.557726794034664,
-            "velocityX": 0.996499915039965,
-            "velocityY": -1.153502450677793,
-            "timestamp": 1.105823421016412
-        },
-        {
-            "x": 2.356480498307365,
-            "y": 4.5087945921361055,
-            "heading": -0.5124957847583047,
-            "angularVelocity": -0.5201316047807618,
-            "velocityX": 0.8938716814183492,
-            "velocityY": -1.0690974663909212,
-            "timestamp": 1.2063528229269949
-        },
-        {
-            "x": 2.433798199341425,
-            "y": 4.411691494217003,
-            "heading": -0.5602485683833155,
-            "angularVelocity": -0.47501310796660834,
-            "velocityX": 0.7691053456052521,
-            "velocityY": -0.9659173928845837,
-            "timestamp": 1.3068822248375778
-        },
-        {
-            "x": 2.4961951429135376,
-            "y": 4.327016530758609,
-            "heading": -0.6027268742617791,
-            "angularVelocity": -0.4225460918867537,
-            "velocityX": 0.620683518971687,
-            "velocityY": -0.8422905312450144,
-            "timestamp": 1.4074116267481598
-        },
-        {
-            "x": 2.541198492257727,
-            "y": 4.256927967764492,
-            "heading": -0.6391760227839379,
-            "angularVelocity": -0.3625720221944257,
-            "velocityX": 0.4476635477259144,
-            "velocityY": -0.6971946668803632,
-            "timestamp": 1.5079410286587427
-        },
-        {
-            "x": 2.5331758624705114,
-            "y": 4.192320334785442,
-            "heading": -0.6763224470307224,
-            "angularVelocity": -0.2449990301925222,
-            "velocityX": -0.052913214713167914,
-            "velocityY": -0.426119279886695,
-            "timestamp": 1.6595596871052694
-        },
-        {
-            "x": 2.46411262676954,
-            "y": 4.160473976285577,
-            "heading": -0.7018758155970488,
-            "angularVelocity": -0.16853709734646205,
-            "velocityX": -0.45550618406914944,
-            "velocityY": -0.21004248027074415,
-            "timestamp": 1.811178345551796
-        },
-        {
-            "x": 2.348779581051416,
-            "y": 4.153307846262027,
-            "heading": -0.7202693675207003,
-            "angularVelocity": -0.121314567375086,
-            "velocityX": -0.7606784530447123,
-            "velocityY": -0.04726417363495295,
-            "timestamp": 1.9627970039983227
-        },
-        {
-            "x": 2.201596779448434,
-            "y": 4.163036990055964,
-            "heading": -0.7333684069992377,
-            "angularVelocity": -0.0863946406365267,
-            "velocityX": -0.9707433369527995,
-            "velocityY": 0.06416850879734587,
-            "timestamp": 2.1144156624448494
-        },
-        {
-            "x": 2.03723470076713,
-            "y": 4.1831659239379135,
-            "heading": -0.7500897294060416,
-            "angularVelocity": -0.11028538710126731,
-            "velocityX": -1.0840491612778367,
-            "velocityY": 0.13276026521150996,
-            "timestamp": 2.266034320891376
-        },
-        {
-            "x": 1.8800877783569956,
-            "y": 4.195672966501764,
-            "heading": -0.7289968148061016,
-            "angularVelocity": 0.13911819814698503,
-            "velocityX": -1.0364616391016628,
-            "velocityY": 0.08249012434493122,
-            "timestamp": 2.4176529793379027
-        },
-        {
-            "x": 1.7046442157621393,
-            "y": 4.233321160679635,
-            "heading": -0.7238359462014516,
-            "angularVelocity": 0.03403847932720532,
-            "velocityX": -1.1571370254440554,
-            "velocityY": 0.24830844691554524,
-            "timestamp": 2.5692716377844294
-        },
-        {
-            "x": 1.572815192920781,
-            "y": 4.2658136358994225,
-            "heading": -0.7241682787394389,
-            "angularVelocity": -0.002191897597681109,
-            "velocityX": -0.8694775764003041,
-            "velocityY": 0.21430393183203963,
-            "timestamp": 2.720890296230956
-        },
-        {
-            "x": 1.4994455554717883,
-            "y": 4.286480902556393,
-            "heading": -0.7254698893818832,
-            "angularVelocity": -0.00858476582372283,
-            "velocityX": -0.4839090323218549,
-            "velocityY": 0.13631083596916982,
-            "timestamp": 2.8725089546774827
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/amp 4 local.traj b/src/main/deploy/choreo/amp 4 local.traj
deleted file mode 100644
index 6dfc86f9..00000000
--- a/src/main/deploy/choreo/amp 4 local.traj	
+++ /dev/null
@@ -1,760 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 0.7470832465334709,
-            "y": 6.625458718233556,
-            "heading": 1.0564954979067924,
-            "angularVelocity": -5.684301329861994e-12,
-            "velocityX": -3.602456269610706e-10,
-            "velocityY": 4.1137821262407583e-10,
-            "timestamp": 0
-        },
-        {
-            "x": 0.7589569772775385,
-            "y": 6.629936126338657,
-            "heading": 1.0434265097431823,
-            "angularVelocity": -0.12778392744309977,
-            "velocityX": 0.11609712676013798,
-            "velocityY": 0.04377849867759475,
-            "timestamp": 0.10227411521599863
-        },
-        {
-            "x": 0.7822022838432713,
-            "y": 6.638629979602176,
-            "heading": 1.0183346599429852,
-            "angularVelocity": -0.2453392014361959,
-            "velocityX": 0.2272843592055021,
-            "velocityY": 0.08500540295845989,
-            "timestamp": 0.20454823043199727
-        },
-        {
-            "x": 0.816345318524229,
-            "y": 6.6512661070932495,
-            "heading": 0.9822355978698938,
-            "angularVelocity": -0.35296381641965074,
-            "velocityX": 0.33383847695279084,
-            "velocityY": 0.12355156118457596,
-            "timestamp": 0.30682234564799593
-        },
-        {
-            "x": 0.8609432271767217,
-            "y": 6.667555740683275,
-            "heading": 0.9361120278121537,
-            "angularVelocity": -0.45097989764859636,
-            "velocityX": 0.4360625240585109,
-            "velocityY": 0.15927424713723962,
-            "timestamp": 0.40909646086399454
-        },
-        {
-            "x": 0.9155883048461597,
-            "y": 6.687191502411657,
-            "heading": 0.8809291072748399,
-            "angularVelocity": -0.5395590113414345,
-            "velocityX": 0.5343001768187209,
-            "velocityY": 0.19199150048411856,
-            "timestamp": 0.5113705760799931
-        },
-        {
-            "x": 0.9799143577111046,
-            "y": 6.709840447255909,
-            "heading": 0.817662056557047,
-            "angularVelocity": -0.6186027670698295,
-            "velocityX": 0.6289573163604619,
-            "velocityY": 0.22145333556813593,
-            "timestamp": 0.6136446912959918
-        },
-        {
-            "x": 1.0536065024635548,
-            "y": 6.735133328172977,
-            "heading": 0.7473295527982347,
-            "angularVelocity": -0.687686259272288,
-            "velocityX": 0.7205356386732681,
-            "velocityY": 0.24730480411634378,
-            "timestamp": 0.7159188065119904
-        },
-        {
-            "x": 1.1364169197016813,
-            "y": 6.762648211314195,
-            "heading": 0.6710254557943266,
-            "angularVelocity": -0.7460743786173882,
-            "velocityX": 0.8096908708297301,
-            "velocityY": 0.2690307532427302,
-            "timestamp": 0.818192921727989
-        },
-        {
-            "x": 1.2281932228229642,
-            "y": 6.7918845460908575,
-            "heading": 0.5899378478266369,
-            "angularVelocity": -0.7928458510838684,
-            "velocityX": 0.8973561208709863,
-            "velocityY": 0.28586249636547506,
-            "timestamp": 0.9204670369439876
-        },
-        {
-            "x": 1.3289409008958384,
-            "y": 6.822217824239773,
-            "heading": 0.5053285615301966,
-            "angularVelocity": -0.8272795718879136,
-            "velocityX": 0.985075041075245,
-            "velocityY": 0.296588019337818,
-            "timestamp": 1.0227411521599863
-        },
-        {
-            "x": 1.4390031913745853,
-            "y": 6.852792823991799,
-            "heading": 0.4183893727045359,
-            "angularVelocity": -0.8500605319021087,
-            "velocityX": 1.076150015287458,
-            "velocityY": 0.29895148738224403,
-            "timestamp": 1.125015267375985
-        },
-        {
-            "x": 1.5597885498551032,
-            "y": 6.882059112916124,
-            "heading": 0.32979940511385675,
-            "angularVelocity": -0.8662012607776253,
-            "velocityX": 1.1809963684348201,
-            "velocityY": 0.28615537738935176,
-            "timestamp": 1.2272893825919837
-        },
-        {
-            "x": 1.700662064049805,
-            "y": 6.9025972281907375,
-            "heading": 0.2406449492427404,
-            "angularVelocity": -0.8717206268168672,
-            "velocityX": 1.3774112254273,
-            "velocityY": 0.20081439319892427,
-            "timestamp": 1.3295634978079824
-        },
-        {
-            "x": 1.8510164004103928,
-            "y": 6.913603640101383,
-            "heading": 0.18435642050200896,
-            "angularVelocity": -0.5503692556839603,
-            "velocityX": 1.4701113396826178,
-            "velocityY": 0.10761678064700267,
-            "timestamp": 1.4318376130239812
-        },
-        {
-            "x": 2.0039488544462833,
-            "y": 6.918843013673937,
-            "heading": 0.15617120081925578,
-            "angularVelocity": -0.2755850738942481,
-            "velocityX": 1.4953192592999736,
-            "velocityY": 0.051228726583383746,
-            "timestamp": 1.5341117282399799
-        },
-        {
-            "x": 2.1557445524327394,
-            "y": 6.919295783765447,
-            "heading": 0.15617120073466711,
-            "angularVelocity": -8.01993499988118e-11,
-            "velocityX": 1.484204462099971,
-            "velocityY": 0.0044270165813428694,
-            "timestamp": 1.6363858434559786
-        },
-        {
-            "x": 2.232501376452394,
-            "y": 6.918644981823818,
-            "heading": 0.15617120066422163,
-            "angularVelocity": -2.7881719949913897e-12,
-            "velocityX": 1.4631774376072004,
-            "velocityY": -0.012405855819009865,
-            "timestamp": 1.6888448428916174
-        },
-        {
-            "x": 2.3078780299226054,
-            "y": 6.916920762093343,
-            "heading": 0.1561712005937278,
-            "angularVelocity": -3.698975819730093e-12,
-            "velocityX": 1.436867928003674,
-            "velocityY": -0.032867889033463694,
-            "timestamp": 1.7413038423272562
-        },
-        {
-            "x": 2.381659462849091,
-            "y": 6.91403766609857,
-            "heading": 0.15617120052323402,
-            "angularVelocity": -3.687857737825465e-12,
-            "velocityX": 1.4064590265589882,
-            "velocityY": -0.05495897496281437,
-            "timestamp": 1.793762841762895
-        },
-        {
-            "x": 2.453627858185915,
-            "y": 6.909923503320222,
-            "heading": 0.15617120045273983,
-            "angularVelocity": -3.68806627753873e-12,
-            "velocityX": 1.3718979863339387,
-            "velocityY": -0.07842619311071317,
-            "timestamp": 1.8462218411985338
-        },
-        {
-            "x": 2.5235476514141553,
-            "y": 6.90450568335544,
-            "heading": 0.15617120038224525,
-            "angularVelocity": -3.688143550349055e-12,
-            "velocityX": 1.3328464960153679,
-            "velocityY": -0.10327716626043287,
-            "timestamp": 1.8986808406341726
-        },
-        {
-            "x": 2.5911718313337544,
-            "y": 6.897715977103231,
-            "heading": 0.1561712003117504,
-            "angularVelocity": -3.687560329325201e-12,
-            "velocityX": 1.2890863531735766,
-            "velocityY": -0.12942875706821344,
-            "timestamp": 1.9511398400698114
-        },
-        {
-            "x": 2.65624736130467,
-            "y": 6.889492702059127,
-            "heading": 0.15617120024125528,
-            "angularVelocity": -3.68944362324301e-12,
-            "velocityX": 1.240502697821755,
-            "velocityY": -0.1567561709974853,
-            "timestamp": 2.0035988395054503
-        },
-        {
-            "x": 2.7185182573768354,
-            "y": 6.879780772610566,
-            "heading": 0.1561712001707086,
-            "angularVelocity": -4.669569996251489e-12,
-            "velocityX": 1.187039346844362,
-            "velocityY": -0.1851336547556834,
-            "timestamp": 2.056057838941089
-        },
-        {
-            "x": 2.844727229024539,
-            "y": 6.826831262995891,
-            "heading": 0.36522912798339696,
-            "angularVelocity": 1.2815424227266763,
-            "velocityX": 0.7736714562084378,
-            "velocityY": -0.32458488264739044,
-            "timestamp": 2.219187770584854
-        },
-        {
-            "x": 2.905891887894138,
-            "y": 6.746880408353911,
-            "heading": 0.5636352419280429,
-            "angularVelocity": 1.2162459209838357,
-            "velocityX": 0.3749444284785087,
-            "velocityY": -0.490105365340026,
-            "timestamp": 2.382317702228619
-        },
-        {
-            "x": 2.909090532344001,
-            "y": 6.6442122814929245,
-            "heading": 0.7377210107974022,
-            "angularVelocity": 1.0671601901772823,
-            "velocityX": 0.0196079562833389,
-            "velocityY": -0.6293641266849203,
-            "timestamp": 2.5454476338723837
-        },
-        {
-            "x": 2.8528015320306115,
-            "y": 6.520167648469734,
-            "heading": 0.8617887991289785,
-            "angularVelocity": 0.7605458243522792,
-            "velocityX": -0.34505623601923857,
-            "velocityY": -0.7604038809886424,
-            "timestamp": 2.7085775655161486
-        },
-        {
-            "x": 2.6673775887021587,
-            "y": 6.358333521535233,
-            "heading": 0.8824380102910534,
-            "angularVelocity": 0.1265813757217392,
-            "velocityX": -1.1366641384675562,
-            "velocityY": -0.9920566103835737,
-            "timestamp": 2.8717074971599135
-        },
-        {
-            "x": 2.433334548523251,
-            "y": 6.191208743378355,
-            "heading": 0.7641593769894643,
-            "angularVelocity": -0.7250578241718747,
-            "velocityX": -1.4347032313583656,
-            "velocityY": -1.0244887408273557,
-            "timestamp": 3.0348374288036783
-        },
-        {
-            "x": 2.190845809692594,
-            "y": 6.024313186516238,
-            "heading": 0.6297541443819642,
-            "angularVelocity": -0.823915214372992,
-            "velocityX": -1.486476064011715,
-            "velocityY": -1.0230835952941921,
-            "timestamp": 3.1979673604474432
-        },
-        {
-            "x": 1.9580404786548902,
-            "y": 5.870547470854263,
-            "heading": 0.47066977289213696,
-            "angularVelocity": -0.9752003809508628,
-            "velocityX": -1.4271159717504125,
-            "velocityY": -0.9425965806313563,
-            "timestamp": 3.361097292091208
-        },
-        {
-            "x": 1.7498273467354928,
-            "y": 5.73486661370809,
-            "heading": 0.32338313207816166,
-            "angularVelocity": -0.9028793141986351,
-            "velocityX": -1.2763637531323493,
-            "velocityY": -0.831734899187721,
-            "timestamp": 3.524227223734973
-        },
-        {
-            "x": 1.577014249967444,
-            "y": 5.624033816048722,
-            "heading": 0.19882585300104294,
-            "angularVelocity": -0.7635464431083702,
-            "velocityX": -1.0593586039568081,
-            "velocityY": -0.6794142353068856,
-            "timestamp": 3.687357155378738
-        },
-        {
-            "x": 1.4506486184747536,
-            "y": 5.5441849844920235,
-            "heading": 0.1067515452077215,
-            "angularVelocity": -0.5644231371867715,
-            "velocityX": -0.7746317925903302,
-            "velocityY": -0.4894799553714506,
-            "timestamp": 3.8504870870225028
-        },
-        {
-            "x": 1.3818649056319274,
-            "y": 5.50129938101995,
-            "heading": 0.05628563946839357,
-            "angularVelocity": -0.30936018439404245,
-            "velocityX": -0.4216498592955241,
-            "velocityY": -0.2628923039688523,
-            "timestamp": 4.013617018666268
-        },
-        {
-            "x": 1.3818649054659777,
-            "y": 5.501299381110093,
-            "heading": 0.05628563946121549,
-            "angularVelocity": -8.20711890061584e-12,
-            "velocityX": -2.918186949691226e-10,
-            "velocityY": -8.516677331367793e-11,
-            "timestamp": 4.176746950310032
-        },
-        {
-            "x": 1.4254749911225055,
-            "y": 5.4996771535618185,
-            "heading": 0.052286234874463405,
-            "angularVelocity": -0.03818204740477268,
-            "velocityX": 0.41634256351427257,
-            "velocityY": -0.015487297221703413,
-            "timestamp": 4.281492632082638
-        },
-        {
-            "x": 1.5054277602383004,
-            "y": 5.496460245033646,
-            "heading": 0.04534031368940726,
-            "angularVelocity": -0.06631224378557163,
-            "velocityX": 0.763303725485265,
-            "velocityY": -0.030711609603001202,
-            "timestamp": 4.386238313855244
-        },
-        {
-            "x": 1.614706352195534,
-            "y": 5.491742221647794,
-            "heading": 0.036622661203774276,
-            "angularVelocity": -0.08322684370600701,
-            "velocityX": 1.0432753896642522,
-            "velocityY": -0.045042652489690874,
-            "timestamp": 4.4909839956278494
-        },
-        {
-            "x": 1.7468754648697162,
-            "y": 5.485736649446459,
-            "heading": 0.02746720464328704,
-            "angularVelocity": -0.08740652985101428,
-            "velocityX": 1.2618096559898304,
-            "velocityY": -0.05733479469253905,
-            "timestamp": 4.595729677400455
-        },
-        {
-            "x": 1.8974421226741423,
-            "y": 5.478814465512338,
-            "heading": 0.0192537076414664,
-            "angularVelocity": -0.07841370510897432,
-            "velocityX": 1.43744978567254,
-            "velocityY": -0.06608562544064354,
-            "timestamp": 4.700475359173061
-        },
-        {
-            "x": 2.0478771487383725,
-            "y": 5.472018862260595,
-            "heading": 0.011842428389913316,
-            "angularVelocity": -0.07075498604111852,
-            "velocityX": 1.436193106275662,
-            "velocityY": -0.06487716815402857,
-            "timestamp": 4.805221040945667
-        },
-        {
-            "x": 2.1991998312667134,
-            "y": 5.465297323381209,
-            "heading": 0.007319920190768939,
-            "angularVelocity": -0.04317608251475354,
-            "velocityX": 1.4446675030153622,
-            "velocityY": -0.06417008055763802,
-            "timestamp": 4.909966722718273
-        },
-        {
-            "x": 2.337086103631991,
-            "y": 5.4645313180146555,
-            "heading": 0.0021217678335590726,
-            "angularVelocity": -0.04962641198543019,
-            "velocityX": 1.316390996187784,
-            "velocityY": -0.007313001468104138,
-            "timestamp": 5.014712404490878
-        },
-        {
-            "x": 2.45341531313919,
-            "y": 5.464492524135499,
-            "heading": -0.0044756440905307565,
-            "angularVelocity": -0.06298504924092899,
-            "velocityX": 1.1105871626237722,
-            "velocityY": -0.0003703621622037101,
-            "timestamp": 5.119458086263484
-        },
-        {
-            "x": 2.541111145262716,
-            "y": 5.463618987693384,
-            "heading": -0.012133903650681083,
-            "angularVelocity": -0.07311289048562652,
-            "velocityX": 0.8372262287970834,
-            "velocityY": -0.008339593435811936,
-            "timestamp": 5.22420376803609
-        },
-        {
-            "x": 2.592916727068705,
-            "y": 5.461223602405139,
-            "heading": -0.020267736871702392,
-            "angularVelocity": -0.07765316033563659,
-            "velocityX": 0.4945844156915881,
-            "velocityY": -0.022868582319897673,
-            "timestamp": 5.328949449808696
-        },
-        {
-            "x": 2.573983182200651,
-            "y": 5.456183859404588,
-            "heading": -0.02941680850548689,
-            "angularVelocity": -0.07029420270393813,
-            "velocityX": -0.14547032678251723,
-            "velocityY": -0.03872138453206129,
-            "timestamp": 5.459103449457666
-        },
-        {
-            "x": 2.4879155076147748,
-            "y": 5.449739339031148,
-            "heading": -0.03668510065770142,
-            "angularVelocity": -0.05584378637679435,
-            "velocityX": -0.6612756796636089,
-            "velocityY": -0.04951457856894505,
-            "timestamp": 5.589257449106635
-        },
-        {
-            "x": 2.35046961428428,
-            "y": 5.4436438239165845,
-            "heading": -0.03977633600135786,
-            "angularVelocity": -0.02375059815495546,
-            "velocityX": -1.056025121733672,
-            "velocityY": -0.046833099245860266,
-            "timestamp": 5.719411448755605
-        },
-        {
-            "x": 2.1764781877651598,
-            "y": 5.443212327064482,
-            "heading": -0.032264347817817784,
-            "angularVelocity": 0.05771615319957162,
-            "velocityX": -1.336811984121226,
-            "velocityY": -0.0033152797571240995,
-            "timestamp": 5.849565448404575
-        },
-        {
-            "x": 1.9752868983122995,
-            "y": 5.4534701667830054,
-            "heading": -0.009494098721117665,
-            "angularVelocity": 0.174948516049147,
-            "velocityX": -1.5457941360881122,
-            "velocityY": 0.07881309585526655,
-            "timestamp": 5.979719448053545
-        },
-        {
-            "x": 1.7725080662079153,
-            "y": 5.4634789711909475,
-            "heading": 0.004869525328448035,
-            "angularVelocity": 0.11035868346800264,
-            "velocityX": -1.5579915534229911,
-            "velocityY": 0.07689970624561784,
-            "timestamp": 6.1098734477025145
-        },
-        {
-            "x": 1.5970859275969194,
-            "y": 5.473760403676058,
-            "heading": 0.019510408429697427,
-            "angularVelocity": 0.11248892189634652,
-            "velocityX": -1.347804439948399,
-            "velocityY": 0.07899436394267238,
-            "timestamp": 6.240027447351484
-        },
-        {
-            "x": 1.463998994938607,
-            "y": 5.481958295434183,
-            "heading": 0.031093440957313423,
-            "angularVelocity": 0.0889948258110792,
-            "velocityX": -1.0225343286428261,
-            "velocityY": 0.06298609118419185,
-            "timestamp": 6.370181447000454
-        },
-        {
-            "x": 1.3892531402609951,
-            "y": 5.486522674152253,
-            "heading": 0.03752685288213878,
-            "angularVelocity": 0.049429229542960515,
-            "velocityX": -0.5742878042328909,
-            "velocityY": 0.03506906174326123,
-            "timestamp": 6.500335446649424
-        },
-        {
-            "x": 1.3892531401370203,
-            "y": 5.486522674374907,
-            "heading": 0.03752685288891443,
-            "angularVelocity": 5.018347137854837e-11,
-            "velocityX": -8.981333628450582e-10,
-            "velocityY": 1.1863234183511445e-9,
-            "timestamp": 6.630489446298394
-        },
-        {
-            "x": 1.410785208000178,
-            "y": 5.466798705442517,
-            "heading": 0.02538794622123859,
-            "angularVelocity": -0.12074981514388294,
-            "velocityX": 0.21418676347263918,
-            "velocityY": -0.1962009963268651,
-            "timestamp": 6.731018848208977
-        },
-        {
-            "x": 1.4512665581597533,
-            "y": 5.429718184658836,
-            "heading": 0.002445244265967334,
-            "angularVelocity": -0.2282188250282345,
-            "velocityX": 0.4026816906907998,
-            "velocityY": -0.36885249450974994,
-            "timestamp": 6.8315482501195595
-        },
-        {
-            "x": 1.5081514675148215,
-            "y": 5.3776444456377,
-            "heading": -0.029986935931297812,
-            "angularVelocity": -0.3226138782330449,
-            "velocityX": 0.5658534482437384,
-            "velocityY": -0.5179951130124598,
-            "timestamp": 6.932077652030142
-        },
-        {
-            "x": 1.578941487701644,
-            "y": 5.312948064530918,
-            "heading": -0.07059739560458454,
-            "angularVelocity": -0.4039659933419245,
-            "velocityX": 0.7041722939494359,
-            "velocityY": -0.6435568078574709,
-            "timestamp": 7.032607053940725
-        },
-        {
-            "x": 1.6611946188865518,
-            "y": 5.238029568056684,
-            "heading": -0.11800108662062794,
-            "angularVelocity": -0.4715405656932785,
-            "velocityX": 0.8181997404499256,
-            "velocityY": -0.7452396506196601,
-            "timestamp": 7.133136455851308
-        },
-        {
-            "x": 1.7524893345353523,
-            "y": 5.155358397604862,
-            "heading": -0.17044488274189246,
-            "angularVelocity": -0.521676197753198,
-            "velocityX": 0.9081394425873396,
-            "velocityY": -0.8223581237255294,
-            "timestamp": 7.233665857761891
-        },
-        {
-            "x": 1.8499500532222615,
-            "y": 5.067431436588914,
-            "heading": -0.22499900927916092,
-            "angularVelocity": -0.5426683689609221,
-            "velocityX": 0.9694747626122975,
-            "velocityY": -0.8746392520744162,
-            "timestamp": 7.334195259672474
-        },
-        {
-            "x": 1.9473638091809122,
-            "y": 4.979075974351683,
-            "heading": -0.27982409115325835,
-            "angularVelocity": -0.5453636534436577,
-            "velocityX": 0.9690076084526664,
-            "velocityY": -0.8789016988127862,
-            "timestamp": 7.434724661583057
-        },
-        {
-            "x": 2.05818821772957,
-            "y": 4.854821291737014,
-            "heading": -0.34544281347225186,
-            "angularVelocity": -0.6527316496725523,
-            "velocityX": 1.1024079106043232,
-            "velocityY": -1.2360033993617228,
-            "timestamp": 7.53525406349364
-        },
-        {
-            "x": 2.1664425710584037,
-            "y": 4.732231232720307,
-            "heading": -0.4041393246178968,
-            "angularVelocity": -0.5838740711290391,
-            "velocityX": 1.076842701113489,
-            "velocityY": -1.2194448247922516,
-            "timestamp": 7.635783465404223
-        },
-        {
-            "x": 2.2666201121554423,
-            "y": 4.61627032113389,
-            "heading": -0.4602072656334145,
-            "angularVelocity": -0.557726794034664,
-            "velocityX": 0.996499915039965,
-            "velocityY": -1.153502450677793,
-            "timestamp": 7.736312867314806
-        },
-        {
-            "x": 2.356480498307365,
-            "y": 4.5087945921361055,
-            "heading": -0.5124957847583047,
-            "angularVelocity": -0.5201316047807618,
-            "velocityX": 0.8938716814183492,
-            "velocityY": -1.0690974663909212,
-            "timestamp": 7.8368422692253885
-        },
-        {
-            "x": 2.433798199341425,
-            "y": 4.411691494217003,
-            "heading": -0.5602485683833155,
-            "angularVelocity": -0.47501310796660834,
-            "velocityX": 0.7691053456052521,
-            "velocityY": -0.9659173928845837,
-            "timestamp": 7.937371671135971
-        },
-        {
-            "x": 2.4961951429135376,
-            "y": 4.327016530758609,
-            "heading": -0.6027268742617791,
-            "angularVelocity": -0.4225460918867537,
-            "velocityX": 0.620683518971687,
-            "velocityY": -0.8422905312450144,
-            "timestamp": 8.037901073046553
-        },
-        {
-            "x": 2.541198492257727,
-            "y": 4.256927967764492,
-            "heading": -0.6391760227839379,
-            "angularVelocity": -0.3625720221944257,
-            "velocityX": 0.4476635477259144,
-            "velocityY": -0.6971946668803632,
-            "timestamp": 8.138430474957136
-        },
-        {
-            "x": 2.5331758624705114,
-            "y": 4.192320334785442,
-            "heading": -0.6763224470307224,
-            "angularVelocity": -0.2449990301925222,
-            "velocityX": -0.052913214713167914,
-            "velocityY": -0.426119279886695,
-            "timestamp": 8.290049133403663
-        },
-        {
-            "x": 2.46411262676954,
-            "y": 4.160473976285577,
-            "heading": -0.7018758155970488,
-            "angularVelocity": -0.16853709734646205,
-            "velocityX": -0.45550618406914944,
-            "velocityY": -0.21004248027074415,
-            "timestamp": 8.44166779185019
-        },
-        {
-            "x": 2.348779581051416,
-            "y": 4.153307846262027,
-            "heading": -0.7202693675207003,
-            "angularVelocity": -0.121314567375086,
-            "velocityX": -0.7606784530447123,
-            "velocityY": -0.04726417363495295,
-            "timestamp": 8.593286450296716
-        },
-        {
-            "x": 2.201596779448434,
-            "y": 4.163036990055964,
-            "heading": -0.7333684069992377,
-            "angularVelocity": -0.0863946406365267,
-            "velocityX": -0.9707433369527995,
-            "velocityY": 0.06416850879734587,
-            "timestamp": 8.744905108743243
-        },
-        {
-            "x": 2.03723470076713,
-            "y": 4.1831659239379135,
-            "heading": -0.7500897294060416,
-            "angularVelocity": -0.11028538710126731,
-            "velocityX": -1.0840491612778367,
-            "velocityY": 0.13276026521150996,
-            "timestamp": 8.89652376718977
-        },
-        {
-            "x": 1.8800877783569956,
-            "y": 4.195672966501764,
-            "heading": -0.7289968148061016,
-            "angularVelocity": 0.13911819814698503,
-            "velocityX": -1.0364616391016628,
-            "velocityY": 0.08249012434493122,
-            "timestamp": 9.048142425636296
-        },
-        {
-            "x": 1.7046442157621393,
-            "y": 4.233321160679635,
-            "heading": -0.7238359462014516,
-            "angularVelocity": 0.03403847932720532,
-            "velocityX": -1.1571370254440554,
-            "velocityY": 0.24830844691554524,
-            "timestamp": 9.199761084082823
-        },
-        {
-            "x": 1.572815192920781,
-            "y": 4.2658136358994225,
-            "heading": -0.7241682787394389,
-            "angularVelocity": -0.002191897597681109,
-            "velocityX": -0.8694775764003041,
-            "velocityY": 0.21430393183203963,
-            "timestamp": 9.35137974252935
-        },
-        {
-            "x": 1.4994455554717883,
-            "y": 4.286480902556393,
-            "heading": -0.7254698893818832,
-            "angularVelocity": -0.00858476582372283,
-            "velocityX": -0.4839090323218549,
-            "velocityY": 0.13631083596916982,
-            "timestamp": 9.502998400975876
-        },
-        {
-            "x": 1.4994455567524874,
-            "y": 4.286480903049883,
-            "heading": -0.7254698893413497,
-            "angularVelocity": 3.8000232610425283e-11,
-            "velocityX": 2.8946950656209713e-9,
-            "velocityY": -5.415769760526697e-10,
-            "timestamp": 9.654617059422403
-        }
-    ]
-}
\ No newline at end of file
diff --git a/src/main/deploy/choreo/test.traj b/src/main/deploy/choreo/test.traj
deleted file mode 100644
index 9ee66c72..00000000
--- a/src/main/deploy/choreo/test.traj
+++ /dev/null
@@ -1,1084 +0,0 @@
-{
-    "samples": [
-        {
-            "x": 2.1199185848236084,
-            "y": 5.957952976226807,
-            "heading": -6.25322258455275e-30,
-            "angularVelocity": 0,
-            "velocityX": 0,
-            "velocityY": 0,
-            "timestamp": 0
-        },
-        {
-            "x": 2.157285981558088,
-            "y": 5.958401136866338,
-            "heading": 0.034313098562848664,
-            "angularVelocity": 0.4715592535645141,
-            "velocityX": 0.5135339695272734,
-            "velocityY": 0.006158997744474669,
-            "timestamp": 0.07276518974757916
-        },
-        {
-            "x": 2.2137097634050407,
-            "y": 5.959057625247641,
-            "heading": 0.03431310711765102,
-            "angularVelocity": 1.1756723768181273e-7,
-            "velocityX": 0.7754227267555416,
-            "velocityY": 0.009022011535733282,
-            "timestamp": 0.14553037949515832
-        },
-        {
-            "x": 2.270133545252147,
-            "y": 5.959714113615942,
-            "heading": 0.03431311567244465,
-            "angularVelocity": 1.1756711762076828e-7,
-            "velocityX": 0.7754227267576637,
-            "velocityY": 0.009022011357058456,
-            "timestamp": 0.21829556924273746
-        },
-        {
-            "x": 2.3265573270992537,
-            "y": 5.960370601984243,
-            "heading": 0.03431312422723829,
-            "angularVelocity": 1.1756711788910916e-7,
-            "velocityX": 0.7754227267576637,
-            "velocityY": 0.009022011357058367,
-            "timestamp": 0.29106075899031664
-        },
-        {
-            "x": 2.38298110894636,
-            "y": 5.961027090352544,
-            "heading": 0.0343131327820319,
-            "angularVelocity": 1.175671175035012e-7,
-            "velocityX": 0.7754227267576638,
-            "velocityY": 0.009022011357058381,
-            "timestamp": 0.3638259487378958
-        },
-        {
-            "x": 2.4394048907934667,
-            "y": 5.961683578720844,
-            "heading": 0.034313141336825545,
-            "angularVelocity": 1.1756711790299388e-7,
-            "velocityX": 0.7754227267576637,
-            "velocityY": 0.009022011357058397,
-            "timestamp": 0.436591138485475
-        },
-        {
-            "x": 2.495828672640573,
-            "y": 5.962340067089145,
-            "heading": 0.03431314989161917,
-            "angularVelocity": 1.1756711758579864e-7,
-            "velocityX": 0.7754227267576637,
-            "velocityY": 0.0090220113570584,
-            "timestamp": 0.5093563282330541
-        },
-        {
-            "x": 2.5522524544876797,
-            "y": 5.962996555457446,
-            "heading": 0.0343131584464128,
-            "angularVelocity": 1.175671177574276e-7,
-            "velocityX": 0.7754227267576639,
-            "velocityY": 0.009022011357058414,
-            "timestamp": 0.5821215179806333
-        },
-        {
-            "x": 2.6086762363347864,
-            "y": 5.963653043825747,
-            "heading": 0.034313167001206415,
-            "angularVelocity": 1.1756711751242314e-7,
-            "velocityX": 0.7754227267576638,
-            "velocityY": 0.009022011357058426,
-            "timestamp": 0.6548867077282124
-        },
-        {
-            "x": 2.6651000181818927,
-            "y": 5.964309532194048,
-            "heading": 0.03431317555600002,
-            "angularVelocity": 1.1756711746464553e-7,
-            "velocityX": 0.7754227267576638,
-            "velocityY": 0.009022011357058426,
-            "timestamp": 0.7276518974757916
-        },
-        {
-            "x": 2.7215238000289994,
-            "y": 5.964966020562349,
-            "heading": 0.03431318411079364,
-            "angularVelocity": 1.1756711761494635e-7,
-            "velocityX": 0.7754227267576637,
-            "velocityY": 0.009022011357058443,
-            "timestamp": 0.8004170872233708
-        },
-        {
-            "x": 2.7779475818761057,
-            "y": 5.965622508930649,
-            "heading": 0.034313192665587286,
-            "angularVelocity": 1.1756711789914241e-7,
-            "velocityX": 0.7754227267576637,
-            "velocityY": 0.009022011357058452,
-            "timestamp": 0.87318227697095
-        },
-        {
-            "x": 2.8343713637232124,
-            "y": 5.96627899729895,
-            "heading": 0.03431320122038093,
-            "angularVelocity": 1.1756711786542428e-7,
-            "velocityX": 0.7754227267576638,
-            "velocityY": 0.009022011357058466,
-            "timestamp": 0.9459474667185291
-        },
-        {
-            "x": 2.8907951455703187,
-            "y": 5.966935485667251,
-            "heading": 0.03431320977517455,
-            "angularVelocity": 1.1756711758478168e-7,
-            "velocityX": 0.7754227267576637,
-            "velocityY": 0.009022011357058475,
-            "timestamp": 1.0187126564661082
-        },
-        {
-            "x": 2.9472189274174254,
-            "y": 5.967591974035552,
-            "heading": 0.03431321832996816,
-            "angularVelocity": 1.1756711747039204e-7,
-            "velocityX": 0.7754227267576637,
-            "velocityY": 0.009022011357058483,
-            "timestamp": 1.0914778462136874
-        },
-        {
-            "x": 3.0036427092645317,
-            "y": 5.968248462403853,
-            "heading": 0.034313226884761766,
-            "angularVelocity": 1.1756711737664598e-7,
-            "velocityX": 0.7754227267576638,
-            "velocityY": 0.009022011357058489,
-            "timestamp": 1.1642430359612665
-        },
-        {
-            "x": 3.0600664911116384,
-            "y": 5.968904950772154,
-            "heading": 0.03431323543955538,
-            "angularVelocity": 1.1756711759331021e-7,
-            "velocityX": 0.7754227267576637,
-            "velocityY": 0.009022011357058496,
-            "timestamp": 1.2370082257088457
-        },
-        {
-            "x": 3.116490272958745,
-            "y": 5.969561439140454,
-            "heading": 0.034313243994349,
-            "angularVelocity": 1.1756711756317827e-7,
-            "velocityX": 0.7754227267576638,
-            "velocityY": 0.009022011357058518,
-            "timestamp": 1.309773415456425
-        },
-        {
-            "x": 3.1729140548058514,
-            "y": 5.970217927508755,
-            "heading": 0.03431325254914262,
-            "angularVelocity": 1.1756711755305748e-7,
-            "velocityX": 0.7754227267576637,
-            "velocityY": 0.009022011357058523,
-            "timestamp": 1.382538605204004
-        },
-        {
-            "x": 3.229337836652958,
-            "y": 5.970874415877057,
-            "heading": 0.03431326110393624,
-            "angularVelocity": 1.175671175226974e-7,
-            "velocityX": 0.7754227267576638,
-            "velocityY": 0.00902201135705854,
-            "timestamp": 1.4553037949515832
-        },
-        {
-            "x": 3.2857616185000644,
-            "y": 5.971530904245357,
-            "heading": 0.03431326965872984,
-            "angularVelocity": 1.1756711738766866e-7,
-            "velocityX": 0.7754227267576638,
-            "velocityY": 0.009022011357058546,
-            "timestamp": 1.5280689846991624
-        },
-        {
-            "x": 3.342185400347171,
-            "y": 5.972187392613658,
-            "heading": 0.03431327821352346,
-            "angularVelocity": 1.1756711754575316e-7,
-            "velocityX": 0.7754227267576638,
-            "velocityY": 0.009022011357058565,
-            "timestamp": 1.6008341744467416
-        },
-        {
-            "x": 3.3986091821942774,
-            "y": 5.972843880981959,
-            "heading": 0.03431328676831708,
-            "angularVelocity": 1.1756711769845501e-7,
-            "velocityX": 0.7754227267576638,
-            "velocityY": 0.009022011357058572,
-            "timestamp": 1.6735993641943208
-        },
-        {
-            "x": 3.455032964041384,
-            "y": 5.973500369350259,
-            "heading": 0.034313295323110705,
-            "angularVelocity": 1.175671175699485e-7,
-            "velocityX": 0.7754227267576638,
-            "velocityY": 0.009022011357058579,
-            "timestamp": 1.7463645539419
-        },
-        {
-            "x": 3.5114567458884904,
-            "y": 5.97415685771856,
-            "heading": 0.03431330387790433,
-            "angularVelocity": 1.1756711766849268e-7,
-            "velocityX": 0.7754227267576638,
-            "velocityY": 0.009022011357058603,
-            "timestamp": 1.819129743689479
-        },
-        {
-            "x": 3.567880527735597,
-            "y": 5.9748133460868615,
-            "heading": 0.03431331243269794,
-            "angularVelocity": 1.1756711756878859e-7,
-            "velocityX": 0.7754227267576637,
-            "velocityY": 0.009022011357058601,
-            "timestamp": 1.8918949334370583
-        },
-        {
-            "x": 3.624304309582704,
-            "y": 5.975469834455162,
-            "heading": 0.03431332098749156,
-            "angularVelocity": 1.1756711758891969e-7,
-            "velocityX": 0.7754227267576638,
-            "velocityY": 0.009022011357058617,
-            "timestamp": 1.9646601231846375
-        },
-        {
-            "x": 3.68072809142981,
-            "y": 5.976126322823463,
-            "heading": 0.03431332954228517,
-            "angularVelocity": 1.1756711749627294e-7,
-            "velocityX": 0.7754227267576637,
-            "velocityY": 0.009022011357058617,
-            "timestamp": 2.0374253129322164
-        },
-        {
-            "x": 3.737151873276917,
-            "y": 5.976782811191764,
-            "heading": 0.03431333809707879,
-            "angularVelocity": 1.1756711754302844e-7,
-            "velocityX": 0.7754227267576637,
-            "velocityY": 0.009022011357058624,
-            "timestamp": 2.1101905026797954
-        },
-        {
-            "x": 3.793575655124023,
-            "y": 5.977439299560064,
-            "heading": 0.03431334665187242,
-            "angularVelocity": 1.175671176839478e-7,
-            "velocityX": 0.7754227267576638,
-            "velocityY": 0.009022011357058646,
-            "timestamp": 2.1829556924273743
-        },
-        {
-            "x": 3.84999943697113,
-            "y": 5.978095787928365,
-            "heading": 0.034313355206666014,
-            "angularVelocity": 1.1756711730438892e-7,
-            "velocityX": 0.7754227267576637,
-            "velocityY": 0.00902201135705865,
-            "timestamp": 2.2557208821749533
-        },
-        {
-            "x": 3.906423218818236,
-            "y": 5.9787522762966665,
-            "heading": 0.03431336376145962,
-            "angularVelocity": 1.1756711741363398e-7,
-            "velocityX": 0.7754227267576638,
-            "velocityY": 0.009022011357058672,
-            "timestamp": 2.328486071922532
-        },
-        {
-            "x": 3.962847000665343,
-            "y": 5.979408764664967,
-            "heading": 0.03431337231625322,
-            "angularVelocity": 1.1756711736937194e-7,
-            "velocityX": 0.7754227267576637,
-            "velocityY": 0.009022011357058674,
-            "timestamp": 2.401251261670111
-        },
-        {
-            "x": 4.019270782512449,
-            "y": 5.980065253033268,
-            "heading": 0.034313380871046816,
-            "angularVelocity": 1.17567117272878e-7,
-            "velocityX": 0.7754227267576638,
-            "velocityY": 0.00902201135705869,
-            "timestamp": 2.47401645141769
-        },
-        {
-            "x": 4.075694564359556,
-            "y": 5.9807217414015685,
-            "heading": 0.034313389425840425,
-            "angularVelocity": 1.1756711748988721e-7,
-            "velocityX": 0.7754227267576637,
-            "velocityY": 0.009022011357058704,
-            "timestamp": 2.546781641165269
-        },
-        {
-            "x": 4.132118346206663,
-            "y": 5.981378229769869,
-            "heading": 0.034313397980634035,
-            "angularVelocity": 1.1756711734690936e-7,
-            "velocityX": 0.7754227267576638,
-            "velocityY": 0.00902201135705871,
-            "timestamp": 2.619546830912848
-        },
-        {
-            "x": 4.188542128053769,
-            "y": 5.98203471813817,
-            "heading": 0.03431340653542763,
-            "angularVelocity": 1.1756711733140912e-7,
-            "velocityX": 0.7754227267576637,
-            "velocityY": 0.009022011357058714,
-            "timestamp": 2.692312020660427
-        },
-        {
-            "x": 4.244965909900876,
-            "y": 5.982691206506428,
-            "heading": 0.03431341509022124,
-            "angularVelocity": 1.1756711743485013e-7,
-            "velocityX": 0.7754227267576708,
-            "velocityY": 0.00902201135645878,
-            "timestamp": 2.765077210408006
-        },
-        {
-            "x": 4.30138969265204,
-            "y": 5.983347617167627,
-            "heading": 0.03431342364501558,
-            "angularVelocity": 1.1756712748463029e-7,
-            "velocityX": 0.7754227391819724,
-            "velocityY": 0.009020943441183957,
-            "timestamp": 2.837842400155585
-        },
-        {
-            "x": 4.337240695953369,
-            "y": 5.957952976226807,
-            "heading": -3.4992233566514893e-26,
-            "angularVelocity": -0.47156372111511136,
-            "velocityX": 0.4926944247063125,
-            "velocityY": -0.3489943615747365,
-            "timestamp": 2.910607589903164
-        },
-        {
-            "x": 4.337075774441293,
-            "y": 5.930386954424772,
-            "heading": -0.09015124251095946,
-            "angularVelocity": -1.224439774519884,
-            "velocityX": -0.002239974219267059,
-            "velocityY": -0.37440342007034494,
-            "timestamp": 2.9842341123234344
-        },
-        {
-            "x": 4.333264434450346,
-            "y": 5.912061346555676,
-            "heading": -0.20505051130417343,
-            "angularVelocity": -1.5605690044322955,
-            "velocityX": -0.05176585645578975,
-            "velocityY": -0.24889954416819945,
-            "timestamp": 3.057860634743705
-        },
-        {
-            "x": 4.325553986362593,
-            "y": 5.887325303672255,
-            "heading": -0.3021610086195407,
-            "angularVelocity": -1.31896080546962,
-            "velocityX": -0.10472378477610225,
-            "velocityY": -0.33596647064523955,
-            "timestamp": 3.1314871571639755
-        },
-        {
-            "x": 4.313628651056446,
-            "y": 5.857761645119009,
-            "heading": -0.3834128542416627,
-            "angularVelocity": -1.1035676132892005,
-            "velocityX": -0.16197064473690057,
-            "velocityY": -0.40153544648615047,
-            "timestamp": 3.205113679584246
-        },
-        {
-            "x": 4.297606686426737,
-            "y": 5.824666184248557,
-            "heading": -0.4508559142750475,
-            "angularVelocity": -0.9160158298447155,
-            "velocityX": -0.2176113186258289,
-            "velocityY": -0.4495046048968324,
-            "timestamp": 3.2787402020045167
-        },
-        {
-            "x": 4.277844157832715,
-            "y": 5.789044520818464,
-            "heading": -0.5065111263049437,
-            "angularVelocity": -0.7559125461910066,
-            "velocityX": -0.26841589069241406,
-            "velocityY": -0.4838156449486921,
-            "timestamp": 3.3523667244247872
-        },
-        {
-            "x": 4.254799811398479,
-            "y": 5.751649129927804,
-            "heading": -0.5522697074374715,
-            "angularVelocity": -0.6214958907244229,
-            "velocityX": -0.3129897444116032,
-            "velocityY": -0.5079065214733656,
-            "timestamp": 3.425993246845058
-        },
-        {
-            "x": 4.228951375108858,
-            "y": 5.713030269053256,
-            "heading": -0.5898366283888508,
-            "angularVelocity": -0.5102362534107197,
-            "velocityX": -0.35107506697211693,
-            "velocityY": -0.524523766776681,
-            "timestamp": 3.4996197692653284
-        },
-        {
-            "x": 4.2007493706625425,
-            "y": 5.673585962715614,
-            "heading": -0.6207093446974378,
-            "angularVelocity": -0.419315150216672,
-            "velocityX": -0.3830413758419039,
-            "velocityY": -0.5357350183197291,
-            "timestamp": 3.573246291685599
-        },
-        {
-            "x": 4.1705955025497214,
-            "y": 5.633604594294146,
-            "heading": -0.6461809126237779,
-            "angularVelocity": -0.34595641745707684,
-            "velocityX": -0.4095517093785753,
-            "velocityY": -0.5430294288958701,
-            "timestamp": 3.6468728141058695
-        },
-        {
-            "x": 4.138835739465679,
-            "y": 5.593298519659669,
-            "heading": -0.6673577962188777,
-            "angularVelocity": -0.28762574815389413,
-            "velocityX": -0.43136307460989803,
-            "velocityY": -0.5474396088464458,
-            "timestamp": 3.72049933652614
-        },
-        {
-            "x": 4.1057614035434415,
-            "y": 5.552829496334061,
-            "heading": -0.6851851845597893,
-            "angularVelocity": -0.2421326955950787,
-            "velocityX": -0.44921768453825217,
-            "velocityY": -0.549652788089118,
-            "timestamp": 3.7941258589464106
-        },
-        {
-            "x": 4.071614137002915,
-            "y": 5.512327581345269,
-            "heading": -0.700475200245641,
-            "angularVelocity": -0.20766994261353336,
-            "velocityX": -0.4637902948289247,
-            "velocityY": -0.5500995247011782,
-            "timestamp": 3.867752381366681
-        },
-        {
-            "x": 4.036592388923184,
-            "y": 5.471905240898815,
-            "heading": -0.7139354213530316,
-            "angularVelocity": -0.18281755901165134,
-            "velocityX": -0.47566755740304917,
-            "velocityY": -0.5490187383252648,
-            "timestamp": 3.9413789037869518
-        },
-        {
-            "x": 4.0008581978077,
-            "y": 5.431668194433558,
-            "heading": -0.7261965659955223,
-            "angularVelocity": -0.16653162799816065,
-            "velocityX": -0.4853440029600765,
-            "velocityY": -0.5465020639651851,
-            "timestamp": 4.015005426207223
-        },
-        {
-            "x": 3.964543737766948,
-            "y": 5.391724222217225,
-            "heading": -0.7378391147106159,
-            "angularVelocity": -0.15812981969508613,
-            "velocityX": -0.49322525153997593,
-            "velocityY": -0.5425215113152707,
-            "timestamp": 4.088631948627493
-        },
-        {
-            "x": 3.9277575195733885,
-            "y": 5.352190902296246,
-            "heading": -0.7494192294944804,
-            "angularVelocity": -0.15728183816374608,
-            "velocityX": -0.4996327000693894,
-            "velocityY": -0.536944006336704,
-            "timestamp": 4.162258471047764
-        },
-        {
-            "x": 3.89059042061129,
-            "y": 5.3132030416510085,
-            "heading": -0.761494708226674,
-            "angularVelocity": -0.16400990207394403,
-            "velocityX": -0.5048058463218383,
-            "velocityY": -0.5295355445784652,
-            "timestamp": 4.2358849934680345
-        },
-        {
-            "x": 3.8531219539867014,
-            "y": 5.274920426877835,
-            "heading": -0.7746519957947015,
-            "angularVelocity": -0.1787030968666938,
-            "velocityX": -0.5088990406298591,
-            "velocityY": -0.5199568513456381,
-            "timestamp": 4.309511515888305
-        },
-        {
-            "x": 3.8154274539330624,
-            "y": 5.23753642375436,
-            "heading": -0.7895355361070671,
-            "angularVelocity": -0.2021491688471734,
-            "velocityX": -0.5119690407007602,
-            "velocityY": -0.507751852112243,
-            "timestamp": 4.383138038308576
-        },
-        {
-            "x": 3.7775872299458775,
-            "y": 5.201287866945812,
-            "heading": -0.8068810412551409,
-            "angularVelocity": -0.23558772814317216,
-            "velocityX": -0.5139482722161929,
-            "velocityY": -0.49233014974734224,
-            "timestamp": 4.456764560728846
-        },
-        {
-            "x": 3.7396993230404707,
-            "y": 5.166466510716887,
-            "heading": -0.8275545372921488,
-            "angularVelocity": -0.2807887070776005,
-            "velocityX": -0.5145959045727775,
-            "velocityY": -0.4729458228402956,
-            "timestamp": 4.530391083149117
-        },
-        {
-            "x": 3.701898417706247,
-            "y": 5.133430805056498,
-            "heading": -0.8525964909440766,
-            "angularVelocity": -0.3401213697013267,
-            "velocityX": -0.5134142438298316,
-            "velocityY": -0.44869300592275085,
-            "timestamp": 4.604017605569387
-        },
-        {
-            "x": 3.664343129524347,
-            "y": 5.101026571041832,
-            "heading": -0.8792757408403825,
-            "angularVelocity": -0.36235922897480916,
-            "velocityX": -0.5100782564132129,
-            "velocityY": -0.44011631881374225,
-            "timestamp": 4.677644127989658
-        },
-        {
-            "x": 3.6270751479601078,
-            "y": 5.068774406382283,
-            "heading": -0.9064492634455257,
-            "angularVelocity": -0.3690724716024585,
-            "velocityX": -0.5061760400893075,
-            "velocityY": -0.4380509033884428,
-            "timestamp": 4.7512706504099285
-        },
-        {
-            "x": 3.590111595107405,
-            "y": 5.036682857394856,
-            "heading": -0.9341662862028731,
-            "angularVelocity": -0.37645432442313,
-            "velocityX": -0.5020412704230309,
-            "velocityY": -0.43586941135483154,
-            "timestamp": 4.824897172830199
-        },
-        {
-            "x": 3.55347222485786,
-            "y": 5.004762522170154,
-            "heading": -0.9624850330743058,
-            "angularVelocity": -0.38462697870999835,
-            "velocityX": -0.4976382021739686,
-            "velocityY": -0.4335439753964831,
-            "timestamp": 4.89852369525047
-        },
-        {
-            "x": 3.517180102788644,
-            "y": 4.973026350131823,
-            "heading": -0.9914744586380927,
-            "angularVelocity": -0.3937361783612594,
-            "velocityX": -0.49292185582330617,
-            "velocityY": -0.4310426595619462,
-            "timestamp": 4.97215021767074
-        },
-        {
-            "x": 3.4812625207112933,
-            "y": 4.941490315870616,
-            "heading": -1.0212172545129166,
-            "angularVelocity": -0.40396850071293,
-            "velocityX": -0.48783483039342795,
-            "velocityY": -0.42832437584372535,
-            "timestamp": 5.045776740091011
-        },
-        {
-            "x": 3.445752274995712,
-            "y": 4.9101744053392995,
-            "heading": -1.051814154831974,
-            "angularVelocity": -0.41556900031765764,
-            "velocityX": -0.4823023626307331,
-            "velocityY": -0.4253346416738305,
-            "timestamp": 5.119403262511281
-        },
-        {
-            "x": 3.4106895099559638,
-            "y": 4.879104067704057,
-            "heading": -1.0833902078498774,
-            "angularVelocity": -0.4288679130824993,
-            "velocityX": -0.4762246523013149,
-            "velocityY": -0.4219992553483426,
-            "timestamp": 5.193029784931552
-        },
-        {
-            "x": 3.376124474151927,
-            "y": 4.848312423976759,
-            "heading": -1.1161042305606605,
-            "angularVelocity": -0.44432388812345175,
-            "velocityX": -0.4694644629109969,
-            "velocityY": -0.4182140173826824,
-            "timestamp": 5.266656307351822
-        },
-        {
-            "x": 3.3421218274128583,
-            "y": 4.817843776695172,
-            "heading": -1.1501637008410697,
-            "angularVelocity": -0.4625978405715368,
-            "velocityX": -0.4618260597040901,
-            "velocityY": -0.4138270595977395,
-            "timestamp": 5.340282829772093
-        },
-        {
-            "x": 3.3087677521953345,
-            "y": 4.787759503770135,
-            "heading": -1.1858495459870186,
-            "angularVelocity": -0.48468736499938486,
-            "velocityX": -0.45301712101970637,
-            "velocityY": -0.40860646321595784,
-            "timestamp": 5.413909352192364
-        },
-        {
-            "x": 3.2761825599174124,
-            "y": 4.758148688152823,
-            "heading": -1.2235604524642407,
-            "angularVelocity": -0.5121918737647652,
-            "velocityX": -0.44257410518347007,
-            "velocityY": -0.4021759366589302,
-            "timestamp": 5.487535874612634
-        },
-        {
-            "x": 3.244545279648596,
-            "y": 4.729149218235016,
-            "heading": -1.2639000345369864,
-            "angularVelocity": -0.547894708953951,
-            "velocityX": -0.4296995054068451,
-            "velocityY": -0.3938726014013467,
-            "timestamp": 5.561162397032905
-        },
-        {
-            "x": 3.2141486367937797,
-            "y": 4.700995814255907,
-            "heading": -1.3078734515357762,
-            "angularVelocity": -0.5972496805944914,
-            "velocityX": -0.4128490910015826,
-            "velocityY": -0.382381281278044,
-            "timestamp": 5.634788919453175
-        },
-        {
-            "x": 3.185551521789466,
-            "y": 4.674155611334741,
-            "heading": -1.357437524753386,
-            "angularVelocity": -0.6731823205595795,
-            "velocityX": -0.38840779197850694,
-            "velocityY": -0.36454530295424753,
-            "timestamp": 5.708415441873446
-        },
-        {
-            "x": 3.161389111124608,
-            "y": 4.65130116728667,
-            "heading": -1.4227635315487523,
-            "angularVelocity": -0.8872618812888667,
-            "velocityX": -0.3281753622279693,
-            "velocityY": -0.3104104784090629,
-            "timestamp": 5.782041964293716
-        },
-        {
-            "x": 3.1613881587982178,
-            "y": 4.647716999053955,
-            "heading": -1.5707963267948966,
-            "angularVelocity": -2.010590618434374,
-            "velocityX": -0.00001293455617664302,
-            "velocityY": -0.048680395527252375,
-            "timestamp": 5.855668486713987
-        },
-        {
-            "x": 3.1590242751538056,
-            "y": 4.648068652143942,
-            "heading": -1.71822885135636,
-            "angularVelocity": -2.0416470093627317,
-            "velocityX": -0.032735083303029204,
-            "velocityY": 0.0048696953514198,
-            "timestamp": 5.927881030746015
-        },
-        {
-            "x": 3.147722843506569,
-            "y": 4.651261007949863,
-            "heading": -1.8460861775851984,
-            "angularVelocity": -1.7705694757427723,
-            "velocityX": -0.15650233347580403,
-            "velocityY": 0.04420777371456313,
-            "timestamp": 6.000093574778043
-        },
-        {
-            "x": 3.1294383579479246,
-            "y": 4.658664086881976,
-            "heading": -1.9555006668792347,
-            "angularVelocity": -1.5151728935835482,
-            "velocityX": -0.25320373078858216,
-            "velocityY": 0.10251790781432463,
-            "timestamp": 6.07230611881007
-        },
-        {
-            "x": 3.105950392035797,
-            "y": 4.670811298853881,
-            "heading": -2.048080530965875,
-            "angularVelocity": -1.2820468427975569,
-            "velocityX": -0.325261576461146,
-            "velocityY": 0.16821470749621126,
-            "timestamp": 6.144518662842098
-        },
-        {
-            "x": 3.078734489629141,
-            "y": 4.687681461713096,
-            "heading": -2.125691691683944,
-            "angularVelocity": -1.0747600954710448,
-            "velocityX": -0.37688607667090807,
-            "velocityY": 0.23361817652806605,
-            "timestamp": 6.216731206874126
-        },
-        {
-            "x": 3.048937913872785,
-            "y": 4.708930614799334,
-            "heading": -2.190285310241282,
-            "angularVelocity": -0.8944930472009076,
-            "velocityX": -0.41262326588495246,
-            "velocityY": 0.2942584750485111,
-            "timestamp": 6.288943750906154
-        },
-        {
-            "x": 3.0174119547024816,
-            "y": 4.734060139937329,
-            "heading": -2.243768219004051,
-            "angularVelocity": -0.7406318317638603,
-            "velocityX": -0.4365717839316291,
-            "velocityY": 0.347993904311833,
-            "timestamp": 6.361156294938182
-        },
-        {
-            "x": 2.9847672349131935,
-            "y": 4.762525988198673,
-            "heading": -2.2879169282753247,
-            "angularVelocity": -0.6113717479845765,
-            "velocityX": -0.4520643916770092,
-            "velocityY": 0.3941953388142518,
-            "timestamp": 6.4333688389702095
-        },
-        {
-            "x": 2.9514318737909195,
-            "y": 4.793802004717416,
-            "heading": -2.3243308683279778,
-            "angularVelocity": -0.5042605899122328,
-            "velocityX": -0.4616283994577112,
-            "velocityY": 0.4331105757037482,
-            "timestamp": 6.505581383002237
-        },
-        {
-            "x": 2.9177026495619596,
-            "y": 4.827411588267587,
-            "heading": -2.3544162803840454,
-            "angularVelocity": -0.4166230737242065,
-            "velocityX": -0.46708261952383673,
-            "velocityY": 0.46542583426037293,
-            "timestamp": 6.577793927034265
-        },
-        {
-            "x": 2.88378589769779,
-            "y": 4.862939604927141,
-            "heading": -2.379391061920221,
-            "angularVelocity": -0.3458510134347111,
-            "velocityX": -0.4696795040087044,
-            "velocityY": 0.4919923142964852,
-            "timestamp": 6.650006471066293
-        },
-        {
-            "x": 2.8498283989976017,
-            "y": 4.900033117168976,
-            "heading": -2.4003021659557313,
-            "angularVelocity": -0.289577168562792,
-            "velocityX": -0.47024376658337114,
-            "velocityY": 0.5136713120837215,
-            "timestamp": 6.722219015098321
-        },
-        {
-            "x": 2.8159399738054236,
-            "y": 4.938396448572123,
-            "heading": -2.4180494138871094,
-            "angularVelocity": -0.2457640589910084,
-            "velocityX": -0.46928723598420935,
-            "velocityY": 0.5312557799671519,
-            "timestamp": 6.794431559130349
-        },
-        {
-            "x": 2.7822098245267197,
-            "y": 4.977783840614619,
-            "heading": -2.4334117988726813,
-            "angularVelocity": -0.2127384541217516,
-            "velocityX": -0.4670954296215342,
-            "velocityY": 0.5454369814893605,
-            "timestamp": 6.866644103162376
-        },
-        {
-            "x": 2.7487184813842567,
-            "y": 5.0179914540575155,
-            "heading": -2.4470741047795617,
-            "angularVelocity": -0.18919574278978962,
-            "velocityX": -0.46378843996424124,
-            "velocityY": 0.5567954152821849,
-            "timestamp": 6.938856647194404
-        },
-        {
-            "x": 2.7155468563385656,
-            "y": 5.0588495367779895,
-            "heading": -2.459652889491748,
-            "angularVelocity": -0.17419113092882704,
-            "velocityX": -0.4593609807041143,
-            "velocityY": 0.5658031200556053,
-            "timestamp": 7.011069191226432
-        },
-        {
-            "x": 2.682783562068564,
-            "y": 5.100215023933873,
-            "heading": -2.471721683170927,
-            "angularVelocity": -0.16712877022898326,
-            "velocityX": -0.45370641221933317,
-            "velocityY": 0.5728296615271857,
-            "timestamp": 7.08328173525846
-        },
-        {
-            "x": 2.650531366839192,
-            "y": 5.141964497495229,
-            "heading": -2.4838357664761026,
-            "angularVelocity": -0.16775594140268155,
-            "velocityX": -0.44662870781934594,
-            "velocityY": 0.5781471089405039,
-            "timestamp": 7.155494279290488
-        },
-        {
-            "x": 2.618913440143494,
-            "y": 5.183987197473736,
-            "heading": -2.496557238457018,
-            "angularVelocity": -0.17616706559006176,
-            "velocityX": -0.4378453511023647,
-            "velocityY": 0.5819307509768665,
-            "timestamp": 7.2277068233225155
-        },
-        {
-            "x": 2.5880798824866296,
-            "y": 5.226177564560792,
-            "heading": -2.5104813496102354,
-            "angularVelocity": -0.1928212243435465,
-            "velocityX": -0.4269834011551906,
-            "velocityY": 0.5842526066986774,
-            "timestamp": 7.299919367354543
-        },
-        {
-            "x": 2.5582148956282778,
-            "y": 5.268426535906725,
-            "heading": -2.5262653286045555,
-            "angularVelocity": -0.21857669198470553,
-            "velocityX": -0.41357062347929907,
-            "velocityY": 0.585064159035808,
-            "timestamp": 7.372131911386571
-        },
-        {
-            "x": 2.5295447873325214,
-            "y": 5.31061044475372,
-            "heading": -2.5446612115646436,
-            "angularVelocity": -0.2547463630685689,
-            "velocityX": -0.39702393372321826,
-            "velocityY": 0.5841631729285931,
-            "timestamp": 7.444344455418599
-        },
-        {
-            "x": 2.5023467091367184,
-            "y": 5.352575812308809,
-            "heading": -2.566554449178874,
-            "angularVelocity": -0.3031777637486395,
-            "velocityX": -0.3766392468286359,
-            "velocityY": 0.5811368110293486,
-            "timestamp": 7.516556999450627
-        },
-        {
-            "x": 2.476956466389222,
-            "y": 5.394117632986223,
-            "heading": -2.593007817119188,
-            "angularVelocity": -0.36632649209230705,
-            "velocityX": -0.3516043242602833,
-            "velocityY": 0.5752715298188288,
-            "timestamp": 7.5887695434826545
-        },
-        {
-            "x": 2.4521989817386887,
-            "y": 5.435268589005595,
-            "heading": -2.621259955334882,
-            "angularVelocity": -0.39123588005934157,
-            "velocityX": -0.3428418840852991,
-            "velocityY": 0.5698588322981821,
-            "timestamp": 7.660982087514682
-        },
-        {
-            "x": 2.427559065081996,
-            "y": 5.476070836186979,
-            "heading": -2.6500802362174434,
-            "angularVelocity": -0.39910352514071706,
-            "velocityX": -0.34121380138282903,
-            "velocityY": 0.5650299089765801,
-            "timestamp": 7.73319463154671
-        },
-        {
-            "x": 2.403044029416898,
-            "y": 5.516501717159452,
-            "heading": -2.6795300674706377,
-            "angularVelocity": -0.407821544690809,
-            "velocityX": -0.3394844482175417,
-            "velocityY": 0.559887226165873,
-            "timestamp": 7.805407175578738
-        },
-        {
-            "x": 2.378663293884139,
-            "y": 5.556534603961008,
-            "heading": -2.7096829003029756,
-            "angularVelocity": -0.41755671727843513,
-            "velocityX": -0.33762465870120245,
-            "velocityY": 0.5543757990828,
-            "timestamp": 7.877619719610766
-        },
-        {
-            "x": 2.354428772799968,
-            "y": 5.59613781059984,
-            "heading": -2.74062708945702,
-            "angularVelocity": -0.42851542718561086,
-            "velocityX": -0.33559987961957327,
-            "velocityY": 0.5484255840822709,
-            "timestamp": 7.949832263642794
-        },
-        {
-            "x": 2.330355697409695,
-            "y": 5.635273029678329,
-            "heading": -2.77247058331099,
-            "angularVelocity": -0.4409690072662031,
-            "velocityX": -0.3333641781072918,
-            "velocityY": 0.5419448878733852,
-            "timestamp": 8.02204480767482
-        },
-        {
-            "x": 2.306463861338364,
-            "y": 5.673893057942382,
-            "heading": -2.8053478457859313,
-            "angularVelocity": -0.45528464501069815,
-            "velocityX": -0.33085437428619124,
-            "velocityY": 0.534810520550616,
-            "timestamp": 8.094257351706847
-        },
-        {
-            "x": 2.282779542094817,
-            "y": 5.711938365696433,
-            "heading": -2.839430372846894,
-            "angularVelocity": -0.47197516051846805,
-            "velocityX": -0.3279806792714956,
-            "velocityY": 0.5268517854346452,
-            "timestamp": 8.166469895738874
-        },
-        {
-            "x": 2.259338596376033,
-            "y": 5.749331681355029,
-            "heading": -2.8749434001407064,
-            "angularVelocity": -0.4917847414164131,
-            "velocityX": -0.3246104403742845,
-            "velocityY": 0.5178229926647014,
-            "timestamp": 8.238682439770901
-        },
-        {
-            "x": 2.2361917393720554,
-            "y": 5.785968937848207,
-            "heading": -2.9121940133772717,
-            "angularVelocity": -0.5158468481603996,
-            "velocityX": -0.3205378970405998,
-            "velocityY": 0.5073530781151929,
-            "timestamp": 8.310894983802928
-        },
-        {
-            "x": 2.2134142469672544,
-            "y": 5.821702966064213,
-            "heading": -2.9516221084722227,
-            "angularVelocity": -0.5460006377488126,
-            "velocityX": -0.3154229325406202,
-            "velocityY": 0.4948451642993865,
-            "timestamp": 8.383107527834955
-        },
-        {
-            "x": 2.191125686501144,
-            "y": 5.856311001786043,
-            "heading": -2.9939026154296866,
-            "angularVelocity": -0.5855008644856994,
-            "velocityX": -0.3086521983801695,
-            "velocityY": 0.4792524094772386,
-            "timestamp": 8.455320071866982
-        },
-        {
-            "x": 2.169536365545648,
-            "y": 5.8894197426408175,
-            "heading": -3.0401828782490323,
-            "angularVelocity": -0.6408895218927577,
-            "velocityX": -0.29896912295350214,
-            "velocityY": 0.4584901598272232,
-            "timestamp": 8.527532615899009
-        },
-        {
-            "x": 2.1490856813219974,
-            "y": 5.9202865649088325,
-            "heading": -3.092780352693255,
-            "angularVelocity": -0.7283703288571992,
-            "velocityX": -0.28320127060722483,
-            "velocityY": 0.42744404980837086,
-            "timestamp": 8.599745159931036
-        },
-        {
-            "x": 2.131117343902588,
-            "y": 5.946754455566406,
-            "heading": -3.158540067224243,
-            "angularVelocity": -0.9106411553901581,
-            "velocityX": -0.24882570833455084,
-            "velocityY": 0.3665276028197312,
-            "timestamp": 8.671957703963063
-        }
-    ]
-}
\ No newline at end of file